1393 files changed, 517452 insertions, 52605 deletions

diff --git a/drivers/Kconfig b/drivers/Kconfig
index 95b9e7eefadc..eee562d0dede 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -94,6 +94,8 @@ source "drivers/memstick/Kconfig"
 
 source "drivers/leds/Kconfig"
 
+source "drivers/switch/Kconfig"
+
 source "drivers/accessibility/Kconfig"
 
 source "drivers/infiniband/Kconfig"
diff --git a/drivers/Makefile b/drivers/Makefile
index 7fa433a7030c..d7f4b63904ac 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -5,6 +5,7 @@
 # Rewritten to use lists instead of if-statements.
 #
 
+obj-$(CONFIG_CPUQUIET_FRAMEWORK)+= cpuquiet/
 obj-y				+= gpio/
 obj-$(CONFIG_PCI)		+= pci/
 obj-$(CONFIG_PARISC)		+= parisc/
@@ -98,6 +99,7 @@ obj-$(CONFIG_CPU_IDLE)		+= cpuidle/
 obj-$(CONFIG_MMC)		+= mmc/
 obj-$(CONFIG_MEMSTICK)		+= memstick/
 obj-y				+= leds/
+obj-$(CONFIG_SWITCH)		+= switch/
 obj-$(CONFIG_INFINIBAND)	+= infiniband/
 obj-$(CONFIG_SGI_SN)		+= sn/
 obj-y				+= firmware/
diff --git a/drivers/acpi/atomicio.c b/drivers/acpi/atomicio.c
index 7489b89c300f..f151afe61aab 100644
--- a/drivers/acpi/atomicio.c
+++ b/drivers/acpi/atomicio.c
@@ -76,7 +76,7 @@ static void __iomem *__acpi_ioremap_fast(phys_addr_t paddr,
 {
 	struct acpi_iomap *map;
 
-	map = __acpi_find_iomap(paddr, size);
+	map = __acpi_find_iomap(paddr, size/8);
 	if (map)
 		return map->vaddr + (paddr - map->paddr);
 	else
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index 5987e0ba8c2d..3807e572aef7 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -83,6 +83,23 @@ config SATA_AHCI_PLATFORM
 
 	  If unsure, say N.
 
+config SATA_AHCI_TEGRA
+	tristate "TEGRA AHCI SATA support"
+	depends on ARCH_TEGRA_3x_SOC
+	help
+	  This option enables support for TEGRA AHCI Serial ATA.
+
+	  If unsure, say N.
+
+config TEGRA_SATA_IDLE_POWERGATE
+	bool "TEGRA SATA idle power-gating"
+	depends on SATA_AHCI_TEGRA && PM && PM_RUNTIME
+	help
+	  This option enables power-gating during SATA idling.
+	  This option should not be enabled if sata clocks are in dvfs_table.
+
+	  If unsure, say N.
+
 config SATA_FSL
 	tristate "Freescale 3.0Gbps SATA support"
 	depends on FSL_SOC
diff --git a/drivers/ata/Makefile b/drivers/ata/Makefile
index 9550d691fd19..5e939d5f49e4 100644
--- a/drivers/ata/Makefile
+++ b/drivers/ata/Makefile
@@ -3,6 +3,8 @@ obj-$(CONFIG_ATA)		+= libata.o
 
 # non-SFF interface
 obj-$(CONFIG_SATA_AHCI)		+= ahci.o libahci.o
+CFLAGS_ahci-tegra.o		= -Werror
+obj-$(CONFIG_SATA_AHCI_TEGRA)	+= ahci-tegra.o libahci.o
 obj-$(CONFIG_SATA_ACARD_AHCI)	+= acard-ahci.o libahci.o
 obj-$(CONFIG_SATA_AHCI_PLATFORM) += ahci_platform.o libahci.o
 obj-$(CONFIG_SATA_FSL)		+= sata_fsl.o
diff --git a/drivers/ata/ahci-tegra.c b/drivers/ata/ahci-tegra.c
new file mode 100644
index 000000000000..32bf46940b0f
--- /dev/null
+++ b/drivers/ata/ahci-tegra.c
@@ -0,0 +1,2155 @@
+/*
+ * ahci-tegra.c - AHCI SATA support for TEGRA AHCI device
+ *
+ * Copyright (c) 2011-2012, NVIDIA 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 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, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ * libata documentation is available via 'make {ps|pdf}docs',
+ * as Documentation/DocBook/libata.*
+ *
+ * AHCI hardware documentation:
+ * http://www.intel.com/technology/serialata/pdf/rev1_0.pdf
+ * http://www.intel.com/technology/serialata/pdf/rev1_1.pdf
+ *
+ */
+
+#include <linux/platform_device.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/blkdev.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/device.h>
+#include <linux/dmi.h>
+#include <scsi/scsi_host.h>
+#include <scsi/scsi_cmnd.h>
+#include <linux/libata.h>
+#include <linux/regulator/machine.h>
+#include <linux/pm_runtime.h>
+#include "ahci.h"
+
+#include <linux/clk.h>
+#include <mach/clk.h>
+#include <mach/iomap.h>
+#include <mach/io.h>
+#include <mach/powergate.h>
+
+#define DRV_NAME	"tegra-sata"
+#define DRV_VERSION	"1.0"
+
+#define ENABLE_AHCI_DBG_PRINT			0
+#if ENABLE_AHCI_DBG_PRINT
+#define AHCI_DBG_PRINT(fmt, arg...)  printk(KERN_ERR fmt, ## arg)
+#else
+#define AHCI_DBG_PRINT(fmt, arg...) do {} while (0)
+#endif
+
+/* number of AHCI ports */
+#define TEGRA_AHCI_NUM_PORTS			1
+
+/* idle timeout for PM in msec */
+#define TEGRA_AHCI_MIN_IDLE_TIME		1000
+#define TEGRA_AHCI_DEFAULT_IDLE_TIME		2000
+
+#ifdef CONFIG_TEGRA_SATA_IDLE_POWERGATE
+static u32 tegra_ahci_idle_time = TEGRA_AHCI_DEFAULT_IDLE_TIME;
+#endif
+
+/* Bit 0 (EN_FPCI) to allow FPCI accesses to SATA */
+#define SATA_CONFIGURATION_0_OFFSET		0x180
+#define EN_FPCI					(1 << 0)
+
+#define SATA_INTR_MASK_0_OFFSET			0x188
+#define IP_INT_MASK				(1 << 16)
+
+/* Need to write 0x00400200 to 0x70020094 */
+#define SATA_FPCI_BAR5_0_OFFSET			0x094
+#define PRI_ICTLR_CPU_IER_SET_0_OFFSET		0x024
+#define CPU_IER_SATA_CTL			(1 << 23)
+
+#define AHCI_BAR5_CONFIG_LOCATION		0x24
+#define TEGRA_SATA_BAR5_INIT_PROGRAM		0xFFFFFFFF
+#define TEGRA_SATA_BAR5_FINAL_PROGRAM		0x40020000
+
+#define FUSE_SATA_CALIB_OFFSET			0x224
+#define FUSE_SATA_CALIB_MASK			0x3
+
+#define T_SATA0_CFG_PHY_REG			0x120
+#define PHY_USE_7BIT_ALIGN_DET_FOR_SPD_MASK	(1 << 11)
+
+#define T_SATA0_CFG_POWER_GATE			0x4ac
+#define POWER_GATE_SSTS_RESTORED_MASK		(1 << 23)
+#define POWER_GATE_SSTS_RESTORED_YES		(1 << 23)
+#define POWER_GATE_SSTS_RESTORED_NO		(0 << 23)
+
+#define T_SATA0_DBG0_OFFSET			0x550
+
+#define T_SATA0_INDEX_OFFSET			0x680
+#define SATA0_NONE_SELECTED			0
+#define SATA0_CH1_SELECTED			(1 << 0)
+
+#define T_SATA0_CHX_PHY_CTRL1_GEN1_OFFSET	0x690
+#define SATA0_CHX_PHY_CTRL1_GEN1_TX_AMP_SHIFT	0
+#define SATA0_CHX_PHY_CTRL1_GEN1_TX_AMP_MASK	(0xff << 0)
+#define SATA0_CHX_PHY_CTRL1_GEN1_TX_PEAK_SHIFT	8
+#define SATA0_CHX_PHY_CTRL1_GEN1_TX_PEAK_MASK	(0xff << 8)
+
+#define T_SATA0_CHX_PHY_CTRL1_GEN2_OFFSET	0x694
+#define SATA0_CHX_PHY_CTRL1_GEN2_TX_AMP_SHIFT	0
+#define SATA0_CHX_PHY_CTRL1_GEN2_TX_AMP_MASK	(0xff << 0)
+#define SATA0_CHX_PHY_CTRL1_GEN2_TX_PEAK_SHIFT	12
+#define SATA0_CHX_PHY_CTRL1_GEN2_TX_PEAK_MASK	(0xff << 12)
+#define SATA0_CHX_PHY_CTRL1_GEN2_RX_EQ_SHIFT	24
+#define SATA0_CHX_PHY_CTRL1_GEN2_RX_EQ_MASK	(0xf << 24)
+
+/* AHCI config space defines */
+#define TEGRA_PRIVATE_AHCI_CC_BKDR		0x4a4
+#define TEGRA_PRIVATE_AHCI_CC_BKDR_OVERRIDE	0x54c
+#define TEGRA_PRIVATE_AHCI_CC_BKDR_OVERRIDE_EN	(1 << 12)
+#define TEGRA_PRIVATE_AHCI_CC_BKDR_PGM		0x01060100
+
+/* AHCI HBA_CAP */
+#define TEGRA_PRIVATE_AHCI_CAP_BKDR		0xa0
+#define T_SATA0_AHCI_HBA_CAP_BKDR		0x300
+
+#define TEGRA_SATA_IO_SPACE_OFFSET		4
+#define TEGRA_SATA_ENABLE_IO_SPACE		(1 << 0)
+#define TEGRA_SATA_ENABLE_MEM_SPACE		(1 << 1)
+#define TEGRA_SATA_ENABLE_BUS_MASTER		(1 << 2)
+#define TEGRA_SATA_ENABLE_SERR			(1 << 8)
+
+#define TEGRA_SATA_CORE_CLOCK_FREQ_HZ		(108*1000*1000)
+#define TEGRA_SATA_OOB_CLOCK_FREQ_HZ		(216*1000*1000)
+
+#define APB_PMC_SATA_PWRGT_0_REG		0x1ac
+#define CLK_RST_SATA_PLL_CFG0_REG		0x490
+#define CLK_RST_SATA_PLL_CFG1_REG		0x494
+#define SATA_AUX_PAD_PLL_CNTL_1_REG		0x1100
+#define SATA_AUX_MISC_CNTL_1_REG		0x1108
+
+/* for APB_PMC_SATA_PWRGT_0_REG */
+#define PG_INFO_MASK				(1 << 6)
+#define PG_INFO_ON				(1 << 6)
+#define PG_INFO_OFF				(0 << 6)
+#define PLLE_IDDQ_SWCTL_MASK			(1 << 4)
+#define PLLE_IDDQ_SWCTL_ON			(1 << 4)
+#define PLLE_IDDQ_SWCTL_OFF			(0 << 4)
+#define PADPHY_IDDQ_OVERRIDE_VALUE_MASK		(1 << 3)
+#define PADPHY_IDDQ_OVERRIDE_VALUE_ON		(1 << 3)
+#define PADPHY_IDDQ_OVERRIDE_VALUE_OFF		(0 << 3)
+#define PADPHY_IDDQ_SWCTL_MASK			(1 << 2)
+#define PADPHY_IDDQ_SWCTL_ON			(1 << 2)
+#define PADPHY_IDDQ_SWCTL_OFF			(0 << 2)
+#define PADPLL_IDDQ_OVERRIDE_VALUE_MASK		(1 << 1)
+#define PADPLL_IDDQ_OVERRIDE_VALUE_ON		(1 << 1)
+#define PADPLL_IDDQ_OVERRIDE_VALUE_OFF		(0 << 1)
+#define PADPLL_IDDQ_SWCTL_MASK			(1 << 0)
+#define PADPLL_IDDQ_SWCTL_ON			(1 << 0)
+#define PADPLL_IDDQ_SWCTL_OFF			(0 << 0)
+
+/* for CLK_RST_SATA_PLL_CFG0_REG */
+#define PADPLL_RESET_OVERRIDE_VALUE_MASK	(1 << 1)
+#define PADPLL_RESET_OVERRIDE_VALUE_ON		(1 << 1)
+#define PADPLL_RESET_OVERRIDE_VALUE_OFF		(0 << 1)
+#define PADPLL_RESET_SWCTL_MASK			(1 << 0)
+#define PADPLL_RESET_SWCTL_ON			(1 << 0)
+#define PADPLL_RESET_SWCTL_OFF			(0 << 0)
+
+/* for CLK_RST_SATA_PLL_CFG1_REG */
+#define IDDQ2LANE_SLUMBER_DLY_MASK		(0xffL << 16)
+#define IDDQ2LANE_SLUMBER_DLY_SHIFT		16
+#define IDDQ2LANE_SLUMBER_DLY_3MS		(3 << 16)
+#define IDDQ2LANE_IDDQ_DLY_SHIFT		0
+#define IDDQ2LANE_IDDQ_DLY_MASK			(0xffL << 0)
+
+/* for SATA_AUX_PAD_PLL_CNTL_1_REG */
+#define REFCLK_SEL_MASK				(3 << 11)
+#define REFCLK_SEL_INT_CML			(0 << 11)
+#define LOCKDET_FIELD				(1 << 6)
+
+/* for SATA_AUX_MISC_CNTL_1_REG */
+#define NVA2SATA_OOB_ON_POR_MASK		(1 << 7)
+#define NVA2SATA_OOB_ON_POR_YES			(1 << 7)
+#define NVA2SATA_OOB_ON_POR_NO			(0 << 7)
+#define L0_RX_IDLE_T_SAX_SHIFT			5
+#define L0_RX_IDLE_T_SAX_MASK			(3 << 5)
+#define L0_RX_IDLE_T_NPG_SHIFT			3
+#define L0_RX_IDLE_T_NPG_MASK			(3 << 3)
+#define L0_RX_IDLE_T_MUX_MASK			(1 << 2)
+#define L0_RX_IDLE_T_MUX_FROM_APB_MISC		(1 << 2)
+#define L0_RX_IDLE_T_MUX_FROM_SATA		(0 << 2)
+
+#define SSTAT_IPM_STATE_MASK			0xF00
+#define SSTAT_IPM_SLUMBER_STATE			0x600
+
+enum {
+	AHCI_PCI_BAR = 5,
+};
+
+enum port_idle_status {
+	PORT_IS_NOT_IDLE,
+	PORT_IS_IDLE,
+	PORT_IS_IDLE_NOT_SLUMBER,
+	PORT_IS_SLUMBER,
+};
+
+enum sata_state {
+	SATA_ON,
+	SATA_OFF,
+	SATA_GOING_ON,
+	SATA_GOING_OFF,
+	SATA_ABORT_OFF,
+};
+
+char *sata_power_rails[] = {
+	"avdd_plle",
+	"avdd_sata",
+	"vdd_sata",
+	"hvdd_sata",
+	"avdd_sata_pll"
+};
+
+#define NUM_SATA_POWER_RAILS	ARRAY_SIZE(sata_power_rails)
+
+struct tegra_qc_list {
+	struct list_head list;
+	struct ata_queued_cmd *qc;
+};
+
+/*
+ *  tegra_ahci_host_priv is the extension of ahci_host_priv
+ *  with extra fields: idle_timer, pg_save, pg_state, etc.
+ */
+struct tegra_ahci_host_priv {
+	struct ahci_host_priv	ahci_host_priv;
+	struct regulator	*power_rails[NUM_SATA_POWER_RAILS];
+	void __iomem		*bars_table[6];
+	struct ata_host		*host;
+	struct timer_list	idle_timer;
+	struct device		*dev;
+	void			*pg_save;
+	enum sata_state		pg_state;
+	struct list_head	qc_list;
+};
+
+static int tegra_ahci_init_one(struct platform_device *pdev);
+static int tegra_ahci_remove_one(struct platform_device *pdev);
+
+#ifdef CONFIG_PM
+static bool tegra_ahci_power_un_gate(struct ata_host *host);
+static bool tegra_ahci_power_gate(struct ata_host *host);
+static void tegra_ahci_abort_power_gate(struct ata_host *host);
+static int tegra_ahci_controller_suspend(struct platform_device *pdev);
+static int tegra_ahci_controller_resume(struct platform_device *pdev);
+static int tegra_ahci_suspend(struct platform_device *pdev, pm_message_t mesg);
+static int tegra_ahci_resume(struct platform_device *pdev);
+static enum port_idle_status tegra_ahci_is_port_idle(struct ata_port *ap);
+static bool tegra_ahci_are_all_ports_idle(struct ata_host *host);
+#ifdef CONFIG_TEGRA_SATA_IDLE_POWERGATE
+static enum port_idle_status tegra_ahci_is_port_slumber(struct ata_port *ap);
+static bool tegra_ahci_are_all_ports_slumber(struct ata_host *host);
+static unsigned int tegra_ahci_qc_issue(struct ata_queued_cmd *qc);
+static int tegra_ahci_hardreset(struct ata_link *link, unsigned int *class,
+				unsigned long deadline);
+static int tegra_ahci_runtime_suspend(struct device *dev);
+static int tegra_ahci_runtime_resume(struct device *dev);
+static void tegra_ahci_idle_timer(unsigned long arg);
+static int tegra_ahci_queue_one_qc(struct tegra_ahci_host_priv *tegra_hpriv,
+				   struct ata_queued_cmd *qc);
+static void tegra_ahci_dequeue_qcs(struct tegra_ahci_host_priv *tegra_hpriv);
+#endif
+#else
+#define tegra_ahci_controller_suspend	NULL
+#define tegra_ahci_controller_resume	NULL
+#define tegra_ahci_suspend		NULL
+#define tegra_ahci_resume		NULL
+#endif
+
+static struct scsi_host_template ahci_sht = {
+	AHCI_SHT("tegra-sata"),
+};
+
+static struct ata_port_operations tegra_ahci_ops = {
+	.inherits	= &ahci_ops,
+#ifdef CONFIG_PM
+#ifdef CONFIG_TEGRA_SATA_IDLE_POWERGATE
+	.qc_issue	= tegra_ahci_qc_issue,
+	.hardreset	= tegra_ahci_hardreset,
+#endif
+#endif
+};
+
+static const struct ata_port_info ahci_port_info = {
+	.flags		= AHCI_FLAG_COMMON,
+	.pio_mask	= 0x1f, /* pio0-4 */
+	.udma_mask	= ATA_UDMA6,
+	.port_ops	= &tegra_ahci_ops,
+};
+
+#ifdef CONFIG_TEGRA_SATA_IDLE_POWERGATE
+static const struct dev_pm_ops tegra_ahci_dev_rt_ops = {
+	.runtime_suspend = tegra_ahci_runtime_suspend,
+	.runtime_resume = tegra_ahci_runtime_resume,
+};
+#endif
+
+static struct platform_driver tegra_platform_ahci_driver = {
+	.probe		= tegra_ahci_init_one,
+	.remove		= __devexit_p(tegra_ahci_remove_one),
+#ifdef CONFIG_PM
+	.suspend	= tegra_ahci_suspend,
+	.resume		= tegra_ahci_resume,
+	.driver = {
+		.name = DRV_NAME,
+#ifdef CONFIG_TEGRA_SATA_IDLE_POWERGATE
+		.pm	= &tegra_ahci_dev_rt_ops,
+#endif
+	}
+#else
+	.driver = {
+		.name = DRV_NAME,
+	}
+#endif
+};
+
+struct tegra_ahci_host_priv *g_tegra_hpriv;
+
+static inline u32 pmc_readl(u32 offset)
+{
+	u32 val;
+	val = readl(IO_ADDRESS(TEGRA_PMC_BASE + offset));
+	AHCI_DBG_PRINT("[0x%x] => 0x%08x\n", TEGRA_PMC_BASE+offset, val);
+	return val;
+}
+
+static inline void pmc_writel(u32 val, u32 offset)
+{
+	AHCI_DBG_PRINT("[0x%x] <= 0x%08x\n", TEGRA_PMC_BASE+offset, val);
+	writel(val, IO_ADDRESS(TEGRA_PMC_BASE + offset));
+}
+
+static inline u32 clk_readl(u32 offset)
+{
+	u32 val;
+
+	val = readl(IO_ADDRESS(TEGRA_CLK_RESET_BASE + offset));
+	AHCI_DBG_PRINT("[0x%x] => 0x%08x\n", TEGRA_CLK_RESET_BASE+offset, val);
+	return val;
+}
+
+static inline void clk_writel(u32 val, u32 offset)
+{
+	AHCI_DBG_PRINT("[0x%x] <= 0x%08x\n", TEGRA_CLK_RESET_BASE+offset, val);
+	writel(val, IO_ADDRESS(TEGRA_CLK_RESET_BASE + offset));
+}
+
+static inline u32 misc_readl(u32 offset)
+{
+	u32 val;
+
+	val = readl(IO_ADDRESS(TEGRA_APB_MISC_BASE + offset));
+	AHCI_DBG_PRINT("[0x%x] => 0x%08x\n", TEGRA_APB_MISC_BASE+offset, val);
+	return val;
+}
+
+static inline void misc_writel(u32 val, u32 offset)
+{
+	AHCI_DBG_PRINT("[0x%x] <= 0x%08x\n", TEGRA_APB_MISC_BASE+offset, val);
+	writel(val, IO_ADDRESS(TEGRA_APB_MISC_BASE + offset));
+}
+
+static inline u32 sata_readl(u32 offset)
+{
+	u32 val;
+
+	val = readl(IO_ADDRESS(TEGRA_SATA_BASE + offset));
+	AHCI_DBG_PRINT("[0x%x] => 0x%08x\n", TEGRA_SATA_BASE+offset, val);
+	return val;
+}
+
+static inline void sata_writel(u32 val, u32 offset)
+{
+	AHCI_DBG_PRINT("[0x%x] <= 0x%08x\n", TEGRA_SATA_BASE+offset, val);
+	writel(val, IO_ADDRESS(TEGRA_SATA_BASE + offset));
+}
+
+static inline u32 scfg_readl(u32 offset)
+{
+	u32 val;
+
+	val = readl(IO_ADDRESS(TEGRA_SATA_CONFIG_BASE + offset));
+	AHCI_DBG_PRINT("[0x%x] => 0x%08x\n", TEGRA_SATA_CONFIG_BASE+offset,
+					     val);
+	return val;
+}
+
+static inline void scfg_writel(u32 val, u32 offset)
+{
+	AHCI_DBG_PRINT("[0x%x] <= 0x%08x\n", TEGRA_SATA_CONFIG_BASE+offset,
+					     val);
+	writel(val, IO_ADDRESS(TEGRA_SATA_CONFIG_BASE + offset));
+}
+
+static inline u32 pictlr_readl(u32 offset)
+{
+	u32 val;
+
+	val = readl(IO_ADDRESS(TEGRA_PRIMARY_ICTLR_BASE + offset));
+	AHCI_DBG_PRINT("[0x%x] => 0x%08x\n", TEGRA_PRIMARY_ICTLR_BASE+offset,
+					     val);
+	return val;
+}
+
+static inline void pictlr_writel(u32 val, u32 offset)
+{
+	AHCI_DBG_PRINT("[0x%x] <= 0x%08x\n", TEGRA_PRIMARY_ICTLR_BASE+offset,
+					     val);
+	writel(val, IO_ADDRESS(TEGRA_PRIMARY_ICTLR_BASE + offset));
+}
+
+static inline u32 fuse_readl(u32 offset)
+{
+	u32 val;
+
+	val = readl(IO_ADDRESS(TEGRA_FUSE_BASE + offset));
+	AHCI_DBG_PRINT("[0x%x] => 0x%08x\n", TEGRA_FUSE_BASE+offset, val);
+
+	return val;
+}
+
+/* Sata Pad Cntrl Values */
+struct sata_pad_cntrl {
+	u8 gen1_tx_amp;
+	u8 gen1_tx_peak;
+	u8 gen2_tx_amp;
+	u8 gen2_tx_peak;
+};
+
+static const struct sata_pad_cntrl sata_calib_pad_val[] = {
+	{	/* SATA_CALIB[1:0]  = 00 */
+		0x0c,
+		0x04,
+		0x0e,
+		0x0a
+	},
+	{	/* SATA_CALIB[1:0]  = 01 */
+		0x0e,
+		0x04,
+		0x14,
+		0x0a
+	},
+	{	/* SATA_CALIB[1:0]  = 10 */
+		0x0e,
+		0x07,
+		0x1a,
+		0x0e
+	},
+	{	/* SATA_CALIB[1:0]  = 11 */
+		0x14,
+		0x0e,
+		0x1a,
+		0x0e
+	}
+};
+
+static void tegra_ahci_set_pad_cntrl_regs(void)
+{
+	int	calib_val;
+	int	val;
+	int	i;
+
+	calib_val = fuse_readl(FUSE_SATA_CALIB_OFFSET) & FUSE_SATA_CALIB_MASK;
+
+	for (i = 0; i < TEGRA_AHCI_NUM_PORTS; ++i) {
+		scfg_writel((1 << i), T_SATA0_INDEX_OFFSET);
+
+		val = scfg_readl(T_SATA0_CHX_PHY_CTRL1_GEN1_OFFSET);
+		val &= ~SATA0_CHX_PHY_CTRL1_GEN1_TX_AMP_MASK;
+		val |= (sata_calib_pad_val[calib_val].gen1_tx_amp <<
+			SATA0_CHX_PHY_CTRL1_GEN1_TX_AMP_SHIFT);
+		scfg_writel(val, T_SATA0_CHX_PHY_CTRL1_GEN1_OFFSET);
+
+		val = scfg_readl(T_SATA0_CHX_PHY_CTRL1_GEN1_OFFSET);
+		val &= ~SATA0_CHX_PHY_CTRL1_GEN1_TX_PEAK_MASK;
+		val |= (sata_calib_pad_val[calib_val].gen1_tx_peak <<
+			SATA0_CHX_PHY_CTRL1_GEN1_TX_PEAK_SHIFT);
+		scfg_writel(val, T_SATA0_CHX_PHY_CTRL1_GEN1_OFFSET);
+
+		val = scfg_readl(T_SATA0_CHX_PHY_CTRL1_GEN2_OFFSET);
+		val &= ~SATA0_CHX_PHY_CTRL1_GEN2_TX_AMP_MASK;
+		val |= (sata_calib_pad_val[calib_val].gen2_tx_amp <<
+			SATA0_CHX_PHY_CTRL1_GEN2_TX_AMP_SHIFT);
+		scfg_writel(val, T_SATA0_CHX_PHY_CTRL1_GEN2_OFFSET);
+
+		val = scfg_readl(T_SATA0_CHX_PHY_CTRL1_GEN2_OFFSET);
+		val &= ~SATA0_CHX_PHY_CTRL1_GEN2_TX_PEAK_MASK;
+		val |= (sata_calib_pad_val[calib_val].gen2_tx_peak <<
+			SATA0_CHX_PHY_CTRL1_GEN2_TX_PEAK_SHIFT);
+		scfg_writel(val, T_SATA0_CHX_PHY_CTRL1_GEN2_OFFSET);
+
+		/* set 2 to SATA0_CHX_PHY_CTRL1_GEN2_RX_EQ field */
+		val = scfg_readl(T_SATA0_CHX_PHY_CTRL1_GEN2_OFFSET);
+		val &= ~SATA0_CHX_PHY_CTRL1_GEN2_RX_EQ_MASK;
+		val |= (2 << SATA0_CHX_PHY_CTRL1_GEN2_RX_EQ_SHIFT);
+		scfg_writel(val, T_SATA0_CHX_PHY_CTRL1_GEN2_OFFSET);
+	}
+	scfg_writel(SATA0_NONE_SELECTED, T_SATA0_INDEX_OFFSET);
+}
+
+int tegra_ahci_get_rails(struct regulator *regulators[])
+{
+	struct regulator *reg;
+	int i;
+	int ret = 0;
+
+	for (i = 0; i < NUM_SATA_POWER_RAILS; ++i) {
+		reg = regulator_get(NULL, sata_power_rails[i]);
+		if (IS_ERR_OR_NULL(reg)) {
+			pr_err("%s: can't get regulator %s\n",
+				__func__, sata_power_rails[i]);
+			WARN_ON(1);
+			ret = PTR_ERR(reg);
+			goto exit;
+		}
+		regulators[i] = reg;
+	}
+exit:
+	return ret;
+}
+
+void tegra_ahci_put_rails(struct regulator *regulators[])
+{
+	int i;
+
+	for (i = 0; i < NUM_SATA_POWER_RAILS; ++i)
+		regulator_put(regulators[i]);
+}
+
+int tegra_ahci_power_on_rails(struct regulator *regulators[])
+{
+	struct regulator *reg;
+	int i;
+	int ret = 0;
+
+	for (i = 0; i < NUM_SATA_POWER_RAILS; ++i) {
+		reg = regulators[i];
+		ret = regulator_enable(reg);
+		if (ret) {
+			pr_err("%s: can't enable regulator[%d]\n",
+				__func__, i);
+			WARN_ON(1);
+			goto exit;
+		}
+	}
+
+exit:
+	return ret;
+}
+
+int tegra_ahci_power_off_rails(struct regulator *regulators[])
+{
+	struct regulator *reg;
+	int i;
+	int ret = 0;
+
+	for (i = 0; i < NUM_SATA_POWER_RAILS; ++i) {
+		reg = regulators[i];
+		if (!IS_ERR_OR_NULL(reg)) {
+			ret = regulator_disable(reg);
+			if (ret) {
+				pr_err("%s: can't disable regulator[%d]\n",
+					__func__, i);
+				WARN_ON(1);
+				goto exit;
+			}
+		}
+	}
+
+exit:
+	return ret;
+}
+static int tegra_ahci_controller_init(struct tegra_ahci_host_priv *tegra_hpriv)
+{
+	int err;
+	struct clk *clk_sata = NULL;
+	struct clk *clk_sata_oob = NULL;
+	struct clk *clk_sata_cold = NULL;
+	struct clk *clk_pllp = NULL;
+	u32 val;
+	u32 timeout;
+
+	err = tegra_ahci_get_rails(tegra_hpriv->power_rails);
+	if (err) {
+		pr_err("%s: fails to get rails (%d)\n", __func__, err);
+		goto exit;
+	}
+
+	err = tegra_ahci_power_on_rails(tegra_hpriv->power_rails);
+	if (err) {
+		pr_err("%s: fails to power on rails (%d)\n", __func__, err);
+		goto exit;
+	}
+
+	/* pll_p is the parent of tegra_sata and tegra_sata_oob */
+	clk_pllp = clk_get_sys(NULL, "pll_p");
+	if (IS_ERR_OR_NULL(clk_pllp)) {
+		pr_err("%s: unable to get PLL_P clock\n", __func__);
+		err = -ENODEV;
+		goto exit;
+	}
+
+	clk_sata = clk_get_sys("tegra_sata", NULL);
+	if (IS_ERR_OR_NULL(clk_sata)) {
+		pr_err("%s: unable to get SATA clock\n", __func__);
+		err = -ENODEV;
+		goto exit;
+	}
+
+	clk_sata_oob = clk_get_sys("tegra_sata_oob", NULL);
+	if (IS_ERR_OR_NULL(clk_sata_oob)) {
+		pr_err("%s: unable to get SATA OOB clock\n", __func__);
+		err = -ENODEV;
+		goto exit;
+	}
+
+	clk_sata_cold = clk_get_sys("tegra_sata_cold", NULL);
+	if (IS_ERR_OR_NULL(clk_sata_cold)) {
+		pr_err("%s: unable to get SATA COLD clock\n", __func__);
+		err = -ENODEV;
+		goto exit;
+	}
+
+	tegra_periph_reset_assert(clk_sata);
+	tegra_periph_reset_assert(clk_sata_oob);
+	tegra_periph_reset_assert(clk_sata_cold);
+	udelay(10);
+
+	/* need to establish both clocks divisors before setting clk sources */
+	clk_set_rate(clk_sata, clk_get_rate(clk_sata)/10);
+	clk_set_rate(clk_sata_oob, clk_get_rate(clk_sata_oob)/10);
+
+	/* set SATA clk and SATA_OOB clk source */
+	clk_set_parent(clk_sata, clk_pllp);
+	clk_set_parent(clk_sata_oob, clk_pllp);
+
+	/* Configure SATA clocks */
+	/* Core clock runs at 108MHz */
+	if (clk_set_rate(clk_sata, TEGRA_SATA_CORE_CLOCK_FREQ_HZ)) {
+		err = -ENODEV;
+		goto exit;
+	}
+	/* OOB clock runs at 216MHz */
+	if (clk_set_rate(clk_sata_oob, TEGRA_SATA_OOB_CLOCK_FREQ_HZ)) {
+		err = -ENODEV;
+		goto exit;
+	}
+
+	/**** Init the SATA PAD PLL ****/
+	/* SATA_PADPLL_IDDQ_SWCTL=1 and SATA_PADPLL_IDDQ_OVERRIDE_VALUE=1 */
+	val = pmc_readl(APB_PMC_SATA_PWRGT_0_REG);
+	val &= ~(PADPLL_IDDQ_SWCTL_MASK | PADPLL_IDDQ_OVERRIDE_VALUE_MASK);
+	val |= (PADPLL_IDDQ_SWCTL_ON | PADPLL_IDDQ_OVERRIDE_VALUE_ON);
+	pmc_writel(val, APB_PMC_SATA_PWRGT_0_REG);
+
+	/* SATA_PADPLL_RESET_OVERRIDE_VALUE=1 and SATA_PADPLL_RESET_SWCTL=1 */
+	val = clk_readl(CLK_RST_SATA_PLL_CFG0_REG);
+	val &= ~(PADPLL_RESET_OVERRIDE_VALUE_MASK | PADPLL_RESET_SWCTL_MASK);
+	val |= (PADPLL_RESET_OVERRIDE_VALUE_ON | PADPLL_RESET_SWCTL_ON);
+	clk_writel(val, CLK_RST_SATA_PLL_CFG0_REG);
+
+	/* SATA_PADPHY_IDDQ_OVERRIDE_VALUE and SATA_PADPHY_IDDQ_SWCTL = 1 */
+	val = pmc_readl(APB_PMC_SATA_PWRGT_0_REG);
+	val &= ~(PADPHY_IDDQ_OVERRIDE_VALUE_MASK | PADPHY_IDDQ_SWCTL_MASK);
+	val |= (PADPHY_IDDQ_OVERRIDE_VALUE_ON | PADPHY_IDDQ_SWCTL_ON);
+	pmc_writel(val, APB_PMC_SATA_PWRGT_0_REG);
+
+	/* Get SATA pad PLL out of IDDQ mode */
+	val = pmc_readl(APB_PMC_SATA_PWRGT_0_REG);
+	val &= ~PADPLL_IDDQ_OVERRIDE_VALUE_MASK;
+	val |= PADPLL_IDDQ_OVERRIDE_VALUE_OFF;
+	pmc_writel(val, APB_PMC_SATA_PWRGT_0_REG);
+	udelay(3);
+
+	/* select internal CML ref clk
+	 * select PLLE as input to IO phy */
+	val = misc_readl(SATA_AUX_PAD_PLL_CNTL_1_REG);
+	val &= ~REFCLK_SEL_MASK;
+	val |= REFCLK_SEL_INT_CML;
+	misc_writel(val, SATA_AUX_PAD_PLL_CNTL_1_REG);
+
+	/* wait for SATA_PADPLL_IDDQ2LANE_SLUMBER_DLY = 3 microseconds. */
+	val = clk_readl(CLK_RST_SATA_PLL_CFG1_REG);
+	val &= ~IDDQ2LANE_SLUMBER_DLY_MASK;
+	val |= IDDQ2LANE_SLUMBER_DLY_3MS;
+	clk_writel(val, CLK_RST_SATA_PLL_CFG1_REG);
+	udelay(3);
+
+	/* de-assert IDDQ mode signal going to PHY */
+	val = pmc_readl(APB_PMC_SATA_PWRGT_0_REG);
+	val &= ~PADPHY_IDDQ_OVERRIDE_VALUE_MASK;
+	val |= PADPHY_IDDQ_OVERRIDE_VALUE_OFF;
+	pmc_writel(val, APB_PMC_SATA_PWRGT_0_REG);
+
+	err = tegra_unpowergate_partition_with_clk_on(TEGRA_POWERGATE_SATA);
+	if (err) {
+		pr_err("%s: ** failed to turn-on SATA (0x%x) **\n",
+				__func__, err);
+		goto exit;
+	}
+
+	/*
+	 * place SATA Pad PLL out of reset by writing
+	 * SATA_PADPLL_RST_OVERRIDE_VALUE = 0
+	 */
+	val = clk_readl(CLK_RST_SATA_PLL_CFG0_REG);
+	val &= ~PADPLL_RESET_OVERRIDE_VALUE_MASK;
+	val |= PADPLL_RESET_OVERRIDE_VALUE_OFF;
+	clk_writel(val, CLK_RST_SATA_PLL_CFG0_REG);
+
+	/*
+	 * Wait for SATA_AUX_PAD_PLL_CNTL_1_0_LOCKDET to turn 1 with a timeout
+	 * of 15 us.
+	 */
+	timeout = 15;
+	while (timeout--) {
+		udelay(1);
+		val = misc_readl(SATA_AUX_PAD_PLL_CNTL_1_REG);
+		if (val & LOCKDET_FIELD)
+			break;
+	}
+	if (timeout == 0)
+		pr_err("%s: AUX_PAD_PLL_CNTL_1 (0x%x) is not locked in 15us.\n",
+			__func__, val);
+
+	/* clear SW control of SATA PADPLL, SATA PHY and PLLE */
+	val = pmc_readl(APB_PMC_SATA_PWRGT_0_REG);
+	val &= ~(PADPLL_IDDQ_SWCTL_MASK | PADPHY_IDDQ_SWCTL_MASK |
+		 PLLE_IDDQ_SWCTL_MASK);
+	val |= (PADPLL_IDDQ_SWCTL_OFF | PADPHY_IDDQ_SWCTL_OFF |
+		PLLE_IDDQ_SWCTL_OFF);
+	pmc_writel(val, APB_PMC_SATA_PWRGT_0_REG);
+
+	val = clk_readl(CLK_RST_SATA_PLL_CFG0_REG);
+	val &= ~PADPLL_RESET_SWCTL_MASK;
+	val |= PADPLL_RESET_SWCTL_OFF;
+	clk_writel(val, CLK_RST_SATA_PLL_CFG0_REG);
+
+	/* clear NVA2SATA_OOB_ON_POR in SATA_AUX_MISC_CNTL_1_REG */
+	val = misc_readl(SATA_AUX_MISC_CNTL_1_REG);
+	val &= ~NVA2SATA_OOB_ON_POR_MASK;
+	misc_writel(val, SATA_AUX_MISC_CNTL_1_REG);
+
+	val = sata_readl(SATA_CONFIGURATION_0_OFFSET);
+	val |= EN_FPCI;
+	sata_writel(val, SATA_CONFIGURATION_0_OFFSET);
+
+	/* program sata pad control based on the fuse */
+	tegra_ahci_set_pad_cntrl_regs();
+
+	/*
+	 * clear bit T_SATA0_CFG_PHY_0_USE_7BIT_ALIGN_DET_FOR_SPD of
+	 * T_SATA0_CFG_PHY_0
+	 */
+	val = scfg_readl(T_SATA0_CFG_PHY_REG);
+	val &= ~PHY_USE_7BIT_ALIGN_DET_FOR_SPD_MASK;
+	scfg_writel(val, T_SATA0_CFG_PHY_REG);
+
+	/*
+	 * WAR: Before enabling SATA PLL shutdown, lockdet needs to be ignored.
+	 *      To ignore lockdet, T_SATA0_DBG0_OFFSET register bit 10 needs to
+	 *      be 1, and bit 8 needs to be 0.
+	 */
+	val = scfg_readl(T_SATA0_DBG0_OFFSET);
+	val |= (1 << 10);
+	val &= ~(1 << 8);
+	scfg_writel(val, T_SATA0_DBG0_OFFSET);
+
+	/* program class code and programming interface for AHCI */
+	val = scfg_readl(TEGRA_PRIVATE_AHCI_CC_BKDR_OVERRIDE);
+	val |= TEGRA_PRIVATE_AHCI_CC_BKDR_OVERRIDE_EN;
+	scfg_writel(val, TEGRA_PRIVATE_AHCI_CC_BKDR_OVERRIDE);
+	scfg_writel(TEGRA_PRIVATE_AHCI_CC_BKDR_PGM, TEGRA_PRIVATE_AHCI_CC_BKDR);
+	val &= ~TEGRA_PRIVATE_AHCI_CC_BKDR_OVERRIDE_EN;
+	scfg_writel(val, TEGRA_PRIVATE_AHCI_CC_BKDR_OVERRIDE);
+
+	/* Program config space registers: */
+
+	/* Enable BUS_MASTER+MEM+IO space, and SERR */
+	val = scfg_readl(TEGRA_SATA_IO_SPACE_OFFSET);
+	val |= TEGRA_SATA_ENABLE_IO_SPACE | TEGRA_SATA_ENABLE_MEM_SPACE |
+	       TEGRA_SATA_ENABLE_BUS_MASTER | TEGRA_SATA_ENABLE_SERR;
+	scfg_writel(val, TEGRA_SATA_IO_SPACE_OFFSET);
+
+	/* program bar5 space, by first writing 1's to bar5 register */
+	scfg_writel(TEGRA_SATA_BAR5_INIT_PROGRAM, AHCI_BAR5_CONFIG_LOCATION);
+	/* flush */
+	val = scfg_readl(AHCI_BAR5_CONFIG_LOCATION);
+
+	/* then, write the BAR5_FINAL_PROGRAM address */
+	scfg_writel(TEGRA_SATA_BAR5_FINAL_PROGRAM, AHCI_BAR5_CONFIG_LOCATION);
+	/* flush */
+	scfg_readl(AHCI_BAR5_CONFIG_LOCATION);
+
+	sata_writel((TEGRA_SATA_BAR5_FINAL_PROGRAM >> 8),
+		    SATA_FPCI_BAR5_0_OFFSET);
+
+	val = scfg_readl(T_SATA0_AHCI_HBA_CAP_BKDR);
+	val |= (HOST_CAP_ALPM | HOST_CAP_SSC | HOST_CAP_PART);
+	scfg_writel(val, T_SATA0_AHCI_HBA_CAP_BKDR);
+
+	/* enable Interrupt channel */
+	val = pictlr_readl(PRI_ICTLR_CPU_IER_SET_0_OFFSET);
+	val |= CPU_IER_SATA_CTL;
+	pictlr_writel(val, PRI_ICTLR_CPU_IER_SET_0_OFFSET);
+
+	/* set IP_INT_MASK */
+	val = sata_readl(SATA_INTR_MASK_0_OFFSET);
+	val |= IP_INT_MASK;
+	sata_writel(val, SATA_INTR_MASK_0_OFFSET);
+
+exit:
+	if (!IS_ERR_OR_NULL(clk_pllp))
+		clk_put(clk_pllp);
+	if (!IS_ERR_OR_NULL(clk_sata))
+		clk_put(clk_sata);
+	if (!IS_ERR_OR_NULL(clk_sata_oob))
+		clk_put(clk_sata_oob);
+	if (!IS_ERR_OR_NULL(clk_sata_cold))
+		clk_put(clk_sata_cold);
+
+	if (err) {
+		/* turn off all SATA power rails; ignore returned status */
+		tegra_ahci_power_off_rails(tegra_hpriv->power_rails);
+		/* return regulators to system */
+		tegra_ahci_put_rails(tegra_hpriv->power_rails);
+	}
+
+	return err;
+}
+
+static void tegra_ahci_save_initial_config(struct platform_device *pdev,
+					   struct ahci_host_priv *hpriv)
+{
+	ahci_save_initial_config(&pdev->dev, hpriv, 0, 0);
+}
+
+static void tegra_ahci_controller_remove(struct platform_device *pdev)
+{
+	struct ata_host *host = dev_get_drvdata(&pdev->dev);
+	struct tegra_ahci_host_priv *tegra_hpriv;
+	int status;
+
+	tegra_hpriv = (struct tegra_ahci_host_priv *)host->private_data;
+
+#ifdef CONFIG_PM
+	/* call tegra_ahci_controller_suspend() to power-down the SATA */
+	status = tegra_ahci_controller_suspend(pdev);
+	if (status)
+		dev_err(host->dev, "remove: error suspend SATA (0x%x)\n",
+				   status);
+#else
+	/* power off the sata */
+	status = tegra_powergate_partition_with_clk_off(TEGRA_POWERGATE_SATA);
+	if (status)
+		dev_err(host->dev, "remove: error turn-off SATA (0x%x)\n",
+				   status);
+	tegra_ahci_power_off_rails(tegra_hpriv->power_rails);
+#endif
+
+	/* return system resources */
+	tegra_ahci_put_rails(tegra_hpriv->power_rails);
+}
+
+#ifdef CONFIG_PM
+static int tegra_ahci_controller_suspend(struct platform_device *pdev)
+{
+	struct ata_host *host = dev_get_drvdata(&pdev->dev);
+	struct tegra_ahci_host_priv *tegra_hpriv;
+	unsigned long flags;
+
+	tegra_hpriv = (struct tegra_ahci_host_priv *)host->private_data;
+
+	/* stop the idle timer */
+	if (timer_pending(&tegra_hpriv->idle_timer))
+		del_timer_sync(&tegra_hpriv->idle_timer);
+
+	spin_lock_irqsave(&host->lock, flags);
+	if (tegra_hpriv->pg_state == SATA_OFF)
+		dev_dbg(host->dev, "suspend: SATA already power gated\n");
+	else {
+		bool pg_ok;
+
+		dev_dbg(host->dev, "suspend: power gating SATA...\n");
+		pg_ok = tegra_ahci_power_gate(host);
+		if (pg_ok) {
+			tegra_hpriv->pg_state = SATA_OFF;
+			dev_dbg(host->dev, "suspend: SATA is power gated\n");
+		} else {
+			dev_err(host->dev, "suspend: abort power gating\n");
+			tegra_ahci_abort_power_gate(host);
+			spin_unlock_irqrestore(&host->lock, flags);
+			return -EBUSY;
+		}
+	}
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	return tegra_ahci_power_off_rails(tegra_hpriv->power_rails);
+}
+
+static int tegra_ahci_controller_resume(struct platform_device *pdev)
+{
+	struct ata_host *host = dev_get_drvdata(&pdev->dev);
+	struct tegra_ahci_host_priv *tegra_hpriv;
+	unsigned long flags;
+	int err;
+
+	tegra_hpriv = (struct tegra_ahci_host_priv *)host->private_data;
+
+	err = tegra_ahci_power_on_rails(tegra_hpriv->power_rails);
+	if (err) {
+		pr_err("%s: fails to power on rails (%d)\n", __func__, err);
+		return err;
+	}
+
+	spin_lock_irqsave(&host->lock, flags);
+	if (tegra_hpriv->pg_state == SATA_ON) {
+		dev_dbg(host->dev, "resume: SATA already powered on\n");
+	} else {
+		dev_dbg(host->dev, "resume: powering on SATA...\n");
+		tegra_ahci_power_un_gate(host);
+		tegra_hpriv->pg_state = SATA_ON;
+	}
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	return 0;
+}
+
+static int tegra_ahci_suspend(struct platform_device *pdev, pm_message_t mesg)
+{
+	struct ata_host *host = dev_get_drvdata(&pdev->dev);
+	void __iomem *mmio = host->iomap[AHCI_PCI_BAR];
+	u32 ctl;
+	int rc;
+
+	dev_dbg(host->dev, "** entering %s: **\n", __func__);
+	if (mesg.event & PM_EVENT_SLEEP) {
+		/*
+		 * AHCI spec rev1.1 section 8.3.3:
+		 * Software must disable interrupts prior to requesting a
+		 * transition of the HBA to D3 state.
+		 */
+		ctl = readl(mmio + HOST_CTL);
+		ctl &= ~HOST_IRQ_EN;
+		writel(ctl, mmio + HOST_CTL);
+		readl(mmio + HOST_CTL); /* flush */
+	}
+
+	rc = ata_host_suspend(host, mesg);
+	if (rc)
+		return rc;
+
+	return tegra_ahci_controller_suspend(pdev);
+}
+
+static int tegra_ahci_resume(struct platform_device *pdev)
+{
+	struct ata_host *host = dev_get_drvdata(&pdev->dev);
+	int rc;
+
+	dev_dbg(host->dev, "** entering %s: **\n", __func__);
+	rc = tegra_ahci_controller_resume(pdev);
+	if (rc)
+		return rc;
+
+	if (pdev->dev.power.power_state.event == PM_EVENT_SUSPEND) {
+		rc = ahci_reset_controller(host);
+		if (rc)
+			return rc;
+
+		ahci_init_controller(host);
+	}
+
+	ata_host_resume(host);
+	return 0;
+}
+
+#ifdef CONFIG_TEGRA_SATA_IDLE_POWERGATE
+static int tegra_ahci_runtime_suspend(struct device *dev)
+{
+	struct ata_host *host;
+	struct tegra_ahci_host_priv *tegra_hpriv;
+	bool pg_ok;
+	unsigned long flags;
+	int err = 0;
+
+	host = dev_get_drvdata(dev);
+	tegra_hpriv = (struct tegra_ahci_host_priv *)host->private_data;
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	switch (tegra_hpriv->pg_state) {
+	case SATA_OFF:
+		dev_dbg(dev, "** rt-suspend: already power gated **\n");
+		break;
+
+	case SATA_ABORT_OFF:
+		dev_dbg(dev, "** rt-suspend: abort suspend **\n");
+		pm_runtime_get_noresume(dev);
+		tegra_hpriv->pg_state = SATA_ON;
+		tegra_ahci_dequeue_qcs(tegra_hpriv);
+		err = -EBUSY;
+		break;
+
+	case SATA_ON:
+	case SATA_GOING_OFF:
+		if (tegra_ahci_are_all_ports_idle(host)) {
+			/* if all ports are in idle, do power-gate */
+			dev_dbg(dev, "** rt-suspend: power-down sata (%u) **\n",
+					tegra_hpriv->pg_state);
+			pg_ok = tegra_ahci_power_gate(host);
+			dev_dbg(dev, "** rt-suspend: done **\n");
+			if (pg_ok) {
+				tegra_hpriv->pg_state = SATA_OFF;
+			} else {
+				dev_err(dev, "** rt-suspend: abort pg **\n");
+				tegra_ahci_abort_power_gate(host);
+				tegra_hpriv->pg_state = SATA_ON;
+				err = -EBUSY;
+			}
+		} else {
+			dev_dbg(dev, "** rt-suspend: port not idle (%u) **\n",
+					tegra_hpriv->pg_state);
+			err = -EBUSY;
+		}
+		break;
+
+	case SATA_GOING_ON:
+	default:
+		dev_err(dev, "** rt-suspend: bad state (%u) **\n",
+			tegra_hpriv->pg_state);
+		WARN_ON(1);
+		err = -EBUSY;
+		break;
+
+	}
+
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	return err;
+}
+
+static int tegra_ahci_runtime_resume(struct device *dev)
+{
+	struct ata_host *host;
+	struct tegra_ahci_host_priv *tegra_hpriv;
+	unsigned long flags;
+	int err = 0;
+
+	host = dev_get_drvdata(dev);
+	tegra_hpriv = (struct tegra_ahci_host_priv *)host->private_data;
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	if (tegra_hpriv->pg_state == SATA_ON) {
+		dev_dbg(dev, "** rt-resume: already power ungated **\n");
+		goto exit;
+	}
+
+	if ((tegra_hpriv->pg_state == SATA_OFF) ||
+	    (tegra_hpriv->pg_state == SATA_GOING_ON)) {
+		dev_dbg(dev, "** rt-resume: power-up sata (%u) **\n",
+				tegra_hpriv->pg_state);
+		tegra_ahci_power_un_gate(host);
+		dev_dbg(dev, "** rt-resume: done **\n");
+		tegra_hpriv->pg_state = SATA_ON;
+
+		/* now qc_issue all qcs in the qc_list */
+		tegra_ahci_dequeue_qcs(tegra_hpriv);
+	} else {
+		dev_err(dev, "** rt-resume: bad state (%u) **\n",
+				tegra_hpriv->pg_state);
+		WARN_ON(1);
+		err = -EBUSY;
+	}
+
+exit:
+	spin_unlock_irqrestore(&host->lock, flags);
+	return err;
+}
+#endif
+
+static u16 pg_save_bar5_registers[] = {
+	0x018,	/* T_AHCI_HBA_CCC_PORTS */
+	0x004,	/* T_AHCI_HBA_GHC */
+	0x014,	/* T_AHCI_HBA_CCC_CTL - OP (optional) */
+	0x01C,	/* T_AHCI_HBA_EM_LOC */
+	0x020	/* T_AHCI_HBA_EM_CTL - OP */
+};
+
+static u16 pg_save_bar5_port_registers[] = {
+	0x100,	/* T_AHCI_PORT_PXCLB */
+	0x104,	/* T_AHCI_PORT_PXCLBU */
+	0x108,	/* T_AHCI_PORT_PXFB */
+	0x10C,	/* T_AHCI_PORT_PXFBU */
+	0x114,	/* T_AHCI_PORT_PXIE */
+	0x118,	/* T_AHCI_PORT_PXCMD */
+	0x12C	/* T_AHCI_PORT_PXSCTL */
+};
+
+/*
+ * pg_save_bar5_bkdr_registers:
+ *    These registers in BAR5 are read only.
+ * To restore back those register values, write the saved value
+ *    to the registers specified in pg_restore_bar5_bkdr_registers[].
+ *    These pg_restore_bar5_bkdr_registers[] are in SATA_CONFIG space.
+ */
+static u16 pg_save_bar5_bkdr_registers[] = {
+	/* Save and restore via bkdr writes */
+	0x000,	/* T_AHCI_HBA_CAP */
+	0x00C,	/* T_AHCI_HBA_PI */
+	0x024	/* T_AHCI_HBA_CAP2 */
+};
+
+static u16 pg_restore_bar5_bkdr_registers[] = {
+	/* Save and restore via bkdr writes */
+	0x300,	/* BKDR of T_AHCI_HBA_CAP */
+	0x33c,	/* BKDR of T_AHCI_HBA_PI */
+	0x330	/* BKDR of T_AHCI_HBA_CAP2 */
+};
+
+/* These registers are saved for each port */
+static u16 pg_save_bar5_bkdr_port_registers[] = {
+	0x120,	/* NV_PROJ__SATA0_CHX_AHCI_PORT_PXTFD  */
+	0x124,	/* NV_PROJ__SATA0_CHX_AHCI_PORT_PXSIG */
+	0x128	/* NV_PROJ__SATA0_CHX_AHCI_PORT_PXSSTS */
+};
+
+static u16 pg_restore_bar5_bkdr_port_registers[] = {
+	/* Save and restore via bkdr writes */
+	0x790,	/* BKDR of NV_PROJ__SATA0_CHX_AHCI_PORT_PXTFD  */
+	0x794,	/* BKDR of NV_PROJ__SATA0_CHX_AHCI_PORT_PXSIG */
+	0x798	/* BKDR of NV_PROJ__SATA0_CHX_AHCI_PORT_PXSSTS */
+};
+
+static u16 pg_save_config_registers[] = {
+	0x004,	/* T_SATA0_CFG_1 */
+	0x00C,	/* T_SATA0_CFG_3 */
+	0x024,	/* T_SATA0_CFG_9 */
+	0x028,	/* T_SATA0_CFG_10 */
+	0x030,	/* T_SATA0_CFG_12 */
+	0x034,	/* T_SATA0_CFG_13 */
+	0x038,	/* T_SATA0_CFG_14 */
+	0x03C,	/* T_SATA0_CFG_15 */
+	0x040,	/* T_SATA0_CFG_16 */
+	0x044,	/* T_SATA0_CFG_17 */
+	0x048,	/* T_SATA0_CFG_18 */
+	0x0B0,	/* T_SATA0_MSI_CTRL */
+	0x0B4,	/* T_SATA0_MSI_ADDR1 */
+	0x0B8,	/* T_SATA0_MSI_ADDR2 */
+	0x0BC,	/* T_SATA0_MSI_DATA */
+	0x0C0,	/* T_SATA0_MSI_QUEUE */
+	0x0EC,	/* T_SATA0_MSI_MAP */
+	0x124,	/* T_SATA0_CFG_PHY_POWER */
+	0x128,	/* T_SATA0_CFG_PHY_POWER_1 */
+	0x12C,	/* T_SATA0_CFG_PHY_1 */
+	0x174,	/* T_SATA0_CFG_LINK_0 */
+	0x178,	/* T_SATA0_CFG_LINK_1 */
+	0x1D0,	/* MCP_SATA0_CFG_TRANS_0 */
+	0x238,	/* T_SATA0_ALPM_CTRL */
+	0x30C,	/* T_SATA0_AHCI_HBA_CYA_0 */
+	0x320,	/* T_SATA0_AHCI_HBA_SPARE_1 */
+	0x324,	/* T_SATA0_AHCI_HBA_SPARE_2 */
+	0x328,	/* T_SATA0_AHCI_HBA_DYN_CLK_CLAMP */
+	0x32C,	/* T_SATA0_AHCI_CFG_ERR_CTRL */
+	0x338,	/* T_SATA0_AHCI_HBA_CYA_1 */
+	0x340,	/* T_SATA0_AHCI_HBA_PRE_STAGING_CONTROL */
+	0x430,	/* T_SATA0_CFG_FPCI_0 */
+	0x494,	/* T_SATA0_CFG_ESATA_CTRL */
+	0x4A0,	/* T_SATA0_CYA1 */
+	0x4B0,	/* T_SATA0_CFG_GLUE */
+	0x534,	/* T_SATA0_PHY_CTRL */
+	0x540,	/* T_SATA0_CTRL */
+	0x550,	/* T_SATA0_DBG0 */
+	0x554	/* T_SATA0_LOW_POWER_COUNT */
+};
+
+static u16 pg_save_config_port_registers[] = {
+	/* Save and restore per port */
+	/* need to have port selected */
+	0x530,	/* T_SATA0_CHXCFG1 */
+	0x684,	/* T_SATA0_CHX_MISC */
+	0x700,	/* T_SATA0_CHXCFG3 */
+	0x704,	/* T_SATA0_CHXCFG4_CHX */
+	0x690,	/* T_SATA0_CHX_PHY_CTRL1_GEN1 */
+	0x694,	/* T_SATA0_CHX_PHY_CTRL1_GEN2 */
+	0x698,	/* T_SATA0_CHX_PHY_CTRL1_GEN3 */
+	0x69C,	/* T_SATA0_CHX_PHY_CTRL_2 */
+	0x6B0,	/* T_SATA0_CHX_PHY_CTRL_3 */
+	0x6B4,	/* T_SATA0_CHX_PHY_CTRL_4 */
+	0x6B8,	/* T_SATA0_CHX_PHY_CTRL_5 */
+	0x6BC,	/* T_SATA0_CHX_PHY_CTRL_6 */
+	0x714,	/* T_SATA0_PRBS_CHX - OP */
+	0x750,	/* T_SATA0_CHX_LINK0 */
+	0x7F0	/* T_SATA0_CHX_GLUE */
+};
+
+static u16 pg_save_ipfs_registers[] = {
+	0x094,	/* SATA_FPCI_BAR5_0 */
+	0x0C0,	/* SATA_MSI_BAR_SZ_0 */
+	0x0C4,	/* SATA_MSI_AXI_BAR_ST_0 */
+	0x0C8,	/* SATA_MSI_FPCI_BAR_ST_0 */
+	0x140,	/* SATA_MSI_EN_VEC0_0 */
+	0x144,	/* SATA_MSI_EN_VEC1_0 */
+	0x148,	/* SATA_MSI_EN_VEC2_0 */
+	0x14C,	/* SATA_MSI_EN_VEC3_0 */
+	0x150,	/* SATA_MSI_EN_VEC4_0 */
+	0x154,	/* SATA_MSI_EN_VEC5_0 */
+	0x158,	/* SATA_MSI_EN_VEC6_0 */
+	0x15C,	/* SATA_MSI_EN_VEC7_0 */
+	0x180,	/* SATA_CONFIGURATION_0 */
+	0x184,	/* SATA_FPCI_ERROR_MASKS_0 */
+	0x188,	/* SATA_INTR_MASK_0 */
+	0x1A0,	/* SATA_CFG_REVID_0 */
+	0x198,	/* SATA_IPFS_INTR_ENABLE_0 */
+	0x1BC,	/* SATA_CLKGATE_HYSTERSIS_0 */
+	0x1DC	/* SATA_SATA_MCCIF_FIFOCTRL_0 */
+};
+
+static void tegra_ahci_save_regs(u32 **save_addr,
+				 u32 reg_base,
+				 u16 reg_array[],
+				 u32 regs)
+{
+	u32 i;
+	u32 *dest = (u32 *)*save_addr;
+	u32 base = (u32)IO_ADDRESS(reg_base);
+
+	for (i = 0; i < regs; ++i, ++dest) {
+		*dest = readl(base + (u32)reg_array[i]);
+		AHCI_DBG_PRINT("save: [0x%x]=0x%08x\n",
+			       (reg_base+(u32)reg_array[i]), *dest);
+	}
+	*save_addr = dest;
+}
+
+static void tegra_ahci_restore_regs(void **save_addr,
+				    u32 reg_base,
+				    u16 reg_array[],
+				    u32 regs)
+{
+	u32 i;
+	u32 *src = (u32 *)*save_addr;
+	u32 base = (u32)IO_ADDRESS(reg_base);
+
+	for (i = 0; i < regs; ++i, ++src) {
+		writel(*src, base + (u32)reg_array[i]);
+		AHCI_DBG_PRINT("restore: [0x%x]=0x%08x\n",
+				(reg_base+(u32)reg_array[i]), *src);
+	}
+	*save_addr = src;
+}
+
+static void tegra_ahci_pg_save_registers(struct ata_host *host)
+{
+	struct tegra_ahci_host_priv *tegra_hpriv;
+	u32 *pg_save;
+	u32 regs;
+	int i;
+
+	tegra_hpriv = (struct tegra_ahci_host_priv *)host->private_data;
+	pg_save = tegra_hpriv->pg_save;
+
+	/*
+	 * Driver should save/restore the registers in the order of
+	 * IPFS, CFG, Ext CFG, BAR5.
+	 */
+
+	/* save IPFS registers */
+	regs = ARRAY_SIZE(pg_save_ipfs_registers);
+	tegra_ahci_save_regs(&pg_save, TEGRA_SATA_BASE,
+			     pg_save_ipfs_registers, regs);
+	/* after the call, pg_save should point to the next address to save */
+
+	/* save CONFIG registers */
+	regs = ARRAY_SIZE(pg_save_config_registers);
+	tegra_ahci_save_regs(&pg_save, TEGRA_SATA_CONFIG_BASE,
+			     pg_save_config_registers, regs);
+
+	/* save CONFIG per port registers */
+	for (i = 0; i < TEGRA_AHCI_NUM_PORTS; ++i) {
+		scfg_writel((1 << i), T_SATA0_INDEX_OFFSET);
+		regs = ARRAY_SIZE(pg_save_config_port_registers);
+		tegra_ahci_save_regs(&pg_save, TEGRA_SATA_CONFIG_BASE,
+				     pg_save_config_port_registers, regs);
+	}
+	scfg_writel(SATA0_NONE_SELECTED, T_SATA0_INDEX_OFFSET);
+
+	/* save BAR5 registers */
+	regs = ARRAY_SIZE(pg_save_bar5_registers);
+	tegra_ahci_save_regs(&pg_save, TEGRA_SATA_BAR5_BASE,
+			     pg_save_bar5_registers, regs);
+
+	/* save BAR5 port_registers */
+	regs = ARRAY_SIZE(pg_save_bar5_port_registers);
+	for (i = 0; i < TEGRA_AHCI_NUM_PORTS; ++i)
+		tegra_ahci_save_regs(&pg_save, TEGRA_SATA_BAR5_BASE + (0x80*i),
+				     pg_save_bar5_port_registers, regs);
+
+	/* save bkdr registers */
+	regs = ARRAY_SIZE(pg_save_bar5_bkdr_registers);
+	tegra_ahci_save_regs(&pg_save, TEGRA_SATA_BAR5_BASE,
+			     pg_save_bar5_bkdr_registers, regs);
+
+	/* and save bkdr per_port registers */
+	for (i = 0; i < TEGRA_AHCI_NUM_PORTS; ++i) {
+		scfg_writel((1 << i), T_SATA0_INDEX_OFFSET);
+		regs = ARRAY_SIZE(pg_save_bar5_bkdr_port_registers);
+		tegra_ahci_save_regs(&pg_save, TEGRA_SATA_BAR5_BASE + (0x80*i),
+				     pg_save_bar5_bkdr_port_registers,
+				     regs);
+	}
+	scfg_writel(SATA0_NONE_SELECTED, T_SATA0_INDEX_OFFSET);
+}
+
+static void tegra_ahci_pg_restore_registers(struct ata_host *host)
+{
+	struct tegra_ahci_host_priv *tegra_hpriv;
+	void *pg_save;
+	u32 regs, val;
+	int i;
+
+	tegra_hpriv = (struct tegra_ahci_host_priv *)host->private_data;
+	pg_save = tegra_hpriv->pg_save;
+
+	/*
+	 * Driver should restore the registers in the order of
+	 * IPFS, CFG, Ext CFG, BAR5.
+	 */
+
+	/* restore IPFS registers */
+	regs = ARRAY_SIZE(pg_save_ipfs_registers);
+	tegra_ahci_restore_regs(&pg_save, TEGRA_SATA_BASE,
+				pg_save_ipfs_registers, regs);
+	/* after the call, pg_save should point to the next addr to restore */
+
+	/* restore CONFIG registers */
+	regs = ARRAY_SIZE(pg_save_config_registers);
+	tegra_ahci_restore_regs(&pg_save, TEGRA_SATA_CONFIG_BASE,
+				pg_save_config_registers, regs);
+
+	/* restore CONFIG per port registers */
+	for (i = 0; i < TEGRA_AHCI_NUM_PORTS; ++i) {
+		scfg_writel((1 << i), T_SATA0_INDEX_OFFSET);
+		regs = ARRAY_SIZE(pg_save_config_port_registers);
+		tegra_ahci_restore_regs(&pg_save, TEGRA_SATA_CONFIG_BASE,
+					pg_save_config_port_registers,
+					regs);
+	}
+	scfg_writel(SATA0_NONE_SELECTED, T_SATA0_INDEX_OFFSET);
+
+	/* restore BAR5 registers */
+	regs = ARRAY_SIZE(pg_save_bar5_registers);
+	tegra_ahci_restore_regs(&pg_save, TEGRA_SATA_BAR5_BASE,
+				pg_save_bar5_registers, regs);
+
+	/* restore BAR5 port_registers */
+	regs = ARRAY_SIZE(pg_save_bar5_port_registers);
+	for (i = 0; i < TEGRA_AHCI_NUM_PORTS; ++i)
+		tegra_ahci_restore_regs(&pg_save, TEGRA_SATA_BAR5_BASE+(0x80*i),
+					pg_save_bar5_port_registers, regs);
+
+	/* restore bkdr registers */
+	regs = ARRAY_SIZE(pg_restore_bar5_bkdr_registers);
+	tegra_ahci_restore_regs(&pg_save, TEGRA_SATA_CONFIG_BASE,
+			     pg_restore_bar5_bkdr_registers, regs);
+
+	/* and restore BAR5 bkdr per_port registers */
+	for (i = 0; i < TEGRA_AHCI_NUM_PORTS; ++i) {
+		scfg_writel((1 << i), T_SATA0_INDEX_OFFSET);
+		regs = ARRAY_SIZE(pg_restore_bar5_bkdr_port_registers);
+		tegra_ahci_restore_regs(&pg_save, TEGRA_SATA_CONFIG_BASE,
+					pg_restore_bar5_bkdr_port_registers,
+					regs);
+	}
+	scfg_writel(SATA0_NONE_SELECTED, T_SATA0_INDEX_OFFSET);
+
+	/* program class code and programming interface for AHCI */
+	val = scfg_readl(TEGRA_PRIVATE_AHCI_CC_BKDR_OVERRIDE);
+	val |= TEGRA_PRIVATE_AHCI_CC_BKDR_OVERRIDE_EN;
+	scfg_writel(val, TEGRA_PRIVATE_AHCI_CC_BKDR_OVERRIDE);
+	scfg_writel(TEGRA_PRIVATE_AHCI_CC_BKDR_PGM, TEGRA_PRIVATE_AHCI_CC_BKDR);
+	val &= ~TEGRA_PRIVATE_AHCI_CC_BKDR_OVERRIDE_EN;
+	scfg_writel(val, TEGRA_PRIVATE_AHCI_CC_BKDR_OVERRIDE);
+}
+
+static u32 tegra_ahci_port_error(struct ata_port *ap)
+{
+	void __iomem *port_mmio = ahci_port_base(ap);
+	u32 err_status;
+
+	err_status = readl(port_mmio + PORT_IRQ_STAT);
+	/* excludes PhyRdy and Connect Change status */
+	err_status &= (PORT_IRQ_ERROR & (~(PORT_IRQ_PHYRDY|PORT_IRQ_CONNECT)));
+	return err_status;
+}
+
+static bool tegra_ahci_check_errors(struct ata_host *host)
+{	int i;
+	struct ata_port *ap;
+	u32 err;
+
+	for (i = 0; i < host->n_ports; i++) {
+		ap = host->ports[i];
+		err = tegra_ahci_port_error(ap);
+		if (err) {
+			dev_err(host->dev,
+				"pg-chk-err = 0x%08x on port %d\n", err, i);
+			return true;
+		}
+	}
+	return false;
+}
+
+static void tegra_ahci_abort_power_gate(struct ata_host *host)
+{
+	u32 val;
+
+	val = pmc_readl(APB_PMC_SATA_PWRGT_0_REG);
+	val &= ~PG_INFO_MASK;
+	val |= PG_INFO_OFF;
+	pmc_writel(val, APB_PMC_SATA_PWRGT_0_REG);
+}
+
+static bool tegra_ahci_power_gate(struct ata_host *host)
+{
+	u32 val;
+	u32 dat;
+	struct tegra_ahci_host_priv *tegra_hpriv;
+	int status;
+
+	tegra_hpriv = (struct tegra_ahci_host_priv *)host->private_data;
+
+	val = pmc_readl(APB_PMC_SATA_PWRGT_0_REG);
+	val &= ~PG_INFO_MASK;
+	val |= PG_INFO_ON;
+	pmc_writel(val, APB_PMC_SATA_PWRGT_0_REG);
+
+	tegra_ahci_pg_save_registers(host);
+
+	/*
+	 * Read SATA_AUX_MISC_CNTL_1_0 register L0_RX_IDLE_T_SAX field and
+	 * write that value into same register L0_RX_IDLE_T_NPG field.
+	 * And write 1 to L0_RX_IDLE_T_MUX field.
+	 */
+	val = misc_readl(SATA_AUX_MISC_CNTL_1_REG);
+	dat = val;
+	dat &= L0_RX_IDLE_T_SAX_MASK;
+	dat >>= L0_RX_IDLE_T_SAX_SHIFT;
+	dat <<= L0_RX_IDLE_T_NPG_SHIFT;
+	val &= ~L0_RX_IDLE_T_NPG_MASK;
+	val |= dat;
+	val &= ~L0_RX_IDLE_T_MUX_MASK;
+	val |= L0_RX_IDLE_T_MUX_FROM_APB_MISC;
+	misc_writel(val, SATA_AUX_MISC_CNTL_1_REG);
+
+	/* abort PG if there are errors occurred */
+	if (tegra_ahci_check_errors(host)) {
+		dev_err(host->dev, "** pg: errors; abort power gating **\n");
+		return false;
+	}
+	/* make sure all ports have no outstanding commands and are idle. */
+	if (!tegra_ahci_are_all_ports_idle(host)) {
+		dev_err(host->dev, "** pg: cmds; abort power gating **\n");
+		return false;
+	}
+
+	/*
+	 * Hw wake up is not needed:
+	 * Driver/RM shall place the SATA PHY and SATA PADPLL in IDDQ.
+	 * SATA_PADPLL_RESET_SWCTL =1
+	 * SATA_PADPLL_RESET_OVERRIDE_VALUE=1
+	 * SATA_PADPHY_IDDQ_SWCTL=1
+	 * SATA_PADPHY_IDDQ_OVERRIDE_VALUE=1
+	 */
+	val = clk_readl(CLK_RST_SATA_PLL_CFG0_REG);
+	val &= ~(PADPLL_RESET_SWCTL_MASK | PADPLL_RESET_OVERRIDE_VALUE_MASK);
+	val |= (PADPLL_RESET_SWCTL_ON | PADPLL_RESET_OVERRIDE_VALUE_ON);
+	clk_writel(val, CLK_RST_SATA_PLL_CFG0_REG);
+	val = pmc_readl(APB_PMC_SATA_PWRGT_0_REG);
+	val &= ~(PADPHY_IDDQ_OVERRIDE_VALUE_MASK | PADPHY_IDDQ_SWCTL_MASK);
+	val |= (PADPHY_IDDQ_SWCTL_ON | PADPHY_IDDQ_OVERRIDE_VALUE_ON);
+	pmc_writel(val, APB_PMC_SATA_PWRGT_0_REG);
+
+	/* Wait for time specified in SATA_LANE_IDDQ2_PADPLL_IDDQ */
+	val = clk_readl(CLK_RST_SATA_PLL_CFG1_REG);
+	dat = (val & IDDQ2LANE_IDDQ_DLY_MASK) >> IDDQ2LANE_IDDQ_DLY_SHIFT;
+	udelay(dat);
+
+	/* SATA_PADPLL_IDDQ_SWCTL=1 & SATA_PADPLL_IDDQ_OVERRIDE_VALUE=1 */
+	val = pmc_readl(APB_PMC_SATA_PWRGT_0_REG);
+	val &= ~(PADPLL_IDDQ_OVERRIDE_VALUE_MASK | PADPLL_IDDQ_SWCTL_MASK);
+	val |= (PADPLL_IDDQ_SWCTL_ON | PADPLL_IDDQ_OVERRIDE_VALUE_ON);
+	pmc_writel(val, APB_PMC_SATA_PWRGT_0_REG);
+
+	/* power off the sata */
+	status = tegra_powergate_partition_with_clk_off(TEGRA_POWERGATE_SATA);
+	if (status)
+		dev_err(host->dev, "** failed to turn-off SATA (0x%x) **\n",
+				   status);
+
+	return true;
+}
+
+static bool tegra_ahci_power_un_gate(struct ata_host *host)
+{
+	u32 val;
+	u32 dat;
+	u32 timeout;
+	struct tegra_ahci_host_priv *tegra_hpriv;
+	int status;
+
+	tegra_hpriv = (struct tegra_ahci_host_priv *)host->private_data;
+
+	/* get sata phy and pll out of iddq: */
+	val = pmc_readl(APB_PMC_SATA_PWRGT_0_REG);
+	val &= ~PADPLL_IDDQ_OVERRIDE_VALUE_MASK;
+	val |= PADPLL_IDDQ_OVERRIDE_VALUE_OFF;
+	pmc_writel(val, APB_PMC_SATA_PWRGT_0_REG);
+	/* wait for delay of IDDQ2LAND_SLUMBER_DLY */
+	val = clk_readl(CLK_RST_SATA_PLL_CFG1_REG);
+	dat = (val & IDDQ2LANE_SLUMBER_DLY_MASK) >> IDDQ2LANE_SLUMBER_DLY_SHIFT;
+	udelay(dat);
+	val = pmc_readl(APB_PMC_SATA_PWRGT_0_REG);
+	val &= ~PADPHY_IDDQ_OVERRIDE_VALUE_MASK;
+	val |= PADPHY_IDDQ_OVERRIDE_VALUE_OFF;
+	pmc_writel(val, APB_PMC_SATA_PWRGT_0_REG);
+
+	status = tegra_unpowergate_partition_with_clk_on(TEGRA_POWERGATE_SATA);
+	if (status)
+		dev_err(host->dev, "** failed to turn-on SATA (0x%x) **\n",
+				   status);
+
+	/* deasset PADPLL and wait until it locks. */
+	val = clk_readl(CLK_RST_SATA_PLL_CFG0_REG);
+	val &= ~PADPLL_RESET_OVERRIDE_VALUE_MASK;
+	val |= PADPLL_RESET_OVERRIDE_VALUE_OFF;
+	clk_writel(val, CLK_RST_SATA_PLL_CFG0_REG);
+
+	/*
+	 * Wait for SATA_AUX_PAD_PLL_CNTL_1_0_LOCKDET to turn 1 with a timeout
+	 * of 15 us.
+	 */
+	timeout = 15;
+	while (timeout--) {
+		udelay(1);
+		val = misc_readl(SATA_AUX_PAD_PLL_CNTL_1_REG);
+		if (val & LOCKDET_FIELD)
+			break;
+	}
+	if (timeout == 0)
+		pr_err("%s: SATA_PAD_PLL is not locked in 15us.\n", __func__);
+
+	/* restore registers */
+	tegra_ahci_pg_restore_registers(host);
+
+	/*
+	 * During the restoration of the registers, the driver would now need to
+	 * restore the register T_SATA0_CFG_POWER_GATE_SSTS_RESTORED after the
+	 * ssts_det, ssts_spd are restored. This register is used to tell the
+	 * controller whether a drive existed earlier or not and move the PHY
+	 * state machines into either HR_slumber or not.
+	 */
+	val = scfg_readl(T_SATA0_CFG_POWER_GATE);
+	val &= ~POWER_GATE_SSTS_RESTORED_MASK;
+	val |= POWER_GATE_SSTS_RESTORED_YES;
+	scfg_writel(val, T_SATA0_CFG_POWER_GATE);
+
+	/*
+	 * Driver needs to switch the rx_idle_t driven source back to from
+	 * Sata controller after SAX is power-ungated.
+	 */
+	val = misc_readl(SATA_AUX_MISC_CNTL_1_REG);
+	val &= ~L0_RX_IDLE_T_MUX_MASK;
+	val |= L0_RX_IDLE_T_MUX_FROM_SATA;
+	misc_writel(val, SATA_AUX_MISC_CNTL_1_REG);
+
+	/*
+	 * Driver can start to use main SATA interrupt instead of the
+	 * rx_stat_t interrupt.
+	 */
+	val = pictlr_readl(PRI_ICTLR_CPU_IER_SET_0_OFFSET);
+	val |= CPU_IER_SATA_CTL;
+	pictlr_writel(val, PRI_ICTLR_CPU_IER_SET_0_OFFSET);
+
+	/* Set the bits in the CAR to allow HW based low power sequencing. */
+	val = clk_readl(CLK_RST_SATA_PLL_CFG0_REG);
+	val &= ~PADPLL_RESET_SWCTL_MASK;
+	val |= PADPLL_RESET_SWCTL_OFF;
+	clk_writel(val, CLK_RST_SATA_PLL_CFG0_REG);
+
+	/*
+	 * power un-gating process is complete by clearing
+	 * APBDEV_PMC_SATA_PWRGT_0.Pmc2sata_pg_info = 0
+	 */
+	val = pmc_readl(APB_PMC_SATA_PWRGT_0_REG);
+	val &= ~PG_INFO_MASK;
+	val |= PG_INFO_OFF;
+	pmc_writel(val, APB_PMC_SATA_PWRGT_0_REG);
+
+	return true;
+}
+
+static enum port_idle_status tegra_ahci_is_port_idle(struct ata_port *ap)
+{
+	void __iomem *port_mmio = ahci_port_base(ap);
+
+	if (readl(port_mmio + PORT_CMD_ISSUE) ||
+	    readl(port_mmio + PORT_SCR_ACT))
+		return PORT_IS_NOT_IDLE;
+	return PORT_IS_IDLE;
+}
+
+/* check if all supported ports are idle (no outstanding commands) */
+static bool tegra_ahci_are_all_ports_idle(struct ata_host *host)
+{	int i;
+	struct ata_port *ap;
+
+	for (i = 0; i < host->n_ports; i++) {
+		ap = host->ports[i];
+		if (ap && (tegra_ahci_is_port_idle(ap) == PORT_IS_NOT_IDLE))
+			return false;
+	}
+	return true;
+}
+
+#ifdef CONFIG_TEGRA_SATA_IDLE_POWERGATE
+static enum port_idle_status tegra_ahci_is_port_slumber(struct ata_port *ap)
+{
+	void __iomem *port_mmio = ahci_port_base(ap);
+	u32 sstat;
+
+	if (tegra_ahci_is_port_idle(ap) == PORT_IS_NOT_IDLE)
+		return PORT_IS_NOT_IDLE;
+
+	/* return 1 if PORT_SCR_STAT is in IPM_SLUMBER_STATE */
+	sstat = readl(port_mmio + PORT_SCR_STAT);
+	if ((sstat & SSTAT_IPM_STATE_MASK) == SSTAT_IPM_SLUMBER_STATE)
+		return PORT_IS_SLUMBER;
+	return PORT_IS_IDLE_NOT_SLUMBER;
+}
+
+/* check if all supported ports are in slumber */
+static bool tegra_ahci_are_all_ports_slumber(struct ata_host *host)
+{	int i;
+	struct ata_port *ap;
+
+	for (i = 0; i < host->n_ports; i++) {
+		ap = host->ports[i];
+		if (ap && (tegra_ahci_is_port_slumber(ap) != PORT_IS_SLUMBER))
+			return false;
+	}
+	return true;
+}
+
+static void tegra_ahci_to_add_idle_timer(struct ata_host *host)
+{
+	struct tegra_ahci_host_priv *tegra_hpriv;
+
+	tegra_hpriv = (struct tegra_ahci_host_priv *)host->private_data;
+
+	/* note: the routine is called from interrupt context */
+	spin_lock(&host->lock);
+	/* start idle-timer if all ports have no outstanding commands */
+	if (tegra_ahci_are_all_ports_idle(host)) {
+		/* adjust tegra_ahci_idle_time to minimum if it is too small */
+		tegra_ahci_idle_time = max((u32)TEGRA_AHCI_MIN_IDLE_TIME,
+					   tegra_ahci_idle_time);
+		tegra_hpriv->idle_timer.expires =
+			ata_deadline(jiffies, tegra_ahci_idle_time);
+		mod_timer(&tegra_hpriv->idle_timer,
+			  tegra_hpriv->idle_timer.expires);
+	}
+	spin_unlock(&host->lock);
+}
+
+static void tegra_ahci_idle_timer(unsigned long arg)
+{
+	struct ata_host *host = (void *)arg;
+	struct tegra_ahci_host_priv *tegra_hpriv;
+	unsigned long flags;
+
+	tegra_hpriv = (struct tegra_ahci_host_priv *)host->private_data;
+
+	spin_lock_irqsave(&host->lock, flags);
+	if (tegra_hpriv->pg_state == SATA_ON)
+		tegra_hpriv->pg_state = SATA_GOING_OFF;
+	else {
+		dev_err(host->dev, "idle_timer: bad state (%u)\n",
+				tegra_hpriv->pg_state);
+		WARN_ON(1);
+		spin_unlock_irqrestore(&host->lock, flags);
+		return;
+	}
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	pm_runtime_put(tegra_hpriv->dev);
+}
+
+static int tegra_ahci_queue_one_qc(struct tegra_ahci_host_priv *tegra_hpriv,
+				   struct ata_queued_cmd *qc)
+{
+	struct tegra_qc_list *qc_list;
+
+	qc_list = kmalloc(sizeof(struct tegra_qc_list), GFP_ATOMIC);
+	if (!qc_list) {
+		dev_err(tegra_hpriv->dev, "failed to alloc qc_list\n");
+		return AC_ERR_SYSTEM;
+	}
+	qc_list->qc = qc;
+	list_add_tail(&(qc_list->list), &(tegra_hpriv->qc_list));
+	dev_dbg(tegra_hpriv->dev, "queuing qc=%x\n", (unsigned int)qc);
+	return 0;
+}
+
+static void tegra_ahci_dequeue_qcs(struct tegra_ahci_host_priv *tegra_hpriv)
+{
+	struct list_head *list, *next;
+	struct tegra_qc_list *qc_list;
+	struct ata_queued_cmd *qc;
+
+	/* now qc_issue all qcs in the qc_list */
+	list_for_each_safe(list, next, &tegra_hpriv->qc_list) {
+		qc_list = list_entry(list, struct tegra_qc_list, list);
+		qc = qc_list->qc;
+		dev_dbg(tegra_hpriv->dev, "dequeue qc=%x\n", (unsigned int)qc);
+		ahci_ops.qc_issue(qc);
+		list_del(list);
+		kfree(qc_list);
+	}
+}
+
+static unsigned int tegra_ahci_qc_issue(struct ata_queued_cmd *qc)
+{
+	struct ata_port *ap = qc->ap;
+	struct ata_host *host = ap->host;
+	struct tegra_ahci_host_priv *tegra_hpriv = host->private_data;
+	int rc;
+
+	/* stop the idle timer */
+	if (timer_pending(&tegra_hpriv->idle_timer))
+		del_timer_sync(&tegra_hpriv->idle_timer);
+
+	/* note: host->lock is locked */
+	switch (tegra_hpriv->pg_state) {
+	case SATA_ON:
+		/* normal case, issue the qc */
+		return ahci_ops.qc_issue(qc);
+	case SATA_GOING_OFF:
+	case SATA_ABORT_OFF:
+		/* SATA is going OFF, let's abort the suspend */
+		dev_dbg(host->dev, "** qc_issue: going OFF **\n");
+		tegra_hpriv->pg_state = SATA_ABORT_OFF;
+		return tegra_ahci_queue_one_qc(tegra_hpriv, qc);
+	case SATA_OFF:
+		dev_dbg(host->dev, "** qc_issue: request power-up sata **\n");
+		rc = pm_runtime_get(tegra_hpriv->dev);
+		/* rc == 0 means the request has been queued successfully */
+		if (rc) {
+			dev_err(host->dev, "** qc_issue: rt_get()=%d **\n",
+						rc);
+			WARN_ON(1);
+			return AC_ERR_SYSTEM;
+		}
+		tegra_hpriv->pg_state = SATA_GOING_ON;
+		/* continue with the following code to queue the qc */
+	case SATA_GOING_ON:
+		return tegra_ahci_queue_one_qc(tegra_hpriv, qc);
+	default:
+		dev_err(host->dev, "** qc_issue: bad state (%u) **\n",
+					tegra_hpriv->pg_state);
+		WARN_ON(1);
+		return AC_ERR_SYSTEM;
+	}
+}
+
+static int tegra_ahci_hardreset(struct ata_link *link, unsigned int *class,
+				unsigned long deadline)
+{
+	struct ata_port *ap = link->ap;
+	struct ata_host *host = ap->host;
+	struct tegra_ahci_host_priv *tegra_hpriv = host->private_data;
+	int rc;
+
+	if (tegra_hpriv->pg_state == SATA_OFF) {
+		dev_dbg(host->dev, "** hreset: request power-up sata **\n");
+		rc = pm_runtime_get_sync(tegra_hpriv->dev);
+		/* rc == 0 means the request has been run successfully */
+		if (rc) {
+			dev_err(host->dev, "** hreset: rt_get()=%d **\n", rc);
+			WARN_ON(1);
+			return AC_ERR_SYSTEM;
+		}
+		tegra_hpriv->pg_state = SATA_ON;
+	}
+
+	return ahci_ops.hardreset(link, class, deadline);
+}
+
+static irqreturn_t tegra_ahci_interrupt(int irq, void *dev_instance)
+{
+	irqreturn_t irq_retval;
+
+	irq_retval = ahci_interrupt(irq, dev_instance);
+	if (irq_retval == IRQ_NONE)
+		return IRQ_NONE;
+
+#ifdef CONFIG_PM
+	tegra_ahci_to_add_idle_timer((struct ata_host *)dev_instance);
+#endif
+
+	return irq_retval;
+}
+#endif
+#endif
+
+static int __devexit tegra_ahci_remove_one(struct platform_device *pdev)
+{
+	struct ata_host *host = dev_get_drvdata(&pdev->dev);
+	struct ahci_host_priv *hpriv;
+
+	BUG_ON(host == NULL);
+	BUG_ON(host->iomap[AHCI_PCI_BAR] == NULL);
+	hpriv = host->private_data;
+
+	tegra_ahci_controller_remove(pdev);
+
+	devm_iounmap(&pdev->dev, host->iomap[AHCI_PCI_BAR]);
+	devres_free(host);
+
+#ifdef CONFIG_PM
+	/* Free PG save/restore area */
+	devm_kfree(&pdev->dev, ((struct tegra_ahci_host_priv *)hpriv)->pg_save);
+
+#ifdef CONFIG_TEGRA_SATA_IDLE_POWERGATE
+	pm_runtime_disable(&pdev->dev);
+#endif
+#endif
+
+	devm_kfree(&pdev->dev, hpriv);
+
+	return 0;
+}
+
+static int __devinit tegra_ahci_init_one(struct platform_device *pdev)
+{
+	struct ata_port_info pi = ahci_port_info;
+	const struct ata_port_info *ppi[] = { &pi, NULL };
+	struct device *dev = &pdev->dev;
+	struct ahci_host_priv *hpriv = NULL;
+	struct tegra_ahci_host_priv *tegra_hpriv;
+	struct ata_host *host = NULL;
+	int n_ports, i, rc;
+	struct resource *res, *irq_res;
+	void __iomem *mmio;
+	u32 save_size;
+	irq_handler_t irq_handler = ahci_interrupt;
+
+	VPRINTK("ENTER\n");
+
+	WARN_ON((int)ATA_MAX_QUEUE > AHCI_MAX_CMDS);
+
+	ata_print_version_once(&pdev->dev, DRV_VERSION);
+
+	/* Simple resource validation */
+	if (pdev->num_resources != 3) {
+		dev_err(dev, "invalid number of resources\n");
+		dev_err(dev, "not enough SATA resources\n");
+		return -EINVAL;
+	}
+
+	/* acquire bar resources */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL)
+		return -EINVAL;
+
+	/* acquire IRQ resource */
+	irq_res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (irq_res == NULL)
+		return -EINVAL;
+	if (irq_res->start <= 0)
+		return -EINVAL;
+
+	/* allocate sizeof tegra_ahci_host_priv, which contains extra fields */
+	hpriv = devm_kzalloc(dev, sizeof(struct tegra_ahci_host_priv),
+			     GFP_KERNEL);
+	if (!hpriv) {
+		rc = -ENOMEM;
+		goto fail;
+	}
+	hpriv->flags |= (unsigned long)pi.private_data;
+	tegra_hpriv = (struct tegra_ahci_host_priv *)hpriv;
+	g_tegra_hpriv = tegra_hpriv;
+
+	/* Call tegra init routine */
+	rc = tegra_ahci_controller_init(tegra_hpriv);
+	if (rc != 0) {
+		dev_err(dev, "TEGRA SATA init failed\n");
+		goto fail;
+	}
+
+	/*
+	 * We reserve a table of 6 BARs in tegra_hpriv to store BARs.
+	 * Save the mapped AHCI_PCI_BAR address to the table.
+	 */
+	mmio = devm_ioremap(dev, res->start, (res->end-res->start+1));
+	tegra_hpriv->bars_table[AHCI_PCI_BAR] = mmio;
+	hpriv->mmio = mmio;
+
+	/* save initial config */
+	tegra_ahci_save_initial_config(pdev, hpriv);
+	dev_dbg(dev, "past save init config\n");
+
+	/* prepare host */
+	if (hpriv->cap & HOST_CAP_NCQ) {
+		pi.flags |= ATA_FLAG_NCQ;
+		pi.flags |= ATA_FLAG_FPDMA_AA;
+	}
+
+	/*
+	 * CAP.NP sometimes indicate the index of the last enabled
+	 * port, at other times, that of the last possible port, so
+	 * determining the maximum port number requires looking at
+	 * both CAP.NP and port_map.
+	 */
+	n_ports = max(ahci_nr_ports(hpriv->cap), fls(hpriv->port_map));
+	host = ata_host_alloc_pinfo(dev, ppi, n_ports);
+	if (!host) {
+		rc = -ENOMEM;
+		goto fail;
+	}
+	host->private_data = hpriv;
+	tegra_hpriv->host = host;
+	tegra_hpriv->dev = dev;
+	host->iomap = tegra_hpriv->bars_table;
+
+	if (!(hpriv->cap & HOST_CAP_SSS))
+		host->flags |= ATA_HOST_PARALLEL_SCAN;
+	else
+		printk(KERN_INFO "ahci: SSS flag set, parallel bus scan disabled\n");
+
+	for (i = 0; i < host->n_ports; i++) {
+		struct ata_port *ap = host->ports[i];
+
+		/* set initial link pm policy */
+		ap->target_lpm_policy = ATA_LPM_UNKNOWN;
+
+		/* disabled/not-implemented port */
+		if (!(hpriv->port_map & (1 << i)))
+			ap->ops = &ata_dummy_port_ops;
+		else
+			ap->target_lpm_policy = ATA_LPM_MIN_POWER;
+	}
+
+	rc = ahci_reset_controller(host);
+	if (rc) {
+		dev_err(dev, "Reset controller failed! (rc=%d)\n", rc);
+		goto fail;
+	}
+
+	ahci_init_controller(host);
+	ahci_print_info(host, "TEGRA-SATA");
+	dev_dbg(dev, "controller init okay\n");
+
+#ifdef CONFIG_PM
+	/* Setup PG save/restore area: */
+
+	/* calculate the size */
+	save_size = ARRAY_SIZE(pg_save_ipfs_registers) +
+			    ARRAY_SIZE(pg_save_config_registers) +
+			    ARRAY_SIZE(pg_save_bar5_registers) +
+			    ARRAY_SIZE(pg_save_bar5_bkdr_registers);
+
+	/* and add save port_registers for all the ports */
+	save_size += TEGRA_AHCI_NUM_PORTS *
+		     (ARRAY_SIZE(pg_save_config_port_registers) +
+		      ARRAY_SIZE(pg_save_bar5_port_registers) +
+		      ARRAY_SIZE(pg_save_bar5_bkdr_port_registers));
+
+	/*
+	 * save_size is number of registers times number of bytes per
+	 * register to get total save size.
+	 */
+	save_size *= sizeof(u32);
+	tegra_hpriv->pg_save = devm_kzalloc(dev, save_size, GFP_KERNEL);
+	if (!tegra_hpriv->pg_save) {
+		rc = -ENOMEM;
+		goto fail;
+	}
+
+#ifdef CONFIG_TEGRA_SATA_IDLE_POWERGATE
+	pm_runtime_set_active(dev);
+	pm_suspend_ignore_children(dev, true);
+	pm_runtime_enable(dev);
+
+	tegra_hpriv->pg_state = SATA_ON;
+
+	/* setup sata idle timer */
+	init_timer_deferrable(&tegra_hpriv->idle_timer);
+	tegra_hpriv->idle_timer.function = tegra_ahci_idle_timer;
+	tegra_hpriv->idle_timer.data = (unsigned long)host;
+
+	INIT_LIST_HEAD(&tegra_hpriv->qc_list);
+
+	/* use our own irq handler */
+	irq_handler = tegra_ahci_interrupt;
+#endif
+
+#endif
+
+	rc = ata_host_activate(host, irq_res->start, irq_handler, 0, &ahci_sht);
+	if (rc == 0)
+		return 0;
+
+	/* Free PG save/restore area */
+	devm_kfree(dev, tegra_hpriv->pg_save);
+
+#ifdef CONFIG_TEGRA_SATA_IDLE_POWERGATE
+	pm_runtime_put(dev);
+	pm_runtime_enable(dev);
+#endif
+
+fail:
+	if (host) {
+		if (host->iomap[AHCI_PCI_BAR])
+			devm_iounmap(dev, host->iomap[AHCI_PCI_BAR]);
+		devres_free(host);
+	}
+	if (hpriv)
+		devm_kfree(dev, hpriv);
+
+	return rc;
+}
+
+static int __init ahci_init(void)
+{
+	return platform_driver_register(&tegra_platform_ahci_driver);
+}
+
+static void __exit ahci_exit(void)
+{
+	platform_driver_unregister(&tegra_platform_ahci_driver);
+}
+
+
+#ifdef	CONFIG_DEBUG_FS
+
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+
+static void dbg_ahci_dump_regs(struct seq_file *s, u32 *ptr, u32 base, u32 regs)
+{
+#define REGS_PER_LINE	4
+
+	u32 i, j;
+	u32 lines = regs / REGS_PER_LINE;
+
+	for (i = 0; i < lines; i++) {
+		seq_printf(s, "0x%08x: ", base+(i*16));
+		for (j = 0; j < REGS_PER_LINE; ++j) {
+			seq_printf(s, "0x%08x ", readl(ptr));
+			++ptr;
+		}
+		seq_printf(s, "\n");
+	}
+#undef REGS_PER_LINE
+}
+
+static int dbg_ahci_dump_show(struct seq_file *s, void *unused)
+{
+	u32 base;
+	u32 *ptr;
+	u32 i;
+
+	base = TEGRA_SATA_CONFIG_BASE;
+	ptr = (u32 *)IO_TO_VIRT(base);
+	seq_printf(s, "SATA CONFIG Registers:\n");
+	seq_printf(s, "----------------------\n");
+	dbg_ahci_dump_regs(s, ptr, base, 0x200);
+
+	base = TEGRA_SATA_BAR5_BASE;
+	ptr = (u32 *)IO_TO_VIRT(base);
+	seq_printf(s, "\nAHCI HBA Registers:\n");
+	seq_printf(s, "-------------------\n");
+	dbg_ahci_dump_regs(s, ptr, base, 64);
+
+	for (i = 0; i < TEGRA_AHCI_NUM_PORTS; ++i) {
+		base = TEGRA_SATA_BAR5_BASE + 0x100 + (0x80*i);
+		ptr = (u32 *)IO_TO_VIRT(base);
+		seq_printf(s, "\nPort %u Registers:\n", i);
+		seq_printf(s, "---------------\n");
+		dbg_ahci_dump_regs(s, ptr, base, 16);
+	}
+
+#ifdef	CONFIG_TEGRA_SATA_IDLE_POWERGATE
+	/* adjust tegra_ahci_idle_time to minimum if it is too small */
+	tegra_ahci_idle_time = max((u32)TEGRA_AHCI_MIN_IDLE_TIME,
+				   tegra_ahci_idle_time);
+	seq_printf(s, "\nIdle Timeout = %u milli-seconds.\n",
+		      tegra_ahci_idle_time);
+#endif
+
+	if (tegra_powergate_is_powered(TEGRA_POWERGATE_SATA))
+		seq_printf(s, "\n=== SATA controller is powered on ===\n\n");
+	else
+		seq_printf(s, "\n=== SATA controller is powered off ===\n\n");
+
+	return 0;
+}
+
+static int dbg_ahci_dump_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dbg_ahci_dump_show, &inode->i_private);
+}
+
+static const struct file_operations debug_fops = {
+	.open		= dbg_ahci_dump_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int __init tegra_ahci_dump_debuginit(void)
+{
+	(void) debugfs_create_file("tegra_ahci", S_IRUGO,
+				   NULL, NULL, &debug_fops);
+#ifdef	CONFIG_TEGRA_SATA_IDLE_POWERGATE
+	(void) debugfs_create_u32("tegra_ahci_idle_ms", S_IRWXUGO,
+				   NULL, &tegra_ahci_idle_time);
+#endif
+	return 0;
+}
+late_initcall(tegra_ahci_dump_debuginit);
+#endif
+
+MODULE_AUTHOR("NVIDIA");
+MODULE_DESCRIPTION("Tegra AHCI SATA low-level driver");
+MODULE_LICENSE("GPL v2");
+MODULE_VERSION(DRV_VERSION);
+
+module_init(ahci_init);
+module_exit(ahci_exit);
diff --git a/drivers/ata/ata_piix.c b/drivers/ata/ata_piix.c
index 43107e9415da..cc431d6bc97f 100644
--- a/drivers/ata/ata_piix.c
+++ b/drivers/ata/ata_piix.c
@@ -113,6 +113,8 @@ enum {
 	PIIX_PATA_FLAGS		= ATA_FLAG_SLAVE_POSS,
 	PIIX_SATA_FLAGS		= ATA_FLAG_SATA | PIIX_FLAG_CHECKINTR,
 
+	PIIX_FLAG_PIO16		= (1 << 30), /*support 16bit PIO only*/
+
 	PIIX_80C_PRI		= (1 << 5) | (1 << 4),
 	PIIX_80C_SEC		= (1 << 7) | (1 << 6),
 
@@ -147,6 +149,7 @@ enum piix_controller_ids {
 	ich8m_apple_sata,	/* locks up on second port enable */
 	tolapai_sata,
 	piix_pata_vmw,			/* PIIX4 for VMware, spurious DMA_ERR */
+	ich8_sata_snb,
 };
 
 struct piix_map_db {
@@ -177,6 +180,7 @@ static int piix_sidpr_scr_write(struct ata_link *link,
 static int piix_sidpr_set_lpm(struct ata_link *link, enum ata_lpm_policy policy,
 			      unsigned hints);
 static bool piix_irq_check(struct ata_port *ap);
+static int piix_port_start(struct ata_port *ap);
 #ifdef CONFIG_PM
 static int piix_pci_device_suspend(struct pci_dev *pdev, pm_message_t mesg);
 static int piix_pci_device_resume(struct pci_dev *pdev);
@@ -298,21 +302,21 @@ static const struct pci_device_id piix_pci_tbl[] = {
 	/* SATA Controller IDE (PCH) */
 	{ 0x8086, 0x3b2e, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_sata },
 	/* SATA Controller IDE (CPT) */
-	{ 0x8086, 0x1c00, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_sata },
+	{ 0x8086, 0x1c00, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_sata_snb },
 	/* SATA Controller IDE (CPT) */
-	{ 0x8086, 0x1c01, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_sata },
+	{ 0x8086, 0x1c01, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_sata_snb },
 	/* SATA Controller IDE (CPT) */
 	{ 0x8086, 0x1c08, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_2port_sata },
 	/* SATA Controller IDE (CPT) */
 	{ 0x8086, 0x1c09, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_2port_sata },
 	/* SATA Controller IDE (PBG) */
-	{ 0x8086, 0x1d00, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_sata },
+	{ 0x8086, 0x1d00, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_sata_snb },
 	/* SATA Controller IDE (PBG) */
 	{ 0x8086, 0x1d08, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_2port_sata },
 	/* SATA Controller IDE (Panther Point) */
-	{ 0x8086, 0x1e00, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_sata },
+	{ 0x8086, 0x1e00, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_sata_snb },
 	/* SATA Controller IDE (Panther Point) */
-	{ 0x8086, 0x1e01, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_sata },
+	{ 0x8086, 0x1e01, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_sata_snb },
 	/* SATA Controller IDE (Panther Point) */
 	{ 0x8086, 0x1e08, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_2port_sata },
 	/* SATA Controller IDE (Panther Point) */
@@ -338,6 +342,7 @@ static struct scsi_host_template piix_sht = {
 static struct ata_port_operations piix_sata_ops = {
 	.inherits		= &ata_bmdma32_port_ops,
 	.sff_irq_check		= piix_irq_check,
+	.port_start		= piix_port_start,
 };
 
 static struct ata_port_operations piix_pata_ops = {
@@ -478,6 +483,7 @@ static const struct piix_map_db *piix_map_db_table[] = {
 	[ich8_2port_sata]	= &ich8_2port_map_db,
 	[ich8m_apple_sata]	= &ich8m_apple_map_db,
 	[tolapai_sata]		= &tolapai_map_db,
+	[ich8_sata_snb]		= &ich8_map_db,
 };
 
 static struct ata_port_info piix_port_info[] = {
@@ -606,6 +612,19 @@ static struct ata_port_info piix_port_info[] = {
 		.port_ops	= &piix_vmw_ops,
 	},
 
+	/*
+	 * some Sandybridge chipsets have broken 32 mode up to now,
+	 * see https://bugzilla.kernel.org/show_bug.cgi?id=40592
+	 */
+	[ich8_sata_snb] =
+	{
+		.flags		= PIIX_SATA_FLAGS | PIIX_FLAG_SIDPR | PIIX_FLAG_PIO16,
+		.pio_mask	= ATA_PIO4,
+		.mwdma_mask	= ATA_MWDMA2,
+		.udma_mask	= ATA_UDMA6,
+		.port_ops	= &piix_sata_ops,
+	},
+
 };
 
 static struct pci_bits piix_enable_bits[] = {
@@ -649,6 +668,14 @@ static const struct ich_laptop ich_laptop[] = {
 	{ 0, }
 };
 
+static int piix_port_start(struct ata_port *ap)
+{
+	if (!(ap->flags & PIIX_FLAG_PIO16))
+		ap->pflags |= ATA_PFLAG_PIO32 | ATA_PFLAG_PIO32CHANGE;
+
+	return ata_bmdma_port_start(ap);
+}
+
 /**
  *	ich_pata_cable_detect - Probe host controller cable detect info
  *	@ap: Port for which cable detect info is desired
diff --git a/drivers/base/Kconfig b/drivers/base/Kconfig
index 21cf46f45245..8a0e87fd9064 100644
--- a/drivers/base/Kconfig
+++ b/drivers/base/Kconfig
@@ -174,4 +174,14 @@ config SYS_HYPERVISOR
 
 source "drivers/base/regmap/Kconfig"
 
+config DMA_SHARED_BUFFER
+	bool "Buffer framework to be shared between drivers"
+	default n
+	select ANON_INODES
+	help
+	  This option enables the framework for buffer-sharing between
+	  multiple drivers. A buffer is associated with a file using driver
+	  APIs extension; the file's descriptor can then be passed on to other
+	  driver.
+
 endmenu
diff --git a/drivers/base/Makefile b/drivers/base/Makefile
index 99a375ad2cc9..d0df046a70d8 100644
--- a/drivers/base/Makefile
+++ b/drivers/base/Makefile
@@ -8,6 +8,7 @@ obj-$(CONFIG_DEVTMPFS)	+= devtmpfs.o
 obj-y			+= power/
 obj-$(CONFIG_HAS_DMA)	+= dma-mapping.o
 obj-$(CONFIG_HAVE_GENERIC_DMA_COHERENT) += dma-coherent.o
+obj-$(CONFIG_DMA_SHARED_BUFFER) += dma-buf.o
 obj-$(CONFIG_ISA)	+= isa.o
 obj-$(CONFIG_FW_LOADER)	+= firmware_class.o
 obj-$(CONFIG_NUMA)	+= node.o
diff --git a/drivers/base/core.c b/drivers/base/core.c
index bc8729d603a7..78445f40c430 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -22,6 +22,7 @@
 #include <linux/kallsyms.h>
 #include <linux/mutex.h>
 #include <linux/async.h>
+#include <linux/pm_runtime.h>
 
 #include "base.h"
 #include "power/power.h"
@@ -1742,6 +1743,8 @@ void device_shutdown(void)
 		 */
 		list_del_init(&dev->kobj.entry);
 		spin_unlock(&devices_kset->list_lock);
+		/* Disable all device's runtime power management */
+		pm_runtime_disable(dev);
 
 		if (dev->bus && dev->bus->shutdown) {
 			dev_dbg(dev, "shutdown\n");
diff --git a/drivers/base/dma-buf.c b/drivers/base/dma-buf.c
new file mode 100644
index 000000000000..e38ad243b4bb
--- /dev/null
+++ b/drivers/base/dma-buf.c
@@ -0,0 +1,291 @@
+/*
+ * Framework for buffer objects that can be shared across devices/subsystems.
+ *
+ * Copyright(C) 2011 Linaro Limited. All rights reserved.
+ * Author: Sumit Semwal <sumit.semwal@ti.com>
+ *
+ * Many thanks to linaro-mm-sig list, and specially
+ * Arnd Bergmann <arnd@arndb.de>, Rob Clark <rob@ti.com> and
+ * Daniel Vetter <daniel@ffwll.ch> for their support in creation and
+ * refining of this idea.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/dma-buf.h>
+#include <linux/anon_inodes.h>
+#include <linux/export.h>
+
+static inline int is_dma_buf_file(struct file *);
+
+static int dma_buf_release(struct inode *inode, struct file *file)
+{
+	struct dma_buf *dmabuf;
+
+	if (!is_dma_buf_file(file))
+		return -EINVAL;
+
+	dmabuf = file->private_data;
+
+	dmabuf->ops->release(dmabuf);
+	kfree(dmabuf);
+	return 0;
+}
+
+static const struct file_operations dma_buf_fops = {
+	.release	= dma_buf_release,
+};
+
+/*
+ * is_dma_buf_file - Check if struct file* is associated with dma_buf
+ */
+static inline int is_dma_buf_file(struct file *file)
+{
+	return file->f_op == &dma_buf_fops;
+}
+
+/**
+ * dma_buf_export - Creates a new dma_buf, and associates an anon file
+ * with this buffer, so it can be exported.
+ * Also connect the allocator specific data and ops to the buffer.
+ *
+ * @priv:	[in]	Attach private data of allocator to this buffer
+ * @ops:	[in]	Attach allocator-defined dma buf ops to the new buffer.
+ * @size:	[in]	Size of the buffer
+ * @flags:	[in]	mode flags for the file.
+ *
+ * Returns, on success, a newly created dma_buf object, which wraps the
+ * supplied private data and operations for dma_buf_ops. On either missing
+ * ops, or error in allocating struct dma_buf, will return negative error.
+ *
+ */
+struct dma_buf *dma_buf_export(void *priv, struct dma_buf_ops *ops,
+				size_t size, int flags)
+{
+	struct dma_buf *dmabuf;
+	struct file *file;
+
+	if (WARN_ON(!priv || !ops
+			  || !ops->map_dma_buf
+			  || !ops->unmap_dma_buf
+			  || !ops->release)) {
+		return ERR_PTR(-EINVAL);
+	}
+
+	dmabuf = kzalloc(sizeof(struct dma_buf), GFP_KERNEL);
+	if (dmabuf == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	dmabuf->priv = priv;
+	dmabuf->ops = ops;
+	dmabuf->size = size;
+
+	file = anon_inode_getfile("dmabuf", &dma_buf_fops, dmabuf, flags);
+
+	dmabuf->file = file;
+
+	mutex_init(&dmabuf->lock);
+	INIT_LIST_HEAD(&dmabuf->attachments);
+
+	return dmabuf;
+}
+EXPORT_SYMBOL_GPL(dma_buf_export);
+
+
+/**
+ * dma_buf_fd - returns a file descriptor for the given dma_buf
+ * @dmabuf:	[in]	pointer to dma_buf for which fd is required.
+ *
+ * On success, returns an associated 'fd'. Else, returns error.
+ */
+int dma_buf_fd(struct dma_buf *dmabuf)
+{
+	int error, fd;
+
+	if (!dmabuf || !dmabuf->file)
+		return -EINVAL;
+
+	error = get_unused_fd();
+	if (error < 0)
+		return error;
+	fd = error;
+
+	fd_install(fd, dmabuf->file);
+
+	return fd;
+}
+EXPORT_SYMBOL_GPL(dma_buf_fd);
+
+/**
+ * dma_buf_get - returns the dma_buf structure related to an fd
+ * @fd:	[in]	fd associated with the dma_buf to be returned
+ *
+ * On success, returns the dma_buf structure associated with an fd; uses
+ * file's refcounting done by fget to increase refcount. returns ERR_PTR
+ * otherwise.
+ */
+struct dma_buf *dma_buf_get(int fd)
+{
+	struct file *file;
+
+	file = fget(fd);
+
+	if (!file)
+		return ERR_PTR(-EBADF);
+
+	if (!is_dma_buf_file(file)) {
+		fput(file);
+		return ERR_PTR(-EINVAL);
+	}
+
+	return file->private_data;
+}
+EXPORT_SYMBOL_GPL(dma_buf_get);
+
+/**
+ * dma_buf_put - decreases refcount of the buffer
+ * @dmabuf:	[in]	buffer to reduce refcount of
+ *
+ * Uses file's refcounting done implicitly by fput()
+ */
+void dma_buf_put(struct dma_buf *dmabuf)
+{
+	if (WARN_ON(!dmabuf || !dmabuf->file))
+		return;
+
+	fput(dmabuf->file);
+}
+EXPORT_SYMBOL_GPL(dma_buf_put);
+
+/**
+ * dma_buf_attach - Add the device to dma_buf's attachments list; optionally,
+ * calls attach() of dma_buf_ops to allow device-specific attach functionality
+ * @dmabuf:	[in]	buffer to attach device to.
+ * @dev:	[in]	device to be attached.
+ *
+ * Returns struct dma_buf_attachment * for this attachment; may return negative
+ * error codes.
+ *
+ */
+struct dma_buf_attachment *dma_buf_attach(struct dma_buf *dmabuf,
+					  struct device *dev)
+{
+	struct dma_buf_attachment *attach;
+	int ret;
+
+	if (WARN_ON(!dmabuf || !dev || !dmabuf->ops))
+		return ERR_PTR(-EINVAL);
+
+	attach = kzalloc(sizeof(struct dma_buf_attachment), GFP_KERNEL);
+	if (attach == NULL)
+		goto err_alloc;
+
+	mutex_lock(&dmabuf->lock);
+
+	attach->dev = dev;
+	attach->dmabuf = dmabuf;
+	if (dmabuf->ops->attach) {
+		ret = dmabuf->ops->attach(dmabuf, dev, attach);
+		if (ret)
+			goto err_attach;
+	}
+	list_add(&attach->node, &dmabuf->attachments);
+
+	mutex_unlock(&dmabuf->lock);
+	return attach;
+
+err_alloc:
+	return ERR_PTR(-ENOMEM);
+err_attach:
+	kfree(attach);
+	mutex_unlock(&dmabuf->lock);
+	return ERR_PTR(ret);
+}
+EXPORT_SYMBOL_GPL(dma_buf_attach);
+
+/**
+ * dma_buf_detach - Remove the given attachment from dmabuf's attachments list;
+ * optionally calls detach() of dma_buf_ops for device-specific detach
+ * @dmabuf:	[in]	buffer to detach from.
+ * @attach:	[in]	attachment to be detached; is free'd after this call.
+ *
+ */
+void dma_buf_detach(struct dma_buf *dmabuf, struct dma_buf_attachment *attach)
+{
+	if (WARN_ON(!dmabuf || !attach || !dmabuf->ops))
+		return;
+
+	mutex_lock(&dmabuf->lock);
+	list_del(&attach->node);
+	if (dmabuf->ops->detach)
+		dmabuf->ops->detach(dmabuf, attach);
+
+	mutex_unlock(&dmabuf->lock);
+	kfree(attach);
+}
+EXPORT_SYMBOL_GPL(dma_buf_detach);
+
+/**
+ * dma_buf_map_attachment - Returns the scatterlist table of the attachment;
+ * mapped into _device_ address space. Is a wrapper for map_dma_buf() of the
+ * dma_buf_ops.
+ * @attach:	[in]	attachment whose scatterlist is to be returned
+ * @direction:	[in]	direction of DMA transfer
+ *
+ * Returns sg_table containing the scatterlist to be returned; may return NULL
+ * or ERR_PTR.
+ *
+ */
+struct sg_table *dma_buf_map_attachment(struct dma_buf_attachment *attach,
+					enum dma_data_direction direction)
+{
+	struct sg_table *sg_table = ERR_PTR(-EINVAL);
+
+	might_sleep();
+
+	if (WARN_ON(!attach || !attach->dmabuf || !attach->dmabuf->ops))
+		return ERR_PTR(-EINVAL);
+
+	mutex_lock(&attach->dmabuf->lock);
+	if (attach->dmabuf->ops->map_dma_buf)
+		sg_table = attach->dmabuf->ops->map_dma_buf(attach, direction);
+	mutex_unlock(&attach->dmabuf->lock);
+
+	return sg_table;
+}
+EXPORT_SYMBOL_GPL(dma_buf_map_attachment);
+
+/**
+ * dma_buf_unmap_attachment - unmaps and decreases usecount of the buffer;might
+ * deallocate the scatterlist associated. Is a wrapper for unmap_dma_buf() of
+ * dma_buf_ops.
+ * @attach:	[in]	attachment to unmap buffer from
+ * @sg_table:	[in]	scatterlist info of the buffer to unmap
+ *
+ */
+void dma_buf_unmap_attachment(struct dma_buf_attachment *attach,
+				struct sg_table *sg_table)
+{
+	if (WARN_ON(!attach || !attach->dmabuf || !sg_table
+			    || !attach->dmabuf->ops))
+		return;
+
+	mutex_lock(&attach->dmabuf->lock);
+	if (attach->dmabuf->ops->unmap_dma_buf)
+		attach->dmabuf->ops->unmap_dma_buf(attach, sg_table);
+	mutex_unlock(&attach->dmabuf->lock);
+
+}
+EXPORT_SYMBOL_GPL(dma_buf_unmap_attachment);
diff --git a/drivers/base/firmware_class.c b/drivers/base/firmware_class.c
index 06ed6b4e7df5..3719c94be19c 100644
--- a/drivers/base/firmware_class.c
+++ b/drivers/base/firmware_class.c
@@ -226,13 +226,13 @@ static ssize_t firmware_loading_store(struct device *dev,
 	int loading = simple_strtol(buf, NULL, 10);
 	int i;
 
+	mutex_lock(&fw_lock);
+
+	if (!fw_priv->fw)
+		goto out;
+
 	switch (loading) {
 	case 1:
-		mutex_lock(&fw_lock);
-		if (!fw_priv->fw) {
-			mutex_unlock(&fw_lock);
-			break;
-		}
 		firmware_free_data(fw_priv->fw);
 		memset(fw_priv->fw, 0, sizeof(struct firmware));
 		/* If the pages are not owned by 'struct firmware' */
@@ -243,7 +243,6 @@ static ssize_t firmware_loading_store(struct device *dev,
 		fw_priv->page_array_size = 0;
 		fw_priv->nr_pages = 0;
 		set_bit(FW_STATUS_LOADING, &fw_priv->status);
-		mutex_unlock(&fw_lock);
 		break;
 	case 0:
 		if (test_bit(FW_STATUS_LOADING, &fw_priv->status)) {
@@ -274,7 +273,8 @@ static ssize_t firmware_loading_store(struct device *dev,
 		fw_load_abort(fw_priv);
 		break;
 	}
-
+out:
+	mutex_unlock(&fw_lock);
 	return count;
 }
 
diff --git a/drivers/base/node.c b/drivers/base/node.c
index 793f796c4da3..5693ecee9a40 100644
--- a/drivers/base/node.c
+++ b/drivers/base/node.c
@@ -127,12 +127,13 @@ static ssize_t node_read_meminfo(struct sys_device * dev,
 		       nid, K(node_page_state(nid, NR_WRITEBACK)),
 		       nid, K(node_page_state(nid, NR_FILE_PAGES)),
 		       nid, K(node_page_state(nid, NR_FILE_MAPPED)),
-		       nid, K(node_page_state(nid, NR_ANON_PAGES)
 #ifdef CONFIG_TRANSPARENT_HUGEPAGE
+		       nid, K(node_page_state(nid, NR_ANON_PAGES)
 			+ node_page_state(nid, NR_ANON_TRANSPARENT_HUGEPAGES) *
-			HPAGE_PMD_NR
+			HPAGE_PMD_NR),
+#else
+		       nid, K(node_page_state(nid, NR_ANON_PAGES)),
 #endif
-		       ),
 		       nid, K(node_page_state(nid, NR_SHMEM)),
 		       nid, node_page_state(nid, NR_KERNEL_STACK) *
 				THREAD_SIZE / 1024,
@@ -143,13 +144,14 @@ static ssize_t node_read_meminfo(struct sys_device * dev,
 		       nid, K(node_page_state(nid, NR_SLAB_RECLAIMABLE) +
 				node_page_state(nid, NR_SLAB_UNRECLAIMABLE)),
 		       nid, K(node_page_state(nid, NR_SLAB_RECLAIMABLE)),
-		       nid, K(node_page_state(nid, NR_SLAB_UNRECLAIMABLE))
 #ifdef CONFIG_TRANSPARENT_HUGEPAGE
+		       nid, K(node_page_state(nid, NR_SLAB_UNRECLAIMABLE))
 			, nid,
 			K(node_page_state(nid, NR_ANON_TRANSPARENT_HUGEPAGES) *
-			HPAGE_PMD_NR)
+			HPAGE_PMD_NR));
+#else
+		       nid, K(node_page_state(nid, NR_SLAB_UNRECLAIMABLE)));
 #endif
-		       );
 	n += hugetlb_report_node_meminfo(nid, buf + n);
 	return n;
 }
diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index a85459126bc6..7b4b78a6e82e 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -27,6 +27,7 @@
 #include <linux/sched.h>
 #include <linux/async.h>
 #include <linux/suspend.h>
+#include <linux/timer.h>
 
 #include "../base.h"
 #include "power.h"
@@ -49,6 +50,12 @@ LIST_HEAD(dpm_noirq_list);
 static DEFINE_MUTEX(dpm_list_mtx);
 static pm_message_t pm_transition;
 
+static void dpm_drv_timeout(unsigned long data);
+struct dpm_drv_wd_data {
+	struct device *dev;
+	struct task_struct *tsk;
+};
+
 static int async_error;
 
 /**
@@ -592,6 +599,30 @@ static bool is_async(struct device *dev)
 }
 
 /**
+ *	dpm_drv_timeout - Driver suspend / resume watchdog handler
+ *	@data: struct device which timed out
+ *
+ * 	Called when a driver has timed out suspending or resuming.
+ * 	There's not much we can do here to recover so
+ * 	BUG() out for a crash-dump
+ *
+ */
+static void dpm_drv_timeout(unsigned long data)
+{
+	struct dpm_drv_wd_data *wd_data = (void *)data;
+	struct device *dev = wd_data->dev;
+	struct task_struct *tsk = wd_data->tsk;
+
+	printk(KERN_EMERG "**** DPM device timeout: %s (%s)\n", dev_name(dev),
+	       (dev->driver ? dev->driver->name : "no driver"));
+
+	printk(KERN_EMERG "dpm suspend stack:\n");
+	show_stack(tsk, NULL);
+
+	BUG();
+}
+
+/**
  * dpm_resume - Execute "resume" callbacks for non-sysdev devices.
  * @state: PM transition of the system being carried out.
  *
@@ -849,9 +880,19 @@ static int legacy_suspend(struct device *dev, pm_message_t state,
 static int __device_suspend(struct device *dev, pm_message_t state, bool async)
 {
 	int error = 0;
+	struct timer_list timer;
+	struct dpm_drv_wd_data data;
 
 	dpm_wait_for_children(dev, async);
 
+	data.dev = dev;
+	data.tsk = get_current();
+	init_timer_on_stack(&timer);
+	timer.expires = jiffies + HZ * 12;
+	timer.function = dpm_drv_timeout;
+	timer.data = (unsigned long)&data;
+	add_timer(&timer);
+
 	if (async_error)
 		return 0;
 
@@ -905,6 +946,10 @@ static int __device_suspend(struct device *dev, pm_message_t state, bool async)
 	dev->power.is_suspended = !error;
 
 	device_unlock(dev);
+
+	del_timer_sync(&timer);
+	destroy_timer_on_stack(&timer);
+
 	complete_all(&dev->power.completion);
 
 	if (error) {
diff --git a/drivers/base/power/runtime.c b/drivers/base/power/runtime.c
index acb3f83b8079..37489733cb9d 100644
--- a/drivers/base/power/runtime.c
+++ b/drivers/base/power/runtime.c
@@ -28,13 +28,10 @@ static int rpm_suspend(struct device *dev, int rpmflags);
 void update_pm_runtime_accounting(struct device *dev)
 {
 	unsigned long now = jiffies;
-	int delta;
+	unsigned long delta;
 
 	delta = now - dev->power.accounting_timestamp;
 
-	if (delta < 0)
-		delta = 0;
-
 	dev->power.accounting_timestamp = now;
 
 	if (dev->power.disable_depth > 0)
@@ -285,6 +282,9 @@ static int rpm_callback(int (*cb)(struct device *), struct device *dev)
  * If a deferred resume was requested while the callback was running then carry
  * it out; otherwise send an idle notification for the device (if the suspend
  * failed) or for its parent (if the suspend succeeded).
+ * If ->runtime_suspend failed with -EAGAIN or -EBUSY, and if the RPM_AUTO
+ * flag is set and the next autosuspend-delay expiration time is in the
+ * future, schedule another autosuspend attempt.
  *
  * This function must be called under dev->power.lock with interrupts disabled.
  */
@@ -396,10 +396,23 @@ static int rpm_suspend(struct device *dev, int rpmflags)
 	if (retval) {
 		__update_runtime_status(dev, RPM_ACTIVE);
 		dev->power.deferred_resume = false;
-		if (retval == -EAGAIN || retval == -EBUSY)
+		if (retval == -EAGAIN || retval == -EBUSY) {
 			dev->power.runtime_error = 0;
-		else
+
+			/*
+			 * If the callback routine failed an autosuspend, and
+			 * if the last_busy time has been updated so that there
+			 * is a new autosuspend expiration time, automatically
+			 * reschedule another autosuspend.
+			 */
+			if ((rpmflags & RPM_AUTO) &&
+			    pm_runtime_autosuspend_expiration(dev) != 0) {
+				wake_up_all(&dev->power.wait_queue);
+				goto repeat;
+			}
+		} else {
 			pm_runtime_cancel_pending(dev);
+		}
 	} else {
  no_callback:
 		__update_runtime_status(dev, RPM_SUSPENDED);
@@ -739,6 +752,8 @@ int __pm_runtime_idle(struct device *dev, int rpmflags)
 	unsigned long flags;
 	int retval;
 
+	might_sleep_if(!(rpmflags & RPM_ASYNC));
+
 	if (rpmflags & RPM_GET_PUT) {
 		if (!atomic_dec_and_test(&dev->power.usage_count))
 			return 0;
@@ -768,6 +783,8 @@ int __pm_runtime_suspend(struct device *dev, int rpmflags)
 	unsigned long flags;
 	int retval;
 
+	might_sleep_if(!(rpmflags & RPM_ASYNC) && !dev->power.irq_safe);
+
 	if (rpmflags & RPM_GET_PUT) {
 		if (!atomic_dec_and_test(&dev->power.usage_count))
 			return 0;
@@ -796,6 +813,8 @@ int __pm_runtime_resume(struct device *dev, int rpmflags)
 	unsigned long flags;
 	int retval;
 
+	might_sleep_if(!(rpmflags & RPM_ASYNC) && !dev->power.irq_safe);
+
 	if (rpmflags & RPM_GET_PUT)
 		atomic_inc(&dev->power.usage_count);
 
@@ -985,6 +1004,7 @@ EXPORT_SYMBOL_GPL(pm_runtime_barrier);
  */
 void __pm_runtime_disable(struct device *dev, bool check_resume)
 {
+	might_sleep();
 	spin_lock_irq(&dev->power.lock);
 
 	if (dev->power.disable_depth > 0) {
@@ -1170,6 +1190,8 @@ void pm_runtime_set_autosuspend_delay(struct device *dev, int delay)
 {
 	int old_delay, old_use;
 
+	might_sleep();
+
 	spin_lock_irq(&dev->power.lock);
 	old_delay = dev->power.autosuspend_delay;
 	old_use = dev->power.use_autosuspend;
diff --git a/drivers/base/regmap/Kconfig b/drivers/base/regmap/Kconfig
index fabbf6cc5367..9ef0a5326f17 100644
--- a/drivers/base/regmap/Kconfig
+++ b/drivers/base/regmap/Kconfig
@@ -4,6 +4,8 @@
 
 config REGMAP
 	default y if (REGMAP_I2C || REGMAP_SPI)
+	select LZO_COMPRESS
+	select LZO_DECOMPRESS
 	bool
 
 config REGMAP_I2C
@@ -11,3 +13,9 @@ config REGMAP_I2C
 
 config REGMAP_SPI
 	tristate
+
+config REGMAP_MMIO
+	tristate
+
+config REGMAP_IRQ
+	bool
diff --git a/drivers/base/regmap/Makefile b/drivers/base/regmap/Makefile
index f476f4571295..5e75d1b683e2 100644
--- a/drivers/base/regmap/Makefile
+++ b/drivers/base/regmap/Makefile
@@ -1,3 +1,7 @@
-obj-$(CONFIG_REGMAP) += regmap.o
+obj-$(CONFIG_REGMAP) += regmap.o regcache.o
+obj-$(CONFIG_REGMAP) += regcache-rbtree.o regcache-lzo.o
+obj-$(CONFIG_DEBUG_FS) += regmap-debugfs.o
 obj-$(CONFIG_REGMAP_I2C) += regmap-i2c.o
 obj-$(CONFIG_REGMAP_SPI) += regmap-spi.o
+obj-$(CONFIG_REGMAP_MMIO) += regmap-mmio.o
+obj-$(CONFIG_REGMAP_IRQ) += regmap-irq.o
diff --git a/drivers/base/regmap/internal.h b/drivers/base/regmap/internal.h
new file mode 100644
index 000000000000..d92e9b1cb83c
--- /dev/null
+++ b/drivers/base/regmap/internal.h
@@ -0,0 +1,142 @@
+/*
+ * Register map access API internal header
+ *
+ * Copyright 2011 Wolfson Microelectronics plc
+ *
+ * Author: Mark Brown <broonie@opensource.wolfsonmicro.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _REGMAP_INTERNAL_H
+#define _REGMAP_INTERNAL_H
+
+#include <linux/regmap.h>
+#include <linux/fs.h>
+
+struct regmap;
+struct regcache_ops;
+
+struct regmap_format {
+	size_t buf_size;
+	size_t reg_bytes;
+	size_t pad_bytes;
+	size_t val_bytes;
+	void (*format_write)(struct regmap *map,
+			     unsigned int reg, unsigned int val);
+	void (*format_reg)(void *buf, unsigned int reg, unsigned int shift);
+	void (*format_val)(void *buf, unsigned int val, unsigned int shift);
+	unsigned int (*parse_val)(void *buf);
+};
+
+typedef void (*regmap_lock)(struct regmap *map);
+typedef void (*regmap_unlock)(struct regmap *map);
+
+struct regmap {
+	struct mutex mutex;
+	spinlock_t spinlock;
+	regmap_lock lock;
+	regmap_unlock unlock;
+
+	struct device *dev; /* Device we do I/O on */
+	void *work_buf;     /* Scratch buffer used to format I/O */
+	struct regmap_format format;  /* Buffer format */
+	const struct regmap_bus *bus;
+	void *bus_context;
+
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *debugfs;
+	const char *debugfs_name;
+#endif
+
+	unsigned int max_register;
+	bool (*writeable_reg)(struct device *dev, unsigned int reg);
+	bool (*readable_reg)(struct device *dev, unsigned int reg);
+	bool (*volatile_reg)(struct device *dev, unsigned int reg);
+	bool (*precious_reg)(struct device *dev, unsigned int reg);
+
+	u8 read_flag_mask;
+	u8 write_flag_mask;
+
+	/* number of bits to (left) shift the reg value when formatting*/
+	int reg_shift;
+	int reg_stride;
+
+	/* regcache specific members */
+	const struct regcache_ops *cache_ops;
+	enum regcache_type cache_type;
+
+	/* number of bytes in reg_defaults_raw */
+	unsigned int cache_size_raw;
+	/* number of bytes per word in reg_defaults_raw */
+	unsigned int cache_word_size;
+	/* number of entries in reg_defaults */
+	unsigned int num_reg_defaults;
+	/* number of entries in reg_defaults_raw */
+	unsigned int num_reg_defaults_raw;
+
+	/* if set, only the cache is modified not the HW */
+	u32 cache_only;
+	/* if set, only the HW is modified not the cache */
+	u32 cache_bypass;
+	/* if set, remember to free reg_defaults_raw */
+	bool cache_free;
+
+	struct reg_default *reg_defaults;
+	const void *reg_defaults_raw;
+	void *cache;
+	u32 cache_dirty;
+
+	struct reg_default *patch;
+	int patch_regs;
+};
+
+struct regcache_ops {
+	const char *name;
+	enum regcache_type type;
+	int (*init)(struct regmap *map);
+	int (*exit)(struct regmap *map);
+	int (*read)(struct regmap *map, unsigned int reg, unsigned int *value);
+	int (*write)(struct regmap *map, unsigned int reg, unsigned int value);
+	int (*sync)(struct regmap *map, unsigned int min, unsigned int max);
+};
+
+bool regmap_writeable(struct regmap *map, unsigned int reg);
+bool regmap_readable(struct regmap *map, unsigned int reg);
+bool regmap_volatile(struct regmap *map, unsigned int reg);
+bool regmap_precious(struct regmap *map, unsigned int reg);
+
+int _regmap_write(struct regmap *map, unsigned int reg,
+		  unsigned int val);
+
+#ifdef CONFIG_DEBUG_FS
+extern void regmap_debugfs_initcall(void);
+extern void regmap_debugfs_init(struct regmap *map, const char *name);
+extern void regmap_debugfs_exit(struct regmap *map);
+#else
+static inline void regmap_debugfs_initcall(void) { }
+static inline void regmap_debugfs_init(struct regmap *map, const char *name) { }
+static inline void regmap_debugfs_exit(struct regmap *map) { }
+#endif
+
+/* regcache core declarations */
+int regcache_init(struct regmap *map, const struct regmap_config *config);
+void regcache_exit(struct regmap *map);
+int regcache_read(struct regmap *map,
+		       unsigned int reg, unsigned int *value);
+int regcache_write(struct regmap *map,
+			unsigned int reg, unsigned int value);
+int regcache_sync(struct regmap *map);
+
+unsigned int regcache_get_val(const void *base, unsigned int idx,
+			      unsigned int word_size);
+bool regcache_set_val(void *base, unsigned int idx,
+		      unsigned int val, unsigned int word_size);
+int regcache_lookup_reg(struct regmap *map, unsigned int reg);
+
+extern struct regcache_ops regcache_rbtree_ops;
+extern struct regcache_ops regcache_lzo_ops;
+
+#endif
diff --git a/drivers/base/regmap/regcache-lzo.c b/drivers/base/regmap/regcache-lzo.c
new file mode 100644
index 000000000000..9b36703025cc
--- /dev/null
+++ b/drivers/base/regmap/regcache-lzo.c
@@ -0,0 +1,379 @@
+/*
+ * Register cache access API - LZO caching support
+ *
+ * Copyright 2011 Wolfson Microelectronics plc
+ *
+ * Author: Dimitris Papastamos <dp@opensource.wolfsonmicro.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/slab.h>
+#include <linux/lzo.h>
+
+#include "internal.h"
+
+static int regcache_lzo_exit(struct regmap *map);
+
+struct regcache_lzo_ctx {
+	void *wmem;
+	void *dst;
+	const void *src;
+	size_t src_len;
+	size_t dst_len;
+	size_t decompressed_size;
+	unsigned long *sync_bmp;
+	int sync_bmp_nbits;
+};
+
+#define LZO_BLOCK_NUM 8
+static int regcache_lzo_block_count(struct regmap *map)
+{
+	return LZO_BLOCK_NUM;
+}
+
+static int regcache_lzo_prepare(struct regcache_lzo_ctx *lzo_ctx)
+{
+	lzo_ctx->wmem = kmalloc(LZO1X_MEM_COMPRESS, GFP_KERNEL);
+	if (!lzo_ctx->wmem)
+		return -ENOMEM;
+	return 0;
+}
+
+static int regcache_lzo_compress(struct regcache_lzo_ctx *lzo_ctx)
+{
+	size_t compress_size;
+	int ret;
+
+	ret = lzo1x_1_compress(lzo_ctx->src, lzo_ctx->src_len,
+			       lzo_ctx->dst, &compress_size, lzo_ctx->wmem);
+	if (ret != LZO_E_OK || compress_size > lzo_ctx->dst_len)
+		return -EINVAL;
+	lzo_ctx->dst_len = compress_size;
+	return 0;
+}
+
+static int regcache_lzo_decompress(struct regcache_lzo_ctx *lzo_ctx)
+{
+	size_t dst_len;
+	int ret;
+
+	dst_len = lzo_ctx->dst_len;
+	ret = lzo1x_decompress_safe(lzo_ctx->src, lzo_ctx->src_len,
+				    lzo_ctx->dst, &dst_len);
+	if (ret != LZO_E_OK || dst_len != lzo_ctx->dst_len)
+		return -EINVAL;
+	return 0;
+}
+
+static int regcache_lzo_compress_cache_block(struct regmap *map,
+		struct regcache_lzo_ctx *lzo_ctx)
+{
+	int ret;
+
+	lzo_ctx->dst_len = lzo1x_worst_compress(PAGE_SIZE);
+	lzo_ctx->dst = kmalloc(lzo_ctx->dst_len, GFP_KERNEL);
+	if (!lzo_ctx->dst) {
+		lzo_ctx->dst_len = 0;
+		return -ENOMEM;
+	}
+
+	ret = regcache_lzo_compress(lzo_ctx);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+
+static int regcache_lzo_decompress_cache_block(struct regmap *map,
+		struct regcache_lzo_ctx *lzo_ctx)
+{
+	int ret;
+
+	lzo_ctx->dst_len = lzo_ctx->decompressed_size;
+	lzo_ctx->dst = kmalloc(lzo_ctx->dst_len, GFP_KERNEL);
+	if (!lzo_ctx->dst) {
+		lzo_ctx->dst_len = 0;
+		return -ENOMEM;
+	}
+
+	ret = regcache_lzo_decompress(lzo_ctx);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+
+static inline int regcache_lzo_get_blkindex(struct regmap *map,
+					    unsigned int reg)
+{
+	return ((reg / map->reg_stride) * map->cache_word_size) /
+		DIV_ROUND_UP(map->cache_size_raw,
+			     regcache_lzo_block_count(map));
+}
+
+static inline int regcache_lzo_get_blkpos(struct regmap *map,
+					  unsigned int reg)
+{
+	return (reg / map->reg_stride) %
+		    (DIV_ROUND_UP(map->cache_size_raw,
+				  regcache_lzo_block_count(map)) /
+		     map->cache_word_size);
+}
+
+static inline int regcache_lzo_get_blksize(struct regmap *map)
+{
+	return DIV_ROUND_UP(map->cache_size_raw,
+			    regcache_lzo_block_count(map));
+}
+
+static int regcache_lzo_init(struct regmap *map)
+{
+	struct regcache_lzo_ctx **lzo_blocks;
+	size_t bmp_size;
+	int ret, i, blksize, blkcount;
+	const char *p, *end;
+	unsigned long *sync_bmp;
+
+	ret = 0;
+
+	blkcount = regcache_lzo_block_count(map);
+	map->cache = kzalloc(blkcount * sizeof *lzo_blocks,
+			     GFP_KERNEL);
+	if (!map->cache)
+		return -ENOMEM;
+	lzo_blocks = map->cache;
+
+	/*
+	 * allocate a bitmap to be used when syncing the cache with
+	 * the hardware.  Each time a register is modified, the corresponding
+	 * bit is set in the bitmap, so we know that we have to sync
+	 * that register.
+	 */
+	bmp_size = map->num_reg_defaults_raw;
+	sync_bmp = kmalloc(BITS_TO_LONGS(bmp_size) * sizeof(long),
+			   GFP_KERNEL);
+	if (!sync_bmp) {
+		ret = -ENOMEM;
+		goto err;
+	}
+	bitmap_zero(sync_bmp, bmp_size);
+
+	/* allocate the lzo blocks and initialize them */
+	for (i = 0; i < blkcount; i++) {
+		lzo_blocks[i] = kzalloc(sizeof **lzo_blocks,
+					GFP_KERNEL);
+		if (!lzo_blocks[i]) {
+			kfree(sync_bmp);
+			ret = -ENOMEM;
+			goto err;
+		}
+		lzo_blocks[i]->sync_bmp = sync_bmp;
+		lzo_blocks[i]->sync_bmp_nbits = bmp_size;
+		/* alloc the working space for the compressed block */
+		ret = regcache_lzo_prepare(lzo_blocks[i]);
+		if (ret < 0)
+			goto err;
+	}
+
+	blksize = regcache_lzo_get_blksize(map);
+	p = map->reg_defaults_raw;
+	end = map->reg_defaults_raw + map->cache_size_raw;
+	/* compress the register map and fill the lzo blocks */
+	for (i = 0; i < blkcount; i++, p += blksize) {
+		lzo_blocks[i]->src = p;
+		if (p + blksize > end)
+			lzo_blocks[i]->src_len = end - p;
+		else
+			lzo_blocks[i]->src_len = blksize;
+		ret = regcache_lzo_compress_cache_block(map,
+						       lzo_blocks[i]);
+		if (ret < 0)
+			goto err;
+		lzo_blocks[i]->decompressed_size =
+			lzo_blocks[i]->src_len;
+	}
+
+	return 0;
+err:
+	regcache_lzo_exit(map);
+	return ret;
+}
+
+static int regcache_lzo_exit(struct regmap *map)
+{
+	struct regcache_lzo_ctx **lzo_blocks;
+	int i, blkcount;
+
+	lzo_blocks = map->cache;
+	if (!lzo_blocks)
+		return 0;
+
+	blkcount = regcache_lzo_block_count(map);
+	/*
+	 * the pointer to the bitmap used for syncing the cache
+	 * is shared amongst all lzo_blocks.  Ensure it is freed
+	 * only once.
+	 */
+	if (lzo_blocks[0])
+		kfree(lzo_blocks[0]->sync_bmp);
+	for (i = 0; i < blkcount; i++) {
+		if (lzo_blocks[i]) {
+			kfree(lzo_blocks[i]->wmem);
+			kfree(lzo_blocks[i]->dst);
+		}
+		/* each lzo_block is a pointer returned by kmalloc or NULL */
+		kfree(lzo_blocks[i]);
+	}
+	kfree(lzo_blocks);
+	map->cache = NULL;
+	return 0;
+}
+
+static int regcache_lzo_read(struct regmap *map,
+			     unsigned int reg, unsigned int *value)
+{
+	struct regcache_lzo_ctx *lzo_block, **lzo_blocks;
+	int ret, blkindex, blkpos;
+	size_t blksize, tmp_dst_len;
+	void *tmp_dst;
+
+	/* index of the compressed lzo block */
+	blkindex = regcache_lzo_get_blkindex(map, reg);
+	/* register index within the decompressed block */
+	blkpos = regcache_lzo_get_blkpos(map, reg);
+	/* size of the compressed block */
+	blksize = regcache_lzo_get_blksize(map);
+	lzo_blocks = map->cache;
+	lzo_block = lzo_blocks[blkindex];
+
+	/* save the pointer and length of the compressed block */
+	tmp_dst = lzo_block->dst;
+	tmp_dst_len = lzo_block->dst_len;
+
+	/* prepare the source to be the compressed block */
+	lzo_block->src = lzo_block->dst;
+	lzo_block->src_len = lzo_block->dst_len;
+
+	/* decompress the block */
+	ret = regcache_lzo_decompress_cache_block(map, lzo_block);
+	if (ret >= 0)
+		/* fetch the value from the cache */
+		*value = regcache_get_val(lzo_block->dst, blkpos,
+					  map->cache_word_size);
+
+	kfree(lzo_block->dst);
+	/* restore the pointer and length of the compressed block */
+	lzo_block->dst = tmp_dst;
+	lzo_block->dst_len = tmp_dst_len;
+
+	return ret;
+}
+
+static int regcache_lzo_write(struct regmap *map,
+			      unsigned int reg, unsigned int value)
+{
+	struct regcache_lzo_ctx *lzo_block, **lzo_blocks;
+	int ret, blkindex, blkpos;
+	size_t blksize, tmp_dst_len;
+	void *tmp_dst;
+
+	/* index of the compressed lzo block */
+	blkindex = regcache_lzo_get_blkindex(map, reg);
+	/* register index within the decompressed block */
+	blkpos = regcache_lzo_get_blkpos(map, reg);
+	/* size of the compressed block */
+	blksize = regcache_lzo_get_blksize(map);
+	lzo_blocks = map->cache;
+	lzo_block = lzo_blocks[blkindex];
+
+	/* save the pointer and length of the compressed block */
+	tmp_dst = lzo_block->dst;
+	tmp_dst_len = lzo_block->dst_len;
+
+	/* prepare the source to be the compressed block */
+	lzo_block->src = lzo_block->dst;
+	lzo_block->src_len = lzo_block->dst_len;
+
+	/* decompress the block */
+	ret = regcache_lzo_decompress_cache_block(map, lzo_block);
+	if (ret < 0) {
+		kfree(lzo_block->dst);
+		goto out;
+	}
+
+	/* write the new value to the cache */
+	if (regcache_set_val(lzo_block->dst, blkpos, value,
+			     map->cache_word_size)) {
+		kfree(lzo_block->dst);
+		goto out;
+	}
+
+	/* prepare the source to be the decompressed block */
+	lzo_block->src = lzo_block->dst;
+	lzo_block->src_len = lzo_block->dst_len;
+
+	/* compress the block */
+	ret = regcache_lzo_compress_cache_block(map, lzo_block);
+	if (ret < 0) {
+		kfree(lzo_block->dst);
+		kfree(lzo_block->src);
+		goto out;
+	}
+
+	/* set the bit so we know we have to sync this register */
+	set_bit(reg / map->reg_stride, lzo_block->sync_bmp);
+	kfree(tmp_dst);
+	kfree(lzo_block->src);
+	return 0;
+out:
+	lzo_block->dst = tmp_dst;
+	lzo_block->dst_len = tmp_dst_len;
+	return ret;
+}
+
+static int regcache_lzo_sync(struct regmap *map, unsigned int min,
+			     unsigned int max)
+{
+	struct regcache_lzo_ctx **lzo_blocks;
+	unsigned int val;
+	int i;
+	int ret;
+
+	lzo_blocks = map->cache;
+	i = min;
+	for_each_set_bit_from(i, lzo_blocks[0]->sync_bmp,
+			      lzo_blocks[0]->sync_bmp_nbits) {
+		if (i > max)
+			continue;
+
+		ret = regcache_read(map, i, &val);
+		if (ret)
+			return ret;
+
+		/* Is this the hardware default?  If so skip. */
+		ret = regcache_lookup_reg(map, i);
+		if (ret > 0 && val == map->reg_defaults[ret].def)
+			continue;
+
+		map->cache_bypass = 1;
+		ret = _regmap_write(map, i, val);
+		map->cache_bypass = 0;
+		if (ret)
+			return ret;
+		dev_dbg(map->dev, "Synced register %#x, value %#x\n",
+			i, val);
+	}
+
+	return 0;
+}
+
+struct regcache_ops regcache_lzo_ops = {
+	.type = REGCACHE_COMPRESSED,
+	.name = "lzo",
+	.init = regcache_lzo_init,
+	.exit = regcache_lzo_exit,
+	.read = regcache_lzo_read,
+	.write = regcache_lzo_write,
+	.sync = regcache_lzo_sync
+};
diff --git a/drivers/base/regmap/regcache-rbtree.c b/drivers/base/regmap/regcache-rbtree.c
new file mode 100644
index 000000000000..be7b068afaac
--- /dev/null
+++ b/drivers/base/regmap/regcache-rbtree.c
@@ -0,0 +1,435 @@
+/*
+ * Register cache access API - rbtree caching support
+ *
+ * Copyright 2011 Wolfson Microelectronics plc
+ *
+ * Author: Dimitris Papastamos <dp@opensource.wolfsonmicro.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/slab.h>
+#include <linux/debugfs.h>
+#include <linux/rbtree.h>
+#include <linux/seq_file.h>
+
+#include "internal.h"
+
+static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
+				 unsigned int value);
+static int regcache_rbtree_exit(struct regmap *map);
+
+struct regcache_rbtree_node {
+	/* the actual rbtree node holding this block */
+	struct rb_node node;
+	/* base register handled by this block */
+	unsigned int base_reg;
+	/* block of adjacent registers */
+	void *block;
+	/* number of registers available in the block */
+	unsigned int blklen;
+} __attribute__ ((packed));
+
+struct regcache_rbtree_ctx {
+	struct rb_root root;
+	struct regcache_rbtree_node *cached_rbnode;
+};
+
+static inline void regcache_rbtree_get_base_top_reg(
+	struct regmap *map,
+	struct regcache_rbtree_node *rbnode,
+	unsigned int *base, unsigned int *top)
+{
+	*base = rbnode->base_reg;
+	*top = rbnode->base_reg + ((rbnode->blklen - 1) * map->reg_stride);
+}
+
+static unsigned int regcache_rbtree_get_register(
+	struct regcache_rbtree_node *rbnode, unsigned int idx,
+	unsigned int word_size)
+{
+	return regcache_get_val(rbnode->block, idx, word_size);
+}
+
+static void regcache_rbtree_set_register(struct regcache_rbtree_node *rbnode,
+					 unsigned int idx, unsigned int val,
+					 unsigned int word_size)
+{
+	regcache_set_val(rbnode->block, idx, val, word_size);
+}
+
+static struct regcache_rbtree_node *regcache_rbtree_lookup(struct regmap *map,
+	unsigned int reg)
+{
+	struct regcache_rbtree_ctx *rbtree_ctx = map->cache;
+	struct rb_node *node;
+	struct regcache_rbtree_node *rbnode;
+	unsigned int base_reg, top_reg;
+
+	rbnode = rbtree_ctx->cached_rbnode;
+	if (rbnode) {
+		regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg,
+						 &top_reg);
+		if (reg >= base_reg && reg <= top_reg)
+			return rbnode;
+	}
+
+	node = rbtree_ctx->root.rb_node;
+	while (node) {
+		rbnode = container_of(node, struct regcache_rbtree_node, node);
+		regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg,
+						 &top_reg);
+		if (reg >= base_reg && reg <= top_reg) {
+			rbtree_ctx->cached_rbnode = rbnode;
+			return rbnode;
+		} else if (reg > top_reg) {
+			node = node->rb_right;
+		} else if (reg < base_reg) {
+			node = node->rb_left;
+		}
+	}
+
+	return NULL;
+}
+
+static int regcache_rbtree_insert(struct regmap *map, struct rb_root *root,
+				  struct regcache_rbtree_node *rbnode)
+{
+	struct rb_node **new, *parent;
+	struct regcache_rbtree_node *rbnode_tmp;
+	unsigned int base_reg_tmp, top_reg_tmp;
+	unsigned int base_reg;
+
+	parent = NULL;
+	new = &root->rb_node;
+	while (*new) {
+		rbnode_tmp = container_of(*new, struct regcache_rbtree_node,
+					  node);
+		/* base and top registers of the current rbnode */
+		regcache_rbtree_get_base_top_reg(map, rbnode_tmp, &base_reg_tmp,
+						 &top_reg_tmp);
+		/* base register of the rbnode to be added */
+		base_reg = rbnode->base_reg;
+		parent = *new;
+		/* if this register has already been inserted, just return */
+		if (base_reg >= base_reg_tmp &&
+		    base_reg <= top_reg_tmp)
+			return 0;
+		else if (base_reg > top_reg_tmp)
+			new = &((*new)->rb_right);
+		else if (base_reg < base_reg_tmp)
+			new = &((*new)->rb_left);
+	}
+
+	/* insert the node into the rbtree */
+	rb_link_node(&rbnode->node, parent, new);
+	rb_insert_color(&rbnode->node, root);
+
+	return 1;
+}
+
+#ifdef CONFIG_DEBUG_FS
+static int rbtree_show(struct seq_file *s, void *ignored)
+{
+	struct regmap *map = s->private;
+	struct regcache_rbtree_ctx *rbtree_ctx = map->cache;
+	struct regcache_rbtree_node *n;
+	struct rb_node *node;
+	unsigned int base, top;
+	int nodes = 0;
+	int registers = 0;
+	int this_registers, average;
+
+	map->lock(map);
+
+	for (node = rb_first(&rbtree_ctx->root); node != NULL;
+	     node = rb_next(node)) {
+		n = container_of(node, struct regcache_rbtree_node, node);
+
+		regcache_rbtree_get_base_top_reg(map, n, &base, &top);
+		this_registers = ((top - base) / map->reg_stride) + 1;
+		seq_printf(s, "%x-%x (%d)\n", base, top, this_registers);
+
+		nodes++;
+		registers += this_registers;
+	}
+
+	if (nodes)
+		average = registers / nodes;
+	else
+		average = 0;
+
+	seq_printf(s, "%d nodes, %d registers, average %d registers\n",
+		   nodes, registers, average);
+
+	map->unlock(map);
+
+	return 0;
+}
+
+static int rbtree_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, rbtree_show, inode->i_private);
+}
+
+static const struct file_operations rbtree_fops = {
+	.open		= rbtree_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static void rbtree_debugfs_init(struct regmap *map)
+{
+	debugfs_create_file("rbtree", 0400, map->debugfs, map, &rbtree_fops);
+}
+#else
+static void rbtree_debugfs_init(struct regmap *map)
+{
+}
+#endif
+
+static int regcache_rbtree_init(struct regmap *map)
+{
+	struct regcache_rbtree_ctx *rbtree_ctx;
+	int i;
+	int ret;
+
+	map->cache = kmalloc(sizeof *rbtree_ctx, GFP_KERNEL);
+	if (!map->cache)
+		return -ENOMEM;
+
+	rbtree_ctx = map->cache;
+	rbtree_ctx->root = RB_ROOT;
+	rbtree_ctx->cached_rbnode = NULL;
+
+	for (i = 0; i < map->num_reg_defaults; i++) {
+		ret = regcache_rbtree_write(map,
+					    map->reg_defaults[i].reg,
+					    map->reg_defaults[i].def);
+		if (ret)
+			goto err;
+	}
+
+	rbtree_debugfs_init(map);
+
+	return 0;
+
+err:
+	regcache_rbtree_exit(map);
+	return ret;
+}
+
+static int regcache_rbtree_exit(struct regmap *map)
+{
+	struct rb_node *next;
+	struct regcache_rbtree_ctx *rbtree_ctx;
+	struct regcache_rbtree_node *rbtree_node;
+
+	/* if we've already been called then just return */
+	rbtree_ctx = map->cache;
+	if (!rbtree_ctx)
+		return 0;
+
+	/* free up the rbtree */
+	next = rb_first(&rbtree_ctx->root);
+	while (next) {
+		rbtree_node = rb_entry(next, struct regcache_rbtree_node, node);
+		next = rb_next(&rbtree_node->node);
+		rb_erase(&rbtree_node->node, &rbtree_ctx->root);
+		kfree(rbtree_node->block);
+		kfree(rbtree_node);
+	}
+
+	/* release the resources */
+	kfree(map->cache);
+	map->cache = NULL;
+
+	return 0;
+}
+
+static int regcache_rbtree_read(struct regmap *map,
+				unsigned int reg, unsigned int *value)
+{
+	struct regcache_rbtree_node *rbnode;
+	unsigned int reg_tmp;
+
+	rbnode = regcache_rbtree_lookup(map, reg);
+	if (rbnode) {
+		reg_tmp = (reg - rbnode->base_reg) / map->reg_stride;
+		*value = regcache_rbtree_get_register(rbnode, reg_tmp,
+						      map->cache_word_size);
+	} else {
+		return -ENOENT;
+	}
+
+	return 0;
+}
+
+
+static int regcache_rbtree_insert_to_block(struct regcache_rbtree_node *rbnode,
+					   unsigned int pos, unsigned int reg,
+					   unsigned int value, unsigned int word_size)
+{
+	u8 *blk;
+
+	blk = krealloc(rbnode->block,
+		       (rbnode->blklen + 1) * word_size, GFP_KERNEL);
+	if (!blk)
+		return -ENOMEM;
+
+	/* insert the register value in the correct place in the rbnode block */
+	memmove(blk + (pos + 1) * word_size,
+		blk + pos * word_size,
+		(rbnode->blklen - pos) * word_size);
+
+	/* update the rbnode block, its size and the base register */
+	rbnode->block = blk;
+	rbnode->blklen++;
+	if (!pos)
+		rbnode->base_reg = reg;
+
+	regcache_rbtree_set_register(rbnode, pos, value, word_size);
+	return 0;
+}
+
+static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
+				 unsigned int value)
+{
+	struct regcache_rbtree_ctx *rbtree_ctx;
+	struct regcache_rbtree_node *rbnode, *rbnode_tmp;
+	struct rb_node *node;
+	unsigned int val;
+	unsigned int reg_tmp;
+	unsigned int pos;
+	int i;
+	int ret;
+
+	rbtree_ctx = map->cache;
+	/* if we can't locate it in the cached rbnode we'll have
+	 * to traverse the rbtree looking for it.
+	 */
+	rbnode = regcache_rbtree_lookup(map, reg);
+	if (rbnode) {
+		reg_tmp = (reg - rbnode->base_reg) / map->reg_stride;
+		val = regcache_rbtree_get_register(rbnode, reg_tmp,
+						   map->cache_word_size);
+		if (val == value)
+			return 0;
+		regcache_rbtree_set_register(rbnode, reg_tmp, value,
+					     map->cache_word_size);
+	} else {
+		/* look for an adjacent register to the one we are about to add */
+		for (node = rb_first(&rbtree_ctx->root); node;
+		     node = rb_next(node)) {
+			rbnode_tmp = rb_entry(node, struct regcache_rbtree_node,
+					      node);
+			for (i = 0; i < rbnode_tmp->blklen; i++) {
+				reg_tmp = rbnode_tmp->base_reg +
+						(i * map->reg_stride);
+				if (abs(reg_tmp - reg) != map->reg_stride)
+					continue;
+				/* decide where in the block to place our register */
+				if (reg_tmp + map->reg_stride == reg)
+					pos = i + 1;
+				else
+					pos = i;
+				ret = regcache_rbtree_insert_to_block(rbnode_tmp, pos,
+								      reg, value,
+								      map->cache_word_size);
+				if (ret)
+					return ret;
+				rbtree_ctx->cached_rbnode = rbnode_tmp;
+				return 0;
+			}
+		}
+		/* we did not manage to find a place to insert it in an existing
+		 * block so create a new rbnode with a single register in its block.
+		 * This block will get populated further if any other adjacent
+		 * registers get modified in the future.
+		 */
+		rbnode = kzalloc(sizeof *rbnode, GFP_KERNEL);
+		if (!rbnode)
+			return -ENOMEM;
+		rbnode->blklen = 1;
+		rbnode->base_reg = reg;
+		rbnode->block = kmalloc(rbnode->blklen * map->cache_word_size,
+					GFP_KERNEL);
+		if (!rbnode->block) {
+			kfree(rbnode);
+			return -ENOMEM;
+		}
+		regcache_rbtree_set_register(rbnode, 0, value, map->cache_word_size);
+		regcache_rbtree_insert(map, &rbtree_ctx->root, rbnode);
+		rbtree_ctx->cached_rbnode = rbnode;
+	}
+
+	return 0;
+}
+
+static int regcache_rbtree_sync(struct regmap *map, unsigned int min,
+				unsigned int max)
+{
+	struct regcache_rbtree_ctx *rbtree_ctx;
+	struct rb_node *node;
+	struct regcache_rbtree_node *rbnode;
+	unsigned int regtmp;
+	unsigned int val;
+	int ret;
+	int i, base, end;
+
+	rbtree_ctx = map->cache;
+	for (node = rb_first(&rbtree_ctx->root); node; node = rb_next(node)) {
+		rbnode = rb_entry(node, struct regcache_rbtree_node, node);
+
+		if (rbnode->base_reg < min)
+			continue;
+		if (rbnode->base_reg > max)
+			break;
+		if (rbnode->base_reg + rbnode->blklen < min)
+			continue;
+
+		if (min > rbnode->base_reg)
+			base = min - rbnode->base_reg;
+		else
+			base = 0;
+
+		if (max < rbnode->base_reg + rbnode->blklen)
+			end = rbnode->base_reg + rbnode->blklen - max;
+		else
+			end = rbnode->blklen;
+
+		for (i = base; i < end; i++) {
+			regtmp = rbnode->base_reg + (i * map->reg_stride);
+			val = regcache_rbtree_get_register(rbnode, i,
+							   map->cache_word_size);
+
+			/* Is this the hardware default?  If so skip. */
+			ret = regcache_lookup_reg(map, regtmp);
+			if (ret >= 0 && val == map->reg_defaults[ret].def)
+				continue;
+
+			map->cache_bypass = 1;
+			ret = _regmap_write(map, regtmp, val);
+			map->cache_bypass = 0;
+			if (ret)
+				return ret;
+			dev_dbg(map->dev, "Synced register %#x, value %#x\n",
+				regtmp, val);
+		}
+	}
+
+	return 0;
+}
+
+struct regcache_ops regcache_rbtree_ops = {
+	.type = REGCACHE_RBTREE,
+	.name = "rbtree",
+	.init = regcache_rbtree_init,
+	.exit = regcache_rbtree_exit,
+	.read = regcache_rbtree_read,
+	.write = regcache_rbtree_write,
+	.sync = regcache_rbtree_sync
+};
diff --git a/drivers/base/regmap/regcache.c b/drivers/base/regmap/regcache.c
new file mode 100644
index 000000000000..bb9701da79bd
--- /dev/null
+++ b/drivers/base/regmap/regcache.c
@@ -0,0 +1,497 @@
+/*
+ * Register cache access API
+ *
+ * Copyright 2011 Wolfson Microelectronics plc
+ *
+ * Author: Dimitris Papastamos <dp@opensource.wolfsonmicro.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/slab.h>
+#include <trace/events/regmap.h>
+#include <linux/bsearch.h>
+#include <linux/sort.h>
+
+#include "internal.h"
+
+static const struct regcache_ops *cache_types[] = {
+	&regcache_rbtree_ops,
+	&regcache_lzo_ops,
+};
+
+static int regcache_hw_init(struct regmap *map)
+{
+	int i, j;
+	int ret;
+	int count;
+	unsigned int val;
+	void *tmp_buf;
+
+	if (!map->num_reg_defaults_raw)
+		return -EINVAL;
+
+	if (!map->reg_defaults_raw) {
+		u32 cache_bypass = map->cache_bypass;
+		dev_warn(map->dev, "No cache defaults, reading back from HW\n");
+
+		/* Bypass the cache access till data read from HW*/
+		map->cache_bypass = 1;
+		tmp_buf = kmalloc(map->cache_size_raw, GFP_KERNEL);
+		if (!tmp_buf)
+			return -EINVAL;
+		ret = regmap_bulk_read(map, 0, tmp_buf,
+				       map->num_reg_defaults_raw);
+		map->cache_bypass = cache_bypass;
+		if (ret < 0) {
+			kfree(tmp_buf);
+			return ret;
+		}
+		map->reg_defaults_raw = tmp_buf;
+		map->cache_free = 1;
+	}
+
+	/* calculate the size of reg_defaults */
+	for (count = 0, i = 0; i < map->num_reg_defaults_raw; i++) {
+		val = regcache_get_val(map->reg_defaults_raw,
+				       i, map->cache_word_size);
+		if (regmap_volatile(map, i * map->reg_stride))
+			continue;
+		count++;
+	}
+
+	map->reg_defaults = kmalloc(count * sizeof(struct reg_default),
+				      GFP_KERNEL);
+	if (!map->reg_defaults) {
+		ret = -ENOMEM;
+		goto err_free;
+	}
+
+	/* fill the reg_defaults */
+	map->num_reg_defaults = count;
+	for (i = 0, j = 0; i < map->num_reg_defaults_raw; i++) {
+		val = regcache_get_val(map->reg_defaults_raw,
+				       i, map->cache_word_size);
+		if (regmap_volatile(map, i * map->reg_stride))
+			continue;
+		map->reg_defaults[j].reg = i * map->reg_stride;
+		map->reg_defaults[j].def = val;
+		j++;
+	}
+
+	return 0;
+
+err_free:
+	if (map->cache_free)
+		kfree(map->reg_defaults_raw);
+
+	return ret;
+}
+
+int regcache_init(struct regmap *map, const struct regmap_config *config)
+{
+	int ret;
+	int i;
+	void *tmp_buf;
+
+	for (i = 0; i < config->num_reg_defaults; i++)
+		if (config->reg_defaults[i].reg % map->reg_stride)
+			return -EINVAL;
+
+	if (map->cache_type == REGCACHE_NONE) {
+		map->cache_bypass = true;
+		return 0;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(cache_types); i++)
+		if (cache_types[i]->type == map->cache_type)
+			break;
+
+	if (i == ARRAY_SIZE(cache_types)) {
+		dev_err(map->dev, "Could not match compress type: %d\n",
+			map->cache_type);
+		return -EINVAL;
+	}
+
+	map->num_reg_defaults = config->num_reg_defaults;
+	map->num_reg_defaults_raw = config->num_reg_defaults_raw;
+	map->reg_defaults_raw = config->reg_defaults_raw;
+	map->cache_word_size = DIV_ROUND_UP(config->val_bits, 8);
+	map->cache_size_raw = map->cache_word_size * config->num_reg_defaults_raw;
+
+	map->cache = NULL;
+	map->cache_ops = cache_types[i];
+
+	if (!map->cache_ops->read ||
+	    !map->cache_ops->write ||
+	    !map->cache_ops->name)
+		return -EINVAL;
+
+	/* We still need to ensure that the reg_defaults
+	 * won't vanish from under us.  We'll need to make
+	 * a copy of it.
+	 */
+	if (config->reg_defaults) {
+		if (!map->num_reg_defaults)
+			return -EINVAL;
+		tmp_buf = kmemdup(config->reg_defaults, map->num_reg_defaults *
+				  sizeof(struct reg_default), GFP_KERNEL);
+		if (!tmp_buf)
+			return -ENOMEM;
+		map->reg_defaults = tmp_buf;
+	} else if (map->num_reg_defaults_raw) {
+		/* Some devices such as PMICs don't have cache defaults,
+		 * we cope with this by reading back the HW registers and
+		 * crafting the cache defaults by hand.
+		 */
+		ret = regcache_hw_init(map);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (!map->max_register)
+		map->max_register = map->num_reg_defaults_raw;
+
+	if (map->cache_ops->init) {
+		dev_dbg(map->dev, "Initializing %s cache\n",
+			map->cache_ops->name);
+		ret = map->cache_ops->init(map);
+		if (ret)
+			goto err_free;
+	}
+	return 0;
+
+err_free:
+	kfree(map->reg_defaults);
+	if (map->cache_free)
+		kfree(map->reg_defaults_raw);
+
+	return ret;
+}
+
+void regcache_exit(struct regmap *map)
+{
+	if (map->cache_type == REGCACHE_NONE)
+		return;
+
+	BUG_ON(!map->cache_ops);
+
+	kfree(map->reg_defaults);
+	if (map->cache_free)
+		kfree(map->reg_defaults_raw);
+
+	if (map->cache_ops->exit) {
+		dev_dbg(map->dev, "Destroying %s cache\n",
+			map->cache_ops->name);
+		map->cache_ops->exit(map);
+	}
+}
+
+/**
+ * regcache_read: Fetch the value of a given register from the cache.
+ *
+ * @map: map to configure.
+ * @reg: The register index.
+ * @value: The value to be returned.
+ *
+ * Return a negative value on failure, 0 on success.
+ */
+int regcache_read(struct regmap *map,
+		  unsigned int reg, unsigned int *value)
+{
+	int ret;
+
+	if (map->cache_type == REGCACHE_NONE)
+		return -ENOSYS;
+
+	BUG_ON(!map->cache_ops);
+
+	if (!regmap_volatile(map, reg)) {
+		ret = map->cache_ops->read(map, reg, value);
+
+		if (ret == 0)
+			trace_regmap_reg_read_cache(map->dev, reg, *value);
+
+		return ret;
+	}
+
+	return -EINVAL;
+}
+
+/**
+ * regcache_write: Set the value of a given register in the cache.
+ *
+ * @map: map to configure.
+ * @reg: The register index.
+ * @value: The new register value.
+ *
+ * Return a negative value on failure, 0 on success.
+ */
+int regcache_write(struct regmap *map,
+		   unsigned int reg, unsigned int value)
+{
+	if (map->cache_type == REGCACHE_NONE)
+		return 0;
+
+	BUG_ON(!map->cache_ops);
+
+	if (!regmap_writeable(map, reg))
+		return -EIO;
+
+	if (!regmap_volatile(map, reg))
+		return map->cache_ops->write(map, reg, value);
+
+	return 0;
+}
+
+/**
+ * regcache_sync: Sync the register cache with the hardware.
+ *
+ * @map: map to configure.
+ *
+ * Any registers that should not be synced should be marked as
+ * volatile.  In general drivers can choose not to use the provided
+ * syncing functionality if they so require.
+ *
+ * Return a negative value on failure, 0 on success.
+ */
+int regcache_sync(struct regmap *map)
+{
+	int ret = 0;
+	unsigned int i;
+	const char *name;
+	unsigned int bypass;
+
+	BUG_ON(!map->cache_ops || !map->cache_ops->sync);
+
+	map->lock(map);
+	/* Remember the initial bypass state */
+	bypass = map->cache_bypass;
+	dev_dbg(map->dev, "Syncing %s cache\n",
+		map->cache_ops->name);
+	name = map->cache_ops->name;
+	trace_regcache_sync(map->dev, name, "start");
+
+	if (!map->cache_dirty)
+		goto out;
+
+	/* Apply any patch first */
+	map->cache_bypass = 1;
+	for (i = 0; i < map->patch_regs; i++) {
+		if (map->patch[i].reg % map->reg_stride) {
+			ret = -EINVAL;
+			goto out;
+		}
+		ret = _regmap_write(map, map->patch[i].reg, map->patch[i].def);
+		if (ret != 0) {
+			dev_err(map->dev, "Failed to write %x = %x: %d\n",
+				map->patch[i].reg, map->patch[i].def, ret);
+			goto out;
+		}
+	}
+	map->cache_bypass = 0;
+
+	ret = map->cache_ops->sync(map, 0, map->max_register);
+
+	if (ret == 0)
+		map->cache_dirty = false;
+
+out:
+	trace_regcache_sync(map->dev, name, "stop");
+	/* Restore the bypass state */
+	map->cache_bypass = bypass;
+	map->unlock(map);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(regcache_sync);
+
+/**
+ * regcache_sync_region: Sync part  of the register cache with the hardware.
+ *
+ * @map: map to sync.
+ * @min: first register to sync
+ * @max: last register to sync
+ *
+ * Write all non-default register values in the specified region to
+ * the hardware.
+ *
+ * Return a negative value on failure, 0 on success.
+ */
+int regcache_sync_region(struct regmap *map, unsigned int min,
+			 unsigned int max)
+{
+	int ret = 0;
+	const char *name;
+	unsigned int bypass;
+
+	BUG_ON(!map->cache_ops || !map->cache_ops->sync);
+
+	map->lock(map);
+
+	/* Remember the initial bypass state */
+	bypass = map->cache_bypass;
+
+	name = map->cache_ops->name;
+	dev_dbg(map->dev, "Syncing %s cache from %d-%d\n", name, min, max);
+
+	trace_regcache_sync(map->dev, name, "start region");
+
+	if (!map->cache_dirty)
+		goto out;
+
+	ret = map->cache_ops->sync(map, min, max);
+
+out:
+	trace_regcache_sync(map->dev, name, "stop region");
+	/* Restore the bypass state */
+	map->cache_bypass = bypass;
+	map->unlock(map);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(regcache_sync_region);
+
+/**
+ * regcache_cache_only: Put a register map into cache only mode
+ *
+ * @map: map to configure
+ * @cache_only: flag if changes should be written to the hardware
+ *
+ * When a register map is marked as cache only writes to the register
+ * map API will only update the register cache, they will not cause
+ * any hardware changes.  This is useful for allowing portions of
+ * drivers to act as though the device were functioning as normal when
+ * it is disabled for power saving reasons.
+ */
+void regcache_cache_only(struct regmap *map, bool enable)
+{
+	map->lock(map);
+	WARN_ON(map->cache_bypass && enable);
+	map->cache_only = enable;
+	trace_regmap_cache_only(map->dev, enable);
+	map->unlock(map);
+}
+EXPORT_SYMBOL_GPL(regcache_cache_only);
+
+/**
+ * regcache_mark_dirty: Mark the register cache as dirty
+ *
+ * @map: map to mark
+ *
+ * Mark the register cache as dirty, for example due to the device
+ * having been powered down for suspend.  If the cache is not marked
+ * as dirty then the cache sync will be suppressed.
+ */
+void regcache_mark_dirty(struct regmap *map)
+{
+	map->lock(map);
+	map->cache_dirty = true;
+	map->unlock(map);
+}
+EXPORT_SYMBOL_GPL(regcache_mark_dirty);
+
+/**
+ * regcache_cache_bypass: Put a register map into cache bypass mode
+ *
+ * @map: map to configure
+ * @cache_bypass: flag if changes should not be written to the hardware
+ *
+ * When a register map is marked with the cache bypass option, writes
+ * to the register map API will only update the hardware and not the
+ * the cache directly.  This is useful when syncing the cache back to
+ * the hardware.
+ */
+void regcache_cache_bypass(struct regmap *map, bool enable)
+{
+	map->lock(map);
+	WARN_ON(map->cache_only && enable);
+	map->cache_bypass = enable;
+	trace_regmap_cache_bypass(map->dev, enable);
+	map->unlock(map);
+}
+EXPORT_SYMBOL_GPL(regcache_cache_bypass);
+
+bool regcache_set_val(void *base, unsigned int idx,
+		      unsigned int val, unsigned int word_size)
+{
+	switch (word_size) {
+	case 1: {
+		u8 *cache = base;
+		if (cache[idx] == val)
+			return true;
+		cache[idx] = val;
+		break;
+	}
+	case 2: {
+		u16 *cache = base;
+		if (cache[idx] == val)
+			return true;
+		cache[idx] = val;
+		break;
+	}
+	case 4: {
+		u32 *cache = base;
+		if (cache[idx] == val)
+			return true;
+		cache[idx] = val;
+		break;
+	}
+	default:
+		BUG();
+	}
+	return false;
+}
+
+unsigned int regcache_get_val(const void *base, unsigned int idx,
+			      unsigned int word_size)
+{
+	if (!base)
+		return -EINVAL;
+
+	switch (word_size) {
+	case 1: {
+		const u8 *cache = base;
+		return cache[idx];
+	}
+	case 2: {
+		const u16 *cache = base;
+		return cache[idx];
+	}
+	case 4: {
+		const u32 *cache = base;
+		return cache[idx];
+	}
+	default:
+		BUG();
+	}
+	/* unreachable */
+	return -1;
+}
+
+static int regcache_default_cmp(const void *a, const void *b)
+{
+	const struct reg_default *_a = a;
+	const struct reg_default *_b = b;
+
+	return _a->reg - _b->reg;
+}
+
+int regcache_lookup_reg(struct regmap *map, unsigned int reg)
+{
+	struct reg_default key;
+	struct reg_default *r;
+
+	key.reg = reg;
+	key.def = 0;
+
+	r = bsearch(&key, map->reg_defaults, map->num_reg_defaults,
+		    sizeof(struct reg_default), regcache_default_cmp);
+
+	if (r)
+		return r - map->reg_defaults;
+	else
+		return -ENOENT;
+}
diff --git a/drivers/base/regmap/regmap-debugfs.c b/drivers/base/regmap/regmap-debugfs.c
new file mode 100644
index 000000000000..34038030f655
--- /dev/null
+++ b/drivers/base/regmap/regmap-debugfs.c
@@ -0,0 +1,299 @@
+/*
+ * Register map access API - debugfs
+ *
+ * Copyright 2011 Wolfson Microelectronics plc
+ *
+ * Author: Mark Brown <broonie@opensource.wolfsonmicro.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/debugfs.h>
+#include <linux/uaccess.h>
+
+#include "internal.h"
+
+static struct dentry *regmap_debugfs_root;
+
+/* Calculate the length of a fixed format  */
+static size_t regmap_calc_reg_len(int max_val, char *buf, size_t buf_size)
+{
+	snprintf(buf, buf_size, "%x", max_val);
+	return strlen(buf);
+}
+
+static int regmap_open_file(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+
+static ssize_t regmap_name_read_file(struct file *file,
+				     char __user *user_buf, size_t count,
+				     loff_t *ppos)
+{
+	struct regmap *map = file->private_data;
+	int ret;
+	char *buf;
+
+	buf = kmalloc(PAGE_SIZE, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	ret = snprintf(buf, PAGE_SIZE, "%s\n", map->dev->driver->name);
+	if (ret < 0) {
+		kfree(buf);
+		return ret;
+	}
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, ret);
+	kfree(buf);
+	return ret;
+}
+
+static const struct file_operations regmap_name_fops = {
+	.open = regmap_open_file,
+	.read = regmap_name_read_file,
+	.llseek = default_llseek,
+};
+
+static ssize_t regmap_map_read_file(struct file *file, char __user *user_buf,
+				    size_t count, loff_t *ppos)
+{
+	int reg_len, val_len, tot_len;
+	size_t buf_pos = 0;
+	loff_t p = 0;
+	ssize_t ret;
+	int i;
+	struct regmap *map = file->private_data;
+	char *buf;
+	unsigned int val;
+
+	if (*ppos < 0 || !count)
+		return -EINVAL;
+
+	buf = kmalloc(count, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	/* Calculate the length of a fixed format  */
+	reg_len = regmap_calc_reg_len(map->max_register, buf, count);
+	val_len = 2 * map->format.val_bytes;
+	tot_len = reg_len + val_len + 3;      /* : \n */
+
+	for (i = 0; i <= map->max_register; i += map->reg_stride) {
+		if (!regmap_readable(map, i))
+			continue;
+
+		if (regmap_precious(map, i))
+			continue;
+
+		/* If we're in the region the user is trying to read */
+		if (p >= *ppos) {
+			/* ...but not beyond it */
+			if (buf_pos >= count - 1 - tot_len)
+				break;
+
+			/* Format the register */
+			snprintf(buf + buf_pos, count - buf_pos, "%.*x: ",
+				 reg_len, i);
+			buf_pos += reg_len + 2;
+
+			/* Format the value, write all X if we can't read */
+			ret = regmap_read(map, i, &val);
+			if (ret == 0)
+				snprintf(buf + buf_pos, count - buf_pos,
+					 "%.*x", val_len, val);
+			else
+				memset(buf + buf_pos, 'X', val_len);
+			buf_pos += 2 * map->format.val_bytes;
+
+			buf[buf_pos++] = '\n';
+		}
+		p += tot_len;
+	}
+
+	ret = buf_pos;
+
+	if (copy_to_user(user_buf, buf, buf_pos)) {
+		ret = -EFAULT;
+		goto out;
+	}
+
+	*ppos += buf_pos;
+
+out:
+	kfree(buf);
+	return ret;
+}
+
+#undef REGMAP_ALLOW_WRITE_DEBUGFS
+#ifdef REGMAP_ALLOW_WRITE_DEBUGFS
+/*
+ * This can be dangerous especially when we have clients such as
+ * PMICs, therefore don't provide any real compile time configuration option
+ * for this feature, people who want to use this will need to modify
+ * the source code directly.
+ */
+static ssize_t regmap_map_write_file(struct file *file,
+				     const char __user *user_buf,
+				     size_t count, loff_t *ppos)
+{
+	char buf[32];
+	size_t buf_size;
+	char *start = buf;
+	unsigned long reg, value;
+	struct regmap *map = file->private_data;
+
+	buf_size = min(count, (sizeof(buf)-1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	while (*start == ' ')
+		start++;
+	reg = simple_strtoul(start, &start, 16);
+	while (*start == ' ')
+		start++;
+	if (strict_strtoul(start, 16, &value))
+		return -EINVAL;
+
+	/* Userspace has been fiddling around behind the kernel's back */
+	add_taint(TAINT_USER);
+
+	regmap_write(map, reg, value);
+	return buf_size;
+}
+#else
+#define regmap_map_write_file NULL
+#endif
+
+static const struct file_operations regmap_map_fops = {
+	.open = regmap_open_file,
+	.read = regmap_map_read_file,
+	.write = regmap_map_write_file,
+	.llseek = default_llseek,
+};
+
+static ssize_t regmap_access_read_file(struct file *file,
+				       char __user *user_buf, size_t count,
+				       loff_t *ppos)
+{
+	int reg_len, tot_len;
+	size_t buf_pos = 0;
+	loff_t p = 0;
+	ssize_t ret;
+	int i;
+	struct regmap *map = file->private_data;
+	char *buf;
+
+	if (*ppos < 0 || !count)
+		return -EINVAL;
+
+	buf = kmalloc(count, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	/* Calculate the length of a fixed format  */
+	reg_len = regmap_calc_reg_len(map->max_register, buf, count);
+	tot_len = reg_len + 10; /* ': R W V P\n' */
+
+	for (i = 0; i <= map->max_register; i += map->reg_stride) {
+		/* Ignore registers which are neither readable nor writable */
+		if (!regmap_readable(map, i) && !regmap_writeable(map, i))
+			continue;
+
+		/* If we're in the region the user is trying to read */
+		if (p >= *ppos) {
+			/* ...but not beyond it */
+			if (buf_pos >= count - 1 - tot_len)
+				break;
+
+			/* Format the register */
+			snprintf(buf + buf_pos, count - buf_pos,
+				 "%.*x: %c %c %c %c\n",
+				 reg_len, i,
+				 regmap_readable(map, i) ? 'y' : 'n',
+				 regmap_writeable(map, i) ? 'y' : 'n',
+				 regmap_volatile(map, i) ? 'y' : 'n',
+				 regmap_precious(map, i) ? 'y' : 'n');
+
+			buf_pos += tot_len;
+		}
+		p += tot_len;
+	}
+
+	ret = buf_pos;
+
+	if (copy_to_user(user_buf, buf, buf_pos)) {
+		ret = -EFAULT;
+		goto out;
+	}
+
+	*ppos += buf_pos;
+
+out:
+	kfree(buf);
+	return ret;
+}
+
+static const struct file_operations regmap_access_fops = {
+	.open = regmap_open_file,
+	.read = regmap_access_read_file,
+	.llseek = default_llseek,
+};
+
+void regmap_debugfs_init(struct regmap *map, const char *name)
+{
+	if (name) {
+		map->debugfs_name = kasprintf(GFP_KERNEL, "%s-%s",
+					      dev_name(map->dev), name);
+		name = map->debugfs_name;
+	} else {
+		name = dev_name(map->dev);
+	}
+
+	map->debugfs = debugfs_create_dir(name, regmap_debugfs_root);
+	if (!map->debugfs) {
+		dev_warn(map->dev, "Failed to create debugfs directory\n");
+		return;
+	}
+
+	debugfs_create_file("name", 0400, map->debugfs,
+			    map, &regmap_name_fops);
+
+	if (map->max_register) {
+		debugfs_create_file("registers", 0400, map->debugfs,
+				    map, &regmap_map_fops);
+		debugfs_create_file("access", 0400, map->debugfs,
+				    map, &regmap_access_fops);
+	}
+
+	if (map->cache_type) {
+		debugfs_create_bool("cache_only", 0400, map->debugfs,
+				    &map->cache_only);
+		debugfs_create_bool("cache_dirty", 0400, map->debugfs,
+				    &map->cache_dirty);
+		debugfs_create_bool("cache_bypass", 0400, map->debugfs,
+				    &map->cache_bypass);
+	}
+}
+
+void regmap_debugfs_exit(struct regmap *map)
+{
+	debugfs_remove_recursive(map->debugfs);
+	kfree(map->debugfs_name);
+}
+
+void regmap_debugfs_initcall(void)
+{
+	regmap_debugfs_root = debugfs_create_dir("regmap", NULL);
+	if (!regmap_debugfs_root) {
+		pr_warn("regmap: Failed to create debugfs root\n");
+		return;
+	}
+}
diff --git a/drivers/base/regmap/regmap-i2c.c b/drivers/base/regmap/regmap-i2c.c
index c4f7a45cd2c3..5f6b2478bf17 100644
--- a/drivers/base/regmap/regmap-i2c.c
+++ b/drivers/base/regmap/regmap-i2c.c
@@ -15,8 +15,9 @@
 #include <linux/module.h>
 #include <linux/init.h>
 
-static int regmap_i2c_write(struct device *dev, const void *data, size_t count)
+static int regmap_i2c_write(void *context, const void *data, size_t count)
 {
+	struct device *dev = context;
 	struct i2c_client *i2c = to_i2c_client(dev);
 	int ret;
 
@@ -29,10 +30,11 @@ static int regmap_i2c_write(struct device *dev, const void *data, size_t count)
 		return -EIO;
 }
 
-static int regmap_i2c_gather_write(struct device *dev,
+static int regmap_i2c_gather_write(void *context,
 				   const void *reg, size_t reg_size,
 				   const void *val, size_t val_size)
 {
+	struct device *dev = context;
 	struct i2c_client *i2c = to_i2c_client(dev);
 	struct i2c_msg xfer[2];
 	int ret;
@@ -62,10 +64,11 @@ static int regmap_i2c_gather_write(struct device *dev,
 		return -EIO;
 }
 
-static int regmap_i2c_read(struct device *dev,
+static int regmap_i2c_read(void *context,
 			   const void *reg, size_t reg_size,
 			   void *val, size_t val_size)
 {
+	struct device *dev = context;
 	struct i2c_client *i2c = to_i2c_client(dev);
 	struct i2c_msg xfer[2];
 	int ret;
@@ -90,11 +93,9 @@ static int regmap_i2c_read(struct device *dev,
 }
 
 static struct regmap_bus regmap_i2c = {
-	.type = &i2c_bus_type,
 	.write = regmap_i2c_write,
 	.gather_write = regmap_i2c_gather_write,
 	.read = regmap_i2c_read,
-	.owner = THIS_MODULE,
 };
 
 /**
@@ -109,8 +110,25 @@ static struct regmap_bus regmap_i2c = {
 struct regmap *regmap_init_i2c(struct i2c_client *i2c,
 			       const struct regmap_config *config)
 {
-	return regmap_init(&i2c->dev, &regmap_i2c, config);
+	return regmap_init(&i2c->dev, &regmap_i2c, &i2c->dev, config);
 }
 EXPORT_SYMBOL_GPL(regmap_init_i2c);
 
+/**
+ * devm_regmap_init_i2c(): Initialise managed register map
+ *
+ * @i2c: Device that will be interacted with
+ * @config: Configuration for register map
+ *
+ * The return value will be an ERR_PTR() on error or a valid pointer
+ * to a struct regmap.  The regmap will be automatically freed by the
+ * device management code.
+ */
+struct regmap *devm_regmap_init_i2c(struct i2c_client *i2c,
+				    const struct regmap_config *config)
+{
+	return devm_regmap_init(&i2c->dev, &regmap_i2c, &i2c->dev, config);
+}
+EXPORT_SYMBOL_GPL(devm_regmap_init_i2c);
+
 MODULE_LICENSE("GPL");
diff --git a/drivers/base/regmap/regmap-irq.c b/drivers/base/regmap/regmap-irq.c
new file mode 100644
index 000000000000..2691907757c4
--- /dev/null
+++ b/drivers/base/regmap/regmap-irq.c
@@ -0,0 +1,305 @@
+/*
+ * regmap based irq_chip
+ *
+ * Copyright 2011 Wolfson Microelectronics plc
+ *
+ * Author: Mark Brown <broonie@opensource.wolfsonmicro.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/export.h>
+#include <linux/regmap.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+
+#include "internal.h"
+
+struct regmap_irq_chip_data {
+	struct mutex lock;
+
+	struct regmap *map;
+	struct regmap_irq_chip *chip;
+
+	int irq_base;
+
+	unsigned int *status_buf;
+	unsigned int *mask_buf;
+	unsigned int *mask_buf_def;
+
+	unsigned int irq_reg_stride;
+};
+
+static inline const
+struct regmap_irq *irq_to_regmap_irq(struct regmap_irq_chip_data *data,
+				     int irq)
+{
+	return &data->chip->irqs[irq - data->irq_base];
+}
+
+static void regmap_irq_lock(struct irq_data *data)
+{
+	struct regmap_irq_chip_data *d = irq_data_get_irq_chip_data(data);
+
+	mutex_lock(&d->lock);
+}
+
+static void regmap_irq_sync_unlock(struct irq_data *data)
+{
+	struct regmap_irq_chip_data *d = irq_data_get_irq_chip_data(data);
+	struct regmap *map = d->map;
+	int i, ret;
+
+	/*
+	 * If there's been a change in the mask write it back to the
+	 * hardware.  We rely on the use of the regmap core cache to
+	 * suppress pointless writes.
+	 */
+	for (i = 0; i < d->chip->num_regs; i++) {
+		ret = regmap_update_bits(d->map, d->chip->mask_base +
+						(i * map->reg_stride *
+						d->irq_reg_stride),
+					 d->mask_buf_def[i], d->mask_buf[i]);
+		if (ret != 0)
+			dev_err(d->map->dev, "Failed to sync masks in %x\n",
+				d->chip->mask_base + (i * map->reg_stride));
+	}
+
+	mutex_unlock(&d->lock);
+}
+
+static void regmap_irq_enable(struct irq_data *data)
+{
+	struct regmap_irq_chip_data *d = irq_data_get_irq_chip_data(data);
+	struct regmap *map = d->map;
+	const struct regmap_irq *irq_data = irq_to_regmap_irq(d, data->irq);
+
+	d->mask_buf[irq_data->reg_offset / map->reg_stride] &= ~irq_data->mask;
+}
+
+static void regmap_irq_disable(struct irq_data *data)
+{
+	struct regmap_irq_chip_data *d = irq_data_get_irq_chip_data(data);
+	struct regmap *map = d->map;
+	const struct regmap_irq *irq_data = irq_to_regmap_irq(d, data->irq);
+
+	d->mask_buf[irq_data->reg_offset / map->reg_stride] |= irq_data->mask;
+}
+
+static struct irq_chip regmap_irq_chip = {
+	.name			= "regmap",
+	.irq_bus_lock		= regmap_irq_lock,
+	.irq_bus_sync_unlock	= regmap_irq_sync_unlock,
+	.irq_disable		= regmap_irq_disable,
+	.irq_enable		= regmap_irq_enable,
+};
+
+static irqreturn_t regmap_irq_thread(int irq, void *d)
+{
+	struct regmap_irq_chip_data *data = d;
+	struct regmap_irq_chip *chip = data->chip;
+	struct regmap *map = data->map;
+	int ret, i;
+	bool handled = false;
+
+	/*
+	 * Ignore masked IRQs and ack if we need to; we ack early so
+	 * there is no race between handling and acknowleding the
+	 * interrupt.  We assume that typically few of the interrupts
+	 * will fire simultaneously so don't worry about overhead from
+	 * doing a write per register.
+	 */
+	for (i = 0; i < data->chip->num_regs; i++) {
+		ret = regmap_read(map, chip->mask_base + (i * map->reg_stride
+				   * data->irq_reg_stride),
+				   &data->status_buf[i]);
+
+		if (ret != 0) {
+			dev_err(map->dev, "Failed to read IRQ status: %d\n",
+					ret);
+			return IRQ_NONE;
+		}
+
+		data->status_buf[i] &= ~data->mask_buf[i];
+
+		if (data->status_buf[i] && chip->ack_base) {
+			ret = regmap_write(map, chip->ack_base +
+						(i * map->reg_stride *
+						data->irq_reg_stride),
+					   data->status_buf[i]);
+			if (ret != 0)
+				dev_err(map->dev, "Failed to ack 0x%x: %d\n",
+					chip->ack_base + (i * map->reg_stride),
+					ret);
+		}
+	}
+
+	for (i = 0; i < chip->num_irqs; i++) {
+		if (data->status_buf[chip->irqs[i].reg_offset /
+				     map->reg_stride] & chip->irqs[i].mask) {
+			handle_nested_irq(data->irq_base + i);
+			handled = true;
+		}
+	}
+
+	if (handled)
+		return IRQ_HANDLED;
+	else
+		return IRQ_NONE;
+}
+
+/**
+ * regmap_add_irq_chip(): Use standard regmap IRQ controller handling
+ *
+ * map:       The regmap for the device.
+ * irq:       The IRQ the device uses to signal interrupts
+ * irq_flags: The IRQF_ flags to use for the primary interrupt.
+ * chip:      Configuration for the interrupt controller.
+ * data:      Runtime data structure for the controller, allocated on success
+ *
+ * Returns 0 on success or an errno on failure.
+ *
+ * In order for this to be efficient the chip really should use a
+ * register cache.  The chip driver is responsible for restoring the
+ * register values used by the IRQ controller over suspend and resume.
+ */
+int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
+			int irq_base, struct regmap_irq_chip *chip,
+			struct regmap_irq_chip_data **data)
+{
+	struct regmap_irq_chip_data *d;
+	int cur_irq, i;
+	int ret = -ENOMEM;
+
+	for (i = 0; i < chip->num_irqs; i++) {
+		if (chip->irqs[i].reg_offset % map->reg_stride)
+			return -EINVAL;
+		if (chip->irqs[i].reg_offset / map->reg_stride >=
+		    chip->num_regs)
+			return -EINVAL;
+	}
+
+	irq_base = irq_alloc_descs(irq_base, 0, chip->num_irqs, 0);
+	if (irq_base < 0) {
+		dev_warn(map->dev, "Failed to allocate IRQs: %d\n",
+			 irq_base);
+		return irq_base;
+	}
+
+	d = kzalloc(sizeof(*d), GFP_KERNEL);
+	if (!d)
+		return -ENOMEM;
+
+	d->status_buf = kzalloc(sizeof(unsigned int) * chip->num_regs,
+				GFP_KERNEL);
+	if (!d->status_buf)
+		goto err_alloc;
+
+	d->mask_buf = kzalloc(sizeof(unsigned int) * chip->num_regs,
+			      GFP_KERNEL);
+	if (!d->mask_buf)
+		goto err_alloc;
+
+	d->mask_buf_def = kzalloc(sizeof(unsigned int) * chip->num_regs,
+				  GFP_KERNEL);
+	if (!d->mask_buf_def)
+		goto err_alloc;
+
+	d->map = map;
+	d->chip = chip;
+	d->irq_base = irq_base;
+
+	if (chip->irq_reg_stride)
+		d->irq_reg_stride = chip->irq_reg_stride;
+	else
+		d->irq_reg_stride = 1;
+
+	mutex_init(&d->lock);
+
+	for (i = 0; i < chip->num_irqs; i++)
+		d->mask_buf_def[chip->irqs[i].reg_offset / map->reg_stride]
+			|= chip->irqs[i].mask;
+
+	/* Mask all the interrupts by default */
+	for (i = 0; i < chip->num_regs; i++) {
+		d->mask_buf[i] = d->mask_buf_def[i];
+		ret = regmap_write(map, chip->mask_base + (i * map->reg_stride
+				   * d->irq_reg_stride),
+				   d->mask_buf[i]);
+		if (ret != 0) {
+			dev_err(map->dev, "Failed to set masks in 0x%x: %d\n",
+				chip->mask_base + (i * map->reg_stride), ret);
+			goto err_alloc;
+		}
+	}
+
+	/* Register them with genirq */
+	for (cur_irq = irq_base;
+	     cur_irq < chip->num_irqs + irq_base;
+	     cur_irq++) {
+		irq_set_chip_data(cur_irq, d);
+		irq_set_chip_and_handler(cur_irq, &regmap_irq_chip,
+					 handle_edge_irq);
+		irq_set_nested_thread(cur_irq, 1);
+
+		/* ARM needs us to explicitly flag the IRQ as valid
+		 * and will set them noprobe when we do so. */
+#ifdef CONFIG_ARM
+		set_irq_flags(cur_irq, IRQF_VALID);
+#else
+		irq_set_noprobe(cur_irq);
+#endif
+	}
+
+	ret = request_threaded_irq(irq, NULL, regmap_irq_thread, irq_flags,
+				   chip->name, d);
+	if (ret != 0) {
+		dev_err(map->dev, "Failed to request IRQ %d: %d\n", irq, ret);
+		goto err_alloc;
+	}
+
+	return 0;
+
+err_alloc:
+	kfree(d->mask_buf_def);
+	kfree(d->mask_buf);
+	kfree(d->status_buf);
+	kfree(d);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(regmap_add_irq_chip);
+
+/**
+ * regmap_del_irq_chip(): Stop interrupt handling for a regmap IRQ chip
+ *
+ * @irq: Primary IRQ for the device
+ * @d:   regmap_irq_chip_data allocated by regmap_add_irq_chip()
+ */
+void regmap_del_irq_chip(int irq, struct regmap_irq_chip_data *d)
+{
+	if (!d)
+		return;
+
+	free_irq(irq, d);
+	kfree(d->mask_buf_def);
+	kfree(d->mask_buf);
+	kfree(d->status_buf);
+	kfree(d);
+}
+EXPORT_SYMBOL_GPL(regmap_del_irq_chip);
+
+/**
+ * regmap_irq_chip_get_base(): Retrieve interrupt base for a regmap IRQ chip
+ *
+ * Useful for drivers to request their own IRQs.
+ *
+ * @data: regmap_irq controller to operate on.
+ */
+int regmap_irq_chip_get_base(struct regmap_irq_chip_data *data)
+{
+	return data->irq_base;
+}
+EXPORT_SYMBOL_GPL(regmap_irq_chip_get_base);
diff --git a/drivers/base/regmap/regmap-mmio.c b/drivers/base/regmap/regmap-mmio.c
new file mode 100644
index 000000000000..febd6de6c8ac
--- /dev/null
+++ b/drivers/base/regmap/regmap-mmio.c
@@ -0,0 +1,224 @@
+/*
+ * Register map access API - MMIO support
+ *
+ * Copyright (c) 2012, NVIDIA 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+
+struct regmap_mmio_context {
+	void __iomem *regs;
+	unsigned val_bytes;
+};
+
+static int regmap_mmio_gather_write(void *context,
+				    const void *reg, size_t reg_size,
+				    const void *val, size_t val_size)
+{
+	struct regmap_mmio_context *ctx = context;
+	u32 offset;
+
+	BUG_ON(reg_size != 4);
+
+	offset = be32_to_cpup(reg);
+
+	while (val_size) {
+		switch (ctx->val_bytes) {
+		case 1:
+			writeb(*(u8 *)val, ctx->regs + offset);
+			break;
+		case 2:
+			writew(be16_to_cpup(val), ctx->regs + offset);
+			break;
+		case 4:
+			writel(be32_to_cpup(val), ctx->regs + offset);
+			break;
+#ifdef CONFIG_64BIT
+		case 8:
+			writeq(be64_to_cpup(val), ctx->regs + offset);
+			break;
+#endif
+		default:
+			/* Should be caught by regmap_mmio_check_config */
+			BUG();
+		}
+		val_size -= ctx->val_bytes;
+		val += ctx->val_bytes;
+		offset += ctx->val_bytes;
+	}
+
+	return 0;
+}
+
+static int regmap_mmio_write(void *context, const void *data, size_t count)
+{
+	BUG_ON(count < 4);
+
+	return regmap_mmio_gather_write(context, data, 4, data + 4, count - 4);
+}
+
+static int regmap_mmio_read(void *context,
+			    const void *reg, size_t reg_size,
+			    void *val, size_t val_size)
+{
+	struct regmap_mmio_context *ctx = context;
+	u32 offset;
+
+	BUG_ON(reg_size != 4);
+
+	offset = be32_to_cpup(reg);
+
+	while (val_size) {
+		switch (ctx->val_bytes) {
+		case 1:
+			*(u8 *)val = readb(ctx->regs + offset);
+			break;
+		case 2:
+			*(u16 *)val = cpu_to_be16(readw(ctx->regs + offset));
+			break;
+		case 4:
+			*(u32 *)val = cpu_to_be32(readl(ctx->regs + offset));
+			break;
+#ifdef CONFIG_64BIT
+		case 8:
+			*(u64 *)val = cpu_to_be32(readq(ctx->regs + offset));
+			break;
+#endif
+		default:
+			/* Should be caught by regmap_mmio_check_config */
+			BUG();
+		}
+		val_size -= ctx->val_bytes;
+		val += ctx->val_bytes;
+		offset += ctx->val_bytes;
+	}
+
+	return 0;
+}
+
+static void regmap_mmio_free_context(void *context)
+{
+	kfree(context);
+}
+
+static struct regmap_bus regmap_mmio = {
+	.fast_io = true,
+	.write = regmap_mmio_write,
+	.gather_write = regmap_mmio_gather_write,
+	.read = regmap_mmio_read,
+	.free_context = regmap_mmio_free_context,
+};
+
+struct regmap_mmio_context *regmap_mmio_gen_context(void __iomem *regs,
+					const struct regmap_config *config)
+{
+	struct regmap_mmio_context *ctx;
+	int min_stride;
+
+	if (config->reg_bits != 32)
+		return ERR_PTR(-EINVAL);
+
+	if (config->pad_bits)
+		return ERR_PTR(-EINVAL);
+
+	switch (config->val_bits) {
+	case 8:
+		/* The core treats 0 as 1 */
+		min_stride = 0;
+		break;
+	case 16:
+		min_stride = 2;
+		break;
+	case 32:
+		min_stride = 4;
+		break;
+#ifdef CONFIG_64BIT
+	case 64:
+		min_stride = 8;
+		break;
+#endif
+		break;
+	default:
+		return ERR_PTR(-EINVAL);
+	}
+
+	if (config->reg_stride < min_stride)
+		return ERR_PTR(-EINVAL);
+
+	ctx = kzalloc(GFP_KERNEL, sizeof(*ctx));
+	if (!ctx)
+		return ERR_PTR(-ENOMEM);
+
+	ctx->regs = regs;
+	ctx->val_bytes = config->val_bits / 8;
+
+	return ctx;
+}
+
+/**
+ * regmap_init_mmio(): Initialise register map
+ *
+ * @dev: Device that will be interacted with
+ * @regs: Pointer to memory-mapped IO region
+ * @config: Configuration for register map
+ *
+ * The return value will be an ERR_PTR() on error or a valid pointer to
+ * a struct regmap.
+ */
+struct regmap *regmap_init_mmio(struct device *dev,
+				void __iomem *regs,
+				const struct regmap_config *config)
+{
+	struct regmap_mmio_context *ctx;
+
+	ctx = regmap_mmio_gen_context(regs, config);
+	if (IS_ERR(ctx))
+		return ERR_CAST(ctx);
+
+	return regmap_init(dev, &regmap_mmio, ctx, config);
+}
+EXPORT_SYMBOL_GPL(regmap_init_mmio);
+
+/**
+ * devm_regmap_init_mmio(): Initialise managed register map
+ *
+ * @dev: Device that will be interacted with
+ * @regs: Pointer to memory-mapped IO region
+ * @config: Configuration for register map
+ *
+ * The return value will be an ERR_PTR() on error or a valid pointer
+ * to a struct regmap.  The regmap will be automatically freed by the
+ * device management code.
+ */
+struct regmap *devm_regmap_init_mmio(struct device *dev,
+				     void __iomem *regs,
+				     const struct regmap_config *config)
+{
+	struct regmap_mmio_context *ctx;
+
+	ctx = regmap_mmio_gen_context(regs, config);
+	if (IS_ERR(ctx))
+		return ERR_CAST(ctx);
+
+	return devm_regmap_init(dev, &regmap_mmio, ctx, config);
+}
+EXPORT_SYMBOL_GPL(devm_regmap_init_mmio);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/base/regmap/regmap-spi.c b/drivers/base/regmap/regmap-spi.c
index f8396945d6ed..ffa46a92ad33 100644
--- a/drivers/base/regmap/regmap-spi.c
+++ b/drivers/base/regmap/regmap-spi.c
@@ -15,17 +15,19 @@
 #include <linux/init.h>
 #include <linux/module.h>
 
-static int regmap_spi_write(struct device *dev, const void *data, size_t count)
+static int regmap_spi_write(void *context, const void *data, size_t count)
 {
+	struct device *dev = context;
 	struct spi_device *spi = to_spi_device(dev);
 
 	return spi_write(spi, data, count);
 }
 
-static int regmap_spi_gather_write(struct device *dev,
+static int regmap_spi_gather_write(void *context,
 				   const void *reg, size_t reg_len,
 				   const void *val, size_t val_len)
 {
+	struct device *dev = context;
 	struct spi_device *spi = to_spi_device(dev);
 	struct spi_message m;
 	struct spi_transfer t[2] = { { .tx_buf = reg, .len = reg_len, },
@@ -38,21 +40,20 @@ static int regmap_spi_gather_write(struct device *dev,
 	return spi_sync(spi, &m);
 }
 
-static int regmap_spi_read(struct device *dev,
+static int regmap_spi_read(void *context,
 			   const void *reg, size_t reg_size,
 			   void *val, size_t val_size)
 {
+	struct device *dev = context;
 	struct spi_device *spi = to_spi_device(dev);
 
 	return spi_write_then_read(spi, reg, reg_size, val, val_size);
 }
 
 static struct regmap_bus regmap_spi = {
-	.type = &spi_bus_type,
 	.write = regmap_spi_write,
 	.gather_write = regmap_spi_gather_write,
 	.read = regmap_spi_read,
-	.owner = THIS_MODULE,
 	.read_flag_mask = 0x80,
 };
 
@@ -68,8 +69,25 @@ static struct regmap_bus regmap_spi = {
 struct regmap *regmap_init_spi(struct spi_device *spi,
 			       const struct regmap_config *config)
 {
-	return regmap_init(&spi->dev, &regmap_spi, config);
+	return regmap_init(&spi->dev, &regmap_spi, &spi->dev, config);
 }
 EXPORT_SYMBOL_GPL(regmap_init_spi);
 
+/**
+ * devm_regmap_init_spi(): Initialise register map
+ *
+ * @spi: Device that will be interacted with
+ * @config: Configuration for register map
+ *
+ * The return value will be an ERR_PTR() on error or a valid pointer
+ * to a struct regmap.  The map will be automatically freed by the
+ * device management code.
+ */
+struct regmap *devm_regmap_init_spi(struct spi_device *spi,
+				    const struct regmap_config *config)
+{
+	return devm_regmap_init(&spi->dev, &regmap_spi, &spi->dev, config);
+}
+EXPORT_SYMBOL_GPL(devm_regmap_init_spi);
+
 MODULE_LICENSE("GPL");
diff --git a/drivers/base/regmap/regmap.c b/drivers/base/regmap/regmap.c
index 20663f8dae45..1eb6de3ac479 100644
--- a/drivers/base/regmap/regmap.c
+++ b/drivers/base/regmap/regmap.c
@@ -10,34 +10,83 @@
  * published by the Free Software Foundation.
  */
 
+#include <linux/device.h>
 #include <linux/slab.h>
-#include <linux/module.h>
+#include <linux/export.h>
 #include <linux/mutex.h>
 #include <linux/err.h>
 
-#include <linux/regmap.h>
+#define CREATE_TRACE_POINTS
+#include <trace/events/regmap.h>
 
-struct regmap;
+#include "internal.h"
 
-struct regmap_format {
-	size_t buf_size;
-	size_t reg_bytes;
-	size_t val_bytes;
-	void (*format_write)(struct regmap *map,
-			     unsigned int reg, unsigned int val);
-	void (*format_reg)(void *buf, unsigned int reg);
-	void (*format_val)(void *buf, unsigned int val);
-	unsigned int (*parse_val)(void *buf);
-};
+bool regmap_writeable(struct regmap *map, unsigned int reg)
+{
+	if (map->max_register && reg > map->max_register)
+		return false;
+
+	if (map->writeable_reg)
+		return map->writeable_reg(map->dev, reg);
+
+	return true;
+}
+
+bool regmap_readable(struct regmap *map, unsigned int reg)
+{
+	if (map->max_register && reg > map->max_register)
+		return false;
+
+	if (map->format.format_write)
+		return false;
+
+	if (map->readable_reg)
+		return map->readable_reg(map->dev, reg);
+
+	return true;
+}
+
+bool regmap_volatile(struct regmap *map, unsigned int reg)
+{
+	if (!regmap_readable(map, reg))
+		return false;
+
+	if (map->volatile_reg)
+		return map->volatile_reg(map->dev, reg);
+
+	return true;
+}
+
+bool regmap_precious(struct regmap *map, unsigned int reg)
+{
+	if (!regmap_readable(map, reg))
+		return false;
+
+	if (map->precious_reg)
+		return map->precious_reg(map->dev, reg);
 
-struct regmap {
-	struct mutex lock;
+	return false;
+}
+
+static bool regmap_volatile_range(struct regmap *map, unsigned int reg,
+	unsigned int num)
+{
+	unsigned int i;
+
+	for (i = 0; i < num; i++)
+		if (!regmap_volatile(map, reg + i))
+			return false;
+
+	return true;
+}
+
+static void regmap_format_2_6_write(struct regmap *map,
+				     unsigned int reg, unsigned int val)
+{
+	u8 *out = map->work_buf;
 
-	struct device *dev; /* Device we do I/O on */
-	void *work_buf;     /* Scratch buffer used to format I/O */
-	struct regmap_format format;  /* Buffer format */
-	const struct regmap_bus *bus;
-};
+	*out = (reg << 6) | val;
+}
 
 static void regmap_format_4_12_write(struct regmap *map,
 				     unsigned int reg, unsigned int val)
@@ -53,18 +102,46 @@ static void regmap_format_7_9_write(struct regmap *map,
 	*out = cpu_to_be16((reg << 9) | val);
 }
 
-static void regmap_format_8(void *buf, unsigned int val)
+static void regmap_format_10_14_write(struct regmap *map,
+				    unsigned int reg, unsigned int val)
+{
+	u8 *out = map->work_buf;
+
+	out[2] = val;
+	out[1] = (val >> 8) | (reg << 6);
+	out[0] = reg >> 2;
+}
+
+static void regmap_format_8(void *buf, unsigned int val, unsigned int shift)
 {
 	u8 *b = buf;
 
-	b[0] = val;
+	b[0] = val << shift;
 }
 
-static void regmap_format_16(void *buf, unsigned int val)
+static void regmap_format_16(void *buf, unsigned int val, unsigned int shift)
 {
 	__be16 *b = buf;
 
-	b[0] = cpu_to_be16(val);
+	b[0] = cpu_to_be16(val << shift);
+}
+
+static void regmap_format_24(void *buf, unsigned int val, unsigned int shift)
+{
+	u8 *b = buf;
+
+	val <<= shift;
+
+	b[0] = val >> 16;
+	b[1] = val >> 8;
+	b[2] = val;
+}
+
+static void regmap_format_32(void *buf, unsigned int val, unsigned int shift)
+{
+	__be32 *b = buf;
+
+	b[0] = cpu_to_be32(val << shift);
 }
 
 static unsigned int regmap_parse_8(void *buf)
@@ -83,11 +160,51 @@ static unsigned int regmap_parse_16(void *buf)
 	return b[0];
 }
 
+static unsigned int regmap_parse_24(void *buf)
+{
+	u8 *b = buf;
+	unsigned int ret = b[2];
+	ret |= ((unsigned int)b[1]) << 8;
+	ret |= ((unsigned int)b[0]) << 16;
+
+	return ret;
+}
+
+static unsigned int regmap_parse_32(void *buf)
+{
+	__be32 *b = buf;
+
+	b[0] = be32_to_cpu(b[0]);
+
+	return b[0];
+}
+
+static void regmap_lock_mutex(struct regmap *map)
+{
+	mutex_lock(&map->mutex);
+}
+
+static void regmap_unlock_mutex(struct regmap *map)
+{
+	mutex_unlock(&map->mutex);
+}
+
+static void regmap_lock_spinlock(struct regmap *map)
+{
+	spin_lock(&map->spinlock);
+}
+
+static void regmap_unlock_spinlock(struct regmap *map)
+{
+	spin_unlock(&map->spinlock);
+}
+
 /**
  * regmap_init(): Initialise register map
  *
  * @dev: Device that will be interacted with
  * @bus: Bus-specific callbacks to use with device
+ * @bus_context: Data passed to bus-specific callbacks
  * @config: Configuration for register map
  *
  * The return value will be an ERR_PTR() on error or a valid pointer to
@@ -96,13 +213,14 @@ static unsigned int regmap_parse_16(void *buf)
  */
 struct regmap *regmap_init(struct device *dev,
 			   const struct regmap_bus *bus,
+			   void *bus_context,
 			   const struct regmap_config *config)
 {
 	struct regmap *map;
 	int ret = -EINVAL;
 
 	if (!bus || !config)
-		return NULL;
+		goto err;
 
 	map = kzalloc(sizeof(*map), GFP_KERNEL);
 	if (map == NULL) {
@@ -110,14 +228,53 @@ struct regmap *regmap_init(struct device *dev,
 		goto err;
 	}
 
-	mutex_init(&map->lock);
+	if (bus->fast_io) {
+		spin_lock_init(&map->spinlock);
+		map->lock = regmap_lock_spinlock;
+		map->unlock = regmap_unlock_spinlock;
+	} else {
+		mutex_init(&map->mutex);
+		map->lock = regmap_lock_mutex;
+		map->unlock = regmap_unlock_mutex;
+	}
 	map->format.buf_size = (config->reg_bits + config->val_bits) / 8;
-	map->format.reg_bytes = config->reg_bits / 8;
-	map->format.val_bytes = config->val_bits / 8;
+	map->format.reg_bytes = DIV_ROUND_UP(config->reg_bits, 8);
+	map->format.pad_bytes = config->pad_bits / 8;
+	map->format.val_bytes = DIV_ROUND_UP(config->val_bits, 8);
+	map->format.buf_size += map->format.pad_bytes;
+	map->reg_shift = config->pad_bits % 8;
+	if (config->reg_stride)
+		map->reg_stride = config->reg_stride;
+	else
+		map->reg_stride = 1;
 	map->dev = dev;
 	map->bus = bus;
+	map->bus_context = bus_context;
+	map->max_register = config->max_register;
+	map->writeable_reg = config->writeable_reg;
+	map->readable_reg = config->readable_reg;
+	map->volatile_reg = config->volatile_reg;
+	map->precious_reg = config->precious_reg;
+	map->cache_type = config->cache_type;
+
+	if (config->read_flag_mask || config->write_flag_mask) {
+		map->read_flag_mask = config->read_flag_mask;
+		map->write_flag_mask = config->write_flag_mask;
+	} else {
+		map->read_flag_mask = bus->read_flag_mask;
+	}
+
+	switch (config->reg_bits + map->reg_shift) {
+	case 2:
+		switch (config->val_bits) {
+		case 6:
+			map->format.format_write = regmap_format_2_6_write;
+			break;
+		default:
+			goto err_map;
+		}
+		break;
 
-	switch (config->reg_bits) {
 	case 4:
 		switch (config->val_bits) {
 		case 12:
@@ -138,6 +295,16 @@ struct regmap *regmap_init(struct device *dev,
 		}
 		break;
 
+	case 10:
+		switch (config->val_bits) {
+		case 14:
+			map->format.format_write = regmap_format_10_14_write;
+			break;
+		default:
+			goto err_map;
+		}
+		break;
+
 	case 8:
 		map->format.format_reg = regmap_format_8;
 		break;
@@ -146,6 +313,10 @@ struct regmap *regmap_init(struct device *dev,
 		map->format.format_reg = regmap_format_16;
 		break;
 
+	case 32:
+		map->format.format_reg = regmap_format_32;
+		break;
+
 	default:
 		goto err_map;
 	}
@@ -159,20 +330,36 @@ struct regmap *regmap_init(struct device *dev,
 		map->format.format_val = regmap_format_16;
 		map->format.parse_val = regmap_parse_16;
 		break;
+	case 24:
+		map->format.format_val = regmap_format_24;
+		map->format.parse_val = regmap_parse_24;
+		break;
+	case 32:
+		map->format.format_val = regmap_format_32;
+		map->format.parse_val = regmap_parse_32;
+		break;
 	}
 
 	if (!map->format.format_write &&
 	    !(map->format.format_reg && map->format.format_val))
 		goto err_map;
 
-	map->work_buf = kmalloc(map->format.buf_size, GFP_KERNEL);
+	map->work_buf = kzalloc(map->format.buf_size, GFP_KERNEL);
 	if (map->work_buf == NULL) {
 		ret = -ENOMEM;
 		goto err_map;
 	}
 
+	regmap_debugfs_init(map, config->name);
+
+	ret = regcache_init(map, config);
+	if (ret < 0)
+		goto err_free_workbuf;
+
 	return map;
 
+err_free_workbuf:
+	kfree(map->work_buf);
 err_map:
 	kfree(map);
 err:
@@ -180,11 +367,95 @@ err:
 }
 EXPORT_SYMBOL_GPL(regmap_init);
 
+static void devm_regmap_release(struct device *dev, void *res)
+{
+	regmap_exit(*(struct regmap **)res);
+}
+
+/**
+ * devm_regmap_init(): Initialise managed register map
+ *
+ * @dev: Device that will be interacted with
+ * @bus: Bus-specific callbacks to use with device
+ * @bus_context: Data passed to bus-specific callbacks
+ * @config: Configuration for register map
+ *
+ * The return value will be an ERR_PTR() on error or a valid pointer
+ * to a struct regmap.  This function should generally not be called
+ * directly, it should be called by bus-specific init functions.  The
+ * map will be automatically freed by the device management code.
+ */
+struct regmap *devm_regmap_init(struct device *dev,
+				const struct regmap_bus *bus,
+				void *bus_context,
+				const struct regmap_config *config)
+{
+	struct regmap **ptr, *regmap;
+
+	ptr = devres_alloc(devm_regmap_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	regmap = regmap_init(dev, bus, bus_context, config);
+	if (!IS_ERR(regmap)) {
+		*ptr = regmap;
+		devres_add(dev, ptr);
+	} else {
+		devres_free(ptr);
+	}
+
+	return regmap;
+}
+EXPORT_SYMBOL_GPL(devm_regmap_init);
+
+/**
+ * regmap_reinit_cache(): Reinitialise the current register cache
+ *
+ * @map: Register map to operate on.
+ * @config: New configuration.  Only the cache data will be used.
+ *
+ * Discard any existing register cache for the map and initialize a
+ * new cache.  This can be used to restore the cache to defaults or to
+ * update the cache configuration to reflect runtime discovery of the
+ * hardware.
+ */
+int regmap_reinit_cache(struct regmap *map, const struct regmap_config *config)
+{
+	int ret;
+
+	map->lock(map);
+
+	regcache_exit(map);
+	regmap_debugfs_exit(map);
+
+	map->max_register = config->max_register;
+	map->writeable_reg = config->writeable_reg;
+	map->readable_reg = config->readable_reg;
+	map->volatile_reg = config->volatile_reg;
+	map->precious_reg = config->precious_reg;
+	map->cache_type = config->cache_type;
+
+	regmap_debugfs_init(map, config->name);
+
+	map->cache_bypass = false;
+	map->cache_only = false;
+
+	ret = regcache_init(map, config);
+
+	map->unlock(map);
+
+	return ret;
+}
+
 /**
  * regmap_exit(): Free a previously allocated register map
  */
 void regmap_exit(struct regmap *map)
 {
+	regcache_exit(map);
+	regmap_debugfs_exit(map);
+	if (map->bus->free_context)
+		map->bus->free_context(map->bus_context);
 	kfree(map->work_buf);
 	kfree(map);
 }
@@ -193,50 +464,120 @@ EXPORT_SYMBOL_GPL(regmap_exit);
 static int _regmap_raw_write(struct regmap *map, unsigned int reg,
 			     const void *val, size_t val_len)
 {
+	u8 *u8 = map->work_buf;
 	void *buf;
 	int ret = -ENOTSUPP;
 	size_t len;
+	int i;
+
+	/* Check for unwritable registers before we start */
+	if (map->writeable_reg)
+		for (i = 0; i < val_len / map->format.val_bytes; i++)
+			if (!map->writeable_reg(map->dev,
+						reg + (i * map->reg_stride)))
+				return -EINVAL;
+
+	if (!map->cache_bypass && map->format.parse_val) {
+		unsigned int ival;
+		int val_bytes = map->format.val_bytes;
+		for (i = 0; i < val_len / val_bytes; i++) {
+			memcpy(map->work_buf, val + (i * val_bytes), val_bytes);
+			ival = map->format.parse_val(map->work_buf);
+			ret = regcache_write(map, reg + (i * map->reg_stride),
+					     ival);
+			if (ret) {
+				dev_err(map->dev,
+				   "Error in caching of register: %u ret: %d\n",
+					reg + i, ret);
+				return ret;
+			}
+		}
+		if (map->cache_only) {
+			map->cache_dirty = true;
+			return 0;
+		}
+	}
+
+	map->format.format_reg(map->work_buf, reg, map->reg_shift);
 
-	map->format.format_reg(map->work_buf, reg);
+	u8[0] |= map->write_flag_mask;
 
-	/* Try to do a gather write if we can */
-	if (map->bus->gather_write)
-		ret = map->bus->gather_write(map->dev, map->work_buf,
-					     map->format.reg_bytes,
+	trace_regmap_hw_write_start(map->dev, reg,
+				    val_len / map->format.val_bytes);
+
+	/* If we're doing a single register write we can probably just
+	 * send the work_buf directly, otherwise try to do a gather
+	 * write.
+	 */
+	if (val == (map->work_buf + map->format.pad_bytes +
+		    map->format.reg_bytes))
+		ret = map->bus->write(map->bus_context, map->work_buf,
+				      map->format.reg_bytes +
+				      map->format.pad_bytes +
+				      val_len);
+	else if (map->bus->gather_write)
+		ret = map->bus->gather_write(map->bus_context, map->work_buf,
+					     map->format.reg_bytes +
+					     map->format.pad_bytes,
 					     val, val_len);
 
-	/* Otherwise fall back on linearising by hand. */
+	/* If that didn't work fall back on linearising by hand. */
 	if (ret == -ENOTSUPP) {
-		len = map->format.reg_bytes + val_len;
-		buf = kmalloc(len, GFP_KERNEL);
+		len = map->format.reg_bytes + map->format.pad_bytes + val_len;
+		buf = kzalloc(len, GFP_KERNEL);
 		if (!buf)
 			return -ENOMEM;
 
 		memcpy(buf, map->work_buf, map->format.reg_bytes);
-		memcpy(buf + map->format.reg_bytes, val, val_len);
-		ret = map->bus->write(map->dev, buf, len);
+		memcpy(buf + map->format.reg_bytes + map->format.pad_bytes,
+		       val, val_len);
+		ret = map->bus->write(map->bus_context, buf, len);
 
 		kfree(buf);
 	}
 
+	trace_regmap_hw_write_done(map->dev, reg,
+				   val_len / map->format.val_bytes);
+
 	return ret;
 }
 
-static int _regmap_write(struct regmap *map, unsigned int reg,
-			 unsigned int val)
+int _regmap_write(struct regmap *map, unsigned int reg,
+		  unsigned int val)
 {
+	int ret;
 	BUG_ON(!map->format.format_write && !map->format.format_val);
 
+	if (!map->cache_bypass && map->format.format_write) {
+		ret = regcache_write(map, reg, val);
+		if (ret != 0)
+			return ret;
+		if (map->cache_only) {
+			map->cache_dirty = true;
+			return 0;
+		}
+	}
+
+	trace_regmap_reg_write(map->dev, reg, val);
+
 	if (map->format.format_write) {
 		map->format.format_write(map, reg, val);
 
-		return map->bus->write(map->dev, map->work_buf,
-				       map->format.buf_size);
+		trace_regmap_hw_write_start(map->dev, reg, 1);
+
+		ret = map->bus->write(map->bus_context, map->work_buf,
+				      map->format.buf_size);
+
+		trace_regmap_hw_write_done(map->dev, reg, 1);
+
+		return ret;
 	} else {
-		map->format.format_val(map->work_buf + map->format.reg_bytes,
-				       val);
+		map->format.format_val(map->work_buf + map->format.reg_bytes
+				       + map->format.pad_bytes, val, 0);
 		return _regmap_raw_write(map, reg,
-					 map->work_buf + map->format.reg_bytes,
+					 map->work_buf +
+					 map->format.reg_bytes +
+					 map->format.pad_bytes,
 					 map->format.val_bytes);
 	}
 }
@@ -255,11 +596,14 @@ int regmap_write(struct regmap *map, unsigned int reg, unsigned int val)
 {
 	int ret;
 
-	mutex_lock(&map->lock);
+	if (reg % map->reg_stride)
+		return -EINVAL;
+
+	map->lock(map);
 
 	ret = _regmap_write(map, reg, val);
 
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 
 	return ret;
 }
@@ -286,39 +630,100 @@ int regmap_raw_write(struct regmap *map, unsigned int reg,
 {
 	int ret;
 
-	mutex_lock(&map->lock);
+	if (val_len % map->format.val_bytes)
+		return -EINVAL;
+	if (reg % map->reg_stride)
+		return -EINVAL;
+
+	map->lock(map);
 
 	ret = _regmap_raw_write(map, reg, val, val_len);
 
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 
 	return ret;
 }
 EXPORT_SYMBOL_GPL(regmap_raw_write);
 
+/*
+ * regmap_bulk_write(): Write multiple registers to the device
+ *
+ * @map: Register map to write to
+ * @reg: First register to be write from
+ * @val: Block of data to be written, in native register size for device
+ * @val_count: Number of registers to write
+ *
+ * This function is intended to be used for writing a large block of
+ * data to be device either in single transfer or multiple transfer.
+ *
+ * A value of zero will be returned on success, a negative errno will
+ * be returned in error cases.
+ */
+int regmap_bulk_write(struct regmap *map, unsigned int reg, const void *val,
+		     size_t val_count)
+{
+	int ret = 0, i;
+	size_t val_bytes = map->format.val_bytes;
+	void *wval;
+
+	if (!map->format.parse_val)
+		return -EINVAL;
+	if (reg % map->reg_stride)
+		return -EINVAL;
+
+	map->lock(map);
+
+	/* No formatting is require if val_byte is 1 */
+	if (val_bytes == 1) {
+		wval = (void *)val;
+	} else {
+		wval = kmemdup(val, val_count * val_bytes, GFP_KERNEL);
+		if (!wval) {
+			ret = -ENOMEM;
+			dev_err(map->dev, "Error in memory allocation\n");
+			goto out;
+		}
+		for (i = 0; i < val_count * val_bytes; i += val_bytes)
+			map->format.parse_val(wval + i);
+	}
+	ret = _regmap_raw_write(map, reg, wval, val_bytes * val_count);
+
+	if (val_bytes != 1)
+		kfree(wval);
+
+out:
+	map->unlock(map);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(regmap_bulk_write);
+
 static int _regmap_raw_read(struct regmap *map, unsigned int reg, void *val,
 			    unsigned int val_len)
 {
 	u8 *u8 = map->work_buf;
 	int ret;
 
-	map->format.format_reg(map->work_buf, reg);
+	map->format.format_reg(map->work_buf, reg, map->reg_shift);
 
 	/*
-	 * Some buses flag reads by setting the high bits in the
+	 * Some buses or devices flag reads by setting the high bits in the
 	 * register addresss; since it's always the high bits for all
 	 * current formats we can do this here rather than in
 	 * formatting.  This may break if we get interesting formats.
 	 */
-	if (map->bus->read_flag_mask)
-		u8[0] |= map->bus->read_flag_mask;
+	u8[0] |= map->read_flag_mask;
+
+	trace_regmap_hw_read_start(map->dev, reg,
+				   val_len / map->format.val_bytes);
 
-	ret = map->bus->read(map->dev, map->work_buf, map->format.reg_bytes,
+	ret = map->bus->read(map->bus_context, map->work_buf,
+			     map->format.reg_bytes + map->format.pad_bytes,
 			     val, val_len);
-	if (ret != 0)
-		return ret;
 
-	return 0;
+	trace_regmap_hw_read_done(map->dev, reg,
+				  val_len / map->format.val_bytes);
+
+	return ret;
 }
 
 static int _regmap_read(struct regmap *map, unsigned int reg,
@@ -326,12 +731,23 @@ static int _regmap_read(struct regmap *map, unsigned int reg,
 {
 	int ret;
 
+	if (!map->cache_bypass) {
+		ret = regcache_read(map, reg, val);
+		if (ret == 0)
+			return 0;
+	}
+
 	if (!map->format.parse_val)
 		return -EINVAL;
 
+	if (map->cache_only)
+		return -EBUSY;
+
 	ret = _regmap_raw_read(map, reg, map->work_buf, map->format.val_bytes);
-	if (ret == 0)
+	if (ret == 0) {
 		*val = map->format.parse_val(map->work_buf);
+		trace_regmap_reg_read(map->dev, reg, *val);
+	}
 
 	return ret;
 }
@@ -350,11 +766,14 @@ int regmap_read(struct regmap *map, unsigned int reg, unsigned int *val)
 {
 	int ret;
 
-	mutex_lock(&map->lock);
+	if (reg % map->reg_stride)
+		return -EINVAL;
+
+	map->lock(map);
 
 	ret = _regmap_read(map, reg, val);
 
-	mutex_unlock(&map->lock);
+	map->unlock(map);
 
 	return ret;
 }
@@ -374,13 +793,39 @@ EXPORT_SYMBOL_GPL(regmap_read);
 int regmap_raw_read(struct regmap *map, unsigned int reg, void *val,
 		    size_t val_len)
 {
-	int ret;
+	size_t val_bytes = map->format.val_bytes;
+	size_t val_count = val_len / val_bytes;
+	unsigned int v;
+	int ret, i;
 
-	mutex_lock(&map->lock);
+	if (val_len % map->format.val_bytes)
+		return -EINVAL;
+	if (reg % map->reg_stride)
+		return -EINVAL;
 
-	ret = _regmap_raw_read(map, reg, val, val_len);
+	map->lock(map);
 
-	mutex_unlock(&map->lock);
+	if (regmap_volatile_range(map, reg, val_count) || map->cache_bypass ||
+	    map->cache_type == REGCACHE_NONE) {
+		/* Physical block read if there's no cache involved */
+		ret = _regmap_raw_read(map, reg, val, val_len);
+
+	} else {
+		/* Otherwise go word by word for the cache; should be low
+		 * cost as we expect to hit the cache.
+		 */
+		for (i = 0; i < val_count; i++) {
+			ret = _regmap_read(map, reg + (i * map->reg_stride),
+					   &v);
+			if (ret != 0)
+				goto out;
+
+			map->format.format_val(val + (i * val_bytes), v, 0);
+		}
+	}
+
+ out:
+	map->unlock(map);
 
 	return ret;
 }
@@ -402,23 +847,64 @@ int regmap_bulk_read(struct regmap *map, unsigned int reg, void *val,
 {
 	int ret, i;
 	size_t val_bytes = map->format.val_bytes;
+	bool vol = regmap_volatile_range(map, reg, val_count);
 
 	if (!map->format.parse_val)
 		return -EINVAL;
+	if (reg % map->reg_stride)
+		return -EINVAL;
 
-	ret = regmap_raw_read(map, reg, val, val_bytes * val_count);
-	if (ret != 0)
-		return ret;
+	if (vol || map->cache_type == REGCACHE_NONE) {
+		ret = regmap_raw_read(map, reg, val, val_bytes * val_count);
+		if (ret != 0)
+			return ret;
 
-	for (i = 0; i < val_count * val_bytes; i += val_bytes)
-		map->format.parse_val(val + i);
+		for (i = 0; i < val_count * val_bytes; i += val_bytes)
+			map->format.parse_val(val + i);
+	} else {
+		for (i = 0; i < val_count; i++) {
+			ret = regmap_read(map, reg + (i * map->reg_stride),
+					  val + (i * val_bytes));
+			if (ret != 0)
+				return ret;
+		}
+	}
 
 	return 0;
 }
 EXPORT_SYMBOL_GPL(regmap_bulk_read);
 
+static int _regmap_update_bits(struct regmap *map, unsigned int reg,
+			       unsigned int mask, unsigned int val,
+			       bool *change)
+{
+	int ret;
+	unsigned int tmp, orig;
+
+	map->lock(map);
+
+	ret = _regmap_read(map, reg, &orig);
+	if (ret != 0)
+		goto out;
+
+	tmp = orig & ~mask;
+	tmp |= val & mask;
+
+	if (tmp != orig) {
+		ret = _regmap_write(map, reg, tmp);
+		*change = true;
+	} else {
+		*change = false;
+	}
+
+out:
+	map->unlock(map);
+
+	return ret;
+}
+
 /**
- * remap_update_bits: Perform a read/modify/write cycle on the register map
+ * regmap_update_bits: Perform a read/modify/write cycle on the register map
  *
  * @map: Register map to update
  * @reg: Register to update
@@ -430,23 +916,143 @@ EXPORT_SYMBOL_GPL(regmap_bulk_read);
 int regmap_update_bits(struct regmap *map, unsigned int reg,
 		       unsigned int mask, unsigned int val)
 {
-	int ret;
+	bool change;
+	return _regmap_update_bits(map, reg, mask, val, &change);
+}
+EXPORT_SYMBOL_GPL(regmap_update_bits);
+
+/**
+ * remap_update_bits_lazy: Perform a read/modify/write cycle on the register
+ * map. Only write new contents if they differ from the previous ones.
+ *
+ * @map: Register map to update
+ * @reg: Register to update
+ * @mask: Bitmask to change
+ * @val: New value for bitmask
+ *
+ * Returns zero for success, a negative number on error.
+ */
+int regmap_update_bits_lazy(struct regmap *map, unsigned int reg,
+		       unsigned int mask, unsigned int val)
+{
+	int ret, new;
 	unsigned int tmp;
 
-	mutex_lock(&map->lock);
+	map->lock(map);
 
 	ret = _regmap_read(map, reg, &tmp);
 	if (ret != 0)
 		goto out;
 
-	tmp &= ~mask;
-	tmp |= val & mask;
+	new = tmp & ~mask;
+	new |= val & mask;
+	if (new != tmp) {
+		ret = _regmap_write(map, reg, new);
+	}
+
+out:
+	map->unlock(map);
 
-	ret = _regmap_write(map, reg, tmp);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(regmap_update_bits_lazy);
+
+/**
+ * regmap_update_bits_check: Perform a read/modify/write cycle on the
+ *                           register map and report if updated
+ * @map: Register map to update
+ * @reg: Register to update
+ * @mask: Bitmask to change
+ * @val: New value for bitmask
+ * @change: Boolean indicating if a write was done
+ *
+ * Returns zero for success, a negative number on error.
+ */
+int regmap_update_bits_check(struct regmap *map, unsigned int reg,
+			     unsigned int mask, unsigned int val,
+			     bool *change)
+{
+	return _regmap_update_bits(map, reg, mask, val, change);
+}
+EXPORT_SYMBOL_GPL(regmap_update_bits_check);
+
+/**
+ * regmap_register_patch: Register and apply register updates to be applied
+ *                        on device initialistion
+ *
+ * @map: Register map to apply updates to.
+ * @regs: Values to update.
+ * @num_regs: Number of entries in regs.
+ *
+ * Register a set of register updates to be applied to the device
+ * whenever the device registers are synchronised with the cache and
+ * apply them immediately.  Typically this is used to apply
+ * corrections to be applied to the device defaults on startup, such
+ * as the updates some vendors provide to undocumented registers.
+ */
+int regmap_register_patch(struct regmap *map, const struct reg_default *regs,
+			  int num_regs)
+{
+	int i, ret;
+	bool bypass;
+
+	/* If needed the implementation can be extended to support this */
+	if (map->patch)
+		return -EBUSY;
+
+	map->lock(map);
+
+	bypass = map->cache_bypass;
+
+	map->cache_bypass = true;
+
+	/* Write out first; it's useful to apply even if we fail later. */
+	for (i = 0; i < num_regs; i++) {
+		ret = _regmap_write(map, regs[i].reg, regs[i].def);
+		if (ret != 0) {
+			dev_err(map->dev, "Failed to write %x = %x: %d\n",
+				regs[i].reg, regs[i].def, ret);
+			goto out;
+		}
+	}
+
+	map->patch = kcalloc(num_regs, sizeof(struct reg_default), GFP_KERNEL);
+	if (map->patch != NULL) {
+		memcpy(map->patch, regs,
+		       num_regs * sizeof(struct reg_default));
+		map->patch_regs = num_regs;
+	} else {
+		ret = -ENOMEM;
+	}
 
 out:
-	mutex_unlock(&map->lock);
+	map->cache_bypass = bypass;
+
+	map->unlock(map);
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(regmap_update_bits);
+EXPORT_SYMBOL_GPL(regmap_register_patch);
+
+/**
+ * regmap_get_val_bytes(): Report the size of a register value
+ *
+ * Report the size of a register value, mainly intended to for use by
+ * generic infrastructure built on top of regmap.
+ */
+int regmap_get_val_bytes(struct regmap *map)
+{
+	if (map->format.format_write)
+		return -EINVAL;
+
+	return map->format.val_bytes;
+}
+EXPORT_SYMBOL_GPL(regmap_get_val_bytes);
+
+static int __init regmap_initcall(void)
+{
+	regmap_debugfs_initcall();
+
+	return 0;
+}
+postcore_initcall(regmap_initcall);
diff --git a/drivers/base/syscore.c b/drivers/base/syscore.c
index e8d11b6630ee..0240f01714a1 100644
--- a/drivers/base/syscore.c
+++ b/drivers/base/syscore.c
@@ -97,12 +97,16 @@ void syscore_resume(void)
 
 	list_for_each_entry(ops, &syscore_ops_list, node)
 		if (ops->resume) {
-			if (initcall_debug)
-				pr_info("PM: Calling %pF\n", ops->resume);
 			ops->resume();
 			WARN_ONCE(!irqs_disabled(),
 				"Interrupts enabled after %pF\n", ops->resume);
 		}
+	if (initcall_debug) {
+		list_for_each_entry(ops, &syscore_ops_list, node)
+			if (ops->resume) {
+				pr_info("PM: Called %pF\n", ops->resume);
+			}
+	}
 }
 EXPORT_SYMBOL_GPL(syscore_resume);
 #endif /* CONFIG_PM_SLEEP */
diff --git a/drivers/block/cciss.c b/drivers/block/cciss.c
index 8f4ef656a1af..c2f9b3e3dec7 100644
--- a/drivers/block/cciss.c
+++ b/drivers/block/cciss.c
@@ -4533,6 +4533,13 @@ static int cciss_controller_hard_reset(struct pci_dev *pdev,
 		pmcsr &= ~PCI_PM_CTRL_STATE_MASK;
 		pmcsr |= PCI_D0;
 		pci_write_config_word(pdev, pos + PCI_PM_CTRL, pmcsr);
+
+		/*
+		 * The P600 requires a small delay when changing states.
+		 * Otherwise we may think the board did not reset and we bail.
+		 * This for kdump only and is particular to the P600.
+		 */
+		msleep(500);
 	}
 	return 0;
 }
diff --git a/drivers/block/xen-blkback/blkback.c b/drivers/block/xen-blkback/blkback.c
index 2330a9ad5e95..2f01073ff558 100644
--- a/drivers/block/xen-blkback/blkback.c
+++ b/drivers/block/xen-blkback/blkback.c
@@ -685,7 +685,7 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
 
 	if (operation == READ)
 		blkif->st_rd_sect += preq.nr_sects;
-	else if (operation == WRITE || operation == WRITE_FLUSH)
+	else if (operation & WRITE)
 		blkif->st_wr_sect += preq.nr_sects;
 
 	return 0;
diff --git a/drivers/bluetooth/Kconfig b/drivers/bluetooth/Kconfig
index 11b41fd40c27..4411cb035784 100644
--- a/drivers/bluetooth/Kconfig
+++ b/drivers/bluetooth/Kconfig
@@ -93,6 +93,26 @@ config BT_HCIBCM203X
 	  Say Y here to compile support for HCI BCM203x devices into the
 	  kernel or say M to compile it as module (bcm203x).
 
+config BT_BLUESLEEP
+        tristate "Bluesleep driver support"
+        help
+          Bluetooth Bluesleep Driver.
+          This driver provides the dynamic active power saving mechanism for
+          bluetooth radio devices.
+
+          Say Y here to compile support for bluesleep support into the kernel
+          or say M to compile it as module (bluesleep).
+
+config BT_TIBLUESLEEP
+        tristate "Bluesleep driver support for TI"
+        help
+          TI Bluetooth Bluesleep Driver.
+          This driver provides the dynamic active power saving mechanism for
+          bluetooth radio devices.
+
+          Say Y here to compile support for bluesleep support into the kernel
+          or say M to compile it as module (bluesleep).
+
 config BT_HCIBPA10X
 	tristate "HCI BPA10x USB driver"
 	depends on USB
diff --git a/drivers/bluetooth/Makefile b/drivers/bluetooth/Makefile
index f4460f4f4b78..1742f0acd2f1 100644
--- a/drivers/bluetooth/Makefile
+++ b/drivers/bluetooth/Makefile
@@ -4,6 +4,7 @@
 
 obj-$(CONFIG_BT_HCIVHCI)	+= hci_vhci.o
 obj-$(CONFIG_BT_HCIUART)	+= hci_uart.o
+obj-$(CONFIG_BT_BLUESLEEP)	+= bluesleep.o
 obj-$(CONFIG_BT_HCIBCM203X)	+= bcm203x.o
 obj-$(CONFIG_BT_HCIBPA10X)	+= bpa10x.o
 obj-$(CONFIG_BT_HCIBFUSB)	+= bfusb.o
@@ -11,6 +12,7 @@ obj-$(CONFIG_BT_HCIDTL1)	+= dtl1_cs.o
 obj-$(CONFIG_BT_HCIBT3C)	+= bt3c_cs.o
 obj-$(CONFIG_BT_HCIBLUECARD)	+= bluecard_cs.o
 obj-$(CONFIG_BT_HCIBTUART)	+= btuart_cs.o
+obj-$(CONFIG_BT_TIBLUESLEEP)	+= ti_bluesleep.o
 
 obj-$(CONFIG_BT_HCIBTUSB)	+= btusb.o
 obj-$(CONFIG_BT_HCIBTSDIO)	+= btsdio.o
diff --git a/drivers/bluetooth/bluesleep.c b/drivers/bluetooth/bluesleep.c
new file mode 100644
index 000000000000..910e510e0b7e
--- /dev/null
+++ b/drivers/bluetooth/bluesleep.c
@@ -0,0 +1,872 @@
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 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
+ *
+ * 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.
+ *
+ * Copyright (C) 2006-2007 - Motorola
+ * Copyright (c) 2008-2009, Code Aurora Forum. All rights reserved.
+ *
+ *  Date         Author           Comment
+ * -----------  --------------   --------------------------------
+ * 2006-Apr-28  Motorola         The kernel module for running the Bluetooth(R)
+ *                               Sleep-Mode Protocol from the Host side
+ * 2006-Sep-08  Motorola         Added workqueue for handling sleep work.
+ * 2007-Jan-24  Motorola         Added mbm_handle_ioi() call to ISR.
+ * 2009-Aug-10  Motorola         Changed "add_timer" to "mod_timer" to solve
+ *                               race when flurry of queued work comes in.
+*/
+
+#include <linux/module.h>       /* kernel module definitions */
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/notifier.h>
+#include <linux/proc_fs.h>
+#include <linux/spinlock.h>
+#include <linux/timer.h>
+#include <linux/uaccess.h>
+#include <linux/version.h>
+#include <linux/workqueue.h>
+#include <linux/platform_device.h>
+
+#include <linux/irq.h>
+#include <linux/ioport.h>
+#include <linux/param.h>
+#include <linux/bitops.h>
+#include <linux/termios.h>
+#include <linux/wakelock.h>
+#include <mach/gpio.h>
+#include <linux/serial_core.h>
+#include <linux/tegra_uart.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h> /* event notifications */
+#include "hci_uart.h"
+
+#define BT_SLEEP_DBG
+#ifndef BT_SLEEP_DBG
+#define BT_DBG(fmt, arg...)
+#endif
+/*
+ * Defines
+ */
+
+#define VERSION	 "1.1"
+#define PROC_DIR	"bluetooth/sleep"
+
+#define POLARITY_LOW 0
+#define POLARITY_HIGH 1
+
+/* enable/disable wake-on-bluetooth */
+#define BT_ENABLE_IRQ_WAKE 1
+
+struct bluesleep_info {
+	unsigned host_wake;
+	unsigned ext_wake;
+	unsigned host_wake_irq;
+	struct uart_port *uport;
+	struct wake_lock wake_lock;
+	int irq_polarity;
+	int has_ext_wake;
+};
+
+/* work function */
+static void bluesleep_sleep_work(struct work_struct *work);
+
+/* work queue */
+DECLARE_DELAYED_WORK(sleep_workqueue, bluesleep_sleep_work);
+
+/* Macros for handling sleep work */
+#define bluesleep_rx_busy()     schedule_delayed_work(&sleep_workqueue, 0)
+#define bluesleep_tx_busy()     schedule_delayed_work(&sleep_workqueue, 0)
+#define bluesleep_rx_idle()     schedule_delayed_work(&sleep_workqueue, 0)
+#define bluesleep_tx_idle()     schedule_delayed_work(&sleep_workqueue, 0)
+
+/* 10 second timeout */
+#define TX_TIMER_INTERVAL  10
+
+/* state variable names and bit positions */
+#define BT_PROTO	 0x01
+#define BT_TXDATA	 0x02
+#define BT_ASLEEP	 0x04
+#define BT_EXT_WAKE	0x08
+#define BT_SUSPEND	0x10
+
+/* global pointer to a single hci device. */
+static struct hci_dev *bluesleep_hdev;
+
+static struct bluesleep_info *bsi;
+
+/* module usage */
+static atomic_t open_count = ATOMIC_INIT(1);
+
+/*
+ * Local function prototypes
+ */
+static int bluesleep_hci_event(struct notifier_block *this,
+			unsigned long event, void *data);
+static int bluesleep_start(void);
+static void bluesleep_stop(void);
+
+/*
+ * Global variables
+ */
+/** Global state flags */
+static unsigned long flags;
+
+/** Tasklet to respond to change in hostwake line */
+static struct tasklet_struct hostwake_task;
+
+/** Transmission timer */
+static void bluesleep_tx_timer_expire(unsigned long data);
+static DEFINE_TIMER(tx_timer, bluesleep_tx_timer_expire, 0, 0);
+
+/** Lock for state transitions */
+static spinlock_t rw_lock;
+
+/** Notifier block for HCI events */
+struct notifier_block hci_event_nblock = {
+	.notifier_call = bluesleep_hci_event,
+};
+
+struct proc_dir_entry *bluetooth_dir, *sleep_dir;
+
+/*
+ * Local functions
+ */
+static void hsuart_power(int on)
+{
+	if (test_bit(BT_SUSPEND, &flags))
+		return;
+	if (on) {
+		tegra_uart_request_clock_on(bsi->uport);
+		tegra_uart_set_mctrl(bsi->uport, TIOCM_RTS);
+	} else {
+		tegra_uart_set_mctrl(bsi->uport, 0);
+		tegra_uart_request_clock_off(bsi->uport);
+	}
+}
+
+/**
+ * @return 1 if the Host can go to sleep, 0 otherwise.
+ */
+int bluesleep_can_sleep(void)
+{
+	/* check if WAKE_BT_GPIO and BT_WAKE_GPIO are both deasserted */
+	return ((gpio_get_value(bsi->host_wake) != bsi->irq_polarity) &&
+		(!test_bit(BT_EXT_WAKE, &flags)) &&
+		(bsi->uport != NULL));
+}
+
+void bluesleep_sleep_wakeup(void)
+{
+	if (test_bit(BT_ASLEEP, &flags)) {
+		BT_DBG("waking up...");
+		wake_lock(&bsi->wake_lock);
+		/* Start the timer */
+		mod_timer(&tx_timer, jiffies + (TX_TIMER_INTERVAL * HZ));
+		if (bsi->has_ext_wake == 1)
+			gpio_set_value(bsi->ext_wake, 1);
+		set_bit(BT_EXT_WAKE, &flags);
+		clear_bit(BT_ASLEEP, &flags);
+		/*Activating UART */
+	}
+}
+
+/**
+ * @brief@  main sleep work handling function which update the flags
+ * and activate and deactivate UART ,check FIFO.
+ */
+static void bluesleep_sleep_work(struct work_struct *work)
+{
+	if (bluesleep_can_sleep()) {
+		/* already asleep, this is an error case */
+		if (test_bit(BT_ASLEEP, &flags)) {
+			BT_DBG("already asleep");
+			return;
+		}
+
+		if (tegra_uart_is_tx_empty(bsi->uport)) {
+			BT_DBG("going to sleep...");
+			set_bit(BT_ASLEEP, &flags);
+			/*Deactivating UART */
+			/* UART clk is not turned off immediately. Release
+			 * wakelock after 500 ms.
+			 */
+			wake_lock_timeout(&bsi->wake_lock, HZ / 2);
+		} else {
+			mod_timer(&tx_timer, jiffies + TX_TIMER_INTERVAL * HZ);
+			return;
+		}
+	} else if (!test_bit(BT_EXT_WAKE, &flags)
+			&& !test_bit(BT_ASLEEP, &flags)) {
+		mod_timer(&tx_timer, jiffies + (TX_TIMER_INTERVAL * HZ));
+		if (bsi->has_ext_wake == 1)
+			gpio_set_value(bsi->ext_wake, 1);
+		set_bit(BT_EXT_WAKE, &flags);
+	} else {
+		bluesleep_sleep_wakeup();
+	}
+}
+
+/**
+ * A tasklet function that runs in tasklet context and reads the value
+ * of the HOST_WAKE GPIO pin and further defer the work.
+ * @param data Not used.
+ */
+static void bluesleep_hostwake_task(unsigned long data)
+{
+	BT_DBG("hostwake line change");
+
+	spin_lock(&rw_lock);
+	if ((gpio_get_value(bsi->host_wake) == bsi->irq_polarity))
+		bluesleep_rx_busy();
+	else
+		bluesleep_rx_idle();
+	spin_unlock(&rw_lock);
+
+}
+
+/**
+ * Handles proper timer action when outgoing data is delivered to the
+ * HCI line discipline. Sets BT_TXDATA.
+ */
+static void bluesleep_outgoing_data(void)
+{
+	unsigned long irq_flags;
+
+	spin_lock_irqsave(&rw_lock, irq_flags);
+	/* log data passing by */
+	set_bit(BT_TXDATA, &flags);
+	/* if the tx side is sleeping... */
+	if (!test_bit(BT_EXT_WAKE, &flags)) {
+		BT_DBG("tx was sleeping");
+		bluesleep_sleep_wakeup();
+	}
+	spin_unlock_irqrestore(&rw_lock, irq_flags);
+}
+
+/**
+ * Handles HCI device events.
+ * @param this Not used.
+ * @param event The event that occurred.
+ * @param data The HCI device associated with the event.
+ * @return <code>NOTIFY_DONE</code>.
+ */
+static int bluesleep_hci_event(struct notifier_block *this,
+				unsigned long event, void *data)
+{
+	struct hci_dev *hdev = (struct hci_dev *) data;
+	struct hci_uart *hu;
+	struct uart_state *state;
+
+	if (!hdev)
+		return NOTIFY_DONE;
+
+	switch (event) {
+	case HCI_DEV_REG:
+		if (!bluesleep_hdev) {
+			bluesleep_hdev = hdev;
+			if (bsi->has_ext_wake == 1) {
+				hu  = (struct hci_uart *) hdev->driver_data;
+				state = (struct uart_state *) \
+							 hu->tty->driver_data;
+				bsi->uport = state->uart_port;
+			}
+			/* if bluetooth started, start bluesleep*/
+			bluesleep_start();
+		}
+		break;
+	case HCI_DEV_UP:
+#if BT_ENABLE_IRQ_WAKE
+		if (enable_irq_wake(bsi->host_wake_irq))
+			BT_ERR("Couldn't enable BT_HOST_WAKE as wakeup interrupt");
+#endif
+		break;
+	case HCI_DEV_DOWN:
+#if BT_ENABLE_IRQ_WAKE
+		if (disable_irq_wake(bsi->host_wake_irq))
+			BT_ERR("Couldn't disable hostwake IRQ wakeup mode\n");
+#endif
+		break;
+	case HCI_DEV_UNREG:
+		bluesleep_stop();
+		bluesleep_hdev = NULL;
+		bsi->uport = NULL;
+		/* if bluetooth stopped, stop bluesleep also */
+		break;
+	case HCI_DEV_WRITE:
+		if (bsi->has_ext_wake == 1)
+			bluesleep_outgoing_data();
+		break;
+	}
+
+	return NOTIFY_DONE;
+}
+
+/**
+ * Handles transmission timer expiration.
+ * @param data Not used.
+ */
+static void bluesleep_tx_timer_expire(unsigned long data)
+{
+	unsigned long irq_flags;
+
+	BT_DBG("Tx timer expired");
+
+	spin_lock_irqsave(&rw_lock, irq_flags);
+
+	/* were we silent during the last timeout? */
+	if (!test_bit(BT_TXDATA, &flags)) {
+		BT_DBG("Tx has been idle");
+		if (bsi->has_ext_wake == 1)
+			gpio_set_value(bsi->ext_wake, 0);
+		clear_bit(BT_EXT_WAKE, &flags);
+		bluesleep_tx_idle();
+	} else {
+		BT_DBG("Tx data during last period");
+		mod_timer(&tx_timer, jiffies + (TX_TIMER_INTERVAL*HZ));
+	}
+
+	/* clear the incoming data flag */
+	clear_bit(BT_TXDATA, &flags);
+
+	spin_unlock_irqrestore(&rw_lock, irq_flags);
+}
+
+/**
+ * Schedules a tasklet to run when receiving an interrupt on the
+ * <code>HOST_WAKE</code> GPIO pin.
+ * @param irq Not used.
+ * @param dev_id Not used.
+ */
+static irqreturn_t bluesleep_hostwake_isr(int irq, void *dev_id)
+{
+	/* schedule a tasklet to handle the change in the host wake line */
+	if (bsi->has_ext_wake == 1)
+		tasklet_schedule(&hostwake_task);
+	return IRQ_HANDLED;
+}
+
+/**
+ * Starts the Sleep-Mode Protocol on the Host.
+ * @return On success, 0. On error, -1, and <code>errno</code> is set
+ * appropriately.
+ */
+static int bluesleep_start(void)
+{
+	unsigned long irq_flags;
+
+	spin_lock_irqsave(&rw_lock, irq_flags);
+	if (test_bit(BT_PROTO, &flags)) {
+		spin_unlock_irqrestore(&rw_lock, irq_flags);
+		return 0;
+	}
+	spin_unlock_irqrestore(&rw_lock, irq_flags);
+
+	if (!atomic_dec_and_test(&open_count)) {
+		atomic_inc(&open_count);
+		return -EBUSY;
+	}
+
+	/* assert BT_WAKE */
+	if (bsi->has_ext_wake == 1) {
+		/* start the timer */
+		mod_timer(&tx_timer, jiffies + (TX_TIMER_INTERVAL * HZ));
+		gpio_set_value(bsi->ext_wake, 1);
+		wake_lock(&bsi->wake_lock);
+		set_bit(BT_EXT_WAKE, &flags);
+	}
+
+	set_bit(BT_PROTO, &flags);
+	return 0;
+fail:
+	if (bsi->has_ext_wake == 1)
+		del_timer(&tx_timer);
+	atomic_inc(&open_count);
+
+	return 0;
+}
+
+/**
+ * Stops the Sleep-Mode Protocol on the Host.
+ */
+static void bluesleep_stop(void)
+{
+	unsigned long irq_flags;
+
+	spin_lock_irqsave(&rw_lock, irq_flags);
+	if (!test_bit(BT_PROTO, &flags)) {
+		spin_unlock_irqrestore(&rw_lock, irq_flags);
+		return;
+	}
+	/* assert BT_WAKE */
+	if (bsi->has_ext_wake == 1) {
+		gpio_set_value(bsi->ext_wake, 1);
+		set_bit(BT_EXT_WAKE, &flags);
+		del_timer(&tx_timer);
+		wake_lock_timeout(&bsi->wake_lock, HZ / 2);
+		if (test_bit(BT_ASLEEP, &flags)) {
+			clear_bit(BT_ASLEEP, &flags);
+			hsuart_power(1);
+		}
+	}
+	clear_bit(BT_PROTO, &flags);
+
+	atomic_inc(&open_count);
+	spin_unlock_irqrestore(&rw_lock, irq_flags);
+}
+/**
+ * Read the <code>BT_WAKE</code> GPIO pin value via the proc interface.
+ * When this function returns, <code>page</code> will contain a 1 if the
+ * pin is high, 0 otherwise.
+ * @param page Buffer for writing data.
+ * @param start Not used.
+ * @param offset Not used.
+ * @param count Not used.
+ * @param eof Whether or not there is more data to be read.
+ * @param data Not used.
+ * @return The number of bytes written.
+ */
+static int bluepower_read_proc_btwake(char *page, char **start, off_t offset,
+					int count, int *eof, void *data)
+{
+	*eof = 1;
+	return sprintf(page, "btwake:%u\n",test_bit(BT_EXT_WAKE, &flags));
+}
+
+/**
+ * Write the <code>BT_WAKE</code> GPIO pin value via the proc interface.
+ * @param file Not used.
+ * @param buffer The buffer to read from.
+ * @param count The number of bytes to be written.
+ * @param data Not used.
+ * @return On success, the number of bytes written. On error, -1, and
+ * <code>errno</code> is set appropriately.
+ */
+static int bluepower_write_proc_btwake(struct file *file, const char *buffer,
+					unsigned long count, void *data)
+{
+	char *buf;
+
+	if (count < 1)
+		return -EINVAL;
+
+	buf = kmalloc(count, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	if (copy_from_user(buf, buffer, count)) {
+		kfree(buf);
+		return -EFAULT;
+	}
+	if (buf[0] == '0') {
+		if (bsi->has_ext_wake == 1)
+			gpio_set_value(bsi->ext_wake, 0);
+		clear_bit(BT_EXT_WAKE, &flags);
+	} else if (buf[0] == '1') {
+		if (bsi->has_ext_wake == 1)
+			gpio_set_value(bsi->ext_wake, 1);
+		set_bit(BT_EXT_WAKE, &flags);
+	} else {
+		kfree(buf);
+		return -EINVAL;
+	}
+
+	kfree(buf);
+	return count;
+}
+
+/**
+ * Read the <code>BT_HOST_WAKE</code> GPIO pin value via the proc interface.
+ * When this function returns, <code>page</code> will contain a 1 if the pin
+ * is high, 0 otherwise.
+ * @param page Buffer for writing data.
+ * @param start Not used.
+ * @param offset Not used.
+ * @param count Not used.
+ * @param eof Whether or not there is more data to be read.
+ * @param data Not used.
+ * @return The number of bytes written.
+ */
+static int bluepower_read_proc_hostwake(char *page, char **start, off_t offset,
+					int count, int *eof, void *data)
+{
+	*eof = 1;
+	return sprintf(page, "hostwake: %u\n", gpio_get_value(bsi->host_wake));
+}
+
+
+/**
+ * Read the low-power status of the Host via the proc interface.
+ * When this function returns, <code>page</code> contains a 1 if the Host
+ * is asleep, 0 otherwise.
+ * @param page Buffer for writing data.
+ * @param start Not used.
+ * @param offset Not used.
+ * @param count Not used.
+ * @param eof Whether or not there is more data to be read.
+ * @param data Not used.
+ * @return The number of bytes written.
+ */
+static int bluesleep_read_proc_asleep(char *page, char **start, off_t offset,
+					int count, int *eof, void *data)
+{
+	unsigned int asleep;
+
+	asleep = test_bit(BT_ASLEEP, &flags) ? 1 : 0;
+	*eof = 1;
+	return sprintf(page, "asleep: %u\n", asleep);
+}
+
+/**
+ * Read the low-power protocol being used by the Host via the proc interface.
+ * When this function returns, <code>page</code> will contain a 1 if the Host
+ * is using the Sleep Mode Protocol, 0 otherwise.
+ * @param page Buffer for writing data.
+ * @param start Not used.
+ * @param offset Not used.
+ * @param count Not used.
+ * @param eof Whether or not there is more data to be read.
+ * @param data Not used.
+ * @return The number of bytes written.
+ */
+static int bluesleep_read_proc_proto(char *page, char **start, off_t offset,
+					int count, int *eof, void *data)
+{
+	unsigned int proto;
+
+	proto = test_bit(BT_PROTO, &flags) ? 1 : 0;
+	*eof = 1;
+	return sprintf(page, "proto: %u\n", proto);
+}
+
+/**
+ * Modify the low-power protocol used by the Host via the proc interface.
+ * @param file Not used.
+ * @param buffer The buffer to read from.
+ * @param count The number of bytes to be written.
+ * @param data Not used.
+ * @return On success, the number of bytes written. On error, -1, and
+ * <code>errno</code> is set appropriately.
+ */
+static int bluesleep_write_proc_proto(struct file *file, const char *buffer,
+					unsigned long count, void *data)
+{
+	char proto;
+
+	if (count < 1)
+		return -EINVAL;
+
+	if (copy_from_user(&proto, buffer, 1))
+		return -EFAULT;
+
+	if (proto == '0')
+		bluesleep_stop();
+	else
+		bluesleep_start();
+
+	/* claim that we wrote everything */
+	return count;
+}
+
+
+static int bluesleep_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct resource *res;
+
+	bsi = kzalloc(sizeof(struct bluesleep_info), GFP_KERNEL);
+	if (!bsi)
+		return -ENOMEM;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_IO,
+				"gpio_host_wake");
+	if (!res) {
+		BT_ERR("couldn't find host_wake gpio\n");
+		ret = -ENODEV;
+		goto free_bsi;
+	}
+	bsi->host_wake = res->start;
+
+	ret = gpio_request(bsi->host_wake, "bt_host_wake");
+	if (ret)
+		goto free_bsi;
+
+	/* configure host_wake as input */
+	ret = gpio_direction_input(bsi->host_wake);
+	if (ret < 0) {
+		pr_err("gpio-keys: failed to configure input"
+				" direction for GPIO %d, error %d\n",
+				bsi->host_wake, ret);
+		gpio_free(bsi->host_wake);
+		goto free_bsi;
+	}
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_IO,
+				"gpio_ext_wake");
+
+	if (!res)
+		bsi->has_ext_wake = 0;
+	else
+		bsi->has_ext_wake = 1;
+
+	if (bsi->has_ext_wake) {
+		bsi->ext_wake = res->start;
+		ret = gpio_request(bsi->ext_wake, "bt_ext_wake");
+		if (ret)
+			goto free_bt_host_wake;
+
+		/* configure ext_wake as output mode*/
+		ret = gpio_direction_output(bsi->ext_wake, 1);
+		if (ret < 0) {
+			pr_err("gpio-keys: failed to configure output"
+				" direction for GPIO %d, error %d\n",
+				  bsi->ext_wake, ret);
+			gpio_free(bsi->ext_wake);
+			goto free_bt_host_wake;
+		}
+	} else
+		set_bit(BT_EXT_WAKE, &flags);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
+						"host_wake");
+	if (!res) {
+		BT_ERR("couldn't find host_wake irq\n");
+		ret = -ENODEV;
+		goto free_bt_host_wake;
+	}
+	bsi->host_wake_irq = res->start;
+	if (bsi->host_wake_irq < 0) {
+		BT_ERR("couldn't find host_wake irq\n");
+		ret = -ENODEV;
+		goto free_bt_ext_wake;
+	}
+	if (res->flags & IORESOURCE_IRQ_LOWEDGE)
+		bsi->irq_polarity = POLARITY_LOW;/*low edge (falling edge)*/
+	else
+		bsi->irq_polarity = POLARITY_HIGH;/*anything else*/
+
+	wake_lock_init(&bsi->wake_lock, WAKE_LOCK_SUSPEND, "bluesleep");
+	clear_bit(BT_SUSPEND, &flags);
+
+	if (bsi->irq_polarity == POLARITY_LOW) {
+		ret = request_irq(bsi->host_wake_irq, bluesleep_hostwake_isr,
+				IRQF_DISABLED | IRQF_TRIGGER_FALLING,
+				"bluetooth hostwake", NULL);
+	} else {
+		ret = request_irq(bsi->host_wake_irq, bluesleep_hostwake_isr,
+				IRQF_DISABLED | IRQF_TRIGGER_RISING,
+				"bluetooth hostwake", NULL);
+	}
+	if (ret  < 0) {
+		BT_ERR("Couldn't acquire BT_HOST_WAKE IRQ");
+		goto free_bt_ext_wake;
+	}
+
+	return 0;
+
+free_bt_ext_wake:
+	gpio_free(bsi->ext_wake);
+free_bt_host_wake:
+	gpio_free(bsi->host_wake);
+free_bsi:
+	kfree(bsi);
+	return ret;
+}
+
+static int bluesleep_remove(struct platform_device *pdev)
+{
+	free_irq(bsi->host_wake_irq, NULL);
+	gpio_free(bsi->host_wake);
+	gpio_free(bsi->ext_wake);
+	wake_lock_destroy(&bsi->wake_lock);
+	kfree(bsi);
+	return 0;
+}
+
+
+static int bluesleep_resume(struct platform_device *pdev)
+{
+	if (test_bit(BT_SUSPEND, &flags)) {
+		BT_DBG("bluesleep resuming...\n");
+	if ((bsi->uport != NULL) &&
+		(gpio_get_value(bsi->host_wake) == bsi->irq_polarity)) {
+			BT_DBG("bluesleep resume form BT event...\n");
+			tegra_uart_request_clock_on(bsi->uport);
+			tegra_uart_set_mctrl(bsi->uport, TIOCM_RTS);
+		}
+		clear_bit(BT_SUSPEND, &flags);
+	}
+	return 0;
+}
+
+static int bluesleep_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	BT_DBG("bluesleep suspending...\n");
+	set_bit(BT_SUSPEND, &flags);
+	return 0;
+}
+
+static struct platform_driver bluesleep_driver = {
+	.probe = bluesleep_probe,
+	.remove = bluesleep_remove,
+	.suspend = bluesleep_suspend,
+	.resume = bluesleep_resume,
+	.driver = {
+		.name = "bluesleep",
+		.owner = THIS_MODULE,
+	},
+};
+/**
+ * Initializes the module.
+ * @return On success, 0. On error, -1, and <code>errno</code> is set
+ * appropriately.
+ */
+static int __init bluesleep_init(void)
+{
+	int retval;
+	struct proc_dir_entry *ent;
+
+	BT_INFO("BlueSleep Mode Driver Ver %s", VERSION);
+
+	retval = platform_driver_register(&bluesleep_driver);
+	if (retval)
+		return retval;
+
+	if (bsi == NULL)
+		return 0;
+
+	bluesleep_hdev = NULL;
+
+	bluetooth_dir = proc_mkdir("bluetooth", NULL);
+	if (bluetooth_dir == NULL) {
+		BT_ERR("Unable to create /proc/bluetooth directory");
+		return -ENOMEM;
+	}
+
+	sleep_dir = proc_mkdir("sleep", bluetooth_dir);
+	if (sleep_dir == NULL) {
+		BT_ERR("Unable to create /proc/%s directory", PROC_DIR);
+		return -ENOMEM;
+	}
+
+	/* Creating read/write "btwake" entry */
+	ent = create_proc_entry("btwake", 0, sleep_dir);
+	if (ent == NULL) {
+		BT_ERR("Unable to create /proc/%s/btwake entry", PROC_DIR);
+		retval = -ENOMEM;
+		goto fail;
+	}
+	ent->read_proc = bluepower_read_proc_btwake;
+	ent->write_proc = bluepower_write_proc_btwake;
+
+	/* read only proc entries */
+	if (create_proc_read_entry("hostwake", 0, sleep_dir,
+				bluepower_read_proc_hostwake, NULL) == NULL) {
+		BT_ERR("Unable to create /proc/%s/hostwake entry", PROC_DIR);
+		retval = -ENOMEM;
+		goto fail;
+	}
+
+	/* read/write proc entries */
+	ent = create_proc_entry("proto", 0, sleep_dir);
+	if (ent == NULL) {
+		BT_ERR("Unable to create /proc/%s/proto entry", PROC_DIR);
+		retval = -ENOMEM;
+		goto fail;
+	}
+	ent->read_proc = bluesleep_read_proc_proto;
+	ent->write_proc = bluesleep_write_proc_proto;
+
+	/* read only proc entries */
+	if (create_proc_read_entry("asleep", 0,
+			sleep_dir, bluesleep_read_proc_asleep, NULL) == NULL) {
+		BT_ERR("Unable to create /proc/%s/asleep entry", PROC_DIR);
+		retval = -ENOMEM;
+		goto fail;
+	}
+
+	flags = 0; /* clear all status bits */
+
+	/* Initialize spinlock. */
+	spin_lock_init(&rw_lock);
+
+	/* assert bt wake */
+	if (bsi->has_ext_wake == 1) {
+		/* Initialize timer */
+		init_timer(&tx_timer);
+		tx_timer.function = bluesleep_tx_timer_expire;
+		tx_timer.data = 0;
+
+		/* initialize host wake tasklet */
+		tasklet_init(&hostwake_task, bluesleep_hostwake_task, 0);
+
+		gpio_set_value(bsi->ext_wake, 1);
+		set_bit(BT_EXT_WAKE, &flags);
+	}
+	hci_register_notifier(&hci_event_nblock);
+
+	return 0;
+
+fail:
+	remove_proc_entry("asleep", sleep_dir);
+	remove_proc_entry("proto", sleep_dir);
+	remove_proc_entry("hostwake", sleep_dir);
+	remove_proc_entry("btwake", sleep_dir);
+	remove_proc_entry("sleep", bluetooth_dir);
+	remove_proc_entry("bluetooth", 0);
+	return retval;
+}
+
+/**
+ * Cleans up the module.
+ */
+static void __exit bluesleep_exit(void)
+{
+	if (bsi == NULL)
+		return;
+
+	/* assert bt wake */
+	if (bsi->has_ext_wake == 1) {
+		gpio_set_value(bsi->ext_wake, 1);
+		del_timer(&tx_timer);
+		if (test_bit(BT_ASLEEP, &flags))
+			hsuart_power(1);
+		set_bit(BT_EXT_WAKE, &flags);
+	}
+	if (test_bit(BT_PROTO, &flags)) {
+		if (disable_irq_wake(bsi->host_wake_irq))
+			BT_ERR("Couldn't disable hostwake IRQ wakeup mode\n");
+		free_irq(bsi->host_wake_irq, NULL);
+	}
+
+	hci_unregister_notifier(&hci_event_nblock);
+	platform_driver_unregister(&bluesleep_driver);
+
+	remove_proc_entry("asleep", sleep_dir);
+	remove_proc_entry("proto", sleep_dir);
+	remove_proc_entry("hostwake", sleep_dir);
+	remove_proc_entry("btwake", sleep_dir);
+	remove_proc_entry("sleep", bluetooth_dir);
+	remove_proc_entry("bluetooth", 0);
+}
+
+module_init(bluesleep_init);
+module_exit(bluesleep_exit);
+
+MODULE_DESCRIPTION("Bluetooth Sleep Mode Driver ver %s " VERSION);
+#ifdef MODULE_LICENSE
+MODULE_LICENSE("GPL");
+#endif
+
diff --git a/drivers/bluetooth/btwilink.c b/drivers/bluetooth/btwilink.c
index 04d353f58d71..c0470bc95e12 100644
--- a/drivers/bluetooth/btwilink.c
+++ b/drivers/bluetooth/btwilink.c
@@ -22,7 +22,6 @@
  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  *
  */
-#define DEBUG
 #include <linux/platform_device.h>
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
diff --git a/drivers/bluetooth/ti_bluesleep.c b/drivers/bluetooth/ti_bluesleep.c
new file mode 100644
index 000000000000..2ab532410c72
--- /dev/null
+++ b/drivers/bluetooth/ti_bluesleep.c
@@ -0,0 +1,261 @@
+/*
+ *  TI Bluesleep driver
+ *	Kernel module responsible for Wake up of Host
+ *  Copyright (C) 2009-2010 Texas Instruments
+
+
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * 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
+ *
+ * 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.
+ *
+ * Copyright (C) 2006-2007 - Motorola
+ * Copyright (c) 2008-2009, Code Aurora Forum. All rights reserved.
+ *
+ *  Date         Author           Comment
+ * -----------  --------------   --------------------------------
+ * 2006-Apr-28  Motorola         The kernel module for running the Bluetooth(R)
+ *                               Sleep-Mode Protocol from the Host side
+ * 2006-Sep-08  Motorola         Added workqueue for handling sleep work.
+ * 2007-Jan-24  Motorola         Added mbm_handle_ioi() call to ISR.
+ * 2009-Aug-10  Motorola         Changed "add_timer" to "mod_timer" to solve
+ *                               race when flurry of queued work comes in.
+*/
+
+#include <linux/module.h>       /* kernel module definitions */
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+
+#include <mach/gpio.h>
+
+#include <net/bluetooth/bluetooth.h>
+
+/*
+ * Defines
+ */
+#define VERSION	 "1.1"
+
+#define POLARITY_LOW 0
+#define POLARITY_HIGH 1
+
+struct bluesleep_info {
+	unsigned host_wake_irq;
+	struct uart_port *uport;
+	int irq_polarity;
+};
+
+
+/* state variable names and bit positions */
+#define FLAG_RESET      	0x00
+#define BT_ACTIVE		0x02
+#define BT_SUSPEND		0x04
+
+static struct bluesleep_info *bsi;
+
+/* module usage */
+static atomic_t open_count = ATOMIC_INIT(1);
+
+/*
+ * Global variables
+ */
+/** Global state flags */
+static unsigned long flags;
+
+/**
+ * Schedules a tasklet to run when receiving an interrupt on the
+ * <code>HOST_WAKE</code> GPIO pin.
+ * @param irq Not used.
+ * @param dev_id Not used.
+ */
+static irqreturn_t bluesleep_hostwake_isr(int irq, void *dev_id)
+{
+	pr_debug("%s", __func__);
+	disable_irq_nosync(bsi->host_wake_irq);
+	return IRQ_HANDLED;
+}
+
+/**
+ * Starts the Sleep-Mode Protocol on the Host.
+ * @return On success, 0. On error, -1, and <code>errno</code> is set
+ * appropriately.
+ */
+int bluesleep_start(struct uart_port *uport)
+{
+	int retval;
+	bsi->uport = uport;
+	pr_debug("%s", __func__);
+
+	if (test_bit(BT_SUSPEND, &flags)) {
+		BT_DBG("bluesleep_acquire irq");
+		if (bsi->irq_polarity == POLARITY_LOW) {
+			retval = request_irq(bsi->host_wake_irq, bluesleep_hostwake_isr,
+					IRQF_DISABLED | IRQF_TRIGGER_FALLING,
+					"bluetooth hostwake", "tibluesleep");
+		} else 	{
+			retval = request_irq(bsi->host_wake_irq, bluesleep_hostwake_isr,
+					IRQF_DISABLED | IRQF_TRIGGER_RISING,
+					"bluetooth hostwake", "tibluesleep");
+		}
+		if (retval  < 0) {
+			BT_ERR("Couldn't acquire BT_HOST_WAKE IRQ");
+			goto fail;
+		}
+
+		retval = enable_irq_wake(bsi->host_wake_irq);
+		if (retval < 0) {
+			BT_ERR("Couldn't enable BT_HOST_WAKE as wakeup"
+					"interrupt retval %d\n", retval);
+			free_irq(bsi->host_wake_irq, NULL);
+			goto fail;
+		}
+	}
+
+	return 0;
+fail:
+	atomic_inc(&open_count);
+	return retval;
+}
+
+/**
+ * Stops the Sleep-Mode Protocol on the Host.
+ */
+void bluesleep_stop(void)
+{
+	pr_debug("%s", __func__);
+
+	if (disable_irq_wake(bsi->host_wake_irq))
+		BT_ERR("Couldn't disable hostwake IRQ wakeup mode\n");
+
+	free_irq(bsi->host_wake_irq, NULL);
+}
+
+static int bluesleep_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct resource *res;
+
+	bsi = kzalloc(sizeof(struct bluesleep_info), GFP_KERNEL);
+	if (!bsi)
+		return -ENOMEM;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
+						"host_wake");
+	if (!res) {
+		BT_ERR("couldn't find host_wake irq\n");
+		ret = -ENODEV;
+		goto free_bsi;
+	}
+
+	bsi->host_wake_irq = res->start;
+
+	if (bsi->host_wake_irq < 0) {
+		BT_ERR("couldn't find host_wake irq");
+		ret = -ENODEV;
+		goto free_bsi;
+	}
+	if (res->flags & IORESOURCE_IRQ_LOWEDGE)
+		bsi->irq_polarity = POLARITY_LOW;/*low edge (falling edge)*/
+	else
+		bsi->irq_polarity = POLARITY_HIGH;/*anything else*/
+
+	clear_bit(BT_SUSPEND, &flags);
+	set_bit(BT_ACTIVE, &flags);
+
+	return 0;
+
+free_bsi:
+	kfree(bsi);
+	return ret;
+}
+
+static int bluesleep_remove(struct platform_device *pdev)
+{
+	pr_debug("%s", __func__);
+	kfree(bsi);
+	return 0;
+}
+
+static int bluesleep_resume(struct platform_device *pdev)
+{
+	pr_debug("%s", __func__);
+	if (test_bit(BT_SUSPEND, &flags)) {
+		free_irq(bsi->host_wake_irq, "tibluesleep");
+		clear_bit(BT_SUSPEND, &flags);
+		set_bit(BT_ACTIVE, &flags);
+	}
+
+	return 0;
+}
+
+static int bluesleep_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	pr_debug("%s", __func__);
+	set_bit(BT_SUSPEND, &flags);
+	return 0;
+}
+
+static struct platform_driver bluesleep_driver = {
+	.probe = bluesleep_probe,
+	.remove = bluesleep_remove,
+	.suspend = bluesleep_suspend,
+	.resume = bluesleep_resume,
+	.driver = {
+		.name = "tibluesleep",
+		.owner = THIS_MODULE,
+	},
+};
+
+/**
+ * Initializes the module.
+ * @return On success, 0. On error, -1, and <code>errno</code> is set
+ * appropriately.
+ */
+static int __init bluesleep_init(void)
+{
+	int retval;
+
+	BT_INFO("BlueSleep Mode Driver Ver %s", VERSION);
+
+	retval = platform_driver_register(&bluesleep_driver);
+	if (retval)
+		goto fail;
+
+	if (bsi == NULL)
+		return 0;
+
+	flags = FLAG_RESET; /* clear all status bits */
+
+	return 0;
+fail:
+	return retval;
+}
+
+/**
+ * Cleans up the module.
+ */
+static void __exit bluesleep_exit(void)
+{
+	if (bsi == NULL)
+		return;
+	/* assert bt wake */
+	free_irq(bsi->host_wake_irq, NULL);
+	platform_driver_unregister(&bluesleep_driver);
+
+}
+
+module_init(bluesleep_init);
+module_exit(bluesleep_exit);
+
+MODULE_DESCRIPTION("TI Bluetooth Sleep Mode Driver ver %s " VERSION);
+#ifdef MODULE_LICENSE
+MODULE_LICENSE("GPL");
+#endif
diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index 423fd56bf612..b2aec0469961 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -6,6 +6,19 @@ menu "Character devices"
 
 source "drivers/tty/Kconfig"
 
+config DEVMEM
+	bool "Memory device driver"
+	default y
+	help
+	  The memory driver provides two character devices, mem and kmem, which
+	  provide access to the system's memory. The mem device is a view of
+	  physical memory, and each byte in the device corresponds to the
+	  matching physical address. The kmem device is the same as mem, but
+	  the addresses correspond to the kernel's virtual address space rather
+	  than physical memory. These devices are standard parts of a Linux
+	  system and most users should say Y here. You might say N if very
+	  security conscience or memory is tight.
+
 config DEVKMEM
 	bool "/dev/kmem virtual device support"
 	default y
@@ -598,6 +611,10 @@ config DEVPORT
 	depends on ISA || PCI
 	default y
 
+config DCC_TTY
+	tristate "DCC tty driver"
+	depends on ARM
+
 source "drivers/s390/char/Kconfig"
 
 config RAMOOPS
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index 32762ba769c2..5e2fd7097027 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -58,6 +58,7 @@ obj-$(CONFIG_IPMI_HANDLER)	+= ipmi/
 obj-$(CONFIG_HANGCHECK_TIMER)	+= hangcheck-timer.o
 obj-$(CONFIG_TCG_TPM)		+= tpm/
 
+obj-$(CONFIG_DCC_TTY)		+= dcc_tty.o
 obj-$(CONFIG_PS3_FLASH)		+= ps3flash.o
 obj-$(CONFIG_RAMOOPS)		+= ramoops.o
 
diff --git a/drivers/char/agp/Makefile b/drivers/char/agp/Makefile
index 8eb56e273e75..2d3b713695c5 100644
--- a/drivers/char/agp/Makefile
+++ b/drivers/char/agp/Makefile
@@ -14,6 +14,7 @@ obj-$(CONFIG_AGP_PARISC)	+= parisc-agp.o
 obj-$(CONFIG_AGP_I460)		+= i460-agp.o
 obj-$(CONFIG_AGP_INTEL)		+= intel-agp.o
 obj-$(CONFIG_AGP_INTEL)		+= intel-gtt.o
+CFLAGS_nvidia-agp.o		 = -Werror
 obj-$(CONFIG_AGP_NVIDIA)	+= nvidia-agp.o
 obj-$(CONFIG_AGP_SGI_TIOCA)	+= sgi-agp.o
 obj-$(CONFIG_AGP_SIS)		+= sis-agp.o
diff --git a/drivers/char/agp/intel-gtt.c b/drivers/char/agp/intel-gtt.c
index 85151019dde1..2774ac1086d3 100644
--- a/drivers/char/agp/intel-gtt.c
+++ b/drivers/char/agp/intel-gtt.c
@@ -30,10 +30,10 @@
 /*
  * If we have Intel graphics, we're not going to have anything other than
  * an Intel IOMMU. So make the correct use of the PCI DMA API contingent
- * on the Intel IOMMU support (CONFIG_DMAR).
+ * on the Intel IOMMU support (CONFIG_INTEL_IOMMU).
  * Only newer chipsets need to bother with this, of course.
  */
-#ifdef CONFIG_DMAR
+#ifdef CONFIG_INTEL_IOMMU
 #define USE_PCI_DMA_API 1
 #else
 #define USE_PCI_DMA_API 0
diff --git a/drivers/char/dcc_tty.c b/drivers/char/dcc_tty.c
new file mode 100644
index 000000000000..a787accdcb14
--- /dev/null
+++ b/drivers/char/dcc_tty.c
@@ -0,0 +1,326 @@
+/* drivers/char/dcc_tty.c
+ *
+ * Copyright (C) 2007 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/console.h>
+#include <linux/hrtimer.h>
+#include <linux/tty.h>
+#include <linux/tty_driver.h>
+#include <linux/tty_flip.h>
+
+MODULE_DESCRIPTION("DCC TTY Driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("1.0");
+
+static spinlock_t g_dcc_tty_lock = SPIN_LOCK_UNLOCKED;
+static struct hrtimer g_dcc_timer;
+static char g_dcc_buffer[16];
+static int g_dcc_buffer_head;
+static int g_dcc_buffer_count;
+static unsigned g_dcc_write_delay_usecs = 1;
+static struct tty_driver *g_dcc_tty_driver;
+static struct tty_struct *g_dcc_tty;
+static int g_dcc_tty_open_count;
+
+static void dcc_poll_locked(void)
+{
+	char ch;
+	int rch;
+	int written;
+
+	while (g_dcc_buffer_count) {
+		ch = g_dcc_buffer[g_dcc_buffer_head];
+		asm(
+			"mrc 14, 0, r15, c0, c1, 0\n"
+			"mcrcc 14, 0, %1, c0, c5, 0\n"
+			"movcc %0, #1\n"
+			"movcs %0, #0\n"
+			: "=r" (written)
+			: "r" (ch)
+		);
+		if (written) {
+			if (ch == '\n')
+				g_dcc_buffer[g_dcc_buffer_head] = '\r';
+			else {
+				g_dcc_buffer_head = (g_dcc_buffer_head + 1) % ARRAY_SIZE(g_dcc_buffer);
+				g_dcc_buffer_count--;
+				if (g_dcc_tty)
+					tty_wakeup(g_dcc_tty);
+			}
+			g_dcc_write_delay_usecs = 1;
+		} else {
+			if (g_dcc_write_delay_usecs > 0x100)
+				break;
+			g_dcc_write_delay_usecs <<= 1;
+			udelay(g_dcc_write_delay_usecs);
+		}
+	}
+
+	if (g_dcc_tty && !test_bit(TTY_THROTTLED, &g_dcc_tty->flags)) {
+		asm(
+			"mrc 14, 0, %0, c0, c1, 0\n"
+			"tst %0, #(1 << 30)\n"
+			"moveq %0, #-1\n"
+			"mrcne 14, 0, %0, c0, c5, 0\n"
+			: "=r" (rch)
+		);
+		if (rch >= 0) {
+			ch = rch;
+			tty_insert_flip_string(g_dcc_tty, &ch, 1);
+			tty_flip_buffer_push(g_dcc_tty);
+		}
+	}
+
+
+	if (g_dcc_buffer_count)
+		hrtimer_start(&g_dcc_timer, ktime_set(0, g_dcc_write_delay_usecs * NSEC_PER_USEC), HRTIMER_MODE_REL);
+	else
+		hrtimer_start(&g_dcc_timer, ktime_set(0, 20 * NSEC_PER_MSEC), HRTIMER_MODE_REL);
+}
+
+static int dcc_tty_open(struct tty_struct * tty, struct file * filp)
+{
+	int ret;
+	unsigned long irq_flags;
+
+	spin_lock_irqsave(&g_dcc_tty_lock, irq_flags);
+	if (g_dcc_tty == NULL || g_dcc_tty == tty) {
+		g_dcc_tty = tty;
+		g_dcc_tty_open_count++;
+		ret = 0;
+	} else
+		ret = -EBUSY;
+	spin_unlock_irqrestore(&g_dcc_tty_lock, irq_flags);
+
+	printk("dcc_tty_open, tty %p, f_flags %x, returned %d\n", tty, filp->f_flags, ret);
+
+	return ret;
+}
+
+static void dcc_tty_close(struct tty_struct * tty, struct file * filp)
+{
+	printk("dcc_tty_close, tty %p, f_flags %x\n", tty, filp->f_flags);
+	if (g_dcc_tty == tty) {
+		if (--g_dcc_tty_open_count == 0)
+			g_dcc_tty = NULL;
+	}
+}
+
+static int dcc_write(const unsigned char *buf_start, int count)
+{
+	const unsigned char *buf = buf_start;
+	unsigned long irq_flags;
+	int copy_len;
+	int space_left;
+	int tail;
+
+	if (count < 1)
+		return 0;
+
+	spin_lock_irqsave(&g_dcc_tty_lock, irq_flags);
+	do {
+		tail = (g_dcc_buffer_head + g_dcc_buffer_count) % ARRAY_SIZE(g_dcc_buffer);
+		copy_len = ARRAY_SIZE(g_dcc_buffer) - tail;
+		space_left = ARRAY_SIZE(g_dcc_buffer) - g_dcc_buffer_count;
+		if (copy_len > space_left)
+			copy_len = space_left;
+		if (copy_len > count)
+			copy_len = count;
+		memcpy(&g_dcc_buffer[tail], buf, copy_len);
+		g_dcc_buffer_count += copy_len;
+		buf += copy_len;
+		count -= copy_len;
+		if (copy_len < count && copy_len < space_left) {
+			space_left -= copy_len;
+			copy_len = count;
+			if (copy_len > space_left) {
+				copy_len = space_left;
+			}
+			memcpy(g_dcc_buffer, buf, copy_len);
+			buf += copy_len;
+			count -= copy_len;
+			g_dcc_buffer_count += copy_len;
+		}
+		dcc_poll_locked();
+		space_left = ARRAY_SIZE(g_dcc_buffer) - g_dcc_buffer_count;
+	} while(count && space_left);
+	spin_unlock_irqrestore(&g_dcc_tty_lock, irq_flags);
+	return buf - buf_start;
+}
+
+static int dcc_tty_write(struct tty_struct * tty, const unsigned char *buf, int count)
+{
+	int ret;
+	/* printk("dcc_tty_write %p, %d\n", buf, count); */
+	ret = dcc_write(buf, count);
+	if (ret != count)
+		printk("dcc_tty_write %p, %d, returned %d\n", buf, count, ret);
+	return ret;
+}
+
+static int dcc_tty_write_room(struct tty_struct *tty)
+{
+	int space_left;
+	unsigned long irq_flags;
+
+	spin_lock_irqsave(&g_dcc_tty_lock, irq_flags);
+	space_left = ARRAY_SIZE(g_dcc_buffer) - g_dcc_buffer_count;
+	spin_unlock_irqrestore(&g_dcc_tty_lock, irq_flags);
+	return space_left;
+}
+
+static int dcc_tty_chars_in_buffer(struct tty_struct *tty)
+{
+	int ret;
+	asm(
+		"mrc 14, 0, %0, c0, c1, 0\n"
+		"mov %0, %0, LSR #30\n"
+		"and %0, %0, #1\n"
+		: "=r" (ret)
+	);
+	return ret;
+}
+
+static void dcc_tty_unthrottle(struct tty_struct * tty)
+{
+	unsigned long irq_flags;
+
+	spin_lock_irqsave(&g_dcc_tty_lock, irq_flags);
+	dcc_poll_locked();
+	spin_unlock_irqrestore(&g_dcc_tty_lock, irq_flags);
+}
+
+static enum hrtimer_restart dcc_tty_timer_func(struct hrtimer *timer)
+{
+	unsigned long irq_flags;
+
+	spin_lock_irqsave(&g_dcc_tty_lock, irq_flags);
+	dcc_poll_locked();
+	spin_unlock_irqrestore(&g_dcc_tty_lock, irq_flags);
+	return HRTIMER_NORESTART;
+}
+
+void dcc_console_write(struct console *co, const char *b, unsigned count)
+{
+#if 1
+	dcc_write(b, count);
+#else
+	/* blocking printk */
+	while (count > 0) {
+		int written;
+		written = dcc_write(b, count);
+		if (written) {
+			b += written;
+			count -= written;
+		}
+	}
+#endif
+}
+
+static struct tty_driver *dcc_console_device(struct console *c, int *index)
+{
+	*index = 0;
+	return g_dcc_tty_driver;
+}
+
+static int __init dcc_console_setup(struct console *co, char *options)
+{
+	if (co->index != 0)
+		return -ENODEV;
+	return 0;
+}
+
+
+static struct console dcc_console =
+{
+	.name		= "ttyDCC",
+	.write		= dcc_console_write,
+	.device		= dcc_console_device,
+	.setup		= dcc_console_setup,
+	.flags		= CON_PRINTBUFFER,
+	.index		= -1,
+};
+
+static struct tty_operations dcc_tty_ops = {
+	.open = dcc_tty_open,
+	.close = dcc_tty_close,
+	.write = dcc_tty_write,
+	.write_room = dcc_tty_write_room,
+	.chars_in_buffer = dcc_tty_chars_in_buffer,
+	.unthrottle = dcc_tty_unthrottle,
+};
+
+static int __init dcc_tty_init(void)
+{
+	int ret;
+
+	hrtimer_init(&g_dcc_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	g_dcc_timer.function = dcc_tty_timer_func;
+
+	g_dcc_tty_driver = alloc_tty_driver(1);
+	if (!g_dcc_tty_driver) {
+		printk(KERN_ERR "dcc_tty_probe: alloc_tty_driver failed\n");
+		ret = -ENOMEM;
+		goto err_alloc_tty_driver_failed;
+	}
+	g_dcc_tty_driver->owner = THIS_MODULE;
+	g_dcc_tty_driver->driver_name = "dcc";
+	g_dcc_tty_driver->name = "ttyDCC";
+	g_dcc_tty_driver->major = 0; // auto assign
+	g_dcc_tty_driver->minor_start = 0;
+	g_dcc_tty_driver->type = TTY_DRIVER_TYPE_SERIAL;
+	g_dcc_tty_driver->subtype = SERIAL_TYPE_NORMAL;
+	g_dcc_tty_driver->init_termios = tty_std_termios;
+	g_dcc_tty_driver->flags = TTY_DRIVER_RESET_TERMIOS | TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV;
+	tty_set_operations(g_dcc_tty_driver, &dcc_tty_ops);
+	ret = tty_register_driver(g_dcc_tty_driver);
+	if (ret) {
+		printk(KERN_ERR "dcc_tty_probe: tty_register_driver failed, %d\n", ret);
+		goto err_tty_register_driver_failed;
+	}
+	tty_register_device(g_dcc_tty_driver, 0, NULL);
+
+	register_console(&dcc_console);
+	hrtimer_start(&g_dcc_timer, ktime_set(0, 0), HRTIMER_MODE_REL);
+
+	return 0;
+
+err_tty_register_driver_failed:
+	put_tty_driver(g_dcc_tty_driver);
+	g_dcc_tty_driver = NULL;
+err_alloc_tty_driver_failed:
+	return ret;
+}
+
+static void  __exit dcc_tty_exit(void)
+{
+	int ret;
+
+	tty_unregister_device(g_dcc_tty_driver, 0);
+	ret = tty_unregister_driver(g_dcc_tty_driver);
+	if (ret < 0) {
+		printk(KERN_ERR "dcc_tty_remove: tty_unregister_driver failed, %d\n", ret);
+	} else {
+		put_tty_driver(g_dcc_tty_driver);
+	}
+	g_dcc_tty_driver = NULL;
+}
+
+module_init(dcc_tty_init);
+module_exit(dcc_tty_exit);
+
+
diff --git a/drivers/char/mem.c b/drivers/char/mem.c
index 8fc04b4f311f..9b1eb188acdc 100644
--- a/drivers/char/mem.c
+++ b/drivers/char/mem.c
@@ -56,6 +56,7 @@ static inline int valid_mmap_phys_addr_range(unsigned long pfn, size_t size)
 }
 #endif
 
+#if defined(CONFIG_DEVMEM) || defined(CONFIG_DEVKMEM)
 #ifdef CONFIG_STRICT_DEVMEM
 static inline int range_is_allowed(unsigned long pfn, unsigned long size)
 {
@@ -81,7 +82,9 @@ static inline int range_is_allowed(unsigned long pfn, unsigned long size)
 	return 1;
 }
 #endif
+#endif
 
+#ifdef CONFIG_DEVMEM
 void __weak unxlate_dev_mem_ptr(unsigned long phys, void *addr)
 {
 }
@@ -208,6 +211,9 @@ static ssize_t write_mem(struct file *file, const char __user *buf,
 	*ppos += written;
 	return written;
 }
+#endif	/* CONFIG_DEVMEM */
+
+#if defined(CONFIG_DEVMEM) || defined(CONFIG_DEVKMEM)
 
 int __weak phys_mem_access_prot_allowed(struct file *file,
 	unsigned long pfn, unsigned long size, pgprot_t *vma_prot)
@@ -329,6 +335,7 @@ static int mmap_mem(struct file *file, struct vm_area_struct *vma)
 	}
 	return 0;
 }
+#endif	/* CONFIG_DEVMEM */
 
 #ifdef CONFIG_DEVKMEM
 static int mmap_kmem(struct file *file, struct vm_area_struct *vma)
@@ -693,6 +700,8 @@ static loff_t null_lseek(struct file *file, loff_t offset, int orig)
 	return file->f_pos = 0;
 }
 
+#if defined(CONFIG_DEVMEM) || defined(CONFIG_DEVKMEM) || defined(CONFIG_DEVPORT)
+
 /*
  * The memory devices use the full 32/64 bits of the offset, and so we cannot
  * check against negative addresses: they are ok. The return value is weird,
@@ -726,10 +735,14 @@ static loff_t memory_lseek(struct file *file, loff_t offset, int orig)
 	return ret;
 }
 
+#endif
+
+#if defined(CONFIG_DEVMEM) || defined(CONFIG_DEVKMEM) || defined(CONFIG_DEVPORT)
 static int open_port(struct inode * inode, struct file * filp)
 {
 	return capable(CAP_SYS_RAWIO) ? 0 : -EPERM;
 }
+#endif
 
 #define zero_lseek	null_lseek
 #define full_lseek      null_lseek
@@ -739,6 +752,7 @@ static int open_port(struct inode * inode, struct file * filp)
 #define open_kmem	open_mem
 #define open_oldmem	open_mem
 
+#ifdef CONFIG_DEVMEM
 static const struct file_operations mem_fops = {
 	.llseek		= memory_lseek,
 	.read		= read_mem,
@@ -747,6 +761,7 @@ static const struct file_operations mem_fops = {
 	.open		= open_mem,
 	.get_unmapped_area = get_unmapped_area_mem,
 };
+#endif
 
 #ifdef CONFIG_DEVKMEM
 static const struct file_operations kmem_fops = {
@@ -850,7 +865,9 @@ static const struct memdev {
 	const struct file_operations *fops;
 	struct backing_dev_info *dev_info;
 } devlist[] = {
+#ifdef CONFIG_DEVMEM
 	 [1] = { "mem", 0, &mem_fops, &directly_mappable_cdev_bdi },
+#endif
 #ifdef CONFIG_DEVKMEM
 	 [2] = { "kmem", 0, &kmem_fops, &directly_mappable_cdev_bdi },
 #endif
diff --git a/drivers/clk/clkdev.c b/drivers/clk/clkdev.c
index 6db161f64ae0..a9a113782821 100644
--- a/drivers/clk/clkdev.c
+++ b/drivers/clk/clkdev.c
@@ -89,6 +89,51 @@ void clk_put(struct clk *clk)
 }
 EXPORT_SYMBOL(clk_put);
 
+static void devm_clk_release(struct device *dev, void *res)
+{
+	clk_put(*(struct clk **)res);
+}
+
+struct clk *devm_clk_get(struct device *dev, const char *id)
+{
+	struct clk **ptr, *clk;
+
+	ptr = devres_alloc(devm_clk_release, sizeof(*ptr), GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	clk = clk_get(dev, id);
+	if (!IS_ERR(clk)) {
+		*ptr = clk;
+		devres_add(dev, ptr);
+	} else {
+		devres_free(ptr);
+	}
+
+	return clk;
+}
+EXPORT_SYMBOL(devm_clk_get);
+
+static int devm_clk_match(struct device *dev, void *res, void *data)
+{
+	struct clk **c = res;
+	if (!c || !*c) {
+		WARN_ON(!c || !*c);
+		return 0;
+	}
+	return *c == data;
+}
+
+void devm_clk_put(struct device *dev, struct clk *clk)
+{
+	int ret;
+
+	ret = devres_destroy(dev, devm_clk_release, devm_clk_match, clk);
+
+	WARN_ON(ret);
+}
+EXPORT_SYMBOL(devm_clk_put);
+
 void clkdev_add(struct clk_lookup *cl)
 {
 	mutex_lock(&clocks_mutex);
diff --git a/drivers/cpufreq/Kconfig b/drivers/cpufreq/Kconfig
index e24a2a1b6666..57f96ebbce4b 100644
--- a/drivers/cpufreq/Kconfig
+++ b/drivers/cpufreq/Kconfig
@@ -99,6 +99,16 @@ config CPU_FREQ_DEFAULT_GOV_CONSERVATIVE
 	  Be aware that not all cpufreq drivers support the conservative
 	  governor. If unsure have a look at the help section of the
 	  driver. Fallback governor will be the performance governor.
+
+config CPU_FREQ_DEFAULT_GOV_INTERACTIVE
+	bool "interactive"
+	select CPU_FREQ_GOV_INTERACTIVE
+	help
+	  Use the CPUFreq governor 'interactive' as default. This allows
+	  you to get a full dynamic cpu frequency capable system by simply
+	  loading your cpufreq low-level hardware driver, using the
+	  'interactive' governor for latency-sensitive workloads.
+
 endchoice
 
 config CPU_FREQ_GOV_PERFORMANCE
@@ -156,6 +166,23 @@ config CPU_FREQ_GOV_ONDEMAND
 
 	  If in doubt, say N.
 
+config CPU_FREQ_GOV_INTERACTIVE
+	tristate "'interactive' cpufreq policy governor"
+	help
+	  'interactive' - This driver adds a dynamic cpufreq policy governor
+	  designed for latency-sensitive workloads.
+
+	  This governor attempts to reduce the latency of clock
+	  increases so that the system is more responsive to
+	  interactive workloads.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called cpufreq_interactive.
+
+	  For details, take a look at linux/Documentation/cpu-freq.
+
+	  If in doubt, say N.
+
 config CPU_FREQ_GOV_CONSERVATIVE
 	tristate "'conservative' cpufreq governor"
 	depends on CPU_FREQ
diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
index a48bc02cd765..d43b39150efa 100644
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -9,6 +9,7 @@ obj-$(CONFIG_CPU_FREQ_GOV_POWERSAVE)	+= cpufreq_powersave.o
 obj-$(CONFIG_CPU_FREQ_GOV_USERSPACE)	+= cpufreq_userspace.o
 obj-$(CONFIG_CPU_FREQ_GOV_ONDEMAND)	+= cpufreq_ondemand.o
 obj-$(CONFIG_CPU_FREQ_GOV_CONSERVATIVE)	+= cpufreq_conservative.o
+obj-$(CONFIG_CPU_FREQ_GOV_INTERACTIVE)	+= cpufreq_interactive.o
 
 # CPUfreq cross-arch helpers
 obj-$(CONFIG_CPU_FREQ_TABLE)		+= freq_table.o
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index 987a165ede26..02e8c9120d6b 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -29,6 +29,7 @@
 #include <linux/completion.h>
 #include <linux/mutex.h>
 #include <linux/syscore_ops.h>
+#include <linux/pm_qos_params.h>
 
 #include <trace/events/power.h>
 
@@ -364,6 +365,8 @@ show_one(cpuinfo_transition_latency, cpuinfo.transition_latency);
 show_one(scaling_min_freq, min);
 show_one(scaling_max_freq, max);
 show_one(scaling_cur_freq, cur);
+show_one(policy_min_freq, user_policy.min);
+show_one(policy_max_freq, user_policy.max);
 
 static int __cpufreq_set_policy(struct cpufreq_policy *data,
 				struct cpufreq_policy *policy);
@@ -387,7 +390,7 @@ static ssize_t store_##file_name					\
 		return -EINVAL;						\
 									\
 	ret = __cpufreq_set_policy(policy, &new_policy);		\
-	policy->user_policy.object = policy->object;			\
+	policy->user_policy.object = new_policy.object;			\
 									\
 	return ret ? ret : count;					\
 }
@@ -582,6 +585,8 @@ cpufreq_freq_attr_rw(scaling_min_freq);
 cpufreq_freq_attr_rw(scaling_max_freq);
 cpufreq_freq_attr_rw(scaling_governor);
 cpufreq_freq_attr_rw(scaling_setspeed);
+cpufreq_freq_attr_ro(policy_min_freq);
+cpufreq_freq_attr_ro(policy_max_freq);
 
 static struct attribute *default_attrs[] = {
 	&cpuinfo_min_freq.attr,
@@ -595,6 +600,8 @@ static struct attribute *default_attrs[] = {
 	&scaling_driver.attr,
 	&scaling_available_governors.attr,
 	&scaling_setspeed.attr,
+	&policy_min_freq.attr,
+	&policy_max_freq.attr,
 	NULL
 };
 
@@ -1442,8 +1449,10 @@ int __cpufreq_driver_target(struct cpufreq_policy *policy,
 
 	pr_debug("target for CPU %u: %u kHz, relation %u\n", policy->cpu,
 		target_freq, relation);
+	trace_cpu_scale(policy->cpu, policy->cur, POWER_CPU_SCALE_START);
 	if (cpu_online(policy->cpu) && cpufreq_driver->target)
 		retval = cpufreq_driver->target(policy, target_freq, relation);
+	trace_cpu_scale(policy->cpu, target_freq, POWER_CPU_SCALE_DONE);
 
 	return retval;
 }
@@ -1626,14 +1635,27 @@ static int __cpufreq_set_policy(struct cpufreq_policy *data,
 				struct cpufreq_policy *policy)
 {
 	int ret = 0;
+	unsigned int qmin, qmax;
+	unsigned int pmin = policy->min;
+	unsigned int pmax = policy->max;
 
-	pr_debug("setting new policy for CPU %u: %u - %u kHz\n", policy->cpu,
-		policy->min, policy->max);
+	qmin = min((unsigned int)pm_qos_request(PM_QOS_CPU_FREQ_MIN),
+		   data->user_policy.max);
+	qmax = max((unsigned int)pm_qos_request(PM_QOS_CPU_FREQ_MAX),
+		   data->user_policy.min);
+
+	pr_debug("setting new policy for CPU %u: %u - %u (%u - %u) kHz\n",
+		policy->cpu, pmin, pmax, qmin, qmax);
+
+	/* clamp the new policy to PM QoS limits */
+	policy->min = max(pmin, qmin);
+	policy->max = min(pmax, qmax);
 
 	memcpy(&policy->cpuinfo, &data->cpuinfo,
 				sizeof(struct cpufreq_cpuinfo));
 
-	if (policy->min > data->max || policy->max < data->min) {
+	if (policy->min > data->user_policy.max ||
+	    policy->max < data->user_policy.min) {
 		ret = -EINVAL;
 		goto error_out;
 	}
@@ -1703,6 +1725,9 @@ static int __cpufreq_set_policy(struct cpufreq_policy *data,
 	}
 
 error_out:
+	/* restore the limits that the policy requested */
+	policy->min = pmin;
+	policy->max = pmax;
 	return ret;
 }
 
@@ -1761,6 +1786,70 @@ no_policy:
 }
 EXPORT_SYMBOL(cpufreq_update_policy);
 
+/*
+ *	cpufreq_set_gov - set governor for a cpu
+ *	@cpu: CPU whose governor needs to be changed
+ *	@target_gov: new governor to be set
+ */
+int cpufreq_set_gov(char *target_gov, unsigned int cpu)
+{
+	int ret = 0;
+	struct cpufreq_policy new_policy;
+	struct cpufreq_policy *cur_policy;
+
+	if (target_gov == NULL)
+		return -EINVAL;
+
+	/* Get current governer */
+	cur_policy = cpufreq_cpu_get(cpu);
+	if (!cur_policy)
+		return -EINVAL;
+
+	if (lock_policy_rwsem_read(cur_policy->cpu) < 0) {
+		ret = -EINVAL;
+		goto err_out;
+	}
+
+	if (cur_policy->governor)
+		ret = strncmp(cur_policy->governor->name, target_gov,
+					strlen(target_gov));
+	else {
+		unlock_policy_rwsem_read(cur_policy->cpu);
+		ret = -EINVAL;
+		goto err_out;
+	}
+	unlock_policy_rwsem_read(cur_policy->cpu);
+
+	if (!ret) {
+		pr_debug(" Target governer & current governer is same\n");
+		ret = -EINVAL;
+		goto err_out;
+	} else {
+		new_policy = *cur_policy;
+		if (cpufreq_parse_governor(target_gov, &new_policy.policy,
+				&new_policy.governor)) {
+			ret = -EINVAL;
+			goto err_out;
+		}
+
+		if (lock_policy_rwsem_write(cur_policy->cpu) < 0) {
+			ret = -EINVAL;
+			goto err_out;
+		}
+
+		ret = __cpufreq_set_policy(cur_policy, &new_policy);
+
+		cur_policy->user_policy.policy = cur_policy->policy;
+		cur_policy->user_policy.governor = cur_policy->governor;
+
+		unlock_policy_rwsem_write(cur_policy->cpu);
+	}
+err_out:
+	cpufreq_cpu_put(cur_policy);
+	return ret;
+}
+EXPORT_SYMBOL(cpufreq_set_gov);
+
 static int __cpuinit cpufreq_cpu_callback(struct notifier_block *nfb,
 					unsigned long action, void *hcpu)
 {
@@ -1898,9 +1987,36 @@ int cpufreq_unregister_driver(struct cpufreq_driver *driver)
 }
 EXPORT_SYMBOL_GPL(cpufreq_unregister_driver);
 
+static int cpu_freq_notify(struct notifier_block *b,
+			   unsigned long l, void *v);
+
+static struct notifier_block min_freq_notifier = {
+	.notifier_call = cpu_freq_notify,
+};
+static struct notifier_block max_freq_notifier = {
+	.notifier_call = cpu_freq_notify,
+};
+
+static int cpu_freq_notify(struct notifier_block *b,
+			   unsigned long l, void *v)
+{
+	int cpu;
+	pr_debug("PM QoS %s %lu\n",
+		b == &min_freq_notifier ? "min" : "max", l);
+	for_each_online_cpu(cpu) {
+		struct cpufreq_policy *policy = cpufreq_cpu_get(cpu);
+		if (policy) {
+			cpufreq_update_policy(policy->cpu);
+			cpufreq_cpu_put(policy);
+		}
+	}
+	return NOTIFY_OK;
+}
+
 static int __init cpufreq_core_init(void)
 {
 	int cpu;
+	int rc;
 
 	for_each_possible_cpu(cpu) {
 		per_cpu(cpufreq_policy_cpu, cpu) = -1;
@@ -1911,6 +2027,12 @@ static int __init cpufreq_core_init(void)
 						&cpu_sysdev_class.kset.kobj);
 	BUG_ON(!cpufreq_global_kobject);
 	register_syscore_ops(&cpufreq_syscore_ops);
+	rc = pm_qos_add_notifier(PM_QOS_CPU_FREQ_MIN,
+				 &min_freq_notifier);
+	BUG_ON(rc);
+	rc = pm_qos_add_notifier(PM_QOS_CPU_FREQ_MAX,
+				 &max_freq_notifier);
+	BUG_ON(rc);
 
 	return 0;
 }
diff --git a/drivers/cpufreq/cpufreq_interactive.c b/drivers/cpufreq/cpufreq_interactive.c
new file mode 100644
index 000000000000..cf6ba6c1958e
--- /dev/null
+++ b/drivers/cpufreq/cpufreq_interactive.c
@@ -0,0 +1,813 @@
+/*
+ * drivers/cpufreq/cpufreq_interactive.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ * Author: Mike Chan (mike@android.com)
+ *
+ */
+
+#include <linux/cpu.h>
+#include <linux/cpumask.h>
+#include <linux/cpufreq.h>
+#include <linux/mutex.h>
+#include <linux/sched.h>
+#include <linux/tick.h>
+#include <linux/timer.h>
+#include <linux/workqueue.h>
+#include <linux/kthread.h>
+#include <linux/mutex.h>
+
+#include <asm/cputime.h>
+
+struct cpufreq_interactive_cpuinfo {
+	struct timer_list cpu_timer;
+	int timer_idlecancel;
+	u64 time_in_idle;
+	u64 time_in_iowait;
+	u64 idle_exit_time;
+	u64 timer_run_time;
+	int idling;
+	u64 freq_change_time;
+	u64 freq_change_time_in_idle;
+	u64 freq_change_time_in_iowait;
+	u64 last_high_freq_time;
+	struct cpufreq_policy *policy;
+	struct cpufreq_frequency_table *freq_table;
+	unsigned int target_freq;
+	int governor_enabled;
+};
+
+static DEFINE_PER_CPU(struct cpufreq_interactive_cpuinfo, cpuinfo);
+
+/* Workqueues handle frequency scaling */
+static struct task_struct *up_task;
+static struct workqueue_struct *down_wq;
+static struct work_struct freq_scale_down_work;
+static cpumask_t up_cpumask;
+static spinlock_t up_cpumask_lock;
+static cpumask_t down_cpumask;
+static spinlock_t down_cpumask_lock;
+static struct mutex set_speed_lock;
+static struct mutex gov_state_lock;
+static struct kobject *interactive_kobj;
+static unsigned int active_count;
+
+/* Go to max speed when CPU load at or above this value. */
+#define DEFAULT_GO_MAXSPEED_LOAD 85
+static unsigned long go_maxspeed_load;
+
+/* Base of exponential raise to max speed; if 0 - jump to maximum */
+static unsigned long boost_factor;
+
+/* Max frequency boost in Hz; if 0 - no max is enforced */
+static unsigned long max_boost;
+
+/* Consider IO as busy */
+static unsigned long io_is_busy;
+
+/*
+ * Targeted sustainable load relatively to current frequency.
+ * If 0, target is set realtively to the max speed
+ */
+static unsigned long sustain_load;
+
+/*
+ * The minimum amount of time to spend at a frequency before we can ramp down.
+ */
+#define DEFAULT_MIN_SAMPLE_TIME 30000;
+static unsigned long min_sample_time;
+
+/*
+ * The sample rate of the timer used to increase frequency
+ */
+#define DEFAULT_TIMER_RATE 20000;
+static unsigned long timer_rate;
+
+/*
+ * The minimum delay before frequency is allowed to raise over normal rate.
+ * Since it must remain at high frequency for a minimum of MIN_SAMPLE_TIME
+ * once it rises, setting this delay to a multiple of MIN_SAMPLE_TIME
+ * becomes the best way to enforce a square wave.
+ * e.g. 5*MIN_SAMPLE_TIME = 20% high freq duty cycle
+ */
+#define DEFAULT_HIGH_FREQ_MIN_DELAY 5*DEFAULT_MIN_SAMPLE_TIME
+static unsigned long high_freq_min_delay;
+
+/*
+ * The maximum frequency CPUs are allowed to run normally
+ * 0 if disabled
+ */
+#define DEFAULT_MAX_NORMAL_FREQ 0
+static unsigned long max_normal_freq;
+
+
+/* Defines to control mid-range frequencies */
+#define DEFAULT_MID_RANGE_GO_MAXSPEED_LOAD 95
+
+static unsigned long midrange_freq;
+static unsigned long midrange_go_maxspeed_load;
+static unsigned long midrange_max_boost;
+
+static int cpufreq_governor_interactive(struct cpufreq_policy *policy,
+		unsigned int event);
+
+#ifndef CONFIG_CPU_FREQ_DEFAULT_GOV_INTERACTIVE
+static
+#endif
+struct cpufreq_governor cpufreq_gov_interactive = {
+	.name = "interactive",
+	.governor = cpufreq_governor_interactive,
+	.max_transition_latency = 10000000,
+	.owner = THIS_MODULE,
+};
+
+static unsigned int cpufreq_interactive_get_target(
+	int cpu_load, int load_since_change, struct cpufreq_policy *policy)
+{
+	unsigned int target_freq;
+	unsigned int maxspeed_load = go_maxspeed_load;
+	unsigned int mboost = max_boost;
+
+	/*
+	 * Choose greater of short-term load (since last idle timer
+	 * started or timer function re-armed itself) or long-term load
+	 * (since last frequency change).
+	 */
+	if (load_since_change > cpu_load)
+		cpu_load = load_since_change;
+
+	if (midrange_freq && policy->cur > midrange_freq) {
+		maxspeed_load = midrange_go_maxspeed_load;
+		mboost = midrange_max_boost;
+	}
+
+	if (cpu_load >= maxspeed_load) {
+		if (!boost_factor)
+			return policy->max;
+
+		target_freq = policy->cur * boost_factor;
+
+		if (mboost && target_freq > policy->cur + mboost)
+			target_freq = policy->cur + mboost;
+	}
+	else {
+		if (!sustain_load)
+			return policy->max * cpu_load / 100;
+
+		target_freq = policy->cur * cpu_load / sustain_load;
+	}
+
+	target_freq = min(target_freq, policy->max);
+	return target_freq;
+}
+
+static inline cputime64_t get_cpu_iowait_time(
+	unsigned int cpu, cputime64_t *wall)
+{
+	u64 iowait_time = get_cpu_iowait_time_us(cpu, wall);
+
+	if (iowait_time == -1ULL)
+		return 0;
+
+	return iowait_time;
+}
+
+static void cpufreq_interactive_timer(unsigned long data)
+{
+	unsigned int delta_idle;
+	unsigned int delta_iowait;
+	unsigned int delta_time;
+	int cpu_load;
+	int load_since_change;
+	u64 time_in_idle;
+	u64 time_in_iowait;
+	u64 idle_exit_time;
+	struct cpufreq_interactive_cpuinfo *pcpu =
+		&per_cpu(cpuinfo, data);
+	u64 now_idle;
+	u64 now_iowait;
+	unsigned int new_freq;
+	unsigned int index;
+	unsigned long flags;
+
+	smp_rmb();
+
+	if (!pcpu->governor_enabled)
+		goto exit;
+
+	/*
+	 * Once pcpu->timer_run_time is updated to >= pcpu->idle_exit_time,
+	 * this lets idle exit know the current idle time sample has
+	 * been processed, and idle exit can generate a new sample and
+	 * re-arm the timer.  This prevents a concurrent idle
+	 * exit on that CPU from writing a new set of info at the same time
+	 * the timer function runs (the timer function can't use that info
+	 * until more time passes).
+	 */
+	time_in_idle = pcpu->time_in_idle;
+	time_in_iowait = pcpu->time_in_iowait;
+	idle_exit_time = pcpu->idle_exit_time;
+	now_idle = get_cpu_idle_time_us(data, &pcpu->timer_run_time);
+	now_iowait = get_cpu_iowait_time(data, NULL);
+	smp_wmb();
+
+	/* If we raced with cancelling a timer, skip. */
+	if (!idle_exit_time)
+		goto exit;
+
+	delta_idle = (unsigned int) cputime64_sub(now_idle, time_in_idle);
+	delta_iowait = (unsigned int) cputime64_sub(now_iowait, time_in_iowait);
+	delta_time = (unsigned int) cputime64_sub(pcpu->timer_run_time,
+						  idle_exit_time);
+
+	/*
+	 * If timer ran less than 1ms after short-term sample started, retry.
+	 */
+	if (delta_time < 1000)
+		goto rearm;
+
+	if (delta_idle > delta_time)
+		cpu_load = 0;
+	else {
+		if (io_is_busy && delta_idle >= delta_iowait)
+			delta_idle -= delta_iowait;
+
+		cpu_load = 100 * (delta_time - delta_idle) / delta_time;
+	}
+
+	delta_idle = (unsigned int) cputime64_sub(now_idle,
+						pcpu->freq_change_time_in_idle);
+	delta_iowait = (unsigned int) cputime64_sub(now_iowait,
+					pcpu->freq_change_time_in_iowait);
+	delta_time = (unsigned int) cputime64_sub(pcpu->timer_run_time,
+						  pcpu->freq_change_time);
+
+	if ((delta_time == 0) || (delta_idle > delta_time))
+		load_since_change = 0;
+	else {
+		if (io_is_busy && delta_idle >= delta_iowait)
+			delta_idle -= delta_iowait;
+
+		load_since_change =
+			100 * (delta_time - delta_idle) / delta_time;
+	}
+
+	/*
+	 * Combine short-term load (since last idle timer started or timer
+	 * function re-armed itself) and long-term load (since last frequency
+	 * change) to determine new target frequency
+	 */
+	new_freq = cpufreq_interactive_get_target(cpu_load, load_since_change,
+						  pcpu->policy);
+
+	if (cpufreq_frequency_table_target(pcpu->policy, pcpu->freq_table,
+					   new_freq, CPUFREQ_RELATION_H,
+					   &index)) {
+		pr_warn_once("timer %d: cpufreq_frequency_table_target error\n",
+			     (int) data);
+		goto rearm;
+	}
+
+	new_freq = pcpu->freq_table[index].frequency;
+
+	if (pcpu->target_freq == new_freq)
+		goto rearm_if_notmax;
+
+	/*
+	 * Do not scale down unless we have been at this frequency for the
+	 * minimum sample time.
+	 */
+	if (new_freq < pcpu->target_freq) {
+		if (cputime64_sub(pcpu->timer_run_time, pcpu->freq_change_time)
+		    < min_sample_time)
+			goto rearm;
+	}
+
+	/*
+	 * Can only overclock if the delay is satisfy. Otherwise, cap it to
+	 * maximum allowed normal frequency
+	 */
+	if (max_normal_freq && (new_freq > max_normal_freq)) {
+		if (cputime64_sub(pcpu->timer_run_time, pcpu->last_high_freq_time)
+				< high_freq_min_delay) {
+			new_freq = max_normal_freq;
+		}
+		else {
+			pcpu->last_high_freq_time = pcpu->timer_run_time;
+		}
+	}
+
+	if (new_freq < pcpu->target_freq) {
+		pcpu->target_freq = new_freq;
+		spin_lock_irqsave(&down_cpumask_lock, flags);
+		cpumask_set_cpu(data, &down_cpumask);
+		spin_unlock_irqrestore(&down_cpumask_lock, flags);
+		queue_work(down_wq, &freq_scale_down_work);
+	} else {
+		pcpu->target_freq = new_freq;
+		spin_lock_irqsave(&up_cpumask_lock, flags);
+		cpumask_set_cpu(data, &up_cpumask);
+		spin_unlock_irqrestore(&up_cpumask_lock, flags);
+		wake_up_process(up_task);
+	}
+
+rearm_if_notmax:
+	/*
+	 * Already set max speed and don't see a need to change that,
+	 * wait until next idle to re-evaluate, don't need timer.
+	 */
+	if (pcpu->target_freq == pcpu->policy->max)
+		goto exit;
+
+rearm:
+	if (!timer_pending(&pcpu->cpu_timer)) {
+		/*
+		 * If already at min: if that CPU is idle, don't set timer.
+		 * Else cancel the timer if that CPU goes idle.  We don't
+		 * need to re-evaluate speed until the next idle exit.
+		 */
+		if (pcpu->target_freq == pcpu->policy->min) {
+			smp_rmb();
+
+			if (pcpu->idling)
+				goto exit;
+
+			pcpu->timer_idlecancel = 1;
+		}
+
+		pcpu->time_in_idle = get_cpu_idle_time_us(
+			data, &pcpu->idle_exit_time);
+		pcpu->time_in_iowait = get_cpu_iowait_time(
+			data, NULL);
+
+		mod_timer(&pcpu->cpu_timer,
+			  jiffies + usecs_to_jiffies(timer_rate));
+	}
+
+exit:
+	return;
+}
+
+static void cpufreq_interactive_idle_start(void)
+{
+	struct cpufreq_interactive_cpuinfo *pcpu =
+		&per_cpu(cpuinfo, smp_processor_id());
+	int pending;
+
+	if (!pcpu->governor_enabled)
+		return;
+
+	pcpu->idling = 1;
+	smp_wmb();
+	pending = timer_pending(&pcpu->cpu_timer);
+
+	if (pcpu->target_freq != pcpu->policy->min) {
+#ifdef CONFIG_SMP
+		/*
+		 * Entering idle while not at lowest speed.  On some
+		 * platforms this can hold the other CPU(s) at that speed
+		 * even though the CPU is idle. Set a timer to re-evaluate
+		 * speed so this idle CPU doesn't hold the other CPUs above
+		 * min indefinitely.  This should probably be a quirk of
+		 * the CPUFreq driver.
+		 */
+		if (!pending) {
+			pcpu->time_in_idle = get_cpu_idle_time_us(
+				smp_processor_id(), &pcpu->idle_exit_time);
+			pcpu->time_in_iowait = get_cpu_iowait_time(
+				smp_processor_id(), NULL);
+			pcpu->timer_idlecancel = 0;
+			mod_timer(&pcpu->cpu_timer,
+				  jiffies + usecs_to_jiffies(timer_rate));
+		}
+#endif
+	} else {
+		/*
+		 * If at min speed and entering idle after load has
+		 * already been evaluated, and a timer has been set just in
+		 * case the CPU suddenly goes busy, cancel that timer.  The
+		 * CPU didn't go busy; we'll recheck things upon idle exit.
+		 */
+		if (pending && pcpu->timer_idlecancel) {
+			del_timer(&pcpu->cpu_timer);
+			/*
+			 * Ensure last timer run time is after current idle
+			 * sample start time, so next idle exit will always
+			 * start a new idle sampling period.
+			 */
+			pcpu->idle_exit_time = 0;
+			pcpu->timer_idlecancel = 0;
+		}
+	}
+
+}
+
+static void cpufreq_interactive_idle_end(void)
+{
+	struct cpufreq_interactive_cpuinfo *pcpu =
+		&per_cpu(cpuinfo, smp_processor_id());
+
+	pcpu->idling = 0;
+	smp_wmb();
+
+	/*
+	 * Arm the timer for 1-2 ticks later if not already, and if the timer
+	 * function has already processed the previous load sampling
+	 * interval.  (If the timer is not pending but has not processed
+	 * the previous interval, it is probably racing with us on another
+	 * CPU.  Let it compute load based on the previous sample and then
+	 * re-arm the timer for another interval when it's done, rather
+	 * than updating the interval start time to be "now", which doesn't
+	 * give the timer function enough time to make a decision on this
+	 * run.)
+	 */
+	if (timer_pending(&pcpu->cpu_timer) == 0 &&
+	    pcpu->timer_run_time >= pcpu->idle_exit_time &&
+	    pcpu->governor_enabled) {
+		pcpu->time_in_idle =
+			get_cpu_idle_time_us(smp_processor_id(),
+					     &pcpu->idle_exit_time);
+		pcpu->time_in_iowait =
+			get_cpu_iowait_time(smp_processor_id(),
+						NULL);
+		pcpu->timer_idlecancel = 0;
+		mod_timer(&pcpu->cpu_timer,
+			  jiffies + usecs_to_jiffies(timer_rate));
+	}
+
+}
+
+static int cpufreq_interactive_up_task(void *data)
+{
+	unsigned int cpu;
+	cpumask_t tmp_mask;
+	unsigned long flags;
+	struct cpufreq_interactive_cpuinfo *pcpu;
+
+	while (1) {
+		set_current_state(TASK_INTERRUPTIBLE);
+		spin_lock_irqsave(&up_cpumask_lock, flags);
+
+		if (cpumask_empty(&up_cpumask)) {
+			spin_unlock_irqrestore(&up_cpumask_lock, flags);
+			schedule();
+
+			if (kthread_should_stop())
+				break;
+
+			spin_lock_irqsave(&up_cpumask_lock, flags);
+		}
+
+		set_current_state(TASK_RUNNING);
+		tmp_mask = up_cpumask;
+		cpumask_clear(&up_cpumask);
+		spin_unlock_irqrestore(&up_cpumask_lock, flags);
+
+		for_each_cpu(cpu, &tmp_mask) {
+			unsigned int j;
+			unsigned int max_freq = 0;
+
+			pcpu = &per_cpu(cpuinfo, cpu);
+			smp_rmb();
+
+			if (!pcpu->governor_enabled)
+				continue;
+
+			mutex_lock(&set_speed_lock);
+
+			for_each_cpu(j, pcpu->policy->cpus) {
+				struct cpufreq_interactive_cpuinfo *pjcpu =
+					&per_cpu(cpuinfo, j);
+
+				if (pjcpu->target_freq > max_freq)
+					max_freq = pjcpu->target_freq;
+			}
+
+			__cpufreq_driver_target(pcpu->policy,
+						max_freq,
+						CPUFREQ_RELATION_H);
+			mutex_unlock(&set_speed_lock);
+
+			pcpu->freq_change_time_in_idle =
+				get_cpu_idle_time_us(cpu,
+						     &pcpu->freq_change_time);
+			pcpu->freq_change_time_in_iowait =
+				get_cpu_iowait_time(cpu, NULL);
+		}
+	}
+
+	return 0;
+}
+
+static void cpufreq_interactive_freq_down(struct work_struct *work)
+{
+	unsigned int cpu;
+	cpumask_t tmp_mask;
+	unsigned long flags;
+	struct cpufreq_interactive_cpuinfo *pcpu;
+
+	spin_lock_irqsave(&down_cpumask_lock, flags);
+	tmp_mask = down_cpumask;
+	cpumask_clear(&down_cpumask);
+	spin_unlock_irqrestore(&down_cpumask_lock, flags);
+
+	for_each_cpu(cpu, &tmp_mask) {
+		unsigned int j;
+		unsigned int max_freq = 0;
+
+		pcpu = &per_cpu(cpuinfo, cpu);
+		smp_rmb();
+
+		if (!pcpu->governor_enabled)
+			continue;
+
+		mutex_lock(&set_speed_lock);
+
+		for_each_cpu(j, pcpu->policy->cpus) {
+			struct cpufreq_interactive_cpuinfo *pjcpu =
+				&per_cpu(cpuinfo, j);
+
+			if (pjcpu->target_freq > max_freq)
+				max_freq = pjcpu->target_freq;
+		}
+
+		__cpufreq_driver_target(pcpu->policy, max_freq,
+					CPUFREQ_RELATION_H);
+
+		mutex_unlock(&set_speed_lock);
+		pcpu->freq_change_time_in_idle =
+			get_cpu_idle_time_us(cpu,
+					     &pcpu->freq_change_time);
+		pcpu->freq_change_time_in_iowait =
+			get_cpu_iowait_time(cpu, NULL);
+	}
+}
+
+#define DECL_CPUFREQ_INTERACTIVE_ATTR(name) \
+static ssize_t show_##name(struct kobject *kobj, \
+	struct attribute *attr, char *buf) \
+{ \
+	return sprintf(buf, "%lu\n", name); \
+} \
+\
+static ssize_t store_##name(struct kobject *kobj,\
+		struct attribute *attr, const char *buf, size_t count) \
+{ \
+	int ret; \
+	unsigned long val; \
+\
+	ret = strict_strtoul(buf, 0, &val); \
+	if (ret < 0) \
+		return ret; \
+	name = val; \
+	return count; \
+} \
+\
+static struct global_attr name##_attr = __ATTR(name, 0644, \
+		show_##name, store_##name);
+
+DECL_CPUFREQ_INTERACTIVE_ATTR(go_maxspeed_load)
+DECL_CPUFREQ_INTERACTIVE_ATTR(midrange_freq)
+DECL_CPUFREQ_INTERACTIVE_ATTR(midrange_go_maxspeed_load)
+DECL_CPUFREQ_INTERACTIVE_ATTR(boost_factor)
+DECL_CPUFREQ_INTERACTIVE_ATTR(io_is_busy)
+DECL_CPUFREQ_INTERACTIVE_ATTR(max_boost)
+DECL_CPUFREQ_INTERACTIVE_ATTR(midrange_max_boost)
+DECL_CPUFREQ_INTERACTIVE_ATTR(sustain_load)
+DECL_CPUFREQ_INTERACTIVE_ATTR(min_sample_time)
+DECL_CPUFREQ_INTERACTIVE_ATTR(timer_rate)
+DECL_CPUFREQ_INTERACTIVE_ATTR(high_freq_min_delay)
+DECL_CPUFREQ_INTERACTIVE_ATTR(max_normal_freq)
+
+#undef DECL_CPUFREQ_INTERACTIVE_ATTR
+
+static struct attribute *interactive_attributes[] = {
+	&go_maxspeed_load_attr.attr,
+	&midrange_freq_attr.attr,
+	&midrange_go_maxspeed_load_attr.attr,
+	&boost_factor_attr.attr,
+	&max_boost_attr.attr,
+	&midrange_max_boost_attr.attr,
+	&io_is_busy_attr.attr,
+	&sustain_load_attr.attr,
+	&min_sample_time_attr.attr,
+	&timer_rate_attr.attr,
+	&high_freq_min_delay_attr.attr,
+	&max_normal_freq_attr.attr,
+	NULL,
+};
+
+static struct attribute_group interactive_attr_group = {
+	.attrs = interactive_attributes,
+	.name = "interactive",
+};
+
+static int cpufreq_governor_interactive(struct cpufreq_policy *policy,
+		unsigned int event)
+{
+	int rc;
+	unsigned int j;
+	struct cpufreq_interactive_cpuinfo *pcpu;
+	struct cpufreq_frequency_table *freq_table;
+
+	switch (event) {
+	case CPUFREQ_GOV_START:
+		if (!cpu_online(policy->cpu))
+			return -EINVAL;
+
+		freq_table =
+			cpufreq_frequency_get_table(policy->cpu);
+
+		for_each_cpu(j, policy->cpus) {
+			pcpu = &per_cpu(cpuinfo, j);
+			pcpu->policy = policy;
+			pcpu->target_freq = policy->cur;
+			pcpu->freq_table = freq_table;
+			pcpu->freq_change_time_in_idle =
+				get_cpu_idle_time_us(j,
+					     &pcpu->freq_change_time);
+			pcpu->time_in_idle = pcpu->freq_change_time_in_idle;
+			pcpu->idle_exit_time = pcpu->freq_change_time;
+			pcpu->freq_change_time_in_iowait =
+				get_cpu_iowait_time(j, NULL);
+			pcpu->time_in_iowait = pcpu->freq_change_time_in_iowait;
+			if (!pcpu->last_high_freq_time)
+				pcpu->last_high_freq_time = pcpu->freq_change_time;
+			pcpu->timer_idlecancel = 1;
+			pcpu->governor_enabled = 1;
+			smp_wmb();
+
+			if (!timer_pending(&pcpu->cpu_timer))
+				mod_timer(&pcpu->cpu_timer, jiffies + 2);
+		}
+
+		mutex_lock(&gov_state_lock);
+		active_count++;
+		/*
+		 * Do not register the idle hook and create sysfs
+		 * entries if we have already done so.
+		 */
+		if (active_count == 1) {
+			rc = sysfs_create_group(cpufreq_global_kobject,
+					&interactive_attr_group);
+			interactive_kobj = kobject_create_and_add(
+						"gov_interactive",
+						cpufreq_global_kobject);
+			kobject_uevent(interactive_kobj, KOBJ_ADD);
+			if (rc) {
+				mutex_unlock(&gov_state_lock);
+				return rc;
+			}
+		}
+		mutex_unlock(&gov_state_lock);
+
+		break;
+
+	case CPUFREQ_GOV_STOP:
+		for_each_cpu(j, policy->cpus) {
+			pcpu = &per_cpu(cpuinfo, j);
+			pcpu->governor_enabled = 0;
+			smp_wmb();
+			del_timer_sync(&pcpu->cpu_timer);
+
+			/*
+			 * Reset idle exit time since we may cancel the timer
+			 * before it can run after the last idle exit time,
+			 * to avoid tripping the check in idle exit for a timer
+			 * that is trying to run.
+			 */
+			pcpu->idle_exit_time = 0;
+		}
+
+		flush_work(&freq_scale_down_work);
+		mutex_lock(&gov_state_lock);
+
+		active_count--;
+
+		if (active_count == 0) {
+			sysfs_remove_group(cpufreq_global_kobject,
+					&interactive_attr_group);
+			kobject_uevent(interactive_kobj, KOBJ_REMOVE);
+			kobject_put(interactive_kobj);
+		}
+
+		mutex_unlock(&gov_state_lock);
+
+		break;
+
+	case CPUFREQ_GOV_LIMITS:
+		if (policy->max < policy->cur)
+			__cpufreq_driver_target(policy,
+					policy->max, CPUFREQ_RELATION_H);
+		else if (policy->min > policy->cur)
+			__cpufreq_driver_target(policy,
+					policy->min, CPUFREQ_RELATION_L);
+		break;
+	}
+	return 0;
+}
+
+static int cpufreq_interactive_idle_notifier(struct notifier_block *nb,
+					     unsigned long val,
+					     void *data)
+{
+	switch (val) {
+	case IDLE_START:
+		cpufreq_interactive_idle_start();
+		break;
+	case IDLE_END:
+		cpufreq_interactive_idle_end();
+		break;
+	}
+
+	return 0;
+}
+
+static struct notifier_block cpufreq_interactive_idle_nb = {
+	.notifier_call = cpufreq_interactive_idle_notifier,
+};
+
+static int __init cpufreq_interactive_init(void)
+{
+	unsigned int i;
+	struct cpufreq_interactive_cpuinfo *pcpu;
+	struct sched_param param = { .sched_priority = MAX_RT_PRIO-1 };
+
+	go_maxspeed_load = DEFAULT_GO_MAXSPEED_LOAD;
+	midrange_go_maxspeed_load = DEFAULT_MID_RANGE_GO_MAXSPEED_LOAD;
+	min_sample_time = DEFAULT_MIN_SAMPLE_TIME;
+	timer_rate = DEFAULT_TIMER_RATE;
+	high_freq_min_delay = DEFAULT_HIGH_FREQ_MIN_DELAY;
+	max_normal_freq = DEFAULT_MAX_NORMAL_FREQ;
+
+	/* Initalize per-cpu timers */
+	for_each_possible_cpu(i) {
+		pcpu = &per_cpu(cpuinfo, i);
+		init_timer(&pcpu->cpu_timer);
+		pcpu->cpu_timer.function = cpufreq_interactive_timer;
+		pcpu->cpu_timer.data = i;
+	}
+
+	up_task = kthread_create(cpufreq_interactive_up_task, NULL,
+				 "kinteractiveup");
+	if (IS_ERR(up_task))
+		return PTR_ERR(up_task);
+
+	sched_setscheduler_nocheck(up_task, SCHED_FIFO, &param);
+	get_task_struct(up_task);
+
+	/* No rescuer thread, bind to CPU queuing the work for possibly
+	   warm cache (probably doesn't matter much). */
+	down_wq = alloc_workqueue("knteractive_down", 0, 1);
+
+	if (!down_wq)
+		goto err_freeuptask;
+
+	INIT_WORK(&freq_scale_down_work,
+		  cpufreq_interactive_freq_down);
+
+	spin_lock_init(&up_cpumask_lock);
+	spin_lock_init(&down_cpumask_lock);
+	mutex_init(&set_speed_lock);
+	mutex_init(&gov_state_lock);
+
+	idle_notifier_register(&cpufreq_interactive_idle_nb);
+
+	return cpufreq_register_governor(&cpufreq_gov_interactive);
+
+err_freeuptask:
+	put_task_struct(up_task);
+	return -ENOMEM;
+}
+
+#ifdef CONFIG_CPU_FREQ_DEFAULT_GOV_INTERACTIVE
+fs_initcall(cpufreq_interactive_init);
+#else
+module_init(cpufreq_interactive_init);
+#endif
+
+static void __exit cpufreq_interactive_exit(void)
+{
+	cpufreq_unregister_governor(&cpufreq_gov_interactive);
+	kthread_stop(up_task);
+	put_task_struct(up_task);
+	destroy_workqueue(down_wq);
+}
+
+module_exit(cpufreq_interactive_exit);
+
+MODULE_AUTHOR("Mike Chan <mike@android.com>");
+MODULE_DESCRIPTION("'cpufreq_interactive' - A cpufreq governor for "
+	"Latency sensitive workloads");
+MODULE_LICENSE("GPL");
diff --git a/drivers/cpufreq/cpufreq_ondemand.c b/drivers/cpufreq/cpufreq_ondemand.c
index 891360edecdd..a9442a349bbd 100644
--- a/drivers/cpufreq/cpufreq_ondemand.c
+++ b/drivers/cpufreq/cpufreq_ondemand.c
@@ -690,11 +690,10 @@ static int cpufreq_governor_dbs(struct cpufreq_policy *policy,
 		mutex_lock(&dbs_mutex);
 		mutex_destroy(&this_dbs_info->timer_mutex);
 		dbs_enable--;
-		mutex_unlock(&dbs_mutex);
 		if (!dbs_enable)
 			sysfs_remove_group(cpufreq_global_kobject,
 					   &dbs_attr_group);
-
+		mutex_unlock(&dbs_mutex);
 		break;
 
 	case CPUFREQ_GOV_LIMITS:
diff --git a/drivers/cpufreq/cpufreq_stats.c b/drivers/cpufreq/cpufreq_stats.c
index faf7c5217848..d0344284a7d8 100644
--- a/drivers/cpufreq/cpufreq_stats.c
+++ b/drivers/cpufreq/cpufreq_stats.c
@@ -59,7 +59,12 @@ static int cpufreq_stats_update(unsigned int cpu)
 	cur_time = get_jiffies_64();
 	spin_lock(&cpufreq_stats_lock);
 	stat = per_cpu(cpufreq_stats_table, cpu);
-	if (stat->time_in_state)
+	if (!stat) {
+		spin_unlock(&cpufreq_stats_lock);
+		return 0;
+	}
+
+	if (stat->time_in_state && stat->last_index >= 0)
 		stat->time_in_state[stat->last_index] =
 			cputime64_add(stat->time_in_state[stat->last_index],
 				      cputime_sub(cur_time, stat->last_time));
@@ -159,10 +164,10 @@ static struct attribute_group stats_attr_group = {
 static int freq_table_get_index(struct cpufreq_stats *stat, unsigned int freq)
 {
 	int index;
-	for (index = 0; index < stat->max_state; index++)
-		if (stat->freq_table[index] == freq)
-			return index;
-	return -1;
+	for (index = 0; index < stat->state_num; index++)
+		if (stat->freq_table[index] > freq)
+			break;
+	return index - 1; /* below lowest freq in table: return -1 */
 }
 
 /* should be called late in the CPU removal sequence so that the stats
@@ -170,12 +175,17 @@ static int freq_table_get_index(struct cpufreq_stats *stat, unsigned int freq)
  */
 static void cpufreq_stats_free_table(unsigned int cpu)
 {
-	struct cpufreq_stats *stat = per_cpu(cpufreq_stats_table, cpu);
+	struct cpufreq_stats *stat;
+
+	spin_lock(&cpufreq_stats_lock);
+	stat = per_cpu(cpufreq_stats_table, cpu);
+	per_cpu(cpufreq_stats_table, cpu) = NULL;
+	spin_unlock(&cpufreq_stats_lock);
+
 	if (stat) {
 		kfree(stat->time_in_state);
 		kfree(stat);
 	}
-	per_cpu(cpufreq_stats_table, cpu) = NULL;
 }
 
 /* must be called early in the CPU removal sequence (before
@@ -193,7 +203,7 @@ static void cpufreq_stats_free_sysfs(unsigned int cpu)
 static int cpufreq_stats_create_table(struct cpufreq_policy *policy,
 		struct cpufreq_frequency_table *table)
 {
-	unsigned int i, j, count = 0, ret = 0;
+	unsigned int i, j, k, l, count = 0, ret = 0;
 	struct cpufreq_stats *stat;
 	struct cpufreq_policy *data;
 	unsigned int alloc_size;
@@ -245,8 +255,16 @@ static int cpufreq_stats_create_table(struct cpufreq_policy *policy,
 		unsigned int freq = table[i].frequency;
 		if (freq == CPUFREQ_ENTRY_INVALID)
 			continue;
-		if (freq_table_get_index(stat, freq) == -1)
-			stat->freq_table[j++] = freq;
+
+		/* Insert in sorted stat->freq_table */
+		for (k = 0; k < j && stat->freq_table[k] < freq; k++)
+			;
+		if (stat->freq_table[k] == freq)
+			continue;
+		for (l = j; l > k; l--)
+			stat->freq_table[l] = stat->freq_table[l - 1];
+		stat->freq_table[k] = freq;
+		j++;
 	}
 	stat->state_num = j;
 	spin_lock(&cpufreq_stats_lock);
@@ -291,32 +309,54 @@ static int cpufreq_stat_notifier_trans(struct notifier_block *nb,
 	if (val != CPUFREQ_POSTCHANGE)
 		return 0;
 
+	cpufreq_stats_update(freq->cpu);
+
+	spin_lock(&cpufreq_stats_lock);
 	stat = per_cpu(cpufreq_stats_table, freq->cpu);
-	if (!stat)
+	if (!stat) {
+		spin_unlock(&cpufreq_stats_lock);
 		return 0;
+	}
 
 	old_index = stat->last_index;
 	new_index = freq_table_get_index(stat, freq->new);
 
-	/* We can't do stat->time_in_state[-1]= .. */
-	if (old_index == -1 || new_index == -1)
-		return 0;
-
-	cpufreq_stats_update(freq->cpu);
-
-	if (old_index == new_index)
+	if (old_index == new_index) {
+		spin_unlock(&cpufreq_stats_lock);
 		return 0;
+	}
 
-	spin_lock(&cpufreq_stats_lock);
 	stat->last_index = new_index;
 #ifdef CONFIG_CPU_FREQ_STAT_DETAILS
-	stat->trans_table[old_index * stat->max_state + new_index]++;
+	if (old_index >= 0 && new_index >= 0)
+		stat->trans_table[old_index * stat->max_state + new_index]++;
 #endif
 	stat->total_trans++;
 	spin_unlock(&cpufreq_stats_lock);
 	return 0;
 }
 
+static int cpufreq_stats_create_table_cpu(unsigned int cpu)
+{
+	struct cpufreq_policy *policy;
+	struct cpufreq_frequency_table *table;
+	int ret = -ENODEV;
+
+	policy = cpufreq_cpu_get(cpu);
+	if (!policy)
+		return -ENODEV;
+
+	table = cpufreq_frequency_get_table(cpu);
+	if (!table)
+		goto out;
+
+	ret = cpufreq_stats_create_table(policy, table);
+
+out:
+	cpufreq_cpu_put(policy);
+	return ret;
+}
+
 static int __cpuinit cpufreq_stat_cpu_callback(struct notifier_block *nfb,
 					       unsigned long action,
 					       void *hcpu)
@@ -329,12 +369,17 @@ static int __cpuinit cpufreq_stat_cpu_callback(struct notifier_block *nfb,
 		cpufreq_update_policy(cpu);
 		break;
 	case CPU_DOWN_PREPARE:
+	case CPU_DOWN_PREPARE_FROZEN:
 		cpufreq_stats_free_sysfs(cpu);
 		break;
 	case CPU_DEAD:
 	case CPU_DEAD_FROZEN:
 		cpufreq_stats_free_table(cpu);
 		break;
+	case CPU_DOWN_FAILED:
+	case CPU_DOWN_FAILED_FROZEN:
+		cpufreq_stats_create_table_cpu(cpu);
+		break;
 	}
 	return NOTIFY_OK;
 }
diff --git a/drivers/cpuidle/governors/menu.c b/drivers/cpuidle/governors/menu.c
index c47f3d09c1ee..e2f7271915dc 100644
--- a/drivers/cpuidle/governors/menu.c
+++ b/drivers/cpuidle/governors/menu.c
@@ -172,7 +172,12 @@ static inline int performance_multiplier(void)
 
 	/* for higher loadavg, we are more reluctant */
 
-	mult += 2 * get_loadavg();
+	/*
+	 * this doesn't work as intended - it is almost always 0, but can
+	 * sometimes, depending on workload, spike very high into the hundreds
+	 * even when the average cpu load is under 10%.
+	 */
+	/* mult += 2 * get_loadavg(); */
 
 	/* for IO wait tasks (per cpu!) we add 5x each */
 	mult += 10 * nr_iowait_cpu(smp_processor_id());
diff --git a/drivers/cpuquiet/Kconfig b/drivers/cpuquiet/Kconfig
new file mode 100644
index 000000000000..844cd34a69b3
--- /dev/null
+++ b/drivers/cpuquiet/Kconfig
@@ -0,0 +1,11 @@
+menu "CPUQUIET Framework"
+
+config CPUQUIET_FRAMEWORK
+	bool "Cpuquiet framework"
+	default n
+	help
+	  Cpuquiet implements pluggable policies for forcing cpu cores into a
+	  quiescent state. Appropriate policies will save power without hurting
+	  performance.
+
+endmenu
diff --git a/drivers/cpuquiet/Makefile b/drivers/cpuquiet/Makefile
new file mode 100644
index 000000000000..e438defaacdd
--- /dev/null
+++ b/drivers/cpuquiet/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_CPUQUIET_FRAMEWORK) += cpuquiet.o driver.o sysfs.o cpuquiet_attribute.o governor.o governors/
diff --git a/drivers/cpuquiet/cpuquiet.c b/drivers/cpuquiet/cpuquiet.c
new file mode 100644
index 000000000000..d902af26c8d7
--- /dev/null
+++ b/drivers/cpuquiet/cpuquiet.c
@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2012 NVIDIA 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; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/cpu.h>
+#include <linux/slab.h>
+#include <linux/cpuquiet.h>
+#include "cpuquiet.h"
+
+DEFINE_MUTEX(cpuquiet_lock);
+
+static int __init cpuquiet_init(void)
+{
+	return cpuquiet_add_class_sysfs(&cpu_sysdev_class);
+}
+
+core_initcall(cpuquiet_init);
diff --git a/drivers/cpuquiet/cpuquiet.h b/drivers/cpuquiet/cpuquiet.h
new file mode 100644
index 000000000000..fa61946ff119
--- /dev/null
+++ b/drivers/cpuquiet/cpuquiet.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2012 NVIDIA 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; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ */
+
+#ifndef __DRIVER_CPUQUIET_H
+#define __DRIVER_CPUQUIET_H
+
+#include <linux/sysdev.h>
+
+extern struct mutex cpuquiet_lock;
+extern struct cpuquiet_governor *cpuquiet_curr_governor;
+extern struct list_head cpuquiet_governors;
+int cpuquiet_add_class_sysfs(struct sysdev_class *cls);
+struct cpuquiet_governor *cpuquiet_find_governor(const char *str);
+int cpuquiet_switch_governor(struct cpuquiet_governor *gov);
+struct cpuquiet_governor *cpuquiet_get_first_governor(void);
+struct cpuquiet_driver *cpuquiet_get_driver(void);
+void cpuquiet_add_dev(struct sys_device *sys_dev, unsigned int cpu);
+void cpuquiet_remove_dev(unsigned int cpu);
+int cpuquiet_cpu_kobject_init(struct kobject *kobj, struct kobj_type *type,
+				char *name, int cpu);
+#endif
diff --git a/drivers/cpuquiet/cpuquiet_attribute.c b/drivers/cpuquiet/cpuquiet_attribute.c
new file mode 100644
index 000000000000..9f1aa430149d
--- /dev/null
+++ b/drivers/cpuquiet/cpuquiet_attribute.c
@@ -0,0 +1,133 @@
+/*
+ * Copyright (c) 2012 NVIDIA 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; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/cpuquiet.h>
+
+ssize_t show_int_attribute(struct cpuquiet_attribute *cattr, char *buf)
+{
+	return sprintf(buf, "%d\n", *((int *)cattr->param));
+}
+
+ssize_t store_int_attribute(struct cpuquiet_attribute *cattr,
+					const char *buf, size_t count)
+{
+	int err, val;
+
+	err = kstrtoint(buf, 0, &val);
+	if (err < 0)
+		return err;
+
+	*((int *)(cattr->param)) = val;
+
+	if (cattr->store_callback)
+		cattr->store_callback(cattr);
+
+	return count;
+}
+
+ssize_t show_bool_attribute(struct cpuquiet_attribute *cattr, char *buf)
+{
+	return sprintf(buf, "%d\n", *((bool *)cattr->param));
+}
+
+ssize_t store_bool_attribute(struct cpuquiet_attribute *cattr,
+					const char *buf, size_t count)
+{
+	int err, val;
+
+	err = kstrtoint(buf, 0, &val);
+	if (err < 0)
+		return err;
+
+	if (val < 0 || val > 1)
+		return -EINVAL;
+
+	*((bool *)(cattr->param)) = val;
+
+	if (cattr->store_callback)
+		cattr->store_callback(cattr);
+
+	return count;
+}
+
+ssize_t show_uint_attribute(struct cpuquiet_attribute *cattr, char *buf)
+{
+	return sprintf(buf, "%u\n", *((unsigned int *)cattr->param));
+}
+
+ssize_t store_uint_attribute(struct cpuquiet_attribute *cattr,
+					const char *buf, size_t count)
+{
+	int err;
+	unsigned int val;
+
+	err = kstrtouint(buf, 0, &val);
+	if (err < 0)
+		return err;
+
+	*((unsigned int *)(cattr->param)) = val;
+
+	if (cattr->store_callback)
+		cattr->store_callback(cattr);
+
+	return count;
+}
+
+ssize_t store_ulong_attribute(struct cpuquiet_attribute *cattr,
+					const char *buf, size_t count)
+{
+	int err;
+	unsigned long val;
+
+	err = kstrtoul(buf, 0, &val);
+	if (err < 0)
+		return err;
+
+	*((unsigned long *)(cattr->param)) = val;
+
+	if (cattr->store_callback)
+		cattr->store_callback(cattr);
+
+	return count;
+}
+
+ssize_t show_ulong_attribute(struct cpuquiet_attribute *cattr,
+					char *buf)
+{
+	return sprintf(buf, "%lu\n", *((unsigned long *)cattr->param));
+}
+
+ssize_t cpuquiet_auto_sysfs_store(struct kobject *kobj,
+	struct attribute *attr, const char *buf, size_t count)
+{
+	struct cpuquiet_attribute *cattr =
+		container_of(attr, struct cpuquiet_attribute, attr);
+
+	if (cattr->store)
+		return cattr->store(cattr, buf, count);
+
+	return -EINVAL;
+}
+
+ssize_t cpuquiet_auto_sysfs_show(struct kobject *kobj,
+		struct attribute *attr, char *buf)
+{
+	struct cpuquiet_attribute *cattr =
+		container_of(attr, struct cpuquiet_attribute, attr);
+
+	return cattr->show(cattr, buf);
+}
diff --git a/drivers/cpuquiet/driver.c b/drivers/cpuquiet/driver.c
new file mode 100644
index 000000000000..d9dbea76994a
--- /dev/null
+++ b/drivers/cpuquiet/driver.c
@@ -0,0 +1,202 @@
+/*
+ * Copyright (c) 2012 NVIDIA 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; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ */
+
+#include <linux/mutex.h>
+#include <linux/module.h>
+#include <linux/cpuquiet.h>
+#include <linux/cpu.h>
+#include <linux/jiffies.h>
+#include <linux/slab.h>
+#include <asm/cputime.h>
+
+#include "cpuquiet.h"
+
+struct cpuquiet_cpu_stat {
+	cputime64_t time_up_total;
+	u64 last_update;
+	unsigned int up_down_count;
+	struct kobject cpu_kobject;
+};
+
+struct cpu_attribute {
+	struct attribute attr;
+	enum { up_down_count, time_up_total } type;
+};
+
+static struct cpuquiet_driver *cpuquiet_curr_driver;
+struct cpuquiet_cpu_stat *stats;
+
+#define CPU_ATTRIBUTE(_name) \
+	static struct cpu_attribute _name ## _attr = {			\
+		.attr =  {.name = __stringify(_name), .mode = 0444 },	\
+		.type	= _name,					\
+}
+
+CPU_ATTRIBUTE(up_down_count);
+CPU_ATTRIBUTE(time_up_total);
+
+static struct attribute *cpu_attributes[] = {
+	&up_down_count_attr.attr,
+	&time_up_total_attr.attr,
+	NULL,
+};
+
+static void stats_update(struct cpuquiet_cpu_stat *stat, bool up)
+{
+	u64 cur_jiffies = get_jiffies_64();
+	bool was_up = stat->up_down_count & 0x1;
+
+	if (was_up)
+		stat->time_up_total = cputime64_add(stat->time_up_total,
+			cputime64_sub(cur_jiffies, stat->last_update));
+
+	if (was_up != up)
+		stat->up_down_count++;
+
+	stat->last_update = cur_jiffies;
+}
+
+int cpuquiet_quiesence_cpu(unsigned int cpunumber)
+{
+	int err = -EPERM;
+
+	if (cpuquiet_curr_driver && cpuquiet_curr_driver->quiesence_cpu)
+		err = cpuquiet_curr_driver->quiesence_cpu(cpunumber);
+
+	if (!err)
+		stats_update(stats + cpunumber, 0);
+
+	return err;
+}
+EXPORT_SYMBOL(cpuquiet_quiesence_cpu);
+
+int cpuquiet_wake_cpu(unsigned int cpunumber)
+{
+	int err = -EPERM;
+
+	if (cpuquiet_curr_driver && cpuquiet_curr_driver->wake_cpu)
+		err = cpuquiet_curr_driver->wake_cpu(cpunumber);
+
+	if (!err)
+		stats_update(stats + cpunumber, 1);
+
+	return err;
+}
+EXPORT_SYMBOL(cpuquiet_wake_cpu);
+
+static ssize_t stats_sysfs_show(struct kobject *kobj,
+			struct attribute *attr, char *buf)
+{
+	struct cpu_attribute *cattr =
+		container_of(attr, struct cpu_attribute, attr);
+	struct cpuquiet_cpu_stat *stat  =
+		container_of(kobj, struct cpuquiet_cpu_stat, cpu_kobject);
+	ssize_t len = 0;
+	bool was_up = stat->up_down_count & 0x1;
+
+	stats_update(stat, was_up);
+
+	switch (cattr->type) {
+	case up_down_count:
+		len = sprintf(buf, "%u\n", stat->up_down_count);
+		break;
+	case time_up_total:
+		len =  sprintf(buf, "%llu\n", stat->time_up_total);
+		break;
+	}
+
+	return len;
+}
+
+static const struct sysfs_ops stats_sysfs_ops = {
+	.show = stats_sysfs_show,
+};
+
+static struct kobj_type ktype_cpu_stats = {
+	.sysfs_ops = &stats_sysfs_ops,
+	.default_attrs = cpu_attributes,
+};
+
+int cpuquiet_register_driver(struct cpuquiet_driver *drv)
+{
+	int err = -EBUSY;
+	unsigned int cpu;
+	struct sys_device *sys_dev;
+	u64 cur_jiffies;
+
+	if (!drv)
+		return -EINVAL;
+
+	stats = kzalloc(nr_cpu_ids * sizeof(*stats), GFP_KERNEL);
+	if (!stats)
+		return -ENOMEM;
+
+	for_each_possible_cpu(cpu) {
+		cur_jiffies = get_jiffies_64();
+		stats[cpu].last_update = cur_jiffies;
+		if (cpu_online(cpu))
+			stats[cpu].up_down_count = 1;
+		sys_dev = get_cpu_sysdev(cpu);
+		if (sys_dev) {
+			cpuquiet_add_dev(sys_dev, cpu);
+			cpuquiet_cpu_kobject_init(&stats[cpu].cpu_kobject,
+					&ktype_cpu_stats, "stats", cpu);
+		}
+	}
+
+	mutex_lock(&cpuquiet_lock);
+	if (!cpuquiet_curr_driver) {
+		err = 0;
+		cpuquiet_curr_driver = drv;
+		cpuquiet_switch_governor(cpuquiet_get_first_governor());
+	}
+	mutex_unlock(&cpuquiet_lock);
+
+	return err;
+}
+EXPORT_SYMBOL(cpuquiet_register_driver);
+
+struct cpuquiet_driver *cpuquiet_get_driver(void)
+{
+	return cpuquiet_curr_driver;
+}
+
+void cpuquiet_unregister_driver(struct cpuquiet_driver *drv)
+{
+	unsigned int cpu;
+
+	if (drv != cpuquiet_curr_driver) {
+		WARN(1, "invalid cpuquiet_unregister_driver(%s)\n",
+			drv->name);
+		return;
+	}
+
+	/* stop current governor first */
+	cpuquiet_switch_governor(NULL);
+
+	mutex_lock(&cpuquiet_lock);
+	cpuquiet_curr_driver = NULL;
+
+	for_each_possible_cpu(cpu) {
+		kobject_put(&stats[cpu].cpu_kobject);
+		cpuquiet_remove_dev(cpu);
+	}
+
+	mutex_unlock(&cpuquiet_lock);
+}
+EXPORT_SYMBOL(cpuquiet_unregister_driver);
diff --git a/drivers/cpuquiet/governor.c b/drivers/cpuquiet/governor.c
new file mode 100644
index 000000000000..176ba3bd705f
--- /dev/null
+++ b/drivers/cpuquiet/governor.c
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2012 NVIDIA 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; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ */
+
+#include <linux/mutex.h>
+#include <linux/module.h>
+#include <linux/cpuquiet.h>
+
+#include "cpuquiet.h"
+
+LIST_HEAD(cpuquiet_governors);
+struct cpuquiet_governor *cpuquiet_curr_governor;
+
+struct cpuquiet_governor *cpuquiet_get_first_governor(void)
+{
+	if (!list_empty(&cpuquiet_governors))
+		return list_entry(cpuquiet_governors.next,
+					struct cpuquiet_governor,
+					governor_list);
+	else
+		return NULL;
+}
+
+struct cpuquiet_governor *cpuquiet_find_governor(const char *str)
+{
+	struct cpuquiet_governor *gov;
+
+	list_for_each_entry(gov, &cpuquiet_governors, governor_list)
+		if (!strnicmp(str, gov->name, CPUQUIET_NAME_LEN))
+			return gov;
+
+	return NULL;
+}
+
+int cpuquiet_switch_governor(struct cpuquiet_governor *gov)
+{
+	int err = 0;
+
+	if (cpuquiet_curr_governor) {
+		if (cpuquiet_curr_governor->stop)
+			cpuquiet_curr_governor->stop();
+		module_put(cpuquiet_curr_governor->owner);
+	}
+
+	cpuquiet_curr_governor = gov;
+
+	if (gov) {
+		if (!try_module_get(cpuquiet_curr_governor->owner))
+			return -EINVAL;
+		if (gov->start)
+			err = gov->start();
+		if (!err)
+			cpuquiet_curr_governor = gov;
+	}
+
+	return err;
+}
+
+int cpuquiet_register_governor(struct cpuquiet_governor *gov)
+{
+	int ret = -EEXIST;
+
+	if (!gov)
+		return -EINVAL;
+
+	mutex_lock(&cpuquiet_lock);
+	if (cpuquiet_find_governor(gov->name) == NULL) {
+		ret = 0;
+		list_add_tail(&gov->governor_list, &cpuquiet_governors);
+		if (!cpuquiet_curr_governor && cpuquiet_get_driver())
+			cpuquiet_switch_governor(gov);
+	}
+	mutex_unlock(&cpuquiet_lock);
+
+	return ret;
+}
+
+void cpuquiet_unregister_governor(struct cpuquiet_governor *gov)
+{
+	if (!gov)
+		return;
+
+	mutex_lock(&cpuquiet_lock);
+	if (cpuquiet_curr_governor == gov)
+		cpuquiet_switch_governor(NULL);
+	list_del(&gov->governor_list);
+	mutex_unlock(&cpuquiet_lock);
+}
+
+void cpuquiet_device_busy(void)
+{
+	if (cpuquiet_curr_governor &&
+			cpuquiet_curr_governor->device_busy_notification)
+		cpuquiet_curr_governor->device_busy_notification();
+}
+
+void cpuquiet_device_free(void)
+{
+	if (cpuquiet_curr_governor &&
+			cpuquiet_curr_governor->device_free_notification)
+		cpuquiet_curr_governor->device_free_notification();
+}
diff --git a/drivers/cpuquiet/governors/Makefile b/drivers/cpuquiet/governors/Makefile
new file mode 100644
index 000000000000..c70803127082
--- /dev/null
+++ b/drivers/cpuquiet/governors/Makefile
@@ -0,0 +1 @@
+obj-y += userspace.o balanced.o
diff --git a/drivers/cpuquiet/governors/balanced.c b/drivers/cpuquiet/governors/balanced.c
new file mode 100644
index 000000000000..f0d2e03ae22b
--- /dev/null
+++ b/drivers/cpuquiet/governors/balanced.c
@@ -0,0 +1,461 @@
+/*
+ * Copyright (c) 2012 NVIDIA 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; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/cpuquiet.h>
+#include <linux/cpumask.h>
+#include <linux/module.h>
+#include <linux/cpufreq.h>
+#include <linux/pm_qos_params.h>
+#include <linux/jiffies.h>
+#include <linux/slab.h>
+#include <linux/cpu.h>
+#include <linux/sched.h>
+#include <linux/tick.h>
+#include <asm/cputime.h>
+
+#define CPUNAMELEN 8
+
+typedef enum {
+	CPU_SPEED_BALANCED,
+	CPU_SPEED_BIASED,
+	CPU_SPEED_SKEWED,
+} CPU_SPEED_BALANCE;
+
+typedef enum {
+	IDLE,
+	DOWN,
+	UP,
+} BALANCED_STATE;
+
+struct idle_info {
+	u64 idle_last;
+	u64 last_timestamp;
+	u64 idle_current;
+	u64 timestamp;
+};
+
+static DEFINE_PER_CPU(struct idle_info, idleinfo);
+static DEFINE_PER_CPU(unsigned int, cpu_load);
+
+static struct timer_list load_timer;
+static bool load_timer_active;
+
+/* configurable parameters */
+static unsigned int  balance_level = 60;
+static unsigned int  idle_bottom_freq;
+static unsigned int  idle_top_freq;
+static unsigned long up_delay;
+static unsigned long down_delay;
+static unsigned long last_change_time;
+static unsigned int  load_sample_rate = 20; /* msec */
+static struct workqueue_struct *balanced_wq;
+static struct delayed_work balanced_work;
+static BALANCED_STATE balanced_state;
+static struct kobject *balanced_kobject;
+
+static void calculate_load_timer(unsigned long data)
+{
+	int i;
+	u64 idle_time, elapsed_time;
+
+	if (!load_timer_active)
+		return;
+
+	for_each_online_cpu(i) {
+		struct idle_info *iinfo = &per_cpu(idleinfo, i);
+		unsigned int *load = &per_cpu(cpu_load, i);
+
+		iinfo->idle_last = iinfo->idle_current;
+		iinfo->last_timestamp = iinfo->timestamp;
+		iinfo->idle_current =
+			get_cpu_idle_time_us(i, &iinfo->timestamp);
+		elapsed_time = iinfo->timestamp - iinfo->last_timestamp;
+
+		idle_time = iinfo->idle_current - iinfo->idle_last;
+		idle_time *= 100;
+		do_div(idle_time, elapsed_time);
+		*load = 100 - idle_time;
+	}
+	mod_timer(&load_timer, jiffies + msecs_to_jiffies(load_sample_rate));
+}
+
+static void start_load_timer(void)
+{
+	int i;
+
+	if (load_timer_active)
+		return;
+
+	load_timer_active = true;
+
+	for_each_online_cpu(i) {
+		struct idle_info *iinfo = &per_cpu(idleinfo, i);
+
+		iinfo->idle_current =
+			get_cpu_idle_time_us(i, &iinfo->timestamp);
+	}
+	mod_timer(&load_timer, jiffies + msecs_to_jiffies(100));
+}
+
+static void stop_load_timer(void)
+{
+	if (!load_timer_active)
+		return;
+
+	load_timer_active = false;
+	del_timer(&load_timer);
+}
+
+static unsigned int get_slowest_cpu_n(void)
+{
+	unsigned int cpu = nr_cpu_ids;
+	unsigned long minload = ULONG_MAX;
+	int i;
+
+	for_each_online_cpu(i) {
+		unsigned int *load = &per_cpu(cpu_load, i);
+
+		if ((i > 0) && (minload > *load)) {
+			cpu = i;
+			minload = *load;
+		}
+	}
+
+	return cpu;
+}
+
+static unsigned int cpu_highest_speed(void)
+{
+	unsigned int maxload = 0;
+	int i;
+
+	for_each_online_cpu(i) {
+		unsigned int *load = &per_cpu(cpu_load, i);
+
+		maxload = max(maxload, *load);
+	}
+
+	return maxload;
+}
+
+static unsigned int count_slow_cpus(unsigned int limit)
+{
+	unsigned int cnt = 0;
+	int i;
+
+	for_each_online_cpu(i) {
+		unsigned int *load = &per_cpu(cpu_load, i);
+
+		if (*load <= limit)
+			cnt++;
+	}
+
+	return cnt;
+}
+
+#define NR_FSHIFT	2
+static unsigned int nr_run_thresholds[] = {
+/*      1,  2,  3,  4 - on-line cpus target */
+	5,  9, 10, UINT_MAX /* avg run threads * 4 (e.g., 9 = 2.25 threads) */
+};
+static unsigned int nr_run_hysteresis = 2;	/* 0.5 thread */
+static unsigned int nr_run_last;
+
+static CPU_SPEED_BALANCE balanced_speed_balance(void)
+{
+	unsigned long highest_speed = cpu_highest_speed();
+	unsigned long balanced_speed = highest_speed * balance_level / 100;
+	unsigned long skewed_speed = balanced_speed / 2;
+	unsigned int nr_cpus = num_online_cpus();
+	unsigned int max_cpus = pm_qos_request(PM_QOS_MAX_ONLINE_CPUS) ? : 4;
+	unsigned int avg_nr_run = avg_nr_running();
+	unsigned int nr_run;
+
+	/* balanced: freq targets for all CPUs are above 50% of highest speed
+	   biased: freq target for at least one CPU is below 50% threshold
+	   skewed: freq targets for at least 2 CPUs are below 25% threshold */
+	for (nr_run = 1; nr_run < ARRAY_SIZE(nr_run_thresholds); nr_run++) {
+		unsigned int nr_threshold = nr_run_thresholds[nr_run - 1];
+		if (nr_run_last <= nr_run)
+			nr_threshold += nr_run_hysteresis;
+		if (avg_nr_run <= (nr_threshold << (FSHIFT - NR_FSHIFT)))
+			break;
+	}
+	nr_run_last = nr_run;
+
+	if (count_slow_cpus(skewed_speed) >= 2 || nr_cpus > max_cpus ||
+		nr_run < nr_cpus)
+		return CPU_SPEED_SKEWED;
+
+	if (count_slow_cpus(balanced_speed) >= 1 || nr_cpus == max_cpus ||
+		nr_run <= nr_cpus)
+		return CPU_SPEED_BIASED;
+
+	return CPU_SPEED_BALANCED;
+}
+
+static void balanced_work_func(struct work_struct *work)
+{
+	bool up = false;
+	unsigned int cpu = nr_cpu_ids;
+	unsigned long now = jiffies;
+
+	CPU_SPEED_BALANCE balance;
+
+	switch (balanced_state) {
+	case IDLE:
+		break;
+	case DOWN:
+		cpu = get_slowest_cpu_n();
+		if (cpu < nr_cpu_ids) {
+			up = false;
+			queue_delayed_work(balanced_wq,
+						 &balanced_work, up_delay);
+		} else
+			stop_load_timer();
+		break;
+	case UP:
+		balance = balanced_speed_balance();
+		switch (balance) {
+
+		/* cpu speed is up and balanced - one more on-line */
+		case CPU_SPEED_BALANCED:
+			cpu = cpumask_next_zero(0, cpu_online_mask);
+			if (cpu < nr_cpu_ids)
+				up = true;
+			break;
+		/* cpu speed is up, but skewed - remove one core */
+		case CPU_SPEED_SKEWED:
+			cpu = get_slowest_cpu_n();
+			if (cpu < nr_cpu_ids)
+				up = false;
+			break;
+		/* cpu speed is up, but under-utilized - do nothing */
+		case CPU_SPEED_BIASED:
+		default:
+			break;
+		}
+		queue_delayed_work(
+			balanced_wq, &balanced_work, up_delay);
+		break;
+	default:
+		pr_err("%s: invalid cpuquiet balanced governor state %d\n",
+		       __func__, balanced_state);
+	}
+
+	if (!up && ((now - last_change_time) < down_delay))
+		cpu = nr_cpu_ids;
+
+	if (cpu < nr_cpu_ids) {
+		last_change_time = now;
+		if (up)
+			cpuquiet_wake_cpu(cpu);
+		else
+			cpuquiet_quiesence_cpu(cpu);
+	}
+}
+
+static int balanced_cpufreq_transition(struct notifier_block *nb,
+	unsigned long state, void *data)
+{
+	struct cpufreq_freqs *freqs = data;
+	unsigned long cpu_freq;
+
+	if (state == CPUFREQ_POSTCHANGE || state == CPUFREQ_RESUMECHANGE) {
+		cpu_freq = freqs->new;
+
+		switch (balanced_state) {
+		case IDLE:
+			if (cpu_freq >= idle_top_freq) {
+				balanced_state = UP;
+				queue_delayed_work(
+					balanced_wq, &balanced_work, up_delay);
+				start_load_timer();
+			} else if (cpu_freq <= idle_bottom_freq) {
+				balanced_state = DOWN;
+				queue_delayed_work(
+					balanced_wq, &balanced_work,
+					down_delay);
+				start_load_timer();
+			}
+			break;
+		case DOWN:
+			if (cpu_freq >= idle_top_freq) {
+				balanced_state = UP;
+				queue_delayed_work(
+					balanced_wq, &balanced_work, up_delay);
+				start_load_timer();
+			}
+			break;
+		case UP:
+			if (cpu_freq <= idle_bottom_freq) {
+				balanced_state = DOWN;
+				queue_delayed_work(balanced_wq,
+					&balanced_work, up_delay);
+				start_load_timer();
+			}
+			break;
+		default:
+			pr_err("%s: invalid cpuquiet balanced governor "
+				"state %d\n", __func__, balanced_state);
+		}
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block balanced_cpufreq_nb = {
+	.notifier_call = balanced_cpufreq_transition,
+};
+
+static void delay_callback(struct cpuquiet_attribute *attr)
+{
+	unsigned long val;
+
+	if (attr) {
+		val = (*((unsigned long *)(attr->param)));
+		(*((unsigned long *)(attr->param))) = msecs_to_jiffies(val);
+	}
+}
+
+CPQ_BASIC_ATTRIBUTE(balance_level, 0644, uint);
+CPQ_BASIC_ATTRIBUTE(idle_bottom_freq, 0644, uint);
+CPQ_BASIC_ATTRIBUTE(idle_top_freq, 0644, uint);
+CPQ_BASIC_ATTRIBUTE(load_sample_rate, 0644, uint);
+CPQ_ATTRIBUTE(up_delay, 0644, ulong, delay_callback);
+CPQ_ATTRIBUTE(down_delay, 0644, ulong, delay_callback);
+
+static struct attribute *balanced_attributes[] = {
+	&balance_level_attr.attr,
+	&idle_bottom_freq_attr.attr,
+	&idle_top_freq_attr.attr,
+	&up_delay_attr.attr,
+	&down_delay_attr.attr,
+	&load_sample_rate_attr.attr,
+	NULL,
+};
+
+static const struct sysfs_ops balanced_sysfs_ops = {
+	.show = cpuquiet_auto_sysfs_show,
+	.store = cpuquiet_auto_sysfs_store,
+};
+
+static struct kobj_type ktype_balanced = {
+	.sysfs_ops = &balanced_sysfs_ops,
+	.default_attrs = balanced_attributes,
+};
+
+static int balanced_sysfs(void)
+{
+	int err;
+
+	balanced_kobject = kzalloc(sizeof(*balanced_kobject),
+				GFP_KERNEL);
+
+	if (!balanced_kobject)
+		return -ENOMEM;
+
+	err = cpuquiet_kobject_init(balanced_kobject, &ktype_balanced,
+				"balanced");
+
+	if (err)
+		kfree(balanced_kobject);
+
+	return err;
+}
+
+static void balanced_stop(void)
+{
+	/*
+	   first unregister the notifiers. This ensures the governor state
+	   can't be modified by a cpufreq transition
+	*/
+	cpufreq_unregister_notifier(&balanced_cpufreq_nb,
+		CPUFREQ_TRANSITION_NOTIFIER);
+
+	/* now we can force the governor to be idle */
+	balanced_state = IDLE;
+	cancel_delayed_work_sync(&balanced_work);
+	destroy_workqueue(balanced_wq);
+	del_timer(&load_timer);
+
+	kobject_put(balanced_kobject);
+}
+
+static int balanced_start(void)
+{
+	int err, count;
+	struct cpufreq_frequency_table *table;
+	struct cpufreq_freqs initial_freq;
+
+	err = balanced_sysfs();
+	if (err)
+		return err;
+
+	balanced_wq = alloc_workqueue("cpuquiet-balanced",
+			WQ_UNBOUND | WQ_RESCUER | WQ_FREEZABLE, 1);
+	if (!balanced_wq)
+		return -ENOMEM;
+
+	INIT_DELAYED_WORK(&balanced_work, balanced_work_func);
+
+	up_delay = msecs_to_jiffies(100);
+	down_delay = msecs_to_jiffies(500);
+
+	table = cpufreq_frequency_get_table(0);
+	for (count = 0; table[count].frequency != CPUFREQ_TABLE_END; count++);
+
+	idle_top_freq = table[(count / 2) - 1].frequency;
+	idle_bottom_freq = table[(count / 2) - 2].frequency;
+
+	cpufreq_register_notifier(&balanced_cpufreq_nb,
+		CPUFREQ_TRANSITION_NOTIFIER);
+
+	init_timer(&load_timer);
+	load_timer.function = calculate_load_timer;
+
+	/*FIXME: Kick start the state machine by faking a freq notification*/
+	initial_freq.new = cpufreq_get(0);
+	if (initial_freq.new != 0)
+		balanced_cpufreq_transition(NULL, CPUFREQ_RESUMECHANGE,
+						&initial_freq);
+	return 0;
+}
+
+struct cpuquiet_governor balanced_governor = {
+	.name		= "balanced",
+	.start		= balanced_start,
+	.stop		= balanced_stop,
+	.owner		= THIS_MODULE,
+};
+
+static int __init init_balanced(void)
+{
+	return cpuquiet_register_governor(&balanced_governor);
+}
+
+static void __exit exit_balanced(void)
+{
+	cpuquiet_unregister_governor(&balanced_governor);
+}
+
+MODULE_LICENSE("GPL");
+module_init(init_balanced);
+module_exit(exit_balanced);
+
diff --git a/drivers/cpuquiet/governors/userspace.c b/drivers/cpuquiet/governors/userspace.c
new file mode 100644
index 000000000000..470056c5e32a
--- /dev/null
+++ b/drivers/cpuquiet/governors/userspace.c
@@ -0,0 +1,56 @@
+/*
+ * Copyright (c) 2012 NVIDIA 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; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ */
+
+#include <linux/mutex.h>
+#include <linux/module.h>
+#include <linux/cpuquiet.h>
+#include <linux/sysfs.h>
+
+static DEFINE_MUTEX(userspace_mutex);
+
+static int governor_set(unsigned int cpu, bool active)
+{
+	mutex_lock(&userspace_mutex);
+	if (active)
+		cpuquiet_wake_cpu(cpu);
+	else
+		cpuquiet_quiesence_cpu(cpu);
+	mutex_unlock(&userspace_mutex);
+
+	return 0;
+}
+
+struct cpuquiet_governor userspace_governor = {
+	.name		= "userspace",
+	.store_active	= governor_set,
+	.owner		= THIS_MODULE,
+};
+
+static int __init init_usermode(void)
+{
+	return cpuquiet_register_governor(&userspace_governor);
+}
+
+static void __exit exit_usermode(void)
+{
+	cpuquiet_unregister_governor(&userspace_governor);
+}
+
+MODULE_LICENSE("GPL");
+module_init(init_usermode);
+module_exit(exit_usermode);
diff --git a/drivers/cpuquiet/sysfs.c b/drivers/cpuquiet/sysfs.c
new file mode 100644
index 000000000000..0d63eee37dce
--- /dev/null
+++ b/drivers/cpuquiet/sysfs.c
@@ -0,0 +1,291 @@
+/*
+ * Copyright (c) 2012 NVIDIA 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; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/sysfs.h>
+#include <linux/slab.h>
+#include <linux/cpuquiet.h>
+
+#include "cpuquiet.h"
+
+struct cpuquiet_dev {
+	unsigned int cpu;
+	struct kobject kobj;
+};
+
+struct cpuquiet_sysfs_attr {
+	struct attribute attr;
+	ssize_t (*show)(char *);
+	ssize_t (*store)(const char *, size_t count);
+};
+
+static struct kobject *cpuquiet_global_kobject;
+struct cpuquiet_dev *cpuquiet_cpu_devices[CONFIG_NR_CPUS];
+
+static ssize_t show_current_governor(char *buf)
+{
+	ssize_t ret;
+
+	mutex_lock(&cpuquiet_lock);
+
+	if (cpuquiet_curr_governor)
+		ret = sprintf(buf, "%s\n", cpuquiet_curr_governor->name);
+	else
+		ret = sprintf(buf, "none\n");
+
+	mutex_unlock(&cpuquiet_lock);
+
+	return ret;
+
+}
+
+static ssize_t store_current_governor(const char *buf, size_t count)
+{
+	char name[CPUQUIET_NAME_LEN];
+	struct cpuquiet_governor *gov;
+	int len = count, ret = -EINVAL;
+
+	if (!len || len >= sizeof(name))
+		return -EINVAL;
+
+	memcpy(name, buf, count);
+	name[len] = '\0';
+	if (name[len - 1] == '\n')
+		name[--len] = '\0';
+
+	mutex_lock(&cpuquiet_lock);
+	gov = cpuquiet_find_governor(name);
+	mutex_unlock(&cpuquiet_lock);
+
+	if (gov)
+		ret = cpuquiet_switch_governor(gov);
+
+	if (ret)
+		return ret;
+	else
+		return count;
+}
+
+static ssize_t available_governors_show(char *buf)
+{
+	ssize_t ret = 0, len;
+	struct cpuquiet_governor *gov;
+
+	mutex_lock(&cpuquiet_lock);
+	if (!list_empty(&cpuquiet_governors)) {
+		list_for_each_entry(gov, &cpuquiet_governors, governor_list) {
+			len = sprintf(buf, "%s ", gov->name);
+			buf += len;
+			ret += len;
+		}
+		buf--;
+		*buf = '\n';
+	} else
+		ret = sprintf(buf, "none\n");
+
+	mutex_unlock(&cpuquiet_lock);
+
+	return ret;
+}
+
+struct cpuquiet_sysfs_attr attr_current_governor = __ATTR(current_governor,
+			0644, show_current_governor, store_current_governor);
+struct cpuquiet_sysfs_attr attr_governors = __ATTR_RO(available_governors);
+
+
+static struct attribute *cpuquiet_default_attrs[] = {
+	&attr_current_governor.attr,
+	&attr_governors.attr,
+	NULL
+};
+
+static ssize_t cpuquiet_sysfs_show(struct kobject *kobj,
+			struct attribute *attr, char *buf)
+{
+	struct cpuquiet_sysfs_attr *cattr =
+			container_of(attr, struct cpuquiet_sysfs_attr, attr);
+
+	return cattr->show(buf);
+}
+
+static ssize_t cpuquiet_sysfs_store(struct kobject *kobj,
+			struct attribute *attr, const char *buf, size_t count)
+{
+	struct cpuquiet_sysfs_attr *cattr =
+			container_of(attr, struct cpuquiet_sysfs_attr, attr);
+
+	if (cattr->store)
+		return cattr->store(buf, count);
+
+	return -EINVAL;
+}
+
+static const struct sysfs_ops cpuquiet_sysfs_ops = {
+	.show = cpuquiet_sysfs_show,
+	.store = cpuquiet_sysfs_store,
+};
+
+static struct kobj_type ktype_cpuquiet_sysfs = {
+	.sysfs_ops = &cpuquiet_sysfs_ops,
+	.default_attrs = cpuquiet_default_attrs,
+};
+
+int cpuquiet_add_group(struct attribute_group *attrs)
+{
+	return sysfs_create_group(cpuquiet_global_kobject, attrs);
+}
+
+void cpuquiet_remove_group(struct attribute_group *attrs)
+{
+	sysfs_remove_group(cpuquiet_global_kobject, attrs);
+}
+
+int cpuquiet_kobject_init(struct kobject *kobj, struct kobj_type *type,
+				char *name)
+{
+	int err;
+
+	err = kobject_init_and_add(kobj, type, cpuquiet_global_kobject, name);
+	if (!err)
+		kobject_uevent(kobj, KOBJ_ADD);
+
+	return err;
+}
+
+int cpuquiet_cpu_kobject_init(struct kobject *kobj, struct kobj_type *type,
+				char *name, int cpu)
+{
+	int err;
+
+	err = kobject_init_and_add(kobj, type, &cpuquiet_cpu_devices[cpu]->kobj,
+					name);
+	if (!err)
+		kobject_uevent(kobj, KOBJ_ADD);
+
+	return err;
+}
+
+int cpuquiet_add_class_sysfs(struct sysdev_class *cls)
+{
+	int err;
+
+	cpuquiet_global_kobject = kzalloc(sizeof(*cpuquiet_global_kobject),
+						GFP_KERNEL);
+	if (!cpuquiet_global_kobject)
+		return -ENOMEM;
+
+	err = kobject_init_and_add(cpuquiet_global_kobject,
+			&ktype_cpuquiet_sysfs, &cls->kset.kobj, "cpuquiet");
+	if (!err)
+		kobject_uevent(cpuquiet_global_kobject, KOBJ_ADD);
+
+	return err;
+}
+
+
+struct cpuquiet_attr {
+	struct attribute attr;
+	ssize_t (*show)(unsigned int, char *);
+	ssize_t (*store)(unsigned int, const char *, size_t count);
+};
+
+
+static ssize_t cpuquiet_state_show(struct kobject *kobj,
+	struct attribute *attr, char *buf)
+{
+	struct cpuquiet_attr *cattr = container_of(attr,
+					struct cpuquiet_attr, attr);
+	struct cpuquiet_dev *dev = container_of(kobj,
+					struct cpuquiet_dev, kobj);
+
+	return cattr->show(dev->cpu, buf);
+}
+
+static ssize_t cpuquiet_state_store(struct kobject *kobj,
+	struct attribute *attr, const char *buf, size_t count)
+{
+	struct cpuquiet_attr *cattr = container_of(attr,
+					struct cpuquiet_attr, attr);
+	struct cpuquiet_dev *dev = container_of(kobj,
+					struct cpuquiet_dev, kobj);
+
+	if (cattr->store)
+		return cattr->store(dev->cpu, buf, count);
+
+	return -EINVAL;
+}
+
+static ssize_t show_active(unsigned int cpu, char *buf)
+{
+	return sprintf(buf, "%u\n", cpu_online(cpu));
+}
+
+static ssize_t store_active(unsigned int cpu, const char *value, size_t count)
+{
+	unsigned int active;
+	int ret;
+
+	if (!cpuquiet_curr_governor->store_active)
+		return -EINVAL;
+
+	ret = sscanf(value, "%u", &active);
+	if (ret != 1)
+		return -EINVAL;
+
+	cpuquiet_curr_governor->store_active(cpu, active);
+
+	return count;
+}
+
+struct cpuquiet_attr attr_active = __ATTR(active, 0644, show_active,
+						store_active);
+
+static struct attribute *cpuquiet_default_cpu_attrs[] = {
+	&attr_active.attr,
+	NULL
+};
+
+static const struct sysfs_ops cpuquiet_cpu_sysfs_ops = {
+	.show = cpuquiet_state_show,
+	.store = cpuquiet_state_store,
+};
+
+static struct kobj_type ktype_cpuquiet = {
+	.sysfs_ops = &cpuquiet_cpu_sysfs_ops,
+	.default_attrs = cpuquiet_default_cpu_attrs,
+};
+
+void cpuquiet_add_dev(struct sys_device *sys_dev, unsigned int cpu)
+{
+	struct cpuquiet_dev *dev;
+	int err;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	dev->cpu = cpu;
+	cpuquiet_cpu_devices[cpu] = dev;
+	err = kobject_init_and_add(&dev->kobj, &ktype_cpuquiet,
+				&sys_dev->kobj, "cpuquiet");
+	if (!err)
+		kobject_uevent(&dev->kobj, KOBJ_ADD);
+}
+
+void cpuquiet_remove_dev(unsigned int cpu)
+{
+	if (cpu < CONFIG_NR_CPUS && cpuquiet_cpu_devices[cpu])
+		kobject_put(&cpuquiet_cpu_devices[cpu]->kobj);
+}
diff --git a/drivers/crypto/Kconfig b/drivers/crypto/Kconfig
index e0b25de1e339..b6994751719d 100644
--- a/drivers/crypto/Kconfig
+++ b/drivers/crypto/Kconfig
@@ -18,7 +18,7 @@ config CRYPTO_DEV_PADLOCK
 	  (so called VIA PadLock ACE, Advanced Cryptography Engine)
 	  that provides instructions for very fast cryptographic
 	  operations with supported algorithms.
-	  
+
 	  The instructions are used only when the CPU supports them.
 	  Otherwise software encryption is used.
 
@@ -292,4 +292,21 @@ config CRYPTO_DEV_S5P
 	  Select this to offload Samsung S5PV210 or S5PC110 from AES
 	  algorithms execution.
 
+config CRYPTO_DEV_TEGRA_AES
+	tristate "Support for TEGRA AES hw engine"
+	depends on ARCH_TEGRA_2x_SOC || ARCH_TEGRA_3x_SOC
+	select CRYPTO_AES
+	select TEGRA_ARB_SEMAPHORE
+	help
+	  TEGRA processors have AES module accelerator. Select this if you
+	  want to use the TEGRA module for AES algorithms.
+
+config CRYPTO_DEV_TEGRA_SE
+	tristate "Tegra SE driver for crypto algorithms"
+	depends on ARCH_TEGRA_3x_SOC
+	select CRYPTO_AES
+	help
+	  This option allows you to have support of Security Engine for crypto
+	  acceleration.
+
 endif # CRYPTO_HW
diff --git a/drivers/crypto/Makefile b/drivers/crypto/Makefile
index 53ea50155319..7ab59856d755 100644
--- a/drivers/crypto/Makefile
+++ b/drivers/crypto/Makefile
@@ -1,3 +1,5 @@
+GCOV_PROFILE := y
+
 obj-$(CONFIG_CRYPTO_DEV_PADLOCK_AES) += padlock-aes.o
 obj-$(CONFIG_CRYPTO_DEV_PADLOCK_SHA) += padlock-sha.o
 obj-$(CONFIG_CRYPTO_DEV_GEODE) += geode-aes.o
@@ -13,3 +15,7 @@ obj-$(CONFIG_CRYPTO_DEV_OMAP_SHAM) += omap-sham.o
 obj-$(CONFIG_CRYPTO_DEV_OMAP_AES) += omap-aes.o
 obj-$(CONFIG_CRYPTO_DEV_PICOXCELL) += picoxcell_crypto.o
 obj-$(CONFIG_CRYPTO_DEV_S5P) += s5p-sss.o
+CFLAGS_tegra-aes.o 	= -Werror
+obj-$(CONFIG_CRYPTO_DEV_TEGRA_AES) += tegra-aes.o
+CFLAGS_tegra-se.o	= -Werror
+obj-$(CONFIG_CRYPTO_DEV_TEGRA_SE) += tegra-se.o
diff --git a/drivers/crypto/mv_cesa.c b/drivers/crypto/mv_cesa.c
index 3cf303ee3fe3..38a3297ae2be 100644
--- a/drivers/crypto/mv_cesa.c
+++ b/drivers/crypto/mv_cesa.c
@@ -342,11 +342,13 @@ static void mv_process_hash_current(int first_block)
 		else
 			op.config |= CFG_MID_FRAG;
 
-		writel(req_ctx->state[0], cpg->reg + DIGEST_INITIAL_VAL_A);
-		writel(req_ctx->state[1], cpg->reg + DIGEST_INITIAL_VAL_B);
-		writel(req_ctx->state[2], cpg->reg + DIGEST_INITIAL_VAL_C);
-		writel(req_ctx->state[3], cpg->reg + DIGEST_INITIAL_VAL_D);
-		writel(req_ctx->state[4], cpg->reg + DIGEST_INITIAL_VAL_E);
+		if (first_block) {
+			writel(req_ctx->state[0], cpg->reg + DIGEST_INITIAL_VAL_A);
+			writel(req_ctx->state[1], cpg->reg + DIGEST_INITIAL_VAL_B);
+			writel(req_ctx->state[2], cpg->reg + DIGEST_INITIAL_VAL_C);
+			writel(req_ctx->state[3], cpg->reg + DIGEST_INITIAL_VAL_D);
+			writel(req_ctx->state[4], cpg->reg + DIGEST_INITIAL_VAL_E);
+		}
 	}
 
 	memcpy(cpg->sram + SRAM_CONFIG, &op, sizeof(struct sec_accel_config));
diff --git a/drivers/crypto/tegra-aes.c b/drivers/crypto/tegra-aes.c
new file mode 100644
index 000000000000..85c1df63bebc
--- /dev/null
+++ b/drivers/crypto/tegra-aes.c
@@ -0,0 +1,1527 @@
+/*
+ * drivers/crypto/tegra-aes.c
+ *
+ * aes driver for NVIDIA tegra aes hardware
+ *
+ * Copyright (c) 2010-2011, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/scatterlist.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+#include <linux/mutex.h>
+#include <linux/interrupt.h>
+#include <linux/completion.h>
+#include <linux/workqueue.h>
+#include <linux/delay.h>
+
+#include <mach/arb_sema.h>
+#include <mach/clk.h>
+#include "../video/tegra/nvmap/nvmap.h"
+
+#include <crypto/scatterwalk.h>
+#include <crypto/aes.h>
+#include <crypto/internal/rng.h>
+
+#include "tegra-aes.h"
+
+#define FLAGS_MODE_MASK		0x00ff
+#define FLAGS_ENCRYPT		BIT(0)
+#define FLAGS_CBC		BIT(1)
+#define FLAGS_GIV		BIT(2)
+#define FLAGS_RNG		BIT(3)
+#define FLAGS_OFB		BIT(4)
+#define FLAGS_INIT		BIT(5)
+#define FLAGS_BUSY		1
+
+/*
+ * Defines AES engine Max process bytes size in one go, which takes 1 msec.
+ * AES engine spends about 176 cycles/16-bytes or 11 cycles/byte
+ * The duration CPU can use the BSE to 1 msec, then the number of available
+ * cycles of AVP/BSE is 216K. In this duration, AES can process 216/11 ~= 19KB
+ * Based on this AES_HW_DMA_BUFFER_SIZE_BYTES is configured to 16KB.
+ */
+#define AES_HW_DMA_BUFFER_SIZE_BYTES	0x4000
+
+/*
+ * The key table length is 64 bytes
+ * (This includes first upto 32 bytes key + 16 bytes original initial vector
+ * and 16 bytes updated initial vector)
+ */
+#define AES_HW_KEY_TABLE_LENGTH_BYTES	64
+
+#define AES_HW_IV_SIZE	16
+#define AES_HW_KEYSCHEDULE_LEN	256
+#define ARB_SEMA_TIMEOUT	500
+
+/*
+ * The memory being used is divides as follows:
+ * 1. Key - 32 bytes
+ * 2. Original IV - 16 bytes
+ * 3. Updated IV - 16 bytes
+ * 4. Key schedule - 256 bytes
+ *
+ * 1+2+3 constitute the hw key table.
+ */
+#define AES_IVKEY_SIZE (AES_HW_KEY_TABLE_LENGTH_BYTES + AES_HW_KEYSCHEDULE_LEN)
+
+#define DEFAULT_RNG_BLK_SZ	16
+
+/* As of now only 5 commands are USED for AES encryption/Decryption */
+#define AES_HW_MAX_ICQ_LENGTH	4
+
+#define ICQBITSHIFT_BLKCNT	0
+
+/* memdma_vd command */
+#define MEMDMA_DIR_DTOVRAM	0
+#define MEMDMA_DIR_VTODRAM	1
+#define MEMDMABITSHIFT_DIR	25
+#define MEMDMABITSHIFT_NUM_WORDS	12
+
+/* Define AES Interactive command Queue commands Bit positions */
+enum {
+	ICQBITSHIFT_KEYTABLEADDR = 0,
+	ICQBITSHIFT_KEYTABLEID = 17,
+	ICQBITSHIFT_VRAMSEL = 23,
+	ICQBITSHIFT_TABLESEL = 24,
+	ICQBITSHIFT_OPCODE = 26,
+};
+
+/* Define Ucq opcodes required for AES operation */
+enum {
+	UCQOPCODE_BLKSTARTENGINE = 0x0E,
+	UCQOPCODE_DMASETUP = 0x10,
+	UCQOPCODE_DMACOMPLETE = 0x11,
+	UCQOPCODE_SETTABLE = 0x15,
+	UCQOPCODE_MEMDMAVD = 0x22,
+};
+
+/* Define Aes command values */
+enum {
+	UCQCMD_VRAM_SEL = 0x1,
+	UCQCMD_CRYPTO_TABLESEL = 0x3,
+	UCQCMD_KEYSCHEDTABLESEL = 0x4,
+	UCQCMD_KEYTABLESEL = 0x8,
+};
+
+#define UCQCMD_KEYTABLEADDRMASK 0x1FFFF
+
+#define AES_NR_KEYSLOTS	8
+#define SSK_SLOT_NUM	4
+
+struct tegra_aes_slot {
+	struct list_head node;
+	int slot_num;
+	bool available;
+};
+
+struct tegra_aes_reqctx {
+	unsigned long mode;
+};
+
+#define TEGRA_AES_QUEUE_LENGTH	500
+
+struct tegra_aes_engine {
+	struct tegra_aes_dev *dd;
+	struct tegra_aes_ctx *ctx;
+	struct clk *iclk;
+	struct clk *pclk;
+	struct ablkcipher_request *req;
+	struct scatterlist *in_sg;
+	struct completion op_complete;
+	struct scatterlist *out_sg;
+	void __iomem *io_base;
+	void __iomem *ivkey_base;
+	unsigned long phys_base;
+	unsigned long iram_phys;
+	void *iram_virt;
+	dma_addr_t ivkey_phys_base;
+	dma_addr_t dma_buf_in;
+	dma_addr_t dma_buf_out;
+	size_t total;
+	size_t in_offset;
+	size_t out_offset;
+	u32 engine_offset;
+	u32 *buf_in;
+	u32 *buf_out;
+	int res_id;
+	unsigned long busy;
+	u8 irq;
+	u32 status;
+};
+
+struct tegra_aes_dev {
+	struct device *dev;
+	struct tegra_aes_slot *slots;
+	struct tegra_aes_engine bsev;
+	struct tegra_aes_engine bsea;
+	struct nvmap_client *client;
+	struct nvmap_handle_ref *h_ref;
+	struct crypto_queue queue;
+	spinlock_t lock;
+	u64 ctr;
+	unsigned long flags;
+	u8 dt[DEFAULT_RNG_BLK_SZ];
+};
+
+static struct tegra_aes_dev *aes_dev;
+
+struct tegra_aes_ctx {
+	struct tegra_aes_dev *dd;
+	struct tegra_aes_engine *eng;
+	struct tegra_aes_slot *slot;
+	int key[AES_MAX_KEY_SIZE];
+	int keylen;
+	bool use_ssk;
+	u8 dt[DEFAULT_RNG_BLK_SZ];
+};
+
+static struct tegra_aes_ctx rng_ctx;
+
+/* keep registered devices data here */
+static LIST_HEAD(slot_list);
+static DEFINE_SPINLOCK(list_lock);
+
+/* Engine specific work queues */
+static void bsev_workqueue_handler(struct work_struct *work);
+static void bsea_workqueue_handler(struct work_struct *work);
+
+static DECLARE_WORK(bsev_work, bsev_workqueue_handler);
+static DECLARE_WORK(bsea_work, bsea_workqueue_handler);
+
+static struct workqueue_struct *bsev_wq;
+static struct workqueue_struct *bsea_wq;
+
+extern unsigned long long tegra_chip_uid(void);
+
+static inline u32 aes_readl(struct tegra_aes_engine *engine, u32 offset)
+{
+	return readl(engine->io_base + offset);
+}
+
+static inline void aes_writel(struct tegra_aes_engine *engine,
+	u32 val, u32 offset)
+{
+	writel(val, engine->io_base + offset);
+}
+
+static int alloc_iram(struct tegra_aes_dev *dd)
+{
+	size_t size;
+	int err;
+
+	dd->h_ref = NULL;
+
+	/* [key+iv+u-iv=64B] * 8 = 512Bytes */
+	size = AES_MAX_KEY_SIZE;
+	dd->client = nvmap_create_client(nvmap_dev, "aes_bsea");
+	if (IS_ERR(dd->client)) {
+		dev_err(dd->dev, "nvmap_create_client failed\n");
+		goto out;
+	}
+
+	dd->h_ref = nvmap_create_handle(dd->client, size);
+	if (IS_ERR(dd->h_ref)) {
+		dev_err(dd->dev, "nvmap_create_handle failed\n");
+		goto out;
+	}
+
+	/* Allocate memory in the iram */
+	err = nvmap_alloc_handle_id(dd->client, nvmap_ref_to_id(dd->h_ref),
+		NVMAP_HEAP_CARVEOUT_IRAM, size, 0);
+	if (err) {
+		dev_err(dd->dev, "nvmap_alloc_handle_id failed\n");
+		nvmap_free_handle_id(dd->client, nvmap_ref_to_id(dd->h_ref));
+		goto out;
+	}
+	dd->bsea.iram_phys = nvmap_handle_address(dd->client,
+					nvmap_ref_to_id(dd->h_ref));
+
+	dd->bsea.iram_virt = nvmap_mmap(dd->h_ref);	/* get virtual address */
+	if (!dd->bsea.iram_virt) {
+		dev_err(dd->dev, "%s: no mem, BSEA IRAM alloc failure\n",
+			__func__);
+		goto out;
+	}
+
+	memset(dd->bsea.iram_virt, 0, size);
+	return 0;
+
+out:
+	if (dd->bsea.iram_virt)
+		nvmap_munmap(dd->h_ref, dd->bsea.iram_virt);
+
+	if (dd->client) {
+		nvmap_free_handle_id(dd->client, nvmap_ref_to_id(dd->h_ref));
+		nvmap_client_put(dd->client);
+	}
+
+	return -ENOMEM;
+}
+
+static void free_iram(struct tegra_aes_dev *dd)
+{
+	if (dd->bsea.iram_virt)
+		nvmap_munmap(dd->h_ref, dd->bsea.iram_virt);
+
+	if (dd->client) {
+		nvmap_free_handle_id(dd->client, nvmap_ref_to_id(dd->h_ref));
+		nvmap_client_put(dd->client);
+	}
+}
+
+static int aes_hw_init(struct tegra_aes_engine *engine)
+{
+	struct tegra_aes_dev *dd = aes_dev;
+	int ret = 0;
+
+	if (engine->pclk) {
+		ret = clk_enable(engine->pclk);
+		if (ret < 0) {
+			dev_err(dd->dev, "%s: pclock enable fail(%d)\n",
+			__func__, ret);
+			return ret;
+		}
+	}
+
+	if (engine->iclk) {
+		ret = clk_enable(engine->iclk);
+		if (ret < 0) {
+			dev_err(dd->dev, "%s: iclock enable fail(%d)\n",
+			__func__, ret);
+			if (engine->pclk)
+				clk_disable(engine->pclk);
+			return ret;
+		}
+	}
+
+	return ret;
+}
+
+static void aes_hw_deinit(struct tegra_aes_engine *engine)
+{
+	if (engine->pclk)
+		clk_disable(engine->pclk);
+
+	if (engine->iclk)
+		clk_disable(engine->iclk);
+}
+
+#define MIN_RETRIES	3
+static int aes_start_crypt(struct tegra_aes_engine *eng, u32 in_addr,
+	u32 out_addr, int nblocks, int mode, bool upd_iv)
+{
+	u32 cmdq[AES_HW_MAX_ICQ_LENGTH];
+	int qlen = 0, i, eng_busy, icq_empty, ret;
+	u32 value;
+	int retries = MIN_RETRIES;
+
+start:
+	do {
+		value = aes_readl(eng, INTR_STATUS);
+		eng_busy = value & BIT(0);
+		icq_empty = value & BIT(3);
+	} while (eng_busy || (!icq_empty));
+
+	aes_writel(eng, 0xFFFFFFFF, INTR_STATUS);
+
+	/* error, dma xfer complete */
+	aes_writel(eng, 0x33, INT_ENB);
+	enable_irq(eng->irq);
+
+	cmdq[qlen++] = UCQOPCODE_DMASETUP << ICQBITSHIFT_OPCODE;
+	cmdq[qlen++] = in_addr;
+	cmdq[qlen++] = UCQOPCODE_BLKSTARTENGINE << ICQBITSHIFT_OPCODE |
+		(nblocks-1) << ICQBITSHIFT_BLKCNT;
+	cmdq[qlen++] = UCQOPCODE_DMACOMPLETE << ICQBITSHIFT_OPCODE;
+
+	value = aes_readl(eng, CMDQUE_CONTROL);
+	/* access SDRAM through AHB */
+	value &= (~CMDQ_CTRL_SRC_STM_SEL_FIELD & ~CMDQ_CTRL_DST_STM_SEL_FIELD);
+	value |= (CMDQ_CTRL_SRC_STM_SEL_FIELD | CMDQ_CTRL_DST_STM_SEL_FIELD |
+		CMDQ_CTRL_ICMDQEN_FIELD | CMDQ_CTRL_ERROR_FLUSH_ENB);
+	aes_writel(eng, value, CMDQUE_CONTROL);
+
+	value = 0;
+	if (mode & FLAGS_CBC) {
+		value = ((0x1 << SECURE_INPUT_ALG_SEL_SHIFT) |
+			((eng->ctx->keylen * 8) << SECURE_INPUT_KEY_LEN_SHIFT) |
+			((u32)upd_iv << SECURE_IV_SELECT_SHIFT) |
+			(((mode & FLAGS_ENCRYPT) ? 2 : 3)
+				<< SECURE_XOR_POS_SHIFT) |
+			(0 << SECURE_INPUT_SEL_SHIFT) |
+			(((mode & FLAGS_ENCRYPT) ? 2 : 3)
+				<< SECURE_VCTRAM_SEL_SHIFT) |
+			((mode & FLAGS_ENCRYPT) ? 1 : 0)
+				<< SECURE_CORE_SEL_SHIFT |
+			(0 << SECURE_RNG_ENB_SHIFT) |
+			(0 << SECURE_HASH_ENB_SHIFT));
+	} else if (mode & FLAGS_OFB) {
+		value = ((0x1 << SECURE_INPUT_ALG_SEL_SHIFT) |
+			((eng->ctx->keylen * 8) << SECURE_INPUT_KEY_LEN_SHIFT) |
+			((u32)upd_iv << SECURE_IV_SELECT_SHIFT) |
+			((u32)0 << SECURE_IV_SELECT_SHIFT) |
+			(SECURE_XOR_POS_FIELD) |
+			(2 << SECURE_INPUT_SEL_SHIFT) |
+			(0 << SECURE_VCTRAM_SEL_SHIFT) |
+			(SECURE_CORE_SEL_FIELD) |
+			(0 << SECURE_RNG_ENB_SHIFT) |
+			(0 << SECURE_HASH_ENB_SHIFT));
+	} else if (mode & FLAGS_RNG){
+		value = ((0x1 << SECURE_INPUT_ALG_SEL_SHIFT) |
+			((eng->ctx->keylen * 8) << SECURE_INPUT_KEY_LEN_SHIFT) |
+			((u32)upd_iv << SECURE_IV_SELECT_SHIFT) |
+			(0 << SECURE_XOR_POS_SHIFT) |
+			(0 << SECURE_INPUT_SEL_SHIFT) |
+			((mode & FLAGS_ENCRYPT) ? 1 : 0)
+				<< SECURE_CORE_SEL_SHIFT |
+			(1 << SECURE_RNG_ENB_SHIFT) |
+			(0 << SECURE_HASH_ENB_SHIFT));
+	} else {
+		value = ((0x1 << SECURE_INPUT_ALG_SEL_SHIFT) |
+			((eng->ctx->keylen * 8) << SECURE_INPUT_KEY_LEN_SHIFT) |
+			((u32)upd_iv << SECURE_IV_SELECT_SHIFT) |
+			(0 << SECURE_XOR_POS_SHIFT) |
+			(0 << SECURE_INPUT_SEL_SHIFT) |
+			(((mode & FLAGS_ENCRYPT) ? 1 : 0)
+				<< SECURE_CORE_SEL_SHIFT) |
+			(0 << SECURE_RNG_ENB_SHIFT) |
+				(0 << SECURE_HASH_ENB_SHIFT));
+	}
+	aes_writel(eng, value, SECURE_INPUT_SELECT);
+
+	aes_writel(eng, out_addr, SECURE_DEST_ADDR);
+	INIT_COMPLETION(eng->op_complete);
+
+	for (i = 0; i < qlen - 1; i++) {
+		do {
+			value = aes_readl(eng, INTR_STATUS);
+			eng_busy = value & BIT(0);
+			icq_empty = value & BIT(3);
+		} while (eng_busy || (!icq_empty));
+		aes_writel(eng, cmdq[i], ICMDQUE_WR);
+	}
+
+	ret = wait_for_completion_timeout(&eng->op_complete,
+		msecs_to_jiffies(150));
+	if (ret == 0) {
+		dev_err(aes_dev->dev, "engine%d timed out (0x%x)\n",
+			eng->res_id, aes_readl(eng, INTR_STATUS));
+		disable_irq(eng->irq);
+		return -ETIMEDOUT;
+	}
+
+	disable_irq(eng->irq);
+	aes_writel(eng, cmdq[qlen - 1], ICMDQUE_WR);
+
+	if ((eng->status != 0) && (retries-- > 0)) {
+		qlen = 0;
+		goto start;
+	}
+
+	return 0;
+}
+
+static void aes_release_key_slot(struct tegra_aes_ctx *ctx)
+{
+	spin_lock(&list_lock);
+	ctx->slot->available = true;
+	ctx->slot = NULL;
+	spin_unlock(&list_lock);
+}
+
+static struct tegra_aes_slot *aes_find_key_slot(struct tegra_aes_dev *dd)
+{
+	struct tegra_aes_slot *slot = NULL;
+	bool found = 0;
+
+	spin_lock(&list_lock);
+	list_for_each_entry(slot, &slot_list, node) {
+		dev_dbg(dd->dev, "empty:%d, num:%d\n", slot->available,
+			slot->slot_num);
+		if (slot->available) {
+			slot->available = false;
+			found = 1;
+			break;
+		}
+	}
+
+	spin_unlock(&list_lock);
+	return found ? slot : NULL;
+}
+
+static int aes_set_key(struct tegra_aes_engine *eng, int slot_num)
+{
+	struct tegra_aes_dev *dd = aes_dev;
+	u32 value, cmdq[2];
+	int i, eng_busy, icq_empty, dma_busy;
+
+	if (!eng) {
+		dev_err(dd->dev, "%s: context invalid\n", __func__);
+		return -EINVAL;
+	}
+
+	/* enable key schedule generation in hardware */
+	value = aes_readl(eng, SECURE_CONFIG_EXT);
+	value &= ~SECURE_KEY_SCH_DIS_FIELD;
+	aes_writel(eng, value, SECURE_CONFIG_EXT);
+
+	/* select the key slot */
+	value = aes_readl(eng, SECURE_CONFIG);
+	value &= ~SECURE_KEY_INDEX_FIELD;
+	value |= (slot_num << SECURE_KEY_INDEX_SHIFT);
+	aes_writel(eng, value, SECURE_CONFIG);
+
+	if (slot_num == SSK_SLOT_NUM)
+		goto out;
+
+	if (eng->res_id == TEGRA_ARB_BSEV) {
+		memset(dd->bsev.ivkey_base, 0, AES_HW_KEY_TABLE_LENGTH_BYTES);
+		memcpy(dd->bsev.ivkey_base, eng->ctx->key, eng->ctx->keylen);
+
+		/* sync the buffer for device */
+		dma_sync_single_for_device(dd->dev, dd->bsev.ivkey_phys_base,
+				eng->ctx->keylen, DMA_TO_DEVICE);
+
+		/* copy the key table from sdram to vram */
+		cmdq[0] = 0;
+		cmdq[0] = UCQOPCODE_MEMDMAVD << ICQBITSHIFT_OPCODE |
+				(MEMDMA_DIR_DTOVRAM << MEMDMABITSHIFT_DIR) |
+			(AES_HW_KEY_TABLE_LENGTH_BYTES/sizeof(u32))
+			<< MEMDMABITSHIFT_NUM_WORDS;
+		cmdq[1] = (u32)eng->ivkey_phys_base;
+		for (i = 0; i < ARRAY_SIZE(cmdq); i++)
+			aes_writel(eng, cmdq[i], ICMDQUE_WR);
+		do {
+			value = aes_readl(eng, INTR_STATUS);
+			eng_busy = value & BIT(0);
+			icq_empty = value & BIT(3);
+			dma_busy = value & BIT(23);
+		} while (eng_busy & (!icq_empty) & dma_busy);
+
+		/* settable command to get key into internal registers */
+		value = 0;
+		value = UCQOPCODE_SETTABLE << ICQBITSHIFT_OPCODE |
+			UCQCMD_CRYPTO_TABLESEL << ICQBITSHIFT_TABLESEL |
+			UCQCMD_VRAM_SEL << ICQBITSHIFT_VRAMSEL |
+			(UCQCMD_KEYTABLESEL | slot_num)
+			<< ICQBITSHIFT_KEYTABLEID;
+		aes_writel(eng, value, ICMDQUE_WR);
+		do {
+			value = aes_readl(eng, INTR_STATUS);
+			eng_busy = value & BIT(0);
+			icq_empty = value & BIT(3);
+		} while (eng_busy & (!icq_empty));
+	} else {
+		memset(dd->bsea.iram_virt, 0, AES_HW_KEY_TABLE_LENGTH_BYTES);
+		memcpy(dd->bsea.iram_virt, eng->ctx->key, eng->ctx->keylen);
+
+		/* set iram access cfg bit 0 if address >128K */
+		if (dd->bsea.iram_phys > 0x00020000)
+			aes_writel(eng, BIT(0), IRAM_ACCESS_CFG);
+		else
+			aes_writel(eng, 0, IRAM_ACCESS_CFG);
+
+		/* settable command to get key into internal registers */
+		value = 0;
+		value = UCQOPCODE_SETTABLE << ICQBITSHIFT_OPCODE |
+			UCQCMD_CRYPTO_TABLESEL << ICQBITSHIFT_TABLESEL |
+			(UCQCMD_KEYTABLESEL | slot_num)
+			<< ICQBITSHIFT_KEYTABLEID |
+			dd->bsea.iram_phys >> 2;
+			aes_writel(eng, value, ICMDQUE_WR);
+		do {
+			value = aes_readl(eng, INTR_STATUS);
+			eng_busy = value & BIT(0);
+			icq_empty = value & BIT(3);
+		} while (eng_busy & (!icq_empty));
+	}
+
+out:
+	return 0;
+}
+
+static int tegra_aes_handle_req(struct tegra_aes_engine *eng)
+{
+	struct tegra_aes_dev *dd = aes_dev;
+	struct tegra_aes_ctx *ctx;
+	struct crypto_async_request *async_req, *backlog;
+	struct tegra_aes_reqctx *rctx;
+	struct ablkcipher_request *req;
+	unsigned long irq_flags;
+	int dma_max = AES_HW_DMA_BUFFER_SIZE_BYTES;
+	int nblocks, total, ret = 0, count = 0;
+	dma_addr_t addr_in, addr_out;
+	struct scatterlist *in_sg, *out_sg;
+
+	spin_lock_irqsave(&dd->lock, irq_flags);
+	backlog = crypto_get_backlog(&dd->queue);
+	async_req = crypto_dequeue_request(&dd->queue);
+	if (!async_req)
+		clear_bit(FLAGS_BUSY, &eng->busy);
+	spin_unlock_irqrestore(&dd->lock, irq_flags);
+
+	if (!async_req)
+		return -ENODATA;
+
+	if (backlog)
+		backlog->complete(backlog, -EINPROGRESS);
+
+	req = ablkcipher_request_cast(async_req);
+	dev_dbg(dd->dev, "%s: get new req (engine #%d)\n", __func__,
+		eng->res_id);
+
+	if (!req->src || !req->dst)
+		return -EINVAL;
+
+	/* take the hardware semaphore */
+	if (tegra_arb_mutex_lock_timeout(eng->res_id, ARB_SEMA_TIMEOUT) < 0) {
+		dev_err(dd->dev, "aes hardware (%d) not available\n",
+		eng->res_id);
+		return -EBUSY;
+	}
+
+	/* assign new request to device */
+	eng->req = req;
+	eng->total = req->nbytes;
+	eng->in_offset = 0;
+	eng->in_sg = req->src;
+	eng->out_offset = 0;
+	eng->out_sg = req->dst;
+
+	in_sg = eng->in_sg;
+	out_sg = eng->out_sg;
+	total = eng->total;
+
+	rctx = ablkcipher_request_ctx(req);
+	ctx = crypto_ablkcipher_ctx(crypto_ablkcipher_reqtfm(req));
+	rctx->mode &= FLAGS_MODE_MASK;
+	dd->flags = (dd->flags & ~FLAGS_MODE_MASK) | rctx->mode;
+	eng->ctx = ctx;
+
+	if (((dd->flags & FLAGS_CBC) || (dd->flags & FLAGS_OFB)) && req->info) {
+		/* set iv to the aes hw slot
+		 * Hw generates updated iv only after iv is set in slot.
+		 * So key and iv is passed asynchronously.
+		*/
+		memcpy(eng->buf_in, (u8 *)req->info, AES_BLOCK_SIZE);
+
+		/* sync the buffer for device  */
+		dma_sync_single_for_device(dd->dev, eng->dma_buf_in,
+				AES_HW_DMA_BUFFER_SIZE_BYTES, DMA_TO_DEVICE);
+
+		ret = aes_start_crypt(eng, (u32)eng->dma_buf_in,
+			(u32)eng->dma_buf_out, 1, FLAGS_CBC, false);
+		if (ret < 0) {
+			dev_err(dd->dev, "aes_start_crypt fail(%d)\n", ret);
+			goto out;
+		}
+
+		/* sync the buffer for cpu  */
+		dma_sync_single_for_cpu(dd->dev, eng->dma_buf_out,
+				AES_HW_DMA_BUFFER_SIZE_BYTES, DMA_FROM_DEVICE);
+	}
+
+	while (total) {
+		dev_dbg(dd->dev, "remain: %d\n", total);
+		ret = dma_map_sg(dd->dev, in_sg, 1, DMA_TO_DEVICE);
+		if (!ret) {
+			dev_err(dd->dev, "dma_map_sg() error\n");
+			goto out;
+		}
+
+		ret = dma_map_sg(dd->dev, out_sg, 1, DMA_FROM_DEVICE);
+		if (!ret) {
+			dev_err(dd->dev, "dma_map_sg() error\n");
+			dma_unmap_sg(dd->dev, eng->in_sg,
+				1, DMA_TO_DEVICE);
+			goto out;
+		}
+
+		addr_in = sg_dma_address(in_sg);
+		addr_out = sg_dma_address(out_sg);
+		count = min((int)sg_dma_len(in_sg), (int)dma_max);
+		WARN_ON(sg_dma_len(in_sg) != sg_dma_len(out_sg));
+		nblocks = DIV_ROUND_UP(count, AES_BLOCK_SIZE);
+
+		ret = aes_start_crypt(eng, addr_in, addr_out, nblocks,
+			dd->flags, true);
+
+		dma_unmap_sg(dd->dev, out_sg, 1, DMA_FROM_DEVICE);
+		dma_unmap_sg(dd->dev, in_sg, 1, DMA_TO_DEVICE);
+		if (ret < 0) {
+			dev_err(dd->dev, "aes_start_crypt fail(%d)\n", ret);
+			goto out;
+		}
+
+		dev_dbg(dd->dev, "out: copied %d\n", count);
+		total -= count;
+		in_sg = sg_next(in_sg);
+		out_sg = sg_next(out_sg);
+		WARN_ON(((total != 0) && (!in_sg || !out_sg)));
+	}
+
+out:
+	/* release the hardware semaphore */
+	tegra_arb_mutex_unlock(eng->res_id);
+	eng->total = total;
+
+	if (eng->req->base.complete)
+		eng->req->base.complete(&eng->req->base, ret);
+
+	dev_dbg(dd->dev, "%s: exit\n", __func__);
+	return ret;
+}
+
+static int tegra_aes_key_save(struct tegra_aes_ctx *ctx)
+{
+	struct tegra_aes_dev *dd = aes_dev;
+	int retry_count, eng_busy, ret, eng_no;
+	struct tegra_aes_engine *eng[2] = {&dd->bsev, &dd->bsea};
+	unsigned long flags;
+
+	/* check for engine free state */
+	for (eng_no = 0; eng_no < ARRAY_SIZE(eng); eng_no++) {
+		for (retry_count = 0; retry_count <= 10; retry_count++) {
+			spin_lock_irqsave(&dd->lock, flags);
+			eng_busy = test_and_set_bit(FLAGS_BUSY,
+							&eng[eng_no]->busy);
+			spin_unlock_irqrestore(&dd->lock, flags);
+
+			if (!eng_busy)
+				break;
+
+			if (retry_count == 10) {
+				dev_err(dd->dev,
+					"%s: eng=%d busy, wait timeout\n",
+					__func__, eng[eng_no]->res_id);
+				ret = -EBUSY;
+				goto out;
+			}
+			mdelay(5);
+		}
+	}
+
+	/* save key in the engine */
+	for (eng_no = 0;  eng_no < ARRAY_SIZE(eng); eng_no++) {
+		ret = aes_hw_init(eng[eng_no]);
+		if (ret < 0) {
+			dev_err(dd->dev, "%s: eng=%d hw init fail(%d)\n",
+			__func__, eng[eng_no]->res_id, ret);
+			goto out;
+		}
+		eng[eng_no]->ctx = ctx;
+		if (ctx->use_ssk)
+			aes_set_key(eng[eng_no], SSK_SLOT_NUM);
+		else
+			aes_set_key(eng[eng_no], ctx->slot->slot_num);
+
+		aes_hw_deinit(eng[eng_no]);
+	}
+out:
+	spin_lock_irqsave(&dd->lock, flags);
+	while (--eng_no >= 0)
+		clear_bit(FLAGS_BUSY, &eng[eng_no]->busy);
+	spin_unlock_irqrestore(&dd->lock, flags);
+
+	return ret;
+}
+
+static int tegra_aes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
+	unsigned int keylen)
+{
+	struct tegra_aes_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	struct tegra_aes_dev *dd = aes_dev;
+	struct tegra_aes_slot *key_slot;
+	int ret = 0;
+
+	if (!ctx || !dd) {
+		pr_err("ctx=0x%x, dd=0x%x\n",
+			(unsigned int)ctx, (unsigned int)dd);
+		return -EINVAL;
+	}
+
+	if ((keylen != AES_KEYSIZE_128) && (keylen != AES_KEYSIZE_192) &&
+		(keylen != AES_KEYSIZE_256)) {
+		dev_err(dd->dev, "unsupported key size\n");
+		return -EINVAL;
+	}
+
+	/* take the hardware semaphore */
+	if (tegra_arb_mutex_lock_timeout(dd->bsev.res_id, ARB_SEMA_TIMEOUT) < 0) {
+		dev_err(dd->dev, "aes hardware (%d) not available\n", dd->bsev.res_id);
+		return -EBUSY;
+	}
+
+	if (tegra_arb_mutex_lock_timeout(dd->bsea.res_id, ARB_SEMA_TIMEOUT) < 0) {
+		dev_err(dd->dev, "aes hardware (%d) not available\n", dd->bsea.res_id);
+		tegra_arb_mutex_unlock(dd->bsev.res_id);
+		return -EBUSY;
+	}
+
+	dev_dbg(dd->dev, "keylen: %d\n", keylen);
+	ctx->dd = dd;
+
+	if (key) {
+		if (!ctx->slot) {
+			key_slot = aes_find_key_slot(dd);
+			if (!key_slot) {
+				dev_err(dd->dev, "no empty slot\n");
+				ret = -EBUSY;
+				goto out;
+			}
+			ctx->slot = key_slot;
+		}
+
+		/* copy the key to the proper slot */
+		memset(ctx->key, 0, AES_MAX_KEY_SIZE);
+		memcpy(ctx->key, key, keylen);
+		ctx->keylen = keylen;
+		ctx->use_ssk = false;
+	} else {
+		ctx->use_ssk = true;
+		ctx->keylen = AES_KEYSIZE_128;
+	}
+
+	ret = tegra_aes_key_save(ctx);
+	if (ret != 0)
+		dev_err(dd->dev, "%s failed\n", __func__);
+out:
+	tegra_arb_mutex_unlock(dd->bsev.res_id);
+	tegra_arb_mutex_unlock(dd->bsea.res_id);
+	dev_dbg(dd->dev, "done\n");
+	return ret;
+}
+
+static void bsev_workqueue_handler(struct work_struct *work)
+{
+	struct tegra_aes_dev *dd = aes_dev;
+	struct tegra_aes_engine *engine = &dd->bsev;
+	int ret;
+
+	aes_hw_init(engine);
+
+	/* empty the crypto queue and then return */
+	do {
+		ret = tegra_aes_handle_req(engine);
+	} while (!ret);
+
+	aes_hw_deinit(engine);
+}
+
+static void bsea_workqueue_handler(struct work_struct *work)
+{
+	struct tegra_aes_dev *dd = aes_dev;
+	struct tegra_aes_engine *engine = &dd->bsea;
+	int ret;
+
+	aes_hw_init(engine);
+
+	/* empty the crypto queue and then return */
+	do {
+		ret = tegra_aes_handle_req(engine);
+	} while (!ret);
+
+	aes_hw_deinit(engine);
+}
+
+#define INT_ERROR_MASK	0xFFF000
+static irqreturn_t aes_bsev_irq(int irq, void *dev_id)
+{
+	struct tegra_aes_dev *dd = (struct tegra_aes_dev *)dev_id;
+	u32 value = aes_readl(&dd->bsev, INTR_STATUS);
+
+	dev_dbg(dd->dev, "bsev irq_stat: 0x%x", value);
+	dd->bsev.status = 0;
+	if (value & INT_ERROR_MASK) {
+		aes_writel(&dd->bsev, INT_ERROR_MASK, INTR_STATUS);
+		dd->bsev.status = value & INT_ERROR_MASK;
+	}
+
+	value = aes_readl(&dd->bsev, INTR_STATUS);
+	if (!(value & ENGINE_BUSY_FIELD))
+		complete(&dd->bsev.op_complete);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t aes_bsea_irq(int irq, void *dev_id)
+{
+	struct tegra_aes_dev *dd = (struct tegra_aes_dev *)dev_id;
+	u32 value = aes_readl(&dd->bsea, INTR_STATUS);
+
+	dev_dbg(dd->dev, "bsea irq_stat: 0x%x", value);
+	dd->bsea.status = 0;
+	if (value & INT_ERROR_MASK) {
+		aes_writel(&dd->bsea, INT_ERROR_MASK, INTR_STATUS);
+		dd->bsea.status = value & INT_ERROR_MASK;
+	}
+
+	value = aes_readl(&dd->bsea, INTR_STATUS);
+	if (!(value & ENGINE_BUSY_FIELD))
+		complete(&dd->bsea.op_complete);
+
+	return IRQ_HANDLED;
+}
+
+static int tegra_aes_crypt(struct ablkcipher_request *req, unsigned long mode)
+{
+	struct tegra_aes_reqctx *rctx = ablkcipher_request_ctx(req);
+	struct tegra_aes_dev *dd = aes_dev;
+	unsigned long flags;
+	int err = 0;
+	int bsev_busy;
+	int bsea_busy;
+
+	dev_dbg(dd->dev, "nbytes: %d, enc: %d, cbc: %d, ofb: %d\n", req->nbytes,
+		!!(mode & FLAGS_ENCRYPT),
+		!!(mode & FLAGS_CBC),
+		!!(mode & FLAGS_OFB));
+
+	rctx->mode = mode;
+
+	spin_lock_irqsave(&dd->lock, flags);
+	err = ablkcipher_enqueue_request(&dd->queue, req);
+	spin_unlock_irqrestore(&dd->lock, flags);
+	bsev_busy = test_and_set_bit(FLAGS_BUSY, &dd->bsev.busy);
+	if (!bsev_busy) {
+		queue_work(bsev_wq, &bsev_work);
+		goto finish;
+	}
+
+	bsea_busy = test_and_set_bit(FLAGS_BUSY, &dd->bsea.busy);
+	if (!bsea_busy)
+		queue_work(bsea_wq, &bsea_work);
+
+finish:
+	return err;
+}
+
+static int tegra_aes_ecb_encrypt(struct ablkcipher_request *req)
+{
+	return tegra_aes_crypt(req, FLAGS_ENCRYPT);
+}
+
+static int tegra_aes_ecb_decrypt(struct ablkcipher_request *req)
+{
+	return tegra_aes_crypt(req, 0);
+}
+
+static int tegra_aes_cbc_encrypt(struct ablkcipher_request *req)
+{
+	return tegra_aes_crypt(req, FLAGS_ENCRYPT | FLAGS_CBC);
+}
+
+static int tegra_aes_cbc_decrypt(struct ablkcipher_request *req)
+{
+	return tegra_aes_crypt(req, FLAGS_CBC);
+}
+static int tegra_aes_ofb_encrypt(struct ablkcipher_request *req)
+{
+	return tegra_aes_crypt(req, FLAGS_ENCRYPT | FLAGS_OFB);
+}
+
+static int tegra_aes_ofb_decrypt(struct ablkcipher_request *req)
+{
+	return tegra_aes_crypt(req, FLAGS_OFB);
+}
+
+static int tegra_aes_get_random(struct crypto_rng *tfm, u8 *rdata,
+	unsigned int dlen)
+{
+	struct tegra_aes_dev *dd = aes_dev;
+	struct tegra_aes_engine *eng = rng_ctx.eng;
+	unsigned long flags;
+	int ret, i;
+	u8 *dest = rdata, *dt = rng_ctx.dt;
+
+	/* take the hardware semaphore */
+	if (tegra_arb_mutex_lock_timeout(eng->res_id, ARB_SEMA_TIMEOUT) < 0) {
+		dev_err(dd->dev, "aes hardware (%d) not available\n",
+		eng->res_id);
+		return -EBUSY;
+	}
+
+	ret = aes_hw_init(eng);
+	if (ret < 0) {
+		dev_err(dd->dev, "%s: hw init fail(%d)\n", __func__, ret);
+		dlen = ret;
+		goto fail;
+	}
+
+	memset(eng->buf_in, 0, AES_BLOCK_SIZE);
+	memcpy(eng->buf_in, dt, DEFAULT_RNG_BLK_SZ);
+
+	/* sync the buffer for device */
+	dma_sync_single_for_device(dd->dev, eng->dma_buf_in,
+			AES_HW_DMA_BUFFER_SIZE_BYTES, DMA_TO_DEVICE);
+
+	ret = aes_start_crypt(eng, (u32)eng->dma_buf_in, (u32)eng->dma_buf_out,
+		1, FLAGS_ENCRYPT | FLAGS_RNG, true);
+	if (ret < 0) {
+		dev_err(dd->dev, "aes_start_crypt fail(%d)\n", ret);
+		dlen = ret;
+		goto out;
+	}
+	/* sync the buffer for cpu */
+	dma_sync_single_for_cpu(dd->dev, eng->dma_buf_out,
+			AES_HW_DMA_BUFFER_SIZE_BYTES, DMA_FROM_DEVICE);
+
+	memcpy(dest, eng->buf_out, dlen);
+
+	/* update the DT */
+	for (i = DEFAULT_RNG_BLK_SZ - 1; i >= 0; i--) {
+		dt[i] += 1;
+		if (dt[i] != 0)
+			break;
+	}
+
+out:
+	aes_hw_deinit(eng);
+
+	spin_lock_irqsave(&dd->lock, flags);
+	clear_bit(FLAGS_BUSY, &eng->busy);
+	spin_unlock_irqrestore(&dd->lock, flags);
+
+fail:
+	/* release the hardware semaphore */
+	tegra_arb_mutex_unlock(eng->res_id);
+	dev_dbg(dd->dev, "%s: done\n", __func__);
+	return dlen;
+}
+
+static int tegra_aes_rng_reset(struct crypto_rng *tfm, u8 *seed,
+	unsigned int slen)
+{
+	struct tegra_aes_dev *dd = aes_dev;
+	struct tegra_aes_ctx *ctx = &rng_ctx;
+	struct tegra_aes_engine *eng = NULL;
+	struct tegra_aes_slot *key_slot;
+	int bsea_busy = false;
+	unsigned long flags;
+	struct timespec ts;
+	u64 nsec, tmp[2];
+	int ret = 0;
+	u8 *dt;
+
+	if (!dd)
+		return -EINVAL;
+
+	if (slen < (DEFAULT_RNG_BLK_SZ + AES_KEYSIZE_128)) {
+		return -ENOMEM;
+	}
+
+	spin_lock_irqsave(&dd->lock, flags);
+	bsea_busy = test_and_set_bit(FLAGS_BUSY, &dd->bsea.busy);
+	spin_unlock_irqrestore(&dd->lock, flags);
+
+	if (!bsea_busy)
+		eng = &dd->bsea;
+	else
+		return -EBUSY;
+
+	ctx->eng = eng;
+	dd->flags = FLAGS_ENCRYPT | FLAGS_RNG;
+
+	if (!ctx->slot) {
+		key_slot = aes_find_key_slot(dd);
+		if (!key_slot) {
+			dev_err(dd->dev, "no empty slot\n");
+			return -ENOMEM;
+		}
+		ctx->slot = key_slot;
+	}
+
+	/* take the hardware semaphore */
+	if (tegra_arb_mutex_lock_timeout(eng->res_id, ARB_SEMA_TIMEOUT) < 0) {
+		dev_err(dd->dev, "aes hardware (%d) not available\n",
+		eng->res_id);
+		return -EBUSY;
+	}
+
+	ret = aes_hw_init(eng);
+	if (ret < 0) {
+		dev_err(dd->dev, "%s: hw init fail(%d)\n", __func__, ret);
+		goto fail;
+	}
+
+	memcpy(ctx->key, seed + DEFAULT_RNG_BLK_SZ, AES_KEYSIZE_128);
+
+	eng->ctx = ctx;
+	eng->ctx->keylen = AES_KEYSIZE_128;
+	aes_set_key(eng, ctx->slot->slot_num);
+
+	/* set seed to the aes hw slot */
+	memset(eng->buf_in, 0, AES_BLOCK_SIZE);
+	memcpy(eng->buf_in, seed, DEFAULT_RNG_BLK_SZ);
+
+	/* sync the buffer for device */
+	dma_sync_single_for_device(dd->dev, eng->dma_buf_in,
+			AES_HW_DMA_BUFFER_SIZE_BYTES, DMA_TO_DEVICE);
+
+	ret = aes_start_crypt(eng, (u32)eng->dma_buf_in,
+	  (u32)eng->dma_buf_out, 1, FLAGS_CBC, false);
+	if (ret < 0) {
+		dev_err(dd->dev, "aes_start_crypt fail(%d)\n", ret);
+		goto out;
+	}
+	/* sync the buffer for cpu */
+	dma_sync_single_for_cpu(dd->dev, eng->dma_buf_out,
+			AES_HW_DMA_BUFFER_SIZE_BYTES, DMA_FROM_DEVICE);
+
+	if (slen >= (2 * DEFAULT_RNG_BLK_SZ + AES_KEYSIZE_128)) {
+		dt = seed + DEFAULT_RNG_BLK_SZ + AES_KEYSIZE_128;
+	} else {
+		getnstimeofday(&ts);
+		nsec = timespec_to_ns(&ts);
+		do_div(nsec, 1000);
+		nsec ^= dd->ctr << 56;
+		dd->ctr++;
+		tmp[0] = nsec;
+		tmp[1] = tegra_chip_uid();
+		dt = (u8 *)tmp;
+	}
+	memcpy(ctx->dt, dt, DEFAULT_RNG_BLK_SZ);
+
+out:
+	aes_hw_deinit(eng);
+
+fail:
+	/* release the hardware semaphore */
+	tegra_arb_mutex_unlock(eng->res_id);
+
+	dev_dbg(dd->dev, "%s: done\n", __func__);
+	return ret;
+}
+
+static int tegra_aes_cra_init(struct crypto_tfm *tfm)
+{
+	tfm->crt_ablkcipher.reqsize = sizeof(struct tegra_aes_reqctx);
+	return 0;
+}
+
+void tegra_aes_cra_exit(struct crypto_tfm *tfm)
+{
+	struct tegra_aes_ctx *ctx = crypto_ablkcipher_ctx((struct crypto_ablkcipher *)tfm);
+
+	if (ctx && ctx->slot)
+		aes_release_key_slot(ctx);
+}
+
+static struct crypto_alg algs[] = {
+	{
+		.cra_name = "disabled_ecb(aes)",
+		.cra_driver_name = "ecb-aes-tegra",
+		.cra_priority = 100,
+		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+		.cra_blocksize = AES_BLOCK_SIZE,
+		.cra_ctxsize = sizeof(struct tegra_aes_ctx),
+		.cra_alignmask = 3,
+		.cra_type = &crypto_ablkcipher_type,
+		.cra_module = THIS_MODULE,
+		.cra_init = tegra_aes_cra_init,
+		.cra_exit = tegra_aes_cra_exit,
+		.cra_u.ablkcipher = {
+			.min_keysize = AES_MIN_KEY_SIZE,
+			.max_keysize = AES_MAX_KEY_SIZE,
+			.setkey = tegra_aes_setkey,
+			.encrypt = tegra_aes_ecb_encrypt,
+			.decrypt = tegra_aes_ecb_decrypt,
+		},
+	}, {
+		.cra_name = "disabled_cbc(aes)",
+		.cra_driver_name = "cbc-aes-tegra",
+		.cra_priority = 100,
+		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+		.cra_blocksize = AES_BLOCK_SIZE,
+		.cra_ctxsize  = sizeof(struct tegra_aes_ctx),
+		.cra_alignmask = 3,
+		.cra_type = &crypto_ablkcipher_type,
+		.cra_module = THIS_MODULE,
+		.cra_init = tegra_aes_cra_init,
+		.cra_exit = tegra_aes_cra_exit,
+		.cra_u.ablkcipher = {
+			.min_keysize = AES_MIN_KEY_SIZE,
+			.max_keysize = AES_MAX_KEY_SIZE,
+			.ivsize = AES_MIN_KEY_SIZE,
+			.setkey = tegra_aes_setkey,
+			.encrypt = tegra_aes_cbc_encrypt,
+			.decrypt = tegra_aes_cbc_decrypt,
+		}
+	}, {
+		.cra_name = "disabled_ofb(aes)",
+		.cra_driver_name = "ofb-aes-tegra",
+		.cra_priority = 100,
+		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+		.cra_blocksize = AES_BLOCK_SIZE,
+		.cra_ctxsize  = sizeof(struct tegra_aes_ctx),
+		.cra_alignmask = 3,
+		.cra_type = &crypto_ablkcipher_type,
+		.cra_module = THIS_MODULE,
+		.cra_init = tegra_aes_cra_init,
+		.cra_exit = tegra_aes_cra_exit,
+		.cra_u.ablkcipher = {
+			.min_keysize = AES_MIN_KEY_SIZE,
+			.max_keysize = AES_MAX_KEY_SIZE,
+			.ivsize = AES_MIN_KEY_SIZE,
+			.setkey = tegra_aes_setkey,
+			.encrypt = tegra_aes_ofb_encrypt,
+			.decrypt = tegra_aes_ofb_decrypt,
+		}
+	}, {
+		.cra_name = "disabled_ansi_cprng",
+		.cra_driver_name = "rng-aes-tegra",
+		.cra_priority = 100,
+		.cra_flags = CRYPTO_ALG_TYPE_RNG,
+		.cra_ctxsize = sizeof(struct tegra_aes_ctx),
+		.cra_type = &crypto_rng_type,
+		.cra_module = THIS_MODULE,
+		.cra_init = tegra_aes_cra_init,
+		.cra_exit = tegra_aes_cra_exit,
+		.cra_u.rng = {
+			.rng_make_random = tegra_aes_get_random,
+			.rng_reset = tegra_aes_rng_reset,
+			.seedsize = AES_KEYSIZE_128 + (2 * DEFAULT_RNG_BLK_SZ),
+		}
+	}
+};
+
+static int tegra_aes_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct tegra_aes_dev *dd;
+	struct resource *res[2];
+	int err = -ENOMEM, i = 0, j;
+
+	if (aes_dev)
+		return -EEXIST;
+
+	dd = kzalloc(sizeof(struct tegra_aes_dev), GFP_KERNEL);
+	if (dd == NULL) {
+		dev_err(dev, "unable to alloc data struct.\n");
+		return -ENOMEM;;
+	}
+	dd->dev = dev;
+	platform_set_drvdata(pdev, dd);
+
+	dd->slots = kzalloc(sizeof(struct tegra_aes_slot) * AES_NR_KEYSLOTS,
+		GFP_KERNEL);
+	if (dd->slots == NULL) {
+		dev_err(dev, "unable to alloc slot struct.\n");
+		goto out;
+	}
+
+	spin_lock_init(&dd->lock);
+	crypto_init_queue(&dd->queue, TEGRA_AES_QUEUE_LENGTH);
+
+	/* Get the module base address */
+	res[0] = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	res[1] = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!res[0] || !res[1]) {
+		dev_err(dev, "invalid resource type: base\n");
+		err = -ENODEV;
+		goto out;
+	}
+	dd->bsev.phys_base = res[0]->start;
+	dd->bsev.io_base = ioremap(dd->bsev.phys_base, resource_size(res[0]));
+	dd->bsea.phys_base = res[1]->start;
+	dd->bsea.io_base = ioremap(dd->bsea.phys_base, resource_size(res[1]));
+
+	if (!dd->bsev.io_base || !dd->bsea.io_base) {
+		dev_err(dev, "can't ioremap phys_base\n");
+		err = -ENOMEM;
+		goto out;
+	}
+
+	err = alloc_iram(dd);
+	if (err < 0) {
+		dev_err(dev, "Failed to allocate IRAM for BSEA\n");
+		goto out;
+	}
+
+	dd->bsev.res_id = TEGRA_ARB_BSEV;
+	dd->bsea.res_id = TEGRA_ARB_BSEA;
+
+	dd->bsev.pclk = clk_get(dev, "bsev");
+	if (IS_ERR(dd->bsev.pclk)) {
+		dev_err(dev, "v: pclock intialization failed.\n");
+		err = -ENODEV;
+		goto out;
+	}
+
+	dd->bsev.iclk = clk_get(dev, "vde");
+	if (IS_ERR(dd->bsev.iclk)) {
+		dev_err(dev, "v: iclock intialization failed.\n");
+		err = -ENODEV;
+		goto out;
+	}
+
+	dd->bsea.pclk = clk_get(dev, "bsea");
+	if (IS_ERR(dd->bsea.pclk)) {
+		dev_err(dev, "a: pclock intialization failed.\n");
+		err = -ENODEV;
+		goto out;
+	}
+
+	dd->bsea.iclk = clk_get(dev, "sclk");
+	if (IS_ERR(dd->bsea.iclk)) {
+		dev_err(dev, "a: iclock intialization failed.\n");
+		err = -ENODEV;
+		goto out;
+	}
+
+	err = clk_set_rate(dd->bsev.iclk, ULONG_MAX);
+	if (err) {
+		dev_err(dd->dev, "bsev iclk set_rate fail(%d)\n", err);
+		goto out;
+	}
+
+	err = clk_set_rate(dd->bsea.iclk, ULONG_MAX);
+	if (err) {
+		dev_err(dd->dev, "bsea iclk set_rate fail(%d)\n", err);
+		goto out;
+	}
+
+	/*
+	 * the foll contiguous memory is allocated as follows -
+	 * - hardware key table
+	 * - key schedule
+	 */
+	dd->bsea.ivkey_base = NULL;
+	dd->bsev.ivkey_base = dma_alloc_coherent(dev, SZ_512,
+		&dd->bsev.ivkey_phys_base, GFP_KERNEL);
+	if (!dd->bsev.ivkey_base) {
+		dev_err(dev, "can not allocate iv/key buffer for BSEV\n");
+		err = -ENOMEM;
+		goto out;
+	}
+	memset(dd->bsev.ivkey_base, 0, SZ_512);
+
+	dd->bsev.buf_in = dma_alloc_coherent(dev, AES_HW_DMA_BUFFER_SIZE_BYTES,
+		&dd->bsev.dma_buf_in, GFP_KERNEL);
+	dd->bsea.buf_in = dma_alloc_coherent(dev, AES_HW_DMA_BUFFER_SIZE_BYTES,
+		&dd->bsea.dma_buf_in, GFP_KERNEL);
+	if (!dd->bsev.buf_in || !dd->bsea.buf_in) {
+		dev_err(dev, "can not allocate dma-in buffer\n");
+		err = -ENOMEM;
+		goto out;
+	}
+
+	dd->bsev.buf_out = dma_alloc_coherent(dev, AES_HW_DMA_BUFFER_SIZE_BYTES,
+		&dd->bsev.dma_buf_out, GFP_KERNEL);
+	dd->bsea.buf_out = dma_alloc_coherent(dev, AES_HW_DMA_BUFFER_SIZE_BYTES,
+		&dd->bsea.dma_buf_out, GFP_KERNEL);
+	if (!dd->bsev.buf_out || !dd->bsea.buf_out) {
+		dev_err(dev, "can not allocate dma-out buffer\n");
+		err = -ENOMEM;
+		goto out;
+	}
+
+	init_completion(&dd->bsev.op_complete);
+	init_completion(&dd->bsea.op_complete);
+
+	bsev_wq = alloc_workqueue("bsev_wq", WQ_HIGHPRI | WQ_UNBOUND, 1);
+	bsea_wq = alloc_workqueue("bsea_wq", WQ_HIGHPRI | WQ_UNBOUND, 1);
+	if (!bsev_wq || !bsea_wq) {
+		dev_err(dev, "alloc_workqueue failed\n");
+		goto out;
+	}
+
+	/* get the irq */
+	dd->bsev.irq = INT_VDE_BSE_V;
+	err = request_irq(dd->bsev.irq, aes_bsev_irq, IRQF_TRIGGER_HIGH,
+		"tegra-aes", dd);
+	if (err) {
+		dev_err(dev, "request_irq failed fir BSEV Engine\n");
+		goto out;
+	}
+	disable_irq(dd->bsev.irq);
+
+	dd->bsea.irq = INT_VDE_BSE_A;
+	err = request_irq(dd->bsea.irq, aes_bsea_irq, IRQF_TRIGGER_HIGH,
+		"tegra-aes", dd);
+	if (err) {
+		dev_err(dev, "request_irq failed for BSEA Engine\n");
+		goto out;
+	}
+	disable_irq(dd->bsea.irq);
+
+	spin_lock_init(&list_lock);
+	spin_lock(&list_lock);
+	for (i = 0; i < AES_NR_KEYSLOTS; i++) {
+		if (i == SSK_SLOT_NUM)
+			continue;
+		dd->slots[i].available = true;
+		dd->slots[i].slot_num = i;
+		INIT_LIST_HEAD(&dd->slots[i].node);
+		list_add_tail(&dd->slots[i].node, &slot_list);
+	}
+	spin_unlock(&list_lock);
+
+	aes_dev = dd;
+
+	for (i = 0; i < ARRAY_SIZE(algs); i++) {
+		INIT_LIST_HEAD(&algs[i].cra_list);
+		err = crypto_register_alg(&algs[i]);
+		if (err)
+			goto out;
+	}
+
+	dev_info(dev, "registered");
+	return 0;
+
+out:
+	for (j = 0; j < i; j++)
+		crypto_unregister_alg(&algs[j]);
+
+	free_iram(dd);
+
+	if (dd->bsev.ivkey_base) {
+		dma_free_coherent(dev, SZ_512, dd->bsev.ivkey_base,
+			dd->bsev.ivkey_phys_base);
+	}
+
+	if (dd->bsev.buf_in && dd->bsea.buf_in) {
+		dma_free_coherent(dev, AES_HW_DMA_BUFFER_SIZE_BYTES,
+			dd->bsev.buf_in, dd->bsev.dma_buf_in);
+		dma_free_coherent(dev, AES_HW_DMA_BUFFER_SIZE_BYTES,
+			dd->bsea.buf_in, dd->bsea.dma_buf_in);
+	}
+
+	if (dd->bsev.buf_out && dd->bsea.buf_out) {
+		dma_free_coherent(dev, AES_HW_DMA_BUFFER_SIZE_BYTES,
+			dd->bsev.buf_out, dd->bsev.dma_buf_out);
+		dma_free_coherent(dev, AES_HW_DMA_BUFFER_SIZE_BYTES,
+			dd->bsea.buf_out, dd->bsea.dma_buf_out);
+	}
+
+	if (dd->bsev.io_base && dd->bsea.io_base) {
+		iounmap(dd->bsev.io_base);
+		iounmap(dd->bsea.io_base);
+	}
+
+	if (dd->bsev.pclk)
+		clk_put(dd->bsev.pclk);
+
+	if (dd->bsev.iclk)
+		clk_put(dd->bsev.iclk);
+
+	if (dd->bsea.pclk)
+		clk_put(dd->bsea.pclk);
+
+	if (bsev_wq)
+		destroy_workqueue(bsev_wq);
+
+	if (bsea_wq)
+		destroy_workqueue(bsea_wq);
+
+	if (dd->bsev.irq)
+		free_irq(dd->bsev.irq, dd);
+
+	if (dd->bsea.irq)
+		free_irq(dd->bsea.irq, dd);
+
+	spin_lock(&list_lock);
+	list_del(&slot_list);
+	spin_unlock(&list_lock);
+
+	kfree(dd->slots);
+	kfree(dd);
+	aes_dev = NULL;
+
+	dev_err(dev, "%s: initialization failed.\n", __func__);
+	return err;
+}
+
+static int __devexit tegra_aes_remove(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct tegra_aes_dev *dd = platform_get_drvdata(pdev);
+	int i;
+
+	if (!dd)
+		return -ENODEV;
+
+	cancel_work_sync(&bsev_work);
+	cancel_work_sync(&bsea_work);
+	destroy_workqueue(bsev_wq);
+	destroy_workqueue(bsea_wq);
+	free_irq(dd->bsev.irq, dd);
+	free_irq(dd->bsea.irq, dd);
+	spin_lock(&list_lock);
+	list_del(&slot_list);
+	spin_unlock(&list_lock);
+
+	for (i = 0; i < ARRAY_SIZE(algs); i++)
+		crypto_unregister_alg(&algs[i]);
+
+	free_iram(dd);
+	dma_free_coherent(dev, SZ_512, dd->bsev.ivkey_base,
+		dd->bsev.ivkey_phys_base);
+	dma_free_coherent(dev, AES_HW_DMA_BUFFER_SIZE_BYTES, dd->bsev.buf_in,
+		dd->bsev.dma_buf_in);
+	dma_free_coherent(dev, AES_HW_DMA_BUFFER_SIZE_BYTES, dd->bsea.buf_in,
+		dd->bsea.dma_buf_in);
+	dma_free_coherent(dev, AES_HW_DMA_BUFFER_SIZE_BYTES, dd->bsev.buf_out,
+		dd->bsev.dma_buf_out);
+	dma_free_coherent(dev, AES_HW_DMA_BUFFER_SIZE_BYTES, dd->bsea.buf_out,
+		dd->bsea.dma_buf_out);
+
+	iounmap(dd->bsev.io_base);
+	iounmap(dd->bsea.io_base);
+	clk_put(dd->bsev.iclk);
+	clk_put(dd->bsev.pclk);
+	clk_put(dd->bsea.pclk);
+	kfree(dd->slots);
+	kfree(dd);
+	aes_dev = NULL;
+
+	return 0;
+}
+
+static struct platform_driver tegra_aes_driver = {
+	.probe  = tegra_aes_probe,
+	.remove = __devexit_p(tegra_aes_remove),
+	.driver = {
+		.name   = "tegra-aes",
+		.owner  = THIS_MODULE,
+	},
+};
+
+static int __init tegra_aes_mod_init(void)
+{
+	INIT_LIST_HEAD(&slot_list);
+	return  platform_driver_register(&tegra_aes_driver);
+}
+
+static void __exit tegra_aes_mod_exit(void)
+{
+	platform_driver_unregister(&tegra_aes_driver);
+}
+
+module_init(tegra_aes_mod_init);
+module_exit(tegra_aes_mod_exit);
+
+MODULE_DESCRIPTION("Tegra AES hw acceleration support.");
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/crypto/tegra-aes.h b/drivers/crypto/tegra-aes.h
new file mode 100644
index 000000000000..45696467cdfc
--- /dev/null
+++ b/drivers/crypto/tegra-aes.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2010, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __CRYPTODEV_TEGRA_AES_H
+#define __CRYPTODEV_TEGRA_AES_H
+
+#define ICMDQUE_WR		0x1000
+#define CMDQUE_CONTROL		0x1008
+#define INTR_STATUS		0x1018
+#define INT_ENB			0x1040
+#define CONFIG			0x1044
+#define IRAM_ACCESS_CFG		0x10A0
+#define SECURE_DEST_ADDR	0x1100
+#define SECURE_INPUT_SELECT	0x1104
+#define SECURE_CONFIG		0x1108
+#define SECURE_CONFIG_EXT	0x110C
+#define SECURE_SECURITY		0x1110
+#define SECURE_HASH_RESULT0	0x1120
+#define SECURE_HASH_RESULT1	0x1124
+#define SECURE_HASH_RESULT2	0x1128
+#define SECURE_HASH_RESULT3	0x112C
+#define SECURE_SEC_SEL0		0x1140
+#define SECURE_SEC_SEL1		0x1144
+#define SECURE_SEC_SEL2		0x1148
+#define SECURE_SEC_SEL3		0x114C
+#define SECURE_SEC_SEL4		0x1150
+#define SECURE_SEC_SEL5		0x1154
+#define SECURE_SEC_SEL6		0x1158
+#define SECURE_SEC_SEL7		0x115C
+
+/* interrupt status reg masks and shifts */
+#define DMA_BUSY_FIELD		BIT(9)
+#define ICQ_EMPTY_FIELD		BIT(3)
+#define ENGINE_BUSY_FIELD	BIT(0)
+
+/* secure select reg masks and shifts */
+#define SECURE_SEL0_KEYREAD_ENB0_FIELD	BIT(0)
+
+/* secure config ext masks and shifts */
+#define SECURE_KEY_SCH_DIS_FIELD	BIT(15)
+
+/* secure config masks and shifts */
+#define SECURE_KEY_INDEX_SHIFT		20
+#define SECURE_KEY_INDEX_FIELD		(0x1F << SECURE_KEY_INDEX_SHIFT)
+#define SECURE_BLOCK_CNT_FIELD		(0xFFFFF)
+
+/* stream interface select masks and shifts */
+#define CMDQ_CTRL_DST_STM_SEL_FIELD	BIT(5)
+#define CMDQ_CTRL_SRC_STM_SEL_FIELD	BIT(4)
+#define CMDQ_CTRL_ERROR_FLUSH_ENB	BIT(2)
+#define CMDQ_CTRL_ICMDQEN_FIELD		BIT(1)
+#define CMDQ_CTRL_UCMDQEN_FIELD		BIT(0)
+
+/* config regsiter masks and shifts */
+#define CONFIG_ENDIAN_ENB_FIELD		BIT(10)
+#define CONFIG_MODE_SEL_FIELD		BIT(0)
+
+/* extended config */
+#define SECURE_OFFSET_CNT_FIELD		(0xFF << 24)
+#define SECURE_KEYSCHED_GEN_FIELD	BIT(15)
+
+/* init vector select */
+#define SECURE_IV_SELECT_SHIFT		10
+#define SECURE_IV_SELECT_FIELD		BIT(10)
+
+/* secure engine input */
+#define SECURE_INPUT_ALG_SEL_SHIFT	28
+#define SECURE_INPUT_ALG_SEL_FIELD	(0xF << SECURE_INPUT_ALG_SEL_SHIFT)
+#define SECURE_INPUT_KEY_LEN_SHIFT	16
+#define SECURE_INPUT_KEY_LEN_FIELD	(0xFFF << SECURE_INPUT_KEY_LEN_SHIFT)
+#define SECURE_RNG_ENB_SHIFT		11
+#define SECURE_RNG_ENB_FIELD		BIT(11)
+#define SECURE_CORE_SEL_SHIFT		9
+#define SECURE_CORE_SEL_FIELD		BIT(9)
+#define SECURE_VCTRAM_SEL_SHIFT		7
+#define SECURE_VCTRAM_SEL_FIELD		(0x3 << SECURE_VCTRAM_SEL_SHIFT)
+#define SECURE_INPUT_SEL_SHIFT		5
+#define SECURE_INPUT_SEL_FIELD		(0x3 << SECURE_INPUT_SEL_SHIFT)
+#define SECURE_XOR_POS_SHIFT		3
+#define SECURE_XOR_POS_FIELD		(0x3 << SECURE_XOR_POS_SHIFT)
+#define SECURE_HASH_ENB_SHIFT		2
+#define SECURE_HASH_ENB_FIELD		BIT(2)
+#define SECURE_ON_THE_FLY_FIELD		BIT(0)
+
+#endif
diff --git a/drivers/crypto/tegra-se.c b/drivers/crypto/tegra-se.c
new file mode 100644
index 000000000000..c03065356984
--- /dev/null
+++ b/drivers/crypto/tegra-se.c
@@ -0,0 +1,2458 @@
+/*
+ * Cryptographic API.
+ * drivers/crypto/tegra-se.c
+ *
+ * Support for Tegra Security Engine hardware crypto algorithms.
+ *
+ * Copyright (c) 2011-2012, NVIDIA 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/scatterlist.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+#include <linux/mutex.h>
+#include <linux/interrupt.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <crypto/scatterwalk.h>
+#include <crypto/algapi.h>
+#include <crypto/aes.h>
+#include <crypto/internal/rng.h>
+#include <crypto/internal/hash.h>
+#include <crypto/sha.h>
+#include <linux/pm_runtime.h>
+
+#include "tegra-se.h"
+
+#define DRIVER_NAME	"tegra-se"
+
+/* Security Engine operation modes */
+enum tegra_se_aes_op_mode {
+	SE_AES_OP_MODE_CBC,	/* Cipher Block Chaining (CBC) mode */
+	SE_AES_OP_MODE_ECB,	/* Electronic Codebook (ECB) mode */
+	SE_AES_OP_MODE_CTR,	/* Counter (CTR) mode */
+	SE_AES_OP_MODE_OFB,	/* Output feedback (CFB) mode */
+	SE_AES_OP_MODE_RNG_X931,	/* Random number generator (RNG) mode */
+	SE_AES_OP_MODE_CMAC,	/* Cipher-based MAC (CMAC) mode */
+	SE_AES_OP_MODE_SHA1,	/* Secure Hash Algorithm-1 (SHA1) mode */
+	SE_AES_OP_MODE_SHA224,	/* Secure Hash Algorithm-224  (SHA224) mode */
+	SE_AES_OP_MODE_SHA256,	/* Secure Hash Algorithm-256  (SHA256) mode */
+	SE_AES_OP_MODE_SHA384,	/* Secure Hash Algorithm-384  (SHA384) mode */
+	SE_AES_OP_MODE_SHA512	/* Secure Hash Algorithm-512  (SHA512) mode */
+};
+
+/* Security Engine key table type */
+enum tegra_se_key_table_type {
+	SE_KEY_TABLE_TYPE_KEY,	/* Key */
+	SE_KEY_TABLE_TYPE_ORGIV,	/* Original IV */
+	SE_KEY_TABLE_TYPE_UPDTDIV	/* Updated IV */
+};
+
+/* Security Engine request context */
+struct tegra_se_req_context {
+	enum tegra_se_aes_op_mode op_mode; /* Security Engine operation mode */
+	bool encrypt;	/* Operation type */
+};
+
+struct tegra_se_dev {
+	struct device *dev;
+	void __iomem *io_reg;	/* se device memory/io */
+	void __iomem *pmc_io_reg;	/* pmc device memory/io */
+	int irq;	/* irq allocated */
+	spinlock_t lock;	/* spin lock */
+	struct clk *pclk;	/* Security Engine clock */
+	struct crypto_queue queue; /* Security Engine crypto queue */
+	struct tegra_se_slot *slot_list;	/* pointer to key slots */
+	u64 ctr;
+	u32 *src_ll_buf;	/* pointer to source linked list buffer */
+	dma_addr_t src_ll_buf_adr; /* Source linked list buffer dma address */
+	u32 src_ll_size;	/* Size of source linked list buffer */
+	u32 *dst_ll_buf;	/* pointer to destination linked list buffer */
+	dma_addr_t dst_ll_buf_adr; /* Destination linked list dma address */
+	u32 dst_ll_size;	/* Size of destination linked list buffer */
+	u32 *ctx_save_buf;	/* LP context buffer pointer*/
+	dma_addr_t ctx_save_buf_adr;	/* LP context buffer dma address*/
+	struct completion complete;	/* Tells the task completion */
+	bool work_q_busy;	/* Work queue busy status */
+};
+
+static struct tegra_se_dev *sg_tegra_se_dev;
+
+/* Security Engine AES context */
+struct tegra_se_aes_context {
+	struct tegra_se_dev *se_dev;	/* Security Engine device */
+	struct tegra_se_slot *slot;	/* Security Engine key slot */
+	u32 keylen;	/* key length in bits */
+	u32 op_mode;	/* AES operation mode */
+};
+
+/* Security Engine random number generator context */
+struct tegra_se_rng_context {
+	struct tegra_se_dev *se_dev;	/* Security Engine device */
+	struct tegra_se_slot *slot;	/* Security Engine key slot */
+	u32 *dt_buf;	/* Destination buffer pointer */
+	dma_addr_t dt_buf_adr;	/* Destination buffer dma address */
+	u32 *rng_buf;	/* RNG buffer pointer */
+	dma_addr_t rng_buf_adr;	/* RNG buffer dma address */
+	bool use_org_iv;	/* Tells whether original IV is be used
+				or not. If it is false updated IV is used*/
+};
+
+/* Security Engine SHA context */
+struct tegra_se_sha_context {
+	struct tegra_se_dev	*se_dev;	/* Security Engine device */
+	u32 op_mode;	/* SHA operation mode */
+};
+
+/* Security Engine AES CMAC context */
+struct tegra_se_aes_cmac_context {
+	struct tegra_se_dev *se_dev;	/* Security Engine device */
+	struct tegra_se_slot *slot;	/* Security Engine key slot */
+	u32 keylen;	/* key length in bits */
+	u8 K1[TEGRA_SE_KEY_128_SIZE];	/* Key1 */
+	u8 K2[TEGRA_SE_KEY_128_SIZE];	/* Key2 */
+	dma_addr_t dma_addr;	/* DMA address of local buffer */
+	u32 buflen;	/* local buffer length */
+	u8	*buffer;	/* local buffer pointer */
+};
+
+/* Security Engine key slot */
+struct tegra_se_slot {
+	struct list_head node;
+	u8 slot_num;	/* Key slot number */
+	bool available; /* Tells whether key slot is free to use */
+};
+
+static struct tegra_se_slot ssk_slot = {
+	.slot_num = 15,
+	.available = false,
+};
+
+static struct tegra_se_slot srk_slot = {
+	.slot_num = 0,
+	.available = false,
+};
+
+/* Security Engine Linked List */
+struct tegra_se_ll {
+	dma_addr_t addr; /* DMA buffer address */
+	u32 data_len; /* Data length in DMA buffer */
+};
+
+static LIST_HEAD(key_slot);
+static DEFINE_SPINLOCK(key_slot_lock);
+static DEFINE_MUTEX(se_hw_lock);
+
+/* create a work for handling the async transfers */
+static void tegra_se_work_handler(struct work_struct *work);
+static DECLARE_WORK(se_work, tegra_se_work_handler);
+static struct workqueue_struct *se_work_q;
+
+#define PMC_SCRATCH43_REG_OFFSET 0x22c
+#define GET_MSB(x)  ((x) >> (8*sizeof(x)-1))
+static void tegra_se_leftshift_onebit(u8 *in_buf, u32 size, u8 *org_msb)
+{
+	u8 carry;
+	u32 i;
+
+	*org_msb = GET_MSB(in_buf[0]);
+
+	/* left shift one bit */
+	in_buf[0] <<= 1;
+	for (carry = 0, i = 1; i < size; i++) {
+		carry = GET_MSB(in_buf[i]);
+		in_buf[i-1] |= carry;
+		in_buf[i] <<= 1;
+	}
+}
+
+extern unsigned long long tegra_chip_uid(void);
+
+static inline void se_writel(struct tegra_se_dev *se_dev,
+	unsigned int val, unsigned int reg_offset)
+{
+	writel(val, se_dev->io_reg + reg_offset);
+}
+
+static inline unsigned int se_readl(struct tegra_se_dev *se_dev,
+	unsigned int reg_offset)
+{
+	unsigned int val;
+
+	val = readl(se_dev->io_reg + reg_offset);
+
+	return val;
+}
+
+static void tegra_se_free_key_slot(struct tegra_se_slot *slot)
+{
+	if (slot) {
+		spin_lock(&key_slot_lock);
+		slot->available = true;
+		spin_unlock(&key_slot_lock);
+	}
+}
+
+static struct tegra_se_slot *tegra_se_alloc_key_slot(void)
+{
+	struct tegra_se_slot *slot = NULL;
+	bool found = false;
+
+	spin_lock(&key_slot_lock);
+	list_for_each_entry(slot, &key_slot, node) {
+		if (slot->available) {
+			slot->available = false;
+			found = true;
+			break;
+		}
+	}
+	spin_unlock(&key_slot_lock);
+	return found ? slot : NULL;
+}
+
+static int tegra_init_key_slot(struct tegra_se_dev *se_dev)
+{
+	int i;
+
+	se_dev->slot_list = kzalloc(sizeof(struct tegra_se_slot) *
+					TEGRA_SE_KEYSLOT_COUNT, GFP_KERNEL);
+	if (se_dev->slot_list == NULL) {
+		dev_err(se_dev->dev, "slot list memory allocation failed\n");
+		return -ENOMEM;
+	}
+	spin_lock_init(&key_slot_lock);
+	spin_lock(&key_slot_lock);
+	for (i = 0; i < TEGRA_SE_KEYSLOT_COUNT; i++) {
+		/*
+		 * Slot 0 and 15 are reserved and will not be added to the
+		 * free slots pool. Slot 0 is used for SRK generation and
+		 * Slot 15 is used for SSK operation
+		 */
+		if ((i == srk_slot.slot_num) || (i == ssk_slot.slot_num))
+			continue;
+		se_dev->slot_list[i].available = true;
+		se_dev->slot_list[i].slot_num = i;
+		INIT_LIST_HEAD(&se_dev->slot_list[i].node);
+		list_add_tail(&se_dev->slot_list[i].node, &key_slot);
+	}
+	spin_unlock(&key_slot_lock);
+
+	return 0;
+}
+
+static void tegra_se_key_read_disable(u8 slot_num)
+{
+	struct tegra_se_dev *se_dev = sg_tegra_se_dev;
+	u32 val;
+
+	val = se_readl(se_dev,
+			(SE_KEY_TABLE_ACCESS_REG_OFFSET + (slot_num * 4)));
+	val &= ~(1 << SE_KEY_READ_DISABLE_SHIFT);
+	se_writel(se_dev,
+		val, (SE_KEY_TABLE_ACCESS_REG_OFFSET + (slot_num * 4)));
+}
+
+static void tegra_se_key_read_disable_all(void)
+{
+	struct tegra_se_dev *se_dev = sg_tegra_se_dev;
+	u8 slot_num;
+
+	mutex_lock(&se_hw_lock);
+	pm_runtime_get_sync(se_dev->dev);
+
+	for (slot_num = 0; slot_num < TEGRA_SE_KEYSLOT_COUNT; slot_num++)
+		tegra_se_key_read_disable(slot_num);
+
+	pm_runtime_put(se_dev->dev);
+	mutex_unlock(&se_hw_lock);
+}
+
+static void tegra_se_config_algo(struct tegra_se_dev *se_dev,
+	enum tegra_se_aes_op_mode mode, bool encrypt, u32 key_len)
+{
+	u32 val = 0;
+
+	switch (mode) {
+	case SE_AES_OP_MODE_CBC:
+	case SE_AES_OP_MODE_CMAC:
+		if (encrypt) {
+			val = SE_CONFIG_ENC_ALG(ALG_AES_ENC);
+			if (key_len == TEGRA_SE_KEY_256_SIZE)
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY256);
+			else if (key_len == TEGRA_SE_KEY_192_SIZE)
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY192);
+			else
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY128);
+			val |= SE_CONFIG_DEC_ALG(ALG_NOP);
+		} else {
+			val = SE_CONFIG_DEC_ALG(ALG_AES_DEC);
+			if (key_len == TEGRA_SE_KEY_256_SIZE)
+				val |= SE_CONFIG_DEC_MODE(MODE_KEY256);
+			else if (key_len == TEGRA_SE_KEY_192_SIZE)
+				val |= SE_CONFIG_DEC_MODE(MODE_KEY192);
+			else
+				val |= SE_CONFIG_DEC_MODE(MODE_KEY128);
+		}
+		if (mode == SE_AES_OP_MODE_CMAC)
+			val |= SE_CONFIG_DST(DST_HASHREG);
+		else
+			val |= SE_CONFIG_DST(DST_MEMORY);
+		break;
+	case SE_AES_OP_MODE_RNG_X931:
+		val = SE_CONFIG_ENC_ALG(ALG_RNG) |
+			SE_CONFIG_ENC_MODE(MODE_KEY128) |
+				SE_CONFIG_DST(DST_MEMORY);
+		break;
+	case SE_AES_OP_MODE_ECB:
+		if (encrypt) {
+			val = SE_CONFIG_ENC_ALG(ALG_AES_ENC);
+			if (key_len == TEGRA_SE_KEY_256_SIZE)
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY256);
+			else if (key_len == TEGRA_SE_KEY_192_SIZE)
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY192);
+			else
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY128);
+		} else {
+			val = SE_CONFIG_DEC_ALG(ALG_AES_DEC);
+			if (key_len == TEGRA_SE_KEY_256_SIZE)
+				val |= SE_CONFIG_DEC_MODE(MODE_KEY256);
+			else if (key_len == TEGRA_SE_KEY_192_SIZE)
+				val |= SE_CONFIG_DEC_MODE(MODE_KEY192);
+			else
+				val |= SE_CONFIG_DEC_MODE(MODE_KEY128);
+		}
+		val |= SE_CONFIG_DST(DST_MEMORY);
+		break;
+	case SE_AES_OP_MODE_CTR:
+		if (encrypt) {
+			val = SE_CONFIG_ENC_ALG(ALG_AES_ENC);
+			if (key_len == TEGRA_SE_KEY_256_SIZE)
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY256);
+			else if (key_len == TEGRA_SE_KEY_192_SIZE)
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY192);
+			else
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY128);
+		} else {
+			val = SE_CONFIG_DEC_ALG(ALG_AES_DEC);
+			if (key_len == TEGRA_SE_KEY_256_SIZE) {
+				val |= SE_CONFIG_DEC_MODE(MODE_KEY256);
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY256);
+			} else if (key_len == TEGRA_SE_KEY_192_SIZE) {
+				val |= SE_CONFIG_DEC_MODE(MODE_KEY192);
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY192);
+			} else {
+				val |= SE_CONFIG_DEC_MODE(MODE_KEY128);
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY128);
+			}
+		}
+		val |= SE_CONFIG_DST(DST_MEMORY);
+		break;
+	case SE_AES_OP_MODE_OFB:
+		if (encrypt) {
+			val = SE_CONFIG_ENC_ALG(ALG_AES_ENC);
+			if (key_len == TEGRA_SE_KEY_256_SIZE)
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY256);
+			else if (key_len == TEGRA_SE_KEY_192_SIZE)
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY192);
+			else
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY128);
+		} else {
+			val = SE_CONFIG_DEC_ALG(ALG_AES_DEC);
+			if (key_len == TEGRA_SE_KEY_256_SIZE) {
+				val |= SE_CONFIG_DEC_MODE(MODE_KEY256);
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY256);
+			} else if (key_len == TEGRA_SE_KEY_192_SIZE) {
+				val |= SE_CONFIG_DEC_MODE(MODE_KEY192);
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY192);
+			} else {
+				val |= SE_CONFIG_DEC_MODE(MODE_KEY128);
+				val |= SE_CONFIG_ENC_MODE(MODE_KEY128);
+			}
+		}
+		val |= SE_CONFIG_DST(DST_MEMORY);
+		break;
+	case SE_AES_OP_MODE_SHA1:
+		val = SE_CONFIG_ENC_ALG(ALG_SHA) |
+			SE_CONFIG_ENC_MODE(MODE_SHA1) |
+				SE_CONFIG_DST(DST_HASHREG);
+		break;
+	case SE_AES_OP_MODE_SHA224:
+		val = SE_CONFIG_ENC_ALG(ALG_SHA) |
+			SE_CONFIG_ENC_MODE(MODE_SHA224) |
+				SE_CONFIG_DST(DST_HASHREG);
+		break;
+	case SE_AES_OP_MODE_SHA256:
+		val = SE_CONFIG_ENC_ALG(ALG_SHA) |
+			SE_CONFIG_ENC_MODE(MODE_SHA256) |
+				SE_CONFIG_DST(DST_HASHREG);
+		break;
+	case SE_AES_OP_MODE_SHA384:
+		val = SE_CONFIG_ENC_ALG(ALG_SHA) |
+			SE_CONFIG_ENC_MODE(MODE_SHA384) |
+				SE_CONFIG_DST(DST_HASHREG);
+		break;
+	case SE_AES_OP_MODE_SHA512:
+		val = SE_CONFIG_ENC_ALG(ALG_SHA) |
+			SE_CONFIG_ENC_MODE(MODE_SHA512) |
+				SE_CONFIG_DST(DST_HASHREG);
+		break;
+	default:
+		dev_warn(se_dev->dev, "Invalid operation mode\n");
+		break;
+	}
+
+	se_writel(se_dev, val, SE_CONFIG_REG_OFFSET);
+}
+
+static void tegra_se_write_seed(struct tegra_se_dev *se_dev, u32 *pdata)
+{
+	u32 i;
+
+	for (i = 0; i < SE_CRYPTO_CTR_REG_COUNT; i++)
+		se_writel(se_dev, pdata[i], SE_CRYPTO_CTR_REG_OFFSET + (i * 4));
+}
+
+static void tegra_se_write_key_table(u8 *pdata, u32 data_len,
+	u8 slot_num, enum tegra_se_key_table_type type)
+{
+	struct tegra_se_dev *se_dev = sg_tegra_se_dev;
+	u32 data_size = SE_KEYTABLE_REG_MAX_DATA;
+	u32 *pdata_buf = (u32 *)pdata;
+	u8 pkt = 0, quad = 0;
+	u32 val = 0, i;
+
+	if ((type == SE_KEY_TABLE_TYPE_KEY) && (slot_num == ssk_slot.slot_num))
+		return;
+
+	if (type == SE_KEY_TABLE_TYPE_ORGIV)
+		quad = QUAD_ORG_IV;
+	else if (type == SE_KEY_TABLE_TYPE_UPDTDIV)
+		quad = QUAD_UPDTD_IV;
+	else
+		quad = QUAD_KEYS_128;
+
+	/* write data to the key table */
+	do {
+		for (i = 0; i < data_size; i += 4, data_len -= 4)
+			se_writel(se_dev, *pdata_buf++,
+				SE_KEYTABLE_DATA0_REG_OFFSET + i);
+
+		pkt = SE_KEYTABLE_SLOT(slot_num) | SE_KEYTABLE_QUAD(quad);
+		val = SE_KEYTABLE_OP_TYPE(OP_WRITE) |
+			SE_KEYTABLE_TABLE_SEL(TABLE_KEYIV) |
+				SE_KEYTABLE_PKT(pkt);
+
+		se_writel(se_dev, val, SE_KEYTABLE_REG_OFFSET);
+
+		data_size = data_len;
+		quad = QUAD_KEYS_256;
+
+	} while (data_len);
+}
+
+static void tegra_se_config_crypto(struct tegra_se_dev *se_dev,
+	enum tegra_se_aes_op_mode mode, bool encrypt, u8 slot_num, bool org_iv)
+{
+	u32 val = 0;
+
+	switch (mode) {
+	case SE_AES_OP_MODE_CMAC:
+	case SE_AES_OP_MODE_CBC:
+		if (encrypt) {
+			val = SE_CRYPTO_INPUT_SEL(INPUT_AHB) |
+				SE_CRYPTO_VCTRAM_SEL(VCTRAM_AESOUT) |
+				SE_CRYPTO_XOR_POS(XOR_TOP) |
+				SE_CRYPTO_CORE_SEL(CORE_ENCRYPT);
+		} else {
+			val = SE_CRYPTO_INPUT_SEL(INPUT_AHB) |
+				SE_CRYPTO_VCTRAM_SEL(VCTRAM_PREVAHB) |
+				SE_CRYPTO_XOR_POS(XOR_BOTTOM) |
+				SE_CRYPTO_CORE_SEL(CORE_DECRYPT);
+		}
+		break;
+	case SE_AES_OP_MODE_RNG_X931:
+		val = SE_CRYPTO_INPUT_SEL(INPUT_AHB) |
+			SE_CRYPTO_XOR_POS(XOR_BYPASS) |
+			SE_CRYPTO_CORE_SEL(CORE_ENCRYPT);
+		break;
+	case SE_AES_OP_MODE_ECB:
+		if (encrypt) {
+			val = SE_CRYPTO_INPUT_SEL(INPUT_AHB) |
+				SE_CRYPTO_XOR_POS(XOR_BYPASS) |
+				SE_CRYPTO_CORE_SEL(CORE_ENCRYPT);
+		} else {
+			val = SE_CRYPTO_INPUT_SEL(INPUT_AHB) |
+				SE_CRYPTO_XOR_POS(XOR_BYPASS) |
+				SE_CRYPTO_CORE_SEL(CORE_DECRYPT);
+		}
+		break;
+	case SE_AES_OP_MODE_CTR:
+		val = SE_CRYPTO_INPUT_SEL(INPUT_LNR_CTR) |
+			SE_CRYPTO_VCTRAM_SEL(VCTRAM_AHB) |
+			SE_CRYPTO_XOR_POS(XOR_BOTTOM) |
+			SE_CRYPTO_CORE_SEL(CORE_ENCRYPT);
+		break;
+	case SE_AES_OP_MODE_OFB:
+		val = SE_CRYPTO_INPUT_SEL(INPUT_AESOUT) |
+			SE_CRYPTO_VCTRAM_SEL(VCTRAM_AHB) |
+			SE_CRYPTO_XOR_POS(XOR_BOTTOM) |
+			SE_CRYPTO_CORE_SEL(CORE_ENCRYPT);
+		break;
+	default:
+		dev_warn(se_dev->dev, "Invalid operation mode\n");
+		break;
+	}
+
+	if (mode == SE_AES_OP_MODE_CTR) {
+		val |= SE_CRYPTO_HASH(HASH_DISABLE) |
+			SE_CRYPTO_KEY_INDEX(slot_num) |
+			SE_CRYPTO_CTR_CNTN(1);
+	} else {
+		val |= SE_CRYPTO_HASH(HASH_DISABLE) |
+			SE_CRYPTO_KEY_INDEX(slot_num) |
+			(org_iv ? SE_CRYPTO_IV_SEL(IV_ORIGINAL) :
+			SE_CRYPTO_IV_SEL(IV_UPDATED));
+	}
+
+	/* enable hash for CMAC */
+	if (mode == SE_AES_OP_MODE_CMAC)
+		val |= SE_CRYPTO_HASH(HASH_ENABLE);
+
+	se_writel(se_dev, val, SE_CRYPTO_REG_OFFSET);
+
+	if (mode == SE_AES_OP_MODE_CTR)
+		se_writel(se_dev, 1, SE_SPARE_0_REG_OFFSET);
+
+	if (mode == SE_AES_OP_MODE_OFB)
+		se_writel(se_dev, 1, SE_SPARE_0_REG_OFFSET);
+
+}
+
+static void tegra_se_config_sha(struct tegra_se_dev *se_dev, u32 count)
+{
+	int i;
+
+	se_writel(se_dev, (count * 8), SE_SHA_MSG_LENGTH_REG_OFFSET);
+	se_writel(se_dev, (count * 8), SE_SHA_MSG_LEFT_REG_OFFSET);
+	for (i = 1; i < 4; i++) {
+		se_writel(se_dev, 0, SE_SHA_MSG_LENGTH_REG_OFFSET + (4 * i));
+		se_writel(se_dev, 0, SE_SHA_MSG_LEFT_REG_OFFSET + (4 * i));
+	}
+	se_writel(se_dev, SHA_ENABLE, SE_SHA_CONFIG_REG_OFFSET);
+}
+
+static int tegra_se_start_operation(struct tegra_se_dev *se_dev, u32 nbytes,
+	bool context_save)
+{
+	u32 nblocks = nbytes / TEGRA_SE_AES_BLOCK_SIZE;
+	int ret = 0;
+	u32 val = 0;
+
+	/* clear any pending interrupts */
+	val = se_readl(se_dev, SE_INT_STATUS_REG_OFFSET);
+	se_writel(se_dev, val, SE_INT_STATUS_REG_OFFSET);
+	se_writel(se_dev, se_dev->src_ll_buf_adr, SE_IN_LL_ADDR_REG_OFFSET);
+	se_writel(se_dev, se_dev->dst_ll_buf_adr, SE_OUT_LL_ADDR_REG_OFFSET);
+
+	if (nblocks)
+		se_writel(se_dev, nblocks-1, SE_BLOCK_COUNT_REG_OFFSET);
+
+	/* enable interupts */
+	val = SE_INT_ERROR(INT_ENABLE) | SE_INT_OP_DONE(INT_ENABLE);
+	se_writel(se_dev, val, SE_INT_ENABLE_REG_OFFSET);
+
+	INIT_COMPLETION(se_dev->complete);
+
+	if (context_save)
+		se_writel(se_dev, SE_OPERATION(OP_CTX_SAVE),
+			SE_OPERATION_REG_OFFSET);
+	else
+		se_writel(se_dev, SE_OPERATION(OP_SRART),
+			SE_OPERATION_REG_OFFSET);
+
+	ret = wait_for_completion_timeout(&se_dev->complete,
+			msecs_to_jiffies(1000));
+	if (ret == 0) {
+		dev_err(se_dev->dev, "operation timed out no interrupt\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static void tegra_se_read_hash_result(struct tegra_se_dev *se_dev,
+	u8 *pdata, u32 nbytes, bool swap32)
+{
+	u32 *result = (u32 *)pdata;
+	u32 i;
+
+	for (i = 0; i < nbytes/4; i++) {
+		result[i] = se_readl(se_dev, SE_HASH_RESULT_REG_OFFSET +
+				(i * sizeof(u32)));
+		if (swap32)
+			result[i] = be32_to_cpu(result[i]);
+	}
+}
+
+static int tegra_se_count_sgs(struct scatterlist *sl, u32 total_bytes)
+{
+	int i = 0;
+
+	if (!total_bytes)
+		return 0;
+
+	do {
+		if (!sl->length)
+			return 0;
+		total_bytes -= min(sl->length, total_bytes);
+		i++;
+		sl = sg_next(sl);
+	} while (total_bytes && sl);
+
+	return i;
+}
+
+static int tegra_se_alloc_ll_buf(struct tegra_se_dev *se_dev,
+	u32 num_src_sgs, u32 num_dst_sgs)
+{
+	if (se_dev->src_ll_buf || se_dev->dst_ll_buf) {
+		dev_err(se_dev->dev, "trying to allocate memory to allocated memory\n");
+		return -EBUSY;
+	}
+
+	if (num_src_sgs) {
+		se_dev->src_ll_size =
+			(sizeof(struct tegra_se_ll) * num_src_sgs) +
+				sizeof(u32);
+		se_dev->src_ll_buf = dma_alloc_coherent(se_dev->dev,
+					se_dev->src_ll_size,
+					&se_dev->src_ll_buf_adr, GFP_KERNEL);
+		if (!se_dev->src_ll_buf) {
+			dev_err(se_dev->dev, "can not allocate src lldma buffer\n");
+			return -ENOMEM;
+		}
+	}
+	if (num_dst_sgs) {
+		se_dev->dst_ll_size =
+				(sizeof(struct tegra_se_ll) * num_dst_sgs) +
+					sizeof(u32);
+		se_dev->dst_ll_buf = dma_alloc_coherent(se_dev->dev,
+					se_dev->dst_ll_size,
+					&se_dev->dst_ll_buf_adr, GFP_KERNEL);
+		if (!se_dev->dst_ll_buf) {
+			dev_err(se_dev->dev, "can not allocate dst ll dma buffer\n");
+			return -ENOMEM;
+		}
+	}
+
+	return 0;
+}
+
+static void tegra_se_free_ll_buf(struct tegra_se_dev *se_dev)
+{
+	if (se_dev->src_ll_buf) {
+		dma_free_coherent(se_dev->dev, se_dev->src_ll_size,
+			se_dev->src_ll_buf, se_dev->src_ll_buf_adr);
+		se_dev->src_ll_buf = NULL;
+	}
+
+	if (se_dev->dst_ll_buf) {
+		dma_free_coherent(se_dev->dev, se_dev->dst_ll_size,
+			se_dev->dst_ll_buf, se_dev->dst_ll_buf_adr);
+		se_dev->dst_ll_buf = NULL;
+	}
+}
+
+static int tegra_se_setup_ablk_req(struct tegra_se_dev *se_dev,
+	struct ablkcipher_request *req)
+{
+	struct scatterlist *src_sg, *dst_sg;
+	struct tegra_se_ll *src_ll, *dst_ll;
+	u32 total, num_src_sgs, num_dst_sgs;
+	int ret = 0;
+
+	num_src_sgs = tegra_se_count_sgs(req->src, req->nbytes);
+	num_dst_sgs = tegra_se_count_sgs(req->dst, req->nbytes);
+
+	if ((num_src_sgs > SE_MAX_SRC_SG_COUNT) ||
+		(num_dst_sgs > SE_MAX_DST_SG_COUNT)) {
+			dev_err(se_dev->dev, "num of SG buffers are more\n");
+			return -EINVAL;
+	}
+
+	*se_dev->src_ll_buf = num_src_sgs-1;
+	*se_dev->dst_ll_buf = num_dst_sgs-1;
+
+	src_ll = (struct tegra_se_ll *)(se_dev->src_ll_buf + 1);
+	dst_ll = (struct tegra_se_ll *)(se_dev->dst_ll_buf + 1);
+
+	src_sg = req->src;
+	dst_sg = req->dst;
+	total = req->nbytes;
+
+	while (total) {
+		ret = dma_map_sg(se_dev->dev, src_sg, 1, DMA_TO_DEVICE);
+		if (!ret) {
+			dev_err(se_dev->dev, "dma_map_sg() error\n");
+			return -EINVAL;
+		}
+
+		ret = dma_map_sg(se_dev->dev, dst_sg, 1, DMA_FROM_DEVICE);
+		if (!ret) {
+			dev_err(se_dev->dev, "dma_map_sg() error\n");
+			dma_unmap_sg(se_dev->dev, src_sg, 1, DMA_TO_DEVICE);
+			return -EINVAL;
+		}
+
+		WARN_ON(src_sg->length != dst_sg->length);
+		src_ll->addr = sg_dma_address(src_sg);
+		src_ll->data_len = min(src_sg->length, total);
+		dst_ll->addr = sg_dma_address(dst_sg);
+		dst_ll->data_len = min(dst_sg->length, total);
+		total -= min(src_sg->length, total);
+
+		src_sg = sg_next(src_sg);
+		dst_sg = sg_next(dst_sg);
+		dst_ll++;
+		src_ll++;
+		WARN_ON(((total != 0) && (!src_sg || !dst_sg)));
+	}
+	return ret;
+}
+
+static void tegra_se_dequeue_complete_req(struct tegra_se_dev *se_dev,
+	struct ablkcipher_request *req)
+{
+	struct scatterlist *src_sg, *dst_sg;
+	u32 total;
+
+	if (req) {
+		src_sg = req->src;
+		dst_sg = req->dst;
+		total = req->nbytes;
+		while (total) {
+			dma_unmap_sg(se_dev->dev, dst_sg, 1, DMA_FROM_DEVICE);
+			dma_unmap_sg(se_dev->dev, src_sg, 1, DMA_TO_DEVICE);
+			total -= min(src_sg->length, total);
+			src_sg = sg_next(src_sg);
+			dst_sg = sg_next(dst_sg);
+		}
+	}
+}
+
+static void tegra_se_process_new_req(struct crypto_async_request *async_req)
+{
+	struct tegra_se_dev *se_dev = sg_tegra_se_dev;
+	struct ablkcipher_request *req = ablkcipher_request_cast(async_req);
+	struct tegra_se_req_context *req_ctx = ablkcipher_request_ctx(req);
+	struct tegra_se_aes_context *aes_ctx =
+			crypto_ablkcipher_ctx(crypto_ablkcipher_reqtfm(req));
+	int ret = 0;
+
+	/* take access to the hw */
+	mutex_lock(&se_hw_lock);
+
+	/* write IV */
+	if (req->info) {
+		if (req_ctx->op_mode == SE_AES_OP_MODE_CTR) {
+			tegra_se_write_seed(se_dev, (u32 *)req->info);
+		} else {
+			tegra_se_write_key_table(req->info,
+				TEGRA_SE_AES_IV_SIZE,
+				aes_ctx->slot->slot_num,
+				SE_KEY_TABLE_TYPE_ORGIV);
+		}
+	}
+	tegra_se_setup_ablk_req(se_dev, req);
+	tegra_se_config_algo(se_dev, req_ctx->op_mode, req_ctx->encrypt,
+		aes_ctx->keylen);
+	tegra_se_config_crypto(se_dev, req_ctx->op_mode, req_ctx->encrypt,
+			aes_ctx->slot->slot_num, req->info ? true : false);
+	ret = tegra_se_start_operation(se_dev, req->nbytes, false);
+	tegra_se_dequeue_complete_req(se_dev, req);
+
+	mutex_unlock(&se_hw_lock);
+	req->base.complete(&req->base, ret);
+}
+
+static irqreturn_t tegra_se_irq(int irq, void *dev)
+{
+	struct tegra_se_dev *se_dev = dev;
+	u32 val;
+
+	val = se_readl(se_dev, SE_INT_STATUS_REG_OFFSET);
+	se_writel(se_dev, val, SE_INT_STATUS_REG_OFFSET);
+
+	if (val & SE_INT_ERROR(INT_SET))
+		dev_err(se_dev->dev, "tegra_se_irq::error");
+
+	if (val & SE_INT_OP_DONE(INT_SET))
+		complete(&se_dev->complete);
+
+	return IRQ_HANDLED;
+}
+
+static void tegra_se_work_handler(struct work_struct *work)
+{
+	struct tegra_se_dev *se_dev = sg_tegra_se_dev;
+	struct crypto_async_request *async_req = NULL;
+	struct crypto_async_request *backlog = NULL;
+
+	pm_runtime_get_sync(se_dev->dev);
+
+	do {
+		spin_lock_irq(&se_dev->lock);
+		backlog = crypto_get_backlog(&se_dev->queue);
+		async_req = crypto_dequeue_request(&se_dev->queue);
+		if (!async_req)
+			se_dev->work_q_busy = false;
+
+		spin_unlock_irq(&se_dev->lock);
+
+		if (backlog) {
+			backlog->complete(backlog, -EINPROGRESS);
+			backlog = NULL;
+		}
+
+		if (async_req) {
+			tegra_se_process_new_req(async_req);
+			async_req = NULL;
+		}
+	} while (se_dev->work_q_busy);
+	pm_runtime_put(se_dev->dev);
+}
+
+static int tegra_se_aes_queue_req(struct ablkcipher_request *req)
+{
+	struct tegra_se_dev *se_dev = sg_tegra_se_dev;
+	unsigned long flags;
+	bool idle = true;
+	int err = 0;
+
+	if (!tegra_se_count_sgs(req->src, req->nbytes))
+		return -EINVAL;
+
+	spin_lock_irqsave(&se_dev->lock, flags);
+	err = ablkcipher_enqueue_request(&se_dev->queue, req);
+	if (se_dev->work_q_busy)
+		idle = false;
+	spin_unlock_irqrestore(&se_dev->lock, flags);
+
+	if (idle) {
+		spin_lock_irq(&se_dev->lock);
+		se_dev->work_q_busy = true;
+		spin_unlock_irq(&se_dev->lock);
+		queue_work(se_work_q, &se_work);
+	}
+
+	return err;
+}
+
+static int tegra_se_aes_cbc_encrypt(struct ablkcipher_request *req)
+{
+	struct tegra_se_req_context *req_ctx = ablkcipher_request_ctx(req);
+
+	req_ctx->encrypt = true;
+	req_ctx->op_mode = SE_AES_OP_MODE_CBC;
+
+	return tegra_se_aes_queue_req(req);
+}
+
+static int tegra_se_aes_cbc_decrypt(struct ablkcipher_request *req)
+{
+	struct tegra_se_req_context *req_ctx = ablkcipher_request_ctx(req);
+
+	req_ctx->encrypt = false;
+	req_ctx->op_mode = SE_AES_OP_MODE_CBC;
+
+	return tegra_se_aes_queue_req(req);
+}
+
+static int tegra_se_aes_ecb_encrypt(struct ablkcipher_request *req)
+{
+	struct tegra_se_req_context *req_ctx = ablkcipher_request_ctx(req);
+
+	req_ctx->encrypt = true;
+	req_ctx->op_mode = SE_AES_OP_MODE_ECB;
+
+	return tegra_se_aes_queue_req(req);
+}
+
+static int tegra_se_aes_ecb_decrypt(struct ablkcipher_request *req)
+{
+	struct tegra_se_req_context *req_ctx = ablkcipher_request_ctx(req);
+
+	req_ctx->encrypt = false;
+	req_ctx->op_mode = SE_AES_OP_MODE_ECB;
+
+	return tegra_se_aes_queue_req(req);
+}
+
+static int tegra_se_aes_ctr_encrypt(struct ablkcipher_request *req)
+{
+	struct tegra_se_req_context *req_ctx = ablkcipher_request_ctx(req);
+
+	req_ctx->encrypt = true;
+	req_ctx->op_mode = SE_AES_OP_MODE_CTR;
+
+	return tegra_se_aes_queue_req(req);
+}
+
+static int tegra_se_aes_ctr_decrypt(struct ablkcipher_request *req)
+{
+	struct tegra_se_req_context *req_ctx = ablkcipher_request_ctx(req);
+
+	req_ctx->encrypt = false;
+	req_ctx->op_mode = SE_AES_OP_MODE_CTR;
+
+	return tegra_se_aes_queue_req(req);
+}
+
+static int tegra_se_aes_ofb_encrypt(struct ablkcipher_request *req)
+{
+	struct tegra_se_req_context *req_ctx = ablkcipher_request_ctx(req);
+
+	req_ctx->encrypt = true;
+	req_ctx->op_mode = SE_AES_OP_MODE_OFB;
+
+	return tegra_se_aes_queue_req(req);
+}
+
+static int tegra_se_aes_ofb_decrypt(struct ablkcipher_request *req)
+{
+	struct tegra_se_req_context *req_ctx = ablkcipher_request_ctx(req);
+
+	req_ctx->encrypt = false;
+	req_ctx->op_mode = SE_AES_OP_MODE_OFB;
+
+	return tegra_se_aes_queue_req(req);
+}
+
+static int tegra_se_aes_setkey(struct crypto_ablkcipher *tfm,
+	const u8 *key, u32 keylen)
+{
+	struct tegra_se_aes_context *ctx = crypto_ablkcipher_ctx(tfm);
+	struct tegra_se_dev *se_dev = ctx->se_dev;
+	struct tegra_se_slot *pslot;
+	u8 *pdata = (u8 *)key;
+
+	if (!ctx) {
+		dev_err(se_dev->dev, "invalid context");
+		return -EINVAL;
+	}
+
+	if ((keylen != TEGRA_SE_KEY_128_SIZE) &&
+		(keylen != TEGRA_SE_KEY_192_SIZE) &&
+		(keylen != TEGRA_SE_KEY_256_SIZE)) {
+		dev_err(se_dev->dev, "invalid key size");
+		return -EINVAL;
+	}
+
+	if (key) {
+		if (!ctx->slot || (ctx->slot &&
+		    ctx->slot->slot_num == ssk_slot.slot_num)) {
+			pslot = tegra_se_alloc_key_slot();
+			if (!pslot) {
+				dev_err(se_dev->dev, "no free key slot\n");
+				return -ENOMEM;
+			}
+			ctx->slot = pslot;
+		}
+		ctx->keylen = keylen;
+	} else {
+		tegra_se_free_key_slot(ctx->slot);
+		ctx->slot = &ssk_slot;
+		ctx->keylen = AES_KEYSIZE_128;
+	}
+
+	/* take access to the hw */
+	mutex_lock(&se_hw_lock);
+	pm_runtime_get_sync(se_dev->dev);
+
+	/* load the key */
+	tegra_se_write_key_table(pdata, keylen, ctx->slot->slot_num,
+		SE_KEY_TABLE_TYPE_KEY);
+
+	pm_runtime_put(se_dev->dev);
+	mutex_unlock(&se_hw_lock);
+
+	return 0;
+}
+
+static int tegra_se_aes_cra_init(struct crypto_tfm *tfm)
+{
+	struct tegra_se_aes_context *ctx = crypto_tfm_ctx(tfm);
+
+	ctx->se_dev = sg_tegra_se_dev;
+	tfm->crt_ablkcipher.reqsize = sizeof(struct tegra_se_req_context);
+
+	return 0;
+}
+
+static void tegra_se_aes_cra_exit(struct crypto_tfm *tfm)
+{
+	struct tegra_se_aes_context *ctx = crypto_tfm_ctx(tfm);
+
+	tegra_se_free_key_slot(ctx->slot);
+	ctx->slot = NULL;
+}
+
+static int tegra_se_rng_init(struct crypto_tfm *tfm)
+{
+	struct tegra_se_rng_context *rng_ctx = crypto_tfm_ctx(tfm);
+	struct tegra_se_dev *se_dev = sg_tegra_se_dev;
+
+	rng_ctx->se_dev = se_dev;
+	rng_ctx->dt_buf = dma_alloc_coherent(se_dev->dev, TEGRA_SE_RNG_DT_SIZE,
+		&rng_ctx->dt_buf_adr, GFP_KERNEL);
+	if (!rng_ctx->dt_buf) {
+		dev_err(se_dev->dev, "can not allocate rng dma buffer");
+		return -ENOMEM;
+	}
+
+	rng_ctx->rng_buf = dma_alloc_coherent(rng_ctx->se_dev->dev,
+		TEGRA_SE_RNG_DT_SIZE, &rng_ctx->rng_buf_adr, GFP_KERNEL);
+	if (!rng_ctx->rng_buf) {
+		dev_err(se_dev->dev, "can not allocate rng dma buffer");
+		dma_free_coherent(rng_ctx->se_dev->dev, TEGRA_SE_RNG_DT_SIZE,
+					rng_ctx->dt_buf, rng_ctx->dt_buf_adr);
+		return -ENOMEM;
+	}
+
+	rng_ctx->slot = tegra_se_alloc_key_slot();
+
+	if (!rng_ctx->slot) {
+		dev_err(rng_ctx->se_dev->dev, "no free slot\n");
+		dma_free_coherent(rng_ctx->se_dev->dev, TEGRA_SE_RNG_DT_SIZE,
+					rng_ctx->dt_buf, rng_ctx->dt_buf_adr);
+		dma_free_coherent(rng_ctx->se_dev->dev, TEGRA_SE_RNG_DT_SIZE,
+					rng_ctx->rng_buf, rng_ctx->rng_buf_adr);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void tegra_se_rng_exit(struct crypto_tfm *tfm)
+{
+	struct tegra_se_rng_context *rng_ctx = crypto_tfm_ctx(tfm);
+
+	if (rng_ctx->dt_buf) {
+		dma_free_coherent(rng_ctx->se_dev->dev, TEGRA_SE_RNG_DT_SIZE,
+			rng_ctx->dt_buf, rng_ctx->dt_buf_adr);
+	}
+
+	if (rng_ctx->rng_buf) {
+		dma_free_coherent(rng_ctx->se_dev->dev, TEGRA_SE_RNG_DT_SIZE,
+			rng_ctx->rng_buf, rng_ctx->rng_buf_adr);
+	}
+
+	tegra_se_free_key_slot(rng_ctx->slot);
+	rng_ctx->slot = NULL;
+	rng_ctx->se_dev = NULL;
+}
+
+static int tegra_se_rng_get_random(struct crypto_rng *tfm, u8 *rdata, u32 dlen)
+{
+	struct tegra_se_rng_context *rng_ctx = crypto_rng_ctx(tfm);
+	struct tegra_se_dev *se_dev = rng_ctx->se_dev;
+	struct tegra_se_ll *src_ll, *dst_ll;
+	unsigned char *dt_buf = (unsigned char *)rng_ctx->dt_buf;
+	u8 *rdata_addr;
+	int ret = 0, i, j, num_blocks, data_len = 0;
+
+	num_blocks = (dlen / TEGRA_SE_RNG_DT_SIZE);
+
+	data_len = (dlen % TEGRA_SE_RNG_DT_SIZE);
+	if (data_len == 0)
+		num_blocks = num_blocks - 1;
+
+	/* take access to the hw */
+	mutex_lock(&se_hw_lock);
+	pm_runtime_get_sync(se_dev->dev);
+
+	*se_dev->src_ll_buf = 0;
+	*se_dev->dst_ll_buf = 0;
+	src_ll = (struct tegra_se_ll *)(se_dev->src_ll_buf + 1);
+	dst_ll = (struct tegra_se_ll *)(se_dev->dst_ll_buf + 1);
+	src_ll->addr = rng_ctx->dt_buf_adr;
+	src_ll->data_len = TEGRA_SE_RNG_DT_SIZE;
+	dst_ll->addr = rng_ctx->rng_buf_adr;
+	dst_ll->data_len = TEGRA_SE_RNG_DT_SIZE;
+
+	tegra_se_config_algo(se_dev, SE_AES_OP_MODE_RNG_X931, true,
+		TEGRA_SE_KEY_128_SIZE);
+	tegra_se_config_crypto(se_dev, SE_AES_OP_MODE_RNG_X931, true,
+				rng_ctx->slot->slot_num, rng_ctx->use_org_iv);
+	for (j = 0; j <= num_blocks; j++) {
+		ret = tegra_se_start_operation(se_dev,
+				TEGRA_SE_RNG_DT_SIZE, false);
+
+		if (!ret) {
+			rdata_addr = (rdata + (j * TEGRA_SE_RNG_DT_SIZE));
+
+			if (data_len && num_blocks == j) {
+				memcpy(rdata_addr, rng_ctx->rng_buf, data_len);
+			} else {
+				memcpy(rdata_addr,
+					rng_ctx->rng_buf, TEGRA_SE_RNG_DT_SIZE);
+			}
+
+			/* update DT vector */
+			for (i = TEGRA_SE_RNG_DT_SIZE - 1; i >= 0; i--) {
+				dt_buf[i] += 1;
+				if (dt_buf[i] != 0)
+					break;
+			}
+		} else {
+			dlen = 0;
+		}
+		if (rng_ctx->use_org_iv) {
+			rng_ctx->use_org_iv = false;
+			tegra_se_config_crypto(se_dev,
+				SE_AES_OP_MODE_RNG_X931, true,
+				rng_ctx->slot->slot_num, rng_ctx->use_org_iv);
+		}
+	}
+
+	pm_runtime_put(se_dev->dev);
+	mutex_unlock(&se_hw_lock);
+
+	return dlen;
+}
+
+static int tegra_se_rng_reset(struct crypto_rng *tfm, u8 *seed, u32 slen)
+{
+	struct tegra_se_rng_context *rng_ctx = crypto_rng_ctx(tfm);
+	struct tegra_se_dev *se_dev = rng_ctx->se_dev;
+	u8 *iv = seed;
+	u8 *key = (u8 *)(seed + TEGRA_SE_RNG_IV_SIZE);
+	u8 *dt = key + TEGRA_SE_RNG_KEY_SIZE;
+	struct timespec ts;
+	u64 nsec, tmp[2];
+
+	BUG_ON(!seed);
+
+	/* take access to the hw */
+	mutex_lock(&se_hw_lock);
+	pm_runtime_get_sync(se_dev->dev);
+
+	tegra_se_write_key_table(key, TEGRA_SE_RNG_KEY_SIZE,
+		rng_ctx->slot->slot_num, SE_KEY_TABLE_TYPE_KEY);
+
+	tegra_se_write_key_table(iv, TEGRA_SE_RNG_IV_SIZE,
+		rng_ctx->slot->slot_num, SE_KEY_TABLE_TYPE_ORGIV);
+
+	pm_runtime_put(se_dev->dev);
+	mutex_unlock(&se_hw_lock);
+
+	if (slen < TEGRA_SE_RNG_SEED_SIZE) {
+		getnstimeofday(&ts);
+		nsec = timespec_to_ns(&ts);
+		do_div(nsec, 1000);
+		nsec ^= se_dev->ctr << 56;
+		se_dev->ctr++;
+		tmp[0] = nsec;
+		tmp[1] = tegra_chip_uid();
+		memcpy(rng_ctx->dt_buf, (u8 *)tmp, TEGRA_SE_RNG_DT_SIZE);
+	} else {
+		memcpy(rng_ctx->dt_buf, dt, TEGRA_SE_RNG_DT_SIZE);
+	}
+
+	rng_ctx->use_org_iv = true;
+
+	return 0;
+}
+
+int tegra_se_sha_init(struct ahash_request *req)
+{
+	return 0;
+}
+
+int tegra_se_sha_update(struct ahash_request *req)
+{
+	return 0;
+}
+
+int tegra_se_sha_finup(struct ahash_request *req)
+{
+	return 0;
+}
+
+int tegra_se_sha_final(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct tegra_se_sha_context *sha_ctx = crypto_ahash_ctx(tfm);
+	struct tegra_se_dev *se_dev = sg_tegra_se_dev;
+	struct scatterlist *src_sg;
+	struct tegra_se_ll *src_ll;
+	u32 total, num_sgs;
+	int err = 0;
+
+	if (!req->nbytes)
+		return -EINVAL;
+
+	if (crypto_ahash_digestsize(tfm) == SHA1_DIGEST_SIZE)
+		sha_ctx->op_mode = SE_AES_OP_MODE_SHA1;
+
+	if (crypto_ahash_digestsize(tfm) == SHA224_DIGEST_SIZE)
+		sha_ctx->op_mode = SE_AES_OP_MODE_SHA224;
+
+	if (crypto_ahash_digestsize(tfm) == SHA256_DIGEST_SIZE)
+		sha_ctx->op_mode = SE_AES_OP_MODE_SHA256;
+
+	if (crypto_ahash_digestsize(tfm) == SHA384_DIGEST_SIZE)
+		sha_ctx->op_mode = SE_AES_OP_MODE_SHA384;
+
+	if (crypto_ahash_digestsize(tfm) == SHA512_DIGEST_SIZE)
+		sha_ctx->op_mode = SE_AES_OP_MODE_SHA512;
+
+	/* take access to the hw */
+	mutex_lock(&se_hw_lock);
+	pm_runtime_get_sync(se_dev->dev);
+
+	num_sgs = tegra_se_count_sgs(req->src, req->nbytes);
+	if ((num_sgs > SE_MAX_SRC_SG_COUNT)) {
+		dev_err(se_dev->dev, "num of SG buffers are more\n");
+		pm_runtime_put(se_dev->dev);
+		mutex_unlock(&se_hw_lock);
+		return -EINVAL;
+	}
+	*se_dev->src_ll_buf = num_sgs-1;
+	src_ll = (struct tegra_se_ll *)(se_dev->src_ll_buf + 1);
+	src_sg = req->src;
+	total = req->nbytes;
+
+	while (total) {
+		err = dma_map_sg(se_dev->dev, src_sg, 1, DMA_TO_DEVICE);
+		if (!err) {
+			dev_err(se_dev->dev, "dma_map_sg() error\n");
+			pm_runtime_put(se_dev->dev);
+			mutex_unlock(&se_hw_lock);
+			return -EINVAL;
+		}
+		src_ll->addr = sg_dma_address(src_sg);
+		src_ll->data_len = src_sg->length;
+
+		total -= src_sg->length;
+		src_sg = sg_next(src_sg);
+		src_ll++;
+	}
+
+	tegra_se_config_algo(se_dev, sha_ctx->op_mode, false, 0);
+	tegra_se_config_sha(se_dev, req->nbytes);
+	err = tegra_se_start_operation(se_dev, 0, false);
+	if (!err) {
+		tegra_se_read_hash_result(se_dev, req->result,
+			crypto_ahash_digestsize(tfm), true);
+		if ((sha_ctx->op_mode == SE_AES_OP_MODE_SHA384) ||
+			(sha_ctx->op_mode == SE_AES_OP_MODE_SHA512)) {
+			u32 *result = (u32 *)req->result;
+			u32 temp, i;
+
+			for (i = 0; i < crypto_ahash_digestsize(tfm)/4;
+				i += 2) {
+				temp = result[i];
+				result[i] = result[i+1];
+				result[i+1] = temp;
+			}
+		}
+	}
+
+	src_sg = req->src;
+	total = req->nbytes;
+	while (total) {
+		dma_unmap_sg(se_dev->dev, src_sg, 1, DMA_TO_DEVICE);
+		total -= src_sg->length;
+		src_sg = sg_next(src_sg);
+	}
+	pm_runtime_put(se_dev->dev);
+	mutex_unlock(&se_hw_lock);
+
+	return err;
+}
+
+static int tegra_se_sha_digest(struct ahash_request *req)
+{
+	return tegra_se_sha_init(req) ?: tegra_se_sha_final(req);
+}
+
+int tegra_se_sha_cra_init(struct crypto_tfm *tfm)
+{
+	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
+				 sizeof(struct tegra_se_sha_context));
+	return 0;
+}
+
+void tegra_se_sha_cra_exit(struct crypto_tfm *tfm)
+{
+	/* do nothing */
+}
+
+int tegra_se_aes_cmac_init(struct ahash_request *req)
+{
+
+	return 0;
+}
+
+int tegra_se_aes_cmac_update(struct ahash_request *req)
+{
+	return 0;
+}
+
+int tegra_se_aes_cmac_final(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct tegra_se_aes_cmac_context *cmac_ctx = crypto_ahash_ctx(tfm);
+	struct tegra_se_dev *se_dev = sg_tegra_se_dev;
+	struct scatterlist *src_sg;
+	struct tegra_se_ll *src_ll;
+	struct sg_mapping_iter miter;
+	u32 num_sgs, blocks_to_process, last_block_bytes = 0, bytes_to_copy = 0;
+	u8 piv[TEGRA_SE_AES_IV_SIZE];
+	int total, ret = 0, i = 0, mapped_sg_count = 0;
+	bool padding_needed = false;
+	unsigned long flags;
+	unsigned int sg_flags = SG_MITER_ATOMIC;
+	u8 *temp_buffer = NULL;
+	bool use_orig_iv = true;
+
+	/* take access to the hw */
+	mutex_lock(&se_hw_lock);
+	pm_runtime_get_sync(se_dev->dev);
+
+	blocks_to_process = req->nbytes / TEGRA_SE_AES_BLOCK_SIZE;
+	/* num of bytes less than block size */
+	if ((req->nbytes % TEGRA_SE_AES_BLOCK_SIZE) || !blocks_to_process) {
+		padding_needed = true;
+		last_block_bytes = req->nbytes % TEGRA_SE_AES_BLOCK_SIZE;
+	} else {
+		/* decrement num of blocks */
+		blocks_to_process--;
+		if (blocks_to_process) {
+			/* there are blocks to process and find last block
+				bytes */
+			last_block_bytes = req->nbytes -
+				(blocks_to_process * TEGRA_SE_AES_BLOCK_SIZE);
+		} else {
+			/* this is the last block and equal to block size */
+			last_block_bytes = req->nbytes;
+		}
+	}
+
+	/* first process all blocks except last block */
+	if (blocks_to_process) {
+		num_sgs = tegra_se_count_sgs(req->src, req->nbytes);
+		if (num_sgs > SE_MAX_SRC_SG_COUNT) {
+			dev_err(se_dev->dev, "num of SG buffers are more\n");
+			goto out;
+		}
+		*se_dev->src_ll_buf = num_sgs - 1;
+		src_ll = (struct tegra_se_ll *)(se_dev->src_ll_buf + 1);
+		src_sg = req->src;
+		total = blocks_to_process * TEGRA_SE_AES_BLOCK_SIZE;
+		while (total > 0) {
+			ret = dma_map_sg(se_dev->dev, src_sg, 1, DMA_TO_DEVICE);
+			mapped_sg_count++;
+			if (!ret) {
+				dev_err(se_dev->dev, "dma_map_sg() error\n");
+				goto out;
+			}
+			src_ll->addr = sg_dma_address(src_sg);
+			if (total > src_sg->length)
+				src_ll->data_len = src_sg->length;
+			else
+				src_ll->data_len = total;
+
+			total -= src_sg->length;
+			if (total > 0) {
+				src_sg = sg_next(src_sg);
+				src_ll++;
+			}
+			WARN_ON(((total != 0) && (!src_sg)));
+		}
+		tegra_se_config_algo(se_dev, SE_AES_OP_MODE_CMAC, true,
+			cmac_ctx->keylen);
+		/* write zero IV */
+		memset(piv, 0, TEGRA_SE_AES_IV_SIZE);
+		tegra_se_write_key_table(piv, TEGRA_SE_AES_IV_SIZE,
+					cmac_ctx->slot->slot_num,
+					SE_KEY_TABLE_TYPE_ORGIV);
+		tegra_se_config_crypto(se_dev, SE_AES_OP_MODE_CMAC, true,
+					cmac_ctx->slot->slot_num, true);
+		tegra_se_start_operation(se_dev,
+			blocks_to_process * TEGRA_SE_AES_BLOCK_SIZE, false);
+		src_sg = req->src;
+		while (mapped_sg_count--) {
+			dma_unmap_sg(se_dev->dev, src_sg, 1, DMA_TO_DEVICE);
+			src_sg = sg_next(src_sg);
+		}
+		use_orig_iv = false;
+	}
+
+	/* get the last block bytes from the sg_dma buffer using miter */
+	src_sg = req->src;
+	num_sgs = tegra_se_count_sgs(req->src, req->nbytes);
+	sg_flags |= SG_MITER_FROM_SG;
+	sg_miter_start(&miter, req->src, num_sgs, sg_flags);
+	local_irq_save(flags);
+	total = 0;
+	cmac_ctx->buffer = dma_alloc_coherent(se_dev->dev,
+				TEGRA_SE_AES_BLOCK_SIZE,
+				&cmac_ctx->dma_addr, GFP_KERNEL);
+
+	if (!cmac_ctx->buffer)
+		goto out;
+
+	temp_buffer = cmac_ctx->buffer;
+	while (sg_miter_next(&miter) && total < req->nbytes) {
+		unsigned int len;
+		len = min(miter.length, req->nbytes - total);
+		if ((req->nbytes - (total + len)) <= last_block_bytes) {
+			bytes_to_copy =
+				last_block_bytes -
+				(req->nbytes - (total + len));
+			memcpy(temp_buffer, miter.addr + (len - bytes_to_copy),
+				bytes_to_copy);
+			last_block_bytes -= bytes_to_copy;
+			temp_buffer += bytes_to_copy;
+		}
+		total += len;
+	}
+	sg_miter_stop(&miter);
+	local_irq_restore(flags);
+
+	/* process last block */
+	if (padding_needed) {
+		/* pad with 0x80, 0, 0 ... */
+		last_block_bytes = req->nbytes % TEGRA_SE_AES_BLOCK_SIZE;
+		cmac_ctx->buffer[last_block_bytes] = 0x80;
+		for (i = last_block_bytes+1; i < TEGRA_SE_AES_BLOCK_SIZE; i++)
+			cmac_ctx->buffer[i] = 0;
+		/* XOR with K2 */
+		for (i = 0; i < TEGRA_SE_AES_BLOCK_SIZE; i++)
+			cmac_ctx->buffer[i] ^= cmac_ctx->K2[i];
+	} else {
+		/* XOR with K1 */
+		for (i = 0; i < TEGRA_SE_AES_BLOCK_SIZE; i++)
+			cmac_ctx->buffer[i] ^= cmac_ctx->K1[i];
+	}
+	*se_dev->src_ll_buf = 0;
+	src_ll = (struct tegra_se_ll *)(se_dev->src_ll_buf + 1);
+	src_ll->addr = cmac_ctx->dma_addr;
+	src_ll->data_len = TEGRA_SE_AES_BLOCK_SIZE;
+
+	if (use_orig_iv) {
+		/* use zero IV, this is when num of bytes is
+			less <= block size */
+		memset(piv, 0, TEGRA_SE_AES_IV_SIZE);
+		tegra_se_write_key_table(piv, TEGRA_SE_AES_IV_SIZE,
+					cmac_ctx->slot->slot_num,
+					SE_KEY_TABLE_TYPE_ORGIV);
+	}
+
+	tegra_se_config_algo(se_dev, SE_AES_OP_MODE_CMAC, true,
+				cmac_ctx->keylen);
+	tegra_se_config_crypto(se_dev, SE_AES_OP_MODE_CMAC, true,
+				cmac_ctx->slot->slot_num, use_orig_iv);
+	tegra_se_start_operation(se_dev, TEGRA_SE_AES_BLOCK_SIZE, false);
+	tegra_se_read_hash_result(se_dev, req->result,
+				TEGRA_SE_AES_CMAC_DIGEST_SIZE, false);
+
+out:
+	pm_runtime_put(se_dev->dev);
+	mutex_unlock(&se_hw_lock);
+
+	if (cmac_ctx->buffer)
+		dma_free_coherent(se_dev->dev, TEGRA_SE_AES_BLOCK_SIZE,
+			cmac_ctx->buffer, cmac_ctx->dma_addr);
+
+	return 0;
+}
+
+int tegra_se_aes_cmac_setkey(struct crypto_ahash *tfm, const u8 *key,
+		unsigned int keylen)
+{
+	struct tegra_se_aes_cmac_context *ctx = crypto_ahash_ctx(tfm);
+	struct tegra_se_dev *se_dev = sg_tegra_se_dev;
+	struct tegra_se_ll *src_ll, *dst_ll;
+	struct tegra_se_slot *pslot;
+	u8 piv[TEGRA_SE_AES_IV_SIZE];
+	u32 *pbuf;
+	dma_addr_t pbuf_adr;
+	int ret = 0;
+	u8 const rb = 0x87;
+	u8 msb;
+
+	if (!ctx) {
+		dev_err(se_dev->dev, "invalid context");
+		return -EINVAL;
+	}
+
+	if ((keylen != TEGRA_SE_KEY_128_SIZE) &&
+		(keylen != TEGRA_SE_KEY_192_SIZE) &&
+		(keylen != TEGRA_SE_KEY_256_SIZE)) {
+		dev_err(se_dev->dev, "invalid key size");
+		return -EINVAL;
+	}
+
+	if (key) {
+		if (!ctx->slot || (ctx->slot &&
+		    ctx->slot->slot_num == ssk_slot.slot_num)) {
+			pslot = tegra_se_alloc_key_slot();
+			if (!pslot) {
+				dev_err(se_dev->dev, "no free key slot\n");
+				return -ENOMEM;
+			}
+			ctx->slot = pslot;
+		}
+		ctx->keylen = keylen;
+	} else {
+		tegra_se_free_key_slot(ctx->slot);
+		ctx->slot = &ssk_slot;
+		ctx->keylen = AES_KEYSIZE_128;
+	}
+
+	pbuf = dma_alloc_coherent(se_dev->dev, TEGRA_SE_AES_BLOCK_SIZE,
+		&pbuf_adr, GFP_KERNEL);
+	if (!pbuf) {
+		dev_err(se_dev->dev, "can not allocate dma buffer");
+		return -ENOMEM;
+	}
+	memset(pbuf, 0, TEGRA_SE_AES_BLOCK_SIZE);
+
+	/* take access to the hw */
+	mutex_lock(&se_hw_lock);
+	pm_runtime_get_sync(se_dev->dev);
+
+	*se_dev->src_ll_buf = 0;
+	*se_dev->dst_ll_buf = 0;
+	src_ll = (struct tegra_se_ll *)(se_dev->src_ll_buf + 1);
+	dst_ll = (struct tegra_se_ll *)(se_dev->dst_ll_buf + 1);
+
+	src_ll->addr = pbuf_adr;
+	src_ll->data_len = TEGRA_SE_AES_BLOCK_SIZE;
+	dst_ll->addr = pbuf_adr;
+	dst_ll->data_len = TEGRA_SE_AES_BLOCK_SIZE;
+
+	/* load the key */
+	tegra_se_write_key_table((u8 *)key, keylen,
+				ctx->slot->slot_num, SE_KEY_TABLE_TYPE_KEY);
+
+	/* write zero IV */
+	memset(piv, 0, TEGRA_SE_AES_IV_SIZE);
+
+	/* load IV */
+	tegra_se_write_key_table(piv, TEGRA_SE_AES_IV_SIZE,
+				ctx->slot->slot_num, SE_KEY_TABLE_TYPE_ORGIV);
+
+	/* config crypto algo */
+	tegra_se_config_algo(se_dev, SE_AES_OP_MODE_CBC, true, keylen);
+
+	tegra_se_config_crypto(se_dev, SE_AES_OP_MODE_CBC, true,
+		ctx->slot->slot_num, true);
+
+	ret = tegra_se_start_operation(se_dev, TEGRA_SE_AES_BLOCK_SIZE, false);
+	if (ret) {
+		dev_err(se_dev->dev, "tegra_se_aes_cmac_setkey:: start op failed\n");
+		goto out;
+	}
+
+	/* compute K1 subkey */
+	memcpy(ctx->K1, pbuf, TEGRA_SE_AES_BLOCK_SIZE);
+	tegra_se_leftshift_onebit(ctx->K1, TEGRA_SE_AES_BLOCK_SIZE, &msb);
+	if (msb)
+		ctx->K1[TEGRA_SE_AES_BLOCK_SIZE - 1] ^= rb;
+
+	/* compute K2 subkey */
+	memcpy(ctx->K2, ctx->K1, TEGRA_SE_AES_BLOCK_SIZE);
+	tegra_se_leftshift_onebit(ctx->K2, TEGRA_SE_AES_BLOCK_SIZE, &msb);
+
+	if (msb)
+		ctx->K2[TEGRA_SE_AES_BLOCK_SIZE - 1] ^= rb;
+
+out:
+	pm_runtime_put(se_dev->dev);
+	mutex_unlock(&se_hw_lock);
+
+	if (pbuf) {
+		dma_free_coherent(se_dev->dev, TEGRA_SE_AES_BLOCK_SIZE,
+			pbuf, pbuf_adr);
+	}
+
+	return 0;
+}
+
+int tegra_se_aes_cmac_digest(struct ahash_request *req)
+{
+	return tegra_se_aes_cmac_init(req) ?: tegra_se_aes_cmac_final(req);
+}
+
+int tegra_se_aes_cmac_finup(struct ahash_request *req)
+{
+	return 0;
+}
+
+int tegra_se_aes_cmac_cra_init(struct crypto_tfm *tfm)
+{
+	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
+				 sizeof(struct tegra_se_aes_cmac_context));
+
+	return 0;
+}
+void tegra_se_aes_cmac_cra_exit(struct crypto_tfm *tfm)
+{
+	struct tegra_se_aes_cmac_context *ctx = crypto_tfm_ctx(tfm);
+
+	tegra_se_free_key_slot(ctx->slot);
+	ctx->slot = NULL;
+}
+
+static struct crypto_alg aes_algs[] = {
+	{
+		.cra_name = "cbc(aes)",
+		.cra_driver_name = "cbc-aes-tegra",
+		.cra_priority = 300,
+		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+		.cra_blocksize = TEGRA_SE_AES_BLOCK_SIZE,
+		.cra_ctxsize  = sizeof(struct tegra_se_aes_context),
+		.cra_alignmask = 0,
+		.cra_type = &crypto_ablkcipher_type,
+		.cra_module = THIS_MODULE,
+		.cra_init = tegra_se_aes_cra_init,
+		.cra_exit = tegra_se_aes_cra_exit,
+		.cra_u.ablkcipher = {
+			.min_keysize = TEGRA_SE_AES_MIN_KEY_SIZE,
+			.max_keysize = TEGRA_SE_AES_MAX_KEY_SIZE,
+			.ivsize = TEGRA_SE_AES_IV_SIZE,
+			.setkey = tegra_se_aes_setkey,
+			.encrypt = tegra_se_aes_cbc_encrypt,
+			.decrypt = tegra_se_aes_cbc_decrypt,
+		}
+	}, {
+		.cra_name = "ecb(aes)",
+		.cra_driver_name = "ecb-aes-tegra",
+		.cra_priority = 300,
+		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+		.cra_blocksize = TEGRA_SE_AES_BLOCK_SIZE,
+		.cra_ctxsize  = sizeof(struct tegra_se_aes_context),
+		.cra_alignmask = 0,
+		.cra_type = &crypto_ablkcipher_type,
+		.cra_module = THIS_MODULE,
+		.cra_init = tegra_se_aes_cra_init,
+		.cra_exit = tegra_se_aes_cra_exit,
+		.cra_u.ablkcipher = {
+			.min_keysize = TEGRA_SE_AES_MIN_KEY_SIZE,
+			.max_keysize = TEGRA_SE_AES_MAX_KEY_SIZE,
+			.ivsize = TEGRA_SE_AES_IV_SIZE,
+			.setkey = tegra_se_aes_setkey,
+			.encrypt = tegra_se_aes_ecb_encrypt,
+			.decrypt = tegra_se_aes_ecb_decrypt,
+		}
+	}, {
+		.cra_name = "ctr(aes)",
+		.cra_driver_name = "ctr-aes-tegra",
+		.cra_priority = 300,
+		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+		.cra_blocksize = TEGRA_SE_AES_BLOCK_SIZE,
+		.cra_ctxsize  = sizeof(struct tegra_se_aes_context),
+		.cra_alignmask = 0,
+		.cra_type = &crypto_ablkcipher_type,
+		.cra_module = THIS_MODULE,
+		.cra_init = tegra_se_aes_cra_init,
+		.cra_exit = tegra_se_aes_cra_exit,
+		.cra_u.ablkcipher = {
+			.min_keysize = TEGRA_SE_AES_MIN_KEY_SIZE,
+			.max_keysize = TEGRA_SE_AES_MAX_KEY_SIZE,
+			.ivsize = TEGRA_SE_AES_IV_SIZE,
+			.setkey = tegra_se_aes_setkey,
+			.encrypt = tegra_se_aes_ctr_encrypt,
+			.decrypt = tegra_se_aes_ctr_decrypt,
+			.geniv = "eseqiv",
+		}
+	}, {
+		.cra_name = "ofb(aes)",
+		.cra_driver_name = "ofb-aes-tegra",
+		.cra_priority = 300,
+		.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
+		.cra_blocksize = TEGRA_SE_AES_BLOCK_SIZE,
+		.cra_ctxsize  = sizeof(struct tegra_se_aes_context),
+		.cra_alignmask = 0,
+		.cra_type = &crypto_ablkcipher_type,
+		.cra_module = THIS_MODULE,
+		.cra_init = tegra_se_aes_cra_init,
+		.cra_exit = tegra_se_aes_cra_exit,
+		.cra_u.ablkcipher = {
+			.min_keysize = TEGRA_SE_AES_MIN_KEY_SIZE,
+			.max_keysize = TEGRA_SE_AES_MAX_KEY_SIZE,
+			.ivsize = TEGRA_SE_AES_IV_SIZE,
+			.setkey = tegra_se_aes_setkey,
+			.encrypt = tegra_se_aes_ofb_encrypt,
+			.decrypt = tegra_se_aes_ofb_decrypt,
+			.geniv = "eseqiv",
+		}
+	}, {
+		.cra_name = "ansi_cprng",
+		.cra_driver_name = "rng-aes-tegra",
+		.cra_priority = 100,
+		.cra_flags = CRYPTO_ALG_TYPE_RNG,
+		.cra_ctxsize = sizeof(struct tegra_se_rng_context),
+		.cra_type = &crypto_rng_type,
+		.cra_module = THIS_MODULE,
+		.cra_init = tegra_se_rng_init,
+		.cra_exit = tegra_se_rng_exit,
+		.cra_u = {
+			.rng = {
+				.rng_make_random = tegra_se_rng_get_random,
+				.rng_reset = tegra_se_rng_reset,
+				.seedsize = TEGRA_SE_RNG_SEED_SIZE,
+			}
+		}
+	}
+};
+
+static struct ahash_alg hash_algs[] = {
+	{
+		.init = tegra_se_aes_cmac_init,
+		.update = tegra_se_aes_cmac_update,
+		.final = tegra_se_aes_cmac_final,
+		.finup = tegra_se_aes_cmac_finup,
+		.digest = tegra_se_aes_cmac_digest,
+		.setkey = tegra_se_aes_cmac_setkey,
+		.halg.digestsize = TEGRA_SE_AES_CMAC_DIGEST_SIZE,
+		.halg.base = {
+			.cra_name = "cmac(aes)",
+			.cra_driver_name = "tegra-se-cmac(aes)",
+			.cra_priority = 100,
+			.cra_flags = CRYPTO_ALG_TYPE_AHASH,
+			.cra_blocksize = TEGRA_SE_AES_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct tegra_se_aes_cmac_context),
+			.cra_alignmask = 0,
+			.cra_module	= THIS_MODULE,
+			.cra_init	= tegra_se_aes_cmac_cra_init,
+			.cra_exit	= tegra_se_aes_cmac_cra_exit,
+		}
+	}, {
+		.init = tegra_se_sha_init,
+		.update = tegra_se_sha_update,
+		.final = tegra_se_sha_final,
+		.finup = tegra_se_sha_finup,
+		.digest = tegra_se_sha_digest,
+		.halg.digestsize = SHA1_DIGEST_SIZE,
+		.halg.base = {
+			.cra_name = "sha1",
+			.cra_driver_name = "tegra-se-sha1",
+			.cra_priority = 100,
+			.cra_flags = CRYPTO_ALG_TYPE_AHASH,
+			.cra_blocksize = SHA1_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct tegra_se_sha_context),
+			.cra_alignmask = 0,
+			.cra_module = THIS_MODULE,
+			.cra_init = tegra_se_sha_cra_init,
+			.cra_exit = tegra_se_sha_cra_exit,
+		}
+	}, {
+		.init = tegra_se_sha_init,
+		.update = tegra_se_sha_update,
+		.final = tegra_se_sha_final,
+		.finup = tegra_se_sha_finup,
+		.digest = tegra_se_sha_digest,
+		.halg.digestsize = SHA224_DIGEST_SIZE,
+		.halg.base = {
+			.cra_name = "sha224",
+			.cra_driver_name = "tegra-se-sha224",
+			.cra_priority = 100,
+			.cra_flags = CRYPTO_ALG_TYPE_AHASH,
+			.cra_blocksize = SHA224_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct tegra_se_sha_context),
+			.cra_alignmask = 0,
+			.cra_module = THIS_MODULE,
+			.cra_init = tegra_se_sha_cra_init,
+			.cra_exit = tegra_se_sha_cra_exit,
+		}
+	}, {
+		.init = tegra_se_sha_init,
+		.update = tegra_se_sha_update,
+		.final = tegra_se_sha_final,
+		.finup = tegra_se_sha_finup,
+		.digest = tegra_se_sha_digest,
+		.halg.digestsize = SHA256_DIGEST_SIZE,
+		.halg.base = {
+			.cra_name = "sha256",
+			.cra_driver_name = "tegra-se-sha256",
+			.cra_priority = 100,
+			.cra_flags = CRYPTO_ALG_TYPE_AHASH,
+			.cra_blocksize = SHA256_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct tegra_se_sha_context),
+			.cra_alignmask = 0,
+			.cra_module = THIS_MODULE,
+			.cra_init = tegra_se_sha_cra_init,
+			.cra_exit = tegra_se_sha_cra_exit,
+		}
+	}, {
+		.init = tegra_se_sha_init,
+		.update = tegra_se_sha_update,
+		.final = tegra_se_sha_final,
+		.finup = tegra_se_sha_finup,
+		.digest = tegra_se_sha_digest,
+		.halg.digestsize = SHA384_DIGEST_SIZE,
+		.halg.base = {
+			.cra_name = "sha384",
+			.cra_driver_name = "tegra-se-sha384",
+			.cra_priority = 100,
+			.cra_flags = CRYPTO_ALG_TYPE_AHASH,
+			.cra_blocksize = SHA384_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct tegra_se_sha_context),
+			.cra_alignmask = 0,
+			.cra_module = THIS_MODULE,
+			.cra_init = tegra_se_sha_cra_init,
+			.cra_exit = tegra_se_sha_cra_exit,
+		}
+	}, {
+		.init = tegra_se_sha_init,
+		.update = tegra_se_sha_update,
+		.final = tegra_se_sha_final,
+		.finup = tegra_se_sha_finup,
+		.digest = tegra_se_sha_digest,
+		.halg.digestsize = SHA512_DIGEST_SIZE,
+		.halg.base = {
+			.cra_name = "sha512",
+			.cra_driver_name = "tegra-se-sha512",
+			.cra_priority = 100,
+			.cra_flags = CRYPTO_ALG_TYPE_AHASH,
+			.cra_blocksize = SHA512_BLOCK_SIZE,
+			.cra_ctxsize = sizeof(struct tegra_se_sha_context),
+			.cra_alignmask = 0,
+			.cra_module = THIS_MODULE,
+			.cra_init = tegra_se_sha_cra_init,
+			.cra_exit = tegra_se_sha_cra_exit,
+		}
+	}
+};
+
+static int tegra_se_probe(struct platform_device *pdev)
+{
+	struct tegra_se_dev *se_dev = NULL;
+	struct resource *res = NULL;
+	int err = 0, i = 0, j = 0, k = 0;
+
+	se_dev = kzalloc(sizeof(struct tegra_se_dev), GFP_KERNEL);
+	if (!se_dev) {
+		dev_err(&pdev->dev, "memory allocation failed\n");
+		return -ENOMEM;
+	}
+
+	spin_lock_init(&se_dev->lock);
+	crypto_init_queue(&se_dev->queue, TEGRA_SE_CRYPTO_QUEUE_LENGTH);
+	platform_set_drvdata(pdev, se_dev);
+	se_dev->dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		err = -ENXIO;
+		dev_err(se_dev->dev, "platform_get_resource failed\n");
+		goto fail;
+	}
+
+	se_dev->io_reg = ioremap(res->start, resource_size(res));
+	if (!se_dev->io_reg) {
+		err = -ENOMEM;
+		dev_err(se_dev->dev, "ioremap failed\n");
+		goto fail;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!res) {
+		err = -ENXIO;
+		dev_err(se_dev->dev, "platform_get_resource failed\n");
+		goto err_pmc;
+	}
+
+	se_dev->pmc_io_reg = ioremap(res->start, resource_size(res));
+	if (!se_dev->pmc_io_reg) {
+		err = -ENOMEM;
+		dev_err(se_dev->dev, "pmc ioremap failed\n");
+		goto err_pmc;
+	}
+
+	se_dev->irq = platform_get_irq(pdev, 0);
+	if (!se_dev->irq) {
+		err = -ENODEV;
+		dev_err(se_dev->dev, "platform_get_irq failed\n");
+		goto err_irq;
+	}
+
+	/* Initialize the clock */
+	se_dev->pclk = clk_get(se_dev->dev, "se");
+	if (IS_ERR(se_dev->pclk)) {
+		dev_err(se_dev->dev, "clock intialization failed (%d)\n",
+			(int)se_dev->pclk);
+		err = -ENODEV;
+		goto clean;
+	}
+
+	err = clk_set_rate(se_dev->pclk, ULONG_MAX);
+	if (err) {
+		dev_err(se_dev->dev, "clock set_rate failed.\n");
+		goto clean;
+	}
+
+	err = tegra_init_key_slot(se_dev);
+	if (err) {
+		dev_err(se_dev->dev, "init_key_slot failed\n");
+		goto clean;
+	}
+
+	init_completion(&se_dev->complete);
+	se_work_q = alloc_workqueue("se_work_q", WQ_HIGHPRI | WQ_UNBOUND, 16);
+	if (!se_work_q) {
+		dev_err(se_dev->dev, "alloc_workqueue failed\n");
+		goto clean;
+	}
+
+	sg_tegra_se_dev = se_dev;
+	pm_runtime_enable(se_dev->dev);
+	tegra_se_key_read_disable_all();
+
+	err = request_irq(se_dev->irq, tegra_se_irq, IRQF_DISABLED,
+			DRIVER_NAME, se_dev);
+	if (err) {
+		dev_err(se_dev->dev, "request_irq failed - irq[%d] err[%d]\n",
+			se_dev->irq, err);
+		goto clean;
+	}
+
+	err = tegra_se_alloc_ll_buf(se_dev, SE_MAX_SRC_SG_COUNT,
+		SE_MAX_DST_SG_COUNT);
+	if (err) {
+		dev_err(se_dev->dev, "can not allocate ll dma buffer\n");
+		goto clean;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(aes_algs); i++) {
+		INIT_LIST_HEAD(&aes_algs[i].cra_list);
+		err = crypto_register_alg(&aes_algs[i]);
+		if (err) {
+			dev_err(se_dev->dev,
+				"crypto_register_alg failed index[%d]\n", i);
+			goto clean;
+		}
+	}
+
+	for (j = 0; j < ARRAY_SIZE(hash_algs); j++) {
+		err = crypto_register_ahash(&hash_algs[j]);
+		if (err) {
+			dev_err(se_dev->dev,
+			"crypto_register_sha alg failed index[%d]\n", i);
+			goto clean;
+		}
+	}
+
+#if defined(CONFIG_PM)
+	se_dev->ctx_save_buf = dma_alloc_coherent(se_dev->dev,
+		SE_CONTEXT_BUFER_SIZE, &se_dev->ctx_save_buf_adr, GFP_KERNEL);
+	if (!se_dev->ctx_save_buf) {
+		dev_err(se_dev->dev, "Context save buffer alloc filed\n");
+		goto clean;
+	}
+#endif
+
+	dev_info(se_dev->dev, "%s: complete", __func__);
+	return 0;
+
+clean:
+	pm_runtime_disable(se_dev->dev);
+	for (k = 0; k < i; k++)
+		crypto_unregister_alg(&aes_algs[k]);
+
+	for (k = 0; k < j; k++)
+		crypto_unregister_ahash(&hash_algs[j]);
+
+	tegra_se_free_ll_buf(se_dev);
+
+	if (se_work_q)
+		destroy_workqueue(se_work_q);
+
+	if (se_dev->pclk)
+		clk_put(se_dev->pclk);
+
+	free_irq(se_dev->irq, &pdev->dev);
+
+err_irq:
+	iounmap(se_dev->pmc_io_reg);
+err_pmc:
+	iounmap(se_dev->io_reg);
+
+fail:
+	platform_set_drvdata(pdev, NULL);
+	kfree(se_dev);
+	sg_tegra_se_dev = NULL;
+
+	return err;
+}
+
+static int __devexit tegra_se_remove(struct platform_device *pdev)
+{
+	struct tegra_se_dev *se_dev = platform_get_drvdata(pdev);
+	int i;
+
+	if (!se_dev)
+		return -ENODEV;
+
+	pm_runtime_disable(se_dev->dev);
+
+	cancel_work_sync(&se_work);
+	if (se_work_q)
+		destroy_workqueue(se_work_q);
+	free_irq(se_dev->irq, &pdev->dev);
+	for (i = 0; i < ARRAY_SIZE(aes_algs); i++)
+		crypto_unregister_alg(&aes_algs[i]);
+	for (i = 0; i < ARRAY_SIZE(hash_algs); i++)
+		crypto_unregister_ahash(&hash_algs[i]);
+	if (se_dev->pclk)
+		clk_put(se_dev->pclk);
+	tegra_se_free_ll_buf(se_dev);
+	if (se_dev->ctx_save_buf) {
+		dma_free_coherent(se_dev->dev, SE_CONTEXT_BUFER_SIZE,
+			se_dev->ctx_save_buf, se_dev->ctx_save_buf_adr);
+		se_dev->ctx_save_buf = NULL;
+	}
+	iounmap(se_dev->io_reg);
+	iounmap(se_dev->pmc_io_reg);
+	kfree(se_dev);
+	sg_tegra_se_dev = NULL;
+
+	return 0;
+}
+
+#if defined(CONFIG_PM)
+static int tegra_se_resume(struct device *dev)
+{
+	return 0;
+}
+
+static int tegra_se_generate_rng_key(struct tegra_se_dev *se_dev)
+{
+	int ret = 0;
+	u32 val = 0;
+
+	*se_dev->src_ll_buf = 0;
+	*se_dev->dst_ll_buf = 0;
+
+	/* Configure algorithm */
+	val = SE_CONFIG_ENC_ALG(ALG_RNG) | SE_CONFIG_ENC_MODE(MODE_KEY128) |
+		SE_CONFIG_DST(DST_KEYTAB);
+	se_writel(se_dev, val, SE_CONFIG_REG_OFFSET);
+
+	/* Configure destination key index number */
+	val = SE_CRYPTO_KEYTABLE_DST_KEY_INDEX(srk_slot.slot_num) |
+		SE_CRYPTO_KEYTABLE_DST_WORD_QUAD(KEYS_0_3);
+	se_writel(se_dev, val, SE_CRYPTO_KEYTABLE_DST_REG_OFFSET);
+
+	/* Configure crypto */
+	val = SE_CRYPTO_INPUT_SEL(INPUT_LFSR) | SE_CRYPTO_XOR_POS(XOR_BYPASS) |
+		SE_CRYPTO_CORE_SEL(CORE_ENCRYPT) |
+		SE_CRYPTO_HASH(HASH_DISABLE) |
+		SE_CRYPTO_KEY_INDEX(ssk_slot.slot_num) |
+		SE_CRYPTO_IV_SEL(IV_ORIGINAL);
+	se_writel(se_dev, val, SE_CRYPTO_REG_OFFSET);
+
+	ret = tegra_se_start_operation(se_dev, TEGRA_SE_KEY_128_SIZE, false);
+
+	return ret;
+}
+
+static int tegra_se_generate_srk(struct tegra_se_dev *se_dev)
+{
+	int ret = 0;
+	u32 val = 0;
+
+	mutex_lock(&se_hw_lock);
+	pm_runtime_get_sync(se_dev->dev);
+
+	ret = tegra_se_generate_rng_key(se_dev);
+	if (ret) {
+		pm_runtime_put(se_dev->dev);
+		mutex_unlock(&se_hw_lock);
+		return ret;
+	}
+
+	*se_dev->src_ll_buf = 0;
+	*se_dev->dst_ll_buf = 0;
+
+	val = SE_CONFIG_ENC_ALG(ALG_RNG) | SE_CONFIG_ENC_MODE(MODE_KEY128) |
+		SE_CONFIG_DEC_ALG(ALG_NOP) | SE_CONFIG_DST(DST_SRK);
+
+	se_writel(se_dev, val, SE_CONFIG_REG_OFFSET);
+
+	val = SE_CRYPTO_XOR_POS(XOR_BYPASS) |
+		SE_CRYPTO_CORE_SEL(CORE_ENCRYPT) |
+		SE_CRYPTO_HASH(HASH_DISABLE) |
+		SE_CRYPTO_KEY_INDEX(srk_slot.slot_num) |
+		SE_CRYPTO_IV_SEL(IV_UPDATED);
+
+	se_writel(se_dev, val, SE_CRYPTO_REG_OFFSET);
+	ret = tegra_se_start_operation(se_dev, TEGRA_SE_KEY_128_SIZE, false);
+
+	pm_runtime_put(se_dev->dev);
+	mutex_unlock(&se_hw_lock);
+
+	return ret;
+}
+
+static int tegra_se_lp_generate_random_data(struct tegra_se_dev *se_dev)
+{
+	struct tegra_se_ll *src_ll, *dst_ll;
+	int ret = 0;
+	u32 val;
+
+	mutex_lock(&se_hw_lock);
+	pm_runtime_get_sync(se_dev->dev);
+
+	*se_dev->src_ll_buf = 0;
+	*se_dev->dst_ll_buf = 0;
+	src_ll = (struct tegra_se_ll *)(se_dev->src_ll_buf + 1);
+	dst_ll = (struct tegra_se_ll *)(se_dev->dst_ll_buf + 1);
+	src_ll->addr = se_dev->ctx_save_buf_adr;
+	src_ll->data_len = SE_CONTEXT_SAVE_RANDOM_DATA_SIZE;
+	dst_ll->addr = se_dev->ctx_save_buf_adr;
+	dst_ll->data_len = SE_CONTEXT_SAVE_RANDOM_DATA_SIZE;
+
+	tegra_se_config_algo(se_dev, SE_AES_OP_MODE_RNG_X931, true,
+		TEGRA_SE_KEY_128_SIZE);
+
+	/* Configure crypto */
+	val = SE_CRYPTO_INPUT_SEL(INPUT_LFSR) | SE_CRYPTO_XOR_POS(XOR_BYPASS) |
+		SE_CRYPTO_CORE_SEL(CORE_ENCRYPT) |
+		SE_CRYPTO_HASH(HASH_DISABLE) |
+		SE_CRYPTO_KEY_INDEX(srk_slot.slot_num) |
+		SE_CRYPTO_IV_SEL(IV_ORIGINAL);
+
+	se_writel(se_dev, val, SE_CRYPTO_REG_OFFSET);
+	ret = tegra_se_start_operation(se_dev,
+		SE_CONTEXT_SAVE_RANDOM_DATA_SIZE, false);
+
+	pm_runtime_put(se_dev->dev);
+	mutex_unlock(&se_hw_lock);
+
+	return ret;
+
+}
+
+static int tegra_se_lp_encrypt_context_data(struct tegra_se_dev *se_dev,
+	u32 context_offset, u32 data_size)
+{
+	struct tegra_se_ll *src_ll, *dst_ll;
+	int ret = 0;
+
+	mutex_lock(&se_hw_lock);
+	pm_runtime_get_sync(se_dev->dev);
+
+	*se_dev->src_ll_buf = 0;
+	*se_dev->dst_ll_buf = 0;
+	src_ll = (struct tegra_se_ll *)(se_dev->src_ll_buf + 1);
+	dst_ll = (struct tegra_se_ll *)(se_dev->dst_ll_buf + 1);
+	src_ll->addr = se_dev->ctx_save_buf_adr + context_offset;
+	src_ll->data_len = data_size;
+	dst_ll->addr = se_dev->ctx_save_buf_adr + context_offset;
+	dst_ll->data_len = data_size;
+
+	se_writel(se_dev, SE_CONTEXT_SAVE_SRC(MEM),
+		SE_CONTEXT_SAVE_CONFIG_REG_OFFSET);
+
+	ret = tegra_se_start_operation(se_dev, data_size, true);
+
+	pm_runtime_put(se_dev->dev);
+
+	mutex_unlock(&se_hw_lock);
+
+	return ret;
+}
+
+static int tegra_se_lp_sticky_bits_context_save(struct tegra_se_dev *se_dev)
+{
+	struct tegra_se_ll *dst_ll;
+	int ret = 0;
+
+	mutex_lock(&se_hw_lock);
+	pm_runtime_get_sync(se_dev->dev);
+
+	*se_dev->src_ll_buf = 0;
+	*se_dev->dst_ll_buf = 0;
+	dst_ll = (struct tegra_se_ll *)(se_dev->dst_ll_buf + 1);
+	dst_ll->addr = (se_dev->ctx_save_buf_adr +
+		SE_CONTEXT_SAVE_STICKY_BITS_OFFSET);
+	dst_ll->data_len = SE_CONTEXT_SAVE_STICKY_BITS_SIZE;
+
+	se_writel(se_dev, SE_CONTEXT_SAVE_SRC(STICKY_BITS),
+		SE_CONTEXT_SAVE_CONFIG_REG_OFFSET);
+
+	ret = tegra_se_start_operation(se_dev,
+		SE_CONTEXT_SAVE_STICKY_BITS_SIZE, true);
+
+	pm_runtime_put(se_dev->dev);
+	mutex_unlock(&se_hw_lock);
+
+	return ret;
+}
+
+static int tegra_se_lp_keytable_context_save(struct tegra_se_dev *se_dev)
+{
+	struct tegra_se_ll *dst_ll;
+	int ret = 0, i, j;
+	u32 val = 0;
+
+	/* take access to the hw */
+	mutex_lock(&se_hw_lock);
+	pm_runtime_get_sync(se_dev->dev);
+
+	*se_dev->dst_ll_buf = 0;
+	dst_ll = (struct tegra_se_ll *)(se_dev->dst_ll_buf + 1);
+	dst_ll->addr = (se_dev->ctx_save_buf_adr + SE_CONTEXT_SAVE_KEYS_OFFSET);
+	dst_ll->data_len = TEGRA_SE_KEY_128_SIZE;
+
+	for (i = 0; i < TEGRA_SE_KEYSLOT_COUNT; i++) {
+		for (j = 0; j < 2; j++) {
+			val = SE_CONTEXT_SAVE_SRC(KEYTABLE) |
+				SE_CONTEXT_SAVE_KEY_INDEX(i) |
+				SE_CONTEXT_SAVE_WORD_QUAD(j);
+			se_writel(se_dev,
+				val, SE_CONTEXT_SAVE_CONFIG_REG_OFFSET);
+			ret = tegra_se_start_operation(se_dev,
+				TEGRA_SE_KEY_128_SIZE, true);
+			if (ret)
+				break;
+			dst_ll->addr += TEGRA_SE_KEY_128_SIZE;
+		}
+	}
+
+	pm_runtime_put(se_dev->dev);
+	mutex_unlock(&se_hw_lock);
+
+	return ret;
+}
+
+static int tegra_se_lp_iv_context_save(struct tegra_se_dev *se_dev,
+	bool org_iv, u32 context_offset)
+{
+	struct tegra_se_ll *dst_ll;
+	int ret = 0, i;
+	u32 val = 0;
+
+	mutex_lock(&se_hw_lock);
+	pm_runtime_get_sync(se_dev->dev);
+
+	*se_dev->dst_ll_buf = 0;
+	dst_ll = (struct tegra_se_ll *)(se_dev->dst_ll_buf + 1);
+	dst_ll->addr = (se_dev->ctx_save_buf_adr + context_offset);
+	dst_ll->data_len = TEGRA_SE_AES_IV_SIZE;
+
+	for (i = 0; i < TEGRA_SE_KEYSLOT_COUNT; i++) {
+		val = SE_CONTEXT_SAVE_SRC(KEYTABLE) |
+			SE_CONTEXT_SAVE_KEY_INDEX(i) |
+			(org_iv ? SE_CONTEXT_SAVE_WORD_QUAD(ORIG_IV) :
+			SE_CONTEXT_SAVE_WORD_QUAD(UPD_IV));
+		se_writel(se_dev, val, SE_CONTEXT_SAVE_CONFIG_REG_OFFSET);
+		ret = tegra_se_start_operation(se_dev,
+			TEGRA_SE_AES_IV_SIZE, true);
+		if (ret)
+			break;
+		dst_ll->addr += TEGRA_SE_AES_IV_SIZE;
+	}
+
+	pm_runtime_put(se_dev->dev);
+	mutex_unlock(&se_hw_lock);
+
+	return ret;
+}
+
+static int tegra_se_save_SRK(struct tegra_se_dev *se_dev)
+{
+	int ret = 0;
+
+	mutex_lock(&se_hw_lock);
+	pm_runtime_get_sync(se_dev->dev);
+
+	se_writel(se_dev, SE_CONTEXT_SAVE_SRC(SRK),
+		SE_CONTEXT_SAVE_CONFIG_REG_OFFSET);
+	ret = tegra_se_start_operation(se_dev, 0, true);
+
+	pm_runtime_put(se_dev->dev);
+	mutex_unlock(&se_hw_lock);
+
+	return ret;
+}
+
+static int tegra_se_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct tegra_se_dev *se_dev = platform_get_drvdata(pdev);
+	int err = 0, i;
+	unsigned char *dt_buf = NULL;
+	u8 pdata[SE_CONTEXT_KNOWN_PATTERN_SIZE] = {
+		0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+		0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f};
+
+	if (!se_dev)
+		return -ENODEV;
+
+	/* Generate SRK */
+	err = tegra_se_generate_srk(se_dev);
+	if (err) {
+		dev_err(se_dev->dev, "\n LP SRK genration failed\n");
+		goto out;
+	}
+
+	/* Generate random data*/
+	err = tegra_se_lp_generate_random_data(se_dev);
+	if (err) {
+		dev_err(se_dev->dev, "\n LP random pattern generation failed\n");
+		goto out;
+	}
+
+	/* Encrypt random data */
+	err = tegra_se_lp_encrypt_context_data(se_dev,
+		SE_CONTEXT_SAVE_RANDOM_DATA_OFFSET,
+		SE_CONTEXT_SAVE_RANDOM_DATA_SIZE);
+	if (err) {
+		dev_err(se_dev->dev, "\n LP random pattern encryption failed\n");
+		goto out;
+	}
+
+	/* Sticky bits context save*/
+	err = tegra_se_lp_sticky_bits_context_save(se_dev);
+	if (err) {
+		dev_err(se_dev->dev, "\n LP sticky bits context save failure\n");
+		goto out;
+	}
+
+	/* Key table context save*/
+	err = tegra_se_lp_keytable_context_save(se_dev);
+	if (err) {
+		dev_err(se_dev->dev, "\n LP key table  save failure\n");
+		goto out;
+	}
+
+	/* Original iv context save*/
+	err = tegra_se_lp_iv_context_save(se_dev,
+		true, SE_CONTEXT_ORIGINAL_IV_OFFSET);
+	if (err) {
+		dev_err(se_dev->dev, "\n LP original iv save failure\n");
+		goto out;
+	}
+
+	/* UPdated iv context save*/
+	err = tegra_se_lp_iv_context_save(se_dev,
+		false, SE_CONTEXT_UPDATED_IV_OFFSET);
+	if (err) {
+		dev_err(se_dev->dev, "\n LP updated iv save failure\n");
+		goto out;
+	}
+
+	/* Encrypt known pattern */
+	dt_buf = (unsigned char *)se_dev->ctx_save_buf;
+	dt_buf += SE_CONTEXT_KNOWN_PATTERN_OFFSET;
+	for (i = 0; i < SE_CONTEXT_KNOWN_PATTERN_SIZE; i++)
+		dt_buf[i] = pdata[i];
+	err = tegra_se_lp_encrypt_context_data(se_dev,
+		SE_CONTEXT_KNOWN_PATTERN_OFFSET, SE_CONTEXT_KNOWN_PATTERN_SIZE);
+	if (err) {
+		dev_err(se_dev->dev, "LP known pattern save failure\n");
+		goto out;
+	}
+
+	/* Write lp context buffer address into PMC scratch register */
+	writel(se_dev->ctx_save_buf_adr,
+		se_dev->pmc_io_reg + PMC_SCRATCH43_REG_OFFSET);
+
+	/* Saves SRK in secure scratch */
+	err = tegra_se_save_SRK(se_dev);
+	if (err) {
+		dev_err(se_dev->dev, "LP SRK save failure\n");
+		goto out;
+	}
+
+out:
+	return err;
+}
+#endif
+
+#if defined(CONFIG_PM_RUNTIME)
+static int tegra_se_runtime_suspend(struct device *dev)
+{
+	/*
+	 * do a dummy read, to avoid scenarios where you have unposted writes
+	 * still on the bus, before disabling clocks
+	 */
+	se_readl(sg_tegra_se_dev, SE_CONFIG_REG_OFFSET);
+
+	clk_disable(sg_tegra_se_dev->pclk);
+	return 0;
+}
+
+static int tegra_se_runtime_resume(struct device *dev)
+{
+	clk_enable(sg_tegra_se_dev->pclk);
+	return 0;
+}
+
+static const struct dev_pm_ops tegra_se_dev_pm_ops = {
+	.runtime_suspend = tegra_se_runtime_suspend,
+	.runtime_resume = tegra_se_runtime_resume,
+#if defined(CONFIG_PM)
+	.suspend = tegra_se_suspend,
+	.resume = tegra_se_resume,
+#endif
+};
+#endif
+
+static struct platform_driver tegra_se_driver = {
+	.probe  = tegra_se_probe,
+	.remove = __devexit_p(tegra_se_remove),
+	.driver = {
+		.name   = "tegra-se",
+		.owner  = THIS_MODULE,
+#if defined(CONFIG_PM_RUNTIME)
+		.pm = &tegra_se_dev_pm_ops,
+#endif
+	},
+};
+
+static int __init tegra_se_module_init(void)
+{
+	return  platform_driver_register(&tegra_se_driver);
+}
+
+static void __exit tegra_se_module_exit(void)
+{
+	platform_driver_unregister(&tegra_se_driver);
+}
+
+module_init(tegra_se_module_init);
+module_exit(tegra_se_module_exit);
+
+MODULE_DESCRIPTION("Tegra Crypto algorithm support");
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("tegra-se");
+
diff --git a/drivers/crypto/tegra-se.h b/drivers/crypto/tegra-se.h
new file mode 100644
index 000000000000..8c54df8991e6
--- /dev/null
+++ b/drivers/crypto/tegra-se.h
@@ -0,0 +1,235 @@
+/*
+ * Driver for Tegra Security Engine
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef _CRYPTO_TEGRA_SE_H
+#define _CRYPTO_TEGRA_SE_H
+
+#include <crypto/hash.h>
+#include <crypto/sha.h>
+
+#define PFX	"tegra-se: "
+
+#define TEGRA_SE_CRA_PRIORITY	300
+#define TEGRA_SE_COMPOSITE_PRIORITY 400
+#define TEGRA_SE_CRYPTO_QUEUE_LENGTH 50
+#define SE_MAX_SRC_SG_COUNT		50
+#define SE_MAX_DST_SG_COUNT		50
+
+#define TEGRA_SE_KEYSLOT_COUNT		16
+
+/* SE register definitions */
+#define SE_CONFIG_REG_OFFSET		0x014
+#define SE_CONFIG_ENC_ALG_SHIFT		12
+#define SE_CONFIG_DEC_ALG_SHIFT		8
+#define ALG_AES_ENC		1
+#define ALG_RNG			2
+#define ALG_SHA			3
+#define ALG_NOP			0
+#define ALG_AES_DEC		1
+#define SE_CONFIG_ENC_ALG(x)		(x << SE_CONFIG_ENC_ALG_SHIFT)
+#define SE_CONFIG_DEC_ALG(x)		(x << SE_CONFIG_DEC_ALG_SHIFT)
+#define SE_CONFIG_DST_SHIFT			2
+#define DST_MEMORY		0
+#define DST_HASHREG		1
+#define DST_KEYTAB		2
+#define DST_SRK			3
+#define SE_CONFIG_DST(x)			(x << SE_CONFIG_DST_SHIFT)
+#define SE_CONFIG_ENC_MODE_SHIFT	24
+#define SE_CONFIG_DEC_MODE_SHIFT	16
+#define MODE_KEY128		0
+#define MODE_KEY192		1
+#define MODE_KEY256		2
+#define MODE_SHA1		0
+#define MODE_SHA224		4
+#define MODE_SHA256		5
+#define MODE_SHA384		6
+#define MODE_SHA512		7
+#define SE_CONFIG_ENC_MODE(x)		(x << SE_CONFIG_ENC_MODE_SHIFT)
+#define SE_CONFIG_DEC_MODE(x)		(x << SE_CONFIG_DEC_MODE_SHIFT)
+
+#define SE_KEYTABLE_REG_OFFSET		0x31c
+#define SE_KEYTABLE_SLOT_SHIFT		4
+#define SE_KEYTABLE_SLOT(x)			(x << SE_KEYTABLE_SLOT_SHIFT)
+#define SE_KEYTABLE_QUAD_SHIFT		2
+#define QUAD_KEYS_128	0
+#define QUAD_KEYS_192	1
+#define QUAD_KEYS_256	1
+#define QUAD_ORG_IV		2
+#define QUAD_UPDTD_IV	3
+#define SE_KEYTABLE_QUAD(x)			(x << SE_KEYTABLE_QUAD_SHIFT)
+#define SE_KEYTABLE_OP_TYPE_SHIFT	9
+#define OP_READ			0
+#define OP_WRITE		1
+#define SE_KEYTABLE_OP_TYPE(x)		(x << SE_KEYTABLE_OP_TYPE_SHIFT)
+#define SE_KEYTABLE_TABLE_SEL_SHIFT		8
+#define TABLE_KEYIV		0
+#define TABLE_SCHEDULE	1
+#define SE_KEYTABLE_TABLE_SEL(x)	(x << SE_KEYTABLE_TABLE_SEL_SHIFT)
+#define SE_KEYTABLE_PKT_SHIFT		0
+#define SE_KEYTABLE_PKT(x)			(x << SE_KEYTABLE_PKT_SHIFT)
+
+#define SE_CRYPTO_REG_OFFSET		0x304
+#define SE_CRYPTO_HASH_SHIFT		0
+#define HASH_DISABLE	0
+#define HASH_ENABLE		1
+#define SE_CRYPTO_HASH(x)			(x << SE_CRYPTO_HASH_SHIFT)
+#define SE_CRYPTO_XOR_POS_SHIFT		1
+#define XOR_BYPASS		0
+#define XOR_TOP			2
+#define XOR_BOTTOM		3
+#define SE_CRYPTO_XOR_POS(x)		(x << SE_CRYPTO_XOR_POS_SHIFT)
+#define SE_CRYPTO_INPUT_SEL_SHIFT	3
+#define INPUT_AHB		0
+#define INPUT_LFSR		1
+#define INPUT_AESOUT	2
+#define INPUT_LNR_CTR	3
+#define SE_CRYPTO_INPUT_SEL(x)		(x << SE_CRYPTO_INPUT_SEL_SHIFT)
+#define SE_CRYPTO_VCTRAM_SEL_SHIFT	5
+#define VCTRAM_AHB		0
+#define VCTRAM_AESOUT	2
+#define VCTRAM_PREVAHB	3
+#define SE_CRYPTO_VCTRAM_SEL(x)		(x << SE_CRYPTO_VCTRAM_SEL_SHIFT)
+#define SE_CRYPTO_IV_SEL_SHIFT		7
+#define IV_ORIGINAL		0
+#define IV_UPDATED		1
+#define SE_CRYPTO_IV_SEL(x)			(x << SE_CRYPTO_IV_SEL_SHIFT)
+#define SE_CRYPTO_CORE_SEL_SHIFT	8
+#define CORE_DECRYPT	0
+#define CORE_ENCRYPT	1
+#define SE_CRYPTO_CORE_SEL(x)		(x << SE_CRYPTO_CORE_SEL_SHIFT)
+#define SE_CRYPTO_CTR_VAL_SHIFT		11
+#define SE_CRYPTO_CTR_VAL(x)		(x << SE_CRYPTO_CTR_VAL_SHIFT)
+#define SE_CRYPTO_KEY_INDEX_SHIFT	24
+#define SE_CRYPTO_KEY_INDEX(x)		(x << SE_CRYPTO_KEY_INDEX_SHIFT)
+#define SE_CRYPTO_CTR_CNTN_SHIFT	11
+#define SE_CRYPTO_CTR_CNTN(x)		(x << SE_CRYPTO_CTR_CNTN_SHIFT)
+
+#define SE_CRYPTO_CTR_REG_COUNT		4
+#define SE_CRYPTO_CTR_REG_OFFSET	0x308
+
+#define SE_OPERATION_REG_OFFSET		0x008
+#define SE_OPERATION_SHIFT			0
+#define OP_ABORT		0
+#define OP_SRART		1
+#define OP_RESTART		2
+#define OP_CTX_SAVE		3
+#define SE_OPERATION(x)				(x << SE_OPERATION_SHIFT)
+
+#define SE_CONTEXT_SAVE_CONFIG_REG_OFFSET		0x070
+#define SE_CONTEXT_SAVE_WORD_QUAD_SHIFT		0
+#define KEYS_0_3		0
+#define KEYS_4_7		1
+#define ORIG_IV		2
+#define UPD_IV		3
+#define SE_CONTEXT_SAVE_WORD_QUAD(x)	(x << SE_CONTEXT_SAVE_WORD_QUAD_SHIFT)
+
+#define SE_CONTEXT_SAVE_KEY_INDEX_SHIFT		8
+#define SE_CONTEXT_SAVE_KEY_INDEX(x)	(x << SE_CONTEXT_SAVE_KEY_INDEX_SHIFT)
+
+
+#define SE_CONTEXT_SAVE_SRC_SHIFT		30
+#define STICKY_BITS		0
+#define KEYTABLE		1
+#define MEM		2
+#define SRK		3
+#define SE_CONTEXT_SAVE_SRC(x)		(x << SE_CONTEXT_SAVE_SRC_SHIFT)
+
+#define SE_INT_ENABLE_REG_OFFSET	0x00c
+#define SE_INT_STATUS_REG_OFFSET	0x010
+#define INT_DISABLE		0
+#define INT_ENABLE		1
+#define INT_UNSET		0
+#define INT_SET			1
+#define SE_INT_OP_DONE_SHIFT		4
+#define SE_INT_OP_DONE(x)			(x << SE_INT_OP_DONE_SHIFT)
+#define SE_INT_ERROR_SHIFT		16
+#define SE_INT_ERROR(x)			(x << SE_INT_ERROR_SHIFT)
+
+#define SE_CRYPTO_KEYTABLE_DST_REG_OFFSET	0X330
+#define SE_CRYPTO_KEYTABLE_DST_WORD_QUAD_SHIFT		0
+#define SE_CRYPTO_KEYTABLE_DST_WORD_QUAD(x)		\
+		(x << SE_CRYPTO_KEYTABLE_DST_WORD_QUAD_SHIFT)
+
+#define SE_KEY_INDEX_SHIFT		8
+#define SE_CRYPTO_KEYTABLE_DST_KEY_INDEX(x)	(x << SE_KEY_INDEX_SHIFT)
+
+#define SE_IN_LL_ADDR_REG_OFFSET	0x018
+#define SE_OUT_LL_ADDR_REG_OFFSET	0x024
+
+#define SE_KEYTABLE_DATA0_REG_OFFSET	0x320
+#define SE_KEYTABLE_REG_MAX_DATA		16
+
+#define SE_BLOCK_COUNT_REG_OFFSET	0x318
+
+#define SE_SPARE_0_REG_OFFSET		0x80c
+
+#define SE_SHA_CONFIG_REG_OFFSET	0x200
+#define SHA_DISABLE		0
+#define SHA_ENABLE		1
+
+#define SE_SHA_MSG_LENGTH_REG_OFFSET	0x204
+#define SE_SHA_MSG_LEFT_REG_OFFSET		0x214
+
+
+#define SE_HASH_RESULT_REG_COUNT	16
+#define SE_HASH_RESULT_REG_OFFSET	0x030
+
+
+#define TEGRA_SE_KEY_256_SIZE		32
+#define TEGRA_SE_KEY_192_SIZE		24
+#define TEGRA_SE_KEY_128_SIZE		16
+#define TEGRA_SE_AES_BLOCK_SIZE		16
+#define TEGRA_SE_AES_MIN_KEY_SIZE	16
+#define TEGRA_SE_AES_MAX_KEY_SIZE	32
+#define TEGRA_SE_AES_IV_SIZE		16
+#define TEGRA_SE_RNG_IV_SIZE		16
+#define TEGRA_SE_RNG_DT_SIZE		16
+#define TEGRA_SE_RNG_KEY_SIZE		16
+#define TEGRA_SE_RNG_SEED_SIZE		(TEGRA_SE_RNG_IV_SIZE + \
+						TEGRA_SE_RNG_KEY_SIZE + \
+						TEGRA_SE_RNG_DT_SIZE)
+#define TEGRA_SE_AES_CMAC_DIGEST_SIZE	16
+
+#define SE_KEY_TABLE_ACCESS_REG_OFFSET	0x284
+#define SE_KEY_READ_DISABLE_SHIFT		0
+
+#define SE_CONTEXT_BUFER_SIZE	1072
+#define SE_CONTEXT_SAVE_RANDOM_DATA_OFFSET	0
+#define SE_CONTEXT_SAVE_RANDOM_DATA_SIZE	16
+#define SE_CONTEXT_SAVE_STICKY_BITS_OFFSET	\
+	(SE_CONTEXT_SAVE_RANDOM_DATA_OFFSET + SE_CONTEXT_SAVE_RANDOM_DATA_SIZE)
+#define SE_CONTEXT_SAVE_STICKY_BITS_SIZE	16
+#define SE_CONTEXT_SAVE_KEYS_OFFSET	(SE_CONTEXT_SAVE_STICKY_BITS_OFFSET + \
+					SE_CONTEXT_SAVE_STICKY_BITS_SIZE)
+#define SE_CONTEXT_SAVE_KEY_LENGTH	512
+#define SE_CONTEXT_ORIGINAL_IV_OFFSET	(SE_CONTEXT_SAVE_KEYS_OFFSET + \
+			SE_CONTEXT_SAVE_KEY_LENGTH)
+#define SE_CONTEXT_ORIGINAL_IV_LENGTH	256
+
+#define SE_CONTEXT_UPDATED_IV_OFFSET	(SE_CONTEXT_ORIGINAL_IV_OFFSET + \
+			SE_CONTEXT_ORIGINAL_IV_LENGTH)
+
+#define SE_CONTEXT_UPDATED_IV_LENGTH	256
+#define SE_CONTEXT_KNOWN_PATTERN_OFFSET	(SE_CONTEXT_UPDATED_IV_OFFSET + \
+			SE_CONTEXT_UPDATED_IV_LENGTH)
+#define SE_CONTEXT_KNOWN_PATTERN_SIZE	16
+
+
+#endif /* _CRYPTO_TEGRA_SE_H */
diff --git a/drivers/dma/amba-pl08x.c b/drivers/dma/amba-pl08x.c
index be21e3f138a8..becfdb4c4f98 100644
--- a/drivers/dma/amba-pl08x.c
+++ b/drivers/dma/amba-pl08x.c
@@ -90,6 +90,8 @@
 
 #include <asm/hardware/pl080.h>
 
+#include "dmaengine.h"
+
 #define DRIVER_NAME	"pl08xdmac"
 
 /**
@@ -966,13 +968,10 @@ static dma_cookie_t pl08x_tx_submit(struct dma_async_tx_descriptor *tx)
 	struct pl08x_dma_chan *plchan = to_pl08x_chan(tx->chan);
 	struct pl08x_txd *txd = to_pl08x_txd(tx);
 	unsigned long flags;
+	dma_cookie_t cookie;
 
 	spin_lock_irqsave(&plchan->lock, flags);
-
-	plchan->chan.cookie += 1;
-	if (plchan->chan.cookie < 0)
-		plchan->chan.cookie = 1;
-	tx->cookie = plchan->chan.cookie;
+	cookie = dma_cookie_assign(tx);
 
 	/* Put this onto the pending list */
 	list_add_tail(&txd->node, &plchan->pend_list);
@@ -992,7 +991,7 @@ static dma_cookie_t pl08x_tx_submit(struct dma_async_tx_descriptor *tx)
 
 	spin_unlock_irqrestore(&plchan->lock, flags);
 
-	return tx->cookie;
+	return cookie;
 }
 
 static struct dma_async_tx_descriptor *pl08x_prep_dma_interrupt(
@@ -1014,31 +1013,17 @@ pl08x_dma_tx_status(struct dma_chan *chan,
 		    struct dma_tx_state *txstate)
 {
 	struct pl08x_dma_chan *plchan = to_pl08x_chan(chan);
-	dma_cookie_t last_used;
-	dma_cookie_t last_complete;
 	enum dma_status ret;
-	u32 bytesleft = 0;
-
-	last_used = plchan->chan.cookie;
-	last_complete = plchan->lc;
 
-	ret = dma_async_is_complete(cookie, last_complete, last_used);
-	if (ret == DMA_SUCCESS) {
-		dma_set_tx_state(txstate, last_complete, last_used, 0);
+	ret = dma_cookie_status(chan, cookie, txstate);
+	if (ret == DMA_SUCCESS)
 		return ret;
-	}
 
 	/*
 	 * This cookie not complete yet
+	 * Get number of bytes left in the active transactions and queue
 	 */
-	last_used = plchan->chan.cookie;
-	last_complete = plchan->lc;
-
-	/* Get number of bytes left in the active transactions and queue */
-	bytesleft = pl08x_getbytes_chan(plchan);
-
-	dma_set_tx_state(txstate, last_complete, last_used,
-			 bytesleft);
+	dma_set_residue(txstate, pl08x_getbytes_chan(plchan));
 
 	if (plchan->state == PL08X_CHAN_PAUSED)
 		return DMA_PAUSED;
@@ -1361,8 +1346,7 @@ static struct dma_async_tx_descriptor *pl08x_prep_dma_memcpy(
 
 static struct dma_async_tx_descriptor *pl08x_prep_slave_sg(
 		struct dma_chan *chan, struct scatterlist *sgl,
-		unsigned int sg_len, enum dma_data_direction direction,
-		unsigned long flags)
+		unsigned long flags, void *context)
 {
 	struct pl08x_dma_chan *plchan = to_pl08x_chan(chan);
 	struct pl08x_driver_data *pl08x = plchan->host;
@@ -1552,7 +1536,7 @@ static void pl08x_tasklet(unsigned long data)
 
 	if (txd) {
 		/* Update last completed */
-		plchan->lc = txd->tx.cookie;
+		dma_cookie_complete(&txd->tx);
 	}
 
 	/* If a new descriptor is queued, set it up plchan->at is NULL here */
@@ -1733,8 +1717,7 @@ static int pl08x_dma_init_virtual_channels(struct pl08x_driver_data *pl08x,
 			 chan->name);
 
 		chan->chan.device = dmadev;
-		chan->chan.cookie = 0;
-		chan->lc = 0;
+		dma_cookie_init(&chan->chan);
 
 		spin_lock_init(&chan->lock);
 		INIT_LIST_HEAD(&chan->pend_list);
diff --git a/drivers/dma/at_hdmac.c b/drivers/dma/at_hdmac.c
index 6a483eac7b3f..24fdf638358b 100644
--- a/drivers/dma/at_hdmac.c
+++ b/drivers/dma/at_hdmac.c
@@ -25,6 +25,7 @@
 #include <linux/slab.h>
 
 #include "at_hdmac_regs.h"
+#include "dmaengine.h"
 
 /*
  * Glossary
@@ -188,27 +189,6 @@ static void atc_desc_chain(struct at_desc **first, struct at_desc **prev,
 }
 
 /**
- * atc_assign_cookie - compute and assign new cookie
- * @atchan: channel we work on
- * @desc: descriptor to assign cookie for
- *
- * Called with atchan->lock held and bh disabled
- */
-static dma_cookie_t
-atc_assign_cookie(struct at_dma_chan *atchan, struct at_desc *desc)
-{
-	dma_cookie_t cookie = atchan->chan_common.cookie;
-
-	if (++cookie < 0)
-		cookie = 1;
-
-	atchan->chan_common.cookie = cookie;
-	desc->txd.cookie = cookie;
-
-	return cookie;
-}
-
-/**
  * atc_dostart - starts the DMA engine for real
  * @atchan: the channel we want to start
  * @first: first descriptor in the list we want to begin with
@@ -265,7 +245,7 @@ atc_chain_complete(struct at_dma_chan *atchan, struct at_desc *desc)
 	dev_vdbg(chan2dev(&atchan->chan_common),
 		"descriptor %u complete\n", txd->cookie);
 
-	atchan->completed_cookie = txd->cookie;
+	dma_cookie_complete(txd);
 
 	/* move children to free_list */
 	list_splice_init(&desc->tx_list, &atchan->free_list);
@@ -540,8 +520,8 @@ static dma_cookie_t atc_tx_submit(struct dma_async_tx_descriptor *tx)
 	struct at_dma_chan	*atchan = to_at_dma_chan(tx->chan);
 	dma_cookie_t		cookie;
 
-	spin_lock_bh(&atchan->lock);
-	cookie = atc_assign_cookie(atchan, desc);
+	spin_lock_irqsave(&atchan->lock, flags);
+	cookie = dma_cookie_assign(tx);
 
 	if (list_empty(&atchan->active_list)) {
 		dev_vdbg(chan2dev(tx->chan), "tx_submit: started %u\n",
@@ -653,11 +633,12 @@ err_desc_get:
  * @sg_len: number of entries in @scatterlist
  * @direction: DMA direction
  * @flags: tx descriptor status flags
+ * @context: transaction context (ignored)
  */
 static struct dma_async_tx_descriptor *
 atc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
-		unsigned int sg_len, enum dma_data_direction direction,
-		unsigned long flags)
+		unsigned int sg_len, enum dma_transfer_direction direction,
+		unsigned long flags, void *context)
 {
 	struct at_dma_chan	*atchan = to_at_dma_chan(chan);
 	struct at_dma_slave	*atslave = chan->private;
@@ -854,10 +835,12 @@ atc_dma_cyclic_fill_desc(struct at_dma_slave *atslave, struct at_desc *desc,
  * @buf_len: total number of bytes for the entire buffer
  * @period_len: number of bytes for each period
  * @direction: transfer direction, to or from device
+ * @context: transfer context (ignored)
  */
 static struct dma_async_tx_descriptor *
 atc_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t buf_addr, size_t buf_len,
-		size_t period_len, enum dma_data_direction direction)
+		size_t period_len, enum dma_transfer_direction direction,
+		void *context)
 {
 	struct at_dma_chan	*atchan = to_at_dma_chan(chan);
 	struct at_dma_slave	*atslave = chan->private;
@@ -1008,26 +991,20 @@ atc_tx_status(struct dma_chan *chan,
 
 	spin_lock_bh(&atchan->lock);
 
-	last_complete = atchan->completed_cookie;
-	last_used = chan->cookie;
-
-	ret = dma_async_is_complete(cookie, last_complete, last_used);
+	ret = dma_cookie_status(chan, cookie, txstate);
 	if (ret != DMA_SUCCESS) {
 		atc_cleanup_descriptors(atchan);
 
-		last_complete = atchan->completed_cookie;
-		last_used = chan->cookie;
-
-		ret = dma_async_is_complete(cookie, last_complete, last_used);
+		ret = dma_cookie_status(chan, cookie, txstate);
 	}
 
+	last_complete = chan->completed_cookie;
+	last_used = chan->cookie;
+
 	spin_unlock_bh(&atchan->lock);
 
 	if (ret != DMA_SUCCESS)
-		dma_set_tx_state(txstate, last_complete, last_used,
-			atc_first_active(atchan)->len);
-	else
-		dma_set_tx_state(txstate, last_complete, last_used, 0);
+		dma_set_residue(txstate, atc_first_active(atchan)->len);
 
 	if (test_bit(ATC_IS_PAUSED, &atchan->status))
 		ret = DMA_PAUSED;
@@ -1119,7 +1096,7 @@ static int atc_alloc_chan_resources(struct dma_chan *chan)
 	spin_lock_bh(&atchan->lock);
 	atchan->descs_allocated = i;
 	list_splice(&tmp_list, &atchan->free_list);
-	atchan->completed_cookie = chan->cookie = 1;
+	dma_cookie_init(chan);
 	spin_unlock_bh(&atchan->lock);
 
 	/* channel parameters */
@@ -1264,8 +1241,8 @@ static int __init at_dma_probe(struct platform_device *pdev)
 		struct at_dma_chan	*atchan = &atdma->chan[i];
 
 		atchan->chan_common.device = &atdma->dma_common;
-		atchan->chan_common.cookie = atchan->completed_cookie = 1;
 		atchan->chan_common.chan_id = i;
+		dma_cookie_init(&atchan->chan_common);
 		list_add_tail(&atchan->chan_common.device_node,
 				&atdma->dma_common.channels);
 
diff --git a/drivers/dma/at_hdmac_regs.h b/drivers/dma/at_hdmac_regs.h
index 087dbf1dd39c..1f5e4bce2094 100644
--- a/drivers/dma/at_hdmac_regs.h
+++ b/drivers/dma/at_hdmac_regs.h
@@ -205,7 +205,6 @@ enum atc_status {
  *                to tasklet (use atomic operations)
  * @tasklet: bottom half to finish transaction work
  * @lock: serializes enqueue/dequeue operations to descriptors lists
- * @completed_cookie: identifier for the most recently completed operation
  * @active_list: list of descriptors dmaengine is being running on
  * @queue: list of descriptors ready to be submitted to engine
  * @free_list: list of descriptors usable by the channel
@@ -222,7 +221,6 @@ struct at_dma_chan {
 	spinlock_t		lock;
 
 	/* these other elements are all protected by lock */
-	dma_cookie_t		completed_cookie;
 	struct list_head	active_list;
 	struct list_head	queue;
 	struct list_head	free_list;
diff --git a/drivers/dma/coh901318.c b/drivers/dma/coh901318.c
index 4234f416ef11..368673c9f73d 100644
--- a/drivers/dma/coh901318.c
+++ b/drivers/dma/coh901318.c
@@ -24,6 +24,7 @@
 #include <mach/coh901318.h>
 
 #include "coh901318_lli.h"
+#include "dmaengine.h"
 
 #define COHC_2_DEV(cohc) (&cohc->chan.dev->device)
 
@@ -59,7 +60,6 @@ struct coh901318_base {
 struct coh901318_chan {
 	spinlock_t lock;
 	int allocated;
-	int completed;
 	int id;
 	int stopped;
 
@@ -318,20 +318,6 @@ static int coh901318_prep_linked_list(struct coh901318_chan *cohc,
 
 	return 0;
 }
-static dma_cookie_t
-coh901318_assign_cookie(struct coh901318_chan *cohc,
-			struct coh901318_desc *cohd)
-{
-	dma_cookie_t cookie = cohc->chan.cookie;
-
-	if (++cookie < 0)
-		cookie = 1;
-
-	cohc->chan.cookie = cookie;
-	cohd->desc.cookie = cookie;
-
-	return cookie;
-}
 
 static struct coh901318_desc *
 coh901318_desc_get(struct coh901318_chan *cohc)
@@ -705,7 +691,7 @@ static void dma_tasklet(unsigned long data)
 	callback_param = cohd_fin->desc.callback_param;
 
 	/* sign this job as completed on the channel */
-	cohc->completed = cohd_fin->desc.cookie;
+	dma_cookie_complete(&cohd_fin->desc);
 
 	/* release the lli allocation and remove the descriptor */
 	coh901318_lli_free(&cohc->base->pool, &cohd_fin->lli);
@@ -929,7 +915,7 @@ static int coh901318_alloc_chan_resources(struct dma_chan *chan)
 	coh901318_config(cohc, NULL);
 
 	cohc->allocated = 1;
-	cohc->completed = chan->cookie = 1;
+	dma_cookie_init(chan);
 
 	spin_unlock_irqrestore(&cohc->lock, flags);
 
@@ -966,16 +952,16 @@ coh901318_tx_submit(struct dma_async_tx_descriptor *tx)
 						   desc);
 	struct coh901318_chan *cohc = to_coh901318_chan(tx->chan);
 	unsigned long flags;
+	dma_cookie_t cookie;
 
 	spin_lock_irqsave(&cohc->lock, flags);
-
-	tx->cookie = coh901318_assign_cookie(cohc, cohd);
+	cookie = dma_cookie_assign(tx);
 
 	coh901318_desc_queue(cohc, cohd);
 
 	spin_unlock_irqrestore(&cohc->lock, flags);
 
-	return tx->cookie;
+	return cookie;
 }
 
 static struct dma_async_tx_descriptor *
@@ -1034,8 +1020,8 @@ coh901318_prep_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 
 static struct dma_async_tx_descriptor *
 coh901318_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
-			unsigned int sg_len, enum dma_data_direction direction,
-			unsigned long flags)
+			unsigned int sg_len, enum dma_transfer_direction direction,
+			unsigned long flags, void *context)
 {
 	struct coh901318_chan *cohc = to_coh901318_chan(chan);
 	struct coh901318_lli *lli;
@@ -1165,17 +1151,12 @@ coh901318_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 		 struct dma_tx_state *txstate)
 {
 	struct coh901318_chan *cohc = to_coh901318_chan(chan);
-	dma_cookie_t last_used;
-	dma_cookie_t last_complete;
-	int ret;
-
-	last_complete = cohc->completed;
-	last_used = chan->cookie;
+	enum dma_status ret;
 
-	ret = dma_async_is_complete(cookie, last_complete, last_used);
+	ret = dma_cookie_status(chan, cookie, txstate);
+	/* FIXME: should be conditional on ret != DMA_SUCCESS? */
+	dma_set_residue(txstate, coh901318_get_bytes_left(chan));
 
-	dma_set_tx_state(txstate, last_complete, last_used,
-			 coh901318_get_bytes_left(chan));
 	if (ret == DMA_IN_PROGRESS && cohc->stopped)
 		ret = DMA_PAUSED;
 
diff --git a/drivers/dma/dmaengine.h b/drivers/dma/dmaengine.h
new file mode 100644
index 000000000000..17f983a4e9ba
--- /dev/null
+++ b/drivers/dma/dmaengine.h
@@ -0,0 +1,89 @@
+/*
+ * The contents of this file are private to DMA engine drivers, and is not
+ * part of the API to be used by DMA engine users.
+ */
+#ifndef DMAENGINE_H
+#define DMAENGINE_H
+
+#include <linux/bug.h>
+#include <linux/dmaengine.h>
+
+/**
+ * dma_cookie_init - initialize the cookies for a DMA channel
+ * @chan: dma channel to initialize
+ */
+static inline void dma_cookie_init(struct dma_chan *chan)
+{
+	chan->cookie = DMA_MIN_COOKIE;
+	chan->completed_cookie = DMA_MIN_COOKIE;
+}
+
+/**
+ * dma_cookie_assign - assign a DMA engine cookie to the descriptor
+ * @tx: descriptor needing cookie
+ *
+ * Assign a unique non-zero per-channel cookie to the descriptor.
+ * Note: caller is expected to hold a lock to prevent concurrency.
+ */
+static inline dma_cookie_t dma_cookie_assign(struct dma_async_tx_descriptor *tx)
+{
+	struct dma_chan *chan = tx->chan;
+	dma_cookie_t cookie;
+
+	cookie = chan->cookie + 1;
+	if (cookie < DMA_MIN_COOKIE)
+		cookie = DMA_MIN_COOKIE;
+	tx->cookie = chan->cookie = cookie;
+
+	return cookie;
+}
+
+/**
+ * dma_cookie_complete - complete a descriptor
+ * @tx: descriptor to complete
+ *
+ * Mark this descriptor complete by updating the channels completed
+ * cookie marker.  Zero the descriptors cookie to prevent accidental
+ * repeated completions.
+ *
+ * Note: caller is expected to hold a lock to prevent concurrency.
+ */
+static inline void dma_cookie_complete(struct dma_async_tx_descriptor *tx)
+{
+	BUG_ON(tx->cookie < DMA_MIN_COOKIE);
+	tx->chan->completed_cookie = tx->cookie;
+	tx->cookie = 0;
+}
+
+/**
+ * dma_cookie_status - report cookie status
+ * @chan: dma channel
+ * @cookie: cookie we are interested in
+ * @state: dma_tx_state structure to return last/used cookies
+ *
+ * Report the status of the cookie, filling in the state structure if
+ * non-NULL.  No locking is required.
+ */
+static inline enum dma_status dma_cookie_status(struct dma_chan *chan,
+	dma_cookie_t cookie, struct dma_tx_state *state)
+{
+	dma_cookie_t used, complete;
+
+	used = chan->cookie;
+	complete = chan->completed_cookie;
+	barrier();
+	if (state) {
+		state->last = complete;
+		state->used = used;
+		state->residue = 0;
+	}
+	return dma_async_is_complete(cookie, complete, used);
+}
+
+static inline void dma_set_residue(struct dma_tx_state *state, u32 residue)
+{
+	if (state)
+		state->residue = residue;
+}
+
+#endif
diff --git a/drivers/dma/dw_dmac.c b/drivers/dma/dw_dmac.c
index 4d180ca9a1d8..aa8cdd8ea44b 100644
--- a/drivers/dma/dw_dmac.c
+++ b/drivers/dma/dw_dmac.c
@@ -22,6 +22,7 @@
 #include <linux/slab.h>
 
 #include "dw_dmac_regs.h"
+#include "dmaengine.h"
 
 /*
  * This supports the Synopsys "DesignWare AHB Central DMA Controller",
@@ -151,19 +152,36 @@ static void dwc_desc_put(struct dw_dma_chan *dwc, struct dw_desc *desc)
 	}
 }
 
-/* Called with dwc->lock held and bh disabled */
-static dma_cookie_t
-dwc_assign_cookie(struct dw_dma_chan *dwc, struct dw_desc *desc)
+static void dwc_initialize(struct dw_dma_chan *dwc)
 {
-	dma_cookie_t cookie = dwc->chan.cookie;
+	struct dw_dma *dw = to_dw_dma(dwc->chan.device);
+	struct dw_dma_slave *dws = dwc->chan.private;
+	u32 cfghi = DWC_CFGH_FIFO_MODE;
+	u32 cfglo = DWC_CFGL_CH_PRIOR(dwc->priority);
 
-	if (++cookie < 0)
-		cookie = 1;
+	if (dwc->initialized == true)
+		return;
 
-	dwc->chan.cookie = cookie;
-	desc->txd.cookie = cookie;
+	if (dws) {
+		/*
+		 * We need controller-specific data to set up slave
+		 * transfers.
+		 */
+		BUG_ON(!dws->dma_dev || dws->dma_dev != dw->dma.dev);
 
-	return cookie;
+		cfghi = dws->cfg_hi;
+		cfglo |= dws->cfg_lo & ~DWC_CFGL_CH_PRIOR_MASK;
+	}
+
+	channel_writel(dwc, CFG_LO, cfglo);
+	channel_writel(dwc, CFG_HI, cfghi);
+
+	/* Enable interrupts */
+	channel_set_bit(dw, MASK.XFER, dwc->mask);
+	channel_set_bit(dw, MASK.BLOCK, dwc->mask);
+	channel_set_bit(dw, MASK.ERROR, dwc->mask);
+
+	dwc->initialized = true;
 }
 
 /*----------------------------------------------------------------------*/
@@ -189,6 +207,8 @@ static void dwc_dostart(struct dw_dma_chan *dwc, struct dw_desc *first)
 		return;
 	}
 
+	dwc_initialize(dwc);
+
 	channel_writel(dwc, LLP, first->txd.phys);
 	channel_writel(dwc, CTL_LO,
 			DWC_CTLL_LLP_D_EN | DWC_CTLL_LLP_S_EN);
@@ -211,7 +231,7 @@ dwc_descriptor_complete(struct dw_dma_chan *dwc, struct dw_desc *desc,
 	dev_vdbg(chan2dev(&dwc->chan), "descriptor %u complete\n", txd->cookie);
 
 	spin_lock_irqsave(&dwc->lock, flags);
-	dwc->completed = txd->cookie;
+	dma_cookie_complete(txd);
 	if (callback_required) {
 		callback = txd->callback;
 		param = txd->callback_param;
@@ -581,7 +601,7 @@ static dma_cookie_t dwc_tx_submit(struct dma_async_tx_descriptor *tx)
 	unsigned long		flags;
 
 	spin_lock_irqsave(&dwc->lock, flags);
-	cookie = dwc_assign_cookie(dwc, desc);
+	cookie = dma_cookie_assign(tx);
 
 	/*
 	 * REVISIT: We should attempt to chain as many descriptors as
@@ -696,8 +716,8 @@ err_desc_get:
 
 static struct dma_async_tx_descriptor *
 dwc_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
-		unsigned int sg_len, enum dma_data_direction direction,
-		unsigned long flags)
+		unsigned int sg_len, enum dma_transfer_direction direction,
+		unsigned long flags, void *context)
 {
 	struct dw_dma_chan	*dwc = to_dw_dma_chan(chan);
 	struct dw_dma_slave	*dws = chan->private;
@@ -917,28 +937,17 @@ dwc_tx_status(struct dma_chan *chan,
 	      struct dma_tx_state *txstate)
 {
 	struct dw_dma_chan	*dwc = to_dw_dma_chan(chan);
-	dma_cookie_t		last_used;
-	dma_cookie_t		last_complete;
-	int			ret;
-
-	last_complete = dwc->completed;
-	last_used = chan->cookie;
+	enum dma_status		ret;
 
-	ret = dma_async_is_complete(cookie, last_complete, last_used);
+	ret = dma_cookie_status(chan, cookie, txstate);
 	if (ret != DMA_SUCCESS) {
 		dwc_scan_descriptors(to_dw_dma(chan->device), dwc);
 
-		last_complete = dwc->completed;
-		last_used = chan->cookie;
-
-		ret = dma_async_is_complete(cookie, last_complete, last_used);
+		ret = dma_cookie_status(chan, cookie, txstate);
 	}
 
 	if (ret != DMA_SUCCESS)
-		dma_set_tx_state(txstate, last_complete, last_used,
-				dwc_first_active(dwc)->len);
-	else
-		dma_set_tx_state(txstate, last_complete, last_used, 0);
+		dma_set_residue(txstate, dwc_first_active(dwc)->len);
 
 	if (dwc->paused)
 		return DMA_PAUSED;
@@ -959,10 +968,7 @@ static int dwc_alloc_chan_resources(struct dma_chan *chan)
 	struct dw_dma_chan	*dwc = to_dw_dma_chan(chan);
 	struct dw_dma		*dw = to_dw_dma(chan->device);
 	struct dw_desc		*desc;
-	struct dw_dma_slave	*dws;
 	int			i;
-	u32			cfghi;
-	u32			cfglo;
 	unsigned long		flags;
 
 	dev_vdbg(chan2dev(chan), "alloc_chan_resources\n");
@@ -973,27 +979,7 @@ static int dwc_alloc_chan_resources(struct dma_chan *chan)
 		return -EIO;
 	}
 
-	dwc->completed = chan->cookie = 1;
-
-	cfghi = DWC_CFGH_FIFO_MODE;
-	cfglo = 0;
-
-	dws = chan->private;
-	if (dws) {
-		/*
-		 * We need controller-specific data to set up slave
-		 * transfers.
-		 */
-		BUG_ON(!dws->dma_dev || dws->dma_dev != dw->dma.dev);
-
-		cfghi = dws->cfg_hi;
-		cfglo = dws->cfg_lo & ~DWC_CFGL_CH_PRIOR_MASK;
-	}
-
-	cfglo |= DWC_CFGL_CH_PRIOR(dwc->priority);
-
-	channel_writel(dwc, CFG_LO, cfglo);
-	channel_writel(dwc, CFG_HI, cfghi);
+	dma_cookie_init(chan);
 
 	/*
 	 * NOTE: some controllers may have additional features that we
@@ -1026,11 +1012,6 @@ static int dwc_alloc_chan_resources(struct dma_chan *chan)
 		i = ++dwc->descs_allocated;
 	}
 
-	/* Enable interrupts */
-	channel_set_bit(dw, MASK.XFER, dwc->mask);
-	channel_set_bit(dw, MASK.BLOCK, dwc->mask);
-	channel_set_bit(dw, MASK.ERROR, dwc->mask);
-
 	spin_unlock_irqrestore(&dwc->lock, flags);
 
 	dev_dbg(chan2dev(chan),
@@ -1058,6 +1039,7 @@ static void dwc_free_chan_resources(struct dma_chan *chan)
 	spin_lock_irqsave(&dwc->lock, flags);
 	list_splice_init(&dwc->free_list, &list);
 	dwc->descs_allocated = 0;
+	dwc->initialized = false;
 
 	/* Disable interrupts */
 	channel_clear_bit(dw, MASK.XFER, dwc->mask);
@@ -1335,6 +1317,8 @@ EXPORT_SYMBOL(dw_dma_cyclic_free);
 
 static void dw_dma_off(struct dw_dma *dw)
 {
+	int i;
+
 	dma_writel(dw, CFG, 0);
 
 	channel_clear_bit(dw, MASK.XFER, dw->all_chan_mask);
@@ -1345,6 +1329,9 @@ static void dw_dma_off(struct dw_dma *dw)
 
 	while (dma_readl(dw, CFG) & DW_CFG_DMA_EN)
 		cpu_relax();
+
+	for (i = 0; i < dw->dma.chancnt; i++)
+		dw->chan[i].initialized = false;
 }
 
 static int __init dw_probe(struct platform_device *pdev)
@@ -1411,8 +1398,8 @@ static int __init dw_probe(struct platform_device *pdev)
 		struct dw_dma_chan	*dwc = &dw->chan[i];
 
 		dwc->chan.device = &dw->dma;
-		dwc->chan.cookie = dwc->completed = 1;
 		dwc->chan.chan_id = i;
+		dma_cookie_init(&dwc->chan);
 		if (pdata->chan_allocation_order == CHAN_ALLOCATION_ASCENDING)
 			list_add_tail(&dwc->chan.device_node,
 					&dw->dma.channels);
@@ -1534,6 +1521,7 @@ static int dw_suspend_noirq(struct device *dev)
 
 	dw_dma_off(platform_get_drvdata(pdev));
 	clk_disable(dw->clk);
+
 	return 0;
 }
 
diff --git a/drivers/dma/dw_dmac_regs.h b/drivers/dma/dw_dmac_regs.h
index c3419518d701..cce1752d8f80 100644
--- a/drivers/dma/dw_dmac_regs.h
+++ b/drivers/dma/dw_dmac_regs.h
@@ -140,12 +140,12 @@ struct dw_dma_chan {
 	u8			mask;
 	u8			priority;
 	bool			paused;
+	bool			initialized;
 
 	spinlock_t		lock;
 
 	/* these other elements are all protected by lock */
 	unsigned long		flags;
-	dma_cookie_t		completed;
 	struct list_head	active_list;
 	struct list_head	queue;
 	struct list_head	free_list;
diff --git a/drivers/dma/ep93xx_dma.c b/drivers/dma/ep93xx_dma.c
index 5d7a49bd7c26..e3c271f35d21 100644
--- a/drivers/dma/ep93xx_dma.c
+++ b/drivers/dma/ep93xx_dma.c
@@ -27,6 +27,8 @@
 
 #include <mach/dma.h>
 
+#include "dmaengine.h"
+
 /* M2P registers */
 #define M2P_CONTROL			0x0000
 #define M2P_CONTROL_STALLINT		BIT(0)
@@ -121,7 +123,6 @@ struct ep93xx_dma_desc {
  * @lock: lock protecting the fields following
  * @flags: flags for the channel
  * @buffer: which buffer to use next (0/1)
- * @last_completed: last completed cookie value
  * @active: flattened chain of descriptors currently being processed
  * @queue: pending descriptors which are handled next
  * @free_list: list of free descriptors which can be used
@@ -156,7 +157,6 @@ struct ep93xx_dma_chan {
 #define EP93XX_DMA_IS_CYCLIC		0
 
 	int				buffer;
-	dma_cookie_t			last_completed;
 	struct list_head		active;
 	struct list_head		queue;
 	struct list_head		free_list;
@@ -674,9 +674,13 @@ static void ep93xx_dma_tasklet(unsigned long data)
 
 	spin_lock_irq(&edmac->lock);
 	desc = ep93xx_dma_get_active(edmac);
-	if (desc->complete) {
-		edmac->last_completed = desc->txd.cookie;
-		list_splice_init(&edmac->active, &list);
+	if (desc) {
+		if (desc->complete) {
+			dma_cookie_complete(&desc->txd);
+			list_splice_init(&edmac->active, &list);
+		}
+		callback = desc->txd.callback;
+		callback_param = desc->txd.callback_param;
 	}
 	spin_unlock_irq(&edmac->lock);
 
@@ -746,17 +750,10 @@ static dma_cookie_t ep93xx_dma_tx_submit(struct dma_async_tx_descriptor *tx)
 	unsigned long flags;
 
 	spin_lock_irqsave(&edmac->lock, flags);
-
-	cookie = edmac->chan.cookie;
-
-	if (++cookie < 0)
-		cookie = 1;
+	cookie = dma_cookie_assign(tx);
 
 	desc = container_of(tx, struct ep93xx_dma_desc, txd);
 
-	edmac->chan.cookie = cookie;
-	desc->txd.cookie = cookie;
-
 	/*
 	 * If nothing is currently prosessed, we push this descriptor
 	 * directly to the hardware. Otherwise we put the descriptor
@@ -824,8 +821,7 @@ static int ep93xx_dma_alloc_chan_resources(struct dma_chan *chan)
 		goto fail_clk_disable;
 
 	spin_lock_irq(&edmac->lock);
-	edmac->last_completed = 1;
-	edmac->chan.cookie = 1;
+	dma_cookie_init(&edmac->chan);
 	ret = edmac->edma->hw_setup(edmac);
 	spin_unlock_irq(&edmac->lock);
 
@@ -946,13 +942,14 @@ fail:
  * @sg_len: number of entries in @sgl
  * @dir: direction of tha DMA transfer
  * @flags: flags for the descriptor
+ * @context: operation context (ignored)
  *
  * Returns a valid DMA descriptor or %NULL in case of failure.
  */
 static struct dma_async_tx_descriptor *
 ep93xx_dma_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
-			 unsigned int sg_len, enum dma_data_direction dir,
-			 unsigned long flags)
+			 unsigned int sg_len, enum dma_transfer_direction dir,
+			 unsigned long flags, void *context)
 {
 	struct ep93xx_dma_chan *edmac = to_ep93xx_dma_chan(chan);
 	struct ep93xx_dma_desc *desc, *first;
@@ -1019,6 +1016,7 @@ fail:
  * @buf_len: length of the buffer (in bytes)
  * @period_len: lenght of a single period
  * @dir: direction of the operation
+ * @context: operation context (ignored)
  *
  * Prepares a descriptor for cyclic DMA operation. This means that once the
  * descriptor is submitted, we will be submitting in a @period_len sized
@@ -1031,7 +1029,7 @@ fail:
 static struct dma_async_tx_descriptor *
 ep93xx_dma_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t dma_addr,
 			   size_t buf_len, size_t period_len,
-			   enum dma_data_direction dir)
+			   enum dma_transfer_direction dir, void *context)
 {
 	struct ep93xx_dma_chan *edmac = to_ep93xx_dma_chan(chan);
 	struct ep93xx_dma_desc *desc, *first;
@@ -1211,18 +1209,13 @@ static enum dma_status ep93xx_dma_tx_status(struct dma_chan *chan,
 					    struct dma_tx_state *state)
 {
 	struct ep93xx_dma_chan *edmac = to_ep93xx_dma_chan(chan);
-	dma_cookie_t last_used, last_completed;
 	enum dma_status ret;
 	unsigned long flags;
 
 	spin_lock_irqsave(&edmac->lock, flags);
-	last_used = chan->cookie;
-	last_completed = edmac->last_completed;
+	ret = dma_cookie_status(chan, cookie, state);
 	spin_unlock_irqrestore(&edmac->lock, flags);
 
-	ret = dma_async_is_complete(cookie, last_completed, last_used);
-	dma_set_tx_state(state, last_completed, last_used, 0);
-
 	return ret;
 }
 
diff --git a/drivers/dma/fsldma.c b/drivers/dma/fsldma.c
index 8a781540590c..b5e68bcabe55 100644
--- a/drivers/dma/fsldma.c
+++ b/drivers/dma/fsldma.c
@@ -35,6 +35,7 @@
 #include <linux/dmapool.h>
 #include <linux/of_platform.h>
 
+#include "dmaengine.h"
 #include "fsldma.h"
 
 #define chan_dbg(chan, fmt, arg...)					\
@@ -413,17 +414,10 @@ static dma_cookie_t fsl_dma_tx_submit(struct dma_async_tx_descriptor *tx)
 	 * assign cookies to all of the software descriptors
 	 * that make up this transaction
 	 */
-	cookie = chan->common.cookie;
 	list_for_each_entry(child, &desc->tx_list, node) {
-		cookie++;
-		if (cookie < DMA_MIN_COOKIE)
-			cookie = DMA_MIN_COOKIE;
-
-		child->async_tx.cookie = cookie;
+		cookie = dma_cookie_assign(&child->async_tx);
 	}
 
-	chan->common.cookie = cookie;
-
 	/* put this transaction onto the tail of the pending queue */
 	append_ld_queue(chan, desc);
 
@@ -765,6 +759,7 @@ fail:
  * @sg_len: number of entries in @scatterlist
  * @direction: DMA direction
  * @flags: DMAEngine flags
+ * @context: transaction context (ignored)
  *
  * Prepare a set of descriptors for a DMA_SLAVE transaction. Following the
  * DMA_SLAVE API, this gets the device-specific information from the
@@ -772,7 +767,8 @@ fail:
  */
 static struct dma_async_tx_descriptor *fsl_dma_prep_slave_sg(
 	struct dma_chan *dchan, struct scatterlist *sgl, unsigned int sg_len,
-	enum dma_data_direction direction, unsigned long flags)
+	enum dma_transfer_direction direction, unsigned long flags,
+	void *context)
 {
 	/*
 	 * This operation is not supported on the Freescale DMA controller
@@ -984,19 +980,14 @@ static enum dma_status fsl_tx_status(struct dma_chan *dchan,
 					struct dma_tx_state *txstate)
 {
 	struct fsldma_chan *chan = to_fsl_chan(dchan);
-	dma_cookie_t last_complete;
-	dma_cookie_t last_used;
+	enum dma_status ret;
 	unsigned long flags;
 
 	spin_lock_irqsave(&chan->desc_lock, flags);
-
-	last_complete = chan->completed_cookie;
-	last_used = dchan->cookie;
-
+	ret = dma_cookie_status(dchan, cookie, txstate);
 	spin_unlock_irqrestore(&chan->desc_lock, flags);
 
-	dma_set_tx_state(txstate, last_complete, last_used, 0);
-	return dma_async_is_complete(cookie, last_complete, last_used);
+	return ret;
 }
 
 /*----------------------------------------------------------------------------*/
@@ -1087,8 +1078,8 @@ static void dma_do_tasklet(unsigned long data)
 
 		desc = to_fsl_desc(chan->ld_running.prev);
 		cookie = desc->async_tx.cookie;
+		dma_cookie_complete(&desc->async_tx);
 
-		chan->completed_cookie = cookie;
 		chan_dbg(chan, "completed_cookie=%d\n", cookie);
 	}
 
diff --git a/drivers/dma/fsldma.h b/drivers/dma/fsldma.h
index 9cb5aa57c677..f5c38791fc74 100644
--- a/drivers/dma/fsldma.h
+++ b/drivers/dma/fsldma.h
@@ -137,7 +137,6 @@ struct fsldma_device {
 struct fsldma_chan {
 	char name[8];			/* Channel name */
 	struct fsldma_chan_regs __iomem *regs;
-	dma_cookie_t completed_cookie;	/* The maximum cookie completed */
 	spinlock_t desc_lock;		/* Descriptor operation lock */
 	struct list_head ld_pending;	/* Link descriptors queue */
 	struct list_head ld_running;	/* Link descriptors queue */
diff --git a/drivers/dma/imx-dma.c b/drivers/dma/imx-dma.c
index d99f71c356b5..c977c670b3ce 100644
--- a/drivers/dma/imx-dma.c
+++ b/drivers/dma/imx-dma.c
@@ -28,6 +28,8 @@
 #include <mach/dma-v1.h>
 #include <mach/hardware.h>
 
+#include "dmaengine.h"
+
 struct imxdma_channel {
 	struct imxdma_engine		*imxdma;
 	unsigned int			channel;
@@ -39,7 +41,6 @@ struct imxdma_channel {
 	struct dma_chan			chan;
 	spinlock_t			lock;
 	struct dma_async_tx_descriptor	desc;
-	dma_cookie_t			last_completed;
 	enum dma_status			status;
 	int				dma_request;
 	struct scatterlist		*sg_list;
@@ -63,7 +64,7 @@ static void imxdma_handle(struct imxdma_channel *imxdmac)
 {
 	if (imxdmac->desc.callback)
 		imxdmac->desc.callback(imxdmac->desc.callback_param);
-	imxdmac->last_completed = imxdmac->desc.cookie;
+	dma_cookie_complete(&imxdmac->desc);
 }
 
 static void imxdma_irq_handler(int channel, void *data)
@@ -150,29 +151,7 @@ static enum dma_status imxdma_tx_status(struct dma_chan *chan,
 					    dma_cookie_t cookie,
 					    struct dma_tx_state *txstate)
 {
-	struct imxdma_channel *imxdmac = to_imxdma_chan(chan);
-	dma_cookie_t last_used;
-	enum dma_status ret;
-
-	last_used = chan->cookie;
-
-	ret = dma_async_is_complete(cookie, imxdmac->last_completed, last_used);
-	dma_set_tx_state(txstate, imxdmac->last_completed, last_used, 0);
-
-	return ret;
-}
-
-static dma_cookie_t imxdma_assign_cookie(struct imxdma_channel *imxdma)
-{
-	dma_cookie_t cookie = imxdma->chan.cookie;
-
-	if (++cookie < 0)
-		cookie = 1;
-
-	imxdma->chan.cookie = cookie;
-	imxdma->desc.cookie = cookie;
-
-	return cookie;
+	return dma_cookie_status(chan, cookie, txstate);
 }
 
 static dma_cookie_t imxdma_tx_submit(struct dma_async_tx_descriptor *tx)
@@ -182,7 +161,7 @@ static dma_cookie_t imxdma_tx_submit(struct dma_async_tx_descriptor *tx)
 
 	spin_lock_irq(&imxdmac->lock);
 
-	cookie = imxdma_assign_cookie(imxdmac);
+	cookie = dma_cookie_assign(tx);
 
 	imx_dma_enable(imxdmac->imxdma_channel);
 
@@ -222,8 +201,8 @@ static void imxdma_free_chan_resources(struct dma_chan *chan)
 
 static struct dma_async_tx_descriptor *imxdma_prep_slave_sg(
 		struct dma_chan *chan, struct scatterlist *sgl,
-		unsigned int sg_len, enum dma_data_direction direction,
-		unsigned long flags)
+		unsigned int sg_len, enum dma_transfer_direction direction,
+		unsigned long flags, void *context)
 {
 	struct imxdma_channel *imxdmac = to_imxdma_chan(chan);
 	struct scatterlist *sg;
@@ -269,7 +248,8 @@ static struct dma_async_tx_descriptor *imxdma_prep_slave_sg(
 
 static struct dma_async_tx_descriptor *imxdma_prep_dma_cyclic(
 		struct dma_chan *chan, dma_addr_t dma_addr, size_t buf_len,
-		size_t period_len, enum dma_data_direction direction)
+		size_t period_len, enum dma_transfer_direction direction,
+		void *context)
 {
 	struct imxdma_channel *imxdmac = to_imxdma_chan(chan);
 	struct imxdma_engine *imxdma = imxdmac->imxdma;
diff --git a/drivers/dma/imx-sdma.c b/drivers/dma/imx-sdma.c
index 7bd7e98548cd..c163f1eefb18 100644
--- a/drivers/dma/imx-sdma.c
+++ b/drivers/dma/imx-sdma.c
@@ -40,6 +40,8 @@
 #include <mach/dma.h>
 #include <mach/hardware.h>
 
+#include "dmaengine.h"
+
 /* SDMA registers */
 #define SDMA_H_C0PTR		0x000
 #define SDMA_H_INTR		0x004
@@ -264,7 +266,6 @@ struct sdma_channel {
 	struct dma_chan			chan;
 	spinlock_t			lock;
 	struct dma_async_tx_descriptor	desc;
-	dma_cookie_t			last_completed;
 	enum dma_status			status;
 };
 
@@ -509,6 +510,7 @@ static void mxc_sdma_handle_channel_normal(struct sdma_channel *sdmac)
 	else
 		sdmac->status = DMA_SUCCESS;
 
+	dma_cookie_complete(&sdmac->desc);
 	if (sdmac->desc.callback)
 		sdmac->desc.callback(sdmac->desc.callback_param);
 	sdmac->last_completed = sdmac->desc.cookie;
@@ -798,19 +800,6 @@ static void sdma_enable_channel(struct sdma_engine *sdma, int channel)
 	__raw_writel(1 << channel, sdma->regs + SDMA_H_START);
 }
 
-static dma_cookie_t sdma_assign_cookie(struct sdma_channel *sdmac)
-{
-	dma_cookie_t cookie = sdmac->chan.cookie;
-
-	if (++cookie < 0)
-		cookie = 1;
-
-	sdmac->chan.cookie = cookie;
-	sdmac->desc.cookie = cookie;
-
-	return cookie;
-}
-
 static struct sdma_channel *to_sdma_chan(struct dma_chan *chan)
 {
 	return container_of(chan, struct sdma_channel, chan);
@@ -824,7 +813,7 @@ static dma_cookie_t sdma_tx_submit(struct dma_async_tx_descriptor *tx)
 
 	spin_lock_irq(&sdmac->lock);
 
-	cookie = sdma_assign_cookie(sdmac);
+	cookie = dma_cookie_assign(tx);
 
 	sdma_enable_channel(sdma, sdmac->channel);
 
@@ -897,8 +886,8 @@ static void sdma_free_chan_resources(struct dma_chan *chan)
 
 static struct dma_async_tx_descriptor *sdma_prep_slave_sg(
 		struct dma_chan *chan, struct scatterlist *sgl,
-		unsigned int sg_len, enum dma_data_direction direction,
-		unsigned long flags)
+		unsigned int sg_len, enum dma_transfer_direction direction,
+		unsigned long flags, void *context)
 {
 	struct sdma_channel *sdmac = to_sdma_chan(chan);
 	struct sdma_engine *sdma = sdmac->sdma;
@@ -994,7 +983,8 @@ err_out:
 
 static struct dma_async_tx_descriptor *sdma_prep_dma_cyclic(
 		struct dma_chan *chan, dma_addr_t dma_addr, size_t buf_len,
-		size_t period_len, enum dma_data_direction direction)
+		size_t period_len, enum dma_transfer_direction direction,
+		void *context)
 {
 	struct sdma_channel *sdmac = to_sdma_chan(chan);
 	struct sdma_engine *sdma = sdmac->sdma;
@@ -1105,16 +1095,19 @@ static enum dma_status sdma_tx_status(struct dma_chan *chan,
 
 	last_used = chan->cookie;
 
-	dma_set_tx_state(txstate, sdmac->last_completed, last_used, 0);
+	dma_set_tx_state(txstate, chan->completed_cookie, last_used,
+			sdmac->chn_count - sdmac->chn_real_count);
 
 	return sdmac->status;
 }
 
 static void sdma_issue_pending(struct dma_chan *chan)
 {
-	/*
-	 * Nothing to do. We only have a single descriptor
-	 */
+	struct sdma_channel *sdmac = to_sdma_chan(chan);
+	struct sdma_engine *sdma = sdmac->sdma;
+
+	if (sdmac->status == DMA_IN_PROGRESS)
+		sdma_enable_channel(sdma, sdmac->channel);
 }
 
 #define SDMA_SCRIPT_ADDRS_ARRAY_SIZE_V1	34
diff --git a/drivers/dma/intel_mid_dma.c b/drivers/dma/intel_mid_dma.c
index 8a3fdd87db97..a4a1fd8de043 100644
--- a/drivers/dma/intel_mid_dma.c
+++ b/drivers/dma/intel_mid_dma.c
@@ -28,6 +28,8 @@
 #include <linux/pm_runtime.h>
 #include <linux/intel_mid_dma.h>
 
+#include "dmaengine.h"
+
 #define MAX_CHAN	4 /*max ch across controllers*/
 #include "intel_mid_dma_regs.h"
 
@@ -288,7 +290,7 @@ static void midc_descriptor_complete(struct intel_mid_dma_chan *midc,
 	struct intel_mid_dma_lli	*llitem;
 	void *param_txd = NULL;
 
-	midc->completed = txd->cookie;
+	dma_cookie_complete(txd);
 	callback_txd = txd->callback;
 	param_txd = txd->callback_param;
 
@@ -433,14 +435,7 @@ static dma_cookie_t intel_mid_dma_tx_submit(struct dma_async_tx_descriptor *tx)
 	dma_cookie_t		cookie;
 
 	spin_lock_bh(&midc->lock);
-	cookie = midc->chan.cookie;
-
-	if (++cookie < 0)
-		cookie = 1;
-
-	midc->chan.cookie = cookie;
-	desc->txd.cookie = cookie;
-
+	cookie = dma_cookie_assign(tx);
 
 	if (list_empty(&midc->active_list))
 		list_add_tail(&desc->desc_node, &midc->active_list);
@@ -481,29 +476,14 @@ static enum dma_status intel_mid_dma_tx_status(struct dma_chan *chan,
 						dma_cookie_t cookie,
 						struct dma_tx_state *txstate)
 {
-	struct intel_mid_dma_chan	*midc = to_intel_mid_dma_chan(chan);
-	dma_cookie_t		last_used;
-	dma_cookie_t		last_complete;
-	int				ret;
+	enum dma_status ret;
 
-	last_complete = midc->completed;
-	last_used = chan->cookie;
-
-	ret = dma_async_is_complete(cookie, last_complete, last_used);
+	ret = dma_cookie_status(chan, cookie, txstate);
 	if (ret != DMA_SUCCESS) {
 		midc_scan_descriptors(to_middma_device(chan->device), midc);
-
-		last_complete = midc->completed;
-		last_used = chan->cookie;
-
-		ret = dma_async_is_complete(cookie, last_complete, last_used);
+		ret = dma_cookie_status(chan, cookie, txstate);
 	}
 
-	if (txstate) {
-		txstate->last = last_complete;
-		txstate->used = last_used;
-		txstate->residue = 0;
-	}
 	return ret;
 }
 
@@ -728,13 +708,14 @@ err_desc_get:
  * @sg_len: length of sg txn
  * @direction: DMA transfer dirtn
  * @flags: DMA flags
+ * @context: transfer context (ignored)
  *
  * Prepares LLI based periphral transfer
  */
 static struct dma_async_tx_descriptor *intel_mid_dma_prep_slave_sg(
 			struct dma_chan *chan, struct scatterlist *sgl,
-			unsigned int sg_len, enum dma_data_direction direction,
-			unsigned long flags)
+			unsigned int sg_len, enum dma_transfer_direction direction,
+			unsigned long flags, void *context)
 {
 	struct intel_mid_dma_chan *midc = NULL;
 	struct intel_mid_dma_slave *mids = NULL;
@@ -882,7 +863,7 @@ static int intel_mid_dma_alloc_chan_resources(struct dma_chan *chan)
 		pm_runtime_put(&mid->pdev->dev);
 		return -EIO;
 	}
-	midc->completed = chan->cookie = 1;
+	dma_cookie_init(chan);
 
 	spin_lock_bh(&midc->lock);
 	while (midc->descs_allocated < DESCS_PER_CHANNEL) {
@@ -1113,8 +1094,8 @@ static int mid_setup_dma(struct pci_dev *pdev)
 		struct intel_mid_dma_chan *midch = &dma->ch[i];
 
 		midch->chan.device = &dma->common;
-		midch->chan.cookie =  1;
 		midch->chan.chan_id = i;
+		dma_cookie_init(&midch->chan);
 		midch->ch_id = dma->chan_base + i;
 		pr_debug("MDMA:Init CH %d, ID %d\n", i, midch->ch_id);
 
diff --git a/drivers/dma/intel_mid_dma_regs.h b/drivers/dma/intel_mid_dma_regs.h
index aea5ee88ce03..542ea85f83f6 100644
--- a/drivers/dma/intel_mid_dma_regs.h
+++ b/drivers/dma/intel_mid_dma_regs.h
@@ -165,7 +165,6 @@ union intel_mid_dma_cfg_hi {
  * @dma_base: MMIO register space DMA engine base pointer
  * @ch_id: DMA channel id
  * @lock: channel spinlock
- * @completed: DMA cookie
  * @active_list: current active descriptors
  * @queue: current queued up descriptors
  * @free_list: current free descriptors
@@ -183,7 +182,6 @@ struct intel_mid_dma_chan {
 	void __iomem		*dma_base;
 	int			ch_id;
 	spinlock_t		lock;
-	dma_cookie_t		completed;
 	struct list_head	active_list;
 	struct list_head	queue;
 	struct list_head	free_list;
diff --git a/drivers/dma/ioat/dma.c b/drivers/dma/ioat/dma.c
index a4d6cb0c0343..97e100ce43eb 100644
--- a/drivers/dma/ioat/dma.c
+++ b/drivers/dma/ioat/dma.c
@@ -40,6 +40,8 @@
 #include "registers.h"
 #include "hw.h"
 
+#include "../dmaengine.h"
+
 int ioat_pending_level = 4;
 module_param(ioat_pending_level, int, 0644);
 MODULE_PARM_DESC(ioat_pending_level,
@@ -235,12 +237,7 @@ static dma_cookie_t ioat1_tx_submit(struct dma_async_tx_descriptor *tx)
 
 	spin_lock_bh(&ioat->desc_lock);
 	/* cookie incr and addition to used_list must be atomic */
-	cookie = c->cookie;
-	cookie++;
-	if (cookie < 0)
-		cookie = 1;
-	c->cookie = cookie;
-	tx->cookie = cookie;
+	cookie = dma_cookie_assign(tx);
 	dev_dbg(to_dev(&ioat->base), "%s: cookie: %d\n", __func__, cookie);
 
 	/* write address into NextDescriptor field of last desc in chain */
@@ -603,8 +600,7 @@ static void __cleanup(struct ioat_dma_chan *ioat, unsigned long phys_complete)
 		 */
 		dump_desc_dbg(ioat, desc);
 		if (tx->cookie) {
-			chan->completed_cookie = tx->cookie;
-			tx->cookie = 0;
+			dma_cookie_complete(tx);
 			ioat_dma_unmap(chan, tx->flags, desc->len, desc->hw);
 			ioat->active -= desc->hw->tx_cnt;
 			if (tx->callback) {
@@ -733,13 +729,15 @@ ioat_dma_tx_status(struct dma_chan *c, dma_cookie_t cookie,
 {
 	struct ioat_chan_common *chan = to_chan_common(c);
 	struct ioatdma_device *device = chan->device;
+	enum dma_status ret;
 
-	if (ioat_tx_status(c, cookie, txstate) == DMA_SUCCESS)
-		return DMA_SUCCESS;
+	ret = dma_cookie_status(c, cookie, txstate);
+	if (ret == DMA_SUCCESS)
+		return ret;
 
 	device->cleanup_fn((unsigned long) c);
 
-	return ioat_tx_status(c, cookie, txstate);
+	return dma_cookie_status(c, cookie, txstate);
 }
 
 static void ioat1_dma_start_null_desc(struct ioat_dma_chan *ioat)
diff --git a/drivers/dma/ioat/dma.h b/drivers/dma/ioat/dma.h
index 5216c8a92a21..c7888bccd974 100644
--- a/drivers/dma/ioat/dma.h
+++ b/drivers/dma/ioat/dma.h
@@ -90,7 +90,6 @@ struct ioat_chan_common {
 	void __iomem *reg_base;
 	unsigned long last_completion;
 	spinlock_t cleanup_lock;
-	dma_cookie_t completed_cookie;
 	unsigned long state;
 	#define IOAT_COMPLETION_PENDING 0
 	#define IOAT_COMPLETION_ACK 1
@@ -143,28 +142,6 @@ static inline struct ioat_dma_chan *to_ioat_chan(struct dma_chan *c)
 	return container_of(chan, struct ioat_dma_chan, base);
 }
 
-/**
- * ioat_tx_status - poll the status of an ioat transaction
- * @c: channel handle
- * @cookie: transaction identifier
- * @txstate: if set, updated with the transaction state
- */
-static inline enum dma_status
-ioat_tx_status(struct dma_chan *c, dma_cookie_t cookie,
-		 struct dma_tx_state *txstate)
-{
-	struct ioat_chan_common *chan = to_chan_common(c);
-	dma_cookie_t last_used;
-	dma_cookie_t last_complete;
-
-	last_used = c->cookie;
-	last_complete = chan->completed_cookie;
-
-	dma_set_tx_state(txstate, last_complete, last_used, 0);
-
-	return dma_async_is_complete(cookie, last_complete, last_used);
-}
-
 /* wrapper around hardware descriptor format + additional software fields */
 
 /**
diff --git a/drivers/dma/ioat/dma_v2.c b/drivers/dma/ioat/dma_v2.c
index 5d65f8377971..e8e110ff3d96 100644
--- a/drivers/dma/ioat/dma_v2.c
+++ b/drivers/dma/ioat/dma_v2.c
@@ -41,6 +41,8 @@
 #include "registers.h"
 #include "hw.h"
 
+#include "../dmaengine.h"
+
 int ioat_ring_alloc_order = 8;
 module_param(ioat_ring_alloc_order, int, 0644);
 MODULE_PARM_DESC(ioat_ring_alloc_order,
@@ -147,8 +149,7 @@ static void __cleanup(struct ioat2_dma_chan *ioat, unsigned long phys_complete)
 		dump_desc_dbg(ioat, desc);
 		if (tx->cookie) {
 			ioat_dma_unmap(chan, tx->flags, desc->len, desc->hw);
-			chan->completed_cookie = tx->cookie;
-			tx->cookie = 0;
+			dma_cookie_complete(tx);
 			if (tx->callback) {
 				tx->callback(tx->callback_param);
 				tx->callback = NULL;
@@ -398,13 +399,9 @@ static dma_cookie_t ioat2_tx_submit_unlock(struct dma_async_tx_descriptor *tx)
 	struct dma_chan *c = tx->chan;
 	struct ioat2_dma_chan *ioat = to_ioat2_chan(c);
 	struct ioat_chan_common *chan = &ioat->base;
-	dma_cookie_t cookie = c->cookie;
+	dma_cookie_t cookie;
 
-	cookie++;
-	if (cookie < 0)
-		cookie = 1;
-	tx->cookie = cookie;
-	c->cookie = cookie;
+	cookie = dma_cookie_assign(tx);
 	dev_dbg(to_dev(&ioat->base), "%s: cookie: %d\n", __func__, cookie);
 
 	if (!test_and_set_bit(IOAT_COMPLETION_PENDING, &chan->state))
diff --git a/drivers/dma/ioat/dma_v3.c b/drivers/dma/ioat/dma_v3.c
index f519c93a61e7..145eda241dee 100644
--- a/drivers/dma/ioat/dma_v3.c
+++ b/drivers/dma/ioat/dma_v3.c
@@ -277,9 +277,8 @@ static void __cleanup(struct ioat2_dma_chan *ioat, unsigned long phys_complete)
 		dump_desc_dbg(ioat, desc);
 		tx = &desc->txd;
 		if (tx->cookie) {
-			chan->completed_cookie = tx->cookie;
+			dma_cookie_complete(tx);
 			ioat3_dma_unmap(ioat, desc, idx + i);
-			tx->cookie = 0;
 			if (tx->callback) {
 				tx->callback(tx->callback_param);
 				tx->callback = NULL;
@@ -411,13 +410,15 @@ ioat3_tx_status(struct dma_chan *c, dma_cookie_t cookie,
 		struct dma_tx_state *txstate)
 {
 	struct ioat2_dma_chan *ioat = to_ioat2_chan(c);
+	enum dma_status ret;
 
-	if (ioat_tx_status(c, cookie, txstate) == DMA_SUCCESS)
-		return DMA_SUCCESS;
+	ret = dma_cookie_status(c, cookie, txstate);
+	if (ret == DMA_SUCCESS)
+		return ret;
 
 	ioat3_cleanup(ioat);
 
-	return ioat_tx_status(c, cookie, txstate);
+	return dma_cookie_status(c, cookie, txstate);
 }
 
 static struct dma_async_tx_descriptor *
diff --git a/drivers/dma/iop-adma.c b/drivers/dma/iop-adma.c
index e03f811a83dd..c38e081a535b 100644
--- a/drivers/dma/iop-adma.c
+++ b/drivers/dma/iop-adma.c
@@ -36,6 +36,8 @@
 
 #include <mach/adma.h>
 
+#include "dmaengine.h"
+
 #define to_iop_adma_chan(chan) container_of(chan, struct iop_adma_chan, common)
 #define to_iop_adma_device(dev) \
 	container_of(dev, struct iop_adma_device, common)
@@ -317,7 +319,7 @@ static void __iop_adma_slot_cleanup(struct iop_adma_chan *iop_chan)
 	}
 
 	if (cookie > 0) {
-		iop_chan->completed_cookie = cookie;
+		iop_chan->common.completed_cookie = cookie;
 		pr_debug("\tcompleted cookie %d\n", cookie);
 	}
 }
@@ -438,18 +440,6 @@ retry:
 	return NULL;
 }
 
-static dma_cookie_t
-iop_desc_assign_cookie(struct iop_adma_chan *iop_chan,
-	struct iop_adma_desc_slot *desc)
-{
-	dma_cookie_t cookie = iop_chan->common.cookie;
-	cookie++;
-	if (cookie < 0)
-		cookie = 1;
-	iop_chan->common.cookie = desc->async_tx.cookie = cookie;
-	return cookie;
-}
-
 static void iop_adma_check_threshold(struct iop_adma_chan *iop_chan)
 {
 	dev_dbg(iop_chan->device->common.dev, "pending: %d\n",
@@ -477,7 +467,7 @@ iop_adma_tx_submit(struct dma_async_tx_descriptor *tx)
 	slots_per_op = grp_start->slots_per_op;
 
 	spin_lock_bh(&iop_chan->lock);
-	cookie = iop_desc_assign_cookie(iop_chan, sw_desc);
+	cookie = dma_cookie_assign(tx);
 
 	old_chain_tail = list_entry(iop_chan->chain.prev,
 		struct iop_adma_desc_slot, chain_node);
@@ -904,24 +894,15 @@ static enum dma_status iop_adma_status(struct dma_chan *chan,
 					struct dma_tx_state *txstate)
 {
 	struct iop_adma_chan *iop_chan = to_iop_adma_chan(chan);
-	dma_cookie_t last_used;
-	dma_cookie_t last_complete;
 	enum dma_status ret;
 
-	last_used = chan->cookie;
-	last_complete = iop_chan->completed_cookie;
-	dma_set_tx_state(txstate, last_complete, last_used, 0);
-	ret = dma_async_is_complete(cookie, last_complete, last_used);
+	ret = dma_cookie_status(chan, cookie, txstate);
 	if (ret == DMA_SUCCESS)
 		return ret;
 
 	iop_adma_slot_cleanup(iop_chan);
 
-	last_used = chan->cookie;
-	last_complete = iop_chan->completed_cookie;
-	dma_set_tx_state(txstate, last_complete, last_used, 0);
-
-	return dma_async_is_complete(cookie, last_complete, last_used);
+	return dma_cookie_status(chan, cookie, txstate);
 }
 
 static irqreturn_t iop_adma_eot_handler(int irq, void *data)
@@ -1650,7 +1631,7 @@ static void iop_chan_start_null_memcpy(struct iop_adma_chan *iop_chan)
 		/* initialize the completed cookie to be less than
 		 * the most recently used cookie
 		 */
-		iop_chan->completed_cookie = cookie - 1;
+		iop_chan->common.completed_cookie = cookie - 1;
 		iop_chan->common.cookie = sw_desc->async_tx.cookie = cookie;
 
 		/* channel should not be busy */
@@ -1707,7 +1688,7 @@ static void iop_chan_start_null_xor(struct iop_adma_chan *iop_chan)
 		/* initialize the completed cookie to be less than
 		 * the most recently used cookie
 		 */
-		iop_chan->completed_cookie = cookie - 1;
+		iop_chan->common.completed_cookie = cookie - 1;
 		iop_chan->common.cookie = sw_desc->async_tx.cookie = cookie;
 
 		/* channel should not be busy */
diff --git a/drivers/dma/ipu/ipu_idmac.c b/drivers/dma/ipu/ipu_idmac.c
index 6815905a772f..fcefeddfa179 100644
--- a/drivers/dma/ipu/ipu_idmac.c
+++ b/drivers/dma/ipu/ipu_idmac.c
@@ -24,6 +24,7 @@
 
 #include <mach/ipu.h>
 
+#include "../dmaengine.h"
 #include "ipu_intern.h"
 
 #define FS_VF_IN_VALID	0x00000002
@@ -888,14 +889,7 @@ static dma_cookie_t idmac_tx_submit(struct dma_async_tx_descriptor *tx)
 
 	dev_dbg(dev, "Submitting sg %p\n", &desc->sg[0]);
 
-	cookie = ichan->dma_chan.cookie;
-
-	if (++cookie < 0)
-		cookie = 1;
-
-	/* from dmaengine.h: "last cookie value returned to client" */
-	ichan->dma_chan.cookie = cookie;
-	tx->cookie = cookie;
+	cookie = dma_cookie_assign(tx);
 
 	/* ipu->lock can be taken under ichan->lock, but not v.v. */
 	spin_lock_irqsave(&ichan->lock, flags);
@@ -1316,7 +1310,7 @@ static irqreturn_t idmac_interrupt(int irq, void *dev_id)
 	/* Flip the active buffer - even if update above failed */
 	ichan->active_buffer = !ichan->active_buffer;
 	if (done)
-		ichan->completed = desc->txd.cookie;
+		dma_cookie_complete(&desc->txd);
 
 	callback = desc->txd.callback;
 	callback_param = desc->txd.callback_param;
@@ -1362,7 +1356,8 @@ static void ipu_gc_tasklet(unsigned long arg)
 /* Allocate and initialise a transfer descriptor. */
 static struct dma_async_tx_descriptor *idmac_prep_slave_sg(struct dma_chan *chan,
 		struct scatterlist *sgl, unsigned int sg_len,
-		enum dma_data_direction direction, unsigned long tx_flags)
+		enum dma_transfer_direction direction, unsigned long tx_flags,
+		void *context)
 {
 	struct idmac_channel *ichan = to_idmac_chan(chan);
 	struct idmac_tx_desc *desc = NULL;
@@ -1512,8 +1507,7 @@ static int idmac_alloc_chan_resources(struct dma_chan *chan)
 	BUG_ON(chan->client_count > 1);
 	WARN_ON(ichan->status != IPU_CHANNEL_FREE);
 
-	chan->cookie		= 1;
-	ichan->completed	= -ENXIO;
+	dma_cookie_init(chan);
 
 	ret = ipu_irq_map(chan->chan_id);
 	if (ret < 0)
@@ -1602,9 +1596,7 @@ static void idmac_free_chan_resources(struct dma_chan *chan)
 static enum dma_status idmac_tx_status(struct dma_chan *chan,
 		       dma_cookie_t cookie, struct dma_tx_state *txstate)
 {
-	struct idmac_channel *ichan = to_idmac_chan(chan);
-
-	dma_set_tx_state(txstate, ichan->completed, chan->cookie, 0);
+	dma_set_tx_state(txstate, chan->completed_cookie, chan->cookie, 0);
 	if (cookie != chan->cookie)
 		return DMA_ERROR;
 	return DMA_SUCCESS;
@@ -1640,11 +1632,10 @@ static int __init ipu_idmac_init(struct ipu *ipu)
 
 		ichan->status		= IPU_CHANNEL_FREE;
 		ichan->sec_chan_en	= false;
-		ichan->completed	= -ENXIO;
 		snprintf(ichan->eof_name, sizeof(ichan->eof_name), "IDMAC EOF %d", i);
 
 		dma_chan->device	= &idmac->dma;
-		dma_chan->cookie	= 1;
+		dma_cookie_init(dma_chan);
 		dma_chan->chan_id	= i;
 		list_add_tail(&dma_chan->device_node, &dma->channels);
 	}
diff --git a/drivers/dma/mpc512x_dma.c b/drivers/dma/mpc512x_dma.c
index b9bae94f2015..3034cb768015 100644
--- a/drivers/dma/mpc512x_dma.c
+++ b/drivers/dma/mpc512x_dma.c
@@ -44,6 +44,8 @@
 
 #include <linux/random.h>
 
+#include "dmaengine.h"
+
 /* Number of DMA Transfer descriptors allocated per channel */
 #define MPC_DMA_DESCRIPTORS	64
 
@@ -188,7 +190,6 @@ struct mpc_dma_chan {
 	struct list_head		completed;
 	struct mpc_dma_tcd		*tcd;
 	dma_addr_t			tcd_paddr;
-	dma_cookie_t			completed_cookie;
 
 	/* Lock for this structure */
 	spinlock_t			lock;
@@ -365,7 +366,7 @@ static void mpc_dma_process_completed(struct mpc_dma *mdma)
 		/* Free descriptors */
 		spin_lock_irqsave(&mchan->lock, flags);
 		list_splice_tail_init(&list, &mchan->free);
-		mchan->completed_cookie = last_cookie;
+		mchan->chan.completed_cookie = last_cookie;
 		spin_unlock_irqrestore(&mchan->lock, flags);
 	}
 }
@@ -438,13 +439,7 @@ static dma_cookie_t mpc_dma_tx_submit(struct dma_async_tx_descriptor *txd)
 		mpc_dma_execute(mchan);
 
 	/* Update cookie */
-	cookie = mchan->chan.cookie + 1;
-	if (cookie <= 0)
-		cookie = 1;
-
-	mchan->chan.cookie = cookie;
-	mdesc->desc.cookie = cookie;
-
+	cookie = dma_cookie_assign(txd);
 	spin_unlock_irqrestore(&mchan->lock, flags);
 
 	return cookie;
@@ -562,17 +557,14 @@ mpc_dma_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 	       struct dma_tx_state *txstate)
 {
 	struct mpc_dma_chan *mchan = dma_chan_to_mpc_dma_chan(chan);
+	enum dma_status ret;
 	unsigned long flags;
-	dma_cookie_t last_used;
-	dma_cookie_t last_complete;
 
 	spin_lock_irqsave(&mchan->lock, flags);
-	last_used = mchan->chan.cookie;
-	last_complete = mchan->completed_cookie;
+	ret = dma_cookie_status(chan, cookie, txstate);
 	spin_unlock_irqrestore(&mchan->lock, flags);
 
-	dma_set_tx_state(txstate, last_complete, last_used, 0);
-	return dma_async_is_complete(cookie, last_complete, last_used);
+	return ret;
 }
 
 /* Prepare descriptor for memory to memory copy */
@@ -742,8 +734,7 @@ static int __devinit mpc_dma_probe(struct platform_device *op)
 
 		mchan->chan.device = dma;
 		mchan->chan.chan_id = i;
-		mchan->chan.cookie = 1;
-		mchan->completed_cookie = mchan->chan.cookie;
+		dma_cookie_init(&mchan->chan);
 
 		INIT_LIST_HEAD(&mchan->free);
 		INIT_LIST_HEAD(&mchan->prepared);
diff --git a/drivers/dma/mv_xor.c b/drivers/dma/mv_xor.c
index 9a353c2216d0..169ae7cc857a 100644
--- a/drivers/dma/mv_xor.c
+++ b/drivers/dma/mv_xor.c
@@ -26,6 +26,8 @@
 #include <linux/platform_device.h>
 #include <linux/memory.h>
 #include <plat/mv_xor.h>
+
+#include "dmaengine.h"
 #include "mv_xor.h"
 
 static void mv_xor_issue_pending(struct dma_chan *chan);
@@ -435,7 +437,7 @@ static void __mv_xor_slot_cleanup(struct mv_xor_chan *mv_chan)
 	}
 
 	if (cookie > 0)
-		mv_chan->completed_cookie = cookie;
+		mv_chan->common.completed_cookie = cookie;
 }
 
 static void
@@ -534,18 +536,6 @@ retry:
 	return NULL;
 }
 
-static dma_cookie_t
-mv_desc_assign_cookie(struct mv_xor_chan *mv_chan,
-		      struct mv_xor_desc_slot *desc)
-{
-	dma_cookie_t cookie = mv_chan->common.cookie;
-
-	if (++cookie < 0)
-		cookie = 1;
-	mv_chan->common.cookie = desc->async_tx.cookie = cookie;
-	return cookie;
-}
-
 /************************ DMA engine API functions ****************************/
 static dma_cookie_t
 mv_xor_tx_submit(struct dma_async_tx_descriptor *tx)
@@ -563,7 +553,7 @@ mv_xor_tx_submit(struct dma_async_tx_descriptor *tx)
 	grp_start = sw_desc->group_head;
 
 	spin_lock_bh(&mv_chan->lock);
-	cookie = mv_desc_assign_cookie(mv_chan, sw_desc);
+	cookie = dma_cookie_assign(tx);
 
 	if (list_empty(&mv_chan->chain))
 		list_splice_init(&sw_desc->tx_list, &mv_chan->chain);
@@ -820,27 +810,16 @@ static enum dma_status mv_xor_status(struct dma_chan *chan,
 					  struct dma_tx_state *txstate)
 {
 	struct mv_xor_chan *mv_chan = to_mv_xor_chan(chan);
-	dma_cookie_t last_used;
-	dma_cookie_t last_complete;
 	enum dma_status ret;
 
-	last_used = chan->cookie;
-	last_complete = mv_chan->completed_cookie;
-	mv_chan->is_complete_cookie = cookie;
-	dma_set_tx_state(txstate, last_complete, last_used, 0);
-
-	ret = dma_async_is_complete(cookie, last_complete, last_used);
+	ret = dma_cookie_status(chan, cookie, txstate);
 	if (ret == DMA_SUCCESS) {
 		mv_xor_clean_completed_slots(mv_chan);
 		return ret;
 	}
 	mv_xor_slot_cleanup(mv_chan);
 
-	last_used = chan->cookie;
-	last_complete = mv_chan->completed_cookie;
-
-	dma_set_tx_state(txstate, last_complete, last_used, 0);
-	return dma_async_is_complete(cookie, last_complete, last_used);
+	return dma_cookie_status(chan, cookie, txstate);
 }
 
 static void mv_dump_xor_regs(struct mv_xor_chan *chan)
diff --git a/drivers/dma/mv_xor.h b/drivers/dma/mv_xor.h
index 977b592e976b..4e4780e1b7e0 100644
--- a/drivers/dma/mv_xor.h
+++ b/drivers/dma/mv_xor.h
@@ -78,7 +78,6 @@ struct mv_xor_device {
 /**
  * struct mv_xor_chan - internal representation of a XOR channel
  * @pending: allows batching of hardware operations
- * @completed_cookie: identifier for the most recently completed operation
  * @lock: serializes enqueue/dequeue operations to the descriptors pool
  * @mmr_base: memory mapped register base
  * @idx: the index of the xor channel
@@ -93,7 +92,6 @@ struct mv_xor_device {
  */
 struct mv_xor_chan {
 	int			pending;
-	dma_cookie_t		completed_cookie;
 	spinlock_t		lock; /* protects the descriptor slot pool */
 	void __iomem		*mmr_base;
 	unsigned int		idx;
diff --git a/drivers/dma/mxs-dma.c b/drivers/dma/mxs-dma.c
index be641cbd36fc..ea5237d05a3c 100644
--- a/drivers/dma/mxs-dma.c
+++ b/drivers/dma/mxs-dma.c
@@ -28,6 +28,8 @@
 #include <mach/dma.h>
 #include <mach/common.h>
 
+#include "dmaengine.h"
+
 /*
  * NOTE: The term "PIO" throughout the mxs-dma implementation means
  * PIO mode of mxs apbh-dma and apbx-dma.  With this working mode,
@@ -111,7 +113,7 @@ struct mxs_dma_chan {
 	int				chan_irq;
 	struct mxs_dma_ccw		*ccw;
 	dma_addr_t			ccw_phys;
-	dma_cookie_t			last_completed;
+	int				desc_count;
 	enum dma_status			status;
 	unsigned int			flags;
 #define MXS_DMA_SG_LOOP			(1 << 0)
@@ -216,19 +218,6 @@ static void mxs_dma_resume_chan(struct mxs_dma_chan *mxs_chan)
 	mxs_chan->status = DMA_IN_PROGRESS;
 }
 
-static dma_cookie_t mxs_dma_assign_cookie(struct mxs_dma_chan *mxs_chan)
-{
-	dma_cookie_t cookie = mxs_chan->chan.cookie;
-
-	if (++cookie < 0)
-		cookie = 1;
-
-	mxs_chan->chan.cookie = cookie;
-	mxs_chan->desc.cookie = cookie;
-
-	return cookie;
-}
-
 static struct mxs_dma_chan *to_mxs_dma_chan(struct dma_chan *chan)
 {
 	return container_of(chan, struct mxs_dma_chan, chan);
@@ -240,7 +229,7 @@ static dma_cookie_t mxs_dma_tx_submit(struct dma_async_tx_descriptor *tx)
 
 	mxs_dma_enable_chan(mxs_chan);
 
-	return mxs_dma_assign_cookie(mxs_chan);
+	return dma_cookie_assign(tx);
 }
 
 static void mxs_dma_tasklet(unsigned long data)
@@ -297,7 +286,7 @@ static irqreturn_t mxs_dma_int_handler(int irq, void *dev_id)
 		stat1 &= ~(1 << channel);
 
 		if (mxs_chan->status == DMA_SUCCESS)
-			mxs_chan->last_completed = mxs_chan->desc.cookie;
+			dma_cookie_complete(&mxs_chan->desc);
 
 		/* schedule tasklet on this channel */
 		tasklet_schedule(&mxs_chan->tasklet);
@@ -374,8 +363,8 @@ static void mxs_dma_free_chan_resources(struct dma_chan *chan)
 
 static struct dma_async_tx_descriptor *mxs_dma_prep_slave_sg(
 		struct dma_chan *chan, struct scatterlist *sgl,
-		unsigned int sg_len, enum dma_data_direction direction,
-		unsigned long append)
+		unsigned int sg_len, enum dma_transfer_direction direction,
+		unsigned long append, void *context)
 {
 	struct mxs_dma_chan *mxs_chan = to_mxs_dma_chan(chan);
 	struct mxs_dma_engine *mxs_dma = mxs_chan->mxs_dma;
@@ -469,7 +458,8 @@ err_out:
 
 static struct dma_async_tx_descriptor *mxs_dma_prep_dma_cyclic(
 		struct dma_chan *chan, dma_addr_t dma_addr, size_t buf_len,
-		size_t period_len, enum dma_data_direction direction)
+		size_t period_len, enum dma_transfer_direction direction,
+		void *context)
 {
 	struct mxs_dma_chan *mxs_chan = to_mxs_dma_chan(chan);
 	struct mxs_dma_engine *mxs_dma = mxs_chan->mxs_dma;
@@ -559,7 +549,7 @@ static enum dma_status mxs_dma_tx_status(struct dma_chan *chan,
 	dma_cookie_t last_used;
 
 	last_used = chan->cookie;
-	dma_set_tx_state(txstate, mxs_chan->last_completed, last_used, 0);
+	dma_set_tx_state(txstate, chan->completed_cookie, last_used, 0);
 
 	return mxs_chan->status;
 }
diff --git a/drivers/dma/pch_dma.c b/drivers/dma/pch_dma.c
index 1ac8d4b580b7..1ef600f39d44 100644
--- a/drivers/dma/pch_dma.c
+++ b/drivers/dma/pch_dma.c
@@ -25,6 +25,8 @@
 #include <linux/module.h>
 #include <linux/pch_dma.h>
 
+#include "dmaengine.h"
+
 #define DRV_NAME "pch-dma"
 
 #define DMA_CTL0_DISABLE		0x0
@@ -105,7 +107,6 @@ struct pch_dma_chan {
 
 	spinlock_t		lock;
 
-	dma_cookie_t		completed_cookie;
 	struct list_head	active_list;
 	struct list_head	queue;
 	struct list_head	free_list;
@@ -416,20 +417,6 @@ static void pdc_advance_work(struct pch_dma_chan *pd_chan)
 	}
 }
 
-static dma_cookie_t pdc_assign_cookie(struct pch_dma_chan *pd_chan,
-				      struct pch_dma_desc *desc)
-{
-	dma_cookie_t cookie = pd_chan->chan.cookie;
-
-	if (++cookie < 0)
-		cookie = 1;
-
-	pd_chan->chan.cookie = cookie;
-	desc->txd.cookie = cookie;
-
-	return cookie;
-}
-
 static dma_cookie_t pd_tx_submit(struct dma_async_tx_descriptor *txd)
 {
 	struct pch_dma_desc *desc = to_pd_desc(txd);
@@ -437,7 +424,7 @@ static dma_cookie_t pd_tx_submit(struct dma_async_tx_descriptor *txd)
 	dma_cookie_t cookie;
 
 	spin_lock(&pd_chan->lock);
-	cookie = pdc_assign_cookie(pd_chan, desc);
+	cookie = dma_cookie_assign(txd);
 
 	if (list_empty(&pd_chan->active_list)) {
 		list_add_tail(&desc->desc_node, &pd_chan->active_list);
@@ -544,7 +531,7 @@ static int pd_alloc_chan_resources(struct dma_chan *chan)
 	spin_lock_irq(&pd_chan->lock);
 	list_splice(&tmp_list, &pd_chan->free_list);
 	pd_chan->descs_allocated = i;
-	pd_chan->completed_cookie = chan->cookie = 1;
+	dma_cookie_init(chan);
 	spin_unlock_irq(&pd_chan->lock);
 
 	pdc_enable_irq(chan, 1);
@@ -578,19 +565,12 @@ static enum dma_status pd_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 				    struct dma_tx_state *txstate)
 {
 	struct pch_dma_chan *pd_chan = to_pd_chan(chan);
-	dma_cookie_t last_used;
-	dma_cookie_t last_completed;
-	int ret;
+	enum dma_status ret;
 
 	spin_lock_irq(&pd_chan->lock);
-	last_completed = pd_chan->completed_cookie;
-	last_used = chan->cookie;
+	ret = dma_cookie_status(chan, cookie, txstate);
 	spin_unlock_irq(&pd_chan->lock);
 
-	ret = dma_async_is_complete(cookie, last_completed, last_used);
-
-	dma_set_tx_state(txstate, last_completed, last_used, 0);
-
 	return ret;
 }
 
@@ -607,7 +587,8 @@ static void pd_issue_pending(struct dma_chan *chan)
 
 static struct dma_async_tx_descriptor *pd_prep_slave_sg(struct dma_chan *chan,
 			struct scatterlist *sgl, unsigned int sg_len,
-			enum dma_data_direction direction, unsigned long flags)
+			enum dma_transfer_direction direction, unsigned long flags,
+			void *context)
 {
 	struct pch_dma_chan *pd_chan = to_pd_chan(chan);
 	struct pch_dma_slave *pd_slave = chan->private;
@@ -934,8 +915,7 @@ static int __devinit pch_dma_probe(struct pci_dev *pdev,
 		struct pch_dma_chan *pd_chan = &pd->channels[i];
 
 		pd_chan->chan.device = &pd->dma;
-		pd_chan->chan.cookie = 1;
-		pd_chan->chan.chan_id = i;
+		dma_cookie_init(&pd_chan->chan);
 
 		pd_chan->membase = &regs->desc[i];
 
diff --git a/drivers/dma/pl330.c b/drivers/dma/pl330.c
index 00eee59e8b33..4378042c6371 100644
--- a/drivers/dma/pl330.c
+++ b/drivers/dma/pl330.c
@@ -18,6 +18,8 @@
 #include <linux/amba/bus.h>
 #include <linux/amba/pl330.h>
 
+#include "dmaengine.h"
+
 #define NR_DEFAULT_DESC	16
 
 enum desc_status {
@@ -48,9 +50,6 @@ struct dma_pl330_chan {
 	/* DMA-Engine Channel */
 	struct dma_chan chan;
 
-	/* Last completed cookie */
-	dma_cookie_t completed;
-
 	/* List of to be xfered descriptors */
 	struct list_head work_list;
 
@@ -193,7 +192,7 @@ static void pl330_tasklet(unsigned long data)
 	/* Pick up ripe tomatoes */
 	list_for_each_entry_safe(desc, _dt, &pch->work_list, node)
 		if (desc->status == DONE) {
-			pch->completed = desc->txd.cookie;
+			dma_cookie_complete(&desc->txd);
 			list_move_tail(&desc->node, &list);
 		}
 
@@ -235,7 +234,8 @@ static int pl330_alloc_chan_resources(struct dma_chan *chan)
 
 	spin_lock_irqsave(&pch->lock, flags);
 
-	pch->completed = chan->cookie = 1;
+	dma_cookie_init(chan);
+	pch->cyclic = false;
 
 	pch->pl330_chid = pl330_request_channel(&pdmac->pif);
 	if (!pch->pl330_chid) {
@@ -295,18 +295,7 @@ static enum dma_status
 pl330_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 		 struct dma_tx_state *txstate)
 {
-	struct dma_pl330_chan *pch = to_pchan(chan);
-	dma_cookie_t last_done, last_used;
-	int ret;
-
-	last_done = pch->completed;
-	last_used = chan->cookie;
-
-	ret = dma_async_is_complete(cookie, last_done, last_used);
-
-	dma_set_tx_state(txstate, last_done, last_used, 0);
-
-	return ret;
+	return dma_cookie_status(chan, cookie, txstate);
 }
 
 static void pl330_issue_pending(struct dma_chan *chan)
@@ -329,26 +318,16 @@ static dma_cookie_t pl330_tx_submit(struct dma_async_tx_descriptor *tx)
 	spin_lock_irqsave(&pch->lock, flags);
 
 	/* Assign cookies to all nodes */
-	cookie = tx->chan->cookie;
-
 	while (!list_empty(&last->node)) {
 		desc = list_entry(last->node.next, struct dma_pl330_desc, node);
 
-		if (++cookie < 0)
-			cookie = 1;
-		desc->txd.cookie = cookie;
+		dma_cookie_assign(&desc->txd);
 
 		list_move_tail(&desc->node, &pch->work_list);
 	}
 
-	if (++cookie < 0)
-		cookie = 1;
-	last->txd.cookie = cookie;
-
+	cookie = dma_cookie_assign(&last->txd);
 	list_add_tail(&last->node, &pch->work_list);
-
-	tx->chan->cookie = cookie;
-
 	spin_unlock_irqrestore(&pch->lock, flags);
 
 	return cookie;
@@ -524,6 +503,54 @@ static inline int get_burst_len(struct dma_pl330_desc *desc, size_t len)
 	return burst_len;
 }
 
+static struct dma_async_tx_descriptor *pl330_prep_dma_cyclic(
+		struct dma_chan *chan, dma_addr_t dma_addr, size_t len,
+		size_t period_len, enum dma_transfer_direction direction,
+		void *context)
+{
+	struct dma_pl330_desc *desc;
+	struct dma_pl330_chan *pch = to_pchan(chan);
+	dma_addr_t dst;
+	dma_addr_t src;
+
+	desc = pl330_get_desc(pch);
+	if (!desc) {
+		dev_err(pch->dmac->pif.dev, "%s:%d Unable to fetch desc\n",
+			__func__, __LINE__);
+		return NULL;
+	}
+
+	switch (direction) {
+	case DMA_MEM_TO_DEV:
+		desc->rqcfg.src_inc = 1;
+		desc->rqcfg.dst_inc = 0;
+		desc->req.rqtype = MEMTODEV;
+		src = dma_addr;
+		dst = pch->fifo_addr;
+		break;
+	case DMA_DEV_TO_MEM:
+		desc->rqcfg.src_inc = 0;
+		desc->rqcfg.dst_inc = 1;
+		desc->req.rqtype = DEVTOMEM;
+		src = pch->fifo_addr;
+		dst = dma_addr;
+		break;
+	default:
+		dev_err(pch->dmac->pif.dev, "%s:%d Invalid dma direction\n",
+		__func__, __LINE__);
+		return NULL;
+	}
+
+	desc->rqcfg.brst_size = pch->burst_sz;
+	desc->rqcfg.brst_len = 1;
+
+	pch->cyclic = true;
+
+	fill_px(&desc->px, dst, src, period_len);
+
+	return &desc->txd;
+}
+
 static struct dma_async_tx_descriptor *
 pl330_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dst,
 		dma_addr_t src, size_t len, unsigned long flags)
@@ -571,8 +598,8 @@ pl330_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dst,
 
 static struct dma_async_tx_descriptor *
 pl330_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
-		unsigned int sg_len, enum dma_data_direction direction,
-		unsigned long flg)
+		unsigned int sg_len, enum dma_transfer_direction direction,
+		unsigned long flg, void *context)
 {
 	struct dma_pl330_desc *first, *desc = NULL;
 	struct dma_pl330_chan *pch = to_pchan(chan);
diff --git a/drivers/dma/ppc4xx/adma.c b/drivers/dma/ppc4xx/adma.c
index fc457a7e8832..86239ea01898 100644
--- a/drivers/dma/ppc4xx/adma.c
+++ b/drivers/dma/ppc4xx/adma.c
@@ -46,6 +46,7 @@
 #include <asm/dcr.h>
 #include <asm/dcr-regs.h>
 #include "adma.h"
+#include "../dmaengine.h"
 
 enum ppc_adma_init_code {
 	PPC_ADMA_INIT_OK = 0,
@@ -1930,7 +1931,7 @@ static void __ppc440spe_adma_slot_cleanup(struct ppc440spe_adma_chan *chan)
 				if (end_of_chain && slot_cnt) {
 					/* Should wait for ZeroSum completion */
 					if (cookie > 0)
-						chan->completed_cookie = cookie;
+						chan->common.completed_cookie = cookie;
 					return;
 				}
 
@@ -1960,7 +1961,7 @@ static void __ppc440spe_adma_slot_cleanup(struct ppc440spe_adma_chan *chan)
 	BUG_ON(!seen_current);
 
 	if (cookie > 0) {
-		chan->completed_cookie = cookie;
+		chan->common.completed_cookie = cookie;
 		pr_debug("\tcompleted cookie %d\n", cookie);
 	}
 
@@ -2150,22 +2151,6 @@ static int ppc440spe_adma_alloc_chan_resources(struct dma_chan *chan)
 }
 
 /**
- * ppc440spe_desc_assign_cookie - assign a cookie
- */
-static dma_cookie_t ppc440spe_desc_assign_cookie(
-		struct ppc440spe_adma_chan *chan,
-		struct ppc440spe_adma_desc_slot *desc)
-{
-	dma_cookie_t cookie = chan->common.cookie;
-
-	cookie++;
-	if (cookie < 0)
-		cookie = 1;
-	chan->common.cookie = desc->async_tx.cookie = cookie;
-	return cookie;
-}
-
-/**
  * ppc440spe_rxor_set_region_data -
  */
 static void ppc440spe_rxor_set_region(struct ppc440spe_adma_desc_slot *desc,
@@ -2235,8 +2220,7 @@ static dma_cookie_t ppc440spe_adma_tx_submit(struct dma_async_tx_descriptor *tx)
 	slots_per_op = group_start->slots_per_op;
 
 	spin_lock_bh(&chan->lock);
-
-	cookie = ppc440spe_desc_assign_cookie(chan, sw_desc);
+	cookie = dma_cookie_assign(tx);
 
 	if (unlikely(list_empty(&chan->chain))) {
 		/* first peer */
@@ -3944,28 +3928,16 @@ static enum dma_status ppc440spe_adma_tx_status(struct dma_chan *chan,
 			dma_cookie_t cookie, struct dma_tx_state *txstate)
 {
 	struct ppc440spe_adma_chan *ppc440spe_chan;
-	dma_cookie_t last_used;
-	dma_cookie_t last_complete;
 	enum dma_status ret;
 
 	ppc440spe_chan = to_ppc440spe_adma_chan(chan);
-	last_used = chan->cookie;
-	last_complete = ppc440spe_chan->completed_cookie;
-
-	dma_set_tx_state(txstate, last_complete, last_used, 0);
-
-	ret = dma_async_is_complete(cookie, last_complete, last_used);
+	ret = dma_cookie_status(chan, cookie, txstate);
 	if (ret == DMA_SUCCESS)
 		return ret;
 
 	ppc440spe_adma_slot_cleanup(ppc440spe_chan);
 
-	last_used = chan->cookie;
-	last_complete = ppc440spe_chan->completed_cookie;
-
-	dma_set_tx_state(txstate, last_complete, last_used, 0);
-
-	return dma_async_is_complete(cookie, last_complete, last_used);
+	return dma_cookie_status(chan, cookie, txstate);
 }
 
 /**
@@ -4058,7 +4030,7 @@ static void ppc440spe_chan_start_null_xor(struct ppc440spe_adma_chan *chan)
 		/* initialize the completed cookie to be less than
 		 * the most recently used cookie
 		 */
-		chan->completed_cookie = cookie - 1;
+		chan->common.completed_cookie = cookie - 1;
 		chan->common.cookie = sw_desc->async_tx.cookie = cookie;
 
 		/* channel should not be busy */
diff --git a/drivers/dma/ppc4xx/adma.h b/drivers/dma/ppc4xx/adma.h
index 8ada5a812e3b..26b7a5ed9ac7 100644
--- a/drivers/dma/ppc4xx/adma.h
+++ b/drivers/dma/ppc4xx/adma.h
@@ -81,7 +81,6 @@ struct ppc440spe_adma_device {
  * @common: common dmaengine channel object members
  * @all_slots: complete domain of slots usable by the channel
  * @pending: allows batching of hardware operations
- * @completed_cookie: identifier for the most recently completed operation
  * @slots_allocated: records the actual size of the descriptor slot pool
  * @hw_chain_inited: h/w descriptor chain initialization flag
  * @irq_tasklet: bottom half where ppc440spe_adma_slot_cleanup runs
@@ -99,7 +98,6 @@ struct ppc440spe_adma_chan {
 	struct list_head all_slots;
 	struct ppc440spe_adma_desc_slot *last_used;
 	int pending;
-	dma_cookie_t completed_cookie;
 	int slots_allocated;
 	int hw_chain_inited;
 	struct tasklet_struct irq_tasklet;
diff --git a/drivers/dma/shdma.c b/drivers/dma/shdma.c
index 7f49235d14b9..39485c36fb7f 100644
--- a/drivers/dma/shdma.c
+++ b/drivers/dma/shdma.c
@@ -31,6 +31,8 @@
 #include <linux/kdebug.h>
 #include <linux/spinlock.h>
 #include <linux/rculist.h>
+
+#include "dmaengine.h"
 #include "shdma.h"
 
 /* DMA descriptor control */
@@ -268,13 +270,7 @@ static dma_cookie_t sh_dmae_tx_submit(struct dma_async_tx_descriptor *tx)
 
 	spin_lock_bh(&sh_chan->desc_lock);
 
-	cookie = sh_chan->common.cookie;
-	cookie++;
-	if (cookie < 0)
-		cookie = 1;
-
-	sh_chan->common.cookie = cookie;
-	tx->cookie = cookie;
+	cookie = dma_cookie_assign(tx);
 
 	/* Mark all chunks of this descriptor as submitted, move to the queue */
 	list_for_each_entry_safe(chunk, c, desc->node.prev, node) {
@@ -628,7 +624,8 @@ static struct dma_async_tx_descriptor *sh_dmae_prep_memcpy(
 
 static struct dma_async_tx_descriptor *sh_dmae_prep_slave_sg(
 	struct dma_chan *chan, struct scatterlist *sgl, unsigned int sg_len,
-	enum dma_data_direction direction, unsigned long flags)
+	enum dma_transfer_direction direction, unsigned long flags,
+	void *context)
 {
 	struct sh_dmae_slave *param;
 	struct sh_dmae_chan *sh_chan;
@@ -718,12 +715,12 @@ static dma_async_tx_callback __ld_cleanup(struct sh_dmae_chan *sh_chan, bool all
 			cookie = tx->cookie;
 
 		if (desc->mark == DESC_COMPLETED && desc->chunks == 1) {
-			if (sh_chan->completed_cookie != desc->cookie - 1)
+			if (sh_chan->common.completed_cookie != desc->cookie - 1)
 				dev_dbg(sh_chan->dev,
 					"Completing cookie %d, expected %d\n",
 					desc->cookie,
-					sh_chan->completed_cookie + 1);
-			sh_chan->completed_cookie = desc->cookie;
+					sh_chan->common.completed_cookie + 1);
+			sh_chan->common.completed_cookie = desc->cookie;
 		}
 
 		/* Call callback on the last chunk */
@@ -771,7 +768,7 @@ static dma_async_tx_callback __ld_cleanup(struct sh_dmae_chan *sh_chan, bool all
 		 * Terminating and the loop completed normally: forgive
 		 * uncompleted cookies
 		 */
-		sh_chan->completed_cookie = sh_chan->common.cookie;
+		sh_chan->common.completed_cookie = sh_chan->common.cookie;
 
 	spin_unlock_bh(&sh_chan->desc_lock);
 
@@ -828,22 +825,13 @@ static enum dma_status sh_dmae_tx_status(struct dma_chan *chan,
 					struct dma_tx_state *txstate)
 {
 	struct sh_dmae_chan *sh_chan = to_sh_chan(chan);
-	dma_cookie_t last_used;
-	dma_cookie_t last_complete;
 	enum dma_status status;
 
 	sh_dmae_chan_ld_cleanup(sh_chan, false);
 
-	/* First read completed cookie to avoid a skew */
-	last_complete = sh_chan->completed_cookie;
-	rmb();
-	last_used = chan->cookie;
-	BUG_ON(last_complete < 0);
-	dma_set_tx_state(txstate, last_complete, last_used, 0);
-
 	spin_lock_bh(&sh_chan->desc_lock);
 
-	status = dma_async_is_complete(cookie, last_complete, last_used);
+	status = dma_cookie_status(chan, cookie, txstate);
 
 	/*
 	 * If we don't find cookie on the queue, it has been aborted and we have
diff --git a/drivers/dma/shdma.h b/drivers/dma/shdma.h
index dc56576f9fdb..e5c4fdfbfcea 100644
--- a/drivers/dma/shdma.h
+++ b/drivers/dma/shdma.h
@@ -24,7 +24,6 @@
 struct device;
 
 struct sh_dmae_chan {
-	dma_cookie_t completed_cookie;	/* The maximum cookie completed */
 	spinlock_t desc_lock;		/* Descriptor operation lock */
 	struct list_head ld_queue;	/* Link descriptors queue */
 	struct list_head ld_free;	/* Link descriptors free */
diff --git a/drivers/dma/sirf-dma.c b/drivers/dma/sirf-dma.c
new file mode 100644
index 000000000000..1f8a83935db4
--- /dev/null
+++ b/drivers/dma/sirf-dma.c
@@ -0,0 +1,697 @@
+/*
+ * DMA controller driver for CSR SiRFprimaII
+ *
+ * Copyright (c) 2011 Cambridge Silicon Radio Limited, a CSR plc group company.
+ *
+ * Licensed under GPLv2 or later.
+ */
+
+#include <linux/module.h>
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/of_platform.h>
+#include <linux/sirfsoc_dma.h>
+
+#define SIRFSOC_DMA_DESCRIPTORS                 16
+#define SIRFSOC_DMA_CHANNELS                    16
+
+#define SIRFSOC_DMA_CH_ADDR                     0x00
+#define SIRFSOC_DMA_CH_XLEN                     0x04
+#define SIRFSOC_DMA_CH_YLEN                     0x08
+#define SIRFSOC_DMA_CH_CTRL                     0x0C
+
+#define SIRFSOC_DMA_WIDTH_0                     0x100
+#define SIRFSOC_DMA_CH_VALID                    0x140
+#define SIRFSOC_DMA_CH_INT                      0x144
+#define SIRFSOC_DMA_INT_EN                      0x148
+#define SIRFSOC_DMA_CH_LOOP_CTRL                0x150
+
+#define SIRFSOC_DMA_MODE_CTRL_BIT               4
+#define SIRFSOC_DMA_DIR_CTRL_BIT                5
+
+/* xlen and dma_width register is in 4 bytes boundary */
+#define SIRFSOC_DMA_WORD_LEN			4
+
+struct sirfsoc_dma_desc {
+	struct dma_async_tx_descriptor	desc;
+	struct list_head		node;
+
+	/* SiRFprimaII 2D-DMA parameters */
+
+	int             xlen;           /* DMA xlen */
+	int             ylen;           /* DMA ylen */
+	int             width;          /* DMA width */
+	int             dir;
+	bool            cyclic;         /* is loop DMA? */
+	u32             addr;		/* DMA buffer address */
+};
+
+struct sirfsoc_dma_chan {
+	struct dma_chan			chan;
+	struct list_head		free;
+	struct list_head		prepared;
+	struct list_head		queued;
+	struct list_head		active;
+	struct list_head		completed;
+	unsigned long			happened_cyclic;
+	unsigned long			completed_cyclic;
+
+	/* Lock for this structure */
+	spinlock_t			lock;
+
+	int				mode;
+};
+
+struct sirfsoc_dma {
+	struct dma_device		dma;
+	struct tasklet_struct		tasklet;
+	struct sirfsoc_dma_chan		channels[SIRFSOC_DMA_CHANNELS];
+	void __iomem			*base;
+	int				irq;
+};
+
+#define DRV_NAME	"sirfsoc_dma"
+
+/* Convert struct dma_chan to struct sirfsoc_dma_chan */
+static inline
+struct sirfsoc_dma_chan *dma_chan_to_sirfsoc_dma_chan(struct dma_chan *c)
+{
+	return container_of(c, struct sirfsoc_dma_chan, chan);
+}
+
+/* Convert struct dma_chan to struct sirfsoc_dma */
+static inline struct sirfsoc_dma *dma_chan_to_sirfsoc_dma(struct dma_chan *c)
+{
+	struct sirfsoc_dma_chan *schan = dma_chan_to_sirfsoc_dma_chan(c);
+	return container_of(schan, struct sirfsoc_dma, channels[c->chan_id]);
+}
+
+/* Execute all queued DMA descriptors */
+static void sirfsoc_dma_execute(struct sirfsoc_dma_chan *schan)
+{
+	struct sirfsoc_dma *sdma = dma_chan_to_sirfsoc_dma(&schan->chan);
+	int cid = schan->chan.chan_id;
+	struct sirfsoc_dma_desc *sdesc = NULL;
+
+	/*
+	 * lock has been held by functions calling this, so we don't hold
+	 * lock again
+	 */
+
+	sdesc = list_first_entry(&schan->queued, struct sirfsoc_dma_desc,
+		node);
+	/* Move the first queued descriptor to active list */
+	list_move_tail(&schan->queued, &schan->active);
+
+	/* Start the DMA transfer */
+	writel_relaxed(sdesc->width, sdma->base + SIRFSOC_DMA_WIDTH_0 +
+		cid * 4);
+	writel_relaxed(cid | (schan->mode << SIRFSOC_DMA_MODE_CTRL_BIT) |
+		(sdesc->dir << SIRFSOC_DMA_DIR_CTRL_BIT),
+		sdma->base + cid * 0x10 + SIRFSOC_DMA_CH_CTRL);
+	writel_relaxed(sdesc->xlen, sdma->base + cid * 0x10 +
+		SIRFSOC_DMA_CH_XLEN);
+	writel_relaxed(sdesc->ylen, sdma->base + cid * 0x10 +
+		SIRFSOC_DMA_CH_YLEN);
+	writel_relaxed(readl_relaxed(sdma->base + SIRFSOC_DMA_INT_EN) |
+		(1 << cid), sdma->base + SIRFSOC_DMA_INT_EN);
+
+	/*
+	 * writel has an implict memory write barrier to make sure data is
+	 * flushed into memory before starting DMA
+	 */
+	writel(sdesc->addr >> 2, sdma->base + cid * 0x10 + SIRFSOC_DMA_CH_ADDR);
+
+	if (sdesc->cyclic) {
+		writel((1 << cid) | 1 << (cid + 16) |
+			readl_relaxed(sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL),
+			sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL);
+		schan->happened_cyclic = schan->completed_cyclic = 0;
+	}
+}
+
+/* Interrupt handler */
+static irqreturn_t sirfsoc_dma_irq(int irq, void *data)
+{
+	struct sirfsoc_dma *sdma = data;
+	struct sirfsoc_dma_chan *schan;
+	struct sirfsoc_dma_desc *sdesc = NULL;
+	u32 is;
+	int ch;
+
+	is = readl(sdma->base + SIRFSOC_DMA_CH_INT);
+	while ((ch = fls(is) - 1) >= 0) {
+		is &= ~(1 << ch);
+		writel_relaxed(1 << ch, sdma->base + SIRFSOC_DMA_CH_INT);
+		schan = &sdma->channels[ch];
+
+		spin_lock(&schan->lock);
+
+		sdesc = list_first_entry(&schan->active, struct sirfsoc_dma_desc,
+			node);
+		if (!sdesc->cyclic) {
+			/* Execute queued descriptors */
+			list_splice_tail_init(&schan->active, &schan->completed);
+			if (!list_empty(&schan->queued))
+				sirfsoc_dma_execute(schan);
+		} else
+			schan->happened_cyclic++;
+
+		spin_unlock(&schan->lock);
+	}
+
+	/* Schedule tasklet */
+	tasklet_schedule(&sdma->tasklet);
+
+	return IRQ_HANDLED;
+}
+
+/* process completed descriptors */
+static void sirfsoc_dma_process_completed(struct sirfsoc_dma *sdma)
+{
+	dma_cookie_t last_cookie = 0;
+	struct sirfsoc_dma_chan *schan;
+	struct sirfsoc_dma_desc *sdesc;
+	struct dma_async_tx_descriptor *desc;
+	unsigned long flags;
+	unsigned long happened_cyclic;
+	LIST_HEAD(list);
+	int i;
+
+	for (i = 0; i < sdma->dma.chancnt; i++) {
+		schan = &sdma->channels[i];
+
+		/* Get all completed descriptors */
+		spin_lock_irqsave(&schan->lock, flags);
+		if (!list_empty(&schan->completed)) {
+			list_splice_tail_init(&schan->completed, &list);
+			spin_unlock_irqrestore(&schan->lock, flags);
+
+			/* Execute callbacks and run dependencies */
+			list_for_each_entry(sdesc, &list, node) {
+				desc = &sdesc->desc;
+
+				if (desc->callback)
+					desc->callback(desc->callback_param);
+
+				last_cookie = desc->cookie;
+				dma_run_dependencies(desc);
+			}
+
+			/* Free descriptors */
+			spin_lock_irqsave(&schan->lock, flags);
+			list_splice_tail_init(&list, &schan->free);
+			schan->chan.completed_cookie = last_cookie;
+			spin_unlock_irqrestore(&schan->lock, flags);
+		} else {
+			/* for cyclic channel, desc is always in active list */
+			sdesc = list_first_entry(&schan->active, struct sirfsoc_dma_desc,
+				node);
+
+			if (!sdesc || (sdesc && !sdesc->cyclic)) {
+				/* without active cyclic DMA */
+				spin_unlock_irqrestore(&schan->lock, flags);
+				continue;
+			}
+
+			/* cyclic DMA */
+			happened_cyclic = schan->happened_cyclic;
+			spin_unlock_irqrestore(&schan->lock, flags);
+
+			desc = &sdesc->desc;
+			while (happened_cyclic != schan->completed_cyclic) {
+				if (desc->callback)
+					desc->callback(desc->callback_param);
+				schan->completed_cyclic++;
+			}
+		}
+	}
+}
+
+/* DMA Tasklet */
+static void sirfsoc_dma_tasklet(unsigned long data)
+{
+	struct sirfsoc_dma *sdma = (void *)data;
+
+	sirfsoc_dma_process_completed(sdma);
+}
+
+/* Submit descriptor to hardware */
+static dma_cookie_t sirfsoc_dma_tx_submit(struct dma_async_tx_descriptor *txd)
+{
+	struct sirfsoc_dma_chan *schan = dma_chan_to_sirfsoc_dma_chan(txd->chan);
+	struct sirfsoc_dma_desc *sdesc;
+	unsigned long flags;
+	dma_cookie_t cookie;
+
+	sdesc = container_of(txd, struct sirfsoc_dma_desc, desc);
+
+	spin_lock_irqsave(&schan->lock, flags);
+
+	/* Move descriptor to queue */
+	list_move_tail(&sdesc->node, &schan->queued);
+
+	/* Update cookie */
+	cookie = dma_cookie_assign(txd);
+
+	spin_unlock_irqrestore(&schan->lock, flags);
+
+	return cookie;
+}
+
+static int sirfsoc_dma_slave_config(struct sirfsoc_dma_chan *schan,
+	struct dma_slave_config *config)
+{
+	unsigned long flags;
+
+	if ((config->src_addr_width != DMA_SLAVE_BUSWIDTH_4_BYTES) ||
+		(config->dst_addr_width != DMA_SLAVE_BUSWIDTH_4_BYTES))
+		return -EINVAL;
+
+	spin_lock_irqsave(&schan->lock, flags);
+	schan->mode = (config->src_maxburst == 4 ? 1 : 0);
+	spin_unlock_irqrestore(&schan->lock, flags);
+
+	return 0;
+}
+
+static int sirfsoc_dma_terminate_all(struct sirfsoc_dma_chan *schan)
+{
+	struct sirfsoc_dma *sdma = dma_chan_to_sirfsoc_dma(&schan->chan);
+	int cid = schan->chan.chan_id;
+	unsigned long flags;
+
+	writel_relaxed(readl_relaxed(sdma->base + SIRFSOC_DMA_INT_EN) &
+		~(1 << cid), sdma->base + SIRFSOC_DMA_INT_EN);
+	writel_relaxed(1 << cid, sdma->base + SIRFSOC_DMA_CH_VALID);
+
+	writel_relaxed(readl_relaxed(sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL)
+		& ~((1 << cid) | 1 << (cid + 16)),
+			sdma->base + SIRFSOC_DMA_CH_LOOP_CTRL);
+
+	spin_lock_irqsave(&schan->lock, flags);
+	list_splice_tail_init(&schan->active, &schan->free);
+	list_splice_tail_init(&schan->queued, &schan->free);
+	spin_unlock_irqrestore(&schan->lock, flags);
+
+	return 0;
+}
+
+static int sirfsoc_dma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
+	unsigned long arg)
+{
+	struct dma_slave_config *config;
+	struct sirfsoc_dma_chan *schan = dma_chan_to_sirfsoc_dma_chan(chan);
+
+	switch (cmd) {
+	case DMA_TERMINATE_ALL:
+		return sirfsoc_dma_terminate_all(schan);
+	case DMA_SLAVE_CONFIG:
+		config = (struct dma_slave_config *)arg;
+		return sirfsoc_dma_slave_config(schan, config);
+
+	default:
+		break;
+	}
+
+	return -ENOSYS;
+}
+
+/* Alloc channel resources */
+static int sirfsoc_dma_alloc_chan_resources(struct dma_chan *chan)
+{
+	struct sirfsoc_dma *sdma = dma_chan_to_sirfsoc_dma(chan);
+	struct sirfsoc_dma_chan *schan = dma_chan_to_sirfsoc_dma_chan(chan);
+	struct sirfsoc_dma_desc *sdesc;
+	unsigned long flags;
+	LIST_HEAD(descs);
+	int i;
+
+	/* Alloc descriptors for this channel */
+	for (i = 0; i < SIRFSOC_DMA_DESCRIPTORS; i++) {
+		sdesc = kzalloc(sizeof(*sdesc), GFP_KERNEL);
+		if (!sdesc) {
+			dev_notice(sdma->dma.dev, "Memory allocation error. "
+				"Allocated only %u descriptors\n", i);
+			break;
+		}
+
+		dma_async_tx_descriptor_init(&sdesc->desc, chan);
+		sdesc->desc.flags = DMA_CTRL_ACK;
+		sdesc->desc.tx_submit = sirfsoc_dma_tx_submit;
+
+		list_add_tail(&sdesc->node, &descs);
+	}
+
+	/* Return error only if no descriptors were allocated */
+	if (i == 0)
+		return -ENOMEM;
+
+	spin_lock_irqsave(&schan->lock, flags);
+
+	list_splice_tail_init(&descs, &schan->free);
+	spin_unlock_irqrestore(&schan->lock, flags);
+
+	return i;
+}
+
+/* Free channel resources */
+static void sirfsoc_dma_free_chan_resources(struct dma_chan *chan)
+{
+	struct sirfsoc_dma_chan *schan = dma_chan_to_sirfsoc_dma_chan(chan);
+	struct sirfsoc_dma_desc *sdesc, *tmp;
+	unsigned long flags;
+	LIST_HEAD(descs);
+
+	spin_lock_irqsave(&schan->lock, flags);
+
+	/* Channel must be idle */
+	BUG_ON(!list_empty(&schan->prepared));
+	BUG_ON(!list_empty(&schan->queued));
+	BUG_ON(!list_empty(&schan->active));
+	BUG_ON(!list_empty(&schan->completed));
+
+	/* Move data */
+	list_splice_tail_init(&schan->free, &descs);
+
+	spin_unlock_irqrestore(&schan->lock, flags);
+
+	/* Free descriptors */
+	list_for_each_entry_safe(sdesc, tmp, &descs, node)
+		kfree(sdesc);
+}
+
+/* Send pending descriptor to hardware */
+static void sirfsoc_dma_issue_pending(struct dma_chan *chan)
+{
+	struct sirfsoc_dma_chan *schan = dma_chan_to_sirfsoc_dma_chan(chan);
+	unsigned long flags;
+
+	spin_lock_irqsave(&schan->lock, flags);
+
+	if (list_empty(&schan->active) && !list_empty(&schan->queued))
+		sirfsoc_dma_execute(schan);
+
+	spin_unlock_irqrestore(&schan->lock, flags);
+}
+
+/* Check request completion status */
+static enum dma_status
+sirfsoc_dma_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
+	struct dma_tx_state *txstate)
+{
+	struct sirfsoc_dma_chan *schan = dma_chan_to_sirfsoc_dma_chan(chan);
+	unsigned long flags;
+	enum dma_status ret;
+
+	spin_lock_irqsave(&schan->lock, flags);
+	ret = dma_cookie_status(chan, cookie, txstate);
+	spin_unlock_irqrestore(&schan->lock, flags);
+
+	return ret;
+}
+
+static struct dma_async_tx_descriptor *sirfsoc_dma_prep_interleaved(
+	struct dma_chan *chan, struct dma_interleaved_template *xt,
+	unsigned long flags)
+{
+	struct sirfsoc_dma *sdma = dma_chan_to_sirfsoc_dma(chan);
+	struct sirfsoc_dma_chan *schan = dma_chan_to_sirfsoc_dma_chan(chan);
+	struct sirfsoc_dma_desc *sdesc = NULL;
+	unsigned long iflags;
+	int ret;
+
+	if ((xt->dir != DMA_MEM_TO_DEV) || (xt->dir != DMA_DEV_TO_MEM)) {
+		ret = -EINVAL;
+		goto err_dir;
+	}
+
+	/* Get free descriptor */
+	spin_lock_irqsave(&schan->lock, iflags);
+	if (!list_empty(&schan->free)) {
+		sdesc = list_first_entry(&schan->free, struct sirfsoc_dma_desc,
+			node);
+		list_del(&sdesc->node);
+	}
+	spin_unlock_irqrestore(&schan->lock, iflags);
+
+	if (!sdesc) {
+		/* try to free completed descriptors */
+		sirfsoc_dma_process_completed(sdma);
+		ret = 0;
+		goto no_desc;
+	}
+
+	/* Place descriptor in prepared list */
+	spin_lock_irqsave(&schan->lock, iflags);
+
+	/*
+	 * Number of chunks in a frame can only be 1 for prima2
+	 * and ylen (number of frame - 1) must be at least 0
+	 */
+	if ((xt->frame_size == 1) && (xt->numf > 0)) {
+		sdesc->cyclic = 0;
+		sdesc->xlen = xt->sgl[0].size / SIRFSOC_DMA_WORD_LEN;
+		sdesc->width = (xt->sgl[0].size + xt->sgl[0].icg) /
+				SIRFSOC_DMA_WORD_LEN;
+		sdesc->ylen = xt->numf - 1;
+		if (xt->dir == DMA_MEM_TO_DEV) {
+			sdesc->addr = xt->src_start;
+			sdesc->dir = 1;
+		} else {
+			sdesc->addr = xt->dst_start;
+			sdesc->dir = 0;
+		}
+
+		list_add_tail(&sdesc->node, &schan->prepared);
+	} else {
+		pr_err("sirfsoc DMA Invalid xfer\n");
+		ret = -EINVAL;
+		goto err_xfer;
+	}
+	spin_unlock_irqrestore(&schan->lock, iflags);
+
+	return &sdesc->desc;
+err_xfer:
+	spin_unlock_irqrestore(&schan->lock, iflags);
+no_desc:
+err_dir:
+	return ERR_PTR(ret);
+}
+
+static struct dma_async_tx_descriptor *
+sirfsoc_dma_prep_cyclic(struct dma_chan *chan, dma_addr_t addr,
+	size_t buf_len, size_t period_len,
+	enum dma_transfer_direction direction, void *context)
+{
+	struct sirfsoc_dma_chan *schan = dma_chan_to_sirfsoc_dma_chan(chan);
+	struct sirfsoc_dma_desc *sdesc = NULL;
+	unsigned long iflags;
+
+	/*
+	 * we only support cycle transfer with 2 period
+	 * If the X-length is set to 0, it would be the loop mode.
+	 * The DMA address keeps increasing until reaching the end of a loop
+	 * area whose size is defined by (DMA_WIDTH x (Y_LENGTH + 1)). Then
+	 * the DMA address goes back to the beginning of this area.
+	 * In loop mode, the DMA data region is divided into two parts, BUFA
+	 * and BUFB. DMA controller generates interrupts twice in each loop:
+	 * when the DMA address reaches the end of BUFA or the end of the
+	 * BUFB
+	 */
+	if (buf_len !=  2 * period_len)
+		return ERR_PTR(-EINVAL);
+
+	/* Get free descriptor */
+	spin_lock_irqsave(&schan->lock, iflags);
+	if (!list_empty(&schan->free)) {
+		sdesc = list_first_entry(&schan->free, struct sirfsoc_dma_desc,
+			node);
+		list_del(&sdesc->node);
+	}
+	spin_unlock_irqrestore(&schan->lock, iflags);
+
+	if (!sdesc)
+		return 0;
+
+	/* Place descriptor in prepared list */
+	spin_lock_irqsave(&schan->lock, iflags);
+	sdesc->addr = addr;
+	sdesc->cyclic = 1;
+	sdesc->xlen = 0;
+	sdesc->ylen = buf_len / SIRFSOC_DMA_WORD_LEN - 1;
+	sdesc->width = 1;
+	list_add_tail(&sdesc->node, &schan->prepared);
+	spin_unlock_irqrestore(&schan->lock, iflags);
+
+	return &sdesc->desc;
+}
+
+/*
+ * The DMA controller consists of 16 independent DMA channels.
+ * Each channel is allocated to a different function
+ */
+bool sirfsoc_dma_filter_id(struct dma_chan *chan, void *chan_id)
+{
+	unsigned int ch_nr = (unsigned int) chan_id;
+
+	if (ch_nr == chan->chan_id +
+		chan->device->dev_id * SIRFSOC_DMA_CHANNELS)
+		return true;
+
+	return false;
+}
+EXPORT_SYMBOL(sirfsoc_dma_filter_id);
+
+static int __devinit sirfsoc_dma_probe(struct platform_device *op)
+{
+	struct device_node *dn = op->dev.of_node;
+	struct device *dev = &op->dev;
+	struct dma_device *dma;
+	struct sirfsoc_dma *sdma;
+	struct sirfsoc_dma_chan *schan;
+	struct resource res;
+	ulong regs_start, regs_size;
+	u32 id;
+	int ret, i;
+
+	sdma = devm_kzalloc(dev, sizeof(*sdma), GFP_KERNEL);
+	if (!sdma) {
+		dev_err(dev, "Memory exhausted!\n");
+		return -ENOMEM;
+	}
+
+	if (of_property_read_u32(dn, "cell-index", &id)) {
+		dev_err(dev, "Fail to get DMAC index\n");
+		ret = -ENODEV;
+		goto free_mem;
+	}
+
+	sdma->irq = irq_of_parse_and_map(dn, 0);
+	if (sdma->irq == NO_IRQ) {
+		dev_err(dev, "Error mapping IRQ!\n");
+		ret = -EINVAL;
+		goto free_mem;
+	}
+
+	ret = of_address_to_resource(dn, 0, &res);
+	if (ret) {
+		dev_err(dev, "Error parsing memory region!\n");
+		goto free_mem;
+	}
+
+	regs_start = res.start;
+	regs_size = resource_size(&res);
+
+	sdma->base = devm_ioremap(dev, regs_start, regs_size);
+	if (!sdma->base) {
+		dev_err(dev, "Error mapping memory region!\n");
+		ret = -ENOMEM;
+		goto irq_dispose;
+	}
+
+	ret = devm_request_irq(dev, sdma->irq, &sirfsoc_dma_irq, 0, DRV_NAME,
+		sdma);
+	if (ret) {
+		dev_err(dev, "Error requesting IRQ!\n");
+		ret = -EINVAL;
+		goto unmap_mem;
+	}
+
+	dma = &sdma->dma;
+	dma->dev = dev;
+	dma->chancnt = SIRFSOC_DMA_CHANNELS;
+
+	dma->device_alloc_chan_resources = sirfsoc_dma_alloc_chan_resources;
+	dma->device_free_chan_resources = sirfsoc_dma_free_chan_resources;
+	dma->device_issue_pending = sirfsoc_dma_issue_pending;
+	dma->device_control = sirfsoc_dma_control;
+	dma->device_tx_status = sirfsoc_dma_tx_status;
+	dma->device_prep_interleaved_dma = sirfsoc_dma_prep_interleaved;
+	dma->device_prep_dma_cyclic = sirfsoc_dma_prep_cyclic;
+
+	INIT_LIST_HEAD(&dma->channels);
+	dma_cap_set(DMA_SLAVE, dma->cap_mask);
+	dma_cap_set(DMA_CYCLIC, dma->cap_mask);
+	dma_cap_set(DMA_INTERLEAVE, dma->cap_mask);
+	dma_cap_set(DMA_PRIVATE, dma->cap_mask);
+
+	for (i = 0; i < dma->chancnt; i++) {
+		schan = &sdma->channels[i];
+
+		schan->chan.device = dma;
+		dma_cookie_init(&schan->chan);
+
+		INIT_LIST_HEAD(&schan->free);
+		INIT_LIST_HEAD(&schan->prepared);
+		INIT_LIST_HEAD(&schan->queued);
+		INIT_LIST_HEAD(&schan->active);
+		INIT_LIST_HEAD(&schan->completed);
+
+		spin_lock_init(&schan->lock);
+		list_add_tail(&schan->chan.device_node, &dma->channels);
+	}
+
+	tasklet_init(&sdma->tasklet, sirfsoc_dma_tasklet, (unsigned long)sdma);
+
+	/* Register DMA engine */
+	dev_set_drvdata(dev, sdma);
+	ret = dma_async_device_register(dma);
+	if (ret)
+		goto free_irq;
+
+	dev_info(dev, "initialized SIRFSOC DMAC driver\n");
+
+	return 0;
+
+free_irq:
+	devm_free_irq(dev, sdma->irq, sdma);
+irq_dispose:
+	irq_dispose_mapping(sdma->irq);
+unmap_mem:
+	iounmap(sdma->base);
+free_mem:
+	devm_kfree(dev, sdma);
+	return ret;
+}
+
+static int __devexit sirfsoc_dma_remove(struct platform_device *op)
+{
+	struct device *dev = &op->dev;
+	struct sirfsoc_dma *sdma = dev_get_drvdata(dev);
+
+	dma_async_device_unregister(&sdma->dma);
+	devm_free_irq(dev, sdma->irq, sdma);
+	irq_dispose_mapping(sdma->irq);
+	iounmap(sdma->base);
+	devm_kfree(dev, sdma);
+	return 0;
+}
+
+static struct of_device_id sirfsoc_dma_match[] = {
+	{ .compatible = "sirf,prima2-dmac", },
+	{},
+};
+
+static struct platform_driver sirfsoc_dma_driver = {
+	.probe		= sirfsoc_dma_probe,
+	.remove		= __devexit_p(sirfsoc_dma_remove),
+	.driver = {
+		.name = DRV_NAME,
+		.owner = THIS_MODULE,
+		.of_match_table	= sirfsoc_dma_match,
+	},
+};
+
+module_platform_driver(sirfsoc_dma_driver);
+
+MODULE_AUTHOR("Rongjun Ying <rongjun.ying@csr.com>, "
+	"Barry Song <baohua.song@csr.com>");
+MODULE_DESCRIPTION("SIRFSOC DMA control driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/dma/ste_dma40.c b/drivers/dma/ste_dma40.c
index 467e4dcb20a0..e682dfa07ddc 100644
--- a/drivers/dma/ste_dma40.c
+++ b/drivers/dma/ste_dma40.c
@@ -18,6 +18,7 @@
 
 #include <plat/ste_dma40.h>
 
+#include "dmaengine.h"
 #include "ste_dma40_ll.h"
 
 #define D40_NAME "dma40"
@@ -163,8 +164,6 @@ struct d40_base;
  *
  * @lock: A spinlock to protect this struct.
  * @log_num: The logical number, if any of this channel.
- * @completed: Starts with 1, after first interrupt it is set to dma engine's
- * current cookie.
  * @pending_tx: The number of pending transfers. Used between interrupt handler
  * and tasklet.
  * @busy: Set to true when transfer is ongoing on this channel.
@@ -194,8 +193,6 @@ struct d40_base;
 struct d40_chan {
 	spinlock_t			 lock;
 	int				 log_num;
-	/* ID of the most recent completed transfer */
-	int				 completed;
 	int				 pending_tx;
 	bool				 busy;
 	struct d40_phy_res		*phy_chan;
@@ -1083,21 +1080,14 @@ static dma_cookie_t d40_tx_submit(struct dma_async_tx_descriptor *tx)
 					     chan);
 	struct d40_desc *d40d = container_of(tx, struct d40_desc, txd);
 	unsigned long flags;
+	dma_cookie_t cookie;
 
 	spin_lock_irqsave(&d40c->lock, flags);
-
-	d40c->chan.cookie++;
-
-	if (d40c->chan.cookie < 0)
-		d40c->chan.cookie = 1;
-
-	d40d->txd.cookie = d40c->chan.cookie;
-
+	cookie = dma_cookie_assign(tx);
 	d40_desc_queue(d40c, d40d);
-
 	spin_unlock_irqrestore(&d40c->lock, flags);
 
-	return tx->cookie;
+	return cookie;
 }
 
 static int d40_start(struct d40_chan *d40c)
@@ -1212,7 +1202,7 @@ static void dma_tasklet(unsigned long data)
 		goto err;
 
 	if (!d40d->cyclic)
-		d40c->completed = d40d->txd.cookie;
+		dma_cookie_complete(&d40d->txd);
 
 	/*
 	 * If terminating a channel pending_tx is set to zero.
@@ -2000,7 +1990,7 @@ static int d40_alloc_chan_resources(struct dma_chan *chan)
 	bool is_free_phy;
 	spin_lock_irqsave(&d40c->lock, flags);
 
-	d40c->completed = chan->cookie = 1;
+	dma_cookie_init(chan);
 
 	/* If no dma configuration is set use default configuration (memcpy) */
 	if (!d40c->configured) {
@@ -2107,8 +2097,9 @@ d40_prep_memcpy_sg(struct dma_chan *chan,
 static struct dma_async_tx_descriptor *d40_prep_slave_sg(struct dma_chan *chan,
 							 struct scatterlist *sgl,
 							 unsigned int sg_len,
-							 enum dma_data_direction direction,
-							 unsigned long dma_flags)
+							 enum dma_transfer_direction direction,
+							 unsigned long dma_flags,
+							 void *context)
 {
 	if (direction != DMA_FROM_DEVICE && direction != DMA_TO_DEVICE)
 		return NULL;
@@ -2119,7 +2110,7 @@ static struct dma_async_tx_descriptor *d40_prep_slave_sg(struct dma_chan *chan,
 static struct dma_async_tx_descriptor *
 dma40_prep_dma_cyclic(struct dma_chan *chan, dma_addr_t dma_addr,
 		     size_t buf_len, size_t period_len,
-		     enum dma_data_direction direction)
+		     enum dma_transfer_direction direction, void *context)
 {
 	unsigned int periods = buf_len / period_len;
 	struct dma_async_tx_descriptor *txd;
@@ -2151,25 +2142,19 @@ static enum dma_status d40_tx_status(struct dma_chan *chan,
 				     struct dma_tx_state *txstate)
 {
 	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
-	dma_cookie_t last_used;
-	dma_cookie_t last_complete;
-	int ret;
+	enum dma_status ret;
 
 	if (d40c->phy_chan == NULL) {
 		chan_err(d40c, "Cannot read status of unallocated channel\n");
 		return -EINVAL;
 	}
 
-	last_complete = d40c->completed;
-	last_used = chan->cookie;
+	ret = dma_cookie_status(chan, cookie, txstate);
+	if (ret != DMA_SUCCESS)
+		dma_set_residue(txstate, stedma40_residue(chan));
 
 	if (d40_is_paused(d40c))
 		ret = DMA_PAUSED;
-	else
-		ret = dma_async_is_complete(cookie, last_complete, last_used);
-
-	dma_set_tx_state(txstate, last_complete, last_used,
-			 stedma40_residue(chan));
 
 	return ret;
 }
diff --git a/drivers/dma/timb_dma.c b/drivers/dma/timb_dma.c
index f69f90a61873..ca164828cffa 100644
--- a/drivers/dma/timb_dma.c
+++ b/drivers/dma/timb_dma.c
@@ -31,6 +31,8 @@
 
 #include <linux/timb_dma.h>
 
+#include "dmaengine.h"
+
 #define DRIVER_NAME "timb-dma"
 
 /* Global DMA registers */
@@ -84,7 +86,6 @@ struct timb_dma_chan {
 					especially the lists and descriptors,
 					from races between the tasklet and calls
 					from above */
-	dma_cookie_t		last_completed_cookie;
 	bool			ongoing;
 	struct list_head	active_list;
 	struct list_head	queue;
@@ -284,7 +285,7 @@ static void __td_finish(struct timb_dma_chan *td_chan)
 	else
 		iowrite32(0, td_chan->membase + TIMBDMA_OFFS_TX_DLAR);
 */
-	td_chan->last_completed_cookie = txd->cookie;
+	dma_cookie_complete(txd);
 	td_chan->ongoing = false;
 
 	callback = txd->callback;
@@ -349,12 +350,7 @@ static dma_cookie_t td_tx_submit(struct dma_async_tx_descriptor *txd)
 	dma_cookie_t cookie;
 
 	spin_lock_bh(&td_chan->lock);
-
-	cookie = txd->chan->cookie;
-	if (++cookie < 0)
-		cookie = 1;
-	txd->chan->cookie = cookie;
-	txd->cookie = cookie;
+	cookie = dma_cookie_assign(txd);
 
 	if (list_empty(&td_chan->active_list)) {
 		dev_dbg(chan2dev(txd->chan), "%s: started %u\n", __func__,
@@ -481,8 +477,7 @@ static int td_alloc_chan_resources(struct dma_chan *chan)
 	}
 
 	spin_lock_bh(&td_chan->lock);
-	td_chan->last_completed_cookie = 1;
-	chan->cookie = 1;
+	dma_cookie_init(chan);
 	spin_unlock_bh(&td_chan->lock);
 
 	return 0;
@@ -517,18 +512,11 @@ static enum dma_status td_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 {
 	struct timb_dma_chan *td_chan =
 		container_of(chan, struct timb_dma_chan, chan);
-	dma_cookie_t		last_used;
-	dma_cookie_t		last_complete;
-	int			ret;
+	enum dma_status ret;
 
 	dev_dbg(chan2dev(chan), "%s: Entry\n", __func__);
 
-	last_complete = td_chan->last_completed_cookie;
-	last_used = chan->cookie;
-
-	ret = dma_async_is_complete(cookie, last_complete, last_used);
-
-	dma_set_tx_state(txstate, last_complete, last_used, 0);
+	ret = dma_cookie_status(chan, cookie, txstate);
 
 	dev_dbg(chan2dev(chan),
 		"%s: exit, ret: %d, last_complete: %d, last_used: %d\n",
@@ -558,7 +546,8 @@ static void td_issue_pending(struct dma_chan *chan)
 
 static struct dma_async_tx_descriptor *td_prep_slave_sg(struct dma_chan *chan,
 	struct scatterlist *sgl, unsigned int sg_len,
-	enum dma_data_direction direction, unsigned long flags)
+	enum dma_transfer_direction direction, unsigned long flags,
+	void *context)
 {
 	struct timb_dma_chan *td_chan =
 		container_of(chan, struct timb_dma_chan, chan);
@@ -766,8 +755,8 @@ static int __devinit td_probe(struct platform_device *pdev)
 		}
 
 		td_chan->chan.device = &td->dma;
-		td_chan->chan.cookie = 1;
 		td_chan->chan.chan_id = i;
+		dma_cookie_init(&td_chan->chan);
 		spin_lock_init(&td_chan->lock);
 		INIT_LIST_HEAD(&td_chan->active_list);
 		INIT_LIST_HEAD(&td_chan->queue);
diff --git a/drivers/dma/txx9dmac.c b/drivers/dma/txx9dmac.c
index cbd83e362b5e..0d850fe9c301 100644
--- a/drivers/dma/txx9dmac.c
+++ b/drivers/dma/txx9dmac.c
@@ -15,6 +15,8 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/scatterlist.h>
+
+#include "dmaengine.h"
 #include "txx9dmac.h"
 
 static struct txx9dmac_chan *to_txx9dmac_chan(struct dma_chan *chan)
@@ -279,21 +281,6 @@ static void txx9dmac_desc_put(struct txx9dmac_chan *dc,
 	}
 }
 
-/* Called with dc->lock held and bh disabled */
-static dma_cookie_t
-txx9dmac_assign_cookie(struct txx9dmac_chan *dc, struct txx9dmac_desc *desc)
-{
-	dma_cookie_t cookie = dc->chan.cookie;
-
-	if (++cookie < 0)
-		cookie = 1;
-
-	dc->chan.cookie = cookie;
-	desc->txd.cookie = cookie;
-
-	return cookie;
-}
-
 /*----------------------------------------------------------------------*/
 
 static void txx9dmac_dump_regs(struct txx9dmac_chan *dc)
@@ -424,7 +411,7 @@ txx9dmac_descriptor_complete(struct txx9dmac_chan *dc,
 	dev_vdbg(chan2dev(&dc->chan), "descriptor %u %p complete\n",
 		 txd->cookie, desc);
 
-	dc->completed = txd->cookie;
+	dma_cookie_complete(txd);
 	callback = txd->callback;
 	param = txd->callback_param;
 
@@ -738,7 +725,7 @@ static dma_cookie_t txx9dmac_tx_submit(struct dma_async_tx_descriptor *tx)
 	dma_cookie_t cookie;
 
 	spin_lock_bh(&dc->lock);
-	cookie = txx9dmac_assign_cookie(dc, desc);
+	cookie = dma_cookie_assign(tx);
 
 	dev_vdbg(chan2dev(tx->chan), "tx_submit: queued %u %p\n",
 		 desc->txd.cookie, desc);
@@ -845,8 +832,8 @@ txx9dmac_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 
 static struct dma_async_tx_descriptor *
 txx9dmac_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
-		unsigned int sg_len, enum dma_data_direction direction,
-		unsigned long flags)
+		unsigned int sg_len, enum dma_transfer_direction direction,
+		unsigned long flags, void *context)
 {
 	struct txx9dmac_chan *dc = to_txx9dmac_chan(chan);
 	struct txx9dmac_dev *ddev = dc->ddev;
@@ -972,27 +959,16 @@ txx9dmac_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 		   struct dma_tx_state *txstate)
 {
 	struct txx9dmac_chan *dc = to_txx9dmac_chan(chan);
-	dma_cookie_t last_used;
-	dma_cookie_t last_complete;
-	int ret;
-
-	last_complete = dc->completed;
-	last_used = chan->cookie;
+	enum dma_status ret;
 
-	ret = dma_async_is_complete(cookie, last_complete, last_used);
+	ret = dma_cookie_status(chan, cookie, txstate);
 	if (ret != DMA_SUCCESS) {
 		spin_lock_bh(&dc->lock);
 		txx9dmac_scan_descriptors(dc);
 		spin_unlock_bh(&dc->lock);
-
-		last_complete = dc->completed;
-		last_used = chan->cookie;
-
-		ret = dma_async_is_complete(cookie, last_complete, last_used);
+		ret = dma_cookie_status(chan, cookie, txstate);
 	}
 
-	dma_set_tx_state(txstate, last_complete, last_used, 0);
-
 	return ret;
 }
 
@@ -1057,7 +1033,7 @@ static int txx9dmac_alloc_chan_resources(struct dma_chan *chan)
 		return -EIO;
 	}
 
-	dc->completed = chan->cookie = 1;
+	dma_cookie_init(chan);
 
 	dc->ccr = TXX9_DMA_CCR_IMMCHN | TXX9_DMA_CCR_INTENE | CCR_LE;
 	txx9dmac_chan_set_SMPCHN(dc);
@@ -1186,7 +1162,7 @@ static int __init txx9dmac_chan_probe(struct platform_device *pdev)
 	dc->ddev->chan[ch] = dc;
 	dc->chan.device = &dc->dma;
 	list_add_tail(&dc->chan.device_node, &dc->chan.device->channels);
-	dc->chan.cookie = dc->completed = 1;
+	dma_cookie_init(&dc->chan);
 
 	if (is_dmac64(dc))
 		dc->ch_regs = &__txx9dmac_regs(dc->ddev)->CHAN[ch];
diff --git a/drivers/dma/txx9dmac.h b/drivers/dma/txx9dmac.h
index 365d42366b9f..f5a760598882 100644
--- a/drivers/dma/txx9dmac.h
+++ b/drivers/dma/txx9dmac.h
@@ -172,7 +172,6 @@ struct txx9dmac_chan {
 	spinlock_t		lock;
 
 	/* these other elements are all protected by lock */
-	dma_cookie_t		completed;
 	struct list_head	active_list;
 	struct list_head	queue;
 	struct list_head	free_list;
diff --git a/drivers/firmware/iscsi_ibft.c b/drivers/firmware/iscsi_ibft.c
index c811cb107904..2cce44a1d7d0 100644
--- a/drivers/firmware/iscsi_ibft.c
+++ b/drivers/firmware/iscsi_ibft.c
@@ -746,6 +746,37 @@ static void __exit ibft_exit(void)
 	ibft_cleanup();
 }
 
+#ifdef CONFIG_ACPI
+static const struct {
+	char *sign;
+} ibft_signs[] = {
+	/*
+	 * One spec says "IBFT", the other says "iBFT". We have to check
+	 * for both.
+	 */
+	{ ACPI_SIG_IBFT },
+	{ "iBFT" },
+};
+
+static void __init acpi_find_ibft_region(void)
+{
+	int i;
+	struct acpi_table_header *table = NULL;
+
+	if (acpi_disabled)
+		return;
+
+	for (i = 0; i < ARRAY_SIZE(ibft_signs) && !ibft_addr; i++) {
+		acpi_get_table(ibft_signs[i].sign, 0, &table);
+		ibft_addr = (struct acpi_table_ibft *)table;
+	}
+}
+#else
+static void __init acpi_find_ibft_region(void)
+{
+}
+#endif
+
 /*
  * ibft_init() - creates sysfs tree entries for the iBFT data.
  */
@@ -753,9 +784,16 @@ static int __init ibft_init(void)
 {
 	int rc = 0;
 
+	/*
+	   As on UEFI systems the setup_arch()/find_ibft_region()
+	   is called before ACPI tables are parsed and it only does
+	   legacy finding.
+	*/
+	if (!ibft_addr)
+		acpi_find_ibft_region();
+
 	if (ibft_addr) {
-		printk(KERN_INFO "iBFT detected at 0x%llx.\n",
-		       (u64)isa_virt_to_bus(ibft_addr));
+		pr_info("iBFT detected.\n");
 
 		rc = ibft_check_device();
 		if (rc)
diff --git a/drivers/firmware/iscsi_ibft_find.c b/drivers/firmware/iscsi_ibft_find.c
index bfe723266fd8..4da4eb9ae926 100644
--- a/drivers/firmware/iscsi_ibft_find.c
+++ b/drivers/firmware/iscsi_ibft_find.c
@@ -45,13 +45,6 @@ EXPORT_SYMBOL_GPL(ibft_addr);
 static const struct {
 	char *sign;
 } ibft_signs[] = {
-#ifdef CONFIG_ACPI
-	/*
-	 * One spec says "IBFT", the other says "iBFT". We have to check
-	 * for both.
-	 */
-	{ ACPI_SIG_IBFT },
-#endif
 	{ "iBFT" },
 	{ "BIFT" },	/* Broadcom iSCSI Offload */
 };
@@ -62,14 +55,6 @@ static const struct {
 #define VGA_MEM 0xA0000 /* VGA buffer */
 #define VGA_SIZE 0x20000 /* 128kB */
 
-#ifdef CONFIG_ACPI
-static int __init acpi_find_ibft(struct acpi_table_header *header)
-{
-	ibft_addr = (struct acpi_table_ibft *)header;
-	return 0;
-}
-#endif /* CONFIG_ACPI */
-
 static int __init find_ibft_in_mem(void)
 {
 	unsigned long pos;
@@ -94,6 +79,7 @@ static int __init find_ibft_in_mem(void)
 				 * the table cannot be valid. */
 				if (pos + len <= (IBFT_END-1)) {
 					ibft_addr = (struct acpi_table_ibft *)virt;
+					pr_info("iBFT found at 0x%lx.\n", pos);
 					goto done;
 				}
 			}
@@ -108,20 +94,12 @@ done:
  */
 unsigned long __init find_ibft_region(unsigned long *sizep)
 {
-#ifdef CONFIG_ACPI
-	int i;
-#endif
 	ibft_addr = NULL;
 
-#ifdef CONFIG_ACPI
-	for (i = 0; i < ARRAY_SIZE(ibft_signs) && !ibft_addr; i++)
-		acpi_table_parse(ibft_signs[i].sign, acpi_find_ibft);
-#endif /* CONFIG_ACPI */
-
 	/* iBFT 1.03 section 1.4.3.1 mandates that UEFI machines will
 	 * only use ACPI for this */
 
-	if (!ibft_addr && !efi_enabled)
+	if (!efi_enabled)
 		find_ibft_in_mem();
 
 	if (ibft_addr) {
diff --git a/drivers/firmware/sigma.c b/drivers/firmware/sigma.c
index f10fc521951b..1eedb6f7fdab 100644
--- a/drivers/firmware/sigma.c
+++ b/drivers/firmware/sigma.c
@@ -14,13 +14,34 @@
 #include <linux/module.h>
 #include <linux/sigma.h>
 
-/* Return: 0==OK, <0==error, =1 ==no more actions */
+static size_t sigma_action_size(struct sigma_action *sa)
+{
+	size_t payload = 0;
+
+	switch (sa->instr) {
+	case SIGMA_ACTION_WRITEXBYTES:
+	case SIGMA_ACTION_WRITESINGLE:
+	case SIGMA_ACTION_WRITESAFELOAD:
+		payload = sigma_action_len(sa);
+		break;
+	default:
+		break;
+	}
+
+	payload = ALIGN(payload, 2);
+
+	return payload + sizeof(struct sigma_action);
+}
+
+/*
+ * Returns a negative error value in case of an error, 0 if processing of
+ * the firmware should be stopped after this action, 1 otherwise.
+ */
 static int
-process_sigma_action(struct i2c_client *client, struct sigma_firmware *ssfw)
+process_sigma_action(struct i2c_client *client, struct sigma_action *sa)
 {
-	struct sigma_action *sa = (void *)(ssfw->fw->data + ssfw->pos);
 	size_t len = sigma_action_len(sa);
-	int ret = 0;
+	int ret;
 
 	pr_debug("%s: instr:%i addr:%#x len:%zu\n", __func__,
 		sa->instr, sa->addr, len);
@@ -29,44 +50,50 @@ process_sigma_action(struct i2c_client *client, struct sigma_firmware *ssfw)
 	case SIGMA_ACTION_WRITEXBYTES:
 	case SIGMA_ACTION_WRITESINGLE:
 	case SIGMA_ACTION_WRITESAFELOAD:
-		if (ssfw->fw->size < ssfw->pos + len)
-			return -EINVAL;
 		ret = i2c_master_send(client, (void *)&sa->addr, len);
 		if (ret < 0)
 			return -EINVAL;
 		break;
-
 	case SIGMA_ACTION_DELAY:
-		ret = 0;
 		udelay(len);
 		len = 0;
 		break;
-
 	case SIGMA_ACTION_END:
-		return 1;
-
+		return 0;
 	default:
 		return -EINVAL;
 	}
 
-	/* when arrive here ret=0 or sent data */
-	ssfw->pos += sigma_action_size(sa, len);
-	return ssfw->pos == ssfw->fw->size;
+	return 1;
 }
 
 static int
 process_sigma_actions(struct i2c_client *client, struct sigma_firmware *ssfw)
 {
-	pr_debug("%s: processing %p\n", __func__, ssfw);
+	struct sigma_action *sa;
+	size_t size;
+	int ret;
+
+	while (ssfw->pos + sizeof(*sa) <= ssfw->fw->size) {
+		sa = (struct sigma_action *)(ssfw->fw->data + ssfw->pos);
+
+		size = sigma_action_size(sa);
+		ssfw->pos += size;
+		if (ssfw->pos > ssfw->fw->size || size == 0)
+			break;
+
+		ret = process_sigma_action(client, sa);
 
-	while (1) {
-		int ret = process_sigma_action(client, ssfw);
 		pr_debug("%s: action returned %i\n", __func__, ret);
-		if (ret == 1)
-			return 0;
-		else if (ret)
+
+		if (ret <= 0)
 			return ret;
 	}
+
+	if (ssfw->pos != ssfw->fw->size)
+		return -EINVAL;
+
+	return 0;
 }
 
 int process_sigma_firmware(struct i2c_client *client, const char *name)
@@ -89,16 +116,24 @@ int process_sigma_firmware(struct i2c_client *client, const char *name)
 
 	/* then verify the header */
 	ret = -EINVAL;
-	if (fw->size < sizeof(*ssfw_head))
+
+	/*
+	 * Reject too small or unreasonable large files. The upper limit has been
+	 * chosen a bit arbitrarily, but it should be enough for all practical
+	 * purposes and having the limit makes it easier to avoid integer
+	 * overflows later in the loading process.
+	 */
+	if (fw->size < sizeof(*ssfw_head) || fw->size >= 0x4000000)
 		goto done;
 
 	ssfw_head = (void *)fw->data;
 	if (memcmp(ssfw_head->magic, SIGMA_MAGIC, ARRAY_SIZE(ssfw_head->magic)))
 		goto done;
 
-	crc = crc32(0, fw->data, fw->size);
+	crc = crc32(0, fw->data + sizeof(*ssfw_head),
+			fw->size - sizeof(*ssfw_head));
 	pr_debug("%s: crc=%x\n", __func__, crc);
-	if (crc != ssfw_head->crc)
+	if (crc != le32_to_cpu(ssfw_head->crc))
 		goto done;
 
 	ssfw.pos = sizeof(*ssfw_head);
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index d539efd96d4b..9b304f254f98 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -271,6 +271,15 @@ config GPIO_PCF857X
 	  This driver provides an in-kernel interface to those GPIOs using
 	  platform-neutral GPIO calls.
 
+config GPIO_RC5T583
+	bool "RICOH RC5T583 GPIO"
+	depends on MFD_RC5T583
+	help
+	  Select this option to enable GPIO driver for the Ricoh RC5T583
+	  chip family.
+	  This driver provides the support for driving/reading the gpio pins
+	  of RC5T583 device through standard gpio library.
+
 config GPIO_SX150X
 	bool "Semtech SX150x I2C GPIO expander"
 	depends on I2C=y
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 9588948c96f0..7754ff0c239d 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -1,5 +1,7 @@
 # generic gpio support: platform drivers, dedicated expander chips, etc
 
+GCOV_PROFILE := y
+
 ccflags-$(CONFIG_DEBUG_GPIO)	+= -DDEBUG
 
 obj-$(CONFIG_GPIOLIB)		+= gpiolib.o
@@ -37,6 +39,7 @@ obj-$(CONFIG_GPIO_PCA953X)	+= gpio-pca953x.o
 obj-$(CONFIG_GPIO_PCF857X)	+= gpio-pcf857x.o
 obj-$(CONFIG_GPIO_PCH)		+= gpio-pch.o
 obj-$(CONFIG_GPIO_PL061)	+= gpio-pl061.o
+obj-$(CONFIG_GPIO_RC5T583)	+= gpio-rc5t583.o
 obj-$(CONFIG_GPIO_RDC321X)	+= gpio-rdc321x.o
 
 obj-$(CONFIG_GPIO_PLAT_SAMSUNG)	+= gpio-plat-samsung.o
@@ -47,6 +50,7 @@ obj-$(CONFIG_GPIO_SCH)		+= gpio-sch.o
 obj-$(CONFIG_GPIO_STMPE)	+= gpio-stmpe.o
 obj-$(CONFIG_GPIO_SX150X)	+= gpio-sx150x.o
 obj-$(CONFIG_GPIO_TC3589X)	+= gpio-tc3589x.o
+CFLAGS_gpio-tegra.o		= -Werror
 obj-$(CONFIG_ARCH_TEGRA)	+= gpio-tegra.o
 obj-$(CONFIG_GPIO_TIMBERDALE)	+= gpio-timberdale.o
 obj-$(CONFIG_GPIO_TPS65910)	+= gpio-tps65910.o
diff --git a/drivers/gpio/gpio-rc5t583.c b/drivers/gpio/gpio-rc5t583.c
new file mode 100644
index 000000000000..08428bf17718
--- /dev/null
+++ b/drivers/gpio/gpio-rc5t583.c
@@ -0,0 +1,180 @@
+/*
+ * GPIO driver for RICOH583 power management chip.
+ *
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ * Author: Laxman dewangan <ldewangan@nvidia.com>
+ *
+ * Based on code
+ *	Copyright (C) 2011 RICOH COMPANY,LTD
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ *
+ */
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/device.h>
+#include <linux/gpio.h>
+#include <linux/mfd/rc5t583.h>
+
+struct rc5t583_gpio {
+	struct gpio_chip gpio_chip;
+	struct rc5t583 *rc5t583;
+};
+
+static inline struct rc5t583_gpio *to_rc5t583_gpio(struct gpio_chip *chip)
+{
+	return container_of(chip, struct rc5t583_gpio, gpio_chip);
+}
+
+static int rc5t583_gpio_get(struct gpio_chip *gc, unsigned int offset)
+{
+	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct device *parent = rc5t583_gpio->rc5t583->dev;
+	uint8_t val = 0;
+	int ret;
+
+	ret = rc5t583_read(parent, RC5T583_GPIO_MON_IOIN, &val);
+	if (ret < 0)
+		return ret;
+
+	return !!(val & BIT(offset));
+}
+
+static void rc5t583_gpio_set(struct gpio_chip *gc, unsigned int offset, int val)
+{
+	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct device *parent = rc5t583_gpio->rc5t583->dev;
+	if (val)
+		rc5t583_set_bits(parent, RC5T583_GPIO_IOOUT, BIT(offset));
+	else
+		rc5t583_clear_bits(parent, RC5T583_GPIO_IOOUT, BIT(offset));
+}
+
+static int rc5t583_gpio_dir_input(struct gpio_chip *gc, unsigned int offset)
+{
+	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct device *parent = rc5t583_gpio->rc5t583->dev;
+	int ret;
+
+	ret = rc5t583_clear_bits(parent, RC5T583_GPIO_IOSEL, BIT(offset));
+	if (ret < 0)
+		return ret;
+
+	/* Set pin to gpio mode */
+	return rc5t583_clear_bits(parent, RC5T583_GPIO_PGSEL, BIT(offset));
+}
+
+static int rc5t583_gpio_dir_output(struct gpio_chip *gc, unsigned offset,
+			int value)
+{
+	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct device *parent = rc5t583_gpio->rc5t583->dev;
+	int ret;
+
+	rc5t583_gpio_set(gc, offset, value);
+	ret = rc5t583_set_bits(parent, RC5T583_GPIO_IOSEL, BIT(offset));
+	if (ret < 0)
+		return ret;
+
+	/* Set pin to gpio mode */
+	return rc5t583_clear_bits(parent, RC5T583_GPIO_PGSEL, BIT(offset));
+}
+
+static int rc5t583_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
+{
+	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+
+	if ((offset >= 0) && (offset < 8))
+		return rc5t583_gpio->rc5t583->irq_base +
+				RC5T583_IRQ_GPIO0 + offset;
+	return -EINVAL;
+}
+
+static void rc5t583_gpio_free(struct gpio_chip *gc, unsigned offset)
+{
+	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct device *parent = rc5t583_gpio->rc5t583->dev;
+
+	rc5t583_set_bits(parent, RC5T583_GPIO_PGSEL, BIT(offset));
+}
+
+static int __devinit rc5t583_gpio_probe(struct platform_device *pdev)
+{
+	struct rc5t583 *rc5t583 = dev_get_drvdata(pdev->dev.parent);
+	struct rc5t583_platform_data *pdata = dev_get_platdata(rc5t583->dev);
+	struct rc5t583_gpio *rc5t583_gpio;
+
+	rc5t583_gpio = devm_kzalloc(&pdev->dev, sizeof(*rc5t583_gpio),
+					GFP_KERNEL);
+	if (!rc5t583_gpio) {
+		dev_warn(&pdev->dev, "Mem allocation for rc5t583_gpio failed");
+		return -ENOMEM;
+	}
+
+	rc5t583_gpio->gpio_chip.label = "gpio-rc5t583",
+	rc5t583_gpio->gpio_chip.owner = THIS_MODULE,
+	rc5t583_gpio->gpio_chip.free = rc5t583_gpio_free,
+	rc5t583_gpio->gpio_chip.direction_input = rc5t583_gpio_dir_input,
+	rc5t583_gpio->gpio_chip.direction_output = rc5t583_gpio_dir_output,
+	rc5t583_gpio->gpio_chip.set = rc5t583_gpio_set,
+	rc5t583_gpio->gpio_chip.get = rc5t583_gpio_get,
+	rc5t583_gpio->gpio_chip.to_irq = rc5t583_gpio_to_irq,
+	rc5t583_gpio->gpio_chip.ngpio = RC5T583_MAX_GPIO,
+	rc5t583_gpio->gpio_chip.can_sleep = 1,
+	rc5t583_gpio->gpio_chip.dev = &pdev->dev;
+	rc5t583_gpio->gpio_chip.base = -1;
+	rc5t583_gpio->rc5t583 = rc5t583;
+
+	if (pdata && pdata->gpio_base)
+		rc5t583_gpio->gpio_chip.base = pdata->gpio_base;
+
+	platform_set_drvdata(pdev, rc5t583_gpio);
+
+	return gpiochip_add(&rc5t583_gpio->gpio_chip);
+}
+
+static int __devexit rc5t583_gpio_remove(struct platform_device *pdev)
+{
+	struct rc5t583_gpio *rc5t583_gpio = platform_get_drvdata(pdev);
+
+	return gpiochip_remove(&rc5t583_gpio->gpio_chip);
+}
+
+static struct platform_driver rc5t583_gpio_driver = {
+	.driver = {
+		.name    = "rc5t583-gpio",
+		.owner   = THIS_MODULE,
+	},
+	.probe		= rc5t583_gpio_probe,
+	.remove		= __devexit_p(rc5t583_gpio_remove),
+};
+
+static int __init rc5t583_gpio_init(void)
+{
+	return platform_driver_register(&rc5t583_gpio_driver);
+}
+subsys_initcall(rc5t583_gpio_init);
+
+static void __exit rc5t583_gpio_exit(void)
+{
+	platform_driver_unregister(&rc5t583_gpio_driver);
+}
+module_exit(rc5t583_gpio_exit);
+
+MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
+MODULE_DESCRIPTION("GPIO interface for RC5T583");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:rc5t583-gpio");
diff --git a/drivers/gpio/gpio-tegra.c b/drivers/gpio/gpio-tegra.c
index 747eb40e8afe..c1a80fb8578d 100644
--- a/drivers/gpio/gpio-tegra.c
+++ b/drivers/gpio/gpio-tegra.c
@@ -6,6 +6,8 @@
  * Author:
  *	Erik Gilling <konkers@google.com>
  *
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION.  All rights reserved.
+ *
  * This software is licensed under the terms of the GNU General Public
  * License version 2, as published by the Free Software Foundation, and
  * may be copied, distributed, and modified under those terms.
@@ -20,24 +22,25 @@
 #include <linux/init.h>
 #include <linux/irq.h>
 #include <linux/interrupt.h>
+#include <linux/delay.h>
 
 #include <linux/io.h>
 #include <linux/gpio.h>
 #include <linux/of.h>
+#include <linux/syscore_ops.h>
 
 #include <asm/mach/irq.h>
 
 #include <mach/iomap.h>
-#include <mach/suspend.h>
+#include <mach/legacy_irq.h>
+#include <mach/pinmux.h>
+
+#include "../../../arch/arm/mach-tegra/pm-irq.h"
 
 #define GPIO_BANK(x)		((x) >> 5)
 #define GPIO_PORT(x)		(((x) >> 3) & 0x3)
 #define GPIO_BIT(x)		((x) & 0x7)
 
-#define GPIO_REG(x)		(IO_TO_VIRT(TEGRA_GPIO_BASE) +	\
-				 GPIO_BANK(x) * 0x80 +		\
-				 GPIO_PORT(x) * 4)
-
 #define GPIO_CNF(x)		(GPIO_REG(x) + 0x00)
 #define GPIO_OE(x)		(GPIO_REG(x) + 0x10)
 #define GPIO_OUT(x)		(GPIO_REG(x) + 0X20)
@@ -47,12 +50,29 @@
 #define GPIO_INT_LVL(x)		(GPIO_REG(x) + 0x60)
 #define GPIO_INT_CLR(x)		(GPIO_REG(x) + 0x70)
 
+#if defined(CONFIG_ARCH_TEGRA_2x_SOC)
+#define GPIO_REG(x)		(IO_TO_VIRT(TEGRA_GPIO_BASE) +	\
+				 GPIO_BANK(x) * 0x80 +		\
+				 GPIO_PORT(x) * 4)
+
 #define GPIO_MSK_CNF(x)		(GPIO_REG(x) + 0x800)
 #define GPIO_MSK_OE(x)		(GPIO_REG(x) + 0x810)
 #define GPIO_MSK_OUT(x)		(GPIO_REG(x) + 0X820)
 #define GPIO_MSK_INT_STA(x)	(GPIO_REG(x) + 0x840)
 #define GPIO_MSK_INT_ENB(x)	(GPIO_REG(x) + 0x850)
 #define GPIO_MSK_INT_LVL(x)	(GPIO_REG(x) + 0x860)
+#else
+#define GPIO_REG(x)		(IO_TO_VIRT(TEGRA_GPIO_BASE) +	\
+				 GPIO_BANK(x) * 0x100 +		\
+				 GPIO_PORT(x) * 4)
+
+#define GPIO_MSK_CNF(x)		(GPIO_REG(x) + 0x80)
+#define GPIO_MSK_OE(x)		(GPIO_REG(x) + 0x90)
+#define GPIO_MSK_OUT(x)		(GPIO_REG(x) + 0XA0)
+#define GPIO_MSK_INT_STA(x)	(GPIO_REG(x) + 0xC0)
+#define GPIO_MSK_INT_ENB(x)	(GPIO_REG(x) + 0xD0)
+#define GPIO_MSK_INT_LVL(x)	(GPIO_REG(x) + 0xE0)
+#endif
 
 #define GPIO_INT_LVL_MASK		0x010101
 #define GPIO_INT_LVL_EDGE_RISING	0x000101
@@ -65,16 +85,16 @@ struct tegra_gpio_bank {
 	int bank;
 	int irq;
 	spinlock_t lvl_lock[4];
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 	u32 cnf[4];
 	u32 out[4];
 	u32 oe[4];
 	u32 int_enb[4];
 	u32 int_lvl[4];
+	u32 wake_enb[4];
 #endif
 };
 
-
 static struct tegra_gpio_bank tegra_gpio_banks[] = {
 	{.bank = 0, .irq = INT_GPIO1},
 	{.bank = 1, .irq = INT_GPIO2},
@@ -83,6 +103,9 @@ static struct tegra_gpio_bank tegra_gpio_banks[] = {
 	{.bank = 4, .irq = INT_GPIO5},
 	{.bank = 5, .irq = INT_GPIO6},
 	{.bank = 6, .irq = INT_GPIO7},
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+	{.bank = 7, .irq = INT_GPIO8},
+#endif
 };
 
 static int tegra_gpio_compose(int bank, int port, int bit)
@@ -90,6 +113,12 @@ static int tegra_gpio_compose(int bank, int port, int bit)
 	return (bank << 5) | ((port & 0x3) << 3) | (bit & 0x7);
 }
 
+void tegra_gpio_set_tristate(int gpio_nr, enum tegra_tristate ts)
+{
+	int pin_group  =  tegra_pinmux_get_pingroup(gpio_nr);
+	tegra_pinmux_set_tristate(pin_group, ts);
+}
+
 static void tegra_gpio_mask_write(u32 reg, int gpio, int value)
 {
 	u32 val;
@@ -100,15 +129,53 @@ static void tegra_gpio_mask_write(u32 reg, int gpio, int value)
 	__raw_writel(val, reg);
 }
 
+int tegra_gpio_get_bank_int_nr(int gpio)
+{
+	int bank;
+	int irq;
+	if (gpio >= TEGRA_NR_GPIOS) {
+		pr_warn("%s : Invalid gpio ID - %d\n", __func__, gpio);
+		return -EINVAL;
+	}
+	bank = gpio >> 5;
+	irq = tegra_gpio_banks[bank].irq;
+	return irq;
+}
+
 void tegra_gpio_enable(int gpio)
 {
+	if (gpio >= TEGRA_NR_GPIOS) {
+		pr_warn("%s : Invalid gpio ID - %d\n", __func__, gpio);
+		return;
+	}
 	tegra_gpio_mask_write(GPIO_MSK_CNF(gpio), gpio, 1);
 }
+EXPORT_SYMBOL_GPL(tegra_gpio_enable);
 
 void tegra_gpio_disable(int gpio)
 {
+	if (gpio >= TEGRA_NR_GPIOS) {
+		pr_warn("%s : Invalid gpio ID - %d\n", __func__, gpio);
+		return;
+	}
 	tegra_gpio_mask_write(GPIO_MSK_CNF(gpio), gpio, 0);
 }
+EXPORT_SYMBOL_GPL(tegra_gpio_disable);
+
+void tegra_gpio_init_configure(unsigned gpio, bool is_input, int value)
+{
+	if (gpio >= TEGRA_NR_GPIOS) {
+		pr_warn("%s : Invalid gpio ID - %d\n", __func__, gpio);
+		return;
+	}
+	if (is_input) {
+		tegra_gpio_mask_write(GPIO_MSK_OE(gpio), gpio, 0);
+	} else {
+		tegra_gpio_mask_write(GPIO_MSK_OUT(gpio), gpio, value);
+		tegra_gpio_mask_write(GPIO_MSK_OE(gpio), gpio, 1);
+	}
+	tegra_gpio_mask_write(GPIO_MSK_CNF(gpio), gpio, 1);
+}
 
 static void tegra_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
@@ -117,12 +184,16 @@ static void tegra_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 
 static int tegra_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
+	if ((__raw_readl(GPIO_OE(offset)) >> GPIO_BIT(offset)) & 0x1)
+		return (__raw_readl(GPIO_OUT(offset)) >>
+			GPIO_BIT(offset)) & 0x1;
 	return (__raw_readl(GPIO_IN(offset)) >> GPIO_BIT(offset)) & 0x1;
 }
 
 static int tegra_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
 	tegra_gpio_mask_write(GPIO_MSK_OE(offset), offset, 0);
+	tegra_gpio_enable(offset);
 	return 0;
 }
 
@@ -131,17 +202,41 @@ static int tegra_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 {
 	tegra_gpio_set(chip, offset, value);
 	tegra_gpio_mask_write(GPIO_MSK_OE(offset), offset, 1);
+	tegra_gpio_enable(offset);
 	return 0;
 }
 
+static int tegra_gpio_set_debounce(struct gpio_chip *chip, unsigned offset,
+				unsigned debounce)
+{
+	return -ENOSYS;
+}
+
+static int tegra_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
+{
+	return TEGRA_GPIO_TO_IRQ(offset);
+}
 
+static int tegra_gpio_request(struct gpio_chip *chip, unsigned offset)
+{
+	return 0;
+}
+
+static void tegra_gpio_free(struct gpio_chip *chip, unsigned offset)
+{
+	tegra_gpio_disable(offset);
+}
 
 static struct gpio_chip tegra_gpio_chip = {
 	.label			= "tegra-gpio",
+	.request		= tegra_gpio_request,
+	.free			= tegra_gpio_free,
 	.direction_input	= tegra_gpio_direction_input,
 	.get			= tegra_gpio_get,
 	.direction_output	= tegra_gpio_direction_output,
 	.set			= tegra_gpio_set,
+	.set_debounce		= tegra_gpio_set_debounce,
+	.to_irq			= tegra_gpio_to_irq,
 	.base			= 0,
 	.ngpio			= TEGRA_NR_GPIOS,
 };
@@ -151,6 +246,15 @@ static void tegra_gpio_irq_ack(struct irq_data *d)
 	int gpio = d->irq - INT_GPIO_BASE;
 
 	__raw_writel(1 << GPIO_BIT(gpio), GPIO_INT_CLR(gpio));
+
+#ifdef CONFIG_TEGRA_FPGA_PLATFORM
+	/* FPGA platforms have a serializer between the GPIO
+	   block and interrupt controller. Allow time for
+	   clearing of the GPIO interrupt to propagate to the
+	   interrupt controller before re-enabling the IRQ
+	   to prevent double interrupts. */
+	udelay(15);
+#endif
 }
 
 static void tegra_gpio_irq_mask(struct irq_data *d)
@@ -210,11 +314,16 @@ static int tegra_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 
 	spin_unlock_irqrestore(&bank->lvl_lock[port], flags);
 
+	tegra_gpio_mask_write(GPIO_MSK_OE(gpio), gpio, 0);
+	tegra_gpio_enable(gpio);
+
 	if (type & (IRQ_TYPE_LEVEL_LOW | IRQ_TYPE_LEVEL_HIGH))
 		__irq_set_handler_locked(d->irq, handle_level_irq);
 	else if (type & (IRQ_TYPE_EDGE_FALLING | IRQ_TYPE_EDGE_RISING))
 		__irq_set_handler_locked(d->irq, handle_edge_irq);
 
+	tegra_pm_irq_set_wake_type(d->irq, type);
+
 	return 0;
 }
 
@@ -223,7 +332,6 @@ static void tegra_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 	struct tegra_gpio_bank *bank;
 	int port;
 	int pin;
-	int unmasked = 0;
 	struct irq_chip *chip = irq_desc_get_chip(desc);
 
 	chained_irq_enter(chip, desc);
@@ -234,31 +342,17 @@ static void tegra_gpio_irq_handler(unsigned int irq, struct irq_desc *desc)
 		int gpio = tegra_gpio_compose(bank->bank, port, 0);
 		unsigned long sta = __raw_readl(GPIO_INT_STA(gpio)) &
 			__raw_readl(GPIO_INT_ENB(gpio));
-		u32 lvl = __raw_readl(GPIO_INT_LVL(gpio));
-
-		for_each_set_bit(pin, &sta, 8) {
-			__raw_writel(1 << pin, GPIO_INT_CLR(gpio));
-
-			/* if gpio is edge triggered, clear condition
-			 * before executing the hander so that we don't
-			 * miss edges
-			 */
-			if (lvl & (0x100 << pin)) {
-				unmasked = 1;
-				chained_irq_exit(chip, desc);
-			}
 
+		for_each_set_bit(pin, &sta, 8)
 			generic_handle_irq(gpio_to_irq(gpio + pin));
-		}
 	}
 
-	if (!unmasked)
-		chained_irq_exit(chip, desc);
+	chained_irq_exit(chip, desc);
 
 }
 
-#ifdef CONFIG_PM
-void tegra_gpio_resume(void)
+#ifdef CONFIG_PM_SLEEP
+static void tegra_gpio_resume(void)
 {
 	unsigned long flags;
 	int b;
@@ -282,7 +376,7 @@ void tegra_gpio_resume(void)
 	local_irq_restore(flags);
 }
 
-void tegra_gpio_suspend(void)
+static int tegra_gpio_suspend(void)
 {
 	unsigned long flags;
 	int b;
@@ -299,27 +393,98 @@ void tegra_gpio_suspend(void)
 			bank->oe[p] = __raw_readl(GPIO_OE(gpio));
 			bank->int_enb[p] = __raw_readl(GPIO_INT_ENB(gpio));
 			bank->int_lvl[p] = __raw_readl(GPIO_INT_LVL(gpio));
+
+			/* disable gpio interrupts that are not wake sources */
+			__raw_writel(bank->wake_enb[p], GPIO_INT_ENB(gpio));
 		}
 	}
 	local_irq_restore(flags);
+
+	return 0;
+}
+
+static int tegra_update_lp1_gpio_wake(struct irq_data *d, bool enable)
+{
+#ifdef CONFIG_PM_SLEEP
+	struct tegra_gpio_bank *bank = irq_data_get_irq_chip_data(d);
+	u8 mask;
+	u8 port_index;
+	u8 pin_index_in_bank;
+	u8 pin_in_port;
+	int gpio = d->irq - INT_GPIO_BASE;
+
+	if (gpio < 0)
+		return -EIO;
+	pin_index_in_bank = (gpio & 0x1F);
+	port_index = pin_index_in_bank >> 3;
+	pin_in_port = (pin_index_in_bank & 0x7);
+	mask = BIT(pin_in_port);
+	if (enable)
+		bank->wake_enb[port_index] |= mask;
+	else
+		bank->wake_enb[port_index] &= ~mask;
+#endif
+
+	return 0;
 }
 
-static int tegra_gpio_wake_enable(struct irq_data *d, unsigned int enable)
+static int tegra_gpio_irq_set_wake(struct irq_data *d, unsigned int enable)
 {
 	struct tegra_gpio_bank *bank = irq_data_get_irq_chip_data(d);
-	return irq_set_irq_wake(bank->irq, enable);
+	int ret = 0;
+
+	/*
+	 * update LP1 mask for gpio port/pin interrupt
+	 * LP1 enable independent of LP0 wake support
+	 */
+	ret = tegra_update_lp1_gpio_wake(d, enable);
+	if (ret) {
+		pr_err("Failed gpio lp1 %s for irq=%d, error=%d\n",
+		(enable ? "enable" : "disable"), d->irq, ret);
+		goto fail;
+	}
+
+	/* LP1 enable for bank interrupt */
+	ret = tegra_update_lp1_irq_wake(bank->irq, enable);
+	if (ret)
+		pr_err("Failed gpio lp1 %s for irq=%d, error=%d\n",
+		(enable ? "enable" : "disable"), bank->irq, ret);
+
+	/* LP0 */
+	ret = tegra_pm_irq_set_wake(d->irq, enable);
+	if (ret)
+		pr_err("Failed gpio lp0 %s for irq=%d, error=%d\n",
+		(enable ? "enable" : "disable"), d->irq, ret);
+
+fail:
+	return ret;
 }
+#else
+#define tegra_gpio_irq_set_wake NULL
+#define tegra_gpio_suspend NULL
+#define tegra_gpio_resume NULL
 #endif
 
+static struct syscore_ops tegra_gpio_syscore_ops = {
+	.suspend = tegra_gpio_suspend,
+	.resume = tegra_gpio_resume,
+};
+
+int tegra_gpio_resume_init(void)
+{
+	register_syscore_ops(&tegra_gpio_syscore_ops);
+
+	return 0;
+}
+
 static struct irq_chip tegra_gpio_irq_chip = {
 	.name		= "GPIO",
 	.irq_ack	= tegra_gpio_irq_ack,
 	.irq_mask	= tegra_gpio_irq_mask,
 	.irq_unmask	= tegra_gpio_irq_unmask,
 	.irq_set_type	= tegra_gpio_irq_set_type,
-#ifdef CONFIG_PM
-	.irq_set_wake	= tegra_gpio_wake_enable,
-#endif
+	.irq_set_wake	= tegra_gpio_irq_set_wake,
+	.flags		= IRQCHIP_MASK_ON_SUSPEND,
 };
 
 
@@ -331,13 +496,15 @@ static struct lock_class_key gpio_lock_class;
 static int __init tegra_gpio_init(void)
 {
 	struct tegra_gpio_bank *bank;
+	int gpio;
 	int i;
 	int j;
 
-	for (i = 0; i < 7; i++) {
+	for (i = 0; i < ARRAY_SIZE(tegra_gpio_banks); i++) {
 		for (j = 0; j < 4; j++) {
 			int gpio = tegra_gpio_compose(i, j, 0);
 			__raw_writel(0x00, GPIO_INT_ENB(gpio));
+			__raw_writel(0x00, GPIO_INT_STA(gpio));
 		}
 	}
 
@@ -352,24 +519,28 @@ static int __init tegra_gpio_init(void)
 
 	gpiochip_add(&tegra_gpio_chip);
 
-	for (i = INT_GPIO_BASE; i < (INT_GPIO_BASE + TEGRA_NR_GPIOS); i++) {
-		bank = &tegra_gpio_banks[GPIO_BANK(irq_to_gpio(i))];
+	for (gpio = 0; gpio < TEGRA_NR_GPIOS; gpio++) {
+		int irq = TEGRA_GPIO_TO_IRQ(gpio);
+		/* No validity check; all Tegra GPIOs are valid IRQs */
+
+		bank = &tegra_gpio_banks[GPIO_BANK(gpio)];
 
-		irq_set_lockdep_class(i, &gpio_lock_class);
-		irq_set_chip_data(i, bank);
-		irq_set_chip_and_handler(i, &tegra_gpio_irq_chip,
+		irq_set_lockdep_class(irq, &gpio_lock_class);
+		irq_set_chip_data(irq, bank);
+		irq_set_chip_and_handler(irq, &tegra_gpio_irq_chip,
 					 handle_simple_irq);
-		set_irq_flags(i, IRQF_VALID);
+		set_irq_flags(irq, IRQF_VALID);
 	}
 
 	for (i = 0; i < ARRAY_SIZE(tegra_gpio_banks); i++) {
 		bank = &tegra_gpio_banks[i];
 
-		irq_set_chained_handler(bank->irq, tegra_gpio_irq_handler);
-		irq_set_handler_data(bank->irq, bank);
-
 		for (j = 0; j < 4; j++)
 			spin_lock_init(&bank->lvl_lock[j]);
+
+		irq_set_handler_data(bank->irq, bank);
+		irq_set_chained_handler(bank->irq, tegra_gpio_irq_handler);
+
 	}
 
 	return 0;
@@ -401,7 +572,8 @@ static int dbg_gpio_show(struct seq_file *s, void *unused)
 	int i;
 	int j;
 
-	for (i = 0; i < 7; i++) {
+	seq_printf(s, "Bank:Port CNF OE OUT IN INT_STA INT_ENB INT_LVL\n");
+	for (i = 0; i < ARRAY_SIZE(tegra_gpio_banks); i++) {
 		for (j = 0; j < 4; j++) {
 			int gpio = tegra_gpio_compose(i, j, 0);
 			seq_printf(s,
diff --git a/drivers/gpio/gpio-tps65910.c b/drivers/gpio/gpio-tps65910.c
index b9c1c297669e..af6dc837ffca 100644
--- a/drivers/gpio/gpio-tps65910.c
+++ b/drivers/gpio/gpio-tps65910.c
@@ -18,14 +18,26 @@
 #include <linux/errno.h>
 #include <linux/gpio.h>
 #include <linux/i2c.h>
+#include <linux/platform_device.h>
 #include <linux/mfd/tps65910.h>
 
+struct tps65910_gpio {
+	struct gpio_chip gpio_chip;
+	struct tps65910 *tps65910;
+};
+
+static inline struct tps65910_gpio *to_tps65910_gpio(struct gpio_chip *chip)
+{
+	return container_of(chip, struct tps65910_gpio, gpio_chip);
+}
+
 static int tps65910_gpio_get(struct gpio_chip *gc, unsigned offset)
 {
-	struct tps65910 *tps65910 = container_of(gc, struct tps65910, gpio);
-	uint8_t val;
+	struct tps65910_gpio *tps65910_gpio = to_tps65910_gpio(gc);
+	struct tps65910 *tps65910 = tps65910_gpio->tps65910;
+	unsigned int val;
 
-	tps65910->read(tps65910, TPS65910_GPIO0 + offset, 1, &val);
+	tps65910_reg_read(tps65910, TPS65910_GPIO0 + offset, &val);
 
 	if (val & GPIO_STS_MASK)
 		return 1;
@@ -36,67 +48,135 @@ static int tps65910_gpio_get(struct gpio_chip *gc, unsigned offset)
 static void tps65910_gpio_set(struct gpio_chip *gc, unsigned offset,
 			      int value)
 {
-	struct tps65910 *tps65910 = container_of(gc, struct tps65910, gpio);
+	struct tps65910_gpio *tps65910_gpio = to_tps65910_gpio(gc);
+	struct tps65910 *tps65910 = tps65910_gpio->tps65910;
 
 	if (value)
-		tps65910_set_bits(tps65910, TPS65910_GPIO0 + offset,
+		tps65910_reg_set_bits(tps65910, TPS65910_GPIO0 + offset,
 						GPIO_SET_MASK);
 	else
-		tps65910_clear_bits(tps65910, TPS65910_GPIO0 + offset,
+		tps65910_reg_clear_bits(tps65910, TPS65910_GPIO0 + offset,
 						GPIO_SET_MASK);
 }
 
 static int tps65910_gpio_output(struct gpio_chip *gc, unsigned offset,
 				int value)
 {
-	struct tps65910 *tps65910 = container_of(gc, struct tps65910, gpio);
+	struct tps65910_gpio *tps65910_gpio = to_tps65910_gpio(gc);
+	struct tps65910 *tps65910 = tps65910_gpio->tps65910;
 
 	/* Set the initial value */
-	tps65910_gpio_set(gc, 0, value);
+	tps65910_gpio_set(gc, offset, value);
 
-	return tps65910_set_bits(tps65910, TPS65910_GPIO0 + offset,
+	return tps65910_reg_set_bits(tps65910, TPS65910_GPIO0 + offset,
 						GPIO_CFG_MASK);
 }
 
 static int tps65910_gpio_input(struct gpio_chip *gc, unsigned offset)
 {
-	struct tps65910 *tps65910 = container_of(gc, struct tps65910, gpio);
+	struct tps65910_gpio *tps65910_gpio = to_tps65910_gpio(gc);
+	struct tps65910 *tps65910 = tps65910_gpio->tps65910;
 
-	return tps65910_clear_bits(tps65910, TPS65910_GPIO0 + offset,
+	return tps65910_reg_clear_bits(tps65910, TPS65910_GPIO0 + offset,
 						GPIO_CFG_MASK);
 }
 
-void tps65910_gpio_init(struct tps65910 *tps65910, int gpio_base)
+static int __devinit tps65910_gpio_probe(struct platform_device *pdev)
 {
+	struct tps65910 *tps65910 = dev_get_drvdata(pdev->dev.parent);
+	struct tps65910_board *pdata = dev_get_platdata(tps65910->dev);
+	struct tps65910_gpio *tps65910_gpio;
 	int ret;
+	int i;
+
+	tps65910_gpio = devm_kzalloc(&pdev->dev,
+				sizeof(*tps65910_gpio), GFP_KERNEL);
+	if (!tps65910_gpio) {
+		dev_err(&pdev->dev, "Could not allocate tps65910_gpio\n");
+		return -ENOMEM;
+	}
 
-	if (!gpio_base)
-		return;
+	tps65910_gpio->tps65910 = tps65910;
 
-	tps65910->gpio.owner		= THIS_MODULE;
-	tps65910->gpio.label		= tps65910->i2c_client->name;
-	tps65910->gpio.dev		= tps65910->dev;
-	tps65910->gpio.base		= gpio_base;
+	tps65910_gpio->gpio_chip.owner = THIS_MODULE;
+	tps65910_gpio->gpio_chip.label = tps65910->i2c_client->name;
 
 	switch(tps65910_chip_id(tps65910)) {
 	case TPS65910:
-		tps65910->gpio.ngpio	= 6;
+		tps65910_gpio->gpio_chip.ngpio = TPS65910_NUM_GPIO;
 		break;
 	case TPS65911:
-		tps65910->gpio.ngpio	= 9;
+		tps65910_gpio->gpio_chip.ngpio = TPS65911_NUM_GPIO;
 		break;
 	default:
-		return;
+		return -EINVAL;
+	}
+	tps65910_gpio->gpio_chip.can_sleep = 1;
+	tps65910_gpio->gpio_chip.direction_input = tps65910_gpio_input;
+	tps65910_gpio->gpio_chip.direction_output = tps65910_gpio_output;
+	tps65910_gpio->gpio_chip.set	= tps65910_gpio_set;
+	tps65910_gpio->gpio_chip.get	= tps65910_gpio_get;
+	tps65910_gpio->gpio_chip.dev = &pdev->dev;
+	if (pdata && pdata->gpio_base)
+		tps65910_gpio->gpio_chip.base = pdata->gpio_base;
+	else
+		tps65910_gpio->gpio_chip.base = -1;
+
+	if (!pdata)
+		goto skip_init;
+
+	/* Configure sleep control for gpios if provided */
+	for (i = 0; i < tps65910_gpio->gpio_chip.ngpio; ++i) {
+		if (!pdata->en_gpio_sleep[i])
+			continue;
+
+		ret = tps65910_reg_set_bits(tps65910,
+			TPS65910_GPIO0 + i, GPIO_SLEEP_MASK);
+		if (ret < 0)
+			dev_warn(tps65910->dev,
+				"GPIO Sleep setting failed with err %d\n", ret);
+	}
+
+skip_init:
+	ret = gpiochip_add(&tps65910_gpio->gpio_chip);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Could not register gpiochip, %d\n", ret);
+		return ret;
 	}
-	tps65910->gpio.can_sleep	= 1;
 
-	tps65910->gpio.direction_input	= tps65910_gpio_input;
-	tps65910->gpio.direction_output	= tps65910_gpio_output;
-	tps65910->gpio.set		= tps65910_gpio_set;
-	tps65910->gpio.get		= tps65910_gpio_get;
+	platform_set_drvdata(pdev, tps65910_gpio);
+
+	return ret;
+}
+
+static int __devexit tps65910_gpio_remove(struct platform_device *pdev)
+{
+	struct tps65910_gpio *tps65910_gpio = platform_get_drvdata(pdev);
+
+	return gpiochip_remove(&tps65910_gpio->gpio_chip);
+}
+
+static struct platform_driver tps65910_gpio_driver = {
+	.driver.name    = "tps65910-gpio",
+	.driver.owner   = THIS_MODULE,
+	.probe		= tps65910_gpio_probe,
+	.remove		= __devexit_p(tps65910_gpio_remove),
+};
 
-	ret = gpiochip_add(&tps65910->gpio);
+static int __init tps65910_gpio_init(void)
+{
+	return platform_driver_register(&tps65910_gpio_driver);
+}
+subsys_initcall(tps65910_gpio_init);
 
-	if (ret)
-		dev_warn(tps65910->dev, "GPIO registration failed: %d\n", ret);
+static void __exit tps65910_gpio_exit(void)
+{
+	platform_driver_unregister(&tps65910_gpio_driver);
 }
+module_exit(tps65910_gpio_exit);
+
+MODULE_AUTHOR("Graeme Gregory <gg@slimlogic.co.uk>");
+MODULE_AUTHOR("Jorge Eduardo Candelaria jedu@slimlogic.co.uk>");
+MODULE_DESCRIPTION("GPIO interface for TPS65910/TPS6511 PMICs");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:tps65910-gpio");
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index a971e3d043ba..a0fa0b25d218 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -58,6 +58,8 @@ struct gpio_desc {
 #define FLAG_TRIG_FALL	5	/* trigger on falling edge */
 #define FLAG_TRIG_RISE	6	/* trigger on rising edge */
 #define FLAG_ACTIVE_LOW	7	/* sysfs value has active low */
+#define FLAG_OPEN_DRAIN	8	/* Gpio is open drain type */
+#define FLAG_OPEN_SOURCE 9	/* Gpio is open source type */
 
 #define ID_SHIFT	16	/* add new flags before this one */
 
@@ -1257,6 +1259,8 @@ void gpio_free(unsigned gpio)
 		module_put(desc->chip->owner);
 		clear_bit(FLAG_ACTIVE_LOW, &desc->flags);
 		clear_bit(FLAG_REQUESTED, &desc->flags);
+		clear_bit(FLAG_OPEN_DRAIN, &desc->flags);
+		clear_bit(FLAG_OPEN_SOURCE, &desc->flags);
 	} else
 		WARN_ON(extra_checks);
 
@@ -1278,6 +1282,12 @@ int gpio_request_one(unsigned gpio, unsigned long flags, const char *label)
 	if (err)
 		return err;
 
+	if (flags & GPIOF_OPEN_DRAIN)
+		set_bit(FLAG_OPEN_DRAIN, &gpio_desc[gpio].flags);
+
+	if (flags & GPIOF_OPEN_SOURCE)
+		set_bit(FLAG_OPEN_SOURCE, &gpio_desc[gpio].flags);
+
 	if (flags & GPIOF_DIR_IN)
 		err = gpio_direction_input(gpio);
 	else
@@ -1427,6 +1437,14 @@ int gpio_direction_output(unsigned gpio, int value)
 	struct gpio_desc	*desc = &gpio_desc[gpio];
 	int			status = -EINVAL;
 
+	/* Open drain pin should not be driven to 1 */
+	if (value && test_bit(FLAG_OPEN_DRAIN,  &desc->flags))
+		return gpio_direction_input(gpio);
+
+	/* Open source pin should not be driven to 0 */
+	if (!value && test_bit(FLAG_OPEN_SOURCE,  &desc->flags))
+		return gpio_direction_input(gpio);
+
 	spin_lock_irqsave(&gpio_lock, flags);
 
 	if (!gpio_is_valid(gpio))
@@ -1563,6 +1581,57 @@ int __gpio_get_value(unsigned gpio)
 }
 EXPORT_SYMBOL_GPL(__gpio_get_value);
 
+/*
+ *  _gpio_set_open_drain_value() - Set the open drain gpio's value.
+ * @gpio: Gpio whose state need to be set.
+ * @chip: Gpio chip.
+ * @value: Non-zero for setting it HIGH otherise it will set to LOW.
+ */
+static void _gpio_set_open_drain_value(unsigned gpio,
+			struct gpio_chip *chip, int value)
+{
+	int err = 0;
+	if (value) {
+		err = chip->direction_input(chip, gpio - chip->base);
+		if (!err)
+			clear_bit(FLAG_IS_OUT, &gpio_desc[gpio].flags);
+	} else {
+		err = chip->direction_output(chip, gpio - chip->base, 0);
+		if (!err)
+			set_bit(FLAG_IS_OUT, &gpio_desc[gpio].flags);
+	}
+	trace_gpio_direction(gpio, value, err);
+	if (err < 0)
+		pr_err("%s: Error in set_value for open drain gpio%d err %d\n",
+					__func__, gpio, err);
+}
+
+/*
+ *  _gpio_set_open_source() - Set the open source gpio's value.
+ * @gpio: Gpio whose state need to be set.
+ * @chip: Gpio chip.
+ * @value: Non-zero for setting it HIGH otherise it will set to LOW.
+ */
+static void _gpio_set_open_source_value(unsigned gpio,
+			struct gpio_chip *chip, int value)
+{
+	int err = 0;
+	if (value) {
+		err = chip->direction_output(chip, gpio - chip->base, 1);
+		if (!err)
+			set_bit(FLAG_IS_OUT, &gpio_desc[gpio].flags);
+	} else {
+		err = chip->direction_input(chip, gpio - chip->base);
+		if (!err)
+			clear_bit(FLAG_IS_OUT, &gpio_desc[gpio].flags);
+	}
+	trace_gpio_direction(gpio, !value, err);
+	if (err < 0)
+		pr_err("%s: Error in set_value for open source gpio%d err %d\n",
+					__func__, gpio, err);
+}
+
+
 /**
  * __gpio_set_value() - assign a gpio's value
  * @gpio: gpio whose value will be assigned
@@ -1579,7 +1648,12 @@ void __gpio_set_value(unsigned gpio, int value)
 	chip = gpio_to_chip(gpio);
 	WARN_ON(chip->can_sleep);
 	trace_gpio_value(gpio, 0, value);
-	chip->set(chip, gpio - chip->base, value);
+	if (test_bit(FLAG_OPEN_DRAIN,  &gpio_desc[gpio].flags))
+		_gpio_set_open_drain_value(gpio, chip, value);
+	else if (test_bit(FLAG_OPEN_SOURCE,  &gpio_desc[gpio].flags))
+		_gpio_set_open_source_value(gpio, chip, value);
+	else
+		chip->set(chip, gpio - chip->base, value);
 }
 EXPORT_SYMBOL_GPL(__gpio_set_value);
 
@@ -1646,7 +1720,12 @@ void gpio_set_value_cansleep(unsigned gpio, int value)
 	might_sleep_if(extra_checks);
 	chip = gpio_to_chip(gpio);
 	trace_gpio_value(gpio, 0, value);
-	chip->set(chip, gpio - chip->base, value);
+	if (test_bit(FLAG_OPEN_DRAIN,  &gpio_desc[gpio].flags))
+		_gpio_set_open_drain_value(gpio, chip, value);
+	else if (test_bit(FLAG_OPEN_SOURCE,  &gpio_desc[gpio].flags))
+		_gpio_set_open_source_value(gpio, chip, value);
+	else
+		chip->set(chip, gpio - chip->base, value);
 }
 EXPORT_SYMBOL_GPL(gpio_set_value_cansleep);
 
diff --git a/drivers/gpu/Makefile b/drivers/gpu/Makefile
index cc9277885dd0..ca2d3b34dbf5 100644
--- a/drivers/gpu/Makefile
+++ b/drivers/gpu/Makefile
@@ -1 +1 @@
-obj-y			+= drm/ vga/ stub/
+obj-y			+= drm/ vga/ stub/ ion/
diff --git a/drivers/gpu/drm/drm_auth.c b/drivers/gpu/drm/drm_auth.c
index 3f46772f0cb2..ba23790450e9 100644
--- a/drivers/gpu/drm/drm_auth.c
+++ b/drivers/gpu/drm/drm_auth.c
@@ -101,7 +101,7 @@ static int drm_add_magic(struct drm_master *master, struct drm_file *priv,
  * Searches and unlinks the entry in drm_device::magiclist with the magic
  * number hash key, while holding the drm_device::struct_mutex lock.
  */
-static int drm_remove_magic(struct drm_master *master, drm_magic_t magic)
+int drm_remove_magic(struct drm_master *master, drm_magic_t magic)
 {
 	struct drm_magic_entry *pt;
 	struct drm_hash_item *hash;
@@ -136,6 +136,8 @@ static int drm_remove_magic(struct drm_master *master, drm_magic_t magic)
  * If there is a magic number in drm_file::magic then use it, otherwise
  * searches an unique non-zero magic number and add it associating it with \p
  * file_priv.
+ * This ioctl needs protection by the drm_global_mutex, which protects
+ * struct drm_file::magic and struct drm_magic_entry::priv.
  */
 int drm_getmagic(struct drm_device *dev, void *data, struct drm_file *file_priv)
 {
@@ -173,6 +175,8 @@ int drm_getmagic(struct drm_device *dev, void *data, struct drm_file *file_priv)
  * \return zero if authentication successed, or a negative number otherwise.
  *
  * Checks if \p file_priv is associated with the magic number passed in \arg.
+ * This ioctl needs protection by the drm_global_mutex, which protects
+ * struct drm_file::magic and struct drm_magic_entry::priv.
  */
 int drm_authmagic(struct drm_device *dev, void *data,
 		  struct drm_file *file_priv)
diff --git a/drivers/gpu/drm/drm_crtc.c b/drivers/gpu/drm/drm_crtc.c
index fe738f05309b..2410c4078f3a 100644
--- a/drivers/gpu/drm/drm_crtc.c
+++ b/drivers/gpu/drm/drm_crtc.c
@@ -1868,6 +1868,10 @@ int drm_mode_dirtyfb_ioctl(struct drm_device *dev,
 	}
 
 	if (num_clips && clips_ptr) {
+		if (num_clips < 0 || num_clips > DRM_MODE_FB_DIRTY_MAX_CLIPS) {
+			ret = -EINVAL;
+			goto out_err1;
+		}
 		clips = kzalloc(num_clips * sizeof(*clips), GFP_KERNEL);
 		if (!clips) {
 			ret = -ENOMEM;
diff --git a/drivers/gpu/drm/drm_fops.c b/drivers/gpu/drm/drm_fops.c
index 2ec7d48fc4a8..c42e12cc2ddb 100644
--- a/drivers/gpu/drm/drm_fops.c
+++ b/drivers/gpu/drm/drm_fops.c
@@ -486,6 +486,11 @@ int drm_release(struct inode *inode, struct file *filp)
 		  (long)old_encode_dev(file_priv->minor->device),
 		  dev->open_count);
 
+	/* Release any auth tokens that might point to this file_priv,
+	   (do that under the drm_global_mutex) */
+	if (file_priv->magic)
+		(void) drm_remove_magic(file_priv->master, file_priv->magic);
+
 	/* if the master has gone away we can't do anything with the lock */
 	if (file_priv->minor->master)
 		drm_master_release(dev, filp);
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index 8a3942c4f099..c72b590f7d81 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -1453,6 +1453,14 @@ unsigned long i915_chipset_val(struct drm_i915_private *dev_priv)
 
 	diff1 = now - dev_priv->last_time1;
 
+	/* Prevent division-by-zero if we are asking too fast.
+	 * Also, we don't get interesting results if we are polling
+	 * faster than once in 10ms, so just return the saved value
+	 * in such cases.
+	 */
+	if (diff1 <= 10)
+		return dev_priv->chipset_power;
+
 	count1 = I915_READ(DMIEC);
 	count2 = I915_READ(DDREC);
 	count3 = I915_READ(CSIEC);
@@ -1483,6 +1491,8 @@ unsigned long i915_chipset_val(struct drm_i915_private *dev_priv)
 	dev_priv->last_count1 = total_count;
 	dev_priv->last_time1 = now;
 
+	dev_priv->chipset_power = ret;
+
 	return ret;
 }
 
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 7916bd97d5c1..1a2a2d1790b6 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -707,6 +707,7 @@ typedef struct drm_i915_private {
 
 	u64 last_count1;
 	unsigned long last_time1;
+	unsigned long chipset_power;
 	u64 last_count2;
 	struct timespec last_time2;
 	unsigned long gfx_power;
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index a546a71fb060..346d5574f0af 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -1475,7 +1475,7 @@ i915_gem_mmap_gtt(struct drm_file *file,
 
 	if (obj->base.size > dev_priv->mm.gtt_mappable_end) {
 		ret = -E2BIG;
-		goto unlock;
+		goto out;
 	}
 
 	if (obj->madv != I915_MADV_WILLNEED) {
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index 9cbb0cd8f46a..73248d0ea170 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -822,6 +822,7 @@ static void i915_gem_record_fences(struct drm_device *dev,
 
 	/* Fences */
 	switch (INTEL_INFO(dev)->gen) {
+	case 7:
 	case 6:
 		for (i = 0; i < 16; i++)
 			error->fence[i] = I915_READ64(FENCE_REG_SANDYBRIDGE_0 + (i * 8));
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index 542453f7498c..2ae29de4172c 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -2554,10 +2554,18 @@
 #define _CURBBASE		0x700c4
 #define _CURBPOS			0x700c8
 
+#define _CURBCNTR_IVB		0x71080
+#define _CURBBASE_IVB		0x71084
+#define _CURBPOS_IVB		0x71088
+
 #define CURCNTR(pipe) _PIPE(pipe, _CURACNTR, _CURBCNTR)
 #define CURBASE(pipe) _PIPE(pipe, _CURABASE, _CURBBASE)
 #define CURPOS(pipe) _PIPE(pipe, _CURAPOS, _CURBPOS)
 
+#define CURCNTR_IVB(pipe) _PIPE(pipe, _CURACNTR, _CURBCNTR_IVB)
+#define CURBASE_IVB(pipe) _PIPE(pipe, _CURABASE, _CURBBASE_IVB)
+#define CURPOS_IVB(pipe) _PIPE(pipe, _CURAPOS, _CURBPOS_IVB)
+
 /* Display A control */
 #define _DSPACNTR                0x70180
 #define   DISPLAY_PLANE_ENABLE			(1<<31)
@@ -3167,6 +3175,7 @@
 #define  FDI_LINK_TRAIN_NONE_IVB            (3<<8)
 
 /* both Tx and Rx */
+#define  FDI_COMPOSITE_SYNC		(1<<11)
 #define  FDI_LINK_TRAIN_AUTO		(1<<10)
 #define  FDI_SCRAMBLING_ENABLE          (0<<7)
 #define  FDI_SCRAMBLING_DISABLE         (1<<7)
@@ -3262,10 +3271,10 @@
 /* or SDVOB */
 #define HDMIB   0xe1140
 #define  PORT_ENABLE    (1 << 31)
-#define  TRANSCODER_A   (0)
-#define  TRANSCODER_B   (1 << 30)
-#define  TRANSCODER(pipe)	((pipe) << 30)
-#define  TRANSCODER_MASK   (1 << 30)
+#define  TRANSCODER(pipe)       ((pipe) << 30)
+#define  TRANSCODER_CPT(pipe)   ((pipe) << 29)
+#define  TRANSCODER_MASK        (1 << 30)
+#define  TRANSCODER_MASK_CPT    (3 << 29)
 #define  COLOR_FORMAT_8bpc      (0)
 #define  COLOR_FORMAT_12bpc     (3 << 26)
 #define  SDVOB_HOTPLUG_ENABLE   (1 << 23)
@@ -3392,6 +3401,10 @@
 #define  GT_FIFO_FREE_ENTRIES			0x120008
 #define    GT_FIFO_NUM_RESERVED_ENTRIES		20
 
+#define GEN6_UCGCTL2				0x9404
+# define GEN6_RCPBUNIT_CLOCK_GATE_DISABLE		(1 << 12)
+# define GEN6_RCCUNIT_CLOCK_GATE_DISABLE		(1 << 11)
+
 #define GEN6_RPNSWREQ				0xA008
 #define   GEN6_TURBO_DISABLE			(1<<31)
 #define   GEN6_FREQUENCY(x)			((x)<<25)
diff --git a/drivers/gpu/drm/i915/i915_suspend.c b/drivers/gpu/drm/i915/i915_suspend.c
index f10742359ec9..9c7706a4c71c 100644
--- a/drivers/gpu/drm/i915/i915_suspend.c
+++ b/drivers/gpu/drm/i915/i915_suspend.c
@@ -370,6 +370,7 @@ static void i915_save_modeset_reg(struct drm_device *dev)
 
 	/* Fences */
 	switch (INTEL_INFO(dev)->gen) {
+	case 7:
 	case 6:
 		for (i = 0; i < 16; i++)
 			dev_priv->saveFENCE[i] = I915_READ64(FENCE_REG_SANDYBRIDGE_0 + (i * 8));
@@ -404,6 +405,7 @@ static void i915_restore_modeset_reg(struct drm_device *dev)
 
 	/* Fences */
 	switch (INTEL_INFO(dev)->gen) {
+	case 7:
 	case 6:
 		for (i = 0; i < 16; i++)
 			I915_WRITE64(FENCE_REG_SANDYBRIDGE_0 + (i * 8), dev_priv->saveFENCE[i]);
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 04411ad2e779..07e7cf38068a 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -2600,6 +2600,7 @@ static void ivb_manual_fdi_link_train(struct drm_crtc *crtc)
 	temp |= FDI_LINK_TRAIN_PATTERN_1_IVB;
 	temp &= ~FDI_LINK_TRAIN_VOL_EMP_MASK;
 	temp |= FDI_LINK_TRAIN_400MV_0DB_SNB_B;
+	temp |= FDI_COMPOSITE_SYNC;
 	I915_WRITE(reg, temp | FDI_TX_ENABLE);
 
 	reg = FDI_RX_CTL(pipe);
@@ -2607,6 +2608,7 @@ static void ivb_manual_fdi_link_train(struct drm_crtc *crtc)
 	temp &= ~FDI_LINK_TRAIN_AUTO;
 	temp &= ~FDI_LINK_TRAIN_PATTERN_MASK_CPT;
 	temp |= FDI_LINK_TRAIN_PATTERN_1_CPT;
+	temp |= FDI_COMPOSITE_SYNC;
 	I915_WRITE(reg, temp | FDI_RX_ENABLE);
 
 	POSTING_READ(reg);
@@ -5758,6 +5760,31 @@ static void i9xx_update_cursor(struct drm_crtc *crtc, u32 base)
 	I915_WRITE(CURBASE(pipe), base);
 }
 
+static void ivb_update_cursor(struct drm_crtc *crtc, u32 base)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int pipe = intel_crtc->pipe;
+	bool visible = base != 0;
+
+	if (intel_crtc->cursor_visible != visible) {
+		uint32_t cntl = I915_READ(CURCNTR_IVB(pipe));
+		if (base) {
+			cntl &= ~CURSOR_MODE;
+			cntl |= CURSOR_MODE_64_ARGB_AX | MCURSOR_GAMMA_ENABLE;
+		} else {
+			cntl &= ~(CURSOR_MODE | MCURSOR_GAMMA_ENABLE);
+			cntl |= CURSOR_MODE_DISABLE;
+		}
+		I915_WRITE(CURCNTR_IVB(pipe), cntl);
+
+		intel_crtc->cursor_visible = visible;
+	}
+	/* and commit changes on next vblank */
+	I915_WRITE(CURBASE_IVB(pipe), base);
+}
+
 /* If no-part of the cursor is visible on the framebuffer, then the GPU may hang... */
 static void intel_crtc_update_cursor(struct drm_crtc *crtc,
 				     bool on)
@@ -5805,11 +5832,16 @@ static void intel_crtc_update_cursor(struct drm_crtc *crtc,
 	if (!visible && !intel_crtc->cursor_visible)
 		return;
 
-	I915_WRITE(CURPOS(pipe), pos);
-	if (IS_845G(dev) || IS_I865G(dev))
-		i845_update_cursor(crtc, base);
-	else
-		i9xx_update_cursor(crtc, base);
+	if (IS_IVYBRIDGE(dev)) {
+		I915_WRITE(CURPOS_IVB(pipe), pos);
+		ivb_update_cursor(crtc, base);
+	} else {
+		I915_WRITE(CURPOS(pipe), pos);
+		if (IS_845G(dev) || IS_I865G(dev))
+			i845_update_cursor(crtc, base);
+		else
+			i9xx_update_cursor(crtc, base);
+	}
 
 	if (visible)
 		intel_mark_busy(dev, to_intel_framebuffer(crtc->fb)->obj);
@@ -7850,6 +7882,20 @@ static void gen6_init_clock_gating(struct drm_device *dev)
 	I915_WRITE(WM2_LP_ILK, 0);
 	I915_WRITE(WM1_LP_ILK, 0);
 
+	/* According to the BSpec vol1g, bit 12 (RCPBUNIT) clock
+	 * gating disable must be set.  Failure to set it results in
+	 * flickering pixels due to Z write ordering failures after
+	 * some amount of runtime in the Mesa "fire" demo, and Unigine
+	 * Sanctuary and Tropics, and apparently anything else with
+	 * alpha test or pixel discard.
+	 *
+	 * According to the spec, bit 11 (RCCUNIT) must also be set,
+	 * but we didn't debug actual testcases to find it out.
+	 */
+	I915_WRITE(GEN6_UCGCTL2,
+		   GEN6_RCPBUNIT_CLOCK_GATE_DISABLE |
+		   GEN6_RCCUNIT_CLOCK_GATE_DISABLE);
+
 	/*
 	 * According to the spec the following bits should be
 	 * set in order to enable memory self-refresh and fbc:
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index 44fef5e1c490..7c64db48c1c7 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -1683,6 +1683,31 @@ g4x_dp_detect(struct intel_dp *intel_dp)
 	return intel_dp_detect_dpcd(intel_dp);
 }
 
+static struct edid *
+intel_dp_get_edid(struct drm_connector *connector, struct i2c_adapter *adapter)
+{
+	struct intel_dp *intel_dp = intel_attached_dp(connector);
+	struct edid	*edid;
+
+	ironlake_edp_panel_vdd_on(intel_dp);
+	edid = drm_get_edid(connector, adapter);
+	ironlake_edp_panel_vdd_off(intel_dp);
+	return edid;
+}
+
+static int
+intel_dp_get_edid_modes(struct drm_connector *connector, struct i2c_adapter *adapter)
+{
+	struct intel_dp *intel_dp = intel_attached_dp(connector);
+	int	ret;
+
+	ironlake_edp_panel_vdd_on(intel_dp);
+	ret = intel_ddc_get_modes(connector, adapter);
+	ironlake_edp_panel_vdd_off(intel_dp);
+	return ret;
+}
+
+
 /**
  * Uses CRT_HOTPLUG_EN and CRT_HOTPLUG_STAT to detect DP connection.
  *
@@ -1715,7 +1740,7 @@ intel_dp_detect(struct drm_connector *connector, bool force)
 	if (intel_dp->force_audio) {
 		intel_dp->has_audio = intel_dp->force_audio > 0;
 	} else {
-		edid = drm_get_edid(connector, &intel_dp->adapter);
+		edid = intel_dp_get_edid(connector, &intel_dp->adapter);
 		if (edid) {
 			intel_dp->has_audio = drm_detect_monitor_audio(edid);
 			connector->display_info.raw_edid = NULL;
@@ -1736,7 +1761,7 @@ static int intel_dp_get_modes(struct drm_connector *connector)
 	/* We should parse the EDID data and find out if it has an audio sink
 	 */
 
-	ret = intel_ddc_get_modes(connector, &intel_dp->adapter);
+	ret = intel_dp_get_edid_modes(connector, &intel_dp->adapter);
 	if (ret) {
 		if (is_edp(intel_dp) && !dev_priv->panel_fixed_mode) {
 			struct drm_display_mode *newmode;
@@ -1772,7 +1797,7 @@ intel_dp_detect_audio(struct drm_connector *connector)
 	struct edid *edid;
 	bool has_audio = false;
 
-	edid = drm_get_edid(connector, &intel_dp->adapter);
+	edid = intel_dp_get_edid(connector, &intel_dp->adapter);
 	if (edid) {
 		has_audio = drm_detect_monitor_audio(edid);
 
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index fe1099d8817e..2480cfa7c0ca 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -184,7 +184,7 @@ struct intel_crtc {
 #define DIP_VERSION_AVI 0x2
 #define DIP_LEN_AVI     13
 
-#define DIP_TYPE_SPD	0x3
+#define DIP_TYPE_SPD	0x83
 #define DIP_VERSION_SPD	0x1
 #define DIP_LEN_SPD	25
 #define DIP_SPD_UNKNOWN	0
diff --git a/drivers/gpu/drm/i915/intel_panel.c b/drivers/gpu/drm/i915/intel_panel.c
index a9e0c7bcd317..007f6ca309d3 100644
--- a/drivers/gpu/drm/i915/intel_panel.c
+++ b/drivers/gpu/drm/i915/intel_panel.c
@@ -226,7 +226,7 @@ static void intel_pch_panel_set_backlight(struct drm_device *dev, u32 level)
 	I915_WRITE(BLC_PWM_CPU_CTL, val | level);
 }
 
-void intel_panel_set_backlight(struct drm_device *dev, u32 level)
+static void intel_panel_actually_set_backlight(struct drm_device *dev, u32 level)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 tmp;
@@ -254,16 +254,21 @@ void intel_panel_set_backlight(struct drm_device *dev, u32 level)
 	I915_WRITE(BLC_PWM_CTL, tmp | level);
 }
 
-void intel_panel_disable_backlight(struct drm_device *dev)
+void intel_panel_set_backlight(struct drm_device *dev, u32 level)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	if (dev_priv->backlight_enabled) {
-		dev_priv->backlight_level = intel_panel_get_backlight(dev);
-		dev_priv->backlight_enabled = false;
-	}
+	dev_priv->backlight_level = level;
+	if (dev_priv->backlight_enabled)
+		intel_panel_actually_set_backlight(dev, level);
+}
 
-	intel_panel_set_backlight(dev, 0);
+void intel_panel_disable_backlight(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	dev_priv->backlight_enabled = false;
+	intel_panel_actually_set_backlight(dev, 0);
 }
 
 void intel_panel_enable_backlight(struct drm_device *dev)
@@ -273,8 +278,8 @@ void intel_panel_enable_backlight(struct drm_device *dev)
 	if (dev_priv->backlight_level == 0)
 		dev_priv->backlight_level = intel_panel_get_max_backlight(dev);
 
-	intel_panel_set_backlight(dev, dev_priv->backlight_level);
 	dev_priv->backlight_enabled = true;
+	intel_panel_actually_set_backlight(dev, dev_priv->backlight_level);
 }
 
 static void intel_panel_init_backlight(struct drm_device *dev)
@@ -321,7 +326,8 @@ static int intel_panel_update_status(struct backlight_device *bd)
 static int intel_panel_get_brightness(struct backlight_device *bd)
 {
 	struct drm_device *dev = bl_get_data(bd);
-	return intel_panel_get_backlight(dev);
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	return dev_priv->backlight_level;
 }
 
 static const struct backlight_ops intel_panel_bl_ops = {
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index 6348c499616f..66e47a0100c3 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -1064,15 +1064,13 @@ static void intel_sdvo_mode_set(struct drm_encoder *encoder,
 
 	/* Set the SDVO control regs. */
 	if (INTEL_INFO(dev)->gen >= 4) {
-		sdvox = 0;
+		/* The real mode polarity is set by the SDVO commands, using
+		 * struct intel_sdvo_dtd. */
+		sdvox = SDVO_VSYNC_ACTIVE_HIGH | SDVO_HSYNC_ACTIVE_HIGH;
 		if (intel_sdvo->is_hdmi)
 			sdvox |= intel_sdvo->color_range;
 		if (INTEL_INFO(dev)->gen < 5)
 			sdvox |= SDVO_BORDER_ENABLE;
-		if (adjusted_mode->flags & DRM_MODE_FLAG_PVSYNC)
-			sdvox |= SDVO_VSYNC_ACTIVE_HIGH;
-		if (adjusted_mode->flags & DRM_MODE_FLAG_PHSYNC)
-			sdvox |= SDVO_HSYNC_ACTIVE_HIGH;
 	} else {
 		sdvox = I915_READ(intel_sdvo->sdvo_reg);
 		switch (intel_sdvo->sdvo_reg) {
@@ -1085,8 +1083,12 @@ static void intel_sdvo_mode_set(struct drm_encoder *encoder,
 		}
 		sdvox |= (9 << 19) | SDVO_BORDER_ENABLE;
 	}
-	if (intel_crtc->pipe == 1)
-		sdvox |= SDVO_PIPE_B_SELECT;
+
+	if (INTEL_PCH_TYPE(dev) >= PCH_CPT)
+		sdvox |= TRANSCODER_CPT(intel_crtc->pipe);
+	else
+		sdvox |= TRANSCODER(intel_crtc->pipe);
+
 	if (intel_sdvo->has_hdmi_audio)
 		sdvox |= SDVO_AUDIO_ENABLE;
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_channel.c b/drivers/gpu/drm/nouveau/nouveau_channel.c
index b0d753f45bbd..0e3241c39b8a 100644
--- a/drivers/gpu/drm/nouveau/nouveau_channel.c
+++ b/drivers/gpu/drm/nouveau/nouveau_channel.c
@@ -158,6 +158,7 @@ nouveau_channel_alloc(struct drm_device *dev, struct nouveau_channel **chan_ret,
 	INIT_LIST_HEAD(&chan->nvsw.vbl_wait);
 	INIT_LIST_HEAD(&chan->nvsw.flip);
 	INIT_LIST_HEAD(&chan->fence.pending);
+	spin_lock_init(&chan->fence.lock);
 
 	/* setup channel's memory and vm */
 	ret = nouveau_gpuobj_channel_init(chan, vram_handle, gart_handle);
diff --git a/drivers/gpu/drm/nouveau/nouveau_fence.c b/drivers/gpu/drm/nouveau/nouveau_fence.c
index c919cfc8f2fd..ae22dfaa2a06 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fence.c
+++ b/drivers/gpu/drm/nouveau/nouveau_fence.c
@@ -539,8 +539,6 @@ nouveau_fence_channel_init(struct nouveau_channel *chan)
 			return ret;
 	}
 
-	INIT_LIST_HEAD(&chan->fence.pending);
-	spin_lock_init(&chan->fence.lock);
 	atomic_set(&chan->fence.last_sequence_irq, 0);
 	return 0;
 }
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index a515b2a09d85..9bb3d6f3b7be 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -558,7 +558,7 @@ static u32 atombios_adjust_pll(struct drm_crtc *crtc,
 				bpc = connector->display_info.bpc;
 			encoder_mode = atombios_get_encoder_mode(encoder);
 			if ((radeon_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT | ATOM_DEVICE_DFP_SUPPORT)) ||
-			    radeon_encoder_is_dp_bridge(encoder)) {
+			    (radeon_encoder_get_dp_bridge_encoder_id(encoder) != ENCODER_OBJECT_ID_NONE)) {
 				if (connector) {
 					struct radeon_connector *radeon_connector = to_radeon_connector(connector);
 					struct radeon_connector_atom_dig *dig_connector =
@@ -638,44 +638,29 @@ static u32 atombios_adjust_pll(struct drm_crtc *crtc,
 				if (ss_enabled && ss->percentage)
 					args.v3.sInput.ucDispPllConfig |=
 						DISPPLL_CONFIG_SS_ENABLE;
-				if (radeon_encoder->devices & (ATOM_DEVICE_DFP_SUPPORT) ||
-				    radeon_encoder_is_dp_bridge(encoder)) {
+				if (encoder_mode == ATOM_ENCODER_MODE_DP) {
+					args.v3.sInput.ucDispPllConfig |=
+						DISPPLL_CONFIG_COHERENT_MODE;
+					/* 16200 or 27000 */
+					args.v3.sInput.usPixelClock = cpu_to_le16(dp_clock / 10);
+				} else if (radeon_encoder->devices & (ATOM_DEVICE_DFP_SUPPORT)) {
 					struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
-					if (encoder_mode == ATOM_ENCODER_MODE_DP) {
+					if (encoder_mode == ATOM_ENCODER_MODE_HDMI)
+						/* deep color support */
+						args.v3.sInput.usPixelClock =
+							cpu_to_le16((mode->clock * bpc / 8) / 10);
+					if (dig->coherent_mode)
 						args.v3.sInput.ucDispPllConfig |=
 							DISPPLL_CONFIG_COHERENT_MODE;
-						/* 16200 or 27000 */
-						args.v3.sInput.usPixelClock = cpu_to_le16(dp_clock / 10);
-					} else {
-						if (encoder_mode == ATOM_ENCODER_MODE_HDMI) {
-							/* deep color support */
-							args.v3.sInput.usPixelClock =
-								cpu_to_le16((mode->clock * bpc / 8) / 10);
-						}
-						if (dig->coherent_mode)
-							args.v3.sInput.ucDispPllConfig |=
-								DISPPLL_CONFIG_COHERENT_MODE;
-						if (mode->clock > 165000)
-							args.v3.sInput.ucDispPllConfig |=
-								DISPPLL_CONFIG_DUAL_LINK;
-					}
-				} else if (radeon_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT)) {
-					if (encoder_mode == ATOM_ENCODER_MODE_DP) {
+					if (mode->clock > 165000)
 						args.v3.sInput.ucDispPllConfig |=
-							DISPPLL_CONFIG_COHERENT_MODE;
-						/* 16200 or 27000 */
-						args.v3.sInput.usPixelClock = cpu_to_le16(dp_clock / 10);
-					} else if (encoder_mode != ATOM_ENCODER_MODE_LVDS) {
-						if (mode->clock > 165000)
-							args.v3.sInput.ucDispPllConfig |=
-								DISPPLL_CONFIG_DUAL_LINK;
-					}
+							DISPPLL_CONFIG_DUAL_LINK;
 				}
-				if (radeon_encoder_is_dp_bridge(encoder)) {
-					struct drm_encoder *ext_encoder = radeon_atom_get_external_encoder(encoder);
-					struct radeon_encoder *ext_radeon_encoder = to_radeon_encoder(ext_encoder);
-					args.v3.sInput.ucExtTransmitterID = ext_radeon_encoder->encoder_id;
-				} else
+				if (radeon_encoder_get_dp_bridge_encoder_id(encoder) !=
+				    ENCODER_OBJECT_ID_NONE)
+					args.v3.sInput.ucExtTransmitterID =
+						radeon_encoder_get_dp_bridge_encoder_id(encoder);
+				else
 					args.v3.sInput.ucExtTransmitterID = 0;
 
 				atom_execute_table(rdev->mode_info.atom_context,
diff --git a/drivers/gpu/drm/radeon/atombios_dp.c b/drivers/gpu/drm/radeon/atombios_dp.c
index 79e8ebc05307..03a347a99d7c 100644
--- a/drivers/gpu/drm/radeon/atombios_dp.c
+++ b/drivers/gpu/drm/radeon/atombios_dp.c
@@ -283,7 +283,7 @@ int radeon_dp_i2c_aux_ch(struct i2c_adapter *adapter, int mode,
 		}
 	}
 
-	DRM_ERROR("aux i2c too many retries, giving up\n");
+	DRM_DEBUG_KMS("aux i2c too many retries, giving up\n");
 	return -EREMOTEIO;
 }
 
@@ -482,7 +482,8 @@ static int radeon_dp_get_dp_link_clock(struct drm_connector *connector,
 	int bpp = convert_bpc_to_bpp(connector->display_info.bpc);
 	int lane_num, max_pix_clock;
 
-	if (radeon_connector_encoder_is_dp_bridge(connector))
+	if (radeon_connector_encoder_get_dp_bridge_encoder_id(connector) !=
+	    ENCODER_OBJECT_ID_NONE)
 		return 270000;
 
 	lane_num = radeon_dp_get_dp_lane_number(connector, dpcd, pix_clock);
@@ -553,17 +554,32 @@ static void radeon_dp_set_panel_mode(struct drm_encoder *encoder,
 {
 	struct drm_device *dev = encoder->dev;
 	struct radeon_device *rdev = dev->dev_private;
+	struct radeon_connector *radeon_connector = to_radeon_connector(connector);
 	int panel_mode = DP_PANEL_MODE_EXTERNAL_DP_MODE;
 
 	if (!ASIC_IS_DCE4(rdev))
 		return;
 
-	if (radeon_connector_encoder_is_dp_bridge(connector))
+	if (radeon_connector_encoder_get_dp_bridge_encoder_id(connector) ==
+	    ENCODER_OBJECT_ID_NUTMEG)
 		panel_mode = DP_PANEL_MODE_INTERNAL_DP1_MODE;
+	else if (radeon_connector_encoder_get_dp_bridge_encoder_id(connector) ==
+		 ENCODER_OBJECT_ID_TRAVIS)
+		panel_mode = DP_PANEL_MODE_INTERNAL_DP2_MODE;
+	else if (connector->connector_type == DRM_MODE_CONNECTOR_eDP) {
+		u8 tmp = radeon_read_dpcd_reg(radeon_connector, DP_EDP_CONFIGURATION_CAP);
+		if (tmp & 1)
+			panel_mode = DP_PANEL_MODE_INTERNAL_DP2_MODE;
+	}
 
 	atombios_dig_encoder_setup(encoder,
 				   ATOM_ENCODER_CMD_SETUP_PANEL_MODE,
 				   panel_mode);
+
+	if ((connector->connector_type == DRM_MODE_CONNECTOR_eDP) &&
+	    (panel_mode == DP_PANEL_MODE_INTERNAL_DP2_MODE)) {
+		radeon_write_dpcd_reg(radeon_connector, DP_EDP_CONFIGURATION_SET, 1);
+	}
 }
 
 void radeon_dp_set_link_config(struct drm_connector *connector,
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index c4ffa14fb2f4..c3f0d428c51d 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -82,6 +82,7 @@ u32 evergreen_page_flip(struct radeon_device *rdev, int crtc_id, u64 crtc_base)
 {
 	struct radeon_crtc *radeon_crtc = rdev->mode_info.crtcs[crtc_id];
 	u32 tmp = RREG32(EVERGREEN_GRPH_UPDATE + radeon_crtc->crtc_offset);
+	int i;
 
 	/* Lock the graphics update lock */
 	tmp |= EVERGREEN_GRPH_UPDATE_LOCK;
@@ -99,7 +100,11 @@ u32 evergreen_page_flip(struct radeon_device *rdev, int crtc_id, u64 crtc_base)
 	       (u32)crtc_base);
 
 	/* Wait for update_pending to go high. */
-	while (!(RREG32(EVERGREEN_GRPH_UPDATE + radeon_crtc->crtc_offset) & EVERGREEN_GRPH_SURFACE_UPDATE_PENDING));
+	for (i = 0; i < rdev->usec_timeout; i++) {
+		if (RREG32(EVERGREEN_GRPH_UPDATE + radeon_crtc->crtc_offset) & EVERGREEN_GRPH_SURFACE_UPDATE_PENDING)
+			break;
+		udelay(1);
+	}
 	DRM_DEBUG("Update pending now high. Unlocking vupdate_lock.\n");
 
 	/* Unlock the lock, so double-buffering can take place inside vblank */
@@ -353,6 +358,7 @@ void evergreen_hpd_init(struct radeon_device *rdev)
 		default:
 			break;
 		}
+		radeon_hpd_set_polarity(rdev, radeon_connector->hpd.hpd);
 	}
 	if (rdev->irq.installed)
 		evergreen_irq_set(rdev);
@@ -3252,6 +3258,18 @@ int evergreen_init(struct radeon_device *rdev)
 			rdev->accel_working = false;
 		}
 	}
+
+	/* Don't start up if the MC ucode is missing on BTC parts.
+	 * The default clocks and voltages before the MC ucode
+	 * is loaded are not suffient for advanced operations.
+	 */
+	if (ASIC_IS_DCE5(rdev)) {
+		if (!rdev->mc_fw && !(rdev->flags & RADEON_IS_IGP)) {
+			DRM_ERROR("radeon: MC ucode required for NI+.\n");
+			return -EINVAL;
+		}
+	}
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/radeon/r100.c b/drivers/gpu/drm/radeon/r100.c
index 7fcdbbbf2979..764249587f14 100644
--- a/drivers/gpu/drm/radeon/r100.c
+++ b/drivers/gpu/drm/radeon/r100.c
@@ -84,13 +84,18 @@ u32 r100_page_flip(struct radeon_device *rdev, int crtc_id, u64 crtc_base)
 {
 	struct radeon_crtc *radeon_crtc = rdev->mode_info.crtcs[crtc_id];
 	u32 tmp = ((u32)crtc_base) | RADEON_CRTC_OFFSET__OFFSET_LOCK;
+	int i;
 
 	/* Lock the graphics update lock */
 	/* update the scanout addresses */
 	WREG32(RADEON_CRTC_OFFSET + radeon_crtc->crtc_offset, tmp);
 
 	/* Wait for update_pending to go high. */
-	while (!(RREG32(RADEON_CRTC_OFFSET + radeon_crtc->crtc_offset) & RADEON_CRTC_OFFSET__GUI_TRIG_OFFSET));
+	for (i = 0; i < rdev->usec_timeout; i++) {
+		if (RREG32(RADEON_CRTC_OFFSET + radeon_crtc->crtc_offset) & RADEON_CRTC_OFFSET__GUI_TRIG_OFFSET)
+			break;
+		udelay(1);
+	}
 	DRM_DEBUG("Update pending now high. Unlocking vupdate_lock.\n");
 
 	/* Unlock the lock, so double-buffering can take place inside vblank */
@@ -434,6 +439,7 @@ void r100_hpd_init(struct radeon_device *rdev)
 		default:
 			break;
 		}
+		radeon_hpd_set_polarity(rdev, radeon_connector->hpd.hpd);
 	}
 	if (rdev->irq.installed)
 		r100_irq_set(rdev);
@@ -2063,6 +2069,7 @@ bool r100_gpu_is_lockup(struct radeon_device *rdev)
 void r100_bm_disable(struct radeon_device *rdev)
 {
 	u32 tmp;
+	u16 tmp16;
 
 	/* disable bus mastering */
 	tmp = RREG32(R_000030_BUS_CNTL);
@@ -2073,8 +2080,8 @@ void r100_bm_disable(struct radeon_device *rdev)
 	WREG32(R_000030_BUS_CNTL, (tmp & 0xFFFFFFFF) | 0x00000040);
 	tmp = RREG32(RADEON_BUS_CNTL);
 	mdelay(1);
-	pci_read_config_word(rdev->pdev, 0x4, (u16*)&tmp);
-	pci_write_config_word(rdev->pdev, 0x4, tmp & 0xFFFB);
+	pci_read_config_word(rdev->pdev, 0x4, &tmp16);
+	pci_write_config_word(rdev->pdev, 0x4, tmp16 & 0xFFFB);
 	mdelay(1);
 }
 
diff --git a/drivers/gpu/drm/radeon/r600.c b/drivers/gpu/drm/radeon/r600.c
index 720dd99163f8..9b62a97742a7 100644
--- a/drivers/gpu/drm/radeon/r600.c
+++ b/drivers/gpu/drm/radeon/r600.c
@@ -762,13 +762,14 @@ void r600_hpd_init(struct radeon_device *rdev)
 	struct drm_device *dev = rdev->ddev;
 	struct drm_connector *connector;
 
-	if (ASIC_IS_DCE3(rdev)) {
-		u32 tmp = DC_HPDx_CONNECTION_TIMER(0x9c4) | DC_HPDx_RX_INT_TIMER(0xfa);
-		if (ASIC_IS_DCE32(rdev))
-			tmp |= DC_HPDx_EN;
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
+		struct radeon_connector *radeon_connector = to_radeon_connector(connector);
+
+		if (ASIC_IS_DCE3(rdev)) {
+			u32 tmp = DC_HPDx_CONNECTION_TIMER(0x9c4) | DC_HPDx_RX_INT_TIMER(0xfa);
+			if (ASIC_IS_DCE32(rdev))
+				tmp |= DC_HPDx_EN;
 
-		list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-			struct radeon_connector *radeon_connector = to_radeon_connector(connector);
 			switch (radeon_connector->hpd.hpd) {
 			case RADEON_HPD_1:
 				WREG32(DC_HPD1_CONTROL, tmp);
@@ -798,10 +799,7 @@ void r600_hpd_init(struct radeon_device *rdev)
 			default:
 				break;
 			}
-		}
-	} else {
-		list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-			struct radeon_connector *radeon_connector = to_radeon_connector(connector);
+		} else {
 			switch (radeon_connector->hpd.hpd) {
 			case RADEON_HPD_1:
 				WREG32(DC_HOT_PLUG_DETECT1_CONTROL, DC_HOT_PLUG_DETECTx_EN);
@@ -819,6 +817,7 @@ void r600_hpd_init(struct radeon_device *rdev)
 				break;
 			}
 		}
+		radeon_hpd_set_polarity(rdev, radeon_connector->hpd.hpd);
 	}
 	if (rdev->irq.installed)
 		r600_irq_set(rdev);
diff --git a/drivers/gpu/drm/radeon/r600_hdmi.c b/drivers/gpu/drm/radeon/r600_hdmi.c
index f5ac7e788d81..c45d92191fd8 100644
--- a/drivers/gpu/drm/radeon/r600_hdmi.c
+++ b/drivers/gpu/drm/radeon/r600_hdmi.c
@@ -196,6 +196,13 @@ static void r600_hdmi_videoinfoframe(
 	frame[0xD] = (right_bar >> 8);
 
 	r600_hdmi_infoframe_checksum(0x82, 0x02, 0x0D, frame);
+	/* Our header values (type, version, length) should be alright, Intel
+	 * is using the same. Checksum function also seems to be OK, it works
+	 * fine for audio infoframe. However calculated value is always lower
+	 * by 2 in comparison to fglrx. It breaks displaying anything in case
+	 * of TVs that strictly check the checksum. Hack it manually here to
+	 * workaround this issue. */
+	frame[0x0] += 2;
 
 	WREG32(offset+R600_HDMI_VIDEOINFOFRAME_0,
 		frame[0x0] | (frame[0x1] << 8) | (frame[0x2] << 16) | (frame[0x3] << 24));
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index c1e056b35b29..184628c7e02b 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -93,6 +93,7 @@ extern int radeon_audio;
 extern int radeon_disp_priority;
 extern int radeon_hw_i2c;
 extern int radeon_pcie_gen2;
+extern int radeon_msi;
 
 /*
  * Copy from radeon_drv.h so we don't have to include both and have conflicting
diff --git a/drivers/gpu/drm/radeon/radeon_atombios.c b/drivers/gpu/drm/radeon/radeon_atombios.c
index bf2b61584cdb..a098edcf6628 100644
--- a/drivers/gpu/drm/radeon/radeon_atombios.c
+++ b/drivers/gpu/drm/radeon/radeon_atombios.c
@@ -85,6 +85,18 @@ static inline struct radeon_i2c_bus_rec radeon_lookup_i2c_gpio(struct radeon_dev
 		for (i = 0; i < num_indices; i++) {
 			gpio = &i2c_info->asGPIO_Info[i];
 
+			/* r4xx mask is technically not used by the hw, so patch in the legacy mask bits */
+			if ((rdev->family == CHIP_R420) ||
+			    (rdev->family == CHIP_R423) ||
+			    (rdev->family == CHIP_RV410)) {
+				if ((le16_to_cpu(gpio->usClkMaskRegisterIndex) == 0x0018) ||
+				    (le16_to_cpu(gpio->usClkMaskRegisterIndex) == 0x0019) ||
+				    (le16_to_cpu(gpio->usClkMaskRegisterIndex) == 0x001a)) {
+					gpio->ucClkMaskShift = 0x19;
+					gpio->ucDataMaskShift = 0x18;
+				}
+			}
+
 			/* some evergreen boards have bad data for this entry */
 			if (ASIC_IS_DCE4(rdev)) {
 				if ((i == 7) &&
@@ -169,6 +181,18 @@ void radeon_atombios_i2c_init(struct radeon_device *rdev)
 			gpio = &i2c_info->asGPIO_Info[i];
 			i2c.valid = false;
 
+			/* r4xx mask is technically not used by the hw, so patch in the legacy mask bits */
+			if ((rdev->family == CHIP_R420) ||
+			    (rdev->family == CHIP_R423) ||
+			    (rdev->family == CHIP_RV410)) {
+				if ((le16_to_cpu(gpio->usClkMaskRegisterIndex) == 0x0018) ||
+				    (le16_to_cpu(gpio->usClkMaskRegisterIndex) == 0x0019) ||
+				    (le16_to_cpu(gpio->usClkMaskRegisterIndex) == 0x001a)) {
+					gpio->ucClkMaskShift = 0x19;
+					gpio->ucDataMaskShift = 0x18;
+				}
+			}
+
 			/* some evergreen boards have bad data for this entry */
 			if (ASIC_IS_DCE4(rdev)) {
 				if ((i == 7) &&
@@ -2544,7 +2568,11 @@ void radeon_atombios_get_power_modes(struct radeon_device *rdev)
 
 	rdev->pm.current_power_state_index = rdev->pm.default_power_state_index;
 	rdev->pm.current_clock_mode_index = 0;
-	rdev->pm.current_vddc = rdev->pm.power_state[rdev->pm.default_power_state_index].clock_info[0].voltage.voltage;
+	if (rdev->pm.default_power_state_index >= 0)
+		rdev->pm.current_vddc =
+			rdev->pm.power_state[rdev->pm.default_power_state_index].clock_info[0].voltage.voltage;
+	else
+		rdev->pm.current_vddc = 0;
 }
 
 void radeon_atom_set_clock_gating(struct radeon_device *rdev, int enable)
diff --git a/drivers/gpu/drm/radeon/radeon_combios.c b/drivers/gpu/drm/radeon/radeon_combios.c
index 63675241c7ff..8bf83c4b4147 100644
--- a/drivers/gpu/drm/radeon/radeon_combios.c
+++ b/drivers/gpu/drm/radeon/radeon_combios.c
@@ -620,8 +620,8 @@ static struct radeon_i2c_bus_rec combios_setup_i2c_bus(struct radeon_device *rde
 		i2c.y_data_mask = 0x80;
 	} else {
 		/* default masks for ddc pads */
-		i2c.mask_clk_mask = RADEON_GPIO_EN_1;
-		i2c.mask_data_mask = RADEON_GPIO_EN_0;
+		i2c.mask_clk_mask = RADEON_GPIO_MASK_1;
+		i2c.mask_data_mask = RADEON_GPIO_MASK_0;
 		i2c.a_clk_mask = RADEON_GPIO_A_1;
 		i2c.a_data_mask = RADEON_GPIO_A_0;
 		i2c.en_clk_mask = RADEON_GPIO_EN_1;
diff --git a/drivers/gpu/drm/radeon/radeon_connectors.c b/drivers/gpu/drm/radeon/radeon_connectors.c
index 449c3d8c6836..a58b4522404d 100644
--- a/drivers/gpu/drm/radeon/radeon_connectors.c
+++ b/drivers/gpu/drm/radeon/radeon_connectors.c
@@ -44,8 +44,6 @@ extern void
 radeon_legacy_backlight_init(struct radeon_encoder *radeon_encoder,
 			     struct drm_connector *drm_connector);
 
-bool radeon_connector_encoder_is_dp_bridge(struct drm_connector *connector);
-
 void radeon_connector_hotplug(struct drm_connector *connector)
 {
 	struct drm_device *dev = connector->dev;
@@ -724,6 +722,7 @@ radeon_vga_detect(struct drm_connector *connector, bool force)
 		dret = radeon_ddc_probe(radeon_connector,
 					radeon_connector->requires_extended_probe);
 	if (dret) {
+		radeon_connector->detected_by_load = false;
 		if (radeon_connector->edid) {
 			kfree(radeon_connector->edid);
 			radeon_connector->edid = NULL;
@@ -750,12 +749,21 @@ radeon_vga_detect(struct drm_connector *connector, bool force)
 	} else {
 
 		/* if we aren't forcing don't do destructive polling */
-		if (!force)
-			return connector->status;
+		if (!force) {
+			/* only return the previous status if we last
+			 * detected a monitor via load.
+			 */
+			if (radeon_connector->detected_by_load)
+				return connector->status;
+			else
+				return ret;
+		}
 
 		if (radeon_connector->dac_load_detect && encoder) {
 			encoder_funcs = encoder->helper_private;
 			ret = encoder_funcs->detect(encoder, connector);
+			if (ret != connector_status_disconnected)
+				radeon_connector->detected_by_load = true;
 		}
 	}
 
@@ -897,6 +905,7 @@ radeon_dvi_detect(struct drm_connector *connector, bool force)
 		dret = radeon_ddc_probe(radeon_connector,
 					radeon_connector->requires_extended_probe);
 	if (dret) {
+		radeon_connector->detected_by_load = false;
 		if (radeon_connector->edid) {
 			kfree(radeon_connector->edid);
 			radeon_connector->edid = NULL;
@@ -959,8 +968,18 @@ radeon_dvi_detect(struct drm_connector *connector, bool force)
 	if ((ret == connector_status_connected) && (radeon_connector->use_digital == true))
 		goto out;
 
+	/* DVI-D and HDMI-A are digital only */
+	if ((connector->connector_type == DRM_MODE_CONNECTOR_DVID) ||
+	    (connector->connector_type == DRM_MODE_CONNECTOR_HDMIA))
+		goto out;
+
+	/* if we aren't forcing don't do destructive polling */
 	if (!force) {
-		ret = connector->status;
+		/* only return the previous status if we last
+		 * detected a monitor via load.
+		 */
+		if (radeon_connector->detected_by_load)
+			ret = connector->status;
 		goto out;
 	}
 
@@ -985,6 +1004,8 @@ radeon_dvi_detect(struct drm_connector *connector, bool force)
 					if (ret == connector_status_connected) {
 						radeon_connector->use_digital = false;
 					}
+					if (ret != connector_status_disconnected)
+						radeon_connector->detected_by_load = true;
 				}
 				break;
 			}
@@ -1181,7 +1202,8 @@ static int radeon_dp_get_modes(struct drm_connector *connector)
 		}
 	} else {
 		/* need to setup ddc on the bridge */
-		if (radeon_connector_encoder_is_dp_bridge(connector)) {
+		if (radeon_connector_encoder_get_dp_bridge_encoder_id(connector) !=
+			ENCODER_OBJECT_ID_NONE) {
 			if (encoder)
 				radeon_atom_ext_encoder_setup_ddc(encoder);
 		}
@@ -1191,13 +1213,12 @@ static int radeon_dp_get_modes(struct drm_connector *connector)
 	return ret;
 }
 
-bool radeon_connector_encoder_is_dp_bridge(struct drm_connector *connector)
+u16 radeon_connector_encoder_get_dp_bridge_encoder_id(struct drm_connector *connector)
 {
 	struct drm_mode_object *obj;
 	struct drm_encoder *encoder;
 	struct radeon_encoder *radeon_encoder;
 	int i;
-	bool found = false;
 
 	for (i = 0; i < DRM_CONNECTOR_MAX_ENCODER; i++) {
 		if (connector->encoder_ids[i] == 0)
@@ -1213,14 +1234,13 @@ bool radeon_connector_encoder_is_dp_bridge(struct drm_connector *connector)
 		switch (radeon_encoder->encoder_id) {
 		case ENCODER_OBJECT_ID_TRAVIS:
 		case ENCODER_OBJECT_ID_NUTMEG:
-			found = true;
-			break;
+			return radeon_encoder->encoder_id;
 		default:
 			break;
 		}
 	}
 
-	return found;
+	return ENCODER_OBJECT_ID_NONE;
 }
 
 bool radeon_connector_encoder_is_hbr2(struct drm_connector *connector)
@@ -1297,7 +1317,8 @@ radeon_dp_detect(struct drm_connector *connector, bool force)
 		if (!radeon_dig_connector->edp_on)
 			atombios_set_edp_panel_power(connector,
 						     ATOM_TRANSMITTER_ACTION_POWER_OFF);
-	} else if (radeon_connector_encoder_is_dp_bridge(connector)) {
+	} else if (radeon_connector_encoder_get_dp_bridge_encoder_id(connector) !=
+		   ENCODER_OBJECT_ID_NONE) {
 		/* DP bridges are always DP */
 		radeon_dig_connector->dp_sink_type = CONNECTOR_OBJECT_ID_DISPLAYPORT;
 		/* get the DPCD from the bridge */
diff --git a/drivers/gpu/drm/radeon/radeon_device.c b/drivers/gpu/drm/radeon/radeon_device.c
index b51e15725c6e..50d105a1c747 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -224,8 +224,11 @@ int radeon_wb_init(struct radeon_device *rdev)
 	if (radeon_no_wb == 1)
 		rdev->wb.enabled = false;
 	else {
-		/* often unreliable on AGP */
 		if (rdev->flags & RADEON_IS_AGP) {
+			/* often unreliable on AGP */
+			rdev->wb.enabled = false;
+		} else if (rdev->family < CHIP_R300) {
+			/* often unreliable on pre-r300 */
 			rdev->wb.enabled = false;
 		} else {
 			rdev->wb.enabled = true;
diff --git a/drivers/gpu/drm/radeon/radeon_display.c b/drivers/gpu/drm/radeon/radeon_display.c
index 6adb3e58affd..07ac48162a13 100644
--- a/drivers/gpu/drm/radeon/radeon_display.c
+++ b/drivers/gpu/drm/radeon/radeon_display.c
@@ -708,7 +708,8 @@ int radeon_ddc_get_modes(struct radeon_connector *radeon_connector)
 
 	if ((radeon_connector->base.connector_type == DRM_MODE_CONNECTOR_DisplayPort) ||
 	    (radeon_connector->base.connector_type == DRM_MODE_CONNECTOR_eDP) ||
-	    radeon_connector_encoder_is_dp_bridge(&radeon_connector->base)) {
+	    (radeon_connector_encoder_get_dp_bridge_encoder_id(&radeon_connector->base) !=
+	     ENCODER_OBJECT_ID_NONE)) {
 		struct radeon_connector_atom_dig *dig = radeon_connector->con_priv;
 
 		if ((dig->dp_sink_type == CONNECTOR_OBJECT_ID_DISPLAYPORT ||
diff --git a/drivers/gpu/drm/radeon/radeon_drv.c b/drivers/gpu/drm/radeon/radeon_drv.c
index e71d2ed7fa11..c12b0775d688 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.c
+++ b/drivers/gpu/drm/radeon/radeon_drv.c
@@ -118,6 +118,7 @@ int radeon_audio = 0;
 int radeon_disp_priority = 0;
 int radeon_hw_i2c = 0;
 int radeon_pcie_gen2 = 0;
+int radeon_msi = -1;
 
 MODULE_PARM_DESC(no_wb, "Disable AGP writeback for scratch registers");
 module_param_named(no_wb, radeon_no_wb, int, 0444);
@@ -164,6 +165,9 @@ module_param_named(hw_i2c, radeon_hw_i2c, int, 0444);
 MODULE_PARM_DESC(pcie_gen2, "PCIE Gen2 mode (1 = enable)");
 module_param_named(pcie_gen2, radeon_pcie_gen2, int, 0444);
 
+MODULE_PARM_DESC(msi, "MSI support (1 = enable, 0 = disable, -1 = auto)");
+module_param_named(msi, radeon_msi, int, 0444);
+
 static int radeon_suspend(struct drm_device *dev, pm_message_t state)
 {
 	drm_radeon_private_t *dev_priv = dev->dev_private;
diff --git a/drivers/gpu/drm/radeon/radeon_encoders.c b/drivers/gpu/drm/radeon/radeon_encoders.c
index eb3f6dc6df83..9838865e223b 100644
--- a/drivers/gpu/drm/radeon/radeon_encoders.c
+++ b/drivers/gpu/drm/radeon/radeon_encoders.c
@@ -266,7 +266,7 @@ struct drm_encoder *radeon_atom_get_external_encoder(struct drm_encoder *encoder
 	return NULL;
 }
 
-bool radeon_encoder_is_dp_bridge(struct drm_encoder *encoder)
+u16 radeon_encoder_get_dp_bridge_encoder_id(struct drm_encoder *encoder)
 {
 	struct drm_encoder *other_encoder = radeon_atom_get_external_encoder(encoder);
 
@@ -368,7 +368,7 @@ static bool radeon_atom_mode_fixup(struct drm_encoder *encoder,
 
 	if (ASIC_IS_DCE3(rdev) &&
 	    ((radeon_encoder->active_device & (ATOM_DEVICE_DFP_SUPPORT | ATOM_DEVICE_LCD_SUPPORT)) ||
-	     radeon_encoder_is_dp_bridge(encoder))) {
+	     (radeon_encoder_get_dp_bridge_encoder_id(encoder) != ENCODER_OBJECT_ID_NONE))) {
 		struct drm_connector *connector = radeon_get_connector_for_encoder(encoder);
 		radeon_dp_set_link_config(connector, mode);
 	}
@@ -658,7 +658,7 @@ atombios_get_encoder_mode(struct drm_encoder *encoder)
 	struct radeon_connector_atom_dig *dig_connector;
 
 	/* dp bridges are always DP */
-	if (radeon_encoder_is_dp_bridge(encoder))
+	if (radeon_encoder_get_dp_bridge_encoder_id(encoder) != ENCODER_OBJECT_ID_NONE)
 		return ATOM_ENCODER_MODE_DP;
 
 	/* DVO is always DVO */
@@ -1638,7 +1638,7 @@ atombios_set_encoder_crtc_source(struct drm_encoder *encoder)
 			break;
 		case 2:
 			args.v2.ucCRTC = radeon_crtc->crtc_id;
-			if (radeon_encoder_is_dp_bridge(encoder)) {
+			if (radeon_encoder_get_dp_bridge_encoder_id(encoder) != ENCODER_OBJECT_ID_NONE) {
 				struct drm_connector *connector = radeon_get_connector_for_encoder(encoder);
 
 				if (connector->connector_type == DRM_MODE_CONNECTOR_LVDS)
@@ -2099,7 +2099,8 @@ static void radeon_atom_encoder_prepare(struct drm_encoder *encoder)
 
 	if ((radeon_encoder->active_device &
 	     (ATOM_DEVICE_DFP_SUPPORT | ATOM_DEVICE_LCD_SUPPORT)) ||
-	    radeon_encoder_is_dp_bridge(encoder)) {
+	    (radeon_encoder_get_dp_bridge_encoder_id(encoder) !=
+	     ENCODER_OBJECT_ID_NONE)) {
 		struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
 		if (dig)
 			dig->dig_encoder = radeon_atom_pick_dig_encoder(encoder);
diff --git a/drivers/gpu/drm/radeon/radeon_irq_kms.c b/drivers/gpu/drm/radeon/radeon_irq_kms.c
index 9ec830c77af0..5feb6e9edd8c 100644
--- a/drivers/gpu/drm/radeon/radeon_irq_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_irq_kms.c
@@ -108,6 +108,52 @@ void radeon_driver_irq_uninstall_kms(struct drm_device *dev)
 	radeon_irq_set(rdev);
 }
 
+static bool radeon_msi_ok(struct radeon_device *rdev)
+{
+	/* RV370/RV380 was first asic with MSI support */
+	if (rdev->family < CHIP_RV380)
+		return false;
+
+	/* MSIs don't work on AGP */
+	if (rdev->flags & RADEON_IS_AGP)
+		return false;
+
+	/* force MSI on */
+	if (radeon_msi == 1)
+		return true;
+	else if (radeon_msi == 0)
+		return false;
+
+	/* Quirks */
+	/* HP RS690 only seems to work with MSIs. */
+	if ((rdev->pdev->device == 0x791f) &&
+	    (rdev->pdev->subsystem_vendor == 0x103c) &&
+	    (rdev->pdev->subsystem_device == 0x30c2))
+		return true;
+
+	/* Dell RS690 only seems to work with MSIs. */
+	if ((rdev->pdev->device == 0x791f) &&
+	    (rdev->pdev->subsystem_vendor == 0x1028) &&
+	    (rdev->pdev->subsystem_device == 0x01fc))
+		return true;
+
+	/* Dell RS690 only seems to work with MSIs. */
+	if ((rdev->pdev->device == 0x791f) &&
+	    (rdev->pdev->subsystem_vendor == 0x1028) &&
+	    (rdev->pdev->subsystem_device == 0x01fd))
+		return true;
+
+	if (rdev->flags & RADEON_IS_IGP) {
+		/* APUs work fine with MSIs */
+		if (rdev->family >= CHIP_PALM)
+			return true;
+		/* lots of IGPs have problems with MSIs */
+		return false;
+	}
+
+	return true;
+}
+
 int radeon_irq_kms_init(struct radeon_device *rdev)
 {
 	int i;
@@ -124,12 +170,8 @@ int radeon_irq_kms_init(struct radeon_device *rdev)
 	}
 	/* enable msi */
 	rdev->msi_enabled = 0;
-	/* MSIs don't seem to work reliably on all IGP
-	 * chips.  Disable MSI on them for now.
-	 */
-	if ((rdev->family >= CHIP_RV380) &&
-	    ((!(rdev->flags & RADEON_IS_IGP)) || (rdev->family >= CHIP_PALM)) &&
-	    (!(rdev->flags & RADEON_IS_AGP))) {
+
+	if (radeon_msi_ok(rdev)) {
 		int ret = pci_enable_msi(rdev->pdev);
 		if (!ret) {
 			rdev->msi_enabled = 1;
diff --git a/drivers/gpu/drm/radeon/radeon_mode.h b/drivers/gpu/drm/radeon/radeon_mode.h
index 68820f5f6303..cbf80de2d9c6 100644
--- a/drivers/gpu/drm/radeon/radeon_mode.h
+++ b/drivers/gpu/drm/radeon/radeon_mode.h
@@ -447,6 +447,7 @@ struct radeon_connector {
 	struct edid *edid;
 	void *con_priv;
 	bool dac_load_detect;
+	bool detected_by_load; /* if the connection status was determined by load */
 	uint16_t connector_object_id;
 	struct radeon_hpd hpd;
 	struct radeon_router router;
@@ -467,8 +468,8 @@ radeon_atombios_get_tv_info(struct radeon_device *rdev);
 extern struct drm_connector *
 radeon_get_connector_for_encoder(struct drm_encoder *encoder);
 
-extern bool radeon_encoder_is_dp_bridge(struct drm_encoder *encoder);
-extern bool radeon_connector_encoder_is_dp_bridge(struct drm_connector *connector);
+extern u16 radeon_encoder_get_dp_bridge_encoder_id(struct drm_encoder *encoder);
+extern u16 radeon_connector_encoder_get_dp_bridge_encoder_id(struct drm_connector *connector);
 extern bool radeon_connector_encoder_is_hbr2(struct drm_connector *connector);
 extern bool radeon_connector_is_dp12_capable(struct drm_connector *connector);
 
diff --git a/drivers/gpu/drm/radeon/rs600.c b/drivers/gpu/drm/radeon/rs600.c
index 4b5d0e6974a8..d9b0bc4547e3 100644
--- a/drivers/gpu/drm/radeon/rs600.c
+++ b/drivers/gpu/drm/radeon/rs600.c
@@ -62,6 +62,7 @@ u32 rs600_page_flip(struct radeon_device *rdev, int crtc_id, u64 crtc_base)
 {
 	struct radeon_crtc *radeon_crtc = rdev->mode_info.crtcs[crtc_id];
 	u32 tmp = RREG32(AVIVO_D1GRPH_UPDATE + radeon_crtc->crtc_offset);
+	int i;
 
 	/* Lock the graphics update lock */
 	tmp |= AVIVO_D1GRPH_UPDATE_LOCK;
@@ -74,7 +75,11 @@ u32 rs600_page_flip(struct radeon_device *rdev, int crtc_id, u64 crtc_base)
 	       (u32)crtc_base);
 
 	/* Wait for update_pending to go high. */
-	while (!(RREG32(AVIVO_D1GRPH_UPDATE + radeon_crtc->crtc_offset) & AVIVO_D1GRPH_SURFACE_UPDATE_PENDING));
+	for (i = 0; i < rdev->usec_timeout; i++) {
+		if (RREG32(AVIVO_D1GRPH_UPDATE + radeon_crtc->crtc_offset) & AVIVO_D1GRPH_SURFACE_UPDATE_PENDING)
+			break;
+		udelay(1);
+	}
 	DRM_DEBUG("Update pending now high. Unlocking vupdate_lock.\n");
 
 	/* Unlock the lock, so double-buffering can take place inside vblank */
@@ -287,6 +292,7 @@ void rs600_hpd_init(struct radeon_device *rdev)
 		default:
 			break;
 		}
+		radeon_hpd_set_polarity(rdev, radeon_connector->hpd.hpd);
 	}
 	if (rdev->irq.installed)
 		rs600_irq_set(rdev);
@@ -318,10 +324,10 @@ void rs600_hpd_fini(struct radeon_device *rdev)
 
 void rs600_bm_disable(struct radeon_device *rdev)
 {
-	u32 tmp;
+	u16 tmp;
 
 	/* disable bus mastering */
-	pci_read_config_word(rdev->pdev, 0x4, (u16*)&tmp);
+	pci_read_config_word(rdev->pdev, 0x4, &tmp);
 	pci_write_config_word(rdev->pdev, 0x4, tmp & 0xFFFB);
 	mdelay(1);
 }
diff --git a/drivers/gpu/drm/radeon/rv770.c b/drivers/gpu/drm/radeon/rv770.c
index b13c2eedc321..ddc206a1f617 100644
--- a/drivers/gpu/drm/radeon/rv770.c
+++ b/drivers/gpu/drm/radeon/rv770.c
@@ -47,6 +47,7 @@ u32 rv770_page_flip(struct radeon_device *rdev, int crtc_id, u64 crtc_base)
 {
 	struct radeon_crtc *radeon_crtc = rdev->mode_info.crtcs[crtc_id];
 	u32 tmp = RREG32(AVIVO_D1GRPH_UPDATE + radeon_crtc->crtc_offset);
+	int i;
 
 	/* Lock the graphics update lock */
 	tmp |= AVIVO_D1GRPH_UPDATE_LOCK;
@@ -66,7 +67,11 @@ u32 rv770_page_flip(struct radeon_device *rdev, int crtc_id, u64 crtc_base)
 	       (u32)crtc_base);
 
 	/* Wait for update_pending to go high. */
-	while (!(RREG32(AVIVO_D1GRPH_UPDATE + radeon_crtc->crtc_offset) & AVIVO_D1GRPH_SURFACE_UPDATE_PENDING));
+	for (i = 0; i < rdev->usec_timeout; i++) {
+		if (RREG32(AVIVO_D1GRPH_UPDATE + radeon_crtc->crtc_offset) & AVIVO_D1GRPH_SURFACE_UPDATE_PENDING)
+			break;
+		udelay(1);
+	}
 	DRM_DEBUG("Update pending now high. Unlocking vupdate_lock.\n");
 
 	/* Unlock the lock, so double-buffering can take place inside vblank */
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
index dfe32e62bd90..8a38c91f4c9c 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
@@ -313,7 +313,7 @@ int vmw_framebuffer_create_handle(struct drm_framebuffer *fb,
 				  unsigned int *handle)
 {
 	if (handle)
-		handle = 0;
+		*handle = 0;
 
 	return 0;
 }
diff --git a/drivers/gpu/ion/Kconfig b/drivers/gpu/ion/Kconfig
new file mode 100644
index 000000000000..9a8cbdd9836d
--- /dev/null
+++ b/drivers/gpu/ion/Kconfig
@@ -0,0 +1,17 @@
+menuconfig ION
+	tristate "Ion Memory Manager"
+	select GENERIC_ALLOCATOR
+	help
+	  Chose this option to enable the ION Memory Manager.
+
+config ION_IOMMU
+       bool
+
+config ION_TEGRA
+	tristate "Ion for Tegra"
+	depends on ARCH_TEGRA && ION
+	select TEGRA_IOMMU_SMMU if !ARCH_TEGRA_2x_SOC
+	select ION_IOMMU if TEGRA_IOMMU_SMMU
+	help
+	  Choose this option if you wish to use ion on an nVidia Tegra.
+
diff --git a/drivers/gpu/ion/Makefile b/drivers/gpu/ion/Makefile
new file mode 100644
index 000000000000..4ddc78e9d41d
--- /dev/null
+++ b/drivers/gpu/ion/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_ION) +=	ion.o ion_heap.o ion_system_heap.o ion_carveout_heap.o
+obj-$(CONFIG_ION_IOMMU)	+= ion_iommu_heap.o
+obj-$(CONFIG_ION_TEGRA) += tegra/
diff --git a/drivers/gpu/ion/ion.c b/drivers/gpu/ion/ion.c
new file mode 100644
index 000000000000..512ebc5cc8ea
--- /dev/null
+++ b/drivers/gpu/ion/ion.c
@@ -0,0 +1,1152 @@
+/*
+ * drivers/gpu/ion/ion.c
+ *
+ * Copyright (C) 2011 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#define pr_fmt(fmt)	"%s():%d: " fmt, __func__, __LINE__
+
+#include <linux/device.h>
+#include <linux/file.h>
+#include <linux/fs.h>
+#include <linux/anon_inodes.h>
+#include <linux/ion.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/mm.h>
+#include <linux/mm_types.h>
+#include <linux/rbtree.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+#include <linux/debugfs.h>
+
+#include "ion_priv.h"
+#define DEBUG
+
+/* this function should only be called while dev->lock is held */
+static void ion_buffer_add(struct ion_device *dev,
+			   struct ion_buffer *buffer)
+{
+	struct rb_node **p = &dev->buffers.rb_node;
+	struct rb_node *parent = NULL;
+	struct ion_buffer *entry;
+
+	while (*p) {
+		parent = *p;
+		entry = rb_entry(parent, struct ion_buffer, node);
+
+		if (buffer < entry) {
+			p = &(*p)->rb_left;
+		} else if (buffer > entry) {
+			p = &(*p)->rb_right;
+		} else {
+			pr_err("buffer already found.");
+			BUG();
+		}
+	}
+
+	rb_link_node(&buffer->node, parent, p);
+	rb_insert_color(&buffer->node, &dev->buffers);
+}
+
+/* this function should only be called while dev->lock is held */
+static struct ion_buffer *ion_buffer_create(struct ion_heap *heap,
+				     struct ion_device *dev,
+				     unsigned long len,
+				     unsigned long align,
+				     unsigned long flags)
+{
+	struct ion_buffer *buffer;
+	int ret;
+
+	buffer = kzalloc(sizeof(struct ion_buffer), GFP_KERNEL);
+	if (!buffer)
+		return ERR_PTR(-ENOMEM);
+
+	buffer->heap = heap;
+	kref_init(&buffer->ref);
+
+	ret = heap->ops->allocate(heap, buffer, len, align, flags);
+	if (ret) {
+		kfree(buffer);
+		return ERR_PTR(ret);
+	}
+	buffer->dev = dev;
+	buffer->size = len;
+	mutex_init(&buffer->lock);
+	ion_buffer_add(dev, buffer);
+	return buffer;
+}
+
+static void ion_buffer_destroy(struct kref *kref)
+{
+	struct ion_buffer *buffer = container_of(kref, struct ion_buffer, ref);
+	struct ion_device *dev = buffer->dev;
+
+	buffer->heap->ops->free(buffer);
+	mutex_lock(&dev->lock);
+	rb_erase(&buffer->node, &dev->buffers);
+	mutex_unlock(&dev->lock);
+	kfree(buffer);
+}
+
+void ion_buffer_get(struct ion_buffer *buffer)
+{
+	kref_get(&buffer->ref);
+}
+
+static int ion_buffer_put(struct ion_buffer *buffer)
+{
+	return kref_put(&buffer->ref, ion_buffer_destroy);
+}
+
+struct ion_handle *ion_handle_create(struct ion_client *client,
+				     struct ion_buffer *buffer)
+{
+	struct ion_handle *handle;
+
+	handle = kzalloc(sizeof(struct ion_handle), GFP_KERNEL);
+	if (!handle)
+		return ERR_PTR(-ENOMEM);
+	kref_init(&handle->ref);
+	rb_init_node(&handle->node);
+	handle->client = client;
+	ion_buffer_get(buffer);
+	handle->buffer = buffer;
+
+	return handle;
+}
+
+static void ion_handle_destroy(struct kref *kref)
+{
+	struct ion_handle *handle = container_of(kref, struct ion_handle, ref);
+	/* XXX Can a handle be destroyed while it's map count is non-zero?:
+	   if (handle->map_cnt) unmap
+	 */
+	ion_buffer_put(handle->buffer);
+	mutex_lock(&handle->client->lock);
+	if (!RB_EMPTY_NODE(&handle->node))
+		rb_erase(&handle->node, &handle->client->handles);
+	mutex_unlock(&handle->client->lock);
+	kfree(handle);
+}
+
+struct ion_buffer *ion_handle_buffer(struct ion_handle *handle)
+{
+	return handle->buffer;
+}
+
+void ion_handle_get(struct ion_handle *handle)
+{
+	kref_get(&handle->ref);
+}
+
+int ion_handle_put(struct ion_handle *handle)
+{
+	return kref_put(&handle->ref, ion_handle_destroy);
+}
+
+static struct ion_handle *ion_handle_lookup(struct ion_client *client,
+					    struct ion_buffer *buffer)
+{
+	struct rb_node *n;
+
+	for (n = rb_first(&client->handles); n; n = rb_next(n)) {
+		struct ion_handle *handle = rb_entry(n, struct ion_handle,
+						     node);
+		if (handle->buffer == buffer)
+			return handle;
+	}
+	return NULL;
+}
+
+bool ion_handle_validate(struct ion_client *client, struct ion_handle *handle)
+{
+	struct rb_node *n = client->handles.rb_node;
+
+	while (n) {
+		struct ion_handle *handle_node = rb_entry(n, struct ion_handle,
+							  node);
+		if (handle < handle_node)
+			n = n->rb_left;
+		else if (handle > handle_node)
+			n = n->rb_right;
+		else
+			return true;
+	}
+	WARN(1, "invalid handle passed h=0x%x,comm=%d\n", handle,
+		current->group_leader->comm);
+	return false;
+}
+
+void ion_handle_add(struct ion_client *client, struct ion_handle *handle)
+{
+	struct rb_node **p = &client->handles.rb_node;
+	struct rb_node *parent = NULL;
+	struct ion_handle *entry;
+
+	while (*p) {
+		parent = *p;
+		entry = rb_entry(parent, struct ion_handle, node);
+
+		if (handle < entry)
+			p = &(*p)->rb_left;
+		else if (handle > entry)
+			p = &(*p)->rb_right;
+		else
+			WARN(1, "%s: buffer already found.", __func__);
+	}
+
+	rb_link_node(&handle->node, parent, p);
+	rb_insert_color(&handle->node, &client->handles);
+}
+
+struct ion_handle *ion_alloc(struct ion_client *client, size_t len,
+			     size_t align, unsigned int flags)
+{
+	struct rb_node *n;
+	struct ion_handle *handle;
+	struct ion_device *dev = client->dev;
+	struct ion_buffer *buffer = NULL;
+
+	/*
+	 * traverse the list of heaps available in this system in priority
+	 * order.  If the heap type is supported by the client, and matches the
+	 * request of the caller allocate from it.  Repeat until allocate has
+	 * succeeded or all heaps have been tried
+	 */
+	mutex_lock(&dev->lock);
+	for (n = rb_first(&dev->heaps); n != NULL; n = rb_next(n)) {
+		struct ion_heap *heap = rb_entry(n, struct ion_heap, node);
+		/* if the client doesn't support this heap type */
+		if (!((1 << heap->type) & client->heap_mask))
+			continue;
+		/* if the caller didn't specify this heap type */
+		if (!((1 << heap->id) & flags))
+			continue;
+		buffer = ion_buffer_create(heap, dev, len, align, flags);
+		if (!IS_ERR_OR_NULL(buffer))
+			break;
+	}
+	mutex_unlock(&dev->lock);
+
+	if (IS_ERR_OR_NULL(buffer))
+		return ERR_PTR(PTR_ERR(buffer));
+
+	handle = ion_handle_create(client, buffer);
+
+	if (IS_ERR_OR_NULL(handle))
+		goto end;
+
+	/*
+	 * ion_buffer_create will create a buffer with a ref_cnt of 1,
+	 * and ion_handle_create will take a second reference, drop one here
+	 */
+	ion_buffer_put(buffer);
+
+	mutex_lock(&client->lock);
+	ion_handle_add(client, handle);
+	mutex_unlock(&client->lock);
+	return handle;
+
+end:
+	ion_buffer_put(buffer);
+	return handle;
+}
+
+void ion_free(struct ion_client *client, struct ion_handle *handle)
+{
+	bool valid_handle;
+
+	BUG_ON(client != handle->client);
+
+	mutex_lock(&client->lock);
+	valid_handle = ion_handle_validate(client, handle);
+	mutex_unlock(&client->lock);
+
+	if (!valid_handle) {
+		WARN(1, "%s: invalid handle passed to free.\n", __func__);
+		return;
+	}
+	ion_handle_put(handle);
+}
+
+static bool _ion_map(int *buffer_cnt, int *handle_cnt)
+{
+	bool map;
+
+	BUG_ON(*handle_cnt != 0 && *buffer_cnt == 0);
+
+	if (*buffer_cnt)
+		map = false;
+	else
+		map = true;
+	if (*handle_cnt == 0)
+		(*buffer_cnt)++;
+	(*handle_cnt)++;
+	return map;
+}
+
+static bool _ion_unmap(int *buffer_cnt, int *handle_cnt)
+{
+	BUG_ON(*handle_cnt == 0);
+	(*handle_cnt)--;
+	if (*handle_cnt != 0)
+		return false;
+	BUG_ON(*buffer_cnt == 0);
+	(*buffer_cnt)--;
+	if (*buffer_cnt == 0)
+		return true;
+	return false;
+}
+
+int ion_phys(struct ion_client *client, struct ion_handle *handle,
+	     ion_phys_addr_t *addr, size_t *len)
+{
+	struct ion_buffer *buffer;
+	int ret;
+
+	mutex_lock(&client->lock);
+	if (!ion_handle_validate(client, handle)) {
+		mutex_unlock(&client->lock);
+		return -EINVAL;
+	}
+
+	buffer = handle->buffer;
+
+	if (!buffer->heap->ops->phys) {
+		pr_err("ion_phys is not implemented by this heap.\n");
+		mutex_unlock(&client->lock);
+		return -ENODEV;
+	}
+	mutex_unlock(&client->lock);
+	ret = buffer->heap->ops->phys(buffer->heap, buffer, addr, len);
+	return ret;
+}
+
+void *ion_map_kernel(struct ion_client *client, struct ion_handle *handle)
+{
+	struct ion_buffer *buffer;
+	void *vaddr;
+
+	mutex_lock(&client->lock);
+	if (!ion_handle_validate(client, handle)) {
+		WARN(1, "invalid handle passed to map_kernel.\n");
+		mutex_unlock(&client->lock);
+		return ERR_PTR(-EINVAL);
+	}
+
+	buffer = handle->buffer;
+	mutex_lock(&buffer->lock);
+
+	if (!handle->buffer->heap->ops->map_kernel) {
+		pr_err("map_kernel is not implemented by this heap.\n");
+		mutex_unlock(&buffer->lock);
+		mutex_unlock(&client->lock);
+		return ERR_PTR(-ENODEV);
+	}
+
+	if (_ion_map(&buffer->kmap_cnt, &handle->kmap_cnt)) {
+		vaddr = buffer->heap->ops->map_kernel(buffer->heap, buffer);
+		if (IS_ERR_OR_NULL(vaddr))
+			_ion_unmap(&buffer->kmap_cnt, &handle->kmap_cnt);
+		buffer->vaddr = vaddr;
+	} else {
+		vaddr = buffer->vaddr;
+	}
+	mutex_unlock(&buffer->lock);
+	mutex_unlock(&client->lock);
+	return vaddr;
+}
+
+struct scatterlist *ion_map_dma(struct ion_client *client,
+				struct ion_handle *handle)
+{
+	struct ion_buffer *buffer;
+	struct scatterlist *sglist;
+
+	mutex_lock(&client->lock);
+	if (!ion_handle_validate(client, handle)) {
+		WARN(1, "invalid handle passed to map_dma.\n");
+		mutex_unlock(&client->lock);
+		return ERR_PTR(-EINVAL);
+	}
+	buffer = handle->buffer;
+	mutex_lock(&buffer->lock);
+
+	if (!handle->buffer->heap->ops->map_dma) {
+		pr_err("map_kernel is not implemented by this heap.\n");
+		mutex_unlock(&buffer->lock);
+		mutex_unlock(&client->lock);
+		return ERR_PTR(-ENODEV);
+	}
+	if (_ion_map(&buffer->dmap_cnt, &handle->dmap_cnt)) {
+		sglist = buffer->heap->ops->map_dma(buffer->heap, buffer);
+		if (IS_ERR_OR_NULL(sglist))
+			_ion_unmap(&buffer->dmap_cnt, &handle->dmap_cnt);
+		buffer->sglist = sglist;
+	} else {
+		sglist = buffer->sglist;
+	}
+	mutex_unlock(&buffer->lock);
+	mutex_unlock(&client->lock);
+	return sglist;
+}
+
+struct scatterlist *iommu_heap_remap_dma(struct ion_heap *heap,
+					      struct ion_buffer *buf,
+					      unsigned long addr);
+int ion_remap_dma(struct ion_client *client,
+			struct ion_handle *handle,
+			unsigned long addr)
+{
+	struct ion_buffer *buffer;
+	int ret;
+
+	mutex_lock(&client->lock);
+	if (!ion_handle_validate(client, handle)) {
+		pr_err("invalid handle passed to map_dma.\n");
+		mutex_unlock(&client->lock);
+		return -EINVAL;
+	}
+	buffer = handle->buffer;
+	mutex_lock(&buffer->lock);
+
+	ret = iommu_heap_remap_dma(buffer->heap, buffer, addr);
+
+	mutex_unlock(&buffer->lock);
+	mutex_unlock(&client->lock);
+	return ret;
+}
+
+void ion_unmap_kernel(struct ion_client *client, struct ion_handle *handle)
+{
+	struct ion_buffer *buffer;
+
+	mutex_lock(&client->lock);
+	buffer = handle->buffer;
+	mutex_lock(&buffer->lock);
+	if (_ion_unmap(&buffer->kmap_cnt, &handle->kmap_cnt)) {
+		buffer->heap->ops->unmap_kernel(buffer->heap, buffer);
+		buffer->vaddr = NULL;
+	}
+	mutex_unlock(&buffer->lock);
+	mutex_unlock(&client->lock);
+}
+
+void ion_unmap_dma(struct ion_client *client, struct ion_handle *handle)
+{
+	struct ion_buffer *buffer;
+
+	mutex_lock(&client->lock);
+	buffer = handle->buffer;
+	mutex_lock(&buffer->lock);
+	if (_ion_unmap(&buffer->dmap_cnt, &handle->dmap_cnt)) {
+		buffer->heap->ops->unmap_dma(buffer->heap, buffer);
+		buffer->sglist = NULL;
+	}
+	mutex_unlock(&buffer->lock);
+	mutex_unlock(&client->lock);
+}
+
+
+struct ion_buffer *ion_share(struct ion_client *client,
+				 struct ion_handle *handle)
+{
+	bool valid_handle;
+
+	mutex_lock(&client->lock);
+	valid_handle = ion_handle_validate(client, handle);
+	mutex_unlock(&client->lock);
+	if (!valid_handle) {
+		WARN(1, "%s: invalid handle passed to share.\n", __func__);
+		return ERR_PTR(-EINVAL);
+	}
+
+	/* do not take an extra reference here, the burden is on the caller
+	 * to make sure the buffer doesn't go away while it's passing it
+	 * to another client -- ion_free should not be called on this handle
+	 * until the buffer has been imported into the other client
+	 */
+	return handle->buffer;
+}
+
+struct ion_handle *ion_import(struct ion_client *client,
+			      struct ion_buffer *buffer)
+{
+	struct ion_handle *handle = NULL;
+
+	mutex_lock(&client->lock);
+	/* if a handle exists for this buffer just take a reference to it */
+	handle = ion_handle_lookup(client, buffer);
+	if (!IS_ERR_OR_NULL(handle)) {
+		ion_handle_get(handle);
+		goto end;
+	}
+	handle = ion_handle_create(client, buffer);
+	if (IS_ERR_OR_NULL(handle)) {
+		pr_err("error during handle create\n");
+		goto end;
+	}
+	ion_handle_add(client, handle);
+end:
+	mutex_unlock(&client->lock);
+	return handle;
+}
+
+static const struct file_operations ion_share_fops;
+
+struct ion_handle *ion_import_fd(struct ion_client *client, int fd)
+{
+	struct file *file = fget(fd);
+	struct ion_handle *handle;
+
+	if (!file) {
+		pr_err("imported fd not found in file table.\n");
+		return ERR_PTR(-EINVAL);
+	}
+	if (file->f_op != &ion_share_fops) {
+		pr_err("imported file is not a shared ion file.\n");
+		handle = ERR_PTR(-EINVAL);
+		goto end;
+	}
+	handle = ion_import(client, file->private_data);
+end:
+	fput(file);
+	return handle;
+}
+
+static int ion_debug_client_show(struct seq_file *s, void *unused)
+{
+	struct ion_client *client = s->private;
+	struct rb_node *n;
+	size_t sizes[ION_NUM_HEAPS] = {0};
+	const char *names[ION_NUM_HEAPS] = {0};
+	int i;
+
+	mutex_lock(&client->lock);
+	for (n = rb_first(&client->handles); n; n = rb_next(n)) {
+		struct ion_handle *handle = rb_entry(n, struct ion_handle,
+						     node);
+		enum ion_heap_type type = handle->buffer->heap->type;
+
+		if (!names[type])
+			names[type] = handle->buffer->heap->name;
+		sizes[type] += handle->buffer->size;
+	}
+	mutex_unlock(&client->lock);
+
+	seq_printf(s, "%16.16s: %16.16s\n", "heap_name", "size_in_bytes");
+	for (i = 0; i < ION_NUM_HEAPS; i++) {
+		if (!names[i])
+			continue;
+		seq_printf(s, "%16.16s: %16u %d\n", names[i], sizes[i],
+			   atomic_read(&client->ref.refcount));
+	}
+	return 0;
+}
+
+static int ion_debug_client_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ion_debug_client_show, inode->i_private);
+}
+
+static const struct file_operations debug_client_fops = {
+	.open = ion_debug_client_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static struct ion_client *ion_client_lookup(struct ion_device *dev,
+					    struct task_struct *task)
+{
+	struct rb_node *n = dev->user_clients.rb_node;
+	struct ion_client *client;
+
+	mutex_lock(&dev->lock);
+	while (n) {
+		client = rb_entry(n, struct ion_client, node);
+		if (task == client->task) {
+			ion_client_get(client);
+			mutex_unlock(&dev->lock);
+			return client;
+		} else if (task < client->task) {
+			n = n->rb_left;
+		} else if (task > client->task) {
+			n = n->rb_right;
+		}
+	}
+	mutex_unlock(&dev->lock);
+	return NULL;
+}
+
+struct ion_client *ion_client_create(struct ion_device *dev,
+				     unsigned int heap_mask,
+				     const char *name)
+{
+	struct ion_client *client;
+	struct task_struct *task;
+	struct rb_node **p;
+	struct rb_node *parent = NULL;
+	struct ion_client *entry;
+	char debug_name[64];
+	pid_t pid;
+
+	get_task_struct(current->group_leader);
+	task_lock(current->group_leader);
+	pid = task_pid_nr(current->group_leader);
+	/* don't bother to store task struct for kernel threads,
+	   they can't be killed anyway */
+	if (current->group_leader->flags & PF_KTHREAD) {
+		put_task_struct(current->group_leader);
+		task = NULL;
+	} else {
+		task = current->group_leader;
+	}
+	task_unlock(current->group_leader);
+
+	/* if this isn't a kernel thread, see if a client already
+	   exists */
+	if (task) {
+		client = ion_client_lookup(dev, task);
+		if (!IS_ERR_OR_NULL(client)) {
+			put_task_struct(current->group_leader);
+			return client;
+		}
+	}
+
+	client = kzalloc(sizeof(struct ion_client), GFP_KERNEL);
+	if (!client) {
+		put_task_struct(current->group_leader);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	client->dev = dev;
+	client->handles = RB_ROOT;
+	mutex_init(&client->lock);
+	client->name = name;
+	client->heap_mask = heap_mask;
+	client->task = task;
+	client->pid = pid;
+	kref_init(&client->ref);
+
+	mutex_lock(&dev->lock);
+	if (task) {
+		p = &dev->user_clients.rb_node;
+		while (*p) {
+			parent = *p;
+			entry = rb_entry(parent, struct ion_client, node);
+
+			if (task < entry->task)
+				p = &(*p)->rb_left;
+			else if (task > entry->task)
+				p = &(*p)->rb_right;
+		}
+		rb_link_node(&client->node, parent, p);
+		rb_insert_color(&client->node, &dev->user_clients);
+	} else {
+		p = &dev->kernel_clients.rb_node;
+		while (*p) {
+			parent = *p;
+			entry = rb_entry(parent, struct ion_client, node);
+
+			if (client < entry)
+				p = &(*p)->rb_left;
+			else if (client > entry)
+				p = &(*p)->rb_right;
+		}
+		rb_link_node(&client->node, parent, p);
+		rb_insert_color(&client->node, &dev->kernel_clients);
+	}
+
+	snprintf(debug_name, 64, "%u", client->pid);
+	client->debug_root = debugfs_create_file(debug_name, 0664,
+						 dev->debug_root, client,
+						 &debug_client_fops);
+	mutex_unlock(&dev->lock);
+
+	return client;
+}
+
+static void _ion_client_destroy(struct kref *kref)
+{
+	struct ion_client *client = container_of(kref, struct ion_client, ref);
+	struct ion_device *dev = client->dev;
+	struct rb_node *n;
+
+	pr_debug("\n");
+	while ((n = rb_first(&client->handles))) {
+		struct ion_handle *handle = rb_entry(n, struct ion_handle,
+						     node);
+		ion_handle_destroy(&handle->ref);
+	}
+	mutex_lock(&dev->lock);
+	if (client->task) {
+		rb_erase(&client->node, &dev->user_clients);
+		put_task_struct(client->task);
+	} else {
+		rb_erase(&client->node, &dev->kernel_clients);
+	}
+	debugfs_remove_recursive(client->debug_root);
+	mutex_unlock(&dev->lock);
+
+	kfree(client);
+}
+
+void ion_client_get(struct ion_client *client)
+{
+	kref_get(&client->ref);
+}
+
+int ion_client_put(struct ion_client *client)
+{
+	return kref_put(&client->ref, _ion_client_destroy);
+}
+
+void ion_client_destroy(struct ion_client *client)
+{
+	ion_client_put(client);
+}
+
+static int ion_share_release(struct inode *inode, struct file* file)
+{
+	struct ion_buffer *buffer = file->private_data;
+
+	pr_debug("\n");
+	/* drop the reference to the buffer -- this prevents the
+	   buffer from going away because the client holding it exited
+	   while it was being passed */
+	ion_buffer_put(buffer);
+	return 0;
+}
+
+static void ion_vma_open(struct vm_area_struct *vma)
+{
+
+	struct ion_buffer *buffer = vma->vm_file->private_data;
+	struct ion_handle *handle = vma->vm_private_data;
+	struct ion_client *client;
+
+	pr_debug("\n");
+	/* check that the client still exists and take a reference so
+	   it can't go away until this vma is closed */
+	client = ion_client_lookup(buffer->dev, current->group_leader);
+	if (IS_ERR_OR_NULL(client)) {
+		vma->vm_private_data = NULL;
+		return;
+	}
+	ion_buffer_get(buffer);
+	ion_handle_get(handle);
+	pr_debug("client_cnt %d handle_cnt %d alloc_cnt %d\n",
+		 atomic_read(&client->ref.refcount),
+		 atomic_read(&handle->ref.refcount),
+		 atomic_read(&buffer->ref.refcount));
+}
+
+static void ion_vma_close(struct vm_area_struct *vma)
+{
+	struct ion_handle *handle = vma->vm_private_data;
+	struct ion_buffer *buffer = vma->vm_file->private_data;
+	struct ion_client *client;
+
+	pr_debug("\n");
+	/* this indicates the client is gone, nothing to do here */
+	if (!handle)
+		return;
+	client = handle->client;
+	pr_debug("client_cnt %d handle_cnt %d alloc_cnt %d\n",
+		 atomic_read(&client->ref.refcount),
+		 atomic_read(&handle->ref.refcount),
+		 atomic_read(&buffer->ref.refcount));
+	ion_handle_put(handle);
+	ion_client_put(client);
+	ion_buffer_put(buffer);
+	pr_debug("client_cnt %d handle_cnt %d alloc_cnt %d\n",
+		 atomic_read(&client->ref.refcount),
+		 atomic_read(&handle->ref.refcount),
+		 atomic_read(&buffer->ref.refcount));
+}
+
+static struct vm_operations_struct ion_vm_ops = {
+	.open = ion_vma_open,
+	.close = ion_vma_close,
+};
+
+static int ion_share_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct ion_buffer *buffer = file->private_data;
+	unsigned long size = vma->vm_end - vma->vm_start;
+	struct ion_client *client;
+	struct ion_handle *handle;
+	int ret;
+
+	pr_debug("\n");
+	/* make sure the client still exists, it's possible for the client to
+	   have gone away but the map/share fd still to be around, take
+	   a reference to it so it can't go away while this mapping exists */
+	client = ion_client_lookup(buffer->dev, current->group_leader);
+	if (IS_ERR_OR_NULL(client)) {
+		WARN(1, "trying to mmap an ion handle in a process with no "
+		       "ion client\n");
+		return -EINVAL;
+	}
+
+	if ((size > buffer->size) || (size + (vma->vm_pgoff << PAGE_SHIFT) >
+				     buffer->size)) {
+		WARN(1, "trying to map larger area than handle has available"
+		       "\n");
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* find the handle and take a reference to it */
+	handle = ion_import(client, buffer);
+	if (IS_ERR_OR_NULL(handle)) {
+		ret = -EINVAL;
+		goto err;
+	}
+	ion_buffer_get(buffer);
+
+	if (!handle->buffer->heap->ops->map_user) {
+		pr_err("this heap does not define a method for mapping "
+		       "to userspace\n");
+		ret = -EINVAL;
+		goto err1;
+	}
+
+	mutex_lock(&buffer->lock);
+	/* now map it to userspace */
+	ret = buffer->heap->ops->map_user(buffer->heap, buffer, vma);
+	mutex_unlock(&buffer->lock);
+	if (ret) {
+		pr_err("failure mapping buffer to userspace\n");
+		goto err1;
+	}
+
+	vma->vm_ops = &ion_vm_ops;
+	/* move the handle into the vm_private_data so we can access it from
+	   vma_open/close */
+	vma->vm_private_data = handle;
+	pr_debug("client_cnt %d handle_cnt %d alloc_cnt %d\n",
+		 atomic_read(&client->ref.refcount),
+		 atomic_read(&handle->ref.refcount),
+		 atomic_read(&buffer->ref.refcount));
+	return 0;
+
+err1:
+	/* drop the reference to the handle */
+	ion_handle_put(handle);
+err:
+	/* drop the reference to the client */
+	ion_client_put(client);
+	return ret;
+}
+
+static const struct file_operations ion_share_fops = {
+	.owner		= THIS_MODULE,
+	.release	= ion_share_release,
+	.mmap		= ion_share_mmap,
+};
+
+static int ion_ioctl_share(struct file *parent, struct ion_client *client,
+			   struct ion_handle *handle)
+{
+	int fd = get_unused_fd();
+	struct file *file;
+
+	if (fd < 0)
+		return -ENFILE;
+
+	file = anon_inode_getfile("ion_share_fd", &ion_share_fops,
+				  handle->buffer, O_RDWR);
+	if (IS_ERR_OR_NULL(file))
+		goto err;
+	ion_buffer_get(handle->buffer);
+	fd_install(fd, file);
+
+	return fd;
+
+err:
+	put_unused_fd(fd);
+	return -ENFILE;
+}
+
+static long ion_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	struct ion_client *client = filp->private_data;
+
+	switch (cmd) {
+	case ION_IOC_ALLOC:
+	{
+		struct ion_allocation_data data;
+
+		if (copy_from_user(&data, (void __user *)arg, sizeof(data)))
+			return -EFAULT;
+		data.handle = ion_alloc(client, data.len, data.align,
+					     data.flags);
+		if (copy_to_user((void __user *)arg, &data, sizeof(data)))
+			return -EFAULT;
+		break;
+	}
+	case ION_IOC_FREE:
+	{
+		struct ion_handle_data data;
+		bool valid;
+
+		if (copy_from_user(&data, (void __user *)arg,
+				   sizeof(struct ion_handle_data)))
+			return -EFAULT;
+		mutex_lock(&client->lock);
+		valid = ion_handle_validate(client, data.handle);
+		mutex_unlock(&client->lock);
+		if (!valid)
+			return -EINVAL;
+		ion_free(client, data.handle);
+		break;
+	}
+	case ION_IOC_MAP:
+	case ION_IOC_SHARE:
+	{
+		struct ion_fd_data data;
+
+		if (copy_from_user(&data, (void __user *)arg, sizeof(data)))
+			return -EFAULT;
+		mutex_lock(&client->lock);
+		if (!ion_handle_validate(client, data.handle)) {
+			WARN(1, "invalid handle passed to share ioctl.\n");
+			mutex_unlock(&client->lock);
+			return -EINVAL;
+		}
+		data.fd = ion_ioctl_share(filp, client, data.handle);
+		mutex_unlock(&client->lock);
+		if (copy_to_user((void __user *)arg, &data, sizeof(data)))
+			return -EFAULT;
+		break;
+	}
+	case ION_IOC_IMPORT:
+	{
+		struct ion_fd_data data;
+		if (copy_from_user(&data, (void __user *)arg,
+				   sizeof(struct ion_fd_data)))
+			return -EFAULT;
+
+		data.handle = ion_import_fd(client, data.fd);
+		if (IS_ERR(data.handle))
+			data.handle = NULL;
+		if (copy_to_user((void __user *)arg, &data,
+				 sizeof(struct ion_fd_data)))
+			return -EFAULT;
+		break;
+	}
+	case ION_IOC_CUSTOM:
+	{
+		struct ion_device *dev = client->dev;
+		struct ion_custom_data data;
+
+		if (!dev->custom_ioctl)
+			return -ENOTTY;
+		if (copy_from_user(&data, (void __user *)arg,
+				sizeof(struct ion_custom_data)))
+			return -EFAULT;
+		return dev->custom_ioctl(client, data.cmd, data.arg);
+	}
+	default:
+		return -ENOTTY;
+	}
+	return 0;
+}
+
+static int ion_release(struct inode *inode, struct file *file)
+{
+	struct ion_client *client = file->private_data;
+
+	pr_debug("\n");
+	ion_client_put(client);
+	return 0;
+}
+
+static int ion_open(struct inode *inode, struct file *file)
+{
+	struct miscdevice *miscdev = file->private_data;
+	struct ion_device *dev = container_of(miscdev, struct ion_device, dev);
+	struct ion_client *client;
+
+	pr_debug("\n");
+	client = ion_client_create(dev, -1, "user");
+	if (IS_ERR_OR_NULL(client))
+		return PTR_ERR(client);
+	file->private_data = client;
+
+	return 0;
+}
+
+static const struct file_operations ion_fops = {
+	.owner          = THIS_MODULE,
+	.open           = ion_open,
+	.release        = ion_release,
+	.unlocked_ioctl = ion_ioctl,
+};
+
+static size_t ion_debug_heap_total(struct ion_client *client,
+				   enum ion_heap_type type)
+{
+	size_t size = 0;
+	struct rb_node *n;
+
+	mutex_lock(&client->lock);
+	for (n = rb_first(&client->handles); n; n = rb_next(n)) {
+		struct ion_handle *handle = rb_entry(n,
+						     struct ion_handle,
+						     node);
+		if (handle->buffer->heap->type == type)
+			size += handle->buffer->size;
+	}
+	mutex_unlock(&client->lock);
+	return size;
+}
+
+static int ion_debug_heap_show(struct seq_file *s, void *unused)
+{
+	struct ion_heap *heap = s->private;
+	struct ion_device *dev = heap->dev;
+	struct rb_node *n;
+
+	seq_printf(s, "%16.s %16.s %16.s\n", "client", "pid", "size");
+	for (n = rb_first(&dev->user_clients); n; n = rb_next(n)) {
+		struct ion_client *client = rb_entry(n, struct ion_client,
+						     node);
+		char task_comm[TASK_COMM_LEN];
+		size_t size = ion_debug_heap_total(client, heap->type);
+		if (!size)
+			continue;
+
+		get_task_comm(task_comm, client->task);
+		seq_printf(s, "%16.s %16u %16u\n", task_comm, client->pid,
+			   size);
+	}
+
+	for (n = rb_first(&dev->kernel_clients); n; n = rb_next(n)) {
+		struct ion_client *client = rb_entry(n, struct ion_client,
+						     node);
+		size_t size = ion_debug_heap_total(client, heap->type);
+		if (!size)
+			continue;
+		seq_printf(s, "%16.s %16u %16u\n", client->name, client->pid,
+			   size);
+	}
+	return 0;
+}
+
+static int ion_debug_heap_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ion_debug_heap_show, inode->i_private);
+}
+
+static const struct file_operations debug_heap_fops = {
+	.open = ion_debug_heap_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+void ion_device_add_heap(struct ion_device *dev, struct ion_heap *heap)
+{
+	struct rb_node **p = &dev->heaps.rb_node;
+	struct rb_node *parent = NULL;
+	struct ion_heap *entry;
+
+	heap->dev = dev;
+	mutex_lock(&dev->lock);
+	while (*p) {
+		parent = *p;
+		entry = rb_entry(parent, struct ion_heap, node);
+
+		if (heap->id < entry->id) {
+			p = &(*p)->rb_left;
+		} else if (heap->id > entry->id ) {
+			p = &(*p)->rb_right;
+		} else {
+			pr_err("can not insert multiple heaps with "
+				"id %d\n", heap->id);
+			goto end;
+		}
+	}
+
+	rb_link_node(&heap->node, parent, p);
+	rb_insert_color(&heap->node, &dev->heaps);
+	debugfs_create_file(heap->name, 0664, dev->debug_root, heap,
+			    &debug_heap_fops);
+end:
+	mutex_unlock(&dev->lock);
+}
+
+struct ion_device *ion_device_create(long (*custom_ioctl)
+				     (struct ion_client *client,
+				      unsigned int cmd,
+				      unsigned long arg))
+{
+	struct ion_device *idev;
+	int ret;
+
+	idev = kzalloc(sizeof(struct ion_device), GFP_KERNEL);
+	if (!idev)
+		return ERR_PTR(-ENOMEM);
+
+	idev->dev.minor = MISC_DYNAMIC_MINOR;
+	idev->dev.name = "ion";
+	idev->dev.fops = &ion_fops;
+	idev->dev.parent = NULL;
+	ret = misc_register(&idev->dev);
+	if (ret) {
+		pr_err("ion: failed to register misc device.\n");
+		return ERR_PTR(ret);
+	}
+
+	idev->debug_root = debugfs_create_dir("ion", NULL);
+	if (IS_ERR_OR_NULL(idev->debug_root))
+		pr_err("ion: failed to create debug files.\n");
+
+	idev->custom_ioctl = custom_ioctl;
+	idev->buffers = RB_ROOT;
+	mutex_init(&idev->lock);
+	idev->heaps = RB_ROOT;
+	idev->user_clients = RB_ROOT;
+	idev->kernel_clients = RB_ROOT;
+	return idev;
+}
+
+void ion_device_destroy(struct ion_device *dev)
+{
+	misc_deregister(&dev->dev);
+	/* XXX need to free the heaps and clients ? */
+	kfree(dev);
+}
+
+struct ion_client *ion_client_get_file(int fd)
+{
+	struct ion_client *client = ERR_PTR(-EFAULT);
+	struct file *f = fget(fd);
+	if (!f)
+		return ERR_PTR(-EINVAL);
+
+	if (f->f_op == &ion_fops) {
+		client = f->private_data;
+		ion_client_get(client);
+	}
+
+	fput(f);
+	return client;
+}
diff --git a/drivers/gpu/ion/ion_carveout_heap.c b/drivers/gpu/ion/ion_carveout_heap.c
new file mode 100644
index 000000000000..606adae13f48
--- /dev/null
+++ b/drivers/gpu/ion/ion_carveout_heap.c
@@ -0,0 +1,162 @@
+/*
+ * drivers/gpu/ion/ion_carveout_heap.c
+ *
+ * Copyright (C) 2011 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/spinlock.h>
+
+#include <linux/err.h>
+#include <linux/genalloc.h>
+#include <linux/io.h>
+#include <linux/ion.h>
+#include <linux/mm.h>
+#include <linux/scatterlist.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include "ion_priv.h"
+
+#include <asm/mach/map.h>
+
+struct ion_carveout_heap {
+	struct ion_heap heap;
+	struct gen_pool *pool;
+	ion_phys_addr_t base;
+};
+
+ion_phys_addr_t ion_carveout_allocate(struct ion_heap *heap,
+				      unsigned long size,
+				      unsigned long align)
+{
+	struct ion_carveout_heap *carveout_heap =
+		container_of(heap, struct ion_carveout_heap, heap);
+	unsigned long offset = gen_pool_alloc(carveout_heap->pool, size);
+
+	if (!offset)
+		return ION_CARVEOUT_ALLOCATE_FAIL;
+
+	return offset;
+}
+
+void ion_carveout_free(struct ion_heap *heap, ion_phys_addr_t addr,
+		       unsigned long size)
+{
+	struct ion_carveout_heap *carveout_heap =
+		container_of(heap, struct ion_carveout_heap, heap);
+
+	if (addr == ION_CARVEOUT_ALLOCATE_FAIL)
+		return;
+	gen_pool_free(carveout_heap->pool, addr, size);
+}
+
+static int ion_carveout_heap_phys(struct ion_heap *heap,
+				  struct ion_buffer *buffer,
+				  ion_phys_addr_t *addr, size_t *len)
+{
+	*addr = buffer->priv_phys;
+	*len = buffer->size;
+	return 0;
+}
+
+static int ion_carveout_heap_allocate(struct ion_heap *heap,
+				      struct ion_buffer *buffer,
+				      unsigned long size, unsigned long align,
+				      unsigned long flags)
+{
+	buffer->priv_phys = ion_carveout_allocate(heap, size, align);
+	return buffer->priv_phys == ION_CARVEOUT_ALLOCATE_FAIL ? -ENOMEM : 0;
+}
+
+static void ion_carveout_heap_free(struct ion_buffer *buffer)
+{
+	struct ion_heap *heap = buffer->heap;
+
+	ion_carveout_free(heap, buffer->priv_phys, buffer->size);
+	buffer->priv_phys = ION_CARVEOUT_ALLOCATE_FAIL;
+}
+
+struct scatterlist *ion_carveout_heap_map_dma(struct ion_heap *heap,
+					      struct ion_buffer *buffer)
+{
+	return ERR_PTR(-EINVAL);
+}
+
+void ion_carveout_heap_unmap_dma(struct ion_heap *heap,
+				 struct ion_buffer *buffer)
+{
+	return;
+}
+
+void *ion_carveout_heap_map_kernel(struct ion_heap *heap,
+				   struct ion_buffer *buffer)
+{
+	return __arch_ioremap(buffer->priv_phys, buffer->size,
+			      MT_MEMORY_NONCACHED);
+}
+
+void ion_carveout_heap_unmap_kernel(struct ion_heap *heap,
+				    struct ion_buffer *buffer)
+{
+	__arch_iounmap(buffer->vaddr);
+	buffer->vaddr = NULL;
+	return;
+}
+
+int ion_carveout_heap_map_user(struct ion_heap *heap, struct ion_buffer *buffer,
+			       struct vm_area_struct *vma)
+{
+	return remap_pfn_range(vma, vma->vm_start,
+			       __phys_to_pfn(buffer->priv_phys) + vma->vm_pgoff,
+			       buffer->size,
+			       pgprot_noncached(vma->vm_page_prot));
+}
+
+static struct ion_heap_ops carveout_heap_ops = {
+	.allocate = ion_carveout_heap_allocate,
+	.free = ion_carveout_heap_free,
+	.phys = ion_carveout_heap_phys,
+	.map_user = ion_carveout_heap_map_user,
+	.map_kernel = ion_carveout_heap_map_kernel,
+	.unmap_kernel = ion_carveout_heap_unmap_kernel,
+};
+
+struct ion_heap *ion_carveout_heap_create(struct ion_platform_heap *heap_data)
+{
+	struct ion_carveout_heap *carveout_heap;
+
+	carveout_heap = kzalloc(sizeof(struct ion_carveout_heap), GFP_KERNEL);
+	if (!carveout_heap)
+		return ERR_PTR(-ENOMEM);
+
+	carveout_heap->pool = gen_pool_create(12, -1);
+	if (!carveout_heap->pool) {
+		kfree(carveout_heap);
+		return ERR_PTR(-ENOMEM);
+	}
+	carveout_heap->base = heap_data->base;
+	gen_pool_add(carveout_heap->pool, carveout_heap->base, heap_data->size,
+		     -1);
+	carveout_heap->heap.ops = &carveout_heap_ops;
+	carveout_heap->heap.type = ION_HEAP_TYPE_CARVEOUT;
+
+	return &carveout_heap->heap;
+}
+
+void ion_carveout_heap_destroy(struct ion_heap *heap)
+{
+	struct ion_carveout_heap *carveout_heap =
+	     container_of(heap, struct  ion_carveout_heap, heap);
+
+	gen_pool_destroy(carveout_heap->pool);
+	kfree(carveout_heap);
+	carveout_heap = NULL;
+}
diff --git a/drivers/gpu/ion/ion_heap.c b/drivers/gpu/ion/ion_heap.c
new file mode 100644
index 000000000000..6d09778745c6
--- /dev/null
+++ b/drivers/gpu/ion/ion_heap.c
@@ -0,0 +1,78 @@
+/*
+ * drivers/gpu/ion/ion_heap.c
+ *
+ * Copyright (C) 2011 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/err.h>
+#include <linux/ion.h>
+#include "ion_priv.h"
+
+struct ion_heap *ion_heap_create(struct ion_platform_heap *heap_data)
+{
+	struct ion_heap *heap = NULL;
+
+	switch (heap_data->type) {
+	case ION_HEAP_TYPE_SYSTEM_CONTIG:
+		heap = ion_system_contig_heap_create(heap_data);
+		break;
+	case ION_HEAP_TYPE_SYSTEM:
+		heap = ion_system_heap_create(heap_data);
+		break;
+	case ION_HEAP_TYPE_CARVEOUT:
+		heap = ion_carveout_heap_create(heap_data);
+		break;
+	case ION_HEAP_TYPE_IOMMU:
+		heap = ion_iommu_heap_create(heap_data);
+		break;
+	default:
+		pr_err("%s: Invalid heap type %d\n", __func__,
+		       heap_data->type);
+		return ERR_PTR(-EINVAL);
+	}
+
+	if (IS_ERR_OR_NULL(heap)) {
+		pr_err("%s: error creating heap %s type %d base %lu size %u\n",
+		       __func__, heap_data->name, heap_data->type,
+		       heap_data->base, heap_data->size);
+		return ERR_PTR(-EINVAL);
+	}
+
+	heap->name = heap_data->name;
+	heap->id = heap_data->id;
+	return heap;
+}
+
+void ion_heap_destroy(struct ion_heap *heap)
+{
+	if (!heap)
+		return;
+
+	switch (heap->type) {
+	case ION_HEAP_TYPE_SYSTEM_CONTIG:
+		ion_system_contig_heap_destroy(heap);
+		break;
+	case ION_HEAP_TYPE_SYSTEM:
+		ion_system_heap_destroy(heap);
+		break;
+	case ION_HEAP_TYPE_CARVEOUT:
+		ion_carveout_heap_destroy(heap);
+		break;
+	case ION_HEAP_TYPE_IOMMU:
+		ion_iommu_heap_destroy(heap);
+		break;
+	default:
+		pr_err("%s: Invalid heap type %d\n", __func__,
+		       heap->type);
+	}
+}
diff --git a/drivers/gpu/ion/ion_iommu_heap.c b/drivers/gpu/ion/ion_iommu_heap.c
new file mode 100644
index 000000000000..a3d2d726bda5
--- /dev/null
+++ b/drivers/gpu/ion/ion_iommu_heap.c
@@ -0,0 +1,382 @@
+/*
+ * drivers/gpu/ion/ion_iommu_heap.c
+ *
+ * Copyright (c) 2012, NVIDIA 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 it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#define pr_fmt(fmt)	"%s(): " fmt, __func__
+
+#include <linux/spinlock.h>
+#include <linux/kernel.h>
+#include <linux/genalloc.h>
+#include <linux/io.h>
+#include <linux/ion.h>
+#include <linux/mm.h>
+#include <linux/scatterlist.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/iommu.h>
+#include <linux/highmem.h>
+#include <linux/platform_device.h>
+
+#include <asm/cacheflush.h>
+
+#include "ion_priv.h"
+
+#define NUM_PAGES(buf)	(PAGE_ALIGN((buf)->size) >> PAGE_SHIFT)
+
+#define GFP_ION		(GFP_KERNEL | __GFP_HIGHMEM | __GFP_NOWARN)
+
+struct ion_iommu_heap {
+	struct ion_heap		heap;
+	struct gen_pool		*pool;
+	struct iommu_domain	*domain;
+	struct device		*dev;
+};
+
+static struct scatterlist *iommu_heap_map_dma(struct ion_heap *heap,
+					      struct ion_buffer *buf)
+{
+	struct ion_iommu_heap *h =
+		container_of(heap, struct ion_iommu_heap, heap);
+	int err, npages = NUM_PAGES(buf);
+	unsigned int i;
+	struct scatterlist *sg;
+	unsigned long da = (unsigned long)buf->priv_virt;
+
+	for_each_sg(buf->sglist, sg, npages, i) {
+		phys_addr_t pa;
+
+		pa = sg_phys(sg);
+		BUG_ON(!IS_ALIGNED(sg->length, PAGE_SIZE));
+		err = iommu_map(h->domain, da, pa, PAGE_SIZE, 0);
+		if (err)
+			goto err_out;
+
+		sg->dma_address = da;
+		da += PAGE_SIZE;
+	}
+
+	pr_debug("da:%p pa:%08x va:%p\n",
+		 buf->priv_virt, sg_phys(buf->sglist), buf->vaddr);
+
+	return buf->sglist;
+
+err_out:
+	if (i-- > 0) {
+		unsigned int j;
+		for_each_sg(buf->sglist, sg, i, j)
+			iommu_unmap(h->domain, sg_dma_address(sg), 0);
+	}
+	return ERR_PTR(err);
+}
+
+static void iommu_heap_unmap_dma(struct ion_heap *heap, struct ion_buffer *buf)
+{
+	struct ion_iommu_heap *h =
+		container_of(heap, struct ion_iommu_heap, heap);
+	unsigned int i;
+	struct scatterlist *sg;
+	int npages = NUM_PAGES(buf);
+
+	for_each_sg(buf->sglist, sg, npages, i)
+		iommu_unmap(h->domain, sg_dma_address(sg), 0);
+
+	pr_debug("da:%p\n", buf->priv_virt);
+}
+
+struct scatterlist *iommu_heap_remap_dma(struct ion_heap *heap,
+					      struct ion_buffer *buf,
+					      unsigned long addr)
+{
+	struct ion_iommu_heap *h =
+		container_of(heap, struct ion_iommu_heap, heap);
+	int err;
+	unsigned int i;
+	unsigned long da, da_to_free = (unsigned long)buf->priv_virt;
+	int npages = NUM_PAGES(buf);
+
+	BUG_ON(!buf->priv_virt);
+
+	da = gen_pool_alloc_addr(h->pool, buf->size, addr);
+	if (da == 0) {
+		pr_err("dma address alloc failed, addr=0x%lx", addr);
+		return ERR_PTR(-ENOMEM);
+	} else {
+		pr_err("iommu_heap_remap_dma passed, addr=0x%lx",
+			addr);
+		iommu_heap_unmap_dma(heap, buf);
+		gen_pool_free(h->pool, da_to_free, buf->size);
+		buf->priv_virt = (void *)da;
+	}
+	for (i = 0; i < npages; i++) {
+		phys_addr_t pa;
+
+		pa = page_to_phys(buf->pages[i]);
+		err = iommu_map(h->domain, da, pa, 0, 0);
+		if (err)
+			goto err_out;
+		da += PAGE_SIZE;
+	}
+
+	pr_debug("da:%p pa:%08x va:%p\n",
+		 buf->priv_virt, page_to_phys(buf->pages[0]), buf->vaddr);
+
+	return (struct scatterlist *)buf->pages;
+
+err_out:
+	if (i-- > 0) {
+		da = (unsigned long)buf->priv_virt;
+		iommu_unmap(h->domain, da + (i << PAGE_SHIFT), 0);
+	}
+	return ERR_PTR(err);
+}
+
+static int ion_buffer_allocate(struct ion_buffer *buf)
+{
+	int i, npages = NUM_PAGES(buf);
+
+	buf->pages = kmalloc(npages * sizeof(*buf->pages), GFP_KERNEL);
+	if (!buf->pages)
+		goto err_pages;
+
+	buf->sglist = vzalloc(npages * sizeof(*buf->sglist));
+	if (!buf->sglist)
+		goto err_sgl;
+
+	sg_init_table(buf->sglist, npages);
+
+	for (i = 0; i < npages; i++) {
+		struct page *page;
+		phys_addr_t pa;
+
+		page = alloc_page(GFP_ION);
+		if (!page)
+			goto err_pgalloc;
+		pa = page_to_phys(page);
+
+		sg_set_page(&buf->sglist[i], page, PAGE_SIZE, 0);
+
+		flush_dcache_page(page);
+		outer_flush_range(pa, pa + PAGE_SIZE);
+
+		buf->pages[i] = page;
+
+		pr_debug_once("pa:%08x\n", pa);
+	}
+	return 0;
+
+err_pgalloc:
+	while (i-- > 0)
+		__free_page(buf->pages[i]);
+	vfree(buf->sglist);
+err_sgl:
+	kfree(buf->pages);
+err_pages:
+	return -ENOMEM;
+}
+
+static void ion_buffer_free(struct ion_buffer *buf)
+{
+	int i, npages = NUM_PAGES(buf);
+
+	for (i = 0; i < npages; i++)
+		__free_page(buf->pages[i]);
+	vfree(buf->sglist);
+	kfree(buf->pages);
+}
+
+static int iommu_heap_allocate(struct ion_heap *heap, struct ion_buffer *buf,
+			       unsigned long len, unsigned long align,
+			       unsigned long flags)
+{
+	int err;
+	struct ion_iommu_heap *h =
+		container_of(heap, struct ion_iommu_heap, heap);
+	unsigned long da;
+	struct scatterlist *sgl;
+
+	len = round_up(len, PAGE_SIZE);
+
+	da = gen_pool_alloc(h->pool, len);
+	if (!da)
+		return -ENOMEM;
+
+	buf->priv_virt = (void *)da;
+	buf->size = len;
+
+	WARN_ON(!IS_ALIGNED(da, PAGE_SIZE));
+
+	err = ion_buffer_allocate(buf);
+	if (err)
+		goto err_alloc_buf;
+
+	sgl = iommu_heap_map_dma(heap, buf);
+	if (IS_ERR_OR_NULL(sgl))
+		goto err_heap_map_dma;
+	buf->vaddr = 0;
+	return 0;
+
+err_heap_map_dma:
+	ion_buffer_free(buf);
+err_alloc_buf:
+	gen_pool_free(h->pool, da, len);
+	buf->size = 0;
+	buf->pages = NULL;
+	buf->priv_virt = NULL;
+	return err;
+}
+
+static void iommu_heap_free(struct ion_buffer *buf)
+{
+	struct ion_heap *heap = buf->heap;
+	struct ion_iommu_heap *h =
+		container_of(heap, struct ion_iommu_heap, heap);
+	void *da = buf->priv_virt;
+
+	iommu_heap_unmap_dma(heap, buf);
+	ion_buffer_free(buf);
+	gen_pool_free(h->pool, (unsigned long)da, buf->size);
+
+	buf->pages = NULL;
+	buf->priv_virt = NULL;
+	pr_debug("da:%p\n", da);
+}
+
+static int iommu_heap_phys(struct ion_heap *heap, struct ion_buffer *buf,
+			   ion_phys_addr_t *addr, size_t *len)
+{
+	*addr = (unsigned long)buf->priv_virt;
+	*len = buf->size;
+	pr_debug("da:%08lx(%x)\n", *addr, *len);
+	return 0;
+}
+
+static void *iommu_heap_map_kernel(struct ion_heap *heap,
+				   struct ion_buffer *buf)
+{
+	int npages = NUM_PAGES(buf);
+
+	BUG_ON(!buf->pages);
+	buf->vaddr = vm_map_ram(buf->pages, npages, -1,
+				pgprot_noncached(pgprot_kernel));
+	pr_debug("va:%p\n", buf->vaddr);
+	WARN_ON(!buf->vaddr);
+	return buf->vaddr;
+}
+
+static void iommu_heap_unmap_kernel(struct ion_heap *heap,
+				    struct ion_buffer *buf)
+{
+	int npages = NUM_PAGES(buf);
+
+	BUG_ON(!buf->pages);
+	WARN_ON(!buf->vaddr);
+	vm_unmap_ram(buf->vaddr, npages);
+	buf->vaddr = NULL;
+	pr_debug("va:%p\n", buf->vaddr);
+}
+
+static int iommu_heap_map_user(struct ion_heap *mapper,
+			       struct ion_buffer *buf,
+			       struct vm_area_struct *vma)
+{
+	int i = vma->vm_pgoff >> PAGE_SHIFT;
+	unsigned long uaddr = vma->vm_start;
+	unsigned long usize = vma->vm_end - vma->vm_start;
+
+	pr_debug("vma:%08lx-%08lx\n", vma->vm_start, vma->vm_end);
+	BUG_ON(!buf->pages);
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	do {
+		int ret;
+		struct page *page = buf->pages[i++];
+
+		ret = vm_insert_page(vma, uaddr, page);
+		if (ret)
+			return ret;
+
+		uaddr += PAGE_SIZE;
+		usize -= PAGE_SIZE;
+	} while (usize > 0);
+
+	return 0;
+}
+
+static struct ion_heap_ops iommu_heap_ops = {
+	.allocate	= iommu_heap_allocate,
+	.free		= iommu_heap_free,
+	.phys		= iommu_heap_phys,
+	.map_dma	= iommu_heap_map_dma,
+	.unmap_dma	= iommu_heap_unmap_dma,
+	.map_kernel	= iommu_heap_map_kernel,
+	.unmap_kernel	= iommu_heap_unmap_kernel,
+	.map_user	= iommu_heap_map_user,
+};
+
+struct ion_heap *ion_iommu_heap_create(struct ion_platform_heap *data)
+{
+	struct ion_iommu_heap *h;
+	int err;
+
+	h = kzalloc(sizeof(*h), GFP_KERNEL);
+	if (!h) {
+		err = -ENOMEM;
+		goto err_heap;
+	}
+
+	h->pool = gen_pool_create(12, -1);
+	if (!h->pool) {
+		err = -ENOMEM;
+		goto err_genpool;
+	}
+	gen_pool_add(h->pool, data->base, data->size, -1);
+
+	h->heap.ops = &iommu_heap_ops;
+	h->domain = iommu_domain_alloc(&platform_bus_type);
+	h->dev = data->priv;
+	if (!h->domain) {
+		err = -ENOMEM;
+		goto err_iommu_alloc;
+	}
+
+	err = iommu_attach_device(h->domain, h->dev);
+	if (err)
+		goto err_iommu_attach;
+
+	return &h->heap;
+
+err_iommu_attach:
+	iommu_domain_free(h->domain);
+err_iommu_alloc:
+	gen_pool_destroy(h->pool);
+err_genpool:
+	kfree(h);
+err_heap:
+	return ERR_PTR(err);
+}
+
+void ion_iommu_heap_destroy(struct ion_heap *heap)
+{
+	struct ion_iommu_heap *h =
+		container_of(heap, struct  ion_iommu_heap, heap);
+
+	iommu_detach_device(h->domain, h->dev);
+	gen_pool_destroy(h->pool);
+	iommu_domain_free(h->domain);
+	kfree(h);
+}
diff --git a/drivers/gpu/ion/ion_priv.h b/drivers/gpu/ion/ion_priv.h
new file mode 100644
index 000000000000..bfe26da9c049
--- /dev/null
+++ b/drivers/gpu/ion/ion_priv.h
@@ -0,0 +1,293 @@
+/*
+ * drivers/gpu/ion/ion_priv.h
+ *
+ * Copyright (C) 2011 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 _ION_PRIV_H
+#define _ION_PRIV_H
+
+#include <linux/kref.h>
+#include <linux/mm_types.h>
+#include <linux/mutex.h>
+#include <linux/rbtree.h>
+#include <linux/ion.h>
+#include <linux/miscdevice.h>
+
+struct ion_mapping;
+
+struct ion_dma_mapping {
+	struct kref ref;
+	struct scatterlist *sglist;
+};
+
+struct ion_kernel_mapping {
+	struct kref ref;
+	void *vaddr;
+};
+
+/**
+ * struct ion_device - the metadata of the ion device node
+ * @dev:		the actual misc device
+ * @buffers:	an rb tree of all the existing buffers
+ * @lock:		lock protecting the buffers & heaps trees
+ * @heaps:		list of all the heaps in the system
+ * @user_clients:	list of all the clients created from userspace
+ */
+struct ion_device {
+	struct miscdevice dev;
+	struct rb_root buffers;
+	struct mutex lock;
+	struct rb_root heaps;
+	long (*custom_ioctl) (struct ion_client *client, unsigned int cmd,
+			      unsigned long arg);
+	struct rb_root user_clients;
+	struct rb_root kernel_clients;
+	struct dentry *debug_root;
+};
+
+/**
+ * struct ion_client - a process/hw block local address space
+ * @ref:		for reference counting the client
+ * @node:		node in the tree of all clients
+ * @dev:		backpointer to ion device
+ * @handles:		an rb tree of all the handles in this client
+ * @lock:		lock protecting the tree of handles
+ * @heap_mask:		mask of all supported heaps
+ * @name:		used for debugging
+ * @task:		used for debugging
+ *
+ * A client represents a list of buffers this client may access.
+ * The mutex stored here is used to protect both handles tree
+ * as well as the handles themselves, and should be held while modifying either.
+ */
+struct ion_client {
+	struct kref ref;
+	struct rb_node node;
+	struct ion_device *dev;
+	struct rb_root handles;
+	struct mutex lock;
+	unsigned int heap_mask;
+	const char *name;
+	struct task_struct *task;
+	pid_t pid;
+	struct dentry *debug_root;
+};
+
+/**
+ * ion_handle - a client local reference to a buffer
+ * @ref:		reference count
+ * @client:		back pointer to the client the buffer resides in
+ * @buffer:		pointer to the buffer
+ * @node:		node in the client's handle rbtree
+ * @kmap_cnt:		count of times this client has mapped to kernel
+ * @dmap_cnt:		count of times this client has mapped for dma
+ * @usermap_cnt:	count of times this client has mapped for userspace
+ *
+ * Modifications to node, map_cnt or mapping should be protected by the
+ * lock in the client.  Other fields are never changed after initialization.
+ */
+struct ion_handle {
+	struct kref ref;
+	struct ion_client *client;
+	struct ion_buffer *buffer;
+	struct rb_node node;
+	unsigned int kmap_cnt;
+	unsigned int dmap_cnt;
+	unsigned int usermap_cnt;
+};
+
+bool ion_handle_validate(struct ion_client *client, struct ion_handle *handle);
+
+void ion_buffer_get(struct ion_buffer *buffer);
+
+struct ion_buffer *ion_handle_buffer(struct ion_handle *handle);
+
+struct ion_client *ion_client_get_file(int fd);
+
+void ion_client_get(struct ion_client *client);
+
+int ion_client_put(struct ion_client *client);
+
+void ion_handle_get(struct ion_handle *handle);
+
+int ion_handle_put(struct ion_handle *handle);
+
+struct ion_handle *ion_handle_create(struct ion_client *client,
+				     struct ion_buffer *buffer);
+
+void ion_handle_add(struct ion_client *client, struct ion_handle *handle);
+
+int ion_remap_dma(struct ion_client *client,
+		    struct ion_handle *handle,
+		    unsigned long addr);
+/**
+ * struct ion_buffer - metadata for a particular buffer
+ * @ref:		refernce count
+ * @node:		node in the ion_device buffers tree
+ * @dev:		back pointer to the ion_device
+ * @heap:		back pointer to the heap the buffer came from
+ * @flags:		buffer specific flags
+ * @size:		size of the buffer
+ * @priv_virt:		private data to the buffer representable as
+ *			a void *
+ * @priv_phys:		private data to the buffer representable as
+ *			an ion_phys_addr_t (and someday a phys_addr_t)
+ * @lock:		protects the buffers cnt fields
+ * @kmap_cnt:		number of times the buffer is mapped to the kernel
+ * @vaddr:		the kenrel mapping if kmap_cnt is not zero
+ * @dmap_cnt:		number of times the buffer is mapped for dma
+ * @sglist:		the scatterlist for the buffer is dmap_cnt is not zero
+ * @pages:		list for allocated pages for the buffer
+ */
+struct ion_buffer {
+	struct kref ref;
+	struct rb_node node;
+	struct ion_device *dev;
+	struct ion_heap *heap;
+	unsigned long flags;
+	size_t size;
+	union {
+		void *priv_virt;
+		ion_phys_addr_t priv_phys;
+	};
+	struct mutex lock;
+	int kmap_cnt;
+	void *vaddr;
+	int dmap_cnt;
+	struct scatterlist *sglist;
+	struct page **pages;
+};
+
+/**
+ * struct ion_heap_ops - ops to operate on a given heap
+ * @allocate:		allocate memory
+ * @free:		free memory
+ * @phys		get physical address of a buffer (only define on
+ *			physically contiguous heaps)
+ * @map_dma		map the memory for dma to a scatterlist
+ * @unmap_dma		unmap the memory for dma
+ * @map_kernel		map memory to the kernel
+ * @unmap_kernel	unmap memory to the kernel
+ * @map_user		map memory to userspace
+ */
+struct ion_heap_ops {
+	int (*allocate) (struct ion_heap *heap,
+			 struct ion_buffer *buffer, unsigned long len,
+			 unsigned long align, unsigned long flags);
+	void (*free) (struct ion_buffer *buffer);
+	int (*phys) (struct ion_heap *heap, struct ion_buffer *buffer,
+		     ion_phys_addr_t *addr, size_t *len);
+	struct scatterlist *(*map_dma) (struct ion_heap *heap,
+					struct ion_buffer *buffer);
+	void (*unmap_dma) (struct ion_heap *heap, struct ion_buffer *buffer);
+	void * (*map_kernel) (struct ion_heap *heap, struct ion_buffer *buffer);
+	void (*unmap_kernel) (struct ion_heap *heap, struct ion_buffer *buffer);
+	int (*map_user) (struct ion_heap *mapper, struct ion_buffer *buffer,
+			 struct vm_area_struct *vma);
+};
+
+/**
+ * struct ion_heap - represents a heap in the system
+ * @node:		rb node to put the heap on the device's tree of heaps
+ * @dev:		back pointer to the ion_device
+ * @type:		type of heap
+ * @ops:		ops struct as above
+ * @id:			id of heap, also indicates priority of this heap when
+ *			allocating.  These are specified by platform data and
+ *			MUST be unique
+ * @name:		used for debugging
+ *
+ * Represents a pool of memory from which buffers can be made.  In some
+ * systems the only heap is regular system memory allocated via vmalloc.
+ * On others, some blocks might require large physically contiguous buffers
+ * that are allocated from a specially reserved heap.
+ */
+struct ion_heap {
+	struct rb_node node;
+	struct ion_device *dev;
+	enum ion_heap_type type;
+	struct ion_heap_ops *ops;
+	int id;
+	const char *name;
+};
+
+/**
+ * ion_device_create - allocates and returns an ion device
+ * @custom_ioctl:	arch specific ioctl function if applicable
+ *
+ * returns a valid device or -PTR_ERR
+ */
+struct ion_device *ion_device_create(long (*custom_ioctl)
+				     (struct ion_client *client,
+				      unsigned int cmd,
+				      unsigned long arg));
+
+/**
+ * ion_device_destroy - free and device and it's resource
+ * @dev:		the device
+ */
+void ion_device_destroy(struct ion_device *dev);
+
+/**
+ * ion_device_add_heap - adds a heap to the ion device
+ * @dev:		the device
+ * @heap:		the heap to add
+ */
+void ion_device_add_heap(struct ion_device *dev, struct ion_heap *heap);
+
+/**
+ * functions for creating and destroying the built in ion heaps.
+ * architectures can add their own custom architecture specific
+ * heaps as appropriate.
+ */
+
+struct ion_heap *ion_heap_create(struct ion_platform_heap *);
+void ion_heap_destroy(struct ion_heap *);
+
+struct ion_heap *ion_system_heap_create(struct ion_platform_heap *);
+void ion_system_heap_destroy(struct ion_heap *);
+
+struct ion_heap *ion_system_contig_heap_create(struct ion_platform_heap *);
+void ion_system_contig_heap_destroy(struct ion_heap *);
+
+struct ion_heap *ion_carveout_heap_create(struct ion_platform_heap *);
+void ion_carveout_heap_destroy(struct ion_heap *);
+/**
+ * kernel api to allocate/free from carveout -- used when carveout is
+ * used to back an architecture specific custom heap
+ */
+ion_phys_addr_t ion_carveout_allocate(struct ion_heap *heap, unsigned long size,
+				      unsigned long align);
+void ion_carveout_free(struct ion_heap *heap, ion_phys_addr_t addr,
+		       unsigned long size);
+
+#ifdef CONFIG_ION_IOMMU
+struct ion_heap *ion_iommu_heap_create(struct ion_platform_heap *);
+void ion_iommu_heap_destroy(struct ion_heap *);
+#else
+static inline struct ion_heap *ion_iommu_heap_create(struct ion_platform_heap *)
+{
+	return NULL;
+}
+static inline void ion_iommu_heap_destroy(struct ion_heap *)
+{
+}
+#endif
+/**
+ * The carveout heap returns physical addresses, since 0 may be a valid
+ * physical address, this is used to indicate allocation failed
+ */
+#define ION_CARVEOUT_ALLOCATE_FAIL -1
+
+#endif /* _ION_PRIV_H */
diff --git a/drivers/gpu/ion/ion_system_heap.c b/drivers/gpu/ion/ion_system_heap.c
new file mode 100644
index 000000000000..c046cf1a3219
--- /dev/null
+++ b/drivers/gpu/ion/ion_system_heap.c
@@ -0,0 +1,198 @@
+/*
+ * drivers/gpu/ion/ion_system_heap.c
+ *
+ * Copyright (C) 2011 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/err.h>
+#include <linux/ion.h>
+#include <linux/mm.h>
+#include <linux/scatterlist.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include "ion_priv.h"
+
+static int ion_system_heap_allocate(struct ion_heap *heap,
+				     struct ion_buffer *buffer,
+				     unsigned long size, unsigned long align,
+				     unsigned long flags)
+{
+	buffer->priv_virt = vmalloc_user(size);
+	if (!buffer->priv_virt)
+		return -ENOMEM;
+	return 0;
+}
+
+void ion_system_heap_free(struct ion_buffer *buffer)
+{
+	vfree(buffer->priv_virt);
+}
+
+struct scatterlist *ion_system_heap_map_dma(struct ion_heap *heap,
+					    struct ion_buffer *buffer)
+{
+	struct scatterlist *sglist;
+	struct page *page;
+	int i;
+	int npages = PAGE_ALIGN(buffer->size) / PAGE_SIZE;
+	void *vaddr = buffer->priv_virt;
+
+	sglist = vmalloc(npages * sizeof(struct scatterlist));
+	if (!sglist)
+		return ERR_PTR(-ENOMEM);
+	memset(sglist, 0, npages * sizeof(struct scatterlist));
+	sg_init_table(sglist, npages);
+	for (i = 0; i < npages; i++) {
+		page = vmalloc_to_page(vaddr);
+		if (!page)
+			goto end;
+		sg_set_page(&sglist[i], page, PAGE_SIZE, 0);
+		vaddr += PAGE_SIZE;
+	}
+	/* XXX do cache maintenance for dma? */
+	return sglist;
+end:
+	vfree(sglist);
+	return NULL;
+}
+
+void ion_system_heap_unmap_dma(struct ion_heap *heap,
+			       struct ion_buffer *buffer)
+{
+	/* XXX undo cache maintenance for dma? */
+	if (buffer->sglist)
+		vfree(buffer->sglist);
+}
+
+void *ion_system_heap_map_kernel(struct ion_heap *heap,
+				 struct ion_buffer *buffer)
+{
+	return buffer->priv_virt;
+}
+
+void ion_system_heap_unmap_kernel(struct ion_heap *heap,
+				  struct ion_buffer *buffer)
+{
+}
+
+int ion_system_heap_map_user(struct ion_heap *heap, struct ion_buffer *buffer,
+			     struct vm_area_struct *vma)
+{
+	return remap_vmalloc_range(vma, buffer->priv_virt, vma->vm_pgoff);
+}
+
+static struct ion_heap_ops vmalloc_ops = {
+	.allocate = ion_system_heap_allocate,
+	.free = ion_system_heap_free,
+	.map_dma = ion_system_heap_map_dma,
+	.unmap_dma = ion_system_heap_unmap_dma,
+	.map_kernel = ion_system_heap_map_kernel,
+	.unmap_kernel = ion_system_heap_unmap_kernel,
+	.map_user = ion_system_heap_map_user,
+};
+
+struct ion_heap *ion_system_heap_create(struct ion_platform_heap *unused)
+{
+	struct ion_heap *heap;
+
+	heap = kzalloc(sizeof(struct ion_heap), GFP_KERNEL);
+	if (!heap)
+		return ERR_PTR(-ENOMEM);
+	heap->ops = &vmalloc_ops;
+	heap->type = ION_HEAP_TYPE_SYSTEM;
+	return heap;
+}
+
+void ion_system_heap_destroy(struct ion_heap *heap)
+{
+	kfree(heap);
+}
+
+static int ion_system_contig_heap_allocate(struct ion_heap *heap,
+					   struct ion_buffer *buffer,
+					   unsigned long len,
+					   unsigned long align,
+					   unsigned long flags)
+{
+	buffer->priv_virt = kzalloc(len, GFP_KERNEL);
+	if (!buffer->priv_virt)
+		return -ENOMEM;
+	return 0;
+}
+
+void ion_system_contig_heap_free(struct ion_buffer *buffer)
+{
+	kfree(buffer->priv_virt);
+}
+
+static int ion_system_contig_heap_phys(struct ion_heap *heap,
+				       struct ion_buffer *buffer,
+				       ion_phys_addr_t *addr, size_t *len)
+{
+	*addr = virt_to_phys(buffer->priv_virt);
+	*len = buffer->size;
+	return 0;
+}
+
+struct scatterlist *ion_system_contig_heap_map_dma(struct ion_heap *heap,
+						   struct ion_buffer *buffer)
+{
+	struct scatterlist *sglist;
+
+	sglist = vmalloc(sizeof(struct scatterlist));
+	if (!sglist)
+		return ERR_PTR(-ENOMEM);
+	sg_init_table(sglist, 1);
+	sg_set_page(sglist, virt_to_page(buffer->priv_virt), buffer->size, 0);
+	return sglist;
+}
+
+int ion_system_contig_heap_map_user(struct ion_heap *heap,
+				    struct ion_buffer *buffer,
+				    struct vm_area_struct *vma)
+{
+	unsigned long pfn = __phys_to_pfn(virt_to_phys(buffer->priv_virt));
+	return remap_pfn_range(vma, vma->vm_start, pfn + vma->vm_pgoff,
+			       vma->vm_end - vma->vm_start,
+			       vma->vm_page_prot);
+
+}
+
+static struct ion_heap_ops kmalloc_ops = {
+	.allocate = ion_system_contig_heap_allocate,
+	.free = ion_system_contig_heap_free,
+	.phys = ion_system_contig_heap_phys,
+	.map_dma = ion_system_contig_heap_map_dma,
+	.unmap_dma = ion_system_heap_unmap_dma,
+	.map_kernel = ion_system_heap_map_kernel,
+	.unmap_kernel = ion_system_heap_unmap_kernel,
+	.map_user = ion_system_contig_heap_map_user,
+};
+
+struct ion_heap *ion_system_contig_heap_create(struct ion_platform_heap *unused)
+{
+	struct ion_heap *heap;
+
+	heap = kzalloc(sizeof(struct ion_heap), GFP_KERNEL);
+	if (!heap)
+		return ERR_PTR(-ENOMEM);
+	heap->ops = &kmalloc_ops;
+	heap->type = ION_HEAP_TYPE_SYSTEM_CONTIG;
+	return heap;
+}
+
+void ion_system_contig_heap_destroy(struct ion_heap *heap)
+{
+	kfree(heap);
+}
+
diff --git a/drivers/gpu/ion/ion_system_mapper.c b/drivers/gpu/ion/ion_system_mapper.c
new file mode 100644
index 000000000000..692458e07b5e
--- /dev/null
+++ b/drivers/gpu/ion/ion_system_mapper.c
@@ -0,0 +1,114 @@
+/*
+ * drivers/gpu/ion/ion_system_mapper.c
+ *
+ * Copyright (C) 2011 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/err.h>
+#include <linux/ion.h>
+#include <linux/memory.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include "ion_priv.h"
+/*
+ * This mapper is valid for any heap that allocates memory that already has
+ * a kernel mapping, this includes vmalloc'd memory, kmalloc'd memory,
+ * pages obtained via io_remap, etc.
+ */
+static void *ion_kernel_mapper_map(struct ion_mapper *mapper,
+				   struct ion_buffer *buffer,
+				   struct ion_mapping **mapping)
+{
+	if (!((1 << buffer->heap->type) & mapper->heap_mask)) {
+		pr_err("%s: attempting to map an unsupported heap\n", __func__);
+		return ERR_PTR(-EINVAL);
+	}
+	/* XXX REVISIT ME!!! */
+	*((unsigned long *)mapping) = (unsigned long)buffer->priv;
+	return buffer->priv;
+}
+
+static void ion_kernel_mapper_unmap(struct ion_mapper *mapper,
+				    struct ion_buffer *buffer,
+				    struct ion_mapping *mapping)
+{
+	if (!((1 << buffer->heap->type) & mapper->heap_mask))
+		pr_err("%s: attempting to unmap an unsupported heap\n",
+		       __func__);
+}
+
+static void *ion_kernel_mapper_map_kernel(struct ion_mapper *mapper,
+					struct ion_buffer *buffer,
+					struct ion_mapping *mapping)
+{
+	if (!((1 << buffer->heap->type) & mapper->heap_mask)) {
+		pr_err("%s: attempting to unmap an unsupported heap\n",
+		       __func__);
+		return ERR_PTR(-EINVAL);
+	}
+	return buffer->priv;
+}
+
+static int ion_kernel_mapper_map_user(struct ion_mapper *mapper,
+				      struct ion_buffer *buffer,
+				      struct vm_area_struct *vma,
+				      struct ion_mapping *mapping)
+{
+	int ret;
+
+	switch (buffer->heap->type) {
+	case ION_HEAP_KMALLOC:
+	{
+		unsigned long pfn = __phys_to_pfn(virt_to_phys(buffer->priv));
+		ret = remap_pfn_range(vma, vma->vm_start, pfn + vma->vm_pgoff,
+				      vma->vm_end - vma->vm_start,
+				      vma->vm_page_prot);
+		break;
+	}
+	case ION_HEAP_VMALLOC:
+		ret = remap_vmalloc_range(vma, buffer->priv, vma->vm_pgoff);
+		break;
+	default:
+		pr_err("%s: attempting to map unsupported heap to userspace\n",
+		       __func__);
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static struct ion_mapper_ops ops = {
+	.map = ion_kernel_mapper_map,
+	.map_kernel = ion_kernel_mapper_map_kernel,
+	.map_user = ion_kernel_mapper_map_user,
+	.unmap = ion_kernel_mapper_unmap,
+};
+
+struct ion_mapper *ion_system_mapper_create(void)
+{
+	struct ion_mapper *mapper;
+	mapper = kzalloc(sizeof(struct ion_mapper), GFP_KERNEL);
+	if (!mapper)
+		return ERR_PTR(-ENOMEM);
+	mapper->type = ION_SYSTEM_MAPPER;
+	mapper->ops = &ops;
+	mapper->heap_mask = (1 << ION_HEAP_VMALLOC) | (1 << ION_HEAP_KMALLOC);
+	return mapper;
+}
+
+void ion_system_mapper_destroy(struct ion_mapper *mapper)
+{
+	kfree(mapper);
+}
+
diff --git a/drivers/gpu/ion/tegra/Makefile b/drivers/gpu/ion/tegra/Makefile
new file mode 100644
index 000000000000..0fe898c6ee49
--- /dev/null
+++ b/drivers/gpu/ion/tegra/Makefile
@@ -0,0 +1,3 @@
+subdir-ccflags-y := -Werror
+
+obj-y += tegra_ion.o
diff --git a/drivers/gpu/ion/tegra/tegra_ion.c b/drivers/gpu/ion/tegra/tegra_ion.c
new file mode 100644
index 000000000000..e929ec84fc53
--- /dev/null
+++ b/drivers/gpu/ion/tegra/tegra_ion.c
@@ -0,0 +1,599 @@
+/*
+ * drivers/gpu/tegra/tegra_ion.c
+ *
+ * Copyright (C) 2011 Google, Inc.
+ * Copyright (C) 2011, NVIDIA Corporation.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#define pr_fmt(fmt)	"%s():%d: " fmt, __func__, __LINE__
+
+#include <linux/err.h>
+#include <linux/ion.h>
+#include <linux/tegra_ion.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/syscalls.h>
+#include <linux/io.h>
+#include "../ion_priv.h"
+
+#define CLIENT_HEAP_MASK 0xFFFFFFFF
+#define HEAP_FLAGS 0xFF
+
+#if !defined(CONFIG_TEGRA_NVMAP)
+#include "linux/nvmap.h"
+struct nvmap_device *nvmap_dev;
+#endif
+
+static struct ion_device *idev;
+static int num_heaps;
+static struct ion_heap **heaps;
+
+static int tegra_ion_pin(struct ion_client *client,
+				unsigned int cmd,
+				unsigned long arg)
+{
+	struct tegra_ion_pin_data data;
+	int ret;
+	struct ion_handle *on_stack[16];
+	struct ion_handle **refs = on_stack;
+	int i;
+	bool valid_handle;
+
+	if (copy_from_user(&data, (void __user *)arg, sizeof(data)))
+		return -EFAULT;
+	if (data.count) {
+		size_t bytes = data.count * sizeof(struct ion_handle *);
+
+		if (data.count > ARRAY_SIZE(on_stack))
+			refs = kmalloc(data.count * sizeof(*refs), GFP_KERNEL);
+		else
+			refs = on_stack;
+		if (!refs)
+			return -ENOMEM;
+		if (copy_from_user(refs, (void *)data.handles, bytes)) {
+			ret = -EFAULT;
+			goto err;
+		}
+	} else
+		return -EINVAL;
+
+	mutex_lock(&client->lock);
+	for (i = 0; i < data.count; i++) {
+		/* Ignore NULL pointers during unpin operation. */
+		if (!refs[i] && cmd == TEGRA_ION_UNPIN)
+			continue;
+		valid_handle = ion_handle_validate(client, refs[i]);
+		if (!valid_handle) {
+			WARN(1, "invalid handle passed h=0x%x", (u32)refs[i]);
+			mutex_unlock(&client->lock);
+			ret = -EINVAL;
+			goto err;
+		}
+	}
+	mutex_unlock(&client->lock);
+
+	if (cmd == TEGRA_ION_PIN) {
+		ion_phys_addr_t addr;
+		size_t len;
+
+		for (i = 0; i < data.count; i++) {
+			ret = ion_phys(client, refs[i], &addr, &len);
+			if (ret)
+				goto err;
+			ion_handle_get(refs[i]);
+			ret = put_user(addr, &data.addr[i]);
+			if (ret)
+				return ret;
+		}
+	} else if (cmd == TEGRA_ION_UNPIN) {
+		for (i = 0; i < data.count; i++) {
+			if (refs[i])
+				ion_handle_put(refs[i]);
+		}
+	}
+
+err:
+	if (ret) {
+		pr_err("error, ret=0x%x", ret);
+		/* FIXME: undo pinning. */
+	}
+	if (refs != on_stack)
+		kfree(refs);
+	return ret;
+}
+
+static int tegra_ion_alloc_from_id(struct ion_client *client,
+				    unsigned int cmd,
+				    unsigned long arg)
+{
+	struct tegra_ion_id_data data;
+	struct ion_buffer *buffer;
+	struct tegra_ion_id_data *user_data = (struct tegra_ion_id_data *)arg;
+
+	if (copy_from_user(&data, (void __user *)arg, sizeof(data)))
+		return -EFAULT;
+	buffer = (struct ion_buffer *)data.id;
+	data.handle = ion_import(client, buffer);
+	data.size = buffer->size;
+	if (put_user(data.handle, &user_data->handle))
+		return -EFAULT;
+	if (put_user(data.size, &user_data->size))
+		return -EFAULT;
+	return 0;
+}
+
+static int tegra_ion_get_id(struct ion_client *client,
+					    unsigned int cmd,
+					    unsigned long arg)
+{
+	bool valid_handle;
+	struct tegra_ion_id_data data;
+	struct tegra_ion_id_data *user_data = (struct tegra_ion_id_data *)arg;
+
+	if (copy_from_user(&data, (void __user *)arg, sizeof(data)))
+		return -EFAULT;
+
+	mutex_lock(&client->lock);
+	valid_handle = ion_handle_validate(client, data.handle);
+	mutex_unlock(&client->lock);
+
+	if (!valid_handle) {
+		WARN(1, "invalid handle passed\n");
+		return -EINVAL;
+	}
+
+	pr_debug("h=0x%x, b=0x%x, bref=%d",
+		(u32)data.handle, (u32)data.handle->buffer,
+		atomic_read(&data.handle->buffer->ref.refcount));
+	if (put_user((unsigned long)ion_handle_buffer(data.handle),
+			&user_data->id))
+		return -EFAULT;
+	return 0;
+}
+
+static int tegra_ion_cache_maint(struct ion_client *client,
+				 unsigned int cmd,
+				 unsigned long arg)
+{
+	wmb();
+	return 0;
+}
+
+static int tegra_ion_rw(struct ion_client *client,
+				unsigned int cmd,
+				unsigned long arg)
+{
+	bool valid_handle;
+	struct tegra_ion_rw_data data;
+	char *kern_addr, *src;
+	int ret = 0;
+	size_t copied = 0;
+
+	if (copy_from_user(&data, (void __user *)arg, sizeof(data)))
+		return -EFAULT;
+
+	if (!data.handle || !data.addr || !data.count || !data.elem_size)
+		return -EINVAL;
+
+	mutex_lock(&client->lock);
+	valid_handle = ion_handle_validate(client, data.handle);
+	mutex_unlock(&client->lock);
+
+	if (!valid_handle) {
+		WARN(1, "%s: invalid handle passed to get id.\n", __func__);
+		return -EINVAL;
+	}
+
+	if (data.elem_size == data.mem_stride &&
+	    data.elem_size == data.user_stride) {
+		data.elem_size *= data.count;
+		data.mem_stride = data.elem_size;
+		data.user_stride = data.elem_size;
+		data.count = 1;
+	}
+
+	kern_addr = ion_map_kernel(client, data.handle);
+
+	while (data.count--) {
+		if (data.offset + data.elem_size > data.handle->buffer->size) {
+			WARN(1, "read/write outside of handle\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		src = kern_addr + data.offset;
+		if (cmd == TEGRA_ION_READ)
+			ret = copy_to_user((void *)data.addr,
+					    src, data.elem_size);
+		else
+			ret = copy_from_user(src,
+					    (void *)data.addr, data.elem_size);
+
+		if (ret)
+			break;
+
+		copied += data.elem_size;
+		data.addr += data.user_stride;
+		data.offset += data.mem_stride;
+	}
+
+	ion_unmap_kernel(client, data.handle);
+	return ret;
+}
+
+static int tegra_ion_get_param(struct ion_client *client,
+					unsigned int cmd,
+					unsigned long arg)
+{
+	bool valid_handle;
+	struct tegra_ion_get_params_data data;
+	struct tegra_ion_get_params_data *user_data =
+				(struct tegra_ion_get_params_data *)arg;
+	struct ion_buffer *buffer;
+
+	if (copy_from_user(&data, (void __user *)arg, sizeof(data)))
+		return -EFAULT;
+
+	mutex_lock(&client->lock);
+	valid_handle = ion_handle_validate(client, data.handle);
+	mutex_unlock(&client->lock);
+
+	if (!valid_handle) {
+		WARN(1, "%s: invalid handle passed to get id.\n", __func__);
+		return -EINVAL;
+	}
+
+	buffer = ion_handle_buffer(data.handle);
+	data.align = 4096;
+	data.heap = 1;
+	ion_phys(client, data.handle, &data.addr, &data.size);
+
+	if (copy_to_user(user_data, &data, sizeof(data)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static long tegra_ion_ioctl(struct ion_client *client,
+				   unsigned int cmd,
+				   unsigned long arg)
+{
+	int ret = -ENOTTY;
+
+	switch (cmd) {
+	case TEGRA_ION_ALLOC_FROM_ID:
+		ret = tegra_ion_alloc_from_id(client, cmd, arg);
+		break;
+	case TEGRA_ION_GET_ID:
+		ret = tegra_ion_get_id(client, cmd, arg);
+		break;
+	case TEGRA_ION_PIN:
+	case TEGRA_ION_UNPIN:
+		ret = tegra_ion_pin(client, cmd, arg);
+		break;
+	case TEGRA_ION_CACHE_MAINT:
+		ret = tegra_ion_cache_maint(client, cmd, arg);
+		break;
+	case TEGRA_ION_READ:
+	case TEGRA_ION_WRITE:
+		ret = tegra_ion_rw(client, cmd, arg);
+		break;
+	case TEGRA_ION_GET_PARAM:
+		ret = tegra_ion_get_param(client, cmd, arg);
+		break;
+	default:
+		WARN(1, "Unknown custom ioctl\n");
+		return -ENOTTY;
+	}
+	return ret;
+}
+
+int tegra_ion_probe(struct platform_device *pdev)
+{
+	struct ion_platform_data *pdata = pdev->dev.platform_data;
+	int i;
+
+	num_heaps = pdata->nr;
+
+	heaps = kzalloc(sizeof(struct ion_heap *) * pdata->nr, GFP_KERNEL);
+
+	idev = ion_device_create(tegra_ion_ioctl);
+	if (IS_ERR_OR_NULL(idev)) {
+		kfree(heaps);
+		return PTR_ERR(idev);
+	}
+
+	/* create the heaps as specified in the board file */
+	for (i = 0; i < num_heaps; i++) {
+		struct ion_platform_heap *heap_data = &pdata->heaps[i];
+
+		heaps[i] = ion_heap_create(heap_data);
+		if (IS_ERR_OR_NULL(heaps[i])) {
+			pr_warn("%s(type:%d id:%d) isn't supported\n",
+				heap_data->name,
+				heap_data->type, heap_data->id);
+			continue;
+		}
+		ion_device_add_heap(idev, heaps[i]);
+	}
+	platform_set_drvdata(pdev, idev);
+#if !defined(CONFIG_TEGRA_NVMAP)
+	nvmap_dev = (struct nvmap_device *)idev;
+#endif
+	return 0;
+}
+
+int tegra_ion_remove(struct platform_device *pdev)
+{
+	struct ion_device *idev = platform_get_drvdata(pdev);
+	int i;
+
+	ion_device_destroy(idev);
+	for (i = 0; i < num_heaps; i++)
+		ion_heap_destroy(heaps[i]);
+	kfree(heaps);
+	return 0;
+}
+
+static struct platform_driver ion_driver = {
+	.probe = tegra_ion_probe,
+	.remove = tegra_ion_remove,
+	.driver = { .name = "ion-tegra" }
+};
+
+static int __init ion_init(void)
+{
+	return platform_driver_register(&ion_driver);
+}
+
+static void __exit ion_exit(void)
+{
+	platform_driver_unregister(&ion_driver);
+}
+
+fs_initcall(ion_init);
+module_exit(ion_exit);
+
+#if !defined(CONFIG_TEGRA_NVMAP)
+struct nvmap_client *nvmap_create_client(struct nvmap_device *dev,
+					 const char *name)
+{
+	return ion_client_create(dev, CLIENT_HEAP_MASK, name);
+}
+
+struct nvmap_handle_ref *nvmap_alloc(struct nvmap_client *client, size_t size,
+				     size_t align, unsigned int flags,
+				     unsigned int heap_mask)
+{
+	return ion_alloc(client, size, align, HEAP_FLAGS);
+}
+
+void nvmap_free(struct nvmap_client *client, struct nvmap_handle_ref *r)
+{
+	ion_free(client, r);
+}
+
+void *nvmap_mmap(struct nvmap_handle_ref *r)
+{
+	return ion_map_kernel(r->client, r);
+}
+
+void nvmap_munmap(struct nvmap_handle_ref *r, void *addr)
+{
+	ion_unmap_kernel(r->client, r);
+}
+
+struct nvmap_client *nvmap_client_get_file(int fd)
+{
+	return ion_client_get_file(fd);
+}
+
+struct nvmap_client *nvmap_client_get(struct nvmap_client *client)
+{
+	ion_client_get(client);
+	return client;
+}
+
+void nvmap_client_put(struct nvmap_client *c)
+{
+	ion_client_put(c);
+}
+
+phys_addr_t nvmap_pin(struct nvmap_client *c, struct nvmap_handle_ref *r)
+{
+	ion_phys_addr_t addr;
+	size_t len;
+
+	ion_handle_get(r);
+	ion_phys(c, r, &addr, &len);
+	wmb();
+	return addr;
+}
+
+phys_addr_t nvmap_handle_address(struct nvmap_client *c, unsigned long id)
+{
+	struct ion_handle *handle;
+	ion_phys_addr_t addr;
+	size_t len;
+
+	handle = nvmap_convert_handle_u2k(id);
+	ion_phys(c, handle, &addr, &len);
+	return addr;
+}
+
+void nvmap_unpin(struct nvmap_client *client, struct nvmap_handle_ref *r)
+{
+	if (r)
+		ion_handle_put(r);
+}
+
+static int nvmap_reloc_pin_array(struct ion_client *client,
+				 const struct nvmap_pinarray_elem *arr,
+				 int nr, struct ion_handle *gather)
+{
+	struct ion_handle *last_patch = NULL;
+	void *patch_addr;
+	ion_phys_addr_t pin_addr;
+	size_t len;
+	int i;
+
+	for (i = 0; i < nr; i++) {
+		struct ion_handle *patch;
+		struct ion_handle *pin;
+		ion_phys_addr_t reloc_addr;
+
+		/* all of the handles are validated and get'ted prior to
+		 * calling this function, so casting is safe here */
+		pin = (struct ion_handle *)arr[i].pin_mem;
+
+		if (arr[i].patch_mem == (unsigned long)last_patch) {
+			patch = last_patch;
+		} else if (arr[i].patch_mem == (unsigned long)gather) {
+			patch = gather;
+		} else {
+			if (last_patch)
+				ion_handle_put(last_patch);
+
+			ion_handle_get((struct ion_handle *)arr[i].patch_mem);
+			patch = (struct ion_handle *)arr[i].patch_mem;
+			if (!patch)
+				return -EPERM;
+			last_patch = patch;
+		}
+
+		patch_addr = ion_map_kernel(client, patch);
+		patch_addr = patch_addr + arr[i].patch_offset;
+
+		ion_phys(client, pin, &pin_addr, &len);
+		reloc_addr = pin_addr + arr[i].pin_offset;
+		__raw_writel(reloc_addr, patch_addr);
+		ion_unmap_kernel(client, patch);
+	}
+
+	if (last_patch)
+		ion_handle_put(last_patch);
+
+	wmb();
+	return 0;
+}
+
+int nvmap_pin_array(struct nvmap_client *client, struct nvmap_handle *gather,
+		    const struct nvmap_pinarray_elem *arr, int nr,
+		    struct nvmap_handle **unique)
+{
+	int i;
+	int count = 0;
+
+	/* FIXME: take care of duplicate ones & validation. */
+	for (i = 0; i < nr; i++) {
+		unique[i] = (struct nvmap_handle *)arr[i].pin_mem;
+		nvmap_pin(client, (struct nvmap_handle_ref *)unique[i]);
+		count++;
+	}
+	nvmap_reloc_pin_array((struct ion_client *)client,
+		arr, nr, (struct ion_handle *)gather);
+	return nr;
+}
+
+void nvmap_unpin_handles(struct nvmap_client *client,
+			 struct nvmap_handle **h, int nr)
+{
+	int i;
+
+	for (i = 0; i < nr; i++)
+		nvmap_unpin(client, h[i]);
+}
+
+int nvmap_patch_word(struct nvmap_client *client,
+		     struct nvmap_handle *patch,
+		     u32 patch_offset, u32 patch_value)
+{
+	void *vaddr;
+	u32 *patch_addr;
+
+	vaddr = ion_map_kernel(client, patch);
+	patch_addr = vaddr + patch_offset;
+	__raw_writel(patch_value, patch_addr);
+	wmb();
+	ion_unmap_kernel(client, patch);
+	return 0;
+}
+
+struct nvmap_handle *nvmap_handle_get(struct nvmap_handle *h);
+struct nvmap_handle *nvmap_get_handle_id(struct nvmap_client *client,
+					 unsigned long id)
+{
+	struct ion_handle *handle;
+
+	handle = (struct ion_handle *)nvmap_convert_handle_u2k(id);
+	pr_debug("id=0x%x, h=0x%x,c=0x%x",
+		(u32)id, (u32)handle, (u32)client);
+	nvmap_handle_get(handle);
+	return handle;
+}
+
+struct nvmap_handle_ref *nvmap_duplicate_handle_id(struct nvmap_client *client,
+						   unsigned long id)
+{
+	struct ion_buffer *buffer;
+	struct ion_handle *handle;
+	struct ion_client *ion_client = client;
+
+	handle = (struct ion_handle *)nvmap_convert_handle_u2k(id);
+	pr_debug("id=0x%x, h=0x%x,c=0x%x",
+		(u32)id, (u32)handle, (u32)client);
+	buffer = handle->buffer;
+
+	handle = ion_handle_create(client, buffer);
+
+	mutex_lock(&ion_client->lock);
+	ion_handle_add(ion_client, handle);
+	mutex_unlock(&ion_client->lock);
+
+	pr_debug("dup id=0x%x, h=0x%x", (u32)id, (u32)handle);
+	return handle;
+}
+
+void _nvmap_handle_free(struct nvmap_handle *h)
+{
+	ion_handle_put(h);
+}
+
+struct nvmap_handle_ref *nvmap_alloc_iovm(struct nvmap_client *client,
+	size_t size, size_t align, unsigned int flags, unsigned int iova_start)
+{
+	struct ion_handle *h;
+
+	h = ion_alloc(client, size, align, 0xFF);
+	ion_remap_dma(client, h, iova_start);
+	return h;
+}
+
+void nvmap_free_iovm(struct nvmap_client *client, struct nvmap_handle_ref *r)
+{
+	ion_free(client, r);
+}
+
+struct nvmap_handle *nvmap_handle_get(struct nvmap_handle *h)
+{
+	ion_handle_get(h);
+	return h;
+}
+
+void nvmap_handle_put(struct nvmap_handle *h)
+{
+	ion_handle_put(h);
+}
+
+#endif
diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index 1130a8987125..7978c55db849 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -69,7 +69,7 @@ config HID_ACRUX
 	Say Y here if you want to enable support for ACRUX game controllers.
 
 config HID_ACRUX_FF
-	tristate "ACRUX force feedback support"
+	bool "ACRUX force feedback support"
 	depends on HID_ACRUX
 	select INPUT_FF_MEMLESS
 	---help---
@@ -328,6 +328,7 @@ config HID_MULTITOUCH
 	  - Hanvon dual touch panels
 	  - Ilitek dual touch panels
 	  - IrTouch Infrared USB panels
+	  - LG Display panels (Dell ST2220Tc)
 	  - Lumio CrystalTouch panels
 	  - MosArt dual-touch panels
 	  - PenMount dual touch panels
diff --git a/drivers/hid/hid-apple.c b/drivers/hid/hid-apple.c
index 18b3bc646bf3..299d23871122 100644
--- a/drivers/hid/hid-apple.c
+++ b/drivers/hid/hid-apple.c
@@ -455,6 +455,9 @@ static const struct hid_device_id apple_devices[] = {
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_ISO),
 		.driver_data = APPLE_NUMLOCK_EMULATION | APPLE_HAS_FN |
 			APPLE_ISO_KEYBOARD },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_2011_ISO),
+		.driver_data = APPLE_NUMLOCK_EMULATION | APPLE_HAS_FN |
+			APPLE_ISO_KEYBOARD },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_JIS),
 		.driver_data = APPLE_NUMLOCK_EMULATION | APPLE_HAS_FN },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING_ANSI),
@@ -493,6 +496,24 @@ static const struct hid_device_id apple_devices[] = {
 		.driver_data = APPLE_HAS_FN | APPLE_ISO_KEYBOARD },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5_JIS),
 		.driver_data = APPLE_HAS_FN | APPLE_RDESC_JIS },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING6_ANSI),
+		.driver_data = APPLE_HAS_FN },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING6_ISO),
+		.driver_data = APPLE_HAS_FN | APPLE_ISO_KEYBOARD },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING6_JIS),
+		.driver_data = APPLE_HAS_FN | APPLE_RDESC_JIS },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING6A_ANSI),
+		.driver_data = APPLE_HAS_FN },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING6A_ISO),
+		.driver_data = APPLE_HAS_FN | APPLE_ISO_KEYBOARD },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING6A_JIS),
+		.driver_data = APPLE_HAS_FN | APPLE_RDESC_JIS },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5A_ANSI),
+		.driver_data = APPLE_HAS_FN },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5A_ISO),
+		.driver_data = APPLE_HAS_FN | APPLE_ISO_KEYBOARD },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5A_JIS),
+		.driver_data = APPLE_HAS_FN | APPLE_RDESC_JIS },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_ANSI),
 		.driver_data = APPLE_NUMLOCK_EMULATION | APPLE_HAS_FN },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_ISO),
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index 242353df3dc4..e9c8f803de1b 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -361,7 +361,7 @@ static int hid_parser_global(struct hid_parser *parser, struct hid_item *item)
 
 	case HID_GLOBAL_ITEM_TAG_REPORT_SIZE:
 		parser->global.report_size = item_udata(item);
-		if (parser->global.report_size > 32) {
+		if (parser->global.report_size > 96) {
 			dbg_hid("invalid report_size %d\n",
 					parser->global.report_size);
 			return -1;
@@ -1340,12 +1340,22 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5_ANSI) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5_ISO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5_JIS) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5A_ANSI) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5A_ISO) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5A_JIS) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING6_ANSI) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING6_ISO) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING6_JIS) },
+ 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING6A_ANSI) },
+ 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING6A_ISO) },
+ 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING6A_JIS) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_REVB_ANSI) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_REVB_ISO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_REVB_JIS) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_ANSI) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_ISO) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_JIS) },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_ALU_WIRELESS_2011_ISO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_FOUNTAIN_TP_ONLY) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_GEYSER1_TP_ONLY) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ASUS, USB_DEVICE_ID_ASUS_T91MT) },
@@ -1399,6 +1409,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_ERGO_525V) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LABTEC, USB_DEVICE_ID_LABTEC_WIRELESS_KEYBOARD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LCPOWER, USB_DEVICE_ID_LCPOWER_LC1000 ) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_LG, USB_DEVICE_ID_LG_MULTITOUCH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_MX3000_RECEIVER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_S510_RECEIVER) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_S510_RECEIVER_2) },
@@ -1717,8 +1728,8 @@ static const struct hid_device_id hid_ignore_list[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ESSENTIAL_REALITY, USB_DEVICE_ID_ESSENTIAL_REALITY_P5) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ETT, USB_DEVICE_ID_TC5UH) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ETT, USB_DEVICE_ID_TC4UM) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_GENERAL_TOUCH, 0x0001) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GENERAL_TOUCH, 0x0002) },
-	{ HID_USB_DEVICE(USB_VENDOR_ID_GENERAL_TOUCH, 0x0003) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GENERAL_TOUCH, 0x0004) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_4_PHIDGETSERVO_30) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_GLAB, USB_DEVICE_ID_1_PHIDGETSERVO_30) },
@@ -1892,6 +1903,9 @@ static const struct hid_device_id hid_mouse_ignore_list[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5_ANSI) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5_ISO) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5_JIS) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5A_ANSI) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5A_ISO) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_WELLSPRING5A_JIS) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_FOUNTAIN_TP_ONLY) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_APPLE, USB_DEVICE_ID_APPLE_GEYSER1_TP_ONLY) },
 	{ }
diff --git a/drivers/hid/hid-debug.c b/drivers/hid/hid-debug.c
index bae48745bb42..9a243ca96e6d 100644
--- a/drivers/hid/hid-debug.c
+++ b/drivers/hid/hid-debug.c
@@ -450,6 +450,11 @@ void hid_dump_field(struct hid_field *field, int n, struct seq_file *f) {
 		seq_printf(f, "Logical(");
 		hid_resolv_usage(field->logical, f); seq_printf(f, ")\n");
 	}
+	if (field->application) {
+		tab(n, f);
+		seq_printf(f, "Application(");
+		hid_resolv_usage(field->application, f); seq_printf(f, ")\n");
+	}
 	tab(n, f); seq_printf(f, "Usage(%d)\n", field->maxusage);
 	for (j = 0; j < field->maxusage; j++) {
 		tab(n+2, f); hid_resolv_usage(field->usage[j].hid, f); seq_printf(f, "\n");
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 7484e1b67249..53c4634fed39 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -109,12 +109,22 @@
 #define USB_DEVICE_ID_APPLE_WELLSPRING5_ANSI	0x0245
 #define USB_DEVICE_ID_APPLE_WELLSPRING5_ISO	0x0246
 #define USB_DEVICE_ID_APPLE_WELLSPRING5_JIS	0x0247
+#define USB_DEVICE_ID_APPLE_WELLSPRING6A_ANSI	0x0249
+#define USB_DEVICE_ID_APPLE_WELLSPRING6A_ISO	0x024a
+#define USB_DEVICE_ID_APPLE_WELLSPRING6A_JIS	0x024b
+#define USB_DEVICE_ID_APPLE_WELLSPRING6_ANSI	0x024c
+#define USB_DEVICE_ID_APPLE_WELLSPRING6_ISO	0x024d
+#define USB_DEVICE_ID_APPLE_WELLSPRING6_JIS	0x024e
 #define USB_DEVICE_ID_APPLE_ALU_REVB_ANSI	0x024f
 #define USB_DEVICE_ID_APPLE_ALU_REVB_ISO	0x0250
 #define USB_DEVICE_ID_APPLE_ALU_REVB_JIS	0x0251
+#define USB_DEVICE_ID_APPLE_WELLSPRING5A_ANSI	0x0252
+#define USB_DEVICE_ID_APPLE_WELLSPRING5A_ISO	0x0253
+#define USB_DEVICE_ID_APPLE_WELLSPRING5A_JIS	0x0254
 #define USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_ANSI  0x0239
 #define USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_ISO   0x023a
 #define USB_DEVICE_ID_APPLE_ALU_WIRELESS_2009_JIS   0x023b
+#define USB_DEVICE_ID_APPLE_ALU_WIRELESS_2011_ISO   0x0256
 #define USB_DEVICE_ID_APPLE_FOUNTAIN_TP_ONLY	0x030a
 #define USB_DEVICE_ID_APPLE_GEYSER1_TP_ONLY	0x030b
 #define USB_DEVICE_ID_APPLE_ATV_IRCONTROL	0x8241
@@ -256,7 +266,7 @@
 #define USB_DEVICE_ID_GAMERON_DUAL_PCS_ADAPTOR	0x0002
 
 #define USB_VENDOR_ID_GENERAL_TOUCH	0x0dfc
-#define USB_DEVICE_ID_GENERAL_TOUCH_WIN7_TWOFINGERS 0x0001
+#define USB_DEVICE_ID_GENERAL_TOUCH_WIN7_TWOFINGERS 0x0003
 
 #define USB_VENDOR_ID_GLAB		0x06c2
 #define USB_DEVICE_ID_4_PHIDGETSERVO_30	0x0038
@@ -423,6 +433,9 @@
 #define USB_DEVICE_ID_LD_HYBRID		0x2090
 #define USB_DEVICE_ID_LD_HEATCONTROL	0x20A0
 
+#define USB_VENDOR_ID_LG		0x1fd2
+#define USB_DEVICE_ID_LG_MULTITOUCH	0x0064
+
 #define USB_VENDOR_ID_LOGITECH		0x046d
 #define USB_DEVICE_ID_LOGITECH_RECEIVER	0xc101
 #define USB_DEVICE_ID_LOGITECH_HARMONY_FIRST  0xc110
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index 6559e2e3364e..1483c8296d57 100644
--- a/drivers/hid/hid-input.c
+++ b/drivers/hid/hid-input.c
@@ -971,6 +971,9 @@ int hidinput_connect(struct hid_device *hid, unsigned int force)
 				 * UGCI) cram a lot of unrelated inputs into the
 				 * same interface. */
 				hidinput->report = report;
+				if (hid->driver->input_register &&
+						hid->driver->input_register(hid, hidinput))
+					goto out_cleanup;
 				if (input_register_device(hidinput->input))
 					goto out_cleanup;
 				hidinput = NULL;
@@ -978,6 +981,10 @@ int hidinput_connect(struct hid_device *hid, unsigned int force)
 		}
 	}
 
+	if (hidinput && hid->driver->input_register &&
+			hid->driver->input_register(hid, hidinput))
+		goto out_cleanup;
+
 	if (hidinput && input_register_device(hidinput->input))
 		goto out_cleanup;
 
diff --git a/drivers/hid/hid-magicmouse.c b/drivers/hid/hid-magicmouse.c
index f0fbd7bd239e..08f5dc773975 100644
--- a/drivers/hid/hid-magicmouse.c
+++ b/drivers/hid/hid-magicmouse.c
@@ -387,8 +387,10 @@ static int magicmouse_raw_event(struct hid_device *hdev,
 	return 1;
 }
 
-static void magicmouse_setup_input(struct input_dev *input, struct hid_device *hdev)
+static int magicmouse_setup_input(struct hid_device *hdev, struct hid_input *hi)
 {
+	struct input_dev *input = hi->input;
+
 	__set_bit(EV_KEY, input->evbit);
 
 	if (input->id.product == USB_DEVICE_ID_APPLE_MAGICMOUSE) {
@@ -462,6 +464,8 @@ static void magicmouse_setup_input(struct input_dev *input, struct hid_device *h
 		__set_bit(EV_MSC, input->evbit);
 		__set_bit(MSC_RAW, input->mscbit);
 	}
+
+	return 0;
 }
 
 static int magicmouse_input_mapping(struct hid_device *hdev,
@@ -514,12 +518,6 @@ static int magicmouse_probe(struct hid_device *hdev,
 		goto err_free;
 	}
 
-	/* We do this after hid-input is done parsing reports so that
-	 * hid-input uses the most natural button and axis IDs.
-	 */
-	if (msc->input)
-		magicmouse_setup_input(msc->input, hdev);
-
 	if (id->product == USB_DEVICE_ID_APPLE_MAGICMOUSE)
 		report = hid_register_report(hdev, HID_INPUT_REPORT,
 			MOUSE_REPORT_ID);
@@ -584,6 +582,7 @@ static struct hid_driver magicmouse_driver = {
 	.remove = magicmouse_remove,
 	.raw_event = magicmouse_raw_event,
 	.input_mapping = magicmouse_input_mapping,
+	.input_register = magicmouse_setup_input,
 };
 
 static int __init magicmouse_init(void)
diff --git a/drivers/hid/hid-multitouch.c b/drivers/hid/hid-multitouch.c
index 58d0e7aaf088..b03a0b0e9b63 100644
--- a/drivers/hid/hid-multitouch.c
+++ b/drivers/hid/hid-multitouch.c
@@ -213,6 +213,16 @@ static int mt_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 	struct mt_class *cls = td->mtclass;
 	__s32 quirks = cls->quirks;
 
+	/* Only map fields from TouchScreen or TouchPad collections.
+         * We need to ignore fields that belong to other collections
+         * such as Mouse that might have the same GenericDesktop usages. */
+	if (field->application == HID_DG_TOUCHSCREEN)
+		set_bit(INPUT_PROP_DIRECT, hi->input->propbit);
+	else if (field->application == HID_DG_TOUCHPAD)
+		set_bit(INPUT_PROP_POINTER, hi->input->propbit);
+	else
+		return 0;
+
 	switch (usage->hid & HID_USAGE_PAGE) {
 
 	case HID_UP_GENDESK:
@@ -672,6 +682,11 @@ static const struct hid_device_id mt_devices[] = {
 		HID_USB_DEVICE(USB_VENDOR_ID_IRTOUCHSYSTEMS,
 			USB_DEVICE_ID_IRTOUCH_INFRARED_USB) },
 
+	/* LG Display panels */
+	{ .driver_data = MT_CLS_DEFAULT,
+		HID_USB_DEVICE(USB_VENDOR_ID_LG,
+			USB_DEVICE_ID_LG_MULTITOUCH) },
+
 	/* Lumio panels */
 	{ .driver_data = MT_CLS_CONFIDENCE_MINUS_ONE,
 		HID_USB_DEVICE(USB_VENDOR_ID_LUMIO,
diff --git a/drivers/hid/hid-sony.c b/drivers/hid/hid-sony.c
index 5cd25bd907f8..81c054e866df 100644
--- a/drivers/hid/hid-sony.c
+++ b/drivers/hid/hid-sony.c
@@ -155,6 +155,26 @@ static int sixaxis_set_operational_bt(struct hid_device *hdev)
 	return hdev->hid_output_raw_report(hdev, buf, sizeof(buf), HID_FEATURE_REPORT);
 }
 
+static void sixaxis_set_led_bt(struct hid_device *hdev)
+{
+	hid_info(hdev, "set LED BT\n");
+	/* set first LED on BT connection */
+	unsigned char led_data[] = {
+			0x01,
+			/* rumble values */
+			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+			/* LED settings 0x02=LED1 .. 0x10=LED4 */
+			0x02,
+			0xff, 0x27, 0x10, 0x00, 0x32,	/* LED 4 */
+			0xff, 0x27, 0x10, 0x00, 0x32,	/* LED 3 */
+			0xff, 0x27, 0x10, 0x00, 0x32,	/* LED 2 */
+			0xff, 0x27, 0x10, 0x00, 0x32,	/* LED 1 */
+			0x00, 0x00, 0x00, 0x00, 0x00
+		};
+	hdev->hid_output_raw_report(hdev, led_data, sizeof(led_data),
+					HID_OUTPUT_REPORT);
+}
+
 static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 {
 	int ret;
@@ -187,8 +207,11 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		hdev->hid_output_raw_report = sixaxis_usb_output_raw_report;
 		ret = sixaxis_set_operational_usb(hdev);
 	}
-	else if (sc->quirks & SIXAXIS_CONTROLLER_BT)
+	else if (sc->quirks & SIXAXIS_CONTROLLER_BT) {
 		ret = sixaxis_set_operational_bt(hdev);
+		if (ret >= 0)
+			sixaxis_set_led_bt(hdev);
+	}
 	else
 		ret = 0;
 
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index 0b62c3c6b7ce..1f46b8d3a791 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -179,6 +179,16 @@ config SENSORS_ADT7411
 	  This driver can also be built as a module. If so, the module
 	  will be called adt7411.
 
+config SENSORS_ADT7461
+	tristate "Analog Devices ADT7461"
+	depends on I2C && EXPERIMENTAL
+	help
+	  If you say yes here you get support for the Analog Devices
+	  ADT7461 temperature monitoring chips.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called adt7461.
+
 config SENSORS_ADT7462
 	tristate "Analog Devices ADT7462"
 	depends on I2C && EXPERIMENTAL
@@ -1058,6 +1068,14 @@ config SENSORS_AMC6821
 	  This driver can also be build as a module.  If so, the module
 	  will be called amc6821.
 
+config SENSORS_TEGRA_TSENSOR
+	tristate "Nvidia Tegra Integrated temperature sensor"
+	depends on ARCH_TEGRA_3x_SOC
+	default n
+	help
+	  If you say yes here you get support for integrated
+	  temperature sensor in Nvidia tegra chipset.
+
 config SENSORS_THMC50
 	tristate "Texas Instruments THMC50 / Analog Devices ADM1022"
 	depends on I2C
@@ -1327,6 +1345,21 @@ config SENSORS_MC13783_ADC
         help
           Support for the A/D converter on MC13783 PMIC.
 
+config SENSORS_INA219
+	tristate "Texas Instruments INA219 POWER MONITOR SENSOR DRIVER"
+	depends on I2C
+	default n
+	help
+		Support for the TI INA219 power monitor sensor.
+
+config SENSORS_INA230
+	tristate "Texas Instruments INA230 POWER MONITOR SENSOR DRIVER"
+	depends on I2C
+	help
+	  Support for the TI INA230 power monitor sensor.
+	  (also works for TI INA226)
+
+
 if ACPI
 
 comment "ACPI drivers"
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index 3c9ccefea791..66af4257ddc7 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -34,6 +34,7 @@ obj-$(CONFIG_SENSORS_ADS1015)	+= ads1015.o
 obj-$(CONFIG_SENSORS_ADS7828)	+= ads7828.o
 obj-$(CONFIG_SENSORS_ADS7871)	+= ads7871.o
 obj-$(CONFIG_SENSORS_ADT7411)	+= adt7411.o
+obj-$(CONFIG_SENSORS_ADT7461)	+= adt7461.o
 obj-$(CONFIG_SENSORS_ADT7462)	+= adt7462.o
 obj-$(CONFIG_SENSORS_ADT7470)	+= adt7470.o
 obj-$(CONFIG_SENSORS_ADT7475)	+= adt7475.o
@@ -123,6 +124,10 @@ obj-$(CONFIG_SENSORS_W83L785TS)	+= w83l785ts.o
 obj-$(CONFIG_SENSORS_W83L786NG)	+= w83l786ng.o
 obj-$(CONFIG_SENSORS_WM831X)	+= wm831x-hwmon.o
 obj-$(CONFIG_SENSORS_WM8350)	+= wm8350-hwmon.o
+obj-$(CONFIG_SENSORS_INA219)	+= ina219.o
+obj-$(CONFIG_SENSORS_INA230)	+= ina230.o
+CFLAGS_tegra-tsensor.o		 = -Werror
+obj-$(CONFIG_SENSORS_TEGRA_TSENSOR) += tegra-tsensor.o
 
 obj-$(CONFIG_PMBUS)		+= pmbus/
 
diff --git a/drivers/hwmon/adt7461.c b/drivers/hwmon/adt7461.c
new file mode 100644
index 000000000000..10ea2eb80668
--- /dev/null
+++ b/drivers/hwmon/adt7461.c
@@ -0,0 +1,823 @@
+/*
+ * adt7461.c - Linux kernel modules for hardware
+ *          monitoring
+ * Copyright (C) 2003-2010  Jean Delvare <khali@linux-fr.org>
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/jiffies.h>
+#include <linux/regulator/consumer.h>
+#include <linux/i2c.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/hwmon.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/sysfs.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/adt7461.h>
+
+#define DRIVER_NAME "adt7461"
+
+/*
+ * The ADT7461 registers
+ */
+
+#define ADT7461_REG_R_MAN_ID			0xFE
+#define ADT7461_REG_R_CHIP_ID			0xFF
+#define ADT7461_REG_R_CONFIG1			0x03
+#define ADT7461_REG_W_CONFIG1			0x09
+#define ADT7461_REG_R_CONVRATE			0x04
+#define ADT7461_REG_W_CONVRATE			0x0A
+#define ADT7461_REG_R_STATUS			0x02
+#define ADT7461_REG_R_LOCAL_TEMP		0x00
+#define ADT7461_REG_R_LOCAL_HIGH		0x05
+#define ADT7461_REG_W_LOCAL_HIGH		0x0B
+#define ADT7461_REG_R_LOCAL_LOW			0x06
+#define ADT7461_REG_W_LOCAL_LOW			0x0C
+#define ADT7461_REG_R_LOCAL_CRIT		0x20
+#define ADT7461_REG_W_LOCAL_CRIT		0x20
+#define ADT7461_REG_R_REMOTE_TEMPH		0x01
+#define ADT7461_REG_R_REMOTE_TEMPL		0x10
+#define ADT7461_REG_R_REMOTE_OFFSH		0x11
+#define ADT7461_REG_W_REMOTE_OFFSH		0x11
+#define ADT7461_REG_R_REMOTE_OFFSL		0x12
+#define ADT7461_REG_W_REMOTE_OFFSL		0x12
+#define ADT7461_REG_R_REMOTE_HIGHH		0x07
+#define ADT7461_REG_W_REMOTE_HIGHH		0x0D
+#define ADT7461_REG_R_REMOTE_HIGHL		0x13
+#define ADT7461_REG_W_REMOTE_HIGHL		0x13
+#define ADT7461_REG_R_REMOTE_LOWH		0x08
+#define ADT7461_REG_W_REMOTE_LOWH		0x0E
+#define ADT7461_REG_R_REMOTE_LOWL		0x14
+#define ADT7461_REG_W_REMOTE_LOWL		0x14
+#define ADT7461_REG_R_REMOTE_CRIT		0x19
+#define ADT7461_REG_W_REMOTE_CRIT		0x19
+#define ADT7461_REG_R_TCRIT_HYST		0x21
+#define ADT7461_REG_W_TCRIT_HYST		0x21
+
+/* Configuration Register Bits */
+#define EXTENDED_RANGE_BIT		BIT(2)
+#define THERM2_BIT			BIT(5)
+#define STANDBY_BIT			BIT(6)
+#define ALERT_BIT			BIT(7)
+
+/* Max Temperature Measurements */
+#define EXTENDED_RANGE_OFFSET	64U
+#define STANDARD_RANGE_MAX		127U
+#define EXTENDED_RANGE_MAX		(150U + EXTENDED_RANGE_OFFSET)
+
+/*
+ * Device flags
+ */
+#define ADT7461_FLAG_ADT7461_EXT		0x01	/* ADT7461 extended mode */
+#define ADT7461_FLAG_THERM2			0x02	/* Pin 6 as Therm2 */
+
+/*
+ * Client data
+ */
+
+struct adt7461_data {
+	struct work_struct work;
+	struct i2c_client *client;
+	struct device *hwmon_dev;
+	struct mutex update_lock;
+	struct regulator *regulator;
+	char valid; /* zero until following fields are valid */
+	unsigned long last_updated; /* in jiffies */
+	int flags;
+
+	u8 config;		/* configuration register value */
+	u8 alert_alarms;	/* Which alarm bits trigger ALERT# */
+
+	/* registers values */
+	s8 temp8[4];	/* 0: local low limit
+			   1: local high limit
+			   2: local critical limit
+			   3: remote critical limit */
+	s16 temp11[5];	/* 0: remote input
+			   1: remote low limit
+			   2: remote high limit
+			   3: remote offset
+			   4: local input */
+	u8 temp_hyst;
+	u8 alarms; /* bitvector */
+	void (*alarm_fn)(bool raised);
+	int irq_gpio;
+};
+
+/*
+ * Conversions
+ */
+
+static inline int temp_from_s8(s8 val)
+{
+	return val * 1000;
+}
+
+static u8 hyst_to_reg(long val)
+{
+	if (val <= 0)
+		return 0;
+	if (val >= 30500)
+		return 31;
+	return (val + 500) / 1000;
+}
+
+/*
+ * ADT7461 attempts to write values that are outside the range
+ * 0 < temp < 127 are treated as the boundary value.
+ *
+ * ADT7461 in "extended mode" operation uses unsigned integers offset by
+ * 64 (e.g., 0 -> -64 degC).  The range is restricted to -64..191 degC.
+ */
+static inline int temp_from_u8(struct adt7461_data *data, u8 val)
+{
+	if (data->flags & ADT7461_FLAG_ADT7461_EXT)
+		return (val - 64) * 1000;
+	else
+		return temp_from_s8(val);
+}
+
+static inline int temp_from_u16(struct adt7461_data *data, u16 val)
+{
+	if (data->flags & ADT7461_FLAG_ADT7461_EXT)
+		return (val - 0x4000) / 64 * 250;
+	else
+		return val / 32 * 125;
+}
+
+static u8 temp_to_u8(struct adt7461_data *data, long val)
+{
+	if (data->flags & ADT7461_FLAG_ADT7461_EXT) {
+		if (val <= -64000)
+			return 0;
+		if (val >= 191000)
+			return 0xFF;
+		return (val + 500 + 64000) / 1000;
+	} else {
+		if (val <= 0)
+			return 0;
+		if (val >= 127000)
+			return 127;
+		return (val + 500) / 1000;
+	}
+}
+
+static u16 temp_to_u16(struct adt7461_data *data, long val)
+{
+	if (data->flags & ADT7461_FLAG_ADT7461_EXT) {
+		if (val <= -64000)
+			return 0;
+		if (val >= 191750)
+			return 0xFFC0;
+		return (val + 64000 + 125) / 250 * 64;
+	} else {
+		if (val <= 0)
+			return 0;
+		if (val >= 127750)
+			return 0x7FC0;
+		return (val + 125) / 250 * 64;
+	}
+}
+
+static int adt7461_read_reg(struct i2c_client* client, u8 reg, u8 *value)
+{
+	int err;
+
+	err = i2c_smbus_read_byte_data(client, reg);
+	if (err < 0) {
+		pr_err("adt7461_read_reg:Register %#02x read failed (%d)\n",
+			 reg, err);
+		return err;
+	}
+	*value = err;
+
+	return 0;
+}
+
+static int adt7461_read16(struct i2c_client *client, u8 regh, u8 regl,
+			u16 *value)
+{
+	int err;
+	u8 oldh, newh, l;
+
+	/*
+	 * There is a trick here. We have to read two registers to have the
+	 * sensor temperature, but we have to beware a conversion could occur
+	 * inbetween the readings. The datasheet says we should either use
+	 * the one-shot conversion register, which we don't want to do
+	 * (disables hardware monitoring) or monitor the busy bit, which is
+	 * impossible (we can't read the values and monitor that bit at the
+	 * exact same time). So the solution used here is to read the high
+	 * byte once, then the low byte, then the high byte again. If the new
+	 * high byte matches the old one, then we have a valid reading. Else
+	 * we have to read the low byte again, and now we believe we have a
+	 * correct reading.
+	 */
+	if ((err = adt7461_read_reg(client, regh, &oldh))
+	 || (err = adt7461_read_reg(client, regl, &l))
+	 || (err = adt7461_read_reg(client, regh, &newh)))
+		return err;
+	if (oldh != newh) {
+		err = adt7461_read_reg(client, regl, &l);
+		if (err)
+			return err;
+	}
+	*value = (newh << 8) | l;
+
+	return 0;
+}
+
+static struct adt7461_data *adt7461_update_device(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct adt7461_data *data = i2c_get_clientdata(client);
+
+	mutex_lock(&data->update_lock);
+
+	if (time_after(jiffies, data->last_updated + HZ / 2 + HZ / 10)
+	 || !data->valid) {
+		u8 h, l;
+
+		adt7461_read_reg(client, ADT7461_REG_R_LOCAL_LOW, &data->temp8[0]);
+		adt7461_read_reg(client, ADT7461_REG_R_LOCAL_HIGH, &data->temp8[1]);
+		adt7461_read_reg(client, ADT7461_REG_R_LOCAL_CRIT, &data->temp8[2]);
+		adt7461_read_reg(client, ADT7461_REG_R_REMOTE_CRIT, &data->temp8[3]);
+		adt7461_read_reg(client, ADT7461_REG_R_TCRIT_HYST, &data->temp_hyst);
+
+		if (adt7461_read_reg(client, ADT7461_REG_R_LOCAL_TEMP, &h) == 0)
+			data->temp11[4] = h << 8;
+
+		adt7461_read16(client, ADT7461_REG_R_REMOTE_TEMPH,
+				ADT7461_REG_R_REMOTE_TEMPL, &data->temp11[0]);
+
+		if (adt7461_read_reg(client, ADT7461_REG_R_REMOTE_LOWH, &h) == 0) {
+			data->temp11[1] = h << 8;
+			if (adt7461_read_reg(client, ADT7461_REG_R_REMOTE_LOWL, &l) == 0)
+				data->temp11[1] |= l;
+		}
+		if (adt7461_read_reg(client, ADT7461_REG_R_REMOTE_HIGHH, &h) == 0) {
+			data->temp11[2] = h << 8;
+			if (adt7461_read_reg(client, ADT7461_REG_R_REMOTE_HIGHL, &l) == 0)
+				data->temp11[2] |= l;
+		}
+
+		if (adt7461_read_reg(client, ADT7461_REG_R_REMOTE_OFFSH,
+					&h) == 0
+		 && adt7461_read_reg(client, ADT7461_REG_R_REMOTE_OFFSL,
+					&l) == 0)
+			data->temp11[3] = (h << 8) | l;
+		adt7461_read_reg(client, ADT7461_REG_R_STATUS, &data->alarms);
+
+		/* Re-enable ALERT# output if relevant alarms are all clear */
+		if (!(data->flags & ADT7461_FLAG_THERM2)
+		 && (data->alarms & data->alert_alarms) == 0) {
+			u8 config;
+
+			adt7461_read_reg(client, ADT7461_REG_R_CONFIG1, &config);
+			if (config & 0x80) {
+				pr_err("adt7461_update_device:Re-enabling ALERT#\n");
+				i2c_smbus_write_byte_data(client,
+							ADT7461_REG_W_CONFIG1,
+							config & ~ALERT_BIT);
+			}
+		}
+
+		data->last_updated = jiffies;
+		data->valid = 1;
+	}
+
+	mutex_unlock(&data->update_lock);
+
+	return data;
+}
+
+/*
+ * Sysfs stuff
+ */
+
+static ssize_t show_temp8(struct device *dev, struct device_attribute *devattr,
+			  char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct adt7461_data *data = adt7461_update_device(dev);
+	int temp;
+
+	temp = temp_from_u8(data, data->temp8[attr->index]);
+
+	return sprintf(buf, "%d\n", temp);
+}
+
+static ssize_t set_temp8(struct device *dev, struct device_attribute *devattr,
+			 const char *buf, size_t count)
+{
+	static const u8 reg[4] = {
+		ADT7461_REG_W_LOCAL_LOW,
+		ADT7461_REG_W_LOCAL_HIGH,
+		ADT7461_REG_W_LOCAL_CRIT,
+		ADT7461_REG_W_REMOTE_CRIT,
+	};
+
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct i2c_client *client = to_i2c_client(dev);
+	struct adt7461_data *data = i2c_get_clientdata(client);
+	long val = simple_strtol(buf, NULL, 10);
+	int nr = attr->index;
+
+	mutex_lock(&data->update_lock);
+	data->temp8[nr] = temp_to_u8(data, val);
+	i2c_smbus_write_byte_data(client, reg[nr], data->temp8[nr]);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t show_temp11(struct device *dev, struct device_attribute *devattr,
+			  char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct adt7461_data *data = adt7461_update_device(dev);
+	int temp;
+
+	temp = temp_from_u16(data, data->temp11[attr->index]);
+
+	return sprintf(buf, "%d\n", temp);
+}
+
+static ssize_t set_temp11(struct device *dev, struct device_attribute *devattr,
+			  const char *buf, size_t count)
+{
+	static const u8 reg[6] = {
+		ADT7461_REG_W_REMOTE_LOWH,
+		ADT7461_REG_W_REMOTE_LOWL,
+		ADT7461_REG_W_REMOTE_HIGHH,
+		ADT7461_REG_W_REMOTE_HIGHL,
+		ADT7461_REG_W_REMOTE_OFFSH,
+		ADT7461_REG_W_REMOTE_OFFSL,
+	};
+
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct i2c_client *client = to_i2c_client(dev);
+	struct adt7461_data *data = i2c_get_clientdata(client);
+	long val = simple_strtol(buf, NULL, 10);
+	int nr = attr->index;
+
+	mutex_lock(&data->update_lock);
+	data->temp11[nr] = temp_to_u16(data, val);
+
+	i2c_smbus_write_byte_data(client, reg[(nr - 1) * 2],
+				  data->temp11[nr] >> 8);
+	i2c_smbus_write_byte_data(client, reg[(nr - 1) * 2 + 1],
+				  data->temp11[nr] & 0xff);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t show_temphyst(struct device *dev,
+		struct device_attribute *devattr,
+		char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct adt7461_data *data = adt7461_update_device(dev);
+	int temp;
+
+	temp = temp_from_u8(data, data->temp8[attr->index]);
+
+	return sprintf(buf, "%d\n", temp - temp_from_s8(data->temp_hyst));
+}
+
+static ssize_t set_temphyst(struct device *dev, struct device_attribute *dummy,
+			 const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct adt7461_data *data = i2c_get_clientdata(client);
+	long val = simple_strtol(buf, NULL, 10);
+	int temp;
+
+	mutex_lock(&data->update_lock);
+	temp = temp_from_u8(data, data->temp8[2]);
+	data->temp_hyst = hyst_to_reg(temp - val);
+	i2c_smbus_write_byte_data(client, ADT7461_REG_W_TCRIT_HYST,
+				  data->temp_hyst);
+	mutex_unlock(&data->update_lock);
+	return count;
+}
+
+static ssize_t show_alarms(struct device *dev, struct device_attribute *dummy,
+			 char *buf)
+{
+	struct adt7461_data *data = adt7461_update_device(dev);
+	return sprintf(buf, "%d\n", data->alarms);
+}
+
+static ssize_t show_alarm(struct device *dev, struct device_attribute
+			  *devattr, char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct adt7461_data *data = adt7461_update_device(dev);
+	int bitnr = attr->index;
+
+	return sprintf(buf, "%d\n", (data->alarms >> bitnr) & 1);
+}
+
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp11, NULL, 4);
+static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_temp11, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO, show_temp8,
+	set_temp8, 0);
+static SENSOR_DEVICE_ATTR(temp2_min, S_IWUSR | S_IRUGO, show_temp11,
+	set_temp11, 1);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp8,
+	set_temp8, 1);
+static SENSOR_DEVICE_ATTR(temp2_max, S_IWUSR | S_IRUGO, show_temp11,
+	set_temp11, 2);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_temp8,
+	set_temp8, 2);
+static SENSOR_DEVICE_ATTR(temp2_crit, S_IWUSR | S_IRUGO, show_temp8,
+	set_temp8, 3);
+static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_temphyst,
+	set_temphyst, 2);
+static SENSOR_DEVICE_ATTR(temp2_crit_hyst, S_IRUGO, show_temphyst, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp2_offset, S_IWUSR | S_IRUGO, show_temp11,
+	set_temp11, 3);
+
+/* Individual alarm files */
+static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO, show_alarm, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp2_crit_alarm, S_IRUGO, show_alarm, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp2_fault, S_IRUGO, show_alarm, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp2_min_alarm, S_IRUGO, show_alarm, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp2_max_alarm, S_IRUGO, show_alarm, NULL, 4);
+static SENSOR_DEVICE_ATTR(temp1_min_alarm, S_IRUGO, show_alarm, NULL, 5);
+static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_alarm, NULL, 6);
+/* Raw alarm file for compatibility */
+static DEVICE_ATTR(alarms, S_IRUGO, show_alarms, NULL);
+
+static struct attribute *adt7461_attributes[] = {
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_min.dev_attr.attr,
+	&sensor_dev_attr_temp2_min.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp2_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit_hyst.dev_attr.attr,
+
+	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_crit_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_fault.dev_attr.attr,
+	&sensor_dev_attr_temp2_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
+	&dev_attr_alarms.attr,
+	NULL
+};
+
+static const struct attribute_group adt7461_group = {
+	.attrs = adt7461_attributes,
+};
+
+static void adt7461_work_func(struct work_struct *work)
+{
+	struct adt7461_data *data =
+			container_of(work, struct adt7461_data, work);
+
+	if (data->alarm_fn) {
+		/* Therm2 line is active low */
+		data->alarm_fn(!gpio_get_value(data->irq_gpio));
+	}
+}
+
+static irqreturn_t adt7461_irq(int irq, void *dev_id)
+{
+	struct adt7461_data *data = dev_id;
+	schedule_work(&data->work);
+
+	return IRQ_HANDLED;
+}
+
+static void adt7461_regulator_enable(struct i2c_client *client)
+{
+	struct adt7461_data *data = i2c_get_clientdata(client);
+
+	data->regulator = regulator_get(NULL, "vdd_vcore_temp");
+	if (IS_ERR_OR_NULL(data->regulator)) {
+		pr_err("adt7461_regulator_enable:Couldn't get regulator vdd_vcore_temp\n");
+		data->regulator = NULL;
+	} else {
+		regulator_enable(data->regulator);
+		/* Optimal time to get the regulator turned on
+		 * before initializing adt7461 chip*/
+		mdelay(5);
+	}
+}
+
+static void adt7461_regulator_disable(struct i2c_client *client)
+{
+	struct adt7461_data *data = i2c_get_clientdata(client);
+	struct regulator *adt7461_reg = data->regulator;
+	int ret;
+
+	if (adt7461_reg) {
+		ret = regulator_is_enabled(adt7461_reg);
+		if (ret > 0)
+			regulator_disable(adt7461_reg);
+		regulator_put(adt7461_reg);
+	}
+	data->regulator = NULL;
+}
+
+static void adt7461_enable(struct i2c_client *client)
+{
+	struct adt7461_data *data = i2c_get_clientdata(client);
+
+	i2c_smbus_write_byte_data(client, ADT7461_REG_W_CONFIG1,
+				  data->config & ~STANDBY_BIT);
+}
+
+static void adt7461_disable(struct i2c_client *client)
+{
+	struct adt7461_data *data = i2c_get_clientdata(client);
+
+	i2c_smbus_write_byte_data(client, ADT7461_REG_W_CONFIG1,
+				  data->config | STANDBY_BIT);
+}
+
+static int adt7461_init_client(struct i2c_client *client)
+{
+	struct adt7461_data *data = i2c_get_clientdata(client);
+	struct adt7461_platform_data *pdata = client->dev.platform_data;
+	u8 config = 0;
+	u8 value;
+	int err;
+
+	if (!pdata || !pdata->supported_hwrev)
+		return -ENODEV;
+
+	data->irq_gpio = -1;
+
+	if (pdata->therm2) {
+		data->flags |= ADT7461_FLAG_THERM2;
+
+		if (gpio_is_valid(pdata->irq_gpio)) {
+			if (!IS_ERR(gpio_request(pdata->irq_gpio, "adt7461"))) {
+				gpio_direction_input(pdata->irq_gpio);
+				data->irq_gpio = pdata->irq_gpio;
+			}
+		}
+	}
+
+	if (pdata->ext_range)
+		data->flags |= ADT7461_FLAG_ADT7461_EXT;
+
+	adt7461_regulator_enable(client);
+
+	/* Start the conversions. */
+	err = i2c_smbus_write_byte_data(client, ADT7461_REG_W_CONVRATE,
+							pdata->conv_rate);
+	if (err < 0)
+		goto error;
+
+	/* External temperature h/w shutdown limit */
+	value = temp_to_u8(data, pdata->shutdown_ext_limit * 1000);
+	err = i2c_smbus_write_byte_data(client,
+				ADT7461_REG_W_REMOTE_CRIT, value);
+	if (err < 0)
+		goto error;
+
+	/* Local temperature h/w shutdown limit */
+	value = temp_to_u8(data, pdata->shutdown_local_limit * 1000);
+	err = i2c_smbus_write_byte_data(client, ADT7461_REG_W_LOCAL_CRIT,
+								value);
+	if (err < 0)
+		goto error;
+
+	/* External Temperature Throttling limit */
+	value = temp_to_u8(data, pdata->throttling_ext_limit * 1000);
+	err = i2c_smbus_write_byte_data(client, ADT7461_REG_W_REMOTE_HIGHH,
+								value);
+	if (err < 0)
+		goto error;
+
+	/* Local Temperature Throttling limit */
+	value = (data->flags & ADT7461_FLAG_ADT7461_EXT) ?
+				EXTENDED_RANGE_MAX : STANDARD_RANGE_MAX;
+	err = i2c_smbus_write_byte_data(client, ADT7461_REG_W_LOCAL_HIGH,
+								value);
+	if (err < 0)
+		goto error;
+
+	/* Remote channel offset */
+	err = i2c_smbus_write_byte_data(client, ADT7461_REG_W_REMOTE_OFFSH,
+							pdata->offset);
+	if (err < 0)
+		goto error;
+
+	/* THERM hysteresis */
+	err = i2c_smbus_write_byte_data(client, ADT7461_REG_W_TCRIT_HYST,
+					pdata->hysteresis);
+	if (err < 0)
+		goto error;
+
+	if (data->flags & ADT7461_FLAG_THERM2) {
+		data->alarm_fn = pdata->alarm_fn;
+		config = (THERM2_BIT | STANDBY_BIT);
+	} else {
+		config = (~ALERT_BIT & ~THERM2_BIT & STANDBY_BIT);
+	}
+
+	err = i2c_smbus_write_byte_data(client, ADT7461_REG_W_CONFIG1, config);
+	if (err < 0)
+		goto error;
+
+	data->config = config;
+	return 0;
+
+error:
+	pr_err("adt7461_init_client:Initialization failed!\n");
+	return err;
+}
+
+static int adt7461_init_irq(struct adt7461_data *data)
+{
+	INIT_WORK(&data->work, adt7461_work_func);
+
+	return request_irq(data->client->irq, adt7461_irq, IRQF_TRIGGER_RISING |
+				IRQF_TRIGGER_FALLING, DRIVER_NAME, data);
+}
+
+static int adt7461_probe(struct i2c_client *new_client,
+		  const struct i2c_device_id *id)
+{
+	struct adt7461_data *data;
+	int err;
+
+	data = kzalloc(sizeof(struct adt7461_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->client = new_client;
+	i2c_set_clientdata(new_client, data);
+	mutex_init(&data->update_lock);
+
+	data->alert_alarms = 0x7c;
+
+	/* Initialize the ADT7461 chip */
+	err = adt7461_init_client(new_client);
+	if (err < 0)
+		goto exit_free;
+
+	if (data->flags & ADT7461_FLAG_THERM2) {
+		err = adt7461_init_irq(data);
+		if (err < 0)
+			goto exit_free;
+	}
+
+	/* Register sysfs hooks */
+	if ((err = sysfs_create_group(&new_client->dev.kobj, &adt7461_group)))
+		goto exit_free;
+	if ((err = device_create_file(&new_client->dev,
+			&sensor_dev_attr_temp2_offset.dev_attr)))
+		goto exit_remove_files;
+
+	data->hwmon_dev = hwmon_device_register(&new_client->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		err = PTR_ERR(data->hwmon_dev);
+		goto exit_remove_files;
+	}
+
+	adt7461_enable(new_client);
+	return 0;
+
+exit_remove_files:
+	sysfs_remove_group(&new_client->dev.kobj, &adt7461_group);
+exit_free:
+	kfree(data);
+	return err;
+}
+
+static int adt7461_remove(struct i2c_client *client)
+{
+	struct adt7461_data *data = i2c_get_clientdata(client);
+
+	if (data->flags & ADT7461_FLAG_THERM2) {
+		free_irq(client->irq, data);
+		cancel_work_sync(&data->work);
+	}
+	if (gpio_is_valid(data->irq_gpio))
+		gpio_free(data->irq_gpio);
+	hwmon_device_unregister(data->hwmon_dev);
+	sysfs_remove_group(&client->dev.kobj, &adt7461_group);
+	device_remove_file(&client->dev,
+			&sensor_dev_attr_temp2_offset.dev_attr);
+	adt7461_regulator_disable(client);
+
+	kfree(data);
+	return 0;
+}
+
+static void adt7461_alert(struct i2c_client *client, unsigned int flag)
+{
+	struct adt7461_data *data = i2c_get_clientdata(client);
+	u8 config, alarms;
+
+	adt7461_read_reg(client, ADT7461_REG_R_STATUS, &alarms);
+	if ((alarms & 0x7f) == 0) {
+		pr_err("adt7461_alert:Everything OK\n");
+	} else {
+		if (alarms & 0x61)
+			pr_err("adt7461_alert:temp%d out of range, please check!\n", 1);
+		if (alarms & 0x1a)
+			pr_err("adt7461_alert:temp%d out of range, please check!\n", 2);
+		if (alarms & 0x04)
+			pr_err("adt7461_alert:temp%d diode open, please check!\n", 2);
+
+		/* Disable ALERT# output, because these chips don't implement
+		  SMBus alert correctly; they should only hold the alert line
+		  low briefly. */
+		if (!(data->flags & ADT7461_FLAG_THERM2)
+		 && (alarms & data->alert_alarms)) {
+			pr_err("adt7461_alert:Disabling ALERT#\n");
+			adt7461_read_reg(client, ADT7461_REG_R_CONFIG1, &config);
+			i2c_smbus_write_byte_data(client, ADT7461_REG_W_CONFIG1,
+					config | ALERT_BIT);
+		}
+	}
+}
+
+#ifdef CONFIG_PM
+static int adt7461_suspend(struct i2c_client *client, pm_message_t state)
+{
+	disable_irq(client->irq);
+	adt7461_disable(client);
+
+	return 0;
+}
+
+static int adt7461_resume(struct i2c_client *client)
+{
+	adt7461_enable(client);
+	enable_irq(client->irq);
+
+	return 0;
+}
+#endif
+
+/*
+ * Driver data
+ */
+static const struct i2c_device_id adt7461_id[] = {
+	{ DRIVER_NAME, 0 },
+};
+
+MODULE_DEVICE_TABLE(i2c, adt7461_id);
+
+static struct i2c_driver adt7461_driver = {
+	.class		= I2C_CLASS_HWMON,
+	.driver = {
+		.name	= DRIVER_NAME,
+	},
+	.probe		= adt7461_probe,
+	.remove		= adt7461_remove,
+	.alert		= adt7461_alert,
+	.id_table	= adt7461_id,
+#ifdef CONFIG_PM
+	.suspend	= adt7461_suspend,
+	.resume		= adt7461_resume,
+#endif
+};
+
+static int __init sensors_adt7461_init(void)
+{
+	return i2c_add_driver(&adt7461_driver);
+}
+
+static void __exit sensors_adt7461_exit(void)
+{
+	i2c_del_driver(&adt7461_driver);
+}
+
+MODULE_AUTHOR("Jean Delvare <khali@linux-fr.org>");
+MODULE_DESCRIPTION("ADT7461 driver");
+MODULE_LICENSE("GPL");
+
+module_init(sensors_adt7461_init);
+module_exit(sensors_adt7461_exit);
diff --git a/drivers/hwmon/coretemp.c b/drivers/hwmon/coretemp.c
index 932383786642..62800ded9410 100644
--- a/drivers/hwmon/coretemp.c
+++ b/drivers/hwmon/coretemp.c
@@ -60,14 +60,13 @@ MODULE_PARM_DESC(tjmax, "TjMax value in degrees Celsius");
 #ifdef CONFIG_SMP
 #define TO_PHYS_ID(cpu)		cpu_data(cpu).phys_proc_id
 #define TO_CORE_ID(cpu)		cpu_data(cpu).cpu_core_id
-#define TO_ATTR_NO(cpu)		(TO_CORE_ID(cpu) + BASE_SYSFS_ATTR_NO)
 #define for_each_sibling(i, cpu)	for_each_cpu(i, cpu_sibling_mask(cpu))
 #else
 #define TO_PHYS_ID(cpu)		(cpu)
 #define TO_CORE_ID(cpu)		(cpu)
-#define TO_ATTR_NO(cpu)		(cpu)
 #define for_each_sibling(i, cpu)	for (i = 0; false; )
 #endif
+#define TO_ATTR_NO(cpu)		(TO_CORE_ID(cpu) + BASE_SYSFS_ATTR_NO)
 
 /*
  * Per-Core Temperature Data
diff --git a/drivers/hwmon/f71805f.c b/drivers/hwmon/f71805f.c
index 92f949767ece..6dbfd3e516e4 100644
--- a/drivers/hwmon/f71805f.c
+++ b/drivers/hwmon/f71805f.c
@@ -283,11 +283,11 @@ static inline long temp_from_reg(u8 reg)
 
 static inline u8 temp_to_reg(long val)
 {
-	if (val < 0)
-		val = 0;
-	else if (val > 1000 * 0xff)
-		val = 0xff;
-	return ((val + 500) / 1000);
+	if (val <= 0)
+		return 0;
+	if (val >= 1000 * 0xff)
+		return 0xff;
+	return (val + 500) / 1000;
 }
 
 /*
diff --git a/drivers/hwmon/ina219.c b/drivers/hwmon/ina219.c
new file mode 100644
index 000000000000..cc5b85fdcf84
--- /dev/null
+++ b/drivers/hwmon/ina219.c
@@ -0,0 +1,414 @@
+/*
+ * ina219.c - driver for TI INA219 current / power monitor sensor
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * The INA219 is a sensor chip made by Texas Instruments. It measures
+ * power, voltage and current on a power rail.
+ * Complete datasheet can be obtained from website:
+ *   http://focus.ti.com/lit/ds/symlink/ina219.pdf
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/sysfs.h>
+#include <linux/kobject.h>
+#include <linux/hrtimer.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/device.h>
+#include <linux/sysdev.h>
+#include "linux/ina219.h"
+#include <linux/init.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/hwmon.h>
+
+#define DRIVER_NAME "ina219"
+
+/* INA219 register offsets */
+#define INA219_CONFIG	0
+#define INA219_SHUNT	1
+#define INA219_VOLTAGE	2
+#define INA219_POWER	3
+#define INA219_CURRENT	4
+#define INA219_CAL	5
+
+/*
+	INA219 Sensor defines
+	Config info for ina219s
+	D15   D14  D13  D12 D11 D10 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0
+	rst  BRNG  PG1 PG0  BADC4 .-.BADC1 SADC4 - SADC1 MODE3 - MODE1
+	reset D15
+	bus_range=0 (d13)
+	pga_gain=0  (D12:D11)
+	bus_adc_setting=0x3     (d10:D7) 12-bit w/o oversampling (532uS)
+	shunt_adc_setting=0xb    (D6:D3) 8x oversampling (4.26ms)
+	mode=0x7                 (D2:D0) continuous shunt & bus
+*/
+#define INA219_CONFIG_DATA 0x1df
+#define INA219_RESET 0x8000
+
+struct power_mon_data {
+	s32 voltage;
+	s32 currentInMillis;
+	s32 power;
+};
+
+struct ina219_data {
+	struct device *hwmon_dev;
+	struct i2c_client *client;
+	struct ina219_platform_data *pInfo;
+	struct power_mon_data pm_data;
+	struct mutex mutex;
+};
+
+/* Set non-zero to enable debug prints */
+#define INA219_DEBUG_PRINTS 0
+
+#if INA219_DEBUG_PRINTS
+#define DEBUG_INA219(x) printk x
+#else
+#define DEBUG_INA219(x)
+#endif
+
+static s16 reorder_bytes(s16 a)
+{
+	s16 ret = ((a >> 8) & 0xff) | ((a & 0xff) << 8);
+	return ret;
+}
+
+/* set ina219 to power down mode */
+static s32 power_down_INA219(struct i2c_client *client)
+{
+	s32 retval;
+	retval = i2c_smbus_write_word_data(client, INA219_CONFIG, 0);
+	if (retval < 0)
+		dev_err(&client->dev, "power down failure sts: 0x%x\n", retval);
+	return retval;
+}
+
+static s32 show_rail_name(struct device *dev,
+			struct device_attribute *attr,
+			char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct ina219_data *data = i2c_get_clientdata(client);
+	return sprintf(buf, "%s\n", data->pInfo->rail_name);
+}
+
+static s32 show_voltage(struct device *dev,
+			struct device_attribute *attr,
+			char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct ina219_data *data = i2c_get_clientdata(client);
+	s32 retval;
+	s32 voltage_mV;
+
+	/* fill config data */
+	retval = i2c_smbus_write_word_data(client, INA219_CONFIG,
+		reorder_bytes(INA219_CONFIG_DATA));
+	if (retval < 0) {
+		dev_err(dev, "config data write failed sts: 0x%x\n", retval);
+		goto error;
+	}
+
+	/* fill calibration data */
+	retval = i2c_smbus_write_word_data(client, INA219_CAL,
+		reorder_bytes(data->pInfo->calibration_data));
+	if (retval < 0) {
+		dev_err(dev, "calib data write failed sts: 0x%x\n", retval);
+		goto error;
+	}
+
+	/* getting voltage readings in milli volts*/
+	voltage_mV =
+		reorder_bytes(i2c_smbus_read_word_data(client,
+			INA219_VOLTAGE));
+	DEBUG_INA219(("Ina219 voltage reg Value: 0x%x\n", voltage_mV));
+	if (voltage_mV < 0)
+		goto error;
+	voltage_mV = voltage_mV >> 1;
+	DEBUG_INA219(("Ina219 voltage in mv: %d\n", voltage_mV));
+
+	/* set ina219 to power down mode */
+	retval = power_down_INA219(client);
+	if (retval < 0)
+		goto error;
+
+	DEBUG_INA219(("%s volt = %d\n", __func__, voltage_mV));
+	return sprintf(buf, "%d mV\n", voltage_mV);
+error:
+	dev_err(dev, "%s: failed\n", __func__);
+	return retval;
+}
+
+
+static s32 show_power(struct device *dev,
+			struct device_attribute *attr,
+			char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct ina219_data *data = i2c_get_clientdata(client);
+	s32 retval;
+	s32 power_mW;
+	s32 voltage_mV;
+	s32 overflow, conversion;
+
+	/* fill config data */
+	retval = i2c_smbus_write_word_data(client, INA219_CONFIG,
+		reorder_bytes(INA219_CONFIG_DATA));
+	if (retval < 0) {
+		dev_err(dev, "config data write failed sts: 0x%x\n", retval);
+		goto error;
+	}
+
+	/* fill calib data */
+	retval = i2c_smbus_write_word_data(client, INA219_CAL,
+		reorder_bytes(data->pInfo->calibration_data));
+	if (retval < 0) {
+		dev_err(dev, "calibration data write failed sts: 0x%x\n",
+			retval);
+		goto error;
+	}
+
+	/* check if the readings are valid */
+	do {
+		/* read power register to clear conversion bit */
+		retval = reorder_bytes(i2c_smbus_read_word_data(client,
+			INA219_POWER));
+		if (retval < 0) {
+			dev_err(dev, "CNVR bit clearing failure sts: 0x%x\n",
+				retval);
+			goto error;
+		}
+
+		voltage_mV =
+			reorder_bytes(i2c_smbus_read_word_data(client,
+				INA219_VOLTAGE));
+		DEBUG_INA219(("Ina219 voltage reg Value: 0x%x\n", voltage_mV));
+		overflow = voltage_mV & 1;
+		if (overflow) {
+			dev_err(dev, "overflow error\n");
+			return 0;
+		}
+		conversion = (voltage_mV >> 1) & 1;
+		DEBUG_INA219(("\n ina219 CNVR value:%d", conversion));
+	} while (!conversion);
+
+	/* getting power readings in milli watts*/
+	power_mW = reorder_bytes(i2c_smbus_read_word_data(client,
+		INA219_POWER));
+	DEBUG_INA219(("Ina219 power Reg: 0x%x\n", power_mW));
+	power_mW *= data->pInfo->power_lsb;
+	DEBUG_INA219(("Ina219 power Val: %d\n", power_mW));
+	if (power_mW < 0)
+		goto error;
+
+	/* set ina219 to power down mode */
+	retval = power_down_INA219(client);
+	if (retval < 0)
+		goto error;
+
+	DEBUG_INA219(("%s pow = %d\n", __func__, power_mW));
+	return sprintf(buf, "%d mW\n", power_mW);
+error:
+	dev_err(dev, "%s: failed\n", __func__);
+	return retval;
+}
+
+static s32 show_current(struct device *dev,
+			struct device_attribute *attr,
+			char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct ina219_data *data = i2c_get_clientdata(client);
+	s32 retval;
+	s32 current_mA;
+	s32 voltage_mV;
+	s32 overflow, conversion;
+
+	/* fill config data */
+	retval = i2c_smbus_write_word_data(client, INA219_CONFIG,
+		reorder_bytes(INA219_CONFIG_DATA));
+	if (retval < 0) {
+		dev_err(dev, "config data write failed sts: 0x%x\n", retval);
+		goto error;
+	}
+
+	/* fill calib data */
+	retval = i2c_smbus_write_word_data(client, INA219_CAL,
+		reorder_bytes(data->pInfo->calibration_data));
+	if (retval < 0) {
+		dev_err(dev, "calibration data write failed sts: 0x%x\n",
+			retval);
+		goto error;
+	}
+
+	/* check if the readings are valid */
+	do {
+		/* read power register to clear conversion bit */
+		retval = reorder_bytes(i2c_smbus_read_word_data(client,
+			INA219_POWER));
+		if (retval < 0) {
+			dev_err(dev, "CNVR bit clearing failure sts: 0x%x\n",
+				retval);
+			goto error;
+		}
+
+		voltage_mV =
+			reorder_bytes(i2c_smbus_read_word_data(client,
+				INA219_VOLTAGE));
+		DEBUG_INA219(("Ina219 voltage reg Value: 0x%x\n", voltage_mV));
+		overflow = voltage_mV & 1;
+		if (overflow) {
+			dev_err(dev, "overflow error\n");
+			return 0;
+		}
+		conversion = (voltage_mV >> 1) & 1;
+		DEBUG_INA219(("\n ina219 CNVR value:%d", conversion));
+	} while (!conversion);
+
+	/* getting current readings in milli amps*/
+	current_mA = reorder_bytes(i2c_smbus_read_word_data(client,
+		INA219_CURRENT));
+	DEBUG_INA219(("Ina219 current Reg: 0x%x\n", current_mA));
+	if (current_mA < 0)
+		goto error;
+	current_mA =
+		(current_mA * data->pInfo->power_lsb) / data->pInfo->divisor;
+	DEBUG_INA219(("Ina219 current Value: %d\n", current_mA));
+
+	/* set ina219 to power down mode */
+	retval = power_down_INA219(client);
+	if (retval < 0)
+		goto error;
+
+
+	DEBUG_INA219(("%s current = %d\n", __func__, current_mA));
+	return sprintf(buf, "%d mA\n", current_mA);
+error:
+	dev_err(dev, "%s: failed\n", __func__);
+	return retval;
+}
+
+static struct sensor_device_attribute ina219[] = {
+	SENSOR_ATTR(rail_name, S_IRUGO, show_rail_name, NULL, 0),
+	SENSOR_ATTR(in1_input, S_IRUGO, show_voltage, NULL, 0),
+	SENSOR_ATTR(curr1_input, S_IRUGO, show_current, NULL, 0),
+	SENSOR_ATTR(power1_input, S_IRUGO, show_power, NULL, 0),
+};
+
+static int __devinit ina219_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
+{
+	struct ina219_data *data;
+	int err;
+	u8 i;
+	data = kzalloc(sizeof(struct ina219_data), GFP_KERNEL);
+	if (!data) {
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	i2c_set_clientdata(client, data);
+	data->pInfo = client->dev.platform_data;
+	mutex_init(&data->mutex);
+	/* reset ina219 */
+	err = i2c_smbus_write_word_data(client, INA219_CONFIG,
+		reorder_bytes(INA219_RESET));
+	if (err < 0) {
+		dev_err(&client->dev, "ina219 reset failure status: 0x%x\n",
+			err);
+		goto exit_free;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ina219); i++) {
+		err = device_create_file(&client->dev, &ina219[i].dev_attr);
+		if (err) {
+			dev_err(&client->dev, "device_create_file failed.\n");
+			goto exit_free;
+		}
+	}
+
+	data->hwmon_dev = hwmon_device_register(&client->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		err = PTR_ERR(data->hwmon_dev);
+		goto exit_remove;
+	}
+
+	/* set ina219 to power down mode */
+	err = power_down_INA219(client);
+	if (err < 0)
+		goto exit_remove;
+
+	return 0;
+
+exit_remove:
+	for (i = 0; i < ARRAY_SIZE(ina219); i++)
+		device_remove_file(&client->dev, &ina219[i].dev_attr);
+exit_free:
+	kfree(data);
+exit:
+	return err;
+}
+
+static int __devexit ina219_remove(struct i2c_client *client)
+{
+	u8 i;
+	struct ina219_data *data = i2c_get_clientdata(client);
+	hwmon_device_unregister(data->hwmon_dev);
+	for (i = 0; i < ARRAY_SIZE(ina219); i++)
+		device_remove_file(&client->dev, &ina219[i].dev_attr);
+	kfree(data);
+	return 0;
+}
+
+static const struct i2c_device_id ina219_id[] = {
+	{DRIVER_NAME, 0 },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, ina219_id);
+
+static struct i2c_driver ina219_driver = {
+	.class		= I2C_CLASS_HWMON,
+	.driver = {
+		.name	= DRIVER_NAME,
+	},
+	.probe		= ina219_probe,
+	.remove		= __devexit_p(ina219_remove),
+	.id_table	= ina219_id,
+};
+
+static int __init ina219_init(void)
+{
+	return i2c_add_driver(&ina219_driver);
+}
+
+static void __exit ina219_exit(void)
+{
+	i2c_del_driver(&ina219_driver);
+}
+
+module_init(ina219_init);
+module_exit(ina219_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/ina230.c b/drivers/hwmon/ina230.c
new file mode 100644
index 000000000000..3591a9463108
--- /dev/null
+++ b/drivers/hwmon/ina230.c
@@ -0,0 +1,561 @@
+/*
+ * ina230.c - driver for TI INA230 current / power monitor sensor
+ * (also compatible with TI INA226)
+ *
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * 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; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ *
+ * The INA230(/INA226) is a sensor chip made by Texas Instruments. It measures
+ * power, voltage and current on a power rail and provides an alert on
+ * over voltage/power
+ * Complete datasheet can be obtained from ti.com
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/sysfs.h>
+#include <linux/kobject.h>
+#include <linux/hrtimer.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/device.h>
+#include <linux/sysdev.h>
+#include <linux/platform_data/ina230.h>
+#include <linux/init.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/hwmon.h>
+#include <linux/cpu.h>
+
+
+#define DRIVER_NAME "ina230"
+#define MEASURE_BUS_VOLT 0
+
+/* ina230 (/ ina226)register offsets */
+#define INA230_CONFIG	0
+#define INA230_SHUNT	1
+#define INA230_VOLTAGE	2
+#define INA230_POWER	3
+#define INA230_CURRENT	4
+#define INA230_CAL	5
+#define INA230_MASK	6
+#define INA230_ALERT	7
+
+/*
+Config register for ina230 (/ ina226):
+D15|D14 D13 D12|D11 D10 D09|D08 D07 D06|D05 D04 D03|D02 D01 D00
+rst|-   -   -  |AVG        |Vbus_CT    |Vsh_CT     |MODE
+*/
+#define INA230_RESET		(1 << 15)
+#define INA230_AVG		(0 << 9) /* 0 Averages */
+#define INA230_VBUS_CT		(0 << 6) /* Vbus 140us conversion time */
+#define INA230_VSH_CT		(0 << 3) /* Vshunt 140us conversion time */
+
+#if MEASURE_BUS_VOLT
+#define INA230_CONT_MODE	5	/* Continuous Shunt measurement */
+#define INA230_TRIG_MODE	1	/* Triggered Shunt measurement */
+#else
+#define INA230_CONT_MODE	7	/* Continuous Bus and shunt measure */
+#define INA230_TRIG_MODE	3	/* Triggered Bus and shunt measure */
+#endif
+
+#define INA230_POWER_DOWN	0
+#define INA230_CONT_CONFIG	(INA230_AVG | INA230_VBUS_CT | \
+				 INA230_VSH_CT | INA230_CONT_MODE)
+#define INA230_TRIG_CONFIG	(INA230_AVG | INA230_VBUS_CT | \
+				 INA230_VSH_CT | INA230_TRIG_MODE)
+
+/*
+Mask register for ina230 (/ina 226):
+D15|D14|D13|D12|D11 D10 D09 D08 D07 D06 D05 D04 D03 D02 D01 D00
+SOL|SUL|BOL|BUL|POL|CVR|-   -   -   -   -  |AFF|CVF|OVF|APO|LEN
+*/
+#define INA230_MASK_SOL		(1 << 15)
+#define INA230_MASK_SUL		(1 << 14)
+
+
+struct ina230_data {
+	struct device *hwmon_dev;
+	struct i2c_client *client;
+	struct ina230_platform_data *pdata;
+	struct mutex mutex;
+	bool running;
+	struct notifier_block nb;
+};
+
+
+/* bus voltage resolution: 1.25mv */
+#define busv_register_to_mv(x) (((x) * 5) >> 2)
+
+/* shunt voltage resolution: 2.5uv */
+#define shuntv_register_to_uv(x) (((x) * 5) >> 1)
+#define uv_to_alert_register(x) (((x) << 1) / 5)
+
+
+
+static s32 ensure_enabled_start(struct i2c_client *client)
+{
+	struct ina230_data *data = i2c_get_clientdata(client);
+	int retval;
+
+	if (data->running)
+		return 0;
+
+	retval = i2c_smbus_write_word_data(client, INA230_CONFIG,
+				    __constant_cpu_to_be16(INA230_TRIG_CONFIG));
+	if (retval < 0)
+		dev_err(&client->dev, "config data write failed sts: 0x%x\n",
+			retval);
+
+	return retval;
+}
+
+
+static void ensure_enabled_end(struct i2c_client *client)
+{
+	struct ina230_data *data = i2c_get_clientdata(client);
+	int retval;
+
+	if (data->running)
+		return;
+
+	retval = i2c_smbus_write_word_data(client, INA230_CONFIG,
+				     __constant_cpu_to_be16(INA230_POWER_DOWN));
+	if (retval < 0)
+		dev_err(&client->dev, "power down failure sts: 0x%x\n",
+			retval);
+}
+
+
+static s32 __locked_power_down_ina230(struct i2c_client *client)
+{
+	s32 retval;
+	struct ina230_data *data = i2c_get_clientdata(client);
+
+	if (!data->running)
+		return 0;
+
+	retval = i2c_smbus_write_word_data(client, INA230_MASK, 0);
+	if (retval < 0)
+		dev_err(&client->dev, "mask write failure sts: 0x%x\n",
+			retval);
+
+	retval = i2c_smbus_write_word_data(client, INA230_CONFIG,
+				     __constant_cpu_to_be16(INA230_POWER_DOWN));
+	if (retval < 0)
+		dev_err(&client->dev, "power down failure sts: 0x%x\n",
+			retval);
+
+	data->running = false;
+
+	return retval;
+}
+
+
+static s32 power_down_ina230(struct i2c_client *client)
+{
+	s32 retval;
+	struct ina230_data *data = i2c_get_clientdata(client);
+
+	mutex_lock(&data->mutex);
+	retval = __locked_power_down_ina230(client);
+	mutex_unlock(&data->mutex);
+
+	return retval;
+}
+
+
+static s32 __locked_start_current_mon(struct i2c_client *client)
+{
+	s32 retval;
+	s16 shunt_limit;
+	s16 alert_mask;
+	struct ina230_data *data = i2c_get_clientdata(client);
+
+	if (!data->pdata->current_threshold) {
+		dev_err(&client->dev, "no current threshold specified\n");
+		return -EINVAL;
+	}
+
+	retval = i2c_smbus_write_word_data(client, INA230_CONFIG,
+				    __constant_cpu_to_be16(INA230_CONT_CONFIG));
+	if (retval < 0) {
+		dev_err(&client->dev, "config data write failed sts: 0x%x\n",
+			retval);
+		return retval;
+	}
+
+	shunt_limit = uv_to_alert_register(data->pdata->resistor *
+					   data->pdata->current_threshold);
+
+	retval = i2c_smbus_write_word_data(client, INA230_ALERT,
+					   cpu_to_be16(shunt_limit));
+	if (retval < 0) {
+		dev_err(&client->dev, "alert data write failed sts: 0x%x\n",
+			retval);
+		return retval;
+	}
+
+	alert_mask = shunt_limit >= 0 ? INA230_MASK_SOL : INA230_MASK_SUL;
+	retval = i2c_smbus_write_word_data(client, INA230_MASK,
+					   cpu_to_be16(alert_mask));
+	if (retval < 0) {
+		dev_err(&client->dev, "mask data write failed sts: 0x%x\n",
+			retval);
+		return retval;
+	}
+
+	data->running = true;
+
+	return 0;
+}
+
+
+static void __locked_evaluate_state(struct i2c_client *client)
+{
+	struct ina230_data *data = i2c_get_clientdata(client);
+	int cpus = num_online_cpus();
+
+	if (data->running) {
+		if (cpus < data->pdata->min_cores_online ||
+		    !data->pdata->current_threshold)
+			__locked_power_down_ina230(client);
+	} else {
+		if (cpus >= data->pdata->min_cores_online &&
+		    data->pdata->current_threshold)
+			__locked_start_current_mon(client);
+	}
+}
+
+
+static void evaluate_state(struct i2c_client *client)
+{
+	struct ina230_data *data = i2c_get_clientdata(client);
+
+	mutex_lock(&data->mutex);
+	__locked_evaluate_state(client);
+	mutex_unlock(&data->mutex);
+}
+
+
+static s32 show_rail_name(struct device *dev,
+			  struct device_attribute *attr,
+			  char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct ina230_data *data = i2c_get_clientdata(client);
+	return sprintf(buf, "%s\n", data->pdata->rail_name);
+}
+
+
+static s32 show_current_threshold(struct device *dev,
+				  struct device_attribute *attr,
+				  char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct ina230_data *data = i2c_get_clientdata(client);
+	return sprintf(buf, "%d mA\n", data->pdata->current_threshold);
+}
+
+
+static s32 set_current_threshold(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct ina230_data *data = i2c_get_clientdata(client);
+	s32 retval;
+
+	mutex_lock(&data->mutex);
+
+	if (strict_strtol(buf, 10, (long *)&(data->pdata->current_threshold))) {
+		retval = -EINVAL;
+		goto out;
+	}
+
+	if (data->pdata->current_threshold) {
+		if (data->running) {
+			/* force restart */
+			retval = __locked_start_current_mon(client);
+		} else {
+			__locked_evaluate_state(client);
+			retval = 0;
+		}
+	} else {
+		retval = __locked_power_down_ina230(client);
+	}
+
+out:
+	mutex_unlock(&data->mutex);
+	if (retval >= 0)
+		return count;
+	return retval;
+}
+
+
+
+
+#if MEASURE_BUS_VOLT
+static s32 show_bus_voltage(struct device *dev,
+			struct device_attribute *attr,
+			char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct ina230_data *data = i2c_get_clientdata(client);
+	s32 voltage_mV;
+	int retval;
+
+	mutex_lock(&data->mutex);
+	retval = ensure_enabled_start(client);
+	if (retval < 0) {
+		mutex_unlock(&data->mutex);
+		return retval;
+	}
+
+	/* getting voltage readings in milli volts*/
+	voltage_mV =
+		(s16)be16_to_cpu(i2c_smbus_read_word_data(client,
+							  INA230_VOLTAGE));
+
+	ensure_enabled_end(client);
+	mutex_unlock(&data->mutex);
+
+	if (voltage_mV < 0) {
+		dev_err(dev, "%s: failed\n", __func__);
+		return -1;
+	}
+
+	voltage_mV = busv_register_to_mv(voltage_mV);
+
+	return sprintf(buf, "%d mV\n", voltage_mV);
+}
+#endif
+
+
+
+
+static s32 show_shunt_voltage(struct device *dev,
+			struct device_attribute *attr,
+			char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct ina230_data *data = i2c_get_clientdata(client);
+	s32 voltage_uV;
+	int retval;
+
+	mutex_lock(&data->mutex);
+	retval = ensure_enabled_start(client);
+	if (retval < 0) {
+		mutex_unlock(&data->mutex);
+		return retval;
+	}
+
+	voltage_uV =
+		(s16)be16_to_cpu(i2c_smbus_read_word_data(client,
+							  INA230_SHUNT));
+
+	ensure_enabled_end(client);
+	mutex_unlock(&data->mutex);
+
+	voltage_uV = shuntv_register_to_uv(voltage_uV);
+
+	return sprintf(buf, "%d uV\n", voltage_uV);
+}
+
+
+static s32 show_current(struct device *dev,
+			struct device_attribute *attr,
+			char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct ina230_data *data = i2c_get_clientdata(client);
+	s32 voltage_uV;
+	s32 current_mA;
+	int retval;
+
+	mutex_lock(&data->mutex);
+	retval = ensure_enabled_start(client);
+	if (retval < 0) {
+		mutex_unlock(&data->mutex);
+		return retval;
+	}
+
+	voltage_uV =
+		(s16)be16_to_cpu(i2c_smbus_read_word_data(client,
+							  INA230_SHUNT));
+
+	ensure_enabled_end(client);
+	mutex_unlock(&data->mutex);
+
+	voltage_uV = shuntv_register_to_uv(voltage_uV);
+	current_mA = voltage_uV / data->pdata->resistor;
+
+	return sprintf(buf, "%d mA\n", current_mA);
+}
+
+
+static int ina230_hotplug_notify(struct notifier_block *nb, unsigned long event,
+				void *hcpu)
+{
+	struct ina230_data *data = container_of(nb, struct ina230_data,
+						nb);
+	struct i2c_client *client = data->client;
+
+	if (event == CPU_ONLINE || event == CPU_DEAD)
+		evaluate_state(client);
+
+	return 0;
+}
+
+
+
+static struct sensor_device_attribute ina230[] = {
+	SENSOR_ATTR(rail_name, S_IRUGO, show_rail_name, NULL, 0),
+	SENSOR_ATTR(current_threshold, S_IWUSR | S_IRUGO,
+		    show_current_threshold, set_current_threshold, 0),
+	SENSOR_ATTR(shuntvolt1_input, S_IRUGO, show_shunt_voltage, NULL, 0),
+	SENSOR_ATTR(current1_input, S_IRUGO, show_current, NULL, 0),
+#if MEASURE_BUS_VOLT
+	SENSOR_ATTR(busvolt1_input, S_IRUGO, show_bus_voltage, NULL, 0),
+#endif
+};
+
+
+static int __devinit ina230_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
+{
+	struct ina230_data *data;
+	int err;
+	u8 i;
+
+	data = devm_kzalloc(&client->dev, sizeof(struct ina230_data),
+			    GFP_KERNEL);
+	if (!data) {
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	i2c_set_clientdata(client, data);
+	data->pdata = client->dev.platform_data;
+	data->running = false;
+	data->nb.notifier_call = ina230_hotplug_notify;
+	data->client = client;
+	mutex_init(&data->mutex);
+
+	err = i2c_smbus_write_word_data(client, INA230_CONFIG,
+		__constant_cpu_to_be16(INA230_RESET));
+	if (err < 0) {
+		dev_err(&client->dev, "ina230 reset failure status: 0x%x\n",
+			err);
+		goto exit;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ina230); i++) {
+		err = device_create_file(&client->dev, &ina230[i].dev_attr);
+		if (err) {
+			dev_err(&client->dev, "device_create_file failed.\n");
+			goto exit_remove;
+		}
+	}
+
+	data->hwmon_dev = hwmon_device_register(&client->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		err = PTR_ERR(data->hwmon_dev);
+		goto exit_remove;
+	}
+
+	register_hotcpu_notifier(&(data->nb));
+
+	evaluate_state(client);
+
+	return 0;
+
+exit_remove:
+	for (i = 0; i < ARRAY_SIZE(ina230); i++)
+		device_remove_file(&client->dev, &ina230[i].dev_attr);
+exit:
+	return err;
+}
+
+
+static int __devexit ina230_remove(struct i2c_client *client)
+{
+	u8 i;
+	struct ina230_data *data = i2c_get_clientdata(client);
+	unregister_hotcpu_notifier(&(data->nb));
+	power_down_ina230(client);
+	hwmon_device_unregister(data->hwmon_dev);
+	for (i = 0; i < ARRAY_SIZE(ina230); i++)
+		device_remove_file(&client->dev, &ina230[i].dev_attr);
+	return 0;
+}
+
+
+static int ina230_suspend(struct i2c_client *client)
+{
+	return power_down_ina230(client);
+}
+
+
+static int ina230_resume(struct i2c_client *client)
+{
+	evaluate_state(client);
+	return 0;
+}
+
+
+static const struct i2c_device_id ina230_id[] = {
+	{DRIVER_NAME, 0 },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, ina230_id);
+
+
+static struct i2c_driver ina230_driver = {
+	.class		= I2C_CLASS_HWMON,
+	.driver = {
+		.name	= DRIVER_NAME,
+	},
+	.probe		= ina230_probe,
+	.remove		= __devexit_p(ina230_remove),
+	.suspend	= ina230_suspend,
+	.resume		= ina230_resume,
+	.id_table	= ina230_id,
+};
+
+
+static int __init ina230_init(void)
+{
+	return i2c_add_driver(&ina230_driver);
+}
+
+
+static void __exit ina230_exit(void)
+{
+	i2c_del_driver(&ina230_driver);
+}
+
+
+module_init(ina230_init);
+module_exit(ina230_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/jz4740-hwmon.c b/drivers/hwmon/jz4740-hwmon.c
index fea292d43407..b65a4dae3f5e 100644
--- a/drivers/hwmon/jz4740-hwmon.c
+++ b/drivers/hwmon/jz4740-hwmon.c
@@ -59,7 +59,7 @@ static ssize_t jz4740_hwmon_read_adcin(struct device *dev,
 {
 	struct jz4740_hwmon *hwmon = dev_get_drvdata(dev);
 	struct completion *completion = &hwmon->read_completion;
-	unsigned long t;
+	long t;
 	unsigned long val;
 	int ret;
 
diff --git a/drivers/hwmon/lm95245.c b/drivers/hwmon/lm95245.c
index dce9e68241e6..1b60fe6de776 100644
--- a/drivers/hwmon/lm95245.c
+++ b/drivers/hwmon/lm95245.c
@@ -34,6 +34,8 @@
 #include <linux/mutex.h>
 #include <linux/sysfs.h>
 
+#include <linux/lm95245.h>
+
 #define DEVNAME "lm95245"
 
 static const unsigned short normal_i2c[] = {
@@ -93,6 +95,7 @@ static const unsigned short normal_i2c[] = {
 #define RATE_CR1000	0x02
 #define RATE_CR2500	0x03
 
+#define STATUS1_ROS		0x10
 #define STATUS1_DIODE_FAULT	0x04
 #define STATUS1_RTCRIT		0x02
 #define STATUS1_LOC		0x01
@@ -107,12 +110,32 @@ static const u8 lm95245_reg_address[] = {
 	LM95245_REG_R_REMOTE_TEMPL_S,
 	LM95245_REG_R_REMOTE_TEMPH_U,
 	LM95245_REG_R_REMOTE_TEMPL_U,
+	LM95245_REG_RW_REMOTE_OS_LIMIT,
 	LM95245_REG_RW_LOCAL_OS_TCRIT_LIMIT,
 	LM95245_REG_RW_REMOTE_TCRIT_LIMIT,
 	LM95245_REG_RW_COMMON_HYSTERESIS,
 	LM95245_REG_R_STATUS1,
 };
 
+/* Indices and offsets into above register array */
+
+enum {
+	INDEX_LOCAL_TEMP = 0,
+	INDEX_REMOTE_TEMP = 2,
+	INDEX_REMOTE_OS_LIMIT = 6,
+	INDEX_LOCAL_OS_TCRIT_LIMIT,
+	INDEX_REMOTE_TCRIT_LIMIT,
+	INDEX_COMMON_HYSTERESIS,
+	INDEX_STATUS1,
+};
+
+enum {
+	OFFSET_HIGH_SIGNED = 0,
+	OFFSET_LOW_SIGNED,
+	OFFSET_HIGH_UNSIGNED,
+	OFFSET_LOW_UNSIGNED,
+};
+
 /* Client data (each client gets its own) */
 struct lm95245_data {
 	struct device *hwmon_dev;
@@ -214,24 +237,42 @@ static unsigned long lm95245_set_conversion_rate(struct i2c_client *client,
 }
 
 /* Sysfs stuff */
-static ssize_t show_input(struct device *dev, struct device_attribute *attr,
-			  char *buf)
+void thermal_get_temp(struct device *dev, int *temp, int index)
 {
 	struct lm95245_data *data = lm95245_update_device(dev);
-	int temp;
-	int index = to_sensor_dev_attr(attr)->index;
 
 	/*
-	 * Index 0 (Local temp) is always signed
-	 * Index 2 (Remote temp) has both signed and unsigned data
+	 * Local temp is always signed
+	 * Remote temp has both signed and unsigned data
 	 * use signed calculation for remote if signed bit is set
 	 */
-	if (index == 0 || data->regs[index] & 0x80)
-		temp = temp_from_reg_signed(data->regs[index],
-			    data->regs[index + 1]);
+	if (index == INDEX_LOCAL_TEMP || data->regs[index + OFFSET_HIGH_SIGNED] & 0x80)
+		*temp = temp_from_reg_signed(data->regs[index + OFFSET_HIGH_SIGNED],
+			    data->regs[index + OFFSET_LOW_SIGNED]);
 	else
-		temp = temp_from_reg_unsigned(data->regs[index + 2],
-			    data->regs[index + 3]);
+		*temp = temp_from_reg_unsigned(data->regs[index + OFFSET_HIGH_UNSIGNED],
+			    data->regs[index + OFFSET_LOW_UNSIGNED]);
+}
+
+void lm95245_get_local_temp(struct device *dev, int *temp)
+{
+	thermal_get_temp(dev, temp, INDEX_LOCAL_TEMP);
+}
+EXPORT_SYMBOL(lm95245_get_local_temp);
+
+void lm95245_get_remote_temp(struct device *dev, int *temp)
+{
+	thermal_get_temp(dev, temp, INDEX_REMOTE_TEMP);
+}
+EXPORT_SYMBOL(lm95245_get_remote_temp);
+
+static ssize_t show_input(struct device *dev, struct device_attribute *attr,
+			  char *buf)
+{
+	int temp = 0;
+	int index = to_sensor_dev_attr(attr)->index;
+
+	thermal_get_temp(dev, &temp, index);
 
 	return snprintf(buf, PAGE_SIZE - 1, "%d\n", temp);
 }
@@ -246,20 +287,14 @@ static ssize_t show_limit(struct device *dev, struct device_attribute *attr,
 			data->regs[index] * 1000);
 }
 
-static ssize_t set_limit(struct device *dev, struct device_attribute *attr,
-			const char *buf, size_t count)
+void thermal_set_limit(struct device *dev, int val, int index)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	struct lm95245_data *data = i2c_get_clientdata(client);
-	int index = to_sensor_dev_attr(attr)->index;
-	unsigned long val;
-
-	if (strict_strtoul(buf, 10, &val) < 0)
-		return -EINVAL;
 
 	val /= 1000;
 
-	val = SENSORS_LIMIT(val, 0, (index == 6 ? 127 : 255));
+	val = SENSORS_LIMIT(val, 0, (index == INDEX_LOCAL_OS_TCRIT_LIMIT ? 127 : 255));
 
 	mutex_lock(&data->update_lock);
 
@@ -268,6 +303,36 @@ static ssize_t set_limit(struct device *dev, struct device_attribute *attr,
 	i2c_smbus_write_byte_data(client, lm95245_reg_address[index], val);
 
 	mutex_unlock(&data->update_lock);
+}
+
+void lm95245_set_remote_os_limit(struct device *dev, int val)
+{
+	thermal_set_limit(dev, val, INDEX_REMOTE_OS_LIMIT);
+}
+EXPORT_SYMBOL(lm95245_set_remote_os_limit);
+
+void lm95245_set_remote_critical_limit(struct device *dev, int val)
+{
+	thermal_set_limit(dev, val, INDEX_REMOTE_TCRIT_LIMIT);
+}
+EXPORT_SYMBOL(lm95245_set_remote_critical_limit);
+
+void lm95245_set_local_shared_os__critical_limit(struct device *dev, int val)
+{
+	thermal_set_limit(dev, val, INDEX_LOCAL_OS_TCRIT_LIMIT);
+}
+EXPORT_SYMBOL(lm95245_set_local_shared_os__critical_limit);
+
+static ssize_t set_limit(struct device *dev, struct device_attribute *attr,
+			const char *buf, size_t count)
+{
+	int index = to_sensor_dev_attr(attr)->index;
+	unsigned long val;
+
+	if (strict_strtoul(buf, 10, &val) < 0)
+		return -EINVAL;
+
+	thermal_set_limit(dev, (int)val, index);
 
 	return count;
 }
@@ -345,7 +410,7 @@ static ssize_t show_alarm(struct device *dev, struct device_attribute *attr,
 	int index = to_sensor_dev_attr(attr)->index;
 
 	return snprintf(buf, PAGE_SIZE - 1, "%d\n",
-			!!(data->regs[9] & index));
+			!!(data->regs[INDEX_STATUS1] & index));
 }
 
 static ssize_t show_interval(struct device *dev, struct device_attribute *attr,
@@ -375,19 +440,23 @@ static ssize_t set_interval(struct device *dev, struct device_attribute *attr,
 	return count;
 }
 
-static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_input, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_input, NULL, INDEX_LOCAL_TEMP);
 static SENSOR_DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_limit,
-		set_limit, 6);
+		set_limit, INDEX_LOCAL_OS_TCRIT_LIMIT);
 static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_limit,
-		set_crit_hyst, 8);
+		set_crit_hyst, INDEX_COMMON_HYSTERESIS);
 static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO, show_alarm, NULL,
 		STATUS1_LOC);
 
-static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_input, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_input, NULL, INDEX_REMOTE_TEMP);
+static SENSOR_DEVICE_ATTR(temp2_os, S_IWUSR | S_IRUGO, show_limit,
+		set_limit, INDEX_REMOTE_OS_LIMIT);
 static SENSOR_DEVICE_ATTR(temp2_crit, S_IWUSR | S_IRUGO, show_limit,
-		set_limit, 7);
+		set_limit, INDEX_REMOTE_TCRIT_LIMIT);
 static SENSOR_DEVICE_ATTR(temp2_crit_hyst, S_IWUSR | S_IRUGO, show_limit,
-		set_crit_hyst, 8);
+		set_crit_hyst, INDEX_COMMON_HYSTERESIS);
+static SENSOR_DEVICE_ATTR(temp2_os_alarm, S_IRUGO, show_alarm, NULL,
+		STATUS1_ROS);
 static SENSOR_DEVICE_ATTR(temp2_crit_alarm, S_IRUGO, show_alarm, NULL,
 		STATUS1_RTCRIT);
 static SENSOR_DEVICE_ATTR(temp2_type, S_IWUSR | S_IRUGO, show_type,
@@ -404,8 +473,10 @@ static struct attribute *lm95245_attributes[] = {
 	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
 	&sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp2_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_os.dev_attr.attr,
 	&sensor_dev_attr_temp2_crit.dev_attr.attr,
 	&sensor_dev_attr_temp2_crit_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp2_os_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp2_crit_alarm.dev_attr.attr,
 	&sensor_dev_attr_temp2_type.dev_attr.attr,
 	&sensor_dev_attr_temp2_fault.dev_attr.attr,
@@ -454,6 +525,13 @@ static void lm95245_init_client(struct i2c_client *client)
 		i2c_smbus_write_byte_data(client, LM95245_REG_RW_CONFIG1,
 			data->config1);
 	}
+
+	/* Configure over-temperature shutdown (OS) output pin */
+	if (client->dev.platform_data && ((struct lm95245_platform_data*)(client->dev.platform_data))->enable_os_pin) {
+		data->config2 |= CFG2_OS_A0;
+		i2c_smbus_write_byte_data(client, LM95245_REG_RW_CONFIG2,
+			data->config2);
+	}
 }
 
 static int lm95245_probe(struct i2c_client *new_client,
@@ -485,6 +563,10 @@ static int lm95245_probe(struct i2c_client *new_client,
 		goto exit_remove_files;
 	}
 
+	/* Notify callback that probe is done */
+	if (new_client->dev.platform_data && ((struct lm95245_platform_data*)(new_client->dev.platform_data))->probe_callback)
+		((struct lm95245_platform_data*)(new_client->dev.platform_data))->probe_callback(&new_client->dev);
+
 	return 0;
 
 exit_remove_files:
diff --git a/drivers/hwmon/sht15.c b/drivers/hwmon/sht15.c
index fe4104c6b764..535792550c6d 100644
--- a/drivers/hwmon/sht15.c
+++ b/drivers/hwmon/sht15.c
@@ -883,7 +883,7 @@ static int sht15_invalidate_voltage(struct notifier_block *nb,
 
 static int __devinit sht15_probe(struct platform_device *pdev)
 {
-	int ret = 0;
+	int ret;
 	struct sht15_data *data = kzalloc(sizeof(*data), GFP_KERNEL);
 	u8 status = 0;
 
@@ -901,6 +901,7 @@ static int __devinit sht15_probe(struct platform_device *pdev)
 	init_waitqueue_head(&data->wait_queue);
 
 	if (pdev->dev.platform_data == NULL) {
+		ret = -EINVAL;
 		dev_err(&pdev->dev, "no platform data supplied\n");
 		goto err_free_data;
 	}
diff --git a/drivers/hwmon/tegra-tsensor.c b/drivers/hwmon/tegra-tsensor.c
new file mode 100644
index 000000000000..c5b16fe97063
--- /dev/null
+++ b/drivers/hwmon/tegra-tsensor.c
@@ -0,0 +1,2063 @@
+/*
+ * NVIDIA Tegra SOC - temperature sensor driver
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/hwmon.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/irqreturn.h>
+#include <linux/err.h>
+#include <linux/spinlock.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/hwmon.h>
+#include <linux/regulator/consumer.h>
+#include <linux/delay.h>
+
+#include <mach/iomap.h>
+#include <mach/clk.h>
+#include <mach/delay.h>
+#include <mach/tsensor.h>
+#include <mach/tegra_fuse.h>
+
+/* macro to enable tsensor hw reset */
+/* FIXME: till tsensor temperature is reliable this should be 0 */
+#define ENABLE_TSENSOR_HW_RESET 1
+
+/* tsensor instance used for temperature calculation */
+#define TSENSOR_FUSE_REV1	8
+#define TSENSOR_FUSE_REV2	21
+
+/* version where tsensor temperature reading is accurate */
+#define STABLE_TSENSOR_FUSE_REV TSENSOR_FUSE_REV2
+
+/* We have multiple tsensor instances with following registers */
+#define SENSOR_CFG0				0x40
+#define SENSOR_CFG1				0x48
+#define SENSOR_CFG2				0x4c
+#define SENSOR_STATUS0				0x58
+#define SENSOR_TS_STATUS1			0x5c
+#define SENSOR_TS_STATUS2			0x60
+
+/* interrupt mask in tsensor status register */
+#define TSENSOR_SENSOR_X_STATUS0_0_INTR_MASK	(1 << 8)
+
+#define SENSOR_CFG0_M_MASK			0xffff
+#define SENSOR_CFG0_M_SHIFT			8
+#define SENSOR_CFG0_N_MASK			0xff
+#define SENSOR_CFG0_N_SHIFT			24
+#define SENSOR_CFG0_RST_INTR_SHIFT		6
+#define SENSOR_CFG0_HW_DIV2_INTR_SHIFT		5
+#define SENSOR_CFG0_OVERFLOW_INTR		4
+#define SENSOR_CFG0_DVFS_INTR_SHIFT		3
+#define SENSOR_CFG0_RST_ENABLE_SHIFT		2
+#define SENSOR_CFG0_HW_DIV2_ENABLE_SHIFT	1
+#define SENSOR_CFG0_STOP_SHIFT			0
+
+#define SENSOR_CFG_X_TH_X_MASK			0xffff
+#define SENSOR_CFG1_TH2_SHIFT			16
+#define SENSOR_CFG1_TH1_SHIFT			0
+#define SENSOR_CFG2_TH3_SHIFT			0
+#define SENSOR_CFG2_TH0_SHIFT			16
+
+#define SENSOR_STATUS_AVG_VALID_SHIFT		10
+#define SENSOR_STATUS_CURR_VALID_SHIFT		9
+
+#define STATE_MASK				0x7
+#define STATUS0_STATE_SHIFT			0
+#define STATUS0_PREV_STATE_SHIFT		4
+
+#define LOCAL_STR_SIZE1				60
+#define MAX_STR_LINE				100
+#define MAX_TSENSOR_LOOP1			(1000 * 2)
+
+#define TSENSOR_COUNTER_TOLERANCE		100
+
+#define SENSOR_CTRL_RST_SHIFT			1
+#define RST_SRC_MASK				0x7
+#define RST_SRC_SENSOR				2
+#define TEGRA_REV_REG_OFFSET			0x804
+#define CCLK_G_BURST_POLICY_REG_REL_OFFSET	0x368
+#define TSENSOR_SLOWDOWN_BIT			23
+
+/* macros used for temperature calculations */
+/* assumed get_temperature_int and get_temperature_fraction
+ * calculate up to 6 decimal places. print temperature
+ * in code assumes 6 decimal place formatting */
+#define get_temperature_int(X)			((X) / 1000000)
+#define get_temperature_fraction(X)		(((int)(abs(X))) % 1000000)
+
+#define get_temperature_round(X)		DIV_ROUND_CLOSEST(X, 1000000)
+
+#define MILLICELSIUS_TO_CELSIUS(i)		((i) / 1000)
+#define CELSIUS_TO_MILLICELSIUS(i)		((i) * 1000)
+
+#define TSENSOR_MILLI_CELSIUS(x) \
+	DIV_ROUND_CLOSEST((x), 1000)
+
+#define get_ts_state(data) tsensor_get_reg_field(data,\
+			((data->instance << 16) | SENSOR_STATUS0), \
+			STATUS0_STATE_SHIFT, STATE_MASK)
+
+/* tsensor states */
+enum ts_state {
+	TS_INVALID = 0,
+	TS_LEVEL0,
+	TS_LEVEL1,
+	TS_LEVEL2,
+	TS_LEVEL3,
+	TS_OVERFLOW,
+	TS_MAX_STATE = TS_OVERFLOW
+};
+
+enum {
+	/* temperature is sensed from 2 points on tegra */
+	TSENSOR_COUNT = 2,
+	TSENSOR_INSTANCE1 = 0,
+	TSENSOR_INSTANCE2 = 1,
+	/* divide by 2 temperature threshold */
+	DIV2_CELSIUS_TEMP_THRESHOLD_DEFAULT = 70,
+	/* reset chip temperature threshold */
+	RESET_CELSIUS_TEMP_THRESHOLD_DEFAULT = 75,
+	/* tsensor frequency in Hz for clk src CLK_M and divisor=24 */
+	DEFAULT_TSENSOR_CLK_HZ = 500000,
+	DEFAULT_TSENSOR_N = 255,
+	DEFAULT_TSENSOR_M = 12500,
+	/* tsensor instance offset */
+	TSENSOR_INSTANCE_OFFSET = 0x40,
+	MIN_THRESHOLD = 0x0,
+	MAX_THRESHOLD = 0xffff,
+	DEFAULT_THRESHOLD_TH0 = MAX_THRESHOLD,
+	DEFAULT_THRESHOLD_TH1 = MAX_THRESHOLD,
+	DEFAULT_THRESHOLD_TH2 = MAX_THRESHOLD,
+	DEFAULT_THRESHOLD_TH3 = MAX_THRESHOLD,
+};
+
+/* constants used to implement sysfs interface */
+enum tsensor_params {
+	TSENSOR_PARAM_TH1 = 0,
+	TSENSOR_PARAM_TH2,
+	TSENSOR_PARAM_TH3,
+	TSENSOR_TEMPERATURE,
+	TSENSOR_STATE,
+	TSENSOR_LIMITS,
+};
+
+enum tsensor_thresholds {
+	TSENSOR_TH0 = 0,
+	TSENSOR_TH1,
+	TSENSOR_TH2,
+	TSENSOR_TH3
+};
+
+/*
+ * For each registered chip, we need to keep some data in memory.
+ * The structure is dynamically allocated.
+ */
+struct tegra_tsensor_data {
+	struct delayed_work work;
+	struct workqueue_struct *workqueue;
+	struct mutex mutex;
+	struct device *hwmon_dev;
+	spinlock_t tsensor_lock;
+	struct clk *dev_clk;
+	/* tsensor register space */
+	void __iomem		*base;
+	unsigned long		phys;
+	unsigned long		phys_end;
+	/* pmc register space */
+	void __iomem		*pmc_rst_base;
+	unsigned long		pmc_phys;
+	unsigned long		pmc_phys_end;
+	/* clk register space */
+	void __iomem		*clk_rst_base;
+	int			irq;
+
+	/* save configuration before suspend and restore after resume */
+	unsigned int config0[TSENSOR_COUNT];
+	unsigned int config1[TSENSOR_COUNT];
+	unsigned int config2[TSENSOR_COUNT];
+	/* temperature readings from instance tsensor - 0/1 */
+	unsigned int instance;
+	s64 A_e_minus12;
+	int B_e_minus6;
+	unsigned int fuse_T1;
+	unsigned int fuse_F1;
+	unsigned int fuse_T2;
+	unsigned int fuse_F2;
+	/* Quadratic fit coefficients: m=-0.003512 n=1.528943 p=-11.1 */
+	int m_e_minus6;
+	int n_e_minus6;
+	int p_e_minus2;
+
+	long current_hi_limit;
+	long current_lo_limit;
+
+	bool is_edp_supported;
+
+	void (*alert_func)(void *);
+	void *alert_data;
+};
+
+enum {
+	TSENSOR_COEFF_SET1 = 0,
+	TSENSOR_COEFF_SET2,
+	TSENSOR_COEFF_END
+};
+
+struct tegra_tsensor_coeff {
+	int e_minus6_m;
+	int e_minus6_n;
+	int e_minus2_p;
+};
+
+static struct tegra_tsensor_coeff coeff_table[] = {
+	/* Quadratic fit coefficients: m=-0.002775 n=1.338811 p=-7.30 */
+	[TSENSOR_COEFF_SET1] = {
+		-2775,
+		1338811,
+		-730
+	},
+	/* Quadratic fit coefficients: m=-0.003512 n=1.528943 p=-11.1 */
+	[TSENSOR_COEFF_SET2] = {
+		-3512,
+		1528943,
+		-1110
+	}
+	/* FIXME: add tsensor coefficients after chip characterization */
+};
+
+/* pTemperature returned in 100 * Celsius */
+static int tsensor_count_2_temp(struct tegra_tsensor_data *data,
+	unsigned int count, int *p_temperature);
+static unsigned int tsensor_get_threshold_counter(
+	struct tegra_tsensor_data *data, int temp);
+
+/* tsensor register access functions */
+
+static void tsensor_writel(struct tegra_tsensor_data *data, u32 val,
+				unsigned long reg)
+{
+	unsigned int reg_offset = reg & 0xffff;
+	unsigned char inst = (reg >> 16) & 0xffff;
+	writel(val, data->base + (inst * TSENSOR_INSTANCE_OFFSET) +
+		reg_offset);
+	return;
+}
+
+static unsigned int tsensor_readl(struct tegra_tsensor_data *data,
+				unsigned long reg)
+{
+	unsigned int reg_offset = reg & 0xffff;
+	unsigned char inst = (reg >> 16) & 0xffff;
+	return readl(data->base +
+		(inst * TSENSOR_INSTANCE_OFFSET) + reg_offset);
+}
+
+static unsigned int tsensor_get_reg_field(
+	struct tegra_tsensor_data *data, unsigned int reg,
+	unsigned int shift, unsigned int mask)
+{
+	unsigned int reg_val;
+	reg_val = tsensor_readl(data, reg);
+	return (reg_val & (mask << shift)) >> shift;
+}
+
+static int tsensor_set_reg_field(
+	struct tegra_tsensor_data *data, unsigned int value,
+	unsigned int reg, unsigned int shift, unsigned int mask)
+{
+	unsigned int reg_val;
+	unsigned int rd_val;
+	reg_val = tsensor_readl(data, reg);
+	reg_val &= ~(mask << shift);
+	reg_val |= ((value & mask) << shift);
+	tsensor_writel(data, reg_val, reg);
+	rd_val = tsensor_readl(data, reg);
+	if (rd_val == reg_val)
+		return 0;
+	else
+		return -EINVAL;
+}
+
+/* enable argument is true to enable reset, false disables pmc reset */
+static void pmc_rst_enable(struct tegra_tsensor_data *data, bool enable)
+{
+	unsigned int val;
+	/* mapped first pmc reg is SENSOR_CTRL */
+	val = readl(data->pmc_rst_base);
+	if (enable)
+		val |= (1 << SENSOR_CTRL_RST_SHIFT);
+	else
+		val &= ~(1 << SENSOR_CTRL_RST_SHIFT);
+	writel(val, data->pmc_rst_base);
+}
+
+/* true returned when pmc reset source is tsensor */
+static bool pmc_check_rst_sensor(struct tegra_tsensor_data *data)
+{
+	unsigned int val;
+	unsigned char src;
+	val = readl(data->pmc_rst_base + 4);
+	src = (unsigned char)(val & RST_SRC_MASK);
+	if (src == RST_SRC_SENSOR)
+		return true;
+	else
+		return false;
+}
+
+/* function to get chip revision */
+static void get_chip_rev(unsigned short *p_id, unsigned short *p_major,
+		unsigned short *p_minor)
+{
+	unsigned int reg;
+
+	reg = readl(IO_TO_VIRT(TEGRA_APB_MISC_BASE) +
+		TEGRA_REV_REG_OFFSET);
+	*p_id = (reg >> 8) & 0xff;
+	*p_major = (reg >> 4) & 0xf;
+	*p_minor = (reg >> 16) & 0xf;
+	pr_info("Tegra chip revision for tsensor detected as: "
+		"Chip Id=%x, Major=%d, Minor=%d\n", (int)*p_id,
+		(int)*p_major, (int)*p_minor);
+}
+
+/*
+ * function to get chip revision specific tsensor coefficients
+ * obtained after chip characterization
+ */
+static void get_chip_tsensor_coeff(struct tegra_tsensor_data *data)
+{
+	unsigned short chip_id, major_rev, minor_rev;
+	unsigned short coeff_index;
+
+	get_chip_rev(&chip_id, &major_rev, &minor_rev);
+	switch (minor_rev) {
+	default:
+		pr_info("Warning: tsensor coefficient for chip pending\n");
+	case 1:
+		coeff_index = TSENSOR_COEFF_SET1;
+		break;
+	}
+	if (data->instance == TSENSOR_INSTANCE1)
+		coeff_index = TSENSOR_COEFF_SET2;
+	data->m_e_minus6 = coeff_table[coeff_index].e_minus6_m;
+	data->n_e_minus6 = coeff_table[coeff_index].e_minus6_n;
+	data->p_e_minus2 = coeff_table[coeff_index].e_minus2_p;
+	pr_info("tsensor coeff: m=%d*10^-6,n=%d*10^-6,p=%d*10^-2\n",
+		data->m_e_minus6, data->n_e_minus6, data->p_e_minus2);
+}
+
+/* tsensor counter read function */
+static int tsensor_read_counter(
+	struct tegra_tsensor_data *data,
+	unsigned int *p_counter)
+{
+	unsigned int status_reg;
+	unsigned int config0;
+	int iter_count = 0;
+	const int max_loop = 50;
+
+	do {
+		config0 = tsensor_readl(data, ((data->instance << 16) |
+			SENSOR_CFG0));
+		if (config0 & (1 << SENSOR_CFG0_STOP_SHIFT)) {
+			dev_dbg(data->hwmon_dev, "Error: tsensor "
+				"counter read with STOP bit not supported\n");
+			*p_counter = 0;
+			return 0;
+		}
+
+		status_reg = tsensor_readl(data,
+			(data->instance << 16) | SENSOR_STATUS0);
+		if (status_reg & (1 <<
+			SENSOR_STATUS_CURR_VALID_SHIFT)) {
+			*p_counter = tsensor_readl(data, (data->instance
+				<< 16) | SENSOR_TS_STATUS1);
+			break;
+		}
+		if (!(iter_count % 10))
+			dev_dbg(data->hwmon_dev, "retry %d\n", iter_count);
+
+		msleep(21);
+		iter_count++;
+	} while (iter_count < max_loop);
+
+	if (iter_count == max_loop)
+		return -ENODEV;
+
+	return 0;
+}
+
+/* tsensor threshold print function */
+static void dump_threshold(struct tegra_tsensor_data *data)
+{
+	unsigned int TH_2_1, TH_0_3;
+	unsigned int curr_avg;
+	int err;
+
+	TH_2_1 = tsensor_readl(data, (data->instance << 16) | SENSOR_CFG1);
+	TH_0_3 = tsensor_readl(data, (data->instance << 16) | SENSOR_CFG2);
+	dev_dbg(data->hwmon_dev, "Tsensor: TH_2_1=0x%x, "
+		"TH_0_3=0x%x\n", TH_2_1, TH_0_3);
+	err = tsensor_read_counter(data, &curr_avg);
+	if (err < 0)
+		pr_err("Error: tsensor counter read, "
+			"err=%d\n", err);
+	else
+		dev_dbg(data->hwmon_dev, "Tsensor: "
+			"curr_avg=0x%x\n", curr_avg);
+}
+
+static int tsensor_get_temperature(
+	struct tegra_tsensor_data *data,
+	int *pTemp, unsigned int *pCounter)
+{
+	int err = 0;
+	unsigned int curr_avg;
+
+	err = tsensor_read_counter(data, &curr_avg);
+	if (err < 0)
+		goto error;
+
+	*pCounter = ((curr_avg & 0xFFFF0000) >> 16);
+	err = tsensor_count_2_temp(data, *pCounter, pTemp);
+
+error:
+	return err;
+}
+
+static ssize_t tsensor_show_state(struct device *dev,
+	struct device_attribute *da, char *buf)
+{
+	int state;
+	struct tegra_tsensor_data *data = dev_get_drvdata(dev);
+
+	state = get_ts_state(data);
+
+	return snprintf(buf, 50, "%d\n", state);
+}
+
+static ssize_t tsensor_show_limits(struct device *dev,
+	struct device_attribute *da, char *buf)
+{
+	struct tegra_tsensor_data *data = dev_get_drvdata(dev);
+	return snprintf(buf, 50, "%ld %ld\n",
+		data->current_lo_limit, data->current_hi_limit);
+}
+
+/* tsensor temperature show function */
+static ssize_t tsensor_show_counters(struct device *dev,
+	struct device_attribute *da, char *buf)
+{
+	unsigned int curr_avg;
+	char err_str[] = "error-sysfs-counter-read\n";
+	char fixed_str[MAX_STR_LINE];
+	struct tegra_tsensor_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	int err;
+	int temp;
+
+	if (attr->index == TSENSOR_TEMPERATURE) {
+		/* use current counter value to calculate temperature */
+		err = tsensor_read_counter(data, &curr_avg);
+		if (err < 0)
+			goto error;
+		err = tsensor_count_2_temp(data,
+			((curr_avg & 0xFFFF0000) >> 16), &temp);
+		if (err < 0)
+			goto error;
+
+		dev_vdbg(data->hwmon_dev, "%s has curr_avg=0x%x, "
+			"temp0=%d\n", __func__, curr_avg, temp);
+
+		snprintf(buf, (((LOCAL_STR_SIZE1 << 1) + 3) +
+			strlen(fixed_str)),
+			"%d.%06dC %#x\n",
+			get_temperature_int(temp),
+			get_temperature_fraction(temp),
+			((curr_avg & 0xFFFF0000) >> 16));
+	}
+	return strlen(buf);
+error:
+	return snprintf(buf, strlen(err_str), "%s", err_str);
+}
+
+/* utility function to check hw clock divide by 2 condition */
+static bool cclkg_check_hwdiv2_sensor(struct tegra_tsensor_data *data)
+{
+	unsigned int val;
+	val = readl(IO_ADDRESS(TEGRA_CLK_RESET_BASE +
+		CCLK_G_BURST_POLICY_REG_REL_OFFSET));
+	if ((1 << TSENSOR_SLOWDOWN_BIT) & val) {
+		dev_err(data->hwmon_dev, "Warning: ***** tsensor "
+			"slowdown bit detected\n");
+		return true;
+	} else {
+		return false;
+	}
+}
+
+/*
+ * function with table to return register, field shift and mask
+ * values for supported parameters
+ */
+static int get_param_values(
+	struct tegra_tsensor_data *data, unsigned int indx,
+	unsigned int *p_reg, unsigned int *p_sft, unsigned int *p_msk,
+	char *info, size_t info_len)
+{
+	switch (indx) {
+	case TSENSOR_PARAM_TH1:
+		*p_reg = ((data->instance << 16) | SENSOR_CFG1);
+		*p_sft = SENSOR_CFG1_TH1_SHIFT;
+		*p_msk = SENSOR_CFG_X_TH_X_MASK;
+		snprintf(info, info_len, "TH1[%d]: ",
+			data->instance);
+		break;
+	case TSENSOR_PARAM_TH2:
+		*p_reg = ((data->instance << 16) | SENSOR_CFG1);
+		*p_sft = SENSOR_CFG1_TH2_SHIFT;
+		*p_msk = SENSOR_CFG_X_TH_X_MASK;
+		snprintf(info, info_len, "TH2[%d]: ",
+			data->instance);
+		break;
+	case TSENSOR_PARAM_TH3:
+		*p_reg = ((data->instance << 16) | SENSOR_CFG2);
+		*p_sft = SENSOR_CFG2_TH3_SHIFT;
+		*p_msk = SENSOR_CFG_X_TH_X_MASK;
+		snprintf(info, info_len, "TH3[%d]: ",
+			data->instance);
+		break;
+	default:
+		return -ENOENT;
+	}
+	return 0;
+}
+
+/* tsensor driver sysfs show function */
+static ssize_t show_tsensor_param(struct device *dev,
+				struct device_attribute *da,
+				char *buf)
+{
+	unsigned int val;
+	struct tegra_tsensor_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	unsigned int reg;
+	unsigned int sft;
+	unsigned int msk;
+	int err;
+	int temp;
+	char info[LOCAL_STR_SIZE1];
+
+	err = get_param_values(data, attr->index, &reg, &sft, &msk,
+			       info, sizeof(info));
+	if (err < 0)
+		goto labelErr;
+	val = tsensor_get_reg_field(data, reg, sft, msk);
+	if (val == MAX_THRESHOLD)
+		snprintf(buf, PAGE_SIZE, "%s un-initialized threshold\n", info);
+	else {
+		err = tsensor_count_2_temp(data, val, &temp);
+		if (err != 0)
+			goto labelErr;
+		snprintf(buf, PAGE_SIZE, "%s threshold: %d.%06d Celsius\n",
+			info, get_temperature_int(temp),
+			get_temperature_fraction(temp));
+	}
+	return strlen(buf);
+
+labelErr:
+	snprintf(buf, PAGE_SIZE, "ERROR:");
+	return strlen(buf);
+}
+
+/* tsensor driver sysfs store function */
+static ssize_t set_tsensor_param(struct device *dev,
+			struct device_attribute *da,
+			const char *buf, size_t count)
+{
+	int num;
+	struct tegra_tsensor_data *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(da);
+	unsigned int reg;
+	unsigned int sft;
+	unsigned int msk;
+	int err;
+	unsigned int counter;
+	unsigned int val;
+	char info[LOCAL_STR_SIZE1];
+
+	if (kstrtoint(buf, 0, &num)) {
+		dev_err(dev, "file: %s, line=%d return %s()\n",
+			__FILE__, __LINE__, __func__);
+		return -EINVAL;
+	}
+
+	counter = tsensor_get_threshold_counter(data, num);
+
+	err = get_param_values(data, attr->index, &reg, &sft, &msk,
+			       info, sizeof(info));
+	if (err < 0)
+		goto labelErr;
+
+	err = tsensor_set_reg_field(data, counter, reg, sft, msk);
+	if (err < 0)
+		goto labelErr;
+
+	/* TH2 clk divide check */
+	if (attr->index == TSENSOR_PARAM_TH2) {
+		msleep(21);
+		(void)cclkg_check_hwdiv2_sensor(data);
+	}
+	val = tsensor_get_reg_field(data, reg, sft, msk);
+	dev_dbg(dev, "%s 0x%x\n", info, val);
+	return count;
+labelErr:
+	dev_err(dev, "file: %s, line=%d, %s(), error=0x%x\n", __FILE__,
+		__LINE__, __func__, err);
+	return 0;
+}
+
+static struct sensor_device_attribute tsensor_nodes[] = {
+	SENSOR_ATTR(tsensor_TH1, S_IRUGO | S_IWUSR,
+		show_tsensor_param, set_tsensor_param, TSENSOR_PARAM_TH1),
+	SENSOR_ATTR(tsensor_TH2, S_IRUGO | S_IWUSR,
+		show_tsensor_param, set_tsensor_param, TSENSOR_PARAM_TH2),
+	SENSOR_ATTR(tsensor_TH3, S_IRUGO | S_IWUSR,
+		show_tsensor_param, set_tsensor_param, TSENSOR_PARAM_TH3),
+	SENSOR_ATTR(tsensor_temperature, S_IRUGO | S_IWUSR,
+		tsensor_show_counters, NULL, TSENSOR_TEMPERATURE),
+	SENSOR_ATTR(tsensor_state, S_IRUGO | S_IWUSR,
+		tsensor_show_state, NULL, TSENSOR_STATE),
+	SENSOR_ATTR(tsensor_limits, S_IRUGO | S_IWUSR,
+		tsensor_show_limits, NULL, TSENSOR_LIMITS),
+};
+
+int tsensor_thermal_get_temp_low(struct tegra_tsensor_data *data,
+					long *milli_temp)
+{
+	/* temp to counter below 20C seems to be inaccurate */
+	*milli_temp = 0;
+	return 0;
+}
+
+int tsensor_thermal_get_temp(struct tegra_tsensor_data *data,
+				long *milli_temp)
+{
+	int counter, temp, err;
+	int temp_state, ts_state;
+
+	err = tsensor_get_temperature(data,
+					&temp,
+					&counter);
+	if (err)
+		return err;
+
+	/* temperature is in milli-Celsius */
+	temp = TSENSOR_MILLI_CELSIUS(temp);
+
+	mutex_lock(&data->mutex);
+
+	/* This section of logic is done in order to make sure that
+	 * the temperature read corresponds to the current hw state.
+	 * If it is not, return the nearest temperature
+	 */
+	if ((data->current_lo_limit != 0) ||
+		(data->current_hi_limit)) {
+
+		if (temp <= data->current_lo_limit)
+			temp_state = TS_LEVEL0;
+		else if (temp < data->current_hi_limit)
+			temp_state = TS_LEVEL1;
+		else
+			temp_state = TS_LEVEL2;
+
+		ts_state = get_ts_state(data);
+
+		if (ts_state != temp_state) {
+
+			switch (ts_state) {
+			case TS_LEVEL0:
+				temp = data->current_lo_limit - 1;
+				break;
+			case TS_LEVEL1:
+				if (temp_state == TS_LEVEL0)
+					temp = data->current_lo_limit + 1;
+				else
+					temp = data->current_hi_limit - 1;
+				break;
+			case TS_LEVEL2:
+				temp = data->current_hi_limit + 1;
+				break;
+			}
+
+		}
+
+	}
+
+	mutex_unlock(&data->mutex);
+
+	*milli_temp = temp;
+
+	return 0;
+}
+
+/* tsensor driver interrupt handler */
+static irqreturn_t tegra_tsensor_isr(int irq, void *arg_data)
+{
+	struct tegra_tsensor_data *data =
+		(struct tegra_tsensor_data *)arg_data;
+	unsigned long flags;
+	unsigned int val;
+	int new_state;
+
+	spin_lock_irqsave(&data->tsensor_lock, flags);
+
+	val = tsensor_readl(data, (data->instance << 16) | SENSOR_STATUS0);
+	if (val & TSENSOR_SENSOR_X_STATUS0_0_INTR_MASK) {
+		new_state = get_ts_state(data);
+
+		/* counter overflow check */
+		if (new_state == TS_OVERFLOW)
+			dev_err(data->hwmon_dev, "Warning: "
+				"***** OVERFLOW tsensor\n");
+
+		/* We only care if we go above hi or below low thresholds */
+		if (data->is_edp_supported && new_state != TS_LEVEL1)
+			queue_delayed_work(data->workqueue, &data->work, 0);
+	}
+
+	tsensor_writel(data, val, (data->instance << 16) | SENSOR_STATUS0);
+
+	spin_unlock_irqrestore(&data->tsensor_lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * function to read fuse registers and give - T1, T2, F1 and F2
+ */
+static int read_tsensor_fuse_regs(struct tegra_tsensor_data *data)
+{
+	unsigned int reg1;
+	unsigned int T1 = 0, T2 = 0;
+	unsigned int spare_bits;
+	int err;
+
+	/* read tsensor calibration register */
+	/*
+	 * High (~90 DegC) Temperature Calibration value (upper 16 bits of
+	 * FUSE_TSENSOR_CALIB_0) - F2
+	 * Low (~25 deg C) Temperature Calibration value (lower 16 bits of
+	 * FUSE_TSENSOR_CALIB_0) - F1
+	 */
+	err = tegra_fuse_get_tsensor_calibration_data(&reg1);
+	if (err)
+		goto errLabel;
+	data->fuse_F1 = reg1 & 0xFFFF;
+	data->fuse_F2 = (reg1 >> 16) & 0xFFFF;
+
+	err = tegra_fuse_get_tsensor_spare_bits(&spare_bits);
+	if (err) {
+		pr_err("tsensor spare bit fuse read error=%d\n", err);
+		goto errLabel;
+	}
+
+	/*
+	 * FUSE_TJ_ADT_LOWT = T1, FUSE_TJ_ADJ = T2
+	 */
+
+	/*
+	 * Low temp is:
+	 * FUSE_TJ_ADT_LOWT = bits [20:14] or’ed with bits [27:21]
+	 */
+	T1 = ((spare_bits >> 14) & 0x7F) |
+		((spare_bits >> 21) & 0x7F);
+	dev_vdbg(data->hwmon_dev, "Tsensor low temp (T1) fuse :\n");
+
+	/*
+	 * High temp is:
+	 * FUSE_TJ_ADJ = bits [6:0] or’ed with bits [13:7]
+	 */
+	dev_vdbg(data->hwmon_dev, "Tsensor low temp (T2) fuse :\n");
+	T2 = (spare_bits & 0x7F) | ((spare_bits >> 7) & 0x7F);
+	pr_info("Tsensor fuse calibration F1=%d(%#x), F2=%d(%#x), T1=%d, T2=%d\n",
+		data->fuse_F1, data->fuse_F1,
+		data->fuse_F2, data->fuse_F2, T1, T2);
+	data->fuse_T1 = T1;
+	data->fuse_T2 = T2;
+	return 0;
+errLabel:
+	return err;
+}
+
+/* function to calculate interim temperature */
+static int calc_interim_temp(struct tegra_tsensor_data *data,
+	unsigned int counter, s64 *p_interim_temp)
+{
+	s64 val1_64;
+	s64 val2;
+	u32 temp_rem;
+	bool is_neg;
+	u32 divisor;
+
+	/*
+	 * T-int = A * Counter + B
+	 * (Counter is the sensor frequency output)
+	 */
+	if ((data->fuse_F2 - data->fuse_F1) <= (data->fuse_T2 -
+		data->fuse_T1)) {
+		dev_err(data->hwmon_dev, "Error: F2=%d, F1=%d "
+			"difference unexpectedly low. "
+			"Aborting temperature processing\n", data->fuse_F2,
+			data->fuse_F1);
+		return -EINVAL;
+	} else {
+		/* expression modified after assuming s_A is 10^6 times,
+		 * s_B is 10^2 times and want end result to be 10^2 times
+		 * actual value
+		 */
+		val1_64 = (data->A_e_minus12 * counter);
+		dev_dbg(data->hwmon_dev, "A_e_-12*counter=%lld\n", val1_64);
+		val2 = (s64)data->B_e_minus6 * 1000000ULL;
+		dev_dbg(data->hwmon_dev, "B_e_-12=%lld\n", val2);
+		val2 += val1_64;
+		dev_dbg(data->hwmon_dev, "A_counter+B=%lld\n", val2);
+		is_neg = false;
+		if (val2 < 0) {
+			is_neg = true;
+			val2 *= -1;
+		}
+		divisor = 1000000;
+		temp_rem = do_div(val2, divisor);
+		if (temp_rem > (divisor >> 1))
+			val2++;
+		if (is_neg)
+			val2 *= -1;
+		*p_interim_temp = val2;
+		dev_dbg(data->hwmon_dev, "counter=%d, interim_temp=%lld\n",
+			counter, *p_interim_temp);
+	}
+	return 0;
+}
+
+/*
+ * function to calculate final temperature, given
+ * interim temperature
+ */
+static void calc_final_temp(struct tegra_tsensor_data *data,
+	s64 interim_temp, int *p_final_temp)
+{
+	s64 temp1_64, temp2_64, temp_64, temp1_64_rem;
+	u32 temp_rem_32;
+	u32 divisor;
+	u64 divisor_64;
+	bool is_neg;
+	/*
+	 * T-final = m * T-int ^2 + n * T-int + p
+	 * m = -0.002775
+	 * n = 1.338811
+	 * p = -7.3
+	 */
+
+	temp1_64 = (interim_temp * interim_temp);
+	/* always positive as squaring value */
+	/* losing accuracy here */
+	divisor = 10000;
+	/* temp1_64 contains quotient and returns remainder */
+	temp_rem_32 = do_div(temp1_64, divisor);
+	if (temp_rem_32 > (divisor >> 1))
+		temp1_64++;
+	temp1_64 *= (s64)data->m_e_minus6;
+	dev_dbg(data->hwmon_dev, "m_T-interim^2_e^14=%lld\n", temp1_64);
+	temp1_64_rem = (s64)data->m_e_minus6 * (s64)temp_rem_32;
+	is_neg = false;
+	if (temp1_64_rem < 0) {
+		is_neg = true;
+		temp1_64_rem *= -1;
+	}
+	temp_rem_32 = do_div(temp1_64_rem, divisor);
+	if (temp_rem_32 > (divisor >> 1))
+		temp1_64_rem++;
+	if (is_neg)
+		temp1_64_rem *= -1;
+	/* temp1_64 is m * t-int * t-int * 10^14 */
+
+	temp2_64 = (s64)data->n_e_minus6 * interim_temp * 100;
+	dev_dbg(data->hwmon_dev, "n_T-interim_e^14=%lld\n", temp2_64);
+	/* temp2_64 is n * t-int * 10^14 */
+
+	temp_64 = ((s64)data->p_e_minus2 * (s64)1000000000000ULL);
+	/* temp_64 is n * 10^14 */
+	temp_64 += temp1_64 + temp2_64 + temp1_64_rem;
+	is_neg = false;
+	if (temp_64 < 0) {
+		is_neg = true;
+		temp_64 *= -1;
+	}
+	divisor_64 = 100000000ULL;
+	temp_rem_32 = do_div(temp_64, divisor_64);
+	if (temp_rem_32 > (divisor_64 >> 1))
+		temp_64++;
+	if (is_neg)
+		temp_64 *= -1;
+	/* temperature * 10^14 / 10^8 */
+	/* get LS decimal digit rounding */
+	*p_final_temp = (s32)temp_64;
+	dev_dbg(data->hwmon_dev, "T-final stage4=%d\n", *p_final_temp);
+}
+
+/*
+ * Function to compute constants A and B needed for temperature
+ * calculation
+ * A = (T2-T1) / (F2-F1)
+ * B = T1 – A * F1
+ */
+static int tsensor_get_const_AB(struct tegra_tsensor_data *data)
+{
+	int err;
+	s64 temp_val1, temp_val2;
+	u32 temp_rem;
+	bool is_neg;
+	u32 divisor;
+
+	/*
+	 * 1. Find fusing registers for 25C (T1, F1) and 90C (T2, F2);
+	 */
+	err = read_tsensor_fuse_regs(data);
+	if (err) {
+		dev_err(data->hwmon_dev, "Fuse register read required "
+			"for internal tsensor returns err=%d\n", err);
+		return err;
+	}
+
+	if (data->fuse_F2 != data->fuse_F1) {
+		if ((data->fuse_F2 - data->fuse_F1) <= (data->fuse_T2 -
+			data->fuse_T1)) {
+			dev_err(data->hwmon_dev, "Error: F2=%d, "
+				"F1=%d, difference"
+				" unexpectedly low. Aborting temperature"
+				"computation\n", data->fuse_F2, data->fuse_F1);
+			return -EINVAL;
+		} else {
+			temp_val1 = (s64)(data->fuse_T2 - data->fuse_T1) *
+				1000000000000ULL;
+			/* temp_val1 always positive as fuse_T2 > fuse_T1 */
+			temp_rem = do_div(temp_val1, (data->fuse_F2 -
+				data->fuse_F1));
+			data->A_e_minus12 = temp_val1;
+			temp_val2 = (s64)(data->fuse_T1 * 1000000000000ULL);
+			temp_val2 -= (data->A_e_minus12 * data->fuse_F1);
+			is_neg = false;
+			if (temp_val2 < 0) {
+				is_neg = true;
+				temp_val2 *= -1;
+			}
+			divisor = 1000000;
+			temp_rem = do_div(temp_val2, divisor);
+			if (temp_rem > (divisor >> 1))
+				temp_val2++;
+			if (is_neg)
+				temp_val2 *= -1;
+			data->B_e_minus6 = (s32)temp_val2;
+			/* B is 10^6 times now */
+		}
+	}
+	dev_info(data->hwmon_dev, "A_e_minus12 = %lld\n", data->A_e_minus12);
+	dev_info(data->hwmon_dev, "B_e_minus6 = %d\n", data->B_e_minus6);
+	return 0;
+}
+
+/*
+ * function calculates expected temperature corresponding to
+ * given tsensor counter value
+ * Value returned is 100 times calculated temperature since the
+ * calculations are using fixed point arithmetic instead of floating point
+ */
+static int tsensor_count_2_temp(struct tegra_tsensor_data *data,
+	unsigned int count, int *p_temperature)
+{
+	s64 interim_temp;
+	int err;
+
+	/*
+	 *
+	 * 2. Calculate interim temperature:
+	 */
+	err = calc_interim_temp(data, count, &interim_temp);
+	if (err < 0) {
+		dev_err(data->hwmon_dev, "tsensor: cannot read temperature\n");
+		*p_temperature = -1;
+		return err;
+	}
+
+	/*
+	 *
+	 * 3. Calculate final temperature:
+	 */
+	calc_final_temp(data, interim_temp, p_temperature);
+	/* logs counter -> temperature conversion */
+	dev_dbg(data->hwmon_dev, "tsensor: counter=0x%x, interim "
+		"temp*10^6=%lld, Final temp=%d.%06d\n",
+		count, interim_temp,
+		get_temperature_int(*p_temperature),
+		get_temperature_fraction(*p_temperature));
+	return 0;
+}
+
+/*
+ * function to solve quadratic roots of equation
+ * used to get counter corresponding to given temperature
+ */
+static void get_quadratic_roots(struct tegra_tsensor_data *data,
+		int temp, unsigned int *p_counter1,
+		unsigned int *p_counter2)
+{
+	/*
+	 * Equation to solve:
+	 * m * A^2 * Counter^2 +
+	 * A * (2 * m * B + n) * Counter +
+	 * (m * B^2 + n * B + p - Temperature) = 0
+
+	To calculate root - assume
+		b = A * (2 * m * B + n)
+		a = m * A^2
+		c = ((m * B^2) + n * B + p - temp)
+	root1 = (-b + sqrt(b^2 - (4*a*c))) / (2 * a)
+	root2 = (-b - sqrt(b^2 - (4*a*c))) / (2 * a)
+	sqrt(k) = sqrt(k * 10^6) / sqrt(10^6)
+
+	Roots are :
+	(-(2*m*B+n)+sqrt(((2*m*B+n)^2-4*m(m*B^2+n*B+p-temp))))/(2*m*A)
+	and
+	(-(2*m*B+n)-sqrt(((2*m*B+n)^2-4*m(m*B^2+n*B+p-temp))))/(2*m*A)
+
+	After simplify ((2*m*B+n)^2-4*m(m*B^2+n*B+p-temp)),
+	Roots are:
+	(-(2*m*B+n)+sqrt((n^2-4*m(p-temp))))/(2*m*A)
+	and
+	(-(2*m*B+n)-sqrt((n^2-4*m(p-temp))))/(2*m*A)
+	*/
+
+	int v_e_minus6_2mB_n;
+	int v_e_minus6_4m_p_minusTemp;
+	int v_e_minus6_n2;
+	int v_e_minus6_b2_minus4ac;
+	int v_e_minus6_sqrt_b2_minus4ac;
+	s64 v_e_minus12_2mA;
+	int root1, root2;
+	int temp_rem;
+	bool is_neg;
+	s64 temp_64;
+
+	dev_dbg(data->hwmon_dev, "m_e-6=%d,n_e-6=%d,p_e-2=%d,A_e-6=%lld,"
+		"B_e-2=%d\n", data->m_e_minus6, data->n_e_minus6,
+		data->p_e_minus2, data->A_e_minus12, data->B_e_minus6);
+
+	temp_64 = (2ULL * (s64)data->m_e_minus6 * (s64)data->B_e_minus6);
+	is_neg = false;
+	if (temp_64 < 0) {
+		is_neg = true;
+		temp_64 *= -1;
+	}
+	temp_rem = do_div(temp_64, 1000000);
+	if (is_neg)
+		temp_64 *= -1;
+	v_e_minus6_2mB_n = (s32)temp_64 + data->n_e_minus6;
+	/* computed 2mB + n */
+
+	temp_64 = ((s64)data->m_e_minus6 * (s64)data->A_e_minus12);
+	temp_64 *= 2;
+	is_neg = false;
+	if (temp_64 < 0) {
+		temp_64 *= -1;
+		is_neg = true;
+	}
+	temp_rem = do_div(temp_64, 1000000);
+	if (is_neg)
+		temp_64 *= -1;
+	v_e_minus12_2mA = temp_64;
+	/* computed 2mA */
+
+	temp_64 = ((s64)data->n_e_minus6 * (s64)data->n_e_minus6);
+	/* squaring give positive value */
+	temp_rem = do_div(temp_64, 1000000);
+	v_e_minus6_n2 = (s32)temp_64;
+	/* computed n^2 */
+
+	v_e_minus6_4m_p_minusTemp = data->p_e_minus2 - (temp * 100);
+	v_e_minus6_4m_p_minusTemp *= 4 * data->m_e_minus6;
+	v_e_minus6_4m_p_minusTemp = DIV_ROUND_CLOSEST(
+		v_e_minus6_4m_p_minusTemp,100);
+	/* computed 4m*(p-T)*/
+
+	v_e_minus6_b2_minus4ac = (v_e_minus6_n2 - v_e_minus6_4m_p_minusTemp);
+
+	/* To preserve 1 decimal digits for sqrt(v_e_minus6_b2_minus4ac),
+	Make it 100 times, so
+	v_e_minus6_sqrt_b2_minus4ac=(int_sqrt(v_e_minus6_b2_minus4ac *100)*10^6)
+					/sqrt(10^6 * 100)
+	To avoid overflow,Simplify it to be:
+	v_e_minus6_sqrt_b2_minus4ac =(int_sqrt(v_e_minus6_b2_minus4ac *100)*100)
+	*/
+
+	v_e_minus6_sqrt_b2_minus4ac = (int_sqrt(v_e_minus6_b2_minus4ac * 100)
+		 * 100);
+	dev_dbg(data->hwmon_dev, "A_e_minus12=%lld, B_e_minus6=%d, "
+		"m_e_minus6=%d, n_e_minus6=%d, p_e_minus2=%d, "
+		"temp=%d\n", data->A_e_minus12, data->B_e_minus6,
+		data->m_e_minus6,
+		data->n_e_minus6, data->p_e_minus2, (int)temp);
+	dev_dbg(data->hwmon_dev, "2mB_n=%d, 2mA=%lld, 4m_p_minusTemp=%d,"
+		"b2_minus4ac=%d\n", v_e_minus6_2mB_n,
+		v_e_minus12_2mA, v_e_minus6_4m_p_minusTemp,
+		v_e_minus6_b2_minus4ac);
+
+	temp_64=(s64)(-v_e_minus6_2mB_n - v_e_minus6_sqrt_b2_minus4ac) * 1000000;
+	root1=(s32)div64_s64(temp_64, v_e_minus12_2mA);
+
+	temp_64=(s64)(-v_e_minus6_2mB_n + v_e_minus6_sqrt_b2_minus4ac) * 1000000;
+	root2=(s32)div64_s64(temp_64, v_e_minus12_2mA);
+
+	dev_dbg(data->hwmon_dev, "new expr: temp=%d, root1=%d, root2=%d\n",
+		temp, root1, root2);
+
+	*p_counter1 = root1;
+	*p_counter2 = root2;
+	/* we find that root2 is more appropriate root */
+
+	/* logs temperature -> counter conversion */
+	dev_dbg(data->hwmon_dev, "temperature=%d, counter1=%#x, "
+		"counter2=%#x\n", temp, *p_counter1, *p_counter2);
+}
+
+/*
+ * function returns tsensor expected counter corresponding to input
+ * temperature in degree Celsius.
+ * e.g. for temperature of 35C, temp=35
+ */
+static void tsensor_temp_2_count(struct tegra_tsensor_data *data,
+				int temp,
+				unsigned int *p_counter1,
+				unsigned int *p_counter2)
+{
+	dev_dbg(data->hwmon_dev, "Trying to calculate counter"
+		" for requested temperature"
+		" threshold=%d\n", temp);
+	/*
+	 * calculate the constants needed to get roots of
+	 * following quadratic eqn:
+	 * m * A^2 * Counter^2 +
+	 * A * (2 * m * B + n) * Counter +
+	 * (m * B^2 + n * B + p - Temperature) = 0
+	 */
+	get_quadratic_roots(data, temp, p_counter1, p_counter2);
+	/*
+	 * checked at current temperature=35 the counter=11418
+	 * for 50 deg temperature: counter1=22731, counter2=11817
+	 * at 35 deg temperature: counter1=23137, counter2=11411
+	 * hence, for above values we are assuming counter2 has
+	 * the correct value
+	 */
+}
+
+/*
+ * function to compare computed and expected values with
+ * certain tolerance setting hard coded here
+ */
+static bool cmp_counter(
+	struct tegra_tsensor_data *data,
+	unsigned int actual, unsigned int exp)
+{
+	unsigned int smaller;
+	unsigned int larger;
+	smaller = (actual > exp) ? exp : actual;
+	larger = (smaller == actual) ? exp : actual;
+	if ((larger - smaller) > TSENSOR_COUNTER_TOLERANCE) {
+		dev_dbg(data->hwmon_dev, "actual=%d, exp=%d, larger=%d, "
+		"smaller=%d, tolerance=%d\n", actual, exp, larger, smaller,
+		TSENSOR_COUNTER_TOLERANCE);
+		return false;
+	}
+	return true;
+}
+
+/* function to print chart of counter to temperature values -
+ * It uses F1, F2, T1, T2 and start data gives reading
+ * for temperature in between the range
+ */
+static void print_counter_2_temperature_table(
+	struct tegra_tsensor_data *data)
+{
+	int i;
+	unsigned int start_counter, end_counter;
+	unsigned int diff;
+	int temperature;
+	const unsigned int num_readings = 40;
+	unsigned int index = 0;
+	dev_dbg(data->hwmon_dev, "***Counter and Temperature chart **********\n");
+	start_counter = data->fuse_F1;
+	end_counter = data->fuse_F2;
+	diff = (end_counter - start_counter) / num_readings;
+
+	/* We want to take num_readings counter values in between
+	and try to report corresponding temperature */
+	for (i = start_counter; i <= (end_counter + diff);
+		i += diff) {
+		tsensor_count_2_temp(data, i, &temperature);
+		dev_dbg(data->hwmon_dev, "[%d]: Counter=%#x, temperature=%d.%06dC\n",
+			++index, i, get_temperature_int(temperature),
+			get_temperature_fraction(temperature));
+	}
+	dev_dbg(data->hwmon_dev, "\n\n");
+	tsensor_count_2_temp(data, end_counter, &temperature);
+	dev_dbg(data->hwmon_dev, "[%d]: Counter=%#x, temperature=%d.%06dC\n",
+		++index, end_counter, get_temperature_int(temperature),
+		get_temperature_fraction(temperature));
+}
+
+static bool temp_matched(int given_temp, int calc_temp)
+{
+	const int temp_diff_max = 4;
+	int diff;
+
+	diff = given_temp - calc_temp;
+	if (diff < 0)
+		diff *= -1;
+	if (diff > temp_diff_max)
+		return false;
+	else
+		return true;
+}
+
+/* function to print chart of temperature to counter values */
+static void print_temperature_2_counter_table(
+	struct tegra_tsensor_data *data)
+{
+	int i;
+	int min = -25;
+	int max = 120;
+	unsigned int counter1, counter2;
+	int temperature;
+
+	dev_dbg(data->hwmon_dev, "Temperature and counter1 and "
+		"counter2 chart **********\n");
+	for (i = min; i <= max; i++) {
+		tsensor_temp_2_count(data, i,
+			&counter1, &counter2);
+		dev_dbg(data->hwmon_dev, "temperature=%d, "
+			"counter1=0x%x, counter2=0x%x\n",
+			i, counter1, counter2);
+		/* verify the counter2 to temperature conversion */
+		tsensor_count_2_temp(data, counter2, &temperature);
+		dev_dbg(data->hwmon_dev, "Given temp=%d: counter2=%d, conv temp=%d.%06d\n",
+			i, counter2, get_temperature_int(temperature),
+			get_temperature_fraction(temperature));
+		if (!temp_matched(i, get_temperature_round(temperature)))
+			dev_dbg(data->hwmon_dev, "tsensor temp to counter to temp conversion failed for temp=%d\n",
+				i);
+	}
+	dev_dbg(data->hwmon_dev, "\n\n");
+}
+
+static void dump_a_tsensor_reg(struct tegra_tsensor_data *data,
+	unsigned int addr)
+{
+	dev_dbg(data->hwmon_dev, "tsensor[%d][0x%x]: 0x%x\n", (addr >> 16),
+		addr & 0xFFFF, tsensor_readl(data, addr));
+}
+
+static void dump_tsensor_regs(struct tegra_tsensor_data *data)
+{
+	int i;
+	for (i = 0; i < TSENSOR_COUNT; i++) {
+		/* if STOP bit is set skip this check */
+		dump_a_tsensor_reg(data, ((i << 16) | SENSOR_CFG0));
+		dump_a_tsensor_reg(data, ((i << 16) | SENSOR_CFG1));
+		dump_a_tsensor_reg(data, ((i << 16) | SENSOR_CFG2));
+		dump_a_tsensor_reg(data, ((i << 16) | SENSOR_STATUS0));
+		dump_a_tsensor_reg(data, ((i << 16) | SENSOR_TS_STATUS1));
+		dump_a_tsensor_reg(data, ((i << 16) | SENSOR_TS_STATUS2));
+		dump_a_tsensor_reg(data, ((i << 16) | 0x0));
+		dump_a_tsensor_reg(data, ((i << 16) | 0x44));
+		dump_a_tsensor_reg(data, ((i << 16) | 0x50));
+		dump_a_tsensor_reg(data, ((i << 16) | 0x54));
+		dump_a_tsensor_reg(data, ((i << 16) | 0x64));
+		dump_a_tsensor_reg(data, ((i << 16) | 0x68));
+	}
+}
+
+/*
+ * function to test if conversion of counter to temperature
+ * and vice-versa is working
+ */
+static int test_temperature_algo(struct tegra_tsensor_data *data)
+{
+	unsigned int actual_counter;
+	unsigned int curr_avg;
+	unsigned int counter1, counter2;
+	int T1;
+	int err = 0;
+	bool result1, result2;
+	bool result = false;
+
+	/* read actual counter */
+	err = tsensor_read_counter(data, &curr_avg);
+	if (err < 0) {
+		pr_err("Error: tsensor0 counter read, err=%d\n", err);
+		goto endLabel;
+	}
+	actual_counter = ((curr_avg & 0xFFFF0000) >> 16);
+	dev_dbg(data->hwmon_dev, "counter read=0x%x\n", actual_counter);
+
+	/* calculate temperature */
+	err = tsensor_count_2_temp(data, actual_counter, &T1);
+	dev_dbg(data->hwmon_dev, "%s actual counter=0x%x, calculated "
+		"temperature=%d.%06d\n", __func__,
+		actual_counter, get_temperature_int(T1),
+		get_temperature_fraction(T1));
+	if (err < 0) {
+		pr_err("Error: calculate temperature step\n");
+		goto endLabel;
+	}
+
+	/* calculate counter corresponding to read temperature */
+	tsensor_temp_2_count(data, get_temperature_round(T1),
+		&counter1, &counter2);
+	dev_dbg(data->hwmon_dev, "given temperature=%d, counter1=0x%x,"
+		" counter2=0x%x\n",
+		get_temperature_round(T1), counter1, counter2);
+
+	err = tsensor_count_2_temp(data, actual_counter, &T1);
+	dev_dbg(data->hwmon_dev, "%s 2nd time actual counter=0x%x, "
+		"calculated temperature=%d.%d\n", __func__,
+		actual_counter, get_temperature_int(T1),
+		get_temperature_fraction(T1));
+	if (err < 0) {
+		pr_err("Error: calculate temperature step\n");
+		goto endLabel;
+	}
+
+	/* compare counter calculated with actual original counter */
+	result1 = cmp_counter(data, actual_counter, counter1);
+	result2 = cmp_counter(data, actual_counter, counter2);
+	if (result1) {
+		dev_dbg(data->hwmon_dev, "counter1 matches: actual=%d,"
+			" calc=%d\n", actual_counter, counter1);
+		result = true;
+	}
+	if (result2) {
+		dev_dbg(data->hwmon_dev, "counter2 matches: actual=%d,"
+			" calc=%d\n", actual_counter, counter2);
+		result = true;
+	}
+	if (!result) {
+		pr_info("NO Match: actual=%d,"
+			" calc counter2=%d, counter1=%d\n", actual_counter,
+			counter2, counter1);
+		err = -EIO;
+	}
+
+endLabel:
+	return err;
+}
+
+/* tsensor threshold temperature to threshold counter conversion function */
+static unsigned int tsensor_get_threshold_counter(
+	struct tegra_tsensor_data *data,
+	int temp_threshold)
+{
+	unsigned int counter1, counter2;
+	unsigned int counter;
+
+	if (temp_threshold < 0)
+		return MAX_THRESHOLD;
+
+	tsensor_temp_2_count(data, temp_threshold, &counter1, &counter2);
+
+	counter = counter2;
+
+	return counter;
+}
+
+/* tsensor temperature threshold setup function */
+static void tsensor_threshold_setup(struct tegra_tsensor_data *data,
+					unsigned char index)
+{
+	unsigned long config0;
+	unsigned char i = index;
+	unsigned int th2_count = DEFAULT_THRESHOLD_TH2;
+	unsigned int th3_count = DEFAULT_THRESHOLD_TH3;
+	unsigned int th1_count = DEFAULT_THRESHOLD_TH1;
+	int th0_diff = 0;
+
+	dev_dbg(data->hwmon_dev, "started tsensor_threshold_setup %d\n",
+		index);
+	config0 = tsensor_readl(data, ((i << 16) | SENSOR_CFG0));
+
+	dev_dbg(data->hwmon_dev, "before threshold program TH dump:\n");
+	dump_threshold(data);
+	dev_dbg(data->hwmon_dev, "th3=0x%x, th2=0x%x, th1=0x%x, th0=0x%x\n",
+		th3_count, th2_count, th1_count, th0_diff);
+	config0 = (((th2_count & SENSOR_CFG_X_TH_X_MASK)
+		<< SENSOR_CFG1_TH2_SHIFT) |
+		((th1_count & SENSOR_CFG_X_TH_X_MASK) <<
+		SENSOR_CFG1_TH1_SHIFT));
+	tsensor_writel(data, config0, ((i << 16) | SENSOR_CFG1));
+	config0 = (((th0_diff & SENSOR_CFG_X_TH_X_MASK)
+		<< SENSOR_CFG2_TH0_SHIFT) |
+		((th3_count & SENSOR_CFG_X_TH_X_MASK) <<
+		SENSOR_CFG2_TH3_SHIFT));
+	tsensor_writel(data, config0, ((i << 16) | SENSOR_CFG2));
+	dev_dbg(data->hwmon_dev, "after threshold program TH dump:\n");
+	dump_threshold(data);
+}
+
+/* tsensor config programming function */
+static int tsensor_config_setup(struct tegra_tsensor_data *data)
+{
+	unsigned int config0;
+	unsigned int i;
+	int err = 0;
+
+	for (i = 0; i < TSENSOR_COUNT; i++) {
+		/*
+		 * Pre-read setup:
+		 * Set M and N values
+		 * Enable HW features HW_FREQ_DIV_EN, THERMAL_RST_EN
+		 */
+		config0 = tsensor_readl(data, ((i << 16) | SENSOR_CFG0));
+		config0 &= ~((SENSOR_CFG0_M_MASK << SENSOR_CFG0_M_SHIFT) |
+			(SENSOR_CFG0_N_MASK << SENSOR_CFG0_N_SHIFT) |
+			(1 << SENSOR_CFG0_OVERFLOW_INTR) |
+			(1 << SENSOR_CFG0_RST_INTR_SHIFT) |
+			(1 << SENSOR_CFG0_DVFS_INTR_SHIFT) |
+			(1 << SENSOR_CFG0_HW_DIV2_INTR_SHIFT) |
+			(1 << SENSOR_CFG0_RST_ENABLE_SHIFT) |
+			(1 << SENSOR_CFG0_HW_DIV2_ENABLE_SHIFT)
+			);
+		/* Set STOP bit */
+		/* Set M and N values */
+		/* Enable HW features HW_FREQ_DIV_EN, THERMAL_RST_EN */
+		config0 |= (
+			((DEFAULT_TSENSOR_M & SENSOR_CFG0_M_MASK) <<
+				SENSOR_CFG0_M_SHIFT) |
+			((DEFAULT_TSENSOR_N & SENSOR_CFG0_N_MASK) <<
+				SENSOR_CFG0_N_SHIFT) |
+			(1 << SENSOR_CFG0_OVERFLOW_INTR) |
+			(1 << SENSOR_CFG0_DVFS_INTR_SHIFT) |
+			(1 << SENSOR_CFG0_HW_DIV2_INTR_SHIFT) |
+#if ENABLE_TSENSOR_HW_RESET
+			(1 << SENSOR_CFG0_RST_ENABLE_SHIFT) |
+#endif
+			(1 << SENSOR_CFG0_STOP_SHIFT));
+
+		tsensor_writel(data, config0, ((i << 16) | SENSOR_CFG0));
+		tsensor_threshold_setup(data, i);
+	}
+
+	/* Disable sensor stop bit */
+	config0 = tsensor_readl(data, (data->instance << 16) | SENSOR_CFG0);
+	config0 &= ~(1 << SENSOR_CFG0_STOP_SHIFT);
+	tsensor_writel(data, config0, (data->instance << 16) | SENSOR_CFG0);
+
+	/* initialize tsensor chip coefficients */
+	get_chip_tsensor_coeff(data);
+
+	return err;
+}
+
+/* function to enable tsensor clock */
+static int tsensor_clk_enable(
+	struct tegra_tsensor_data *data,
+	bool enable)
+{
+	int err = 0;
+	unsigned long rate;
+	struct clk *clk_m;
+
+	if (enable) {
+		clk_enable(data->dev_clk);
+		rate = clk_get_rate(data->dev_clk);
+		clk_m = clk_get_sys(NULL, "clk_m");
+		if (clk_get_parent(data->dev_clk) != clk_m) {
+			err = clk_set_parent(data->dev_clk, clk_m);
+			if (err < 0)
+				goto fail;
+		}
+		rate = DEFAULT_TSENSOR_CLK_HZ;
+		if (rate != clk_get_rate(clk_m)) {
+			err = clk_set_rate(data->dev_clk, rate);
+			if (err < 0)
+				goto fail;
+		}
+	} else {
+		clk_disable(data->dev_clk);
+		clk_put(data->dev_clk);
+	}
+fail:
+	return err;
+}
+
+/*
+ * function to set counter threshold corresponding to
+ * given temperature
+ */
+static void tsensor_set_limits(
+	struct tegra_tsensor_data *data,
+	int temp,
+	int threshold_index)
+{
+	unsigned int th_count;
+	unsigned int config;
+	unsigned short sft, offset;
+	unsigned int th1_count;
+
+	th_count = tsensor_get_threshold_counter(data, temp);
+	dev_dbg(data->hwmon_dev, "%s : input temp=%d, counter=0x%x\n", __func__,
+		temp, th_count);
+	switch (threshold_index) {
+	case TSENSOR_TH0:
+		sft = 16;
+		offset = SENSOR_CFG2;
+		/* assumed TH1 set before TH0, else we program
+		 * TH0 as TH1 which means hysteresis will be
+		 * same as TH1. Also, caller expected to pass
+		 * (TH1 - hysteresis) as temp argument for this case */
+		th1_count = tsensor_readl(data,
+			((data->instance << 16) |
+			SENSOR_CFG1));
+		th_count = (th1_count > th_count) ?
+			(th1_count - th_count) :
+			th1_count;
+		break;
+	case TSENSOR_TH1:
+	default:
+		sft = 0;
+		offset = SENSOR_CFG1;
+		break;
+	case TSENSOR_TH2:
+		sft = 16;
+		offset = SENSOR_CFG1;
+		break;
+	case TSENSOR_TH3:
+		sft = 0;
+		offset = SENSOR_CFG2;
+		break;
+	}
+	config = tsensor_readl(data, ((data->instance << 16) | offset));
+	dev_dbg(data->hwmon_dev, "%s: old config=0x%x, sft=%d, offset=0x%x\n",
+		__func__, config, sft, offset);
+	config &= ~(SENSOR_CFG_X_TH_X_MASK << sft);
+	config |= ((th_count & SENSOR_CFG_X_TH_X_MASK) << sft);
+	dev_dbg(data->hwmon_dev, "new config=0x%x\n", config);
+	tsensor_writel(data, config, ((data->instance << 16) | offset));
+}
+
+int tsensor_thermal_set_limits(struct tegra_tsensor_data *data,
+					long lo_limit_milli,
+					long hi_limit_milli)
+{
+	long lo_limit = MILLICELSIUS_TO_CELSIUS(lo_limit_milli);
+	long hi_limit = MILLICELSIUS_TO_CELSIUS(hi_limit_milli);
+	int i, j, hi_limit_first;
+
+	if (lo_limit_milli == hi_limit_milli)
+		return -EINVAL;
+
+	mutex_lock(&data->mutex);
+
+	if (data->current_lo_limit == lo_limit_milli &&
+		data->current_hi_limit == hi_limit_milli) {
+		goto done;
+	}
+
+	/* If going up, change hi limit first.  If going down, change lo
+	   limit first */
+	hi_limit_first = hi_limit_milli > data->current_hi_limit;
+
+	for (i = 0; i < 2; i++) {
+		j = (i + hi_limit_first) % 2;
+
+		switch (j) {
+		case 0:
+			tsensor_set_limits(data, hi_limit, TSENSOR_TH2);
+			data->current_hi_limit = hi_limit_milli;
+			break;
+		case 1:
+			tsensor_set_limits(data, lo_limit, TSENSOR_TH1);
+			data->current_lo_limit = lo_limit_milli;
+			break;
+		}
+	}
+
+
+done:
+	mutex_unlock(&data->mutex);
+	return 0;
+}
+
+int tsensor_thermal_set_alert(struct tegra_tsensor_data *data,
+				void (*alert_func)(void *),
+				void *alert_data)
+{
+	mutex_lock(&data->mutex);
+
+	data->alert_data = alert_data;
+	data->alert_func = alert_func;
+
+	mutex_unlock(&data->mutex);
+
+	return 0;
+}
+
+int tsensor_thermal_set_shutdown_temp(struct tegra_tsensor_data *data,
+					long shutdown_temp_milli)
+{
+	long shutdown_temp = MILLICELSIUS_TO_CELSIUS(shutdown_temp_milli);
+	tsensor_set_limits(data, shutdown_temp, TSENSOR_TH3);
+
+	return 0;
+}
+
+static int tsensor_within_limits(struct tegra_tsensor_data *data)
+{
+	int ts_state = get_ts_state(data);
+
+	return (ts_state == TS_LEVEL1);
+}
+
+static void tsensor_work_func(struct work_struct *work)
+{
+	struct tegra_tsensor_data *data = container_of(to_delayed_work(work),
+		struct tegra_tsensor_data, work);
+
+	if (!data->alert_func)
+		return;
+
+	if (!tsensor_within_limits(data)) {
+		data->alert_func(data->alert_data);
+
+		if (!tsensor_within_limits(data))
+			dev_dbg(data->hwmon_dev,
+				"repeated work queueing state=%d\n",
+				get_ts_state(data));
+			queue_delayed_work(data->workqueue, &data->work,
+				HZ * DEFAULT_TSENSOR_M /
+				DEFAULT_TSENSOR_CLK_HZ);
+	}
+}
+
+/*
+ * This function enables the tsensor using default configuration
+ * 1. We would need some configuration APIs to calibrate
+ * the tsensor counters to right temperature
+ * 2. hardware triggered divide cpu clock by 2 as well pmu reset is enabled
+ * implementation. No software actions are enabled at this point
+ */
+static int tegra_tsensor_setup(struct platform_device *pdev)
+{
+	struct tegra_tsensor_data *data = platform_get_drvdata(pdev);
+	struct resource *r;
+	int err = 0;
+	struct tegra_tsensor_platform_data *tsensor_data;
+	unsigned int reg;
+
+	data->dev_clk = clk_get(&pdev->dev, NULL);
+	if ((!data->dev_clk) || ((int)data->dev_clk == -(ENOENT))) {
+		dev_err(&pdev->dev, "Couldn't get the clock\n");
+		err = PTR_ERR(data->dev_clk);
+		goto fail;
+	}
+
+	/* Enable tsensor clock */
+	err = tsensor_clk_enable(data, true);
+	if (err < 0)
+		goto err_irq;
+
+	/* Reset tsensor */
+	dev_dbg(&pdev->dev, "before tsensor reset %s\n", __func__);
+	tegra_periph_reset_assert(data->dev_clk);
+	udelay(100);
+	tegra_periph_reset_deassert(data->dev_clk);
+	udelay(100);
+
+	dev_dbg(&pdev->dev, "before tsensor chk pmc reset %s\n",
+		__func__);
+	/* Check for previous resets in pmc */
+	if (pmc_check_rst_sensor(data)) {
+		dev_err(data->hwmon_dev, "Warning: ***** Last PMC "
+			"Reset source: tsensor detected\n");
+	}
+
+	dev_dbg(&pdev->dev, "before tsensor pmc reset enable %s\n",
+		__func__);
+	/* Enable the sensor reset in PMC */
+	pmc_rst_enable(data, true);
+
+	dev_dbg(&pdev->dev, "before tsensor get platform data %s\n",
+		__func__);
+	dev_dbg(&pdev->dev, "tsensor platform_data=0x%x\n",
+		(unsigned int)pdev->dev.platform_data);
+	tsensor_data = pdev->dev.platform_data;
+
+	/* register interrupt */
+	r = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!r) {
+		dev_err(&pdev->dev, "Failed to get IRQ\n");
+		err = -ENXIO;
+		goto err_irq;
+	}
+	data->irq = r->start;
+	err = request_irq(data->irq, tegra_tsensor_isr,
+			IRQF_DISABLED, pdev->name, data);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Failed to register IRQ\n");
+		goto err_irq;
+	}
+
+	dev_dbg(&pdev->dev, "tsensor platform_data=0x%x\n",
+		(unsigned int)pdev->dev.platform_data);
+
+	dev_dbg(&pdev->dev, "before tsensor_config_setup\n");
+	err = tsensor_config_setup(data);
+	if (err) {
+		dev_err(&pdev->dev, "[%s,line=%d]: tsensor counters dead!\n",
+			__func__, __LINE__);
+		goto err_setup;
+	}
+	dev_dbg(&pdev->dev, "before tsensor_get_const_AB\n");
+	/* calculate constants needed for temperature conversion */
+	err = tsensor_get_const_AB(data);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Failed to extract temperature\n"
+			"const\n");
+		goto err_setup;
+	}
+
+	/* test if counter-to-temperature and temperature-to-counter
+	 * are matching */
+	err = test_temperature_algo(data);
+	if (err) {
+		dev_err(&pdev->dev, "Error: read temperature\n"
+			"algorithm broken\n");
+		goto err_setup;
+	}
+
+	print_temperature_2_counter_table(data);
+
+	print_counter_2_temperature_table(data);
+
+	/* EDP and throttling support using tsensor enabled
+	 * based on fuse revision */
+	err = tegra_fuse_get_revision(&reg);
+	if (err)
+		goto err_setup;
+
+	data->is_edp_supported = (reg >= STABLE_TSENSOR_FUSE_REV);
+
+	if (data->is_edp_supported) {
+		data->workqueue = create_singlethread_workqueue("tsensor");
+		INIT_DELAYED_WORK(&data->work, tsensor_work_func);
+	}
+
+	return 0;
+err_setup:
+	free_irq(data->irq, data);
+err_irq:
+	tsensor_clk_enable(data, false);
+fail:
+	dev_err(&pdev->dev, "%s error=%d returned\n", __func__, err);
+	return err;
+}
+
+static int __devinit tegra_tsensor_probe(struct platform_device *pdev)
+{
+	struct tegra_tsensor_data *data;
+	struct resource *r;
+	int err;
+	unsigned int reg;
+	u8 i;
+	struct tegra_tsensor_platform_data *tsensor_data;
+
+	data = kzalloc(sizeof(struct tegra_tsensor_data), GFP_KERNEL);
+	if (!data) {
+		dev_err(&pdev->dev, "[%s,line=%d]: Failed to allocate "
+			"memory\n", __func__, __LINE__);
+		err = -ENOMEM;
+		goto exit;
+	}
+	mutex_init(&data->mutex);
+	platform_set_drvdata(pdev, data);
+
+	/* Register sysfs hooks */
+	for (i = 0; i < ARRAY_SIZE(tsensor_nodes); i++) {
+		err = device_create_file(&pdev->dev,
+			&tsensor_nodes[i].dev_attr);
+		if (err) {
+			dev_err(&pdev->dev, "device_create_file failed.\n");
+			goto err0;
+		}
+	}
+
+	data->hwmon_dev = hwmon_device_register(&pdev->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		err = PTR_ERR(data->hwmon_dev);
+		goto err1;
+	}
+
+	dev_set_drvdata(data->hwmon_dev, data);
+
+	spin_lock_init(&data->tsensor_lock);
+
+	/* map tsensor register space */
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (r == NULL) {
+		dev_err(&pdev->dev, "[%s,line=%d]: Failed to get io "
+			"resource\n", __func__, __LINE__);
+		err = -ENODEV;
+		goto err2;
+	}
+
+	if (!request_mem_region(r->start, (r->end - r->start) + 1,
+				dev_name(&pdev->dev))) {
+		dev_err(&pdev->dev, "[%s,line=%d]: Error mem busy\n",
+			__func__, __LINE__);
+		err = -EBUSY;
+		goto err2;
+	}
+
+	data->phys = r->start;
+	data->phys_end = r->end;
+	data->base = ioremap(r->start, r->end - r->start + 1);
+	if (!data->base) {
+		dev_err(&pdev->dev, "[%s, line=%d]: can't ioremap "
+			"tsensor iomem\n", __FILE__, __LINE__);
+		err = -ENOMEM;
+		goto err3;
+	}
+
+	/* map pmc rst_status register */
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (r == NULL) {
+		dev_err(&pdev->dev, "[%s,line=%d]: Failed to get io "
+			"resource\n", __func__, __LINE__);
+		err = -ENODEV;
+		goto err4;
+	}
+
+	if (!request_mem_region(r->start, (r->end - r->start) + 1,
+				dev_name(&pdev->dev))) {
+		dev_err(&pdev->dev, "[%s, line=%d]: Error mem busy\n",
+			__func__, __LINE__);
+		err = -EBUSY;
+		goto err4;
+	}
+
+	data->pmc_phys = r->start;
+	data->pmc_phys_end = r->end;
+	data->pmc_rst_base = ioremap(r->start, r->end - r->start + 1);
+	if (!data->pmc_rst_base) {
+		dev_err(&pdev->dev, "[%s, line=%d]: can't ioremap "
+			"pmc iomem\n", __FILE__, __LINE__);
+		err = -ENOMEM;
+		goto err5;
+	}
+
+	/* fuse revisions less than TSENSOR_FUSE_REV1
+	   bypass tsensor driver init */
+	/* tsensor active instance decided based on fuse revision */
+	err = tegra_fuse_get_revision(&reg);
+	if (err)
+		goto err6;
+	/* check for higher revision done first */
+	/* instance 0 is used for fuse revision TSENSOR_FUSE_REV2 onwards */
+	if (reg >= TSENSOR_FUSE_REV2)
+		data->instance = TSENSOR_INSTANCE1;
+	/* instance 1 is used for fuse revision TSENSOR_FUSE_REV1 till
+	   TSENSOR_FUSE_REV2 */
+	else if (reg >= TSENSOR_FUSE_REV1)
+		data->instance = TSENSOR_INSTANCE2;
+	pr_info("tsensor active instance=%d\n", data->instance);
+
+	/* tegra tsensor - setup and init */
+	err = tegra_tsensor_setup(pdev);
+	if (err)
+		goto err6;
+
+	dump_tsensor_regs(data);
+	dev_dbg(&pdev->dev, "end tegra_tsensor_probe\n");
+
+	tsensor_data = pdev->dev.platform_data;
+	if (tsensor_data->probe_callback)
+		tsensor_data->probe_callback(data);
+
+	return 0;
+err6:
+	iounmap(data->pmc_rst_base);
+err5:
+	release_mem_region(data->pmc_phys, (data->pmc_phys_end -
+		data->pmc_phys) + 1);
+err4:
+	iounmap(data->base);
+err3:
+	release_mem_region(data->phys, (data->phys_end -
+		data->phys) + 1);
+err2:
+	hwmon_device_unregister(data->hwmon_dev);
+err1:
+	for (i = 0; i < ARRAY_SIZE(tsensor_nodes); i++)
+		device_remove_file(&pdev->dev, &tsensor_nodes[i].dev_attr);
+err0:
+	kfree(data);
+exit:
+	dev_err(&pdev->dev, "%s error=%d returned\n", __func__, err);
+	return err;
+}
+
+static int __devexit tegra_tsensor_remove(struct platform_device *pdev)
+{
+	struct tegra_tsensor_data *data = platform_get_drvdata(pdev);
+	u8 i;
+
+	hwmon_device_unregister(data->hwmon_dev);
+	for (i = 0; i < ARRAY_SIZE(tsensor_nodes); i++)
+		device_remove_file(&pdev->dev, &tsensor_nodes[i].dev_attr);
+
+	if (data->is_edp_supported) {
+		cancel_delayed_work_sync(&data->work);
+		destroy_workqueue(data->workqueue);
+		data->workqueue = NULL;
+	}
+
+	free_irq(data->irq, data);
+
+	iounmap(data->pmc_rst_base);
+	release_mem_region(data->pmc_phys, (data->pmc_phys_end -
+		data->pmc_phys) + 1);
+	iounmap(data->base);
+	release_mem_region(data->phys, (data->phys_end -
+		data->phys) + 1);
+
+	kfree(data);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+
+static void save_tsensor_regs(struct tegra_tsensor_data *data)
+{
+	int i;
+	for (i = 0; i < TSENSOR_COUNT; i++) {
+		data->config0[i] = tsensor_readl(data,
+			((i << 16) | SENSOR_CFG0));
+		data->config1[i] = tsensor_readl(data,
+			((i << 16) | SENSOR_CFG1));
+		data->config2[i] = tsensor_readl(data,
+			((i << 16) | SENSOR_CFG2));
+	}
+}
+
+static void restore_tsensor_regs(struct tegra_tsensor_data *data)
+{
+	int i;
+	for (i = 0; i < TSENSOR_COUNT; i++) {
+		tsensor_writel(data, data->config0[i],
+			((i << 16) | SENSOR_CFG0));
+		tsensor_writel(data, data->config1[i],
+			((i << 16) | SENSOR_CFG1));
+		tsensor_writel(data, data->config2[i],
+			((i << 16) | SENSOR_CFG2));
+	}
+}
+
+
+static int tsensor_suspend(struct platform_device *pdev,
+	pm_message_t state)
+{
+	struct tegra_tsensor_data *data = platform_get_drvdata(pdev);
+	unsigned int config0;
+
+	disable_irq(data->irq);
+	cancel_delayed_work_sync(&data->work);
+	/* set STOP bit, else OVERFLOW interrupt seen in LP1 */
+	config0 = tsensor_readl(data, ((data->instance << 16) | SENSOR_CFG0));
+	config0 |= (1 << SENSOR_CFG0_STOP_SHIFT);
+	tsensor_writel(data, config0, ((data->instance << 16) | SENSOR_CFG0));
+
+	/* save current settings before suspend, when STOP bit is set */
+	save_tsensor_regs(data);
+	tsensor_clk_enable(data, false);
+
+	return 0;
+}
+
+static int tsensor_resume(struct platform_device *pdev)
+{
+	struct tegra_tsensor_data *data = platform_get_drvdata(pdev);
+	unsigned int config0;
+
+	tsensor_clk_enable(data, true);
+	/* restore current settings before suspend, no need
+	 * to clear STOP bit */
+	restore_tsensor_regs(data);
+
+	/* clear STOP bit, after restoring regs */
+	config0 = tsensor_readl(data, ((data->instance << 16) | SENSOR_CFG0));
+	config0 &= ~(1 << SENSOR_CFG0_STOP_SHIFT);
+	tsensor_writel(data, config0, ((data->instance << 16) | SENSOR_CFG0));
+
+	if (data->is_edp_supported)
+		schedule_delayed_work(&data->work, 0);
+
+	enable_irq(data->irq);
+	return 0;
+}
+#endif
+
+static struct platform_driver tegra_tsensor_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "tegra-tsensor",
+	},
+	.probe		= tegra_tsensor_probe,
+	.remove		= __devexit_p(tegra_tsensor_remove),
+#ifdef CONFIG_PM
+	.suspend	= tsensor_suspend,
+	.resume		= tsensor_resume,
+#endif
+};
+
+static int __init tegra_tsensor_init(void)
+{
+	return platform_driver_register(&tegra_tsensor_driver);
+}
+module_init(tegra_tsensor_init);
+
+static void __exit tegra_tsensor_exit(void)
+{
+	platform_driver_unregister(&tegra_tsensor_driver);
+}
+module_exit(tegra_tsensor_exit);
+
+MODULE_AUTHOR("nvidia");
+MODULE_DESCRIPTION("Nvidia Tegra Temperature Sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/w83627ehf.c b/drivers/hwmon/w83627ehf.c
index 36d7f270b14d..628451572455 100644
--- a/drivers/hwmon/w83627ehf.c
+++ b/drivers/hwmon/w83627ehf.c
@@ -1295,6 +1295,7 @@ store_pwm_mode(struct device *dev, struct device_attribute *attr,
 {
 	struct w83627ehf_data *data = dev_get_drvdata(dev);
 	struct sensor_device_attribute *sensor_attr = to_sensor_dev_attr(attr);
+	struct w83627ehf_sio_data *sio_data = dev->platform_data;
 	int nr = sensor_attr->index;
 	unsigned long val;
 	int err;
@@ -1306,6 +1307,11 @@ store_pwm_mode(struct device *dev, struct device_attribute *attr,
 
 	if (val > 1)
 		return -EINVAL;
+
+	/* On NCT67766F, DC mode is only supported for pwm1 */
+	if (sio_data->kind == nct6776 && nr && val != 1)
+		return -EINVAL;
+
 	mutex_lock(&data->update_lock);
 	reg = w83627ehf_read_value(data, W83627EHF_REG_PWM_ENABLE[nr]);
 	data->pwm_mode[nr] = val;
@@ -1756,7 +1762,17 @@ static inline void __devinit w83627ehf_init_device(struct w83627ehf_data *data,
 		diode = 0x70;
 	}
 	for (i = 0; i < 3; i++) {
-		if ((tmp & (0x02 << i)))
+		const char *label = NULL;
+
+		if (data->temp_label)
+			label = data->temp_label[data->temp_src[i]];
+
+		/* Digital source overrides analog type */
+		if (label && strncmp(label, "PECI", 4) == 0)
+			data->temp_type[i] = 6;
+		else if (label && strncmp(label, "AMD", 3) == 0)
+			data->temp_type[i] = 5;
+		else if ((tmp & (0x02 << i)))
 			data->temp_type[i] = (diode & (0x10 << i)) ? 1 : 3;
 		else
 			data->temp_type[i] = 4; /* thermistor */
diff --git a/drivers/hwspinlock/hwspinlock_core.c b/drivers/hwspinlock/hwspinlock_core.c
index 43a62714b4fb..12f7c8300c75 100644
--- a/drivers/hwspinlock/hwspinlock_core.c
+++ b/drivers/hwspinlock/hwspinlock_core.c
@@ -26,6 +26,7 @@
 #include <linux/radix-tree.h>
 #include <linux/hwspinlock.h>
 #include <linux/pm_runtime.h>
+#include <linux/mutex.h>
 
 #include "hwspinlock_internal.h"
 
@@ -52,10 +53,12 @@
 static RADIX_TREE(hwspinlock_tree, GFP_KERNEL);
 
 /*
- * Synchronization of access to the tree is achieved using this spinlock,
+ * Synchronization of access to the tree is achieved using this mutex,
  * as the radix-tree API requires that users provide all synchronisation.
+ * A mutex is needed because we're using non-atomic radix tree allocations.
  */
-static DEFINE_SPINLOCK(hwspinlock_tree_lock);
+static DEFINE_MUTEX(hwspinlock_tree_lock);
+
 
 /**
  * __hwspin_trylock() - attempt to lock a specific hwspinlock
@@ -261,8 +264,7 @@ EXPORT_SYMBOL_GPL(__hwspin_unlock);
  * This function should be called from the underlying platform-specific
  * implementation, to register a new hwspinlock instance.
  *
- * Can be called from an atomic context (will not sleep) but not from
- * within interrupt context.
+ * Should be called from a process context (might sleep)
  *
  * Returns 0 on success, or an appropriate error code on failure
  */
@@ -279,7 +281,7 @@ int hwspin_lock_register(struct hwspinlock *hwlock)
 
 	spin_lock_init(&hwlock->lock);
 
-	spin_lock(&hwspinlock_tree_lock);
+	mutex_lock(&hwspinlock_tree_lock);
 
 	ret = radix_tree_insert(&hwspinlock_tree, hwlock->id, hwlock);
 	if (ret)
@@ -293,7 +295,7 @@ int hwspin_lock_register(struct hwspinlock *hwlock)
 	WARN_ON(tmp != hwlock);
 
 out:
-	spin_unlock(&hwspinlock_tree_lock);
+	mutex_unlock(&hwspinlock_tree_lock);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(hwspin_lock_register);
@@ -305,8 +307,7 @@ EXPORT_SYMBOL_GPL(hwspin_lock_register);
  * This function should be called from the underlying platform-specific
  * implementation, to unregister an existing (and unused) hwspinlock.
  *
- * Can be called from an atomic context (will not sleep) but not from
- * within interrupt context.
+ * Should be called from a process context (might sleep)
  *
  * Returns the address of hwspinlock @id on success, or NULL on failure
  */
@@ -315,7 +316,7 @@ struct hwspinlock *hwspin_lock_unregister(unsigned int id)
 	struct hwspinlock *hwlock = NULL;
 	int ret;
 
-	spin_lock(&hwspinlock_tree_lock);
+	mutex_lock(&hwspinlock_tree_lock);
 
 	/* make sure the hwspinlock is not in use (tag is set) */
 	ret = radix_tree_tag_get(&hwspinlock_tree, id, HWSPINLOCK_UNUSED);
@@ -331,7 +332,7 @@ struct hwspinlock *hwspin_lock_unregister(unsigned int id)
 	}
 
 out:
-	spin_unlock(&hwspinlock_tree_lock);
+	mutex_unlock(&hwspinlock_tree_lock);
 	return hwlock;
 }
 EXPORT_SYMBOL_GPL(hwspin_lock_unregister);
@@ -400,9 +401,7 @@ EXPORT_SYMBOL_GPL(hwspin_lock_get_id);
  * to the remote core before it can be used for synchronization (to get the
  * id of a given hwlock, use hwspin_lock_get_id()).
  *
- * Can be called from an atomic context (will not sleep) but not from
- * within interrupt context (simply because there is no use case for
- * that yet).
+ * Should be called from a process context (might sleep)
  *
  * Returns the address of the assigned hwspinlock, or NULL on error
  */
@@ -411,7 +410,7 @@ struct hwspinlock *hwspin_lock_request(void)
 	struct hwspinlock *hwlock;
 	int ret;
 
-	spin_lock(&hwspinlock_tree_lock);
+	mutex_lock(&hwspinlock_tree_lock);
 
 	/* look for an unused lock */
 	ret = radix_tree_gang_lookup_tag(&hwspinlock_tree, (void **)&hwlock,
@@ -431,7 +430,7 @@ struct hwspinlock *hwspin_lock_request(void)
 		hwlock = NULL;
 
 out:
-	spin_unlock(&hwspinlock_tree_lock);
+	mutex_unlock(&hwspinlock_tree_lock);
 	return hwlock;
 }
 EXPORT_SYMBOL_GPL(hwspin_lock_request);
@@ -445,9 +444,7 @@ EXPORT_SYMBOL_GPL(hwspin_lock_request);
  * Usually early board code will be calling this function in order to
  * reserve specific hwspinlock ids for predefined purposes.
  *
- * Can be called from an atomic context (will not sleep) but not from
- * within interrupt context (simply because there is no use case for
- * that yet).
+ * Should be called from a process context (might sleep)
  *
  * Returns the address of the assigned hwspinlock, or NULL on error
  */
@@ -456,7 +453,7 @@ struct hwspinlock *hwspin_lock_request_specific(unsigned int id)
 	struct hwspinlock *hwlock;
 	int ret;
 
-	spin_lock(&hwspinlock_tree_lock);
+	mutex_lock(&hwspinlock_tree_lock);
 
 	/* make sure this hwspinlock exists */
 	hwlock = radix_tree_lookup(&hwspinlock_tree, id);
@@ -482,7 +479,7 @@ struct hwspinlock *hwspin_lock_request_specific(unsigned int id)
 		hwlock = NULL;
 
 out:
-	spin_unlock(&hwspinlock_tree_lock);
+	mutex_unlock(&hwspinlock_tree_lock);
 	return hwlock;
 }
 EXPORT_SYMBOL_GPL(hwspin_lock_request_specific);
@@ -495,9 +492,7 @@ EXPORT_SYMBOL_GPL(hwspin_lock_request_specific);
  * Should only be called with an @hwlock that was retrieved from
  * an earlier call to omap_hwspin_lock_request{_specific}.
  *
- * Can be called from an atomic context (will not sleep) but not from
- * within interrupt context (simply because there is no use case for
- * that yet).
+ * Should be called from a process context (might sleep)
  *
  * Returns 0 on success, or an appropriate error code on failure
  */
@@ -511,7 +506,7 @@ int hwspin_lock_free(struct hwspinlock *hwlock)
 		return -EINVAL;
 	}
 
-	spin_lock(&hwspinlock_tree_lock);
+	mutex_lock(&hwspinlock_tree_lock);
 
 	/* make sure the hwspinlock is used */
 	ret = radix_tree_tag_get(&hwspinlock_tree, hwlock->id,
@@ -538,7 +533,7 @@ int hwspin_lock_free(struct hwspinlock *hwlock)
 	module_put(hwlock->owner);
 
 out:
-	spin_unlock(&hwspinlock_tree_lock);
+	mutex_unlock(&hwspinlock_tree_lock);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(hwspin_lock_free);
diff --git a/drivers/i2c/Kconfig b/drivers/i2c/Kconfig
index 5f13c62e64b4..c4b583dc2567 100644
--- a/drivers/i2c/Kconfig
+++ b/drivers/i2c/Kconfig
@@ -60,6 +60,16 @@ config I2C_MUX
 
 source drivers/i2c/muxes/Kconfig
 
+config I2C_SLAVE
+	bool "I2C slave driver support"
+	default n
+	help
+	  Say Y here if you want the I2C slave functionality in the driver.
+	  The external system will be master and the system on which this
+	  driver is running act as i2c slave.
+	  This drivers supports read/write data from master devices through
+	  I2C.
+
 config I2C_HELPER_AUTO
 	bool "Autoselect pertinent helper modules"
 	default y
diff --git a/drivers/i2c/Makefile b/drivers/i2c/Makefile
index beee6b2d361d..b4fc25188117 100644
--- a/drivers/i2c/Makefile
+++ b/drivers/i2c/Makefile
@@ -1,12 +1,14 @@
 #
 # Makefile for the i2c core.
 #
+GCOV_PROFILE := y
 
 obj-$(CONFIG_I2C_BOARDINFO)	+= i2c-boardinfo.o
 obj-$(CONFIG_I2C)		+= i2c-core.o
 obj-$(CONFIG_I2C_SMBUS)		+= i2c-smbus.o
 obj-$(CONFIG_I2C_CHARDEV)	+= i2c-dev.o
 obj-$(CONFIG_I2C_MUX)		+= i2c-mux.o
+obj-$(CONFIG_I2C_SLAVE)		+= i2c-slave.o
 obj-y				+= algos/ busses/ muxes/
 
 ccflags-$(CONFIG_I2C_DEBUG_CORE) := -DDEBUG
diff --git a/drivers/i2c/algos/i2c-algo-bit.c b/drivers/i2c/algos/i2c-algo-bit.c
index d6d58684712b..eca3bcc4599d 100644
--- a/drivers/i2c/algos/i2c-algo-bit.c
+++ b/drivers/i2c/algos/i2c-algo-bit.c
@@ -486,7 +486,7 @@ static int bit_doAddress(struct i2c_adapter *i2c_adap, struct i2c_msg *msg)
 
 	if (flags & I2C_M_TEN) {
 		/* a ten bit address */
-		addr = 0xf0 | ((msg->addr >> 7) & 0x03);
+		addr = 0xf0 | ((msg->addr >> 7) & 0x06);
 		bit_dbg(2, &i2c_adap->dev, "addr0: %d\n", addr);
 		/* try extended address code...*/
 		ret = try_address(i2c_adap, addr, retries);
@@ -496,7 +496,7 @@ static int bit_doAddress(struct i2c_adapter *i2c_adap, struct i2c_msg *msg)
 			return -EREMOTEIO;
 		}
 		/* the remaining 8 bit address */
-		ret = i2c_outb(i2c_adap, msg->addr & 0x7f);
+		ret = i2c_outb(i2c_adap, msg->addr & 0xff);
 		if ((ret != 1) && !nak_ok) {
 			/* the chip did not ack / xmission error occurred */
 			dev_err(&i2c_adap->dev, "died at 2nd address code\n");
diff --git a/drivers/i2c/busses/Kconfig b/drivers/i2c/busses/Kconfig
index 646068e5100b..4c922f39bc47 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -641,6 +641,14 @@ config I2C_TEGRA
 	  If you say yes to this option, support will be included for the
 	  I2C controller embedded in NVIDIA Tegra SOCs
 
+config I2C_SLAVE_TEGRA
+	tristate "NVIDIA Tegra internal I2C slave controller"
+	depends on ARCH_TEGRA && I2C_SLAVE
+	default n
+	help
+	  If you say yes to this option, support will be included for the
+	  I2C slave controller embedded in NVIDIA Tegra SOCs
+
 config I2C_VERSATILE
 	tristate "ARM Versatile/Realview I2C bus support"
 	depends on ARCH_VERSATILE || ARCH_REALVIEW || ARCH_VEXPRESS
diff --git a/drivers/i2c/busses/Makefile b/drivers/i2c/busses/Makefile
index e6cf294d3729..381a6ca1a6bb 100644
--- a/drivers/i2c/busses/Makefile
+++ b/drivers/i2c/busses/Makefile
@@ -1,6 +1,7 @@
 #
 # Makefile for the i2c bus drivers.
 #
+GCOV_PROFILE := y
 
 # ACPI drivers
 obj-$(CONFIG_I2C_SCMI)		+= i2c-scmi.o
@@ -61,7 +62,10 @@ obj-$(CONFIG_I2C_SH7760)	+= i2c-sh7760.o
 obj-$(CONFIG_I2C_SH_MOBILE)	+= i2c-sh_mobile.o
 obj-$(CONFIG_I2C_SIMTEC)	+= i2c-simtec.o
 obj-$(CONFIG_I2C_STU300)	+= i2c-stu300.o
+CFLAGS_i2c-tegra.o		 = -Werror
 obj-$(CONFIG_I2C_TEGRA)		+= i2c-tegra.o
+CFLAGS_i2c-slave-tegra.o	 = -Werror
+obj-$(CONFIG_I2C_SLAVE_TEGRA)   += i2c-slave-tegra.o
 obj-$(CONFIG_I2C_VERSATILE)	+= i2c-versatile.o
 obj-$(CONFIG_I2C_OCTEON)	+= i2c-octeon.o
 obj-$(CONFIG_I2C_XILINX)	+= i2c-xiic.o
diff --git a/drivers/i2c/busses/i2c-slave-tegra.c b/drivers/i2c/busses/i2c-slave-tegra.c
new file mode 100644
index 000000000000..f2468d85c1f8
--- /dev/null
+++ b/drivers/i2c/busses/i2c-slave-tegra.c
@@ -0,0 +1,1114 @@
+/*
+ * drivers/i2c/busses/i2c-slave-tegra.c
+ * I2c slave driver for the Nvidia's tegra SOC.
+ *
+ * Copyright (c) 2009-2011, NVIDIA Corporation.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+/*#define DEBUG           1*/
+/*#define VERBOSE_DEBUG   1*/
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/i2c.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/i2c-tegra.h>
+#include <linux/i2c-slave.h>
+#include <asm/unaligned.h>
+#include <mach/clk.h>
+#include <mach/pinmux.h>
+#include <linux/pm_runtime.h>
+#define BYTES_PER_FIFO_WORD 4
+#define to_jiffies(msecs) msecs_to_jiffies(msecs)
+
+#define I2C_CNFG				0x000
+#define I2C_CNFG_PACKET_MODE_EN			(1<<10)
+#define I2C_CNFG_NEW_MASTER_FSM			(1<<11)
+#define I2C_CNFG_DEBOUNCE_CNT_SHIFT		12
+#define I2C_CNFG_DEBOUNCE_CNT_MASK		(0x7)
+
+#define I2C_STATUS				0x01C
+
+#define I2C_SLV_CNFG				0x020
+#define I2C_SLV_CNFG_NEWSL			(1<<2)
+#define I2C_SLV_CNFG_ENABLE_SL			(1<<3)
+#define I2C_SLV_CNFG_PKT_MODE_EN		(1<<4)
+#define I2C_SLV_CNFG_FIFO_XFER_EN		(1<<20)
+#define I2C_SLV_CNFG_ACK_LAST_BYTE		(1<<6)
+#define I2C_SLV_CNFG_ACK_LAST_BYTE_VALID	(1<<7)
+
+#define I2C_SLV_ADDR1				0x02c
+#define I2C_SLV_ADDR1_ADDR_SHIFT		0x0
+
+#define I2C_SLV_ADDR2				0x030
+#define I2C_SLV_ADDR2_ADDR0_HI_SHIFT		0x1
+#define I2C_SLV_ADDR2_ADDR0_MASK		0x7
+#define I2C_SLV_ADDR2_ADDR0_TEN_BIT_ADDR_MODE	0x1
+
+#define I2C_SLV_INT_MASK			0x040
+
+#define I2C_TX_FIFO				0x050
+#define I2C_RX_FIFO				0x054
+#define I2C_PACKET_TRANSFER_STATUS		0x058
+
+#define I2C_FIFO_CONTROL			0x05c
+#define I2C_FIFO_CONTROL_SLV_TX_FLUSH		(1<<9)
+#define I2C_FIFO_CONTROL_SLV_RX_FLUSH		(1<<8)
+#define I2C_FIFO_CONTROL_SLV_TX_TRIG_SHIFT	13
+#define I2C_FIFO_CONTROL_SLV_TX_TRIG_MASK	(0x7 << 13)
+#define I2C_FIFO_CONTROL_SLV_RX_TRIG_SHIFT	10
+#define I2C_FIFO_CONTROL_SLV_RX_TRIG_MASK	(1 << 10)
+
+#define I2C_FIFO_STATUS				0x060
+#define I2C_FIFO_STATUS_SLV_TX_MASK		(0xF << 20)
+#define I2C_FIFO_STATUS_SLV_TX_SHIFT		20
+#define I2C_FIFO_STATUS_SLV_RX_MASK		(0x0F << 16)
+#define I2C_FIFO_STATUS_SLV_RX_SHIFT		16
+
+#define I2C_INT_MASK				0x064
+#define I2C_INT_STATUS				0x068
+#define I2C_INT_PACKET_XFER_COMPLETE		(1<<7)
+#define I2C_INT_ALL_PACKETS_XFER_COMPLETE	(1<<6)
+#define I2C_INT_TX_FIFO_OVERFLOW		(1<<5)
+#define I2C_INT_RX_FIFO_UNDERFLOW		(1<<4)
+#define I2C_INT_NO_ACK				(1<<3)
+#define I2C_INT_ARBITRATION_LOST		(1<<2)
+#define I2C_INT_TX_FIFO_DATA_REQ		(1<<1)
+#define I2C_INT_RX_FIFO_DATA_REQ		(1<<0)
+
+#define I2C_INT_SLV_PKT_XFER_ERR		(1 << 25)
+#define I2C_INT_SLV_TX_BUFFER_REQ		(1 << 24)
+#define I2C_INT_SLV_RX_BUFFER_FILLED		(1 << 23)
+#define I2C_INT_SLV_PACKET_XFER_COMPLETE	(1 << 22)
+#define I2C_INT_SLV_TFIFO_OVF_REQ		(1 << 21)
+#define I2C_INT_SLV_RFIFO_UNF_REQ		(1 << 20)
+#define I2C_INT_SLV_TFIFO_DATA_REQ		(1 << 17)
+#define I2C_INT_SLV_RFIFO_DATA_REQ		(1 << 16)
+
+#define I2C_SLV_TX_FIFO				0x078
+#define I2C_SLV_RX_FIFO				0x07c
+
+#define I2C_SLV_PACKET_STATUS			0x80
+#define I2C_SLV_PACKET_STATUS_BYTENUM_SHIFT	4
+#define I2C_SLV_PACKET_STATUS_BYTENUM_MASK	0xFFF0
+
+#define I2C_CLK_DIVISOR				0x06c
+
+#define DVC_CTRL_REG1				0x000
+#define DVC_CTRL_REG1_INTR_EN			(1<<10)
+#define DVC_CTRL_REG2				0x004
+#define DVC_CTRL_REG3				0x008
+#define DVC_CTRL_REG3_SW_PROG			(1<<26)
+#define DVC_CTRL_REG3_I2C_DONE_INTR_EN		(1<<30)
+#define DVC_STATUS				0x00c
+#define DVC_STATUS_I2C_DONE_INTR		(1<<30)
+
+#define I2C_ERR_NONE				0x00
+#define I2C_ERR_NO_ACK				0x01
+#define I2C_ERR_ARBITRATION_LOST		0x02
+#define I2C_ERR_UNKNOWN_INTERRUPT		0x04
+
+#define PACKET_HEADER0_HEADER_SIZE_SHIFT	28
+#define PACKET_HEADER0_PACKET_ID_SHIFT		16
+#define PACKET_HEADER0_CONT_ID_SHIFT		12
+#define PACKET_HEADER0_PROTOCOL_I2C		(1<<4)
+
+#define I2C_HEADER_HIGHSPEED_MODE		(1<<22)
+#define I2C_HEADER_CONT_ON_NAK			(1<<21)
+#define I2C_HEADER_SEND_START_BYTE		(1<<20)
+#define I2C_HEADER_READ				(1<<19)
+#define I2C_HEADER_10BIT_ADDR			(1<<18)
+#define I2C_HEADER_IE_ENABLE			(1<<17)
+#define I2C_HEADER_REPEAT_START			(1<<16)
+#define I2C_HEADER_MASTER_ADDR_SHIFT		12
+#define I2C_HEADER_SLAVE_ADDR_SHIFT		1
+
+#define I2C_FIFO_DEPTH				8
+/* Transfer state of the i2c slave */
+#define TRANSFER_STATE_NONE			0
+#define TRANSFER_STATE_READ			1
+#define TRANSFER_STATE_WRITE			2
+
+#define I2C_SLV_TRANS_PREMATURE_END I2C_INT_SLV_PKT_XFER_ERR
+
+#define I2C_SLV_TRANS_ALL_XFER_END I2C_INT_SLV_PACKET_XFER_COMPLETE
+
+#define I2C_SLV_TRANS_END \
+	(I2C_INT_SLV_PKT_XFER_ERR | I2C_INT_SLV_PACKET_XFER_COMPLETE)
+
+#define I2C_INT_STATUS_RX_BUFFER_FILLED I2C_INT_SLV_RX_BUFFER_FILLED
+
+#define I2C_INT_STATUS_RX_DATA_AVAILABLE \
+	(I2C_INT_SLV_RX_BUFFER_FILLED | I2C_INT_SLV_RFIFO_DATA_REQ)
+
+#define I2C_INT_STATUS_TX_BUFFER_REQUEST \
+		(I2C_INT_SLV_TX_BUFFER_REQ | I2C_INT_SLV_TFIFO_DATA_REQ)
+
+#define I2C_SLV_ERRORS_INT_MASK  (I2C_INT_SLV_TFIFO_OVF_REQ | \
+		 I2C_INT_SLV_RFIFO_UNF_REQ | I2C_INT_SLV_PKT_XFER_ERR)
+
+#define I2C_SLV_DEFAULT_INT_MASK (I2C_INT_SLV_TFIFO_OVF_REQ | \
+		 I2C_INT_SLV_RFIFO_UNF_REQ | I2C_INT_SLV_PKT_XFER_ERR | \
+		 I2C_INT_SLV_RX_BUFFER_FILLED |  I2C_INT_SLV_TX_BUFFER_REQ)
+
+struct tegra_i2c_slave_dev;
+
+struct tegra_i2c_slave_bus {
+	struct tegra_i2c_slave_dev *dev;
+	const struct tegra_pingroup_config *pinmux;
+	int mux_len;
+	unsigned long bus_clk_rate;
+	struct i2c_slave_adapter slv_adap;
+};
+
+struct tegra_i2c_slave_dev {
+	struct device *dev;
+	struct clk *clk;
+	struct resource *iomem;
+	void __iomem *base;
+	int cont_id;
+	int irq;
+	spinlock_t lock;
+	struct completion rx_msg_complete;
+	struct completion tx_msg_complete;
+	bool is_rx_waiting;
+	bool is_tx_waiting;
+	u8 *rx_msg_buff;
+	int rx_msg_buf_size;
+	int rx_msg_head;
+	int rx_msg_tail;
+	u8 *tx_msg_buff;
+	int tx_msg_buf_size;
+	int tx_msg_head;
+	int tx_msg_tail;
+	bool is_slave_started;
+	int slave_add;
+	bool is_ten_bit_addr;
+	u32 dummy_word;
+	unsigned long rx_pack_hdr1;
+	unsigned long rx_pack_hdr2;
+	unsigned long rx_pack_hdr3;
+	int curr_transfer;
+	unsigned long int_mask;
+	int nack_packet_count;
+	bool is_first_byte_read_wait;
+	int curr_packet_bytes_read;
+	unsigned int cont_status;
+	bool is_dummy_char_cycle;
+	unsigned long curr_packet_tx_tail;
+	const struct tegra_pingroup_config *pin_mux;
+	int bus_clk;
+	struct tegra_i2c_slave_bus bus;
+};
+
+#define get_space_count(rInd, wInd, maxsize) \
+	    (((wInd > rInd) ? (maxsize - wInd + rInd) : (rInd - wInd)) - 1)
+
+#define get_data_count(rInd, wInd, maxsize) \
+	    ((wInd >= rInd) ? (wInd - rInd) : (maxsize - rInd + wInd - 1))
+
+static void set_tx_trigger_level(struct tegra_i2c_slave_dev *i2c_dev, int trig)
+{
+	unsigned long fifo_control = readl(i2c_dev->base + I2C_FIFO_CONTROL);
+	if (trig) {
+		fifo_control &= ~I2C_FIFO_CONTROL_SLV_TX_TRIG_MASK;
+		fifo_control |= (trig-1) << I2C_FIFO_CONTROL_SLV_TX_TRIG_SHIFT;
+		writel(fifo_control, i2c_dev->base + I2C_FIFO_CONTROL);
+	}
+}
+
+static void set_rx_trigger_level(struct tegra_i2c_slave_dev *i2c_dev, int trig)
+{
+	unsigned long fifo_control = readl(i2c_dev->base + I2C_FIFO_CONTROL);
+	if (trig) {
+		fifo_control &= ~I2C_FIFO_CONTROL_SLV_RX_TRIG_MASK;
+		fifo_control |= (trig-1) << I2C_FIFO_CONTROL_SLV_RX_TRIG_SHIFT;
+		writel(fifo_control, i2c_dev->base + I2C_FIFO_CONTROL);
+	}
+}
+
+static void reset_slave_tx_fifo(struct tegra_i2c_slave_dev *i2c_dev)
+{
+	unsigned long fifo_control = readl(i2c_dev->base + I2C_FIFO_CONTROL);
+	unsigned long timeout_count = 1000;
+
+	writel(fifo_control | I2C_FIFO_CONTROL_SLV_TX_FLUSH,
+					i2c_dev->base + I2C_FIFO_CONTROL);
+	while (timeout_count--) {
+		fifo_control = readl(i2c_dev->base + I2C_FIFO_CONTROL);
+		if (!(fifo_control & I2C_FIFO_CONTROL_SLV_TX_FLUSH))
+			break;
+		udelay(1);
+	}
+	if (!timeout_count) {
+		dev_err(i2c_dev->dev, "Not able to flush tx fifo\n");
+		BUG();
+	}
+}
+
+static void do_tx_fifo_empty(struct tegra_i2c_slave_dev *i2c_dev,
+			unsigned long *empty_count)
+{
+	unsigned long fifo_status = readl(i2c_dev->base + I2C_FIFO_STATUS);
+	unsigned long tx_fifo_empty_count;
+
+	tx_fifo_empty_count = (fifo_status & I2C_FIFO_STATUS_SLV_TX_MASK) >>
+					I2C_FIFO_STATUS_SLV_TX_SHIFT;
+	if (tx_fifo_empty_count < I2C_FIFO_DEPTH)
+		reset_slave_tx_fifo(i2c_dev);
+	if (empty_count)
+		*empty_count = tx_fifo_empty_count;
+}
+
+static void get_packet_headers(struct tegra_i2c_slave_dev *i2c_dev, u32 msg_len,
+		u32 flags, unsigned long *packet_header1,
+		unsigned long *packet_header2, unsigned long *packet_header3)
+{
+	unsigned long packet_header;
+	*packet_header1 = (0 << PACKET_HEADER0_HEADER_SIZE_SHIFT) |
+			PACKET_HEADER0_PROTOCOL_I2C |
+			(i2c_dev->cont_id << PACKET_HEADER0_CONT_ID_SHIFT) |
+			(1 << PACKET_HEADER0_PACKET_ID_SHIFT);
+	*packet_header2 = msg_len-1;
+	if (i2c_dev->is_ten_bit_addr)
+		packet_header = i2c_dev->slave_add | I2C_HEADER_10BIT_ADDR;
+	else
+		packet_header = i2c_dev->slave_add <<
+						I2C_HEADER_SLAVE_ADDR_SHIFT;
+
+	if (flags & I2C_M_RD)
+		packet_header |= I2C_HEADER_READ;
+
+	*packet_header3 = packet_header;
+}
+
+static void configure_i2c_slave_packet_mode(struct tegra_i2c_slave_dev *i2c_dev)
+{
+	unsigned long i2c_config;
+	i2c_config = I2C_CNFG_PACKET_MODE_EN | I2C_CNFG_NEW_MASTER_FSM;
+	i2c_config |= (2 << I2C_CNFG_DEBOUNCE_CNT_SHIFT);
+	writel(i2c_config, i2c_dev->base + I2C_CNFG);
+}
+
+static void configure_i2c_slave_address(struct tegra_i2c_slave_dev *i2c_dev)
+{
+
+	unsigned long slave_add_reg;
+	unsigned long i2c_slv_config;
+	unsigned long slave_add;
+
+	if (i2c_dev->is_ten_bit_addr) {
+		slave_add = i2c_dev->slave_add & 0xFF;
+		slave_add_reg = readl(i2c_dev->base + I2C_SLV_ADDR1);
+		slave_add_reg &= ~(0xFF);
+		slave_add_reg |= slave_add << I2C_SLV_ADDR1_ADDR_SHIFT;
+		writel(slave_add_reg, i2c_dev->base + I2C_SLV_ADDR1);
+
+		slave_add = (i2c_dev->slave_add >> 8) & 0x3;
+		slave_add_reg = readl(i2c_dev->base + I2C_SLV_ADDR2);
+		slave_add_reg &= ~I2C_SLV_ADDR2_ADDR0_MASK;
+		slave_add_reg |= slave_add |
+					I2C_SLV_ADDR2_ADDR0_TEN_BIT_ADDR_MODE;
+		writel(slave_add_reg, i2c_dev->base + I2C_SLV_ADDR2);
+	} else {
+		slave_add = (i2c_dev->slave_add & 0x3FF);
+		slave_add_reg = readl(i2c_dev->base + I2C_SLV_ADDR1);
+		slave_add_reg &= ~(0x3FF);
+		slave_add_reg |= slave_add << I2C_SLV_ADDR1_ADDR_SHIFT;
+		writel(slave_add_reg, i2c_dev->base + I2C_SLV_ADDR1);
+
+		slave_add_reg = readl(i2c_dev->base + I2C_SLV_ADDR2);
+		slave_add_reg &= ~I2C_SLV_ADDR2_ADDR0_MASK;
+		writel(slave_add_reg, i2c_dev->base + I2C_SLV_ADDR2);
+	}
+
+	i2c_slv_config = I2C_SLV_CNFG_NEWSL;
+	if (i2c_dev->slave_add) {
+		i2c_slv_config = I2C_SLV_CNFG_ENABLE_SL |
+				I2C_SLV_CNFG_PKT_MODE_EN |
+				I2C_SLV_CNFG_FIFO_XFER_EN;
+	}
+	writel(i2c_slv_config, i2c_dev->base + I2C_SLV_CNFG);
+}
+
+static void copy_rx_data(struct tegra_i2c_slave_dev *i2c_dev, u8 rcv_char)
+{
+	if (get_space_count(i2c_dev->rx_msg_tail, i2c_dev->rx_msg_head,
+			i2c_dev->rx_msg_buf_size)){
+		i2c_dev->rx_msg_buff[i2c_dev->rx_msg_head++] = rcv_char;
+		if (i2c_dev->rx_msg_head == i2c_dev->rx_msg_buf_size)
+			i2c_dev->rx_msg_head = 0;
+	} else {
+		dev_warn(i2c_dev->dev, "The slave rx buffer is full, ignoring "
+					"new receive data\n");
+	}
+}
+
+static void handle_packet_first_byte_read(struct tegra_i2c_slave_dev *i2c_dev)
+{
+	unsigned long fifo_status;
+	int filled_slots;
+	unsigned long i2c_sl_config;
+	unsigned long recv_data;
+
+	fifo_status = readl(i2c_dev->base + I2C_FIFO_STATUS);
+	filled_slots = (fifo_status & I2C_FIFO_STATUS_SLV_RX_MASK) >>
+					I2C_FIFO_STATUS_SLV_RX_SHIFT;
+
+	writel(I2C_INT_STATUS_RX_DATA_AVAILABLE,
+				i2c_dev->base + I2C_INT_STATUS);
+	if (unlikely(filled_slots != 1)) {
+		dev_err(i2c_dev->dev, "Unexpected number of filed slots %d",
+					filled_slots);
+		BUG();
+	}
+	recv_data = readl(i2c_dev->base + I2C_SLV_RX_FIFO);
+	copy_rx_data(i2c_dev, (u8)recv_data);
+
+	i2c_dev->is_first_byte_read_wait = false;
+	i2c_dev->curr_transfer = TRANSFER_STATE_READ;
+	i2c_dev->curr_packet_bytes_read = 0;
+
+	/* Write  packet Header */
+	writel(i2c_dev->rx_pack_hdr1, i2c_dev->base + I2C_SLV_TX_FIFO);
+	writel(i2c_dev->rx_pack_hdr2, i2c_dev->base + I2C_SLV_TX_FIFO);
+	writel(i2c_dev->rx_pack_hdr3, i2c_dev->base + I2C_SLV_TX_FIFO);
+
+	set_rx_trigger_level(i2c_dev, 4);
+	i2c_dev->int_mask |= I2C_INT_SLV_RFIFO_DATA_REQ;
+	writel(i2c_dev->int_mask, i2c_dev->base + I2C_INT_MASK);
+
+	/* Ack the master */
+	i2c_sl_config = readl(i2c_dev->base + I2C_SLV_CNFG);
+	i2c_sl_config |= I2C_SLV_CNFG_ACK_LAST_BYTE |
+				I2C_SLV_CNFG_ACK_LAST_BYTE_VALID;
+	writel(i2c_sl_config, i2c_dev->base + I2C_SLV_CNFG);
+}
+
+static void handle_packet_byte_read(struct tegra_i2c_slave_dev *i2c_dev)
+{
+	unsigned long fifo_status;
+	int i, j;
+	int filled_slots;
+	unsigned long recv_data;
+	int curr_xfer_size;
+
+	fifo_status = readl(i2c_dev->base + I2C_FIFO_STATUS);
+	filled_slots = (fifo_status & I2C_FIFO_STATUS_SLV_RX_MASK) >>
+					I2C_FIFO_STATUS_SLV_RX_SHIFT;
+
+	curr_xfer_size = BYTES_PER_FIFO_WORD * filled_slots;
+	if (i2c_dev->cont_status & I2C_SLV_TRANS_PREMATURE_END) {
+		curr_xfer_size = readl(i2c_dev->base + I2C_SLV_PACKET_STATUS);
+		curr_xfer_size =
+			(curr_xfer_size & I2C_SLV_PACKET_STATUS_BYTENUM_MASK) >>
+				I2C_SLV_PACKET_STATUS_BYTENUM_SHIFT;
+
+		BUG_ON(filled_slots != ((curr_xfer_size -
+				i2c_dev->curr_packet_bytes_read + 3) >> 2));
+		curr_xfer_size -= i2c_dev->curr_packet_bytes_read;
+	}
+
+	i2c_dev->curr_packet_bytes_read += curr_xfer_size;
+	for (i = 0; i < filled_slots; ++i) {
+		recv_data = readl(i2c_dev->base + I2C_SLV_RX_FIFO);
+		for (j = 0; j < BYTES_PER_FIFO_WORD; ++j) {
+			copy_rx_data(i2c_dev, (u8)(recv_data >> j*8));
+			curr_xfer_size--;
+			if (!curr_xfer_size)
+				break;
+		}
+	}
+	if (i2c_dev->cont_status & I2C_SLV_TRANS_PREMATURE_END) {
+		writel(I2C_SLV_TRANS_END | I2C_INT_STATUS_RX_BUFFER_FILLED,
+					i2c_dev->base + I2C_INT_STATUS);
+
+		i2c_dev->is_first_byte_read_wait = true;
+		i2c_dev->curr_transfer = TRANSFER_STATE_NONE;
+		i2c_dev->curr_packet_bytes_read = 0;
+		set_rx_trigger_level(i2c_dev, 1);
+		writel(0, i2c_dev->base + I2C_SLV_INT_MASK);
+		i2c_dev->int_mask = I2C_SLV_DEFAULT_INT_MASK;
+		writel(i2c_dev->int_mask, i2c_dev->base + I2C_INT_MASK);
+	}
+}
+
+static void handle_rx_interrupt(struct tegra_i2c_slave_dev *i2c_dev)
+{
+	if (i2c_dev->is_first_byte_read_wait)
+		handle_packet_first_byte_read(i2c_dev);
+	else
+		handle_packet_byte_read(i2c_dev);
+
+	if (i2c_dev->is_rx_waiting) {
+		complete(&i2c_dev->rx_msg_complete);
+		i2c_dev->is_rx_waiting = false;
+	}
+}
+
+static void handle_tx_transaction_end(struct tegra_i2c_slave_dev *i2c_dev)
+{
+	unsigned long curr_packet_size;
+
+	i2c_dev->curr_transfer = TRANSFER_STATE_NONE;
+	curr_packet_size = readl(i2c_dev->base + I2C_SLV_PACKET_STATUS);
+	curr_packet_size =
+		(curr_packet_size & I2C_SLV_PACKET_STATUS_BYTENUM_MASK) >>
+				I2C_SLV_PACKET_STATUS_BYTENUM_SHIFT;
+
+	/* Get transfer count from request size.*/
+	if ((curr_packet_size == 0) &&
+		   (i2c_dev->cont_status & I2C_SLV_TRANS_ALL_XFER_END) &&
+		    (!(i2c_dev->cont_status & I2C_SLV_TRANS_PREMATURE_END))) {
+		if (!i2c_dev->is_dummy_char_cycle)
+			i2c_dev->tx_msg_tail = i2c_dev->curr_packet_tx_tail;
+	} else {
+		if (!i2c_dev->is_dummy_char_cycle) {
+			i2c_dev->tx_msg_tail += curr_packet_size;
+			if (i2c_dev->tx_msg_tail >= i2c_dev->tx_msg_buf_size)
+				i2c_dev->tx_msg_tail -=
+						i2c_dev->tx_msg_buf_size;
+		}
+	}
+	writel(I2C_SLV_TRANS_END, i2c_dev->base + I2C_INT_STATUS);
+
+	i2c_dev->curr_transfer = TRANSFER_STATE_NONE;
+	set_tx_trigger_level(i2c_dev, 1);
+	writel(0, i2c_dev->base + I2C_SLV_INT_MASK);
+	i2c_dev->int_mask = I2C_SLV_DEFAULT_INT_MASK;
+	writel(i2c_dev->int_mask, i2c_dev->base + I2C_INT_MASK);
+	if (i2c_dev->is_tx_waiting) {
+		complete(&i2c_dev->tx_msg_complete);
+		i2c_dev->is_tx_waiting = false;
+	}
+}
+
+static void handle_tx_trigger_int(struct tegra_i2c_slave_dev *i2c_dev)
+{
+	unsigned long fifo_status;
+	int empty_slots;
+	int i, j;
+	int data_available;
+	unsigned long header1, header2, header3;
+	unsigned long tx_data;
+	int word_to_write;
+	int bytes_remain;
+	int bytes_in_curr_word;
+	int tx_tail;
+	int packet_len;
+
+	fifo_status = readl(i2c_dev->base + I2C_FIFO_STATUS);
+	empty_slots = (fifo_status & I2C_FIFO_STATUS_SLV_TX_MASK) >>
+					I2C_FIFO_STATUS_SLV_TX_SHIFT;
+	BUG_ON(empty_slots <= 3);
+	if (i2c_dev->curr_transfer == TRANSFER_STATE_NONE) {
+		empty_slots -= 3;
+
+		/* Clear the tfifo request. */
+		writel(I2C_INT_STATUS_TX_BUFFER_REQUEST,
+					i2c_dev->base + I2C_INT_STATUS);
+
+		/* Get Number of bytes it can transfer in current */
+		data_available = get_data_count(i2c_dev->tx_msg_tail,
+			i2c_dev->tx_msg_head, i2c_dev->tx_msg_buf_size);
+		if (data_available)
+			packet_len = min(empty_slots*BYTES_PER_FIFO_WORD,
+							 data_available);
+		else
+			packet_len = empty_slots*BYTES_PER_FIFO_WORD;
+
+		get_packet_headers(i2c_dev, packet_len, I2C_M_RD,
+					&header1, &header2, &header3);
+
+		/* Write  packet Header */
+		writel(header1, i2c_dev->base + I2C_SLV_TX_FIFO);
+		writel(header2, i2c_dev->base + I2C_SLV_TX_FIFO);
+		writel(header3, i2c_dev->base + I2C_SLV_TX_FIFO);
+
+		fifo_status = readl(i2c_dev->base + I2C_FIFO_STATUS);
+		if (data_available) {
+			word_to_write = (packet_len + 3) >> 2;
+			bytes_remain = packet_len;
+			tx_tail = i2c_dev->tx_msg_tail;
+			for (i = 0; i < word_to_write; i++) {
+				bytes_in_curr_word =
+					 min(bytes_remain, BYTES_PER_FIFO_WORD);
+				tx_data = 0;
+				for (j = 0; j < bytes_in_curr_word; ++j) {
+					tx_data |= (i2c_dev->tx_msg_buff[
+						  tx_tail++]<<(j*8));
+					if (tx_tail >= i2c_dev->tx_msg_buf_size)
+						tx_tail = 0;
+				}
+				writel(tx_data, i2c_dev->base +
+							I2C_SLV_TX_FIFO);
+				bytes_remain -= bytes_in_curr_word;
+			}
+			i2c_dev->curr_packet_tx_tail = tx_tail;
+			i2c_dev->is_dummy_char_cycle = false;
+		} else {
+			i2c_dev->curr_packet_tx_tail = i2c_dev->tx_msg_tail;
+			for (i = 0; i < empty_slots; i++)
+				writel(i2c_dev->dummy_word,
+					i2c_dev->base + I2C_SLV_TX_FIFO);
+			i2c_dev->is_dummy_char_cycle = true;
+		}
+
+		i2c_dev->curr_transfer = TRANSFER_STATE_WRITE;
+		i2c_dev->int_mask &= ~I2C_INT_SLV_TFIFO_DATA_REQ;
+		i2c_dev->int_mask |= I2C_SLV_TRANS_END;
+		writel(i2c_dev->int_mask, i2c_dev->base + I2C_INT_MASK);
+	} else {
+		dev_err(i2c_dev->dev, "I2cSlaveIsr(): Illegal transfer at "
+				"this point\n");
+		BUG();
+	}
+}
+
+static void handle_tx_interrupt(struct tegra_i2c_slave_dev *i2c_dev)
+{
+	if (i2c_dev->cont_status & I2C_SLV_TRANS_END)
+		handle_tx_transaction_end(i2c_dev);
+	else
+		handle_tx_trigger_int(i2c_dev);
+}
+
+static irqreturn_t tegra_i2c_slave_isr(int irq, void *dev_id)
+{
+	struct tegra_i2c_slave_dev *i2c_dev = dev_id;
+	unsigned long flags;
+
+	/* Read the Interrupt status register & PKT_STATUS */
+	i2c_dev->cont_status = readl(i2c_dev->base + I2C_INT_STATUS);
+
+	dev_dbg(i2c_dev->dev, "ISR ContStatus 0x%08x\n", i2c_dev->cont_status);
+	spin_lock_irqsave(&i2c_dev->lock, flags);
+
+	if ((i2c_dev->cont_status & I2C_INT_STATUS_RX_DATA_AVAILABLE) ||
+		(i2c_dev->curr_transfer == TRANSFER_STATE_READ)) {
+		handle_rx_interrupt(i2c_dev);
+		goto Done;
+	}
+
+	if ((i2c_dev->cont_status & I2C_INT_STATUS_TX_BUFFER_REQUEST) ||
+		(i2c_dev->curr_transfer == TRANSFER_STATE_WRITE)) {
+		handle_tx_interrupt(i2c_dev);
+		goto Done;
+	}
+
+	dev_err(i2c_dev->dev, "Tegra I2c Slave got unwanted interrupt "
+			"IntStatus 0x%08x\n", i2c_dev->cont_status);
+	BUG();
+
+Done:
+	spin_unlock_irqrestore(&i2c_dev->lock, flags);
+	return IRQ_HANDLED;
+}
+
+static int tegra_i2c_slave_start(struct i2c_slave_adapter *slv_adap, int addr,
+		int is_ten_bit_addr, unsigned char dummy_char)
+{
+	struct tegra_i2c_slave_bus *i2c_bus = i2c_get_slave_adapdata(slv_adap);
+	struct tegra_i2c_slave_dev *i2c_dev = i2c_bus->dev;
+	unsigned long flags;
+
+	spin_lock_irqsave(&i2c_dev->lock, flags);
+	if (i2c_dev->is_slave_started) {
+		spin_unlock_irqrestore(&i2c_dev->lock, flags);
+		return -EBUSY;
+	}
+
+	i2c_dev->rx_msg_buff = (u8 *)(i2c_dev+1);
+	i2c_dev->rx_msg_head = 0;
+	i2c_dev->rx_msg_tail = 0;
+	i2c_dev->is_rx_waiting = false;
+
+	i2c_dev->tx_msg_head = 0;
+	i2c_dev->tx_msg_tail = 0;
+	i2c_dev->is_tx_waiting = true;
+
+	i2c_dev->dummy_word =  (dummy_char << 8) | dummy_char;
+	i2c_dev->dummy_word |=  i2c_dev->dummy_word << 16;
+
+	i2c_dev->slave_add = addr;
+	i2c_dev->is_ten_bit_addr = is_ten_bit_addr;
+
+	get_packet_headers(i2c_dev, 4096, 0, &i2c_dev->rx_pack_hdr1,
+		&i2c_dev->rx_pack_hdr2, &i2c_dev->rx_pack_hdr3);
+
+	pm_runtime_get_sync(i2c_dev->dev);
+	configure_i2c_slave_packet_mode(i2c_dev);
+	configure_i2c_slave_address(i2c_dev);
+	do_tx_fifo_empty(i2c_dev, NULL);
+	set_rx_trigger_level(i2c_dev, 1);
+	writel(0, i2c_dev->base + I2C_SLV_INT_MASK);
+
+	if (i2c_bus->pinmux)
+		tegra_pinmux_config_tristate_table(i2c_bus->pinmux,
+				i2c_bus->mux_len, TEGRA_TRI_NORMAL);
+
+	i2c_dev->curr_transfer = 0;
+	i2c_dev->is_slave_started = true;
+	i2c_dev->int_mask =  I2C_SLV_DEFAULT_INT_MASK;
+	i2c_dev->is_first_byte_read_wait = true;
+	writel(i2c_dev->int_mask, i2c_dev->base + I2C_INT_MASK);
+	spin_unlock_irqrestore(&i2c_dev->lock, flags);
+	return 0;
+}
+
+static void tegra_i2c_slave_stop(struct i2c_slave_adapter *slv_adap,
+		int is_buffer_clear)
+{
+	struct tegra_i2c_slave_bus *i2c_bus = i2c_get_slave_adapdata(slv_adap);
+	struct tegra_i2c_slave_dev *i2c_dev = i2c_bus->dev;
+	unsigned long flags;
+
+	spin_lock_irqsave(&i2c_dev->lock, flags);
+	if (!i2c_dev->is_slave_started) {
+		spin_unlock_irqrestore(&i2c_dev->lock, flags);
+		return;
+	}
+
+	i2c_dev->slave_add = 0;
+	i2c_dev->is_ten_bit_addr = false;
+	configure_i2c_slave_address(i2c_dev);
+	writel(0, i2c_dev->base + I2C_SLV_INT_MASK);
+	writel(0, i2c_dev->base + I2C_INT_MASK);
+	i2c_dev->curr_transfer = 0;
+	i2c_dev->is_slave_started = false;
+	pm_runtime_put_sync(i2c_dev->dev);
+	if (is_buffer_clear) {
+		i2c_dev->rx_msg_head = 0;
+		i2c_dev->rx_msg_tail = 0;
+		i2c_dev->is_rx_waiting = false;
+		i2c_dev->tx_msg_head = 0;
+		i2c_dev->tx_msg_tail = 0;
+		i2c_dev->is_tx_waiting = false;
+	}
+	if (i2c_bus->pinmux)
+		tegra_pinmux_config_tristate_table(i2c_bus->pinmux,
+				i2c_bus->mux_len, TEGRA_TRI_TRISTATE);
+	spin_unlock_irqrestore(&i2c_dev->lock, flags);
+}
+
+static int tegra_i2c_slave_send(struct i2c_slave_adapter *slv_adap,
+		const char *buf, int count)
+{
+	struct tegra_i2c_slave_bus *i2c_bus = i2c_get_slave_adapdata(slv_adap);
+	struct tegra_i2c_slave_dev *i2c_dev = i2c_bus->dev;
+	unsigned long flags;
+	unsigned long space_available;
+	int i;
+
+	spin_lock_irqsave(&i2c_dev->lock, flags);
+	if (!i2c_dev->is_slave_started) {
+		spin_unlock_irqrestore(&i2c_dev->lock, flags);
+		return -EPERM;
+	}
+
+	space_available = get_space_count(i2c_dev->tx_msg_tail,
+				i2c_dev->tx_msg_head, i2c_dev->tx_msg_buf_size);
+	if (space_available < count) {
+		spin_unlock_irqrestore(&i2c_dev->lock, flags);
+		return 0;
+	}
+
+	for (i = 0; i < count; ++i) {
+		i2c_dev->tx_msg_buff[i2c_dev->tx_msg_head++] = *buf++;
+		if (i2c_dev->tx_msg_head >= i2c_dev->tx_msg_buf_size)
+			i2c_dev->tx_msg_head = 0;
+	}
+	i2c_dev->is_tx_waiting = false;
+	spin_unlock_irqrestore(&i2c_dev->lock, flags);
+	return count;
+}
+
+static int tegra_i2c_slave_get_tx_status(struct i2c_slave_adapter *slv_adap,
+		int timeout_ms)
+{
+	struct tegra_i2c_slave_bus *i2c_bus = i2c_get_slave_adapdata(slv_adap);
+	struct tegra_i2c_slave_dev *i2c_dev = i2c_bus->dev;
+	unsigned long flags;
+	unsigned long data_available;
+
+	spin_lock_irqsave(&i2c_dev->lock, flags);
+	if (!i2c_dev->is_slave_started) {
+		spin_unlock_irqrestore(&i2c_dev->lock, flags);
+		return -EPERM;
+	}
+
+	data_available = get_data_count(i2c_dev->tx_msg_tail,
+				i2c_dev->tx_msg_head, i2c_dev->tx_msg_buf_size);
+	if (!data_available) {
+		spin_unlock_irqrestore(&i2c_dev->lock, flags);
+		return 0;
+	}
+
+	INIT_COMPLETION(i2c_dev->tx_msg_complete);
+	if (timeout_ms)
+		i2c_dev->is_tx_waiting = true;
+	spin_unlock_irqrestore(&i2c_dev->lock, flags);
+	if (timeout_ms) {
+		wait_for_completion_timeout(&i2c_dev->tx_msg_complete,
+					to_jiffies(timeout_ms));
+		spin_lock_irqsave(&i2c_dev->lock, flags);
+		i2c_dev->is_tx_waiting = false;
+		data_available = get_data_count(i2c_dev->tx_msg_tail,
+					i2c_dev->tx_msg_head,
+					i2c_dev->tx_msg_buf_size);
+		spin_unlock_irqrestore(&i2c_dev->lock, flags);
+		if (data_available)
+			return -ETIMEDOUT;
+	}
+	return data_available;
+}
+
+/*
+ * Timeoutms = 0, MinBytesRead = 0, read without waiting.
+ * Timeoutms = 0, MinBytesRead != 0, block till min bytes read.
+ * Timeoutms != 0, wait till timeout to read data..
+ * Timeoutms  = INF, wait till all req bytes read.
+ */
+
+static int tegra_i2c_slave_recv(struct i2c_slave_adapter *slv_adap, char *buf,
+		int count, int min_count, int timeout_ms)
+{
+	struct tegra_i2c_slave_bus *i2c_bus = i2c_get_slave_adapdata(slv_adap);
+	struct tegra_i2c_slave_dev *i2c_dev = i2c_bus->dev;
+	unsigned long flags;
+	int data_available;
+	int bytes_copy;
+	int i;
+	int read_count = 0;
+	bool is_inf_wait = false;
+	int run_count = 0;
+
+	spin_lock_irqsave(&i2c_dev->lock, flags);
+	if (!i2c_dev->is_slave_started) {
+		spin_unlock_irqrestore(&i2c_dev->lock, flags);
+		return -EPERM;
+	}
+
+	do {
+		data_available = get_data_count(i2c_dev->rx_msg_tail,
+				i2c_dev->rx_msg_head, i2c_dev->rx_msg_buf_size);
+
+		bytes_copy = min(data_available, count);
+
+		if (!data_available) {
+			spin_unlock_irqrestore(&i2c_dev->lock, flags);
+			return 0;
+		}
+		for (i = 0; i < bytes_copy; ++i) {
+			*buf++ = i2c_dev->rx_msg_buff[i2c_dev->rx_msg_tail++];
+			if (i2c_dev->rx_msg_tail >= i2c_dev->rx_msg_buf_size)
+				i2c_dev->rx_msg_tail = 0;
+			read_count++;
+		}
+		if (!timeout_ms) {
+			if ((!min_count) || (read_count >= min_count))
+				break;
+			is_inf_wait = true;
+		} else {
+			if ((read_count == count) || run_count)
+				break;
+		}
+		i2c_dev->is_rx_waiting = true;
+		INIT_COMPLETION(i2c_dev->rx_msg_complete);
+		spin_unlock_irqrestore(&i2c_dev->lock, flags);
+		if (is_inf_wait)
+			wait_for_completion(&i2c_dev->rx_msg_complete);
+		else
+			wait_for_completion_timeout(&i2c_dev->rx_msg_complete,
+						to_jiffies(timeout_ms));
+
+		spin_lock_irqsave(&i2c_dev->lock, flags);
+	} while (1);
+	spin_unlock_irqrestore(&i2c_dev->lock, flags);
+	i2c_dev->is_rx_waiting = false;
+	return read_count;
+}
+
+static int tegra_i2c_slave_flush_buffer(struct i2c_slave_adapter *slv_adap,
+		int is_flush_tx_buffer, int is_flush_rx_buffer)
+{
+	struct tegra_i2c_slave_bus *i2c_bus = i2c_get_slave_adapdata(slv_adap);
+	struct tegra_i2c_slave_dev *i2c_dev = i2c_bus->dev;
+	unsigned long flags;
+
+	spin_lock_irqsave(&i2c_dev->lock, flags);
+	if (!i2c_dev->is_slave_started) {
+		spin_unlock_irqrestore(&i2c_dev->lock, flags);
+		return -EPERM;
+	}
+	if (is_flush_tx_buffer) {
+		i2c_dev->tx_msg_head = 0;
+		i2c_dev->tx_msg_tail = 0;
+	}
+	if (is_flush_rx_buffer) {
+		i2c_dev->rx_msg_head = 0;
+		i2c_dev->rx_msg_tail = 0;
+	}
+	spin_unlock_irqrestore(&i2c_dev->lock, flags);
+	return 0;
+}
+
+static int tegra_i2c_slave_get_nack_cycle(struct i2c_slave_adapter *slv_adap,
+		int is_cout_reset)
+{
+	struct tegra_i2c_slave_bus *i2c_bus = i2c_get_slave_adapdata(slv_adap);
+	struct tegra_i2c_slave_dev *i2c_dev = i2c_bus->dev;
+	unsigned long flags;
+	int retval;
+
+	spin_lock_irqsave(&i2c_dev->lock, flags);
+	if (!i2c_dev->is_slave_started) {
+		spin_unlock_irqrestore(&i2c_dev->lock, flags);
+		dev_dbg(i2c_dev->dev, "The slave bus is already started\n");
+		return -EPERM;
+	}
+
+	retval = i2c_dev->nack_packet_count;
+	if (is_cout_reset)
+		i2c_dev->nack_packet_count = 0;
+
+	spin_unlock_irqrestore(&i2c_dev->lock, flags);
+	return retval;
+}
+
+static const struct i2c_slave_algorithm tegra_i2c_slave_algo = {
+	.slave_start		= tegra_i2c_slave_start,
+	.slave_stop		= tegra_i2c_slave_stop,
+	.slave_send		= tegra_i2c_slave_send,
+	.slave_get_tx_status	= tegra_i2c_slave_get_tx_status,
+	.slave_recv		= tegra_i2c_slave_recv,
+	.slave_flush_buffer	= tegra_i2c_slave_flush_buffer,
+	.slave_get_nack_cycle	= tegra_i2c_slave_get_nack_cycle,
+};
+
+static int tegra_i2c_slave_probe(struct platform_device *pdev)
+{
+	struct tegra_i2c_slave_dev *i2c_dev;
+	struct tegra_i2c_slave_bus *i2c_bus = NULL;
+	struct tegra_i2c_slave_platform_data *pdata = pdev->dev.platform_data;
+	struct resource *res;
+	struct resource *iomem;
+	struct clk *clk;
+	void *base;
+	int irq;
+	int ret = 0;
+	int rx_buffer_size;
+	int tx_buffer_size;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform data?\n");
+		return -ENODEV;
+	}
+
+	if (pdata->adapter_nr < 0) {
+		dev_err(&pdev->dev, "invalid platform data?\n");
+		return -ENODEV;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "no mem resource?\n");
+		return -ENODEV;
+	}
+	iomem = request_mem_region(res->start, resource_size(res), pdev->name);
+	if (!iomem) {
+		dev_err(&pdev->dev, "I2C region already claimed\n");
+		return -EBUSY;
+	}
+
+	base = ioremap(iomem->start, resource_size(iomem));
+	if (!base) {
+		dev_err(&pdev->dev, "Can't ioremap I2C region\n");
+		return -ENOMEM;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "no irq resource?\n");
+		ret = -ENODEV;
+		goto err_iounmap;
+	}
+	irq = res->start;
+
+	clk = clk_get(&pdev->dev, NULL);
+	if (!clk) {
+		ret = -ENODEV;
+		goto err_release_region;
+	}
+
+	rx_buffer_size = (pdata->max_rx_buffer_size)?:4096;
+	tx_buffer_size = (pdata->max_tx_buffer_size)?:4096;
+	i2c_dev = kzalloc(sizeof(struct tegra_i2c_slave_dev) +
+			rx_buffer_size + tx_buffer_size, GFP_KERNEL);
+	if (!i2c_dev) {
+		ret = -ENOMEM;
+		goto err_clk_put;
+	}
+
+	i2c_dev->base = base;
+	i2c_dev->clk = clk;
+	i2c_dev->iomem = iomem;
+	i2c_dev->irq = irq;
+	i2c_dev->cont_id = pdev->id;
+	i2c_dev->dev = &pdev->dev;
+	i2c_dev->bus_clk = pdata->bus_clk_rate?: 100000;
+	i2c_dev->rx_msg_buff = (u8 *)(i2c_dev+1);
+	i2c_dev->rx_msg_buf_size = rx_buffer_size;
+	i2c_dev->rx_msg_head = 0;
+	i2c_dev->rx_msg_tail = 0;
+	i2c_dev->is_rx_waiting = 0;
+	i2c_dev->tx_msg_buff = i2c_dev->rx_msg_buff + rx_buffer_size;
+	i2c_dev->tx_msg_buf_size = tx_buffer_size;
+	i2c_dev->tx_msg_head = 0;
+	i2c_dev->tx_msg_tail = 0;
+	i2c_dev->is_tx_waiting = 0;
+
+	i2c_dev->is_slave_started = false;
+	spin_lock_init(&i2c_dev->lock);
+
+	init_completion(&i2c_dev->rx_msg_complete);
+	init_completion(&i2c_dev->tx_msg_complete);
+
+	platform_set_drvdata(pdev, i2c_dev);
+
+	ret = request_irq(i2c_dev->irq, tegra_i2c_slave_isr, IRQF_DISABLED,
+					pdev->name, i2c_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to request irq %i\n", i2c_dev->irq);
+		goto err_free;
+	}
+
+	i2c_bus = &i2c_dev->bus;
+	i2c_bus->dev = i2c_dev;
+	i2c_bus->pinmux = pdata->pinmux;
+	i2c_bus->mux_len = pdata->bus_mux_len;
+	i2c_bus->bus_clk_rate = pdata->bus_clk_rate ?: 100000;
+
+	i2c_bus->slv_adap.slv_algo = &tegra_i2c_slave_algo;
+	i2c_bus->slv_adap.owner = THIS_MODULE;
+	i2c_bus->slv_adap.class = I2C_CLASS_HWMON;
+	strlcpy(i2c_bus->slv_adap.name, "Tegra I2C SLAVE adapter",
+				sizeof(i2c_bus->slv_adap.name));
+	i2c_bus->slv_adap.parent_dev = &pdev->dev;
+	i2c_bus->slv_adap.dev = NULL;
+	i2c_bus->slv_adap.nr = pdata->adapter_nr;
+	ret = i2c_add_slave_adapter(&i2c_bus->slv_adap, true);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to add I2C adapter\n");
+		goto err_free_irq;
+	}
+	i2c_set_slave_adapdata(&i2c_bus->slv_adap, i2c_bus);
+	dev_dbg(&pdev->dev, "%s() suucess\n", __func__);
+	pm_runtime_enable(i2c_dev->dev);
+	return 0;
+
+err_free_irq:
+	free_irq(i2c_dev->irq, i2c_dev);
+err_free:
+	kfree(i2c_dev);
+err_clk_put:
+	clk_put(clk);
+err_release_region:
+	release_mem_region(iomem->start, resource_size(iomem));
+err_iounmap:
+	iounmap(base);
+	dev_dbg(&pdev->dev, "%s() failed %d\n", __func__, ret);
+	return ret;
+}
+
+static int tegra_i2c_slave_remove(struct platform_device *pdev)
+{
+	struct tegra_i2c_slave_dev *i2c_dev = platform_get_drvdata(pdev);
+
+	i2c_del_slave_adapter(&i2c_dev->bus.slv_adap);
+	pm_runtime_disable(i2c_dev->dev);
+	free_irq(i2c_dev->irq, i2c_dev);
+	clk_put(i2c_dev->clk);
+	release_mem_region(i2c_dev->iomem->start,
+			resource_size(i2c_dev->iomem));
+	iounmap(i2c_dev->base);
+	kfree(i2c_dev);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int tegra_i2c_slave_suspend(struct platform_device *pdev,
+	pm_message_t state)
+{
+	return 0;
+}
+
+static int tegra_i2c_slave_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+#endif
+#if defined(CONFIG_PM_RUNTIME)
+static int tegra_i2c_slave_runtime_idle(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct tegra_i2c_slave_dev *i2c_dev = platform_get_drvdata(pdev);
+	clk_disable(i2c_dev->clk);
+	return 0;
+}
+static int tegra_i2c_slave_runtime_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct tegra_i2c_slave_dev *i2c_dev = platform_get_drvdata(pdev);
+	clk_enable(i2c_dev->clk);
+	return 0;
+}
+static const struct dev_pm_ops tegra_i2c_slave_dev_pm_ops = {
+	.runtime_idle = tegra_i2c_slave_runtime_idle,
+	.runtime_resume = tegra_i2c_slave_runtime_resume,
+};
+#endif
+
+static struct platform_driver tegra_i2c_slave_driver = {
+	.probe   = tegra_i2c_slave_probe,
+	.remove  = tegra_i2c_slave_remove,
+#ifdef CONFIG_PM
+	.suspend = tegra_i2c_slave_suspend,
+	.resume  = tegra_i2c_slave_resume,
+#endif
+	.driver  = {
+		.name  = "tegra-i2c-slave",
+		.owner = THIS_MODULE,
+#if defined(CONFIG_PM_RUNTIME)
+		.pm = &tegra_i2c_slave_dev_pm_ops,
+#endif
+	},
+};
+
+static int __init tegra_i2c_slave_init_driver(void)
+{
+	return platform_driver_register(&tegra_i2c_slave_driver);
+}
+
+static void __exit tegra_i2c_slave_exit_driver(void)
+{
+	platform_driver_unregister(&tegra_i2c_slave_driver);
+}
+subsys_initcall(tegra_i2c_slave_init_driver);
+module_exit(tegra_i2c_slave_exit_driver);
diff --git a/drivers/i2c/busses/i2c-tegra.c b/drivers/i2c/busses/i2c-tegra.c
index 3c94c4a81a55..b3c7a762760a 100644
--- a/drivers/i2c/busses/i2c-tegra.c
+++ b/drivers/i2c/busses/i2c-tegra.c
@@ -4,6 +4,8 @@
  * Copyright (C) 2010 Google, Inc.
  * Author: Colin Cross <ccross@android.com>
  *
+ * Copyright (C) 2010-2012 NVIDIA Corporation
+ *
  * This software is licensed under the terms of the GNU General Public
  * License version 2, as published by the Free Software Foundation, and
  * may be copied, distributed, and modified under those terms.
@@ -15,6 +17,9 @@
  *
  */
 
+/*#define DEBUG           1*/
+/*#define VERBOSE_DEBUG   1*/
+
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/platform_device.h>
@@ -24,22 +29,27 @@
 #include <linux/io.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
+#include <linux/pm_runtime.h>
 #include <linux/slab.h>
 #include <linux/i2c-tegra.h>
 #include <linux/of_i2c.h>
+#include <linux/spinlock.h>
 
 #include <asm/unaligned.h>
 
 #include <mach/clk.h>
+#include <mach/pinmux.h>
 
-#define TEGRA_I2C_TIMEOUT (msecs_to_jiffies(1000))
-#define BYTES_PER_FIFO_WORD 4
+#define TEGRA_I2C_TIMEOUT			(msecs_to_jiffies(1000))
+#define TEGRA_I2C_RETRIES			3
+#define BYTES_PER_FIFO_WORD			4
 
 #define I2C_CNFG				0x000
 #define I2C_CNFG_DEBOUNCE_CNT_SHIFT		12
 #define I2C_CNFG_PACKET_MODE_EN			(1<<10)
 #define I2C_CNFG_NEW_MASTER_FSM			(1<<11)
 #define I2C_STATUS				0x01C
+#define I2C_STATUS_BUSY				(1<<8)
 #define I2C_SL_CNFG				0x020
 #define I2C_SL_CNFG_NACK			(1<<1)
 #define I2C_SL_CNFG_NEWSL			(1<<2)
@@ -83,6 +93,7 @@
 #define I2C_ERR_NO_ACK				0x01
 #define I2C_ERR_ARBITRATION_LOST		0x02
 #define I2C_ERR_UNKNOWN_INTERRUPT		0x04
+#define I2C_ERR_UNEXPECTED_STATUS		0x08
 
 #define PACKET_HEADER0_HEADER_SIZE_SHIFT	28
 #define PACKET_HEADER0_PACKET_ID_SHIFT		16
@@ -96,16 +107,55 @@
 #define I2C_HEADER_10BIT_ADDR			(1<<18)
 #define I2C_HEADER_IE_ENABLE			(1<<17)
 #define I2C_HEADER_REPEAT_START			(1<<16)
+#define I2C_HEADER_CONTINUE_XFER		(1<<15)
 #define I2C_HEADER_MASTER_ADDR_SHIFT		12
 #define I2C_HEADER_SLAVE_ADDR_SHIFT		1
 
+#define SL_ADDR1(addr) (addr & 0xff)
+#define SL_ADDR2(addr) ((addr >> 8) & 0xff)
+
+/*
+ * msg_end_type: The bus control which need to be send at end of transfer.
+ * @MSG_END_STOP: Send stop pulse at end of transfer.
+ * @MSG_END_REPEAT_START: Send repeat start at end of transfer.
+ * @MSG_END_CONTINUE: The following on message is coming and so do not send
+ *		stop or repeat start.
+ */
+
+enum msg_end_type {
+	MSG_END_STOP,
+	MSG_END_REPEAT_START,
+	MSG_END_CONTINUE,
+};
+
+struct tegra_i2c_dev;
+
+/**
+ * struct tegra_i2c_hw_feature : Different HW support on Tegra
+ * @has_continue_xfer_support: Continue transfer supports.
+ */
+
+struct tegra_i2c_hw_feature {
+	bool has_continue_xfer_support;
+};
+
+struct tegra_i2c_bus {
+	struct tegra_i2c_dev *dev;
+	const struct tegra_pingroup_config *mux;
+	int mux_len;
+	unsigned long bus_clk_rate;
+	struct i2c_adapter adapter;
+	int scl_gpio;
+	int sda_gpio;
+};
+
 /**
  * struct tegra_i2c_dev	- per device i2c context
  * @dev: device reference for power management
+ * @hw: Tegra i2c hw feature.
  * @adapter: core i2c layer adapter information
- * @clk: clock reference for i2c controller
- * @i2c_clk: clock reference for i2c bus
- * @iomem: memory resource for registers
+ * @div_clk: clock reference for div clock of i2c controller.
+ * @fast_clk: clock reference for fast clock of i2c controller.
  * @base: ioremapped registers cookie
  * @cont_id: i2c controller id, used for for packet header
  * @irq: irq number of transfer complete interrupt
@@ -120,10 +170,11 @@
  */
 struct tegra_i2c_dev {
 	struct device *dev;
-	struct i2c_adapter adapter;
-	struct clk *clk;
-	struct clk *i2c_clk;
-	struct resource *iomem;
+	struct tegra_i2c_hw_feature *hw;
+	struct clk *div_clk;
+	struct clk *fast_clk;
+	struct rt_mutex dev_lock;
+	spinlock_t fifo_lock;
 	void __iomem *base;
 	int cont_id;
 	int irq;
@@ -132,10 +183,25 @@ struct tegra_i2c_dev {
 	struct completion msg_complete;
 	int msg_err;
 	u8 *msg_buf;
+	u32 packet_header;
+	u32 payload_size;
+	u32 io_header;
 	size_t msg_buf_remaining;
 	int msg_read;
-	unsigned long bus_clk_rate;
+	struct i2c_msg *msgs;
+	int msg_add;
+	int msgs_num;
 	bool is_suspended;
+	int bus_count;
+	const struct tegra_pingroup_config *last_mux;
+	int last_mux_len;
+	unsigned long last_bus_clk_rate;
+	u16 slave_addr;
+	bool is_clkon_always;
+	bool is_high_speed_enable;
+	u16 hs_master_code;
+	int (*arb_recovery)(int scl_gpio, int sda_gpio);
+	struct tegra_i2c_bus busses[1];
 };
 
 static void dvc_writel(struct tegra_i2c_dev *i2c_dev, u32 val, unsigned long reg)
@@ -148,6 +214,20 @@ static u32 dvc_readl(struct tegra_i2c_dev *i2c_dev, unsigned long reg)
 	return readl(i2c_dev->base + reg);
 }
 
+static void dvc_i2c_mask_irq(struct tegra_i2c_dev *i2c_dev, u32 mask)
+{
+	u32 int_mask = dvc_readl(i2c_dev, DVC_CTRL_REG3);
+	int_mask &= ~mask;
+	dvc_writel(i2c_dev, int_mask, DVC_CTRL_REG3);
+}
+
+static void dvc_i2c_unmask_irq(struct tegra_i2c_dev *i2c_dev, u32 mask)
+{
+	u32 int_mask = dvc_readl(i2c_dev, DVC_CTRL_REG3);
+	int_mask |= mask;
+	dvc_writel(i2c_dev, int_mask, DVC_CTRL_REG3);
+}
+
 /*
  * i2c_writel and i2c_readl will offset the register if necessary to talk
  * to the I2C block inside the DVC block
@@ -164,6 +244,10 @@ static void i2c_writel(struct tegra_i2c_dev *i2c_dev, u32 val,
 	unsigned long reg)
 {
 	writel(val, i2c_dev->base + tegra_i2c_reg_addr(i2c_dev, reg));
+
+	/* Read back register to make sure that register writes completed */
+	if (reg != I2C_TX_FIFO)
+		readl(i2c_dev->base + tegra_i2c_reg_addr(i2c_dev, reg));
 }
 
 static u32 i2c_readl(struct tegra_i2c_dev *i2c_dev, unsigned long reg)
@@ -260,9 +344,19 @@ static int tegra_i2c_fill_tx_fifo(struct tegra_i2c_dev *i2c_dev)
 {
 	u32 val;
 	int tx_fifo_avail;
-	u8 *buf = i2c_dev->msg_buf;
-	size_t buf_remaining = i2c_dev->msg_buf_remaining;
+	u8 *buf;
+	size_t buf_remaining;
 	int words_to_transfer;
+	unsigned long flags;
+
+	spin_lock_irqsave(&i2c_dev->fifo_lock, flags);
+	if (!i2c_dev->msg_buf_remaining) {
+		spin_unlock_irqrestore(&i2c_dev->fifo_lock, flags);
+		return 0;
+	}
+
+	buf = i2c_dev->msg_buf;
+	buf_remaining = i2c_dev->msg_buf_remaining;
 
 	val = i2c_readl(i2c_dev, I2C_FIFO_STATUS);
 	tx_fifo_avail = (val & I2C_FIFO_STATUS_TX_MASK) >>
@@ -301,7 +395,12 @@ static int tegra_i2c_fill_tx_fifo(struct tegra_i2c_dev *i2c_dev)
 	 * boundary and fault.
 	 */
 	if (tx_fifo_avail > 0 && buf_remaining > 0) {
-		BUG_ON(buf_remaining > 3);
+		if (buf_remaining > 3) {
+			dev_err(i2c_dev->dev,
+				"Remaining buffer more than 3 %d\n",
+				buf_remaining);
+			BUG();
+		}
 		memcpy(&val, buf, buf_remaining);
 
 		/* Again update before writing to FIFO to make sure isr sees. */
@@ -312,6 +411,8 @@ static int tegra_i2c_fill_tx_fifo(struct tegra_i2c_dev *i2c_dev)
 		i2c_writel(i2c_dev, val, I2C_TX_FIFO);
 	}
 
+	spin_unlock_irqrestore(&i2c_dev->fifo_lock, flags);
+
 	return 0;
 }
 
@@ -327,7 +428,6 @@ static void tegra_dvc_init(struct tegra_i2c_dev *i2c_dev)
 	u32 val = 0;
 	val = dvc_readl(i2c_dev, DVC_CTRL_REG3);
 	val |= DVC_CTRL_REG3_SW_PROG;
-	val |= DVC_CTRL_REG3_I2C_DONE_INTR_EN;
 	dvc_writel(i2c_dev, val, DVC_CTRL_REG3);
 
 	val = dvc_readl(i2c_dev, DVC_CTRL_REG1);
@@ -335,16 +435,54 @@ static void tegra_dvc_init(struct tegra_i2c_dev *i2c_dev)
 	dvc_writel(i2c_dev, val, DVC_CTRL_REG1);
 }
 
+static void tegra_i2c_slave_init(struct tegra_i2c_dev *i2c_dev)
+{
+	u32 val = I2C_SL_CNFG_NEWSL | I2C_SL_CNFG_NACK;
+
+	i2c_writel(i2c_dev, val, I2C_SL_CNFG);
+
+	if (i2c_dev->slave_addr) {
+		u16 addr = i2c_dev->slave_addr;
+
+		i2c_writel(i2c_dev, SL_ADDR1(addr), I2C_SL_ADDR1);
+		i2c_writel(i2c_dev, SL_ADDR2(addr), I2C_SL_ADDR2);
+	}
+}
+
+static inline int tegra_i2c_clock_enable(struct tegra_i2c_dev *i2c_dev)
+{
+	int ret;
+	ret = clk_enable(i2c_dev->fast_clk);
+	if (ret < 0) {
+		dev_err(i2c_dev->dev,
+			"Enabling fast clk failed, err %d\n", ret);
+		return ret;
+	}
+	ret = clk_enable(i2c_dev->div_clk);
+	if (ret < 0) {
+		dev_err(i2c_dev->dev,
+			"Enabling div clk failed, err %d\n", ret);
+		clk_disable(i2c_dev->fast_clk);
+	}
+	return ret;
+}
+
+static inline void tegra_i2c_clock_disable(struct tegra_i2c_dev *i2c_dev)
+{
+	clk_disable(i2c_dev->div_clk);
+	clk_disable(i2c_dev->fast_clk);
+}
+
 static int tegra_i2c_init(struct tegra_i2c_dev *i2c_dev)
 {
 	u32 val;
 	int err = 0;
 
-	clk_enable(i2c_dev->clk);
+	tegra_i2c_clock_enable(i2c_dev);
 
-	tegra_periph_reset_assert(i2c_dev->clk);
+	tegra_periph_reset_assert(i2c_dev->div_clk);
 	udelay(2);
-	tegra_periph_reset_deassert(i2c_dev->clk);
+	tegra_periph_reset_deassert(i2c_dev->div_clk);
 
 	if (i2c_dev->is_dvc)
 		tegra_dvc_init(i2c_dev);
@@ -353,7 +491,8 @@ static int tegra_i2c_init(struct tegra_i2c_dev *i2c_dev)
 		(0x2 << I2C_CNFG_DEBOUNCE_CNT_SHIFT);
 	i2c_writel(i2c_dev, val, I2C_CNFG);
 	i2c_writel(i2c_dev, 0, I2C_INT_MASK);
-	clk_set_rate(i2c_dev->clk, i2c_dev->bus_clk_rate * 8);
+	clk_set_rate(i2c_dev->div_clk, i2c_dev->last_bus_clk_rate * 8);
+	i2c_writel(i2c_dev, 0x3, I2C_CLK_DIVISOR);
 
 	if (!i2c_dev->is_dvc) {
 		u32 sl_cfg = i2c_readl(i2c_dev, I2C_SL_CNFG);
@@ -368,10 +507,13 @@ static int tegra_i2c_init(struct tegra_i2c_dev *i2c_dev)
 		0 << I2C_FIFO_CONTROL_RX_TRIG_SHIFT;
 	i2c_writel(i2c_dev, val, I2C_FIFO_CONTROL);
 
+	if (!i2c_dev->is_dvc)
+		tegra_i2c_slave_init(i2c_dev);
+
 	if (tegra_i2c_flush_fifos(i2c_dev))
 		err = -ETIMEDOUT;
 
-	clk_disable(i2c_dev->clk);
+	tegra_i2c_clock_disable(i2c_dev);
 
 	if (i2c_dev->irq_disabled) {
 		i2c_dev->irq_disabled = 0;
@@ -384,16 +526,14 @@ static int tegra_i2c_init(struct tegra_i2c_dev *i2c_dev)
 static irqreturn_t tegra_i2c_isr(int irq, void *dev_id)
 {
 	u32 status;
-	const u32 status_err = I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST;
+	const u32 status_err = I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST | I2C_INT_TX_FIFO_OVERFLOW;
 	struct tegra_i2c_dev *i2c_dev = dev_id;
 
 	status = i2c_readl(i2c_dev, I2C_INT_STATUS);
 
 	if (status == 0) {
-		dev_warn(i2c_dev->dev, "irq status 0 %08x %08x %08x\n",
-			 i2c_readl(i2c_dev, I2C_PACKET_TRANSFER_STATUS),
-			 i2c_readl(i2c_dev, I2C_STATUS),
-			 i2c_readl(i2c_dev, I2C_CNFG));
+		dev_warn(i2c_dev->dev, "unknown interrupt Add 0x%02x\n",
+						i2c_dev->msg_add);
 		i2c_dev->msg_err |= I2C_ERR_UNKNOWN_INTERRUPT;
 
 		if (!i2c_dev->irq_disabled) {
@@ -401,16 +541,49 @@ static irqreturn_t tegra_i2c_isr(int irq, void *dev_id)
 			i2c_dev->irq_disabled = 1;
 		}
 
-		complete(&i2c_dev->msg_complete);
 		goto err;
 	}
 
 	if (unlikely(status & status_err)) {
-		if (status & I2C_INT_NO_ACK)
+		dev_warn(i2c_dev->dev, "I2c error status 0x%08x\n", status);
+		if (status & I2C_INT_NO_ACK) {
 			i2c_dev->msg_err |= I2C_ERR_NO_ACK;
-		if (status & I2C_INT_ARBITRATION_LOST)
+			dev_warn(i2c_dev->dev, "no acknowledge from address"
+					" 0x%x\n", i2c_dev->msg_add);
+			dev_warn(i2c_dev->dev, "Packet status 0x%08x\n",
+				i2c_readl(i2c_dev, I2C_PACKET_TRANSFER_STATUS));
+		}
+
+		if (status & I2C_INT_ARBITRATION_LOST) {
 			i2c_dev->msg_err |= I2C_ERR_ARBITRATION_LOST;
-		complete(&i2c_dev->msg_complete);
+			dev_warn(i2c_dev->dev, "arbitration lost during "
+				" communicate to add 0x%x\n", i2c_dev->msg_add);
+			dev_warn(i2c_dev->dev, "Packet status 0x%08x\n",
+				i2c_readl(i2c_dev, I2C_PACKET_TRANSFER_STATUS));
+		}
+
+		if (status & I2C_INT_TX_FIFO_OVERFLOW) {
+			i2c_dev->msg_err |= I2C_INT_TX_FIFO_OVERFLOW;
+			dev_warn(i2c_dev->dev, "Tx fifo overflow during "
+				" communicate to add 0x%x\n", i2c_dev->msg_add);
+			dev_warn(i2c_dev->dev, "Packet status 0x%08x\n",
+				i2c_readl(i2c_dev, I2C_PACKET_TRANSFER_STATUS));
+		}
+		goto err;
+	}
+
+	if (unlikely((i2c_readl(i2c_dev, I2C_STATUS) & I2C_STATUS_BUSY)
+				&& (status == I2C_INT_TX_FIFO_DATA_REQ)
+				&& i2c_dev->msg_read
+				&& i2c_dev->msg_buf_remaining)) {
+		dev_warn(i2c_dev->dev, "unexpected status\n");
+		i2c_dev->msg_err |= I2C_ERR_UNEXPECTED_STATUS;
+
+		if (!i2c_dev->irq_disabled) {
+			disable_irq_nosync(i2c_dev->irq);
+			i2c_dev->irq_disabled = 1;
+		}
+
 		goto err;
 	}
 
@@ -428,70 +601,115 @@ static irqreturn_t tegra_i2c_isr(int irq, void *dev_id)
 			tegra_i2c_mask_irq(i2c_dev, I2C_INT_TX_FIFO_DATA_REQ);
 	}
 
+	i2c_writel(i2c_dev, status, I2C_INT_STATUS);
+
+	if (i2c_dev->is_dvc)
+		dvc_writel(i2c_dev, DVC_STATUS_I2C_DONE_INTR, DVC_STATUS);
+
 	if (status & I2C_INT_PACKET_XFER_COMPLETE) {
 		BUG_ON(i2c_dev->msg_buf_remaining);
 		complete(&i2c_dev->msg_complete);
 	}
 
-	i2c_writel(i2c_dev, status, I2C_INT_STATUS);
-	if (i2c_dev->is_dvc)
-		dvc_writel(i2c_dev, DVC_STATUS_I2C_DONE_INTR, DVC_STATUS);
 	return IRQ_HANDLED;
+
 err:
+	dev_dbg(i2c_dev->dev, "reg: 0x%08x 0x%08x 0x%08x 0x%08x\n",
+		 i2c_readl(i2c_dev, I2C_CNFG), i2c_readl(i2c_dev, I2C_STATUS),
+		 i2c_readl(i2c_dev, I2C_INT_STATUS),
+		 i2c_readl(i2c_dev, I2C_PACKET_TRANSFER_STATUS));
+
+	dev_dbg(i2c_dev->dev, "packet: 0x%08x %u 0x%08x\n",
+		 i2c_dev->packet_header, i2c_dev->payload_size,
+		 i2c_dev->io_header);
+
+	if (i2c_dev->msgs) {
+		struct i2c_msg *msgs = i2c_dev->msgs;
+		int i;
+
+		for (i = 0; i < i2c_dev->msgs_num; i++)
+			dev_dbg(i2c_dev->dev,
+				 "msgs[%d] %c, addr=0x%04x, len=%d\n",
+				 i, (msgs[i].flags & I2C_M_RD) ? 'R' : 'W',
+				 msgs[i].addr, msgs[i].len);
+	}
+
 	/* An error occurred, mask all interrupts */
 	tegra_i2c_mask_irq(i2c_dev, I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST |
 		I2C_INT_PACKET_XFER_COMPLETE | I2C_INT_TX_FIFO_DATA_REQ |
-		I2C_INT_RX_FIFO_DATA_REQ);
+		I2C_INT_RX_FIFO_DATA_REQ | I2C_INT_TX_FIFO_OVERFLOW);
+
 	i2c_writel(i2c_dev, status, I2C_INT_STATUS);
+
+	/* An error occured, mask dvc interrupt */
+	if (i2c_dev->is_dvc)
+		dvc_i2c_mask_irq(i2c_dev, DVC_CTRL_REG3_I2C_DONE_INTR_EN);
+
 	if (i2c_dev->is_dvc)
 		dvc_writel(i2c_dev, DVC_STATUS_I2C_DONE_INTR, DVC_STATUS);
+
+	complete(&i2c_dev->msg_complete);
 	return IRQ_HANDLED;
 }
 
-static int tegra_i2c_xfer_msg(struct tegra_i2c_dev *i2c_dev,
-	struct i2c_msg *msg, int stop)
+static int tegra_i2c_xfer_msg(struct tegra_i2c_bus *i2c_bus,
+	struct i2c_msg *msg, enum msg_end_type end_state)
 {
-	u32 packet_header;
+	struct tegra_i2c_dev *i2c_dev = i2c_bus->dev;
 	u32 int_mask;
 	int ret;
-
-	tegra_i2c_flush_fifos(i2c_dev);
-	i2c_writel(i2c_dev, 0xFF, I2C_INT_STATUS);
+	int arb_stat;
 
 	if (msg->len == 0)
 		return -EINVAL;
 
+	tegra_i2c_flush_fifos(i2c_dev);
+
 	i2c_dev->msg_buf = msg->buf;
 	i2c_dev->msg_buf_remaining = msg->len;
 	i2c_dev->msg_err = I2C_ERR_NONE;
 	i2c_dev->msg_read = (msg->flags & I2C_M_RD);
 	INIT_COMPLETION(i2c_dev->msg_complete);
+	i2c_dev->msg_add = msg->addr;
 
-	packet_header = (0 << PACKET_HEADER0_HEADER_SIZE_SHIFT) |
+	i2c_dev->packet_header = (0 << PACKET_HEADER0_HEADER_SIZE_SHIFT) |
 			PACKET_HEADER0_PROTOCOL_I2C |
 			(i2c_dev->cont_id << PACKET_HEADER0_CONT_ID_SHIFT) |
 			(1 << PACKET_HEADER0_PACKET_ID_SHIFT);
-	i2c_writel(i2c_dev, packet_header, I2C_TX_FIFO);
-
-	packet_header = msg->len - 1;
-	i2c_writel(i2c_dev, packet_header, I2C_TX_FIFO);
-
-	packet_header = msg->addr << I2C_HEADER_SLAVE_ADDR_SHIFT;
-	packet_header |= I2C_HEADER_IE_ENABLE;
-	if (!stop)
-		packet_header |= I2C_HEADER_REPEAT_START;
-	if (msg->flags & I2C_M_TEN)
-		packet_header |= I2C_HEADER_10BIT_ADDR;
+	i2c_writel(i2c_dev, i2c_dev->packet_header, I2C_TX_FIFO);
+
+	i2c_dev->payload_size = msg->len - 1;
+	i2c_writel(i2c_dev, i2c_dev->payload_size, I2C_TX_FIFO);
+
+	i2c_dev->io_header = I2C_HEADER_IE_ENABLE;
+	if (end_state == MSG_END_CONTINUE)
+		i2c_dev->io_header |= I2C_HEADER_CONTINUE_XFER;
+	else if (end_state == MSG_END_REPEAT_START)
+		i2c_dev->io_header |= I2C_HEADER_REPEAT_START;
+
+	if (msg->flags & I2C_M_TEN) {
+		i2c_dev->io_header |= msg->addr;
+		i2c_dev->io_header |= I2C_HEADER_10BIT_ADDR;
+	} else {
+		i2c_dev->io_header |= (msg->addr << I2C_HEADER_SLAVE_ADDR_SHIFT);
+	}
 	if (msg->flags & I2C_M_IGNORE_NAK)
-		packet_header |= I2C_HEADER_CONT_ON_NAK;
+		i2c_dev->io_header |= I2C_HEADER_CONT_ON_NAK;
 	if (msg->flags & I2C_M_RD)
-		packet_header |= I2C_HEADER_READ;
-	i2c_writel(i2c_dev, packet_header, I2C_TX_FIFO);
+		i2c_dev->io_header |= I2C_HEADER_READ;
+	if (i2c_dev->is_high_speed_enable) {
+		i2c_dev->io_header |= I2C_HEADER_HIGHSPEED_MODE;
+		i2c_dev->io_header |= ((i2c_dev->hs_master_code & 0x7) <<  I2C_HEADER_MASTER_ADDR_SHIFT);
+	}
+	i2c_writel(i2c_dev, i2c_dev->io_header, I2C_TX_FIFO);
 
 	if (!(msg->flags & I2C_M_RD))
 		tegra_i2c_fill_tx_fifo(i2c_dev);
 
-	int_mask = I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST;
+	if (i2c_dev->is_dvc)
+		dvc_i2c_unmask_irq(i2c_dev, DVC_CTRL_REG3_I2C_DONE_INTR_EN);
+
+	int_mask = I2C_INT_NO_ACK | I2C_INT_ARBITRATION_LOST | I2C_INT_TX_FIFO_OVERFLOW;
 	if (msg->flags & I2C_M_RD)
 		int_mask |= I2C_INT_RX_FIFO_DATA_REQ;
 	else if (i2c_dev->msg_buf_remaining)
@@ -500,11 +718,17 @@ static int tegra_i2c_xfer_msg(struct tegra_i2c_dev *i2c_dev,
 	dev_dbg(i2c_dev->dev, "unmasked irq: %02x\n",
 		i2c_readl(i2c_dev, I2C_INT_MASK));
 
-	ret = wait_for_completion_timeout(&i2c_dev->msg_complete, TEGRA_I2C_TIMEOUT);
+	ret = wait_for_completion_timeout(&i2c_dev->msg_complete,
+					TEGRA_I2C_TIMEOUT);
 	tegra_i2c_mask_irq(i2c_dev, int_mask);
 
+	if (i2c_dev->is_dvc)
+		dvc_i2c_mask_irq(i2c_dev, DVC_CTRL_REG3_I2C_DONE_INTR_EN);
+
 	if (WARN_ON(ret == 0)) {
-		dev_err(i2c_dev->dev, "i2c transfer timed out\n");
+		dev_err(i2c_dev->dev,
+			"i2c transfer timed out, addr 0x%04x, data 0x%02x\n",
+			msg->addr, msg->buf[0]);
 
 		tegra_i2c_init(i2c_dev);
 		return -ETIMEDOUT;
@@ -516,40 +740,111 @@ static int tegra_i2c_xfer_msg(struct tegra_i2c_dev *i2c_dev,
 	if (likely(i2c_dev->msg_err == I2C_ERR_NONE))
 		return 0;
 
+	/* Arbitration Lost occurs, Start recovery */
+	if (i2c_dev->msg_err == I2C_ERR_ARBITRATION_LOST) {
+		if (i2c_dev->arb_recovery) {
+			arb_stat = i2c_dev->arb_recovery(i2c_bus->scl_gpio, i2c_bus->sda_gpio);
+			if (!arb_stat)
+				return -EAGAIN;
+		}
+	}
+
+	/*
+	 * NACK interrupt is generated before the I2C controller generates the
+	 * STOP condition on the bus. So wait for 2 clock periods before resetting
+	 * the controller so that STOP condition has been delivered properly.
+	 */
+	if (i2c_dev->msg_err == I2C_ERR_NO_ACK)
+		udelay(DIV_ROUND_UP(2 * 1000000, i2c_dev->last_bus_clk_rate));
+
 	tegra_i2c_init(i2c_dev);
+
 	if (i2c_dev->msg_err == I2C_ERR_NO_ACK) {
 		if (msg->flags & I2C_M_IGNORE_NAK)
 			return 0;
 		return -EREMOTEIO;
 	}
 
+	if (i2c_dev->msg_err & I2C_ERR_UNEXPECTED_STATUS)
+		return -EAGAIN;
+
 	return -EIO;
 }
 
 static int tegra_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[],
 	int num)
 {
-	struct tegra_i2c_dev *i2c_dev = i2c_get_adapdata(adap);
+	struct tegra_i2c_bus *i2c_bus = i2c_get_adapdata(adap);
+	struct tegra_i2c_dev *i2c_dev = i2c_bus->dev;
 	int i;
 	int ret = 0;
 
-	if (i2c_dev->is_suspended)
+	rt_mutex_lock(&i2c_dev->dev_lock);
+
+	if (i2c_dev->is_suspended) {
+		rt_mutex_unlock(&i2c_dev->dev_lock);
 		return -EBUSY;
+	}
+
+	/* Support I2C_M_NOSTART only if HW support continue xfer. */
+	for (i = 0; i < num - 1; i++) {
+			if ((msgs[i + 1].flags & I2C_M_NOSTART) &&
+			!i2c_dev->hw->has_continue_xfer_support) {
+			dev_err(i2c_dev->dev,
+				"mesg %d have illegal flag\n", i + 1);
+			rt_mutex_unlock(&i2c_dev->dev_lock);
+			return -EINVAL;
+		}
+	}
+
+	if (i2c_dev->last_mux != i2c_bus->mux) {
+		tegra_pinmux_set_safe_pinmux_table(i2c_dev->last_mux,
+			i2c_dev->last_mux_len);
+		tegra_pinmux_config_pinmux_table(i2c_bus->mux,
+			i2c_bus->mux_len);
+		i2c_dev->last_mux = i2c_bus->mux;
+		i2c_dev->last_mux_len = i2c_bus->mux_len;
+	}
+
+	if (i2c_dev->last_bus_clk_rate != i2c_bus->bus_clk_rate) {
+		clk_set_rate(i2c_dev->div_clk, i2c_bus->bus_clk_rate * 8);
+		i2c_dev->last_bus_clk_rate = i2c_bus->bus_clk_rate;
+	}
+
+	i2c_dev->msgs = msgs;
+	i2c_dev->msgs_num = num;
+
+	pm_runtime_get_sync(&adap->dev);
+	tegra_i2c_clock_enable(i2c_dev);
 
-	clk_enable(i2c_dev->clk);
 	for (i = 0; i < num; i++) {
-		int stop = (i == (num - 1)) ? 1  : 0;
-		ret = tegra_i2c_xfer_msg(i2c_dev, &msgs[i], stop);
+		enum msg_end_type end_type = MSG_END_STOP;
+		if (i < (num - 1)) {
+			if (msgs[i + 1].flags & I2C_M_NOSTART)
+				end_type = MSG_END_CONTINUE;
+			else
+				end_type = MSG_END_REPEAT_START;
+		}
+		ret = tegra_i2c_xfer_msg(i2c_bus, &msgs[i], end_type);
 		if (ret)
 			break;
 	}
-	clk_disable(i2c_dev->clk);
+
+	tegra_i2c_clock_disable(i2c_dev);
+	pm_runtime_put(&adap->dev);
+
+	rt_mutex_unlock(&i2c_dev->dev_lock);
+
+	i2c_dev->msgs = NULL;
+	i2c_dev->msgs_num = 0;
+
 	return ret ?: i;
 }
 
 static u32 tegra_i2c_func(struct i2c_adapter *adap)
 {
-	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL | I2C_FUNC_10BIT_ADDR |
+			I2C_FUNC_PROTOCOL_MANGLING;
 }
 
 static const struct i2c_algorithm tegra_i2c_algo = {
@@ -557,208 +852,274 @@ static const struct i2c_algorithm tegra_i2c_algo = {
 	.functionality	= tegra_i2c_func,
 };
 
-static int tegra_i2c_probe(struct platform_device *pdev)
+static struct tegra_i2c_hw_feature tegra20_i2c_hw = {
+	.has_continue_xfer_support = false,
+};
+
+static struct tegra_i2c_hw_feature tegra30_i2c_hw = {
+	.has_continue_xfer_support = true,
+};
+
+#if defined(CONFIG_OF)
+/* Match table for of_platform binding */
+static const struct of_device_id tegra_i2c_of_match[] __devinitconst = {
+	{ .compatible = "nvidia,tegra20-i2c", .data = &tegra20_i2c_hw, },
+	{},
+};
+MODULE_DEVICE_TABLE(of, tegra_i2c_of_match);
+#endif
+
+static int __devinit tegra_i2c_probe(struct platform_device *pdev)
 {
 	struct tegra_i2c_dev *i2c_dev;
-	struct tegra_i2c_platform_data *pdata = pdev->dev.platform_data;
+	struct tegra_i2c_platform_data *plat = pdev->dev.platform_data;
 	struct resource *res;
-	struct resource *iomem;
-	struct clk *clk;
-	struct clk *i2c_clk;
+	struct clk *div_clk;
+	struct clk *fast_clk = NULL;
 	const unsigned int *prop;
-	void *base;
+	void __iomem *base;
 	int irq;
+	int nbus;
+	int i = 0;
 	int ret = 0;
 
+	if (!plat) {
+		dev_err(&pdev->dev, "no platform data?\n");
+		return -ENODEV;
+	}
+
+	if (plat->bus_count <= 0 || plat->adapter_nr < 0) {
+		dev_err(&pdev->dev, "invalid platform data?\n");
+		return -ENODEV;
+	}
+
+	WARN_ON(plat->bus_count > TEGRA_I2C_MAX_BUS);
+	nbus = min(TEGRA_I2C_MAX_BUS, plat->bus_count);
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_err(&pdev->dev, "no mem resource\n");
 		return -EINVAL;
 	}
-	iomem = request_mem_region(res->start, resource_size(res), pdev->name);
-	if (!iomem) {
-		dev_err(&pdev->dev, "I2C region already claimed\n");
-		return -EBUSY;
-	}
 
-	base = ioremap(iomem->start, resource_size(iomem));
+	base = devm_request_and_ioremap(&pdev->dev, res);
 	if (!base) {
-		dev_err(&pdev->dev, "Cannot ioremap I2C region\n");
-		return -ENOMEM;
+		dev_err(&pdev->dev, "Cannot request/ioremap I2C registers\n");
+		return -EADDRNOTAVAIL;
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (!res) {
 		dev_err(&pdev->dev, "no irq resource\n");
-		ret = -EINVAL;
-		goto err_iounmap;
+		return -EINVAL;
 	}
 	irq = res->start;
 
-	clk = clk_get(&pdev->dev, NULL);
-	if (IS_ERR(clk)) {
+	div_clk = devm_clk_get(&pdev->dev, "div-clk");
+	if (IS_ERR(div_clk)) {
 		dev_err(&pdev->dev, "missing controller clock");
-		ret = PTR_ERR(clk);
-		goto err_release_region;
+		return PTR_ERR(div_clk);
 	}
 
-	i2c_clk = clk_get(&pdev->dev, "i2c");
-	if (IS_ERR(i2c_clk)) {
-		dev_err(&pdev->dev, "missing bus clock");
-		ret = PTR_ERR(i2c_clk);
-		goto err_clk_put;
+	fast_clk = devm_clk_get(&pdev->dev, "fast-clk");
+	if (IS_ERR(fast_clk)) {
+		dev_err(&pdev->dev, "missing controller fast clock");
+		return PTR_ERR(fast_clk);
 	}
 
-	i2c_dev = kzalloc(sizeof(struct tegra_i2c_dev), GFP_KERNEL);
+	i2c_dev = devm_kzalloc(&pdev->dev, sizeof(struct tegra_i2c_dev) +
+			  (nbus-1) * sizeof(struct tegra_i2c_bus), GFP_KERNEL);
 	if (!i2c_dev) {
-		ret = -ENOMEM;
-		goto err_i2c_clk_put;
+		dev_err(&pdev->dev, "Could not allocate struct tegra_i2c_dev");
+		return -ENOMEM;
 	}
 
 	i2c_dev->base = base;
-	i2c_dev->clk = clk;
-	i2c_dev->i2c_clk = i2c_clk;
-	i2c_dev->iomem = iomem;
-	i2c_dev->adapter.algo = &tegra_i2c_algo;
+	i2c_dev->div_clk = div_clk;
+	i2c_dev->fast_clk = fast_clk;
 	i2c_dev->irq = irq;
 	i2c_dev->cont_id = pdev->id;
 	i2c_dev->dev = &pdev->dev;
+	i2c_dev->is_clkon_always = plat->is_clkon_always;
+
+
+#ifdef ARCH_TEGRA_2x_SOC
+	i2c_dev->hw = &tegra20_i2c_hw;
+#else
+	i2c_dev->hw = &tegra30_i2c_hw;
+#endif
 
-	i2c_dev->bus_clk_rate = 100000; /* default clock rate */
-	if (pdata) {
-		i2c_dev->bus_clk_rate = pdata->bus_clk_rate;
+	i2c_dev->last_bus_clk_rate = 100000; /* default clock rate */
+	if (plat) {
+		i2c_dev->last_bus_clk_rate = plat->bus_clk_rate[0];
 
 	} else if (i2c_dev->dev->of_node) {    /* if there is a device tree node ... */
+		/* TODO: DAN: this doesn't work for DT */
 		prop = of_get_property(i2c_dev->dev->of_node,
 				"clock-frequency", NULL);
 		if (prop)
-			i2c_dev->bus_clk_rate = be32_to_cpup(prop);
+			i2c_dev->last_bus_clk_rate = be32_to_cpup(prop);
+
+		/* FIXME! Populate the Tegra30 and then support M_NOSTART */
+		i2c_dev->hw = &tegra20_i2c_hw;
 	}
 
-	if (pdev->id == 3)
-		i2c_dev->is_dvc = 1;
+	i2c_dev->is_high_speed_enable = plat->is_high_speed_enable;
+	i2c_dev->last_bus_clk_rate = plat->bus_clk_rate[0] ?: 100000;
+	i2c_dev->msgs = NULL;
+	i2c_dev->msgs_num = 0;
+	rt_mutex_init(&i2c_dev->dev_lock);
+	spin_lock_init(&i2c_dev->fifo_lock);
+
+	i2c_dev->slave_addr = plat->slave_addr;
+	i2c_dev->hs_master_code = plat->hs_master_code;
+	i2c_dev->is_dvc = plat->is_dvc;
+	i2c_dev->arb_recovery = plat->arb_recovery;
 	init_completion(&i2c_dev->msg_complete);
 
 	platform_set_drvdata(pdev, i2c_dev);
 
+	if (i2c_dev->is_clkon_always)
+		tegra_i2c_clock_enable(i2c_dev);
+
 	ret = tegra_i2c_init(i2c_dev);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to initialize i2c controller");
-		goto err_free;
+		return ret;
 	}
 
-	ret = request_irq(i2c_dev->irq, tegra_i2c_isr, 0, pdev->name, i2c_dev);
+	ret = devm_request_irq(&pdev->dev, i2c_dev->irq,
+			tegra_i2c_isr, 0, pdev->name, i2c_dev);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to request irq %i\n", i2c_dev->irq);
-		goto err_free;
+		return ret;
 	}
 
-	clk_enable(i2c_dev->i2c_clk);
+	pm_runtime_enable(&pdev->dev);
 
-	i2c_set_adapdata(&i2c_dev->adapter, i2c_dev);
-	i2c_dev->adapter.owner = THIS_MODULE;
-	i2c_dev->adapter.class = I2C_CLASS_HWMON;
-	strlcpy(i2c_dev->adapter.name, "Tegra I2C adapter",
-		sizeof(i2c_dev->adapter.name));
-	i2c_dev->adapter.algo = &tegra_i2c_algo;
-	i2c_dev->adapter.dev.parent = &pdev->dev;
-	i2c_dev->adapter.nr = pdev->id;
-	i2c_dev->adapter.dev.of_node = pdev->dev.of_node;
+	for (i = 0; i < nbus; i++) {
+		struct tegra_i2c_bus *i2c_bus = &i2c_dev->busses[i];
 
-	ret = i2c_add_numbered_adapter(&i2c_dev->adapter);
-	if (ret) {
-		dev_err(&pdev->dev, "Failed to add I2C adapter\n");
-		goto err_free_irq;
+		i2c_bus->dev = i2c_dev;
+		i2c_bus->mux = plat->bus_mux[i];
+		i2c_bus->mux_len = plat->bus_mux_len[i];
+		i2c_bus->bus_clk_rate = plat->bus_clk_rate[i] ?: 100000;
+
+		i2c_bus->scl_gpio = plat->scl_gpio[i];
+		i2c_bus->sda_gpio = plat->sda_gpio[i];
+
+		i2c_bus->adapter.dev.of_node = pdev->dev.of_node;
+		i2c_bus->adapter.algo = &tegra_i2c_algo;
+		i2c_set_adapdata(&i2c_bus->adapter, i2c_bus);
+		i2c_bus->adapter.owner = THIS_MODULE;
+		i2c_bus->adapter.class = I2C_CLASS_HWMON;
+		strlcpy(i2c_bus->adapter.name, "Tegra I2C adapter",
+			sizeof(i2c_bus->adapter.name));
+		i2c_bus->adapter.dev.parent = &pdev->dev;
+		i2c_bus->adapter.nr = plat->adapter_nr + i;
+
+		if (plat->retries)
+			i2c_bus->adapter.retries = plat->retries;
+		else
+			i2c_bus->adapter.retries = TEGRA_I2C_RETRIES;
+
+		if (plat->timeout)
+			i2c_bus->adapter.timeout = plat->timeout;
+
+		ret = i2c_add_numbered_adapter(&i2c_bus->adapter);
+		if (ret) {
+			dev_err(&pdev->dev, "Failed to add I2C adapter\n");
+			goto err_del_bus;
+		}
+		of_i2c_register_devices(&i2c_bus->adapter);
+		pm_runtime_enable(&i2c_bus->adapter.dev);
+
+		i2c_dev->bus_count++;
 	}
 
-	of_i2c_register_devices(&i2c_dev->adapter);
 
 	return 0;
-err_free_irq:
-	free_irq(i2c_dev->irq, i2c_dev);
-err_free:
-	kfree(i2c_dev);
-err_i2c_clk_put:
-	clk_put(i2c_clk);
-err_clk_put:
-	clk_put(clk);
-err_release_region:
-	release_mem_region(iomem->start, resource_size(iomem));
-err_iounmap:
-	iounmap(base);
+
+err_del_bus:
+	while (i2c_dev->bus_count--)
+		i2c_del_adapter(&i2c_dev->busses[i2c_dev->bus_count].adapter);
 	return ret;
 }
 
-static int tegra_i2c_remove(struct platform_device *pdev)
+static int __devexit tegra_i2c_remove(struct platform_device *pdev)
 {
 	struct tegra_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
-	i2c_del_adapter(&i2c_dev->adapter);
-	free_irq(i2c_dev->irq, i2c_dev);
-	clk_put(i2c_dev->i2c_clk);
-	clk_put(i2c_dev->clk);
-	release_mem_region(i2c_dev->iomem->start,
-		resource_size(i2c_dev->iomem));
-	iounmap(i2c_dev->base);
-	kfree(i2c_dev);
+
+	while (i2c_dev->bus_count--) {
+		i2c_del_adapter(&i2c_dev->busses[i2c_dev->bus_count].adapter);
+		pm_runtime_disable(&i2c_dev->busses[i2c_dev->bus_count].adapter.dev);
+	}
+
+	if (i2c_dev->is_clkon_always)
+		tegra_i2c_clock_disable(i2c_dev);
+	pm_runtime_disable(&pdev->dev);
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int tegra_i2c_suspend(struct platform_device *pdev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int tegra_i2c_suspend_noirq(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	struct tegra_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
 
-	i2c_lock_adapter(&i2c_dev->adapter);
+	rt_mutex_lock(&i2c_dev->dev_lock);
+
 	i2c_dev->is_suspended = true;
-	i2c_unlock_adapter(&i2c_dev->adapter);
+	if (i2c_dev->is_clkon_always)
+		tegra_i2c_clock_disable(i2c_dev);
+
+	rt_mutex_unlock(&i2c_dev->dev_lock);
 
 	return 0;
 }
 
-static int tegra_i2c_resume(struct platform_device *pdev)
+static int tegra_i2c_resume_noirq(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	struct tegra_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
 	int ret;
 
-	i2c_lock_adapter(&i2c_dev->adapter);
+	rt_mutex_lock(&i2c_dev->dev_lock);
+
+	if (i2c_dev->is_clkon_always)
+		tegra_i2c_clock_enable(i2c_dev);
 
 	ret = tegra_i2c_init(i2c_dev);
 
 	if (ret) {
-		i2c_unlock_adapter(&i2c_dev->adapter);
+		rt_mutex_unlock(&i2c_dev->dev_lock);
 		return ret;
 	}
 
 	i2c_dev->is_suspended = false;
 
-	i2c_unlock_adapter(&i2c_dev->adapter);
+	rt_mutex_unlock(&i2c_dev->dev_lock);
 
 	return 0;
 }
-#endif
 
-#if defined(CONFIG_OF)
-/* Match table for of_platform binding */
-static const struct of_device_id tegra_i2c_of_match[] __devinitconst = {
-	{ .compatible = "nvidia,tegra20-i2c", },
-	{},
+static const struct dev_pm_ops tegra_i2c_pm = {
+	.suspend_noirq = tegra_i2c_suspend_noirq,
+	.resume_noirq = tegra_i2c_resume_noirq,
 };
-MODULE_DEVICE_TABLE(of, tegra_i2c_of_match);
+#define TEGRA_I2C_PM	(&tegra_i2c_pm)
 #else
-#define tegra_i2c_of_match NULL
+#define TEGRA_I2C_PM	NULL
 #endif
 
 static struct platform_driver tegra_i2c_driver = {
 	.probe   = tegra_i2c_probe,
-	.remove  = tegra_i2c_remove,
-#ifdef CONFIG_PM
-	.suspend = tegra_i2c_suspend,
-	.resume  = tegra_i2c_resume,
-#endif
+	.remove  = __devexit_p(tegra_i2c_remove),
 	.driver  = {
 		.name  = "tegra-i2c",
 		.owner = THIS_MODULE,
-		.of_match_table = tegra_i2c_of_match,
+		.of_match_table = of_match_ptr(tegra_i2c_of_match),
+		.pm    = TEGRA_I2C_PM,
 	},
 };
 
diff --git a/drivers/i2c/i2c-slave.c b/drivers/i2c/i2c-slave.c
new file mode 100755
index 000000000000..280a860cd2e8
--- /dev/null
+++ b/drivers/i2c/i2c-slave.c
@@ -0,0 +1,281 @@
+/*
+ * i2c-slave.c - a device driver for the iic-slave bus interface.
+ *
+ * Copyright (c) 2009-2011, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA	02110-1301, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/i2c-slave.h>
+struct i2c_slave_priv {
+	struct i2c_adapter master_adap;
+	struct i2c_slave_adapter *slave_adap;
+	struct i2c_algorithm master_algo;
+};
+
+/**
+ * i2c_slave_send - Sends data to master. When master issues a read cycle, the
+ * data is sent by the slave.
+ * This function copies the client data into the slave tx buffer and return to
+ * client. This is not a blocking call. Data will be sent to master later once
+ * slave got the master-ready cycle transfer.
+ * if there is no sufficient space to write the client buffer, it will return
+ * error. it will not write partial data.
+ * @client: Handle to i2c-slave client.
+ * @buf: Data that will be written to the master
+ * @count: How many bytes to write.
+ *
+ * Returns negative errno, or else the number of bytes written.
+ */
+int i2c_slave_send(struct i2c_client *client, const char *buf, int count)
+{
+	struct i2c_adapter *adap = client->adapter;
+	struct i2c_slave_priv *priv = adap->algo_data;
+
+	if (!(adap->algo->functionality(adap) & I2C_FUNC_I2C_SLAVE_SUPPORT))
+		BUG();
+
+	if (priv->slave_adap->slv_algo->slave_send)
+		return priv->slave_adap->slv_algo->slave_send(priv->slave_adap,
+			 buf, count);
+	return -ENODEV;
+}
+EXPORT_SYMBOL(i2c_slave_send);
+
+/**
+ * i2c_slave_get_tx_status - Get amount of data available in tx buffer. If there
+ * is still data in tx buffer then wait for given time to transfer complete
+ * for a give timeout.
+ * @client: Handle to i2c-slave client.
+ * @timeout_ms: Time to wait for transfer to complete.
+ *
+ * Returns negative errno, or else the number of bytes remaining in tx buffer.
+ */
+int i2c_slave_get_tx_status(struct i2c_client *client, int timeout_ms)
+{
+	struct i2c_adapter *adap = client->adapter;
+	struct i2c_slave_priv *priv = adap->algo_data;
+
+	if (!(adap->algo->functionality(adap) & I2C_FUNC_I2C_SLAVE_SUPPORT))
+		BUG();
+
+	if (priv->slave_adap->slv_algo->slave_get_tx_status)
+		return priv->slave_adap->slv_algo->slave_get_tx_status(
+			priv->slave_adap, timeout_ms);
+	return -ENODEV;
+}
+EXPORT_SYMBOL(i2c_slave_get_tx_status);
+
+/**
+ * i2c_slave_recv - Receive data from master. The data receive from master is
+ * stored on slave rx buffer. When this api will be called, the data will be
+ * copied from the slave rx buffer to client buffer. If requested amount (count)
+ * of data is not available then it will wait for either min_count to be receive
+ * or timeout whatever first.
+ *
+ * if timeout_ms = 0, then wait for min_count data to be read.
+ * if timoue_ms non zero then wait for the data till timeout happen.
+ * @client: Handle to i2c-slave client.
+ * @buf: Data that will be read from the master
+ * @count: How many bytes to read.
+ * @min_count: Block till read min_count of data.
+ * @timeout_ms: Time to wait for read to be complete.
+ *
+ * Returns negative errno, or else the number of bytes read.
+ */
+int i2c_slave_recv(struct i2c_client *client, char *buf, int count,
+			int min_count, int timeout_ms)
+{
+	struct i2c_adapter *adap = client->adapter;
+	struct i2c_slave_priv *priv = adap->algo_data;
+
+	if (!(adap->algo->functionality(adap) & I2C_FUNC_I2C_SLAVE_SUPPORT))
+		BUG();
+
+	if (priv->slave_adap->slv_algo->slave_recv)
+		return priv->slave_adap->slv_algo->slave_recv(priv->slave_adap,
+			 buf, count, min_count,	timeout_ms);
+
+	return -ENODEV;
+}
+EXPORT_SYMBOL(i2c_slave_recv);
+
+/**
+ * i2c_slave_start - Start the i2c slave to receive/transmit data.
+ * After this i2c controller starts responding master.
+ * The dummy-char will send to master if there is no data to send on slave tx
+ * buffer.
+ * @client: Handle to i2c-slave client.
+ * @dummy_char: Data which will be send to master if there is no data to be send
+ * in slave tx buffer.
+ *
+ * Returns negative errno, or else 0 for success.
+ */
+int i2c_slave_start(struct i2c_client *client, unsigned char dummy_char)
+{
+	struct i2c_adapter *adap = client->adapter;
+	struct i2c_slave_priv *priv = adap->algo_data;
+	int slave_add;
+	int is_10bit_addr;
+
+	if (!(adap->algo->functionality(adap) & I2C_FUNC_I2C_SLAVE_SUPPORT))
+		BUG();
+	slave_add = client->addr;
+	is_10bit_addr = (client->flags & I2C_CLIENT_TEN) ? 1 : 0;
+	if (priv->slave_adap->slv_algo->slave_start)
+		return priv->slave_adap->slv_algo->slave_start(priv->slave_adap,
+	slave_add, is_10bit_addr, dummy_char);
+	return -ENODEV;
+}
+EXPORT_SYMBOL(i2c_slave_start);
+
+/**
+ * i2c_slave_stop - Stop slave to receive/transmit data.
+ * After this i2c controller stops responding master.
+ * @client: Handle to i2c-slave client.
+ * @is_buffer_clear: Reset the tx and rx slave buffer or not.
+ */
+void i2c_slave_stop(struct i2c_client *client, int is_buffer_clear)
+{
+	struct i2c_adapter *adap = client->adapter;
+	struct i2c_slave_priv *priv = adap->algo_data;
+
+	if (!(adap->algo->functionality(adap) & I2C_FUNC_I2C_SLAVE_SUPPORT))
+		BUG();
+
+	if (priv->slave_adap->slv_algo->slave_stop)
+		return priv->slave_adap->slv_algo->slave_stop(priv->slave_adap,
+				 is_buffer_clear);
+}
+EXPORT_SYMBOL(i2c_slave_stop);
+
+/**
+ * i2c_slave_flush_buffer - Flush the receive and transmit buffer.
+ * @client: Handle to i2c-slave client.
+ * @is_flush_tx_buffer: Reset the tx slave buffer or not.
+ * @is_flush_rx_buffer: Reset the rx slave buffer or not.
+ *
+ * Returns negative errno, or else 0 for success.
+ */
+int i2c_slave_flush_buffer(struct i2c_client *client,
+			int is_flush_tx_buffer, int is_flush_rx_buffer)
+{
+	struct i2c_adapter *adap = client->adapter;
+	struct i2c_slave_priv *priv = adap->algo_data;
+
+	if (!(adap->algo->functionality(adap) & I2C_FUNC_I2C_SLAVE_SUPPORT))
+		BUG();
+
+	if (priv->slave_adap->slv_algo->slave_flush_buffer)
+		return priv->slave_adap->slv_algo->slave_flush_buffer(
+			priv->slave_adap, is_flush_tx_buffer,
+			is_flush_rx_buffer);
+
+	return -ENODEV;
+}
+EXPORT_SYMBOL(i2c_slave_flush_buffer);
+
+/**
+ * i2c_slave_get_nack_cycle - Get the number of master read cycle on which
+ * dummy char sent. This is the way to find that how much cycle slave sent the
+ * NACK packet.
+ *
+ * @client: Handle to i2c-slave client.
+ * @is_cout_reset: Reset the nack count or not.
+ *
+ * Returns negative errno, or else 0 for success.
+ */
+int i2c_slave_get_nack_cycle(struct i2c_client *client,
+			int is_cout_reset)
+{
+	struct i2c_adapter *adap = client->adapter;
+	struct i2c_slave_priv *priv = adap->algo_data;
+
+	if (!(adap->algo->functionality(adap) & I2C_FUNC_I2C_SLAVE_SUPPORT))
+		BUG();
+
+	if (priv->slave_adap->slv_algo->slave_get_nack_cycle)
+		return priv->slave_adap->slv_algo->slave_get_nack_cycle(
+			priv->slave_adap, is_cout_reset);
+
+	return -ENODEV;
+}
+EXPORT_SYMBOL(i2c_slave_get_nack_cycle);
+
+static u32 i2c_slave_func(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C_SLAVE_SUPPORT;
+}
+
+int i2c_add_slave_adapter(struct i2c_slave_adapter *slv_adap, bool force_nr)
+{
+	struct i2c_slave_priv *priv;
+	int ret;
+
+	priv = kzalloc(sizeof(struct i2c_slave_priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	/* Set up private adapter data */
+	priv->slave_adap = slv_adap;
+	slv_adap->parent_data = priv;
+
+	priv->master_algo.functionality = i2c_slave_func;
+
+	/* Now fill out new adapter structure */
+	snprintf(priv->master_adap.name, sizeof(priv->master_adap.name),
+		"i2c-%d-slave", slv_adap->nr);
+	priv->master_adap.owner = THIS_MODULE;
+	priv->master_adap.class = slv_adap->class;
+	priv->master_adap.algo = &priv->master_algo;
+	priv->master_adap.algo_data = priv;
+	priv->master_adap.dev.parent = slv_adap->parent_dev;
+
+	if (force_nr) {
+		priv->master_adap.nr = slv_adap->nr;
+		ret = i2c_add_numbered_adapter(&priv->master_adap);
+	} else {
+		ret = i2c_add_adapter(&priv->master_adap);
+	}
+	if (ret < 0) {
+		dev_err(slv_adap->parent_dev,
+			"failed to add slave-adapter (error=%d)\n", ret);
+		kfree(priv);
+		return ret;
+	}
+	slv_adap->dev = &priv->master_adap.dev;
+	dev_info(slv_adap->parent_dev, "Added slave i2c bus %d\n",
+		 i2c_adapter_id(&priv->master_adap));
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(i2c_add_slave_adapter);
+
+int i2c_del_slave_adapter(struct i2c_slave_adapter *slv_adap)
+{
+	struct i2c_slave_priv *priv = slv_adap->parent_data;
+	int ret;
+
+	ret = i2c_del_adapter(&priv->master_adap);
+	if (ret < 0)
+		return ret;
+	kfree(priv);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(i2c_del_slave_adapter);
diff --git a/drivers/i2c/muxes/pca954x.c b/drivers/i2c/muxes/pca954x.c
index 6f8953664636..dd14ae38d3ee 100644
--- a/drivers/i2c/muxes/pca954x.c
+++ b/drivers/i2c/muxes/pca954x.c
@@ -41,6 +41,9 @@
 #include <linux/device.h>
 #include <linux/i2c.h>
 #include <linux/i2c-mux.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/delay.h>
 
 #include <linux/i2c/pca954x.h>
 
@@ -62,6 +65,8 @@ struct pca954x {
 	struct i2c_adapter *virt_adaps[PCA954X_MAX_NCHANS];
 
 	u8 last_chan;		/* last register value */
+	struct regulator *vcc_reg;
+	struct regulator *i2c_reg;
 };
 
 struct chip_desc {
@@ -123,6 +128,26 @@ static int pca954x_reg_write(struct i2c_adapter *adap,
 			     struct i2c_client *client, u8 val)
 {
 	int ret = -ENODEV;
+	struct pca954x *data = i2c_get_clientdata(client);
+
+	/* Increase ref count for pca954x vcc */
+	if (data->vcc_reg) {
+		ret = regulator_enable(data->vcc_reg);
+		if (ret) {
+			dev_err(&client->dev, "%s: failed to enable vcc\n",
+				__func__);
+			goto vcc_regulator_failed;
+		}
+	}
+	/* Increase ref count for pca954x vcc_i2c */
+	if (data->i2c_reg) {
+		ret = regulator_enable(data->i2c_reg);
+		if (ret) {
+			dev_err(&client->dev, "%s: failed to enable vcc_i2c\n",
+				__func__);
+			goto i2c_regulator_failed;
+		}
+	}
 
 	if (adap->algo->master_xfer) {
 		struct i2c_msg msg;
@@ -142,6 +167,15 @@ static int pca954x_reg_write(struct i2c_adapter *adap,
 					     val, I2C_SMBUS_BYTE, &data);
 	}
 
+	/* Decrease ref count for pca954x vcc_i2c */
+	if (data->i2c_reg)
+		regulator_disable(data->i2c_reg);
+
+i2c_regulator_failed:
+	/* Decrease ref count for pca954x vcc */
+	if (data->vcc_reg)
+		regulator_disable(data->vcc_reg);
+vcc_regulator_failed:
 	return ret;
 }
 
@@ -201,15 +235,71 @@ static int pca954x_probe(struct i2c_client *client,
 
 	i2c_set_clientdata(client, data);
 
+	/* Get regulator pointer for pca954x vcc */
+	data->vcc_reg = regulator_get(&client->dev, "vcc");
+	if (PTR_ERR(data->vcc_reg) == -ENODEV)
+		data->vcc_reg = NULL;
+	else if (IS_ERR(data->vcc_reg)) {
+		dev_err(&client->dev, "%s: failed to get vcc\n",
+			__func__);
+		ret = PTR_ERR(data->vcc_reg);
+		goto exit_free;
+	}
+	/* Get regulator pointer for pca954x vcc_i2c */
+	data->i2c_reg = regulator_get(&client->dev, "vcc_i2c");
+	if (PTR_ERR(data->i2c_reg) == -ENODEV)
+		data->i2c_reg = NULL;
+	else if (IS_ERR(data->i2c_reg)) {
+		dev_err(&client->dev, "%s: failed to get vcc_i2c\n",
+			__func__);
+		ret = PTR_ERR(data->i2c_reg);
+		regulator_put(data->vcc_reg);
+		goto exit_free;
+	}
+
+	/* Increase ref count for pca954x vcc */
+	if (data->vcc_reg) {
+		pr_info("%s: enable vcc\n", __func__);
+		ret = regulator_enable(data->vcc_reg);
+		if (ret) {
+			dev_err(&client->dev, "%s: failed to enable vcc\n",
+				__func__);
+			goto exit_regulator_put;
+		}
+	}
+	/* Increase ref count for pca954x vcc_i2c */
+	if (data->i2c_reg) {
+		pr_info("%s: enable vcc_i2c\n", __func__);
+		ret = regulator_enable(data->i2c_reg);
+		if (ret) {
+			dev_err(&client->dev, "%s: failed to enable vcc_i2c\n",
+				__func__);
+			goto exit_vcc_regulator_disable;
+		}
+	}
+
+	/*
+	 * Power-On Reset takes time.
+	 * I2C is ready after Power-On Reset.
+	 */
+	msleep(1);
+
 	/* Write the mux register at addr to verify
 	 * that the mux is in fact present. This also
 	 * initializes the mux to disconnected state.
 	 */
 	if (i2c_smbus_write_byte(client, 0) < 0) {
 		dev_warn(&client->dev, "probe failed\n");
-		goto exit_free;
+		goto exit_regulator_disable;
 	}
 
+	/* Decrease ref count for pca954x vcc */
+	if (data->vcc_reg)
+		regulator_disable(data->vcc_reg);
+	/* Decrease ref count for pca954x vcc_i2c */
+	if (data->i2c_reg)
+		regulator_disable(data->i2c_reg);
+
 	data->type = id->driver_data;
 	data->last_chan = 0;		   /* force the first selection */
 
@@ -250,6 +340,15 @@ static int pca954x_probe(struct i2c_client *client,
 virt_reg_failed:
 	for (num--; num >= 0; num--)
 		i2c_del_mux_adapter(data->virt_adaps[num]);
+exit_regulator_disable:
+	if (data->i2c_reg)
+		regulator_disable(data->i2c_reg);
+exit_vcc_regulator_disable:
+	if (data->vcc_reg)
+		regulator_disable(data->vcc_reg);
+exit_regulator_put:
+	regulator_put(data->i2c_reg);
+	regulator_put(data->vcc_reg);
 exit_free:
 	kfree(data);
 err:
@@ -270,6 +369,9 @@ static int pca954x_remove(struct i2c_client *client)
 			data->virt_adaps[i] = NULL;
 		}
 
+	regulator_put(data->i2c_reg);
+	regulator_put(data->vcc_reg);
+
 	kfree(data);
 	return 0;
 }
diff --git a/drivers/infiniband/core/addr.c b/drivers/infiniband/core/addr.c
index 236ad9a89c0a..f2a84c6f8543 100644
--- a/drivers/infiniband/core/addr.c
+++ b/drivers/infiniband/core/addr.c
@@ -215,7 +215,9 @@ static int addr4_resolve(struct sockaddr_in *src_in,
 
 	neigh = neigh_lookup(&arp_tbl, &rt->rt_gateway, rt->dst.dev);
 	if (!neigh || !(neigh->nud_state & NUD_VALID)) {
+		rcu_read_lock();
 		neigh_event_send(dst_get_neighbour(&rt->dst), NULL);
+		rcu_read_unlock();
 		ret = -ENODATA;
 		if (neigh)
 			goto release;
@@ -273,15 +275,16 @@ static int addr6_resolve(struct sockaddr_in6 *src_in,
 		goto put;
 	}
 
+	rcu_read_lock();
 	neigh = dst_get_neighbour(dst);
 	if (!neigh || !(neigh->nud_state & NUD_VALID)) {
 		if (neigh)
 			neigh_event_send(neigh, NULL);
 		ret = -ENODATA;
-		goto put;
+	} else {
+		ret = rdma_copy_addr(addr, dst->dev, neigh->ha);
 	}
-
-	ret = rdma_copy_addr(addr, dst->dev, neigh->ha);
+	rcu_read_unlock();
 put:
 	dst_release(dst);
 	return ret;
diff --git a/drivers/infiniband/hw/cxgb3/iwch_cm.c b/drivers/infiniband/hw/cxgb3/iwch_cm.c
index 6cd642aaa4de..e55ce7a428be 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_cm.c
+++ b/drivers/infiniband/hw/cxgb3/iwch_cm.c
@@ -1365,8 +1365,10 @@ static int pass_accept_req(struct t3cdev *tdev, struct sk_buff *skb, void *ctx)
 		goto reject;
 	}
 	dst = &rt->dst;
+	rcu_read_lock();
 	neigh = dst_get_neighbour(dst);
 	l2t = t3_l2t_get(tdev, neigh, neigh->dev);
+	rcu_read_unlock();
 	if (!l2t) {
 		printk(KERN_ERR MOD "%s - failed to allocate l2t entry!\n",
 		       __func__);
@@ -1936,10 +1938,12 @@ int iwch_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 	}
 	ep->dst = &rt->dst;
 
+	rcu_read_lock();
 	neigh = dst_get_neighbour(ep->dst);
 
 	/* get a l2t entry */
 	ep->l2t = t3_l2t_get(ep->com.tdev, neigh, neigh->dev);
+	rcu_read_unlock();
 	if (!ep->l2t) {
 		printk(KERN_ERR MOD "%s - cannot alloc l2e.\n", __func__);
 		err = -ENOMEM;
diff --git a/drivers/infiniband/hw/cxgb4/cm.c b/drivers/infiniband/hw/cxgb4/cm.c
index 77f769d9227d..daa93e942e16 100644
--- a/drivers/infiniband/hw/cxgb4/cm.c
+++ b/drivers/infiniband/hw/cxgb4/cm.c
@@ -1358,6 +1358,7 @@ static int pass_accept_req(struct c4iw_dev *dev, struct sk_buff *skb)
 		goto reject;
 	}
 	dst = &rt->dst;
+	rcu_read_lock();
 	neigh = dst_get_neighbour(dst);
 	if (neigh->dev->flags & IFF_LOOPBACK) {
 		pdev = ip_dev_find(&init_net, peer_ip);
@@ -1384,6 +1385,7 @@ static int pass_accept_req(struct c4iw_dev *dev, struct sk_buff *skb)
 		rss_qid = dev->rdev.lldi.rxq_ids[
 			  cxgb4_port_idx(neigh->dev) * step];
 	}
+	rcu_read_unlock();
 	if (!l2t) {
 		printk(KERN_ERR MOD "%s - failed to allocate l2t entry!\n",
 		       __func__);
@@ -1909,6 +1911,7 @@ int c4iw_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 	}
 	ep->dst = &rt->dst;
 
+	rcu_read_lock();
 	neigh = dst_get_neighbour(ep->dst);
 
 	/* get a l2t entry */
@@ -1945,6 +1948,7 @@ int c4iw_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 		ep->rss_qid = ep->com.dev->rdev.lldi.rxq_ids[
 			      cxgb4_port_idx(neigh->dev) * step];
 	}
+	rcu_read_unlock();
 	if (!ep->l2t) {
 		printk(KERN_ERR MOD "%s - cannot alloc l2e.\n", __func__);
 		err = -ENOMEM;
diff --git a/drivers/infiniband/hw/mlx4/main.c b/drivers/infiniband/hw/mlx4/main.c
index fa643f4f4e28..059a865b2b7c 100644
--- a/drivers/infiniband/hw/mlx4/main.c
+++ b/drivers/infiniband/hw/mlx4/main.c
@@ -1144,7 +1144,8 @@ err_reg:
 
 err_counter:
 	for (; i; --i)
-		mlx4_counter_free(ibdev->dev, ibdev->counters[i - 1]);
+		if (ibdev->counters[i - 1] != -1)
+			mlx4_counter_free(ibdev->dev, ibdev->counters[i - 1]);
 
 err_map:
 	iounmap(ibdev->uar_map);
@@ -1175,7 +1176,8 @@ static void mlx4_ib_remove(struct mlx4_dev *dev, void *ibdev_ptr)
 	}
 	iounmap(ibdev->uar_map);
 	for (p = 0; p < ibdev->num_ports; ++p)
-		mlx4_counter_free(ibdev->dev, ibdev->counters[p]);
+		if (ibdev->counters[p] != -1)
+			mlx4_counter_free(ibdev->dev, ibdev->counters[p]);
 	mlx4_foreach_port(p, dev, MLX4_PORT_TYPE_IB)
 		mlx4_CLOSE_PORT(dev, p);
 
diff --git a/drivers/infiniband/hw/mlx4/qp.c b/drivers/infiniband/hw/mlx4/qp.c
index 3a91d9d8dc51..5c22514b8718 100644
--- a/drivers/infiniband/hw/mlx4/qp.c
+++ b/drivers/infiniband/hw/mlx4/qp.c
@@ -1309,7 +1309,7 @@ static int build_mlx_header(struct mlx4_ib_sqp *sqp, struct ib_send_wr *wr,
 	int is_eth;
 	int is_vlan = 0;
 	int is_grh;
-	u16 vlan;
+	u16 vlan = 0;
 
 	send_size = 0;
 	for (i = 0; i < wr->num_sge; ++i)
diff --git a/drivers/infiniband/hw/nes/nes_cm.c b/drivers/infiniband/hw/nes/nes_cm.c
index c118663e4437..a237547330ba 100644
--- a/drivers/infiniband/hw/nes/nes_cm.c
+++ b/drivers/infiniband/hw/nes/nes_cm.c
@@ -1150,9 +1150,11 @@ static int nes_addr_resolve_neigh(struct nes_vnic *nesvnic, u32 dst_ip, int arpi
 		neigh_release(neigh);
 	}
 
-	if ((neigh == NULL) || (!(neigh->nud_state & NUD_VALID)))
+	if ((neigh == NULL) || (!(neigh->nud_state & NUD_VALID))) {
+		rcu_read_lock();
 		neigh_event_send(dst_get_neighbour(&rt->dst), NULL);
-
+		rcu_read_unlock();
+	}
 	ip_rt_put(rt);
 	return rc;
 }
diff --git a/drivers/infiniband/hw/qib/qib_iba6120.c b/drivers/infiniband/hw/qib/qib_iba6120.c
index d8ca0a0b970d..65df26ce538a 100644
--- a/drivers/infiniband/hw/qib/qib_iba6120.c
+++ b/drivers/infiniband/hw/qib/qib_iba6120.c
@@ -2076,9 +2076,11 @@ static void qib_6120_config_ctxts(struct qib_devdata *dd)
 static void qib_update_6120_usrhead(struct qib_ctxtdata *rcd, u64 hd,
 				    u32 updegr, u32 egrhd, u32 npkts)
 {
-	qib_write_ureg(rcd->dd, ur_rcvhdrhead, hd, rcd->ctxt);
 	if (updegr)
 		qib_write_ureg(rcd->dd, ur_rcvegrindexhead, egrhd, rcd->ctxt);
+	mmiowb();
+	qib_write_ureg(rcd->dd, ur_rcvhdrhead, hd, rcd->ctxt);
+	mmiowb();
 }
 
 static u32 qib_6120_hdrqempty(struct qib_ctxtdata *rcd)
diff --git a/drivers/infiniband/hw/qib/qib_iba7220.c b/drivers/infiniband/hw/qib/qib_iba7220.c
index e1f947446c2a..4250c05a14f9 100644
--- a/drivers/infiniband/hw/qib/qib_iba7220.c
+++ b/drivers/infiniband/hw/qib/qib_iba7220.c
@@ -2724,9 +2724,11 @@ static int qib_7220_set_loopback(struct qib_pportdata *ppd, const char *what)
 static void qib_update_7220_usrhead(struct qib_ctxtdata *rcd, u64 hd,
 				    u32 updegr, u32 egrhd, u32 npkts)
 {
-	qib_write_ureg(rcd->dd, ur_rcvhdrhead, hd, rcd->ctxt);
 	if (updegr)
 		qib_write_ureg(rcd->dd, ur_rcvegrindexhead, egrhd, rcd->ctxt);
+	mmiowb();
+	qib_write_ureg(rcd->dd, ur_rcvhdrhead, hd, rcd->ctxt);
+	mmiowb();
 }
 
 static u32 qib_7220_hdrqempty(struct qib_ctxtdata *rcd)
diff --git a/drivers/infiniband/hw/qib/qib_iba7322.c b/drivers/infiniband/hw/qib/qib_iba7322.c
index 5ea9ece23b33..b7c3c7df268b 100644
--- a/drivers/infiniband/hw/qib/qib_iba7322.c
+++ b/drivers/infiniband/hw/qib/qib_iba7322.c
@@ -4084,10 +4084,12 @@ static void qib_update_7322_usrhead(struct qib_ctxtdata *rcd, u64 hd,
 	 */
 	if (hd >> IBA7322_HDRHEAD_PKTINT_SHIFT)
 		adjust_rcv_timeout(rcd, npkts);
-	qib_write_ureg(rcd->dd, ur_rcvhdrhead, hd, rcd->ctxt);
-	qib_write_ureg(rcd->dd, ur_rcvhdrhead, hd, rcd->ctxt);
 	if (updegr)
 		qib_write_ureg(rcd->dd, ur_rcvegrindexhead, egrhd, rcd->ctxt);
+	mmiowb();
+	qib_write_ureg(rcd->dd, ur_rcvhdrhead, hd, rcd->ctxt);
+	qib_write_ureg(rcd->dd, ur_rcvhdrhead, hd, rcd->ctxt);
+	mmiowb();
 }
 
 static u32 qib_7322_hdrqempty(struct qib_ctxtdata *rcd)
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_main.c b/drivers/infiniband/ulp/ipoib/ipoib_main.c
index fe89c4660d55..a98c414978ec 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_main.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_main.c
@@ -555,6 +555,7 @@ static int path_rec_start(struct net_device *dev,
 	return 0;
 }
 
+/* called with rcu_read_lock */
 static void neigh_add_path(struct sk_buff *skb, struct net_device *dev)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
@@ -636,6 +637,7 @@ err_drop:
 	spin_unlock_irqrestore(&priv->lock, flags);
 }
 
+/* called with rcu_read_lock */
 static void ipoib_path_lookup(struct sk_buff *skb, struct net_device *dev)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(skb->dev);
@@ -720,13 +722,14 @@ static int ipoib_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct neighbour *n = NULL;
 	unsigned long flags;
 
+	rcu_read_lock();
 	if (likely(skb_dst(skb)))
 		n = dst_get_neighbour(skb_dst(skb));
 
 	if (likely(n)) {
 		if (unlikely(!*to_ipoib_neigh(n))) {
 			ipoib_path_lookup(skb, dev);
-			return NETDEV_TX_OK;
+			goto unlock;
 		}
 
 		neigh = *to_ipoib_neigh(n);
@@ -749,17 +752,17 @@ static int ipoib_start_xmit(struct sk_buff *skb, struct net_device *dev)
 			ipoib_neigh_free(dev, neigh);
 			spin_unlock_irqrestore(&priv->lock, flags);
 			ipoib_path_lookup(skb, dev);
-			return NETDEV_TX_OK;
+			goto unlock;
 		}
 
 		if (ipoib_cm_get(neigh)) {
 			if (ipoib_cm_up(neigh)) {
 				ipoib_cm_send(dev, skb, ipoib_cm_get(neigh));
-				return NETDEV_TX_OK;
+				goto unlock;
 			}
 		} else if (neigh->ah) {
 			ipoib_send(dev, skb, neigh->ah, IPOIB_QPN(n->ha));
-			return NETDEV_TX_OK;
+			goto unlock;
 		}
 
 		if (skb_queue_len(&neigh->queue) < IPOIB_MAX_PATH_REC_QUEUE) {
@@ -793,13 +796,14 @@ static int ipoib_start_xmit(struct sk_buff *skb, struct net_device *dev)
 					   phdr->hwaddr + 4);
 				dev_kfree_skb_any(skb);
 				++dev->stats.tx_dropped;
-				return NETDEV_TX_OK;
+				goto unlock;
 			}
 
 			unicast_arp_send(skb, dev, phdr);
 		}
 	}
-
+unlock:
+	rcu_read_unlock();
 	return NETDEV_TX_OK;
 }
 
@@ -837,7 +841,7 @@ static int ipoib_hard_header(struct sk_buff *skb,
 	dst = skb_dst(skb);
 	n = NULL;
 	if (dst)
-		n = dst_get_neighbour(dst);
+		n = dst_get_neighbour_raw(dst);
 	if ((!dst || !n) && daddr) {
 		struct ipoib_pseudoheader *phdr =
 			(struct ipoib_pseudoheader *) skb_push(skb, sizeof *phdr);
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_multicast.c b/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
index ecea4fe1ed00..a8d2a891b840 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
@@ -265,7 +265,7 @@ static int ipoib_mcast_join_finish(struct ipoib_mcast *mcast,
 
 		skb->dev = dev;
 		if (dst)
-			n = dst_get_neighbour(dst);
+			n = dst_get_neighbour_raw(dst);
 		if (!dst || !n) {
 			/* put pseudoheader back on for next time */
 			skb_push(skb, sizeof (struct ipoib_pseudoheader));
@@ -721,6 +721,8 @@ out:
 	if (mcast && mcast->ah) {
 		struct dst_entry *dst = skb_dst(skb);
 		struct neighbour *n = NULL;
+
+		rcu_read_lock();
 		if (dst)
 			n = dst_get_neighbour(dst);
 		if (n && !*to_ipoib_neigh(n)) {
@@ -733,7 +735,7 @@ out:
 				list_add_tail(&neigh->list, &mcast->neigh_list);
 			}
 		}
-
+		rcu_read_unlock();
 		spin_unlock_irqrestore(&priv->lock, flags);
 		ipoib_send(dev, skb, mcast->ah, IB_MULTICAST_QPN);
 		return;
diff --git a/drivers/input/Kconfig b/drivers/input/Kconfig
index 23e82e46656d..c0e639c1b179 100644
--- a/drivers/input/Kconfig
+++ b/drivers/input/Kconfig
@@ -161,6 +161,15 @@ config INPUT_APMPOWER
 	  To compile this driver as a module, choose M here: the
 	  module will be called apm-power.
 
+config INPUT_KEYRESET
+	tristate "Reset key"
+	depends on INPUT
+	---help---
+	  Say Y here if you want to reboot when some keys are pressed;
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called keyreset.
+
 comment "Input Device Drivers"
 
 source "drivers/input/keyboard/Kconfig"
diff --git a/drivers/input/Makefile b/drivers/input/Makefile
index 0c789490e0b3..5d4593d3101d 100644
--- a/drivers/input/Makefile
+++ b/drivers/input/Makefile
@@ -24,3 +24,4 @@ obj-$(CONFIG_INPUT_TOUCHSCREEN)	+= touchscreen/
 obj-$(CONFIG_INPUT_MISC)	+= misc/
 
 obj-$(CONFIG_INPUT_APMPOWER)	+= apm-power.o
+obj-$(CONFIG_INPUT_KEYRESET)	+= keyreset.o
diff --git a/drivers/input/evdev.c b/drivers/input/evdev.c
index 4cf25347b015..6288d7d84fa7 100644
--- a/drivers/input/evdev.c
+++ b/drivers/input/evdev.c
@@ -23,6 +23,7 @@
 #include <linux/input.h>
 #include <linux/major.h>
 #include <linux/device.h>
+#include <linux/wakelock.h>
 #include "input-compat.h"
 
 struct evdev {
@@ -43,6 +44,9 @@ struct evdev_client {
 	unsigned int tail;
 	unsigned int packet_head; /* [future] position of the first element of next packet */
 	spinlock_t buffer_lock; /* protects access to buffer, head and tail */
+	struct wake_lock wake_lock;
+	bool use_wake_lock;
+	char name[28];
 	struct fasync_struct *fasync;
 	struct evdev *evdev;
 	struct list_head node;
@@ -75,10 +79,14 @@ static void evdev_pass_event(struct evdev_client *client,
 		client->buffer[client->tail].value = 0;
 
 		client->packet_head = client->tail;
+		if (client->use_wake_lock)
+			wake_unlock(&client->wake_lock);
 	}
 
 	if (event->type == EV_SYN && event->code == SYN_REPORT) {
 		client->packet_head = client->head;
+		if (client->use_wake_lock)
+			wake_lock(&client->wake_lock);
 		kill_fasync(&client->fasync, SIGIO, POLL_IN);
 	}
 
@@ -94,8 +102,11 @@ static void evdev_event(struct input_handle *handle,
 	struct evdev *evdev = handle->private;
 	struct evdev_client *client;
 	struct input_event event;
+	struct timespec ts;
 
-	do_gettimeofday(&event.time);
+	ktime_get_ts(&ts);
+	event.time.tv_sec = ts.tv_sec;
+	event.time.tv_usec = ts.tv_nsec / NSEC_PER_USEC;
 	event.type = type;
 	event.code = code;
 	event.value = value;
@@ -255,6 +266,8 @@ static int evdev_release(struct inode *inode, struct file *file)
 	mutex_unlock(&evdev->mutex);
 
 	evdev_detach_client(evdev, client);
+	if (client->use_wake_lock)
+		wake_lock_destroy(&client->wake_lock);
 	kfree(client);
 
 	evdev_close_device(evdev);
@@ -306,6 +319,8 @@ static int evdev_open(struct inode *inode, struct file *file)
 
 	client->bufsize = bufsize;
 	spin_lock_init(&client->buffer_lock);
+	snprintf(client->name, sizeof(client->name), "%s-%d",
+			dev_name(&evdev->dev), task_tgid_vnr(current));
 	client->evdev = evdev;
 	evdev_attach_client(evdev, client);
 
@@ -369,10 +384,13 @@ static int evdev_fetch_next_event(struct evdev_client *client,
 
 	spin_lock_irq(&client->buffer_lock);
 
-	have_event = client->head != client->tail;
+	have_event = client->packet_head != client->tail;
 	if (have_event) {
 		*event = client->buffer[client->tail++];
 		client->tail &= client->bufsize - 1;
+		if (client->use_wake_lock &&
+		    client->packet_head == client->tail)
+			wake_unlock(&client->wake_lock);
 	}
 
 	spin_unlock_irq(&client->buffer_lock);
@@ -386,19 +404,17 @@ static ssize_t evdev_read(struct file *file, char __user *buffer,
 	struct evdev_client *client = file->private_data;
 	struct evdev *evdev = client->evdev;
 	struct input_event event;
-	int retval;
+	int retval = 0;
 
 	if (count < input_event_size())
 		return -EINVAL;
 
-	if (client->packet_head == client->tail && evdev->exist &&
-	    (file->f_flags & O_NONBLOCK))
-		return -EAGAIN;
-
-	retval = wait_event_interruptible(evdev->wait,
-		client->packet_head != client->tail || !evdev->exist);
-	if (retval)
-		return retval;
+	if (!(file->f_flags & O_NONBLOCK)) {
+		retval = wait_event_interruptible(evdev->wait,
+			 client->packet_head != client->tail || !evdev->exist);
+		if (retval)
+			return retval;
+	}
 
 	if (!evdev->exist)
 		return -ENODEV;
@@ -412,6 +428,8 @@ static ssize_t evdev_read(struct file *file, char __user *buffer,
 		retval += input_event_size();
 	}
 
+	if (retval == 0 && file->f_flags & O_NONBLOCK)
+		retval = -EAGAIN;
 	return retval;
 }
 
@@ -621,6 +639,35 @@ static int evdev_handle_set_keycode_v2(struct input_dev *dev, void __user *p)
 	return input_set_keycode(dev, &ke);
 }
 
+static int evdev_enable_suspend_block(struct evdev *evdev,
+				      struct evdev_client *client)
+{
+	if (client->use_wake_lock)
+		return 0;
+
+	spin_lock_irq(&client->buffer_lock);
+	wake_lock_init(&client->wake_lock, WAKE_LOCK_SUSPEND, client->name);
+	client->use_wake_lock = true;
+	if (client->packet_head != client->tail)
+		wake_lock(&client->wake_lock);
+	spin_unlock_irq(&client->buffer_lock);
+	return 0;
+}
+
+static int evdev_disable_suspend_block(struct evdev *evdev,
+				       struct evdev_client *client)
+{
+	if (!client->use_wake_lock)
+		return 0;
+
+	spin_lock_irq(&client->buffer_lock);
+	client->use_wake_lock = false;
+	wake_lock_destroy(&client->wake_lock);
+	spin_unlock_irq(&client->buffer_lock);
+
+	return 0;
+}
+
 static long evdev_do_ioctl(struct file *file, unsigned int cmd,
 			   void __user *p, int compat_mode)
 {
@@ -694,6 +741,15 @@ static long evdev_do_ioctl(struct file *file, unsigned int cmd,
 
 	case EVIOCSKEYCODE_V2:
 		return evdev_handle_set_keycode_v2(dev, p);
+
+	case EVIOCGSUSPENDBLOCK:
+		return put_user(client->use_wake_lock, ip);
+
+	case EVIOCSSUSPENDBLOCK:
+		if (p)
+			return evdev_enable_suspend_block(evdev, client);
+		else
+			return evdev_disable_suspend_block(evdev, client);
 	}
 
 	size = _IOC_SIZE(cmd);
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index ddde0fd476f7..aced870f4e9d 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -1,6 +1,7 @@
 #
 # Makefile for the input core drivers.
 #
+GCOV_PROFILE_tegra-kbc.o := y
 
 # Each configuration option enables a list of files.
 
@@ -46,6 +47,7 @@ obj-$(CONFIG_KEYBOARD_STMPE)		+= stmpe-keypad.o
 obj-$(CONFIG_KEYBOARD_STOWAWAY)		+= stowaway.o
 obj-$(CONFIG_KEYBOARD_SUNKBD)		+= sunkbd.o
 obj-$(CONFIG_KEYBOARD_TC3589X)		+= tc3589x-keypad.o
+CFLAGS_tegra-kbc.o			= -Werror
 obj-$(CONFIG_KEYBOARD_TEGRA)		+= tegra-kbc.o
 obj-$(CONFIG_KEYBOARD_TNETV107X)	+= tnetv107x-keypad.o
 obj-$(CONFIG_KEYBOARD_TWL4030)		+= twl4030_keypad.o
diff --git a/drivers/input/keyboard/gpio_keys.c b/drivers/input/keyboard/gpio_keys.c
index 67df91af8424..ec1bd9009288 100644
--- a/drivers/input/keyboard/gpio_keys.c
+++ b/drivers/input/keyboard/gpio_keys.c
@@ -4,6 +4,8 @@
  * Copyright 2005 Phil Blundell
  * Copyright 2010, 2011 David Jander <david@protonic.nl>
  *
+ * Copyright 2010-2011 NVIDIA Corporation
+ *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
@@ -28,14 +30,18 @@
 #include <linux/gpio.h>
 #include <linux/of_platform.h>
 #include <linux/of_gpio.h>
+#include <linux/spinlock.h>
 
 struct gpio_button_data {
-	struct gpio_keys_button *button;
+	const struct gpio_keys_button *button;
 	struct input_dev *input;
 	struct timer_list timer;
 	struct work_struct work;
-	int timer_debounce;	/* in msecs */
+	unsigned int timer_debounce;	/* in msecs */
+	unsigned int irq;
+	spinlock_t lock;
 	bool disabled;
+	bool key_pressed;
 };
 
 struct gpio_keys_drvdata {
@@ -114,7 +120,7 @@ static void gpio_keys_disable_button(struct gpio_button_data *bdata)
 		/*
 		 * Disable IRQ and possible debouncing timer.
 		 */
-		disable_irq(gpio_to_irq(bdata->button->gpio));
+		disable_irq(bdata->irq);
 		if (bdata->timer_debounce)
 			del_timer_sync(&bdata->timer);
 
@@ -135,7 +141,7 @@ static void gpio_keys_disable_button(struct gpio_button_data *bdata)
 static void gpio_keys_enable_button(struct gpio_button_data *bdata)
 {
 	if (bdata->disabled) {
-		enable_irq(gpio_to_irq(bdata->button->gpio));
+		enable_irq(bdata->irq);
 		bdata->disabled = false;
 	}
 }
@@ -195,7 +201,7 @@ static ssize_t gpio_keys_attr_show_helper(struct gpio_keys_drvdata *ddata,
  * @type: button type (%EV_KEY, %EV_SW)
  *
  * This function parses stringified bitmap from @buf and disables/enables
- * GPIO buttons accordinly. Returns 0 on success and negative error
+ * GPIO buttons accordingly. Returns 0 on success and negative error
  * on failure.
  */
 static ssize_t gpio_keys_attr_store_helper(struct gpio_keys_drvdata *ddata,
@@ -320,9 +326,9 @@ static struct attribute_group gpio_keys_attr_group = {
 	.attrs = gpio_keys_attrs,
 };
 
-static void gpio_keys_report_event(struct gpio_button_data *bdata)
+static void gpio_keys_gpio_report_event(struct gpio_button_data *bdata)
 {
-	struct gpio_keys_button *button = bdata->button;
+	const struct gpio_keys_button *button = bdata->button;
 	struct input_dev *input = bdata->input;
 	unsigned int type = button->type ?: EV_KEY;
 	int state = (gpio_get_value_cansleep(button->gpio) ? 1 : 0) ^ button->active_low;
@@ -336,27 +342,26 @@ static void gpio_keys_report_event(struct gpio_button_data *bdata)
 	input_sync(input);
 }
 
-static void gpio_keys_work_func(struct work_struct *work)
+static void gpio_keys_gpio_work_func(struct work_struct *work)
 {
 	struct gpio_button_data *bdata =
 		container_of(work, struct gpio_button_data, work);
 
-	gpio_keys_report_event(bdata);
+	gpio_keys_gpio_report_event(bdata);
 }
 
-static void gpio_keys_timer(unsigned long _data)
+static void gpio_keys_gpio_timer(unsigned long _data)
 {
-	struct gpio_button_data *data = (struct gpio_button_data *)_data;
+	struct gpio_button_data *bdata = (struct gpio_button_data *)_data;
 
-	schedule_work(&data->work);
+	schedule_work(&bdata->work);
 }
 
-static irqreturn_t gpio_keys_isr(int irq, void *dev_id)
+static irqreturn_t gpio_keys_gpio_isr(int irq, void *dev_id)
 {
 	struct gpio_button_data *bdata = dev_id;
-	struct gpio_keys_button *button = bdata->button;
 
-	BUG_ON(irq != gpio_to_irq(button->gpio));
+	BUG_ON(irq != bdata->irq);
 
 	if (bdata->timer_debounce)
 		mod_timer(&bdata->timer,
@@ -367,50 +372,133 @@ static irqreturn_t gpio_keys_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+static void gpio_keys_irq_timer(unsigned long _data)
+{
+	struct gpio_button_data *bdata = (struct gpio_button_data *)_data;
+	struct input_dev *input = bdata->input;
+	unsigned long flags;
+
+	spin_lock_irqsave(&bdata->lock, flags);
+	if (bdata->key_pressed) {
+		input_event(input, EV_KEY, bdata->button->code, 0);
+		input_sync(input);
+		bdata->key_pressed = false;
+	}
+	spin_unlock_irqrestore(&bdata->lock, flags);
+}
+
+static irqreturn_t gpio_keys_irq_isr(int irq, void *dev_id)
+{
+	struct gpio_button_data *bdata = dev_id;
+	const struct gpio_keys_button *button = bdata->button;
+	struct input_dev *input = bdata->input;
+	unsigned long flags;
+
+	BUG_ON(irq != bdata->irq);
+
+	spin_lock_irqsave(&bdata->lock, flags);
+
+	if (!bdata->key_pressed) {
+		input_event(input, EV_KEY, button->code, 1);
+		input_sync(input);
+
+		if (!bdata->timer_debounce) {
+			input_event(input, EV_KEY, button->code, 0);
+			input_sync(input);
+			goto out;
+		}
+
+		bdata->key_pressed = true;
+	}
+
+	if (bdata->timer_debounce)
+		mod_timer(&bdata->timer,
+			jiffies + msecs_to_jiffies(bdata->timer_debounce));
+out:
+	spin_unlock_irqrestore(&bdata->lock, flags);
+	return IRQ_HANDLED;
+}
+
 static int __devinit gpio_keys_setup_key(struct platform_device *pdev,
+					 struct input_dev *input,
 					 struct gpio_button_data *bdata,
-					 struct gpio_keys_button *button)
+					 const struct gpio_keys_button *button)
 {
 	const char *desc = button->desc ? button->desc : "gpio_keys";
 	struct device *dev = &pdev->dev;
+	irq_handler_t isr;
 	unsigned long irqflags;
 	int irq, error;
 
-	setup_timer(&bdata->timer, gpio_keys_timer, (unsigned long)bdata);
-	INIT_WORK(&bdata->work, gpio_keys_work_func);
+	bdata->input = input;
+	bdata->button = button;
+	spin_lock_init(&bdata->lock);
 
-	error = gpio_request(button->gpio, desc);
-	if (error < 0) {
-		dev_err(dev, "failed to request GPIO %d, error %d\n",
-			button->gpio, error);
-		goto fail2;
-	}
+	if (gpio_is_valid(button->gpio)) {
 
-	error = gpio_direction_input(button->gpio);
-	if (error < 0) {
-		dev_err(dev, "failed to configure"
-			" direction for GPIO %d, error %d\n",
-			button->gpio, error);
-		goto fail3;
-	}
+		error = gpio_request(button->gpio, desc);
+		if (error < 0) {
+			dev_err(dev, "Failed to request GPIO %d, error %d\n",
+				button->gpio, error);
+			return error;
+		}
 
-	if (button->debounce_interval) {
-		error = gpio_set_debounce(button->gpio,
-					  button->debounce_interval * 1000);
-		/* use timer if gpiolib doesn't provide debounce */
-		if (error < 0)
-			bdata->timer_debounce = button->debounce_interval;
-	}
+		error = gpio_direction_input(button->gpio);
+		if (error < 0) {
+			dev_err(dev,
+				"Failed to configure direction for GPIO %d, error %d\n",
+				button->gpio, error);
+			goto fail;
+		}
 
-	irq = gpio_to_irq(button->gpio);
-	if (irq < 0) {
-		error = irq;
-		dev_err(dev, "Unable to get irq number for GPIO %d, error %d\n",
-			button->gpio, error);
-		goto fail3;
+		if (button->debounce_interval) {
+			error = gpio_set_debounce(button->gpio,
+					button->debounce_interval * 1000);
+			/* use timer if gpiolib doesn't provide debounce */
+			if (error < 0)
+				bdata->timer_debounce =
+						button->debounce_interval;
+		}
+
+		irq = gpio_to_irq(button->gpio);
+		if (irq < 0) {
+			error = irq;
+			dev_err(dev,
+				"Unable to get irq number for GPIO %d, error %d\n",
+				button->gpio, error);
+			goto fail;
+		}
+		bdata->irq = irq;
+
+		INIT_WORK(&bdata->work, gpio_keys_gpio_work_func);
+		setup_timer(&bdata->timer,
+			    gpio_keys_gpio_timer, (unsigned long)bdata);
+
+		isr = gpio_keys_gpio_isr;
+		irqflags = IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING;
+
+	} else {
+		if (!button->irq) {
+			dev_err(dev, "No IRQ specified\n");
+			return -EINVAL;
+		}
+		bdata->irq = button->irq;
+
+		if (button->type && button->type != EV_KEY) {
+			dev_err(dev, "Only EV_KEY allowed for IRQ buttons.\n");
+			return -EINVAL;
+		}
+
+		bdata->timer_debounce = button->debounce_interval;
+		setup_timer(&bdata->timer,
+			    gpio_keys_irq_timer, (unsigned long)bdata);
+
+		isr = gpio_keys_irq_isr;
+		irqflags = 0;
 	}
 
-	irqflags = IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING;
+	input_set_capability(input, button->type ?: EV_KEY, button->code);
+
 	/*
 	 * If platform has specified that the button can be disabled,
 	 * we don't want it to share the interrupt line.
@@ -418,18 +506,19 @@ static int __devinit gpio_keys_setup_key(struct platform_device *pdev,
 	if (!button->can_disable)
 		irqflags |= IRQF_SHARED;
 
-	error = request_threaded_irq(irq, NULL, gpio_keys_isr, irqflags, desc, bdata);
+	error = request_any_context_irq(bdata->irq, isr, irqflags, desc, bdata);
 	if (error < 0) {
 		dev_err(dev, "Unable to claim irq %d; error %d\n",
-			irq, error);
-		goto fail3;
+			bdata->irq, error);
+		goto fail;
 	}
 
 	return 0;
 
-fail3:
-	gpio_free(button->gpio);
-fail2:
+fail:
+	if (gpio_is_valid(button->gpio))
+		gpio_free(button->gpio);
+
 	return error;
 }
 
@@ -461,8 +550,7 @@ static int gpio_keys_get_devtree_pdata(struct device *dev,
 	struct device_node *node, *pp;
 	int i;
 	struct gpio_keys_button *buttons;
-	const u32 *reg;
-	int len;
+	u32 reg;
 
 	node = dev->of_node;
 	if (node == NULL)
@@ -470,7 +558,7 @@ static int gpio_keys_get_devtree_pdata(struct device *dev,
 
 	memset(pdata, 0, sizeof *pdata);
 
-	pdata->rep = !!of_get_property(node, "autorepeat", &len);
+	pdata->rep = !!of_get_property(node, "autorepeat", NULL);
 
 	/* First count the subnodes */
 	pdata->nbuttons = 0;
@@ -498,22 +586,25 @@ static int gpio_keys_get_devtree_pdata(struct device *dev,
 		buttons[i].gpio = of_get_gpio_flags(pp, 0, &flags);
 		buttons[i].active_low = flags & OF_GPIO_ACTIVE_LOW;
 
-		reg = of_get_property(pp, "linux,code", &len);
-		if (!reg) {
+		if (of_property_read_u32(pp, "linux,code", &reg)) {
 			dev_err(dev, "Button without keycode: 0x%x\n", buttons[i].gpio);
 			goto out_fail;
 		}
-		buttons[i].code = be32_to_cpup(reg);
+		buttons[i].code = reg;
 
-		buttons[i].desc = of_get_property(pp, "label", &len);
+		buttons[i].desc = of_get_property(pp, "label", NULL);
 
-		reg = of_get_property(pp, "linux,input-type", &len);
-		buttons[i].type = reg ? be32_to_cpup(reg) : EV_KEY;
+		if (of_property_read_u32(pp, "linux,input-type", &reg) == 0)
+			buttons[i].type = reg;
+		else
+			buttons[i].type = EV_KEY;
 
 		buttons[i].wakeup = !!of_get_property(pp, "gpio-key,wakeup", NULL);
 
-		reg = of_get_property(pp, "debounce-interval", &len);
-		buttons[i].debounce_interval = reg ? be32_to_cpup(reg) : 5;
+		if (of_property_read_u32(pp, "debounce-interval", &reg) == 0)
+			buttons[i].debounce_interval = reg;
+		else
+			buttons[i].debounce_interval = 5;
 
 		i++;
 	}
@@ -545,9 +636,19 @@ static int gpio_keys_get_devtree_pdata(struct device *dev,
 
 #endif
 
+static void gpio_remove_key(struct gpio_button_data *bdata)
+{
+	free_irq(bdata->irq, bdata);
+	if (bdata->timer_debounce)
+		del_timer_sync(&bdata->timer);
+	cancel_work_sync(&bdata->work);
+	if (gpio_is_valid(bdata->button->gpio))
+		gpio_free(bdata->button->gpio);
+}
+
 static int __devinit gpio_keys_probe(struct platform_device *pdev)
 {
-	struct gpio_keys_platform_data *pdata = pdev->dev.platform_data;
+	const struct gpio_keys_platform_data *pdata = pdev->dev.platform_data;
 	struct gpio_keys_drvdata *ddata;
 	struct device *dev = &pdev->dev;
 	struct gpio_keys_platform_data alt_pdata;
@@ -597,21 +698,15 @@ static int __devinit gpio_keys_probe(struct platform_device *pdev)
 		__set_bit(EV_REP, input->evbit);
 
 	for (i = 0; i < pdata->nbuttons; i++) {
-		struct gpio_keys_button *button = &pdata->buttons[i];
+		const struct gpio_keys_button *button = &pdata->buttons[i];
 		struct gpio_button_data *bdata = &ddata->data[i];
-		unsigned int type = button->type ?: EV_KEY;
-
-		bdata->input = input;
-		bdata->button = button;
 
-		error = gpio_keys_setup_key(pdev, bdata, button);
+		error = gpio_keys_setup_key(pdev, input, bdata, button);
 		if (error)
 			goto fail2;
 
 		if (button->wakeup)
 			wakeup = 1;
-
-		input_set_capability(input, type, button->code);
 	}
 
 	error = sysfs_create_group(&pdev->dev.kobj, &gpio_keys_attr_group);
@@ -628,9 +723,12 @@ static int __devinit gpio_keys_probe(struct platform_device *pdev)
 		goto fail3;
 	}
 
-	/* get current state of buttons */
-	for (i = 0; i < pdata->nbuttons; i++)
-		gpio_keys_report_event(&ddata->data[i]);
+	/* get current state of buttons that are connected to GPIOs */
+	for (i = 0; i < pdata->nbuttons; i++) {
+		struct gpio_button_data *bdata = &ddata->data[i];
+		if (gpio_is_valid(bdata->button->gpio))
+			gpio_keys_gpio_report_event(bdata);
+	}
 	input_sync(input);
 
 	device_init_wakeup(&pdev->dev, wakeup);
@@ -640,13 +738,8 @@ static int __devinit gpio_keys_probe(struct platform_device *pdev)
  fail3:
 	sysfs_remove_group(&pdev->dev.kobj, &gpio_keys_attr_group);
  fail2:
-	while (--i >= 0) {
-		free_irq(gpio_to_irq(pdata->buttons[i].gpio), &ddata->data[i]);
-		if (ddata->data[i].timer_debounce)
-			del_timer_sync(&ddata->data[i].timer);
-		cancel_work_sync(&ddata->data[i].work);
-		gpio_free(pdata->buttons[i].gpio);
-	}
+	while (--i >= 0)
+		gpio_remove_key(&ddata->data[i]);
 
 	platform_set_drvdata(pdev, NULL);
  fail1:
@@ -669,14 +762,8 @@ static int __devexit gpio_keys_remove(struct platform_device *pdev)
 
 	device_init_wakeup(&pdev->dev, 0);
 
-	for (i = 0; i < ddata->n_buttons; i++) {
-		int irq = gpio_to_irq(ddata->data[i].button->gpio);
-		free_irq(irq, &ddata->data[i]);
-		if (ddata->data[i].timer_debounce)
-			del_timer_sync(&ddata->data[i].timer);
-		cancel_work_sync(&ddata->data[i].work);
-		gpio_free(ddata->data[i].button->gpio);
-	}
+	for (i = 0; i < ddata->n_buttons; i++)
+		gpio_remove_key(&ddata->data[i]);
 
 	input_unregister_device(input);
 
@@ -701,11 +788,9 @@ static int gpio_keys_suspend(struct device *dev)
 
 	if (device_may_wakeup(dev)) {
 		for (i = 0; i < ddata->n_buttons; i++) {
-			struct gpio_keys_button *button = ddata->data[i].button;
-			if (button->wakeup) {
-				int irq = gpio_to_irq(button->gpio);
-				enable_irq_wake(irq);
-			}
+			struct gpio_button_data *bdata = &ddata->data[i];
+			if (bdata->button->wakeup)
+				WARN_ON(enable_irq_wake(bdata->irq));
 		}
 	}
 
@@ -714,18 +799,30 @@ static int gpio_keys_suspend(struct device *dev)
 
 static int gpio_keys_resume(struct device *dev)
 {
+	struct platform_device *pdev = to_platform_device(dev);
 	struct gpio_keys_drvdata *ddata = dev_get_drvdata(dev);
+	struct gpio_keys_platform_data *pdata = pdev->dev.platform_data;
+	int wakeup_key = KEY_RESERVED;
 	int i;
 
-	for (i = 0; i < ddata->n_buttons; i++) {
+	if (pdata && pdata->wakeup_key)
+		wakeup_key = pdata->wakeup_key();
 
-		struct gpio_keys_button *button = ddata->data[i].button;
-		if (button->wakeup && device_may_wakeup(dev)) {
-			int irq = gpio_to_irq(button->gpio);
-			disable_irq_wake(irq);
+	for (i = 0; i < ddata->n_buttons; i++) {
+		struct gpio_button_data *bdata = &ddata->data[i];
+		if (bdata->button->wakeup && device_may_wakeup(dev)) {
+			disable_irq_wake(bdata->irq);
+			if (wakeup_key == bdata->button->code) {
+				unsigned int type = bdata->button->type ?: EV_KEY;
+
+				input_event(ddata->input, type, bdata->button->code, 1);
+				input_event(ddata->input, type, bdata->button->code, 0);
+				input_sync(ddata->input);
+			}
 		}
 
-		gpio_keys_report_event(&ddata->data[i]);
+		if (gpio_is_valid(bdata->button->gpio))
+			gpio_keys_gpio_report_event(bdata);
 	}
 	input_sync(ddata->input);
 
diff --git a/drivers/input/keyboard/tegra-kbc.c b/drivers/input/keyboard/tegra-kbc.c
index a5a77915c650..01ee74852368 100644
--- a/drivers/input/keyboard/tegra-kbc.c
+++ b/drivers/input/keyboard/tegra-kbc.c
@@ -48,6 +48,7 @@
 #define KBC_DEBOUNCE_CNT_SHIFT(cnt)	(cnt << 4)
 #define KBC_CONTROL_FIFO_CNT_INT_EN	(1 << 3)
 #define KBC_CONTROL_KBC_EN		(1 << 0)
+#define KBC_CONTROL_KP_INT_EN		(1<<1)
 
 /* KBC Interrupt Register */
 #define KBC_INT_0	0x4
@@ -55,6 +56,7 @@
 
 #define KBC_ROW_CFG0_0	0x8
 #define KBC_COL_CFG0_0	0x18
+#define KBC_TO_CNT_0	0x24
 #define KBC_INIT_DLY_0	0x28
 #define KBC_RPT_DLY_0	0x2c
 #define KBC_KP_ENT0_0	0x30
@@ -62,11 +64,15 @@
 #define KBC_ROW0_MASK_0	0x38
 
 #define KBC_ROW_SHIFT	3
+#define DEFAULT_SCAN_COUNT 2
+#define DEFAULT_INIT_DLY   5
 
 struct tegra_kbc {
 	void __iomem *mmio;
 	struct input_dev *idev;
 	unsigned int irq;
+	unsigned int wake_enable_rows;
+	unsigned int wake_enable_cols;
 	spinlock_t lock;
 	unsigned int repoll_dly;
 	unsigned long cp_dly_jiffies;
@@ -78,6 +84,9 @@ struct tegra_kbc {
 	unsigned int num_pressed_keys;
 	struct timer_list timer;
 	struct clk *clk;
+	int is_open;
+	unsigned long scan_timeout_count;
+	unsigned long one_scan_time;
 };
 
 static const u32 tegra_kbc_default_keymap[] = {
@@ -417,11 +426,21 @@ static void tegra_kbc_setup_wakekeys(struct tegra_kbc *kbc, bool filter)
 	int i;
 	unsigned int rst_val;
 
-	/* Either mask all keys or none. */
-	rst_val = (filter && !pdata->wakeup) ? ~0 : 0;
+	BUG_ON(pdata->wake_cnt > KBC_MAX_KEY);
+	rst_val = (filter && pdata->wake_cnt) ? ~0 : 0;
 
 	for (i = 0; i < KBC_MAX_ROW; i++)
 		writel(rst_val, kbc->mmio + KBC_ROW0_MASK_0 + i * 4);
+
+	if (filter) {
+		for (i = 0; i < pdata->wake_cnt; i++) {
+			u32 val, addr;
+			addr = pdata->wake_cfg[i].row * 4 + KBC_ROW0_MASK_0;
+			val = readl(kbc->mmio + addr);
+			val &= ~(1 << pdata->wake_cfg[i].col);
+			writel(val, kbc->mmio + addr);
+		}
+	}
 }
 
 static void tegra_kbc_config_pins(struct tegra_kbc *kbc)
@@ -442,11 +461,14 @@ static void tegra_kbc_config_pins(struct tegra_kbc *kbc)
 		row_cfg &= ~r_mask;
 		col_cfg &= ~c_mask;
 
-		if (pdata->pin_cfg[i].is_row)
-			row_cfg |= ((pdata->pin_cfg[i].num << 1) | 1) << r_shft;
-		else
-			col_cfg |= ((pdata->pin_cfg[i].num << 1) | 1) << c_shft;
-
+		if (pdata->pin_cfg[i].en) {
+			if (pdata->pin_cfg[i].is_row)
+				row_cfg |= ((pdata->pin_cfg[i].num << 1) | 1)
+						<< r_shft;
+			else
+				col_cfg |= ((pdata->pin_cfg[i].num << 1) | 1)
+						<< c_shft;
+		}
 		writel(row_cfg, kbc->mmio + r_offs);
 		writel(col_cfg, kbc->mmio + c_offs);
 	}
@@ -460,6 +482,7 @@ static int tegra_kbc_start(struct tegra_kbc *kbc)
 	u32 val = 0;
 
 	clk_enable(kbc->clk);
+	kbc->is_open = 1;
 
 	/* Reset the KBC controller to clear all previous status.*/
 	tegra_periph_reset_assert(kbc->clk);
@@ -478,8 +501,12 @@ static int tegra_kbc_start(struct tegra_kbc *kbc)
 	val |= KBC_FIFO_TH_CNT_SHIFT(1); /* set fifo interrupt threshold to 1 */
 	val |= KBC_CONTROL_FIFO_CNT_INT_EN;  /* interrupt on FIFO threshold */
 	val |= KBC_CONTROL_KBC_EN;     /* enable */
+	val |= KBC_CONTROL_KP_INT_EN;
 	writel(val, kbc->mmio + KBC_CONTROL_0);
 
+	writel(DEFAULT_INIT_DLY, kbc->mmio + KBC_INIT_DLY_0);
+	writel(kbc->scan_timeout_count, kbc->mmio + KBC_TO_CNT_0);
+
 	/*
 	 * Compute the delay(ns) from interrupt mode to continuous polling
 	 * mode so the timer routine is scheduled appropriately.
@@ -526,6 +553,7 @@ static void tegra_kbc_stop(struct tegra_kbc *kbc)
 	del_timer_sync(&kbc->timer);
 
 	clk_disable(kbc->clk);
+	kbc->is_open = 0;
 }
 
 static int tegra_kbc_open(struct input_dev *dev)
@@ -583,9 +611,13 @@ static int __devinit tegra_kbc_probe(struct platform_device *pdev)
 	struct resource *res;
 	int irq;
 	int err;
+	int i;
 	int num_rows = 0;
 	unsigned int debounce_cnt;
 	unsigned int scan_time_rows;
+	unsigned long scan_tc;
+
+	dev_dbg(&pdev->dev, "KBC: tegra_kbc_probe\n");
 
 	if (!pdata)
 		return -EINVAL;
@@ -639,6 +671,14 @@ static int __devinit tegra_kbc_probe(struct platform_device *pdev)
 		goto err_iounmap;
 	}
 
+	kbc->is_open = 0;
+	kbc->wake_enable_rows = 0;
+	kbc->wake_enable_cols = 0;
+	for (i = 0; i < pdata->wake_cnt; i++) {
+		kbc->wake_enable_rows |= (1 << pdata->wake_cfg[i].row);
+		kbc->wake_enable_cols |= (1 << pdata->wake_cfg[i].col);
+	}
+
 	/*
 	 * The time delay between two consecutive reads of the FIFO is
 	 * the sum of the repeat time and the time taken for scanning
@@ -650,6 +690,17 @@ static int __devinit tegra_kbc_probe(struct platform_device *pdev)
 	kbc->repoll_dly = KBC_ROW_SCAN_DLY + scan_time_rows + pdata->repeat_cnt;
 	kbc->repoll_dly = DIV_ROUND_UP(kbc->repoll_dly, KBC_CYCLE_MS);
 
+	if (pdata->scan_count)
+		scan_tc = DEFAULT_INIT_DLY + (scan_time_rows +
+				pdata->repeat_cnt) * pdata->scan_count;
+	else
+		scan_tc = DEFAULT_INIT_DLY + (scan_time_rows +
+				pdata->repeat_cnt) * DEFAULT_SCAN_COUNT;
+
+	kbc->one_scan_time = scan_time_rows + pdata->repeat_cnt;
+	/* Bit 19:0 is for scan timeout count */
+	kbc->scan_timeout_count = scan_tc & 0xFFFFF;
+
 	input_dev->name = pdev->name;
 	input_dev->id.bustype = BUS_HOST;
 	input_dev->dev.parent = &pdev->dev;
@@ -658,7 +709,10 @@ static int __devinit tegra_kbc_probe(struct platform_device *pdev)
 
 	input_set_drvdata(input_dev, kbc);
 
-	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REP);
+	input_dev->evbit[0] = BIT_MASK(EV_KEY);
+	if (!pdata->disable_ev_rep)
+		input_dev->evbit[0] |= BIT_MASK(EV_REP);
+
 	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
 
 	input_dev->keycode = kbc->keycode;
@@ -733,8 +787,31 @@ static int tegra_kbc_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct tegra_kbc *kbc = platform_get_drvdata(pdev);
+	int timeout;
+	unsigned long int_st;
+
+	dev_dbg(&pdev->dev, "KBC: tegra_kbc_suspend\n");
+
+	if (!kbc->is_open)
+		return 0;
 
 	if (device_may_wakeup(&pdev->dev)) {
+		timeout = DIV_ROUND_UP((kbc->scan_timeout_count +
+				kbc->one_scan_time), 32);
+		timeout = DIV_ROUND_UP(timeout, 10);
+		while (timeout--) {
+			int_st = readl(kbc->mmio + KBC_INT_0);
+			if (int_st & 0x8) {
+				msleep(10);
+				continue;
+			}
+			break;
+		}
+		int_st = readl(kbc->mmio + KBC_INT_0);
+		if (int_st & 0x8)
+			dev_err(&pdev->dev, "KBC: Controller is not in "
+				"wakeupmode\n");
+
 		tegra_kbc_setup_wakekeys(kbc, true);
 		enable_irq_wake(kbc->irq);
 		/* Forcefully clear the interrupt status */
@@ -756,6 +833,9 @@ static int tegra_kbc_resume(struct device *dev)
 	struct tegra_kbc *kbc = platform_get_drvdata(pdev);
 	int err = 0;
 
+	if (!kbc->is_open)
+		return tegra_kbc_start(kbc);
+
 	if (device_may_wakeup(&pdev->dev)) {
 		disable_irq_wake(kbc->irq);
 		tegra_kbc_setup_wakekeys(kbc, false);
diff --git a/drivers/input/keyreset.c b/drivers/input/keyreset.c
new file mode 100644
index 000000000000..36208fe0baae
--- /dev/null
+++ b/drivers/input/keyreset.c
@@ -0,0 +1,239 @@
+/* drivers/input/keyreset.c
+ *
+ * Copyright (C) 2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/input.h>
+#include <linux/keyreset.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/syscalls.h>
+
+
+struct keyreset_state {
+	struct input_handler input_handler;
+	unsigned long keybit[BITS_TO_LONGS(KEY_CNT)];
+	unsigned long upbit[BITS_TO_LONGS(KEY_CNT)];
+	unsigned long key[BITS_TO_LONGS(KEY_CNT)];
+	spinlock_t lock;
+	int key_down_target;
+	int key_down;
+	int key_up;
+	int restart_disabled;
+	int (*reset_fn)(void);
+};
+
+int restart_requested;
+static void deferred_restart(struct work_struct *dummy)
+{
+	restart_requested = 2;
+	sys_sync();
+	restart_requested = 3;
+	kernel_restart(NULL);
+}
+static DECLARE_WORK(restart_work, deferred_restart);
+
+static void keyreset_event(struct input_handle *handle, unsigned int type,
+			   unsigned int code, int value)
+{
+	unsigned long flags;
+	struct keyreset_state *state = handle->private;
+
+	if (type != EV_KEY)
+		return;
+
+	if (code >= KEY_MAX)
+		return;
+
+	if (!test_bit(code, state->keybit))
+		return;
+
+	spin_lock_irqsave(&state->lock, flags);
+	if (!test_bit(code, state->key) == !value)
+		goto done;
+	__change_bit(code, state->key);
+	if (test_bit(code, state->upbit)) {
+		if (value) {
+			state->restart_disabled = 1;
+			state->key_up++;
+		} else
+			state->key_up--;
+	} else {
+		if (value)
+			state->key_down++;
+		else
+			state->key_down--;
+	}
+	if (state->key_down == 0 && state->key_up == 0)
+		state->restart_disabled = 0;
+
+	pr_debug("reset key changed %d %d new state %d-%d-%d\n", code, value,
+		 state->key_down, state->key_up, state->restart_disabled);
+
+	if (value && !state->restart_disabled &&
+	    state->key_down == state->key_down_target) {
+		state->restart_disabled = 1;
+		if (restart_requested)
+			panic("keyboard reset failed, %d", restart_requested);
+		if (state->reset_fn) {
+			restart_requested = state->reset_fn();
+		} else {
+			pr_info("keyboard reset\n");
+			schedule_work(&restart_work);
+			restart_requested = 1;
+		}
+	}
+done:
+	spin_unlock_irqrestore(&state->lock, flags);
+}
+
+static int keyreset_connect(struct input_handler *handler,
+					  struct input_dev *dev,
+					  const struct input_device_id *id)
+{
+	int i;
+	int ret;
+	struct input_handle *handle;
+	struct keyreset_state *state =
+		container_of(handler, struct keyreset_state, input_handler);
+
+	for (i = 0; i < KEY_MAX; i++) {
+		if (test_bit(i, state->keybit) && test_bit(i, dev->keybit))
+			break;
+	}
+	if (i == KEY_MAX)
+		return -ENODEV;
+
+	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
+	if (!handle)
+		return -ENOMEM;
+
+	handle->dev = dev;
+	handle->handler = handler;
+	handle->name = "keyreset";
+	handle->private = state;
+
+	ret = input_register_handle(handle);
+	if (ret)
+		goto err_input_register_handle;
+
+	ret = input_open_device(handle);
+	if (ret)
+		goto err_input_open_device;
+
+	pr_info("using input dev %s for key reset\n", dev->name);
+
+	return 0;
+
+err_input_open_device:
+	input_unregister_handle(handle);
+err_input_register_handle:
+	kfree(handle);
+	return ret;
+}
+
+static void keyreset_disconnect(struct input_handle *handle)
+{
+	input_close_device(handle);
+	input_unregister_handle(handle);
+	kfree(handle);
+}
+
+static const struct input_device_id keyreset_ids[] = {
+	{
+		.flags = INPUT_DEVICE_ID_MATCH_EVBIT,
+		.evbit = { BIT_MASK(EV_KEY) },
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(input, keyreset_ids);
+
+static int keyreset_probe(struct platform_device *pdev)
+{
+	int ret;
+	int key, *keyp;
+	struct keyreset_state *state;
+	struct keyreset_platform_data *pdata = pdev->dev.platform_data;
+
+	if (!pdata)
+		return -EINVAL;
+
+	state = kzalloc(sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	spin_lock_init(&state->lock);
+	keyp = pdata->keys_down;
+	while ((key = *keyp++)) {
+		if (key >= KEY_MAX)
+			continue;
+		state->key_down_target++;
+		__set_bit(key, state->keybit);
+	}
+	if (pdata->keys_up) {
+		keyp = pdata->keys_up;
+		while ((key = *keyp++)) {
+			if (key >= KEY_MAX)
+				continue;
+			__set_bit(key, state->keybit);
+			__set_bit(key, state->upbit);
+		}
+	}
+
+	if (pdata->reset_fn)
+		state->reset_fn = pdata->reset_fn;
+
+	state->input_handler.event = keyreset_event;
+	state->input_handler.connect = keyreset_connect;
+	state->input_handler.disconnect = keyreset_disconnect;
+	state->input_handler.name = KEYRESET_NAME;
+	state->input_handler.id_table = keyreset_ids;
+	ret = input_register_handler(&state->input_handler);
+	if (ret) {
+		kfree(state);
+		return ret;
+	}
+	platform_set_drvdata(pdev, state);
+	return 0;
+}
+
+int keyreset_remove(struct platform_device *pdev)
+{
+	struct keyreset_state *state = platform_get_drvdata(pdev);
+	input_unregister_handler(&state->input_handler);
+	kfree(state);
+	return 0;
+}
+
+
+struct platform_driver keyreset_driver = {
+	.driver.name = KEYRESET_NAME,
+	.probe = keyreset_probe,
+	.remove = keyreset_remove,
+};
+
+static int __init keyreset_init(void)
+{
+	return platform_driver_register(&keyreset_driver);
+}
+
+static void __exit keyreset_exit(void)
+{
+	return platform_driver_unregister(&keyreset_driver);
+}
+
+module_init(keyreset_init);
+module_exit(keyreset_exit);
diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig
index c9104bb4db06..5bad288384dd 100644
--- a/drivers/input/misc/Kconfig
+++ b/drivers/input/misc/Kconfig
@@ -214,6 +214,17 @@ config INPUT_ATI_REMOTE2
 	  To compile this driver as a module, choose M here: the module will be
 	  called ati_remote2.
 
+config INPUT_KEYCHORD
+	tristate "Key chord input driver support"
+	help
+	  Say Y here if you want to enable the key chord driver
+	  accessible at /dev/keychord.  This driver can be used
+	  for receiving notifications when client specified key
+	  combinations are pressed.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called keychord.
+
 config INPUT_KEYSPAN_REMOTE
 	tristate "Keyspan DMR USB remote control (EXPERIMENTAL)"
 	depends on EXPERIMENTAL
@@ -343,6 +354,11 @@ config INPUT_SGI_BTNS
 	  To compile this driver as a module, choose M here: the
 	  module will be called sgi_btns.
 
+config INPUT_GPIO
+	tristate "GPIO driver support"
+	help
+	  Say Y here if you want to support gpio based keys, wheels etc...
+
 config HP_SDC_RTC
 	tristate "HP SDC Real Time Clock"
 	depends on (GSC || HP300) && SERIO
@@ -527,4 +543,26 @@ config INPUT_XEN_KBDDEV_FRONTEND
 	  To compile this driver as a module, choose M here: the
 	  module will be called xen-kbdfront.
 
+config INPUT_ALPS_GPIO_SCROLLWHEEL
+	tristate "Alps GPIO Scrollwheel"
+	depends on GENERIC_GPIO
+	help
+	  This driver implements support for Alps SRBE
+          ScrollWheel connected to GPIO pins of various
+          CPUs (and some other chips).
+
+	  Say Y here if your device has ScrollWheel connected
+	  directly to such GPIO pins.  Your board-specific
+	  setup logic must also provide a platform device,
+	  with configuration data saying which GPIOs are used.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called alps_gpio_scrollwheel.
+
+config INPUT_CAPELLA_CM3217
+	tristate "CM3217 light sensor"
+	depends on I2C
+	help
+	  Say Y here to enable the CM3217 Ambient Light Sensor.
+
 endif
diff --git a/drivers/input/misc/Makefile b/drivers/input/misc/Makefile
index 299ad5edba84..f19eb0cec2f7 100644
--- a/drivers/input/misc/Makefile
+++ b/drivers/input/misc/Makefile
@@ -12,18 +12,22 @@ obj-$(CONFIG_INPUT_AD714X_SPI)		+= ad714x-spi.o
 obj-$(CONFIG_INPUT_ADXL34X)		+= adxl34x.o
 obj-$(CONFIG_INPUT_ADXL34X_I2C)		+= adxl34x-i2c.o
 obj-$(CONFIG_INPUT_ADXL34X_SPI)		+= adxl34x-spi.o
+obj-$(CONFIG_INPUT_ALPS_GPIO_SCROLLWHEEL) += alps_gpio_scrollwheel.o
 obj-$(CONFIG_INPUT_APANEL)		+= apanel.o
 obj-$(CONFIG_INPUT_ATI_REMOTE)		+= ati_remote.o
 obj-$(CONFIG_INPUT_ATI_REMOTE2)		+= ati_remote2.o
 obj-$(CONFIG_INPUT_ATLAS_BTNS)		+= atlas_btns.o
 obj-$(CONFIG_INPUT_BFIN_ROTARY)		+= bfin_rotary.o
+obj-$(CONFIG_INPUT_CAPELLA_CM3217)	+= cm3217.o
 obj-$(CONFIG_INPUT_CM109)		+= cm109.o
 obj-$(CONFIG_INPUT_CMA3000)		+= cma3000_d0x.o
 obj-$(CONFIG_INPUT_CMA3000_I2C)		+= cma3000_d0x_i2c.o
 obj-$(CONFIG_INPUT_COBALT_BTNS)		+= cobalt_btns.o
 obj-$(CONFIG_INPUT_DM355EVM)		+= dm355evm_keys.o
+obj-$(CONFIG_INPUT_GPIO)		+= gpio_event.o gpio_matrix.o gpio_input.o gpio_output.o gpio_axis.o
 obj-$(CONFIG_HP_SDC_RTC)		+= hp_sdc_rtc.o
 obj-$(CONFIG_INPUT_IXP4XX_BEEPER)	+= ixp4xx-beeper.o
+obj-$(CONFIG_INPUT_KEYCHORD)		+= keychord.o
 obj-$(CONFIG_INPUT_KEYSPAN_REMOTE)	+= keyspan_remote.o
 obj-$(CONFIG_INPUT_KXTJ9)		+= kxtj9.o
 obj-$(CONFIG_INPUT_M68K_BEEP)		+= m68kspkr.o
diff --git a/drivers/input/misc/alps_gpio_scrollwheel.c b/drivers/input/misc/alps_gpio_scrollwheel.c
new file mode 100644
index 000000000000..4a789267c475
--- /dev/null
+++ b/drivers/input/misc/alps_gpio_scrollwheel.c
@@ -0,0 +1,428 @@
+/*
+ * kernel/drivers/input/misc/alps_gpio_scrollwheel.c
+ *
+ * Copyright (c) 2010, NVIDIA Corporation.
+ *
+ * Driver for ScrollWheel on GPIO lines capable of generating interrupts.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/fs.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/sched.h>
+#include <linux/pm.h>
+#include <linux/slab.h>
+#include <linux/sysctl.h>
+#include <linux/proc_fs.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/input.h>
+#include <linux/gpio_scrollwheel.h>
+#include <linux/workqueue.h>
+#include <linux/gpio.h>
+
+struct scrollwheel_button_data {
+	struct gpio_scrollwheel_button *button;
+	struct input_dev *input;
+	struct timer_list timer;
+	struct work_struct work;
+	int timer_debounce;	/* in msecs */
+	int rotgpio;
+	bool disabled;
+};
+
+struct gpio_scrollwheel_drvdata {
+	struct input_dev *input;
+	struct mutex disable_lock;
+	unsigned int n_buttons;
+	int (*enable)(struct device *dev);
+	void (*disable)(struct device *dev);
+	struct scrollwheel_button_data data[0];
+};
+
+static void scrollwheel_report_key(struct scrollwheel_button_data *bdata)
+{
+	struct gpio_scrollwheel_button *button = bdata->button;
+	struct input_dev *input = bdata->input;
+	int state = (gpio_get_value(button->gpio) ? 1 : 0) ^ \
+				button->active_low;
+	int state2 = 0;
+
+	switch (button->pinaction) {
+	case GPIO_SCROLLWHEEL_PIN_PRESS:
+		input_report_key(input, KEY_ENTER, 1);
+		input_report_key(input, KEY_ENTER, 0);
+		input_sync(input);
+		break;
+
+	case GPIO_SCROLLWHEEL_PIN_ROT1:
+	case GPIO_SCROLLWHEEL_PIN_ROT2:
+		state2 = (gpio_get_value(bdata->rotgpio) ? 1 : 0) \
+					^ button->active_low;
+		if (state != state2) {
+			input_report_key(input, KEY_DOWN, 1);
+			input_report_key(input, KEY_DOWN, 0);
+		} else {
+			input_report_key(input, KEY_UP, 1);
+			input_report_key(input, KEY_UP, 0);
+		}
+		input_sync(input);
+		break;
+
+	default:
+		pr_err("%s:Line=%d, Invalid Pinaction\n", __func__, __LINE__);
+	}
+}
+
+static void scrollwheel_work_func(struct work_struct *work)
+{
+	struct scrollwheel_button_data *bdata =
+		container_of(work, struct scrollwheel_button_data, work);
+
+	scrollwheel_report_key(bdata);
+}
+
+static void scrollwheel_timer(unsigned long _data)
+{
+	struct scrollwheel_button_data *data = \
+		(struct scrollwheel_button_data *)_data;
+
+	schedule_work(&data->work);
+}
+
+static irqreturn_t scrollwheel_isr(int irq, void *dev_id)
+{
+	struct scrollwheel_button_data *bdata = dev_id;
+	struct gpio_scrollwheel_button *button = bdata->button;
+
+	BUG_ON(irq != gpio_to_irq(button->gpio));
+
+	if (bdata->timer_debounce)
+		mod_timer(&bdata->timer,
+			jiffies + msecs_to_jiffies(bdata->timer_debounce));
+	else
+		schedule_work(&bdata->work);
+
+	return IRQ_HANDLED;
+}
+
+static int __devinit gpio_scrollwheel_setup_key(struct platform_device *pdev,
+			struct scrollwheel_button_data *bdata,
+			struct gpio_scrollwheel_button *button)
+{
+	char *desc = button->desc ? button->desc : "gpio_scrollwheel";
+	struct device *dev = &pdev->dev;
+	unsigned long irqflags;
+	int irq, error;
+
+	setup_timer(&bdata->timer, scrollwheel_timer, (unsigned long)bdata);
+	INIT_WORK(&bdata->work, scrollwheel_work_func);
+
+	error = gpio_request(button->gpio, desc);
+	if (error < 0) {
+		dev_err(dev, "failed to request GPIO %d, error %d\n",
+			button->gpio, error);
+		return error;
+	}
+
+	error = gpio_direction_input(button->gpio);
+	if (error < 0) {
+		dev_err(dev, "failed to configure"
+			" direction for GPIO %d, error %d\n",
+			button->gpio, error);
+		goto fail;
+	}
+
+	if (button->debounce_interval) {
+		error = gpio_set_debounce(button->gpio,
+					  button->debounce_interval * 1000);
+		/* use timer if gpiolib doesn't provide debounce */
+		if (error < 0)
+			bdata->timer_debounce = button->debounce_interval;
+	}
+
+	irq = gpio_to_irq(button->gpio);
+	if (irq < 0) {
+		error = irq;
+		dev_err(dev, "Unable to get irq no for GPIO %d, error %d\n",
+			button->gpio, error);
+		goto fail;
+	}
+
+	irqflags = IRQF_TRIGGER_FALLING;
+
+	error = request_irq(irq, scrollwheel_isr, irqflags, desc, bdata);
+	if (error) {
+		dev_err(dev, "Unable to claim irq %d; error %d\n",
+			irq, error);
+		goto fail;
+	}
+
+	return 0;
+
+fail:
+	return error;
+}
+
+static int gpio_scrollwheel_open(struct input_dev *input)
+{
+	struct gpio_scrollwheel_drvdata *ddata = input_get_drvdata(input);
+
+	return ddata->enable ? ddata->enable(input->dev.parent) : 0;
+}
+
+static void gpio_scrollwheel_close(struct input_dev *input)
+{
+	struct gpio_scrollwheel_drvdata *ddata = input_get_drvdata(input);
+
+	if (ddata->disable)
+		ddata->disable(input->dev.parent);
+}
+
+static int __devinit gpio_scrollwheel_probe(struct platform_device *pdev)
+{
+	struct gpio_scrollwheel_platform_data *pdata = pdev->dev.platform_data;
+	struct gpio_scrollwheel_drvdata *ddata;
+	struct device *dev = &pdev->dev;
+	struct input_dev *input;
+	int i, error;
+
+	ddata = kzalloc(sizeof(struct gpio_scrollwheel_drvdata) +
+		pdata->nbuttons * sizeof(struct scrollwheel_button_data),
+		GFP_KERNEL);
+	if (ddata == NULL) {
+		dev_err(dev, "failed to allocate memory\n");
+		error = -ENOMEM;
+		return error;
+	}
+
+	input = input_allocate_device();
+	if (input == NULL) {
+		dev_err(dev, "failed to allocate input device\n");
+		error = -ENOMEM;
+		kfree(ddata);
+		return error;
+	}
+
+	ddata->input = input;
+	ddata->n_buttons = pdata->nbuttons;
+	ddata->enable = pdata->enable;
+	ddata->disable = pdata->disable;
+	mutex_init(&ddata->disable_lock);
+
+	platform_set_drvdata(pdev, ddata);
+	input_set_drvdata(input, ddata);
+
+	input->name = pdev->name;
+	input->phys = "gpio-scrollwheel/input0";
+	input->dev.parent = &pdev->dev;
+	input->open = gpio_scrollwheel_open;
+	input->close = gpio_scrollwheel_close;
+
+	input->id.bustype = BUS_HOST;
+	input->id.vendor = 0x0001;
+	input->id.product = 0x0001;
+	input->id.version = 0x0100;
+
+	/* Enable auto repeat feature of Linux input subsystem */
+	if (pdata->rep)
+		__set_bit(EV_REP, input->evbit);
+
+	for (i = 0; i < pdata->nbuttons; i++) {
+		struct gpio_scrollwheel_button *button = &pdata->buttons[i];
+		struct scrollwheel_button_data *bdata = &ddata->data[i];
+
+		bdata->input = input;
+		bdata->button = button;
+
+		if (button->pinaction == GPIO_SCROLLWHEEL_PIN_PRESS ||
+			button->pinaction == GPIO_SCROLLWHEEL_PIN_ROT1) {
+			error = gpio_scrollwheel_setup_key(pdev, bdata, button);
+			if (error)
+				goto fail;
+		} else {
+			if (button->pinaction == GPIO_SCROLLWHEEL_PIN_ONOFF) {
+				gpio_request(button->gpio, button->desc);
+				gpio_direction_output(button->gpio, 0);
+			}
+
+			if (button->pinaction == GPIO_SCROLLWHEEL_PIN_ROT2) {
+				gpio_request(button->gpio, button->desc);
+				gpio_direction_input(button->gpio);
+				/* Save rot2 gpio number in rot1 context */
+				ddata->data[2].rotgpio = button->gpio;
+			}
+		}
+	}
+
+	/* set input capability */
+	__set_bit(EV_KEY, input->evbit);
+	__set_bit(KEY_ENTER, input->keybit);
+	__set_bit(KEY_UP, input->keybit);
+	__set_bit(KEY_DOWN, input->keybit);
+
+	error = input_register_device(input);
+	if (error) {
+		dev_err(dev, "Unable to register input device, error: %d\n",
+			error);
+		goto fail;
+	}
+
+	input_sync(input);
+
+	return 0;
+
+fail:
+	while (--i >= 0) {
+		if (pdata->buttons[i].pinaction == GPIO_SCROLLWHEEL_PIN_PRESS ||
+			pdata->buttons[i].pinaction == GPIO_SCROLLWHEEL_PIN_ROT1) {
+			free_irq(gpio_to_irq(pdata->buttons[i].gpio), &ddata->data[i]);
+			if (ddata->data[i].timer_debounce)
+				del_timer_sync(&ddata->data[i].timer);
+			cancel_work_sync(&ddata->data[i].work);
+		}
+		gpio_free(pdata->buttons[i].gpio);
+	}
+
+	platform_set_drvdata(pdev, NULL);
+	input_free_device(input);
+	kfree(ddata);
+	return error;
+}
+
+static int __devexit gpio_scrollwheel_remove(struct platform_device *pdev)
+{
+	struct gpio_scrollwheel_platform_data *pdata = pdev->dev.platform_data;
+	struct gpio_scrollwheel_drvdata *ddata = platform_get_drvdata(pdev);
+	struct input_dev *input = ddata->input;
+	int i;
+
+	for (i = 0; i < pdata->nbuttons; i++) {
+		if (pdata->buttons[i].pinaction == GPIO_SCROLLWHEEL_PIN_PRESS ||
+			pdata->buttons[i].pinaction == GPIO_SCROLLWHEEL_PIN_ROT1) {
+			int irq = gpio_to_irq(pdata->buttons[i].gpio);
+			free_irq(irq, &ddata->data[i]);
+			if (ddata->data[i].timer_debounce)
+				del_timer_sync(&ddata->data[i].timer);
+			cancel_work_sync(&ddata->data[i].work);
+		}
+		gpio_free(pdata->buttons[i].gpio);
+	}
+
+	input_unregister_device(input);
+
+	return 0;
+}
+
+
+#ifdef CONFIG_PM
+static int gpio_scrollwheel_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct gpio_scrollwheel_platform_data *pdata = pdev->dev.platform_data;
+	struct gpio_scrollwheel_drvdata *ddata = platform_get_drvdata(pdev);
+	int i, irq;
+
+	for (i = 0; i < pdata->nbuttons; i++) {
+		if (pdata->buttons[i].pinaction == GPIO_SCROLLWHEEL_PIN_PRESS ||
+			pdata->buttons[i].pinaction == GPIO_SCROLLWHEEL_PIN_ROT1) {
+			irq = gpio_to_irq(pdata->buttons[i].gpio);
+			disable_irq(irq);
+			if (ddata->data[i].timer_debounce)
+				del_timer_sync(&ddata->data[i].timer);
+			cancel_work_sync(&ddata->data[i].work);
+		} else {
+			if (pdata->buttons[i].pinaction == GPIO_SCROLLWHEEL_PIN_ONOFF)
+				gpio_direction_output(pdata->buttons[i].gpio, 1);
+			else {
+				irq = gpio_to_irq(pdata->buttons[i].gpio);
+				disable_irq(irq);
+			}
+		}
+	}
+	return 0;
+}
+
+static int gpio_scrollwheel_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct gpio_scrollwheel_platform_data *pdata = pdev->dev.platform_data;
+	struct gpio_scrollwheel_drvdata *ddata = platform_get_drvdata(pdev);
+	int i, irq;
+
+	for (i = 0; i < pdata->nbuttons; i++) {
+		if (pdata->buttons[i].pinaction == GPIO_SCROLLWHEEL_PIN_PRESS ||
+			pdata->buttons[i].pinaction == GPIO_SCROLLWHEEL_PIN_ROT1) {
+			irq = gpio_to_irq(pdata->buttons[i].gpio);
+			enable_irq(irq);
+			if (ddata->data[i].timer_debounce)
+				setup_timer(&ddata->data[i].timer,\
+				scrollwheel_timer, (unsigned long)&ddata->data[i]);
+
+			INIT_WORK(&ddata->data[i].work, scrollwheel_work_func);
+		} else {
+			if (pdata->buttons[i].pinaction == GPIO_SCROLLWHEEL_PIN_ONOFF)
+				gpio_direction_output(pdata->buttons[i].gpio, 0);
+			else {
+				irq = gpio_to_irq(pdata->buttons[i].gpio);
+				enable_irq(irq);
+			}
+		}
+	}
+
+	return 0;
+}
+
+static const struct dev_pm_ops gpio_scrollwheel_pm_ops = {
+	.suspend	= gpio_scrollwheel_suspend,
+	.resume		= gpio_scrollwheel_resume,
+};
+#endif
+
+static struct platform_driver gpio_scrollwheel_device_driver = {
+	.probe		= gpio_scrollwheel_probe,
+	.remove		= __devexit_p(gpio_scrollwheel_remove),
+	.driver		= {
+		.name	= "alps-gpio-scrollwheel",
+		.owner	= THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm	= &gpio_scrollwheel_pm_ops,
+#endif
+	}
+};
+
+static int __init gpio_scrollwheel_init(void)
+{
+	return platform_driver_register(&gpio_scrollwheel_device_driver);
+}
+
+static void __exit gpio_scrollwheel_exit(void)
+{
+	platform_driver_unregister(&gpio_scrollwheel_device_driver);
+}
+
+module_init(gpio_scrollwheel_init);
+module_exit(gpio_scrollwheel_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_DESCRIPTION("Alps SRBE ScrollWheel driver");
+
+MODULE_ALIAS("platform:alps-gpio-scrollwheel");
diff --git a/drivers/input/misc/cm3217.c b/drivers/input/misc/cm3217.c
new file mode 100644
index 000000000000..416d0b3806a9
--- /dev/null
+++ b/drivers/input/misc/cm3217.c
@@ -0,0 +1,1084 @@
+/* drivers/input/misc/cm3217.c - cm3217 optical sensors driver
+ *
+ * Copyright (C) 2011 Capella Microsystems Inc.
+ * Author: Frank Hsieh <pengyueh@gmail.com>
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/delay.h>
+#include <linux/earlysuspend.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+#include <linux/irq.h>
+#include <linux/errno.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/miscdevice.h>
+#include <linux/lightsensor.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/cm3217.h>
+#include <linux/wakelock.h>
+#include <linux/jiffies.h>
+#include <asm/mach-types.h>
+#include <asm/setup.h>
+
+#define D(x...)			pr_debug(x)
+
+#define I2C_RETRY_COUNT		10
+
+#define LS_POLLING_DELAY	1000 /* mSec */
+
+static void report_do_work(struct work_struct *w);
+static DECLARE_DELAYED_WORK(report_work, report_do_work);
+
+struct cm3217_info {
+	struct class *cm3217_class;
+	struct device *ls_dev;
+	struct input_dev *ls_input_dev;
+
+	struct early_suspend early_suspend;
+	struct i2c_client *i2c_client;
+	struct workqueue_struct *lp_wq;
+
+	int als_enable;
+	int als_enabled_before_suspend;
+	uint16_t *adc_table;
+	uint16_t cali_table[10];
+	int irq;
+	int ls_calibrate;
+	int (*power) (int, uint8_t);	/* power to the chip */
+
+	uint32_t als_kadc;
+	uint32_t als_gadc;
+	uint16_t golden_adc;
+
+	int lightsensor_opened;
+	int current_level;
+	uint16_t current_adc;
+	int polling_delay;
+};
+
+struct cm3217_info *lp_info;
+
+int enable_log;
+int fLevel = -1;
+
+static struct mutex als_enable_mutex, als_disable_mutex, als_get_adc_mutex;
+
+static int lightsensor_enable(struct cm3217_info *lpi);
+static int lightsensor_disable(struct cm3217_info *lpi);
+
+int32_t als_kadc;
+
+static int I2C_RxData(uint16_t slaveAddr, uint8_t *rxData, int length)
+{
+	uint8_t loop_i;
+
+	struct i2c_msg msgs[] = {
+		{
+			.addr = slaveAddr,
+			.flags = I2C_M_RD,
+			.len = length,
+			.buf = rxData,
+		},
+	};
+
+	for (loop_i = 0; loop_i < I2C_RETRY_COUNT; loop_i++) {
+		if (i2c_transfer(lp_info->i2c_client->adapter, msgs, 1) > 0)
+			break;
+		msleep(10);
+	}
+
+	if (loop_i >= I2C_RETRY_COUNT) {
+		printk(KERN_ERR "[ERR][CM3217 error] %s retry over %d\n",
+		       __func__, I2C_RETRY_COUNT);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int I2C_TxData(uint16_t slaveAddr, uint8_t *txData, int length)
+{
+	uint8_t loop_i;
+
+	struct i2c_msg msg[] = {
+		{
+			.addr = slaveAddr,
+			.flags = 0,
+			.len = length,
+			.buf = txData,
+		},
+	};
+
+	for (loop_i = 0; loop_i < I2C_RETRY_COUNT; loop_i++) {
+		if (i2c_transfer(lp_info->i2c_client->adapter, msg, 1) > 0)
+			break;
+		msleep(10);
+	}
+
+	if (loop_i >= I2C_RETRY_COUNT) {
+		printk(KERN_ERR "[ERR][CM3217 error] %s retry over %d\n",
+		       __func__, I2C_RETRY_COUNT);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int _cm3217_I2C_Read_Byte(uint16_t slaveAddr, uint8_t *pdata)
+{
+	uint8_t buffer = 0;
+	int ret = 0;
+
+	if (pdata == NULL)
+		return -EFAULT;
+
+	ret = I2C_RxData(slaveAddr, &buffer, 1);
+	if (ret < 0) {
+		pr_err("[ERR][CM3217 error]%s: I2C_RxData fail, slave addr: 0x%x\n",
+		       __func__, slaveAddr);
+		return ret;
+	}
+
+	*pdata = buffer;
+
+#if 0
+	/* Debug use */
+	printk(KERN_DEBUG "[CM3217] %s: I2C_RxData[0x%x] = 0x%x\n",
+	       __func__, slaveAddr, buffer);
+#endif
+
+	return ret;
+}
+
+static int _cm3217_I2C_Write_Byte(uint16_t SlaveAddress, uint8_t data)
+{
+	char buffer[2];
+	int ret = 0;
+
+#if 0
+	/* Debug use */
+	printk(KERN_DEBUG
+	       "[CM3217] %s: _cm3217_I2C_Write_Byte[0x%x, 0x%x, 0x%x]\n",
+	       __func__, SlaveAddress, cmd, data);
+#endif
+
+	buffer[0] = data;
+
+	ret = I2C_TxData(SlaveAddress, buffer, 1);
+	if (ret < 0) {
+		pr_err("[ERR][CM3217 error]%s: I2C_TxData fail\n", __func__);
+		return -EIO;
+	}
+
+	return ret;
+}
+
+static int get_ls_adc_value(uint16_t *als_step, bool resume)
+{
+	uint8_t lsb, msb;
+	int ret = 0;
+	struct cm3217_info *lpi = lp_info;
+
+	if (als_step == NULL)
+		return -EFAULT;
+
+	/* Read ALS data: LSB */
+	ret = _cm3217_I2C_Read_Byte(ALS_R_LSB_addr, &lsb);
+	if (ret < 0) {
+		pr_err("[LS][CM3217 error]%s: _cm3217_I2C_Read_Byte LSB fail\n",
+		       __func__);
+		return -EIO;
+	}
+
+	/* Read ALS data: MSB */
+	ret = _cm3217_I2C_Read_Byte(ALS_R_MSB_addr, &msb);
+	if (ret < 0) {
+		pr_err("[LS][CM3217 error]%s: _cm3217_I2C_Read_Byte MSB fail\n",
+		       __func__);
+		return -EIO;
+	}
+
+	*als_step = (uint16_t) msb;
+	*als_step <<= 8;
+	*als_step |= (uint16_t) lsb;
+
+	D("[LS][CM3217] %s: raw adc = 0x%X\n", __func__, *als_step);
+
+	if (!lpi->ls_calibrate) {
+		*als_step = (*als_step) * lpi->als_gadc / lpi->als_kadc;
+		if (*als_step > 0xFFFF)
+			*als_step = 0xFFFF;
+	}
+
+	return ret;
+}
+
+static void report_lsensor_input_event(struct cm3217_info *lpi, bool resume)
+{
+	uint16_t adc_value = 0;
+	int level = 0, i, ret = 0;
+
+	mutex_lock(&als_get_adc_mutex);
+
+	ret = get_ls_adc_value(&adc_value, resume);
+
+	if (lpi->ls_calibrate) {
+		for (i = 0; i < 10; i++) {
+			if (adc_value <= (*(lpi->cali_table + i))) {
+				level = i;
+				if (*(lpi->cali_table + i))
+					break;
+			}
+			/* avoid i = 10, because 'cali_table' of size is 10 */
+			if (i == 9) {
+				level = i;
+				break;
+			}
+		}
+	} else {
+		for (i = 0; i < 10; i++) {
+			if (adc_value <= (*(lpi->adc_table + i))) {
+				level = i;
+				if (*(lpi->adc_table + i))
+					break;
+			}
+			/* avoid i = 10, because 'cali_table' of size is 10 */
+			if (i == 9) {
+				level = i;
+				break;
+			}
+		}
+	}
+
+	if ((i == 0) || (adc_value == 0))
+		D("[LS][CM3217] %s: ADC=0x%03X, Level=%d, l_thd equal 0, "
+		  "h_thd = 0x%x\n", __func__, adc_value, level,
+		  *(lpi->cali_table + i));
+	else
+		D("[LS][CM3217] %s: ADC=0x%03X, Level=%d, l_thd = 0x%x, "
+		  "h_thd = 0x%x\n", __func__, adc_value, level,
+		  *(lpi->cali_table + (i - 1)) + 1, *(lpi->cali_table + i));
+
+	lpi->current_level = level;
+	lpi->current_adc = adc_value;
+
+	/*
+	D("[CM3217] %s: *(lpi->cali_table + (i - 1)) + 1 = 0x%X, "
+	  "*(lpi->cali_table + i) = 0x%x\n", __func__,
+	  *(lpi->cali_table + (i - 1)) + 1, *(lpi->cali_table + i));
+	*/
+
+	if (fLevel >= 0) {
+		D("[LS][CM3217] L-sensor force level enable level=%d "
+		  "fLevel=%d\n", level, fLevel);
+		level = fLevel;
+	}
+
+	input_report_abs(lpi->ls_input_dev, ABS_MISC, level);
+	input_sync(lpi->ls_input_dev);
+
+	mutex_unlock(&als_get_adc_mutex);
+}
+
+static void report_do_work(struct work_struct *work)
+{
+	struct cm3217_info *lpi = lp_info;
+
+	if (enable_log)
+		D("[CM3217] %s\n", __func__);
+
+	report_lsensor_input_event(lpi, 0);
+
+	queue_delayed_work(lpi->lp_wq, &report_work, lpi->polling_delay);
+}
+
+static int als_power(int enable)
+{
+	struct cm3217_info *lpi = lp_info;
+
+	if (lpi->power)
+		lpi->power(LS_PWR_ON, 1);
+
+	return 0;
+}
+
+void lightsensor_set_kvalue(struct cm3217_info *lpi)
+{
+	if (!lpi) {
+		pr_err("[LS][CM3217 error]%s: ls_info is empty\n", __func__);
+		return;
+	}
+
+	D("[LS][CM3217] %s: ALS calibrated als_kadc=0x%x\n",
+	  __func__, als_kadc);
+
+	if (als_kadc >> 16 == ALS_CALIBRATED)
+		lpi->als_kadc = als_kadc & 0xFFFF;
+	else {
+		lpi->als_kadc = 0;
+		D("[LS][CM3217] %s: no ALS calibrated\n", __func__);
+	}
+
+	if (lpi->als_kadc && lpi->golden_adc > 0) {
+		lpi->als_kadc = (lpi->als_kadc > 0 && lpi->als_kadc < 0x1000) ?
+				lpi->als_kadc : lpi->golden_adc;
+		lpi->als_gadc = lpi->golden_adc;
+	} else {
+		lpi->als_kadc = 1;
+		lpi->als_gadc = 1;
+	}
+
+	D("[LS][CM3217] %s: als_kadc=0x%x, als_gadc=0x%x\n",
+	  __func__, lpi->als_kadc, lpi->als_gadc);
+}
+
+static int lightsensor_update_table(struct cm3217_info *lpi)
+{
+	uint32_t tmpData[10];
+	int i;
+
+	for (i = 0; i < 10; i++) {
+		tmpData[i] = (uint32_t) (*(lpi->adc_table + i))
+			     * lpi->als_kadc / lpi->als_gadc;
+		if (tmpData[i] <= 0xFFFF)
+			lpi->cali_table[i] = (uint16_t) tmpData[i];
+		else
+			lpi->cali_table[i] = 0xFFFF;
+		D("[LS][CM3217] %s: Calibrated adc_table: data[%d], %x\n",
+		  __func__, i, lpi->cali_table[i]);
+	}
+
+	return 0;
+}
+
+static int lightsensor_enable(struct cm3217_info *lpi)
+{
+	int ret = 0;
+	uint8_t cmd = 0;
+
+	mutex_lock(&als_enable_mutex);
+
+	D("[LS][CM3217] %s\n", __func__);
+
+	cmd = (CM3217_ALS_IT_2_T | CM3217_ALS_BIT5_Default_1 |
+	       CM3217_ALS_WDM_DEFAULT_1);
+	ret = _cm3217_I2C_Write_Byte(ALS_W_CMD1_addr, cmd);
+	if (ret < 0)
+		pr_err("[LS][CM3217 error]%s: set auto light sensor fail\n",
+		       __func__);
+
+	queue_work(lpi->lp_wq, &report_work);
+	lpi->als_enable = 1;
+
+	mutex_unlock(&als_enable_mutex);
+
+	return ret;
+}
+
+static int lightsensor_disable(struct cm3217_info *lpi)
+{
+	int ret = 0;
+	char cmd = 0;
+
+	mutex_lock(&als_disable_mutex);
+
+	D("[LS][CM3217] %s\n", __func__);
+
+	cmd = (CM3217_ALS_IT_2_T | CM3217_ALS_BIT5_Default_1 |
+	       CM3217_ALS_WDM_DEFAULT_1 | CM3217_ALS_SD);
+	ret = _cm3217_I2C_Write_Byte(ALS_W_CMD1_addr, cmd);
+	if (ret < 0)
+		pr_err("[LS][CM3217 error]%s: disable auto light sensor fail\n",
+		       __func__);
+	else {
+		lpi->als_enable = 0;
+	}
+
+	cancel_delayed_work(&report_work);
+	lpi->als_enable = 0;
+
+	mutex_unlock(&als_disable_mutex);
+
+	return ret;
+}
+
+static int lightsensor_open(struct inode *inode, struct file *file)
+{
+	struct cm3217_info *lpi = lp_info;
+	int rc = 0;
+
+	D("[LS][CM3217] %s\n", __func__);
+	if (lpi->lightsensor_opened) {
+		pr_err("[LS][CM3217 error]%s: already opened\n", __func__);
+		rc = -EBUSY;
+	}
+	lpi->lightsensor_opened = 1;
+
+	return rc;
+}
+
+static int lightsensor_release(struct inode *inode, struct file *file)
+{
+	struct cm3217_info *lpi = lp_info;
+
+	D("[LS][CM3217] %s\n", __func__);
+	lpi->lightsensor_opened = 0;
+
+	return 0;
+}
+
+static long lightsensor_ioctl(struct file *file, unsigned int cmd,
+			      unsigned long arg)
+{
+	int rc = 0;
+	int val;
+	struct cm3217_info *lpi = lp_info;
+	unsigned long delay;
+
+	/* D("[CM3217] %s cmd %d\n", __func__, _IOC_NR(cmd)); */
+
+	switch (cmd) {
+	case LIGHTSENSOR_IOCTL_ENABLE:
+		if (get_user(val, (unsigned long __user *)arg)) {
+			rc = -EFAULT;
+			break;
+		}
+		D("[LS][CM3217] %s LIGHTSENSOR_IOCTL_ENABLE, value = %d\n",
+		  __func__, val);
+		rc = val ? lightsensor_enable(lpi) : lightsensor_disable(lpi);
+		break;
+
+	case LIGHTSENSOR_IOCTL_GET_ENABLED:
+		val = lpi->als_enable;
+		D("[LS][CM3217] %s LIGHTSENSOR_IOCTL_GET_ENABLED, enabled %d\n",
+		  __func__, val);
+		rc = put_user(val, (unsigned long __user *)arg);
+		break;
+
+	case LIGHTSENSOR_IOCTL_SET_DELAY:
+		if (get_user(delay, (unsigned long __user *)arg)) {
+			rc = -EFAULT;
+			break;
+		}
+		D("[LS][CM3217] %s LIGHTSENSOR_IOCTL_SET_DELAY, delay %ld\n",
+			__func__, delay);
+		delay = delay / 1000;
+		lpi->polling_delay = msecs_to_jiffies(delay);
+		break;
+
+	default:
+		pr_err("[LS][CM3217 error]%s: invalid cmd %d\n",
+		       __func__, _IOC_NR(cmd));
+		rc = -EINVAL;
+	}
+
+	return rc;
+}
+
+static const struct file_operations lightsensor_fops = {
+	.owner = THIS_MODULE,
+	.open = lightsensor_open,
+	.release = lightsensor_release,
+	.unlocked_ioctl = lightsensor_ioctl
+};
+
+static struct miscdevice lightsensor_misc = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "lightsensor",
+	.fops = &lightsensor_fops
+};
+
+static ssize_t ls_adc_show(struct device *dev,
+			   struct device_attribute *attr, char *buf)
+{
+	int ret;
+	struct cm3217_info *lpi = lp_info;
+
+	D("[LS][CM3217] %s: ADC = 0x%04X, Level = %d\n",
+	  __func__, lpi->current_adc, lpi->current_level);
+
+	ret = sprintf(buf, "ADC[0x%04X] => level %d\n",
+		      lpi->current_adc, lpi->current_level);
+
+	return ret;
+}
+
+static DEVICE_ATTR(ls_adc, 0664, ls_adc_show, NULL);
+
+static ssize_t ls_enable_show(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	int ret = 0;
+	struct cm3217_info *lpi = lp_info;
+
+	ret = sprintf(buf, "Light sensor Auto Enable = %d\n", lpi->als_enable);
+
+	return ret;
+}
+
+static ssize_t ls_enable_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	int ret = 0;
+	int ls_auto;
+	struct cm3217_info *lpi = lp_info;
+
+	ls_auto = -1;
+	sscanf(buf, "%d", &ls_auto);
+
+	if (ls_auto != 0 && ls_auto != 1)
+		return -EINVAL;
+
+	if (ls_auto)
+		ret = lightsensor_enable(lpi);
+	else
+		ret = lightsensor_disable(lpi);
+
+	D("[LS][CM3217] %s: lpi->als_enable = %d, lpi->ls_calibrate = %d, "
+	  "ls_auto=%d\n", __func__, lpi->als_enable, lpi->ls_calibrate,
+	  ls_auto);
+
+	if (ret < 0)
+		pr_err("[LS][CM3217 error]%s: set auto light sensor fail\n",
+		       __func__);
+
+	return count;
+}
+
+static DEVICE_ATTR(ls_auto, 0664, ls_enable_show, ls_enable_store);
+
+static ssize_t ls_kadc_show(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	struct cm3217_info *lpi = lp_info;
+	int ret;
+
+	ret = sprintf(buf, "kadc = 0x%x", lpi->als_kadc);
+
+	return ret;
+}
+
+static ssize_t ls_kadc_store(struct device *dev,
+			     struct device_attribute *attr,
+			     const char *buf, size_t count)
+{
+	struct cm3217_info *lpi = lp_info;
+	int kadc_temp = 0;
+
+	sscanf(buf, "%d", &kadc_temp);
+
+	/* if (kadc_temp <= 0 || lpi->golden_adc <= 0) {
+		printk(KERN_ERR "[LS][CM3217 error] %s: kadc_temp=0x%x, "
+		       "als_gadc=0x%x\n", __func__, kadc_temp,
+		       lpi->golden_adc);
+		return -EINVAL;
+	} */
+
+	mutex_lock(&als_get_adc_mutex);
+
+	if (kadc_temp != 0) {
+		lpi->als_kadc = kadc_temp;
+		if (lpi->als_gadc != 0) {
+			if (lightsensor_update_table(lpi) < 0)
+				printk(KERN_ERR
+				       "[LS][CM3217 error] %s: "
+				       "update ls table fail\n", __func__);
+		} else {
+			printk(KERN_INFO
+			       "[LS]%s: als_gadc =0x%x wait to be set\n",
+			       __func__, lpi->als_gadc);
+		}
+	} else {
+		printk(KERN_INFO "[LS]%s: als_kadc can't be set to zero\n",
+		       __func__);
+	}
+
+	mutex_unlock(&als_get_adc_mutex);
+
+	return count;
+}
+
+static DEVICE_ATTR(ls_kadc, 0664, ls_kadc_show, ls_kadc_store);
+
+static ssize_t ls_gadc_show(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	struct cm3217_info *lpi = lp_info;
+	int ret;
+
+	ret = sprintf(buf, "gadc = 0x%x\n", lpi->als_gadc);
+
+	return ret;
+}
+
+static ssize_t ls_gadc_store(struct device *dev,
+			     struct device_attribute *attr,
+			     const char *buf, size_t count)
+{
+	struct cm3217_info *lpi = lp_info;
+	int gadc_temp = 0;
+
+	sscanf(buf, "%d", &gadc_temp);
+
+	/* if (gadc_temp <= 0 || lpi->golden_adc <= 0) {
+		printk(KERN_ERR "[LS][CM3217 error] %s: kadc_temp=0x%x, "
+		       "als_gadc=0x%x\n", __func__, kadc_temp,
+		       lpi->golden_adc);
+		return -EINVAL;
+	} */
+
+	mutex_lock(&als_get_adc_mutex);
+
+	if (gadc_temp != 0) {
+		lpi->als_gadc = gadc_temp;
+		if (lpi->als_kadc != 0) {
+			if (lightsensor_update_table(lpi) < 0)
+				printk(KERN_ERR
+				       "[LS][CM3217 error] %s: "
+				       "update ls table fail\n", __func__);
+		} else {
+			printk(KERN_INFO
+			       "[LS]%s: als_kadc =0x%x wait to be set\n",
+			       __func__, lpi->als_kadc);
+		}
+	} else {
+		printk(KERN_INFO "[LS]%s: als_gadc can't be set to zero\n",
+		       __func__);
+	}
+
+	mutex_unlock(&als_get_adc_mutex);
+
+	return count;
+}
+
+static DEVICE_ATTR(ls_gadc, 0664, ls_gadc_show, ls_gadc_store);
+
+static ssize_t ls_adc_table_show(struct device *dev,
+				 struct device_attribute *attr, char *buf)
+{
+	unsigned length = 0;
+	int i;
+
+	for (i = 0; i < 10; i++) {
+		length += sprintf(buf + length,
+				  "[CM3217]Get adc_table[%d] =  0x%x ; %d, "
+				  "Get cali_table[%d] =  0x%x ; %d,\n",
+				  i, *(lp_info->adc_table + i),
+				  *(lp_info->adc_table + i),
+				  i, *(lp_info->cali_table + i),
+				  *(lp_info->cali_table + i));
+	}
+
+	return length;
+}
+
+static ssize_t ls_adc_table_store(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t count)
+{
+	struct cm3217_info *lpi = lp_info;
+	char *token[10];
+	unsigned long tempdata[10];
+	int i, r;
+
+	printk(KERN_INFO "[LS][CM3217]%s\n", buf);
+	for (i = 0; i < 10; i++) {
+		token[i] = strsep((char **)&buf, " ");
+		r = kstrtoul(token[i], 16, &tempdata[i]);
+		if (tempdata[i] < 1 || tempdata[i] > 0xffff || r) {
+			printk(KERN_ERR
+			       "[LS][CM3217 error] adc_table[%d] = "
+			       "0x%lx Err\n", i, tempdata[i]);
+			return count;
+		}
+	}
+
+	mutex_lock(&als_get_adc_mutex);
+
+	for (i = 0; i < 10; i++) {
+		lpi->adc_table[i] = tempdata[i];
+		printk(KERN_INFO
+		       "[LS][CM3217]Set lpi->adc_table[%d] =  0x%x\n",
+		       i, *(lp_info->adc_table + i));
+	}
+	if (lightsensor_update_table(lpi) < 0)
+		printk(KERN_ERR "[LS][CM3217 error] %s: update ls table fail\n",
+		       __func__);
+
+	mutex_unlock(&als_get_adc_mutex);
+
+	D("[LS][CM3217] %s\n", __func__);
+
+	return count;
+}
+
+static DEVICE_ATTR(ls_adc_table, 0664, ls_adc_table_show, ls_adc_table_store);
+
+static uint8_t ALS_CONF1;
+
+static ssize_t ls_conf1_show(struct device *dev,
+			     struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "ALS_CONF1 = %x\n", ALS_CONF1);
+}
+
+static ssize_t ls_conf1_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t count)
+{
+	int value = 0;
+
+	sscanf(buf, "0x%x", &value);
+
+	ALS_CONF1 = value;
+	printk(KERN_INFO "[LS]set ALS_CONF1 = %x\n", ALS_CONF1);
+	_cm3217_I2C_Write_Byte(ALS_W_CMD1_addr, ALS_CONF1);
+
+	return count;
+}
+
+static DEVICE_ATTR(ls_conf1, 0664, ls_conf1_show, ls_conf1_store);
+
+static uint8_t ALS_CONF2;
+static ssize_t ls_conf2_show(struct device *dev,
+			     struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "ALS_CONF2 = %x\n", ALS_CONF2);
+}
+
+static ssize_t ls_conf2_store(struct device *dev,
+			      struct device_attribute *attr,
+			      const char *buf, size_t count)
+{
+	int value = 0;
+
+	sscanf(buf, "0x%x", &value);
+
+	ALS_CONF2 = value;
+	printk(KERN_INFO "[LS]set ALS_CONF2 = %x\n", ALS_CONF2);
+	_cm3217_I2C_Write_Byte(ALS_W_CMD2_addr, ALS_CONF2);
+
+	return count;
+}
+
+static DEVICE_ATTR(ls_conf2, 0664, ls_conf2_show, ls_conf2_store);
+
+static ssize_t ls_fLevel_show(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "fLevel = %d\n", fLevel);
+}
+
+static ssize_t ls_fLevel_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	struct cm3217_info *lpi = lp_info;
+	int value = 0;
+
+	sscanf(buf, "%d", &value);
+	(value >= 0) ? (value = min(value, 10)) : (value = max(value, -1));
+	fLevel = value;
+
+	input_report_abs(lpi->ls_input_dev, ABS_MISC, fLevel);
+	input_sync(lpi->ls_input_dev);
+
+	printk(KERN_INFO "[LS]set fLevel = %d\n", fLevel);
+
+	msleep(1000);
+	fLevel = -1;
+
+	return count;
+}
+
+static DEVICE_ATTR(ls_flevel, 0664, ls_fLevel_show, ls_fLevel_store);
+
+static int lightsensor_setup(struct cm3217_info *lpi)
+{
+	int ret;
+
+	lpi->ls_input_dev = input_allocate_device();
+	if (!lpi->ls_input_dev) {
+		pr_err("[LS][CM3217 error]%s: "
+		       "could not allocate ls input device\n", __func__);
+		return -ENOMEM;
+	}
+	lpi->ls_input_dev->name = "cm3217-ls";
+	set_bit(EV_ABS, lpi->ls_input_dev->evbit);
+	input_set_abs_params(lpi->ls_input_dev, ABS_MISC, 0, 9, 0, 0);
+
+	ret = input_register_device(lpi->ls_input_dev);
+	if (ret < 0) {
+		pr_err("[LS][CM3217 error]%s: "
+		       "can not register ls input device\n", __func__);
+		goto err_free_ls_input_device;
+	}
+
+	ret = misc_register(&lightsensor_misc);
+	if (ret < 0) {
+		pr_err("[LS][CM3217 error]%s: "
+		       "can not register ls misc device\n", __func__);
+		goto err_unregister_ls_input_device;
+	}
+
+	return ret;
+
+err_unregister_ls_input_device:
+	input_unregister_device(lpi->ls_input_dev);
+err_free_ls_input_device:
+	input_free_device(lpi->ls_input_dev);
+	return ret;
+}
+
+static int cm3217_setup(struct cm3217_info *lpi)
+{
+	int ret = 0;
+
+	als_power(1);
+	msleep(5);
+
+	ret = _cm3217_I2C_Write_Byte(ALS_W_CMD2_addr,
+				     CM3217_ALS_WDM_DEFAULT_1
+				     | CM3217_ALS_IT_2_T
+				     | CM3217_ALS_BIT5_Default_1);
+	if (ret < 0)
+		return ret;
+
+	ret = _cm3217_I2C_Write_Byte(ALS_W_CMD2_addr, CM3217_ALS_IT_100ms);
+	msleep(10);
+
+	return ret;
+}
+
+static void cm3217_early_suspend(struct early_suspend *h)
+{
+	struct cm3217_info *lpi = lp_info;
+
+	D("[LS][CM3217] %s\n", __func__);
+
+	lpi->als_enabled_before_suspend = lpi->als_enable;
+	if (lpi->als_enable)
+		lightsensor_disable(lpi);
+}
+
+static void cm3217_late_resume(struct early_suspend *h)
+{
+	struct cm3217_info *lpi = lp_info;
+
+	D("[LS][CM3217] %s\n", __func__);
+
+	if (lpi->als_enabled_before_suspend)
+		lightsensor_enable(lpi);
+}
+
+static int cm3217_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int ret = 0;
+	struct cm3217_info *lpi;
+	struct cm3217_platform_data *pdata;
+
+	D("[CM3217] %s\n", __func__);
+
+	lpi = kzalloc(sizeof(struct cm3217_info), GFP_KERNEL);
+	if (!lpi)
+		return -ENOMEM;
+
+	/* D("[CM3217] %s: client->irq = %d\n", __func__, client->irq); */
+
+	lpi->i2c_client = client;
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		pr_err("[CM3217 error]%s: Assign platform_data error!!\n",
+		       __func__);
+		ret = -EBUSY;
+		goto err_platform_data_null;
+	}
+
+	lpi->irq = client->irq;
+
+	i2c_set_clientdata(client, lpi);
+	lpi->adc_table = pdata->levels;
+	lpi->golden_adc = pdata->golden_adc;
+	lpi->power = pdata->power;
+
+	lpi->polling_delay = msecs_to_jiffies(LS_POLLING_DELAY);
+
+	lp_info = lpi;
+
+	mutex_init(&als_enable_mutex);
+	mutex_init(&als_disable_mutex);
+	mutex_init(&als_get_adc_mutex);
+
+	ret = lightsensor_setup(lpi);
+	if (ret < 0) {
+		pr_err("[LS][CM3217 error]%s: lightsensor_setup error!!\n",
+		       __func__);
+		goto err_lightsensor_setup;
+	}
+
+	/* SET LUX STEP FACTOR HERE
+	 * if adc raw value one step = 0.3 lux
+	 * the following will set the factor 0.3 = 3/10
+	 * and lpi->golden_adc = 3;
+	 * set als_kadc = (ALS_CALIBRATED <<16) | 10; */
+
+	als_kadc = (ALS_CALIBRATED << 16) | 10;
+	lpi->golden_adc = 3;
+
+	/* ls calibrate always set to 1 */
+	lpi->ls_calibrate = 1;
+
+	lightsensor_set_kvalue(lpi);
+	ret = lightsensor_update_table(lpi);
+	if (ret < 0) {
+		pr_err("[LS][CM3217 error]%s: update ls table fail\n",
+		       __func__);
+		goto err_lightsensor_update_table;
+	}
+
+	lpi->lp_wq = create_singlethread_workqueue("cm3217_wq");
+	if (!lpi->lp_wq) {
+		pr_err("[CM3217 error]%s: can't create workqueue\n", __func__);
+		ret = -ENOMEM;
+		goto err_create_singlethread_workqueue;
+	}
+
+	ret = cm3217_setup(lpi);
+	if (ret < 0) {
+		pr_err("[ERR][CM3217 error]%s: cm3217_setup error!\n",
+		       __func__);
+		goto err_cm3217_setup;
+	}
+
+	lpi->cm3217_class = class_create(THIS_MODULE, "optical_sensors");
+	if (IS_ERR(lpi->cm3217_class)) {
+		ret = PTR_ERR(lpi->cm3217_class);
+		lpi->cm3217_class = NULL;
+		goto err_create_class;
+	}
+
+	lpi->ls_dev = device_create(lpi->cm3217_class,
+				    NULL, 0, "%s", "lightsensor");
+	if (unlikely(IS_ERR(lpi->ls_dev))) {
+		ret = PTR_ERR(lpi->ls_dev);
+		lpi->ls_dev = NULL;
+		goto err_create_ls_device;
+	}
+
+	/* register the attributes */
+	ret = device_create_file(lpi->ls_dev, &dev_attr_ls_adc);
+	if (ret)
+		goto err_create_ls_device_file;
+
+	/* register the attributes */
+	ret = device_create_file(lpi->ls_dev, &dev_attr_ls_auto);
+	if (ret)
+		goto err_create_ls_device_file;
+
+	/* register the attributes */
+	ret = device_create_file(lpi->ls_dev, &dev_attr_ls_kadc);
+	if (ret)
+		goto err_create_ls_device_file;
+
+	ret = device_create_file(lpi->ls_dev, &dev_attr_ls_gadc);
+	if (ret)
+		goto err_create_ls_device_file;
+
+	ret = device_create_file(lpi->ls_dev, &dev_attr_ls_adc_table);
+	if (ret)
+		goto err_create_ls_device_file;
+
+	ret = device_create_file(lpi->ls_dev, &dev_attr_ls_conf1);
+	if (ret)
+		goto err_create_ls_device_file;
+
+	ret = device_create_file(lpi->ls_dev, &dev_attr_ls_conf2);
+	if (ret)
+		goto err_create_ls_device_file;
+
+	ret = device_create_file(lpi->ls_dev, &dev_attr_ls_flevel);
+	if (ret)
+		goto err_create_ls_device_file;
+
+	lpi->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	lpi->early_suspend.suspend = cm3217_early_suspend;
+	lpi->early_suspend.resume = cm3217_late_resume;
+	register_early_suspend(&lpi->early_suspend);
+
+	lpi->als_enable = 0;
+	lpi->als_enabled_before_suspend = 0;
+	D("[CM3217] %s: Probe success!\n", __func__);
+
+	return ret;
+
+err_create_ls_device_file:
+	device_unregister(lpi->ls_dev);
+err_create_ls_device:
+	class_destroy(lpi->cm3217_class);
+err_create_class:
+err_cm3217_setup:
+	destroy_workqueue(lpi->lp_wq);
+	mutex_destroy(&als_enable_mutex);
+	mutex_destroy(&als_disable_mutex);
+	mutex_destroy(&als_get_adc_mutex);
+	input_unregister_device(lpi->ls_input_dev);
+	input_free_device(lpi->ls_input_dev);
+err_create_singlethread_workqueue:
+err_lightsensor_update_table:
+	misc_deregister(&lightsensor_misc);
+err_lightsensor_setup:
+err_platform_data_null:
+	kfree(lpi);
+	return ret;
+}
+
+static const struct i2c_device_id cm3217_i2c_id[] = {
+	{CM3217_I2C_NAME, 0},
+	{}
+};
+
+static struct i2c_driver cm3217_driver = {
+	.id_table = cm3217_i2c_id,
+	.probe = cm3217_probe,
+	.driver = {
+		.name = CM3217_I2C_NAME,
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init cm3217_init(void)
+{
+	return i2c_add_driver(&cm3217_driver);
+}
+
+static void __exit cm3217_exit(void)
+{
+	i2c_del_driver(&cm3217_driver);
+}
+
+module_init(cm3217_init);
+module_exit(cm3217_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("CM3217 Driver");
+MODULE_AUTHOR("Frank Hsieh <pengyueh@gmail.com>");
diff --git a/drivers/input/misc/gpio_axis.c b/drivers/input/misc/gpio_axis.c
new file mode 100644
index 000000000000..0acf4a576f53
--- /dev/null
+++ b/drivers/input/misc/gpio_axis.c
@@ -0,0 +1,192 @@
+/* drivers/input/misc/gpio_axis.c
+ *
+ * Copyright (C) 2007 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/kernel.h>
+#include <linux/gpio.h>
+#include <linux/gpio_event.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+
+struct gpio_axis_state {
+	struct gpio_event_input_devs *input_devs;
+	struct gpio_event_axis_info *info;
+	uint32_t pos;
+};
+
+uint16_t gpio_axis_4bit_gray_map_table[] = {
+	[0x0] = 0x0, [0x1] = 0x1, /* 0000 0001 */
+	[0x3] = 0x2, [0x2] = 0x3, /* 0011 0010 */
+	[0x6] = 0x4, [0x7] = 0x5, /* 0110 0111 */
+	[0x5] = 0x6, [0x4] = 0x7, /* 0101 0100 */
+	[0xc] = 0x8, [0xd] = 0x9, /* 1100 1101 */
+	[0xf] = 0xa, [0xe] = 0xb, /* 1111 1110 */
+	[0xa] = 0xc, [0xb] = 0xd, /* 1010 1011 */
+	[0x9] = 0xe, [0x8] = 0xf, /* 1001 1000 */
+};
+uint16_t gpio_axis_4bit_gray_map(struct gpio_event_axis_info *info, uint16_t in)
+{
+	return gpio_axis_4bit_gray_map_table[in];
+}
+
+uint16_t gpio_axis_5bit_singletrack_map_table[] = {
+	[0x10] = 0x00, [0x14] = 0x01, [0x1c] = 0x02, /*     10000 10100 11100 */
+	[0x1e] = 0x03, [0x1a] = 0x04, [0x18] = 0x05, /*     11110 11010 11000 */
+	[0x08] = 0x06, [0x0a] = 0x07, [0x0e] = 0x08, /*    01000 01010 01110  */
+	[0x0f] = 0x09, [0x0d] = 0x0a, [0x0c] = 0x0b, /*    01111 01101 01100  */
+	[0x04] = 0x0c, [0x05] = 0x0d, [0x07] = 0x0e, /*   00100 00101 00111   */
+	[0x17] = 0x0f, [0x16] = 0x10, [0x06] = 0x11, /*   10111 10110 00110   */
+	[0x02] = 0x12, [0x12] = 0x13, [0x13] = 0x14, /*  00010 10010 10011    */
+	[0x1b] = 0x15, [0x0b] = 0x16, [0x03] = 0x17, /*  11011 01011 00011    */
+	[0x01] = 0x18, [0x09] = 0x19, [0x19] = 0x1a, /* 00001 01001 11001     */
+	[0x1d] = 0x1b, [0x15] = 0x1c, [0x11] = 0x1d, /* 11101 10101 10001     */
+};
+uint16_t gpio_axis_5bit_singletrack_map(
+	struct gpio_event_axis_info *info, uint16_t in)
+{
+	return gpio_axis_5bit_singletrack_map_table[in];
+}
+
+static void gpio_event_update_axis(struct gpio_axis_state *as, int report)
+{
+	struct gpio_event_axis_info *ai = as->info;
+	int i;
+	int change;
+	uint16_t state = 0;
+	uint16_t pos;
+	uint16_t old_pos = as->pos;
+	for (i = ai->count - 1; i >= 0; i--)
+		state = (state << 1) | gpio_get_value(ai->gpio[i]);
+	pos = ai->map(ai, state);
+	if (ai->flags & GPIOEAF_PRINT_RAW)
+		pr_info("axis %d-%d raw %x, pos %d -> %d\n",
+			ai->type, ai->code, state, old_pos, pos);
+	if (report && pos != old_pos) {
+		if (ai->type == EV_REL) {
+			change = (ai->decoded_size + pos - old_pos) %
+				  ai->decoded_size;
+			if (change > ai->decoded_size / 2)
+				change -= ai->decoded_size;
+			if (change == ai->decoded_size / 2) {
+				if (ai->flags & GPIOEAF_PRINT_EVENT)
+					pr_info("axis %d-%d unknown direction, "
+						"pos %d -> %d\n", ai->type,
+						ai->code, old_pos, pos);
+				change = 0; /* no closest direction */
+			}
+			if (ai->flags & GPIOEAF_PRINT_EVENT)
+				pr_info("axis %d-%d change %d\n",
+					ai->type, ai->code, change);
+			input_report_rel(as->input_devs->dev[ai->dev],
+						ai->code, change);
+		} else {
+			if (ai->flags & GPIOEAF_PRINT_EVENT)
+				pr_info("axis %d-%d now %d\n",
+					ai->type, ai->code, pos);
+			input_event(as->input_devs->dev[ai->dev],
+					ai->type, ai->code, pos);
+		}
+		input_sync(as->input_devs->dev[ai->dev]);
+	}
+	as->pos = pos;
+}
+
+static irqreturn_t gpio_axis_irq_handler(int irq, void *dev_id)
+{
+	struct gpio_axis_state *as = dev_id;
+	gpio_event_update_axis(as, 1);
+	return IRQ_HANDLED;
+}
+
+int gpio_event_axis_func(struct gpio_event_input_devs *input_devs,
+			 struct gpio_event_info *info, void **data, int func)
+{
+	int ret;
+	int i;
+	int irq;
+	struct gpio_event_axis_info *ai;
+	struct gpio_axis_state *as;
+
+	ai = container_of(info, struct gpio_event_axis_info, info);
+	if (func == GPIO_EVENT_FUNC_SUSPEND) {
+		for (i = 0; i < ai->count; i++)
+			disable_irq(gpio_to_irq(ai->gpio[i]));
+		return 0;
+	}
+	if (func == GPIO_EVENT_FUNC_RESUME) {
+		for (i = 0; i < ai->count; i++)
+			enable_irq(gpio_to_irq(ai->gpio[i]));
+		return 0;
+	}
+
+	if (func == GPIO_EVENT_FUNC_INIT) {
+		*data = as = kmalloc(sizeof(*as), GFP_KERNEL);
+		if (as == NULL) {
+			ret = -ENOMEM;
+			goto err_alloc_axis_state_failed;
+		}
+		as->input_devs = input_devs;
+		as->info = ai;
+		if (ai->dev >= input_devs->count) {
+			pr_err("gpio_event_axis: bad device index %d >= %d "
+				"for %d:%d\n", ai->dev, input_devs->count,
+				ai->type, ai->code);
+			ret = -EINVAL;
+			goto err_bad_device_index;
+		}
+
+		input_set_capability(input_devs->dev[ai->dev],
+				     ai->type, ai->code);
+		if (ai->type == EV_ABS) {
+			input_set_abs_params(input_devs->dev[ai->dev], ai->code,
+					     0, ai->decoded_size - 1, 0, 0);
+		}
+		for (i = 0; i < ai->count; i++) {
+			ret = gpio_request(ai->gpio[i], "gpio_event_axis");
+			if (ret < 0)
+				goto err_request_gpio_failed;
+			ret = gpio_direction_input(ai->gpio[i]);
+			if (ret < 0)
+				goto err_gpio_direction_input_failed;
+			ret = irq = gpio_to_irq(ai->gpio[i]);
+			if (ret < 0)
+				goto err_get_irq_num_failed;
+			ret = request_irq(irq, gpio_axis_irq_handler,
+					  IRQF_TRIGGER_RISING |
+					  IRQF_TRIGGER_FALLING,
+					  "gpio_event_axis", as);
+			if (ret < 0)
+				goto err_request_irq_failed;
+		}
+		gpio_event_update_axis(as, 0);
+		return 0;
+	}
+
+	ret = 0;
+	as = *data;
+	for (i = ai->count - 1; i >= 0; i--) {
+		free_irq(gpio_to_irq(ai->gpio[i]), as);
+err_request_irq_failed:
+err_get_irq_num_failed:
+err_gpio_direction_input_failed:
+		gpio_free(ai->gpio[i]);
+err_request_gpio_failed:
+		;
+	}
+err_bad_device_index:
+	kfree(as);
+	*data = NULL;
+err_alloc_axis_state_failed:
+	return ret;
+}
diff --git a/drivers/input/misc/gpio_event.c b/drivers/input/misc/gpio_event.c
new file mode 100644
index 000000000000..a98be67d1ab0
--- /dev/null
+++ b/drivers/input/misc/gpio_event.c
@@ -0,0 +1,260 @@
+/* drivers/input/misc/gpio_event.c
+ *
+ * Copyright (C) 2007 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/earlysuspend.h>
+#include <linux/module.h>
+#include <linux/input.h>
+#include <linux/gpio_event.h>
+#include <linux/hrtimer.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+struct gpio_event {
+	struct gpio_event_input_devs *input_devs;
+	const struct gpio_event_platform_data *info;
+	struct early_suspend early_suspend;
+	void *state[0];
+};
+
+static int gpio_input_event(
+	struct input_dev *dev, unsigned int type, unsigned int code, int value)
+{
+	int i;
+	int devnr;
+	int ret = 0;
+	int tmp_ret;
+	struct gpio_event_info **ii;
+	struct gpio_event *ip = input_get_drvdata(dev);
+
+	for (devnr = 0; devnr < ip->input_devs->count; devnr++)
+		if (ip->input_devs->dev[devnr] == dev)
+			break;
+	if (devnr == ip->input_devs->count) {
+		pr_err("gpio_input_event: unknown device %p\n", dev);
+		return -EIO;
+	}
+
+	for (i = 0, ii = ip->info->info; i < ip->info->info_count; i++, ii++) {
+		if ((*ii)->event) {
+			tmp_ret = (*ii)->event(ip->input_devs, *ii,
+						&ip->state[i],
+						devnr, type, code, value);
+			if (tmp_ret)
+				ret = tmp_ret;
+		}
+	}
+	return ret;
+}
+
+static int gpio_event_call_all_func(struct gpio_event *ip, int func)
+{
+	int i;
+	int ret;
+	struct gpio_event_info **ii;
+
+	if (func == GPIO_EVENT_FUNC_INIT || func == GPIO_EVENT_FUNC_RESUME) {
+		ii = ip->info->info;
+		for (i = 0; i < ip->info->info_count; i++, ii++) {
+			if ((*ii)->func == NULL) {
+				ret = -ENODEV;
+				pr_err("gpio_event_probe: Incomplete pdata, "
+					"no function\n");
+				goto err_no_func;
+			}
+			if (func == GPIO_EVENT_FUNC_RESUME && (*ii)->no_suspend)
+				continue;
+			ret = (*ii)->func(ip->input_devs, *ii, &ip->state[i],
+					  func);
+			if (ret) {
+				pr_err("gpio_event_probe: function failed\n");
+				goto err_func_failed;
+			}
+		}
+		return 0;
+	}
+
+	ret = 0;
+	i = ip->info->info_count;
+	ii = ip->info->info + i;
+	while (i > 0) {
+		i--;
+		ii--;
+		if ((func & ~1) == GPIO_EVENT_FUNC_SUSPEND && (*ii)->no_suspend)
+			continue;
+		(*ii)->func(ip->input_devs, *ii, &ip->state[i], func & ~1);
+err_func_failed:
+err_no_func:
+		;
+	}
+	return ret;
+}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+void gpio_event_suspend(struct early_suspend *h)
+{
+	struct gpio_event *ip;
+	ip = container_of(h, struct gpio_event, early_suspend);
+	gpio_event_call_all_func(ip, GPIO_EVENT_FUNC_SUSPEND);
+	ip->info->power(ip->info, 0);
+}
+
+void gpio_event_resume(struct early_suspend *h)
+{
+	struct gpio_event *ip;
+	ip = container_of(h, struct gpio_event, early_suspend);
+	ip->info->power(ip->info, 1);
+	gpio_event_call_all_func(ip, GPIO_EVENT_FUNC_RESUME);
+}
+#endif
+
+static int gpio_event_probe(struct platform_device *pdev)
+{
+	int err;
+	struct gpio_event *ip;
+	struct gpio_event_platform_data *event_info;
+	int dev_count = 1;
+	int i;
+	int registered = 0;
+
+	event_info = pdev->dev.platform_data;
+	if (event_info == NULL) {
+		pr_err("gpio_event_probe: No pdata\n");
+		return -ENODEV;
+	}
+	if ((!event_info->name && !event_info->names[0]) ||
+	    !event_info->info || !event_info->info_count) {
+		pr_err("gpio_event_probe: Incomplete pdata\n");
+		return -ENODEV;
+	}
+	if (!event_info->name)
+		while (event_info->names[dev_count])
+			dev_count++;
+	ip = kzalloc(sizeof(*ip) +
+		     sizeof(ip->state[0]) * event_info->info_count +
+		     sizeof(*ip->input_devs) +
+		     sizeof(ip->input_devs->dev[0]) * dev_count, GFP_KERNEL);
+	if (ip == NULL) {
+		err = -ENOMEM;
+		pr_err("gpio_event_probe: Failed to allocate private data\n");
+		goto err_kp_alloc_failed;
+	}
+	ip->input_devs = (void*)&ip->state[event_info->info_count];
+	platform_set_drvdata(pdev, ip);
+
+	for (i = 0; i < dev_count; i++) {
+		struct input_dev *input_dev = input_allocate_device();
+		if (input_dev == NULL) {
+			err = -ENOMEM;
+			pr_err("gpio_event_probe: "
+				"Failed to allocate input device\n");
+			goto err_input_dev_alloc_failed;
+		}
+		input_set_drvdata(input_dev, ip);
+		input_dev->name = event_info->name ?
+					event_info->name : event_info->names[i];
+		input_dev->event = gpio_input_event;
+		ip->input_devs->dev[i] = input_dev;
+	}
+	ip->input_devs->count = dev_count;
+	ip->info = event_info;
+	if (event_info->power) {
+#ifdef CONFIG_HAS_EARLYSUSPEND
+		ip->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+		ip->early_suspend.suspend = gpio_event_suspend;
+		ip->early_suspend.resume = gpio_event_resume;
+		register_early_suspend(&ip->early_suspend);
+#endif
+		ip->info->power(ip->info, 1);
+	}
+
+	err = gpio_event_call_all_func(ip, GPIO_EVENT_FUNC_INIT);
+	if (err)
+		goto err_call_all_func_failed;
+
+	for (i = 0; i < dev_count; i++) {
+		err = input_register_device(ip->input_devs->dev[i]);
+		if (err) {
+			pr_err("gpio_event_probe: Unable to register %s "
+				"input device\n", ip->input_devs->dev[i]->name);
+			goto err_input_register_device_failed;
+		}
+		registered++;
+	}
+
+	return 0;
+
+err_input_register_device_failed:
+	gpio_event_call_all_func(ip, GPIO_EVENT_FUNC_UNINIT);
+err_call_all_func_failed:
+	if (event_info->power) {
+#ifdef CONFIG_HAS_EARLYSUSPEND
+		unregister_early_suspend(&ip->early_suspend);
+#endif
+		ip->info->power(ip->info, 0);
+	}
+	for (i = 0; i < registered; i++)
+		input_unregister_device(ip->input_devs->dev[i]);
+	for (i = dev_count - 1; i >= registered; i--) {
+		input_free_device(ip->input_devs->dev[i]);
+err_input_dev_alloc_failed:
+		;
+	}
+	kfree(ip);
+err_kp_alloc_failed:
+	return err;
+}
+
+static int gpio_event_remove(struct platform_device *pdev)
+{
+	struct gpio_event *ip = platform_get_drvdata(pdev);
+	int i;
+
+	gpio_event_call_all_func(ip, GPIO_EVENT_FUNC_UNINIT);
+	if (ip->info->power) {
+#ifdef CONFIG_HAS_EARLYSUSPEND
+		unregister_early_suspend(&ip->early_suspend);
+#endif
+		ip->info->power(ip->info, 0);
+	}
+	for (i = 0; i < ip->input_devs->count; i++)
+		input_unregister_device(ip->input_devs->dev[i]);
+	kfree(ip);
+	return 0;
+}
+
+static struct platform_driver gpio_event_driver = {
+	.probe		= gpio_event_probe,
+	.remove		= gpio_event_remove,
+	.driver		= {
+		.name	= GPIO_EVENT_DEV_NAME,
+	},
+};
+
+static int __devinit gpio_event_init(void)
+{
+	return platform_driver_register(&gpio_event_driver);
+}
+
+static void __exit gpio_event_exit(void)
+{
+	platform_driver_unregister(&gpio_event_driver);
+}
+
+module_init(gpio_event_init);
+module_exit(gpio_event_exit);
+
+MODULE_DESCRIPTION("GPIO Event Driver");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/input/misc/gpio_input.c b/drivers/input/misc/gpio_input.c
new file mode 100644
index 000000000000..6a0c31510968
--- /dev/null
+++ b/drivers/input/misc/gpio_input.c
@@ -0,0 +1,376 @@
+/* drivers/input/misc/gpio_input.c
+ *
+ * Copyright (C) 2007 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/kernel.h>
+#include <linux/gpio.h>
+#include <linux/gpio_event.h>
+#include <linux/hrtimer.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/wakelock.h>
+
+enum {
+	DEBOUNCE_UNSTABLE     = BIT(0),	/* Got irq, while debouncing */
+	DEBOUNCE_PRESSED      = BIT(1),
+	DEBOUNCE_NOTPRESSED   = BIT(2),
+	DEBOUNCE_WAIT_IRQ     = BIT(3),	/* Stable irq state */
+	DEBOUNCE_POLL         = BIT(4),	/* Stable polling state */
+
+	DEBOUNCE_UNKNOWN =
+		DEBOUNCE_PRESSED | DEBOUNCE_NOTPRESSED,
+};
+
+struct gpio_key_state {
+	struct gpio_input_state *ds;
+	uint8_t debounce;
+};
+
+struct gpio_input_state {
+	struct gpio_event_input_devs *input_devs;
+	const struct gpio_event_input_info *info;
+	struct hrtimer timer;
+	int use_irq;
+	int debounce_count;
+	spinlock_t irq_lock;
+	struct wake_lock wake_lock;
+	struct gpio_key_state key_state[0];
+};
+
+static enum hrtimer_restart gpio_event_input_timer_func(struct hrtimer *timer)
+{
+	int i;
+	int pressed;
+	struct gpio_input_state *ds =
+		container_of(timer, struct gpio_input_state, timer);
+	unsigned gpio_flags = ds->info->flags;
+	unsigned npolarity;
+	int nkeys = ds->info->keymap_size;
+	const struct gpio_event_direct_entry *key_entry;
+	struct gpio_key_state *key_state;
+	unsigned long irqflags;
+	uint8_t debounce;
+	bool sync_needed;
+
+#if 0
+	key_entry = kp->keys_info->keymap;
+	key_state = kp->key_state;
+	for (i = 0; i < nkeys; i++, key_entry++, key_state++)
+		pr_info("gpio_read_detect_status %d %d\n", key_entry->gpio,
+			gpio_read_detect_status(key_entry->gpio));
+#endif
+	key_entry = ds->info->keymap;
+	key_state = ds->key_state;
+	sync_needed = false;
+	spin_lock_irqsave(&ds->irq_lock, irqflags);
+	for (i = 0; i < nkeys; i++, key_entry++, key_state++) {
+		debounce = key_state->debounce;
+		if (debounce & DEBOUNCE_WAIT_IRQ)
+			continue;
+		if (key_state->debounce & DEBOUNCE_UNSTABLE) {
+			debounce = key_state->debounce = DEBOUNCE_UNKNOWN;
+			enable_irq(gpio_to_irq(key_entry->gpio));
+			if (gpio_flags & GPIOEDF_PRINT_KEY_UNSTABLE)
+				pr_info("gpio_keys_scan_keys: key %x-%x, %d "
+					"(%d) continue debounce\n",
+					ds->info->type, key_entry->code,
+					i, key_entry->gpio);
+		}
+		npolarity = !(gpio_flags & GPIOEDF_ACTIVE_HIGH);
+		pressed = gpio_get_value(key_entry->gpio) ^ npolarity;
+		if (debounce & DEBOUNCE_POLL) {
+			if (pressed == !(debounce & DEBOUNCE_PRESSED)) {
+				ds->debounce_count++;
+				key_state->debounce = DEBOUNCE_UNKNOWN;
+				if (gpio_flags & GPIOEDF_PRINT_KEY_DEBOUNCE)
+					pr_info("gpio_keys_scan_keys: key %x-"
+						"%x, %d (%d) start debounce\n",
+						ds->info->type, key_entry->code,
+						i, key_entry->gpio);
+			}
+			continue;
+		}
+		if (pressed && (debounce & DEBOUNCE_NOTPRESSED)) {
+			if (gpio_flags & GPIOEDF_PRINT_KEY_DEBOUNCE)
+				pr_info("gpio_keys_scan_keys: key %x-%x, %d "
+					"(%d) debounce pressed 1\n",
+					ds->info->type, key_entry->code,
+					i, key_entry->gpio);
+			key_state->debounce = DEBOUNCE_PRESSED;
+			continue;
+		}
+		if (!pressed && (debounce & DEBOUNCE_PRESSED)) {
+			if (gpio_flags & GPIOEDF_PRINT_KEY_DEBOUNCE)
+				pr_info("gpio_keys_scan_keys: key %x-%x, %d "
+					"(%d) debounce pressed 0\n",
+					ds->info->type, key_entry->code,
+					i, key_entry->gpio);
+			key_state->debounce = DEBOUNCE_NOTPRESSED;
+			continue;
+		}
+		/* key is stable */
+		ds->debounce_count--;
+		if (ds->use_irq)
+			key_state->debounce |= DEBOUNCE_WAIT_IRQ;
+		else
+			key_state->debounce |= DEBOUNCE_POLL;
+		if (gpio_flags & GPIOEDF_PRINT_KEYS)
+			pr_info("gpio_keys_scan_keys: key %x-%x, %d (%d) "
+				"changed to %d\n", ds->info->type,
+				key_entry->code, i, key_entry->gpio, pressed);
+		input_event(ds->input_devs->dev[key_entry->dev], ds->info->type,
+			    key_entry->code, pressed);
+		sync_needed = true;
+	}
+	if (sync_needed) {
+		for (i = 0; i < ds->input_devs->count; i++)
+			input_sync(ds->input_devs->dev[i]);
+	}
+
+#if 0
+	key_entry = kp->keys_info->keymap;
+	key_state = kp->key_state;
+	for (i = 0; i < nkeys; i++, key_entry++, key_state++) {
+		pr_info("gpio_read_detect_status %d %d\n", key_entry->gpio,
+			gpio_read_detect_status(key_entry->gpio));
+	}
+#endif
+
+	if (ds->debounce_count)
+		hrtimer_start(timer, ds->info->debounce_time, HRTIMER_MODE_REL);
+	else if (!ds->use_irq)
+		hrtimer_start(timer, ds->info->poll_time, HRTIMER_MODE_REL);
+	else
+		wake_unlock(&ds->wake_lock);
+
+	spin_unlock_irqrestore(&ds->irq_lock, irqflags);
+
+	return HRTIMER_NORESTART;
+}
+
+static irqreturn_t gpio_event_input_irq_handler(int irq, void *dev_id)
+{
+	struct gpio_key_state *ks = dev_id;
+	struct gpio_input_state *ds = ks->ds;
+	int keymap_index = ks - ds->key_state;
+	const struct gpio_event_direct_entry *key_entry;
+	unsigned long irqflags;
+	int pressed;
+
+	if (!ds->use_irq)
+		return IRQ_HANDLED;
+
+	key_entry = &ds->info->keymap[keymap_index];
+
+	if (ds->info->debounce_time.tv64) {
+		spin_lock_irqsave(&ds->irq_lock, irqflags);
+		if (ks->debounce & DEBOUNCE_WAIT_IRQ) {
+			ks->debounce = DEBOUNCE_UNKNOWN;
+			if (ds->debounce_count++ == 0) {
+				wake_lock(&ds->wake_lock);
+				hrtimer_start(
+					&ds->timer, ds->info->debounce_time,
+					HRTIMER_MODE_REL);
+			}
+			if (ds->info->flags & GPIOEDF_PRINT_KEY_DEBOUNCE)
+				pr_info("gpio_event_input_irq_handler: "
+					"key %x-%x, %d (%d) start debounce\n",
+					ds->info->type, key_entry->code,
+					keymap_index, key_entry->gpio);
+		} else {
+			disable_irq_nosync(irq);
+			ks->debounce = DEBOUNCE_UNSTABLE;
+		}
+		spin_unlock_irqrestore(&ds->irq_lock, irqflags);
+	} else {
+		pressed = gpio_get_value(key_entry->gpio) ^
+			!(ds->info->flags & GPIOEDF_ACTIVE_HIGH);
+		if (ds->info->flags & GPIOEDF_PRINT_KEYS)
+			pr_info("gpio_event_input_irq_handler: key %x-%x, %d "
+				"(%d) changed to %d\n",
+				ds->info->type, key_entry->code, keymap_index,
+				key_entry->gpio, pressed);
+		input_event(ds->input_devs->dev[key_entry->dev], ds->info->type,
+			    key_entry->code, pressed);
+		input_sync(ds->input_devs->dev[key_entry->dev]);
+	}
+	return IRQ_HANDLED;
+}
+
+static int gpio_event_input_request_irqs(struct gpio_input_state *ds)
+{
+	int i;
+	int err;
+	unsigned int irq;
+	unsigned long req_flags = IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING;
+
+	for (i = 0; i < ds->info->keymap_size; i++) {
+		err = irq = gpio_to_irq(ds->info->keymap[i].gpio);
+		if (err < 0)
+			goto err_gpio_get_irq_num_failed;
+		err = request_irq(irq, gpio_event_input_irq_handler,
+				  req_flags, "gpio_keys", &ds->key_state[i]);
+		if (err) {
+			pr_err("gpio_event_input_request_irqs: request_irq "
+				"failed for input %d, irq %d\n",
+				ds->info->keymap[i].gpio, irq);
+			goto err_request_irq_failed;
+		}
+		if (ds->info->info.no_suspend) {
+			err = enable_irq_wake(irq);
+			if (err) {
+				pr_err("gpio_event_input_request_irqs: "
+					"enable_irq_wake failed for input %d, "
+					"irq %d\n",
+					ds->info->keymap[i].gpio, irq);
+				goto err_enable_irq_wake_failed;
+			}
+		}
+	}
+	return 0;
+
+	for (i = ds->info->keymap_size - 1; i >= 0; i--) {
+		irq = gpio_to_irq(ds->info->keymap[i].gpio);
+		if (ds->info->info.no_suspend)
+			disable_irq_wake(irq);
+err_enable_irq_wake_failed:
+		free_irq(irq, &ds->key_state[i]);
+err_request_irq_failed:
+err_gpio_get_irq_num_failed:
+		;
+	}
+	return err;
+}
+
+int gpio_event_input_func(struct gpio_event_input_devs *input_devs,
+			struct gpio_event_info *info, void **data, int func)
+{
+	int ret;
+	int i;
+	unsigned long irqflags;
+	struct gpio_event_input_info *di;
+	struct gpio_input_state *ds = *data;
+
+	di = container_of(info, struct gpio_event_input_info, info);
+
+	if (func == GPIO_EVENT_FUNC_SUSPEND) {
+		if (ds->use_irq)
+			for (i = 0; i < di->keymap_size; i++)
+				disable_irq(gpio_to_irq(di->keymap[i].gpio));
+		hrtimer_cancel(&ds->timer);
+		return 0;
+	}
+	if (func == GPIO_EVENT_FUNC_RESUME) {
+		spin_lock_irqsave(&ds->irq_lock, irqflags);
+		if (ds->use_irq)
+			for (i = 0; i < di->keymap_size; i++)
+				enable_irq(gpio_to_irq(di->keymap[i].gpio));
+		hrtimer_start(&ds->timer, ktime_set(0, 0), HRTIMER_MODE_REL);
+		spin_unlock_irqrestore(&ds->irq_lock, irqflags);
+		return 0;
+	}
+
+	if (func == GPIO_EVENT_FUNC_INIT) {
+		if (ktime_to_ns(di->poll_time) <= 0)
+			di->poll_time = ktime_set(0, 20 * NSEC_PER_MSEC);
+
+		*data = ds = kzalloc(sizeof(*ds) + sizeof(ds->key_state[0]) *
+					di->keymap_size, GFP_KERNEL);
+		if (ds == NULL) {
+			ret = -ENOMEM;
+			pr_err("gpio_event_input_func: "
+				"Failed to allocate private data\n");
+			goto err_ds_alloc_failed;
+		}
+		ds->debounce_count = di->keymap_size;
+		ds->input_devs = input_devs;
+		ds->info = di;
+		wake_lock_init(&ds->wake_lock, WAKE_LOCK_SUSPEND, "gpio_input");
+		spin_lock_init(&ds->irq_lock);
+
+		for (i = 0; i < di->keymap_size; i++) {
+			int dev = di->keymap[i].dev;
+			if (dev >= input_devs->count) {
+				pr_err("gpio_event_input_func: bad device "
+					"index %d >= %d for key code %d\n",
+					dev, input_devs->count,
+					di->keymap[i].code);
+				ret = -EINVAL;
+				goto err_bad_keymap;
+			}
+			input_set_capability(input_devs->dev[dev], di->type,
+					     di->keymap[i].code);
+			ds->key_state[i].ds = ds;
+			ds->key_state[i].debounce = DEBOUNCE_UNKNOWN;
+		}
+
+		for (i = 0; i < di->keymap_size; i++) {
+			ret = gpio_request(di->keymap[i].gpio, "gpio_kp_in");
+			if (ret) {
+				pr_err("gpio_event_input_func: gpio_request "
+					"failed for %d\n", di->keymap[i].gpio);
+				goto err_gpio_request_failed;
+			}
+			ret = gpio_direction_input(di->keymap[i].gpio);
+			if (ret) {
+				pr_err("gpio_event_input_func: "
+					"gpio_direction_input failed for %d\n",
+					di->keymap[i].gpio);
+				goto err_gpio_configure_failed;
+			}
+		}
+
+		ret = gpio_event_input_request_irqs(ds);
+
+		spin_lock_irqsave(&ds->irq_lock, irqflags);
+		ds->use_irq = ret == 0;
+
+		pr_info("GPIO Input Driver: Start gpio inputs for %s%s in %s "
+			"mode\n", input_devs->dev[0]->name,
+			(input_devs->count > 1) ? "..." : "",
+			ret == 0 ? "interrupt" : "polling");
+
+		hrtimer_init(&ds->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+		ds->timer.function = gpio_event_input_timer_func;
+		hrtimer_start(&ds->timer, ktime_set(0, 0), HRTIMER_MODE_REL);
+		spin_unlock_irqrestore(&ds->irq_lock, irqflags);
+		return 0;
+	}
+
+	ret = 0;
+	spin_lock_irqsave(&ds->irq_lock, irqflags);
+	hrtimer_cancel(&ds->timer);
+	if (ds->use_irq) {
+		for (i = di->keymap_size - 1; i >= 0; i--) {
+			int irq = gpio_to_irq(di->keymap[i].gpio);
+			if (ds->info->info.no_suspend)
+				disable_irq_wake(irq);
+			free_irq(irq, &ds->key_state[i]);
+		}
+	}
+	spin_unlock_irqrestore(&ds->irq_lock, irqflags);
+
+	for (i = di->keymap_size - 1; i >= 0; i--) {
+err_gpio_configure_failed:
+		gpio_free(di->keymap[i].gpio);
+err_gpio_request_failed:
+		;
+	}
+err_bad_keymap:
+	wake_lock_destroy(&ds->wake_lock);
+	kfree(ds);
+err_ds_alloc_failed:
+	return ret;
+}
diff --git a/drivers/input/misc/gpio_matrix.c b/drivers/input/misc/gpio_matrix.c
new file mode 100644
index 000000000000..eaa9e89d473a
--- /dev/null
+++ b/drivers/input/misc/gpio_matrix.c
@@ -0,0 +1,441 @@
+/* drivers/input/misc/gpio_matrix.c
+ *
+ * Copyright (C) 2007 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/kernel.h>
+#include <linux/gpio.h>
+#include <linux/gpio_event.h>
+#include <linux/hrtimer.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/wakelock.h>
+
+struct gpio_kp {
+	struct gpio_event_input_devs *input_devs;
+	struct gpio_event_matrix_info *keypad_info;
+	struct hrtimer timer;
+	struct wake_lock wake_lock;
+	int current_output;
+	unsigned int use_irq:1;
+	unsigned int key_state_changed:1;
+	unsigned int last_key_state_changed:1;
+	unsigned int some_keys_pressed:2;
+	unsigned int disabled_irq:1;
+	unsigned long keys_pressed[0];
+};
+
+static void clear_phantom_key(struct gpio_kp *kp, int out, int in)
+{
+	struct gpio_event_matrix_info *mi = kp->keypad_info;
+	int key_index = out * mi->ninputs + in;
+	unsigned short keyentry = mi->keymap[key_index];
+	unsigned short keycode = keyentry & MATRIX_KEY_MASK;
+	unsigned short dev = keyentry >> MATRIX_CODE_BITS;
+
+	if (!test_bit(keycode, kp->input_devs->dev[dev]->key)) {
+		if (mi->flags & GPIOKPF_PRINT_PHANTOM_KEYS)
+			pr_info("gpiomatrix: phantom key %x, %d-%d (%d-%d) "
+				"cleared\n", keycode, out, in,
+				mi->output_gpios[out], mi->input_gpios[in]);
+		__clear_bit(key_index, kp->keys_pressed);
+	} else {
+		if (mi->flags & GPIOKPF_PRINT_PHANTOM_KEYS)
+			pr_info("gpiomatrix: phantom key %x, %d-%d (%d-%d) "
+				"not cleared\n", keycode, out, in,
+				mi->output_gpios[out], mi->input_gpios[in]);
+	}
+}
+
+static int restore_keys_for_input(struct gpio_kp *kp, int out, int in)
+{
+	int rv = 0;
+	int key_index;
+
+	key_index = out * kp->keypad_info->ninputs + in;
+	while (out < kp->keypad_info->noutputs) {
+		if (test_bit(key_index, kp->keys_pressed)) {
+			rv = 1;
+			clear_phantom_key(kp, out, in);
+		}
+		key_index += kp->keypad_info->ninputs;
+		out++;
+	}
+	return rv;
+}
+
+static void remove_phantom_keys(struct gpio_kp *kp)
+{
+	int out, in, inp;
+	int key_index;
+
+	if (kp->some_keys_pressed < 3)
+		return;
+
+	for (out = 0; out < kp->keypad_info->noutputs; out++) {
+		inp = -1;
+		key_index = out * kp->keypad_info->ninputs;
+		for (in = 0; in < kp->keypad_info->ninputs; in++, key_index++) {
+			if (test_bit(key_index, kp->keys_pressed)) {
+				if (inp == -1) {
+					inp = in;
+					continue;
+				}
+				if (inp >= 0) {
+					if (!restore_keys_for_input(kp, out + 1,
+									inp))
+						break;
+					clear_phantom_key(kp, out, inp);
+					inp = -2;
+				}
+				restore_keys_for_input(kp, out, in);
+			}
+		}
+	}
+}
+
+static void report_key(struct gpio_kp *kp, int key_index, int out, int in)
+{
+	struct gpio_event_matrix_info *mi = kp->keypad_info;
+	int pressed = test_bit(key_index, kp->keys_pressed);
+	unsigned short keyentry = mi->keymap[key_index];
+	unsigned short keycode = keyentry & MATRIX_KEY_MASK;
+	unsigned short dev = keyentry >> MATRIX_CODE_BITS;
+
+	if (pressed != test_bit(keycode, kp->input_devs->dev[dev]->key)) {
+		if (keycode == KEY_RESERVED) {
+			if (mi->flags & GPIOKPF_PRINT_UNMAPPED_KEYS)
+				pr_info("gpiomatrix: unmapped key, %d-%d "
+					"(%d-%d) changed to %d\n",
+					out, in, mi->output_gpios[out],
+					mi->input_gpios[in], pressed);
+		} else {
+			if (mi->flags & GPIOKPF_PRINT_MAPPED_KEYS)
+				pr_info("gpiomatrix: key %x, %d-%d (%d-%d) "
+					"changed to %d\n", keycode,
+					out, in, mi->output_gpios[out],
+					mi->input_gpios[in], pressed);
+			input_report_key(kp->input_devs->dev[dev], keycode, pressed);
+		}
+	}
+}
+
+static void report_sync(struct gpio_kp *kp)
+{
+	int i;
+
+	for (i = 0; i < kp->input_devs->count; i++)
+		input_sync(kp->input_devs->dev[i]);
+}
+
+static enum hrtimer_restart gpio_keypad_timer_func(struct hrtimer *timer)
+{
+	int out, in;
+	int key_index;
+	int gpio;
+	struct gpio_kp *kp = container_of(timer, struct gpio_kp, timer);
+	struct gpio_event_matrix_info *mi = kp->keypad_info;
+	unsigned gpio_keypad_flags = mi->flags;
+	unsigned polarity = !!(gpio_keypad_flags & GPIOKPF_ACTIVE_HIGH);
+
+	out = kp->current_output;
+	if (out == mi->noutputs) {
+		out = 0;
+		kp->last_key_state_changed = kp->key_state_changed;
+		kp->key_state_changed = 0;
+		kp->some_keys_pressed = 0;
+	} else {
+		key_index = out * mi->ninputs;
+		for (in = 0; in < mi->ninputs; in++, key_index++) {
+			gpio = mi->input_gpios[in];
+			if (gpio_get_value(gpio) ^ !polarity) {
+				if (kp->some_keys_pressed < 3)
+					kp->some_keys_pressed++;
+				kp->key_state_changed |= !__test_and_set_bit(
+						key_index, kp->keys_pressed);
+			} else
+				kp->key_state_changed |= __test_and_clear_bit(
+						key_index, kp->keys_pressed);
+		}
+		gpio = mi->output_gpios[out];
+		if (gpio_keypad_flags & GPIOKPF_DRIVE_INACTIVE)
+			gpio_set_value(gpio, !polarity);
+		else
+			gpio_direction_input(gpio);
+		out++;
+	}
+	kp->current_output = out;
+	if (out < mi->noutputs) {
+		gpio = mi->output_gpios[out];
+		if (gpio_keypad_flags & GPIOKPF_DRIVE_INACTIVE)
+			gpio_set_value(gpio, polarity);
+		else
+			gpio_direction_output(gpio, polarity);
+		hrtimer_start(timer, mi->settle_time, HRTIMER_MODE_REL);
+		return HRTIMER_NORESTART;
+	}
+	if (gpio_keypad_flags & GPIOKPF_DEBOUNCE) {
+		if (kp->key_state_changed) {
+			hrtimer_start(&kp->timer, mi->debounce_delay,
+				      HRTIMER_MODE_REL);
+			return HRTIMER_NORESTART;
+		}
+		kp->key_state_changed = kp->last_key_state_changed;
+	}
+	if (kp->key_state_changed) {
+		if (gpio_keypad_flags & GPIOKPF_REMOVE_SOME_PHANTOM_KEYS)
+			remove_phantom_keys(kp);
+		key_index = 0;
+		for (out = 0; out < mi->noutputs; out++)
+			for (in = 0; in < mi->ninputs; in++, key_index++)
+				report_key(kp, key_index, out, in);
+		report_sync(kp);
+	}
+	if (!kp->use_irq || kp->some_keys_pressed) {
+		hrtimer_start(timer, mi->poll_time, HRTIMER_MODE_REL);
+		return HRTIMER_NORESTART;
+	}
+
+	/* No keys are pressed, reenable interrupt */
+	for (out = 0; out < mi->noutputs; out++) {
+		if (gpio_keypad_flags & GPIOKPF_DRIVE_INACTIVE)
+			gpio_set_value(mi->output_gpios[out], polarity);
+		else
+			gpio_direction_output(mi->output_gpios[out], polarity);
+	}
+	for (in = 0; in < mi->ninputs; in++)
+		enable_irq(gpio_to_irq(mi->input_gpios[in]));
+	wake_unlock(&kp->wake_lock);
+	return HRTIMER_NORESTART;
+}
+
+static irqreturn_t gpio_keypad_irq_handler(int irq_in, void *dev_id)
+{
+	int i;
+	struct gpio_kp *kp = dev_id;
+	struct gpio_event_matrix_info *mi = kp->keypad_info;
+	unsigned gpio_keypad_flags = mi->flags;
+
+	if (!kp->use_irq) {
+		/* ignore interrupt while registering the handler */
+		kp->disabled_irq = 1;
+		disable_irq_nosync(irq_in);
+		return IRQ_HANDLED;
+	}
+
+	for (i = 0; i < mi->ninputs; i++)
+		disable_irq_nosync(gpio_to_irq(mi->input_gpios[i]));
+	for (i = 0; i < mi->noutputs; i++) {
+		if (gpio_keypad_flags & GPIOKPF_DRIVE_INACTIVE)
+			gpio_set_value(mi->output_gpios[i],
+				!(gpio_keypad_flags & GPIOKPF_ACTIVE_HIGH));
+		else
+			gpio_direction_input(mi->output_gpios[i]);
+	}
+	wake_lock(&kp->wake_lock);
+	hrtimer_start(&kp->timer, ktime_set(0, 0), HRTIMER_MODE_REL);
+	return IRQ_HANDLED;
+}
+
+static int gpio_keypad_request_irqs(struct gpio_kp *kp)
+{
+	int i;
+	int err;
+	unsigned int irq;
+	unsigned long request_flags;
+	struct gpio_event_matrix_info *mi = kp->keypad_info;
+
+	switch (mi->flags & (GPIOKPF_ACTIVE_HIGH|GPIOKPF_LEVEL_TRIGGERED_IRQ)) {
+	default:
+		request_flags = IRQF_TRIGGER_FALLING;
+		break;
+	case GPIOKPF_ACTIVE_HIGH:
+		request_flags = IRQF_TRIGGER_RISING;
+		break;
+	case GPIOKPF_LEVEL_TRIGGERED_IRQ:
+		request_flags = IRQF_TRIGGER_LOW;
+		break;
+	case GPIOKPF_LEVEL_TRIGGERED_IRQ | GPIOKPF_ACTIVE_HIGH:
+		request_flags = IRQF_TRIGGER_HIGH;
+		break;
+	}
+
+	for (i = 0; i < mi->ninputs; i++) {
+		err = irq = gpio_to_irq(mi->input_gpios[i]);
+		if (err < 0)
+			goto err_gpio_get_irq_num_failed;
+		err = request_irq(irq, gpio_keypad_irq_handler, request_flags,
+				  "gpio_kp", kp);
+		if (err) {
+			pr_err("gpiomatrix: request_irq failed for input %d, "
+				"irq %d\n", mi->input_gpios[i], irq);
+			goto err_request_irq_failed;
+		}
+		err = enable_irq_wake(irq);
+		if (err) {
+			pr_err("gpiomatrix: set_irq_wake failed for input %d, "
+				"irq %d\n", mi->input_gpios[i], irq);
+		}
+		disable_irq(irq);
+		if (kp->disabled_irq) {
+			kp->disabled_irq = 0;
+			enable_irq(irq);
+		}
+	}
+	return 0;
+
+	for (i = mi->noutputs - 1; i >= 0; i--) {
+		free_irq(gpio_to_irq(mi->input_gpios[i]), kp);
+err_request_irq_failed:
+err_gpio_get_irq_num_failed:
+		;
+	}
+	return err;
+}
+
+int gpio_event_matrix_func(struct gpio_event_input_devs *input_devs,
+	struct gpio_event_info *info, void **data, int func)
+{
+	int i;
+	int err;
+	int key_count;
+	struct gpio_kp *kp;
+	struct gpio_event_matrix_info *mi;
+
+	mi = container_of(info, struct gpio_event_matrix_info, info);
+	if (func == GPIO_EVENT_FUNC_SUSPEND || func == GPIO_EVENT_FUNC_RESUME) {
+		/* TODO: disable scanning */
+		return 0;
+	}
+
+	if (func == GPIO_EVENT_FUNC_INIT) {
+		if (mi->keymap == NULL ||
+		   mi->input_gpios == NULL ||
+		   mi->output_gpios == NULL) {
+			err = -ENODEV;
+			pr_err("gpiomatrix: Incomplete pdata\n");
+			goto err_invalid_platform_data;
+		}
+		key_count = mi->ninputs * mi->noutputs;
+
+		*data = kp = kzalloc(sizeof(*kp) + sizeof(kp->keys_pressed[0]) *
+				     BITS_TO_LONGS(key_count), GFP_KERNEL);
+		if (kp == NULL) {
+			err = -ENOMEM;
+			pr_err("gpiomatrix: Failed to allocate private data\n");
+			goto err_kp_alloc_failed;
+		}
+		kp->input_devs = input_devs;
+		kp->keypad_info = mi;
+		for (i = 0; i < key_count; i++) {
+			unsigned short keyentry = mi->keymap[i];
+			unsigned short keycode = keyentry & MATRIX_KEY_MASK;
+			unsigned short dev = keyentry >> MATRIX_CODE_BITS;
+			if (dev >= input_devs->count) {
+				pr_err("gpiomatrix: bad device index %d >= "
+					"%d for key code %d\n",
+					dev, input_devs->count, keycode);
+				err = -EINVAL;
+				goto err_bad_keymap;
+			}
+			if (keycode && keycode <= KEY_MAX)
+				input_set_capability(input_devs->dev[dev],
+							EV_KEY, keycode);
+		}
+
+		for (i = 0; i < mi->noutputs; i++) {
+			err = gpio_request(mi->output_gpios[i], "gpio_kp_out");
+			if (err) {
+				pr_err("gpiomatrix: gpio_request failed for "
+					"output %d\n", mi->output_gpios[i]);
+				goto err_request_output_gpio_failed;
+			}
+			if (gpio_cansleep(mi->output_gpios[i])) {
+				pr_err("gpiomatrix: unsupported output gpio %d,"
+					" can sleep\n", mi->output_gpios[i]);
+				err = -EINVAL;
+				goto err_output_gpio_configure_failed;
+			}
+			if (mi->flags & GPIOKPF_DRIVE_INACTIVE)
+				err = gpio_direction_output(mi->output_gpios[i],
+					!(mi->flags & GPIOKPF_ACTIVE_HIGH));
+			else
+				err = gpio_direction_input(mi->output_gpios[i]);
+			if (err) {
+				pr_err("gpiomatrix: gpio_configure failed for "
+					"output %d\n", mi->output_gpios[i]);
+				goto err_output_gpio_configure_failed;
+			}
+		}
+		for (i = 0; i < mi->ninputs; i++) {
+			err = gpio_request(mi->input_gpios[i], "gpio_kp_in");
+			if (err) {
+				pr_err("gpiomatrix: gpio_request failed for "
+					"input %d\n", mi->input_gpios[i]);
+				goto err_request_input_gpio_failed;
+			}
+			err = gpio_direction_input(mi->input_gpios[i]);
+			if (err) {
+				pr_err("gpiomatrix: gpio_direction_input failed"
+					" for input %d\n", mi->input_gpios[i]);
+				goto err_gpio_direction_input_failed;
+			}
+		}
+		kp->current_output = mi->noutputs;
+		kp->key_state_changed = 1;
+
+		hrtimer_init(&kp->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+		kp->timer.function = gpio_keypad_timer_func;
+		wake_lock_init(&kp->wake_lock, WAKE_LOCK_SUSPEND, "gpio_kp");
+		err = gpio_keypad_request_irqs(kp);
+		kp->use_irq = err == 0;
+
+		pr_info("GPIO Matrix Keypad Driver: Start keypad matrix for "
+			"%s%s in %s mode\n", input_devs->dev[0]->name,
+			(input_devs->count > 1) ? "..." : "",
+			kp->use_irq ? "interrupt" : "polling");
+
+		if (kp->use_irq)
+			wake_lock(&kp->wake_lock);
+		hrtimer_start(&kp->timer, ktime_set(0, 0), HRTIMER_MODE_REL);
+
+		return 0;
+	}
+
+	err = 0;
+	kp = *data;
+
+	if (kp->use_irq)
+		for (i = mi->noutputs - 1; i >= 0; i--)
+			free_irq(gpio_to_irq(mi->input_gpios[i]), kp);
+
+	hrtimer_cancel(&kp->timer);
+	wake_lock_destroy(&kp->wake_lock);
+	for (i = mi->noutputs - 1; i >= 0; i--) {
+err_gpio_direction_input_failed:
+		gpio_free(mi->input_gpios[i]);
+err_request_input_gpio_failed:
+		;
+	}
+	for (i = mi->noutputs - 1; i >= 0; i--) {
+err_output_gpio_configure_failed:
+		gpio_free(mi->output_gpios[i]);
+err_request_output_gpio_failed:
+		;
+	}
+err_bad_keymap:
+	kfree(kp);
+err_kp_alloc_failed:
+err_invalid_platform_data:
+	return err;
+}
diff --git a/drivers/input/misc/gpio_output.c b/drivers/input/misc/gpio_output.c
new file mode 100644
index 000000000000..2aac2fad0a17
--- /dev/null
+++ b/drivers/input/misc/gpio_output.c
@@ -0,0 +1,97 @@
+/* drivers/input/misc/gpio_output.c
+ *
+ * Copyright (C) 2007 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/kernel.h>
+#include <linux/gpio.h>
+#include <linux/gpio_event.h>
+
+int gpio_event_output_event(
+	struct gpio_event_input_devs *input_devs, struct gpio_event_info *info,
+	void **data, unsigned int dev, unsigned int type,
+	unsigned int code, int value)
+{
+	int i;
+	struct gpio_event_output_info *oi;
+	oi = container_of(info, struct gpio_event_output_info, info);
+	if (type != oi->type)
+		return 0;
+	if (!(oi->flags & GPIOEDF_ACTIVE_HIGH))
+		value = !value;
+	for (i = 0; i < oi->keymap_size; i++)
+		if (dev == oi->keymap[i].dev && code == oi->keymap[i].code)
+			gpio_set_value(oi->keymap[i].gpio, value);
+	return 0;
+}
+
+int gpio_event_output_func(
+	struct gpio_event_input_devs *input_devs, struct gpio_event_info *info,
+	void **data, int func)
+{
+	int ret;
+	int i;
+	struct gpio_event_output_info *oi;
+	oi = container_of(info, struct gpio_event_output_info, info);
+
+	if (func == GPIO_EVENT_FUNC_SUSPEND || func == GPIO_EVENT_FUNC_RESUME)
+		return 0;
+
+	if (func == GPIO_EVENT_FUNC_INIT) {
+		int output_level = !(oi->flags & GPIOEDF_ACTIVE_HIGH);
+
+		for (i = 0; i < oi->keymap_size; i++) {
+			int dev = oi->keymap[i].dev;
+			if (dev >= input_devs->count) {
+				pr_err("gpio_event_output_func: bad device "
+					"index %d >= %d for key code %d\n",
+					dev, input_devs->count,
+					oi->keymap[i].code);
+				ret = -EINVAL;
+				goto err_bad_keymap;
+			}
+			input_set_capability(input_devs->dev[dev], oi->type,
+					     oi->keymap[i].code);
+		}
+
+		for (i = 0; i < oi->keymap_size; i++) {
+			ret = gpio_request(oi->keymap[i].gpio,
+					   "gpio_event_output");
+			if (ret) {
+				pr_err("gpio_event_output_func: gpio_request "
+					"failed for %d\n", oi->keymap[i].gpio);
+				goto err_gpio_request_failed;
+			}
+			ret = gpio_direction_output(oi->keymap[i].gpio,
+						    output_level);
+			if (ret) {
+				pr_err("gpio_event_output_func: "
+					"gpio_direction_output failed for %d\n",
+					oi->keymap[i].gpio);
+				goto err_gpio_direction_output_failed;
+			}
+		}
+		return 0;
+	}
+
+	ret = 0;
+	for (i = oi->keymap_size - 1; i >= 0; i--) {
+err_gpio_direction_output_failed:
+		gpio_free(oi->keymap[i].gpio);
+err_gpio_request_failed:
+		;
+	}
+err_bad_keymap:
+	return ret;
+}
+
diff --git a/drivers/input/misc/keychord.c b/drivers/input/misc/keychord.c
new file mode 100644
index 000000000000..3ffab6da411b
--- /dev/null
+++ b/drivers/input/misc/keychord.c
@@ -0,0 +1,387 @@
+/*
+ *  drivers/input/misc/keychord.c
+ *
+ * Copyright (C) 2008 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/poll.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/spinlock.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/keychord.h>
+#include <linux/sched.h>
+
+#define KEYCHORD_NAME		"keychord"
+#define BUFFER_SIZE			16
+
+MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
+MODULE_DESCRIPTION("Key chord input driver");
+MODULE_SUPPORTED_DEVICE("keychord");
+MODULE_LICENSE("GPL");
+
+#define NEXT_KEYCHORD(kc) ((struct input_keychord *) \
+		((char *)kc + sizeof(struct input_keychord) + \
+		kc->count * sizeof(kc->keycodes[0])))
+
+struct keychord_device {
+	struct input_handler	input_handler;
+	int			registered;
+
+	/* list of keychords to monitor */
+	struct input_keychord	*keychords;
+	int			keychord_count;
+
+	/* bitmask of keys contained in our keychords */
+	unsigned long keybit[BITS_TO_LONGS(KEY_CNT)];
+	/* current state of the keys */
+	unsigned long keystate[BITS_TO_LONGS(KEY_CNT)];
+	/* number of keys that are currently pressed */
+	int key_down;
+
+	/* second input_device_id is needed for null termination */
+	struct input_device_id  device_ids[2];
+
+	spinlock_t		lock;
+	wait_queue_head_t	waitq;
+	unsigned char		head;
+	unsigned char		tail;
+	__u16			buff[BUFFER_SIZE];
+};
+
+static int check_keychord(struct keychord_device *kdev,
+		struct input_keychord *keychord)
+{
+	int i;
+
+	if (keychord->count != kdev->key_down)
+		return 0;
+
+	for (i = 0; i < keychord->count; i++) {
+		if (!test_bit(keychord->keycodes[i], kdev->keystate))
+			return 0;
+	}
+
+	/* we have a match */
+	return 1;
+}
+
+static void keychord_event(struct input_handle *handle, unsigned int type,
+			   unsigned int code, int value)
+{
+	struct keychord_device *kdev = handle->private;
+	struct input_keychord *keychord;
+	unsigned long flags;
+	int i, got_chord = 0;
+
+	if (type != EV_KEY || code >= KEY_MAX)
+		return;
+
+	spin_lock_irqsave(&kdev->lock, flags);
+	/* do nothing if key state did not change */
+	if (!test_bit(code, kdev->keystate) == !value)
+		goto done;
+	__change_bit(code, kdev->keystate);
+	if (value)
+		kdev->key_down++;
+	else
+		kdev->key_down--;
+
+	/* don't notify on key up */
+	if (!value)
+		goto done;
+	/* ignore this event if it is not one of the keys we are monitoring */
+	if (!test_bit(code, kdev->keybit))
+		goto done;
+
+	keychord = kdev->keychords;
+	if (!keychord)
+		goto done;
+
+	/* check to see if the keyboard state matches any keychords */
+	for (i = 0; i < kdev->keychord_count; i++) {
+		if (check_keychord(kdev, keychord)) {
+			kdev->buff[kdev->head] = keychord->id;
+			kdev->head = (kdev->head + 1) % BUFFER_SIZE;
+			got_chord = 1;
+			break;
+		}
+		/* skip to next keychord */
+		keychord = NEXT_KEYCHORD(keychord);
+	}
+
+done:
+	spin_unlock_irqrestore(&kdev->lock, flags);
+
+	if (got_chord)
+		wake_up_interruptible(&kdev->waitq);
+}
+
+static int keychord_connect(struct input_handler *handler,
+					  struct input_dev *dev,
+					  const struct input_device_id *id)
+{
+	int i, ret;
+	struct input_handle *handle;
+	struct keychord_device *kdev =
+		container_of(handler, struct keychord_device, input_handler);
+
+	/*
+	 * ignore this input device if it does not contain any keycodes
+	 * that we are monitoring
+	 */
+	for (i = 0; i < KEY_MAX; i++) {
+		if (test_bit(i, kdev->keybit) && test_bit(i, dev->keybit))
+			break;
+	}
+	if (i == KEY_MAX)
+		return -ENODEV;
+
+	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
+	if (!handle)
+		return -ENOMEM;
+
+	handle->dev = dev;
+	handle->handler = handler;
+	handle->name = KEYCHORD_NAME;
+	handle->private = kdev;
+
+	ret = input_register_handle(handle);
+	if (ret)
+		goto err_input_register_handle;
+
+	ret = input_open_device(handle);
+	if (ret)
+		goto err_input_open_device;
+
+	pr_info("keychord: using input dev %s for fevent\n", dev->name);
+
+	return 0;
+
+err_input_open_device:
+	input_unregister_handle(handle);
+err_input_register_handle:
+	kfree(handle);
+	return ret;
+}
+
+static void keychord_disconnect(struct input_handle *handle)
+{
+	input_close_device(handle);
+	input_unregister_handle(handle);
+	kfree(handle);
+}
+
+/*
+ * keychord_read is used to read keychord events from the driver
+ */
+static ssize_t keychord_read(struct file *file, char __user *buffer,
+		size_t count, loff_t *ppos)
+{
+	struct keychord_device *kdev = file->private_data;
+	__u16   id;
+	int retval;
+	unsigned long flags;
+
+	if (count < sizeof(id))
+		return -EINVAL;
+	count = sizeof(id);
+
+	if (kdev->head == kdev->tail && (file->f_flags & O_NONBLOCK))
+		return -EAGAIN;
+
+	retval = wait_event_interruptible(kdev->waitq,
+			kdev->head != kdev->tail);
+	if (retval)
+		return retval;
+
+	spin_lock_irqsave(&kdev->lock, flags);
+	/* pop a keychord ID off the queue */
+	id = kdev->buff[kdev->tail];
+	kdev->tail = (kdev->tail + 1) % BUFFER_SIZE;
+	spin_unlock_irqrestore(&kdev->lock, flags);
+
+	if (copy_to_user(buffer, &id, count))
+		return -EFAULT;
+
+	return count;
+}
+
+/*
+ * keychord_write is used to configure the driver
+ */
+static ssize_t keychord_write(struct file *file, const char __user *buffer,
+		size_t count, loff_t *ppos)
+{
+	struct keychord_device *kdev = file->private_data;
+	struct input_keychord *keychords = 0;
+	struct input_keychord *keychord, *next, *end;
+	int ret, i, key;
+	unsigned long flags;
+
+	if (count < sizeof(struct input_keychord))
+		return -EINVAL;
+	keychords = kzalloc(count, GFP_KERNEL);
+	if (!keychords)
+		return -ENOMEM;
+
+	/* read list of keychords from userspace */
+	if (copy_from_user(keychords, buffer, count)) {
+		kfree(keychords);
+		return -EFAULT;
+	}
+
+	/* unregister handler before changing configuration */
+	if (kdev->registered) {
+		input_unregister_handler(&kdev->input_handler);
+		kdev->registered = 0;
+	}
+
+	spin_lock_irqsave(&kdev->lock, flags);
+	/* clear any existing configuration */
+	kfree(kdev->keychords);
+	kdev->keychords = 0;
+	kdev->keychord_count = 0;
+	kdev->key_down = 0;
+	memset(kdev->keybit, 0, sizeof(kdev->keybit));
+	memset(kdev->keystate, 0, sizeof(kdev->keystate));
+	kdev->head = kdev->tail = 0;
+
+	keychord = keychords;
+	end = (struct input_keychord *)((char *)keychord + count);
+
+	while (keychord < end) {
+		next = NEXT_KEYCHORD(keychord);
+		if (keychord->count <= 0 || next > end) {
+			pr_err("keychord: invalid keycode count %d\n",
+				keychord->count);
+			goto err_unlock_return;
+		}
+		if (keychord->version != KEYCHORD_VERSION) {
+			pr_err("keychord: unsupported version %d\n",
+				keychord->version);
+			goto err_unlock_return;
+		}
+
+		/* keep track of the keys we are monitoring in keybit */
+		for (i = 0; i < keychord->count; i++) {
+			key = keychord->keycodes[i];
+			if (key < 0 || key >= KEY_CNT) {
+				pr_err("keychord: keycode %d out of range\n",
+					key);
+				goto err_unlock_return;
+			}
+			__set_bit(key, kdev->keybit);
+		}
+
+		kdev->keychord_count++;
+		keychord = next;
+	}
+
+	kdev->keychords = keychords;
+	spin_unlock_irqrestore(&kdev->lock, flags);
+
+	ret = input_register_handler(&kdev->input_handler);
+	if (ret) {
+		kfree(keychords);
+		kdev->keychords = 0;
+		return ret;
+	}
+	kdev->registered = 1;
+
+	return count;
+
+err_unlock_return:
+	spin_unlock_irqrestore(&kdev->lock, flags);
+	kfree(keychords);
+	return -EINVAL;
+}
+
+static unsigned int keychord_poll(struct file *file, poll_table *wait)
+{
+	struct keychord_device *kdev = file->private_data;
+
+	poll_wait(file, &kdev->waitq, wait);
+
+	if (kdev->head != kdev->tail)
+		return POLLIN | POLLRDNORM;
+
+	return 0;
+}
+
+static int keychord_open(struct inode *inode, struct file *file)
+{
+	struct keychord_device *kdev;
+
+	kdev = kzalloc(sizeof(struct keychord_device), GFP_KERNEL);
+	if (!kdev)
+		return -ENOMEM;
+
+	spin_lock_init(&kdev->lock);
+	init_waitqueue_head(&kdev->waitq);
+
+	kdev->input_handler.event = keychord_event;
+	kdev->input_handler.connect = keychord_connect;
+	kdev->input_handler.disconnect = keychord_disconnect;
+	kdev->input_handler.name = KEYCHORD_NAME;
+	kdev->input_handler.id_table = kdev->device_ids;
+
+	kdev->device_ids[0].flags = INPUT_DEVICE_ID_MATCH_EVBIT;
+	__set_bit(EV_KEY, kdev->device_ids[0].evbit);
+
+	file->private_data = kdev;
+
+	return 0;
+}
+
+static int keychord_release(struct inode *inode, struct file *file)
+{
+	struct keychord_device *kdev = file->private_data;
+
+	if (kdev->registered)
+		input_unregister_handler(&kdev->input_handler);
+	kfree(kdev);
+
+	return 0;
+}
+
+static const struct file_operations keychord_fops = {
+	.owner		= THIS_MODULE,
+	.open		= keychord_open,
+	.release	= keychord_release,
+	.read		= keychord_read,
+	.write		= keychord_write,
+	.poll		= keychord_poll,
+};
+
+static struct miscdevice keychord_misc = {
+	.fops		= &keychord_fops,
+	.name		= KEYCHORD_NAME,
+	.minor		= MISC_DYNAMIC_MINOR,
+};
+
+static int __init keychord_init(void)
+{
+	return misc_register(&keychord_misc);
+}
+
+static void __exit keychord_exit(void)
+{
+	misc_deregister(&keychord_misc);
+}
+
+module_init(keychord_init);
+module_exit(keychord_exit);
diff --git a/drivers/input/mouse/synaptics.c b/drivers/input/mouse/synaptics.c
index 5538fc657af1..767536372181 100644
--- a/drivers/input/mouse/synaptics.c
+++ b/drivers/input/mouse/synaptics.c
@@ -24,6 +24,7 @@
  */
 
 #include <linux/module.h>
+#include <linux/delay.h>
 #include <linux/dmi.h>
 #include <linux/input/mt.h>
 #include <linux/serio.h>
@@ -786,6 +787,16 @@ static int synaptics_reconnect(struct psmouse *psmouse)
 
 	do {
 		psmouse_reset(psmouse);
+		if (retry) {
+			/*
+			 * On some boxes, right after resuming, the touchpad
+			 * needs some time to finish initializing (I assume
+			 * it needs time to calibrate) and start responding
+			 * to Synaptics-specific queries, so let's wait a
+			 * bit.
+			 */
+			ssleep(1);
+		}
 		error = synaptics_detect(psmouse, 0);
 	} while (error && ++retry < 3);
 
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index cabd9e54863f..936f1e71a21f 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -374,6 +374,19 @@ config TOUCHSCREEN_MIGOR
 	  To compile this driver as a module, choose M here: the
 	  module will be called migor_ts.
 
+config TOUCHSCREEN_PANJIT_I2C
+	tristate "PANJIT I2C touchscreen driver"
+	depends on I2C
+	default n
+	help
+	  Say Y here to enable PANJIT I2C capacitive touchscreen support,
+	  covering devices such as the MGG1010AI06 and EGG1010AI06
+
+	  If unsure, say N
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called panjit_i2c.
+
 config TOUCHSCREEN_TNETV107X
 	tristate "TI TNETV107X touchscreen support"
 	depends on ARCH_DAVINCI_TNETV107X
@@ -383,6 +396,12 @@ config TOUCHSCREEN_TNETV107X
 	  To compile this driver as a module, choose M here: the
 	  module will be called tnetv107x-ts.
 
+config TOUCHSCREEN_SYNAPTICS_I2C_RMI
+	tristate "Synaptics i2c touchscreen"
+	depends on I2C
+	help
+	  This enables support for Synaptics RMI over I2C based touchscreens.
+
 config TOUCHSCREEN_TOUCHRIGHT
 	tristate "Touchright serial touchscreen"
 	select SERIO
@@ -726,4 +745,36 @@ config TOUCHSCREEN_TPS6507X
 	  To compile this driver as a module, choose M here: the
 	  module will be called tps6507x_ts.
 
+config TOUCHSCREEN_RM31080A
+	tristate "RAYDIUM_31080A based touchscreens"
+	depends on SPI_MASTER
+	help
+	  Say Y here if you have a touchscreen interface using the
+	  RAYDIUM_T31080A controller, and your board-specific initialization
+	  code includes that in its table of SPI devices.
+
+	  If unsure, say N (but it's safe to say "Y").
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called RAYDIUM_31080A.
+
+config TOUCHSCREEN_SYN_RMI4_SPI
+	tristate "RMI4 SPI Support"
+	depends on SPI_MASTER
+	help
+	  Say Y here if you want to support RMI4 devices connect
+	  to an SPI bus.
+
+	  If unsure, say Y.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called rmi-spi.
+
+config TOUCHSCREEN_FUSION_F0710A
+	tristate "TouchRevolution Fusion F0710A Touchscreens"
+	depends on I2C
+	help
+	  Say Y here if you want to support the multi-touch input driver for
+	  the TouchRevolution Fusion 7 and 10 panels.
+
 endif
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index 282d6f76ae26..2fa571b65088 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -37,12 +37,14 @@ obj-$(CONFIG_TOUCHSCREEN_HP600)		+= hp680_ts_input.o
 obj-$(CONFIG_TOUCHSCREEN_HP7XX)		+= jornada720_ts.o
 obj-$(CONFIG_TOUCHSCREEN_HTCPEN)	+= htcpen.o
 obj-$(CONFIG_TOUCHSCREEN_USB_COMPOSITE)	+= usbtouchscreen.o
+obj-$(CONFIG_TOUCHSCREEN_PANJIT_I2C)	+= panjit_i2c.o
 obj-$(CONFIG_TOUCHSCREEN_PCAP)		+= pcap_ts.o
 obj-$(CONFIG_TOUCHSCREEN_PENMOUNT)	+= penmount.o
 obj-$(CONFIG_TOUCHSCREEN_S3C2410)	+= s3c2410_ts.o
 obj-$(CONFIG_TOUCHSCREEN_ST1232)	+= st1232.o
 obj-$(CONFIG_TOUCHSCREEN_STMPE)		+= stmpe-ts.o
 obj-$(CONFIG_TOUCHSCREEN_TNETV107X)	+= tnetv107x-ts.o
+obj-$(CONFIG_TOUCHSCREEN_SYNAPTICS_I2C_RMI)	+= synaptics_i2c_rmi.o
 obj-$(CONFIG_TOUCHSCREEN_TOUCHIT213)	+= touchit213.o
 obj-$(CONFIG_TOUCHSCREEN_TOUCHRIGHT)	+= touchright.o
 obj-$(CONFIG_TOUCHSCREEN_TOUCHWIN)	+= touchwin.o
@@ -60,3 +62,6 @@ obj-$(CONFIG_TOUCHSCREEN_WM97XX_MAINSTONE)	+= mainstone-wm97xx.o
 obj-$(CONFIG_TOUCHSCREEN_WM97XX_ZYLONITE)	+= zylonite-wm97xx.o
 obj-$(CONFIG_TOUCHSCREEN_W90X900)	+= w90p910_ts.o
 obj-$(CONFIG_TOUCHSCREEN_TPS6507X)	+= tps6507x-ts.o
+obj-$(CONFIG_TOUCHSCREEN_RM31080A)	+= rm31080a_ts.o
+obj-$(CONFIG_TOUCHSCREEN_SYN_RMI4_SPI)  += rmi4/
+obj-$(CONFIG_TOUCHSCREEN_FUSION_F0710A)	+= fusion_F0710A.o
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index f5d66859f232..6721a1fd2456 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -2,6 +2,8 @@
  * Atmel maXTouch Touchscreen driver
  *
  * Copyright (C) 2010 Samsung Electronics Co.Ltd
+ * Copyright (C) 2011 Atmel Corporation
+ * Copyright (C) 2011-2012 NVIDIA Corporation
  * Author: Joonyoung Shim <jy0922.shim@samsung.com>
  *
  * This program is free software; you can redistribute  it and/or modify it
@@ -17,9 +19,19 @@
 #include <linux/firmware.h>
 #include <linux/i2c.h>
 #include <linux/i2c/atmel_mxt_ts.h>
-#include <linux/input/mt.h>
+#include <linux/input.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+#include <linux/earlysuspend.h>
+#endif
+#define CREATE_TRACE_POINTS
+#include <trace/events/nvevent.h>
+
+/* Family ID */
+#define MXT224_ID		0x80
+#define MXT768E_ID		0xA1
+#define MXT1386_ID		0xA0
 
 /* Version */
 #define MXT_VER_20		20
@@ -172,14 +184,37 @@
 #define MXT_VOLTAGE_DEFAULT	2700000
 #define MXT_VOLTAGE_STEP	10000
 
+/* Defines for Suspend/Resume */
+#define MXT_SUSPEND_STATIC	0
+#define MXT_SUSPEND_DYNAMIC	1
+#define MXT_T7_IDLEACQ_DISABLE	0
+#define MXT_T7_ACTVACQ_DISABLE	0
+#define MXT_T7_ACTV2IDLE_DISABLE 0
+#define MXT_T9_DISABLE		0
+#define MXT_T9_ENABLE		0x83
+#define MXT_T22_DISABLE		0
+
 /* Define for MXT_GEN_COMMAND_T6 */
 #define MXT_BOOT_VALUE		0xa5
 #define MXT_BACKUP_VALUE	0x55
-#define MXT_BACKUP_TIME		25	/* msec */
-#define MXT_RESET_TIME		65	/* msec */
+#define MXT_BACKUP_TIME		200	/* msec */
+#define MXT224_RESET_TIME       65      /* msec */
+#define MXT768E_RESET_TIME	250	/* msec */
+#define MXT1386_RESET_TIME      200     /* msec */
+#define MXT_RESET_TIME		200	/* msec */
+#define MXT_RESET_NOCHGREAD     400     /* msec */
+
+#define MXT_WAKEUP_TIME		25	/* msec */
 
 #define MXT_FWRESET_TIME	175	/* msec */
 
+/* Defines for MXT_SLOWSCAN_EXTENSIONS */
+#define SLOSCAN_DISABLE		0	/* Disable slow scan */
+#define SLOSCAN_ENABLE		1	/* Enable slow scan */
+#define SLOSCAN_SET_ACTVACQINT	2	/* Set ACTV scan rate */
+#define SLOSCAN_SET_IDLEACQINT	3	/* Set IDLE scan rate */
+#define SLOSCAN_SET_ACTV2IDLETO 4	/* Set the ACTIVE to IDLE TimeOut */
+
 /* Command to unlock bootloader */
 #define MXT_UNLOCK_CMD_MSB	0xaa
 #define MXT_UNLOCK_CMD_LSB	0xdc
@@ -210,8 +245,15 @@
 /* Touchscreen absolute values */
 #define MXT_MAX_AREA		0xff
 
+/* Fixed Report ID values */
+#define MXT_RPTID_NOMSG		0xFF	/* No messages available to read */
+
 #define MXT_MAX_FINGER		10
 
+#define RESUME_READS		100
+
+#define MXT_DEFAULT_PRESSURE	100
+
 struct mxt_info {
 	u8 family_id;
 	u8 variant_id;
@@ -225,11 +267,12 @@ struct mxt_info {
 struct mxt_object {
 	u8 type;
 	u16 start_address;
-	u8 size;
-	u8 instances;
+	u16 size;
+	u16 instances;
 	u8 num_report_ids;
 
 	/* to map object and message */
+	u8 min_reportid;
 	u8 max_reportid;
 };
 
@@ -247,6 +290,15 @@ struct mxt_finger {
 	int pressure;
 };
 
+/* This structure is used to save/restore values during suspend/resume */
+struct mxt_suspend {
+	u8 suspend_obj;
+	u8 suspend_reg;
+	u8 suspend_val;
+	u8 suspend_flags;
+	u8 restore_val;
+};
+
 /* Each client has this additional data */
 struct mxt_data {
 	struct i2c_client *client;
@@ -258,8 +310,42 @@ struct mxt_data {
 	unsigned int irq;
 	unsigned int max_x;
 	unsigned int max_y;
+	u8(*read_chg) (void);
+	u16 msg_address;
+	u16 last_address;
+	u8 actv_cycle_time;
+	u8 idle_cycle_time;
+	u8 actv2idle_timeout;
+	u8 is_stopped;
+	struct mutex access_mutex;
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+	struct early_suspend early_suspend;
+#endif
+	unsigned int driver_paused;
+	struct bin_attribute mem_access_attr;
+	int debug_enabled;
+	int slowscan_enabled;
+	u8 slowscan_actv_cycle_time;
+	u8 slowscan_idle_cycle_time;
+	u8 slowscan_actv2idle_timeout;
+	u8 slowscan_shad_actv_cycle_time;
+	u8 slowscan_shad_idle_cycle_time;
+	u8 slowscan_shad_actv2idle_timeout;
+};
+
+static struct mxt_suspend mxt_save[] = {
+	{MXT_TOUCH_MULTI_T9, MXT_TOUCH_CTRL, MXT_T9_DISABLE, MXT_SUSPEND_DYNAMIC, 0},
+	{MXT_PROCG_NOISE_T22, MXT_NOISE_CTRL, MXT_T22_DISABLE, MXT_SUSPEND_DYNAMIC, 0},
+	{MXT_GEN_POWER_T7, MXT_POWER_IDLEACQINT, MXT_T7_IDLEACQ_DISABLE, MXT_SUSPEND_DYNAMIC, 0},
+	{MXT_GEN_POWER_T7, MXT_POWER_ACTVACQINT, MXT_T7_ACTVACQ_DISABLE, MXT_SUSPEND_DYNAMIC, 0},
+	{MXT_GEN_POWER_T7, MXT_POWER_ACTV2IDLETO, MXT_T7_ACTV2IDLE_DISABLE, MXT_SUSPEND_DYNAMIC, 0}
 };
 
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+static void mxt_early_suspend(struct early_suspend *es);
+static void mxt_early_resume(struct early_suspend *es);
+#endif
+
 static bool mxt_object_readable(unsigned int type)
 {
 	switch (type) {
@@ -400,30 +486,46 @@ static int mxt_fw_write(struct i2c_client *client,
 static int __mxt_read_reg(struct i2c_client *client,
 			       u16 reg, u16 len, void *val)
 {
-	struct i2c_msg xfer[2];
 	u8 buf[2];
+	int retval = 0;
+	struct mxt_data *data = i2c_get_clientdata(client);
 
 	buf[0] = reg & 0xff;
 	buf[1] = (reg >> 8) & 0xff;
 
-	/* Write register */
-	xfer[0].addr = client->addr;
-	xfer[0].flags = 0;
-	xfer[0].len = 2;
-	xfer[0].buf = buf;
+	mutex_lock(&data->access_mutex);
 
-	/* Read data */
-	xfer[1].addr = client->addr;
-	xfer[1].flags = I2C_M_RD;
-	xfer[1].len = len;
-	xfer[1].buf = val;
+	if ((data->last_address != reg) || (reg != data->msg_address)) {
+		if (i2c_master_send(client, (u8 *)buf, 2) != 2) {
+			dev_dbg(&client->dev, "i2c retry\n");
+			msleep(MXT_WAKEUP_TIME);
 
-	if (i2c_transfer(client->adapter, xfer, 2) != 2) {
-		dev_err(&client->dev, "%s: i2c transfer failed\n", __func__);
-		return -EIO;
+			if (i2c_master_send(client, (u8 *)buf, 2) != 2) {
+				dev_err(&client->dev, "%s: i2c send failed\n",
+					__func__);
+				retval = -EIO;
+				goto mxt_read_exit;
+			}
+		}
 	}
 
-	return 0;
+	if (i2c_master_recv(client, (u8 *)val, len) != len) {
+		dev_dbg(&client->dev, "i2c retry\n");
+		msleep(MXT_WAKEUP_TIME);
+
+		if (i2c_master_recv(client, (u8 *)val, len) != len) {
+			dev_err(&client->dev, "%s: i2c recv failed\n",
+				__func__);
+			retval = -EIO;
+			goto mxt_read_exit;
+		}
+	}
+
+	data->last_address = reg;
+
+mxt_read_exit:
+	mutex_unlock(&data->access_mutex);
+	return retval;
 }
 
 static int mxt_read_reg(struct i2c_client *client, u16 reg, u8 *val)
@@ -434,17 +536,29 @@ static int mxt_read_reg(struct i2c_client *client, u16 reg, u8 *val)
 static int mxt_write_reg(struct i2c_client *client, u16 reg, u8 val)
 {
 	u8 buf[3];
+	int retval = 0;
+	struct mxt_data *data = i2c_get_clientdata(client);
 
 	buf[0] = reg & 0xff;
 	buf[1] = (reg >> 8) & 0xff;
 	buf[2] = val;
 
+	mutex_lock(&data->access_mutex);
 	if (i2c_master_send(client, buf, 3) != 3) {
-		dev_err(&client->dev, "%s: i2c send failed\n", __func__);
-		return -EIO;
+		dev_dbg(&client->dev, "i2c retry\n");
+		msleep(MXT_WAKEUP_TIME);
+
+		if (i2c_master_send(client, buf, 3) != 3) {
+			dev_err(&client->dev, "%s: i2c send failed\n", __func__);
+			retval = -EIO;
+			goto mxt_write_exit;
+		}
 	}
+	data->last_address = reg + 1;
 
-	return 0;
+mxt_write_exit:
+	mutex_unlock(&data->access_mutex);
+	return retval;
 }
 
 static int mxt_read_object_table(struct i2c_client *client,
@@ -466,7 +580,7 @@ mxt_get_object(struct mxt_data *data, u8 type)
 			return object;
 	}
 
-	dev_err(&data->client->dev, "Invalid object type\n");
+	dev_err(&data->client->dev, "Invalid object type T%d\n", type);
 	return NULL;
 }
 
@@ -525,23 +639,21 @@ static void mxt_input_report(struct mxt_data *data, int single_id)
 		if (!finger[id].status)
 			continue;
 
-		input_mt_slot(input_dev, id);
-		input_mt_report_slot_state(input_dev, MT_TOOL_FINGER,
-				finger[id].status != MXT_RELEASE);
-
-		if (finger[id].status != MXT_RELEASE) {
-			finger_num++;
-			input_report_abs(input_dev, ABS_MT_TOUCH_MAJOR,
-					finger[id].area);
-			input_report_abs(input_dev, ABS_MT_POSITION_X,
-					finger[id].x);
-			input_report_abs(input_dev, ABS_MT_POSITION_Y,
-					finger[id].y);
-			input_report_abs(input_dev, ABS_MT_PRESSURE,
-					finger[id].pressure);
-		} else {
+		input_report_abs(input_dev, ABS_MT_TOUCH_MAJOR,
+				finger[id].status != MXT_RELEASE ?
+				finger[id].area : 0);
+		input_report_abs(input_dev, ABS_MT_POSITION_X,
+				finger[id].x);
+		input_report_abs(input_dev, ABS_MT_POSITION_Y,
+				finger[id].y);
+		input_report_abs(input_dev, ABS_MT_PRESSURE,
+				finger[id].pressure);
+		input_mt_sync(input_dev);
+
+		if (finger[id].status == MXT_RELEASE)
 			finger[id].status = 0;
-		}
+		else
+			finger_num++;
 	}
 
 	input_report_key(input_dev, BTN_TOUCH, finger_num > 0);
@@ -549,8 +661,7 @@ static void mxt_input_report(struct mxt_data *data, int single_id)
 	if (status != MXT_RELEASE) {
 		input_report_abs(input_dev, ABS_X, finger[single_id].x);
 		input_report_abs(input_dev, ABS_Y, finger[single_id].y);
-		input_report_abs(input_dev,
-				 ABS_PRESSURE, finger[single_id].pressure);
+		input_report_abs(input_dev, ABS_PRESSURE, finger[single_id].pressure);
 	}
 
 	input_sync(input_dev);
@@ -573,6 +684,7 @@ static void mxt_input_touchevent(struct mxt_data *data,
 			dev_dbg(dev, "[%d] released\n", id);
 
 			finger[id].status = MXT_RELEASE;
+			finger[id].pressure = 0;
 			mxt_input_report(data, id);
 		}
 		return;
@@ -592,6 +704,9 @@ static void mxt_input_touchevent(struct mxt_data *data,
 	area = message->message[4];
 	pressure = message->message[5];
 
+	if ((pressure <= 0) || (pressure > 255))
+		pressure = MXT_DEFAULT_PRESSURE;
+
 	dev_dbg(dev, "[%d] %s x: %d, y: %d, area: %d\n", id,
 		status & MXT_MOVE ? "moved" : "pressed",
 		x, y, area);
@@ -603,6 +718,7 @@ static void mxt_input_touchevent(struct mxt_data *data,
 	finger[id].area = area;
 	finger[id].pressure = pressure;
 
+	trace_nvevent_irq_data_submit("mxt_input_touchevent");
 	mxt_input_report(data, id);
 }
 
@@ -610,144 +726,293 @@ static irqreturn_t mxt_interrupt(int irq, void *dev_id)
 {
 	struct mxt_data *data = dev_id;
 	struct mxt_message message;
-	struct mxt_object *object;
+	struct mxt_object *touch_object;
 	struct device *dev = &data->client->dev;
-	int id;
+	int touchid;
 	u8 reportid;
-	u8 max_reportid;
-	u8 min_reportid;
 
+	trace_nvevent_irq_data_read_start_series("mxt_input_interrupt");
 	do {
+		trace_nvevent_irq_data_read_start_single("mxt_input_interrupt");
 		if (mxt_read_message(data, &message)) {
 			dev_err(dev, "Failed to read message\n");
 			goto end;
 		}
+		trace_nvevent_irq_data_read_finish_single(
+					"mxt_input_interrupt");
 
 		reportid = message.reportid;
 
-		/* whether reportid is thing of MXT_TOUCH_MULTI_T9 */
-		object = mxt_get_object(data, MXT_TOUCH_MULTI_T9);
-		if (!object)
+		touch_object = mxt_get_object(data, MXT_TOUCH_MULTI_T9);
+		if (!touch_object)
 			goto end;
 
-		max_reportid = object->max_reportid;
-		min_reportid = max_reportid - object->num_report_ids + 1;
-		id = reportid - min_reportid;
+		if (data->debug_enabled)
+			print_hex_dump(KERN_DEBUG, "MXT MSG:", DUMP_PREFIX_NONE,
+				16, 1, &message, sizeof(struct mxt_message), false);
 
-		if (reportid >= min_reportid && reportid <= max_reportid)
-			mxt_input_touchevent(data, &message, id);
-		else
+		if (reportid >= touch_object->min_reportid
+			&& reportid <= touch_object->max_reportid) {
+			touchid = reportid - touch_object->min_reportid;
+			mxt_input_touchevent(data, &message, touchid);
+		} else if (reportid != MXT_RPTID_NOMSG)
 			mxt_dump_message(dev, &message);
-	} while (reportid != 0xff);
+	} while (reportid != MXT_RPTID_NOMSG);
+	trace_nvevent_irq_data_read_finish_series("mxt_input_interrupt");
 
 end:
 	return IRQ_HANDLED;
 }
 
+static int mxt_make_highchg(struct mxt_data *data)
+{
+	struct device *dev = &data->client->dev;
+	struct mxt_message message;
+	int count = 30;
+	int error;
+
+	/* Read dummy message to make high CHG pin */
+	do {
+		error = mxt_read_message(data, &message);
+		if (error)
+			return error;
+	} while (message.reportid != MXT_RPTID_NOMSG && --count);
+
+	if (!count) {
+		dev_err(dev, "CHG pin isn't cleared\n");
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
 static int mxt_check_reg_init(struct mxt_data *data)
 {
+	struct i2c_client *client = data->client;
 	const struct mxt_platform_data *pdata = data->pdata;
 	struct mxt_object *object;
+	struct mxt_message message;
 	struct device *dev = &data->client->dev;
 	int index = 0;
+	int timeout_counter = 0;
 	int i, j, config_offset;
+	int error;
+	unsigned long current_crc;
+	u8 command_register;
 
 	if (!pdata->config) {
 		dev_dbg(dev, "No cfg data defined, skipping reg init\n");
 		return 0;
 	}
 
+	/* Try to read the config checksum of the existing cfg */
+	mxt_write_object(data, MXT_GEN_COMMAND_T6,
+			 MXT_COMMAND_REPORTALL, 1);
+	msleep(30);
+
+	error = mxt_read_message(data, &message);
+	if (error)
+		return error;
+
+	object = mxt_get_object(data, MXT_GEN_COMMAND_T6);
+	if (!object)
+		return -EIO;
+
+	/* Check if this message is from command processor (which has
+	   only one reporting ID), if so, bytes 1-3 are the checksum. */
+	if (message.reportid == object->max_reportid) {
+		current_crc = message.message[1] | (message.message[2] << 8) |
+			      (message.message[3] << 16);
+	} else {
+		dev_info(dev, "Couldn't retrieve the current cfg checksum, "
+			 "forcing load\n");
+		current_crc = 0xFFFFFFFF;
+	}
+	dev_info(dev,
+		 "Config CRC read from the mXT: %X\n",
+		 (unsigned int) current_crc);
+
+	if (current_crc == pdata->config_crc) {
+		dev_info(dev,
+			 "Matching CRC's, skipping CFG load.\n");
+		return 0;
+	} else {
+		dev_info(dev, "Doesn't match platform data config CRC (%X), "
+			 "writing config from platform data...\n",
+			 (unsigned int) pdata->config_crc);
+	}
+
 	for (i = 0; i < data->info.object_num; i++) {
 		object = data->object_table + i;
 
 		if (!mxt_object_writable(object->type))
 			continue;
-
+		dev_info(dev, "Writing object type %d, config offset %d", data->object_table[i].type, index);
 		for (j = 0;
-		     j < (object->size + 1) * (object->instances + 1);
+		     j < object->size * object->instances;
 		     j++) {
 			config_offset = index + j;
 			if (config_offset > pdata->config_length) {
 				dev_err(dev, "Not enough config data!\n");
+				dev_err(dev, "config base is %d, offset is %d\n", index, config_offset);
 				return -EINVAL;
 			}
 			mxt_write_object(data, object->type, j,
 					 pdata->config[config_offset]);
 		}
-		index += (object->size + 1) * (object->instances + 1);
+		index += object->size * object->instances;
+	}
+	dev_info(dev, "Config written!");
+
+	error = mxt_make_highchg(data);
+	if (error)
+		return error;
+
+	/* Backup to memory */
+	mxt_write_object(data, MXT_GEN_COMMAND_T6,
+			MXT_COMMAND_BACKUPNV,
+			MXT_BACKUP_VALUE);
+	msleep(MXT_BACKUP_TIME);
+	do {
+		error =  mxt_read_object(data, MXT_GEN_COMMAND_T6,
+					MXT_COMMAND_BACKUPNV,
+					&command_register);
+		if (error)
+			return error;
+		msleep(10);
+	} while ((command_register != 0) && (timeout_counter++ <= 100));
+	if (timeout_counter > 100) {
+		dev_err(&client->dev, "No response after backup!\n");
+		return -EIO;
+	}
+
+	/* Clear the interrupt line */
+	error = mxt_make_highchg(data);
+	if (error)
+		return error;
+
+	/* Soft reset */
+	mxt_write_object(data, MXT_GEN_COMMAND_T6,
+			MXT_COMMAND_RESET, 1);
+	if (data->pdata->read_chg == NULL) {
+		msleep(MXT_RESET_NOCHGREAD);
+	} else {
+		switch (data->info.family_id) {
+		case MXT224_ID:
+			msleep(MXT224_RESET_TIME);
+			break;
+		case MXT768E_ID:
+			msleep(MXT768E_RESET_TIME);
+			break;
+		case MXT1386_ID:
+			msleep(MXT1386_RESET_TIME);
+			break;
+		default:
+			msleep(MXT_RESET_TIME);
+		}
+		timeout_counter = 0;
+		while ((timeout_counter++ <= 100) && data->pdata->read_chg())
+			msleep(10);
+		if (timeout_counter > 100) {
+			dev_err(&client->dev, "No response after reset!\n");
+			return -EIO;
+		}
 	}
 
 	return 0;
 }
 
-static int mxt_make_highchg(struct mxt_data *data)
+
+static void mxt_handle_pdata(struct mxt_data *data)
+{
+	const struct mxt_platform_data *pdata = data->pdata;
+
+	if (pdata->read_chg != NULL)
+		data->read_chg = pdata->read_chg;
+}
+
+static int mxt_set_power_cfg(struct mxt_data *data, u8 sleep)
 {
 	struct device *dev = &data->client->dev;
-	struct mxt_message message;
-	int count = 10;
 	int error;
+	u8 actv_cycle_time = 0;
+	u8 idle_cycle_time = 0;
+	u8 actv2idle_timeout = data->actv2idle_timeout;
 
-	/* Read dummy message to make high CHG pin */
-	do {
-		error = mxt_read_message(data, &message);
-		if (error)
-			return error;
-	} while (message.reportid != 0xff && --count);
-
-	if (!count) {
-		dev_err(dev, "CHG pin isn't cleared\n");
-		return -EBUSY;
+	if (!sleep) {
+		actv_cycle_time = data->actv_cycle_time;
+		idle_cycle_time = data->idle_cycle_time;
 	}
 
+	error = mxt_write_object(data, MXT_GEN_POWER_T7, MXT_POWER_ACTVACQINT,
+				 actv_cycle_time);
+	if (error)
+		goto i2c_error;
+
+	error = mxt_write_object(data, MXT_GEN_POWER_T7, MXT_POWER_IDLEACQINT,
+				 idle_cycle_time);
+	if (error)
+		goto i2c_error;
+
+	error = mxt_write_object(data, MXT_GEN_POWER_T7, MXT_POWER_ACTV2IDLETO,
+				 actv2idle_timeout);
+	if (error)
+		goto i2c_error;
+
+	dev_dbg(dev, "%s: Set ACTV %d, IDLE %d", __func__,
+		actv_cycle_time, idle_cycle_time);
+
 	return 0;
+
+i2c_error:
+	dev_err(dev, "Failed to set power cfg");
+	return error;
 }
 
-static void mxt_handle_pdata(struct mxt_data *data)
+static int mxt_init_power_cfg(struct mxt_data *data)
 {
 	const struct mxt_platform_data *pdata = data->pdata;
-	u8 voltage;
-
-	/* Set touchscreen lines */
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9, MXT_TOUCH_XSIZE,
-			pdata->x_line);
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9, MXT_TOUCH_YSIZE,
-			pdata->y_line);
-
-	/* Set touchscreen orient */
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9, MXT_TOUCH_ORIENT,
-			pdata->orient);
-
-	/* Set touchscreen burst length */
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9,
-			MXT_TOUCH_BLEN, pdata->blen);
-
-	/* Set touchscreen threshold */
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9,
-			MXT_TOUCH_TCHTHR, pdata->threshold);
-
-	/* Set touchscreen resolution */
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9,
-			MXT_TOUCH_XRANGE_LSB, (pdata->x_size - 1) & 0xff);
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9,
-			MXT_TOUCH_XRANGE_MSB, (pdata->x_size - 1) >> 8);
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9,
-			MXT_TOUCH_YRANGE_LSB, (pdata->y_size - 1) & 0xff);
-	mxt_write_object(data, MXT_TOUCH_MULTI_T9,
-			MXT_TOUCH_YRANGE_MSB, (pdata->y_size - 1) >> 8);
-
-	/* Set touchscreen voltage */
-	if (pdata->voltage) {
-		if (pdata->voltage < MXT_VOLTAGE_DEFAULT) {
-			voltage = (MXT_VOLTAGE_DEFAULT - pdata->voltage) /
-				MXT_VOLTAGE_STEP;
-			voltage = 0xff - voltage + 1;
-		} else
-			voltage = (pdata->voltage - MXT_VOLTAGE_DEFAULT) /
-				MXT_VOLTAGE_STEP;
-
-		mxt_write_object(data, MXT_SPT_CTECONFIG_T28,
-				MXT_CTE_VOLTAGE, voltage);
+	struct device *dev = &data->client->dev;
+	int error;
+
+	data->slowscan_actv_cycle_time = 120;	/* 120mS */
+	data->slowscan_idle_cycle_time = 10;	/* 10mS */
+	data->slowscan_actv2idle_timeout = 100;	/* 10 seconds */
+	if (pdata->actv_cycle_time > 0 && pdata->idle_cycle_time > 0) {
+		data->actv_cycle_time = pdata->actv_cycle_time;
+		data->idle_cycle_time = pdata->idle_cycle_time;
+	} else {
+		error = mxt_read_object(data, MXT_GEN_POWER_T7,
+			MXT_POWER_ACTVACQINT,
+			&data->actv_cycle_time);
+
+		if (error)
+			return error;
+
+		error = mxt_read_object(data, MXT_GEN_POWER_T7,
+			MXT_POWER_IDLEACQINT,
+			&data->idle_cycle_time);
+
+		if (error)
+			return error;
 	}
+
+	error = mxt_read_object(data, MXT_GEN_POWER_T7,
+		MXT_POWER_ACTV2IDLETO,
+		&data->actv2idle_timeout);
+
+	if (error)
+		return error;
+
+	/* On init, power up */
+	error = mxt_set_power_cfg(data, 0);
+	if (error)
+		return error;
+
+	dev_info(dev, "Initialised power cfg: ACTV %d, IDLE %d",
+		data->actv_cycle_time, data->idle_cycle_time);
+
+	return 0;
 }
 
 static int mxt_get_info(struct mxt_data *data)
@@ -757,6 +1022,9 @@ static int mxt_get_info(struct mxt_data *data)
 	int error;
 	u8 val;
 
+	/* force send of address pointer on first read during probe */
+	data->last_address = -1;
+
 	error = mxt_read_reg(client, MXT_FAMILY_ID, &val);
 	if (error)
 		return error;
@@ -787,6 +1055,7 @@ static int mxt_get_info(struct mxt_data *data)
 
 static int mxt_get_object_table(struct mxt_data *data)
 {
+	struct device *dev = &data->client->dev;
 	int error;
 	int i;
 	u16 reg;
@@ -803,15 +1072,27 @@ static int mxt_get_object_table(struct mxt_data *data)
 
 		object->type = buf[0];
 		object->start_address = (buf[2] << 8) | buf[1];
-		object->size = buf[3];
-		object->instances = buf[4];
+		object->size = buf[3] + 1;
+		object->instances = buf[4] + 1;
 		object->num_report_ids = buf[5];
 
 		if (object->num_report_ids) {
-			reportid += object->num_report_ids *
-					(object->instances + 1);
+			reportid += object->num_report_ids * object->instances;
 			object->max_reportid = reportid;
+			object->min_reportid = object->max_reportid -
+				object->instances * object->num_report_ids + 1;
 		}
+
+		/* Store message window address so we don't have to
+		   search the object table every time we read message */
+		if (object->type == MXT_GEN_MESSAGE_T5)
+			data->msg_address = object->start_address;
+
+		dev_dbg(dev, "T%d, start:%d size:%d instances:%d "
+			"min_reportid:%d max_reportid:%d\n",
+			object->type, object->start_address, object->size,
+			object->instances,
+			object->min_reportid, object->max_reportid);
 	}
 
 	return 0;
@@ -822,7 +1103,6 @@ static int mxt_initialize(struct mxt_data *data)
 	struct i2c_client *client = data->client;
 	struct mxt_info *info = &data->info;
 	int error;
-	u8 val;
 
 	error = mxt_get_info(data);
 	if (error)
@@ -838,37 +1118,25 @@ static int mxt_initialize(struct mxt_data *data)
 
 	/* Get object table information */
 	error = mxt_get_object_table(data);
-	if (error)
+	if (error) {
+		dev_err(&client->dev, "Failed to read object table\n");
 		return error;
+	}
 
-	/* Check register init values */
+	/* Load initial touch chip configuration */
 	error = mxt_check_reg_init(data);
-	if (error)
+	if (error) {
+		dev_err(&client->dev, "Failed to initialize configuration\n");
 		return error;
+	}
 
 	mxt_handle_pdata(data);
 
-	/* Backup to memory */
-	mxt_write_object(data, MXT_GEN_COMMAND_T6,
-			MXT_COMMAND_BACKUPNV,
-			MXT_BACKUP_VALUE);
-	msleep(MXT_BACKUP_TIME);
-
-	/* Soft reset */
-	mxt_write_object(data, MXT_GEN_COMMAND_T6,
-			MXT_COMMAND_RESET, 1);
-	msleep(MXT_RESET_TIME);
-
-	/* Update matrix size at info struct */
-	error = mxt_read_reg(client, MXT_MATRIX_X_SIZE, &val);
-	if (error)
-		return error;
-	info->matrix_xsize = val;
-
-	error = mxt_read_reg(client, MXT_MATRIX_Y_SIZE, &val);
-	if (error)
+	error = mxt_init_power_cfg(data);
+	if (error) {
+		dev_err(&client->dev, "Failed to initialize power cfg\n");
 		return error;
-	info->matrix_ysize = val;
+	}
 
 	dev_info(&client->dev,
 			"Family ID: %d Variant ID: %d Version: %d Build: %d\n",
@@ -910,26 +1178,35 @@ static ssize_t mxt_object_show(struct device *dev,
 	for (i = 0; i < data->info.object_num; i++) {
 		object = data->object_table + i;
 
-		count += sprintf(buf + count,
-				"Object Table Element %d(Type %d)\n",
+		count += snprintf(buf + count, PAGE_SIZE - count,
+				"Object[%d] (Type %d)\n",
 				i + 1, object->type);
+		if (count >= PAGE_SIZE)
+			return PAGE_SIZE - 1;
 
 		if (!mxt_object_readable(object->type)) {
-			count += sprintf(buf + count, "\n");
+			count += snprintf(buf + count, PAGE_SIZE - count,
+					"\n");
+			if (count >= PAGE_SIZE)
+				return PAGE_SIZE - 1;
 			continue;
 		}
 
-		for (j = 0; j < object->size + 1; j++) {
+		for (j = 0; j < object->size; j++) {
 			error = mxt_read_object(data,
 						object->type, j, &val);
 			if (error)
 				return error;
 
-			count += sprintf(buf + count,
-					"  Byte %d: 0x%x (%d)\n", j, val, val);
+			count += snprintf(buf + count, PAGE_SIZE - count,
+					"\t[%2d]: %02x (%d)\n", j, val, val);
+			if (count >= PAGE_SIZE)
+				return PAGE_SIZE - 1;
 		}
 
-		count += sprintf(buf + count, "\n");
+		count += snprintf(buf + count, PAGE_SIZE - count, "\n");
+		if (count >= PAGE_SIZE)
+			return PAGE_SIZE - 1;
 	}
 
 	return count;
@@ -1040,12 +1317,240 @@ static ssize_t mxt_update_fw_store(struct device *dev,
 	return count;
 }
 
+static ssize_t mxt_pause_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct mxt_data *data = dev_get_drvdata(dev);
+	int count = 0;
+
+	count += sprintf(buf + count, "%d", data->driver_paused);
+	count += sprintf(buf + count, "\n");
+
+	return count;
+}
+
+static ssize_t mxt_pause_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mxt_data *data = dev_get_drvdata(dev);
+	int i;
+
+	if (sscanf(buf, "%u", &i) == 1 && i < 2) {
+		data->driver_paused = i;
+
+		dev_dbg(dev, "%s\n", i ? "paused" : "unpaused");
+	} else {
+		dev_dbg(dev, "pause_driver write error\n");
+	}
+	return count;
+}
+
+static ssize_t mxt_debug_enable_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct mxt_data *data = dev_get_drvdata(dev);
+	int count = 0;
+
+	count += sprintf(buf + count, "%d", data->debug_enabled);
+	count += sprintf(buf + count, "\n");
+
+	return count;
+}
+
+static ssize_t mxt_debug_enable_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mxt_data *data = dev_get_drvdata(dev);
+	int i;
+
+	if (sscanf(buf, "%u", &i) == 1 && i < 2) {
+		data->debug_enabled = i;
+
+		dev_dbg(dev, "%s\n", i ? "debug enabled" : "debug disabled");
+	} else {
+		dev_dbg(dev, "debug_enabled write error\n");
+	}
+	return count;
+}
+
+static ssize_t mxt_slowscan_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct mxt_data *data = dev_get_drvdata(dev);
+	int count = 0;
+	int error;
+	u8 actv_cycle_time;
+	u8 idle_cycle_time;
+	u8 actv2idle_timeout;
+	dev_info(dev, "Calling mxt_slowscan_show()\n");
+
+	error = mxt_read_object(data, MXT_GEN_POWER_T7,
+		MXT_POWER_ACTVACQINT,
+		&actv_cycle_time);
+
+	if (error)
+		return error;
+
+	error = mxt_read_object(data, MXT_GEN_POWER_T7,
+		MXT_POWER_IDLEACQINT,
+		&idle_cycle_time);
+
+	if (error)
+		return error;
+
+	error = mxt_read_object(data, MXT_GEN_POWER_T7,
+		MXT_POWER_ACTV2IDLETO,
+		&actv2idle_timeout);
+
+	if (error)
+		return error;
+
+	count += sprintf(buf + count, "SLOW SCAN (enable/disable) = %s.\n", data->slowscan_enabled ? "enabled" : "disabled");
+	count += sprintf(buf + count, "SLOW SCAN (actv_cycle_time) = %umS.\n", data->slowscan_actv_cycle_time);
+	count += sprintf(buf + count, "SLOW SCAN (idle_cycle_time) = %umS.\n", data->slowscan_idle_cycle_time);
+	count += sprintf(buf + count, "SLOW SCAN (actv2idle_timeout) = %u.%0uS.\n", data->slowscan_actv2idle_timeout / 10, \
+											data->slowscan_actv2idle_timeout % 10);
+	count += sprintf(buf + count, "CURRENT   (actv_cycle_time) = %umS.\n", actv_cycle_time);
+	count += sprintf(buf + count, "CURRENT   (idle_cycle_time) = %umS.\n", idle_cycle_time);
+	count += sprintf(buf + count, "CURRENT   (actv2idle_timeout) = %u.%0uS.\n", actv2idle_timeout / 10, \
+											actv2idle_timeout % 10);
+
+	return count;
+}
+
+static ssize_t mxt_slowscan_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mxt_data *data = dev_get_drvdata(dev);
+	int fn;
+	int val;
+	int ret;
+
+	dev_info(dev, "Calling mxt_slowscan_store()\n");
+	ret = sscanf(buf, "%u %u", &fn, &val);
+	if ((ret == 1) || (ret == 2)) {
+		switch (fn) {
+		case SLOSCAN_DISABLE:
+			if (data->slowscan_enabled) {
+				data->actv_cycle_time = data->slowscan_shad_actv_cycle_time;
+				data->idle_cycle_time = data->slowscan_shad_idle_cycle_time;
+				data->actv2idle_timeout = data->slowscan_shad_actv2idle_timeout;
+				data->slowscan_enabled = 0;
+				mxt_set_power_cfg(data, 0);
+			}
+			break;
+
+		case SLOSCAN_ENABLE:
+			if (!data->slowscan_enabled) {
+				data->slowscan_shad_actv_cycle_time = data->actv_cycle_time;
+				data->slowscan_shad_idle_cycle_time = data->idle_cycle_time;
+				data->slowscan_shad_actv2idle_timeout = data->actv2idle_timeout;
+				data->actv_cycle_time = data->slowscan_actv_cycle_time;
+				data->idle_cycle_time = data->slowscan_idle_cycle_time;
+				data->actv2idle_timeout = data->slowscan_actv2idle_timeout;
+				data->slowscan_enabled = 1;
+				mxt_set_power_cfg(data, 0);
+			}
+			break;
+
+		case SLOSCAN_SET_ACTVACQINT:
+			data->slowscan_actv_cycle_time = val;
+			break;
+
+		case SLOSCAN_SET_IDLEACQINT:
+			data->slowscan_idle_cycle_time = val;
+			break;
+
+		case SLOSCAN_SET_ACTV2IDLETO:
+			data->slowscan_actv2idle_timeout = val;
+			break;
+		}
+	}
+	return count;
+}
+
+static ssize_t mxt_mem_access_read(struct file *filp, struct kobject *kobj,
+	struct bin_attribute *bin_attr, char *buf, loff_t off, size_t count)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct mxt_data *data = dev_get_drvdata(dev);
+	int ret = 0;
+
+	if (off >= 32768)
+		return -EIO;
+
+	if (off + count > 32768)
+		count = 32768 - off;
+
+	if (count > 256)
+		count = 256;
+
+	if (count > 0)
+		ret = __mxt_read_reg(data->client, off, count, buf);
+
+	return ret == 0 ? count : ret;
+}
+
+int mxt_write_block(struct i2c_client *client, u16 addr, u16 length, u8 *value)
+{
+	int i;
+	struct {
+		__le16 le_addr;
+		u8  data[256];
+	} i2c_block_transfer;
+
+	if (length > 256)
+		return -EINVAL;
+
+	i2c_get_clientdata(client);
+
+	for (i = 0; i < length; i++)
+		i2c_block_transfer.data[i] = *value++;
+
+	i2c_block_transfer.le_addr = cpu_to_le16(addr);
+
+	i = i2c_master_send(client, (u8 *) &i2c_block_transfer, length + 2);
+
+	if (i == (length + 2))
+		return 0;
+	else
+		return -EIO;
+}
+
+static ssize_t mxt_mem_access_write(struct file *filp, struct kobject *kobj,
+	struct bin_attribute *bin_attr, char *buf, loff_t off, size_t count)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct mxt_data *data = dev_get_drvdata(dev);
+	int ret = 0;
+
+	if (off >= 32768)
+		return -EIO;
+
+	if (off + count > 32768)
+		count = 32768 - off;
+
+	if (count > 256)
+		count = 256;
+
+	if (count > 0)
+		ret = mxt_write_block(data->client, off, count, buf);
+
+	return ret == 0 ? count : 0;
+}
+
 static DEVICE_ATTR(object, 0444, mxt_object_show, NULL);
 static DEVICE_ATTR(update_fw, 0664, NULL, mxt_update_fw_store);
+static DEVICE_ATTR(pause_driver, 0664, mxt_pause_show, mxt_pause_store);
+static DEVICE_ATTR(debug_enable, 0664, mxt_debug_enable_show, mxt_debug_enable_store);
+static DEVICE_ATTR(slowscan_enable, 0664, mxt_slowscan_show, mxt_slowscan_store);
 
 static struct attribute *mxt_attrs[] = {
 	&dev_attr_object.attr,
 	&dev_attr_update_fw.attr,
+	&dev_attr_pause_driver.attr,
+	&dev_attr_debug_enable.attr,
+	&dev_attr_slowscan_enable.attr,
 	NULL
 };
 
@@ -1055,16 +1560,54 @@ static const struct attribute_group mxt_attr_group = {
 
 static void mxt_start(struct mxt_data *data)
 {
+	int error = 0;
+	int cnt;
+	struct device *dev = &data->client->dev;
+
+	dev_info(dev, "mxt_start:  is_stopped = %d\n", data->is_stopped);
+	if (data->is_stopped == 0)
+		return;
+
 	/* Touch enable */
-	mxt_write_object(data,
-			MXT_TOUCH_MULTI_T9, MXT_TOUCH_CTRL, 0x83);
+	cnt = ARRAY_SIZE(mxt_save);
+	while (cnt--)
+		error |= mxt_write_object(data, mxt_save[cnt].suspend_obj,
+						mxt_save[cnt].suspend_reg,
+						mxt_save[cnt].restore_val);
+
+	if (!error)
+		dev_info(dev, "MXT started\n");
+
+	data->is_stopped = 0;
 }
 
 static void mxt_stop(struct mxt_data *data)
 {
+	int error = 0;
+	int i, cnt;
+	struct device *dev = &data->client->dev;
+
+	dev_info(dev, "mxt_stop:  is_stopped = %d\n", data->is_stopped);
+	if (data->is_stopped)
+		return;
+
 	/* Touch disable */
-	mxt_write_object(data,
-			MXT_TOUCH_MULTI_T9, MXT_TOUCH_CTRL, 0);
+	cnt = ARRAY_SIZE(mxt_save);
+	for (i = 0; i < cnt; i++) {
+		if (mxt_save[i].suspend_flags == MXT_SUSPEND_DYNAMIC)
+			error |= mxt_read_object(data,
+						mxt_save[i].suspend_obj,
+						mxt_save[i].suspend_reg,
+						&mxt_save[i].restore_val);
+		error |= mxt_write_object(data, mxt_save[i].suspend_obj,
+						mxt_save[i].suspend_reg,
+						mxt_save[i].suspend_val);
+	}
+
+	if (!error)
+		dev_info(dev, "MXT suspended\n");
+
+	data->is_stopped = 1;
 }
 
 static int mxt_input_open(struct input_dev *dev)
@@ -1102,7 +1645,7 @@ static int __devinit mxt_probe(struct i2c_client *client,
 		goto err_free_mem;
 	}
 
-	input_dev->name = "Atmel maXTouch Touchscreen";
+	input_dev->name = "atmel-maxtouch";
 	input_dev->id.bustype = BUS_I2C;
 	input_dev->dev.parent = &client->dev;
 	input_dev->open = mxt_input_open;
@@ -1112,6 +1655,7 @@ static int __devinit mxt_probe(struct i2c_client *client,
 	data->input_dev = input_dev;
 	data->pdata = pdata;
 	data->irq = client->irq;
+	data->is_stopped = 0;
 
 	mxt_calc_resolution(data);
 
@@ -1128,7 +1672,6 @@ static int __devinit mxt_probe(struct i2c_client *client,
 			     0, 255, 0, 0);
 
 	/* For multi touch */
-	input_mt_init_slots(input_dev, MXT_MAX_FINGER);
 	input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR,
 			     0, MXT_MAX_AREA, 0, 0);
 	input_set_abs_params(input_dev, ABS_MT_POSITION_X,
@@ -1141,6 +1684,8 @@ static int __devinit mxt_probe(struct i2c_client *client,
 	input_set_drvdata(input_dev, data);
 	i2c_set_clientdata(client, data);
 
+	mutex_init(&data->access_mutex);
+
 	error = mxt_initialize(data);
 	if (error)
 		goto err_free_object;
@@ -1152,17 +1697,42 @@ static int __devinit mxt_probe(struct i2c_client *client,
 		goto err_free_object;
 	}
 
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+	data->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	data->early_suspend.suspend = mxt_early_suspend;
+	data->early_suspend.resume = mxt_early_resume;
+	register_early_suspend(&data->early_suspend);
+#endif
+
 	error = mxt_make_highchg(data);
-	if (error)
+	if (error) {
+		dev_err(&client->dev, "Failed to make high CHG\n");
 		goto err_free_irq;
+	}
 
 	error = input_register_device(input_dev);
-	if (error)
+	if (error) {
+		dev_err(&client->dev, "Failed to register input device\n");
 		goto err_free_irq;
+	}
 
 	error = sysfs_create_group(&client->dev.kobj, &mxt_attr_group);
-	if (error)
+	if (error) {
+		dev_err(&client->dev, "Failed to create sysfs group\n");
+		goto err_unregister_device;
+	}
+
+	sysfs_bin_attr_init(&data->mem_access_attr);
+	data->mem_access_attr.attr.name = "mem_access";
+	data->mem_access_attr.attr.mode = S_IRUGO | S_IWUSR | S_IWGRP;
+	data->mem_access_attr.read = mxt_mem_access_read;
+	data->mem_access_attr.write = mxt_mem_access_write;
+	data->mem_access_attr.size = 65535;
+
+	if (sysfs_create_bin_file(&client->dev.kobj, &data->mem_access_attr) < 0) {
+		dev_err(&client->dev, "Failed to create %s\n", data->mem_access_attr.attr.name);
 		goto err_unregister_device;
+	}
 
 	return 0;
 
@@ -1183,6 +1753,7 @@ static int __devexit mxt_remove(struct i2c_client *client)
 {
 	struct mxt_data *data = i2c_get_clientdata(client);
 
+	sysfs_remove_bin_file(&client->dev.kobj, &data->mem_access_attr);
 	sysfs_remove_group(&client->dev.kobj, &mxt_attr_group);
 	free_irq(data->irq, data);
 	input_unregister_device(data->input_dev);
@@ -1216,8 +1787,7 @@ static int mxt_resume(struct device *dev)
 	struct input_dev *input_dev = data->input_dev;
 
 	/* Soft reset */
-	mxt_write_object(data, MXT_GEN_COMMAND_T6,
-			MXT_COMMAND_RESET, 1);
+	mxt_write_object(data, MXT_GEN_COMMAND_T6, MXT_COMMAND_RESET, 1);
 
 	msleep(MXT_RESET_TIME);
 
@@ -1231,11 +1801,39 @@ static int mxt_resume(struct device *dev)
 	return 0;
 }
 
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+static void mxt_early_suspend(struct early_suspend *es)
+{
+	struct mxt_data *mxt;
+	struct device *dev;
+	mxt = container_of(es, struct mxt_data, early_suspend);
+	dev = &mxt->client->dev;
+	dev_info(dev, "MXT Early Suspend entered\n");
+
+	if (mxt_suspend(&mxt->client->dev) != 0)
+		dev_err(&mxt->client->dev, "%s: failed\n", __func__);
+	dev_info(dev, "MXT Early Suspended\n");
+}
+
+static void mxt_early_resume(struct early_suspend *es)
+{
+	struct mxt_data *mxt;
+	struct device *dev;
+	mxt = container_of(es, struct mxt_data, early_suspend);
+	dev = &mxt->client->dev;
+	dev_info(dev, "MXT Early Resume entered\n");
+
+	if (mxt_resume(&mxt->client->dev) != 0)
+		dev_err(&mxt->client->dev, "%s: failed\n", __func__);
+	dev_info(dev, "MXT Early Resumed\n");
+}
+#else
 static const struct dev_pm_ops mxt_pm_ops = {
 	.suspend	= mxt_suspend,
 	.resume		= mxt_resume,
 };
 #endif
+#endif
 
 static const struct i2c_device_id mxt_id[] = {
 	{ "qt602240_ts", 0 },
@@ -1249,7 +1847,7 @@ static struct i2c_driver mxt_driver = {
 	.driver = {
 		.name	= "atmel_mxt_ts",
 		.owner	= THIS_MODULE,
-#ifdef CONFIG_PM
+#if defined(CONFIG_PM) && !defined(CONFIG_HAS_EARLYSUSPEND)
 		.pm	= &mxt_pm_ops,
 #endif
 	},
diff --git a/drivers/input/touchscreen/fusion_F0710A.c b/drivers/input/touchscreen/fusion_F0710A.c
new file mode 100644
index 000000000000..c5c0f1fc38ea
--- /dev/null
+++ b/drivers/input/touchscreen/fusion_F0710A.c
@@ -0,0 +1,524 @@
+/*
+ *  "fusion_F0710A"  touchscreen driver
+ *	
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <asm/irq.h>
+#include <linux/gpio.h>
+#include <linux/input/fusion_F0710A.h>
+
+#include "fusion_F0710A.h"
+
+#define DRV_NAME		"fusion_F0710A"
+
+
+static struct fusion_F0710A_data fusion_F0710A;
+
+static unsigned short normal_i2c[] = { fusion_F0710A_I2C_SLAVE_ADDR, I2C_CLIENT_END };
+
+//I2C_CLIENT_INSMOD;
+
+static int fusion_F0710A_write_u8(u8 addr, u8 data) 
+{
+	return i2c_smbus_write_byte_data(fusion_F0710A.client, addr, data);
+}
+
+static int fusion_F0710A_read_u8(u8 addr)
+{
+	return i2c_smbus_read_byte_data(fusion_F0710A.client, addr);
+}
+
+static int fusion_F0710A_read_block(u8 addr, u8 len, u8 *data)
+{
+#if 0
+	/* When i2c_smbus_read_i2c_block_data() takes a block length parameter, we can do
+	 * this. lm-sensors lists hints this has been fixed, but I can't tell whether it
+	 * was or will be merged upstream. */
+
+	return i2c_smbus_read_i2c_block_data(&fusion_F0710A.client, addr, data);
+#else
+	u8 msgbuf0[1] = { addr };
+	u16 slave = fusion_F0710A.client->addr;
+	u16 flags = fusion_F0710A.client->flags;
+	struct i2c_msg msg[2] = { { slave, flags, 1, msgbuf0 },
+				  { slave, flags | I2C_M_RD, len, data }
+	};
+
+	return i2c_transfer(fusion_F0710A.client->adapter, msg, ARRAY_SIZE(msg));
+#endif
+}
+
+
+static int fusion_F0710A_register_input(void)
+{
+	int ret;
+	struct input_dev *dev;
+
+	dev = fusion_F0710A.input = input_allocate_device();
+	if (dev == NULL)
+		return -ENOMEM;
+
+	dev->name = "fusion_F0710A";
+
+	set_bit(EV_KEY, dev->evbit);
+	set_bit(EV_ABS, dev->evbit);
+
+	input_set_abs_params(dev, ABS_MT_POSITION_X, 0, fusion_F0710A.info.xres-1, 0, 0);
+	input_set_abs_params(dev, ABS_MT_POSITION_Y, 0, fusion_F0710A.info.yres-1, 0, 0);
+#ifdef CONFIG_ANDROID
+	input_set_abs_params(dev, ABS_MT_TRACKING_ID, 0, 15, 0, 0);
+#else
+	input_set_abs_params(dev, ABS_MT_TOUCH_MAJOR, 0, 255, 0, 0);
+	input_set_abs_params(dev, ABS_MT_WIDTH_MAJOR, 0, 15, 0, 0);
+#endif
+
+	input_set_abs_params(dev, ABS_X, 0, fusion_F0710A.info.xres-1, 0, 0);
+	input_set_abs_params(dev, ABS_Y, 0, fusion_F0710A.info.yres-1, 0, 0);
+	input_set_abs_params(dev, ABS_PRESSURE, 0, 255, 0, 0);
+
+	ret = input_register_device(dev);
+	if (ret < 0)
+		goto bail1;
+
+	return 0;
+
+bail1:
+	input_free_device(dev);
+	return ret;
+}
+
+#define WC_RETRY_COUNT 		3
+static int fusion_F0710A_write_complete(void)
+{
+	int ret, i;
+
+	for(i=0; i<WC_RETRY_COUNT; i++)
+	{
+		ret = fusion_F0710A_write_u8(fusion_F0710A_SCAN_COMPLETE, 0);
+		if(ret == 0)
+			break;
+		else
+			dev_err(&fusion_F0710A.client->dev, "Write complete failed(%d): %d\n", i, ret);
+	}
+
+	return ret;
+}
+
+#define DATA_START	fusion_F0710A_DATA_INFO
+#define	DATA_END	fusion_F0710A_SEC_TIDTS
+#define DATA_LEN	(DATA_END - DATA_START + 1)
+#define DATA_OFF(x)	((x) - DATA_START)
+
+static int fusion_F0710A_read_sensor(void)
+{
+	int ret;
+	u8 data[DATA_LEN];
+
+#define DATA(x) (data[DATA_OFF(x)])
+	/* To ensure data coherency, read the sensor with a single transaction. */
+	ret = fusion_F0710A_read_block(DATA_START, DATA_LEN, data);
+	if (ret < 0) {
+		dev_err(&fusion_F0710A.client->dev,
+			"Read block failed: %d\n", ret);
+		
+		return ret;
+	}
+
+	fusion_F0710A.f_num = DATA(fusion_F0710A_DATA_INFO)&0x03;
+	
+	fusion_F0710A.y1 = DATA(fusion_F0710A_POS_X1_HI) << 8;
+	fusion_F0710A.y1 |= DATA(fusion_F0710A_POS_X1_LO);
+	fusion_F0710A.x1 = DATA(fusion_F0710A_POS_Y1_HI) << 8;
+	fusion_F0710A.x1 |= DATA(fusion_F0710A_POS_Y1_LO);
+	fusion_F0710A.z1 = DATA(fusion_F0710A_FIR_PRESS);
+	fusion_F0710A.tip1 = DATA(fusion_F0710A_FIR_TIDTS)&0x0f;
+	fusion_F0710A.tid1 = (DATA(fusion_F0710A_FIR_TIDTS)&0xf0)>>4;
+	
+	
+	fusion_F0710A.y2 = DATA(fusion_F0710A_POS_X2_HI) << 8;
+	fusion_F0710A.y2 |= DATA(fusion_F0710A_POS_X2_LO);
+	fusion_F0710A.x2 = DATA(fusion_F0710A_POS_Y2_HI) << 8;
+	fusion_F0710A.x2 |= DATA(fusion_F0710A_POS_Y2_LO);
+	fusion_F0710A.z2 = DATA(fusion_F0710A_SEC_PRESS);
+	fusion_F0710A.tip2 = DATA(fusion_F0710A_SEC_TIDTS)&0x0f;
+	fusion_F0710A.tid2 =(DATA(fusion_F0710A_SEC_TIDTS)&0xf0)>>4;
+#undef DATA
+
+	return 0;
+}
+
+#define val_cut_max(x, max, reverse)	\
+do					\
+{					\
+	if(x > max)			\
+		x = max;		\
+	if(reverse)			\
+		x = (max) - (x);	\
+}					\
+while(0)
+
+static void fusion_F0710A_wq(struct work_struct *work)
+{
+	struct input_dev *dev = fusion_F0710A.input;
+	int save_points = 0;
+	int x1 = 0, y1 = 0, z1 = 0, x2 = 0, y2 = 0, z2 = 0;
+
+	if (fusion_F0710A_read_sensor() < 0)
+		goto restore_irq;
+
+#ifdef DEBUG
+	printk(KERN_DEBUG "tip1, tid1, x1, y1, z1 (%x,%x,%d,%d,%d); tip2, tid2, x2, y2, z2 (%x,%x,%d,%d,%d)\n",
+		fusion_F0710A.tip1, fusion_F0710A.tid1, fusion_F0710A.x1, fusion_F0710A.y1, fusion_F0710A.z1,
+		fusion_F0710A.tip2, fusion_F0710A.tid2, fusion_F0710A.x2, fusion_F0710A.y2, fusion_F0710A.z2);
+#endif /* DEBUG */
+
+	val_cut_max(fusion_F0710A.x1, fusion_F0710A.info.xres-1, fusion_F0710A.info.xy_reverse);
+	val_cut_max(fusion_F0710A.y1, fusion_F0710A.info.yres-1, fusion_F0710A.info.xy_reverse);
+	val_cut_max(fusion_F0710A.x2, fusion_F0710A.info.xres-1, fusion_F0710A.info.xy_reverse);
+	val_cut_max(fusion_F0710A.y2, fusion_F0710A.info.yres-1, fusion_F0710A.info.xy_reverse);
+
+	if(fusion_F0710A.tip1 == 1)
+	{
+		if(fusion_F0710A.tid1 == 1)
+		{
+			/* first point */
+			x1 = fusion_F0710A.x1;
+			y1 = fusion_F0710A.y1;
+			z1 = fusion_F0710A.z1;
+			save_points |= fusion_F0710A_SAVE_PT1;
+		}
+		else if(fusion_F0710A.tid1 == 2)
+		{
+			/* second point ABS_DISTANCE second point pressure, BTN_2 second point touch */
+			x2 = fusion_F0710A.x1;
+			y2 = fusion_F0710A.y1;
+			z2 = fusion_F0710A.z1;
+			save_points |= fusion_F0710A_SAVE_PT2;
+		}
+	}
+
+	if(fusion_F0710A.tip2 == 1)
+	{
+		if(fusion_F0710A.tid2 == 2)
+		{
+			/* second point ABS_DISTANCE second point pressure, BTN_2 second point touch */
+			x2 = fusion_F0710A.x2;
+			y2 = fusion_F0710A.y2;
+			z2 = fusion_F0710A.z2;
+			save_points |= fusion_F0710A_SAVE_PT2;
+		}
+		else if(fusion_F0710A.tid2 == 1)/* maybe this will never happen */
+		{
+			/* first point */
+			x1 = fusion_F0710A.x2;
+			y1 = fusion_F0710A.y2;
+			z1 = fusion_F0710A.z2;
+			save_points |= fusion_F0710A_SAVE_PT1;
+		}
+	}
+
+#ifdef CONFIG_ANDROID
+	if(z1)
+	{	
+		input_report_abs(dev, ABS_MT_TRACKING_ID, 1);
+		input_report_abs(dev, ABS_MT_POSITION_X, x1);
+		input_report_abs(dev, ABS_MT_POSITION_Y, y1);
+	}
+	input_mt_sync(dev);
+	
+	if(z2)
+	{	
+		input_report_abs(dev, ABS_MT_TRACKING_ID, 2);
+		input_report_abs(dev, ABS_MT_POSITION_X, x2);
+		input_report_abs(dev, ABS_MT_POSITION_Y, y2);
+	}
+	input_mt_sync(dev);
+#else /* CONFIG_ANDROID */
+	input_report_abs(dev, ABS_MT_TOUCH_MAJOR, z1);
+	input_report_abs(dev, ABS_MT_WIDTH_MAJOR, 1);
+	input_report_abs(dev, ABS_MT_POSITION_X, x1);
+	input_report_abs(dev, ABS_MT_POSITION_Y, y1);
+	input_mt_sync(dev);
+	input_report_abs(dev, ABS_MT_TOUCH_MAJOR, z2);
+	input_report_abs(dev, ABS_MT_WIDTH_MAJOR, 2);
+	input_report_abs(dev, ABS_MT_POSITION_X, x2);
+	input_report_abs(dev, ABS_MT_POSITION_Y, y2);
+	input_mt_sync(dev);
+#endif /* CONFIG_ANDROID */
+
+	input_report_abs(dev, ABS_X, x1);
+	input_report_abs(dev, ABS_Y, y1);
+	input_report_abs(dev, ABS_PRESSURE, z1);
+	input_report_key(dev, BTN_TOUCH, fusion_F0710A.tip1);
+
+	input_sync(dev);
+
+restore_irq:
+	enable_irq(fusion_F0710A.client->irq);
+
+	/* Clear fusion_F0710A interrupt */
+	fusion_F0710A_write_complete();
+}
+static DECLARE_WORK(fusion_F0710A_work, fusion_F0710A_wq);
+
+static irqreturn_t fusion_F0710A_interrupt(int irq, void *dev_id)
+{
+	disable_irq_nosync(fusion_F0710A.client->irq);
+
+	queue_work(fusion_F0710A.workq, &fusion_F0710A_work);
+
+	return IRQ_HANDLED;
+}
+
+const static u8* g_ver_product[4] = {
+	"10Z8", "70Z7", "43Z6", ""
+};
+
+static int fusion_F0710A_probe(struct i2c_client *i2c, const struct i2c_device_id *id)
+{
+	struct fusion_f0710a_init_data *pdata = i2c->dev.platform_data;
+	int ret;
+	u8 ver_product, ver_id;
+	u32 version;
+
+	if (pdata == NULL)
+	{
+		dev_err(&i2c->dev, "No platform data for Fusion driver\n");
+		return -ENODEV;
+	}
+
+	/* Request pinmuxing, if necessary */
+	if (pdata->pinmux_fusion_pins != NULL)
+	{
+		ret = pdata->pinmux_fusion_pins();
+		if (ret < 0) {
+			dev_err(&i2c->dev, "muxing GPIOs failed\n");
+			return -ENODEV;
+		}
+	}
+
+	if ((gpio_request(pdata->gpio_int, "Fusion pen down interrupt") == 0) &&
+	    (gpio_direction_input(pdata->gpio_int) == 0)) {
+		gpio_export(pdata->gpio_int, 0);
+	} else {
+		dev_warn(&i2c->dev, "Could not obtain GPIO for Fusion pen down\n");
+		return -ENODEV;
+	}
+
+	if ((gpio_request(pdata->gpio_reset, "Fusion reset") == 0) &&
+	    (gpio_direction_output(pdata->gpio_reset, 1) == 0)) {
+
+		/* Generate a 0 => 1 edge explicitly, and wait for startup... */
+		gpio_set_value(pdata->gpio_reset, 0);
+		msleep(10);
+		gpio_set_value(pdata->gpio_reset, 1);
+		/* Wait for startup (up to 125ms according to datasheet) */
+		msleep(125);
+
+		gpio_export(pdata->gpio_reset, 0);
+	} else {
+		dev_warn(&i2c->dev, "Could not obtain GPIO for Fusion reset\n");
+		ret = -ENODEV;
+		goto bail0;
+	}
+
+	/* Use Pen Down GPIO as sampling interrupt */
+	i2c->irq = gpio_to_irq(pdata->gpio_int);
+
+	if(!i2c->irq)
+	{
+		dev_err(&i2c->dev, "fusion_F0710A irq < 0 \n");
+		ret = -ENOMEM;
+		goto bail1;
+	}
+
+	/* Attach the I2C client */
+	fusion_F0710A.client =  i2c;
+	i2c_set_clientdata(i2c, &fusion_F0710A);
+
+	dev_info(&i2c->dev, "Touchscreen registered with bus id (%d) with slave address 0x%x\n",
+			i2c_adapter_id(fusion_F0710A.client->adapter),	fusion_F0710A.client->addr);
+
+	/* Read out a lot of registers */
+	ret = fusion_F0710A_read_u8(fusion_F0710A_VIESION_INFO_LO);
+	if (ret < 0) {
+		dev_err(&i2c->dev, "query failed: %d\n", ret);
+		goto bail1;
+	}
+	ver_product = (((u8)ret) & 0xc0) >> 6;
+	version = (10 + ((((u32)ret)&0x30) >> 4)) * 100000;
+	version += (((u32)ret)&0xf) * 1000;
+	/* Read out a lot of registers */
+	ret = fusion_F0710A_read_u8(fusion_F0710A_VIESION_INFO);
+		if (ret < 0) {
+		dev_err(&i2c->dev, "query failed: %d\n", ret);
+		goto bail1;
+	}
+	ver_id = ((u8)(ret) & 0x6) >> 1;
+	version += ((((u32)ret) & 0xf8) >> 3) * 10;
+	version += (((u32)ret) & 0x1) + 1; /* 0 is build 1, 1 is build 2 */
+	dev_info(&i2c->dev, "version product %s(%d)\n", g_ver_product[ver_product] ,ver_product);
+	dev_info(&i2c->dev, "version id %s(%d)\n", ver_id ? "1.4" : "1.0", ver_id);
+	dev_info(&i2c->dev, "version series (%d)\n", version);
+
+	switch(ver_product)
+	{
+	case fusion_F0710A_VIESION_07: /* 7 inch */
+		fusion_F0710A.info.xres = fusion_F0710A07_XMAX;
+		fusion_F0710A.info.yres = fusion_F0710A07_YMAX;
+		fusion_F0710A.info.xy_reverse = fusion_F0710A07_REV;
+		break;
+	case fusion_F0710A_VIESION_43: /* 4.3 inch */
+		fusion_F0710A.info.xres = fusion_F0710A43_XMAX;
+		fusion_F0710A.info.yres = fusion_F0710A43_YMAX;
+		fusion_F0710A.info.xy_reverse = fusion_F0710A43_REV;
+		break;
+	default: /* fusion_F0710A_VIESION_10 10 inch */
+		fusion_F0710A.info.xres = fusion_F0710A10_XMAX;
+		fusion_F0710A.info.yres = fusion_F0710A10_YMAX;
+		fusion_F0710A.info.xy_reverse = fusion_F0710A10_REV;
+		break;
+	}
+
+	/* Register the input device. */
+	ret = fusion_F0710A_register_input();
+	if (ret < 0) {
+		dev_err(&i2c->dev, "can't register input: %d\n", ret);
+		goto bail1;
+	}
+
+	/* Create a worker thread */
+	fusion_F0710A.workq = create_singlethread_workqueue(DRV_NAME);
+	if (fusion_F0710A.workq == NULL) {
+		dev_err(&i2c->dev, "can't create work queue\n");
+		ret = -ENOMEM;
+		goto bail2;
+	}
+
+
+	/* Register for the interrupt and enable it. Our handler will
+	*  start getting invoked after this call. */
+	ret = request_irq(i2c->irq, fusion_F0710A_interrupt, IRQF_TRIGGER_RISING,
+	i2c->name, &fusion_F0710A);
+	if (ret < 0) {
+		dev_err(&i2c->dev, "can't get irq %d: %d\n", i2c->irq, ret);
+		goto bail3;
+	}
+	/* clear the irq first */
+	ret = fusion_F0710A_write_u8(fusion_F0710A_SCAN_COMPLETE, 0);
+	if (ret < 0) {
+		dev_err(&i2c->dev, "Clear irq failed: %d\n", ret);
+		goto bail4;
+	}
+
+	return 0;
+
+bail4:
+	free_irq(i2c->irq, &fusion_F0710A);
+
+bail3:
+	destroy_workqueue(fusion_F0710A.workq);
+	fusion_F0710A.workq = NULL;
+
+bail2:
+	input_unregister_device(fusion_F0710A.input);
+bail1:
+	gpio_free(pdata->gpio_reset);
+bail0:
+	gpio_free(pdata->gpio_int);
+
+	return ret;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int fusion_F0710A_suspend(struct device *dev)
+{
+	struct i2c_client *i2c = to_i2c_client(dev);
+	disable_irq(i2c->irq);
+	flush_workqueue(fusion_F0710A.workq);
+
+	return 0;
+}
+
+static int fusion_F0710A_resume(struct device *dev)
+{
+	struct i2c_client *i2c = to_i2c_client(dev);
+	enable_irq(i2c->irq);
+
+	return 0;
+}
+#endif
+
+static int fusion_F0710A_remove(struct i2c_client *i2c)
+{
+	struct fusion_f0710a_init_data *pdata = i2c->dev.platform_data;
+
+	gpio_free(pdata->gpio_int);
+	gpio_free(pdata->gpio_reset);
+	destroy_workqueue(fusion_F0710A.workq);
+	free_irq(i2c->irq, &fusion_F0710A);
+	input_unregister_device(fusion_F0710A.input);
+	i2c_set_clientdata(i2c, NULL);
+
+	dev_info(&i2c->dev, "driver removed\n");
+	
+	return 0;
+}
+
+static struct i2c_device_id fusion_F0710A_id[] = {
+	{"fusion_F0710A", 0},
+	{},
+};
+
+static const struct dev_pm_ops fusion_F0710A_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(fusion_F0710A_suspend, fusion_F0710A_resume)
+};
+
+static struct i2c_driver fusion_F0710A_i2c_drv = {
+	.driver = {
+		.owner		= THIS_MODULE,
+		.name		= DRV_NAME,
+		.pm		= &fusion_F0710A_pm_ops,
+	},
+	.probe          = fusion_F0710A_probe,
+	.remove         = fusion_F0710A_remove,
+	.id_table       = fusion_F0710A_id,
+	.address_list   = normal_i2c,
+};
+
+static int __init fusion_F0710A_init( void )
+{
+	int ret;
+
+	memset(&fusion_F0710A, 0, sizeof(fusion_F0710A));
+
+	/* Probe for fusion_F0710A on I2C. */
+	ret = i2c_add_driver(&fusion_F0710A_i2c_drv);
+	if (ret < 0) {
+		printk(KERN_WARNING DRV_NAME " can't add i2c driver: %d\n", ret);
+	}
+
+	return ret;
+}
+
+static void __exit fusion_F0710A_exit( void )
+{
+	i2c_del_driver(&fusion_F0710A_i2c_drv);
+}
+module_init(fusion_F0710A_init);
+module_exit(fusion_F0710A_exit);
+
+MODULE_DESCRIPTION("fusion_F0710A Touchscreen Driver");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/input/touchscreen/fusion_F0710A.h b/drivers/input/touchscreen/fusion_F0710A.h
new file mode 100644
index 000000000000..85f8210345a9
--- /dev/null
+++ b/drivers/input/touchscreen/fusion_F0710A.h
@@ -0,0 +1,87 @@
+/*
+ *  "fusion_F0710A" touchscreen driver
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ */
+
+/* I2C slave address */
+#define fusion_F0710A_I2C_SLAVE_ADDR		0x10
+
+/* I2C registers */
+#define fusion_F0710A_DATA_INFO			0x00
+
+/* First Point*/
+#define fusion_F0710A_POS_X1_HI			0x01 	/* 16-bit register, MSB */
+#define fusion_F0710A_POS_X1_LO			0x02 	/* 16-bit register, LSB */
+#define fusion_F0710A_POS_Y1_HI			0x03 	/* 16-bit register, MSB */
+#define fusion_F0710A_POS_Y1_LO			0x04 	/* 16-bit register, LSB */
+#define fusion_F0710A_FIR_PRESS			0X05
+#define fusion_F0710A_FIR_TIDTS			0X06
+
+/* Second Point */
+#define fusion_F0710A_POS_X2_HI			0x07 	/* 16-bit register, MSB */
+#define fusion_F0710A_POS_X2_LO			0x08 	/* 16-bit register, LSB */
+#define fusion_F0710A_POS_Y2_HI			0x09 	/* 16-bit register, MSB */
+#define fusion_F0710A_POS_Y2_LO			0x0A 	/* 16-bit register, LSB */
+#define fusion_F0710A_SEC_PRESS			0x0B
+#define fusion_F0710A_SEC_TIDTS			0x0C
+
+#define fusion_F0710A_VIESION_INFO_LO		0X0E
+#define fusion_F0710A_VIESION_INFO			0X0F
+
+#define fusion_F0710A_RESET				0x10
+#define fusion_F0710A_SCAN_COMPLETE		0x11
+
+
+#define fusion_F0710A_VIESION_10			0
+#define fusion_F0710A_VIESION_07			1
+#define fusion_F0710A_VIESION_43			2
+
+/* fusion_F0710A 10 inch panel */
+#define fusion_F0710A10_XMAX				2275
+#define fusion_F0710A10_YMAX				1275
+#define fusion_F0710A10_REV				1
+
+/* fusion_F0710A 7 inch panel */
+#define fusion_F0710A07_XMAX				1500
+#define fusion_F0710A07_YMAX				900
+#define fusion_F0710A07_REV				0
+
+/* fusion_F0710A 4.3 inch panel */
+#define fusion_F0710A43_XMAX				900
+#define fusion_F0710A43_YMAX				500
+#define fusion_F0710A43_REV				0
+
+#define	fusion_F0710A_SAVE_PT1				0x1
+#define	fusion_F0710A_SAVE_PT2				0x2
+
+
+
+/* fusion_F0710A touch screen information */
+struct fusion_F0710A_info {
+	int xres; /* x resolution */
+	int yres; /* y resolution */
+	int xy_reverse; /* if need reverse in the x,y value x=xres-1-x, y=yres-1-y*/    
+};
+
+struct fusion_F0710A_data {
+	struct fusion_F0710A_info		info;
+	struct i2c_client		*client;
+	struct workqueue_struct	*workq;
+	struct input_dev		*input;
+	u16						x1;
+	u16						y1;
+	u8						z1;
+	u8						tip1;
+	u8						tid1;
+	u16						x2;
+	u16						y2;
+	u8						z2;
+	u8						tip2;
+	u8						tid2;
+	u8						f_num;
+	u8						save_points;
+};
+
diff --git a/drivers/input/touchscreen/panjit_i2c.c b/drivers/input/touchscreen/panjit_i2c.c
new file mode 100644
index 000000000000..4fccebc52ea8
--- /dev/null
+++ b/drivers/input/touchscreen/panjit_i2c.c
@@ -0,0 +1,361 @@
+/*
+ * drivers/input/touchscreen/panjit_i2c.c
+ *
+ * Touchscreen class input driver for Panjit touch panel using I2C bus
+ *
+ * Copyright (c) 2010, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/input.h>
+#include <linux/delay.h>
+#include <linux/earlysuspend.h>
+#include <linux/i2c.h>
+#include <linux/i2c/panjit_ts.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+
+#define CSR		0x00
+ #define CSR_SCAN_EN	(1 << 3)
+ #define CSR_SLEEP_EN	(1 << 7)
+#define C_FLAG		0x01
+#define X1_H		0x03
+
+#define DRIVER_NAME	"panjit_touch"
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void pj_early_suspend(struct early_suspend *h);
+static void pj_late_resume(struct early_suspend *h);
+#endif
+
+struct pj_data {
+	struct input_dev	*input_dev;
+	struct i2c_client	*client;
+	int			gpio_reset;
+	struct early_suspend	early_suspend;
+};
+
+struct pj_event {
+	__be16	coord[2][2];
+	__u8	fingers;
+	__u8	gesture;
+};
+
+union pj_buff {
+	struct pj_event	data;
+	unsigned char	buff[sizeof(struct pj_data)];
+};
+
+static void pj_reset(struct pj_data *touch)
+{
+	if (touch->gpio_reset < 0)
+		return;
+
+	gpio_set_value(touch->gpio_reset, 1);
+	msleep(50);
+	gpio_set_value(touch->gpio_reset, 0);
+	msleep(50);
+}
+
+static irqreturn_t pj_irq(int irq, void *dev_id)
+{
+	struct pj_data *touch = dev_id;
+	struct i2c_client *client = touch->client;
+	union pj_buff event;
+	int ret, i;
+
+	ret = i2c_smbus_read_i2c_block_data(client, X1_H,
+					    sizeof(event.buff), event.buff);
+	if (WARN_ON(ret < 0)) {
+		dev_err(&client->dev, "error %d reading event data\n", ret);
+		return IRQ_NONE;
+	}
+	ret = i2c_smbus_write_byte_data(client, C_FLAG, 0);
+	if (WARN_ON(ret < 0)) {
+		dev_err(&client->dev, "error %d clearing interrupt\n", ret);
+		return IRQ_NONE;
+	}
+
+	input_report_key(touch->input_dev, BTN_TOUCH,
+			 (event.data.fingers == 1 || event.data.fingers == 2));
+	input_report_key(touch->input_dev, BTN_2, (event.data.fingers == 2));
+
+	if (!event.data.fingers || (event.data.fingers > 2))
+		goto out;
+
+	for (i = 0; i < event.data.fingers; i++) {
+		input_report_abs(touch->input_dev, ABS_MT_POSITION_X,
+				 __be16_to_cpu(event.data.coord[i][0]));
+		input_report_abs(touch->input_dev, ABS_MT_POSITION_Y,
+				 __be16_to_cpu(event.data.coord[i][1]));
+		input_report_abs(touch->input_dev, ABS_MT_TRACKING_ID, i + 1);
+		input_mt_sync(touch->input_dev);
+	}
+
+out:
+	input_sync(touch->input_dev);
+	return IRQ_HANDLED;
+}
+
+static int pj_probe(struct i2c_client *client,
+	const struct i2c_device_id *id)
+{
+	struct panjit_i2c_ts_platform_data *pdata = client->dev.platform_data;
+	struct pj_data *touch = NULL;
+	struct input_dev *input_dev = NULL;
+	int ret = 0;
+
+	touch = kzalloc(sizeof(struct pj_data), GFP_KERNEL);
+	if (!touch) {
+		dev_err(&client->dev, "%s: no memory\n", __func__);
+		return -ENOMEM;
+	}
+
+	touch->gpio_reset = -EINVAL;
+
+	if (pdata) {
+		ret = gpio_request(pdata->gpio_reset, "panjit_reset");
+		if (!ret) {
+			ret = gpio_direction_output(pdata->gpio_reset, 1);
+			if (ret < 0)
+				gpio_free(pdata->gpio_reset);
+		}
+
+		if (!ret)
+			touch->gpio_reset = pdata->gpio_reset;
+		else
+			dev_warn(&client->dev, "unable to configure GPIO\n");
+	}
+
+	input_dev = input_allocate_device();
+	if (!input_dev) {
+		dev_err(&client->dev, "%s: no memory\n", __func__);
+		kfree(touch);
+		return -ENOMEM;
+	}
+
+	touch->client = client;
+	i2c_set_clientdata(client, touch);
+
+	pj_reset(touch);
+
+	/* clear interrupt */
+	ret = i2c_smbus_write_byte_data(touch->client, C_FLAG, 0);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: clear interrupt failed\n",
+			__func__);
+		goto fail_i2c_or_register;
+	}
+
+	/* enable scanning */
+	ret = i2c_smbus_write_byte_data(touch->client, CSR, CSR_SCAN_EN);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: enable interrupt failed\n",
+			__func__);
+		goto fail_i2c_or_register;
+	}
+
+	touch->input_dev = input_dev;
+	touch->input_dev->name = DRIVER_NAME;
+
+	set_bit(EV_SYN, touch->input_dev->evbit);
+	set_bit(EV_KEY, touch->input_dev->evbit);
+	set_bit(EV_ABS, touch->input_dev->evbit);
+	set_bit(BTN_TOUCH, touch->input_dev->keybit);
+	set_bit(BTN_2, touch->input_dev->keybit);
+
+	/* expose multi-touch capabilities */
+	set_bit(ABS_MT_POSITION_X, touch->input_dev->keybit);
+	set_bit(ABS_MT_POSITION_Y, touch->input_dev->keybit);
+	set_bit(ABS_X, touch->input_dev->keybit);
+	set_bit(ABS_Y, touch->input_dev->keybit);
+
+	/* all coordinates are reported in 0..4095 */
+	input_set_abs_params(touch->input_dev, ABS_X, 0, 4095, 0, 0);
+	input_set_abs_params(touch->input_dev, ABS_Y, 0, 4095, 0, 0);
+	input_set_abs_params(touch->input_dev, ABS_HAT0X, 0, 4095, 0, 0);
+	input_set_abs_params(touch->input_dev, ABS_HAT0Y, 0, 4095, 0, 0);
+	input_set_abs_params(touch->input_dev, ABS_HAT1X, 0, 4095, 0, 0);
+	input_set_abs_params(touch->input_dev, ABS_HAT1Y, 0, 4095, 0, 0);
+
+	input_set_abs_params(touch->input_dev, ABS_MT_POSITION_X, 0, 4095, 0, 0);
+	input_set_abs_params(touch->input_dev, ABS_MT_POSITION_Y, 0, 4095, 0, 0);
+	input_set_abs_params(touch->input_dev, ABS_MT_TRACKING_ID, 0, 2, 1, 0);
+
+	ret = input_register_device(touch->input_dev);
+	if (ret) {
+		dev_err(&client->dev, "%s: input_register_device failed\n",
+			__func__);
+		goto fail_i2c_or_register;
+	}
+
+	/* get the irq */
+	ret = request_threaded_irq(touch->client->irq, NULL, pj_irq,
+				   IRQF_ONESHOT | IRQF_TRIGGER_LOW,
+				   DRIVER_NAME, touch);
+	if (ret) {
+		dev_err(&client->dev, "%s: request_irq(%d) failed\n",
+			__func__, touch->client->irq);
+		goto fail_irq;
+	}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	touch->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	touch->early_suspend.suspend = pj_early_suspend;
+	touch->early_suspend.resume = pj_late_resume;
+	register_early_suspend(&touch->early_suspend);
+#endif
+	dev_info(&client->dev, "%s: initialized\n", __func__);
+	return 0;
+
+fail_irq:
+	input_unregister_device(touch->input_dev);
+
+fail_i2c_or_register:
+	if (touch->gpio_reset >= 0)
+		gpio_free(touch->gpio_reset);
+
+	input_free_device(input_dev);
+	kfree(touch);
+	return ret;
+}
+
+static int pj_suspend(struct i2c_client *client, pm_message_t state)
+{
+	struct pj_data *touch = i2c_get_clientdata(client);
+	int ret;
+
+	if (WARN_ON(!touch))
+		return -EINVAL;
+
+	disable_irq(client->irq);
+
+	/* disable scanning and enable deep sleep */
+	ret = i2c_smbus_write_byte_data(client, CSR, CSR_SLEEP_EN);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: sleep enable fail\n", __func__);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int pj_resume(struct i2c_client *client)
+{
+	struct pj_data *touch = i2c_get_clientdata(client);
+	int ret = 0;
+
+	if (WARN_ON(!touch))
+		return -EINVAL;
+
+	pj_reset(touch);
+
+	/* enable scanning and disable deep sleep */
+	ret = i2c_smbus_write_byte_data(client, C_FLAG, 0);
+	if (ret >= 0)
+		ret = i2c_smbus_write_byte_data(client, CSR, CSR_SCAN_EN);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: scan enable fail\n", __func__);
+		return ret;
+	}
+
+	enable_irq(client->irq);
+
+	return 0;
+}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void pj_early_suspend(struct early_suspend *es)
+{
+	struct pj_data *touch;
+	touch = container_of(es, struct pj_data, early_suspend);
+
+	if (pj_suspend(touch->client, PMSG_SUSPEND) != 0)
+		dev_err(&touch->client->dev, "%s: failed\n", __func__);
+}
+
+static void pj_late_resume(struct early_suspend *es)
+{
+	struct pj_data *touch;
+	touch = container_of(es, struct pj_data, early_suspend);
+
+	if (pj_resume(touch->client) != 0)
+		dev_err(&touch->client->dev, "%s: failed\n", __func__);
+}
+#endif
+
+static int pj_remove(struct i2c_client *client)
+{
+	struct pj_data *touch = i2c_get_clientdata(client);
+
+	if (!touch)
+		return -EINVAL;
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	unregister_early_suspend(&touch->early_suspend);
+#endif
+	free_irq(touch->client->irq, touch);
+	if (touch->gpio_reset >= 0)
+		gpio_free(touch->gpio_reset);
+	input_unregister_device(touch->input_dev);
+	input_free_device(touch->input_dev);
+	kfree(touch);
+	return 0;
+}
+
+static const struct i2c_device_id panjit_ts_id[] = {
+	{ DRIVER_NAME, 0 },
+	{ }
+};
+
+static struct i2c_driver panjit_driver = {
+	.probe		= pj_probe,
+	.remove		= pj_remove,
+#ifndef CONFIG_HAS_EARLYSUSPEND
+	.suspend	= pj_suspend,
+	.resume		= pj_resume,
+#endif
+	.id_table	= panjit_ts_id,
+	.driver		= {
+		.name	= DRIVER_NAME,
+	},
+};
+
+static int __devinit panjit_init(void)
+{
+	int e;
+
+	e = i2c_add_driver(&panjit_driver);
+	if (e != 0) {
+		pr_err("%s: failed to register with I2C bus with "
+		       "error: 0x%x\n", __func__, e);
+	}
+	return e;
+}
+
+static void __exit panjit_exit(void)
+{
+	i2c_del_driver(&panjit_driver);
+}
+
+module_init(panjit_init);
+module_exit(panjit_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Panjit I2C touch driver");
diff --git a/drivers/input/touchscreen/rm31080a_ts.c b/drivers/input/touchscreen/rm31080a_ts.c
new file mode 100644
index 000000000000..35e85a254862
--- /dev/null
+++ b/drivers/input/touchscreen/rm31080a_ts.c
@@ -0,0 +1,1370 @@
+/*
+
+ * Raydium RM31080(T007) touchscreen (SPI bus) - Android version
+ *
+ * Copyright (C) 2011-2012 Raydium Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ *
+ * Version : 0.04
+ */
+
+//=============================================================================
+//INCLUDED FILES
+//=============================================================================
+#include <linux/input.h>	// BUS_SPI
+#include <linux/spi/spi.h>
+#include <linux/device.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/gpio.h>
+#include <linux/sched.h>	// wake_up_process()
+#include <linux/kthread.h>	// kthread_create()、kthread_run()
+#include <asm/uaccess.h>	// copy_to_user(),
+#include <linux/miscdevice.h>
+#include <asm/siginfo.h>	// siginfo
+#include <linux/rcupdate.h>	// rcu_read_lock
+#include <linux/sched.h>	// find_task_by_pid_type
+#include <linux/syscalls.h>	// sys_clock_gettime()
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+#include <linux/earlysuspend.h>
+#endif
+
+#include <linux/spi/rm31080a_ts.h>
+//=============================================================================
+//DEFINITIONS
+//=============================================================================
+#define ENABLE_WORK_QUEUE
+#define ENABLE_REPORT_TO_UART
+#define ENABLE_RM31080_DEEP_SLEEP
+#define ENABLE_AUTO_SCAN
+//#define ENABLE_AUTO_FREQ
+//#define ENABLE_SPEED_TEST_FUNCTION
+//#define ENABLE_TEST_AVERAGE
+//#define ENABLE_CALC_QUEUE_COUNT
+
+#define MAX_SPI_FREQ_HZ      50000000
+#define TS_PEN_UP_TIMEOUT    msecs_to_jiffies(50)
+
+#ifdef ENABLE_RAW_DATA_QUEUE
+#define QUEUE_COUNT       128
+#define RAW_DATA_LENGTH  2048
+
+#define RM_SCAN_MODE_MANUAL          0x00
+#define RM_SCAN_MODE_PREPARE_AUTO    0x01
+#define RM_SCAN_MODE_AUTO_SCAN       0x02
+
+#define RM_NEED_NONE                 0x00
+#define RM_NEED_TO_SEND_SCAN         0x01
+#define RM_NEED_TO_READ_RAW_DATA     0x02
+#define RM_NEED_TO_SEND_SIGNAL       0x03
+#endif
+
+#ifdef ENABLE_WORK_QUEUE
+#include <linux/workqueue.h>
+#endif
+
+//=============================================================================
+//STRUCTURE DECLARATION
+//=============================================================================
+struct rm31080a_ts_para {
+	unsigned long ulHalPID;
+	bool bInitFinish;
+	bool bCalcFinish;
+	bool bEnableScriber;
+	bool bEnableAutoScan;
+	bool bIsSuspended;
+	struct mutex mutex;
+#ifdef ENABLE_WORK_QUEUE
+	struct workqueue_struct *rm_workqueue;
+	struct work_struct rm_work;
+	bool bIsWorkQueueExecuting;
+#endif
+#ifdef ENABLE_RAW_DATA_QUEUE
+	u8 u8ScanModeState;
+#endif
+};
+
+struct rm31080_ts {
+	const struct rm31080_bus_ops *bops;
+	struct device *dev;
+	struct input_dev *input;
+	unsigned int irq;
+	bool disabled;
+	bool suspended;
+	char phys[32];
+	struct mutex access_mutex;
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+	struct early_suspend early_suspend;
+#endif
+};
+
+struct rm31080_bus_ops {
+	u16 bustype;
+	int (*read) (struct device * dev, u8 reg);
+	int (*multi_read) (struct device * dev, u8 first_reg, u8 count,
+			   u16 * buf);
+	int (*write) (struct device * dev, u8 reg, u16 val);
+};
+
+#ifdef ENABLE_RAW_DATA_QUEUE
+struct rm31080_queue_info {
+	u8(*pQueue)[RAW_DATA_LENGTH];
+	u16 u16Front;
+	u16 u16Rear;
+};
+#endif
+
+//=============================================================================
+//GLOBAL VARIABLES DECLARATION
+//=============================================================================
+struct input_dev *g_input_dev;
+struct spi_device *g_spi;
+struct rm31080a_ts_para g_stTs;
+
+#ifdef ENABLE_RAW_DATA_QUEUE
+struct rm31080_queue_info g_stQ;
+#endif
+
+//=============================================================================
+//FUNCTION DECLARATION
+//=============================================================================
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+static void rm31080_early_suspend(struct early_suspend *es);
+static void rm31080_early_resume(struct early_suspend *es);
+#endif
+//=============================================================================
+// Description:
+//      Debug function: test speed.
+// Input:
+//      N/A
+// Output:
+//      1:succeed
+//      0:failed
+//=============================================================================
+#ifdef ENABLE_SPEED_TEST_FUNCTION
+void my_calc_time(int iStart)
+{
+	static volatile unsigned int u32Max = UINT_MAX;
+
+	static long iTimebuffer[1000];
+	static unsigned long long t1, t2;
+	unsigned long nanosec_rem;
+	static int iIndex = 0;
+
+	if (iStart) {
+		t1 = cpu_clock(u32Max);
+		return;
+	} else
+		t2 = cpu_clock(u32Max);
+
+	t2 = t2 - t1;
+
+	nanosec_rem = do_div(t2, 1000000000);
+
+	if (t2) {			//more than 1 Second
+		iTimebuffer[iIndex] = 999999;
+	} else {
+		iTimebuffer[iIndex] = nanosec_rem / 1000;	//micro second
+	}
+
+	iIndex++;
+	if (iIndex == 1000) {
+		for (iIndex = 0; iIndex < 1000; iIndex++) {
+			printk("   %04d,%06d\n", iIndex,
+			       (u32) iTimebuffer[iIndex]);
+		}
+		iIndex = 0;
+	}
+
+}
+#endif				//ENABLE_SPEED_TEST_FUNCTION
+//=============================================================================
+// Description:
+//      RM31080 spi interface.
+// Input:
+//      N/A
+// Output:
+//      1:succeed
+//      0:failed
+//=============================================================================
+int rm31080_spi_read(u8 u8addr, u8 * rxbuf, size_t len)
+{
+	static DEFINE_MUTEX(lock);
+
+	int status;
+	struct spi_message message;
+	struct spi_transfer x[2];
+
+	if (!mutex_trylock(&lock)) {
+		//printk("Raydium TS: rm31080_spi_read trylock fail\n");
+		return -EINVAL;
+	}
+
+	spi_message_init(&message);
+	memset(x, 0, sizeof x);
+
+	u8addr |= 0x80;
+	x[0].len = 1;
+	x[0].tx_buf = &u8addr;
+	spi_message_add_tail(&x[0], &message);
+
+	x[1].len = len;
+	x[1].rx_buf = rxbuf;
+	spi_message_add_tail(&x[1], &message);
+
+	status = spi_sync(g_spi, &message);
+
+	mutex_unlock(&lock);
+	return status;		// 0 = succeed
+}
+
+int rm31080_spi_write(u8 * txbuf, size_t len)
+{
+	return spi_write(g_spi, txbuf, len);
+}
+
+static int rm31080_spi_byte_read(u8 u8Addr, u8 * pu8Value)
+{
+	int iErrorCode;
+	iErrorCode = rm31080_spi_read(u8Addr, pu8Value, 1);
+	if (iErrorCode != 0) {
+		return 0;	//fail
+	}
+	return 1;
+}
+
+static int rm31080_spi_byte_write(u8 u8Addr, u8 u8Value)
+{
+	int iErrorCode;
+	u8 buf[2];
+	buf[0] = u8Addr;
+	buf[1] = u8Value;
+
+	iErrorCode = rm31080_spi_write(buf, 2);
+
+	if (iErrorCode != 0) {
+		//printk("rm31080_spi_write_byte failed:Reg=%x", u8Addr);
+		return 0;	//fail
+	}
+	return 1;
+}
+
+//=============================================================================
+// Description:
+//      RM31080 control functions.
+// Input:
+//      N/A
+// Output:
+//      1:succeed
+//      0:failed
+//=============================================================================
+#ifdef ENABLE_RAW_DATA_QUEUE
+
+#define RM31080_REG_01 0x01
+#define RM31080_REG_02 0x02
+#define RM31080_REG_09 0x09
+#define RM31080_REG_0E 0x0E
+#define RM31080_REG_10 0x10
+#define RM31080_REG_11 0x11
+#define RM31080_REG_1F 0x1F
+#define RM31080_REG_40 0x40
+#define RM31080_REG_41 0x41
+#define RM31080_REG_80 0x80
+#define RM31080_REG_F2 0xF2
+
+#define RM31080_RAW_DATA_LENGTH 1530
+static int rm31080_ctrl_clear_int(void)
+{
+	u8 u8Flag;
+	return rm31080_spi_byte_read(RM31080_REG_F2, &u8Flag);
+}
+
+#ifdef ENABLE_AUTO_SCAN
+void rm31080_ctrl_enter_auto_mode(void)
+{
+	//Enable auto scan
+	rm31080_spi_byte_write(RM31080_REG_09, 0x10 | 0x40);
+}
+
+void rm31080_ctrl_leave_auto_mode(void)
+{
+	//Disable auto scan
+	rm31080_spi_byte_write(RM31080_REG_09, 0x00);
+}
+#endif				//ENABLE_AUTO_SCAN
+
+#ifdef ENABLE_RM31080_DEEP_SLEEP
+static int rm31080_ctrl_suspend(void)
+{
+	//Flow designed by Roger 20110930
+	//rm31080_ts_send_signal(g_stTs.ulHalPID,RM_SIGNAL_SUSPEND);
+	g_stTs.bInitFinish = 0;
+	msleep(8);
+	rm31080_ctrl_clear_int();
+	//disable auto scan
+	rm31080_spi_byte_write(RM31080_REG_09, 0x00);
+	rm31080_spi_byte_write(RM31080_REG_10, 0x14);
+	rm31080_spi_byte_write(RM31080_REG_11, 0x17);
+	msleep(15);
+	rm31080_spi_byte_write(RM31080_REG_11, 0x06);
+	return 1;
+}
+#endif
+
+static int rm31080_ctrl_scan_start(void)
+{
+	return rm31080_spi_byte_write(RM31080_REG_11, 0x17);
+}
+
+static u32 rm31080_ctrl_configure(void)
+{
+	u32 u32Flag;
+
+	switch (g_stTs.u8ScanModeState) {
+	case RM_SCAN_MODE_MANUAL:
+		u32Flag =
+		    RM_NEED_TO_SEND_SCAN | RM_NEED_TO_READ_RAW_DATA |
+		    RM_NEED_TO_SEND_SIGNAL;
+		break;
+#ifdef ENABLE_AUTO_SCAN
+	case RM_SCAN_MODE_PREPARE_AUTO:
+		rm31080_ctrl_enter_auto_mode();
+		g_stTs.u8ScanModeState = RM_SCAN_MODE_AUTO_SCAN;
+		u32Flag = RM_NEED_NONE;
+		break;
+	case RM_SCAN_MODE_AUTO_SCAN:
+		rm31080_ctrl_leave_auto_mode();
+		rm31080_ctrl_scan_start();
+		g_stTs.u8ScanModeState = RM_SCAN_MODE_MANUAL;
+		u32Flag =
+		    RM_NEED_TO_SEND_SCAN | RM_NEED_TO_READ_RAW_DATA |
+		    RM_NEED_TO_SEND_SIGNAL;
+		break;
+#endif				//ENABLE_AUTO_SCAN
+	default:
+		u32Flag = RM_NEED_NONE;
+		break;
+	}
+
+	return u32Flag;
+}
+
+static void rm31080_enter_manual_mode(void)
+{
+	flush_workqueue(g_stTs.rm_workqueue);
+
+	if (g_stTs.u8ScanModeState == RM_SCAN_MODE_MANUAL)
+		return;
+
+	if (g_stTs.u8ScanModeState == RM_SCAN_MODE_PREPARE_AUTO) {
+		g_stTs.u8ScanModeState = RM_SCAN_MODE_MANUAL;
+		return;
+	}
+
+	if (g_stTs.u8ScanModeState == RM_SCAN_MODE_AUTO_SCAN) {
+		rm31080_ctrl_leave_auto_mode();
+		g_stTs.u8ScanModeState = RM_SCAN_MODE_MANUAL;
+		msleep(10);
+	}
+}
+
+static int rm31080_ctrl_read_raw_data(u8 * p)
+{
+	int iRet;
+	iRet = rm31080_spi_byte_write(RM31080_REG_01, 0x10);
+	if (iRet)
+		iRet = rm31080_spi_byte_write(RM31080_REG_02, 0x00);
+
+	if (iRet) {
+		iRet = rm31080_spi_read(RM31080_REG_80, p, RM31080_RAW_DATA_LENGTH);	//return 0 =succeed
+		iRet = !iRet;
+	}
+
+	if (!iRet) {
+		//printk("rm31080 read raw data failed\n");
+	}
+
+	return iRet;
+}
+#endif				//ENABLE_RAW_DATA_QUEUE
+//=============================================================================
+// Description:
+//      Queuing functions.
+// Input:
+//      N/A
+// Output:
+//      0:succeed
+//      others:error code
+//=============================================================================
+#ifdef ENABLE_RAW_DATA_QUEUE
+
+static void rm31080_queue_reset(void)
+{
+	g_stQ.u16Rear = 0;
+	g_stQ.u16Front = 0;
+}
+
+static int rm31080_queue_init(void)
+{
+	rm31080_queue_reset();
+	g_stQ.pQueue = kmalloc(QUEUE_COUNT * RAW_DATA_LENGTH, GFP_KERNEL);
+	if (g_stQ.pQueue == NULL) {
+		//printk("rm31080_queue_init failed\n");
+		return -ENOMEM;
+	}
+	return 0;
+}
+
+static void rm31080_queue_free(void)
+{
+	if (!g_stQ.pQueue)
+		return;
+	kfree(g_stQ.pQueue);
+	g_stQ.pQueue = NULL;
+}
+
+//=============================================================================
+// Description:
+//  About full/empty buffer distinction,
+//  There are a number of solutions like:
+//  1.Always keep one slot open.
+//  2.Use a fill count to distinguish the two cases.
+//  3.Use read and write counts to get the fill count from.
+//  4.Use absolute indices.
+//  we chose "keep one slot open" to make it simple and robust
+//  and also avoid race condition.
+// Input:
+//      N/A
+// Output:
+//      1:empty
+//      0:not empty
+//=============================================================================
+static int rm31080_queue_is_empty(void)
+{
+	if (g_stQ.u16Rear == g_stQ.u16Front)
+		return 1;
+	return 0;
+}
+
+//=============================================================================
+// Description:
+//  check queue full.
+// Input:
+//      N/A
+// Output:
+//      1:full
+//      0:not full
+//=============================================================================
+static int rm31080_queue_is_full(void)
+{
+	if (g_stQ.u16Rear + 1 == g_stQ.u16Front)
+		return 1;
+
+	if ((g_stQ.u16Rear == (QUEUE_COUNT - 1)) && (g_stQ.u16Front == 0))
+		return 1;
+
+	return 0;
+}
+
+#ifdef ENABLE_CALC_QUEUE_COUNT
+static int rm31080_queue_get_current_count(void)
+{
+	if (g_stQ.u16Rear >= g_stQ.u16Front)
+		return g_stQ.u16Rear - g_stQ.u16Front;
+
+	return (QUEUE_COUNT - g_stQ.u16Front) + g_stQ.u16Rear;
+}
+#endif
+static void *rm31080_enqueue_start(void)
+{
+	if (!g_stQ.pQueue)	//error handling for no memory
+		return NULL;
+
+	if (!rm31080_queue_is_full())
+		return &g_stQ.pQueue[g_stQ.u16Rear];
+
+	//printk("rm31080 Queue full with Queue Count:%d\n", QUEUE_COUNT);
+	return NULL;
+}
+
+static void rm31080_enqueue_finish(void)
+{
+	if (g_stQ.u16Rear == (QUEUE_COUNT - 1))
+		g_stQ.u16Rear = 0;
+	else
+		g_stQ.u16Rear++;
+}
+
+static void *rm31080_dequeue_start(void)
+{
+	if (!rm31080_queue_is_empty())
+		return &g_stQ.pQueue[g_stQ.u16Front];
+
+	return NULL;
+}
+
+static void rm31080_dequeue_finish(void)
+{
+	if (g_stQ.u16Front == (QUEUE_COUNT - 1))
+		g_stQ.u16Front = 0;
+	else
+		g_stQ.u16Front++;
+}
+
+static long rm31080_queue_read_raw_data(u8 * p, u32 u32Len)
+{
+	u8 *pQueue;
+	u32 u32Ret;
+	pQueue = rm31080_dequeue_start();
+	if (!pQueue)
+		return 0;
+
+	u32Ret = copy_to_user(p, pQueue, u32Len);
+	if (u32Ret != 0)
+		return 0;
+
+	rm31080_dequeue_finish();
+	return 1;
+
+}
+#endif				//ENABLE_RAW_DATA_QUEUE
+
+//=============================================================================
+// Description:
+//      Copy Config(Parameters) to HAL's Buffer
+// Input:
+//      p: HAL's buffer
+//      u32Len : buffer size
+// Output:
+//      1: succeed
+//      0: failed
+//=============================================================================
+static long rm31080_get_config(u8 * p, u32 u32Len)
+{
+	u32 u32Ret;
+	struct rm_spi_ts_platform_data *pdata;
+	pdata = g_input_dev->dev.parent->platform_data;
+	u32Ret = copy_to_user(p, pdata->config, u32Len);
+	if (u32Ret != 0)
+		return 0;
+	return 1;
+}
+
+#ifdef ENABLE_AUTO_FREQ
+void raydium_auto_freq()
+{
+	g_stTs.bInitFinish = 0;
+	msleep(10);
+	rm31080_ctrl_clear_int();
+
+	//roger_auto_freq_detection();
+
+	g_stTs.bInitFinish = 1;
+	rm31080_ctrl_scan_start();
+
+}
+#endif				//ENABLE_TEST_AUTO_FREQ
+//=============================================================================
+#ifdef ENABLE_AUTO_SCAN
+void raydium_change_scan_mode(u8 u8TouchCount)
+{
+	static u32 u32NoTouchCount = 0;
+	if (u8TouchCount) {
+		u32NoTouchCount = 0;
+		return;
+	}
+	if (u32NoTouchCount < 100) {
+		u32NoTouchCount++;
+	} else if (g_stTs.u8ScanModeState == RM_SCAN_MODE_MANUAL) {
+#ifdef ENABLE_AUTO_FREQ
+		raydium_auto_freq();
+#else
+		if (g_stTs.bEnableAutoScan)
+			g_stTs.u8ScanModeState = RM_SCAN_MODE_PREPARE_AUTO;
+#endif
+		u32NoTouchCount = 0;
+	}
+}
+#endif				//ENABLE_AUTO_SCAN
+//=============================================================================
+//report touch data for scriber
+//
+//=============================================================================
+#ifdef ENABLE_REPORT_TO_UART
+void raydium_report_to_uart_printf(unsigned char *ucData, unsigned char ucCount)
+{
+	unsigned char i;
+	for (i = 0; i < ucCount; i++) {
+		printk("%02X", ucData[i]);
+	}
+	printk("\n");
+}
+
+void raydium_report_to_uart(void *p)
+{
+	unsigned char ucData[1 + 1 + (4 * 12) + 1];	//1=Tag,1=Touch count,4=(xH xL ,yH yL) ,12=max point,1=Check sum
+	rm_touch_event *spTP;
+	unsigned short usX, usY;
+	int i, j;
+
+	if (g_stTs.bEnableScriber == 0)
+		return;
+
+	spTP = (rm_touch_event *) p;
+
+	ucData[0] = 0x8E;
+	ucData[1] = spTP->ucTouchCount;
+	j = 2;
+	for (i = 0; i < spTP->ucTouchCount; i++) {
+		usX = spTP->usX[i] + 1;	//1~1536
+		usY = spTP->usY[i] + 1;	//1~960
+		ucData[j++] = ((usX >> 8) & 0xFF) | (spTP->ucID[i] << 4);	//add id
+		ucData[j++] = ((usX) & 0xFF);
+		ucData[j++] = ((usY >> 8) & 0xFF);
+		ucData[j++] = ((usY) & 0xFF);
+	}
+
+	//check sum
+	ucData[j] = 0;
+	for (i = 0; i < j; i++) {
+		ucData[j] += ucData[i];
+	}
+	ucData[j] = 0x100 - ucData[j];
+	j++;
+
+	//print
+	raydium_report_to_uart_printf(ucData, j);
+	if (spTP->ucTouchCount == 0)	//send more , to avoid losing
+	{
+		raydium_report_to_uart_printf(ucData, j);
+		raydium_report_to_uart_printf(ucData, j);
+	}
+}
+#endif
+//=============================================================================
+void raydium_report_pointer(void *p)
+{
+	static unsigned char ucLastTouchCount = 0;
+	int i;
+	int iCount;
+	rm_touch_event *spTP;
+	spTP = (rm_touch_event *) p;
+
+	iCount = max(ucLastTouchCount, spTP->ucTouchCount);
+	if (iCount) {
+		for (i = 0; i < iCount; i++) {
+			if (i == 10)
+				break;	//due to the "touch test" can't support great than 10 points
+
+			if (i < spTP->ucTouchCount) {
+				input_report_abs(g_input_dev,
+						 ABS_MT_TRACKING_ID,
+						 spTP->ucID[i]);
+
+				input_report_abs(g_input_dev,
+						 ABS_MT_TOUCH_MAJOR, 100);
+				if (spTP->usX[i] >= (RM_INPUT_RESOLUTION_X - 1))
+					input_report_abs(g_input_dev, ABS_MT_POSITION_X, (RM_INPUT_RESOLUTION_X - 1) - 1);	//fixed bug: OS scale fail
+				else
+					input_report_abs(g_input_dev,
+							 ABS_MT_POSITION_X,
+							 spTP->usX[i]);
+
+				if (spTP->usY[i] >= (RM_INPUT_RESOLUTION_Y - 1))
+					input_report_abs(g_input_dev, ABS_MT_POSITION_Y, (RM_INPUT_RESOLUTION_Y - 1) - 1);	//fixed bug: OS scale fail
+				else
+					input_report_abs(g_input_dev,
+							 ABS_MT_POSITION_Y,
+							 spTP->usY[i]);
+			}
+			input_mt_sync(g_input_dev);
+		}
+		ucLastTouchCount = spTP->ucTouchCount;
+		input_report_key(g_input_dev, BTN_TOUCH,
+				 spTP->ucTouchCount > 0);
+		input_sync(g_input_dev);
+#ifdef ENABLE_REPORT_TO_UART
+		raydium_report_to_uart(p);
+#endif
+
+	}
+
+#ifdef ENABLE_AUTO_SCAN
+	raydium_change_scan_mode(spTP->ucTouchCount);
+#endif
+}
+
+//=============================================================================
+
+//=============================================================================
+int rm31080_ts_send_signal(int pid, int iInfo)
+{
+	struct siginfo info;
+	struct task_struct *t;
+	int ret;
+
+	/* send the signal */
+	memset(&info, 0, sizeof(struct siginfo));
+	info.si_signo = RM_TS_SIGNAL;
+	info.si_code = SI_QUEUE;	// this is bit of a trickery: SI_QUEUE is normally used by sigqueue from user space,
+	// and kernel space should use SI_KERNEL. But if SI_KERNEL is used the real_time data
+	// is not delivered to the user space signal handler function.
+	info.si_int = iInfo;	//real time signals may have 32 bits of data.
+
+	rcu_read_lock();
+	t = find_task_by_vpid(pid);
+	if (t == NULL) {
+		//printk("no such pid\n");
+		rcu_read_unlock();
+		return -ENODEV;
+	}
+	rcu_read_unlock();
+	ret = send_sig_info(RM_TS_SIGNAL, &info, t);	//send the signal
+	if (ret < 0) {
+		//printk("error sending signal\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+//=============================================================================
+static void __rm31080_enable(struct rm31080_ts *ts)
+{
+	enable_irq(ts->irq);
+}
+
+static void __rm31080_disable(struct rm31080_ts *ts)
+{
+	disable_irq(ts->irq);
+}
+
+static void vtest_toggle(struct rm31080_ts *ts, bool disable)
+{
+	mutex_lock(&ts->input->mutex);
+
+	if (!ts->suspended && ts->input->users != 0) {
+
+		if (disable) {
+			if (ts->disabled)
+				__rm31080_enable(ts);
+		} else {
+			if (!ts->disabled)
+				__rm31080_disable(ts);
+		}
+	}
+
+	ts->disabled = disable;
+
+	mutex_unlock(&ts->input->mutex);
+}
+
+static ssize_t vtest_disable_show(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	struct rm31080_ts *ts = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%u\n", ts->disabled);
+}
+
+static ssize_t vtest_disable_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t count)
+{
+	struct rm31080_ts *ts = dev_get_drvdata(dev);
+	unsigned long val;
+	int error;
+
+	error = strict_strtoul(buf, 10, &val);
+	if (error)
+		return error;
+
+	vtest_toggle(ts, val);
+
+	return count;
+}
+
+static DEVICE_ATTR(disable, 0664, vtest_disable_show, vtest_disable_store);
+static struct attribute *vtest_attributes[] = {
+	&dev_attr_disable.attr,
+	NULL
+};
+
+static const struct attribute_group vtest_attr_group = {
+	.attrs = vtest_attributes,
+};
+
+static int rm31080_input_open(struct input_dev *input)
+{
+	struct rm31080_ts *ts = input_get_drvdata(input);
+
+	/* protected by input->mutex */
+	if (!ts->disabled && !ts->suspended)
+		__rm31080_enable(ts);
+
+	return 0;
+}
+
+static void rm31080_input_close(struct input_dev *input)
+{
+	struct rm31080_ts *ts = input_get_drvdata(input);
+
+	/* protected by input->mutex */
+	if (!ts->disabled && !ts->suspended)
+		__rm31080_disable(ts);
+}
+
+//=============================================================================
+
+#ifdef ENABLE_TEST_AVERAGE	//only for test
+#define _AVERAGE_COUNT 2
+s8 g_bAverageBuf[_AVERAGE_COUNT][2048];
+int test_soft_average(s8 * pSource)
+{
+	static u8 u8AverageIndex = 0;
+	static u8 u8StartAverage = 0;
+	u16 i, j;
+	s16 s16Sum;
+
+	for (i = 0; i < RM31080_RAW_DATA_LENGTH; i++)	//RM31080_RAW_DATA_LENGTH =1530
+		g_bAverageBuf[u8AverageIndex][i] = pSource[i] - 0x80;
+	u8AverageIndex++;
+
+	if (u8AverageIndex == _AVERAGE_COUNT) {
+		u8StartAverage = 1;
+		u8AverageIndex = 0;
+	}
+	else
+	{
+		u8StartAverage = 0;
+	}
+
+	if (u8StartAverage) {
+		for (i = 0; i < RM31080_RAW_DATA_LENGTH; i++) {
+			s16Sum = 0;
+			for (j = 0; j < _AVERAGE_COUNT; j++)
+				s16Sum += g_bAverageBuf[j][i];
+			pSource[i] = (s16Sum / _AVERAGE_COUNT) + 0x80;
+		}
+		return 1;
+	}
+	return 0;
+}
+#endif
+
+#ifdef ENABLE_WORK_QUEUE
+//1.2
+static void rm_work_handler(struct work_struct *work)
+{
+	void *pKernelBuffer;
+	u32 u32Flag;
+	int iRet;
+
+
+	if (g_stTs.bIsSuspended) {
+		//printk("rm_work_handler stops after suspend\n");
+		return;
+	}
+
+	g_stTs.bIsWorkQueueExecuting = 1;
+
+	iRet = rm31080_ctrl_clear_int();
+
+	u32Flag = rm31080_ctrl_configure();
+
+	if (u32Flag | RM_NEED_TO_SEND_SCAN) {
+		rm31080_ctrl_scan_start();
+	}
+
+	if (u32Flag | RM_NEED_TO_READ_RAW_DATA) {
+		pKernelBuffer = rm31080_enqueue_start();
+		if (pKernelBuffer) {
+			iRet = rm31080_ctrl_read_raw_data((u8 *) pKernelBuffer);
+#ifdef ENABLE_TEST_AVERAGE
+			if (iRet) {
+				iRet = test_soft_average((s8 *) pKernelBuffer);
+			}
+#endif
+			if (iRet) {
+				rm31080_enqueue_finish();
+			}
+		}
+	}
+
+	if (u32Flag | RM_NEED_TO_SEND_SIGNAL) {
+		if (g_stTs.bCalcFinish) {
+			g_stTs.bCalcFinish = 0;
+			rm31080_ts_send_signal(g_stTs.ulHalPID, RM_SIGNAL_INTR);
+		}
+	}
+	g_stTs.bIsWorkQueueExecuting = 0;
+}
+#endif
+
+static irqreturn_t rm31080_irq(int irq, void *handle)
+{
+
+	//struct rm31080_ts *ts = handle;
+	if (!g_stTs.bInitFinish) {
+		return IRQ_HANDLED;
+	}
+
+#ifdef ENABLE_WORK_QUEUE
+	queue_work(g_stTs.rm_workqueue, &g_stTs.rm_work);
+#endif
+
+	return IRQ_HANDLED;
+}
+
+//=============================================================================
+static void rm31080_init_ts_structure_part(void)
+{
+	g_stTs.bInitFinish = 0;
+	g_stTs.bCalcFinish = 0;
+	g_stTs.bEnableScriber = 0;
+	g_stTs.bIsSuspended = 0;
+	g_stTs.bEnableAutoScan = 1;
+
+#ifdef ENABLE_RAW_DATA_QUEUE
+	g_stTs.u8ScanModeState = RM_SCAN_MODE_MANUAL;
+#endif
+}
+
+static void rm31080_init_ts_structure(void)
+{
+	g_stTs.ulHalPID = 0;
+
+	memset(&g_stTs, 0, sizeof(struct rm31080a_ts_para));
+
+#ifdef ENABLE_WORK_QUEUE
+	g_stTs.rm_workqueue = create_singlethread_workqueue("rm_work");
+	INIT_WORK(&g_stTs.rm_work, rm_work_handler);
+	g_stTs.bIsWorkQueueExecuting = 0;
+#endif
+}
+
+//=============================================================================
+static void rm31080_start(struct rm31080_ts *ts)
+{
+#ifdef ENABLE_RM31080_DEEP_SLEEP
+	struct rm_spi_ts_platform_data *pdata;
+#endif
+
+	if (!g_stTs.bIsSuspended)
+		return;
+	g_stTs.bIsSuspended = 0;
+
+#ifdef ENABLE_RM31080_DEEP_SLEEP
+	//flow designed by Roger //20110930
+	pdata = g_input_dev->dev.parent->platform_data;
+	gpio_set_value(pdata->gpio_reset, 0);
+	msleep(120);
+	gpio_set_value(pdata->gpio_reset, 1);
+	msleep(10);
+	rm31080_init_ts_structure_part();
+	rm31080_ts_send_signal(g_stTs.ulHalPID, RM_SIGNAL_RESUME);
+#elif defined(ENABLE_AUTO_SCAN)
+	rm31080_ctrl_clear_int();
+	rm31080_ctrl_scan_start();
+#endif
+
+}
+
+static void rm31080_stop(struct rm31080_ts *ts)
+{
+	int iCount;
+	if (g_stTs.bIsSuspended)
+		return;
+
+	iCount = 0;
+	while (g_stTs.bIsWorkQueueExecuting) {
+		//printk("Raydium TS: Work_Queue is Executing.\n");
+		msleep(1);
+		iCount++;
+		if (iCount > 1000)
+			break;
+	}
+	g_stTs.bIsSuspended = 1;
+
+#ifdef ENABLE_RM31080_DEEP_SLEEP
+	rm31080_ctrl_suspend();
+#endif
+}
+
+#ifdef CONFIG_PM
+static int rm31080_suspend(struct device *dev)
+{
+	struct rm31080_ts *ts = dev_get_drvdata(dev);
+	rm31080_stop(ts);
+	return 0;
+}
+
+static int rm31080_resume(struct device *dev)
+{
+	struct rm31080_ts *ts = dev_get_drvdata(dev);
+	rm31080_start(ts);
+	return 0;
+}
+
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+static void rm31080_early_suspend(struct early_suspend *es)
+{
+	struct rm31080_ts *ts;
+	struct device *dev;
+
+	ts = container_of(es, struct rm31080_ts, early_suspend);
+	dev = ts->dev;
+
+	if (rm31080_suspend(dev) != 0) {
+		dev_err(dev, "%s: failed\n", __func__);
+	}
+}
+
+static void rm31080_early_resume(struct early_suspend *es)
+{
+	struct rm31080_ts *ts;
+	struct device *dev;
+
+	ts = container_of(es, struct rm31080_ts, early_suspend);
+	dev = ts->dev;
+
+	if (rm31080_resume(dev) != 0) {
+		dev_err(dev, "%s: failed\n", __func__);
+	}
+}
+#else
+static const struct dev_pm_ops rm31080_pm_ops = {
+	.suspend = rm31080_suspend,
+	.resume = rm31080_resume,
+};
+#endif
+#endif
+
+struct rm31080_ts *rm31080_input_init(struct device *dev, unsigned int irq,
+				      const struct rm31080_bus_ops *bops)
+{
+
+	struct rm31080_ts *ts;
+	struct input_dev *input_dev;
+	int err;
+
+	if (!irq) {
+		dev_err(dev, "no IRQ?\n");
+		err = -EINVAL;
+		goto err_out;
+	}
+
+	ts = kzalloc(sizeof(*ts), GFP_KERNEL);
+
+	input_dev = input_allocate_device();
+
+	if (!ts || !input_dev) {
+		dev_err(dev, "Failed to allocate memory\n");
+		err = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	g_input_dev = input_dev;
+
+	ts->bops = bops;
+	ts->dev = dev;
+	ts->input = input_dev;
+	ts->irq = irq;
+
+	snprintf(ts->phys, sizeof(ts->phys), "%s/input0", dev_name(dev));
+
+	input_dev->name = "raydium_ts";
+	input_dev->phys = ts->phys;
+	input_dev->dev.parent = dev;
+	input_dev->id.bustype = bops->bustype;
+
+	input_dev->open = rm31080_input_open;
+	input_dev->close = rm31080_input_close;
+
+	input_set_drvdata(input_dev, ts);
+
+	__set_bit(EV_ABS, input_dev->evbit);
+	__set_bit(ABS_X, input_dev->absbit);
+	__set_bit(ABS_Y, input_dev->absbit);
+	__set_bit(ABS_PRESSURE, input_dev->absbit);
+
+	__set_bit(EV_KEY, input_dev->evbit);
+	__set_bit(BTN_TOUCH, input_dev->keybit);
+
+
+	input_set_abs_params(input_dev, ABS_X,
+			     0, RM_INPUT_RESOLUTION_X - 1, 0, 0);
+	input_set_abs_params(input_dev, ABS_Y,
+			     0, RM_INPUT_RESOLUTION_Y - 1, 0, 0);
+	input_set_abs_params(input_dev, ABS_PRESSURE, 0, 1, 0, 0);
+
+	input_set_abs_params(input_dev, ABS_MT_TOUCH_MAJOR, 0, 0xFF, 0, 0);
+	input_set_abs_params(input_dev, ABS_MT_POSITION_X,
+			     0, RM_INPUT_RESOLUTION_X - 1, 0, 0);
+	input_set_abs_params(input_dev, ABS_MT_POSITION_Y,
+			     0, RM_INPUT_RESOLUTION_Y - 1, 0, 0);
+	input_set_abs_params(input_dev, ABS_MT_TRACKING_ID, 0, 32, 0, 0);
+
+	err = request_threaded_irq(ts->irq, NULL, rm31080_irq,
+				   IRQF_TRIGGER_RISING, dev_name(dev), ts);
+	if (err) {
+		dev_err(dev, "irq %d busy?\n", ts->irq);
+		goto err_free_mem;
+	}
+
+	mutex_init(&ts->access_mutex);
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+	ts->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	ts->early_suspend.suspend = rm31080_early_suspend;
+	ts->early_suspend.resume = rm31080_early_resume;
+	register_early_suspend(&ts->early_suspend);
+#endif
+
+	__rm31080_disable(ts);
+
+	err = sysfs_create_group(&dev->kobj, &vtest_attr_group);
+	if (err)
+		goto err_free_irq;
+
+	err = input_register_device(input_dev);
+	if (err)
+		goto err_remove_attr;
+
+	return ts;
+
+ err_remove_attr:
+	sysfs_remove_group(&dev->kobj, &vtest_attr_group);
+ err_free_irq:
+	free_irq(ts->irq, ts);
+ err_free_mem:
+	input_free_device(input_dev);
+	kfree(ts);
+ err_out:
+	return ERR_PTR(err);
+}
+
+static int dev_open(struct inode *inode, struct file *filp)
+{
+	return 0;
+}
+
+static int dev_release(struct inode *inode, struct file *filp)
+{
+	g_stTs.bInitFinish = 0;
+	rm31080_enter_manual_mode();
+	return 0;
+}
+
+static ssize_t
+dev_read(struct file *filp, char __user * buf, size_t count, loff_t * pos)
+{
+	unsigned long missing;
+	ssize_t status = 0;
+	u8 *pMyBuf;
+
+	pMyBuf = kmalloc(count, GFP_KERNEL);
+	if (pMyBuf == NULL)
+		return -ENOMEM;
+
+	pMyBuf[0] = buf[0];
+	status = rm31080_spi_read(pMyBuf[0], pMyBuf, count);
+
+	if (status != 0) {
+		//printk("rm31080_spi_read() fail\n");
+	}
+	status = count;
+	missing = copy_to_user(buf, pMyBuf, count);
+
+	if (missing == status)
+		status = -EFAULT;
+	else
+		status = status - missing;
+
+	kfree(pMyBuf);
+	return status;
+}
+
+static ssize_t
+dev_write(struct file *filp, const char __user * buf,
+	  size_t count, loff_t * pos)
+{
+	u8 *pMyBuf;
+	unsigned long missing;
+	ssize_t status = 0;
+
+	pMyBuf = kmalloc(count, GFP_KERNEL);
+	if (pMyBuf == NULL)
+		return -ENOMEM;
+
+	missing = copy_from_user(pMyBuf, buf, count);
+	if (missing == 0) {
+		status = rm31080_spi_write(pMyBuf, count);
+	} else
+		status = -EFAULT;
+
+	kfree(pMyBuf);
+	return count;
+}
+
+//=============================================================================
+// Description:
+//      I/O Control routin.
+// Input:
+//      file:
+//      cmd :
+//      arg :
+// Output:
+//      1: succeed
+//      0: failed
+//=============================================================================
+static long dev_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	long ret = 1;
+	switch (cmd & 0xFFFF) {
+	case RM_IOCTL_REPORT_POINT:
+		raydium_report_pointer((void *)arg);
+		break;
+	case RM_IOCTL_SET_HAL_PID:
+		g_stTs.ulHalPID = arg;
+		break;
+	case RM_IOCTL_INIT_START:
+		g_stTs.bInitFinish = 0;
+		rm31080_enter_manual_mode();
+		break;
+	case RM_IOCTL_INIT_END:
+		g_stTs.bInitFinish = 1;
+		g_stTs.bCalcFinish = 1;
+#ifdef ENABLE_RAW_DATA_QUEUE
+		ret = rm31080_ctrl_scan_start();
+#endif
+		break;
+	case RM_IOCTL_FINISH_CALC:
+		g_stTs.bCalcFinish = 1;
+		break;
+	case RM_IOCTL_SCRIBER_CTRL:
+		g_stTs.bEnableScriber = (bool) arg;
+		break;
+	case RM_IOCTL_AUTOSCAN_CTRL:
+		g_stTs.bEnableAutoScan = (bool) arg;
+		break;
+#ifdef ENABLE_RAW_DATA_QUEUE
+	case RM_IOCTL_READ_RAW_DATA:
+		ret =
+		    rm31080_queue_read_raw_data((u8 *) arg,
+						(cmd >> 16) & 0xFFFF);
+		break;
+#endif
+	case RM_IOCTL_SET_PARAMETER:
+		ret = rm31080_get_config((u8 *) arg, (cmd >> 16) & 0xFFFF);
+		break;
+	default:
+		break;
+	}
+	return ret;
+}
+
+static struct file_operations dev_fops = {
+	.owner = THIS_MODULE,
+	.open = dev_open,
+	.release = dev_release,
+	.read = dev_read,
+	.write = dev_write,
+	.unlocked_ioctl = dev_ioctl,
+};
+
+static struct miscdevice raydium_ts_miscdev = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "raydium_ts",
+	.fops = &dev_fops,
+};
+
+static const struct rm31080_bus_ops rm31080_spi_bus_ops = {
+	.bustype = BUS_SPI,
+};
+
+static int __devexit rm31080_spi_remove(struct spi_device *spi)
+{
+	struct rm31080_ts *ts = spi_get_drvdata(spi);
+
+#ifdef ENABLE_RAW_DATA_QUEUE
+	rm31080_queue_free();
+#endif
+
+#ifdef ENABLE_WORK_QUEUE
+	if (g_stTs.rm_workqueue)
+		destroy_workqueue(g_stTs.rm_workqueue);
+#endif
+
+	misc_deregister(&raydium_ts_miscdev);
+
+	sysfs_remove_group(&ts->dev->kobj, &vtest_attr_group);
+	free_irq(ts->irq, ts);
+	input_unregister_device(ts->input);
+	kfree(ts);
+	spi_set_drvdata(spi, NULL);
+	return 0;
+}
+
+static int __devinit rm31080_spi_probe(struct spi_device *spi)
+{
+	struct rm31080_ts *ts;
+
+	rm31080_init_ts_structure();
+	rm31080_init_ts_structure_part();
+
+	if (spi->max_speed_hz > MAX_SPI_FREQ_HZ) {
+		dev_err(&spi->dev, "SPI CLK %d Hz?\n", spi->max_speed_hz);
+		return -EINVAL;
+	}
+
+	ts = rm31080_input_init(&spi->dev, spi->irq, &rm31080_spi_bus_ops);
+	if (IS_ERR(ts))
+		return PTR_ERR(ts);
+	spi_set_drvdata(spi, ts);
+
+	g_spi = spi;
+
+	if (misc_register(&raydium_ts_miscdev) != 0) {
+		dev_err(&spi->dev, "Raydium TS: cannot register miscdev\n");
+		return 0;
+	}
+#ifdef ENABLE_RAW_DATA_QUEUE
+	rm31080_queue_init();
+#endif
+
+	return 0;
+}
+
+static struct spi_driver rm31080_spi_driver = {
+	.driver = {
+			.name = "rm_ts_spidev",
+			.bus = &spi_bus_type,
+			.owner = THIS_MODULE,
+#if !defined(CONFIG_HAS_EARLYSUSPEND)
+			.pm = &rm31080_pm_ops,
+#endif
+			},
+	.probe = rm31080_spi_probe,
+	.remove = __devexit_p(rm31080_spi_remove),
+};
+
+static int __init rm31080_spi_init(void)
+{
+	return spi_register_driver(&rm31080_spi_driver);
+}
+module_init(rm31080_spi_init);
+
+static void __exit rm31080_spi_exit(void)
+{
+	spi_unregister_driver(&rm31080_spi_driver);
+}
+module_exit(rm31080_spi_exit);
+
+MODULE_AUTHOR("Valentine Hsu <valentine.hsu@rad-ic.com>");
+MODULE_DESCRIPTION("Raydium touchscreen SPI bus driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("spi:raydium-t007");
diff --git a/drivers/input/touchscreen/rmi4/Makefile b/drivers/input/touchscreen/rmi4/Makefile
new file mode 100644
index 000000000000..a7375ed07a39
--- /dev/null
+++ b/drivers/input/touchscreen/rmi4/Makefile
@@ -0,0 +1,18 @@
+#
+# Makefile for Synaptics Touchscreen (RMI4/SPI)
+#
+GCOV_PROFILE := y
+
+ccflags-$(CONFIG_SPI_DEBUG) := -DDEBUG
+
+#Synaptics SPI Sensor (2002)
+obj-$(CONFIG_TOUCHSCREEN_SYN_RMI4_SPI) += rmi_bus.o
+obj-$(CONFIG_TOUCHSCREEN_SYN_RMI4_SPI) += rmi_i2c.o
+obj-$(CONFIG_TOUCHSCREEN_SYN_RMI4_SPI) += rmi_spi.o
+obj-$(CONFIG_TOUCHSCREEN_SYN_RMI4_SPI) += rmi_driver.o rmi_f01.o
+obj-$(CONFIG_TOUCHSCREEN_SYN_RMI4_SPI) += rmi_f09.o
+obj-$(CONFIG_TOUCHSCREEN_SYN_RMI4_SPI) += rmi_f11.o
+obj-$(CONFIG_TOUCHSCREEN_SYN_RMI4_SPI) += rmi_f19.o
+obj-$(CONFIG_TOUCHSCREEN_SYN_RMI4_SPI) += rmi_f34.o
+obj-$(CONFIG_TOUCHSCREEN_SYN_RMI4_SPI) += rmi_f54.o
+obj-$(CONFIG_TOUCHSCREEN_SYN_RMI4_SPI) += rmi_dev.o
diff --git a/drivers/input/touchscreen/rmi4/rmi_bus.c b/drivers/input/touchscreen/rmi4/rmi_bus.c
new file mode 100644
index 000000000000..def8a2083d98
--- /dev/null
+++ b/drivers/input/touchscreen/rmi4/rmi_bus.c
@@ -0,0 +1,313 @@
+/*
+ * Copyright (c) 2011 Synaptics Incorporated
+ * Copyright (c) 2011 Unixphere
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/pm.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/rmi.h>
+
+static struct rmi_function_list {
+	struct list_head list;
+	struct rmi_function_handler *fh;
+} rmi_supported_functions;
+
+static int rmi_bus_match(struct device *dev, struct device_driver *driver)
+{
+	struct rmi_driver *rmi_driver;
+	struct rmi_device *rmi_dev;
+	struct rmi_device_platform_data *pdata;
+
+	pr_info("in function ____%s____  \n", __func__);
+	rmi_driver = to_rmi_driver(driver);
+	rmi_dev = to_rmi_device(dev);
+	pdata = to_rmi_platform_data(rmi_dev);
+
+	pr_info("rmi_driver->driver.name =  %s\n", rmi_driver->driver.name);
+	pr_info("device:rmi_device:rmi_device_platform_data:driver_name = %s\n",
+		pdata->driver_name);
+
+	if (!strcmp(pdata->driver_name, rmi_driver->driver.name)) {
+		rmi_dev->driver = rmi_driver;
+		return 1;
+	}
+	pr_info("names DO NOT match, so return nothing\n");
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int rmi_bus_suspend(struct device *dev)
+{
+#ifdef GENERIC_SUBSYS_PM_OPS
+	const struct dev_pm_ops *pm = dev->driver ? dev->driver->pm : NULL;
+
+	if (pm && pm->suspend)
+		return pm->suspend(dev);
+#endif
+
+	return 0;
+}
+
+static int rmi_bus_resume(struct device *dev)
+{
+#ifdef GENERIC_SUBSYS_PM_OPS
+	const struct dev_pm_ops *pm = dev->driver ? dev->driver->pm : NULL;
+	pr_info("in function ____%s____  \n", __func__);
+
+	if (pm && pm->resume)
+		return pm->resume(dev);
+#endif
+
+	return 0;
+}
+#endif
+
+static int rmi_bus_probe(struct device *dev)
+{
+	struct rmi_driver *driver;
+	struct rmi_device *rmi_dev = to_rmi_device(dev);
+
+	pr_info("in function ____%s____  \n", __func__);
+	driver = rmi_dev->driver;
+	if (driver && driver->probe)
+		return driver->probe(rmi_dev);
+
+	return 0;
+}
+
+static int rmi_bus_remove(struct device *dev)
+{
+	struct rmi_driver *driver;
+	struct rmi_device *rmi_dev = to_rmi_device(dev);
+
+	pr_info("in function ____%s____  \n", __func__);
+	driver = rmi_dev->driver;
+	if (driver && driver->remove)
+		return driver->remove(rmi_dev);
+
+	return 0;
+}
+
+static void rmi_bus_shutdown(struct device *dev)
+{
+	struct rmi_driver *driver;
+	struct rmi_device *rmi_dev = to_rmi_device(dev);
+
+	driver = rmi_dev->driver;
+	if (driver && driver->shutdown)
+		driver->shutdown(rmi_dev);
+}
+
+static SIMPLE_DEV_PM_OPS(rmi_bus_pm_ops,
+			 rmi_bus_suspend, rmi_bus_resume);
+
+struct bus_type rmi_bus_type = {
+	.name		= "rmi",
+	.match		= rmi_bus_match,
+	.probe		= rmi_bus_probe,
+	.remove		= rmi_bus_remove,
+	.shutdown	= rmi_bus_shutdown,
+	.pm		= &rmi_bus_pm_ops
+};
+
+int rmi_register_phys_device(struct rmi_phys_device *phys)
+{
+	static int phys_device_num;
+	struct rmi_device_platform_data *pdata = phys->dev->platform_data;
+	struct rmi_device *rmi_dev;
+
+	pr_info("in function ____%s____  \n", __func__);
+
+	if (!pdata) {
+		dev_err(phys->dev, "no platform data!\n");
+		return -EINVAL;
+	}
+
+	rmi_dev = kzalloc(sizeof(struct rmi_device), GFP_KERNEL);
+	if (!rmi_dev)
+		return -ENOMEM;
+
+	rmi_dev->phys = phys;
+	rmi_dev->dev.bus = &rmi_bus_type;
+	dev_set_name(&rmi_dev->dev, "sensor%02d", phys_device_num++);
+
+	phys->rmi_dev = rmi_dev;
+	pr_info("registering physical device:\n");
+	pr_info("dev.init_name = %s\n", rmi_dev->dev.init_name);
+	pr_info("dev.bus->name = %s\n", rmi_dev->dev.bus->name);
+	return device_register(&rmi_dev->dev);
+}
+EXPORT_SYMBOL(rmi_register_phys_device);
+
+void rmi_unregister_phys_device(struct rmi_phys_device *phys)
+{
+	struct rmi_device *rmi_dev = phys->rmi_dev;
+	pr_info("in function ____%s____  \n", __func__);
+
+	device_unregister(&rmi_dev->dev);
+	kfree(rmi_dev);
+}
+EXPORT_SYMBOL(rmi_unregister_phys_device);
+
+int rmi_register_driver(struct rmi_driver *driver)
+{
+	pr_info("in function ____%s____  \n", __func__);
+	driver->driver.bus = &rmi_bus_type;
+	return driver_register(&driver->driver);
+}
+EXPORT_SYMBOL(rmi_register_driver);
+
+static int __rmi_driver_remove(struct device *dev, void *data)
+{
+	struct rmi_driver *driver = data;
+	struct rmi_device *rmi_dev = to_rmi_device(dev);
+
+	if (rmi_dev->driver == driver)
+		rmi_dev->driver = NULL;
+
+	return 0;
+}
+
+void rmi_unregister_driver(struct rmi_driver *driver)
+{
+	bus_for_each_dev(&rmi_bus_type, NULL, driver, __rmi_driver_remove);
+	driver_unregister(&driver->driver);
+}
+EXPORT_SYMBOL(rmi_unregister_driver);
+
+static int __rmi_bus_fh_add(struct device *dev, void *data)
+{
+	struct rmi_driver *driver;
+	struct rmi_device *rmi_dev = to_rmi_device(dev);
+	pr_info("in function ____%s____  \n", __func__);
+
+	driver = rmi_dev->driver;
+	if (driver && driver->fh_add)
+		driver->fh_add(rmi_dev, data);
+
+	return 0;
+}
+
+int rmi_register_function_driver(struct rmi_function_handler *fh)
+{
+	struct rmi_function_list *entry;
+	struct rmi_function_handler *fh_dup;
+
+	fh_dup = rmi_get_function_handler(fh->func);
+	if (fh_dup) {
+		pr_err("%s: function f%.2x already registered!\n", __func__,
+			fh->func);
+		return -EINVAL;
+	}
+
+	entry = kzalloc(sizeof(struct rmi_function_list), GFP_KERNEL);
+	if (!entry)
+		return -ENOMEM;
+
+	entry->fh = fh;
+	list_add_tail(&entry->list, &rmi_supported_functions.list);
+
+	/* notify devices of the new function handler */
+	bus_for_each_dev(&rmi_bus_type, NULL, fh, __rmi_bus_fh_add);
+
+	return 0;
+}
+EXPORT_SYMBOL(rmi_register_function_driver);
+
+static int __rmi_bus_fh_remove(struct device *dev, void *data)
+{
+	struct rmi_driver *driver;
+	struct rmi_device *rmi_dev = to_rmi_device(dev);
+
+	pr_info("in function ____%s____  \n", __func__);
+	driver = rmi_dev->driver;
+	if (driver && driver->fh_remove)
+		driver->fh_remove(rmi_dev, data);
+
+	return 0;
+}
+
+void rmi_unregister_function_driver(struct rmi_function_handler *fh)
+{
+	struct rmi_function_list *entry, *n;
+	pr_info("in function ____%s____  \n", __func__);
+
+	/* notify devices of the removal of the function handler */
+	bus_for_each_dev(&rmi_bus_type, NULL, fh, __rmi_bus_fh_remove);
+
+	list_for_each_entry_safe(entry, n, &rmi_supported_functions.list, list)
+		if (entry->fh->func == fh->func) {
+			list_del(&entry->list);
+			kfree(entry);
+		}
+}
+EXPORT_SYMBOL(rmi_unregister_function_driver);
+
+struct rmi_function_handler *rmi_get_function_handler(int id)
+{
+	struct rmi_function_list *entry;
+	pr_info("in function ____%s____  \n", __func__);
+
+	list_for_each_entry(entry, &rmi_supported_functions.list, list)
+		if (entry->fh->func == id)
+			return entry->fh;
+
+	return NULL;
+}
+EXPORT_SYMBOL(rmi_get_function_handler);
+
+static int __init rmi_bus_init(void)
+{
+	int error;
+
+	pr_info("in function ____%s____  \n", __func__);
+	INIT_LIST_HEAD(&rmi_supported_functions.list);
+
+	error = bus_register(&rmi_bus_type);
+	if (error < 0) {
+		pr_err("%s: error registering the RMI bus: %d\n", __func__,
+		       error);
+		return error;
+	}
+	pr_info("%s: successfully registered RMI bus.\n", __func__);
+
+	return 0;
+}
+
+static void __exit rmi_bus_exit(void)
+{
+	struct rmi_function_list *entry, *n;
+	pr_info("in function ____%s____  \n", __func__);
+
+	list_for_each_entry_safe(entry, n, &rmi_supported_functions.list,
+				 list) {
+		list_del(&entry->list);
+		kfree(entry);
+	}
+
+	bus_unregister(&rmi_bus_type);
+}
+
+module_init(rmi_bus_init);
+module_exit(rmi_bus_exit);
+
+MODULE_AUTHOR("Eric Andersson <eric.andersson@unixphere.com>");
+MODULE_DESCRIPTION("RMI bus");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/rmi4/rmi_dev.c b/drivers/input/touchscreen/rmi4/rmi_dev.c
new file mode 100644
index 000000000000..fa21ef9b20fa
--- /dev/null
+++ b/drivers/input/touchscreen/rmi4/rmi_dev.c
@@ -0,0 +1,428 @@
+/*
+ * Copyright (c) 2011 Synaptics Incorporated
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/fs.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/syscalls.h>
+
+#include <linux/rmi.h>
+#include "rmi_driver.h"
+
+#define CHAR_DEVICE_NAME "rmi"
+
+#define REG_ADDR_LIMIT 0xFFFF
+
+/*store dynamically allocated major number of char device*/
+static int rmi_char_dev_major_num;
+
+
+/* file operations for RMI char device */
+
+/*
+ * rmi_char_dev_llseek: - use to setup register address
+ *
+ * @filp: file structure for seek
+ * @off: offset
+ *       if whence == SEEK_SET,
+ *       high 16 bits: page address
+ *       low 16 bits: register address
+ *
+ *       if whence == SEEK_CUR,
+ *       offset from current position
+ *
+ *       if whence == SEEK_END,
+ *       offset from END(0xFFFF)
+ *
+ * @whence: SEEK_SET , SEEK_CUR or SEEK_END
+ */
+static loff_t rmi_char_dev_llseek(struct file *filp, loff_t off, int whence)
+{
+	loff_t newpos;
+	struct rmi_char_dev *my_char_dev = filp->private_data;
+
+	if (IS_ERR(my_char_dev)) {
+		pr_err("%s: pointer of char device is invalid", __func__);
+		return -EBADF;
+	}
+
+	mutex_lock(&(my_char_dev->mutex_file_op));
+
+	switch (whence) {
+	case SEEK_SET:
+		newpos = off;
+		break;
+
+	case SEEK_CUR:
+		newpos = filp->f_pos + off;
+		break;
+
+	case SEEK_END:
+		newpos = REG_ADDR_LIMIT + off;
+		break;
+
+	default:		/* can't happen */
+		newpos = -EINVAL;
+		goto clean_up;
+	}
+
+	if (newpos < 0 || newpos > REG_ADDR_LIMIT) {
+		dev_err(my_char_dev->phys->dev, "newpos 0x%04x is invalid.\n",
+			(unsigned int)newpos);
+		newpos = -EINVAL;
+		goto clean_up;
+	}
+
+	filp->f_pos = newpos;
+
+clean_up:
+	mutex_unlock(&(my_char_dev->mutex_file_op));
+	return newpos;
+}
+
+/*
+ *  rmi_char_dev_read: - use to read data from RMI stream
+ *
+ *  @filp: file structure for read
+ *  @buf: user-level buffer pointer
+ *
+ *  @count: number of byte read
+ *  @f_pos: offset (starting register address)
+ *
+ *	@return number of bytes read into user buffer (buf) if succeeds
+ *          negative number if error occurs.
+ */
+static ssize_t rmi_char_dev_read(struct file *filp, char __user *buf,
+		size_t count, loff_t *f_pos)
+{
+	struct rmi_char_dev *my_char_dev = filp->private_data;
+	ssize_t ret_value  = 0;
+	unsigned char tmpbuf[count+1];
+	struct rmi_phys_device *phys;
+
+	/* limit offset to REG_ADDR_LIMIT-1 */
+	if (count > (REG_ADDR_LIMIT - *f_pos))
+		count = REG_ADDR_LIMIT - *f_pos;
+
+	if (count == 0)
+		return 0;
+
+	if (IS_ERR(my_char_dev)) {
+		pr_err("%s: pointer of char device is invalid", __func__);
+		ret_value = -EBADF;
+		return ret_value;
+	}
+
+	mutex_lock(&(my_char_dev->mutex_file_op));
+
+	phys = my_char_dev->phys;
+	/*
+	 * just let it go through , because we do not know the register is FIFO
+	 * register or not
+	 */
+
+	ret_value = phys->read_block(phys, *f_pos, tmpbuf, count);
+
+	if (ret_value < 0)
+		goto clean_up;
+	else
+		*f_pos += ret_value;
+
+	if (copy_to_user(buf, tmpbuf, count))
+		ret_value = -EFAULT;
+
+clean_up:
+
+	mutex_unlock(&(my_char_dev->mutex_file_op));
+
+	return ret_value;
+}
+
+/*
+ * rmi_char_dev_write: - use to write data into RMI stream
+ *
+ * @filep : file structure for write
+ * @buf: user-level buffer pointer contains data to be written
+ * @count: number of byte be be written
+ * @f_pos: offset (starting register address)
+ *
+ * @return number of bytes written from user buffer (buf) if succeeds
+ *         negative number if error occurs.
+ */
+static ssize_t rmi_char_dev_write(struct file *filp, const char __user *buf,
+		size_t count, loff_t *f_pos)
+{
+	struct rmi_char_dev *my_char_dev = filp->private_data;
+	ssize_t ret_value  = 0;
+	unsigned char tmpbuf[count+1];
+	struct rmi_phys_device *phys;
+
+	/* limit offset to REG_ADDR_LIMIT-1 */
+	if (count > (REG_ADDR_LIMIT - *f_pos))
+		count = REG_ADDR_LIMIT - *f_pos;
+
+	if (count == 0)
+		return 0;
+
+	if (IS_ERR(my_char_dev)) {
+		pr_err("%s: pointer of char device is invalid", __func__);
+		ret_value = -EBADF;
+		return ret_value;
+	}
+
+	if (copy_from_user(tmpbuf, buf, count)) {
+		ret_value = -EFAULT;
+		return ret_value;
+	}
+
+	mutex_lock(&(my_char_dev->mutex_file_op));
+
+	phys = my_char_dev->phys;
+	/*
+	 * just let it go through , because we do not know the register is FIFO
+	 * register or not
+	 */
+
+	ret_value = phys->write_block(phys, *f_pos, tmpbuf, count);
+
+	if (ret_value >= 0)
+		*f_pos += count;
+
+	mutex_unlock(&(my_char_dev->mutex_file_op));
+
+	return ret_value;
+}
+
+/*
+ * rmi_char_dev_open: - get a new handle for from RMI stream
+ * @inp : inode struture
+ * @filp: file structure for read/write
+ *
+ * @return 0 if succeeds
+ */
+static int rmi_char_dev_open(struct inode *inp, struct file *filp)
+{
+	/* store the device pointer to file structure */
+	struct rmi_char_dev *my_dev = container_of(inp->i_cdev,
+			struct rmi_char_dev, main_dev);
+	struct rmi_phys_device *phys = my_dev->phys;
+	int ret_value = 0;
+
+	filp->private_data = my_dev;
+
+	if (!phys)
+		return -EACCES;
+
+	mutex_lock(&(my_dev->mutex_file_op));
+	if (my_dev->ref_count < 1)
+		my_dev->ref_count++;
+	else
+		ret_value = -EACCES;
+
+	mutex_unlock(&(my_dev->mutex_file_op));
+
+	return ret_value;
+}
+
+/*
+ *  rmi_char_dev_release: - release an existing handle
+ *  @inp: inode structure
+ *  @filp: file structure for read/write
+ *
+ *  @return 0 if succeeds
+ */
+static int rmi_char_dev_release(struct inode *inp, struct file *filp)
+{
+	struct rmi_char_dev *my_dev = container_of(inp->i_cdev,
+			struct rmi_char_dev, main_dev);
+	struct rmi_phys_device *phys = my_dev->phys;
+
+	if (!phys)
+		return -EACCES;
+
+	mutex_lock(&(my_dev->mutex_file_op));
+
+	my_dev->ref_count--;
+	if (my_dev->ref_count < 0)
+		my_dev->ref_count = 0;
+
+	mutex_unlock(&(my_dev->mutex_file_op));
+
+	return 0;
+}
+
+static const struct file_operations rmi_char_dev_fops = {
+	.owner =    THIS_MODULE,
+	.llseek =   rmi_char_dev_llseek,
+	.read =     rmi_char_dev_read,
+	.write =    rmi_char_dev_write,
+	.open =     rmi_char_dev_open,
+	.release =  rmi_char_dev_release,
+};
+
+/*
+ * rmi_char_dev_clean_up - release memory or unregister driver
+ * @rmi_char_dev: rmi_char_dev structure
+ *
+ */
+static void rmi_char_dev_clean_up(struct rmi_char_dev *char_dev,
+			struct class *char_device_class)
+{
+	dev_t devno;
+
+	/* Get rid of our char dev entries */
+	if (char_dev) {
+		devno = char_dev->main_dev.dev;
+
+		cdev_del(&char_dev->main_dev);
+		kfree(char_dev);
+
+		if (char_device_class) {
+			device_destroy(char_device_class, devno);
+			class_unregister(char_device_class);
+			class_destroy(char_device_class);
+		}
+
+		/* cleanup_module is never called if registering failed */
+		unregister_chrdev_region(devno, 1);
+		pr_debug("%s: rmi_char_dev is removed\n", __func__);
+	}
+}
+
+/*
+ * rmi_char_devnode - return device permission
+ *
+ * @dev: char device structure
+ * @mode: file permission
+ *
+ */
+static char *rmi_char_devnode(struct device *dev, mode_t *mode)
+{
+	if (!mode)
+		return NULL;
+	/* rmi** */
+	/**mode = 0666*/
+	*mode = (S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH|S_IWOTH);
+
+	return kasprintf(GFP_KERNEL, "rmi/%s", dev_name(dev));
+}
+
+/*
+ * rmi_char_dev_register - register char device (called from up-level)
+ *
+ * @phy: a pointer to an rmi_phys_devices structure
+ *
+ * @return: zero if suceeds
+ */
+int rmi_char_dev_register(struct rmi_phys_device *phys)
+{
+	struct rmi_char_dev *char_dev;
+	dev_t dev_no;
+	int err;
+	int result;
+	struct device *device_ptr;
+
+	if (rmi_char_dev_major_num) {
+		dev_no = MKDEV(rmi_char_dev_major_num, 0);
+		result = register_chrdev_region(dev_no, 1, CHAR_DEVICE_NAME);
+	} else {
+		result = alloc_chrdev_region(&dev_no, 0, 1, CHAR_DEVICE_NAME);
+		/* let kernel allocate a major for us */
+		rmi_char_dev_major_num = MAJOR(dev_no);
+		dev_info(phys->dev, "Major number of rmi_char_dev: %d\n",
+				 rmi_char_dev_major_num);
+	}
+	if (result < 0)
+		return result;
+
+	char_dev = kzalloc(sizeof(struct rmi_char_dev), GFP_KERNEL);
+	if (!char_dev) {
+		dev_err(phys->dev, "Failed to allocate rmi_char_dev.\n");
+		/* unregister the char device region */
+		__unregister_chrdev(rmi_char_dev_major_num, MINOR(dev_no), 1,
+				CHAR_DEVICE_NAME);
+		return -ENOMEM;
+	}
+
+	mutex_init(&char_dev->mutex_file_op);
+
+	phys->char_dev = char_dev;
+	char_dev->phys = phys;
+
+	cdev_init(&char_dev->main_dev, &rmi_char_dev_fops);
+
+	err = cdev_add(&char_dev->main_dev, dev_no, 1);
+	if (err) {
+		dev_err(phys->dev, "Error %d adding rmi_char_dev.\n", err);
+		rmi_char_dev_clean_up(phys->char_dev,
+				 phys->rmi_char_device_class);
+		return err;
+	}
+
+	/* create device node */
+	phys->rmi_char_device_class =
+		class_create(THIS_MODULE, CHAR_DEVICE_NAME);
+
+	if (IS_ERR(phys->rmi_char_device_class)) {
+		dev_err(phys->dev, "Failed to create /dev/%s.\n",
+			CHAR_DEVICE_NAME);
+		rmi_char_dev_clean_up(phys->char_dev,
+				 phys->rmi_char_device_class);
+		return -ENODEV;
+	}
+	/* setup permission */
+	phys->rmi_char_device_class->devnode = rmi_char_devnode;
+
+	/* class creation */
+	device_ptr = device_create(
+			phys->rmi_char_device_class,
+			NULL, dev_no, NULL,
+			CHAR_DEVICE_NAME"%d",
+			MINOR(dev_no));
+
+	if (IS_ERR(device_ptr)) {
+		dev_err(phys->dev, "Failed to create rmi device.\n");
+		rmi_char_dev_clean_up(phys->char_dev,
+				 phys->rmi_char_device_class);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(rmi_char_dev_register);
+
+/* rmi_char_dev_unregister - unregister char device (called from up-level)
+ *
+ * @phys: pointer to an rmi_phys_device structure
+ */
+
+void rmi_char_dev_unregister(struct rmi_phys_device *phys)
+{
+	/* clean up */
+	if (phys)
+		rmi_char_dev_clean_up(phys->char_dev,
+				 phys->rmi_char_device_class);
+}
+EXPORT_SYMBOL(rmi_char_dev_unregister);
+
+MODULE_AUTHOR("Synaptics, Inc.");
+MODULE_DESCRIPTION("RMI4 Char Device");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/rmi4/rmi_driver.c b/drivers/input/touchscreen/rmi4/rmi_driver.c
new file mode 100644
index 000000000000..78b30095b086
--- /dev/null
+++ b/drivers/input/touchscreen/rmi4/rmi_driver.c
@@ -0,0 +1,1354 @@
+/*
+ * Copyright (c) 2011 Synaptics Incorporated
+ * Copyright (c) 2011 Unixphere
+ *
+ * This driver adds support for generic RMI4 devices from Synpatics. It
+ * implements the mandatory f01 RMI register and depends on the presence of
+ * other required RMI functions.
+ *
+ * The RMI4 specification can be found here (URL split after files/ for
+ * style reasons):
+ * http://www.synaptics.com/sites/default/files/
+ *           511-000136-01-Rev-E-RMI4%20Intrfacing%20Guide.pdf
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/pm.h>
+#include <linux/gpio.h>
+#include <linux/workqueue.h>
+#include <linux/rmi.h>
+#include "rmi_driver.h"
+
+#define DELAY_DEBUG 0
+#define REGISTER_DEBUG 0
+
+#define PDT_END_SCAN_LOCATION	0x0005
+#define PDT_PROPERTIES_LOCATION 0x00EF
+#define BSR_LOCATION 0x00FE
+#define HAS_BSR_MASK 0x20
+#define HAS_NONSTANDARD_PDT_MASK 0x40
+#define RMI4_END_OF_PDT(id) ((id) == 0x00 || (id) == 0xff)
+#define RMI4_MAX_PAGE 0xff
+#define RMI4_PAGE_SIZE 0x100
+
+#define RMI_DEVICE_RESET_CMD	0x01
+#define INITIAL_RESET_WAIT_MS	20
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void rmi_driver_early_suspend(struct early_suspend *h);
+static void rmi_driver_late_resume(struct early_suspend *h);
+#endif
+
+
+/* sysfs files for attributes for driver values. */
+static ssize_t rmi_driver_hasbsr_show(struct device *dev,
+				      struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_driver_bsr_show(struct device *dev,
+				   struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_driver_bsr_store(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count);
+
+static ssize_t rmi_driver_enabled_show(struct device *dev,
+				       struct device_attribute *attr,
+				       char *buf);
+
+static ssize_t rmi_driver_enabled_store(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count);
+
+static ssize_t rmi_driver_phys_show(struct device *dev,
+				       struct device_attribute *attr,
+				       char *buf);
+
+static ssize_t rmi_driver_version_show(struct device *dev,
+				       struct device_attribute *attr,
+				       char *buf);
+
+#if REGISTER_DEBUG
+static ssize_t rmi_driver_reg_store(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count);
+#endif
+
+#if DELAY_DEBUG
+static ssize_t rmi_delay_show(struct device *dev,
+				       struct device_attribute *attr,
+				       char *buf);
+
+static ssize_t rmi_delay_store(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count);
+#endif
+
+static struct device_attribute attrs[] = {
+	__ATTR(hasbsr, RMI_RO_ATTR,
+	       rmi_driver_hasbsr_show, rmi_store_error),
+	__ATTR(bsr, RMI_RO_ATTR,
+	       rmi_driver_bsr_show, rmi_driver_bsr_store),
+	__ATTR(enabled, RMI_RO_ATTR,
+	       rmi_driver_enabled_show, rmi_driver_enabled_store),
+	__ATTR(phys, RMI_RO_ATTR,
+	       rmi_driver_phys_show, rmi_store_error),
+#if REGISTER_DEBUG
+	__ATTR(reg, RMI_WO_ATTR,
+	       rmi_show_error, rmi_driver_reg_store),
+#endif
+#if DELAY_DEBUG
+	__ATTR(delay, RMI_RW_ATTR,
+	       rmi_delay_show, rmi_delay_store),
+#endif
+	__ATTR(version, RMI_RO_ATTR,
+	       rmi_driver_version_show, rmi_store_error),
+};
+
+
+/*
+** ONLY needed for POLLING mode of the driver
+*/
+struct rmi_device *polled_synaptics_rmi_device = NULL;
+EXPORT_SYMBOL(polled_synaptics_rmi_device);
+
+/* Useful helper functions for u8* */
+
+void u8_set_bit(u8 *target, int pos)
+{
+	target[pos/8] |= 1<<pos%8;
+}
+
+void u8_clear_bit(u8 *target, int pos)
+{
+	target[pos/8] &= ~(1<<pos%8);
+}
+
+bool u8_is_set(u8 *target, int pos)
+{
+	return target[pos/8] & 1<<pos%8;
+}
+
+bool u8_is_any_set(u8 *target, int size)
+{
+	int i;
+	for (i = 0; i < size; i++) {
+		if (target[i])
+			return true;
+	}
+	return false;
+}
+
+void u8_or(u8 *dest, u8 *target1, u8 *target2, int size)
+{
+	int i;
+	for (i = 0; i < size; i++)
+		dest[i] = target1[i] | target2[i];
+}
+
+void u8_and(u8 *dest, u8 *target1, u8 *target2, int size)
+{
+	int i;
+	for (i = 0; i < size; i++)
+		dest[i] = target1[i] & target2[i];
+}
+
+/* Helper fn to convert a byte array representing a short in the RMI
+ * endian-ness to a short in the native processor's specific endianness.
+ * We don't use ntohs/htons here because, well, we're not dealing with
+ * a pair of shorts. And casting dest to short* wouldn't work, because
+ * that would imply knowing the byte order of short in the first place.
+ */
+void batohs(unsigned short *dest, unsigned char *src)
+{
+	*dest = src[1] * 0x100 + src[0];
+}
+
+/* Helper function to convert a short (in host processor endianess) to
+ * a byte array in the RMI endianess for shorts.  See above comment for
+ * why we dont us htons or something like that.
+ */
+void hstoba(unsigned char *dest, unsigned short src)
+{
+	dest[0] = src % 0x100;
+	dest[1] = src / 0x100;
+}
+
+static bool has_bsr(struct rmi_driver_data *data)
+{
+	return (data->pdt_props & HAS_BSR_MASK) != 0;
+}
+
+/* Utility routine to set bits in a register. */
+int rmi_set_bits(struct rmi_device *rmi_dev, unsigned short address,
+		 unsigned char bits)
+{
+	unsigned char reg_contents;
+	int retval;
+
+	retval = rmi_read_block(rmi_dev, address, &reg_contents, 1);
+	if (retval)
+		return retval;
+	reg_contents = reg_contents | bits;
+	retval = rmi_write_block(rmi_dev, address, &reg_contents, 1);
+	if (retval == 1)
+		return 0;
+	else if (retval == 0)
+		return -EIO;
+	return retval;
+}
+EXPORT_SYMBOL(rmi_set_bits);
+
+/* Utility routine to clear bits in a register. */
+int rmi_clear_bits(struct rmi_device *rmi_dev, unsigned short address,
+		   unsigned char bits)
+{
+	unsigned char reg_contents;
+	int retval;
+
+	retval = rmi_read_block(rmi_dev, address, &reg_contents, 1);
+	if (retval)
+		return retval;
+	reg_contents = reg_contents & ~bits;
+	retval = rmi_write_block(rmi_dev, address, &reg_contents, 1);
+	if (retval == 1)
+		return 0;
+	else if (retval == 0)
+		return -EIO;
+	return retval;
+}
+EXPORT_SYMBOL(rmi_clear_bits);
+
+static void rmi_free_function_list(struct rmi_device *rmi_dev)
+{
+	struct rmi_function_container *entry, *n;
+	struct rmi_driver_data *data = rmi_get_driverdata(rmi_dev);
+
+	list_for_each_entry_safe(entry, n, &data->rmi_functions.list, list) {
+		kfree(entry->irq_mask);
+		list_del(&entry->list);
+	}
+}
+
+static int init_one_function(struct rmi_device *rmi_dev,
+			     struct rmi_function_container *fc)
+{
+	int retval;
+
+	dev_info(&rmi_dev->dev, "Initializing F%02X.\n",
+			fc->fd.function_number);
+	dev_dbg(&rmi_dev->dev, "Initializing F%02X.\n",
+			fc->fd.function_number);
+
+	if (!fc->fh) {
+		struct rmi_function_handler *fh =
+			rmi_get_function_handler(fc->fd.function_number);
+		if (!fh) {
+			dev_dbg(&rmi_dev->dev, "No handler for F%02X.\n",
+				fc->fd.function_number);
+			return 0;
+		}
+		fc->fh = fh;
+	}
+
+	if (!fc->fh->init)
+		return 0;
+
+	dev_set_name(&(fc->dev), "fn%02x", fc->fd.function_number);
+
+	fc->dev.parent = &rmi_dev->dev;
+
+	retval = device_register(&fc->dev);
+	if (retval) {
+		dev_err(&rmi_dev->dev, "Failed device_register for F%02X.\n",
+			fc->fd.function_number);
+		return retval;
+	}
+
+	retval = fc->fh->init(fc);
+	if (retval < 0) {
+		dev_err(&rmi_dev->dev, "Failed to initialize function F%02x\n",
+			fc->fd.function_number);
+		goto error_exit;
+	}
+
+	return 0;
+
+error_exit:
+	device_unregister(&fc->dev);
+	return retval;
+}
+
+static void rmi_driver_fh_add(struct rmi_device *rmi_dev,
+			      struct rmi_function_handler *fh)
+{
+	struct rmi_function_container *entry;
+	struct rmi_driver_data *data = rmi_get_driverdata(rmi_dev);
+
+	if (fh->func == 0x01)
+		data->f01_container->fh = fh;
+	else {
+		list_for_each_entry(entry, &data->rmi_functions.list, list)
+			if (entry->fd.function_number == fh->func) {
+				entry->fh = fh;
+				init_one_function(rmi_dev, entry);
+			}
+	}
+
+}
+
+static void rmi_driver_fh_remove(struct rmi_device *rmi_dev,
+				 struct rmi_function_handler *fh)
+{
+	struct rmi_function_container *entry;
+	struct rmi_driver_data *data = rmi_get_driverdata(rmi_dev);
+
+	list_for_each_entry(entry, &data->rmi_functions.list, list)
+		if (entry->fd.function_number == fh->func) {
+			if (fh->remove)
+				fh->remove(entry);
+
+			entry->fh = NULL;
+		}
+}
+
+static void construct_mask(u8 *mask, int num, int pos)
+{
+	int i;
+
+	for (i = 0; i < num; i++)
+		u8_set_bit(mask, pos+i);
+}
+
+static int process_interrupt_requests(struct rmi_device *rmi_dev)
+{
+	struct rmi_driver_data *data = rmi_get_driverdata(rmi_dev);
+	struct device *dev = &rmi_dev->dev;
+	struct rmi_function_container *entry;
+	u8 irq_status[data->num_of_irq_regs];
+	u8 irq_bits[data->num_of_irq_regs];
+	int error;
+
+	error = rmi_read_block(rmi_dev,
+				data->f01_container->fd.data_base_addr + 1,
+				irq_status, data->num_of_irq_regs);
+	if (error < 0) {
+		dev_err(dev, "%s: failed to read irqs.", __func__);
+		return error;
+	}
+	/* Device control (F01) is handled before anything else. */
+	u8_and(irq_bits, irq_status, data->f01_container->irq_mask,
+			data->num_of_irq_regs);
+	if (u8_is_any_set(irq_bits, data->num_of_irq_regs))
+		data->f01_container->fh->attention(
+				data->f01_container, irq_bits);
+
+	//dev_info(dev, "  irq_status = 0x%2x data->current_irq_mask = 0x%2x data->num_of_irq_regs = %d\n",
+	//	 irq_status[0], data->current_irq_mask[0], data->num_of_irq_regs );
+
+
+	u8_and(irq_status, irq_status, data->current_irq_mask,
+	       data->num_of_irq_regs);
+
+	/* At this point, irq_status has all bits that are set in the
+	 * interrupt status register and are enabled.
+	 */
+
+	list_for_each_entry(entry, &data->rmi_functions.list, list){
+		if (entry->irq_mask && entry->fh && entry->fh->attention) {
+
+			u8_and(irq_bits, irq_status, entry->irq_mask,
+			       data->num_of_irq_regs);
+			if (u8_is_any_set(irq_bits, data->num_of_irq_regs)) {
+				error = entry->fh->attention(entry, irq_bits);
+				if (error < 0)
+					dev_err(dev, "%s: f%.2x"
+						" attention handler failed:"
+						" %d\n", __func__,
+						entry->fh->func, error);
+			}
+		}
+	}
+	return 0;
+}
+
+
+static int rmi_driver_irq_handler(struct rmi_device *rmi_dev, int irq)
+{
+	struct rmi_driver_data *data = rmi_get_driverdata(rmi_dev);
+
+	/* Can get called before the driver is fully ready to deal with
+	 * interrupts.
+	 */
+	if (!data || !data->f01_container || !data->f01_container->fh) {
+		dev_warn(&rmi_dev->dev,
+			 "Not ready to handle interrupts yet!\n");
+		return 0;
+	}
+
+	return process_interrupt_requests(rmi_dev);
+}
+
+/*
+ * Construct a function's IRQ mask. This should
+ * be called once and stored.
+ */
+static u8 *rmi_driver_irq_get_mask(struct rmi_device *rmi_dev,
+				   struct rmi_function_container *fc) {
+	struct rmi_driver_data *data = rmi_get_driverdata(rmi_dev);
+
+	/* TODO: Where should this be freed? */
+	u8 *irq_mask = kzalloc(sizeof(u8) * data->num_of_irq_regs, GFP_KERNEL);
+	if (irq_mask)
+		construct_mask(irq_mask, fc->num_of_irqs, fc->irq_pos);
+
+	return irq_mask;
+}
+
+/*
+ * This pair of functions allows functions like function 54 to request to have
+ * other interupts disabled until the restore function is called. Only one store
+ * happens at a time.
+ */
+static int rmi_driver_irq_save(struct rmi_device *rmi_dev, u8 * new_ints)
+{
+	int retval = 0;
+	struct rmi_driver_data *data = rmi_get_driverdata(rmi_dev);
+	struct device *dev = &rmi_dev->dev;
+
+	mutex_lock(&data->irq_mutex);
+	if (!data->irq_stored) {
+		/* Save current enabled interupts */
+		retval = rmi_read_block(rmi_dev,
+				data->f01_container->fd.control_base_addr+1,
+				data->irq_mask_store, data->num_of_irq_regs);
+		if (retval < 0) {
+			dev_err(dev, "%s: Failed to read enabled interrupts!",
+								__func__);
+			goto error_unlock;
+		}
+		/*
+		 * Disable every interupt except for function 54
+		 * TODO:Will also want to not disable function 1-like functions.
+		 * No need to take care of this now, since there's no good way
+		 * to identify them.
+		 */
+		retval = rmi_write_block(rmi_dev,
+				data->f01_container->fd.control_base_addr+1,
+				new_ints, data->num_of_irq_regs);
+		if (retval < 0) {
+			dev_err(dev, "%s: Failed to change enabled interrupts!",
+								__func__);
+			goto error_unlock;
+		}
+		memcpy(data->current_irq_mask, new_ints,
+					data->num_of_irq_regs * sizeof(u8));
+		data->irq_stored = true;
+	} else {
+		retval = -ENOSPC; /* No space to store IRQs.*/
+		dev_err(dev, "%s: Attempted to save values when"
+						" already stored!", __func__);
+	}
+
+error_unlock:
+	mutex_unlock(&data->irq_mutex);
+	return retval;
+}
+
+static int rmi_driver_irq_restore(struct rmi_device *rmi_dev)
+{
+	int retval = 0;
+	struct rmi_driver_data *data = rmi_get_driverdata(rmi_dev);
+	struct device *dev = &rmi_dev->dev;
+	mutex_lock(&data->irq_mutex);
+
+	if (data->irq_stored) {
+		retval = rmi_write_block(rmi_dev,
+				data->f01_container->fd.control_base_addr+1,
+				data->irq_mask_store, data->num_of_irq_regs);
+		if (retval < 0) {
+			dev_err(dev, "%s: Failed to write enabled interupts!",
+								__func__);
+			goto error_unlock;
+		}
+		memcpy(data->current_irq_mask, data->irq_mask_store,
+					data->num_of_irq_regs * sizeof(u8));
+		data->irq_stored = false;
+	} else {
+		retval = -EINVAL;
+		dev_err(dev, "%s: Attempted to restore values when not stored!",
+			__func__);
+	}
+
+error_unlock:
+	mutex_unlock(&data->irq_mutex);
+	return retval;
+}
+
+static int rmi_driver_fn_01_specific(struct rmi_device *rmi_dev,
+				     struct pdt_entry *pdt_ptr,
+				     int *current_irq_count,
+				     u16 page_start)
+{
+	struct rmi_driver_data *data = NULL;
+	struct rmi_function_container *fc = NULL;
+	int retval = 0;
+	struct device *dev = &rmi_dev->dev;
+	struct rmi_function_handler *fh =
+		rmi_get_function_handler(0x01);
+
+	data = rmi_get_driverdata(rmi_dev);
+
+	dev_info(dev, "%s: Found F01, initializing.\n", __func__);
+	if (!fh)
+		dev_dbg(dev, "%s: No function handler for F01?!", __func__);
+
+	fc = kzalloc(sizeof(struct rmi_function_container), GFP_KERNEL);
+	if (!fc) {
+		retval = -ENOMEM;
+		return retval;
+	}
+
+	copy_pdt_entry_to_fd(pdt_ptr, &fc->fd, page_start);
+	fc->num_of_irqs = pdt_ptr->interrupt_source_count;
+	fc->irq_pos = *current_irq_count;
+
+	*current_irq_count += fc->num_of_irqs;
+
+	fc->rmi_dev        = rmi_dev;
+	fc->dev.parent     = &fc->rmi_dev->dev;
+	fc->fh = fh;
+
+	dev_set_name(&(fc->dev), "fn%02x", fc->fd.function_number);
+
+	retval = device_register(&fc->dev);
+	if (retval) {
+		dev_err(dev, "%s: Failed device_register for F01.\n", __func__);
+		goto error_free_data;
+	}
+
+	data->f01_container = fc;
+
+	return retval;
+
+error_free_data:
+	kfree(fc);
+	return retval;
+}
+
+/*
+ * Scan the PDT for F01 so we can force a reset before anything else
+ * is done.  This forces the sensor into a known state, and also
+ * forces application of any pending updates from reflashing the
+ * firmware or configuration.  We have to do this before actually
+ * building the PDT because the reflash might cause various registers
+ * to move around.
+ */
+static int do_initial_reset(struct rmi_device* rmi_dev)
+{
+	struct pdt_entry pdt_entry;
+	int page;
+	struct device *dev = &rmi_dev->dev;
+	bool done = false;
+	int i;
+	int retval;
+
+	pr_info("in function ____%s____  \n", __func__);
+
+	polled_synaptics_rmi_device = rmi_dev;
+
+	for (page = 0; (page <= RMI4_MAX_PAGE) && !done; page++) {
+
+		u16 page_start = RMI4_PAGE_SIZE * page;
+		u16 pdt_start = page_start + PDT_START_SCAN_LOCATION;
+		u16 pdt_end = page_start + PDT_END_SCAN_LOCATION;
+		pr_info("               reading page = %d\n", page );
+		done = true;
+		for (i = pdt_start; i >= pdt_end; i -= sizeof(pdt_entry)) {
+
+		        pr_info("               reading PDT entry %3d (block %3d)\n",
+				i%sizeof(pdt_entry), i);
+
+		        retval = rmi_read_block(rmi_dev, i, (u8 *)&pdt_entry,
+					       sizeof(pdt_entry));
+			if (retval != sizeof(pdt_entry)) {
+				dev_err(dev, "Read PDT entry at 0x%04x"
+					"failed, code = %d.\n", i, retval);
+				return retval;
+			}
+
+			if (RMI4_END_OF_PDT(pdt_entry.function_number))
+				break;
+			done = false;
+
+			if (pdt_entry.function_number == 0x01) {
+				u16 cmd_addr = page_start +
+					pdt_entry.command_base_addr;
+				u8 cmd_buf = RMI_DEVICE_RESET_CMD;
+				retval = rmi_write_block(rmi_dev, cmd_addr,
+						&cmd_buf, 1);
+				if (retval < 0) {
+					dev_err(dev, "Initial reset failed. "
+						"Code = %d.\n", retval);
+					return retval;
+				}
+				mdelay(INITIAL_RESET_WAIT_MS);
+				return 0;
+			}
+		}
+	}
+
+	dev_warn(dev, "WARNING: Failed to find F01 for initial reset.\n");
+	return -ENODEV;
+}
+
+
+static int rmi_scan_pdt(struct rmi_device *rmi_dev)
+{
+	struct rmi_driver_data *data;
+	struct rmi_function_container *fc;
+	struct pdt_entry pdt_entry;
+	int page;
+	struct device *dev = &rmi_dev->dev;
+	int irq_count = 0;
+	bool done = false;
+	int i;
+	int retval;
+        pr_info("in function ____%s____  \n", __func__);
+        pr_info("   doing initial reset  \n");
+
+	retval = do_initial_reset(rmi_dev);
+        pr_info("   back in %s  \n", __func__);
+
+	if (retval)
+		dev_err(dev, "WARNING: Initial reset failed! Soldiering on.\n");
+
+	data = rmi_get_driverdata(rmi_dev);
+
+	INIT_LIST_HEAD(&data->rmi_functions.list);
+
+	/* parse the PDT */
+	for (page = 0; (page <= RMI4_MAX_PAGE) && !done; page++) {
+		u16 page_start = RMI4_PAGE_SIZE * page;
+		u16 pdt_start = page_start + PDT_START_SCAN_LOCATION;
+		u16 pdt_end = page_start + PDT_END_SCAN_LOCATION;
+
+		done = true;
+		for (i = pdt_start; i >= pdt_end; i -= sizeof(pdt_entry)) {
+			retval = rmi_read_block(rmi_dev, i, (u8 *)&pdt_entry,
+					       sizeof(pdt_entry));
+			if (retval != sizeof(pdt_entry)) {
+				dev_err(dev, "Read PDT entry at 0x%04x"
+					"failed.\n", i);
+				goto error_exit;
+			}
+
+			if (RMI4_END_OF_PDT(pdt_entry.function_number))
+				break;
+
+			dev_dbg(dev, "%s: Found F%.2X on page 0x%02X\n",
+				__func__, pdt_entry.function_number, page);
+			done = false;
+
+			/*
+			 * F01 is handled by rmi_driver. Hopefully we will get
+			 * rid of the special treatment of f01 at some point
+			 * in time.
+			 */
+			if (pdt_entry.function_number == 0x01) {
+				retval = rmi_driver_fn_01_specific(rmi_dev,
+						&pdt_entry, &irq_count,
+						page_start);
+				if (retval)
+					goto error_exit;
+				continue;
+			}
+
+			fc = kzalloc(sizeof(struct rmi_function_container),
+				     GFP_KERNEL);
+			if (!fc) {
+				dev_err(dev, "Failed to allocate function "
+					"container for F%02X.\n",
+					pdt_entry.function_number);
+				retval = -ENOMEM;
+				goto error_exit;
+			}
+
+			copy_pdt_entry_to_fd(&pdt_entry, &fc->fd, page_start);
+
+			fc->rmi_dev = rmi_dev;
+			fc->num_of_irqs = pdt_entry.interrupt_source_count;
+			fc->irq_pos = irq_count;
+			irq_count += fc->num_of_irqs;
+
+			retval = init_one_function(rmi_dev, fc);
+			if (retval < 0) {
+				dev_err(dev, "Failed to initialize F%.2x\n",
+					pdt_entry.function_number);
+				kfree(fc);
+				goto error_exit;
+			}
+
+			list_add_tail(&fc->list, &data->rmi_functions.list);
+		}
+	}
+	data->num_of_irq_regs = (irq_count + 7) / 8;
+	dev_dbg(dev, "%s: Done with PDT scan.\n", __func__);
+	return 0;
+
+error_exit:
+	return retval;
+}
+
+
+#ifdef SYNAPTICS_SENSOR_POLL
+void synaptics_sensor_poller(unsigned long data){
+  pr_info("in function ____%s____ ,  rmi_device= 0x%8x \n", __func__, polled_synaptics_rmi_device);
+  // msleep(10000);
+  for (;;) {
+    msleep(100);
+    rmi_driver_irq_handler(polled_synaptics_rmi_device, 0);
+  }
+
+  return;
+}
+
+struct workqueue_struct *synaptics_rmi_polling_queue = NULL;
+struct delayed_work synaptics_rmi_polling_work;
+
+#endif
+
+
+static int rmi_driver_probe(struct rmi_device *rmi_dev)
+{
+	struct rmi_driver_data *data;
+	struct rmi_function_container *fc;
+	struct rmi_device_platform_data *pdata;
+	int error = 0;
+	struct device *dev = &rmi_dev->dev;
+	int attr_count = 0;
+
+	dev_dbg(dev, "%s: Starting probe.\n", __func__);
+
+	pdata = to_rmi_platform_data(rmi_dev);
+
+	data = kzalloc(sizeof(struct rmi_driver_data), GFP_KERNEL);
+	if (!data) {
+		dev_err(dev, "%s: Failed to allocate driver data.\n", __func__);
+		return -ENOMEM;
+	}
+
+	rmi_set_driverdata(rmi_dev, data);
+
+	error = rmi_scan_pdt(rmi_dev);
+	if (error) {
+		dev_err(dev, "PDT scan failed with code %d.\n", error);
+		goto err_free_data;
+	}
+
+	if (!data->f01_container) {
+		dev_err(dev, "missing f01 function!\n");
+		error = -EINVAL;
+		goto err_free_data;
+	}
+
+	data->f01_container->irq_mask = kzalloc(
+			sizeof(u8) * data->num_of_irq_regs, GFP_KERNEL);
+	if (!data->f01_container->irq_mask) {
+		dev_err(dev, "Failed to allocate F01 IRQ mask.\n");
+		error = -ENOMEM;
+		goto err_free_data;
+	}
+	construct_mask(data->f01_container->irq_mask,
+		       data->f01_container->num_of_irqs,
+		       data->f01_container->irq_pos);
+	list_for_each_entry(fc, &data->rmi_functions.list, list)
+		fc->irq_mask = rmi_driver_irq_get_mask(rmi_dev, fc);
+
+	error = rmi_driver_f01_init(rmi_dev);
+	if (error < 0) {
+		dev_err(dev, "Failed to initialize F01.\n");
+		goto err_free_data;
+	}
+
+	error = rmi_read(rmi_dev, PDT_PROPERTIES_LOCATION,
+			 (char *) &data->pdt_props);
+	if (error < 0) {
+		/* we'll print out a warning and continue since
+		 * failure to get the PDT properties is not a cause to fail
+		 */
+		dev_warn(dev, "Could not read PDT properties from 0x%04x. "
+			 "Assuming 0x00.\n", PDT_PROPERTIES_LOCATION);
+	}
+
+	dev_dbg(dev, "%s: Creating sysfs files.", __func__);
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		error = device_create_file(dev, &attrs[attr_count]);
+		if (error < 0) {
+			dev_err(dev, "%s: Failed to create sysfs file %s.\n",
+				__func__, attrs[attr_count].attr.name);
+			goto err_free_data;
+		}
+	}
+
+	__mutex_init(&data->irq_mutex, "irq_mutex", &data->irq_key);
+	data->current_irq_mask = kzalloc(sizeof(u8) * data->num_of_irq_regs,
+					 GFP_KERNEL);
+	if (!data->current_irq_mask) {
+		dev_err(dev, "Failed to allocate current_irq_mask.\n");
+		error = -ENOMEM;
+		goto err_free_data;
+	}
+	error = rmi_read_block(rmi_dev,
+				data->f01_container->fd.control_base_addr+1,
+				data->current_irq_mask, data->num_of_irq_regs);
+	if (error < 0) {
+		dev_err(dev, "%s: Failed to read current IRQ mask.\n",
+			__func__);
+		goto err_free_data;
+	}
+	data->irq_mask_store = kzalloc(sizeof(u8) * data->num_of_irq_regs,
+				       GFP_KERNEL);
+	if (!data->irq_mask_store) {
+		dev_err(dev, "Failed to allocate mask store.\n");
+		error = -ENOMEM;
+		goto err_free_data;
+	}
+
+#if defined(CONFIG_RMI4_DEV)
+	if (rmi_char_dev_register(rmi_dev->phys))
+		pr_err("%s: error register char device", __func__);
+#endif /*CONFIG_RMI4_DEV*/
+
+#ifdef	CONFIG_PM
+	data->pm_data = pdata->pm_data;
+	data->pre_suspend = pdata->pre_suspend;
+	data->post_resume = pdata->post_resume;
+
+	mutex_init(&data->suspend_mutex);
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	rmi_dev->early_suspend_handler.level =
+		EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	rmi_dev->early_suspend_handler.suspend = rmi_driver_early_suspend;
+	rmi_dev->early_suspend_handler.resume = rmi_driver_late_resume;
+	register_early_suspend(&rmi_dev->early_suspend_handler);
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+#endif /* CONFIG_PM */
+	data->enabled = true;
+
+	dev_info(dev, "connected RMI device manufacturer: %s product: %s\n",
+		 data->manufacturer_id == 1 ? "synaptics" : "unknown",
+		 data->product_id);
+
+#ifdef SYNAPTICS_SENSOR_POLL
+	synaptics_rmi_polling_queue = create_singlethread_workqueue("rmi_poll_work");
+	INIT_DELAYED_WORK_DEFERRABLE(&synaptics_rmi_polling_work, synaptics_sensor_poller);
+	pr_info("%s: setting up POLLING mode, rmi_device= 0x%8x \n", __func__, polled_synaptics_rmi_device);
+	queue_delayed_work(synaptics_rmi_polling_queue, &synaptics_rmi_polling_work, 1000);
+#endif
+	return 0;
+
+ err_free_data:
+	rmi_free_function_list(rmi_dev);
+	for (attr_count--; attr_count >= 0; attr_count--)
+		device_remove_file(dev, &attrs[attr_count]);
+	kfree(data->f01_container->irq_mask);
+	kfree(data->irq_mask_store);
+	kfree(data->current_irq_mask);
+	kfree(data);
+	return error;
+}
+
+#ifdef CONFIG_PM
+static int rmi_driver_suspend(struct device *dev)
+{
+	struct rmi_device *rmi_dev;
+	struct rmi_driver_data *data;
+	struct rmi_function_container *entry;
+	int retval = 0;
+
+	rmi_dev = to_rmi_device(dev);
+	data = rmi_get_driverdata(rmi_dev);
+
+	mutex_lock(&data->suspend_mutex);
+	if (data->suspended)
+		goto exit;
+
+	if (data->pre_suspend) {
+		retval = data->pre_suspend(data->pm_data);
+		if (retval)
+			goto exit;
+	}
+
+	list_for_each_entry(entry, &data->rmi_functions.list, list)
+		if (entry->fh && entry->fh->suspend) {
+			retval = entry->fh->suspend(entry);
+			if (retval < 0)
+				goto exit;
+		}
+
+	if (data->f01_container && data->f01_container->fh
+				&& data->f01_container->fh->suspend) {
+		retval = data->f01_container->fh->suspend(data->f01_container);
+		if (retval < 0)
+			goto exit;
+	}
+	data->suspended = true;
+
+exit:
+	mutex_unlock(&data->suspend_mutex);
+	return retval;
+}
+
+static int rmi_driver_resume(struct device *dev)
+{
+	struct rmi_device *rmi_dev;
+	struct rmi_driver_data *data;
+	struct rmi_function_container *entry;
+	int retval = 0;
+
+	rmi_dev = to_rmi_device(dev);
+	data = rmi_get_driverdata(rmi_dev);
+
+	mutex_lock(&data->suspend_mutex);
+	if (!data->suspended)
+		goto exit;
+
+	if (data->f01_container && data->f01_container->fh
+				&& data->f01_container->fh->resume) {
+		retval = data->f01_container->fh->resume(data->f01_container);
+		if (retval < 0)
+			goto exit;
+	}
+
+	list_for_each_entry(entry, &data->rmi_functions.list, list)
+		if (entry->fh && entry->fh->resume) {
+			retval = entry->fh->resume(entry);
+			if (retval < 0)
+				goto exit;
+		}
+
+	if (data->post_resume) {
+		retval = data->post_resume(data->pm_data);
+		if (retval)
+			goto exit;
+	}
+
+	data->suspended = false;
+
+exit:
+	mutex_unlock(&data->suspend_mutex);
+	return retval;
+}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void rmi_driver_early_suspend(struct early_suspend *h)
+{
+	struct rmi_device *rmi_dev =
+	    container_of(h, struct rmi_device, early_suspend_handler);
+
+	dev_dbg(&rmi_dev->dev, "Early suspend.\n");
+	rmi_driver_suspend(&rmi_dev->dev);
+}
+
+static void rmi_driver_late_resume(struct early_suspend *h)
+{
+	struct rmi_device *rmi_dev =
+	    container_of(h, struct rmi_device, early_suspend_handler);
+
+	dev_dbg(&rmi_dev->dev, "Late resume.\n");
+	rmi_driver_resume(&rmi_dev->dev);
+}
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+#endif /* CONFIG_PM */
+
+static int __devexit rmi_driver_remove(struct rmi_device *rmi_dev)
+{
+	struct rmi_driver_data *data = rmi_get_driverdata(rmi_dev);
+	struct rmi_function_container *entry;
+	int i;
+
+	list_for_each_entry(entry, &data->rmi_functions.list, list)
+		if (entry->fh && entry->fh->remove)
+			entry->fh->remove(entry);
+
+	rmi_free_function_list(rmi_dev);
+	for (i = 0; i < ARRAY_SIZE(attrs); i++)
+		device_remove_file(&rmi_dev->dev, &attrs[i]);
+	kfree(data->f01_container->irq_mask);
+	kfree(data->irq_mask_store);
+	kfree(data->current_irq_mask);
+	kfree(data);
+
+	return 0;
+}
+
+#ifdef UNIVERSAL_DEV_PM_OPS
+static UNIVERSAL_DEV_PM_OPS(rmi_driver_pm, rmi_driver_suspend,
+			    rmi_driver_resume, NULL);
+#endif
+
+static struct rmi_driver sensor_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		// .name = "rmi-generic",
+		.name = "rmi_generic",
+#ifdef UNIVERSAL_DEV_PM_OPS
+		.pm = &rmi_driver_pm,
+#endif
+	},
+	.probe = rmi_driver_probe,
+	.irq_handler = rmi_driver_irq_handler,
+	.fh_add = rmi_driver_fh_add,
+	.fh_remove = rmi_driver_fh_remove,
+	.get_func_irq_mask = rmi_driver_irq_get_mask,
+	.store_irq_mask = rmi_driver_irq_save,
+	.restore_irq_mask = rmi_driver_irq_restore,
+	.remove = __devexit_p(rmi_driver_remove)
+};
+
+/* Utility routine to handle writes to read-only attributes.  Hopefully
+ * this will never happen, but if the user does something stupid, we don't
+ * want to accept it quietly (which is what can happen if you just put NULL
+ * for the attribute's store function).
+ */
+ssize_t rmi_store_error(struct device *dev,
+			struct device_attribute *attr,
+			const char *buf, size_t count)
+{
+	dev_warn(dev,
+		 "RMI4 WARNING: Attempt to write %d characters to read-only "
+		 "attribute %s.", count, attr->attr.name);
+	return -EPERM;
+}
+
+/* Utility routine to handle reads of write-only attributes.  Hopefully
+ * this will never happen, but if the user does something stupid, we don't
+ * want to accept it quietly (which is what can happen if you just put NULL
+ * for the attribute's show function).
+ */
+ssize_t rmi_show_error(struct device *dev,
+		       struct device_attribute *attr,
+		       char *buf)
+{
+	dev_warn(dev,
+		 "RMI4 WARNING: Attempt to read from write-only attribute %s.",
+		 attr->attr.name);
+	return -EPERM;
+}
+
+/* sysfs show and store fns for driver attributes */
+static ssize_t rmi_driver_hasbsr_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	struct rmi_device *rmi_dev;
+	struct rmi_driver_data *data;
+	rmi_dev = to_rmi_device(dev);
+	data = rmi_get_driverdata(rmi_dev);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", has_bsr(data));
+}
+
+static ssize_t rmi_driver_bsr_show(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	struct rmi_device *rmi_dev;
+	struct rmi_driver_data *data;
+	rmi_dev = to_rmi_device(dev);
+	data = rmi_get_driverdata(rmi_dev);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", data->bsr);
+}
+
+static ssize_t rmi_driver_bsr_store(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	int retval;
+	unsigned long val;
+	struct rmi_device *rmi_dev;
+	struct rmi_driver_data *data;
+
+	rmi_dev = to_rmi_device(dev);
+	data = rmi_get_driverdata(rmi_dev);
+
+	/* need to convert the string data to an actual value */
+	retval = strict_strtoul(buf, 10, &val);
+	if (retval < 0) {
+		dev_err(dev, "Invalid value '%s' written to BSR.\n", buf);
+		return -EINVAL;
+	}
+
+	retval = rmi_write(rmi_dev, BSR_LOCATION, (unsigned char)val);
+	if (retval) {
+		dev_err(dev, "%s : failed to write bsr %u to 0x%x\n",
+			__func__, (unsigned int)val, BSR_LOCATION);
+		return retval;
+	}
+
+	data->bsr = val;
+
+	return count;
+}
+
+static void disable_sensor(struct rmi_device *rmi_dev)
+{
+	struct rmi_driver_data *data = rmi_get_driverdata(rmi_dev);
+
+	rmi_dev->phys->disable_device(rmi_dev->phys);
+
+	data->enabled = false;
+}
+
+static int enable_sensor(struct rmi_device *rmi_dev)
+{
+	struct rmi_driver_data *data = rmi_get_driverdata(rmi_dev);
+	int retval = 0;
+	pr_info("in function ____%s____  \n", __func__);
+	retval = rmi_dev->phys->enable_device(rmi_dev->phys);
+	/* non-zero means error occurred */
+	if (retval)
+		return retval;
+
+	data->enabled = true;
+
+	return 0;
+}
+
+static ssize_t rmi_driver_enabled_show(struct device *dev,
+				       struct device_attribute *attr, char *buf)
+{
+	struct rmi_device *rmi_dev;
+	struct rmi_driver_data *data;
+
+	rmi_dev = to_rmi_device(dev);
+	data = rmi_get_driverdata(rmi_dev);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", data->enabled);
+}
+
+static ssize_t rmi_driver_enabled_store(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	int retval;
+	int new_value;
+	struct rmi_device *rmi_dev;
+	struct rmi_driver_data *data;
+
+	rmi_dev = to_rmi_device(dev);
+	data = rmi_get_driverdata(rmi_dev);
+
+	if (sysfs_streq(buf, "0"))
+		new_value = false;
+	else if (sysfs_streq(buf, "1"))
+		new_value = true;
+	else
+		return -EINVAL;
+
+	if (new_value) {
+		retval = enable_sensor(rmi_dev);
+		if (retval) {
+			dev_err(dev, "Failed to enable sensor, code=%d.\n",
+				retval);
+			return -EIO;
+		}
+	} else {
+		disable_sensor(rmi_dev);
+	}
+
+	return count;
+}
+
+#if REGISTER_DEBUG
+static ssize_t rmi_driver_reg_store(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	int retval;
+	unsigned int address;
+	unsigned int bytes;
+	struct rmi_device *rmi_dev;
+	struct rmi_driver_data *data;
+	u8 readbuf[128];
+	unsigned char outbuf[512];
+	unsigned char *bufptr = outbuf;
+	int i;
+
+	rmi_dev = to_rmi_device(dev);
+	data = rmi_get_driverdata(rmi_dev);
+
+	retval = sscanf(buf, "%x %u", &address, &bytes);
+	if (retval != 2) {
+		dev_err(dev, "Invalid input (code %d) for reg store: %s",
+			retval, buf);
+		return -EINVAL;
+	}
+	if (address < 0 || address > 0xFFFF) {
+		dev_err(dev, "Invalid address for reg store '%#06x'.\n",
+			address);
+		return -EINVAL;
+	}
+	if (bytes < 0 || bytes >= sizeof(readbuf) || address+bytes > 65535) {
+		dev_err(dev, "Invalid byte count for reg store '%d'.\n",
+			bytes);
+		return -EINVAL;
+	}
+
+	retval = rmi_read_block(rmi_dev, address, readbuf, bytes);
+	if (retval != bytes) {
+		dev_err(dev, "Failed to read %d registers at %#06x, code %d.\n",
+			bytes, address, retval);
+		return retval;
+	}
+
+	dev_info(dev, "Reading %d bytes from %#06x.\n", bytes, address);
+	for (i = 0; i < bytes; i++) {
+		retval = snprintf(bufptr, 4, "%02X ", readbuf[i]);
+		if (retval < 0) {
+			dev_err(dev, "Failed to format string. Code: %d",
+				retval);
+			return retval;
+		}
+		bufptr += retval;
+	}
+	dev_info(dev, "%s\n", outbuf);
+
+	return count;
+}
+#endif
+
+#if DELAY_DEBUG
+static ssize_t rmi_delay_store(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	int retval;
+	struct rmi_device *rmi_dev;
+	struct rmi_device_platform_data *pdata;
+	unsigned int new_read_delay;
+	unsigned int new_write_delay;
+	unsigned int new_block_delay;
+	unsigned int new_pre_delay;
+	unsigned int new_post_delay;
+
+	retval = sscanf(buf, "%u %u %u %u %u", &new_read_delay,
+			&new_write_delay, &new_block_delay,
+			&new_pre_delay, &new_post_delay);
+	if (retval != 5) {
+		dev_err(dev, "Incorrect number of values provided for delay.");
+		return -EINVAL;
+	}
+	if (new_read_delay < 0) {
+		dev_err(dev, "Byte delay must be positive microseconds.\n");
+		return -EINVAL;
+	}
+	if (new_write_delay < 0) {
+		dev_err(dev, "Write delay must be positive microseconds.\n");
+		return -EINVAL;
+	}
+	if (new_block_delay < 0) {
+		dev_err(dev, "Block delay must be positive microseconds.\n");
+		return -EINVAL;
+	}
+	if (new_pre_delay < 0) {
+		dev_err(dev,
+			"Pre-transfer delay must be positive microseconds.\n");
+		return -EINVAL;
+	}
+	if (new_post_delay < 0) {
+		dev_err(dev,
+			"Post-transfer delay must be positive microseconds.\n");
+		return -EINVAL;
+	}
+
+	rmi_dev = to_rmi_device(dev);
+	pdata = rmi_dev->phys->dev->platform_data;
+
+	dev_info(dev, "Setting delays to %u %u %u %u %u.\n", new_read_delay,
+		 new_write_delay, new_block_delay, new_pre_delay,
+		 new_post_delay);
+	pdata->spi_data.read_delay_us = new_read_delay;
+	pdata->spi_data.write_delay_us = new_write_delay;
+	pdata->spi_data.block_delay_us = new_block_delay;
+	pdata->spi_data.pre_delay_us = new_pre_delay;
+	pdata->spi_data.post_delay_us = new_post_delay;
+
+	return count;
+}
+
+static ssize_t rmi_delay_show(struct device *dev,
+				       struct device_attribute *attr, char *buf)
+{
+	struct rmi_device *rmi_dev;
+	struct rmi_device_platform_data *pdata;
+
+	rmi_dev = to_rmi_device(dev);
+	pdata = rmi_dev->phys->dev->platform_data;
+
+	return snprintf(buf, PAGE_SIZE, "%d %d %d %d %d\n",
+		pdata->spi_data.read_delay_us, pdata->spi_data.write_delay_us,
+		pdata->spi_data.block_delay_us,
+		pdata->spi_data.pre_delay_us, pdata->spi_data.post_delay_us);
+}
+#endif
+
+static ssize_t rmi_driver_phys_show(struct device *dev,
+				       struct device_attribute *attr, char *buf)
+{
+	struct rmi_device *rmi_dev;
+	struct rmi_phys_info *info;
+
+	rmi_dev = to_rmi_device(dev);
+	info = &rmi_dev->phys->info;
+
+	return snprintf(buf, PAGE_SIZE, "%-5s %ld %ld %ld %ld %ld %ld %ld\n",
+		 info->proto ? info->proto : "unk",
+		 info->tx_count, info->tx_bytes, info->tx_errs,
+		 info->rx_count, info->rx_bytes, info->rx_errs,
+		 info->attn_count);
+}
+
+static ssize_t rmi_driver_version_show(struct device *dev,
+				       struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%s\n",
+		 RMI_DRIVER_VERSION_STRING);
+}
+
+static int __init rmi_driver_init(void)
+{
+	return rmi_register_driver(&sensor_driver);
+}
+
+static void __exit rmi_driver_exit(void)
+{
+	rmi_unregister_driver(&sensor_driver);
+}
+
+module_init(rmi_driver_init);
+module_exit(rmi_driver_exit);
+
+MODULE_AUTHOR("Eric Andersson <eric.andersson@unixphere.com>");
+MODULE_DESCRIPTION("RMI generic driver");
diff --git a/drivers/input/touchscreen/rmi4/rmi_driver.h b/drivers/input/touchscreen/rmi4/rmi_driver.h
new file mode 100644
index 000000000000..9e9f2c098225
--- /dev/null
+++ b/drivers/input/touchscreen/rmi4/rmi_driver.h
@@ -0,0 +1,109 @@
+/*
+ * Copyright (c) 2011 Synaptics Incorporated
+ * Copyright (c) 2011 Unixphere
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+#ifndef _RMI_DRIVER_H
+#define _RMI_DRIVER_H
+
+#define RMI_DRIVER_MAJOR_VERSION     1
+#define RMI_DRIVER_MINOR_VERSION     3
+#define RMI_DRIVER_SUB_MINOR_VERSION 0
+
+/* TODO: Figure out some way to construct this string in the define macro
+ * using the values defined above.
+ */
+#define RMI_DRIVER_VERSION_STRING "1.3.0"
+
+
+#define RMI_PRODUCT_ID_LENGTH    10
+#define RMI_PRODUCT_INFO_LENGTH   2
+#define RMI_DATE_CODE_LENGTH      3
+
+struct rmi_driver_data {
+	struct rmi_function_container rmi_functions;
+
+	struct rmi_function_container *f01_container;
+
+	int num_of_irq_regs;
+	u8 *current_irq_mask;
+	u8 *irq_mask_store;
+	bool irq_stored;
+	struct mutex irq_mutex;
+	struct lock_class_key irq_key;
+
+	unsigned char pdt_props;
+	unsigned char bsr;
+	bool enabled;
+
+	u8 manufacturer_id;
+	/* product id + null termination */
+	u8 product_id[RMI_PRODUCT_ID_LENGTH + 1];
+
+#ifdef CONFIG_PM
+	bool suspended;
+	struct mutex suspend_mutex;
+
+	void *pm_data;
+	int (*pre_suspend) (const void *pm_data);
+	int (*post_resume) (const void *pm_data);
+#endif
+
+	void *data;
+};
+
+struct pdt_entry {
+	u8 query_base_addr:8;
+	u8 command_base_addr:8;
+	u8 control_base_addr:8;
+	u8 data_base_addr:8;
+	u8 interrupt_source_count:3;
+	u8 bits3and4:2;
+	u8 function_version:2;
+	u8 bit7:1;
+	u8 function_number:8;
+};
+
+int rmi_driver_f01_init(struct rmi_device *rmi_dev);
+
+static inline void copy_pdt_entry_to_fd(struct pdt_entry *pdt,
+				 struct rmi_function_descriptor *fd,
+				 u16 page_start)
+{
+	fd->query_base_addr = pdt->query_base_addr + page_start;
+	fd->command_base_addr = pdt->command_base_addr + page_start;
+	fd->control_base_addr = pdt->control_base_addr + page_start;
+	fd->data_base_addr = pdt->data_base_addr + page_start;
+	fd->function_number = pdt->function_number;
+	fd->interrupt_source_count = pdt->interrupt_source_count;
+	fd->function_version = pdt->function_version;
+}
+
+/* Helper function to convert a short (in host processor endianess) to
+ * a byte array in the RMI endianess for shorts.  See above comment for
+ * why we dont us htons or something like that.
+ */
+void hstoba(u8 *dest, u16 src);
+
+/* Helper fn to convert a byte array representing a short in the RMI
+ * endian-ness to a short in the native processor's specific endianness.
+ * We don't use ntohs/htons here because, well, we're not dealing with
+ * a pair of shorts. And casting dest to short* wouldn't work, because
+ * that would imply knowing the byte order of short in the first place.
+ */
+void batohs(u16 *dest, u8 *src);
+
+#endif
diff --git a/drivers/input/touchscreen/rmi4/rmi_f01.c b/drivers/input/touchscreen/rmi4/rmi_f01.c
new file mode 100644
index 000000000000..6ccdf2443609
--- /dev/null
+++ b/drivers/input/touchscreen/rmi4/rmi_f01.c
@@ -0,0 +1,775 @@
+/*
+ * Copyright (c) 2011 Synaptics Incorporated
+ * Copyright (c) 2011 Unixphere
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#define DEBUG
+
+#include <linux/kernel.h>
+#include <linux/rmi.h>
+#include <linux/slab.h>
+#include "rmi_driver.h"
+
+/* control register bits */
+#define RMI_SLEEP_MODE_NORMAL (0x00)
+#define RMI_SLEEP_MODE_SENSOR_SLEEP (0x01)
+#define RMI_SLEEP_MODE_RESERVED0 (0x02)
+#define RMI_SLEEP_MODE_RESERVED1 (0x03)
+
+#define RMI_IS_VALID_SLEEPMODE(mode) \
+	(mode >= RMI_SLEEP_MODE_NORMAL && mode <= RMI_SLEEP_MODE_RESERVED1)
+
+union f01_device_commands {
+	struct {
+		u8 reset:1;
+		u8 reserved:1;
+	};
+	u8 reg;
+};
+
+union f01_device_control {
+	struct {
+		u8 sleep_mode:2;
+		u8 nosleep:1;
+		u8 reserved:2;
+		u8 charger_input:1;
+		u8 report_rate:1;
+		u8 configured:1;
+	};
+	u8 reg;
+};
+
+union f01_device_status {
+	struct {
+		u8 status_code:4;
+		u8 reserved:2;
+		u8 flash_prog:1;
+		u8 unconfigured:1;
+	};
+	u8 reg;
+};
+
+union f01_basic_queries {
+	struct {
+		u8 manufacturer_id:8;
+
+		u8 custom_map:1;
+		u8 non_compliant:1;
+		u8 q1_bit_2:1;
+		u8 has_sensor_id:1;
+		u8 has_charger_input:1;
+		u8 has_adjustable_doze:1;
+		u8 has_adjustable_doze_holdoff:1;
+		u8 q1_bit_7:1;
+
+		u8 productinfo_1:7;
+		u8 q2_bit_7:1;
+		u8 productinfo_2:7;
+		u8 q3_bit_7:1;
+
+		u8 year:5;
+		u8 month:4;
+		u8 day:5;
+		u8 cp1:1;
+		u8 cp2:1;
+		u8 wafer_id1_lsb:8;
+		u8 wafer_id1_msb:8;
+		u8 wafer_id2_lsb:8;
+		u8 wafer_id2_msb:8;
+		u8 wafer_id3_lsb:8;
+	};
+	u8 regs[11];
+};
+
+struct f01_data {
+	union f01_device_control device_control;
+	union f01_basic_queries basic_queries;
+	union f01_device_status device_status;
+	u8 product_id[RMI_PRODUCT_ID_LENGTH+1];
+
+#ifdef	CONFIG_PM
+	bool suspended;
+	bool old_nosleep;
+#endif
+};
+
+
+static ssize_t rmi_fn_01_productinfo_show(struct device *dev,
+					  struct device_attribute *attr,
+					  char *buf);
+
+static ssize_t rmi_fn_01_productid_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf);
+
+static ssize_t rmi_fn_01_manufacturer_show(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf);
+
+static ssize_t rmi_fn_01_datecode_show(struct device *dev,
+				       struct device_attribute *attr,
+				       char *buf);
+
+static ssize_t rmi_fn_01_reportrate_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf);
+
+static ssize_t rmi_fn_01_reportrate_store(struct device *dev,
+					  struct device_attribute *attr,
+					  const char *buf, size_t count);
+
+static ssize_t rmi_fn_01_reset_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count);
+
+static ssize_t rmi_fn_01_sleepmode_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf);
+
+static ssize_t rmi_fn_01_sleepmode_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count);
+
+static ssize_t rmi_fn_01_nosleep_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf);
+
+static ssize_t rmi_fn_01_nosleep_store(struct device *dev,
+				       struct device_attribute *attr,
+				       const char *buf, size_t count);
+
+static ssize_t rmi_fn_01_chargerinput_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf);
+
+static ssize_t rmi_fn_01_chargerinput_store(struct device *dev,
+				       struct device_attribute *attr,
+				       const char *buf, size_t count);
+
+static ssize_t rmi_fn_01_configured_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf);
+
+static ssize_t rmi_fn_01_unconfigured_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf);
+
+static ssize_t rmi_fn_01_flashprog_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf);
+
+static ssize_t rmi_fn_01_statuscode_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf);
+
+static struct device_attribute fn_01_attrs[] = {
+	__ATTR(productinfo, RMI_RO_ATTR,
+	       rmi_fn_01_productinfo_show, rmi_store_error),
+	__ATTR(productid, RMI_RO_ATTR,
+	       rmi_fn_01_productid_show, rmi_store_error),
+	__ATTR(manufacturer, RMI_RO_ATTR,
+	       rmi_fn_01_manufacturer_show, rmi_store_error),
+	__ATTR(datecode, RMI_RO_ATTR,
+	       rmi_fn_01_datecode_show, rmi_store_error),
+
+	/* control register access */
+	__ATTR(sleepmode, RMI_RO_ATTR,
+	       rmi_fn_01_sleepmode_show, rmi_fn_01_sleepmode_store),
+	__ATTR(nosleep, RMI_RO_ATTR,
+	       rmi_fn_01_nosleep_show, rmi_fn_01_nosleep_store),
+	__ATTR(chargerinput, RMI_RO_ATTR,
+	       rmi_fn_01_chargerinput_show, rmi_fn_01_chargerinput_store),
+	__ATTR(reportrate, RMI_RO_ATTR,
+	       rmi_fn_01_reportrate_show, rmi_fn_01_reportrate_store),
+	/* We make report rate RO, since the driver uses that to look for
+	 * resets.  We don't want someone faking us out by changing that
+	 * bit.
+	 */
+	__ATTR(configured, RMI_RO_ATTR,
+	       rmi_fn_01_configured_show, rmi_store_error),
+
+	/* Command register access. */
+	__ATTR(reset, RMI_RO_ATTR,
+	       rmi_show_error, rmi_fn_01_reset_store),
+
+	/* STatus register access. */
+	__ATTR(unconfigured, RMI_RO_ATTR,
+	       rmi_fn_01_unconfigured_show, rmi_store_error),
+	__ATTR(flashprog, RMI_RO_ATTR,
+	       rmi_fn_01_flashprog_show, rmi_store_error),
+	__ATTR(statuscode, RMI_RO_ATTR,
+	       rmi_fn_01_statuscode_show, rmi_store_error),
+};
+
+/* Utility routine to set the value of a bit field in a register. */
+int rmi_set_bit_field(struct rmi_device *rmi_dev,
+		      unsigned short address,
+		      unsigned char field_mask,
+		      unsigned char bits)
+{
+	unsigned char reg_contents;
+	int retval;
+
+	retval = rmi_read(rmi_dev, address, &reg_contents);
+	if (retval)
+		return retval;
+	reg_contents = (reg_contents & ~field_mask) | bits;
+	retval = rmi_write(rmi_dev, address, reg_contents);
+	if (retval == 1)
+		return 0;
+	else if (retval == 0)
+		return -EIO;
+	return retval;
+}
+
+static ssize_t rmi_fn_01_productinfo_show(struct device *dev,
+					  struct device_attribute *attr,
+					  char *buf)
+{
+	struct f01_data *data = NULL;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "0x%02x 0x%02x\n",
+			data->basic_queries.productinfo_1,
+			data->basic_queries.productinfo_2);
+}
+
+static ssize_t rmi_fn_01_productid_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct f01_data *data = NULL;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%s\n", data->product_id);
+}
+
+static ssize_t rmi_fn_01_manufacturer_show(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct f01_data *data = NULL;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "0x%02x\n",
+			data->basic_queries.manufacturer_id);
+}
+
+static ssize_t rmi_fn_01_datecode_show(struct device *dev,
+				       struct device_attribute *attr,
+				       char *buf)
+{
+	struct f01_data *data = NULL;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "20%02u-%02u-%02u\n",
+			data->basic_queries.year,
+			data->basic_queries.month,
+			data->basic_queries.day);
+}
+
+static ssize_t rmi_fn_01_reset_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	struct rmi_function_container *fc = NULL;
+	unsigned int reset;
+	int retval = 0;
+	/* Command register always reads as 0, so we can just use a local. */
+	union f01_device_commands commands = {};
+
+	fc = to_rmi_function_container(dev);
+
+	if (sscanf(buf, "%u", &reset) != 1)
+		return -EINVAL;
+	if (reset < 0 || reset > 1)
+		return -EINVAL;
+
+	/* Per spec, 0 has no effect, so we skip it entirely. */
+	if (reset) {
+		commands.reset = 1;
+		retval = rmi_write_block(fc->rmi_dev, fc->fd.command_base_addr,
+				&commands.reg, sizeof(commands.reg));
+		if (retval < 0) {
+			dev_err(dev, "%s: failed to issue reset command, "
+				"error = %d.", __func__, retval);
+			return retval;
+		}
+	}
+
+	return count;
+}
+
+static ssize_t rmi_fn_01_sleepmode_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct f01_data *data = NULL;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE,
+			"%d\n", data->device_control.sleep_mode);
+}
+
+static ssize_t rmi_fn_01_sleepmode_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf,
+					 size_t count)
+{
+	struct f01_data *data = NULL;
+	unsigned long new_value;
+	int retval;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	retval = strict_strtoul(buf, 10, &new_value);
+	if (retval < 0 || !RMI_IS_VALID_SLEEPMODE(new_value)) {
+		dev_err(dev, "%s: Invalid sleep mode %s.", __func__, buf);
+		return -EINVAL;
+	}
+
+	dev_dbg(dev, "Setting sleep mode to %ld.", new_value);
+	data->device_control.sleep_mode = new_value;
+	retval = rmi_write_block(fc->rmi_dev, fc->fd.control_base_addr,
+			&data->device_control.reg,
+			sizeof(data->device_control.reg));
+	if (retval >= 0)
+		retval = count;
+	else
+		dev_err(dev, "Failed to write sleep mode, code %d.\n", retval);
+	return retval;
+}
+
+static ssize_t rmi_fn_01_nosleep_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	struct f01_data *data = NULL;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", data->device_control.nosleep);
+}
+
+static ssize_t rmi_fn_01_nosleep_store(struct device *dev,
+				       struct device_attribute *attr,
+				       const char *buf,
+				       size_t count)
+{
+	struct f01_data *data = NULL;
+	unsigned long new_value;
+	int retval;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	retval = strict_strtoul(buf, 10, &new_value);
+	if (retval < 0 || new_value < 0 || new_value > 1) {
+		dev_err(dev, "%s: Invalid nosleep bit %s.", __func__, buf);
+		return -EINVAL;
+	}
+
+	data->device_control.nosleep = new_value;
+	retval = rmi_write_block(fc->rmi_dev, fc->fd.control_base_addr,
+			&data->device_control.reg,
+			sizeof(data->device_control.reg));
+	if (retval >= 0)
+		retval = count;
+	else
+		dev_err(dev, "Failed to write nosleep bit.\n");
+	return retval;
+}
+
+static ssize_t rmi_fn_01_chargerinput_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	struct f01_data *data = NULL;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			data->device_control.charger_input);
+}
+
+static ssize_t rmi_fn_01_chargerinput_store(struct device *dev,
+				       struct device_attribute *attr,
+				       const char *buf,
+				       size_t count)
+{
+	struct f01_data *data = NULL;
+	unsigned long new_value;
+	int retval;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	retval = strict_strtoul(buf, 10, &new_value);
+	if (retval < 0 || new_value < 0 || new_value > 1) {
+		dev_err(dev, "%s: Invalid chargerinput bit %s.", __func__, buf);
+		return -EINVAL;
+	}
+
+	data->device_control.charger_input = new_value;
+	retval = rmi_write_block(fc->rmi_dev, fc->fd.control_base_addr,
+			&data->device_control.reg,
+			sizeof(data->device_control.reg));
+	if (retval >= 0)
+		retval = count;
+	else
+		dev_err(dev, "Failed to write chargerinput bit.\n");
+	return retval;
+}
+
+static ssize_t rmi_fn_01_reportrate_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	struct f01_data *data = NULL;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			data->device_control.report_rate);
+}
+
+static ssize_t rmi_fn_01_reportrate_store(struct device *dev,
+				       struct device_attribute *attr,
+				       const char *buf,
+				       size_t count)
+{
+	struct f01_data *data = NULL;
+	unsigned long new_value;
+	int retval;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	retval = strict_strtoul(buf, 10, &new_value);
+	if (retval < 0 || new_value < 0 || new_value > 1) {
+		dev_err(dev, "%s: Invalid reportrate bit %s.", __func__, buf);
+		return -EINVAL;
+	}
+
+	data->device_control.report_rate = new_value;
+	retval = rmi_write_block(fc->rmi_dev, fc->fd.control_base_addr,
+			&data->device_control.reg,
+			sizeof(data->device_control.reg));
+	if (retval >= 0)
+		retval = count;
+	else
+		dev_err(dev, "Failed to write reportrate bit.\n");
+	return retval;
+}
+
+static ssize_t rmi_fn_01_configured_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	struct f01_data *data = NULL;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			data->device_control.configured);
+}
+
+static ssize_t rmi_fn_01_unconfigured_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	struct f01_data *data = NULL;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			data->device_status.unconfigured);
+}
+
+static ssize_t rmi_fn_01_flashprog_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	struct f01_data *data = NULL;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			data->device_status.flash_prog);
+}
+
+static ssize_t rmi_fn_01_statuscode_show(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	struct f01_data *data = NULL;
+	struct rmi_function_container *fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "0x%02x\n",
+			data->device_status.status_code);
+}
+
+int rmi_driver_f01_init(struct rmi_device *rmi_dev)
+{
+	struct rmi_driver_data *driver_data = rmi_get_driverdata(rmi_dev);
+	struct rmi_function_container *fc = driver_data->f01_container;
+	struct f01_data *data;
+	int error;
+	u8 temp;
+	int attr_count;
+
+	data = kzalloc(sizeof(struct f01_data), GFP_KERNEL);
+	if (!data) {
+		dev_err(&rmi_dev->dev, "Failed to allocate F01 data.\n");
+		return -ENOMEM;
+	}
+	fc->data = data;
+
+	/* Set the configured bit. */
+	error = rmi_read_block(rmi_dev, fc->fd.control_base_addr,
+			&data->device_control.reg,
+			sizeof(data->device_control.reg));
+	if (error < 0) {
+		dev_err(&fc->dev, "Failed to read F01 control.\n");
+		goto error_exit;
+	}
+
+	/* Sleep mode might be set as a hangover from a system crash or
+	 * reboot without power cycle.  If so, clear it so the sensor
+	 * is certain to function.
+	 */
+	if (data->device_control.sleep_mode != RMI_SLEEP_MODE_NORMAL) {
+		dev_warn(&fc->dev,
+			 "WARNING: Non-zero sleep mode found. Clearing...\n");
+		data->device_control.sleep_mode = RMI_SLEEP_MODE_NORMAL;
+	}
+
+	data->device_control.configured = 1;
+	error = rmi_write_block(rmi_dev, fc->fd.control_base_addr,
+			&data->device_control.reg,
+			sizeof(data->device_control.reg));
+	if (error < 0) {
+		dev_err(&fc->dev, "Failed to write F01 control.\n");
+		goto error_exit;
+	}
+
+	/* dummy read in order to clear irqs */
+	error = rmi_read(rmi_dev, fc->fd.data_base_addr + 1, &temp);
+	if (error < 0) {
+		dev_err(&fc->dev, "Failed to read Interrupt Status.\n");
+		goto error_exit;
+	}
+
+	error = rmi_read_block(rmi_dev, fc->fd.query_base_addr,
+				data->basic_queries.regs,
+				sizeof(data->basic_queries.regs));
+	if (error < 0) {
+		dev_err(&fc->dev, "Failed to read device query registers.\n");
+		goto error_exit;
+	}
+	driver_data->manufacturer_id = data->basic_queries.manufacturer_id;
+
+	error = rmi_read_block(rmi_dev,
+		fc->fd.query_base_addr + sizeof(data->basic_queries.regs),
+		data->product_id, RMI_PRODUCT_ID_LENGTH);
+	if (error < 0) {
+		dev_err(&fc->dev, "Failed to read product ID.\n");
+		goto error_exit;
+	}
+	data->product_id[RMI_PRODUCT_ID_LENGTH] = '\0';
+	memcpy(driver_data->product_id, data->product_id,
+	       sizeof(data->product_id));
+
+	error = rmi_read_block(rmi_dev, fc->fd.data_base_addr,
+			&data->device_status.reg,
+			sizeof(data->device_status.reg));
+	if (error < 0) {
+		dev_err(&fc->dev, "Failed to read device status.\n");
+		goto error_exit;
+	}
+	if (data->device_status.unconfigured) {
+		dev_err(&fc->dev,
+			"Device reset during configuration process, status: "
+			"%#02x!\n", data->device_status.status_code);
+		error = -EINVAL;
+		goto error_exit;
+	}
+	/*
+	**  attach the routines that handle sysfs interaction
+	** Meaning:  Set up sysfs device attributes.
+	*/
+	for (attr_count = 0; attr_count < ARRAY_SIZE(fn_01_attrs);
+			attr_count++) {
+		if (sysfs_create_file(&fc->dev.kobj,
+				      &fn_01_attrs[attr_count].attr) < 0) {
+			dev_err(&fc->dev, "Failed to create sysfs file for %s.",
+			       fn_01_attrs[attr_count].attr.name);
+			error = -ENODEV;
+			goto error_exit;
+		}
+	}
+
+	return error;
+
+ error_exit:
+	kfree(data);
+	return error;
+}
+
+#ifdef CONFIG_PM
+
+static int rmi_f01_suspend(struct rmi_function_container *fc)
+{
+	struct rmi_device *rmi_dev = fc->rmi_dev;
+	struct rmi_driver_data *driver_data = rmi_get_driverdata(rmi_dev);
+	struct f01_data *data = driver_data->f01_container->data;
+	int retval = 0;
+
+	dev_dbg(&fc->dev, "Suspending...\n");
+	if (data->suspended)
+		return 0;
+
+	data->old_nosleep = data->device_control.nosleep;
+	data->device_control.nosleep = 0;
+	data->device_control.sleep_mode = RMI_SLEEP_MODE_SENSOR_SLEEP;
+
+	retval = rmi_write_block(rmi_dev,
+			driver_data->f01_container->fd.control_base_addr,
+			&data->device_control.reg,
+			sizeof(data->device_control.reg));
+	if (retval < 0) {
+		dev_err(&fc->dev, "Failed to write sleep mode. "
+			"Code: %d.\n", retval);
+		data->device_control.nosleep = data->old_nosleep;
+		data->device_control.sleep_mode = RMI_SLEEP_MODE_NORMAL;
+	} else {
+		data->suspended = true;
+		retval = 0;
+	}
+
+	return retval;
+}
+
+static int rmi_f01_resume(struct rmi_function_container *fc)
+{
+	struct rmi_device *rmi_dev = fc->rmi_dev;
+	struct rmi_driver_data *driver_data = rmi_get_driverdata(rmi_dev);
+	struct f01_data *data = driver_data->f01_container->data;
+	int retval = 0;
+
+	dev_dbg(&fc->dev, "Resuming...\n");
+	if (!data->suspended)
+		return 0;
+
+	data->device_control.nosleep = data->old_nosleep;
+	data->device_control.sleep_mode = RMI_SLEEP_MODE_NORMAL;
+
+	retval = rmi_write_block(rmi_dev,
+			driver_data->f01_container->fd.control_base_addr,
+			&data->device_control.reg,
+			sizeof(data->device_control.reg));
+	if (retval < 0)
+		dev_err(&fc->dev, "Failed to restore normal operation. "
+			"Code: %d.\n", retval);
+	else {
+		data->suspended = false;
+		retval = 0;
+	}
+
+	return retval;
+}
+#endif /* CONFIG_PM */
+
+static int rmi_f01_init(struct rmi_function_container *fc)
+{
+	return 0;
+}
+
+static int rmi_f01_attention(struct rmi_function_container *fc, u8 *irq_bits)
+{
+	struct rmi_device *rmi_dev = fc->rmi_dev;
+	struct f01_data *data = fc->data;
+	int error;
+
+	error = rmi_read_block(rmi_dev, fc->fd.data_base_addr,
+			&data->device_status.reg,
+			sizeof(data->device_status.reg));
+	if (error < 0) {
+		dev_err(&fc->dev, "Failed to read device status.\n");
+		return error;
+	}
+
+	/* TODO: Do we handle reset here or elsewhere? */
+	if (data->device_status.unconfigured)
+		dev_warn(&rmi_dev->dev, "Reset detected! Status code: %#04x.\n",
+			data->device_status.status_code);
+	return 0;
+}
+
+static struct rmi_function_handler function_handler = {
+	.func = 0x01,
+	.init = rmi_f01_init,
+	.attention = rmi_f01_attention,
+#ifdef	CONFIG_PM
+	.suspend = rmi_f01_suspend,
+	.resume = rmi_f01_resume,
+#endif
+};
+
+static int __init rmi_f01_module_init(void)
+{
+	int error;
+
+	error = rmi_register_function_driver(&function_handler);
+	if (error < 0) {
+		pr_err("%s: register failed!\n", __func__);
+		return error;
+	}
+
+	return 0;
+}
+
+static void __exit rmi_f01_module_exit(void)
+{
+	rmi_unregister_function_driver(&function_handler);
+}
+
+module_init(rmi_f01_module_init);
+module_exit(rmi_f01_module_exit);
+
+MODULE_AUTHOR("Christopher Heiny <cheiny@synaptics.com>");
+MODULE_DESCRIPTION("RMI F01 module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/rmi4/rmi_f09.c b/drivers/input/touchscreen/rmi4/rmi_f09.c
new file mode 100644
index 000000000000..4328d49ddcbf
--- /dev/null
+++ b/drivers/input/touchscreen/rmi4/rmi_f09.c
@@ -0,0 +1,300 @@
+/*
+ * Copyright (c) 2011 Synaptics Incorporated
+ * Copyright (c) 2011 Unixphere
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+#include <linux/kernel.h>
+#include <linux/rmi.h>
+#include <linux/input.h>
+#include <linux/slab.h>
+
+#define QUERY_BASE_INDEX 1
+#define MAX_LEN 256
+
+/* data specific to fn $09 that needs to be kept around */
+struct f09_query {
+	u8 Limit_Register_Count;
+	union {
+		struct {
+			u8 Result_Register_Count:3;
+			u8 Reserved:3;
+			u8 InternalLimits:1;
+			u8 HostTestEn:1;
+		};
+		u8 f09_bist_query1;
+	};
+};
+
+struct f09_control {
+	/* test1 */
+	u8 Test1LimitLo;
+	u8 Test1LimitHi;
+	u8 Test1LimitDiff;
+	/* test2 */
+	u8 Test2LimitLo;
+	u8 Test2LimitHi;
+	u8 Test2LimitDiff;
+};
+
+struct f09_data {
+	u8 TestNumberControl;
+	u8 Overall_BIST_Result;
+	u8 TestResult1;
+	u8 TestResult2;
+	u8 Transmitter_Number;
+
+	union {
+		struct {
+			u8 Receiver_Number:6;
+			u8 Limit_Failure_Code:2;
+		};
+		u8 f09_bist_data2;
+	};
+};
+
+struct f09_cmd {
+	union {
+		struct {
+			u8 RunBIST:1;
+		};
+		u8 f09_bist_cmd0;
+	};
+};
+
+struct rmi_fn_09_data {
+	struct f09_query query;
+};
+
+static ssize_t rmi_f09_Limit_Register_Count_show(struct device *dev,
+				      struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_f09_HostTestEn_show(struct device *dev,
+				      struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_f09_HostTestEn_store(struct device *dev,
+				      struct device_attribute *attr,
+					const char *buf, size_t count);
+
+static ssize_t rmi_f09_Result_Register_Count_show(struct device *dev,
+				      struct device_attribute *attr, char *buf);
+#if defined(RMI_SYS_ATTR)
+static ssize_t rmi_f09_InternalLimits_show(struct device *dev,
+				      struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_f09_Overall_BIST_Result_show(struct device *dev,
+				      struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_f09_Overall_BIST_Result_store(struct device *dev,
+				       struct device_attribute *attr,
+				       const char *buf, size_t count);
+#endif
+
+static struct device_attribute attrs[] = {
+	__ATTR(Limit_Register_Count, RMI_RO_ATTR,
+	       rmi_f09_Limit_Register_Count_show, rmi_store_error),
+	__ATTR(HostTestEn, RMI_RW_ATTR,
+	       rmi_f09_HostTestEn_show, rmi_f09_HostTestEn_store),
+	__ATTR(InternalLimits, RMI_RO_ATTR,
+	       rmi_f09_Limit_Register_Count_show, rmi_store_error),
+	__ATTR(Result_Register_Count, RMI_RO_ATTR,
+	       rmi_f09_Result_Register_Count_show, rmi_store_error),
+};
+
+static int rmi_f09_init(struct rmi_function_container *fc)
+{
+	struct rmi_device *rmi_dev = fc->rmi_dev;
+	struct rmi_device_platform_data *pdata;
+	struct rmi_fn_09_data  *f09;
+	u8 query_base_addr;
+	int rc;
+	int attr_count = 0;
+	int retval = 0;
+
+	dev_info(&fc->dev, "Intializing F09 values.");
+
+	f09 = kzalloc(sizeof(struct rmi_fn_09_data), GFP_KERNEL);
+	if (!f09) {
+		dev_err(&fc->dev, "Failed to allocate rmi_fn_09_data.\n");
+		retval = -ENOMEM;
+		goto error_exit;
+	}
+	fc->data = f09;
+
+	pdata = to_rmi_platform_data(rmi_dev);
+	query_base_addr = fc->fd.query_base_addr;
+
+	/* initial all default values for f09 query here */
+	rc = rmi_read_block(rmi_dev, query_base_addr,
+		(u8 *)&f09->query, sizeof(f09->query));
+	if (rc < 0) {
+		dev_err(&fc->dev, "Failed to read query register."
+			" from 0x%04x\n", query_base_addr);
+		goto error_exit;
+	}
+
+	dev_dbg(&fc->dev, "Creating sysfs files.");
+	/* Set up sysfs device attributes. */
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		if (sysfs_create_file
+		    (&fc->dev.kobj, &attrs[attr_count].attr) < 0) {
+			dev_err(&fc->dev, "Failed to create sysfs file for %s.",
+			     attrs[attr_count].attr.name);
+			retval = -ENODEV;
+			goto error_exit;
+		}
+	}
+	return 0;
+
+error_exit:
+	dev_err(&fc->dev, "An error occured in F09 init!\n");
+	for (attr_count--; attr_count >= 0; attr_count--)
+		sysfs_remove_file(&fc->dev.kobj,
+				  &attrs[attr_count].attr);
+	kfree(f09);
+	return retval;
+}
+
+static void rmi_f09_remove(struct rmi_function_container *fc)
+{
+	struct rmi_fn_09_data *data = fc->data;
+	if (data) {
+		kfree(&data->query.Limit_Register_Count);
+		kfree(&data->query.f09_bist_query1);
+	}
+	kfree(fc->data);
+}
+
+static struct rmi_function_handler function_handler = {
+	.func = 0x09,
+	.init = rmi_f09_init,
+	.remove = rmi_f09_remove
+};
+
+static int __init rmi_f09_module_init(void)
+{
+	int error;
+
+	error = rmi_register_function_driver(&function_handler);
+	if (error < 0) {
+		pr_err("%s: register failed!\n", __func__);
+		return error;
+	}
+
+	return 0;
+}
+
+static void rmi_f09_module_exit(void)
+{
+	rmi_unregister_function_driver(&function_handler);
+}
+
+
+static ssize_t rmi_f09_Limit_Register_Count_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct rmi_function_container *fc;
+	struct rmi_fn_09_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.Limit_Register_Count);
+}
+
+static ssize_t rmi_f09_HostTestEn_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct rmi_function_container *fc;
+	struct rmi_fn_09_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.HostTestEn);
+}
+
+static ssize_t rmi_f09_HostTestEn_store(struct device *dev,
+				      struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct rmi_function_container *fc;
+	struct rmi_fn_09_data *data;
+	unsigned int new_value;
+	int result;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+	if (sscanf(buf, "%u", &new_value) != 1) {
+		dev_err(dev,
+		"%s: Error - HostTestEn_store has an "
+		"invalid len.\n",
+		__func__);
+		return -EINVAL;
+	}
+
+	if (new_value < 0 || new_value > 1) {
+		dev_err(dev, "%s: Invalid HostTestEn bit %s.", __func__, buf);
+		return -EINVAL;
+	}
+	data->query.HostTestEn = new_value;
+	result = rmi_write(fc->rmi_dev, fc->fd.query_base_addr,
+		data->query.HostTestEn);
+	if (result < 0) {
+		dev_err(dev, "%s : Could not write HostTestEn_store to 0x%x\n",
+				__func__, fc->fd.query_base_addr);
+		return result;
+	}
+
+	return count;
+
+}
+
+#if defined(RMI_SYS_ATTR)
+ ssize_t rmi_f09_InternalLimits_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct rmi_function_container *fc;
+	struct rmi_fn_09_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.InternalLimits);
+}
+#endif
+
+static ssize_t rmi_f09_Result_Register_Count_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct rmi_function_container *fc;
+	struct rmi_fn_09_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.Result_Register_Count);
+}
+
+module_init(rmi_f09_module_init);
+module_exit(rmi_f09_module_exit);
+
+MODULE_AUTHOR("Allie Xiong <axiong@Synaptics.com>");
+MODULE_DESCRIPTION("RMI F09 module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/rmi4/rmi_f11.c b/drivers/input/touchscreen/rmi4/rmi_f11.c
new file mode 100644
index 000000000000..7530720d61f1
--- /dev/null
+++ b/drivers/input/touchscreen/rmi4/rmi_f11.c
@@ -0,0 +1,1513 @@
+/*
+ * Copyright (c) 2011 Synaptics Incorporated
+ * Copyright (c) 2011 Unixphere
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/rmi.h>
+
+#define F11_MAX_NUM_OF_SENSORS		8
+#define F11_MAX_NUM_OF_FINGERS		10
+#define F11_MAX_NUM_OF_TOUCH_SHAPES	16
+
+#define F11_REL_POS_MIN		-128
+#define F11_REL_POS_MAX		127
+
+#define F11_FINGER_STATE_MASK	0x03
+#define F11_FINGER_STATE_SIZE	0x02
+#define F11_FINGER_STATE_MASK_N(i) \
+		(F11_FINGER_STATE_MASK << (i%4 * F11_FINGER_STATE_SIZE))
+
+#define F11_FINGER_STATE_VAL_N(f_state, i) \
+		(f_state >> (i%4 * F11_FINGER_STATE_SIZE))
+
+#define F11_CTRL_SENSOR_MAX_X_POS_OFFSET	6
+#define F11_CTRL_SENSOR_MAX_Y_POS_OFFSET	8
+
+#define F11_CEIL(x, y) (((x) + ((y)-1)) / (y))
+
+/* By default, we'll support two fingers if we can't figure out how many we
+ * really need to handle.
+ */
+#define DEFAULT_NR_OF_FINGERS 2
+#define DEFAULT_XY_MAX 9999
+#define DEFAULT_MAX_ABS_MT_PRESSURE 255
+#define DEFAULT_MAX_ABS_MT_TOUCH 15
+#define DEFAULT_MAX_ABS_MT_ORIENTATION 1
+#define DEFAULT_MIN_ABS_MT_TRACKING_ID 1
+#define DEFAULT_MAX_ABS_MT_TRACKING_ID 10
+#define MAX_LEN 256
+
+static ssize_t rmi_fn_11_flip_show(struct device *dev,
+				   struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_11_flip_store(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count);
+
+static ssize_t rmi_fn_11_clip_show(struct device *dev,
+				   struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_11_clip_store(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count);
+
+static ssize_t rmi_fn_11_offset_show(struct device *dev,
+				     struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_11_offset_store(struct device *dev,
+				      struct device_attribute *attr,
+				      const char *buf, size_t count);
+
+static ssize_t rmi_fn_11_swap_show(struct device *dev,
+				   struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_11_swap_store(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count);
+
+static ssize_t rmi_fn_11_relreport_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf);
+
+static ssize_t rmi_fn_11_relreport_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count);
+
+static ssize_t rmi_fn_11_maxPos_show(struct device *dev,
+				     struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_f11_rezero_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count);
+
+
+static struct device_attribute attrs[] = {
+	__ATTR(flip, RMI_RO_ATTR, rmi_fn_11_flip_show, rmi_fn_11_flip_store),
+	__ATTR(clip, RMI_RO_ATTR, rmi_fn_11_clip_show, rmi_fn_11_clip_store),
+	__ATTR(offset, RMI_RO_ATTR,
+		rmi_fn_11_offset_show, rmi_fn_11_offset_store),
+	__ATTR(swap, RMI_RO_ATTR, rmi_fn_11_swap_show, rmi_fn_11_swap_store),
+	__ATTR(relreport, RMI_RO_ATTR,
+		rmi_fn_11_relreport_show, rmi_fn_11_relreport_store),
+	__ATTR(maxPos, RMI_RO_ATTR, rmi_fn_11_maxPos_show, rmi_store_error),
+	__ATTR(rezero, RMI_RO_ATTR, rmi_show_error, rmi_f11_rezero_store)
+};
+
+
+union f11_2d_commands {
+	struct {
+		u8 rezero:1;
+	};
+	u8 reg;
+};
+
+
+struct f11_2d_device_query {
+	union {
+		struct {
+			u8 nbr_of_sensors:3;
+			u8 has_query9:1;
+			u8 has_query11:1;
+		};
+		u8 f11_2d_query0;
+	};
+
+	u8 f11_2d_query9;
+
+	union {
+		struct {
+			u8 has_z_tuning:1;
+			u8 has_pos_interpolation_tuning:1;
+			u8 has_w_tuning:1;
+			u8 has_pitch_info:1;
+			u8 has_default_finger_width:1;
+			u8 has_segmentation_aggressiveness:1;
+			u8 has_tx_rw_clip:1;
+			u8 has_drumming_correction:1;
+		};
+		u8 f11_2d_query11;
+	};
+};
+
+struct f11_2d_sensor_query {
+	union {
+		struct {
+			/* query1 */
+			u8 number_of_fingers:3;
+			u8 has_rel:1;
+			u8 has_abs:1;
+			u8 has_gestures:1;
+			u8 has_sensitivity_adjust:1;
+			u8 configurable:1;
+			/* query2 */
+			u8 num_of_x_electrodes:7;
+			/* query3 */
+			u8 num_of_y_electrodes:7;
+			/* query4 */
+			u8 max_electrodes:7;
+		};
+		u8 f11_2d_query1__4[4];
+	};
+
+	union {
+		struct {
+			u8 abs_data_size:3;
+			u8 has_anchored_finger:1;
+			u8 has_adj_hyst:1;
+			u8 has_dribble:1;
+		};
+		u8 f11_2d_query5;
+	};
+
+	u8 f11_2d_query6;
+
+	union {
+		struct {
+			u8 has_single_tap:1;
+			u8 has_tap_n_hold:1;
+			u8 has_double_tap:1;
+			u8 has_early_tap:1;
+			u8 has_flick:1;
+			u8 has_press:1;
+			u8 has_pinch:1;
+			u8 padding:1;
+
+			u8 has_palm_det:1;
+			u8 has_rotate:1;
+			u8 has_touch_shapes:1;
+			u8 has_scroll_zones:1;
+			u8 has_individual_scroll_zones:1;
+			u8 has_multi_finger_scroll:1;
+		};
+		u8 f11_2d_query7__8[2];
+	};
+
+	/* Empty */
+	u8 f11_2d_query9;
+
+	union {
+		struct {
+			u8 nbr_touch_shapes:5;
+		};
+		u8 f11_2d_query10;
+	};
+};
+
+struct f11_2d_data_0 {
+	u8 finger_n;
+};
+
+struct f11_2d_data_1_5 {
+	u8 x_msb;
+	u8 y_msb;
+	u8 x_lsb:4;
+	u8 y_lsb:4;
+	u8 w_y:4;
+	u8 w_x:4;
+	u8 z;
+};
+
+struct f11_2d_data_6_7 {
+	s8 delta_x;
+	s8 delta_y;
+};
+
+struct f11_2d_data_8 {
+	u8 single_tap:1;
+	u8 tap_and_hold:1;
+	u8 double_tap:1;
+	u8 early_tap:1;
+	u8 flick:1;
+	u8 press:1;
+	u8 pinch:1;
+};
+
+struct f11_2d_data_9 {
+	u8 palm_detect:1;
+	u8 rotate:1;
+	u8 shape:1;
+	u8 scrollzone:1;
+	u8 finger_count:3;
+};
+
+struct f11_2d_data_10 {
+	u8 pinch_motion;
+};
+
+struct f11_2d_data_10_12 {
+	u8 x_flick_dist;
+	u8 y_flick_dist;
+	u8 flick_time;
+};
+
+struct f11_2d_data_11_12 {
+	u8 motion;
+	u8 finger_separation;
+};
+
+struct f11_2d_data_13 {
+	u8 shape_n;
+};
+
+struct f11_2d_data_14_15 {
+	u8 horizontal;
+	u8 vertical;
+};
+
+struct f11_2d_data_14_17 {
+	u8 x_low;
+	u8 y_right;
+	u8 x_upper;
+	u8 y_left;
+};
+
+struct f11_2d_data {
+	const struct f11_2d_data_0	*f_state;
+	const struct f11_2d_data_1_5	*abs_pos;
+	const struct f11_2d_data_6_7	*rel_pos;
+	const struct f11_2d_data_8	*gest_1;
+	const struct f11_2d_data_9	*gest_2;
+	const struct f11_2d_data_10	*pinch;
+	const struct f11_2d_data_10_12	*flick;
+	const struct f11_2d_data_11_12	*rotate;
+	const struct f11_2d_data_13	*shapes;
+	const struct f11_2d_data_14_15	*multi_scroll;
+	const struct f11_2d_data_14_17	*scroll_zones;
+};
+
+struct f11_2d_sensor {
+	struct rmi_f11_2d_axis_alignment axis_align;
+	struct f11_2d_sensor_query sens_query;
+	struct f11_2d_data data;
+	u16 max_x;
+	u16 max_y;
+	u8 nbr_fingers;
+	u8 finger_tracker[F11_MAX_NUM_OF_FINGERS];
+	u8 *data_pkt;
+	int pkt_size;
+	u8 sensor_index;
+	char input_name[MAX_LEN];
+	char input_phys[MAX_LEN];
+
+	struct input_dev *input;
+	struct input_dev *mouse_input;
+};
+
+struct f11_data {
+	struct f11_2d_device_query dev_query;
+	struct rmi_f11_2d_ctrl dev_controls;
+	struct f11_2d_sensor sensors[F11_MAX_NUM_OF_SENSORS];
+};
+
+enum finger_state_values {
+	F11_NO_FINGER	= 0x00,
+	F11_PRESENT	= 0x01,
+	F11_INACCURATE	= 0x02,
+	F11_RESERVED	= 0x03
+};
+
+static void rmi_f11_rel_pos_report(struct f11_2d_sensor *sensor, u8 n_finger)
+{
+	struct f11_2d_data *data = &sensor->data;
+	struct rmi_f11_2d_axis_alignment *axis_align = &sensor->axis_align;
+	s8 x, y;
+	s8 temp;
+
+	x = data->rel_pos[n_finger].delta_x;
+	y = data->rel_pos[n_finger].delta_y;
+
+	x = min(F11_REL_POS_MAX, max(F11_REL_POS_MIN, (int)x));
+	y = min(F11_REL_POS_MAX, max(F11_REL_POS_MIN, (int)y));
+
+	if (axis_align->swap_axes) {
+		temp = x;
+		x = y;
+		y = temp;
+	}
+	if (axis_align->flip_x)
+		x = min(F11_REL_POS_MAX, -x);
+	if (axis_align->flip_y)
+		y = min(F11_REL_POS_MAX, -y);
+
+	if (x || y) {
+		input_report_rel(sensor->input, REL_X, x);
+		input_report_rel(sensor->input, REL_Y, y);
+		input_report_rel(sensor->mouse_input, REL_X, x);
+		input_report_rel(sensor->mouse_input, REL_Y, y);
+	}
+	input_sync(sensor->mouse_input);
+}
+
+static void rmi_f11_abs_pos_report(struct f11_2d_sensor *sensor,
+					u8 finger_state, u8 n_finger)
+{
+	struct f11_2d_data *data = &sensor->data;
+	struct rmi_f11_2d_axis_alignment *axis_align = &sensor->axis_align;
+	int prev_state = sensor->finger_tracker[n_finger];
+	int x, y, z;
+	int w_x, w_y, w_max, w_min, orient;
+	int temp;
+	if (prev_state && !finger_state) {
+		/* this is a release */
+		x = y = z = w_max = w_min = orient = 0;
+	} else if (!prev_state && !finger_state) {
+		/* nothing to report */
+		return;
+	} else {
+		x = ((data->abs_pos[n_finger].x_msb << 4) |
+			data->abs_pos[n_finger].x_lsb);
+		y = ((data->abs_pos[n_finger].y_msb << 4) |
+			data->abs_pos[n_finger].y_lsb);
+		z = data->abs_pos[n_finger].z;
+		w_x = data->abs_pos[n_finger].w_x;
+		w_y = data->abs_pos[n_finger].w_y;
+		w_max = max(w_x, w_y);
+		w_min = min(w_x, w_y);
+
+		if (axis_align->swap_axes) {
+			temp = x;
+			x = y;
+			y = temp;
+			temp = w_x;
+			w_x = w_y;
+			w_y = temp;
+		}
+
+		orient = w_x > w_y ? 1 : 0;
+
+		if (axis_align->flip_x)
+			x = max(sensor->max_x - x, 0);
+
+		if (axis_align->flip_y)
+			y = max(sensor->max_y - y, 0);
+
+		/*
+		** here checking if X offset or y offset are specified is
+		**  redundant.  We just add the offsets or, clip the values
+		**
+		** note: offsets need to be done before clipping occurs,
+		** or we could get funny values that are outside
+		** clipping boundaries.
+		*/
+		x += axis_align->offset_X;
+		y += axis_align->offset_Y;
+		x =  max(axis_align->clip_X_low, x);
+		y =  max(axis_align->clip_Y_low, y);
+		if (axis_align->clip_X_high)
+			x = min(axis_align->clip_X_high, x);
+		if (axis_align->clip_Y_high)
+			y =  min(axis_align->clip_Y_high, y);
+
+	}
+
+	pr_debug("%s: f_state[%d]:%d - x:%d y:%d z:%d w_max:%d w_min:%d\n",
+		__func__, n_finger, finger_state, x, y, z, w_max, w_min);
+
+
+#ifdef ABS_MT_PRESSURE
+	input_report_abs(sensor->input, ABS_MT_PRESSURE, z);
+#endif
+	input_report_abs(sensor->input, ABS_MT_TOUCH_MAJOR, w_max);
+	input_report_abs(sensor->input, ABS_MT_TOUCH_MINOR, w_min);
+	input_report_abs(sensor->input, ABS_MT_ORIENTATION, orient);
+	input_report_abs(sensor->input, ABS_MT_POSITION_X, x);
+	input_report_abs(sensor->input, ABS_MT_POSITION_Y, y);
+	input_report_abs(sensor->input, ABS_MT_TRACKING_ID, n_finger);
+
+	/* MT sync between fingers */
+	input_mt_sync(sensor->input);
+	sensor->finger_tracker[n_finger] = finger_state;
+}
+
+static void rmi_f11_finger_handler(struct f11_2d_sensor *sensor)
+{
+	const struct f11_2d_data_0 *f_state = sensor->data.f_state;
+	u8 finger_state;
+	u8 finger_pressed_count;
+	u8 i;
+
+	for (i = 0, finger_pressed_count = 0; i < sensor->nbr_fingers; i++) {
+		/* Possible of having 4 fingers per f_statet register */
+		finger_state = (f_state[i >> 2].finger_n &
+					F11_FINGER_STATE_MASK_N(i));
+		finger_state = F11_FINGER_STATE_VAL_N(finger_state, i);
+
+		if (finger_state == F11_RESERVED) {
+			pr_err("%s: Invalid finger state[%d]:0x%02x.", __func__,
+					i, finger_state);
+			continue;
+		} else if ((finger_state == F11_PRESENT) ||
+				(finger_state == F11_INACCURATE)) {
+			finger_pressed_count++;
+		}
+
+		if (sensor->data.abs_pos)
+			rmi_f11_abs_pos_report(sensor, finger_state, i);
+
+		if (sensor->data.rel_pos)
+			rmi_f11_rel_pos_report(sensor, i);
+	}
+	input_report_key(sensor->input, BTN_TOUCH, finger_pressed_count);
+	input_sync(sensor->input);
+}
+
+static inline int rmi_f11_2d_construct_data(struct f11_2d_sensor *sensor)
+{
+	struct f11_2d_sensor_query *query = &sensor->sens_query;
+	struct f11_2d_data *data = &sensor->data;
+	int i;
+
+	sensor->nbr_fingers = (query->number_of_fingers == 5 ? 10 :
+				query->number_of_fingers + 1);
+
+	sensor->pkt_size = F11_CEIL(sensor->nbr_fingers, 4);
+
+	if (query->has_abs)
+		sensor->pkt_size += (sensor->nbr_fingers * 5);
+
+	if (query->has_rel)
+		sensor->pkt_size +=  (sensor->nbr_fingers * 2);
+
+	/* Check if F11_2D_Query7 is non-zero */
+	if (query->f11_2d_query7__8[0])
+		sensor->pkt_size += sizeof(u8);
+
+	/* Check if F11_2D_Query7 or F11_2D_Query8 is non-zero */
+	if (query->f11_2d_query7__8[0] || query->f11_2d_query7__8[1])
+		sensor->pkt_size += sizeof(u8);
+
+	if (query->has_pinch || query->has_flick || query->has_rotate) {
+		sensor->pkt_size += 3;
+		if (!query->has_flick)
+			sensor->pkt_size--;
+		if (!query->has_rotate)
+			sensor->pkt_size--;
+	}
+
+	if (query->has_touch_shapes)
+		sensor->pkt_size += F11_CEIL(query->nbr_touch_shapes + 1, 8);
+
+	sensor->data_pkt = kzalloc(sensor->pkt_size, GFP_KERNEL);
+	if (!sensor->data_pkt)
+		return -ENOMEM;
+
+	data->f_state = (struct f11_2d_data_0 *)sensor->data_pkt;
+	i = F11_CEIL(sensor->nbr_fingers, 4);
+
+	if (query->has_abs) {
+		data->abs_pos = (struct f11_2d_data_1_5 *)
+				&sensor->data_pkt[i];
+		i += (sensor->nbr_fingers * 5);
+	}
+
+	if (query->has_rel) {
+		data->rel_pos = (struct f11_2d_data_6_7 *)
+				&sensor->data_pkt[i];
+		i += (sensor->nbr_fingers * 2);
+	}
+
+	if (query->f11_2d_query7__8[0]) {
+		data->gest_1 = (struct f11_2d_data_8 *)&sensor->data_pkt[i];
+		i++;
+	}
+
+	if (query->f11_2d_query7__8[0] || query->f11_2d_query7__8[1]) {
+		data->gest_2 = (struct f11_2d_data_9 *)&sensor->data_pkt[i];
+		i++;
+	}
+
+	if (query->has_pinch) {
+		data->pinch = (struct f11_2d_data_10 *)&sensor->data_pkt[i];
+		i++;
+	}
+
+	if (query->has_flick) {
+		if (query->has_pinch) {
+			data->flick = (struct f11_2d_data_10_12 *)data->pinch;
+			i += 2;
+		} else {
+			data->flick = (struct f11_2d_data_10_12 *)
+					&sensor->data_pkt[i];
+			i += 3;
+		}
+	}
+
+	if (query->has_rotate) {
+		if (query->has_flick) {
+			data->rotate = (struct f11_2d_data_11_12 *)
+					(data->flick + 1);
+		} else {
+			data->rotate = (struct f11_2d_data_11_12 *)
+					&sensor->data_pkt[i];
+			i += 2;
+		}
+	}
+
+	if (query->has_touch_shapes)
+		data->shapes = (struct f11_2d_data_13 *)&sensor->data_pkt[i];
+
+	return 0;
+}
+
+static int rmi_f11_read_control_parameters(struct rmi_device *rmi_dev,
+					   struct f11_2d_device_query *query,
+					   struct rmi_f11_2d_ctrl *ctrl,
+					   int ctrl_base_addr) {
+	int read_address = ctrl_base_addr;
+	int error = 0;
+
+	if (ctrl->ctrl0) {
+		error = rmi_read_block(rmi_dev, read_address, &ctrl->ctrl0->reg,
+			sizeof(union rmi_f11_2d_ctrl0));
+		if (error < 0) {
+			dev_err(&rmi_dev->dev,
+				"Failed to read F11 ctrl0, code: %d.\n", error);
+			return error;
+		}
+		read_address = read_address + sizeof(union rmi_f11_2d_ctrl0);
+	}
+
+	if (ctrl->ctrl1) {
+		error = rmi_read_block(rmi_dev, read_address, &ctrl->ctrl1->reg,
+			sizeof(union rmi_f11_2d_ctrl1));
+		if (error < 0) {
+			dev_err(&rmi_dev->dev,
+				"Failed to read F11 ctrl1, code: %d.\n", error);
+			return error;
+		}
+		read_address = read_address + sizeof(union rmi_f11_2d_ctrl1);
+	}
+
+	if (ctrl->ctrl2__3) {
+		error = rmi_read_block(rmi_dev, read_address,
+				ctrl->ctrl2__3->regs,
+				sizeof(ctrl->ctrl2__3->regs));
+		if (error < 0) {
+			dev_err(&rmi_dev->dev,
+				"Failed to read F11 ctrl2__3, code: %d.\n",
+				error);
+			return error;
+		}
+		read_address = read_address + sizeof(ctrl->ctrl2__3->regs);
+	}
+
+	if (ctrl->ctrl4) {
+		error = rmi_read_block(rmi_dev, read_address, &ctrl->ctrl4->reg,
+			sizeof(ctrl->ctrl4->reg));
+		if (error < 0) {
+			dev_err(&rmi_dev->dev,
+				"Failed to read F11 ctrl4, code: %d.\n", error);
+			return error;
+		}
+		read_address = read_address + sizeof(ctrl->ctrl4->reg);
+	}
+
+	if (ctrl->ctrl5) {
+		error = rmi_read_block(rmi_dev, read_address, &ctrl->ctrl5->reg,
+			sizeof(ctrl->ctrl5->reg));
+		if (error < 0) {
+			dev_err(&rmi_dev->dev,
+				"Failed to read F11 ctrl5, code: %d.\n", error);
+			return error;
+		}
+		read_address = read_address + sizeof(ctrl->ctrl5->reg);
+	}
+
+	if (ctrl->ctrl6__7) {
+		error = rmi_read_block(rmi_dev, read_address,
+				ctrl->ctrl6__7->regs,
+				sizeof(ctrl->ctrl6__7->regs));
+		if (error < 0) {
+			dev_err(&rmi_dev->dev,
+				"Failed to read F11 ctrl6__7, code: %d.\n",
+				error);
+			return error;
+		}
+		read_address = read_address + sizeof(ctrl->ctrl6__7->regs);
+	}
+
+	if (ctrl->ctrl8__9) {
+		error = rmi_read_block(rmi_dev, read_address,
+				ctrl->ctrl8__9->regs,
+				sizeof(ctrl->ctrl8__9->regs));
+		if (error < 0) {
+			dev_err(&rmi_dev->dev,
+				"Failed to read F11 ctrl8__9, code: %d.\n",
+				error);
+			return error;
+		}
+		read_address = read_address + sizeof(ctrl->ctrl8__9->regs);
+	}
+
+	return 0;
+}
+
+static int rmi_f11_initialize_control_parameters(struct rmi_device *rmi_dev,
+					   struct f11_2d_device_query *query,
+					   struct rmi_f11_2d_ctrl *ctrl,
+					   int ctrl_base_addr) {
+	int error = 0;
+
+	ctrl->ctrl0 = kzalloc(sizeof(union rmi_f11_2d_ctrl0), GFP_KERNEL);
+	if (!ctrl->ctrl0) {
+		dev_err(&rmi_dev->dev, "Failed to allocate F11 ctrl0.\n");
+		error = -ENOMEM;
+		goto error_exit;
+	}
+
+	ctrl->ctrl1 = kzalloc(sizeof(union rmi_f11_2d_ctrl1), GFP_KERNEL);
+	if (!ctrl->ctrl1) {
+		dev_err(&rmi_dev->dev, "Failed to allocate F11 ctrl1.\n");
+		error = -ENOMEM;
+		goto error_exit;
+	}
+
+	ctrl->ctrl2__3 = kzalloc(sizeof(union rmi_f11_2d_ctrl2__3), GFP_KERNEL);
+	if (!ctrl->ctrl2__3) {
+		dev_err(&rmi_dev->dev, "Failed to allocate F11 ctrl2__3.\n");
+		error = -ENOMEM;
+		goto error_exit;
+	}
+
+	ctrl->ctrl4 = kzalloc(sizeof(union rmi_f11_2d_ctrl4), GFP_KERNEL);
+	if (!ctrl->ctrl4) {
+		dev_err(&rmi_dev->dev, "Failed to allocate F11 ctrl4.\n");
+		error = -ENOMEM;
+		goto error_exit;
+	}
+
+	ctrl->ctrl5 = kzalloc(sizeof(union rmi_f11_2d_ctrl5), GFP_KERNEL);
+	if (!ctrl->ctrl5) {
+		dev_err(&rmi_dev->dev, "Failed to allocate F11 ctrl5.\n");
+		error = -ENOMEM;
+		goto error_exit;
+	}
+
+	ctrl->ctrl6__7 = kzalloc(sizeof(union rmi_f11_2d_ctrl6__7), GFP_KERNEL);
+	if (!ctrl->ctrl6__7) {
+		dev_err(&rmi_dev->dev, "Failed to allocate F11 ctrl6__7.\n");
+		error = -ENOMEM;
+		goto error_exit;
+	}
+
+	ctrl->ctrl8__9 = kzalloc(sizeof(union rmi_f11_2d_ctrl8__9), GFP_KERNEL);
+	if (!ctrl->ctrl8__9) {
+		dev_err(&rmi_dev->dev, "Failed to allocate F11 ctrl8__9.\n");
+		error = -ENOMEM;
+		goto error_exit;
+	}
+
+	return rmi_f11_read_control_parameters(rmi_dev, query,
+					       ctrl, ctrl_base_addr);
+
+error_exit:
+	kfree(ctrl->ctrl0);
+	kfree(ctrl->ctrl1);
+	kfree(ctrl->ctrl2__3);
+	kfree(ctrl->ctrl4);
+	kfree(ctrl->ctrl5);
+	kfree(ctrl->ctrl6__7);
+	kfree(ctrl->ctrl8__9);
+
+	return error;
+}
+
+static inline int rmi_f11_set_control_parameters(struct rmi_device *rmi_dev,
+					struct f11_2d_sensor_query *query,
+					struct rmi_f11_2d_ctrl *ctrl,
+					int ctrl_base_addr)
+{
+	int write_address = ctrl_base_addr;
+	int error;
+
+	if (ctrl->ctrl0) {
+		error = rmi_write_block(rmi_dev, write_address,
+					&ctrl->ctrl0->reg,
+					1);
+		if (error < 0)
+			return error;
+		write_address++;
+	}
+
+	if (ctrl->ctrl1) {
+		error = rmi_write_block(rmi_dev, write_address,
+					&ctrl->ctrl1->reg,
+					1);
+		if (error < 0)
+			return error;
+		write_address++;
+	}
+
+	if (ctrl->ctrl2__3) {
+		error = rmi_write_block(rmi_dev, write_address,
+					ctrl->ctrl2__3->regs,
+					sizeof(ctrl->ctrl2__3->regs));
+		if (error < 0)
+			return error;
+		write_address += sizeof(ctrl->ctrl2__3->regs);
+	}
+
+	if (ctrl->ctrl4) {
+		error = rmi_write_block(rmi_dev, write_address,
+					&ctrl->ctrl4->reg,
+					1);
+		if (error < 0)
+			return error;
+		write_address++;
+	}
+
+	if (ctrl->ctrl5) {
+		error = rmi_write_block(rmi_dev, write_address,
+					&ctrl->ctrl5->reg,
+					1);
+		if (error < 0)
+			return error;
+		write_address++;
+	}
+
+	if (ctrl->ctrl6__7) {
+		error = rmi_write_block(rmi_dev, write_address,
+					&ctrl->ctrl6__7->regs[0],
+					sizeof(ctrl->ctrl6__7->regs));
+		if (error < 0)
+			return error;
+		write_address += sizeof(ctrl->ctrl6__7->regs);
+	}
+
+	if (ctrl->ctrl8__9) {
+		error = rmi_write_block(rmi_dev, write_address,
+					&ctrl->ctrl8__9->regs[0],
+					sizeof(ctrl->ctrl8__9->regs));
+		if (error < 0)
+			return error;
+		write_address += sizeof(ctrl->ctrl8__9->regs);
+	}
+
+	if (ctrl->ctrl10) {
+		error = rmi_write_block(rmi_dev, write_address,
+					&ctrl->ctrl10->reg,
+					1);
+		if (error < 0)
+			return error;
+		write_address++;
+	}
+
+	if (ctrl->ctrl11) {
+		error = rmi_write_block(rmi_dev, write_address,
+					&ctrl->ctrl11->reg,
+					1);
+		if (error < 0)
+			return error;
+		write_address++;
+	}
+
+	if (ctrl->ctrl12 && ctrl->ctrl12_size && query->configurable) {
+		if (ctrl->ctrl12_size > query->max_electrodes) {
+			dev_err(&rmi_dev->dev,
+				"%s: invalid cfg size:%d, should be < %d.\n",
+				__func__, ctrl->ctrl12_size,
+				query->max_electrodes);
+			return -EINVAL;
+		}
+		error = rmi_write_block(rmi_dev, write_address,
+						&ctrl->ctrl12->reg,
+						ctrl->ctrl12_size);
+		if (error < 0)
+			return error;
+		write_address += ctrl->ctrl12_size;
+	}
+
+	if (ctrl->ctrl14) {
+		error = rmi_write_block(rmi_dev,
+					write_address,
+					&ctrl->ctrl0->reg,
+					1);
+		if (error < 0)
+			return error;
+		write_address++;
+	}
+
+	if (ctrl->ctrl15) {
+		error = rmi_write_block(rmi_dev, write_address,
+					ctrl->ctrl15,
+					1);
+		if (error < 0)
+			return error;
+		write_address++;
+	}
+
+	if (ctrl->ctrl16) {
+		error = rmi_write_block(rmi_dev, write_address,
+					ctrl->ctrl16,
+					1);
+		if (error < 0)
+			return error;
+		write_address++;
+	}
+
+	if (ctrl->ctrl17) {
+		error = rmi_write_block(rmi_dev, write_address,
+					ctrl->ctrl17,
+					1);
+		if (error < 0)
+			return error;
+		write_address++;
+	}
+
+	if (ctrl->ctrl18) {
+		error = rmi_write_block(rmi_dev, write_address,
+					ctrl->ctrl18,
+					1);
+		if (error < 0)
+			return error;
+		write_address++;
+	}
+
+	if (ctrl->ctrl19) {
+		error = rmi_write_block(rmi_dev, write_address,
+					ctrl->ctrl19,
+					1);
+		if (error < 0)
+			return error;
+		write_address++;
+	}
+
+	return 0;
+}
+
+static inline int rmi_f11_get_query_parameters(struct rmi_device *rmi_dev,
+			struct f11_2d_sensor_query *query, u8 query_base_addr)
+{
+	int query_size;
+	int rc;
+
+	rc = rmi_read_block(rmi_dev, query_base_addr, query->f11_2d_query1__4,
+					sizeof(query->f11_2d_query1__4));
+	if (rc < 0)
+		return rc;
+	query_size = rc;
+
+	if (query->has_abs) {
+		rc = rmi_read(rmi_dev, query_base_addr + query_size,
+					&query->f11_2d_query5);
+		if (rc < 0)
+			return rc;
+		query_size++;
+	}
+
+	if (query->has_rel) {
+		rc = rmi_read(rmi_dev, query_base_addr + query_size,
+					&query->f11_2d_query6);
+		if (rc < 0)
+			return rc;
+		query_size++;
+	}
+
+	if (query->has_gestures) {
+		rc = rmi_read_block(rmi_dev, query_base_addr + query_size,
+					query->f11_2d_query7__8,
+					sizeof(query->f11_2d_query7__8));
+		if (rc < 0)
+			return rc;
+		query_size += sizeof(query->f11_2d_query7__8);
+	}
+
+	if (query->has_touch_shapes) {
+		rc = rmi_read(rmi_dev, query_base_addr + query_size,
+					&query->f11_2d_query10);
+		if (rc < 0)
+			return rc;
+		query_size++;
+	}
+
+	return query_size;
+}
+
+/* This operation is done in a number of places, so we have a handy routine
+ * for it.
+ */
+static void f11_set_abs_params(struct rmi_function_container *fc, int index)
+{
+	struct f11_data *instance_data =  fc->data;
+	struct input_dev *input = instance_data->sensors[index].input;
+	int device_x_max =
+		instance_data->dev_controls.ctrl6__7->sensor_max_x_pos;
+	int device_y_max =
+		instance_data->dev_controls.ctrl8__9->sensor_max_y_pos;
+	int x_min, x_max, y_min, y_max;
+
+	if (instance_data->sensors[index].axis_align.swap_axes) {
+		int temp = device_x_max;
+		device_x_max = device_y_max;
+		device_y_max = temp;
+	}
+
+	/* Use the max X and max Y read from the device, or the clip values,
+	 * whichever is stricter.
+	 */
+	x_min = instance_data->sensors[index].axis_align.clip_X_low;
+	if (instance_data->sensors[index].axis_align.clip_X_high)
+		x_max = min((int) device_x_max,
+			instance_data->sensors[index].axis_align.clip_X_high);
+	else
+		x_max = device_x_max;
+
+	y_min = instance_data->sensors[index].axis_align.clip_Y_low;
+	if (instance_data->sensors[index].axis_align.clip_Y_high)
+		y_max = min((int) device_y_max,
+			instance_data->sensors[index].axis_align.clip_Y_high);
+	else
+		y_max = device_y_max;
+
+	dev_dbg(&fc->dev, "Set ranges X=[%d..%d] Y=[%d..%d].",
+			x_min, x_max, y_min, y_max);
+
+#ifdef ABS_MT_PRESSURE
+		input_set_abs_params(input, ABS_MT_PRESSURE, 0,
+				DEFAULT_MAX_ABS_MT_PRESSURE, 0, 0);
+#endif
+		input_set_abs_params(input, ABS_MT_TOUCH_MAJOR,
+				0, DEFAULT_MAX_ABS_MT_TOUCH, 0, 0);
+		input_set_abs_params(input, ABS_MT_TOUCH_MINOR,
+				0, DEFAULT_MAX_ABS_MT_TOUCH, 0, 0);
+		input_set_abs_params(input, ABS_MT_ORIENTATION,
+				0, DEFAULT_MAX_ABS_MT_ORIENTATION, 0, 0);
+		input_set_abs_params(input, ABS_MT_TRACKING_ID,
+				DEFAULT_MIN_ABS_MT_TRACKING_ID,
+				DEFAULT_MAX_ABS_MT_TRACKING_ID, 0, 0);
+		/* TODO get max_x_pos (and y) from control registers. */
+		input_set_abs_params(input, ABS_MT_POSITION_X,
+				x_min, x_max, 0, 0);
+		input_set_abs_params(input, ABS_MT_POSITION_Y,
+				y_min, y_max, 0, 0);
+}
+
+static int rmi_f11_init(struct rmi_function_container *fc)
+{
+	struct rmi_device *rmi_dev = fc->rmi_dev;
+	struct rmi_device_platform_data *pdata;
+	struct f11_data *f11;
+	struct input_dev *input_dev;
+	struct input_dev *input_dev_mouse;
+	u8 query_offset;
+	u8 query_base_addr;
+	u8 control_base_addr;
+	u16 max_x_pos, max_y_pos, temp;
+	int rc;
+	int i;
+	int retval = 0;
+	int attr_count = 0;
+
+	dev_info(&fc->dev, "Intializing F11 values.");
+
+	/*
+	** init instance data, fill in values and create any sysfs files
+	*/
+	f11 = kzalloc(sizeof(struct f11_data), GFP_KERNEL);
+	if (!f11)
+		return -ENOMEM;
+	fc->data = f11;
+
+	query_base_addr = fc->fd.query_base_addr;
+	control_base_addr = fc->fd.control_base_addr;
+
+	rc = rmi_read(rmi_dev, query_base_addr, &f11->dev_query.f11_2d_query0);
+	if (rc < 0)
+		goto err_free_data;
+
+	rc = rmi_f11_initialize_control_parameters(rmi_dev, &f11->dev_query,
+					&f11->dev_controls, control_base_addr);
+	if (rc < 0) {
+		dev_err(&fc->dev,
+			"Failed to initialize F11 control params.\n");
+		goto err_free_data;
+	}
+
+	query_offset = (query_base_addr + 1);
+	/* Increase with one since number of sensors is zero based */
+	for (i = 0; i < (f11->dev_query.nbr_of_sensors + 1); i++) {
+		f11->sensors[i].sensor_index = i;
+
+		rc = rmi_f11_get_query_parameters(rmi_dev,
+					&f11->sensors[i].sens_query,
+					query_offset);
+		if (rc < 0)
+			goto err_free_data;
+
+		query_offset += rc;
+
+		pdata = to_rmi_platform_data(rmi_dev);
+		if (pdata)
+			f11->sensors[i].axis_align = pdata->axis_align;
+
+		if (pdata && pdata->f11_ctrl) {
+			rc = rmi_f11_set_control_parameters(rmi_dev,
+						&f11->sensors[i].sens_query,
+						pdata->f11_ctrl,
+						control_base_addr);
+			if (rc < 0)
+				goto err_free_data;
+		}
+
+		if (pdata && pdata->f11_ctrl &&
+				pdata->f11_ctrl->ctrl6__7 &&
+				pdata->f11_ctrl->ctrl8__9) {
+			max_x_pos = pdata->f11_ctrl->ctrl6__7->sensor_max_x_pos;
+			max_y_pos = pdata->f11_ctrl->ctrl8__9->sensor_max_y_pos;
+
+		} else {
+			rc = rmi_read_block(rmi_dev,
+			  control_base_addr + F11_CTRL_SENSOR_MAX_X_POS_OFFSET,
+			  (u8 *)&max_x_pos, sizeof(max_x_pos));
+			if (rc < 0)
+				goto err_free_data;
+
+			rc = rmi_read_block(rmi_dev,
+			  control_base_addr + F11_CTRL_SENSOR_MAX_Y_POS_OFFSET,
+			  (u8 *)&max_y_pos, sizeof(max_y_pos));
+			if (rc < 0)
+				goto err_free_data;
+		}
+
+		if (pdata->axis_align.swap_axes) {
+			temp = max_x_pos;
+			max_x_pos = max_y_pos;
+			max_y_pos = temp;
+		}
+		f11->sensors[i].max_x = max_x_pos;
+		f11->sensors[i].max_y = max_y_pos;
+
+		rc = rmi_f11_2d_construct_data(&f11->sensors[i]);
+		if (rc < 0)
+			goto err_free_data;
+
+		input_dev = input_allocate_device();
+		if (!input_dev) {
+			rc = -ENOMEM;
+			goto err_free_data;
+		}
+
+		f11->sensors[i].input = input_dev;
+		/* TODO how to modify the dev name and
+		* phys name for input device */
+		sprintf(f11->sensors[i].input_name, "%sfn%02x",
+			dev_name(&rmi_dev->dev), fc->fd.function_number);
+		input_dev->name = f11->sensors[i].input_name;
+		sprintf(f11->sensors[i].input_phys, "%s/input0",
+			input_dev->name);
+		input_dev->phys = f11->sensors[i].input_phys;
+		input_dev->dev.parent = &rmi_dev->dev;
+		input_set_drvdata(input_dev, f11);
+
+		set_bit(EV_SYN, input_dev->evbit);
+		set_bit(EV_KEY, input_dev->evbit);
+		set_bit(EV_ABS, input_dev->evbit);
+
+		f11_set_abs_params(fc, i);
+
+		dev_dbg(&fc->dev, "%s: Sensor %d hasRel %d.\n",
+			__func__, i, f11->sensors[i].sens_query.has_rel);
+		if (f11->sensors[i].sens_query.has_rel) {
+			set_bit(EV_REL, input_dev->evbit);
+			set_bit(REL_X, input_dev->relbit);
+			set_bit(REL_Y, input_dev->relbit);
+		}
+		rc = input_register_device(input_dev);
+		if (rc < 0)
+			goto err_free_input;
+
+		if (f11->sensors[i].sens_query.has_rel) {
+			/*create input device for mouse events  */
+			input_dev_mouse = input_allocate_device();
+			if (!input_dev_mouse) {
+				rc = -ENOMEM;
+				goto err_free_data;
+			}
+
+			f11->sensors[i].mouse_input = input_dev_mouse;
+			input_dev_mouse->name = "rmi_mouse";
+			input_dev_mouse->phys = "rmi_f11/input0";
+
+			input_dev_mouse->id.vendor  = 0x18d1;
+			input_dev_mouse->id.product = 0x0210;
+			input_dev_mouse->id.version = 0x0100;
+
+			set_bit(EV_REL, input_dev_mouse->evbit);
+			set_bit(REL_X, input_dev_mouse->relbit);
+			set_bit(REL_Y, input_dev_mouse->relbit);
+
+			set_bit(BTN_MOUSE, input_dev_mouse->evbit);
+			/* Register device's buttons and keys */
+			set_bit(EV_KEY, input_dev_mouse->evbit);
+			set_bit(BTN_LEFT, input_dev_mouse->keybit);
+			set_bit(BTN_MIDDLE, input_dev_mouse->keybit);
+			set_bit(BTN_RIGHT, input_dev_mouse->keybit);
+
+			rc = input_register_device(input_dev_mouse);
+			if (rc < 0)
+				goto err_free_input;
+				set_bit(BTN_RIGHT, input_dev_mouse->keybit);
+		}
+
+	}
+
+	dev_info(&fc->dev, "Creating sysfs files.");
+	dev_dbg(&fc->dev, "Creating fn11 sysfs files.");
+
+	/* Set up sysfs device attributes. */
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		if (sysfs_create_file
+		    (&fc->dev.kobj, &attrs[attr_count].attr) < 0) {
+			dev_err(&fc->dev, "Failed to create sysfs file for %s.",
+				 attrs[attr_count].attr.name);
+			retval = -ENODEV;
+			goto err_free_input;
+		}
+	}
+
+	dev_info(&fc->dev, "Done Creating fn11 sysfs files.");
+	return 0;
+
+err_free_input:
+	for (i = 0; i < (f11->dev_query.nbr_of_sensors + 1); i++) {
+		if (f11->sensors[i].input)
+			input_free_device(f11->sensors[i].input);
+		if (f11->sensors[i].sens_query.has_rel &&
+				f11->sensors[i].mouse_input)
+			input_free_device(f11->sensors[i].mouse_input);
+	}
+err_free_data:
+	for (attr_count--; attr_count >= 0; attr_count--)
+		device_remove_file(&fc->rmi_dev->dev, &attrs[attr_count]);
+
+	kfree(f11);
+	return rc;
+}
+
+int rmi_f11_attention(struct rmi_function_container *fc, u8 *irq_bits)
+{
+	struct rmi_device *rmi_dev = fc->rmi_dev;
+	struct f11_data *f11 = fc->data;
+	u8 data_base_addr = fc->fd.data_base_addr;
+	int data_base_addr_offset = 0;
+	int error;
+	int i;
+
+	for (i = 0; i < f11->dev_query.nbr_of_sensors + 1; i++) {
+		error = rmi_read_block(rmi_dev,
+				data_base_addr + data_base_addr_offset,
+				f11->sensors[i].data_pkt,
+				f11->sensors[i].pkt_size);
+		if (error < 0)
+			return error;
+
+		rmi_f11_finger_handler(&f11->sensors[i]);
+		data_base_addr_offset += f11->sensors[i].pkt_size;
+	}
+	return 0;
+}
+
+static void rmi_f11_remove(struct rmi_function_container *fc)
+{
+	struct f11_data *data = fc->data;
+	int i;
+
+	for (i = 0; i < (data->dev_query.nbr_of_sensors + 1); i++) {
+		input_unregister_device(data->sensors[i].input);
+		if (data->sensors[i].sens_query.has_rel)
+			input_unregister_device(data->sensors[i].mouse_input);
+	}
+	kfree(fc->data);
+}
+
+static struct rmi_function_handler function_handler = {
+	.func = 0x11,
+	.init = rmi_f11_init,
+	.attention = rmi_f11_attention,
+	.remove = rmi_f11_remove
+};
+
+static int __init rmi_f11_module_init(void)
+{
+	int error;
+
+	error = rmi_register_function_driver(&function_handler);
+	if (error < 0) {
+		pr_err("%s: register failed!\n", __func__);
+		return error;
+	}
+
+	return 0;
+}
+
+static void __exit rmi_f11_module_exit(void)
+{
+	rmi_unregister_function_driver(&function_handler);
+}
+
+static ssize_t rmi_fn_11_maxPos_show(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f11_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u %u\n",
+			data->sensors[0].max_x, data->sensors[0].max_y);
+}
+
+static ssize_t rmi_fn_11_flip_show(struct device *dev,
+				   struct device_attribute *attr,
+				   char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f11_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u %u\n",
+			data->sensors[0].axis_align.flip_x,
+			data->sensors[0].axis_align.flip_y);
+}
+
+static ssize_t rmi_fn_11_flip_store(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf,
+				    size_t count)
+{
+	struct rmi_function_container *fc;
+	struct f11_data *instance_data;
+	unsigned int new_X, new_Y;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+
+	if (sscanf(buf, "%u %u", &new_X, &new_Y) != 2)
+		return -EINVAL;
+	if (new_X < 0 || new_X > 1 || new_Y < 0 || new_Y > 1)
+		return -EINVAL;
+	instance_data->sensors[0].axis_align.flip_x = new_X;
+	instance_data->sensors[0].axis_align.flip_y = new_Y;
+
+	return count;
+}
+
+static ssize_t rmi_fn_11_swap_show(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f11_data *instance_data;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			instance_data->sensors[0].axis_align.swap_axes);
+}
+
+static ssize_t rmi_fn_11_swap_store(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf, size_t count)
+{
+	struct rmi_function_container *fc;
+	struct f11_data *instance_data;
+	unsigned int newSwap;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+
+	if (sscanf(buf, "%u", &newSwap) != 1)
+		return -EINVAL;
+	if (newSwap < 0 || newSwap > 1)
+		return -EINVAL;
+	instance_data->sensors[0].axis_align.swap_axes = newSwap;
+
+	f11_set_abs_params(fc, 0);
+
+	return count;
+}
+
+static ssize_t rmi_fn_11_relreport_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f11_data *instance_data;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			instance_data->
+			sensors[0].axis_align.rel_report_enabled);
+}
+
+static ssize_t rmi_fn_11_relreport_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf,
+					 size_t count)
+{
+	struct rmi_function_container *fc;
+	struct f11_data *instance_data;
+	unsigned int new_value;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+
+	if (sscanf(buf, "%u", &new_value) != 1)
+		return -EINVAL;
+	if (new_value < 0 || new_value > 1)
+		return -EINVAL;
+	instance_data->sensors[0].axis_align.rel_report_enabled = new_value;
+
+	return count;
+}
+
+static ssize_t rmi_fn_11_offset_show(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f11_data *instance_data;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%d %d\n",
+			instance_data->sensors[0].axis_align.offset_X,
+			instance_data->sensors[0].axis_align.offset_Y);
+}
+
+static ssize_t rmi_fn_11_offset_store(struct device *dev,
+				      struct device_attribute *attr,
+				      const char *buf,
+				      size_t count)
+{
+	struct rmi_function_container *fc;
+	struct f11_data *instance_data;
+	int new_X, new_Y;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+
+	if (sscanf(buf, "%d %d", &new_X, &new_Y) != 2)
+		return -EINVAL;
+	instance_data->sensors[0].axis_align.offset_X = new_X;
+	instance_data->sensors[0].axis_align.offset_Y = new_Y;
+
+	return count;
+}
+
+static ssize_t rmi_fn_11_clip_show(struct device *dev,
+				   struct device_attribute *attr,
+				   char *buf)
+{
+
+	struct rmi_function_container *fc;
+	struct f11_data *instance_data;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u %u %u %u\n",
+			instance_data->sensors[0].axis_align.clip_X_low,
+			instance_data->sensors[0].axis_align.clip_X_high,
+			instance_data->sensors[0].axis_align.clip_Y_low,
+			instance_data->sensors[0].axis_align.clip_Y_high);
+}
+
+static ssize_t rmi_fn_11_clip_store(struct device *dev,
+				    struct device_attribute *attr,
+				    const char *buf,
+				    size_t count)
+{
+	struct rmi_function_container *fc;
+	struct f11_data *instance_data;
+	unsigned int new_X_low, new_X_high, new_Y_low, new_Y_high;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	if (sscanf(buf, "%u %u %u %u",
+		   &new_X_low, &new_X_high, &new_Y_low, &new_Y_high) != 4)
+		return -EINVAL;
+	if (new_X_low < 0 || new_X_low >= new_X_high || new_Y_low < 0
+	    || new_Y_low >= new_Y_high)
+		return -EINVAL;
+	instance_data->sensors[0].axis_align.clip_X_low = new_X_low;
+	instance_data->sensors[0].axis_align.clip_X_high = new_X_high;
+	instance_data->sensors[0].axis_align.clip_Y_low = new_Y_low;
+	instance_data->sensors[0].axis_align.clip_Y_high = new_Y_high;
+
+	/*
+	** for now, we assume this is sensor index 0
+	*/
+	f11_set_abs_params(fc, 0);
+
+	return count;
+}
+
+static ssize_t rmi_f11_rezero_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	struct rmi_function_container *fc = NULL;
+	unsigned int rezero;
+	int retval = 0;
+	/* Command register always reads as 0, so we can just use a local. */
+	union f11_2d_commands commands = {};
+
+	fc = to_rmi_function_container(dev);
+
+	if (sscanf(buf, "%u", &rezero) != 1)
+		return -EINVAL;
+	if (rezero < 0 || rezero > 1)
+		return -EINVAL;
+
+	/* Per spec, 0 has no effect, so we skip it entirely. */
+	if (rezero) {
+		commands.rezero = 1;
+		retval = rmi_write_block(fc->rmi_dev, fc->fd.command_base_addr,
+				&commands.reg, sizeof(commands.reg));
+		if (retval < 0) {
+			dev_err(dev, "%s: failed to issue rezero command, "
+				"error = %d.", __func__, retval);
+			return retval;
+		}
+	}
+
+	return count;
+}
+
+
+module_init(rmi_f11_module_init);
+module_exit(rmi_f11_module_exit);
+
+MODULE_AUTHOR("Stefan Nilsson <stefan.nilsson@unixphere.com>");
+MODULE_DESCRIPTION("RMI F11 module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/rmi4/rmi_f19.c b/drivers/input/touchscreen/rmi4/rmi_f19.c
new file mode 100644
index 000000000000..9ff9ff99f91f
--- /dev/null
+++ b/drivers/input/touchscreen/rmi4/rmi_f19.c
@@ -0,0 +1,1419 @@
+/*
+ * Copyright (c) 2011 Synaptics Incorporated
+ * Copyright (c) 2011 Unixphere
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+#include <linux/kernel.h>
+#include <linux/rmi.h>
+#include <linux/input.h>
+#include <linux/slab.h>
+
+#define QUERY_BASE_INDEX 1
+#define MAX_LEN 256
+
+struct f19_0d_query {
+	union {
+		struct {
+			u8 configurable:1;
+			u8 has_sensitivity_adjust:1;
+			u8 has_hysteresis_threshold:1;
+		};
+		u8 f19_0d_query0;
+	};
+	u8 f19_0d_query1:5;
+};
+
+struct f19_0d_control_0 {
+	union {
+		struct {
+			u8 button_usage:2;
+			u8 filter_mode:2;
+		};
+		u8 f19_0d_control0;
+	};
+};
+
+struct f19_0d_control_1 {
+	u8 int_enabled_button;
+};
+
+struct f19_0d_control_2 {
+	u8 single_button;
+};
+
+struct f19_0d_control_3_4 {
+	u8 sensor_map_button:7;
+	/*u8 sensitivity_button;*/
+};
+
+struct f19_0d_control_5 {
+	u8 sensitivity_adj;
+};
+struct f19_0d_control_6 {
+	u8 hysteresis_threshold;
+};
+
+struct f19_0d_control {
+	struct f19_0d_control_0 *general_control;
+	struct f19_0d_control_1 *button_int_enable;
+	struct f19_0d_control_2 *single_button_participation;
+	struct f19_0d_control_3_4 *sensor_map;
+	struct f19_0d_control_5 *all_button_sensitivity_adj;
+	struct f19_0d_control_6 *all_button_hysteresis_threshold;
+};
+/* data specific to fn $19 that needs to be kept around */
+struct f19_data {
+	struct f19_0d_control *button_control;
+	struct f19_0d_query button_query;
+	u8 button_rezero;
+	bool *button_down;
+	unsigned char button_count;
+	unsigned char button_data_buffer_size;
+	unsigned char *button_data_buffer;
+	unsigned char *button_map;
+	char input_name[MAX_LEN];
+	char input_phys[MAX_LEN];
+	struct input_dev *input;
+};
+
+static ssize_t rmi_f19_button_count_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf);
+
+static ssize_t rmi_f19_button_map_show(struct device *dev,
+				      struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_f19_button_map_store(struct device *dev,
+				       struct device_attribute *attr,
+				       const char *buf, size_t count);
+static ssize_t rmi_f19_rezero_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf);
+static ssize_t rmi_f19_rezero_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count);
+static ssize_t rmi_f19_has_hysteresis_threshold_show(struct device *dev,
+				      struct device_attribute *attr, char *buf);
+static ssize_t rmi_f19_has_sensitivity_adjust_show(struct device *dev,
+				      struct device_attribute *attr, char *buf);
+static ssize_t rmi_f19_configurable_show(struct device *dev,
+				      struct device_attribute *attr, char *buf);
+static ssize_t rmi_f19_filter_mode_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf);
+static ssize_t rmi_f19_filter_mode_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count);
+static ssize_t rmi_f19_button_usage_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf);
+static ssize_t rmi_f19_button_usage_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count);
+static ssize_t rmi_f19_interrupt_enable_button_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf);
+static ssize_t rmi_f19_interrupt_enable_button_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count);
+static ssize_t rmi_f19_single_button_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf);
+static ssize_t rmi_f19_single_button_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count);
+static ssize_t rmi_f19_sensor_map_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf);
+static ssize_t rmi_f19_sensor_map_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count);
+static ssize_t rmi_f19_sensitivity_adjust_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf);
+static ssize_t rmi_f19_sensitivity_adjust_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count);
+static ssize_t rmi_f19_hysteresis_threshold_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf);
+static ssize_t rmi_f19_hysteresis_threshold_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count);
+
+
+static struct device_attribute attrs[] = {
+	__ATTR(button_count, RMI_RO_ATTR,
+		rmi_f19_button_count_show, rmi_store_error),
+	__ATTR(button_map, RMI_RW_ATTR,
+		rmi_f19_button_map_show, rmi_f19_button_map_store),
+	__ATTR(rezero, RMI_RW_ATTR,
+		rmi_f19_rezero_show, rmi_f19_rezero_store),
+	__ATTR(has_hysteresis_threshold, RMI_RO_ATTR,
+		rmi_f19_has_hysteresis_threshold_show, rmi_store_error),
+	__ATTR(has_sensitivity_adjust, RMI_RO_ATTR,
+		rmi_f19_has_sensitivity_adjust_show, rmi_store_error),
+	__ATTR(configurable, RMI_RO_ATTR,
+		rmi_f19_configurable_show, rmi_store_error),
+	__ATTR(filter_mode, RMI_RW_ATTR,
+		rmi_f19_filter_mode_show, rmi_f19_filter_mode_store),
+	__ATTR(button_usage, RMI_RW_ATTR,
+		rmi_f19_button_usage_show, rmi_f19_button_usage_store),
+	__ATTR(interrupt_enable_button, RMI_RW_ATTR,
+		rmi_f19_interrupt_enable_button_show,
+		rmi_f19_interrupt_enable_button_store),
+	__ATTR(single_button, RMI_RW_ATTR,
+		rmi_f19_single_button_show, rmi_f19_single_button_store),
+	__ATTR(sensor_map, RMI_RW_ATTR,
+		rmi_f19_sensor_map_show, rmi_f19_sensor_map_store),
+	__ATTR(sensitivity_adjust, RMI_RW_ATTR,
+		rmi_f19_sensitivity_adjust_show,
+		rmi_f19_sensitivity_adjust_store),
+	__ATTR(hysteresis_threshold, RMI_RW_ATTR,
+		rmi_f19_hysteresis_threshold_show,
+		rmi_f19_hysteresis_threshold_store)
+};
+
+
+int rmi_f19_read_control_parameters(struct rmi_device *rmi_dev,
+	struct f19_0d_control *button_control,
+	unsigned char button_count,
+	unsigned char int_button_enabled_count,
+	u8 ctrl_base_addr)
+{
+	int error = 0;
+	int i;
+
+	if (button_control->general_control) {
+		error = rmi_read_block(rmi_dev, ctrl_base_addr,
+				(u8 *)button_control->general_control,
+				sizeof(struct f19_0d_control_0));
+		if (error < 0) {
+			dev_err(&rmi_dev->dev,
+				"Failed to read f19_0d_control_0, code:"
+				" %d.\n", error);
+			return error;
+		}
+		ctrl_base_addr = ctrl_base_addr +
+				sizeof(struct f19_0d_control_0);
+	}
+
+	if (button_control->button_int_enable) {
+		for (i = 0; i < int_button_enabled_count; i++) {
+			error = rmi_read_block(rmi_dev, ctrl_base_addr,
+				(u8 *)&button_control->button_int_enable[i],
+				sizeof(struct f19_0d_control_1));
+			if (error < 0) {
+				dev_err(&rmi_dev->dev,
+					"Failed to read f19_0d_control_2,"
+					" code: %d.\n", error);
+				return error;
+			}
+			ctrl_base_addr = ctrl_base_addr +
+				sizeof(struct f19_0d_control_1);
+		}
+	}
+
+	if (button_control->single_button_participation) {
+		for (i = 0; i < int_button_enabled_count; i++) {
+			error = rmi_read_block(rmi_dev, ctrl_base_addr,
+					(u8 *)&button_control->
+						single_button_participation[i],
+					sizeof(struct f19_0d_control_2));
+			if (error < 0) {
+				dev_err(&rmi_dev->dev,
+					"Failed to read f19_0d_control_2,"
+					" code: %d.\n", error);
+				return error;
+			}
+			ctrl_base_addr = ctrl_base_addr +
+				sizeof(struct f19_0d_control_2);
+		}
+	}
+
+	if (button_control->sensor_map) {
+		for (i = 0; i < button_count; i++) {
+			error = rmi_read_block(rmi_dev, ctrl_base_addr,
+					(u8 *)&button_control->sensor_map[i],
+					sizeof(struct f19_0d_control_3_4));
+			if (error < 0) {
+				dev_err(&rmi_dev->dev,
+				"Failed to read f19_0d_control_3_4,"
+				" code: %d.\n", error);
+				return error;
+			}
+			ctrl_base_addr = ctrl_base_addr +
+				sizeof(struct f19_0d_control_3_4);
+		}
+	}
+
+	if (button_control->all_button_sensitivity_adj) {
+		error = rmi_read_block(rmi_dev, ctrl_base_addr,
+				(u8 *)button_control->
+					all_button_sensitivity_adj,
+				sizeof(struct f19_0d_control_5));
+		if (error < 0) {
+			dev_err(&rmi_dev->dev,
+				"Failed to read f19_0d_control_5,"
+				" code: %d.\n", error);
+			return error;
+		}
+		ctrl_base_addr = ctrl_base_addr +
+			sizeof(struct f19_0d_control_5);
+	}
+
+	if (button_control->all_button_hysteresis_threshold) {
+		error = rmi_read_block(rmi_dev, ctrl_base_addr,
+				(u8 *)button_control->
+					all_button_hysteresis_threshold,
+				sizeof(struct f19_0d_control_6));
+		if (error < 0) {
+			dev_err(&rmi_dev->dev,
+				"Failed to read f19_0d_control_6,"
+				" code: %d.\n", error);
+			return error;
+		}
+		ctrl_base_addr = ctrl_base_addr +
+			sizeof(struct f19_0d_control_6);
+	}
+	return 0;
+}
+
+
+int rmi_f19_initialize_control_parameters(struct rmi_device *rmi_dev,
+	struct f19_0d_control *button_control,
+	unsigned char button_count,
+	unsigned char int_button_enabled_count,
+	int control_base_addr)
+{
+	int error = 0;
+
+	button_control->general_control =
+		kzalloc(sizeof(struct f19_0d_control_0), GFP_KERNEL);
+	if (!button_control->general_control) {
+		dev_err(&rmi_dev->dev, "Failed to allocate"
+			" f19_0d_control_0.\n");
+		error = -ENOMEM;
+		goto error_exit;
+	}
+
+	button_control->button_int_enable =
+		kzalloc(int_button_enabled_count *
+			sizeof(struct f19_0d_control_2), GFP_KERNEL);
+	if (!button_control->button_int_enable) {
+		dev_err(&rmi_dev->dev, "Failed to allocate f19_0d_control_1.\n");
+		error = -ENOMEM;
+		goto error_exit;
+	}
+
+	button_control->single_button_participation =
+		kzalloc(int_button_enabled_count *
+			sizeof(struct f19_0d_control_2), GFP_KERNEL);
+	if (!button_control->single_button_participation) {
+		dev_err(&rmi_dev->dev, "Failed to allocate"
+			" f19_0d_control_2.\n");
+		error = -ENOMEM;
+		goto error_exit;
+	}
+
+	button_control->sensor_map =
+		kzalloc(button_count *
+			sizeof(struct f19_0d_control_3_4), GFP_KERNEL);
+	if (!button_control->sensor_map) {
+		dev_err(&rmi_dev->dev, "Failed to allocate"
+			" f19_0d_control_3_4.\n");
+		error = -ENOMEM;
+		goto error_exit;
+	}
+
+	button_control->all_button_sensitivity_adj =
+		kzalloc(sizeof(struct f19_0d_control_5), GFP_KERNEL);
+	if (!button_control->all_button_sensitivity_adj) {
+		dev_err(&rmi_dev->dev, "Failed to allocate"
+			" f19_0d_control_5.\n");
+		error = -ENOMEM;
+		goto error_exit;
+	}
+
+	button_control->all_button_hysteresis_threshold =
+		kzalloc(sizeof(struct f19_0d_control_6), GFP_KERNEL);
+	if (!button_control->all_button_hysteresis_threshold) {
+		dev_err(&rmi_dev->dev, "Failed to allocate"
+			" f19_0d_control_6.\n");
+		error = -ENOMEM;
+		goto error_exit;
+	}
+	return rmi_f19_read_control_parameters(rmi_dev, button_control,
+		button_count, int_button_enabled_count, control_base_addr);
+
+error_exit:
+	kfree(button_control->general_control);
+	kfree(button_control->button_int_enable);
+	kfree(button_control->single_button_participation);
+	kfree(button_control->sensor_map);
+	kfree(button_control->all_button_sensitivity_adj);
+	kfree(button_control->all_button_hysteresis_threshold);
+	return error;
+}
+
+static int rmi_f19_init(struct rmi_function_container *fc)
+{
+	struct rmi_device *rmi_dev = fc->rmi_dev;
+	struct rmi_device_platform_data *pdata;
+	struct f19_data *f19;
+	struct input_dev *input_dev;
+	u8 query_base_addr;
+	int rc;
+	int i;
+	int attr_count = 0;
+
+	dev_info(&fc->dev, "Intializing F19 values.");
+
+	f19 = kzalloc(sizeof(struct f19_data), GFP_KERNEL);
+	if (!f19) {
+		dev_err(&fc->dev, "Failed to allocate function data.\n");
+		return -ENOMEM;
+	}
+	pdata = to_rmi_platform_data(rmi_dev);
+	query_base_addr = fc->fd.query_base_addr;
+
+	/* initial all default values for f19 data here */
+	rc = rmi_read(rmi_dev, fc->fd.command_base_addr,
+		(u8 *)&f19->button_rezero);
+	if (rc < 0) {
+		dev_err(&fc->dev, "Failed to read command register.\n");
+		goto err_free_data;
+	}
+
+	f19->button_rezero = f19->button_rezero & 1;
+
+	rc = rmi_read_block(rmi_dev, query_base_addr, (u8 *)&f19->button_query,
+			sizeof(struct f19_0d_query));
+	f19->button_count = f19->button_query.f19_0d_query1;
+
+	if (rc < 0) {
+		dev_err(&fc->dev, "Failed to read query register.\n");
+		goto err_free_data;
+	}
+
+
+	/* Figure out just how much data we'll need to read. */
+	f19->button_down = kcalloc(f19->button_count,
+			sizeof(bool), GFP_KERNEL);
+	if (!f19->button_down) {
+		dev_err(&fc->dev, "Failed to allocate button state buffer.\n");
+		rc = -ENOMEM;
+		goto err_free_data;
+	}
+
+	f19->button_data_buffer_size = (f19->button_count + 7) / 8;
+	f19->button_data_buffer =
+	    kcalloc(f19->button_data_buffer_size,
+		    sizeof(unsigned char), GFP_KERNEL);
+	if (!f19->button_data_buffer) {
+		dev_err(&fc->dev, "Failed to allocate button data buffer.\n");
+		rc = -ENOMEM;
+		goto err_free_data;
+	}
+
+	f19->button_map = kcalloc(f19->button_count,
+				sizeof(unsigned char), GFP_KERNEL);
+	if (!f19->button_map) {
+		dev_err(&fc->dev, "Failed to allocate button map.\n");
+		rc = -ENOMEM;
+		goto err_free_data;
+	}
+
+	if (pdata) {
+		if (pdata->button_map->nbuttons != f19->button_count) {
+			dev_warn(&fc->dev,
+				"Platformdata button map size (%d) != number "
+				"of buttons on device (%d) - ignored.\n",
+				pdata->button_map->nbuttons,
+				f19->button_count);
+		} else if (!pdata->button_map->map) {
+			dev_warn(&fc->dev,
+				 "Platformdata button map is missing!\n");
+		} else {
+			for (i = 0; i < pdata->button_map->nbuttons; i++)
+				f19->button_map[i] = pdata->button_map->map[i];
+		}
+	}
+
+	f19->button_control = kzalloc(sizeof(struct f19_0d_control),
+				GFP_KERNEL);
+
+	rc = rmi_f19_initialize_control_parameters(fc->rmi_dev,
+		f19->button_control, f19->button_count,
+		f19->button_data_buffer_size, fc->fd.control_base_addr);
+	if (rc < 0) {
+		dev_err(&fc->dev,
+			"Failed to initialize F19 control params.\n");
+		goto err_free_data;
+	}
+
+	input_dev = input_allocate_device();
+	if (!input_dev) {
+		dev_err(&fc->dev, "Failed to allocate input device.\n");
+		rc = -ENOMEM;
+		goto err_free_data;
+	}
+
+	f19->input = input_dev;
+	snprintf(f19->input_name, MAX_LEN, "%sfn%02x", dev_name(&rmi_dev->dev),
+		fc->fd.function_number);
+	input_dev->name = f19->input_name;
+	snprintf(f19->input_phys, MAX_LEN, "%s/input0", input_dev->name);
+	input_dev->phys = f19->input_phys;
+	input_dev->dev.parent = &rmi_dev->dev;
+	input_set_drvdata(input_dev, f19);
+
+	/* Set up any input events. */
+	set_bit(EV_SYN, input_dev->evbit);
+	set_bit(EV_KEY, input_dev->evbit);
+	/* set bits for each button... */
+	for (i = 0; i < f19->button_count; i++)
+		set_bit(f19->button_map[i], input_dev->keybit);
+	rc = input_register_device(input_dev);
+	if (rc < 0) {
+		dev_err(&fc->dev, "Failed to register input device.\n");
+		goto err_free_input;
+	}
+
+	dev_dbg(&fc->dev, "Creating sysfs files.\n");
+	/* Set up sysfs device attributes. */
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		if (sysfs_create_file
+		    (&fc->dev.kobj, &attrs[attr_count].attr) < 0) {
+			dev_err(&fc->dev,
+				"Failed to create sysfs file for %s.",
+				attrs[attr_count].attr.name);
+			rc = -ENODEV;
+			goto err_free_data;
+		}
+	}
+	fc->data = f19;
+	return 0;
+
+err_free_input:
+	input_free_device(f19->input);
+
+err_free_data:
+	if (f19) {
+		kfree(f19->button_down);
+		kfree(f19->button_data_buffer);
+		kfree(f19->button_map);
+	}
+	kfree(f19);
+	for (attr_count--; attr_count >= 0; attr_count--)
+		sysfs_remove_file(&fc->dev.kobj,
+				  &attrs[attr_count].attr);
+	return rc;
+}
+
+int rmi_f19_attention(struct rmi_function_container *fc, u8 *irq_bits)
+{
+	struct rmi_device *rmi_dev = fc->rmi_dev;
+	struct f19_data *f19 = fc->data;
+	u8 data_base_addr = fc->fd.data_base_addr;
+	int error;
+	int button;
+
+	/* Read the button data. */
+
+	error = rmi_read_block(rmi_dev, data_base_addr, f19->button_data_buffer,
+			f19->button_data_buffer_size);
+	if (error < 0) {
+		dev_err(&fc->dev, "%s: Failed to read button data registers.\n",
+			__func__);
+		return error;
+	}
+
+	/* Generate events for buttons that change state. */
+	for (button = 0; button < f19->button_count;
+	     button++) {
+		int button_reg;
+		int button_shift;
+		bool button_status;
+
+		/* determine which data byte the button status is in */
+		button_reg = button / 7;
+		/* bit shift to get button's status */
+		button_shift = button % 8;
+		button_status =
+		    ((f19->button_data_buffer[button_reg] >> button_shift)
+			& 0x01) != 0;
+
+		/* if the button state changed from the last time report it
+		 * and store the new state */
+		if (button_status != f19->button_down[button]) {
+			dev_dbg(&fc->dev, "%s: Button %d (code %d) -> %d.\n",
+				__func__, button, f19->button_map[button],
+				 button_status);
+			/* Generate an event here. */
+			input_report_key(f19->input, f19->button_map[button],
+					 button_status);
+			f19->button_down[button] = button_status;
+		}
+	}
+
+	input_sync(f19->input); /* sync after groups of events */
+	return 0;
+}
+
+static void rmi_f19_remove(struct rmi_function_container *fc)
+{
+	struct f19_data *data = fc->data;
+	if (data) {
+		kfree(data->button_down);
+		kfree(data->button_data_buffer);
+		kfree(data->button_map);
+		input_unregister_device(data->input);
+		if (data->button_control) {
+			kfree(data->button_control->general_control);
+			kfree(data->button_control->button_int_enable);
+			kfree(data->button_control->
+				single_button_participation);
+			kfree(data->button_control->sensor_map);
+			kfree(data->button_control->
+				all_button_sensitivity_adj);
+			kfree(data->button_control->
+				all_button_hysteresis_threshold);
+		}
+		kfree(data->button_control);
+	}
+	kfree(fc->data);
+}
+
+static struct rmi_function_handler function_handler = {
+	.func = 0x19,
+	.init = rmi_f19_init,
+	.attention = rmi_f19_attention,
+	.remove = rmi_f19_remove
+};
+
+static int __init rmi_f19_module_init(void)
+{
+	int error;
+
+	error = rmi_register_function_driver(&function_handler);
+	if (error < 0) {
+		pr_err("%s: register failed!\n", __func__);
+		return error;
+	}
+
+	return 0;
+}
+
+static void rmi_f19_module_exit(void)
+{
+	rmi_unregister_function_driver(&function_handler);
+}
+
+static ssize_t rmi_f19_filter_mode_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->button_control->general_control->filter_mode);
+
+}
+
+static ssize_t rmi_f19_filter_mode_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+	unsigned int new_value;
+	int result;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+	if (sscanf(buf, "%u", &new_value) != 1) {
+		dev_err(dev,
+		"%s: Error - filter_mode_store has an "
+		"invalid len.\n",
+		__func__);
+		return -EINVAL;
+	}
+
+	if (new_value < 0 || new_value > 4) {
+		dev_err(dev, "%s: Error - filter_mode_store has an "
+		"invalid value %d.\n",
+		__func__, new_value);
+		return -EINVAL;
+	}
+	data->button_control->general_control->filter_mode = new_value;
+	result = rmi_write_block(fc->rmi_dev, fc->fd.control_base_addr,
+		(u8 *)data->button_control->general_control,
+			sizeof(struct f19_0d_control_0));
+	if (result < 0) {
+		dev_err(dev, "%s : Could not write filter_mode_store to 0x%x\n",
+				__func__, fc->fd.control_base_addr);
+		return result;
+	}
+
+	return count;
+}
+
+static ssize_t rmi_f19_button_usage_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->button_control->general_control->button_usage);
+
+}
+
+static ssize_t rmi_f19_button_usage_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+	unsigned int new_value;
+	int result;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+	if (sscanf(buf, "%u", &new_value) != 1) {
+		dev_err(dev,
+		"%s: Error - button_usage_store has an "
+		"invalid len.\n",
+		__func__);
+		return -EINVAL;
+	}
+
+	if (new_value < 0 || new_value > 4) {
+		dev_err(dev, "%s: Error - button_usage_store has an "
+		"invalid value %d.\n",
+		__func__, new_value);
+		return -EINVAL;
+	}
+	data->button_control->general_control->button_usage = new_value;
+	result = rmi_write_block(fc->rmi_dev, fc->fd.control_base_addr,
+		(u8 *)data->button_control->general_control,
+			sizeof(struct f19_0d_control_0));
+	if (result < 0) {
+		dev_err(dev, "%s : Could not write button_usage_store to 0x%x\n",
+				__func__, fc->fd.control_base_addr);
+		return result;
+	}
+
+	return count;
+
+}
+
+static ssize_t rmi_f19_interrupt_enable_button_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+	int i, len, total_len = 0;
+	char *current_buf = buf;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+	/* loop through each button map value and copy its
+	 * string representation into buf */
+	for (i = 0; i < data->button_count; i++) {
+		int button_reg;
+		int button_shift;
+		int interrupt_button;
+
+		button_reg = i / 7;
+		button_shift = i % 8;
+		interrupt_button =
+		    ((data->button_control->
+			button_int_enable[button_reg].int_enabled_button >>
+				button_shift) & 0x01);
+
+		/* get next button mapping value and write it to buf */
+		len = snprintf(current_buf, PAGE_SIZE - total_len,
+			"%u ", interrupt_button);
+		/* bump up ptr to next location in buf if the
+		 * snprintf was valid.  Otherwise issue an error
+		 * and return. */
+		if (len > 0) {
+			current_buf += len;
+			total_len += len;
+		} else {
+			dev_err(dev, "%s: Failed to build interrupt button"
+				" buffer, code = %d.\n", __func__, len);
+			return snprintf(buf, PAGE_SIZE, "unknown\n");
+		}
+	}
+	len = snprintf(current_buf, PAGE_SIZE - total_len, "\n");
+	if (len > 0)
+		total_len += len;
+	else
+		dev_warn(dev, "%s: Failed to append carriage return.\n",
+			 __func__);
+	return total_len;
+
+}
+
+static ssize_t rmi_f19_interrupt_enable_button_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+	int i;
+	int button_count = 0;
+	int retval = count;
+	int button_reg = 0;
+	int ctrl_bass_addr;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+	for (i = 0; i < data->button_count && *buf != 0;
+	     i++) {
+		int button_shift;
+		int button;
+
+		button_reg = i / 7;
+		button_shift = i % 8;
+		/* get next button mapping value and store and bump up to
+		 * point to next item in buf */
+		sscanf(buf, "%u", &button);
+
+		if (button != 0 && button != 1) {
+			dev_err(dev,
+				"%s: Error - interrupt enable button for"
+				" button %d is not a valid value 0x%x.\n",
+				__func__, i, button);
+			return -EINVAL;
+		}
+
+		if (button_shift == 0)
+			data->button_control->button_int_enable[button_reg].
+				int_enabled_button = 0;
+		data->button_control->button_int_enable[button_reg].
+			int_enabled_button |= (button << button_shift);
+		button_count++;
+		/* bump up buf to point to next item to read */
+		while (*buf != 0) {
+			buf++;
+			if (*(buf - 1) == ' ')
+				break;
+		}
+	}
+
+	/* Make sure the button count matches */
+	if (button_count != data->button_count) {
+		dev_err(dev,
+			"%s: Error - interrupt enable button count of %d"
+			" doesn't match device button count of %d.\n",
+			 __func__, button_count, data->button_count);
+		return -EINVAL;
+	}
+
+	/* write back to the control register */
+	ctrl_bass_addr = fc->fd.control_base_addr +
+			sizeof(struct f19_0d_control_0);
+	retval = rmi_write_block(fc->rmi_dev, ctrl_bass_addr,
+		(u8 *)data->button_control->button_int_enable,
+			sizeof(struct f19_0d_control_1)*(button_reg + 1));
+	if (retval < 0) {
+		dev_err(dev, "%s : Could not write interrupt_enable_store"
+			" to 0x%x\n", __func__, ctrl_bass_addr);
+		return retval;
+	}
+
+	return count;
+}
+
+static ssize_t rmi_f19_single_button_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+	int i, len, total_len = 0;
+	char *current_buf = buf;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+	/* loop through each button map value and copy its
+	 * string representation into buf */
+	for (i = 0; i < data->button_count; i++) {
+		int button_reg;
+		int button_shift;
+		int single_button;
+
+		button_reg = i / 7;
+		button_shift = i % 8;
+		single_button = ((data->button_control->
+			single_button_participation[button_reg].single_button
+			>> button_shift) & 0x01);
+
+		/* get next button mapping value and write it to buf */
+		len = snprintf(current_buf, PAGE_SIZE - total_len,
+			"%u ", single_button);
+		/* bump up ptr to next location in buf if the
+		 * snprintf was valid.  Otherwise issue an error
+		 * and return. */
+		if (len > 0) {
+			current_buf += len;
+			total_len += len;
+		} else {
+			dev_err(dev, "%s: Failed to build signle button buffer"
+				", code = %d.\n", __func__, len);
+			return snprintf(buf, PAGE_SIZE, "unknown\n");
+		}
+	}
+	len = snprintf(current_buf, PAGE_SIZE - total_len, "\n");
+	if (len > 0)
+		total_len += len;
+	else
+		dev_warn(dev, "%s: Failed to append carriage return.\n",
+			 __func__);
+
+	return total_len;
+
+}
+
+static ssize_t rmi_f19_single_button_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+	int i;
+	int button_count = 0;
+	int retval = count;
+	int ctrl_bass_addr;
+	int button_reg = 0;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+	for (i = 0; i < data->button_count && *buf != 0;
+	     i++) {
+		int button_shift;
+		int button;
+
+		button_reg = i / 7;
+		button_shift = i % 8;
+		/* get next button mapping value and store and bump up to
+		 * point to next item in buf */
+		sscanf(buf, "%u", &button);
+
+		if (button != 0 && button != 1) {
+			dev_err(dev,
+				"%s: Error - single button for button %d"
+				" is not a valid value 0x%x.\n",
+				__func__, i, button);
+			return -EINVAL;
+		}
+		if (button_shift == 0)
+			data->button_control->
+				single_button_participation[button_reg].
+				single_button = 0;
+		data->button_control->single_button_participation[button_reg].
+			single_button |=  (button << button_shift);
+		button_count++;
+		/* bump up buf to point to next item to read */
+		while (*buf != 0) {
+			buf++;
+			if (*(buf - 1) == ' ')
+				break;
+		}
+	}
+
+	/* Make sure the button count matches */
+	if (button_count != data->button_count) {
+		dev_err(dev,
+		    "%s: Error - single button count of %d doesn't match"
+		     " device button count of %d.\n", __func__, button_count,
+		     data->button_count);
+		return -EINVAL;
+	}
+	/* write back to the control register */
+	ctrl_bass_addr = fc->fd.control_base_addr +
+		sizeof(struct f19_0d_control_0) +
+		sizeof(struct f19_0d_control_2)*(button_reg + 1);
+	retval = rmi_write_block(fc->rmi_dev, ctrl_bass_addr,
+		(u8 *)data->button_control->single_button_participation,
+			sizeof(struct f19_0d_control_2)*(button_reg + 1));
+	if (retval < 0) {
+		dev_err(dev, "%s : Could not write interrupt_enable_store to"
+			" 0x%x\n", __func__, ctrl_bass_addr);
+		return -EINVAL;
+	}
+	return count;
+}
+
+static ssize_t rmi_f19_sensor_map_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+	int i, len, total_len = 0;
+	char *current_buf = buf;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	for (i = 0; i < data->button_count; i++) {
+		len = snprintf(current_buf, PAGE_SIZE - total_len,
+			"%u ", data->button_control->sensor_map[i].
+			sensor_map_button);
+		/* bump up ptr to next location in buf if the
+		 * snprintf was valid.  Otherwise issue an error
+		 * and return. */
+		if (len > 0) {
+			current_buf += len;
+			total_len += len;
+		} else {
+			dev_err(dev, "%s: Failed to build sensor map buffer, "
+				"code = %d.\n", __func__, len);
+			return snprintf(buf, PAGE_SIZE, "unknown\n");
+		}
+	}
+	len = snprintf(current_buf, PAGE_SIZE - total_len, "\n");
+	if (len > 0)
+		total_len += len;
+	else
+		dev_warn(dev, "%s: Failed to append carriage return.\n",
+			 __func__);
+	return total_len;
+
+
+}
+
+static ssize_t rmi_f19_sensor_map_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+	int sensor_map;
+	int i;
+	int retval = count;
+	int button_count = 0;
+	int ctrl_bass_addr;
+	int button_reg;
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	if (data->button_query.configurable == 0) {
+		dev_err(dev,
+			"%s: Error - sensor map is not configuralbe at"
+			" run-time", __func__);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < data->button_count && *buf != 0; i++) {
+		/* get next button mapping value and store and bump up to
+		 * point to next item in buf */
+		sscanf(buf, "%u", &sensor_map);
+
+		/* Make sure the key is a valid key */
+		if (sensor_map < 0 || sensor_map > 127) {
+			dev_err(dev,
+				"%s: Error - sensor map for button %d is"
+				" not a valid value 0x%x.\n",
+				__func__, i, sensor_map);
+			return -EINVAL;
+		}
+
+		data->button_control->sensor_map[i].sensor_map_button =
+			sensor_map;
+		button_count++;
+
+		/* bump up buf to point to next item to read */
+		while (*buf != 0) {
+			buf++;
+			if (*(buf - 1) == ' ')
+				break;
+		}
+	}
+
+	if (button_count != data->button_count) {
+		dev_err(dev,
+		    "%s: Error - button map count of %d doesn't match device "
+		     "button count of %d.\n", __func__, button_count,
+		     data->button_count);
+		return -EINVAL;
+	}
+
+	/* write back to the control register */
+	button_reg = (button_count / 7) + 1;
+	ctrl_bass_addr = fc->fd.control_base_addr +
+		sizeof(struct f19_0d_control_0) +
+		sizeof(struct f19_0d_control_1)*button_reg +
+		sizeof(struct f19_0d_control_2)*button_reg;
+	retval = rmi_write_block(fc->rmi_dev, ctrl_bass_addr,
+		(u8 *)data->button_control->sensor_map,
+			sizeof(struct f19_0d_control_3_4)*button_count);
+	if (retval < 0) {
+		dev_err(dev, "%s : Could not sensor_map_store to 0x%x\n",
+				__func__, ctrl_bass_addr);
+		return -EINVAL;
+	}
+	return count;
+}
+
+static ssize_t rmi_f19_sensitivity_adjust_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", data->button_control->
+		all_button_sensitivity_adj->sensitivity_adj);
+
+}
+
+static ssize_t rmi_f19_sensitivity_adjust_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+	unsigned int new_value;
+	int len;
+	int ctrl_bass_addr;
+	int button_reg;
+
+	fc = to_rmi_function_container(dev);
+
+	data = fc->data;
+
+	if (data->button_query.configurable == 0) {
+		dev_err(dev,
+			"%s: Error - sensitivity_adjust is not"
+			" configuralbe at run-time", __func__);
+		return -EINVAL;
+	}
+
+	len = sscanf(buf, "%u", &new_value);
+	if (new_value < 0 || new_value > 31)
+		return -EINVAL;
+
+	data->button_control->all_button_sensitivity_adj->sensitivity_adj =
+		 new_value;
+	/* write back to the control register */
+	button_reg = (data->button_count / 7) + 1;
+	ctrl_bass_addr = fc->fd.control_base_addr +
+		sizeof(struct f19_0d_control_0) +
+		sizeof(struct f19_0d_control_1)*button_reg +
+		sizeof(struct f19_0d_control_2)*button_reg +
+		sizeof(struct f19_0d_control_3_4)*data->button_count;
+	len = rmi_write_block(fc->rmi_dev, ctrl_bass_addr,
+		(u8 *)data->button_control->all_button_sensitivity_adj,
+			sizeof(struct f19_0d_control_5));
+	if (len < 0) {
+		dev_err(dev, "%s : Could not sensitivity_adjust_store to"
+			" 0x%x\n", __func__, ctrl_bass_addr);
+		return len;
+	}
+
+	return len;
+}
+
+static ssize_t rmi_f19_hysteresis_threshold_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", data->button_control->
+		all_button_hysteresis_threshold->hysteresis_threshold);
+
+}
+static ssize_t rmi_f19_hysteresis_threshold_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+	unsigned int new_value;
+	int len;
+	int ctrl_bass_addr;
+	int button_reg;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+	len = sscanf(buf, "%u", &new_value);
+	if (new_value < 0 || new_value > 15) {
+		dev_err(dev, "%s: Error - hysteresis_threshold_store has an "
+		"invalid value %d.\n",
+		__func__, new_value);
+		return -EINVAL;
+	}
+	data->button_control->all_button_hysteresis_threshold->
+		hysteresis_threshold = new_value;
+	/* write back to the control register */
+	button_reg = (data->button_count / 7) + 1;
+	ctrl_bass_addr = fc->fd.control_base_addr +
+		sizeof(struct f19_0d_control_0) +
+		sizeof(struct f19_0d_control_1)*button_reg +
+		sizeof(struct f19_0d_control_2)*button_reg +
+		sizeof(struct f19_0d_control_3_4)*data->button_count+
+		sizeof(struct f19_0d_control_5);
+	len = rmi_write_block(fc->rmi_dev, ctrl_bass_addr,
+		(u8 *)data->button_control->all_button_sensitivity_adj,
+			sizeof(struct f19_0d_control_6));
+	if (len < 0) {
+		dev_err(dev, "%s : Could not write all_button hysteresis "
+			"threshold to 0x%x\n", __func__, ctrl_bass_addr);
+		return -EINVAL;
+	}
+
+	return count;
+}
+
+static ssize_t rmi_f19_has_hysteresis_threshold_show(struct device *dev,
+				      struct device_attribute *attr, char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->button_query.has_hysteresis_threshold);
+}
+
+static ssize_t rmi_f19_has_sensitivity_adjust_show(struct device *dev,
+				      struct device_attribute *attr, char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->button_query.has_sensitivity_adjust);
+}
+
+static ssize_t rmi_f19_configurable_show(struct device *dev,
+				      struct device_attribute *attr, char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->button_query.configurable);
+}
+
+static ssize_t rmi_f19_rezero_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->button_rezero);
+
+}
+
+static ssize_t rmi_f19_rezero_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf,
+					 size_t count)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+	unsigned int new_value;
+	int len;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+	len = sscanf(buf, "%u", &new_value);
+	if (new_value != 0 && new_value != 1) {
+		dev_err(dev,
+			"%s: Error - rezero is not a "
+			"valid value 0x%x.\n",
+			__func__, new_value);
+		return -EINVAL;
+	}
+	data->button_rezero = new_value & 1;
+	len = rmi_write(fc->rmi_dev, fc->fd.command_base_addr,
+		data->button_rezero);
+
+	if (len < 0) {
+		dev_err(dev, "%s : Could not write rezero to 0x%x\n",
+				__func__, fc->fd.command_base_addr);
+		return -EINVAL;
+	}
+	return count;
+}
+
+static ssize_t rmi_f19_button_count_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->button_count);
+}
+
+static ssize_t rmi_f19_button_map_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+	int i, len, total_len = 0;
+	char *current_buf = buf;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+	/* loop through each button map value and copy its
+	 * string representation into buf */
+	for (i = 0; i < data->button_count; i++) {
+		/* get next button mapping value and write it to buf */
+		len = snprintf(current_buf, PAGE_SIZE - total_len,
+			"%u ", data->button_map[i]);
+		/* bump up ptr to next location in buf if the
+		 * snprintf was valid.  Otherwise issue an error
+		 * and return. */
+		if (len > 0) {
+			current_buf += len;
+			total_len += len;
+		} else {
+			dev_err(dev, "%s: Failed to build button map buffer, "
+				"code = %d.\n", __func__, len);
+			return snprintf(buf, PAGE_SIZE, "unknown\n");
+		}
+	}
+	len = snprintf(current_buf, PAGE_SIZE - total_len, "\n");
+	if (len > 0)
+		total_len += len;
+	else
+		dev_warn(dev, "%s: Failed to append carriage return.\n",
+			 __func__);
+	return total_len;
+}
+
+static ssize_t rmi_f19_button_map_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf,
+				size_t count)
+{
+	struct rmi_function_container *fc;
+	struct f19_data *data;
+	unsigned int button;
+	int i;
+	int retval = count;
+	int button_count = 0;
+	unsigned char temp_button_map[KEY_MAX];
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	/* Do validation on the button map data passed in.  Store button
+	 * mappings into a temp buffer and then verify button count and
+	 * data prior to clearing out old button mappings and storing the
+	 * new ones. */
+	for (i = 0; i < data->button_count && *buf != 0;
+	     i++) {
+		/* get next button mapping value and store and bump up to
+		 * point to next item in buf */
+		sscanf(buf, "%u", &button);
+
+		/* Make sure the key is a valid key */
+		if (button > KEY_MAX) {
+			dev_err(dev,
+				"%s: Error - button map for button %d is not a"
+				" valid value 0x%x.\n", __func__, i, button);
+			retval = -EINVAL;
+			goto err_ret;
+		}
+
+		temp_button_map[i] = button;
+		button_count++;
+
+		/* bump up buf to point to next item to read */
+		while (*buf != 0) {
+			buf++;
+			if (*(buf - 1) == ' ')
+				break;
+		}
+	}
+
+	/* Make sure the button count matches */
+	if (button_count != data->button_count) {
+		dev_err(dev,
+		    "%s: Error - button map count of %d doesn't match device "
+		     "button count of %d.\n", __func__, button_count,
+		     data->button_count);
+		retval = -EINVAL;
+		goto err_ret;
+	}
+
+	/* Clear the key bits for the old button map. */
+	for (i = 0; i < button_count; i++)
+		clear_bit(data->button_map[i], data->input->keybit);
+
+	/* Switch to the new map. */
+	memcpy(data->button_map, temp_button_map,
+	       data->button_count);
+
+	/* Loop through the key map and set the key bit for the new mapping. */
+	for (i = 0; i < button_count; i++)
+		set_bit(data->button_map[i], data->input->keybit);
+
+err_ret:
+	return retval;
+}
+
+module_init(rmi_f19_module_init);
+module_exit(rmi_f19_module_exit);
+
+MODULE_AUTHOR("Vivian Ly <vly@synaptics.com>");
+MODULE_DESCRIPTION("RMI F19 module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/rmi4/rmi_f34.c b/drivers/input/touchscreen/rmi4/rmi_f34.c
new file mode 100644
index 000000000000..abade244b834
--- /dev/null
+++ b/drivers/input/touchscreen/rmi4/rmi_f34.c
@@ -0,0 +1,821 @@
+/*
+ * Copyright (c) 2011 Synaptics Incorporated
+ * Copyright (c) 2011 Unixphere
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+#include <linux/kernel.h>
+#include <linux/rmi.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include "rmi_driver.h"
+
+/* define fn $34 commands */
+#define WRITE_FW_BLOCK            0x2
+#define ERASE_ALL                 0x3
+#define READ_CONFIG_BLOCK         0x5
+#define WRITE_CONFIG_BLOCK        0x6
+#define ERASE_CONFIG              0x7
+#define ENABLE_FLASH_PROG         0xf
+
+#define STATUS_IN_PROGRESS        0xff
+#define STATUS_IDLE		  0x80
+
+#define PDT_START_SCAN_LOCATION	0x00e9
+#define PDT_END_SCAN_LOCATION	0x0005
+
+#define BLK_SZ_OFF	3
+#define IMG_BLK_CNT_OFF	5
+#define CFG_BLK_CNT_OFF	7
+
+#define BLK_NUM_OFF 2
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 32)
+#define KERNEL_VERSION_ABOVE_2_6_32 1
+#endif
+
+/* data specific to fn $34 that needs to be kept around */
+struct rmi_fn_34_data {
+	unsigned char status;
+	unsigned char cmd;
+	unsigned short bootloaderid;
+	unsigned short blocksize;
+	unsigned short imageblockcount;
+	unsigned short configblockcount;
+	unsigned short blocknum;
+	bool inflashprogmode;
+};
+
+static ssize_t rmi_fn_34_status_show(struct device *dev,
+				     struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_34_cmd_show(struct device *dev,
+				  struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_34_cmd_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t count);
+
+#ifdef KERNEL_VERSION_ABOVE_2_6_32
+static ssize_t rmi_fn_34_data_read(struct file *data_file, struct kobject *kobj,
+				   struct bin_attribute *attributes,
+				   char *buf, loff_t pos, size_t count);
+
+static ssize_t rmi_fn_34_data_write(struct file *data_file,
+				    struct kobject *kobj,
+				    struct bin_attribute *attributes, char *buf,
+				    loff_t pos, size_t count);
+#else
+static ssize_t rmi_fn_34_data_read(struct kobject *kobj,
+				   struct bin_attribute *attributes,
+				   char *buf, loff_t pos, size_t count);
+
+static ssize_t rmi_fn_34_data_write(struct kobject *kobj,
+				    struct bin_attribute *attributes, char *buf,
+				    loff_t pos, size_t count);
+#endif
+
+static ssize_t rmi_fn_34_bootloaderid_show(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf);
+
+static ssize_t rmi_fn_34_bootloaderid_store(struct device *dev,
+					    struct device_attribute *attr,
+					    const char *buf, size_t count);
+
+static ssize_t rmi_fn_34_blocksize_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf);
+
+static ssize_t rmi_fn_34_imageblockcount_show(struct device *dev,
+					      struct device_attribute *attr,
+					      char *buf);
+
+static ssize_t rmi_fn_34_configblockcount_show(struct device *dev,
+					       struct device_attribute *attr,
+					       char *buf);
+
+static ssize_t rmi_fn_34_blocknum_show(struct device *dev,
+				       struct device_attribute *attr,
+				       char *buf);
+
+static ssize_t rmi_fn_34_blocknum_store(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count);
+
+static ssize_t rmi_fn_34_rescanPDT_store(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count);
+
+static struct device_attribute attrs[] = {
+	__ATTR(status, RMI_RO_ATTR,
+	       rmi_fn_34_status_show, rmi_store_error),
+	/* Also, sysfs will need to have a file set up to distinguish
+	 * between commands - like Config write/read, Image write/verify. */
+	__ATTR(cmd, RMI_RO_ATTR,
+	       rmi_fn_34_cmd_show, rmi_fn_34_cmd_store),
+	__ATTR(bootloaderid, RMI_RO_ATTR,
+	       rmi_fn_34_bootloaderid_show, rmi_fn_34_bootloaderid_store),
+	__ATTR(blocksize, RMI_RO_ATTR,
+	       rmi_fn_34_blocksize_show, rmi_store_error),
+	__ATTR(imageblockcount, RMI_RO_ATTR,
+	       rmi_fn_34_imageblockcount_show, rmi_store_error),
+	__ATTR(configblockcount, RMI_RO_ATTR,
+	       rmi_fn_34_configblockcount_show, rmi_store_error),
+	__ATTR(blocknum, RMI_RO_ATTR,
+	       rmi_fn_34_blocknum_show, rmi_fn_34_blocknum_store),
+	__ATTR(rescanPDT, RMI_RO_ATTR,
+	       rmi_show_error, rmi_fn_34_rescanPDT_store)
+};
+
+struct bin_attribute dev_attr_data = {
+	.attr = {
+		 .name = "data",
+		 .mode = 0644},
+	.size = 0,
+	.read = rmi_fn_34_data_read,
+	.write = rmi_fn_34_data_write,
+};
+
+static int rmi_f34_init(struct rmi_function_container *fc)
+{
+	int retval = 0;
+	int attr_count = 0;
+	struct rmi_fn_34_data *f34;
+	u16 query_base_addr;
+	u16 control_base_addr;
+	unsigned char buf[2];
+
+	dev_info(&fc->dev, "Intializing f34 values.");
+
+	/* init instance data, fill in values and create any sysfs files */
+	f34 = kzalloc(sizeof(struct rmi_fn_34_data), GFP_KERNEL);
+	if (!f34) {
+		dev_err(&fc->dev, "Failed to allocate rmi_fn_34_data.\n");
+		return -ENOMEM;
+	}
+
+	fc->data = f34;
+
+	/* get the Bootloader ID and Block Size. */
+	query_base_addr = fc->fd.query_base_addr;
+	control_base_addr = fc->fd.control_base_addr;
+
+	retval = rmi_read_block(fc->rmi_dev, query_base_addr, buf,
+			ARRAY_SIZE(buf));
+
+	if (retval < 0) {
+		dev_err(&fc->dev, "Could not read bootloaderid from 0x%04x.\n",
+			query_base_addr);
+		goto exit_free_data;
+	}
+	batohs(&f34->bootloaderid, buf);
+
+	retval = rmi_read_block(fc->rmi_dev, query_base_addr + BLK_SZ_OFF, buf,
+			ARRAY_SIZE(buf));
+
+	if (retval < 0) {
+		dev_err(&fc->dev, "Could not read block size from 0x%04x, "
+			"error=%d.\n", query_base_addr + BLK_SZ_OFF, retval);
+		goto exit_free_data;
+	}
+	batohs(&f34->blocksize, buf);
+
+	/* Get firmware image block count and store it in the instance data */
+	retval = rmi_read_block(fc->rmi_dev, query_base_addr + IMG_BLK_CNT_OFF,
+			buf, ARRAY_SIZE(buf));
+
+	if (retval < 0) {
+		dev_err(&fc->dev, "Couldn't read image block count from 0x%x, "
+			"error=%d.\n", query_base_addr + IMG_BLK_CNT_OFF,
+			retval);
+		goto exit_free_data;
+	}
+	batohs(&f34->imageblockcount, buf);
+
+	/* Get config block count and store it in the instance data */
+	retval = rmi_read_block(fc->rmi_dev, query_base_addr + 7, buf,
+			ARRAY_SIZE(buf));
+
+	if (retval < 0) {
+		dev_err(&fc->dev, "Couldn't read config block count from 0x%x, "
+			"error=%d.\n", query_base_addr + CFG_BLK_CNT_OFF,
+			retval);
+		goto exit_free_data;
+	}
+	batohs(&f34->configblockcount, buf);
+
+	/* We need a sysfs file for the image/config block to write or read.
+	 * Set up sysfs bin file for binary data block. Since the image is
+	 * already in our format there is no need to convert the data for
+	 * endianess. */
+	retval = sysfs_create_bin_file(&fc->dev.kobj,
+				&dev_attr_data);
+	if (retval < 0) {
+		dev_err(&fc->dev, "Failed to create sysfs file for F34 data "
+		     "(error = %d).\n", retval);
+		retval = -ENODEV;
+		goto exit_free_data;
+	}
+
+	dev_dbg(&fc->dev, "Creating sysfs files.\n");
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		if (sysfs_create_file
+		    (&fc->dev.kobj, &attrs[attr_count].attr) < 0) {
+			dev_err(&fc->dev, "Failed to create sysfs file for %s.",
+				attrs[attr_count].attr.name);
+			retval = -ENODEV;
+			goto exit_free_attrs;
+		}
+	}
+
+	return retval;
+
+exit_free_attrs:
+	for (attr_count--; attr_count >= 0; attr_count--)
+		sysfs_remove_file(&fc->dev.kobj,
+				  &attrs[attr_count].attr);
+exit_free_data:
+	kfree(f34);
+	return retval;
+}
+
+static int f34_read_status(struct rmi_function_container *fc)
+{
+	struct rmi_fn_34_data *instance_data = fc->data;
+	u16 data_base_addr = fc->fd.data_base_addr;
+	u8 status;
+	int retval;
+
+	/* Read the Fn $34 status from F34_Flash_Data3 to see the previous
+	 * commands status. F34_Flash_Data3 will be the address after the
+	 * 2 block number registers plus blocksize Data registers.
+	 *  inform user space - through a sysfs param. */
+	retval = rmi_read(fc->rmi_dev,
+			  data_base_addr + instance_data->blocksize +
+			  BLK_NUM_OFF, &status);
+
+	if (retval < 0) {
+		dev_err(&fc->dev, "Could not read status from 0x%x\n",
+		       data_base_addr + instance_data->blocksize + BLK_NUM_OFF);
+		status = 0xff;	/* failure */
+	}
+
+	/* set a sysfs value that the user mode can read - only
+	 * upper 4 bits are the status. successful is $80, anything
+	 * else is failure */
+	instance_data->status = status & 0xf0;
+
+	/* put mode into Flash Prog Mode when we successfully do
+	 * an Enable Flash Prog cmd. */
+	if ((instance_data->status == STATUS_IDLE) &&
+		(instance_data->cmd == ENABLE_FLASH_PROG))
+		instance_data->inflashprogmode = true;
+
+	return retval;
+}
+
+int rmi_f34_attention(struct rmi_function_container *fc, u8 *irq_bits)
+{
+	return f34_read_status(fc);
+}
+
+static struct rmi_function_handler function_handler = {
+	.func = 0x34,
+	.init = rmi_f34_init,
+	.attention = rmi_f34_attention
+};
+
+static ssize_t rmi_fn_34_bootloaderid_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct rmi_function_container *fc;
+	struct rmi_fn_34_data *instance_data;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", instance_data->bootloaderid);
+}
+
+static ssize_t rmi_fn_34_bootloaderid_store(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf,
+					size_t count)
+{
+	int error;
+	unsigned long val;
+	unsigned char data[2];
+	struct rmi_function_container *fc;
+	struct rmi_fn_34_data *instance_data;
+	u16 data_base_addr;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	/* need to convert the string data to an actual value */
+	error = strict_strtoul(buf, 10, &val);
+
+	if (error)
+		return error;
+
+	instance_data->bootloaderid = val;
+
+	/* Write the Bootloader ID key data back to the first two Block
+	 * Data registers (F34_Flash_Data2.0 and F34_Flash_Data2.1). */
+	hstoba(data, (unsigned short)val);
+	data_base_addr = fc->fd.data_base_addr;
+
+	error = rmi_write_block(fc->rmi_dev,
+				data_base_addr + BLK_NUM_OFF,
+				data,
+				ARRAY_SIZE(data));
+
+	if (error < 0) {
+		dev_err(dev, "%s : Could not write bootloader id to 0x%x\n",
+		       __func__, data_base_addr + BLK_NUM_OFF);
+		return error;
+	}
+
+	return count;
+}
+
+static ssize_t rmi_fn_34_blocksize_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct rmi_function_container *fc;
+	struct rmi_fn_34_data *instance_data;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", instance_data->blocksize);
+}
+
+static ssize_t rmi_fn_34_imageblockcount_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct rmi_function_container *fc;
+	struct rmi_fn_34_data *instance_data;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			instance_data->imageblockcount);
+}
+
+static ssize_t rmi_fn_34_configblockcount_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct rmi_function_container *fc;
+	struct rmi_fn_34_data *instance_data;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			instance_data->configblockcount);
+}
+
+static ssize_t rmi_fn_34_status_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct rmi_function_container *fc;
+	struct rmi_fn_34_data *instance_data;
+	int retval;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	retval = f34_read_status(fc);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", instance_data->status);
+}
+
+static ssize_t rmi_fn_34_cmd_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct rmi_function_container *fc;
+	struct rmi_fn_34_data *instance_data;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", instance_data->cmd);
+}
+
+static ssize_t rmi_fn_34_cmd_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf,
+				size_t count)
+{
+	struct rmi_function_container *fc;
+	struct rmi_fn_34_data *instance_data;
+	unsigned long val;
+	u16 data_base_addr;
+	int error;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+	data_base_addr = fc->fd.data_base_addr;
+
+	/* need to convert the string data to an actual value */
+	error = strict_strtoul(buf, 10, &val);
+	if (error)
+		return error;
+
+	/* make sure we are in Flash Prog mode for all cmds except the
+	 * Enable Flash Programming cmd - otherwise we are in error */
+	if ((val != ENABLE_FLASH_PROG) && !instance_data->inflashprogmode) {
+		dev_err(dev, "%s: CANNOT SEND CMD %d TO SENSOR - "
+			"NOT IN FLASH PROG MODE\n"
+			, __func__, data_base_addr);
+		return -EINVAL;
+	}
+
+	instance_data->cmd = val;
+
+	/* Validate command value and (if necessary) write it to the command
+	 * register.
+	 */
+	switch (instance_data->cmd) {
+	case ENABLE_FLASH_PROG:
+	case ERASE_ALL:
+	case ERASE_CONFIG:
+	case WRITE_FW_BLOCK:
+	case READ_CONFIG_BLOCK:
+	case WRITE_CONFIG_BLOCK:
+		/* Reset the status to indicate we are in progress on a cmd. */
+		/* The status will change when the ATTN interrupt happens
+		   and the status of the cmd that was issued is read from
+		   the F34_Flash_Data3 register - result should be 0x80 for
+		   success - any other value indicates an error */
+
+		/* Issue the command to the device. */
+		error = rmi_write(fc->rmi_dev,
+				data_base_addr + instance_data->blocksize +
+				BLK_NUM_OFF, instance_data->cmd);
+
+		if (error < 0) {
+			dev_err(dev, "%s: Could not write command 0x%02x "
+				"to 0x%04x\n", __func__, instance_data->cmd,
+				data_base_addr + instance_data->blocksize +
+				BLK_NUM_OFF);
+			return error;
+		}
+
+		if (instance_data->cmd == ENABLE_FLASH_PROG)
+			instance_data->inflashprogmode = true;
+
+		/* set status to indicate we are in progress */
+		instance_data->status = STATUS_IN_PROGRESS;
+		break;
+	default:
+		dev_dbg(dev, "%s: RMI4 function $34 - "
+				"unknown command 0x%02lx.\n", __func__, val);
+		count = -EINVAL;
+		break;
+	}
+
+	return count;
+}
+
+static ssize_t rmi_fn_34_blocknum_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct rmi_function_container *fc;
+	struct rmi_fn_34_data *instance_data;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", instance_data->blocknum);
+}
+
+static ssize_t rmi_fn_34_blocknum_store(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf,
+					size_t count)
+{
+	int error;
+	unsigned long val;
+	unsigned char data[2];
+	struct rmi_function_container *fc;
+	struct rmi_fn_34_data *instance_data;
+	u16 data_base_addr;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+	data_base_addr = fc->fd.data_base_addr;
+
+	/* need to convert the string data to an actual value */
+	error = strict_strtoul(buf, 10, &val);
+
+	if (error)
+		return error;
+
+	instance_data->blocknum = val;
+
+	/* Write the Block Number data back to the first two Block
+	 * Data registers (F34_Flash_Data_0 and F34_Flash_Data_1). */
+	hstoba(data, (unsigned short)val);
+
+	error = rmi_write_block(fc->rmi_dev,
+				data_base_addr,
+				data,
+				ARRAY_SIZE(data));
+
+	if (error < 0) {
+		dev_err(dev, "%s : Could not write block number %u to 0x%x\n",
+		       __func__, instance_data->blocknum, data_base_addr);
+		return error;
+	}
+
+	return count;
+}
+
+static ssize_t rmi_fn_34_rescanPDT_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	struct rmi_function_container *fc;
+	struct rmi_fn_34_data *instance_data;
+	struct rmi_device *rmi_dev;
+	struct rmi_driver_data *driver_data;
+	struct pdt_entry pdt_entry;
+	bool fn01found = false;
+	bool fn34found = false;
+	unsigned int rescan;
+	int irq_count = 0;
+	int retval = 0;
+	int i;
+
+	/* Rescan of the PDT is needed since issuing the Flash Enable cmd
+	 * the device registers for Fn$01 and Fn$34 moving around because
+	 * of the change from Bootloader mode to Flash Programming mode
+	 * may change to a different PDT with only Fn$01 and Fn$34 that
+	 * could have addresses for query, control, data, command registers
+	 * that differ from the PDT scan done at device initialization. */
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+	rmi_dev = fc->rmi_dev;
+	driver_data = rmi_get_driverdata(rmi_dev);
+
+	/* Make sure we are only in Flash Programming mode  - DON'T
+	 * ALLOW THIS IN UI MODE. */
+	if (instance_data->cmd != ENABLE_FLASH_PROG) {
+		dev_err(dev, "%s: NOT IN FLASH PROG MODE - CAN'T RESCAN PDT.\n"
+				, __func__);
+		return -EINVAL;
+	}
+
+	/* The only good value to write to this is 1, we allow 0, but with
+	 * no effect (this is consistent with the way the command bit works. */
+	if (sscanf(buf, "%u", &rescan) != 1)
+		return -EINVAL;
+	if (rescan < 0 || rescan > 1)
+		return -EINVAL;
+
+	/* 0 has no effect, so we skip it entirely. */
+	if (rescan) {
+		/* rescan the PDT - filling in Fn01 and Fn34 addresses -
+		 * this is only temporary - the device will need to be reset
+		 * to return the PDT to the normal values. */
+
+		/* mini-parse the PDT - we only have to get Fn$01 and Fn$34 and
+		   since we are Flash Programming mode we only have page 0. */
+		for (i = PDT_START_SCAN_LOCATION; i >= PDT_END_SCAN_LOCATION;
+			i -= sizeof(pdt_entry)) {
+			retval = rmi_read_block(rmi_dev, i, (u8 *)&pdt_entry,
+					       sizeof(pdt_entry));
+			if (retval != sizeof(pdt_entry)) {
+				dev_err(dev, "%s: err frm rmi_read_block pdt "
+					"entry data from PDT, "
+					"error = %d.", __func__, retval);
+				return retval;
+			}
+
+			if ((pdt_entry.function_number == 0x00) ||
+				(pdt_entry.function_number == 0xff))
+				break;
+
+			dev_dbg(dev, "%s: Found F%.2X\n",
+				__func__, pdt_entry.function_number);
+
+			/* f01 found - just fill in the new addresses in
+			 * the existing fc. */
+			if (pdt_entry.function_number == 0x01) {
+				struct rmi_function_container *f01_fc =
+					driver_data->f01_container;
+				fn01found = true;
+				f01_fc->fd.query_base_addr =
+					pdt_entry.query_base_addr;
+				f01_fc->fd.command_base_addr =
+				  pdt_entry.command_base_addr;
+				f01_fc->fd.control_base_addr =
+				  pdt_entry.control_base_addr;
+				f01_fc->fd.data_base_addr =
+				  pdt_entry.data_base_addr;
+				f01_fc->fd.function_number =
+				  pdt_entry.function_number;
+				f01_fc->fd.interrupt_source_count =
+				  pdt_entry.interrupt_source_count;
+				f01_fc->num_of_irqs =
+				  pdt_entry.interrupt_source_count;
+				f01_fc->irq_pos = irq_count;
+
+				irq_count += f01_fc->num_of_irqs;
+
+				if (fn34found)
+					break;
+			}
+
+			/* f34 found - just fill in the new addresses in
+			 * the existing fc. */
+			if (pdt_entry.function_number == 0x34) {
+				fn34found = true;
+				fc->fd.query_base_addr =
+				  pdt_entry.query_base_addr;
+				fc->fd.command_base_addr =
+				  pdt_entry.command_base_addr;
+				fc->fd.control_base_addr =
+				  pdt_entry.control_base_addr;
+				fc->fd.data_base_addr =
+				  pdt_entry.data_base_addr;
+				fc->fd.function_number =
+				  pdt_entry.function_number;
+				fc->fd.interrupt_source_count =
+				  pdt_entry.interrupt_source_count;
+				fc->num_of_irqs =
+				  pdt_entry.interrupt_source_count;
+				fc->irq_pos = irq_count;
+
+				irq_count += fc->num_of_irqs;
+
+				if (fn01found)
+					break;
+			}
+
+		}
+
+		if (!fn01found || !fn34found) {
+			dev_err(dev, "%s: failed to find fn$01 or fn$34 trying "
+				"to do rescan PDT.\n"
+				, __func__);
+			return -EINVAL;
+		}
+	}
+
+	return count;
+}
+
+#ifdef KERNEL_VERSION_ABOVE_2_6_32
+static ssize_t rmi_fn_34_data_read(struct file *data_file,
+				struct kobject *kobj,
+				struct bin_attribute *attributes,
+				char *buf,
+				loff_t pos,
+				size_t count)
+#else
+static ssize_t rmi_fn_34_data_read(struct kobject *kobj,
+				struct bin_attribute *attributes,
+				char *buf,
+				loff_t pos,
+				size_t count)
+#endif
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct rmi_function_container *fc;
+	struct rmi_fn_34_data *instance_data;
+	u16 data_base_addr;
+	int error;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	data_base_addr = fc->fd.data_base_addr;
+
+	if (count != instance_data->blocksize) {
+		dev_err(dev,
+			"%s : Incorrect F34 block size %d. "
+			"Expected size %d.\n",
+			__func__, count, instance_data->blocksize);
+		return -EINVAL;
+	}
+
+	/* Read the data from flash into buf.  The app layer will be blocked
+	 * at reading from the sysfs file.  When we return the count (or
+	 * error if we fail) the app will resume. */
+	error = rmi_read_block(fc->rmi_dev, data_base_addr + BLK_NUM_OFF,
+			(unsigned char *)buf, count);
+
+	if (error < 0) {
+		dev_err(dev, "%s : Could not read data from 0x%04x\n",
+		       __func__, data_base_addr + BLK_NUM_OFF);
+		return error;
+	}
+
+	return count;
+}
+
+#ifdef KERNEL_VERSION_ABOVE_2_6_32
+static ssize_t rmi_fn_34_data_write(struct file *data_file,
+				struct kobject *kobj,
+				struct bin_attribute *attributes,
+				char *buf,
+				loff_t pos,
+				size_t count)
+#else
+static ssize_t rmi_fn_34_data_write(struct kobject *kobj,
+				struct bin_attribute *attributes,
+				char *buf,
+				loff_t pos,
+				size_t count)
+#endif
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct rmi_function_container *fc;
+	struct rmi_fn_34_data *instance_data;
+	u16 data_base_addr;
+	int error;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	data_base_addr = fc->fd.data_base_addr;
+
+	/* Write the data from buf to flash. The app layer will be
+	 * blocked at writing to the sysfs file.  When we return the
+	 * count (or error if we fail) the app will resume. */
+
+	if (count != instance_data->blocksize) {
+		dev_err(dev,
+			"%s : Incorrect F34 block size %d. "
+			"Expected size %d.\n",
+			__func__, count, instance_data->blocksize);
+		return -EINVAL;
+	}
+
+	/* Write the data block - only if the count is non-zero  */
+	if (count) {
+		error = rmi_write_block(fc->rmi_dev,
+				data_base_addr + BLK_NUM_OFF,
+				(unsigned char *)buf,
+				count);
+
+		if (error < 0) {
+			dev_err(dev, "%s : Could not write block data "
+				"to 0x%x\n", __func__,
+				data_base_addr + BLK_NUM_OFF);
+			return error;
+		}
+	}
+
+	return count;
+}
+
+static int __init rmi_f34_module_init(void)
+{
+	int error;
+
+	error = rmi_register_function_driver(&function_handler);
+	if (error < 0) {
+		pr_err("%s : register failed !\n", __func__);
+		return error;
+	}
+
+	return 0;
+}
+
+static void rmi_f34_module_exit(void)
+{
+	rmi_unregister_function_driver(&function_handler);
+}
+
+module_init(rmi_f34_module_init);
+module_exit(rmi_f34_module_exit);
+
+MODULE_AUTHOR("Eric Andersson <eric.andersson@unixphere.com>");
+MODULE_DESCRIPTION("RMI f34 module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/rmi4/rmi_f54.c b/drivers/input/touchscreen/rmi4/rmi_f54.c
new file mode 100644
index 000000000000..f4f2e3ac2903
--- /dev/null
+++ b/drivers/input/touchscreen/rmi4/rmi_f54.c
@@ -0,0 +1,1347 @@
+
+/*
+ * Copyright (c) 2011 Synaptics Incorporated
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+#include <linux/hrtimer.h>
+#include <linux/kernel.h>
+#include <linux/rmi.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include <linux/delay.h>
+#include "rmi_driver.h"
+
+/* Set this to 1 for raw hex dump of returned data. */
+#define RAW_HEX 0
+/* Set this to 1 for human readable dump of returned data. */
+#define HUMAN_READABLE 0
+/* The watchdog timer can be useful when debugging certain firmware related
+ * issues.
+ */
+#define F54_WATCHDOG 1
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 32)
+#define KERNEL_VERSION_ABOVE_2_6_32 1
+#endif
+
+/* define fn $54 commands */
+#define GET_REPORT                1
+#define FORCE_CAL                 2
+
+/* status */
+#define BUSY 1
+#define IDLE 0
+
+/* Offsets for data */
+#define RMI_F54_REPORT_DATA_OFFSET	3
+#define RMI_F54_FIFO_OFFSET		1
+#define RMI_F54_NUM_TX_OFFSET		1
+#define RMI_F54_NUM_RX_OFFSET		0
+
+/* Fixed sizes of reports */
+#define RMI_54_FULL_RAW_CAP_MIN_MAX_SIZE 4
+#define RMI_54_HIGH_RESISTANCE_SIZE 6
+
+/* definitions for F54 Query Registers in ultra-portable unionstruct form */
+struct f54_ad_query {
+	/* query 0 */
+	u8 number_of_receiver_electrodes;
+
+	/* query 1 */
+	u8 number_of_transmitter_electrodes;
+
+	union {
+		struct {
+			/* query2 */
+			u8 f54_ad_query2_b0__1:2;
+			u8 has_baseline:1;
+			u8 has_image8:1;
+			u8 f54_ad_query2_b4__5:2;
+			u8 has_image16:1;
+			u8 f54_ad_query2_b7:1;
+		};
+		u8 f54_ad_query2;
+	};
+
+	/* query 3.0 and 3.1 */
+	u16 clock_rate;
+
+	/* query 4 */
+	u8 touch_controller_family;
+
+	/* query 5 */
+	union {
+		struct {
+			u8 has_pixel_touch_threshold_adjustment:1;
+			u8 f54_ad_query5_b1__7:7;
+		};
+		u8 f54_ad_query5;
+	};
+
+	/* query 6 */
+	union {
+		struct {
+		u8 has_sensor_assignment:1;
+		u8 has_interference_metric:1;
+		u8 has_sense_frequency_control:1;
+		u8 has_firmware_noise_mitigation:1;
+		u8 f54_ad_query6_b4:1;
+		u8 has_two_byte_report_rate:1;
+		u8 has_one_byte_report_rate:1;
+		u8 has_relaxation_control:1;
+		};
+		u8 f54_ad_query6;
+	};
+
+	/* query 7 */
+	union {
+		struct {
+			u8 curve_compensation_mode:2;
+			u8 f54_ad_query7_b2__7:6;
+		};
+		u8 f54_ad_query7;
+	};
+
+	/* query 8 */
+	union {
+		struct {
+		u8 f54_ad_query2_b0:1;
+		u8 has_iir_filter:1;
+		u8 has_cmn_removal:1;
+		u8 has_cmn_maximum:1;
+		u8 has_pixel_threshold_hysteresis:1;
+		u8 has_edge_compensation:1;
+		u8 has_perf_frequency_noisecontrol:1;
+		u8 f54_ad_query8_b7:1;
+		};
+		u8 f54_ad_query8;
+	};
+
+	u8 f54_ad_query9;
+	u8 f54_ad_query10;
+	u8 f54_ad_query11;
+
+	/* query 12 */
+	union {
+		struct {
+			u8 number_of_sensing_frequencies:4;
+			u8 f54_ad_query12_b4__7:4;
+		};
+		u8 f54_ad_query12;
+	};
+};
+
+/* define report types */
+enum f54_report_types {
+	/* The numbering should follow automatically, here for clarity */
+	F54_8BIT_IMAGE = 1,
+	F54_16BIT_IMAGE = 2,
+	F54_RAW_16BIT_IMAGE = 3,
+	F54_HIGH_RESISTANCE = 4,
+	F54_TX_TO_TX_SHORT = 5,
+	F54_RX_TO_RX1 = 7,
+	F54_TRUE_BASELINE = 9,
+	F54_FULL_RAW_CAP_MIN_MAX = 13,
+	F54_RX_OPENS1 = 14,
+	F54_TX_OPEN = 15,
+	F54_TX_TO_GROUND = 16,
+	F54_RX_TO_RX2 = 17,
+	F54_RX_OPENS2 = 18,
+	F54_FULL_RAW_CAP = 19,
+	F54_FULL_RAW_CAP_RX_COUPLING_COMP = 20
+};
+
+/* data specific to fn $54 that needs to be kept around */
+struct rmi_fn_54_data {
+	struct f54_ad_query query;
+	u8 cmd;
+	enum f54_report_types report_type;
+	u16 fifoindex;
+	signed char status;
+	bool no_auto_cal;
+	/*
+	 * May need to do something to make sure this reflects what is currently
+	 * in data.
+	 */
+	unsigned int report_size;
+	unsigned char *report_data;
+	unsigned int bufsize;
+	struct mutex data_mutex;
+	struct lock_class_key data_key;
+	struct mutex status_mutex;
+	struct lock_class_key status_key;
+#if F54_WATCHDOG
+	struct hrtimer watchdog;
+#endif
+	struct rmi_function_container *fc;
+	struct work_struct work;
+};
+
+/* sysfs functions */
+static ssize_t rmi_fn_54_report_type_show(struct device *dev,
+				struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_report_type_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t count);
+
+static ssize_t rmi_fn_54_get_report_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t count);
+
+static ssize_t rmi_fn_54_force_cal_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t count);
+
+static ssize_t rmi_fn_54_status_show(struct device *dev,
+				struct device_attribute *attr, char *buf);
+
+#ifdef KERNEL_VERSION_ABOVE_2_6_32
+static ssize_t rmi_fn_54_data_read(struct file *data_file, struct kobject *kobj,
+#else
+static ssize_t rmi_fn_54_data_read(struct kobject *kobj,
+#endif
+					struct bin_attribute *attributes,
+					char *buf, loff_t pos, size_t count);
+
+static ssize_t rmi_fn_54_num_rx_electrodes_show(struct device *dev,
+				     struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_num_tx_electrodes_show(struct device *dev,
+				struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_image16_show(struct device *dev,
+				     struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_image8_show(struct device *dev,
+				struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_baseline_show(struct device *dev,
+				struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_clock_rate_show(struct device *dev,
+				struct device_attribute *attr, char *buf);
+
+
+static ssize_t rmi_fn_54_touch_controller_family_show(struct device *dev,
+				struct device_attribute *attr, char *buf);
+
+
+static ssize_t rmi_fn_54_has_pixel_touch_threshold_adjustment_show(
+		struct device *dev, struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_sensor_assignment_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_interference_metric_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_sense_frequency_control_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_firmware_noise_mitigation_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_two_byte_report_rate_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_one_byte_report_rate_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_relaxation_control_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_curve_compensation_mode_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_iir_filter_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_cmn_removal_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_cmn_maximum_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_pixel_threshold_hysteresis_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_edge_compensation_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_has_perf_frequency_noisecontrol_show(
+		struct device *dev, struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_number_of_sensing_frequencies_show(struct device *dev,
+			struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_no_auto_cal_show(struct device *dev,
+				struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_no_auto_cal_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t count);
+
+static ssize_t rmi_fn_54_fifoindex_show(struct device *dev,
+				  struct device_attribute *attr, char *buf);
+
+static ssize_t rmi_fn_54_fifoindex_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t count);
+
+static struct device_attribute attrs[] = {
+	__ATTR(report_type, RMI_RO_ATTR,
+		rmi_fn_54_report_type_show, rmi_fn_54_report_type_store),
+	__ATTR(get_report, RMI_RO_ATTR,
+		rmi_show_error, rmi_fn_54_get_report_store),
+	__ATTR(force_cal, RMI_RO_ATTR,
+		rmi_show_error, rmi_fn_54_force_cal_store),
+	__ATTR(status, RMI_RO_ATTR,
+		rmi_fn_54_status_show, rmi_store_error),
+	__ATTR(num_rx_electrodes, RMI_RO_ATTR,
+		rmi_fn_54_num_rx_electrodes_show, rmi_store_error),
+	__ATTR(num_tx_electrodes, RMI_RO_ATTR,
+		rmi_fn_54_num_tx_electrodes_show, rmi_store_error),
+	__ATTR(has_image16, RMI_RO_ATTR,
+		rmi_fn_54_has_image16_show, rmi_store_error),
+	__ATTR(has_image8, RMI_RO_ATTR,
+		rmi_fn_54_has_image8_show, rmi_store_error),
+	__ATTR(has_baseline, RMI_RO_ATTR,
+		rmi_fn_54_has_baseline_show, rmi_store_error),
+	__ATTR(clock_rate, RMI_RO_ATTR,
+		rmi_fn_54_clock_rate_show, rmi_store_error),
+	__ATTR(touch_controller_family, RMI_RO_ATTR,
+		rmi_fn_54_touch_controller_family_show, rmi_store_error),
+	__ATTR(has_pixel_touch_threshold_adjustment, RMI_RO_ATTR,
+		rmi_fn_54_has_pixel_touch_threshold_adjustment_show
+							, rmi_store_error),
+	__ATTR(has_sensor_assignment, RMI_RO_ATTR,
+		rmi_fn_54_has_sensor_assignment_show, rmi_store_error),
+	__ATTR(has_interference_metric, RMI_RO_ATTR,
+		rmi_fn_54_has_interference_metric_show, rmi_store_error),
+	__ATTR(has_sense_frequency_control, RMI_RO_ATTR,
+		rmi_fn_54_has_sense_frequency_control_show, rmi_store_error),
+	__ATTR(has_firmware_noise_mitigation, RMI_RO_ATTR,
+		rmi_fn_54_has_firmware_noise_mitigation_show, rmi_store_error),
+	__ATTR(has_two_byte_report_rate, RMI_RO_ATTR,
+		rmi_fn_54_has_two_byte_report_rate_show, rmi_store_error),
+	__ATTR(has_one_byte_report_rate, RMI_RO_ATTR,
+		rmi_fn_54_has_one_byte_report_rate_show, rmi_store_error),
+	__ATTR(has_relaxation_control, RMI_RO_ATTR,
+		rmi_fn_54_has_relaxation_control_show, rmi_store_error),
+	__ATTR(curve_compensation_mode, RMI_RO_ATTR,
+		rmi_fn_54_curve_compensation_mode_show, rmi_store_error),
+	__ATTR(has_iir_filter, RMI_RO_ATTR,
+		rmi_fn_54_has_iir_filter_show, rmi_store_error),
+	__ATTR(has_cmn_removal, RMI_RO_ATTR,
+		rmi_fn_54_has_cmn_removal_show, rmi_store_error),
+	__ATTR(has_cmn_maximum, RMI_RO_ATTR,
+		rmi_fn_54_has_cmn_maximum_show, rmi_store_error),
+	__ATTR(has_pixel_threshold_hysteresis, RMI_RO_ATTR,
+		rmi_fn_54_has_pixel_threshold_hysteresis_show, rmi_store_error),
+	__ATTR(has_edge_compensation, RMI_RO_ATTR,
+		rmi_fn_54_has_edge_compensation_show, rmi_store_error),
+	__ATTR(has_perf_frequency_noisecontrol, RMI_RO_ATTR,
+	      rmi_fn_54_has_perf_frequency_noisecontrol_show, rmi_store_error),
+	__ATTR(number_of_sensing_frequencies, RMI_RO_ATTR,
+		rmi_fn_54_number_of_sensing_frequencies_show, rmi_store_error),
+	__ATTR(no_auto_cal, RMI_RO_ATTR,
+		rmi_fn_54_no_auto_cal_show, rmi_fn_54_no_auto_cal_store),
+	__ATTR(fifoindex, RMI_RO_ATTR,
+		rmi_fn_54_fifoindex_show, rmi_fn_54_fifoindex_store),
+};
+
+struct bin_attribute dev_rep_data = {
+	.attr = {
+		 .name = "rep_data",
+		 .mode = RMI_RO_ATTR},
+	.size = 0,
+	.read = rmi_fn_54_data_read,
+};
+
+#if F54_WATCHDOG
+static enum hrtimer_restart clear_status(struct hrtimer *timer);
+
+static void clear_status_worker(struct work_struct *work);
+#endif
+
+static int rmi_f54_init(struct rmi_function_container *fc)
+{
+	struct rmi_fn_54_data *instance_data;
+	int retval = 0;
+	int attr_count = 0;
+
+	dev_info(&fc->dev, "Intializing F54.");
+
+	instance_data = kzalloc(sizeof(struct rmi_fn_54_data), GFP_KERNEL);
+	if (!instance_data) {
+		dev_err(&fc->dev, "Failed to allocate rmi_fn_54_data.\n");
+		retval = -ENOMEM;
+		goto error_exit;
+	}
+	fc->data = instance_data;
+	instance_data->fc = fc;
+
+#if F54_WATCHDOG
+	/* Set up watchdog timer to catch unanswered get_report commands */
+	hrtimer_init(&instance_data->watchdog, CLOCK_MONOTONIC,
+							HRTIMER_MODE_REL);
+	instance_data->watchdog.function = clear_status;
+
+	/* work function to do unlocking */
+	INIT_WORK(&instance_data->work, clear_status_worker);
+#endif
+
+	/* Read F54 Query Data */
+	retval = rmi_read_block(fc->rmi_dev, fc->fd.query_base_addr,
+		(u8 *)&instance_data->query, sizeof(instance_data->query));
+	if (retval < 0) {
+		dev_err(&fc->dev, "Could not read query registers"
+			" from 0x%04x\n", fc->fd.query_base_addr);
+		goto error_exit;
+	}
+
+	__mutex_init(&instance_data->data_mutex, "data_mutex",
+		     &instance_data->data_key);
+
+	__mutex_init(&instance_data->status_mutex, "status_mutex",
+		     &instance_data->status_key);
+
+	dev_dbg(&fc->dev, "Creating sysfs files.");
+	/* Set up sysfs device attributes. */
+	for (attr_count = 0; attr_count < ARRAY_SIZE(attrs); attr_count++) {
+		if (sysfs_create_file
+		    (&fc->dev.kobj, &attrs[attr_count].attr) < 0) {
+			dev_err(&fc->dev, "Failed to create sysfs file for %s.",
+			     attrs[attr_count].attr.name);
+			retval = -ENODEV;
+			goto error_exit;
+		}
+	}
+	/* Binary sysfs file to report the data back */
+	retval = sysfs_create_bin_file(&fc->dev.kobj, &dev_rep_data);
+	if (retval < 0) {
+		dev_err(&fc->dev, "Failed to create sysfs file for F54 data "
+					"(error = %d).\n", retval);
+		retval = -ENODEV;
+		goto error_exit;
+	}
+	instance_data->status = IDLE;
+	return retval;
+
+error_exit:
+	dev_err(&fc->dev, "An error occured in F54 init!\n");
+	for (attr_count--; attr_count >= 0; attr_count--)
+		sysfs_remove_file(&fc->dev.kobj,
+				  &attrs[attr_count].attr);
+	kfree(instance_data);
+	return retval;
+}
+
+static void set_report_size(struct rmi_fn_54_data *data)
+{
+	u8 rx = data->query.number_of_receiver_electrodes;
+	u8 tx = data->query.number_of_transmitter_electrodes;
+	switch (data->report_type) {
+	case F54_8BIT_IMAGE:
+		data->report_size = rx * tx;
+		break;
+	case F54_16BIT_IMAGE:
+	case F54_RAW_16BIT_IMAGE:
+	case F54_TRUE_BASELINE:
+	case F54_FULL_RAW_CAP:
+	case F54_FULL_RAW_CAP_RX_COUPLING_COMP:
+		data->report_size = 2 * rx * tx;
+		break;
+	case F54_HIGH_RESISTANCE:
+		data->report_size = RMI_54_HIGH_RESISTANCE_SIZE;
+		break;
+	case F54_FULL_RAW_CAP_MIN_MAX:
+		data->report_size = RMI_54_FULL_RAW_CAP_MIN_MAX_SIZE;
+		break;
+	case F54_TX_TO_TX_SHORT:
+	case F54_TX_OPEN:
+	case F54_TX_TO_GROUND:
+		data->report_size =  (tx + 7) / 8;
+		break;
+	case F54_RX_TO_RX1:
+	case F54_RX_OPENS1:
+		if (rx < tx)
+			data->report_size = 2 * rx * rx;
+		else
+			data->report_size = 2 * rx * tx;
+		break;
+	case F54_RX_TO_RX2:
+	case F54_RX_OPENS2:
+		if (rx <= tx)
+			data->report_size = 0;
+		else
+			data->report_size = 2 * rx * (rx - tx);
+		break;
+	default:
+		data->report_size = 0;
+	}
+}
+
+int rmi_f54_attention(struct rmi_function_container *fc, u8 *irq_bits)
+{
+	struct rmi_driver *driver = fc->rmi_dev->driver;
+	char fifo[2];
+	struct rmi_fn_54_data *data = fc->data;
+	int error = 0;
+
+	set_report_size(data);
+	if (data->report_size == 0) {
+		dev_err(&fc->dev, "Invalid report type set in %s. "
+				"This should never happen.\n", __func__);
+		error = -EINVAL;
+		goto error_exit;
+	}
+	/*
+	 * We need to ensure the buffer is big enough. A Buffer size of 0 means
+	 * that the buffer has not been allocated.
+	 */
+	if (data->bufsize < data->report_size) {
+		mutex_lock(&data->data_mutex);
+		if (data->bufsize > 0)
+			kfree(data->report_data);
+		data->report_data = kzalloc(data->report_size, GFP_KERNEL);
+		if (!data->report_data) {
+			dev_err(&fc->dev, "Failed to allocate report_data.\n");
+			error = -ENOMEM;
+			data->bufsize = 0;
+			mutex_unlock(&data->data_mutex);
+			goto error_exit;
+		}
+		data->bufsize = data->report_size;
+		mutex_unlock(&data->data_mutex);
+	}
+	dev_vdbg(&fc->dev, "F54 Interrupt handler is running.\nSize: %d\n",
+		 data->report_size);
+	/*
+	 * Read report type, fifo high, and fifo low
+	 * error = rmi_read_multiple(rmifninfo->sensor,
+	 *	rmifninfo->function_descriptor.data_base_addr ,
+	 *	repfifo,3);
+	 */
+	/* Write 0 to fifohi and fifolo. */
+	fifo[0] = 0;
+	fifo[1] = 0;
+	error = rmi_write_block(fc->rmi_dev, fc->fd.data_base_addr
+				+ RMI_F54_FIFO_OFFSET, fifo,	sizeof(fifo));
+	if (error < 0)
+		dev_err(&fc->dev, "Failed to write fifo to zero!\n");
+	else
+		error = rmi_read_block(fc->rmi_dev,
+			fc->fd.data_base_addr + RMI_F54_REPORT_DATA_OFFSET,
+			data->report_data, data->report_size);
+	if (error < 0)
+		dev_err(&fc->dev, "F54 data read failed. Code: %d.\n", error);
+	else if (error != data->report_size) {
+		error = -EINVAL;
+		goto error_exit;
+	}
+#if RAW_HEX
+	int l;
+	/* Debugging: Print out the file in hex. */
+	pr_info("Report data (raw hex):\n");
+	for (l = 0; l < data->report_size; l += 2) {
+		pr_info("%03d: 0x%02x%02x\n", l/2,
+			data->report_data[l+1], data->report_data[l]);
+	}
+#endif
+#if HUMAN_READABLE
+	/* Debugging: Print out file in human understandable image */
+	switch (data->report_type) {
+	case F54_16BIT_IMAGE:
+	case F54_RAW_16BIT_IMAGE:
+	case F54_TRUE_BASELINE:
+	case F54_FULL_RAW_CAP:
+	case F54_FULL_RAW_CAP_RX_COUPLING_COMP:
+		pr_info("Report data (Image):\n");
+		int i, j, k;
+		char c[2];
+		short s;
+		k = 0;
+		for (i = 0; i < data->query.number_of_transmitter_electrodes;
+									i++) {
+			for (j = 0; j <
+			     data->query.number_of_receiver_electrodes; j++) {
+				c[0] = data->report_data[k];
+				c[1] = data->report_data[k+1];
+				memcpy(&s, &c, 2);
+				if (s < -64)
+					printk(".");
+				else if (s < 0)
+					printk("-");
+				else if (s > 64)
+					printk("*");
+				else if (s > 0)
+					printk("+");
+				else
+					printk("0");
+				k += 2;
+			}
+			pr_info("\n");
+		}
+		pr_info("EOF\n");
+		break;
+	default:
+		pr_info("Report type %d debug image not supported",
+							data->report_type);
+	}
+#endif
+	error = IDLE;
+error_exit:
+	mutex_lock(&data->status_mutex);
+	/* Turn back on other interupts, if it
+	 * appears that we turned them off. */
+	if (driver->restore_irq_mask) {
+		dev_dbg(&fc->dev, "Restoring interupts!\n");
+		driver->restore_irq_mask(fc->rmi_dev);
+	} else {
+		dev_err(&fc->dev, "No way to restore interrupts!\n");
+	}
+	data->status = error;
+	mutex_unlock(&data->status_mutex);
+	return data->status;
+}
+
+
+#if F54_WATCHDOG
+static void clear_status_worker(struct work_struct *work)
+{
+	struct rmi_fn_54_data *data = container_of(work,
+					struct rmi_fn_54_data, work);
+	struct rmi_function_container *fc = data->fc;
+	struct rmi_driver *driver = fc->rmi_dev->driver;
+	char command;
+	int result;
+
+	mutex_lock(&data->status_mutex);
+	if (data->status == BUSY) {
+		pr_info("F54 Timout Occured: Determining status.\n");
+		result = rmi_read_block(fc->rmi_dev, fc->fd.command_base_addr,
+								&command, 1);
+		if (result < 0) {
+			dev_err(&fc->dev, "Could not read get_report register "
+				"from 0x%04x\n", fc->fd.command_base_addr);
+			data->status = -ETIMEDOUT;
+		} else {
+			if (command & GET_REPORT) {
+				dev_warn(&fc->dev, "Report type unsupported!");
+				data->status = -EINVAL;
+			} else {
+				data->status = -ETIMEDOUT;
+			}
+		}
+		if (driver->restore_irq_mask) {
+			dev_dbg(&fc->dev, "Restoring interupts!\n");
+			driver->restore_irq_mask(fc->rmi_dev);
+		} else {
+			dev_err(&fc->dev, "No way to restore interrupts!\n");
+		}
+	}
+	mutex_unlock(&data->status_mutex);
+}
+
+static enum hrtimer_restart clear_status(struct hrtimer *timer)
+{
+	struct rmi_fn_54_data *data = container_of(timer,
+					struct rmi_fn_54_data, watchdog);
+	schedule_work(&(data->work));
+	return HRTIMER_NORESTART;
+}
+#endif
+
+/* Check if report_type is valid */
+static bool is_report_type_valid(enum f54_report_types reptype)
+{
+	/* Basic checks on report_type to ensure we write a valid type
+	 * to the sensor.
+	 * TODO: Check Query3 to see if some specific reports are
+	 * available. This is currently listed as a reserved register.
+	 */
+	switch (reptype) {
+	case F54_8BIT_IMAGE:
+	case F54_16BIT_IMAGE:
+	case F54_RAW_16BIT_IMAGE:
+	case F54_HIGH_RESISTANCE:
+	case F54_TX_TO_TX_SHORT:
+	case F54_RX_TO_RX1:
+	case F54_TRUE_BASELINE:
+	case F54_FULL_RAW_CAP_MIN_MAX:
+	case F54_RX_OPENS1:
+	case F54_TX_OPEN:
+	case F54_TX_TO_GROUND:
+	case F54_RX_TO_RX2:
+	case F54_RX_OPENS2:
+	case F54_FULL_RAW_CAP:
+	case F54_FULL_RAW_CAP_RX_COUPLING_COMP:
+		return true;
+		break;
+	default:
+		return false;
+	}
+}
+
+/* SYSFS file show/store functions */
+static ssize_t rmi_fn_54_report_type_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *instance_data;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", instance_data->report_type);
+}
+
+static ssize_t rmi_fn_54_report_type_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t count) {
+	int result;
+	unsigned long val;
+	unsigned char data;
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *instance_data;
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	/* need to convert the string data to an actual value */
+	result = strict_strtoul(buf, 10, &val);
+	if (result)
+		return result;
+	if (!is_report_type_valid(val)) {
+		dev_err(dev, "%s : Report type %d is invalid.\n",
+					__func__, (u8) val);
+		return -EINVAL;
+	}
+	mutex_lock(&instance_data->status_mutex);
+	if (instance_data->status != BUSY) {
+		instance_data->report_type = (enum f54_report_types)val;
+		data = (char)val;
+		/* Write the Report Type back to the first Block
+		 * Data registers (F54_AD_Data0). */
+		result =
+		    rmi_write_block(fc->rmi_dev, fc->fd.data_base_addr,
+								&data, 1);
+		mutex_unlock(&instance_data->status_mutex);
+		if (result < 0) {
+			dev_err(dev, "%s : Could not write report type to"
+				" 0x%x\n", __func__, fc->fd.data_base_addr);
+			return result;
+		}
+		return count;
+	} else {
+		dev_err(dev, "%s : Report type cannot be changed in the middle"
+				" of command.\n", __func__);
+		mutex_unlock(&instance_data->status_mutex);
+		return -EINVAL;
+	}
+}
+
+static ssize_t rmi_fn_54_get_report_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t count) {
+	unsigned long val;
+	int error, result;
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *instance_data;
+	struct rmi_driver *driver;
+	u8 command;
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+	driver = fc->rmi_dev->driver;
+
+	/* need to convert the string data to an actual value */
+	error = strict_strtoul(buf, 10, &val);
+	if (error)
+		return error;
+	/* Do nothing if not set to 1. This prevents accidental commands. */
+	if (val != 1)
+		return count;
+	command = (unsigned char)GET_REPORT;
+	/* Basic checks on report_type to ensure we write a valid type
+	 * to the sensor.
+	 * TODO: Check Query3 to see if some specific reports are
+	 * available. This is currently listed as a reserved register.
+	 */
+	if (!is_report_type_valid(instance_data->report_type)) {
+		dev_err(dev, "%s : Report type %d is invalid.\n",
+				__func__, instance_data->report_type);
+		return -EINVAL;
+	}
+	mutex_lock(&instance_data->status_mutex);
+	if (instance_data->status != IDLE) {
+		if (instance_data->status != BUSY) {
+			dev_err(dev, "F54 status is in an abnormal state: 0x%x",
+							instance_data->status);
+		}
+		mutex_unlock(&instance_data->status_mutex);
+		return count;
+	}
+	/* Store interrupts */
+	/* Do not exit if we fail to turn off interupts. We are likely
+	 * to still get useful data. The report data can, however, be
+	 * corrupted, and there may be unexpected behavior.
+	 */
+	dev_dbg(dev, "Storing and overriding interupts\n");
+	if (driver->store_irq_mask)
+		driver->store_irq_mask(fc->rmi_dev,
+					fc->irq_mask);
+	else
+		dev_err(dev, "No way to store interupts!\n");
+	instance_data->status = BUSY;
+
+	/* small delay to avoid race condition in firmare. This value is a bit
+	 * higher than absolutely necessary. Should be removed once issue is
+	 * resolved in firmware. */
+
+	mdelay(2);
+
+	/* Write the command to the command register */
+	result = rmi_write_block(fc->rmi_dev, fc->fd.command_base_addr,
+						&command, 1);
+	mutex_unlock(&instance_data->status_mutex);
+	if (result < 0) {
+		dev_err(dev, "%s : Could not write command to 0x%x\n",
+				__func__, fc->fd.command_base_addr);
+		return result;
+	}
+#if F54_WATCHDOG
+	/* start watchdog timer */
+	hrtimer_start(&instance_data->watchdog, ktime_set(1, 0),
+							HRTIMER_MODE_REL);
+#endif
+	return count;
+}
+
+static ssize_t rmi_fn_54_force_cal_store(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t count) {
+	unsigned long val;
+	int error, result;
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *instance_data;
+	struct rmi_driver *driver;
+	u8 command;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+	driver = fc->rmi_dev->driver;
+
+	/* need to convert the string data to an actual value */
+	error = strict_strtoul(buf, 10, &val);
+	if (error)
+		return error;
+	/* Do nothing if not set to 1. This prevents accidental commands. */
+	if (val != 1)
+		return count;
+
+	command = (unsigned char)FORCE_CAL;
+
+	if (instance_data->status == BUSY)
+		return -EBUSY;
+	/* Write the command to the command register */
+	result = rmi_write_block(fc->rmi_dev, fc->fd.command_base_addr,
+						&command, 1);
+	if (result < 0) {
+		dev_err(dev, "%s : Could not write command to 0x%x\n",
+				__func__, fc->fd.command_base_addr);
+		return result;
+	}
+	return count;
+}
+
+static ssize_t rmi_fn_54_status_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *instance_data;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", instance_data->status);
+}
+
+static ssize_t rmi_fn_54_num_rx_electrodes_show(struct device *dev,
+				     struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+				data->query.number_of_receiver_electrodes);
+}
+
+static ssize_t rmi_fn_54_num_tx_electrodes_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.number_of_transmitter_electrodes);
+}
+
+static ssize_t rmi_fn_54_has_image16_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_image16);
+}
+
+static ssize_t rmi_fn_54_has_image8_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_image8);
+}
+
+static ssize_t rmi_fn_54_has_baseline_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_baseline);
+}
+
+static ssize_t rmi_fn_54_clock_rate_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.clock_rate);
+}
+
+
+static ssize_t rmi_fn_54_touch_controller_family_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.touch_controller_family);
+}
+
+
+static ssize_t rmi_fn_54_has_pixel_touch_threshold_adjustment_show(
+		struct device *dev, struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_pixel_touch_threshold_adjustment);
+}
+
+static ssize_t rmi_fn_54_has_sensor_assignment_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_sensor_assignment);
+}
+
+static ssize_t rmi_fn_54_has_interference_metric_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_interference_metric);
+}
+
+static ssize_t rmi_fn_54_has_sense_frequency_control_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_sense_frequency_control);
+}
+
+static ssize_t rmi_fn_54_has_firmware_noise_mitigation_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_firmware_noise_mitigation);
+}
+
+static ssize_t rmi_fn_54_has_two_byte_report_rate_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_two_byte_report_rate);
+}
+
+static ssize_t rmi_fn_54_has_one_byte_report_rate_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_one_byte_report_rate);
+}
+
+static ssize_t rmi_fn_54_has_relaxation_control_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_relaxation_control);
+}
+
+static ssize_t rmi_fn_54_curve_compensation_mode_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.curve_compensation_mode);
+}
+
+static ssize_t rmi_fn_54_has_iir_filter_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_iir_filter);
+}
+
+static ssize_t rmi_fn_54_has_cmn_removal_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_cmn_removal);
+}
+
+static ssize_t rmi_fn_54_has_cmn_maximum_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_cmn_maximum);
+}
+
+static ssize_t rmi_fn_54_has_pixel_threshold_hysteresis_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_pixel_threshold_hysteresis);
+}
+
+static ssize_t rmi_fn_54_has_edge_compensation_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_edge_compensation);
+}
+
+static ssize_t rmi_fn_54_has_perf_frequency_noisecontrol_show(
+		struct device *dev, struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.has_perf_frequency_noisecontrol);
+}
+
+static ssize_t rmi_fn_54_number_of_sensing_frequencies_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *data;
+
+	fc = to_rmi_function_container(dev);
+	data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+			data->query.number_of_sensing_frequencies);
+}
+
+
+static ssize_t rmi_fn_54_no_auto_cal_show(struct device *dev,
+				struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *instance_data;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	return snprintf(buf, PAGE_SIZE, "%u\n",
+				instance_data->no_auto_cal ? 1 : 0);
+}
+
+static ssize_t rmi_fn_54_no_auto_cal_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t count) {
+	int result;
+	unsigned long val;
+	unsigned char data;
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *instance_data;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	/* need to convert the string data to an actual value */
+	result = strict_strtoul(buf, 10, &val);
+
+	/* if an error occured, return it */
+	if (result)
+		return result;
+	/* Do nothing if not 0 or 1. This prevents accidental commands. */
+	if (val > 1)
+		return count;
+	/* Read current control values */
+	result =
+	    rmi_read_block(fc->rmi_dev, fc->fd.control_base_addr, &data, 1);
+
+	/* if the current control registers are already set as we want them, do
+	 * nothing to them */
+	if ((data & 1) == val)
+		return count;
+	/* Write the control back to the control register (F54_AD_Ctrl0)
+	 * Ignores everything but bit 0 */
+	data = (data & ~1) | (val & 0x01); /* bit mask for lowest bit */
+	result =
+	    rmi_write_block(fc->rmi_dev, fc->fd.control_base_addr, &data, 1);
+	if (result < 0) {
+		dev_err(dev, "%s : Could not write control to 0x%x\n",
+		       __func__, fc->fd.control_base_addr);
+		return result;
+	}
+	/* update our internal representation iff the write succeeds */
+	instance_data->no_auto_cal = (val == 1);
+	return count;
+}
+
+static ssize_t rmi_fn_54_fifoindex_show(struct device *dev,
+				  struct device_attribute *attr, char *buf) {
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *instance_data;
+	struct rmi_driver *driver;
+	unsigned char temp_buf[2];
+	int retval;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+	driver = fc->rmi_dev->driver;
+
+	/* Read fifoindex from device */
+	retval = rmi_read_block(fc->rmi_dev,
+				fc->fd.data_base_addr + RMI_F54_FIFO_OFFSET,
+				temp_buf, ARRAY_SIZE(temp_buf));
+
+	if (retval < 0) {
+		dev_err(dev, "Could not read fifoindex from 0x%04x\n",
+		       fc->fd.data_base_addr + RMI_F54_FIFO_OFFSET);
+		return retval;
+	}
+	batohs(&instance_data->fifoindex, temp_buf);
+	return snprintf(buf, PAGE_SIZE, "%u\n", instance_data->fifoindex);
+}
+static ssize_t rmi_fn_54_fifoindex_store(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf,
+					size_t count)
+{
+	int error;
+	unsigned long val;
+	unsigned char data[2];
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *instance_data;
+
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+
+	/* need to convert the string data to an actual value */
+	error = strict_strtoul(buf, 10, &val);
+
+	if (error)
+		return error;
+
+	instance_data->fifoindex = val;
+
+	/* Write the FifoIndex back to the first data registers. */
+	hstoba(data, (unsigned short)val);
+
+	error = rmi_write_block(fc->rmi_dev,
+				fc->fd.data_base_addr + RMI_F54_FIFO_OFFSET,
+				data,
+				ARRAY_SIZE(data));
+
+	if (error < 0) {
+		dev_err(dev, "%s : Could not write fifoindex to 0x%x\n",
+		       __func__, fc->fd.data_base_addr + RMI_F54_FIFO_OFFSET);
+		return error;
+	}
+	return count;
+}
+
+/* Provide access to last report */
+#ifdef KERNEL_VERSION_ABOVE_2_6_32
+static ssize_t rmi_fn_54_data_read(struct file *data_file, struct kobject *kobj,
+#else
+static ssize_t rmi_fn_54_data_read(struct kobject *kobj,
+#endif
+				struct bin_attribute *attributes,
+				char *buf, loff_t pos, size_t count)
+{
+	struct device *dev;
+	struct rmi_function_container *fc;
+	struct rmi_fn_54_data *instance_data;
+
+	dev = container_of(kobj, struct device, kobj);
+	fc = to_rmi_function_container(dev);
+	instance_data = fc->data;
+	mutex_lock(&instance_data->data_mutex);
+	if (count < instance_data->report_size) {
+		dev_err(dev,
+			"%s: F54 report size too large for buffer: %d."
+				" Need at least: %d for Report type: %d.\n",
+			__func__, count, instance_data->report_size,
+			instance_data->report_type);
+		mutex_unlock(&instance_data->data_mutex);
+		return -EINVAL;
+	}
+	if (instance_data->report_data) {
+		/* Copy data from instance_data to buffer */
+		memcpy(buf, instance_data->report_data,
+					instance_data->report_size);
+		mutex_unlock(&instance_data->data_mutex);
+		dev_dbg(dev, "%s: Presumably successful.", __func__);
+		return instance_data->report_size;
+	} else {
+		dev_err(dev, "%s: F54 report_data does not exist!\n", __func__);
+		mutex_unlock(&instance_data->data_mutex);
+		return -EINVAL;
+	}
+}
+
+static struct rmi_function_handler function_handler = {
+	.func = 0x54,
+	.init = rmi_f54_init,
+	.attention = rmi_f54_attention
+};
+
+static int __init rmi_f54_module_init(void)
+{
+	int error;
+
+	error = rmi_register_function_driver(&function_handler);
+	if (error < 0) {
+		pr_err("%s: register failed!\n", __func__);
+		return error;
+	}
+	return 0;
+}
+
+static void rmi_f54_module_exit(void)
+{
+	rmi_unregister_function_driver(&function_handler);
+}
+
+module_init(rmi_f54_module_init);
+module_exit(rmi_f54_module_exit);
+
+MODULE_AUTHOR("Daniel Rosenberg <daniel.rosenberg@synaptics.com>");
+MODULE_DESCRIPTION("RMI F54 module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/rmi4/rmi_i2c.c b/drivers/input/touchscreen/rmi4/rmi_i2c.c
new file mode 100644
index 000000000000..15624f9cf54b
--- /dev/null
+++ b/drivers/input/touchscreen/rmi4/rmi_i2c.c
@@ -0,0 +1,421 @@
+/*
+ * Copyright (c) 2011 Synaptics Incorporated
+ * Copyright (c) 2011 Unixphere
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+#include <linux/kernel.h>
+#include <linux/lockdep.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/pm.h>
+#include <linux/gpio.h>
+#include <linux/rmi.h>
+
+#define COMMS_DEBUG 0
+
+#define IRQ_DEBUG 0
+
+#define RMI_PAGE_SELECT_REGISTER 0xff
+#define RMI_I2C_PAGE(addr) (((addr) >> 8) & 0xff)
+
+static char *phys_proto_name = "i2c";
+
+struct rmi_i2c_data {
+	struct mutex page_mutex;
+	int page;
+	int enabled;
+	int irq;
+	int irq_flags;
+	struct rmi_phys_device *phys;
+};
+
+static irqreturn_t rmi_i2c_irq_thread(int irq, void *p)
+{
+	struct rmi_phys_device *phys = p;
+	struct rmi_device *rmi_dev = phys->rmi_dev;
+	struct rmi_driver *driver = rmi_dev->driver;
+	struct rmi_device_platform_data *pdata = phys->dev->platform_data;
+
+#if IRQ_DEBUG
+	dev_dbg(phys->dev, "ATTN gpio, value: %d.\n",
+			gpio_get_value(irq_to_gpio(irq)));
+#endif
+	if (gpio_get_value(irq_to_gpio(irq)) == pdata->irq_polarity) {
+		phys->info.attn_count++;
+		if (driver && driver->irq_handler && rmi_dev)
+			driver->irq_handler(rmi_dev, irq);
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * rmi_set_page - Set RMI page
+ * @phys: The pointer to the rmi_phys_device struct
+ * @page: The new page address.
+ *
+ * RMI devices have 16-bit addressing, but some of the physical
+ * implementations (like SMBus) only have 8-bit addressing. So RMI implements
+ * a page address at 0xff of every page so we can reliable page addresses
+ * every 256 registers.
+ *
+ * The page_mutex lock must be held when this function is entered.
+ *
+ * Returns zero on success, non-zero on failure.
+ */
+static int rmi_set_page(struct rmi_phys_device *phys, unsigned int page)
+{
+	struct i2c_client *client = to_i2c_client(phys->dev);
+	struct rmi_i2c_data *data = phys->data;
+	char txbuf[2] = {RMI_PAGE_SELECT_REGISTER, page};
+	int retval;
+
+#if COMMS_DEBUG
+	dev_dbg(&client->dev, "RMI4 I2C writes 3 bytes: %02x %02x\n",
+		txbuf[0], txbuf[1]);
+#endif
+	phys->info.tx_count++;
+	phys->info.tx_bytes += sizeof(txbuf);
+	retval = i2c_master_send(client, txbuf, sizeof(txbuf));
+	if (retval != sizeof(txbuf)) {
+		phys->info.tx_errs++;
+		dev_err(&client->dev,
+			"%s: set page failed: %d.", __func__, retval);
+		return (retval < 0) ? retval : -EIO;
+	}
+	data->page = page;
+	return 0;
+}
+
+static int rmi_i2c_write_block(struct rmi_phys_device *phys, u16 addr, u8 *buf,
+			       int len)
+{
+	struct i2c_client *client = to_i2c_client(phys->dev);
+	struct rmi_i2c_data *data = phys->data;
+	u8 txbuf[len + 1];
+	int retval;
+#if	COMMS_DEBUG
+	int i;
+#endif
+
+	txbuf[0] = addr & 0xff;
+	memcpy(txbuf + 1, buf, len);
+
+	mutex_lock(&data->page_mutex);
+
+	if (RMI_I2C_PAGE(addr) != data->page) {
+		retval = rmi_set_page(phys, RMI_I2C_PAGE(addr));
+		if (retval < 0)
+			goto exit;
+	}
+
+#if COMMS_DEBUG
+	dev_dbg(&client->dev, "RMI4 I2C writes %d bytes: ", sizeof(txbuf));
+	for (i = 0; i < sizeof(txbuf); i++)
+		dev_dbg(&client->dev, "%02x ", txbuf[i]);
+	dev_dbg(&client->dev, "\n");
+#endif
+
+	phys->info.tx_count++;
+	phys->info.tx_bytes += sizeof(txbuf);
+	retval = i2c_master_send(client, txbuf, sizeof(txbuf));
+	if (retval < 0)
+		phys->info.tx_errs++;
+
+exit:
+	mutex_unlock(&data->page_mutex);
+	return retval;
+}
+
+static int rmi_i2c_write(struct rmi_phys_device *phys, u16 addr, u8 data)
+{
+	int retval = rmi_i2c_write_block(phys, addr, &data, 1);
+	return (retval < 0) ? retval : 0;
+}
+
+static int rmi_i2c_read_block(struct rmi_phys_device *phys, u16 addr, u8 *buf,
+			      int len)
+{
+	struct i2c_client *client = to_i2c_client(phys->dev);
+	struct rmi_i2c_data *data = phys->data;
+	u8 txbuf[1] = {addr & 0xff};
+	int retval;
+#if	COMMS_DEBUG
+	int i;
+#endif
+
+	mutex_lock(&data->page_mutex);
+
+	if (RMI_I2C_PAGE(addr) != data->page) {
+		retval = rmi_set_page(phys, RMI_I2C_PAGE(addr));
+		if (retval < 0)
+			goto exit;
+	}
+
+#if COMMS_DEBUG
+	dev_dbg(&client->dev, "RMI4 I2C writes 1 bytes: %02x\n", txbuf[0]);
+#endif
+	phys->info.tx_count++;
+	phys->info.tx_bytes += sizeof(txbuf);
+	retval = i2c_master_send(client, txbuf, sizeof(txbuf));
+	if (retval != sizeof(txbuf)) {
+		phys->info.tx_errs++;
+		retval = (retval < 0) ? retval : -EIO;
+		goto exit;
+	}
+
+	retval = i2c_master_recv(client, buf, len);
+
+	phys->info.rx_count++;
+	phys->info.rx_bytes += len;
+	if (retval < 0)
+		phys->info.rx_errs++;
+#if COMMS_DEBUG
+	else {
+		dev_dbg(&client->dev, "RMI4 I2C received %d bytes: ", len);
+		for (i = 0; i < len; i++)
+			dev_dbg(&client->dev, "%02x ", buf[i]);
+		dev_dbg(&client->dev, "\n");
+	}
+#endif
+
+exit:
+	mutex_unlock(&data->page_mutex);
+	return retval;
+}
+
+static int rmi_i2c_read(struct rmi_phys_device *phys, u16 addr, u8 *buf)
+{
+	int retval = rmi_i2c_read_block(phys, addr, buf, 1);
+	return (retval < 0) ? retval : 0;
+}
+
+
+static int acquire_attn_irq(struct rmi_i2c_data *data)
+{
+	return request_threaded_irq(data->irq, NULL, rmi_i2c_irq_thread,
+			data->irq_flags, dev_name(data->phys->dev), data->phys);
+}
+
+static int enable_device(struct rmi_phys_device *phys)
+{
+	int retval = 0;
+
+	struct rmi_i2c_data *data = phys->data;
+
+	if (data->enabled)
+		return 0;
+
+	retval = acquire_attn_irq(data);
+	if (retval)
+		goto error_exit;
+
+	data->enabled = true;
+	dev_dbg(phys->dev, "Physical device enabled.\n");
+	return 0;
+
+error_exit:
+	dev_err(phys->dev, "Failed to enable physical device. Code=%d.\n",
+		retval);
+	return retval;
+}
+
+
+static void disable_device(struct rmi_phys_device *phys)
+{
+	struct rmi_i2c_data *data = phys->data;
+
+	if (!data->enabled)
+		return;
+
+	disable_irq(data->irq);
+	free_irq(data->irq, data->phys);
+
+	dev_dbg(phys->dev, "Physical device disabled.\n");
+	data->enabled = false;
+}
+
+
+static int __devinit rmi_i2c_probe(struct i2c_client *client,
+				  const struct i2c_device_id *id)
+{
+	struct rmi_phys_device *rmi_phys;
+	struct rmi_i2c_data *data;
+	struct rmi_device_platform_data *pdata = client->dev.platform_data;
+	int error;
+
+	if (!pdata) {
+		dev_err(&client->dev, "no platform data\n");
+		return -EINVAL;
+	}
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "i2c_check_functionality error\n");
+		return -EIO;
+	}
+
+	rmi_phys = kzalloc(sizeof(struct rmi_phys_device), GFP_KERNEL);
+	if (!rmi_phys)
+		return -ENOMEM;
+
+	data = kzalloc(sizeof(struct rmi_i2c_data), GFP_KERNEL);
+	if (!data) {
+		error = -ENOMEM;
+		goto err_phys;
+	}
+
+	data->enabled = true;	/* We plan to come up enabled. */
+	data->irq = gpio_to_irq(pdata->irq);
+	data->irq_flags = (pdata->irq_polarity == RMI_IRQ_ACTIVE_HIGH) ?
+		IRQF_TRIGGER_RISING : IRQF_TRIGGER_FALLING;
+	data->phys = rmi_phys;
+
+	rmi_phys->data = data;
+	rmi_phys->dev = &client->dev;
+
+	rmi_phys->write = rmi_i2c_write;
+	rmi_phys->write_block = rmi_i2c_write_block;
+	rmi_phys->read = rmi_i2c_read;
+	rmi_phys->read_block = rmi_i2c_read_block;
+	rmi_phys->enable_device = enable_device;
+	rmi_phys->disable_device = disable_device;
+
+	rmi_phys->info.proto = phys_proto_name;
+
+	mutex_init(&data->page_mutex);
+
+	/* Setting the page to zero will (a) make sure the PSR is in a
+	 * known state, and (b) make sure we can talk to the device.
+	 */
+	error = rmi_set_page(rmi_phys, 0);
+	if (error) {
+		dev_err(&client->dev, "Failed to set page select to 0.\n");
+		goto err_data;
+	}
+
+	if (pdata->gpio_config) {
+		error = pdata->gpio_config(&client->dev, true);
+		if (error < 0) {
+			dev_err(&client->dev, "failed to setup irq %d\n",
+				pdata->irq);
+			goto err_data;
+		}
+	}
+
+	error = rmi_register_phys_device(rmi_phys);
+	if (error) {
+		dev_err(&client->dev,
+			"failed to register physical driver at 0x%.2X.\n",
+			client->addr);
+		goto err_data;
+	}
+	i2c_set_clientdata(client, rmi_phys);
+
+	if (pdata->irq > 0) {
+		error = acquire_attn_irq(data);
+		if (error < 0) {
+			dev_err(&client->dev,
+				"request_threaded_irq failed %d\n",
+				pdata->irq);
+			goto err_unregister;
+		}
+	}
+
+#if defined(CONFIG_RMI4_DEV)
+	error = gpio_export(pdata->irq, false);
+	if (error) {
+		dev_warn(&client->dev, "%s: WARNING: Failed to "
+				 "export ATTN gpio!\n", __func__);
+		error = 0;
+	} else {
+		error = gpio_export_link(&(rmi_phys->rmi_dev->dev), "attn",
+					pdata->irq);
+		if (error) {
+			dev_warn(&(rmi_phys->rmi_dev->dev), "%s: WARNING: "
+				"Failed to symlink ATTN gpio!\n", __func__);
+			error = 0;
+		} else {
+			dev_info(&(rmi_phys->rmi_dev->dev),
+				"%s: Exported GPIO %d.", __func__, pdata->irq);
+		}
+	}
+#endif /* CONFIG_RMI4_DEV */
+
+	dev_info(&client->dev, "registered rmi i2c driver at 0x%.2X.\n",
+			client->addr);
+	return 0;
+
+err_unregister:
+	rmi_unregister_phys_device(rmi_phys);
+err_data:
+	kfree(data);
+err_phys:
+	kfree(rmi_phys);
+	return error;
+}
+
+static int __devexit rmi_i2c_remove(struct i2c_client *client)
+{
+	struct rmi_phys_device *phys = i2c_get_clientdata(client);
+	struct rmi_device_platform_data *pd = client->dev.platform_data;
+
+	rmi_unregister_phys_device(phys);
+	kfree(phys->data);
+	kfree(phys);
+
+	if (pd->gpio_config)
+		pd->gpio_config(&client->dev, false);
+
+	return 0;
+}
+
+static const struct i2c_device_id rmi_id[] = {
+	{ "rmi", 0 },
+	{ "rmi-i2c", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, rmi_id);
+
+static struct i2c_driver rmi_i2c_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "rmi-i2c"
+	},
+	.id_table	= rmi_id,
+	.probe		= rmi_i2c_probe,
+	.remove		= __devexit_p(rmi_i2c_remove),
+};
+
+static int __init rmi_i2c_init(void)
+{
+	return i2c_add_driver(&rmi_i2c_driver);
+}
+
+static void __exit rmi_i2c_exit(void)
+{
+	i2c_del_driver(&rmi_i2c_driver);
+}
+
+MODULE_AUTHOR("Christopher Heiny <cheiny@synaptics.com>");
+MODULE_AUTHOR("Eric Andersson <eric.andersson@unixphere.com>");
+MODULE_DESCRIPTION("RMI i2c driver");
+MODULE_LICENSE("GPL");
+
+module_init(rmi_i2c_init);
+module_exit(rmi_i2c_exit);
diff --git a/drivers/input/touchscreen/rmi4/rmi_spi.c b/drivers/input/touchscreen/rmi4/rmi_spi.c
new file mode 100644
index 000000000000..71c98bc48453
--- /dev/null
+++ b/drivers/input/touchscreen/rmi4/rmi_spi.c
@@ -0,0 +1,866 @@
+/*
+ * Copyright (c) 2011 Synaptics Incorporated
+ * Copyright (c) 2011 Unixphere
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/spi/spi.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/completion.h>
+#include <linux/sched.h>
+#include <linux/gpio.h>
+#include <linux/rmi.h>
+
+#define COMMS_DEBUG 0
+
+#define RMI_PROTOCOL_VERSION_ADDRESS	0xa0fd
+#define SPI_V2_UNIFIED_READ		0xc0
+#define SPI_V2_WRITE			0x40
+#define SPI_V2_PREPARE_SPLIT_READ	0xc8
+#define SPI_V2_EXECUTE_SPLIT_READ	0xca
+
+#define RMI_SPI_BLOCK_DELAY_US		65
+#define RMI_SPI_BYTE_DELAY_US		65
+#define RMI_SPI_WRITE_DELAY_US		0
+
+#define RMI_V1_READ_FLAG		0x80
+
+#define RMI_PAGE_SELECT_REGISTER 0x00FF
+#define RMI_SPI_PAGE(addr) (((addr) >> 8) & 0x80)
+
+static char *spi_v1_proto_name = "spi";
+static char *spi_v2_proto_name = "spiv2";
+
+struct rmi_spi_data {
+	struct mutex page_mutex;
+	int page;
+	int (*set_page) (struct rmi_phys_device *phys, u8 page);
+	bool split_read_pending;
+	int enabled;
+	int irq;
+	int irq_flags;
+	struct rmi_phys_device *phys;
+	struct completion irq_comp;
+};
+
+static irqreturn_t rmi_spi_hard_irq(int irq, void *p)
+{
+	struct rmi_phys_device *phys = p;
+	struct rmi_spi_data *data = phys->data;
+	struct rmi_device_platform_data *pdata = phys->dev->platform_data;
+
+	if (data->split_read_pending &&
+		      gpio_get_value(irq_to_gpio(irq)) == pdata->irq_polarity) {
+		phys->info.attn_count++;
+		complete(&data->irq_comp);
+		return IRQ_HANDLED;
+	}
+
+	return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t rmi_spi_irq_thread(int irq, void *p)
+{
+	struct rmi_phys_device *phys = p;
+	struct rmi_device *rmi_dev = phys->rmi_dev;
+	struct rmi_driver *driver = rmi_dev->driver;
+	struct rmi_device_platform_data *pdata = phys->dev->platform_data;
+
+	if (gpio_get_value(irq_to_gpio(irq)) == pdata->irq_polarity) {
+		phys->info.attn_count++;
+		if (driver && driver->irq_handler)
+			driver->irq_handler(rmi_dev, irq);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int rmi_spi_xfer(struct rmi_phys_device *phys,
+		    const u8 *txbuf, unsigned n_tx, u8 *rxbuf, unsigned n_rx)
+{
+	struct spi_device *client = to_spi_device(phys->dev);
+	struct rmi_spi_data *v2_data = phys->data;
+	struct rmi_device_platform_data *pdata = phys->dev->platform_data;
+	int status;
+	struct spi_message message;
+	struct spi_transfer *xfers;
+	int total_bytes = n_tx + n_rx;
+	u8 local_buf[total_bytes];
+	int xfer_count = 0;
+	int xfer_index = 0;
+	int block_delay = n_rx > 0 ? pdata->spi_data.block_delay_us : 0;
+	int byte_delay = n_rx > 1 ? pdata->spi_data.read_delay_us : 0;
+	int write_delay = n_tx > 1 ? pdata->spi_data.write_delay_us : 0;
+#if COMMS_DEBUG
+	int i;
+#endif
+	// pr_info("in function ____%s____  \n", __func__);
+
+	if (v2_data->split_read_pending) {
+		block_delay =
+		    n_rx > 0 ? pdata->spi_data.split_read_block_delay_us : 0;
+		byte_delay =
+		    n_tx > 1 ? pdata->spi_data.split_read_byte_delay_us : 0;
+		write_delay = 0;
+	}
+
+	if (n_tx) {
+		phys->info.tx_count++;
+		phys->info.tx_bytes += n_tx;
+		if (write_delay)
+			xfer_count += n_tx;
+		else
+			xfer_count += 1;
+	}
+
+	if (n_rx) {
+		phys->info.rx_count++;
+		phys->info.rx_bytes += n_rx;
+		if (byte_delay)
+			xfer_count += n_rx;
+		else
+			xfer_count += 1;
+	}
+
+	xfers = kcalloc(xfer_count,
+			    sizeof(struct spi_transfer), GFP_KERNEL);
+	if (!xfers)
+		return -ENOMEM;
+
+	spi_message_init(&message);
+
+	if (n_tx) {
+		if (write_delay) {
+			for (xfer_index = 0; xfer_index < n_tx;
+					xfer_index++) {
+				memset(&xfers[xfer_index], 0,
+				       sizeof(struct spi_transfer));
+				xfers[xfer_index].len = 1;
+				xfers[xfer_index].delay_usecs = write_delay;
+				xfers[xfer_index].cs_change = 1;
+				xfers[xfer_index].tx_buf = txbuf + xfer_index;
+				spi_message_add_tail(&xfers[xfer_index],
+						     &message);
+			}
+		} else {
+			memset(&xfers[0], 0, sizeof(struct spi_transfer));
+			xfers[0].len = n_tx;
+			spi_message_add_tail(&xfers[0], &message);
+			memcpy(local_buf, txbuf, n_tx);
+			xfers[0].tx_buf = local_buf;
+			xfer_index++;
+		}
+		if (block_delay){
+			xfers[xfer_index-1].delay_usecs = block_delay;
+			xfers[xfer_index].cs_change = 1;
+		}
+	}
+	if (n_rx) {
+		if (byte_delay) {
+			int buffer_offset = n_tx;
+			for (; xfer_index < xfer_count; xfer_index++) {
+				memset(&xfers[xfer_index], 0,
+				       sizeof(struct spi_transfer));
+				xfers[xfer_index].len = 1;
+				xfers[xfer_index].delay_usecs = byte_delay;
+				xfers[xfer_index].cs_change = 1;
+				xfers[xfer_index].rx_buf =
+				    local_buf + buffer_offset;
+				buffer_offset++;
+				spi_message_add_tail(&xfers[xfer_index],
+						     &message);
+			}
+		} else {
+			memset(&xfers[xfer_index], 0,
+			       sizeof(struct spi_transfer));
+			xfers[xfer_index].len = n_rx;
+			xfers[xfer_index].rx_buf = local_buf + n_tx;
+			spi_message_add_tail(&xfers[xfer_index], &message);
+			xfer_index++;
+		}
+	}
+
+#if COMMS_DEBUG
+	if (n_tx) {
+		dev_info(&client->dev, "SPI sends %d bytes: ", n_tx);
+		for (i = 0; i < n_tx; i++)
+		  // dev_info(&client->dev, "%02X ", txbuf[i]);
+		  pr_info(": %02X ", txbuf[i]);
+		// dev_info(&client->dev, "\n");
+		pr_info("\n");
+	}
+#endif
+
+	/* do the i/o */
+	if (pdata->spi_data.cs_assert) {
+		status = pdata->spi_data.cs_assert(
+			pdata->spi_data.cs_assert_data, true);
+		if (!status) {
+			dev_err(phys->dev, "Failed to assert CS.");
+			/* nonzero means error */
+			status = -1;
+			goto error_exit;
+		} else
+			status = 0;
+	}
+
+	if (pdata->spi_data.pre_delay_us)
+		udelay(pdata->spi_data.pre_delay_us);
+
+	status = spi_sync(client, &message);
+
+	if (pdata->spi_data.post_delay_us)
+		udelay(pdata->spi_data.post_delay_us);
+
+	if (pdata->spi_data.cs_assert) {
+		status = pdata->spi_data.cs_assert(
+			pdata->spi_data.cs_assert_data, false);
+		if (!status) {
+			dev_err(phys->dev, "Failed to deassert CS.");
+			/* nonzero means error */
+			status = -1;
+			goto error_exit;
+		} else
+			status = 0;
+	}
+
+	if (status == 0) {
+		memcpy(rxbuf, local_buf + n_tx, n_rx);
+		status = message.status;
+	} else {
+		phys->info.tx_errs++;
+		phys->info.rx_errs++;
+		dev_err(phys->dev, "spi_sync failed with error code %d.",
+		       status);
+	}
+
+#if COMMS_DEBUG
+	if (n_rx) {
+		dev_info(&client->dev, "SPI received %d bytes: ", n_rx);
+		for (i = 0; i < n_rx; i++)
+		  // dev_info(&client->dev, "%02X ", rxbuf[i]);
+			pr_info(": %02X ", rxbuf[i]);
+		// dev_info(&client->dev, "\n");
+		pr_info("\n");
+	}
+#endif
+
+error_exit:
+	kfree(xfers);
+	return status;
+}
+
+static int rmi_spi_v2_write_block(struct rmi_phys_device *phys, u16 addr,
+				  u8 *buf, int len)
+{
+	struct rmi_spi_data *data = phys->data;
+	u8 txbuf[len + 4];
+	int error;
+	// pr_info("in function ____%s____  \n", __func__);
+
+	txbuf[0] = SPI_V2_WRITE;
+	txbuf[1] = (addr >> 8) & 0x00FF;
+	txbuf[2] = addr & 0x00FF;
+	txbuf[3] = len;
+
+	memcpy(&txbuf[4], buf, len);
+
+	mutex_lock(&data->page_mutex);
+
+	if (RMI_SPI_PAGE(addr) != data->page) {
+		error = data->set_page(phys, RMI_SPI_PAGE(addr));
+		if (error < 0)
+			goto exit;
+	}
+
+	error = rmi_spi_xfer(phys, buf, len + 4, NULL, 0);
+	if (error < 0)
+		goto exit;
+	error = len;
+
+exit:
+	mutex_unlock(&data->page_mutex);
+	return error;
+}
+
+static int rmi_spi_v2_write(struct rmi_phys_device *phys, u16 addr, u8 data)
+{
+	int error = rmi_spi_v2_write_block(phys, addr, &data, 1);
+	// pr_info("in function ____%s____  \n", __func__);
+
+	return (error == 1) ? 0 : error;
+}
+
+static int rmi_spi_v1_write_block(struct rmi_phys_device *phys, u16 addr,
+				  u8 *buf, int len)
+{
+	struct rmi_spi_data *data = phys->data;
+	unsigned char txbuf[len + 2];
+	int error;
+	// pr_info("in function ____%s____  \n", __func__);
+
+	txbuf[0] = addr >> 8;
+	txbuf[1] = addr;
+	memcpy(txbuf+2, buf, len);
+
+	mutex_lock(&data->page_mutex);
+
+	if (RMI_SPI_PAGE(addr) != data->page) {
+		error = data->set_page(phys, RMI_SPI_PAGE(addr));
+		if (error < 0)
+			goto exit;
+	}
+
+	error = rmi_spi_xfer(phys, txbuf, len + 2, NULL, 0);
+	if (error < 0)
+		goto exit;
+	error = len;
+
+exit:
+	mutex_unlock(&data->page_mutex);
+	return error;
+}
+
+static int rmi_spi_v1_write(struct rmi_phys_device *phys, u16 addr, u8 data)
+{
+	int error = rmi_spi_v1_write_block(phys, addr, &data, 1);
+	// pr_info("in function ____%s____  \n", __func__);
+
+	return (error == 1) ? 0 : error;
+}
+
+static int rmi_spi_v2_split_read_block(struct rmi_phys_device *phys, u16 addr,
+				       u8 *buf, int len)
+{
+	struct rmi_spi_data *data = phys->data;
+	u8 txbuf[4];
+	u8 rxbuf[len + 1]; /* one extra byte for read length */
+	int error;
+	// pr_info("in function ____%s____  \n", __func__);
+
+	txbuf[0] = SPI_V2_PREPARE_SPLIT_READ;
+	txbuf[1] = (addr >> 8) & 0x00FF;
+	txbuf[2] = addr & 0x00ff;
+	txbuf[3] = len;
+
+	mutex_lock(&data->page_mutex);
+
+	if (RMI_SPI_PAGE(addr) != data->page) {
+		error = data->set_page(phys, RMI_SPI_PAGE(addr));
+		if (error < 0)
+			goto exit;
+	}
+
+	data->split_read_pending = true;
+
+	error = rmi_spi_xfer(phys, txbuf, 4, NULL, 0);
+	if (error < 0) {
+		data->split_read_pending = false;
+		goto exit;
+	}
+
+	wait_for_completion(&data->irq_comp);
+
+	txbuf[0] = SPI_V2_EXECUTE_SPLIT_READ;
+	txbuf[1] = 0;
+
+	error = rmi_spi_xfer(phys, txbuf, 2, rxbuf, len + 1);
+	data->split_read_pending = false;
+	if (error < 0)
+		goto exit;
+
+	/* first byte is length */
+	if (rxbuf[0] != len) {
+		error = -EIO;
+		goto exit;
+	}
+
+	memcpy(buf, rxbuf + 1, len);
+	error = len;
+
+exit:
+	mutex_unlock(&data->page_mutex);
+	return error;
+}
+
+static int rmi_spi_v2_read_block(struct rmi_phys_device *phys, u16 addr,
+				 u8 *buf, int len)
+{
+	struct rmi_spi_data *data = phys->data;
+	u8 txbuf[4];
+	int error;
+	// pr_info("in function ____%s____  \n", __func__);
+
+	txbuf[0] = SPI_V2_UNIFIED_READ;
+	txbuf[1] = (addr >> 8) & 0x00FF;
+	txbuf[2] = addr & 0x00ff;
+	txbuf[3] = len;
+
+	mutex_lock(&data->page_mutex);
+
+	if (RMI_SPI_PAGE(addr) != data->page) {
+		error = data->set_page(phys, RMI_SPI_PAGE(addr));
+		if (error < 0)
+			goto exit;
+	}
+
+	error = rmi_spi_xfer(phys, txbuf, 4, buf, len);
+	if (error < 0)
+		goto exit;
+	error = len;
+
+exit:
+	mutex_unlock(&data->page_mutex);
+	return error;
+}
+
+static int rmi_spi_v2_read(struct rmi_phys_device *phys, u16 addr, u8 *buf)
+{
+	int error = rmi_spi_v2_read_block(phys, addr, buf, 1);
+
+	return (error == 1) ? 0 : error;
+}
+
+static int rmi_spi_v1_read_block(struct rmi_phys_device *phys, u16 addr,
+				 u8 *buf, int len)
+{
+	struct rmi_spi_data *data = phys->data;
+	u8 txbuf[2];
+	int error;
+	// pr_info("in function ____%s____  \n", __func__);
+
+	txbuf[0] = (addr >> 8) | RMI_V1_READ_FLAG;
+	txbuf[1] = addr;
+
+	mutex_lock(&data->page_mutex);
+
+	if (RMI_SPI_PAGE(addr) != data->page) {
+		error = data->set_page(phys, RMI_SPI_PAGE(addr));
+		if (error < 0)
+			goto exit;
+	}
+
+	error = rmi_spi_xfer(phys, txbuf, 2, buf, len);
+	// pr_info("  back in function %s, rmi_spi_xfer returned %d\n", __func__, error);
+
+	if (error < 0)
+		goto exit;
+	error = len;
+
+exit:
+	mutex_unlock(&data->page_mutex);
+	return error;
+}
+
+static int rmi_spi_v1_read(struct rmi_phys_device *phys, u16 addr, u8 *buf)
+{
+	int error = rmi_spi_v1_read_block(phys, addr, buf, 1);
+	pr_info("in function ____%s____  \n", __func__);
+
+	return (error == 1) ? 0 : error;
+}
+
+#define RMI_SPI_PAGE_SELECT_WRITE_LENGTH 1
+
+static int rmi_spi_v1_set_page(struct rmi_phys_device *phys, u8 page)
+{
+	struct rmi_spi_data *data = phys->data;
+	u8 txbuf[] = {RMI_PAGE_SELECT_REGISTER >> 8,
+		RMI_PAGE_SELECT_REGISTER & 0xFF, page};
+	int error;
+	pr_info("in function ____%s____  \n", __func__);
+
+	error = rmi_spi_xfer(phys, txbuf, sizeof(txbuf), NULL, 0);
+	if (error < 0) {
+		dev_err(phys->dev, "Failed to set page select, code: %d.\n",
+			error);
+		return error;
+	}
+
+	data->page = page;
+
+	return RMI_SPI_PAGE_SELECT_WRITE_LENGTH;
+}
+
+static int rmi_spi_v2_set_page(struct rmi_phys_device *phys, u8 page)
+{
+	struct rmi_spi_data *data = phys->data;
+
+	u8 txbuf[] = {SPI_V2_WRITE, RMI_PAGE_SELECT_REGISTER >> 8,
+		RMI_PAGE_SELECT_REGISTER & 0xFF,
+		RMI_SPI_PAGE_SELECT_WRITE_LENGTH, page};
+	int error;
+	pr_info("in function ____%s____  \n", __func__);
+
+	error = rmi_spi_xfer(phys, txbuf, sizeof(txbuf), NULL, 0);
+	if (error < 0) {
+		dev_err(phys->dev, "Failed to set page select, code: %d.\n",
+			error);
+		return error;
+	}
+
+	data->page = page;
+
+	return RMI_SPI_PAGE_SELECT_WRITE_LENGTH;
+}
+
+
+static int acquire_attn_irq(struct rmi_spi_data *data)
+{
+        int retval = 0;
+	pr_info("in function ____%s____\n", __func__);
+	pr_info("irq = %d\n", data->irq);
+	pr_info("data->irq_flags = 0x%8x\n", data->irq_flags);
+	pr_info("dev_name(data->phys->dev) = %s\n", dev_name(data->phys->dev));
+
+	retval =  request_threaded_irq(data->irq, rmi_spi_hard_irq,
+			rmi_spi_irq_thread, data->irq_flags,
+			dev_name(data->phys->dev), data->phys);
+
+	pr_info("retval = %d\n", retval);
+	return retval;
+}
+
+static int enable_device(struct rmi_phys_device *phys)
+{
+	int retval = 0;
+
+	struct rmi_spi_data *data = phys->data;
+
+	if (data->enabled) {
+		dev_info(phys->dev, "Physical device already enabled.\n");
+		return 0;
+	}
+
+	retval = acquire_attn_irq(data);
+	if (retval)
+		goto error_exit;
+
+	data->enabled = true;
+	dev_info(phys->dev, "Physical device enabled.\n");
+	return 0;
+
+error_exit:
+	dev_err(phys->dev, "Failed to enable physical device. Code=%d.\n",
+		retval);
+	return retval;
+}
+
+
+static void disable_device(struct rmi_phys_device *phys)
+{
+	struct rmi_spi_data *data = phys->data;
+
+	pr_info("in function ____%s____  \n", __func__);
+	if (!data->enabled) {
+		dev_warn(phys->dev, "Physical device already disabled.\n");
+		return;
+	}
+	disable_irq(data->irq);
+	free_irq(data->irq, data->phys);
+
+	dev_info(phys->dev, "Physical device disabled.\n");
+	data->enabled = false;
+}
+
+
+
+#define DUMMY_READ_SLEEP_US 10
+
+static int rmi_spi_check_device(struct rmi_phys_device *rmi_phys)
+{
+	u8 buf[6];
+	int error;
+	int i;
+
+	pr_info("in function ____%s____  \n", __func__);
+
+	/* Some SPI subsystems return 0 for the very first read you do.  So
+	 * we use this dummy read to get that out of the way.
+	 */
+	error = rmi_spi_v1_read_block(rmi_phys, PDT_START_SCAN_LOCATION,
+				      buf, sizeof(buf));
+	if (error < 0) {
+		dev_err(rmi_phys->dev, "dummy read failed with %d.\n", error);
+		return error;
+	}
+	udelay(DUMMY_READ_SLEEP_US);
+
+	/* Force page select to 0.
+	 */
+	error = rmi_spi_v1_set_page(rmi_phys, 0x00);
+	if (error < 0)
+		return error;
+
+	/* Now read the first PDT entry.  We know where this is, and if the
+	 * RMI4 device is out there, these 6 bytes will be something other
+	 * than all 0x00 or 0xFF.  We need to check for 0x00 and 0xFF,
+	 * because many (maybe all) SPI implementations will return all 0x00
+	 * or all 0xFF on read if the device is not connected.
+	 */
+	error = rmi_spi_v1_read_block(rmi_phys, PDT_START_SCAN_LOCATION,
+				      buf, sizeof(buf));
+	if (error < 0) {
+		dev_err(rmi_phys->dev, "probe read failed with %d.\n", error);
+		return error;
+	}
+
+	dev_info(rmi_phys->dev, "probe read succeeded  with %d.\n", error);
+	for (i = 0; i < sizeof(buf); i++) {
+		if (buf[i] != 0x00 && buf[i] != 0xFF)
+			return error;
+	}
+
+	dev_err(rmi_phys->dev, "probe read returned invalid block.\n");
+	return -ENODEV;
+}
+
+static int __devinit rmi_spi_probe(struct spi_device *spi)
+{
+	struct rmi_phys_device *rmi_phys;
+	struct rmi_spi_data *data;
+	struct rmi_device_platform_data *pdata = spi->dev.platform_data;
+	u8 buf[2];
+	int error;
+
+        pr_info("%s: probe for rmi_spi device\n", __func__);
+
+	if (!pdata) {
+		dev_err(&spi->dev, "no platform data\n");
+		return -EINVAL;
+	}
+
+	if (spi->master->flags & SPI_MASTER_HALF_DUPLEX)
+		return -EINVAL;
+
+	spi->bits_per_word = 8;
+	spi->mode = SPI_MODE_3;
+	error = spi_setup(spi);
+	if (error < 0) {
+		dev_err(&spi->dev, "spi_setup failed!\n");
+		return error;
+	}
+
+	rmi_phys = kzalloc(sizeof(struct rmi_phys_device), GFP_KERNEL);
+	if (!rmi_phys)
+		return -ENOMEM;
+
+	data = kzalloc(sizeof(struct rmi_spi_data), GFP_KERNEL);
+	if (!data) {
+		error = -ENOMEM;
+		goto err_phys;
+	}
+	data->enabled = true;	/* We plan to come up enabled. */
+	data->irq = gpio_to_irq(pdata->irq);
+	data->irq_flags = (pdata->irq_polarity == RMI_IRQ_ACTIVE_HIGH) ?
+		IRQF_TRIGGER_RISING : IRQF_TRIGGER_FALLING;
+	data->phys = rmi_phys;
+
+	rmi_phys->data = data;
+	rmi_phys->dev = &spi->dev;
+
+	rmi_phys->write = rmi_spi_v1_write;
+	rmi_phys->write_block = rmi_spi_v1_write_block;
+	rmi_phys->read = rmi_spi_v1_read;
+	rmi_phys->read_block = rmi_spi_v1_read_block;
+	rmi_phys->enable_device = enable_device;
+	rmi_phys->disable_device = disable_device;
+	data->set_page = rmi_spi_v1_set_page;
+
+	rmi_phys->info.proto = spi_v1_proto_name;
+
+	mutex_init(&data->page_mutex);
+
+	pr_info("%s:    setting the driverdata on the device\n", __func__);
+
+	dev_set_drvdata(&spi->dev, rmi_phys);
+
+	pr_info("%s:    done setting driverdata %s\n", __func__, dev_name(&spi->dev));
+
+
+	pdata->spi_data.block_delay_us = pdata->spi_data.block_delay_us ?
+			pdata->spi_data.block_delay_us : RMI_SPI_BLOCK_DELAY_US;
+	pdata->spi_data.read_delay_us = pdata->spi_data.read_delay_us ?
+			pdata->spi_data.read_delay_us : RMI_SPI_BYTE_DELAY_US;
+	pdata->spi_data.write_delay_us = pdata->spi_data.write_delay_us ?
+			pdata->spi_data.write_delay_us : RMI_SPI_BYTE_DELAY_US;
+	pdata->spi_data.split_read_block_delay_us =
+			pdata->spi_data.split_read_block_delay_us ?
+			pdata->spi_data.split_read_block_delay_us :
+			RMI_SPI_BLOCK_DELAY_US;
+	pdata->spi_data.split_read_byte_delay_us =
+			pdata->spi_data.split_read_byte_delay_us ?
+			pdata->spi_data.split_read_byte_delay_us :
+			RMI_SPI_BYTE_DELAY_US;
+
+	pr_info("%s     configuring GPIOs\n", __func__);
+
+	if (pdata->gpio_config) {
+		error = pdata->gpio_config(&spi->dev, true);
+		if (error < 0) {
+			dev_err(&spi->dev, "Failed to setup GPIOs, code: %d.\n",
+				error);
+			goto err_data;
+		}
+	}
+
+	error = rmi_spi_check_device(rmi_phys);
+	if (error < 0)
+		goto err_data;
+
+	/* check if this is an SPI v2 device */
+	dev_info(&spi->dev, "%s:    checking SPI version on RMI device\n", __func__);
+	error = rmi_spi_v1_read_block(rmi_phys, RMI_PROTOCOL_VERSION_ADDRESS,
+				      buf, 2);
+
+	if (error < 0) {
+	        dev_info(&spi->dev, "failed to get SPI version number!\n");
+		dev_err(&spi->dev, "failed to get SPI version number!\n");
+		goto err_data;
+	}
+
+	dev_info(&spi->dev, "SPI version is %d", buf[0]);
+
+	if (buf[0] == 1) {
+		/* SPIv2 */
+		rmi_phys->write		= rmi_spi_v2_write;
+		rmi_phys->write_block	= rmi_spi_v2_write_block;
+		rmi_phys->read		= rmi_spi_v2_read;
+		data->set_page		= rmi_spi_v2_set_page;
+
+		rmi_phys->info.proto = spi_v2_proto_name;
+
+		if (pdata->irq > 0) {
+			init_completion(&data->irq_comp);
+			rmi_phys->read_block = rmi_spi_v2_split_read_block;
+		} else {
+			rmi_phys->read_block = rmi_spi_v2_read_block;
+		}
+	} else if (buf[0] != 0) {
+		dev_err(&spi->dev, "Unrecognized SPI version %d.\n", buf[0]);
+		error = -ENODEV;
+		goto err_data;
+	}
+
+	error = rmi_register_phys_device(rmi_phys);
+	if (error) {
+		dev_err(&spi->dev, "failed to register physical driver\n");
+		goto err_data;
+	}
+
+	if (pdata->irq > 0) {
+		error = acquire_attn_irq(data);
+		if (error < 0) {
+			dev_err(&spi->dev, "request_threaded_irq failed %d\n",
+				pdata->irq);
+			goto err_unregister;
+		}
+	}
+
+#if defined(CONFIG_RMI4_DEV)
+	pr_info("          CONFIG_RMI4_DEV is defined\n");
+
+	error = gpio_export(pdata->irq, false);
+	if (error) {
+		dev_warn(&spi->dev, "WARNING: Failed to export ATTN gpio!\n");
+		error = 0;
+	} else {
+		error = gpio_export_link(&(rmi_phys->rmi_dev->dev), "attn",
+					pdata->irq);
+		if (error) {
+			dev_warn(&(rmi_phys->rmi_dev->dev), "WARNING: "
+				"Failed to symlink ATTN gpio!\n");
+			error = 0;
+		} else {
+			dev_info(&(rmi_phys->rmi_dev->dev),
+				"%s: Exported GPIO %d.", __func__, pdata->irq);
+		}
+	}
+#endif /* CONFIG_RMI4_DEV */
+
+	dev_info(&spi->dev, "registered RMI SPI driver\n");
+	return 0;
+
+err_unregister:
+	rmi_unregister_phys_device(rmi_phys);
+err_data:
+	kfree(data);
+err_phys:
+	kfree(rmi_phys);
+	return error;
+}
+
+static int __devexit rmi_spi_remove(struct spi_device *spi)
+{
+	struct rmi_phys_device *phys = dev_get_drvdata(&spi->dev);
+	struct rmi_device_platform_data *pd = spi->dev.platform_data;
+	pr_info("in function ____%s____  \n", __func__);
+
+	rmi_unregister_phys_device(phys);
+	kfree(phys->data);
+	kfree(phys);
+
+	if (pd->gpio_config)
+		pd->gpio_config(&spi->dev, false);
+
+	return 0;
+}
+
+static const struct spi_device_id rmi_id[] = {
+	{ "rmi", 0 },
+	{ "rmi_spi", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(spi, rmi_id);
+
+static struct spi_driver rmi_spi_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "rmi_spi",
+		//.mod_name = "rmi_spi",
+		//.bus    = &spi_bus_type,
+	},
+	.id_table	= rmi_id,
+	.probe		= rmi_spi_probe,
+	.remove		= __devexit_p(rmi_spi_remove),
+};
+
+static int __init rmi_spi_init(void)
+{
+        pr_info("%s: registering synaptics spi driver (ref=124)\n", __func__);
+        pr_info("             driver.owner        =  0x%x\n", (unsigned int)rmi_spi_driver.driver.owner);
+        pr_info("             driver.name         =  %s\n", rmi_spi_driver.driver.name);
+        pr_info("             id_table[0].name    =  %s\n", rmi_spi_driver.id_table[0].name );
+        pr_info("             id_table[1].name    =  %s\n", rmi_spi_driver.id_table[1].name );
+        pr_info("             probe function ptr  =  0x%x\n", (unsigned int)rmi_spi_driver.probe );
+
+
+	return spi_register_driver(&rmi_spi_driver);
+}
+
+static void __exit rmi_spi_exit(void)
+{
+	spi_unregister_driver(&rmi_spi_driver);
+}
+
+MODULE_AUTHOR("Christopher Heiny <cheiny@synaptics.com>");
+MODULE_DESCRIPTION("RMI SPI driver");
+MODULE_LICENSE("GPL");
+
+module_init(rmi_spi_init);
+module_exit(rmi_spi_exit);
diff --git a/drivers/input/touchscreen/stmpe-ts.c b/drivers/input/touchscreen/stmpe-ts.c
index ae88e13c99ff..dbd94f3c957a 100644
--- a/drivers/input/touchscreen/stmpe-ts.c
+++ b/drivers/input/touchscreen/stmpe-ts.c
@@ -48,17 +48,6 @@
 
 #define STMPE_IRQ_TOUCH_DET		0
 
-#define SAMPLE_TIME(x)			((x & 0xf) << 4)
-#define MOD_12B(x)			((x & 0x1) << 3)
-#define REF_SEL(x)			((x & 0x1) << 1)
-#define ADC_FREQ(x)			(x & 0x3)
-#define AVE_CTRL(x)			((x & 0x3) << 6)
-#define DET_DELAY(x)			((x & 0x7) << 3)
-#define SETTLING(x)			(x & 0x7)
-#define FRACTION_Z(x)			(x & 0x7)
-#define I_DRIVE(x)			(x & 0x1)
-#define OP_MODE(x)			((x & 0x7) << 1)
-
 #define STMPE_TS_NAME			"stmpe-ts"
 #define XY_MASK				0xfff
 
@@ -118,6 +107,7 @@ static void stmpe_work(struct work_struct *work)
 	__stmpe_reset_fifo(ts->stmpe);
 
 	input_report_abs(ts->idev, ABS_PRESSURE, 0);
+	input_report_key(ts->idev, BTN_TOUCH, 0);
 	input_sync(ts->idev);
 }
 
@@ -148,9 +138,17 @@ static irqreturn_t stmpe_ts_handler(int irq, void *data)
 	y = ((data_set[1] & 0xf) << 8) | data_set[2];
 	z = data_set[3];
 
+#ifndef CONFIG_ANDROID
 	input_report_abs(ts->idev, ABS_X, x);
 	input_report_abs(ts->idev, ABS_Y, y);
+#else /* !CONFIG_ANDROID */
+	/* Hack: rotate touch for now due to missing calibration integration
+	   Note: 12-bit touch resolution */
+	input_report_abs(ts->idev, ABS_X, 4096 - x);
+	input_report_abs(ts->idev, ABS_Y, 4096 - y);
+#endif /* !CONFIG_ANDROID */
 	input_report_abs(ts->idev, ABS_PRESSURE, z);
+	input_report_key(ts->idev, BTN_TOUCH, (z != 0));
 	input_sync(ts->idev);
 
        /* flush the FIFO after we have read out our values. */
@@ -161,7 +159,7 @@ static irqreturn_t stmpe_ts_handler(int irq, void *data)
 			STMPE_TSC_CTRL_TSC_EN, STMPE_TSC_CTRL_TSC_EN);
 
 	/* start polling for touch_det to detect release */
-	schedule_delayed_work(&ts->work, HZ / 50);
+	schedule_delayed_work(&ts->work, HZ / 10);
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/input/touchscreen/synaptics_i2c_rmi.c b/drivers/input/touchscreen/synaptics_i2c_rmi.c
new file mode 100644
index 000000000000..6f9b83af0359
--- /dev/null
+++ b/drivers/input/touchscreen/synaptics_i2c_rmi.c
@@ -0,0 +1,699 @@
+/* drivers/input/keyboard/synaptics_i2c_rmi.c
+ *
+ * Copyright (C) 2007 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/module.h>
+#include <linux/delay.h>
+#include <linux/earlysuspend.h>
+#include <linux/hrtimer.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/synaptics_i2c_rmi.h>
+
+#define ABS_DIFF(a, b)   (((a) > (b)) ? ((a) - (b)) : ((b) - (a)))
+
+static struct workqueue_struct *synaptics_wq;
+
+struct synaptics_ts_data {
+	uint16_t addr;
+	struct i2c_client *client;
+	struct input_dev *input_dev;
+	int use_irq;
+	bool has_relative_report;
+	struct hrtimer timer;
+	struct work_struct  work;
+	uint16_t max[2];
+	int snap_state[2][2];
+	int snap_down_on[2];
+	int snap_down_off[2];
+	int snap_up_on[2];
+	int snap_up_off[2];
+	int snap_down[2];
+	int snap_up[2];
+	uint32_t flags;
+	int reported_finger_count;
+	int last_pos[2][2];
+	int8_t sensitivity_adjust;
+	int (*power)(int on);
+	struct early_suspend early_suspend;
+};
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void synaptics_ts_early_suspend(struct early_suspend *h);
+static void synaptics_ts_late_resume(struct early_suspend *h);
+#endif
+
+static int synaptics_init_panel(struct synaptics_ts_data *ts)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(ts->client, 0xff, 0x10); /* page select = 0x10 */
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_write_byte_data failed for page select\n");
+		goto err_page_select_failed;
+	}
+	ret = i2c_smbus_write_byte_data(ts->client, 0x41, 0x04); /* Set "No Clip Z" */
+	if (ret < 0)
+		printk(KERN_ERR "i2c_smbus_write_byte_data failed for No Clip Z\n");
+
+	ret = i2c_smbus_write_byte_data(ts->client, 0x44,
+					ts->sensitivity_adjust);
+	if (ret < 0)
+		pr_err("synaptics_ts: failed to set Sensitivity Adjust\n");
+
+err_page_select_failed:
+	ret = i2c_smbus_write_byte_data(ts->client, 0xff, 0x04); /* page select = 0x04 */
+	if (ret < 0)
+		printk(KERN_ERR "i2c_smbus_write_byte_data failed for page select\n");
+	ret = i2c_smbus_write_byte_data(ts->client, 0xf0, 0x81); /* normal operation, 80 reports per second */
+	if (ret < 0)
+		printk(KERN_ERR "synaptics_ts_resume: i2c_smbus_write_byte_data failed\n");
+	return ret;
+}
+
+static void synaptics_ts_work_func(struct work_struct *work)
+{
+	int i;
+	int ret;
+	int bad_data = 0;
+	struct i2c_msg msg[2];
+	uint8_t start_reg;
+	uint8_t buf[15];
+	struct synaptics_ts_data *ts = container_of(work, struct synaptics_ts_data, work);
+	int buf_len = ts->has_relative_report ? 15 : 13;
+
+	msg[0].addr = ts->client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &start_reg;
+	start_reg = 0x00;
+	msg[1].addr = ts->client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = buf_len;
+	msg[1].buf = buf;
+
+	/* printk("synaptics_ts_work_func\n"); */
+	for (i = 0; i < ((ts->use_irq && !bad_data) ? 1 : 10); i++) {
+		ret = i2c_transfer(ts->client->adapter, msg, 2);
+		if (ret < 0) {
+			printk(KERN_ERR "synaptics_ts_work_func: i2c_transfer failed\n");
+			bad_data = 1;
+		} else {
+			/* printk("synaptics_ts_work_func: %x %x %x %x %x %x" */
+			/*        " %x %x %x %x %x %x %x %x %x, ret %d\n", */
+			/*        buf[0], buf[1], buf[2], buf[3], */
+			/*        buf[4], buf[5], buf[6], buf[7], */
+			/*        buf[8], buf[9], buf[10], buf[11], */
+			/*        buf[12], buf[13], buf[14], ret); */
+			if ((buf[buf_len - 1] & 0xc0) != 0x40) {
+				printk(KERN_WARNING "synaptics_ts_work_func:"
+				       " bad read %x %x %x %x %x %x %x %x %x"
+				       " %x %x %x %x %x %x, ret %d\n",
+				       buf[0], buf[1], buf[2], buf[3],
+				       buf[4], buf[5], buf[6], buf[7],
+				       buf[8], buf[9], buf[10], buf[11],
+				       buf[12], buf[13], buf[14], ret);
+				if (bad_data)
+					synaptics_init_panel(ts);
+				bad_data = 1;
+				continue;
+			}
+			bad_data = 0;
+			if ((buf[buf_len - 1] & 1) == 0) {
+				/* printk("read %d coordinates\n", i); */
+				break;
+			} else {
+				int pos[2][2];
+				int rmpos_x, rmpos_y;
+				int f, a;
+				int base;
+				/* int x = buf[3] | (uint16_t)(buf[2] & 0x1f) << 8; */
+				/* int y = buf[5] | (uint16_t)(buf[4] & 0x1f) << 8; */
+				int z = buf[1];
+				int w = buf[0] >> 4;
+				int finger = buf[0] & 7;
+
+				/* int x2 = buf[3+6] | (uint16_t)(buf[2+6] & 0x1f) << 8; */
+				/* int y2 = buf[5+6] | (uint16_t)(buf[4+6] & 0x1f) << 8; */
+				/* int z2 = buf[1+6]; */
+				/* int w2 = buf[0+6] >> 4; */
+				/* int finger2 = buf[0+6] & 7; */
+
+				/* int dx = (int8_t)buf[12]; */
+				/* int dy = (int8_t)buf[13]; */
+				int finger2_pressed;
+
+				/* printk("x %4d, y %4d, z %3d, w %2d, F %d, 2nd: x %4d, y %4d, z %3d, w %2d, F %d, dx %4d, dy %4d\n", */
+				/*	x, y, z, w, finger, */
+				/*	x2, y2, z2, w2, finger2, */
+				/*	dx, dy); */
+
+				base = 2;
+				for (f = 0; f < 2; f++) {
+					uint32_t flip_flag = SYNAPTICS_FLIP_X;
+					for (a = 0; a < 2; a++) {
+						int p = buf[base + 1];
+						p |= (uint16_t)(buf[base] & 0x1f) << 8;
+						if (ts->flags & flip_flag)
+							p = ts->max[a] - p;
+						if (ts->flags & SYNAPTICS_SNAP_TO_INACTIVE_EDGE) {
+							if (ts->snap_state[f][a]) {
+								if (p <= ts->snap_down_off[a])
+									p = ts->snap_down[a];
+								else if (p >= ts->snap_up_off[a])
+									p = ts->snap_up[a];
+								else
+									ts->snap_state[f][a] = 0;
+							} else {
+								if (p <= ts->snap_down_on[a]) {
+									p = ts->snap_down[a];
+									ts->snap_state[f][a] = 1;
+								} else if (p >= ts->snap_up_on[a]) {
+									p = ts->snap_up[a];
+									ts->snap_state[f][a] = 1;
+								}
+							}
+						}
+						pos[f][a] = p;
+						base += 2;
+						flip_flag <<= 1;
+					}
+					base += 2;
+					if (ts->flags & SYNAPTICS_SWAP_XY)
+						swap(pos[f][0], pos[f][1]);
+				}
+
+				if (!finger) {
+					z = 0;
+					if (ts->reported_finger_count > 0) {
+						pos[0][0] = ts->last_pos[0][0];
+						pos[0][1] = ts->last_pos[0][1];
+					}
+					else
+						continue; /* skip touch noise */
+				}
+				finger = (finger == 7) ? 1 : finger; /* correct wrong finger count */
+				finger2_pressed = finger > 1;
+
+				input_report_abs(ts->input_dev, ABS_MT_TOUCH_MAJOR, z);
+				input_report_abs(ts->input_dev, ABS_MT_WIDTH_MAJOR, w);
+				input_report_abs(ts->input_dev, ABS_MT_POSITION_X, pos[0][0]);
+				input_report_abs(ts->input_dev, ABS_MT_POSITION_Y, pos[0][1]);
+				input_mt_sync(ts->input_dev);
+				if (finger2_pressed) {
+					input_report_abs(ts->input_dev, ABS_MT_TOUCH_MAJOR, z);
+					input_report_abs(ts->input_dev, ABS_MT_WIDTH_MAJOR, w);
+					input_report_abs(ts->input_dev, ABS_MT_POSITION_X, pos[1][0]);
+					input_report_abs(ts->input_dev, ABS_MT_POSITION_Y, pos[1][1]);
+					input_mt_sync(ts->input_dev);
+					ts->last_pos[1][0] = pos[1][0];
+					ts->last_pos[1][1] = pos[1][1];
+				} else if (ts->reported_finger_count > 1) {
+					/* check which point was removed */
+					if ((ABS_DIFF(pos[0][0],ts->last_pos[0][0]) +
+					ABS_DIFF(pos[0][1],ts->last_pos[0][1])) <
+					(ABS_DIFF(pos[0][0],ts->last_pos[1][0]) +
+					ABS_DIFF(pos[0][1],ts->last_pos[1][1]))) {
+						rmpos_x = ts->last_pos[1][0];
+						rmpos_y = ts->last_pos[1][1];
+					}
+					else {
+						rmpos_x = ts->last_pos[0][0];
+						rmpos_y = ts->last_pos[0][1];
+					}
+					input_report_abs(ts->input_dev, ABS_MT_TOUCH_MAJOR, 0);
+					input_report_abs(ts->input_dev, ABS_MT_WIDTH_MAJOR, 0);
+					input_report_abs(ts->input_dev, ABS_MT_POSITION_X, rmpos_x);
+					input_report_abs(ts->input_dev, ABS_MT_POSITION_Y, rmpos_y);
+					input_mt_sync(ts->input_dev);
+				}
+
+				if (z) {
+					input_report_abs(ts->input_dev, ABS_X, pos[0][0]);
+					input_report_abs(ts->input_dev, ABS_Y, pos[0][1]);
+					ts->last_pos[0][0] = pos[0][0];
+					ts->last_pos[0][1] = pos[0][1];
+				}
+				input_report_abs(ts->input_dev, ABS_PRESSURE, z);
+				input_report_abs(ts->input_dev, ABS_TOOL_WIDTH, w);
+				input_report_key(ts->input_dev, BTN_TOUCH, finger);
+				ts->reported_finger_count = finger;
+				input_sync(ts->input_dev);
+			}
+		}
+	}
+	if (ts->use_irq)
+		enable_irq(ts->client->irq);
+}
+
+static enum hrtimer_restart synaptics_ts_timer_func(struct hrtimer *timer)
+{
+	struct synaptics_ts_data *ts = container_of(timer, struct synaptics_ts_data, timer);
+	/* printk("synaptics_ts_timer_func\n"); */
+
+	queue_work(synaptics_wq, &ts->work);
+
+	hrtimer_start(&ts->timer, ktime_set(0, 12500000), HRTIMER_MODE_REL);
+	return HRTIMER_NORESTART;
+}
+
+static irqreturn_t synaptics_ts_irq_handler(int irq, void *dev_id)
+{
+	struct synaptics_ts_data *ts = dev_id;
+
+	/* printk("synaptics_ts_irq_handler\n"); */
+	disable_irq_nosync(ts->client->irq);
+	queue_work(synaptics_wq, &ts->work);
+	return IRQ_HANDLED;
+}
+
+static int synaptics_ts_probe(
+	struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct synaptics_ts_data *ts;
+	uint8_t buf0[4];
+	uint8_t buf1[8];
+	struct i2c_msg msg[2];
+	int ret = 0;
+	uint16_t max_x, max_y;
+	int fuzz_x, fuzz_y, fuzz_p, fuzz_w;
+	struct synaptics_i2c_rmi_platform_data *pdata;
+	unsigned long irqflags;
+	int inactive_area_left;
+	int inactive_area_right;
+	int inactive_area_top;
+	int inactive_area_bottom;
+	int snap_left_on;
+	int snap_left_off;
+	int snap_right_on;
+	int snap_right_off;
+	int snap_top_on;
+	int snap_top_off;
+	int snap_bottom_on;
+	int snap_bottom_off;
+	uint32_t panel_version;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		printk(KERN_ERR "synaptics_ts_probe: need I2C_FUNC_I2C\n");
+		ret = -ENODEV;
+		goto err_check_functionality_failed;
+	}
+
+	ts = kzalloc(sizeof(*ts), GFP_KERNEL);
+	if (ts == NULL) {
+		ret = -ENOMEM;
+		goto err_alloc_data_failed;
+	}
+	INIT_WORK(&ts->work, synaptics_ts_work_func);
+	ts->client = client;
+	i2c_set_clientdata(client, ts);
+	pdata = client->dev.platform_data;
+	if (pdata)
+		ts->power = pdata->power;
+	if (ts->power) {
+		ret = ts->power(1);
+		if (ret < 0) {
+			printk(KERN_ERR "synaptics_ts_probe power on failed\n");
+			goto err_power_failed;
+		}
+	}
+
+	ret = i2c_smbus_write_byte_data(ts->client, 0xf4, 0x01); /* device command = reset */
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_write_byte_data failed\n");
+		/* fail? */
+	}
+	{
+		int retry = 10;
+		while (retry-- > 0) {
+			ret = i2c_smbus_read_byte_data(ts->client, 0xe4);
+			if (ret >= 0)
+				break;
+			msleep(100);
+		}
+	}
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_byte_data failed\n");
+		goto err_detect_failed;
+	}
+	printk(KERN_INFO "synaptics_ts_probe: Product Major Version %x\n", ret);
+	panel_version = ret << 8;
+	ret = i2c_smbus_read_byte_data(ts->client, 0xe5);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_byte_data failed\n");
+		goto err_detect_failed;
+	}
+	printk(KERN_INFO "synaptics_ts_probe: Product Minor Version %x\n", ret);
+	panel_version |= ret;
+
+	ret = i2c_smbus_read_byte_data(ts->client, 0xe3);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_byte_data failed\n");
+		goto err_detect_failed;
+	}
+	printk(KERN_INFO "synaptics_ts_probe: product property %x\n", ret);
+
+	if (pdata) {
+		while (pdata->version > panel_version)
+			pdata++;
+		ts->flags = pdata->flags;
+		ts->sensitivity_adjust = pdata->sensitivity_adjust;
+		irqflags = pdata->irqflags;
+		inactive_area_left = pdata->inactive_left;
+		inactive_area_right = pdata->inactive_right;
+		inactive_area_top = pdata->inactive_top;
+		inactive_area_bottom = pdata->inactive_bottom;
+		snap_left_on = pdata->snap_left_on;
+		snap_left_off = pdata->snap_left_off;
+		snap_right_on = pdata->snap_right_on;
+		snap_right_off = pdata->snap_right_off;
+		snap_top_on = pdata->snap_top_on;
+		snap_top_off = pdata->snap_top_off;
+		snap_bottom_on = pdata->snap_bottom_on;
+		snap_bottom_off = pdata->snap_bottom_off;
+		fuzz_x = pdata->fuzz_x;
+		fuzz_y = pdata->fuzz_y;
+		fuzz_p = pdata->fuzz_p;
+		fuzz_w = pdata->fuzz_w;
+	} else {
+		irqflags = 0;
+		inactive_area_left = 0;
+		inactive_area_right = 0;
+		inactive_area_top = 0;
+		inactive_area_bottom = 0;
+		snap_left_on = 0;
+		snap_left_off = 0;
+		snap_right_on = 0;
+		snap_right_off = 0;
+		snap_top_on = 0;
+		snap_top_off = 0;
+		snap_bottom_on = 0;
+		snap_bottom_off = 0;
+		fuzz_x = 0;
+		fuzz_y = 0;
+		fuzz_p = 0;
+		fuzz_w = 0;
+	}
+
+	ret = i2c_smbus_read_byte_data(ts->client, 0xf0);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_byte_data failed\n");
+		goto err_detect_failed;
+	}
+	printk(KERN_INFO "synaptics_ts_probe: device control %x\n", ret);
+
+	ret = i2c_smbus_read_byte_data(ts->client, 0xf1);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_byte_data failed\n");
+		goto err_detect_failed;
+	}
+	printk(KERN_INFO "synaptics_ts_probe: interrupt enable %x\n", ret);
+
+	ret = i2c_smbus_write_byte_data(ts->client, 0xf1, 0); /* disable interrupt */
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_write_byte_data failed\n");
+		goto err_detect_failed;
+	}
+
+	msg[0].addr = ts->client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = buf0;
+	buf0[0] = 0xe0;
+	msg[1].addr = ts->client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 8;
+	msg[1].buf = buf1;
+	ret = i2c_transfer(ts->client->adapter, msg, 2);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_transfer failed\n");
+		goto err_detect_failed;
+	}
+	printk(KERN_INFO "synaptics_ts_probe: 0xe0: %x %x %x %x %x %x %x %x\n",
+	       buf1[0], buf1[1], buf1[2], buf1[3],
+	       buf1[4], buf1[5], buf1[6], buf1[7]);
+
+	ret = i2c_smbus_write_byte_data(ts->client, 0xff, 0x10); /* page select = 0x10 */
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_write_byte_data failed for page select\n");
+		goto err_detect_failed;
+	}
+	ret = i2c_smbus_read_word_data(ts->client, 0x02);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_word_data failed\n");
+		goto err_detect_failed;
+	}
+	ts->has_relative_report = !(ret & 0x100);
+	printk(KERN_INFO "synaptics_ts_probe: Sensor properties %x\n", ret);
+	ret = i2c_smbus_read_word_data(ts->client, 0x04);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_word_data failed\n");
+		goto err_detect_failed;
+	}
+	ts->max[0] = max_x = (ret >> 8 & 0xff) | ((ret & 0x1f) << 8);
+	ret = i2c_smbus_read_word_data(ts->client, 0x06);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_word_data failed\n");
+		goto err_detect_failed;
+	}
+	ts->max[1] = max_y = (ret >> 8 & 0xff) | ((ret & 0x1f) << 8);
+	if (ts->flags & SYNAPTICS_SWAP_XY)
+		swap(max_x, max_y);
+
+	ret = synaptics_init_panel(ts); /* will also switch back to page 0x04 */
+	if (ret < 0) {
+		printk(KERN_ERR "synaptics_init_panel failed\n");
+		goto err_detect_failed;
+	}
+
+	ts->input_dev = input_allocate_device();
+	if (ts->input_dev == NULL) {
+		ret = -ENOMEM;
+		printk(KERN_ERR "synaptics_ts_probe: Failed to allocate input device\n");
+		goto err_input_dev_alloc_failed;
+	}
+	ts->input_dev->name = "synaptics-rmi-touchscreen";
+	set_bit(EV_SYN, ts->input_dev->evbit);
+	set_bit(EV_KEY, ts->input_dev->evbit);
+	set_bit(BTN_TOUCH, ts->input_dev->keybit);
+	set_bit(EV_ABS, ts->input_dev->evbit);
+	inactive_area_left = inactive_area_left * max_x / 0x10000;
+	inactive_area_right = inactive_area_right * max_x / 0x10000;
+	inactive_area_top = inactive_area_top * max_y / 0x10000;
+	inactive_area_bottom = inactive_area_bottom * max_y / 0x10000;
+	snap_left_on = snap_left_on * max_x / 0x10000;
+	snap_left_off = snap_left_off * max_x / 0x10000;
+	snap_right_on = snap_right_on * max_x / 0x10000;
+	snap_right_off = snap_right_off * max_x / 0x10000;
+	snap_top_on = snap_top_on * max_y / 0x10000;
+	snap_top_off = snap_top_off * max_y / 0x10000;
+	snap_bottom_on = snap_bottom_on * max_y / 0x10000;
+	snap_bottom_off = snap_bottom_off * max_y / 0x10000;
+	fuzz_x = fuzz_x * max_x / 0x10000;
+	fuzz_y = fuzz_y * max_y / 0x10000;
+	ts->snap_down[!!(ts->flags & SYNAPTICS_SWAP_XY)] = -inactive_area_left;
+	ts->snap_up[!!(ts->flags & SYNAPTICS_SWAP_XY)] = max_x + inactive_area_right;
+	ts->snap_down[!(ts->flags & SYNAPTICS_SWAP_XY)] = -inactive_area_top;
+	ts->snap_up[!(ts->flags & SYNAPTICS_SWAP_XY)] = max_y + inactive_area_bottom;
+	ts->snap_down_on[!!(ts->flags & SYNAPTICS_SWAP_XY)] = snap_left_on;
+	ts->snap_down_off[!!(ts->flags & SYNAPTICS_SWAP_XY)] = snap_left_off;
+	ts->snap_up_on[!!(ts->flags & SYNAPTICS_SWAP_XY)] = max_x - snap_right_on;
+	ts->snap_up_off[!!(ts->flags & SYNAPTICS_SWAP_XY)] = max_x - snap_right_off;
+	ts->snap_down_on[!(ts->flags & SYNAPTICS_SWAP_XY)] = snap_top_on;
+	ts->snap_down_off[!(ts->flags & SYNAPTICS_SWAP_XY)] = snap_top_off;
+	ts->snap_up_on[!(ts->flags & SYNAPTICS_SWAP_XY)] = max_y - snap_bottom_on;
+	ts->snap_up_off[!(ts->flags & SYNAPTICS_SWAP_XY)] = max_y - snap_bottom_off;
+	printk(KERN_INFO "synaptics_ts_probe: max_x %d, max_y %d\n", max_x, max_y);
+	printk(KERN_INFO "synaptics_ts_probe: inactive_x %d %d, inactive_y %d %d\n",
+	       inactive_area_left, inactive_area_right,
+	       inactive_area_top, inactive_area_bottom);
+	printk(KERN_INFO "synaptics_ts_probe: snap_x %d-%d %d-%d, snap_y %d-%d %d-%d\n",
+	       snap_left_on, snap_left_off, snap_right_on, snap_right_off,
+	       snap_top_on, snap_top_off, snap_bottom_on, snap_bottom_off);
+	input_set_abs_params(ts->input_dev, ABS_X, -inactive_area_left, max_x + inactive_area_right, fuzz_x, 0);
+	input_set_abs_params(ts->input_dev, ABS_Y, -inactive_area_top, max_y + inactive_area_bottom, fuzz_y, 0);
+	input_set_abs_params(ts->input_dev, ABS_PRESSURE, 0, 255, fuzz_p, 0);
+	input_set_abs_params(ts->input_dev, ABS_TOOL_WIDTH, 0, 15, fuzz_w, 0);
+	input_set_abs_params(ts->input_dev, ABS_MT_POSITION_X, -inactive_area_left, max_x + inactive_area_right, fuzz_x, 0);
+	input_set_abs_params(ts->input_dev, ABS_MT_POSITION_Y, -inactive_area_top, max_y + inactive_area_bottom, fuzz_y, 0);
+	input_set_abs_params(ts->input_dev, ABS_MT_TOUCH_MAJOR, 0, 255, fuzz_p, 0);
+	input_set_abs_params(ts->input_dev, ABS_MT_WIDTH_MAJOR, 0, 15, fuzz_w, 0);
+	/* ts->input_dev->name = ts->keypad_info->name; */
+	ret = input_register_device(ts->input_dev);
+	if (ret) {
+		printk(KERN_ERR "synaptics_ts_probe: Unable to register %s input device\n", ts->input_dev->name);
+		goto err_input_register_device_failed;
+	}
+	if (client->irq) {
+		ret = request_irq(client->irq, synaptics_ts_irq_handler, irqflags, client->name, ts);
+		if (ret == 0) {
+			ret = i2c_smbus_write_byte_data(ts->client, 0xf1, 0x01); /* enable abs int */
+			if (ret)
+				free_irq(client->irq, ts);
+		}
+		if (ret == 0)
+			ts->use_irq = 1;
+		else
+			dev_err(&client->dev, "request_irq failed\n");
+	}
+	if (!ts->use_irq) {
+		hrtimer_init(&ts->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+		ts->timer.function = synaptics_ts_timer_func;
+		hrtimer_start(&ts->timer, ktime_set(1, 0), HRTIMER_MODE_REL);
+	}
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	ts->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	ts->early_suspend.suspend = synaptics_ts_early_suspend;
+	ts->early_suspend.resume = synaptics_ts_late_resume;
+	register_early_suspend(&ts->early_suspend);
+#endif
+
+	printk(KERN_INFO "synaptics_ts_probe: Start touchscreen %s in %s mode\n", ts->input_dev->name, ts->use_irq ? "interrupt" : "polling");
+
+	return 0;
+
+err_input_register_device_failed:
+	input_free_device(ts->input_dev);
+
+err_input_dev_alloc_failed:
+err_detect_failed:
+err_power_failed:
+	kfree(ts);
+err_alloc_data_failed:
+err_check_functionality_failed:
+	return ret;
+}
+
+static int synaptics_ts_remove(struct i2c_client *client)
+{
+	struct synaptics_ts_data *ts = i2c_get_clientdata(client);
+	unregister_early_suspend(&ts->early_suspend);
+	if (ts->use_irq)
+		free_irq(client->irq, ts);
+	else
+		hrtimer_cancel(&ts->timer);
+	input_unregister_device(ts->input_dev);
+	kfree(ts);
+	return 0;
+}
+
+static int synaptics_ts_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+	int ret;
+	struct synaptics_ts_data *ts = i2c_get_clientdata(client);
+
+	if (ts->use_irq)
+		disable_irq(client->irq);
+	else
+		hrtimer_cancel(&ts->timer);
+	ret = cancel_work_sync(&ts->work);
+	if (ret && ts->use_irq) /* if work was pending disable-count is now 2 */
+		enable_irq(client->irq);
+	ret = i2c_smbus_write_byte_data(ts->client, 0xf1, 0); /* disable interrupt */
+	if (ret < 0)
+		printk(KERN_ERR "synaptics_ts_suspend: i2c_smbus_write_byte_data failed\n");
+
+	ret = i2c_smbus_write_byte_data(client, 0xf0, 0x86); /* deep sleep */
+	if (ret < 0)
+		printk(KERN_ERR "synaptics_ts_suspend: i2c_smbus_write_byte_data failed\n");
+	if (ts->power) {
+		ret = ts->power(0);
+		if (ret < 0)
+			printk(KERN_ERR "synaptics_ts_resume power off failed\n");
+	}
+	return 0;
+}
+
+static int synaptics_ts_resume(struct i2c_client *client)
+{
+	int ret;
+	struct synaptics_ts_data *ts = i2c_get_clientdata(client);
+
+	if (ts->power) {
+		ret = ts->power(1);
+		if (ret < 0)
+			printk(KERN_ERR "synaptics_ts_resume power on failed\n");
+	}
+
+	synaptics_init_panel(ts);
+
+	if (ts->use_irq)
+		enable_irq(client->irq);
+
+	if (!ts->use_irq)
+		hrtimer_start(&ts->timer, ktime_set(1, 0), HRTIMER_MODE_REL);
+	else
+		i2c_smbus_write_byte_data(ts->client, 0xf1, 0x01); /* enable abs int */
+
+	return 0;
+}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void synaptics_ts_early_suspend(struct early_suspend *h)
+{
+	struct synaptics_ts_data *ts;
+	ts = container_of(h, struct synaptics_ts_data, early_suspend);
+	synaptics_ts_suspend(ts->client, PMSG_SUSPEND);
+}
+
+static void synaptics_ts_late_resume(struct early_suspend *h)
+{
+	struct synaptics_ts_data *ts;
+	ts = container_of(h, struct synaptics_ts_data, early_suspend);
+	synaptics_ts_resume(ts->client);
+}
+#endif
+
+static const struct i2c_device_id synaptics_ts_id[] = {
+	{ SYNAPTICS_I2C_RMI_NAME, 0 },
+	{ }
+};
+
+static struct i2c_driver synaptics_ts_driver = {
+	.probe		= synaptics_ts_probe,
+	.remove		= synaptics_ts_remove,
+#ifndef CONFIG_HAS_EARLYSUSPEND
+	.suspend	= synaptics_ts_suspend,
+	.resume		= synaptics_ts_resume,
+#endif
+	.id_table	= synaptics_ts_id,
+	.driver = {
+		.name	= SYNAPTICS_I2C_RMI_NAME,
+	},
+};
+
+static int __devinit synaptics_ts_init(void)
+{
+	synaptics_wq = create_singlethread_workqueue("synaptics_wq");
+	if (!synaptics_wq)
+		return -ENOMEM;
+	return i2c_add_driver(&synaptics_ts_driver);
+}
+
+static void __exit synaptics_ts_exit(void)
+{
+	i2c_del_driver(&synaptics_ts_driver);
+	if (synaptics_wq)
+		destroy_workqueue(synaptics_wq);
+}
+
+module_init(synaptics_ts_init);
+module_exit(synaptics_ts_exit);
+
+MODULE_DESCRIPTION("Synaptics Touchscreen Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/wm97xx-core.c b/drivers/input/touchscreen/wm97xx-core.c
index 5dbe73af2f8f..72e2abea2c28 100644
--- a/drivers/input/touchscreen/wm97xx-core.c
+++ b/drivers/input/touchscreen/wm97xx-core.c
@@ -54,6 +54,10 @@
 #define WM_CORE_VERSION		"1.00"
 #define DEFAULT_PRESSURE	0xb0c0
 
+#ifdef CONFIG_MACH_COLIBRI_T20
+extern void *get_colibri_t20_audio_platform_data(void);
+#endif
+
 
 /*
  * Touchscreen absolute values
@@ -442,8 +446,18 @@ static int wm97xx_read_samples(struct wm97xx *wm)
 			"pen down: x=%x:%d, y=%x:%d, pressure=%x:%d\n",
 			data.x >> 12, data.x & 0xfff, data.y >> 12,
 			data.y & 0xfff, data.p >> 12, data.p & 0xfff);
+#ifndef CONFIG_ANDROID
 		input_report_abs(wm->input_dev, ABS_X, data.x & 0xfff);
 		input_report_abs(wm->input_dev, ABS_Y, data.y & 0xfff);
+#else /* !CONFIG_ANDROID */
+		/* Hack: rotate touch for now due to missing calibration
+			 integration
+		   Note: 12-bit touch resolution */
+		input_report_abs(wm->input_dev, ABS_X, 4096 - (data.x & 0xfff));
+		input_report_abs(wm->input_dev, ABS_Y, 4096 - (data.y & 0xfff));
+#endif /* !CONFIG_ANDROID */
+
+
 		input_report_abs(wm->input_dev, ABS_PRESSURE, data.p & 0xfff);
 		input_report_key(wm->input_dev, BTN_TOUCH, 1);
 		input_sync(wm->input_dev);
@@ -640,7 +654,12 @@ static int wm97xx_probe(struct device *dev)
 	}
 
 	/* set up touch configuration */
+#ifdef CONFIG_ANDROID
+	/* Hack: rename due to idc parser having issues with spaces in names */
+	wm->input_dev->name = "wm97xx-ts";
+#else /* CONFIG_ANDROID */
 	wm->input_dev->name = "wm97xx touchscreen";
+#endif /* CONFIG_ANDROID */
 	wm->input_dev->phys = "wm97xx";
 	wm->input_dev->open = wm97xx_ts_input_open;
 	wm->input_dev->close = wm97xx_ts_input_close;
@@ -671,7 +690,13 @@ static int wm97xx_probe(struct device *dev)
 	}
 	platform_set_drvdata(wm->battery_dev, wm);
 	wm->battery_dev->dev.parent = dev;
+
+#if defined(CONFIG_MACH_COLIBRI_T20) && !defined(CONFIG_ANDROID)
+	wm->battery_dev->dev.platform_data = get_colibri_t20_audio_platform_data();
+#else
 	wm->battery_dev->dev.platform_data = pdata;
+#endif
+
 	ret = platform_device_add(wm->battery_dev);
 	if (ret < 0)
 		goto batt_reg_err;
diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index b57b3fa492f3..b394c6c5fbe7 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -34,7 +34,9 @@ config AMD_IOMMU
 	bool "AMD IOMMU support"
 	select SWIOTLB
 	select PCI_MSI
-	select PCI_IOV
+	select PCI_ATS
+	select PCI_PRI
+	select PCI_PASID
 	select IOMMU_API
 	depends on X86_64 && PCI && ACPI
 	---help---
@@ -58,11 +60,24 @@ config AMD_IOMMU_STATS
 	  information to userspace via debugfs.
 	  If unsure, say N.
 
+config AMD_IOMMU_V2
+	tristate "AMD IOMMU Version 2 driver (EXPERIMENTAL)"
+	depends on AMD_IOMMU && PROFILING && EXPERIMENTAL
+	select MMU_NOTIFIER
+	---help---
+	  This option enables support for the AMD IOMMUv2 features of the IOMMU
+	  hardware. Select this option if you want to use devices that support
+	  the the PCI PRI and PASID interface.
+
 # Intel IOMMU support
-config DMAR
-	bool "Support for DMA Remapping Devices"
+config DMAR_TABLE
+	bool
+
+config INTEL_IOMMU
+	bool "Support for Intel IOMMU using DMA Remapping Devices"
 	depends on PCI_MSI && ACPI && (X86 || IA64_GENERIC)
 	select IOMMU_API
+	select DMAR_TABLE
 	help
 	  DMA remapping (DMAR) devices support enables independent address
 	  translations for Direct Memory Access (DMA) from devices.
@@ -70,18 +85,18 @@ config DMAR
 	  and include PCI device scope covered by these DMA
 	  remapping devices.
 
-config DMAR_DEFAULT_ON
+config INTEL_IOMMU_DEFAULT_ON
 	def_bool y
-	prompt "Enable DMA Remapping Devices by default"
-	depends on DMAR
+	prompt "Enable Intel DMA Remapping Devices by default"
+	depends on INTEL_IOMMU
 	help
 	  Selecting this option will enable a DMAR device at boot time if
 	  one is found. If this option is not selected, DMAR support can
 	  be enabled by passing intel_iommu=on to the kernel.
 
-config DMAR_BROKEN_GFX_WA
+config INTEL_IOMMU_BROKEN_GFX_WA
 	bool "Workaround broken graphics drivers (going away soon)"
-	depends on DMAR && BROKEN && X86
+	depends on INTEL_IOMMU && BROKEN && X86
 	---help---
 	  Current Graphics drivers tend to use physical address
 	  for DMA and avoid using DMA APIs. Setting this config
@@ -90,21 +105,61 @@ config DMAR_BROKEN_GFX_WA
 	  to use physical addresses for DMA, at least until this
 	  option is removed in the 2.6.32 kernel.
 
-config DMAR_FLOPPY_WA
+config INTEL_IOMMU_FLOPPY_WA
 	def_bool y
-	depends on DMAR && X86
+	depends on INTEL_IOMMU && X86
 	---help---
 	  Floppy disk drivers are known to bypass DMA API calls
 	  thereby failing to work when IOMMU is enabled. This
 	  workaround will setup a 1:1 mapping for the first
 	  16MiB to make floppy (an ISA device) work.
 
-config INTR_REMAP
+config IRQ_REMAP
 	bool "Support for Interrupt Remapping (EXPERIMENTAL)"
 	depends on X86_64 && X86_IO_APIC && PCI_MSI && ACPI && EXPERIMENTAL
+	select DMAR_TABLE
 	---help---
 	  Supports Interrupt remapping for IO-APIC and MSI devices.
 	  To use x2apic mode in the CPU's which support x2APIC enhancements or
 	  to support platforms with CPU's having > 8 bit APIC ID, say Y.
 
+# OMAP IOMMU support
+config OMAP_IOMMU
+	bool "OMAP IOMMU Support"
+	depends on ARCH_OMAP
+	select IOMMU_API
+
+config OMAP_IOVMM
+	tristate "OMAP IO Virtual Memory Manager Support"
+	depends on OMAP_IOMMU
+
+config OMAP_IOMMU_DEBUG
+       tristate "Export OMAP IOMMU/IOVMM internals in DebugFS"
+       depends on OMAP_IOVMM && DEBUG_FS
+       help
+         Select this to see extensive information about
+         the internal state of OMAP IOMMU/IOVMM in debugfs.
+
+         Say N unless you know you need this.
+
+config TEGRA_IOMMU_SMMU
+	bool "Tegra SMMU IOMMU Support"
+	depends on ARCH_TEGRA
+	select IOMMU_API
+	help
+	  Enables support for remapping discontiguous physical memory
+	  shared with the operating system into contiguous I/O virtual
+	  space through the SMMU (System Memory Management Unit)
+	  hardware included on Tegra SoCs.
+
+config TEGRA_IOMMU_GART
+	bool "Tegra GART IOMMU Support"
+	depends on ARCH_TEGRA_2x_SOC
+	select IOMMU_API
+	help
+	  Enables support for remapping discontiguous physical memory
+	  shared with the operating system into contiguous I/O virtual
+	  space through the GART (Graphics Address Relocation Table)
+	  hardware included on Tegra SoCs.
+
 endif # IOMMU_SUPPORT
diff --git a/drivers/iommu/Makefile b/drivers/iommu/Makefile
index 4d4d77df7cac..ed59619d1af8 100644
--- a/drivers/iommu/Makefile
+++ b/drivers/iommu/Makefile
@@ -1,5 +1,14 @@
 obj-$(CONFIG_IOMMU_API) += iommu.o
 obj-$(CONFIG_MSM_IOMMU) += msm_iommu.o msm_iommu_dev.o
 obj-$(CONFIG_AMD_IOMMU) += amd_iommu.o amd_iommu_init.o
-obj-$(CONFIG_DMAR) += dmar.o iova.o intel-iommu.o
-obj-$(CONFIG_INTR_REMAP) += dmar.o intr_remapping.o
+obj-$(CONFIG_AMD_IOMMU_V2) += amd_iommu_v2.o
+obj-$(CONFIG_DMAR_TABLE) += dmar.o
+obj-$(CONFIG_INTEL_IOMMU) += iova.o intel-iommu.o
+obj-$(CONFIG_IRQ_REMAP) += intr_remapping.o
+obj-$(CONFIG_OMAP_IOMMU) += omap-iommu.o
+obj-$(CONFIG_OMAP_IOVMM) += omap-iovmm.o
+obj-$(CONFIG_OMAP_IOMMU_DEBUG) += omap-iommu-debug.o
+CFLAGS_tegra-gart.o	= -Werror
+obj-$(CONFIG_TEGRA_IOMMU_GART) += tegra-gart.o
+CFLAGS_tegra-smmu.o	= -Werror
+obj-$(CONFIG_TEGRA_IOMMU_SMMU) += tegra-smmu.o
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index 0e4227f457af..cce1f03b8895 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -17,6 +17,7 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  */
 
+#include <linux/ratelimit.h>
 #include <linux/pci.h>
 #include <linux/pci-ats.h>
 #include <linux/bitmap.h>
@@ -28,6 +29,8 @@
 #include <linux/iommu.h>
 #include <linux/delay.h>
 #include <linux/amd-iommu.h>
+#include <linux/notifier.h>
+#include <linux/export.h>
 #include <asm/msidef.h>
 #include <asm/proto.h>
 #include <asm/iommu.h>
@@ -41,6 +44,24 @@
 
 #define LOOP_TIMEOUT	100000
 
+/*
+ * This bitmap is used to advertise the page sizes our hardware support
+ * to the IOMMU core, which will then use this information to split
+ * physically contiguous memory regions it is mapping into page sizes
+ * that we support.
+ *
+ * Traditionally the IOMMU core just handed us the mappings directly,
+ * after making sure the size is an order of a 4KiB page and that the
+ * mapping has natural alignment.
+ *
+ * To retain this behavior, we currently advertise that we support
+ * all page sizes that are an order of 4KiB.
+ *
+ * If at some point we'd like to utilize the IOMMU core's new behavior,
+ * we could change this to advertise the real page sizes we support.
+ */
+#define AMD_IOMMU_PGSIZES	(~0xFFFUL)
+
 static DEFINE_RWLOCK(amd_iommu_devtable_lock);
 
 /* A list of preallocated protection domains */
@@ -59,6 +80,9 @@ static struct protection_domain *pt_domain;
 
 static struct iommu_ops amd_iommu_ops;
 
+static ATOMIC_NOTIFIER_HEAD(ppr_notifier);
+int amd_iommu_max_glx_val = -1;
+
 /*
  * general struct to manage commands send to an IOMMU
  */
@@ -67,6 +91,7 @@ struct iommu_cmd {
 };
 
 static void update_domain(struct protection_domain *domain);
+static int __init alloc_passthrough_domain(void);
 
 /****************************************************************************
  *
@@ -147,6 +172,33 @@ static struct iommu_dev_data *get_dev_data(struct device *dev)
 	return dev->archdata.iommu;
 }
 
+static bool pci_iommuv2_capable(struct pci_dev *pdev)
+{
+	static const int caps[] = {
+		PCI_EXT_CAP_ID_ATS,
+		PCI_EXT_CAP_ID_PRI,
+		PCI_EXT_CAP_ID_PASID,
+	};
+	int i, pos;
+
+	for (i = 0; i < 3; ++i) {
+		pos = pci_find_ext_capability(pdev, caps[i]);
+		if (pos == 0)
+			return false;
+	}
+
+	return true;
+}
+
+static bool pdev_pri_erratum(struct pci_dev *pdev, u32 erratum)
+{
+	struct iommu_dev_data *dev_data;
+
+	dev_data = get_dev_data(&pdev->dev);
+
+	return dev_data->errata & (1 << erratum) ? true : false;
+}
+
 /*
  * In this function the list of preallocated protection domains is traversed to
  * find the domain for a specific device
@@ -204,6 +256,7 @@ static bool check_device(struct device *dev)
 
 static int iommu_init_device(struct device *dev)
 {
+	struct pci_dev *pdev = to_pci_dev(dev);
 	struct iommu_dev_data *dev_data;
 	u16 alias;
 
@@ -228,6 +281,13 @@ static int iommu_init_device(struct device *dev)
 		dev_data->alias_data = alias_data;
 	}
 
+	if (pci_iommuv2_capable(pdev)) {
+		struct amd_iommu *iommu;
+
+		iommu              = amd_iommu_rlookup_table[dev_data->devid];
+		dev_data->iommu_v2 = iommu->is_iommu_v2;
+	}
+
 	dev->archdata.iommu = dev_data;
 
 	return 0;
@@ -317,6 +377,11 @@ DECLARE_STATS_COUNTER(domain_flush_single);
 DECLARE_STATS_COUNTER(domain_flush_all);
 DECLARE_STATS_COUNTER(alloced_io_mem);
 DECLARE_STATS_COUNTER(total_map_requests);
+DECLARE_STATS_COUNTER(complete_ppr);
+DECLARE_STATS_COUNTER(invalidate_iotlb);
+DECLARE_STATS_COUNTER(invalidate_iotlb_all);
+DECLARE_STATS_COUNTER(pri_requests);
+
 
 static struct dentry *stats_dir;
 static struct dentry *de_fflush;
@@ -351,6 +416,10 @@ static void amd_iommu_stats_init(void)
 	amd_iommu_stats_add(&domain_flush_all);
 	amd_iommu_stats_add(&alloced_io_mem);
 	amd_iommu_stats_add(&total_map_requests);
+	amd_iommu_stats_add(&complete_ppr);
+	amd_iommu_stats_add(&invalidate_iotlb);
+	amd_iommu_stats_add(&invalidate_iotlb_all);
+	amd_iommu_stats_add(&pri_requests);
 }
 
 #endif
@@ -365,8 +434,8 @@ static void dump_dte_entry(u16 devid)
 {
 	int i;
 
-	for (i = 0; i < 8; ++i)
-		pr_err("AMD-Vi: DTE[%d]: %08x\n", i,
+	for (i = 0; i < 4; ++i)
+		pr_err("AMD-Vi: DTE[%d]: %016llx\n", i,
 			amd_iommu_dev_table[devid].data[i]);
 }
 
@@ -461,12 +530,84 @@ static void iommu_poll_events(struct amd_iommu *iommu)
 	spin_unlock_irqrestore(&iommu->lock, flags);
 }
 
+static void iommu_handle_ppr_entry(struct amd_iommu *iommu, u32 head)
+{
+	struct amd_iommu_fault fault;
+	volatile u64 *raw;
+	int i;
+
+	INC_STATS_COUNTER(pri_requests);
+
+	raw = (u64 *)(iommu->ppr_log + head);
+
+	/*
+	 * Hardware bug: Interrupt may arrive before the entry is written to
+	 * memory. If this happens we need to wait for the entry to arrive.
+	 */
+	for (i = 0; i < LOOP_TIMEOUT; ++i) {
+		if (PPR_REQ_TYPE(raw[0]) != 0)
+			break;
+		udelay(1);
+	}
+
+	if (PPR_REQ_TYPE(raw[0]) != PPR_REQ_FAULT) {
+		pr_err_ratelimited("AMD-Vi: Unknown PPR request received\n");
+		return;
+	}
+
+	fault.address   = raw[1];
+	fault.pasid     = PPR_PASID(raw[0]);
+	fault.device_id = PPR_DEVID(raw[0]);
+	fault.tag       = PPR_TAG(raw[0]);
+	fault.flags     = PPR_FLAGS(raw[0]);
+
+	/*
+	 * To detect the hardware bug we need to clear the entry
+	 * to back to zero.
+	 */
+	raw[0] = raw[1] = 0;
+
+	atomic_notifier_call_chain(&ppr_notifier, 0, &fault);
+}
+
+static void iommu_poll_ppr_log(struct amd_iommu *iommu)
+{
+	unsigned long flags;
+	u32 head, tail;
+
+	if (iommu->ppr_log == NULL)
+		return;
+
+	spin_lock_irqsave(&iommu->lock, flags);
+
+	head = readl(iommu->mmio_base + MMIO_PPR_HEAD_OFFSET);
+	tail = readl(iommu->mmio_base + MMIO_PPR_TAIL_OFFSET);
+
+	while (head != tail) {
+
+		/* Handle PPR entry */
+		iommu_handle_ppr_entry(iommu, head);
+
+		/* Update and refresh ring-buffer state*/
+		head = (head + PPR_ENTRY_SIZE) % PPR_LOG_SIZE;
+		writel(head, iommu->mmio_base + MMIO_PPR_HEAD_OFFSET);
+		tail = readl(iommu->mmio_base + MMIO_PPR_TAIL_OFFSET);
+	}
+
+	/* enable ppr interrupts again */
+	writel(MMIO_STATUS_PPR_INT_MASK, iommu->mmio_base + MMIO_STATUS_OFFSET);
+
+	spin_unlock_irqrestore(&iommu->lock, flags);
+}
+
 irqreturn_t amd_iommu_int_thread(int irq, void *data)
 {
 	struct amd_iommu *iommu;
 
-	for_each_iommu(iommu)
+	for_each_iommu(iommu) {
 		iommu_poll_events(iommu);
+		iommu_poll_ppr_log(iommu);
+	}
 
 	return IRQ_HANDLED;
 }
@@ -595,6 +736,60 @@ static void build_inv_iotlb_pages(struct iommu_cmd *cmd, u16 devid, int qdep,
 		cmd->data[2] |= CMD_INV_IOMMU_PAGES_SIZE_MASK;
 }
 
+static void build_inv_iommu_pasid(struct iommu_cmd *cmd, u16 domid, int pasid,
+				  u64 address, bool size)
+{
+	memset(cmd, 0, sizeof(*cmd));
+
+	address &= ~(0xfffULL);
+
+	cmd->data[0]  = pasid & PASID_MASK;
+	cmd->data[1]  = domid;
+	cmd->data[2]  = lower_32_bits(address);
+	cmd->data[3]  = upper_32_bits(address);
+	cmd->data[2] |= CMD_INV_IOMMU_PAGES_PDE_MASK;
+	cmd->data[2] |= CMD_INV_IOMMU_PAGES_GN_MASK;
+	if (size)
+		cmd->data[2] |= CMD_INV_IOMMU_PAGES_SIZE_MASK;
+	CMD_SET_TYPE(cmd, CMD_INV_IOMMU_PAGES);
+}
+
+static void build_inv_iotlb_pasid(struct iommu_cmd *cmd, u16 devid, int pasid,
+				  int qdep, u64 address, bool size)
+{
+	memset(cmd, 0, sizeof(*cmd));
+
+	address &= ~(0xfffULL);
+
+	cmd->data[0]  = devid;
+	cmd->data[0] |= (pasid & 0xff) << 16;
+	cmd->data[0] |= (qdep  & 0xff) << 24;
+	cmd->data[1]  = devid;
+	cmd->data[1] |= ((pasid >> 8) & 0xfff) << 16;
+	cmd->data[2]  = lower_32_bits(address);
+	cmd->data[2] |= CMD_INV_IOMMU_PAGES_GN_MASK;
+	cmd->data[3]  = upper_32_bits(address);
+	if (size)
+		cmd->data[2] |= CMD_INV_IOMMU_PAGES_SIZE_MASK;
+	CMD_SET_TYPE(cmd, CMD_INV_IOTLB_PAGES);
+}
+
+static void build_complete_ppr(struct iommu_cmd *cmd, u16 devid, int pasid,
+			       int status, int tag, bool gn)
+{
+	memset(cmd, 0, sizeof(*cmd));
+
+	cmd->data[0]  = devid;
+	if (gn) {
+		cmd->data[1]  = pasid & PASID_MASK;
+		cmd->data[2]  = CMD_INV_IOMMU_PAGES_GN_MASK;
+	}
+	cmd->data[3]  = tag & 0x1ff;
+	cmd->data[3] |= (status & PPR_STATUS_MASK) << PPR_STATUS_SHIFT;
+
+	CMD_SET_TYPE(cmd, CMD_COMPLETE_PPR);
+}
+
 static void build_inv_all(struct iommu_cmd *cmd)
 {
 	memset(cmd, 0, sizeof(*cmd));
@@ -1283,7 +1478,7 @@ static int alloc_new_range(struct dma_ops_domain *dma_dom,
 		if (!pte || !IOMMU_PTE_PRESENT(*pte))
 			continue;
 
-		dma_ops_reserve_addresses(dma_dom, i << PAGE_SHIFT, 1);
+		dma_ops_reserve_addresses(dma_dom, i >> PAGE_SHIFT, 1);
 	}
 
 	update_domain(&dma_dom->domain);
@@ -1496,6 +1691,48 @@ static void free_pagetable(struct protection_domain *domain)
 	domain->pt_root = NULL;
 }
 
+static void free_gcr3_tbl_level1(u64 *tbl)
+{
+	u64 *ptr;
+	int i;
+
+	for (i = 0; i < 512; ++i) {
+		if (!(tbl[i] & GCR3_VALID))
+			continue;
+
+		ptr = __va(tbl[i] & PAGE_MASK);
+
+		free_page((unsigned long)ptr);
+	}
+}
+
+static void free_gcr3_tbl_level2(u64 *tbl)
+{
+	u64 *ptr;
+	int i;
+
+	for (i = 0; i < 512; ++i) {
+		if (!(tbl[i] & GCR3_VALID))
+			continue;
+
+		ptr = __va(tbl[i] & PAGE_MASK);
+
+		free_gcr3_tbl_level1(ptr);
+	}
+}
+
+static void free_gcr3_table(struct protection_domain *domain)
+{
+	if (domain->glx == 2)
+		free_gcr3_tbl_level2(domain->gcr3_tbl);
+	else if (domain->glx == 1)
+		free_gcr3_tbl_level1(domain->gcr3_tbl);
+	else if (domain->glx != 0)
+		BUG();
+
+	free_page((unsigned long)domain->gcr3_tbl);
+}
+
 /*
  * Free a domain, only used if something went wrong in the
  * allocation path and we need to free an already allocated page table
@@ -1582,20 +1819,52 @@ static bool dma_ops_domain(struct protection_domain *domain)
 
 static void set_dte_entry(u16 devid, struct protection_domain *domain, bool ats)
 {
-	u64 pte_root = virt_to_phys(domain->pt_root);
-	u32 flags = 0;
+	u64 pte_root = 0;
+	u64 flags = 0;
+
+	if (domain->mode != PAGE_MODE_NONE)
+		pte_root = virt_to_phys(domain->pt_root);
 
 	pte_root |= (domain->mode & DEV_ENTRY_MODE_MASK)
 		    << DEV_ENTRY_MODE_SHIFT;
 	pte_root |= IOMMU_PTE_IR | IOMMU_PTE_IW | IOMMU_PTE_P | IOMMU_PTE_TV;
 
+	flags = amd_iommu_dev_table[devid].data[1];
+
 	if (ats)
 		flags |= DTE_FLAG_IOTLB;
 
-	amd_iommu_dev_table[devid].data[3] |= flags;
-	amd_iommu_dev_table[devid].data[2]  = domain->id;
-	amd_iommu_dev_table[devid].data[1]  = upper_32_bits(pte_root);
-	amd_iommu_dev_table[devid].data[0]  = lower_32_bits(pte_root);
+	if (domain->flags & PD_IOMMUV2_MASK) {
+		u64 gcr3 = __pa(domain->gcr3_tbl);
+		u64 glx  = domain->glx;
+		u64 tmp;
+
+		pte_root |= DTE_FLAG_GV;
+		pte_root |= (glx & DTE_GLX_MASK) << DTE_GLX_SHIFT;
+
+		/* First mask out possible old values for GCR3 table */
+		tmp = DTE_GCR3_VAL_B(~0ULL) << DTE_GCR3_SHIFT_B;
+		flags    &= ~tmp;
+
+		tmp = DTE_GCR3_VAL_C(~0ULL) << DTE_GCR3_SHIFT_C;
+		flags    &= ~tmp;
+
+		/* Encode GCR3 table into DTE */
+		tmp = DTE_GCR3_VAL_A(gcr3) << DTE_GCR3_SHIFT_A;
+		pte_root |= tmp;
+
+		tmp = DTE_GCR3_VAL_B(gcr3) << DTE_GCR3_SHIFT_B;
+		flags    |= tmp;
+
+		tmp = DTE_GCR3_VAL_C(gcr3) << DTE_GCR3_SHIFT_C;
+		flags    |= tmp;
+	}
+
+	flags &= ~(0xffffUL);
+	flags |= domain->id;
+
+	amd_iommu_dev_table[devid].data[1]  = flags;
+	amd_iommu_dev_table[devid].data[0]  = pte_root;
 }
 
 static void clear_dte_entry(u16 devid)
@@ -1603,7 +1872,6 @@ static void clear_dte_entry(u16 devid)
 	/* remove entry from the device table seen by the hardware */
 	amd_iommu_dev_table[devid].data[0] = IOMMU_PTE_P | IOMMU_PTE_TV;
 	amd_iommu_dev_table[devid].data[1] = 0;
-	amd_iommu_dev_table[devid].data[2] = 0;
 
 	amd_iommu_apply_erratum_63(devid);
 }
@@ -1696,6 +1964,93 @@ out_unlock:
 	return ret;
 }
 
+
+static void pdev_iommuv2_disable(struct pci_dev *pdev)
+{
+	pci_disable_ats(pdev);
+	pci_disable_pri(pdev);
+	pci_disable_pasid(pdev);
+}
+
+/* FIXME: Change generic reset-function to do the same */
+static int pri_reset_while_enabled(struct pci_dev *pdev)
+{
+	u16 control;
+	int pos;
+
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PRI);
+	if (!pos)
+		return -EINVAL;
+
+	pci_read_config_word(pdev, pos + PCI_PRI_CTRL, &control);
+	control |= PCI_PRI_CTRL_RESET;
+	pci_write_config_word(pdev, pos + PCI_PRI_CTRL, control);
+
+	return 0;
+}
+
+static int pdev_iommuv2_enable(struct pci_dev *pdev)
+{
+	bool reset_enable;
+	int reqs, ret;
+
+	/* FIXME: Hardcode number of outstanding requests for now */
+	reqs = 32;
+	if (pdev_pri_erratum(pdev, AMD_PRI_DEV_ERRATUM_LIMIT_REQ_ONE))
+		reqs = 1;
+	reset_enable = pdev_pri_erratum(pdev, AMD_PRI_DEV_ERRATUM_ENABLE_RESET);
+
+	/* Only allow access to user-accessible pages */
+	ret = pci_enable_pasid(pdev, 0);
+	if (ret)
+		goto out_err;
+
+	/* First reset the PRI state of the device */
+	ret = pci_reset_pri(pdev);
+	if (ret)
+		goto out_err;
+
+	/* Enable PRI */
+	ret = pci_enable_pri(pdev, reqs);
+	if (ret)
+		goto out_err;
+
+	if (reset_enable) {
+		ret = pri_reset_while_enabled(pdev);
+		if (ret)
+			goto out_err;
+	}
+
+	ret = pci_enable_ats(pdev, PAGE_SHIFT);
+	if (ret)
+		goto out_err;
+
+	return 0;
+
+out_err:
+	pci_disable_pri(pdev);
+	pci_disable_pasid(pdev);
+
+	return ret;
+}
+
+/* FIXME: Move this to PCI code */
+#define PCI_PRI_TLP_OFF		(1 << 2)
+
+bool pci_pri_tlp_required(struct pci_dev *pdev)
+{
+	u16 control;
+	int pos;
+
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PRI);
+	if (!pos)
+		return false;
+
+	pci_read_config_word(pdev, pos + PCI_PRI_CTRL, &control);
+
+	return (control & PCI_PRI_TLP_OFF) ? true : false;
+}
+
 /*
  * If a device is not yet associated with a domain, this function does
  * assigns it visible for the hardware
@@ -1710,7 +2065,18 @@ static int attach_device(struct device *dev,
 
 	dev_data = get_dev_data(dev);
 
-	if (amd_iommu_iotlb_sup && pci_enable_ats(pdev, PAGE_SHIFT) == 0) {
+	if (domain->flags & PD_IOMMUV2_MASK) {
+		if (!dev_data->iommu_v2 || !dev_data->passthrough)
+			return -EINVAL;
+
+		if (pdev_iommuv2_enable(pdev) != 0)
+			return -EINVAL;
+
+		dev_data->ats.enabled = true;
+		dev_data->ats.qdep    = pci_ats_queue_depth(pdev);
+		dev_data->pri_tlp     = pci_pri_tlp_required(pdev);
+	} else if (amd_iommu_iotlb_sup &&
+		   pci_enable_ats(pdev, PAGE_SHIFT) == 0) {
 		dev_data->ats.enabled = true;
 		dev_data->ats.qdep    = pci_ats_queue_depth(pdev);
 	}
@@ -1760,7 +2126,7 @@ static void __detach_device(struct iommu_dev_data *dev_data)
 	 * passthrough domain if it is detached from any other domain.
 	 * Make sure we can deassign from the pt_domain itself.
 	 */
-	if (iommu_pass_through &&
+	if (dev_data->passthrough &&
 	    (dev_data->domain == NULL && domain != pt_domain))
 		__attach_device(dev_data, pt_domain);
 }
@@ -1770,20 +2136,24 @@ static void __detach_device(struct iommu_dev_data *dev_data)
  */
 static void detach_device(struct device *dev)
 {
+	struct protection_domain *domain;
 	struct iommu_dev_data *dev_data;
 	unsigned long flags;
 
 	dev_data = get_dev_data(dev);
+	domain   = dev_data->domain;
 
 	/* lock device table */
 	write_lock_irqsave(&amd_iommu_devtable_lock, flags);
 	__detach_device(dev_data);
 	write_unlock_irqrestore(&amd_iommu_devtable_lock, flags);
 
-	if (dev_data->ats.enabled) {
+	if (domain->flags & PD_IOMMUV2_MASK)
+		pdev_iommuv2_disable(to_pci_dev(dev));
+	else if (dev_data->ats.enabled)
 		pci_disable_ats(to_pci_dev(dev));
-		dev_data->ats.enabled = false;
-	}
+
+	dev_data->ats.enabled = false;
 }
 
 /*
@@ -1818,18 +2188,20 @@ static struct protection_domain *domain_for_device(struct device *dev)
 static int device_change_notifier(struct notifier_block *nb,
 				  unsigned long action, void *data)
 {
-	struct device *dev = data;
-	u16 devid;
-	struct protection_domain *domain;
 	struct dma_ops_domain *dma_domain;
+	struct protection_domain *domain;
+	struct iommu_dev_data *dev_data;
+	struct device *dev = data;
 	struct amd_iommu *iommu;
 	unsigned long flags;
+	u16 devid;
 
 	if (!check_device(dev))
 		return 0;
 
-	devid  = get_device_id(dev);
-	iommu  = amd_iommu_rlookup_table[devid];
+	devid    = get_device_id(dev);
+	iommu    = amd_iommu_rlookup_table[devid];
+	dev_data = get_dev_data(dev);
 
 	switch (action) {
 	case BUS_NOTIFY_UNBOUND_DRIVER:
@@ -1838,7 +2210,7 @@ static int device_change_notifier(struct notifier_block *nb,
 
 		if (!domain)
 			goto out;
-		if (iommu_pass_through)
+		if (dev_data->passthrough)
 			break;
 		detach_device(dev);
 		break;
@@ -2434,8 +2806,9 @@ static int amd_iommu_dma_supported(struct device *dev, u64 mask)
  */
 static void prealloc_protection_domains(void)
 {
-	struct pci_dev *dev = NULL;
+	struct iommu_dev_data *dev_data;
 	struct dma_ops_domain *dma_dom;
+	struct pci_dev *dev = NULL;
 	u16 devid;
 
 	for_each_pci_dev(dev) {
@@ -2444,6 +2817,16 @@ static void prealloc_protection_domains(void)
 		if (!check_device(&dev->dev))
 			continue;
 
+		dev_data = get_dev_data(&dev->dev);
+		if (!amd_iommu_force_isolation && dev_data->iommu_v2) {
+			/* Make sure passthrough domain is allocated */
+			alloc_passthrough_domain();
+			dev_data->passthrough = true;
+			attach_device(&dev->dev, pt_domain);
+			pr_info("AMD-Vi: Using passthough domain for device %s\n",
+				dev_name(&dev->dev));
+		}
+
 		/* Is there already any domain for it? */
 		if (domain_for_device(&dev->dev))
 			continue;
@@ -2474,6 +2857,7 @@ static struct dma_map_ops amd_iommu_dma_ops = {
 
 static unsigned device_dma_ops_init(void)
 {
+	struct iommu_dev_data *dev_data;
 	struct pci_dev *pdev = NULL;
 	unsigned unhandled = 0;
 
@@ -2483,7 +2867,12 @@ static unsigned device_dma_ops_init(void)
 			continue;
 		}
 
-		pdev->dev.archdata.dma_ops = &amd_iommu_dma_ops;
+		dev_data = get_dev_data(&pdev->dev);
+
+		if (!dev_data->passthrough)
+			pdev->dev.archdata.dma_ops = &amd_iommu_dma_ops;
+		else
+			pdev->dev.archdata.dma_ops = &nommu_dma_ops;
 	}
 
 	return unhandled;
@@ -2495,7 +2884,7 @@ static unsigned device_dma_ops_init(void)
 
 void __init amd_iommu_init_api(void)
 {
-	register_iommu(&amd_iommu_ops);
+	bus_set_iommu(&pci_bus_type, &amd_iommu_ops);
 }
 
 int __init amd_iommu_init_dma_ops(void)
@@ -2610,6 +2999,20 @@ out_err:
 	return NULL;
 }
 
+static int __init alloc_passthrough_domain(void)
+{
+	if (pt_domain != NULL)
+		return 0;
+
+	/* allocate passthrough domain */
+	pt_domain = protection_domain_alloc();
+	if (!pt_domain)
+		return -ENOMEM;
+
+	pt_domain->mode = PAGE_MODE_NONE;
+
+	return 0;
+}
 static int amd_iommu_domain_init(struct iommu_domain *dom)
 {
 	struct protection_domain *domain;
@@ -2623,6 +3026,8 @@ static int amd_iommu_domain_init(struct iommu_domain *dom)
 	if (!domain->pt_root)
 		goto out_free;
 
+	domain->iommu_domain = dom;
+
 	dom->priv = domain;
 
 	return 0;
@@ -2645,7 +3050,11 @@ static void amd_iommu_domain_destroy(struct iommu_domain *dom)
 
 	BUG_ON(domain->dev_cnt != 0);
 
-	free_pagetable(domain);
+	if (domain->mode != PAGE_MODE_NONE)
+		free_pagetable(domain);
+
+	if (domain->flags & PD_IOMMUV2_MASK)
+		free_gcr3_table(domain);
 
 	protection_domain_free(domain);
 
@@ -2702,13 +3111,15 @@ static int amd_iommu_attach_device(struct iommu_domain *dom,
 }
 
 static int amd_iommu_map(struct iommu_domain *dom, unsigned long iova,
-			 phys_addr_t paddr, int gfp_order, int iommu_prot)
+			 phys_addr_t paddr, size_t page_size, int iommu_prot)
 {
-	unsigned long page_size = 0x1000UL << gfp_order;
 	struct protection_domain *domain = dom->priv;
 	int prot = 0;
 	int ret;
 
+	if (domain->mode == PAGE_MODE_NONE)
+		return -EINVAL;
+
 	if (iommu_prot & IOMMU_READ)
 		prot |= IOMMU_PROT_IR;
 	if (iommu_prot & IOMMU_WRITE)
@@ -2721,13 +3132,14 @@ static int amd_iommu_map(struct iommu_domain *dom, unsigned long iova,
 	return ret;
 }
 
-static int amd_iommu_unmap(struct iommu_domain *dom, unsigned long iova,
-			   int gfp_order)
+static size_t amd_iommu_unmap(struct iommu_domain *dom, unsigned long iova,
+			   size_t page_size)
 {
 	struct protection_domain *domain = dom->priv;
-	unsigned long page_size, unmap_size;
+	size_t unmap_size;
 
-	page_size  = 0x1000UL << gfp_order;
+	if (domain->mode == PAGE_MODE_NONE)
+		return -EINVAL;
 
 	mutex_lock(&domain->api_lock);
 	unmap_size = iommu_unmap_page(domain, iova, page_size);
@@ -2735,7 +3147,7 @@ static int amd_iommu_unmap(struct iommu_domain *dom, unsigned long iova,
 
 	domain_flush_tlb_pde(domain);
 
-	return get_order(unmap_size);
+	return unmap_size;
 }
 
 static phys_addr_t amd_iommu_iova_to_phys(struct iommu_domain *dom,
@@ -2746,6 +3158,9 @@ static phys_addr_t amd_iommu_iova_to_phys(struct iommu_domain *dom,
 	phys_addr_t paddr;
 	u64 *pte, __pte;
 
+	if (domain->mode == PAGE_MODE_NONE)
+		return iova;
+
 	pte = fetch_pte(domain, iova);
 
 	if (!pte || !IOMMU_PTE_PRESENT(*pte))
@@ -2773,6 +3188,26 @@ static int amd_iommu_domain_has_cap(struct iommu_domain *domain,
 	return 0;
 }
 
+static int amd_iommu_device_group(struct device *dev, unsigned int *groupid)
+{
+	struct iommu_dev_data *dev_data = dev->archdata.iommu;
+	struct pci_dev *pdev = to_pci_dev(dev);
+	u16 devid;
+
+	if (!dev_data)
+		return -ENODEV;
+
+	if (pdev->is_virtfn || !iommu_group_mf)
+		devid = dev_data->devid;
+	else
+		devid = calc_devid(pdev->bus->number,
+				   PCI_DEVFN(PCI_SLOT(pdev->devfn), 0));
+
+	*groupid = amd_iommu_alias_table[devid];
+
+	return 0;
+}
+
 static struct iommu_ops amd_iommu_ops = {
 	.domain_init = amd_iommu_domain_init,
 	.domain_destroy = amd_iommu_domain_destroy,
@@ -2782,6 +3217,8 @@ static struct iommu_ops amd_iommu_ops = {
 	.unmap = amd_iommu_unmap,
 	.iova_to_phys = amd_iommu_iova_to_phys,
 	.domain_has_cap = amd_iommu_domain_has_cap,
+	.device_group = amd_iommu_device_group,
+	.pgsize_bitmap	= AMD_IOMMU_PGSIZES,
 };
 
 /*****************************************************************************
@@ -2796,21 +3233,23 @@ static struct iommu_ops amd_iommu_ops = {
 
 int __init amd_iommu_init_passthrough(void)
 {
-	struct amd_iommu *iommu;
+	struct iommu_dev_data *dev_data;
 	struct pci_dev *dev = NULL;
+	struct amd_iommu *iommu;
 	u16 devid;
+	int ret;
 
-	/* allocate passthrough domain */
-	pt_domain = protection_domain_alloc();
-	if (!pt_domain)
-		return -ENOMEM;
-
-	pt_domain->mode |= PAGE_MODE_NONE;
+	ret = alloc_passthrough_domain();
+	if (ret)
+		return ret;
 
 	for_each_pci_dev(dev) {
 		if (!check_device(&dev->dev))
 			continue;
 
+		dev_data = get_dev_data(&dev->dev);
+		dev_data->passthrough = true;
+
 		devid = get_device_id(&dev->dev);
 
 		iommu = amd_iommu_rlookup_table[devid];
@@ -2820,7 +3259,375 @@ int __init amd_iommu_init_passthrough(void)
 		attach_device(&dev->dev, pt_domain);
 	}
 
+	amd_iommu_stats_init();
+
 	pr_info("AMD-Vi: Initialized for Passthrough Mode\n");
 
 	return 0;
 }
+
+/* IOMMUv2 specific functions */
+int amd_iommu_register_ppr_notifier(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_register(&ppr_notifier, nb);
+}
+EXPORT_SYMBOL(amd_iommu_register_ppr_notifier);
+
+int amd_iommu_unregister_ppr_notifier(struct notifier_block *nb)
+{
+	return atomic_notifier_chain_unregister(&ppr_notifier, nb);
+}
+EXPORT_SYMBOL(amd_iommu_unregister_ppr_notifier);
+
+void amd_iommu_domain_direct_map(struct iommu_domain *dom)
+{
+	struct protection_domain *domain = dom->priv;
+	unsigned long flags;
+
+	spin_lock_irqsave(&domain->lock, flags);
+
+	/* Update data structure */
+	domain->mode    = PAGE_MODE_NONE;
+	domain->updated = true;
+
+	/* Make changes visible to IOMMUs */
+	update_domain(domain);
+
+	/* Page-table is not visible to IOMMU anymore, so free it */
+	free_pagetable(domain);
+
+	spin_unlock_irqrestore(&domain->lock, flags);
+}
+EXPORT_SYMBOL(amd_iommu_domain_direct_map);
+
+int amd_iommu_domain_enable_v2(struct iommu_domain *dom, int pasids)
+{
+	struct protection_domain *domain = dom->priv;
+	unsigned long flags;
+	int levels, ret;
+
+	if (pasids <= 0 || pasids > (PASID_MASK + 1))
+		return -EINVAL;
+
+	/* Number of GCR3 table levels required */
+	for (levels = 0; (pasids - 1) & ~0x1ff; pasids >>= 9)
+		levels += 1;
+
+	if (levels > amd_iommu_max_glx_val)
+		return -EINVAL;
+
+	spin_lock_irqsave(&domain->lock, flags);
+
+	/*
+	 * Save us all sanity checks whether devices already in the
+	 * domain support IOMMUv2. Just force that the domain has no
+	 * devices attached when it is switched into IOMMUv2 mode.
+	 */
+	ret = -EBUSY;
+	if (domain->dev_cnt > 0 || domain->flags & PD_IOMMUV2_MASK)
+		goto out;
+
+	ret = -ENOMEM;
+	domain->gcr3_tbl = (void *)get_zeroed_page(GFP_ATOMIC);
+	if (domain->gcr3_tbl == NULL)
+		goto out;
+
+	domain->glx      = levels;
+	domain->flags   |= PD_IOMMUV2_MASK;
+	domain->updated  = true;
+
+	update_domain(domain);
+
+	ret = 0;
+
+out:
+	spin_unlock_irqrestore(&domain->lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_domain_enable_v2);
+
+static int __flush_pasid(struct protection_domain *domain, int pasid,
+			 u64 address, bool size)
+{
+	struct iommu_dev_data *dev_data;
+	struct iommu_cmd cmd;
+	int i, ret;
+
+	if (!(domain->flags & PD_IOMMUV2_MASK))
+		return -EINVAL;
+
+	build_inv_iommu_pasid(&cmd, domain->id, pasid, address, size);
+
+	/*
+	 * IOMMU TLB needs to be flushed before Device TLB to
+	 * prevent device TLB refill from IOMMU TLB
+	 */
+	for (i = 0; i < amd_iommus_present; ++i) {
+		if (domain->dev_iommu[i] == 0)
+			continue;
+
+		ret = iommu_queue_command(amd_iommus[i], &cmd);
+		if (ret != 0)
+			goto out;
+	}
+
+	/* Wait until IOMMU TLB flushes are complete */
+	domain_flush_complete(domain);
+
+	/* Now flush device TLBs */
+	list_for_each_entry(dev_data, &domain->dev_list, list) {
+		struct amd_iommu *iommu;
+		int qdep;
+
+		BUG_ON(!dev_data->ats.enabled);
+
+		qdep  = dev_data->ats.qdep;
+		iommu = amd_iommu_rlookup_table[dev_data->devid];
+
+		build_inv_iotlb_pasid(&cmd, dev_data->devid, pasid,
+				      qdep, address, size);
+
+		ret = iommu_queue_command(iommu, &cmd);
+		if (ret != 0)
+			goto out;
+	}
+
+	/* Wait until all device TLBs are flushed */
+	domain_flush_complete(domain);
+
+	ret = 0;
+
+out:
+
+	return ret;
+}
+
+static int __amd_iommu_flush_page(struct protection_domain *domain, int pasid,
+				  u64 address)
+{
+	INC_STATS_COUNTER(invalidate_iotlb);
+
+	return __flush_pasid(domain, pasid, address, false);
+}
+
+int amd_iommu_flush_page(struct iommu_domain *dom, int pasid,
+			 u64 address)
+{
+	struct protection_domain *domain = dom->priv;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&domain->lock, flags);
+	ret = __amd_iommu_flush_page(domain, pasid, address);
+	spin_unlock_irqrestore(&domain->lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_flush_page);
+
+static int __amd_iommu_flush_tlb(struct protection_domain *domain, int pasid)
+{
+	INC_STATS_COUNTER(invalidate_iotlb_all);
+
+	return __flush_pasid(domain, pasid, CMD_INV_IOMMU_ALL_PAGES_ADDRESS,
+			     true);
+}
+
+int amd_iommu_flush_tlb(struct iommu_domain *dom, int pasid)
+{
+	struct protection_domain *domain = dom->priv;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&domain->lock, flags);
+	ret = __amd_iommu_flush_tlb(domain, pasid);
+	spin_unlock_irqrestore(&domain->lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_flush_tlb);
+
+static u64 *__get_gcr3_pte(u64 *root, int level, int pasid, bool alloc)
+{
+	int index;
+	u64 *pte;
+
+	while (true) {
+
+		index = (pasid >> (9 * level)) & 0x1ff;
+		pte   = &root[index];
+
+		if (level == 0)
+			break;
+
+		if (!(*pte & GCR3_VALID)) {
+			if (!alloc)
+				return NULL;
+
+			root = (void *)get_zeroed_page(GFP_ATOMIC);
+			if (root == NULL)
+				return NULL;
+
+			*pte = __pa(root) | GCR3_VALID;
+		}
+
+		root = __va(*pte & PAGE_MASK);
+
+		level -= 1;
+	}
+
+	return pte;
+}
+
+static int __set_gcr3(struct protection_domain *domain, int pasid,
+		      unsigned long cr3)
+{
+	u64 *pte;
+
+	if (domain->mode != PAGE_MODE_NONE)
+		return -EINVAL;
+
+	pte = __get_gcr3_pte(domain->gcr3_tbl, domain->glx, pasid, true);
+	if (pte == NULL)
+		return -ENOMEM;
+
+	*pte = (cr3 & PAGE_MASK) | GCR3_VALID;
+
+	return __amd_iommu_flush_tlb(domain, pasid);
+}
+
+static int __clear_gcr3(struct protection_domain *domain, int pasid)
+{
+	u64 *pte;
+
+	if (domain->mode != PAGE_MODE_NONE)
+		return -EINVAL;
+
+	pte = __get_gcr3_pte(domain->gcr3_tbl, domain->glx, pasid, false);
+	if (pte == NULL)
+		return 0;
+
+	*pte = 0;
+
+	return __amd_iommu_flush_tlb(domain, pasid);
+}
+
+int amd_iommu_domain_set_gcr3(struct iommu_domain *dom, int pasid,
+			      unsigned long cr3)
+{
+	struct protection_domain *domain = dom->priv;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&domain->lock, flags);
+	ret = __set_gcr3(domain, pasid, cr3);
+	spin_unlock_irqrestore(&domain->lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_domain_set_gcr3);
+
+int amd_iommu_domain_clear_gcr3(struct iommu_domain *dom, int pasid)
+{
+	struct protection_domain *domain = dom->priv;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&domain->lock, flags);
+	ret = __clear_gcr3(domain, pasid);
+	spin_unlock_irqrestore(&domain->lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_domain_clear_gcr3);
+
+int amd_iommu_complete_ppr(struct pci_dev *pdev, int pasid,
+			   int status, int tag)
+{
+	struct iommu_dev_data *dev_data;
+	struct amd_iommu *iommu;
+	struct iommu_cmd cmd;
+
+	INC_STATS_COUNTER(complete_ppr);
+
+	dev_data = get_dev_data(&pdev->dev);
+	iommu    = amd_iommu_rlookup_table[dev_data->devid];
+
+	build_complete_ppr(&cmd, dev_data->devid, pasid, status,
+			   tag, dev_data->pri_tlp);
+
+	return iommu_queue_command(iommu, &cmd);
+}
+EXPORT_SYMBOL(amd_iommu_complete_ppr);
+
+struct iommu_domain *amd_iommu_get_v2_domain(struct pci_dev *pdev)
+{
+	struct protection_domain *domain;
+
+	domain = get_domain(&pdev->dev);
+	if (IS_ERR(domain))
+		return NULL;
+
+	/* Only return IOMMUv2 domains */
+	if (!(domain->flags & PD_IOMMUV2_MASK))
+		return NULL;
+
+	return domain->iommu_domain;
+}
+EXPORT_SYMBOL(amd_iommu_get_v2_domain);
+
+void amd_iommu_enable_device_erratum(struct pci_dev *pdev, u32 erratum)
+{
+	struct iommu_dev_data *dev_data;
+
+	if (!amd_iommu_v2_supported())
+		return;
+
+	dev_data = get_dev_data(&pdev->dev);
+	dev_data->errata |= (1 << erratum);
+}
+EXPORT_SYMBOL(amd_iommu_enable_device_erratum);
+
+int amd_iommu_device_info(struct pci_dev *pdev,
+                          struct amd_iommu_device_info *info)
+{
+	int max_pasids;
+	int pos;
+
+	if (pdev == NULL || info == NULL)
+		return -EINVAL;
+
+	if (!amd_iommu_v2_supported())
+		return -EINVAL;
+
+	memset(info, 0, sizeof(*info));
+
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_ATS);
+	if (pos)
+		info->flags |= AMD_IOMMU_DEVICE_FLAG_ATS_SUP;
+
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PRI);
+	if (pos)
+		info->flags |= AMD_IOMMU_DEVICE_FLAG_PRI_SUP;
+
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_PASID);
+	if (pos) {
+		int features;
+
+		max_pasids = 1 << (9 * (amd_iommu_max_glx_val + 1));
+		max_pasids = min(max_pasids, (1 << 20));
+
+		info->flags |= AMD_IOMMU_DEVICE_FLAG_PASID_SUP;
+		info->max_pasids = min(pci_max_pasids(pdev), max_pasids);
+
+		features = pci_pasid_features(pdev);
+		if (features & PCI_PASID_CAP_EXEC)
+			info->flags |= AMD_IOMMU_DEVICE_FLAG_EXEC_SUP;
+		if (features & PCI_PASID_CAP_PRIV)
+			info->flags |= AMD_IOMMU_DEVICE_FLAG_PRIV_SUP;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(amd_iommu_device_info);
diff --git a/drivers/iommu/amd_iommu_init.c b/drivers/iommu/amd_iommu_init.c
index 82d2410f4205..bdea288dc185 100644
--- a/drivers/iommu/amd_iommu_init.c
+++ b/drivers/iommu/amd_iommu_init.c
@@ -25,6 +25,7 @@
 #include <linux/interrupt.h>
 #include <linux/msi.h>
 #include <linux/amd-iommu.h>
+#include <linux/export.h>
 #include <asm/pci-direct.h>
 #include <asm/iommu.h>
 #include <asm/gart.h>
@@ -141,6 +142,12 @@ int amd_iommus_present;
 bool amd_iommu_np_cache __read_mostly;
 bool amd_iommu_iotlb_sup __read_mostly = true;
 
+u32 amd_iommu_max_pasids __read_mostly = ~0;
+
+bool amd_iommu_v2_present __read_mostly;
+
+bool amd_iommu_force_isolation __read_mostly;
+
 /*
  * The ACPI table parsing functions set this variable on an error
  */
@@ -299,6 +306,16 @@ static void iommu_feature_disable(struct amd_iommu *iommu, u8 bit)
 	writel(ctrl, iommu->mmio_base + MMIO_CONTROL_OFFSET);
 }
 
+static void iommu_set_inv_tlb_timeout(struct amd_iommu *iommu, int timeout)
+{
+	u32 ctrl;
+
+	ctrl = readl(iommu->mmio_base + MMIO_CONTROL_OFFSET);
+	ctrl &= ~CTRL_INV_TO_MASK;
+	ctrl |= (timeout << CONTROL_INV_TIMEOUT) & CTRL_INV_TO_MASK;
+	writel(ctrl, iommu->mmio_base + MMIO_CONTROL_OFFSET);
+}
+
 /* Function to enable the hardware */
 static void iommu_enable(struct amd_iommu *iommu)
 {
@@ -581,21 +598,69 @@ static void __init free_event_buffer(struct amd_iommu *iommu)
 	free_pages((unsigned long)iommu->evt_buf, get_order(EVT_BUFFER_SIZE));
 }
 
+/* allocates the memory where the IOMMU will log its events to */
+static u8 * __init alloc_ppr_log(struct amd_iommu *iommu)
+{
+	iommu->ppr_log = (u8 *)__get_free_pages(GFP_KERNEL | __GFP_ZERO,
+						get_order(PPR_LOG_SIZE));
+
+	if (iommu->ppr_log == NULL)
+		return NULL;
+
+	return iommu->ppr_log;
+}
+
+static void iommu_enable_ppr_log(struct amd_iommu *iommu)
+{
+	u64 entry;
+
+	if (iommu->ppr_log == NULL)
+		return;
+
+	entry = (u64)virt_to_phys(iommu->ppr_log) | PPR_LOG_SIZE_512;
+
+	memcpy_toio(iommu->mmio_base + MMIO_PPR_LOG_OFFSET,
+		    &entry, sizeof(entry));
+
+	/* set head and tail to zero manually */
+	writel(0x00, iommu->mmio_base + MMIO_PPR_HEAD_OFFSET);
+	writel(0x00, iommu->mmio_base + MMIO_PPR_TAIL_OFFSET);
+
+	iommu_feature_enable(iommu, CONTROL_PPFLOG_EN);
+	iommu_feature_enable(iommu, CONTROL_PPR_EN);
+}
+
+static void __init free_ppr_log(struct amd_iommu *iommu)
+{
+	if (iommu->ppr_log == NULL)
+		return;
+
+	free_pages((unsigned long)iommu->ppr_log, get_order(PPR_LOG_SIZE));
+}
+
+static void iommu_enable_gt(struct amd_iommu *iommu)
+{
+	if (!iommu_feature(iommu, FEATURE_GT))
+		return;
+
+	iommu_feature_enable(iommu, CONTROL_GT_EN);
+}
+
 /* sets a specific bit in the device table entry. */
 static void set_dev_entry_bit(u16 devid, u8 bit)
 {
-	int i = (bit >> 5) & 0x07;
-	int _bit = bit & 0x1f;
+	int i = (bit >> 6) & 0x03;
+	int _bit = bit & 0x3f;
 
-	amd_iommu_dev_table[devid].data[i] |= (1 << _bit);
+	amd_iommu_dev_table[devid].data[i] |= (1UL << _bit);
 }
 
 static int get_dev_entry_bit(u16 devid, u8 bit)
 {
-	int i = (bit >> 5) & 0x07;
-	int _bit = bit & 0x1f;
+	int i = (bit >> 6) & 0x03;
+	int _bit = bit & 0x3f;
 
-	return (amd_iommu_dev_table[devid].data[i] & (1 << _bit)) >> _bit;
+	return (amd_iommu_dev_table[devid].data[i] & (1UL << _bit)) >> _bit;
 }
 
 
@@ -699,6 +764,32 @@ static void __init init_iommu_from_pci(struct amd_iommu *iommu)
 
 	iommu->features = ((u64)high << 32) | low;
 
+	if (iommu_feature(iommu, FEATURE_GT)) {
+		int glxval;
+		u32 pasids;
+		u64 shift;
+
+		shift   = iommu->features & FEATURE_PASID_MASK;
+		shift >>= FEATURE_PASID_SHIFT;
+		pasids  = (1 << shift);
+
+		amd_iommu_max_pasids = min(amd_iommu_max_pasids, pasids);
+
+		glxval   = iommu->features & FEATURE_GLXVAL_MASK;
+		glxval >>= FEATURE_GLXVAL_SHIFT;
+
+		if (amd_iommu_max_glx_val == -1)
+			amd_iommu_max_glx_val = glxval;
+		else
+			amd_iommu_max_glx_val = min(amd_iommu_max_glx_val, glxval);
+	}
+
+	if (iommu_feature(iommu, FEATURE_GT) &&
+	    iommu_feature(iommu, FEATURE_PPR)) {
+		iommu->is_iommu_v2   = true;
+		amd_iommu_v2_present = true;
+	}
+
 	if (!is_rd890_iommu(iommu->dev))
 		return;
 
@@ -901,6 +992,7 @@ static void __init free_iommu_one(struct amd_iommu *iommu)
 {
 	free_command_buffer(iommu);
 	free_event_buffer(iommu);
+	free_ppr_log(iommu);
 	iommu_unmap_mmio_space(iommu);
 }
 
@@ -964,6 +1056,12 @@ static int __init init_iommu_one(struct amd_iommu *iommu, struct ivhd_header *h)
 	init_iommu_from_acpi(iommu, h);
 	init_iommu_devices(iommu);
 
+	if (iommu_feature(iommu, FEATURE_PPR)) {
+		iommu->ppr_log = alloc_ppr_log(iommu);
+		if (!iommu->ppr_log)
+			return -ENOMEM;
+	}
+
 	if (iommu->cap & (1UL << IOMMU_CAP_NPCACHE))
 		amd_iommu_np_cache = true;
 
@@ -1050,6 +1148,9 @@ static int iommu_setup_msi(struct amd_iommu *iommu)
 	iommu->int_enabled = true;
 	iommu_feature_enable(iommu, CONTROL_EVT_INT_EN);
 
+	if (iommu->ppr_log != NULL)
+		iommu_feature_enable(iommu, CONTROL_PPFINT_EN);
+
 	return 0;
 }
 
@@ -1209,6 +1310,9 @@ static void iommu_init_flags(struct amd_iommu *iommu)
 	 * make IOMMU memory accesses cache coherent
 	 */
 	iommu_feature_enable(iommu, CONTROL_COHERENT_EN);
+
+	/* Set IOTLB invalidation timeout to 1s */
+	iommu_set_inv_tlb_timeout(iommu, CTRL_INV_TO_1S);
 }
 
 static void iommu_apply_resume_quirks(struct amd_iommu *iommu)
@@ -1274,6 +1378,8 @@ static void enable_iommus(void)
 		iommu_set_device_table(iommu);
 		iommu_enable_command_buffer(iommu);
 		iommu_enable_event_buffer(iommu);
+		iommu_enable_ppr_log(iommu);
+		iommu_enable_gt(iommu);
 		iommu_set_exclusion_range(iommu);
 		iommu_init_msi(iommu);
 		iommu_enable(iommu);
@@ -1303,13 +1409,6 @@ static void amd_iommu_resume(void)
 
 	/* re-load the hardware */
 	enable_iommus();
-
-	/*
-	 * we have to flush after the IOMMUs are enabled because a
-	 * disabled IOMMU will never execute the commands we send
-	 */
-	for_each_iommu(iommu)
-		iommu_flush_all_caches(iommu);
 }
 
 static int amd_iommu_suspend(void)
@@ -1560,6 +1659,8 @@ static int __init parse_amd_iommu_options(char *str)
 			amd_iommu_unmap_flush = true;
 		if (strncmp(str, "off", 3) == 0)
 			amd_iommu_disabled = true;
+		if (strncmp(str, "force_isolation", 15) == 0)
+			amd_iommu_force_isolation = true;
 	}
 
 	return 1;
@@ -1572,3 +1673,9 @@ IOMMU_INIT_FINISH(amd_iommu_detect,
 		  gart_iommu_hole_init,
 		  0,
 		  0);
+
+bool amd_iommu_v2_supported(void)
+{
+	return amd_iommu_v2_present;
+}
+EXPORT_SYMBOL(amd_iommu_v2_supported);
diff --git a/drivers/iommu/amd_iommu_proto.h b/drivers/iommu/amd_iommu_proto.h
index 7ffaa64410b0..1a7f41c6cc66 100644
--- a/drivers/iommu/amd_iommu_proto.h
+++ b/drivers/iommu/amd_iommu_proto.h
@@ -31,6 +31,30 @@ extern int amd_iommu_init_devices(void);
 extern void amd_iommu_uninit_devices(void);
 extern void amd_iommu_init_notifier(void);
 extern void amd_iommu_init_api(void);
+
+/* IOMMUv2 specific functions */
+struct iommu_domain;
+
+extern bool amd_iommu_v2_supported(void);
+extern int amd_iommu_register_ppr_notifier(struct notifier_block *nb);
+extern int amd_iommu_unregister_ppr_notifier(struct notifier_block *nb);
+extern void amd_iommu_domain_direct_map(struct iommu_domain *dom);
+extern int amd_iommu_domain_enable_v2(struct iommu_domain *dom, int pasids);
+extern int amd_iommu_flush_page(struct iommu_domain *dom, int pasid,
+				u64 address);
+extern int amd_iommu_flush_tlb(struct iommu_domain *dom, int pasid);
+extern int amd_iommu_domain_set_gcr3(struct iommu_domain *dom, int pasid,
+				     unsigned long cr3);
+extern int amd_iommu_domain_clear_gcr3(struct iommu_domain *dom, int pasid);
+extern struct iommu_domain *amd_iommu_get_v2_domain(struct pci_dev *pdev);
+
+#define PPR_SUCCESS			0x0
+#define PPR_INVALID			0x1
+#define PPR_FAILURE			0xf
+
+extern int amd_iommu_complete_ppr(struct pci_dev *pdev, int pasid,
+				  int status, int tag);
+
 #ifndef CONFIG_AMD_IOMMU_STATS
 
 static inline void amd_iommu_stats_init(void) { }
diff --git a/drivers/iommu/amd_iommu_types.h b/drivers/iommu/amd_iommu_types.h
index 5b9c5075e81a..2452f3b71736 100644
--- a/drivers/iommu/amd_iommu_types.h
+++ b/drivers/iommu/amd_iommu_types.h
@@ -69,11 +69,14 @@
 #define MMIO_EXCL_BASE_OFFSET   0x0020
 #define MMIO_EXCL_LIMIT_OFFSET  0x0028
 #define MMIO_EXT_FEATURES	0x0030
+#define MMIO_PPR_LOG_OFFSET	0x0038
 #define MMIO_CMD_HEAD_OFFSET	0x2000
 #define MMIO_CMD_TAIL_OFFSET	0x2008
 #define MMIO_EVT_HEAD_OFFSET	0x2010
 #define MMIO_EVT_TAIL_OFFSET	0x2018
 #define MMIO_STATUS_OFFSET	0x2020
+#define MMIO_PPR_HEAD_OFFSET	0x2030
+#define MMIO_PPR_TAIL_OFFSET	0x2038
 
 
 /* Extended Feature Bits */
@@ -87,8 +90,17 @@
 #define FEATURE_HE		(1ULL<<8)
 #define FEATURE_PC		(1ULL<<9)
 
+#define FEATURE_PASID_SHIFT	32
+#define FEATURE_PASID_MASK	(0x1fULL << FEATURE_PASID_SHIFT)
+
+#define FEATURE_GLXVAL_SHIFT	14
+#define FEATURE_GLXVAL_MASK	(0x03ULL << FEATURE_GLXVAL_SHIFT)
+
+#define PASID_MASK		0x000fffff
+
 /* MMIO status bits */
-#define MMIO_STATUS_COM_WAIT_INT_MASK	0x04
+#define MMIO_STATUS_COM_WAIT_INT_MASK	(1 << 2)
+#define MMIO_STATUS_PPR_INT_MASK	(1 << 6)
 
 /* event logging constants */
 #define EVENT_ENTRY_SIZE	0x10
@@ -115,6 +127,7 @@
 #define CONTROL_EVT_LOG_EN      0x02ULL
 #define CONTROL_EVT_INT_EN      0x03ULL
 #define CONTROL_COMWAIT_EN      0x04ULL
+#define CONTROL_INV_TIMEOUT	0x05ULL
 #define CONTROL_PASSPW_EN       0x08ULL
 #define CONTROL_RESPASSPW_EN    0x09ULL
 #define CONTROL_COHERENT_EN     0x0aULL
@@ -122,18 +135,34 @@
 #define CONTROL_CMDBUF_EN       0x0cULL
 #define CONTROL_PPFLOG_EN       0x0dULL
 #define CONTROL_PPFINT_EN       0x0eULL
+#define CONTROL_PPR_EN          0x0fULL
+#define CONTROL_GT_EN           0x10ULL
+
+#define CTRL_INV_TO_MASK	(7 << CONTROL_INV_TIMEOUT)
+#define CTRL_INV_TO_NONE	0
+#define CTRL_INV_TO_1MS		1
+#define CTRL_INV_TO_10MS	2
+#define CTRL_INV_TO_100MS	3
+#define CTRL_INV_TO_1S		4
+#define CTRL_INV_TO_10S		5
+#define CTRL_INV_TO_100S	6
 
 /* command specific defines */
 #define CMD_COMPL_WAIT          0x01
 #define CMD_INV_DEV_ENTRY       0x02
 #define CMD_INV_IOMMU_PAGES	0x03
 #define CMD_INV_IOTLB_PAGES	0x04
+#define CMD_COMPLETE_PPR	0x07
 #define CMD_INV_ALL		0x08
 
 #define CMD_COMPL_WAIT_STORE_MASK	0x01
 #define CMD_COMPL_WAIT_INT_MASK		0x02
 #define CMD_INV_IOMMU_PAGES_SIZE_MASK	0x01
 #define CMD_INV_IOMMU_PAGES_PDE_MASK	0x02
+#define CMD_INV_IOMMU_PAGES_GN_MASK	0x04
+
+#define PPR_STATUS_MASK			0xf
+#define PPR_STATUS_SHIFT		12
 
 #define CMD_INV_IOMMU_ALL_PAGES_ADDRESS	0x7fffffffffffffffULL
 
@@ -165,6 +194,23 @@
 #define EVT_BUFFER_SIZE		8192 /* 512 entries */
 #define EVT_LEN_MASK		(0x9ULL << 56)
 
+/* Constants for PPR Log handling */
+#define PPR_LOG_ENTRIES		512
+#define PPR_LOG_SIZE_SHIFT	56
+#define PPR_LOG_SIZE_512	(0x9ULL << PPR_LOG_SIZE_SHIFT)
+#define PPR_ENTRY_SIZE		16
+#define PPR_LOG_SIZE		(PPR_ENTRY_SIZE * PPR_LOG_ENTRIES)
+
+#define PPR_REQ_TYPE(x)		(((x) >> 60) & 0xfULL)
+#define PPR_FLAGS(x)		(((x) >> 48) & 0xfffULL)
+#define PPR_DEVID(x)		((x) & 0xffffULL)
+#define PPR_TAG(x)		(((x) >> 32) & 0x3ffULL)
+#define PPR_PASID1(x)		(((x) >> 16) & 0xffffULL)
+#define PPR_PASID2(x)		(((x) >> 42) & 0xfULL)
+#define PPR_PASID(x)		((PPR_PASID2(x) << 16) | PPR_PASID1(x))
+
+#define PPR_REQ_FAULT		0x01
+
 #define PAGE_MODE_NONE    0x00
 #define PAGE_MODE_1_LEVEL 0x01
 #define PAGE_MODE_2_LEVEL 0x02
@@ -230,7 +276,24 @@
 #define IOMMU_PTE_IR (1ULL << 61)
 #define IOMMU_PTE_IW (1ULL << 62)
 
-#define DTE_FLAG_IOTLB	0x01
+#define DTE_FLAG_IOTLB	(0x01UL << 32)
+#define DTE_FLAG_GV	(0x01ULL << 55)
+#define DTE_GLX_SHIFT	(56)
+#define DTE_GLX_MASK	(3)
+
+#define DTE_GCR3_VAL_A(x)	(((x) >> 12) & 0x00007ULL)
+#define DTE_GCR3_VAL_B(x)	(((x) >> 15) & 0x0ffffULL)
+#define DTE_GCR3_VAL_C(x)	(((x) >> 31) & 0xfffffULL)
+
+#define DTE_GCR3_INDEX_A	0
+#define DTE_GCR3_INDEX_B	1
+#define DTE_GCR3_INDEX_C	1
+
+#define DTE_GCR3_SHIFT_A	58
+#define DTE_GCR3_SHIFT_B	16
+#define DTE_GCR3_SHIFT_C	43
+
+#define GCR3_VALID		0x01ULL
 
 #define IOMMU_PAGE_MASK (((1ULL << 52) - 1) & ~0xfffULL)
 #define IOMMU_PTE_PRESENT(pte) ((pte) & IOMMU_PTE_P)
@@ -257,6 +320,7 @@
 					      domain for an IOMMU */
 #define PD_PASSTHROUGH_MASK	(1UL << 2) /* domain has no page
 					      translation */
+#define PD_IOMMUV2_MASK		(1UL << 3) /* domain has gcr3 table */
 
 extern bool amd_iommu_dump;
 #define DUMP_printk(format, arg...)					\
@@ -285,6 +349,29 @@ extern bool amd_iommu_iotlb_sup;
 #define APERTURE_RANGE_INDEX(a)	((a) >> APERTURE_RANGE_SHIFT)
 #define APERTURE_PAGE_INDEX(a)	(((a) >> 21) & 0x3fULL)
 
+
+/*
+ * This struct is used to pass information about
+ * incoming PPR faults around.
+ */
+struct amd_iommu_fault {
+	u64 address;    /* IO virtual address of the fault*/
+	u32 pasid;      /* Address space identifier */
+	u16 device_id;  /* Originating PCI device id */
+	u16 tag;        /* PPR tag */
+	u16 flags;      /* Fault flags */
+
+};
+
+#define PPR_FAULT_EXEC	(1 << 1)
+#define PPR_FAULT_READ  (1 << 2)
+#define PPR_FAULT_WRITE (1 << 5)
+#define PPR_FAULT_USER  (1 << 6)
+#define PPR_FAULT_RSVD  (1 << 7)
+#define PPR_FAULT_GN    (1 << 8)
+
+struct iommu_domain;
+
 /*
  * This structure contains generic data for  IOMMU protection domains
  * independent of their use.
@@ -297,11 +384,15 @@ struct protection_domain {
 	u16 id;			/* the domain id written to the device table */
 	int mode;		/* paging mode (0-6 levels) */
 	u64 *pt_root;		/* page table root pointer */
+	int glx;		/* Number of levels for GCR3 table */
+	u64 *gcr3_tbl;		/* Guest CR3 table */
 	unsigned long flags;	/* flags to find out type of domain */
 	bool updated;		/* complete domain flush required */
 	unsigned dev_cnt;	/* devices assigned to this domain */
 	unsigned dev_iommu[MAX_IOMMUS]; /* per-IOMMU reference count */
 	void *priv;		/* private data */
+	struct iommu_domain *iommu_domain; /* Pointer to generic
+					      domain structure */
 
 };
 
@@ -315,10 +406,15 @@ struct iommu_dev_data {
 	struct protection_domain *domain; /* Domain the device is bound to */
 	atomic_t bind;			  /* Domain attach reverent count */
 	u16 devid;			  /* PCI Device ID */
+	bool iommu_v2;			  /* Device can make use of IOMMUv2 */
+	bool passthrough;		  /* Default for device is pt_domain */
 	struct {
 		bool enabled;
 		int qdep;
 	} ats;				  /* ATS state */
+	bool pri_tlp;			  /* PASID TLB required for
+					     PPR completions */
+	u32 errata;			  /* Bitmap for errata to apply */
 };
 
 /*
@@ -399,6 +495,9 @@ struct amd_iommu {
 	/* Extended features */
 	u64 features;
 
+	/* IOMMUv2 */
+	bool is_iommu_v2;
+
 	/*
 	 * Capability pointer. There could be more than one IOMMU per PCI
 	 * device function if there are more than one AMD IOMMU capability
@@ -431,6 +530,9 @@ struct amd_iommu {
 	/* MSI number for event interrupt */
 	u16 evt_msi_num;
 
+	/* Base of the PPR log, if present */
+	u8 *ppr_log;
+
 	/* true if interrupts for this IOMMU are already enabled */
 	bool int_enabled;
 
@@ -484,7 +586,7 @@ extern struct list_head amd_iommu_pd_list;
  * Structure defining one entry in the device table
  */
 struct dev_table_entry {
-	u32 data[8];
+	u64 data[4];
 };
 
 /*
@@ -549,6 +651,16 @@ extern unsigned long *amd_iommu_pd_alloc_bitmap;
  */
 extern bool amd_iommu_unmap_flush;
 
+/* Smallest number of PASIDs supported by any IOMMU in the system */
+extern u32 amd_iommu_max_pasids;
+
+extern bool amd_iommu_v2_present;
+
+extern bool amd_iommu_force_isolation;
+
+/* Max levels of glxval supported */
+extern int amd_iommu_max_glx_val;
+
 /* takes bus and device/function and returns the device id
  * FIXME: should that be in generic PCI code? */
 static inline u16 calc_devid(u8 bus, u8 devfn)
diff --git a/drivers/iommu/amd_iommu_v2.c b/drivers/iommu/amd_iommu_v2.c
new file mode 100644
index 000000000000..8add9f125d3e
--- /dev/null
+++ b/drivers/iommu/amd_iommu_v2.c
@@ -0,0 +1,994 @@
+/*
+ * Copyright (C) 2010-2012 Advanced Micro Devices, Inc.
+ * Author: Joerg Roedel <joerg.roedel@amd.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms 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/mmu_notifier.h>
+#include <linux/amd-iommu.h>
+#include <linux/mm_types.h>
+#include <linux/profile.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/iommu.h>
+#include <linux/wait.h>
+#include <linux/pci.h>
+#include <linux/gfp.h>
+
+#include "amd_iommu_types.h"
+#include "amd_iommu_proto.h"
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Joerg Roedel <joerg.roedel@amd.com>");
+
+#define MAX_DEVICES		0x10000
+#define PRI_QUEUE_SIZE		512
+
+struct pri_queue {
+	atomic_t inflight;
+	bool finish;
+	int status;
+};
+
+struct pasid_state {
+	struct list_head list;			/* For global state-list */
+	atomic_t count;				/* Reference count */
+	struct task_struct *task;		/* Task bound to this PASID */
+	struct mm_struct *mm;			/* mm_struct for the faults */
+	struct mmu_notifier mn;                 /* mmu_otifier handle */
+	struct pri_queue pri[PRI_QUEUE_SIZE];	/* PRI tag states */
+	struct device_state *device_state;	/* Link to our device_state */
+	int pasid;				/* PASID index */
+	spinlock_t lock;			/* Protect pri_queues */
+	wait_queue_head_t wq;			/* To wait for count == 0 */
+};
+
+struct device_state {
+	atomic_t count;
+	struct pci_dev *pdev;
+	struct pasid_state **states;
+	struct iommu_domain *domain;
+	int pasid_levels;
+	int max_pasids;
+	amd_iommu_invalid_ppr_cb inv_ppr_cb;
+	amd_iommu_invalidate_ctx inv_ctx_cb;
+	spinlock_t lock;
+	wait_queue_head_t wq;
+};
+
+struct fault {
+	struct work_struct work;
+	struct device_state *dev_state;
+	struct pasid_state *state;
+	struct mm_struct *mm;
+	u64 address;
+	u16 devid;
+	u16 pasid;
+	u16 tag;
+	u16 finish;
+	u16 flags;
+};
+
+struct device_state **state_table;
+static spinlock_t state_lock;
+
+/* List and lock for all pasid_states */
+static LIST_HEAD(pasid_state_list);
+static DEFINE_SPINLOCK(ps_lock);
+
+static struct workqueue_struct *iommu_wq;
+
+/*
+ * Empty page table - Used between
+ * mmu_notifier_invalidate_range_start and
+ * mmu_notifier_invalidate_range_end
+ */
+static u64 *empty_page_table;
+
+static void free_pasid_states(struct device_state *dev_state);
+static void unbind_pasid(struct device_state *dev_state, int pasid);
+static int task_exit(struct notifier_block *nb, unsigned long e, void *data);
+
+static u16 device_id(struct pci_dev *pdev)
+{
+	u16 devid;
+
+	devid = pdev->bus->number;
+	devid = (devid << 8) | pdev->devfn;
+
+	return devid;
+}
+
+static struct device_state *get_device_state(u16 devid)
+{
+	struct device_state *dev_state;
+	unsigned long flags;
+
+	spin_lock_irqsave(&state_lock, flags);
+	dev_state = state_table[devid];
+	if (dev_state != NULL)
+		atomic_inc(&dev_state->count);
+	spin_unlock_irqrestore(&state_lock, flags);
+
+	return dev_state;
+}
+
+static void free_device_state(struct device_state *dev_state)
+{
+	/*
+	 * First detach device from domain - No more PRI requests will arrive
+	 * from that device after it is unbound from the IOMMUv2 domain.
+	 */
+	iommu_detach_device(dev_state->domain, &dev_state->pdev->dev);
+
+	/* Everything is down now, free the IOMMUv2 domain */
+	iommu_domain_free(dev_state->domain);
+
+	/* Finally get rid of the device-state */
+	kfree(dev_state);
+}
+
+static void put_device_state(struct device_state *dev_state)
+{
+	if (atomic_dec_and_test(&dev_state->count))
+		wake_up(&dev_state->wq);
+}
+
+static void put_device_state_wait(struct device_state *dev_state)
+{
+	DEFINE_WAIT(wait);
+
+	prepare_to_wait(&dev_state->wq, &wait, TASK_UNINTERRUPTIBLE);
+	if (!atomic_dec_and_test(&dev_state->count))
+		schedule();
+	finish_wait(&dev_state->wq, &wait);
+
+	free_device_state(dev_state);
+}
+
+static struct notifier_block profile_nb = {
+	.notifier_call = task_exit,
+};
+
+static void link_pasid_state(struct pasid_state *pasid_state)
+{
+	spin_lock(&ps_lock);
+	list_add_tail(&pasid_state->list, &pasid_state_list);
+	spin_unlock(&ps_lock);
+}
+
+static void __unlink_pasid_state(struct pasid_state *pasid_state)
+{
+	list_del(&pasid_state->list);
+}
+
+static void unlink_pasid_state(struct pasid_state *pasid_state)
+{
+	spin_lock(&ps_lock);
+	__unlink_pasid_state(pasid_state);
+	spin_unlock(&ps_lock);
+}
+
+/* Must be called under dev_state->lock */
+static struct pasid_state **__get_pasid_state_ptr(struct device_state *dev_state,
+						  int pasid, bool alloc)
+{
+	struct pasid_state **root, **ptr;
+	int level, index;
+
+	level = dev_state->pasid_levels;
+	root  = dev_state->states;
+
+	while (true) {
+
+		index = (pasid >> (9 * level)) & 0x1ff;
+		ptr   = &root[index];
+
+		if (level == 0)
+			break;
+
+		if (*ptr == NULL) {
+			if (!alloc)
+				return NULL;
+
+			*ptr = (void *)get_zeroed_page(GFP_ATOMIC);
+			if (*ptr == NULL)
+				return NULL;
+		}
+
+		root   = (struct pasid_state **)*ptr;
+		level -= 1;
+	}
+
+	return ptr;
+}
+
+static int set_pasid_state(struct device_state *dev_state,
+			   struct pasid_state *pasid_state,
+			   int pasid)
+{
+	struct pasid_state **ptr;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&dev_state->lock, flags);
+	ptr = __get_pasid_state_ptr(dev_state, pasid, true);
+
+	ret = -ENOMEM;
+	if (ptr == NULL)
+		goto out_unlock;
+
+	ret = -ENOMEM;
+	if (*ptr != NULL)
+		goto out_unlock;
+
+	*ptr = pasid_state;
+
+	ret = 0;
+
+out_unlock:
+	spin_unlock_irqrestore(&dev_state->lock, flags);
+
+	return ret;
+}
+
+static void clear_pasid_state(struct device_state *dev_state, int pasid)
+{
+	struct pasid_state **ptr;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev_state->lock, flags);
+	ptr = __get_pasid_state_ptr(dev_state, pasid, true);
+
+	if (ptr == NULL)
+		goto out_unlock;
+
+	*ptr = NULL;
+
+out_unlock:
+	spin_unlock_irqrestore(&dev_state->lock, flags);
+}
+
+static struct pasid_state *get_pasid_state(struct device_state *dev_state,
+					   int pasid)
+{
+	struct pasid_state **ptr, *ret = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev_state->lock, flags);
+	ptr = __get_pasid_state_ptr(dev_state, pasid, false);
+
+	if (ptr == NULL)
+		goto out_unlock;
+
+	ret = *ptr;
+	if (ret)
+		atomic_inc(&ret->count);
+
+out_unlock:
+	spin_unlock_irqrestore(&dev_state->lock, flags);
+
+	return ret;
+}
+
+static void free_pasid_state(struct pasid_state *pasid_state)
+{
+	kfree(pasid_state);
+}
+
+static void put_pasid_state(struct pasid_state *pasid_state)
+{
+	if (atomic_dec_and_test(&pasid_state->count)) {
+		put_device_state(pasid_state->device_state);
+		wake_up(&pasid_state->wq);
+	}
+}
+
+static void put_pasid_state_wait(struct pasid_state *pasid_state)
+{
+	DEFINE_WAIT(wait);
+
+	prepare_to_wait(&pasid_state->wq, &wait, TASK_UNINTERRUPTIBLE);
+
+	if (atomic_dec_and_test(&pasid_state->count))
+		put_device_state(pasid_state->device_state);
+	else
+		schedule();
+
+	finish_wait(&pasid_state->wq, &wait);
+	mmput(pasid_state->mm);
+	free_pasid_state(pasid_state);
+}
+
+static void __unbind_pasid(struct pasid_state *pasid_state)
+{
+	struct iommu_domain *domain;
+
+	domain = pasid_state->device_state->domain;
+
+	amd_iommu_domain_clear_gcr3(domain, pasid_state->pasid);
+	clear_pasid_state(pasid_state->device_state, pasid_state->pasid);
+
+	/* Make sure no more pending faults are in the queue */
+	flush_workqueue(iommu_wq);
+
+	mmu_notifier_unregister(&pasid_state->mn, pasid_state->mm);
+
+	put_pasid_state(pasid_state); /* Reference taken in bind() function */
+}
+
+static void unbind_pasid(struct device_state *dev_state, int pasid)
+{
+	struct pasid_state *pasid_state;
+
+	pasid_state = get_pasid_state(dev_state, pasid);
+	if (pasid_state == NULL)
+		return;
+
+	unlink_pasid_state(pasid_state);
+	__unbind_pasid(pasid_state);
+	put_pasid_state_wait(pasid_state); /* Reference taken in this function */
+}
+
+static void free_pasid_states_level1(struct pasid_state **tbl)
+{
+	int i;
+
+	for (i = 0; i < 512; ++i) {
+		if (tbl[i] == NULL)
+			continue;
+
+		free_page((unsigned long)tbl[i]);
+	}
+}
+
+static void free_pasid_states_level2(struct pasid_state **tbl)
+{
+	struct pasid_state **ptr;
+	int i;
+
+	for (i = 0; i < 512; ++i) {
+		if (tbl[i] == NULL)
+			continue;
+
+		ptr = (struct pasid_state **)tbl[i];
+		free_pasid_states_level1(ptr);
+	}
+}
+
+static void free_pasid_states(struct device_state *dev_state)
+{
+	struct pasid_state *pasid_state;
+	int i;
+
+	for (i = 0; i < dev_state->max_pasids; ++i) {
+		pasid_state = get_pasid_state(dev_state, i);
+		if (pasid_state == NULL)
+			continue;
+
+		put_pasid_state(pasid_state);
+		unbind_pasid(dev_state, i);
+	}
+
+	if (dev_state->pasid_levels == 2)
+		free_pasid_states_level2(dev_state->states);
+	else if (dev_state->pasid_levels == 1)
+		free_pasid_states_level1(dev_state->states);
+	else if (dev_state->pasid_levels != 0)
+		BUG();
+
+	free_page((unsigned long)dev_state->states);
+}
+
+static struct pasid_state *mn_to_state(struct mmu_notifier *mn)
+{
+	return container_of(mn, struct pasid_state, mn);
+}
+
+static void __mn_flush_page(struct mmu_notifier *mn,
+			    unsigned long address)
+{
+	struct pasid_state *pasid_state;
+	struct device_state *dev_state;
+
+	pasid_state = mn_to_state(mn);
+	dev_state   = pasid_state->device_state;
+
+	amd_iommu_flush_page(dev_state->domain, pasid_state->pasid, address);
+}
+
+static int mn_clear_flush_young(struct mmu_notifier *mn,
+				struct mm_struct *mm,
+				unsigned long address)
+{
+	__mn_flush_page(mn, address);
+
+	return 0;
+}
+
+static void mn_change_pte(struct mmu_notifier *mn,
+			  struct mm_struct *mm,
+			  unsigned long address,
+			  pte_t pte)
+{
+	__mn_flush_page(mn, address);
+}
+
+static void mn_invalidate_page(struct mmu_notifier *mn,
+			       struct mm_struct *mm,
+			       unsigned long address)
+{
+	__mn_flush_page(mn, address);
+}
+
+static void mn_invalidate_range_start(struct mmu_notifier *mn,
+				      struct mm_struct *mm,
+				      unsigned long start, unsigned long end)
+{
+	struct pasid_state *pasid_state;
+	struct device_state *dev_state;
+
+	pasid_state = mn_to_state(mn);
+	dev_state   = pasid_state->device_state;
+
+	amd_iommu_domain_set_gcr3(dev_state->domain, pasid_state->pasid,
+				  __pa(empty_page_table));
+}
+
+static void mn_invalidate_range_end(struct mmu_notifier *mn,
+				    struct mm_struct *mm,
+				    unsigned long start, unsigned long end)
+{
+	struct pasid_state *pasid_state;
+	struct device_state *dev_state;
+
+	pasid_state = mn_to_state(mn);
+	dev_state   = pasid_state->device_state;
+
+	amd_iommu_domain_set_gcr3(dev_state->domain, pasid_state->pasid,
+				  __pa(pasid_state->mm->pgd));
+}
+
+static struct mmu_notifier_ops iommu_mn = {
+	.clear_flush_young      = mn_clear_flush_young,
+	.change_pte             = mn_change_pte,
+	.invalidate_page        = mn_invalidate_page,
+	.invalidate_range_start = mn_invalidate_range_start,
+	.invalidate_range_end   = mn_invalidate_range_end,
+};
+
+static void set_pri_tag_status(struct pasid_state *pasid_state,
+			       u16 tag, int status)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&pasid_state->lock, flags);
+	pasid_state->pri[tag].status = status;
+	spin_unlock_irqrestore(&pasid_state->lock, flags);
+}
+
+static void finish_pri_tag(struct device_state *dev_state,
+			   struct pasid_state *pasid_state,
+			   u16 tag)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&pasid_state->lock, flags);
+	if (atomic_dec_and_test(&pasid_state->pri[tag].inflight) &&
+	    pasid_state->pri[tag].finish) {
+		amd_iommu_complete_ppr(dev_state->pdev, pasid_state->pasid,
+				       pasid_state->pri[tag].status, tag);
+		pasid_state->pri[tag].finish = false;
+		pasid_state->pri[tag].status = PPR_SUCCESS;
+	}
+	spin_unlock_irqrestore(&pasid_state->lock, flags);
+}
+
+static void do_fault(struct work_struct *work)
+{
+	struct fault *fault = container_of(work, struct fault, work);
+	int npages, write;
+	struct page *page;
+
+	write = !!(fault->flags & PPR_FAULT_WRITE);
+
+	npages = get_user_pages(fault->state->task, fault->state->mm,
+				fault->address, 1, write, 0, &page, NULL);
+
+	if (npages == 1) {
+		put_page(page);
+	} else if (fault->dev_state->inv_ppr_cb) {
+		int status;
+
+		status = fault->dev_state->inv_ppr_cb(fault->dev_state->pdev,
+						      fault->pasid,
+						      fault->address,
+						      fault->flags);
+		switch (status) {
+		case AMD_IOMMU_INV_PRI_RSP_SUCCESS:
+			set_pri_tag_status(fault->state, fault->tag, PPR_SUCCESS);
+			break;
+		case AMD_IOMMU_INV_PRI_RSP_INVALID:
+			set_pri_tag_status(fault->state, fault->tag, PPR_INVALID);
+			break;
+		case AMD_IOMMU_INV_PRI_RSP_FAIL:
+			set_pri_tag_status(fault->state, fault->tag, PPR_FAILURE);
+			break;
+		default:
+			BUG();
+		}
+	} else {
+		set_pri_tag_status(fault->state, fault->tag, PPR_INVALID);
+	}
+
+	finish_pri_tag(fault->dev_state, fault->state, fault->tag);
+
+	put_pasid_state(fault->state);
+
+	kfree(fault);
+}
+
+static int ppr_notifier(struct notifier_block *nb, unsigned long e, void *data)
+{
+	struct amd_iommu_fault *iommu_fault;
+	struct pasid_state *pasid_state;
+	struct device_state *dev_state;
+	unsigned long flags;
+	struct fault *fault;
+	bool finish;
+	u16 tag;
+	int ret;
+
+	iommu_fault = data;
+	tag         = iommu_fault->tag & 0x1ff;
+	finish      = (iommu_fault->tag >> 9) & 1;
+
+	ret = NOTIFY_DONE;
+	dev_state = get_device_state(iommu_fault->device_id);
+	if (dev_state == NULL)
+		goto out;
+
+	pasid_state = get_pasid_state(dev_state, iommu_fault->pasid);
+	if (pasid_state == NULL) {
+		/* We know the device but not the PASID -> send INVALID */
+		amd_iommu_complete_ppr(dev_state->pdev, iommu_fault->pasid,
+				       PPR_INVALID, tag);
+		goto out_drop_state;
+	}
+
+	spin_lock_irqsave(&pasid_state->lock, flags);
+	atomic_inc(&pasid_state->pri[tag].inflight);
+	if (finish)
+		pasid_state->pri[tag].finish = true;
+	spin_unlock_irqrestore(&pasid_state->lock, flags);
+
+	fault = kzalloc(sizeof(*fault), GFP_ATOMIC);
+	if (fault == NULL) {
+		/* We are OOM - send success and let the device re-fault */
+		finish_pri_tag(dev_state, pasid_state, tag);
+		goto out_drop_state;
+	}
+
+	fault->dev_state = dev_state;
+	fault->address   = iommu_fault->address;
+	fault->state     = pasid_state;
+	fault->tag       = tag;
+	fault->finish    = finish;
+	fault->flags     = iommu_fault->flags;
+	INIT_WORK(&fault->work, do_fault);
+
+	queue_work(iommu_wq, &fault->work);
+
+	ret = NOTIFY_OK;
+
+out_drop_state:
+	put_device_state(dev_state);
+
+out:
+	return ret;
+}
+
+static struct notifier_block ppr_nb = {
+	.notifier_call = ppr_notifier,
+};
+
+static int task_exit(struct notifier_block *nb, unsigned long e, void *data)
+{
+	struct pasid_state *pasid_state;
+	struct task_struct *task;
+
+	task = data;
+
+	/*
+	 * Using this notifier is a hack - but there is no other choice
+	 * at the moment. What I really want is a sleeping notifier that
+	 * is called when an MM goes down. But such a notifier doesn't
+	 * exist yet. The notifier needs to sleep because it has to make
+	 * sure that the device does not use the PASID and the address
+	 * space anymore before it is destroyed. This includes waiting
+	 * for pending PRI requests to pass the workqueue. The
+	 * MMU-Notifiers would be a good fit, but they use RCU and so
+	 * they are not allowed to sleep. Lets see how we can solve this
+	 * in a more intelligent way in the future.
+	 */
+again:
+	spin_lock(&ps_lock);
+	list_for_each_entry(pasid_state, &pasid_state_list, list) {
+		struct device_state *dev_state;
+		int pasid;
+
+		if (pasid_state->task != task)
+			continue;
+
+		/* Drop Lock and unbind */
+		spin_unlock(&ps_lock);
+
+		dev_state = pasid_state->device_state;
+		pasid     = pasid_state->pasid;
+
+		if (pasid_state->device_state->inv_ctx_cb)
+			dev_state->inv_ctx_cb(dev_state->pdev, pasid);
+
+		unbind_pasid(dev_state, pasid);
+
+		/* Task may be in the list multiple times */
+		goto again;
+	}
+	spin_unlock(&ps_lock);
+
+	return NOTIFY_OK;
+}
+
+int amd_iommu_bind_pasid(struct pci_dev *pdev, int pasid,
+			 struct task_struct *task)
+{
+	struct pasid_state *pasid_state;
+	struct device_state *dev_state;
+	u16 devid;
+	int ret;
+
+	might_sleep();
+
+	if (!amd_iommu_v2_supported())
+		return -ENODEV;
+
+	devid     = device_id(pdev);
+	dev_state = get_device_state(devid);
+
+	if (dev_state == NULL)
+		return -EINVAL;
+
+	ret = -EINVAL;
+	if (pasid < 0 || pasid >= dev_state->max_pasids)
+		goto out;
+
+	ret = -ENOMEM;
+	pasid_state = kzalloc(sizeof(*pasid_state), GFP_KERNEL);
+	if (pasid_state == NULL)
+		goto out;
+
+	atomic_set(&pasid_state->count, 1);
+	init_waitqueue_head(&pasid_state->wq);
+	pasid_state->task         = task;
+	pasid_state->mm           = get_task_mm(task);
+	pasid_state->device_state = dev_state;
+	pasid_state->pasid        = pasid;
+	pasid_state->mn.ops       = &iommu_mn;
+
+	if (pasid_state->mm == NULL)
+		goto out_free;
+
+	mmu_notifier_register(&pasid_state->mn, pasid_state->mm);
+
+	ret = set_pasid_state(dev_state, pasid_state, pasid);
+	if (ret)
+		goto out_unregister;
+
+	ret = amd_iommu_domain_set_gcr3(dev_state->domain, pasid,
+					__pa(pasid_state->mm->pgd));
+	if (ret)
+		goto out_clear_state;
+
+	link_pasid_state(pasid_state);
+
+	return 0;
+
+out_clear_state:
+	clear_pasid_state(dev_state, pasid);
+
+out_unregister:
+	mmu_notifier_unregister(&pasid_state->mn, pasid_state->mm);
+
+out_free:
+	free_pasid_state(pasid_state);
+
+out:
+	put_device_state(dev_state);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_bind_pasid);
+
+void amd_iommu_unbind_pasid(struct pci_dev *pdev, int pasid)
+{
+	struct device_state *dev_state;
+	u16 devid;
+
+	might_sleep();
+
+	if (!amd_iommu_v2_supported())
+		return;
+
+	devid = device_id(pdev);
+	dev_state = get_device_state(devid);
+	if (dev_state == NULL)
+		return;
+
+	if (pasid < 0 || pasid >= dev_state->max_pasids)
+		goto out;
+
+	unbind_pasid(dev_state, pasid);
+
+out:
+	put_device_state(dev_state);
+}
+EXPORT_SYMBOL(amd_iommu_unbind_pasid);
+
+int amd_iommu_init_device(struct pci_dev *pdev, int pasids)
+{
+	struct device_state *dev_state;
+	unsigned long flags;
+	int ret, tmp;
+	u16 devid;
+
+	might_sleep();
+
+	if (!amd_iommu_v2_supported())
+		return -ENODEV;
+
+	if (pasids <= 0 || pasids > (PASID_MASK + 1))
+		return -EINVAL;
+
+	devid = device_id(pdev);
+
+	dev_state = kzalloc(sizeof(*dev_state), GFP_KERNEL);
+	if (dev_state == NULL)
+		return -ENOMEM;
+
+	spin_lock_init(&dev_state->lock);
+	init_waitqueue_head(&dev_state->wq);
+	dev_state->pdev = pdev;
+
+	tmp = pasids;
+	for (dev_state->pasid_levels = 0; (tmp - 1) & ~0x1ff; tmp >>= 9)
+		dev_state->pasid_levels += 1;
+
+	atomic_set(&dev_state->count, 1);
+	dev_state->max_pasids = pasids;
+
+	ret = -ENOMEM;
+	dev_state->states = (void *)get_zeroed_page(GFP_KERNEL);
+	if (dev_state->states == NULL)
+		goto out_free_dev_state;
+
+	dev_state->domain = iommu_domain_alloc(&pci_bus_type);
+	if (dev_state->domain == NULL)
+		goto out_free_states;
+
+	amd_iommu_domain_direct_map(dev_state->domain);
+
+	ret = amd_iommu_domain_enable_v2(dev_state->domain, pasids);
+	if (ret)
+		goto out_free_domain;
+
+	ret = iommu_attach_device(dev_state->domain, &pdev->dev);
+	if (ret != 0)
+		goto out_free_domain;
+
+	spin_lock_irqsave(&state_lock, flags);
+
+	if (state_table[devid] != NULL) {
+		spin_unlock_irqrestore(&state_lock, flags);
+		ret = -EBUSY;
+		goto out_free_domain;
+	}
+
+	state_table[devid] = dev_state;
+
+	spin_unlock_irqrestore(&state_lock, flags);
+
+	return 0;
+
+out_free_domain:
+	iommu_domain_free(dev_state->domain);
+
+out_free_states:
+	free_page((unsigned long)dev_state->states);
+
+out_free_dev_state:
+	kfree(dev_state);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_init_device);
+
+void amd_iommu_free_device(struct pci_dev *pdev)
+{
+	struct device_state *dev_state;
+	unsigned long flags;
+	u16 devid;
+
+	if (!amd_iommu_v2_supported())
+		return;
+
+	devid = device_id(pdev);
+
+	spin_lock_irqsave(&state_lock, flags);
+
+	dev_state = state_table[devid];
+	if (dev_state == NULL) {
+		spin_unlock_irqrestore(&state_lock, flags);
+		return;
+	}
+
+	state_table[devid] = NULL;
+
+	spin_unlock_irqrestore(&state_lock, flags);
+
+	/* Get rid of any remaining pasid states */
+	free_pasid_states(dev_state);
+
+	put_device_state_wait(dev_state);
+}
+EXPORT_SYMBOL(amd_iommu_free_device);
+
+int amd_iommu_set_invalid_ppr_cb(struct pci_dev *pdev,
+				 amd_iommu_invalid_ppr_cb cb)
+{
+	struct device_state *dev_state;
+	unsigned long flags;
+	u16 devid;
+	int ret;
+
+	if (!amd_iommu_v2_supported())
+		return -ENODEV;
+
+	devid = device_id(pdev);
+
+	spin_lock_irqsave(&state_lock, flags);
+
+	ret = -EINVAL;
+	dev_state = state_table[devid];
+	if (dev_state == NULL)
+		goto out_unlock;
+
+	dev_state->inv_ppr_cb = cb;
+
+	ret = 0;
+
+out_unlock:
+	spin_unlock_irqrestore(&state_lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_set_invalid_ppr_cb);
+
+int amd_iommu_set_invalidate_ctx_cb(struct pci_dev *pdev,
+				    amd_iommu_invalidate_ctx cb)
+{
+	struct device_state *dev_state;
+	unsigned long flags;
+	u16 devid;
+	int ret;
+
+	if (!amd_iommu_v2_supported())
+		return -ENODEV;
+
+	devid = device_id(pdev);
+
+	spin_lock_irqsave(&state_lock, flags);
+
+	ret = -EINVAL;
+	dev_state = state_table[devid];
+	if (dev_state == NULL)
+		goto out_unlock;
+
+	dev_state->inv_ctx_cb = cb;
+
+	ret = 0;
+
+out_unlock:
+	spin_unlock_irqrestore(&state_lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(amd_iommu_set_invalidate_ctx_cb);
+
+static int __init amd_iommu_v2_init(void)
+{
+	size_t state_table_size;
+	int ret;
+
+	pr_info("AMD IOMMUv2 driver by Joerg Roedel <joerg.roedel@amd.com>");
+
+	spin_lock_init(&state_lock);
+
+	state_table_size = MAX_DEVICES * sizeof(struct device_state *);
+	state_table = (void *)__get_free_pages(GFP_KERNEL | __GFP_ZERO,
+					       get_order(state_table_size));
+	if (state_table == NULL)
+		return -ENOMEM;
+
+	ret = -ENOMEM;
+	iommu_wq = create_workqueue("amd_iommu_v2");
+	if (iommu_wq == NULL)
+		goto out_free;
+
+	ret = -ENOMEM;
+	empty_page_table = (u64 *)get_zeroed_page(GFP_KERNEL);
+	if (empty_page_table == NULL)
+		goto out_destroy_wq;
+
+	amd_iommu_register_ppr_notifier(&ppr_nb);
+	profile_event_register(PROFILE_TASK_EXIT, &profile_nb);
+
+	return 0;
+
+out_destroy_wq:
+	destroy_workqueue(iommu_wq);
+
+out_free:
+	free_pages((unsigned long)state_table, get_order(state_table_size));
+
+	return ret;
+}
+
+static void __exit amd_iommu_v2_exit(void)
+{
+	struct device_state *dev_state;
+	size_t state_table_size;
+	int i;
+
+	profile_event_unregister(PROFILE_TASK_EXIT, &profile_nb);
+	amd_iommu_unregister_ppr_notifier(&ppr_nb);
+
+	flush_workqueue(iommu_wq);
+
+	/*
+	 * The loop below might call flush_workqueue(), so call
+	 * destroy_workqueue() after it
+	 */
+	for (i = 0; i < MAX_DEVICES; ++i) {
+		dev_state = get_device_state(i);
+
+		if (dev_state == NULL)
+			continue;
+
+		WARN_ON_ONCE(1);
+
+		put_device_state(dev_state);
+		amd_iommu_free_device(dev_state->pdev);
+	}
+
+	destroy_workqueue(iommu_wq);
+
+	state_table_size = MAX_DEVICES * sizeof(struct device_state *);
+	free_pages((unsigned long)state_table, get_order(state_table_size));
+
+	free_page((unsigned long)empty_page_table);
+}
+
+module_init(amd_iommu_v2_init);
+module_exit(amd_iommu_v2_exit);
diff --git a/drivers/iommu/dmar.c b/drivers/iommu/dmar.c
index 6dcc7e2d54de..35c1e17fce1d 100644
--- a/drivers/iommu/dmar.c
+++ b/drivers/iommu/dmar.c
@@ -46,7 +46,7 @@
  */
 LIST_HEAD(dmar_drhd_units);
 
-static struct acpi_table_header * __initdata dmar_tbl;
+struct acpi_table_header * __initdata dmar_tbl;
 static acpi_size dmar_tbl_size;
 
 static void __init dmar_register_drhd_unit(struct dmar_drhd_unit *drhd)
@@ -118,8 +118,8 @@ static int __init dmar_parse_one_dev_scope(struct acpi_dmar_device_scope *scope,
 	return 0;
 }
 
-static int __init dmar_parse_dev_scope(void *start, void *end, int *cnt,
-				       struct pci_dev ***devices, u16 segment)
+int __init dmar_parse_dev_scope(void *start, void *end, int *cnt,
+				struct pci_dev ***devices, u16 segment)
 {
 	struct acpi_dmar_device_scope *scope;
 	void * tmp = start;
@@ -217,133 +217,6 @@ static int __init dmar_parse_dev(struct dmar_drhd_unit *dmaru)
 	return ret;
 }
 
-#ifdef CONFIG_DMAR
-LIST_HEAD(dmar_rmrr_units);
-
-static void __init dmar_register_rmrr_unit(struct dmar_rmrr_unit *rmrr)
-{
-	list_add(&rmrr->list, &dmar_rmrr_units);
-}
-
-
-static int __init
-dmar_parse_one_rmrr(struct acpi_dmar_header *header)
-{
-	struct acpi_dmar_reserved_memory *rmrr;
-	struct dmar_rmrr_unit *rmrru;
-
-	rmrru = kzalloc(sizeof(*rmrru), GFP_KERNEL);
-	if (!rmrru)
-		return -ENOMEM;
-
-	rmrru->hdr = header;
-	rmrr = (struct acpi_dmar_reserved_memory *)header;
-	rmrru->base_address = rmrr->base_address;
-	rmrru->end_address = rmrr->end_address;
-
-	dmar_register_rmrr_unit(rmrru);
-	return 0;
-}
-
-static int __init
-rmrr_parse_dev(struct dmar_rmrr_unit *rmrru)
-{
-	struct acpi_dmar_reserved_memory *rmrr;
-	int ret;
-
-	rmrr = (struct acpi_dmar_reserved_memory *) rmrru->hdr;
-	ret = dmar_parse_dev_scope((void *)(rmrr + 1),
-		((void *)rmrr) + rmrr->header.length,
-		&rmrru->devices_cnt, &rmrru->devices, rmrr->segment);
-
-	if (ret || (rmrru->devices_cnt == 0)) {
-		list_del(&rmrru->list);
-		kfree(rmrru);
-	}
-	return ret;
-}
-
-static LIST_HEAD(dmar_atsr_units);
-
-static int __init dmar_parse_one_atsr(struct acpi_dmar_header *hdr)
-{
-	struct acpi_dmar_atsr *atsr;
-	struct dmar_atsr_unit *atsru;
-
-	atsr = container_of(hdr, struct acpi_dmar_atsr, header);
-	atsru = kzalloc(sizeof(*atsru), GFP_KERNEL);
-	if (!atsru)
-		return -ENOMEM;
-
-	atsru->hdr = hdr;
-	atsru->include_all = atsr->flags & 0x1;
-
-	list_add(&atsru->list, &dmar_atsr_units);
-
-	return 0;
-}
-
-static int __init atsr_parse_dev(struct dmar_atsr_unit *atsru)
-{
-	int rc;
-	struct acpi_dmar_atsr *atsr;
-
-	if (atsru->include_all)
-		return 0;
-
-	atsr = container_of(atsru->hdr, struct acpi_dmar_atsr, header);
-	rc = dmar_parse_dev_scope((void *)(atsr + 1),
-				(void *)atsr + atsr->header.length,
-				&atsru->devices_cnt, &atsru->devices,
-				atsr->segment);
-	if (rc || !atsru->devices_cnt) {
-		list_del(&atsru->list);
-		kfree(atsru);
-	}
-
-	return rc;
-}
-
-int dmar_find_matched_atsr_unit(struct pci_dev *dev)
-{
-	int i;
-	struct pci_bus *bus;
-	struct acpi_dmar_atsr *atsr;
-	struct dmar_atsr_unit *atsru;
-
-	dev = pci_physfn(dev);
-
-	list_for_each_entry(atsru, &dmar_atsr_units, list) {
-		atsr = container_of(atsru->hdr, struct acpi_dmar_atsr, header);
-		if (atsr->segment == pci_domain_nr(dev->bus))
-			goto found;
-	}
-
-	return 0;
-
-found:
-	for (bus = dev->bus; bus; bus = bus->parent) {
-		struct pci_dev *bridge = bus->self;
-
-		if (!bridge || !pci_is_pcie(bridge) ||
-		    bridge->pcie_type == PCI_EXP_TYPE_PCI_BRIDGE)
-			return 0;
-
-		if (bridge->pcie_type == PCI_EXP_TYPE_ROOT_PORT) {
-			for (i = 0; i < atsru->devices_cnt; i++)
-				if (atsru->devices[i] == bridge)
-					return 1;
-			break;
-		}
-	}
-
-	if (atsru->include_all)
-		return 1;
-
-	return 0;
-}
-#endif
-
 #ifdef CONFIG_ACPI_NUMA
 static int __init
 dmar_parse_one_rhsa(struct acpi_dmar_header *header)
@@ -484,14 +357,10 @@ parse_dmar_table(void)
 			ret = dmar_parse_one_drhd(entry_header);
 			break;
 		case ACPI_DMAR_TYPE_RESERVED_MEMORY:
-#ifdef CONFIG_DMAR
 			ret = dmar_parse_one_rmrr(entry_header);
-#endif
 			break;
 		case ACPI_DMAR_TYPE_ATSR:
-#ifdef CONFIG_DMAR
 			ret = dmar_parse_one_atsr(entry_header);
-#endif
 			break;
 		case ACPI_DMAR_HARDWARE_AFFINITY:
 #ifdef CONFIG_ACPI_NUMA
@@ -557,34 +426,31 @@ dmar_find_matched_drhd_unit(struct pci_dev *dev)
 
 int __init dmar_dev_scope_init(void)
 {
+	static int dmar_dev_scope_initialized;
 	struct dmar_drhd_unit *drhd, *drhd_n;
 	int ret = -ENODEV;
 
+	if (dmar_dev_scope_initialized)
+		return dmar_dev_scope_initialized;
+
+	if (list_empty(&dmar_drhd_units))
+		goto fail;
+
 	list_for_each_entry_safe(drhd, drhd_n, &dmar_drhd_units, list) {
 		ret = dmar_parse_dev(drhd);
 		if (ret)
-			return ret;
+			goto fail;
 	}
 
-#ifdef CONFIG_DMAR
-	{
-		struct dmar_rmrr_unit *rmrr, *rmrr_n;
-		struct dmar_atsr_unit *atsr, *atsr_n;
-
-		list_for_each_entry_safe(rmrr, rmrr_n, &dmar_rmrr_units, list) {
-			ret = rmrr_parse_dev(rmrr);
-			if (ret)
-				return ret;
-		}
+	ret = dmar_parse_rmrr_atsr_dev();
+	if (ret)
+		goto fail;
 
-		list_for_each_entry_safe(atsr, atsr_n, &dmar_atsr_units, list) {
-			ret = atsr_parse_dev(atsr);
-			if (ret)
-				return ret;
-		}
-	}
-#endif
+	dmar_dev_scope_initialized = 1;
+	return 0;
 
+fail:
+	dmar_dev_scope_initialized = ret;
 	return ret;
 }
 
@@ -611,14 +477,6 @@ int __init dmar_table_init(void)
 		return -ENODEV;
 	}
 
-#ifdef CONFIG_DMAR
-	if (list_empty(&dmar_rmrr_units))
-		printk(KERN_INFO PREFIX "No RMRR found\n");
-
-	if (list_empty(&dmar_atsr_units))
-		printk(KERN_INFO PREFIX "No ATSR found\n");
-#endif
-
 	return 0;
 }
 
@@ -682,9 +540,6 @@ int __init check_zero_address(void)
 	return 1;
 
 failed:
-#ifdef CONFIG_DMAR
-	dmar_disabled = 1;
-#endif
 	return 0;
 }
 
@@ -696,22 +551,21 @@ int __init detect_intel_iommu(void)
 	if (ret)
 		ret = check_zero_address();
 	{
-#ifdef CONFIG_INTR_REMAP
 		struct acpi_table_dmar *dmar;
 
 		dmar = (struct acpi_table_dmar *) dmar_tbl;
-		if (ret && cpu_has_x2apic && dmar->flags & 0x1)
+
+		if (ret && intr_remapping_enabled && cpu_has_x2apic &&
+		    dmar->flags & 0x1)
 			printk(KERN_INFO
-			       "Queued invalidation will be enabled to support "
-			       "x2apic and Intr-remapping.\n");
-#endif
-#ifdef CONFIG_DMAR
+			       "Queued invalidation will be enabled to support x2apic and Intr-remapping.\n");
+
 		if (ret && !no_iommu && !iommu_detected && !dmar_disabled) {
 			iommu_detected = 1;
 			/* Make sure ACS will be enabled */
 			pci_request_acs();
 		}
-#endif
+
 #ifdef CONFIG_X86
 		if (ret)
 			x86_init.iommu.iommu_init = intel_iommu_init;
@@ -758,7 +612,6 @@ int alloc_iommu(struct dmar_drhd_unit *drhd)
 		goto err_unmap;
 	}
 
-#ifdef CONFIG_DMAR
 	agaw = iommu_calculate_agaw(iommu);
 	if (agaw < 0) {
 		printk(KERN_ERR
@@ -773,7 +626,6 @@ int alloc_iommu(struct dmar_drhd_unit *drhd)
 			iommu->seq_id);
 		goto err_unmap;
 	}
-#endif
 	iommu->agaw = agaw;
 	iommu->msagaw = msagaw;
 
@@ -800,7 +652,7 @@ int alloc_iommu(struct dmar_drhd_unit *drhd)
 		(unsigned long long)iommu->cap,
 		(unsigned long long)iommu->ecap);
 
-	spin_lock_init(&iommu->register_lock);
+	raw_spin_lock_init(&iommu->register_lock);
 
 	drhd->iommu = iommu;
 	return 0;
@@ -817,9 +669,7 @@ void free_iommu(struct intel_iommu *iommu)
 	if (!iommu)
 		return;
 
-#ifdef CONFIG_DMAR
 	free_dmar_iommu(iommu);
-#endif
 
 	if (iommu->reg)
 		iounmap(iommu->reg);
@@ -921,11 +771,11 @@ int qi_submit_sync(struct qi_desc *desc, struct intel_iommu *iommu)
 restart:
 	rc = 0;
 
-	spin_lock_irqsave(&qi->q_lock, flags);
+	raw_spin_lock_irqsave(&qi->q_lock, flags);
 	while (qi->free_cnt < 3) {
-		spin_unlock_irqrestore(&qi->q_lock, flags);
+		raw_spin_unlock_irqrestore(&qi->q_lock, flags);
 		cpu_relax();
-		spin_lock_irqsave(&qi->q_lock, flags);
+		raw_spin_lock_irqsave(&qi->q_lock, flags);
 	}
 
 	index = qi->free_head;
@@ -965,15 +815,15 @@ restart:
 		if (rc)
 			break;
 
-		spin_unlock(&qi->q_lock);
+		raw_spin_unlock(&qi->q_lock);
 		cpu_relax();
-		spin_lock(&qi->q_lock);
+		raw_spin_lock(&qi->q_lock);
 	}
 
 	qi->desc_status[index] = QI_DONE;
 
 	reclaim_free_desc(qi);
-	spin_unlock_irqrestore(&qi->q_lock, flags);
+	raw_spin_unlock_irqrestore(&qi->q_lock, flags);
 
 	if (rc == -EAGAIN)
 		goto restart;
@@ -1062,7 +912,7 @@ void dmar_disable_qi(struct intel_iommu *iommu)
 	if (!ecap_qis(iommu->ecap))
 		return;
 
-	spin_lock_irqsave(&iommu->register_lock, flags);
+	raw_spin_lock_irqsave(&iommu->register_lock, flags);
 
 	sts =  dmar_readq(iommu->reg + DMAR_GSTS_REG);
 	if (!(sts & DMA_GSTS_QIES))
@@ -1082,7 +932,7 @@ void dmar_disable_qi(struct intel_iommu *iommu)
 	IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG, readl,
 		      !(sts & DMA_GSTS_QIES), sts);
 end:
-	spin_unlock_irqrestore(&iommu->register_lock, flags);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flags);
 }
 
 /*
@@ -1097,7 +947,7 @@ static void __dmar_enable_qi(struct intel_iommu *iommu)
 	qi->free_head = qi->free_tail = 0;
 	qi->free_cnt = QI_LENGTH;
 
-	spin_lock_irqsave(&iommu->register_lock, flags);
+	raw_spin_lock_irqsave(&iommu->register_lock, flags);
 
 	/* write zero to the tail reg */
 	writel(0, iommu->reg + DMAR_IQT_REG);
@@ -1110,7 +960,7 @@ static void __dmar_enable_qi(struct intel_iommu *iommu)
 	/* Make sure hardware complete it */
 	IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG, readl, (sts & DMA_GSTS_QIES), sts);
 
-	spin_unlock_irqrestore(&iommu->register_lock, flags);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flags);
 }
 
 /*
@@ -1159,7 +1009,7 @@ int dmar_enable_qi(struct intel_iommu *iommu)
 	qi->free_head = qi->free_tail = 0;
 	qi->free_cnt = QI_LENGTH;
 
-	spin_lock_init(&qi->q_lock);
+	raw_spin_lock_init(&qi->q_lock);
 
 	__dmar_enable_qi(iommu);
 
@@ -1225,11 +1075,11 @@ void dmar_msi_unmask(struct irq_data *data)
 	unsigned long flag;
 
 	/* unmask it */
-	spin_lock_irqsave(&iommu->register_lock, flag);
+	raw_spin_lock_irqsave(&iommu->register_lock, flag);
 	writel(0, iommu->reg + DMAR_FECTL_REG);
 	/* Read a reg to force flush the post write */
 	readl(iommu->reg + DMAR_FECTL_REG);
-	spin_unlock_irqrestore(&iommu->register_lock, flag);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flag);
 }
 
 void dmar_msi_mask(struct irq_data *data)
@@ -1238,11 +1088,11 @@ void dmar_msi_mask(struct irq_data *data)
 	struct intel_iommu *iommu = irq_data_get_irq_handler_data(data);
 
 	/* mask it */
-	spin_lock_irqsave(&iommu->register_lock, flag);
+	raw_spin_lock_irqsave(&iommu->register_lock, flag);
 	writel(DMA_FECTL_IM, iommu->reg + DMAR_FECTL_REG);
 	/* Read a reg to force flush the post write */
 	readl(iommu->reg + DMAR_FECTL_REG);
-	spin_unlock_irqrestore(&iommu->register_lock, flag);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flag);
 }
 
 void dmar_msi_write(int irq, struct msi_msg *msg)
@@ -1250,11 +1100,11 @@ void dmar_msi_write(int irq, struct msi_msg *msg)
 	struct intel_iommu *iommu = irq_get_handler_data(irq);
 	unsigned long flag;
 
-	spin_lock_irqsave(&iommu->register_lock, flag);
+	raw_spin_lock_irqsave(&iommu->register_lock, flag);
 	writel(msg->data, iommu->reg + DMAR_FEDATA_REG);
 	writel(msg->address_lo, iommu->reg + DMAR_FEADDR_REG);
 	writel(msg->address_hi, iommu->reg + DMAR_FEUADDR_REG);
-	spin_unlock_irqrestore(&iommu->register_lock, flag);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flag);
 }
 
 void dmar_msi_read(int irq, struct msi_msg *msg)
@@ -1262,11 +1112,11 @@ void dmar_msi_read(int irq, struct msi_msg *msg)
 	struct intel_iommu *iommu = irq_get_handler_data(irq);
 	unsigned long flag;
 
-	spin_lock_irqsave(&iommu->register_lock, flag);
+	raw_spin_lock_irqsave(&iommu->register_lock, flag);
 	msg->data = readl(iommu->reg + DMAR_FEDATA_REG);
 	msg->address_lo = readl(iommu->reg + DMAR_FEADDR_REG);
 	msg->address_hi = readl(iommu->reg + DMAR_FEUADDR_REG);
-	spin_unlock_irqrestore(&iommu->register_lock, flag);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flag);
 }
 
 static int dmar_fault_do_one(struct intel_iommu *iommu, int type,
@@ -1303,7 +1153,7 @@ irqreturn_t dmar_fault(int irq, void *dev_id)
 	u32 fault_status;
 	unsigned long flag;
 
-	spin_lock_irqsave(&iommu->register_lock, flag);
+	raw_spin_lock_irqsave(&iommu->register_lock, flag);
 	fault_status = readl(iommu->reg + DMAR_FSTS_REG);
 	if (fault_status)
 		printk(KERN_ERR "DRHD: handling fault status reg %x\n",
@@ -1342,7 +1192,7 @@ irqreturn_t dmar_fault(int irq, void *dev_id)
 		writel(DMA_FRCD_F, iommu->reg + reg +
 			fault_index * PRIMARY_FAULT_REG_LEN + 12);
 
-		spin_unlock_irqrestore(&iommu->register_lock, flag);
+		raw_spin_unlock_irqrestore(&iommu->register_lock, flag);
 
 		dmar_fault_do_one(iommu, type, fault_reason,
 				source_id, guest_addr);
@@ -1350,14 +1200,14 @@ irqreturn_t dmar_fault(int irq, void *dev_id)
 		fault_index++;
 		if (fault_index >= cap_num_fault_regs(iommu->cap))
 			fault_index = 0;
-		spin_lock_irqsave(&iommu->register_lock, flag);
+		raw_spin_lock_irqsave(&iommu->register_lock, flag);
 	}
 clear_rest:
 	/* clear all the other faults */
 	fault_status = readl(iommu->reg + DMAR_FSTS_REG);
 	writel(fault_status, iommu->reg + DMAR_FSTS_REG);
 
-	spin_unlock_irqrestore(&iommu->register_lock, flag);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flag);
 	return IRQ_HANDLED;
 }
 
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index a88f3cbb100b..77e3b917c8da 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -24,6 +24,7 @@
 #include <linux/init.h>
 #include <linux/bitmap.h>
 #include <linux/debugfs.h>
+#include <linux/export.h>
 #include <linux/slab.h>
 #include <linux/irq.h>
 #include <linux/interrupt.h>
@@ -77,6 +78,24 @@
 #define LEVEL_STRIDE		(9)
 #define LEVEL_MASK		(((u64)1 << LEVEL_STRIDE) - 1)
 
+/*
+ * This bitmap is used to advertise the page sizes our hardware support
+ * to the IOMMU core, which will then use this information to split
+ * physically contiguous memory regions it is mapping into page sizes
+ * that we support.
+ *
+ * Traditionally the IOMMU core just handed us the mappings directly,
+ * after making sure the size is an order of a 4KiB page and that the
+ * mapping has natural alignment.
+ *
+ * To retain this behavior, we currently advertise that we support
+ * all page sizes that are an order of 4KiB.
+ *
+ * If at some point we'd like to utilize the IOMMU core's new behavior,
+ * we could change this to advertise the real page sizes we support.
+ */
+#define INTEL_IOMMU_PGSIZES	(~0xFFFUL)
+
 static inline int agaw_to_level(int agaw)
 {
 	return agaw + 2;
@@ -398,11 +417,14 @@ static long list_size;
 
 static void domain_remove_dev_info(struct dmar_domain *domain);
 
-#ifdef CONFIG_DMAR_DEFAULT_ON
+#ifdef CONFIG_INTEL_IOMMU_DEFAULT_ON
 int dmar_disabled = 0;
 #else
 int dmar_disabled = 1;
-#endif /*CONFIG_DMAR_DEFAULT_ON*/
+#endif /*CONFIG_INTEL_IOMMU_DEFAULT_ON*/
+
+int intel_iommu_enabled = 0;
+EXPORT_SYMBOL_GPL(intel_iommu_enabled);
 
 static int dmar_map_gfx = 1;
 static int dmar_forcedac;
@@ -939,7 +961,7 @@ static void iommu_set_root_entry(struct intel_iommu *iommu)
 
 	addr = iommu->root_entry;
 
-	spin_lock_irqsave(&iommu->register_lock, flag);
+	raw_spin_lock_irqsave(&iommu->register_lock, flag);
 	dmar_writeq(iommu->reg + DMAR_RTADDR_REG, virt_to_phys(addr));
 
 	writel(iommu->gcmd | DMA_GCMD_SRTP, iommu->reg + DMAR_GCMD_REG);
@@ -948,7 +970,7 @@ static void iommu_set_root_entry(struct intel_iommu *iommu)
 	IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG,
 		      readl, (sts & DMA_GSTS_RTPS), sts);
 
-	spin_unlock_irqrestore(&iommu->register_lock, flag);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flag);
 }
 
 static void iommu_flush_write_buffer(struct intel_iommu *iommu)
@@ -959,14 +981,14 @@ static void iommu_flush_write_buffer(struct intel_iommu *iommu)
 	if (!rwbf_quirk && !cap_rwbf(iommu->cap))
 		return;
 
-	spin_lock_irqsave(&iommu->register_lock, flag);
+	raw_spin_lock_irqsave(&iommu->register_lock, flag);
 	writel(iommu->gcmd | DMA_GCMD_WBF, iommu->reg + DMAR_GCMD_REG);
 
 	/* Make sure hardware complete it */
 	IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG,
 		      readl, (!(val & DMA_GSTS_WBFS)), val);
 
-	spin_unlock_irqrestore(&iommu->register_lock, flag);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flag);
 }
 
 /* return value determine if we need a write buffer flush */
@@ -993,14 +1015,14 @@ static void __iommu_flush_context(struct intel_iommu *iommu,
 	}
 	val |= DMA_CCMD_ICC;
 
-	spin_lock_irqsave(&iommu->register_lock, flag);
+	raw_spin_lock_irqsave(&iommu->register_lock, flag);
 	dmar_writeq(iommu->reg + DMAR_CCMD_REG, val);
 
 	/* Make sure hardware complete it */
 	IOMMU_WAIT_OP(iommu, DMAR_CCMD_REG,
 		dmar_readq, (!(val & DMA_CCMD_ICC)), val);
 
-	spin_unlock_irqrestore(&iommu->register_lock, flag);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flag);
 }
 
 /* return value determine if we need a write buffer flush */
@@ -1039,7 +1061,7 @@ static void __iommu_flush_iotlb(struct intel_iommu *iommu, u16 did,
 	if (cap_write_drain(iommu->cap))
 		val |= DMA_TLB_WRITE_DRAIN;
 
-	spin_lock_irqsave(&iommu->register_lock, flag);
+	raw_spin_lock_irqsave(&iommu->register_lock, flag);
 	/* Note: Only uses first TLB reg currently */
 	if (val_iva)
 		dmar_writeq(iommu->reg + tlb_offset, val_iva);
@@ -1049,7 +1071,7 @@ static void __iommu_flush_iotlb(struct intel_iommu *iommu, u16 did,
 	IOMMU_WAIT_OP(iommu, tlb_offset + 8,
 		dmar_readq, (!(val & DMA_TLB_IVT)), val);
 
-	spin_unlock_irqrestore(&iommu->register_lock, flag);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flag);
 
 	/* check IOTLB invalidation granularity */
 	if (DMA_TLB_IAIG(val) == 0)
@@ -1165,7 +1187,7 @@ static void iommu_disable_protect_mem_regions(struct intel_iommu *iommu)
 	u32 pmen;
 	unsigned long flags;
 
-	spin_lock_irqsave(&iommu->register_lock, flags);
+	raw_spin_lock_irqsave(&iommu->register_lock, flags);
 	pmen = readl(iommu->reg + DMAR_PMEN_REG);
 	pmen &= ~DMA_PMEN_EPM;
 	writel(pmen, iommu->reg + DMAR_PMEN_REG);
@@ -1174,7 +1196,7 @@ static void iommu_disable_protect_mem_regions(struct intel_iommu *iommu)
 	IOMMU_WAIT_OP(iommu, DMAR_PMEN_REG,
 		readl, !(pmen & DMA_PMEN_PRS), pmen);
 
-	spin_unlock_irqrestore(&iommu->register_lock, flags);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flags);
 }
 
 static int iommu_enable_translation(struct intel_iommu *iommu)
@@ -1182,7 +1204,7 @@ static int iommu_enable_translation(struct intel_iommu *iommu)
 	u32 sts;
 	unsigned long flags;
 
-	spin_lock_irqsave(&iommu->register_lock, flags);
+	raw_spin_lock_irqsave(&iommu->register_lock, flags);
 	iommu->gcmd |= DMA_GCMD_TE;
 	writel(iommu->gcmd, iommu->reg + DMAR_GCMD_REG);
 
@@ -1190,7 +1212,7 @@ static int iommu_enable_translation(struct intel_iommu *iommu)
 	IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG,
 		      readl, (sts & DMA_GSTS_TES), sts);
 
-	spin_unlock_irqrestore(&iommu->register_lock, flags);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flags);
 	return 0;
 }
 
@@ -1199,7 +1221,7 @@ static int iommu_disable_translation(struct intel_iommu *iommu)
 	u32 sts;
 	unsigned long flag;
 
-	spin_lock_irqsave(&iommu->register_lock, flag);
+	raw_spin_lock_irqsave(&iommu->register_lock, flag);
 	iommu->gcmd &= ~DMA_GCMD_TE;
 	writel(iommu->gcmd, iommu->reg + DMAR_GCMD_REG);
 
@@ -1207,7 +1229,7 @@ static int iommu_disable_translation(struct intel_iommu *iommu)
 	IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG,
 		      readl, (!(sts & DMA_GSTS_TES)), sts);
 
-	spin_unlock_irqrestore(&iommu->register_lock, flag);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flag);
 	return 0;
 }
 
@@ -2157,7 +2179,7 @@ static inline int iommu_prepare_rmrr_dev(struct dmar_rmrr_unit *rmrr,
 		rmrr->end_address);
 }
 
-#ifdef CONFIG_DMAR_FLOPPY_WA
+#ifdef CONFIG_INTEL_IOMMU_FLOPPY_WA
 static inline void iommu_prepare_isa(void)
 {
 	struct pci_dev *pdev;
@@ -2180,7 +2202,7 @@ static inline void iommu_prepare_isa(void)
 {
 	return;
 }
-#endif /* !CONFIG_DMAR_FLPY_WA */
+#endif /* !CONFIG_INTEL_IOMMU_FLPY_WA */
 
 static int md_domain_init(struct dmar_domain *domain, int guest_width);
 
@@ -2491,7 +2513,7 @@ static int __init init_dmars(void)
 	if (iommu_pass_through)
 		iommu_identity_mapping |= IDENTMAP_ALL;
 
-#ifdef CONFIG_DMAR_BROKEN_GFX_WA
+#ifdef CONFIG_INTEL_IOMMU_BROKEN_GFX_WA
 	iommu_identity_mapping |= IDENTMAP_GFX;
 #endif
 
@@ -3329,7 +3351,7 @@ static int iommu_suspend(void)
 	for_each_active_iommu(iommu, drhd) {
 		iommu_disable_translation(iommu);
 
-		spin_lock_irqsave(&iommu->register_lock, flag);
+		raw_spin_lock_irqsave(&iommu->register_lock, flag);
 
 		iommu->iommu_state[SR_DMAR_FECTL_REG] =
 			readl(iommu->reg + DMAR_FECTL_REG);
@@ -3340,7 +3362,7 @@ static int iommu_suspend(void)
 		iommu->iommu_state[SR_DMAR_FEUADDR_REG] =
 			readl(iommu->reg + DMAR_FEUADDR_REG);
 
-		spin_unlock_irqrestore(&iommu->register_lock, flag);
+		raw_spin_unlock_irqrestore(&iommu->register_lock, flag);
 	}
 	return 0;
 
@@ -3367,7 +3389,7 @@ static void iommu_resume(void)
 
 	for_each_active_iommu(iommu, drhd) {
 
-		spin_lock_irqsave(&iommu->register_lock, flag);
+		raw_spin_lock_irqsave(&iommu->register_lock, flag);
 
 		writel(iommu->iommu_state[SR_DMAR_FECTL_REG],
 			iommu->reg + DMAR_FECTL_REG);
@@ -3378,7 +3400,7 @@ static void iommu_resume(void)
 		writel(iommu->iommu_state[SR_DMAR_FEUADDR_REG],
 			iommu->reg + DMAR_FEUADDR_REG);
 
-		spin_unlock_irqrestore(&iommu->register_lock, flag);
+		raw_spin_unlock_irqrestore(&iommu->register_lock, flag);
 	}
 
 	for_each_active_iommu(iommu, drhd)
@@ -3399,6 +3421,151 @@ static void __init init_iommu_pm_ops(void)
 static inline void init_iommu_pm_ops(void) {}
 #endif	/* CONFIG_PM */
 
+LIST_HEAD(dmar_rmrr_units);
+
+static void __init dmar_register_rmrr_unit(struct dmar_rmrr_unit *rmrr)
+{
+	list_add(&rmrr->list, &dmar_rmrr_units);
+}
+
+
+int __init dmar_parse_one_rmrr(struct acpi_dmar_header *header)
+{
+	struct acpi_dmar_reserved_memory *rmrr;
+	struct dmar_rmrr_unit *rmrru;
+
+	rmrru = kzalloc(sizeof(*rmrru), GFP_KERNEL);
+	if (!rmrru)
+		return -ENOMEM;
+
+	rmrru->hdr = header;
+	rmrr = (struct acpi_dmar_reserved_memory *)header;
+	rmrru->base_address = rmrr->base_address;
+	rmrru->end_address = rmrr->end_address;
+
+	dmar_register_rmrr_unit(rmrru);
+	return 0;
+}
+
+static int __init
+rmrr_parse_dev(struct dmar_rmrr_unit *rmrru)
+{
+	struct acpi_dmar_reserved_memory *rmrr;
+	int ret;
+
+	rmrr = (struct acpi_dmar_reserved_memory *) rmrru->hdr;
+	ret = dmar_parse_dev_scope((void *)(rmrr + 1),
+		((void *)rmrr) + rmrr->header.length,
+		&rmrru->devices_cnt, &rmrru->devices, rmrr->segment);
+
+	if (ret || (rmrru->devices_cnt == 0)) {
+		list_del(&rmrru->list);
+		kfree(rmrru);
+	}
+	return ret;
+}
+
+static LIST_HEAD(dmar_atsr_units);
+
+int __init dmar_parse_one_atsr(struct acpi_dmar_header *hdr)
+{
+	struct acpi_dmar_atsr *atsr;
+	struct dmar_atsr_unit *atsru;
+
+	atsr = container_of(hdr, struct acpi_dmar_atsr, header);
+	atsru = kzalloc(sizeof(*atsru), GFP_KERNEL);
+	if (!atsru)
+		return -ENOMEM;
+
+	atsru->hdr = hdr;
+	atsru->include_all = atsr->flags & 0x1;
+
+	list_add(&atsru->list, &dmar_atsr_units);
+
+	return 0;
+}
+
+static int __init atsr_parse_dev(struct dmar_atsr_unit *atsru)
+{
+	int rc;
+	struct acpi_dmar_atsr *atsr;
+
+	if (atsru->include_all)
+		return 0;
+
+	atsr = container_of(atsru->hdr, struct acpi_dmar_atsr, header);
+	rc = dmar_parse_dev_scope((void *)(atsr + 1),
+				(void *)atsr + atsr->header.length,
+				&atsru->devices_cnt, &atsru->devices,
+				atsr->segment);
+	if (rc || !atsru->devices_cnt) {
+		list_del(&atsru->list);
+		kfree(atsru);
+	}
+
+	return rc;
+}
+
+int dmar_find_matched_atsr_unit(struct pci_dev *dev)
+{
+	int i;
+	struct pci_bus *bus;
+	struct acpi_dmar_atsr *atsr;
+	struct dmar_atsr_unit *atsru;
+
+	dev = pci_physfn(dev);
+
+	list_for_each_entry(atsru, &dmar_atsr_units, list) {
+		atsr = container_of(atsru->hdr, struct acpi_dmar_atsr, header);
+		if (atsr->segment == pci_domain_nr(dev->bus))
+			goto found;
+	}
+
+	return 0;
+
+found:
+	for (bus = dev->bus; bus; bus = bus->parent) {
+		struct pci_dev *bridge = bus->self;
+
+		if (!bridge || !pci_is_pcie(bridge) ||
+		    bridge->pcie_type == PCI_EXP_TYPE_PCI_BRIDGE)
+			return 0;
+
+		if (bridge->pcie_type == PCI_EXP_TYPE_ROOT_PORT) {
+			for (i = 0; i < atsru->devices_cnt; i++)
+				if (atsru->devices[i] == bridge)
+					return 1;
+			break;
+		}
+	}
+
+	if (atsru->include_all)
+		return 1;
+
+	return 0;
+}
+
+int __init dmar_parse_rmrr_atsr_dev(void)
+{
+	struct dmar_rmrr_unit *rmrr, *rmrr_n;
+	struct dmar_atsr_unit *atsr, *atsr_n;
+	int ret = 0;
+
+	list_for_each_entry_safe(rmrr, rmrr_n, &dmar_rmrr_units, list) {
+		ret = rmrr_parse_dev(rmrr);
+		if (ret)
+			return ret;
+	}
+
+	list_for_each_entry_safe(atsr, atsr_n, &dmar_atsr_units, list) {
+		ret = atsr_parse_dev(atsr);
+		if (ret)
+			return ret;
+	}
+
+	return ret;
+}
+
 /*
  * Here we only respond to action of unbound device from driver.
  *
@@ -3448,16 +3615,12 @@ int __init intel_iommu_init(void)
 		return 	-ENODEV;
 	}
 
-	if (dmar_dev_scope_init()) {
+	if (dmar_dev_scope_init() < 0) {
 		if (force_on)
 			panic("tboot: Failed to initialize DMAR device scope\n");
 		return 	-ENODEV;
 	}
 
-	/*
-	 * Check the need for DMA-remapping initialization now.
-	 * Above initialization will also be used by Interrupt-remapping.
-	 */
 	if (no_iommu || dmar_disabled)
 		return -ENODEV;
 
@@ -3467,6 +3630,12 @@ int __init intel_iommu_init(void)
 		return 	-ENODEV;
 	}
 
+	if (list_empty(&dmar_rmrr_units))
+		printk(KERN_INFO "DMAR: No RMRR found\n");
+
+	if (list_empty(&dmar_atsr_units))
+		printk(KERN_INFO "DMAR: No ATSR found\n");
+
 	if (dmar_init_reserved_ranges()) {
 		if (force_on)
 			panic("tboot: Failed to reserve iommu ranges\n");
@@ -3495,10 +3664,12 @@ int __init intel_iommu_init(void)
 
 	init_iommu_pm_ops();
 
-	register_iommu(&intel_iommu_ops);
+	bus_set_iommu(&pci_bus_type, &intel_iommu_ops);
 
 	bus_register_notifier(&pci_bus_type, &device_nb);
 
+	intel_iommu_enabled = 1;
+
 	return 0;
 }
 
@@ -3831,12 +4002,11 @@ static void intel_iommu_detach_device(struct iommu_domain *domain,
 
 static int intel_iommu_map(struct iommu_domain *domain,
 			   unsigned long iova, phys_addr_t hpa,
-			   int gfp_order, int iommu_prot)
+			   size_t size, int iommu_prot)
 {
 	struct dmar_domain *dmar_domain = domain->priv;
 	u64 max_addr;
 	int prot = 0;
-	size_t size;
 	int ret;
 
 	if (iommu_prot & IOMMU_READ)
@@ -3846,7 +4016,6 @@ static int intel_iommu_map(struct iommu_domain *domain,
 	if ((iommu_prot & IOMMU_CACHE) && dmar_domain->iommu_snooping)
 		prot |= DMA_PTE_SNP;
 
-	size     = PAGE_SIZE << gfp_order;
 	max_addr = iova + size;
 	if (dmar_domain->max_addr < max_addr) {
 		u64 end;
@@ -3869,11 +4038,10 @@ static int intel_iommu_map(struct iommu_domain *domain,
 	return ret;
 }
 
-static int intel_iommu_unmap(struct iommu_domain *domain,
-			     unsigned long iova, int gfp_order)
+static size_t intel_iommu_unmap(struct iommu_domain *domain,
+			     unsigned long iova, size_t size)
 {
 	struct dmar_domain *dmar_domain = domain->priv;
-	size_t size = PAGE_SIZE << gfp_order;
 	int order;
 
 	order = dma_pte_clear_range(dmar_domain, iova >> VTD_PAGE_SHIFT,
@@ -3882,7 +4050,7 @@ static int intel_iommu_unmap(struct iommu_domain *domain,
 	if (dmar_domain->max_addr == iova + size)
 		dmar_domain->max_addr = iova;
 
-	return order;
+	return PAGE_SIZE << order;
 }
 
 static phys_addr_t intel_iommu_iova_to_phys(struct iommu_domain *domain,
@@ -3912,6 +4080,54 @@ static int intel_iommu_domain_has_cap(struct iommu_domain *domain,
 	return 0;
 }
 
+/*
+ * Group numbers are arbitrary.  Device with the same group number
+ * indicate the iommu cannot differentiate between them.  To avoid
+ * tracking used groups we just use the seg|bus|devfn of the lowest
+ * level we're able to differentiate devices
+ */
+static int intel_iommu_device_group(struct device *dev, unsigned int *groupid)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct pci_dev *bridge;
+	union {
+		struct {
+			u8 devfn;
+			u8 bus;
+			u16 segment;
+		} pci;
+		u32 group;
+	} id;
+
+	if (iommu_no_mapping(dev))
+		return -ENODEV;
+
+	id.pci.segment = pci_domain_nr(pdev->bus);
+	id.pci.bus = pdev->bus->number;
+	id.pci.devfn = pdev->devfn;
+
+	if (!device_to_iommu(id.pci.segment, id.pci.bus, id.pci.devfn))
+		return -ENODEV;
+
+	bridge = pci_find_upstream_pcie_bridge(pdev);
+	if (bridge) {
+		if (pci_is_pcie(bridge)) {
+			id.pci.bus = bridge->subordinate->number;
+			id.pci.devfn = 0;
+		} else {
+			id.pci.bus = bridge->bus->number;
+			id.pci.devfn = bridge->devfn;
+		}
+	}
+
+	if (!pdev->is_virtfn && iommu_group_mf)
+		id.pci.devfn = PCI_DEVFN(PCI_SLOT(id.pci.devfn), 0);
+
+	*groupid = id.group;
+
+	return 0;
+}
+
 static struct iommu_ops intel_iommu_ops = {
 	.domain_init	= intel_iommu_domain_init,
 	.domain_destroy = intel_iommu_domain_destroy,
@@ -3921,6 +4137,8 @@ static struct iommu_ops intel_iommu_ops = {
 	.unmap		= intel_iommu_unmap,
 	.iova_to_phys	= intel_iommu_iova_to_phys,
 	.domain_has_cap = intel_iommu_domain_has_cap,
+	.device_group	= intel_iommu_device_group,
+	.pgsize_bitmap	= INTEL_IOMMU_PGSIZES,
 };
 
 static void __devinit quirk_iommu_rwbf(struct pci_dev *dev)
diff --git a/drivers/iommu/intr_remapping.c b/drivers/iommu/intr_remapping.c
index 1a89d4a2cadf..6777ca049471 100644
--- a/drivers/iommu/intr_remapping.c
+++ b/drivers/iommu/intr_remapping.c
@@ -21,6 +21,7 @@ int intr_remapping_enabled;
 
 static int disable_intremap;
 static int disable_sourceid_checking;
+static int no_x2apic_optout;
 
 static __init int setup_nointremap(char *str)
 {
@@ -34,18 +35,26 @@ static __init int setup_intremap(char *str)
 	if (!str)
 		return -EINVAL;
 
-	if (!strncmp(str, "on", 2))
-		disable_intremap = 0;
-	else if (!strncmp(str, "off", 3))
-		disable_intremap = 1;
-	else if (!strncmp(str, "nosid", 5))
-		disable_sourceid_checking = 1;
+	while (*str) {
+		if (!strncmp(str, "on", 2))
+			disable_intremap = 0;
+		else if (!strncmp(str, "off", 3))
+			disable_intremap = 1;
+		else if (!strncmp(str, "nosid", 5))
+			disable_sourceid_checking = 1;
+		else if (!strncmp(str, "no_x2apic_optout", 16))
+			no_x2apic_optout = 1;
+
+		str += strcspn(str, ",");
+		while (*str == ',')
+			str++;
+	}
 
 	return 0;
 }
 early_param("intremap", setup_intremap);
 
-static DEFINE_SPINLOCK(irq_2_ir_lock);
+static DEFINE_RAW_SPINLOCK(irq_2_ir_lock);
 
 static struct irq_2_iommu *irq_2_iommu(unsigned int irq)
 {
@@ -62,12 +71,12 @@ int get_irte(int irq, struct irte *entry)
 	if (!entry || !irq_iommu)
 		return -1;
 
-	spin_lock_irqsave(&irq_2_ir_lock, flags);
+	raw_spin_lock_irqsave(&irq_2_ir_lock, flags);
 
 	index = irq_iommu->irte_index + irq_iommu->sub_handle;
 	*entry = *(irq_iommu->iommu->ir_table->base + index);
 
-	spin_unlock_irqrestore(&irq_2_ir_lock, flags);
+	raw_spin_unlock_irqrestore(&irq_2_ir_lock, flags);
 	return 0;
 }
 
@@ -101,7 +110,7 @@ int alloc_irte(struct intel_iommu *iommu, int irq, u16 count)
 		return -1;
 	}
 
-	spin_lock_irqsave(&irq_2_ir_lock, flags);
+	raw_spin_lock_irqsave(&irq_2_ir_lock, flags);
 	do {
 		for (i = index; i < index + count; i++)
 			if  (table->base[i].present)
@@ -113,7 +122,7 @@ int alloc_irte(struct intel_iommu *iommu, int irq, u16 count)
 		index = (index + count) % INTR_REMAP_TABLE_ENTRIES;
 
 		if (index == start_index) {
-			spin_unlock_irqrestore(&irq_2_ir_lock, flags);
+			raw_spin_unlock_irqrestore(&irq_2_ir_lock, flags);
 			printk(KERN_ERR "can't allocate an IRTE\n");
 			return -1;
 		}
@@ -127,7 +136,7 @@ int alloc_irte(struct intel_iommu *iommu, int irq, u16 count)
 	irq_iommu->sub_handle = 0;
 	irq_iommu->irte_mask = mask;
 
-	spin_unlock_irqrestore(&irq_2_ir_lock, flags);
+	raw_spin_unlock_irqrestore(&irq_2_ir_lock, flags);
 
 	return index;
 }
@@ -152,10 +161,10 @@ int map_irq_to_irte_handle(int irq, u16 *sub_handle)
 	if (!irq_iommu)
 		return -1;
 
-	spin_lock_irqsave(&irq_2_ir_lock, flags);
+	raw_spin_lock_irqsave(&irq_2_ir_lock, flags);
 	*sub_handle = irq_iommu->sub_handle;
 	index = irq_iommu->irte_index;
-	spin_unlock_irqrestore(&irq_2_ir_lock, flags);
+	raw_spin_unlock_irqrestore(&irq_2_ir_lock, flags);
 	return index;
 }
 
@@ -167,14 +176,14 @@ int set_irte_irq(int irq, struct intel_iommu *iommu, u16 index, u16 subhandle)
 	if (!irq_iommu)
 		return -1;
 
-	spin_lock_irqsave(&irq_2_ir_lock, flags);
+	raw_spin_lock_irqsave(&irq_2_ir_lock, flags);
 
 	irq_iommu->iommu = iommu;
 	irq_iommu->irte_index = index;
 	irq_iommu->sub_handle = subhandle;
 	irq_iommu->irte_mask = 0;
 
-	spin_unlock_irqrestore(&irq_2_ir_lock, flags);
+	raw_spin_unlock_irqrestore(&irq_2_ir_lock, flags);
 
 	return 0;
 }
@@ -190,7 +199,7 @@ int modify_irte(int irq, struct irte *irte_modified)
 	if (!irq_iommu)
 		return -1;
 
-	spin_lock_irqsave(&irq_2_ir_lock, flags);
+	raw_spin_lock_irqsave(&irq_2_ir_lock, flags);
 
 	iommu = irq_iommu->iommu;
 
@@ -202,7 +211,7 @@ int modify_irte(int irq, struct irte *irte_modified)
 	__iommu_flush_cache(iommu, irte, sizeof(*irte));
 
 	rc = qi_flush_iec(iommu, index, 0);
-	spin_unlock_irqrestore(&irq_2_ir_lock, flags);
+	raw_spin_unlock_irqrestore(&irq_2_ir_lock, flags);
 
 	return rc;
 }
@@ -270,7 +279,7 @@ int free_irte(int irq)
 	if (!irq_iommu)
 		return -1;
 
-	spin_lock_irqsave(&irq_2_ir_lock, flags);
+	raw_spin_lock_irqsave(&irq_2_ir_lock, flags);
 
 	rc = clear_entries(irq_iommu);
 
@@ -279,7 +288,7 @@ int free_irte(int irq)
 	irq_iommu->sub_handle = 0;
 	irq_iommu->irte_mask = 0;
 
-	spin_unlock_irqrestore(&irq_2_ir_lock, flags);
+	raw_spin_unlock_irqrestore(&irq_2_ir_lock, flags);
 
 	return rc;
 }
@@ -409,7 +418,7 @@ static void iommu_set_intr_remapping(struct intel_iommu *iommu, int mode)
 
 	addr = virt_to_phys((void *)iommu->ir_table->base);
 
-	spin_lock_irqsave(&iommu->register_lock, flags);
+	raw_spin_lock_irqsave(&iommu->register_lock, flags);
 
 	dmar_writeq(iommu->reg + DMAR_IRTA_REG,
 		    (addr) | IR_X2APIC_MODE(mode) | INTR_REMAP_TABLE_REG_SIZE);
@@ -420,7 +429,7 @@ static void iommu_set_intr_remapping(struct intel_iommu *iommu, int mode)
 
 	IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG,
 		      readl, (sts & DMA_GSTS_IRTPS), sts);
-	spin_unlock_irqrestore(&iommu->register_lock, flags);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flags);
 
 	/*
 	 * global invalidation of interrupt entry cache before enabling
@@ -428,7 +437,7 @@ static void iommu_set_intr_remapping(struct intel_iommu *iommu, int mode)
 	 */
 	qi_global_iec(iommu);
 
-	spin_lock_irqsave(&iommu->register_lock, flags);
+	raw_spin_lock_irqsave(&iommu->register_lock, flags);
 
 	/* Enable interrupt-remapping */
 	iommu->gcmd |= DMA_GCMD_IRE;
@@ -437,7 +446,7 @@ static void iommu_set_intr_remapping(struct intel_iommu *iommu, int mode)
 	IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG,
 		      readl, (sts & DMA_GSTS_IRES), sts);
 
-	spin_unlock_irqrestore(&iommu->register_lock, flags);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flags);
 }
 
 
@@ -485,7 +494,7 @@ static void iommu_disable_intr_remapping(struct intel_iommu *iommu)
 	 */
 	qi_global_iec(iommu);
 
-	spin_lock_irqsave(&iommu->register_lock, flags);
+	raw_spin_lock_irqsave(&iommu->register_lock, flags);
 
 	sts = dmar_readq(iommu->reg + DMAR_GSTS_REG);
 	if (!(sts & DMA_GSTS_IRES))
@@ -498,7 +507,16 @@ static void iommu_disable_intr_remapping(struct intel_iommu *iommu)
 		      readl, !(sts & DMA_GSTS_IRES), sts);
 
 end:
-	spin_unlock_irqrestore(&iommu->register_lock, flags);
+	raw_spin_unlock_irqrestore(&iommu->register_lock, flags);
+}
+
+static int __init dmar_x2apic_optout(void)
+{
+	struct acpi_table_dmar *dmar;
+	dmar = (struct acpi_table_dmar *)dmar_tbl;
+	if (!dmar || no_x2apic_optout)
+		return 0;
+	return dmar->flags & DMAR_X2APIC_OPT_OUT;
 }
 
 int __init intr_remapping_supported(void)
@@ -521,16 +539,25 @@ int __init intr_remapping_supported(void)
 	return 1;
 }
 
-int __init enable_intr_remapping(int eim)
+int __init enable_intr_remapping(void)
 {
 	struct dmar_drhd_unit *drhd;
 	int setup = 0;
+	int eim = 0;
 
 	if (parse_ioapics_under_ir() != 1) {
 		printk(KERN_INFO "Not enable interrupt remapping\n");
 		return -1;
 	}
 
+	if (x2apic_supported()) {
+		eim = !dmar_x2apic_optout();
+		WARN(!eim, KERN_WARNING
+			   "Your BIOS is broken and requested that x2apic be disabled\n"
+			   "This will leave your machine vulnerable to irq-injection attacks\n"
+			   "Use 'intremap=no_x2apic_optout' to override BIOS request\n");
+	}
+
 	for_each_drhd_unit(drhd) {
 		struct intel_iommu *iommu = drhd->iommu;
 
@@ -606,8 +633,9 @@ int __init enable_intr_remapping(int eim)
 		goto error;
 
 	intr_remapping_enabled = 1;
+	pr_info("Enabled IRQ remapping in %s mode\n", eim ? "x2apic" : "xapic");
 
-	return 0;
+	return eim ? IRQ_REMAP_X2APIC_MODE : IRQ_REMAP_XAPIC_MODE;
 
 error:
 	/*
@@ -745,6 +773,15 @@ int __init parse_ioapics_under_ir(void)
 	return ir_supported;
 }
 
+int __init ir_dev_scope_init(void)
+{
+	if (!intr_remapping_enabled)
+		return 0;
+
+	return dmar_dev_scope_init();
+}
+rootfs_initcall(ir_dev_scope_init);
+
 void disable_intr_remapping(void)
 {
 	struct dmar_drhd_unit *drhd;
diff --git a/drivers/iommu/iommu.c b/drivers/iommu/iommu.c
index 6e6b6a11b3ce..2198b2dbbcd3 100644
--- a/drivers/iommu/iommu.c
+++ b/drivers/iommu/iommu.c
@@ -16,6 +16,10 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  */
 
+#define pr_fmt(fmt)    "%s: " fmt, __func__
+
+#include <linux/device.h>
+#include <linux/kernel.h>
 #include <linux/bug.h>
 #include <linux/types.h>
 #include <linux/module.h>
@@ -23,32 +27,129 @@
 #include <linux/errno.h>
 #include <linux/iommu.h>
 
-static struct iommu_ops *iommu_ops;
+static ssize_t show_iommu_group(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	unsigned int groupid;
+
+	if (iommu_device_group(dev, &groupid))
+		return 0;
+
+	return sprintf(buf, "%u", groupid);
+}
+static DEVICE_ATTR(iommu_group, S_IRUGO, show_iommu_group, NULL);
+
+static int add_iommu_group(struct device *dev, void *data)
+{
+	unsigned int groupid;
+
+	if (iommu_device_group(dev, &groupid) == 0)
+		return device_create_file(dev, &dev_attr_iommu_group);
+
+	return 0;
+}
+
+static int remove_iommu_group(struct device *dev)
+{
+	unsigned int groupid;
+
+	if (iommu_device_group(dev, &groupid) == 0)
+		device_remove_file(dev, &dev_attr_iommu_group);
+
+	return 0;
+}
+
+static int iommu_device_notifier(struct notifier_block *nb,
+				 unsigned long action, void *data)
+{
+	struct device *dev = data;
+
+	if (action == BUS_NOTIFY_ADD_DEVICE)
+		return add_iommu_group(dev, NULL);
+	else if (action == BUS_NOTIFY_DEL_DEVICE)
+		return remove_iommu_group(dev);
+
+	return 0;
+}
 
-void register_iommu(struct iommu_ops *ops)
+static struct notifier_block iommu_device_nb = {
+	.notifier_call = iommu_device_notifier,
+};
+
+static void iommu_bus_init(struct bus_type *bus, struct iommu_ops *ops)
+{
+	bus_register_notifier(bus, &iommu_device_nb);
+	bus_for_each_dev(bus, NULL, NULL, add_iommu_group);
+}
+
+/**
+ * bus_set_iommu - set iommu-callbacks for the bus
+ * @bus: bus.
+ * @ops: the callbacks provided by the iommu-driver
+ *
+ * This function is called by an iommu driver to set the iommu methods
+ * used for a particular bus. Drivers for devices on that bus can use
+ * the iommu-api after these ops are registered.
+ * This special function is needed because IOMMUs are usually devices on
+ * the bus itself, so the iommu drivers are not initialized when the bus
+ * is set up. With this function the iommu-driver can set the iommu-ops
+ * afterwards.
+ */
+int bus_set_iommu(struct bus_type *bus, struct iommu_ops *ops)
 {
-	if (iommu_ops)
-		BUG();
+	if (bus->iommu_ops != NULL)
+		return -EBUSY;
 
-	iommu_ops = ops;
+	bus->iommu_ops = ops;
+
+	/* Do IOMMU specific setup for this bus-type */
+	iommu_bus_init(bus, ops);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(bus_set_iommu);
+
+bool iommu_present(struct bus_type *bus)
+{
+	return bus->iommu_ops != NULL;
 }
+EXPORT_SYMBOL_GPL(iommu_present);
 
-bool iommu_found(void)
+/**
+ * iommu_set_fault_handler() - set a fault handler for an iommu domain
+ * @domain: iommu domain
+ * @handler: fault handler
+ *
+ * This function should be used by IOMMU users which want to be notified
+ * whenever an IOMMU fault happens.
+ *
+ * The fault handler itself should return 0 on success, and an appropriate
+ * error code otherwise.
+ */
+void iommu_set_fault_handler(struct iommu_domain *domain,
+					iommu_fault_handler_t handler)
 {
-	return iommu_ops != NULL;
+	BUG_ON(!domain);
+
+	domain->handler = handler;
 }
-EXPORT_SYMBOL_GPL(iommu_found);
+EXPORT_SYMBOL_GPL(iommu_set_fault_handler);
 
-struct iommu_domain *iommu_domain_alloc(void)
+struct iommu_domain *iommu_domain_alloc(struct bus_type *bus)
 {
 	struct iommu_domain *domain;
 	int ret;
 
-	domain = kmalloc(sizeof(*domain), GFP_KERNEL);
+	if (bus == NULL || bus->iommu_ops == NULL)
+		return NULL;
+
+	domain = kzalloc(sizeof(*domain), GFP_KERNEL);
 	if (!domain)
 		return NULL;
 
-	ret = iommu_ops->domain_init(domain);
+	domain->ops = bus->iommu_ops;
+
+	ret = domain->ops->domain_init(domain);
 	if (ret)
 		goto out_free;
 
@@ -63,62 +164,180 @@ EXPORT_SYMBOL_GPL(iommu_domain_alloc);
 
 void iommu_domain_free(struct iommu_domain *domain)
 {
-	iommu_ops->domain_destroy(domain);
+	if (likely(domain->ops->domain_destroy != NULL))
+		domain->ops->domain_destroy(domain);
+
 	kfree(domain);
 }
 EXPORT_SYMBOL_GPL(iommu_domain_free);
 
 int iommu_attach_device(struct iommu_domain *domain, struct device *dev)
 {
-	return iommu_ops->attach_dev(domain, dev);
+	if (unlikely(domain->ops->attach_dev == NULL))
+		return -ENODEV;
+
+	return domain->ops->attach_dev(domain, dev);
 }
 EXPORT_SYMBOL_GPL(iommu_attach_device);
 
 void iommu_detach_device(struct iommu_domain *domain, struct device *dev)
 {
-	iommu_ops->detach_dev(domain, dev);
+	if (unlikely(domain->ops->detach_dev == NULL))
+		return;
+
+	domain->ops->detach_dev(domain, dev);
 }
 EXPORT_SYMBOL_GPL(iommu_detach_device);
 
 phys_addr_t iommu_iova_to_phys(struct iommu_domain *domain,
 			       unsigned long iova)
 {
-	return iommu_ops->iova_to_phys(domain, iova);
+	if (unlikely(domain->ops->iova_to_phys == NULL))
+		return 0;
+
+	return domain->ops->iova_to_phys(domain, iova);
 }
 EXPORT_SYMBOL_GPL(iommu_iova_to_phys);
 
 int iommu_domain_has_cap(struct iommu_domain *domain,
 			 unsigned long cap)
 {
-	return iommu_ops->domain_has_cap(domain, cap);
+	if (unlikely(domain->ops->domain_has_cap == NULL))
+		return 0;
+
+	return domain->ops->domain_has_cap(domain, cap);
 }
 EXPORT_SYMBOL_GPL(iommu_domain_has_cap);
 
 int iommu_map(struct iommu_domain *domain, unsigned long iova,
-	      phys_addr_t paddr, int gfp_order, int prot)
+	      phys_addr_t paddr, size_t size, int prot)
 {
-	unsigned long invalid_mask;
-	size_t size;
+	unsigned long orig_iova = iova;
+	unsigned int min_pagesz;
+	size_t orig_size = size;
+	int ret = 0;
+
+	if (unlikely(domain->ops->map == NULL))
+		return -ENODEV;
+
+	/* find out the minimum page size supported */
+	min_pagesz = 1 << __ffs(domain->ops->pgsize_bitmap);
+
+	/*
+	 * both the virtual address and the physical one, as well as
+	 * the size of the mapping, must be aligned (at least) to the
+	 * size of the smallest page supported by the hardware
+	 */
+	if (!IS_ALIGNED(iova | paddr | size, min_pagesz)) {
+		pr_err("unaligned: iova 0x%lx pa 0x%lx size 0x%lx min_pagesz "
+			"0x%x\n", iova, (unsigned long)paddr,
+			(unsigned long)size, min_pagesz);
+		return -EINVAL;
+	}
+
+	pr_debug("map: iova 0x%lx pa 0x%lx size 0x%lx\n", iova,
+				(unsigned long)paddr, (unsigned long)size);
+
+	while (size) {
+		unsigned long pgsize, addr_merge = iova | paddr;
+		unsigned int pgsize_idx;
+
+		/* Max page size that still fits into 'size' */
+		pgsize_idx = __fls(size);
+
+		/* need to consider alignment requirements ? */
+		if (likely(addr_merge)) {
+			/* Max page size allowed by both iova and paddr */
+			unsigned int align_pgsize_idx = __ffs(addr_merge);
+
+			pgsize_idx = min(pgsize_idx, align_pgsize_idx);
+		}
 
-	size         = 0x1000UL << gfp_order;
-	invalid_mask = size - 1;
+		/* build a mask of acceptable page sizes */
+		pgsize = (1UL << (pgsize_idx + 1)) - 1;
 
-	BUG_ON((iova | paddr) & invalid_mask);
+		/* throw away page sizes not supported by the hardware */
+		pgsize &= domain->ops->pgsize_bitmap;
 
-	return iommu_ops->map(domain, iova, paddr, gfp_order, prot);
+		/* make sure we're still sane */
+		BUG_ON(!pgsize);
+
+		/* pick the biggest page */
+		pgsize_idx = __fls(pgsize);
+		pgsize = 1UL << pgsize_idx;
+
+		pr_debug("mapping: iova 0x%lx pa 0x%lx pgsize %lu\n", iova,
+					(unsigned long)paddr, pgsize);
+
+		ret = domain->ops->map(domain, iova, paddr, pgsize, prot);
+		if (ret)
+			break;
+
+		iova += pgsize;
+		paddr += pgsize;
+		size -= pgsize;
+	}
+
+	/* unroll mapping in case something went wrong */
+	if (ret)
+		iommu_unmap(domain, orig_iova, orig_size - size);
+
+	return ret;
 }
 EXPORT_SYMBOL_GPL(iommu_map);
 
-int iommu_unmap(struct iommu_domain *domain, unsigned long iova, int gfp_order)
+size_t iommu_unmap(struct iommu_domain *domain, unsigned long iova, size_t size)
 {
-	unsigned long invalid_mask;
-	size_t size;
+	size_t unmapped_page, unmapped = 0;
+	unsigned int min_pagesz;
+
+	if (unlikely(domain->ops->unmap == NULL))
+		return -ENODEV;
+
+	/* find out the minimum page size supported */
+	min_pagesz = 1 << __ffs(domain->ops->pgsize_bitmap);
 
-	size         = 0x1000UL << gfp_order;
-	invalid_mask = size - 1;
+	/*
+	 * The virtual address, as well as the size of the mapping, must be
+	 * aligned (at least) to the size of the smallest page supported
+	 * by the hardware
+	 */
+	if (!IS_ALIGNED(iova | size, min_pagesz)) {
+		pr_err("unaligned: iova 0x%lx size 0x%lx min_pagesz 0x%x\n",
+					iova, (unsigned long)size, min_pagesz);
+		return -EINVAL;
+	}
 
-	BUG_ON(iova & invalid_mask);
+	pr_debug("unmap this: iova 0x%lx size 0x%lx\n", iova,
+							(unsigned long)size);
 
-	return iommu_ops->unmap(domain, iova, gfp_order);
+	/*
+	 * Keep iterating until we either unmap 'size' bytes (or more)
+	 * or we hit an area that isn't mapped.
+	 */
+	while (unmapped < size) {
+		size_t left = size - unmapped;
+
+		unmapped_page = domain->ops->unmap(domain, iova, left);
+		if (!unmapped_page)
+			break;
+
+		pr_debug("unmapped: iova 0x%lx size %lx\n", iova,
+					(unsigned long)unmapped_page);
+
+		iova += unmapped_page;
+		unmapped += unmapped_page;
+	}
+
+	return unmapped;
 }
 EXPORT_SYMBOL_GPL(iommu_unmap);
+
+int iommu_device_group(struct device *dev, unsigned int *groupid)
+{
+	if (iommu_present(dev->bus) && dev->bus->iommu_ops->device_group)
+		return dev->bus->iommu_ops->device_group(dev, groupid);
+
+	return -ENODEV;
+}
+EXPORT_SYMBOL_GPL(iommu_device_group);
diff --git a/drivers/iommu/msm_iommu.c b/drivers/iommu/msm_iommu.c
index 1a584e077c61..08a90b88e40d 100644
--- a/drivers/iommu/msm_iommu.c
+++ b/drivers/iommu/msm_iommu.c
@@ -42,6 +42,9 @@ __asm__ __volatile__ (							\
 #define RCP15_PRRR(reg)		MRC(reg, p15, 0, c10, c2, 0)
 #define RCP15_NMRR(reg)		MRC(reg, p15, 0, c10, c2, 1)
 
+/* bitmap of the page sizes currently supported */
+#define MSM_IOMMU_PGSIZES	(SZ_4K | SZ_64K | SZ_1M | SZ_16M)
+
 static int msm_iommu_tex_class[4];
 
 DEFINE_SPINLOCK(msm_iommu_lock);
@@ -352,7 +355,7 @@ fail:
 }
 
 static int msm_iommu_map(struct iommu_domain *domain, unsigned long va,
-			 phys_addr_t pa, int order, int prot)
+			 phys_addr_t pa, size_t len, int prot)
 {
 	struct msm_priv *priv;
 	unsigned long flags;
@@ -363,7 +366,6 @@ static int msm_iommu_map(struct iommu_domain *domain, unsigned long va,
 	unsigned long *sl_pte;
 	unsigned long sl_offset;
 	unsigned int pgprot;
-	size_t len = 0x1000UL << order;
 	int ret = 0, tex, sh;
 
 	spin_lock_irqsave(&msm_iommu_lock, flags);
@@ -463,8 +465,8 @@ fail:
 	return ret;
 }
 
-static int msm_iommu_unmap(struct iommu_domain *domain, unsigned long va,
-			    int order)
+static size_t msm_iommu_unmap(struct iommu_domain *domain, unsigned long va,
+			    size_t len)
 {
 	struct msm_priv *priv;
 	unsigned long flags;
@@ -474,7 +476,6 @@ static int msm_iommu_unmap(struct iommu_domain *domain, unsigned long va,
 	unsigned long *sl_table;
 	unsigned long *sl_pte;
 	unsigned long sl_offset;
-	size_t len = 0x1000UL << order;
 	int i, ret = 0;
 
 	spin_lock_irqsave(&msm_iommu_lock, flags);
@@ -543,9 +544,13 @@ static int msm_iommu_unmap(struct iommu_domain *domain, unsigned long va,
 	}
 
 	ret = __flush_iotlb(domain);
+
 fail:
 	spin_unlock_irqrestore(&msm_iommu_lock, flags);
-	return ret;
+
+	/* the IOMMU API requires us to return how many bytes were unmapped */
+	len = ret ? 0 : len;
+	return len;
 }
 
 static phys_addr_t msm_iommu_iova_to_phys(struct iommu_domain *domain,
@@ -677,7 +682,8 @@ static struct iommu_ops msm_iommu_ops = {
 	.map = msm_iommu_map,
 	.unmap = msm_iommu_unmap,
 	.iova_to_phys = msm_iommu_iova_to_phys,
-	.domain_has_cap = msm_iommu_domain_has_cap
+	.domain_has_cap = msm_iommu_domain_has_cap,
+	.pgsize_bitmap = MSM_IOMMU_PGSIZES,
 };
 
 static int __init get_tex_class(int icp, int ocp, int mt, int nos)
@@ -721,7 +727,7 @@ static void __init setup_iommu_tex_classes(void)
 static int __init msm_iommu_init(void)
 {
 	setup_iommu_tex_classes();
-	register_iommu(&msm_iommu_ops);
+	bus_set_iommu(&platform_bus_type, &msm_iommu_ops);
 	return 0;
 }
 
diff --git a/drivers/iommu/omap-iommu-debug.c b/drivers/iommu/omap-iommu-debug.c
new file mode 100644
index 000000000000..288da5c1499d
--- /dev/null
+++ b/drivers/iommu/omap-iommu-debug.c
@@ -0,0 +1,419 @@
+/*
+ * omap iommu: debugfs interface
+ *
+ * Copyright (C) 2008-2009 Nokia Corporation
+ *
+ * Written by Hiroshi DOYU <Hiroshi.DOYU@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/platform_device.h>
+#include <linux/debugfs.h>
+
+#include <plat/iommu.h>
+#include <plat/iovmm.h>
+
+#include <plat/iopgtable.h>
+
+#define MAXCOLUMN 100 /* for short messages */
+
+static DEFINE_MUTEX(iommu_debug_lock);
+
+static struct dentry *iommu_debug_root;
+
+static ssize_t debug_read_ver(struct file *file, char __user *userbuf,
+			      size_t count, loff_t *ppos)
+{
+	u32 ver = omap_iommu_arch_version();
+	char buf[MAXCOLUMN], *p = buf;
+
+	p += sprintf(p, "H/W version: %d.%d\n", (ver >> 4) & 0xf , ver & 0xf);
+
+	return simple_read_from_buffer(userbuf, count, ppos, buf, p - buf);
+}
+
+static ssize_t debug_read_regs(struct file *file, char __user *userbuf,
+			       size_t count, loff_t *ppos)
+{
+	struct omap_iommu *obj = file->private_data;
+	char *p, *buf;
+	ssize_t bytes;
+
+	buf = kmalloc(count, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+	p = buf;
+
+	mutex_lock(&iommu_debug_lock);
+
+	bytes = omap_iommu_dump_ctx(obj, p, count);
+	bytes = simple_read_from_buffer(userbuf, count, ppos, buf, bytes);
+
+	mutex_unlock(&iommu_debug_lock);
+	kfree(buf);
+
+	return bytes;
+}
+
+static ssize_t debug_read_tlb(struct file *file, char __user *userbuf,
+			      size_t count, loff_t *ppos)
+{
+	struct omap_iommu *obj = file->private_data;
+	char *p, *buf;
+	ssize_t bytes, rest;
+
+	buf = kmalloc(count, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+	p = buf;
+
+	mutex_lock(&iommu_debug_lock);
+
+	p += sprintf(p, "%8s %8s\n", "cam:", "ram:");
+	p += sprintf(p, "-----------------------------------------\n");
+	rest = count - (p - buf);
+	p += omap_dump_tlb_entries(obj, p, rest);
+
+	bytes = simple_read_from_buffer(userbuf, count, ppos, buf, p - buf);
+
+	mutex_unlock(&iommu_debug_lock);
+	kfree(buf);
+
+	return bytes;
+}
+
+static ssize_t debug_write_pagetable(struct file *file,
+		     const char __user *userbuf, size_t count, loff_t *ppos)
+{
+	struct iotlb_entry e;
+	struct cr_regs cr;
+	int err;
+	struct omap_iommu *obj = file->private_data;
+	char buf[MAXCOLUMN], *p = buf;
+
+	count = min(count, sizeof(buf));
+
+	mutex_lock(&iommu_debug_lock);
+	if (copy_from_user(p, userbuf, count)) {
+		mutex_unlock(&iommu_debug_lock);
+		return -EFAULT;
+	}
+
+	sscanf(p, "%x %x", &cr.cam, &cr.ram);
+	if (!cr.cam || !cr.ram) {
+		mutex_unlock(&iommu_debug_lock);
+		return -EINVAL;
+	}
+
+	omap_iotlb_cr_to_e(&cr, &e);
+	err = omap_iopgtable_store_entry(obj, &e);
+	if (err)
+		dev_err(obj->dev, "%s: fail to store cr\n", __func__);
+
+	mutex_unlock(&iommu_debug_lock);
+	return count;
+}
+
+#define dump_ioptable_entry_one(lv, da, val)			\
+	({							\
+		int __err = 0;					\
+		ssize_t bytes;					\
+		const int maxcol = 22;				\
+		const char *str = "%d: %08x %08x\n";		\
+		bytes = snprintf(p, maxcol, str, lv, da, val);	\
+		p += bytes;					\
+		len -= bytes;					\
+		if (len < maxcol)				\
+			__err = -ENOMEM;			\
+		__err;						\
+	})
+
+static ssize_t dump_ioptable(struct omap_iommu *obj, char *buf, ssize_t len)
+{
+	int i;
+	u32 *iopgd;
+	char *p = buf;
+
+	spin_lock(&obj->page_table_lock);
+
+	iopgd = iopgd_offset(obj, 0);
+	for (i = 0; i < PTRS_PER_IOPGD; i++, iopgd++) {
+		int j, err;
+		u32 *iopte;
+		u32 da;
+
+		if (!*iopgd)
+			continue;
+
+		if (!(*iopgd & IOPGD_TABLE)) {
+			da = i << IOPGD_SHIFT;
+
+			err = dump_ioptable_entry_one(1, da, *iopgd);
+			if (err)
+				goto out;
+			continue;
+		}
+
+		iopte = iopte_offset(iopgd, 0);
+
+		for (j = 0; j < PTRS_PER_IOPTE; j++, iopte++) {
+			if (!*iopte)
+				continue;
+
+			da = (i << IOPGD_SHIFT) + (j << IOPTE_SHIFT);
+			err = dump_ioptable_entry_one(2, da, *iopgd);
+			if (err)
+				goto out;
+		}
+	}
+out:
+	spin_unlock(&obj->page_table_lock);
+
+	return p - buf;
+}
+
+static ssize_t debug_read_pagetable(struct file *file, char __user *userbuf,
+				    size_t count, loff_t *ppos)
+{
+	struct omap_iommu *obj = file->private_data;
+	char *p, *buf;
+	size_t bytes;
+
+	buf = (char *)__get_free_page(GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+	p = buf;
+
+	p += sprintf(p, "L: %8s %8s\n", "da:", "pa:");
+	p += sprintf(p, "-----------------------------------------\n");
+
+	mutex_lock(&iommu_debug_lock);
+
+	bytes = PAGE_SIZE - (p - buf);
+	p += dump_ioptable(obj, p, bytes);
+
+	bytes = simple_read_from_buffer(userbuf, count, ppos, buf, p - buf);
+
+	mutex_unlock(&iommu_debug_lock);
+	free_page((unsigned long)buf);
+
+	return bytes;
+}
+
+static ssize_t debug_read_mmap(struct file *file, char __user *userbuf,
+			       size_t count, loff_t *ppos)
+{
+	struct omap_iommu *obj = file->private_data;
+	char *p, *buf;
+	struct iovm_struct *tmp;
+	int uninitialized_var(i);
+	ssize_t bytes;
+
+	buf = (char *)__get_free_page(GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+	p = buf;
+
+	p += sprintf(p, "%-3s %-8s %-8s %6s %8s\n",
+		     "No", "start", "end", "size", "flags");
+	p += sprintf(p, "-------------------------------------------------\n");
+
+	mutex_lock(&iommu_debug_lock);
+
+	list_for_each_entry(tmp, &obj->mmap, list) {
+		size_t len;
+		const char *str = "%3d %08x-%08x %6x %8x\n";
+		const int maxcol = 39;
+
+		len = tmp->da_end - tmp->da_start;
+		p += snprintf(p, maxcol, str,
+			      i, tmp->da_start, tmp->da_end, len, tmp->flags);
+
+		if (PAGE_SIZE - (p - buf) < maxcol)
+			break;
+		i++;
+	}
+
+	bytes = simple_read_from_buffer(userbuf, count, ppos, buf, p - buf);
+
+	mutex_unlock(&iommu_debug_lock);
+	free_page((unsigned long)buf);
+
+	return bytes;
+}
+
+static ssize_t debug_read_mem(struct file *file, char __user *userbuf,
+			      size_t count, loff_t *ppos)
+{
+	struct omap_iommu *obj = file->private_data;
+	char *p, *buf;
+	struct iovm_struct *area;
+	ssize_t bytes;
+
+	count = min_t(ssize_t, count, PAGE_SIZE);
+
+	buf = (char *)__get_free_page(GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+	p = buf;
+
+	mutex_lock(&iommu_debug_lock);
+
+	area = omap_find_iovm_area(obj, (u32)ppos);
+	if (IS_ERR(area)) {
+		bytes = -EINVAL;
+		goto err_out;
+	}
+	memcpy(p, area->va, count);
+	p += count;
+
+	bytes = simple_read_from_buffer(userbuf, count, ppos, buf, p - buf);
+err_out:
+	mutex_unlock(&iommu_debug_lock);
+	free_page((unsigned long)buf);
+
+	return bytes;
+}
+
+static ssize_t debug_write_mem(struct file *file, const char __user *userbuf,
+			       size_t count, loff_t *ppos)
+{
+	struct omap_iommu *obj = file->private_data;
+	struct iovm_struct *area;
+	char *p, *buf;
+
+	count = min_t(size_t, count, PAGE_SIZE);
+
+	buf = (char *)__get_free_page(GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+	p = buf;
+
+	mutex_lock(&iommu_debug_lock);
+
+	if (copy_from_user(p, userbuf, count)) {
+		count =  -EFAULT;
+		goto err_out;
+	}
+
+	area = omap_find_iovm_area(obj, (u32)ppos);
+	if (IS_ERR(area)) {
+		count = -EINVAL;
+		goto err_out;
+	}
+	memcpy(area->va, p, count);
+err_out:
+	mutex_unlock(&iommu_debug_lock);
+	free_page((unsigned long)buf);
+
+	return count;
+}
+
+static int debug_open_generic(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+
+#define DEBUG_FOPS(name)						\
+	static const struct file_operations debug_##name##_fops = {	\
+		.open = debug_open_generic,				\
+		.read = debug_read_##name,				\
+		.write = debug_write_##name,				\
+		.llseek = generic_file_llseek,				\
+	};
+
+#define DEBUG_FOPS_RO(name)						\
+	static const struct file_operations debug_##name##_fops = {	\
+		.open = debug_open_generic,				\
+		.read = debug_read_##name,				\
+		.llseek = generic_file_llseek,				\
+	};
+
+DEBUG_FOPS_RO(ver);
+DEBUG_FOPS_RO(regs);
+DEBUG_FOPS_RO(tlb);
+DEBUG_FOPS(pagetable);
+DEBUG_FOPS_RO(mmap);
+DEBUG_FOPS(mem);
+
+#define __DEBUG_ADD_FILE(attr, mode)					\
+	{								\
+		struct dentry *dent;					\
+		dent = debugfs_create_file(#attr, mode, parent,		\
+					   obj, &debug_##attr##_fops);	\
+		if (!dent)						\
+			return -ENOMEM;					\
+	}
+
+#define DEBUG_ADD_FILE(name) __DEBUG_ADD_FILE(name, 600)
+#define DEBUG_ADD_FILE_RO(name) __DEBUG_ADD_FILE(name, 400)
+
+static int iommu_debug_register(struct device *dev, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct omap_iommu *obj = platform_get_drvdata(pdev);
+	struct dentry *d, *parent;
+
+	if (!obj || !obj->dev)
+		return -EINVAL;
+
+	d = debugfs_create_dir(obj->name, iommu_debug_root);
+	if (!d)
+		return -ENOMEM;
+	parent = d;
+
+	d = debugfs_create_u8("nr_tlb_entries", 400, parent,
+			      (u8 *)&obj->nr_tlb_entries);
+	if (!d)
+		return -ENOMEM;
+
+	DEBUG_ADD_FILE_RO(ver);
+	DEBUG_ADD_FILE_RO(regs);
+	DEBUG_ADD_FILE_RO(tlb);
+	DEBUG_ADD_FILE(pagetable);
+	DEBUG_ADD_FILE_RO(mmap);
+	DEBUG_ADD_FILE(mem);
+
+	return 0;
+}
+
+static int __init iommu_debug_init(void)
+{
+	struct dentry *d;
+	int err;
+
+	d = debugfs_create_dir("iommu", NULL);
+	if (!d)
+		return -ENOMEM;
+	iommu_debug_root = d;
+
+	err = omap_foreach_iommu_device(d, iommu_debug_register);
+	if (err)
+		goto err_out;
+	return 0;
+
+err_out:
+	debugfs_remove_recursive(iommu_debug_root);
+	return err;
+}
+module_init(iommu_debug_init)
+
+static void __exit iommu_debugfs_exit(void)
+{
+	debugfs_remove_recursive(iommu_debug_root);
+}
+module_exit(iommu_debugfs_exit)
+
+MODULE_DESCRIPTION("omap iommu: debugfs interface");
+MODULE_AUTHOR("Hiroshi DOYU <Hiroshi.DOYU@nokia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iommu/omap-iommu.c b/drivers/iommu/omap-iommu.c
new file mode 100644
index 000000000000..d8edd979d01b
--- /dev/null
+++ b/drivers/iommu/omap-iommu.c
@@ -0,0 +1,1239 @@
+/*
+ * omap iommu: tlb and pagetable primitives
+ *
+ * Copyright (C) 2008-2010 Nokia Corporation
+ *
+ * Written by Hiroshi DOYU <Hiroshi.DOYU@nokia.com>,
+ *		Paul Mundt and Toshihiro Kobayashi
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/ioport.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/iommu.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+
+#include <asm/cacheflush.h>
+
+#include <plat/iommu.h>
+
+#include <plat/iopgtable.h>
+
+#define for_each_iotlb_cr(obj, n, __i, cr)				\
+	for (__i = 0;							\
+	     (__i < (n)) && (cr = __iotlb_read_cr((obj), __i), true);	\
+	     __i++)
+
+/* bitmap of the page sizes currently supported */
+#define OMAP_IOMMU_PGSIZES	(SZ_4K | SZ_64K | SZ_1M | SZ_16M)
+
+/**
+ * struct omap_iommu_domain - omap iommu domain
+ * @pgtable:	the page table
+ * @iommu_dev:	an omap iommu device attached to this domain. only a single
+ *		iommu device can be attached for now.
+ * @lock:	domain lock, should be taken when attaching/detaching
+ */
+struct omap_iommu_domain {
+	u32 *pgtable;
+	struct omap_iommu *iommu_dev;
+	spinlock_t lock;
+};
+
+/* accommodate the difference between omap1 and omap2/3 */
+static const struct iommu_functions *arch_iommu;
+
+static struct platform_driver omap_iommu_driver;
+static struct kmem_cache *iopte_cachep;
+
+/**
+ * omap_install_iommu_arch - Install archtecure specific iommu functions
+ * @ops:	a pointer to architecture specific iommu functions
+ *
+ * There are several kind of iommu algorithm(tlb, pagetable) among
+ * omap series. This interface installs such an iommu algorighm.
+ **/
+int omap_install_iommu_arch(const struct iommu_functions *ops)
+{
+	if (arch_iommu)
+		return -EBUSY;
+
+	arch_iommu = ops;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(omap_install_iommu_arch);
+
+/**
+ * omap_uninstall_iommu_arch - Uninstall archtecure specific iommu functions
+ * @ops:	a pointer to architecture specific iommu functions
+ *
+ * This interface uninstalls the iommu algorighm installed previously.
+ **/
+void omap_uninstall_iommu_arch(const struct iommu_functions *ops)
+{
+	if (arch_iommu != ops)
+		pr_err("%s: not your arch\n", __func__);
+
+	arch_iommu = NULL;
+}
+EXPORT_SYMBOL_GPL(omap_uninstall_iommu_arch);
+
+/**
+ * omap_iommu_save_ctx - Save registers for pm off-mode support
+ * @dev:	client device
+ **/
+void omap_iommu_save_ctx(struct device *dev)
+{
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
+
+	arch_iommu->save_ctx(obj);
+}
+EXPORT_SYMBOL_GPL(omap_iommu_save_ctx);
+
+/**
+ * omap_iommu_restore_ctx - Restore registers for pm off-mode support
+ * @dev:	client device
+ **/
+void omap_iommu_restore_ctx(struct device *dev)
+{
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
+
+	arch_iommu->restore_ctx(obj);
+}
+EXPORT_SYMBOL_GPL(omap_iommu_restore_ctx);
+
+/**
+ * omap_iommu_arch_version - Return running iommu arch version
+ **/
+u32 omap_iommu_arch_version(void)
+{
+	return arch_iommu->version;
+}
+EXPORT_SYMBOL_GPL(omap_iommu_arch_version);
+
+static int iommu_enable(struct omap_iommu *obj)
+{
+	int err;
+
+	if (!obj)
+		return -EINVAL;
+
+	if (!arch_iommu)
+		return -ENODEV;
+
+	clk_enable(obj->clk);
+
+	err = arch_iommu->enable(obj);
+
+	clk_disable(obj->clk);
+	return err;
+}
+
+static void iommu_disable(struct omap_iommu *obj)
+{
+	if (!obj)
+		return;
+
+	clk_enable(obj->clk);
+
+	arch_iommu->disable(obj);
+
+	clk_disable(obj->clk);
+}
+
+/*
+ *	TLB operations
+ */
+void omap_iotlb_cr_to_e(struct cr_regs *cr, struct iotlb_entry *e)
+{
+	BUG_ON(!cr || !e);
+
+	arch_iommu->cr_to_e(cr, e);
+}
+EXPORT_SYMBOL_GPL(omap_iotlb_cr_to_e);
+
+static inline int iotlb_cr_valid(struct cr_regs *cr)
+{
+	if (!cr)
+		return -EINVAL;
+
+	return arch_iommu->cr_valid(cr);
+}
+
+static inline struct cr_regs *iotlb_alloc_cr(struct omap_iommu *obj,
+					     struct iotlb_entry *e)
+{
+	if (!e)
+		return NULL;
+
+	return arch_iommu->alloc_cr(obj, e);
+}
+
+static u32 iotlb_cr_to_virt(struct cr_regs *cr)
+{
+	return arch_iommu->cr_to_virt(cr);
+}
+
+static u32 get_iopte_attr(struct iotlb_entry *e)
+{
+	return arch_iommu->get_pte_attr(e);
+}
+
+static u32 iommu_report_fault(struct omap_iommu *obj, u32 *da)
+{
+	return arch_iommu->fault_isr(obj, da);
+}
+
+static void iotlb_lock_get(struct omap_iommu *obj, struct iotlb_lock *l)
+{
+	u32 val;
+
+	val = iommu_read_reg(obj, MMU_LOCK);
+
+	l->base = MMU_LOCK_BASE(val);
+	l->vict = MMU_LOCK_VICT(val);
+
+}
+
+static void iotlb_lock_set(struct omap_iommu *obj, struct iotlb_lock *l)
+{
+	u32 val;
+
+	val = (l->base << MMU_LOCK_BASE_SHIFT);
+	val |= (l->vict << MMU_LOCK_VICT_SHIFT);
+
+	iommu_write_reg(obj, val, MMU_LOCK);
+}
+
+static void iotlb_read_cr(struct omap_iommu *obj, struct cr_regs *cr)
+{
+	arch_iommu->tlb_read_cr(obj, cr);
+}
+
+static void iotlb_load_cr(struct omap_iommu *obj, struct cr_regs *cr)
+{
+	arch_iommu->tlb_load_cr(obj, cr);
+
+	iommu_write_reg(obj, 1, MMU_FLUSH_ENTRY);
+	iommu_write_reg(obj, 1, MMU_LD_TLB);
+}
+
+/**
+ * iotlb_dump_cr - Dump an iommu tlb entry into buf
+ * @obj:	target iommu
+ * @cr:		contents of cam and ram register
+ * @buf:	output buffer
+ **/
+static inline ssize_t iotlb_dump_cr(struct omap_iommu *obj, struct cr_regs *cr,
+				    char *buf)
+{
+	BUG_ON(!cr || !buf);
+
+	return arch_iommu->dump_cr(obj, cr, buf);
+}
+
+/* only used in iotlb iteration for-loop */
+static struct cr_regs __iotlb_read_cr(struct omap_iommu *obj, int n)
+{
+	struct cr_regs cr;
+	struct iotlb_lock l;
+
+	iotlb_lock_get(obj, &l);
+	l.vict = n;
+	iotlb_lock_set(obj, &l);
+	iotlb_read_cr(obj, &cr);
+
+	return cr;
+}
+
+/**
+ * load_iotlb_entry - Set an iommu tlb entry
+ * @obj:	target iommu
+ * @e:		an iommu tlb entry info
+ **/
+#ifdef PREFETCH_IOTLB
+static int load_iotlb_entry(struct omap_iommu *obj, struct iotlb_entry *e)
+{
+	int err = 0;
+	struct iotlb_lock l;
+	struct cr_regs *cr;
+
+	if (!obj || !obj->nr_tlb_entries || !e)
+		return -EINVAL;
+
+	clk_enable(obj->clk);
+
+	iotlb_lock_get(obj, &l);
+	if (l.base == obj->nr_tlb_entries) {
+		dev_warn(obj->dev, "%s: preserve entries full\n", __func__);
+		err = -EBUSY;
+		goto out;
+	}
+	if (!e->prsvd) {
+		int i;
+		struct cr_regs tmp;
+
+		for_each_iotlb_cr(obj, obj->nr_tlb_entries, i, tmp)
+			if (!iotlb_cr_valid(&tmp))
+				break;
+
+		if (i == obj->nr_tlb_entries) {
+			dev_dbg(obj->dev, "%s: full: no entry\n", __func__);
+			err = -EBUSY;
+			goto out;
+		}
+
+		iotlb_lock_get(obj, &l);
+	} else {
+		l.vict = l.base;
+		iotlb_lock_set(obj, &l);
+	}
+
+	cr = iotlb_alloc_cr(obj, e);
+	if (IS_ERR(cr)) {
+		clk_disable(obj->clk);
+		return PTR_ERR(cr);
+	}
+
+	iotlb_load_cr(obj, cr);
+	kfree(cr);
+
+	if (e->prsvd)
+		l.base++;
+	/* increment victim for next tlb load */
+	if (++l.vict == obj->nr_tlb_entries)
+		l.vict = l.base;
+	iotlb_lock_set(obj, &l);
+out:
+	clk_disable(obj->clk);
+	return err;
+}
+
+#else /* !PREFETCH_IOTLB */
+
+static int load_iotlb_entry(struct omap_iommu *obj, struct iotlb_entry *e)
+{
+	return 0;
+}
+
+#endif /* !PREFETCH_IOTLB */
+
+static int prefetch_iotlb_entry(struct omap_iommu *obj, struct iotlb_entry *e)
+{
+	return load_iotlb_entry(obj, e);
+}
+
+/**
+ * flush_iotlb_page - Clear an iommu tlb entry
+ * @obj:	target iommu
+ * @da:		iommu device virtual address
+ *
+ * Clear an iommu tlb entry which includes 'da' address.
+ **/
+static void flush_iotlb_page(struct omap_iommu *obj, u32 da)
+{
+	int i;
+	struct cr_regs cr;
+
+	clk_enable(obj->clk);
+
+	for_each_iotlb_cr(obj, obj->nr_tlb_entries, i, cr) {
+		u32 start;
+		size_t bytes;
+
+		if (!iotlb_cr_valid(&cr))
+			continue;
+
+		start = iotlb_cr_to_virt(&cr);
+		bytes = iopgsz_to_bytes(cr.cam & 3);
+
+		if ((start <= da) && (da < start + bytes)) {
+			dev_dbg(obj->dev, "%s: %08x<=%08x(%x)\n",
+				__func__, start, da, bytes);
+			iotlb_load_cr(obj, &cr);
+			iommu_write_reg(obj, 1, MMU_FLUSH_ENTRY);
+		}
+	}
+	clk_disable(obj->clk);
+
+	if (i == obj->nr_tlb_entries)
+		dev_dbg(obj->dev, "%s: no page for %08x\n", __func__, da);
+}
+
+/**
+ * flush_iotlb_all - Clear all iommu tlb entries
+ * @obj:	target iommu
+ **/
+static void flush_iotlb_all(struct omap_iommu *obj)
+{
+	struct iotlb_lock l;
+
+	clk_enable(obj->clk);
+
+	l.base = 0;
+	l.vict = 0;
+	iotlb_lock_set(obj, &l);
+
+	iommu_write_reg(obj, 1, MMU_GFLUSH);
+
+	clk_disable(obj->clk);
+}
+
+#if defined(CONFIG_OMAP_IOMMU_DEBUG) || defined(CONFIG_OMAP_IOMMU_DEBUG_MODULE)
+
+ssize_t omap_iommu_dump_ctx(struct omap_iommu *obj, char *buf, ssize_t bytes)
+{
+	if (!obj || !buf)
+		return -EINVAL;
+
+	clk_enable(obj->clk);
+
+	bytes = arch_iommu->dump_ctx(obj, buf, bytes);
+
+	clk_disable(obj->clk);
+
+	return bytes;
+}
+EXPORT_SYMBOL_GPL(omap_iommu_dump_ctx);
+
+static int
+__dump_tlb_entries(struct omap_iommu *obj, struct cr_regs *crs, int num)
+{
+	int i;
+	struct iotlb_lock saved;
+	struct cr_regs tmp;
+	struct cr_regs *p = crs;
+
+	clk_enable(obj->clk);
+	iotlb_lock_get(obj, &saved);
+
+	for_each_iotlb_cr(obj, num, i, tmp) {
+		if (!iotlb_cr_valid(&tmp))
+			continue;
+		*p++ = tmp;
+	}
+
+	iotlb_lock_set(obj, &saved);
+	clk_disable(obj->clk);
+
+	return  p - crs;
+}
+
+/**
+ * omap_dump_tlb_entries - dump cr arrays to given buffer
+ * @obj:	target iommu
+ * @buf:	output buffer
+ **/
+size_t omap_dump_tlb_entries(struct omap_iommu *obj, char *buf, ssize_t bytes)
+{
+	int i, num;
+	struct cr_regs *cr;
+	char *p = buf;
+
+	num = bytes / sizeof(*cr);
+	num = min(obj->nr_tlb_entries, num);
+
+	cr = kcalloc(num, sizeof(*cr), GFP_KERNEL);
+	if (!cr)
+		return 0;
+
+	num = __dump_tlb_entries(obj, cr, num);
+	for (i = 0; i < num; i++)
+		p += iotlb_dump_cr(obj, cr + i, p);
+	kfree(cr);
+
+	return p - buf;
+}
+EXPORT_SYMBOL_GPL(omap_dump_tlb_entries);
+
+int omap_foreach_iommu_device(void *data, int (*fn)(struct device *, void *))
+{
+	return driver_for_each_device(&omap_iommu_driver.driver,
+				      NULL, data, fn);
+}
+EXPORT_SYMBOL_GPL(omap_foreach_iommu_device);
+
+#endif /* CONFIG_OMAP_IOMMU_DEBUG_MODULE */
+
+/*
+ *	H/W pagetable operations
+ */
+static void flush_iopgd_range(u32 *first, u32 *last)
+{
+	/* FIXME: L2 cache should be taken care of if it exists */
+	do {
+		asm("mcr	p15, 0, %0, c7, c10, 1 @ flush_pgd"
+		    : : "r" (first));
+		first += L1_CACHE_BYTES / sizeof(*first);
+	} while (first <= last);
+}
+
+static void flush_iopte_range(u32 *first, u32 *last)
+{
+	/* FIXME: L2 cache should be taken care of if it exists */
+	do {
+		asm("mcr	p15, 0, %0, c7, c10, 1 @ flush_pte"
+		    : : "r" (first));
+		first += L1_CACHE_BYTES / sizeof(*first);
+	} while (first <= last);
+}
+
+static void iopte_free(u32 *iopte)
+{
+	/* Note: freed iopte's must be clean ready for re-use */
+	kmem_cache_free(iopte_cachep, iopte);
+}
+
+static u32 *iopte_alloc(struct omap_iommu *obj, u32 *iopgd, u32 da)
+{
+	u32 *iopte;
+
+	/* a table has already existed */
+	if (*iopgd)
+		goto pte_ready;
+
+	/*
+	 * do the allocation outside the page table lock
+	 */
+	spin_unlock(&obj->page_table_lock);
+	iopte = kmem_cache_zalloc(iopte_cachep, GFP_KERNEL);
+	spin_lock(&obj->page_table_lock);
+
+	if (!*iopgd) {
+		if (!iopte)
+			return ERR_PTR(-ENOMEM);
+
+		*iopgd = virt_to_phys(iopte) | IOPGD_TABLE;
+		flush_iopgd_range(iopgd, iopgd);
+
+		dev_vdbg(obj->dev, "%s: a new pte:%p\n", __func__, iopte);
+	} else {
+		/* We raced, free the reduniovant table */
+		iopte_free(iopte);
+	}
+
+pte_ready:
+	iopte = iopte_offset(iopgd, da);
+
+	dev_vdbg(obj->dev,
+		 "%s: da:%08x pgd:%p *pgd:%08x pte:%p *pte:%08x\n",
+		 __func__, da, iopgd, *iopgd, iopte, *iopte);
+
+	return iopte;
+}
+
+static int iopgd_alloc_section(struct omap_iommu *obj, u32 da, u32 pa, u32 prot)
+{
+	u32 *iopgd = iopgd_offset(obj, da);
+
+	if ((da | pa) & ~IOSECTION_MASK) {
+		dev_err(obj->dev, "%s: %08x:%08x should aligned on %08lx\n",
+			__func__, da, pa, IOSECTION_SIZE);
+		return -EINVAL;
+	}
+
+	*iopgd = (pa & IOSECTION_MASK) | prot | IOPGD_SECTION;
+	flush_iopgd_range(iopgd, iopgd);
+	return 0;
+}
+
+static int iopgd_alloc_super(struct omap_iommu *obj, u32 da, u32 pa, u32 prot)
+{
+	u32 *iopgd = iopgd_offset(obj, da);
+	int i;
+
+	if ((da | pa) & ~IOSUPER_MASK) {
+		dev_err(obj->dev, "%s: %08x:%08x should aligned on %08lx\n",
+			__func__, da, pa, IOSUPER_SIZE);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < 16; i++)
+		*(iopgd + i) = (pa & IOSUPER_MASK) | prot | IOPGD_SUPER;
+	flush_iopgd_range(iopgd, iopgd + 15);
+	return 0;
+}
+
+static int iopte_alloc_page(struct omap_iommu *obj, u32 da, u32 pa, u32 prot)
+{
+	u32 *iopgd = iopgd_offset(obj, da);
+	u32 *iopte = iopte_alloc(obj, iopgd, da);
+
+	if (IS_ERR(iopte))
+		return PTR_ERR(iopte);
+
+	*iopte = (pa & IOPAGE_MASK) | prot | IOPTE_SMALL;
+	flush_iopte_range(iopte, iopte);
+
+	dev_vdbg(obj->dev, "%s: da:%08x pa:%08x pte:%p *pte:%08x\n",
+		 __func__, da, pa, iopte, *iopte);
+
+	return 0;
+}
+
+static int iopte_alloc_large(struct omap_iommu *obj, u32 da, u32 pa, u32 prot)
+{
+	u32 *iopgd = iopgd_offset(obj, da);
+	u32 *iopte = iopte_alloc(obj, iopgd, da);
+	int i;
+
+	if ((da | pa) & ~IOLARGE_MASK) {
+		dev_err(obj->dev, "%s: %08x:%08x should aligned on %08lx\n",
+			__func__, da, pa, IOLARGE_SIZE);
+		return -EINVAL;
+	}
+
+	if (IS_ERR(iopte))
+		return PTR_ERR(iopte);
+
+	for (i = 0; i < 16; i++)
+		*(iopte + i) = (pa & IOLARGE_MASK) | prot | IOPTE_LARGE;
+	flush_iopte_range(iopte, iopte + 15);
+	return 0;
+}
+
+static int
+iopgtable_store_entry_core(struct omap_iommu *obj, struct iotlb_entry *e)
+{
+	int (*fn)(struct omap_iommu *, u32, u32, u32);
+	u32 prot;
+	int err;
+
+	if (!obj || !e)
+		return -EINVAL;
+
+	switch (e->pgsz) {
+	case MMU_CAM_PGSZ_16M:
+		fn = iopgd_alloc_super;
+		break;
+	case MMU_CAM_PGSZ_1M:
+		fn = iopgd_alloc_section;
+		break;
+	case MMU_CAM_PGSZ_64K:
+		fn = iopte_alloc_large;
+		break;
+	case MMU_CAM_PGSZ_4K:
+		fn = iopte_alloc_page;
+		break;
+	default:
+		fn = NULL;
+		BUG();
+		break;
+	}
+
+	prot = get_iopte_attr(e);
+
+	spin_lock(&obj->page_table_lock);
+	err = fn(obj, e->da, e->pa, prot);
+	spin_unlock(&obj->page_table_lock);
+
+	return err;
+}
+
+/**
+ * omap_iopgtable_store_entry - Make an iommu pte entry
+ * @obj:	target iommu
+ * @e:		an iommu tlb entry info
+ **/
+int omap_iopgtable_store_entry(struct omap_iommu *obj, struct iotlb_entry *e)
+{
+	int err;
+
+	flush_iotlb_page(obj, e->da);
+	err = iopgtable_store_entry_core(obj, e);
+	if (!err)
+		prefetch_iotlb_entry(obj, e);
+	return err;
+}
+EXPORT_SYMBOL_GPL(omap_iopgtable_store_entry);
+
+/**
+ * iopgtable_lookup_entry - Lookup an iommu pte entry
+ * @obj:	target iommu
+ * @da:		iommu device virtual address
+ * @ppgd:	iommu pgd entry pointer to be returned
+ * @ppte:	iommu pte entry pointer to be returned
+ **/
+static void
+iopgtable_lookup_entry(struct omap_iommu *obj, u32 da, u32 **ppgd, u32 **ppte)
+{
+	u32 *iopgd, *iopte = NULL;
+
+	iopgd = iopgd_offset(obj, da);
+	if (!*iopgd)
+		goto out;
+
+	if (iopgd_is_table(*iopgd))
+		iopte = iopte_offset(iopgd, da);
+out:
+	*ppgd = iopgd;
+	*ppte = iopte;
+}
+
+static size_t iopgtable_clear_entry_core(struct omap_iommu *obj, u32 da)
+{
+	size_t bytes;
+	u32 *iopgd = iopgd_offset(obj, da);
+	int nent = 1;
+
+	if (!*iopgd)
+		return 0;
+
+	if (iopgd_is_table(*iopgd)) {
+		int i;
+		u32 *iopte = iopte_offset(iopgd, da);
+
+		bytes = IOPTE_SIZE;
+		if (*iopte & IOPTE_LARGE) {
+			nent *= 16;
+			/* rewind to the 1st entry */
+			iopte = iopte_offset(iopgd, (da & IOLARGE_MASK));
+		}
+		bytes *= nent;
+		memset(iopte, 0, nent * sizeof(*iopte));
+		flush_iopte_range(iopte, iopte + (nent - 1) * sizeof(*iopte));
+
+		/*
+		 * do table walk to check if this table is necessary or not
+		 */
+		iopte = iopte_offset(iopgd, 0);
+		for (i = 0; i < PTRS_PER_IOPTE; i++)
+			if (iopte[i])
+				goto out;
+
+		iopte_free(iopte);
+		nent = 1; /* for the next L1 entry */
+	} else {
+		bytes = IOPGD_SIZE;
+		if ((*iopgd & IOPGD_SUPER) == IOPGD_SUPER) {
+			nent *= 16;
+			/* rewind to the 1st entry */
+			iopgd = iopgd_offset(obj, (da & IOSUPER_MASK));
+		}
+		bytes *= nent;
+	}
+	memset(iopgd, 0, nent * sizeof(*iopgd));
+	flush_iopgd_range(iopgd, iopgd + (nent - 1) * sizeof(*iopgd));
+out:
+	return bytes;
+}
+
+/**
+ * iopgtable_clear_entry - Remove an iommu pte entry
+ * @obj:	target iommu
+ * @da:		iommu device virtual address
+ **/
+static size_t iopgtable_clear_entry(struct omap_iommu *obj, u32 da)
+{
+	size_t bytes;
+
+	spin_lock(&obj->page_table_lock);
+
+	bytes = iopgtable_clear_entry_core(obj, da);
+	flush_iotlb_page(obj, da);
+
+	spin_unlock(&obj->page_table_lock);
+
+	return bytes;
+}
+
+static void iopgtable_clear_entry_all(struct omap_iommu *obj)
+{
+	int i;
+
+	spin_lock(&obj->page_table_lock);
+
+	for (i = 0; i < PTRS_PER_IOPGD; i++) {
+		u32 da;
+		u32 *iopgd;
+
+		da = i << IOPGD_SHIFT;
+		iopgd = iopgd_offset(obj, da);
+
+		if (!*iopgd)
+			continue;
+
+		if (iopgd_is_table(*iopgd))
+			iopte_free(iopte_offset(iopgd, 0));
+
+		*iopgd = 0;
+		flush_iopgd_range(iopgd, iopgd);
+	}
+
+	flush_iotlb_all(obj);
+
+	spin_unlock(&obj->page_table_lock);
+}
+
+/*
+ *	Device IOMMU generic operations
+ */
+static irqreturn_t iommu_fault_handler(int irq, void *data)
+{
+	u32 da, errs;
+	u32 *iopgd, *iopte;
+	struct omap_iommu *obj = data;
+	struct iommu_domain *domain = obj->domain;
+
+	if (!obj->refcount)
+		return IRQ_NONE;
+
+	clk_enable(obj->clk);
+	errs = iommu_report_fault(obj, &da);
+	clk_disable(obj->clk);
+	if (errs == 0)
+		return IRQ_HANDLED;
+
+	/* Fault callback or TLB/PTE Dynamic loading */
+	if (!report_iommu_fault(domain, obj->dev, da, 0))
+		return IRQ_HANDLED;
+
+	iommu_disable(obj);
+
+	iopgd = iopgd_offset(obj, da);
+
+	if (!iopgd_is_table(*iopgd)) {
+		dev_err(obj->dev, "%s: errs:0x%08x da:0x%08x pgd:0x%p "
+			"*pgd:px%08x\n", obj->name, errs, da, iopgd, *iopgd);
+		return IRQ_NONE;
+	}
+
+	iopte = iopte_offset(iopgd, da);
+
+	dev_err(obj->dev, "%s: errs:0x%08x da:0x%08x pgd:0x%p *pgd:0x%08x "
+		"pte:0x%p *pte:0x%08x\n", obj->name, errs, da, iopgd, *iopgd,
+		iopte, *iopte);
+
+	return IRQ_NONE;
+}
+
+static int device_match_by_alias(struct device *dev, void *data)
+{
+	struct omap_iommu *obj = to_iommu(dev);
+	const char *name = data;
+
+	pr_debug("%s: %s %s\n", __func__, obj->name, name);
+
+	return strcmp(obj->name, name) == 0;
+}
+
+/**
+ * omap_iommu_attach() - attach iommu device to an iommu domain
+ * @name:	name of target omap iommu device
+ * @iopgd:	page table
+ **/
+static struct omap_iommu *omap_iommu_attach(const char *name, u32 *iopgd)
+{
+	int err = -ENOMEM;
+	struct device *dev;
+	struct omap_iommu *obj;
+
+	dev = driver_find_device(&omap_iommu_driver.driver, NULL,
+				(void *)name,
+				device_match_by_alias);
+	if (!dev)
+		return NULL;
+
+	obj = to_iommu(dev);
+
+	spin_lock(&obj->iommu_lock);
+
+	/* an iommu device can only be attached once */
+	if (++obj->refcount > 1) {
+		dev_err(dev, "%s: already attached!\n", obj->name);
+		err = -EBUSY;
+		goto err_enable;
+	}
+
+	obj->iopgd = iopgd;
+	err = iommu_enable(obj);
+	if (err)
+		goto err_enable;
+	flush_iotlb_all(obj);
+
+	if (!try_module_get(obj->owner))
+		goto err_module;
+
+	spin_unlock(&obj->iommu_lock);
+
+	dev_dbg(obj->dev, "%s: %s\n", __func__, obj->name);
+	return obj;
+
+err_module:
+	if (obj->refcount == 1)
+		iommu_disable(obj);
+err_enable:
+	obj->refcount--;
+	spin_unlock(&obj->iommu_lock);
+	return ERR_PTR(err);
+}
+
+/**
+ * omap_iommu_detach - release iommu device
+ * @obj:	target iommu
+ **/
+static void omap_iommu_detach(struct omap_iommu *obj)
+{
+	if (!obj || IS_ERR(obj))
+		return;
+
+	spin_lock(&obj->iommu_lock);
+
+	if (--obj->refcount == 0)
+		iommu_disable(obj);
+
+	module_put(obj->owner);
+
+	obj->iopgd = NULL;
+
+	spin_unlock(&obj->iommu_lock);
+
+	dev_dbg(obj->dev, "%s: %s\n", __func__, obj->name);
+}
+
+/*
+ *	OMAP Device MMU(IOMMU) detection
+ */
+static int __devinit omap_iommu_probe(struct platform_device *pdev)
+{
+	int err = -ENODEV;
+	int irq;
+	struct omap_iommu *obj;
+	struct resource *res;
+	struct iommu_platform_data *pdata = pdev->dev.platform_data;
+
+	if (pdev->num_resources != 2)
+		return -EINVAL;
+
+	obj = kzalloc(sizeof(*obj) + MMU_REG_SIZE, GFP_KERNEL);
+	if (!obj)
+		return -ENOMEM;
+
+	obj->clk = clk_get(&pdev->dev, pdata->clk_name);
+	if (IS_ERR(obj->clk))
+		goto err_clk;
+
+	obj->nr_tlb_entries = pdata->nr_tlb_entries;
+	obj->name = pdata->name;
+	obj->dev = &pdev->dev;
+	obj->ctx = (void *)obj + sizeof(*obj);
+	obj->da_start = pdata->da_start;
+	obj->da_end = pdata->da_end;
+
+	spin_lock_init(&obj->iommu_lock);
+	mutex_init(&obj->mmap_lock);
+	spin_lock_init(&obj->page_table_lock);
+	INIT_LIST_HEAD(&obj->mmap);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		err = -ENODEV;
+		goto err_mem;
+	}
+
+	res = request_mem_region(res->start, resource_size(res),
+				 dev_name(&pdev->dev));
+	if (!res) {
+		err = -EIO;
+		goto err_mem;
+	}
+
+	obj->regbase = ioremap(res->start, resource_size(res));
+	if (!obj->regbase) {
+		err = -ENOMEM;
+		goto err_ioremap;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		err = -ENODEV;
+		goto err_irq;
+	}
+	err = request_irq(irq, iommu_fault_handler, IRQF_SHARED,
+			  dev_name(&pdev->dev), obj);
+	if (err < 0)
+		goto err_irq;
+	platform_set_drvdata(pdev, obj);
+
+	dev_info(&pdev->dev, "%s registered\n", obj->name);
+	return 0;
+
+err_irq:
+	iounmap(obj->regbase);
+err_ioremap:
+	release_mem_region(res->start, resource_size(res));
+err_mem:
+	clk_put(obj->clk);
+err_clk:
+	kfree(obj);
+	return err;
+}
+
+static int __devexit omap_iommu_remove(struct platform_device *pdev)
+{
+	int irq;
+	struct resource *res;
+	struct omap_iommu *obj = platform_get_drvdata(pdev);
+
+	platform_set_drvdata(pdev, NULL);
+
+	iopgtable_clear_entry_all(obj);
+
+	irq = platform_get_irq(pdev, 0);
+	free_irq(irq, obj);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	release_mem_region(res->start, resource_size(res));
+	iounmap(obj->regbase);
+
+	clk_put(obj->clk);
+	dev_info(&pdev->dev, "%s removed\n", obj->name);
+	kfree(obj);
+	return 0;
+}
+
+static struct platform_driver omap_iommu_driver = {
+	.probe	= omap_iommu_probe,
+	.remove	= __devexit_p(omap_iommu_remove),
+	.driver	= {
+		.name	= "omap-iommu",
+	},
+};
+
+static void iopte_cachep_ctor(void *iopte)
+{
+	clean_dcache_area(iopte, IOPTE_TABLE_SIZE);
+}
+
+static int omap_iommu_map(struct iommu_domain *domain, unsigned long da,
+			 phys_addr_t pa, size_t bytes, int prot)
+{
+	struct omap_iommu_domain *omap_domain = domain->priv;
+	struct omap_iommu *oiommu = omap_domain->iommu_dev;
+	struct device *dev = oiommu->dev;
+	struct iotlb_entry e;
+	int omap_pgsz;
+	u32 ret, flags;
+
+	/* we only support mapping a single iommu page for now */
+	omap_pgsz = bytes_to_iopgsz(bytes);
+	if (omap_pgsz < 0) {
+		dev_err(dev, "invalid size to map: %d\n", bytes);
+		return -EINVAL;
+	}
+
+	dev_dbg(dev, "mapping da 0x%lx to pa 0x%x size 0x%x\n", da, pa, bytes);
+
+	flags = omap_pgsz | prot;
+
+	iotlb_init_entry(&e, da, pa, flags);
+
+	ret = omap_iopgtable_store_entry(oiommu, &e);
+	if (ret)
+		dev_err(dev, "omap_iopgtable_store_entry failed: %d\n", ret);
+
+	return ret;
+}
+
+static size_t omap_iommu_unmap(struct iommu_domain *domain, unsigned long da,
+			    size_t size)
+{
+	struct omap_iommu_domain *omap_domain = domain->priv;
+	struct omap_iommu *oiommu = omap_domain->iommu_dev;
+	struct device *dev = oiommu->dev;
+
+	dev_dbg(dev, "unmapping da 0x%lx size %u\n", da, size);
+
+	return iopgtable_clear_entry(oiommu, da);
+}
+
+static int
+omap_iommu_attach_dev(struct iommu_domain *domain, struct device *dev)
+{
+	struct omap_iommu_domain *omap_domain = domain->priv;
+	struct omap_iommu *oiommu;
+	struct omap_iommu_arch_data *arch_data = dev->archdata.iommu;
+	int ret = 0;
+
+	spin_lock(&omap_domain->lock);
+
+	/* only a single device is supported per domain for now */
+	if (omap_domain->iommu_dev) {
+		dev_err(dev, "iommu domain is already attached\n");
+		ret = -EBUSY;
+		goto out;
+	}
+
+	/* get a handle to and enable the omap iommu */
+	oiommu = omap_iommu_attach(arch_data->name, omap_domain->pgtable);
+	if (IS_ERR(oiommu)) {
+		ret = PTR_ERR(oiommu);
+		dev_err(dev, "can't get omap iommu: %d\n", ret);
+		goto out;
+	}
+
+	omap_domain->iommu_dev = arch_data->iommu_dev = oiommu;
+	oiommu->domain = domain;
+
+out:
+	spin_unlock(&omap_domain->lock);
+	return ret;
+}
+
+static void omap_iommu_detach_dev(struct iommu_domain *domain,
+				 struct device *dev)
+{
+	struct omap_iommu_domain *omap_domain = domain->priv;
+	struct omap_iommu_arch_data *arch_data = dev->archdata.iommu;
+	struct omap_iommu *oiommu = dev_to_omap_iommu(dev);
+
+	spin_lock(&omap_domain->lock);
+
+	/* only a single device is supported per domain for now */
+	if (omap_domain->iommu_dev != oiommu) {
+		dev_err(dev, "invalid iommu device\n");
+		goto out;
+	}
+
+	iopgtable_clear_entry_all(oiommu);
+
+	omap_iommu_detach(oiommu);
+
+	omap_domain->iommu_dev = arch_data->iommu_dev = NULL;
+
+out:
+	spin_unlock(&omap_domain->lock);
+}
+
+static int omap_iommu_domain_init(struct iommu_domain *domain)
+{
+	struct omap_iommu_domain *omap_domain;
+
+	omap_domain = kzalloc(sizeof(*omap_domain), GFP_KERNEL);
+	if (!omap_domain) {
+		pr_err("kzalloc failed\n");
+		goto out;
+	}
+
+	omap_domain->pgtable = kzalloc(IOPGD_TABLE_SIZE, GFP_KERNEL);
+	if (!omap_domain->pgtable) {
+		pr_err("kzalloc failed\n");
+		goto fail_nomem;
+	}
+
+	/*
+	 * should never fail, but please keep this around to ensure
+	 * we keep the hardware happy
+	 */
+	BUG_ON(!IS_ALIGNED((long)omap_domain->pgtable, IOPGD_TABLE_SIZE));
+
+	clean_dcache_area(omap_domain->pgtable, IOPGD_TABLE_SIZE);
+	spin_lock_init(&omap_domain->lock);
+
+	domain->priv = omap_domain;
+
+	return 0;
+
+fail_nomem:
+	kfree(omap_domain);
+out:
+	return -ENOMEM;
+}
+
+/* assume device was already detached */
+static void omap_iommu_domain_destroy(struct iommu_domain *domain)
+{
+	struct omap_iommu_domain *omap_domain = domain->priv;
+
+	domain->priv = NULL;
+
+	kfree(omap_domain->pgtable);
+	kfree(omap_domain);
+}
+
+static phys_addr_t omap_iommu_iova_to_phys(struct iommu_domain *domain,
+					  unsigned long da)
+{
+	struct omap_iommu_domain *omap_domain = domain->priv;
+	struct omap_iommu *oiommu = omap_domain->iommu_dev;
+	struct device *dev = oiommu->dev;
+	u32 *pgd, *pte;
+	phys_addr_t ret = 0;
+
+	iopgtable_lookup_entry(oiommu, da, &pgd, &pte);
+
+	if (pte) {
+		if (iopte_is_small(*pte))
+			ret = omap_iommu_translate(*pte, da, IOPTE_MASK);
+		else if (iopte_is_large(*pte))
+			ret = omap_iommu_translate(*pte, da, IOLARGE_MASK);
+		else
+			dev_err(dev, "bogus pte 0x%x, da 0x%lx", *pte, da);
+	} else {
+		if (iopgd_is_section(*pgd))
+			ret = omap_iommu_translate(*pgd, da, IOSECTION_MASK);
+		else if (iopgd_is_super(*pgd))
+			ret = omap_iommu_translate(*pgd, da, IOSUPER_MASK);
+		else
+			dev_err(dev, "bogus pgd 0x%x, da 0x%lx", *pgd, da);
+	}
+
+	return ret;
+}
+
+static int omap_iommu_domain_has_cap(struct iommu_domain *domain,
+				    unsigned long cap)
+{
+	return 0;
+}
+
+static struct iommu_ops omap_iommu_ops = {
+	.domain_init	= omap_iommu_domain_init,
+	.domain_destroy	= omap_iommu_domain_destroy,
+	.attach_dev	= omap_iommu_attach_dev,
+	.detach_dev	= omap_iommu_detach_dev,
+	.map		= omap_iommu_map,
+	.unmap		= omap_iommu_unmap,
+	.iova_to_phys	= omap_iommu_iova_to_phys,
+	.domain_has_cap	= omap_iommu_domain_has_cap,
+	.pgsize_bitmap	= OMAP_IOMMU_PGSIZES,
+};
+
+static int __init omap_iommu_init(void)
+{
+	struct kmem_cache *p;
+	const unsigned long flags = SLAB_HWCACHE_ALIGN;
+	size_t align = 1 << 10; /* L2 pagetable alignement */
+
+	p = kmem_cache_create("iopte_cache", IOPTE_TABLE_SIZE, align, flags,
+			      iopte_cachep_ctor);
+	if (!p)
+		return -ENOMEM;
+	iopte_cachep = p;
+
+	bus_set_iommu(&platform_bus_type, &omap_iommu_ops);
+
+	return platform_driver_register(&omap_iommu_driver);
+}
+module_init(omap_iommu_init);
+
+static void __exit omap_iommu_exit(void)
+{
+	kmem_cache_destroy(iopte_cachep);
+
+	platform_driver_unregister(&omap_iommu_driver);
+}
+module_exit(omap_iommu_exit);
+
+MODULE_DESCRIPTION("omap iommu: tlb and pagetable primitives");
+MODULE_ALIAS("platform:omap-iommu");
+MODULE_AUTHOR("Hiroshi DOYU, Paul Mundt and Toshihiro Kobayashi");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iommu/omap-iovmm.c b/drivers/iommu/omap-iovmm.c
new file mode 100644
index 000000000000..2e10c3e0a7ae
--- /dev/null
+++ b/drivers/iommu/omap-iovmm.c
@@ -0,0 +1,747 @@
+/*
+ * omap iommu: simple virtual address space management
+ *
+ * Copyright (C) 2008-2009 Nokia Corporation
+ *
+ * Written by Hiroshi DOYU <Hiroshi.DOYU@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/device.h>
+#include <linux/scatterlist.h>
+#include <linux/iommu.h>
+
+#include <asm/cacheflush.h>
+#include <asm/mach/map.h>
+
+#include <plat/iommu.h>
+#include <plat/iovmm.h>
+
+#include <plat/iopgtable.h>
+
+static struct kmem_cache *iovm_area_cachep;
+
+/* return the offset of the first scatterlist entry in a sg table */
+static unsigned int sgtable_offset(const struct sg_table *sgt)
+{
+	if (!sgt || !sgt->nents)
+		return 0;
+
+	return sgt->sgl->offset;
+}
+
+/* return total bytes of sg buffers */
+static size_t sgtable_len(const struct sg_table *sgt)
+{
+	unsigned int i, total = 0;
+	struct scatterlist *sg;
+
+	if (!sgt)
+		return 0;
+
+	for_each_sg(sgt->sgl, sg, sgt->nents, i) {
+		size_t bytes;
+
+		bytes = sg->length + sg->offset;
+
+		if (!iopgsz_ok(bytes)) {
+			pr_err("%s: sg[%d] not iommu pagesize(%u %u)\n",
+			       __func__, i, bytes, sg->offset);
+			return 0;
+		}
+
+		if (i && sg->offset) {
+			pr_err("%s: sg[%d] offset not allowed in internal "
+					"entries\n", __func__, i);
+			return 0;
+		}
+
+		total += bytes;
+	}
+
+	return total;
+}
+#define sgtable_ok(x)	(!!sgtable_len(x))
+
+static unsigned max_alignment(u32 addr)
+{
+	int i;
+	unsigned pagesize[] = { SZ_16M, SZ_1M, SZ_64K, SZ_4K, };
+	for (i = 0; i < ARRAY_SIZE(pagesize) && addr & (pagesize[i] - 1); i++)
+		;
+	return (i < ARRAY_SIZE(pagesize)) ? pagesize[i] : 0;
+}
+
+/*
+ * calculate the optimal number sg elements from total bytes based on
+ * iommu superpages
+ */
+static unsigned sgtable_nents(size_t bytes, u32 da, u32 pa)
+{
+	unsigned nr_entries = 0, ent_sz;
+
+	if (!IS_ALIGNED(bytes, PAGE_SIZE)) {
+		pr_err("%s: wrong size %08x\n", __func__, bytes);
+		return 0;
+	}
+
+	while (bytes) {
+		ent_sz = max_alignment(da | pa);
+		ent_sz = min_t(unsigned, ent_sz, iopgsz_max(bytes));
+		nr_entries++;
+		da += ent_sz;
+		pa += ent_sz;
+		bytes -= ent_sz;
+	}
+
+	return nr_entries;
+}
+
+/* allocate and initialize sg_table header(a kind of 'superblock') */
+static struct sg_table *sgtable_alloc(const size_t bytes, u32 flags,
+							u32 da, u32 pa)
+{
+	unsigned int nr_entries;
+	int err;
+	struct sg_table *sgt;
+
+	if (!bytes)
+		return ERR_PTR(-EINVAL);
+
+	if (!IS_ALIGNED(bytes, PAGE_SIZE))
+		return ERR_PTR(-EINVAL);
+
+	if (flags & IOVMF_LINEAR) {
+		nr_entries = sgtable_nents(bytes, da, pa);
+		if (!nr_entries)
+			return ERR_PTR(-EINVAL);
+	} else
+		nr_entries =  bytes / PAGE_SIZE;
+
+	sgt = kzalloc(sizeof(*sgt), GFP_KERNEL);
+	if (!sgt)
+		return ERR_PTR(-ENOMEM);
+
+	err = sg_alloc_table(sgt, nr_entries, GFP_KERNEL);
+	if (err) {
+		kfree(sgt);
+		return ERR_PTR(err);
+	}
+
+	pr_debug("%s: sgt:%p(%d entries)\n", __func__, sgt, nr_entries);
+
+	return sgt;
+}
+
+/* free sg_table header(a kind of superblock) */
+static void sgtable_free(struct sg_table *sgt)
+{
+	if (!sgt)
+		return;
+
+	sg_free_table(sgt);
+	kfree(sgt);
+
+	pr_debug("%s: sgt:%p\n", __func__, sgt);
+}
+
+/* map 'sglist' to a contiguous mpu virtual area and return 'va' */
+static void *vmap_sg(const struct sg_table *sgt)
+{
+	u32 va;
+	size_t total;
+	unsigned int i;
+	struct scatterlist *sg;
+	struct vm_struct *new;
+	const struct mem_type *mtype;
+
+	mtype = get_mem_type(MT_DEVICE);
+	if (!mtype)
+		return ERR_PTR(-EINVAL);
+
+	total = sgtable_len(sgt);
+	if (!total)
+		return ERR_PTR(-EINVAL);
+
+	new = __get_vm_area(total, VM_IOREMAP, VMALLOC_START, VMALLOC_END);
+	if (!new)
+		return ERR_PTR(-ENOMEM);
+	va = (u32)new->addr;
+
+	for_each_sg(sgt->sgl, sg, sgt->nents, i) {
+		size_t bytes;
+		u32 pa;
+		int err;
+
+		pa = sg_phys(sg) - sg->offset;
+		bytes = sg->length + sg->offset;
+
+		BUG_ON(bytes != PAGE_SIZE);
+
+		err = ioremap_page(va,  pa, mtype);
+		if (err)
+			goto err_out;
+
+		va += bytes;
+	}
+
+	flush_cache_vmap((unsigned long)new->addr,
+				(unsigned long)(new->addr + total));
+	return new->addr;
+
+err_out:
+	WARN_ON(1); /* FIXME: cleanup some mpu mappings */
+	vunmap(new->addr);
+	return ERR_PTR(-EAGAIN);
+}
+
+static inline void vunmap_sg(const void *va)
+{
+	vunmap(va);
+}
+
+static struct iovm_struct *__find_iovm_area(struct omap_iommu *obj,
+							const u32 da)
+{
+	struct iovm_struct *tmp;
+
+	list_for_each_entry(tmp, &obj->mmap, list) {
+		if ((da >= tmp->da_start) && (da < tmp->da_end)) {
+			size_t len;
+
+			len = tmp->da_end - tmp->da_start;
+
+			dev_dbg(obj->dev, "%s: %08x-%08x-%08x(%x) %08x\n",
+				__func__, tmp->da_start, da, tmp->da_end, len,
+				tmp->flags);
+
+			return tmp;
+		}
+	}
+
+	return NULL;
+}
+
+/**
+ * omap_find_iovm_area  -  find iovma which includes @da
+ * @dev:	client device
+ * @da:		iommu device virtual address
+ *
+ * Find the existing iovma starting at @da
+ */
+struct iovm_struct *omap_find_iovm_area(struct device *dev, u32 da)
+{
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
+	struct iovm_struct *area;
+
+	mutex_lock(&obj->mmap_lock);
+	area = __find_iovm_area(obj, da);
+	mutex_unlock(&obj->mmap_lock);
+
+	return area;
+}
+EXPORT_SYMBOL_GPL(omap_find_iovm_area);
+
+/*
+ * This finds the hole(area) which fits the requested address and len
+ * in iovmas mmap, and returns the new allocated iovma.
+ */
+static struct iovm_struct *alloc_iovm_area(struct omap_iommu *obj, u32 da,
+					   size_t bytes, u32 flags)
+{
+	struct iovm_struct *new, *tmp;
+	u32 start, prev_end, alignment;
+
+	if (!obj || !bytes)
+		return ERR_PTR(-EINVAL);
+
+	start = da;
+	alignment = PAGE_SIZE;
+
+	if (~flags & IOVMF_DA_FIXED) {
+		/* Don't map address 0 */
+		start = obj->da_start ? obj->da_start : alignment;
+
+		if (flags & IOVMF_LINEAR)
+			alignment = iopgsz_max(bytes);
+		start = roundup(start, alignment);
+	} else if (start < obj->da_start || start > obj->da_end ||
+					obj->da_end - start < bytes) {
+		return ERR_PTR(-EINVAL);
+	}
+
+	tmp = NULL;
+	if (list_empty(&obj->mmap))
+		goto found;
+
+	prev_end = 0;
+	list_for_each_entry(tmp, &obj->mmap, list) {
+
+		if (prev_end > start)
+			break;
+
+		if (tmp->da_start > start && (tmp->da_start - start) >= bytes)
+			goto found;
+
+		if (tmp->da_end >= start && ~flags & IOVMF_DA_FIXED)
+			start = roundup(tmp->da_end + 1, alignment);
+
+		prev_end = tmp->da_end;
+	}
+
+	if ((start >= prev_end) && (obj->da_end - start >= bytes))
+		goto found;
+
+	dev_dbg(obj->dev, "%s: no space to fit %08x(%x) flags: %08x\n",
+		__func__, da, bytes, flags);
+
+	return ERR_PTR(-EINVAL);
+
+found:
+	new = kmem_cache_zalloc(iovm_area_cachep, GFP_KERNEL);
+	if (!new)
+		return ERR_PTR(-ENOMEM);
+
+	new->iommu = obj;
+	new->da_start = start;
+	new->da_end = start + bytes;
+	new->flags = flags;
+
+	/*
+	 * keep ascending order of iovmas
+	 */
+	if (tmp)
+		list_add_tail(&new->list, &tmp->list);
+	else
+		list_add(&new->list, &obj->mmap);
+
+	dev_dbg(obj->dev, "%s: found %08x-%08x-%08x(%x) %08x\n",
+		__func__, new->da_start, start, new->da_end, bytes, flags);
+
+	return new;
+}
+
+static void free_iovm_area(struct omap_iommu *obj, struct iovm_struct *area)
+{
+	size_t bytes;
+
+	BUG_ON(!obj || !area);
+
+	bytes = area->da_end - area->da_start;
+
+	dev_dbg(obj->dev, "%s: %08x-%08x(%x) %08x\n",
+		__func__, area->da_start, area->da_end, bytes, area->flags);
+
+	list_del(&area->list);
+	kmem_cache_free(iovm_area_cachep, area);
+}
+
+/**
+ * omap_da_to_va - convert (d) to (v)
+ * @dev:	client device
+ * @da:		iommu device virtual address
+ * @va:		mpu virtual address
+ *
+ * Returns mpu virtual addr which corresponds to a given device virtual addr
+ */
+void *omap_da_to_va(struct device *dev, u32 da)
+{
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
+	void *va = NULL;
+	struct iovm_struct *area;
+
+	mutex_lock(&obj->mmap_lock);
+
+	area = __find_iovm_area(obj, da);
+	if (!area) {
+		dev_dbg(obj->dev, "%s: no da area(%08x)\n", __func__, da);
+		goto out;
+	}
+	va = area->va;
+out:
+	mutex_unlock(&obj->mmap_lock);
+
+	return va;
+}
+EXPORT_SYMBOL_GPL(omap_da_to_va);
+
+static void sgtable_fill_vmalloc(struct sg_table *sgt, void *_va)
+{
+	unsigned int i;
+	struct scatterlist *sg;
+	void *va = _va;
+	void *va_end;
+
+	for_each_sg(sgt->sgl, sg, sgt->nents, i) {
+		struct page *pg;
+		const size_t bytes = PAGE_SIZE;
+
+		/*
+		 * iommu 'superpage' isn't supported with 'omap_iommu_vmalloc()'
+		 */
+		pg = vmalloc_to_page(va);
+		BUG_ON(!pg);
+		sg_set_page(sg, pg, bytes, 0);
+
+		va += bytes;
+	}
+
+	va_end = _va + PAGE_SIZE * i;
+}
+
+static inline void sgtable_drain_vmalloc(struct sg_table *sgt)
+{
+	/*
+	 * Actually this is not necessary at all, just exists for
+	 * consistency of the code readability.
+	 */
+	BUG_ON(!sgt);
+}
+
+/* create 'da' <-> 'pa' mapping from 'sgt' */
+static int map_iovm_area(struct iommu_domain *domain, struct iovm_struct *new,
+			const struct sg_table *sgt, u32 flags)
+{
+	int err;
+	unsigned int i, j;
+	struct scatterlist *sg;
+	u32 da = new->da_start;
+
+	if (!domain || !sgt)
+		return -EINVAL;
+
+	BUG_ON(!sgtable_ok(sgt));
+
+	for_each_sg(sgt->sgl, sg, sgt->nents, i) {
+		u32 pa;
+		size_t bytes;
+
+		pa = sg_phys(sg) - sg->offset;
+		bytes = sg->length + sg->offset;
+
+		flags &= ~IOVMF_PGSZ_MASK;
+
+		if (bytes_to_iopgsz(bytes) < 0)
+			goto err_out;
+
+		pr_debug("%s: [%d] %08x %08x(%x)\n", __func__,
+			 i, da, pa, bytes);
+
+		err = iommu_map(domain, da, pa, bytes, flags);
+		if (err)
+			goto err_out;
+
+		da += bytes;
+	}
+	return 0;
+
+err_out:
+	da = new->da_start;
+
+	for_each_sg(sgt->sgl, sg, i, j) {
+		size_t bytes;
+
+		bytes = sg->length + sg->offset;
+
+		/* ignore failures.. we're already handling one */
+		iommu_unmap(domain, da, bytes);
+
+		da += bytes;
+	}
+	return err;
+}
+
+/* release 'da' <-> 'pa' mapping */
+static void unmap_iovm_area(struct iommu_domain *domain, struct omap_iommu *obj,
+						struct iovm_struct *area)
+{
+	u32 start;
+	size_t total = area->da_end - area->da_start;
+	const struct sg_table *sgt = area->sgt;
+	struct scatterlist *sg;
+	int i;
+	size_t unmapped;
+
+	BUG_ON(!sgtable_ok(sgt));
+	BUG_ON((!total) || !IS_ALIGNED(total, PAGE_SIZE));
+
+	start = area->da_start;
+	for_each_sg(sgt->sgl, sg, sgt->nents, i) {
+		size_t bytes;
+
+		bytes = sg->length + sg->offset;
+
+		unmapped = iommu_unmap(domain, start, bytes);
+		if (unmapped < bytes)
+			break;
+
+		dev_dbg(obj->dev, "%s: unmap %08x(%x) %08x\n",
+				__func__, start, bytes, area->flags);
+
+		BUG_ON(!IS_ALIGNED(bytes, PAGE_SIZE));
+
+		total -= bytes;
+		start += bytes;
+	}
+	BUG_ON(total);
+}
+
+/* template function for all unmapping */
+static struct sg_table *unmap_vm_area(struct iommu_domain *domain,
+				      struct omap_iommu *obj, const u32 da,
+				      void (*fn)(const void *), u32 flags)
+{
+	struct sg_table *sgt = NULL;
+	struct iovm_struct *area;
+
+	if (!IS_ALIGNED(da, PAGE_SIZE)) {
+		dev_err(obj->dev, "%s: alignment err(%08x)\n", __func__, da);
+		return NULL;
+	}
+
+	mutex_lock(&obj->mmap_lock);
+
+	area = __find_iovm_area(obj, da);
+	if (!area) {
+		dev_dbg(obj->dev, "%s: no da area(%08x)\n", __func__, da);
+		goto out;
+	}
+
+	if ((area->flags & flags) != flags) {
+		dev_err(obj->dev, "%s: wrong flags(%08x)\n", __func__,
+			area->flags);
+		goto out;
+	}
+	sgt = (struct sg_table *)area->sgt;
+
+	unmap_iovm_area(domain, obj, area);
+
+	fn(area->va);
+
+	dev_dbg(obj->dev, "%s: %08x-%08x-%08x(%x) %08x\n", __func__,
+		area->da_start, da, area->da_end,
+		area->da_end - area->da_start, area->flags);
+
+	free_iovm_area(obj, area);
+out:
+	mutex_unlock(&obj->mmap_lock);
+
+	return sgt;
+}
+
+static u32 map_iommu_region(struct iommu_domain *domain, struct omap_iommu *obj,
+				u32 da, const struct sg_table *sgt, void *va,
+				size_t bytes, u32 flags)
+{
+	int err = -ENOMEM;
+	struct iovm_struct *new;
+
+	mutex_lock(&obj->mmap_lock);
+
+	new = alloc_iovm_area(obj, da, bytes, flags);
+	if (IS_ERR(new)) {
+		err = PTR_ERR(new);
+		goto err_alloc_iovma;
+	}
+	new->va = va;
+	new->sgt = sgt;
+
+	if (map_iovm_area(domain, new, sgt, new->flags))
+		goto err_map;
+
+	mutex_unlock(&obj->mmap_lock);
+
+	dev_dbg(obj->dev, "%s: da:%08x(%x) flags:%08x va:%p\n",
+		__func__, new->da_start, bytes, new->flags, va);
+
+	return new->da_start;
+
+err_map:
+	free_iovm_area(obj, new);
+err_alloc_iovma:
+	mutex_unlock(&obj->mmap_lock);
+	return err;
+}
+
+static inline u32
+__iommu_vmap(struct iommu_domain *domain, struct omap_iommu *obj,
+				u32 da, const struct sg_table *sgt,
+				void *va, size_t bytes, u32 flags)
+{
+	return map_iommu_region(domain, obj, da, sgt, va, bytes, flags);
+}
+
+/**
+ * omap_iommu_vmap  -  (d)-(p)-(v) address mapper
+ * @domain:	iommu domain
+ * @dev:	client device
+ * @sgt:	address of scatter gather table
+ * @flags:	iovma and page property
+ *
+ * Creates 1-n-1 mapping with given @sgt and returns @da.
+ * All @sgt element must be io page size aligned.
+ */
+u32 omap_iommu_vmap(struct iommu_domain *domain, struct device *dev, u32 da,
+		const struct sg_table *sgt, u32 flags)
+{
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
+	size_t bytes;
+	void *va = NULL;
+
+	if (!obj || !obj->dev || !sgt)
+		return -EINVAL;
+
+	bytes = sgtable_len(sgt);
+	if (!bytes)
+		return -EINVAL;
+	bytes = PAGE_ALIGN(bytes);
+
+	if (flags & IOVMF_MMIO) {
+		va = vmap_sg(sgt);
+		if (IS_ERR(va))
+			return PTR_ERR(va);
+	}
+
+	flags |= IOVMF_DISCONT;
+	flags |= IOVMF_MMIO;
+
+	da = __iommu_vmap(domain, obj, da, sgt, va, bytes, flags);
+	if (IS_ERR_VALUE(da))
+		vunmap_sg(va);
+
+	return da + sgtable_offset(sgt);
+}
+EXPORT_SYMBOL_GPL(omap_iommu_vmap);
+
+/**
+ * omap_iommu_vunmap  -  release virtual mapping obtained by 'omap_iommu_vmap()'
+ * @domain:	iommu domain
+ * @dev:	client device
+ * @da:		iommu device virtual address
+ *
+ * Free the iommu virtually contiguous memory area starting at
+ * @da, which was returned by 'omap_iommu_vmap()'.
+ */
+struct sg_table *
+omap_iommu_vunmap(struct iommu_domain *domain, struct device *dev, u32 da)
+{
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
+	struct sg_table *sgt;
+	/*
+	 * 'sgt' is allocated before 'omap_iommu_vmalloc()' is called.
+	 * Just returns 'sgt' to the caller to free
+	 */
+	da &= PAGE_MASK;
+	sgt = unmap_vm_area(domain, obj, da, vunmap_sg,
+					IOVMF_DISCONT | IOVMF_MMIO);
+	if (!sgt)
+		dev_dbg(obj->dev, "%s: No sgt\n", __func__);
+	return sgt;
+}
+EXPORT_SYMBOL_GPL(omap_iommu_vunmap);
+
+/**
+ * omap_iommu_vmalloc  -  (d)-(p)-(v) address allocator and mapper
+ * @dev:	client device
+ * @da:		contiguous iommu virtual memory
+ * @bytes:	allocation size
+ * @flags:	iovma and page property
+ *
+ * Allocate @bytes linearly and creates 1-n-1 mapping and returns
+ * @da again, which might be adjusted if 'IOVMF_DA_FIXED' is not set.
+ */
+u32
+omap_iommu_vmalloc(struct iommu_domain *domain, struct device *dev, u32 da,
+						size_t bytes, u32 flags)
+{
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
+	void *va;
+	struct sg_table *sgt;
+
+	if (!obj || !obj->dev || !bytes)
+		return -EINVAL;
+
+	bytes = PAGE_ALIGN(bytes);
+
+	va = vmalloc(bytes);
+	if (!va)
+		return -ENOMEM;
+
+	flags |= IOVMF_DISCONT;
+	flags |= IOVMF_ALLOC;
+
+	sgt = sgtable_alloc(bytes, flags, da, 0);
+	if (IS_ERR(sgt)) {
+		da = PTR_ERR(sgt);
+		goto err_sgt_alloc;
+	}
+	sgtable_fill_vmalloc(sgt, va);
+
+	da = __iommu_vmap(domain, obj, da, sgt, va, bytes, flags);
+	if (IS_ERR_VALUE(da))
+		goto err_iommu_vmap;
+
+	return da;
+
+err_iommu_vmap:
+	sgtable_drain_vmalloc(sgt);
+	sgtable_free(sgt);
+err_sgt_alloc:
+	vfree(va);
+	return da;
+}
+EXPORT_SYMBOL_GPL(omap_iommu_vmalloc);
+
+/**
+ * omap_iommu_vfree  -  release memory allocated by 'omap_iommu_vmalloc()'
+ * @dev:	client device
+ * @da:		iommu device virtual address
+ *
+ * Frees the iommu virtually continuous memory area starting at
+ * @da, as obtained from 'omap_iommu_vmalloc()'.
+ */
+void omap_iommu_vfree(struct iommu_domain *domain, struct device *dev,
+								const u32 da)
+{
+	struct omap_iommu *obj = dev_to_omap_iommu(dev);
+	struct sg_table *sgt;
+
+	sgt = unmap_vm_area(domain, obj, da, vfree,
+						IOVMF_DISCONT | IOVMF_ALLOC);
+	if (!sgt)
+		dev_dbg(obj->dev, "%s: No sgt\n", __func__);
+	sgtable_free(sgt);
+}
+EXPORT_SYMBOL_GPL(omap_iommu_vfree);
+
+static int __init iovmm_init(void)
+{
+	const unsigned long flags = SLAB_HWCACHE_ALIGN;
+	struct kmem_cache *p;
+
+	p = kmem_cache_create("iovm_area_cache", sizeof(struct iovm_struct), 0,
+			      flags, NULL);
+	if (!p)
+		return -ENOMEM;
+	iovm_area_cachep = p;
+
+	return 0;
+}
+module_init(iovmm_init);
+
+static void __exit iovmm_exit(void)
+{
+	kmem_cache_destroy(iovm_area_cachep);
+}
+module_exit(iovmm_exit);
+
+MODULE_DESCRIPTION("omap iommu: simple virtual address space management");
+MODULE_AUTHOR("Hiroshi DOYU <Hiroshi.DOYU@nokia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iommu/tegra-gart.c b/drivers/iommu/tegra-gart.c
new file mode 100644
index 000000000000..618f1bf52780
--- /dev/null
+++ b/drivers/iommu/tegra-gart.c
@@ -0,0 +1,447 @@
+/*
+ * IOMMU API for GART in Tegra20
+ *
+ * Copyright (c) 2010-2012, NVIDIA 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 it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#define pr_fmt(fmt)	"%s(): " fmt, __func__
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/mm.h>
+#include <linux/list.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/iommu.h>
+
+#include <asm/cacheflush.h>
+
+/* bitmap of the page sizes currently supported */
+#define GART_IOMMU_PGSIZES	(SZ_4K)
+
+#define GART_CONFIG		0x24
+#define GART_ENTRY_ADDR		0x28
+#define GART_ENTRY_DATA		0x2c
+#define GART_ENTRY_PHYS_ADDR_VALID	(1 << 31)
+
+#define GART_PAGE_SHIFT		12
+#define GART_PAGE_SIZE		(1 << GART_PAGE_SHIFT)
+#define GART_PAGE_MASK						\
+	(~(GART_PAGE_SIZE - 1) & ~GART_ENTRY_PHYS_ADDR_VALID)
+
+struct gart_client {
+	struct device		*dev;
+	struct list_head	list;
+};
+
+struct gart_device {
+	void __iomem		*regs;
+	u32			*savedata;
+	u32			page_count;	/* total remappable size */
+	dma_addr_t		iovmm_base;	/* offset to vmm_area */
+	spinlock_t		pte_lock;	/* for pagetable */
+	struct list_head	client;
+	spinlock_t		client_lock;	/* for client list */
+	struct device		*dev;
+};
+
+static struct gart_device *gart_handle; /* unique for a system */
+
+#define GART_PTE(_pfn)						\
+	(GART_ENTRY_PHYS_ADDR_VALID | ((_pfn) << PAGE_SHIFT))
+
+/*
+ * Any interaction between any block on PPSB and a block on APB or AHB
+ * must have these read-back to ensure the APB/AHB bus transaction is
+ * complete before initiating activity on the PPSB block.
+ */
+#define FLUSH_GART_REGS(gart)	((void)readl((gart)->regs + GART_CONFIG))
+
+#define for_each_gart_pte(gart, iova)					\
+	for (iova = gart->iovmm_base;					\
+	     iova < gart->iovmm_base + GART_PAGE_SIZE * gart->page_count; \
+	     iova += GART_PAGE_SIZE)
+
+static inline void gart_set_pte(struct gart_device *gart,
+				unsigned long offs, u32 pte)
+{
+	writel(offs, gart->regs + GART_ENTRY_ADDR);
+	writel(pte, gart->regs + GART_ENTRY_DATA);
+
+	dev_dbg(gart->dev, "%s %08lx:%08x\n",
+		 pte ? "map" : "unmap", offs, pte & GART_PAGE_MASK);
+}
+
+static inline unsigned long gart_read_pte(struct gart_device *gart,
+					  unsigned long offs)
+{
+	unsigned long pte;
+
+	writel(offs, gart->regs + GART_ENTRY_ADDR);
+	pte = readl(gart->regs + GART_ENTRY_DATA);
+
+	return pte;
+}
+
+static void do_gart_setup(struct gart_device *gart, const u32 *data)
+{
+	unsigned long iova;
+
+	for_each_gart_pte(gart, iova)
+		gart_set_pte(gart, iova, data ? *(data++) : 0);
+
+	writel(1, gart->regs + GART_CONFIG);
+	FLUSH_GART_REGS(gart);
+}
+
+#ifdef DEBUG
+static void gart_dump_table(struct gart_device *gart)
+{
+	unsigned long iova;
+	unsigned long flags;
+
+	spin_lock_irqsave(&gart->pte_lock, flags);
+	for_each_gart_pte(gart, iova) {
+		unsigned long pte;
+
+		pte = gart_read_pte(gart, iova);
+
+		dev_dbg(gart->dev, "%s %08lx:%08lx\n",
+			(GART_ENTRY_PHYS_ADDR_VALID & pte) ? "v" : " ",
+			iova, pte & GART_PAGE_MASK);
+	}
+	spin_unlock_irqrestore(&gart->pte_lock, flags);
+}
+#else
+static inline void gart_dump_table(struct gart_device *gart)
+{
+}
+#endif
+
+static inline bool gart_iova_range_valid(struct gart_device *gart,
+					 unsigned long iova, size_t bytes)
+{
+	unsigned long iova_start, iova_end, gart_start, gart_end;
+
+	iova_start = iova;
+	iova_end = iova_start + bytes - 1;
+	gart_start = gart->iovmm_base;
+	gart_end = gart_start + gart->page_count * GART_PAGE_SIZE - 1;
+
+	if (iova_start < gart_start)
+		return false;
+	if (iova_end > gart_end)
+		return false;
+	return true;
+}
+
+static int gart_iommu_attach_dev(struct iommu_domain *domain,
+				 struct device *dev)
+{
+	struct gart_device *gart;
+	struct gart_client *client, *c;
+	int err = 0;
+
+	gart = gart_handle;
+	if (!gart)
+		return -EINVAL;
+	domain->priv = gart;
+
+	client = devm_kzalloc(gart->dev, sizeof(*c), GFP_KERNEL);
+	if (!client)
+		return -ENOMEM;
+	client->dev = dev;
+
+	spin_lock(&gart->client_lock);
+	list_for_each_entry(c, &gart->client, list) {
+		if (c->dev == dev) {
+			dev_err(gart->dev,
+				"%s is already attached\n", dev_name(dev));
+			err = -EINVAL;
+			goto fail;
+		}
+	}
+	list_add(&client->list, &gart->client);
+	spin_unlock(&gart->client_lock);
+	dev_dbg(gart->dev, "Attached %s\n", dev_name(dev));
+	return 0;
+
+fail:
+	devm_kfree(gart->dev, client);
+	spin_unlock(&gart->client_lock);
+	return err;
+}
+
+static void gart_iommu_detach_dev(struct iommu_domain *domain,
+				  struct device *dev)
+{
+	struct gart_device *gart = domain->priv;
+	struct gart_client *c;
+
+	spin_lock(&gart->client_lock);
+
+	list_for_each_entry(c, &gart->client, list) {
+		if (c->dev == dev) {
+			list_del(&c->list);
+			devm_kfree(gart->dev, c);
+			dev_dbg(gart->dev, "Detached %s\n", dev_name(dev));
+			goto out;
+		}
+	}
+	dev_err(gart->dev, "Couldn't find\n");
+out:
+	spin_unlock(&gart->client_lock);
+}
+
+static int gart_iommu_domain_init(struct iommu_domain *domain)
+{
+	return 0;
+}
+
+static void gart_iommu_domain_destroy(struct iommu_domain *domain)
+{
+	struct gart_device *gart = domain->priv;
+
+	if (!gart)
+		return;
+
+	spin_lock(&gart->client_lock);
+	if (!list_empty(&gart->client)) {
+		struct gart_client *c;
+
+		list_for_each_entry(c, &gart->client, list)
+			gart_iommu_detach_dev(domain, c->dev);
+	}
+	spin_unlock(&gart->client_lock);
+	domain->priv = NULL;
+}
+
+static int gart_iommu_map(struct iommu_domain *domain, unsigned long iova,
+			  phys_addr_t pa, size_t bytes, int prot)
+{
+	struct gart_device *gart = domain->priv;
+	unsigned long flags;
+	unsigned long pfn;
+
+	if (!gart_iova_range_valid(gart, iova, bytes))
+		return -EINVAL;
+
+	spin_lock_irqsave(&gart->pte_lock, flags);
+	pfn = __phys_to_pfn(pa);
+	if (!pfn_valid(pfn)) {
+		dev_err(gart->dev, "Invalid page: %08x\n", pa);
+		spin_unlock_irqrestore(&gart->pte_lock, flags);
+		return -EINVAL;
+	}
+	gart_set_pte(gart, iova, GART_PTE(pfn));
+	FLUSH_GART_REGS(gart);
+	spin_unlock_irqrestore(&gart->pte_lock, flags);
+	return 0;
+}
+
+static size_t gart_iommu_unmap(struct iommu_domain *domain, unsigned long iova,
+			       size_t bytes)
+{
+	struct gart_device *gart = domain->priv;
+	unsigned long flags;
+
+	if (!gart_iova_range_valid(gart, iova, bytes))
+		return 0;
+
+	spin_lock_irqsave(&gart->pte_lock, flags);
+	gart_set_pte(gart, iova, 0);
+	FLUSH_GART_REGS(gart);
+	spin_unlock_irqrestore(&gart->pte_lock, flags);
+	return 0;
+}
+
+static phys_addr_t gart_iommu_iova_to_phys(struct iommu_domain *domain,
+					   unsigned long iova)
+{
+	struct gart_device *gart = domain->priv;
+	unsigned long pte;
+	phys_addr_t pa;
+	unsigned long flags;
+
+	if (!gart_iova_range_valid(gart, iova, 0))
+		return -EINVAL;
+
+	spin_lock_irqsave(&gart->pte_lock, flags);
+	pte = gart_read_pte(gart, iova);
+	spin_unlock_irqrestore(&gart->pte_lock, flags);
+
+	pa = (pte & GART_PAGE_MASK);
+	if (!pfn_valid(__phys_to_pfn(pa))) {
+		dev_err(gart->dev, "No entry for %08lx:%08x\n", iova, pa);
+		gart_dump_table(gart);
+		return -EINVAL;
+	}
+	return pa;
+}
+
+static int gart_iommu_domain_has_cap(struct iommu_domain *domain,
+				     unsigned long cap)
+{
+	return 0;
+}
+
+static struct iommu_ops gart_iommu_ops = {
+	.domain_init	= gart_iommu_domain_init,
+	.domain_destroy	= gart_iommu_domain_destroy,
+	.attach_dev	= gart_iommu_attach_dev,
+	.detach_dev	= gart_iommu_detach_dev,
+	.map		= gart_iommu_map,
+	.unmap		= gart_iommu_unmap,
+	.iova_to_phys	= gart_iommu_iova_to_phys,
+	.domain_has_cap	= gart_iommu_domain_has_cap,
+	.pgsize_bitmap	= GART_IOMMU_PGSIZES,
+};
+
+static int tegra_gart_suspend(struct device *dev)
+{
+	struct gart_device *gart = dev_get_drvdata(dev);
+	unsigned long iova;
+	u32 *data = gart->savedata;
+	unsigned long flags;
+
+	spin_lock_irqsave(&gart->pte_lock, flags);
+	for_each_gart_pte(gart, iova)
+		*(data++) = gart_read_pte(gart, iova);
+	spin_unlock_irqrestore(&gart->pte_lock, flags);
+	return 0;
+}
+
+static int tegra_gart_resume(struct device *dev)
+{
+	struct gart_device *gart = dev_get_drvdata(dev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&gart->pte_lock, flags);
+	do_gart_setup(gart, gart->savedata);
+	spin_unlock_irqrestore(&gart->pte_lock, flags);
+	return 0;
+}
+
+static int tegra_gart_probe(struct platform_device *pdev)
+{
+	struct gart_device *gart;
+	struct resource *res, *res_remap;
+	void __iomem *gart_regs;
+	int err;
+	struct device *dev = &pdev->dev;
+
+	if (gart_handle)
+		return -EIO;
+
+	BUILD_BUG_ON(PAGE_SHIFT != GART_PAGE_SHIFT);
+
+	/* the GART memory aperture is required */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	res_remap = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!res || !res_remap) {
+		dev_err(dev, "GART memory aperture expected\n");
+		return -ENXIO;
+	}
+
+	gart = devm_kzalloc(dev, sizeof(*gart), GFP_KERNEL);
+	if (!gart) {
+		dev_err(dev, "failed to allocate gart_device\n");
+		return -ENOMEM;
+	}
+
+	gart_regs = devm_ioremap(dev, res->start, resource_size(res));
+	if (!gart_regs) {
+		dev_err(dev, "failed to remap GART registers\n");
+		err = -ENXIO;
+		goto fail;
+	}
+
+	gart->dev = &pdev->dev;
+	spin_lock_init(&gart->pte_lock);
+	spin_lock_init(&gart->client_lock);
+	INIT_LIST_HEAD(&gart->client);
+	gart->regs = gart_regs;
+	gart->iovmm_base = (dma_addr_t)res_remap->start;
+	gart->page_count = (resource_size(res_remap) >> GART_PAGE_SHIFT);
+
+	gart->savedata = vmalloc(sizeof(u32) * gart->page_count);
+	if (!gart->savedata) {
+		dev_err(dev, "failed to allocate context save area\n");
+		err = -ENOMEM;
+		goto fail;
+	}
+
+	platform_set_drvdata(pdev, gart);
+	do_gart_setup(gart, NULL);
+
+	gart_handle = gart;
+	return 0;
+
+fail:
+	if (gart_regs)
+		devm_iounmap(dev, gart_regs);
+	if (gart && gart->savedata)
+		vfree(gart->savedata);
+	devm_kfree(dev, gart);
+	return err;
+}
+
+static int tegra_gart_remove(struct platform_device *pdev)
+{
+	struct gart_device *gart = platform_get_drvdata(pdev);
+	struct device *dev = gart->dev;
+
+	writel(0, gart->regs + GART_CONFIG);
+	if (gart->savedata)
+		vfree(gart->savedata);
+	if (gart->regs)
+		devm_iounmap(dev, gart->regs);
+	devm_kfree(dev, gart);
+	gart_handle = NULL;
+	return 0;
+}
+
+const struct dev_pm_ops tegra_gart_pm_ops = {
+	.suspend	= tegra_gart_suspend,
+	.resume		= tegra_gart_resume,
+};
+
+static struct platform_driver tegra_gart_driver = {
+	.probe		= tegra_gart_probe,
+	.remove		= tegra_gart_remove,
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "tegra_gart",
+		.pm	= &tegra_gart_pm_ops,
+	},
+};
+
+static int __devinit tegra_gart_init(void)
+{
+	bus_set_iommu(&platform_bus_type, &gart_iommu_ops);
+	return platform_driver_register(&tegra_gart_driver);
+}
+
+static void __exit tegra_gart_exit(void)
+{
+	platform_driver_unregister(&tegra_gart_driver);
+}
+
+subsys_initcall(tegra_gart_init);
+module_exit(tegra_gart_exit);
diff --git a/drivers/iommu/tegra-smmu.c b/drivers/iommu/tegra-smmu.c
new file mode 100644
index 000000000000..25bff5c02bb4
--- /dev/null
+++ b/drivers/iommu/tegra-smmu.c
@@ -0,0 +1,1044 @@
+/*
+ * IOMMU API for SMMU in Tegra30
+ *
+ * Copyright (c) 2011-2012, NVIDIA 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 it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#define pr_fmt(fmt)	"%s(): " fmt, __func__
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/mm.h>
+#include <linux/pagemap.h>
+#include <linux/device.h>
+#include <linux/sched.h>
+#include <linux/iommu.h>
+#include <linux/io.h>
+
+#include <asm/page.h>
+#include <asm/cacheflush.h>
+
+#include <mach/iomap.h>
+#include <mach/smmu.h>
+#include <mach/tegra_smmu.h>
+
+/* REVISIT: With new configurations for t114/124/148 passed from DT */
+#define SKIP_SWGRP_CHECK
+
+/* bitmap of the page sizes currently supported */
+#define SMMU_IOMMU_PGSIZES	(SZ_4K)
+
+#define SMMU_CONFIG				0x10
+#define SMMU_CONFIG_DISABLE			0
+#define SMMU_CONFIG_ENABLE			1
+
+#define SMMU_TLB_CONFIG				0x14
+#define SMMU_TLB_CONFIG_STATS__MASK		(1 << 31)
+#define SMMU_TLB_CONFIG_STATS__ENABLE		(1 << 31)
+#define SMMU_TLB_CONFIG_HIT_UNDER_MISS__ENABLE	(1 << 29)
+#define SMMU_TLB_CONFIG_ACTIVE_LINES__VALUE	0x10
+#define SMMU_TLB_CONFIG_RESET_VAL		0x20000010
+
+#define SMMU_PTC_CONFIG				0x18
+#define SMMU_PTC_CONFIG_STATS__MASK		(1 << 31)
+#define SMMU_PTC_CONFIG_STATS__ENABLE		(1 << 31)
+#define SMMU_PTC_CONFIG_CACHE__ENABLE		(1 << 29)
+#define SMMU_PTC_CONFIG_INDEX_MAP__PATTERN	0x3f
+#define SMMU_PTC_CONFIG_RESET_VAL		0x2000003f
+
+#define SMMU_PTB_ASID				0x1c
+#define SMMU_PTB_ASID_CURRENT_SHIFT		0
+
+#define SMMU_PTB_DATA				0x20
+#define SMMU_PTB_DATA_RESET_VAL			0
+#define SMMU_PTB_DATA_ASID_NONSECURE_SHIFT	29
+#define SMMU_PTB_DATA_ASID_WRITABLE_SHIFT	30
+#define SMMU_PTB_DATA_ASID_READABLE_SHIFT	31
+
+#define SMMU_TLB_FLUSH				0x30
+#define SMMU_TLB_FLUSH_VA_MATCH_ALL		0
+#define SMMU_TLB_FLUSH_VA_MATCH_SECTION		2
+#define SMMU_TLB_FLUSH_VA_MATCH_GROUP		3
+#define SMMU_TLB_FLUSH_ASID_SHIFT		29
+#define SMMU_TLB_FLUSH_ASID_MATCH_DISABLE	0
+#define SMMU_TLB_FLUSH_ASID_MATCH_ENABLE	1
+#define SMMU_TLB_FLUSH_ASID_MATCH_SHIFT		31
+
+#define SMMU_PTC_FLUSH				0x34
+#define SMMU_PTC_FLUSH_TYPE_ALL			0
+#define SMMU_PTC_FLUSH_TYPE_ADR			1
+#define SMMU_PTC_FLUSH_ADR_SHIFT		4
+
+#define SMMU_ASID_SECURITY			0x38
+
+#define SMMU_STATS_TLB_HIT_COUNT		0x1f0
+#define SMMU_STATS_TLB_MISS_COUNT		0x1f4
+#define SMMU_STATS_PTC_HIT_COUNT		0x1f8
+#define SMMU_STATS_PTC_MISS_COUNT		0x1fc
+
+#define SMMU_TRANSLATION_ENABLE_0		0x228
+#define SMMU_TRANSLATION_ENABLE_1		0x22c
+#define SMMU_TRANSLATION_ENABLE_2		0x230
+
+#define SMMU_AFI_ASID	0x238   /* PCIE */
+#define SMMU_AVPC_ASID	0x23c   /* AVP */
+#define SMMU_DC_ASID	0x240   /* Display controller */
+#define SMMU_DCB_ASID	0x244   /* Display controller B */
+#define SMMU_EPP_ASID	0x248   /* Encoder pre-processor */
+#define SMMU_G2_ASID	0x24c   /* 2D engine */
+#define SMMU_HC_ASID	0x250   /* Host1x */
+#define SMMU_HDA_ASID	0x254   /* High-def audio */
+#define SMMU_ISP_ASID	0x258   /* Image signal processor */
+#define SMMU_MPE_ASID	0x264   /* MPEG encoder */
+#define SMMU_NV_ASID	0x268   /* (3D) */
+#define SMMU_NV2_ASID	0x26c   /* (3D) */
+#define SMMU_PPCS_ASID	0x270   /* AHB */
+#define SMMU_SATA_ASID	0x278   /* SATA */
+#define SMMU_VDE_ASID	0x27c   /* Video decoder */
+#define SMMU_VI_ASID	0x280   /* Video input */
+
+#define SMMU_PDE_NEXT_SHIFT		28
+
+/* AHB Arbiter Registers */
+#define AHB_XBAR_CTRL				0xe0
+#define AHB_XBAR_CTRL_SMMU_INIT_DONE_DONE	1
+#define AHB_XBAR_CTRL_SMMU_INIT_DONE_SHIFT	17
+
+#define SMMU_NUM_ASIDS				4
+#define SMMU_TLB_FLUSH_VA_SECTION__MASK		0xffc00000
+#define SMMU_TLB_FLUSH_VA_SECTION__SHIFT	12 /* right shift */
+#define SMMU_TLB_FLUSH_VA_GROUP__MASK		0xffffc000
+#define SMMU_TLB_FLUSH_VA_GROUP__SHIFT		12 /* right shift */
+#define SMMU_TLB_FLUSH_VA(iova, which)	\
+	((((iova) & SMMU_TLB_FLUSH_VA_##which##__MASK) >> \
+		SMMU_TLB_FLUSH_VA_##which##__SHIFT) |	\
+	SMMU_TLB_FLUSH_VA_MATCH_##which)
+#define SMMU_PTB_ASID_CUR(n)	\
+		((n) << SMMU_PTB_ASID_CURRENT_SHIFT)
+#define SMMU_TLB_FLUSH_ASID_MATCH_disable		\
+		(SMMU_TLB_FLUSH_ASID_MATCH_DISABLE <<	\
+			SMMU_TLB_FLUSH_ASID_MATCH_SHIFT)
+#define SMMU_TLB_FLUSH_ASID_MATCH__ENABLE		\
+		(SMMU_TLB_FLUSH_ASID_MATCH_ENABLE <<	\
+			SMMU_TLB_FLUSH_ASID_MATCH_SHIFT)
+
+#define SMMU_PAGE_SHIFT 12
+#define SMMU_PAGE_SIZE	(1 << SMMU_PAGE_SHIFT)
+
+#define SMMU_PDIR_COUNT	1024
+#define SMMU_PDIR_SIZE	(sizeof(unsigned long) * SMMU_PDIR_COUNT)
+#define SMMU_PTBL_COUNT	1024
+#define SMMU_PTBL_SIZE	(sizeof(unsigned long) * SMMU_PTBL_COUNT)
+#define SMMU_PDIR_SHIFT	12
+#define SMMU_PDE_SHIFT	12
+#define SMMU_PTE_SHIFT	12
+#define SMMU_PFN_MASK	0x000fffff
+
+#define SMMU_ADDR_TO_PFN(addr)	((addr) >> 12)
+#define SMMU_ADDR_TO_PDN(addr)	((addr) >> 22)
+#define SMMU_PDN_TO_ADDR(addr)	((pdn) << 22)
+
+#define _READABLE	(1 << SMMU_PTB_DATA_ASID_READABLE_SHIFT)
+#define _WRITABLE	(1 << SMMU_PTB_DATA_ASID_WRITABLE_SHIFT)
+#define _NONSECURE	(1 << SMMU_PTB_DATA_ASID_NONSECURE_SHIFT)
+#define _PDE_NEXT	(1 << SMMU_PDE_NEXT_SHIFT)
+#define _MASK_ATTR	(_READABLE | _WRITABLE | _NONSECURE)
+
+#define _PDIR_ATTR	(_READABLE | _WRITABLE | _NONSECURE)
+
+#define _PDE_ATTR	(_READABLE | _WRITABLE | _NONSECURE)
+#define _PDE_ATTR_N	(_PDE_ATTR | _PDE_NEXT)
+#define _PDE_VACANT(pdn)	(((pdn) << 10) | _PDE_ATTR)
+
+#define _PTE_ATTR	(_READABLE | _WRITABLE | _NONSECURE)
+#define _PTE_VACANT(addr)	(((addr) >> SMMU_PAGE_SHIFT) | _PTE_ATTR)
+
+#define SMMU_MK_PDIR(page, attr)	\
+		((page_to_phys(page) >> SMMU_PDIR_SHIFT) | (attr))
+#define SMMU_MK_PDE(page, attr)		\
+		(unsigned long)((page_to_phys(page) >> SMMU_PDE_SHIFT) | (attr))
+#define SMMU_EX_PTBL_PAGE(pde)		\
+		pfn_to_page((unsigned long)(pde) & SMMU_PFN_MASK)
+#define SMMU_PFN_TO_PTE(pfn, attr)	(unsigned long)((pfn) | (attr))
+
+#define SMMU_ASID_ENABLE(asid)	((asid) | (1 << 31))
+#define SMMU_ASID_DISABLE	0
+#define SMMU_ASID_ASID(n)	((n) & ~SMMU_ASID_ENABLE(0))
+
+#define smmu_client_enable_hwgrp(c, m)	smmu_client_set_hwgrp(c, m, 1)
+#define smmu_client_disable_hwgrp(c)	smmu_client_set_hwgrp(c, 0, 0)
+#define __smmu_client_enable_hwgrp(c, m) __smmu_client_set_hwgrp(c, m, 1)
+#define __smmu_client_disable_hwgrp(c)	__smmu_client_set_hwgrp(c, 0, 0)
+
+#define HWGRP_INIT(client) [HWGRP_##client] = SMMU_##client##_ASID
+
+static const u32 smmu_hwgrp_asid_reg[] = {
+	HWGRP_INIT(AFI),
+	HWGRP_INIT(AVPC),
+	HWGRP_INIT(DC),
+	HWGRP_INIT(DCB),
+	HWGRP_INIT(EPP),
+	HWGRP_INIT(G2),
+	HWGRP_INIT(HC),
+	HWGRP_INIT(HDA),
+	HWGRP_INIT(ISP),
+	HWGRP_INIT(MPE),
+	HWGRP_INIT(NV),
+	HWGRP_INIT(NV2),
+	HWGRP_INIT(PPCS),
+	HWGRP_INIT(SATA),
+	HWGRP_INIT(VDE),
+	HWGRP_INIT(VI),
+};
+#define HWGRP_ASID_REG(x) (smmu_hwgrp_asid_reg[x])
+
+/*
+ * Per client for address space
+ */
+struct smmu_client {
+	struct device		*dev;
+	struct list_head	list;
+	struct smmu_as		*as;
+	u32			hwgrp;
+};
+
+/*
+ * Per address space
+ */
+struct smmu_as {
+	struct smmu_device	*smmu;	/* back pointer to container */
+	unsigned int		asid;
+	spinlock_t		lock;	/* for pagetable */
+	struct page		*pdir_page;
+	unsigned long		pdir_attr;
+	unsigned long		pde_attr;
+	unsigned long		pte_attr;
+	unsigned int		*pte_count;
+
+	struct list_head	client;
+	spinlock_t		client_lock; /* for client list */
+};
+
+/*
+ * Per SMMU device - IOMMU device
+ */
+struct smmu_device {
+	void __iomem	*regs, *regs_ahbarb;
+	unsigned long	iovmm_base;	/* remappable base address */
+	unsigned long	page_count;	/* total remappable size */
+	spinlock_t	lock;
+	char		*name;
+	struct device	*dev;
+	int		num_as;
+	struct smmu_as	*as;		/* Run-time allocated array */
+	struct page *avp_vector_page;	/* dummy page shared by all AS's */
+
+	/*
+	 * Register image savers for suspend/resume
+	 */
+	unsigned long translation_enable_0;
+	unsigned long translation_enable_1;
+	unsigned long translation_enable_2;
+	unsigned long asid_security;
+};
+
+static struct smmu_device *smmu_handle; /* unique for a system */
+
+/*
+ *	SMMU/AHB register accessors
+ */
+static inline u32 smmu_read(struct smmu_device *smmu, size_t offs)
+{
+	return readl(smmu->regs + offs);
+}
+static inline void smmu_write(struct smmu_device *smmu, u32 val, size_t offs)
+{
+	writel(val, smmu->regs + offs);
+}
+
+static inline u32 ahb_read(struct smmu_device *smmu, size_t offs)
+{
+	return readl(smmu->regs_ahbarb + offs);
+}
+static inline void ahb_write(struct smmu_device *smmu, u32 val, size_t offs)
+{
+	writel(val, smmu->regs_ahbarb + offs);
+}
+
+#define VA_PAGE_TO_PA(va, page)	\
+	(page_to_phys(page) + ((unsigned long)(va) & ~PAGE_MASK))
+
+#define FLUSH_CPU_DCACHE(va, page, size)	\
+	do {	\
+		unsigned long _pa_ = VA_PAGE_TO_PA(va, page);		\
+		__cpuc_flush_dcache_area((void *)(va), (size_t)(size));	\
+		outer_flush_range(_pa_, _pa_+(size_t)(size));		\
+	} while (0)
+
+/*
+ * Any interaction between any block on PPSB and a block on APB or AHB
+ * must have these read-back barriers to ensure the APB/AHB bus
+ * transaction is complete before initiating activity on the PPSB
+ * block.
+ */
+#define FLUSH_SMMU_REGS(smmu)	smmu_read(smmu, SMMU_CONFIG)
+
+#define smmu_client_hwgrp(c) (u32)((c)->dev->platform_data)
+
+static int __smmu_client_set_hwgrp(struct smmu_client *c,
+				   unsigned long map, int on)
+{
+	int i;
+	struct smmu_as *as = c->as;
+	u32 val, offs, mask = SMMU_ASID_ENABLE(as->asid);
+	struct smmu_device *smmu = as->smmu;
+
+	WARN_ON(!on && map);
+	if (on && !map)
+		return -EINVAL;
+	if (!on)
+		map = smmu_client_hwgrp(c);
+
+	for_each_set_bit(i, &map, HWGRP_COUNT) {
+		offs = HWGRP_ASID_REG(i);
+		val = smmu_read(smmu, offs);
+		if (on) {
+#if !defined(SKIP_SWGRP_CHECK)
+			if (WARN_ON(val & mask)) {
+				for_each_set_bit(i, &map, HWGRP_COUNT) {
+					offs = HWGRP_ASID_REG(i);
+					val = smmu_read(smmu, offs);
+					val &= ~mask;
+					smmu_write(smmu, val, offs);
+				}
+				return -EBUSY;
+			}
+#endif
+			val |= mask;
+		} else {
+#if !defined(SKIP_SWGRP_CHECK)
+			WARN_ON((val & mask) == mask);
+#endif
+			val &= ~mask;
+		}
+		smmu_write(smmu, val, offs);
+	}
+	FLUSH_SMMU_REGS(smmu);
+	c->hwgrp = map;
+	return 0;
+
+}
+
+static int smmu_client_set_hwgrp(struct smmu_client *c, u32 map, int on)
+{
+	u32 val;
+	unsigned long flags;
+	struct smmu_as *as = c->as;
+	struct smmu_device *smmu = as->smmu;
+
+	spin_lock_irqsave(&smmu->lock, flags);
+	val = __smmu_client_set_hwgrp(c, map, on);
+	spin_unlock_irqrestore(&smmu->lock, flags);
+	return val;
+}
+
+/*
+ * Flush all TLB entries and all PTC entries
+ * Caller must lock smmu
+ */
+static void smmu_flush_regs(struct smmu_device *smmu, int enable)
+{
+	u32 val;
+
+	smmu_write(smmu, SMMU_PTC_FLUSH_TYPE_ALL, SMMU_PTC_FLUSH);
+	FLUSH_SMMU_REGS(smmu);
+	val = SMMU_TLB_FLUSH_VA_MATCH_ALL |
+		SMMU_TLB_FLUSH_ASID_MATCH_disable;
+	smmu_write(smmu, val, SMMU_TLB_FLUSH);
+
+	if (enable)
+		smmu_write(smmu, SMMU_CONFIG_ENABLE, SMMU_CONFIG);
+	FLUSH_SMMU_REGS(smmu);
+}
+
+static void smmu_setup_regs(struct smmu_device *smmu)
+{
+	int i;
+	u32 val;
+
+	for (i = 0; i < smmu->num_as; i++) {
+		struct smmu_as *as = &smmu->as[i];
+		struct smmu_client *c;
+
+		smmu_write(smmu, SMMU_PTB_ASID_CUR(as->asid), SMMU_PTB_ASID);
+		val = as->pdir_page ?
+			SMMU_MK_PDIR(as->pdir_page, as->pdir_attr) :
+			SMMU_PTB_DATA_RESET_VAL;
+		smmu_write(smmu, val, SMMU_PTB_DATA);
+
+		list_for_each_entry(c, &as->client, list)
+			__smmu_client_set_hwgrp(c, c->hwgrp, 1);
+	}
+
+	smmu_write(smmu, smmu->translation_enable_0, SMMU_TRANSLATION_ENABLE_0);
+	smmu_write(smmu, smmu->translation_enable_1, SMMU_TRANSLATION_ENABLE_1);
+	smmu_write(smmu, smmu->translation_enable_2, SMMU_TRANSLATION_ENABLE_2);
+	smmu_write(smmu, smmu->asid_security, SMMU_ASID_SECURITY);
+	smmu_write(smmu, SMMU_TLB_CONFIG_RESET_VAL, SMMU_TLB_CONFIG);
+	smmu_write(smmu, SMMU_PTC_CONFIG_RESET_VAL, SMMU_PTC_CONFIG);
+
+	smmu_flush_regs(smmu, 1);
+
+	val = ahb_read(smmu, AHB_XBAR_CTRL);
+	val |= AHB_XBAR_CTRL_SMMU_INIT_DONE_DONE <<
+		AHB_XBAR_CTRL_SMMU_INIT_DONE_SHIFT;
+	ahb_write(smmu, val, AHB_XBAR_CTRL);
+}
+
+static void flush_ptc_and_tlb(struct smmu_device *smmu,
+		      struct smmu_as *as, dma_addr_t iova,
+		      unsigned long *pte, struct page *page, int is_pde)
+{
+	u32 val;
+	unsigned long tlb_flush_va = is_pde
+		?  SMMU_TLB_FLUSH_VA(iova, SECTION)
+		:  SMMU_TLB_FLUSH_VA(iova, GROUP);
+
+	val = SMMU_PTC_FLUSH_TYPE_ADR | VA_PAGE_TO_PA(pte, page);
+	smmu_write(smmu, val, SMMU_PTC_FLUSH);
+	FLUSH_SMMU_REGS(smmu);
+	val = tlb_flush_va |
+		SMMU_TLB_FLUSH_ASID_MATCH__ENABLE |
+		(as->asid << SMMU_TLB_FLUSH_ASID_SHIFT);
+	smmu_write(smmu, val, SMMU_TLB_FLUSH);
+	FLUSH_SMMU_REGS(smmu);
+}
+
+static void free_ptbl(struct smmu_as *as, dma_addr_t iova)
+{
+	unsigned long pdn = SMMU_ADDR_TO_PDN(iova);
+	unsigned long *pdir = (unsigned long *)page_address(as->pdir_page);
+
+	if (pdir[pdn] != _PDE_VACANT(pdn)) {
+		dev_dbg(as->smmu->dev, "pdn: %lx\n", pdn);
+
+		ClearPageReserved(SMMU_EX_PTBL_PAGE(pdir[pdn]));
+		__free_page(SMMU_EX_PTBL_PAGE(pdir[pdn]));
+		pdir[pdn] = _PDE_VACANT(pdn);
+		FLUSH_CPU_DCACHE(&pdir[pdn], as->pdir_page, sizeof pdir[pdn]);
+		flush_ptc_and_tlb(as->smmu, as, iova, &pdir[pdn],
+				  as->pdir_page, 1);
+	}
+}
+
+static void free_pdir(struct smmu_as *as)
+{
+	unsigned addr;
+	int count;
+	struct device *dev = as->smmu->dev;
+
+	if (!as->pdir_page)
+		return;
+
+	addr = as->smmu->iovmm_base;
+	count = as->smmu->page_count;
+	while (count-- > 0) {
+		free_ptbl(as, addr);
+		addr += SMMU_PAGE_SIZE * SMMU_PTBL_COUNT;
+	}
+	ClearPageReserved(as->pdir_page);
+	__free_page(as->pdir_page);
+	as->pdir_page = NULL;
+	devm_kfree(dev, as->pte_count);
+	as->pte_count = NULL;
+}
+
+/*
+ * Maps PTBL for given iova and returns the PTE address
+ * Caller must unmap the mapped PTBL returned in *ptbl_page_p
+ */
+static unsigned long *locate_pte(struct smmu_as *as,
+				 dma_addr_t iova, bool allocate,
+				 struct page **ptbl_page_p,
+				 unsigned int **count)
+{
+	unsigned long ptn = SMMU_ADDR_TO_PFN(iova);
+	unsigned long pdn = SMMU_ADDR_TO_PDN(iova);
+	unsigned long *pdir = page_address(as->pdir_page);
+	unsigned long *ptbl;
+
+	if (pdir[pdn] != _PDE_VACANT(pdn)) {
+		/* Mapped entry table already exists */
+		*ptbl_page_p = SMMU_EX_PTBL_PAGE(pdir[pdn]);
+		ptbl = page_address(*ptbl_page_p);
+	} else if (!allocate) {
+		return NULL;
+	} else {
+		int pn;
+		unsigned long addr = SMMU_PDN_TO_ADDR(pdn);
+
+		/* Vacant - allocate a new page table */
+		dev_dbg(as->smmu->dev, "New PTBL pdn: %lx\n", pdn);
+
+		*ptbl_page_p = alloc_page(GFP_ATOMIC);
+		if (!*ptbl_page_p) {
+			dev_err(as->smmu->dev,
+				"failed to allocate smmu_device page table\n");
+			return NULL;
+		}
+		SetPageReserved(*ptbl_page_p);
+		ptbl = (unsigned long *)page_address(*ptbl_page_p);
+		for (pn = 0; pn < SMMU_PTBL_COUNT;
+		     pn++, addr += SMMU_PAGE_SIZE) {
+			ptbl[pn] = _PTE_VACANT(addr);
+		}
+		FLUSH_CPU_DCACHE(ptbl, *ptbl_page_p, SMMU_PTBL_SIZE);
+		pdir[pdn] = SMMU_MK_PDE(*ptbl_page_p,
+					as->pde_attr | _PDE_NEXT);
+		FLUSH_CPU_DCACHE(&pdir[pdn], as->pdir_page, sizeof pdir[pdn]);
+		flush_ptc_and_tlb(as->smmu, as, iova, &pdir[pdn],
+				  as->pdir_page, 1);
+	}
+	*count = &as->pte_count[pdn];
+
+	return &ptbl[ptn % SMMU_PTBL_COUNT];
+}
+
+#ifdef CONFIG_SMMU_SIG_DEBUG
+static void put_signature(struct smmu_as *as,
+			  dma_addr_t iova, unsigned long pfn)
+{
+	struct page *page;
+	unsigned long *vaddr;
+
+	page = pfn_to_page(pfn);
+	vaddr = page_address(page);
+	if (!vaddr)
+		return;
+
+	vaddr[0] = iova;
+	vaddr[1] = pfn << PAGE_SHIFT;
+	FLUSH_CPU_DCACHE(vaddr, page, sizeof(vaddr[0]) * 2);
+}
+#else
+static inline void put_signature(struct smmu_as *as,
+				 unsigned long addr, unsigned long pfn)
+{
+}
+#endif
+
+/*
+ * Caller must lock/unlock as
+ */
+static int alloc_pdir(struct smmu_as *as)
+{
+	unsigned long *pdir;
+	int pdn;
+	u32 val;
+	struct smmu_device *smmu = as->smmu;
+
+	if (as->pdir_page)
+		return 0;
+
+	as->pte_count = devm_kzalloc(smmu->dev,
+		     sizeof(as->pte_count[0]) * SMMU_PDIR_COUNT, GFP_KERNEL);
+	if (!as->pte_count) {
+		dev_err(smmu->dev,
+			"failed to allocate smmu_device PTE cunters\n");
+		return -ENOMEM;
+	}
+	as->pdir_page = alloc_page(GFP_KERNEL | __GFP_DMA);
+	if (!as->pdir_page) {
+		dev_err(smmu->dev,
+			"failed to allocate smmu_device page directory\n");
+		devm_kfree(smmu->dev, as->pte_count);
+		as->pte_count = NULL;
+		return -ENOMEM;
+	}
+	SetPageReserved(as->pdir_page);
+	pdir = page_address(as->pdir_page);
+
+	for (pdn = 0; pdn < SMMU_PDIR_COUNT; pdn++)
+		pdir[pdn] = _PDE_VACANT(pdn);
+	FLUSH_CPU_DCACHE(pdir, as->pdir_page, SMMU_PDIR_SIZE);
+	val = SMMU_PTC_FLUSH_TYPE_ADR | VA_PAGE_TO_PA(pdir, as->pdir_page);
+	smmu_write(smmu, val, SMMU_PTC_FLUSH);
+	FLUSH_SMMU_REGS(as->smmu);
+	val = SMMU_TLB_FLUSH_VA_MATCH_ALL |
+		SMMU_TLB_FLUSH_ASID_MATCH__ENABLE |
+		(as->asid << SMMU_TLB_FLUSH_ASID_SHIFT);
+	smmu_write(smmu, val, SMMU_TLB_FLUSH);
+	FLUSH_SMMU_REGS(as->smmu);
+
+	return 0;
+}
+
+static void __smmu_iommu_unmap(struct smmu_as *as, dma_addr_t iova)
+{
+	unsigned long *pte;
+	struct page *page;
+	unsigned int *count;
+
+	pte = locate_pte(as, iova, false, &page, &count);
+	if (WARN_ON(!pte))
+		return;
+
+	if (WARN_ON(*pte == _PTE_VACANT(iova)))
+		return;
+
+	*pte = _PTE_VACANT(iova);
+	FLUSH_CPU_DCACHE(pte, page, sizeof(*pte));
+	flush_ptc_and_tlb(as->smmu, as, iova, pte, page, 0);
+	if (!--(*count)) {
+		free_ptbl(as, iova);
+		smmu_flush_regs(as->smmu, 0);
+	}
+}
+
+static void __smmu_iommu_map_pfn(struct smmu_as *as, dma_addr_t iova,
+				 unsigned long pfn)
+{
+	struct smmu_device *smmu = as->smmu;
+	unsigned long *pte;
+	unsigned int *count;
+	struct page *page;
+
+	pte = locate_pte(as, iova, true, &page, &count);
+	if (WARN_ON(!pte))
+		return;
+
+	if (*pte == _PTE_VACANT(iova))
+		(*count)++;
+	*pte = SMMU_PFN_TO_PTE(pfn, as->pte_attr);
+	if (unlikely((*pte == _PTE_VACANT(iova))))
+		(*count)--;
+	FLUSH_CPU_DCACHE(pte, page, sizeof(*pte));
+	flush_ptc_and_tlb(smmu, as, iova, pte, page, 0);
+	put_signature(as, iova, pfn);
+}
+
+static int smmu_iommu_map(struct iommu_domain *domain, unsigned long iova,
+			  phys_addr_t pa, size_t bytes, int prot)
+{
+	struct smmu_as *as = domain->priv;
+	unsigned long pfn = __phys_to_pfn(pa);
+	unsigned long flags;
+
+	dev_dbg(as->smmu->dev, "[%d] %08lx:%08x\n", as->asid, iova, pa);
+
+	if (!pfn_valid(pfn))
+		return -ENOMEM;
+
+	spin_lock_irqsave(&as->lock, flags);
+	__smmu_iommu_map_pfn(as, iova, pfn);
+	spin_unlock_irqrestore(&as->lock, flags);
+	return 0;
+}
+
+static size_t smmu_iommu_unmap(struct iommu_domain *domain, unsigned long iova,
+			       size_t bytes)
+{
+	struct smmu_as *as = domain->priv;
+	unsigned long flags;
+
+	dev_dbg(as->smmu->dev, "[%d] %08lx\n", as->asid, iova);
+
+	spin_lock_irqsave(&as->lock, flags);
+	__smmu_iommu_unmap(as, iova);
+	spin_unlock_irqrestore(&as->lock, flags);
+	return SMMU_PAGE_SIZE;
+}
+
+static phys_addr_t smmu_iommu_iova_to_phys(struct iommu_domain *domain,
+					   unsigned long iova)
+{
+	struct smmu_as *as = domain->priv;
+	unsigned long *pte;
+	unsigned int *count;
+	struct page *page;
+	unsigned long pfn;
+	unsigned long flags;
+
+	spin_lock_irqsave(&as->lock, flags);
+
+	pte = locate_pte(as, iova, true, &page, &count);
+	pfn = *pte & SMMU_PFN_MASK;
+	WARN_ON(!pfn_valid(pfn));
+	dev_dbg(as->smmu->dev,
+		"iova:%08lx pfn:%08lx asid:%d\n", iova, pfn, as->asid);
+
+	spin_unlock_irqrestore(&as->lock, flags);
+	return PFN_PHYS(pfn);
+}
+
+static int smmu_iommu_domain_has_cap(struct iommu_domain *domain,
+				     unsigned long cap)
+{
+	return 0;
+}
+
+static int smmu_iommu_attach_dev(struct iommu_domain *domain,
+				 struct device *dev)
+{
+	struct smmu_as *as = domain->priv;
+	struct smmu_device *smmu = as->smmu;
+	struct smmu_client *client, *c;
+	u32 map;
+	int err;
+
+	client = devm_kzalloc(smmu->dev, sizeof(*c), GFP_KERNEL);
+	if (!client)
+		return -ENOMEM;
+	client->dev = dev;
+	client->as = as;
+
+#ifdef SKIP_SWGRP_CHECK
+	/* Enable all SWGRP blindly by default */
+	map = (1 << HWGRP_COUNT) - 1;
+#else
+	map = (unsigned long)dev->platform_data;
+	if (!map)
+		return -EINVAL;
+#endif
+
+	err = smmu_client_enable_hwgrp(client, map);
+	if (err)
+		goto err_hwgrp;
+
+	spin_lock(&as->client_lock);
+	list_for_each_entry(c, &as->client, list) {
+		if (c->dev == dev) {
+			dev_err(smmu->dev,
+				"%s is already attached\n", dev_name(c->dev));
+			err = -EINVAL;
+			goto err_client;
+		}
+	}
+	list_add(&client->list, &as->client);
+	spin_unlock(&as->client_lock);
+
+	/*
+	 * Reserve "page zero" for AVP vectors using a common dummy
+	 * page.
+	 */
+	if (map & HWG_AVPC) {
+		struct page *page;
+
+		page = as->smmu->avp_vector_page;
+		__smmu_iommu_map_pfn(as, 0, page_to_pfn(page));
+
+		pr_info("Reserve \"page zero\" for AVP vectors using a common dummy\n");
+	}
+
+	dev_dbg(smmu->dev, "%s is attached\n", dev_name(dev));
+	return 0;
+
+err_client:
+	smmu_client_disable_hwgrp(client);
+	spin_unlock(&as->client_lock);
+err_hwgrp:
+	devm_kfree(smmu->dev, client);
+	return err;
+}
+
+static void smmu_iommu_detach_dev(struct iommu_domain *domain,
+				  struct device *dev)
+{
+	struct smmu_as *as = domain->priv;
+	struct smmu_device *smmu = as->smmu;
+	struct smmu_client *c;
+
+	spin_lock(&as->client_lock);
+
+	list_for_each_entry(c, &as->client, list) {
+		if (c->dev == dev) {
+			smmu_client_disable_hwgrp(c);
+			list_del(&c->list);
+			devm_kfree(smmu->dev, c);
+			c->as = NULL;
+			dev_dbg(smmu->dev,
+				"%s is detached\n", dev_name(c->dev));
+			goto out;
+		}
+	}
+	dev_err(smmu->dev, "Couldn't find %s\n", dev_name(c->dev));
+out:
+	spin_unlock(&as->client_lock);
+}
+
+static int smmu_iommu_domain_init(struct iommu_domain *domain)
+{
+	int i;
+	unsigned long flags;
+	struct smmu_as *as;
+	struct smmu_device *smmu = smmu_handle;
+
+	/* Look for a free AS with lock held */
+	for  (i = 0; i < smmu->num_as; i++) {
+		struct smmu_as *tmp = &smmu->as[i];
+
+		spin_lock_irqsave(&tmp->lock, flags);
+		if (!tmp->pdir_page) {
+			as = tmp;
+			goto found;
+		}
+		spin_unlock_irqrestore(&tmp->lock, flags);
+	}
+	dev_err(smmu->dev, "no free AS\n");
+	return -ENODEV;
+
+found:
+	if (alloc_pdir(as) < 0)
+		goto err_alloc_pdir;
+
+	spin_lock(&smmu->lock);
+
+	/* Update PDIR register */
+	smmu_write(smmu, SMMU_PTB_ASID_CUR(as->asid), SMMU_PTB_ASID);
+	smmu_write(smmu,
+		   SMMU_MK_PDIR(as->pdir_page, as->pdir_attr), SMMU_PTB_DATA);
+	FLUSH_SMMU_REGS(smmu);
+
+	spin_unlock(&smmu->lock);
+
+	spin_unlock_irqrestore(&as->lock, flags);
+	domain->priv = as;
+
+	dev_dbg(smmu->dev, "smmu_as@%p\n", as);
+	return 0;
+
+err_alloc_pdir:
+	spin_unlock_irqrestore(&as->lock, flags);
+	return -ENODEV;
+}
+
+static void smmu_iommu_domain_destroy(struct iommu_domain *domain)
+{
+	struct smmu_as *as = domain->priv;
+	struct smmu_device *smmu = as->smmu;
+	unsigned long flags;
+
+	spin_lock_irqsave(&as->lock, flags);
+
+	if (as->pdir_page) {
+		spin_lock(&smmu->lock);
+		smmu_write(smmu, SMMU_PTB_ASID_CUR(as->asid), SMMU_PTB_ASID);
+		smmu_write(smmu, SMMU_PTB_DATA_RESET_VAL, SMMU_PTB_DATA);
+		FLUSH_SMMU_REGS(smmu);
+		spin_unlock(&smmu->lock);
+
+		free_pdir(as);
+	}
+
+	if (!list_empty(&as->client)) {
+		struct smmu_client *c;
+
+		list_for_each_entry(c, &as->client, list)
+			smmu_iommu_detach_dev(domain, c->dev);
+	}
+
+	spin_unlock_irqrestore(&as->lock, flags);
+
+	domain->priv = NULL;
+	dev_dbg(smmu->dev, "smmu_as@%p\n", as);
+}
+
+static struct iommu_ops smmu_iommu_ops = {
+	.domain_init	= smmu_iommu_domain_init,
+	.domain_destroy	= smmu_iommu_domain_destroy,
+	.attach_dev	= smmu_iommu_attach_dev,
+	.detach_dev	= smmu_iommu_detach_dev,
+	.map		= smmu_iommu_map,
+	.unmap		= smmu_iommu_unmap,
+	.iova_to_phys	= smmu_iommu_iova_to_phys,
+	.domain_has_cap	= smmu_iommu_domain_has_cap,
+	.pgsize_bitmap	= SMMU_IOMMU_PGSIZES,
+};
+
+static int tegra_smmu_suspend(struct device *dev)
+{
+	struct smmu_device *smmu = dev_get_drvdata(dev);
+
+	smmu->translation_enable_0 = smmu_read(smmu, SMMU_TRANSLATION_ENABLE_0);
+	smmu->translation_enable_1 = smmu_read(smmu, SMMU_TRANSLATION_ENABLE_1);
+	smmu->translation_enable_2 = smmu_read(smmu, SMMU_TRANSLATION_ENABLE_2);
+	smmu->asid_security = smmu_read(smmu, SMMU_ASID_SECURITY);
+	return 0;
+}
+
+static int tegra_smmu_resume(struct device *dev)
+{
+	struct smmu_device *smmu = dev_get_drvdata(dev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&smmu->lock, flags);
+	smmu_setup_regs(smmu);
+	spin_unlock_irqrestore(&smmu->lock, flags);
+	return 0;
+}
+
+static int tegra_smmu_probe(struct platform_device *pdev)
+{
+	struct smmu_device *smmu;
+	struct resource *regs, *regs2;
+	struct tegra_smmu_window *window;
+	struct device *dev = &pdev->dev;
+	int i, err = 0;
+
+	if (smmu_handle)
+		return -EIO;
+
+	BUILD_BUG_ON(PAGE_SHIFT != SMMU_PAGE_SHIFT);
+
+	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	regs2 = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	window = tegra_smmu_window(0);
+	if (!regs || !regs2 || !window) {
+		dev_err(dev, "No SMMU resources\n");
+		return -ENODEV;
+	}
+
+	smmu = devm_kzalloc(dev, sizeof(*smmu), GFP_KERNEL);
+	if (!smmu) {
+		dev_err(dev, "failed to allocate smmu_device\n");
+		return -ENOMEM;
+	}
+
+	smmu->dev = dev;
+	smmu->num_as = SMMU_NUM_ASIDS;
+	smmu->iovmm_base = (unsigned long)window->start;
+	smmu->page_count = (window->end + 1 - window->start) >> SMMU_PAGE_SHIFT;
+	smmu->regs = devm_ioremap(dev, regs->start, resource_size(regs));
+	smmu->regs_ahbarb = devm_ioremap(dev, regs2->start,
+					 resource_size(regs2));
+	if (!smmu->regs || !smmu->regs_ahbarb) {
+		dev_err(dev, "failed to remap SMMU registers\n");
+		err = -ENXIO;
+		goto fail;
+	}
+
+	smmu->translation_enable_0 = ~0;
+	smmu->translation_enable_1 = ~0;
+	smmu->translation_enable_2 = ~0;
+	smmu->asid_security = 0;
+
+	smmu->as = devm_kzalloc(dev,
+			sizeof(smmu->as[0]) * smmu->num_as, GFP_KERNEL);
+	if (!smmu->as) {
+		dev_err(dev, "failed to allocate smmu_as\n");
+		err = -ENOMEM;
+		goto fail;
+	}
+
+	for (i = 0; i < smmu->num_as; i++) {
+		struct smmu_as *as = &smmu->as[i];
+
+		as->smmu = smmu;
+		as->asid = i;
+		as->pdir_attr = _PDIR_ATTR;
+		as->pde_attr = _PDE_ATTR;
+		as->pte_attr = _PTE_ATTR;
+
+		spin_lock_init(&as->lock);
+		INIT_LIST_HEAD(&as->client);
+	}
+	spin_lock_init(&smmu->lock);
+	smmu_setup_regs(smmu);
+	platform_set_drvdata(pdev, smmu);
+
+	smmu->avp_vector_page = alloc_page(GFP_KERNEL);
+	if (!smmu->avp_vector_page)
+		goto fail;
+
+	smmu_handle = smmu;
+	return 0;
+
+fail:
+	if (smmu->avp_vector_page)
+		__free_page(smmu->avp_vector_page);
+	if (smmu->regs)
+		devm_iounmap(dev, smmu->regs);
+	if (smmu->regs_ahbarb)
+		devm_iounmap(dev, smmu->regs_ahbarb);
+	if (smmu && smmu->as) {
+		for (i = 0; i < smmu->num_as; i++) {
+			if (smmu->as[i].pdir_page) {
+				ClearPageReserved(smmu->as[i].pdir_page);
+				__free_page(smmu->as[i].pdir_page);
+			}
+		}
+		devm_kfree(dev, smmu->as);
+	}
+	devm_kfree(dev, smmu);
+	return err;
+}
+
+static int tegra_smmu_remove(struct platform_device *pdev)
+{
+	struct smmu_device *smmu = platform_get_drvdata(pdev);
+	struct device *dev = smmu->dev;
+
+	smmu_write(smmu, SMMU_CONFIG_DISABLE, SMMU_CONFIG);
+	platform_set_drvdata(pdev, NULL);
+	if (smmu->as) {
+		int i;
+
+		for (i = 0; i < smmu->num_as; i++)
+			free_pdir(&smmu->as[i]);
+		devm_kfree(dev, smmu->as);
+	}
+	if (smmu->avp_vector_page)
+		__free_page(smmu->avp_vector_page);
+	if (smmu->regs)
+		devm_iounmap(dev, smmu->regs);
+	if (smmu->regs_ahbarb)
+		devm_iounmap(dev, smmu->regs_ahbarb);
+	devm_kfree(dev, smmu);
+	smmu_handle = NULL;
+	return 0;
+}
+
+const struct dev_pm_ops tegra_smmu_pm_ops = {
+	.suspend	= tegra_smmu_suspend,
+	.resume		= tegra_smmu_resume,
+};
+
+static struct platform_driver tegra_smmu_driver = {
+	.probe		= tegra_smmu_probe,
+	.remove		= tegra_smmu_remove,
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "tegra_smmu",
+		.pm	= &tegra_smmu_pm_ops,
+	},
+};
+
+static int __devinit tegra_smmu_init(void)
+{
+	bus_set_iommu(&platform_bus_type, &smmu_iommu_ops);
+	return platform_driver_register(&tegra_smmu_driver);
+}
+
+static void __exit tegra_smmu_exit(void)
+{
+	platform_driver_unregister(&tegra_smmu_driver);
+}
+
+core_initcall(tegra_smmu_init);
+module_exit(tegra_smmu_exit);
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index b591e726a6fa..7f9bed7697ac 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -450,6 +450,12 @@ config LEDS_TRIGGER_DEFAULT_ON
 	  This allows LEDs to be initialised in the ON state.
 	  If unsure, say Y.
 
+config LEDS_TRIGGER_SLEEP
+	tristate "LED Sleep Mode Trigger"
+	depends on LEDS_TRIGGERS && HAS_EARLYSUSPEND
+	help
+	  This turns LEDs on when the screen is off but the cpu still running.
+
 comment "iptables trigger is under Netfilter config (LED target)"
 	depends on LEDS_TRIGGERS
 
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index bbfd2e367dc0..cb77b9bb2f98 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -54,3 +54,4 @@ obj-$(CONFIG_LEDS_TRIGGER_HEARTBEAT)	+= ledtrig-heartbeat.o
 obj-$(CONFIG_LEDS_TRIGGER_BACKLIGHT)	+= ledtrig-backlight.o
 obj-$(CONFIG_LEDS_TRIGGER_GPIO)		+= ledtrig-gpio.o
 obj-$(CONFIG_LEDS_TRIGGER_DEFAULT_ON)	+= ledtrig-default-on.o
+obj-$(CONFIG_LEDS_TRIGGER_SLEEP)	+= ledtrig-sleep.o
diff --git a/drivers/leds/led-class.c b/drivers/leds/led-class.c
index dc3d3d83191a..6d5628bb0601 100644
--- a/drivers/leds/led-class.c
+++ b/drivers/leds/led-class.c
@@ -267,6 +267,8 @@ void led_blink_set(struct led_classdev *led_cdev,
 		   unsigned long *delay_on,
 		   unsigned long *delay_off)
 {
+	del_timer_sync(&led_cdev->blink_timer);
+
 	if (led_cdev->blink_set &&
 	    !led_cdev->blink_set(led_cdev, delay_on, delay_off))
 		return;
diff --git a/drivers/leds/ledtrig-sleep.c b/drivers/leds/ledtrig-sleep.c
new file mode 100644
index 000000000000..f16404212152
--- /dev/null
+++ b/drivers/leds/ledtrig-sleep.c
@@ -0,0 +1,80 @@
+/* drivers/leds/ledtrig-sleep.c
+ *
+ * Copyright (C) 2007 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/earlysuspend.h>
+#include <linux/leds.h>
+#include <linux/suspend.h>
+
+static int ledtrig_sleep_pm_callback(struct notifier_block *nfb,
+					unsigned long action,
+					void *ignored);
+
+DEFINE_LED_TRIGGER(ledtrig_sleep)
+static struct notifier_block ledtrig_sleep_pm_notifier = {
+	.notifier_call = ledtrig_sleep_pm_callback,
+	.priority = 0,
+};
+
+static void ledtrig_sleep_early_suspend(struct early_suspend *h)
+{
+	led_trigger_event(ledtrig_sleep, LED_FULL);
+}
+
+static void ledtrig_sleep_early_resume(struct early_suspend *h)
+{
+	led_trigger_event(ledtrig_sleep, LED_OFF);
+}
+
+static struct early_suspend ledtrig_sleep_early_suspend_handler = {
+	.suspend = ledtrig_sleep_early_suspend,
+	.resume = ledtrig_sleep_early_resume,
+};
+
+static int ledtrig_sleep_pm_callback(struct notifier_block *nfb,
+					unsigned long action,
+					void *ignored)
+{
+	switch (action) {
+	case PM_HIBERNATION_PREPARE:
+	case PM_SUSPEND_PREPARE:
+		led_trigger_event(ledtrig_sleep, LED_OFF);
+		return NOTIFY_OK;
+	case PM_POST_HIBERNATION:
+	case PM_POST_SUSPEND:
+		led_trigger_event(ledtrig_sleep, LED_FULL);
+		return NOTIFY_OK;
+	}
+
+	return NOTIFY_DONE;
+}
+
+static int __init ledtrig_sleep_init(void)
+{
+	led_trigger_register_simple("sleep", &ledtrig_sleep);
+	register_pm_notifier(&ledtrig_sleep_pm_notifier);
+	register_early_suspend(&ledtrig_sleep_early_suspend_handler);
+	return 0;
+}
+
+static void __exit ledtrig_sleep_exit(void)
+{
+	unregister_early_suspend(&ledtrig_sleep_early_suspend_handler);
+	unregister_pm_notifier(&ledtrig_sleep_pm_notifier);
+	led_trigger_unregister_simple(ledtrig_sleep);
+}
+
+module_init(ledtrig_sleep_init);
+module_exit(ledtrig_sleep_exit);
+
diff --git a/drivers/md/dm-crypt.c b/drivers/md/dm-crypt.c
index 8c2a000cf3f5..1f1d3423d394 100644
--- a/drivers/md/dm-crypt.c
+++ b/drivers/md/dm-crypt.c
@@ -18,14 +18,10 @@
 #include <linux/crypto.h>
 #include <linux/workqueue.h>
 #include <linux/backing-dev.h>
-#include <linux/percpu.h>
-#include <linux/atomic.h>
+#include <asm/atomic.h>
 #include <linux/scatterlist.h>
 #include <asm/page.h>
 #include <asm/unaligned.h>
-#include <crypto/hash.h>
-#include <crypto/md5.h>
-#include <crypto/algapi.h>
 
 #include <linux/device-mapper.h>
 
@@ -66,7 +62,6 @@ struct dm_crypt_request {
 	struct convert_context *ctx;
 	struct scatterlist sg_in;
 	struct scatterlist sg_out;
-	sector_t iv_sector;
 };
 
 struct crypt_config;
@@ -77,13 +72,11 @@ struct crypt_iv_operations {
 	void (*dtr)(struct crypt_config *cc);
 	int (*init)(struct crypt_config *cc);
 	int (*wipe)(struct crypt_config *cc);
-	int (*generator)(struct crypt_config *cc, u8 *iv,
-			 struct dm_crypt_request *dmreq);
-	int (*post)(struct crypt_config *cc, u8 *iv,
-		    struct dm_crypt_request *dmreq);
+	int (*generator)(struct crypt_config *cc, u8 *iv, sector_t sector);
 };
 
 struct iv_essiv_private {
+	struct crypto_cipher *tfm;
 	struct crypto_hash *hash_tfm;
 	u8 *salt;
 };
@@ -92,32 +85,11 @@ struct iv_benbi_private {
 	int shift;
 };
 
-#define LMK_SEED_SIZE 64 /* hash + 0 */
-struct iv_lmk_private {
-	struct crypto_shash *hash_tfm;
-	u8 *seed;
-};
-
 /*
  * Crypt: maps a linear range of a block device
  * and encrypts / decrypts at the same time.
  */
 enum flags { DM_CRYPT_SUSPENDED, DM_CRYPT_KEY_VALID };
-
-/*
- * Duplicated per-CPU state for cipher.
- */
-struct crypt_cpu {
-	struct ablkcipher_request *req;
-	/* ESSIV: struct crypto_cipher *essiv_tfm */
-	void *iv_private;
-	struct crypto_ablkcipher *tfms[0];
-};
-
-/*
- * The fields in here must be read only after initialization,
- * changing state should be in crypt_cpu.
- */
 struct crypt_config {
 	struct dm_dev *dev;
 	sector_t start;
@@ -141,19 +113,11 @@ struct crypt_config {
 	union {
 		struct iv_essiv_private essiv;
 		struct iv_benbi_private benbi;
-		struct iv_lmk_private lmk;
 	} iv_gen_private;
 	sector_t iv_offset;
 	unsigned int iv_size;
 
 	/*
-	 * Duplicated per cpu state. Access through
-	 * per_cpu_ptr() only.
-	 */
-	struct crypt_cpu __percpu *cpu;
-	unsigned tfms_count;
-
-	/*
 	 * Layout of each crypto request:
 	 *
 	 *   struct ablkcipher_request
@@ -167,10 +131,11 @@ struct crypt_config {
 	 * correctly aligned.
 	 */
 	unsigned int dmreq_start;
+	struct ablkcipher_request *req;
 
+	struct crypto_ablkcipher *tfm;
 	unsigned long flags;
 	unsigned int key_size;
-	unsigned int key_parts;
 	u8 key[0];
 };
 
@@ -182,20 +147,6 @@ static struct kmem_cache *_crypt_io_pool;
 
 static void clone_init(struct dm_crypt_io *, struct bio *);
 static void kcryptd_queue_crypt(struct dm_crypt_io *io);
-static u8 *iv_of_dmreq(struct crypt_config *cc, struct dm_crypt_request *dmreq);
-
-static struct crypt_cpu *this_crypt_config(struct crypt_config *cc)
-{
-	return this_cpu_ptr(cc->cpu);
-}
-
-/*
- * Use this to access cipher attributes that are the same for each CPU.
- */
-static struct crypto_ablkcipher *any_tfm(struct crypt_config *cc)
-{
-	return __this_cpu_ptr(cc->cpu)->tfms[0];
-}
 
 /*
  * Different IV generation algorithms:
@@ -216,38 +167,23 @@ static struct crypto_ablkcipher *any_tfm(struct crypt_config *cc)
  * null: the initial vector is always zero.  Provides compatibility with
  *       obsolete loop_fish2 devices.  Do not use for new devices.
  *
- * lmk:  Compatible implementation of the block chaining mode used
- *       by the Loop-AES block device encryption system
- *       designed by Jari Ruusu. See http://loop-aes.sourceforge.net/
- *       It operates on full 512 byte sectors and uses CBC
- *       with an IV derived from the sector number, the data and
- *       optionally extra IV seed.
- *       This means that after decryption the first block
- *       of sector must be tweaked according to decrypted data.
- *       Loop-AES can use three encryption schemes:
- *         version 1: is plain aes-cbc mode
- *         version 2: uses 64 multikey scheme with lmk IV generator
- *         version 3: the same as version 2 with additional IV seed
- *                   (it uses 65 keys, last key is used as IV seed)
- *
  * plumb: unimplemented, see:
  * http://article.gmane.org/gmane.linux.kernel.device-mapper.dm-crypt/454
  */
 
-static int crypt_iv_plain_gen(struct crypt_config *cc, u8 *iv,
-			      struct dm_crypt_request *dmreq)
+static int crypt_iv_plain_gen(struct crypt_config *cc, u8 *iv, sector_t sector)
 {
 	memset(iv, 0, cc->iv_size);
-	*(__le32 *)iv = cpu_to_le32(dmreq->iv_sector & 0xffffffff);
+	*(u32 *)iv = cpu_to_le32(sector & 0xffffffff);
 
 	return 0;
 }
 
 static int crypt_iv_plain64_gen(struct crypt_config *cc, u8 *iv,
-				struct dm_crypt_request *dmreq)
+				sector_t sector)
 {
 	memset(iv, 0, cc->iv_size);
-	*(__le64 *)iv = cpu_to_le64(dmreq->iv_sector);
+	*(u64 *)iv = cpu_to_le64(sector);
 
 	return 0;
 }
@@ -258,8 +194,7 @@ static int crypt_iv_essiv_init(struct crypt_config *cc)
 	struct iv_essiv_private *essiv = &cc->iv_gen_private.essiv;
 	struct hash_desc desc;
 	struct scatterlist sg;
-	struct crypto_cipher *essiv_tfm;
-	int err, cpu;
+	int err;
 
 	sg_init_one(&sg, cc->key, cc->key_size);
 	desc.tfm = essiv->hash_tfm;
@@ -269,16 +204,8 @@ static int crypt_iv_essiv_init(struct crypt_config *cc)
 	if (err)
 		return err;
 
-	for_each_possible_cpu(cpu) {
-		essiv_tfm = per_cpu_ptr(cc->cpu, cpu)->iv_private,
-
-		err = crypto_cipher_setkey(essiv_tfm, essiv->salt,
+	return crypto_cipher_setkey(essiv->tfm, essiv->salt,
 				    crypto_hash_digestsize(essiv->hash_tfm));
-		if (err)
-			return err;
-	}
-
-	return 0;
 }
 
 /* Wipe salt and reset key derived from volume key */
@@ -286,76 +213,24 @@ static int crypt_iv_essiv_wipe(struct crypt_config *cc)
 {
 	struct iv_essiv_private *essiv = &cc->iv_gen_private.essiv;
 	unsigned salt_size = crypto_hash_digestsize(essiv->hash_tfm);
-	struct crypto_cipher *essiv_tfm;
-	int cpu, r, err = 0;
 
 	memset(essiv->salt, 0, salt_size);
 
-	for_each_possible_cpu(cpu) {
-		essiv_tfm = per_cpu_ptr(cc->cpu, cpu)->iv_private;
-		r = crypto_cipher_setkey(essiv_tfm, essiv->salt, salt_size);
-		if (r)
-			err = r;
-	}
-
-	return err;
-}
-
-/* Set up per cpu cipher state */
-static struct crypto_cipher *setup_essiv_cpu(struct crypt_config *cc,
-					     struct dm_target *ti,
-					     u8 *salt, unsigned saltsize)
-{
-	struct crypto_cipher *essiv_tfm;
-	int err;
-
-	/* Setup the essiv_tfm with the given salt */
-	essiv_tfm = crypto_alloc_cipher(cc->cipher, 0, CRYPTO_ALG_ASYNC);
-	if (IS_ERR(essiv_tfm)) {
-		ti->error = "Error allocating crypto tfm for ESSIV";
-		return essiv_tfm;
-	}
-
-	if (crypto_cipher_blocksize(essiv_tfm) !=
-	    crypto_ablkcipher_ivsize(any_tfm(cc))) {
-		ti->error = "Block size of ESSIV cipher does "
-			    "not match IV size of block cipher";
-		crypto_free_cipher(essiv_tfm);
-		return ERR_PTR(-EINVAL);
-	}
-
-	err = crypto_cipher_setkey(essiv_tfm, salt, saltsize);
-	if (err) {
-		ti->error = "Failed to set key for ESSIV cipher";
-		crypto_free_cipher(essiv_tfm);
-		return ERR_PTR(err);
-	}
-
-	return essiv_tfm;
+	return crypto_cipher_setkey(essiv->tfm, essiv->salt, salt_size);
 }
 
 static void crypt_iv_essiv_dtr(struct crypt_config *cc)
 {
-	int cpu;
-	struct crypt_cpu *cpu_cc;
-	struct crypto_cipher *essiv_tfm;
 	struct iv_essiv_private *essiv = &cc->iv_gen_private.essiv;
 
+	crypto_free_cipher(essiv->tfm);
+	essiv->tfm = NULL;
+
 	crypto_free_hash(essiv->hash_tfm);
 	essiv->hash_tfm = NULL;
 
 	kzfree(essiv->salt);
 	essiv->salt = NULL;
-
-	for_each_possible_cpu(cpu) {
-		cpu_cc = per_cpu_ptr(cc->cpu, cpu);
-		essiv_tfm = cpu_cc->iv_private;
-
-		if (essiv_tfm)
-			crypto_free_cipher(essiv_tfm);
-
-		cpu_cc->iv_private = NULL;
-	}
 }
 
 static int crypt_iv_essiv_ctr(struct crypt_config *cc, struct dm_target *ti,
@@ -364,7 +239,7 @@ static int crypt_iv_essiv_ctr(struct crypt_config *cc, struct dm_target *ti,
 	struct crypto_cipher *essiv_tfm = NULL;
 	struct crypto_hash *hash_tfm = NULL;
 	u8 *salt = NULL;
-	int err, cpu;
+	int err;
 
 	if (!opts) {
 		ti->error = "Digest algorithm missing for ESSIV mode";
@@ -386,44 +261,48 @@ static int crypt_iv_essiv_ctr(struct crypt_config *cc, struct dm_target *ti,
 		goto bad;
 	}
 
+	/* Allocate essiv_tfm */
+	essiv_tfm = crypto_alloc_cipher(cc->cipher, 0, CRYPTO_ALG_ASYNC);
+	if (IS_ERR(essiv_tfm)) {
+		ti->error = "Error allocating crypto tfm for ESSIV";
+		err = PTR_ERR(essiv_tfm);
+		goto bad;
+	}
+	if (crypto_cipher_blocksize(essiv_tfm) !=
+	    crypto_ablkcipher_ivsize(cc->tfm)) {
+		ti->error = "Block size of ESSIV cipher does "
+			    "not match IV size of block cipher";
+		err = -EINVAL;
+		goto bad;
+	}
+
 	cc->iv_gen_private.essiv.salt = salt;
+	cc->iv_gen_private.essiv.tfm = essiv_tfm;
 	cc->iv_gen_private.essiv.hash_tfm = hash_tfm;
 
-	for_each_possible_cpu(cpu) {
-		essiv_tfm = setup_essiv_cpu(cc, ti, salt,
-					crypto_hash_digestsize(hash_tfm));
-		if (IS_ERR(essiv_tfm)) {
-			crypt_iv_essiv_dtr(cc);
-			return PTR_ERR(essiv_tfm);
-		}
-		per_cpu_ptr(cc->cpu, cpu)->iv_private = essiv_tfm;
-	}
-
 	return 0;
 
 bad:
+	if (essiv_tfm && !IS_ERR(essiv_tfm))
+		crypto_free_cipher(essiv_tfm);
 	if (hash_tfm && !IS_ERR(hash_tfm))
 		crypto_free_hash(hash_tfm);
 	kfree(salt);
 	return err;
 }
 
-static int crypt_iv_essiv_gen(struct crypt_config *cc, u8 *iv,
-			      struct dm_crypt_request *dmreq)
+static int crypt_iv_essiv_gen(struct crypt_config *cc, u8 *iv, sector_t sector)
 {
-	struct crypto_cipher *essiv_tfm = this_crypt_config(cc)->iv_private;
-
 	memset(iv, 0, cc->iv_size);
-	*(__le64 *)iv = cpu_to_le64(dmreq->iv_sector);
-	crypto_cipher_encrypt_one(essiv_tfm, iv, iv);
-
+	*(u64 *)iv = cpu_to_le64(sector);
+	crypto_cipher_encrypt_one(cc->iv_gen_private.essiv.tfm, iv, iv);
 	return 0;
 }
 
 static int crypt_iv_benbi_ctr(struct crypt_config *cc, struct dm_target *ti,
 			      const char *opts)
 {
-	unsigned bs = crypto_ablkcipher_blocksize(any_tfm(cc));
+	unsigned bs = crypto_ablkcipher_blocksize(cc->tfm);
 	int log = ilog2(bs);
 
 	/* we need to calculate how far we must shift the sector count
@@ -448,177 +327,25 @@ static void crypt_iv_benbi_dtr(struct crypt_config *cc)
 {
 }
 
-static int crypt_iv_benbi_gen(struct crypt_config *cc, u8 *iv,
-			      struct dm_crypt_request *dmreq)
+static int crypt_iv_benbi_gen(struct crypt_config *cc, u8 *iv, sector_t sector)
 {
 	__be64 val;
 
 	memset(iv, 0, cc->iv_size - sizeof(u64)); /* rest is cleared below */
 
-	val = cpu_to_be64(((u64)dmreq->iv_sector << cc->iv_gen_private.benbi.shift) + 1);
+	val = cpu_to_be64(((u64)sector << cc->iv_gen_private.benbi.shift) + 1);
 	put_unaligned(val, (__be64 *)(iv + cc->iv_size - sizeof(u64)));
 
 	return 0;
 }
 
-static int crypt_iv_null_gen(struct crypt_config *cc, u8 *iv,
-			     struct dm_crypt_request *dmreq)
+static int crypt_iv_null_gen(struct crypt_config *cc, u8 *iv, sector_t sector)
 {
 	memset(iv, 0, cc->iv_size);
 
 	return 0;
 }
 
-static void crypt_iv_lmk_dtr(struct crypt_config *cc)
-{
-	struct iv_lmk_private *lmk = &cc->iv_gen_private.lmk;
-
-	if (lmk->hash_tfm && !IS_ERR(lmk->hash_tfm))
-		crypto_free_shash(lmk->hash_tfm);
-	lmk->hash_tfm = NULL;
-
-	kzfree(lmk->seed);
-	lmk->seed = NULL;
-}
-
-static int crypt_iv_lmk_ctr(struct crypt_config *cc, struct dm_target *ti,
-			    const char *opts)
-{
-	struct iv_lmk_private *lmk = &cc->iv_gen_private.lmk;
-
-	lmk->hash_tfm = crypto_alloc_shash("md5", 0, 0);
-	if (IS_ERR(lmk->hash_tfm)) {
-		ti->error = "Error initializing LMK hash";
-		return PTR_ERR(lmk->hash_tfm);
-	}
-
-	/* No seed in LMK version 2 */
-	if (cc->key_parts == cc->tfms_count) {
-		lmk->seed = NULL;
-		return 0;
-	}
-
-	lmk->seed = kzalloc(LMK_SEED_SIZE, GFP_KERNEL);
-	if (!lmk->seed) {
-		crypt_iv_lmk_dtr(cc);
-		ti->error = "Error kmallocing seed storage in LMK";
-		return -ENOMEM;
-	}
-
-	return 0;
-}
-
-static int crypt_iv_lmk_init(struct crypt_config *cc)
-{
-	struct iv_lmk_private *lmk = &cc->iv_gen_private.lmk;
-	int subkey_size = cc->key_size / cc->key_parts;
-
-	/* LMK seed is on the position of LMK_KEYS + 1 key */
-	if (lmk->seed)
-		memcpy(lmk->seed, cc->key + (cc->tfms_count * subkey_size),
-		       crypto_shash_digestsize(lmk->hash_tfm));
-
-	return 0;
-}
-
-static int crypt_iv_lmk_wipe(struct crypt_config *cc)
-{
-	struct iv_lmk_private *lmk = &cc->iv_gen_private.lmk;
-
-	if (lmk->seed)
-		memset(lmk->seed, 0, LMK_SEED_SIZE);
-
-	return 0;
-}
-
-static int crypt_iv_lmk_one(struct crypt_config *cc, u8 *iv,
-			    struct dm_crypt_request *dmreq,
-			    u8 *data)
-{
-	struct iv_lmk_private *lmk = &cc->iv_gen_private.lmk;
-	struct {
-		struct shash_desc desc;
-		char ctx[crypto_shash_descsize(lmk->hash_tfm)];
-	} sdesc;
-	struct md5_state md5state;
-	u32 buf[4];
-	int i, r;
-
-	sdesc.desc.tfm = lmk->hash_tfm;
-	sdesc.desc.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
-
-	r = crypto_shash_init(&sdesc.desc);
-	if (r)
-		return r;
-
-	if (lmk->seed) {
-		r = crypto_shash_update(&sdesc.desc, lmk->seed, LMK_SEED_SIZE);
-		if (r)
-			return r;
-	}
-
-	/* Sector is always 512B, block size 16, add data of blocks 1-31 */
-	r = crypto_shash_update(&sdesc.desc, data + 16, 16 * 31);
-	if (r)
-		return r;
-
-	/* Sector is cropped to 56 bits here */
-	buf[0] = cpu_to_le32(dmreq->iv_sector & 0xFFFFFFFF);
-	buf[1] = cpu_to_le32((((u64)dmreq->iv_sector >> 32) & 0x00FFFFFF) | 0x80000000);
-	buf[2] = cpu_to_le32(4024);
-	buf[3] = 0;
-	r = crypto_shash_update(&sdesc.desc, (u8 *)buf, sizeof(buf));
-	if (r)
-		return r;
-
-	/* No MD5 padding here */
-	r = crypto_shash_export(&sdesc.desc, &md5state);
-	if (r)
-		return r;
-
-	for (i = 0; i < MD5_HASH_WORDS; i++)
-		__cpu_to_le32s(&md5state.hash[i]);
-	memcpy(iv, &md5state.hash, cc->iv_size);
-
-	return 0;
-}
-
-static int crypt_iv_lmk_gen(struct crypt_config *cc, u8 *iv,
-			    struct dm_crypt_request *dmreq)
-{
-	u8 *src;
-	int r = 0;
-
-	if (bio_data_dir(dmreq->ctx->bio_in) == WRITE) {
-		src = kmap_atomic(sg_page(&dmreq->sg_in), KM_USER0);
-		r = crypt_iv_lmk_one(cc, iv, dmreq, src + dmreq->sg_in.offset);
-		kunmap_atomic(src, KM_USER0);
-	} else
-		memset(iv, 0, cc->iv_size);
-
-	return r;
-}
-
-static int crypt_iv_lmk_post(struct crypt_config *cc, u8 *iv,
-			     struct dm_crypt_request *dmreq)
-{
-	u8 *dst;
-	int r;
-
-	if (bio_data_dir(dmreq->ctx->bio_in) == WRITE)
-		return 0;
-
-	dst = kmap_atomic(sg_page(&dmreq->sg_out), KM_USER0);
-	r = crypt_iv_lmk_one(cc, iv, dmreq, dst + dmreq->sg_out.offset);
-
-	/* Tweak the first block of plaintext sector */
-	if (!r)
-		crypto_xor(dst + dmreq->sg_out.offset, iv, cc->iv_size);
-
-	kunmap_atomic(dst, KM_USER0);
-	return r;
-}
-
 static struct crypt_iv_operations crypt_iv_plain_ops = {
 	.generator = crypt_iv_plain_gen
 };
@@ -645,15 +372,6 @@ static struct crypt_iv_operations crypt_iv_null_ops = {
 	.generator = crypt_iv_null_gen
 };
 
-static struct crypt_iv_operations crypt_iv_lmk_ops = {
-	.ctr	   = crypt_iv_lmk_ctr,
-	.dtr	   = crypt_iv_lmk_dtr,
-	.init	   = crypt_iv_lmk_init,
-	.wipe	   = crypt_iv_lmk_wipe,
-	.generator = crypt_iv_lmk_gen,
-	.post	   = crypt_iv_lmk_post
-};
-
 static void crypt_convert_init(struct crypt_config *cc,
 			       struct convert_context *ctx,
 			       struct bio *bio_out, struct bio *bio_in,
@@ -681,13 +399,6 @@ static struct ablkcipher_request *req_of_dmreq(struct crypt_config *cc,
 	return (struct ablkcipher_request *)((char *)dmreq - cc->dmreq_start);
 }
 
-static u8 *iv_of_dmreq(struct crypt_config *cc,
-		       struct dm_crypt_request *dmreq)
-{
-	return (u8 *)ALIGN((unsigned long)(dmreq + 1),
-		crypto_ablkcipher_alignmask(any_tfm(cc)) + 1);
-}
-
 static int crypt_convert_block(struct crypt_config *cc,
 			       struct convert_context *ctx,
 			       struct ablkcipher_request *req)
@@ -699,9 +410,9 @@ static int crypt_convert_block(struct crypt_config *cc,
 	int r = 0;
 
 	dmreq = dmreq_of_req(cc, req);
-	iv = iv_of_dmreq(cc, dmreq);
+	iv = (u8 *)ALIGN((unsigned long)(dmreq + 1),
+			 crypto_ablkcipher_alignmask(cc->tfm) + 1);
 
-	dmreq->iv_sector = ctx->sector;
 	dmreq->ctx = ctx;
 	sg_init_table(&dmreq->sg_in, 1);
 	sg_set_page(&dmreq->sg_in, bv_in->bv_page, 1 << SECTOR_SHIFT,
@@ -724,7 +435,7 @@ static int crypt_convert_block(struct crypt_config *cc,
 	}
 
 	if (cc->iv_gen_ops) {
-		r = cc->iv_gen_ops->generator(cc, iv, dmreq);
+		r = cc->iv_gen_ops->generator(cc, iv, ctx->sector);
 		if (r < 0)
 			return r;
 	}
@@ -737,28 +448,21 @@ static int crypt_convert_block(struct crypt_config *cc,
 	else
 		r = crypto_ablkcipher_decrypt(req);
 
-	if (!r && cc->iv_gen_ops && cc->iv_gen_ops->post)
-		r = cc->iv_gen_ops->post(cc, iv, dmreq);
-
 	return r;
 }
 
 static void kcryptd_async_done(struct crypto_async_request *async_req,
 			       int error);
-
 static void crypt_alloc_req(struct crypt_config *cc,
 			    struct convert_context *ctx)
 {
-	struct crypt_cpu *this_cc = this_crypt_config(cc);
-	unsigned key_index = ctx->sector & (cc->tfms_count - 1);
-
-	if (!this_cc->req)
-		this_cc->req = mempool_alloc(cc->req_pool, GFP_NOIO);
-
-	ablkcipher_request_set_tfm(this_cc->req, this_cc->tfms[key_index]);
-	ablkcipher_request_set_callback(this_cc->req,
-	    CRYPTO_TFM_REQ_MAY_BACKLOG | CRYPTO_TFM_REQ_MAY_SLEEP,
-	    kcryptd_async_done, dmreq_of_req(cc, this_cc->req));
+	if (!cc->req)
+		cc->req = mempool_alloc(cc->req_pool, GFP_NOIO);
+	ablkcipher_request_set_tfm(cc->req, cc->tfm);
+	ablkcipher_request_set_callback(cc->req, CRYPTO_TFM_REQ_MAY_BACKLOG |
+					CRYPTO_TFM_REQ_MAY_SLEEP,
+					kcryptd_async_done,
+					dmreq_of_req(cc, cc->req));
 }
 
 /*
@@ -767,7 +471,6 @@ static void crypt_alloc_req(struct crypt_config *cc,
 static int crypt_convert(struct crypt_config *cc,
 			 struct convert_context *ctx)
 {
-	struct crypt_cpu *this_cc = this_crypt_config(cc);
 	int r;
 
 	atomic_set(&ctx->pending, 1);
@@ -779,7 +482,7 @@ static int crypt_convert(struct crypt_config *cc,
 
 		atomic_inc(&ctx->pending);
 
-		r = crypt_convert_block(cc, ctx, this_cc->req);
+		r = crypt_convert_block(cc, ctx, cc->req);
 
 		switch (r) {
 		/* async */
@@ -788,7 +491,7 @@ static int crypt_convert(struct crypt_config *cc,
 			INIT_COMPLETION(ctx->restart);
 			/* fall through*/
 		case -EINPROGRESS:
-			this_cc->req = NULL;
+			cc->req = NULL;
 			ctx->sector++;
 			continue;
 
@@ -947,9 +650,6 @@ static void crypt_dec_pending(struct dm_crypt_io *io)
  * They must be separated as otherwise the final stages could be
  * starved by new requests which can block in the first stages due
  * to memory allocation.
- *
- * The work is done per CPU global for all dm-crypt instances.
- * They should not depend on each other and do not block.
  */
 static void crypt_endio(struct bio *clone, int error)
 {
@@ -990,22 +690,25 @@ static void clone_init(struct dm_crypt_io *io, struct bio *clone)
 	clone->bi_destructor = dm_crypt_bio_destructor;
 }
 
-static int kcryptd_io_read(struct dm_crypt_io *io, gfp_t gfp)
+static void kcryptd_io_read(struct dm_crypt_io *io)
 {
 	struct crypt_config *cc = io->target->private;
 	struct bio *base_bio = io->base_bio;
 	struct bio *clone;
 
+	crypt_inc_pending(io);
+
 	/*
 	 * The block layer might modify the bvec array, so always
 	 * copy the required bvecs because we need the original
 	 * one in order to decrypt the whole bio data *afterwards*.
 	 */
-	clone = bio_alloc_bioset(gfp, bio_segments(base_bio), cc->bs);
-	if (!clone)
-		return 1;
-
-	crypt_inc_pending(io);
+	clone = bio_alloc_bioset(GFP_NOIO, bio_segments(base_bio), cc->bs);
+	if (unlikely(!clone)) {
+		io->error = -ENOMEM;
+		crypt_dec_pending(io);
+		return;
+	}
 
 	clone_init(io, clone);
 	clone->bi_idx = 0;
@@ -1016,7 +719,6 @@ static int kcryptd_io_read(struct dm_crypt_io *io, gfp_t gfp)
 	       sizeof(struct bio_vec) * clone->bi_vcnt);
 
 	generic_make_request(clone);
-	return 0;
 }
 
 static void kcryptd_io_write(struct dm_crypt_io *io)
@@ -1029,12 +731,9 @@ static void kcryptd_io(struct work_struct *work)
 {
 	struct dm_crypt_io *io = container_of(work, struct dm_crypt_io, work);
 
-	if (bio_data_dir(io->base_bio) == READ) {
-		crypt_inc_pending(io);
-		if (kcryptd_io_read(io, GFP_NOIO))
-			io->error = -ENOMEM;
-		crypt_dec_pending(io);
-	} else
+	if (bio_data_dir(io->base_bio) == READ)
+		kcryptd_io_read(io);
+	else
 		kcryptd_io_write(io);
 }
 
@@ -1201,9 +900,6 @@ static void kcryptd_async_done(struct crypto_async_request *async_req,
 		return;
 	}
 
-	if (!error && cc->iv_gen_ops && cc->iv_gen_ops->post)
-		error = cc->iv_gen_ops->post(cc, iv_of_dmreq(cc, dmreq), dmreq);
-
 	mempool_free(req_of_dmreq(cc, dmreq), cc->req_pool);
 
 	if (!atomic_dec_and_test(&ctx->pending))
@@ -1274,93 +970,34 @@ static void crypt_encode_key(char *hex, u8 *key, unsigned int size)
 	}
 }
 
-static void crypt_free_tfms(struct crypt_config *cc, int cpu)
-{
-	struct crypt_cpu *cpu_cc = per_cpu_ptr(cc->cpu, cpu);
-	unsigned i;
-
-	for (i = 0; i < cc->tfms_count; i++)
-		if (cpu_cc->tfms[i] && !IS_ERR(cpu_cc->tfms[i])) {
-			crypto_free_ablkcipher(cpu_cc->tfms[i]);
-			cpu_cc->tfms[i] = NULL;
-		}
-}
-
-static int crypt_alloc_tfms(struct crypt_config *cc, int cpu, char *ciphermode)
-{
-	struct crypt_cpu *cpu_cc = per_cpu_ptr(cc->cpu, cpu);
-	unsigned i;
-	int err;
-
-	for (i = 0; i < cc->tfms_count; i++) {
-		cpu_cc->tfms[i] = crypto_alloc_ablkcipher(ciphermode, 0, 0);
-		if (IS_ERR(cpu_cc->tfms[i])) {
-			err = PTR_ERR(cpu_cc->tfms[i]);
-			crypt_free_tfms(cc, cpu);
-			return err;
-		}
-	}
-
-	return 0;
-}
-
-static int crypt_setkey_allcpus(struct crypt_config *cc)
-{
-	unsigned subkey_size = cc->key_size >> ilog2(cc->tfms_count);
-	int cpu, err = 0, i, r;
-
-	for_each_possible_cpu(cpu) {
-		for (i = 0; i < cc->tfms_count; i++) {
-			r = crypto_ablkcipher_setkey(per_cpu_ptr(cc->cpu, cpu)->tfms[i],
-						     cc->key + (i * subkey_size), subkey_size);
-			if (r)
-				err = r;
-		}
-	}
-
-	return err;
-}
-
 static int crypt_set_key(struct crypt_config *cc, char *key)
 {
-	int r = -EINVAL;
-	int key_string_len = strlen(key);
-
 	/* The key size may not be changed. */
-	if (cc->key_size != (key_string_len >> 1))
-		goto out;
+	if (cc->key_size != (strlen(key) >> 1))
+		return -EINVAL;
 
 	/* Hyphen (which gives a key_size of zero) means there is no key. */
 	if (!cc->key_size && strcmp(key, "-"))
-		goto out;
+		return -EINVAL;
 
 	if (cc->key_size && crypt_decode_key(cc->key, key, cc->key_size) < 0)
-		goto out;
+		return -EINVAL;
 
 	set_bit(DM_CRYPT_KEY_VALID, &cc->flags);
 
-	r = crypt_setkey_allcpus(cc);
-
-out:
-	/* Hex key string not needed after here, so wipe it. */
-	memset(key, '0', key_string_len);
-
-	return r;
+	return crypto_ablkcipher_setkey(cc->tfm, cc->key, cc->key_size);
 }
 
 static int crypt_wipe_key(struct crypt_config *cc)
 {
 	clear_bit(DM_CRYPT_KEY_VALID, &cc->flags);
 	memset(&cc->key, 0, cc->key_size * sizeof(u8));
-
-	return crypt_setkey_allcpus(cc);
+	return crypto_ablkcipher_setkey(cc->tfm, cc->key, cc->key_size);
 }
 
 static void crypt_dtr(struct dm_target *ti)
 {
 	struct crypt_config *cc = ti->private;
-	struct crypt_cpu *cpu_cc;
-	int cpu;
 
 	ti->private = NULL;
 
@@ -1372,14 +1009,6 @@ static void crypt_dtr(struct dm_target *ti)
 	if (cc->crypt_queue)
 		destroy_workqueue(cc->crypt_queue);
 
-	if (cc->cpu)
-		for_each_possible_cpu(cpu) {
-			cpu_cc = per_cpu_ptr(cc->cpu, cpu);
-			if (cpu_cc->req)
-				mempool_free(cpu_cc->req, cc->req_pool);
-			crypt_free_tfms(cc, cpu);
-		}
-
 	if (cc->bs)
 		bioset_free(cc->bs);
 
@@ -1393,12 +1022,12 @@ static void crypt_dtr(struct dm_target *ti)
 	if (cc->iv_gen_ops && cc->iv_gen_ops->dtr)
 		cc->iv_gen_ops->dtr(cc);
 
+	if (cc->tfm && !IS_ERR(cc->tfm))
+		crypto_free_ablkcipher(cc->tfm);
+
 	if (cc->dev)
 		dm_put_device(ti, cc->dev);
 
-	if (cc->cpu)
-		free_percpu(cc->cpu);
-
 	kzfree(cc->cipher);
 	kzfree(cc->cipher_string);
 
@@ -1410,9 +1039,9 @@ static int crypt_ctr_cipher(struct dm_target *ti,
 			    char *cipher_in, char *key)
 {
 	struct crypt_config *cc = ti->private;
-	char *tmp, *cipher, *chainmode, *ivmode, *ivopts, *keycount;
+	char *tmp, *cipher, *chainmode, *ivmode, *ivopts;
 	char *cipher_api = NULL;
-	int cpu, ret = -EINVAL;
+	int ret = -EINVAL;
 
 	/* Convert to crypto api definition? */
 	if (strchr(cipher_in, '(')) {
@@ -1426,20 +1055,10 @@ static int crypt_ctr_cipher(struct dm_target *ti,
 
 	/*
 	 * Legacy dm-crypt cipher specification
-	 * cipher[:keycount]-mode-iv:ivopts
+	 * cipher-mode-iv:ivopts
 	 */
 	tmp = cipher_in;
-	keycount = strsep(&tmp, "-");
-	cipher = strsep(&keycount, ":");
-
-	if (!keycount)
-		cc->tfms_count = 1;
-	else if (sscanf(keycount, "%u", &cc->tfms_count) != 1 ||
-		 !is_power_of_2(cc->tfms_count)) {
-		ti->error = "Bad cipher key count specification";
-		return -EINVAL;
-	}
-	cc->key_parts = cc->tfms_count;
+	cipher = strsep(&tmp, "-");
 
 	cc->cipher = kstrdup(cipher, GFP_KERNEL);
 	if (!cc->cipher)
@@ -1452,14 +1071,6 @@ static int crypt_ctr_cipher(struct dm_target *ti,
 	if (tmp)
 		DMWARN("Ignoring unexpected additional cipher options");
 
-	cc->cpu = __alloc_percpu(sizeof(*(cc->cpu)) +
-				 cc->tfms_count * sizeof(*(cc->cpu->tfms)),
-				 __alignof__(struct crypt_cpu));
-	if (!cc->cpu) {
-		ti->error = "Cannot allocate per cpu state";
-		goto bad_mem;
-	}
-
 	/*
 	 * For compatibility with the original dm-crypt mapping format, if
 	 * only the cipher name is supplied, use cbc-plain.
@@ -1486,12 +1097,11 @@ static int crypt_ctr_cipher(struct dm_target *ti,
 	}
 
 	/* Allocate cipher */
-	for_each_possible_cpu(cpu) {
-		ret = crypt_alloc_tfms(cc, cpu, cipher_api);
-		if (ret < 0) {
-			ti->error = "Error allocating crypto tfm";
-			goto bad;
-		}
+	cc->tfm = crypto_alloc_ablkcipher(cipher_api, 0, 0);
+	if (IS_ERR(cc->tfm)) {
+		ret = PTR_ERR(cc->tfm);
+		ti->error = "Error allocating crypto tfm";
+		goto bad;
 	}
 
 	/* Initialize and set key */
@@ -1502,7 +1112,7 @@ static int crypt_ctr_cipher(struct dm_target *ti,
 	}
 
 	/* Initialize IV */
-	cc->iv_size = crypto_ablkcipher_ivsize(any_tfm(cc));
+	cc->iv_size = crypto_ablkcipher_ivsize(cc->tfm);
 	if (cc->iv_size)
 		/* at least a 64 bit sector number should fit in our buffer */
 		cc->iv_size = max(cc->iv_size,
@@ -1525,15 +1135,7 @@ static int crypt_ctr_cipher(struct dm_target *ti,
 		cc->iv_gen_ops = &crypt_iv_benbi_ops;
 	else if (strcmp(ivmode, "null") == 0)
 		cc->iv_gen_ops = &crypt_iv_null_ops;
-	else if (strcmp(ivmode, "lmk") == 0) {
-		cc->iv_gen_ops = &crypt_iv_lmk_ops;
-		/* Version 2 and 3 is recognised according
-		 * to length of provided multi-key string.
-		 * If present (version 3), last key is used as IV seed.
-		 */
-		if (cc->key_size % cc->key_parts)
-			cc->key_parts++;
-	} else {
+	else {
 		ret = -EINVAL;
 		ti->error = "Invalid IV mode";
 		goto bad;
@@ -1611,9 +1213,9 @@ static int crypt_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 	}
 
 	cc->dmreq_start = sizeof(struct ablkcipher_request);
-	cc->dmreq_start += crypto_ablkcipher_reqsize(any_tfm(cc));
+	cc->dmreq_start += crypto_ablkcipher_reqsize(cc->tfm);
 	cc->dmreq_start = ALIGN(cc->dmreq_start, crypto_tfm_ctx_alignment());
-	cc->dmreq_start += crypto_ablkcipher_alignmask(any_tfm(cc)) &
+	cc->dmreq_start += crypto_ablkcipher_alignmask(cc->tfm) &
 			   ~(crypto_tfm_ctx_alignment() - 1);
 
 	cc->req_pool = mempool_create_kmalloc_pool(MIN_IOS, cc->dmreq_start +
@@ -1622,6 +1224,7 @@ static int crypt_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 		ti->error = "Cannot allocate crypt request mempool";
 		goto bad;
 	}
+	cc->req = NULL;
 
 	cc->page_pool = mempool_create_page_pool(MIN_POOL_PAGES, 0);
 	if (!cc->page_pool) {
@@ -1678,20 +1281,13 @@ static int crypt_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 	}
 
 	ret = -ENOMEM;
-	cc->io_queue = alloc_workqueue("kcryptd_io",
-				       WQ_NON_REENTRANT|
-				       WQ_MEM_RECLAIM,
-				       1);
+	cc->io_queue = create_singlethread_workqueue("kcryptd_io");
 	if (!cc->io_queue) {
 		ti->error = "Couldn't create kcryptd io queue";
 		goto bad;
 	}
 
-	cc->crypt_queue = alloc_workqueue("kcryptd",
-					  WQ_NON_REENTRANT|
-					  WQ_CPU_INTENSIVE|
-					  WQ_MEM_RECLAIM,
-					  1);
+	cc->crypt_queue = create_singlethread_workqueue("kcryptd");
 	if (!cc->crypt_queue) {
 		ti->error = "Couldn't create kcryptd queue";
 		goto bad;
@@ -1728,10 +1324,9 @@ static int crypt_map(struct dm_target *ti, struct bio *bio,
 
 	io = crypt_io_alloc(ti, bio, dm_target_offset(ti, bio->bi_sector));
 
-	if (bio_data_dir(io->base_bio) == READ) {
-		if (kcryptd_io_read(io, GFP_NOWAIT))
-			kcryptd_queue_io(io);
-	} else
+	if (bio_data_dir(io->base_bio) == READ)
+		kcryptd_queue_io(io);
+	else
 		kcryptd_queue_crypt(io);
 
 	return DM_MAPIO_SUBMITTED;
@@ -1865,7 +1460,7 @@ static int crypt_iterate_devices(struct dm_target *ti,
 
 static struct target_type crypt_target = {
 	.name   = "crypt",
-	.version = {1, 11, 0},
+	.version = {1, 8, 0},
 	.module = THIS_MODULE,
 	.ctr    = crypt_ctr,
 	.dtr    = crypt_dtr,
diff --git a/drivers/md/raid1.c b/drivers/md/raid1.c
index d9587dffe533..606fc04fd76a 100644
--- a/drivers/md/raid1.c
+++ b/drivers/md/raid1.c
@@ -508,8 +508,17 @@ static int read_balance(conf_t *conf, r1bio_t *r1_bio, int *max_sectors)
 		if (test_bit(WriteMostly, &rdev->flags)) {
 			/* Don't balance among write-mostly, just
 			 * use the first as a last resort */
-			if (best_disk < 0)
+			if (best_disk < 0) {
+				if (is_badblock(rdev, this_sector, sectors,
+						&first_bad, &bad_sectors)) {
+					if (first_bad < this_sector)
+						/* Cannot use this */
+						continue;
+					best_good_sectors = first_bad - this_sector;
+				} else
+					best_good_sectors = sectors;
 				best_disk = disk;
+			}
 			continue;
 		}
 		/* This is a reasonable device to use.  It might
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index 0cd9672cf9cb..1d44228530ab 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -1337,7 +1337,7 @@ static int raid10_add_disk(mddev_t *mddev, mdk_rdev_t *rdev)
 		mirror_info_t *p = &conf->mirrors[mirror];
 		if (p->recovery_disabled == mddev->recovery_disabled)
 			continue;
-		if (!p->rdev)
+		if (p->rdev)
 			continue;
 
 		disk_stack_limits(mddev->gendisk, rdev->bdev,
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index ac5e8b57e50f..b6200c3935ca 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -3069,7 +3069,7 @@ static void analyse_stripe(struct stripe_head *sh, struct stripe_head_state *s)
 			}
 		} else if (test_bit(In_sync, &rdev->flags))
 			set_bit(R5_Insync, &dev->flags);
-		else {
+		else if (!test_bit(Faulty, &rdev->flags)) {
 			/* in sync if before recovery_offset */
 			if (sh->sector + STRIPE_SECTORS <= rdev->recovery_offset)
 				set_bit(R5_Insync, &dev->flags);
@@ -3116,7 +3116,7 @@ static void handle_stripe(struct stripe_head *sh)
 	struct r5dev *pdev, *qdev;
 
 	clear_bit(STRIPE_HANDLE, &sh->state);
-	if (test_and_set_bit(STRIPE_ACTIVE, &sh->state)) {
+	if (test_and_set_bit_lock(STRIPE_ACTIVE, &sh->state)) {
 		/* already being handled, ensure it gets handled
 		 * again when current action finishes */
 		set_bit(STRIPE_HANDLE, &sh->state);
@@ -3165,10 +3165,14 @@ static void handle_stripe(struct stripe_head *sh)
 	/* check if the array has lost more than max_degraded devices and,
 	 * if so, some requests might need to be failed.
 	 */
-	if (s.failed > conf->max_degraded && s.to_read+s.to_write+s.written)
-		handle_failed_stripe(conf, sh, &s, disks, &s.return_bi);
-	if (s.failed > conf->max_degraded && s.syncing)
-		handle_failed_sync(conf, sh, &s);
+	if (s.failed > conf->max_degraded) {
+		sh->check_state = 0;
+		sh->reconstruct_state = 0;
+		if (s.to_read+s.to_write+s.written)
+			handle_failed_stripe(conf, sh, &s, disks, &s.return_bi);
+		if (s.syncing)
+			handle_failed_sync(conf, sh, &s);
+	}
 
 	/*
 	 * might be able to return some write requests if the parity blocks
@@ -3377,7 +3381,7 @@ finish:
 
 	return_io(s.return_bi);
 
-	clear_bit(STRIPE_ACTIVE, &sh->state);
+	clear_bit_unlock(STRIPE_ACTIVE, &sh->state);
 }
 
 static void raid5_activate_delayed(raid5_conf_t *conf)
diff --git a/drivers/media/dvb/dvb-usb/dib0700_core.c b/drivers/media/dvb/dvb-usb/dib0700_core.c
index 5eb91b4f8fd0..a224e94325b7 100644
--- a/drivers/media/dvb/dvb-usb/dib0700_core.c
+++ b/drivers/media/dvb/dvb-usb/dib0700_core.c
@@ -30,6 +30,11 @@ int dib0700_get_version(struct dvb_usb_device *d, u32 *hwversion,
 	struct dib0700_state *st = d->priv;
 	int ret;
 
+	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
+		deb_info("could not acquire lock");
+		return 0;
+	}
+
 	ret = usb_control_msg(d->udev, usb_rcvctrlpipe(d->udev, 0),
 				  REQUEST_GET_VERSION,
 				  USB_TYPE_VENDOR | USB_DIR_IN, 0, 0,
@@ -46,6 +51,7 @@ int dib0700_get_version(struct dvb_usb_device *d, u32 *hwversion,
 	if (fwtype != NULL)
 		*fwtype     = (st->buf[12] << 24) | (st->buf[13] << 16) |
 			(st->buf[14] << 8) | st->buf[15];
+	mutex_unlock(&d->usb_mutex);
 	return ret;
 }
 
@@ -108,7 +114,12 @@ int dib0700_ctrl_rd(struct dvb_usb_device *d, u8 *tx, u8 txlen, u8 *rx, u8 rxlen
 int dib0700_set_gpio(struct dvb_usb_device *d, enum dib07x0_gpios gpio, u8 gpio_dir, u8 gpio_val)
 {
 	struct dib0700_state *st = d->priv;
-	s16 ret;
+	int ret;
+
+	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
+		deb_info("could not acquire lock");
+		return 0;
+	}
 
 	st->buf[0] = REQUEST_SET_GPIO;
 	st->buf[1] = gpio;
@@ -116,6 +127,7 @@ int dib0700_set_gpio(struct dvb_usb_device *d, enum dib07x0_gpios gpio, u8 gpio_
 
 	ret = dib0700_ctrl_wr(d, st->buf, 3);
 
+	mutex_unlock(&d->usb_mutex);
 	return ret;
 }
 
@@ -125,6 +137,11 @@ static int dib0700_set_usb_xfer_len(struct dvb_usb_device *d, u16 nb_ts_packets)
 	int ret;
 
 	if (st->fw_version >= 0x10201) {
+		if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
+			deb_info("could not acquire lock");
+			return 0;
+		}
+
 		st->buf[0] = REQUEST_SET_USB_XFER_LEN;
 		st->buf[1] = (nb_ts_packets >> 8) & 0xff;
 		st->buf[2] = nb_ts_packets & 0xff;
@@ -132,6 +149,7 @@ static int dib0700_set_usb_xfer_len(struct dvb_usb_device *d, u16 nb_ts_packets)
 		deb_info("set the USB xfer len to %i Ts packet\n", nb_ts_packets);
 
 		ret = dib0700_ctrl_wr(d, st->buf, 3);
+		mutex_unlock(&d->usb_mutex);
 	} else {
 		deb_info("this firmware does not allow to change the USB xfer len\n");
 		ret = -EIO;
@@ -208,6 +226,10 @@ static int dib0700_i2c_xfer_new(struct i2c_adapter *adap, struct i2c_msg *msg,
 
 		} else {
 			/* Write request */
+			if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
+				deb_info("could not acquire lock");
+				return 0;
+			}
 			st->buf[0] = REQUEST_NEW_I2C_WRITE;
 			st->buf[1] = msg[i].addr << 1;
 			st->buf[2] = (en_start << 7) | (en_stop << 6) |
@@ -227,6 +249,7 @@ static int dib0700_i2c_xfer_new(struct i2c_adapter *adap, struct i2c_msg *msg,
 						 USB_TYPE_VENDOR | USB_DIR_OUT,
 						 0, 0, st->buf, msg[i].len + 4,
 						 USB_CTRL_GET_TIMEOUT);
+			mutex_unlock(&d->usb_mutex);
 			if (result < 0) {
 				deb_info("i2c write error (status = %d)\n", result);
 				break;
@@ -249,6 +272,10 @@ static int dib0700_i2c_xfer_legacy(struct i2c_adapter *adap,
 
 	if (mutex_lock_interruptible(&d->i2c_mutex) < 0)
 		return -EAGAIN;
+	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
+		deb_info("could not acquire lock");
+		return 0;
+	}
 
 	for (i = 0; i < num; i++) {
 		/* fill in the address */
@@ -279,6 +306,7 @@ static int dib0700_i2c_xfer_legacy(struct i2c_adapter *adap,
 				break;
 		}
 	}
+	mutex_unlock(&d->usb_mutex);
 	mutex_unlock(&d->i2c_mutex);
 
 	return i;
@@ -337,7 +365,12 @@ static int dib0700_set_clock(struct dvb_usb_device *d, u8 en_pll,
 	u16 pll_loopdiv, u16 free_div, u16 dsuScaler)
 {
 	struct dib0700_state *st = d->priv;
-	s16 ret;
+	int ret;
+
+	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
+		deb_info("could not acquire lock");
+		return 0;
+	}
 
 	st->buf[0] = REQUEST_SET_CLOCK;
 	st->buf[1] = (en_pll << 7) | (pll_src << 6) |
@@ -352,6 +385,7 @@ static int dib0700_set_clock(struct dvb_usb_device *d, u8 en_pll,
 	st->buf[9] =  dsuScaler         & 0xff; /* LSB */
 
 	ret = dib0700_ctrl_wr(d, st->buf, 10);
+	mutex_unlock(&d->usb_mutex);
 
 	return ret;
 }
@@ -360,10 +394,16 @@ int dib0700_set_i2c_speed(struct dvb_usb_device *d, u16 scl_kHz)
 {
 	struct dib0700_state *st = d->priv;
 	u16 divider;
+	int ret;
 
 	if (scl_kHz == 0)
 		return -EINVAL;
 
+	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
+		deb_info("could not acquire lock");
+		return 0;
+	}
+
 	st->buf[0] = REQUEST_SET_I2C_PARAM;
 	divider = (u16) (30000 / scl_kHz);
 	st->buf[1] = 0;
@@ -379,7 +419,11 @@ int dib0700_set_i2c_speed(struct dvb_usb_device *d, u16 scl_kHz)
 	deb_info("setting I2C speed: %04x %04x %04x (%d kHz).",
 		(st->buf[2] << 8) | (st->buf[3]), (st->buf[4] << 8) |
 		st->buf[5], (st->buf[6] << 8) | st->buf[7], scl_kHz);
-	return dib0700_ctrl_wr(d, st->buf, 8);
+
+	ret = dib0700_ctrl_wr(d, st->buf, 8);
+	mutex_unlock(&d->usb_mutex);
+
+	return ret;
 }
 
 
@@ -515,6 +559,11 @@ int dib0700_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
 		}
 	}
 
+	if (mutex_lock_interruptible(&adap->dev->usb_mutex) < 0) {
+		deb_info("could not acquire lock");
+		return 0;
+	}
+
 	st->buf[0] = REQUEST_ENABLE_VIDEO;
 	/* this bit gives a kind of command,
 	 * rather than enabling something or not */
@@ -548,7 +597,10 @@ int dib0700_streaming_ctrl(struct dvb_usb_adapter *adap, int onoff)
 
 	deb_info("data for streaming: %x %x\n", st->buf[1], st->buf[2]);
 
-	return dib0700_ctrl_wr(adap->dev, st->buf, 4);
+	ret = dib0700_ctrl_wr(adap->dev, st->buf, 4);
+	mutex_unlock(&adap->dev->usb_mutex);
+
+	return ret;
 }
 
 int dib0700_change_protocol(struct rc_dev *rc, u64 rc_type)
@@ -557,6 +609,11 @@ int dib0700_change_protocol(struct rc_dev *rc, u64 rc_type)
 	struct dib0700_state *st = d->priv;
 	int new_proto, ret;
 
+	if (mutex_lock_interruptible(&d->usb_mutex) < 0) {
+		deb_info("could not acquire lock");
+		return 0;
+	}
+
 	st->buf[0] = REQUEST_SET_RC;
 	st->buf[1] = 0;
 	st->buf[2] = 0;
@@ -567,23 +624,29 @@ int dib0700_change_protocol(struct rc_dev *rc, u64 rc_type)
 	else if (rc_type == RC_TYPE_NEC)
 		new_proto = 0;
 	else if (rc_type == RC_TYPE_RC6) {
-		if (st->fw_version < 0x10200)
-			return -EINVAL;
+		if (st->fw_version < 0x10200) {
+			ret = -EINVAL;
+			goto out;
+		}
 
 		new_proto = 2;
-	} else
-		return -EINVAL;
+	} else {
+		ret = -EINVAL;
+		goto out;
+	}
 
 	st->buf[1] = new_proto;
 
 	ret = dib0700_ctrl_wr(d, st->buf, 3);
 	if (ret < 0) {
 		err("ir protocol setup failed");
-		return ret;
+		goto out;
 	}
 
 	d->props.rc.core.protocol = rc_type;
 
+out:
+	mutex_unlock(&d->usb_mutex);
 	return ret;
 }
 
diff --git a/drivers/media/dvb/frontends/dib0070.c b/drivers/media/dvb/frontends/dib0070.c
index 1d47d4da7d4c..dc1cb17a6ea7 100644
--- a/drivers/media/dvb/frontends/dib0070.c
+++ b/drivers/media/dvb/frontends/dib0070.c
@@ -27,6 +27,7 @@
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
+#include <linux/mutex.h>
 
 #include "dvb_frontend.h"
 
@@ -78,10 +79,18 @@ struct dib0070_state {
 	struct i2c_msg msg[2];
 	u8 i2c_write_buffer[3];
 	u8 i2c_read_buffer[2];
+	struct mutex i2c_buffer_lock;
 };
 
-static uint16_t dib0070_read_reg(struct dib0070_state *state, u8 reg)
+static u16 dib0070_read_reg(struct dib0070_state *state, u8 reg)
 {
+	u16 ret;
+
+	if (mutex_lock_interruptible(&state->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return 0;
+	}
+
 	state->i2c_write_buffer[0] = reg;
 
 	memset(state->msg, 0, 2 * sizeof(struct i2c_msg));
@@ -96,13 +105,23 @@ static uint16_t dib0070_read_reg(struct dib0070_state *state, u8 reg)
 
 	if (i2c_transfer(state->i2c, state->msg, 2) != 2) {
 		printk(KERN_WARNING "DiB0070 I2C read failed\n");
-		return 0;
-	}
-	return (state->i2c_read_buffer[0] << 8) | state->i2c_read_buffer[1];
+		ret = 0;
+	} else
+		ret = (state->i2c_read_buffer[0] << 8)
+			| state->i2c_read_buffer[1];
+
+	mutex_unlock(&state->i2c_buffer_lock);
+	return ret;
 }
 
 static int dib0070_write_reg(struct dib0070_state *state, u8 reg, u16 val)
 {
+	int ret;
+
+	if (mutex_lock_interruptible(&state->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return -EINVAL;
+	}
 	state->i2c_write_buffer[0] = reg;
 	state->i2c_write_buffer[1] = val >> 8;
 	state->i2c_write_buffer[2] = val & 0xff;
@@ -115,9 +134,12 @@ static int dib0070_write_reg(struct dib0070_state *state, u8 reg, u16 val)
 
 	if (i2c_transfer(state->i2c, state->msg, 1) != 1) {
 		printk(KERN_WARNING "DiB0070 I2C write failed\n");
-		return -EREMOTEIO;
-	}
-	return 0;
+		ret = -EREMOTEIO;
+	} else
+		ret = 0;
+
+	mutex_unlock(&state->i2c_buffer_lock);
+	return ret;
 }
 
 #define HARD_RESET(state) do { \
@@ -734,6 +756,7 @@ struct dvb_frontend *dib0070_attach(struct dvb_frontend *fe, struct i2c_adapter
 	state->cfg = cfg;
 	state->i2c = i2c;
 	state->fe  = fe;
+	mutex_init(&state->i2c_buffer_lock);
 	fe->tuner_priv = state;
 
 	if (dib0070_reset(fe) != 0)
diff --git a/drivers/media/dvb/frontends/dib0090.c b/drivers/media/dvb/frontends/dib0090.c
index c9c935ae41e4..b174d1c78583 100644
--- a/drivers/media/dvb/frontends/dib0090.c
+++ b/drivers/media/dvb/frontends/dib0090.c
@@ -27,6 +27,7 @@
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
+#include <linux/mutex.h>
 
 #include "dvb_frontend.h"
 
@@ -196,6 +197,7 @@ struct dib0090_state {
 	struct i2c_msg msg[2];
 	u8 i2c_write_buffer[3];
 	u8 i2c_read_buffer[2];
+	struct mutex i2c_buffer_lock;
 };
 
 struct dib0090_fw_state {
@@ -208,10 +210,18 @@ struct dib0090_fw_state {
 	struct i2c_msg msg;
 	u8 i2c_write_buffer[2];
 	u8 i2c_read_buffer[2];
+	struct mutex i2c_buffer_lock;
 };
 
 static u16 dib0090_read_reg(struct dib0090_state *state, u8 reg)
 {
+	u16 ret;
+
+	if (mutex_lock_interruptible(&state->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return 0;
+	}
+
 	state->i2c_write_buffer[0] = reg;
 
 	memset(state->msg, 0, 2 * sizeof(struct i2c_msg));
@@ -226,14 +236,24 @@ static u16 dib0090_read_reg(struct dib0090_state *state, u8 reg)
 
 	if (i2c_transfer(state->i2c, state->msg, 2) != 2) {
 		printk(KERN_WARNING "DiB0090 I2C read failed\n");
-		return 0;
-	}
+		ret = 0;
+	} else
+		ret = (state->i2c_read_buffer[0] << 8)
+			| state->i2c_read_buffer[1];
 
-	return (state->i2c_read_buffer[0] << 8) | state->i2c_read_buffer[1];
+	mutex_unlock(&state->i2c_buffer_lock);
+	return ret;
 }
 
 static int dib0090_write_reg(struct dib0090_state *state, u32 reg, u16 val)
 {
+	int ret;
+
+	if (mutex_lock_interruptible(&state->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return -EINVAL;
+	}
+
 	state->i2c_write_buffer[0] = reg & 0xff;
 	state->i2c_write_buffer[1] = val >> 8;
 	state->i2c_write_buffer[2] = val & 0xff;
@@ -246,13 +266,23 @@ static int dib0090_write_reg(struct dib0090_state *state, u32 reg, u16 val)
 
 	if (i2c_transfer(state->i2c, state->msg, 1) != 1) {
 		printk(KERN_WARNING "DiB0090 I2C write failed\n");
-		return -EREMOTEIO;
-	}
-	return 0;
+		ret = -EREMOTEIO;
+	} else
+		ret = 0;
+
+	mutex_unlock(&state->i2c_buffer_lock);
+	return ret;
 }
 
 static u16 dib0090_fw_read_reg(struct dib0090_fw_state *state, u8 reg)
 {
+	u16 ret;
+
+	if (mutex_lock_interruptible(&state->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return 0;
+	}
+
 	state->i2c_write_buffer[0] = reg;
 
 	memset(&state->msg, 0, sizeof(struct i2c_msg));
@@ -262,13 +292,24 @@ static u16 dib0090_fw_read_reg(struct dib0090_fw_state *state, u8 reg)
 	state->msg.len = 2;
 	if (i2c_transfer(state->i2c, &state->msg, 1) != 1) {
 		printk(KERN_WARNING "DiB0090 I2C read failed\n");
-		return 0;
-	}
-	return (state->i2c_read_buffer[0] << 8) | state->i2c_read_buffer[1];
+		ret = 0;
+	} else
+		ret = (state->i2c_read_buffer[0] << 8)
+			| state->i2c_read_buffer[1];
+
+	mutex_unlock(&state->i2c_buffer_lock);
+	return ret;
 }
 
 static int dib0090_fw_write_reg(struct dib0090_fw_state *state, u8 reg, u16 val)
 {
+	int ret;
+
+	if (mutex_lock_interruptible(&state->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return -EINVAL;
+	}
+
 	state->i2c_write_buffer[0] = val >> 8;
 	state->i2c_write_buffer[1] = val & 0xff;
 
@@ -279,9 +320,12 @@ static int dib0090_fw_write_reg(struct dib0090_fw_state *state, u8 reg, u16 val)
 	state->msg.len = 2;
 	if (i2c_transfer(state->i2c, &state->msg, 1) != 1) {
 		printk(KERN_WARNING "DiB0090 I2C write failed\n");
-		return -EREMOTEIO;
-	}
-	return 0;
+		ret = -EREMOTEIO;
+	} else
+		ret = 0;
+
+	mutex_unlock(&state->i2c_buffer_lock);
+	return ret;
 }
 
 #define HARD_RESET(state) do {  if (cfg->reset) {  if (cfg->sleep) cfg->sleep(fe, 0); msleep(10);  cfg->reset(fe, 1); msleep(10);  cfg->reset(fe, 0); msleep(10);  }  } while (0)
@@ -2440,6 +2484,7 @@ struct dvb_frontend *dib0090_register(struct dvb_frontend *fe, struct i2c_adapte
 	st->config = config;
 	st->i2c = i2c;
 	st->fe = fe;
+	mutex_init(&st->i2c_buffer_lock);
 	fe->tuner_priv = st;
 
 	if (config->wbd == NULL)
@@ -2471,6 +2516,7 @@ struct dvb_frontend *dib0090_fw_register(struct dvb_frontend *fe, struct i2c_ada
 	st->config = config;
 	st->i2c = i2c;
 	st->fe = fe;
+	mutex_init(&st->i2c_buffer_lock);
 	fe->tuner_priv = st;
 
 	if (dib0090_fw_reset_digital(fe, st->config) != 0)
diff --git a/drivers/media/dvb/frontends/dib7000m.c b/drivers/media/dvb/frontends/dib7000m.c
index 79cb1c20df24..dbb76d75c932 100644
--- a/drivers/media/dvb/frontends/dib7000m.c
+++ b/drivers/media/dvb/frontends/dib7000m.c
@@ -11,6 +11,7 @@
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
+#include <linux/mutex.h>
 
 #include "dvb_frontend.h"
 
@@ -55,6 +56,7 @@ struct dib7000m_state {
 	struct i2c_msg msg[2];
 	u8 i2c_write_buffer[4];
 	u8 i2c_read_buffer[2];
+	struct mutex i2c_buffer_lock;
 };
 
 enum dib7000m_power_mode {
@@ -69,6 +71,13 @@ enum dib7000m_power_mode {
 
 static u16 dib7000m_read_word(struct dib7000m_state *state, u16 reg)
 {
+	u16 ret;
+
+	if (mutex_lock_interruptible(&state->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return 0;
+	}
+
 	state->i2c_write_buffer[0] = (reg >> 8) | 0x80;
 	state->i2c_write_buffer[1] = reg & 0xff;
 
@@ -85,11 +94,21 @@ static u16 dib7000m_read_word(struct dib7000m_state *state, u16 reg)
 	if (i2c_transfer(state->i2c_adap, state->msg, 2) != 2)
 		dprintk("i2c read error on %d",reg);
 
-	return (state->i2c_read_buffer[0] << 8) | state->i2c_read_buffer[1];
+	ret = (state->i2c_read_buffer[0] << 8) | state->i2c_read_buffer[1];
+	mutex_unlock(&state->i2c_buffer_lock);
+
+	return ret;
 }
 
 static int dib7000m_write_word(struct dib7000m_state *state, u16 reg, u16 val)
 {
+	int ret;
+
+	if (mutex_lock_interruptible(&state->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return -EINVAL;
+	}
+
 	state->i2c_write_buffer[0] = (reg >> 8) & 0xff;
 	state->i2c_write_buffer[1] = reg & 0xff;
 	state->i2c_write_buffer[2] = (val >> 8) & 0xff;
@@ -101,7 +120,10 @@ static int dib7000m_write_word(struct dib7000m_state *state, u16 reg, u16 val)
 	state->msg[0].buf = state->i2c_write_buffer;
 	state->msg[0].len = 4;
 
-	return i2c_transfer(state->i2c_adap, state->msg, 1) != 1 ? -EREMOTEIO : 0;
+	ret = (i2c_transfer(state->i2c_adap, state->msg, 1) != 1 ?
+			-EREMOTEIO : 0);
+	mutex_unlock(&state->i2c_buffer_lock);
+	return ret;
 }
 static void dib7000m_write_tab(struct dib7000m_state *state, u16 *buf)
 {
@@ -1385,6 +1407,7 @@ struct dvb_frontend * dib7000m_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr,
 	demod                   = &st->demod;
 	demod->demodulator_priv = st;
 	memcpy(&st->demod.ops, &dib7000m_ops, sizeof(struct dvb_frontend_ops));
+	mutex_init(&st->i2c_buffer_lock);
 
 	st->timf_default = cfg->bw->timf;
 
diff --git a/drivers/media/dvb/frontends/dib7000p.c b/drivers/media/dvb/frontends/dib7000p.c
index a64a538ba364..4eb9c2b49cd5 100644
--- a/drivers/media/dvb/frontends/dib7000p.c
+++ b/drivers/media/dvb/frontends/dib7000p.c
@@ -10,6 +10,7 @@
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
+#include <linux/mutex.h>
 
 #include "dvb_math.h"
 #include "dvb_frontend.h"
@@ -68,6 +69,7 @@ struct dib7000p_state {
 	struct i2c_msg msg[2];
 	u8 i2c_write_buffer[4];
 	u8 i2c_read_buffer[2];
+	struct mutex i2c_buffer_lock;
 };
 
 enum dib7000p_power_mode {
@@ -81,6 +83,13 @@ static int dib7090_set_diversity_in(struct dvb_frontend *fe, int onoff);
 
 static u16 dib7000p_read_word(struct dib7000p_state *state, u16 reg)
 {
+	u16 ret;
+
+	if (mutex_lock_interruptible(&state->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return 0;
+	}
+
 	state->i2c_write_buffer[0] = reg >> 8;
 	state->i2c_write_buffer[1] = reg & 0xff;
 
@@ -97,11 +106,20 @@ static u16 dib7000p_read_word(struct dib7000p_state *state, u16 reg)
 	if (i2c_transfer(state->i2c_adap, state->msg, 2) != 2)
 		dprintk("i2c read error on %d", reg);
 
-	return (state->i2c_read_buffer[0] << 8) | state->i2c_read_buffer[1];
+	ret = (state->i2c_read_buffer[0] << 8) | state->i2c_read_buffer[1];
+	mutex_unlock(&state->i2c_buffer_lock);
+	return ret;
 }
 
 static int dib7000p_write_word(struct dib7000p_state *state, u16 reg, u16 val)
 {
+	int ret;
+
+	if (mutex_lock_interruptible(&state->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return -EINVAL;
+	}
+
 	state->i2c_write_buffer[0] = (reg >> 8) & 0xff;
 	state->i2c_write_buffer[1] = reg & 0xff;
 	state->i2c_write_buffer[2] = (val >> 8) & 0xff;
@@ -113,7 +131,10 @@ static int dib7000p_write_word(struct dib7000p_state *state, u16 reg, u16 val)
 	state->msg[0].buf = state->i2c_write_buffer;
 	state->msg[0].len = 4;
 
-	return i2c_transfer(state->i2c_adap, state->msg, 1) != 1 ? -EREMOTEIO : 0;
+	ret = (i2c_transfer(state->i2c_adap, state->msg, 1) != 1 ?
+			-EREMOTEIO : 0);
+	mutex_unlock(&state->i2c_buffer_lock);
+	return ret;
 }
 
 static void dib7000p_write_tab(struct dib7000p_state *state, u16 * buf)
@@ -1646,6 +1667,7 @@ int dib7000p_i2c_enumeration(struct i2c_adapter *i2c, int no_of_demods, u8 defau
 		return -ENOMEM;
 
 	dpst->i2c_adap = i2c;
+	mutex_init(&dpst->i2c_buffer_lock);
 
 	for (k = no_of_demods - 1; k >= 0; k--) {
 		dpst->cfg = cfg[k];
@@ -2324,6 +2346,7 @@ struct dvb_frontend *dib7000p_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr,
 	demod = &st->demod;
 	demod->demodulator_priv = st;
 	memcpy(&st->demod.ops, &dib7000p_ops, sizeof(struct dvb_frontend_ops));
+	mutex_init(&st->i2c_buffer_lock);
 
 	dib7000p_write_word(st, 1287, 0x0003);	/* sram lead in, rdy */
 
@@ -2333,8 +2356,9 @@ struct dvb_frontend *dib7000p_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr,
 	st->version = dib7000p_read_word(st, 897);
 
 	/* FIXME: make sure the dev.parent field is initialized, or else
-		request_firmware() will hit an OOPS (this should be moved somewhere
-		more common) */
+	   request_firmware() will hit an OOPS (this should be moved somewhere
+	   more common) */
+	st->i2c_master.gated_tuner_i2c_adap.dev.parent = i2c_adap->dev.parent;
 
 	/* FIXME: make sure the dev.parent field is initialized, or else
 	   request_firmware() will hit an OOPS (this should be moved somewhere
diff --git a/drivers/media/dvb/frontends/dib8000.c b/drivers/media/dvb/frontends/dib8000.c
index 7d2ea112ae2b..fe284d5292f5 100644
--- a/drivers/media/dvb/frontends/dib8000.c
+++ b/drivers/media/dvb/frontends/dib8000.c
@@ -10,6 +10,8 @@
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
+#include <linux/mutex.h>
+
 #include "dvb_math.h"
 
 #include "dvb_frontend.h"
@@ -37,6 +39,7 @@ struct i2c_device {
 	u8 addr;
 	u8 *i2c_write_buffer;
 	u8 *i2c_read_buffer;
+	struct mutex *i2c_buffer_lock;
 };
 
 struct dib8000_state {
@@ -77,6 +80,7 @@ struct dib8000_state {
 	struct i2c_msg msg[2];
 	u8 i2c_write_buffer[4];
 	u8 i2c_read_buffer[2];
+	struct mutex i2c_buffer_lock;
 };
 
 enum dib8000_power_mode {
@@ -86,24 +90,39 @@ enum dib8000_power_mode {
 
 static u16 dib8000_i2c_read16(struct i2c_device *i2c, u16 reg)
 {
+	u16 ret;
 	struct i2c_msg msg[2] = {
-		{.addr = i2c->addr >> 1, .flags = 0,
-			.buf = i2c->i2c_write_buffer, .len = 2},
-		{.addr = i2c->addr >> 1, .flags = I2C_M_RD,
-			.buf = i2c->i2c_read_buffer, .len = 2},
+		{.addr = i2c->addr >> 1, .flags = 0, .len = 2},
+		{.addr = i2c->addr >> 1, .flags = I2C_M_RD, .len = 2},
 	};
 
+	if (mutex_lock_interruptible(i2c->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return 0;
+	}
+
+	msg[0].buf    = i2c->i2c_write_buffer;
 	msg[0].buf[0] = reg >> 8;
 	msg[0].buf[1] = reg & 0xff;
+	msg[1].buf    = i2c->i2c_read_buffer;
 
 	if (i2c_transfer(i2c->adap, msg, 2) != 2)
 		dprintk("i2c read error on %d", reg);
 
-	return (msg[1].buf[0] << 8) | msg[1].buf[1];
+	ret = (msg[1].buf[0] << 8) | msg[1].buf[1];
+	mutex_unlock(i2c->i2c_buffer_lock);
+	return ret;
 }
 
 static u16 dib8000_read_word(struct dib8000_state *state, u16 reg)
 {
+	u16 ret;
+
+	if (mutex_lock_interruptible(&state->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return 0;
+	}
+
 	state->i2c_write_buffer[0] = reg >> 8;
 	state->i2c_write_buffer[1] = reg & 0xff;
 
@@ -120,7 +139,10 @@ static u16 dib8000_read_word(struct dib8000_state *state, u16 reg)
 	if (i2c_transfer(state->i2c.adap, state->msg, 2) != 2)
 		dprintk("i2c read error on %d", reg);
 
-	return (state->i2c_read_buffer[0] << 8) | state->i2c_read_buffer[1];
+	ret = (state->i2c_read_buffer[0] << 8) | state->i2c_read_buffer[1];
+	mutex_unlock(&state->i2c_buffer_lock);
+
+	return ret;
 }
 
 static u32 dib8000_read32(struct dib8000_state *state, u16 reg)
@@ -135,22 +157,35 @@ static u32 dib8000_read32(struct dib8000_state *state, u16 reg)
 
 static int dib8000_i2c_write16(struct i2c_device *i2c, u16 reg, u16 val)
 {
-	struct i2c_msg msg = {.addr = i2c->addr >> 1, .flags = 0,
-		.buf = i2c->i2c_write_buffer, .len = 4};
+	struct i2c_msg msg = {.addr = i2c->addr >> 1, .flags = 0, .len = 4};
 	int ret = 0;
 
+	if (mutex_lock_interruptible(i2c->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return -EINVAL;
+	}
+
+	msg.buf    = i2c->i2c_write_buffer;
 	msg.buf[0] = (reg >> 8) & 0xff;
 	msg.buf[1] = reg & 0xff;
 	msg.buf[2] = (val >> 8) & 0xff;
 	msg.buf[3] = val & 0xff;
 
 	ret = i2c_transfer(i2c->adap, &msg, 1) != 1 ? -EREMOTEIO : 0;
+	mutex_unlock(i2c->i2c_buffer_lock);
 
 	return ret;
 }
 
 static int dib8000_write_word(struct dib8000_state *state, u16 reg, u16 val)
 {
+	int ret;
+
+	if (mutex_lock_interruptible(&state->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return -EINVAL;
+	}
+
 	state->i2c_write_buffer[0] = (reg >> 8) & 0xff;
 	state->i2c_write_buffer[1] = reg & 0xff;
 	state->i2c_write_buffer[2] = (val >> 8) & 0xff;
@@ -162,7 +197,11 @@ static int dib8000_write_word(struct dib8000_state *state, u16 reg, u16 val)
 	state->msg[0].buf = state->i2c_write_buffer;
 	state->msg[0].len = 4;
 
-	return i2c_transfer(state->i2c.adap, state->msg, 1) != 1 ? -EREMOTEIO : 0;
+	ret = (i2c_transfer(state->i2c.adap, state->msg, 1) != 1 ?
+			-EREMOTEIO : 0);
+	mutex_unlock(&state->i2c_buffer_lock);
+
+	return ret;
 }
 
 static const s16 coeff_2k_sb_1seg_dqpsk[8] = {
@@ -2434,8 +2473,15 @@ int dib8000_i2c_enumeration(struct i2c_adapter *host, int no_of_demods, u8 defau
 	if (!client.i2c_read_buffer) {
 		dprintk("%s: not enough memory", __func__);
 		ret = -ENOMEM;
-		goto error_memory;
+		goto error_memory_read;
+	}
+	client.i2c_buffer_lock = kzalloc(sizeof(struct mutex), GFP_KERNEL);
+	if (!client.i2c_buffer_lock) {
+		dprintk("%s: not enough memory", __func__);
+		ret = -ENOMEM;
+		goto error_memory_lock;
 	}
+	mutex_init(client.i2c_buffer_lock);
 
 	for (k = no_of_demods - 1; k >= 0; k--) {
 		/* designated i2c address */
@@ -2476,8 +2522,10 @@ int dib8000_i2c_enumeration(struct i2c_adapter *host, int no_of_demods, u8 defau
 	}
 
 error:
+	kfree(client.i2c_buffer_lock);
+error_memory_lock:
 	kfree(client.i2c_read_buffer);
-error_memory:
+error_memory_read:
 	kfree(client.i2c_write_buffer);
 
 	return ret;
@@ -2581,6 +2629,8 @@ struct dvb_frontend *dib8000_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, s
 	state->i2c.addr = i2c_addr;
 	state->i2c.i2c_write_buffer = state->i2c_write_buffer;
 	state->i2c.i2c_read_buffer = state->i2c_read_buffer;
+	mutex_init(&state->i2c_buffer_lock);
+	state->i2c.i2c_buffer_lock = &state->i2c_buffer_lock;
 	state->gpio_val = cfg->gpio_val;
 	state->gpio_dir = cfg->gpio_dir;
 
diff --git a/drivers/media/dvb/frontends/dib9000.c b/drivers/media/dvb/frontends/dib9000.c
index a0855883b5ce..b931074a9521 100644
--- a/drivers/media/dvb/frontends/dib9000.c
+++ b/drivers/media/dvb/frontends/dib9000.c
@@ -38,6 +38,15 @@ struct i2c_device {
 #define DibInitLock(lock) mutex_init(lock)
 #define DibFreeLock(lock)
 
+struct dib9000_pid_ctrl {
+#define DIB9000_PID_FILTER_CTRL 0
+#define DIB9000_PID_FILTER      1
+	u8 cmd;
+	u8 id;
+	u16 pid;
+	u8 onoff;
+};
+
 struct dib9000_state {
 	struct i2c_device i2c;
 
@@ -99,6 +108,10 @@ struct dib9000_state {
 	struct i2c_msg msg[2];
 	u8 i2c_write_buffer[255];
 	u8 i2c_read_buffer[255];
+	DIB_LOCK demod_lock;
+	u8 get_frontend_internal;
+	struct dib9000_pid_ctrl pid_ctrl[10];
+	s8 pid_ctrl_index; /* -1: empty list; -2: do not use the list */
 };
 
 static const u32 fe_info[44] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
@@ -1743,19 +1756,56 @@ EXPORT_SYMBOL(dib9000_set_gpio);
 int dib9000_fw_pid_filter_ctrl(struct dvb_frontend *fe, u8 onoff)
 {
 	struct dib9000_state *state = fe->demodulator_priv;
-	u16 val = dib9000_read_word(state, 294 + 1) & 0xffef;
+	u16 val;
+	int ret;
+
+	if ((state->pid_ctrl_index != -2) && (state->pid_ctrl_index < 9)) {
+		/* postpone the pid filtering cmd */
+		dprintk("pid filter cmd postpone");
+		state->pid_ctrl_index++;
+		state->pid_ctrl[state->pid_ctrl_index].cmd = DIB9000_PID_FILTER_CTRL;
+		state->pid_ctrl[state->pid_ctrl_index].onoff = onoff;
+		return 0;
+	}
+
+	DibAcquireLock(&state->demod_lock);
+
+	val = dib9000_read_word(state, 294 + 1) & 0xffef;
 	val |= (onoff & 0x1) << 4;
 
 	dprintk("PID filter enabled %d", onoff);
-	return dib9000_write_word(state, 294 + 1, val);
+	ret = dib9000_write_word(state, 294 + 1, val);
+	DibReleaseLock(&state->demod_lock);
+	return ret;
+
 }
 EXPORT_SYMBOL(dib9000_fw_pid_filter_ctrl);
 
 int dib9000_fw_pid_filter(struct dvb_frontend *fe, u8 id, u16 pid, u8 onoff)
 {
 	struct dib9000_state *state = fe->demodulator_priv;
+	int ret;
+
+	if (state->pid_ctrl_index != -2) {
+		/* postpone the pid filtering cmd */
+		dprintk("pid filter postpone");
+		if (state->pid_ctrl_index < 9) {
+			state->pid_ctrl_index++;
+			state->pid_ctrl[state->pid_ctrl_index].cmd = DIB9000_PID_FILTER;
+			state->pid_ctrl[state->pid_ctrl_index].id = id;
+			state->pid_ctrl[state->pid_ctrl_index].pid = pid;
+			state->pid_ctrl[state->pid_ctrl_index].onoff = onoff;
+		} else
+			dprintk("can not add any more pid ctrl cmd");
+		return 0;
+	}
+
+	DibAcquireLock(&state->demod_lock);
 	dprintk("Index %x, PID %d, OnOff %d", id, pid, onoff);
-	return dib9000_write_word(state, 300 + 1 + id, onoff ? (1 << 13) | pid : 0);
+	ret = dib9000_write_word(state, 300 + 1 + id,
+			onoff ? (1 << 13) | pid : 0);
+	DibReleaseLock(&state->demod_lock);
+	return ret;
 }
 EXPORT_SYMBOL(dib9000_fw_pid_filter);
 
@@ -1778,6 +1828,7 @@ static void dib9000_release(struct dvb_frontend *demod)
 	DibFreeLock(&state->platform.risc.mbx_lock);
 	DibFreeLock(&state->platform.risc.mem_lock);
 	DibFreeLock(&state->platform.risc.mem_mbx_lock);
+	DibFreeLock(&state->demod_lock);
 	dibx000_exit_i2c_master(&st->i2c_master);
 
 	i2c_del_adapter(&st->tuner_adap);
@@ -1795,14 +1846,19 @@ static int dib9000_sleep(struct dvb_frontend *fe)
 {
 	struct dib9000_state *state = fe->demodulator_priv;
 	u8 index_frontend;
-	int ret;
+	int ret = 0;
 
+	DibAcquireLock(&state->demod_lock);
 	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
 		ret = state->fe[index_frontend]->ops.sleep(state->fe[index_frontend]);
 		if (ret < 0)
-			return ret;
+			goto error;
 	}
-	return dib9000_mbx_send(state, OUT_MSG_FE_SLEEP, NULL, 0);
+	ret = dib9000_mbx_send(state, OUT_MSG_FE_SLEEP, NULL, 0);
+
+error:
+	DibReleaseLock(&state->demod_lock);
+	return ret;
 }
 
 static int dib9000_fe_get_tune_settings(struct dvb_frontend *fe, struct dvb_frontend_tune_settings *tune)
@@ -1816,7 +1872,10 @@ static int dib9000_get_frontend(struct dvb_frontend *fe, struct dvb_frontend_par
 	struct dib9000_state *state = fe->demodulator_priv;
 	u8 index_frontend, sub_index_frontend;
 	fe_status_t stat;
-	int ret;
+	int ret = 0;
+
+	if (state->get_frontend_internal == 0)
+		DibAcquireLock(&state->demod_lock);
 
 	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
 		state->fe[index_frontend]->ops.read_status(state->fe[index_frontend], &stat);
@@ -1846,14 +1905,15 @@ static int dib9000_get_frontend(struct dvb_frontend *fe, struct dvb_frontend_par
 					    state->fe[index_frontend]->dtv_property_cache.rolloff;
 				}
 			}
-			return 0;
+			ret = 0;
+			goto return_value;
 		}
 	}
 
 	/* get the channel from master chip */
 	ret = dib9000_fw_get_channel(fe, fep);
 	if (ret != 0)
-		return ret;
+		goto return_value;
 
 	/* synchronize the cache with the other frontends */
 	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
@@ -1866,8 +1926,12 @@ static int dib9000_get_frontend(struct dvb_frontend *fe, struct dvb_frontend_par
 		state->fe[index_frontend]->dtv_property_cache.code_rate_LP = fe->dtv_property_cache.code_rate_LP;
 		state->fe[index_frontend]->dtv_property_cache.rolloff = fe->dtv_property_cache.rolloff;
 	}
+	ret = 0;
 
-	return 0;
+return_value:
+	if (state->get_frontend_internal == 0)
+		DibReleaseLock(&state->demod_lock);
+	return ret;
 }
 
 static int dib9000_set_tune_state(struct dvb_frontend *fe, enum frontend_tune_state tune_state)
@@ -1912,6 +1976,10 @@ static int dib9000_set_frontend(struct dvb_frontend *fe, struct dvb_frontend_par
 		dprintk("dib9000: must specify bandwidth ");
 		return 0;
 	}
+
+	state->pid_ctrl_index = -1; /* postpone the pid filtering cmd */
+	DibAcquireLock(&state->demod_lock);
+
 	fe->dtv_property_cache.delivery_system = SYS_DVBT;
 
 	/* set the master status */
@@ -1974,13 +2042,18 @@ static int dib9000_set_frontend(struct dvb_frontend *fe, struct dvb_frontend_par
 	/* check the tune result */
 	if (exit_condition == 1) {	/* tune failed */
 		dprintk("tune failed");
+		DibReleaseLock(&state->demod_lock);
+		/* tune failed; put all the pid filtering cmd to junk */
+		state->pid_ctrl_index = -1;
 		return 0;
 	}
 
 	dprintk("tune success on frontend%i", index_frontend_success);
 
 	/* synchronize all the channel cache */
+	state->get_frontend_internal = 1;
 	dib9000_get_frontend(state->fe[0], fep);
+	state->get_frontend_internal = 0;
 
 	/* retune the other frontends with the found channel */
 	channel_status.status = CHANNEL_STATUS_PARAMETERS_SET;
@@ -2025,6 +2098,28 @@ static int dib9000_set_frontend(struct dvb_frontend *fe, struct dvb_frontend_par
 	/* turn off the diversity for the last frontend */
 	dib9000_fw_set_diversity_in(state->fe[index_frontend - 1], 0);
 
+	DibReleaseLock(&state->demod_lock);
+	if (state->pid_ctrl_index >= 0) {
+		u8 index_pid_filter_cmd;
+		u8 pid_ctrl_index = state->pid_ctrl_index;
+
+		state->pid_ctrl_index = -2;
+		for (index_pid_filter_cmd = 0;
+				index_pid_filter_cmd <= pid_ctrl_index;
+				index_pid_filter_cmd++) {
+			if (state->pid_ctrl[index_pid_filter_cmd].cmd == DIB9000_PID_FILTER_CTRL)
+				dib9000_fw_pid_filter_ctrl(state->fe[0],
+						state->pid_ctrl[index_pid_filter_cmd].onoff);
+			else if (state->pid_ctrl[index_pid_filter_cmd].cmd == DIB9000_PID_FILTER)
+				dib9000_fw_pid_filter(state->fe[0],
+						state->pid_ctrl[index_pid_filter_cmd].id,
+						state->pid_ctrl[index_pid_filter_cmd].pid,
+						state->pid_ctrl[index_pid_filter_cmd].onoff);
+		}
+	}
+	/* do not postpone any more the pid filtering */
+	state->pid_ctrl_index = -2;
+
 	return 0;
 }
 
@@ -2041,6 +2136,7 @@ static int dib9000_read_status(struct dvb_frontend *fe, fe_status_t * stat)
 	u8 index_frontend;
 	u16 lock = 0, lock_slave = 0;
 
+	DibAcquireLock(&state->demod_lock);
 	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++)
 		lock_slave |= dib9000_read_lock(state->fe[index_frontend]);
 
@@ -2059,6 +2155,8 @@ static int dib9000_read_status(struct dvb_frontend *fe, fe_status_t * stat)
 	if ((lock & 0x0008) || (lock_slave & 0x0008))
 		*stat |= FE_HAS_LOCK;
 
+	DibReleaseLock(&state->demod_lock);
+
 	return 0;
 }
 
@@ -2066,10 +2164,14 @@ static int dib9000_read_ber(struct dvb_frontend *fe, u32 * ber)
 {
 	struct dib9000_state *state = fe->demodulator_priv;
 	u16 *c;
+	int ret = 0;
 
+	DibAcquireLock(&state->demod_lock);
 	DibAcquireLock(&state->platform.risc.mem_mbx_lock);
-	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0)
-		return -EIO;
+	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0) {
+		ret = -EIO;
+		goto error;
+	}
 	dib9000_risc_mem_read(state, FE_MM_R_FE_MONITOR,
 			state->i2c_read_buffer, 16 * 2);
 	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
@@ -2077,7 +2179,10 @@ static int dib9000_read_ber(struct dvb_frontend *fe, u32 * ber)
 	c = (u16 *)state->i2c_read_buffer;
 
 	*ber = c[10] << 16 | c[11];
-	return 0;
+
+error:
+	DibReleaseLock(&state->demod_lock);
+	return ret;
 }
 
 static int dib9000_read_signal_strength(struct dvb_frontend *fe, u16 * strength)
@@ -2086,7 +2191,9 @@ static int dib9000_read_signal_strength(struct dvb_frontend *fe, u16 * strength)
 	u8 index_frontend;
 	u16 *c = (u16 *)state->i2c_read_buffer;
 	u16 val;
+	int ret = 0;
 
+	DibAcquireLock(&state->demod_lock);
 	*strength = 0;
 	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++) {
 		state->fe[index_frontend]->ops.read_signal_strength(state->fe[index_frontend], &val);
@@ -2097,8 +2204,10 @@ static int dib9000_read_signal_strength(struct dvb_frontend *fe, u16 * strength)
 	}
 
 	DibAcquireLock(&state->platform.risc.mem_mbx_lock);
-	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0)
-		return -EIO;
+	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0) {
+		ret = -EIO;
+		goto error;
+	}
 	dib9000_risc_mem_read(state, FE_MM_R_FE_MONITOR, (u8 *) c, 16 * 2);
 	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
 
@@ -2107,7 +2216,10 @@ static int dib9000_read_signal_strength(struct dvb_frontend *fe, u16 * strength)
 		*strength = 65535;
 	else
 		*strength += val;
-	return 0;
+
+error:
+	DibReleaseLock(&state->demod_lock);
+	return ret;
 }
 
 static u32 dib9000_get_snr(struct dvb_frontend *fe)
@@ -2151,6 +2263,7 @@ static int dib9000_read_snr(struct dvb_frontend *fe, u16 * snr)
 	u8 index_frontend;
 	u32 snr_master;
 
+	DibAcquireLock(&state->demod_lock);
 	snr_master = dib9000_get_snr(fe);
 	for (index_frontend = 1; (index_frontend < MAX_NUMBER_OF_FRONTENDS) && (state->fe[index_frontend] != NULL); index_frontend++)
 		snr_master += dib9000_get_snr(state->fe[index_frontend]);
@@ -2161,6 +2274,8 @@ static int dib9000_read_snr(struct dvb_frontend *fe, u16 * snr)
 	} else
 		*snr = 0;
 
+	DibReleaseLock(&state->demod_lock);
+
 	return 0;
 }
 
@@ -2168,15 +2283,22 @@ static int dib9000_read_unc_blocks(struct dvb_frontend *fe, u32 * unc)
 {
 	struct dib9000_state *state = fe->demodulator_priv;
 	u16 *c = (u16 *)state->i2c_read_buffer;
+	int ret = 0;
 
+	DibAcquireLock(&state->demod_lock);
 	DibAcquireLock(&state->platform.risc.mem_mbx_lock);
-	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0)
-		return -EIO;
+	if (dib9000_fw_memmbx_sync(state, FE_SYNC_CHANNEL) < 0) {
+		ret = -EIO;
+		goto error;
+	}
 	dib9000_risc_mem_read(state, FE_MM_R_FE_MONITOR, (u8 *) c, 16 * 2);
 	DibReleaseLock(&state->platform.risc.mem_mbx_lock);
 
 	*unc = c[12];
-	return 0;
+
+error:
+	DibReleaseLock(&state->demod_lock);
+	return ret;
 }
 
 int dib9000_i2c_enumeration(struct i2c_adapter *i2c, int no_of_demods, u8 default_addr, u8 first_addr)
@@ -2322,6 +2444,10 @@ struct dvb_frontend *dib9000_attach(struct i2c_adapter *i2c_adap, u8 i2c_addr, c
 	DibInitLock(&st->platform.risc.mbx_lock);
 	DibInitLock(&st->platform.risc.mem_lock);
 	DibInitLock(&st->platform.risc.mem_mbx_lock);
+	DibInitLock(&st->demod_lock);
+	st->get_frontend_internal = 0;
+
+	st->pid_ctrl_index = -2;
 
 	st->fe[0] = fe;
 	fe->demodulator_priv = st;
diff --git a/drivers/media/dvb/frontends/dibx000_common.c b/drivers/media/dvb/frontends/dibx000_common.c
index dc5d17a67579..774d507b66cc 100644
--- a/drivers/media/dvb/frontends/dibx000_common.c
+++ b/drivers/media/dvb/frontends/dibx000_common.c
@@ -1,4 +1,5 @@
 #include <linux/i2c.h>
+#include <linux/mutex.h>
 
 #include "dibx000_common.h"
 
@@ -10,6 +11,13 @@ MODULE_PARM_DESC(debug, "turn on debugging (default: 0)");
 
 static int dibx000_write_word(struct dibx000_i2c_master *mst, u16 reg, u16 val)
 {
+	int ret;
+
+	if (mutex_lock_interruptible(&mst->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return -EINVAL;
+	}
+
 	mst->i2c_write_buffer[0] = (reg >> 8) & 0xff;
 	mst->i2c_write_buffer[1] = reg & 0xff;
 	mst->i2c_write_buffer[2] = (val >> 8) & 0xff;
@@ -21,11 +29,21 @@ static int dibx000_write_word(struct dibx000_i2c_master *mst, u16 reg, u16 val)
 	mst->msg[0].buf = mst->i2c_write_buffer;
 	mst->msg[0].len = 4;
 
-	return i2c_transfer(mst->i2c_adap, mst->msg, 1) != 1 ? -EREMOTEIO : 0;
+	ret = i2c_transfer(mst->i2c_adap, mst->msg, 1) != 1 ? -EREMOTEIO : 0;
+	mutex_unlock(&mst->i2c_buffer_lock);
+
+	return ret;
 }
 
 static u16 dibx000_read_word(struct dibx000_i2c_master *mst, u16 reg)
 {
+	u16 ret;
+
+	if (mutex_lock_interruptible(&mst->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return 0;
+	}
+
 	mst->i2c_write_buffer[0] = reg >> 8;
 	mst->i2c_write_buffer[1] = reg & 0xff;
 
@@ -42,7 +60,10 @@ static u16 dibx000_read_word(struct dibx000_i2c_master *mst, u16 reg)
 	if (i2c_transfer(mst->i2c_adap, mst->msg, 2) != 2)
 		dprintk("i2c read error on %d", reg);
 
-	return (mst->i2c_read_buffer[0] << 8) | mst->i2c_read_buffer[1];
+	ret = (mst->i2c_read_buffer[0] << 8) | mst->i2c_read_buffer[1];
+	mutex_unlock(&mst->i2c_buffer_lock);
+
+	return ret;
 }
 
 static int dibx000_is_i2c_done(struct dibx000_i2c_master *mst)
@@ -257,6 +278,7 @@ static int dibx000_i2c_gated_gpio67_xfer(struct i2c_adapter *i2c_adap,
 					struct i2c_msg msg[], int num)
 {
 	struct dibx000_i2c_master *mst = i2c_get_adapdata(i2c_adap);
+	int ret;
 
 	if (num > 32) {
 		dprintk("%s: too much I2C message to be transmitted (%i).\
@@ -264,10 +286,15 @@ static int dibx000_i2c_gated_gpio67_xfer(struct i2c_adapter *i2c_adap,
 		return -ENOMEM;
 	}
 
-	memset(mst->msg, 0, sizeof(struct i2c_msg) * (2 + num));
-
 	dibx000_i2c_select_interface(mst, DIBX000_I2C_INTERFACE_GPIO_6_7);
 
+	if (mutex_lock_interruptible(&mst->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return -EINVAL;
+	}
+
+	memset(mst->msg, 0, sizeof(struct i2c_msg) * (2 + num));
+
 	/* open the gate */
 	dibx000_i2c_gate_ctrl(mst, &mst->i2c_write_buffer[0], msg[0].addr, 1);
 	mst->msg[0].addr = mst->i2c_addr;
@@ -282,7 +309,11 @@ static int dibx000_i2c_gated_gpio67_xfer(struct i2c_adapter *i2c_adap,
 	mst->msg[num + 1].buf = &mst->i2c_write_buffer[4];
 	mst->msg[num + 1].len = 4;
 
-	return i2c_transfer(mst->i2c_adap, mst->msg, 2 + num) == 2 + num ? num : -EIO;
+	ret = (i2c_transfer(mst->i2c_adap, mst->msg, 2 + num) == 2 + num ?
+			num : -EIO);
+
+	mutex_unlock(&mst->i2c_buffer_lock);
+	return ret;
 }
 
 static struct i2c_algorithm dibx000_i2c_gated_gpio67_algo = {
@@ -294,6 +325,7 @@ static int dibx000_i2c_gated_tuner_xfer(struct i2c_adapter *i2c_adap,
 					struct i2c_msg msg[], int num)
 {
 	struct dibx000_i2c_master *mst = i2c_get_adapdata(i2c_adap);
+	int ret;
 
 	if (num > 32) {
 		dprintk("%s: too much I2C message to be transmitted (%i).\
@@ -301,10 +333,14 @@ static int dibx000_i2c_gated_tuner_xfer(struct i2c_adapter *i2c_adap,
 		return -ENOMEM;
 	}
 
-	memset(mst->msg, 0, sizeof(struct i2c_msg) * (2 + num));
-
 	dibx000_i2c_select_interface(mst, DIBX000_I2C_INTERFACE_TUNER);
 
+	if (mutex_lock_interruptible(&mst->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return -EINVAL;
+	}
+	memset(mst->msg, 0, sizeof(struct i2c_msg) * (2 + num));
+
 	/* open the gate */
 	dibx000_i2c_gate_ctrl(mst, &mst->i2c_write_buffer[0], msg[0].addr, 1);
 	mst->msg[0].addr = mst->i2c_addr;
@@ -319,7 +355,10 @@ static int dibx000_i2c_gated_tuner_xfer(struct i2c_adapter *i2c_adap,
 	mst->msg[num + 1].buf = &mst->i2c_write_buffer[4];
 	mst->msg[num + 1].len = 4;
 
-	return i2c_transfer(mst->i2c_adap, mst->msg, 2 + num) == 2 + num ? num : -EIO;
+	ret = (i2c_transfer(mst->i2c_adap, mst->msg, 2 + num) == 2 + num ?
+			num : -EIO);
+	mutex_unlock(&mst->i2c_buffer_lock);
+	return ret;
 }
 
 static struct i2c_algorithm dibx000_i2c_gated_tuner_algo = {
@@ -390,8 +429,18 @@ static int i2c_adapter_init(struct i2c_adapter *i2c_adap,
 int dibx000_init_i2c_master(struct dibx000_i2c_master *mst, u16 device_rev,
 				struct i2c_adapter *i2c_adap, u8 i2c_addr)
 {
-	u8 tx[4];
-	struct i2c_msg m = {.addr = i2c_addr >> 1,.buf = tx,.len = 4 };
+	int ret;
+
+	mutex_init(&mst->i2c_buffer_lock);
+	if (mutex_lock_interruptible(&mst->i2c_buffer_lock) < 0) {
+		dprintk("could not acquire lock");
+		return -EINVAL;
+	}
+	memset(mst->msg, 0, sizeof(struct i2c_msg));
+	mst->msg[0].addr = i2c_addr >> 1;
+	mst->msg[0].flags = 0;
+	mst->msg[0].buf = mst->i2c_write_buffer;
+	mst->msg[0].len = 4;
 
 	mst->device_rev = device_rev;
 	mst->i2c_adap = i2c_adap;
@@ -431,9 +480,12 @@ int dibx000_init_i2c_master(struct dibx000_i2c_master *mst, u16 device_rev,
 				"DiBX000: could not initialize the master i2c_adapter\n");
 
 	/* initialize the i2c-master by closing the gate */
-	dibx000_i2c_gate_ctrl(mst, tx, 0, 0);
+	dibx000_i2c_gate_ctrl(mst, mst->i2c_write_buffer, 0, 0);
+
+	ret = (i2c_transfer(i2c_adap, mst->msg, 1) == 1);
+	mutex_unlock(&mst->i2c_buffer_lock);
 
-	return i2c_transfer(i2c_adap, &m, 1) == 1;
+	return ret;
 }
 
 EXPORT_SYMBOL(dibx000_init_i2c_master);
diff --git a/drivers/media/dvb/frontends/dibx000_common.h b/drivers/media/dvb/frontends/dibx000_common.h
index f031165c0459..5e011474be43 100644
--- a/drivers/media/dvb/frontends/dibx000_common.h
+++ b/drivers/media/dvb/frontends/dibx000_common.h
@@ -33,6 +33,7 @@ struct dibx000_i2c_master {
 	struct i2c_msg msg[34];
 	u8 i2c_write_buffer[8];
 	u8 i2c_read_buffer[2];
+	struct mutex i2c_buffer_lock;
 };
 
 extern int dibx000_init_i2c_master(struct dibx000_i2c_master *mst,
diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index f574dc012cad..a24d191a491d 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -60,12 +60,18 @@ config VIDEOBUF2_VMALLOC
 	select VIDEOBUF2_MEMOPS
 	tristate
 
-
 config VIDEOBUF2_DMA_SG
 	#depends on HAS_DMA
 	select VIDEOBUF2_CORE
 	select VIDEOBUF2_MEMOPS
 	tristate
+
+config VIDEOBUF2_DMA_NVMAP
+	select VIDEOBUF2_CORE
+	select VIDEOBUF2_MEMOPS
+	select NVMAP_ALLOW_SYSMEM
+	tristate
+
 #
 # Multimedia Video device configuration
 #
@@ -273,6 +279,15 @@ config VIDEO_ADV7180
 	  To compile this driver as a module, choose M here: the
 	  module will be called adv7180.
 
+config VIDEO_ADV7280
+	tristate "Analog Devices ADV7280 decoder (NEW)"
+	depends on VIDEO_V4L2 && I2C
+	---help---
+	  Support for the Analog Devices ADV7280 video decoder.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called adv7280.
+
 config VIDEO_BT819
 	tristate "BT819A VideoStream decoder"
 	depends on VIDEO_V4L2 && I2C
@@ -581,6 +596,7 @@ config VIDEO_VIVI
 source "drivers/media/video/davinci/Kconfig"
 
 source "drivers/media/video/omap/Kconfig"
+source "drivers/media/video/tegra/Kconfig"
 
 source "drivers/media/video/bt8xx/Kconfig"
 
@@ -763,8 +779,7 @@ source "drivers/media/video/m5mols/Kconfig"
 
 config VIDEO_OMAP3
 	tristate "OMAP 3 Camera support (EXPERIMENTAL)"
-	select OMAP_IOMMU
-	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API && ARCH_OMAP3 && EXPERIMENTAL
+	depends on OMAP_IOVMM && VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API && ARCH_OMAP3 && EXPERIMENTAL
 	---help---
 	  Driver for an OMAP 3 camera controller.
 
@@ -784,12 +799,24 @@ config SOC_CAMERA
 	  over a bus like PCI or USB. For example some i2c camera connected
 	  directly to the data bus of an SoC.
 
+config SOC_CAMERA_AS0260
+	tristate "as0260soc support (NEW)"
+	depends on SOC_CAMERA && I2C
+	help
+	  This is a V4L2 SoC camera driver for the Aptina AS0260 chip
+
 config SOC_CAMERA_IMX074
 	tristate "imx074 support"
 	depends on SOC_CAMERA && I2C
 	help
 	  This driver supports IMX074 cameras from Sony
 
+config SOC_CAMERA_MAX9526
+	tristate "max9526 support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This driver supports MAX9526 video decoders from Maxim Integrated
+
 config SOC_CAMERA_MT9M001
 	tristate "mt9m001 support"
 	depends on SOC_CAMERA && I2C
@@ -848,18 +875,36 @@ config SOC_CAMERA_OV2640
 	help
 	  This is a ov2640 camera driver
 
+config SOC_CAMERA_OV5640
+	tristate "ov5640 camera support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This is a V4L2 camera driver for the OmniVision OV5640 sensor
+
 config SOC_CAMERA_OV5642
 	tristate "ov5642 camera support"
 	depends on SOC_CAMERA && I2C
 	help
 	  This is a V4L2 camera driver for the OmniVision OV5642 sensor
 
+config SOC_CAMERA_OV5650
+	tristate "ov5650 sensor support"
+	depends on SOC_CAMERA && I2C
+	help
+	  This is a V4L2 camera driver for the OmniVision OV5650 sensor
+
 config SOC_CAMERA_OV6650
 	tristate "ov6650 sensor support"
 	depends on SOC_CAMERA && I2C
 	---help---
 	  This is a V4L2 SoC camera driver for the OmniVision OV6650 sensor
 
+config SOC_CAMERA_OV7670SOC
+	tristate "ov7670soc sensor support (NEW)"
+	depends on SOC_CAMERA && I2C
+	---help---
+	  This is a V4L2 SoC camera driver for the OmniVision OV7670 sensor
+
 config SOC_CAMERA_OV772X
 	tristate "ov772x camera support"
 	depends on SOC_CAMERA && I2C
@@ -878,6 +923,12 @@ config SOC_CAMERA_OV9740
 	help
 	  This is a ov9740 camera driver
 
+config SOC_CAMERA_TVP5150
+	tristate "tvp5150soc support (NEW)"
+	depends on SOC_CAMERA && I2C
+	help
+	  This is a V4L2 SoC camera driver for the Texas Instruments TVP5150 chip
+
 config MX1_VIDEO
 	bool
 
@@ -921,6 +972,13 @@ config VIDEO_SH_MOBILE_CEU
 	---help---
 	  This is a v4l2 driver for the SuperH Mobile CEU Interface
 
+config VIDEO_TEGRA
+	tristate "Tegra soc_camera host driver"
+	depends on VIDEO_DEV && ARCH_TEGRA && SOC_CAMERA && HAS_DMA && HAVE_CLK
+	select VIDEOBUF2_DMA_NVMAP
+	---help---
+	  This is a v4l2 driver for the Tegra camera interface
+
 config VIDEO_OMAP1
 	tristate "OMAP1 Camera Interface driver"
 	depends on VIDEO_DEV && ARCH_OMAP1 && SOC_CAMERA
diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
index 272390072aef..d7e80cf2fbe9 100644
--- a/drivers/media/video/Makefile
+++ b/drivers/media/video/Makefile
@@ -40,6 +40,7 @@ obj-$(CONFIG_VIDEO_SAA7191) += saa7191.o
 obj-$(CONFIG_VIDEO_ADV7170) += adv7170.o
 obj-$(CONFIG_VIDEO_ADV7175) += adv7175.o
 obj-$(CONFIG_VIDEO_ADV7180) += adv7180.o
+obj-$(CONFIG_VIDEO_ADV7280) += adv7280.o
 obj-$(CONFIG_VIDEO_ADV7343) += adv7343.o
 obj-$(CONFIG_VIDEO_VPX3220) += vpx3220.o
 obj-$(CONFIG_VIDEO_BT819) += bt819.o
@@ -72,19 +73,25 @@ obj-$(CONFIG_VIDEO_NOON010PC30)	+= noon010pc30.o
 obj-$(CONFIG_VIDEO_M5MOLS)	+= m5mols/
 obj-$(CONFIG_VIDEO_ADP1653)	+= adp1653.o
 
+obj-$(CONFIG_SOC_CAMERA_AS0260)		+= as0260soc.o
 obj-$(CONFIG_SOC_CAMERA_IMX074)		+= imx074.o
+obj-$(CONFIG_SOC_CAMERA_MAX9526)	+= max9526.o
 obj-$(CONFIG_SOC_CAMERA_MT9M001)	+= mt9m001.o
 obj-$(CONFIG_SOC_CAMERA_MT9M111)	+= mt9m111.o
 obj-$(CONFIG_SOC_CAMERA_MT9T031)	+= mt9t031.o
 obj-$(CONFIG_SOC_CAMERA_MT9T112)	+= mt9t112.o
 obj-$(CONFIG_SOC_CAMERA_MT9V022)	+= mt9v022.o
 obj-$(CONFIG_SOC_CAMERA_OV2640)		+= ov2640.o
+obj-$(CONFIG_SOC_CAMERA_OV5640)		+= ov5640.o
 obj-$(CONFIG_SOC_CAMERA_OV5642)		+= ov5642.o
+obj-$(CONFIG_SOC_CAMERA_OV5650)		+= ov5650.o
 obj-$(CONFIG_SOC_CAMERA_OV6650)		+= ov6650.o
+obj-$(CONFIG_SOC_CAMERA_OV7670SOC)	+= ov7670soc.o
 obj-$(CONFIG_SOC_CAMERA_OV772X)		+= ov772x.o
 obj-$(CONFIG_SOC_CAMERA_OV9640)		+= ov9640.o
 obj-$(CONFIG_SOC_CAMERA_OV9740)		+= ov9740.o
 obj-$(CONFIG_SOC_CAMERA_RJ54N1)		+= rj54n1cb0c.o
+obj-$(CONFIG_SOC_CAMERA_TVP5150)	+= tvp5150soc.o
 obj-$(CONFIG_SOC_CAMERA_TW9910)		+= tw9910.o
 
 # And now the v4l2 drivers:
@@ -122,6 +129,7 @@ obj-$(CONFIG_VIDEOBUF2_MEMOPS)		+= videobuf2-memops.o
 obj-$(CONFIG_VIDEOBUF2_VMALLOC)		+= videobuf2-vmalloc.o
 obj-$(CONFIG_VIDEOBUF2_DMA_CONTIG)	+= videobuf2-dma-contig.o
 obj-$(CONFIG_VIDEOBUF2_DMA_SG)		+= videobuf2-dma-sg.o
+obj-$(CONFIG_VIDEOBUF2_DMA_NVMAP)	+= videobuf2-dma-nvmap.o
 
 obj-$(CONFIG_V4L2_MEM2MEM_DEV) += v4l2-mem2mem.o
 
@@ -168,6 +176,7 @@ obj-$(CONFIG_VIDEO_MX3)			+= mx3_camera.o
 obj-$(CONFIG_VIDEO_PXA27x)		+= pxa_camera.o
 obj-$(CONFIG_VIDEO_SH_MOBILE_CSI2)	+= sh_mobile_csi2.o
 obj-$(CONFIG_VIDEO_SH_MOBILE_CEU)	+= sh_mobile_ceu_camera.o
+obj-$(CONFIG_VIDEO_TEGRA)		+= tegra_v4l2_camera.o
 obj-$(CONFIG_VIDEO_OMAP1)		+= omap1_camera.o
 obj-$(CONFIG_VIDEO_ATMEL_ISI)		+= atmel-isi.o
 
@@ -189,7 +198,9 @@ obj-$(CONFIG_VIDEO_IR_I2C)  += ir-kbd-i2c.o
 obj-y	+= davinci/
 
 obj-$(CONFIG_ARCH_OMAP)	+= omap/
+obj-$(CONFIG_ARCH_TEGRA) += tegra/
 
 EXTRA_CFLAGS += -Idrivers/media/dvb/dvb-core
 EXTRA_CFLAGS += -Idrivers/media/dvb/frontends
 EXTRA_CFLAGS += -Idrivers/media/common/tuners
+EXTRA_CFLAGS += -Idrivers/video/tegra/host
diff --git a/drivers/media/video/adv7180.c b/drivers/media/video/adv7180.c
index d2138d06bcad..d8f2f2ba193d 100644
--- a/drivers/media/video/adv7180.c
+++ b/drivers/media/video/adv7180.c
@@ -27,13 +27,20 @@
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-chip-ident.h>
+#include <media/soc_camera.h>
 #include <linux/mutex.h>
 
 #define DRIVER_NAME "adv7180"
 
 #define ADV7180_INPUT_CONTROL_REG			0x00
+#define ADV7180_INPUT_CONTROL_COMPOSITE_IN1		0x00
+#define ADV7180_INPUT_CONTROL_COMPOSITE_IN2		0x01
+#define ADV7180_INPUT_CONTROL_COMPOSITE_IN3		0x02
+#define ADV7180_INPUT_CONTROL_COMPOSITE_IN4		0x03
+#define ADV7180_INPUT_CONTROL_COMPOSITE_IN5		0x04
+#define ADV7180_INPUT_CONTROL_COMPOSITE_IN6		0x05
 #define ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM	0x00
-#define ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM_PED 0x10
+#define ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_M_SECAM	0x10
 #define ADV7180_INPUT_CONTROL_AD_PAL_N_NTSC_J_SECAM	0x20
 #define ADV7180_INPUT_CONTROL_AD_PAL_N_NTSC_M_SECAM	0x30
 #define ADV7180_INPUT_CONTROL_NTSC_J			0x40
@@ -71,7 +78,8 @@
 #define ADV7180_STATUS1_AUTOD_SECAM_525	0x70
 
 #define ADV7180_IDENT_REG 0x11
-#define ADV7180_ID_7180 0x18
+#define ADV7180_ID_7180		0x1C	/* 64-lead and 40-lead models only */
+#define ADV7180_ID2_7180	0x1E	/* 48-lead and 32-lead devices only */
 
 #define ADV7180_ICONF1_ADI		0x40
 #define ADV7180_ICONF1_ACTIVE_LOW	0x01
@@ -90,6 +98,8 @@
 #define ADV7180_IMR3_ADI	0x4C
 #define ADV7180_IMR4_ADI	0x50
 
+#define ADV7180_VS_FIELD_REG	0x58
+
 struct adv7180_state {
 	struct v4l2_subdev	sd;
 	struct work_struct	work;
@@ -97,6 +107,7 @@ struct adv7180_state {
 	int			irq;
 	v4l2_std_id		curr_norm;
 	bool			autodetect;
+	int			active_input;
 };
 
 static v4l2_std_id adv7180_std_to_v4l2(u8 status1)
@@ -224,14 +235,18 @@ static int adv7180_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
 	if (std == V4L2_STD_ALL) {
 		ret = i2c_smbus_write_byte_data(client,
 			ADV7180_INPUT_CONTROL_REG,
-			ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM);
+			ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM |
+					(ADV7180_INPUT_CONTROL_COMPOSITE_IN1 +
+					 state->active_input));
 		if (ret < 0)
 			goto out;
 
 		__adv7180_status(client, NULL, &state->curr_norm);
 		state->autodetect = true;
 	} else {
-		ret = v4l2_std_to_adv7180(std);
+		ret = v4l2_std_to_adv7180(std) |
+				(ADV7180_INPUT_CONTROL_COMPOSITE_IN1 +
+				 state->active_input);
 		if (ret < 0)
 			goto out;
 
@@ -249,19 +264,99 @@ out:
 	return ret;
 }
 
+static int adv7180_set_bus_param(struct soc_camera_device *icd,
+				 unsigned long flags)
+{
+	return 0;
+}
+
+/* Request bus settings on camera side */
+static unsigned long adv7180_query_bus_param(struct soc_camera_device *icd)
+{
+	struct soc_camera_link *icl = to_soc_camera_link(icd);
+
+	unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
+		SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
+		SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8;
+
+	return soc_camera_apply_sensor_flags(icl, flags);
+}
+
+static enum v4l2_mbus_pixelcode adv7180_codes[] = {
+	V4L2_MBUS_FMT_YUYV8_2X8,
+};
+
+static int adv7180_s_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	enum v4l2_colorspace cspace;
+	enum v4l2_mbus_pixelcode code = mf->code;
+//	struct i2c_client *client = v4l2_get_subdevdata(sd);
+//	u8 status1;
+
+//	status1 = i2c_smbus_read_byte_data(client, ADV7180_STATUS1_REG);
+//	printk(KERN_ERR "*********************************** status1 = 0x%02x\n", status1);
+
+	switch (code) {
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+		cspace = V4L2_COLORSPACE_SRGB;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	mf->code        = code;
+	mf->colorspace  = cspace;
+
+	return adv7180_s_std(sd, V4L2_STD_ALL);
+}
+
+static int adv7180_try_fmt(struct v4l2_subdev *sd,
+                          struct v4l2_mbus_framefmt *mf)
+{
+	mf->field = V4L2_FIELD_INTERLACED_TB;
+	mf->code = V4L2_MBUS_FMT_YUYV8_2X8;
+	mf->colorspace = V4L2_COLORSPACE_SRGB;
+
+	// PAL
+	mf->width = 720;
+	mf->height = 576;
+
+	return 0;
+}
+
+static int adv7180_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+                           enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(adv7180_codes))
+		return -EINVAL;
+
+	*code = adv7180_codes[index];
+
+	return 0;
+}
+
+static struct soc_camera_ops adv7180_ops = {
+	.set_bus_param		= adv7180_set_bus_param,
+	.query_bus_param	= adv7180_query_bus_param,
+};
+
 static const struct v4l2_subdev_video_ops adv7180_video_ops = {
-	.querystd = adv7180_querystd,
-	.g_input_status = adv7180_g_input_status,
+	.s_mbus_fmt		= adv7180_s_fmt,
+	.try_mbus_fmt		= adv7180_try_fmt,
+	.enum_mbus_fmt		= adv7180_enum_fmt,
+	.querystd		= adv7180_querystd,
+	.g_input_status		= adv7180_g_input_status,
 };
 
 static const struct v4l2_subdev_core_ops adv7180_core_ops = {
-	.g_chip_ident = adv7180_g_chip_ident,
-	.s_std = adv7180_s_std,
+	.g_chip_ident		= adv7180_g_chip_ident,
+	.s_std			= adv7180_s_std,
 };
 
-static const struct v4l2_subdev_ops adv7180_ops = {
-	.core = &adv7180_core_ops,
-	.video = &adv7180_video_ops,
+static const struct v4l2_subdev_ops adv7180_subdev_ops = {
+	.core			= &adv7180_core_ops,
+	.video			= &adv7180_video_ops,
 };
 
 static void adv7180_work(struct work_struct *work)
@@ -297,6 +392,36 @@ static irqreturn_t adv7180_irq(int irq, void *devid)
 	return IRQ_HANDLED;
 }
 
+static int vidioc_s_input(struct file *file, void *priv, unsigned int i) {
+	struct soc_camera_device *icd = file->private_data;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct adv7180_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 val;
+
+	if (i < 6) {
+		state->active_input = i;
+		val = i2c_smbus_read_byte_data(client,
+				ADV7180_INPUT_CONTROL_REG);
+		val &= 0xf0;
+		val |= (ADV7180_INPUT_CONTROL_COMPOSITE_IN1 +
+			state->active_input);
+		return i2c_smbus_write_byte_data(client,
+				ADV7180_INPUT_CONTROL_REG, val);
+	}
+	return -EINVAL;
+}
+
+static int vidioc_g_input(struct file *file, void *priv, unsigned int *i) {
+	struct soc_camera_device *icd = file->private_data;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct adv7180_state *state = to_state(sd);
+
+	*i = state->active_input;
+
+	return 0;
+}
+
 /*
  * Generic i2c probe
  * concerning the addresses: i2c wants 7 bit (without the r/w bit), so '>>1'
@@ -306,8 +431,11 @@ static __devinit int adv7180_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
 	struct adv7180_state *state;
+	struct soc_camera_device *icd = client->dev.platform_data;
 	struct v4l2_subdev *sd;
+	u8 ident;
 	int ret;
+	struct v4l2_ioctl_ops *ops;
 
 	/* Check if the adapter supports the needed features */
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
@@ -322,17 +450,25 @@ static __devinit int adv7180_probe(struct i2c_client *client,
 		goto err;
 	}
 
+	ident = i2c_smbus_read_byte_data(client, ADV7180_IDENT_REG);
+	WARN_ON((ident != ADV7180_ID_7180) && (ident != ADV7180_ID2_7180));
+	v4l_info(client, "ident reg is 0x%02x\n", ident);
+
 	state->irq = client->irq;
 	INIT_WORK(&state->work, adv7180_work);
 	mutex_init(&state->mutex);
 	state->autodetect = true;
+	state->active_input = 0; // input 1
 	sd = &state->sd;
-	v4l2_i2c_subdev_init(sd, client, &adv7180_ops);
+	v4l2_i2c_subdev_init(sd, client, &adv7180_subdev_ops);
+	icd->ops = &adv7180_ops;
 
 	/* Initialize adv7180 */
 	/* Enable autodetection */
 	ret = i2c_smbus_write_byte_data(client, ADV7180_INPUT_CONTROL_REG,
-		ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM);
+		ADV7180_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM |
+				(ADV7180_INPUT_CONTROL_COMPOSITE_IN1 +
+				 state->active_input));
 	if (ret < 0)
 		goto err_unreg_subdev;
 
@@ -351,6 +487,12 @@ static __devinit int adv7180_probe(struct i2c_client *client,
 	/* read current norm */
 	__adv7180_status(client, NULL, &state->curr_norm);
 
+	/* VSYNC output (not FIELD) */
+	ret = i2c_smbus_write_byte_data(client,
+		ADV7180_VS_FIELD_REG, 0x01);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
 	/* register for interrupts */
 	if (state->irq > 0) {
 		ret = request_irq(state->irq, adv7180_irq, 0, DRIVER_NAME,
@@ -393,6 +535,15 @@ static __devinit int adv7180_probe(struct i2c_client *client,
 			goto err_unreg_subdev;
 	}
 
+	/*
+	 * this is the only way to support more than one input as soc_camera
+	 * assumes in its own vidioc_s(g)_input implementation that only one
+	 * input is present we have to override that with our own handlers.
+	 */
+	ops = (struct v4l2_ioctl_ops*)icd->vdev->ioctl_ops;
+	ops->vidioc_s_input = &vidioc_s_input;
+	ops->vidioc_g_input = &vidioc_g_input;
+
 	return 0;
 
 err_unreg_subdev:
@@ -432,7 +583,7 @@ static const struct i2c_device_id adv7180_id[] = {
 	{},
 };
 
-MODULE_DEVICE_TABLE(i2c, adv7180_id);
+//MODULE_DEVICE_TABLE(i2c, adv7180_id);
 
 static struct i2c_driver adv7180_driver = {
 	.driver = {
@@ -460,4 +611,3 @@ module_exit(adv7180_exit);
 MODULE_DESCRIPTION("Analog Devices ADV7180 video decoder driver");
 MODULE_AUTHOR("Mocean Laboratories");
 MODULE_LICENSE("GPL v2");
-
diff --git a/drivers/media/video/adv7280.c b/drivers/media/video/adv7280.c
new file mode 100644
index 000000000000..47572b65dfd9
--- /dev/null
+++ b/drivers/media/video/adv7280.c
@@ -0,0 +1,749 @@
+/*
+ * ADV7280 camera decoder driver
+ *
+ * Copyright (c) 2014 Antmicro Ltd <www.antmicro.com>
+ * Based on ADV7180 video decoder driver,
+ * Copyright (c) 2009 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <media/v4l2-ioctl.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/soc_camera.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+
+#define DRIVER_NAME "adv7280"
+
+#define HW_DEINT /* Enable hardware deinterlacer */
+#define VPP_SLAVE_ADDRESS 0x42
+
+/* User Sub Map Regs */
+#define ADV7280_INPUT_CONTROL 0x00
+#define ADV7280_VIDEO_SELECTION_1 0x01
+#define ADV7280_VIDEO_SELECTION_2 0x02
+#define ADV7280_OUTPUT_CONTROL 0x03
+#define ADV7280_EXTENDED_OUTPUT_CONTROL 0x04
+#define ADV7280_AUTODETECT_ENABLE 0x07
+#define ADV7280_ADI_CONTROL_1 0x0E
+#define ADV7280_POWER_MANAGEMENT 0x0F
+#define ADV7280_STATUS_1 0x10
+#define ADV7280_IDENT 0x11
+#define ADV7280_SHAPING_FILTER_CONTROL_1 0x17
+#define ADV7280_ADI_CONTROL_2 0x1D
+#define ADV7280_PIXEL_DELAY_CONTROL 0x27
+#define ADV7280_VPP_SLAVE_ADDRESS 0xFD
+#define ADV7280_OUTPUT_SYNC_SELECT_2 0x6B
+
+/* VPP regs */
+#define VPP_DEINT_RESET 0x41
+#define VPP_I2C_DEINT_ENABLE 0x55
+#define VPP_ADV_TIMING_MODE_EN 0x5B
+
+#define ADV7280_EXTENDED_OUTPUT_CONTROL_NTSCDIS		0xC5
+#define ADV7280_AUTODETECT_DEFAULT			0x7f
+
+#define ADV7280_INPUT_CONTROL_COMPOSITE_IN1		0x00
+#define ADV7280_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM	0x00
+
+#define ADV7280_INPUT_CONTROL_NTSC_M 0x50
+#define ADV7280_INPUT_CONTROL_PAL60 0x60
+#define ADV7280_INPUT_CONTROL_NTSC_443 0x70
+#define ADV7280_INPUT_CONTROL_PAL_BG 0x80
+#define ADV7280_INPUT_CONTROL_PAL_N 0x90
+#define ADV7280_INPUT_CONTROL_PAL_M 0xa0
+#define ADV7280_INPUT_CONTROL_PAL_M_PED 0xb0
+#define ADV7280_INPUT_CONTROL_PAL_COMB_N 0xc0
+#define ADV7280_INPUT_CONTROL_PAL_COMB_N_PED 0xd0
+#define ADV7280_INPUT_CONTROL_PAL_SECAM 0xe0
+
+#define ADV7280_ADI_CTRL_IRQ_SPACE			0x20
+
+#define ADV7280_STATUS1_IN_LOCK		0x01
+#define ADV7280_STATUS1_AUTOD_MASK	0x70
+#define ADV7280_STATUS1_AUTOD_NTSM_M_J	0x00
+#define ADV7280_STATUS1_AUTOD_NTSC_4_43 0x10
+#define ADV7280_STATUS1_AUTOD_PAL_M	0x20
+#define ADV7280_STATUS1_AUTOD_PAL_60	0x30
+#define ADV7280_STATUS1_AUTOD_PAL_B_G	0x40
+#define ADV7280_STATUS1_AUTOD_SECAM	0x50
+#define ADV7280_STATUS1_AUTOD_PAL_COMB	0x60
+#define ADV7280_STATUS1_AUTOD_SECAM_525	0x70
+
+#define ADV7280_ICONF1_ADI		0x40
+#define ADV7280_ICONF1_ACTIVE_LOW	0x01
+#define ADV7280_ICONF1_PSYNC_ONLY	0x10
+
+#define ADV7280_IMR1_ADI	0x44
+#define ADV7280_IMR2_ADI	0x48
+#define ADV7280_IRQ3_AD_CHANGE	0x08
+#define ADV7280_ISR3_ADI	0x4A
+#define ADV7280_ICR3_ADI	0x4B
+#define ADV7280_IMR3_ADI	0x4C
+#define ADV7280_IMR4_ADI	0x50
+
+struct adv7280_state {
+	struct v4l2_subdev	sd;
+	struct work_struct	work;
+	struct mutex		mutex; /* mutual excl. when accessing chip */
+	int			irq;
+	v4l2_std_id		curr_norm;
+	bool			autodetect;
+	int			active_input;
+};
+
+static v4l2_std_id adv7280_std_to_v4l2(u8 status1)
+{
+	switch (status1 & ADV7280_STATUS1_AUTOD_MASK) {
+	case ADV7280_STATUS1_AUTOD_NTSM_M_J:
+		return V4L2_STD_NTSC;
+	case ADV7280_STATUS1_AUTOD_NTSC_4_43:
+		return V4L2_STD_NTSC_443;
+	case ADV7280_STATUS1_AUTOD_PAL_M:
+		return V4L2_STD_PAL_M;
+	case ADV7280_STATUS1_AUTOD_PAL_60:
+		return V4L2_STD_PAL_60;
+	case ADV7280_STATUS1_AUTOD_PAL_B_G:
+		return V4L2_STD_PAL;
+	case ADV7280_STATUS1_AUTOD_SECAM:
+		return V4L2_STD_SECAM;
+	case ADV7280_STATUS1_AUTOD_PAL_COMB:
+		return V4L2_STD_PAL_Nc | V4L2_STD_PAL_N;
+	case ADV7280_STATUS1_AUTOD_SECAM_525:
+		return V4L2_STD_SECAM;
+	default:
+		return V4L2_STD_UNKNOWN;
+	}
+}
+
+static int v4l2_std_to_adv7280(v4l2_std_id std)
+{
+	if (std == V4L2_STD_PAL_60)
+		return ADV7280_INPUT_CONTROL_PAL60;
+	if (std == V4L2_STD_NTSC_443)
+		return ADV7280_INPUT_CONTROL_NTSC_443;
+	if (std == V4L2_STD_PAL_N)
+		return ADV7280_INPUT_CONTROL_PAL_N;
+	if (std == V4L2_STD_PAL_M)
+		return ADV7280_INPUT_CONTROL_PAL_M;
+	if (std == V4L2_STD_PAL_Nc)
+		return ADV7280_INPUT_CONTROL_PAL_COMB_N;
+
+	if (std & V4L2_STD_PAL)
+		return ADV7280_INPUT_CONTROL_PAL_BG;
+	if (std & V4L2_STD_NTSC)
+		return ADV7280_INPUT_CONTROL_NTSC_M;
+	if (std & V4L2_STD_SECAM)
+		return ADV7280_INPUT_CONTROL_PAL_SECAM;
+
+	return -EINVAL;
+}
+
+static int adv7280_write_reg(struct i2c_client *client, u8 reg, u8 val)
+{
+	/* printk("(0x%02X) 0x%02X --> 0x%02X\n", client->addr, reg, val); */
+	i2c_smbus_write_byte_data(client, reg, val);
+	return 0;
+}
+
+static u32 adv7280_status_to_v4l2(u8 status1)
+{
+	if (!(status1 & ADV7280_STATUS1_IN_LOCK))
+		return V4L2_IN_ST_NO_SIGNAL;
+
+	return 0;
+}
+
+static int __adv7280_status(struct i2c_client *client, u32 *status,
+	v4l2_std_id *std)
+{
+	int status1 = i2c_smbus_read_byte_data(client, ADV7280_STATUS_1);
+
+	if (status1 < 0)
+		return status1;
+
+	if (status)
+		*status = adv7280_status_to_v4l2(status1);
+	if (std)
+		*std = adv7280_std_to_v4l2(status1);
+
+	return 0;
+}
+
+static inline struct adv7280_state *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct adv7280_state, sd);
+}
+
+static int adv7280_querystd(struct v4l2_subdev *sd, v4l2_std_id *std)
+{
+	struct adv7280_state *state = to_state(sd);
+	int err = mutex_lock_interruptible(&state->mutex);
+	if (err)
+		return err;
+
+	/* when we are interrupt driven we know the state */
+	if (!state->autodetect || state->irq > 0)
+		*std = state->curr_norm;
+	else
+		err = __adv7280_status(v4l2_get_subdevdata(sd), NULL, std);
+
+	mutex_unlock(&state->mutex);
+	return err;
+}
+
+static int adv7280_g_input_status(struct v4l2_subdev *sd, u32 *status)
+{
+	struct adv7280_state *state = to_state(sd);
+	int ret = mutex_lock_interruptible(&state->mutex);
+	if (ret)
+		return ret;
+
+	ret = __adv7280_status(v4l2_get_subdevdata(sd), status, NULL);
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
+static int adv7280_g_chip_ident(struct v4l2_subdev *sd,
+	struct v4l2_dbg_chip_ident *chip)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_ADV7280, 0);
+}
+
+static int adv7280_s_std(struct v4l2_subdev *sd, v4l2_std_id std)
+{
+	struct adv7280_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = mutex_lock_interruptible(&state->mutex);
+	if (ret)
+		return ret;
+
+	/* all standards -> autodetect */
+	if (std == V4L2_STD_ALL) {
+		ret = adv7280_write_reg(client,
+			ADV7280_INPUT_CONTROL,
+			ADV7280_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM |
+					(ADV7280_INPUT_CONTROL_COMPOSITE_IN1 +
+					 state->active_input));
+		if (ret < 0)
+			goto out;
+
+		__adv7280_status(client, NULL, &state->curr_norm);
+		state->autodetect = true;
+	} else {
+		ret = v4l2_std_to_adv7280(std) |
+				(ADV7280_INPUT_CONTROL_COMPOSITE_IN1 +
+				 state->active_input);
+		if (ret < 0)
+			goto out;
+
+		ret = adv7280_write_reg(client,
+			ADV7280_INPUT_CONTROL, ret);
+		if (ret < 0)
+			goto out;
+
+		state->curr_norm = std;
+		state->autodetect = false;
+	}
+	ret = 0;
+out:
+	mutex_unlock(&state->mutex);
+	return ret;
+}
+
+static int adv7280_set_bus_param(struct soc_camera_device *icd,
+				 unsigned long flags)
+{
+	return 0;
+}
+
+/* Request bus settings on camera side */
+static unsigned long adv7280_query_bus_param(struct soc_camera_device *icd)
+{
+	struct soc_camera_link *icl = to_soc_camera_link(icd);
+
+	unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
+		SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
+		SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8;
+
+	return soc_camera_apply_sensor_flags(icl, flags);
+}
+
+static enum v4l2_mbus_pixelcode adv7280_codes[] = {
+	V4L2_MBUS_FMT_YUYV8_2X8,
+};
+
+static int adv7280_s_fmt(struct v4l2_subdev *sd,
+			 struct v4l2_mbus_framefmt *mf)
+{
+	enum v4l2_colorspace cspace;
+	enum v4l2_mbus_pixelcode code = mf->code;
+
+	switch (code) {
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+		cspace = V4L2_COLORSPACE_SRGB;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	mf->code        = code;
+	mf->colorspace  = cspace;
+
+	return adv7280_s_std(sd, V4L2_STD_ALL);
+}
+
+static int adv7280_try_fmt(struct v4l2_subdev *sd,
+                          struct v4l2_mbus_framefmt *mf)
+{
+#ifdef HW_DEINT
+	mf->field = V4L2_FIELD_NONE;
+#else
+	mf->field = V4L2_FIELD_INTERLACED_TB;
+#endif
+	mf->code = V4L2_MBUS_FMT_YVYU8_2X8;
+	mf->colorspace = V4L2_COLORSPACE_SRGB;
+
+	// PAL
+	mf->width = 720;
+	mf->height = 576;
+
+	return 0;
+}
+
+static int adv7280_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+                           enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(adv7280_codes))
+		return -EINVAL;
+
+	*code = adv7280_codes[index];
+
+	return 0;
+}
+
+static struct soc_camera_ops adv7280_ops = {
+	.set_bus_param		= adv7280_set_bus_param,
+	.query_bus_param	= adv7280_query_bus_param,
+};
+
+static const struct v4l2_subdev_video_ops adv7280_video_ops = {
+	.s_mbus_fmt		= adv7280_s_fmt,
+	.try_mbus_fmt		= adv7280_try_fmt,
+	.enum_mbus_fmt		= adv7280_enum_fmt,
+	.querystd		= adv7280_querystd,
+	.g_input_status		= adv7280_g_input_status,
+};
+
+static const struct v4l2_subdev_core_ops adv7280_core_ops = {
+	.g_chip_ident		= adv7280_g_chip_ident,
+	.s_std			= adv7280_s_std,
+};
+
+static const struct v4l2_subdev_ops adv7280_subdev_ops = {
+	.core			= &adv7280_core_ops,
+	.video			= &adv7280_video_ops,
+};
+
+static void adv7280_work(struct work_struct *work)
+{
+	struct adv7280_state *state = container_of(work, struct adv7280_state,
+		work);
+	struct i2c_client *client = v4l2_get_subdevdata(&state->sd);
+	u8 isr3;
+
+	mutex_lock(&state->mutex);
+	adv7280_write_reg(client, ADV7280_ADI_CONTROL_1,
+		ADV7280_ADI_CTRL_IRQ_SPACE);
+	isr3 = i2c_smbus_read_byte_data(client, ADV7280_ISR3_ADI);
+	/* clear */
+	adv7280_write_reg(client, ADV7280_ICR3_ADI, isr3);
+	adv7280_write_reg(client, ADV7280_ADI_CONTROL_1, 0);
+
+	if (isr3 & ADV7280_IRQ3_AD_CHANGE && state->autodetect)
+		__adv7280_status(client, NULL, &state->curr_norm);
+	mutex_unlock(&state->mutex);
+
+	enable_irq(state->irq);
+}
+
+static irqreturn_t adv7280_irq(int irq, void *devid)
+{
+	struct adv7280_state *state = devid;
+
+	schedule_work(&state->work);
+
+	disable_irq_nosync(state->irq);
+
+	return IRQ_HANDLED;
+}
+
+static int vidioc_s_input(struct file *file, void *priv, unsigned int i) {
+	struct soc_camera_device *icd = file->private_data;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct adv7280_state *state = to_state(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 val;
+
+	if (i < 6) {
+		state->active_input = i;
+		val = i2c_smbus_read_byte_data(client,
+				ADV7280_INPUT_CONTROL);
+		val &= 0xf0;
+		val |= (ADV7280_INPUT_CONTROL_COMPOSITE_IN1 +
+			state->active_input);
+		return adv7280_write_reg(client,
+				ADV7280_INPUT_CONTROL, val);
+	}
+	return -EINVAL;
+}
+
+static int vidioc_g_input(struct file *file, void *priv, unsigned int *i) {
+	struct soc_camera_device *icd = file->private_data;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct adv7280_state *state = to_state(sd);
+
+	*i = state->active_input;
+
+	return 0;
+}
+
+/*
+ * Generic i2c probe
+ * concerning the addresses: i2c wants 7 bit (without the r/w bit), so '>>1'
+ */
+static __devinit int adv7280_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct adv7280_state *state;
+	struct soc_camera_device *icd = client->dev.platform_data;
+	struct v4l2_subdev *sd;
+	u8 ident;
+	int ret;
+	struct v4l2_ioctl_ops *ops;
+	struct i2c_client vpp_client = {
+		.flags = client->flags,
+		.addr = VPP_SLAVE_ADDRESS,
+		.name = "ADV7180_VPP",
+		.adapter = client->adapter,
+		.dev = client->dev,
+	};
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	v4l_info(client, "chip found @ 0x%02x (%s)\n",
+			client->addr << 1, client->adapter->name);
+
+	state = kzalloc(sizeof(struct adv7280_state), GFP_KERNEL);
+	if (state == NULL) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	ident = i2c_smbus_read_byte_data(client, ADV7280_IDENT);
+	v4l_info(client, "ident reg is 0x%02x\n", ident);
+
+	state->irq = client->irq;
+	INIT_WORK(&state->work, adv7280_work);
+	mutex_init(&state->mutex);
+	state->autodetect = true;
+	state->active_input = 0; // input 1
+	sd = &state->sd;
+	v4l2_i2c_subdev_init(sd, client, &adv7280_subdev_ops);
+	icd->ops = &adv7280_ops;
+
+	/* Reset */
+	ret = adv7280_write_reg(client,
+		ADV7280_POWER_MANAGEMENT, 0xA0);
+	if (ret < 0)
+		goto err_unreg_subdev;
+	msleep(10);
+
+	/* Initialize adv7280 */
+	/* Exit Power Down Mode */
+	ret = adv7280_write_reg(client,
+		ADV7280_POWER_MANAGEMENT, 0x00);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* analog devices recommends */
+	ret = adv7280_write_reg(client,
+		ADV7280_ADI_CONTROL_1, 0x80);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* analog devices recommends */
+	ret = adv7280_write_reg(client,
+		0x9C, 0x00);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* analog devices recommends */
+	ret = adv7280_write_reg(client,
+		0x9C, 0xFF);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* Enter User Sub Map */
+	ret = adv7280_write_reg(client,
+		ADV7280_ADI_CONTROL_1, 0x00);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* Enable Pixel & Sync output drivers */
+	ret = adv7280_write_reg(client,
+		ADV7280_OUTPUT_CONTROL, 0x0C);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* Power-up INTRQ, HS & VS pads */
+	ret = adv7280_write_reg(client,
+		ADV7280_EXTENDED_OUTPUT_CONTROL, 0x07);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* Enable SH1 */
+	/*
+	ret = adv7280_write_reg(client,
+		ADV7280_SHAPING_FILTER_CONTROL_1, 0x41);
+	if (ret < 0)
+		goto err_unreg_subdev;
+	*/
+
+	/* Disable comb filtering */
+	/*
+	ret = adv7280_write_reg(client,
+		0x39, 0x24);
+	if (ret < 0)
+		goto err_unreg_subdev;
+	*/
+
+	/* Enable LLC output driver */
+	ret = adv7280_write_reg(client,
+		ADV7280_ADI_CONTROL_2, 0x40);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* VSYNC on VS/FIELD/SFL pin */
+	ret = adv7280_write_reg(client,
+		ADV7280_OUTPUT_SYNC_SELECT_2, 0x01);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* Enable autodetection */
+	ret = adv7280_write_reg(client, ADV7280_INPUT_CONTROL,
+		ADV7280_INPUT_CONTROL_AD_PAL_BG_NTSC_J_SECAM |
+				(ADV7280_INPUT_CONTROL_COMPOSITE_IN1 +
+				 state->active_input));
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	ret = adv7280_write_reg(client, ADV7280_AUTODETECT_ENABLE,
+		ADV7280_AUTODETECT_DEFAULT);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* ITU-R BT.656-4 compatible
+	ret = adv7280_write_reg(client,
+		ADV7280_EXTENDED_OUTPUT_CONTROL,
+		ADV7280_EXTENDED_OUTPUT_CONTROL_NTSCDIS);
+	if (ret < 0)
+		goto err_unreg_subdev;
+		*/
+
+	/* analog devices recommends */
+	ret = adv7280_write_reg(client,
+		0x52, 0xCD);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* analog devices recommends */
+	ret = adv7280_write_reg(client,
+		0x80, 0x51);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* analog devices recommends */
+	ret = adv7280_write_reg(client,
+		0x81, 0x51);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* analog devices recommends */
+	ret = adv7280_write_reg(client,
+		0x82, 0x68);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+#ifdef HW_DEINT
+	/* Set VPP Map */
+	ret = adv7280_write_reg(client,
+		ADV7280_VPP_SLAVE_ADDRESS, (VPP_SLAVE_ADDRESS << 1));
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* VPP - not documented */
+	ret = adv7280_write_reg(&vpp_client,
+		0xA3, 0x00);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* VPP - Enbable Advanced Timing Mode */
+	ret = adv7280_write_reg(&vpp_client,
+		VPP_ADV_TIMING_MODE_EN, 0x00);
+	if (ret < 0)
+		goto err_unreg_subdev;
+
+	/* VPP - Enable Deinterlacer */
+	ret = adv7280_write_reg(&vpp_client,
+		VPP_I2C_DEINT_ENABLE, 0x80);
+	if (ret < 0)
+		goto err_unreg_subdev;
+#endif
+
+	/* read current norm */
+	__adv7280_status(client, NULL, &state->curr_norm);
+
+	/* register for interrupts */
+	if (state->irq > 0) {
+		ret = request_irq(state->irq, adv7280_irq, 0, DRIVER_NAME,
+			state);
+		if (ret)
+			goto err_unreg_subdev;
+
+		ret = adv7280_write_reg(client, ADV7280_ADI_CONTROL_1,
+			ADV7280_ADI_CTRL_IRQ_SPACE);
+		if (ret < 0)
+			goto err_unreg_subdev;
+
+		/* config the Interrupt pin to be active low */
+		ret = adv7280_write_reg(client, ADV7280_ICONF1_ADI,
+			ADV7280_ICONF1_ACTIVE_LOW | ADV7280_ICONF1_PSYNC_ONLY);
+		if (ret < 0)
+			goto err_unreg_subdev;
+
+		ret = adv7280_write_reg(client, ADV7280_IMR1_ADI, 0);
+		if (ret < 0)
+			goto err_unreg_subdev;
+
+		ret = adv7280_write_reg(client, ADV7280_IMR2_ADI, 0);
+		if (ret < 0)
+			goto err_unreg_subdev;
+
+		/* enable AD change interrupts interrupts */
+		ret = adv7280_write_reg(client, ADV7280_IMR3_ADI,
+			ADV7280_IRQ3_AD_CHANGE);
+		if (ret < 0)
+			goto err_unreg_subdev;
+
+		ret = adv7280_write_reg(client, ADV7280_IMR4_ADI, 0);
+		if (ret < 0)
+			goto err_unreg_subdev;
+
+		ret = adv7280_write_reg(client, ADV7280_ADI_CONTROL_1,
+			0);
+		if (ret < 0)
+			goto err_unreg_subdev;
+	}
+
+	/*
+	 * this is the only way to support more than one input as soc_camera
+	 * assumes in its own vidioc_s(g)_input implementation that only one
+	 * input is present we have to override that with our own handlers.
+	 */
+	ops = (struct v4l2_ioctl_ops*)icd->vdev->ioctl_ops;
+	ops->vidioc_s_input = &vidioc_s_input;
+	ops->vidioc_g_input = &vidioc_g_input;
+
+	return 0;
+
+err_unreg_subdev:
+	mutex_destroy(&state->mutex);
+	v4l2_device_unregister_subdev(sd);
+	kfree(state);
+err:
+	printk(KERN_ERR DRIVER_NAME ": Failed to probe: %d\n", ret);
+	return ret;
+}
+
+static __devexit int adv7280_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct adv7280_state *state = to_state(sd);
+
+	if (state->irq > 0) {
+		free_irq(client->irq, state);
+		if (cancel_work_sync(&state->work)) {
+			/*
+			 * Work was pending, therefore we need to enable
+			 * IRQ here to balance the disable_irq() done in the
+			 * interrupt handler.
+			 */
+			enable_irq(state->irq);
+		}
+	}
+
+	mutex_destroy(&state->mutex);
+	v4l2_device_unregister_subdev(sd);
+	kfree(to_state(sd));
+	return 0;
+}
+
+static const struct i2c_device_id adv7280_id[] = {
+	{DRIVER_NAME, 0},
+	{},
+};
+
+//MODULE_DEVICE_TABLE(i2c, adv7280_id);
+
+static struct i2c_driver adv7280_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= DRIVER_NAME,
+	},
+	.probe		= adv7280_probe,
+	.remove		= __devexit_p(adv7280_remove),
+	.id_table	= adv7280_id,
+};
+
+static __init int adv7280_init(void)
+{
+	return i2c_add_driver(&adv7280_driver);
+}
+
+static __exit void adv7280_exit(void)
+{
+	i2c_del_driver(&adv7280_driver);
+}
+
+module_init(adv7280_init);
+module_exit(adv7280_exit);
+
+MODULE_DESCRIPTION("Analog Devices ADV7280 video decoder driver");
+MODULE_AUTHOR("Antmicro Ltd <www.antmicro.com>, Mocean Laboratories");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/video/as0260soc.c b/drivers/media/video/as0260soc.c
new file mode 100644
index 000000000000..1faed420330a
--- /dev/null
+++ b/drivers/media/video/as0260soc.c
@@ -0,0 +1,802 @@
+/*
+ * Aptina AS0260 SoC Camera Driver
+ *
+ * Copyright (c) 2014 Antmicro Ltd <www.antmicro.com>
+ * Based on Generic Platform Camera Driver,
+ * Copyright (C) 2008 Magnus Damm
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+
+#include <media/soc_camera.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-ioctl.h>
+#include <media/tegra_v4l2_camera.h>
+
+#define MODULE_NAME "as0260soc"
+#define I2C_RETRY_COUNT 5
+
+static unsigned int debug = 0;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level");
+
+/* SYSCTL Regs */
+#define CHIP_VERSION_REG 0x0000
+#define RESET_AND_MISC_CONTROL 0x001A
+#define MCU_BOOT_MODE 0x001C
+#define PAD_SLEW 0x001E
+#define PHYSICAL_ADDRESS_ACCESS 0x098A
+#define LOGICAL_ADDRESS_ACCESS 0x098E
+#define ACCESS_CTL_STAT 0x0982
+#define COMMAND_REGISTER 0x0080
+#define SYSMGR_NEXT_STATE 0xDC00
+
+/* CAM Regs */
+/* PLL_settings */
+#define CAM_SYSCTL_PLL_ENABLE 0xCA12
+#define CAM_SYSCTL_PLL_DIVIDER_M_N 0xCA14
+#define CAM_SYSCTL_PLL_DIVIDER_P 0xCA16
+#define CAM_SYSCTL_PLL_DIVIDER_P4_P5_P6 0xCA18
+#define CAM_PORT_OUTPUT_CONTROL 0xCA1C
+#define CAM_PORT_PORCH 0xCA1E
+#define CAM_PORT_MIPI_TIMING_T_HS_ZERO 0xCA20
+#define CAM_PORT_MIPI_TIMING_T_HS_EXIT_HS_TRAIL 0xCA22
+#define CAM_PORT_MIPI_TIMING_T_CLK_POST_CLK_PRE 0xCA24
+#define CAM_PORT_MIPI_TIMING_T_CLK_TRAIL_CLK_ZERO 0xCA26
+#define CAM_PORT_MIPI_TIMING_T_LPX 0xCA28
+#define CAM_PORT_MIPI_TIMING_INIT_TIMING 0xCA2A
+#define CAM_PORT_MIPI_TIMING_T_HS_PRE 0xCA2C
+/* Timing_settings */
+#define CAM_SENSOR_CFG_Y_ADDR_START 0xC800
+#define CAM_SENSOR_CFG_X_ADDR_START 0xC802
+#define CAM_SENSOR_CFG_Y_ADDR_END 0xC804
+#define CAM_SENSOR_CFG_X_ADDR_END 0xC806
+#define CAM_SENSOR_CFG_PIXCLK_H 0xC808
+#define CAM_SENSOR_CFG_PIXCLK_L 0xC80A
+#define CAM_SENSOR_CFG_ROW_SPEED 0xC80C
+#define CAM_SENSOR_CFG_FINE_INTEG_TIME_MIN 0xC80E
+#define CAM_SENSOR_CFG_FINE_INTEG_TIME_MAX 0xC810
+#define CAM_SENSOR_CFG_FRAME_LENGTH_LINES 0xC812
+#define CAM_SENSOR_CFG_LINE_LENGTH_PCK 0xC814
+#define CAM_SENSOR_CFG_FINE_CORRECTION 0xC816
+#define CAM_SENSOR_CFG_CPIPE_LAST_ROW 0xC818
+#define RESERVED_CAM_20 0xC820
+#define CAM_SENSOR_CONTROL_READ_MODE 0xC830
+#define CAM_CROP_WINDOW_XOFFSET 0xC858
+#define CAM_CROP_WINDOW_YOFFSET 0xC85A
+#define CAM_CROP_WINDOW_WIDTH 0xC85C
+#define CAM_CROP_WINDOW_HEIGHT 0xC85E
+#define CAM_OUTPUT_WIDTH 0xC86C
+#define CAM_OUTPUT_HEIGHT 0xC86E
+#define CAM_OUTPUT_FORMAT 0xC870
+#define CAM_AET_AEMODE 0xC87C
+#define CAM_AET_MAX_FRAME_RATE 0xC88E
+#define CAM_AET_MIN_FRAME_RATE 0xC890
+#define CAM_STAT_AWB_CLIP_WINDOW_XSTART 0xC94C
+#define CAM_STAT_AWB_CLIP_WINDOW_YSTART 0xC94E
+#define CAM_STAT_AWB_CLIP_WINDOW_XEND 0xC950
+#define CAM_STAT_AWB_CLIP_WINDOW_YEND 0xC952
+#define CAM_STAT_AE_INITIAL_WINDOW_XSTART 0xC954
+#define CAM_STAT_AE_INITIAL_WINDOW_YSTART 0xC956
+#define CAM_STAT_AE_INITIAL_WINDOW_XEND 0xC958
+#define CAM_STAT_AE_INITIAL_WINDOW_YEND 0xC95A
+#define CAM_SFX_CONTROL 0xC878
+/* UVC Regs */
+#define UVC_AE_MODE_CONTROL 0xCC00
+#define UVC_AE_PRIORITY_CONTROL 0xCC02
+#define UVC_BRIGHTNESS_CONTROL 0xCC0A
+#define UVC_CONTRAST_CONTROL 0xCC0C
+#define UVC_GAIN_CONTROL 0xCC0E
+#define UVC_HUE_CONTROL 0xCC10
+#define UVC_SATURATION_CONTROL 0xCC12
+#define UVC_SHARPNESS_CONTROL 0xCC14
+#define UVC_GAMMA_CONTROL 0xCC16
+#define UVC_MANUAL_EXPOSURE_CONFIGURATION 0xCC20
+
+typedef struct {
+	u32 width;
+	u32 height;
+	enum v4l2_mbus_pixelcode mbus_code;
+	enum v4l2_field field;
+	enum v4l2_colorspace colorspace;
+} as0260soc_format_struct;
+
+
+struct as0260soc_decoder {
+	struct v4l2_subdev sd;
+	const as0260soc_format_struct *fmt_list;
+	as0260soc_format_struct *fmt;
+	int num_fmts;
+	int active_input;
+	int *port; /* 1 - CSI_A, 2 - CSI_B, 3 - Parallell */
+	int mipi_lanes; /* TODO */
+	u16 chip_ver;
+};
+
+static const struct v4l2_queryctrl as0260soc_controls[] = {
+	{
+		.id		    = V4L2_CID_BRIGHTNESS,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "brightness",
+		.minimum	= 0x00,
+		.maximum	= 0xFF,
+		.step		= 1,
+		.default_value	= 0x37,
+	},
+	{
+		.id		    = V4L2_CID_CONTRAST,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "contrast",
+		.minimum	= 0x10,
+		.maximum	= 0x40,
+		.step		= 1,
+		.default_value	= 0x20,
+	},
+	{
+		.id		    = V4L2_CID_SATURATION,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "saturation",
+		.minimum	= 0x0000,
+		.maximum	= 0x0100,
+		.step		= 1,
+		.default_value	= 0x0080,
+	},
+	{
+		.id		    = V4L2_CID_HUE,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "hue",
+		.minimum	= -2200,
+		.maximum	= 2200,
+		.step		= 1,
+		.default_value	= 0,
+	},
+	{
+		.id		    = V4L2_CID_GAMMA,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "gamma",
+		.minimum	= 0x064,
+		.maximum	= 0x118,
+		.step		= 1,
+		.default_value	= 0x0DC,
+	},
+	{
+		.id		    = V4L2_CID_COLORFX,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "special effects",
+		.minimum	= 0,
+		.maximum	= 5,
+		.step		= 1,
+		.default_value	= 0,
+	},
+	/* and much more... */
+};
+
+struct as0260soc_reg {
+	u16	addr;
+	u16	val;
+	u16	mask;
+};
+
+/* 1080p@18.75 FPS, YUY2 */
+static const struct as0260soc_reg as0260soc_preset_1080p_18[] = {
+	{ CAM_SENSOR_CFG_Y_ADDR_START, 0x0020, 2},
+	{ CAM_SENSOR_CFG_X_ADDR_START, 0x0020, 2},
+	{ CAM_SENSOR_CFG_Y_ADDR_END, 0x045F, 2},
+	{ CAM_SENSOR_CFG_X_ADDR_END, 0x07A7, 2},
+	{ CAM_SENSOR_CFG_ROW_SPEED, 0x0001, 2},
+	{ CAM_SENSOR_CFG_FINE_INTEG_TIME_MIN, 0x0336, 2},
+	{ CAM_SENSOR_CFG_FINE_INTEG_TIME_MAX, 0x11CF, 2},
+	{ CAM_SENSOR_CFG_FRAME_LENGTH_LINES, 0x0491, 2},
+	{ CAM_SENSOR_CFG_LINE_LENGTH_PCK, 0x138D, 2},
+	{ CAM_SENSOR_CFG_FINE_CORRECTION, 0x00D4, 2},
+	{ CAM_SENSOR_CFG_CPIPE_LAST_ROW, 0x043B, 2},
+	{ CAM_SENSOR_CONTROL_READ_MODE, 0x0002, 2},
+	{ CAM_CROP_WINDOW_XOFFSET, 0x0000, 2},
+	{ CAM_CROP_WINDOW_YOFFSET, 0x0000, 2},
+	{ CAM_CROP_WINDOW_WIDTH, 0x0780, 2},
+	{ CAM_CROP_WINDOW_HEIGHT, 0x0438, 2},
+	{ CAM_OUTPUT_WIDTH, 0x0780, 2},
+	{ CAM_OUTPUT_HEIGHT, 0x0438, 2},
+	{ CAM_OUTPUT_FORMAT, 0x4010, 2},
+	{ CAM_AET_MAX_FRAME_RATE, 0x12C0, 2},
+	{ CAM_AET_MIN_FRAME_RATE, 0x12C0, 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_XSTART, 0x0000, 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_YSTART, 0x0000, 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_XEND, 0x077F, 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_YEND, 0x0437, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_XSTART, 0x0000, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_YSTART, 0x0000, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_XEND, 0x017F, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_YEND, 0x00D7, 2}
+};
+
+/* 1920x1080@30 FPS */
+static const struct as0260soc_reg as0260soc_preset_1080p_30[] = {
+	{ CAM_SENSOR_CFG_Y_ADDR_START, 0x0020, 2},
+	{ CAM_SENSOR_CFG_X_ADDR_START, 0x0020, 2},
+	{ CAM_SENSOR_CFG_Y_ADDR_END, 0x045F, 2},
+	{ CAM_SENSOR_CFG_X_ADDR_END, 0x07A7, 2},
+	{ CAM_SENSOR_CFG_FINE_INTEG_TIME_MIN, 0x0336, 2},
+	{ CAM_SENSOR_CFG_FINE_INTEG_TIME_MAX, 0x0A7A, 2},
+	{ CAM_SENSOR_CFG_FRAME_LENGTH_LINES, 0x0491, 2},
+	{ CAM_SENSOR_CFG_LINE_LENGTH_PCK, 0x0C38, 2},
+	{ CAM_SENSOR_CFG_FINE_CORRECTION, 0x00D4, 2},
+	{ CAM_SENSOR_CFG_CPIPE_LAST_ROW, 0x043B, 2},
+	{ CAM_SENSOR_CONTROL_READ_MODE, 0x0002, 2},
+	{ CAM_CROP_WINDOW_XOFFSET, 0x0000, 2},
+	{ CAM_CROP_WINDOW_YOFFSET, 0x0000, 2},
+	{ CAM_CROP_WINDOW_WIDTH, 1920, 2},
+	{ CAM_CROP_WINDOW_HEIGHT, 1080, 2},
+	{ CAM_OUTPUT_WIDTH, 1920, 2},
+	{ CAM_OUTPUT_HEIGHT, 1080, 2},
+	{ CAM_AET_MAX_FRAME_RATE, (30 * 256), 2},
+	{ CAM_AET_MIN_FRAME_RATE, (30 * 256), 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_XSTART, 0x0000, 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_YSTART, 0x0000, 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_XEND, 0x077F, 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_YEND, 0x0437, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_XSTART, 0x0000, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_YSTART, 0x0000, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_XEND, 0x017F, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_YEND, 0x00D7, 2},
+};
+
+/* 640x480@30 FPS */
+static const struct as0260soc_reg as0260soc_preset_vga_30[] = {
+	{ CAM_SENSOR_CFG_Y_ADDR_START, 0x0020, 2},
+	{ CAM_SENSOR_CFG_X_ADDR_START, 0x0100, 2},
+	{ CAM_SENSOR_CFG_Y_ADDR_END, 0x045D, 2},
+	{ CAM_SENSOR_CFG_X_ADDR_END, 0x06AD, 2},
+	{ CAM_SENSOR_CFG_ROW_SPEED, 0x0001, 2},
+	{ CAM_SENSOR_CFG_FINE_INTEG_TIME_MIN, 0x06A4, 2},
+	{ CAM_SENSOR_CFG_FINE_INTEG_TIME_MAX, 0x088C, 2},
+	{ CAM_SENSOR_CFG_FRAME_LENGTH_LINES, 0x026D, 2},
+	{ CAM_SENSOR_CFG_LINE_LENGTH_PCK, 0x0BAE, 2},
+	{ CAM_SENSOR_CFG_FINE_CORRECTION, 0x01D9, 2},
+	{ CAM_SENSOR_CFG_CPIPE_LAST_ROW, 0x021B, 2},
+	{ CAM_SENSOR_CONTROL_READ_MODE, 0x0012, 2},
+	{ CAM_CROP_WINDOW_XOFFSET, 0x0000, 2},
+	{ CAM_CROP_WINDOW_YOFFSET, 0x0000, 2},
+	{ CAM_CROP_WINDOW_WIDTH, 0x02D0, 2},
+	{ CAM_CROP_WINDOW_HEIGHT, 0x0218, 2},
+	{ CAM_OUTPUT_WIDTH, 640, 2},
+	{ CAM_OUTPUT_HEIGHT, 480, 2},
+	{ CAM_OUTPUT_FORMAT, 0x4010, 2},
+	{ CAM_AET_MAX_FRAME_RATE, 0x1E00, 2},
+	{ CAM_AET_MIN_FRAME_RATE, 0x1E00, 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_XSTART, 0x0000, 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_YSTART, 0x0000, 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_XEND, 0x027F, 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_YEND, 0x01DF, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_XSTART, 0x0000, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_YSTART, 0x0000, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_XEND, 0x007F, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_YEND, 0x005F, 2}
+};
+
+/* 640x480@60 FPS */
+static const struct as0260soc_reg as0260soc_preset_vga_60[] = {
+	{ CAM_SENSOR_CFG_Y_ADDR_START, 0x0020, 2},
+	{ CAM_SENSOR_CFG_X_ADDR_START, 0x0100, 2},
+	{ CAM_SENSOR_CFG_Y_ADDR_END, 0x045D, 2},
+	{ CAM_SENSOR_CFG_X_ADDR_END, 0x06AD, 2},
+	{ CAM_SENSOR_CFG_FINE_INTEG_TIME_MIN, 0x06A4, 2},
+	{ CAM_SENSOR_CFG_FINE_INTEG_TIME_MAX, 0x085E, 2},
+	{ CAM_SENSOR_CFG_FRAME_LENGTH_LINES, 0x026D, 2},
+	{ CAM_SENSOR_CFG_LINE_LENGTH_PCK, 0x0B80, 2},
+	{ CAM_SENSOR_CFG_FINE_CORRECTION, 0x01D9, 2},
+	{ CAM_SENSOR_CFG_CPIPE_LAST_ROW, 0x021B, 2},
+	{ CAM_SENSOR_CONTROL_READ_MODE, 0x0012, 2},
+	{ CAM_CROP_WINDOW_XOFFSET, 0x0000, 2},
+	{ CAM_CROP_WINDOW_YOFFSET, 0x0000, 2},
+	{ CAM_CROP_WINDOW_WIDTH, 0x02D0, 2},
+	{ CAM_CROP_WINDOW_HEIGHT, 0x0218, 2},
+	{ CAM_OUTPUT_WIDTH, 640, 2},
+	{ CAM_OUTPUT_HEIGHT, 480, 2},
+	{ CAM_AET_MAX_FRAME_RATE, (60 * 256), 2},
+	{ CAM_AET_MIN_FRAME_RATE, (60 * 256), 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_XSTART, 0x0000, 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_YSTART, 0x0000, 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_XEND, 0x027F, 2},
+	{ CAM_STAT_AWB_CLIP_WINDOW_YEND, 0x01DF, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_XSTART, 0x0000, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_YSTART, 0x0000, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_XEND, 0x007F, 2},
+	{ CAM_STAT_AE_INITIAL_WINDOW_YEND, 0x005F, 2},
+};
+
+as0260soc_format_struct as0260soc_formats[] = {
+	{
+		.width		= 1920,
+		.height		= 1080,
+		.mbus_code	= V4L2_MBUS_FMT_YUYV8_2X8,
+		.field		= V4L2_FIELD_NONE,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+	},
+	{
+		.width		= 640,
+		.height		= 480,
+		.mbus_code	= V4L2_MBUS_FMT_YUYV8_2X8,
+		.field		= V4L2_FIELD_NONE,
+		.colorspace	= V4L2_COLORSPACE_JPEG,
+	}
+};
+
+#define AS0260_FMTS ARRAY_SIZE(as0260soc_formats)
+
+static inline struct as0260soc_decoder *to_decoder(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct as0260soc_decoder, sd);
+}
+
+static int as0260soc_read_reg(struct v4l2_subdev *sd, u16 reg_addr, u16 *val, u8 reg_size)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct i2c_msg msg[] = {
+		{
+			.addr	= client->addr,
+			.flags	= 0,
+			.len	= 2,
+			.buf	= (u8 *)&reg_addr,
+		},
+		{
+			.addr	= client->addr,
+			.flags	= I2C_M_RD,
+			.len	= reg_size,
+			.buf	= (u8 *)val,
+		},
+	};
+	int ret, retry = 0;
+
+	reg_addr = swab16(reg_addr);
+
+read_again:
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret < 0) {
+		printk("Failed reading register 0x%04x!\n", reg_addr);
+		if (retry <= I2C_RETRY_COUNT) {
+			v4l2_warn(sd, "as0260soc: i2c error, retrying ... %d\n", retry);
+			retry++;
+			msleep_interruptible(10);
+			goto read_again;
+		}
+		return ret;
+	}
+
+	if (reg_size == 2)
+		*val = swab16(*val);
+
+	if (debug)
+		printk(KERN_INFO "as0260soc: i2c R 0x%04X <- 0x%04X\n", swab16(reg_addr), *val);
+
+	msleep_interruptible(5);
+	return 0;
+}
+
+static int as0260soc_write_reg(struct v4l2_subdev *sd, u16 addr, u16 val, u8 reg_size)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct i2c_msg msg;
+	struct {
+		u16 addr;
+		u16 val;
+	} __packed buf;
+	int ret, retry = 0;
+
+	if (reg_size < 1 && reg_size > 2)
+		return -1;
+
+	//return 0;
+
+	msg.addr	= client->addr;
+	msg.flags	= 0; /* write */
+	msg.len 	= 2 + reg_size;
+	buf.addr 	= swab16(addr);
+	if (reg_size == 1)
+		buf.val = (u8)val;
+	else
+		buf.val = swab16(val);
+
+	msg.buf		= (u8 *)&buf;
+
+	if (debug)
+		printk(KERN_INFO "as0260soc: i2c W 0x%04X -> 0x%04X (len=%db)\n", addr, val, msg.len);
+
+write_again:
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed writing register 0x%04x!\n", addr);
+		if (retry <= I2C_RETRY_COUNT) {
+			v4l2_warn(sd, "as0260soc: i2c error, retrying ... %d\n", retry);
+			retry++;
+			msleep_interruptible(10);
+			goto write_again;
+		}
+		return ret;
+	}
+
+	msleep_interruptible(5);
+
+	return 0;
+}
+
+/* write register array */
+static int as0260soc_write_reg_array(struct v4l2_subdev *sd,
+		const struct as0260soc_reg *regarray,
+		int regarraylen)
+{
+	int i;
+	int ret = 0;
+
+	for (i = 0; i < regarraylen; i++) {
+		ret = as0260soc_write_reg(sd, regarray[i].addr,
+				regarray[i].val, regarray[i].mask);
+		if (ret < 0)
+			return ret;
+	}
+
+	return ret;
+}
+
+/* change config of the camera */
+static int as0260soc_change_config(struct v4l2_subdev *sd)
+{
+	int ret = 0;
+
+	ret += as0260soc_write_reg(sd, SYSMGR_NEXT_STATE, 0x28, 1);
+	ret += as0260soc_write_reg(sd, COMMAND_REGISTER, 0x8002, 2);
+
+	/* msleep? */
+	return ret;
+}
+
+static int as0260soc_init(struct v4l2_subdev *sd)
+{
+	struct as0260soc_decoder *decoder =  to_decoder(sd);
+	int ret = 0;
+
+	ret += as0260soc_write_reg(sd, RESET_AND_MISC_CONTROL, 0x0015, 2);
+	ret += as0260soc_write_reg(sd, MCU_BOOT_MODE, 0x000C, 2);
+	ret += as0260soc_write_reg(sd, RESET_AND_MISC_CONTROL, 0x0014, 2);
+	ret += as0260soc_write_reg(sd, ACCESS_CTL_STAT, 0x0001, 2);
+	ret += as0260soc_write_reg(sd, PHYSICAL_ADDRESS_ACCESS, 0x6A44, 2);
+	ret += as0260soc_write_reg(sd, LOGICAL_ADDRESS_ACCESS, 0x0000, 2);
+	ret += as0260soc_write_reg(sd, PHYSICAL_ADDRESS_ACCESS, 0x5F38, 2);
+	ret += as0260soc_write_reg(sd, MCU_BOOT_MODE, 0x0600, 2);
+	ret += as0260soc_write_reg(sd, LOGICAL_ADDRESS_ACCESS, 0xCA12, 2);
+
+	/* PLL and clock stuff for 96 MHz */
+	ret += as0260soc_write_reg(sd, CAM_SYSCTL_PLL_ENABLE, 0x01, 1);
+	ret += as0260soc_write_reg(sd, CAM_SYSCTL_PLL_DIVIDER_M_N, 0x0010, 2);
+	ret += as0260soc_write_reg(sd, CAM_SYSCTL_PLL_DIVIDER_P, 0x0070, 2);
+	ret += as0260soc_write_reg(sd, CAM_SYSCTL_PLL_DIVIDER_P4_P5_P6, 0x7F7D, 2);
+
+	ret += as0260soc_write_reg(sd, CAM_PORT_MIPI_TIMING_T_HS_ZERO, 0x0B00, 2);
+	ret += as0260soc_write_reg(sd, CAM_PORT_MIPI_TIMING_T_HS_EXIT_HS_TRAIL, 0x0006, 2);
+	ret += as0260soc_write_reg(sd, CAM_PORT_MIPI_TIMING_T_CLK_POST_CLK_PRE, 0x0C02, 2);
+	ret += as0260soc_write_reg(sd, CAM_PORT_MIPI_TIMING_T_CLK_TRAIL_CLK_ZERO, 0x0719, 2);
+	ret += as0260soc_write_reg(sd, CAM_PORT_MIPI_TIMING_T_LPX, 0x0005, 2);
+	ret += as0260soc_write_reg(sd, CAM_PORT_MIPI_TIMING_INIT_TIMING, 0x0A0C, 2);
+	ret += as0260soc_write_reg(sd, CAM_PORT_MIPI_TIMING_T_HS_PRE, 0x00, 1);
+
+	ret += as0260soc_write_reg(sd, CAM_SENSOR_CFG_PIXCLK_H, 0x0345, 2);
+	ret += as0260soc_write_reg(sd, CAM_SENSOR_CFG_PIXCLK_L, 0x0DB6, 2);
+	ret += as0260soc_write_reg(sd, CAM_SENSOR_CFG_ROW_SPEED, 0x0001, 2);
+
+	/* output interface MIPI-CSI/VIP */
+	if (*decoder->port == 1 || *decoder->port == 2) { /* using 2-lane mipi-csi */
+		ret += as0260soc_write_reg(sd, CAM_PORT_OUTPUT_CONTROL, 0x8043, 2); /* CSI-2 */
+		ret += as0260soc_write_reg(sd, CAM_PORT_PORCH, 0x0008, 2);
+	}
+	else { /* using VIP port */
+		ret += as0260soc_write_reg(sd, PAD_SLEW, 0x0777, 2);
+		ret += as0260soc_write_reg(sd, CAM_PORT_OUTPUT_CONTROL, 0x8040, 2); /* VIP */
+		ret += as0260soc_write_reg(sd, CAM_PORT_PORCH, 0x0005, 2);
+	}
+
+	/* AE (Auto Exposure) indoor mode */
+	ret += as0260soc_write_reg(sd, CAM_AET_AEMODE, (1 << 2), 1); /* CAM_AET_EXEC_SET_INDOOR */
+
+	return ret;
+}
+
+static int as0260soc_set_bus_param(struct soc_camera_device *icd, unsigned long flags)
+{
+	/* TODO implement this functionality */
+	if (debug)
+		printk(KERN_INFO "as0260soc driver: set_bus_param function.\n");
+	return 0;
+}
+
+static unsigned long as0260soc_query_bus_param(struct soc_camera_device *icd)
+{
+	struct soc_camera_link *icl = to_soc_camera_link(icd);
+
+	unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
+		SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
+		SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8;
+
+	return soc_camera_apply_sensor_flags(icl, flags);
+}
+
+static int as0260soc_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	if (debug)
+		printk(KERN_ERR "as0260soc: g_ctrl function.\n");
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		return as0260soc_read_reg(sd, UVC_BRIGHTNESS_CONTROL, (u16 *)&ctrl->value, 2);
+	case V4L2_CID_CONTRAST:
+		return as0260soc_read_reg(sd, UVC_CONTRAST_CONTROL, (u16 *)&ctrl->value, 2);
+	case V4L2_CID_SATURATION:
+		return as0260soc_read_reg(sd, UVC_SATURATION_CONTROL, (u16 *)&ctrl->value, 2);
+	case V4L2_CID_HUE:
+		return as0260soc_read_reg(sd, UVC_HUE_CONTROL, (u16 *)&ctrl->value, 2);
+	case V4L2_CID_GAMMA:
+		return as0260soc_read_reg(sd, UVC_GAMMA_CONTROL, (u16 *)&ctrl->value, 2);
+	case V4L2_CID_COLORFX:
+		return as0260soc_read_reg(sd, CAM_SFX_CONTROL, (u16 *)&ctrl->value, 1);
+	}
+	return -EINVAL;
+}
+
+static int as0260soc_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	int ret = 0;
+
+	if (debug)
+		printk(KERN_INFO "as0260soc: s_ctrl function.\n");
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		return as0260soc_write_reg(sd, UVC_BRIGHTNESS_CONTROL, (u16)ctrl->value, 2);
+	case V4L2_CID_CONTRAST:
+		return as0260soc_write_reg(sd, UVC_CONTRAST_CONTROL, (u16)ctrl->value, 2);
+	case V4L2_CID_SATURATION:
+		return as0260soc_write_reg(sd, UVC_SATURATION_CONTROL, (u16)ctrl->value, 2);
+	case V4L2_CID_HUE:
+		return as0260soc_write_reg(sd, UVC_HUE_CONTROL, (s16)ctrl->value, 2);
+	case V4L2_CID_GAMMA:
+		return as0260soc_write_reg(sd, UVC_GAMMA_CONTROL, (u16)ctrl->value, 2);
+	case V4L2_CID_COLORFX:
+		if (ctrl->value < 0 || ctrl->value > 5)
+			return -EINVAL;
+		ret += as0260soc_write_reg(sd, CAM_SFX_CONTROL, (u16)ctrl->value, 1);
+		ret += as0260soc_change_config(sd);
+		return ret;
+	}
+	return -EINVAL;
+}
+
+static int as0260soc_reset(struct v4l2_subdev *sd, u32 val)
+{
+	int ret = 0;
+
+	if (debug)
+		printk(KERN_ERR "as0260soc driver: reset function.\n");
+
+	ret += as0260soc_write_reg(sd, RESET_AND_MISC_CONTROL, 0x0001, 2);
+	ret += as0260soc_write_reg(sd, RESET_AND_MISC_CONTROL, 0x0002, 2);
+
+	return ret;
+}
+
+static int as0260soc_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
+{
+	struct as0260soc_decoder *decoder =  to_decoder(sd);
+	int ret = 0;
+
+	if (debug) {
+		printk(KERN_INFO "as0260soc driver: s_mbus_fmt function.\n");
+		printk(KERN_INFO "setting format: %dx%d, code=0x%04X, field=%d, colorspace=%d\n",
+			fmt->width, fmt->height, fmt->code, fmt->field, fmt->colorspace);
+	}
+
+	if (*decoder->port == 1 || *decoder->port == 2) { /* using 2-lane mipi-csi */
+		if (fmt->width == 640)
+			ret += as0260soc_write_reg_array(sd, as0260soc_preset_vga_30, ARRAY_SIZE(as0260soc_preset_vga_30));
+		if (fmt->width == 1920)
+			ret += as0260soc_write_reg_array(sd, as0260soc_preset_1080p_30, ARRAY_SIZE(as0260soc_preset_1080p_30));
+	}
+	else { /* using VIP port */
+		if (fmt->width == 640)
+			ret += as0260soc_write_reg_array(sd, as0260soc_preset_vga_30, ARRAY_SIZE(as0260soc_preset_vga_30));
+		if (fmt->width == 1920)
+			ret += as0260soc_write_reg_array(sd, as0260soc_preset_1080p_18, ARRAY_SIZE(as0260soc_preset_1080p_18));
+	}
+
+	/* YUV 4:2:2 */
+	ret += as0260soc_write_reg(sd, CAM_OUTPUT_FORMAT, 0x00, 2);
+
+	ret += as0260soc_write_reg(sd, UVC_AE_MODE_CONTROL, (1 << 1), 1);
+	/* Disable AWB */
+	/* ret += as0260soc_write_reg(sd, UVC_AE_PRIORITY_CONTROL, 1, 1); */
+	/* Fixed framerate and disable flicker avoidance */
+	/* ret += as0260soc_write_reg(sd, UVC_MANUAL_EXPOSURE_CONFIGURATION, 0, 1); */
+
+	ret += as0260soc_change_config(sd);
+
+	return ret;
+}
+
+static int as0260soc_try_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
+{
+	struct as0260soc_decoder *decoder = to_decoder(sd);
+	int index = 0;
+
+	if (debug)
+		printk(KERN_INFO "as0260soc driver: try_mbus_fmt function: %dx%d, pixcode=0x%04X, field=%d, colorspace=%d ??\n",
+			fmt->width, fmt->height, fmt->code, fmt->field, fmt->colorspace);
+
+	if (fmt->width <= 640)
+		index = 1;
+	else
+		index = 0;
+
+	fmt->width = as0260soc_formats[index].width;
+	fmt->height = as0260soc_formats[index].height;
+	fmt->code = as0260soc_formats[index].mbus_code;
+	fmt->field = as0260soc_formats[index].field;
+	fmt->colorspace = as0260soc_formats[index].colorspace;
+
+	/* Store the current format */
+	decoder->fmt = &as0260soc_formats[index];
+
+	return 0;
+}
+
+static int as0260soc_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned int index, enum v4l2_mbus_pixelcode *code)
+{
+	if (debug)
+		printk(KERN_INFO "as0260soc driver: enum_mbus_fmt function: index = %d\n", index);
+
+	if (index >= ARRAY_SIZE(as0260soc_formats)) {
+		printk(KERN_ERR "as0260soc driver: enum_mbus_fmt function, index error.\n");
+		return -EINVAL;
+	}
+
+	*code = as0260soc_formats[index].mbus_code;
+
+	if (debug)
+		printk(KERN_INFO "as0260soc driver: enum_mbus_fmt function exit.\n");
+
+	return 0;
+}
+
+static struct soc_camera_ops as0260soc_camera_ops = {
+	.set_bus_param		= as0260soc_set_bus_param,
+	.query_bus_param	= as0260soc_query_bus_param,
+	.controls			= as0260soc_controls,
+	.num_controls		= ARRAY_SIZE(as0260soc_controls),
+};
+
+static const struct v4l2_subdev_core_ops as0260soc_core_ops = {
+	.g_ctrl = as0260soc_g_ctrl,
+	.s_ctrl = as0260soc_s_ctrl,
+	.reset = as0260soc_reset,
+};
+
+static const struct v4l2_subdev_video_ops as0260soc_video_ops = {
+	.s_mbus_fmt 	= as0260soc_s_mbus_fmt,
+	.try_mbus_fmt 	= as0260soc_try_mbus_fmt,
+	.enum_mbus_fmt 	= as0260soc_enum_mbus_fmt,
+};
+
+static const struct v4l2_subdev_ops as0260soc_ops = {
+	.core = &as0260soc_core_ops,
+	.video = &as0260soc_video_ops,
+};
+
+
+static int as0260soc_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct soc_camera_device *icd = client->dev.platform_data;
+	struct soc_camera_link *icl;
+	struct tegra_camera_platform_data *as0260soc_platform_data;
+	struct as0260soc_decoder *decoder;
+	struct v4l2_subdev *sd;
+	int ret;
+
+	if (debug)
+		printk(KERN_ERR "as0260soc driver: probe function.\n");
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
+		printk(KERN_ERR "as0260soc driver: probe function, i2c_check_functionality returns error.\n");
+		return -EIO;
+	}
+
+	icl = to_soc_camera_link(icd);
+	if (!icl) {
+		printk(KERN_ERR "as0260soc driver: probe function, to_soc_camera_link returns data.\n");
+		dev_err(&client->dev, "No platform data!!\n");
+		return -ENODEV;
+	}
+
+	decoder = kzalloc(sizeof(struct as0260soc_decoder), GFP_KERNEL);
+	if (!decoder) {
+		printk(KERN_ERR "as0260soc driver: probe function, canot allocate memory for decoder struct.\n");
+		dev_err(&client->dev, "Failed to allocate memory for private data!\n");
+		return -ENOMEM;
+	}
+
+	sd = &decoder->sd;
+	as0260soc_platform_data = icl->priv;
+	decoder->port = (int *)&as0260soc_platform_data->port;
+
+	/* Register with V4L2 layer as slave device */
+	v4l2_i2c_subdev_init(sd, client, &as0260soc_ops);
+
+	ret = as0260soc_read_reg(sd, CHIP_VERSION_REG, &decoder->chip_ver, 2);
+	if (ret)
+		return -1;
+	printk(KERN_INFO "detected chip 0x%04X\n", decoder->chip_ver);
+
+	ret = as0260soc_init(sd);
+	if (ret) {
+		dev_err(&client->dev, "Failed to init camera\n");
+		return -1;
+	}
+
+	icd->ops = &as0260soc_camera_ops;
+
+	if (debug)
+		printk(KERN_INFO "as0260soc driver: probe function exit.\n");
+
+	return 0;
+}
+
+static int as0260soc_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct as0260soc_decoder *decoder = to_decoder(sd);
+
+	if (debug)
+		printk(KERN_INFO "as0260soc driver: remove function.\n");
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(decoder);
+	return 0;
+}
+
+static const struct i2c_device_id as0260soc_id[] = {
+	{ MODULE_NAME, 0 },
+	{ }
+};
+
+static struct i2c_driver as0260soc_driver = {
+	.driver 	= {
+		.name	= MODULE_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= as0260soc_probe,
+	.remove		= as0260soc_remove,
+	.id_table   = as0260soc_id
+};
+
+static int __init init_as0260soc(void)
+{
+	return i2c_add_driver(&as0260soc_driver);
+}
+
+static void __exit exit_as0260soc(void)
+{
+	i2c_del_driver(&as0260soc_driver);
+}
+
+module_init(init_as0260soc);
+module_exit(exit_as0260soc);
+
+MODULE_DESCRIPTION("Aptina AS0260 SoC Camera driver");
+MODULE_AUTHOR("Wojciech Bieganski <wbieganski@antmicro.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/video/cx23885/cx23885-dvb.c b/drivers/media/video/cx23885/cx23885-dvb.c
index aa83f07b1b0f..bcb45be44bb2 100644
--- a/drivers/media/video/cx23885/cx23885-dvb.c
+++ b/drivers/media/video/cx23885/cx23885-dvb.c
@@ -844,7 +844,7 @@ static int dvb_register(struct cx23885_tsport *port)
 			static struct xc2028_ctrl ctl = {
 				.fname   = XC3028L_DEFAULT_FIRMWARE,
 				.max_len = 64,
-				.demod   = 5000,
+				.demod   = XC3028_FE_DIBCOM52,
 				/* This is true for all demods with
 					v36 firmware? */
 				.type    = XC2028_D2633,
diff --git a/drivers/media/video/max9526.c b/drivers/media/video/max9526.c
new file mode 100644
index 000000000000..1f13dcb29c5a
--- /dev/null
+++ b/drivers/media/video/max9526.c
@@ -0,0 +1,1102 @@
+/*
+ * drivers/media/video/max9526.c
+ *
+ * MAXIM MAX9526 decoder driver
+ *
+ * Copyright (c) 2011 Ming-Yao Chen <mychen0518@gmail.com>
+ * (based on tvp514x.c)
+ *
+ * Copyright (c) 2013 Ant Micro <www.antmicro.com>
+ *
+ * This package is free software; you can redistribute it and/or modify
+ * it under the terms 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+
+#include <media/soc_camera.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-ioctl.h>
+
+/*MODULE NAME*/
+#define MAX9526_MODULE_NAME "max9526"
+
+/*Private macros for MAX9526*/
+
+#define LOCK_RETRY_DELAY	(200)
+#define LOCK_RETRY_COUNT	(5)
+#define I2C_RETRY_COUNT		(5)
+
+
+ /* registers */
+#define REG_STATUS_0                            0x00
+#define REG_STATUS_1                            0x01
+#define REG_IRQMASK_0                           0x02
+#define REG_IRQMASK_1                           0x03
+#define REG_STANDARD_SELECT_SHUTDOWN_CONTROL    0x04
+#define REG_CONTRAST                            0x05
+#define REG_BRIGHTNESS                          0x06
+#define REG_HUE                                 0x07
+#define REG_SATURATION                          0x08
+#define REG_VIDEO_INPUT_SELECT_AND_CLAMP        0x09
+#define REG_GAIN_CONTROL                        0x0A
+#define REG_COLOR_KILL                          0x0B
+#define REG_OUTPUT_TEST_SIGNAL                  0x0C
+#define REG_CLOCK_AND_OUTPUT                    0x0D
+#define REG_PLL_CONTROL                         0x0E
+#define REG_MISCELLANEOUS                       0x0F
+
+#define PAL_NUM_ACTIVE_PIXELS          (720)
+#define PAL_NUM_ACTIVE_LINES           (576)
+
+#define NTSC_NUM_ACTIVE_PIXELS         (720)
+#define NTSC_NUM_ACTIVE_LINES          (480)
+
+#define REG_VIDEO_INPUT_SELECT_IN1	0x00
+#define REG_VIDEO_INPUT_SELECT_IN2	0x40
+#define REG_VIDEO_INPUT_SELECT_AUTO	0x80
+
+
+struct max9526_reg {
+        u8 token;
+        u8 reg;
+        u32 val;
+};
+
+enum max9526_tokens {
+        TOK_TERM,
+        TOK_SKIP,
+        TOK_DELAY,
+        TOK_WRITE,
+};
+
+enum {
+        VIDEO_STDSEL_NTSC_M_BIT,
+        VIDEO_STDSEL_PAL_BGHID_BIT,
+};
+
+MODULE_AUTHOR("Texas Instruments");
+MODULE_DESCRIPTION("MAX9526 linux decoder driver");
+MODULE_LICENSE("GPL");
+
+
+/* enum max9526_std - enum for supported standards*/
+
+enum max9526_std {
+	STD_NTSC_MJ = 0,
+	STD_PAL_BDGHIN,
+	STD_INVALID
+};
+
+
+/**
+ * struct max9526_std_info - Structure to store standard informations
+ * @width: Line width in pixels
+ * @height:Number of active lines
+ * @video_std: Value to write in REG_VIDEO_STD register
+ * @standard: v4l2 standard structure information
+ */
+
+
+struct max9526_std_info {
+	unsigned long width;
+	unsigned long height;
+	u8 video_std;
+	struct v4l2_standard standard;
+};
+
+// TODO: redo this
+static const struct max9526_reg max9526_reg_list_default[] = {
+	{TOK_SKIP , REG_STATUS_0, 0x84},
+	{TOK_SKIP , REG_STATUS_1, 0x40},
+	{TOK_SKIP , REG_IRQMASK_0, 0x00},
+	{TOK_SKIP , REG_IRQMASK_1, 0x00},
+	/*Standard Select, Shutdown, and Control Register*/
+	{TOK_WRITE, REG_STANDARD_SELECT_SHUTDOWN_CONTROL, 0x10},   // was 0x10 (autodetect), 0x0=PAL, 0x40=NTSC
+	{TOK_SKIP, REG_CONTRAST, 0x80},
+	{TOK_SKIP, REG_BRIGHTNESS, 0x00},
+	{TOK_SKIP, REG_HUE, 0x80},
+	{TOK_SKIP, REG_SATURATION, 0x88},
+	{TOK_WRITE, REG_VIDEO_INPUT_SELECT_AND_CLAMP, 0x80}, // auto-select
+							// between input 1 and 2
+	{TOK_SKIP, REG_GAIN_CONTROL, 0x00},
+	{TOK_SKIP, REG_COLOR_KILL, 0x23},
+	{TOK_WRITE, REG_OUTPUT_TEST_SIGNAL, 0x03}, // select 100% color bars
+	{TOK_WRITE, REG_CLOCK_AND_OUTPUT, 0x04}, // select HSVS
+	{TOK_SKIP, REG_PLL_CONTROL, 0x03},
+	{TOK_SKIP, REG_MISCELLANEOUS, 0x18},
+	{TOK_TERM, 0, 0},
+};
+
+
+//static struct max9526_reg max9526_reg_list_default[0x11];
+
+/*MAX9526 default register values*/
+
+static int max9526_s_stream(struct v4l2_subdev *sd, int enable);
+
+/**
+ * struct max9526_decoder - MAX9526 decoder object
+ * @sd: Subdevice Slave handle
+ * @max9526_regs: copy of hw's regs with preset values.
+ * @pdata: Board specific
+ * @ver: Chip version
+ * @streaming: MAX9526 decoder streaming - enabled or disabled.
+ * @pix: Current pixel format
+ * @num_fmts: Number of formats
+ * @fmt_list: Format list
+ * @current_std: Current standard
+ * @num_stds: Number of standards
+ * @std_list: Standards list
+ * @input: Input routing at chip level
+ * @output: Output routing at chip level
+ */
+struct max9526_decoder {
+	struct v4l2_subdev sd;
+	struct max9526_reg max9526_regs[ARRAY_SIZE(max9526_reg_list_default)];
+	const struct max9526_platform_data *pdata;
+
+	int ver;
+	int streaming;
+
+	struct v4l2_pix_format pix;
+	int num_fmts;
+	const struct v4l2_fmtdesc *fmt_list;
+
+	enum max9526_std current_std;
+	int num_stds;
+	struct max9526_std_info *std_list;
+	/* Input and Output Routing parameters */
+	u32 input;
+	u32 output;
+
+	int active_input;
+};
+
+/**
+ * List of image formats supported by max9526 decoder
+ * Currently we are using 8 bit mode only, but can be
+ * extended to 10/20 bit mode.
+ */
+static const struct v4l2_fmtdesc max9526_fmt_list[] = {
+	{
+	 .index = 0,
+	 .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+	 .flags = 0,
+	 .description = "8-bit YUYV 4:2:2 Format",
+	 .pixelformat = V4L2_PIX_FMT_YUYV,
+	},
+        {
+         .index = 1,
+         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+         .flags = 0,
+         .description = "8-bit UYVY 4:2:2 Format",
+         .pixelformat = V4L2_PIX_FMT_UYVY,
+        },
+};
+
+/**
+ * Supported standards -
+ *
+ * Currently supports two standards only, need to add support for rest of the
+ * modes, like SECAM, etc...
+ */
+static struct max9526_std_info max9526_std_list[] = {
+	/* Standard: STD_NTSC_MJ */
+	[STD_NTSC_MJ] = {
+	 .width = NTSC_NUM_ACTIVE_PIXELS,
+	 .height = NTSC_NUM_ACTIVE_LINES,
+	 .video_std = VIDEO_STDSEL_NTSC_M_BIT,
+	 .standard = {
+		      .index = 0,
+		      .id = V4L2_STD_NTSC,
+		      .name = "NTSC",
+		      .frameperiod = {1001, 30000},
+		      .framelines = 525
+		     },
+	/* Standard: STD_PAL_BDGHIN */
+	},
+	[STD_PAL_BDGHIN] = {
+	 .width = PAL_NUM_ACTIVE_PIXELS,
+	 .height = PAL_NUM_ACTIVE_LINES,
+	 .video_std = VIDEO_STDSEL_PAL_BGHID_BIT,
+	 .standard = {
+		      .index = 1,
+		      .id = V4L2_STD_PAL,
+		      .name = "PAL",
+		      .frameperiod = {1, 25},
+		      .framelines = 625
+		     },
+	},
+	/* Standard: need to add for additional standard */
+};
+
+static inline struct max9526_decoder *to_decoder(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct max9526_decoder, sd);
+}
+
+/**
+ * max9526_read_reg() - Read a value from a register in an MAX9526.
+ * @sd: ptr to v4l2_subdev struct
+ * @reg: max9526 register address
+ *
+ * Returns value read if successful, or non-zero (-1) otherwise.
+ */
+static int max9526_read_reg(struct v4l2_subdev *sd, u8 reg)
+{
+	int err, retry = 0;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+read_again:
+
+	err = i2c_smbus_read_byte_data(client, reg);
+	if (err < 0) {
+		if (retry <= I2C_RETRY_COUNT) {
+			v4l2_warn(sd, "Read: retry ... %d\n", retry);
+			retry++;
+			msleep_interruptible(10);
+			goto read_again;
+		}
+	}
+
+	return err;
+}
+
+#if 0
+/**
+ * dump_reg() - dump the register content of MAX9526.
+ * @sd: ptr to v4l2_subdev struct
+ * @reg: MAX9526 register address
+ */
+static void dump_reg(struct v4l2_subdev *sd, u8 reg)
+{
+	u32 val;
+
+	val = max9526_read_reg(sd, reg);
+	v4l2_info(sd, "Reg(0x%.2X): 0x%.2X\n", reg, val);
+}
+#endif
+
+/**
+ * max9526_write_reg() - Write a value to a register in MAX9526
+ * @sd: ptr to v4l2_subdev struct
+ * @reg: MAX9526 register address
+ * @val: value to be written to the register
+ *
+ * Write a value to a register in an MAX9526 decoder device.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int max9526_write_reg(struct v4l2_subdev *sd, u8 reg, u8 val)
+{
+	int err, retry = 0;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+write_again:
+
+	err = i2c_smbus_write_byte_data(client, reg, val);
+	if (err) {
+		if (retry <= I2C_RETRY_COUNT) {
+			v4l2_warn(sd, "Write: retry ... %d\n", retry);
+			retry++;
+			msleep_interruptible(10);
+			goto write_again;
+		}
+	}
+//	if (!err) {
+//		v4l2_warn(sd, "max9526: wrote %X to register %X.\n", val, reg);
+//	}
+	return err;
+}
+
+
+/**
+ * max9526_write_regs() : Initializes a list of MAX9526 registers
+ * @sd: ptr to v4l2_subdev struct
+ * @reglist: list of MAX9526 registers and values
+ *
+ * Initializes a list of MAX9526 registers: token is state flag
+ *		if token is TOK_TERM, then entire write operation terminates
+ *		if token is TOK_DELAY, then a delay of 'val' msec is introduced
+ *		if token is TOK_SKIP, then the register write is skipped
+ *		if token is TOK_WRITE, then the register write is performed
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int max9526_write_regs(struct v4l2_subdev *sd,
+			      const struct max9526_reg reglist[])
+{
+	int err;
+	const struct max9526_reg *next = reglist;
+	for (; next->token != TOK_TERM; next++) {
+		if (next->token == TOK_DELAY) {
+			msleep(next->val);
+			continue;
+		}
+
+		if (next->token == TOK_SKIP)
+			continue;
+
+		err = max9526_write_reg(sd, next->reg, (u8) next->val);
+		if (err) {
+			v4l2_err(sd, "Write failed. Err[%d]\n", err);
+			return err;
+		}
+	}
+	return 0;
+}
+
+
+/**
+ * max9526_get_current_std() : Get the current standard detected by max9526
+ * @sd: ptr to v4l2_subdev struct
+ *
+ * Get current standard detected by MAX9526, STD_INVALID if there is no
+ * standard detected.
+ */
+static enum max9526_std max9526_get_current_std(struct v4l2_subdev *sd)
+{
+	u8 std, std_status;
+
+	std = max9526_read_reg(sd, REG_STANDARD_SELECT_SHUTDOWN_CONTROL);
+
+	std_status = std>>5;
+	switch (std_status) {
+	case VIDEO_STDSEL_NTSC_M_BIT:
+		return STD_NTSC_MJ;
+
+	case VIDEO_STDSEL_PAL_BGHID_BIT:
+		return STD_PAL_BDGHIN;
+
+	default:
+		return STD_INVALID;
+	}
+
+	return STD_INVALID;
+}
+
+
+/**
+ * max9526_configure() - Configure the MAX9526 registers
+ * @sd: ptr to v4l2_subdev struct
+ * @decoder: ptr to max9526_decoder structure
+ *
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int max9526_configure(struct v4l2_subdev *sd, struct max9526_decoder *decoder)
+{
+	int err;
+
+	/* common register initialization */
+	err =
+	    max9526_write_regs(sd, decoder->max9526_regs);
+	if (err)
+		return err;
+
+//	if (debug)
+//		max9526_reg_dump(sd);
+
+	return 0;
+}
+
+
+/**
+ * max9526_querystd() - V4L2 decoder interface handler for querystd
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @std_id: standard V4L2 std_id ioctl enum
+ *
+ * Returns the current standard detected by MAX9526. If no active input is
+ * detected, returns -EINVAL
+ */
+static int max9526_querystd(struct v4l2_subdev *sd, v4l2_std_id *std_id)
+{
+	struct max9526_decoder *decoder = to_decoder(sd);
+	enum max9526_std current_std;
+	//enum max9526_input input_sel;
+	//u8 sync_lock_status, lock_mask;
+	//int err;
+
+	if (std_id == NULL)
+		return -EINVAL;
+
+	msleep(LOCK_RETRY_DELAY);
+
+	/* get the current standard */
+	current_std = max9526_get_current_std(sd);
+	if (current_std == STD_INVALID)
+		return -EINVAL;
+
+	decoder->current_std = current_std;
+	*std_id = decoder->std_list[current_std].standard.id;
+
+//	v4l2_dbg(1, debug, sd, "Current STD: %s",
+//			decoder->std_list[current_std].standard.name);
+	return 0;
+}
+
+/**
+ * max9526_s_std() - V4L2 decoder interface handler for s_std
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @std_id: standard V4L2 v4l2_std_id ioctl enum
+ *
+ * If std_id is supported, sets the requested standard. Otherwise, returns
+ * -EINVAL
+ */
+static int max9526_s_std(struct v4l2_subdev *sd, v4l2_std_id std_id)
+{
+	struct max9526_decoder *decoder = to_decoder(sd);
+	int err, i;
+
+	for (i = 0; i < decoder->num_stds; i++)
+		if (std_id & decoder->std_list[i].standard.id)
+			break;
+
+	if ((i == decoder->num_stds) || (i == STD_INVALID))
+		return -EINVAL;
+
+	err = max9526_write_reg(sd, REG_STANDARD_SELECT_SHUTDOWN_CONTROL,
+				decoder->std_list[i].video_std);
+	if (err)
+		return err;
+
+	decoder->current_std = i;
+	decoder->max9526_regs[REG_STANDARD_SELECT_SHUTDOWN_CONTROL].val =
+		decoder->std_list[i].video_std;
+
+//	v4l2_dbg(1, debug, sd, "Standard set to: %s",
+//			decoder->std_list[i].standard.name);
+	return 0;
+}
+
+/**
+ * max9526_s_routing() - V4L2 decoder interface handler for s_routing
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @input: input selector for routing the signal
+ * @output: output selector for routing the signal
+ * @config: config value. Not used
+ *
+ * If index is valid, selects the requested input. Otherwise, returns -EINVAL if
+ * the input is not supported or there is no active signal present in the
+ * selected input.
+ */
+static int max9526_s_routing(struct v4l2_subdev *sd,
+				u32 input, u32 output, u32 config)
+{
+	//struct max9526_decoder *decoder = to_decoder(sd);
+
+	/*
+	 * For the sequence streamon -> streamoff and again s_input, most of
+	 * the time it fails to lock the signal, since streamoff puts MAX9526
+	 * into power off state which leads to failure in sub-sequent s_input.
+	 */
+	max9526_s_stream(sd, 1);
+	return 0;
+}
+
+
+/**
+ * max9526_queryctrl() - V4L2 decoder interface handler for queryctrl
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @qctrl: standard V4L2 v4l2_queryctrl structure
+ *
+ * If the requested control is supported, returns the control information.
+ * Otherwise, returns -EINVAL if the control is not supported.
+ */
+static int
+max9526_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qctrl)
+{
+	int err = -EINVAL;
+
+	if (qctrl == NULL)
+		return err;
+
+	switch (qctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		/* Brightness supported is (0-255), */
+		err = v4l2_ctrl_query_fill(qctrl, 0, 255, 1, 128);
+		break;
+	case V4L2_CID_CONTRAST:
+	case V4L2_CID_SATURATION:
+		/**
+		 * Saturation and Contrast supported is -
+		 *	Contrast: 0 - 255 (Default - 128)
+		 *	Saturation: 0 - 255 (Default - 128)
+		 */
+		err = v4l2_ctrl_query_fill(qctrl, 0, 255, 1, 128);
+		break;
+	case V4L2_CID_HUE:
+		/* Hue Supported is -
+		 *	Hue - -180 - +180 (Default - 0, Step - +180)
+		 */
+		err = v4l2_ctrl_query_fill(qctrl, -180, 180, 180, 0);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		/**
+		 * Auto Gain supported is -
+		 * 	0 - 1 (Default - 1)
+		 */
+		err = v4l2_ctrl_query_fill(qctrl, 0, 1, 1, 1);
+		break;
+	default:
+		v4l2_err(sd, "invalid control id %d\n", qctrl->id);
+		return err;
+	}
+
+//	v4l2_dbg(1, debug, sd, "Query Control:%s: Min - %d, Max - %d, Def - %d",
+//			qctrl->name, qctrl->minimum, qctrl->maximum,
+//			qctrl->default_value);
+
+	return err;
+}
+
+
+/**
+ * max9526_g_ctrl() - V4L2 decoder interface handler for g_ctrl
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @ctrl: pointer to v4l2_control structure
+ *
+ * If the requested control is supported, returns the control's current
+ * value from the decoder. Otherwise, returns -EINVAL if the control is not
+ * supported.
+ */
+static int
+max9526_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct max9526_decoder *decoder = to_decoder(sd);
+
+	if (ctrl == NULL)
+		return -EINVAL;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		ctrl->value = decoder->max9526_regs[REG_BRIGHTNESS].val;
+		break;
+	case V4L2_CID_CONTRAST:
+		ctrl->value = decoder->max9526_regs[REG_CONTRAST].val;
+		break;
+	case V4L2_CID_SATURATION:
+		ctrl->value = decoder->max9526_regs[REG_SATURATION].val;
+		break;
+	case V4L2_CID_HUE:
+		ctrl->value = decoder->max9526_regs[REG_HUE].val;
+		if (ctrl->value == 0x7F)
+			ctrl->value = 180;
+		else if (ctrl->value == 0x80)
+			ctrl->value = -180;
+		else
+			ctrl->value = 0;
+
+		break;
+	default:
+		v4l2_err(sd, "invalid control id %d\n", ctrl->id);
+		return -EINVAL;
+	}
+
+//	v4l2_dbg(1, debug, sd, "Get Control: ID - %d - %d",
+//			ctrl->id, ctrl->value);
+	return 0;
+}
+
+/**
+ * max9526_s_ctrl() - V4L2 decoder interface handler for s_ctrl
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @ctrl: pointer to v4l2_control structure
+ *
+ * If the requested control is supported, sets the control's current
+ * value in HW. Otherwise, returns -EINVAL if the control is not supported.
+ */
+static int
+max9526_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+	struct max9526_decoder *decoder = to_decoder(sd);
+	int err = -EINVAL, value;
+
+	if (ctrl == NULL)
+		return err;
+
+	value = ctrl->value;
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		if (ctrl->value < 0 || ctrl->value > 255) {
+			v4l2_err(sd, "invalid brightness setting %d\n",
+					ctrl->value);
+			return -ERANGE;
+		}
+		err = max9526_write_reg(sd, REG_BRIGHTNESS,
+				value);
+		if (err)
+			return err;
+
+		decoder->max9526_regs[REG_BRIGHTNESS].val = value;
+		break;
+	case V4L2_CID_CONTRAST:
+		if (ctrl->value < 0 || ctrl->value > 255) {
+			v4l2_err(sd, "invalid contrast setting %d\n",
+					ctrl->value);
+			return -ERANGE;
+		}
+		err = max9526_write_reg(sd, REG_CONTRAST, value);
+		if (err)
+			return err;
+
+		decoder->max9526_regs[REG_CONTRAST].val = value;
+		break;
+	case V4L2_CID_SATURATION:
+		if (ctrl->value < 0 || ctrl->value > 255) {
+			v4l2_err(sd, "invalid saturation setting %d\n",
+					ctrl->value);
+			return -ERANGE;
+		}
+		err = max9526_write_reg(sd, REG_SATURATION, value);
+		if (err)
+			return err;
+
+		decoder->max9526_regs[REG_SATURATION].val = value;
+		break;
+	case V4L2_CID_HUE:
+		if (value == 180)
+			value = 0x7F;
+		else if (value == -180)
+			value = 0x80;
+		else if (value == 0)
+			value = 0;
+		else {
+			v4l2_err(sd, "invalid hue setting %d\n", ctrl->value);
+			return -ERANGE;
+		}
+		err = max9526_write_reg(sd, REG_HUE, value);
+		if (err)
+			return err;
+
+		decoder->max9526_regs[REG_HUE].val = value;
+		break;
+	default:
+		v4l2_err(sd, "invalid control id %d\n", ctrl->id);
+		return err;
+	}
+
+//	v4l2_dbg(1, debug, sd, "Set Control: ID - %d - %d",
+//			ctrl->id, ctrl->value);
+
+	return err;
+}
+
+
+static int max9526_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned int index,
+				 enum v4l2_mbus_pixelcode *code)
+{
+	struct max9526_decoder *decoder = to_decoder(sd);
+
+	if (index < 0 || index >= decoder->num_fmts)
+		return -EINVAL;
+	switch (index) {
+	case 0:
+		*code = V4L2_MBUS_FMT_YUYV8_2X8;
+		break;
+        case 1:
+                *code = V4L2_MBUS_FMT_UYVY8_2X8;
+                break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int max9526_try_mbus_fmt(struct v4l2_subdev *sd,
+				struct v4l2_mbus_framefmt *mf)
+{
+/*	pr_info("%s width:%d\n", __func__, mf->width);
+	pr_info("%s height:%d\n", __func__, mf->height);
+	pr_info("%s field:0x%X (V4L2_FIELD_NONE==0x%X)\n", __func__, mf->field, V4L2_FIELD_NONE);
+	pr_info("%s code:0x%X (V4L2_MBUS_FMT_YUYV8_2X8==0x%X)\n", __func__, mf->code, V4L2_MBUS_FMT_YUYV8_2X8);
+	pr_info("%s colorspace:0x%X (V4L2_COLORSPACE_SRGB==0x%X)\n", __func__, mf->colorspace, V4L2_COLORSPACE_SRGB);*/
+
+	if (mf->code == V4L2_MBUS_FMT_UYVY8_2X8) {
+	        mf->width = PAL_NUM_ACTIVE_PIXELS;
+	        mf->height = PAL_NUM_ACTIVE_LINES;
+	} else if (mf->code == V4L2_MBUS_FMT_YUYV8_2X8) {
+	        mf->width = PAL_NUM_ACTIVE_PIXELS;
+	        mf->height = PAL_NUM_ACTIVE_LINES;
+	}
+	mf->field = V4L2_FIELD_INTERLACED_TB;
+	mf->colorspace =    V4L2_COLORSPACE_SMPTE170M;
+
+	return 0;
+}
+
+
+/**
+ * max9526_g_parm() - V4L2 decoder interface handler for g_parm
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @a: pointer to standard V4L2 VIDIOC_G_PARM ioctl structure
+ *
+ * Returns the decoder's video CAPTURE parameters.
+ */
+static int
+max9526_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *a)
+{
+	struct max9526_decoder *decoder = to_decoder(sd);
+	struct v4l2_captureparm *cparm;
+	enum max9526_std current_std;
+
+	if (a == NULL)
+		return -EINVAL;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		/* only capture is supported */
+		return -EINVAL;
+
+	memset(a, 0, sizeof(*a));
+	a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	/* get the current standard */
+	current_std = max9526_get_current_std(sd);
+	if (current_std == STD_INVALID)
+		return -EINVAL;
+
+	decoder->current_std = current_std;
+
+	cparm = &a->parm.capture;
+	cparm->capability = V4L2_CAP_TIMEPERFRAME;
+	cparm->timeperframe =
+		decoder->std_list[current_std].standard.frameperiod;
+
+	return 0;
+}
+
+
+/**
+ * max9526_s_parm() - V4L2 decoder interface handler for s_parm
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @a: pointer to standard V4L2 VIDIOC_S_PARM ioctl structure
+ *
+ * Configures the decoder to use the input parameters, if possible. If
+ * not possible, returns the appropriate error code.
+ */
+static int
+max9526_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *a)
+{
+	struct max9526_decoder *decoder = to_decoder(sd);
+	struct v4l2_fract *timeperframe;
+	enum max9526_std current_std;
+
+	if (a == NULL)
+		return -EINVAL;
+
+	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		/* only capture is supported */
+		return -EINVAL;
+
+	timeperframe = &a->parm.capture.timeperframe;
+
+	/* get the current standard */
+	current_std = max9526_get_current_std(sd);
+	if (current_std == STD_INVALID)
+		return -EINVAL;
+
+	decoder->current_std = current_std;
+
+	*timeperframe =
+	    decoder->std_list[current_std].standard.frameperiod;
+
+	return 0;
+}
+
+/**
+ * max9526_s_stream() - V4L2 decoder i/f handler for s_stream
+ * @sd: pointer to standard V4L2 sub-device structure
+ * @enable: streaming enable or disable
+ *
+ * Sets streaming to enable or disable, if possible.
+ */
+static int max9526_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	int err = 0;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct max9526_decoder *decoder = to_decoder(sd);
+
+	if (decoder->streaming == enable)
+		return 0;
+
+	switch (enable) {
+	case 0:
+	{
+		decoder->streaming = enable;
+		break;
+	}
+	case 1:
+	{
+		struct max9526_reg *int_seq = (struct max9526_reg *)client->driver->id_table->driver_data;
+
+		err = max9526_write_regs(sd, int_seq);
+		if (err) {
+			v4l2_err(sd, "Unable to turn on decoder\n");
+			return err;
+		}
+		err = max9526_configure(sd, decoder);
+		if (err) {
+			v4l2_err(sd, "Unable to configure decoder\n");
+			return err;
+		}
+		decoder->streaming = enable;
+		break;
+	}
+	default:
+		err = -ENODEV;
+		break;
+	}
+
+	return err;
+}
+
+static int vidioc_s_input(struct file *file, void *priv, unsigned int i) {
+	struct soc_camera_device *icd = file->private_data;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct max9526_decoder *decoder = to_decoder(sd);
+	u8 val;
+
+	if (i < 3) {
+		decoder->active_input = i;
+		switch (decoder->active_input) {
+		case 0:
+			val = REG_VIDEO_INPUT_SELECT_IN1;
+			break;
+		case 1:
+			val = REG_VIDEO_INPUT_SELECT_IN2;
+			break;
+		default:
+			val = REG_VIDEO_INPUT_SELECT_AUTO;
+		}
+		return max9526_write_reg(sd, REG_VIDEO_INPUT_SELECT_AND_CLAMP,
+					 val);
+	}
+
+	return -EINVAL;
+}
+
+static int vidioc_g_input(struct file *file, void *priv, unsigned int *i) {
+	struct soc_camera_device *icd = file->private_data;
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct max9526_decoder *decoder = to_decoder(sd);
+
+	*i = decoder->active_input;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops max9526_core_ops = {
+	.queryctrl = max9526_queryctrl,
+	.g_ctrl = max9526_g_ctrl,
+	.s_ctrl = max9526_s_ctrl,
+	.s_std = max9526_s_std,
+};
+
+
+static int max9526_s_mbus_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+#if 0
+	pr_info("%s width:%d\n", __func__, mf->width);
+	pr_info("%s height:%d\n", __func__, mf->height);
+	pr_info("%s field:0x%X (V4L2_FIELD_NONE==0x%X)\n", __func__, mf->field, V4L2_FIELD_NONE);
+	pr_info("%s code:0x%X (V4L2_MBUS_FMT_YUYV8_2X8==0x%X)\n", __func__, mf->code, V4L2_MBUS_FMT_YUYV8_2X8);
+	pr_info("%s colorspace:0x%X (V4L2_COLORSPACE_SRGB==0x%X)\n", __func__, mf->colorspace, V4L2_COLORSPACE_SRGB);
+
+	mf->width = PAL_NUM_ACTIVE_PIXELS;
+	mf->height = PAL_NUM_ACTIVE_LINES;
+	mf->colorspace = V4L2_COLORSPACE_SMPTE170M;
+#endif
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops max9526_video_ops = {
+	.s_routing = max9526_s_routing,
+	.querystd = max9526_querystd,
+	.enum_mbus_fmt = max9526_enum_mbus_fmt,
+	.try_mbus_fmt = max9526_try_mbus_fmt,
+	.s_mbus_fmt = max9526_s_mbus_fmt,
+	.g_parm = max9526_g_parm,
+	.s_parm = max9526_s_parm,
+	.s_stream = max9526_s_stream,
+};
+
+/* Alter bus settings on camera side */
+static int max9526_set_bus_param(struct soc_camera_device *icd,
+				unsigned long flags)
+{
+	return 0;
+}
+
+static unsigned long max9526_query_bus_param(struct soc_camera_device *icd)
+{
+	struct soc_camera_link *icl = to_soc_camera_link(icd);
+
+	unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
+		SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
+		SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8;
+
+	return soc_camera_apply_sensor_flags(icl, flags);
+}
+
+static struct soc_camera_ops max9526_soc_camera_ops = {
+	.set_bus_param		= max9526_set_bus_param,
+	.query_bus_param	= max9526_query_bus_param,
+	.num_controls		= 0,
+};
+
+static const struct v4l2_subdev_ops max9526_ops = {
+	.core = &max9526_core_ops,
+	.video = &max9526_video_ops,
+};
+
+static struct max9526_decoder max9526_dev = {
+	.streaming = 0,
+
+	.fmt_list = max9526_fmt_list,
+	.num_fmts = ARRAY_SIZE(max9526_fmt_list),
+
+	.pix = {
+		/* Default to PAL 8-bit YUV 422 */
+		.width = PAL_NUM_ACTIVE_PIXELS,
+		.height = PAL_NUM_ACTIVE_LINES,
+		.pixelformat = V4L2_PIX_FMT_UYVY,
+		.field = V4L2_FIELD_INTERLACED_TB,
+		.bytesperline = PAL_NUM_ACTIVE_PIXELS * 2,
+		.sizeimage =
+		PAL_NUM_ACTIVE_PIXELS * 2 * PAL_NUM_ACTIVE_LINES,
+		.colorspace = V4L2_COLORSPACE_SMPTE170M,
+		},
+
+	.current_std = STD_PAL_BDGHIN,
+	.std_list = max9526_std_list,
+	.num_stds = ARRAY_SIZE(max9526_std_list),
+
+        .active_input = 2, // auto-select between input 1 and 2
+};
+
+/**
+ * max9526_probe() - decoder driver i2c probe handler
+ * @client: i2c driver client device structure
+ * @id: i2c driver id table
+ *
+ * Register decoder as an i2c client device and V4L2
+ * device.
+ */
+
+static int max9526_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct max9526_decoder *decoder;
+	struct v4l2_subdev *sd;
+        struct soc_camera_device *icd = client->dev.platform_data;
+        struct v4l2_ioctl_ops *ops;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	if (!client->dev.platform_data) {
+		v4l2_err(client, "No platform data!!\n");
+		return -ENODEV;
+	}
+
+	decoder = kzalloc(sizeof(*decoder), GFP_KERNEL);
+	if (!decoder)
+		return -ENOMEM;
+
+	/* Initialize the max9526_decoder with default configuration */
+	*decoder = max9526_dev;
+	/* Copy default register configuration */
+	memcpy(decoder->max9526_regs, max9526_reg_list_default,
+			sizeof(max9526_reg_list_default));
+
+	/* Copy board specific information here */
+	decoder->pdata = client->dev.platform_data;
+
+
+	/* Register with V4L2 layer as slave device */
+	sd = &decoder->sd;
+	v4l2_i2c_subdev_init(sd, client, &max9526_ops);
+
+	icd->ops = &max9526_soc_camera_ops;
+
+	/*
+	 * This is the only way to support more than one input as soc_camera
+	 * assumes in its own vidioc_s(g)_input implementation that only one
+	 * input is present we have to override that with our own handlers.
+	 */
+	ops = (struct v4l2_ioctl_ops*)icd->vdev->ioctl_ops;
+	ops->vidioc_s_input = &vidioc_s_input;
+	ops->vidioc_g_input = &vidioc_g_input;
+
+	v4l2_info(sd, "%s decoder driver registered !!\n", sd->name);
+
+	return 0;
+
+}
+
+
+/**
+ * max9526_remove() - decoder driver i2c remove handler
+ * @client: i2c driver client device structure
+ *
+ * Unregister decoder as an i2c client device and V4L2
+ * device. Complement of max9526_probe().
+ */
+static int max9526_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct max9526_decoder *decoder = to_decoder(sd);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(decoder);
+	return 0;
+}
+
+/**
+ * I2C Device Table -
+ *
+ * name - Name of the actual device/chip.
+ * driver_data - Driver data
+ */
+static const struct i2c_device_id max9526_id[] = {
+	{"max9526", (unsigned long)max9526_reg_list_default},
+};
+
+
+static struct i2c_driver max9526_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = MAX9526_MODULE_NAME,
+	},
+	.probe = max9526_probe,
+	.remove = max9526_remove,
+	.id_table = max9526_id,
+};
+
+static int __init max9526_init(void)
+{
+	return i2c_add_driver(&max9526_driver);
+}
+
+static void __exit max9526_exit(void)
+{
+	i2c_del_driver(&max9526_driver);
+}
+
+module_init(max9526_init);
+module_exit(max9526_exit);
diff --git a/drivers/media/video/mx3_camera.c b/drivers/media/video/mx3_camera.c
index c045b47803ad..8fd828129350 100644
--- a/drivers/media/video/mx3_camera.c
+++ b/drivers/media/video/mx3_camera.c
@@ -292,6 +292,38 @@ static void mx3_videobuf_queue(struct vb2_buffer *vb)
 	dma_cookie_t cookie;
 	u32 fourcc = icd->current_fmt->host_fmt->fourcc;
 	unsigned long flags;
+	size_t new_size;
+
+	BUG_ON(bytes_per_line <= 0);
+
+	new_size = bytes_per_line * icd->user_height;
+
+	if (vb2_plane_size(vb, 0) < new_size) {
+		dev_err(icd->parent, "Buffer #%d too small (%lu < %zu)\n",
+			vb->v4l2_buf.index, vb2_plane_size(vb, 0), new_size);
+		goto error;
+	}
+
+	if (buf->state == CSI_BUF_NEEDS_INIT) {
+		sg_dma_address(sg)	= vb2_dma_contig_plane_dma_addr(vb, 0);
+		sg_dma_len(sg)		= new_size;
+
+		txd = dmaengine_prep_slave_sg(
+			&ichan->dma_chan, sg, 1, DMA_DEV_TO_MEM,
+			DMA_PREP_INTERRUPT);
+		if (!txd)
+			goto error;
+
+		txd->callback_param	= txd;
+		txd->callback		= mx3_cam_dma_done;
+
+		buf->state		= CSI_BUF_PREPARED;
+		buf->txd		= txd;
+	} else {
+		txd = buf->txd;
+	}
+
+	vb2_set_plane_payload(vb, 0, new_size);
 
 	/* This is the configuration of one sg-element */
 	video->out_pixel_fmt	= fourcc_to_ipu_pix(fourcc);
diff --git a/drivers/media/video/omap/omap_vout.c b/drivers/media/video/omap/omap_vout.c
index b3a5ecdb33ac..3422da0034f4 100644
--- a/drivers/media/video/omap/omap_vout.c
+++ b/drivers/media/video/omap/omap_vout.c
@@ -38,6 +38,7 @@
 #include <linux/irq.h>
 #include <linux/videodev2.h>
 #include <linux/dma-mapping.h>
+#include <linux/slab.h>
 
 #include <media/videobuf-dma-contig.h>
 #include <media/v4l2-device.h>
diff --git a/drivers/media/video/omap3isp/isp.c b/drivers/media/video/omap3isp/isp.c
index 5cea2bbd7014..abc26854e29c 100644
--- a/drivers/media/video/omap3isp/isp.c
+++ b/drivers/media/video/omap3isp/isp.c
@@ -1107,8 +1107,7 @@ isp_restore_context(struct isp_device *isp, struct isp_reg *reg_list)
 static void isp_save_ctx(struct isp_device *isp)
 {
 	isp_save_context(isp, isp_reg_list);
-	if (isp->iommu)
-		iommu_save_ctx(isp->iommu);
+	omap_iommu_save_ctx(isp->dev);
 }
 
 /*
@@ -1121,8 +1120,7 @@ static void isp_save_ctx(struct isp_device *isp)
 static void isp_restore_ctx(struct isp_device *isp)
 {
 	isp_restore_context(isp, isp_reg_list);
-	if (isp->iommu)
-		iommu_restore_ctx(isp->iommu);
+	omap_iommu_restore_ctx(isp->dev);
 	omap3isp_ccdc_restore_context(isp);
 	omap3isp_preview_restore_context(isp);
 }
@@ -1975,7 +1973,8 @@ static int isp_remove(struct platform_device *pdev)
 	isp_cleanup_modules(isp);
 
 	omap3isp_get(isp);
-	iommu_put(isp->iommu);
+	iommu_detach_device(isp->domain, &pdev->dev);
+	iommu_domain_free(isp->domain);
 	omap3isp_put(isp);
 
 	free_irq(isp->irq_num, isp);
@@ -2122,26 +2121,31 @@ static int isp_probe(struct platform_device *pdev)
 		}
 	}
 
-	/* IOMMU */
-	isp->iommu = iommu_get("isp");
-	if (IS_ERR_OR_NULL(isp->iommu)) {
-		isp->iommu = NULL;
-		ret = -ENODEV;
+	isp->domain = iommu_domain_alloc(pdev->dev.bus);
+	if (!isp->domain) {
+		dev_err(isp->dev, "can't alloc iommu domain\n");
+		ret = -ENOMEM;
 		goto error_isp;
 	}
 
+	ret = iommu_attach_device(isp->domain, &pdev->dev);
+	if (ret) {
+		dev_err(&pdev->dev, "can't attach iommu device: %d\n", ret);
+		goto free_domain;
+	}
+
 	/* Interrupt */
 	isp->irq_num = platform_get_irq(pdev, 0);
 	if (isp->irq_num <= 0) {
 		dev_err(isp->dev, "No IRQ resource\n");
 		ret = -ENODEV;
-		goto error_isp;
+		goto detach_dev;
 	}
 
 	if (request_irq(isp->irq_num, isp_isr, IRQF_SHARED, "OMAP3 ISP", isp)) {
 		dev_err(isp->dev, "Unable to request IRQ\n");
 		ret = -EINVAL;
-		goto error_isp;
+		goto detach_dev;
 	}
 
 	/* Entities */
@@ -2162,8 +2166,11 @@ error_modules:
 	isp_cleanup_modules(isp);
 error_irq:
 	free_irq(isp->irq_num, isp);
+detach_dev:
+	iommu_detach_device(isp->domain, &pdev->dev);
+free_domain:
+	iommu_domain_free(isp->domain);
 error_isp:
-	iommu_put(isp->iommu);
 	omap3isp_put(isp);
 error:
 	isp_put_clocks(isp);
diff --git a/drivers/media/video/omap3isp/isp.h b/drivers/media/video/omap3isp/isp.h
index 529e582ef948..60e0e29e3edc 100644
--- a/drivers/media/video/omap3isp/isp.h
+++ b/drivers/media/video/omap3isp/isp.h
@@ -32,6 +32,7 @@
 #include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/wait.h>
+#include <linux/iommu.h>
 #include <plat/iommu.h>
 #include <plat/iovmm.h>
 
@@ -294,7 +295,7 @@ struct isp_device {
 	unsigned int sbl_resources;
 	unsigned int subclk_resources;
 
-	struct iommu *iommu;
+	struct iommu_domain *domain;
 
 	struct isp_platform_callback platform_cb;
 };
diff --git a/drivers/media/video/omap3isp/ispccdc.c b/drivers/media/video/omap3isp/ispccdc.c
index 80796eb0c53e..8748e0855c61 100644
--- a/drivers/media/video/omap3isp/ispccdc.c
+++ b/drivers/media/video/omap3isp/ispccdc.c
@@ -366,7 +366,7 @@ static void ccdc_lsc_free_request(struct isp_ccdc_device *ccdc,
 		dma_unmap_sg(isp->dev, req->iovm->sgt->sgl,
 			     req->iovm->sgt->nents, DMA_TO_DEVICE);
 	if (req->table)
-		iommu_vfree(isp->iommu, req->table);
+		omap_iommu_vfree(isp->domain, isp->dev, req->table);
 	kfree(req);
 }
 
@@ -438,15 +438,15 @@ static int ccdc_lsc_config(struct isp_ccdc_device *ccdc,
 
 		req->enable = 1;
 
-		req->table = iommu_vmalloc(isp->iommu, 0, req->config.size,
-					   IOMMU_FLAG);
+		req->table = omap_iommu_vmalloc(isp->domain, isp->dev, 0,
+					req->config.size, IOMMU_FLAG);
 		if (IS_ERR_VALUE(req->table)) {
 			req->table = 0;
 			ret = -ENOMEM;
 			goto done;
 		}
 
-		req->iovm = find_iovm_area(isp->iommu, req->table);
+		req->iovm = omap_find_iovm_area(isp->dev, req->table);
 		if (req->iovm == NULL) {
 			ret = -ENOMEM;
 			goto done;
@@ -462,7 +462,7 @@ static int ccdc_lsc_config(struct isp_ccdc_device *ccdc,
 		dma_sync_sg_for_cpu(isp->dev, req->iovm->sgt->sgl,
 				    req->iovm->sgt->nents, DMA_TO_DEVICE);
 
-		table = da_to_va(isp->iommu, req->table);
+		table = omap_da_to_va(isp->dev, req->table);
 		if (copy_from_user(table, config->lsc, req->config.size)) {
 			ret = -EFAULT;
 			goto done;
@@ -731,18 +731,19 @@ static int ccdc_config(struct isp_ccdc_device *ccdc,
 
 			/*
 			 * table_new must be 64-bytes aligned, but it's
-			 * already done by iommu_vmalloc().
+			 * already done by omap_iommu_vmalloc().
 			 */
 			size = ccdc->fpc.fpnum * 4;
-			table_new = iommu_vmalloc(isp->iommu, 0, size,
-						  IOMMU_FLAG);
+			table_new = omap_iommu_vmalloc(isp->domain, isp->dev,
+							0, size, IOMMU_FLAG);
 			if (IS_ERR_VALUE(table_new))
 				return -ENOMEM;
 
-			if (copy_from_user(da_to_va(isp->iommu, table_new),
+			if (copy_from_user(omap_da_to_va(isp->dev, table_new),
 					   (__force void __user *)
 					   ccdc->fpc.fpcaddr, size)) {
-				iommu_vfree(isp->iommu, table_new);
+				omap_iommu_vfree(isp->domain, isp->dev,
+								table_new);
 				return -EFAULT;
 			}
 
@@ -752,7 +753,7 @@ static int ccdc_config(struct isp_ccdc_device *ccdc,
 
 		ccdc_configure_fpc(ccdc);
 		if (table_old != 0)
-			iommu_vfree(isp->iommu, table_old);
+			omap_iommu_vfree(isp->domain, isp->dev, table_old);
 	}
 
 	return ccdc_lsc_config(ccdc, ccdc_struct);
@@ -1405,11 +1406,14 @@ static int __ccdc_handle_stopping(struct isp_ccdc_device *ccdc, u32 event)
 
 static void ccdc_hs_vs_isr(struct isp_ccdc_device *ccdc)
 {
+	struct isp_pipeline *pipe =
+		to_isp_pipeline(&ccdc->video_out.video.entity);
 	struct video_device *vdev = &ccdc->subdev.devnode;
 	struct v4l2_event event;
 
 	memset(&event, 0, sizeof(event));
-	event.type = V4L2_EVENT_OMAP3ISP_HS_VS;
+	event.type = V4L2_EVENT_FRAME_SYNC;
+	event.u.frame_sync.frame_sequence = atomic_read(&pipe->frame_number);
 
 	v4l2_event_queue(vdev, &event);
 }
@@ -1691,7 +1695,11 @@ static long ccdc_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
 static int ccdc_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
 				struct v4l2_event_subscription *sub)
 {
-	if (sub->type != V4L2_EVENT_OMAP3ISP_HS_VS)
+	if (sub->type != V4L2_EVENT_FRAME_SYNC)
+		return -EINVAL;
+
+	/* line number is zero at frame start */
+	if (sub->id != 0)
 		return -EINVAL;
 
 	return v4l2_event_subscribe(fh, sub, OMAP3ISP_CCDC_NEVENTS);
@@ -1828,7 +1836,7 @@ ccdc_try_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
 		 * callers to request an output size bigger than the input size
 		 * up to the nearest multiple of 16.
 		 */
-		fmt->width = clamp_t(u32, width, 32, (fmt->width + 15) & ~15);
+		fmt->width = clamp_t(u32, width, 32, fmt->width + 15);
 		fmt->width &= ~15;
 		fmt->height = clamp_t(u32, height, 32, fmt->height);
 		break;
@@ -2144,6 +2152,37 @@ static const struct media_entity_operations ccdc_media_ops = {
 	.link_setup = ccdc_link_setup,
 };
 
+void omap3isp_ccdc_unregister_entities(struct isp_ccdc_device *ccdc)
+{
+	v4l2_device_unregister_subdev(&ccdc->subdev);
+	omap3isp_video_unregister(&ccdc->video_out);
+}
+
+int omap3isp_ccdc_register_entities(struct isp_ccdc_device *ccdc,
+	struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video node. */
+	ret = v4l2_device_register_subdev(vdev, &ccdc->subdev);
+	if (ret < 0)
+		goto error;
+
+	ret = omap3isp_video_register(&ccdc->video_out, vdev);
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	omap3isp_ccdc_unregister_entities(ccdc);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * ISP CCDC initialisation and cleanup
+ */
+
 /*
  * ccdc_init_entities - Initialize V4L2 subdev and media entity
  * @ccdc: ISP CCDC module
@@ -2185,50 +2224,23 @@ static int ccdc_init_entities(struct isp_ccdc_device *ccdc)
 
 	ret = omap3isp_video_init(&ccdc->video_out, "CCDC");
 	if (ret < 0)
-		return ret;
+		goto error_video;
 
 	/* Connect the CCDC subdev to the video node. */
 	ret = media_entity_create_link(&ccdc->subdev.entity, CCDC_PAD_SOURCE_OF,
 			&ccdc->video_out.video.entity, 0, 0);
 	if (ret < 0)
-		return ret;
-
-	return 0;
-}
-
-void omap3isp_ccdc_unregister_entities(struct isp_ccdc_device *ccdc)
-{
-	media_entity_cleanup(&ccdc->subdev.entity);
-
-	v4l2_device_unregister_subdev(&ccdc->subdev);
-	omap3isp_video_unregister(&ccdc->video_out);
-}
-
-int omap3isp_ccdc_register_entities(struct isp_ccdc_device *ccdc,
-	struct v4l2_device *vdev)
-{
-	int ret;
-
-	/* Register the subdev and video node. */
-	ret = v4l2_device_register_subdev(vdev, &ccdc->subdev);
-	if (ret < 0)
-		goto error;
-
-	ret = omap3isp_video_register(&ccdc->video_out, vdev);
-	if (ret < 0)
-		goto error;
+		goto error_link;
 
 	return 0;
 
-error:
-	omap3isp_ccdc_unregister_entities(ccdc);
+error_link:
+	omap3isp_video_cleanup(&ccdc->video_out);
+error_video:
+	media_entity_cleanup(me);
 	return ret;
 }
 
-/* -----------------------------------------------------------------------------
- * ISP CCDC initialisation and cleanup
- */
-
 /*
  * omap3isp_ccdc_init - CCDC module initialization.
  * @dev: Device pointer specific to the OMAP3 ISP.
@@ -2240,6 +2252,7 @@ error:
 int omap3isp_ccdc_init(struct isp_device *isp)
 {
 	struct isp_ccdc_device *ccdc = &isp->isp_ccdc;
+	int ret;
 
 	spin_lock_init(&ccdc->lock);
 	init_waitqueue_head(&ccdc->wait);
@@ -2268,7 +2281,13 @@ int omap3isp_ccdc_init(struct isp_device *isp)
 	ccdc->update = OMAP3ISP_CCDC_BLCLAMP;
 	ccdc_apply_controls(ccdc);
 
-	return ccdc_init_entities(ccdc);
+	ret = ccdc_init_entities(ccdc);
+	if (ret < 0) {
+		mutex_destroy(&ccdc->ioctl_lock);
+		return ret;
+	}
+
+	return 0;
 }
 
 /*
@@ -2279,6 +2298,9 @@ void omap3isp_ccdc_cleanup(struct isp_device *isp)
 {
 	struct isp_ccdc_device *ccdc = &isp->isp_ccdc;
 
+	omap3isp_video_cleanup(&ccdc->video_out);
+	media_entity_cleanup(&ccdc->subdev.entity);
+
 	/* Free LSC requests. As the CCDC is stopped there's no active request,
 	 * so only the pending request and the free queue need to be handled.
 	 */
@@ -2287,5 +2309,7 @@ void omap3isp_ccdc_cleanup(struct isp_device *isp)
 	ccdc_lsc_free_queue(ccdc, &ccdc->lsc.free_queue);
 
 	if (ccdc->fpc.fpcaddr != 0)
-		iommu_vfree(isp->iommu, ccdc->fpc.fpcaddr);
+		omap_iommu_vfree(isp->domain, isp->dev, ccdc->fpc.fpcaddr);
+
+	mutex_destroy(&ccdc->ioctl_lock);
 }
diff --git a/drivers/media/video/omap3isp/ispstat.c b/drivers/media/video/omap3isp/ispstat.c
index 808065948ac1..036fc938bbb5 100644
--- a/drivers/media/video/omap3isp/ispstat.c
+++ b/drivers/media/video/omap3isp/ispstat.c
@@ -366,7 +366,8 @@ static void isp_stat_bufs_free(struct ispstat *stat)
 				dma_unmap_sg(isp->dev, buf->iovm->sgt->sgl,
 					     buf->iovm->sgt->nents,
 					     DMA_FROM_DEVICE);
-			iommu_vfree(isp->iommu, buf->iommu_addr);
+			omap_iommu_vfree(isp->domain, isp->dev,
+							buf->iommu_addr);
 		} else {
 			if (!buf->virt_addr)
 				continue;
@@ -399,8 +400,8 @@ static int isp_stat_bufs_alloc_iommu(struct ispstat *stat, unsigned int size)
 		struct iovm_struct *iovm;
 
 		WARN_ON(buf->dma_addr);
-		buf->iommu_addr = iommu_vmalloc(isp->iommu, 0, size,
-						IOMMU_FLAG);
+		buf->iommu_addr = omap_iommu_vmalloc(isp->domain, isp->dev, 0,
+							size, IOMMU_FLAG);
 		if (IS_ERR((void *)buf->iommu_addr)) {
 			dev_err(stat->isp->dev,
 				 "%s: Can't acquire memory for "
@@ -409,7 +410,7 @@ static int isp_stat_bufs_alloc_iommu(struct ispstat *stat, unsigned int size)
 			return -ENOMEM;
 		}
 
-		iovm = find_iovm_area(isp->iommu, buf->iommu_addr);
+		iovm = omap_find_iovm_area(isp->dev, buf->iommu_addr);
 		if (!iovm ||
 		    !dma_map_sg(isp->dev, iovm->sgt->sgl, iovm->sgt->nents,
 				DMA_FROM_DEVICE)) {
@@ -418,7 +419,7 @@ static int isp_stat_bufs_alloc_iommu(struct ispstat *stat, unsigned int size)
 		}
 		buf->iovm = iovm;
 
-		buf->virt_addr = da_to_va(stat->isp->iommu,
+		buf->virt_addr = omap_da_to_va(stat->isp->dev,
 					  (u32)buf->iommu_addr);
 		buf->empty = 1;
 		dev_dbg(stat->isp->dev, "%s: buffer[%d] allocated."
diff --git a/drivers/media/video/omap3isp/ispvideo.c b/drivers/media/video/omap3isp/ispvideo.c
index fd965adfd597..6d1d55b7c72c 100644
--- a/drivers/media/video/omap3isp/ispvideo.c
+++ b/drivers/media/video/omap3isp/ispvideo.c
@@ -446,7 +446,7 @@ ispmmu_vmap(struct isp_device *isp, const struct scatterlist *sglist, int sglen)
 	sgt->nents = sglen;
 	sgt->orig_nents = sglen;
 
-	da = iommu_vmap(isp->iommu, 0, sgt, IOMMU_FLAG);
+	da = omap_iommu_vmap(isp->domain, isp->dev, 0, sgt, IOMMU_FLAG);
 	if (IS_ERR_VALUE(da))
 		kfree(sgt);
 
@@ -462,7 +462,7 @@ static void ispmmu_vunmap(struct isp_device *isp, dma_addr_t da)
 {
 	struct sg_table *sgt;
 
-	sgt = iommu_vunmap(isp->iommu, (u32)da);
+	sgt = omap_iommu_vunmap(isp->domain, isp->dev, (u32)da);
 	kfree(sgt);
 }
 
diff --git a/drivers/media/video/ov5640.c b/drivers/media/video/ov5640.c
new file mode 100644
index 000000000000..04b0443a4d22
--- /dev/null
+++ b/drivers/media/video/ov5640.c
@@ -0,0 +1,1594 @@
+/*
+ * Copyright (c) 2012-2013, NVIDIA 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/soc_camera.h>
+
+struct ov5640_reg {
+	u16 addr;
+	u16 val;
+};
+
+#define OV5640_TABLE_WAIT_MS 0
+#define OV5640_TABLE_END 1
+
+static struct ov5640_reg mode_2592x1944[] = {
+	/* PLL Control MIPI bit rate/lane = 672MHz, 16-bit mode.
+	 * Output size: 2608x1948 (0, 0) - (2623, 1951),
+	 * Line Length = 2844, Frame Length = 1968
+	 */
+	{0x3103, 0x11},
+	{0x3008, 0x82},
+	{OV5640_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42},
+	{0x3103, 0x03},
+	{0x3017, 0x00},
+	{0x3018, 0x00},
+	{0x3034, 0x18},
+	{0x3035, 0x11},
+	{0x3036, 0x7d},
+	{0x3037, 0x13},
+	{0x3108, 0x01},
+	{0x3630, 0x36},
+	{0x3631, 0x0e},
+	{0x3632, 0xe2},
+	{0x3633, 0x12},
+	{0x3621, 0xe0},
+	{0x3704, 0xa0},
+	{0x3703, 0x5a},
+	{0x3715, 0x78},
+	{0x3717, 0x01},
+	{0x370b, 0x60},
+	{0x3705, 0x1a},
+	{0x3905, 0x02},
+	{0x3906, 0x10},
+	{0x3901, 0x0a},
+	{0x3731, 0x12},
+	{0x3600, 0x08},
+	{0x3601, 0x33},
+	{0x302d, 0x60},
+	{0x3620, 0x52},
+	{0x371b, 0x20},
+	{0x471c, 0x50},
+	{0x3a13, 0x43},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3635, 0x13},
+	{0x3636, 0x03},
+	{0x3634, 0x40},
+	{0x3622, 0x01},
+	{0x3c01, 0x34},
+	{0x3c04, 0x28},
+	{0x3c05, 0x98},
+	{0x3c06, 0x00},
+	{0x3c07, 0x07},
+	{0x3c08, 0x00},
+	{0x3c09, 0x1c},
+	{0x3c0a, 0x9c},
+	{0x3c0b, 0x40},
+	{0x3820, 0x40},
+	{0x3821, 0x06},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0x9f},
+	{0x3808, 0x0a},
+	{0x3809, 0x30},
+	{0x380a, 0x07},
+	{0x380b, 0x9c},
+	{0x380c, 0x0b},
+	{0x380d, 0x1c},
+	{0x380e, 0x07},
+	{0x380f, 0xb0},
+	{0x3810, 0x00},
+	{0x3811, 0x08},
+	{0x3812, 0x00},
+	{0x3813, 0x02},
+	{0x3618, 0x04},
+	{0x3612, 0x2b},
+	{0x3708, 0x64},
+	{0x3709, 0x12},
+	{0x370c, 0x00},
+	{0x3a02, 0x07},
+	{0x3a03, 0xb0},
+	{0x3a08, 0x01},
+	{0x3a09, 0x27},
+	{0x3a0a, 0x00},
+	{0x3a0b, 0xf6},
+	{0x3a0e, 0x06},
+	{0x3a0d, 0x08},
+	{0x3a14, 0x07},
+	{0x3a15, 0xb0},
+	{0x4001, 0x02},
+	{0x4004, 0x06},
+	{0x3000, 0x00},
+	{0x3002, 0x1c},
+	{0x3004, 0xff},
+	{0x3006, 0xc3},
+	{0x300e, 0x45},
+	{0x302e, 0x08},
+	{0x4300, 0x32},
+	{0x4800, 0x24},
+	{0x4837, 0x0a},
+	{0x501f, 0x00},
+	{0x440e, 0x00},
+	{0x5000, 0xa7},
+	{0x5001, 0x83},
+	{0x5180, 0xff},
+	{0x5181, 0xf2},
+	{0x5182, 0x00},
+	{0x5183, 0x14},
+	{0x5184, 0x25},
+	{0x5185, 0x24},
+	{0x5186, 0x09},
+	{0x5187, 0x09},
+	{0x5188, 0x09},
+	{0x5189, 0x75},
+	{0x518a, 0x54},
+	{0x518b, 0xe0},
+	{0x518c, 0xb2},
+	{0x518d, 0x42},
+	{0x518e, 0x3d},
+	{0x518f, 0x56},
+	{0x5190, 0x46},
+	{0x5191, 0xf8},
+	{0x5192, 0x04},
+	{0x5193, 0x70},
+	{0x5194, 0xf0},
+	{0x5195, 0xf0},
+	{0x5196, 0x03},
+	{0x5197, 0x01},
+	{0x5198, 0x04},
+	{0x5199, 0x12},
+	{0x519a, 0x04},
+	{0x519b, 0x00},
+	{0x519c, 0x06},
+	{0x519d, 0x82},
+	{0x519e, 0x38},
+	{0x5381, 0x1e},
+	{0x5382, 0x5b},
+	{0x5383, 0x08},
+	{0x5384, 0x0a},
+	{0x5385, 0x7e},
+	{0x5386, 0x88},
+	{0x5387, 0x7c},
+	{0x5388, 0x6c},
+	{0x5389, 0x10},
+	{0x538a, 0x01},
+	{0x538b, 0x98},
+	{0x5300, 0x08},
+	{0x5301, 0x30},
+	{0x5302, 0x10},
+	{0x5303, 0x00},
+	{0x5304, 0x08},
+	{0x5305, 0x30},
+	{0x5306, 0x08},
+	{0x5307, 0x16},
+	{0x5309, 0x08},
+	{0x530a, 0x30},
+	{0x530b, 0x04},
+	{0x530c, 0x06},
+	{0x5480, 0x01},
+	{0x5481, 0x08},
+	{0x5482, 0x14},
+	{0x5483, 0x28},
+	{0x5484, 0x51},
+	{0x5485, 0x65},
+	{0x5486, 0x71},
+	{0x5487, 0x7d},
+	{0x5488, 0x87},
+	{0x5489, 0x91},
+	{0x548a, 0x9a},
+	{0x548b, 0xaa},
+	{0x548c, 0xb8},
+	{0x548d, 0xcd},
+	{0x548e, 0xdd},
+	{0x548f, 0xea},
+	{0x5490, 0x1d},
+	{0x5580, 0x02},
+	{0x5583, 0x40},
+	{0x5584, 0x10},
+	{0x5589, 0x10},
+	{0x558a, 0x00},
+	{0x558b, 0xf8},
+	{0x5800, 0x23},
+	{0x5801, 0x14},
+	{0x5802, 0x0f},
+	{0x5803, 0x0f},
+	{0x5804, 0x12},
+	{0x5805, 0x26},
+	{0x5806, 0x0c},
+	{0x5807, 0x08},
+	{0x5808, 0x05},
+	{0x5809, 0x05},
+	{0x580a, 0x08},
+	{0x580b, 0x0d},
+	{0x580c, 0x08},
+	{0x580d, 0x03},
+	{0x580e, 0x00},
+	{0x580f, 0x00},
+	{0x5810, 0x03},
+	{0x5811, 0x09},
+	{0x5812, 0x07},
+	{0x5813, 0x03},
+	{0x5814, 0x00},
+	{0x5815, 0x01},
+	{0x5816, 0x03},
+	{0x5817, 0x08},
+	{0x5818, 0x0d},
+	{0x5819, 0x08},
+	{0x581a, 0x05},
+	{0x581b, 0x06},
+	{0x581c, 0x08},
+	{0x581d, 0x0e},
+	{0x581e, 0x29},
+	{0x581f, 0x17},
+	{0x5820, 0x11},
+	{0x5821, 0x11},
+	{0x5822, 0x15},
+	{0x5823, 0x28},
+	{0x5824, 0x46},
+	{0x5825, 0x26},
+	{0x5826, 0x08},
+	{0x5827, 0x26},
+	{0x5828, 0x64},
+	{0x5829, 0x26},
+	{0x582a, 0x24},
+	{0x582b, 0x22},
+	{0x582c, 0x24},
+	{0x582d, 0x24},
+	{0x582e, 0x06},
+	{0x582f, 0x22},
+	{0x5830, 0x40},
+	{0x5831, 0x42},
+	{0x5832, 0x24},
+	{0x5833, 0x26},
+	{0x5834, 0x24},
+	{0x5835, 0x22},
+	{0x5836, 0x22},
+	{0x5837, 0x26},
+	{0x5838, 0x44},
+	{0x5839, 0x24},
+	{0x583a, 0x26},
+	{0x583b, 0x28},
+	{0x583c, 0x42},
+	{0x583d, 0xce},
+	{0x5025, 0x00},
+	{0x3a0f, 0x30},
+	{0x3a10, 0x28},
+	{0x3a1b, 0x30},
+	{0x3a1e, 0x26},
+	{0x3a11, 0x60},
+	{0x3a1f, 0x14},
+	{0x3008, 0x02},
+	{OV5640_TABLE_END, 0x0000}
+};
+
+static struct ov5640_reg mode_1920x1088[] = {
+	/* PLL Control MIPI bit rate/lane = 672MHz, 16-bit mode.
+	 * Output size: 1936x1096 (336, 426) - (2287, 1529),
+	 * Line Length = 2500, Frame Length = 1120.
+	 */
+	{0x3103, 0x11},
+	{0x3008, 0x82},
+	{OV5640_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42},
+	{0x3103, 0x03},
+	{0x3017, 0x00},
+	{0x3018, 0x00},
+	{0x3034, 0x18},
+	{0x3035, 0x11},
+	{0x3036, 0x54},
+	{0x3037, 0x13},
+	{0x3108, 0x01},
+	{0x3630, 0x36},
+	{0x3631, 0x0e},
+	{0x3632, 0xe2},
+	{0x3633, 0x12},
+	{0x3621, 0xe0},
+	{0x3704, 0xa0},
+	{0x3703, 0x5a},
+	{0x3715, 0x78},
+	{0x3717, 0x01},
+	{0x370b, 0x60},
+	{0x3705, 0x1a},
+	{0x3905, 0x02},
+	{0x3906, 0x10},
+	{0x3901, 0x0a},
+	{0x3731, 0x12},
+	{0x3600, 0x08},
+	{0x3601, 0x33},
+	{0x302d, 0x60},
+	{0x3620, 0x52},
+	{0x371b, 0x20},
+	{0x471c, 0x50},
+	{0x3a13, 0x43},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3635, 0x13},
+	{0x3636, 0x03},
+	{0x3634, 0x40},
+	{0x3622, 0x01},
+	{0x3c01, 0x34},
+	{0x3c04, 0x28},
+	{0x3c05, 0x98},
+	{0x3c06, 0x00},
+	{0x3c07, 0x07},
+	{0x3c08, 0x00},
+	{0x3c09, 0x1c},
+	{0x3c0a, 0x9c},
+	{0x3c0b, 0x40},
+	{0x3820, 0x40},
+	{0x3821, 0x06},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x3800, 0x01},
+	{0x3801, 0x50},
+	{0x3802, 0x01},
+	{0x3803, 0xaa},
+	{0x3804, 0x08},
+	{0x3805, 0xef},
+	{0x3806, 0x05},
+	{0x3807, 0xf9},
+	{0x3808, 0x07},
+	{0x3809, 0x90},
+	{0x380a, 0x04},
+	{0x380b, 0x48},
+	{0x380c, 0x09},
+	{0x380d, 0xc4},
+	{0x380e, 0x04},
+	{0x380f, 0x60},
+	{0x3810, 0x00},
+	{0x3811, 0x08},
+	{0x3812, 0x00},
+	{0x3813, 0x04},
+	{0x3618, 0x04},
+	{0x3612, 0x2b},
+	{0x3708, 0x64},
+	{0x3709, 0x12},
+	{0x370c, 0x00},
+	{0x3a02, 0x04},
+	{0x3a03, 0x60},
+	{0x3a08, 0x01},
+	{0x3a09, 0x50},
+	{0x3a0a, 0x01},
+	{0x3a0b, 0x18},
+	{0x3a0e, 0x03},
+	{0x3a0d, 0x04},
+	{0x3a14, 0x04},
+	{0x3a15, 0x60},
+	{0x4001, 0x02},
+	{0x4004, 0x06},
+	{0x3000, 0x00},
+	{0x3002, 0x1c},
+	{0x3004, 0xff},
+	{0x3006, 0xc3},
+	{0x300e, 0x45},
+	{0x302e, 0x08},
+	{0x4300, 0x32},
+	{0x501f, 0x00},
+	{0x4713, 0x02},
+	{0x4407, 0x04},
+	{0x440e, 0x00},
+	{0x460b, 0x37},
+	{0x460c, 0x20},
+	{0x4800, 0x24},
+	{0x4837, 0x0a},
+	{0x3824, 0x04},
+	{0x5000, 0xa7},
+	{0x5001, 0x83},
+	{0x5180, 0xff},
+	{0x5181, 0xf2},
+	{0x5182, 0x00},
+	{0x5183, 0x14},
+	{0x5184, 0x25},
+	{0x5185, 0x24},
+	{0x5186, 0x09},
+	{0x5187, 0x09},
+	{0x5188, 0x09},
+	{0x5189, 0x75},
+	{0x518a, 0x54},
+	{0x518b, 0xe0},
+	{0x518c, 0xb2},
+	{0x518d, 0x42},
+	{0x518e, 0x3d},
+	{0x518f, 0x56},
+	{0x5190, 0x46},
+	{0x5191, 0xf8},
+	{0x5192, 0x04},
+	{0x5193, 0x70},
+	{0x5194, 0xf0},
+	{0x5195, 0xf0},
+	{0x5196, 0x03},
+	{0x5197, 0x01},
+	{0x5198, 0x04},
+	{0x5199, 0x12},
+	{0x519a, 0x04},
+	{0x519b, 0x00},
+	{0x519c, 0x06},
+	{0x519d, 0x82},
+	{0x519e, 0x38},
+	{0x5381, 0x1e},
+	{0x5382, 0x5b},
+	{0x5383, 0x08},
+	{0x5384, 0x0a},
+	{0x5385, 0x7e},
+	{0x5386, 0x88},
+	{0x5387, 0x7c},
+	{0x5388, 0x6c},
+	{0x5389, 0x10},
+	{0x538a, 0x01},
+	{0x538b, 0x98},
+	{0x5300, 0x08},
+	{0x5301, 0x30},
+	{0x5302, 0x10},
+	{0x5303, 0x00},
+	{0x5304, 0x08},
+	{0x5305, 0x30},
+	{0x5306, 0x08},
+	{0x5307, 0x16},
+	{0x5309, 0x08},
+	{0x530a, 0x30},
+	{0x530b, 0x04},
+	{0x530c, 0x06},
+	{0x5480, 0x01},
+	{0x5481, 0x08},
+	{0x5482, 0x14},
+	{0x5483, 0x28},
+	{0x5484, 0x51},
+	{0x5485, 0x65},
+	{0x5486, 0x71},
+	{0x5487, 0x7d},
+
+	{0x5488, 0x87},
+	{0x5489, 0x91},
+	{0x548a, 0x9a},
+	{0x548b, 0xaa},
+	{0x548c, 0xb8},
+	{0x548d, 0xcd},
+	{0x548e, 0xdd},
+	{0x548f, 0xea},
+	{0x5490, 0x1d},
+	{0x5580, 0x02},
+	{0x5583, 0x40},
+	{0x5584, 0x10},
+	{0x5589, 0x10},
+	{0x558a, 0x00},
+	{0x558b, 0xf8},
+	{0x5800, 0x23},
+	{0x5801, 0x14},
+	{0x5802, 0x0f},
+	{0x5803, 0x0f},
+	{0x5804, 0x12},
+	{0x5805, 0x26},
+	{0x5806, 0x0c},
+	{0x5807, 0x08},
+	{0x5808, 0x05},
+	{0x5809, 0x05},
+	{0x580a, 0x08},
+	{0x580b, 0x0d},
+	{0x580c, 0x08},
+	{0x580d, 0x03},
+	{0x580e, 0x00},
+	{0x580f, 0x00},
+	{0x5810, 0x03},
+	{0x5811, 0x09},
+	{0x5812, 0x07},
+	{0x5813, 0x03},
+	{0x5814, 0x00},
+	{0x5815, 0x01},
+	{0x5816, 0x03},
+	{0x5817, 0x08},
+	{0x5818, 0x0d},
+	{0x5819, 0x08},
+	{0x581a, 0x05},
+	{0x581b, 0x06},
+	{0x581c, 0x08},
+	{0x581d, 0x0e},
+	{0x581e, 0x29},
+	{0x581f, 0x17},
+	{0x5820, 0x11},
+	{0x5821, 0x11},
+	{0x5822, 0x15},
+	{0x5823, 0x28},
+	{0x5824, 0x46},
+	{0x5825, 0x26},
+	{0x5826, 0x08},
+	{0x5827, 0x26},
+	{0x5828, 0x64},
+	{0x5829, 0x26},
+	{0x582a, 0x24},
+	{0x582b, 0x22},
+	{0x582c, 0x24},
+	{0x582d, 0x24},
+	{0x582e, 0x06},
+	{0x582f, 0x22},
+	{0x5830, 0x40},
+	{0x5831, 0x42},
+	{0x5832, 0x24},
+	{0x5833, 0x26},
+	{0x5834, 0x24},
+	{0x5835, 0x22},
+	{0x5836, 0x22},
+	{0x5837, 0x26},
+	{0x5838, 0x44},
+	{0x5839, 0x24},
+	{0x583a, 0x26},
+	{0x583b, 0x28},
+	{0x583c, 0x42},
+	{0x583d, 0xce},
+	{0x5025, 0x00},
+	{0x3a0f, 0x30},
+	{0x3a10, 0x28},
+	{0x3a1b, 0x30},
+	{0x3a1e, 0x26},
+	{0x3a11, 0x60},
+	{0x3a1f, 0x14},
+	{0x3008, 0x02},
+	{OV5640_TABLE_END, 0x0000}
+};
+
+static struct ov5640_reg mode_1296x972[] = {
+	/* PLL Control MIPI bit rate/lane = 448MHz, 16-bit mode.
+	 * Output size: 1304x972 (0, 0) - (2623, 1951),
+	 * Line Length = 1886, Frame Length = 990.
+	 */
+	{0x3103, 0x11},
+	{0x3008, 0x82},
+	{OV5640_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42},
+	{0x3103, 0x03},
+	{0x3017, 0x00},
+	{0x3018, 0x00},
+	{0x3034, 0x18},
+	{0x3035, 0x21},
+	{0x3036, 0x70},
+	{0x3037, 0x13},
+	{0x3108, 0x01},
+	{0x3630, 0x36},
+	{0x3631, 0x0e},
+	{0x3632, 0xe2},
+	{0x3633, 0x12},
+	{0x3621, 0xe0},
+	{0x3704, 0xa0},
+	{0x3703, 0x5a},
+	{0x3715, 0x78},
+	{0x3717, 0x01},
+	{0x370b, 0x60},
+	{0x3705, 0x1a},
+	{0x3905, 0x02},
+	{0x3906, 0x10},
+	{0x3901, 0x0a},
+	{0x3731, 0x12},
+	{0x3600, 0x08},
+	{0x3601, 0x33},
+	{0x302d, 0x60},
+	{0x3620, 0x52},
+	{0x371b, 0x20},
+	{0x471c, 0x50},
+	{0x3a13, 0x43},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3635, 0x13},
+	{0x3636, 0x03},
+	{0x3634, 0x40},
+	{0x3622, 0x01},
+	{0x3c01, 0x34},
+	{0x3c04, 0x28},
+	{0x3c05, 0x98},
+	{0x3c06, 0x00},
+	{0x3c07, 0x07},
+	{0x3c08, 0x00},
+	{0x3c09, 0x1c},
+	{0x3c0a, 0x9c},
+	{0x3c0b, 0x40},
+	{0x3820, 0x41},
+	{0x3821, 0x07},
+	{0x3814, 0x31},
+	{0x3815, 0x31},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0x9f},
+	{0x3808, 0x05},
+	{0x3809, 0x18},
+	{0x380a, 0x03},
+	{0x380b, 0xcc},
+	{0x380c, 0x07},
+	{0x380d, 0x5e},
+	{0x380e, 0x03},
+	{0x380f, 0xde},
+	{0x3810, 0x00},
+	{0x3811, 0x06},
+	{0x3812, 0x00},
+	{0x3813, 0x02},
+	{0x3618, 0x00},
+	{0x3612, 0x29},
+	{0x3708, 0x62},
+	{0x3709, 0x52},
+	{0x370c, 0x03},
+	{0x3a02, 0x03},
+	{0x3a03, 0xd8},
+	{0x3a08, 0x01},
+	{0x3a09, 0x27},
+	{0x3a0a, 0x00},
+	{0x3a0b, 0xf6},
+	{0x3a0e, 0x03},
+	{0x3a0d, 0x04},
+	{0x3a14, 0x03},
+	{0x3a15, 0xd8},
+	{0x4001, 0x02},
+	{0x4004, 0x02},
+	{0x3000, 0x00},
+	{0x3002, 0x1c},
+	{0x3004, 0xff},
+	{0x3006, 0xc3},
+	{0x300e, 0x45},
+	{0x302e, 0x08},
+	{0x4300, 0x32},
+	{0x501f, 0x00},
+	{0x4713, 0x02},
+	{0x4407, 0x04},
+	{0x440e, 0x00},
+	{0x460b, 0x37},
+	{0x460c, 0x20},
+	{0x4800, 0x24},
+	{0x4837, 0x10},
+	{0x3824, 0x04},
+	{0x5000, 0xa7},
+	{0x5001, 0x83},
+	{0x5180, 0xff},
+	{0x5181, 0xf2},
+	{0x5182, 0x00},
+	{0x5183, 0x14},
+	{0x5184, 0x25},
+	{0x5185, 0x24},
+	{0x5186, 0x09},
+	{0x5187, 0x09},
+	{0x5188, 0x09},
+	{0x5189, 0x75},
+	{0x518a, 0x54},
+	{0x518b, 0xe0},
+	{0x518c, 0xb2},
+	{0x518d, 0x42},
+	{0x518e, 0x3d},
+	{0x518f, 0x56},
+	{0x5190, 0x46},
+	{0x5191, 0xf8},
+	{0x5192, 0x04},
+	{0x5193, 0x70},
+	{0x5194, 0xf0},
+	{0x5195, 0xf0},
+	{0x5196, 0x03},
+	{0x5197, 0x01},
+	{0x5198, 0x04},
+	{0x5199, 0x12},
+	{0x519a, 0x04},
+	{0x519b, 0x00},
+	{0x519c, 0x06},
+	{0x519d, 0x82},
+	{0x519e, 0x38},
+	{0x5381, 0x1e},
+	{0x5382, 0x5b},
+	{0x5383, 0x08},
+	{0x5384, 0x0a},
+	{0x5385, 0x7e},
+	{0x5386, 0x88},
+	{0x5387, 0x7c},
+	{0x5388, 0x6c},
+	{0x5389, 0x10},
+	{0x538a, 0x01},
+	{0x538b, 0x98},
+	{0x5300, 0x08},
+	{0x5301, 0x30},
+	{0x5302, 0x10},
+	{0x5303, 0x00},
+	{0x5304, 0x08},
+	{0x5305, 0x30},
+	{0x5306, 0x08},
+	{0x5307, 0x16},
+	{0x5309, 0x08},
+	{0x530a, 0x30},
+	{0x530b, 0x04},
+	{0x530c, 0x06},
+	{0x5480, 0x01},
+	{0x5481, 0x08},
+	{0x5482, 0x14},
+	{0x5483, 0x28},
+	{0x5484, 0x51},
+	{0x5485, 0x65},
+	{0x5486, 0x71},
+	{0x5487, 0x7d},
+	{0x5488, 0x87},
+	{0x5489, 0x91},
+	{0x548a, 0x9a},
+	{0x548b, 0xaa},
+	{0x548c, 0xb8},
+	{0x548d, 0xcd},
+	{0x548e, 0xdd},
+	{0x548f, 0xea},
+	{0x5490, 0x1d},
+	{0x5580, 0x02},
+	{0x5583, 0x40},
+	{0x5584, 0x10},
+	{0x5589, 0x10},
+	{0x558a, 0x00},
+	{0x558b, 0xf8},
+	{0x5800, 0x23},
+	{0x5801, 0x14},
+	{0x5802, 0x0f},
+	{0x5803, 0x0f},
+	{0x5804, 0x12},
+	{0x5805, 0x26},
+	{0x5806, 0x0c},
+	{0x5807, 0x08},
+	{0x5808, 0x05},
+	{0x5809, 0x05},
+	{0x580a, 0x08},
+	{0x580b, 0x0d},
+	{0x580c, 0x08},
+	{0x580d, 0x03},
+	{0x580e, 0x00},
+	{0x580f, 0x00},
+	{0x5810, 0x03},
+	{0x5811, 0x09},
+	{0x5812, 0x07},
+	{0x5813, 0x03},
+	{0x5814, 0x00},
+	{0x5815, 0x01},
+	{0x5816, 0x03},
+	{0x5817, 0x08},
+	{0x5818, 0x0d},
+	{0x5819, 0x08},
+	{0x581a, 0x05},
+	{0x581b, 0x06},
+	{0x581c, 0x08},
+	{0x581d, 0x0e},
+	{0x581e, 0x29},
+	{0x581f, 0x17},
+	{0x5820, 0x11},
+	{0x5821, 0x11},
+	{0x5822, 0x15},
+	{0x5823, 0x28},
+	{0x5824, 0x46},
+	{0x5825, 0x26},
+	{0x5826, 0x08},
+	{0x5827, 0x26},
+	{0x5828, 0x64},
+	{0x5829, 0x26},
+	{0x582a, 0x24},
+	{0x582b, 0x22},
+	{0x582c, 0x24},
+	{0x582d, 0x24},
+	{0x582e, 0x06},
+	{0x582f, 0x22},
+	{0x5830, 0x40},
+	{0x5831, 0x42},
+	{0x5832, 0x24},
+	{0x5833, 0x26},
+	{0x5834, 0x24},
+	{0x5835, 0x22},
+	{0x5836, 0x22},
+	{0x5837, 0x26},
+	{0x5838, 0x44},
+	{0x5839, 0x24},
+	{0x583a, 0x26},
+	{0x583b, 0x28},
+	{0x583c, 0x42},
+	{0x583d, 0xce},
+	{0x5025, 0x00},
+	{0x3a0f, 0x30},
+	{0x3a10, 0x28},
+	{0x3a1b, 0x30},
+	{0x3a1e, 0x26},
+	{0x3a11, 0x60},
+	{0x3a1f, 0x14},
+	{0x3008, 0x02},
+	{OV5640_TABLE_END, 0x0000}
+};
+
+static struct ov5640_reg mode_640x480[] = {
+	{0x3103, 0x11},
+	{0x3008, 0x82},
+	{OV5640_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42},
+	{0x3103, 0x03},
+	{0x3017, 0x00},
+	{0x3018, 0x00},
+	{0x3034, 0x18},
+	{0x3035, 0x14},
+	{0x3036, 0x70},
+	{0x3037, 0x13},
+	{0x4800, 0x24}, /* noncontinuous clock */
+	{0x3108, 0x01},
+	{0x3630, 0x36},
+	{0x3631, 0x0e},
+	{0x3632, 0xe2},
+	{0x3633, 0x12},
+	{0x3621, 0xe0},
+	{0x3704, 0xa0},
+	{0x3703, 0x5a},
+	{0x3715, 0x78},
+	{0x3717, 0x01},
+	{0x370b, 0x60},
+	{0x3705, 0x1a},
+	{0x3905, 0x02},
+	{0x3906, 0x10},
+	{0x3901, 0x0a},
+	{0x3731, 0x12},
+	{0x3600, 0x08},
+	{0x3601, 0x33},
+	{0x302d, 0x60},
+	{0x3620, 0x52},
+	{0x371b, 0x20},
+	{0x471c, 0x50},
+	{0x3a13, 0x43},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3635, 0x13},
+	{0x3636, 0x03},
+	{0x3634, 0x40},
+	{0x3622, 0x01},
+	{0x3c01, 0x34},
+	{0x3c04, 0x28},
+	{0x3c05, 0x98},
+	{0x3c06, 0x00},
+	{0x3c07, 0x08},
+	{0x3c08, 0x00},
+	{0x3c09, 0x1c},
+	{0x3c0a, 0x9c},
+	{0x3c0b, 0x40},
+	{0x3820, 0x41},
+	{0x3821, 0x07},
+	{0x3814, 0x31},
+	{0x3815, 0x31},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x04},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0x9b},
+	{0x3808, 0x02},
+	{0x3809, 0x80},
+	{0x380a, 0x01},
+	{0x380b, 0xe0},
+	{0x380c, 0x07},
+	{0x380d, 0x68},
+	{0x380e, 0x03},
+	{0x380f, 0xd8},
+	{0x3810, 0x00},
+	{0x3811, 0x10},
+	{0x3812, 0x00},
+	{0x3813, 0x06},
+	{0x3618, 0x00},
+	{0x3612, 0x29},
+	{0x3708, 0x64},
+	{0x3709, 0x52},
+	{0x370c, 0x03},
+
+	/* AEC/AGC Power Down Domain Control */
+	{0x3a02, 0x03},
+	{0x3a03, 0xd8},
+	{0x3a08, 0x01},
+	{0x3a09, 0x27},
+	{0x3a0a, 0x00},
+	{0x3a0b, 0xf6},
+	{0x3a0e, 0x03},
+	{0x3a0d, 0x04},
+	{0x3a14, 0x03},
+	{0x3a15, 0xd8},
+
+	{0x4001, 0x02},
+	{0x4004, 0x02},
+	{0x3000, 0x00},
+	{0x3002, 0x1c},
+	{0x3004, 0xff},
+	{0x3006, 0xc3},
+	{0x300e, 0x45},
+	{0x302e, 0x08},
+	/* org:30 bit[3:0]
+	   0x0:YUYV 0x1:YVYU 0x2:UYVY
+	   0x3:VYUY 0xF:UYVY 0x4~0xE:Not-allowed
+	 */
+	{0x4300, 0x32},
+	{0x501f, 0x00},
+	{0x4713, 0x03},
+	{0x4407, 0x04},
+	{0x440e, 0x00},
+	{0x460b, 0x35},
+	{0x460c, 0x22},
+	{0x4837, 0x44},
+	{0x3824, 0x02},
+	{0x5000, 0xa7},
+	{0x5001, 0xa3},
+
+	/* AWB Control */
+	{0x5180, 0xff},
+	{0x5181, 0xf2},
+	{0x5182, 0x00},
+	{0x5183, 0x14},
+	{0x5184, 0x25},
+	{0x5185, 0x24},
+	{0x5186, 0x09},
+	{0x5187, 0x09},
+	{0x5188, 0x09},
+	{0x5189, 0x75},
+	{0x518a, 0x54},
+	{0x518b, 0xe0},
+	{0x518c, 0xb2},
+	{0x518d, 0x42},
+	{0x518e, 0x3d},
+	{0x518d, 0x56},
+	{0x5190, 0x46},
+	{0x5191, 0xf8},
+	{0x5192, 0x04},
+	{0x5193, 0x70},
+	{0x5194, 0xf0},
+	{0x5195, 0xf0},
+	{0x5196, 0x03},
+	{0x5197, 0x01},
+	{0x5198, 0x04},
+	{0x5199, 0x12},
+	{0x519a, 0x04},
+	{0x519b, 0x00},
+	{0x519c, 0x06},
+	{0x519d, 0x82},
+	{0x519e, 0x38},
+
+	/* CMX Control */
+	{0x5381, 0x1e},
+	{0x5382, 0x5b},
+	{0x5383, 0x08},
+	{0x5384, 0x0a},
+	{0x5385, 0x7e},
+	{0x5386, 0x88},
+	{0x5387, 0x7c},
+	{0x5388, 0x6c},
+	{0x5389, 0x10},
+	{0x538a, 0x01},
+	{0x538b, 0x98},
+
+	/* CIP Control */
+	{0x5300, 0x08},
+	{0x5301, 0x30},
+	{0x5302, 0x10},
+	{0x5303, 0x00},
+	{0x5304, 0x08},
+	{0x5305, 0x30},
+	{0x5306, 0x08},
+	{0x5307, 0x16},
+	{0x5309, 0x08},
+	{0x530a, 0x30},
+	{0x530b, 0x04},
+	{0x530c, 0x06},
+
+	/* Gamma Control */
+	{0x5480, 0x01},
+	{0x5481, 0x08},
+	{0x5482, 0x14},
+	{0x5483, 0x28},
+	{0x5484, 0x51},
+	{0x5485, 0x65},
+	{0x5486, 0x71},
+	{0x5487, 0x7d},
+	{0x5488, 0x87},
+	{0x5489, 0x91},
+	{0x548a, 0x9a},
+	{0x548b, 0xaa},
+	{0x548c, 0xb8},
+	{0x548d, 0xcd},
+	{0x548e, 0xdd},
+	{0x548f, 0xea},
+	{0x5490, 0x1d},
+
+	/* SDE Control */
+	{0x5580, 0x02},
+	{0x5583, 0x40},
+	{0x5584, 0x10},
+	{0x5589, 0x10},
+	{0x558a, 0x00},
+	{0x558b, 0xf8},
+
+	/* LENC Control */
+	{0x5800, 0x23},
+	{0x5801, 0x14},
+	{0x5802, 0x0f},
+	{0x5803, 0x0f},
+	{0x5804, 0x12},
+	{0x5805, 0x26},
+	{0x5806, 0x0c},
+	{0x5807, 0x08},
+	{0x5808, 0x05},
+	{0x5809, 0x05},
+	{0x580a, 0x08},
+	{0x580b, 0x0d},
+	{0x580c, 0x08},
+	{0x580d, 0x03},
+	{0x580e, 0x00},
+	{0x580f, 0x00},
+	{0x5810, 0x03},
+	{0x5811, 0x09},
+	{0x5812, 0x07},
+	{0x5813, 0x03},
+	{0x5814, 0x00},
+	{0x5815, 0x01},
+	{0x5816, 0x03},
+	{0x5817, 0x08},
+	{0x5818, 0x0d},
+	{0x5819, 0x08},
+	{0x581a, 0x05},
+	{0x581b, 0x06},
+	{0x581c, 0x08},
+	{0x581d, 0x0e},
+	{0x581e, 0x29},
+	{0x581f, 0x17},
+	{0x5820, 0x11},
+	{0x5821, 0x11},
+	{0x5822, 0x15},
+	{0x5823, 0x28},
+	{0x5824, 0x46},
+	{0x5825, 0x26},
+	{0x5826, 0x08},
+	{0x5827, 0x26},
+	{0x5828, 0x64},
+	{0x5829, 0x26},
+	{0x582a, 0x24},
+	{0x582b, 0x22},
+	{0x582c, 0x24},
+	{0x582d, 0x24},
+	{0x582e, 0x06},
+	{0x582f, 0x22},
+	{0x5830, 0x40},
+	{0x5831, 0x42},
+	{0x5832, 0x24},
+	{0x5833, 0x26},
+	{0x5834, 0x24},
+	{0x5835, 0x22},
+	{0x5836, 0x22},
+	{0x5837, 0x26},
+	{0x5838, 0x44},
+	{0x5839, 0x24},
+	{0x583a, 0x26},
+	{0x583b, 0x28},
+	{0x583c, 0x42},
+	{0x583d, 0xce},
+
+	{0x5025, 0x00},
+	{0x3a0f, 0x30},
+	{0x3a10, 0x28},
+	{0x3a1b, 0x30},
+	{0x3a1e, 0x26},
+	{0x3a11, 0x60},
+	{0x3a1f, 0x14},
+	{0x3008, 0x02},
+	{OV5640_TABLE_END, 0x0000},
+};
+
+enum {
+	OV5640_MODE_640x480,
+	OV5640_MODE_1296x972,
+	OV5640_MODE_1920x1088,
+	OV5640_MODE_2592x1944,
+	OV5640_SIZE_LAST,
+};
+
+static struct ov5640_reg *mode_table[] = {
+	[OV5640_MODE_640x480] = mode_640x480,
+	[OV5640_MODE_1296x972] = mode_1296x972,
+	[OV5640_MODE_1920x1088] = mode_1920x1088,
+	[OV5640_MODE_2592x1944] = mode_2592x1944,
+};
+
+static int test_pattern;
+module_param(test_pattern, int, 0644);
+
+static struct ov5640_reg tp_cbars[] = {
+	{0x503D, 0x80},
+	{0x503E, 0x00},
+	{0x5046, 0x01},
+	{OV5640_TABLE_END, 0x0000}
+};
+
+#define to_ov5640(sd)		container_of(sd, struct ov5640_priv, subdev)
+
+#define SIZEOF_I2C_TRANSBUF 32
+
+struct ov5640_priv {
+	struct v4l2_subdev		subdev;
+	struct v4l2_mbus_framefmt	mf;
+
+	int				ident;
+	u16				chip_id;
+	u8				revision;
+
+	int mode;
+
+	struct i2c_client *client;
+	u8 i2c_trans_buf[SIZEOF_I2C_TRANSBUF];
+};
+
+static enum v4l2_mbus_pixelcode ov5640_codes[] = {
+	V4L2_MBUS_FMT_UYVY8_2X8,
+};
+
+static const struct v4l2_frmsize_discrete ov5640_frmsizes[OV5640_SIZE_LAST] = {
+	{640, 480},
+	{1296, 972},
+	{1920, 1088},
+	{2592, 1944},
+};
+
+static int ov5640_find_mode(u32 width, u32 height)
+{
+	int i;
+
+	for (i = 0; i < OV5640_SIZE_LAST; i++) {
+		if ((ov5640_frmsizes[i].width >= width) &&
+		    (ov5640_frmsizes[i].height >= height))
+			break;
+	}
+
+	/* If not found, select biggest */
+	if (i >= OV5640_SIZE_LAST)
+		i = OV5640_SIZE_LAST - 1;
+
+	return i;
+}
+
+static int ov5640_read_reg(struct i2c_client *client, u16 addr, u8 *val)
+{
+	int err;
+	struct i2c_msg msg[2];
+	unsigned char data[3];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 2;
+	msg[0].buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8) (addr >> 8);
+	data[1] = (u8) (addr & 0xff);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = data + 2;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+
+	if (err != 2)
+		return -EINVAL;
+
+	*val = data[2];
+
+	return 0;
+}
+
+static int ov5640_write_reg(struct i2c_client *client, u16 addr, u8 value)
+{
+	int count;
+	struct i2c_msg msg[1];
+	unsigned char data[4];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = addr;
+	data[1] = (u8) (addr & 0xff);
+	data[2] = value;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 3;
+	msg[0].buf = data;
+
+	count = i2c_transfer(client->adapter, msg, ARRAY_SIZE(msg));
+	if (count == ARRAY_SIZE(msg))
+		return 0;
+	dev_err(&client->dev,
+		"ov5840: i2c transfer failed, addr: %x, value: %02x\n",
+	       addr, (u32)value);
+	return -EIO;
+}
+
+static int ov5640_write_bulk_reg(struct i2c_client *client, u8 *data, int len)
+{
+	int err;
+	struct i2c_msg msg;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = len;
+	msg.buf = data;
+
+	err = i2c_transfer(client->adapter, &msg, 1);
+	if (err == 1)
+		return 0;
+
+	dev_err(&client->dev, "ov5640: i2c transfer failed at %x\n",
+		(int)data[0] << 8 | data[1]);
+
+	return err;
+}
+
+static int ov5640_write_table(struct ov5640_priv *priv,
+			      struct ov5640_reg table[])
+{
+	int err;
+	struct ov5640_reg *next, *n_next;
+	u8 *b_ptr = priv->i2c_trans_buf;
+	unsigned int buf_filled = 0;
+	u16 val;
+
+	for (next = table; next->addr != OV5640_TABLE_END; next++) {
+		if (next->addr == OV5640_TABLE_WAIT_MS) {
+			msleep(next->val);
+			continue;
+		}
+
+		val = next->val;
+
+		if (!buf_filled) {
+			b_ptr = priv->i2c_trans_buf;
+			*b_ptr++ = next->addr >> 8;
+			*b_ptr++ = next->addr & 0xff;
+			buf_filled = 2;
+		}
+		*b_ptr++ = val;
+		buf_filled++;
+
+		n_next = next + 1;
+		if (n_next->addr != OV5640_TABLE_END &&
+			n_next->addr != OV5640_TABLE_WAIT_MS &&
+			buf_filled < SIZEOF_I2C_TRANSBUF &&
+			n_next->addr == next->addr + 1) {
+			continue;
+		}
+
+		err = ov5640_write_bulk_reg(priv->client,
+			priv->i2c_trans_buf, buf_filled);
+		if (err)
+			return err;
+
+		buf_filled = 0;
+	}
+	return 0;
+}
+
+static void ov5640_set_default_fmt(struct ov5640_priv *priv)
+{
+	struct v4l2_mbus_framefmt *mf = &priv->mf;
+
+	mf->width = ov5640_frmsizes[OV5640_MODE_2592x1944].width;
+	mf->height = ov5640_frmsizes[OV5640_MODE_2592x1944].height;
+	mf->code = V4L2_MBUS_FMT_UYVY8_2X8;
+	mf->field = V4L2_FIELD_NONE;
+	mf->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+/* Start/Stop streaming from the device */
+static int ov5640_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov5640_priv *priv = to_ov5640(sd);
+	int ret = 0;
+
+	if (!enable) {
+		ov5640_set_default_fmt(priv);
+		return 0;
+	}
+
+	ret = ov5640_write_table(priv, mode_table[priv->mode]);
+	if (ret)
+		return ret;
+
+	if (test_pattern == 1) {
+		ret = ov5640_write_table(priv, tp_cbars);
+		if (ret)
+			return ret;
+	}
+
+	return ret;
+}
+
+static int ov5640_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_mbus_framefmt *mf)
+{
+	int mode;
+
+	mode = ov5640_find_mode(mf->width, mf->height);
+	mf->width = ov5640_frmsizes[mode].width;
+	mf->height = ov5640_frmsizes[mode].height;
+
+	mf->field = V4L2_FIELD_NONE;
+	mf->code = V4L2_MBUS_FMT_UYVY8_2X8;
+	mf->colorspace = V4L2_COLORSPACE_SRGB;
+
+	return 0;
+}
+
+/* set the format we will capture in */
+static int ov5640_s_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct ov5640_priv *priv = to_ov5640(sd);
+	int ret;
+
+	ret = ov5640_try_fmt(sd, mf);
+	if (ret < 0)
+		return ret;
+
+	priv->mode = ov5640_find_mode(mf->width, mf->height);
+
+	memcpy(&priv->mf, mf, sizeof(struct v4l2_mbus_framefmt));
+
+	return 0;
+}
+
+static int ov5640_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			   enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= ARRAY_SIZE(ov5640_codes))
+		return -EINVAL;
+
+	*code = ov5640_codes[index];
+
+	return 0;
+}
+
+static int ov5640_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left		= 0;
+	a->bounds.top		= 0;
+	a->bounds.width		= ov5640_frmsizes[OV5640_MODE_2592x1944].width;
+	a->bounds.height	= ov5640_frmsizes[OV5640_MODE_2592x1944].height;
+	a->defrect		= a->bounds;
+	a->type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int ov5640_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	a->c.left		= 0;
+	a->c.top		= 0;
+	a->c.width		= ov5640_frmsizes[OV5640_MODE_2592x1944].width;
+	a->c.height		= ov5640_frmsizes[OV5640_MODE_2592x1944].height;
+	a->type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+/* Get chip identification */
+static int ov5640_g_chip_ident(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_chip_ident *id)
+{
+	struct ov5640_priv *priv = to_ov5640(sd);
+
+	id->ident = priv->ident;
+	id->revision = priv->revision;
+
+	return 0;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ov5640_get_register(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u8 val;
+
+	if (reg->reg & ~0xffff)
+		return -EINVAL;
+
+	reg->size = 2;
+
+	ret = ov5640_read_reg(client, reg->reg, &val);
+	if (ret)
+		return ret;
+
+	reg->val = (__u64)val;
+
+	return ret;
+}
+
+static int ov5640_set_register(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_register *reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (reg->reg & ~0xffff || reg->val & ~0xff)
+		return -EINVAL;
+
+	return ov5640_write_reg(client, reg->reg, reg->val);
+}
+#endif
+
+/* Alter bus settings on camera side */
+static int ov5640_set_bus_param(struct soc_camera_device *icd,
+		unsigned long flags)
+{
+	return 0;
+}
+
+/* Request bus settings on camera side */
+static unsigned long ov5640_query_bus_param(struct soc_camera_device *icd)
+{
+	struct soc_camera_link *icl = to_soc_camera_link(icd);
+
+	unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
+		SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
+		SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8;
+
+	return soc_camera_apply_sensor_flags(icl, flags);
+}
+
+static struct soc_camera_ops ov5640_ops = {
+	.set_bus_param		= ov5640_set_bus_param,
+	.query_bus_param	= ov5640_query_bus_param,
+};
+
+static struct v4l2_subdev_video_ops ov5640_video_ops = {
+	.s_stream		= ov5640_s_stream,
+	.s_mbus_fmt		= ov5640_s_fmt,
+	.try_mbus_fmt		= ov5640_try_fmt,
+	.enum_mbus_fmt		= ov5640_enum_fmt,
+	.cropcap		= ov5640_cropcap,
+	.g_crop			= ov5640_g_crop,
+};
+
+static struct v4l2_subdev_core_ops ov5640_core_ops = {
+	.g_chip_ident		= ov5640_g_chip_ident,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register		= ov5640_get_register,
+	.s_register		= ov5640_set_register,
+#endif
+};
+
+static struct v4l2_subdev_ops ov5640_subdev_ops = {
+	.core			= &ov5640_core_ops,
+	.video			= &ov5640_video_ops,
+};
+
+/*
+ * i2c_driver function
+ */
+static int ov5640_probe(struct i2c_client *client,
+			const struct i2c_device_id *did)
+{
+	struct ov5640_priv *priv;
+	struct soc_camera_device *icd	= client->dev.platform_data;
+	struct soc_camera_link *icl;
+	u8 chip_id_hi, chip_id_lo;
+	int ret;
+
+	if (!icd) {
+		dev_err(&client->dev, "Missing soc-camera data!\n");
+		return -EINVAL;
+	}
+
+	icl = to_soc_camera_link(icd);
+	if (!icl) {
+		dev_err(&client->dev, "Missing platform_data for driver\n");
+		return -EINVAL;
+	}
+
+	priv = devm_kzalloc(&client->dev, sizeof(struct ov5640_priv),
+				GFP_KERNEL);
+	if (!priv) {
+		dev_err(&client->dev, "Failed to allocate private data!\n");
+		return -ENOMEM;
+	}
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &ov5640_subdev_ops);
+	icd->ops = &ov5640_ops;
+
+	priv->client = client;
+
+	priv->ident = V4L2_IDENT_OV5640;
+
+	/*
+	 * check and show product ID and manufacturer ID
+	 */
+	ret = ov5640_read_reg(client, 0x300A, &chip_id_hi);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failure to read Chip ID (high byte)\n");
+		return ret;
+	}
+
+	ret = ov5640_read_reg(client, 0x300B, &chip_id_lo);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failure to read Chip ID (low byte)\n");
+		return ret;
+	}
+
+	priv->chip_id = (chip_id_hi << 8) | chip_id_lo;
+
+	if (priv->chip_id != 0x5640) {
+		dev_err(&client->dev, "Chip ID: %x not supported!\n",
+			priv->chip_id);
+		ret = -ENODEV;
+		return ret;
+	}
+
+	ov5640_set_default_fmt(priv);
+
+	dev_info(&client->dev, "Chip ID 0x%04x\n", priv->chip_id);
+	return ret;
+}
+
+static int ov5640_remove(struct i2c_client *client)
+{
+	return 0;
+}
+
+static const struct i2c_device_id ov5640_id[] = {
+	{ "ov5640", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov5640_id);
+
+static struct i2c_driver ov5640_i2c_driver = {
+	.driver = {
+		.name = "ov5640",
+	},
+	.probe    = ov5640_probe,
+	.remove   = ov5640_remove,
+	.id_table = ov5640_id,
+};
+
+static int __init ov5640_module_init(void)
+{
+	return i2c_add_driver(&ov5640_i2c_driver);
+}
+
+static void __exit ov5640_module_exit(void)
+{
+	i2c_del_driver(&ov5640_i2c_driver);
+}
+
+module_init(ov5640_module_init);
+module_exit(ov5640_module_exit);
+
+MODULE_DESCRIPTION("SoC Camera driver for OmniVision OV5640");
+MODULE_AUTHOR("Andrew Chew <achew@nvidia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/video/ov5650.c b/drivers/media/video/ov5650.c
new file mode 100644
index 000000000000..9b47d66536ef
--- /dev/null
+++ b/drivers/media/video/ov5650.c
@@ -0,0 +1,1406 @@
+/*
+ * OmniVision OV5650 sensor driver
+ *
+ * Copyright (c) 2013, NVIDIA 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 version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/videodev2.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/log2.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/soc_camera.h>
+
+#include <media/ov5650.h>
+
+#define SIZEOF_I2C_TRANSBUF 32
+
+struct ov5650_priv {
+	struct v4l2_subdev subdev;
+	struct v4l2_mbus_framefmt mf;
+	const struct ov5650_platform_data *pdata;
+
+	int ident;
+	u16 chip_id;
+	u8 revision;
+
+	int mode;
+
+	struct i2c_client *client;
+	u8 i2c_trans_buf[SIZEOF_I2C_TRANSBUF];
+};
+
+static struct ov5650_priv *to_ov5650(const struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct ov5650_priv, subdev);
+}
+
+/**
+ * struct ov5650_reg - ov5650 register format
+ * @addr: 16-bit offset to register
+ * @val: 8/16/32-bit register value
+ *
+ * Define a structure for OV5650 register initialization values
+ */
+struct ov5650_reg {
+	u16	addr;
+	u16	val;
+};
+
+#define OV5650_TABLE_WAIT_MS 0
+#define OV5650_TABLE_END 1
+#define OV5650_MAX_RETRIES 3
+
+static struct ov5650_reg tp_none_seq[] = {
+	{0x5046, 0x00},
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg tp_cbars_seq[] = {
+	{0x503D, 0xC0},
+	{0x503E, 0x00},
+	{0x5046, 0x01},
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg tp_checker_seq[] = {
+	{0x503D, 0xC0},
+	{0x503E, 0x0A},
+	{0x5046, 0x01},
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg *test_pattern_modes[] = {
+	tp_none_seq,
+	tp_cbars_seq,
+	tp_checker_seq,
+};
+
+enum {
+	TP_NONE = 1,
+	TP_COLORBARS = 2,
+	TP_CHECKER = 3,
+};
+
+static int test_mode = 2;
+module_param(test_mode, int, 0644);
+
+static struct ov5650_reg reset_seq[] = {
+	{0x3008, 0x82},
+	{OV5650_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42},
+	{OV5650_TABLE_WAIT_MS, 5},
+	{OV5650_TABLE_END, 0x0000},
+};
+
+static struct ov5650_reg mode_start[] = {
+	{0x3103, 0x93},
+	{0x3017, 0xff},
+	{0x3018, 0xfc},
+
+	{0x3600, 0x50},
+	{0x3601, 0x0d},
+	{0x3604, 0x50},
+	{0x3605, 0x04},
+	{0x3606, 0x3f},
+	{0x3612, 0x1a},
+	{0x3630, 0x22},
+	{0x3631, 0x22},
+	{0x3702, 0x3a},
+	{0x3704, 0x18},
+	{0x3705, 0xda},
+	{0x3706, 0x41},
+	{0x370a, 0x80},
+	{0x370b, 0x40},
+	{0x370e, 0x00},
+	{0x3710, 0x28},
+	{0x3712, 0x13},
+	{0x3830, 0x50},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3a00, 0x38},
+
+
+	{0x3603, 0xa7},
+	{0x3615, 0x50},
+	{0x3620, 0x56},
+	{0x3810, 0x00},
+	{0x3836, 0x00},
+	{0x3a1a, 0x06},
+	{0x4000, 0x01},
+	{0x401c, 0x48},
+	{0x401d, 0x08},
+	{0x5000, 0x06},
+	{0x5001, 0x00},
+	{0x5002, 0x00},
+	{0x503d, 0x00},
+	{0x5046, 0x00},
+
+	{0x300f, 0x8f},
+
+	{0x3010, 0x10},
+	{0x3011, 0x14},
+	{0x3012, 0x02},
+	{0x3815, 0x82},
+	{0x3503, 0x00},
+	{0x3613, 0x44},
+	{OV5650_TABLE_END, 0x0},
+};
+
+static struct ov5650_reg mode_2592x1944[] = {
+	{0x3621, 0x2f},
+
+	{0x3632, 0x55},
+	{0x3703, 0xe6},
+	{0x370c, 0xa0},
+	{0x370d, 0x04},
+	{0x3713, 0x2f},
+	{0x3800, 0x02},
+	{0x3801, 0x58},
+	{0x3802, 0x00},
+	{0x3803, 0x0c},
+	{0x3804, 0x0a},
+	{0x3805, 0x20},
+	{0x3806, 0x07},
+	{0x3807, 0xa0},
+	{0x3808, 0x0a},
+
+	{0x3809, 0x20},
+
+	{0x380a, 0x07},
+
+	{0x380b, 0xa0},
+
+	{0x380c, 0x0c},
+
+	{0x380d, 0xb4},
+
+	{0x380e, 0x07},
+
+	{0x380f, 0xb0},
+
+	{0x3818, 0xc0},
+	{0x381a, 0x3c},
+	{0x3a0d, 0x06},
+	{0x3c01, 0x00},
+	{0x3007, 0x3f},
+	{0x5059, 0x80},
+	{0x3003, 0x03},
+	{0x3500, 0x00},
+	{0x3501, 0x7a},
+
+	{0x3502, 0xd0},
+
+	{0x350a, 0x00},
+	{0x350b, 0x00},
+	{0x401d, 0x08},
+	{0x4801, 0x0f},
+	{0x300e, 0x0c},
+	{0x4803, 0x50},
+	{0x4800, 0x34},
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_1296x972[] = {
+	{0x3621, 0xaf},
+
+	{0x3632, 0x5a},
+	{0x3703, 0xb0},
+	{0x370c, 0xc5},
+	{0x370d, 0x42},
+	{0x3713, 0x2f},
+	{0x3800, 0x03},
+	{0x3801, 0x3c},
+	{0x3802, 0x00},
+	{0x3803, 0x62},
+	{0x3804, 0x05},
+	{0x3805, 0x10},
+	{0x3806, 0x03},
+	{0x3807, 0xd0},
+	{0x3808, 0x05},
+
+	{0x3809, 0x10},
+
+	{0x380a, 0x03},
+
+	{0x380b, 0xd0},
+
+	{0x380c, 0x08},
+
+	{0x380d, 0xa8},
+
+	{0x380e, 0x05},
+
+	{0x380f, 0xa4},
+
+	{0x3818, 0xc1},
+	{0x381a, 0x00},
+	{0x3a0d, 0x08},
+	{0x3c01, 0x00},
+	{0x3007, 0x3b},
+	{0x5059, 0x80},
+	{0x3003, 0x03},
+	{0x3500, 0x00},
+
+	{0x3501, 0x5a},
+	{0x3502, 0x10},
+	{0x350a, 0x00},
+	{0x350b, 0x10},
+	{0x401d, 0x08},
+	{0x4801, 0x0f},
+	{0x300e, 0x0c},
+	{0x4803, 0x50},
+	{0x4800, 0x34},
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_2080x1164[] = {
+	{0x3103, 0x93},
+	{0x3007, 0x3b},
+	{0x3017, 0xff},
+	{0x3018, 0xfc},
+
+	{0x3600, 0x54},
+	{0x3601, 0x05},
+	{0x3603, 0xa7},
+	{0x3604, 0x40},
+	{0x3605, 0x04},
+	{0x3606, 0x3f},
+	{0x3612, 0x1a},
+	{0x3613, 0x44},
+	{0x3615, 0x52},
+	{0x3620, 0x56},
+	{0x3623, 0x01},
+	{0x3630, 0x22},
+	{0x3631, 0x36},
+	{0x3632, 0x5f},
+	{0x3633, 0x24},
+
+	{0x3702, 0x3a},
+	{0x3704, 0x18},
+	{0x3706, 0x41},
+	{0x370b, 0x40},
+	{0x370e, 0x00},
+	{0x3710, 0x28},
+	{0x3711, 0x24},
+	{0x3712, 0x13},
+
+	{0x3810, 0x00},
+	{0x3815, 0x82},
+	{0x3830, 0x50},
+	{0x3836, 0x00},
+
+	{0x3a1a, 0x06},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3a00, 0x38},
+
+	{0x3a0d, 0x06},
+	{0x3c01, 0x34},
+
+	{0x401f, 0x03},
+	{0x4000, 0x05},
+	{0x401d, 0x08},
+	{0x4001, 0x02},
+
+	{0x5001, 0x00},
+	{0x5002, 0x00},
+	{0x503d, 0x00},
+	{0x5046, 0x00},
+
+	{0x300f, 0x8f},
+
+	{0x3010, 0x10},
+	{0x3011, 0x14},
+	{0x3012, 0x02},
+	{0x3503, 0x00},
+
+
+	{0x3621, 0x2f},
+
+	{0x3703, 0xe6},
+	{0x370c, 0x00},
+	{0x370d, 0x04},
+	{0x3713, 0x22},
+	{0x3714, 0x27},
+	{0x3705, 0xda},
+	{0x370a, 0x80},
+
+	{0x3800, 0x02},
+	{0x3801, 0x12},
+	{0x3802, 0x00},
+	{0x3803, 0x0a},
+	{0x3804, 0x08},
+	{0x3805, 0x20},
+	{0x3806, 0x04},
+	{0x3807, 0x92},
+	{0x3808, 0x08},
+
+	{0x3809, 0x20},
+
+	{0x380a, 0x04},
+
+	{0x380b, 0x92},
+
+	{0x380c, 0x0a},
+
+	{0x380d, 0x96},
+
+	{0x380e, 0x04},
+
+	{0x380f, 0x9e},
+
+	{0x3818, 0xc0},
+	{0x381a, 0x3c},
+	{0x381c, 0x31},
+	{0x381d, 0x8e},
+	{0x381e, 0x04},
+	{0x381f, 0x92},
+	{0x3820, 0x04},
+	{0x3821, 0x19},
+	{0x3824, 0x01},
+	{0x3827, 0x0a},
+	{0x401c, 0x46},
+
+	{0x3003, 0x03},
+	{0x3500, 0x00},
+	{0x3501, 0x49},
+	{0x3502, 0xa0},
+	{0x350a, 0x00},
+	{0x350b, 0x00},
+	{0x4801, 0x0f},
+	{0x300e, 0x0c},
+	{0x4803, 0x50},
+	{0x4800, 0x34},
+
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_1920x1080[] = {
+	{0x3103, 0x93},
+	{0x3007, 0x3b},
+	{0x3017, 0xff},
+	{0x3018, 0xfc},
+
+	{0x3600, 0x54},
+	{0x3601, 0x05},
+	{0x3603, 0xa7},
+	{0x3604, 0x40},
+	{0x3605, 0x04},
+	{0x3606, 0x3f},
+	{0x3612, 0x1a},
+	{0x3613, 0x44},
+	{0x3615, 0x52},
+	{0x3620, 0x56},
+	{0x3623, 0x01},
+	{0x3630, 0x22},
+	{0x3631, 0x36},
+	{0x3632, 0x5f},
+	{0x3633, 0x24},
+
+	{0x3702, 0x3a},
+	{0x3704, 0x18},
+	{0x3706, 0x41},
+	{0x370b, 0x40},
+	{0x370e, 0x00},
+	{0x3710, 0x28},
+	{0x3711, 0x24},
+	{0x3712, 0x13},
+
+	{0x3810, 0x00},
+	{0x3815, 0x82},
+
+	{0x3830, 0x50},
+	{0x3836, 0x00},
+
+	{0x3a1a, 0x06},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3a00, 0x38},
+	{0x3a0d, 0x06},
+	{0x3c01, 0x34},
+
+	{0x401f, 0x03},
+	{0x4000, 0x05},
+	{0x401d, 0x08},
+	{0x4001, 0x02},
+
+	{0x5001, 0x00},
+	{0x5002, 0x00},
+	{0x503d, 0x00},
+	{0x5046, 0x00},
+
+	{0x300f, 0x8f},
+	{0x3010, 0x10},
+	{0x3011, 0x14},
+	{0x3012, 0x02},
+	{0x3503, 0x00},
+
+	{0x3621, 0x2f},
+	{0x3703, 0xe6},
+	{0x370c, 0x00},
+	{0x370d, 0x04},
+	{0x3713, 0x22},
+	{0x3714, 0x27},
+	{0x3705, 0xda},
+	{0x370a, 0x80},
+
+	{0x3800, 0x02},
+	{0x3801, 0x94},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x07},
+	{0x3805, 0x80},
+	{0x3806, 0x04},
+	{0x3807, 0x40},
+	{0x3808, 0x07},
+	{0x3809, 0x80},
+	{0x380a, 0x04},
+	{0x380b, 0x40},
+	{0x380c, 0x0a},
+	{0x380d, 0x84},
+	{0x380e, 0x04},
+	{0x380f, 0xa4},
+	{0x3818, 0xc0},
+	{0x381a, 0x3c},
+	{0x381c, 0x31},
+	{0x381d, 0xa4},
+	{0x381e, 0x04},
+	{0x381f, 0x60},
+	{0x3820, 0x03},
+	{0x3821, 0x1a},
+	{0x3824, 0x01},
+	{0x3827, 0x0a},
+	{0x401c, 0x46},
+
+	{0x3003, 0x03},
+	{0x3500, 0x00},
+	{0x3501, 0x49},
+	{0x3502, 0xa0},
+	{0x350a, 0x00},
+	{0x350b, 0x00},
+	{0x4801, 0x0f},
+	{0x300e, 0x0c},
+	{0x4803, 0x50},
+	{0x4800, 0x34},
+
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_1280x720[] = {
+	{0x3103, 0x93},
+	{0x3b07, 0x0c},
+	{0x3017, 0xff},
+	{0x3018, 0xfc},
+	{0x3706, 0x41},
+	{0x3613, 0xc4},
+	{0x370d, 0x42},
+	{0x3703, 0x9a},
+	{0x3630, 0x22},
+	{0x3605, 0x04},
+	{0x3606, 0x3f},
+	{0x3712, 0x13},
+	{0x370e, 0x00},
+	{0x370b, 0x40},
+	{0x3600, 0x54},
+	{0x3601, 0x05},
+	{0x3713, 0x22},
+	{0x3714, 0x27},
+	{0x3631, 0x22},
+	{0x3612, 0x1a},
+	{0x3604, 0x40},
+	{0x3705, 0xdb},
+	{0x370a, 0x81},
+	{0x370c, 0x00},
+	{0x3710, 0x28},
+	{0x3702, 0x3a},
+	{0x3704, 0x18},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3a00, 0x38},
+	{0x3800, 0x02},
+	{0x3801, 0x54},
+	{0x3803, 0x0c},
+	{0x380c, 0x0c},
+	{0x380d, 0xb4},
+	{0x380e, 0x07},
+	{0x380f, 0xb0},
+	{0x3830, 0x50},
+	{0x3a08, 0x12},
+	{0x3a09, 0x70},
+	{0x3a0a, 0x0f},
+	{0x3a0b, 0x60},
+	{0x3a0d, 0x06},
+	{0x3a0e, 0x06},
+	{0x3a13, 0x54},
+	{0x3815, 0x82},
+	{0x5059, 0x80},
+	{0x3615, 0x52},
+	{0x505a, 0x0a},
+	{0x505b, 0x2e},
+	{0x3713, 0x92},
+	{0x3714, 0x17},
+	{0x3804, 0x05},
+	{0x3805, 0x00},
+	{0x3806, 0x02},
+	{0x3807, 0xd0},
+	{0x3808, 0x05},
+	{0x3809, 0x00},
+	{0x380a, 0x02},
+	{0x380b, 0xd0},
+	{0x380c, 0x08},
+	{0x380d, 0x72},
+	{0x380e, 0x02},
+	{0x380f, 0xe4},
+	{0x3815, 0x81},
+	{0x381c, 0x10},
+	{0x381d, 0x82},
+	{0x381e, 0x05},
+	{0x381f, 0xc0},
+	{0x3821, 0x20},
+	{0x3824, 0x23},
+	{0x3825, 0x2c},
+	{0x3826, 0x00},
+	{0x3827, 0x0c},
+	{0x3a08, 0x1b},
+	{0x3a09, 0xc0},
+	{0x3a0a, 0x17},
+	{0x3a0b, 0x20},
+	{0x3a0d, 0x01},
+	{0x3a0e, 0x01},
+	{0x3a1a, 0x06},
+	{0x3503, 0x00},
+	{0x3623, 0x01},
+	{0x3633, 0x24},
+	{0x3c01, 0x34},
+	{0x3c04, 0x28},
+	{0x3c05, 0x98},
+	{0x3c07, 0x07},
+	{0x3c09, 0xc2},
+	{0x4000, 0x05},
+	{0x401d, 0x28},
+	{0x4001, 0x02},
+	{0x401c, 0x42},
+	{0x5046, 0x09},
+	{0x3810, 0x40},
+	{0x3836, 0x41},
+	{0x505f, 0x04},
+	{0x5000, 0xfe},
+	{0x5001, 0x01},
+	{0x5002, 0x00},
+	{0x503d, 0x00},  /* bit[7]=1 enable test_pattern */
+	{0x5901, 0x00},
+	{0x585a, 0x01},
+	{0x585b, 0x2c},
+	{0x585c, 0x01},
+	{0x585d, 0x93},
+	{0x585e, 0x01},
+	{0x585f, 0x90},
+	{0x5860, 0x01},
+	{0x5861, 0x0d},
+	{0x5180, 0xc0},
+	{0x5184, 0x00},
+	{0x470a, 0x00},
+	{0x470b, 0x00},
+	{0x470c, 0x00},
+	{0x300f, 0x8e},
+	{0x3603, 0xa7},
+	{0x3632, 0x55},
+	{0x3620, 0x56},
+	{0x3621, 0xaf},
+	{0x3818, 0xc1},
+	{0x3631, 0x36},
+	{0x3632, 0x5f},
+	{0x3711, 0x24},
+	{0x401f, 0x03},
+	{0x3008, 0x02},
+
+	{0x3011, 0x14},
+	{0x3007, 0x3B},
+	{0x4801, 0x0f},
+	{0x3003, 0x03},
+	{0x300e, 0x0c},
+	{0x4803, 0x50},
+	{0x4800, 0x04},  /* bit[5]=0 as CSI continuous clock */
+	{0x300f, 0x8f},
+	{0x3010, 0x10},
+	{0x3815, 0x82},
+	{0x3003, 0x01},
+
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_1264x704[] = {
+	{0x3600, 0x54},
+	{0x3601, 0x05},
+	{0x3604, 0x40},
+	{0x3705, 0xdb},
+	{0x370a, 0x81},
+	{0x3615, 0x52},
+	{0x3810, 0x40},
+	{0x3836, 0x41},
+	{0x4000, 0x05},
+	{0x401c, 0x42},
+	{0x401d, 0x08},
+	{0x5046, 0x09},
+	{0x3010, 0x00},
+	{0x3503, 0x00},
+	{0x3613, 0xc4},
+
+	{0x3621, 0xaf},
+
+	{0x3632, 0x55},
+	{0x3703, 0x9a},
+	{0x370c, 0x00},
+	{0x370d, 0x42},
+	{0x3713, 0x22},
+	{0x3800, 0x02},
+	{0x3801, 0x54},
+	{0x3802, 0x00},
+	{0x3803, 0x0c},
+	{0x3804, 0x05},
+	{0x3805, 0x00},
+	{0x3806, 0x02},
+	{0x3807, 0xd0},
+	{0x3808, 0x05},
+
+	{0x3809, 0x00},
+
+	{0x380a, 0x02},
+
+	{0x380b, 0xd0},
+
+	{0x380c, 0x08},
+
+	{0x380d, 0x72},
+
+	{0x380e, 0x02},
+
+	{0x380f, 0xe4},
+
+	{0x3818, 0xc1},
+	{0x381a, 0x3c},
+	{0x3a0d, 0x06},
+	{0x3c01, 0x34},
+	{0x3007, 0x3b},
+	{0x5059, 0x80},
+	{0x3003, 0x03},
+	{0x3500, 0x04},
+	{0x3501, 0xa5},
+
+	{0x3502, 0x10},
+
+	{0x350a, 0x00},
+	{0x350b, 0x00},
+	{0x4801, 0x0f},
+	{0x300e, 0x0c},
+	{0x4803, 0x50},
+	{0x4800, 0x24},
+	{0x300f, 0x8b},
+
+	{0x3711, 0x24},
+	{0x3713, 0x92},
+	{0x3714, 0x17},
+	{0x381c, 0x10},
+	{0x381d, 0x82},
+	{0x381e, 0x05},
+	{0x381f, 0xc0},
+	{0x3821, 0x20},
+	{0x3824, 0x23},
+	{0x3825, 0x2c},
+	{0x3826, 0x00},
+	{0x3827, 0x0c},
+	{0x3623, 0x01},
+	{0x3633, 0x24},
+	{0x3632, 0x5f},
+	{0x401f, 0x03},
+
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_320x240[] = {
+	{0x3103, 0x93},
+	{0x3b07, 0x0c},
+	{0x3017, 0xff},
+	{0x3018, 0xfc},
+	{0x3706, 0x41},
+	{0x3613, 0xc4},
+	{0x370d, 0x42},
+	{0x3703, 0x9a},
+	{0x3630, 0x22},
+	{0x3605, 0x04},
+	{0x3606, 0x3f},
+	{0x3712, 0x13},
+	{0x370e, 0x00},
+	{0x370b, 0x40},
+	{0x3600, 0x54},
+	{0x3601, 0x05},
+	{0x3713, 0x22},
+	{0x3714, 0x27},
+	{0x3631, 0x22},
+	{0x3612, 0x1a},
+	{0x3604, 0x40},
+	{0x3705, 0xdc},
+	{0x370a, 0x83},
+	{0x370c, 0xc8},
+	{0x3710, 0x28},
+	{0x3702, 0x3a},
+	{0x3704, 0x18},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3a00, 0x38},
+	{0x3800, 0x02},
+	{0x3801, 0x54},
+	{0x3803, 0x0c},
+	{0x380c, 0x0c},
+	{0x380d, 0xb4},
+	{0x380e, 0x07},
+	{0x380f, 0xb0},
+	{0x3830, 0x50},
+	{0x3a08, 0x12},
+	{0x3a09, 0x70},
+	{0x3a0a, 0x0f},
+	{0x3a0b, 0x60},
+	{0x3a0d, 0x06},
+	{0x3a0e, 0x06},
+	{0x3a13, 0x54},
+	{0x3815, 0x82},
+	{0x5059, 0x80},
+	{0x3615, 0x52},
+	{0x505a, 0x0a},
+	{0x505b, 0x2e},
+	{0x3713, 0x92},
+	{0x3714, 0x17},
+	{0x3803, 0x0a},
+	{0x3804, 0x05},
+	{0x3805, 0x00},
+	{0x3806, 0x01},
+	{0x3807, 0x00},
+	{0x3808, 0x01},
+	{0x3809, 0x40},
+	{0x380a, 0x01},
+	{0x380b, 0x00},
+	{0x380c, 0x0a},
+
+	{0x380d, 0x04},
+
+	{0x380e, 0x01},
+
+	{0x380f, 0x38},
+
+	{0x3500, 0x00},
+	{0x3501, 0x13},
+	{0x3502, 0x80},
+	{0x350b, 0x7f},
+
+	{0x3815, 0x81},
+	{0x3824, 0x23},
+	{0x3825, 0x20},
+	{0x3826, 0x00},
+	{0x3827, 0x08},
+	{0x370d, 0xc2},
+	{0x3a08, 0x17},
+	{0x3a09, 0x64},
+	{0x3a0a, 0x13},
+	{0x3a0b, 0x80},
+	{0x3a00, 0x58},
+	{0x3a1a, 0x06},
+	{0x3503, 0x00},
+	{0x3623, 0x01},
+	{0x3633, 0x24},
+	{0x3c01, 0x34},
+	{0x3c04, 0x28},
+	{0x3c05, 0x98},
+	{0x3c07, 0x07},
+	{0x3c09, 0xc2},
+	{0x4000, 0x05},
+	{0x401d, 0x08},
+	{0x4001, 0x02},
+	{0x401c, 0x42},
+	{0x5046, 0x09},
+	{0x3810, 0x40},
+	{0x3836, 0x41},
+	{0x505f, 0x04},
+	{0x5001, 0x00},
+	{0x5002, 0x02},
+	{0x503d, 0x00},
+	{0x5901, 0x08},
+	{0x585a, 0x01},
+	{0x585b, 0x2c},
+	{0x585c, 0x01},
+	{0x585d, 0x93},
+	{0x585e, 0x01},
+	{0x585f, 0x90},
+	{0x5860, 0x01},
+	{0x5861, 0x0d},
+	{0x5180, 0xc0},
+	{0x5184, 0x00},
+	{0x470a, 0x00},
+	{0x470b, 0x00},
+	{0x470c, 0x00},
+	{0x300f, 0x8e},
+	{0x3603, 0xa7},
+	{0x3632, 0x55},
+	{0x3620, 0x56},
+	{0x3621, 0xaf},
+	{0x3818, 0xc3},
+	{0x3631, 0x36},
+	{0x3632, 0x5f},
+	{0x3711, 0x24},
+	{0x401f, 0x03},
+
+	{0x3011, 0x14},
+	{0x3007, 0x3B},
+	{0x300f, 0x8f},
+	{0x4801, 0x0f},
+	{0x3003, 0x03},
+	{0x300e, 0x0c},
+	{0x3010, 0x15},
+	{0x4803, 0x50},
+	{0x4800, 0x24},
+	{0x4837, 0x40},
+	{0x3815, 0x82},
+
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_end[] = {
+	{0x3212, 0x00},
+	{0x3003, 0x01},
+	{0x3212, 0x10},
+	{0x3212, 0xa0},
+	{0x3008, 0x02},
+
+	{OV5650_TABLE_END, 0x0000}
+};
+
+enum {
+	OV5650_MODE_2592x1944,
+	OV5650_MODE_2080x1164,
+	OV5650_MODE_1920x1080,
+	OV5650_MODE_1296x972,
+	OV5650_MODE_1280x720,
+	OV5650_MODE_1264x704,
+	OV5650_MODE_320x240,
+	OV5650_MODE_INVALID
+};
+
+static struct ov5650_reg *mode_table[] = {
+	[OV5650_MODE_2592x1944] = mode_2592x1944,
+	[OV5650_MODE_2080x1164] = mode_2080x1164,
+	[OV5650_MODE_1920x1080] = mode_1920x1080,
+	[OV5650_MODE_1296x972]  = mode_1296x972,
+	[OV5650_MODE_1280x720]  = mode_1280x720,
+	[OV5650_MODE_1264x704]  = mode_1264x704,
+	[OV5650_MODE_320x240]   = mode_320x240
+};
+
+static const struct v4l2_frmsize_discrete ov5650_frmsizes[] = {
+	{2592, 1944},
+	{2080, 1164},
+	{1920, 1080},
+	{1296, 972},
+	{1280, 720},
+	{1264, 704},
+	{320, 240},
+};
+
+static int ov5650_find_mode(u32 width, u32 height)
+{
+	if (width == 2592 && height == 1944)
+		return OV5650_MODE_2592x1944;
+	else if (width == 2080 && height == 1164)
+		return OV5650_MODE_2080x1164;
+	else if (width == 1920 && height == 1080)
+		return OV5650_MODE_1920x1080;
+	else if (width == 1296 && height == 972)
+		return OV5650_MODE_1296x972;
+	else if (width == 1280 && height == 720)
+		return OV5650_MODE_1280x720;
+	else if (width == 1264 && height == 704)
+		return OV5650_MODE_1264x704;
+	else if (width == 320 && height == 240)
+		return OV5650_MODE_320x240;
+	else {
+		pr_err("ov5650: %dx%d is not supported\n", width, height);
+		return OV5650_MODE_2592x1944;
+	}
+}
+
+/**
+ * ov5650_reg_read - Read a value from a register in an ov5650 sensor device
+ * @client: i2c driver client structure
+ * @reg: register address / offset
+ * @val: stores the value that gets read
+ *
+ * Read a value from a register in an ov5650 sensor device.
+ * The value is returned in 'val'.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int ov5650_reg_read(struct i2c_client *client, u16 reg, u8 *val)
+{
+	int ret;
+	u8 data[2] = {0};
+	struct i2c_msg msg = {
+		.addr	= client->addr,
+		.flags	= 0,
+		.len	= 2,
+		.buf	= data,
+	};
+
+	data[0] = (u8)(reg >> 8);
+	data[1] = (u8)(reg & 0xff);
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0)
+		goto err;
+
+	msg.flags = I2C_M_RD;
+	msg.len = 1;
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0)
+		goto err;
+
+	*val = data[0];
+	return 0;
+
+err:
+	dev_err(&client->dev, "Failed reading register 0x%02x!\n", reg);
+	return ret;
+}
+
+/**
+ * Write a value to a register in ov5650 sensor device.
+ * @client: i2c driver client structure.
+ * @reg: Address of the register to read value from.
+ * @val: Value to be written to a specific register.
+ * Returns zero if successful, or non-zero otherwise.
+ */
+static int ov5650_reg_write(struct i2c_client *client, u16 reg, u8 val)
+{
+	int ret;
+	unsigned char data[3] = { (u8)(reg >> 8), (u8)(reg & 0xff), val };
+	struct i2c_msg msg = {
+		.addr	= client->addr,
+		.flags	= 0,
+		.len	= 3,
+		.buf	= data,
+	};
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0) {
+		dev_err(&client->dev, "Failed writing register 0x%02x!\n", reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ov5650_write_bulk_reg(struct ov5650_priv *priv, int len)
+{
+	struct i2c_client *client = priv->client;
+	u8 *data = priv->i2c_trans_buf;
+	int err;
+	struct i2c_msg msg;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = len;
+	msg.buf = data;
+
+	err = i2c_transfer(client->adapter, &msg, 1);
+	if (err != 1) {
+		dev_err(&client->dev, "I2C bulk transfer failed at %x\n",
+			(int)data[0] << 8 | data[1]);
+		return err;
+	}
+
+	return 0;
+}
+static int ov5650_write_table(struct ov5650_priv *priv,
+				const struct ov5650_reg table[],
+				const struct ov5650_reg override_list[],
+				int num_override_regs)
+{
+	int err;
+	const struct ov5650_reg *next, *n_next;
+	u8 *b_ptr = priv->i2c_trans_buf;
+	unsigned int buf_filled = 0;
+	unsigned int i;
+	u16 val;
+
+	for (next = table; next->addr != OV5650_TABLE_END; next++) {
+		if (next->addr == OV5650_TABLE_WAIT_MS) {
+			msleep(next->val);
+			continue;
+		}
+
+		val = next->val;
+		/* When an override list is passed in, replace the reg */
+		/* value to write if the reg is in the list            */
+		if (override_list) {
+			for (i = 0; i < num_override_regs; i++) {
+				if (next->addr == override_list[i].addr) {
+					val = override_list[i].val;
+					break;
+				}
+			}
+		}
+
+		if (!buf_filled) {
+			b_ptr = priv->i2c_trans_buf;
+			*b_ptr++ = next->addr >> 8;
+			*b_ptr++ = next->addr & 0xff;
+			buf_filled = 2;
+		}
+		*b_ptr++ = val;
+		buf_filled++;
+
+		n_next = next + 1;
+		if (n_next->addr != OV5650_TABLE_END &&
+			n_next->addr != OV5650_TABLE_WAIT_MS &&
+			buf_filled < SIZEOF_I2C_TRANSBUF &&
+			n_next->addr == next->addr + 1) {
+			continue;
+		}
+
+		err = ov5650_write_bulk_reg(priv, buf_filled);
+		if (err)
+			return err;
+
+		buf_filled = 0;
+	}
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev internal operations
+ */
+
+static void ov5650_set_default_fmt(struct ov5650_priv *priv)
+{
+	struct v4l2_mbus_framefmt *mf = &priv->mf;
+
+	mf->width = ov5650_frmsizes[OV5650_MODE_2592x1944].width;
+	mf->height = ov5650_frmsizes[OV5650_MODE_2592x1944].height;
+	mf->code = V4L2_MBUS_FMT_SBGGR10_1X10;
+	mf->field = V4L2_FIELD_NONE;
+	mf->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+static int ov5650_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov5650_priv *priv = to_ov5650(sd);
+	struct ov5650_reg reg_list[6];
+	int ret = 0;
+
+	if (enable) {
+		ret = ov5650_write_table(priv, reset_seq, NULL, 0);
+		if (ret)
+			return ret;
+
+		ret = ov5650_write_table(priv, mode_start, NULL, 0);
+		if (ret)
+			return ret;
+
+		ret = ov5650_write_table(priv, mode_table[priv->mode],
+				reg_list, 6);
+		if (ret)
+			return ret;
+
+		ret = ov5650_write_table(priv, mode_end, NULL, 0);
+		if (ret)
+			return ret;
+
+		switch (test_mode) {
+		case TP_NONE:
+			ret = ov5650_write_table(priv, tp_none_seq, NULL, 0);
+			break;
+		case TP_COLORBARS:
+			ret = ov5650_write_table(priv, tp_cbars_seq, NULL, 0);
+			break;
+		case TP_CHECKER:
+			ret = ov5650_write_table(priv, tp_checker_seq, NULL, 0);
+			break;
+		}
+		if (ret)
+			return ret;
+
+	} else
+		ov5650_set_default_fmt(priv);
+
+	return ret;
+}
+
+/* Alter bus settings on camera side */
+static int ov5650_set_bus_param(struct soc_camera_device *icd,
+		unsigned long flags)
+{
+	return 0;
+}
+
+/* Request bus settings on camera side */
+static unsigned long ov5650_query_bus_param(struct soc_camera_device *icd)
+{
+	struct soc_camera_link *icl = to_soc_camera_link(icd);
+
+	unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
+		SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
+		SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_10;
+
+	return soc_camera_apply_sensor_flags(icl, flags);
+}
+
+static int ov5650_s_fmt(struct v4l2_subdev *sd,
+			struct v4l2_mbus_framefmt *mf)
+{
+	struct ov5650_priv *priv = to_ov5650(sd);
+
+	priv->mode = ov5650_find_mode(mf->width, mf->height);
+	memcpy(&priv->mf, mf, sizeof(struct v4l2_mbus_framefmt));
+
+	return 0;
+}
+
+static int ov5650_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct ov5650_priv *priv = to_ov5650(sd);
+
+	if (on) {
+		ov5650_s_fmt(sd, &priv->mf);
+		ov5650_s_stream(sd, 1);
+	} else
+		ov5650_s_stream(sd, 0);
+
+	return 0;
+}
+
+static int ov5650_try_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_mbus_framefmt *mf)
+{
+	int mode;
+
+	mode = ov5650_find_mode(mf->width, mf->height);
+	mf->width = ov5650_frmsizes[mode].width;
+	mf->height = ov5650_frmsizes[mode].height;
+
+	if (mf->code != V4L2_MBUS_FMT_SBGGR8_1X8 &&
+		mf->code != V4L2_MBUS_FMT_SBGGR10_1X10)
+		mf->code = V4L2_MBUS_FMT_SBGGR10_1X10;
+
+	mf->field = V4L2_FIELD_NONE;
+	mf->colorspace = V4L2_COLORSPACE_SRGB;
+
+	ov5650_s_fmt(sd, mf);
+
+	return 0;
+}
+
+static int ov5650_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
+			   enum v4l2_mbus_pixelcode *code)
+{
+	if (index >= 2)
+		return -EINVAL;
+
+	switch (index) {
+	case 0:
+		*code = V4L2_MBUS_FMT_SBGGR10_1X10;
+		break;
+	case 1:
+		*code = V4L2_MBUS_FMT_SBGGR8_1X8;
+		break;
+	}
+
+	return 0;
+}
+
+static int ov5650_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
+{
+	a->bounds.left		= 0;
+	a->bounds.top		= 0;
+	a->bounds.width		= ov5650_frmsizes[OV5650_MODE_2592x1944].width;
+	a->bounds.height	= ov5650_frmsizes[OV5650_MODE_2592x1944].height;
+	a->defrect		= a->bounds;
+	a->type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	a->pixelaspect.numerator	= 1;
+	a->pixelaspect.denominator	= 1;
+
+	return 0;
+}
+
+static int ov5650_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
+{
+	a->c.left		= 0;
+	a->c.top		= 0;
+	a->c.width		= ov5650_frmsizes[OV5650_MODE_2592x1944].width;
+	a->c.height		= ov5650_frmsizes[OV5650_MODE_2592x1944].height;
+	a->type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	return 0;
+}
+
+/* Get chip identification */
+static int ov5650_g_chip_ident(struct v4l2_subdev *sd,
+			       struct v4l2_dbg_chip_ident *id)
+{
+	struct ov5650_priv *priv = to_ov5650(sd);
+
+	id->ident = priv->ident;
+	id->revision = priv->revision;
+
+	return 0;
+}
+
+static struct soc_camera_ops ov5650_ops = {
+	.set_bus_param		= ov5650_set_bus_param,
+	.query_bus_param	= ov5650_query_bus_param,
+};
+
+static struct v4l2_subdev_video_ops ov5650_video_ops = {
+	.s_stream		= ov5650_s_stream,
+	.s_mbus_fmt		= ov5650_s_fmt,
+	.try_mbus_fmt		= ov5650_try_fmt,
+	.enum_mbus_fmt		= ov5650_enum_fmt,
+	.cropcap		= ov5650_cropcap,
+	.g_crop			= ov5650_g_crop,
+};
+
+static struct v4l2_subdev_core_ops ov5650_core_ops = {
+	.g_chip_ident		= ov5650_g_chip_ident,
+	.s_power		= ov5650_s_power,
+};
+
+static struct v4l2_subdev_ops ov5650_subdev_ops = {
+	.core			= &ov5650_core_ops,
+	.video			= &ov5650_video_ops,
+};
+
+static int ov5650_probe(struct i2c_client *client,
+			 const struct i2c_device_id *did)
+{
+	struct ov5650_priv *priv;
+	struct soc_camera_device *icd	= client->dev.platform_data;
+	struct soc_camera_link *icl;
+	u8 chipid[2];
+	int ret;
+
+	/* Checking soc-camera interface */
+	if (!icd) {
+		dev_err(&client->dev, "Missing soc-camera data!\n");
+		return -EINVAL;
+	}
+
+	icl = to_soc_camera_link(icd);
+	if (!icl) {
+		dev_err(&client->dev, "Missing platform_data for driver\n");
+		return -EINVAL;
+	}
+
+	/* Register OV5650 soc_camera device interface */
+	priv = kzalloc(sizeof(struct ov5650_priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(&client->dev, "Failed to allocate private data!\n");
+		return -ENOMEM;
+	}
+
+	v4l2_i2c_subdev_init(&priv->subdev, client, &ov5650_subdev_ops);
+	icd->ops = &ov5650_ops;
+
+	priv->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+
+	priv->ident = V4L2_IDENT_OV5650;
+
+	ret = ov5650_reg_read(client, 0x300A, &chipid[0]);
+	if (ret) {
+		dev_err(&client->dev, "Failure to read Chip ID (high byte)\n");
+		goto err;
+	}
+
+	ret = ov5650_reg_read(client, 0x300B, &chipid[1]);
+	if (ret) {
+		dev_err(&client->dev, "Failure to read Chip ID (low byte)\n");
+		goto err;
+	}
+
+	if ((chipid[0] != 0x56) || ((chipid[1] & 0x51) != chipid[1])) {
+		dev_err(&client->dev, "Chip ID: %x%x not supported!\n",
+			chipid[0], chipid[1]);
+		ret = -ENODEV;
+		goto err;
+	}
+
+	priv->chip_id = (chipid[0] << 8) | chipid[1];
+	priv->revision = (chipid[1] == 0x50) ? 0x1A : 0x1B;
+
+	priv->client = client;
+
+	ov5650_set_default_fmt(priv);
+
+	dev_info(&client->dev, "Detected a OV%x chip, revision %x\n",
+			priv->chip_id, priv->revision);
+
+	return 0;
+
+err:
+	kfree(priv);
+
+	return ret;
+}
+
+static int ov5650_remove(struct i2c_client *client)
+{
+	struct ov5650_priv *priv = i2c_get_clientdata(client);
+
+	kfree(priv);
+	return 0;
+}
+
+static const struct i2c_device_id ov5650_id[] = {
+	{ "ov5650", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ov5650_id);
+
+static struct i2c_driver ov5650_i2c_driver = {
+	.driver = {
+		.name = "ov5650",
+	},
+	.probe		= ov5650_probe,
+	.remove		= ov5650_remove,
+	.id_table	= ov5650_id,
+};
+
+static int __init ov5650_module_init(void)
+{
+	return i2c_add_driver(&ov5650_i2c_driver);
+}
+
+static void __exit ov5650_module_exit(void)
+{
+	i2c_del_driver(&ov5650_i2c_driver);
+}
+
+module_init(ov5650_module_init);
+module_exit(ov5650_module_exit);
+
+MODULE_DESCRIPTION("OmniVision OV5650 Camera driver");
+MODULE_AUTHOR("Bryan Wu <pengw@nvidia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/video/ov7670soc.c b/drivers/media/video/ov7670soc.c
new file mode 100644
index 000000000000..5e88d2ea3995
--- /dev/null
+++ b/drivers/media/video/ov7670soc.c
@@ -0,0 +1,746 @@
+/*
+ * drivers/media/video/ov7670soc.c
+ *
+ * OmniVision OV7670 cameras driver
+ *
+ * Copyright (c) 2011 Ming-Yao Chen <mychen0518@gmail.com>
+ * (based on tvp514x.c)
+ *
+ * Copyright (c) 2013 Ant Micro <www.antmicro.com>
+ *
+ * This package is free software; you can redistribute it and/or modify
+ * it under the terms 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+
+#include <media/soc_camera.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-ioctl.h>
+
+/* MODULE NAME*/
+#define OV7670SOC_MODULE_NAME "ov7670soc"
+
+/* Private macros for OV7670 */
+#define I2C_RETRY_COUNT                (5)
+#define VGA_WIDTH                        (640)
+#define VGA_HEIGHT                        (480)
+
+/* Registers */
+#define REG_GAIN        0x00        /* Gain lower 8 bits (rest in vref) */
+#define REG_BLUE        0x01        /* blue gain */
+#define REG_RED                0x02        /* red gain */
+#define REG_VREF        0x03        /* Pieces of GAIN, VSTART, VSTOP */
+#define REG_COM1        0x04        /* Control 1 */
+#define  COM1_CCIR656          0x40  /* CCIR656 enable */
+#define REG_BAVE        0x05        /* U/B Average level */
+#define REG_GbAVE        0x06        /* Y/Gb Average level */
+#define REG_AECHH        0x07        /* AEC MS 5 bits */
+#define REG_RAVE        0x08        /* V/R Average level */
+#define REG_COM2        0x09        /* Control 2 */
+#define  COM2_SSLEEP          0x10        /* Soft sleep mode */
+#define REG_PID                0x0a        /* Product ID MSB */
+#define REG_VER                0x0b        /* Product ID LSB */
+#define REG_COM3        0x0c        /* Control 3 */
+#define  COM3_SWAP          0x40          /* Byte swap */
+#define  COM3_SCALEEN          0x08          /* Enable scaling */
+#define  COM3_DCWEN          0x04          /* Enable downsamp/crop/window */
+#define REG_COM4        0x0d        /* Control 4 */
+#define REG_COM5        0x0e        /* All "reserved" */
+#define REG_COM6        0x0f        /* Control 6 */
+#define REG_AECH        0x10        /* More bits of AEC value */
+#define REG_CLKRC        0x11        /* Clocl control */
+#define   CLK_EXT          0x40          /* Use external clock directly */
+#define   CLK_SCALE          0x3f          /* Mask for internal clock scale */
+#define REG_COM7        0x12        /* Control 7 */
+#define   COM7_RESET          0x80          /* Register reset */
+#define   COM7_FMT_MASK          0x38
+#define   COM7_FMT_VGA          0x00
+#define          COM7_FMT_CIF          0x20          /* CIF format */
+#define   COM7_FMT_QVGA          0x10          /* QVGA format */
+#define   COM7_FMT_QCIF          0x08          /* QCIF format */
+#define          COM7_RGB          0x04          /* bits 0 and 2 - RGB format */
+#define          COM7_YUV          0x00          /* YUV */
+#define          COM7_BAYER          0x01          /* Bayer format */
+#define          COM7_PBAYER          0x05          /* "Processed bayer" */
+#define REG_COM8        0x13        /* Control 8 */
+#define   COM8_FASTAEC          0x80          /* Enable fast AGC/AEC */
+#define   COM8_AECSTEP          0x40          /* Unlimited AEC step size */
+#define   COM8_BFILT          0x20          /* Band filter enable */
+#define   COM8_AGC          0x04          /* Auto gain enable */
+#define   COM8_AWB          0x02          /* White balance enable */
+#define   COM8_AEC          0x01          /* Auto exposure enable */
+#define REG_COM9        0x14        /* Control 9  - gain ceiling */
+#define REG_COM10        0x15        /* Control 10 */
+#define   COM10_HSYNC          0x40          /* HSYNC instead of HREF */
+#define   COM10_PCLK_HB          0x20          /* Suppress PCLK on horiz blank */
+#define   COM10_HREF_REV  0x08          /* Reverse HREF */
+#define   COM10_VS_LEAD          0x04          /* VSYNC on clock leading edge */
+#define   COM10_VS_NEG          0x02          /* VSYNC negative */
+#define   COM10_HS_NEG          0x01          /* HSYNC negative */
+#define REG_HSTART        0x17        /* Horiz start high bits */
+#define REG_HSTOP        0x18        /* Horiz stop high bits */
+#define REG_VSTART        0x19        /* Vert start high bits */
+#define REG_VSTOP        0x1a        /* Vert stop high bits */
+#define REG_PSHFT        0x1b        /* Pixel delay after HREF */
+#define REG_MIDH        0x1c        /* Manuf. ID high */
+#define REG_MIDL        0x1d        /* Manuf. ID low */
+#define REG_MVFP        0x1e        /* Mirror / vflip */
+#define   MVFP_MIRROR          0x20          /* Mirror image */
+#define   MVFP_FLIP          0x10          /* Vertical flip */
+#define REG_AEW                0x24        /* AGC upper limit */
+#define REG_AEB                0x25        /* AGC lower limit */
+#define REG_VPT                0x26        /* AGC/AEC fast mode op region */
+#define REG_HSYST        0x30        /* HSYNC rising edge delay */
+#define REG_HSYEN        0x31        /* HSYNC falling edge delay */
+#define REG_HREF        0x32        /* HREF pieces */
+#define REG_TSLB        0x3a        /* lots of stuff */
+#define   TSLB_YLAST          0x08          /* UYVY or VYUY - see com13 */
+#define REG_COM11        0x3b        /* Control 11 */
+#define   COM11_NIGHT          0x80          /* NIght mode enable */
+#define   COM11_NMFR          0x60          /* Two bit NM frame rate */
+#define   COM11_HZAUTO          0x10          /* Auto detect 50/60 Hz */
+#define          COM11_50HZ          0x08          /* Manual 50Hz select */
+#define   COM11_EXP          0x02
+#define REG_COM12        0x3c        /* Control 12 */
+#define   COM12_HREF          0x80          /* HREF always */
+#define REG_COM13        0x3d        /* Control 13 */
+#define   COM13_GAMMA          0x80          /* Gamma enable */
+#define          COM13_UVSAT          0x40          /* UV saturation auto adjustment */
+#define   COM13_UVSWAP          0x01          /* V before U - w/TSLB */
+#define REG_COM14        0x3e        /* Control 14 */
+#define   COM14_DCWEN          0x10          /* DCW/PCLK-scale enable */
+#define REG_EDGE        0x3f        /* Edge enhancement factor */
+#define REG_COM15        0x40        /* Control 15 */
+#define   COM15_R10F0          0x00          /* Data range 10 to F0 */
+#define          COM15_R01FE          0x80          /*            01 to FE */
+#define   COM15_R00FF          0xc0          /*            00 to FF */
+#define   COM15_RGB565          0x10          /* RGB565 output */
+#define   COM15_RGB555          0x30          /* RGB555 output */
+#define REG_COM16        0x41        /* Control 16 */
+#define   COM16_AWBGAIN   0x08          /* AWB gain enable */
+#define REG_COM17        0x42        /* Control 17 */
+#define   COM17_AECWIN          0xc0          /* AEC window - must match COM4 */
+#define   COM17_CBAR          0x08          /* DSP Color bar */
+/*
+ * This matrix defines how the colors are generated, must be
+ * tweaked to adjust hue and saturation.
+ *
+ * Order: v-red, v-green, v-blue, u-red, u-green, u-blue
+ *
+ * They are nine-bit signed quantities, with the sign bit
+ * stored in 0x58.  Sign for v-red is bit 0, and up from there.
+ */
+#define        REG_CMATRIX_BASE        0x4f
+#define   CMATRIX_LEN 6
+#define REG_MTX1                        0x4f
+#define REG_MTX2                        0x50
+#define REG_MTX3                        0x51
+#define REG_MTX4                        0x52
+#define REG_MTX5                        0x53
+#define REG_MTX6                        0x54
+#define REG_BRIGHTNESS                0x55 /* Brightness */
+#define REG_CONTRAST                0x56 /* Contrast control */
+#define REG_CMATRIX_SIGN         0x58
+#define REG_GFIX                        0x69 /* Fix gain control */
+#define REG_DBLV                        0x6b /* PLL control an debugging */
+#define   DBLV_BYPASS        0x00         /* Bypass PLL */
+#define   DBLV_X4                  0x01         /* clock x4 */
+#define   DBLV_X6                  0x10         /* clock x6 */
+#define   DBLV_X8                  0x11         /* clock x8 */
+#define REG_SCALING_X           0x70
+#define REG_SCALING_Y           0x71
+#define REG_REG76                        0x76 /* OV's name */
+#define   R76_BLKPCOR        0x80         /* Black pixel correction enable */
+#define   R76_WHTPCOR        0x40         /* White pixel correction enable */
+#define REG_RGB444                        0x8c /* RGB 444 control */
+#define   R444_ENABLE        0x02         /* Turn on RGB444, overrides 5x5 */
+#define   R444_RGBX                  0x01         /* Empty nibble at end */
+#define REG_HAECC1                        0x9f /* Hist AEC/AGC control 1 */
+#define REG_HAECC2                        0xa0 /* Hist AEC/AGC control 2 */
+#define REG_BD50MAX                        0xa5 /* 50hz banding step limit */
+#define REG_HAECC3                        0xa6 /* Hist AEC/AGC control 3 */
+#define REG_HAECC4                        0xa7 /* Hist AEC/AGC control 4 */
+#define REG_HAECC5                        0xa8 /* Hist AEC/AGC control 5 */
+#define REG_HAECC6                        0xa9 /* Hist AEC/AGC control 6 */
+#define REG_HAECC7                        0xaa /* Hist AEC/AGC control 7 */
+#define REG_BD60MAX                        0xab /* 60hz banding step limit */
+
+/*
+ * Store information about the video data format.  The color matrix
+ * is deeply tied into the format, so keep the relevant values here.
+ * The magic matrix numbers come from OmniVision.
+ */
+static struct ov7670soc_format_struct {
+        enum v4l2_mbus_pixelcode mbus_code;
+        enum v4l2_colorspace colorspace;
+        struct regval_list *regs;
+        int cmatrix[CMATRIX_LEN];
+} ov7670soc_formats[] = {
+        {
+                /* TODO: registers are set for UYUV, but  we are present as YUYV, otherwise image is 
+                 * invalid 
+                 */
+                .mbus_code        = V4L2_MBUS_FMT_YVYU8_2X8,
+                .colorspace        = V4L2_COLORSPACE_JPEG,
+                /*TODO: list of regs with preset values for specified format
+                .regs                 = ov7670soc_fmt_yuv422,*/
+                .cmatrix        = { -64, -52, -12, -23, -41, 132 },
+        },
+};
+#define N_OV7670_FMTS ARRAY_SIZE(ov7670soc_formats)
+
+/**
+ * struct ov7670soc_decoder - OV7670 decoder object
+ * sd: Subdevice Slave handle
+ * TODO: ov7670soc_regs: copy of hw's regs with preset values.
+ * pdata: Board specific
+ * fmt_list: Format list
+ * num_fmts: Number of formats
+ * fmt: Current format
+ * brightness: only for queries
+ * contrast: only for queries
+ * saturation: only for queries
+ * hue: only for queries
+ */
+struct ov7670soc_decoder {
+        struct v4l2_subdev sd;
+        /*
+        TODO: list of def reg values for read queries and RMW operations.
+        Local cache due to impossibility of registers read
+        struct ov7670soc_reg ov7670soc_regs[ARRAY_SIZE(ov7670soc_reg_list_default)];*/
+        const struct ov7670soc_platform_data *pdata;
+        const struct ov7670soc_format_struct *fmt_list;
+        int num_fmts;
+        struct ov7670soc_format_struct *fmt;  /* Current format */
+        int brightness;
+        int contrast;
+        int saturation;
+        int hue;
+};
+
+static const struct v4l2_queryctrl ov7670soc_controls[] = {
+        {
+                .id                = V4L2_CID_BRIGHTNESS,
+                .type                = V4L2_CTRL_TYPE_INTEGER,
+                .name                = "brightness",
+                .minimum        = 0,
+                .maximum        = 255,
+                .step                = 1,
+                .default_value        = 128,
+        },
+        {
+                .id                = V4L2_CID_CONTRAST,
+                .type                = V4L2_CTRL_TYPE_INTEGER,
+                .name                = "contrast",
+                .minimum        = 0,
+                .maximum        = 127,
+                .step                = 1,
+                .default_value        = 64,
+        },
+        {
+                .id                = V4L2_CID_SATURATION,
+                .type                = V4L2_CTRL_TYPE_INTEGER,
+                .name                = "saturation",
+                .minimum        = 0,
+                .maximum        = 256,
+                .step                = 1,
+                .default_value        = 128,
+        },
+        {
+                .id                = V4L2_CID_HUE,
+                .type                = V4L2_CTRL_TYPE_INTEGER,
+                .name                = "hue",
+                .minimum        = -180,
+                .maximum        = 180,
+                .step                = 5,
+                .default_value        = 0,
+        },
+};
+
+static inline struct ov7670soc_decoder *to_decoder(struct v4l2_subdev *sd)
+{
+        return container_of(sd, struct ov7670soc_decoder, sd);
+}
+
+/* 
+ * Read register prohibited on Tegra T-20 due to
+ * arbitration lost after sending device read addres
+ */
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+static int ov7670soc_read_reg(struct i2c_client *client, unsigned char reg, unsigned char *val)
+{
+        int err, retry = 0;
+
+read_again:
+        err = i2c_smbus_read_byte_data(client, reg);
+        if (err < 0) {
+                if (retry <= I2C_RETRY_COUNT) {
+                        retry++;
+                        msleep_interruptible(10);
+                        goto read_again;
+                }
+                dev_err(&client->dev, "Failed to read register 0x%02X!\n", reg);
+        }
+        *val = (unsigned char)err;
+        return err;
+}
+#endif
+
+static int ov7670soc_write_reg(struct i2c_client *client, unsigned char reg, unsigned char val)
+{
+        int err, retry = 0;
+
+write_again:
+        err = i2c_smbus_write_byte_data(client, reg, val);
+        if (err) {
+                if (retry <= I2C_RETRY_COUNT) {
+                        retry++;
+                        msleep_interruptible(10);
+                        goto write_again;
+                }
+                dev_err(&client->dev, "Failed to write 0x%02X to register 0x%02X!\n", val, reg);
+        }
+
+        return err;
+}
+
+/* 
+ * Reset all camera registers to default values
+ */
+static int ov7670soc_reset(struct i2c_client *client)
+{
+        int ret;
+        
+        ret = ov7670soc_write_reg(client, REG_COM7, COM7_RESET);
+        if (ret)
+            dev_err(&client->dev, "An error occurred while entering soft reset!\n");
+
+        return ret;
+}
+
+/*
+ * ov7670soc_setup - initializes a list of OV7670 registers
+ */
+static int ov7670soc_setup(struct v4l2_subdev *sd, u32 val)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+    ov7670soc_reset(client);
+
+    /* Configure HSYNC & VSYNC */
+        ov7670soc_write_reg(client, REG_COM10, COM10_HSYNC | COM10_VS_NEG);
+        /* not sure what this does, but improves colors quality */
+        ov7670soc_write_reg(client, 0xb0, 0x84);
+
+        /* Config HSYNC offset */
+        ov7670soc_write_reg(client, REG_HSTART, 0x00);
+        ov7670soc_write_reg(client, REG_HSTOP, 0x00); 
+        ov7670soc_write_reg(client, REG_HSYST, 0x00);
+        ov7670soc_write_reg(client, REG_HSYEN, 0x05);
+        /* default MTX1..MTX5 and MTX6 = 0x84 makes better image */
+        ov7670soc_write_reg(client, REG_MTX6, 0x84);
+        
+    //TODO: output format, move to mbus_s_fmt
+    ov7670soc_write_reg(client, REG_COM7, COM7_YUV);  /* Selects YUV mode */
+    /* U before V */
+    ov7670soc_write_reg(client, REG_COM13, COM13_GAMMA | COM13_UVSAT);
+      
+        return 0;
+}
+
+/*
+ * Hue also requires messing with the color matrix.  It also requires
+ * trig functions, which tend not to be well supported in the kernel.
+ * So here is a simple table of sine values, 0-90 degrees, in steps
+ * of five degrees.  Values are multiplied by 1000.
+ *
+ * The following naive approximate trig functions require an argument
+ * carefully limited to -180 <= theta <= 180.
+ */
+#define SIN_STEP 5
+static const int ov7670soc_sin_table[] = {
+           0,         87,   173,   258,   342,   422,
+         499,        573,   642,   707,   766,   819,
+         866,        906,   939,   965,   984,   996,
+        1000
+};
+
+static int ov7670soc_sine(int theta)
+{
+        int chs = 1;
+        int sine;
+
+        if (theta < 0) {
+                theta = -theta;
+                chs = -1;
+        }
+        if (theta <= 90)
+                sine = ov7670soc_sin_table[theta/SIN_STEP];
+        else {
+                theta -= 90;
+                sine = 1000 - ov7670soc_sin_table[theta/SIN_STEP];
+        }
+        return sine*chs;
+}
+
+static int ov7670soc_cosine(int theta)
+{
+        theta = 90 - theta;
+        if (theta > 180)
+                theta -= 360;
+        else if (theta < -180)
+                theta += 360;
+        return ov7670soc_sine(theta);
+}
+
+static void ov7670soc_calc_cmatrix(struct ov7670soc_decoder *decoder, int matrix[CMATRIX_LEN], int sat, int hue)
+{
+        int i;
+        /*
+         * Apply the current saturation setting first.
+         */
+        for (i = 0; i < CMATRIX_LEN; i++)
+                matrix[i] = (decoder->fmt->cmatrix[i] * sat) >> 7;
+        /*
+         * Then, if need be, rotate the hue value.
+         */
+        if (hue != 0) {
+                int sinth, costh;
+                sinth = ov7670soc_sine(hue);
+                costh = ov7670soc_cosine(hue);
+
+                matrix[0] = (matrix[3]*sinth + matrix[0]*costh)/1000;
+                matrix[1] = (matrix[4]*sinth + matrix[1]*costh)/1000;
+                matrix[2] = (matrix[5]*sinth + matrix[2]*costh)/1000;
+                matrix[3] = (matrix[3]*costh - matrix[0]*sinth)/1000;
+                matrix[4] = (matrix[4]*costh - matrix[1]*sinth)/1000;
+                matrix[5] = (matrix[5]*costh - matrix[2]*sinth)/1000;
+        }
+}
+
+static int ov7670soc_store_cmatrix(struct v4l2_subdev *sd, int matrix[CMATRIX_LEN])
+{
+        int i, ret;
+        unsigned char signbits = 0;
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        /*
+         * Weird crap seems to exist in the upper part of
+         * the sign bits register, so let's preserve it.
+         */
+/*#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+        /* TODO: read forbidden on T-20 
+        ret = ov7670soc_read_reg(client, REG_CMATRIX_SIGN, &signbits);
+        signbits &= 0xc0;
+#endif*/
+        for (i = 0; i < CMATRIX_LEN; i++) {
+                unsigned char raw;
+
+                if (matrix[i] < 0) {
+                        signbits |= (1 << i);
+                        if (matrix[i] < -255)
+                                raw = 0xff;
+                        else
+                                raw = (-1 * matrix[i]) & 0xff;
+                }
+                else {
+                        if (matrix[i] > 255)
+                                raw = 0xff;
+                        else
+                                raw = matrix[i] & 0xff;
+                }
+                ret += ov7670soc_write_reg(client, REG_CMATRIX_BASE + i, raw);
+        }
+        ret += ov7670soc_write_reg(client, REG_CMATRIX_SIGN, signbits);
+        return ret;
+}
+
+static unsigned char ov7670soc_abs_to_sm(unsigned char v)
+{
+        if (v > 127)
+                return v & 0x7f;
+        return (128 - v) | 0x80;
+}
+
+static int ov7670soc_s_brightness(struct v4l2_subdev *sd, int value)
+{
+        unsigned char v;//, com8 = 0;
+        int ret;
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        /* TODO: read forbidden on T-20 */
+/*#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+        unsigned char com8 = 0;
+        ret = ov7670soc_read_reg(client, REG_COM8, &com8);
+        com8 &= ~COM8_AEC;
+#endif*/
+
+        ov7670soc_write_reg(client, REG_COM8, 0x8e); //defaul val is 0x8f, 0x8e->disable AEC
+        v = ov7670soc_abs_to_sm(value);
+        ret = ov7670soc_write_reg(client, REG_BRIGHTNESS, v);
+        return ret;
+}
+
+static int ov7670soc_s_contrast(struct v4l2_subdev *sd, int value)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+        return ov7670soc_write_reg(client, REG_CONTRAST, (unsigned char) value);
+}
+
+static int ov7670soc_s_sat_hue(struct v4l2_subdev *sd, int sat, int hue)
+{
+        struct ov7670soc_decoder *decoder = to_decoder(sd);
+        int matrix[CMATRIX_LEN];
+        int ret;
+
+        ov7670soc_calc_cmatrix(decoder, matrix, sat, hue);
+        ret = ov7670soc_store_cmatrix(sd, matrix);
+        return ret;
+}
+
+static int ov7670soc_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+        struct ov7670soc_decoder *decoder = to_decoder(sd);
+        
+        switch (ctrl->id) {
+        case V4L2_CID_BRIGHTNESS:
+                return ov7670soc_s_brightness(sd, ctrl->value);
+        case V4L2_CID_CONTRAST:
+                return ov7670soc_s_contrast(sd, ctrl->value);
+        case V4L2_CID_SATURATION:
+                return ov7670soc_s_sat_hue(sd, ctrl->value, decoder->hue);
+        case V4L2_CID_HUE:
+                return ov7670soc_s_sat_hue(sd, decoder->saturation, ctrl->value);
+        }
+        return -EINVAL;
+}
+
+static int ov7670soc_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+        struct ov7670soc_decoder *decoder = to_decoder(sd);
+
+        switch (ctrl->id) {
+        case V4L2_CID_BRIGHTNESS:
+                ctrl->value = decoder->brightness;
+                break;
+        case V4L2_CID_CONTRAST:
+                ctrl->value = decoder->contrast;
+                break;
+        case V4L2_CID_SATURATION:
+                ctrl->value = decoder->saturation;
+                break;
+        case V4L2_CID_HUE:
+                ctrl->value = decoder->hue;
+                break;
+        }
+        return 0;
+}
+
+/* Functions required by soc_camera_ops ***************************************/
+/* Alter bus settings on camera side */
+static int ov7670soc_set_bus_param(struct soc_camera_device *icd, unsigned long flags)
+{
+        return 0;
+}
+
+static unsigned long ov7670soc_query_bus_param(struct soc_camera_device *icd)
+{
+        struct soc_camera_link *icl = to_soc_camera_link(icd);
+                
+        unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
+                SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
+                SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8;
+
+        return soc_camera_apply_sensor_flags(icl, flags);
+}
+/******************************************************************************/
+
+/* Functions required by v4l2_subdev_video_ops ********************************/
+static int ov7670soc_s_stream(struct v4l2_subdev *sd, int enable)
+{
+        /* 
+        The OV7670 camera dose not have ability to start/stop streaming
+        */
+        return 0;
+}
+
+static int ov7670soc_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
+{
+        /*
+         * Set the image format. Currently we support only one format with
+         * fixed resolution, so we can set the format as it is on camera startup.
+         */
+        ov7670soc_setup(sd, 0);
+
+        return 0;
+}
+
+static int ov7670soc_try_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
+{
+        struct ov7670soc_decoder *decoder = to_decoder(sd);
+        int index;
+
+        /* Check if we support queried image format. */        
+        for (index = 0; index < N_OV7670_FMTS; index++)
+                if (ov7670soc_formats[index].mbus_code == mf->code)
+                        break;
+        /* If not, set the only one which we support */
+        if (index >= N_OV7670_FMTS) {
+                /* default to first format */
+                index = 0;
+                mf->code = ov7670soc_formats[0].mbus_code;
+        }
+
+        /* Store the current format */        
+        decoder->fmt = &ov7670soc_formats[index];
+        
+        /* Fixed value, move to ov7670_formats */
+        mf->field = V4L2_FIELD_NONE;
+        /* TODO: support for other resolutions (CIF/QCIF etc).*/
+    mf->width = VGA_WIDTH;
+        mf->height = VGA_HEIGHT;
+        mf->colorspace = decoder->fmt->colorspace;
+
+        return 0;
+}
+
+static int ov7670soc_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned int index, enum v4l2_mbus_pixelcode *code)
+{
+        if (index >= ARRAY_SIZE(ov7670soc_formats))
+                return -EINVAL;
+
+    *code = ov7670soc_formats[index].mbus_code;
+
+        return 0;
+}
+/******************************************************************************/
+
+static struct soc_camera_ops ov7670soc_soc_camera_ops = {
+        .set_bus_param                = ov7670soc_set_bus_param,
+        .query_bus_param        = ov7670soc_query_bus_param,
+        .controls                        = ov7670soc_controls,
+        .num_controls                = ARRAY_SIZE(ov7670soc_controls),
+};
+
+static const struct v4l2_subdev_core_ops ov7670soc_core_ops = {
+        .g_ctrl = ov7670soc_g_ctrl,
+        .s_ctrl = ov7670soc_s_ctrl,
+};
+
+static const struct v4l2_subdev_video_ops ov7670soc_video_ops = {
+        .s_stream = ov7670soc_s_stream,
+        .s_mbus_fmt = ov7670soc_s_mbus_fmt,
+        .try_mbus_fmt = ov7670soc_try_mbus_fmt,
+        .enum_mbus_fmt = ov7670soc_enum_mbus_fmt,
+};
+
+static const struct v4l2_subdev_ops ov7670soc_ops = {
+        .core = &ov7670soc_core_ops,
+        .video = &ov7670soc_video_ops,
+};
+
+static int ov7670soc_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+    struct ov7670soc_decoder *decoder;
+    struct soc_camera_device *icd = client->dev.platform_data;
+    struct soc_camera_link *icl;
+        struct v4l2_subdev *sd;
+
+        /* Check if the adapter supports the needed features */
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+                return -EIO;
+
+    icl = to_soc_camera_link(icd);
+        if (!icl) {
+                dev_err(&client->dev, "No platform data!!\n");
+                return -ENODEV;
+        }
+
+        decoder = kzalloc(sizeof(struct ov7670soc_decoder), GFP_KERNEL);
+        if (!decoder)
+        {
+            dev_err(&client->dev, "Failed to allocate memory for private data!\n");
+                return -ENOMEM;
+        }
+        sd = &decoder->sd;
+
+        /* Initialize the ov7670soc_decoder with default configuration */
+        decoder->fmt_list = ov7670soc_formats;
+        decoder->num_fmts = ARRAY_SIZE(ov7670soc_formats);
+        decoder->fmt = &ov7670soc_formats[0];
+        decoder->pdata = client->dev.platform_data;
+        decoder->brightness = 128;
+        decoder->contrast = 64;
+        decoder->saturation = 128;
+        decoder->hue = 0;
+        /* Register with V4L2 layer as slave device */
+        v4l2_i2c_subdev_init(sd, client, &ov7670soc_ops);
+
+        icd->ops = &ov7670soc_soc_camera_ops;
+
+        return 0;
+}
+
+static int ov7670soc_remove(struct i2c_client *client)
+{
+        struct v4l2_subdev *sd = i2c_get_clientdata(client);
+        struct ov7670soc_decoder *decoder = to_decoder(sd);
+
+        v4l2_device_unregister_subdev(sd);
+        kfree(decoder);
+
+        return 0;
+}
+
+static const struct i2c_device_id ov7670soc_id[] = 
+{
+        {OV7670SOC_MODULE_NAME, 0},
+        { }
+};
+MODULE_DEVICE_TABLE(i2c, ov7670soc_id);
+
+static struct i2c_driver ov7670soc_driver = {
+        .driver = {
+                .owner = THIS_MODULE,
+                .name = OV7670SOC_MODULE_NAME,
+        },
+        .probe = ov7670soc_probe,
+        .remove = ov7670soc_remove,
+        .id_table = ov7670soc_id,
+};
+
+static int __init ov7670soc_init(void)
+{
+        return i2c_add_driver(&ov7670soc_driver);
+}
+
+static void __exit ov7670soc_exit(void)
+{
+        i2c_del_driver(&ov7670soc_driver);
+}
+
+module_init(ov7670soc_init);
+module_exit(ov7670soc_exit);
+
+MODULE_AUTHOR("Antmicro Ltd.");
+MODULE_DESCRIPTION("OV7670 linux decoder driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/s5p-fimc/fimc-core.c b/drivers/media/video/s5p-fimc/fimc-core.c
index aa550666cc0b..b062b1a67687 100644
--- a/drivers/media/video/s5p-fimc/fimc-core.c
+++ b/drivers/media/video/s5p-fimc/fimc-core.c
@@ -35,7 +35,7 @@ static char *fimc_clocks[MAX_FIMC_CLOCKS] = {
 static struct fimc_fmt fimc_formats[] = {
 	{
 		.name		= "RGB565",
-		.fourcc		= V4L2_PIX_FMT_RGB565X,
+		.fourcc		= V4L2_PIX_FMT_RGB565,
 		.depth		= { 16 },
 		.color		= S5P_FIMC_RGB565,
 		.memplanes	= 1,
diff --git a/drivers/media/video/saa7164/saa7164-cards.c b/drivers/media/video/saa7164/saa7164-cards.c
index 69822a4e7275..c71369173fae 100644
--- a/drivers/media/video/saa7164/saa7164-cards.c
+++ b/drivers/media/video/saa7164/saa7164-cards.c
@@ -203,6 +203,66 @@ struct saa7164_board saa7164_boards[] = {
 			.i2c_reg_len	= REGLEN_8bit,
 		} },
 	},
+	[SAA7164_BOARD_HAUPPAUGE_HVR2200_4] = {
+		.name		= "Hauppauge WinTV-HVR2200",
+		.porta		= SAA7164_MPEG_DVB,
+		.portb		= SAA7164_MPEG_DVB,
+		.portc		= SAA7164_MPEG_ENCODER,
+		.portd		= SAA7164_MPEG_ENCODER,
+		.porte		= SAA7164_MPEG_VBI,
+		.portf		= SAA7164_MPEG_VBI,
+		.chiprev	= SAA7164_CHIP_REV3,
+		.unit		= {{
+			.id		= 0x1d,
+			.type		= SAA7164_UNIT_EEPROM,
+			.name		= "4K EEPROM",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_0,
+			.i2c_bus_addr	= 0xa0 >> 1,
+			.i2c_reg_len	= REGLEN_8bit,
+		}, {
+			.id		= 0x04,
+			.type		= SAA7164_UNIT_TUNER,
+			.name		= "TDA18271-1",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_1,
+			.i2c_bus_addr	= 0xc0 >> 1,
+			.i2c_reg_len	= REGLEN_8bit,
+		}, {
+			.id		= 0x05,
+			.type		= SAA7164_UNIT_ANALOG_DEMODULATOR,
+			.name		= "TDA8290-1",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_1,
+			.i2c_bus_addr	= 0x84 >> 1,
+			.i2c_reg_len	= REGLEN_8bit,
+		}, {
+			.id		= 0x1b,
+			.type		= SAA7164_UNIT_TUNER,
+			.name		= "TDA18271-2",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_2,
+			.i2c_bus_addr	= 0xc0 >> 1,
+			.i2c_reg_len	= REGLEN_8bit,
+		}, {
+			.id		= 0x1c,
+			.type		= SAA7164_UNIT_ANALOG_DEMODULATOR,
+			.name		= "TDA8290-2",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_2,
+			.i2c_bus_addr	= 0x84 >> 1,
+			.i2c_reg_len	= REGLEN_8bit,
+		}, {
+			.id		= 0x1e,
+			.type		= SAA7164_UNIT_DIGITAL_DEMODULATOR,
+			.name		= "TDA10048-1",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_1,
+			.i2c_bus_addr	= 0x10 >> 1,
+			.i2c_reg_len	= REGLEN_8bit,
+		}, {
+			.id		= 0x1f,
+			.type		= SAA7164_UNIT_DIGITAL_DEMODULATOR,
+			.name		= "TDA10048-2",
+			.i2c_bus_nr	= SAA7164_I2C_BUS_2,
+			.i2c_bus_addr	= 0x12 >> 1,
+			.i2c_reg_len	= REGLEN_8bit,
+		} },
+	},
 	[SAA7164_BOARD_HAUPPAUGE_HVR2250] = {
 		.name		= "Hauppauge WinTV-HVR2250",
 		.porta		= SAA7164_MPEG_DVB,
@@ -426,6 +486,10 @@ struct saa7164_subid saa7164_subids[] = {
 		.subvendor = 0x0070,
 		.subdevice = 0x8851,
 		.card      = SAA7164_BOARD_HAUPPAUGE_HVR2250_2,
+	}, {
+		.subvendor = 0x0070,
+		.subdevice = 0x8940,
+		.card      = SAA7164_BOARD_HAUPPAUGE_HVR2200_4,
 	},
 };
 const unsigned int saa7164_idcount = ARRAY_SIZE(saa7164_subids);
@@ -469,6 +533,7 @@ void saa7164_gpio_setup(struct saa7164_dev *dev)
 	case SAA7164_BOARD_HAUPPAUGE_HVR2200:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2200_2:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2200_3:
+	case SAA7164_BOARD_HAUPPAUGE_HVR2200_4:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2250:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2250_2:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2250_3:
@@ -549,6 +614,7 @@ void saa7164_card_setup(struct saa7164_dev *dev)
 	case SAA7164_BOARD_HAUPPAUGE_HVR2200:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2200_2:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2200_3:
+	case SAA7164_BOARD_HAUPPAUGE_HVR2200_4:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2250:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2250_2:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2250_3:
diff --git a/drivers/media/video/saa7164/saa7164-dvb.c b/drivers/media/video/saa7164/saa7164-dvb.c
index f65eab63ca87..d3779379197f 100644
--- a/drivers/media/video/saa7164/saa7164-dvb.c
+++ b/drivers/media/video/saa7164/saa7164-dvb.c
@@ -475,6 +475,7 @@ int saa7164_dvb_register(struct saa7164_port *port)
 	case SAA7164_BOARD_HAUPPAUGE_HVR2200:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2200_2:
 	case SAA7164_BOARD_HAUPPAUGE_HVR2200_3:
+	case SAA7164_BOARD_HAUPPAUGE_HVR2200_4:
 		i2c_bus = &dev->i2c_bus[port->nr + 1];
 		switch (port->nr) {
 		case 0:
diff --git a/drivers/media/video/saa7164/saa7164.h b/drivers/media/video/saa7164/saa7164.h
index 6678bf1e7816..35b64306ba95 100644
--- a/drivers/media/video/saa7164/saa7164.h
+++ b/drivers/media/video/saa7164/saa7164.h
@@ -82,6 +82,7 @@
 #define SAA7164_BOARD_HAUPPAUGE_HVR2200_3	6
 #define SAA7164_BOARD_HAUPPAUGE_HVR2250_2	7
 #define SAA7164_BOARD_HAUPPAUGE_HVR2250_3	8
+#define SAA7164_BOARD_HAUPPAUGE_HVR2200_4	9
 
 #define SAA7164_MAX_UNITS		8
 #define SAA7164_TS_NUMBER_OF_LINES	312
diff --git a/drivers/media/video/tegra/Kconfig b/drivers/media/video/tegra/Kconfig
new file mode 100644
index 000000000000..5099fe12c3e2
--- /dev/null
+++ b/drivers/media/video/tegra/Kconfig
@@ -0,0 +1,104 @@
+source "drivers/media/video/tegra/avp/Kconfig"
+source "drivers/media/video/tegra/mediaserver/Kconfig"
+source "drivers/media/video/tegra/nvavp/Kconfig"
+
+config TEGRA_CAMERA
+        bool "Enable support for tegra camera/isp hardware"
+        depends on ARCH_TEGRA
+        default y
+        help
+          Enables support for the Tegra camera interface
+
+          If unsure, say Y
+
+config TEGRA_DTV
+        bool "Enable support for tegra dtv interface"
+        depends on ARCH_TEGRA
+        default y
+        help
+          Enables support for the Tegra dtv interface
+
+          If unsure, say Y
+
+config VIDEO_OV5650
+        tristate "OV5650 camera sensor support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the Omnivision OV5650 5MP camera sensor
+          for use with the tegra isp.
+
+config VIDEO_OV5640
+        tristate "OV5640 camera sensor support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the Omnivision OV5640 5MP camera sensor
+          for use with the tegra isp.
+
+config VIDEO_OV14810
+        tristate "OV14810 camera sensor support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the Omnivision OV14810 14MP camera sensor
+          for use with the tegra isp.
+
+
+config VIDEO_OV9726
+        tristate "OV9726 camera sensor support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the Omnivision OV9726 camera sensor
+          for use with the tegra isp.
+
+config VIDEO_OV2710
+        tristate "OV2710 camera sensor support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the Omnivision OV2710 camera sensor
+          for use with the tegra isp.
+
+config VIDEO_AR0832
+        tristate "AR0832 camera sensor support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the AR0832 camera sensor
+          for use with the tegra isp.
+
+config VIDEO_SOC380
+        tristate "SOC380 camera sensor support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the Semco soc380 camera sensor
+          for use with the tegra isp.
+
+config TORCH_SSL3250A
+        tristate "SSL3250A flash/torch support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the SSL3250A flash/torch camera device
+
+config TORCH_TPS61050
+        tristate "TPS61050 flash/torch support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the TPS61050 flash/torch camera device
+
+config VIDEO_SH532U
+        tristate "SH532U focuser support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the SEMCO SH532U focuser
+          for use with the tegra isp.
+
+config VIDEO_AD5820
+        tristate "AD5820 focuser support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the AD5820 focuser
+          for use with the tegra isp.
+
+config VIDEO_AD5816
+        tristate "AD5816 focuser support"
+        depends on I2C && ARCH_TEGRA
+        ---help---
+          This is a driver for the AD5816 focuser
+          for use with the tegra isp.
diff --git a/drivers/media/video/tegra/Makefile b/drivers/media/video/tegra/Makefile
new file mode 100644
index 000000000000..7d7285e06fb7
--- /dev/null
+++ b/drivers/media/video/tegra/Makefile
@@ -0,0 +1,24 @@
+GCOV_PROFILE := y
+
+subdir-ccflags-y := -Werror
+#
+# Makefile for the video capture/playback device drivers.
+#
+obj-y				+= avp/
+obj-$(CONFIG_TEGRA_MEDIASERVER)	+= mediaserver/
+obj-$(CONFIG_TEGRA_NVAVP)	+= nvavp/
+obj-$(CONFIG_TEGRA_DTV)		+= tegra_dtv.o
+obj-$(CONFIG_TEGRA_CAMERA)	+= tegra_camera.o
+obj-$(CONFIG_VIDEO_AR0832)	+= ar0832_main.o
+obj-$(CONFIG_VIDEO_OV5650)	+= ov5650.o
+obj-$(CONFIG_VIDEO_OV5640)	+= ov5640.o
+obj-$(CONFIG_VIDEO_OV14810)	+= ov14810.o
+obj-$(CONFIG_VIDEO_OV9726)	+= ov9726.o
+obj-$(CONFIG_VIDEO_OV2710)	+= ov2710.o
+obj-$(CONFIG_VIDEO_SOC380)	+= soc380.o
+obj-$(CONFIG_TORCH_SSL3250A)	+= ssl3250a.o
+obj-$(CONFIG_TORCH_TPS61050)	+= tps61050.o
+obj-$(CONFIG_VIDEO_SH532U)	+= sh532u.o
+obj-$(CONFIG_VIDEO_AD5820)	+= ad5820.o
+obj-$(CONFIG_VIDEO_AD5816)	+= ad5816.o
+
diff --git a/drivers/media/video/tegra/ad5816.c b/drivers/media/video/tegra/ad5816.c
new file mode 100644
index 000000000000..6fdb9f184614
--- /dev/null
+++ b/drivers/media/video/tegra/ad5816.c
@@ -0,0 +1,1251 @@
+/* Copyright (C) 2011-2012 NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms 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
+ */
+/* This is a NVC kernel driver for a focuser device called
+ * ad5816.
+ */
+/* Implementation
+ * --------------
+ * The board level details about the device need to be provided in the board
+ * file with the <device>_platform_data structure.
+ * Standard among NVC kernel drivers in this structure is:
+ * .cfg = Use the NVC_CFG_ defines that are in nvc.h.
+ *  Descriptions of the configuration options are with the defines.
+ *      This value is typically 0.
+ * .num = The number of the instance of the device.  This should start at 1 and
+ *      and increment for each device on the board.  This number will be
+ *      appended to the MISC driver name, Example: /dev/focuser.1
+ *      If not used or 0, then nothing is appended to the name.
+ * .sync = If there is a need to synchronize two devices, then this value is
+ *       the number of the device instance (.num above) this device is to
+ *       sync to.  For example:
+ *       Device 1 platform entries =
+ *       .num = 1,
+ *       .sync = 2,
+ *       Device 2 platfrom entries =
+ *       .num = 2,
+ *       .sync = 1,
+ *       The above example sync's device 1 and 2.
+ *       To disable sync, set .sync = 0.  Note that the .num = 0 device is not
+ *       allowed to be synced to.
+ *       This is typically used for stereo applications.
+ * .dev_name = The MISC driver name the device registers as.  If not used,
+ *       then the part number of the device is used for the driver name.
+ *       If using the NVC user driver then use the name found in this
+ *       driver under _default_pdata.
+ * .gpio_count = The ARRAY_SIZE of the nvc_gpio_pdata table.
+ * .gpio = A pointer to the nvc_gpio_pdata structure's platform GPIO data.
+ *       The GPIO mechanism works by cross referencing the .gpio_type key
+ *       among the nvc_gpio_pdata GPIO data and the driver's nvc_gpio_init
+ *       GPIO data to build a GPIO table the driver can use.  The GPIO's
+ *       defined in the device header file's _gpio_type enum are the
+ *       gpio_type keys for the nvc_gpio_pdata and nvc_gpio_init structures.
+ *       These need to be present in the board file's nvc_gpio_pdata
+ *       structure for the GPIO's that are used.
+ *       The driver's GPIO logic uses assert/deassert throughout until the
+ *       low level _gpio_wr/rd calls where the .assert_high is used to
+ *       convert the value to the correct signal level.
+ *       See the GPIO notes in nvc.h for additional information.
+ *
+ * The following is specific to NVC kernel focus drivers:
+ * .nvc = Pointer to the nvc_focus_nvc structure.  This structure needs to
+ *      be defined and populated if overriding the driver defaults.
+ * .cap = Pointer to the nvc_focus_cap structure.  This structure needs to
+ *      be defined and populated if overriding the driver defaults.
+ *
+ * The following is specific to this NVC kernel focus driver:
+ * .info = Pointer to the ad5816_pdata_info structure.  This structure does
+ *       not need to be defined and populated unless overriding ROM data.
+ *
+ * Power Requirements:
+ * The device's header file defines the voltage regulators needed with the
+ * enumeration <device>_vreg.  The order these are enumerated is the order
+ * the regulators will be enabled when powering on the device.  When the
+ * device is powered off the regulators are disabled in descending order.
+ * The <device>_vregs table in this driver uses the nvc_regulator_init
+ * structure to define the regulator ID strings that go with the regulators
+ * defined with <device>_vreg.  These regulator ID strings (or supply names)
+ * will be used in the regulator_get function in the _vreg_init function.
+ * The board power file and <device>_vregs regulator ID strings must match.
+ */
+
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+#include <linux/list.h>
+#include <linux/regulator/consumer.h>
+#include <linux/gpio.h>
+#include <media/ad5816.h>
+
+#define AD5816_ID				0x04
+#define AD5816_FOCAL_LENGTH	 (4.570f)
+#define AD5816_FNUMBER			(2.8f)
+#define AD5816_SLEW_RATE		1
+#define AD5816_ACTUATOR_RANGE	1023
+#define AD5816_SETTLETIME		50
+#define AD5816_FOCUS_MACRO		810
+#define AD5816_FOCUS_INFINITY	50 /* Exact value needs to be decided */
+#define AD5816_POS_LOW_DEFAULT	0
+#define AD5816_POS_HIGH_DEFAULT	1023
+#define AD5816_POS_CLAMP		0x03ff
+/* Need to decide exact value of VCM_THRESHOLD and its use */
+/* define AD5816_VCM_THRESHOLD	20 */
+
+static u8 ad5816_ids[] = {
+	0x04,
+};
+
+static struct nvc_gpio_init ad5816_gpios[] = {
+	{ AD5816_GPIO_RESET, GPIOF_OUT_INIT_LOW, "reset", false, true, },
+	{ AD5816_GPIO_I2CMUX, 0, "i2c_mux", 0, false},
+	{ AD5816_GPIO_GP1, 0, "gp1", 0, false},
+	{ AD5816_GPIO_GP2, 0, "gp2", 0, false},
+	{ AD5816_GPIO_GP3, 0, "gp3", 0, false},
+};
+
+static struct nvc_regulator_init ad5816_vregs[] = {
+	{ AD5816_VREG_VDD, "vdd"},
+	{ AD5816_VREG_VDD_AF, "vdd_af"},
+	{ AD5816_VREG_VDD_I2C, "vdd_i2c"},
+};
+
+struct ad5816_info {
+	atomic_t in_use;
+	struct i2c_client *i2c_client;
+	struct ad5816_platform_data *pdata;
+	struct miscdevice miscdev;
+	struct list_head list;
+	struct nvc_gpio gpio[ARRAY_SIZE(ad5816_gpios)];
+	struct nvc_regulator vreg[ARRAY_SIZE(ad5816_vregs)];
+	int pwr_api;
+	int pwr_dev;
+	int id_minor;
+	u32 pos;
+	u8 s_mode;
+	bool reset_flag;
+	struct ad5816_info *s_info;
+	struct nvc_focus_nvc nvc;
+	struct nvc_focus_cap cap;
+	struct nv_focuser_config nv_config;
+	struct ad5816_pdata_info config;
+};
+
+/**
+ * The following are default values
+ */
+
+static struct ad5816_pdata_info ad5816_default_info = {
+	.pos_low = AD5816_POS_LOW_DEFAULT,
+	.pos_high = AD5816_POS_HIGH_DEFAULT,
+};
+
+static struct nvc_focus_cap ad5816_default_cap = {
+	.version = NVC_FOCUS_CAP_VER2,
+	.slew_rate = AD5816_SLEW_RATE,
+	.actuator_range = AD5816_ACTUATOR_RANGE,
+	.settle_time = AD5816_SETTLETIME,
+	.focus_macro = AD5816_FOCUS_MACRO,
+	.focus_infinity = AD5816_FOCUS_INFINITY,
+	.focus_hyper = AD5816_FOCUS_INFINITY,
+};
+
+static struct nvc_focus_nvc ad5816_default_nvc = {
+	.focal_length = AD5816_FOCAL_LENGTH,
+	.fnumber = AD5816_FNUMBER,
+};
+
+static struct ad5816_platform_data ad5816_default_pdata = {
+	.cfg = 0,
+	.num = 0,
+	.sync = 0,
+	.dev_name = "focuser",
+};
+static LIST_HEAD(ad5816_info_list);
+static DEFINE_SPINLOCK(ad5816_spinlock);
+
+static int ad5816_i2c_rd8(struct ad5816_info *info, u8 addr, u8 reg, u8 *val)
+{
+	struct i2c_msg msg[2];
+	u8 buf[2];
+	buf[0] = reg;
+	if (addr) {
+		msg[0].addr = addr;
+		msg[1].addr = addr;
+	} else {
+		msg[0].addr = info->i2c_client->addr;
+		msg[1].addr = info->i2c_client->addr;
+	}
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &buf[0];
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = &buf[1];
+	*val = 0;
+	if (i2c_transfer(info->i2c_client->adapter, msg, 2) != 2)
+		return -EIO;
+	*val = buf[1];
+	return 0;
+}
+
+static int ad5816_i2c_wr8(struct ad5816_info *info, u8 reg, u8 val)
+{
+	struct i2c_msg msg;
+	u8 buf[2];
+	buf[0] = reg;
+	buf[1] = val;
+	msg.addr = info->i2c_client->addr;
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = &buf[0];
+	if (i2c_transfer(info->i2c_client->adapter, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+static int ad5816_i2c_rd16(struct ad5816_info *info, u8 reg, u16 *val)
+{
+	struct i2c_msg msg[2];
+	u8 buf[3];
+	buf[0] = reg;
+	msg[0].addr = info->i2c_client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &buf[0];
+	msg[1].addr = info->i2c_client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 2;
+	msg[1].buf = &buf[1];
+	if (i2c_transfer(info->i2c_client->adapter, msg, 2) != 2)
+		return -EIO;
+	*val = (((u16)buf[1] << 8) | (u16)buf[2]);
+	return 0;
+}
+
+static int ad5816_i2c_wr16(struct ad5816_info *info, u8 reg, u16 val)
+{
+	struct i2c_msg msg;
+	u8 buf[3];
+	buf[0] = reg;
+	buf[1] = (u8)(val >> 8);
+	buf[2] = (u8)(val & 0xff);
+	msg.addr = info->i2c_client->addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = &buf[0];
+	if (i2c_transfer(info->i2c_client->adapter, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+static int ad5816_gpio_wr(struct ad5816_info *info, ad5816_gpio_types i,
+				int val) /* val: 0=deassert, 1=assert */
+{
+	int err = -EINVAL;
+	if (info->gpio[i].valid) {
+		val = !!val;
+		if (!info->gpio[i].active_high)
+			val = !val;
+		err = val;
+		gpio_set_value_cansleep(info->gpio[i].gpio, val);
+		dev_dbg(&info->i2c_client->dev, "%s %u %d\n", __func__, info->gpio[i].gpio, val);
+	}
+	return err; /* return value written or error */
+}
+
+static int ad5816_gpio_reset(struct ad5816_info *info, int val)
+{
+	int err = 0;
+
+	if (val) {
+		if(!info->reset_flag) {
+			info->reset_flag = true;
+			err = ad5816_gpio_wr(info, AD5816_GPIO_RESET, 1);
+			if (err < 0)
+				return 0; /* flag no reset */
+
+			mdelay(1);
+			ad5816_gpio_wr(info, AD5816_GPIO_RESET, 0);
+			mdelay(10); /* startup delay needs to be modified*/
+			err = 1; /* flag that a reset was done */
+		}
+	} else {
+		info->reset_flag = false;
+	}
+	return err;
+}
+
+static void ad5816_gpio_able(struct ad5816_info *info, int val)
+{
+	/**
+	* This is a feature that allows driver to control GPIOs
+	* that may be needed for the board (not the device).
+	* */
+	if (val) {
+		ad5816_gpio_wr(info, AD5816_GPIO_GP1, val);
+		ad5816_gpio_wr(info, AD5816_GPIO_GP2, val);
+		ad5816_gpio_wr(info, AD5816_GPIO_GP3, val);
+	} else {
+		ad5816_gpio_wr(info, AD5816_GPIO_GP3, val);
+		ad5816_gpio_wr(info, AD5816_GPIO_GP2, val);
+		ad5816_gpio_wr(info, AD5816_GPIO_GP1, val);
+	}
+}
+static void ad5816_gpio_exit(struct ad5816_info *info)
+{
+	unsigned i;
+	for (i = 0; i <= ARRAY_SIZE(ad5816_gpios); i++) {
+		if (info->gpio[i].flag && info->gpio[i].own) {
+			gpio_free(info->gpio[i].gpio);
+			info->gpio[i].own = false;
+		}
+	}
+}
+
+static void ad5816_gpio_init(struct ad5816_info *info)
+{
+	char label[32];
+	unsigned long flags;
+	unsigned type;
+	unsigned i;
+	unsigned j;
+	int err;
+	for (i = 0; i < ARRAY_SIZE(ad5816_gpios); i++)
+		info->gpio[i].flag = false;
+
+	if (!info->pdata->gpio_count || !info->pdata->gpio)
+		return;
+
+	for (i = 0; i < ARRAY_SIZE(ad5816_gpios); i++) {
+		type = ad5816_gpios[i].gpio_type;
+
+		for (j = 0; j < info->pdata->gpio_count; j++) {
+			if (type == info->pdata->gpio[j].gpio_type)
+				break;
+		}
+
+		if (j == info->pdata->gpio_count)
+			continue;
+		info->gpio[type].gpio = info->pdata->gpio[j].gpio;
+		info->gpio[type].flag = true;
+
+		if (ad5816_gpios[i].use_flags) {
+			flags = ad5816_gpios[i].flags;
+			info->gpio[type].active_high = ad5816_gpios[i].active_high;
+		} else {
+			info->gpio[type].active_high = info->pdata->gpio[j].active_high;
+			if (info->gpio[type].active_high)
+				flags = GPIOF_OUT_INIT_LOW;
+			else
+				flags = GPIOF_OUT_INIT_HIGH;
+		}
+
+		if (!info->pdata->gpio[j].init_en)
+			continue;
+		snprintf(label, sizeof(label), "ad5816_%u_%s",
+				info->pdata->num, ad5816_gpios[i].label);
+		err = gpio_request_one(info->gpio[type].gpio, flags, label);
+		if (err) {
+			dev_err(&info->i2c_client->dev, "%s ERR %s %u\n",
+					__func__, label, info->gpio[type].gpio);
+		} else {
+			info->gpio[type].own = true;
+			dev_dbg(&info->i2c_client->dev, "%s %s %u\n",
+					__func__, label, info->gpio[type].gpio);
+		}
+	}
+}
+
+static int ad5816_vreg_dis(struct ad5816_info *info, ad5816_vreg i)
+{
+	int err = 0;
+	if (info->vreg[i].vreg_flag && (info->vreg[i].vreg != NULL)) {
+		err = regulator_disable(info->vreg[i].vreg);
+		if (!err)
+			dev_dbg(&info->i2c_client->dev, "%s: %s\n",
+					__func__, info->vreg[i].vreg_name);
+		else
+			dev_err(&info->i2c_client->dev, "%s %s ERR\n",
+					__func__, info->vreg[i].vreg_name);
+	}
+	info->vreg[i].vreg_flag = false;
+	return err;
+}
+
+static int ad5816_vreg_dis_all(struct ad5816_info *info)
+{
+	unsigned i;
+	int err = 0;
+	for (i = ARRAY_SIZE(ad5816_vregs); i > 0; i--)
+		err |= ad5816_vreg_dis(info, (i - 1));
+	return err;
+}
+
+static int ad5816_vreg_en(struct ad5816_info *info, ad5816_vreg i)
+{
+	int err = 0;
+	if (!info->vreg[i].vreg_flag && (info->vreg[i].vreg != NULL)) {
+		err = regulator_enable(info->vreg[i].vreg);
+
+		if (!err) {
+			dev_dbg(&info->i2c_client->dev, "%s: %s\n",
+					__func__, info->vreg[i].vreg_name);
+			info->vreg[i].vreg_flag = true;
+			err = 1; /* flag regulator state change */
+		} else {
+			dev_err(&info->i2c_client->dev, "%s %s ERR\n",
+					__func__, info->vreg[i].vreg_name);
+		}
+
+	}
+	return err;
+}
+
+static int ad5816_vreg_en_all(struct ad5816_info *info)
+{
+	unsigned i;
+	int err = 0;
+	for (i = 0; i < ARRAY_SIZE(ad5816_vregs); i++)
+		err |= ad5816_vreg_en(info, i);
+	return err;
+}
+
+static void ad5816_vreg_exit(struct ad5816_info *info)
+{
+	unsigned i;
+	for (i = 0; i < ARRAY_SIZE(ad5816_vregs); i++) {
+		regulator_put(info->vreg[i].vreg);
+		info->vreg[i].vreg = NULL;
+	}
+}
+
+static int ad5816_vreg_init(struct ad5816_info *info)
+{
+	unsigned i;
+	unsigned j;
+	int err = 0;
+	for (i = 0; i < ARRAY_SIZE(ad5816_vregs); i++) {
+		j = ad5816_vregs[i].vreg_num;
+		info->vreg[j].vreg_name = ad5816_vregs[i].vreg_name;
+		info->vreg[j].vreg_flag = false;
+		info->vreg[j].vreg = regulator_get(&info->i2c_client->dev,
+								info->vreg[j].vreg_name);
+		if (IS_ERR_OR_NULL(info->vreg[j].vreg)) {
+			dev_err(&info->i2c_client->dev, "%s %s ERR: %d\n",
+					__func__, info->vreg[j].vreg_name,
+					(int)info->vreg[j].vreg);
+			err |= PTR_ERR(info->vreg[j].vreg);
+			info->vreg[j].vreg = NULL;
+		} else {
+			dev_dbg(&info->i2c_client->dev, "%s: %s\n",
+					__func__, info->vreg[j].vreg_name);
+		}
+	}
+	return err;
+}
+
+void ad5816_set_power_down(struct ad5816_info *info)
+{
+	int err;
+	err = ad5816_i2c_wr16(info, VCM_CODE_MSB, 0x0000);
+	if (err)
+		dev_err(&info->i2c_client->dev, " %s: failed \n",
+			__func__);
+}
+
+void ad5816_set_arc_mode(struct ad5816_info *info)
+{
+	int err;
+	/* set ARC enable */
+	err = ad5816_i2c_wr8(info, CONTROL, 0x02);
+	if (err)
+		dev_err(&info->i2c_client->dev,
+		"%s: CONTROL reg write failed \n", __func__);
+
+	/* set the ARC RES2 */
+	err = ad5816_i2c_wr8(info, MODE, 0x01);
+	if (err)
+		dev_err(&info->i2c_client->dev,
+		"%s: MODE reg write failed \n", __func__);
+
+	/* set the VCM_FREQ to 12.8mS */
+	err = ad5816_i2c_wr8(info, VCM_FREQ, 0x80);
+	if (err)
+		dev_err(&info->i2c_client->dev,
+		"%s: VCM_FREQ reg write failed \n", __func__);
+}
+
+static int ad5816_pm_wr(struct ad5816_info *info, int pwr)
+{
+	int err = 0;
+
+	if ((info->pdata->cfg & (NVC_CFG_OFF2STDBY | NVC_CFG_BOOT_INIT)) &&
+		(pwr == NVC_PWR_OFF ||
+		pwr == NVC_PWR_STDBY_OFF))
+			pwr = NVC_PWR_STDBY;
+
+	if (pwr == info->pwr_dev)
+		return 0;
+
+	switch (pwr) {
+	case NVC_PWR_OFF_FORCE:
+	case NVC_PWR_OFF:
+		err = ad5816_vreg_dis_all(info);
+		ad5816_gpio_able(info, 0);
+		ad5816_gpio_reset(info, 0);
+		break;
+	case NVC_PWR_STDBY_OFF:
+	case NVC_PWR_STDBY:
+		err = ad5816_vreg_en_all(info);
+		ad5816_gpio_able(info, 1);
+		ad5816_gpio_reset(info, 1);
+		break;
+	case NVC_PWR_COMM:
+	case NVC_PWR_ON:
+		err = ad5816_vreg_en_all(info);
+		ad5816_gpio_able(info, 1);
+		ad5816_gpio_reset(info, 1);
+		break;
+	default:
+		err = -EINVAL;
+		break;
+	}
+
+	if (err < 0) {
+		dev_err(&info->i2c_client->dev, "%s err %d\n", __func__, err);
+		pwr = NVC_PWR_ERR;
+	}
+
+	info->pwr_dev = pwr;
+	dev_dbg(&info->i2c_client->dev, "%s pwr_dev=%d\n", __func__, info->pwr_dev);
+
+	if (err > 0)
+		return 0;
+
+	return err;
+}
+static int ad5816_pm_wr_s(struct ad5816_info *info, int pwr)
+{
+	int err1 = 0;
+	int err2 = 0;
+	if ((info->s_mode == NVC_SYNC_OFF) ||
+		(info->s_mode == NVC_SYNC_MASTER) ||
+		(info->s_mode == NVC_SYNC_STEREO))
+		err1 = ad5816_pm_wr(info, pwr);
+	if ((info->s_mode == NVC_SYNC_SLAVE) ||
+		(info->s_mode == NVC_SYNC_STEREO))
+		err2 = ad5816_pm_wr(info->s_info, pwr);
+	return err1 | err2;
+}
+
+static int ad5816_pm_api_wr(struct ad5816_info *info, int pwr)
+{
+	int err = 0;
+	if (!pwr || (pwr > NVC_PWR_ON))
+		return 0;
+	if (pwr > info->pwr_dev) {
+		err = ad5816_pm_wr_s(info, pwr);
+	}
+	if (!err) {
+		info->pwr_api = pwr;
+	} else {
+		info->pwr_api = NVC_PWR_ERR;
+	}
+	if (info->pdata->cfg & NVC_CFG_NOERR)
+		return 0;
+	return err;
+}
+
+static int ad5816_pm_dev_wr(struct ad5816_info *info, int pwr)
+{
+	if (pwr < info->pwr_api)
+		pwr = info->pwr_api;
+	return ad5816_pm_wr(info, pwr);
+}
+
+static void ad5816_pm_exit(struct ad5816_info *info)
+{
+	ad5816_pm_wr(info, NVC_PWR_OFF_FORCE);
+	ad5816_vreg_exit(info);
+	ad5816_gpio_exit(info);
+}
+static void ad5816_pm_init(struct ad5816_info *info)
+{
+	ad5816_gpio_init(info);
+	ad5816_vreg_init(info);
+}
+
+static int ad5816_reset(struct ad5816_info *info, u32 level)
+{
+	int err;
+	if (level == NVC_RESET_SOFT) {
+		err = ad5816_pm_wr(info, NVC_PWR_COMM);
+		err |= ad5816_i2c_wr8(info, CONTROL, 0x01); /* SW reset */
+	} else {
+		err = ad5816_pm_wr(info, NVC_PWR_OFF_FORCE);
+	}
+	err |= ad5816_pm_wr(info, info->pwr_api);
+	return err;
+}
+
+static int ad5816_dev_id(struct ad5816_info *info)
+{
+	u16 val = 0;
+	unsigned i;
+	int err;
+	ad5816_pm_dev_wr(info, NVC_PWR_COMM);
+	err = ad5816_i2c_rd16(info, IC_INFO, &val);
+	if (!err) {
+		dev_dbg(&info->i2c_client->dev, "%s found devId: %x\n", __func__, val);
+		info->id_minor = 0;
+		val = val & 0xff;
+		for (i = 0; i < ARRAY_SIZE(ad5816_ids); i++) {
+			if (val == ad5816_ids[i]) {
+				info->id_minor = val;
+				break;
+			}
+		}
+		if (!info->id_minor) {
+			err = -ENODEV;
+			dev_dbg(&info->i2c_client->dev, "%s No devId match\n", __func__);
+		}
+	}
+	ad5816_pm_dev_wr(info, NVC_PWR_OFF);
+	return err;
+}
+
+/**
+ * Below are device specific functions.
+ */
+
+static int ad5816_position_rd(struct ad5816_info *info, unsigned *position)
+{
+
+	u16 pos = 0;
+	u8 t1 = 0;
+	int err = 0;
+
+	err = ad5816_i2c_rd8(info, 0, VCM_CODE_MSB, &t1);
+	pos = t1;
+	err  = ad5816_i2c_rd8(info, 0, VCM_CODE_LSB, &t1);
+	pos = (pos << 8) | t1;
+
+	if (pos < info->config.pos_low)
+		pos = info->config.pos_low;
+	else if (pos > info->config.pos_high)
+		pos = info->config.pos_high;
+
+	*position = pos;
+
+	return err;
+}
+
+static int ad5816_position_wr(struct ad5816_info *info, s32 position)
+{
+	s16 data;
+
+	ad5816_set_arc_mode(info);
+
+	if (position > info->config.pos_high)
+		return -EINVAL;
+
+	data = position & AD5816_POS_CLAMP;
+	return ad5816_i2c_wr16(info, VCM_CODE_MSB, data);
+
+}
+
+static void ad5816_get_focuser_capabilities(struct ad5816_info *info)
+{
+	memset(&info->nv_config, 0, sizeof(info->nv_config));
+
+	info->nv_config.focal_length = info->nvc.focal_length;
+	info->nv_config.fnumber = info->nvc.fnumber;
+	info->nv_config.max_aperture = info->nvc.fnumber;
+	info->nv_config.range_ends_reversed = 0;
+	info->nv_config.settle_time = info->cap.settle_time;
+
+	info->nv_config.pos_working_low = AF_POS_INVALID_VALUE;
+	info->nv_config.pos_working_high = AF_POS_INVALID_VALUE;
+
+	info->nv_config.pos_actual_low = info->config.pos_low;
+	info->nv_config.pos_actual_high = info->config.pos_high;
+
+	info->nv_config.slew_rate = info->cap.slew_rate;
+	info->nv_config.circle_of_confusion = -1;
+	info->nv_config.num_focuser_sets = 1;
+	info->nv_config.focuser_set[0].macro = info->cap.focus_macro;
+	info->nv_config.focuser_set[0].hyper = info->cap.focus_hyper;
+	info->nv_config.focuser_set[0].inf = info->cap.focus_infinity;
+	info->nv_config.focuser_set[0].settle_time = info->cap.settle_time;
+}
+
+static int ad5816_set_focuser_capabilities(struct ad5816_info *info,
+					struct nvc_param *params)
+{
+	if (copy_from_user(&info->nv_config, (const void __user *)params->p_value,
+		sizeof(struct nv_focuser_config))) {
+			dev_err(&info->i2c_client->dev, "%s Error: copy_from_user bytes %d\n",
+			__func__, sizeof(struct nv_focuser_config));
+			return -EFAULT;
+	}
+
+	/* set pre-set value, as currently ODM sets incorrect value */
+	info->cap.settle_time = AD5816_SETTLETIME;
+
+	dev_dbg(&info->i2c_client->dev, "%s: copy_from_user bytes %d info->cap.settle_time %d\n",
+		__func__,  sizeof(struct nv_focuser_config), info->cap.settle_time);
+
+	return 0;
+}
+
+static int ad5816_param_rd(struct ad5816_info *info, unsigned long arg)
+{
+	struct nvc_param params;
+	const void *data_ptr = NULL;
+	u32 data_size = 0;
+	u32 position;
+	int err;
+	if (copy_from_user(&params,
+		(const void __user *)arg,
+		sizeof(struct nvc_param))) {
+		dev_err(&info->i2c_client->dev, "%s %d copy_from_user err\n", __func__, __LINE__);
+		return -EFAULT;
+	}
+	if (info->s_mode == NVC_SYNC_SLAVE)
+		info = info->s_info;
+	switch (params.param) {
+	case NVC_PARAM_LOCUS:
+		ad5816_pm_dev_wr(info, NVC_PWR_COMM);
+		err = ad5816_position_rd(info, &position);
+		if (err && !(info->pdata->cfg & NVC_CFG_NOERR))
+			return err;
+		data_ptr = &position;
+		data_size = sizeof(position);
+		ad5816_pm_dev_wr(info, NVC_PWR_STDBY);
+		dev_dbg(&info->i2c_client->dev, "%s LOCUS: %d\n",
+			__func__, position);
+		break;
+	case NVC_PARAM_FOCAL_LEN:
+		info->nvc.focal_length = AD5816_FOCAL_LENGTH;
+		data_ptr = &info->nvc.focal_length;
+		data_size = sizeof(info->nvc.focal_length);
+		break;
+	case NVC_PARAM_MAX_APERTURE:
+		data_ptr = &info->nvc.max_aperature;
+		data_size = sizeof(info->nvc.max_aperature);
+		dev_dbg(&info->i2c_client->dev, "%s MAX_APERTURE: %x\n",
+				__func__, info->nvc.max_aperature);
+		break;
+	case NVC_PARAM_FNUMBER:
+		data_ptr = &info->nvc.fnumber;
+		data_size = sizeof(info->nvc.fnumber);
+		dev_dbg(&info->i2c_client->dev, "%s FNUMBER: %u\n",
+				__func__, info->nvc.fnumber);
+		break;
+	case NVC_PARAM_CAPS:
+		/* send back just what's requested or our max size */
+		ad5816_get_focuser_capabilities(info);
+		data_ptr = &info->nv_config;
+		data_size = sizeof(info->nv_config);
+		dev_err(&info->i2c_client->dev, "%s CAPS\n", __func__);
+		break;
+	case NVC_PARAM_STS:
+		/*data_ptr = &info->sts;
+		data_size = sizeof(info->sts);*/
+		dev_dbg(&info->i2c_client->dev, "%s \n", __func__);
+		break;
+	case NVC_PARAM_STEREO:
+		data_ptr = &info->s_mode;
+		data_size = sizeof(info->s_mode);
+		dev_err(&info->i2c_client->dev, "%s STEREO: %d\n", __func__, info->s_mode);
+		break;
+	default:
+		dev_err(&info->i2c_client->dev, "%s unsupported parameter: %d\n",
+			__func__, params.param);
+		return -EINVAL;
+	}
+	if (params.sizeofvalue < data_size) {
+		dev_err(&info->i2c_client->dev,
+			"%s data size mismatch %d != %d Param: %d\n",
+		__func__, params.sizeofvalue, data_size, params.param);
+		return -EINVAL;
+	}
+	if (copy_to_user((void __user *)params.p_value, data_ptr, data_size)) {
+		dev_err(&info->i2c_client->dev, "%s copy_to_user err line %d\n",
+			__func__, __LINE__);
+		return -EFAULT;
+	}
+	return 0;
+}
+
+static int ad5816_param_wr_s(struct ad5816_info *info,
+		struct nvc_param *params, s32 s32val)
+{
+	int err = 0;
+	switch (params->param) {
+	case NVC_PARAM_LOCUS:
+		dev_dbg(&info->i2c_client->dev, "%s LOCUS: %d\n", __func__, s32val);
+		err = ad5816_position_wr(info, s32val);
+		return err;
+	case NVC_PARAM_RESET:
+		err = ad5816_reset(info, s32val);
+		dev_dbg(&info->i2c_client->dev, "%s RESET: %d\n", __func__, err);
+		return err;
+	case NVC_PARAM_SELF_TEST:
+		err = 0;
+		dev_dbg(&info->i2c_client->dev, "%s SELF_TEST: %d\n", __func__, err);
+		return err;
+	default:
+		dev_dbg(&info->i2c_client->dev,
+			"%s unsupported parameter: %d\n",
+			__func__, params->param);
+		return -EINVAL;
+	}
+}
+
+static int ad5816_param_wr(struct ad5816_info *info, unsigned long arg)
+{
+	struct nvc_param params;
+	u8 u8val;
+	s32 s32val;
+	int err = 0;
+	if (copy_from_user(&params, (const void __user *)arg,
+		sizeof(struct nvc_param))) {
+		dev_err(&info->i2c_client->dev, "%s copy_from_user err line %d\n",
+				__func__, __LINE__);
+		return -EFAULT;
+	}
+	if (copy_from_user(&s32val, (const void __user *)params.p_value, sizeof(s32val))) {
+		dev_err(&info->i2c_client->dev, "%s %d copy_from_user err\n", __func__, __LINE__);
+		return -EFAULT;
+	}
+	u8val = (u8)s32val;
+	/* parameters independent of sync mode */
+	switch (params.param) {
+	case NVC_PARAM_STEREO:
+		dev_dbg(&info->i2c_client->dev, "%s STEREO: %d\n", __func__, u8val);
+		if (u8val == info->s_mode)
+			return 0;
+		switch (u8val) {
+		case NVC_SYNC_OFF:
+			info->s_mode = u8val;
+			ad5816_gpio_wr(info, AD5816_GPIO_I2CMUX, 0);
+			if (info->s_info != NULL) {
+				info->s_info->s_mode = u8val;
+				ad5816_pm_wr(info->s_info, NVC_PWR_OFF);
+			}
+			break;
+		case NVC_SYNC_MASTER:
+			info->s_mode = u8val;
+			ad5816_gpio_wr(info, AD5816_GPIO_I2CMUX, 0);
+			if (info->s_info != NULL)
+				info->s_info->s_mode = u8val;
+			break;
+		case NVC_SYNC_SLAVE:
+			if (info->s_info != NULL) {
+				/* default slave lens position */
+				err = ad5816_position_wr(info->s_info,
+						info->s_info->cap.focus_infinity);
+				if (!err) {
+					info->s_mode = u8val;
+					info->s_info->s_mode = u8val;
+					ad5816_gpio_wr(info,
+					AD5816_GPIO_I2CMUX, 0);
+				}
+				else {
+					if (info->s_mode != NVC_SYNC_STEREO)
+						ad5816_pm_wr(info->s_info,
+						NVC_PWR_OFF);
+						err = -EIO;
+				}
+			} else {
+				err = -EINVAL;
+			}
+			break;
+		case NVC_SYNC_STEREO:
+			if (info->s_info != NULL) {
+				/* sync power */
+				info->s_info->pwr_api = info->pwr_api;
+				/* move slave lens to master position */
+				err = ad5816_position_wr(info->s_info, (s32)info->pos);
+				if (!err) {
+					info->s_mode = u8val;
+					info->s_info->s_mode = u8val;
+					ad5816_gpio_wr(info, AD5816_GPIO_I2CMUX, 1);
+				}
+				else {
+					if (info->s_mode != NVC_SYNC_SLAVE)
+						ad5816_pm_wr(info->s_info, NVC_PWR_OFF);
+					err = -EIO;
+				}
+			} else {
+				err = -EINVAL;
+			}
+			break;
+		default:
+			err = -EINVAL;
+		}
+		if (info->pdata->cfg & NVC_CFG_NOERR)
+			return 0;
+		return err;
+
+	case NVC_PARAM_CAPS:
+		if (ad5816_set_focuser_capabilities(info, &params)) {
+			dev_err(&info->i2c_client->dev, "%s: Error: copy_from_user bytes %d\n",
+				__func__, params.sizeofvalue);
+			return -EFAULT;
+		}
+		return 0;
+
+	default:
+		/* parameters dependent on sync mode */
+		switch (info->s_mode) {
+		case NVC_SYNC_OFF:
+		case NVC_SYNC_MASTER:
+			return ad5816_param_wr_s(info, &params, s32val);
+		case NVC_SYNC_SLAVE:
+			return ad5816_param_wr_s(info->s_info, &params, s32val);
+		case NVC_SYNC_STEREO:
+			err = ad5816_param_wr_s(info, &params, s32val);
+			if (!(info->pdata->cfg & NVC_CFG_SYNC_I2C_MUX))
+				err |= ad5816_param_wr_s(info->s_info,
+						&params,
+						s32val);
+			return err;
+		default:
+			dev_err(&info->i2c_client->dev, "%s %d internal err\n",
+					__func__, __LINE__);
+			return -EINVAL;
+		}
+	}
+}
+
+static long ad5816_ioctl(struct file *file,
+					unsigned int cmd,
+					unsigned long arg)
+{
+	struct ad5816_info *info = file->private_data;
+	int pwr;
+	int err = 0;
+	switch (cmd) {
+	case NVC_IOCTL_PARAM_WR:
+		ad5816_pm_dev_wr(info, NVC_PWR_ON);
+		err = ad5816_param_wr(info, arg);
+		ad5816_pm_dev_wr(info, NVC_PWR_OFF);
+		return err;
+	case NVC_IOCTL_PARAM_RD:
+		ad5816_pm_dev_wr(info, NVC_PWR_ON);
+		err = ad5816_param_rd(info, arg);
+		ad5816_pm_dev_wr(info, NVC_PWR_OFF);
+		return err;
+	case NVC_IOCTL_PWR_WR:
+		/* This is a Guaranteed Level of Service (GLOS) call */
+		pwr = (int)arg * 2;
+		dev_dbg(&info->i2c_client->dev, "%s PWR_WR: %d\n",
+				__func__, pwr);
+		err = ad5816_pm_api_wr(info, pwr);
+		return err;
+	case NVC_IOCTL_PWR_RD:
+		if (info->s_mode == NVC_SYNC_SLAVE)
+			pwr = info->s_info->pwr_api / 2;
+		else
+			pwr = info->pwr_api / 2;
+		dev_dbg(&info->i2c_client->dev, "%s PWR_RD: %d\n",
+				__func__, pwr);
+		if (copy_to_user((void __user *)arg, (const void *)&pwr, sizeof(pwr))) {
+			dev_err(&info->i2c_client->dev, "%s copy_to_user err line %d\n",
+					__func__, __LINE__);
+			return -EFAULT;
+		}
+		return 0;
+	default:
+		dev_dbg(&info->i2c_client->dev, "%s unsupported ioctl: %x\n", __func__, cmd);
+	}
+	return -EINVAL;
+}
+
+
+static void ad5816_sdata_init(struct ad5816_info *info)
+{
+	/* set defaults */
+	memcpy(&info->config, &ad5816_default_info, sizeof(info->config));
+	memcpy(&info->nvc, &ad5816_default_nvc, sizeof(info->nvc));
+	memcpy(&info->cap, &ad5816_default_cap, sizeof(info->cap));
+
+	info->config.settle_time = AD5816_SETTLETIME;
+	info->config.focal_length = AD5816_FOCAL_LENGTH;
+	info->config.fnumber = AD5816_FNUMBER;
+	info->config.pos_low = AD5816_POS_LOW_DEFAULT;
+	info->config.pos_high = AD5816_POS_HIGH_DEFAULT;
+
+	/* set to proper value */
+	info->cap.actuator_range = info->config.pos_high - info->config.pos_low;
+
+	/* set overrides if any */
+	if (info->pdata->nvc) {
+		if (info->pdata->nvc->fnumber)
+			info->nvc.fnumber = info->pdata->nvc->fnumber;
+		if (info->pdata->nvc->focal_length)
+			info->nvc.focal_length = info->pdata->nvc->focal_length;
+		if (info->pdata->nvc->max_aperature)
+			info->nvc.max_aperature = info->pdata->nvc->max_aperature;
+	}
+
+	if (info->pdata->cap) {
+		if (info->pdata->cap->actuator_range)
+			info->cap.actuator_range = info->pdata->cap->actuator_range;
+		if (info->pdata->cap->settle_time)
+			info->cap.settle_time = info->pdata->cap->settle_time;
+		if (info->pdata->cap->focus_macro)
+			info->cap.focus_macro = info->pdata->cap->focus_macro;
+		if (info->pdata->cap->focus_hyper)
+			info->cap.focus_hyper = info->pdata->cap->focus_hyper;
+		if (info->pdata->cap->focus_infinity)
+			info->cap.focus_infinity = info->pdata->cap->focus_infinity;
+	}
+}
+
+static int ad5816_sync_en(unsigned num, unsigned sync)
+{
+	struct ad5816_info *master = NULL;
+	struct ad5816_info *slave = NULL;
+	struct ad5816_info *pos = NULL;
+	rcu_read_lock();
+	list_for_each_entry_rcu(pos, &ad5816_info_list, list) {
+		if (pos->pdata->num == num) {
+			master = pos;
+			break;
+		}
+	}
+	pos = NULL;
+	list_for_each_entry_rcu(pos, &ad5816_info_list, list) {
+		if (pos->pdata->num == sync) {
+			slave = pos;
+			break;
+		}
+	}
+	rcu_read_unlock();
+	if (master != NULL)
+		master->s_info = NULL;
+	if (slave != NULL)
+		slave->s_info = NULL;
+	if (!sync)
+		return 0; /* no err if sync disabled */
+	if (num == sync)
+		return -EINVAL; /* err if sync instance is itself */
+	if ((master != NULL) && (slave != NULL)) {
+		master->s_info = slave;
+		slave->s_info = master;
+	}
+	return 0;
+}
+
+static int ad5816_sync_dis(struct ad5816_info *info)
+{
+	if (info->s_info != NULL) {
+		info->s_info->s_mode = 0;
+		info->s_info->s_info = NULL;
+		info->s_mode = 0;
+		info->s_info = NULL;
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int ad5816_open(struct inode *inode, struct file *file)
+{
+	struct ad5816_info *info = NULL;
+	struct ad5816_info *pos = NULL;
+	int err;
+	rcu_read_lock();
+	list_for_each_entry_rcu(pos, &ad5816_info_list, list) {
+		if (pos->miscdev.minor == iminor(inode)) {
+			info = pos;
+			break;
+		}
+	}
+	rcu_read_unlock();
+	if (!info)
+		return -ENODEV;
+	err = ad5816_sync_en(info->pdata->num, info->pdata->sync);
+	if (err == -EINVAL)
+		dev_err(&info->i2c_client->dev, "%s err: invalid num (%u) and sync (%u) instance\n",
+		__func__, info->pdata->num, info->pdata->sync);
+	if (atomic_xchg(&info->in_use, 1))
+		return -EBUSY;
+	if (info->s_info != NULL) {
+		if (atomic_xchg(&info->s_info->in_use, 1))
+			return -EBUSY;
+	}
+	file->private_data = info;
+	ad5816_pm_dev_wr(info, NVC_PWR_ON);
+	ad5816_position_wr(info, info->cap.focus_infinity);
+	ad5816_pm_dev_wr(info, NVC_PWR_OFF);
+	dev_dbg(&info->i2c_client->dev, "%s\n", __func__);
+
+	return 0;
+}
+
+static int ad5816_release(struct inode *inode, struct file *file)
+{
+	struct ad5816_info *info = file->private_data;
+	dev_dbg(&info->i2c_client->dev, "%s\n", __func__);
+	ad5816_pm_wr_s(info, NVC_PWR_OFF);
+	file->private_data = NULL;
+	WARN_ON(!atomic_xchg(&info->in_use, 0));
+	if (info->s_info != NULL)
+		WARN_ON(!atomic_xchg(&info->s_info->in_use, 0));
+	ad5816_sync_dis(info);
+	return 0;
+}
+
+static const struct file_operations ad5816_fileops = {
+	.owner = THIS_MODULE,
+	.open = ad5816_open,
+	.unlocked_ioctl = ad5816_ioctl,
+	.release = ad5816_release,
+};
+
+static void ad5816_del(struct ad5816_info *info)
+{
+	ad5816_pm_exit(info);
+	if ((info->s_mode == NVC_SYNC_SLAVE) ||
+		(info->s_mode == NVC_SYNC_STEREO))
+		ad5816_pm_exit(info->s_info);
+
+	ad5816_sync_dis(info);
+	spin_lock(&ad5816_spinlock);
+	list_del_rcu(&info->list);
+	spin_unlock(&ad5816_spinlock);
+	synchronize_rcu();
+}
+
+static int ad5816_remove(struct i2c_client *client)
+{
+	struct ad5816_info *info = i2c_get_clientdata(client);
+	dev_dbg(&info->i2c_client->dev, "%s\n", __func__);
+	misc_deregister(&info->miscdev);
+	ad5816_del(info);
+	return 0;
+}
+
+static int ad5816_probe(
+		struct i2c_client *client,
+		const struct i2c_device_id *id)
+{
+	struct ad5816_info *info;
+	char dname[16];
+	int err;
+
+	pr_info("ad5816: probing focuser.\n");
+	dev_dbg(&client->dev, "%s\n", __func__);
+	info = devm_kzalloc(&client->dev, sizeof(*info), GFP_KERNEL);
+	if (info == NULL) {
+		dev_err(&client->dev, "%s: kzalloc error\n", __func__);
+		return -ENOMEM;
+	}
+	info->i2c_client = client;
+	if (client->dev.platform_data) {
+		info->pdata = client->dev.platform_data;
+	} else {
+		info->pdata = &ad5816_default_pdata;
+		dev_dbg(&client->dev,"%s No platform data.  Using defaults.\n", __func__);
+	}
+
+	i2c_set_clientdata(client, info);
+	INIT_LIST_HEAD(&info->list);
+	spin_lock(&ad5816_spinlock);
+	list_add_rcu(&info->list, &ad5816_info_list);
+	spin_unlock(&ad5816_spinlock);
+	ad5816_pm_init(info);
+	ad5816_sdata_init(info);
+
+	if (info->pdata->cfg & (NVC_CFG_NODEV | NVC_CFG_BOOT_INIT)) {
+		err = ad5816_dev_id(info);
+		if (err < 0) {
+			dev_err(&client->dev, "%s device not found\n", __func__);
+			ad5816_pm_wr(info, NVC_PWR_OFF);
+			if (info->pdata->cfg & NVC_CFG_NODEV) {
+				ad5816_del(info);
+				return -ENODEV;
+			}
+		} else {
+			dev_dbg(&client->dev, "%s device found\n", __func__);
+			if (info->pdata->cfg & NVC_CFG_BOOT_INIT) {
+				/* initial move causes full initialization */
+				ad5816_pm_dev_wr(info, NVC_PWR_ON);
+				ad5816_position_wr(info, info->cap.focus_infinity);
+				ad5816_pm_dev_wr(info, NVC_PWR_OFF);
+			}
+		}
+	}
+
+	if (info->pdata->dev_name != 0)
+		strcpy(dname, info->pdata->dev_name);
+	else
+		strcpy(dname, "ad5816");
+
+	if (info->pdata->num)
+		snprintf(dname, sizeof(dname), "%s.%u", dname, info->pdata->num);
+
+	info->miscdev.name = dname;
+	info->miscdev.fops = &ad5816_fileops;
+	info->miscdev.minor = MISC_DYNAMIC_MINOR;
+	if (misc_register(&info->miscdev)) {
+		dev_err(&client->dev, "%s unable to register misc device %s\n",
+			__func__, dname);
+		ad5816_del(info);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+
+static const struct i2c_device_id ad5816_id[] = {
+	{ "ad5816", 0 },
+	{ },
+};
+
+
+MODULE_DEVICE_TABLE(i2c, ad5816_id);
+
+static struct i2c_driver ad5816_i2c_driver = {
+	.driver = {
+		.name = "ad5816",
+		.owner = THIS_MODULE,
+	},
+	.id_table = ad5816_id,
+	.probe = ad5816_probe,
+	.remove = ad5816_remove,
+};
+
+static int __init ad5816_init(void)
+{
+	return i2c_add_driver(&ad5816_i2c_driver);
+}
+
+static void __exit ad5816_exit(void)
+{
+	i2c_del_driver(&ad5816_i2c_driver);
+}
+
+module_init(ad5816_init);
+module_exit(ad5816_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/tegra/ad5820.c b/drivers/media/video/tegra/ad5820.c
new file mode 100644
index 000000000000..adfda3e712dc
--- /dev/null
+++ b/drivers/media/video/tegra/ad5820.c
@@ -0,0 +1,231 @@
+/*
+ * AD5820 focuser driver.
+ *
+ * Copyright (C) 2010-2011 NVIDIA Corporation.
+ *
+ * Contributors:
+ *      Sachin Nikam <snikam@nvidia.com>
+ *
+ * Based on ov5650.c.
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <media/ad5820.h>
+
+/* Focuser single step & full scale transition time truth table
+ * in the format of:
+ *   index	mode		single step transition	full scale transition
+ *	0	0			0			0
+ *	1	1			50uS			51.2mS
+ *	2	1			100uS			102.3mS
+ *	3	1			200uS			204.6mS
+ *	4	1			400uS			409.2mS
+ *	5	1			800uS			818.4mS
+ *	6	1			1600uS			1636.8mS
+ *	7	1			3200uS			3273.6mS
+ *	8	0			0			0
+ *	9	2			50uS			1.1mS
+ *	A	2			100uS			2.2mS
+ *	B	2			200uS			4.4mS
+ *	C	2			400uS			8.8mS
+ *	D	2			800uS			17.6mS
+ *	E	2			1600uS			35.2mS
+ *	F	2			3200uS			70.4mS
+ */
+
+/* pick up the mode index setting and its settle time from the above table */
+#define AD5820_TRANSITION_MODE 0x0B
+#define SETTLETIME_MS 5
+
+#define POS_LOW (0)
+#define POS_HIGH (1023)
+#define FOCAL_LENGTH (4.507f)
+#define FNUMBER (2.8f)
+#define FPOS_COUNT 1024
+
+struct ad5820_info {
+	struct i2c_client *i2c_client;
+	struct regulator *regulator;
+	struct ad5820_config config;
+};
+
+static int ad5820_write(struct i2c_client *client, u32 value)
+{
+	int count;
+	struct i2c_msg msg[1];
+	unsigned char data[2];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = (u8) ((value >> 4) & 0x3F);
+	data[1] = (u8) ((value & 0xF) << 4) | AD5820_TRANSITION_MODE;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = ARRAY_SIZE(data);
+	msg[0].buf = data;
+
+	count = i2c_transfer(client->adapter, msg, ARRAY_SIZE(msg));
+	if (count == ARRAY_SIZE(msg))
+		return 0;
+
+	return -EIO;
+}
+
+static int ad5820_set_position(struct ad5820_info *info, u32 position)
+{
+	if (position < info->config.pos_low ||
+	    position > info->config.pos_high)
+		return -EINVAL;
+
+	return ad5820_write(info->i2c_client, position);
+}
+
+static long ad5820_ioctl(struct file *file,
+			unsigned int cmd, unsigned long arg)
+{
+	struct ad5820_info *info = file->private_data;
+
+	switch (cmd) {
+	case AD5820_IOCTL_GET_CONFIG:
+	{
+		if (copy_to_user((void __user *) arg,
+				 &info->config,
+				 sizeof(info->config))) {
+			pr_err("%s: 0x%x\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+
+		break;
+	}
+	case AD5820_IOCTL_SET_POSITION:
+		return ad5820_set_position(info, (u32) arg);
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+struct ad5820_info *info;
+
+static int ad5820_open(struct inode *inode, struct file *file)
+{
+	file->private_data = info;
+	if (info->regulator)
+		regulator_enable(info->regulator);
+	return 0;
+}
+
+int ad5820_release(struct inode *inode, struct file *file)
+{
+	if (info->regulator)
+		regulator_disable(info->regulator);
+	file->private_data = NULL;
+	return 0;
+}
+
+
+static const struct file_operations ad5820_fileops = {
+	.owner = THIS_MODULE,
+	.open = ad5820_open,
+	.unlocked_ioctl = ad5820_ioctl,
+	.release = ad5820_release,
+};
+
+static struct miscdevice ad5820_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "ad5820",
+	.fops = &ad5820_fileops,
+};
+
+static int ad5820_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int err;
+
+	pr_info("ad5820: probing sensor.\n");
+
+	info = kzalloc(sizeof(struct ad5820_info), GFP_KERNEL);
+	if (!info) {
+		pr_err("ad5820: Unable to allocate memory!\n");
+		return -ENOMEM;
+	}
+
+	err = misc_register(&ad5820_device);
+	if (err) {
+		pr_err("ad5820: Unable to register misc device!\n");
+		kfree(info);
+		return err;
+	}
+
+	info->regulator = regulator_get(&client->dev, "vdd_vcore_af");
+	if (IS_ERR_OR_NULL(info->regulator)) {
+		dev_err(&client->dev, "unable to get regulator %s\n",
+			dev_name(&client->dev));
+		info->regulator = NULL;
+	} else {
+		regulator_enable(info->regulator);
+	}
+
+	info->i2c_client = client;
+	info->config.settle_time = SETTLETIME_MS;
+	info->config.focal_length = FOCAL_LENGTH;
+	info->config.fnumber = FNUMBER;
+	info->config.pos_low = POS_LOW;
+	info->config.pos_high = POS_HIGH;
+	i2c_set_clientdata(client, info);
+	return 0;
+}
+
+static int ad5820_remove(struct i2c_client *client)
+{
+	struct ad5820_info *info;
+	info = i2c_get_clientdata(client);
+	misc_deregister(&ad5820_device);
+	kfree(info);
+	return 0;
+}
+
+static const struct i2c_device_id ad5820_id[] = {
+	{ "ad5820", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, ad5820_id);
+
+static struct i2c_driver ad5820_i2c_driver = {
+	.driver = {
+		.name = "ad5820",
+		.owner = THIS_MODULE,
+	},
+	.probe = ad5820_probe,
+	.remove = ad5820_remove,
+	.id_table = ad5820_id,
+};
+
+static int __init ad5820_init(void)
+{
+	pr_info("ad5820 sensor driver loading\n");
+	return i2c_add_driver(&ad5820_i2c_driver);
+}
+
+static void __exit ad5820_exit(void)
+{
+	i2c_del_driver(&ad5820_i2c_driver);
+}
+
+module_init(ad5820_init);
+module_exit(ad5820_exit);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/video/tegra/ar0832_main.c b/drivers/media/video/tegra/ar0832_main.c
new file mode 100644
index 000000000000..b1f43da22ec8
--- /dev/null
+++ b/drivers/media/video/tegra/ar0832_main.c
@@ -0,0 +1,2588 @@
+/*
+* ar0832_main.c - Aptina AR0832 8M Bayer type sensor driver
+*
+* Copyright (c) 2011, NVIDIA, All Rights Reserved.
+*
+* This file is licensed under the terms of the GNU General Public License
+* version 2. This program is licensed "as is" without any warranty of any
+* kind, whether express or implied.
+*/
+
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <mach/hardware.h>
+#include <linux/gpio.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <asm/atomic.h>
+#include <linux/regulator/consumer.h>
+#include <media/ar0832_main.h>
+
+
+#define POS_ACTUAL_LOW			0
+#define POS_ACTUAL_HIGH			255
+#define SETTLE_TIME				100
+#define AR0832_SLEW_RATE_DISABLED  0
+#define AR0832_SLEW_RATE_SLOWEST   7
+
+
+struct ar0832_sensor_info {
+	int mode;
+	struct ar0832_stereo_region region;
+};
+
+struct ar0832_focuser_info {
+	struct nv_focuser_config config;
+	int focuser_init_flag;
+	u16 last_position;
+};
+
+struct ar0832_power_rail {
+	struct regulator *sen_1v8_reg;
+	struct regulator *sen_2v8_reg;
+};
+
+struct ar0832_dev {
+	struct ar0832_sensor_info *sensor_info;
+	struct ar0832_focuser_info *focuser_info;
+	struct ar0832_platform_data *pdata;
+	struct i2c_client *i2c_client;
+	struct mutex ar0832_camera_lock;
+	struct miscdevice misc_dev;
+	struct ar0832_power_rail power_rail;
+	int brd_power_cnt;
+	atomic_t in_use;
+	char dname[20];
+	int is_stereo;
+	u16 sensor_id_data;
+	struct dentry *debugdir;
+};
+
+#define UpperByte16to8(x) ((u8)((x & 0xFF00) >> 8))
+#define LowerByte16to8(x) ((u8)(x & 0x00FF))
+
+#define ar0832_TABLE_WAIT_MS 0
+#define ar0832_TABLE_END 1
+#define ar0832_MAX_RETRIES 3
+
+/* AR0832 Register */
+#define AR0832_SENSORID_REG	0x0002
+#define AR0832_RESET_REG	0x301A
+#define AR0832_ID_REG		0x31FC
+#define AR0832_GLOBAL_GAIN_REG	0x305E
+#define AR0832_TEST_PATTERN_REG	0x0600
+#define AR0832_GROUP_HOLD_REG	0x0104
+#define AR0832_TEST_RED_REG	0x0602
+#define AR0832_TEST_GREENR_REG	0x0604
+#define AR0832_TEST_BLUE_REG	0x0606
+#define AR0832_TEST_GREENB_REG	0x0608
+
+
+
+/* AR0832_RESET_REG */
+#define AR0832_RESET_REG_GROUPED_PARAMETER_HOLD		(1 << 15)
+#define AR0832_RESET_REG_GAIN_INSERT			(1 << 14)
+#define AR0832_RESET_REG_SMIA_SERIALIZER_DIS		(1 << 12)
+#define AR0832_RESET_REG_RESTART_BAD			(1 << 10)
+#define AR0832_RESET_REG_MASK_BAD			(1 << 9)
+#define AR0832_RESET_REG_GPI_EN				(1 << 8)
+#define AR0832_RESET_REG_PARALLEL_EN			(1 << 7)
+#define AR0832_RESET_REG_DRIVE_PINS			(1 << 6)
+#define AR0832_RESET_REG_STDBY_EOF			(1 << 4)
+#define AR0832_RESET_REG_LOCK_REG			(1 << 3)
+#define AR0832_RESET_REG_STREAM				(1 << 2)
+#define AR0832_RESET_REG_RESTART			(1 << 1)
+#define AR0832_RESET_REG_RESET				(1 << 0)
+
+static struct ar0832_reg mode_start[] = {
+	{ar0832_TABLE_END, 0x0000}
+};
+
+static struct ar0832_reg mode_3264X2448_8140[] = {
+	/* mode start */
+	{0x301A, 0x0058},	/* RESET_REGISTER */
+	{0x301A, 0x0050},	/* RESET_REGISTER */
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+	{0x3064, 0x7800},	/* RESERVED_MFR_3064 */
+	{0x31AE, 0x0202},	/* SERIAL_FORMAT */
+	/* MT9E013 Recommended Settings */
+	{0x31B0, 0x0083},	/* FRAME_PREAMBLE */
+	{0x31B2, 0x004D},	/* LINE_PREAMBLE */
+	{0x31B4, 0x0E77},	/* MIPI_TIMING_0 */
+	{0x31B6, 0x0D20},	/* MIPI_TIMING_1 */
+	{0x31B8, 0x020E},	/* MIPI_TIMING_2 */
+	{0x31BA, 0x0710},	/* MIPI_TIMING_3 */
+	{0x31BC, 0x2A0D},	/* MIPI_TIMING_4 */
+	{ar0832_TABLE_WAIT_MS, 0x0005},
+	{0x0112, 0x0A0A},	/* CCP_DATA_FORMAT */
+	{0x3044, 0x0590},
+	{0x306E, 0xFC80},
+	{0x30B2, 0xC000},
+	{0x30D6, 0x0800},
+	{0x316C, 0xB42F},
+	{0x316E, 0x869A},
+	{0x3170, 0x210E},
+	{0x317A, 0x010E},
+	{0x31E0, 0x1FB9},
+	{0x31E6, 0x07FC},
+	{0x37C0, 0x0000},
+	{0x37C2, 0x0000},
+	{0x37C4, 0x0000},
+	{0x37C6, 0x0000},
+	{0x3E00, 0x0011},
+	{0x3E02, 0x8801},
+	{0x3E04, 0x2801},
+	{0x3E06, 0x8449},
+	{0x3E08, 0x6841},
+	{0x3E0A, 0x400C},
+	{0x3E0C, 0x1001},
+	{0x3E0E, 0x2603},
+	{0x3E10, 0x4B41},
+	{0x3E12, 0x4B24},
+	{0x3E14, 0xA3CF},
+	{0x3E16, 0x8802},
+	{0x3E18, 0x84FF},
+	{0x3E1A, 0x8601},
+	{0x3E1C, 0x8401},
+	{0x3E1E, 0x840A},
+	{0x3E20, 0xFF00},
+	{0x3E22, 0x8401},
+	{0x3E24, 0x00FF},
+	{0x3E26, 0x0088},
+	{0x3E28, 0x2E8A},
+	{0x3E30, 0x0000},
+	{0x3E32, 0x8801},
+	{0x3E34, 0x4029},
+	{0x3E36, 0x00FF},
+	{0x3E38, 0x8469},
+	{0x3E3A, 0x00FF},
+	{0x3E3C, 0x2801},
+	{0x3E3E, 0x3E2A},
+	{0x3E40, 0x1C01},
+	{0x3E42, 0xFF84},
+	{0x3E44, 0x8401},
+	{0x3E46, 0x0C01},
+	{0x3E48, 0x8401},
+	{0x3E4A, 0x00FF},
+	{0x3E4C, 0x8402},
+	{0x3E4E, 0x8984},
+	{0x3E50, 0x6628},
+	{0x3E52, 0x8340},
+	{0x3E54, 0x00FF},
+	{0x3E56, 0x4A42},
+	{0x3E58, 0x2703},
+	{0x3E5A, 0x6752},
+	{0x3E5C, 0x3F2A},
+	{0x3E5E, 0x846A},
+	{0x3E60, 0x4C01},
+	{0x3E62, 0x8401},
+	{0x3E66, 0x3901},
+	{0x3E90, 0x2C01},
+	{0x3E98, 0x2B02},
+	{0x3E92, 0x2A04},
+	{0x3E94, 0x2509},
+	{0x3E96, 0x0000},
+	{0x3E9A, 0x2905},
+	{0x3E9C, 0x00FF},
+	{0x3ECC, 0x00EB},
+	{0x3ED0, 0x1E24},
+	{0x3ED4, 0xAFC4},
+	{0x3ED6, 0x909B},
+	{0x3EE0, 0x2424},
+	{0x3EE4, 0xC100},
+	{0x3EE6, 0x0540},
+	{0x3174, 0x8000},
+
+	/* mode end */
+	{0x0300, 0x0004},	/* VT_PIX_CLK_DIV */
+	{0x0302, 0x0001},	/* VT_SYS_CLK_DIV */
+	{0x0304, 0x0002},	/* PRE_PLL_CLK_DIV */
+	{0x0306, 0x0040},	/* PLL_MULTIPLIER */
+	{0x0308, 0x000A},	/* OP_PIX_CLK_DIV */
+	{0x030A, 0x0001},	/* OP_SYS_CLK_DIV */
+	{ar0832_TABLE_WAIT_MS, 0x0001},
+	{0x3064, 0x7400},	/* RESERVED_MFR_3064 */
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+	{0x0344, 0x0004},	/* X_ADDR_START */
+	{0x0348, 0x0CCB},	/* X_ADDR_END */
+	{0x0346, 0x0004},	/* Y_ADDR_START */
+	{0x034A, 0x099B},	/* Y_ADDR_END */
+	{0x034C, 0x0CC8},	/* X_OUTPUT_SIZE */
+	{0x034E, 0x0998},	/* Y_OUTPUT_SIZE */
+	{0x3040, 0xC041},	/* READ_MODE */
+	{0x306E, 0xFC80},	/* DATAPATH_SELECT */
+	{0x0400, 0x0000},	/* SCALING_MODE */
+	{0x0404, 0x0010},	/* SCALE_M */
+	{0x3178, 0x0000},	/* RESERVED_MFR_3178 */
+	{0x3ED0, 0x1E24},	/* RESERVED_MFR_3ED0 */
+	{0x0400, 0x0000},	/* SCALING_MODE */
+	{0x0404, 0x0010},	/* SCALE_M */
+	{0x0342, 0x133C},	/* LINE_LENGTH_PCK */
+	{0x0340, 0x0A27},	/* FRAME_LENGTH_LINES */
+	{0x0202, 0x0A27},	/* COARSE_INTEGRATION_TIME */
+	{0x3014, 0x09DC},	/* FINE_INTEGRATION_TIME */
+	{0x3010, 0x0078},	/* FINE_CORRECTION */
+	{0x301A, 0x8250},	/* RESET_REGISTER */
+	{0x301A, 0x8650},	/* RESET_REGISTER */
+	{0x301A, 0x8658},	/* RESET_REGISTER */
+	/* gain */
+	{0x305e, 0x10AA},	/* gain */
+	{0x0104, 0x0000},	/* GROUPED_PARAMETER_HOLD */
+	{0x301A, 0x065C},	/* RESET_REGISTER */
+	{ar0832_TABLE_END, 0x0000}
+};
+
+static struct ar0832_reg mode_3264X2448_8141[] = {
+	/* mode start */
+	{0x301A, 0x0058},	/* RESET_REGISTER */
+	{0x301A, 0x0050},	/* RESET_REGISTER */
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+	{0x3064, 0x7800},	/* RESERVED_MFR_3064 */
+	{0x31AE, 0x0202},	/* SERIAL_FORMAT */
+	/* AR0832 Recommended Settings */
+	{0x3044, 0x0590},
+	{0x306E, 0xFC80},
+	{0x30B2, 0xC000},
+	{0x30D6, 0x0800},
+	{0x316C, 0xB42A},
+	{0x316E, 0x869C},
+	{0x3170, 0x210E},
+	{0x317A, 0x010E},
+	{0x31E0, 0x1FB9},
+	{0x31E6, 0x07FC},
+	{0x37C0, 0x0000},
+	{0x37C2, 0x0000},
+	{0x37C4, 0x0000},
+	{0x37C6, 0x0000},
+	{0x3E00, 0x0011},
+	{0x3E02, 0x8801},
+	{0x3E04, 0x2801},
+	{0x3E06, 0x8449},
+	{0x3E08, 0x6841},
+	{0x3E0A, 0x400C},
+	{0x3E0C, 0x1001},
+	{0x3E0E, 0x2603},
+	{0x3E10, 0x4B41},
+	{0x3E12, 0x4B24},
+	{0x3E14, 0xA3CF},
+	{0x3E16, 0x8802},
+	{0x3E18, 0x8401},
+	{0x3E1A, 0x8601},
+	{0x3E1C, 0x8401},
+	{0x3E1E, 0x840A},
+	{0x3E20, 0xFF00},
+	{0x3E22, 0x8401},
+	{0x3E24, 0x00FF},
+	{0x3E26, 0x0088},
+	{0x3E28, 0x2E8A},
+	{0x3E30, 0x0000},
+	{0x3E32, 0x00FF},
+	{0x3E34, 0x4029},
+	{0x3E36, 0x00FF},
+	{0x3E38, 0x8469},
+	{0x3E3A, 0x00FF},
+	{0x3E3C, 0x2801},
+	{0x3E3E, 0x3E2A},
+	{0x3E40, 0x1C01},
+	{0x3E42, 0xFF84},
+	{0x3E44, 0x8401},
+	{0x3E46, 0x0C01},
+	{0x3E48, 0x8401},
+	{0x3E4A, 0x00FF},
+	{0x3E4C, 0x8402},
+	{0x3E4E, 0x8984},
+	{0x3E50, 0x6628},
+	{0x3E52, 0x8340},
+	{0x3E54, 0x00FF},
+	{0x3E56, 0x4A42},
+	{0x3E58, 0x2703},
+	{0x3E5A, 0x6752},
+	{0x3E5C, 0x3F2A},
+	{0x3E5E, 0x846A},
+	{0x3E60, 0x4C01},
+	{0x3E62, 0x8401},
+	{0x3E66, 0x3901},
+	{0x3E90, 0x2C01},
+	{0x3E98, 0x2B02},
+	{0x3E92, 0x2A04},
+	{0x3E94, 0x2509},
+	{0x3E96, 0xF000},
+	{0x3E9A, 0x2905},
+	{0x3E9C, 0x00FF},
+	{0x3ECC, 0x00EB},
+	{0x3ED0, 0x1E24},
+	{0x3ED4, 0xFAA4},
+	{0x3ED6, 0x909B},
+	{0x3EE0, 0x2424},
+	{0x3EE4, 0xC100},
+	{0x3EE6, 0x0540},
+	{0x3174, 0x8000},
+
+	/* mode end */
+	{0x0300, 0x0004},	/* VT_PIX_CLK_DIV */
+	{0x0302, 0x0001},	/* VT_SYS_CLK_DIV */
+	{0x0304, 0x0002},	/* PRE_PLL_CLK_DIV */
+	{0x0306, 0x0040},	/* PLL_MULTIPLIER */
+	{0x0308, 0x000A},	/* OP_PIX_CLK_DIV */
+	{0x030A, 0x0001},	/* OP_SYS_CLK_DIV */
+	{ar0832_TABLE_WAIT_MS, 0x0001},
+	{0x3064, 0x7400},	/* RESERVED_MFR_3064 */
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+	{0x0344, 0x0004},	/* X_ADDR_START */
+	{0x0348, 0x0CCB},	/* X_ADDR_END */
+	{0x0346, 0x0004},	/* Y_ADDR_START */
+	{0x034A, 0x099B},	/* Y_ADDR_END */
+	{0x034C, 0x0CC8},	/* X_OUTPUT_SIZE */
+	{0x034E, 0x0998},	/* Y_OUTPUT_SIZE */
+	{0x3040, 0xC041},	/* READ_MODE */
+	{0x306E, 0xFC80},	/* DATAPATH_SELECT */
+	{0x0400, 0x0000},	/* SCALING_MODE */
+	{0x0404, 0x0010},	/* SCALE_M */
+	{0x3178, 0x0000},	/* RESERVED_MFR_3178 */
+	{0x3ED0, 0x1E24},	/* RESERVED_MFR_3ED0 */
+	{0x0400, 0x0000},	/* SCALING_MODE */
+	{0x0404, 0x0010},	/* SCALE_M */
+	{0x0342, 0x133C},	/* LINE_LENGTH_PCK */
+	{0x0340, 0x0A27},	/* FRAME_LENGTH_LINES */
+	{0x0202, 0x0A27},	/* COARSE_INTEGRATION_TIME */
+	{0x3014, 0x09DC},	/* FINE_INTEGRATION_TIME */
+	{0x3010, 0x0078},	/* FINE_CORRECTION */
+	{0x301A, 0x8250},	/* RESET_REGISTER */
+	{0x301A, 0x8650},	/* RESET_REGISTER */
+	{0x301A, 0x8658},	/* RESET_REGISTER */
+	/* gain */
+	{0x305e, 0x10AA},	/* gain */
+	{0x0104, 0x0000},	/* GROUPED_PARAMETER_HOLD */
+	{0x301A, 0x065C},	/* RESET_REGISTER */
+	{ar0832_TABLE_END, 0x0000}
+};
+
+static struct ar0832_reg mode_2880X1620_8140[] = {
+	/* mode start */
+	{0x301A, 0x0058},	/* RESET_REGISTER */
+	{0x301A, 0x0050},	/* RESET_REGISTER */
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+	{0x3064, 0x7800},	/* RESERVED_MFR_3064 */
+	{0x31AE, 0x0202},	/* SERIAL_FORMAT */
+	/* MT9E013 Recommended Settings */
+	{0x31B0, 0x0083},	/* FRAME_PREAMBLE */
+	{0x31B2, 0x004D},	/* LINE_PREAMBLE */
+	{0x31B4, 0x0E77},	/* MIPI_TIMING_0 */
+	{0x31B6, 0x0D20},	/* MIPI_TIMING_1 */
+	{0x31B8, 0x020E},	/* MIPI_TIMING_2 */
+	{0x31BA, 0x0710},	/* MIPI_TIMING_3 */
+	{0x31BC, 0x2A0D},	/* MIPI_TIMING_4 */
+	{ar0832_TABLE_WAIT_MS, 0x0005},
+	{0x0112, 0x0A0A},	/* CCP_DATA_FORMAT */
+	{0x3044, 0x0590},
+	{0x306E, 0xFC80},
+	{0x30B2, 0xC000},
+	{0x30D6, 0x0800},
+	{0x316C, 0xB42F},
+	{0x316E, 0x869A},
+	{0x3170, 0x210E},
+	{0x317A, 0x010E},
+	{0x31E0, 0x1FB9},
+	{0x31E6, 0x07FC},
+	{0x37C0, 0x0000},
+	{0x37C2, 0x0000},
+	{0x37C4, 0x0000},
+	{0x37C6, 0x0000},
+	{0x3E00, 0x0011},
+	{0x3E02, 0x8801},
+	{0x3E04, 0x2801},
+	{0x3E06, 0x8449},
+	{0x3E08, 0x6841},
+	{0x3E0A, 0x400C},
+	{0x3E0C, 0x1001},
+	{0x3E0E, 0x2603},
+	{0x3E10, 0x4B41},
+	{0x3E12, 0x4B24},
+	{0x3E14, 0xA3CF},
+	{0x3E16, 0x8802},
+	{0x3E18, 0x8401},
+	{0x3E1A, 0x8601},
+	{0x3E1C, 0x8401},
+	{0x3E1E, 0x840A},
+	{0x3E20, 0xFF00},
+	{0x3E22, 0x8401},
+	{0x3E24, 0x00FF},
+	{0x3E26, 0x0088},
+	{0x3E28, 0x2E8A},
+	{0x3E30, 0x0000},
+	{0x3E32, 0x8801},
+	{0x3E34, 0x4029},
+	{0x3E36, 0x00FF},
+	{0x3E38, 0x8469},
+	{0x3E3A, 0x00FF},
+	{0x3E3C, 0x2801},
+	{0x3E3E, 0x3E2A},
+	{0x3E40, 0x1C01},
+	{0x3E42, 0xFF84},
+	{0x3E44, 0x8401},
+	{0x3E46, 0x0C01},
+	{0x3E48, 0x8401},
+	{0x3E4A, 0x00FF},
+	{0x3E4C, 0x8402},
+	{0x3E4E, 0x8984},
+	{0x3E50, 0x6628},
+	{0x3E52, 0x8340},
+	{0x3E54, 0x00FF},
+	{0x3E56, 0x4A42},
+	{0x3E58, 0x2703},
+	{0x3E5A, 0x6752},
+	{0x3E5C, 0x3F2A},
+	{0x3E5E, 0x846A},
+	{0x3E60, 0x4C01},
+	{0x3E62, 0x8401},
+	{0x3E66, 0x3901},
+	{0x3E90, 0x2C01},
+	{0x3E98, 0x2B02},
+	{0x3E92, 0x2A04},
+	{0x3E94, 0x2509},
+	{0x3E96, 0x0000},
+	{0x3E9A, 0x2905},
+	{0x3E9C, 0x00FF},
+	{0x3ECC, 0x00EB},
+	{0x3ED0, 0x1E24},
+	{0x3ED4, 0xAFC4},
+	{0x3ED6, 0x909B},
+	{0x3EE0, 0x2424},
+	{0x3EE2, 0x9797},
+	{0x3EE4, 0xC100},
+	{0x3EE6, 0x0540},
+	{0x3174, 0x8000},
+
+	/* mode end */
+	{0x0300, 0x0004},	/* VT_PIX_CLK_DIV */
+	{0x0302, 0x0001},	/* VT_SYS_CLK_DIV */
+	{0x0304, 0x0002},	/* PRE_PLL_CLK_DIV */
+	{0x0306, 0x0040},	/* PLL_MULTIPLIER */
+	{0x0308, 0x000A},	/* OP_PIX_CLK_DIV */
+	{0x030A, 0x0001},	/* OP_SYS_CLK_DIV */
+	{ar0832_TABLE_WAIT_MS, 0x0001},
+	{0x3064, 0x7400},	/* RESERVED_MFR_3064 */
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+	{0x0344, 0x00C8},	/* X_ADDR_START */
+	{0x0348, 0x0C07},	/* X_ADDR_END */
+	{0x0346, 0x01A6},	/* Y_ADDR_START */
+	{0x034A, 0x07F9},	/* Y_ADDR_END */
+	{0x034C, 0x0B40},	/* X_OUTPUT_SIZE */
+	{0x034E, 0x0654},	/* Y_OUTPUT_SIZE */
+	{0x3040, 0xC041},	/* READ_MODE */
+	{0x306E, 0xFC80},	/* DATAPATH_SELECT */
+	{0x0400, 0x0000},	/* SCALING_MODE */
+	{0x0404, 0x0010},	/* SCALE_M */
+	{0x3178, 0x0000},	/* RESERVED_MFR_3178 */
+	{0x3ED0, 0x1E24},	/* RESERVED_MFR_3ED0 */
+
+	{0x0342, 0x11B8},	/* LINE_LENGTH_PCK */
+	{0x0340, 0x06E3},	/* FRAME_LENGTH_LINES */
+	{0x0202, 0x06E3},	/* COARSE_INTEGRATION_TIME */
+	{0x3014, 0x0BD8},	/* FINE_INTEGRATION_TIME */
+	{0x3010, 0x0078},	/* FINE_CORRECTION */
+	{0x301A, 0x8250},	/* RESET_REGISTER */
+	{0x301A, 0x8650},	/* RESET_REGISTER */
+	{0x301A, 0x8658},	/* RESET_REGISTER */
+	/* gain */
+	{0x305e, 0x10AA},	/* gain */
+	{0x0104, 0x0000},	/* GROUPED_PARAMETER_HOLD */
+	{0x301A, 0x065C},	/* RESET_REGISTER */
+	{ar0832_TABLE_END, 0x0000}
+};
+
+static struct ar0832_reg mode_2880X1620_8141[] = {
+	/* mode start */
+	{0x301A, 0x0058},	/* RESET_REGISTER */
+	{0x301A, 0x0050},	/* RESET_REGISTER */
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+	{0x3064, 0x7800},	/* RESERVED_MFR_3064 */
+	{0x31AE, 0x0202},	/* SERIAL_FORMAT */
+	/* AR0832 Recommended Settings */
+	{0x3044, 0x0590},
+	{0x306E, 0xFC80},
+	{0x30B2, 0xC000},
+	{0x30D6, 0x0800},
+	{0x316C, 0xB42F},
+	{0x316E, 0x869A},
+	{0x3170, 0x210E},
+	{0x317A, 0x010E},
+	{0x31E0, 0x1FB9},
+	{0x31E6, 0x07FC},
+	{0x37C0, 0x0000},
+	{0x37C2, 0x0000},
+	{0x37C4, 0x0000},
+	{0x37C6, 0x0000},
+	{0x3E00, 0x0011},
+	{0x3E02, 0x8801},
+	{0x3E04, 0x2801},
+	{0x3E06, 0x8449},
+	{0x3E08, 0x6841},
+	{0x3E0A, 0x400C},
+	{0x3E0C, 0x1001},
+	{0x3E0E, 0x2603},
+	{0x3E10, 0x4B41},
+	{0x3E12, 0x4B24},
+	{0x3E14, 0xA3CF},
+	{0x3E16, 0x8802},
+	{0x3E18, 0x8401},
+	{0x3E1A, 0x8601},
+	{0x3E1C, 0x8401},
+	{0x3E1E, 0x840A},
+	{0x3E20, 0xFF00},
+	{0x3E22, 0x8401},
+	{0x3E24, 0x00FF},
+	{0x3E26, 0x0088},
+	{0x3E28, 0x2E8A},
+	{0x3E30, 0x0000},
+	{0x3E32, 0x8801},
+	{0x3E34, 0x4029},
+	{0x3E36, 0x00FF},
+	{0x3E38, 0x8469},
+	{0x3E3A, 0x00FF},
+	{0x3E3C, 0x2801},
+	{0x3E3E, 0x3E2A},
+	{0x3E40, 0x1C01},
+	{0x3E42, 0xFF84},
+	{0x3E44, 0x8401},
+	{0x3E46, 0x0C01},
+	{0x3E48, 0x8401},
+	{0x3E4A, 0x00FF},
+	{0x3E4C, 0x8402},
+	{0x3E4E, 0x8984},
+	{0x3E50, 0x6628},
+	{0x3E52, 0x8340},
+	{0x3E54, 0x00FF},
+	{0x3E56, 0x4A42},
+	{0x3E58, 0x2703},
+	{0x3E5A, 0x6752},
+	{0x3E5C, 0x3F2A},
+	{0x3E5E, 0x846A},
+	{0x3E60, 0x4C01},
+	{0x3E62, 0x8401},
+	{0x3E66, 0x3901},
+	{0x3E90, 0x2C01},
+	{0x3E98, 0x2B02},
+	{0x3E92, 0x2A04},
+	{0x3E94, 0x2509},
+	{0x3E96, 0x0000},
+	{0x3E9A, 0x2905},
+	{0x3E9C, 0x00FF},
+	{0x3ECC, 0x00EB},
+	{0x3ED0, 0x1E24},
+	{0x3ED4, 0xAFC4},
+	{0x3ED6, 0x909B},
+	{0x3EE0, 0x2424},
+	{0x3EE2, 0x9797},
+	{0x3EE4, 0xC100},
+	{0x3EE6, 0x0540},
+	{0x3174, 0x8000},
+
+	/* mode end */
+	{0x0300, 0x0004},	/* VT_PIX_CLK_DIV */
+	{0x0302, 0x0001},	/* VT_SYS_CLK_DIV */
+	{0x0304, 0x0002},	/* PRE_PLL_CLK_DIV */
+	{0x0306, 0x0040},	/* PLL_MULTIPLIER */
+	{0x0308, 0x000A},	/* OP_PIX_CLK_DIV */
+	{0x030A, 0x0001},	/* OP_SYS_CLK_DIV */
+	{ar0832_TABLE_WAIT_MS, 0x0001},
+	{0x3064, 0x7400},	/* RESERVED_MFR_3064 */
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+	{0x0344, 0x00C8},	/* X_ADDR_START */
+	{0x0348, 0x0C07},	/* X_ADDR_END */
+	{0x0346, 0x01A6},	/* Y_ADDR_START */
+	{0x034A, 0x07F9},	/* Y_ADDR_END */
+	{0x034C, 0x0B40},	/* X_OUTPUT_SIZE */
+	{0x034E, 0x0654},	/* Y_OUTPUT_SIZE */
+	{0x3040, 0xC041},	/* READ_MODE */
+	{0x306E, 0xFC80},	/* DATAPATH_SELECT */
+	{0x0400, 0x0000},	/* SCALING_MODE */
+	{0x0404, 0x0010},	/* SCALE_M */
+	{0x3178, 0x0000},	/* RESERVED_MFR_3178 */
+	{0x3ED0, 0x1E24},	/* RESERVED_MFR_3ED0 */
+
+	{0x0342, 0x11B8},	/* LINE_LENGTH_PCK */
+	{0x0340, 0x06E3},	/* FRAME_LENGTH_LINES */
+	{0x0202, 0x06E3},	/* COARSE_INTEGRATION_TIME */
+	{0x3014, 0x0BD8},	/* FINE_INTEGRATION_TIME */
+	{0x3010, 0x0078},	/* FINE_CORRECTION */
+	{0x301A, 0x8250},	/* RESET_REGISTER */
+	{0x301A, 0x8650},	/* RESET_REGISTER */
+	{0x301A, 0x8658},	/* RESET_REGISTER */
+	/* gain */
+	{0x305e, 0x10AA},	/* gain */
+	{0x0104, 0x0000},	/* GROUPED_PARAMETER_HOLD */
+	{0x301A, 0x065C},	/* RESET_REGISTER */
+	{ar0832_TABLE_END, 0x0000}
+};
+
+static struct ar0832_reg mode_1920X1080_8140[] = {
+	/* mode start */
+	{0x301A, 0x0058},	/* RESET_REGISTER */
+	{0x301A, 0x0050},	/* RESET_REGISTER */
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+	{0x3064, 0x7800},	/* RESERVED_MFR_3064 */
+	{0x31AE, 0x0202},	/* SERIAL_FORMAT */
+	/* MT9E013 Recommended Settings */
+	{0x31B0, 0x0083},	/* FRAME_PREAMBLE */
+	{0x31B2, 0x004D},	/* LINE_PREAMBLE */
+	{0x31B4, 0x0E77},	/* MIPI_TIMING_0 */
+	{0x31B6, 0x0D20},	/* MIPI_TIMING_1 */
+	{0x31B8, 0x020E},	/* MIPI_TIMING_2 */
+	{0x31BA, 0x0710},	/* MIPI_TIMING_3 */
+	{0x31BC, 0x2A0D},	/* MIPI_TIMING_4 */
+	{ar0832_TABLE_WAIT_MS, 0x0005},
+	{0x0112, 0x0A0A},	/* CCP_DATA_FORMAT */
+	{0x3044, 0x0590},
+	{0x306E, 0xFC80},
+	{0x30B2, 0xC000},
+	{0x30D6, 0x0800},
+	{0x316C, 0xB42F},
+	{0x316E, 0x869A},
+	{0x3170, 0x210E},
+	{0x317A, 0x010E},
+	{0x31E0, 0x1FB9},
+	{0x31E6, 0x07FC},
+	{0x37C0, 0x0000},
+	{0x37C2, 0x0000},
+	{0x37C4, 0x0000},
+	{0x37C6, 0x0000},
+	{0x3E00, 0x0011},
+	{0x3E02, 0x8801},
+	{0x3E04, 0x2801},
+	{0x3E06, 0x8449},
+	{0x3E08, 0x6841},
+	{0x3E0A, 0x400C},
+	{0x3E0C, 0x1001},
+	{0x3E0E, 0x2603},
+	{0x3E10, 0x4B41},
+	{0x3E12, 0x4B24},
+	{0x3E14, 0xA3CF},
+	{0x3E16, 0x8802},
+	{0x3E18, 0x8401},
+	{0x3E1A, 0x8601},
+	{0x3E1C, 0x8401},
+	{0x3E1E, 0x840A},
+	{0x3E20, 0xFF00},
+	{0x3E22, 0x8401},
+	{0x3E24, 0x00FF},
+	{0x3E26, 0x0088},
+	{0x3E28, 0x2E8A},
+	{0x3E30, 0x0000},
+	{0x3E32, 0x8801},
+	{0x3E34, 0x4029},
+	{0x3E36, 0x00FF},
+	{0x3E38, 0x8469},
+	{0x3E3A, 0x00FF},
+	{0x3E3C, 0x2801},
+	{0x3E3E, 0x3E2A},
+	{0x3E40, 0x1C01},
+	{0x3E42, 0xFF84},
+	{0x3E44, 0x8401},
+	{0x3E46, 0x0C01},
+	{0x3E48, 0x8401},
+	{0x3E4A, 0x00FF},
+	{0x3E4C, 0x8402},
+	{0x3E4E, 0x8984},
+	{0x3E50, 0x6628},
+	{0x3E52, 0x8340},
+	{0x3E54, 0x00FF},
+	{0x3E56, 0x4A42},
+	{0x3E58, 0x2703},
+	{0x3E5A, 0x6752},
+	{0x3E5C, 0x3F2A},
+	{0x3E5E, 0x846A},
+	{0x3E60, 0x4C01},
+	{0x3E62, 0x8401},
+	{0x3E66, 0x3901},
+	{0x3E90, 0x2C01},
+	{0x3E98, 0x2B02},
+	{0x3E92, 0x2A04},
+	{0x3E94, 0x2509},
+	{0x3E96, 0x0000},
+	{0x3E9A, 0x2905},
+	{0x3E9C, 0x00FF},
+	{0x3ECC, 0x00EB},
+	{0x3ED0, 0x1E24},
+	{0x3ED4, 0xAFC4},
+	{0x3ED6, 0x909B},
+	{0x3EE0, 0x2424},
+	{0x3EE2, 0x9797},
+	{0x3EE4, 0xC100},
+	{0x3EE6, 0x0540},
+	{0x3174, 0x8000},
+
+	/* mode end */
+	{0x0300, 0x0004},	/* VT_PIX_CLK_DIV */
+	{0x0302, 0x0001},	/* VT_SYS_CLK_DIV */
+	{0x0304, 0x0002},	/* PRE_PLL_CLK_DIV */
+	{0x0306, 0x0040},	/* PLL_MULTIPLIER */
+	{0x0308, 0x000A},	/* OP_PIX_CLK_DIV */
+	{0x030A, 0x0001},	/* OP_SYS_CLK_DIV */
+	{ar0832_TABLE_WAIT_MS, 0x0001},
+	{0x3064, 0x7400},	/* RESERVED_MFR_3064 */
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+	{0x0344, 0x028C},	/* X_ADDR_START */
+	{0x0348, 0x0A0B},	/* X_ADDR_END */
+	{0x0346, 0x006E},	/* Y_ADDR_START */
+	{0x034A, 0x04A5},	/* Y_ADDR_END */
+	{0x034C, 0x0780},	/* X_OUTPUT_SIZE */
+	{0x034E, 0x0438},	/* Y_OUTPUT_SIZE */
+	{0x3040, 0xC041},	/* READ_MODE */
+	{0x306E, 0xFC80},	/* DATAPATH_SELECT */
+	{0x0400, 0x0000},	/* SCALING_MODE */
+	{0x0404, 0x0010},	/* SCALE_M */
+	{0x3178, 0x0000},	/* RESERVED_MFR_3178 */
+	{0x3ED0, 0x1E24},	/* RESERVED_MFR_3ED0 */
+
+	{0x0342, 0x1139},	/* LINE_LENGTH_PCK */
+	{0x0340, 0x05C4},	/* FRAME_LENGTH_LINES */
+	{0x0202, 0x05C4},	/* COARSE_INTEGRATION_TIME */
+	{0x3014, 0x0702},	/* FINE_INTEGRATION_TIME */
+	{0x3010, 0x0078},	/* FINE_CORRECTION */
+	{0x301A, 0x8250},	/* RESET_REGISTER */
+	{0x301A, 0x8650},	/* RESET_REGISTER */
+	{0x301A, 0x8658},	/* RESET_REGISTER */
+	/* gain */
+	{0x305e, 0x10AA},	/* gain */
+	{0x0104, 0x0000},	/* GROUPED_PARAMETER_HOLD */
+	{0x301A, 0x065C},	/* RESET_REGISTER */
+	{ar0832_TABLE_END, 0x0000}
+};
+
+static struct ar0832_reg mode_1920X1080_8141[] = {
+	/* mode start */
+	{0x301A, 0x0058},	/* RESET_REGISTER */
+	{0x301A, 0x0050},	/* RESET_REGISTER */
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+	{0x3064, 0x7800},	/* RESERVED_MFR_3064 */
+	{0x31AE, 0x0202},	/* SERIAL_FORMAT */
+	/* AR0832 Recommended Settings */
+	{0x3044, 0x0590},
+	{0x306E, 0xFC80},
+	{0x30B2, 0xC000},
+	{0x30D6, 0x0800},
+	{0x316C, 0xB42F},
+	{0x316E, 0x869A},
+	{0x3170, 0x210E},
+	{0x317A, 0x010E},
+	{0x31E0, 0x1FB9},
+	{0x31E6, 0x07FC},
+	{0x37C0, 0x0000},
+	{0x37C2, 0x0000},
+	{0x37C4, 0x0000},
+	{0x37C6, 0x0000},
+	{0x3E00, 0x0011},
+	{0x3E02, 0x8801},
+	{0x3E04, 0x2801},
+	{0x3E06, 0x8449},
+	{0x3E08, 0x6841},
+	{0x3E0A, 0x400C},
+	{0x3E0C, 0x1001},
+	{0x3E0E, 0x2603},
+	{0x3E10, 0x4B41},
+	{0x3E12, 0x4B24},
+	{0x3E14, 0xA3CF},
+	{0x3E16, 0x8802},
+	{0x3E18, 0x8401},
+	{0x3E1A, 0x8601},
+	{0x3E1C, 0x8401},
+	{0x3E1E, 0x840A},
+	{0x3E20, 0xFF00},
+	{0x3E22, 0x8401},
+	{0x3E24, 0x00FF},
+	{0x3E26, 0x0088},
+	{0x3E28, 0x2E8A},
+	{0x3E30, 0x0000},
+	{0x3E32, 0x8801},
+	{0x3E34, 0x4029},
+	{0x3E36, 0x00FF},
+	{0x3E38, 0x8469},
+	{0x3E3A, 0x00FF},
+	{0x3E3C, 0x2801},
+	{0x3E3E, 0x3E2A},
+	{0x3E40, 0x1C01},
+	{0x3E42, 0xFF84},
+	{0x3E44, 0x8401},
+	{0x3E46, 0x0C01},
+	{0x3E48, 0x8401},
+	{0x3E4A, 0x00FF},
+	{0x3E4C, 0x8402},
+	{0x3E4E, 0x8984},
+	{0x3E50, 0x6628},
+	{0x3E52, 0x8340},
+	{0x3E54, 0x00FF},
+	{0x3E56, 0x4A42},
+	{0x3E58, 0x2703},
+	{0x3E5A, 0x6752},
+	{0x3E5C, 0x3F2A},
+	{0x3E5E, 0x846A},
+	{0x3E60, 0x4C01},
+	{0x3E62, 0x8401},
+	{0x3E66, 0x3901},
+	{0x3E90, 0x2C01},
+	{0x3E98, 0x2B02},
+	{0x3E92, 0x2A04},
+	{0x3E94, 0x2509},
+	{0x3E96, 0x0000},
+	{0x3E9A, 0x2905},
+	{0x3E9C, 0x00FF},
+	{0x3ECC, 0x00EB},
+	{0x3ED0, 0x1E24},
+	{0x3ED4, 0xAFC4},
+	{0x3ED6, 0x909B},
+	{0x3EE0, 0x2424},
+	{0x3EE2, 0x9797},
+	{0x3EE4, 0xC100},
+	{0x3EE6, 0x0540},
+	{0x3174, 0x8000},
+
+	/* mode end */
+	{0x0300, 0x0004},	/* VT_PIX_CLK_DIV */
+	{0x0302, 0x0001},	/* VT_SYS_CLK_DIV */
+	{0x0304, 0x0002},	/* PRE_PLL_CLK_DIV */
+	{0x0306, 0x0040},	/* PLL_MULTIPLIER */
+	{0x0308, 0x000A},	/* OP_PIX_CLK_DIV */
+	{0x030A, 0x0001},	/* OP_SYS_CLK_DIV */
+	{ar0832_TABLE_WAIT_MS, 0x0001},
+	{0x3064, 0x7400},	/* RESERVED_MFR_3064 */
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+	{0x0344, 0x028C},	/* X_ADDR_START */
+	{0x0348, 0x0A0B},	/* X_ADDR_END */
+	{0x0346, 0x006E},	/* Y_ADDR_START */
+	{0x034A, 0x04A5},	/* Y_ADDR_END */
+	{0x034C, 0x0780},	/* X_OUTPUT_SIZE */
+	{0x034E, 0x0438},	/* Y_OUTPUT_SIZE */
+	{0x3040, 0xC041},	/* READ_MODE */
+	{0x306E, 0xFC80},	/* DATAPATH_SELECT */
+	{0x0400, 0x0000},	/* SCALING_MODE */
+	{0x0404, 0x0010},	/* SCALE_M */
+	{0x3178, 0x0000},	/* RESERVED_MFR_3178 */
+	{0x3ED0, 0x1E24},	/* RESERVED_MFR_3ED0 */
+
+	{0x0342, 0x1139},	/* LINE_LENGTH_PCK */
+	{0x0340, 0x05C4},	/* FRAME_LENGTH_LINES */
+	{0x0202, 0x05C4},	/* COARSE_INTEGRATION_TIME */
+	{0x3014, 0x0702},	/* FINE_INTEGRATION_TIME */
+	{0x3010, 0x0078},	/* FINE_CORRECTION */
+	{0x301A, 0x8250},	/* RESET_REGISTER */
+	{0x301A, 0x8650},	/* RESET_REGISTER */
+	{0x301A, 0x8658},	/* RESET_REGISTER */
+	/* gain */
+	{0x305e, 0x10AA},	/* gain */
+	{0x0104, 0x0000},	/* GROUPED_PARAMETER_HOLD */
+	{0x301A, 0x065C},	/* RESET_REGISTER */
+	{ar0832_TABLE_END, 0x0000}
+};
+
+static struct ar0832_reg mode_1632X1224_8140[] = {
+	/* mode start */
+	{0x301A, 0x0058},	/* RESET_REGISTER */
+	{0x301A, 0x0050},	/* RESET_REGISTER */
+
+	/* SC-CHANGE: to-do 8 bit write */
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+
+	{0x3064, 0x7800},	/* RESERVED_MFR_3064 */
+	{0x31AE, 0x0202},	/* SERIAL_FORMAT */
+	/* MT9E013 Recommended Settings */
+	{0x31B0, 0x0083},	/* FRAME_PREAMBLE */
+	{0x31B2, 0x004D},	/* LINE_PREAMBLE */
+	{0x31B4, 0x0E77},	/* MIPI_TIMING_0 */
+	{0x31B6, 0x0D20},	/* MIPI_TIMING_1 */
+	{0x31B8, 0x020E},	/* MIPI_TIMING_2 */
+	{0x31BA, 0x0710},	/* MIPI_TIMING_3 */
+	{0x31BC, 0x2A0D},	/* MIPI_TIMING_4 */
+	{ar0832_TABLE_WAIT_MS, 0x0005},
+	{0x0112, 0x0A0A},	/* CCP_DATA_FORMAT */
+	{0x3044, 0x0590},
+	{0x306E, 0xFC80},
+	{0x30B2, 0xC000},
+	{0x30D6, 0x0800},
+	{0x316C, 0xB42F},
+	{0x316E, 0x869A},
+	{0x3170, 0x210E},
+	{0x317A, 0x010E},
+	{0x31E0, 0x1FB9},
+	{0x31E6, 0x07FC},
+	{0x37C0, 0x0000},
+	{0x37C2, 0x0000},
+	{0x37C4, 0x0000},
+	{0x37C6, 0x0000},
+	{0x3E00, 0x0011},
+	{0x3E02, 0x8801},
+	{0x3E04, 0x2801},
+	{0x3E06, 0x8449},
+	{0x3E08, 0x6841},
+	{0x3E0A, 0x400C},
+	{0x3E0C, 0x1001},
+	{0x3E0E, 0x2603},
+	{0x3E10, 0x4B41},
+	{0x3E12, 0x4B24},
+	{0x3E14, 0xA3CF},
+	{0x3E16, 0x8802},
+	{0x3E18, 0x8401},
+	{0x3E1A, 0x8601},
+	{0x3E1C, 0x8401},
+	{0x3E1E, 0x840A},
+	{0x3E20, 0xFF00},
+	{0x3E22, 0x8401},
+	{0x3E24, 0x00FF},
+	{0x3E26, 0x0088},
+	{0x3E28, 0x2E8A},
+	{0x3E30, 0x0000},
+	{0x3E32, 0x8801},
+	{0x3E34, 0x4029},
+	{0x3E36, 0x00FF},
+	{0x3E38, 0x8469},
+	{0x3E3A, 0x00FF},
+	{0x3E3C, 0x2801},
+	{0x3E3E, 0x3E2A},
+	{0x3E40, 0x1C01},
+	{0x3E42, 0xFF84},
+	{0x3E44, 0x8401},
+	{0x3E46, 0x0C01},
+	{0x3E48, 0x8401},
+	{0x3E4A, 0x00FF},
+	{0x3E4C, 0x8402},
+	{0x3E4E, 0x8984},
+	{0x3E50, 0x6628},
+	{0x3E52, 0x8340},
+	{0x3E54, 0x00FF},
+	{0x3E56, 0x4A42},
+	{0x3E58, 0x2703},
+	{0x3E5A, 0x6752},
+	{0x3E5C, 0x3F2A},
+	{0x3E5E, 0x846A},
+	{0x3E60, 0x4C01},
+	{0x3E62, 0x8401},
+	{0x3E66, 0x3901},
+	{0x3E90, 0x2C01},
+	{0x3E98, 0x2B02},
+	{0x3E92, 0x2A04},
+	{0x3E94, 0x2509},
+	{0x3E96, 0x0000},
+	{0x3E9A, 0x2905},
+	{0x3E9C, 0x00FF},
+	{0x3ECC, 0x00EB},
+	{0x3ED0, 0x1E24},
+	{0x3ED4, 0xAFC4},
+	{0x3ED6, 0x909B},
+	{0x3EE0, 0x2424},
+	{0x3EE2, 0x9797},
+	{0x3EE4, 0xC100},
+	{0x3EE6, 0x0540},
+	{0x3174, 0x8000},
+
+	/* mode end */
+	{0x0300, 0x0004},	/* VT_PIX_CLK_DIV */
+	{0x0302, 0x0001},	/* VT_SYS_CLK_DIV */
+	{0x0304, 0x0002},	/* PRE_PLL_CLK_DIV */
+
+	{0x0306, 0x0040},	/* PLL_MULTIPLIER */
+
+	{0x0308, 0x000A},	/* OP_PIX_CLK_DIV */
+	{0x030A, 0x0001},	/* OP_SYS_CLK_DIV */
+	{ar0832_TABLE_WAIT_MS, 0x0001}, /* waitmsec 1 */
+
+	{0x3064, 0x7400},	/* RESERVED_MFR_3064 */
+
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+
+	{0x0344, 0x0008},	/* X_ADDR_START */
+	{0x0348, 0x0CC9},	/* X_ADDR_END */
+	{0x0346, 0x0008},	/* Y_ADDR_START */
+	{0x034A, 0x0999},	/* Y_ADDR_END */
+	{0x034C, 0x0660},	/* X_OUTPUT_SIZE */
+	{0x034E, 0x04C8},	/* Y_OUTPUT_SIZE */
+	{0x3040, 0xC4C3},	/* READ_MODE */
+	{0x306E, 0xFC80},	/* DATAPATH_SELECT */
+	{0x3178, 0x0000},	/* RESERVED_MFR_3178 */
+	{0x3ED0, 0x1E24},	/* RESERVED_MFR_3ED0 */
+	{0x0400, 0x0002},	/* SCALING_MODE */
+	{0x0404, 0x0010},	/* SCALE_M */
+	{0x0342, 0x101A},	/* LINE_LENGTH_PCK */
+	{0x0340, 0x0610},	/* FRAME_LENGTH_LINES */
+	{0x0202, 0x0557},	/* COARSE_INTEGRATION_TIME */
+	{0x3014, 0x0988},	/* FINE_INTEGRATION_TIME */
+	{0x3010, 0x0130},	/* FINE_CORRECTION */
+	{0x301A, 0x8250},	/* RESET_REGISTER */
+	{0x301A, 0x8650},	/* RESET_REGISTER */
+	{0x301A, 0x8658},	/* RESET_REGISTER */
+
+	/* gain */
+	{0x305e, 0x10AA},	/* gain */
+
+	/* todo 8-bit write */
+	{0x0104, 0x0000},	/* GROUPED_PARAMETER_HOLD */
+	{0x301A, 0x065C},	/* RESET_REGISTER */
+	{ar0832_TABLE_END, 0x0000}
+};
+
+static struct ar0832_reg mode_1632X1224_8141[] = {
+	/* mode start */
+	{0x301A, 0x0058},	/* RESET_REGISTER */
+	{0x301A, 0x0050},	/* RESET_REGISTER */
+
+	/* SC-CHANGE: to-do 8 bit write */
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+
+	{0x3064, 0x7800},	/* RESERVED_MFR_3064 */
+	{0x31AE, 0x0202},	/* SERIAL_FORMAT */
+	/* AR0832 Recommended Settings */
+	{0x31B0, 0x0083},	/* FRAME_PREAMBLE */
+	{0x31B2, 0x004D},	/* LINE_PREAMBLE */
+	{0x31B4, 0x0E77},	/* MIPI_TIMING_0 */
+	{0x31B6, 0x0D20},	/* MIPI_TIMING_1 */
+	{0x31B8, 0x020E},	/* MIPI_TIMING_2 */
+	{0x31BA, 0x0710},	/* MIPI_TIMING_3 */
+	{0x31BC, 0x2A0D},	/* MIPI_TIMING_4 */
+	{ar0832_TABLE_WAIT_MS, 0x0005},
+	{0x0112, 0x0A0A},	/* CCP_DATA_FORMAT */
+	{0x3044, 0x0590},
+	{0x306E, 0xFC80},
+	{0x30B2, 0xC000},
+	{0x30D6, 0x0800},
+	{0x316C, 0xB42F},
+	{0x316E, 0x869A},
+	{0x3170, 0x210E},
+	{0x317A, 0x010E},
+	{0x31E0, 0x1FB9},
+	{0x31E6, 0x07FC},
+	{0x37C0, 0x0000},
+	{0x37C2, 0x0000},
+	{0x37C4, 0x0000},
+	{0x37C6, 0x0000},
+	{0x3E00, 0x0011},
+	{0x3E02, 0x8801},
+	{0x3E04, 0x2801},
+	{0x3E06, 0x8449},
+	{0x3E08, 0x6841},
+	{0x3E0A, 0x400C},
+	{0x3E0C, 0x1001},
+	{0x3E0E, 0x2603},
+	{0x3E10, 0x4B41},
+	{0x3E12, 0x4B24},
+	{0x3E14, 0xA3CF},
+	{0x3E16, 0x8802},
+	{0x3E18, 0x8401},
+	{0x3E1A, 0x8601},
+	{0x3E1C, 0x8401},
+	{0x3E1E, 0x840A},
+	{0x3E20, 0xFF00},
+	{0x3E22, 0x8401},
+	{0x3E24, 0x00FF},
+	{0x3E26, 0x0088},
+	{0x3E28, 0x2E8A},
+	{0x3E30, 0x0000},
+	{0x3E32, 0x8801},
+	{0x3E34, 0x4029},
+	{0x3E36, 0x00FF},
+	{0x3E38, 0x8469},
+	{0x3E3A, 0x00FF},
+	{0x3E3C, 0x2801},
+	{0x3E3E, 0x3E2A},
+	{0x3E40, 0x1C01},
+	{0x3E42, 0xFF84},
+	{0x3E44, 0x8401},
+	{0x3E46, 0x0C01},
+	{0x3E48, 0x8401},
+	{0x3E4A, 0x00FF},
+	{0x3E4C, 0x8402},
+	{0x3E4E, 0x8984},
+	{0x3E50, 0x6628},
+	{0x3E52, 0x8340},
+	{0x3E54, 0x00FF},
+	{0x3E56, 0x4A42},
+	{0x3E58, 0x2703},
+	{0x3E5A, 0x6752},
+	{0x3E5C, 0x3F2A},
+	{0x3E5E, 0x846A},
+	{0x3E60, 0x4C01},
+	{0x3E62, 0x8401},
+	{0x3E66, 0x3901},
+	{0x3E90, 0x2C01},
+	{0x3E98, 0x2B02},
+	{0x3E92, 0x2A04},
+	{0x3E94, 0x2509},
+	{0x3E96, 0x0000},
+	{0x3E9A, 0x2905},
+	{0x3E9C, 0x00FF},
+	{0x3ECC, 0x00EB},
+	{0x3ED0, 0x1E24},
+	{0x3ED4, 0xAFC4},
+	{0x3ED6, 0x909B},
+	{0x3EE0, 0x2424},
+	{0x3EE2, 0x9797},
+	{0x3EE4, 0xC100},
+	{0x3EE6, 0x0540},
+	{0x3174, 0x8000},
+
+	/* mode end */
+	{0x0300, 0x0004},	/* VT_PIX_CLK_DIV */
+	{0x0302, 0x0001},	/* VT_SYS_CLK_DIV */
+	{0x0304, 0x0002},	/* PRE_PLL_CLK_DIV */
+
+	{0x0306, 0x0040},	/* PLL_MULTIPLIER */
+
+	{0x0308, 0x000A},	/* OP_PIX_CLK_DIV */
+	{0x030A, 0x0001},	/* OP_SYS_CLK_DIV */
+	{ar0832_TABLE_WAIT_MS, 0x0001}, /* waitmsec 1 */
+
+	{0x3064, 0x7400},	/* RESERVED_MFR_3064 */
+
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+
+	{0x0344, 0x0008},	/* X_ADDR_START */
+	{0x0348, 0x0CC9},	/* X_ADDR_END */
+	{0x0346, 0x0008},	/* Y_ADDR_START */
+	{0x034A, 0x0999},	/* Y_ADDR_END */
+	{0x034C, 0x0660},	/* X_OUTPUT_SIZE */
+	{0x034E, 0x04C8},	/* Y_OUTPUT_SIZE */
+	{0x3040, 0xC4C3},	/* READ_MODE */
+	{0x306E, 0xFC80},	/* DATAPATH_SELECT */
+	{0x3178, 0x0000},	/* RESERVED_MFR_3178 */
+	{0x3ED0, 0x1E24},	/* RESERVED_MFR_3ED0 */
+	{0x0400, 0x0002},	/* SCALING_MODE */
+	{0x0404, 0x0010},	/* SCALE_M */
+	{0x0342, 0x101A},	/* LINE_LENGTH_PCK */
+	{0x0340, 0x0610},	/* FRAME_LENGTH_LINES */
+	{0x0202, 0x0557},	/* COARSE_INTEGRATION_TIME */
+	{0x3014, 0x0988},	/* FINE_INTEGRATION_TIME */
+	{0x3010, 0x0130},	/* FINE_CORRECTION */
+	{0x301A, 0x8250},	/* RESET_REGISTER */
+	{0x301A, 0x8650},	/* RESET_REGISTER */
+	{0x301A, 0x8658},	/* RESET_REGISTER */
+
+	/* gain */
+	{0x305e, 0x10AA},	/* gain */
+
+	/* todo 8-bit write */
+	{0x0104, 0x0000},	/* GROUPED_PARAMETER_HOLD */
+	{0x301A, 0x065C},	/* RESET_REGISTER */
+	{ar0832_TABLE_END, 0x0000}
+};
+
+static struct ar0832_reg mode_800X600_8140[] = {
+	/* mode start */
+	{0x301A, 0x0058},
+	{0x301A, 0x0050},
+	{0x0104, 0x0100},
+	{0x3064, 0x7800},
+	{0x31AE, 0x0202},
+	{0x31B8, 0x0E3F},
+	{0x31BE, 0xC003},
+	{0x3070, 0x0000},
+	{ar0832_TABLE_WAIT_MS, 0x0005},
+
+	/* MT9E013 Recommended Settings */
+	{0x3044, 0x0590},
+	{0x306E, 0xFC80},
+	{0x30B2, 0xC000},
+	{0x30D6, 0x0800},
+	{0x316C, 0xB42F},
+	{0x316E, 0x869A},
+	{0x3170, 0x210E},
+	{0x317A, 0x010E},
+	{0x31E0, 0x1FB9},
+	{0x31E6, 0x07FC},
+	{0x37C0, 0x0000},
+	{0x37C2, 0x0000},
+	{0x37C4, 0x0000},
+	{0x37C6, 0x0000},
+	{0x3E00, 0x0011},
+	{0x3E02, 0x8801},
+	{0x3E04, 0x2801},
+	{0x3E06, 0x8449},
+	{0x3E08, 0x6841},
+	{0x3E0A, 0x400C},
+	{0x3E0C, 0x1001},
+	{0x3E0E, 0x2603},
+	{0x3E10, 0x4B41},
+	{0x3E12, 0x4B24},
+	{0x3E14, 0xA3CF},
+	{0x3E16, 0x8802},
+	{0x3E18, 0x8401},
+	{0x3E1A, 0x8601},
+	{0x3E1C, 0x8401},
+	{0x3E1E, 0x840A},
+	{0x3E20, 0xFF00},
+	{0x3E22, 0x8401},
+	{0x3E24, 0x00FF},
+	{0x3E26, 0x0088},
+	{0x3E28, 0x2E8A},
+	{0x3E30, 0x0000},
+	{0x3E32, 0x8801},
+	{0x3E34, 0x4029},
+	{0x3E36, 0x00FF},
+	{0x3E38, 0x8469},
+	{0x3E3A, 0x00FF},
+	{0x3E3C, 0x2801},
+	{0x3E3E, 0x3E2A},
+	{0x3E40, 0x1C01},
+	{0x3E42, 0xFF84},
+	{0x3E44, 0x8401},
+	{0x3E46, 0x0C01},
+	{0x3E48, 0x8401},
+	{0x3E4A, 0x00FF},
+	{0x3E4C, 0x8402},
+	{0x3E4E, 0x8984},
+	{0x3E50, 0x6628},
+	{0x3E52, 0x8340},
+	{0x3E54, 0x00FF},
+	{0x3E56, 0x4A42},
+	{0x3E58, 0x2703},
+	{0x3E5A, 0x6752},
+	{0x3E5C, 0x3F2A},
+	{0x3E5E, 0x846A},
+	{0x3E60, 0x4C01},
+	{0x3E62, 0x8401},
+	{0x3E66, 0x3901},
+	{0x3E90, 0x2C01},
+	{0x3E98, 0x2B02},
+	{0x3E92, 0x2A04},
+	{0x3E94, 0x2509},
+	{0x3E96, 0x0000},
+	{0x3E9A, 0x2905},
+	{0x3E9C, 0x00FF},
+	{0x3ECC, 0x00EB},
+	{0x3ED0, 0x1E24},
+	{0x3ED4, 0xAFC4},
+	{0x3ED6, 0x909B},
+	{0x3EE0, 0x2424},
+	{0x3EE2, 0x9797},
+	{0x3EE4, 0xC100},
+	{0x3EE6, 0x0540},
+
+	/* mode end */
+	{0x3174, 0x8000},
+
+	/* [RAW10] */
+	{0x0112, 0x0A0A},
+
+	/* PLL Configuration Ext=24MHz */
+	{0x0300, 0x0004},
+	{0x0302, 0x0001},
+	{0x0304, 0x0002},
+	{0x0306, 0x0042},
+	{0x0308, 0x000A},
+	{0x030A, 0x0001},
+	{ar0832_TABLE_WAIT_MS, 0x0001},
+
+	/* Output size */
+	{0x0344, 0x04D8},
+	{0x0348, 0x07F7},
+	{0x0346, 0x03A4},
+	{0x034A, 0x05FB},
+	{0x034C, 0x0320},
+	{0x034E, 0x0258},
+	{0x3040, 0xC041},
+
+	{0x306E, 0xFC80},
+	{0x3178, 0x0000},
+	{0x3ED0, 0x1E24},
+
+	/* Scale Configuration */
+	{0x0400, 0x0000},
+	{0x0404, 0x0010},
+
+	/* Timing Configuration */
+	{0x0342, 0x08A8},
+	{0x0340, 0x02E7},
+	{0x0202, 0x02E7},
+	{0x3014, 0x03F6},
+	{0x3010, 0x0078},
+
+	{0x301A, 0x8250},
+	{0x301A, 0x8650},
+	{0x301A, 0x8658},
+	/* STATE= Minimum Gain, 1500 */
+	{0x305E, 0x13AF},
+
+	{0x0104, 0x0000},
+	{0x301A, 0x065C},
+	{ar0832_TABLE_END, 0x0000}
+};
+
+static struct ar0832_reg mode_800X600_8141[] = {
+	/* mode start */
+	{0x301A, 0x0058},	/* RESET_REGISTER */
+	{0x301A, 0x0050},	/* RESET_REGISTER */
+
+	/* SC-CHANGE: to-do 8 bit write */
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+
+	{0x3064, 0x7800},	/* RESERVED_MFR_3064 */
+	{0x31AE, 0x0202},	/* SERIAL_FORMAT */
+	/* AR0832 Recommended Settings */
+	{0x31B0, 0x0083},	/* FRAME_PREAMBLE */
+	{0x31B2, 0x004D},	/* LINE_PREAMBLE */
+	{0x31B4, 0x0E88},	/* MIPI_TIMING_0 */
+	{0x31B6, 0x0D24},	/* MIPI_TIMING_1 */
+	{0x31B8, 0x020E},	/* MIPI_TIMING_2 */
+	{0x31BA, 0x0710},	/* MIPI_TIMING_3 */
+	{0x31BC, 0x2A0D},	/* MIPI_TIMING_4 */
+	{ar0832_TABLE_WAIT_MS, 0x0005},
+	{0x0112, 0x0A0A},	/* CCP_DATA_FORMAT */
+	{0x3044, 0x0590},	/* RESERVED_MFR_3044 */
+	{0x306E, 0xFC80},	/* DATAPATH_SELECT */
+	{0x30B2, 0xC000},	/* RESERVED_MFR_30B2 */
+	{0x30D6, 0x0800},	/* RESERVED_MFR_30D6 */
+	{0x316C, 0xB42F},	/* RESERVED_MFR_316C */
+	{0x316E, 0x869A},	/* RESERVED_MFR_316E */
+	{0x3170, 0x210E},	/* RESERVED_MFR_3170 */
+	{0x317A, 0x010E},	/* RESERVED_MFR_317A */
+	{0x31E0, 0x1FB9},	/* RESERVED_MFR_31E0 */
+	{0x31E6, 0x07FC},	/* RESERVED_MFR_31E6 */
+	{0x37C0, 0x0000},	/* P_GR_Q5 */
+	{0x37C2, 0x0000},	/* P_RD_Q5 */
+	{0x37C4, 0x0000},	/* P_BL_Q5 */
+	{0x37C6, 0x0000},	/* P_GB_Q5 */
+	{0x3E00, 0x0011},	/* RESERVED_MFR_3E00 */
+	{0x3E02, 0x8801},	/* RESERVED_MFR_3E02 */
+	{0x3E04, 0x2801},	/* RESERVED_MFR_3E04 */
+	{0x3E06, 0x8449},	/* RESERVED_MFR_3E06 */
+	{0x3E08, 0x6841},	/* RESERVED_MFR_3E08 */
+	{0x3E0A, 0x400C},	/* RESERVED_MFR_3E0A */
+	{0x3E0C, 0x1001},	/* RESERVED_MFR_3E0C */
+	{0x3E0E, 0x2603},	/* RESERVED_MFR_3E0E */
+	{0x3E10, 0x4B41},	/* RESERVED_MFR_3E10 */
+	{0x3E12, 0x4B24},	/* RESERVED_MFR_3E12 */
+	{0x3E14, 0xA3CF},	/* RESERVED_MFR_3E14 */
+	{0x3E16, 0x8802},	/* RESERVED_MFR_3E16 */
+	{0x3E18, 0x84FF},	/* RESERVED_MFR_3E18 */
+	{0x3E1A, 0x8601},	/* RESERVED_MFR_3E1A */
+	{0x3E1C, 0x8401},	/* RESERVED_MFR_3E1C */
+	{0x3E1E, 0x840A},	/* RESERVED_MFR_3E1E */
+	{0x3E20, 0xFF00},	/* RESERVED_MFR_3E20 */
+	{0x3E22, 0x8401},	/* RESERVED_MFR_3E22 */
+	{0x3E24, 0x00FF},	/* RESERVED_MFR_3E24 */
+	{0x3E26, 0x0088},	/* RESERVED_MFR_3E26 */
+	{0x3E28, 0x2E8A},	/* RESERVED_MFR_3E28 */
+	{0x3E30, 0x0000},	/* RESERVED_MFR_3E30 */
+	{0x3E32, 0x8801},	/* RESERVED_MFR_3E32 */
+	{0x3E34, 0x4029},	/* RESERVED_MFR_3E34 */
+	{0x3E36, 0x00FF},	/* RESERVED_MFR_3E36 */
+	{0x3E38, 0x8469},	/* RESERVED_MFR_3E38 */
+	{0x3E3A, 0x00FF},	/* RESERVED_MFR_3E3A */
+	{0x3E3C, 0x2801},	/* RESERVED_MFR_3E3C */
+	{0x3E3E, 0x3E2A},	/* RESERVED_MFR_3E3E */
+	{0x3E40, 0x1C01},	/* RESERVED_MFR_3E40 */
+	{0x3E42, 0xFF84},	/* RESERVED_MFR_3E42 */
+	{0x3E44, 0x8401},	/* RESERVED_MFR_3E44 */
+	{0x3E46, 0x0C01},	/* RESERVED_MFR_3E46 */
+	{0x3E48, 0x8401},	/* RESERVED_MFR_3E48 */
+	{0x3E4A, 0x00FF},	/* RESERVED_MFR_3E4A */
+	{0x3E4C, 0x8402},	/* RESERVED_MFR_3E4C */
+	{0x3E4E, 0x8984},	/* RESERVED_MFR_3E4E */
+	{0x3E50, 0x6628},	/* RESERVED_MFR_3E50 */
+	{0x3E52, 0x8340},	/* RESERVED_MFR_3E52 */
+	{0x3E54, 0x00FF},	/* RESERVED_MFR_3E54 */
+	{0x3E56, 0x4A42},	/* RESERVED_MFR_3E56 */
+	{0x3E58, 0x2703},	/* RESERVED_MFR_3E58 */
+	{0x3E5A, 0x6752},	/* RESERVED_MFR_3E5A */
+	{0x3E5C, 0x3F2A},	/* RESERVED_MFR_3E5C */
+	{0x3E5E, 0x846A},	/* RESERVED_MFR_3E5E */
+	{0x3E60, 0x4C01},	/* RESERVED_MFR_3E60 */
+	{0x3E62, 0x8401},	/* RESERVED_MFR_3E62 */
+	{0x3E66, 0x3901},	/* RESERVED_MFR_3E66 */
+	{0x3E90, 0x2C01},	/* RESERVED_MFR_3E90 */
+	{0x3E98, 0x2B02},	/* RESERVED_MFR_3E98 */
+	{0x3E92, 0x2A04},	/* RESERVED_MFR_3E92 */
+	{0x3E94, 0x2509},	/* RESERVED_MFR_3E94 */
+	{0x3E96, 0x0000},	/* RESERVED_MFR_3E96 */
+	{0x3E9A, 0x2905},	/* RESERVED_MFR_3E9A */
+	{0x3E9C, 0x00FF},	/* RESERVED_MFR_3E9C */
+	{0x3ECC, 0x00EB},	/* RESERVED_MFR_3ECC */
+	{0x3ED0, 0x1E24},	/* RESERVED_MFR_3ED0 */
+	{0x3ED4, 0xAFC4},	/* RESERVED_MFR_3ED4 */
+	{0x3ED6, 0x909B},	/* RESERVED_MFR_3ED6 */
+	{0x3EE0, 0x2424},	/* RESERVED_MFR_3EE0 */
+	{0x3EE2, 0x9797},	/* RESERVED_MFR_3EE2 */
+	{0x3EE4, 0xC100},	/* RESERVED_MFR_3EE4 */
+	{0x3EE6, 0x0540},	/* RESERVED_MFR_3EE6 */
+
+	/* mode end */
+	{0x3174, 0x8000},	/* RESERVED_MFR_3174 */
+	{0x0300, 0x0004},	/* VT_PIX_CLK_DIV */
+	{0x0302, 0x0001},	/* VT_SYS_CLK_DIV */
+	{0x0304, 0x0002},	/* PRE_PLL_CLK_DIV */
+
+	{0x0306, 0x0042},	/* PLL_MULTIPLIER */
+
+	{0x0308, 0x000A},	/* OP_PIX_CLK_DIV */
+	{0x030A, 0x0001},	/* OP_SYS_CLK_DIV */
+	{ar0832_TABLE_WAIT_MS, 0x0001}, /* waitmsec 1 */
+
+	{0x3064, 0x7400},	/* RESERVED_MFR_3064 */
+
+	{0x0104, 0x0100},	/* GROUPED_PARAMETER_HOLD */
+
+	{0x0344, 0x04D8},	/* X_ADDR_START */
+	{0x0348, 0x07F7},	/* X_ADDR_END */
+	{0x0346, 0x03A4},	/* Y_ADDR_START */
+	{0x034A, 0x05FB},	/* Y_ADDR_END */
+	{0x034C, 0x0320},	/* X_OUTPUT_SIZE */
+	{0x034E, 0x0260},	/* Y_OUTPUT_SIZE */
+	{0x3040, 0xC041},	/* READ_MODE */
+	{0x306E, 0xFC80},	/* DATAPATH_SELECT */
+	{0x3178, 0x0000},	/* RESERVED_MFR_3178 */
+	{0x3ED0, 0x1E24},	/* RESERVED_MFR_3ED0 */
+	{0x0400, 0x0000},	/* SCALING_MODE */
+	{0x0404, 0x0010},	/* SCALE_M */
+	{0x0342, 0x08A8},	/* LINE_LENGTH_PCK */
+	{0x0340, 0x02E7},	/* FRAME_LENGTH_LINES */
+	{0x0202, 0x02E7},	/* COARSE_INTEGRATION_TIME */
+	{0x3014, 0x03F6},	/* FINE_INTEGRATION_TIME */
+	{0x3010, 0x0078},	/* FINE_CORRECTION */
+	{0x301A, 0x8250},	/* RESET_REGISTER */
+	{0x301A, 0x8650},	/* RESET_REGISTER */
+	{0x301A, 0x8658},	/* RESET_REGISTER */
+
+	/* gain */
+	{0x305e, 0x10AA},	/* gain */
+
+	/* todo 8-bit write */
+	{0x0104, 0x0000},	/* GROUPED_PARAMETER_HOLD */
+	{0x301A, 0x065C},	/* RESET_REGISTER */
+	{ar0832_TABLE_END, 0x0000}
+};
+
+static struct ar0832_reg mode_end[] = {
+	{ar0832_TABLE_END, 0x0000}
+};
+
+enum {
+	ar0832_MODE_3264X2448,
+	ar0832_MODE_2880X1620,
+	ar0832_MODE_1920X1080,
+	ar0832_MODE_1632X1224,
+	ar0832_MODE_800X600
+};
+
+static struct ar0832_reg *mode_table_8140[] = {
+	[ar0832_MODE_3264X2448] = mode_3264X2448_8140,
+	[ar0832_MODE_2880X1620] = mode_2880X1620_8140,
+	[ar0832_MODE_1920X1080] = mode_1920X1080_8140,
+	[ar0832_MODE_1632X1224] = mode_1632X1224_8140,
+	[ar0832_MODE_800X600]   = mode_800X600_8140,
+};
+
+static struct ar0832_reg *mode_table_8141[] = {
+	[ar0832_MODE_3264X2448] = mode_3264X2448_8141,
+	[ar0832_MODE_2880X1620] = mode_2880X1620_8141,
+	[ar0832_MODE_1920X1080] = mode_1920X1080_8141,
+	[ar0832_MODE_1632X1224] = mode_1632X1224_8141,
+	[ar0832_MODE_800X600]   = mode_800X600_8141,
+};
+
+static inline void ar0832_msleep(u32 t)
+{
+	/*
+	why usleep_range() instead of msleep() ?
+	Read Documentation/timers/timers-howto.txt
+	*/
+	usleep_range(t*1000, t*1000 + 500);
+}
+
+/* 16 bit reg to program frame length */
+static inline void ar0832_get_frame_length_regs(struct ar0832_reg *regs,
+						u32 frame_length)
+{
+	regs->addr = 0x0340;
+	regs->val = (frame_length) & 0xFFFF;
+}
+
+static inline void ar0832_get_coarse_time_regs(struct ar0832_reg *regs,
+						u32 coarse_time)
+{
+	regs->addr = 0x0202;
+	regs->val = (coarse_time) & 0xFFFF;
+}
+
+static inline void ar0832_get_focuser_vcm_control_regs(struct ar0832_reg *regs,
+							u16 value)
+{
+	regs->addr = 0x30F0;
+	regs->val = (value) & 0xFFFF;
+}
+
+static inline void ar0832_get_focuser_vcm_step_time_regs
+	(struct ar0832_reg *regs, u16 value)
+{
+	regs->addr = 0x30F4;
+	regs->val = (value) & 0xFFFF;
+}
+
+static inline void ar0832_get_focuser_data_regs(struct ar0832_reg *regs,
+						u16 value)
+{
+	regs->addr = 0x30F2;
+	regs->val = (value) & 0xFFFF;
+}
+
+static inline void ar0832_get_gain_regs(struct ar0832_reg *regs, u16 gain)
+{
+	/* global_gain register*/
+	regs->addr = AR0832_GLOBAL_GAIN_REG;
+	regs->val = gain;
+}
+
+static int ar0832_write_reg8(struct i2c_client *client, u16 addr, u8 val)
+{
+	int err;
+	struct i2c_msg msg;
+	unsigned char data[3];
+	int retry = 0;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = (u8) (addr >> 8);;
+	data[1] = (u8) (addr & 0xff);
+	data[2] = (u8) (val & 0xff);
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = data;
+
+	dev_dbg(&client->dev, "0x%x = 0x%x\n", addr, val);
+
+	do {
+		err = i2c_transfer(client->adapter, &msg, 1);
+		if (err > 0)
+			return 0;
+		retry++;
+		dev_err(&client->dev,
+			"%s: i2c transfer failed, retrying %x %x\n",
+			__func__, addr, val);
+		ar0832_msleep(3);
+	} while (retry < ar0832_MAX_RETRIES);
+
+	return err;
+}
+
+static int ar0832_write_reg16(struct i2c_client *client, u16 addr, u16 val)
+{
+	int count;
+	struct i2c_msg msg;
+	unsigned char data[4];
+	int retry = 0;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = (u8) (addr >> 8);
+	data[1] = (u8) (addr & 0xff);
+	data[2] = (u8) (val >> 8);
+	data[3] = (u8) (val & 0xff);
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 4;
+	msg.buf = data;
+
+	dev_dbg(&client->dev, "0x%x = 0x%x\n", addr, val);
+
+	do {
+		count = i2c_transfer(client->adapter, &msg, 1);
+		if (count == 1)
+			return 0;
+		retry++;
+		dev_err(&client->dev,
+			"%s: i2c transfer failed, retrying %x %x\n",
+		       __func__, addr, val);
+		ar0832_msleep(3);
+	} while (retry <= ar0832_MAX_RETRIES);
+
+	return -EIO;
+}
+
+static int ar0832_read_reg16(struct i2c_client *client, u16 addr, u16 *val)
+{
+	struct i2c_msg msg[2];
+	u8 data[4];
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 2;
+	msg[0].buf = data;
+	data[0] = (addr >> 8);
+	data[1] = (addr & 0xff);
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 2;
+	msg[1].buf = data + 2;
+
+	if (i2c_transfer(client->adapter, msg, 2) == 2) {
+		*val = ((data[2] << 8) | data[3]);
+		dev_dbg(&client->dev, "0x%x = 0x%x\n", addr, *val);
+		return 0;
+	} else {
+		*val = 0;
+		dev_err(&client->dev,
+			"%s: i2c read failed.\n", __func__);
+		return -1;
+	}
+}
+
+static int ar0832_write_reg_helper(struct ar0832_dev *dev,
+					u16 addr,
+					u16 val)
+{
+	int ret;
+
+	if (addr == 0x104)
+		ret = ar0832_write_reg8(dev->i2c_client, addr,
+					(val >> 8 & 0xff));
+	else
+		ret = ar0832_write_reg16(dev->i2c_client, addr, val);
+
+	return ret;
+}
+
+static int ar0832_write_table(struct ar0832_dev *dev,
+				const struct ar0832_reg table[],
+				const struct ar0832_reg override_list[],
+				int num_override_regs)
+{
+	int err;
+	const struct ar0832_reg *next;
+	u16 val;
+	int i;
+
+	for (next = table; next->addr != ar0832_TABLE_END; next++) {
+		if (next->addr ==  ar0832_TABLE_WAIT_MS) {
+			ar0832_msleep(next->val);
+			continue;
+		}
+
+		val = next->val;
+		/* When an override list is passed in, replace the reg */
+		/* value to write if the reg is in the list            */
+		if (override_list) {
+			for (i = 0; i < num_override_regs; i++) {
+				if (next->addr == override_list[i].addr) {
+					val = override_list[i].val;
+					break;
+				}
+			}
+		}
+
+		err = ar0832_write_reg_helper(dev, next->addr, val);
+		if (err)
+			return err;
+	}
+	return 0;
+}
+
+static int ar0832_set_frame_length(struct ar0832_dev *dev,
+					u32 frame_length)
+{
+	struct ar0832_reg reg_list;
+	struct i2c_client *i2c_client = dev->i2c_client;
+	int ret;
+
+	dev_dbg(&i2c_client->dev, "[%s] (0x%08x)\n", __func__,  frame_length);
+
+	ar0832_get_frame_length_regs(&reg_list, frame_length);
+	ret = ar0832_write_reg8(i2c_client, AR0832_GROUP_HOLD_REG, 0x1);
+	if (ret)
+		return ret;
+
+	ret = ar0832_write_reg16(i2c_client, reg_list.addr,
+			reg_list.val);
+	if (ret)
+		return ret;
+
+	ret = ar0832_write_reg8(i2c_client, AR0832_GROUP_HOLD_REG, 0x0);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int ar0832_set_coarse_time(struct ar0832_dev *dev,
+				  u32 coarse_time)
+{
+	int ret;
+	struct ar0832_reg reg_list;
+	struct i2c_client *i2c_client = dev->i2c_client;
+
+	dev_dbg(&i2c_client->dev, "[%s] (0x%08x)\n", __func__,  coarse_time);
+	ar0832_get_coarse_time_regs(&reg_list, coarse_time);
+
+	ret = ar0832_write_reg8(i2c_client, AR0832_GROUP_HOLD_REG, 0x1);
+	if (ret)
+		return ret;
+
+	ret = ar0832_write_reg16(i2c_client, reg_list.addr,
+			reg_list.val);
+	if (ret)
+		return ret;
+
+	ret = ar0832_write_reg8(i2c_client, AR0832_GROUP_HOLD_REG, 0x0);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int ar0832_set_gain(struct ar0832_dev *dev, u16 gain)
+{
+	int ret = 0;
+	struct ar0832_reg reg_list_gain;
+
+	ret = ar0832_write_reg8(dev->i2c_client, AR0832_GROUP_HOLD_REG, 0x1);
+	/* Gain Registers Start */
+	ar0832_get_gain_regs(&reg_list_gain, gain);
+	ret |= ar0832_write_reg16(dev->i2c_client,
+				reg_list_gain.addr,
+				reg_list_gain.val);
+	if (ret)
+		return ret;
+
+	/* Gain register End */
+	ret |= ar0832_write_reg8(dev->i2c_client, AR0832_GROUP_HOLD_REG, 0x0);
+
+	return ret;
+}
+
+static int ar0832_set_mode(struct ar0832_dev *dev,
+				struct ar0832_mode *mode)
+{
+	int sensor_mode;
+	int err;
+	struct i2c_client *i2c_client = dev->i2c_client;
+	struct ar0832_reg reg_ovr[3];
+	struct ar0832_reg *mode_seq;
+
+	dev_dbg(&i2c_client->dev, "%s: ++\n", __func__);
+
+	if (mode->xres == 3264 && mode->yres == 2448)
+		sensor_mode = ar0832_MODE_3264X2448;
+	else if (mode->xres == 2880 && mode->yres == 1620)
+		sensor_mode = ar0832_MODE_2880X1620;
+	else if (mode->xres == 1920 && mode->yres == 1080)
+		sensor_mode = ar0832_MODE_1920X1080;
+	else if (mode->xres == 1632 && mode->yres == 1224)
+		sensor_mode = ar0832_MODE_1632X1224;
+	else if (mode->xres == 800 && mode->yres == 600)
+		sensor_mode = ar0832_MODE_800X600;
+	else {
+		dev_err(&i2c_client->dev,
+			"%s: invalid resolution supplied to set mode %d %d\n",
+			__func__ , mode->xres, mode->yres);
+		return -EINVAL;
+	}
+
+	if (dev->sensor_id_data == AR0832_SENSOR_ID_8141)
+		mode_seq = mode_table_8141[sensor_mode];
+	else
+		mode_seq = mode_table_8140[sensor_mode];
+	/* get a list of override regs for the asking frame length,    */
+	/* coarse integration time, and gain.*/
+	err = ar0832_write_table(dev, mode_start, NULL, 0);
+	if (err)
+		return err;
+
+	/* When we change the resolution */
+	ar0832_get_frame_length_regs(&reg_ovr[0], mode->frame_length);
+	ar0832_get_coarse_time_regs(&reg_ovr[1], mode->coarse_time);
+	ar0832_get_gain_regs(&reg_ovr[2], mode->gain);
+	err = ar0832_write_table(dev, mode_seq, reg_ovr, ARRAY_SIZE(reg_ovr));
+	if (err)
+		return err;
+
+	err = ar0832_write_table(dev, mode_end, NULL, 0);
+	if (err)
+		return err;
+
+	dev->sensor_info->mode = sensor_mode;
+	dev_dbg(&i2c_client->dev, "%s: --\n", __func__);
+
+	return 0;
+}
+
+static int ar0832_get_status(struct ar0832_dev *dev, u8 *status)
+{
+	int err = 0;
+	struct i2c_client *i2c_client = dev->i2c_client;
+
+	*status = 0;
+	/* FixMe */
+	/*
+	err = ar0832_read_reg(dev->i2c_client, 0x001, status);
+	*/
+	dev_dbg(&i2c_client->dev, "%s: %u %d\n", __func__, *status, err);
+	return err;
+}
+
+static int ar0832_set_alternate_addr(struct i2c_client *client)
+{
+	int ret = 0;
+	u8 new_addr = client->addr;
+	u16 val;
+
+	/* Default slave address of ar0832 is 0x36 */
+	client->addr = 0x36;
+	ret = ar0832_read_reg16(client, AR0832_RESET_REG, &val);
+	val &= ~AR0832_RESET_REG_LOCK_REG;
+	ret |= ar0832_write_reg16(client, AR0832_RESET_REG, val);
+	ret |= ar0832_write_reg16(client, AR0832_ID_REG, new_addr << 1);
+
+	if (!ret) {
+		client->addr = new_addr;
+		dev_dbg(&client->dev,
+			"new slave address is set to 0x%x\n", new_addr);
+	}
+
+	ret |= ar0832_read_reg16(client, AR0832_RESET_REG, &val);
+	val |= AR0832_RESET_REG_LOCK_REG;
+	ret |= ar0832_write_reg16(client, AR0832_RESET_REG, val);
+
+	return ret;
+}
+
+static int ar0832_power_on(struct ar0832_dev *dev)
+{
+	struct i2c_client *i2c_client = dev->i2c_client;
+	int ret = 0;
+
+	dev_dbg(&i2c_client->dev, "%s: ++ %d %d\n",
+		__func__, dev->is_stereo,
+		dev->brd_power_cnt);
+
+	/* Board specific power-on sequence */
+	mutex_lock(&dev->ar0832_camera_lock);
+	if (dev->brd_power_cnt == 0) {
+		/* Plug 1.8V and 2.8V power to sensor */
+		if (dev->power_rail.sen_1v8_reg) {
+			ret = regulator_enable(dev->power_rail.sen_1v8_reg);
+			if (ret) {
+				dev_err(&i2c_client->dev,
+					"%s: failed to enable vdd\n",
+					__func__);
+				goto fail_regulator_1v8_reg;
+			}
+		}
+
+		if (dev->power_rail.sen_2v8_reg) {
+			ret = regulator_enable(dev->power_rail.sen_2v8_reg);
+			if (ret) {
+				dev_err(&i2c_client->dev,
+					"%s: failed to enable vaa\n",
+					__func__);
+				goto fail_regulator_2v8_reg;
+			}
+		}
+		dev->pdata->power_on(dev->is_stereo);
+	}
+	dev->brd_power_cnt++;
+	mutex_unlock(&dev->ar0832_camera_lock);
+
+	/* Change slave address */
+	if (i2c_client->addr)
+		ret = ar0832_set_alternate_addr(i2c_client);
+
+	return 0;
+
+fail_regulator_2v8_reg:
+	regulator_put(dev->power_rail.sen_2v8_reg);
+	dev->power_rail.sen_2v8_reg = NULL;
+	regulator_disable(dev->power_rail.sen_1v8_reg);
+fail_regulator_1v8_reg:
+	regulator_put(dev->power_rail.sen_1v8_reg);
+	dev->power_rail.sen_1v8_reg = NULL;
+	return ret;
+}
+
+static void ar0832_power_off(struct ar0832_dev *dev)
+{
+	struct i2c_client *i2c_client = dev->i2c_client;
+	dev_dbg(&i2c_client->dev, "%s: ++ %d\n", __func__, dev->brd_power_cnt);
+
+	/* Board specific power-down sequence */
+	mutex_lock(&dev->ar0832_camera_lock);
+
+	if (dev->brd_power_cnt <= 0)
+		goto ar0832_pwdn_exit;
+
+	if (dev->brd_power_cnt-- == 1) {
+		/* Unplug 1.8V and 2.8V power from sensor */
+		if (dev->power_rail.sen_2v8_reg)
+			regulator_disable(dev->power_rail.sen_2v8_reg);
+		if (dev->power_rail.sen_1v8_reg)
+			regulator_disable(dev->power_rail.sen_1v8_reg);
+		dev->pdata->power_off(dev->is_stereo);
+	}
+
+ar0832_pwdn_exit:
+	mutex_unlock(&dev->ar0832_camera_lock);
+}
+
+static int ar0832_focuser_set_config(struct ar0832_dev *dev)
+{
+	struct i2c_client *i2c_client = dev->i2c_client;
+	struct ar0832_reg reg_vcm_ctrl, reg_vcm_step_time;
+	int ret = 0;
+	u8 vcm_slew;
+	u16 vcm_control_data;
+	u16 vcm_step_time = 1024;
+
+	/* slew_rate of disabled (0) or default value (1) will disable    */
+	/* the slew rate in this case. Any value of 2 onwards will enable */
+	/* the slew rate to a different degree */
+	if (dev->focuser_info->config.slew_rate == SLEW_RATE_DISABLED ||
+	   dev->focuser_info->config.slew_rate == SLEW_RATE_DEFAULT)
+		vcm_slew = AR0832_SLEW_RATE_DISABLED;
+	else
+		vcm_slew = dev->focuser_info->config.slew_rate - 1;
+
+	if (vcm_slew > AR0832_SLEW_RATE_SLOWEST)
+		vcm_slew = AR0832_SLEW_RATE_SLOWEST;
+
+	/* bit15(0x80) means that VCM driver enable bit. */
+	/* bit3(0x08) means that keep VCM(AF position) */
+	/* while sensor is in soft standby mode during mode transitions. */
+	vcm_control_data = (0x80 << 8 | (0x08 | (vcm_slew & 0x07)));
+
+	ar0832_get_focuser_vcm_control_regs(&reg_vcm_ctrl, vcm_control_data);
+	ret = ar0832_write_reg16(dev->i2c_client, reg_vcm_ctrl.addr,
+					reg_vcm_ctrl.val);
+
+	dev_dbg(&i2c_client->dev, "%s Reg 0x%X Value 0x%X\n", __func__,
+			reg_vcm_ctrl.addr, reg_vcm_ctrl.val);
+
+	if (ret) {
+		dev_dbg(&i2c_client->dev, "%s Error writing to register 0x%X\n",
+				__func__, reg_vcm_ctrl.addr);
+		return ret;
+	}
+
+	ar0832_get_focuser_vcm_step_time_regs(&reg_vcm_step_time,
+						vcm_step_time);
+	ret = ar0832_write_reg16(dev->i2c_client, reg_vcm_step_time.addr,
+					reg_vcm_step_time.val);
+
+	dev_dbg(&i2c_client->dev, "%s Reg step_time 0x%X Value 0x%X\n",
+			__func__, reg_vcm_step_time.addr,
+			reg_vcm_step_time.val);
+
+	return ret;
+}
+
+static int ar0832_focuser_set_position(struct ar0832_dev *dev,
+					u32 position)
+{
+	int ret = 0;
+	struct ar0832_reg reg_data;
+
+	ar0832_get_focuser_data_regs(&reg_data, position);
+	ret = ar0832_write_reg16(dev->i2c_client, reg_data.addr,
+				     reg_data.val);
+	dev->focuser_info->last_position = position;
+
+	return ret;
+}
+
+#ifdef AR0832_FOCUSER_DYNAMIC_STEP_TIME
+/*
+ * This function is not currently called as we have the hardcoded
+ * step time in ar0832_focuser_set_config function. If we need to
+ * compute the actual step time based on a number of clocks, we need
+ * to use this function. The formula for computing the clock-based
+ * step time is obtained from Aptina and is not part of external
+ * documentation and hence this code needs to be saved.
+ */
+static u16 ar0832_get_focuser_vcm_step_time(struct ar0832_dev *dev)
+{
+	struct i2c_client *i2c_client = dev->i2c_client;
+	int ret;
+	u16 pll_multiplier = 0;
+	u16 pre_pll_clk_div = 0;
+	u16 vt_sys_clk_div = 0;
+	u16 vt_pix_clk_div = 0;
+	u16 vt_pix_clk_freq_mhz = 0;
+
+	ret = ar0832_read_reg16(dev->i2c_client, 0x306, &pll_multiplier);
+	if (ret) {
+		dev_err(&i2c_client->dev, "%s pll_multiplier read failed\n",
+				__func__);
+	}
+
+	ret = ar0832_read_reg16(dev->i2c_client, 0x304, &pre_pll_clk_div);
+	if (ret) {
+		dev_err(&i2c_client->dev, "%s pre_pll_clk_div read failed\n",
+				__func__);
+	}
+
+	ret = ar0832_read_reg16(dev->i2c_client, 0x302, &vt_sys_clk_div);
+	if (ret) {
+		dev_err(&i2c_client->dev, "%s vt_sys_clk_div read failed\n",
+				__func__);
+	}
+
+	ret = ar0832_read_reg16(dev->i2c_client, 0x300, &vt_pix_clk_div);
+	if (ret) {
+		dev_err(&i2c_client->dev, "%s vt_pix_clk_div read failed\n",
+				__func__);
+	}
+
+	vt_pix_clk_freq_mhz =
+		(24 * pll_multiplier) / (pre_pll_clk_div * vt_sys_clk_div *
+		 vt_pix_clk_div);
+
+	dev_dbg(&i2c_client->dev, "%s pll_multiplier 0x%X pre_pll_clk_div 0x%X "
+			"vt_sys_clk_div 0x%X vt_pix_clk_div 0x%X vt_pix_clk_freq_mhz 0x%X\n",
+			__func__, pll_multiplier,
+			pre_pll_clk_div, vt_sys_clk_div,
+			vt_pix_clk_div, vt_pix_clk_freq_mhz);
+
+	return vt_pix_clk_freq_mhz;
+
+}
+#endif
+
+static inline
+int ar0832_get_sensorid(struct ar0832_dev *dev, u16 *sensor_id)
+{
+	int ret;
+	struct i2c_client *i2c_client = dev->i2c_client;
+
+	ret = ar0832_power_on(dev);
+	if (ret)
+		return ret;
+
+	ret = ar0832_read_reg16(i2c_client, AR0832_SENSORID_REG, sensor_id);
+	dev_dbg(&i2c_client->dev,
+		"%s: sensor_id - %04x\n", __func__, *sensor_id);
+
+	ar0832_power_off(dev);
+
+	return ret;
+}
+
+static long ar0832_ioctl(struct file *file,
+				unsigned int cmd, unsigned long arg)
+{
+	int err;
+	struct ar0832_dev *dev = file->private_data;
+	struct i2c_client *i2c_client = dev->i2c_client;
+	struct ar0832_mode mode;
+	u16 pos;
+
+	switch (cmd) {
+	case AR0832_IOCTL_SET_POWER_ON:
+		dev_dbg(&i2c_client->dev, "AR0832_IOCTL_SET_POWER_ON\n");
+		if (copy_from_user(&mode,
+			(const void __user *)arg,
+			sizeof(struct ar0832_mode))) {
+			dev_err(&i2c_client->dev,
+				"%s: AR0832_IOCTL_SET_POWER_ON failed\n",
+				__func__);
+			return -EFAULT;
+		}
+		dev->is_stereo = mode.stereo;
+		return ar0832_power_on(dev);
+	case AR0832_IOCTL_SET_MODE:
+	{
+		dev_dbg(&i2c_client->dev, "AR0832_IOCTL_SET_MODE\n");
+		if (copy_from_user(&mode,
+			(const void __user *)arg,
+			sizeof(struct ar0832_mode))) {
+			dev_err(&i2c_client->dev,
+				"%s: AR0832_IOCTL_SET_MODE failed\n",
+				__func__);
+			return -EFAULT;
+		}
+		mutex_lock(&dev->ar0832_camera_lock);
+		err = ar0832_set_mode(dev, &mode);
+
+		/*
+		 * We need to re-initialize the Focuser registers during mode
+		 * switch due to the known issue of focuser retracting
+		 */
+		ar0832_focuser_set_config(dev);
+		dev->focuser_info->focuser_init_flag = true;
+
+		/*
+		 * If the last focuser position is not at infinity when we
+		 * did the mode switch, we need to go there. Before that,
+		 * we need to come back to 0.
+		 */
+		if (dev->focuser_info->last_position > 0) {
+			pos = dev->focuser_info->last_position;
+			dev_dbg(&i2c_client->dev, "%s: AR0832_IOCTL_SET_MODE: "
+					" Move to 0, restore the backedup focuser position of %d\n",
+					__func__, pos);
+			ar0832_focuser_set_position(dev, 0);
+			ar0832_msleep(10);
+
+			ar0832_focuser_set_position(dev, pos);
+			ar0832_msleep(10);
+		}
+		mutex_unlock(&dev->ar0832_camera_lock);
+		return err;
+	}
+	case AR0832_IOCTL_SET_FRAME_LENGTH:
+		mutex_lock(&dev->ar0832_camera_lock);
+		err = ar0832_set_frame_length(dev, (u32)arg);
+		mutex_unlock(&dev->ar0832_camera_lock);
+		return err;
+	case AR0832_IOCTL_SET_COARSE_TIME:
+		mutex_lock(&dev->ar0832_camera_lock);
+		err = ar0832_set_coarse_time(dev, (u32)arg);
+		mutex_unlock(&dev->ar0832_camera_lock);
+		return err;
+	case AR0832_IOCTL_SET_GAIN:
+		mutex_lock(&dev->ar0832_camera_lock);
+		err = ar0832_set_gain(dev, (u16)arg);
+		mutex_unlock(&dev->ar0832_camera_lock);
+		return err;
+	case AR0832_IOCTL_GET_STATUS:
+	{
+		u8 status;
+		dev_dbg(&i2c_client->dev, "AR0832_IOCTL_GET_STATUS\n");
+		err = ar0832_get_status(dev, &status);
+		if (err)
+			return err;
+		if (copy_to_user((void __user *)arg, &status,
+				 2)) {
+			dev_err(&i2c_client->dev,
+				"%s: AR0832_IOCTL_GET_STATUS failed\n",
+				__func__);
+			return -EFAULT;
+		}
+		return 0;
+	}
+	case AR0832_IOCTL_SET_SENSOR_REGION:
+	{
+		dev_dbg(&i2c_client->dev, "AR0832_IOCTL_SET_SENSOR_REGION\n");
+		/* Right now, it doesn't do anything */
+
+		return 0;
+	}
+
+	case AR0832_FOCUSER_IOCTL_GET_CONFIG:
+		dev_dbg(&i2c_client->dev,
+			"%s AR0832_FOCUSER_IOCTL_GET_CONFIG\n", __func__);
+		if (copy_to_user((void __user *) arg,
+				 &dev->focuser_info->config,
+				 sizeof(struct nv_focuser_config)))
+        {
+			dev_err(&i2c_client->dev,
+				"%s: AR0832_FOCUSER_IOCTL_GET_CONFIG failed\n",
+				__func__);
+			return -EFAULT;
+		}
+		return 0;
+
+	case AR0832_FOCUSER_IOCTL_SET_CONFIG:
+		dev_info(&i2c_client->dev,
+				"%s AR0832_FOCUSER_IOCTL_SET_CONFIG\n", __func__);
+		if (copy_from_user(&dev->focuser_info->config,
+			(const void __user *)arg,
+			sizeof(struct nv_focuser_config)))
+		{
+			dev_err(&i2c_client->dev,
+					"%s: AR0832_FOCUSER_IOCTL_SET_CONFIG failed\n", __func__);
+			return -EFAULT;
+		}
+		dev_dbg(&i2c_client->dev,
+			"%s AR0832_FOCUSER_IOCTL_SET_CONFIG sucess "
+			"slew_rate %i, pos_working_high %i, pos_working_low %i\n",
+			__func__, dev->focuser_info->config.slew_rate,
+			dev->focuser_info->config.pos_working_low,
+			dev->focuser_info->config.pos_working_high);
+		return 0;
+
+	case AR0832_FOCUSER_IOCTL_SET_POSITION:
+		dev_dbg(&i2c_client->dev,
+			"%s AR0832_FOCUSER_IOCTL_SET_POSITION\n", __func__);
+		mutex_lock(&dev->ar0832_camera_lock);
+		if (dev->focuser_info->focuser_init_flag == false) {
+			ar0832_focuser_set_config(dev);
+			dev->focuser_info->focuser_init_flag = true;
+		}
+		err = ar0832_focuser_set_position(dev, (u32)arg);
+		mutex_unlock(&dev->ar0832_camera_lock);
+		return err;
+
+	case AR0832_IOCTL_GET_SENSOR_ID:
+		dev_dbg(&i2c_client->dev,
+			"%s AR0832_IOCTL_GET_SENSOR_ID\n", __func__);
+
+		if (!dev->sensor_id_data) {
+			err = ar0832_get_sensorid(dev, &dev->sensor_id_data);
+			if (err) {
+				dev_err(&i2c_client->dev,
+					"%s: failed to get sensor id\n",
+					__func__);
+				return -EFAULT;
+			}
+		}
+
+		if (copy_to_user((void __user *) arg,
+				&dev->sensor_id_data,
+				sizeof(dev->sensor_id_data))) {
+			dev_err(&i2c_client->dev,
+				"%s: AR0832_IOCTL_GET_SENSOR_ID failed\n",
+				__func__);
+			return -EFAULT;
+		}
+		return 0;
+
+	default:
+		dev_err(&i2c_client->dev, "(error) %s NONE IOCTL\n",
+			__func__);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int ar0832_open(struct inode *inode, struct file *file)
+{
+	struct miscdevice *miscdev = file->private_data;
+	struct ar0832_dev *dev = dev_get_drvdata(miscdev->parent);
+	struct i2c_client *i2c_client = dev->i2c_client;
+
+	dev_dbg(&i2c_client->dev, "%s: ++\n", __func__);
+	if (atomic_xchg(&dev->in_use, 1))
+		return -EBUSY;
+
+	dev->focuser_info->focuser_init_flag = false;
+	file->private_data = dev;
+
+	return 0;
+}
+
+static int ar0832_release(struct inode *inode, struct file *file)
+{
+	struct ar0832_dev *dev = file->private_data;
+	struct i2c_client *i2c_client = dev->i2c_client;
+
+	dev_dbg(&i2c_client->dev, "%s: ++\n", __func__);
+
+	ar0832_power_off(dev);
+
+	file->private_data = NULL;
+
+	dev->focuser_info->focuser_init_flag = false;
+
+	WARN_ON(!atomic_xchg(&dev->in_use, 0));
+	return 0;
+}
+
+static const struct file_operations ar0832_fileops = {
+	.owner = THIS_MODULE,
+	.open = ar0832_open,
+	.unlocked_ioctl = ar0832_ioctl,
+	.release = ar0832_release,
+};
+
+static int ar0832_debugfs_show(struct seq_file *s, void *unused)
+{
+	struct ar0832_dev *dev = s->private;
+	struct i2c_client *i2c_client = dev->i2c_client;
+	int ret;
+	u16 test_pattern_reg;
+
+	dev_dbg(&dev->i2c_client->dev, "%s: ++\n", __func__);
+	if (!dev->brd_power_cnt) {
+		dev_info(&i2c_client->dev,
+			"%s: camera is off\n", __func__);
+		return 0;
+	}
+
+	mutex_lock(&dev->ar0832_camera_lock);
+	ret = ar0832_read_reg16(i2c_client,
+			AR0832_TEST_PATTERN_REG, &test_pattern_reg);
+	mutex_unlock(&dev->ar0832_camera_lock);
+
+	if (ret) {
+		dev_err(&i2c_client->dev,
+			"%s: test pattern write failed\n", __func__);
+		return -EFAULT;
+	}
+
+	seq_printf(s, "%d\n", test_pattern_reg);
+	return 0;
+}
+
+static ssize_t ar0832_debugfs_write(
+	struct file *file,
+	char const __user *buf,
+	size_t count,
+	loff_t *offset)
+{
+	struct ar0832_dev *dev = ((struct seq_file *)file->private_data)->private;
+	struct i2c_client *i2c_client = dev->i2c_client;
+	int ret = 0;
+	char buffer[10];
+	u16 input, val, red = 0, green = 0, blue = 0;
+
+	dev_dbg(&i2c_client->dev, "%s: ++\n", __func__);
+	if (!dev->brd_power_cnt) {
+		dev_info(&i2c_client->dev,
+			"%s: camera is off\n", __func__);
+		return count;
+	}
+
+	if (copy_from_user(&buffer, buf, sizeof(buffer)))
+		goto debugfs_write_fail;
+
+	input = (u16)simple_strtoul(buffer, NULL, 10);
+
+	mutex_lock(&dev->ar0832_camera_lock);
+	ret = ar0832_write_reg8(i2c_client, AR0832_GROUP_HOLD_REG, 0x1);
+
+	switch (input) {
+	case 1: /* color bar */
+		val = 2;
+		break;
+	case 2:	/* Red */
+		val = 1;
+		red = 0x300; /* 10 bit value */
+		green = 0;
+		blue = 0;
+		break;
+	case 3: /* Green */
+		val = 1;
+		red = 0;
+		green = 0x300; /* 10 bit value */
+		blue = 0;
+		break;
+	case 4:	/* Blue */
+		val = 1;
+		red = 0;
+		green = 0;
+		blue = 0x300; /* 10 bit value */
+		break;
+	default:
+		val = 0;
+		break;
+	}
+
+	if (input == 2 || input == 3 || input == 4) {
+		ret |= ar0832_write_reg_helper(dev,
+				AR0832_TEST_RED_REG, red);
+		ret |= ar0832_write_reg_helper(dev,
+				AR0832_TEST_GREENR_REG, green);
+		ret |= ar0832_write_reg_helper(dev,
+				AR0832_TEST_GREENR_REG, green);
+		ret |= ar0832_write_reg_helper(dev,
+				AR0832_TEST_BLUE_REG, blue);
+	}
+
+	ret |= ar0832_write_reg_helper(dev, AR0832_TEST_PATTERN_REG, val);
+	ret |= ar0832_write_reg8(i2c_client, AR0832_GROUP_HOLD_REG, 0x0);
+	mutex_unlock(&dev->ar0832_camera_lock);
+
+	if (ret)
+		goto debugfs_write_fail;
+
+	return count;
+
+debugfs_write_fail:
+	dev_err(&i2c_client->dev,
+			"%s: test pattern write failed\n", __func__);
+	return -EFAULT;
+}
+
+static int ar0832_debugfs_open(struct inode *inode, struct file *file)
+{
+	struct ar0832_dev *dev = inode->i_private;
+	struct i2c_client *i2c_client = dev->i2c_client;
+
+	dev_dbg(&i2c_client->dev, "%s: ++\n", __func__);
+
+	return single_open(file, ar0832_debugfs_show, inode->i_private);
+}
+
+static const struct file_operations ar0832_debugfs_fops = {
+	.open		= ar0832_debugfs_open,
+	.read		= seq_read,
+	.write		= ar0832_debugfs_write,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static void __devexit ar0832_remove_debugfs(struct ar0832_dev *dev)
+{
+	struct i2c_client *i2c_client = dev->i2c_client;
+
+	dev_dbg(&i2c_client->dev, "%s: ++\n", __func__);
+
+	if (dev->debugdir)
+		debugfs_remove_recursive(dev->debugdir);
+	dev->debugdir = NULL;
+}
+
+static void ar0832_create_debugfs(struct ar0832_dev *dev)
+{
+	struct dentry *ret;
+	struct i2c_client *i2c_client = dev->i2c_client;
+
+	dev_dbg(&i2c_client->dev, "%s\n", __func__);
+
+	dev->debugdir = debugfs_create_dir(dev->dname, NULL);
+	if (!dev->debugdir)
+		goto remove_debugfs;
+
+	ret = debugfs_create_file("test_pattern",
+				S_IWUSR | S_IRUGO,
+				dev->debugdir, dev,
+				&ar0832_debugfs_fops);
+	if (!ret)
+		goto remove_debugfs;
+
+	return;
+remove_debugfs:
+	dev_err(&i2c_client->dev, "couldn't create debugfs\n");
+	ar0832_remove_debugfs(dev);
+}
+
+static int ar0832_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
+{
+	int err;
+	struct ar0832_dev *dev = NULL;
+	int ret;
+
+	dev_info(&client->dev, "ar0832: probing sensor.(id:%s)\n",
+		id->name);
+
+	dev = kzalloc(sizeof(struct ar0832_dev), GFP_KERNEL);
+	if (!dev)
+		goto probe_fail_release;
+
+	dev->sensor_info = kzalloc(sizeof(struct ar0832_sensor_info),
+					GFP_KERNEL);
+	if (!dev->sensor_info)
+		goto probe_fail_release;
+
+	dev->focuser_info = kzalloc(sizeof(struct ar0832_focuser_info),
+					GFP_KERNEL);
+	if (!dev->focuser_info)
+		goto probe_fail_release;
+
+	/* sensor */
+	dev->pdata = client->dev.platform_data;
+	dev->i2c_client = client;
+
+	/* focuser */
+	dev->focuser_info->config.settle_time = SETTLE_TIME;
+	dev->focuser_info->config.slew_rate = SLEW_RATE_DEFAULT;
+	dev->focuser_info->config.pos_actual_low = POS_ACTUAL_LOW;
+	dev->focuser_info->config.pos_actual_high = POS_ACTUAL_HIGH;
+	dev->focuser_info->config.pos_working_low = POS_ACTUAL_LOW;
+	dev->focuser_info->config.pos_working_high = POS_ACTUAL_HIGH;
+
+	snprintf(dev->dname, sizeof(dev->dname), "%s-%s",
+		id->name, dev->pdata->id);
+	dev->misc_dev.minor = MISC_DYNAMIC_MINOR;
+	dev->misc_dev.name = dev->dname;
+	dev->misc_dev.fops = &ar0832_fileops;
+	dev->misc_dev.mode = S_IRWXUGO;
+	dev->misc_dev.parent = &client->dev;
+	err = misc_register(&dev->misc_dev);
+	if (err) {
+		dev_err(&client->dev, "Unable to register misc device!\n");
+		ret = -ENOMEM;
+		goto probe_fail_free;
+	}
+
+	i2c_set_clientdata(client, dev);
+	mutex_init(&dev->ar0832_camera_lock);
+
+	dev->power_rail.sen_1v8_reg = regulator_get(&client->dev, "vdd");
+	if (IS_ERR_OR_NULL(dev->power_rail.sen_1v8_reg)) {
+		dev_err(&client->dev, "%s: failed to get vdd\n",
+			__func__);
+		ret = PTR_ERR(dev->power_rail.sen_1v8_reg);
+		goto probe_fail_free;
+	}
+
+	dev->power_rail.sen_2v8_reg = regulator_get(&client->dev, "vaa");
+	if (IS_ERR_OR_NULL(dev->power_rail.sen_2v8_reg)) {
+		dev_err(&client->dev, "%s: failed to get vaa\n",
+			__func__);
+		ret = PTR_ERR(dev->power_rail.sen_2v8_reg);
+		regulator_put(dev->power_rail.sen_1v8_reg);
+		dev->power_rail.sen_1v8_reg = NULL;
+		goto probe_fail_free;
+	}
+	/* create debugfs interface */
+	ar0832_create_debugfs(dev);
+
+	return 0;
+
+probe_fail_release:
+	dev_err(&client->dev, "%s: unable to allocate memory!\n", __func__);
+	ret = -ENOMEM;
+probe_fail_free:
+	if (dev) {
+		kfree(dev->focuser_info);
+		kfree(dev->sensor_info);
+	}
+	kfree(dev);
+	return ret;
+}
+
+static int ar0832_remove(struct i2c_client *client)
+{
+	struct ar0832_dev *dev = i2c_get_clientdata(client);
+
+	if (dev->power_rail.sen_1v8_reg)
+		regulator_put(dev->power_rail.sen_1v8_reg);
+	if (dev->power_rail.sen_2v8_reg)
+		regulator_put(dev->power_rail.sen_2v8_reg);
+
+	misc_deregister(&dev->misc_dev);
+	if (dev) {
+		kfree(dev->sensor_info);
+		kfree(dev->focuser_info);
+	}
+
+	ar0832_remove_debugfs(dev);
+
+	kfree(dev);
+	return 0;
+}
+
+static const struct i2c_device_id ar0832_id[] = {
+	{ "ar0832", 0 },
+	{ }
+};
+
+static struct i2c_driver ar0832_i2c_driver = {
+	.probe = ar0832_probe,
+	.remove = ar0832_remove,
+	.id_table = ar0832_id,
+	.driver = {
+		.name = "ar0832",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ar0832_init(void)
+{
+	pr_info("%s: ++\n", __func__);
+	return i2c_add_driver(&ar0832_i2c_driver);
+}
+
+static void __exit ar0832_exit(void)
+{
+	i2c_del_driver(&ar0832_i2c_driver);
+}
+
+module_init(ar0832_init);
+module_exit(ar0832_exit);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/video/tegra/avp/Kconfig b/drivers/media/video/tegra/avp/Kconfig
new file mode 100644
index 000000000000..fdd208510fcb
--- /dev/null
+++ b/drivers/media/video/tegra/avp/Kconfig
@@ -0,0 +1,25 @@
+config TEGRA_RPC
+	bool "Enable support for Tegra RPC"
+	depends on ARCH_TEGRA
+	default y
+	help
+	  Enables support for the RPC mechanism necessary for the Tegra
+	  multimedia framework. It is both used to communicate locally on the
+	  CPU between multiple multimedia components as well as to communicate
+	  with the AVP for offloading media decode.
+
+	  Exports the local tegra RPC interface on device node
+	  /dev/tegra_rpc. Also provides tegra fd based semaphores needed by
+	  the tegra multimedia framework.
+
+	  If unsure, say Y
+
+config TEGRA_AVP
+	bool "Enable support for the AVP multimedia offload engine"
+	depends on ARCH_TEGRA && TEGRA_RPC
+	default y
+	help
+	  Enables support for the multimedia offload engine used by Tegra
+	  multimedia framework.
+
+	  If unsure, say Y
diff --git a/drivers/media/video/tegra/avp/Makefile b/drivers/media/video/tegra/avp/Makefile
new file mode 100644
index 000000000000..148265648a40
--- /dev/null
+++ b/drivers/media/video/tegra/avp/Makefile
@@ -0,0 +1,7 @@
+GCOV_PROFILE := y
+obj-$(CONFIG_TEGRA_RPC)		+= tegra_rpc.o
+obj-$(CONFIG_TEGRA_RPC)		+= trpc_local.o
+obj-$(CONFIG_TEGRA_RPC)		+= trpc_sema.o
+obj-$(CONFIG_TEGRA_AVP)		+= avp.o
+obj-$(CONFIG_TEGRA_AVP)		+= avp_svc.o
+obj-$(CONFIG_TEGRA_AVP)		+= headavp.o
diff --git a/drivers/media/video/tegra/avp/avp.c b/drivers/media/video/tegra/avp/avp.c
new file mode 100644
index 000000000000..fc965e4f5b53
--- /dev/null
+++ b/drivers/media/video/tegra/avp/avp.c
@@ -0,0 +1,1949 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Dima Zavin <dima@android.com>
+ *
+ * Copyright (C) 2010-2012 NVIDIA Corporation
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/firmware.h>
+#include <linux/fs.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/ioctl.h>
+#include <linux/irq.h>
+#include <linux/kref.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/rbtree.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+#include <linux/tegra_rpc.h>
+#include <linux/types.h>
+#include <linux/uaccess.h>
+#include <linux/workqueue.h>
+
+#include <mach/clk.h>
+#include <mach/io.h>
+#include <mach/iomap.h>
+#include <linux/nvmap.h>
+#include <mach/legacy_irq.h>
+#include <mach/hardware.h>
+
+#include "../../../../video/tegra/nvmap/nvmap.h"
+
+#include "headavp.h"
+#include "avp_msg.h"
+#include "trpc.h"
+#include "avp.h"
+#include "nvavp.h"
+
+enum {
+	AVP_DBG_TRACE_XPC	= 1U << 0,
+	AVP_DBG_TRACE_XPC_IRQ	= 1U << 1,
+	AVP_DBG_TRACE_XPC_MSG	= 1U << 2,
+	AVP_DBG_TRACE_XPC_CONN	= 1U << 3,
+	AVP_DBG_TRACE_TRPC_MSG	= 1U << 4,
+	AVP_DBG_TRACE_TRPC_CONN	= 1U << 5,
+	AVP_DBG_TRACE_LIB	= 1U << 6,
+};
+
+static u32 avp_debug_mask =
+	AVP_DBG_TRACE_XPC	|
+	/* AVP_DBG_TRACE_XPC_IRQ	| */
+	/* AVP_DBG_TRACE_XPC_MSG	| */
+	/* AVP_DBG_TRACE_TRPC_MSG	| */
+	AVP_DBG_TRACE_XPC_CONN	|
+	AVP_DBG_TRACE_TRPC_CONN	|
+	AVP_DBG_TRACE_LIB;
+
+module_param_named(debug_mask, avp_debug_mask, uint, S_IWUSR | S_IRUGO);
+
+#define DBG(flag, args...) \
+	do { if (unlikely(avp_debug_mask & (flag))) pr_info(args); } while (0)
+
+#define TEGRA_AVP_NAME			"tegra-avp"
+
+#define TEGRA_AVP_RESET_VECTOR_ADDR \
+		(IO_ADDRESS(TEGRA_EXCEPTION_VECTORS_BASE) + 0x200)
+
+#define TEGRA_AVP_RESUME_ADDR		IO_ADDRESS(TEGRA_IRAM_BASE + \
+						TEGRA_RESET_HANDLER_SIZE)
+
+#define FLOW_CTRL_HALT_COP_EVENTS	IO_ADDRESS(TEGRA_FLOW_CTRL_BASE + 0x4)
+#define FLOW_MODE_STOP			(0x2 << 29)
+#define FLOW_MODE_NONE			0x0
+
+#define MBOX_FROM_AVP			IO_ADDRESS(TEGRA_RES_SEMA_BASE + 0x10)
+#define MBOX_TO_AVP			IO_ADDRESS(TEGRA_RES_SEMA_BASE + 0x20)
+
+/* Layout of the mailbox registers:
+ * bit 31    - pending message interrupt enable (mailbox full, i.e. valid=1)
+ * bit 30    - message cleared interrupt enable (mailbox empty, i.e. valid=0)
+ * bit 29    - message valid. peer clears this bit after reading msg
+ * bits 27:0 - message data
+ */
+#define MBOX_MSG_PENDING_INT_EN		(1 << 31)
+#define MBOX_MSG_READ_INT_EN		(1 << 30)
+#define MBOX_MSG_VALID			(1 << 29)
+
+#define AVP_MSG_MAX_CMD_LEN	16
+#define AVP_MSG_AREA_SIZE	(AVP_MSG_MAX_CMD_LEN + TEGRA_RPC_MAX_MSG_LEN)
+
+struct tegra_avp_info {
+	struct clk		*cop_clk;
+
+	int			mbox_from_avp_pend_irq;
+
+	dma_addr_t		msg_area_addr;
+	u32			msg;
+	void			*msg_to_avp;
+	void			*msg_from_avp;
+	struct mutex		to_avp_lock;
+	struct mutex		from_avp_lock;
+
+	struct work_struct	recv_work;
+	struct workqueue_struct	*recv_wq;
+
+	struct trpc_node	*rpc_node;
+	struct miscdevice	misc_dev;
+	int			refcount;
+	struct mutex		open_lock;
+
+	spinlock_t		state_lock;
+	bool			initialized;
+	bool			shutdown;
+	bool			suspending;
+	bool			defer_remote;
+
+	struct mutex		libs_lock;
+	struct list_head	libs;
+	struct nvmap_client	*nvmap_libs;
+
+	/* client for driver allocations, persistent */
+	struct nvmap_client	*nvmap_drv;
+	struct nvmap_handle_ref	*kernel_handle;
+	void			*kernel_data;
+	phys_addr_t		kernel_phys;
+
+	struct nvmap_handle_ref *iram_backup_handle;
+	void			*iram_backup_data;
+	phys_addr_t		iram_backup_phys;
+	unsigned long		resume_addr;
+	unsigned long		reset_addr;
+
+	struct trpc_endpoint	*avp_ep;
+	struct rb_root		endpoints;
+
+	struct avp_svc_info	*avp_svc;
+};
+
+struct remote_info {
+	u32			loc_id;
+	u32			rem_id;
+	struct kref		ref;
+
+	struct trpc_endpoint	*trpc_ep;
+	struct rb_node		rb_node;
+};
+
+struct lib_item {
+	struct list_head	list;
+	u32			handle;
+	char			name[TEGRA_AVP_LIB_MAX_NAME];
+};
+
+static struct tegra_avp_info *tegra_avp;
+
+static int avp_trpc_send(struct trpc_endpoint *ep, void *buf, size_t len);
+static void avp_trpc_close(struct trpc_endpoint *ep);
+static void avp_trpc_show(struct seq_file *s, struct trpc_endpoint *ep);
+static void libs_cleanup(struct tegra_avp_info *avp);
+
+static struct trpc_ep_ops remote_ep_ops = {
+	.send   = avp_trpc_send,
+	.close  = avp_trpc_close,
+	.show   = avp_trpc_show,
+};
+
+static struct remote_info *rinfo_alloc(struct tegra_avp_info *avp)
+{
+	struct remote_info *rinfo;
+
+	rinfo = kzalloc(sizeof(struct remote_info), GFP_KERNEL);
+	if (!rinfo)
+		return NULL;
+	kref_init(&rinfo->ref);
+	return rinfo;
+}
+
+static void _rinfo_release(struct kref *ref)
+{
+	struct remote_info *rinfo = container_of(ref, struct remote_info, ref);
+	kfree(rinfo);
+}
+
+static inline void rinfo_get(struct remote_info *rinfo)
+{
+	kref_get(&rinfo->ref);
+}
+
+static inline void rinfo_put(struct remote_info *rinfo)
+{
+	kref_put(&rinfo->ref, _rinfo_release);
+}
+
+static int remote_insert(struct tegra_avp_info *avp, struct remote_info *rinfo)
+{
+	struct rb_node **p;
+	struct rb_node *parent;
+	struct remote_info *tmp;
+
+	p = &avp->endpoints.rb_node;
+	parent = NULL;
+	while (*p) {
+		parent = *p;
+		tmp = rb_entry(parent, struct remote_info, rb_node);
+
+		if (rinfo->loc_id < tmp->loc_id)
+			p = &(*p)->rb_left;
+		else if (rinfo->loc_id > tmp->loc_id)
+			p = &(*p)->rb_right;
+		else {
+			pr_info("%s: avp endpoint id=%x (%s) already exists\n",
+				__func__, rinfo->loc_id,
+				trpc_name(rinfo->trpc_ep));
+			return -EEXIST;
+		}
+	}
+	rb_link_node(&rinfo->rb_node, parent, p);
+	rb_insert_color(&rinfo->rb_node, &avp->endpoints);
+	rinfo_get(rinfo);
+	return 0;
+}
+
+static struct remote_info *remote_find(struct tegra_avp_info *avp, u32 local_id)
+{
+	struct rb_node *n = avp->endpoints.rb_node;
+	struct remote_info *rinfo;
+
+	while (n) {
+		rinfo = rb_entry(n, struct remote_info, rb_node);
+
+		if (local_id < rinfo->loc_id)
+			n = n->rb_left;
+		else if (local_id > rinfo->loc_id)
+			n = n->rb_right;
+		else
+			return rinfo;
+	}
+	return NULL;
+}
+
+static void remote_remove(struct tegra_avp_info *avp, struct remote_info *rinfo)
+{
+	rb_erase(&rinfo->rb_node, &avp->endpoints);
+	rinfo_put(rinfo);
+}
+
+/* test whether or not the trpc endpoint provided is a valid AVP node
+ * endpoint */
+static struct remote_info *validate_trpc_ep(struct tegra_avp_info *avp,
+						struct trpc_endpoint *ep)
+{
+	struct remote_info *tmp = trpc_priv(ep);
+	struct remote_info *rinfo;
+
+	if (!tmp)
+		return NULL;
+	rinfo = remote_find(avp, tmp->loc_id);
+	if (rinfo && rinfo == tmp && rinfo->trpc_ep == ep)
+		return rinfo;
+	return NULL;
+}
+
+static void avp_trpc_show(struct seq_file *s, struct trpc_endpoint *ep)
+{
+	struct tegra_avp_info *avp = tegra_avp;
+	struct remote_info *rinfo;
+	unsigned long flags;
+
+	spin_lock_irqsave(&avp->state_lock, flags);
+	rinfo = validate_trpc_ep(avp, ep);
+	if (!rinfo) {
+		seq_printf(s, "    <unknown>\n");
+		goto out;
+	}
+	seq_printf(s, "    loc_id:0x%x\n	rem_id:0x%x\n",
+		   rinfo->loc_id, rinfo->rem_id);
+out:
+	spin_unlock_irqrestore(&avp->state_lock, flags);
+}
+
+static inline void mbox_writel(u32 val, void __iomem *mbox)
+{
+	writel(val, mbox);
+}
+
+static inline u32 mbox_readl(void __iomem *mbox)
+{
+	return readl(mbox);
+}
+
+static inline void msg_ack_remote(struct tegra_avp_info *avp, u32 cmd, u32 arg)
+{
+	struct msg_ack *ack = avp->msg_from_avp;
+
+	/* must make sure the arg is there first */
+	ack->arg = arg;
+	wmb();
+	ack->cmd = cmd;
+	wmb();
+}
+
+static inline u32 msg_recv_get_cmd(struct tegra_avp_info *avp)
+{
+	volatile u32 *cmd = avp->msg_from_avp;
+	rmb();
+	return *cmd;
+}
+
+static inline int __msg_write(struct tegra_avp_info *avp, void *hdr,
+			      size_t hdr_len, void *buf, size_t len)
+{
+	memcpy(avp->msg_to_avp, hdr, hdr_len);
+	if (buf && len)
+		memcpy(avp->msg_to_avp + hdr_len, buf, len);
+	mbox_writel(avp->msg, MBOX_TO_AVP);
+	return 0;
+}
+
+static inline int msg_write(struct tegra_avp_info *avp, void *hdr,
+			    size_t hdr_len, void *buf, size_t len)
+{
+	/* rem_ack is a pointer into shared memory that the AVP modifies */
+	volatile u32 *rem_ack = avp->msg_to_avp;
+	unsigned long endtime = jiffies + HZ;
+
+	/* the other side ack's the message by clearing the first word,
+	 * wait for it to do so */
+	rmb();
+	while (*rem_ack != 0 && time_before(jiffies, endtime)) {
+		usleep_range(100, 2000);
+		rmb();
+	}
+	if (*rem_ack != 0)
+		return -ETIMEDOUT;
+	__msg_write(avp, hdr, hdr_len, buf, len);
+	return 0;
+}
+
+static inline int msg_check_ack(struct tegra_avp_info *avp, u32 cmd, u32 *arg)
+{
+	struct msg_ack ack;
+
+	rmb();
+	memcpy(&ack, avp->msg_to_avp, sizeof(ack));
+	if (ack.cmd != cmd)
+		return -ENOENT;
+	if (arg)
+		*arg = ack.arg;
+	return 0;
+}
+
+/* XXX: add timeout */
+static int msg_wait_ack_locked(struct tegra_avp_info *avp, u32 cmd, u32 *arg)
+{
+	/* rem_ack is a pointer into shared memory that the AVP modifies */
+	volatile u32 *rem_ack = avp->msg_to_avp;
+	unsigned long endtime = jiffies + msecs_to_jiffies(400);
+	int ret;
+
+	do {
+		ret = msg_check_ack(avp, cmd, arg);
+		usleep_range(1000, 5000);
+	} while (ret && time_before(jiffies, endtime));
+
+	/* if we timed out, try one more time */
+	if (ret)
+		ret = msg_check_ack(avp, cmd, arg);
+
+	/* clear out the ack */
+	*rem_ack = 0;
+	wmb();
+	return ret;
+}
+
+static int avp_trpc_send(struct trpc_endpoint *ep, void *buf, size_t len)
+{
+	struct tegra_avp_info *avp = tegra_avp;
+	struct remote_info *rinfo;
+	struct msg_port_data msg;
+	int ret;
+	unsigned long flags;
+
+	DBG(AVP_DBG_TRACE_TRPC_MSG, "%s: ep=%p priv=%p buf=%p len=%d\n",
+		__func__, ep, trpc_priv(ep), buf, len);
+
+	spin_lock_irqsave(&avp->state_lock, flags);
+	if (unlikely(avp->suspending && trpc_peer(ep) != avp->avp_ep)) {
+		ret = -EBUSY;
+		goto err_state_locked;
+	} else if (avp->shutdown) {
+		ret = -ENODEV;
+		goto err_state_locked;
+	}
+	rinfo = validate_trpc_ep(avp, ep);
+	if (!rinfo) {
+		ret = -ENOTTY;
+		goto err_state_locked;
+	}
+	rinfo_get(rinfo);
+	spin_unlock_irqrestore(&avp->state_lock, flags);
+
+	msg.cmd = CMD_MESSAGE;
+	msg.port_id = rinfo->rem_id;
+	msg.msg_len = len;
+
+	mutex_lock(&avp->to_avp_lock);
+	ret = msg_write(avp, &msg, sizeof(msg), buf, len);
+	mutex_unlock(&avp->to_avp_lock);
+
+	DBG(AVP_DBG_TRACE_TRPC_MSG, "%s: msg sent for %s (%x->%x) (%d)\n",
+		__func__, trpc_name(ep), rinfo->loc_id, rinfo->rem_id, ret);
+	rinfo_put(rinfo);
+	return ret;
+
+err_state_locked:
+	spin_unlock_irqrestore(&avp->state_lock, flags);
+	return ret;
+}
+
+static int _send_disconnect(struct tegra_avp_info *avp, u32 port_id)
+{
+	struct msg_disconnect msg;
+	int ret;
+
+	msg.cmd = CMD_DISCONNECT;
+	msg.port_id = port_id;
+
+	mutex_lock(&avp->to_avp_lock);
+	ret = msg_write(avp, &msg, sizeof(msg), NULL, 0);
+	if (ret) {
+		pr_err("%s: remote has not acked last message (%x)\n", __func__,
+			   port_id);
+		goto err_msg_write;
+	}
+
+	ret = msg_wait_ack_locked(avp, CMD_ACK, NULL);
+	if (ret) {
+		pr_err("%s: remote end won't respond for %x\n", __func__,
+			   port_id);
+		goto err_wait_ack;
+	}
+
+	DBG(AVP_DBG_TRACE_XPC_CONN, "%s: sent disconnect msg for %x\n",
+		__func__, port_id);
+
+err_wait_ack:
+err_msg_write:
+	mutex_unlock(&avp->to_avp_lock);
+	return ret;
+}
+
+/* Note: Assumes that the rinfo was previously successfully added to the
+ * endpoints rb_tree. The initial refcnt of 1 is inherited by the port when the
+ * trpc endpoint is created with thi trpc_xxx functions. Thus, on close,
+ * we must drop that reference here.
+ * The avp->endpoints rb_tree keeps its own reference on rinfo objects.
+ *
+ * The try_connect function does not use this on error because it needs to
+ * split the close of trpc_ep port and the put.
+ */
+static inline void remote_close(struct remote_info *rinfo)
+{
+	trpc_close(rinfo->trpc_ep);
+	rinfo_put(rinfo);
+}
+
+static void avp_trpc_close(struct trpc_endpoint *ep)
+{
+	struct tegra_avp_info *avp = tegra_avp;
+	struct remote_info *rinfo;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&avp->state_lock, flags);
+	if (avp->shutdown) {
+		spin_unlock_irqrestore(&avp->state_lock, flags);
+		return;
+	}
+
+	rinfo = validate_trpc_ep(avp, ep);
+	if (!rinfo) {
+		pr_err("%s: tried to close invalid port '%s' endpoint (%p)\n",
+			   __func__, trpc_name(ep), ep);
+		spin_unlock_irqrestore(&avp->state_lock, flags);
+		return;
+	}
+	rinfo_get(rinfo);
+	remote_remove(avp, rinfo);
+	spin_unlock_irqrestore(&avp->state_lock, flags);
+
+	DBG(AVP_DBG_TRACE_TRPC_CONN, "%s: closing '%s' (%x)\n", __func__,
+		trpc_name(ep), rinfo->rem_id);
+
+	ret = _send_disconnect(avp, rinfo->rem_id);
+	if (ret)
+		pr_err("%s: error while closing remote port '%s' (%x)\n",
+			   __func__, trpc_name(ep), rinfo->rem_id);
+	remote_close(rinfo);
+	rinfo_put(rinfo);
+}
+
+/* takes and holds avp->from_avp_lock */
+static void recv_msg_lock(struct tegra_avp_info *avp)
+{
+	unsigned long flags;
+
+	mutex_lock(&avp->from_avp_lock);
+	spin_lock_irqsave(&avp->state_lock, flags);
+	avp->defer_remote = true;
+	spin_unlock_irqrestore(&avp->state_lock, flags);
+}
+
+/* MUST be called with avp->from_avp_lock held */
+static void recv_msg_unlock(struct tegra_avp_info *avp)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&avp->state_lock, flags);
+	avp->defer_remote = false;
+	spin_unlock_irqrestore(&avp->state_lock, flags);
+	mutex_unlock(&avp->from_avp_lock);
+}
+
+static int avp_node_try_connect(struct trpc_node *node,
+				struct trpc_node *src_node,
+				struct trpc_endpoint *from)
+{
+	struct tegra_avp_info *avp = tegra_avp;
+	const char *port_name = trpc_name(from);
+	struct remote_info *rinfo;
+	struct msg_connect msg;
+	int ret;
+	unsigned long flags;
+	int len;
+	const int max_retry_cnt = 6;
+	int cnt = 0;
+
+	DBG(AVP_DBG_TRACE_TRPC_CONN, "%s: trying connect from %s\n", __func__,
+		port_name);
+
+	if (node != avp->rpc_node || node->priv != avp)
+		return -ENODEV;
+
+	len = strlen(port_name);
+	if (len > XPC_PORT_NAME_LEN) {
+		pr_err("%s: port name (%s) too long\n", __func__, port_name);
+		return -EINVAL;
+	}
+
+	ret = 0;
+	spin_lock_irqsave(&avp->state_lock, flags);
+	if (avp->suspending) {
+		ret = -EBUSY;
+	} else if (likely(src_node != avp->rpc_node)) {
+		/* only check for initialized when the source is not ourselves
+		 * since we'll end up calling into here during initialization */
+		if (!avp->initialized)
+			ret = -ENODEV;
+	} else if (strncmp(port_name, "RPC_AVP_PORT", XPC_PORT_NAME_LEN)) {
+		/* we only allow connections to ourselves for the cpu-to-avp
+		   port */
+		ret = -EINVAL;
+	}
+	spin_unlock_irqrestore(&avp->state_lock, flags);
+	if (ret)
+		return ret;
+
+	rinfo = rinfo_alloc(avp);
+	if (!rinfo) {
+		pr_err("%s: cannot alloc mem for rinfo\n", __func__);
+		ret = -ENOMEM;
+		goto err_alloc_rinfo;
+	}
+	rinfo->loc_id = (u32)rinfo;
+
+	msg.cmd = CMD_CONNECT;
+	msg.port_id = rinfo->loc_id;
+	memcpy(msg.name, port_name, len);
+	memset(msg.name + len, 0, XPC_PORT_NAME_LEN - len);
+
+	/* when trying to connect to remote, we need to block remote
+	 * messages until we get our ack and can insert it into our lists.
+	 * Otherwise, we can get a message from the other side for a port
+	 * that we haven't finished setting up.
+	 *
+	 * 'defer_remote' will force the irq handler to not process messages
+	 * at irq context but to schedule work to do so. The work function will
+	 * take the from_avp_lock and everything should stay consistent.
+	 */
+	recv_msg_lock(avp);
+	for (cnt = 0; cnt < max_retry_cnt; cnt++) {
+		/* Retry to connect to AVP at this function maximum 6 times.
+		 * Because this section is protected by mutex and
+		 * needed to re-send the CMD_CONNECT command by CPU
+		 * if AVP didn't receive the command.
+		 */
+		mutex_lock(&avp->to_avp_lock);
+		ret = msg_write(avp, &msg, sizeof(msg), NULL, 0);
+		if (ret) {
+			pr_err("%s: remote has not acked last message (%s)\n",
+				   __func__, port_name);
+			mutex_unlock(&avp->to_avp_lock);
+			goto err_msg_write;
+		}
+		ret = msg_wait_ack_locked(avp, CMD_RESPONSE, &rinfo->rem_id);
+		mutex_unlock(&avp->to_avp_lock);
+		if (!ret && rinfo->rem_id)
+			break;
+
+		/* Skip the sleep function at last retry count */
+		if ((cnt + 1) < max_retry_cnt)
+			usleep_range(100, 2000);
+	}
+
+	if (ret) {
+		pr_err("%s: remote end won't respond for '%s'\n", __func__,
+			   port_name);
+		goto err_wait_ack;
+	}
+	if (!rinfo->rem_id) {
+		pr_err("%s: can't connect to '%s'\n", __func__, port_name);
+		ret = -ECONNREFUSED;
+		goto err_nack;
+	}
+
+	DBG(AVP_DBG_TRACE_TRPC_CONN, "%s: got conn ack '%s' (%x <-> %x)\n",
+		__func__, port_name, rinfo->loc_id, rinfo->rem_id);
+
+	rinfo->trpc_ep = trpc_create_peer(node, from, &remote_ep_ops,
+						   rinfo);
+	if (!rinfo->trpc_ep) {
+		pr_err("%s: cannot create peer for %s\n", __func__, port_name);
+		ret = -EINVAL;
+		goto err_create_peer;
+	}
+
+	spin_lock_irqsave(&avp->state_lock, flags);
+	ret = remote_insert(avp, rinfo);
+	spin_unlock_irqrestore(&avp->state_lock, flags);
+	if (ret)
+		goto err_ep_insert;
+
+	recv_msg_unlock(avp);
+	return 0;
+
+err_ep_insert:
+	trpc_close(rinfo->trpc_ep);
+err_create_peer:
+	_send_disconnect(avp, rinfo->rem_id);
+err_nack:
+err_wait_ack:
+err_msg_write:
+	recv_msg_unlock(avp);
+	rinfo_put(rinfo);
+err_alloc_rinfo:
+	return ret;
+}
+
+static void process_disconnect_locked(struct tegra_avp_info *avp,
+					  struct msg_data *raw_msg)
+{
+	struct msg_disconnect *disconn_msg = (struct msg_disconnect *)raw_msg;
+	unsigned long flags;
+	struct remote_info *rinfo;
+
+	DBG(AVP_DBG_TRACE_XPC_CONN, "%s: got disconnect (%x)\n", __func__,
+		disconn_msg->port_id);
+
+	if (avp_debug_mask & AVP_DBG_TRACE_XPC_MSG)
+		print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, disconn_msg,
+					 sizeof(struct msg_disconnect));
+
+	spin_lock_irqsave(&avp->state_lock, flags);
+	rinfo = remote_find(avp, disconn_msg->port_id);
+	if (!rinfo) {
+		spin_unlock_irqrestore(&avp->state_lock, flags);
+		pr_warning("%s: got disconnect for unknown port %x\n",
+			   __func__, disconn_msg->port_id);
+		goto ack;
+	}
+	rinfo_get(rinfo);
+	remote_remove(avp, rinfo);
+	spin_unlock_irqrestore(&avp->state_lock, flags);
+
+	remote_close(rinfo);
+	rinfo_put(rinfo);
+ack:
+	msg_ack_remote(avp, CMD_ACK, 0);
+}
+
+static void process_connect_locked(struct tegra_avp_info *avp,
+				   struct msg_data *raw_msg)
+{
+	struct msg_connect *conn_msg = (struct msg_connect *)raw_msg;
+	struct trpc_endpoint *trpc_ep;
+	struct remote_info *rinfo;
+	char name[XPC_PORT_NAME_LEN + 1];
+	int ret;
+	u32 local_port_id = 0;
+	unsigned long flags;
+
+	DBG(AVP_DBG_TRACE_XPC_CONN, "%s: got connect (%x)\n", __func__,
+		conn_msg->port_id);
+	if (avp_debug_mask & AVP_DBG_TRACE_XPC_MSG)
+		print_hex_dump_bytes("", DUMP_PREFIX_OFFSET,
+					 conn_msg, sizeof(struct msg_connect));
+
+	rinfo = rinfo_alloc(avp);
+	if (!rinfo) {
+		pr_err("%s: cannot alloc mem for rinfo\n", __func__);
+		ret = -ENOMEM;
+		goto ack;
+	}
+	rinfo->loc_id = (u32)rinfo;
+	rinfo->rem_id = conn_msg->port_id;
+
+	memcpy(name, conn_msg->name, XPC_PORT_NAME_LEN);
+	name[XPC_PORT_NAME_LEN] = '\0';
+	trpc_ep = trpc_create_connect(avp->rpc_node, name, &remote_ep_ops,
+					  rinfo, 0);
+	if (IS_ERR(trpc_ep)) {
+		pr_err("%s: remote requested unknown port '%s' (%d)\n",
+			   __func__, name, (int)PTR_ERR(trpc_ep));
+		goto nack;
+	}
+	rinfo->trpc_ep = trpc_ep;
+
+	spin_lock_irqsave(&avp->state_lock, flags);
+	ret = remote_insert(avp, rinfo);
+	spin_unlock_irqrestore(&avp->state_lock, flags);
+	if (ret)
+		goto err_ep_insert;
+
+	local_port_id = rinfo->loc_id;
+	goto ack;
+
+err_ep_insert:
+	trpc_close(trpc_ep);
+nack:
+	rinfo_put(rinfo);
+	local_port_id = 0;
+ack:
+	msg_ack_remote(avp, CMD_RESPONSE, local_port_id);
+}
+
+static int process_message(struct tegra_avp_info *avp, struct msg_data *raw_msg,
+				gfp_t gfp_flags)
+{
+	struct msg_port_data *port_msg = (struct msg_port_data *)raw_msg;
+	struct remote_info *rinfo;
+	unsigned long flags;
+	int len;
+	int ret;
+
+	len = min(port_msg->msg_len, (u32)TEGRA_RPC_MAX_MSG_LEN);
+
+	if (avp_debug_mask & AVP_DBG_TRACE_XPC_MSG) {
+		pr_info("%s: got message cmd=%x port=%x len=%d\n", __func__,
+			port_msg->cmd, port_msg->port_id, port_msg->msg_len);
+		print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, port_msg,
+					 sizeof(struct msg_port_data) + len);
+	}
+
+	if (len != port_msg->msg_len)
+		pr_err("%s: message sent is too long (%d bytes)\n", __func__,
+			   port_msg->msg_len);
+
+	spin_lock_irqsave(&avp->state_lock, flags);
+	rinfo = remote_find(avp, port_msg->port_id);
+	if (rinfo) {
+		rinfo_get(rinfo);
+		trpc_get(rinfo->trpc_ep);
+	} else {
+		pr_err("%s: port %x not found\n", __func__, port_msg->port_id);
+		spin_unlock_irqrestore(&avp->state_lock, flags);
+		ret = -ENOENT;
+		goto ack;
+	}
+	spin_unlock_irqrestore(&avp->state_lock, flags);
+
+	ret = trpc_send_msg(avp->rpc_node, rinfo->trpc_ep, port_msg->data,
+				 len, gfp_flags);
+	if (ret == -ENOMEM) {
+		trpc_put(rinfo->trpc_ep);
+		rinfo_put(rinfo);
+		goto no_ack;
+	} else if (ret) {
+		pr_err("%s: cannot queue message for port %s/%x (%d)\n",
+			   __func__, trpc_name(rinfo->trpc_ep), rinfo->loc_id,
+			   ret);
+	} else {
+		DBG(AVP_DBG_TRACE_XPC_MSG, "%s: msg queued\n", __func__);
+	}
+
+	trpc_put(rinfo->trpc_ep);
+	rinfo_put(rinfo);
+ack:
+	msg_ack_remote(avp, CMD_ACK, 0);
+no_ack:
+	return ret;
+}
+
+static void process_avp_message(struct work_struct *work)
+{
+	struct tegra_avp_info *avp = container_of(work, struct tegra_avp_info,
+						  recv_work);
+	struct msg_data *msg = avp->msg_from_avp;
+
+	mutex_lock(&avp->from_avp_lock);
+	rmb();
+	switch (msg->cmd) {
+	case CMD_CONNECT:
+		process_connect_locked(avp, msg);
+		break;
+	case CMD_DISCONNECT:
+		process_disconnect_locked(avp, msg);
+		break;
+	case CMD_MESSAGE:
+		process_message(avp, msg, GFP_KERNEL);
+		break;
+	default:
+		pr_err("%s: unknown cmd (%x) received\n", __func__, msg->cmd);
+		break;
+	}
+	mutex_unlock(&avp->from_avp_lock);
+}
+
+static irqreturn_t avp_mbox_pending_isr(int irq, void *data)
+{
+	struct tegra_avp_info *avp = data;
+	struct msg_data *msg = avp->msg_from_avp;
+	u32 mbox_msg;
+	unsigned long flags;
+	int ret;
+
+	mbox_msg = mbox_readl(MBOX_FROM_AVP);
+	mbox_writel(0, MBOX_FROM_AVP);
+
+	DBG(AVP_DBG_TRACE_XPC_IRQ, "%s: got msg %x\n", __func__, mbox_msg);
+
+	/* XXX: re-use previous message? */
+	if (!(mbox_msg & MBOX_MSG_VALID)) {
+		WARN_ON(1);
+		goto done;
+	}
+
+	mbox_msg <<= 4;
+	if (mbox_msg == 0x2f00bad0UL) {
+		pr_info("%s: petting watchdog\n", __func__);
+		goto done;
+	}
+
+	spin_lock_irqsave(&avp->state_lock, flags);
+	if (avp->shutdown) {
+		spin_unlock_irqrestore(&avp->state_lock, flags);
+		goto done;
+	} else if (avp->defer_remote) {
+		spin_unlock_irqrestore(&avp->state_lock, flags);
+		goto defer;
+	}
+	spin_unlock_irqrestore(&avp->state_lock, flags);
+
+	rmb();
+	if (msg->cmd == CMD_MESSAGE) {
+		ret = process_message(avp, msg, GFP_ATOMIC);
+		if (ret != -ENOMEM)
+			goto done;
+		pr_info("%s: deferring message (%d)\n", __func__, ret);
+	}
+defer:
+	queue_work(avp->recv_wq, &avp->recv_work);
+done:
+	return IRQ_HANDLED;
+}
+
+static int avp_reset(struct tegra_avp_info *avp, unsigned long reset_addr)
+{
+	unsigned long stub_code_phys = virt_to_phys(_tegra_avp_boot_stub);
+	dma_addr_t stub_data_phys;
+	unsigned long timeout;
+	int ret = 0;
+
+	writel(FLOW_MODE_STOP, FLOW_CTRL_HALT_COP_EVENTS);
+
+	_tegra_avp_boot_stub_data.map_phys_addr = avp->kernel_phys;
+	_tegra_avp_boot_stub_data.jump_addr = reset_addr;
+	wmb();
+	stub_data_phys = dma_map_single(NULL, &_tegra_avp_boot_stub_data,
+					sizeof(_tegra_avp_boot_stub_data),
+					DMA_TO_DEVICE);
+
+	writel(stub_code_phys, TEGRA_AVP_RESET_VECTOR_ADDR);
+
+	pr_debug("%s: TEGRA_AVP_RESET_VECTOR=%x\n", __func__, readl(TEGRA_AVP_RESET_VECTOR_ADDR));
+	pr_info("%s: Resetting AVP: reset_addr=%lx\n", __func__, reset_addr);
+
+	tegra_periph_reset_assert(avp->cop_clk);
+	udelay(10);
+	tegra_periph_reset_deassert(avp->cop_clk);
+
+	writel(FLOW_MODE_NONE, FLOW_CTRL_HALT_COP_EVENTS);
+
+	/* the AVP firmware will reprogram its reset vector as the kernel
+	 * starts, so a dead kernel can be detected by polling this value */
+	timeout = jiffies + msecs_to_jiffies(2000);
+	while (time_before(jiffies, timeout)) {
+		pr_debug("%s: TEGRA_AVP_RESET_VECTOR=%x\n", __func__, readl(TEGRA_AVP_RESET_VECTOR_ADDR));
+		if (readl(TEGRA_AVP_RESET_VECTOR_ADDR) != stub_code_phys)
+			break;
+		cpu_relax();
+	}
+	if (readl(TEGRA_AVP_RESET_VECTOR_ADDR) == stub_code_phys) {
+		pr_err("%s: Timed out waiting for AVP kernel to start\n", __func__);
+		ret = -EINVAL;
+	}
+	pr_debug("%s: TEGRA_AVP_RESET_VECTOR=%x\n", __func__, readl(TEGRA_AVP_RESET_VECTOR_ADDR));
+	WARN_ON(ret);
+	dma_unmap_single(NULL, stub_data_phys,
+			 sizeof(_tegra_avp_boot_stub_data),
+			 DMA_TO_DEVICE);
+	return ret;
+}
+
+static void avp_halt(struct tegra_avp_info *avp)
+{
+	/* ensure the AVP is halted */
+	writel(FLOW_MODE_STOP, FLOW_CTRL_HALT_COP_EVENTS);
+	tegra_periph_reset_assert(avp->cop_clk);
+
+	/* set up the initial memory areas and mailbox contents */
+	*((u32 *)avp->msg_from_avp) = 0;
+	*((u32 *)avp->msg_to_avp) = 0xfeedf00d;
+	mbox_writel(0, MBOX_FROM_AVP);
+	mbox_writel(0, MBOX_TO_AVP);
+}
+
+/* Note: CPU_PORT server and AVP_PORT client are registered with the avp
+ * node, but are actually meant to be processed on our side (either
+ * by the svc thread for processing remote calls or by the client
+ * of the char dev for receiving replies for managing remote
+ * libraries/modules. */
+
+static int avp_init(struct tegra_avp_info *avp)
+{
+	const struct firmware *avp_fw;
+	int ret;
+	struct trpc_endpoint *ep;
+	char fw_file[30];
+
+	avp->nvmap_libs = nvmap_create_client(nvmap_dev, "avp_libs");
+	if (IS_ERR_OR_NULL(avp->nvmap_libs)) {
+		pr_err("%s: cannot create libs nvmap client\n", __func__);
+		ret = PTR_ERR(avp->nvmap_libs);
+		goto err_nvmap_create_libs_client;
+	}
+
+	/* put the address of the shared mem area into the mailbox for AVP
+	 * to read out when its kernel boots. */
+	mbox_writel(avp->msg, MBOX_TO_AVP);
+
+#if defined(CONFIG_TEGRA_AVP_KERNEL_ON_MMU) /* Tegra2 with AVP MMU */
+	/* paddr is any address returned from nvmap_pin */
+	/* vaddr is AVP_KERNEL_VIRT_BASE */
+	pr_info("%s: Using AVP MMU to relocate AVP kernel\n", __func__);
+	sprintf(fw_file, "nvrm_avp.bin");
+	avp->reset_addr = AVP_KERNEL_VIRT_BASE;
+#elif defined(CONFIG_TEGRA_AVP_KERNEL_ON_SMMU) /* Tegra3 with SMMU */
+	/* paddr is any address behind SMMU */
+	/* vaddr is TEGRA_SMMU_BASE */
+	pr_info("%s: Using SMMU at %lx to load AVP kernel\n",
+		__func__, (unsigned long)avp->kernel_phys);
+	BUG_ON(avp->kernel_phys != 0xeff00000
+		&& avp->kernel_phys != 0x0ff00000);
+	sprintf(fw_file, "nvrm_avp_%08lx.bin", (unsigned long)avp->kernel_phys);
+	avp->reset_addr = avp->kernel_phys;
+#else /* nvmem= carveout */
+	/* paddr is found in nvmem= carveout */
+	/* vaddr is same as paddr */
+	/* Find nvmem carveout */
+	if (!pfn_valid(__phys_to_pfn(0x8e000000))) {
+		avp->kernel_phys = 0x8e000000;
+	}
+	else if (!pfn_valid(__phys_to_pfn(0x9e000000))) {
+		avp->kernel_phys = 0x9e000000;
+	}
+	else if (!pfn_valid(__phys_to_pfn(0xbe000000))) {
+		avp->kernel_phys = 0xbe000000;
+	}
+	else {
+		pr_err("Cannot find nvmem= carveout to load AVP kernel\n");
+		pr_err("Check kernel command line "
+			"to see if nvmem= is defined\n");
+		BUG();
+	}
+	pr_info("%s: Using nvmem= carveout at %lx to load AVP kernel\n",
+		__func__, (unsigned long)avp->kernel_phys);
+	sprintf(fw_file, "nvrm_avp_%08lx.bin", (unsigned long)avp->kernel_phys);
+	avp->reset_addr = avp->kernel_phys;
+	avp->kernel_data = ioremap(avp->kernel_phys, SZ_1M);
+#endif
+
+	ret = request_firmware(&avp_fw, fw_file, avp->misc_dev.this_device);
+	if (ret) {
+		pr_err("%s: Cannot read firmware '%s'\n", __func__, fw_file);
+		goto err_req_fw;
+	}
+	pr_info("%s: Reading firmware from '%s' (%d bytes)\n", __func__,
+		fw_file, avp_fw->size);
+
+	pr_info("%s: Loading AVP kernel at vaddr=%p paddr=%lx\n",
+		__func__, avp->kernel_data, (unsigned long)avp->kernel_phys);
+	memcpy(avp->kernel_data, avp_fw->data, avp_fw->size);
+	memset(avp->kernel_data + avp_fw->size, 0, SZ_1M - avp_fw->size);
+
+	wmb();
+	release_firmware(avp_fw);
+
+	tegra_init_legacy_irq_cop();
+
+	ret = avp_reset(avp, avp->reset_addr);
+	if (ret) {
+		pr_err("%s: cannot reset the AVP.. aborting..\n", __func__);
+		goto err_reset;
+	}
+
+	enable_irq(avp->mbox_from_avp_pend_irq);
+	/* Initialize the avp_svc *first*. This creates RPC_CPU_PORT to be
+	 * ready for remote commands. Then, connect to the
+	 * remote RPC_AVP_PORT to be able to send library load/unload and
+	 * suspend commands to it */
+	ret = avp_svc_start(avp->avp_svc);
+	if (ret)
+		goto err_avp_svc_start;
+
+	ep = trpc_create_connect(avp->rpc_node, "RPC_AVP_PORT", NULL,
+					  NULL, -1);
+	if (IS_ERR(ep)) {
+		pr_err("%s: can't connect to RPC_AVP_PORT server\n", __func__);
+		ret = PTR_ERR(ep);
+		goto err_rpc_avp_port;
+	}
+	avp->avp_ep = ep;
+
+	avp->initialized = true;
+	smp_wmb();
+	pr_info("%s: avp init done\n", __func__);
+	return 0;
+
+err_rpc_avp_port:
+	avp_svc_stop(avp->avp_svc);
+err_avp_svc_start:
+	disable_irq(avp->mbox_from_avp_pend_irq);
+err_reset:
+	avp_halt(avp);
+err_req_fw:
+	nvmap_client_put(avp->nvmap_libs);
+err_nvmap_create_libs_client:
+	avp->nvmap_libs = NULL;
+	return ret;
+}
+
+static void avp_uninit(struct tegra_avp_info *avp)
+{
+	unsigned long flags;
+	struct rb_node *n;
+	struct remote_info *rinfo;
+
+	spin_lock_irqsave(&avp->state_lock, flags);
+	avp->initialized = false;
+	avp->shutdown = true;
+	spin_unlock_irqrestore(&avp->state_lock, flags);
+
+	disable_irq(avp->mbox_from_avp_pend_irq);
+	cancel_work_sync(&avp->recv_work);
+
+	avp_halt(avp);
+
+	spin_lock_irqsave(&avp->state_lock, flags);
+	while ((n = rb_first(&avp->endpoints)) != NULL) {
+		rinfo = rb_entry(n, struct remote_info, rb_node);
+		rinfo_get(rinfo);
+		remote_remove(avp, rinfo);
+		spin_unlock_irqrestore(&avp->state_lock, flags);
+
+		remote_close(rinfo);
+		rinfo_put(rinfo);
+
+		spin_lock_irqsave(&avp->state_lock, flags);
+	}
+	spin_unlock_irqrestore(&avp->state_lock, flags);
+
+	avp_svc_stop(avp->avp_svc);
+
+	if (avp->avp_ep) {
+		trpc_close(avp->avp_ep);
+		avp->avp_ep = NULL;
+	}
+
+	libs_cleanup(avp);
+
+	avp->shutdown = false;
+	smp_wmb();
+	pr_info("%s: avp teardown done\n", __func__);
+}
+
+/* returns the remote lib handle in lib->handle */
+static int _load_lib(struct tegra_avp_info *avp, struct tegra_avp_lib *lib,
+		     bool from_user)
+{
+	struct svc_lib_attach svc;
+	struct svc_lib_attach_resp resp;
+	const struct firmware *fw;
+	void *args;
+	struct nvmap_handle_ref *lib_handle;
+	void *lib_data;
+	phys_addr_t lib_phys;
+	int ret;
+
+	DBG(AVP_DBG_TRACE_LIB, "avp_lib: loading library '%s'\n", lib->name);
+
+	args = kmalloc(lib->args_len, GFP_KERNEL);
+	if (!args) {
+		pr_err("avp_lib: can't alloc mem for args (%d)\n",
+			lib->args_len);
+		return -ENOMEM;
+	}
+
+	if (!from_user)
+		memcpy(args, lib->args, lib->args_len);
+	else if (copy_from_user(args, lib->args, lib->args_len)) {
+		pr_err("avp_lib: can't copy lib args\n");
+		ret = -EFAULT;
+		goto err_cp_args;
+	}
+
+	ret = request_firmware(&fw, lib->name, avp->misc_dev.this_device);
+	if (ret) {
+		pr_err("avp_lib: Cannot read firmware '%s'\n", lib->name);
+		goto err_req_fw;
+	}
+
+	lib_handle = nvmap_alloc(avp->nvmap_libs, fw->size, L1_CACHE_BYTES,
+				 NVMAP_HANDLE_UNCACHEABLE, 0);
+	if (IS_ERR_OR_NULL(lib_handle)) {
+		pr_err("avp_lib: can't nvmap alloc for lib '%s'\n", lib->name);
+		ret = PTR_ERR(lib_handle);
+		goto err_nvmap_alloc;
+	}
+
+	lib_data = nvmap_mmap(lib_handle);
+	if (!lib_data) {
+		pr_err("avp_lib: can't nvmap map for lib '%s'\n", lib->name);
+		ret = -ENOMEM;
+		goto err_nvmap_mmap;
+	}
+
+	lib_phys = nvmap_pin(avp->nvmap_libs, lib_handle);
+	if (IS_ERR_VALUE(lib_phys)) {
+		pr_err("avp_lib: can't nvmap pin for lib '%s'\n", lib->name);
+		ret = lib_phys;
+		goto err_nvmap_pin;
+	}
+
+	memcpy(lib_data, fw->data, fw->size);
+
+	svc.svc_id = SVC_LIBRARY_ATTACH;
+	svc.address = lib_phys;
+	svc.args_len = lib->args_len;
+	svc.lib_size = fw->size;
+	svc.reason = lib->greedy ? AVP_LIB_REASON_ATTACH_GREEDY :
+		AVP_LIB_REASON_ATTACH;
+	memcpy(svc.args, args, lib->args_len);
+	wmb();
+
+	/* send message, wait for reply */
+	ret = trpc_send_msg(avp->rpc_node, avp->avp_ep, &svc, sizeof(svc),
+				 GFP_KERNEL);
+	if (ret)
+		goto err_send_msg;
+
+	ret = trpc_recv_msg(avp->rpc_node, avp->avp_ep, &resp,
+				 sizeof(resp), -1);
+	if (ret != sizeof(resp)) {
+		pr_err("avp_lib: Couldn't get lib load reply (%d)\n", ret);
+		goto err_recv_msg;
+	} else if (resp.err) {
+		pr_err("avp_lib: got remote error (%d) while loading lib %s\n",
+			   resp.err, lib->name);
+		ret = -EPROTO;
+		goto err_recv_msg;
+	}
+	lib->handle = resp.lib_id;
+	ret = 0;
+	DBG(AVP_DBG_TRACE_LIB,
+		"avp_lib: Successfully loaded library %s (lib_id=%x)\n",
+		lib->name, resp.lib_id);
+
+	/* We free the memory here because by this point the AVP has already
+	 * requested memory for the library for all the sections since it does
+	 * it's own relocation and memory management. So, our allocations were
+	 * temporary to hand the library code over to the AVP.
+	 */
+
+err_recv_msg:
+err_send_msg:
+	nvmap_unpin(avp->nvmap_libs, lib_handle);
+err_nvmap_pin:
+	nvmap_munmap(lib_handle, lib_data);
+err_nvmap_mmap:
+	nvmap_free(avp->nvmap_libs, lib_handle);
+err_nvmap_alloc:
+	release_firmware(fw);
+err_req_fw:
+err_cp_args:
+	kfree(args);
+	return ret;
+}
+
+static int send_unload_lib_msg(struct tegra_avp_info *avp, u32 handle,
+				   const char *name)
+{
+	struct svc_lib_detach svc;
+	struct svc_lib_detach_resp resp;
+	int ret;
+
+	svc.svc_id = SVC_LIBRARY_DETACH;
+	svc.reason = AVP_LIB_REASON_DETACH;
+	svc.lib_id = handle;
+
+	ret = trpc_send_msg(avp->rpc_node, avp->avp_ep, &svc, sizeof(svc),
+				 GFP_KERNEL);
+	if (ret) {
+		pr_err("avp_lib: can't send unload message to avp for '%s'\n",
+			   name);
+		goto err;
+	}
+
+	/* Give it a few extra moments to unload. */
+	msleep(20);
+
+	ret = trpc_recv_msg(avp->rpc_node, avp->avp_ep, &resp,
+				 sizeof(resp), -1);
+	if (ret != sizeof(resp)) {
+		pr_err("avp_lib: Couldn't get unload reply for '%s' (%d)\n",
+			   name, ret);
+	} else if (resp.err) {
+		pr_err("avp_lib: remote error (%d) while unloading lib %s\n",
+			   resp.err, name);
+		ret = -EPROTO;
+	} else {
+		pr_info("avp_lib: Successfully unloaded '%s'\n",
+			   name);
+		ret = 0;
+	}
+
+err:
+	return ret;
+}
+
+static struct lib_item *_find_lib_locked(struct tegra_avp_info *avp, u32 handle)
+{
+	struct lib_item *item;
+
+	list_for_each_entry(item, &avp->libs, list) {
+		if (item->handle == handle)
+			return item;
+	}
+	return NULL;
+}
+
+static int _insert_lib_locked(struct tegra_avp_info *avp, u32 handle,
+			      char *name)
+{
+	struct lib_item *item;
+
+	item = kzalloc(sizeof(struct lib_item), GFP_KERNEL);
+	if (!item)
+		return -ENOMEM;
+	item->handle = handle;
+	strlcpy(item->name, name, TEGRA_AVP_LIB_MAX_NAME);
+	list_add_tail(&item->list, &avp->libs);
+	return 0;
+}
+
+static void _delete_lib_locked(struct tegra_avp_info *avp,
+			       struct lib_item *item)
+{
+	list_del(&item->list);
+	kfree(item);
+}
+
+static int handle_load_lib_ioctl(struct tegra_avp_info *avp, unsigned long arg)
+{
+	struct tegra_avp_lib lib;
+	int ret;
+
+	pr_debug("%s: ioctl\n", __func__);
+	if (copy_from_user(&lib, (void __user *)arg, sizeof(lib)))
+		return -EFAULT;
+	lib.name[TEGRA_AVP_LIB_MAX_NAME - 1] = '\0';
+
+	if (lib.args_len > TEGRA_AVP_LIB_MAX_ARGS) {
+		pr_err("%s: library args too long (%d)\n", __func__,
+			lib.args_len);
+		return -E2BIG;
+	}
+
+	mutex_lock(&avp->libs_lock);
+	ret = _load_lib(avp, &lib, true);
+	if (ret)
+		goto err_load_lib;
+
+	if (copy_to_user((void __user *)arg, &lib, sizeof(lib))) {
+		/* TODO: probably need to free the library from remote
+		 * we just loaded */
+		ret = -EFAULT;
+		goto err_copy_to_user;
+	}
+	ret = _insert_lib_locked(avp, lib.handle, lib.name);
+	if (ret) {
+		pr_err("%s: can't insert lib (%d)\n", __func__, ret);
+		goto err_insert_lib;
+	}
+
+	mutex_unlock(&avp->libs_lock);
+	return 0;
+
+err_insert_lib:
+err_copy_to_user:
+	send_unload_lib_msg(avp, lib.handle, lib.name);
+err_load_lib:
+	mutex_unlock(&avp->libs_lock);
+	return ret;
+}
+
+static void libs_cleanup(struct tegra_avp_info *avp)
+{
+	struct lib_item *lib;
+	struct lib_item *lib_tmp;
+
+	mutex_lock(&avp->libs_lock);
+	list_for_each_entry_safe(lib, lib_tmp, &avp->libs, list) {
+		_delete_lib_locked(avp, lib);
+	}
+
+	nvmap_client_put(avp->nvmap_libs);
+	avp->nvmap_libs = NULL;
+	mutex_unlock(&avp->libs_lock);
+}
+
+static long tegra_avp_ioctl(struct file *file, unsigned int cmd,
+				unsigned long arg)
+{
+	struct tegra_avp_info *avp = tegra_avp;
+	int ret;
+
+	if (_IOC_TYPE(cmd) != TEGRA_AVP_IOCTL_MAGIC ||
+		_IOC_NR(cmd) < TEGRA_AVP_IOCTL_MIN_NR ||
+		_IOC_NR(cmd) > TEGRA_AVP_IOCTL_MAX_NR)
+		return -ENOTTY;
+
+	switch (cmd) {
+	case TEGRA_AVP_IOCTL_LOAD_LIB:
+		ret = handle_load_lib_ioctl(avp, arg);
+		break;
+	case TEGRA_AVP_IOCTL_UNLOAD_LIB:
+		ret = tegra_avp_unload_lib(avp, arg);
+		break;
+	default:
+		pr_err("avp_lib: Unknown tegra_avp ioctl 0x%x\n", _IOC_NR(cmd));
+		ret = -ENOTTY;
+		break;
+	}
+	return ret;
+}
+
+int tegra_avp_open(struct tegra_avp_info **avp)
+{
+	struct tegra_avp_info *new_avp = tegra_avp;
+	int ret = 0;
+
+	pr_debug("%s: open\n", __func__);
+	mutex_lock(&new_avp->open_lock);
+
+	if (!new_avp->refcount)
+		ret = avp_init(new_avp);
+
+	if (ret < 0) {
+		mutex_unlock(&new_avp->open_lock);
+		new_avp = 0;
+		goto out;
+	}
+
+	new_avp->refcount++;
+
+	mutex_unlock(&new_avp->open_lock);
+out:
+	*avp = new_avp;
+	return ret;
+}
+
+static int tegra_avp_open_fops(struct inode *inode, struct file *file)
+{
+	struct tegra_avp_info *avp;
+
+	nonseekable_open(inode, file);
+	return tegra_avp_open(&avp);
+}
+
+int tegra_avp_release(struct tegra_avp_info *avp)
+{
+	int ret = 0;
+
+	pr_debug("%s: close\n", __func__);
+	mutex_lock(&avp->open_lock);
+	if (!avp->refcount) {
+		pr_err("%s: releasing while in invalid state\n", __func__);
+		ret = -EINVAL;
+		goto out;
+	}
+	if (avp->refcount > 0)
+		avp->refcount--;
+	if (!avp->refcount)
+		avp_uninit(avp);
+
+out:
+	mutex_unlock(&avp->open_lock);
+	return ret;
+}
+
+static int tegra_avp_release_fops(struct inode *inode, struct file *file)
+{
+	struct tegra_avp_info *avp = tegra_avp;
+	return tegra_avp_release(avp);
+}
+
+static int avp_enter_lp0(struct tegra_avp_info *avp)
+{
+	volatile u32 *avp_suspend_done = avp->iram_backup_data
+		+ TEGRA_IRAM_SIZE - TEGRA_RESET_HANDLER_SIZE;
+	struct svc_enter_lp0 svc;
+	unsigned long endtime;
+	int ret;
+
+	svc.svc_id = SVC_ENTER_LP0;
+	svc.src_addr = (u32)TEGRA_IRAM_BASE + TEGRA_RESET_HANDLER_SIZE;
+	svc.buf_addr = (u32)avp->iram_backup_phys;
+	svc.buf_size = TEGRA_IRAM_SIZE - TEGRA_RESET_HANDLER_SIZE;
+
+	*avp_suspend_done = 0;
+	wmb();
+
+	ret = trpc_send_msg(avp->rpc_node, avp->avp_ep, &svc, sizeof(svc),
+				 GFP_KERNEL);
+	if (ret) {
+		pr_err("%s: cannot send AVP suspend message\n", __func__);
+		return ret;
+	}
+
+	endtime = jiffies + msecs_to_jiffies(1000);
+	rmb();
+	while ((*avp_suspend_done == 0) && time_before(jiffies, endtime)) {
+		udelay(10);
+		rmb();
+	}
+
+	rmb();
+	if (*avp_suspend_done == 0) {
+		pr_err("%s: AVP failed to suspend\n", __func__);
+		ret = -ETIMEDOUT;
+		goto err;
+	}
+
+	return 0;
+
+err:
+	return ret;
+}
+
+static int tegra_avp_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct tegra_avp_info *avp = tegra_avp;
+	unsigned long flags;
+	int ret;
+
+	pr_info("%s()+\n", __func__);
+	spin_lock_irqsave(&avp->state_lock, flags);
+	if (!avp->initialized) {
+		spin_unlock_irqrestore(&avp->state_lock, flags);
+		return 0;
+	}
+	avp->suspending = true;
+	spin_unlock_irqrestore(&avp->state_lock, flags);
+
+	ret = avp_enter_lp0(avp);
+	if (ret)
+		goto err;
+
+	avp->resume_addr = readl(TEGRA_AVP_RESUME_ADDR);
+	if (!avp->resume_addr) {
+		pr_err("%s: AVP failed to set it's resume address\n", __func__);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	disable_irq(avp->mbox_from_avp_pend_irq);
+
+	pr_info("avp_suspend: resume_addr=%lx\n", avp->resume_addr);
+	avp->resume_addr &= 0xfffffffeUL;
+	pr_info("%s()-\n", __func__);
+
+	return 0;
+
+err:
+	/* TODO: we need to kill the AVP so that when we come back
+	 * it could be reinitialized.. We'd probably need to kill
+	 * the users of it so they don't have the wrong state.
+	 */
+	return ret;
+}
+
+static int tegra_avp_resume(struct platform_device *pdev)
+{
+	struct tegra_avp_info *avp = tegra_avp;
+	int ret = 0;
+
+	pr_info("%s()+\n", __func__);
+	smp_rmb();
+	if (!avp->initialized)
+		goto out;
+
+	BUG_ON(!avp->resume_addr);
+
+	avp_reset(avp, avp->resume_addr);
+	avp->resume_addr = 0;
+	avp->suspending = false;
+	smp_wmb();
+	enable_irq(avp->mbox_from_avp_pend_irq);
+
+	pr_info("%s()-\n", __func__);
+
+out:
+	return ret;
+}
+
+static const struct file_operations tegra_avp_fops = {
+	.owner		= THIS_MODULE,
+	.open		= tegra_avp_open_fops,
+	.release	= tegra_avp_release_fops,
+	.unlocked_ioctl = tegra_avp_ioctl,
+};
+
+static struct trpc_node avp_trpc_node = {
+	.name		= "avp-remote",
+	.type		= TRPC_NODE_REMOTE,
+	.try_connect	= avp_node_try_connect,
+};
+
+static int tegra_avp_probe(struct platform_device *pdev)
+{
+	void *msg_area;
+	struct tegra_avp_info *avp;
+	int ret = 0;
+	int irq;
+	unsigned int heap_mask;
+
+	irq = platform_get_irq_byname(pdev, "mbox_from_avp_pending");
+	if (irq < 0) {
+		pr_err("%s: invalid platform data\n", __func__);
+		return -EINVAL;
+	}
+
+	avp = kzalloc(sizeof(struct tegra_avp_info), GFP_KERNEL);
+	if (!avp) {
+		pr_err("%s: cannot allocate tegra_avp_info\n", __func__);
+		return -ENOMEM;
+	}
+
+	avp->nvmap_drv = nvmap_create_client(nvmap_dev, "avp_core");
+	if (IS_ERR_OR_NULL(avp->nvmap_drv)) {
+		pr_err("%s: cannot create drv nvmap client\n", __func__);
+		ret = PTR_ERR(avp->nvmap_drv);
+		goto err_nvmap_create_drv_client;
+	}
+
+#if defined(CONFIG_TEGRA_AVP_KERNEL_ON_MMU) /* Tegra2 with AVP MMU */
+	heap_mask = NVMAP_HEAP_CARVEOUT_GENERIC;
+#elif defined(CONFIG_TEGRA_AVP_KERNEL_ON_SMMU) /* Tegra3 with SMMU */
+	heap_mask = NVMAP_HEAP_IOVMM;
+#else /* nvmem= carveout */
+	heap_mask = 0;
+#endif
+
+	if (heap_mask == NVMAP_HEAP_IOVMM) {
+		int i;
+		/* Tegra3 A01 has different SMMU address in 0xe00000000- */
+		u32 iovmm_addr[] = {0x0ff00000, 0xeff00000};
+
+		for (i = 0; i < ARRAY_SIZE(iovmm_addr); i++) {
+			avp->kernel_handle = nvmap_alloc_iovm(avp->nvmap_drv,
+						SZ_1M, L1_CACHE_BYTES,
+						NVMAP_HANDLE_WRITE_COMBINE,
+						iovmm_addr[i]);
+			if (!IS_ERR_OR_NULL(avp->kernel_handle))
+				break;
+		}
+		if (IS_ERR_OR_NULL(avp->kernel_handle)) {
+			pr_err("%s: cannot create handle\n", __func__);
+			ret = PTR_ERR(avp->kernel_handle);
+			goto err_nvmap_alloc;
+		}
+
+		avp->kernel_data = nvmap_mmap(avp->kernel_handle);
+		if (!avp->kernel_data) {
+			pr_err("%s: cannot map kernel handle\n", __func__);
+			ret = -ENOMEM;
+			goto err_nvmap_mmap;
+		}
+
+		avp->kernel_phys =
+			nvmap_pin(avp->nvmap_drv, avp->kernel_handle);
+		if (IS_ERR_VALUE(avp->kernel_phys)) {
+			pr_err("%s: cannot pin kernel handle\n", __func__);
+			ret = avp->kernel_phys;
+			goto err_nvmap_pin;
+		}
+
+		pr_info("%s: allocated IOVM at %lx for AVP kernel\n",
+			__func__, (unsigned long)avp->kernel_phys);
+	}
+
+	if (heap_mask == NVMAP_HEAP_CARVEOUT_GENERIC) {
+		avp->kernel_handle = nvmap_alloc(avp->nvmap_drv, SZ_1M, SZ_1M,
+						NVMAP_HANDLE_UNCACHEABLE, 0);
+		if (IS_ERR_OR_NULL(avp->kernel_handle)) {
+			pr_err("%s: cannot create handle\n", __func__);
+			ret = PTR_ERR(avp->kernel_handle);
+			goto err_nvmap_alloc;
+		}
+
+		avp->kernel_data = nvmap_mmap(avp->kernel_handle);
+		if (!avp->kernel_data) {
+			pr_err("%s: cannot map kernel handle\n", __func__);
+			ret = -ENOMEM;
+			goto err_nvmap_mmap;
+		}
+
+		avp->kernel_phys = nvmap_pin(avp->nvmap_drv,
+					avp->kernel_handle);
+		if (IS_ERR_VALUE(avp->kernel_phys)) {
+			pr_err("%s: cannot pin kernel handle\n", __func__);
+			ret = avp->kernel_phys;
+			goto err_nvmap_pin;
+		}
+
+		pr_info("%s: allocated carveout memory at %lx for AVP kernel\n",
+			__func__, (unsigned long)avp->kernel_phys);
+	}
+
+	/* allocate an extra 4 bytes at the end which AVP uses to signal to
+	 * us that it is done suspending.
+	 */
+	avp->iram_backup_handle =
+		nvmap_alloc(avp->nvmap_drv, TEGRA_IRAM_SIZE + 4,
+				L1_CACHE_BYTES, NVMAP_HANDLE_UNCACHEABLE, 0);
+	if (IS_ERR_OR_NULL(avp->iram_backup_handle)) {
+		pr_err("%s: cannot create handle for iram backup\n", __func__);
+		ret = PTR_ERR(avp->iram_backup_handle);
+		goto err_iram_nvmap_alloc;
+	}
+	avp->iram_backup_data = nvmap_mmap(avp->iram_backup_handle);
+	if (!avp->iram_backup_data) {
+		pr_err("%s: cannot map iram backup handle\n", __func__);
+		ret = -ENOMEM;
+		goto err_iram_nvmap_mmap;
+	}
+	avp->iram_backup_phys = nvmap_pin(avp->nvmap_drv,
+					  avp->iram_backup_handle);
+	if (IS_ERR_VALUE(avp->iram_backup_phys)) {
+		pr_err("%s: cannot pin iram backup handle\n", __func__);
+		ret = avp->iram_backup_phys;
+		goto err_iram_nvmap_pin;
+	}
+
+	avp->mbox_from_avp_pend_irq = irq;
+	avp->endpoints = RB_ROOT;
+	spin_lock_init(&avp->state_lock);
+	mutex_init(&avp->open_lock);
+	mutex_init(&avp->to_avp_lock);
+	mutex_init(&avp->from_avp_lock);
+	INIT_WORK(&avp->recv_work, process_avp_message);
+
+	mutex_init(&avp->libs_lock);
+	INIT_LIST_HEAD(&avp->libs);
+
+	avp->recv_wq = alloc_workqueue("avp-msg-recv",
+					   WQ_NON_REENTRANT | WQ_HIGHPRI, 1);
+	if (!avp->recv_wq) {
+		pr_err("%s: can't create recve workqueue\n", __func__);
+		ret = -ENOMEM;
+		goto err_create_wq;
+	}
+
+	avp->cop_clk = clk_get(&pdev->dev, "cop");
+	if (IS_ERR_OR_NULL(avp->cop_clk)) {
+		pr_err("%s: Couldn't get cop clock\n", TEGRA_AVP_NAME);
+		ret = -ENOENT;
+		goto err_get_cop_clk;
+	}
+
+	msg_area = dma_alloc_coherent(&pdev->dev, AVP_MSG_AREA_SIZE * 2,
+					  &avp->msg_area_addr, GFP_KERNEL);
+	if (!msg_area) {
+		pr_err("%s: cannot allocate msg_area\n", __func__);
+		ret = -ENOMEM;
+		goto err_alloc_msg_area;
+	}
+	memset(msg_area, 0, AVP_MSG_AREA_SIZE * 2);
+	avp->msg = ((avp->msg_area_addr >> 4) |
+			MBOX_MSG_VALID | MBOX_MSG_PENDING_INT_EN);
+	avp->msg_to_avp = msg_area;
+	avp->msg_from_avp = msg_area + AVP_MSG_AREA_SIZE;
+
+	avp_halt(avp);
+
+	avp_trpc_node.priv = avp;
+	ret = trpc_node_register(&avp_trpc_node);
+	if (ret) {
+		pr_err("%s: Can't register avp rpc node\n", __func__);
+		goto err_node_reg;
+	}
+	avp->rpc_node = &avp_trpc_node;
+
+	avp->avp_svc = avp_svc_init(pdev, avp->rpc_node);
+	if (IS_ERR_OR_NULL(avp->avp_svc)) {
+		pr_err("%s: Cannot initialize avp_svc\n", __func__);
+		ret = PTR_ERR(avp->avp_svc);
+		goto err_avp_svc_init;
+	}
+
+	avp->misc_dev.minor = MISC_DYNAMIC_MINOR;
+	avp->misc_dev.name = "tegra_avp";
+	avp->misc_dev.fops = &tegra_avp_fops;
+
+	ret = misc_register(&avp->misc_dev);
+	if (ret) {
+		pr_err("%s: Unable to register misc device!\n", TEGRA_AVP_NAME);
+		goto err_misc_reg;
+	}
+
+	ret = request_irq(irq, avp_mbox_pending_isr, 0, TEGRA_AVP_NAME, avp);
+	if (ret) {
+		pr_err("%s: cannot register irq handler\n", __func__);
+		goto err_req_irq_pend;
+	}
+	disable_irq(avp->mbox_from_avp_pend_irq);
+
+	tegra_avp = avp;
+
+	pr_info("%s: message area %lx/%lx\n", __func__,
+		(unsigned long)avp->msg_area_addr,
+		(unsigned long)avp->msg_area_addr + AVP_MSG_AREA_SIZE);
+
+	return 0;
+
+err_req_irq_pend:
+	misc_deregister(&avp->misc_dev);
+err_misc_reg:
+	avp_svc_destroy(avp->avp_svc);
+err_avp_svc_init:
+	trpc_node_unregister(avp->rpc_node);
+err_node_reg:
+	dma_free_coherent(&pdev->dev, AVP_MSG_AREA_SIZE * 2, msg_area,
+			  avp->msg_area_addr);
+err_alloc_msg_area:
+	clk_put(avp->cop_clk);
+err_get_cop_clk:
+	destroy_workqueue(avp->recv_wq);
+err_create_wq:
+	nvmap_unpin(avp->nvmap_drv, avp->iram_backup_handle);
+err_iram_nvmap_pin:
+	nvmap_munmap(avp->iram_backup_handle, avp->iram_backup_data);
+err_iram_nvmap_mmap:
+	nvmap_free(avp->nvmap_drv, avp->iram_backup_handle);
+err_iram_nvmap_alloc:
+	nvmap_unpin(avp->nvmap_drv, avp->kernel_handle);
+err_nvmap_pin:
+	nvmap_munmap(avp->kernel_handle, avp->kernel_data);
+err_nvmap_mmap:
+	nvmap_free(avp->nvmap_drv, avp->kernel_handle);
+err_nvmap_alloc:
+	nvmap_client_put(avp->nvmap_drv);
+err_nvmap_create_drv_client:
+	kfree(avp);
+	tegra_avp = NULL;
+	return ret;
+}
+
+static int tegra_avp_remove(struct platform_device *pdev)
+{
+	struct tegra_avp_info *avp = tegra_avp;
+
+	if (!avp)
+		return 0;
+
+	mutex_lock(&avp->open_lock);
+	/* ensure that noone can open while we tear down */
+	if (avp->refcount) {
+		mutex_unlock(&avp->open_lock);
+		return -EBUSY;
+	}
+	mutex_unlock(&avp->open_lock);
+
+	misc_deregister(&avp->misc_dev);
+
+	avp_halt(avp);
+
+	avp_svc_destroy(avp->avp_svc);
+	trpc_node_unregister(avp->rpc_node);
+	dma_free_coherent(&pdev->dev, AVP_MSG_AREA_SIZE * 2, avp->msg_to_avp,
+			  avp->msg_area_addr);
+	clk_put(avp->cop_clk);
+	destroy_workqueue(avp->recv_wq);
+	nvmap_unpin(avp->nvmap_drv, avp->iram_backup_handle);
+	nvmap_munmap(avp->iram_backup_handle, avp->iram_backup_data);
+	nvmap_free(avp->nvmap_drv, avp->iram_backup_handle);
+	nvmap_unpin(avp->nvmap_drv, avp->kernel_handle);
+	nvmap_munmap(avp->kernel_handle, avp->kernel_data);
+	nvmap_free(avp->nvmap_drv, avp->kernel_handle);
+	nvmap_client_put(avp->nvmap_drv);
+	kfree(avp);
+	tegra_avp = NULL;
+	return 0;
+}
+
+int tegra_avp_load_lib(struct tegra_avp_info *avp, struct tegra_avp_lib *lib)
+{
+	int ret;
+
+	if (!avp)
+		return -ENODEV;
+
+	if (!lib)
+		return -EFAULT;
+
+	lib->name[TEGRA_AVP_LIB_MAX_NAME - 1] = '\0';
+
+	if (lib->args_len > TEGRA_AVP_LIB_MAX_ARGS) {
+		pr_err("%s: library args too long (%d)\n", __func__,
+			lib->args_len);
+		return -E2BIG;
+	}
+
+	mutex_lock(&avp->libs_lock);
+	ret = _load_lib(avp, lib, false);
+	if (ret)
+		goto err_load_lib;
+
+	ret = _insert_lib_locked(avp, lib->handle, lib->name);
+	if (ret) {
+		pr_err("%s: can't insert lib (%d)\n", __func__, ret);
+		goto err_insert_lib;
+	}
+
+	mutex_unlock(&avp->libs_lock);
+	return 0;
+
+err_insert_lib:
+	ret = send_unload_lib_msg(avp, lib->handle, lib->name);
+	if (!ret)
+		DBG(AVP_DBG_TRACE_LIB, "avp_lib: unloaded '%s'\n", lib->name);
+	else
+		pr_err("avp_lib: can't unload lib '%s' (%d)\n", lib->name, ret);
+	lib->handle = 0;
+err_load_lib:
+	mutex_unlock(&avp->libs_lock);
+	return ret;
+}
+
+int tegra_avp_unload_lib(struct tegra_avp_info *avp, unsigned long handle)
+{
+	struct lib_item *item;
+	int ret;
+
+	if (!avp)
+		return -ENODEV;
+
+	mutex_lock(&avp->libs_lock);
+	item = _find_lib_locked(avp, handle);
+	if (!item) {
+		pr_err("avp_lib: avp lib with handle 0x%x not found\n",
+			   (u32)handle);
+		ret = -ENOENT;
+		goto err_find;
+	}
+	ret = send_unload_lib_msg(avp, item->handle, item->name);
+	if (!ret)
+		DBG(AVP_DBG_TRACE_LIB, "avp_lib: unloaded '%s'\n", item->name);
+	else
+		pr_err("avp_lib: can't unload lib '%s'/0x%x (%d)\n", item->name,
+			   item->handle, ret);
+	_delete_lib_locked(avp, item);
+
+err_find:
+	mutex_unlock(&avp->libs_lock);
+	return ret;
+}
+
+static struct platform_driver tegra_avp_driver = {
+	.probe		= tegra_avp_probe,
+	.remove		= tegra_avp_remove,
+	.suspend	= tegra_avp_suspend,
+	.resume		= tegra_avp_resume,
+	.driver = {
+		.name	= TEGRA_AVP_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init tegra_avp_init(void)
+{
+	return platform_driver_register(&tegra_avp_driver);
+}
+
+static void __exit tegra_avp_exit(void)
+{
+	platform_driver_unregister(&tegra_avp_driver);
+}
+
+module_init(tegra_avp_init);
+module_exit(tegra_avp_exit);
diff --git a/drivers/media/video/tegra/avp/avp.h b/drivers/media/video/tegra/avp/avp.h
new file mode 100644
index 000000000000..4f2287743a06
--- /dev/null
+++ b/drivers/media/video/tegra/avp/avp.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Dima Zavin <dima@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 __MEDIA_VIDEO_TEGRA_AVP_H
+#define __MEDIA_VIDEO_TEGRA_AVP_H
+
+#include <linux/platform_device.h>
+#include <linux/types.h>
+
+#include "trpc.h"
+
+struct avp_svc_info;
+
+struct avp_svc_info *avp_svc_init(struct platform_device *pdev,
+				  struct trpc_node *rpc_node);
+void avp_svc_destroy(struct avp_svc_info *avp_svc);
+int avp_svc_start(struct avp_svc_info *svc);
+void avp_svc_stop(struct avp_svc_info *svc);
+
+#endif
diff --git a/drivers/media/video/tegra/avp/avp_msg.h b/drivers/media/video/tegra/avp/avp_msg.h
new file mode 100644
index 000000000000..615d890d5444
--- /dev/null
+++ b/drivers/media/video/tegra/avp/avp_msg.h
@@ -0,0 +1,358 @@
+/* drivers/media/video/tegra/avp/avp_msg.h
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Dima Zavin <dima@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 __MEDIA_VIDEO_TEGRA_AVP_MSG_H
+#define __MEDIA_VIDEO_TEGRA_AVP_MSG_H
+
+#include <linux/tegra_avp.h>
+#include <linux/types.h>
+
+/* Note: the port name string is not NUL terminated, so make sure to
+ * allocate appropriate space locally when operating on the string */
+#define XPC_PORT_NAME_LEN		16
+
+#define SVC_ARGS_MAX_LEN		220
+#define SVC_MAX_STRING_LEN		200
+
+#define AVP_ERR_ENOTSUP			0x2
+#define AVP_ERR_EINVAL			0x4
+#define AVP_ERR_ENOMEM			0x6
+#define AVP_ERR_EACCES			0x00030010
+
+enum {
+	SVC_NVMAP_CREATE		= 0,
+	SVC_NVMAP_CREATE_RESPONSE	= 1,
+	SVC_NVMAP_FREE			= 3,
+	SVC_NVMAP_ALLOC			= 4,
+	SVC_NVMAP_ALLOC_RESPONSE	= 5,
+	SVC_NVMAP_PIN			= 6,
+	SVC_NVMAP_PIN_RESPONSE		= 7,
+	SVC_NVMAP_UNPIN			= 8,
+	SVC_NVMAP_UNPIN_RESPONSE	= 9,
+	SVC_NVMAP_GET_ADDRESS		= 10,
+	SVC_NVMAP_GET_ADDRESS_RESPONSE	= 11,
+	SVC_NVMAP_FROM_ID		= 12,
+	SVC_NVMAP_FROM_ID_RESPONSE	= 13,
+	SVC_MODULE_CLOCK		= 14,
+	SVC_MODULE_CLOCK_RESPONSE	= 15,
+	SVC_MODULE_RESET		= 16,
+	SVC_MODULE_RESET_RESPONSE	= 17,
+	SVC_POWER_REGISTER		= 18,
+	SVC_POWER_UNREGISTER		= 19,
+	SVC_POWER_STARVATION		= 20,
+	SVC_POWER_BUSY_HINT		= 21,
+	SVC_POWER_BUSY_HINT_MULTI	= 22,
+	SVC_DFS_GETSTATE		= 23,
+	SVC_DFS_GETSTATE_RESPONSE	= 24,
+	SVC_POWER_RESPONSE		= 25,
+	SVC_POWER_MAXFREQ		= 26,
+	SVC_ENTER_LP0			= 27,
+	SVC_ENTER_LP0_RESPONSE		= 28,
+	SVC_PRINTF			= 29,
+	SVC_LIBRARY_ATTACH		= 30,
+	SVC_LIBRARY_ATTACH_RESPONSE	= 31,
+	SVC_LIBRARY_DETACH		= 32,
+	SVC_LIBRARY_DETACH_RESPONSE	= 33,
+	SVC_AVP_WDT_RESET		= 34,
+	SVC_DFS_GET_CLK_UTIL		= 35,
+	SVC_DFS_GET_CLK_UTIL_RESPONSE	= 36,
+	SVC_MODULE_CLOCK_SET		= 37,
+	SVC_MODULE_CLOCK_SET_RESPONSE	= 38,
+	SVC_MODULE_CLOCK_GET		= 39,
+	SVC_MODULE_CLOCK_GET_RESPONSE	= 40,
+};
+
+struct svc_msg {
+	u32	svc_id;
+	u8	data[0];
+};
+
+struct svc_common_resp {
+	u32	svc_id;
+	u32	err;
+};
+
+struct svc_printf {
+	u32		svc_id;
+	const char	str[SVC_MAX_STRING_LEN];
+};
+
+struct svc_enter_lp0 {
+	u32	svc_id;
+	u32	src_addr;
+	u32	buf_addr;
+	u32	buf_size;
+};
+
+/* nvmap messages */
+struct svc_nvmap_create {
+	u32	svc_id;
+	u32	size;
+};
+
+struct svc_nvmap_create_resp {
+	u32	svc_id;
+	u32	handle_id;
+	u32	err;
+};
+
+enum {
+	AVP_NVMAP_HEAP_EXTERNAL			= 1,
+	AVP_NVMAP_HEAP_GART			= 2,
+	AVP_NVMAP_HEAP_EXTERNAL_CARVEOUT	= 3,
+	AVP_NVMAP_HEAP_IRAM			= 4,
+};
+
+struct svc_nvmap_alloc {
+	u32	svc_id;
+	u32	handle_id;
+	u32	heaps[4];
+	u32	num_heaps;
+	u32	align;
+	u32	mapping_type;
+};
+
+struct svc_nvmap_free {
+	u32	svc_id;
+	u32	handle_id;
+};
+
+struct svc_nvmap_pin {
+	u32	svc_id;
+	u32	handle_id;
+};
+
+struct svc_nvmap_pin_resp {
+	u32	svc_id;
+	u32	addr;
+};
+
+struct svc_nvmap_unpin {
+	u32	svc_id;
+	u32	handle_id;
+};
+
+struct svc_nvmap_from_id {
+	u32	svc_id;
+	u32	handle_id;
+};
+
+struct svc_nvmap_get_addr {
+	u32	svc_id;
+	u32	handle_id;
+	u32	offs;
+};
+
+struct svc_nvmap_get_addr_resp {
+	u32	svc_id;
+	u32	addr;
+};
+
+/* library management messages */
+enum {
+	AVP_LIB_REASON_ATTACH		= 0,
+	AVP_LIB_REASON_DETACH		= 1,
+	AVP_LIB_REASON_ATTACH_GREEDY	= 2,
+};
+
+struct svc_lib_attach {
+	u32	svc_id;
+	u32	address;
+	u32	args_len;
+	u32	lib_size;
+	u8	args[SVC_ARGS_MAX_LEN];
+	u32	reason;
+};
+
+struct svc_lib_attach_resp {
+	u32	svc_id;
+	u32	err;
+	u32	lib_id;
+};
+
+struct svc_lib_detach {
+	u32	svc_id;
+	u32	reason;
+	u32	lib_id;
+};
+
+struct svc_lib_detach_resp {
+	u32	svc_id;
+	u32	err;
+};
+
+/* hw module management from the AVP side */
+enum {
+	AVP_MODULE_ID_AVP	= 2,
+	AVP_MODULE_ID_VCP	= 3,
+	AVP_MODULE_ID_BSEA	= 27,
+	AVP_MODULE_ID_VDE	= 28,
+	AVP_MODULE_ID_MPE	= 29,
+};
+
+struct svc_module_ctrl {
+	u32	svc_id;
+	u32	module_id;
+	u32	client_id;
+	u8	enable;
+};
+
+struct svc_clock_ctrl {
+	u32	svc_id;
+	u32	module_id;
+	u32	clk_freq;
+};
+
+struct svc_clock_ctrl_response {
+	u32	svc_id;
+	u32	err;
+	u32	act_freq;
+};
+
+/* power messages */
+struct svc_pwr_register {
+	u32	svc_id;
+	u32	client_id;
+	u32	unused;
+};
+
+struct svc_pwr_register_resp {
+	u32	svc_id;
+	u32	err;
+	u32	client_id;
+};
+
+struct svc_pwr_starve_hint {
+	u32	svc_id;
+	u32	dfs_clk_id;
+	u32	client_id;
+	u8	starving;
+};
+
+struct svc_pwr_busy_hint {
+	u32	svc_id;
+	u32	dfs_clk_id;
+	u32	client_id;
+	u32	boost_ms; /* duration */
+	u32	boost_freq; /* in khz */
+};
+
+struct svc_pwr_max_freq {
+	u32	svc_id;
+	u32	module_id;
+};
+
+struct svc_pwr_max_freq_resp {
+	u32	svc_id;
+	u32	freq;
+};
+
+/* dfs related messages */
+enum {
+	AVP_DFS_STATE_INVALID		= 0,
+	AVP_DFS_STATE_DISABLED		= 1,
+	AVP_DFS_STATE_STOPPED		= 2,
+	AVP_DFS_STATE_CLOSED_LOOP	= 3,
+	AVP_DFS_STATE_PROFILED_LOOP	= 4,
+};
+
+struct svc_dfs_get_state_resp {
+	u32	svc_id;
+	u32	state;
+};
+
+enum {
+	AVP_DFS_CLK_CPU		= 1,
+	AVP_DFS_CLK_AVP		= 2,
+	AVP_DFS_CLK_SYSTEM	= 3,
+	AVP_DFS_CLK_AHB		= 4,
+	AVP_DFS_CLK_APB		= 5,
+	AVP_DFS_CLK_VDE		= 6,
+	/* external memory controller */
+	AVP_DFS_CLK_EMC		= 7,
+};
+
+struct avp_clk_usage {
+	u32	min;
+	u32	max;
+	u32	curr_min;
+	u32	curr_max;
+	u32	curr;
+	u32	avg; /* average activity.. whatever that means */
+};
+
+struct svc_dfs_get_clk_util {
+	u32	svc_id;
+	u32	dfs_clk_id;
+};
+
+/* all units are in kHz */
+struct svc_dfs_get_clk_util_resp {
+	u32			svc_id;
+	u32			err;
+	struct avp_clk_usage	usage;
+};
+
+/************************/
+
+enum {
+	CMD_ACK		= 0,
+	CMD_CONNECT	= 2,
+	CMD_DISCONNECT	= 3,
+	CMD_MESSAGE	= 4,
+	CMD_RESPONSE	= 5,
+};
+
+struct msg_data {
+	u32		cmd;
+	u8		data[0];
+};
+
+struct msg_ack {
+	u32		cmd;
+	u32		arg;
+};
+
+struct msg_connect {
+	u32		cmd;
+	u32		port_id;
+	/* not NUL terminated, just 0 padded */
+	char		name[XPC_PORT_NAME_LEN];
+};
+
+struct msg_connect_reply {
+	u32		cmd;
+	u32		port_id;
+};
+
+struct msg_disconnect {
+	u32		cmd;
+	u32		port_id;
+};
+
+struct msg_disconnect_reply {
+	u32		cmd;
+	u32		ack;
+};
+
+struct msg_port_data {
+	u32		cmd;
+	u32		port_id;
+	u32		msg_len;
+	u8		data[0];
+};
+
+#endif
diff --git a/drivers/media/video/tegra/avp/avp_svc.c b/drivers/media/video/tegra/avp/avp_svc.c
new file mode 100644
index 000000000000..4063ac17e570
--- /dev/null
+++ b/drivers/media/video/tegra/avp/avp_svc.c
@@ -0,0 +1,890 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Dima Zavin <dima@android.com>
+ *
+ * Copyright (C) 2010-2011 NVIDIA Corporation
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/kthread.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/tegra_rpc.h>
+#include <linux/tegra_avp.h>
+#include <linux/types.h>
+
+#include <mach/clk.h>
+#include <linux/nvmap.h>
+
+#include "../../../../video/tegra/nvmap/nvmap.h"
+
+#include "avp_msg.h"
+#include "trpc.h"
+#include "avp.h"
+
+enum {
+	AVP_DBG_TRACE_SVC		= 1U << 0,
+};
+
+static u32 debug_mask;
+module_param_named(debug_mask, debug_mask, uint, S_IWUSR | S_IRUGO);
+
+#define DBG(flag, args...) \
+	do { if (unlikely(debug_mask & (flag))) pr_info(args); } while (0)
+
+enum {
+	CLK_REQUEST_VCP		= 0,
+	CLK_REQUEST_BSEA	= 1,
+	CLK_REQUEST_VDE		= 2,
+	CLK_REQUEST_AVP		= 3,
+	NUM_CLK_REQUESTS,
+};
+
+struct avp_module {
+	const char		*name;
+	u32			clk_req;
+};
+
+static struct avp_module avp_modules[] = {
+	[AVP_MODULE_ID_AVP] = {
+		.name		= "cop",
+		.clk_req	= CLK_REQUEST_AVP,
+	},
+	[AVP_MODULE_ID_VCP] = {
+		.name		= "vcp",
+		.clk_req	= CLK_REQUEST_VCP,
+	},
+	[AVP_MODULE_ID_BSEA]	= {
+		.name		= "bsea",
+		.clk_req	= CLK_REQUEST_BSEA,
+	},
+	[AVP_MODULE_ID_VDE]	= {
+		.name		= "vde",
+		.clk_req	= CLK_REQUEST_VDE,
+	},
+};
+#define NUM_AVP_MODULES		ARRAY_SIZE(avp_modules)
+
+struct avp_clk {
+	struct clk		*clk;
+	int			refcnt;
+	struct avp_module	*mod;
+};
+
+struct avp_svc_info {
+	struct avp_clk			clks[NUM_CLK_REQUESTS];
+	/* used for dvfs */
+	struct clk			*sclk;
+	struct clk			*emcclk;
+
+	struct mutex			clk_lock;
+
+	struct trpc_endpoint		*cpu_ep;
+	struct task_struct		*svc_thread;
+
+	/* client for remote allocations, for easy tear down */
+	struct nvmap_client		*nvmap_remote;
+	struct trpc_node		*rpc_node;
+	unsigned long			max_avp_rate;
+	unsigned long			emc_rate;
+
+	/* variable to check if video is present */
+	bool				is_vde_on;
+};
+
+static void do_svc_nvmap_create(struct avp_svc_info *avp_svc,
+				struct svc_msg *_msg,
+				size_t len)
+{
+	struct svc_nvmap_create *msg = (struct svc_nvmap_create *)_msg;
+	struct svc_nvmap_create_resp resp;
+	struct nvmap_handle_ref *handle;
+	u32 handle_id = 0;
+	u32 err = 0;
+
+	handle = nvmap_create_handle(avp_svc->nvmap_remote, msg->size);
+	if (unlikely(IS_ERR(handle))) {
+		pr_err("avp_svc: error creating handle (%d bytes) for remote\n",
+		       msg->size);
+		err = AVP_ERR_ENOMEM;
+	} else
+		handle_id = (u32)nvmap_ref_to_id(handle);
+
+	resp.svc_id = SVC_NVMAP_CREATE_RESPONSE;
+	resp.err = err;
+	resp.handle_id = handle_id;
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+		      sizeof(resp), GFP_KERNEL);
+	/* TODO: do we need to put the handle if send_msg failed? */
+}
+
+static void do_svc_nvmap_alloc(struct avp_svc_info *avp_svc,
+			       struct svc_msg *_msg,
+			       size_t len)
+{
+	struct svc_nvmap_alloc *msg = (struct svc_nvmap_alloc *)_msg;
+	struct svc_common_resp resp;
+	struct nvmap_handle *handle;
+	u32 err = 0;
+	u32 heap_mask = 0;
+	int i;
+	size_t align;
+
+	handle = nvmap_get_handle_id(avp_svc->nvmap_remote, msg->handle_id);
+	if (IS_ERR(handle)) {
+		pr_err("avp_svc: unknown remote handle 0x%x\n", msg->handle_id);
+		err = AVP_ERR_EACCES;
+		goto out;
+	}
+
+	if (msg->num_heaps > 4) {
+		pr_err("avp_svc: invalid remote alloc request (%d heaps?!)\n",
+		       msg->num_heaps);
+		/* TODO: should we error out instead ? */
+		msg->num_heaps = 0;
+	}
+	if (msg->num_heaps == 0)
+		heap_mask = NVMAP_HEAP_CARVEOUT_GENERIC | NVMAP_HEAP_SYSMEM;
+
+	for (i = 0; i < msg->num_heaps; i++) {
+		switch (msg->heaps[i]) {
+		case AVP_NVMAP_HEAP_EXTERNAL:
+			heap_mask |= NVMAP_HEAP_SYSMEM;
+			break;
+		case AVP_NVMAP_HEAP_GART:
+			heap_mask |= NVMAP_HEAP_IOVMM;
+			break;
+		case AVP_NVMAP_HEAP_EXTERNAL_CARVEOUT:
+			heap_mask |= NVMAP_HEAP_CARVEOUT_GENERIC;
+			break;
+		case AVP_NVMAP_HEAP_IRAM:
+			heap_mask |= NVMAP_HEAP_CARVEOUT_IRAM;
+			break;
+		default:
+			break;
+		}
+	}
+
+	align = max_t(size_t, L1_CACHE_BYTES, msg->align);
+	err = nvmap_alloc_handle_id(avp_svc->nvmap_remote, msg->handle_id,
+				    heap_mask, align, 0);
+	nvmap_handle_put(handle);
+	if (err) {
+		pr_err("avp_svc: can't allocate for handle 0x%x (%d)\n",
+		       msg->handle_id, err);
+		err = AVP_ERR_ENOMEM;
+	}
+
+out:
+	resp.svc_id = SVC_NVMAP_ALLOC_RESPONSE;
+	resp.err = err;
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+		      sizeof(resp), GFP_KERNEL);
+}
+
+static void do_svc_nvmap_free(struct avp_svc_info *avp_svc,
+			      struct svc_msg *_msg,
+			      size_t len)
+{
+	struct svc_nvmap_free *msg = (struct svc_nvmap_free *)_msg;
+
+	nvmap_free_handle_id(avp_svc->nvmap_remote, msg->handle_id);
+}
+
+static void do_svc_nvmap_pin(struct avp_svc_info *avp_svc,
+			     struct svc_msg *_msg,
+			     size_t len)
+{
+	struct svc_nvmap_pin *msg = (struct svc_nvmap_pin *)_msg;
+	struct svc_nvmap_pin_resp resp;
+	struct nvmap_handle_ref *handle;
+	phys_addr_t addr = ~0UL;
+	unsigned long id = msg->handle_id;
+	int err;
+
+	handle = nvmap_duplicate_handle_id(avp_svc->nvmap_remote, id);
+	if (IS_ERR(handle)) {
+		pr_err("avp_svc: can't dup handle %lx\n", id);
+		goto out;
+	}
+	err = nvmap_pin_ids(avp_svc->nvmap_remote, 1, &id);
+	if (err) {
+		pr_err("avp_svc: can't pin for handle %lx (%d)\n", id, err);
+		goto out;
+	}
+	addr = nvmap_handle_address(avp_svc->nvmap_remote, id);
+
+out:
+	resp.svc_id = SVC_NVMAP_PIN_RESPONSE;
+	resp.addr = addr;
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+		      sizeof(resp), GFP_KERNEL);
+}
+
+static void do_svc_nvmap_unpin(struct avp_svc_info *avp_svc,
+			       struct svc_msg *_msg,
+			       size_t len)
+{
+	struct svc_nvmap_unpin *msg = (struct svc_nvmap_unpin *)_msg;
+	struct svc_common_resp resp;
+	unsigned long id = msg->handle_id;
+
+	nvmap_unpin_ids(avp_svc->nvmap_remote, 1, &id);
+	nvmap_free_handle_id(avp_svc->nvmap_remote, id);
+
+	resp.svc_id = SVC_NVMAP_UNPIN_RESPONSE;
+	resp.err = 0;
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+		      sizeof(resp), GFP_KERNEL);
+}
+
+static void do_svc_nvmap_from_id(struct avp_svc_info *avp_svc,
+				 struct svc_msg *_msg,
+				 size_t len)
+{
+	struct svc_nvmap_from_id *msg = (struct svc_nvmap_from_id *)_msg;
+	struct svc_common_resp resp;
+	struct nvmap_handle_ref *handle;
+	int err = 0;
+
+	handle = nvmap_duplicate_handle_id(avp_svc->nvmap_remote,
+					   msg->handle_id);
+	if (IS_ERR(handle)) {
+		pr_err("avp_svc: can't duplicate handle for id 0x%x (%d)\n",
+		       msg->handle_id, (int)PTR_ERR(handle));
+		err = AVP_ERR_ENOMEM;
+	}
+
+	resp.svc_id = SVC_NVMAP_FROM_ID_RESPONSE;
+	resp.err = err;
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+		      sizeof(resp), GFP_KERNEL);
+}
+
+static void do_svc_nvmap_get_addr(struct avp_svc_info *avp_svc,
+				  struct svc_msg *_msg,
+				  size_t len)
+{
+	struct svc_nvmap_get_addr *msg = (struct svc_nvmap_get_addr *)_msg;
+	struct svc_nvmap_get_addr_resp resp;
+
+	resp.svc_id = SVC_NVMAP_GET_ADDRESS_RESPONSE;
+	resp.addr = nvmap_handle_address(avp_svc->nvmap_remote, msg->handle_id);
+	resp.addr += msg->offs;
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+		      sizeof(resp), GFP_KERNEL);
+}
+
+static void do_svc_pwr_register(struct avp_svc_info *avp_svc,
+				struct svc_msg *_msg,
+				size_t len)
+{
+	struct svc_pwr_register *msg = (struct svc_pwr_register *)_msg;
+	struct svc_pwr_register_resp resp;
+
+	resp.svc_id = SVC_POWER_RESPONSE;
+	resp.err = 0;
+	resp.client_id = msg->client_id;
+
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+		      sizeof(resp), GFP_KERNEL);
+}
+
+static struct avp_module *find_avp_module(struct avp_svc_info *avp_svc, u32 id)
+{
+	if (id < NUM_AVP_MODULES && avp_modules[id].name)
+		return &avp_modules[id];
+	return NULL;
+}
+
+static void do_svc_module_reset(struct avp_svc_info *avp_svc,
+				struct svc_msg *_msg,
+				size_t len)
+{
+	struct svc_module_ctrl *msg = (struct svc_module_ctrl *)_msg;
+	struct svc_common_resp resp;
+	struct avp_module *mod;
+	struct avp_clk *aclk;
+
+	mod = find_avp_module(avp_svc, msg->module_id);
+	if (!mod) {
+		if (msg->module_id == AVP_MODULE_ID_AVP)
+			pr_err("avp_svc: AVP suicidal?!?!\n");
+		else
+			pr_err("avp_svc: Unknown module reset requested: %d\n",
+			       msg->module_id);
+		/* other side doesn't handle errors for reset */
+		resp.err = 0;
+		goto send_response;
+	}
+
+	aclk = &avp_svc->clks[mod->clk_req];
+	tegra_periph_reset_assert(aclk->clk);
+	udelay(10);
+	tegra_periph_reset_deassert(aclk->clk);
+	resp.err = 0;
+
+send_response:
+	resp.svc_id = SVC_MODULE_RESET_RESPONSE;
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+		      sizeof(resp), GFP_KERNEL);
+}
+
+static void do_svc_module_clock(struct avp_svc_info *avp_svc,
+				struct svc_msg *_msg,
+				size_t len)
+{
+	struct svc_module_ctrl *msg = (struct svc_module_ctrl *)_msg;
+	struct svc_common_resp resp;
+	struct avp_module *mod;
+	struct avp_clk *aclk;
+	unsigned long emc_rate = 0;
+
+	mod = find_avp_module(avp_svc, msg->module_id);
+	if (!mod) {
+		pr_err("avp_svc: unknown module clock requested: %d\n",
+		       msg->module_id);
+		resp.err = AVP_ERR_EINVAL;
+		goto send_response;
+	}
+
+	if (msg->module_id == AVP_MODULE_ID_VDE)
+		avp_svc->is_vde_on = msg->enable;
+
+	if (avp_svc->is_vde_on == true)
+		emc_rate = ULONG_MAX;
+
+	mutex_lock(&avp_svc->clk_lock);
+	aclk = &avp_svc->clks[mod->clk_req];
+	if (msg->enable) {
+		if (aclk->refcnt++ == 0) {
+			clk_set_rate(avp_svc->emcclk, emc_rate);
+			clk_enable(avp_svc->emcclk);
+			clk_enable(avp_svc->sclk);
+			clk_enable(aclk->clk);
+		}
+	} else {
+		if (unlikely(aclk->refcnt == 0)) {
+			pr_err("avp_svc: unbalanced clock disable for '%s'\n",
+			       aclk->mod->name);
+		} else if (--aclk->refcnt == 0) {
+			clk_disable(aclk->clk);
+			clk_set_rate(avp_svc->sclk, 0);
+			clk_disable(avp_svc->sclk);
+			clk_set_rate(avp_svc->emcclk, 0);
+			clk_disable(avp_svc->emcclk);
+		}
+	}
+	mutex_unlock(&avp_svc->clk_lock);
+	resp.err = 0;
+
+send_response:
+	resp.svc_id = SVC_MODULE_CLOCK_RESPONSE;
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+		      sizeof(resp), GFP_KERNEL);
+}
+
+static void do_svc_null_response(struct avp_svc_info *avp_svc,
+				 struct svc_msg *_msg,
+				 size_t len, u32 resp_svc_id)
+{
+	struct svc_common_resp resp;
+	resp.svc_id = resp_svc_id;
+	resp.err = 0;
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+		      sizeof(resp), GFP_KERNEL);
+}
+
+static void do_svc_dfs_get_state(struct avp_svc_info *avp_svc,
+				 struct svc_msg *_msg,
+				 size_t len)
+{
+	struct svc_dfs_get_state_resp resp;
+	resp.svc_id = SVC_DFS_GETSTATE_RESPONSE;
+	resp.state = AVP_DFS_STATE_STOPPED;
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+		      sizeof(resp), GFP_KERNEL);
+}
+
+static void do_svc_dfs_get_clk_util(struct avp_svc_info *avp_svc,
+				    struct svc_msg *_msg,
+				    size_t len)
+{
+	struct svc_dfs_get_clk_util_resp resp;
+
+	resp.svc_id = SVC_DFS_GET_CLK_UTIL_RESPONSE;
+	resp.err = 0;
+	memset(&resp.usage, 0, sizeof(struct avp_clk_usage));
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+		      sizeof(resp), GFP_KERNEL);
+}
+
+static void do_svc_pwr_max_freq(struct avp_svc_info *avp_svc,
+				struct svc_msg *_msg,
+				size_t len)
+{
+	struct svc_pwr_max_freq_resp resp;
+
+	resp.svc_id = SVC_POWER_MAXFREQ;
+	resp.freq = 0;
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+		      sizeof(resp), GFP_KERNEL);
+}
+
+static void do_svc_printf(struct avp_svc_info *avp_svc, struct svc_msg *_msg,
+			  size_t len)
+{
+	struct svc_printf *msg = (struct svc_printf *)_msg;
+	char tmp_str[SVC_MAX_STRING_LEN];
+
+	/* ensure we null terminate the source */
+	strlcpy(tmp_str, msg->str, SVC_MAX_STRING_LEN);
+	pr_info("[AVP]: %s", tmp_str);
+}
+
+static void do_svc_module_clock_set(struct avp_svc_info *avp_svc,
+				    struct svc_msg *_msg,
+				    size_t len)
+{
+	struct svc_clock_ctrl *msg = (struct svc_clock_ctrl *)_msg;
+	struct svc_clock_ctrl_response resp;
+	struct avp_module *mod;
+	struct avp_clk *aclk = NULL;
+	int ret = 0;
+
+	mod = find_avp_module(avp_svc, msg->module_id);
+	if (!mod) {
+		pr_err("avp_svc: unknown module clock requested: %d\n",
+		       msg->module_id);
+		resp.err = AVP_ERR_EINVAL;
+		goto send_response;
+	}
+
+	mutex_lock(&avp_svc->clk_lock);
+	if (msg->module_id == AVP_MODULE_ID_AVP) {
+		/* check if max avp clock is asked and set max emc frequency */
+		if (msg->clk_freq >= avp_svc->max_avp_rate) {
+			clk_set_rate(avp_svc->emcclk, ULONG_MAX);
+		}
+		else {
+			/* if no, set emc frequency as per platform data.
+			 * if no platform data is send, set it to maximum */
+			if (avp_svc->emc_rate)
+				clk_set_rate(avp_svc->emcclk, avp_svc->emc_rate);
+			else
+				clk_set_rate(avp_svc->emcclk, ULONG_MAX);
+		}
+		ret = clk_set_rate(avp_svc->sclk, msg->clk_freq);
+	} else {
+		aclk = &avp_svc->clks[mod->clk_req];
+		ret = clk_set_rate(aclk->clk, msg->clk_freq);
+	}
+	if (ret) {
+		pr_err("avp_svc: Failed to set module (id = %d) frequency to %d Hz\n",
+			msg->module_id, msg->clk_freq);
+		resp.err = AVP_ERR_EINVAL;
+		resp.act_freq = 0;
+		mutex_unlock(&avp_svc->clk_lock);
+		goto send_response;
+	}
+
+	if (msg->module_id == AVP_MODULE_ID_AVP)
+		resp.act_freq = clk_get_rate(avp_svc->sclk);
+	else
+		resp.act_freq = clk_get_rate(aclk->clk);
+
+	mutex_unlock(&avp_svc->clk_lock);
+	resp.err = 0;
+
+send_response:
+	resp.svc_id = SVC_MODULE_CLOCK_SET_RESPONSE;
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+		      sizeof(resp), GFP_KERNEL);
+}
+
+static void do_svc_unsupported_msg(struct avp_svc_info *avp_svc,
+			u32 resp_svc_id)
+{
+	struct svc_common_resp resp;
+
+	resp.err = AVP_ERR_ENOTSUP;
+	resp.svc_id = resp_svc_id;
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+			sizeof(resp), GFP_KERNEL);
+}
+
+static void do_svc_module_clock_get(struct avp_svc_info *avp_svc,
+				struct svc_msg *_msg,
+				size_t len)
+{
+	struct svc_clock_ctrl *msg = (struct svc_clock_ctrl *)_msg;
+	struct svc_clock_ctrl_response resp;
+	struct avp_module *mod;
+	struct avp_clk *aclk;
+
+	mod = find_avp_module(avp_svc, msg->module_id);
+	if (!mod) {
+		pr_err("avp_svc: unknown module get clock requested: %d\n",
+		       msg->module_id);
+		resp.err = AVP_ERR_EINVAL;
+		goto send_response;
+	}
+
+	mutex_lock(&avp_svc->clk_lock);
+	aclk = &avp_svc->clks[mod->clk_req];
+	resp.act_freq = clk_get_rate(aclk->clk);
+	mutex_unlock(&avp_svc->clk_lock);
+	resp.err = 0;
+
+send_response:
+	resp.svc_id = SVC_MODULE_CLOCK_GET_RESPONSE;
+	trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
+			sizeof(resp), GFP_KERNEL);
+}
+
+static int dispatch_svc_message(struct avp_svc_info *avp_svc,
+				struct svc_msg *msg,
+				size_t len)
+{
+	int ret = 0;
+
+	switch (msg->svc_id) {
+	case SVC_NVMAP_CREATE:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got nvmap_create\n", __func__);
+		do_svc_nvmap_create(avp_svc, msg, len);
+		break;
+	case SVC_NVMAP_ALLOC:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got nvmap_alloc\n", __func__);
+		do_svc_nvmap_alloc(avp_svc, msg, len);
+		break;
+	case SVC_NVMAP_FREE:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got nvmap_free\n", __func__);
+		do_svc_nvmap_free(avp_svc, msg, len);
+		break;
+	case SVC_NVMAP_PIN:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got nvmap_pin\n", __func__);
+		do_svc_nvmap_pin(avp_svc, msg, len);
+		break;
+	case SVC_NVMAP_UNPIN:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got nvmap_unpin\n", __func__);
+		do_svc_nvmap_unpin(avp_svc, msg, len);
+		break;
+	case SVC_NVMAP_FROM_ID:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got nvmap_from_id\n", __func__);
+		do_svc_nvmap_from_id(avp_svc, msg, len);
+		break;
+	case SVC_NVMAP_GET_ADDRESS:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got nvmap_get_addr\n", __func__);
+		do_svc_nvmap_get_addr(avp_svc, msg, len);
+		break;
+	case SVC_POWER_REGISTER:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got power_register\n", __func__);
+		do_svc_pwr_register(avp_svc, msg, len);
+		break;
+	case SVC_POWER_UNREGISTER:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got power_unregister\n", __func__);
+		/* nothing to do */
+		break;
+	case SVC_POWER_BUSY_HINT_MULTI:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got power_busy_hint_multi\n",
+		    __func__);
+		/* nothing to do */
+		break;
+	case SVC_POWER_BUSY_HINT:
+	case SVC_POWER_STARVATION:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got power busy/starve hint\n",
+		    __func__);
+		do_svc_null_response(avp_svc, msg, len, SVC_POWER_RESPONSE);
+		break;
+	case SVC_POWER_MAXFREQ:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got power get_max_freq\n",
+		    __func__);
+		do_svc_pwr_max_freq(avp_svc, msg, len);
+		break;
+	case SVC_DFS_GETSTATE:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got dfs_get_state\n", __func__);
+		do_svc_dfs_get_state(avp_svc, msg, len);
+		break;
+	case SVC_MODULE_RESET:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got module_reset\n", __func__);
+		do_svc_module_reset(avp_svc, msg, len);
+		break;
+	case SVC_MODULE_CLOCK:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got module_clock\n", __func__);
+		do_svc_module_clock(avp_svc, msg, len);
+		break;
+	case SVC_DFS_GET_CLK_UTIL:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got get_clk_util\n", __func__);
+		do_svc_dfs_get_clk_util(avp_svc, msg, len);
+		break;
+	case SVC_PRINTF:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got remote printf\n", __func__);
+		do_svc_printf(avp_svc, msg, len);
+		break;
+	case SVC_AVP_WDT_RESET:
+		pr_err("avp_svc: AVP has been reset by watchdog\n");
+		break;
+	case SVC_MODULE_CLOCK_SET:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got module_clock_set\n", __func__);
+		do_svc_module_clock_set(avp_svc, msg, len);
+		break;
+	case SVC_MODULE_CLOCK_GET:
+		DBG(AVP_DBG_TRACE_SVC, "%s: got module_clock_get\n", __func__);
+		do_svc_module_clock_get(avp_svc, msg, len);
+		break;
+	default:
+		pr_warning("avp_svc: Unsupported SVC call 0x%x\n", msg->svc_id);
+		do_svc_unsupported_msg(avp_svc, msg->svc_id);
+		ret = -ENOMSG;
+		break;
+	}
+
+	return ret;
+}
+
+static int avp_svc_thread(void *data)
+{
+	struct avp_svc_info *avp_svc = data;
+	u8 buf[TEGRA_RPC_MAX_MSG_LEN];
+	struct svc_msg *msg = (struct svc_msg *)buf;
+	int ret;
+	long timeout;
+
+	BUG_ON(!avp_svc->cpu_ep);
+
+	ret = trpc_wait_peer(avp_svc->cpu_ep, -1);
+	if (ret) {
+		pr_err("%s: no connection from AVP (%d)\n", __func__, ret);
+		goto err;
+	}
+
+	pr_info("%s: got remote peer\n", __func__);
+
+	while (!kthread_should_stop()) {
+		DBG(AVP_DBG_TRACE_SVC, "%s: waiting for message\n", __func__);
+		ret = trpc_recv_msg(avp_svc->rpc_node, avp_svc->cpu_ep, buf,
+				    TEGRA_RPC_MAX_MSG_LEN, -1);
+		DBG(AVP_DBG_TRACE_SVC, "%s: got message\n", __func__);
+
+		if (ret == -ECONNRESET || ret == -ENOTCONN) {
+			wait_queue_head_t wq;
+			init_waitqueue_head(&wq);
+
+			pr_info("%s: AVP seems to be down; "
+				"wait for kthread_stop\n", __func__);
+			timeout = msecs_to_jiffies(100);
+			timeout = wait_event_interruptible_timeout(wq,
+					kthread_should_stop(), timeout);
+			if (timeout == 0)
+				pr_err("%s: timed out while waiting for "
+					"kthread_stop\n", __func__);
+			continue;
+		} else if (ret <= 0) {
+			pr_err("%s: couldn't receive msg (ret=%d)\n",
+				__func__, ret);
+			continue;
+		}
+		dispatch_svc_message(avp_svc, msg, ret);
+	}
+
+err:
+	trpc_put(avp_svc->cpu_ep);
+	pr_info("%s: exiting\n", __func__);
+	return ret;
+}
+
+int avp_svc_start(struct avp_svc_info *avp_svc)
+{
+	struct trpc_endpoint *ep;
+	int ret;
+
+	avp_svc->nvmap_remote = nvmap_create_client(nvmap_dev, "avp_remote");
+	if (IS_ERR(avp_svc->nvmap_remote)) {
+		pr_err("%s: cannot create remote nvmap client\n", __func__);
+		ret = PTR_ERR(avp_svc->nvmap_remote);
+		goto err_nvmap_create_remote_client;
+	}
+
+	ep = trpc_create(avp_svc->rpc_node, "RPC_CPU_PORT", NULL, NULL);
+	if (IS_ERR(ep)) {
+		pr_err("%s: can't create RPC_CPU_PORT\n", __func__);
+		ret = PTR_ERR(ep);
+		goto err_cpu_port_create;
+	}
+
+	/* TODO: protect this */
+	avp_svc->cpu_ep = ep;
+
+	/* the service thread should get an extra reference for the port */
+	trpc_get(avp_svc->cpu_ep);
+	avp_svc->svc_thread = kthread_run(avp_svc_thread, avp_svc,
+					  "avp_svc_thread");
+	if (IS_ERR_OR_NULL(avp_svc->svc_thread)) {
+		avp_svc->svc_thread = NULL;
+		pr_err("%s: can't create svc thread\n", __func__);
+		ret = -ENOMEM;
+		goto err_kthread;
+	}
+	return 0;
+
+err_kthread:
+	trpc_close(avp_svc->cpu_ep);
+	trpc_put(avp_svc->cpu_ep);
+	avp_svc->cpu_ep = NULL;
+err_cpu_port_create:
+	nvmap_client_put(avp_svc->nvmap_remote);
+err_nvmap_create_remote_client:
+	avp_svc->nvmap_remote = NULL;
+	return ret;
+}
+
+void avp_svc_stop(struct avp_svc_info *avp_svc)
+{
+	int ret;
+	int i;
+
+	trpc_close(avp_svc->cpu_ep);
+	ret = kthread_stop(avp_svc->svc_thread);
+	if (ret == -EINTR) {
+		/* the thread never started, drop it's extra reference */
+		trpc_put(avp_svc->cpu_ep);
+	}
+	avp_svc->cpu_ep = NULL;
+
+	nvmap_client_put(avp_svc->nvmap_remote);
+	avp_svc->nvmap_remote = NULL;
+
+	mutex_lock(&avp_svc->clk_lock);
+	for (i = 0; i < NUM_CLK_REQUESTS; i++) {
+		struct avp_clk *aclk = &avp_svc->clks[i];
+		BUG_ON(aclk->refcnt < 0);
+		if (aclk->refcnt > 0) {
+			pr_info("%s: remote left clock '%s' on\n", __func__,
+				aclk->mod->name);
+			clk_disable(aclk->clk);
+			/* sclk/emcclk was enabled once for every clock */
+			clk_set_rate(avp_svc->sclk, 0);
+			clk_disable(avp_svc->sclk);
+			clk_set_rate(avp_svc->emcclk, 0);
+			clk_disable(avp_svc->emcclk);
+		}
+		aclk->refcnt = 0;
+	}
+	mutex_unlock(&avp_svc->clk_lock);
+}
+
+struct avp_svc_info *avp_svc_init(struct platform_device *pdev,
+				  struct trpc_node *rpc_node)
+{
+	struct tegra_avp_platform_data *pdata;
+	struct avp_svc_info *avp_svc;
+	int ret;
+	int i;
+	int cnt = 0;
+
+	BUG_ON(!rpc_node);
+
+	avp_svc = kzalloc(sizeof(struct avp_svc_info), GFP_KERNEL);
+	if (!avp_svc) {
+		ret = -ENOMEM;
+		goto err_alloc;
+	}
+
+	BUILD_BUG_ON(NUM_CLK_REQUESTS > BITS_PER_LONG);
+
+	pdata = pdev->dev.platform_data;
+
+	for (i = 0; i < NUM_AVP_MODULES; i++) {
+		struct avp_module *mod = &avp_modules[i];
+		struct clk *clk;
+		if (!mod->name)
+			continue;
+		BUG_ON(mod->clk_req >= NUM_CLK_REQUESTS ||
+		       cnt++ >= NUM_CLK_REQUESTS);
+
+		clk = clk_get(&pdev->dev, mod->name);
+		if (IS_ERR(clk)) {
+			ret = PTR_ERR(clk);
+			pr_err("avp_svc: Couldn't get required clocks\n");
+			goto err_get_clks;
+		}
+		avp_svc->clks[mod->clk_req].clk = clk;
+		avp_svc->clks[mod->clk_req].mod = mod;
+		avp_svc->clks[mod->clk_req].refcnt = 0;
+	}
+
+	avp_svc->sclk = clk_get(&pdev->dev, "sclk");
+	if (IS_ERR(avp_svc->sclk)) {
+		pr_err("avp_svc: Couldn't get sclk for dvfs\n");
+		ret = -ENOENT;
+		goto err_get_clks;
+	}
+	avp_svc->max_avp_rate = clk_round_rate(avp_svc->sclk, ULONG_MAX);
+	clk_set_rate(avp_svc->sclk, 0);
+
+	avp_svc->emcclk = clk_get(&pdev->dev, "emc");
+	if (IS_ERR(avp_svc->emcclk)) {
+		pr_err("avp_svc: Couldn't get emcclk for dvfs\n");
+		ret = -ENOENT;
+		goto err_get_clks;
+	}
+
+	/*
+	 * The emc is a shared clock, it will be set to the rate
+	 * requested in platform data.  Set the rate to ULONG_MAX
+	 * if platform data is NULL.
+	 */
+	avp_svc->emc_rate = 0;
+	if (pdata) {
+		clk_set_rate(avp_svc->emcclk, pdata->emc_clk_rate);
+		avp_svc->emc_rate = pdata->emc_clk_rate;
+	}
+	else {
+		clk_set_rate(avp_svc->emcclk, ULONG_MAX);
+	}
+
+	avp_svc->rpc_node = rpc_node;
+
+	mutex_init(&avp_svc->clk_lock);
+
+	return avp_svc;
+
+err_get_clks:
+	for (i = 0; i < NUM_CLK_REQUESTS; i++)
+		if (avp_svc->clks[i].clk)
+			clk_put(avp_svc->clks[i].clk);
+	if (!IS_ERR_OR_NULL(avp_svc->sclk))
+		clk_put(avp_svc->sclk);
+	if (!IS_ERR_OR_NULL(avp_svc->emcclk))
+		clk_put(avp_svc->emcclk);
+	kfree(avp_svc);
+err_alloc:
+	return ERR_PTR(ret);
+}
+
+void avp_svc_destroy(struct avp_svc_info *avp_svc)
+{
+	int i;
+
+	for (i = 0; i < NUM_CLK_REQUESTS; i++)
+		clk_put(avp_svc->clks[i].clk);
+	clk_put(avp_svc->sclk);
+	clk_put(avp_svc->emcclk);
+
+	kfree(avp_svc);
+}
diff --git a/drivers/media/video/tegra/avp/headavp.S b/drivers/media/video/tegra/avp/headavp.S
new file mode 100644
index 000000000000..c1f8e9fea1cb
--- /dev/null
+++ b/drivers/media/video/tegra/avp/headavp.S
@@ -0,0 +1,68 @@
+/*
+ * arch/arm/mach-tegra/headavp.S
+ *
+ * AVP kernel launcher stub; programs the AVP MMU and jumps to the
+ * kernel code. Must use ONLY ARMv4 instructions, and must be compiled
+ * in ARM mode.
+ *
+ * Copyright (c) 2010, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+  */
+
+#include <linux/linkage.h>
+#include <asm/assembler.h>
+#include "headavp.h"
+
+#define PTE0_COMPARE	0
+/* the default translation will translate any VA within
+ * 0x0010:0000..0x001f:ffff to the (megabyte-aligned) value written to
+ * _tegra_avp_boot_stub_data.map_phys_addr
+ */
+#define PTE0_DEFAULT	(AVP_KERNEL_VIRT_BASE | 0x3ff0)
+
+#define PTE0_TRANSLATE	4
+
+#define TRANSLATE_DATA	(1 << 11)
+#define TRANSLATE_CODE	(1 << 10)
+#define TRANSLATE_WR	(1 << 9)
+#define TRANSLATE_RD	(1 << 8)
+#define TRANSLATE_HIT	(1 << 7)
+#define TRANSLATE_EN	(1 << 2)
+
+#define TRANSLATE_OPT (TRANSLATE_DATA | TRANSLATE_CODE | TRANSLATE_WR | \
+		       TRANSLATE_RD | TRANSLATE_HIT)
+
+ENTRY(_tegra_avp_boot_stub)
+	adr	r4, _tegra_avp_boot_stub_data
+	ldmia	r4, {r0-r3}
+#ifdef CONFIG_TEGRA_AVP_KERNEL_ON_MMU
+	str	r2, [r0, #PTE0_COMPARE]
+	bic	r3, r3, #0xff0
+	bic	r3, r3, #0x00f
+	orr	r3, r3, #TRANSLATE_OPT
+	orr	r3, r3, #TRANSLATE_EN
+	str	r3, [r0, #PTE0_TRANSLATE]
+#endif
+	bx	r1
+	b	.
+ENDPROC(_tegra_avp_boot_stub)
+	.type	_tegra_avp_boot_stub_data, %object
+ENTRY(_tegra_avp_boot_stub_data)
+	.long	AVP_MMU_TLB_BASE
+	.long	0xdeadbeef
+	.long	PTE0_DEFAULT
+	.long	0xdeadd00d
+	.size	_tegra_avp_boot_stub_data, . - _tegra_avp_boot_stub_data
diff --git a/drivers/media/video/tegra/avp/headavp.h b/drivers/media/video/tegra/avp/headavp.h
new file mode 100644
index 000000000000..2bcc3297bfa4
--- /dev/null
+++ b/drivers/media/video/tegra/avp/headavp.h
@@ -0,0 +1,41 @@
+/*
+ * arch/arm/mach-tegra/headavp.h
+ *
+ * Copyright (c) 2010, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef _MACH_TEGRA_HEADAVP_H
+#define _MACH_TEGRA_HEADAVP_H
+
+#define AVP_MMU_TLB_BASE		0xF000F000
+#define AVP_KERNEL_VIRT_BASE		0x00100000
+
+#ifndef __ASSEMBLY__
+
+struct tegra_avp_boot_stub_data {
+	unsigned long	mmu_tlb_base;
+	unsigned long	jump_addr;
+	unsigned long	map_virt_addr;
+	unsigned long	map_phys_addr;
+};
+
+extern void _tegra_avp_boot_stub(void);
+extern struct tegra_avp_boot_stub_data _tegra_avp_boot_stub_data;
+
+#endif
+
+#endif
diff --git a/drivers/media/video/tegra/avp/nvavp.h b/drivers/media/video/tegra/avp/nvavp.h
new file mode 100644
index 000000000000..dbc62b485882
--- /dev/null
+++ b/drivers/media/video/tegra/avp/nvavp.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2011 Nvidia Corp
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 __MEDIA_VIDEO_TEGRA_NVAVP_H
+#define __MEDIA_VIDEO_TEGRA_NVAVP_H
+
+#include <linux/tegra_avp.h>
+
+struct tegra_avp_info;
+
+int tegra_avp_open(struct tegra_avp_info **avp);
+int tegra_avp_release(struct tegra_avp_info *avp);
+int tegra_avp_load_lib(struct tegra_avp_info *avp, struct tegra_avp_lib *lib);
+int tegra_avp_unload_lib(struct tegra_avp_info *avp, unsigned long handle);
+
+
+#include <linux/tegra_sema.h>
+
+struct tegra_sema_info;
+
+int tegra_sema_open(struct tegra_sema_info **sema);
+int tegra_sema_release(struct tegra_sema_info *sema);
+int tegra_sema_wait(struct tegra_sema_info *sema, long* timeout);
+int tegra_sema_signal(struct tegra_sema_info *sema);
+
+
+#include <linux/tegra_rpc.h>
+
+struct tegra_rpc_info;
+
+int tegra_rpc_open(struct tegra_rpc_info **rpc);
+int tegra_rpc_release(struct tegra_rpc_info *rpc);
+int tegra_rpc_port_create(struct tegra_rpc_info *rpc, char *name,
+			  struct tegra_sema_info *sema);
+int tegra_rpc_get_name(struct tegra_rpc_info *rpc, char* name);
+int tegra_rpc_port_connect(struct tegra_rpc_info *rpc, long timeout);
+int tegra_rpc_port_listen(struct tegra_rpc_info *rpc, long timeout);
+int tegra_rpc_write(struct tegra_rpc_info *rpc, u8* buf, size_t size);
+int tegra_rpc_read(struct tegra_rpc_info *rpc, u8 *buf, size_t max);
+
+
+#endif
diff --git a/drivers/media/video/tegra/avp/tegra_rpc.c b/drivers/media/video/tegra/avp/tegra_rpc.c
new file mode 100644
index 000000000000..a0fd1dc999f4
--- /dev/null
+++ b/drivers/media/video/tegra/avp/tegra_rpc.c
@@ -0,0 +1,796 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * Author:
+ *   Dima Zavin <dima@android.com>
+ *
+ * Based on original NVRM code from NVIDIA, and a partial rewrite by:
+ *   Gary King <gking@nvidia.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/kref.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/rbtree.h>
+#include <linux/sched.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+#include <linux/tegra_rpc.h>
+#include <linux/types.h>
+#include <linux/wait.h>
+
+#include "trpc.h"
+
+struct trpc_port;
+struct trpc_endpoint {
+	struct list_head	msg_list;
+	wait_queue_head_t	msg_waitq;
+
+	struct trpc_endpoint	*out;
+	struct trpc_port	*port;
+
+	struct trpc_node	*owner;
+
+	struct completion	*connect_done;
+	bool			ready;
+	struct trpc_ep_ops	*ops;
+	void			*priv;
+};
+
+struct trpc_port {
+	char			name[TEGRA_RPC_MAX_NAME_LEN];
+
+	/* protects peer and closed state */
+	spinlock_t		lock;
+	struct trpc_endpoint	peers[2];
+	bool			closed;
+
+	/* private */
+	struct kref		ref;
+	struct rb_node		rb_node;
+};
+
+enum {
+	TRPC_TRACE_MSG	= 1U << 0,
+	TRPC_TRACE_CONN	= 1U << 1,
+	TRPC_TRACE_PORT	= 1U << 2,
+};
+
+static u32 trpc_debug_mask;
+module_param_named(debug_mask, trpc_debug_mask, uint, S_IWUSR | S_IRUGO);
+
+#define DBG(flag, args...) \
+	do { if (trpc_debug_mask & (flag)) pr_info(args); } while (0)
+
+struct tegra_rpc_info {
+	struct kmem_cache		*msg_cache;
+
+	spinlock_t			ports_lock;
+	struct rb_root			ports;
+
+	struct list_head		node_list;
+	struct mutex			node_lock;
+};
+
+struct trpc_msg {
+	struct list_head		list;
+
+	size_t				len;
+	u8				payload[TEGRA_RPC_MAX_MSG_LEN];
+};
+
+static struct tegra_rpc_info *tegra_rpc;
+static struct dentry *trpc_debug_root;
+
+static struct trpc_msg *dequeue_msg_locked(struct trpc_endpoint *ep);
+
+/* a few accessors for the outside world to keep the trpc_endpoint struct
+ * definition private to this module */
+void *trpc_priv(struct trpc_endpoint *ep)
+{
+	return ep->priv;
+}
+
+struct trpc_endpoint *trpc_peer(struct trpc_endpoint *ep)
+{
+	return ep->out;
+}
+
+const char *trpc_name(struct trpc_endpoint *ep)
+{
+	return ep->port->name;
+}
+
+static inline bool is_connected(struct trpc_port *port)
+{
+	return port->peers[0].ready && port->peers[1].ready;
+}
+
+static inline bool is_closed(struct trpc_port *port)
+{
+	return port->closed;
+}
+
+static void rpc_port_free(struct tegra_rpc_info *info, struct trpc_port *port)
+{
+	struct trpc_msg *msg;
+	int i;
+
+	for (i = 0; i < 2; ++i) {
+		struct list_head *list = &port->peers[i].msg_list;
+		while (!list_empty(list)) {
+			msg = list_first_entry(list, struct trpc_msg, list);
+			list_del(&msg->list);
+			kmem_cache_free(info->msg_cache, msg);
+		}
+	}
+	kfree(port);
+}
+
+static void _rpc_port_release(struct kref *kref)
+{
+	struct tegra_rpc_info *info = tegra_rpc;
+	struct trpc_port *port = container_of(kref, struct trpc_port, ref);
+	unsigned long flags;
+
+	DBG(TRPC_TRACE_PORT, "%s: releasing port '%s' (%p)\n", __func__,
+	    port->name, port);
+	spin_lock_irqsave(&info->ports_lock, flags);
+	rb_erase(&port->rb_node, &info->ports);
+	spin_unlock_irqrestore(&info->ports_lock, flags);
+	rpc_port_free(info, port);
+}
+
+/* note that the refcount is actually on the port and not on the endpoint */
+void trpc_put(struct trpc_endpoint *ep)
+{
+	kref_put(&ep->port->ref, _rpc_port_release);
+}
+
+void trpc_get(struct trpc_endpoint *ep)
+{
+	kref_get(&ep->port->ref);
+}
+
+/* Searches the rb_tree for a port with the provided name. If one is not found,
+ * the new port in inserted. Otherwise, the existing port is returned.
+ * Must be called with the ports_lock held */
+static struct trpc_port *rpc_port_find_insert(struct tegra_rpc_info *info,
+					      struct trpc_port *port)
+{
+	struct rb_node **p;
+	struct rb_node *parent;
+	struct trpc_port *tmp;
+	int ret = 0;
+
+	p = &info->ports.rb_node;
+	parent = NULL;
+	while (*p) {
+		parent = *p;
+		tmp = rb_entry(parent, struct trpc_port, rb_node);
+
+		ret = strncmp(port->name, tmp->name, TEGRA_RPC_MAX_NAME_LEN);
+		if (ret < 0)
+			p = &(*p)->rb_left;
+		else if (ret > 0)
+			p = &(*p)->rb_right;
+		else
+			return tmp;
+	}
+	rb_link_node(&port->rb_node, parent, p);
+	rb_insert_color(&port->rb_node, &info->ports);
+	DBG(TRPC_TRACE_PORT, "%s: inserted port '%s' (%p)\n", __func__,
+	    port->name, port);
+	return port;
+}
+
+static int nodes_try_connect(struct tegra_rpc_info *info,
+			     struct trpc_node *src,
+			     struct trpc_endpoint *from)
+{
+	struct trpc_node *node;
+	int ret;
+
+	mutex_lock(&info->node_lock);
+	list_for_each_entry(node, &info->node_list, list) {
+		if (!node->try_connect)
+			continue;
+		ret = node->try_connect(node, src, from);
+		if (!ret) {
+			mutex_unlock(&info->node_lock);
+			return 0;
+		}
+	}
+	mutex_unlock(&info->node_lock);
+	return -ECONNREFUSED;
+}
+
+static struct trpc_port *rpc_port_alloc(const char *name)
+{
+	struct trpc_port *port;
+	int i;
+
+	port = kzalloc(sizeof(struct trpc_port), GFP_KERNEL);
+	if (!port) {
+		pr_err("%s: can't alloc rpc_port\n", __func__);
+		return NULL;
+	}
+	BUILD_BUG_ON(2 != ARRAY_SIZE(port->peers));
+
+	spin_lock_init(&port->lock);
+	kref_init(&port->ref);
+	strlcpy(port->name, name, TEGRA_RPC_MAX_NAME_LEN);
+	for (i = 0; i < 2; i++) {
+		struct trpc_endpoint *ep = port->peers + i;
+		INIT_LIST_HEAD(&ep->msg_list);
+		init_waitqueue_head(&ep->msg_waitq);
+		ep->port = port;
+	}
+	port->peers[0].out = &port->peers[1];
+	port->peers[1].out = &port->peers[0];
+
+	return port;
+}
+
+/* must be holding the ports lock */
+static inline void handle_port_connected(struct trpc_port *port)
+{
+	int i;
+
+	DBG(TRPC_TRACE_CONN, "tegra_rpc: port '%s' connected\n", port->name);
+
+	for (i = 0; i < 2; i++)
+		if (port->peers[i].connect_done)
+			complete(port->peers[i].connect_done);
+}
+
+static inline void _ready_ep(struct trpc_endpoint *ep,
+			     struct trpc_node *owner,
+			     struct trpc_ep_ops *ops,
+			     void *priv)
+{
+	ep->ready = true;
+	ep->owner = owner;
+	ep->ops = ops;
+	ep->priv = priv;
+}
+
+/* this keeps a reference on the port */
+static struct trpc_endpoint *_create_peer(struct tegra_rpc_info *info,
+					  struct trpc_node *owner,
+					  struct trpc_endpoint *ep,
+					  struct trpc_ep_ops *ops,
+					  void *priv)
+{
+	struct trpc_port *port = ep->port;
+	struct trpc_endpoint *peer = ep->out;
+	unsigned long flags;
+
+	spin_lock_irqsave(&port->lock, flags);
+	BUG_ON(port->closed);
+	if (peer->ready || !ep->ready) {
+		peer = NULL;
+		goto out;
+	}
+	_ready_ep(peer, owner, ops, priv);
+	if (WARN_ON(!is_connected(port)))
+		pr_warning("%s: created peer but no connection established?!\n",
+			   __func__);
+	else
+		handle_port_connected(port);
+	trpc_get(peer);
+out:
+	spin_unlock_irqrestore(&port->lock, flags);
+	return peer;
+}
+
+/* Exported code. This is out interface to the outside world */
+struct trpc_endpoint *trpc_create(struct trpc_node *owner, const char *name,
+				  struct trpc_ep_ops *ops, void *priv)
+{
+	struct tegra_rpc_info *info = tegra_rpc;
+	struct trpc_endpoint *ep;
+	struct trpc_port *new_port;
+	struct trpc_port *port;
+	unsigned long flags;
+
+	BUG_ON(!owner);
+
+	/* we always allocate a new port even if one already might exist. This
+	 * is slightly inefficient, but it allows us to do the allocation
+	 * without holding our ports_lock spinlock. */
+	new_port = rpc_port_alloc(name);
+	if (!new_port) {
+		pr_err("%s: can't allocate memory for '%s'\n", __func__, name);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	spin_lock_irqsave(&info->ports_lock, flags);
+	port = rpc_port_find_insert(info, new_port);
+	if (port != new_port) {
+		rpc_port_free(info, new_port);
+		/* There was already a port by that name in the rb_tree,
+		 * so just try to create its peer[1], i.e. peer for peer[0]
+		 */
+		ep = _create_peer(info, owner, &port->peers[0], ops, priv);
+		if (!ep) {
+			pr_err("%s: port '%s' is not in a connectable state\n",
+			       __func__, port->name);
+			ep = ERR_PTR(-EINVAL);
+		}
+		goto out;
+	}
+	/* don't need to grab the individual port lock here since we must be
+	 * holding the ports_lock to add the new element, and never dropped
+	 * it, and thus noone could have gotten a reference to this port
+	 * and thus the state couldn't have been touched */
+	ep = &port->peers[0];
+	_ready_ep(ep, owner, ops, priv);
+out:
+	spin_unlock_irqrestore(&info->ports_lock, flags);
+	return ep;
+}
+
+struct trpc_endpoint *trpc_create_peer(struct trpc_node *owner,
+				       struct trpc_endpoint *ep,
+				       struct trpc_ep_ops *ops,
+				       void *priv)
+{
+	struct tegra_rpc_info *info = tegra_rpc;
+	struct trpc_endpoint *peer;
+	unsigned long flags;
+
+	BUG_ON(!owner);
+
+	spin_lock_irqsave(&info->ports_lock, flags);
+	peer = _create_peer(info, owner, ep, ops, priv);
+	spin_unlock_irqrestore(&info->ports_lock, flags);
+	return peer;
+}
+
+/* timeout == -1, waits forever
+ * timeout == 0, return immediately
+ */
+int trpc_connect(struct trpc_endpoint *from, long timeout)
+{
+	struct tegra_rpc_info *info = tegra_rpc;
+	struct trpc_port *port = from->port;
+	struct trpc_node *src = from->owner;
+	int ret;
+	bool no_retry = !timeout;
+	unsigned long endtime = jiffies + msecs_to_jiffies(timeout);
+	unsigned long flags;
+
+	spin_lock_irqsave(&port->lock, flags);
+	/* XXX: add state for connections and ports to prevent invalid
+	 * states like multiple connections, etc. ? */
+	if (unlikely(is_closed(port))) {
+		ret = -ECONNRESET;
+		pr_err("%s: can't connect to %s, closed\n", __func__,
+		       port->name);
+		goto out;
+	} else if (is_connected(port)) {
+		ret = 0;
+		goto out;
+	}
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	do {
+		ret = nodes_try_connect(info, src, from);
+
+		spin_lock_irqsave(&port->lock, flags);
+		if (is_connected(port)) {
+			ret = 0;
+			goto out;
+		} else if (no_retry) {
+			goto out;
+		} else if (signal_pending(current)) {
+			ret = -EINTR;
+			goto out;
+		}
+		spin_unlock_irqrestore(&port->lock, flags);
+		usleep_range(5000, 20000);
+	} while (timeout < 0 || time_before(jiffies, endtime));
+
+	return -ETIMEDOUT;
+
+out:
+	spin_unlock_irqrestore(&port->lock, flags);
+	return ret;
+}
+
+/* convenience function for doing this common pattern in a single call */
+struct trpc_endpoint *trpc_create_connect(struct trpc_node *src,
+					  char *name,
+					  struct trpc_ep_ops *ops,
+					  void *priv,
+					  long timeout)
+{
+	struct trpc_endpoint *ep;
+	int ret;
+
+	ep = trpc_create(src, name, ops, priv);
+	if (IS_ERR(ep))
+		return ep;
+
+	ret = trpc_connect(ep, timeout);
+	if (ret) {
+		trpc_close(ep);
+		return ERR_PTR(ret);
+	}
+
+	return ep;
+}
+
+void trpc_close(struct trpc_endpoint *ep)
+{
+	struct trpc_port *port = ep->port;
+	struct trpc_endpoint *peer = ep->out;
+	bool need_close_op = false;
+	unsigned long flags;
+
+	spin_lock_irqsave(&port->lock, flags);
+	BUG_ON(!ep->ready);
+	ep->ready = false;
+	port->closed = true;
+	if (peer->ready) {
+		need_close_op = true;
+		/* the peer may be waiting for a message */
+		wake_up_all(&peer->msg_waitq);
+		if (peer->connect_done)
+			complete(peer->connect_done);
+	}
+	spin_unlock_irqrestore(&port->lock, flags);
+	if (need_close_op && peer->ops && peer->ops->close)
+		peer->ops->close(peer);
+	trpc_put(ep);
+}
+
+int trpc_wait_peer(struct trpc_endpoint *ep, long timeout)
+{
+	struct trpc_port *port = ep->port;
+	DECLARE_COMPLETION_ONSTACK(event);
+	int ret;
+	unsigned long flags;
+
+	if (timeout < 0)
+		timeout = MAX_SCHEDULE_TIMEOUT;
+	else if (timeout > 0)
+		timeout = msecs_to_jiffies(timeout);
+
+	spin_lock_irqsave(&port->lock, flags);
+	if (ep->connect_done) {
+		ret = -EBUSY;
+		goto done;
+	} else if (is_connected(port)) {
+		ret = 0;
+		goto done;
+	} else if (is_closed(port)) {
+		ret = -ECONNRESET;
+		goto done;
+	} else if (!timeout) {
+		ret = -EAGAIN;
+		goto done;
+	}
+	ep->connect_done = &event;
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	ret = wait_for_completion_interruptible_timeout(&event, timeout);
+
+	spin_lock_irqsave(&port->lock, flags);
+	ep->connect_done = NULL;
+
+	if (is_connected(port)) {
+		ret = 0;
+	} else {
+		if (is_closed(port))
+			ret = -ECONNRESET;
+		else if (ret == -ERESTARTSYS)
+			ret = -EINTR;
+		else if (!ret)
+			ret = -ETIMEDOUT;
+	}
+
+done:
+	spin_unlock_irqrestore(&port->lock, flags);
+	return ret;
+}
+
+static inline int _ep_id(struct trpc_endpoint *ep)
+{
+	return ep - ep->port->peers;
+}
+
+static int queue_msg(struct trpc_node *src, struct trpc_endpoint *from,
+		     void *buf, size_t len, gfp_t gfp_flags)
+{
+	struct tegra_rpc_info *info = tegra_rpc;
+	struct trpc_endpoint *peer = from->out;
+	struct trpc_port *port = from->port;
+	struct trpc_msg *msg;
+	unsigned long flags;
+	int ret;
+
+	BUG_ON(len > TEGRA_RPC_MAX_MSG_LEN);
+	/* shouldn't be enqueueing to the endpoint */
+	BUG_ON(peer->ops && peer->ops->send);
+
+	DBG(TRPC_TRACE_MSG, "%s: queueing message for %s.%d\n", __func__,
+	    port->name, _ep_id(peer));
+
+	msg = kmem_cache_alloc(info->msg_cache, gfp_flags);
+	if (!msg) {
+		pr_err("%s: can't alloc memory for msg\n", __func__);
+		return -ENOMEM;
+	}
+
+	memcpy(msg->payload, buf, len);
+	msg->len = len;
+
+	spin_lock_irqsave(&port->lock, flags);
+	if (is_closed(port)) {
+		pr_err("%s: cannot send message for closed port %s.%d\n",
+		       __func__, port->name, _ep_id(peer));
+		ret = -ECONNRESET;
+		goto err;
+	} else if (!is_connected(port)) {
+		pr_err("%s: cannot send message for unconnected port %s.%d\n",
+		       __func__, port->name, _ep_id(peer));
+		ret = -ENOTCONN;
+		goto err;
+	}
+
+	list_add_tail(&msg->list, &peer->msg_list);
+	if (peer->ops && peer->ops->notify_recv)
+		peer->ops->notify_recv(peer);
+	wake_up_all(&peer->msg_waitq);
+	spin_unlock_irqrestore(&port->lock, flags);
+	return 0;
+
+err:
+	spin_unlock_irqrestore(&port->lock, flags);
+	kmem_cache_free(info->msg_cache, msg);
+	return ret;
+}
+
+/* Returns -ENOMEM if failed to allocate memory for the message. */
+int trpc_send_msg(struct trpc_node *src, struct trpc_endpoint *from,
+		  void *buf, size_t len, gfp_t gfp_flags)
+{
+	struct trpc_endpoint *peer = from->out;
+	struct trpc_port *port = from->port;
+
+	BUG_ON(len > TEGRA_RPC_MAX_MSG_LEN);
+
+	DBG(TRPC_TRACE_MSG, "%s: sending message from %s.%d to %s.%d\n",
+	    __func__, port->name, _ep_id(from), port->name, _ep_id(peer));
+
+	if (peer->ops && peer->ops->send) {
+		might_sleep();
+		return peer->ops->send(peer, buf, len);
+	} else {
+		might_sleep_if(gfp_flags & __GFP_WAIT);
+		return queue_msg(src, from, buf, len, gfp_flags);
+	}
+}
+
+static inline struct trpc_msg *dequeue_msg_locked(struct trpc_endpoint *ep)
+{
+	struct trpc_msg *msg = NULL;
+
+	if (!list_empty(&ep->msg_list)) {
+		msg = list_first_entry(&ep->msg_list, struct trpc_msg, list);
+		list_del_init(&msg->list);
+	}
+
+	return msg;
+}
+
+static bool __should_wake(struct trpc_endpoint *ep)
+{
+	struct trpc_port *port = ep->port;
+	unsigned long flags;
+	bool ret;
+
+	spin_lock_irqsave(&port->lock, flags);
+	ret = !list_empty(&ep->msg_list) || is_closed(port);
+	spin_unlock_irqrestore(&port->lock, flags);
+	return ret;
+}
+
+int trpc_recv_msg(struct trpc_node *src, struct trpc_endpoint *ep,
+		  void *buf, size_t buf_len, long timeout)
+{
+	struct tegra_rpc_info *info = tegra_rpc;
+	struct trpc_port *port = ep->port;
+	struct trpc_msg *msg;
+	size_t len;
+	long ret;
+	unsigned long flags;
+
+	BUG_ON(buf_len > TEGRA_RPC_MAX_MSG_LEN);
+
+	spin_lock_irqsave(&port->lock, flags);
+	/* we allow closed ports to finish receiving already-queued messages */
+	msg = dequeue_msg_locked(ep);
+	if (msg) {
+		goto got_msg;
+	} else if (is_closed(port)) {
+		ret = -ECONNRESET;
+		goto out;
+	} else if (!is_connected(port)) {
+		ret = -ENOTCONN;
+		goto out;
+	}
+
+	if (timeout == 0) {
+		ret = 0;
+		goto out;
+	} else if (timeout < 0) {
+		timeout = MAX_SCHEDULE_TIMEOUT;
+	} else {
+		timeout = msecs_to_jiffies(timeout);
+	}
+	spin_unlock_irqrestore(&port->lock, flags);
+	DBG(TRPC_TRACE_MSG, "%s: waiting for message for %s.%d\n", __func__,
+	    port->name, _ep_id(ep));
+
+	ret = wait_event_interruptible_timeout(ep->msg_waitq, __should_wake(ep),
+					       timeout);
+
+	DBG(TRPC_TRACE_MSG, "%s: woke up for %s\n", __func__, port->name);
+	spin_lock_irqsave(&port->lock, flags);
+	msg = dequeue_msg_locked(ep);
+	if (!msg) {
+		if (is_closed(port))
+			ret = -ECONNRESET;
+		else if (!ret)
+			ret = -ETIMEDOUT;
+		else if (ret == -ERESTARTSYS)
+			ret = -EINTR;
+		else
+			pr_err("%s: error (%d) while receiving msg for '%s'\n",
+			       __func__, (int)ret, port->name);
+		goto out;
+	}
+
+got_msg:
+	spin_unlock_irqrestore(&port->lock, flags);
+	len = min(buf_len, msg->len);
+	memcpy(buf, msg->payload, len);
+	kmem_cache_free(info->msg_cache, msg);
+	return len;
+
+out:
+	spin_unlock_irqrestore(&port->lock, flags);
+	return ret;
+}
+
+int trpc_node_register(struct trpc_node *node)
+{
+	struct tegra_rpc_info *info = tegra_rpc;
+
+	if (!info)
+		return -ENOMEM;
+
+	pr_info("%s: Adding '%s' to node list\n", __func__, node->name);
+
+	mutex_lock(&info->node_lock);
+	if (node->type == TRPC_NODE_LOCAL)
+		list_add(&node->list, &info->node_list);
+	else
+		list_add_tail(&node->list, &info->node_list);
+	mutex_unlock(&info->node_lock);
+	return 0;
+}
+
+void trpc_node_unregister(struct trpc_node *node)
+{
+	struct tegra_rpc_info *info = tegra_rpc;
+
+	mutex_lock(&info->node_lock);
+	list_del(&node->list);
+	mutex_unlock(&info->node_lock);
+}
+
+static int trpc_debug_ports_show(struct seq_file *s, void *data)
+{
+	struct tegra_rpc_info *info = s->private;
+	struct rb_node *n;
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&info->ports_lock, flags);
+	for (n = rb_first(&info->ports); n; n = rb_next(n)) {
+		struct trpc_port *port = rb_entry(n, struct trpc_port, rb_node);
+		seq_printf(s, "port: %s\n closed:%s\n", port->name,
+			   port->closed ? "yes" : "no");
+
+		spin_lock(&port->lock);
+		for (i = 0; i < ARRAY_SIZE(port->peers); i++) {
+			struct trpc_endpoint *ep = &port->peers[i];
+			seq_printf(s, "  peer%d: %s\n    ready:%s\n", i,
+				   ep->owner ? ep->owner->name : "<none>",
+				   ep->ready ? "yes" : "no");
+			if (ep->ops && ep->ops->show)
+				ep->ops->show(s, ep);
+		}
+		spin_unlock(&port->lock);
+	}
+	spin_unlock_irqrestore(&info->ports_lock, flags);
+
+	return 0;
+}
+
+static int trpc_debug_ports_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, trpc_debug_ports_show, inode->i_private);
+}
+
+static const struct file_operations trpc_debug_ports_fops = {
+	.open = trpc_debug_ports_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static void trpc_debug_init(struct tegra_rpc_info *info)
+{
+	trpc_debug_root = debugfs_create_dir("tegra_rpc", NULL);
+	if (IS_ERR_OR_NULL(trpc_debug_root)) {
+		pr_err("%s: couldn't create debug files\n", __func__);
+		return;
+	}
+
+	debugfs_create_file("ports", 0664, trpc_debug_root, info,
+			    &trpc_debug_ports_fops);
+}
+
+static int __init tegra_rpc_init(void)
+{
+	struct tegra_rpc_info *rpc_info;
+	int ret;
+
+	rpc_info = kzalloc(sizeof(struct tegra_rpc_info), GFP_KERNEL);
+	if (!rpc_info) {
+		pr_err("%s: error allocating rpc_info\n", __func__);
+		return -ENOMEM;
+	}
+
+	rpc_info->ports = RB_ROOT;
+	spin_lock_init(&rpc_info->ports_lock);
+	INIT_LIST_HEAD(&rpc_info->node_list);
+	mutex_init(&rpc_info->node_lock);
+
+	rpc_info->msg_cache = KMEM_CACHE(trpc_msg, 0);
+	if (!rpc_info->msg_cache) {
+		pr_err("%s: unable to create message cache\n", __func__);
+		ret = -ENOMEM;
+		goto err_kmem_cache;
+	}
+
+	trpc_debug_init(rpc_info);
+	tegra_rpc = rpc_info;
+
+	return 0;
+
+err_kmem_cache:
+	kfree(rpc_info);
+	return ret;
+}
+
+subsys_initcall(tegra_rpc_init);
diff --git a/drivers/media/video/tegra/avp/trpc.h b/drivers/media/video/tegra/avp/trpc.h
new file mode 100644
index 000000000000..e7b0d2d55788
--- /dev/null
+++ b/drivers/media/video/tegra/avp/trpc.h
@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * Author:
+ *   Dima Zavin <dima@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 __ARM_MACH_TEGRA_RPC_H
+#define __ARM_MACH_TEGRA_RPC_H
+
+#include <linux/list.h>
+#include <linux/seq_file.h>
+#include <linux/tegra_rpc.h>
+
+struct trpc_endpoint;
+struct trpc_ep_ops {
+	/* send is allowed to sleep */
+	int	(*send)(struct trpc_endpoint *ep, void *buf, size_t len);
+	/* notify_recv is NOT allowed to sleep */
+	void	(*notify_recv)(struct trpc_endpoint *ep);
+	/* close is allowed to sleep */
+	void	(*close)(struct trpc_endpoint *ep);
+	/* not allowed to sleep, not allowed to call back into trpc */
+	void	(*show)(struct seq_file *s, struct trpc_endpoint *ep);
+};
+
+enum {
+	TRPC_NODE_LOCAL,
+	TRPC_NODE_REMOTE,
+};
+
+struct trpc_node {
+	struct list_head	list;
+	const char		*name;
+	int			type;
+	void			*priv;
+
+	int			(*try_connect)(struct trpc_node *node,
+					       struct trpc_node *src,
+					       struct trpc_endpoint *from);
+};
+
+struct trpc_endpoint *trpc_peer(struct trpc_endpoint *ep);
+void *trpc_priv(struct trpc_endpoint *ep);
+const char *trpc_name(struct trpc_endpoint *ep);
+
+void trpc_put(struct trpc_endpoint *ep);
+void trpc_get(struct trpc_endpoint *ep);
+
+int trpc_send_msg(struct trpc_node *src, struct trpc_endpoint *ep, void *buf,
+		  size_t len, gfp_t gfp_flags);
+int trpc_recv_msg(struct trpc_node *src, struct trpc_endpoint *ep,
+		  void *buf, size_t len, long timeout);
+struct trpc_endpoint *trpc_create(struct trpc_node *owner, const char *name,
+				  struct trpc_ep_ops *ops, void *priv);
+struct trpc_endpoint *trpc_create_connect(struct trpc_node *src, char *name,
+					  struct trpc_ep_ops *ops, void *priv,
+					  long timeout);
+int trpc_connect(struct trpc_endpoint *from, long timeout);
+struct trpc_endpoint *trpc_create_peer(struct trpc_node *owner,
+				       struct trpc_endpoint *ep,
+				       struct trpc_ep_ops *ops,
+				       void *priv);
+void trpc_close(struct trpc_endpoint *ep);
+int trpc_wait_peer(struct trpc_endpoint *ep, long timeout);
+
+int trpc_node_register(struct trpc_node *node);
+void trpc_node_unregister(struct trpc_node *node);
+
+#endif
diff --git a/drivers/media/video/tegra/avp/trpc_local.c b/drivers/media/video/tegra/avp/trpc_local.c
new file mode 100644
index 000000000000..77692e094385
--- /dev/null
+++ b/drivers/media/video/tegra/avp/trpc_local.c
@@ -0,0 +1,419 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * Author:
+ *   Dima Zavin <dima@android.com>
+ *
+ * Based on original NVRM code from NVIDIA, and a partial rewrite by
+ *   Gary King <gking@nvidia.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/err.h>
+#include <linux/file.h>
+#include <linux/fs.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/tegra_rpc.h>
+#include <linux/types.h>
+#include <linux/uaccess.h>
+#include <linux/wait.h>
+
+#include "trpc.h"
+#include "trpc_sema.h"
+#include "nvavp.h"
+
+struct tegra_rpc_info {
+	struct trpc_endpoint	*rpc_ep;
+	struct tegra_sema_info  *sema;
+};
+
+/* ports names reserved for system functions, i.e. communicating with the
+ * AVP */
+static const char reserved_ports[][TEGRA_RPC_MAX_NAME_LEN] = {
+	"RPC_AVP_PORT",
+	"RPC_CPU_PORT",
+};
+static int num_reserved_ports = ARRAY_SIZE(reserved_ports);
+
+static void rpc_notify_recv(struct trpc_endpoint *ep);
+
+/* TODO: do we need to do anything when port is closed from the other side? */
+static struct trpc_ep_ops ep_ops = {
+	.notify_recv	= rpc_notify_recv,
+};
+
+static struct trpc_node rpc_node = {
+	.name   = "local",
+	.type   = TRPC_NODE_LOCAL,
+};
+
+static void rpc_notify_recv(struct trpc_endpoint *ep)
+{
+	struct tegra_rpc_info *info = trpc_priv(ep);
+
+	if (WARN_ON(!info))
+		return;
+	if (info->sema)
+		tegra_sema_signal(info->sema);
+}
+
+int tegra_rpc_open(struct tegra_rpc_info **info)
+{
+	struct tegra_rpc_info *new_info;
+
+	new_info = kzalloc(sizeof(struct tegra_rpc_info), GFP_KERNEL);
+	if (!new_info)
+		return -ENOMEM;
+
+	*info = new_info;
+	return 0;
+}
+
+static int local_rpc_open(struct inode *inode, struct file *file)
+{
+	struct tegra_rpc_info *info;
+	int ret = 0;
+
+	ret = tegra_rpc_open(&info);
+	if (ret < 0)
+		return -ENOMEM;
+
+	nonseekable_open(inode, file);
+	file->private_data = info;
+	return 0;
+}
+
+int tegra_rpc_release(struct tegra_rpc_info *info)
+{
+	if (info->rpc_ep)
+		trpc_close(info->rpc_ep);
+	if (info->sema)
+		trpc_sema_put(info->sema);
+	kfree(info);
+	return 0;
+}
+EXPORT_SYMBOL(tegra_rpc_release);
+
+static int local_rpc_release(struct inode *inode, struct file *file)
+{
+	struct tegra_rpc_info *info = file->private_data;
+
+	tegra_rpc_release(info);
+	file->private_data = NULL;
+	return 0;
+}
+
+static char uniq_name[] = "aaaaaaaa+";
+static const int uniq_len = sizeof(uniq_name) - 1;
+static DEFINE_MUTEX(uniq_lock);
+
+static void _gen_port_name(char *new_name)
+{
+	int i;
+
+	mutex_lock(&uniq_lock);
+	for (i = 0; i < uniq_len - 1; i++) {
+		++uniq_name[i];
+		if (uniq_name[i] != 'z')
+			break;
+		uniq_name[i] = 'a';
+	}
+	strlcpy(new_name, uniq_name, TEGRA_RPC_MAX_NAME_LEN);
+	mutex_unlock(&uniq_lock);
+}
+
+static int _validate_port_name(const char *name)
+{
+	int i;
+
+	for (i = 0; i < num_reserved_ports; i++)
+		if (!strncmp(name, reserved_ports[i], TEGRA_RPC_MAX_NAME_LEN))
+			return -EINVAL;
+	return 0;
+}
+
+int tegra_rpc_port_create(struct tegra_rpc_info *info, char *name,
+			  struct tegra_sema_info *sema)
+{
+	struct trpc_endpoint *ep;
+	int ret = 0;
+
+	if (info->rpc_ep) {
+		ret = -EINVAL;
+		goto err;
+	}
+
+	name[TEGRA_RPC_MAX_NAME_LEN - 1] = '\0';
+	if (name[0]) {
+		ret = _validate_port_name(name);
+		if (ret)
+			goto err;
+	} else {
+		_gen_port_name(name);
+	}
+	ep = trpc_create(&rpc_node, name, &ep_ops, info);
+	if (IS_ERR(ep)) {
+		ret = PTR_ERR(ep);
+		goto err;
+	}
+	info->rpc_ep = ep;
+	info->sema = sema;
+	return 0;
+
+err:
+	return ret;
+}
+
+int tegra_rpc_get_name(struct tegra_rpc_info *info, char* name)
+{
+	if (!info->rpc_ep)
+		return -EINVAL;
+
+	strcpy(name, trpc_name(info->rpc_ep));
+	return 0;
+}
+
+int tegra_rpc_port_connect(struct tegra_rpc_info *info, long timeout)
+{
+	if (!info->rpc_ep)
+		return -EINVAL;
+
+	return trpc_connect(info->rpc_ep, timeout);
+
+}
+
+int tegra_rpc_port_listen(struct tegra_rpc_info *info, long timeout)
+{
+	if (!info->rpc_ep)
+		return -EINVAL;
+
+	return trpc_wait_peer(info->rpc_ep, timeout);
+}
+
+static long local_rpc_ioctl(struct file *file, unsigned int cmd,
+				unsigned long arg)
+{
+	struct tegra_rpc_info *info = file->private_data;
+	struct tegra_rpc_port_desc desc;
+	struct tegra_sema_info *sema = NULL;
+	int ret = 0;
+
+	if (_IOC_TYPE(cmd) != TEGRA_RPC_IOCTL_MAGIC ||
+		_IOC_NR(cmd) < TEGRA_RPC_IOCTL_MIN_NR ||
+		_IOC_NR(cmd) > TEGRA_RPC_IOCTL_MAX_NR) {
+		ret = -ENOTTY;
+		goto err;
+	}
+
+	switch (cmd) {
+	case TEGRA_RPC_IOCTL_PORT_CREATE:
+
+		if (_IOC_SIZE(cmd) != sizeof(struct tegra_rpc_port_desc))
+			return -EINVAL;
+		if (copy_from_user(&desc, (void __user *)arg, sizeof(desc)))
+			return -EFAULT;
+		if (desc.notify_fd != -1) {
+			sema = trpc_sema_get_from_fd(desc.notify_fd);
+			if (IS_ERR(sema)) {
+				ret = PTR_ERR(sema);
+				goto err;
+			}
+		}
+
+		ret = tegra_rpc_port_create(info, desc.name, sema);
+		if (ret < 0)
+			goto err;
+
+		break;
+	case TEGRA_RPC_IOCTL_PORT_GET_NAME:
+		if (!info->rpc_ep) {
+			ret = -EINVAL;
+			goto err;
+		}
+		if (copy_to_user((void __user *)arg,
+				 trpc_name(info->rpc_ep),
+				 TEGRA_RPC_MAX_NAME_LEN)) {
+			ret = -EFAULT;
+			goto err;
+		}
+		break;
+	case TEGRA_RPC_IOCTL_PORT_CONNECT:
+		if (!info->rpc_ep) {
+			ret = -EINVAL;
+			goto err;
+		}
+		ret = trpc_connect(info->rpc_ep, (long)arg);
+		if (ret) {
+			pr_err("%s: can't connect to '%s' (%d)\n", __func__,
+				   trpc_name(info->rpc_ep), ret);
+			goto err;
+		}
+		break;
+	case TEGRA_RPC_IOCTL_PORT_LISTEN:
+		if (!info->rpc_ep) {
+			ret = -EINVAL;
+			goto err;
+		}
+		ret = trpc_wait_peer(info->rpc_ep, (long)arg);
+		if (ret) {
+			pr_err("%s: error waiting for peer for '%s' (%d)\n",
+				   __func__, trpc_name(info->rpc_ep), ret);
+			goto err;
+		}
+		break;
+	default:
+		pr_err("%s: unknown cmd %d\n", __func__, _IOC_NR(cmd));
+		ret = -EINVAL;
+		goto err;
+	}
+
+	return 0;
+
+err:
+	if (ret && ret != -ERESTARTSYS)
+		pr_err("tegra_rpc: pid=%d ioctl=%x/%lx (%x) ret=%d\n",
+			   current->pid, cmd, arg, _IOC_NR(cmd), ret);
+	return (long)ret;
+}
+
+int tegra_rpc_write(struct tegra_rpc_info *info, u8* buf, size_t size)
+{
+	int ret;
+
+	if (!info->rpc_ep)
+		return -EINVAL;
+
+	if (TEGRA_RPC_MAX_MSG_LEN < size)
+		return -EINVAL;
+
+	ret = trpc_send_msg(&rpc_node, info->rpc_ep, buf, size,
+						GFP_KERNEL);
+	if (ret)
+		return ret;
+	return size;
+}
+
+static ssize_t local_rpc_write(struct file *file, const char __user *buf,
+				   size_t count, loff_t *ppos)
+{
+	struct tegra_rpc_info *info = file->private_data;
+	u8 data[TEGRA_RPC_MAX_MSG_LEN];
+	int ret;
+
+	if (!info)
+		return -EINVAL;
+	else if (count > TEGRA_RPC_MAX_MSG_LEN)
+		return -EINVAL;
+
+	if (copy_from_user(data, buf, count))
+		return -EFAULT;
+
+	ret = trpc_send_msg(&rpc_node, info->rpc_ep, data, count,
+				GFP_KERNEL);
+	if (ret)
+		return ret;
+	return count;
+}
+
+int tegra_rpc_read(struct tegra_rpc_info *info, u8 *buf, size_t max)
+{
+	int ret;
+
+	if (max > TEGRA_RPC_MAX_MSG_LEN)
+		return -EINVAL;
+
+	ret = trpc_recv_msg(&rpc_node, info->rpc_ep, buf,
+				TEGRA_RPC_MAX_MSG_LEN, 0);
+	if (ret == 0)
+		return 0;
+	else if (ret < 0)
+		return ret;
+	else if (ret > max)
+		return -ENOSPC;
+
+	return ret;
+}
+
+static ssize_t local_rpc_read(struct file *file, char __user *buf, size_t max,
+				  loff_t *ppos)
+{
+	struct tegra_rpc_info *info = file->private_data;
+	int ret;
+	u8 data[TEGRA_RPC_MAX_MSG_LEN];
+
+	if (max > TEGRA_RPC_MAX_MSG_LEN)
+		return -EINVAL;
+
+	ret = trpc_recv_msg(&rpc_node, info->rpc_ep, data,
+				TEGRA_RPC_MAX_MSG_LEN, 0);
+	if (ret == 0)
+		return 0;
+	else if (ret < 0)
+		return ret;
+	else if (ret > max)
+		return -ENOSPC;
+	else if (copy_to_user(buf, data, ret))
+		return -EFAULT;
+
+	return ret;
+}
+
+static const struct file_operations local_rpc_misc_fops = {
+	.owner		= THIS_MODULE,
+	.open		= local_rpc_open,
+	.release	= local_rpc_release,
+	.unlocked_ioctl	= local_rpc_ioctl,
+	.write		= local_rpc_write,
+	.read		= local_rpc_read,
+};
+
+static struct miscdevice local_rpc_misc_device = {
+	.minor  = MISC_DYNAMIC_MINOR,
+	.name   = "tegra_rpc",
+	.fops   = &local_rpc_misc_fops,
+};
+
+int __init rpc_local_init(void)
+{
+	int ret;
+
+	ret = trpc_sema_init();
+	if (ret) {
+		pr_err("%s: error in trpc_sema_init\n", __func__);
+		goto err_sema_init;
+	}
+
+	ret = misc_register(&local_rpc_misc_device);
+	if (ret) {
+		pr_err("%s: can't register misc device\n", __func__);
+		goto err_misc;
+	}
+
+	ret = trpc_node_register(&rpc_node);
+	if (ret) {
+		pr_err("%s: can't register rpc node\n", __func__);
+		goto err_node_reg;
+	}
+	return 0;
+
+err_node_reg:
+	misc_deregister(&local_rpc_misc_device);
+err_misc:
+err_sema_init:
+	return ret;
+}
+
+module_init(rpc_local_init);
diff --git a/drivers/media/video/tegra/avp/trpc_sema.c b/drivers/media/video/tegra/avp/trpc_sema.c
new file mode 100644
index 000000000000..cd717a1a0ca3
--- /dev/null
+++ b/drivers/media/video/tegra/avp/trpc_sema.c
@@ -0,0 +1,244 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * Author:
+ *   Dima Zavin <dima@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/err.h>
+#include <linux/file.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/tegra_sema.h>
+#include <linux/types.h>
+#include <linux/uaccess.h>
+#include <linux/wait.h>
+
+#include "trpc_sema.h"
+
+struct tegra_sema_info {
+	struct file		*file;
+	wait_queue_head_t	wq;
+	spinlock_t		lock;
+	int			count;
+};
+
+static int rpc_sema_minor = -1;
+
+static inline bool is_trpc_sema_file(struct file *file)
+{
+	dev_t rdev = file->f_dentry->d_inode->i_rdev;
+
+	if (MAJOR(rdev) == MISC_MAJOR && MINOR(rdev) == rpc_sema_minor)
+		return true;
+	return false;
+}
+
+struct tegra_sema_info *trpc_sema_get_from_fd(int fd)
+{
+	struct file *file;
+
+	file = fget(fd);
+	if (unlikely(file == NULL)) {
+		pr_err("%s: fd %d is invalid\n", __func__, fd);
+		return ERR_PTR(-EINVAL);
+	}
+
+	if (!is_trpc_sema_file(file)) {
+		pr_err("%s: fd (%d) is not a trpc_sema file\n", __func__, fd);
+		fput(file);
+		return ERR_PTR(-EINVAL);
+	}
+
+	return file->private_data;
+}
+
+void trpc_sema_put(struct tegra_sema_info *info)
+{
+	if (info->file)
+		fput(info->file);
+}
+
+int tegra_sema_signal(struct tegra_sema_info *info)
+{
+	unsigned long flags;
+
+	if (!info)
+		return -EINVAL;
+
+	spin_lock_irqsave(&info->lock, flags);
+	info->count++;
+	wake_up_interruptible_all(&info->wq);
+	spin_unlock_irqrestore(&info->lock, flags);
+	return 0;
+}
+
+int tegra_sema_wait(struct tegra_sema_info *info, long *timeout)
+{
+	unsigned long flags;
+	int ret = 0;
+	unsigned long endtime;
+	long timeleft = *timeout;
+
+	*timeout = 0;
+	if (timeleft < 0)
+		timeleft = MAX_SCHEDULE_TIMEOUT;
+
+	timeleft = msecs_to_jiffies(timeleft);
+	endtime = jiffies + timeleft;
+
+again:
+	if (timeleft)
+		ret = wait_event_interruptible_timeout(info->wq,
+						       info->count > 0,
+						       timeleft);
+	spin_lock_irqsave(&info->lock, flags);
+	if (info->count > 0) {
+		info->count--;
+		ret = 0;
+	} else if (ret == 0 || timeout == 0) {
+		ret = -ETIMEDOUT;
+	} else if (ret < 0) {
+		ret = -EINTR;
+		if (timeleft != MAX_SCHEDULE_TIMEOUT &&
+		    time_before(jiffies, endtime))
+			*timeout = jiffies_to_msecs(endtime - jiffies);
+		else
+			*timeout = 0;
+	} else {
+		/* we woke up but someone else got the semaphore and we have
+		 * time left, try again */
+		timeleft = ret;
+		spin_unlock_irqrestore(&info->lock, flags);
+		goto again;
+	}
+	spin_unlock_irqrestore(&info->lock, flags);
+	return ret;
+}
+
+int tegra_sema_open(struct tegra_sema_info **sema)
+{
+	struct tegra_sema_info *info;
+	info = kzalloc(sizeof(struct tegra_sema_info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	init_waitqueue_head(&info->wq);
+	spin_lock_init(&info->lock);
+	*sema = info;
+	return 0;
+}
+
+static int trpc_sema_open(struct inode *inode, struct file *file)
+{
+	struct tegra_sema_info *info;
+	int ret;
+
+	ret = tegra_sema_open(&info);
+	if (ret < 0)
+		return ret;
+
+	info->file = file;
+	nonseekable_open(inode, file);
+	file->private_data = info;
+	return 0;
+}
+
+int tegra_sema_release(struct tegra_sema_info *sema)
+{
+	kfree(sema);
+	return 0;
+}
+
+static int trpc_sema_release(struct inode *inode, struct file *file)
+{
+	struct tegra_sema_info *info = file->private_data;
+
+	file->private_data = NULL;
+	tegra_sema_release(info);
+	return 0;
+}
+
+static long trpc_sema_ioctl(struct file *file, unsigned int cmd,
+			    unsigned long arg)
+{
+	struct tegra_sema_info *info = file->private_data;
+	int ret;
+	long timeout;
+
+	if (_IOC_TYPE(cmd) != TEGRA_SEMA_IOCTL_MAGIC ||
+	    _IOC_NR(cmd) < TEGRA_SEMA_IOCTL_MIN_NR ||
+	    _IOC_NR(cmd) > TEGRA_SEMA_IOCTL_MAX_NR)
+		return -ENOTTY;
+	else if (!info)
+		return -EINVAL;
+
+	switch (cmd) {
+	case TEGRA_SEMA_IOCTL_WAIT:
+		if (copy_from_user(&timeout, (void __user *)arg, sizeof(long)))
+			return -EFAULT;
+		ret = tegra_sema_wait(info, &timeout);
+		if (ret != -EINTR)
+			break;
+		if (copy_to_user((void __user *)arg, &timeout, sizeof(long)))
+			ret = -EFAULT;
+		break;
+	case TEGRA_SEMA_IOCTL_SIGNAL:
+		ret = tegra_sema_signal(info);
+		break;
+	default:
+		pr_err("%s: Unknown tegra_sema ioctl 0x%x\n", __func__,
+		       _IOC_NR(cmd));
+		ret = -ENOTTY;
+		break;
+	}
+	return ret;
+}
+
+static const struct file_operations trpc_sema_misc_fops = {
+	.owner		= THIS_MODULE,
+	.open		= trpc_sema_open,
+	.release	= trpc_sema_release,
+	.unlocked_ioctl	= trpc_sema_ioctl,
+};
+
+static struct miscdevice trpc_sema_misc_device = {
+	.minor	= MISC_DYNAMIC_MINOR,
+	.name	= "tegra_sema",
+	.fops	= &trpc_sema_misc_fops,
+};
+
+int __init trpc_sema_init(void)
+{
+	int ret;
+
+	if (rpc_sema_minor >= 0) {
+		pr_err("%s: trpc_sema already registered\n", __func__);
+		return -EBUSY;
+	}
+
+	ret = misc_register(&trpc_sema_misc_device);
+	if (ret) {
+		pr_err("%s: can't register misc device\n", __func__);
+		return ret;
+	}
+
+	rpc_sema_minor = trpc_sema_misc_device.minor;
+	pr_info("%s: registered misc dev %d:%d\n", __func__, MISC_MAJOR,
+		rpc_sema_minor);
+
+	return 0;
+}
diff --git a/drivers/media/video/tegra/avp/trpc_sema.h b/drivers/media/video/tegra/avp/trpc_sema.h
new file mode 100644
index 000000000000..2a7c42245b7f
--- /dev/null
+++ b/drivers/media/video/tegra/avp/trpc_sema.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * Author:
+ *   Dima Zavin <dima@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 __ARM_MACH_TEGRA_RPC_SEMA_H
+#define __ARM_MACH_TEGRA_RPC_SEMA_H
+
+#include <linux/types.h>
+#include <linux/fs.h>
+
+struct tegra_sema_info;
+
+struct tegra_sema_info *trpc_sema_get_from_fd(int fd);
+void trpc_sema_put(struct tegra_sema_info *sema);
+int __init trpc_sema_init(void);
+
+#endif
diff --git a/drivers/media/video/tegra/mediaserver/Kconfig b/drivers/media/video/tegra/mediaserver/Kconfig
new file mode 100644
index 000000000000..9e60a5b49cd3
--- /dev/null
+++ b/drivers/media/video/tegra/mediaserver/Kconfig
@@ -0,0 +1,10 @@
+config TEGRA_MEDIASERVER
+bool "Tegra Media Server support"
+depends on ARCH_TEGRA && TEGRA_RPC
+default y
+help
+  Enables support for the multiple OpenMAX clients.  Exports the
+  interface on the device node /dev/tegra_mediaserver.
+
+  If unsure, say Y
+
diff --git a/drivers/media/video/tegra/mediaserver/Makefile b/drivers/media/video/tegra/mediaserver/Makefile
new file mode 100644
index 000000000000..ed24e91932bc
--- /dev/null
+++ b/drivers/media/video/tegra/mediaserver/Makefile
@@ -0,0 +1,3 @@
+GCOV_PROFILE := y
+obj-$(CONFIG_TEGRA_MEDIASERVER)	+= tegra_mediaserver.o
+
diff --git a/drivers/media/video/tegra/mediaserver/tegra_mediaserver.c b/drivers/media/video/tegra/mediaserver/tegra_mediaserver.c
new file mode 100644
index 000000000000..f6f5b1ec8b7d
--- /dev/null
+++ b/drivers/media/video/tegra/mediaserver/tegra_mediaserver.c
@@ -0,0 +1,556 @@
+/*
+ * Copyright (C) 2011 NVIDIA Corp.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/mm.h>
+
+#include <linux/tegra_mediaserver.h>
+#include "../avp/nvavp.h"
+#include "../../../../video/tegra/nvmap/nvmap.h"
+
+#define CHECK_STATUS(e, tag) \
+	do { if (e < 0) goto tag; } while (0)
+
+#define CHECK_NULL(ptr, tag) \
+	do { if (!ptr) goto tag; } while (0)
+
+#define CHECK_CONDITION(c, tag) \
+	do { if (c) goto tag; } while (0)
+
+struct tegra_mediasrv_block {
+	struct list_head entry;
+	struct tegra_mediaserver_block_info block;
+};
+
+struct tegra_mediasrv_iram {
+	struct list_head entry;
+	struct tegra_mediaserver_iram_info iram;
+};
+
+struct tegra_mediasrv_node {
+	struct tegra_mediasrv_info *mediasrv;
+	struct list_head blocks;
+	int nr_iram_shared;
+};
+
+struct tegra_mediasrv_manager {
+	struct tegra_avp_lib   lib;
+	struct tegra_rpc_info  *rpc;
+	struct tegra_sema_info *sema;
+};
+
+struct tegra_mediasrv_info {
+	int minor;
+	struct mutex lock;
+	struct nvmap_client *nvmap;
+	struct tegra_avp_info *avp;
+	struct tegra_mediasrv_manager manager;
+	int nr_nodes;
+	int nr_blocks;
+	struct tegra_mediaserver_iram_info iram; /* only one supported */
+	int nr_iram_shared;
+};
+
+static struct tegra_mediasrv_info *mediasrv_info;
+
+
+/*
+ * File entry points
+ */
+static int mediasrv_open(struct inode *inode, struct file *file)
+{
+	struct tegra_mediasrv_info *mediasrv = mediasrv_info;
+	struct tegra_mediasrv_node *node = NULL;
+	struct tegra_mediasrv_manager *manager = &mediasrv->manager;
+	struct tegra_avp_lib *lib = &manager->lib;
+	int e;
+
+	node = kzalloc(sizeof(struct tegra_mediasrv_node), GFP_KERNEL);
+	CHECK_NULL(node, node_alloc_fail);
+	INIT_LIST_HEAD(&node->blocks);
+	node->mediasrv = mediasrv;
+
+	mutex_lock(&mediasrv->lock);
+	nonseekable_open(inode, file);
+
+	if (!mediasrv->nr_nodes) {
+		e = tegra_sema_open(&manager->sema);
+		CHECK_STATUS(e, fail);
+
+		e = tegra_rpc_open(&manager->rpc);
+		CHECK_STATUS(e, fail);
+
+		e = tegra_rpc_port_create(manager->rpc, "NVMM_MANAGER_SRV",
+					  manager->sema);
+		CHECK_STATUS(e, fail);
+
+		e = tegra_avp_open(&mediasrv->avp);
+		CHECK_STATUS(e, fail);
+
+		memcpy(lib->name, "nvmm_manager.axf\0",
+			   strlen("nvmm_manager.axf") + 1);
+		lib->args = &mediasrv;
+		lib->args_len = sizeof(unsigned long);
+		e = tegra_avp_load_lib(mediasrv->avp, lib);
+		CHECK_STATUS(e, fail);
+
+		e = tegra_rpc_port_connect(manager->rpc, 50000);
+		CHECK_STATUS(e, fail);
+	}
+
+	mediasrv->nr_nodes++;
+	try_module_get(THIS_MODULE);
+
+	mutex_unlock(&mediasrv->lock);
+
+	file->private_data = node;
+
+	return 0;
+
+fail:
+	if (lib->handle) {
+		tegra_avp_unload_lib(mediasrv->avp, lib->handle);
+		lib->handle = 0;
+	}
+
+	if (mediasrv->avp) {
+		tegra_avp_release(mediasrv->avp);
+		mediasrv->avp = NULL;
+	}
+
+	if (manager->rpc) {
+		tegra_rpc_release(manager->rpc);
+		manager->rpc = NULL;
+	}
+	if (manager->sema) {
+		tegra_sema_release(manager->sema);
+		manager->sema = NULL;
+	}
+
+	kfree(node);
+
+	mutex_unlock(&mediasrv->lock);
+	return e;
+
+node_alloc_fail:
+	e = -ENOMEM;
+	return e;
+}
+
+static int mediasrv_release(struct inode *inode, struct file *file)
+{
+	struct tegra_mediasrv_info *mediasrv = mediasrv_info;
+	struct tegra_mediasrv_node *node = file->private_data;
+	struct tegra_mediasrv_block *block;
+	struct list_head *entry;
+	struct list_head *temp;
+	u32 message[2];
+	int e;
+
+	mutex_lock(&mediasrv->lock);
+
+	list_for_each_safe(entry, temp, &node->blocks) {
+		block = list_entry(entry, struct tegra_mediasrv_block, entry);
+
+		pr_info("Improperly closed block found!");
+		pr_info("  NVMM Block Handle: 0x%08x\n",
+			block->block.nvmm_block_handle);
+		pr_info("  AVP Block Handle: 0x%08x\n",
+			block->block.avp_block_handle);
+
+		message[0] = 1; /* NvmmManagerMsgType_AbnormalTerm */
+		message[1] = block->block.avp_block_handle;
+
+		e = tegra_rpc_write(mediasrv->manager.rpc, (u8 *)message,
+				    sizeof(u32) * 2);
+		pr_info("Abnormal termination message result: %d\n", e);
+
+		if (block->block.avp_block_library_handle) {
+			e = tegra_avp_unload_lib(mediasrv->avp,
+				block->block.avp_block_library_handle);
+			pr_info("Unload block (0x%08x) result: %d\n",
+				block->block.avp_block_library_handle, e);
+		}
+
+		if (block->block.service_library_handle) {
+			e = tegra_avp_unload_lib(mediasrv->avp,
+				block->block.service_library_handle);
+			pr_info("Unload service (0x%08x) result: %d\n",
+				block->block.service_library_handle, e);
+		}
+
+		mediasrv->nr_blocks--;
+		list_del(entry);
+		kfree(block);
+	}
+
+	mediasrv->nr_iram_shared -= node->nr_iram_shared;
+	if (mediasrv->iram.rm_handle && !mediasrv->nr_iram_shared) {
+		pr_info("Improperly freed shared iram found!");
+		nvmap_unpin_ids(mediasrv->nvmap, 1, &mediasrv->iram.rm_handle);
+		nvmap_free_handle_id(mediasrv->nvmap, mediasrv->iram.rm_handle);
+		mediasrv->iram.rm_handle = 0;
+		mediasrv->iram.physical_address = 0;
+	}
+
+	kfree(node);
+	mediasrv->nr_nodes--;
+	if (!mediasrv->nr_nodes) {
+		struct tegra_mediasrv_manager *manager = &mediasrv->manager;
+
+		tegra_avp_unload_lib(mediasrv->avp, manager->lib.handle);
+		manager->lib.handle = 0;
+
+		tegra_avp_release(mediasrv->avp);
+		mediasrv->avp = NULL;
+
+		tegra_rpc_release(manager->rpc);
+		manager->rpc = NULL;
+
+		tegra_sema_release(manager->sema);
+		manager->sema = NULL;
+	}
+
+	mutex_unlock(&mediasrv->lock);
+	module_put(THIS_MODULE);
+	return 0;
+}
+
+static int mediasrv_alloc(struct tegra_mediasrv_node *node,
+			  union tegra_mediaserver_alloc_info *in,
+			  union tegra_mediaserver_alloc_info *out)
+{
+	struct tegra_mediasrv_info *mediasrv = node->mediasrv;
+	int e;
+
+	switch (in->in.tegra_mediaserver_resource_type) {
+	case TEGRA_MEDIASERVER_RESOURCE_BLOCK:
+	{
+		struct tegra_mediasrv_block *block;
+
+		block = kzalloc(sizeof(struct tegra_mediasrv_block),
+				GFP_KERNEL);
+		CHECK_NULL(block, block_alloc_fail);
+
+		block->block = in->in.u.block;
+		list_add(&block->entry, &node->blocks);
+		goto block_done;
+
+block_alloc_fail:
+		e = -ENOMEM;
+	    goto fail;
+
+block_done:
+		mediasrv->nr_blocks++;
+		out->out.u.block.count = mediasrv->nr_blocks;
+	}
+	break;
+
+	case TEGRA_MEDIASERVER_RESOURCE_IRAM:
+	{
+		if (in->in.u.iram.tegra_mediaserver_iram_type ==
+		    TEGRA_MEDIASERVER_IRAM_SHARED) {
+			if (!mediasrv->nr_iram_shared) {
+				size_t align, size;
+				struct nvmap_handle_ref *r = NULL;
+				unsigned long id;
+				int physical_address;
+
+				size = PAGE_ALIGN(in->in.u.iram.size);
+				r = nvmap_create_handle(mediasrv->nvmap, size);
+				CHECK_CONDITION((r < 0),
+						iram_shared_handle_fail);
+
+				id = nvmap_ref_to_id(r);
+
+				align = max_t(size_t, in->in.u.iram.alignment,
+					      PAGE_SIZE);
+				e = nvmap_alloc_handle_id(mediasrv->nvmap, id,
+					NVMAP_HEAP_CARVEOUT_IRAM, align,
+					NVMAP_HANDLE_WRITE_COMBINE);
+				CHECK_STATUS(e, iram_shared_alloc_fail);
+
+				physical_address =
+					nvmap_pin_ids(mediasrv->nvmap, 1, &id);
+				CHECK_CONDITION((physical_address < 0),
+						iram_shared_pin_fail);
+
+				mediasrv->iram.rm_handle = id;
+				mediasrv->iram.physical_address =
+					physical_address;
+				goto iram_shared_done;
+
+iram_shared_pin_fail:
+				e = physical_address;
+iram_shared_alloc_fail:
+				nvmap_free_handle_id(mediasrv->nvmap, id);
+iram_shared_handle_fail:
+				goto fail;
+			}
+
+iram_shared_done:
+			out->out.u.iram.rm_handle = mediasrv->iram.rm_handle;
+			out->out.u.iram.physical_address =
+				mediasrv->iram.physical_address;
+			mediasrv->nr_iram_shared++;
+			node->nr_iram_shared++;
+		} else if (in->in.u.iram.tegra_mediaserver_iram_type ==
+			   TEGRA_MEDIASERVER_IRAM_SCRATCH) {
+			e = -EINVAL;
+			goto fail;
+		}
+	}
+	break;
+
+	default:
+	{
+		e = -EINVAL;
+		goto fail;
+	}
+	break;
+	}
+
+	return 0;
+
+fail:
+	return e;
+}
+
+static void mediasrv_free(struct tegra_mediasrv_node *node,
+			  union tegra_mediaserver_free_info *in)
+{
+	struct tegra_mediasrv_info *mediasrv = node->mediasrv;
+
+	switch (in->in.tegra_mediaserver_resource_type) {
+	case TEGRA_MEDIASERVER_RESOURCE_BLOCK:
+	{
+		struct tegra_mediasrv_block *block = NULL;
+		struct tegra_mediasrv_block *temp;
+		struct list_head *entry;
+
+		list_for_each(entry, &node->blocks) {
+			temp = list_entry(entry, struct tegra_mediasrv_block,
+					  entry);
+			if (temp->block.nvmm_block_handle !=
+			    in->in.u.nvmm_block_handle)
+				continue;
+
+			block = temp;
+			break;
+		}
+
+		CHECK_NULL(block, done);
+		list_del(&block->entry);
+		kfree(block);
+	}
+	break;
+
+	case TEGRA_MEDIASERVER_RESOURCE_IRAM:
+	{
+		if (in->in.u.iram_rm_handle == mediasrv->iram.rm_handle &&
+		    node->nr_iram_shared) {
+			node->nr_iram_shared--;
+			mediasrv->nr_iram_shared--;
+
+			if (!mediasrv->nr_iram_shared) {
+				nvmap_unpin_ids(mediasrv->nvmap, 1,
+						&mediasrv->iram.rm_handle);
+				nvmap_free_handle_id(mediasrv->nvmap,
+						     mediasrv->iram.rm_handle);
+				mediasrv->iram.rm_handle = 0;
+				mediasrv->iram.physical_address = 0;
+			}
+		}
+
+		else
+			goto done;
+	}
+	break;
+	}
+
+done:
+	return;
+}
+
+static int mediasrv_update_block_info(
+	struct tegra_mediasrv_node *node,
+	union tegra_mediaserver_update_block_info *in
+)
+{
+	struct tegra_mediasrv_block *entry = NULL;
+	struct tegra_mediasrv_block *block = NULL;
+	int e;
+
+	list_for_each_entry(entry, &node->blocks, entry) {
+		if (entry->block.nvmm_block_handle != in->in.nvmm_block_handle)
+			continue;
+
+		block = entry;
+		break;
+	}
+
+	CHECK_NULL(block, fail);
+
+	block->block = in->in;
+	return 0;
+
+fail:
+	e = -EINVAL;
+	return e;
+}
+
+static long mediasrv_unlocked_ioctl(struct file *file, unsigned int cmd,
+				    unsigned long arg)
+{
+	struct tegra_mediasrv_info *mediasrv = mediasrv_info;
+	struct tegra_mediasrv_node *node = file->private_data;
+	int e = -ENODEV;
+
+	mutex_lock(&mediasrv->lock);
+
+	switch (cmd) {
+	case TEGRA_MEDIASERVER_IOCTL_ALLOC:
+	{
+		union tegra_mediaserver_alloc_info in, out;
+		e = copy_from_user(&in, (void __user *)arg, sizeof(in));
+		CHECK_CONDITION(e, copy_fail);
+		e = mediasrv_alloc(node, &in, &out);
+		CHECK_STATUS(e, fail);
+		e = copy_to_user((void __user *)arg, &out, sizeof(out));
+		CHECK_CONDITION(e, copy_fail);
+	}
+	break;
+
+	case TEGRA_MEDIASERVER_IOCTL_FREE:
+	{
+		union tegra_mediaserver_free_info in;
+		e = copy_from_user(&in, (void __user *)arg, sizeof(in));
+		CHECK_CONDITION(e, copy_fail);
+		mediasrv_free(node, &in);
+	}
+	break;
+
+	case TEGRA_MEDIASERVER_IOCTL_UPDATE_BLOCK_INFO:
+	{
+		union tegra_mediaserver_update_block_info in;
+		e = copy_from_user(&in, (void __user *)arg, sizeof(in));
+		CHECK_CONDITION(e, copy_fail);
+		e = mediasrv_update_block_info(node, &in);
+		CHECK_CONDITION(e, fail);
+	}
+	break;
+
+	default:
+	{
+		e = -ENODEV;
+		goto fail;
+	}
+	break;
+	}
+
+	mutex_unlock(&mediasrv->lock);
+	return 0;
+
+copy_fail:
+	e = -EFAULT;
+fail:
+	mutex_unlock(&mediasrv->lock);
+	return e;
+}
+
+/*
+ * Kernel structures and entry points
+ */
+static const struct file_operations mediaserver_fops = {
+	.owner			= THIS_MODULE,
+	.open			= mediasrv_open,
+	.release		= mediasrv_release,
+	.unlocked_ioctl = mediasrv_unlocked_ioctl,
+};
+
+static struct miscdevice mediaserver_misc_device = {
+	.minor	= MISC_DYNAMIC_MINOR,
+	.name	= "tegra_mediaserver",
+	.fops	= &mediaserver_fops,
+};
+
+static int __init tegra_mediaserver_init(void)
+{
+	struct tegra_mediasrv_info *mediasrv;
+	int e = 0;
+
+	CHECK_NULL(!mediasrv_info, busy);
+
+	mediasrv = kzalloc(sizeof(struct tegra_mediasrv_info), GFP_KERNEL);
+	CHECK_NULL(mediasrv, alloc_fail);
+
+	mediasrv->nvmap = nvmap_create_client(nvmap_dev, "tegra_mediaserver");
+	CHECK_NULL(mediasrv, nvmap_create_fail);
+
+	e = misc_register(&mediaserver_misc_device);
+	CHECK_STATUS(e, register_fail);
+
+	mediasrv->nr_nodes = 0;
+	mutex_init(&mediasrv->lock);
+
+	mediasrv_info = mediasrv;
+	goto done;
+
+nvmap_create_fail:
+	e = -ENOMEM;
+	kfree(mediasrv);
+	goto done;
+
+register_fail:
+	nvmap_client_put(mediasrv->nvmap);
+	kfree(mediasrv);
+	goto done;
+
+alloc_fail:
+	e = -ENOMEM;
+	goto done;
+
+busy:
+	e = -EBUSY;
+	goto done;
+
+done:
+	return e;
+}
+
+void __exit tegra_mediaserver_cleanup(void)
+{
+	struct tegra_mediasrv_info *mediasrv = mediasrv_info;
+	int e;
+
+	e = misc_deregister(&mediaserver_misc_device);
+	CHECK_STATUS(e, fail);
+
+	nvmap_client_put(mediasrv->nvmap);
+	kfree(mediasrv);
+	mediasrv_info = NULL;
+
+fail:
+	return;
+}
+
+module_init(tegra_mediaserver_init);
+module_exit(tegra_mediaserver_cleanup);
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/media/video/tegra/nvavp/Kconfig b/drivers/media/video/tegra/nvavp/Kconfig
new file mode 100644
index 000000000000..294253a0de49
--- /dev/null
+++ b/drivers/media/video/tegra/nvavp/Kconfig
@@ -0,0 +1,21 @@
+config TEGRA_NVAVP
+	bool "Enable support for Tegra NVAVP driver"
+	depends on ARCH_TEGRA && TEGRA_GRHOST
+	default n
+	help
+	  Enables support for the push-buffer mechanism based driver for the Tegra
+	  multimedia framework. Exports the Tegra nvavp interface on device node
+	  /dev/tegra_avpchannel.
+
+	  If unsure, say N
+
+config TEGRA_NVAVP_AUDIO
+	bool "Enable Audio Channel support for Tegra NVAVP driver"
+	depends on TEGRA_NVAVP
+	default n
+	help
+	  Enables support for the push-buffer mechanism based driver for the Tegra
+	  audio multimedia framework. Exports the Tegra nvavp interface on device node
+	  /dev/tegra_audio_avpchannel.
+
+	  If unsure, say N
diff --git a/drivers/media/video/tegra/nvavp/Makefile b/drivers/media/video/tegra/nvavp/Makefile
new file mode 100644
index 000000000000..af2659e84664
--- /dev/null
+++ b/drivers/media/video/tegra/nvavp/Makefile
@@ -0,0 +1,5 @@
+GCOV_PROFILE := y
+EXTRA_CFLAGS += -Idrivers/video/tegra/host
+
+obj-$(CONFIG_TEGRA_NVAVP)		+= nvavp_dev.o
+obj-$(CONFIG_TEGRA_AVP_KERNEL_ON_MMU)	+= ../avp/headavp.o
diff --git a/drivers/media/video/tegra/nvavp/nvavp_dev.c b/drivers/media/video/tegra/nvavp/nvavp_dev.c
new file mode 100644
index 000000000000..d5edcbe685bf
--- /dev/null
+++ b/drivers/media/video/tegra/nvavp/nvavp_dev.c
@@ -0,0 +1,1840 @@
+/*
+ * drivers/media/video/tegra/nvavp/nvavp_dev.c
+ *
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+#include <linux/uaccess.h>
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/firmware.h>
+#include <linux/fs.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/ioctl.h>
+#include <linux/irq.h>
+#include <linux/kref.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/mutex.h>
+#include <linux/nvhost.h>
+#include <linux/platform_device.h>
+#include <linux/rbtree.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/tegra_nvavp.h>
+#include <linux/types.h>
+#include <linux/vmalloc.h>
+#include <linux/workqueue.h>
+
+#include <mach/clk.h>
+#include <mach/hardware.h>
+#include <mach/io.h>
+#include <mach/iomap.h>
+#include <mach/legacy_irq.h>
+#include <linux/nvmap.h>
+
+#if defined(CONFIG_TEGRA_AVP_KERNEL_ON_MMU)
+#include "../avp/headavp.h"
+#endif
+#include "nvavp_os.h"
+
+#define TEGRA_NVAVP_NAME			"nvavp"
+
+#define NVAVP_PUSHBUFFER_SIZE			4096
+
+#define NVAVP_PUSHBUFFER_MIN_UPDATE_SPACE	(sizeof(u32) * 3)
+
+#define TEGRA_NVAVP_RESET_VECTOR_ADDR	\
+		(IO_ADDRESS(TEGRA_EXCEPTION_VECTORS_BASE) + 0x200)
+
+#define FLOW_CTRL_HALT_COP_EVENTS	IO_ADDRESS(TEGRA_FLOW_CTRL_BASE + 0x4)
+#define FLOW_MODE_STOP			(0x2 << 29)
+#define FLOW_MODE_NONE			0x0
+
+#define NVAVP_OS_INBOX			IO_ADDRESS(TEGRA_RES_SEMA_BASE + 0x10)
+#define NVAVP_OS_OUTBOX			IO_ADDRESS(TEGRA_RES_SEMA_BASE + 0x20)
+
+#define NVAVP_INBOX_VALID		(1 << 29)
+
+/* AVP behavior params */
+#define NVAVP_OS_IDLE_TIMEOUT		100 /* milli-seconds */
+
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+/* Two control channels: Audio and Video channels */
+#define NVAVP_NUM_CHANNELS		2
+
+#define NVAVP_AUDIO_CHANNEL		1
+
+#define IS_AUDIO_CHANNEL_ID(channel_id)	(channel_id == NVAVP_AUDIO_CHANNEL ? 1: 0)
+#else
+#define NVAVP_NUM_CHANNELS		1
+#endif
+
+/* Channel ID 0 represents the Video channel control area */
+#define NVAVP_VIDEO_CHANNEL		0
+/* Channel ID 1 represents the Audio channel control area */
+
+#define IS_VIDEO_CHANNEL_ID(channel_id)	(channel_id == NVAVP_VIDEO_CHANNEL ? 1: 0)
+
+
+struct nvavp_channel {
+	struct mutex			pushbuffer_lock;
+	struct nvmap_handle_ref		*pushbuf_handle;
+	unsigned long			pushbuf_phys;
+	u8				*pushbuf_data;
+	u32				pushbuf_index;
+	u32				pushbuf_fence;
+	struct nv_e276_control		*os_control;
+};
+
+struct nvavp_info {
+	u32				clk_enabled;
+	struct clk			*bsev_clk;
+	struct clk			*vde_clk;
+	struct clk			*cop_clk;
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+	struct clk			*bsea_clk;
+	struct clk			*vcp_clk;
+#endif
+
+	/* used for dvfs */
+	struct clk			*sclk;
+	struct clk			*emc_clk;
+	unsigned long			sclk_rate;
+	unsigned long			emc_clk_rate;
+
+	int				mbox_from_avp_pend_irq;
+
+	struct mutex			open_lock;
+	int				refcount;
+	int				video_initialized;
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+	int				audio_initialized;
+	struct work_struct		app_notify_work;
+#endif
+	struct work_struct		clock_disable_work;
+
+	/* os information */
+	struct nvavp_os_info		os_info;
+
+	/* ucode information */
+	struct nvavp_ucode_info		ucode_info;
+
+	/* client for driver allocations, persistent */
+	struct nvmap_client		*nvmap;
+
+	bool				pending;
+
+	struct nvavp_channel		channel_info[NVAVP_NUM_CHANNELS];
+
+	u32				syncpt_id;
+	u32				syncpt_value;
+
+	struct nvhost_device		*nvhost_dev;
+	struct miscdevice		video_misc_dev;
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+	struct miscdevice		audio_misc_dev;
+#endif
+};
+
+struct nvavp_clientctx {
+	struct nvmap_client *nvmap;
+	struct nvavp_pushbuffer_submit_hdr submit_hdr;
+	struct nvavp_reloc relocs[NVAVP_MAX_RELOCATION_COUNT];
+	struct nvmap_handle_ref *gather_mem;
+	int num_relocs;
+	struct nvavp_info *nvavp;
+	u32 clk_reqs;
+	int channel_id;
+};
+
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+static int nvavp_get_audio_init_status(struct nvavp_info *nvavp)
+{
+	return nvavp->audio_initialized;
+}
+
+static void nvavp_set_audio_init_status(struct nvavp_info *nvavp, int status)
+{
+	nvavp->audio_initialized = status;
+}
+#endif
+
+static void nvavp_set_video_init_status(struct nvavp_info *nvavp, int status)
+{
+	nvavp->video_initialized = status;
+}
+
+static int nvavp_get_video_init_status(struct nvavp_info *nvavp)
+{
+	return nvavp->video_initialized;
+}
+
+static struct nvavp_channel *nvavp_get_channel_info(struct nvavp_info *nvavp, int channel_id)
+{
+	return &nvavp->channel_info[channel_id];
+}
+
+static void nvavp_set_channel_control_area(struct nvavp_info *nvavp, int channel_id)
+{
+	struct nv_e276_control *control;
+	struct nvavp_os_info *os = &nvavp->os_info;
+	u32 temp;
+	void *ptr;
+	struct nvavp_channel *channel_info;
+
+	ptr = os->data + os->control_offset + (sizeof(struct nv_e276_control) * channel_id);
+
+	channel_info = nvavp_get_channel_info(nvavp, channel_id);
+	channel_info->os_control = (struct nv_e276_control *)ptr;
+
+	control = channel_info->os_control;
+
+	/* init get and put pointers */
+	writel(0x0, &control->put);
+	writel(0x0, &control->get);
+
+	pr_debug("nvavp_set_channel_control_area for channel_id (%d):\
+		control->put (0x%08x) control->get (0x%08x)\n",
+		channel_id, (u32) &control->put, (u32) &control->get);
+
+	/* enable avp VDE clock control and disable iram clock gating */
+	writel(0x0, &control->idle_clk_enable);
+	writel(0x0, &control->iram_clk_gating);
+
+	/* enable avp idle timeout interrupt */
+	writel(0x1, &control->idle_notify_enable);
+	writel(NVAVP_OS_IDLE_TIMEOUT, &control->idle_notify_delay);
+
+	/* init dma start and end pointers */
+	writel(channel_info->pushbuf_phys, &control->dma_start);
+	writel((channel_info->pushbuf_phys + NVAVP_PUSHBUFFER_SIZE),
+						&control->dma_end);
+
+	writel(0x00, &channel_info->pushbuf_index);
+	temp = NVAVP_PUSHBUFFER_SIZE - NVAVP_PUSHBUFFER_MIN_UPDATE_SPACE;
+	writel(temp, &channel_info->pushbuf_fence);
+}
+
+static struct clk *nvavp_clk_get(struct nvavp_info *nvavp, int id)
+{
+	if (!nvavp)
+		return NULL;
+
+	if (id == NVAVP_MODULE_ID_AVP)
+		return nvavp->sclk;
+	if (id == NVAVP_MODULE_ID_VDE)
+		return nvavp->vde_clk;
+	if (id == NVAVP_MODULE_ID_EMC)
+		return nvavp->emc_clk;
+
+	return NULL;
+}
+
+static void nvavp_clks_enable(struct nvavp_info *nvavp)
+{
+	if (nvavp->clk_enabled++ == 0) {
+		nvhost_module_busy_ext(nvhost_get_parent(nvavp->nvhost_dev));
+		clk_enable(nvavp->bsev_clk);
+		clk_enable(nvavp->vde_clk);
+		clk_set_rate(nvavp->emc_clk, nvavp->emc_clk_rate);
+		clk_set_rate(nvavp->sclk, nvavp->sclk_rate);
+		dev_dbg(&nvavp->nvhost_dev->dev, "%s: setting sclk to %lu\n",
+				__func__, nvavp->sclk_rate);
+		dev_dbg(&nvavp->nvhost_dev->dev, "%s: setting emc_clk to %lu\n",
+				__func__, nvavp->emc_clk_rate);
+	}
+}
+
+static void nvavp_clks_disable(struct nvavp_info *nvavp)
+{
+	if (--nvavp->clk_enabled == 0) {
+		clk_disable(nvavp->bsev_clk);
+		clk_disable(nvavp->vde_clk);
+		clk_set_rate(nvavp->emc_clk, 0);
+		clk_set_rate(nvavp->sclk, 0);
+		nvhost_module_idle_ext(nvhost_get_parent(nvavp->nvhost_dev));
+		dev_dbg(&nvavp->nvhost_dev->dev, "%s: resetting emc_clk "
+				"and sclk\n", __func__);
+	}
+}
+
+static u32 nvavp_check_idle(struct nvavp_info *nvavp, int channel_id)
+{
+	struct nvavp_channel *channel_info = nvavp_get_channel_info(nvavp, channel_id);
+	struct nv_e276_control *control = channel_info->os_control;
+
+	return (control->put == control->get) ? 1 : 0;
+}
+
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+static void app_notify_handler(struct work_struct *work)
+{
+	struct nvavp_info *nvavp;
+
+	nvavp = container_of(work, struct nvavp_info,
+			    app_notify_work);
+
+	kobject_uevent(&nvavp->nvhost_dev->dev.kobj, KOBJ_CHANGE);
+}
+#endif
+
+static void clock_disable_handler(struct work_struct *work)
+{
+	struct nvavp_info *nvavp;
+	struct nvavp_channel *channel_info;
+
+	nvavp = container_of(work, struct nvavp_info,
+			    clock_disable_work);
+	channel_info = nvavp_get_channel_info(nvavp, NVAVP_VIDEO_CHANNEL);
+
+	mutex_lock(&channel_info->pushbuffer_lock);
+	mutex_lock(&nvavp->open_lock);
+	if (nvavp_check_idle(nvavp, NVAVP_VIDEO_CHANNEL) && nvavp->pending) {
+		nvavp->pending = false;
+		nvavp_clks_disable(nvavp);
+	}
+	mutex_unlock(&nvavp->open_lock);
+	mutex_unlock(&channel_info->pushbuffer_lock);
+}
+
+static int nvavp_service(struct nvavp_info *nvavp)
+{
+	struct nvavp_os_info *os = &nvavp->os_info;
+	u8 *debug_print;
+	u32 inbox;
+
+	inbox = readl(NVAVP_OS_INBOX);
+	if (!(inbox & NVAVP_INBOX_VALID))
+		inbox = 0x00000000;
+
+	if (inbox & NVE276_OS_INTERRUPT_VIDEO_IDLE)
+		schedule_work(&nvavp->clock_disable_work);
+
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+	if (inbox & NVE276_OS_INTERRUPT_AUDIO_IDLE)
+		pr_debug("nvavp_service NVE276_OS_INTERRUPT_AUDIO_IDLE\n");
+#endif
+	if (inbox & NVE276_OS_INTERRUPT_DEBUG_STRING) {
+		/* Should only occur with debug AVP OS builds */
+		debug_print = os->data;
+		debug_print += os->debug_offset;
+		dev_info(&nvavp->nvhost_dev->dev, "%s\n", debug_print);
+	}
+	if (inbox & (NVE276_OS_INTERRUPT_SEMAPHORE_AWAKEN |
+		     NVE276_OS_INTERRUPT_EXECUTE_AWAKEN)) {
+		dev_info(&nvavp->nvhost_dev->dev,
+			"AVP awaken event (0x%x)\n", inbox);
+	}
+	if (inbox & NVE276_OS_INTERRUPT_AVP_FATAL_ERROR) {
+		dev_err(&nvavp->nvhost_dev->dev,
+			"fatal AVP error (0x%08X)\n", inbox);
+	}
+	if (inbox & NVE276_OS_INTERRUPT_AVP_BREAKPOINT)
+		dev_err(&nvavp->nvhost_dev->dev, "AVP breakpoint hit\n");
+	if (inbox & NVE276_OS_INTERRUPT_TIMEOUT)
+		dev_err(&nvavp->nvhost_dev->dev, "AVP timeout\n");
+	writel(inbox & NVAVP_INBOX_VALID, NVAVP_OS_INBOX);
+
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+	if (inbox & NVE276_OS_INTERRUPT_APP_NOTIFY) {
+		pr_debug("nvavp_service NVE276_OS_INTERRUPT_APP_NOTIFY\n");
+		schedule_work(&nvavp->app_notify_work);
+	}
+#endif
+
+	return 0;
+}
+
+static irqreturn_t nvavp_mbox_pending_isr(int irq, void *data)
+{
+	struct nvavp_info *nvavp = data;
+
+	nvavp_service(nvavp);
+
+	return IRQ_HANDLED;
+}
+
+static void nvavp_halt_avp(struct nvavp_info *nvavp)
+{
+	/* ensure the AVP is halted */
+	writel(FLOW_MODE_STOP, FLOW_CTRL_HALT_COP_EVENTS);
+	tegra_periph_reset_assert(nvavp->cop_clk);
+
+	writel(0, NVAVP_OS_OUTBOX);
+	writel(0, NVAVP_OS_INBOX);
+}
+
+static int nvavp_reset_avp(struct nvavp_info *nvavp, unsigned long reset_addr)
+{
+#if defined(CONFIG_TEGRA_AVP_KERNEL_ON_MMU)
+	unsigned long stub_code_phys = virt_to_phys(_tegra_avp_boot_stub);
+	dma_addr_t stub_data_phys;
+
+	_tegra_avp_boot_stub_data.map_phys_addr = avp->kernel_phys;
+	_tegra_avp_boot_stub_data.jump_addr = reset_addr;
+	wmb();
+	stub_data_phys = dma_map_single(NULL, &_tegra_avp_boot_stub_data,
+					sizeof(_tegra_avp_boot_stub_data),
+					DMA_TO_DEVICE);
+	rmb();
+	reset_addr = (unsigned long)stub_data_phys;
+#endif
+	writel(FLOW_MODE_STOP, FLOW_CTRL_HALT_COP_EVENTS);
+
+	writel(reset_addr, TEGRA_NVAVP_RESET_VECTOR_ADDR);
+
+	clk_enable(nvavp->sclk);
+	clk_enable(nvavp->emc_clk);
+
+	/* If sclk_rate and emc_clk is not set by user space,
+	 * max clock in dvfs table will be used to get best performance.
+	 */
+	nvavp->sclk_rate = ULONG_MAX;
+	nvavp->emc_clk_rate = ULONG_MAX;
+
+	tegra_periph_reset_assert(nvavp->cop_clk);
+	udelay(2);
+	tegra_periph_reset_deassert(nvavp->cop_clk);
+
+	writel(FLOW_MODE_NONE, FLOW_CTRL_HALT_COP_EVENTS);
+
+#if defined(CONFIG_TEGRA_AVP_KERNEL_ON_MMU)
+	dma_unmap_single(NULL, stub_data_phys,
+			 sizeof(_tegra_avp_boot_stub_data),
+			 DMA_TO_DEVICE);
+#endif
+	return 0;
+}
+
+static void nvavp_halt_vde(struct nvavp_info *nvavp)
+{
+	if (nvavp->clk_enabled && !nvavp->pending)
+		BUG();
+
+	if (nvavp->pending) {
+		nvavp_clks_disable(nvavp);
+		nvavp->pending = false;
+	}
+
+	tegra_periph_reset_assert(nvavp->bsev_clk);
+	tegra_periph_reset_assert(nvavp->vde_clk);
+}
+
+static int nvavp_reset_vde(struct nvavp_info *nvavp)
+{
+	if (nvavp->clk_enabled)
+		BUG();
+
+	nvavp_clks_enable(nvavp);
+
+	tegra_periph_reset_assert(nvavp->bsev_clk);
+	udelay(2);
+	tegra_periph_reset_deassert(nvavp->bsev_clk);
+
+	tegra_periph_reset_assert(nvavp->vde_clk);
+	udelay(2);
+	tegra_periph_reset_deassert(nvavp->vde_clk);
+
+	/*
+	 * VDE clock is set to max freq by default.
+	 * VDE clock can be set to different freq if needed
+	 * through ioctl.
+	 */
+	clk_set_rate(nvavp->vde_clk, ULONG_MAX);
+
+	nvavp_clks_disable(nvavp);
+
+	return 0;
+}
+
+static int nvavp_pushbuffer_alloc(struct nvavp_info *nvavp, int channel_id)
+{
+	int ret = 0;
+
+	struct nvavp_channel *channel_info = nvavp_get_channel_info(
+							nvavp, channel_id);
+
+	channel_info->pushbuf_handle = nvmap_alloc(nvavp->nvmap,
+						NVAVP_PUSHBUFFER_SIZE,
+						SZ_1M, NVMAP_HANDLE_UNCACHEABLE,
+						0);
+	if (IS_ERR(channel_info->pushbuf_handle)) {
+		dev_err(&nvavp->nvhost_dev->dev,
+			"cannot create pushbuffer handle\n");
+		ret = PTR_ERR(channel_info->pushbuf_handle);
+		goto err_pushbuf_alloc;
+	}
+	channel_info->pushbuf_data = (u8 *)nvmap_mmap(
+						channel_info->pushbuf_handle);
+
+	if (!channel_info->pushbuf_data) {
+		dev_err(&nvavp->nvhost_dev->dev,
+			"cannot map pushbuffer handle\n");
+		ret = -ENOMEM;
+		goto err_pushbuf_mmap;
+	}
+	channel_info->pushbuf_phys = nvmap_pin(nvavp->nvmap,
+					channel_info->pushbuf_handle);
+	if (IS_ERR((void *)channel_info->pushbuf_phys)) {
+		dev_err(&nvavp->nvhost_dev->dev,
+			"cannot pin pushbuffer handle\n");
+		ret = PTR_ERR((void *)channel_info->pushbuf_phys);
+		goto err_pushbuf_pin;
+	}
+
+	memset(channel_info->pushbuf_data, 0, NVAVP_PUSHBUFFER_SIZE);
+
+	return 0;
+
+err_pushbuf_pin:
+	nvmap_munmap(channel_info->pushbuf_handle, channel_info->pushbuf_data);
+err_pushbuf_mmap:
+	nvmap_free(nvavp->nvmap, channel_info->pushbuf_handle);
+err_pushbuf_alloc:
+	return ret;
+}
+
+static void nvavp_pushbuffer_free(struct nvavp_info *nvavp)
+{
+	int channel_id;
+
+	for (channel_id = 0; channel_id < NVAVP_NUM_CHANNELS; channel_id++) {
+		if (nvavp->channel_info[channel_id].pushbuf_data) {
+			nvmap_unpin(nvavp->nvmap,
+				nvavp->channel_info[channel_id].pushbuf_handle);
+			nvmap_munmap(
+				nvavp->channel_info[channel_id].pushbuf_handle,
+				nvavp->channel_info[channel_id].pushbuf_data);
+			nvmap_free(nvavp->nvmap,
+				nvavp->channel_info[channel_id].pushbuf_handle);
+		}
+	}
+}
+
+
+static int nvavp_pushbuffer_init(struct nvavp_info *nvavp)
+{
+	int ret, channel_id;
+
+	for (channel_id = 0; channel_id < NVAVP_NUM_CHANNELS; channel_id++) {
+		ret = nvavp_pushbuffer_alloc(nvavp, channel_id);
+		if (ret) {
+			dev_err(&nvavp->nvhost_dev->dev,
+				"unable to alloc pushbuffer\n");
+			return ret;
+		}
+		nvavp_set_channel_control_area(nvavp, channel_id);
+		if (IS_VIDEO_CHANNEL_ID(channel_id)) {
+			nvavp->syncpt_id = NVSYNCPT_AVP_0;
+			nvavp->syncpt_value = nvhost_syncpt_read_ext(
+				nvavp->nvhost_dev, nvavp->syncpt_id);
+		}
+
+	}
+	return 0;
+}
+
+static void nvavp_pushbuffer_deinit(struct nvavp_info *nvavp)
+{
+	nvavp_pushbuffer_free(nvavp);
+}
+
+static int nvavp_pushbuffer_update(struct nvavp_info *nvavp, u32 phys_addr,
+			u32 gather_count, struct nvavp_syncpt *syncpt,
+			u32 ext_ucode_flag, int channel_id)
+{
+	struct nvavp_channel  *channel_info;
+	struct nv_e276_control *control;
+	u32 gather_cmd, setucode_cmd, sync = 0;
+	u32 wordcount = 0;
+	u32 index, value = -1;
+
+	channel_info = nvavp_get_channel_info(nvavp, channel_id);
+
+	control = channel_info->os_control;
+	pr_debug("nvavp_pushbuffer_update for channel_id (%d):\
+		control->put (0x%x) control->get (0x%x)\n",
+		channel_id, (u32) &control->put, (u32) &control->get);
+
+	mutex_lock(&channel_info->pushbuffer_lock);
+
+	/* check for pushbuffer wrapping */
+	if (channel_info->pushbuf_index >= channel_info->pushbuf_fence)
+		channel_info->pushbuf_index = 0;
+
+	if (!ext_ucode_flag) {
+		setucode_cmd =
+			NVE26E_CH_OPCODE_INCR(NVE276_SET_MICROCODE_A, 3);
+
+		index = wordcount + channel_info->pushbuf_index;
+		writel(setucode_cmd, (channel_info->pushbuf_data + index));
+		wordcount += sizeof(u32);
+
+		index = wordcount + channel_info->pushbuf_index;
+		writel(0, (channel_info->pushbuf_data + index));
+		wordcount += sizeof(u32);
+
+		index = wordcount + channel_info->pushbuf_index;
+		writel(nvavp->ucode_info.phys,
+			(channel_info->pushbuf_data + index));
+		wordcount += sizeof(u32);
+
+		index = wordcount + channel_info->pushbuf_index;
+		writel(nvavp->ucode_info.size,
+			(channel_info->pushbuf_data + index));
+		wordcount += sizeof(u32);
+	}
+
+	gather_cmd = NVE26E_CH_OPCODE_GATHER(0, 0, 0, gather_count);
+
+	if (syncpt) {
+		value = ++nvavp->syncpt_value;
+		/* XXX: NvSchedValueWrappingComparison */
+		sync = NVE26E_CH_OPCODE_IMM(NVE26E_HOST1X_INCR_SYNCPT,
+			(NVE26E_HOST1X_INCR_SYNCPT_COND_OP_DONE << 8) |
+			(nvavp->syncpt_id & 0xFF));
+	}
+
+	/* write commands out */
+	index = wordcount + channel_info->pushbuf_index;
+	writel(gather_cmd, (channel_info->pushbuf_data + index));
+	wordcount += sizeof(u32);
+
+	index = wordcount + channel_info->pushbuf_index;
+	writel(phys_addr, (channel_info->pushbuf_data + index));
+	wordcount += sizeof(u32);
+
+	if (syncpt) {
+		index = wordcount + channel_info->pushbuf_index;
+		writel(sync, (channel_info->pushbuf_data + index));
+		wordcount += sizeof(u32);
+	}
+
+	/* enable clocks to VDE/BSEV */
+	if (IS_VIDEO_CHANNEL_ID(channel_id)) {
+		mutex_lock(&nvavp->open_lock);
+		if (!nvavp->pending) {
+			nvavp_clks_enable(nvavp);
+			nvavp->pending = true;
+		}
+		mutex_unlock(&nvavp->open_lock);
+	}
+
+	/* update put pointer */
+	channel_info->pushbuf_index = (channel_info->pushbuf_index + wordcount)&
+					(NVAVP_PUSHBUFFER_SIZE - 1);
+
+	writel(channel_info->pushbuf_index, &control->put);
+	wmb();
+
+	/* wake up avp */
+
+	if (IS_VIDEO_CHANNEL_ID(channel_id)) {
+		pr_debug("Wake up Video Channel\n");
+		writel(0xA0000001, NVAVP_OS_OUTBOX);
+	}
+	else {
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+		if (IS_AUDIO_CHANNEL_ID(channel_id)) {
+			pr_debug("Wake up Audio Channel\n");
+			writel(0xA0000002, NVAVP_OS_OUTBOX);
+		}
+#endif
+	}
+	/* Fill out fence struct */
+	if (syncpt) {
+		syncpt->id = nvavp->syncpt_id;
+		syncpt->value = value;
+	}
+
+	mutex_unlock(&channel_info->pushbuffer_lock);
+
+	return 0;
+}
+
+static void nvavp_unload_ucode(struct nvavp_info *nvavp)
+{
+	nvmap_unpin(nvavp->nvmap, nvavp->ucode_info.handle);
+	nvmap_munmap(nvavp->ucode_info.handle, nvavp->ucode_info.data);
+	nvmap_free(nvavp->nvmap, nvavp->ucode_info.handle);
+	kfree(nvavp->ucode_info.ucode_bin);
+}
+
+static int nvavp_load_ucode(struct nvavp_info *nvavp)
+{
+	struct nvavp_ucode_info *ucode_info = &nvavp->ucode_info;
+	const struct firmware *nvavp_ucode_fw;
+	char fw_ucode_file[32];
+	void *ptr;
+	int ret = 0;
+
+	if (!ucode_info->ucode_bin) {
+		sprintf(fw_ucode_file, "nvavp_vid_ucode.bin");
+
+		ret = request_firmware(&nvavp_ucode_fw, fw_ucode_file,
+					nvavp->video_misc_dev.this_device);
+		if (ret) {
+			/* Try alternative version */
+			sprintf(fw_ucode_file, "nvavp_vid_ucode_alt.bin");
+
+			ret = request_firmware(&nvavp_ucode_fw,
+						fw_ucode_file,
+						nvavp->video_misc_dev.this_device);
+
+			if (ret) {
+				dev_err(&nvavp->nvhost_dev->dev,
+					"cannot read ucode firmware '%s'\n",
+					fw_ucode_file);
+				goto err_req_ucode;
+			}
+		}
+
+		dev_info(&nvavp->nvhost_dev->dev,
+			"read ucode firmware from '%s' (%d bytes)\n",
+			fw_ucode_file, nvavp_ucode_fw->size);
+
+		ptr = (void *)nvavp_ucode_fw->data;
+
+		if (strncmp((const char *)ptr, "NVAVPAPP", 8)) {
+			dev_info(&nvavp->nvhost_dev->dev,
+				"ucode hdr string mismatch\n");
+			ret = -EINVAL;
+			goto err_req_ucode;
+		}
+		ptr += 8;
+		ucode_info->size = nvavp_ucode_fw->size - 8;
+
+		ucode_info->ucode_bin = kzalloc(ucode_info->size,
+						GFP_KERNEL);
+		if (!ucode_info->ucode_bin) {
+			dev_err(&nvavp->nvhost_dev->dev,
+				"cannot allocate ucode bin\n");
+			ret = -ENOMEM;
+			goto err_ubin_alloc;
+		}
+
+		ucode_info->handle = nvmap_alloc(nvavp->nvmap,
+						nvavp->ucode_info.size,
+					SZ_1M, NVMAP_HANDLE_UNCACHEABLE, 0);
+		if (IS_ERR(ucode_info->handle)) {
+			dev_err(&nvavp->nvhost_dev->dev,
+				"cannot create ucode handle\n");
+			ret = PTR_ERR(ucode_info->handle);
+			goto err_ucode_alloc;
+		}
+		ucode_info->data = (u8 *)nvmap_mmap(ucode_info->handle);
+		if (!ucode_info->data) {
+			dev_err(&nvavp->nvhost_dev->dev,
+				"cannot map ucode handle\n");
+			ret = -ENOMEM;
+			goto err_ucode_mmap;
+		}
+		ucode_info->phys = nvmap_pin(nvavp->nvmap, ucode_info->handle);
+		if (IS_ERR((void *)ucode_info->phys)) {
+			dev_err(&nvavp->nvhost_dev->dev,
+				"cannot pin ucode handle\n");
+			ret = PTR_ERR((void *)ucode_info->phys);
+			goto err_ucode_pin;
+		}
+		memcpy(ucode_info->ucode_bin, ptr, ucode_info->size);
+		release_firmware(nvavp_ucode_fw);
+	}
+
+	memcpy(ucode_info->data, ucode_info->ucode_bin, ucode_info->size);
+	return 0;
+
+err_ucode_pin:
+	nvmap_munmap(ucode_info->handle, ucode_info->data);
+err_ucode_mmap:
+	nvmap_free(nvavp->nvmap, ucode_info->handle);
+err_ucode_alloc:
+	kfree(nvavp->ucode_info.ucode_bin);
+err_ubin_alloc:
+	release_firmware(nvavp_ucode_fw);
+err_req_ucode:
+	return ret;
+}
+
+static void nvavp_unload_os(struct nvavp_info *nvavp)
+{
+	nvmap_unpin(nvavp->nvmap, nvavp->os_info.handle);
+	nvmap_munmap(nvavp->os_info.handle, nvavp->os_info.data);
+#if defined(CONFIG_TEGRA_AVP_KERNEL_ON_MMU)
+	nvmap_free(nvavp->nvmap, nvavp->os_info.handle);
+#elif defined(CONFIG_TEGRA_AVP_KERNEL_ON_SMMU)
+	nvmap_free_iovm(nvavp->nvmap, nvavp->os_info.handle);
+#endif
+	kfree(nvavp->os_info.os_bin);
+}
+
+static int nvavp_load_os(struct nvavp_info *nvavp, char *fw_os_file)
+{
+	struct nvavp_os_info *os_info = &nvavp->os_info;
+	const struct firmware *nvavp_os_fw;
+	void *ptr;
+	u32 size;
+	int ret = 0;
+
+	if (!os_info->os_bin) {
+		ret = request_firmware(&nvavp_os_fw, fw_os_file,
+					nvavp->video_misc_dev.this_device);
+		if (ret) {
+			dev_err(&nvavp->nvhost_dev->dev,
+				"cannot read os firmware '%s'\n", fw_os_file);
+			goto err_req_fw;
+		}
+
+		dev_info(&nvavp->nvhost_dev->dev,
+			"read firmware from '%s' (%d bytes)\n",
+			fw_os_file, nvavp_os_fw->size);
+
+		ptr = (void *)nvavp_os_fw->data;
+
+		if (strncmp((const char *)ptr, "NVAVP-OS", 8)) {
+			dev_info(&nvavp->nvhost_dev->dev,
+				"os hdr string mismatch\n");
+			ret = -EINVAL;
+			goto err_os_bin;
+		}
+
+		ptr += 8;
+		os_info->entry_offset = *((u32 *)ptr);
+		ptr += sizeof(u32);
+		os_info->control_offset = *((u32 *)ptr);
+		ptr += sizeof(u32);
+		os_info->debug_offset = *((u32 *)ptr);
+		ptr += sizeof(u32);
+
+		size = *((u32 *)ptr);    ptr += sizeof(u32);
+
+		os_info->size = size;
+		os_info->os_bin = kzalloc(os_info->size,
+						GFP_KERNEL);
+		if (!os_info->os_bin) {
+			dev_err(&nvavp->nvhost_dev->dev,
+				"cannot allocate os bin\n");
+			ret = -ENOMEM;
+			goto err_os_bin;
+		}
+
+		memcpy(os_info->os_bin, ptr, os_info->size);
+		memset(os_info->data + os_info->size, 0, SZ_1M - os_info->size);
+
+		dev_info(&nvavp->nvhost_dev->dev,
+			"entry=%08x control=%08x debug=%08x size=%d\n",
+			os_info->entry_offset, os_info->control_offset,
+			os_info->debug_offset, os_info->size);
+		release_firmware(nvavp_os_fw);
+	}
+
+	memcpy(os_info->data, os_info->os_bin, os_info->size);
+	os_info->reset_addr = os_info->phys + os_info->entry_offset;
+
+	dev_info(&nvavp->nvhost_dev->dev,
+		"AVP os at vaddr=%p paddr=%lx reset_addr=%p\n",
+		os_info->data, (unsigned long)(os_info->phys),
+				(void *)os_info->reset_addr);
+	return 0;
+
+err_os_bin:
+	release_firmware(nvavp_os_fw);
+err_req_fw:
+	return ret;
+}
+
+
+static int nvavp_os_init(struct nvavp_info *nvavp)
+{
+	char fw_os_file[32];
+	int ret = 0;
+	int video_initialized, audio_initialized = 0;
+
+	video_initialized = nvavp_get_video_init_status(nvavp);
+
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+	audio_initialized = nvavp_get_audio_init_status(nvavp);
+#endif
+	pr_debug("video_initialized(%d) audio_initialized(%d)\n",
+		video_initialized, audio_initialized);
+
+	/* Video and Audio both are initialized */
+	if (video_initialized || audio_initialized)
+		return ret;
+
+	/* Video or Audio both are uninitialized */
+	pr_debug("video_initialized == audio_initialized (%d)\n",
+		nvavp->video_initialized);
+#if defined(CONFIG_TEGRA_AVP_KERNEL_ON_MMU) /* Tegra2 with AVP MMU */
+	/* paddr is any address returned from nvmap_pin */
+	/* vaddr is AVP_KERNEL_VIRT_BASE */
+	dev_info(&nvavp->nvhost_dev->dev,
+		"using AVP MMU to relocate AVP os\n");
+	sprintf(fw_os_file, "nvavp_os.bin");
+	nvavp->os_info.reset_addr = AVP_KERNEL_VIRT_BASE;
+#elif defined(CONFIG_TEGRA_AVP_KERNEL_ON_SMMU) /* Tegra3 with SMMU */
+	/* paddr is any address behind SMMU */
+	/* vaddr is TEGRA_SMMU_BASE */
+	dev_info(&nvavp->nvhost_dev->dev,
+		"using SMMU at %lx to load AVP kernel\n",
+		(unsigned long)nvavp->os_info.phys);
+	BUG_ON(nvavp->os_info.phys != 0xeff00000
+		&& nvavp->os_info.phys != 0x0ff00000);
+	sprintf(fw_os_file, "nvavp_os_%08lx.bin",
+		(unsigned long)nvavp->os_info.phys);
+	nvavp->os_info.reset_addr = nvavp->os_info.phys;
+#else /* nvmem= carveout */
+	/* paddr is found in nvmem= carveout */
+	/* vaddr is same as paddr */
+	/* Find nvmem carveout */
+	if (!pfn_valid(__phys_to_pfn(0x8e000000))) {
+		nvavp->os_info.phys = 0x8e000000;
+	} else if (!pfn_valid(__phys_to_pfn(0x9e000000))) {
+		nvavp->os_info.phys = 0x9e000000;
+	} else if (!pfn_valid(__phys_to_pfn(0xbe000000))) {
+		nvavp->os_info.phys = 0xbe000000;
+	} else {
+		dev_err(&nvavp->nvhost_dev->dev,
+			"cannot find nvmem= carveout to load AVP os\n");
+		dev_err(&nvavp->nvhost_dev->dev,
+			"check kernel command line "
+			"to see if nvmem= is defined\n");
+		BUG();
+	}
+	dev_info(&nvavp->nvhost_dev->dev,
+		"using nvmem= carveout at %lx to load AVP os\n",
+		nvavp->os_info.phys);
+	sprintf(fw_os_file, "nvavp_os_%08lx.bin", nvavp->os_info.phys);
+	nvavp->os_info.reset_addr = nvavp->os_info.phys;
+	nvavp->os_info.data = ioremap(nvavp->os_info.phys, SZ_1M);
+#endif
+	ret = nvavp_load_os(nvavp, fw_os_file);
+	if (ret) {
+		dev_err(&nvavp->nvhost_dev->dev,
+			"unable to load os firmware '%s'\n", fw_os_file);
+		goto err_exit;
+	}
+
+	ret = nvavp_pushbuffer_init(nvavp);
+	if (ret) {
+		dev_err(&nvavp->nvhost_dev->dev,
+			"unable to init pushbuffer\n");
+		goto err_exit;
+	}
+	tegra_init_legacy_irq_cop();
+	enable_irq(nvavp->mbox_from_avp_pend_irq);
+err_exit:
+	return ret;
+}
+
+static int nvavp_init(struct nvavp_info *nvavp, int channel_id)
+{
+	int ret = 0;
+
+	ret = nvavp_os_init(nvavp);
+	if (ret) {
+		dev_err(&nvavp->nvhost_dev->dev,
+			"unable to load os firmware and allocate buffers\n");
+	}
+
+	if (IS_VIDEO_CHANNEL_ID(channel_id) &&
+		(!nvavp_get_video_init_status(nvavp)) ) {
+		pr_debug("nvavp_init : channel_ID (%d)\n", channel_id);
+		ret = nvavp_load_ucode(nvavp);
+		if (ret) {
+			dev_err(&nvavp->nvhost_dev->dev,
+				"unable to load ucode\n");
+			goto err_exit;
+		}
+
+		nvavp_reset_vde(nvavp);
+		nvavp_reset_avp(nvavp, nvavp->os_info.reset_addr);
+
+		nvavp_set_video_init_status(nvavp, 1);
+	}
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+	if (IS_AUDIO_CHANNEL_ID(channel_id) &&
+		(!nvavp_get_audio_init_status(nvavp))) {
+		pr_debug("nvavp_init : channel_ID (%d)\n", channel_id);
+		nvavp_reset_avp(nvavp, nvavp->os_info.reset_addr);
+		nvavp_set_audio_init_status(nvavp, 1);
+	}
+#endif
+
+err_exit:
+	return ret;
+}
+
+static void nvavp_uninit(struct nvavp_info *nvavp)
+{
+	int video_initialized, audio_initialized = 0;
+
+	video_initialized = nvavp_get_video_init_status(nvavp);
+
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+	audio_initialized = nvavp_get_audio_init_status(nvavp);
+#endif
+
+	pr_debug("nvavp_uninit video_initialized(%d) audio_initialized(%d)\n",
+		video_initialized, audio_initialized);
+
+	/* Video and Audio both are uninitialized */
+	if (!video_initialized && !audio_initialized)
+		return;
+
+	if (video_initialized) {
+		pr_debug("nvavp_uninit nvavp->video_initialized\n");
+		cancel_work_sync(&nvavp->clock_disable_work);
+		nvavp_halt_vde(nvavp);
+		nvavp_set_video_init_status(nvavp, 0);
+		video_initialized = 0;
+	}
+
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+	if (audio_initialized) {
+		cancel_work_sync(&nvavp->app_notify_work);
+		nvavp_set_audio_init_status(nvavp, 0);
+		audio_initialized = 0;
+	}
+#endif
+
+	/* Video and Audio both becomes uninitialized */
+	if (video_initialized == audio_initialized) {
+		pr_debug("nvavp_uninit both channels unitialized\n");
+
+		clk_disable(nvavp->sclk);
+		clk_disable(nvavp->emc_clk);
+		disable_irq(nvavp->mbox_from_avp_pend_irq);
+		nvavp_pushbuffer_deinit(nvavp);
+		nvavp_halt_avp(nvavp);
+	}
+}
+
+static int nvavp_set_clock_ioctl(struct file *filp, unsigned int cmd,
+							unsigned long arg)
+{
+	struct nvavp_clientctx *clientctx = filp->private_data;
+	struct nvavp_info *nvavp = clientctx->nvavp;
+	struct clk *c;
+	struct nvavp_clock_args config;
+
+	if (copy_from_user(&config, (void __user *)arg, sizeof(struct nvavp_clock_args)))
+		return -EFAULT;
+
+	dev_dbg(&nvavp->nvhost_dev->dev, "%s: clk_id=%d, clk_rate=%u\n",
+			__func__, config.id, config.rate);
+
+	if (config.id == NVAVP_MODULE_ID_AVP)
+		nvavp->sclk_rate = config.rate;
+	else if	(config.id == NVAVP_MODULE_ID_EMC)
+		nvavp->emc_clk_rate = config.rate;
+
+	c = nvavp_clk_get(nvavp, config.id);
+	if (IS_ERR_OR_NULL(c))
+		return -EINVAL;
+
+	clk_enable(c);
+	clk_set_rate(c, config.rate);
+
+	config.rate = clk_get_rate(c);
+	clk_disable(c);
+	if (copy_to_user((void __user *)arg, &config, sizeof(struct nvavp_clock_args)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static int nvavp_get_clock_ioctl(struct file *filp, unsigned int cmd,
+							unsigned long arg)
+{
+	struct nvavp_clientctx *clientctx = filp->private_data;
+	struct nvavp_info *nvavp = clientctx->nvavp;
+	struct clk *c;
+	struct nvavp_clock_args config;
+
+	if (copy_from_user(&config, (void __user *)arg, sizeof(struct nvavp_clock_args)))
+		return -EFAULT;
+
+	c = nvavp_clk_get(nvavp, config.id);
+	if (IS_ERR_OR_NULL(c))
+		return -EINVAL;
+
+	clk_enable(c);
+	config.rate = clk_get_rate(c);
+	clk_disable(c);
+
+	if (copy_to_user((void __user *)arg, &config, sizeof(struct nvavp_clock_args)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static int nvavp_get_syncpointid_ioctl(struct file *filp, unsigned int cmd,
+							unsigned long arg)
+{
+	struct nvavp_clientctx *clientctx = filp->private_data;
+	struct nvavp_info *nvavp = clientctx->nvavp;
+	u32 id = nvavp->syncpt_id;
+
+	if (_IOC_DIR(cmd) & _IOC_READ) {
+		if (copy_to_user((void __user *)arg, &id, sizeof(u32)))
+			return -EFAULT;
+		else
+			return 0;
+	}
+	return -EFAULT;
+}
+
+static int nvavp_set_nvmapfd_ioctl(struct file *filp, unsigned int cmd,
+							unsigned long arg)
+{
+	struct nvavp_clientctx *clientctx = filp->private_data;
+	struct nvavp_set_nvmap_fd_args buf;
+	struct nvmap_client *new_client;
+	int fd;
+
+	if (_IOC_DIR(cmd) & _IOC_WRITE) {
+		if (copy_from_user(&buf, (void __user *)arg, _IOC_SIZE(cmd)))
+			return -EFAULT;
+	}
+
+	fd = buf.fd;
+	new_client = nvmap_client_get_file(fd);
+	if (IS_ERR(new_client))
+		return PTR_ERR(new_client);
+
+	clientctx->nvmap = new_client;
+	return 0;
+}
+
+static int nvavp_pushbuffer_submit_ioctl(struct file *filp, unsigned int cmd,
+							unsigned long arg)
+{
+	struct nvavp_clientctx *clientctx = filp->private_data;
+	struct nvavp_info *nvavp = clientctx->nvavp;
+	struct nvavp_pushbuffer_submit_hdr hdr;
+	u32 *cmdbuf_data;
+	struct nvmap_handle *cmdbuf_handle = NULL;
+	struct nvmap_handle_ref *cmdbuf_dupe;
+	int ret = 0, i;
+	unsigned long phys_addr;
+	unsigned long virt_addr;
+	struct nvavp_pushbuffer_submit_hdr *user_hdr =
+			(struct nvavp_pushbuffer_submit_hdr *) arg;
+	struct nvavp_syncpt syncpt;
+
+	syncpt.id = NVSYNCPT_INVALID;
+	syncpt.value = 0;
+
+	if (_IOC_DIR(cmd) & _IOC_WRITE) {
+		if (copy_from_user(&hdr, (void __user *)arg,
+			sizeof(struct nvavp_pushbuffer_submit_hdr)))
+			return -EFAULT;
+	}
+
+	if (!hdr.cmdbuf.mem)
+		return 0;
+
+	if (copy_from_user(clientctx->relocs, (void __user *)hdr.relocs,
+			sizeof(struct nvavp_reloc) * hdr.num_relocs)) {
+		return -EFAULT;
+	}
+
+	cmdbuf_handle = nvmap_get_handle_id(clientctx->nvmap, hdr.cmdbuf.mem);
+	if (cmdbuf_handle == NULL) {
+		dev_err(&nvavp->nvhost_dev->dev,
+			"invalid cmd buffer handle %08x\n", hdr.cmdbuf.mem);
+		return -EPERM;
+	}
+
+	/* duplicate the new pushbuffer's handle into the nvavp driver's
+	 * nvmap context, to ensure that the handle won't be freed as
+	 * long as it is in-use by the fb driver */
+	cmdbuf_dupe = nvmap_duplicate_handle_id(nvavp->nvmap, hdr.cmdbuf.mem);
+	nvmap_handle_put(cmdbuf_handle);
+
+	if (IS_ERR(cmdbuf_dupe)) {
+		dev_err(&nvavp->nvhost_dev->dev,
+			"could not duplicate handle\n");
+		return PTR_ERR(cmdbuf_dupe);
+	}
+
+	phys_addr = nvmap_pin(nvavp->nvmap, cmdbuf_dupe);
+	if (IS_ERR((void *)phys_addr)) {
+		dev_err(&nvavp->nvhost_dev->dev, "could not pin handle\n");
+		nvmap_free(nvavp->nvmap, cmdbuf_dupe);
+		return PTR_ERR((void *)phys_addr);
+	}
+
+	virt_addr = (unsigned long)nvmap_mmap(cmdbuf_dupe);
+	if (!virt_addr) {
+		dev_err(&nvavp->nvhost_dev->dev, "cannot map cmdbuf handle\n");
+		ret = -ENOMEM;
+		goto err_cmdbuf_mmap;
+	}
+
+	cmdbuf_data = (u32 *)(virt_addr + hdr.cmdbuf.offset);
+
+	for (i = 0; i < hdr.num_relocs; i++) {
+		u32 *reloc_addr, target_phys_addr;
+
+		if (clientctx->relocs[i].cmdbuf_mem != hdr.cmdbuf.mem) {
+			dev_err(&nvavp->nvhost_dev->dev,
+				"reloc info does not match target bufferID\n");
+			ret = -EPERM;
+			goto err_reloc_info;
+		}
+
+		reloc_addr = cmdbuf_data +
+			     (clientctx->relocs[i].cmdbuf_offset >> 2);
+
+		target_phys_addr = nvmap_handle_address(clientctx->nvmap,
+					    clientctx->relocs[i].target);
+		target_phys_addr += clientctx->relocs[i].target_offset;
+		writel(target_phys_addr, reloc_addr);
+	}
+
+	if (hdr.syncpt) {
+		ret = nvavp_pushbuffer_update(nvavp,
+					     (phys_addr + hdr.cmdbuf.offset),
+					      hdr.cmdbuf.words, &syncpt,
+					      (hdr.flags & NVAVP_UCODE_EXT),
+						clientctx->channel_id);
+
+		if (copy_to_user((void __user *)user_hdr->syncpt, &syncpt,
+				sizeof(struct nvavp_syncpt))) {
+			ret = -EFAULT;
+			goto err_reloc_info;
+		}
+	} else {
+		ret = nvavp_pushbuffer_update(nvavp,
+					     (phys_addr + hdr.cmdbuf.offset),
+					      hdr.cmdbuf.words, NULL,
+					      (hdr.flags & NVAVP_UCODE_EXT),
+						clientctx->channel_id);
+	}
+
+err_reloc_info:
+	nvmap_munmap(cmdbuf_dupe, (void *)virt_addr);
+err_cmdbuf_mmap:
+	nvmap_unpin(nvavp->nvmap, cmdbuf_dupe);
+	nvmap_free(nvavp->nvmap, cmdbuf_dupe);
+	return ret;
+}
+
+static int nvavp_wake_avp_ioctl(struct file *filp, unsigned int cmd,
+							unsigned long arg)
+{
+	wmb();
+	/* wake up avp */
+	writel(0xA0000001, NVAVP_OS_OUTBOX);
+	return 0;
+}
+
+static int nvavp_force_clock_stay_on_ioctl(struct file *filp, unsigned int cmd,
+							unsigned long arg)
+{
+	struct nvavp_clientctx *clientctx = filp->private_data;
+	struct nvavp_info *nvavp = clientctx->nvavp;
+	struct nvavp_clock_stay_on_state_args clock;
+
+	if (copy_from_user(&clock, (void __user *)arg,
+			   sizeof(struct nvavp_clock_stay_on_state_args)))
+		return -EFAULT;
+
+	dev_dbg(&nvavp->nvhost_dev->dev, "%s: state=%d\n",
+		__func__, clock.state);
+
+	if (clock.state != NVAVP_CLOCK_STAY_ON_DISABLED &&
+	    clock.state !=  NVAVP_CLOCK_STAY_ON_ENABLED) {
+		dev_err(&nvavp->nvhost_dev->dev, "%s: invalid argument=%d\n",
+			__func__, clock.state);
+		return -EINVAL;
+	}
+
+	mutex_lock(&nvavp->open_lock);
+	if (clock.state) {
+		if (clientctx->clk_reqs++ == 0)
+			nvavp_clks_enable(nvavp);
+	} else {
+		if (--clientctx->clk_reqs == 0)
+			nvavp_clks_disable(nvavp);
+	}
+	mutex_unlock(&nvavp->open_lock);
+	return 0;
+}
+
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+static int nvavp_enable_audio_clocks(struct file *filp, unsigned int cmd,
+					unsigned long arg)
+{
+	struct nvavp_clientctx *clientctx = filp->private_data;
+	struct nvavp_info *nvavp = clientctx->nvavp;
+	struct nvavp_clock_args config;
+
+	if (copy_from_user(&config, (void __user *)arg, sizeof(struct nvavp_clock_args)))
+		return -EFAULT;
+
+	dev_dbg(&nvavp->nvhost_dev->dev, "%s: clk_id=%d\n",
+			__func__, config.id);
+
+	if (config.id == NVAVP_MODULE_ID_VCP)
+		clk_enable(nvavp->vcp_clk);
+	else if	(config.id == NVAVP_MODULE_ID_BSEA)
+		clk_enable(nvavp->bsea_clk);
+
+	return 0;
+}
+
+static int nvavp_disable_audio_clocks(struct file *filp, unsigned int cmd,
+					unsigned long arg)
+{
+	struct nvavp_clientctx *clientctx = filp->private_data;
+	struct nvavp_info *nvavp = clientctx->nvavp;
+	struct nvavp_clock_args config;
+
+	if (copy_from_user(&config, (void __user *)arg, sizeof(struct nvavp_clock_args)))
+		return -EFAULT;
+
+	dev_dbg(&nvavp->nvhost_dev->dev, "%s: clk_id=%d\n",
+			__func__, config.id);
+
+	if (config.id == NVAVP_MODULE_ID_VCP)
+		clk_disable(nvavp->vcp_clk);
+	else if	(config.id == NVAVP_MODULE_ID_BSEA)
+		clk_disable(nvavp->bsea_clk);
+
+	return 0;
+}
+#else
+static int nvavp_enable_audio_clocks(struct file *filp, unsigned int cmd,
+					unsigned long arg)
+{
+	return 0;
+}
+
+static int nvavp_disable_audio_clocks(struct file *filp, unsigned int cmd,
+					unsigned long arg)
+{
+	return 0;
+}
+#endif
+
+static int tegra_nvavp_open(struct inode *inode, struct file *filp, int channel_id)
+{
+	struct miscdevice *miscdev = filp->private_data;
+	struct nvavp_info *nvavp = dev_get_drvdata(miscdev->parent);
+	int ret = 0;
+	struct nvavp_clientctx *clientctx;
+
+	dev_dbg(&nvavp->nvhost_dev->dev, "%s: ++\n", __func__);
+
+	nonseekable_open(inode, filp);
+
+	clientctx = kzalloc(sizeof(*clientctx), GFP_KERNEL);
+	if (!clientctx)
+		return -ENOMEM;
+
+	mutex_lock(&nvavp->open_lock);
+
+	pr_debug("tegra_nvavp_open channel_id (%d)\n", channel_id);
+
+	clientctx->channel_id = channel_id;
+
+	ret = nvavp_init(nvavp, channel_id);
+
+	if (!ret)
+		nvavp->refcount++;
+
+	clientctx->nvavp = nvavp;
+
+	filp->private_data = clientctx;
+
+	mutex_unlock(&nvavp->open_lock);
+
+	return ret;
+}
+
+static int tegra_nvavp_video_open(struct inode *inode, struct file *filp)
+{
+	pr_debug("tegra_nvavp_video_open NVAVP_VIDEO_CHANNEL\n");
+	return tegra_nvavp_open(inode, filp, NVAVP_VIDEO_CHANNEL);
+}
+
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+static int tegra_nvavp_audio_open(struct inode *inode, struct file *filp)
+{
+	pr_debug("tegra_nvavp_audio_open NVAVP_AUDIO_CHANNEL\n");
+	return tegra_nvavp_open(inode, filp, NVAVP_AUDIO_CHANNEL);
+}
+#endif
+
+static int tegra_nvavp_release(struct inode *inode, struct file *filp)
+{
+	struct nvavp_clientctx *clientctx = filp->private_data;
+	struct nvavp_info *nvavp = clientctx->nvavp;
+	int ret = 0;
+
+	dev_dbg(&nvavp->nvhost_dev->dev, "%s: ++\n", __func__);
+
+	filp->private_data = NULL;
+
+	mutex_lock(&nvavp->open_lock);
+
+	if (!nvavp->refcount) {
+		dev_err(&nvavp->nvhost_dev->dev,
+			"releasing while in invalid state\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* if this client had any requests, drop our clk ref */
+	if (clientctx->clk_reqs)
+		nvavp_clks_disable(nvavp);
+
+	if (nvavp->refcount > 0)
+		nvavp->refcount--;
+	if (!nvavp->refcount)
+		nvavp_uninit(nvavp);
+
+out:
+	nvmap_client_put(clientctx->nvmap);
+	mutex_unlock(&nvavp->open_lock);
+	kfree(clientctx);
+	return ret;
+}
+
+static long tegra_nvavp_ioctl(struct file *filp, unsigned int cmd,
+			    unsigned long arg)
+{
+	int ret = 0;
+
+	if (_IOC_TYPE(cmd) != NVAVP_IOCTL_MAGIC ||
+	    _IOC_NR(cmd) < NVAVP_IOCTL_MIN_NR ||
+	    _IOC_NR(cmd) > NVAVP_IOCTL_MAX_NR)
+		return -EFAULT;
+
+	switch (cmd) {
+	case NVAVP_IOCTL_SET_NVMAP_FD:
+		ret = nvavp_set_nvmapfd_ioctl(filp, cmd, arg);
+		break;
+	case NVAVP_IOCTL_GET_SYNCPOINT_ID:
+		ret = nvavp_get_syncpointid_ioctl(filp, cmd, arg);
+		break;
+	case NVAVP_IOCTL_PUSH_BUFFER_SUBMIT:
+		ret = nvavp_pushbuffer_submit_ioctl(filp, cmd, arg);
+		break;
+	case NVAVP_IOCTL_SET_CLOCK:
+		ret = nvavp_set_clock_ioctl(filp, cmd, arg);
+		break;
+	case NVAVP_IOCTL_GET_CLOCK:
+		ret = nvavp_get_clock_ioctl(filp, cmd, arg);
+		break;
+	case NVAVP_IOCTL_WAKE_AVP:
+		ret = nvavp_wake_avp_ioctl(filp, cmd, arg);
+		break;
+	case NVAVP_IOCTL_FORCE_CLOCK_STAY_ON:
+		ret = nvavp_force_clock_stay_on_ioctl(filp, cmd, arg);
+		break;
+	case NVAVP_IOCTL_ENABLE_AUDIO_CLOCKS:
+		ret = nvavp_enable_audio_clocks(filp, cmd, arg);
+		break;
+	case NVAVP_IOCTL_DISABLE_AUDIO_CLOCKS:
+		ret = nvavp_disable_audio_clocks(filp, cmd, arg);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+static const struct file_operations tegra_video_nvavp_fops = {
+	.owner		= THIS_MODULE,
+	.open		= tegra_nvavp_video_open,
+	.release	= tegra_nvavp_release,
+	.unlocked_ioctl	= tegra_nvavp_ioctl,
+};
+
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+static const struct file_operations tegra_audio_nvavp_fops = {
+	.owner          = THIS_MODULE,
+	.open           = tegra_nvavp_audio_open,
+	.release        = tegra_nvavp_release,
+	.unlocked_ioctl = tegra_nvavp_ioctl,
+};
+#endif
+
+static int tegra_nvavp_probe(struct nvhost_device *ndev,
+	struct nvhost_device_id *id_table)
+{
+	struct nvavp_info *nvavp;
+	int irq;
+	unsigned int heap_mask;
+	u32 iovmm_addr;
+	int ret = 0, channel_id;
+
+	irq = nvhost_get_irq_byname(ndev, "mbox_from_nvavp_pending");
+	if (irq < 0) {
+		dev_err(&ndev->dev, "invalid nvhost data\n");
+		return -EINVAL;
+	}
+
+	nvavp = kzalloc(sizeof(struct nvavp_info), GFP_KERNEL);
+	if (!nvavp) {
+		dev_err(&ndev->dev, "cannot allocate avp_info\n");
+		return -ENOMEM;
+	}
+
+	memset(nvavp, 0, sizeof(*nvavp));
+
+	nvavp->nvmap = nvmap_create_client(nvmap_dev, "nvavp_drv");
+	if (IS_ERR_OR_NULL(nvavp->nvmap)) {
+		dev_err(&ndev->dev, "cannot create nvmap client\n");
+		ret = PTR_ERR(nvavp->nvmap);
+		goto err_nvmap_create_drv_client;
+	}
+
+#if defined(CONFIG_TEGRA_AVP_KERNEL_ON_MMU) /* Tegra2 with AVP MMU */
+	heap_mask = NVMAP_HEAP_CARVEOUT_GENERIC;
+#elif defined(CONFIG_TEGRA_AVP_KERNEL_ON_SMMU) /* Tegra3 with SMMU */
+	heap_mask = NVMAP_HEAP_IOVMM;
+#else /* nvmem= carveout */
+	heap_mask = 0;
+#endif
+	switch (heap_mask) {
+	case NVMAP_HEAP_IOVMM:
+
+		iovmm_addr = 0x0ff00000;
+
+		nvavp->os_info.handle = nvmap_alloc_iovm(nvavp->nvmap, SZ_1M,
+						L1_CACHE_BYTES,
+						NVMAP_HANDLE_UNCACHEABLE,
+						iovmm_addr);
+		if (IS_ERR_OR_NULL(nvavp->os_info.handle)) {
+			dev_err(&ndev->dev,
+				"cannot create os handle\n");
+			ret = PTR_ERR(nvavp->os_info.handle);
+			goto err_nvmap_alloc;
+		}
+
+		nvavp->os_info.data = nvmap_mmap(nvavp->os_info.handle);
+		if (!nvavp->os_info.data) {
+			dev_err(&ndev->dev,
+				"cannot map os handle\n");
+			ret = -ENOMEM;
+			goto err_nvmap_mmap;
+		}
+
+		nvavp->os_info.phys =
+			nvmap_pin(nvavp->nvmap, nvavp->os_info.handle);
+		if (IS_ERR_OR_NULL((void *)nvavp->os_info.phys)) {
+			dev_err(&ndev->dev,
+				"cannot pin os handle\n");
+			ret = PTR_ERR((void *)nvavp->os_info.phys);
+			goto err_nvmap_pin;
+		}
+
+		dev_info(&ndev->dev,
+			"allocated IOVM at %lx for AVP os\n",
+			(unsigned long)nvavp->os_info.phys);
+		break;
+	case NVMAP_HEAP_CARVEOUT_GENERIC:
+		nvavp->os_info.handle = nvmap_alloc(nvavp->nvmap, SZ_1M, SZ_1M,
+						NVMAP_HANDLE_UNCACHEABLE, 0);
+		if (IS_ERR_OR_NULL(nvavp->os_info.handle)) {
+			dev_err(&ndev->dev, "cannot create AVP os handle\n");
+			ret = PTR_ERR(nvavp->os_info.handle);
+			goto err_nvmap_alloc;
+		}
+
+		nvavp->os_info.data = nvmap_mmap(nvavp->os_info.handle);
+		if (!nvavp->os_info.data) {
+			dev_err(&ndev->dev, "cannot map AVP os handle\n");
+			ret = -ENOMEM;
+			goto err_nvmap_mmap;
+		}
+
+		nvavp->os_info.phys = nvmap_pin(nvavp->nvmap,
+					nvavp->os_info.handle);
+		if (IS_ERR_OR_NULL((void *)nvavp->os_info.phys)) {
+			dev_err(&ndev->dev, "cannot pin AVP os handle\n");
+			ret = PTR_ERR((void *)nvavp->os_info.phys);
+			goto err_nvmap_pin;
+		}
+
+		dev_info(&ndev->dev,
+			"allocated carveout memory at %lx for AVP os\n",
+			(unsigned long)nvavp->os_info.phys);
+		break;
+	default:
+		dev_err(&ndev->dev, "invalid/non-supported heap for AVP os\n");
+		ret = -EINVAL;
+		goto err_get_syncpt;
+	}
+
+	nvavp->mbox_from_avp_pend_irq = irq;
+	mutex_init(&nvavp->open_lock);
+
+	for (channel_id = 0; channel_id < NVAVP_NUM_CHANNELS; channel_id++)
+		mutex_init(&nvavp->channel_info[channel_id].pushbuffer_lock);
+
+	/* TODO DO NOT USE NVAVP DEVICE */
+	nvavp->cop_clk = clk_get(&ndev->dev, "cop");
+	if (IS_ERR(nvavp->cop_clk)) {
+		dev_err(&ndev->dev, "cannot get cop clock\n");
+		ret = -ENOENT;
+		goto err_get_cop_clk;
+	}
+
+	nvavp->vde_clk = clk_get(&ndev->dev, "vde");
+	if (IS_ERR(nvavp->vde_clk)) {
+		dev_err(&ndev->dev, "cannot get vde clock\n");
+		ret = -ENOENT;
+		goto err_get_vde_clk;
+	}
+
+	nvavp->bsev_clk = clk_get(&ndev->dev, "bsev");
+	if (IS_ERR(nvavp->bsev_clk)) {
+		dev_err(&ndev->dev, "cannot get bsev clock\n");
+		ret = -ENOENT;
+		goto err_get_bsev_clk;
+	}
+
+	nvavp->sclk = clk_get(&ndev->dev, "sclk");
+	if (IS_ERR(nvavp->sclk)) {
+		dev_err(&ndev->dev, "cannot get avp.sclk clock\n");
+		ret = -ENOENT;
+		goto err_get_sclk;
+	}
+
+	nvavp->emc_clk = clk_get(&ndev->dev, "emc");
+	if (IS_ERR(nvavp->emc_clk)) {
+		dev_err(&ndev->dev, "cannot get emc clock\n");
+		ret = -ENOENT;
+		goto err_get_emc_clk;
+	}
+
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+	nvavp->bsea_clk = clk_get(&ndev->dev, "bsea");
+	if (IS_ERR(nvavp->bsea_clk)) {
+		dev_err(&ndev->dev, "cannot get bsea clock\n");
+		ret = -ENOENT;
+		goto err_get_bsea_clk;
+	}
+
+	nvavp->vcp_clk = clk_get(&ndev->dev, "vcp");
+	if (IS_ERR(nvavp->vcp_clk)) {
+		dev_err(&ndev->dev, "cannot get vcp clock\n");
+		ret = -ENOENT;
+		goto err_get_vcp_clk;
+	}
+#endif
+
+	nvavp->clk_enabled = 0;
+	nvavp_halt_avp(nvavp);
+
+	INIT_WORK(&nvavp->clock_disable_work, clock_disable_handler);
+
+	nvavp->video_misc_dev.minor = MISC_DYNAMIC_MINOR;
+	nvavp->video_misc_dev.name = "tegra_avpchannel";
+	nvavp->video_misc_dev.fops = &tegra_video_nvavp_fops;
+	nvavp->video_misc_dev.mode = S_IRWXUGO;
+	nvavp->video_misc_dev.parent = &ndev->dev;
+
+	ret = misc_register(&nvavp->video_misc_dev);
+	if (ret) {
+		dev_err(&ndev->dev, "unable to register misc device!\n");
+		goto err_misc_reg;
+	}
+
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+	INIT_WORK(&nvavp->app_notify_work, app_notify_handler);
+	nvavp->audio_misc_dev.minor = MISC_DYNAMIC_MINOR;
+	nvavp->audio_misc_dev.name = "tegra_audio_avpchannel";
+	nvavp->audio_misc_dev.fops = &tegra_audio_nvavp_fops;
+	nvavp->audio_misc_dev.mode = S_IRWXUGO;
+	nvavp->audio_misc_dev.parent = &ndev->dev;
+
+	ret = misc_register(&nvavp->audio_misc_dev);
+	if (ret) {
+	dev_err(&ndev->dev, "unable to register misc device!\n");
+		goto err_audio_misc_reg;
+	}
+#endif
+
+	ret = request_irq(irq, nvavp_mbox_pending_isr, 0,
+			  TEGRA_NVAVP_NAME, nvavp);
+	if (ret) {
+		dev_err(&ndev->dev, "cannot register irq handler\n");
+		goto err_req_irq_pend;
+	}
+	disable_irq(nvavp->mbox_from_avp_pend_irq);
+
+	nvhost_set_drvdata(ndev, nvavp);
+	nvavp->nvhost_dev = ndev;
+
+	return 0;
+
+err_req_irq_pend:
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+	misc_deregister(&nvavp->audio_misc_dev);
+err_audio_misc_reg:
+#endif
+	misc_deregister(&nvavp->video_misc_dev);
+err_misc_reg:
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+	clk_put(nvavp->vcp_clk);
+err_get_vcp_clk:
+	clk_put(nvavp->bsea_clk);
+err_get_bsea_clk:
+#endif
+	clk_put(nvavp->emc_clk);
+err_get_emc_clk:
+	clk_put(nvavp->sclk);
+err_get_sclk:
+	clk_put(nvavp->bsev_clk);
+err_get_bsev_clk:
+	clk_put(nvavp->vde_clk);
+err_get_vde_clk:
+	clk_put(nvavp->cop_clk);
+err_get_cop_clk:
+	nvmap_unpin(nvavp->nvmap, nvavp->os_info.handle);
+err_nvmap_pin:
+	nvmap_munmap(nvavp->os_info.handle, nvavp->os_info.data);
+err_nvmap_mmap:
+#if defined(CONFIG_TEGRA_AVP_KERNEL_ON_MMU)
+	nvmap_free(nvavp->nvmap, nvavp->os_info.handle);
+#elif defined(CONFIG_TEGRA_AVP_KERNEL_ON_SMMU)
+	nvmap_free_iovm(nvavp->nvmap, nvavp->os_info.handle);
+#endif
+err_nvmap_alloc:
+	nvmap_client_put(nvavp->nvmap);
+err_nvmap_create_drv_client:
+err_get_syncpt:
+	kfree(nvavp);
+	return ret;
+}
+
+static int tegra_nvavp_remove(struct nvhost_device *ndev)
+{
+	struct nvavp_info *nvavp = nvhost_get_drvdata(ndev);
+
+	if (!nvavp)
+		return 0;
+
+	mutex_lock(&nvavp->open_lock);
+	if (nvavp->refcount) {
+		mutex_unlock(&nvavp->open_lock);
+		return -EBUSY;
+	}
+	mutex_unlock(&nvavp->open_lock);
+
+	nvavp_unload_ucode(nvavp);
+	nvavp_unload_os(nvavp);
+
+	misc_deregister(&nvavp->video_misc_dev);
+
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+	misc_deregister(&nvavp->audio_misc_dev);
+	clk_put(nvavp->vcp_clk);
+	clk_put(nvavp->bsea_clk);
+#endif
+	clk_put(nvavp->bsev_clk);
+	clk_put(nvavp->vde_clk);
+	clk_put(nvavp->cop_clk);
+
+	clk_put(nvavp->emc_clk);
+	clk_put(nvavp->sclk);
+
+	nvmap_client_put(nvavp->nvmap);
+
+	kfree(nvavp);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int tegra_nvavp_suspend(struct nvhost_device *ndev, pm_message_t state)
+{
+	struct nvavp_info *nvavp = nvhost_get_drvdata(ndev);
+	int ret = 0;
+
+	mutex_lock(&nvavp->open_lock);
+
+	if (nvavp->refcount) {
+		if (!nvavp->clk_enabled) {
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+			if (nvavp_check_idle(nvavp, NVAVP_AUDIO_CHANNEL))
+				nvavp_uninit(nvavp);
+			else
+				ret = -EBUSY;
+#else
+			nvavp_uninit(nvavp);
+#endif
+		}
+		else {
+			ret = -EBUSY;
+		}
+	}
+
+	mutex_unlock(&nvavp->open_lock);
+	return ret;
+}
+
+static int tegra_nvavp_resume(struct nvhost_device *ndev)
+{
+	struct nvavp_info *nvavp = nvhost_get_drvdata(ndev);
+
+	mutex_lock(&nvavp->open_lock);
+
+	if (nvavp->refcount) {
+		nvavp_init(nvavp, NVAVP_VIDEO_CHANNEL);
+#if defined(CONFIG_TEGRA_NVAVP_AUDIO)
+		nvavp_init(nvavp, NVAVP_AUDIO_CHANNEL);
+#endif
+	}
+	mutex_unlock(&nvavp->open_lock);
+
+	return 0;
+}
+#endif
+
+static struct nvhost_driver tegra_nvavp_driver = {
+	.driver	= {
+		.name	= TEGRA_NVAVP_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= tegra_nvavp_probe,
+	.remove		= tegra_nvavp_remove,
+#ifdef CONFIG_PM
+	.suspend	= tegra_nvavp_suspend,
+	.resume		= tegra_nvavp_resume,
+#endif
+};
+
+static int __init tegra_nvavp_init(void)
+{
+	return nvhost_driver_register(&tegra_nvavp_driver);
+}
+
+static void __exit tegra_nvavp_exit(void)
+{
+	nvhost_driver_unregister(&tegra_nvavp_driver);
+}
+
+module_init(tegra_nvavp_init);
+module_exit(tegra_nvavp_exit);
+
+MODULE_AUTHOR("NVIDIA");
+MODULE_DESCRIPTION("Channel based AVP driver for Tegra");
+MODULE_VERSION("1.0");
+MODULE_LICENSE("Dual BSD/GPL");
diff --git a/drivers/media/video/tegra/nvavp/nvavp_os.h b/drivers/media/video/tegra/nvavp/nvavp_os.h
new file mode 100644
index 000000000000..4d7f6776f110
--- /dev/null
+++ b/drivers/media/video/tegra/nvavp/nvavp_os.h
@@ -0,0 +1,103 @@
+/*
+ * drivers/media/video/tegra/nvavp/nvavp_os.h
+ *
+ * Copyright (C) 2011 NVIDIA Corp.
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+#ifndef __MEDIA_VIDEO_TEGRA_NVAVP_OS_H
+#define __MEDIA_VIDEO_TEGRA_NVAVP_OS_H
+
+#include <linux/types.h>
+
+#include "../../../../video/tegra/nvmap/nvmap.h"
+
+#define NVE2_AVP				(0x0000E276)
+
+struct nv_e276_control {
+	u32 reserved00[5];
+	u32 dma_start;
+	u32 reserved01[2];
+	u32 dma_end;
+	u32 reserved02[7];
+	u32 put;
+	u32 reserved03[15];
+	u32 get;
+	u32 reserved04[10];
+	u32 watchdog_timeout;
+	u32 idle_notify_enable;
+	u32 idle_notify_delay;
+	u32 idle_clk_enable;
+	u32 iram_clk_gating;
+	u32 idle;
+	u32 outbox_data;
+	u32 app_intr_enable;
+	u32 app_start_time;
+	u32 app_in_iram;
+	u32 iram_ucode_addr;
+	u32 iram_ucode_size;
+	u32 dbg_state[57];
+	u32 os_method_data[16];
+	u32 app_method_data[128];
+};
+
+#define NVE26E_HOST1X_INCR_SYNCPT		(0x00000000)
+#define NVE26E_HOST1X_INCR_SYNCPT_COND_OP_DONE	(0x00000001)
+
+#define NVE26E_CH_OPCODE_INCR(Addr, Count) \
+	/* op, addr, count */ \
+	((1UL << 28) | ((Addr) << 16) | (Count))
+
+#define NVE26E_CH_OPCODE_IMM(addr, value) \
+	/* op, addr, count */ \
+	((4UL << 28) | ((addr) << 16) | (value))
+
+#define NVE26E_CH_OPCODE_GATHER(off, ins, type, cnt) \
+	/* op, offset, insert, type, count */ \
+	((6UL << 28) | ((off) << 16) | ((ins) << 15) | ((type) << 14) | cnt)
+
+/* AVP OS methods */
+#define NVE276_NOP				(0x00000080)
+#define NVE276_SET_APP_TIMEOUT			(0x00000084)
+#define NVE276_SET_MICROCODE_A			(0x00000085)
+#define NVE276_SET_MICROCODE_B			(0x00000086)
+#define NVE276_SET_MICROCODE_C			(0x00000087)
+
+/* Interrupt codes through inbox/outbox data codes (cpu->avp or avp->cpu) */
+#define NVE276_OS_INTERRUPT_NOP			(0x00000000) /* wake up avp */
+#define NVE276_OS_INTERRUPT_TIMEOUT		(0x00000001)
+#define NVE276_OS_INTERRUPT_SEMAPHORE_AWAKEN	(0x00000002)
+#define NVE276_OS_INTERRUPT_EXECUTE_AWAKEN	(0x00000004)
+#define NVE276_OS_INTERRUPT_DEBUG_STRING	(0x00000008)
+#define NVE276_OS_INTERRUPT_DH_KEYEXCHANGE	(0x00000010)
+#define NVE276_OS_INTERRUPT_APP_NOTIFY		(0x00000020)
+#define NVE276_OS_INTERRUPT_VIDEO_IDLE		(0x00000040)
+#define NVE276_OS_INTERRUPT_AUDIO_IDLE		(0x00000080)
+#define NVE276_OS_INTERRUPT_AVP_BREAKPOINT	(0x00800000)
+#define NVE276_OS_INTERRUPT_AVP_FATAL_ERROR	(0x01000000)
+
+struct nvavp_os_info {
+	u32			entry_offset;
+	u32			control_offset;
+	u32			debug_offset;
+
+	struct nvmap_handle_ref	*handle;
+	void			*data;
+	u32			size;
+	phys_addr_t		phys;
+	void			*os_bin;
+	phys_addr_t		reset_addr;
+};
+
+struct nvavp_ucode_info {
+	struct nvmap_handle_ref	*handle;
+	void			*data;
+	u32			size;
+	phys_addr_t		phys;
+	void			*ucode_bin;
+};
+
+#endif /* __MEDIA_VIDEO_TEGRA_NVAVP_OS_H */
diff --git a/drivers/media/video/tegra/ov14810.c b/drivers/media/video/tegra/ov14810.c
new file mode 100644
index 000000000000..8a72cd7aae89
--- /dev/null
+++ b/drivers/media/video/tegra/ov14810.c
@@ -0,0 +1,1390 @@
+/*
+ * ov14810.c - ov14810 sensor driver
+ *
+ * Copyright (c) 2011-2012, NVIDIA, All Rights Reserved.
+ *
+ * Contributors:
+ *	  Krupal Divvela <kdivvela@nvidia.com>
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <media/ov14810.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+
+
+#define OV14810_I2C_WRITE8(ADDR, OFFSET, VAL) do { \
+		if (0 != ov14810_write8(ADDR, OFFSET, VAL)) \
+			return 1; \
+	} while(0)
+
+#define OV14810_FRAME_LENGTH_REG_ADDR0 0x380e
+#define OV14810_FRAME_LENGTH_REG_ADDR1 0x380f
+
+#define OV14810_COARSE_TIME_REG_ADDR0 0x3500
+#define OV14810_COARSE_TIME_REG_ADDR1 0x3501
+#define OV14810_COARSE_TIME_REG_ADDR2 0x3502
+
+#define OV14810_GAIN_REG_ADDR0        0x350b
+
+#define OV14810_GROUP_ACCESS_REG_ADDR 0x3212
+
+static u8 uCProgram[] = {
+ 0x02,0x03,0x6E,0x02,0x19,0x74,0xBB,0x01,0x06,0x89,0x82,0x8A,0x83,0xE0,0x22,0x50
+,0x02,0xE7,0x22,0xBB,0xFE,0x02,0xE3,0x22,0x89,0x82,0x8A,0x83,0xE4,0x93,0x22,0xBB
+,0x01,0x0C,0xE5,0x82,0x29,0xF5,0x82,0xE5,0x83,0x3A,0xF5,0x83,0xE0,0x22,0x50,0x06
+,0xE9,0x25,0x82,0xF8,0xE6,0x22,0xBB,0xFE,0x06,0xE9,0x25,0x82,0xF8,0xE2,0x22,0xE5
+,0x82,0x29,0xF5,0x82,0xE5,0x83,0x3A,0xF5,0x83,0xE4,0x93,0x22,0xBB,0x01,0x06,0x89
+,0x82,0x8A,0x83,0xF0,0x22,0x50,0x02,0xF7,0x22,0xBB,0xFE,0x01,0xF3,0x22,0xEF,0x8D
+,0xF0,0xA4,0xA8,0xF0,0xCF,0x8C,0xF0,0xA4,0x28,0xCE,0x8D,0xF0,0xA4,0x2E,0xFE,0x22
+,0xBC,0x00,0x0B,0xBE,0x00,0x29,0xEF,0x8D,0xF0,0x84,0xFF,0xAD,0xF0,0x22,0xE4,0xCC
+,0xF8,0x75,0xF0,0x08,0xEF,0x2F,0xFF,0xEE,0x33,0xFE,0xEC,0x33,0xFC,0xEE,0x9D,0xEC
+,0x98,0x40,0x05,0xFC,0xEE,0x9D,0xFE,0x0F,0xD5,0xF0,0xE9,0xE4,0xCE,0xFD,0x22,0xED
+,0xF8,0xF5,0xF0,0xEE,0x84,0x20,0xD2,0x1C,0xFE,0xAD,0xF0,0x75,0xF0,0x08,0xEF,0x2F
+,0xFF,0xED,0x33,0xFD,0x40,0x07,0x98,0x50,0x06,0xD5,0xF0,0xF2,0x22,0xC3,0x98,0xFD
+,0x0F,0xD5,0xF0,0xEA,0x22,0xC2,0xD5,0xEC,0x30,0xE7,0x09,0xB2,0xD5,0xE4,0xC3,0x9D
+,0xFD,0xE4,0x9C,0xFC,0xEE,0x30,0xE7,0x15,0xB2,0xD5,0xE4,0xC3,0x9F,0xFF,0xE4,0x9E
+,0xFE,0x12,0x00,0x70,0xC3,0xE4,0x9D,0xFD,0xE4,0x9C,0xFC,0x80,0x03,0x12,0x00,0x70
+,0x30,0xD5,0x07,0xC3,0xE4,0x9F,0xFF,0xE4,0x9E,0xFE,0x22,0xC5,0xF0,0xF8,0xA3,0xE0
+,0x28,0xF0,0xC5,0xF0,0xF8,0xE5,0x82,0x15,0x82,0x70,0x02,0x15,0x83,0xE0,0x38,0xF0
+,0x22,0xBB,0x01,0x0A,0x89,0x82,0x8A,0x83,0xE0,0xF5,0xF0,0xA3,0xE0,0x22,0x50,0x06
+,0x87,0xF0,0x09,0xE7,0x19,0x22,0xBB,0xFE,0x07,0xE3,0xF5,0xF0,0x09,0xE3,0x19,0x22
+,0x89,0x82,0x8A,0x83,0xE4,0x93,0xF5,0xF0,0x74,0x01,0x93,0x22,0xBB,0x01,0x10,0xE5
+,0x82,0x29,0xF5,0x82,0xE5,0x83,0x3A,0xF5,0x83,0xE0,0xF5,0xF0,0xA3,0xE0,0x22,0x50
+,0x09,0xE9,0x25,0x82,0xF8,0x86,0xF0,0x08,0xE6,0x22,0xBB,0xFE,0x0A,0xE9,0x25,0x82
+,0xF8,0xE2,0xF5,0xF0,0x08,0xE2,0x22,0xE5,0x83,0x2A,0xF5,0x83,0xE9,0x93,0xF5,0xF0
+,0xA3,0xE9,0x93,0x22,0xE8,0x8F,0xF0,0xA4,0xCC,0x8B,0xF0,0xA4,0x2C,0xFC,0xE9,0x8E
+,0xF0,0xA4,0x2C,0xFC,0x8A,0xF0,0xED,0xA4,0x2C,0xFC,0xEA,0x8E,0xF0,0xA4,0xCD,0xA8
+,0xF0,0x8B,0xF0,0xA4,0x2D,0xCC,0x38,0x25,0xF0,0xFD,0xE9,0x8F,0xF0,0xA4,0x2C,0xCD
+,0x35,0xF0,0xFC,0xEB,0x8E,0xF0,0xA4,0xFE,0xA9,0xF0,0xEB,0x8F,0xF0,0xA4,0xCF,0xC5
+,0xF0,0x2E,0xCD,0x39,0xFE,0xE4,0x3C,0xFC,0xEA,0xA4,0x2D,0xCE,0x35,0xF0,0xFD,0xE4
+,0x3C,0xFC,0x22,0x75,0xF0,0x08,0x75,0x82,0x00,0xEF,0x2F,0xFF,0xEE,0x33,0xFE,0xCD
+,0x33,0xCD,0xCC,0x33,0xCC,0xC5,0x82,0x33,0xC5,0x82,0x9B,0xED,0x9A,0xEC,0x99,0xE5
+,0x82,0x98,0x40,0x0C,0xF5,0x82,0xEE,0x9B,0xFE,0xED,0x9A,0xFD,0xEC,0x99,0xFC,0x0F
+,0xD5,0xF0,0xD6,0xE4,0xCE,0xFB,0xE4,0xCD,0xFA,0xE4,0xCC,0xF9,0xA8,0x82,0x22,0xB8
+,0x00,0xC1,0xB9,0x00,0x59,0xBA,0x00,0x2D,0xEC,0x8B,0xF0,0x84,0xCF,0xCE,0xCD,0xFC
+,0xE5,0xF0,0xCB,0xF9,0x78,0x18,0xEF,0x2F,0xFF,0xEE,0x33,0xFE,0xED,0x33,0xFD,0xEC
+,0x33,0xFC,0xEB,0x33,0xFB,0x10,0xD7,0x03,0x99,0x40,0x04,0xEB,0x99,0xFB,0x0F,0xD8
+,0xE5,0xE4,0xF9,0xFA,0x22,0x78,0x18,0xEF,0x2F,0xFF,0xEE,0x33,0xFE,0xED,0x33,0xFD
+,0xEC,0x33,0xFC,0xC9,0x33,0xC9,0x10,0xD7,0x05,0x9B,0xE9,0x9A,0x40,0x07,0xEC,0x9B
+,0xFC,0xE9,0x9A,0xF9,0x0F,0xD8,0xE0,0xE4,0xC9,0xFA,0xE4,0xCC,0xFB,0x22,0x75,0xF0
+,0x10,0xEF,0x2F,0xFF,0xEE,0x33,0xFE,0xED,0x33,0xFD,0xCC,0x33,0xCC,0xC8,0x33,0xC8
+,0x10,0xD7,0x07,0x9B,0xEC,0x9A,0xE8,0x99,0x40,0x0A,0xED,0x9B,0xFD,0xEC,0x9A,0xFC
+,0xE8,0x99,0xF8,0x0F,0xD5,0xF0,0xDA,0xE4,0xCD,0xFB,0xE4,0xCC,0xFA,0xE4,0xC8,0xF9
+,0x22,0xEB,0x9F,0xF5,0xF0,0xEA,0x9E,0x42,0xF0,0xE9,0x9D,0x42,0xF0,0xE8,0x9C,0x45
+,0xF0,0x22,0xE8,0x60,0x0F,0xEC,0xC3,0x13,0xFC,0xED,0x13,0xFD,0xEE,0x13,0xFE,0xEF
+,0x13,0xFF,0xD8,0xF1,0x22,0xE8,0x60,0x0F,0xEF,0xC3,0x33,0xFF,0xEE,0x33,0xFE,0xED
+,0x33,0xFD,0xEC,0x33,0xFC,0xD8,0xF1,0x22,0xE0,0xFC,0xA3,0xE0,0xFD,0xA3,0xE0,0xFE
+,0xA3,0xE0,0xFF,0x22,0xE0,0xF8,0xA3,0xE0,0xF9,0xA3,0xE0,0xFA,0xA3,0xE0,0xFB,0x22
+,0xEC,0xF0,0xA3,0xED,0xF0,0xA3,0xEE,0xF0,0xA3,0xEF,0xF0,0x22,0xE0,0xFB,0xA3,0xE0
+,0xFA,0xA3,0xE0,0xF9,0x22,0xF8,0xE0,0xFB,0xA3,0xA3,0xE0,0xF9,0x25,0xF0,0xF0,0xE5
+,0x82,0x15,0x82,0x70,0x02,0x15,0x83,0xE0,0xFA,0x38,0xF0,0x22,0xEB,0xF0,0xA3,0xEA
+,0xF0,0xA3,0xE9,0xF0,0x22,0xD0,0x83,0xD0,0x82,0xF8,0xE4,0x93,0x70,0x12,0x74,0x01
+,0x93,0x70,0x0D,0xA3,0xA3,0x93,0xF8,0x74,0x01,0x93,0xF5,0x82,0x88,0x83,0xE4,0x73
+,0x74,0x02,0x93,0x68,0x60,0xEF,0xA3,0xA3,0xA3,0x80,0xDF,0x8A,0x83,0x89,0x82,0xE4
+,0x73,0xEC,0x8E,0xF0,0xA4,0xCC,0xC5,0xF0,0xCC,0xCD,0xF8,0xEF,0xA4,0xCE,0xC5,0xF0
+,0x2D,0xFD,0xE4,0x3C,0xFC,0xE8,0xA4,0x2E,0xC8,0xC5,0xF0,0x3D,0xFD,0xE4,0x3C,0xFC
+,0xEF,0xA4,0xFF,0xE5,0xF0,0x28,0xFE,0xE4,0x3D,0xFD,0xE4,0x3C,0xFC,0x22,0x78,0x7F
+,0xE4,0xF6,0xD8,0xFD,0x75,0x81,0x7F,0x02,0x03,0xB5,0x02,0x17,0x7F,0xE4,0x93,0xA3
+,0xF8,0xE4,0x93,0xA3,0x40,0x03,0xF6,0x80,0x01,0xF2,0x08,0xDF,0xF4,0x80,0x29,0xE4
+,0x93,0xA3,0xF8,0x54,0x07,0x24,0x0C,0xC8,0xC3,0x33,0xC4,0x54,0x0F,0x44,0x20,0xC8
+,0x83,0x40,0x04,0xF4,0x56,0x80,0x01,0x46,0xF6,0xDF,0xE4,0x80,0x0B,0x01,0x02,0x04
+,0x08,0x10,0x20,0x40,0x80,0x90,0x03,0xFA,0xE4,0x7E,0x01,0x93,0x60,0xBC,0xA3,0xFF
+,0x54,0x3F,0x30,0xE5,0x09,0x54,0x1F,0xFE,0xE4,0x93,0xA3,0x60,0x01,0x0E,0xCF,0x54
+,0xC0,0x25,0xE0,0x60,0xA8,0x40,0xB8,0xE4,0x93,0xA3,0xFA,0xE4,0x93,0xA3,0xF8,0xE4
+,0x93,0xA3,0xC8,0xC5,0x82,0xC8,0xCA,0xC5,0x83,0xCA,0xF0,0xA3,0xC8,0xC5,0x82,0xC8
+,0xCA,0xC5,0x83,0xCA,0xDF,0xE9,0xDE,0xE7,0x80,0xBE,0x41,0x1B,0x5F,0x00,0x60,0x26
+,0x1B,0x23,0x07,0x83,0x07,0xE6,0x08,0x0D,0x1F,0x5D,0x1F,0x98,0x07,0x5F,0x04,0x73
+,0xFC,0x18,0x03,0xE8,0xFF,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x01,0x00,0x0B
+,0x00,0x01,0x00,0x04,0x00,0xA0,0x00,0x64,0xC1,0x45,0xC1,0x46,0xC1,0x48,0xC1,0x47
+,0x00,0xE4,0xFD,0xFC,0xC2,0x8D,0xC2,0xA9,0xD2,0x8C,0xED,0x6F,0x70,0x02,0xEC,0x6E
+,0x60,0x0C,0x30,0x8D,0xFD,0x0D,0xBD,0x00,0x01,0x0C,0xC2,0x8D,0x80,0xEC,0xC2,0x8C
+,0x22,0xC2,0x8C,0xC2,0xA9,0x53,0x89,0xF0,0x43,0x89,0x02,0x75,0x8C,0x9C,0x75,0x8A
+,0x9C,0xC2,0x8D,0x22,0xD0,0xE0,0xD0,0xE0,0xE4,0xC0,0xE0,0xC0,0xE0,0x32,0x22,0x02
+,0xFF,0xFC,0x22,0x42,0x30,0x00,0x47,0x30,0x03,0x47,0x30,0x0C,0x41,0x30,0x18,0x42
+,0x30,0x1B,0x41,0x30,0x1E,0x41,0x30,0x20,0x41,0x31,0x06,0x41,0x35,0x03,0x42,0x36
+,0x00,0x42,0x36,0x03,0x42,0x36,0x0A,0x41,0x36,0x0F,0x41,0x36,0x11,0x41,0x36,0x13
+,0x41,0x36,0x15,0x44,0x37,0x02,0x43,0x37,0x07,0x41,0x37,0x0E,0x42,0x37,0x10,0x42
+,0x37,0x14,0x43,0x37,0x17,0x44,0x37,0x1B,0x45,0x37,0x22,0x44,0x38,0x08,0x41,0x38
+,0x19,0x41,0x3B,0x09,0x41,0x3C,0x01,0x41,0x40,0x00,0x44,0x40,0x02,0x41,0x40,0x09
+,0x41,0x40,0x0C,0x41,0x40,0x4F,0x43,0x43,0x00,0x48,0x47,0x00,0x41,0x47,0x09,0x42
+,0x47,0x0B,0x42,0x48,0x00,0x41,0x48,0x03,0x41,0x48,0x06,0x41,0x48,0x37,0x41,0x48
+,0x42,0x41,0x48,0x4A,0x42,0x50,0x00,0x41,0x50,0x1F,0x41,0x50,0x25,0x42,0x50,0x3B
+,0x41,0x50,0x41,0x41,0x50,0x43,0x41,0x5B,0x01,0x41,0x5B,0x03,0x42,0x38,0x2C,0x41
+,0x01,0x00,0x41,0x32,0x12,0x41,0x30,0x13,0x41,0x36,0x02,0x41,0x36,0x05,0x42,0x36
+,0x0C,0x41,0x36,0x14,0x44,0x37,0x0A,0x41,0x37,0x0F,0x41,0x37,0x13,0x41,0x37,0x16
+,0x41,0x37,0x21,0x42,0x37,0x27,0x45,0x38,0x03,0x46,0x38,0x0C,0x42,0x38,0x17,0x46
+,0x38,0x1C,0x42,0x38,0x2C,0x41,0x40,0x01,0x42,0x40,0x50,0x41,0x40,0x53,0x41,0x50
+,0x02,0x41,0x50,0x3D,0x41,0x50,0x42,0x41,0x50,0x47,0x41,0x59,0x01,0xEF,0xFF,0xFF
+,0x41,0x32,0x12,0x41,0x32,0x12,0x84,0xEE,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x09
+,0x00,0xA7,0xA0,0x08,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x40,0x04,0xE0,0xF8
+,0xF0,0x01,0x05,0x03,0x2D,0x1F,0x20,0x80,0x2E,0x00,0x24,0x6C,0x84,0x13,0x20,0x3D
+,0x28,0xD1,0x73,0x01,0x00,0x04,0x40,0x16,0x5F,0x58,0x80,0x11,0x11,0xA0,0x46,0x40
+,0x2C,0x1A,0x30,0x2E,0x2E,0x70,0x00,0x40,0x00,0xF0,0x80,0x0A,0x80,0x29,0xC5,0x08
+,0x02,0x10,0x40,0x00,0xFF,0xFF,0x00,0xF0,0x04,0x01,0x00,0x00,0x00,0x00,0x00,0x00
+,0x00,0x00,0x00,0x04,0x0F,0x50,0x80,0x1B,0x01,0x00,0x5F,0x4E,0x00,0x10,0x01,0x10
+,0x0E,0x08,0x03,0x00,0x02,0x02,0x00,0x00,0x1F,0x53,0x11,0x42,0x13,0x0F,0x83,0x00
+,0x04,0x81,0x00,0x57,0xB6,0x08,0x66,0x02,0x07,0x02,0x88,0x01,0xE6,0x0F,0xA0,0x01
+,0xF4,0x22,0x02,0x24,0x4A,0x32,0xAC,0x07,0xA4,0x03,0xA0,0x10,0x10,0x00,0xC0,0x00
+,0xA1,0x00,0x00,0x21,0x00,0x00,0xFF,0xFF,0x10,0xA0,0x02,0x88,0x01,0xE6,0xFF,0xFF
+,0xFF,0xFF,0xFF,0x00,0x00,0x09,0x00,0xA7,0xA0,0x08,0x00,0x00,0x00,0x00,0x00,0x00
+,0x00,0x00,0x40,0x04,0xE0,0xF8,0xF0,0x01,0x05,0x03,0x2D,0x1F,0x20,0x80,0x2E,0x00
+,0x24,0x6C,0x84,0x13,0x20,0x3D,0x28,0xD1,0x73,0x01,0x00,0x04,0x40,0x16,0x5F,0x58
+,0x80,0x11,0x11,0xA0,0x46,0x40,0x2C,0x1A,0x30,0x2E,0x2E,0x70,0x00,0x40,0x00,0xF0
+,0x80,0x0A,0x80,0x29,0xC5,0x08,0x02,0x10,0x40,0x00,0xFF,0xFF,0x00,0xF0,0x04,0x01
+,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x04,0x0F,0x50,0x80,0x1B,0x01,0x00
+,0x5F,0x4E,0x00,0x10,0x01,0x10,0x0E,0x08,0x03,0x00,0x02,0x02,0x00,0x00,0x1F,0x42
+,0x11,0x42,0x13,0x30,0x80,0x00,0x84,0x01,0x61,0xF6,0x17,0x08,0x66,0x0C,0x0B,0x11
+,0x40,0x0C,0xF0,0x1F,0x00,0x0D,0x08,0x44,0x96,0x24,0x40,0x30,0x0C,0x0C,0xFC,0x00
+,0x08,0x04,0x04,0x02,0xC0,0x00,0xA1,0x00,0x00,0x21,0x00,0x00,0xFF,0xFF,0x10,0xA0
+,0x11,0x40,0x0C,0xF0,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x09,0x00,0xA7,0xA0,0x08
+,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x40,0x04,0xE0,0xF8,0xF0,0x01,0x05,0x03
+,0x2D,0x1F,0x20,0x80,0x2E,0x00,0x24,0x6C,0x84,0x13,0x20,0x3D,0x28,0xD1,0x73,0x01
+,0x00,0x04,0x40,0x16,0x5F,0x58,0x80,0x11,0x11,0xA0,0x46,0x40,0x2C,0x1A,0x30,0x2E
+,0x2E,0x70,0x00,0x40,0x00,0xF0,0x80,0x0A,0x80,0x29,0xC5,0x08,0x02,0x10,0x40,0x00
+,0xFF,0xFF,0x00,0xF0,0x04,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x04
+,0x0F,0x50,0x80,0x1B,0x01,0x00,0x5F,0x4E,0x00,0x10,0x01,0x10,0x0E,0x08,0x03,0x00
+,0x02,0x02,0x00,0x00,0x1F,0x42,0x11,0x42,0x13,0x30,0x80,0x00,0x84,0x01,0x61,0xF6
+,0x17,0x08,0x66,0x0C,0x0B,0x02,0x88,0x01,0xE6,0x0A,0x40,0x01,0xFC,0x44,0x96,0x24
+,0x40,0x35,0x85,0x01,0xF2,0x07,0x6C,0x04,0x04,0x02,0xC0,0x00,0xA1,0x00,0x00,0x31
+,0x00,0x00,0xFF,0xFF,0x10,0xA0,0x02,0x88,0x01,0xE6,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0x00,0x05,0x6D,0x07,0x18,0x06,0x13,0xFF,0x05,0x06,0xB9,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0x01,0x0D,0x4C,0x02,0x08,0xB9,0x03,0x0F,0xF5,0x05,0x0D,0xC7,0x0A
+,0x14,0x6D,0x0B,0x14,0xB6,0x0C,0x0F,0xD2,0x0D,0x14,0xED,0x0F,0x15,0x59,0x10,0x15
+,0x5D,0x14,0x15,0x70,0x18,0x11,0xAF,0x19,0x12,0x79,0x1F,0x15,0x61,0x20,0x17,0x29
+,0x22,0x14,0x06,0x23,0x14,0x2D,0x29,0x18,0xAF,0x2A,0x10,0x9F,0x39,0x17,0x9F,0x3B
+,0x0E,0xA6,0x3C,0x17,0xEB,0x4A,0x18,0x92,0x4B,0x14,0x88,0x50,0x17,0x42,0x57,0x15
+,0x61,0x59,0x11,0x3D,0x60,0x17,0x65,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x22,0xA2,0xC1,0xCC,0xC1,0xCB,0x25,0x11,0x26,0x88
+,0x60,0x00,0x64,0x45,0x30,0x03,0x41,0x01,0x00,0x60,0x00,0x64,0x41,0x01,0x00,0xEF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x0B,0x02,0xF7
+,0x00,0x00,0x00,0x00,0x00,0x04,0x14,0x28,0x36,0x64,0x04,0x00,0x09,0x00,0x9D,0xC3
+,0x0D,0x01,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0x90,0x1B,0x23,0x12,0x18,0x70,0x90,0x1B,0x68,0x12,0x03,0x0C,0x90,0x1B,0x68,0xF1
+,0xE3,0xFF,0xF4,0x60,0x34,0xEF,0x14,0xB5,0xC5,0x23,0x75,0xC5,0xFE,0xE4,0xFD,0x7F
+,0x8E,0x11,0x89,0x90,0x1B,0x68,0x12,0x02,0xEC,0x90,0x00,0x01,0x12,0x01,0x3C,0xAA
+,0xF0,0xF9,0x12,0x03,0x3B,0x7D,0x04,0x7F,0x8E,0x11,0x89,0x80,0x0F,0x90,0x1B,0x69
+,0xE4,0x75,0xF0,0x03,0x12,0x00,0xFB,0x80,0xC3,0x75,0xC1,0x06,0x75,0xC5,0xFF,0x22
+,0xFD,0x7F,0x20,0x11,0x89,0xE4,0xFD,0x7F,0x12,0x8F,0xFA,0x8D,0xFB,0xE4,0xF5,0xF8
+,0x90,0x1B,0x35,0xE0,0xFE,0xA3,0xE0,0xFF,0x43,0xF8,0x01,0xEF,0x4E,0x70,0x08,0xE5
+,0xF9,0x64,0x0F,0x60,0x13,0x80,0xF8,0xE5,0xF9,0x64,0x0F,0x60,0x0B,0xEF,0x1F,0xAC
+,0x06,0x70,0x01,0x1E,0x4C,0x70,0xF0,0x22,0x22,0xAF,0xC1,0xEF,0x14,0x60,0x15,0x24
+,0xFE,0x60,0x3E,0x24,0x03,0x70,0x63,0xE5,0xC2,0xB4,0x01,0x06,0x12,0x17,0x92,0x02
+,0x04,0x64,0x80,0x56,0xE5,0xC2,0x64,0x01,0x70,0x25,0x31,0x35,0xE4,0xFD,0x7F,0x20
+,0x11,0x89,0x7D,0x03,0x7F,0x12,0x11,0x89,0x7D,0x07,0x7F,0x14,0x11,0x89,0x7D,0x03
+,0x7F,0x13,0x11,0x89,0x75,0xC5,0xFF,0xE4,0xF5,0xC1,0xFD,0x7F,0x18,0x80,0x8A,0x80
+,0x29,0xE5,0xC2,0x70,0x12,0x31,0x35,0x7F,0x20,0xB1,0x22,0x90,0x1B,0x5F,0xEF,0xF0
+,0xE4,0x11,0x80,0x7D,0x03,0x80,0x0D,0xE5,0xC2,0xB4,0x01,0x0C,0x90,0x1B,0x5F,0xE0
+,0x11,0x80,0xE4,0xFD,0x7F,0x13,0xA1,0xAD,0x80,0x00,0x75,0xC1,0x01,0x22,0xE4,0xFD
+,0xFF,0x7E,0x01,0x80,0x0E,0x31,0x2E,0x7F,0x0A,0x7E,0x00,0x02,0x04,0x31,0xFD,0x7F
+,0x02,0x7E,0x35,0xAB,0x07,0xAA,0x06,0xEA,0xF5,0xFA,0xEB,0xF5,0xFB,0x8D,0xFC,0x75
+,0xF8,0x10,0x01,0x90,0xE4,0xFD,0x7F,0x12,0x7E,0x32,0x31,0x43,0xAD,0x43,0xAC,0x42
+,0x7F,0x00,0x7E,0x35,0x31,0x84,0xE5,0x45,0x31,0x3E,0xAD,0x49,0xAC,0x48,0x7F,0x0A
+,0x7E,0x35,0x31,0x84,0x7D,0x10,0x7F,0x12,0x7E,0x32,0x31,0x43,0x7D,0xA0,0x7F,0x12
+,0x7E,0x32,0x80,0xBF,0x8E,0x31,0x8F,0x32,0x8C,0x33,0x8D,0x34,0xE5,0x33,0xFD,0x31
+,0x43,0xE5,0x32,0x24,0x01,0xFF,0xE4,0x35,0x31,0xFE,0xAD,0x34,0x80,0xA5,0x8E,0x2F
+,0x8F,0x30,0x90,0x1B,0x10,0xE0,0xFF,0x90,0x1B,0x0F,0xE0,0xFD,0x12,0x18,0x35,0xAA
+,0x06,0xA9,0x07,0x7B,0xFF,0x90,0x1B,0x4E,0x12,0x03,0x0C,0x90,0x00,0x80,0x12,0x00
+,0x1F,0xFE,0x90,0x00,0x81,0x12,0x00,0x1F,0x7C,0x00,0x90,0x1B,0x5B,0xB1,0x18,0x90
+,0x00,0x82,0x12,0x00,0x1F,0xFE,0x90,0x00,0x83,0x12,0x00,0x1F,0x90,0x1B,0x5D,0xB1
+,0x18,0xB1,0x05,0x7C,0x41,0x7D,0x1A,0x12,0x03,0x41,0x7B,0x00,0x7A,0x71,0x79,0x02
+,0x78,0x00,0x12,0x01,0xFF,0x90,0x1B,0x51,0xEE,0xF0,0xA3,0xEF,0xF0,0xAE,0x2F,0xAF
+,0x30,0x7C,0x27,0x7D,0x10,0x12,0x03,0x41,0xC0,0x06,0xC0,0x07,0x90,0x1B,0x51,0xE0
+,0xFE,0xA3,0xE0,0xFB,0xAA,0x06,0xE4,0xF9,0xF8,0xD0,0x07,0xD0,0x06,0x12,0x01,0xFF
+,0x90,0x1B,0x53,0x12,0x02,0xE0,0xB1,0x05,0xE4,0xFC,0xFD,0x90,0x1B,0x53,0x12,0x02
+,0xD4,0x12,0x01,0x74,0xE4,0x7B,0x20,0xFA,0xF9,0xF8,0x12,0x01,0x74,0xE4,0x7B,0xE0
+,0x7A,0x2E,0xF9,0xF8,0x12,0x01,0xFF,0x90,0x1B,0x57,0x12,0x02,0xE0,0x90,0x1B,0x57
+,0x12,0x02,0xC8,0x78,0x08,0x12,0x02,0xA2,0x90,0x1B,0x11,0xEE,0xF0,0xA3,0xEF,0xF0
+,0x90,0x1B,0x57,0x12,0x02,0xC8,0xE4,0x90,0x1B,0x13,0xEF,0xF0,0x7F,0x18,0x7E,0x38
+,0xB1,0x3A,0xEF,0x30,0xE0,0x0D,0x51,0xD8,0x78,0x01,0x12,0x02,0xA2,0x90,0x1B,0x53
+,0x12,0x02,0xE0,0x51,0xD8,0xEF,0x54,0xF0,0xFF,0xE4,0xF5,0x44,0x8F,0x45,0x51,0xD8
+,0x78,0x08,0x12,0x02,0xA2,0xE4,0x8E,0x42,0x8F,0x43,0x51,0xD8,0x78,0x04,0x12,0x02
+,0xA2,0xF1,0x35,0x90,0x1B,0x5E,0xE0,0x24,0xF8,0xFF,0x90,0x1B,0x5D,0xE0,0x34,0xFF
+,0xFE,0xE4,0xFC,0xFD,0xD3,0x12,0x02,0x91,0x40,0x10,0x51,0xD8,0x78,0x04,0x12,0x02
+,0xA2,0xEF,0x24,0x08,0xFD,0xE4,0x3E,0xFC,0x80,0x08,0x90,0x1B,0x5D,0xE0,0xFC,0xA3
+,0xE0,0xFD,0x7F,0x0E,0x7E,0x38,0x21,0x84,0x90,0x1B,0x53,0x02,0x02,0xC8,0x90,0x1B
+,0x2F,0xE0,0xFE,0xA3,0xE0,0xFF,0x90,0x1B,0x49,0x12,0x02,0xEC,0xAC,0x02,0xAD,0x01
+,0x8E,0x33,0x8F,0x34,0x8C,0x35,0x8D,0x36,0x8F,0x82,0x8E,0x83,0xE4,0x93,0x91,0xFC
+,0x70,0x02,0x05,0x33,0x90,0x1B,0x4C,0xE0,0xFF,0xF4,0x70,0x02,0x81,0x19,0xEF,0x54
+,0xE0,0xFB,0x70,0x24,0xE0,0x54,0x1F,0xB1,0x0E,0x05,0x34,0xE5,0x34,0x70,0x02,0x05
+,0x33,0xF5,0x82,0x85,0x33,0x83,0xE4,0x93,0x90,0x1B,0x4C,0xF0,0x60,0xEB,0x05,0x34
+,0xE5,0x34,0x70,0x02,0x05,0x33,0x80,0xCC,0x90,0x1B,0x4C,0xE0,0x54,0x1F,0xA3,0xF0
+,0x90,0x1B,0x4C,0xEB,0xF0,0x64,0x20,0x60,0x0A,0xE0,0xFF,0x64,0x80,0x60,0x04,0xEF
+,0xB4,0xC0,0x15,0x91,0x2A,0xFF,0x90,0x1B,0x4E,0xE4,0xF0,0xA3,0xEF,0xF0,0x05,0x34
+,0xE5,0x34,0x70,0x02,0x05,0x33,0x80,0x19,0x91,0x2A,0xFF,0x74,0x01,0x93,0x90,0x1B
+,0x4E,0xCF,0xF0,0xA3,0xEF,0xF0,0x74,0x02,0x25,0x34,0xF5,0x34,0xE4,0x35,0x33,0xF5
+,0x33,0x90,0x1B,0x4C,0xE0,0xB4,0x60,0x08,0x91,0x33,0xFF,0x12,0x04,0x31,0x80,0x7F
+,0x90,0x1B,0x4C,0xE0,0xB4,0xE0,0x21,0xA3,0xE0,0xB4,0x02,0x16,0xAA,0x35,0xA9,0x36
+,0x7B,0xFF,0x12,0x01,0x11,0x85,0xF0,0x35,0xF5,0x36,0x91,0x33,0x8E,0x33,0xF5,0x34
+,0x80,0x5D,0x74,0x02,0xB1,0x0E,0x80,0x57,0x90,0x1B,0x4D,0xE0,0xD3,0x94,0x00,0x40
+,0x4E,0x90,0x1B,0x4C,0xE0,0xB4,0xC0,0x07,0x91,0x1A,0x12,0x18,0x7A,0x80,0x26,0x90
+,0x1B,0x4C,0xE0,0xB4,0x80,0x09,0x91,0x1A,0x7B,0x01,0x12,0x15,0x27,0x80,0x16,0x90
+,0x1B,0x4C,0xE0,0x90,0x1B,0x4E,0xB4,0x40,0x08,0xE0,0xFE,0x91,0x1D,0x31,0x43,0x80
+,0x04,0x91,0x1D,0x11,0x89,0x05,0x36,0xE5,0x36,0x70,0x02,0x05,0x35,0x90,0x1B,0x4E
+,0xE4,0x75,0xF0,0x01,0x12,0x00,0xFB,0x90,0x1B,0x4D,0xE0,0x14,0xF0,0x80,0xA9,0x91
+,0x2A,0x91,0xFC,0x70,0x02,0x05,0x33,0x61,0x04,0x22,0x90,0x1B,0x4E,0xA3,0xE0,0xFF
+,0x85,0x36,0x82,0x85,0x35,0x83,0xE4,0x93,0xFD,0x22,0x85,0x34,0x82,0x85,0x33,0x83
+,0xE4,0x93,0x22,0x90,0x1B,0x4E,0xE0,0xFE,0xA3,0xE0,0x22,0xE4,0x90,0x1B,0x4E,0xF0
+,0xA3,0xF0,0x30,0x47,0x05,0x90,0x1B,0x01,0x80,0x03,0x90,0x1B,0x45,0xE0,0xFA,0xA3
+,0xE0,0xFB,0x90,0x1B,0x44,0xE0,0x2B,0xFE,0x90,0x1B,0x43,0xE0,0x3A,0x90,0x1B,0x50
+,0xF0,0xA3,0xCE,0xF0,0xA3,0xEA,0xF0,0xA3,0xEB,0xF0,0x30,0x48,0x6B,0xD2,0x45,0x30
+,0x47,0x53,0x91,0xF3,0x90,0x1B,0x43,0xE0,0xFE,0xA3,0xE0,0xFF,0xD3,0x94,0x00,0xEE
+,0x94,0x00,0x40,0x0A,0xEF,0x24,0x06,0xFF,0xE4,0x3E,0xFE,0x12,0x04,0x31,0x91,0x33
+,0xFF,0x90,0x1B,0x52,0xD1,0x25,0x40,0x54,0xB1,0x2C,0xC3,0x90,0x1B,0x38,0xE0,0x9F
+,0x90,0x1B,0x37,0xE0,0x9E,0x50,0x45,0x90,0x1B,0x39,0xA3,0xE0,0xFF,0xBF,0xFF,0x02
+,0x80,0x22,0x90,0x1B,0x37,0xF1,0xBE,0x90,0x1B,0x4D,0xE0,0x9F,0xFF,0x90,0x1B,0x4C
+,0xE0,0x9E,0xFE,0x80,0x24,0x91,0xF3,0x91,0x33,0xFF,0xA3,0xD1,0x25,0x40,0x1D,0xB1
+,0x2C,0x4E,0x60,0x18,0xF1,0xB2,0x80,0x11,0x91,0xF3,0x90,0x1B,0x50,0xE0,0xFE,0xA3
+,0xE0,0xFF,0x7C,0x00,0x7D,0x05,0x12,0x00,0x5E,0x12,0x04,0x31,0x31,0x37,0x53,0xCB
+,0xFB,0x21,0x2E,0xF1,0xAA,0x30,0x48,0x03,0x43,0xCB,0x04,0x22,0x90,0x1B,0x4C,0xF0
+,0x05,0x34,0xE5,0x34,0x22,0x90,0x1B,0x5B,0xE0,0xFE,0xA3,0xE0,0xFF,0x22,0x25,0x36
+,0xF5,0x36,0xE4,0x35,0x35,0xF5,0x35,0x22,0x24,0x00,0xFF,0xEC,0x3E,0xF0,0xA3,0xEF
+,0xF0,0x22,0x8F,0xFA,0x75,0xF8,0x02,0x11,0x90,0xAF,0xFB,0x22,0xED,0x9F,0xFF,0xEC
+,0x9E,0xFE,0x90,0x1B,0x4C,0xF0,0xA3,0xEF,0xF0,0x22,0xAD,0x07,0xAC,0x06,0xEC,0xF5
+,0xFA,0xED,0xF5,0xFB,0x75,0xF8,0x12,0x11,0x90,0xAF,0xFB,0x22,0x12,0x04,0x51,0x90
+,0x1B,0x25,0xF1,0xC5,0x90,0x1F,0xF8,0xE4,0x93,0xF4,0x70,0x4D,0x7F,0x05,0x7E,0x3D
+,0xB1,0x3A,0xEF,0x60,0x44,0x75,0x2F,0x3D,0x75,0x30,0x0E,0x75,0x31,0x1F,0x75,0x32
+,0x83,0xE4,0x90,0x1B,0x49,0xF0,0xB1,0xB3,0x70,0x02,0x05,0x31,0x05,0x30,0xE5,0x30
+,0x70,0x02,0x05,0x2F,0xD1,0x11,0x94,0x04,0x40,0xEC,0x75,0x31,0x1F,0x75,0x32,0xB8
+,0xE4,0x90,0x1B,0x49,0xF0,0xB1,0xB3,0x70,0x02,0x05,0x31,0x05,0x30,0xE5,0x30,0x70
+,0x02,0x05,0x2F,0xD1,0x11,0x94,0x1E,0x40,0xEC,0x7D,0x04,0x7F,0x8E,0x11,0x89,0xE4
+,0xF5,0xC1,0x22,0xAF,0x30,0xAE,0x2F,0xB1,0x3A,0xEF,0xF4,0x85,0x32,0x82,0x85,0x31
+,0x83,0xF0,0x05,0x32,0xE5,0x32,0x22,0x7F,0x03,0x7E,0x30,0xB1,0x3A,0x8F,0x2F,0x7F
+,0x06,0x7E,0x30,0xB1,0x3A,0x8F,0x30,0xAF,0xC1,0xEF,0x24,0xFC,0x60,0x12,0x24,0x02
+,0x70,0x1C,0x53,0x2F,0xFC,0x43,0x2F,0x01,0x53,0x30,0xDF,0x43,0x30,0x20,0x80,0x09
+,0x53,0x2F,0xFC,0x43,0x2F,0x02,0x53,0x30,0xDF,0xE4,0xF5,0xC1,0x80,0x03,0x75,0xC1
+,0x01,0xAD,0x2F,0x7F,0x03,0x7E,0x30,0x31,0x43,0xAD,0x30,0x7F,0x06,0x7E,0x30,0x21
+,0x43,0x90,0x1B,0x49,0xE0,0x04,0xF0,0xE0,0xC3,0x22,0x90,0x1B,0x41,0xE0,0xFE,0xA3
+,0xE0,0xFF,0x90,0x1B,0x3F,0xE0,0xFC,0xA3,0xE0,0xFD,0xD3,0x9F,0xEC,0x9E,0x22,0x30
+,0x47,0x22,0xD1,0x1A,0x40,0x11,0xED,0x9F,0xFF,0xEC,0x9E,0xFE,0x90,0x1B,0x02,0xE0
+,0x90,0x1B,0x01,0xF1,0x15,0x80,0x30,0xF1,0x0C,0x90,0x1B,0x02,0xE0,0x2F,0xFF,0x90
+,0x1B,0x01,0x80,0x23,0x30,0x48,0x27,0xD1,0x1A,0x40,0x11,0xED,0x9F,0xFF,0xEC,0x9E
+,0xFE,0x90,0x1B,0x46,0xE0,0x90,0x1B,0x45,0xF1,0x15,0x80,0x0B,0xF1,0x0C,0x90,0x1B
+,0x46,0xE0,0x2F,0xFF,0x90,0x1B,0x45,0xE0,0x3E,0x90,0x1B,0x4C,0x80,0x0B,0x90,0x1B
+,0x45,0xE0,0xFF,0xA3,0xE0,0x90,0x1B,0x4C,0xCF,0xF0,0xA3,0xEF,0xF0,0xF1,0xB2,0x31
+,0x9E,0x30,0x47,0x05,0x90,0x1B,0x03,0x80,0x03,0x90,0x1B,0x47,0xE0,0xFE,0xA3,0xE0
+,0xFF,0x12,0x15,0xB2,0x21,0x54,0x7D,0x01,0xAF,0xC1,0x12,0x18,0x35,0x12,0x19,0xB7
+,0x70,0x06,0xE9,0xF4,0x70,0x02,0xEA,0xF4,0x70,0x04,0x75,0xC1,0x01,0x22,0xD2,0x46
+,0x12,0x19,0x3B,0x90,0x1B,0x0F,0xE0,0x70,0x06,0x31,0x2E,0xF1,0x5C,0x51,0xDE,0x90
+,0x1B,0x0F,0x74,0x01,0xF0,0xA3,0xE5,0xC1,0xF0,0x30,0x47,0x05,0x90,0x1B,0x01,0x80
+,0x03,0x90,0x1B,0x45,0xE0,0xFF,0xA3,0xE0,0x90,0x1B,0x17,0xCF,0xF0,0xA3,0xEF,0xF0
+,0xD1,0x2F,0xE4,0x90,0x1B,0x19,0xF0,0x90,0x1B,0x14,0xE0,0xFF,0xA3,0xE0,0x90,0x1B
+,0x1A,0xCF,0xF0,0xA3,0xEF,0xF0,0x91,0x3B,0xE4,0xF5,0xC1,0x22,0x90,0x1B,0x3D,0xE0
+,0xFE,0xA3,0xE0,0xFF,0x22,0x2F,0xFF,0xE0,0x3E,0xFE,0x90,0x1B,0x3E,0xE0,0x2F,0xFF
+,0x90,0x1B,0x3D,0x22,0xE4,0x33,0x24,0x01,0xFF,0xE4,0x33,0xFE,0xE4,0x33,0xFD,0xE4
+,0x33,0xFC,0x12,0x01,0x74,0xA8,0x04,0xA9,0x05,0xAA,0x06,0xAB,0x07,0x22,0xAB,0x07
+,0xAA,0x06,0xB1,0x3A,0x90,0x1B,0x4C,0xEF,0xF0,0xEB,0x24,0x01,0xFF,0xE4,0x3A,0xFE
+,0xB1,0x3A,0xEF,0xFD,0x90,0x1B,0x4C,0xE0,0xFE,0xED,0xFF,0x22,0x7F,0x00,0x7E,0x35
+,0xF1,0x3E,0x90,0x1B,0x1D,0xEE,0xF0,0xA3,0xEF,0xF0,0x7F,0x02,0x7E,0x35,0xB1,0x3A
+,0x90,0x1B,0x1F,0xE4,0xF0,0xA3,0xEF,0xF0,0x7F,0x0A,0x7E,0x35,0xF1,0x3E,0x90,0x1B
+,0x21,0xEE,0xF0,0xA3,0xEF,0xF0,0x22,0x90,0x1B,0x1D,0xE0,0xFC,0xA3,0xE0,0xFD,0x7F
+,0x00,0x7E,0x35,0x31,0x84,0x90,0x1B,0x1F,0xA3,0xE0,0x31,0x3E,0x90,0x1B,0x21,0xE0
+,0xFC,0xA3,0xE0,0xFD,0x7F,0x0A,0x7E,0x35,0x21,0x84,0x7D,0x01,0x7F,0x00,0x7E,0x01
+,0x21,0x43,0x90,0x1B,0x4C,0xE0,0xFE,0xA3,0xE0,0xFF,0x22,0x90,0x1B,0x3B,0xE0,0xFE
+,0xA3,0xE0,0xFF,0xC3,0x22,0xE0,0xFE,0xA3,0xE0,0xFF,0xA3,0xE0,0xFC,0xA3,0xE0,0xFD
+,0x41,0xF0,0xE5,0xC1,0xC3,0x94,0xC1,0x50,0x06,0xAD,0xC4,0xAF,0xC1,0xA1,0xAD,0x75
+,0xC1,0x01,0x22,0x12,0x02,0xEC,0x02,0x00,0x06,0x7F,0x0A,0x7E,0x30,0xE1,0x3E,0x7F
+,0x2A,0x7E,0x30,0xA1,0x3A,0x90,0x1B,0x4B,0xE5,0xC1,0xF0,0xE4,0xF5,0xC1,0xE0,0x12
+,0x03,0x15,0x10,0x21,0x00,0x10,0x2B,0x01,0x10,0x40,0x02,0x10,0x52,0x03,0x10,0x5C
+,0x04,0x10,0x63,0x05,0x10,0x6A,0x07,0x10,0x7B,0x08,0x10,0x7F,0x09,0x00,0x00,0x10
+,0x88,0x75,0xC2,0x4B,0xE4,0xF5,0xC3,0x75,0xC4,0x01,0x22,0x12,0x0F,0xE9,0x11,0x8C
+,0x12,0x0F,0xEF,0x8F,0xC2,0x90,0x1B,0x49,0x11,0x98,0xF5,0xC3,0xED,0xF5,0xC4,0x22
+,0x75,0xC2,0x08,0xE4,0xFF,0x12,0x0D,0x22,0x8F,0xC3,0x7F,0x01,0x12,0x0D,0x22,0x8F
+,0xC4,0x22,0xE4,0xF5,0xC2,0x75,0xC3,0x2C,0x75,0xC4,0x38,0x22,0x75,0xC2,0x07,0xE4
+,0xF5,0xC3,0x22,0x75,0xC2,0x5D,0x75,0xC3,0xC0,0x22,0x7F,0x02,0x71,0x44,0x11,0x8C
+,0x90,0x1B,0x49,0x11,0x98,0xF5,0xC2,0xED,0xF5,0xC3,0x22,0x75,0xC2,0x0A,0x22,0x31
+,0x36,0x8F,0xC2,0x31,0x29,0x8F,0xC3,0x22,0x75,0xC1,0x01,0x22,0x90,0x1B,0x49,0xEE
+,0xF0,0xA3,0xEF,0xF0,0x22,0x90,0x1B,0x11,0xE0,0xFC,0xA3,0xE0,0xFD,0xEC,0x22,0xE4
+,0x90,0x1B,0x49,0xF0,0xA3,0xF0,0xA3,0xF0,0xA3,0xF0,0xAF,0xC1,0xEF,0x12,0x03,0x15
+,0x10,0xC9,0x00,0x10,0xD2,0x01,0x10,0xD7,0x05,0x10,0xDC,0x10,0x10,0xED,0x11,0x10
+,0xF6,0x15,0x11,0x00,0x20,0x00,0x00,0x11,0x08,0x11,0x95,0x31,0x21,0x90,0x1B,0x13
+,0x80,0x14,0x90,0x1B,0x14,0x80,0x19,0x90,0x1B,0x16,0x80,0x1D,0x90,0x1B,0x17,0x11
+,0x98,0x31,0x21,0x90,0x1B,0x19,0xE0,0x90,0x1B,0x4C,0xF0,0x80,0x21,0x90,0x1B,0x1A
+,0x11,0x98,0x31,0x21,0x80,0x18,0x90,0x1B,0x1C,0xE0,0x90,0x1B,0x4A,0xF0,0x80,0x0E
+,0x90,0x1B,0x49,0x74,0x04,0xF0,0x80,0x06,0x90,0x1B,0x49,0x74,0x01,0xF0,0x90,0x1B
+,0x49,0xE0,0xF5,0xC1,0xA3,0xE0,0xF5,0xC2,0xA3,0xE0,0xF5,0xC3,0xA3,0xE0,0xF5,0xC4
+,0x22,0x90,0x1B,0x4A,0xF0,0xED,0xA3,0xF0,0x22,0x7F,0x01,0x8F,0xFA,0x75,0xF8,0x22
+,0x12,0x08,0x90,0xAF,0xFB,0x22,0xE4,0xFF,0x31,0x2B,0x7E,0x00,0x22,0xE4,0xF5,0x2F
+,0xAF,0xC1,0xEF,0x14,0x60,0x11,0x14,0x60,0x18,0x14,0x60,0x15,0x14,0x60,0x17,0x24
+,0x04,0x70,0x1F,0x31,0x8B,0x80,0x1E,0x90,0x1B,0x64,0x11,0x98,0xFF,0x8F,0xC3,0x80
+,0x0C,0x75,0x2F,0x04,0x80,0x0F,0x90,0x1B,0x66,0x11,0x98,0xF5,0xC3,0xED,0xF5,0xC4
+,0x80,0x03,0x75,0x2F,0x01,0x85,0x2F,0xC1,0x22,0xE0,0x54,0x1F,0xFC,0xA3,0xE0,0xFD
+,0x7F,0x50,0x51,0x62,0x7F,0xE8,0x7E,0x03,0x12,0x04,0x31,0x71,0x3C,0xC3,0x33,0xCE
+,0x33,0xCE,0xD8,0xF9,0xFF,0x7C,0x00,0x7D,0x08,0x12,0x00,0xC5,0x90,0x1B,0x64,0xEE
+,0xF0,0xA3,0xEF,0xF0,0x71,0xAA,0x90,0x1B,0x66,0xEE,0xF0,0xA3,0xEF,0xF0,0x22,0x90
+,0x1B,0x2B,0xE0,0xFE,0xA3,0xE0,0x8E,0x2F,0xF5,0x30,0x8E,0x31,0xF5,0x32,0x90,0x1B
+,0x29,0x12,0x0F,0xC5,0xAF,0xC1,0xEF,0x24,0xFD,0x60,0x0B,0x04,0x70,0x10,0x74,0x36
+,0x51,0x58,0x74,0x38,0x80,0x0E,0x74,0x3A,0x51,0x58,0x74,0x3C,0x80,0x06,0x74,0x32
+,0x51,0x58,0x74,0x34,0x25,0x32,0xF5,0x32,0xE4,0x35,0x31,0xF5,0x31,0xE5,0xC1,0x60
+,0x3A,0x85,0x30,0x82,0x85,0x2F,0x83,0xE4,0x93,0xFE,0x05,0x30,0xE5,0x30,0x70,0x02
+,0x05,0x2F,0xF5,0x82,0x85,0x2F,0x83,0xE4,0x93,0x90,0x1B,0x31,0x51,0x70,0x85,0x32
+,0x82,0x85,0x31,0x83,0xE4,0x93,0xFE,0x05,0x32,0xE5,0x32,0x70,0x02,0x05,0x31,0xF5
+,0x82,0x85,0x31,0x83,0xE4,0x93,0x90,0x1B,0x33,0x51,0x70,0x90,0x1B,0x31,0x31,0x79
+,0x90,0x1B,0x33,0x31,0x79,0x90,0x1B,0x31,0xE0,0xF5,0x3E,0xA3,0xE0,0xF5,0x3F,0xA3
+,0xE0,0xF5,0x40,0xA3,0xE0,0xF5,0x41,0xE4,0xF5,0x3A,0xF5,0x3B,0x75,0xC1,0x01,0x75
+,0xC2,0x44,0x51,0x79,0xE4,0xF5,0xC1,0x22,0x25,0x30,0xF5,0x30,0xE4,0x35,0x2F,0xF5
+,0x2F,0x22,0x8F,0xFA,0xED,0xF5,0xFB,0xEC,0xF5,0xFC,0x75,0xF8,0x04,0x02,0x08,0x90
+,0xFD,0xED,0xFF,0xEE,0xF0,0xA3,0xEF,0xF0,0x22,0xE4,0xF5,0x33,0xAF,0xC1,0xEF,0xAD
+,0xC2,0xF5,0x3A,0xED,0xF5,0x3B,0xD3,0x94,0x00,0xE5,0x3A,0x94,0x02,0x40,0x09,0x75
+,0x33,0x01,0x75,0x3A,0x02,0x75,0x3B,0x00,0xC3,0xE5,0x3A,0x94,0x00,0x50,0x08,0x75
+,0x33,0x01,0xE4,0xF5,0x3A,0xF5,0x3B,0xAF,0x3B,0xAE,0x3A,0x71,0x6A,0x8E,0x3C,0x8F
+,0x3D,0x53,0xCB,0xF7,0xE5,0x3C,0x54,0x1F,0xFC,0xAD,0x3D,0x7F,0x50,0x51,0x62,0x75
+,0x34,0x00,0x75,0x35,0xFA,0x7F,0x05,0x7E,0x00,0x12,0x04,0x31,0x85,0x3C,0x36,0x85
+,0x3D,0x37,0x71,0x3C,0xC3,0x33,0xCE,0x33,0xCE,0xD8,0xF9,0x78,0x03,0xCE,0xA2,0xE7
+,0x13,0xCE,0x13,0xD8,0xF8,0xFF,0xC3,0xE5,0x37,0x9F,0xF5,0x37,0xE5,0x36,0x9E,0xF5
+,0x36,0xC3,0x64,0x80,0x94,0x80,0x50,0x0F,0xAE,0x36,0xAF,0x37,0x7C,0xFF,0x7D,0xFF
+,0x12,0x00,0x5E,0x8E,0x36,0x8F,0x37,0xE5,0x35,0x15,0x35,0x70,0x02,0x15,0x34,0xC3
+,0xE5,0x37,0x94,0x14,0xE5,0x36,0x64,0x80,0x94,0x80,0x40,0x06,0xE5,0x35,0x45,0x34
+,0x70,0xA3,0xE5,0x35,0x45,0x34,0x70,0x03,0x75,0x33,0x04,0x71,0x55,0xE4,0xF5,0xC1
+,0x85,0x33,0xC2,0xE5,0x34,0xF5,0xC3,0xE5,0x35,0xF5,0xC4,0x22,0x7F,0x5A,0x71,0x44
+,0xEF,0x78,0x03,0x22,0x8F,0xFA,0x75,0xF8,0x06,0x12,0x08,0x90,0xAF,0xFC,0xEF,0xFE
+,0xAD,0xFB,0xED,0xFF,0x22,0xE5,0x3C,0x54,0x1F,0xFE,0xE4,0x25,0x3D,0xFD,0xEE,0x34
+,0x20,0xFC,0x7F,0x50,0x51,0x62,0x43,0xCB,0x08,0x22,0x8E,0x38,0x8F,0x39,0x71,0xFB
+,0x78,0x02,0x71,0xF2,0x12,0x0F,0x35,0xAE,0x38,0xAF,0x39,0xE4,0xFC,0xFD,0x12,0x01
+,0x74,0x78,0x09,0x12,0x02,0xA2,0xAD,0x07,0xAC,0x06,0xE5,0x41,0xAE,0x40,0x78,0x02
+,0xC3,0x33,0xCE,0x33,0xCE,0xD8,0xF9,0xC3,0x9D,0xFF,0xEE,0x9C,0xFE,0xEF,0x78,0x02
+,0xCE,0xA2,0xE7,0x13,0xCE,0x13,0xD8,0xF8,0xFF,0x22,0xD3,0xEF,0x95,0x41,0xE5,0x40
+,0x64,0x80,0xF8,0xEE,0x64,0x80,0x98,0x40,0x04,0xE4,0xFE,0xFF,0x22,0xC3,0xEF,0x95
+,0x3F,0xE5,0x3E,0x64,0x80,0xF8,0xEE,0x64,0x80,0x98,0x50,0x05,0x7E,0x02,0x7F,0x00
+,0x22,0xC3,0xE5,0x41,0x9F,0xFF,0xE5,0x40,0x9E,0x78,0x09,0x71,0xF2,0xC0,0x06,0xC0
+,0x07,0x71,0xFB,0xAB,0x07,0xFA,0x33,0x95,0xE0,0xF9,0xF8,0xD0,0x07,0xD0,0x06,0x02
+,0x01,0xFF,0xFE,0x33,0x95,0xE0,0xFD,0xFC,0x02,0x02,0xB5,0xC3,0xE5,0x41,0x95,0x3F
+,0xFF,0xE5,0x40,0x95,0x3E,0x22,0x91,0x55,0x4E,0x70,0x05,0x75,0xC1,0x01,0x80,0x05
+,0x90,0x1B,0x45,0x91,0x21,0x11,0x95,0xF5,0xC2,0xED,0xF5,0xC3,0xA3,0xE0,0xF5,0xC4
+,0x22,0xEE,0xF0,0xA3,0xEF,0xF0,0x12,0x0E,0x2F,0xE4,0xF5,0xC1,0x22,0x91,0x55,0xD3
+,0x94,0x80,0xEE,0x94,0x0C,0x50,0x09,0xC3,0xEF,0x94,0x32,0xEE,0x94,0x00,0x50,0x05
+,0x75,0xC1,0x01,0x80,0x05,0x90,0x1B,0x47,0x91,0x21,0x90,0x1B,0x14,0x11,0x98,0xF5
+,0xC2,0xED,0xF5,0xC3,0x22,0xAF,0xC1,0xEF,0xFF,0xAD,0xC2,0xED,0x90,0x1B,0x49,0xCF
+,0xF0,0xA3,0xEF,0xF0,0x90,0x1B,0x49,0xE0,0xFE,0xA3,0xE0,0xFF,0x22,0x91,0x55,0xC3
+,0x94,0xFF,0xEE,0x94,0x5F,0x50,0x0A,0xAD,0xC4,0x12,0x09,0x43,0xE4,0xF5,0xC1,0x80
+,0x03,0x75,0xC1,0x01,0xE4,0xF5,0xC2,0x22,0x90,0x1B,0x49,0xE5,0xC2,0xF0,0xE4,0xA3
+,0xF0,0x90,0x1B,0x49,0xE0,0xFF,0xC3,0x94,0xFF,0x50,0x0C,0x31,0x2B,0x90,0x1B,0x4A
+,0xEF,0xF0,0xE4,0xF5,0xC1,0x80,0x03,0x75,0xC1,0x01,0xE4,0xF5,0xC2,0xF5,0xC3,0x90
+,0x1B,0x4A,0xE0,0xF5,0xC4,0x22,0xAF,0xC1,0xEF,0xFF,0xAD,0xC2,0xED,0x90,0x1B,0x49
+,0xCF,0xF0,0xA3,0xEF,0xF0,0xE4,0xA3,0x91,0x63,0xC3,0x94,0xFF,0xEE,0x94,0x5F,0x50
+,0x0D,0x12,0x0D,0x3A,0x90,0x1B,0x4B,0xEF,0xF0,0xE4,0xF5,0xC1,0x80,0x03,0x75,0xC1
+,0x01,0xE4,0xF5,0xC2,0xF5,0xC3,0x90,0x1B,0x4B,0xE0,0xF5,0xC4,0x22,0xE4,0xFF,0xE5
+,0xC1,0xC3,0x94,0xC1,0x50,0x0A,0xAF,0xC1,0x12,0x0D,0x22,0xE4,0xF5,0xC1,0x80,0x03
+,0x75,0xC1,0x01,0x8F,0xC4,0xE4,0xF5,0xC3,0xF5,0xC2,0x22,0x8F,0x2F,0xAB,0x2F,0xAD
+,0xC3,0xAF,0xC2,0xB1,0x27,0x8F,0xC3,0x05,0xC2,0xAB,0x2F,0xAD,0xC4,0xAF,0xC2,0xB1
+,0x27,0x8F,0xC4,0xE4,0xF5,0xC1,0x22,0x8D,0x37,0xAE,0x03,0xEF,0x7C,0x00,0x7B,0x01
+,0x24,0x23,0xF9,0xEC,0x34,0x1B,0xFA,0x90,0x1B,0x50,0x12,0x03,0x0C,0xEE,0x60,0x11
+,0xEF,0xC3,0x94,0x26,0x50,0x0B,0x90,0x1B,0x50,0x12,0x02,0xEC,0xE5,0x37,0x12,0x00
+,0x4C,0x90,0x1B,0x50,0x12,0x0F,0xE3,0xFF,0x22,0xE4,0xFF,0x80,0xAE,0x7F,0x01,0x80
+,0xAA,0xAD,0x3B,0xAC,0x3A,0xE4,0xF5,0xC1,0x8C,0xC3,0xAF,0x05,0xEF,0xF5,0xC4,0x22
+,0xAF,0xC2,0x7E,0x00,0xEF,0x78,0x10,0xC3,0x33,0xCE,0x33,0xCE,0xD8,0xF9,0xFD,0xAC
+,0x06,0xAF,0xC1,0x7E,0x00,0xEF,0x78,0x18,0xC3,0x33,0xCE,0x33,0xCE,0xD8,0xF9,0xB1
+,0xAA,0xAF,0xC3,0xEF,0x4C,0xFE,0xED,0xFF,0xAD,0xC4,0xEF,0x4D,0xFF,0xE4,0xFC,0xFD
+,0x90,0x1B,0x60,0x12,0x02,0xE0,0xE4,0xF5,0xC1,0x22,0xFF,0xEE,0x4C,0xFC,0xEF,0x4D
+,0xFD,0x22,0x8E,0x2F,0x8F,0x30,0xC3,0xE5,0x30,0x94,0x64,0xE5,0x2F,0x94,0x00,0x50
+,0x0C,0xF1,0x20,0xC2,0x44,0xF1,0x17,0x7E,0x40,0x7D,0x06,0x80,0x6D,0xC3,0xE5,0x30
+,0x94,0xC8,0xE5,0x2F,0x94,0x00,0x50,0x0A,0xF1,0x20,0xF1,0x11,0x7E,0x80,0x7D,0x0C
+,0x80,0x58,0xC3,0xE5,0x30,0x94,0x90,0xE5,0x2F,0x94,0x01,0x50,0x0F,0xC2,0x40,0xD2
+,0x41,0xC2,0x42,0xC2,0x43,0xF1,0x11,0xFE,0x7D,0x19,0x80,0x3E,0xC3,0xE5,0x30,0x94
+,0x20,0xE5,0x2F,0x94,0x03,0x50,0x0D,0xD2,0x40,0xC2,0x41,0xC2,0x42,0xF1,0x0F,0xFE
+,0x7D,0x32,0x80,0x26,0xC3,0xE5,0x30,0x94,0x40,0xE5,0x2F,0x94,0x06,0x50,0x09,0xC2
+,0x40,0xF1,0x0B,0xFE,0x7D,0x64,0x80,0x12,0xC3,0xE5,0x30,0x94,0x80,0xE5,0x2F,0x94
+,0x0C,0x50,0x0F,0xD2,0x40,0xF1,0x0B,0xFE,0x7D,0xC8,0xFC,0x12,0x01,0xFF,0x8E,0x31
+,0x8F,0x32,0xC3,0xE4,0x95,0x32,0xF5,0x34,0x74,0x20,0x95,0x31,0xF5,0x33,0xA2,0x41
+,0xE4,0x33,0x24,0x01,0xFB,0xE4,0x33,0xFA,0xE4,0x33,0xF9,0xE4,0x33,0xF8,0xA2,0x40
+,0x12,0x0F,0x24,0xA2,0x42,0x12,0x0F,0x24,0xA2,0x43,0x12,0x0F,0x24,0xA2,0x44,0xE4
+,0x33,0x24,0x01,0xFF,0xE4,0x33,0xFE,0xE4,0x33,0xFD,0xE4,0x33,0xFC,0x12,0x01,0x74
+,0xC0,0x04,0x12,0x0F,0x37,0xE5,0x33,0xFF,0xC3,0x74,0x20,0x9F,0xFD,0xE4,0x94,0x00
+,0xFC,0x7E,0x06,0x7F,0x40,0x12,0x00,0x70,0xE4,0xFC,0xFD,0xD0,0x00,0x12,0x01,0x74
+,0x90,0x1B,0x14,0xEE,0xF0,0xA3,0xEF,0xF0,0xA2,0x41,0xE4,0xFE,0x33,0x54,0x01,0x78
+,0x07,0xC3,0x33,0xCE,0x33,0xCE,0xD8,0xF9,0xFD,0xAC,0x06,0xA2,0x40,0xE4,0x33,0x54
+,0x01,0x4C,0xFC,0xA2,0x42,0xE4,0xFE,0x33,0x54,0x01,0x78,0x06,0xC3,0x33,0xCE,0x33
+,0xCE,0xD8,0xF9,0xB1,0xAA,0xA2,0x43,0xE4,0xFE,0x33,0x54,0x01,0x78,0x05,0xC3,0x33
+,0xCE,0x33,0xCE,0xD8,0xF9,0xB1,0xAA,0xA2,0x44,0xE4,0x33,0x54,0x01,0xC4,0xF8,0x54
+,0x0F,0xC8,0x68,0xFF,0xE4,0xC4,0x54,0xF0,0x48,0x4C,0xFC,0xEF,0x4D,0xFD,0xE5,0x33
+,0x54,0x0F,0xFF,0xEC,0xF5,0x48,0xEF,0x4D,0xF5,0x49,0x22,0xD2,0x41,0xD2,0x42,0xD2
+,0x43,0xD2,0x44,0xAE,0x2F,0xAF,0x30,0xAB,0x07,0xAA,0x06,0xE4,0xF9,0xF8,0xFF,0x22
+,0xC2,0x40,0xC2,0x41,0xC2,0x42,0xC2,0x43,0x22,0xE4,0xF5,0xC1,0xF1,0x31,0x8F,0xC2
+,0x22,0x7F,0x20,0x12,0x0D,0x22,0xEF,0x54,0x03,0x64,0x03,0x7F,0x00,0x60,0x02,0x7F
+,0x01,0x22,0xAF,0xC1,0xEF,0x14,0x60,0x0C,0x04,0x70,0x16,0x74,0xFF,0x90,0x1B,0x3B
+,0xF0,0xA3,0x80,0x08,0x90,0x1B,0x3B,0xE4,0xF0,0xA3,0x74,0x0A,0xF0,0xE4,0xF5,0xC1
+,0x22,0x75,0xC1,0x01,0x22,0xAF,0xC1,0xEF,0xFE,0xAD,0xC2,0xED,0xFF,0xE4,0xF5,0xC1
+,0x8F,0x82,0x8E,0x83,0x93,0xFC,0x74,0x01,0x93,0xF5,0xC3,0xEC,0xF5,0xC4,0x22,0xE4
+,0xF5,0x8F,0xF1,0x92,0x75,0xA8,0x81,0x53,0xC9,0xFE,0x75,0xC5,0xFF,0x12,0x04,0x6F
+,0x80,0xFB,0x75,0xC9,0xFF,0x75,0xCA,0xFF,0x75,0xC7,0xFF,0x75,0xC8,0xFF,0x22,0xE5
+,0xC1,0xB4,0x80,0x05,0x12,0x09,0x2E,0x80,0x3E,0xE4,0xFD,0xAF,0xC1,0x12,0x18,0x35
+,0x12,0x19,0xB7,0x70,0x06,0xE9,0xF4,0x70,0x02,0xEA,0xF4,0x70,0x04,0x75,0xC1,0x01
+,0x22,0x90,0x1B,0x0F,0xE0,0x60,0x03,0x12,0x0F,0x87,0xE4,0x90,0x1B,0x0F,0xF0,0xA3
+,0xE5,0xC1,0xF0,0xC2,0x48,0xC2,0x47,0x12,0x0A,0xDE,0x12,0x0F,0xAA,0x7F,0x03,0x7E
+,0x00,0x12,0x04,0x31,0x12,0x0E,0x2F,0xE4,0xF5,0xC1,0x22,0xAD,0xC2,0xAF,0xC3,0x12
+,0x18,0x35,0x12,0x19,0xB7,0x70,0x06,0xE9,0xF4,0x70,0x02,0xEA,0xF4,0x70,0x08,0x75
+,0xC1,0x01,0xF5,0xC2,0xF5,0xC3,0x22,0xE5,0xC1,0x90,0x1B,0x4A,0x70,0x05,0x75,0xF0
+,0x9D,0x80,0x04,0xE4,0x75,0xF0,0x9F,0x12,0x00,0xFB,0x90,0x1B,0x49,0xE4,0x75,0xF0
+,0x01,0x12,0x02,0xF5,0x12,0x00,0x06,0xF5,0xC2,0x90,0x1B,0x49,0x12,0x0F,0xE3,0xF5
+,0xC3,0xE4,0xF5,0xC1,0x22,0xAC,0x07,0x90,0x1B,0x2D,0x11,0x70,0x12,0x01,0x11,0xFF
+,0xAE,0xF0,0xF4,0x70,0x02,0xEE,0xF4,0x60,0x1D,0xED,0x70,0x04,0xEF,0x6C,0x60,0x16
+,0xBD,0x01,0x0A,0x12,0x01,0x11,0xE5,0xF0,0xB5,0x04,0x02,0x80,0x09,0x74,0x04,0x29
+,0xF9,0xE4,0x3A,0xFA,0x80,0xD6,0x90,0x00,0x02,0x12,0x01,0x3C,0xFF,0xAE,0xF0,0x22
+,0xE0,0xFE,0xA3,0xE0,0xAA,0x06,0xF9,0x7B,0xFF,0x22,0xEF,0x24,0x34,0x60,0x06,0x04
+,0x70,0x05,0x8D,0xCB,0x22,0x8D,0xCC,0x22,0x8F,0xFA,0x8D,0xFB,0x75,0xF8,0x20,0x02
+,0x08,0x90,0x90,0x1B,0x49,0xE5,0xC2,0xF0,0xE0,0xFF,0xC3,0x94,0xFF,0x50,0x09,0xAD
+,0xC4,0x11,0x88,0xE4,0xF5,0xC1,0x80,0x03,0x75,0xC1,0x01,0xE4,0xF5,0xC2,0x22,0xE4
+,0xFD,0xAF,0xC1,0xEF,0x14,0x60,0x25,0x14,0x60,0x39,0x24,0xFC,0x60,0x51,0x14,0x60
+,0x56,0x14,0x60,0x5B,0x24,0x08,0x70,0x63,0xE5,0xC2,0xD3,0x94,0x01,0x40,0x04,0x7D
+,0x01,0x80,0x5A,0xAF,0xC2,0x90,0x1B,0x00,0xEF,0xF0,0x80,0x51,0xAF,0xC3,0xAC,0xC2
+,0xEC,0x2F,0xFF,0xE4,0x33,0x4F,0x70,0x04,0x7D,0x01,0x80,0x41,0x31,0x30,0x90,0x1B
+,0x01,0x80,0x29,0x31,0x30,0xD3,0x94,0x80,0xEE,0x94,0x0C,0x50,0x09,0xC3,0xEF,0x94
+,0x32,0xEE,0x94,0x00,0x50,0x04,0x7D,0x01,0x80,0x23,0x90,0x1B,0x03,0x80,0x0D,0x90
+,0x1B,0x09,0xE5,0xC2,0xF0,0x80,0x16,0x31,0x30,0x90,0x1B,0x37,0xEE,0x80,0x06,0xAF
+,0xC2,0x90,0x1B,0x39,0xE4,0xF0,0xA3,0xEF,0xF0,0x80,0x02,0x7D,0x01,0x8D,0xC1,0x22
+,0xAF,0xC2,0xEF,0xFE,0xAC,0xC3,0xEC,0xFB,0xEB,0xFF,0x22,0x90,0x1B,0x00,0xE0,0x60
+,0x04,0xD2,0x47,0x80,0x02,0xC2,0x47,0x30,0x47,0x15,0x12,0x0F,0xBB,0x90,0x1B,0x02
+,0xE0,0x9F,0x90,0x1B,0x01,0xE0,0x9E,0x40,0x03,0xD2,0x48,0x22,0xC2,0x48,0x22,0x12
+,0x0F,0xBB,0x90,0x1B,0x46,0xE0,0x9F,0x90,0x1B,0x45,0xE0,0x9E,0x40,0x03,0xD2,0x48
+,0x22,0xC2,0x48,0x22,0xC0,0xE0,0xC0,0xF0,0xC0,0x83,0xC0,0x82,0xC0,0xD0,0x75,0xD0
+,0x00,0xC0,0x00,0xC0,0x01,0xC0,0x02,0xC0,0x03,0xC0,0x04,0xC0,0x05,0xC0,0x06,0xC0
+,0x07,0xE5,0xC7,0x30,0xE0,0x06,0x12,0x08,0x30,0x53,0xC7,0x01,0xD0,0x07,0xD0,0x06
+,0xD0,0x05,0xD0,0x04,0xD0,0x03,0xD0,0x02,0xD0,0x01,0xD0,0x00,0xD0,0xD0,0xD0,0x82
+,0xD0,0x83,0xD0,0xF0,0xD0,0xE0,0x32,0xAA,0x06,0xA9,0x07,0x7B,0xFF,0x90,0x1B,0x49
+,0x12,0x03,0x0C,0x90,0x1B,0x49,0x12,0x02,0xEC,0x74,0xFF,0xF5,0x83,0xF5,0x82,0x6B
+,0x22,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xEF,0xFF,0xFF
+,0x21,0x20,0x24,0x90,0x21,0xB2,0x21,0x95,0x21,0x94,0x60,0x00,0x64,0x21,0x95,0x26
+,0x5E,0x25,0x66,0x2A,0x22,0x21,0x2E,0x26,0x30,0x22,0x38,0x24,0x40,0x23,0x9E,0x22
+,0x9A,0x84,0x0E,0x84,0xEE,0x84,0xEE,0x22,0x52,0x21,0x20,0x21,0x58,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x40,0x03,0x00,0x01
+,0x03,0x02,0x01,0x00,0xFF,0x60,0x03,0x18,0x00,0xE1,0x0F,0xF8,0xF4,0xF8,0x28,0x24
+,0x0C,0x26,0x00,0x27,0x0F,0x00,0x0E,0x02,0x01,0xD0,0x07,0x64,0x00,0x94,0x11,0xE8
+,0x03,0x64,0x00,0xF4,0x01,0x02,0x11,0x00,0xE8,0x03,0xFC,0x18,0x03,0xE9,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x60,0x03,0x07,0x06,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF
+};
+
+struct ov14810_reg {
+	u16 addr;
+	u16 val;
+};
+
+struct ov14810_sensor {
+	struct i2c_client *i2c_client;
+	struct ov14810_platform_data *pdata;
+};
+
+struct ov14810_info {
+	int mode;
+	int uC_programmed;
+	struct ov14810_sensor sensor;
+	struct ov14810_sensor uC;
+	struct ov14810_sensor slaveDev;
+};
+
+static struct ov14810_info *info;
+
+#define OV14810_TABLE_WAIT_MS 0
+#define OV14810_TABLE_END 1
+
+static struct ov14810_reg mode_4416x3312[] = {
+	{0x0103, 0x01},
+
+	{0x3003, 0x09},
+	{0x3004, 0x00},
+	{0x3005, 0xa7},
+	{0x3006, 0x80},
+	{0x3007, 0x08},
+	{0x3013, 0x1f},
+
+	{0x3018, 0x04},
+	{0x301b, 0xe0},
+	{0x301c, 0xf8},
+	{0x3020, 0x01},
+	{0x3106, 0x05},
+	{0x3600, 0x2d},
+	{0x3601, 0x1f},
+	{0x360a, 0x2e},
+	{0x360f, 0x24},
+	{0x3611, 0x6c},
+	{0x3613, 0x84},
+	{0x3705, 0xd1},
+	{0x3707, 0x73},
+	{0x3708, 0x01},
+	{0x370e, 0x04},
+	{0x3710, 0x40},
+	{0x3711, 0x1c},
+	{0x3717, 0x80},
+	{0x3718, 0x11},
+	{0x3719, 0x11},
+	{0x371b, 0xa0},
+	{0x371e, 0x2c},
+	{0x3723, 0x30},
+	{0x3726, 0x70},
+	{0x3808, 0x00},
+	{0x380a, 0x00},
+	{0x3817, 0x24},
+	{0x3819, 0x80},
+	{0x3a00, 0x78},
+	{0x3a13, 0x46},
+	{0x3a18, 0x00},
+	{0x3a19, 0x7f},
+	{0x3a1a, 0x06},
+	{0x3a25, 0x83},
+	{0x3b09, 0x0a},
+	{0x4002, 0xc5},
+	{0x4004, 0x02},
+	{0x4005, 0x10},
+	{0x4009, 0x40},
+	{0x404f, 0xff},
+	{0x4709, 0x00},
+	{0x4801, 0x0f},
+	{0x4806, 0x80},
+	{0x4842, 0x01},
+	{0x5000, 0x00},
+	{0x5001, 0x00},
+	{0x5002, 0x00},
+	{0x503b, 0x01},
+	{0x503c, 0x10},
+	{0x5041, 0x0e},
+	{0x5780, 0xfc},
+	{0x5b01, 0x03},
+	{0x5b03, 0x00},
+
+	{0x3003, 0x0a},
+	{0x3005, 0xa7},
+
+	{0x3006, 0x80},
+	{0x3007, 0x08},
+	{0x3013, 0x1f},
+
+	{0x3602, 0x42},
+	{0x3604, 0x80},
+	{0x3605, 0x11},
+	{0x360c, 0x42},
+	{0x360d, 0x13},
+	{0x3614, 0x05},
+
+	{0x3702, 0x10},
+	{0x3704, 0x14},
+	{0x3707, 0x73},
+	{0x370a, 0x80},
+	{0x370b, 0x00},
+	{0x370c, 0x04},
+	{0x370d, 0x0d},
+	{0x370f, 0x61},
+	{0x3713, 0xfa},
+	{0x3714, 0x2f},
+	{0x3715, 0x2c},
+	{0x3716, 0x0b},
+	{0x371c, 0x28},
+	{0x371d, 0x20},
+	{0x3721, 0x08},
+	{0x3724, 0x18},
+	{0x3725, 0x17},
+	{0x3727, 0x65},
+	{0x3728, 0x0c},
+
+	{0x3803, 0x0b},
+	{0x3804, 0x11},
+	{0x3805, 0x40},
+	{0x3806, 0x0c},
+	{0x3807, 0xf9},
+	{0x380c, 0x09},
+	{0x380d, 0x5c},
+	{0x380e, 0x0d},
+	{0x380f, 0x08},
+	{0x3810, 0x44},
+	{0x3811, 0x96},
+	{0x3818, 0x40},
+	{0x381c, 0x30},
+	{0x381d, 0x10},
+	{0x381e, 0x0c},
+	{0x381f, 0xf8},
+	{0x3820, 0x00},
+	{0x3821, 0x0c},
+	{0x3503, 0x13},
+
+	{0x4050, 0xc0},
+	{0x4051, 0x00},
+	{0x4053, 0xa1},
+	{0x4837, 0x1b},
+	{0x503d, 0x00},
+	{0x5042, 0x21},
+	{0x5047, 0x00},
+
+	{0x3a08, 0x1f},
+	{0x3a09, 0x40},
+	{0x3a0a, 0x1a},
+	{0x3a0b, 0x00},
+	{0x3a0d, 0x08},
+	{0x3a0e, 0x06},
+
+	{0x503d, 0x00},
+
+	{0x0100, 0x01},
+	{OV14810_TABLE_END, 0x0000}
+};
+
+static struct ov14810_reg mode_1280x720[] = {
+	{0x0103, 0x01},
+	{OV14810_TABLE_WAIT_MS, 20},
+
+	{0x3003, 0x0a},
+	{0x3004, 0x00},
+	{0x3005, 0xa7},
+	{0x3006, 0x80},
+	{0x3007, 0x08},
+
+	{0x3018, 0x04},
+	{0x301b, 0xe0},
+	{0x301c, 0xf8},
+	{0x3020, 0x01},
+	{0x3106, 0x05},
+	{0x3600, 0x2d},
+	{0x3601, 0x1f},
+	{0x3609, 0x00},
+	{0x360a, 0x2e},
+	{0x360f, 0x24},
+	{0x3611, 0x6c},
+	{0x3613, 0x84},
+	{0x3702, 0x20},
+	{0x3704, 0x28},
+	{0x3705, 0xd1},
+	{0x3708, 0x01},
+	{0x370e, 0x04},
+	{0x3710, 0x40},
+	{0x3711, 0x1c},
+	{0x3714, 0x5f},
+	{0x3715, 0x58},
+	{0x3717, 0x80},
+	{0x3718, 0x11},
+	{0x3719, 0x11},
+	{0x371b, 0xa0},
+	{0x371c, 0x46},
+	{0x371d, 0x40},
+	{0x371e, 0x2c},
+	{0x3723, 0x30},
+	{0x3725, 0x2e},
+	{0x3726, 0x70},
+	{0x3808, 0x00},
+	{0x380a, 0x00},
+	{0x3817, 0x24},
+	{0x3819, 0x80},
+	{0x382c, 0x02},
+	{0x382d, 0x01},
+	{0x3a00, 0x78},
+	{0x3a13, 0x46},
+	{0x3a18, 0x00},
+	{0x3a19, 0x7f},
+	{0x3a1a, 0x06},
+	{0x3a25, 0x83},
+	{0x3b09, 0x0a},
+	{0x4002, 0xc5},
+	{0x4004, 0x02},
+	{0x4005, 0x10},
+	{0x4009, 0x40},
+	{0x404f, 0xff},
+	{0x4709, 0x00},
+	{0x4801, 0x0f},
+	{0x4806, 0x80},
+	{0x4842, 0x01},
+	{0x5000, 0x00},
+	{0x5001, 0x00},
+	{0x5002, 0x00},
+	{0x503b, 0x01},
+	{0x503c, 0x10},
+	{0x5041, 0x0e},
+	{0x5780, 0xfc},
+	{0x5b00, 0x10},
+	{0x5b01, 0x5b},
+	{0x5b03, 0x00},
+
+
+	{0x3005, 0xa7},
+	{0x3006, 0x80},
+	{0x3007, 0x08},
+	{0x3013, 0x1f},
+
+	{0x3602, 0x53},
+	{0x3604, 0x80},
+	{0x3605, 0x01},
+	{0x360b, 0x0c},
+	{0x360c, 0x45},
+	{0x360d, 0x03},
+	{0x3614, 0x05},
+
+	{0x3707, 0x73},
+	{0x370a, 0x81},
+	{0x370b, 0x20},
+	{0x370c, 0x04},
+	{0x370d, 0x01},
+	{0x370f, 0x00},
+	{0x3713, 0xe6},
+	{0x3716, 0xf0},
+	{0x3721, 0x08},
+	{0x3724, 0x2e},
+	{0x3727, 0x60},
+	{0x3728, 0x02},
+
+	{0x3803, 0x07},
+	{0x3804, 0x05}, /* width */
+	{0x3805, 0x09},
+	{0x3806, 0x02}, /* height */
+	{0x3807, 0xd8},
+	{0x380c, 0x05},
+	{0x380d, 0x66},
+	{0x380e, 0x02},
+	{0x380f, 0xe4},
+	{0x3810, 0x22},
+	{0x3811, 0x02},
+	{0x3818, 0x45},
+	{0x381c, 0x13},
+	{0x381d, 0xb8},
+	{0x381e, 0x05}, /* height w/o skipping */
+	{0x381f, 0xc0},
+	{0x3820, 0x03},
+	{0x3821, 0xa8},
+	{0x3503, 0x13}, /* Manual exposure, gain control */
+
+	{0x4050, 0xc0},
+	{0x4051, 0x00},
+	{0x4053, 0xa1},
+	{0x4837, 0x1b},
+	{0x503d, 0x00},
+	{0x5042, 0x31},
+	{0x5047, 0x00},
+
+	{0x100, 0x01},
+	{OV14810_TABLE_END, 0x0000}
+};
+
+enum {
+	OV14810_MODE_4416x3312,
+	OV14810_MODE_1280x720
+};
+
+static struct ov14810_reg *mode_table[] = {
+	[OV14810_MODE_4416x3312] = mode_4416x3312,
+	[OV14810_MODE_1280x720]  = mode_1280x720
+};
+
+static inline void ov14810_get_frame_length_regs(struct ov14810_reg *regs,
+						u32 frame_length)
+{
+	regs->addr = OV14810_FRAME_LENGTH_REG_ADDR0;
+	regs->val = (frame_length >> 8) & 0xff;
+	(regs + 1)->addr = OV14810_FRAME_LENGTH_REG_ADDR1;
+	(regs + 1)->val = (frame_length) & 0xff;
+}
+
+static inline void ov14810_get_coarse_time_regs(struct ov14810_reg *regs,
+						u32 coarse_time)
+{
+	regs->addr = OV14810_COARSE_TIME_REG_ADDR0;
+	regs->val = (coarse_time >> 12) & 0xff;
+	(regs + 1)->addr = OV14810_COARSE_TIME_REG_ADDR1;
+	(regs + 1)->val = (coarse_time >> 4) & 0xff;
+	(regs + 2)->addr = OV14810_COARSE_TIME_REG_ADDR2;
+	(regs + 2)->val = (coarse_time & 0xf) << 4;
+}
+
+static inline void ov14810_get_gain_reg(struct ov14810_reg *regs, u16 gain)
+{
+	regs->addr = OV14810_GAIN_REG_ADDR0;
+	regs->val = gain;
+}
+
+static int ov14810_write16(struct i2c_client *client, u16 addr, u8 val)
+{
+	int err;
+	struct i2c_msg msg;
+	unsigned char data[3];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = (u8) (addr >> 8);
+	data[1] = (u8) (addr & 0xff);
+	data[2] = (u8) (val & 0xff);
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = data;
+
+	err = i2c_transfer(client->adapter, &msg, 1);
+	if (err != 1) {
+		pr_err("ov14810: i2c transfer failed %x %x\n", addr, val);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int ov14810_write8(struct i2c_client *client, u8 addr, u8 val)
+{
+	int err;
+	struct i2c_msg msg;
+	unsigned char data[2];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = (u8) (addr);
+	data[1] = (u8) (val & 0xff);
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = data;
+
+	err = i2c_transfer(client->adapter, &msg, 1);
+	if (err != 1) {
+		pr_err("ov14810: i2c transfer failed %x %x\n",addr, val);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int ov14810_write_reg_helper(struct ov14810_info *info,
+					u16 addr, u8 val)
+{
+	return ov14810_write16(info->sensor.i2c_client, addr, val);
+}
+
+static int ov14810_write_table(struct ov14810_info *info,
+				const struct ov14810_reg table[],
+				const struct ov14810_reg override_list[],
+				int num_override_regs)
+{
+	int err;
+	const struct ov14810_reg *next;
+	int i;
+	u16 val;
+
+	for (next = table; next->addr != OV14810_TABLE_END; next++) {
+		val = next->val;
+
+		if (next->addr == OV14810_TABLE_WAIT_MS) {
+			msleep(val);
+			continue;
+		}
+
+		/* When an override list is passed in, replace the reg */
+		/* value to write if the reg is in the list */
+		if (override_list) {
+			for (i = 0; i < num_override_regs; i++) {
+				if (next->addr == override_list[i].addr) {
+					val = override_list[i].val;
+					break;
+				}
+			}
+		}
+		err = ov14810_write_reg_helper(info, next->addr, val);
+	}
+	return err;
+}
+
+static int ov14810_set_mode(struct ov14810_info *info, struct ov14810_mode *mode)
+{
+	int sensor_mode;
+	int err;
+	struct ov14810_reg reg_list[6];
+
+	pr_info("%s: xres %u yres %u framelength %u coarsetime %u gain %u\n",
+		__func__, mode->xres, mode->yres, mode->frame_length,
+		mode->coarse_time, mode->gain);
+	if (mode->xres == 1280 && mode->yres == 720)
+		sensor_mode = OV14810_MODE_1280x720;
+	else if (mode->xres == 4416 && mode->yres == 3312)
+		sensor_mode = OV14810_MODE_4416x3312;
+	else {
+		pr_err("%s: invalid resolution supplied to set mode %d %d\n",
+		       __func__, mode->xres, mode->yres);
+		return -EINVAL;
+	}
+
+	/* get a list of override regs for the asking frame length, */
+	/* coarse integration time, and gain.	*/
+	ov14810_get_frame_length_regs(reg_list, mode->frame_length);
+	ov14810_get_coarse_time_regs(reg_list + 2, mode->coarse_time);
+	ov14810_get_gain_reg(reg_list + 5, mode->gain);
+
+	err = ov14810_write_table(info, mode_table[sensor_mode],
+		reg_list, 6);
+
+	if (err)
+		return err;
+
+	info->mode = sensor_mode;
+	return 0;
+}
+
+static int ov14810_set_frame_length(struct ov14810_info *info, u32 frame_length)
+{
+	struct ov14810_reg reg_list[2];
+	int i;
+	int ret;
+
+	ov14810_get_frame_length_regs(reg_list, frame_length);
+
+	for (i = 0; i < 2; i++) {
+		ret = ov14810_write_reg_helper(info, reg_list[i].addr,
+			reg_list[i].val);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int ov14810_set_coarse_time(struct ov14810_info *info, u32 coarse_time)
+{
+	int ret;
+
+	struct ov14810_reg reg_list[3];
+	int i;
+
+	ov14810_get_coarse_time_regs(reg_list, coarse_time);
+
+	ret = ov14810_write_reg_helper(info, OV14810_GROUP_ACCESS_REG_ADDR, 0x01);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < 3; i++) {
+		ret = ov14810_write_reg_helper(info, reg_list[i].addr,
+			reg_list[i].val);
+		if (ret)
+			return ret;
+	}
+
+	ret = ov14810_write_reg_helper(info, OV14810_GROUP_ACCESS_REG_ADDR, 0x11);
+	if (ret)
+		return ret;
+
+	ret = ov14810_write_reg_helper(info, OV14810_GROUP_ACCESS_REG_ADDR, 0xa1);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int ov14810_set_gain(struct ov14810_info *info, u16 gain)
+{
+	int ret;
+	struct ov14810_reg reg_list;
+
+	ov14810_get_gain_reg(&reg_list, gain);
+
+	ret = ov14810_write_reg_helper(info, reg_list.addr, reg_list.val);
+
+	return ret;
+}
+
+static int ov14810_set_power(int powerLevel)
+{
+	pr_info("%s: powerLevel=%d \n", __func__, powerLevel);
+
+	if (info->sensor.pdata) {
+		if (powerLevel && info->sensor.pdata->power_on) {
+			info->sensor.pdata->power_on();
+			msleep(1000);
+		}
+		else if (info->sensor.pdata->power_off) {
+			info->sensor.pdata->power_off();
+		}
+	}
+
+	return 0;
+}
+
+static long ov14810_ioctl(struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	struct ov14810_info *info = file->private_data;
+	int err;
+
+	switch (cmd) {
+	case OV14810_IOCTL_SET_MODE:
+	{
+		struct ov14810_mode mode;
+
+		err = copy_from_user(&mode,(const void __user *)arg,
+					sizeof(struct ov14810_mode));
+		if (err) {
+			pr_err("%s %d\n", __func__, __LINE__);
+			return err;
+		}
+
+		return ov14810_set_mode(info, &mode);
+	}
+	case OV14810_IOCTL_SET_FRAME_LENGTH:
+		return ov14810_set_frame_length(info, (u32)arg);
+	case OV14810_IOCTL_SET_COARSE_TIME:
+		return ov14810_set_coarse_time(info, (u32)arg);
+	case OV14810_IOCTL_SET_GAIN:
+		return ov14810_set_gain(info, (u16)arg);
+	case OV14810_IOCTL_GET_STATUS:
+	{
+		u16 status = 0;
+		err = copy_to_user((void __user *)arg, &status,2);
+		if (err) {
+			pr_err("%s %d\n", __func__, __LINE__);
+			return err;
+		}
+		return 0;
+	}
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int ov14810_slavedev_open(void)
+{
+	pr_info("%s\n", __func__);
+
+	OV14810_I2C_WRITE8(info->slaveDev.i2c_client, 0x19, 0x67);
+	OV14810_I2C_WRITE8(info->slaveDev.i2c_client, 0x18, 0x02);
+
+	return 0;
+}
+
+static int ov14810_slavedev_reset(void)
+{
+	pr_info("%s\n", __func__);
+
+	OV14810_I2C_WRITE8(info->slaveDev.i2c_client, 0x18, 0x03);
+	msleep(1000);
+
+	OV14810_I2C_WRITE8(info->slaveDev.i2c_client, 0xc1, 0x0);
+	OV14810_I2C_WRITE8(info->slaveDev.i2c_client, 0xc2, 0x0);
+	OV14810_I2C_WRITE8(info->slaveDev.i2c_client, 0xc3, 0x0);
+	OV14810_I2C_WRITE8(info->slaveDev.i2c_client, 0xc4, 0x0);
+	OV14810_I2C_WRITE8(info->slaveDev.i2c_client, 0xc5, 0x0);
+
+	msleep(1000);
+
+	OV14810_I2C_WRITE8(info->slaveDev.i2c_client, 0xc1, 0x0);
+	OV14810_I2C_WRITE8(info->slaveDev.i2c_client, 0xc2, 0x0);
+	OV14810_I2C_WRITE8(info->slaveDev.i2c_client, 0xc3, 0x0);
+	OV14810_I2C_WRITE8(info->slaveDev.i2c_client, 0xc4, 0x0);
+	OV14810_I2C_WRITE8(info->slaveDev.i2c_client, 0xc5, 0x17);
+
+	msleep(1000);
+
+	return 0;
+}
+
+static int ov14810uC_open(void)
+{
+	int i;
+	int err;
+
+	pr_info("ov14810uC programmming started \n");
+
+	for (i = 0; i < sizeof(uCProgram); i++) {
+		ov14810_write16(info->uC.i2c_client,
+			( ( (i & 0xff) << 8) | ( (i & 0xff00) >> 8) ), uCProgram[i]);
+	}
+	pr_info("ov14810uC programmming finished \n");
+
+	err = ov14810_slavedev_reset();
+
+	return err;
+}
+
+static int ov14810_open(struct inode *inode, struct file *file)
+{
+	int err;
+	pr_info("%s\n", __func__);
+	file->private_data = info;
+
+	err = ov14810_set_power(1);
+
+	if (err)
+		return err;
+
+	if (info->uC_programmed == 0) {
+		err = ov14810_slavedev_open();
+
+		if (err)
+			return err;
+
+		err = ov14810uC_open();
+		if (!err)
+			info->uC_programmed = 1;
+	}
+
+	return err;
+}
+
+int ov14810_release(struct inode *inode, struct file *file)
+{
+	pr_info("%s\n", __func__);
+	ov14810_set_power(0);
+	file->private_data = NULL;
+	return 0;
+}
+
+static const struct file_operations ov14810_fileops = {
+	.owner = THIS_MODULE,
+	.open = ov14810_open,
+	.unlocked_ioctl = ov14810_ioctl,
+	.release = ov14810_release,
+};
+
+static struct miscdevice ov14810_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "ov14810",
+	.fops = &ov14810_fileops,
+};
+
+static int ov14810_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int err;
+	pr_info("%s: probing sensor.\n", __func__);
+
+	if (!info) {
+		info = kzalloc(sizeof(struct ov14810_info), GFP_KERNEL);
+		if (!info) {
+			pr_err("ov14810: Unable to allocate memory!\n");
+			return -ENOMEM;
+		}
+	}
+
+	err = misc_register(&ov14810_device);
+	if (err) {
+		pr_err("ov14810: Unable to register misc device!\n");
+		kfree(info);
+		return err;
+	}
+
+	info->sensor.pdata = client->dev.platform_data;
+	info->sensor.i2c_client = client;
+
+	return 0;
+}
+
+static int ov14810_remove(struct i2c_client *client)
+{
+	misc_deregister(&ov14810_device);
+	kfree(info);
+	return 0;
+}
+
+static int ov14810_uC_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	if (!info) {
+		info = kzalloc(sizeof(struct ov14810_sensor), GFP_KERNEL);
+		if (!info) {
+			pr_err("ov14810uC: Unable to allocate memory!\n");
+			return -ENOMEM;
+		}
+	}
+	info->uC.pdata = client->dev.platform_data;
+	info->uC.i2c_client = client;
+
+	return 0;
+}
+
+static int ov14810_uC_remove(struct i2c_client *client)
+{
+	return 0;
+}
+
+static int ov14810_slavedev_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	pr_info("%s: probing slave Dev of sensor.\n", __func__);
+
+	if (!info) {
+		info = kzalloc(sizeof(struct ov14810_sensor), GFP_KERNEL);
+		if (!info) {
+			pr_err("ov14810uC: Unable to allocate memory!\n");
+			return -ENOMEM;
+		}
+	}
+
+	info->slaveDev.pdata = client->dev.platform_data;
+	info->slaveDev.i2c_client = client;
+	info->uC_programmed = 0;
+
+	return 0;
+}
+
+static int ov14810_slavedev_remove(struct i2c_client *client)
+{
+	return 0;
+}
+
+static const struct i2c_device_id ov14810_id[] = {
+	{ "ov14810", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, ov14810_id);
+
+static struct i2c_driver ov14810_i2c_driver = {
+	.driver = {
+		.name = "ov14810",
+		.owner = THIS_MODULE,
+	},
+	.probe = ov14810_probe,
+	.remove = ov14810_remove,
+	.id_table = ov14810_id,
+};
+
+
+static const struct i2c_device_id ov14810_uC_id[] = {
+	{ "ov14810uC", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, ov14810_uC_id);
+
+static struct i2c_driver ov14810_uC_i2c_driver = {
+	.driver = {
+		.name = "ov14810uC",
+		.owner = THIS_MODULE,
+	},
+	.probe = ov14810_uC_probe,
+	.remove = ov14810_uC_remove,
+	.id_table = ov14810_uC_id,
+};
+
+static const struct i2c_device_id ov14810_slavedev_id[] = {
+	{ "ov14810SlaveDev", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, ov14810_slavedev_id);
+
+static struct i2c_driver ov14810_slavedev_i2c_driver = {
+	.driver = {
+		.name = "ov14810SlaveDev",
+		.owner = THIS_MODULE,
+	},
+	.probe = ov14810_slavedev_probe,
+	.remove = ov14810_slavedev_remove,
+	.id_table = ov14810_slavedev_id,
+};
+
+static int __init ov14810_init(void)
+{
+	int ret;
+	pr_info("ov14810 sensor driver loading\n");
+	ret = i2c_add_driver(&ov14810_i2c_driver);
+	if (ret)
+		return ret;
+
+	ret = i2c_add_driver(&ov14810_uC_i2c_driver);
+	if (ret)
+		return ret;
+
+	return i2c_add_driver(&ov14810_slavedev_i2c_driver);
+}
+
+static void __exit ov14810_exit(void)
+{
+	i2c_del_driver(&ov14810_slavedev_i2c_driver);
+	i2c_del_driver(&ov14810_uC_i2c_driver);
+	i2c_del_driver(&ov14810_i2c_driver);
+}
+
+module_init(ov14810_init);
+module_exit(ov14810_exit);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/video/tegra/ov2710.c b/drivers/media/video/tegra/ov2710.c
new file mode 100644
index 000000000000..293cb8932dfb
--- /dev/null
+++ b/drivers/media/video/tegra/ov2710.c
@@ -0,0 +1,774 @@
+/*
+ * ov2710.c - ov2710 sensor driver
+ *
+ * Copyright (c) 2011, NVIDIA, All Rights Reserved.
+ *
+ * Contributors:
+ *      erik lilliebjerg <elilliebjerg@nvidia.com>
+ *
+ * Leverage OV5650.c
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <media/ov2710.h>
+
+#define SIZEOF_I2C_TRANSBUF 32
+
+struct ov2710_reg {
+	u16 addr;
+	u16 val;
+};
+
+struct ov2710_info {
+	int mode;
+	struct i2c_client *i2c_client;
+	struct ov2710_platform_data *pdata;
+	u8 i2c_trans_buf[SIZEOF_I2C_TRANSBUF];
+};
+
+#define OV2710_TABLE_WAIT_MS 0
+#define OV2710_TABLE_END 1
+#define OV2710_MAX_RETRIES 3
+
+static struct ov2710_reg mode_1920x1080[] = {
+	{0x3103, 0x93},
+	{0x3008, 0x82},
+	{OV2710_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42},
+	{OV2710_TABLE_WAIT_MS, 5},
+	{0x3017, 0x7f},
+	{0x3018, 0xfc},
+	{0x3706, 0x61},
+	{0x3712, 0x0c},
+	{0x3630, 0x6d},
+	{0x3801, 0xb4},
+
+	{0x3621, 0x04},
+	{0x3604, 0x60},
+	{0x3603, 0xa7},
+	{0x3631, 0x26},
+	{0x3600, 0x04},
+	{0x3620, 0x37},
+	{0x3623, 0x00},
+	{0x3702, 0x9e},
+	{0x3703, 0x5c},
+	{0x3704, 0x40},
+	{0x370d, 0x0f},
+	{0x3713, 0x9f},
+	{0x3714, 0x4c},
+	{0x3710, 0x9e},
+	{0x3801, 0xc4},
+	{0x3605, 0x05},
+	{0x3606, 0x3f},
+	{0x302d, 0x90},
+	{0x370b, 0x40},
+	{0x3716, 0x31},
+	{0x3707, 0x52},
+	{0x380d, 0x74},
+	{0x5181, 0x20},
+	{0x518f, 0x00},
+	{0x4301, 0xff},
+	{0x4303, 0x00},
+	{0x3a00, 0x78},
+	{0x300f, 0x88},
+	{0x3011, 0x28},
+	{0x3a1a, 0x06},
+	{0x3a18, 0x00},
+	{0x3a19, 0x7a},
+	{0x3a13, 0x54},
+	{0x382e, 0x0f},
+	{0x381a, 0x1a},
+	{0x401d, 0x02},
+
+	{0x381c, 0x00},
+	{0x381d, 0x02},
+	{0x381e, 0x04},
+	{0x381f, 0x38},
+	{0x3820, 0x00},
+	{0x3821, 0x98},
+	{0x3800, 0x01},
+	{0x3802, 0x00},
+	{0x3803, 0x0a},
+	{0x3804, 0x07},
+	{0x3805, 0x90},
+	{0x3806, 0x04},
+	{0x3807, 0x40},
+	{0x3808, 0x07},
+	{0x3809, 0x90},
+	{0x380a, 0x04},
+	{0x380b, 0x40},
+	{0x380e, 0x04},
+	{0x380f, 0x50},
+	{0x380c, 0x09},
+	{0x380d, 0x74},
+	{0x3810, 0x08},
+	{0x3811, 0x02},
+
+	{0x5688, 0x03},
+	{0x5684, 0x07},
+	{0x5685, 0xa0},
+	{0x5686, 0x04},
+	{0x5687, 0x43},
+	{0x3011, 0x0a},
+	{0x300f, 0x8a},
+	{0x3017, 0x00},
+	{0x3018, 0x00},
+	{0x4800, 0x24},
+	{0x300e, 0x04},
+	{0x4801, 0x0f},
+
+	{0x300f, 0xc3},
+	{0x3010, 0x00},
+	{0x3011, 0x0a},
+	{0x3012, 0x01},
+
+	{0x3a0f, 0x40},
+	{0x3a10, 0x38},
+	{0x3a1b, 0x48},
+	{0x3a1e, 0x30},
+	{0x3a11, 0x90},
+	{0x3a1f, 0x10},
+
+	{0x3a0e, 0x03},
+	{0x3a0d, 0x04},
+	{0x3a08, 0x14},
+	{0x3a09, 0xc0},
+	{0x3a0a, 0x11},
+	{0x3a0b, 0x40},
+
+	{0x300f, 0xc3},
+	{0x3010, 0x00},
+	{0x3011, 0x0e},
+	{0x3012, 0x02},
+	{0x380c, 0x09},
+	{0x380d, 0xec},
+	{0x3703, 0x61},
+	{0x3704, 0x44},
+	{0x3801, 0xd2},
+
+	{0x3503, 0x33},
+	{0x3500, 0x00},
+	{0x3501, 0x00},
+	{0x3502, 0x00},
+	{0x350a, 0x00},
+	{0x350b, 0x00},
+	{0x5001, 0x4e},
+	{0x5000, 0x5f},
+	{0x3008, 0x02},
+
+	{OV2710_TABLE_END, 0x0000}
+};
+
+static struct ov2710_reg mode_1280x720[] = {
+	{0x3103, 0x93},
+	{0x3008, 0x82},
+	{OV2710_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42},
+	{OV2710_TABLE_WAIT_MS, 5},
+	{0x3017, 0x7f},
+	{0x3018, 0xfc},
+
+	{0x3706, 0x61},
+	{0x3712, 0x0c},
+	{0x3630, 0x6d},
+	{0x3801, 0xb4},
+	{0x3621, 0x04},
+	{0x3604, 0x60},
+	{0x3603, 0xa7},
+	{0x3631, 0x26},
+	{0x3600, 0x04},
+	{0x3620, 0x37},
+	{0x3623, 0x00},
+	{0x3702, 0x9e},
+	{0x3703, 0x5c},
+	{0x3704, 0x40},
+	{0x370d, 0x0f},
+	{0x3713, 0x9f},
+	{0x3714, 0x4c},
+	{0x3710, 0x9e},
+	{0x3801, 0xc4},
+	{0x3605, 0x05},
+	{0x3606, 0x3f},
+	{0x302d, 0x90},
+	{0x370b, 0x40},
+	{0x3716, 0x31},
+	{0x3707, 0x52},
+	{0x380d, 0x74},
+	{0x5181, 0x20},
+	{0x518f, 0x00},
+	{0x4301, 0xff},
+	{0x4303, 0x00},
+	{0x3a00, 0x78},
+	{0x300f, 0x88},
+	{0x3011, 0x28},
+	{0x3a1a, 0x06},
+	{0x3a18, 0x00},
+	{0x3a19, 0x7a},
+	{0x3a13, 0x54},
+	{0x382e, 0x0f},
+	{0x381a, 0x1a},
+	{0x401d, 0x02},
+
+	{0x381c, 0x10},
+	{0x381d, 0xb0},
+	{0x381e, 0x02},
+	{0x381f, 0xec},
+	{0x3800, 0x01},
+	{0x3820, 0x0a},
+	{0x3821, 0x2a},
+	{0x3804, 0x05},
+	{0x3805, 0x10},
+	{0x3802, 0x00},
+	{0x3803, 0x04},
+	{0x3806, 0x02},
+	{0x3807, 0xe0},
+	{0x3808, 0x05},
+	{0x3809, 0x10},
+	{0x380a, 0x02},
+	{0x380b, 0xe0},
+	{0x380e, 0x02},
+	{0x380f, 0xf0},
+	{0x380c, 0x07},
+	{0x380d, 0x00},
+	{0x3810, 0x10},
+	{0x3811, 0x06},
+
+	{0x5688, 0x03},
+	{0x5684, 0x05},
+	{0x5685, 0x00},
+	{0x5686, 0x02},
+	{0x5687, 0xd0},
+
+	{0x3a08, 0x1b},
+	{0x3a09, 0xe6},
+	{0x3a0a, 0x17},
+	{0x3a0b, 0x40},
+	{0x3a0e, 0x01},
+	{0x3a0d, 0x02},
+	{0x3011, 0x0a},
+	{0x300f, 0x8a},
+	{0x3017, 0x00},
+	{0x3018, 0x00},
+	{0x4800, 0x24},
+	{0x300e, 0x04},
+	{0x4801, 0x0f},
+	{0x300f, 0xc3},
+	{0x3a0f, 0x40},
+	{0x3a10, 0x38},
+	{0x3a1b, 0x48},
+	{0x3a1e, 0x30},
+	{0x3a11, 0x90},
+	{0x3a1f, 0x10},
+
+	{0x3010, 0x10},
+	{0x3a0e, 0x02},
+	{0x3a0d, 0x03},
+	{0x3a08, 0x0d},
+	{0x3a09, 0xf3},
+	{0x3a0a, 0x0b},
+	{0x3a0b, 0xa0},
+
+	{0x300f, 0xc3},
+	{0x3011, 0x0e},
+	{0x3012, 0x02},
+	{0x380c, 0x07},
+	{0x380d, 0x6a},
+	{0x3703, 0x5c},
+	{0x3704, 0x40},
+	{0x3801, 0xbc},
+
+	{0x3503, 0x33},
+	{0x3500, 0x00},
+	{0x3501, 0x00},
+	{0x3502, 0x00},
+	{0x350a, 0x00},
+	{0x350b, 0x00},
+	{0x5001, 0x4e},
+	{0x5000, 0x5f},
+	{0x3008, 0x02},
+
+	{OV2710_TABLE_END, 0x0000}
+};
+
+enum {
+	OV2710_MODE_1920x1080,
+	OV2710_MODE_1280x720,
+};
+
+
+static struct ov2710_reg *mode_table[] = {
+	[OV2710_MODE_1920x1080] = mode_1920x1080,
+	[OV2710_MODE_1280x720] = mode_1280x720,
+};
+
+static inline void ov2710_get_frame_length_regs(struct ov2710_reg *regs,
+						u32 frame_length)
+{
+	regs->addr = 0x380e;
+	regs->val = (frame_length >> 8) & 0xff;
+	(regs + 1)->addr = 0x380f;
+	(regs + 1)->val = (frame_length) & 0xff;
+}
+
+static inline void ov2710_get_coarse_time_regs(struct ov2710_reg *regs,
+					       u32 coarse_time)
+{
+	regs->addr = 0x3500;
+	regs->val = (coarse_time >> 12) & 0xff;
+	(regs + 1)->addr = 0x3501;
+	(regs + 1)->val = (coarse_time >> 4) & 0xff;
+	(regs + 2)->addr = 0x3502;
+	(regs + 2)->val = (coarse_time & 0xf) << 4;
+}
+
+static inline void ov2710_get_gain_reg(struct ov2710_reg *regs, u16 gain)
+{
+	regs->addr = 0x350b;
+	regs->val = gain;
+}
+
+static int ov2710_read_reg(struct i2c_client *client, u16 addr, u8 *val)
+{
+	int err;
+	struct i2c_msg msg[2];
+	unsigned char data[3];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 2;
+	msg[0].buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8) (addr >> 8);;
+	data[1] = (u8) (addr & 0xff);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = data + 2;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+
+	if (err != 2)
+
+		return -EINVAL;
+
+	*val = data[2];
+
+	return 0;
+}
+
+static int ov2710_write_reg(struct i2c_client *client, u16 addr, u8 val)
+{
+	int err;
+	struct i2c_msg msg;
+	unsigned char data[3];
+	int retry = 0;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = (u8) (addr >> 8);;
+	data[1] = (u8) (addr & 0xff);
+	data[2] = (u8) (val & 0xff);
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = data;
+
+	do {
+		err = i2c_transfer(client->adapter, &msg, 1);
+		if (err == 1)
+			return 0;
+		retry++;
+		pr_err("ov2710: i2c transfer failed, retrying %x %x\n",
+		       addr, val);
+
+		msleep(3);
+	} while (retry <= OV2710_MAX_RETRIES);
+
+	return err;
+}
+
+static int ov2710_write_bulk_reg(struct i2c_client *client, u8 *data, int len)
+{
+	int err;
+	struct i2c_msg msg;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = len;
+	msg.buf = data;
+
+	err = i2c_transfer(client->adapter, &msg, 1);
+	if (err == 1)
+		return 0;
+
+	pr_err("ov2710: i2c bulk transfer failed at %x\n",
+		(int)data[0] << 8 | data[1]);
+
+	return err;
+}
+
+static int ov2710_write_table(struct ov2710_info *info,
+			      const struct ov2710_reg table[],
+			      const struct ov2710_reg override_list[],
+			      int num_override_regs)
+{
+	int err;
+	const struct ov2710_reg *next, *n_next;
+	u8 *b_ptr = info->i2c_trans_buf;
+	unsigned int buf_filled = 0;
+	unsigned int i;
+	u16 val;
+
+	for (next = table; next->addr != OV2710_TABLE_END; next++) {
+		if (next->addr == OV2710_TABLE_WAIT_MS) {
+			msleep(next->val);
+			continue;
+		}
+
+		val = next->val;
+		/* When an override list is passed in, replace the reg */
+		/* value to write if the reg is in the list            */
+		if (override_list) {
+			for (i = 0; i < num_override_regs; i++) {
+				if (next->addr == override_list[i].addr) {
+					val = override_list[i].val;
+					break;
+				}
+			}
+		}
+
+		if (!buf_filled) {
+			b_ptr = info->i2c_trans_buf;
+			*b_ptr++ = next->addr >> 8;
+			*b_ptr++ = next->addr & 0xff;
+			buf_filled = 2;
+		}
+		*b_ptr++ = val;
+		buf_filled++;
+
+		n_next = next + 1;
+		if (n_next->addr != OV2710_TABLE_END &&
+			n_next->addr != OV2710_TABLE_WAIT_MS &&
+			buf_filled < SIZEOF_I2C_TRANSBUF &&
+			n_next->addr == next->addr + 1) {
+			continue;
+		}
+
+		err = ov2710_write_bulk_reg(info->i2c_client,
+			info->i2c_trans_buf, buf_filled);
+		if (err)
+			return err;
+		buf_filled = 0;
+	}
+	return 0;
+}
+
+static int ov2710_set_mode(struct ov2710_info *info, struct ov2710_mode *mode)
+{
+	int sensor_mode;
+	int err;
+	struct ov2710_reg reg_list[6];
+
+	pr_info("%s: xres %u yres %u framelength %u coarsetime %u gain %u\n",
+		__func__, mode->xres, mode->yres, mode->frame_length,
+		mode->coarse_time, mode->gain);
+
+	if (mode->xres == 1920 && mode->yres == 1080)
+		sensor_mode = OV2710_MODE_1920x1080;
+	else if (mode->xres == 1280 && mode->yres == 720)
+		sensor_mode = OV2710_MODE_1280x720;
+	else {
+		pr_err("%s: invalid resolution supplied to set mode %d %d\n",
+		       __func__, mode->xres, mode->yres);
+		return -EINVAL;
+	}
+
+	/* get a list of override regs for the asking frame length, */
+	/* coarse integration time, and gain.                       */
+	ov2710_get_frame_length_regs(reg_list, mode->frame_length);
+	ov2710_get_coarse_time_regs(reg_list + 2, mode->coarse_time);
+	ov2710_get_gain_reg(reg_list + 5, mode->gain);
+
+	err = ov2710_write_table(info, mode_table[sensor_mode],
+	reg_list, 6);
+	if (err)
+		return err;
+
+	info->mode = sensor_mode;
+	return 0;
+}
+
+static int ov2710_set_frame_length(struct ov2710_info *info, u32 frame_length)
+{
+	int ret;
+	struct ov2710_reg reg_list[2];
+	u8 *b_ptr = info->i2c_trans_buf;
+
+	ov2710_get_frame_length_regs(reg_list, frame_length);
+
+	*b_ptr++ = reg_list[0].addr >> 8;
+	*b_ptr++ = reg_list[0].addr & 0xff;
+	*b_ptr++ = reg_list[0].val & 0xff;
+	*b_ptr++ = reg_list[1].val & 0xff;
+	ret = ov2710_write_bulk_reg(info->i2c_client, info->i2c_trans_buf, 4);
+
+	return ret;
+}
+
+static int ov2710_set_coarse_time(struct ov2710_info *info, u32 coarse_time)
+{
+	int ret;
+	struct ov2710_reg reg_list[3];
+	u8 *b_ptr = info->i2c_trans_buf;
+
+	ov2710_get_coarse_time_regs(reg_list, coarse_time);
+
+	*b_ptr++ = reg_list[0].addr >> 8;
+	*b_ptr++ = reg_list[0].addr & 0xff;
+	*b_ptr++ = reg_list[0].val & 0xff;
+	*b_ptr++ = reg_list[1].val & 0xff;
+	*b_ptr++ = reg_list[2].val & 0xff;
+	ret = ov2710_write_bulk_reg(info->i2c_client, info->i2c_trans_buf, 5);
+
+	return ret;
+}
+
+static int ov2710_set_gain(struct ov2710_info *info, u16 gain)
+{
+	int ret;
+	struct ov2710_reg reg_list;
+
+	ov2710_get_gain_reg(&reg_list, gain);
+
+	ret = ov2710_write_reg(info->i2c_client, reg_list.addr, reg_list.val);
+
+	return ret;
+}
+
+static int ov2710_set_group_hold(struct ov2710_info *info, struct ov2710_ae *ae)
+{
+	int ret;
+	int count = 0;
+	bool groupHoldEnabled = false;
+
+	if (ae->gain_enable)
+		count++;
+	if (ae->coarse_time_enable)
+		count++;
+	if (ae->frame_length_enable)
+		count++;
+	if (count >= 2)
+		groupHoldEnabled = true;
+
+	if (groupHoldEnabled) {
+		ret = ov2710_write_reg(info->i2c_client, 0x3212, 0x01);
+		if (ret)
+			return ret;
+	}
+
+	if (ae->gain_enable)
+		ov2710_set_gain(info, ae->gain);
+	if (ae->coarse_time_enable)
+		ov2710_set_coarse_time(info, ae->coarse_time);
+	if (ae->frame_length_enable)
+		ov2710_set_frame_length(info, ae->frame_length);
+
+	if (groupHoldEnabled) {
+		ret = ov2710_write_reg(info->i2c_client, 0x3212, 0x11);
+		if (ret)
+			return ret;
+
+		ret = ov2710_write_reg(info->i2c_client, 0x3212, 0xa1);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+
+static int ov2710_get_status(struct ov2710_info *info, u8 *status)
+{
+	int err;
+
+	*status = 0;
+	err = ov2710_read_reg(info->i2c_client, 0x002, status);
+	return err;
+}
+
+
+static long ov2710_ioctl(struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	int err;
+	struct ov2710_info *info = file->private_data;
+
+	switch (cmd) {
+	case OV2710_IOCTL_SET_MODE:
+	{
+		struct ov2710_mode mode;
+		if (copy_from_user(&mode,
+				   (const void __user *)arg,
+				   sizeof(struct ov2710_mode))) {
+			return -EFAULT;
+		}
+
+		return ov2710_set_mode(info, &mode);
+	}
+	case OV2710_IOCTL_SET_FRAME_LENGTH:
+		return ov2710_set_frame_length(info, (u32)arg);
+	case OV2710_IOCTL_SET_COARSE_TIME:
+		return ov2710_set_coarse_time(info, (u32)arg);
+	case OV2710_IOCTL_SET_GAIN:
+		return ov2710_set_gain(info, (u16)arg);
+	case OV2710_IOCTL_SET_GROUP_HOLD:
+	{
+		struct ov2710_ae ae;
+		if (copy_from_user(&ae,
+				(const void __user *)arg,
+				sizeof(struct ov2710_ae))) {
+			pr_info("%s %d\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+		return ov2710_set_group_hold(info, &ae);
+	}
+	case OV2710_IOCTL_GET_STATUS:
+	{
+		u8 status;
+
+		err = ov2710_get_status(info, &status);
+		if (err)
+			return err;
+		if (copy_to_user((void __user *)arg, &status,
+				 2)) {
+			return -EFAULT;
+		}
+		return 0;
+	}
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static struct ov2710_info *info;
+
+static int ov2710_open(struct inode *inode, struct file *file)
+{
+	u8 status;
+
+	file->private_data = info;
+	if (info->pdata && info->pdata->power_on)
+		info->pdata->power_on();
+	ov2710_get_status(info, &status);
+	return 0;
+}
+
+int ov2710_release(struct inode *inode, struct file *file)
+{
+	if (info->pdata && info->pdata->power_off)
+		info->pdata->power_off();
+	file->private_data = NULL;
+	return 0;
+}
+
+
+static const struct file_operations ov2710_fileops = {
+	.owner = THIS_MODULE,
+	.open = ov2710_open,
+	.unlocked_ioctl = ov2710_ioctl,
+	.release = ov2710_release,
+};
+
+static struct miscdevice ov2710_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "ov2710",
+	.fops = &ov2710_fileops,
+};
+
+static int ov2710_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int err;
+
+	pr_info("ov2710: probing sensor.\n");
+
+	info = kzalloc(sizeof(struct ov2710_info), GFP_KERNEL);
+	if (!info) {
+		pr_err("ov2710: Unable to allocate memory!\n");
+		return -ENOMEM;
+	}
+
+	err = misc_register(&ov2710_device);
+	if (err) {
+		pr_err("ov2710: Unable to register misc device!\n");
+		kfree(info);
+		return err;
+	}
+
+	info->pdata = client->dev.platform_data;
+	info->i2c_client = client;
+
+	i2c_set_clientdata(client, info);
+	return 0;
+}
+
+static int ov2710_remove(struct i2c_client *client)
+{
+	struct ov2710_info *info;
+	info = i2c_get_clientdata(client);
+	misc_deregister(&ov2710_device);
+	kfree(info);
+	return 0;
+}
+
+static const struct i2c_device_id ov2710_id[] = {
+	{ "ov2710", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, ov2710_id);
+
+static struct i2c_driver ov2710_i2c_driver = {
+	.driver = {
+		.name = "ov2710",
+		.owner = THIS_MODULE,
+	},
+	.probe = ov2710_probe,
+	.remove = ov2710_remove,
+	.id_table = ov2710_id,
+};
+
+static int __init ov2710_init(void)
+{
+	pr_info("ov2710 sensor driver loading\n");
+	return i2c_add_driver(&ov2710_i2c_driver);
+}
+
+static void __exit ov2710_exit(void)
+{
+	i2c_del_driver(&ov2710_i2c_driver);
+}
+
+module_init(ov2710_init);
+module_exit(ov2710_exit);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/video/tegra/ov5640.c b/drivers/media/video/tegra/ov5640.c
new file mode 100644
index 000000000000..26580b0105e3
--- /dev/null
+++ b/drivers/media/video/tegra/ov5640.c
@@ -0,0 +1,493 @@
+/*
+ * ov5640.c - ov5640 sensor driver
+ *
+ * Copyright (c) 2011 - 2012, NVIDIA, All Rights Reserved.
+ *
+ * Contributors:
+ *      Abhinav Sinha <absinha@nvidia.com>
+ *
+ * Leverage soc380.c
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+/**
+ * SetMode Sequence for 640x480. Phase 0. Sensor Dependent.
+ * This sequence should put sensor in streaming mode for 640x480
+ * This is usually given by the FAE or the sensor vendor.
+ */
+
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <media/ov5640.h>
+
+#include "ov5640_tables.h"
+
+/* Focuser single step & full scale transition time truth table
+ * in the format of:
+ *    index	mode		single step transition	full scale transition
+ *	0	0			0			0
+ *	1	1			50uS			51.2mS
+ *	2	1			100uS			102.3mS
+ *	3	1			200uS			204.6mS
+ *	4	1			400uS			409.2mS
+ *	5	1			800uS			818.4mS
+ *	6	1			1600uS			1637.0mS
+ *	7	1			3200uS			3274.0mS
+ *	8	0			0			0
+ *	9	2			50uS			1.1mS
+ *	A	2			100uS			2.2mS
+ *	B	2			200uS			4.4mS
+ *	C	2			400uS			8.8mS
+ *	D	2			800uS			17.6mS
+ *	E	2			1600uS			35.2mS
+ *	F	2			3200uS			70.4mS
+ */
+
+/* pick up the mode index setting and its settle time from the above table */
+#define OV5640_VCM_DACMODE 0x3602
+#define OV5640_TRANSITION_MODE 0x0B
+#define SETTLETIME_MS 5
+
+#define POS_LOW (0)
+#define POS_HIGH (1023)
+#define FPOS_COUNT 1024
+#define FOCAL_LENGTH (10.0f)
+#define FNUMBER (2.8f)
+
+#define SIZEOF_I2C_TRANSBUF 64
+
+struct ov5640_info {
+	int mode;
+	struct miscdevice miscdev_info;
+	struct i2c_client *i2c_client;
+	struct ov5640_platform_data *pdata;
+	struct ov5640_config focuser;
+	int af_fw_loaded;
+	struct kobject *kobj;
+	struct device *dev;
+	u8 i2c_trans_buf[SIZEOF_I2C_TRANSBUF];
+};
+
+static int ov5640_read_reg(struct i2c_client *client, u16 addr, u8 *val)
+{
+	int err;
+	struct i2c_msg msg[2];
+	unsigned char data[3];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 2;
+	msg[0].buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8) (addr >> 8);
+	data[1] = (u8) (addr & 0xff);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = data + 2;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+
+	if (err != 2)
+		return -EINVAL;
+
+	*val = data[2];
+
+	return 0;
+}
+
+#ifdef KERNEL_WARNING
+static int ov5640_write_reg(struct i2c_client *client, u8 addr, u8 value)
+{
+	int count;
+	struct i2c_msg msg[1];
+	unsigned char data[4];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = addr;
+	data[1] = (u8) (addr & 0xff);
+	data[2] = value;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 3;
+	msg[0].buf = data;
+
+	count = i2c_transfer(client->adapter, msg, ARRAY_SIZE(msg));
+	if (count == ARRAY_SIZE(msg))
+		return 0;
+	dev_err(&client->dev,
+		"ov5840: i2c transfer failed, addr: %x, value: %02x\n",
+	       addr, (u32)value);
+	return -EIO;
+}
+#endif
+
+static int ov5640_write_bulk_reg(struct i2c_client *client, u8 *data, int len)
+{
+	int err;
+	struct i2c_msg msg;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = len;
+	msg.buf = data;
+
+	err = i2c_transfer(client->adapter, &msg, 1);
+	if (err == 1)
+		return 0;
+
+	dev_err(&client->dev, "ov5640: i2c transfer failed at %x\n",
+		(int)data[0] << 8 | data[1]);
+
+	return err;
+}
+
+static int ov5640_write_table(struct ov5640_info *info,
+			      struct ov5640_reg table[],
+			      struct ov5640_reg override_list[],
+			      int num_override_regs)
+{
+	int err;
+	struct ov5640_reg *next, *n_next;
+	u8 *b_ptr = info->i2c_trans_buf;
+	unsigned int buf_filled = 0;
+	int i;
+	u16 val;
+
+	for (next = table; next->addr != OV5640_TABLE_END; next++) {
+		if (next->addr == OV5640_TABLE_WAIT_MS) {
+			msleep(next->val);
+			continue;
+		}
+
+		val = next->val;
+
+		/* When an override list is passed in, replace the reg */
+		/* value to write if the reg is in the list            */
+		if (override_list) {
+			for (i = 0; i < num_override_regs; i++) {
+				if (next->addr == override_list[i].addr) {
+					val = override_list[i].val;
+					break;
+				}
+			}
+		}
+
+		if (!buf_filled) {
+			b_ptr = info->i2c_trans_buf;
+			*b_ptr++ = next->addr >> 8;
+			*b_ptr++ = next->addr & 0xff;
+			buf_filled = 2;
+		}
+		*b_ptr++ = val;
+		buf_filled++;
+
+		n_next = next + 1;
+		if (n_next->addr != OV5640_TABLE_END &&
+			n_next->addr != OV5640_TABLE_WAIT_MS &&
+			buf_filled < SIZEOF_I2C_TRANSBUF &&
+			n_next->addr == next->addr + 1) {
+			continue;
+		}
+
+		err = ov5640_write_bulk_reg(info->i2c_client,
+			info->i2c_trans_buf, buf_filled);
+		if (err)
+			return err;
+
+		buf_filled = 0;
+	}
+	return 0;
+}
+
+static int ov5640_set_mode(struct ov5640_info *info, struct ov5640_mode *mode)
+{
+	int sensor_mode;
+	int err;
+
+	dev_info(info->dev, "%s: xres %u yres %u\n",
+			__func__, mode->xres, mode->yres);
+	if (!info->af_fw_loaded) {
+		err = ov5640_write_table(info, tbl_af_firmware, NULL, 0);
+		if (err)
+			return err;
+		info->af_fw_loaded = 1;
+	}
+
+	if (mode->xres == 2592 && mode->yres == 1944)
+		sensor_mode = OV5640_MODE_2592x1944;
+	else if (mode->xres == 1920 && mode->yres == 1080)
+		sensor_mode = OV5640_MODE_1920x1080;
+	else if (mode->xres == 1296 && mode->yres == 964)
+		sensor_mode = OV5640_MODE_1296x972;
+	else {
+		dev_info(info->dev, "%s: invalid resolution: %d %d\n",
+				__func__, mode->xres, mode->yres);
+		return -EINVAL;
+	}
+
+	err = ov5640_write_table(info, mode_table[sensor_mode],
+		NULL, 0);
+	if (err)
+		return err;
+
+	info->mode = sensor_mode;
+	return 0;
+}
+
+static int ov5640_set_af_mode(struct ov5640_info *info, u8 mode)
+{
+	dev_info(info->dev, "%s: mode %d\n", __func__, mode);
+	if (mode == OV5640_AF_INIFINITY)
+		return ov5640_write_table(info, tbl_release_focus, NULL, 0);
+
+	if (mode == OV5640_AF_TRIGGER)
+		return ov5640_write_table(info, tbl_single_focus, NULL, 0);
+
+	return -EINVAL;
+}
+
+static int ov5640_get_af_status(struct ov5640_info *info, u8 *val)
+{
+	int err;
+
+	err = ov5640_read_reg(info->i2c_client, 0x3023, val);
+	if (err)
+		return -EINVAL;
+
+	dev_info(info->dev, "%s: value %02x\n", __func__, (u32)val);
+	return 0;
+}
+
+static int ov5640_set_position(struct ov5640_info *info, u32 position)
+{
+	u8 data[4];
+
+	if (position < info->focuser.pos_low ||
+	    position > info->focuser.pos_high)
+		return -EINVAL;
+
+	data[0] = (OV5640_VCM_DACMODE >> 8) & 0xff;
+	data[1] = OV5640_VCM_DACMODE & 0xff;
+	data[2] = ((position & 0xf) << 4) | OV5640_TRANSITION_MODE;
+	data[3] = (position * 0x3f0) >> 4;
+	return ov5640_write_bulk_reg(info->i2c_client, data, 4);
+}
+
+static int ov5640_set_power(struct ov5640_info *info, u32 level)
+{
+	switch (level) {
+	case OV5640_POWER_LEVEL_OFF:
+	case OV5640_POWER_LEVEL_SUS:
+		if (info->pdata && info->pdata->power_off)
+			info->pdata->power_off();
+		info->af_fw_loaded = 0;
+		info->mode = 0;
+		break;
+	case OV5640_POWER_LEVEL_ON:
+		if (info->pdata && info->pdata->power_on)
+			info->pdata->power_on();
+		break;
+	default:
+		dev_err(info->dev, "unknown power level %d.\n", level);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static long ov5640_ioctl(struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	struct ov5640_info *info = file->private_data;
+
+	switch (cmd) {
+	case OV5640_IOCTL_SET_SENSOR_MODE:
+	{
+		struct ov5640_mode mode;
+		if (copy_from_user(&mode,
+				   (const void __user *)arg,
+				   sizeof(struct ov5640_mode))) {
+			return -EFAULT;
+		}
+
+		return ov5640_set_mode(info, &mode);
+	}
+	case OV5640_IOCTL_GET_CONFIG:
+	{
+		if (copy_to_user((void __user *) arg,
+				 &info->focuser,
+				 sizeof(info->focuser))) {
+			dev_err(info->dev, "%s: 0x%x\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+
+		break;
+	}
+	case OV5640_IOCTL_GET_AF_STATUS:
+	{
+		int err;
+		u8 val;
+
+		if (!info->af_fw_loaded) {
+			dev_err(info->dev, "OV5640 AF fw not loaded!\n");
+			break;
+		}
+
+		err = ov5640_get_af_status(info, &val);
+		if (err)
+			return err;
+
+		if (copy_to_user((void __user *) arg,
+				 &val, sizeof(val))) {
+			dev_err(info->dev, "%s: 0x%x\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+		break;
+	}
+	case OV5640_IOCTL_SET_AF_MODE:
+		if (!info->af_fw_loaded) {
+			dev_err(info->dev, "OV5640 AF fw not loaded!\n");
+			break;
+		}
+		return ov5640_set_af_mode(info, (u8)arg);
+	case OV5640_IOCTL_POWER_LEVEL:
+		return ov5640_set_power(info, (u32)arg);
+	case OV5640_IOCTL_SET_FPOSITION:
+		return ov5640_set_position(info, (u32)arg);
+	case OV5640_IOCTL_GET_SENSOR_STATUS:
+	{
+		u8 status = 0;
+		if (copy_to_user((void __user *)arg, &status,
+				 1)) {
+			dev_info(info->dev, "%s %d\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+		return 0;
+	}
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int ov5640_open(struct inode *inode, struct file *file)
+{
+	struct miscdevice *miscdev = file->private_data;
+	struct ov5640_info *info;
+
+	pr_info("%s\n", __func__);
+	if (!miscdev) {
+		pr_err("miscdev == NULL\n");
+		return -1;
+	}
+	info = container_of(miscdev, struct ov5640_info, miscdev_info);
+	file->private_data = info;
+
+	return 0;
+}
+
+int ov5640_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	pr_info("%s\n", __func__);
+	return 0;
+}
+
+static const struct file_operations ov5640_fileops = {
+	.owner = THIS_MODULE,
+	.open = ov5640_open,
+	.unlocked_ioctl = ov5640_ioctl,
+	.release = ov5640_release,
+};
+
+static struct miscdevice ov5640_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "ov5640",
+	.fops = &ov5640_fileops,
+};
+
+static int ov5640_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct ov5640_info *info;
+	int err;
+
+	dev_info(&client->dev, "ov5640: probing sensor.\n");
+
+	info = devm_kzalloc(&client->dev,
+			sizeof(struct ov5640_info), GFP_KERNEL);
+	if (!info) {
+		dev_err(&client->dev, "ov5640: Unable to allocate memory!\n");
+		return -ENOMEM;
+	}
+
+	memcpy(&(info->miscdev_info),
+		&ov5640_device,
+		sizeof(struct miscdevice));
+
+	err = misc_register(&(info->miscdev_info));
+	if (err) {
+		dev_err(&client->dev,
+			"ov5640: Unable to register misc device!\n");
+		devm_kfree(&client->dev, info);
+		return err;
+	}
+
+	info->dev = &client->dev;
+	info->pdata = client->dev.platform_data;
+	info->i2c_client = client;
+	info->focuser.settle_time = SETTLETIME_MS;
+	info->focuser.focal_length = FOCAL_LENGTH;
+	info->focuser.fnumber = FNUMBER;
+	info->focuser.pos_low = POS_LOW;
+	info->focuser.pos_high = POS_HIGH;
+
+	i2c_set_clientdata(client, info);
+	return 0;
+}
+
+static int ov5640_remove(struct i2c_client *client)
+{
+	struct ov5640_info *info;
+	info = i2c_get_clientdata(client);
+	misc_deregister(&ov5640_device);
+	return 0;
+}
+
+static const struct i2c_device_id ov5640_id[] = {
+	{ "ov5640", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, ov5640_id);
+
+static struct i2c_driver ov5640_i2c_driver = {
+	.driver = {
+		.name = "ov5640",
+		.owner = THIS_MODULE,
+	},
+	.probe = ov5640_probe,
+	.remove = ov5640_remove,
+	.id_table = ov5640_id,
+};
+
+module_i2c_driver(ov5640_i2c_driver);
diff --git a/drivers/media/video/tegra/ov5640_tables.h b/drivers/media/video/tegra/ov5640_tables.h
new file mode 100644
index 000000000000..94cf1da31ec2
--- /dev/null
+++ b/drivers/media/video/tegra/ov5640_tables.h
@@ -0,0 +1,4582 @@
+/*
+ * ov5640_tables.h - table header for YUV camera sensor OV5640 driver.
+ *
+ * Copyright (C) 2012 NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms 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
+ */
+
+#ifndef OV5640_I2C_TABLES
+#define OV5640_I2C_TABLES
+
+struct ov5640_reg {
+	u16 addr;
+	u16 val;
+};
+
+#define OV5640_TABLE_WAIT_MS 0
+#define OV5640_TABLE_END 1
+
+static struct ov5640_reg mode_2592x1944[] = {
+	/* PLL Control MIPI bit rate/lane = 672MHz, 16-bit mode.
+	 * Output size: 2608x1948 (0, 0) - (2623, 1951),
+	 * Line Length = 2844, Frame Length = 1968
+	 */
+	{0x3103, 0x11},
+	{0x3008, 0x82},
+	{OV5640_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42},
+	{0x3103, 0x03},
+	{0x3017, 0x00},
+	{0x3018, 0x00},
+	{0x3034, 0x18},
+	{0x3035, 0x11},
+	{0x3036, 0x54},
+	{0x3037, 0x13},
+	{0x3108, 0x01},
+	{0x3630, 0x36},
+	{0x3631, 0x0e},
+	{0x3632, 0xe2},
+	{0x3633, 0x12},
+	{0x3621, 0xe0},
+	{0x3704, 0xa0},
+	{0x3703, 0x5a},
+	{0x3715, 0x78},
+	{0x3717, 0x01},
+	{0x370b, 0x60},
+	{0x3705, 0x1a},
+	{0x3905, 0x02},
+	{0x3906, 0x10},
+	{0x3901, 0x0a},
+	{0x3731, 0x12},
+	{0x3600, 0x08},
+	{0x3601, 0x33},
+	{0x302d, 0x60},
+	{0x3620, 0x52},
+	{0x371b, 0x20},
+	{0x471c, 0x50},
+	{0x3a13, 0x43},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3635, 0x13},
+	{0x3636, 0x03},
+	{0x3634, 0x40},
+	{0x3622, 0x01},
+	{0x3c01, 0x34},
+	{0x3c04, 0x28},
+	{0x3c05, 0x98},
+	{0x3c06, 0x00},
+	{0x3c07, 0x07},
+	{0x3c08, 0x00},
+	{0x3c09, 0x1c},
+	{0x3c0a, 0x9c},
+	{0x3c0b, 0x40},
+	{0x3820, 0x40},
+	{0x3821, 0x06},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0x9f},
+	{0x3808, 0x0a},
+	{0x3809, 0x30},
+	{0x380a, 0x07},
+	{0x380b, 0x9c},
+	{0x380c, 0x0b},
+	{0x380d, 0x1c},
+	{0x380e, 0x07},
+	{0x380f, 0xb0},
+	{0x3810, 0x00},
+	{0x3811, 0x08},
+	{0x3812, 0x00},
+	{0x3813, 0x02},
+	{0x3618, 0x04},
+	{0x3612, 0x2b},
+	{0x3708, 0x64},
+	{0x3709, 0x12},
+	{0x370c, 0x00},
+	{0x3a02, 0x07},
+	{0x3a03, 0xb0},
+	{0x3a08, 0x01},
+	{0x3a09, 0x27},
+	{0x3a0a, 0x00},
+	{0x3a0b, 0xf6},
+	{0x3a0e, 0x06},
+	{0x3a0d, 0x08},
+	{0x3a14, 0x07},
+	{0x3a15, 0xb0},
+	{0x4001, 0x02},
+	{0x4004, 0x06},
+	{0x3000, 0x00},
+	{0x3002, 0x1c},
+	{0x3004, 0xff},
+	{0x3006, 0xc3},
+	{0x300e, 0x45},
+	{0x302e, 0x08},
+	{0x4300, 0x32},
+	{0x4800, 0x24},
+	{0x4837, 0x0a},
+	{0x501f, 0x00},
+	{0x440e, 0x00},
+	{0x5000, 0xa7},
+	{0x5001, 0x83},
+	{0x5180, 0xff},
+	{0x5181, 0xf2},
+	{0x5182, 0x00},
+	{0x5183, 0x14},
+	{0x5184, 0x25},
+	{0x5185, 0x24},
+	{0x5186, 0x09},
+	{0x5187, 0x09},
+	{0x5188, 0x09},
+	{0x5189, 0x75},
+	{0x518a, 0x54},
+	{0x518b, 0xe0},
+	{0x518c, 0xb2},
+	{0x518d, 0x42},
+	{0x518e, 0x3d},
+	{0x518f, 0x56},
+	{0x5190, 0x46},
+	{0x5191, 0xf8},
+	{0x5192, 0x04},
+	{0x5193, 0x70},
+	{0x5194, 0xf0},
+	{0x5195, 0xf0},
+	{0x5196, 0x03},
+	{0x5197, 0x01},
+	{0x5198, 0x04},
+	{0x5199, 0x12},
+	{0x519a, 0x04},
+	{0x519b, 0x00},
+	{0x519c, 0x06},
+	{0x519d, 0x82},
+	{0x519e, 0x38},
+	{0x5381, 0x1e},
+	{0x5382, 0x5b},
+	{0x5383, 0x08},
+	{0x5384, 0x0a},
+	{0x5385, 0x7e},
+	{0x5386, 0x88},
+	{0x5387, 0x7c},
+	{0x5388, 0x6c},
+	{0x5389, 0x10},
+	{0x538a, 0x01},
+	{0x538b, 0x98},
+	{0x5300, 0x08},
+	{0x5301, 0x30},
+	{0x5302, 0x10},
+	{0x5303, 0x00},
+	{0x5304, 0x08},
+	{0x5305, 0x30},
+	{0x5306, 0x08},
+	{0x5307, 0x16},
+	{0x5309, 0x08},
+	{0x530a, 0x30},
+	{0x530b, 0x04},
+	{0x530c, 0x06},
+	{0x5480, 0x01},
+	{0x5481, 0x08},
+	{0x5482, 0x14},
+	{0x5483, 0x28},
+	{0x5484, 0x51},
+	{0x5485, 0x65},
+	{0x5486, 0x71},
+	{0x5487, 0x7d},
+	{0x5488, 0x87},
+	{0x5489, 0x91},
+	{0x548a, 0x9a},
+	{0x548b, 0xaa},
+	{0x548c, 0xb8},
+	{0x548d, 0xcd},
+	{0x548e, 0xdd},
+	{0x548f, 0xea},
+	{0x5490, 0x1d},
+	{0x5580, 0x02},
+	{0x5583, 0x40},
+	{0x5584, 0x10},
+	{0x5589, 0x10},
+	{0x558a, 0x00},
+	{0x558b, 0xf8},
+	{0x5800, 0x23},
+	{0x5801, 0x14},
+	{0x5802, 0x0f},
+	{0x5803, 0x0f},
+	{0x5804, 0x12},
+	{0x5805, 0x26},
+	{0x5806, 0x0c},
+	{0x5807, 0x08},
+	{0x5808, 0x05},
+	{0x5809, 0x05},
+	{0x580a, 0x08},
+	{0x580b, 0x0d},
+	{0x580c, 0x08},
+	{0x580d, 0x03},
+	{0x580e, 0x00},
+	{0x580f, 0x00},
+	{0x5810, 0x03},
+	{0x5811, 0x09},
+	{0x5812, 0x07},
+	{0x5813, 0x03},
+	{0x5814, 0x00},
+	{0x5815, 0x01},
+	{0x5816, 0x03},
+	{0x5817, 0x08},
+	{0x5818, 0x0d},
+	{0x5819, 0x08},
+	{0x581a, 0x05},
+	{0x581b, 0x06},
+	{0x581c, 0x08},
+	{0x581d, 0x0e},
+	{0x581e, 0x29},
+	{0x581f, 0x17},
+	{0x5820, 0x11},
+	{0x5821, 0x11},
+	{0x5822, 0x15},
+	{0x5823, 0x28},
+	{0x5824, 0x46},
+	{0x5825, 0x26},
+	{0x5826, 0x08},
+	{0x5827, 0x26},
+	{0x5828, 0x64},
+	{0x5829, 0x26},
+	{0x582a, 0x24},
+	{0x582b, 0x22},
+	{0x582c, 0x24},
+	{0x582d, 0x24},
+	{0x582e, 0x06},
+	{0x582f, 0x22},
+	{0x5830, 0x40},
+	{0x5831, 0x42},
+	{0x5832, 0x24},
+	{0x5833, 0x26},
+	{0x5834, 0x24},
+	{0x5835, 0x22},
+	{0x5836, 0x22},
+	{0x5837, 0x26},
+	{0x5838, 0x44},
+	{0x5839, 0x24},
+	{0x583a, 0x26},
+	{0x583b, 0x28},
+	{0x583c, 0x42},
+	{0x583d, 0xce},
+	{0x5025, 0x00},
+	{0x3a0f, 0x30},
+	{0x3a10, 0x28},
+	{0x3a1b, 0x30},
+	{0x3a1e, 0x26},
+	{0x3a11, 0x60},
+	{0x3a1f, 0x14},
+	{0x3008, 0x02},
+	{OV5640_TABLE_END, 0x0000}
+};
+
+static struct ov5640_reg mode_1920x1080[] = {
+	/* PLL Control MIPI bit rate/lane = 672MHz, 16-bit mode.
+	 * Output size: 1936x1096 (336, 426) - (2287, 1529),
+	 * Line Length = 2500, Frame Length = 1120.
+	 */
+	{0x3103, 0x11},
+	{0x3008, 0x82},
+	{OV5640_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42},
+	{0x3103, 0x03},
+	{0x3017, 0x00},
+	{0x3018, 0x00},
+	{0x3034, 0x18},
+	{0x3035, 0x11},
+	{0x3036, 0x54},
+	{0x3037, 0x13},
+	{0x3108, 0x01},
+	{0x3630, 0x36},
+	{0x3631, 0x0e},
+	{0x3632, 0xe2},
+	{0x3633, 0x12},
+	{0x3621, 0xe0},
+	{0x3704, 0xa0},
+	{0x3703, 0x5a},
+	{0x3715, 0x78},
+	{0x3717, 0x01},
+	{0x370b, 0x60},
+	{0x3705, 0x1a},
+	{0x3905, 0x02},
+	{0x3906, 0x10},
+	{0x3901, 0x0a},
+	{0x3731, 0x12},
+	{0x3600, 0x08},
+	{0x3601, 0x33},
+	{0x302d, 0x60},
+	{0x3620, 0x52},
+	{0x371b, 0x20},
+	{0x471c, 0x50},
+	{0x3a13, 0x43},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3635, 0x13},
+	{0x3636, 0x03},
+	{0x3634, 0x40},
+	{0x3622, 0x01},
+	{0x3c01, 0x34},
+	{0x3c04, 0x28},
+	{0x3c05, 0x98},
+	{0x3c06, 0x00},
+	{0x3c07, 0x07},
+	{0x3c08, 0x00},
+	{0x3c09, 0x1c},
+	{0x3c0a, 0x9c},
+	{0x3c0b, 0x40},
+	{0x3820, 0x40},
+	{0x3821, 0x06},
+	{0x3814, 0x11},
+	{0x3815, 0x11},
+	{0x3800, 0x01},
+	{0x3801, 0x50},
+	{0x3802, 0x01},
+	{0x3803, 0xaa},
+	{0x3804, 0x08},
+	{0x3805, 0xef},
+	{0x3806, 0x05},
+	{0x3807, 0xf9},
+	{0x3808, 0x07},
+	{0x3809, 0x90},
+	{0x380a, 0x04},
+	{0x380b, 0x48},
+	{0x380c, 0x09},
+	{0x380d, 0xc4},
+	{0x380e, 0x04},
+	{0x380f, 0x60},
+	{0x3810, 0x00},
+	{0x3811, 0x08},
+	{0x3812, 0x00},
+	{0x3813, 0x04},
+	{0x3618, 0x04},
+	{0x3612, 0x2b},
+	{0x3708, 0x64},
+	{0x3709, 0x12},
+	{0x370c, 0x00},
+	{0x3a02, 0x04},
+	{0x3a03, 0x60},
+	{0x3a08, 0x01},
+	{0x3a09, 0x50},
+	{0x3a0a, 0x01},
+	{0x3a0b, 0x18},
+	{0x3a0e, 0x03},
+	{0x3a0d, 0x04},
+	{0x3a14, 0x04},
+	{0x3a15, 0x60},
+	{0x4001, 0x02},
+	{0x4004, 0x06},
+	{0x3000, 0x00},
+	{0x3002, 0x1c},
+	{0x3004, 0xff},
+	{0x3006, 0xc3},
+	{0x300e, 0x45},
+	{0x302e, 0x08},
+	{0x4300, 0x32},
+	{0x501f, 0x00},
+	{0x4713, 0x02},
+	{0x4407, 0x04},
+	{0x440e, 0x00},
+	{0x460b, 0x37},
+	{0x460c, 0x20},
+	{0x4800, 0x24},
+	{0x4837, 0x0a},
+	{0x3824, 0x04},
+	{0x5000, 0xa7},
+	{0x5001, 0x83},
+	{0x5180, 0xff},
+	{0x5181, 0xf2},
+	{0x5182, 0x00},
+	{0x5183, 0x14},
+	{0x5184, 0x25},
+	{0x5185, 0x24},
+	{0x5186, 0x09},
+	{0x5187, 0x09},
+	{0x5188, 0x09},
+	{0x5189, 0x75},
+	{0x518a, 0x54},
+	{0x518b, 0xe0},
+	{0x518c, 0xb2},
+	{0x518d, 0x42},
+	{0x518e, 0x3d},
+	{0x518f, 0x56},
+	{0x5190, 0x46},
+	{0x5191, 0xf8},
+	{0x5192, 0x04},
+	{0x5193, 0x70},
+	{0x5194, 0xf0},
+	{0x5195, 0xf0},
+	{0x5196, 0x03},
+	{0x5197, 0x01},
+	{0x5198, 0x04},
+	{0x5199, 0x12},
+	{0x519a, 0x04},
+	{0x519b, 0x00},
+	{0x519c, 0x06},
+	{0x519d, 0x82},
+	{0x519e, 0x38},
+	{0x5381, 0x1e},
+	{0x5382, 0x5b},
+	{0x5383, 0x08},
+	{0x5384, 0x0a},
+	{0x5385, 0x7e},
+	{0x5386, 0x88},
+	{0x5387, 0x7c},
+	{0x5388, 0x6c},
+	{0x5389, 0x10},
+	{0x538a, 0x01},
+	{0x538b, 0x98},
+	{0x5300, 0x08},
+	{0x5301, 0x30},
+	{0x5302, 0x10},
+	{0x5303, 0x00},
+	{0x5304, 0x08},
+	{0x5305, 0x30},
+	{0x5306, 0x08},
+	{0x5307, 0x16},
+	{0x5309, 0x08},
+	{0x530a, 0x30},
+	{0x530b, 0x04},
+	{0x530c, 0x06},
+	{0x5480, 0x01},
+	{0x5481, 0x08},
+	{0x5482, 0x14},
+	{0x5483, 0x28},
+	{0x5484, 0x51},
+	{0x5485, 0x65},
+	{0x5486, 0x71},
+	{0x5487, 0x7d},
+
+	{0x5488, 0x87},
+	{0x5489, 0x91},
+	{0x548a, 0x9a},
+	{0x548b, 0xaa},
+	{0x548c, 0xb8},
+	{0x548d, 0xcd},
+	{0x548e, 0xdd},
+	{0x548f, 0xea},
+	{0x5490, 0x1d},
+	{0x5580, 0x02},
+	{0x5583, 0x40},
+	{0x5584, 0x10},
+	{0x5589, 0x10},
+	{0x558a, 0x00},
+	{0x558b, 0xf8},
+	{0x5800, 0x23},
+	{0x5801, 0x14},
+	{0x5802, 0x0f},
+	{0x5803, 0x0f},
+	{0x5804, 0x12},
+	{0x5805, 0x26},
+	{0x5806, 0x0c},
+	{0x5807, 0x08},
+	{0x5808, 0x05},
+	{0x5809, 0x05},
+	{0x580a, 0x08},
+	{0x580b, 0x0d},
+	{0x580c, 0x08},
+	{0x580d, 0x03},
+	{0x580e, 0x00},
+	{0x580f, 0x00},
+	{0x5810, 0x03},
+	{0x5811, 0x09},
+	{0x5812, 0x07},
+	{0x5813, 0x03},
+	{0x5814, 0x00},
+	{0x5815, 0x01},
+	{0x5816, 0x03},
+	{0x5817, 0x08},
+	{0x5818, 0x0d},
+	{0x5819, 0x08},
+	{0x581a, 0x05},
+	{0x581b, 0x06},
+	{0x581c, 0x08},
+	{0x581d, 0x0e},
+	{0x581e, 0x29},
+	{0x581f, 0x17},
+	{0x5820, 0x11},
+	{0x5821, 0x11},
+	{0x5822, 0x15},
+	{0x5823, 0x28},
+	{0x5824, 0x46},
+	{0x5825, 0x26},
+	{0x5826, 0x08},
+	{0x5827, 0x26},
+	{0x5828, 0x64},
+	{0x5829, 0x26},
+	{0x582a, 0x24},
+	{0x582b, 0x22},
+	{0x582c, 0x24},
+	{0x582d, 0x24},
+	{0x582e, 0x06},
+	{0x582f, 0x22},
+	{0x5830, 0x40},
+	{0x5831, 0x42},
+	{0x5832, 0x24},
+	{0x5833, 0x26},
+	{0x5834, 0x24},
+	{0x5835, 0x22},
+	{0x5836, 0x22},
+	{0x5837, 0x26},
+	{0x5838, 0x44},
+	{0x5839, 0x24},
+	{0x583a, 0x26},
+	{0x583b, 0x28},
+	{0x583c, 0x42},
+	{0x583d, 0xce},
+	{0x5025, 0x00},
+	{0x3a0f, 0x30},
+	{0x3a10, 0x28},
+	{0x3a1b, 0x30},
+	{0x3a1e, 0x26},
+	{0x3a11, 0x60},
+	{0x3a1f, 0x14},
+	{0x3008, 0x02},
+	{OV5640_TABLE_END, 0x0000}
+};
+
+static struct ov5640_reg mode_1296x972[] = {
+	/* PLL Control MIPI bit rate/lane = 448MHz, 16-bit mode.
+	 * Output size: 1304x972 (0, 0) - (2623, 1951),
+	 * Line Length = 1886, Frame Length = 990.
+	 */
+	{0x3103, 0x11},
+	{0x3008, 0x82},
+	{OV5640_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42},
+	{0x3103, 0x03},
+	{0x3017, 0x00},
+	{0x3018, 0x00},
+	{0x3034, 0x18},
+	{0x3035, 0x21},
+	{0x3036, 0x70},
+	{0x3037, 0x13},
+	{0x3108, 0x01},
+	{0x3630, 0x36},
+	{0x3631, 0x0e},
+	{0x3632, 0xe2},
+	{0x3633, 0x12},
+	{0x3621, 0xe0},
+	{0x3704, 0xa0},
+	{0x3703, 0x5a},
+	{0x3715, 0x78},
+	{0x3717, 0x01},
+	{0x370b, 0x60},
+	{0x3705, 0x1a},
+	{0x3905, 0x02},
+	{0x3906, 0x10},
+	{0x3901, 0x0a},
+	{0x3731, 0x12},
+	{0x3600, 0x08},
+	{0x3601, 0x33},
+	{0x302d, 0x60},
+	{0x3620, 0x52},
+	{0x371b, 0x20},
+	{0x471c, 0x50},
+	{0x3a13, 0x43},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3635, 0x13},
+	{0x3636, 0x03},
+	{0x3634, 0x40},
+	{0x3622, 0x01},
+	{0x3c01, 0x34},
+	{0x3c04, 0x28},
+	{0x3c05, 0x98},
+	{0x3c06, 0x00},
+	{0x3c07, 0x07},
+	{0x3c08, 0x00},
+	{0x3c09, 0x1c},
+	{0x3c0a, 0x9c},
+	{0x3c0b, 0x40},
+	{0x3820, 0x41},
+	{0x3821, 0x07},
+	{0x3814, 0x31},
+	{0x3815, 0x31},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x0a},
+	{0x3805, 0x3f},
+	{0x3806, 0x07},
+	{0x3807, 0x9f},
+	{0x3808, 0x05},
+	{0x3809, 0x18},
+	{0x380a, 0x03},
+	{0x380b, 0xcc},
+	{0x380c, 0x07},
+	{0x380d, 0x5e},
+	{0x380e, 0x03},
+	{0x380f, 0xde},
+	{0x3810, 0x00},
+	{0x3811, 0x06},
+	{0x3812, 0x00},
+	{0x3813, 0x02},
+	{0x3618, 0x00},
+	{0x3612, 0x29},
+	{0x3708, 0x62},
+	{0x3709, 0x52},
+	{0x370c, 0x03},
+	{0x3a02, 0x03},
+	{0x3a03, 0xd8},
+	{0x3a08, 0x01},
+	{0x3a09, 0x27},
+	{0x3a0a, 0x00},
+	{0x3a0b, 0xf6},
+	{0x3a0e, 0x03},
+	{0x3a0d, 0x04},
+	{0x3a14, 0x03},
+	{0x3a15, 0xd8},
+	{0x4001, 0x02},
+	{0x4004, 0x02},
+	{0x3000, 0x00},
+	{0x3002, 0x1c},
+	{0x3004, 0xff},
+	{0x3006, 0xc3},
+	{0x300e, 0x45},
+	{0x302e, 0x08},
+	{0x4300, 0x32},
+	{0x501f, 0x00},
+	{0x4713, 0x02},
+	{0x4407, 0x04},
+	{0x440e, 0x00},
+	{0x460b, 0x37},
+	{0x460c, 0x20},
+	{0x4800, 0x24},
+	{0x4837, 0x10},
+	{0x3824, 0x04},
+	{0x5000, 0xa7},
+	{0x5001, 0x83},
+	{0x5180, 0xff},
+	{0x5181, 0xf2},
+	{0x5182, 0x00},
+	{0x5183, 0x14},
+	{0x5184, 0x25},
+	{0x5185, 0x24},
+	{0x5186, 0x09},
+	{0x5187, 0x09},
+	{0x5188, 0x09},
+	{0x5189, 0x75},
+	{0x518a, 0x54},
+	{0x518b, 0xe0},
+	{0x518c, 0xb2},
+	{0x518d, 0x42},
+	{0x518e, 0x3d},
+	{0x518f, 0x56},
+	{0x5190, 0x46},
+	{0x5191, 0xf8},
+	{0x5192, 0x04},
+	{0x5193, 0x70},
+	{0x5194, 0xf0},
+	{0x5195, 0xf0},
+	{0x5196, 0x03},
+	{0x5197, 0x01},
+	{0x5198, 0x04},
+	{0x5199, 0x12},
+	{0x519a, 0x04},
+	{0x519b, 0x00},
+	{0x519c, 0x06},
+	{0x519d, 0x82},
+	{0x519e, 0x38},
+	{0x5381, 0x1e},
+	{0x5382, 0x5b},
+	{0x5383, 0x08},
+	{0x5384, 0x0a},
+	{0x5385, 0x7e},
+	{0x5386, 0x88},
+	{0x5387, 0x7c},
+	{0x5388, 0x6c},
+	{0x5389, 0x10},
+	{0x538a, 0x01},
+	{0x538b, 0x98},
+	{0x5300, 0x08},
+	{0x5301, 0x30},
+	{0x5302, 0x10},
+	{0x5303, 0x00},
+	{0x5304, 0x08},
+	{0x5305, 0x30},
+	{0x5306, 0x08},
+	{0x5307, 0x16},
+	{0x5309, 0x08},
+	{0x530a, 0x30},
+	{0x530b, 0x04},
+	{0x530c, 0x06},
+	{0x5480, 0x01},
+	{0x5481, 0x08},
+	{0x5482, 0x14},
+	{0x5483, 0x28},
+	{0x5484, 0x51},
+	{0x5485, 0x65},
+	{0x5486, 0x71},
+	{0x5487, 0x7d},
+	{0x5488, 0x87},
+	{0x5489, 0x91},
+	{0x548a, 0x9a},
+	{0x548b, 0xaa},
+	{0x548c, 0xb8},
+	{0x548d, 0xcd},
+	{0x548e, 0xdd},
+	{0x548f, 0xea},
+	{0x5490, 0x1d},
+	{0x5580, 0x02},
+	{0x5583, 0x40},
+	{0x5584, 0x10},
+	{0x5589, 0x10},
+	{0x558a, 0x00},
+	{0x558b, 0xf8},
+	{0x5800, 0x23},
+	{0x5801, 0x14},
+	{0x5802, 0x0f},
+	{0x5803, 0x0f},
+	{0x5804, 0x12},
+	{0x5805, 0x26},
+	{0x5806, 0x0c},
+	{0x5807, 0x08},
+	{0x5808, 0x05},
+	{0x5809, 0x05},
+	{0x580a, 0x08},
+	{0x580b, 0x0d},
+	{0x580c, 0x08},
+	{0x580d, 0x03},
+	{0x580e, 0x00},
+	{0x580f, 0x00},
+	{0x5810, 0x03},
+	{0x5811, 0x09},
+	{0x5812, 0x07},
+	{0x5813, 0x03},
+	{0x5814, 0x00},
+	{0x5815, 0x01},
+	{0x5816, 0x03},
+	{0x5817, 0x08},
+	{0x5818, 0x0d},
+	{0x5819, 0x08},
+	{0x581a, 0x05},
+	{0x581b, 0x06},
+	{0x581c, 0x08},
+	{0x581d, 0x0e},
+	{0x581e, 0x29},
+	{0x581f, 0x17},
+	{0x5820, 0x11},
+	{0x5821, 0x11},
+	{0x5822, 0x15},
+	{0x5823, 0x28},
+	{0x5824, 0x46},
+	{0x5825, 0x26},
+	{0x5826, 0x08},
+	{0x5827, 0x26},
+	{0x5828, 0x64},
+	{0x5829, 0x26},
+	{0x582a, 0x24},
+	{0x582b, 0x22},
+	{0x582c, 0x24},
+	{0x582d, 0x24},
+	{0x582e, 0x06},
+	{0x582f, 0x22},
+	{0x5830, 0x40},
+	{0x5831, 0x42},
+	{0x5832, 0x24},
+	{0x5833, 0x26},
+	{0x5834, 0x24},
+	{0x5835, 0x22},
+	{0x5836, 0x22},
+	{0x5837, 0x26},
+	{0x5838, 0x44},
+	{0x5839, 0x24},
+	{0x583a, 0x26},
+	{0x583b, 0x28},
+	{0x583c, 0x42},
+	{0x583d, 0xce},
+	{0x5025, 0x00},
+	{0x3a0f, 0x30},
+	{0x3a10, 0x28},
+	{0x3a1b, 0x30},
+	{0x3a1e, 0x26},
+	{0x3a11, 0x60},
+	{0x3a1f, 0x14},
+	{0x3008, 0x02},
+	{OV5640_TABLE_END, 0x0000}
+};
+
+enum {
+	OV5640_MODE_2592x1944 = 1,
+	OV5640_MODE_1920x1080,
+	OV5640_MODE_1296x972,
+};
+
+static struct ov5640_reg *mode_table[] = {
+	[OV5640_MODE_2592x1944] = mode_2592x1944,
+	[OV5640_MODE_1920x1080] = mode_1920x1080,
+	[OV5640_MODE_1296x972] = mode_1296x972,
+};
+
+static struct ov5640_reg tbl_af_firmware[] = {
+	{0x3000, 0x20},
+	{0x8000, 0x02},
+	{0x8001, 0x0b},
+	{0x8002, 0x1f},
+	{0x8003, 0x02},
+	{0x8004, 0x07},
+	{0x8005, 0x89},
+	{0x8006, 0xc2},
+	{0x8007, 0x01},
+	{0x8008, 0x22},
+	{0x8009, 0x22},
+	{0x800a, 0x00},
+	{0x800b, 0x02},
+	{0x800c, 0x0b},
+	{0x800d, 0x09},
+	{0x800e, 0xe5},
+	{0x800f, 0x40},
+	{0x8010, 0x60},
+	{0x8011, 0x03},
+	{0x8012, 0x02},
+	{0x8013, 0x00},
+	{0x8014, 0x97},
+	{0x8015, 0xf5},
+	{0x8016, 0x3f},
+	{0x8017, 0xd2},
+	{0x8018, 0x34},
+	{0x8019, 0x75},
+	{0x801a, 0x29},
+	{0x801b, 0xff},
+	{0x801c, 0x75},
+	{0x801d, 0x2a},
+	{0x801e, 0x0e},
+	{0x801f, 0x75},
+	{0x8020, 0x2b},
+	{0x8021, 0x55},
+	{0x8022, 0x75},
+	{0x8023, 0x2c},
+	{0x8024, 0x01},
+	{0x8025, 0x12},
+	{0x8026, 0x0a},
+	{0x8027, 0x0e},
+	{0x8028, 0xe4},
+	{0x8029, 0xff},
+	{0x802a, 0xef},
+	{0x802b, 0x25},
+	{0x802c, 0xe0},
+	{0x802d, 0x24},
+	{0x802e, 0x41},
+	{0x802f, 0xf8},
+	{0x8030, 0xe4},
+	{0x8031, 0xf6},
+	{0x8032, 0x08},
+	{0x8033, 0xf6},
+	{0x8034, 0x0f},
+	{0x8035, 0xbf},
+	{0x8036, 0x34},
+	{0x8037, 0xf2},
+	{0x8038, 0x90},
+	{0x8039, 0x0e},
+	{0x803a, 0x88},
+	{0x803b, 0xe4},
+	{0x803c, 0x93},
+	{0x803d, 0xff},
+	{0x803e, 0xe5},
+	{0x803f, 0x3e},
+	{0x8040, 0xc3},
+	{0x8041, 0x9f},
+	{0x8042, 0x50},
+	{0x8043, 0x04},
+	{0x8044, 0x7f},
+	{0x8045, 0x05},
+	{0x8046, 0x80},
+	{0x8047, 0x02},
+	{0x8048, 0x7f},
+	{0x8049, 0xfb},
+	{0x804a, 0x78},
+	{0x804b, 0xb0},
+	{0x804c, 0xa6},
+	{0x804d, 0x07},
+	{0x804e, 0x12},
+	{0x804f, 0x0a},
+	{0x8050, 0x78},
+	{0x8051, 0x40},
+	{0x8052, 0x04},
+	{0x8053, 0x7f},
+	{0x8054, 0x03},
+	{0x8055, 0x80},
+	{0x8056, 0x02},
+	{0x8057, 0x7f},
+	{0x8058, 0x30},
+	{0x8059, 0x78},
+	{0x805a, 0xaf},
+	{0x805b, 0xa6},
+	{0x805c, 0x07},
+	{0x805d, 0xe6},
+	{0x805e, 0x18},
+	{0x805f, 0xf6},
+	{0x8060, 0x08},
+	{0x8061, 0xe6},
+	{0x8062, 0x78},
+	{0x8063, 0xac},
+	{0x8064, 0xf6},
+	{0x8065, 0x78},
+	{0x8066, 0xaf},
+	{0x8067, 0xe6},
+	{0x8068, 0x78},
+	{0x8069, 0xad},
+	{0x806a, 0xf6},
+	{0x806b, 0x78},
+	{0x806c, 0xb2},
+	{0x806d, 0x76},
+	{0x806e, 0x33},
+	{0x806f, 0xe4},
+	{0x8070, 0x08},
+	{0x8071, 0xf6},
+	{0x8072, 0x78},
+	{0x8073, 0xab},
+	{0x8074, 0x76},
+	{0x8075, 0x01},
+	{0x8076, 0x75},
+	{0x8077, 0x3d},
+	{0x8078, 0x02},
+	{0x8079, 0x78},
+	{0x807a, 0xa9},
+	{0x807b, 0xf6},
+	{0x807c, 0x08},
+	{0x807d, 0xf6},
+	{0x807e, 0x74},
+	{0x807f, 0xff},
+	{0x8080, 0x78},
+	{0x8081, 0xb4},
+	{0x8082, 0xf6},
+	{0x8083, 0x08},
+	{0x8084, 0xf6},
+	{0x8085, 0x75},
+	{0x8086, 0x40},
+	{0x8087, 0x01},
+	{0x8088, 0x78},
+	{0x8089, 0xaf},
+	{0x808a, 0xe6},
+	{0x808b, 0x75},
+	{0x808c, 0xf0},
+	{0x808d, 0x05},
+	{0x808e, 0xa4},
+	{0x808f, 0xf5},
+	{0x8090, 0x3e},
+	{0x8091, 0x12},
+	{0x8092, 0x08},
+	{0x8093, 0x1d},
+	{0x8094, 0xc2},
+	{0x8095, 0x36},
+	{0x8096, 0x22},
+	{0x8097, 0x78},
+	{0x8098, 0xab},
+	{0x8099, 0xe6},
+	{0x809a, 0xd3},
+	{0x809b, 0x94},
+	{0x809c, 0x00},
+	{0x809d, 0x40},
+	{0x809e, 0x02},
+	{0x809f, 0x16},
+	{0x80a0, 0x22},
+	{0x80a1, 0xe5},
+	{0x80a2, 0x40},
+	{0x80a3, 0x64},
+	{0x80a4, 0x05},
+	{0x80a5, 0x70},
+	{0x80a6, 0x28},
+	{0x80a7, 0xf5},
+	{0x80a8, 0x40},
+	{0x80a9, 0xc2},
+	{0x80aa, 0x01},
+	{0x80ab, 0x78},
+	{0x80ac, 0xac},
+	{0x80ad, 0xe6},
+	{0x80ae, 0x25},
+	{0x80af, 0xe0},
+	{0x80b0, 0x24},
+	{0x80b1, 0x41},
+	{0x80b2, 0xf8},
+	{0x80b3, 0xe6},
+	{0x80b4, 0xfe},
+	{0x80b5, 0x08},
+	{0x80b6, 0xe6},
+	{0x80b7, 0xff},
+	{0x80b8, 0x78},
+	{0x80b9, 0x41},
+	{0x80ba, 0xa6},
+	{0x80bb, 0x06},
+	{0x80bc, 0x08},
+	{0x80bd, 0xa6},
+	{0x80be, 0x07},
+	{0x80bf, 0xa2},
+	{0x80c0, 0x36},
+	{0x80c1, 0xe4},
+	{0x80c2, 0x33},
+	{0x80c3, 0xf5},
+	{0x80c4, 0x31},
+	{0x80c5, 0x90},
+	{0x80c6, 0x30},
+	{0x80c7, 0x28},
+	{0x80c8, 0xf0},
+	{0x80c9, 0x75},
+	{0x80ca, 0x3f},
+	{0x80cb, 0x10},
+	{0x80cc, 0xd2},
+	{0x80cd, 0x34},
+	{0x80ce, 0x22},
+	{0x80cf, 0xe5},
+	{0x80d0, 0x3e},
+	{0x80d1, 0x75},
+	{0x80d2, 0xf0},
+	{0x80d3, 0x05},
+	{0x80d4, 0x84},
+	{0x80d5, 0x78},
+	{0x80d6, 0xaf},
+	{0x80d7, 0xf6},
+	{0x80d8, 0x90},
+	{0x80d9, 0x0e},
+	{0x80da, 0x85},
+	{0x80db, 0xe4},
+	{0x80dc, 0x93},
+	{0x80dd, 0xff},
+	{0x80de, 0x25},
+	{0x80df, 0xe0},
+	{0x80e0, 0x24},
+	{0x80e1, 0x0a},
+	{0x80e2, 0xf8},
+	{0x80e3, 0xe6},
+	{0x80e4, 0xfc},
+	{0x80e5, 0x08},
+	{0x80e6, 0xe6},
+	{0x80e7, 0xfd},
+	{0x80e8, 0x78},
+	{0x80e9, 0xaf},
+	{0x80ea, 0xe6},
+	{0x80eb, 0x25},
+	{0x80ec, 0xe0},
+	{0x80ed, 0x24},
+	{0x80ee, 0x41},
+	{0x80ef, 0xf8},
+	{0x80f0, 0xa6},
+	{0x80f1, 0x04},
+	{0x80f2, 0x08},
+	{0x80f3, 0xa6},
+	{0x80f4, 0x05},
+	{0x80f5, 0xef},
+	{0x80f6, 0x12},
+	{0x80f7, 0x0a},
+	{0x80f8, 0x7f},
+	{0x80f9, 0xd3},
+	{0x80fa, 0x78},
+	{0x80fb, 0xaa},
+	{0x80fc, 0x96},
+	{0x80fd, 0xee},
+	{0x80fe, 0x18},
+	{0x80ff, 0x96},
+	{0x8100, 0x40},
+	{0x8101, 0x0d},
+	{0x8102, 0x78},
+	{0x8103, 0xaf},
+	{0x8104, 0xe6},
+	{0x8105, 0x78},
+	{0x8106, 0xac},
+	{0x8107, 0xf6},
+	{0x8108, 0x78},
+	{0x8109, 0xa9},
+	{0x810a, 0xa6},
+	{0x810b, 0x06},
+	{0x810c, 0x08},
+	{0x810d, 0xa6},
+	{0x810e, 0x07},
+	{0x810f, 0x90},
+	{0x8110, 0x0e},
+	{0x8111, 0x85},
+	{0x8112, 0xe4},
+	{0x8113, 0x93},
+	{0x8114, 0x12},
+	{0x8115, 0x0a},
+	{0x8116, 0x7f},
+	{0x8117, 0xc3},
+	{0x8118, 0x78},
+	{0x8119, 0xb5},
+	{0x811a, 0x96},
+	{0x811b, 0xee},
+	{0x811c, 0x18},
+	{0x811d, 0x96},
+	{0x811e, 0x50},
+	{0x811f, 0x0d},
+	{0x8120, 0x78},
+	{0x8121, 0xaf},
+	{0x8122, 0xe6},
+	{0x8123, 0x78},
+	{0x8124, 0xad},
+	{0x8125, 0xf6},
+	{0x8126, 0x78},
+	{0x8127, 0xb4},
+	{0x8128, 0xa6},
+	{0x8129, 0x06},
+	{0x812a, 0x08},
+	{0x812b, 0xa6},
+	{0x812c, 0x07},
+	{0x812d, 0x78},
+	{0x812e, 0xa9},
+	{0x812f, 0xe6},
+	{0x8130, 0xfe},
+	{0x8131, 0x08},
+	{0x8132, 0xe6},
+	{0x8133, 0xc3},
+	{0x8134, 0x78},
+	{0x8135, 0xb5},
+	{0x8136, 0x96},
+	{0x8137, 0xff},
+	{0x8138, 0xee},
+	{0x8139, 0x18},
+	{0x813a, 0x96},
+	{0x813b, 0x78},
+	{0x813c, 0xb6},
+	{0x813d, 0xf6},
+	{0x813e, 0x08},
+	{0x813f, 0xa6},
+	{0x8140, 0x07},
+	{0x8141, 0x90},
+	{0x8142, 0x0e},
+	{0x8143, 0x8a},
+	{0x8144, 0xe4},
+	{0x8145, 0x18},
+	{0x8146, 0x12},
+	{0x8147, 0x0a},
+	{0x8148, 0x5d},
+	{0x8149, 0x40},
+	{0x814a, 0x02},
+	{0x814b, 0xd2},
+	{0x814c, 0x36},
+	{0x814d, 0x78},
+	{0x814e, 0xaf},
+	{0x814f, 0xe6},
+	{0x8150, 0x08},
+	{0x8151, 0x26},
+	{0x8152, 0x08},
+	{0x8153, 0xf6},
+	{0x8154, 0xe5},
+	{0x8155, 0x40},
+	{0x8156, 0x64},
+	{0x8157, 0x01},
+	{0x8158, 0x70},
+	{0x8159, 0x55},
+	{0x815a, 0xe6},
+	{0x815b, 0xc3},
+	{0x815c, 0x78},
+	{0x815d, 0xb3},
+	{0x815e, 0x12},
+	{0x815f, 0x0a},
+	{0x8160, 0x53},
+	{0x8161, 0x40},
+	{0x8162, 0x10},
+	{0x8163, 0x12},
+	{0x8164, 0x0a},
+	{0x8165, 0x4e},
+	{0x8166, 0x50},
+	{0x8167, 0x0b},
+	{0x8168, 0x30},
+	{0x8169, 0x36},
+	{0x816a, 0x41},
+	{0x816b, 0x78},
+	{0x816c, 0xaf},
+	{0x816d, 0xe6},
+	{0x816e, 0x78},
+	{0x816f, 0xac},
+	{0x8170, 0x66},
+	{0x8171, 0x60},
+	{0x8172, 0x39},
+	{0x8173, 0x12},
+	{0x8174, 0x0a},
+	{0x8175, 0x76},
+	{0x8176, 0x40},
+	{0x8177, 0x04},
+	{0x8178, 0x7f},
+	{0x8179, 0xfe},
+	{0x817a, 0x80},
+	{0x817b, 0x02},
+	{0x817c, 0x7f},
+	{0x817d, 0x02},
+	{0x817e, 0x78},
+	{0x817f, 0xb0},
+	{0x8180, 0xa6},
+	{0x8181, 0x07},
+	{0x8182, 0x78},
+	{0x8183, 0xac},
+	{0x8184, 0xe6},
+	{0x8185, 0x24},
+	{0x8186, 0x03},
+	{0x8187, 0x78},
+	{0x8188, 0xb2},
+	{0x8189, 0xf6},
+	{0x818a, 0x78},
+	{0x818b, 0xac},
+	{0x818c, 0xe6},
+	{0x818d, 0x24},
+	{0x818e, 0xfd},
+	{0x818f, 0x78},
+	{0x8190, 0xb3},
+	{0x8191, 0xf6},
+	{0x8192, 0x12},
+	{0x8193, 0x0a},
+	{0x8194, 0x76},
+	{0x8195, 0x40},
+	{0x8196, 0x06},
+	{0x8197, 0x78},
+	{0x8198, 0xb3},
+	{0x8199, 0xe6},
+	{0x819a, 0xff},
+	{0x819b, 0x80},
+	{0x819c, 0x04},
+	{0x819d, 0x78},
+	{0x819e, 0xb2},
+	{0x819f, 0xe6},
+	{0x81a0, 0xff},
+	{0x81a1, 0x78},
+	{0x81a2, 0xb1},
+	{0x81a3, 0xa6},
+	{0x81a4, 0x07},
+	{0x81a5, 0x75},
+	{0x81a6, 0x40},
+	{0x81a7, 0x02},
+	{0x81a8, 0x78},
+	{0x81a9, 0xab},
+	{0x81aa, 0x76},
+	{0x81ab, 0x01},
+	{0x81ac, 0x02},
+	{0x81ad, 0x02},
+	{0x81ae, 0x6e},
+	{0x81af, 0xe5},
+	{0x81b0, 0x40},
+	{0x81b1, 0x64},
+	{0x81b2, 0x02},
+	{0x81b3, 0x60},
+	{0x81b4, 0x03},
+	{0x81b5, 0x02},
+	{0x81b6, 0x02},
+	{0x81b7, 0x4e},
+	{0x81b8, 0x78},
+	{0x81b9, 0xb1},
+	{0x81ba, 0xe6},
+	{0x81bb, 0xff},
+	{0x81bc, 0xc3},
+	{0x81bd, 0x78},
+	{0x81be, 0xb3},
+	{0x81bf, 0x12},
+	{0x81c0, 0x0a},
+	{0x81c1, 0x54},
+	{0x81c2, 0x40},
+	{0x81c3, 0x08},
+	{0x81c4, 0x12},
+	{0x81c5, 0x0a},
+	{0x81c6, 0x4e},
+	{0x81c7, 0x50},
+	{0x81c8, 0x03},
+	{0x81c9, 0x02},
+	{0x81ca, 0x02},
+	{0x81cb, 0x4c},
+	{0x81cc, 0x12},
+	{0x81cd, 0x0a},
+	{0x81ce, 0x76},
+	{0x81cf, 0x40},
+	{0x81d0, 0x04},
+	{0x81d1, 0x7f},
+	{0x81d2, 0xff},
+	{0x81d3, 0x80},
+	{0x81d4, 0x02},
+	{0x81d5, 0x7f},
+	{0x81d6, 0x01},
+	{0x81d7, 0x78},
+	{0x81d8, 0xb0},
+	{0x81d9, 0xa6},
+	{0x81da, 0x07},
+	{0x81db, 0x78},
+	{0x81dc, 0xac},
+	{0x81dd, 0xe6},
+	{0x81de, 0x04},
+	{0x81df, 0x78},
+	{0x81e0, 0xb2},
+	{0x81e1, 0xf6},
+	{0x81e2, 0x78},
+	{0x81e3, 0xac},
+	{0x81e4, 0xe6},
+	{0x81e5, 0x14},
+	{0x81e6, 0x78},
+	{0x81e7, 0xb3},
+	{0x81e8, 0xf6},
+	{0x81e9, 0x18},
+	{0x81ea, 0x12},
+	{0x81eb, 0x0a},
+	{0x81ec, 0x78},
+	{0x81ed, 0x40},
+	{0x81ee, 0x04},
+	{0x81ef, 0xe6},
+	{0x81f0, 0xff},
+	{0x81f1, 0x80},
+	{0x81f2, 0x02},
+	{0x81f3, 0x7f},
+	{0x81f4, 0x00},
+	{0x81f5, 0x78},
+	{0x81f6, 0xb2},
+	{0x81f7, 0xa6},
+	{0x81f8, 0x07},
+	{0x81f9, 0xd3},
+	{0x81fa, 0x08},
+	{0x81fb, 0xe6},
+	{0x81fc, 0x64},
+	{0x81fd, 0x80},
+	{0x81fe, 0x94},
+	{0x81ff, 0x80},
+	{0x8200, 0x40},
+	{0x8201, 0x04},
+	{0x8202, 0xe6},
+	{0x8203, 0xff},
+	{0x8204, 0x80},
+	{0x8205, 0x02},
+	{0x8206, 0x7f},
+	{0x8207, 0x00},
+	{0x8208, 0x78},
+	{0x8209, 0xb3},
+	{0x820a, 0xa6},
+	{0x820b, 0x07},
+	{0x820c, 0xc3},
+	{0x820d, 0x18},
+	{0x820e, 0xe6},
+	{0x820f, 0x64},
+	{0x8210, 0x80},
+	{0x8211, 0x94},
+	{0x8212, 0xb3},
+	{0x8213, 0x50},
+	{0x8214, 0x04},
+	{0x8215, 0xe6},
+	{0x8216, 0xff},
+	{0x8217, 0x80},
+	{0x8218, 0x02},
+	{0x8219, 0x7f},
+	{0x821a, 0x33},
+	{0x821b, 0x78},
+	{0x821c, 0xb2},
+	{0x821d, 0xa6},
+	{0x821e, 0x07},
+	{0x821f, 0xc3},
+	{0x8220, 0x08},
+	{0x8221, 0xe6},
+	{0x8222, 0x64},
+	{0x8223, 0x80},
+	{0x8224, 0x94},
+	{0x8225, 0xb3},
+	{0x8226, 0x50},
+	{0x8227, 0x04},
+	{0x8228, 0xe6},
+	{0x8229, 0xff},
+	{0x822a, 0x80},
+	{0x822b, 0x02},
+	{0x822c, 0x7f},
+	{0x822d, 0x33},
+	{0x822e, 0x78},
+	{0x822f, 0xb3},
+	{0x8230, 0xa6},
+	{0x8231, 0x07},
+	{0x8232, 0x12},
+	{0x8233, 0x0a},
+	{0x8234, 0x76},
+	{0x8235, 0x40},
+	{0x8236, 0x06},
+	{0x8237, 0x78},
+	{0x8238, 0xb3},
+	{0x8239, 0xe6},
+	{0x823a, 0xff},
+	{0x823b, 0x80},
+	{0x823c, 0x04},
+	{0x823d, 0x78},
+	{0x823e, 0xb2},
+	{0x823f, 0xe6},
+	{0x8240, 0xff},
+	{0x8241, 0x78},
+	{0x8242, 0xb1},
+	{0x8243, 0xa6},
+	{0x8244, 0x07},
+	{0x8245, 0x75},
+	{0x8246, 0x40},
+	{0x8247, 0x03},
+	{0x8248, 0x78},
+	{0x8249, 0xab},
+	{0x824a, 0x76},
+	{0x824b, 0x01},
+	{0x824c, 0x80},
+	{0x824d, 0x20},
+	{0x824e, 0xe5},
+	{0x824f, 0x40},
+	{0x8250, 0x64},
+	{0x8251, 0x03},
+	{0x8252, 0x70},
+	{0x8253, 0x26},
+	{0x8254, 0x78},
+	{0x8255, 0xb1},
+	{0x8256, 0xe6},
+	{0x8257, 0xff},
+	{0x8258, 0xc3},
+	{0x8259, 0x78},
+	{0x825a, 0xb3},
+	{0x825b, 0x12},
+	{0x825c, 0x0a},
+	{0x825d, 0x54},
+	{0x825e, 0x40},
+	{0x825f, 0x05},
+	{0x8260, 0x12},
+	{0x8261, 0x0a},
+	{0x8262, 0x4e},
+	{0x8263, 0x40},
+	{0x8264, 0x09},
+	{0x8265, 0x78},
+	{0x8266, 0xac},
+	{0x8267, 0xe6},
+	{0x8268, 0x78},
+	{0x8269, 0xb1},
+	{0x826a, 0xf6},
+	{0x826b, 0x75},
+	{0x826c, 0x40},
+	{0x826d, 0x04},
+	{0x826e, 0x78},
+	{0x826f, 0xb1},
+	{0x8270, 0xe6},
+	{0x8271, 0x75},
+	{0x8272, 0xf0},
+	{0x8273, 0x05},
+	{0x8274, 0xa4},
+	{0x8275, 0xf5},
+	{0x8276, 0x3e},
+	{0x8277, 0x02},
+	{0x8278, 0x08},
+	{0x8279, 0x1d},
+	{0x827a, 0xe5},
+	{0x827b, 0x40},
+	{0x827c, 0xb4},
+	{0x827d, 0x04},
+	{0x827e, 0x1f},
+	{0x827f, 0x90},
+	{0x8280, 0x0e},
+	{0x8281, 0x89},
+	{0x8282, 0xe4},
+	{0x8283, 0x78},
+	{0x8284, 0xb6},
+	{0x8285, 0x12},
+	{0x8286, 0x0a},
+	{0x8287, 0x5d},
+	{0x8288, 0x40},
+	{0x8289, 0x02},
+	{0x828a, 0xd2},
+	{0x828b, 0x36},
+	{0x828c, 0x75},
+	{0x828d, 0x40},
+	{0x828e, 0x05},
+	{0x828f, 0x75},
+	{0x8290, 0x29},
+	{0x8291, 0xff},
+	{0x8292, 0x75},
+	{0x8293, 0x2a},
+	{0x8294, 0x0e},
+	{0x8295, 0x75},
+	{0x8296, 0x2b},
+	{0x8297, 0x59},
+	{0x8298, 0x75},
+	{0x8299, 0x2c},
+	{0x829a, 0x01},
+	{0x829b, 0x12},
+	{0x829c, 0x0a},
+	{0x829d, 0x0e},
+	{0x829e, 0x22},
+	{0x829f, 0xef},
+	{0x82a0, 0x8d},
+	{0x82a1, 0xf0},
+	{0x82a2, 0xa4},
+	{0x82a3, 0xa8},
+	{0x82a4, 0xf0},
+	{0x82a5, 0xcf},
+	{0x82a6, 0x8c},
+	{0x82a7, 0xf0},
+	{0x82a8, 0xa4},
+	{0x82a9, 0x28},
+	{0x82aa, 0xce},
+	{0x82ab, 0x8d},
+	{0x82ac, 0xf0},
+	{0x82ad, 0xa4},
+	{0x82ae, 0x2e},
+	{0x82af, 0xfe},
+	{0x82b0, 0x22},
+	{0x82b1, 0xbc},
+	{0x82b2, 0x00},
+	{0x82b3, 0x0b},
+	{0x82b4, 0xbe},
+	{0x82b5, 0x00},
+	{0x82b6, 0x29},
+	{0x82b7, 0xef},
+	{0x82b8, 0x8d},
+	{0x82b9, 0xf0},
+	{0x82ba, 0x84},
+	{0x82bb, 0xff},
+	{0x82bc, 0xad},
+	{0x82bd, 0xf0},
+	{0x82be, 0x22},
+	{0x82bf, 0xe4},
+	{0x82c0, 0xcc},
+	{0x82c1, 0xf8},
+	{0x82c2, 0x75},
+	{0x82c3, 0xf0},
+	{0x82c4, 0x08},
+	{0x82c5, 0xef},
+	{0x82c6, 0x2f},
+	{0x82c7, 0xff},
+	{0x82c8, 0xee},
+	{0x82c9, 0x33},
+	{0x82ca, 0xfe},
+	{0x82cb, 0xec},
+	{0x82cc, 0x33},
+	{0x82cd, 0xfc},
+	{0x82ce, 0xee},
+	{0x82cf, 0x9d},
+	{0x82d0, 0xec},
+	{0x82d1, 0x98},
+	{0x82d2, 0x40},
+	{0x82d3, 0x05},
+	{0x82d4, 0xfc},
+	{0x82d5, 0xee},
+	{0x82d6, 0x9d},
+	{0x82d7, 0xfe},
+	{0x82d8, 0x0f},
+	{0x82d9, 0xd5},
+	{0x82da, 0xf0},
+	{0x82db, 0xe9},
+	{0x82dc, 0xe4},
+	{0x82dd, 0xce},
+	{0x82de, 0xfd},
+	{0x82df, 0x22},
+	{0x82e0, 0xed},
+	{0x82e1, 0xf8},
+	{0x82e2, 0xf5},
+	{0x82e3, 0xf0},
+	{0x82e4, 0xee},
+	{0x82e5, 0x84},
+	{0x82e6, 0x20},
+	{0x82e7, 0xd2},
+	{0x82e8, 0x1c},
+	{0x82e9, 0xfe},
+	{0x82ea, 0xad},
+	{0x82eb, 0xf0},
+	{0x82ec, 0x75},
+	{0x82ed, 0xf0},
+	{0x82ee, 0x08},
+	{0x82ef, 0xef},
+	{0x82f0, 0x2f},
+	{0x82f1, 0xff},
+	{0x82f2, 0xed},
+	{0x82f3, 0x33},
+	{0x82f4, 0xfd},
+	{0x82f5, 0x40},
+	{0x82f6, 0x07},
+	{0x82f7, 0x98},
+	{0x82f8, 0x50},
+	{0x82f9, 0x06},
+	{0x82fa, 0xd5},
+	{0x82fb, 0xf0},
+	{0x82fc, 0xf2},
+	{0x82fd, 0x22},
+	{0x82fe, 0xc3},
+	{0x82ff, 0x98},
+	{0x8300, 0xfd},
+	{0x8301, 0x0f},
+	{0x8302, 0xd5},
+	{0x8303, 0xf0},
+	{0x8304, 0xea},
+	{0x8305, 0x22},
+	{0x8306, 0xe8},
+	{0x8307, 0x8f},
+	{0x8308, 0xf0},
+	{0x8309, 0xa4},
+	{0x830a, 0xcc},
+	{0x830b, 0x8b},
+	{0x830c, 0xf0},
+	{0x830d, 0xa4},
+	{0x830e, 0x2c},
+	{0x830f, 0xfc},
+	{0x8310, 0xe9},
+	{0x8311, 0x8e},
+	{0x8312, 0xf0},
+	{0x8313, 0xa4},
+	{0x8314, 0x2c},
+	{0x8315, 0xfc},
+	{0x8316, 0x8a},
+	{0x8317, 0xf0},
+	{0x8318, 0xed},
+	{0x8319, 0xa4},
+	{0x831a, 0x2c},
+	{0x831b, 0xfc},
+	{0x831c, 0xea},
+	{0x831d, 0x8e},
+	{0x831e, 0xf0},
+	{0x831f, 0xa4},
+	{0x8320, 0xcd},
+	{0x8321, 0xa8},
+	{0x8322, 0xf0},
+	{0x8323, 0x8b},
+	{0x8324, 0xf0},
+	{0x8325, 0xa4},
+	{0x8326, 0x2d},
+	{0x8327, 0xcc},
+	{0x8328, 0x38},
+	{0x8329, 0x25},
+	{0x832a, 0xf0},
+	{0x832b, 0xfd},
+	{0x832c, 0xe9},
+	{0x832d, 0x8f},
+	{0x832e, 0xf0},
+	{0x832f, 0xa4},
+	{0x8330, 0x2c},
+	{0x8331, 0xcd},
+	{0x8332, 0x35},
+	{0x8333, 0xf0},
+	{0x8334, 0xfc},
+	{0x8335, 0xeb},
+	{0x8336, 0x8e},
+	{0x8337, 0xf0},
+	{0x8338, 0xa4},
+	{0x8339, 0xfe},
+	{0x833a, 0xa9},
+	{0x833b, 0xf0},
+	{0x833c, 0xeb},
+	{0x833d, 0x8f},
+	{0x833e, 0xf0},
+	{0x833f, 0xa4},
+	{0x8340, 0xcf},
+	{0x8341, 0xc5},
+	{0x8342, 0xf0},
+	{0x8343, 0x2e},
+	{0x8344, 0xcd},
+	{0x8345, 0x39},
+	{0x8346, 0xfe},
+	{0x8347, 0xe4},
+	{0x8348, 0x3c},
+	{0x8349, 0xfc},
+	{0x834a, 0xea},
+	{0x834b, 0xa4},
+	{0x834c, 0x2d},
+	{0x834d, 0xce},
+	{0x834e, 0x35},
+	{0x834f, 0xf0},
+	{0x8350, 0xfd},
+	{0x8351, 0xe4},
+	{0x8352, 0x3c},
+	{0x8353, 0xfc},
+	{0x8354, 0x22},
+	{0x8355, 0x75},
+	{0x8356, 0xf0},
+	{0x8357, 0x08},
+	{0x8358, 0x75},
+	{0x8359, 0x82},
+	{0x835a, 0x00},
+	{0x835b, 0xef},
+	{0x835c, 0x2f},
+	{0x835d, 0xff},
+	{0x835e, 0xee},
+	{0x835f, 0x33},
+	{0x8360, 0xfe},
+	{0x8361, 0xcd},
+	{0x8362, 0x33},
+	{0x8363, 0xcd},
+	{0x8364, 0xcc},
+	{0x8365, 0x33},
+	{0x8366, 0xcc},
+	{0x8367, 0xc5},
+	{0x8368, 0x82},
+	{0x8369, 0x33},
+	{0x836a, 0xc5},
+	{0x836b, 0x82},
+	{0x836c, 0x9b},
+	{0x836d, 0xed},
+	{0x836e, 0x9a},
+	{0x836f, 0xec},
+	{0x8370, 0x99},
+	{0x8371, 0xe5},
+	{0x8372, 0x82},
+	{0x8373, 0x98},
+	{0x8374, 0x40},
+	{0x8375, 0x0c},
+	{0x8376, 0xf5},
+	{0x8377, 0x82},
+	{0x8378, 0xee},
+	{0x8379, 0x9b},
+	{0x837a, 0xfe},
+	{0x837b, 0xed},
+	{0x837c, 0x9a},
+	{0x837d, 0xfd},
+	{0x837e, 0xec},
+	{0x837f, 0x99},
+	{0x8380, 0xfc},
+	{0x8381, 0x0f},
+	{0x8382, 0xd5},
+	{0x8383, 0xf0},
+	{0x8384, 0xd6},
+	{0x8385, 0xe4},
+	{0x8386, 0xce},
+	{0x8387, 0xfb},
+	{0x8388, 0xe4},
+	{0x8389, 0xcd},
+	{0x838a, 0xfa},
+	{0x838b, 0xe4},
+	{0x838c, 0xcc},
+	{0x838d, 0xf9},
+	{0x838e, 0xa8},
+	{0x838f, 0x82},
+	{0x8390, 0x22},
+	{0x8391, 0xb8},
+	{0x8392, 0x00},
+	{0x8393, 0xc1},
+	{0x8394, 0xb9},
+	{0x8395, 0x00},
+	{0x8396, 0x59},
+	{0x8397, 0xba},
+	{0x8398, 0x00},
+	{0x8399, 0x2d},
+	{0x839a, 0xec},
+	{0x839b, 0x8b},
+	{0x839c, 0xf0},
+	{0x839d, 0x84},
+	{0x839e, 0xcf},
+	{0x839f, 0xce},
+	{0x83a0, 0xcd},
+	{0x83a1, 0xfc},
+	{0x83a2, 0xe5},
+	{0x83a3, 0xf0},
+	{0x83a4, 0xcb},
+	{0x83a5, 0xf9},
+	{0x83a6, 0x78},
+	{0x83a7, 0x18},
+	{0x83a8, 0xef},
+	{0x83a9, 0x2f},
+	{0x83aa, 0xff},
+	{0x83ab, 0xee},
+	{0x83ac, 0x33},
+	{0x83ad, 0xfe},
+	{0x83ae, 0xed},
+	{0x83af, 0x33},
+	{0x83b0, 0xfd},
+	{0x83b1, 0xec},
+	{0x83b2, 0x33},
+	{0x83b3, 0xfc},
+	{0x83b4, 0xeb},
+	{0x83b5, 0x33},
+	{0x83b6, 0xfb},
+	{0x83b7, 0x10},
+	{0x83b8, 0xd7},
+	{0x83b9, 0x03},
+	{0x83ba, 0x99},
+	{0x83bb, 0x40},
+	{0x83bc, 0x04},
+	{0x83bd, 0xeb},
+	{0x83be, 0x99},
+	{0x83bf, 0xfb},
+	{0x83c0, 0x0f},
+	{0x83c1, 0xd8},
+	{0x83c2, 0xe5},
+	{0x83c3, 0xe4},
+	{0x83c4, 0xf9},
+	{0x83c5, 0xfa},
+	{0x83c6, 0x22},
+	{0x83c7, 0x78},
+	{0x83c8, 0x18},
+	{0x83c9, 0xef},
+	{0x83ca, 0x2f},
+	{0x83cb, 0xff},
+	{0x83cc, 0xee},
+	{0x83cd, 0x33},
+	{0x83ce, 0xfe},
+	{0x83cf, 0xed},
+	{0x83d0, 0x33},
+	{0x83d1, 0xfd},
+	{0x83d2, 0xec},
+	{0x83d3, 0x33},
+	{0x83d4, 0xfc},
+	{0x83d5, 0xc9},
+	{0x83d6, 0x33},
+	{0x83d7, 0xc9},
+	{0x83d8, 0x10},
+	{0x83d9, 0xd7},
+	{0x83da, 0x05},
+	{0x83db, 0x9b},
+	{0x83dc, 0xe9},
+	{0x83dd, 0x9a},
+	{0x83de, 0x40},
+
+	{0x83df, 0x07},
+	{0x83e0, 0xec},
+	{0x83e1, 0x9b},
+	{0x83e2, 0xfc},
+	{0x83e3, 0xe9},
+	{0x83e4, 0x9a},
+	{0x83e5, 0xf9},
+	{0x83e6, 0x0f},
+	{0x83e7, 0xd8},
+	{0x83e8, 0xe0},
+	{0x83e9, 0xe4},
+	{0x83ea, 0xc9},
+	{0x83eb, 0xfa},
+	{0x83ec, 0xe4},
+	{0x83ed, 0xcc},
+	{0x83ee, 0xfb},
+	{0x83ef, 0x22},
+	{0x83f0, 0x75},
+	{0x83f1, 0xf0},
+	{0x83f2, 0x10},
+	{0x83f3, 0xef},
+	{0x83f4, 0x2f},
+	{0x83f5, 0xff},
+	{0x83f6, 0xee},
+	{0x83f7, 0x33},
+	{0x83f8, 0xfe},
+	{0x83f9, 0xed},
+	{0x83fa, 0x33},
+	{0x83fb, 0xfd},
+	{0x83fc, 0xcc},
+	{0x83fd, 0x33},
+	{0x83fe, 0xcc},
+	{0x83ff, 0xc8},
+	{0x8400, 0x33},
+	{0x8401, 0xc8},
+	{0x8402, 0x10},
+	{0x8403, 0xd7},
+	{0x8404, 0x07},
+	{0x8405, 0x9b},
+	{0x8406, 0xec},
+	{0x8407, 0x9a},
+	{0x8408, 0xe8},
+	{0x8409, 0x99},
+	{0x840a, 0x40},
+	{0x840b, 0x0a},
+	{0x840c, 0xed},
+	{0x840d, 0x9b},
+	{0x840e, 0xfd},
+	{0x840f, 0xec},
+	{0x8410, 0x9a},
+	{0x8411, 0xfc},
+	{0x8412, 0xe8},
+	{0x8413, 0x99},
+	{0x8414, 0xf8},
+	{0x8415, 0x0f},
+	{0x8416, 0xd5},
+	{0x8417, 0xf0},
+	{0x8418, 0xda},
+	{0x8419, 0xe4},
+	{0x841a, 0xcd},
+	{0x841b, 0xfb},
+	{0x841c, 0xe4},
+	{0x841d, 0xcc},
+	{0x841e, 0xfa},
+	{0x841f, 0xe4},
+	{0x8420, 0xc8},
+	{0x8421, 0xf9},
+	{0x8422, 0x22},
+	{0x8423, 0xeb},
+	{0x8424, 0x9f},
+	{0x8425, 0xf5},
+	{0x8426, 0xf0},
+	{0x8427, 0xea},
+	{0x8428, 0x9e},
+	{0x8429, 0x42},
+	{0x842a, 0xf0},
+	{0x842b, 0xe9},
+	{0x842c, 0x9d},
+	{0x842d, 0x42},
+	{0x842e, 0xf0},
+	{0x842f, 0xe8},
+	{0x8430, 0x9c},
+	{0x8431, 0x45},
+	{0x8432, 0xf0},
+	{0x8433, 0x22},
+	{0x8434, 0xe8},
+	{0x8435, 0x60},
+	{0x8436, 0x0f},
+	{0x8437, 0xef},
+	{0x8438, 0xc3},
+	{0x8439, 0x33},
+	{0x843a, 0xff},
+	{0x843b, 0xee},
+	{0x843c, 0x33},
+	{0x843d, 0xfe},
+	{0x843e, 0xed},
+	{0x843f, 0x33},
+	{0x8440, 0xfd},
+	{0x8441, 0xec},
+	{0x8442, 0x33},
+	{0x8443, 0xfc},
+	{0x8444, 0xd8},
+	{0x8445, 0xf1},
+	{0x8446, 0x22},
+	{0x8447, 0xe4},
+	{0x8448, 0x93},
+	{0x8449, 0xfc},
+	{0x844a, 0x74},
+	{0x844b, 0x01},
+	{0x844c, 0x93},
+	{0x844d, 0xfd},
+	{0x844e, 0x74},
+	{0x844f, 0x02},
+	{0x8450, 0x93},
+	{0x8451, 0xfe},
+	{0x8452, 0x74},
+	{0x8453, 0x03},
+	{0x8454, 0x93},
+	{0x8455, 0xff},
+	{0x8456, 0x22},
+	{0x8457, 0xe6},
+	{0x8458, 0xfb},
+	{0x8459, 0x08},
+	{0x845a, 0xe6},
+	{0x845b, 0xf9},
+	{0x845c, 0x08},
+	{0x845d, 0xe6},
+	{0x845e, 0xfa},
+	{0x845f, 0x08},
+	{0x8460, 0xe6},
+	{0x8461, 0xcb},
+	{0x8462, 0xf8},
+	{0x8463, 0x22},
+	{0x8464, 0xec},
+	{0x8465, 0xf6},
+	{0x8466, 0x08},
+	{0x8467, 0xed},
+	{0x8468, 0xf6},
+	{0x8469, 0x08},
+	{0x846a, 0xee},
+	{0x846b, 0xf6},
+	{0x846c, 0x08},
+	{0x846d, 0xef},
+	{0x846e, 0xf6},
+	{0x846f, 0x22},
+	{0x8470, 0xd0},
+	{0x8471, 0x83},
+	{0x8472, 0xd0},
+	{0x8473, 0x82},
+	{0x8474, 0xe4},
+	{0x8475, 0x93},
+	{0x8476, 0xf6},
+	{0x8477, 0x08},
+	{0x8478, 0x74},
+	{0x8479, 0x01},
+	{0x847a, 0x93},
+	{0x847b, 0xf6},
+	{0x847c, 0x08},
+	{0x847d, 0x74},
+	{0x847e, 0x02},
+	{0x847f, 0x93},
+	{0x8480, 0xf6},
+	{0x8481, 0x08},
+	{0x8482, 0x74},
+	{0x8483, 0x03},
+	{0x8484, 0x93},
+	{0x8485, 0xf6},
+	{0x8486, 0x74},
+	{0x8487, 0x04},
+	{0x8488, 0x73},
+	{0x8489, 0xa4},
+	{0x848a, 0x25},
+	{0x848b, 0x82},
+	{0x848c, 0xf5},
+	{0x848d, 0x82},
+	{0x848e, 0xe5},
+	{0x848f, 0xf0},
+	{0x8490, 0x35},
+	{0x8491, 0x83},
+	{0x8492, 0xf5},
+	{0x8493, 0x83},
+	{0x8494, 0x22},
+	{0x8495, 0xd0},
+	{0x8496, 0x83},
+	{0x8497, 0xd0},
+	{0x8498, 0x82},
+	{0x8499, 0xf8},
+	{0x849a, 0xe4},
+	{0x849b, 0x93},
+	{0x849c, 0x70},
+	{0x849d, 0x12},
+	{0x849e, 0x74},
+	{0x849f, 0x01},
+	{0x84a0, 0x93},
+	{0x84a1, 0x70},
+	{0x84a2, 0x0d},
+	{0x84a3, 0xa3},
+	{0x84a4, 0xa3},
+	{0x84a5, 0x93},
+	{0x84a6, 0xf8},
+	{0x84a7, 0x74},
+	{0x84a8, 0x01},
+	{0x84a9, 0x93},
+	{0x84aa, 0xf5},
+	{0x84ab, 0x82},
+	{0x84ac, 0x88},
+	{0x84ad, 0x83},
+	{0x84ae, 0xe4},
+	{0x84af, 0x73},
+	{0x84b0, 0x74},
+	{0x84b1, 0x02},
+	{0x84b2, 0x93},
+	{0x84b3, 0x68},
+	{0x84b4, 0x60},
+	{0x84b5, 0xef},
+	{0x84b6, 0xa3},
+	{0x84b7, 0xa3},
+	{0x84b8, 0xa3},
+	{0x84b9, 0x80},
+	{0x84ba, 0xdf},
+	{0x84bb, 0x90},
+	{0x84bc, 0x38},
+	{0x84bd, 0x04},
+	{0x84be, 0x78},
+	{0x84bf, 0x45},
+	{0x84c0, 0x12},
+	{0x84c1, 0x09},
+	{0x84c2, 0x1f},
+	{0x84c3, 0x90},
+	{0x84c4, 0x38},
+	{0x84c5, 0x00},
+	{0x84c6, 0xe0},
+	{0x84c7, 0xfe},
+	{0x84c8, 0xa3},
+	{0x84c9, 0xe0},
+	{0x84ca, 0xfd},
+	{0x84cb, 0xed},
+	{0x84cc, 0xff},
+	{0x84cd, 0xc3},
+	{0x84ce, 0x12},
+	{0x84cf, 0x08},
+	{0x84d0, 0xcb},
+	{0x84d1, 0x90},
+	{0x84d2, 0x38},
+	{0x84d3, 0x10},
+	{0x84d4, 0x12},
+	{0x84d5, 0x08},
+	{0x84d6, 0xbf},
+	{0x84d7, 0x90},
+	{0x84d8, 0x38},
+	{0x84d9, 0x06},
+	{0x84da, 0x78},
+	{0x84db, 0x47},
+	{0x84dc, 0x12},
+	{0x84dd, 0x09},
+	{0x84de, 0x1f},
+	{0x84df, 0x90},
+	{0x84e0, 0x38},
+	{0x84e1, 0x02},
+	{0x84e2, 0xe0},
+	{0x84e3, 0xfe},
+	{0x84e4, 0xa3},
+	{0x84e5, 0xe0},
+	{0x84e6, 0xfd},
+	{0x84e7, 0xed},
+	{0x84e8, 0xff},
+	{0x84e9, 0xc3},
+	{0x84ea, 0x12},
+	{0x84eb, 0x08},
+	{0x84ec, 0xcb},
+	{0x84ed, 0x90},
+	{0x84ee, 0x38},
+	{0x84ef, 0x12},
+	{0x84f0, 0x12},
+	{0x84f1, 0x08},
+	{0x84f2, 0xbf},
+	{0x84f3, 0xa3},
+	{0x84f4, 0xe0},
+	{0x84f5, 0xb4},
+	{0x84f6, 0x31},
+	{0x84f7, 0x07},
+	{0x84f8, 0x78},
+	{0x84f9, 0x45},
+	{0x84fa, 0x79},
+	{0x84fb, 0x45},
+	{0x84fc, 0x12},
+	{0x84fd, 0x09},
+	{0x84fe, 0x2a},
+	{0x84ff, 0x90},
+	{0x8500, 0x38},
+	{0x8501, 0x14},
+	{0x8502, 0xe0},
+	{0x8503, 0xb4},
+	{0x8504, 0x71},
+	{0x8505, 0x15},
+	{0x8506, 0x78},
+	{0x8507, 0x45},
+	{0x8508, 0xe6},
+	{0x8509, 0xfe},
+	{0x850a, 0x08},
+	{0x850b, 0xe6},
+	{0x850c, 0x78},
+	{0x850d, 0x02},
+	{0x850e, 0xce},
+	{0x850f, 0xc3},
+	{0x8510, 0x13},
+	{0x8511, 0xce},
+	{0x8512, 0x13},
+	{0x8513, 0xd8},
+	{0x8514, 0xf9},
+	{0x8515, 0x79},
+	{0x8516, 0x46},
+	{0x8517, 0xf7},
+	{0x8518, 0xee},
+	{0x8519, 0x19},
+	{0x851a, 0xf7},
+	{0x851b, 0x90},
+	{0x851c, 0x38},
+	{0x851d, 0x15},
+	{0x851e, 0xe0},
+	{0x851f, 0xb4},
+	{0x8520, 0x31},
+	{0x8521, 0x07},
+	{0x8522, 0x78},
+	{0x8523, 0x47},
+	{0x8524, 0x79},
+	{0x8525, 0x47},
+	{0x8526, 0x12},
+	{0x8527, 0x09},
+	{0x8528, 0x2a},
+	{0x8529, 0x90},
+	{0x852a, 0x38},
+	{0x852b, 0x15},
+	{0x852c, 0xe0},
+	{0x852d, 0xb4},
+	{0x852e, 0x71},
+	{0x852f, 0x15},
+	{0x8530, 0x78},
+	{0x8531, 0x47},
+	{0x8532, 0xe6},
+	{0x8533, 0xfe},
+	{0x8534, 0x08},
+	{0x8535, 0xe6},
+	{0x8536, 0x78},
+	{0x8537, 0x02},
+	{0x8538, 0xce},
+	{0x8539, 0xc3},
+	{0x853a, 0x13},
+	{0x853b, 0xce},
+	{0x853c, 0x13},
+	{0x853d, 0xd8},
+	{0x853e, 0xf9},
+	{0x853f, 0x79},
+	{0x8540, 0x48},
+	{0x8541, 0xf7},
+	{0x8542, 0xee},
+	{0x8543, 0x19},
+	{0x8544, 0xf7},
+	{0x8545, 0x79},
+	{0x8546, 0x45},
+	{0x8547, 0x12},
+	{0x8548, 0x08},
+	{0x8549, 0xfb},
+	{0x854a, 0x09},
+	{0x854b, 0x12},
+	{0x854c, 0x08},
+	{0x854d, 0xfb},
+	{0x854e, 0xaf},
+	{0x854f, 0x3a},
+	{0x8550, 0x12},
+	{0x8551, 0x08},
+	{0x8552, 0xb0},
+	{0x8553, 0x7d},
+	{0x8554, 0x50},
+	{0x8555, 0x12},
+	{0x8556, 0x02},
+	{0x8557, 0xb1},
+	{0x8558, 0x78},
+	{0x8559, 0x4d},
+	{0x855a, 0xa6},
+	{0x855b, 0x06},
+	{0x855c, 0x08},
+	{0x855d, 0xa6},
+	{0x855e, 0x07},
+	{0x855f, 0xaf},
+	{0x8560, 0x38},
+	{0x8561, 0x12},
+	{0x8562, 0x08},
+	{0x8563, 0xb0},
+	{0x8564, 0x7d},
+	{0x8565, 0x50},
+	{0x8566, 0x12},
+	{0x8567, 0x02},
+	{0x8568, 0xb1},
+	{0x8569, 0x78},
+	{0x856a, 0x49},
+	{0x856b, 0xa6},
+	{0x856c, 0x06},
+	{0x856d, 0x08},
+	{0x856e, 0xa6},
+	{0x856f, 0x07},
+	{0x8570, 0xaf},
+	{0x8571, 0x3b},
+	{0x8572, 0x78},
+	{0x8573, 0x47},
+	{0x8574, 0x12},
+	{0x8575, 0x08},
+	{0x8576, 0xb2},
+	{0x8577, 0x7d},
+	{0x8578, 0x3c},
+	{0x8579, 0x12},
+	{0x857a, 0x02},
+	{0x857b, 0xb1},
+	{0x857c, 0x78},
+	{0x857d, 0x4f},
+	{0x857e, 0xa6},
+	{0x857f, 0x06},
+	{0x8580, 0x08},
+	{0x8581, 0xa6},
+	{0x8582, 0x07},
+	{0x8583, 0xaf},
+	{0x8584, 0x39},
+	{0x8585, 0x7e},
+	{0x8586, 0x00},
+	{0x8587, 0x78},
+	{0x8588, 0x47},
+	{0x8589, 0x12},
+	{0x858a, 0x08},
+	{0x858b, 0xb4},
+	{0x858c, 0x7d},
+	{0x858d, 0x3c},
+	{0x858e, 0x12},
+	{0x858f, 0x02},
+	{0x8590, 0xb1},
+	{0x8591, 0x78},
+	{0x8592, 0x4b},
+	{0x8593, 0xa6},
+	{0x8594, 0x06},
+	{0x8595, 0x08},
+	{0x8596, 0xa6},
+	{0x8597, 0x07},
+	{0x8598, 0xc3},
+	{0x8599, 0x78},
+	{0x859a, 0x4e},
+	{0x859b, 0xe6},
+	{0x859c, 0x94},
+	{0x859d, 0x08},
+	{0x859e, 0x18},
+	{0x859f, 0xe6},
+	{0x85a0, 0x94},
+	{0x85a1, 0x00},
+	{0x85a2, 0x50},
+	{0x85a3, 0x05},
+	{0x85a4, 0x76},
+	{0x85a5, 0x00},
+	{0x85a6, 0x08},
+	{0x85a7, 0x76},
+	{0x85a8, 0x08},
+	{0x85a9, 0xc3},
+	{0x85aa, 0x78},
+	{0x85ab, 0x50},
+	{0x85ac, 0xe6},
+	{0x85ad, 0x94},
+	{0x85ae, 0x08},
+	{0x85af, 0x18},
+	{0x85b0, 0xe6},
+	{0x85b1, 0x94},
+	{0x85b2, 0x00},
+	{0x85b3, 0x50},
+	{0x85b4, 0x05},
+	{0x85b5, 0x76},
+	{0x85b6, 0x00},
+	{0x85b7, 0x08},
+	{0x85b8, 0x76},
+	{0x85b9, 0x08},
+	{0x85ba, 0x78},
+	{0x85bb, 0x4d},
+	{0x85bc, 0x12},
+	{0x85bd, 0x08},
+	{0x85be, 0xe8},
+	{0x85bf, 0xff},
+	{0x85c0, 0xc3},
+	{0x85c1, 0x78},
+	{0x85c2, 0x4a},
+	{0x85c3, 0xe6},
+	{0x85c4, 0x9f},
+	{0x85c5, 0xff},
+	{0x85c6, 0x18},
+	{0x85c7, 0xe6},
+	{0x85c8, 0x9e},
+	{0x85c9, 0x78},
+	{0x85ca, 0x51},
+	{0x85cb, 0x12},
+	{0x85cc, 0x08},
+	{0x85cd, 0xdf},
+	{0x85ce, 0xff},
+	{0x85cf, 0xc3},
+	{0x85d0, 0x78},
+	{0x85d1, 0x4c},
+	{0x85d2, 0xe6},
+	{0x85d3, 0x9f},
+	{0x85d4, 0xff},
+	{0x85d5, 0x18},
+	{0x85d6, 0xe6},
+	{0x85d7, 0x9e},
+	{0x85d8, 0xfe},
+	{0x85d9, 0xe4},
+	{0x85da, 0xfc},
+	{0x85db, 0xfd},
+	{0x85dc, 0x78},
+	{0x85dd, 0x55},
+	{0x85de, 0x12},
+	{0x85df, 0x04},
+	{0x85e0, 0x64},
+	{0x85e1, 0x78},
+	{0x85e2, 0x4d},
+	{0x85e3, 0x12},
+	{0x85e4, 0x08},
+	{0x85e5, 0xe8},
+	{0x85e6, 0x78},
+	{0x85e7, 0x4a},
+	{0x85e8, 0x26},
+	{0x85e9, 0xff},
+	{0x85ea, 0xee},
+	{0x85eb, 0x18},
+	{0x85ec, 0x36},
+	{0x85ed, 0x78},
+	{0x85ee, 0x59},
+	{0x85ef, 0x12},
+	{0x85f0, 0x08},
+	{0x85f1, 0xdf},
+	{0x85f2, 0x78},
+	{0x85f3, 0x4c},
+	{0x85f4, 0x26},
+	{0x85f5, 0xff},
+	{0x85f6, 0xee},
+	{0x85f7, 0x18},
+	{0x85f8, 0x36},
+	{0x85f9, 0xfe},
+	{0x85fa, 0xe4},
+	{0x85fb, 0xfc},
+	{0x85fc, 0xfd},
+	{0x85fd, 0x78},
+	{0x85fe, 0x5d},
+	{0x85ff, 0x12},
+	{0x8600, 0x04},
+	{0x8601, 0x64},
+	{0x8602, 0x78},
+	{0x8603, 0x51},
+	{0x8604, 0x12},
+	{0x8605, 0x09},
+	{0x8606, 0x13},
+	{0x8607, 0x50},
+	{0x8608, 0x09},
+	{0x8609, 0x78},
+	{0x860a, 0x51},
+	{0x860b, 0x12},
+	{0x860c, 0x04},
+	{0x860d, 0x70},
+	{0x860e, 0x00},
+	{0x860f, 0x00},
+	{0x8610, 0x00},
+	{0x8611, 0x00},
+	{0x8612, 0x78},
+	{0x8613, 0x55},
+	{0x8614, 0x12},
+	{0x8615, 0x09},
+	{0x8616, 0x13},
+	{0x8617, 0x50},
+	{0x8618, 0x09},
+	{0x8619, 0x78},
+	{0x861a, 0x55},
+	{0x861b, 0x12},
+	{0x861c, 0x04},
+	{0x861d, 0x70},
+	{0x861e, 0x00},
+	{0x861f, 0x00},
+	{0x8620, 0x00},
+	{0x8621, 0x00},
+	{0x8622, 0x12},
+	{0x8623, 0x08},
+	{0x8624, 0xf0},
+	{0x8625, 0x78},
+	{0x8626, 0x59},
+	{0x8627, 0x12},
+	{0x8628, 0x04},
+	{0x8629, 0x57},
+	{0x862a, 0xd3},
+	{0x862b, 0x12},
+	{0x862c, 0x04},
+	{0x862d, 0x23},
+	{0x862e, 0x40},
+	{0x862f, 0x08},
+	{0x8630, 0x12},
+	{0x8631, 0x08},
+	{0x8632, 0xf0},
+	{0x8633, 0x78},
+	{0x8634, 0x59},
+	{0x8635, 0x12},
+	{0x8636, 0x04},
+	{0x8637, 0x64},
+	{0x8638, 0x78},
+	{0x8639, 0x47},
+	{0x863a, 0x12},
+	{0x863b, 0x08},
+	{0x863c, 0xf2},
+	{0x863d, 0x78},
+	{0x863e, 0x5d},
+	{0x863f, 0x12},
+	{0x8640, 0x04},
+	{0x8641, 0x57},
+	{0x8642, 0xd3},
+	{0x8643, 0x12},
+	{0x8644, 0x04},
+	{0x8645, 0x23},
+	{0x8646, 0x40},
+	{0x8647, 0x0a},
+	{0x8648, 0x78},
+	{0x8649, 0x47},
+	{0x864a, 0x12},
+	{0x864b, 0x08},
+	{0x864c, 0xf2},
+	{0x864d, 0x78},
+	{0x864e, 0x5d},
+	{0x864f, 0x12},
+	{0x8650, 0x04},
+	{0x8651, 0x64},
+	{0x8652, 0xe4},
+	{0x8653, 0xfd},
+	{0x8654, 0x78},
+	{0x8655, 0x54},
+	{0x8656, 0x12},
+	{0x8657, 0x09},
+	{0x8658, 0x0b},
+	{0x8659, 0x24},
+	{0x865a, 0x01},
+	{0x865b, 0x12},
+	{0x865c, 0x08},
+	{0x865d, 0xd3},
+	{0x865e, 0x78},
+	{0x865f, 0x58},
+	{0x8660, 0x12},
+	{0x8661, 0x09},
+	{0x8662, 0x0b},
+	{0x8663, 0x24},
+	{0x8664, 0x02},
+	{0x8665, 0x12},
+	{0x8666, 0x08},
+	{0x8667, 0xd3},
+	{0x8668, 0x78},
+	{0x8669, 0x5c},
+	{0x866a, 0x12},
+	{0x866b, 0x09},
+	{0x866c, 0x0b},
+	{0x866d, 0x24},
+	{0x866e, 0x03},
+	{0x866f, 0x12},
+	{0x8670, 0x08},
+	{0x8671, 0xd3},
+	{0x8672, 0x78},
+	{0x8673, 0x60},
+	{0x8674, 0x12},
+	{0x8675, 0x09},
+	{0x8676, 0x0b},
+	{0x8677, 0x24},
+	{0x8678, 0x04},
+	{0x8679, 0x12},
+	{0x867a, 0x08},
+	{0x867b, 0xd3},
+	{0x867c, 0x0d},
+	{0x867d, 0xbd},
+	{0x867e, 0x05},
+	{0x867f, 0xd4},
+	{0x8680, 0xc2},
+	{0x8681, 0x0e},
+	{0x8682, 0xc2},
+	{0x8683, 0x06},
+	{0x8684, 0x22},
+	{0x8685, 0x85},
+	{0x8686, 0x08},
+	{0x8687, 0x36},
+	{0x8688, 0x90},
+	{0x8689, 0x30},
+	{0x868a, 0x24},
+	{0x868b, 0xe0},
+	{0x868c, 0xf5},
+	{0x868d, 0x32},
+	{0x868e, 0xa3},
+	{0x868f, 0xe0},
+	{0x8690, 0xf5},
+	{0x8691, 0x33},
+	{0x8692, 0xa3},
+	{0x8693, 0xe0},
+	{0x8694, 0xf5},
+	{0x8695, 0x34},
+	{0x8696, 0xa3},
+	{0x8697, 0xe0},
+	{0x8698, 0xf5},
+	{0x8699, 0x35},
+	{0x869a, 0xa3},
+	{0x869b, 0xe0},
+	{0x869c, 0xf5},
+	{0x869d, 0x31},
+	{0x869e, 0xd2},
+	{0x869f, 0x33},
+	{0x86a0, 0xe5},
+	{0x86a1, 0x36},
+	{0x86a2, 0x12},
+	{0x86a3, 0x04},
+	{0x86a4, 0x95},
+	{0x86a5, 0x06},
+	{0x86a6, 0xdf},
+	{0x86a7, 0x03},
+	{0x86a8, 0x06},
+	{0x86a9, 0xe3},
+	{0x86aa, 0x04},
+	{0x86ab, 0x06},
+	{0x86ac, 0xe9},
+	{0x86ad, 0x07},
+	{0x86ae, 0x06},
+	{0x86af, 0xf1},
+	{0x86b0, 0x08},
+	{0x86b1, 0x07},
+	{0x86b2, 0x14},
+	{0x86b3, 0x18},
+	{0x86b4, 0x07},
+	{0x86b5, 0x2a},
+	{0x86b6, 0x19},
+	{0x86b7, 0x07},
+	{0x86b8, 0x01},
+	{0x86b9, 0x1a},
+	{0x86ba, 0x07},
+	{0x86bb, 0x0c},
+	{0x86bc, 0x1b},
+	{0x86bd, 0x07},
+	{0x86be, 0x4e},
+	{0x86bf, 0x80},
+	{0x86c0, 0x07},
+	{0x86c1, 0x51},
+	{0x86c2, 0x81},
+	{0x86c3, 0x07},
+	{0x86c4, 0x6d},
+	{0x86c5, 0x8f},
+	{0x86c6, 0x07},
+	{0x86c7, 0x5c},
+	{0x86c8, 0x90},
+	{0x86c9, 0x07},
+	{0x86ca, 0x6d},
+	{0x86cb, 0x91},
+	{0x86cc, 0x07},
+	{0x86cd, 0x6d},
+	{0x86ce, 0x92},
+	{0x86cf, 0x07},
+	{0x86d0, 0x6d},
+	{0x86d1, 0x93},
+	{0x86d2, 0x07},
+	{0x86d3, 0x6d},
+	{0x86d4, 0x94},
+	{0x86d5, 0x07},
+	{0x86d6, 0x6d},
+	{0x86d7, 0x98},
+	{0x86d8, 0x07},
+	{0x86d9, 0x6a},
+	{0x86da, 0x9f},
+	{0x86db, 0x00},
+	{0x86dc, 0x00},
+	{0x86dd, 0x07},
+	{0x86de, 0x88},
+	{0x86df, 0x12},
+	{0x86e0, 0x0a},
+	{0x86e1, 0xb8},
+	{0x86e2, 0x22},
+	{0x86e3, 0x12},
+	{0x86e4, 0x0a},
+	{0x86e5, 0xb8},
+	{0x86e6, 0xd2},
+	{0x86e7, 0x03},
+	{0x86e8, 0x22},
+	{0x86e9, 0xa2},
+	{0x86ea, 0x36},
+	{0x86eb, 0xe4},
+	{0x86ec, 0x33},
+	{0x86ed, 0xf5},
+	{0x86ee, 0x31},
+	{0x86ef, 0x80},
+	{0x86f0, 0x7c},
+	{0x86f1, 0xc2},
+	{0x86f2, 0x01},
+	{0x86f3, 0xc2},
+	{0x86f4, 0x02},
+	{0x86f5, 0xc2},
+	{0x86f6, 0x03},
+	{0x86f7, 0x12},
+	{0x86f8, 0x09},
+	{0x86f9, 0x34},
+	{0x86fa, 0x75},
+	{0x86fb, 0x3f},
+	{0x86fc, 0x70},
+	{0x86fd, 0xd2},
+	{0x86fe, 0x34},
+	{0x86ff, 0x80},
+	{0x8700, 0x6c},
+	{0x8701, 0x85},
+	{0x8702, 0x35},
+	{0x8703, 0x3d},
+	{0x8704, 0x85},
+	{0x8705, 0x31},
+	{0x8706, 0x3e},
+	{0x8707, 0x12},
+	{0x8708, 0x08},
+	{0x8709, 0x1d},
+	{0x870a, 0x80},
+	{0x870b, 0x61},
+	{0x870c, 0x85},
+	{0x870d, 0x3d},
+	{0x870e, 0x35},
+	{0x870f, 0x85},
+	{0x8710, 0x3e},
+	{0x8711, 0x31},
+	{0x8712, 0x80},
+	{0x8713, 0x59},
+	{0x8714, 0xe4},
+	{0x8715, 0xf5},
+	{0x8716, 0x22},
+	{0x8717, 0xf5},
+	{0x8718, 0x23},
+	{0x8719, 0x85},
+	{0x871a, 0x35},
+	{0x871b, 0x1e},
+	{0x871c, 0x85},
+	{0x871d, 0x34},
+	{0x871e, 0x1d},
+	{0x871f, 0x85},
+	{0x8720, 0x33},
+	{0x8721, 0x1c},
+	{0x8722, 0x85},
+	{0x8723, 0x32},
+	{0x8724, 0x1b},
+	{0x8725, 0x12},
+	{0x8726, 0x0a},
+	{0x8727, 0x8a},
+	{0x8728, 0x80},
+	{0x8729, 0x1f},
+	{0x872a, 0x75},
+	{0x872b, 0x22},
+	{0x872c, 0x00},
+	{0x872d, 0x75},
+	{0x872e, 0x23},
+	{0x872f, 0x01},
+	{0x8730, 0x74},
+	{0x8731, 0xff},
+	{0x8732, 0xf5},
+	{0x8733, 0x1a},
+	{0x8734, 0xf5},
+	{0x8735, 0x19},
+	{0x8736, 0xf5},
+	{0x8737, 0x18},
+	{0x8738, 0xf5},
+	{0x8739, 0x17},
+	{0x873a, 0x12},
+	{0x873b, 0x0a},
+	{0x873c, 0x8a},
+	{0x873d, 0x85},
+	{0x873e, 0x1a},
+	{0x873f, 0x35},
+	{0x8740, 0x85},
+	{0x8741, 0x19},
+	{0x8742, 0x34},
+	{0x8743, 0x85},
+	{0x8744, 0x18},
+	{0x8745, 0x33},
+	{0x8746, 0x85},
+	{0x8747, 0x17},
+	{0x8748, 0x32},
+	{0x8749, 0xe4},
+	{0x874a, 0xf5},
+	{0x874b, 0x31},
+	{0x874c, 0x80},
+	{0x874d, 0x1f},
+	{0x874e, 0x02},
+	{0x874f, 0x0a},
+	{0x8750, 0xef},
+	{0x8751, 0x85},
+	{0x8752, 0x32},
+	{0x8753, 0x38},
+	{0x8754, 0x85},
+	{0x8755, 0x33},
+	{0x8756, 0x39},
+	{0x8757, 0x12},
+	{0x8758, 0x04},
+	{0x8759, 0xbb},
+	{0x875a, 0x80},
+	{0x875b, 0x11},
+	{0x875c, 0x85},
+	{0x875d, 0x35},
+	{0x875e, 0x3b},
+	{0x875f, 0x85},
+	{0x8760, 0x34},
+	{0x8761, 0x3a},
+	{0x8762, 0x85},
+	{0x8763, 0x33},
+	{0x8764, 0x39},
+	{0x8765, 0x85},
+	{0x8766, 0x32},
+	{0x8767, 0x38},
+	{0x8768, 0x80},
+	{0x8769, 0x03},
+	{0x876a, 0x02},
+	{0x876b, 0x04},
+	{0x876c, 0xbb},
+	{0x876d, 0x90},
+	{0x876e, 0x30},
+	{0x876f, 0x24},
+	{0x8770, 0xe5},
+	{0x8771, 0x32},
+	{0x8772, 0xf0},
+	{0x8773, 0xa3},
+	{0x8774, 0xe5},
+	{0x8775, 0x33},
+	{0x8776, 0xf0},
+	{0x8777, 0xa3},
+	{0x8778, 0xe5},
+	{0x8779, 0x34},
+	{0x877a, 0xf0},
+	{0x877b, 0xa3},
+	{0x877c, 0xe5},
+	{0x877d, 0x35},
+	{0x877e, 0xf0},
+	{0x877f, 0xa3},
+	{0x8780, 0xe5},
+	{0x8781, 0x31},
+	{0x8782, 0xf0},
+	{0x8783, 0x90},
+	{0x8784, 0x30},
+	{0x8785, 0x23},
+	{0x8786, 0xe4},
+	{0x8787, 0xf0},
+	{0x8788, 0x22},
+	{0x8789, 0xc0},
+	{0x878a, 0xe0},
+	{0x878b, 0xc0},
+	{0x878c, 0x83},
+	{0x878d, 0xc0},
+	{0x878e, 0x82},
+	{0x878f, 0xc0},
+	{0x8790, 0xd0},
+	{0x8791, 0x90},
+	{0x8792, 0x3f},
+	{0x8793, 0x0c},
+	{0x8794, 0xe0},
+	{0x8795, 0xf5},
+	{0x8796, 0x27},
+	{0x8797, 0xe5},
+	{0x8798, 0x27},
+	{0x8799, 0x30},
+	{0x879a, 0xe3},
+	{0x879b, 0x42},
+	{0x879c, 0x30},
+	{0x879d, 0x35},
+	{0x879e, 0x34},
+	{0x879f, 0x90},
+	{0x87a0, 0x60},
+	{0x87a1, 0x19},
+	{0x87a2, 0xe0},
+	{0x87a3, 0xf5},
+	{0x87a4, 0x0a},
+	{0x87a5, 0xa3},
+	{0x87a6, 0xe0},
+	{0x87a7, 0xf5},
+	{0x87a8, 0x0b},
+	{0x87a9, 0x30},
+	{0x87aa, 0x01},
+	{0x87ab, 0x06},
+	{0x87ac, 0x30},
+	{0x87ad, 0x32},
+	{0x87ae, 0x03},
+	{0x87af, 0xd3},
+	{0x87b0, 0x80},
+	{0x87b1, 0x01},
+	{0x87b2, 0xc3},
+	{0x87b3, 0x92},
+	{0x87b4, 0x09},
+	{0x87b5, 0x30},
+	{0x87b6, 0x02},
+	{0x87b7, 0x06},
+	{0x87b8, 0x30},
+	{0x87b9, 0x32},
+	{0x87ba, 0x03},
+	{0x87bb, 0xd3},
+	{0x87bc, 0x80},
+	{0x87bd, 0x01},
+	{0x87be, 0xc3},
+	{0x87bf, 0x92},
+	{0x87c0, 0x0a},
+	{0x87c1, 0x30},
+	{0x87c2, 0x32},
+	{0x87c3, 0x0c},
+	{0x87c4, 0x30},
+	{0x87c5, 0x03},
+	{0x87c6, 0x09},
+	{0x87c7, 0x20},
+	{0x87c8, 0x02},
+	{0x87c9, 0x06},
+	{0x87ca, 0x20},
+	{0x87cb, 0x01},
+	{0x87cc, 0x03},
+	{0x87cd, 0xd3},
+	{0x87ce, 0x80},
+	{0x87cf, 0x01},
+	{0x87d0, 0xc3},
+	{0x87d1, 0x92},
+	{0x87d2, 0x0b},
+	{0x87d3, 0x90},
+	{0x87d4, 0x30},
+	{0x87d5, 0x01},
+	{0x87d6, 0xe0},
+	{0x87d7, 0x44},
+	{0x87d8, 0x40},
+	{0x87d9, 0xf0},
+	{0x87da, 0xe0},
+	{0x87db, 0x54},
+	{0x87dc, 0xbf},
+	{0x87dd, 0xf0},
+	{0x87de, 0xe5},
+	{0x87df, 0x27},
+	{0x87e0, 0x30},
+	{0x87e1, 0xe1},
+	{0x87e2, 0x14},
+	{0x87e3, 0x30},
+	{0x87e4, 0x33},
+	{0x87e5, 0x11},
+	{0x87e6, 0x90},
+	{0x87e7, 0x30},
+	{0x87e8, 0x22},
+	{0x87e9, 0xe0},
+	{0x87ea, 0xf5},
+	{0x87eb, 0x08},
+	{0x87ec, 0xe4},
+	{0x87ed, 0xf0},
+	{0x87ee, 0x30},
+	{0x87ef, 0x00},
+	{0x87f0, 0x03},
+	{0x87f1, 0xd3},
+	{0x87f2, 0x80},
+	{0x87f3, 0x01},
+	{0x87f4, 0xc3},
+	{0x87f5, 0x92},
+	{0x87f6, 0x08},
+	{0x87f7, 0xe5},
+	{0x87f8, 0x27},
+	{0x87f9, 0x30},
+	{0x87fa, 0xe5},
+	{0x87fb, 0x12},
+	{0x87fc, 0x90},
+	{0x87fd, 0x56},
+	{0x87fe, 0x90},
+	{0x87ff, 0xe0},
+	{0x8800, 0xf5},
+	{0x8801, 0x09},
+	{0x8802, 0x30},
+	{0x8803, 0x30},
+	{0x8804, 0x09},
+	{0x8805, 0x30},
+	{0x8806, 0x05},
+	{0x8807, 0x03},
+	{0x8808, 0xd3},
+	{0x8809, 0x80},
+	{0x880a, 0x01},
+	{0x880b, 0xc3},
+	{0x880c, 0x92},
+	{0x880d, 0x0d},
+	{0x880e, 0x90},
+	{0x880f, 0x3f},
+	{0x8810, 0x0c},
+	{0x8811, 0xe5},
+	{0x8812, 0x27},
+	{0x8813, 0xf0},
+	{0x8814, 0xd0},
+	{0x8815, 0xd0},
+	{0x8816, 0xd0},
+	{0x8817, 0x82},
+	{0x8818, 0xd0},
+	{0x8819, 0x83},
+	{0x881a, 0xd0},
+	{0x881b, 0xe0},
+	{0x881c, 0x32},
+	{0x881d, 0x90},
+	{0x881e, 0x0e},
+	{0x881f, 0x7d},
+	{0x8820, 0xe4},
+	{0x8821, 0x93},
+	{0x8822, 0xfe},
+	{0x8823, 0x74},
+	{0x8824, 0x01},
+	{0x8825, 0x93},
+	{0x8826, 0xff},
+	{0x8827, 0xc3},
+	{0x8828, 0x90},
+	{0x8829, 0x0e},
+	{0x882a, 0x7b},
+	{0x882b, 0x74},
+	{0x882c, 0x01},
+	{0x882d, 0x93},
+	{0x882e, 0x9f},
+	{0x882f, 0xff},
+	{0x8830, 0xe4},
+	{0x8831, 0x93},
+	{0x8832, 0x9e},
+	{0x8833, 0xfe},
+	{0x8834, 0xe4},
+	{0x8835, 0x8f},
+	{0x8836, 0x30},
+	{0x8837, 0x8e},
+	{0x8838, 0x2f},
+	{0x8839, 0xf5},
+	{0x883a, 0x2e},
+	{0x883b, 0xf5},
+	{0x883c, 0x2d},
+	{0x883d, 0xab},
+	{0x883e, 0x30},
+	{0x883f, 0xaa},
+	{0x8840, 0x2f},
+	{0x8841, 0xa9},
+	{0x8842, 0x2e},
+	{0x8843, 0xa8},
+	{0x8844, 0x2d},
+	{0x8845, 0xaf},
+	{0x8846, 0x3e},
+	{0x8847, 0xfc},
+	{0x8848, 0xfd},
+	{0x8849, 0xfe},
+	{0x884a, 0x12},
+	{0x884b, 0x03},
+	{0x884c, 0x06},
+	{0x884d, 0x12},
+	{0x884e, 0x0a},
+	{0x884f, 0xd4},
+	{0x8850, 0xe4},
+	{0x8851, 0x7b},
+	{0x8852, 0xff},
+	{0x8853, 0xfa},
+	{0x8854, 0xf9},
+	{0x8855, 0xf8},
+	{0x8856, 0x12},
+	{0x8857, 0x03},
+	{0x8858, 0x91},
+	{0x8859, 0x12},
+	{0x885a, 0x0a},
+	{0x885b, 0xd4},
+	{0x885c, 0x90},
+	{0x885d, 0x0e},
+	{0x885e, 0x69},
+	{0x885f, 0xe4},
+	{0x8860, 0x12},
+	{0x8861, 0x0a},
+	{0x8862, 0xe9},
+	{0x8863, 0x12},
+	{0x8864, 0x0a},
+	{0x8865, 0xd4},
+	{0x8866, 0xe4},
+	{0x8867, 0x85},
+	{0x8868, 0x3d},
+	{0x8869, 0x2c},
+	{0x886a, 0xf5},
+	{0x886b, 0x2b},
+	{0x886c, 0xf5},
+	{0x886d, 0x2a},
+	{0x886e, 0xf5},
+	{0x886f, 0x29},
+	{0x8870, 0xaf},
+	{0x8871, 0x2c},
+	{0x8872, 0xae},
+	{0x8873, 0x2b},
+	{0x8874, 0xad},
+	{0x8875, 0x2a},
+	{0x8876, 0xac},
+	{0x8877, 0x29},
+	{0x8878, 0xa3},
+	{0x8879, 0x12},
+	{0x887a, 0x0a},
+	{0x887b, 0xe9},
+	{0x887c, 0x8f},
+	{0x887d, 0x2c},
+	{0x887e, 0x8e},
+	{0x887f, 0x2b},
+	{0x8880, 0x8d},
+	{0x8881, 0x2a},
+	{0x8882, 0x8c},
+	{0x8883, 0x29},
+	{0x8884, 0xe5},
+	{0x8885, 0x30},
+	{0x8886, 0x45},
+	{0x8887, 0x2c},
+	{0x8888, 0xf5},
+	{0x8889, 0x30},
+	{0x888a, 0xe5},
+	{0x888b, 0x2f},
+	{0x888c, 0x45},
+	{0x888d, 0x2b},
+	{0x888e, 0xf5},
+	{0x888f, 0x2f},
+	{0x8890, 0xe5},
+	{0x8891, 0x2e},
+	{0x8892, 0x45},
+	{0x8893, 0x2a},
+	{0x8894, 0xf5},
+	{0x8895, 0x2e},
+	{0x8896, 0xe5},
+	{0x8897, 0x2d},
+	{0x8898, 0x45},
+	{0x8899, 0x29},
+	{0x889a, 0xf5},
+	{0x889b, 0x2d},
+	{0x889c, 0xe4},
+	{0x889d, 0xf5},
+	{0x889e, 0x22},
+	{0x889f, 0xf5},
+	{0x88a0, 0x23},
+	{0x88a1, 0x85},
+	{0x88a2, 0x30},
+	{0x88a3, 0x1e},
+	{0x88a4, 0x85},
+	{0x88a5, 0x2f},
+	{0x88a6, 0x1d},
+	{0x88a7, 0x85},
+	{0x88a8, 0x2e},
+	{0x88a9, 0x1c},
+	{0x88aa, 0x85},
+	{0x88ab, 0x2d},
+	{0x88ac, 0x1b},
+	{0x88ad, 0x02},
+	{0x88ae, 0x0a},
+	{0x88af, 0x8a},
+	{0x88b0, 0x78},
+	{0x88b1, 0x45},
+	{0x88b2, 0x7e},
+	{0x88b3, 0x00},
+	{0x88b4, 0xe6},
+	{0x88b5, 0xfc},
+	{0x88b6, 0x08},
+	{0x88b7, 0xe6},
+	{0x88b8, 0xfd},
+	{0x88b9, 0x12},
+	{0x88ba, 0x02},
+	{0x88bb, 0x9f},
+	{0x88bc, 0x7c},
+	{0x88bd, 0x00},
+	{0x88be, 0x22},
+	{0x88bf, 0xe0},
+	{0x88c0, 0xa3},
+	{0x88c1, 0xe0},
+	{0x88c2, 0x75},
+	{0x88c3, 0xf0},
+	{0x88c4, 0x02},
+	{0x88c5, 0xa4},
+	{0x88c6, 0xff},
+	{0x88c7, 0xae},
+	{0x88c8, 0xf0},
+	{0x88c9, 0xc3},
+	{0x88ca, 0x08},
+	{0x88cb, 0xe6},
+	{0x88cc, 0x9f},
+	{0x88cd, 0xf6},
+	{0x88ce, 0x18},
+	{0x88cf, 0xe6},
+	{0x88d0, 0x9e},
+	{0x88d1, 0xf6},
+	{0x88d2, 0x22},
+	{0x88d3, 0xff},
+	{0x88d4, 0xe5},
+	{0x88d5, 0xf0},
+	{0x88d6, 0x34},
+	{0x88d7, 0x60},
+	{0x88d8, 0x8f},
+	{0x88d9, 0x82},
+	{0x88da, 0xf5},
+	{0x88db, 0x83},
+	{0x88dc, 0xec},
+	{0x88dd, 0xf0},
+	{0x88de, 0x22},
+	{0x88df, 0xfe},
+	{0x88e0, 0xe4},
+	{0x88e1, 0xfc},
+	{0x88e2, 0xfd},
+	{0x88e3, 0x12},
+	{0x88e4, 0x04},
+	{0x88e5, 0x64},
+	{0x88e6, 0x78},
+	{0x88e7, 0x4f},
+	{0x88e8, 0xe6},
+	{0x88e9, 0xc3},
+	{0x88ea, 0x13},
+	{0x88eb, 0xfe},
+	{0x88ec, 0x08},
+	{0x88ed, 0xe6},
+	{0x88ee, 0x13},
+	{0x88ef, 0x22},
+	{0x88f0, 0x78},
+	{0x88f1, 0x45},
+	{0x88f2, 0xe6},
+	{0x88f3, 0xfe},
+	{0x88f4, 0x08},
+	{0x88f5, 0xe6},
+	{0x88f6, 0xff},
+	{0x88f7, 0xe4},
+	{0x88f8, 0xfc},
+	{0x88f9, 0xfd},
+	{0x88fa, 0x22},
+	{0x88fb, 0xe7},
+	{0x88fc, 0xc4},
+	{0x88fd, 0xf8},
+	{0x88fe, 0x54},
+	{0x88ff, 0xf0},
+	{0x8900, 0xc8},
+	{0x8901, 0x68},
+	{0x8902, 0xf7},
+	{0x8903, 0x09},
+	{0x8904, 0xe7},
+	{0x8905, 0xc4},
+	{0x8906, 0x54},
+	{0x8907, 0x0f},
+	{0x8908, 0x48},
+	{0x8909, 0xf7},
+	{0x890a, 0x22},
+	{0x890b, 0xe6},
+	{0x890c, 0xfc},
+	{0x890d, 0xed},
+	{0x890e, 0x75},
+	{0x890f, 0xf0},
+	{0x8910, 0x04},
+	{0x8911, 0xa4},
+	{0x8912, 0x22},
+	{0x8913, 0xe4},
+	{0x8914, 0xff},
+	{0x8915, 0xfe},
+	{0x8916, 0xfd},
+	{0x8917, 0xfc},
+	{0x8918, 0x12},
+	{0x8919, 0x04},
+	{0x891a, 0x57},
+	{0x891b, 0xc3},
+	{0x891c, 0x02},
+	{0x891d, 0x04},
+	{0x891e, 0x23},
+	{0x891f, 0xe0},
+	{0x8920, 0xfe},
+	{0x8921, 0xa3},
+	{0x8922, 0xe0},
+	{0x8923, 0xfd},
+	{0x8924, 0xee},
+	{0x8925, 0xf6},
+	{0x8926, 0xed},
+	{0x8927, 0x08},
+	{0x8928, 0xf6},
+	{0x8929, 0x22},
+	{0x892a, 0xe6},
+	{0x892b, 0xc3},
+	{0x892c, 0x13},
+	{0x892d, 0xf7},
+	{0x892e, 0x08},
+	{0x892f, 0xe6},
+	{0x8930, 0x13},
+	{0x8931, 0x09},
+	{0x8932, 0xf7},
+	{0x8933, 0x22},
+	{0x8934, 0xe4},
+	{0x8935, 0xf5},
+	{0x8936, 0x3e},
+	{0x8937, 0x90},
+	{0x8938, 0x0e},
+	{0x8939, 0x77},
+	{0x893a, 0x93},
+	{0x893b, 0xff},
+	{0x893c, 0xe4},
+	{0x893d, 0x8f},
+	{0x893e, 0x2c},
+	{0x893f, 0xf5},
+	{0x8940, 0x2b},
+	{0x8941, 0xf5},
+	{0x8942, 0x2a},
+	{0x8943, 0xf5},
+	{0x8944, 0x29},
+	{0x8945, 0xaf},
+	{0x8946, 0x2c},
+	{0x8947, 0xae},
+	{0x8948, 0x2b},
+	{0x8949, 0xad},
+	{0x894a, 0x2a},
+	{0x894b, 0xac},
+	{0x894c, 0x29},
+	{0x894d, 0x90},
+	{0x894e, 0x0e},
+	{0x894f, 0x6a},
+	{0x8950, 0x12},
+	{0x8951, 0x0a},
+	{0x8952, 0xe9},
+	{0x8953, 0x8f},
+	{0x8954, 0x2c},
+	{0x8955, 0x8e},
+	{0x8956, 0x2b},
+	{0x8957, 0x8d},
+	{0x8958, 0x2a},
+	{0x8959, 0x8c},
+	{0x895a, 0x29},
+	{0x895b, 0x90},
+	{0x895c, 0x0e},
+	{0x895d, 0x72},
+	{0x895e, 0x12},
+	{0x895f, 0x04},
+	{0x8960, 0x47},
+	{0x8961, 0xef},
+	{0x8962, 0x45},
+	{0x8963, 0x2c},
+	{0x8964, 0xf5},
+	{0x8965, 0x2c},
+	{0x8966, 0xee},
+	{0x8967, 0x45},
+	{0x8968, 0x2b},
+	{0x8969, 0xf5},
+	{0x896a, 0x2b},
+	{0x896b, 0xed},
+	{0x896c, 0x45},
+	{0x896d, 0x2a},
+	{0x896e, 0xf5},
+	{0x896f, 0x2a},
+	{0x8970, 0xec},
+	{0x8971, 0x45},
+	{0x8972, 0x29},
+	{0x8973, 0xf5},
+	{0x8974, 0x29},
+	{0x8975, 0xe4},
+	{0x8976, 0xf5},
+	{0x8977, 0x22},
+	{0x8978, 0xf5},
+	{0x8979, 0x23},
+	{0x897a, 0x85},
+	{0x897b, 0x2c},
+	{0x897c, 0x1e},
+	{0x897d, 0x85},
+	{0x897e, 0x2b},
+	{0x897f, 0x1d},
+	{0x8980, 0x85},
+	{0x8981, 0x2a},
+	{0x8982, 0x1c},
+	{0x8983, 0x85},
+	{0x8984, 0x29},
+	{0x8985, 0x1b},
+	{0x8986, 0x12},
+	{0x8987, 0x0a},
+	{0x8988, 0x8a},
+	{0x8989, 0xe4},
+	{0x898a, 0xf5},
+	{0x898b, 0x22},
+	{0x898c, 0xf5},
+	{0x898d, 0x23},
+	{0x898e, 0x90},
+	{0x898f, 0x0e},
+	{0x8990, 0x72},
+	{0x8991, 0x12},
+	{0x8992, 0x0a},
+	{0x8993, 0xdd},
+	{0x8994, 0x12},
+	{0x8995, 0x0a},
+	{0x8996, 0x8a},
+	{0x8997, 0xe4},
+	{0x8998, 0xf5},
+	{0x8999, 0x22},
+	{0x899a, 0xf5},
+	{0x899b, 0x23},
+	{0x899c, 0x90},
+	{0x899d, 0x0e},
+	{0x899e, 0x6e},
+	{0x899f, 0x12},
+	{0x89a0, 0x0a},
+	{0x89a1, 0xdd},
+	{0x89a2, 0x02},
+	{0x89a3, 0x0a},
+	{0x89a4, 0x8a},
+	{0x89a5, 0x75},
+	{0x89a6, 0x89},
+	{0x89a7, 0x03},
+	{0x89a8, 0x75},
+	{0x89a9, 0xa8},
+	{0x89aa, 0x01},
+	{0x89ab, 0x75},
+	{0x89ac, 0xb8},
+	{0x89ad, 0x04},
+	{0x89ae, 0x75},
+	{0x89af, 0x29},
+	{0x89b0, 0xff},
+	{0x89b1, 0x75},
+	{0x89b2, 0x2a},
+	{0x89b3, 0x0e},
+	{0x89b4, 0x75},
+	{0x89b5, 0x2b},
+	{0x89b6, 0x15},
+	{0x89b7, 0x75},
+	{0x89b8, 0x2c},
+	{0x89b9, 0x0d},
+	{0x89ba, 0x12},
+	{0x89bb, 0x0a},
+	{0x89bc, 0x0e},
+	{0x89bd, 0x12},
+	{0x89be, 0x00},
+	{0x89bf, 0x09},
+	{0x89c0, 0x12},
+	{0x89c1, 0x0a},
+	{0x89c2, 0xef},
+	{0x89c3, 0x12},
+	{0x89c4, 0x00},
+	{0x89c5, 0x06},
+	{0x89c6, 0xd2},
+	{0x89c7, 0x00},
+	{0x89c8, 0xd2},
+	{0x89c9, 0x33},
+	{0x89ca, 0xd2},
+	{0x89cb, 0xaf},
+	{0x89cc, 0x75},
+	{0x89cd, 0x29},
+	{0x89ce, 0xff},
+	{0x89cf, 0x75},
+	{0x89d0, 0x2a},
+	{0x89d1, 0x0e},
+	{0x89d2, 0x75},
+	{0x89d3, 0x2b},
+	{0x89d4, 0x49},
+	{0x89d5, 0x75},
+	{0x89d6, 0x2c},
+	{0x89d7, 0x03},
+	{0x89d8, 0x12},
+	{0x89d9, 0x0a},
+	{0x89da, 0x0e},
+	{0x89db, 0x30},
+	{0x89dc, 0x08},
+	{0x89dd, 0x09},
+	{0x89de, 0xc2},
+	{0x89df, 0x33},
+	{0x89e0, 0x12},
+	{0x89e1, 0x06},
+	{0x89e2, 0x85},
+	{0x89e3, 0xc2},
+	{0x89e4, 0x08},
+	{0x89e5, 0xd2},
+	{0x89e6, 0x33},
+	{0x89e7, 0x30},
+	{0x89e8, 0x09},
+	{0x89e9, 0x09},
+	{0x89ea, 0xc2},
+	{0x89eb, 0x35},
+	{0x89ec, 0x12},
+	{0x89ed, 0x00},
+	{0x89ee, 0x0e},
+	{0x89ef, 0xc2},
+	{0x89f0, 0x09},
+	{0x89f1, 0xd2},
+	{0x89f2, 0x35},
+	{0x89f3, 0x30},
+	{0x89f4, 0x0e},
+	{0x89f5, 0x03},
+	{0x89f6, 0x12},
+	{0x89f7, 0x04},
+	{0x89f8, 0xbb},
+	{0x89f9, 0x30},
+	{0x89fa, 0x34},
+	{0x89fb, 0xdf},
+	{0x89fc, 0x90},
+	{0x89fd, 0x30},
+	{0x89fe, 0x29},
+	{0x89ff, 0xe5},
+	{0x8a00, 0x3f},
+	{0x8a01, 0xf0},
+	{0x8a02, 0xb4},
+	{0x8a03, 0x10},
+	{0x8a04, 0x05},
+	{0x8a05, 0x90},
+	{0x8a06, 0x30},
+	{0x8a07, 0x23},
+	{0x8a08, 0xe4},
+	{0x8a09, 0xf0},
+	{0x8a0a, 0xc2},
+	{0x8a0b, 0x34},
+	{0x8a0c, 0x80},
+	{0x8a0d, 0xcd},
+	{0x8a0e, 0xae},
+	{0x8a0f, 0x2a},
+	{0x8a10, 0xaf},
+	{0x8a11, 0x2b},
+	{0x8a12, 0xe4},
+	{0x8a13, 0xfd},
+	{0x8a14, 0xed},
+	{0x8a15, 0xc3},
+	{0x8a16, 0x95},
+	{0x8a17, 0x2c},
+	{0x8a18, 0x50},
+	{0x8a19, 0x33},
+	{0x8a1a, 0x12},
+	{0x8a1b, 0x0b},
+	{0x8a1c, 0x36},
+	{0x8a1d, 0xe4},
+	{0x8a1e, 0x93},
+	{0x8a1f, 0xf5},
+	{0x8a20, 0x2d},
+	{0x8a21, 0x74},
+	{0x8a22, 0x01},
+	{0x8a23, 0x93},
+	{0x8a24, 0xf5},
+	{0x8a25, 0x2e},
+	{0x8a26, 0x45},
+	{0x8a27, 0x2d},
+	{0x8a28, 0x60},
+	{0x8a29, 0x23},
+	{0x8a2a, 0x85},
+	{0x8a2b, 0x2e},
+	{0x8a2c, 0x82},
+	{0x8a2d, 0x85},
+	{0x8a2e, 0x2d},
+	{0x8a2f, 0x83},
+	{0x8a30, 0xe0},
+	{0x8a31, 0xfc},
+	{0x8a32, 0x12},
+	{0x8a33, 0x0b},
+	{0x8a34, 0x36},
+	{0x8a35, 0x74},
+	{0x8a36, 0x03},
+	{0x8a37, 0x93},
+	{0x8a38, 0x52},
+	{0x8a39, 0x04},
+	{0x8a3a, 0x12},
+	{0x8a3b, 0x0b},
+	{0x8a3c, 0x36},
+	{0x8a3d, 0x74},
+	{0x8a3e, 0x02},
+	{0x8a3f, 0x93},
+	{0x8a40, 0x42},
+	{0x8a41, 0x04},
+	{0x8a42, 0x85},
+	{0x8a43, 0x2e},
+	{0x8a44, 0x82},
+	{0x8a45, 0x85},
+	{0x8a46, 0x2d},
+	{0x8a47, 0x83},
+	{0x8a48, 0xec},
+	{0x8a49, 0xf0},
+	{0x8a4a, 0x0d},
+	{0x8a4b, 0x80},
+	{0x8a4c, 0xc7},
+	{0x8a4d, 0x22},
+	{0x8a4e, 0x78},
+	{0x8a4f, 0xb1},
+	{0x8a50, 0xe6},
+	{0x8a51, 0xd3},
+	{0x8a52, 0x08},
+	{0x8a53, 0xff},
+	{0x8a54, 0xe6},
+	{0x8a55, 0x64},
+	{0x8a56, 0x80},
+	{0x8a57, 0xf8},
+	{0x8a58, 0xef},
+	{0x8a59, 0x64},
+	{0x8a5a, 0x80},
+	{0x8a5b, 0x98},
+	{0x8a5c, 0x22},
+	{0x8a5d, 0x93},
+	{0x8a5e, 0xff},
+	{0x8a5f, 0x7e},
+	{0x8a60, 0x00},
+	{0x8a61, 0xe6},
+	{0x8a62, 0xfc},
+	{0x8a63, 0x08},
+	{0x8a64, 0xe6},
+	{0x8a65, 0xfd},
+	{0x8a66, 0x12},
+	{0x8a67, 0x02},
+	{0x8a68, 0x9f},
+	{0x8a69, 0x78},
+	{0x8a6a, 0xb4},
+	{0x8a6b, 0xe6},
+	{0x8a6c, 0xfc},
+	{0x8a6d, 0x08},
+	{0x8a6e, 0xe6},
+	{0x8a6f, 0xfd},
+	{0x8a70, 0xd3},
+	{0x8a71, 0xef},
+	{0x8a72, 0x9d},
+	{0x8a73, 0xee},
+	{0x8a74, 0x9c},
+	{0x8a75, 0x22},
+	{0x8a76, 0x78},
+	{0x8a77, 0xb0},
+	{0x8a78, 0xd3},
+	{0x8a79, 0xe6},
+	{0x8a7a, 0x64},
+	{0x8a7b, 0x80},
+	{0x8a7c, 0x94},
+	{0x8a7d, 0x80},
+	{0x8a7e, 0x22},
+	{0x8a7f, 0x25},
+	{0x8a80, 0xe0},
+	{0x8a81, 0x24},
+	{0x8a82, 0x0a},
+	{0x8a83, 0xf8},
+	{0x8a84, 0xe6},
+	{0x8a85, 0xfe},
+	{0x8a86, 0x08},
+	{0x8a87, 0xe6},
+	{0x8a88, 0xff},
+	{0x8a89, 0x22},
+	{0x8a8a, 0xa2},
+	{0x8a8b, 0xaf},
+	{0x8a8c, 0x92},
+	{0x8a8d, 0x31},
+	{0x8a8e, 0xc2},
+	{0x8a8f, 0xaf},
+	{0x8a90, 0xe5},
+	{0x8a91, 0x23},
+	{0x8a92, 0x45},
+	{0x8a93, 0x22},
+	{0x8a94, 0x90},
+	{0x8a95, 0x0e},
+	{0x8a96, 0x5d},
+	{0x8a97, 0x60},
+	{0x8a98, 0x0b},
+	{0x8a99, 0x12},
+	{0x8a9a, 0x0b},
+	{0x8a9b, 0x2b},
+	{0x8a9c, 0xe0},
+	{0x8a9d, 0xf5},
+	{0x8a9e, 0x19},
+	{0x8a9f, 0xe0},
+	{0x8aa0, 0xf5},
+	{0x8aa1, 0x1a},
+	{0x8aa2, 0x80},
+	{0x8aa3, 0x0f},
+	{0x8aa4, 0x12},
+	{0x8aa5, 0x0b},
+	{0x8aa6, 0x2b},
+	{0x8aa7, 0xe5},
+	{0x8aa8, 0x1d},
+	{0x8aa9, 0xf0},
+	{0x8aaa, 0x90},
+	{0x8aab, 0x0e},
+	{0x8aac, 0x5f},
+	{0x8aad, 0x12},
+	{0x8aae, 0x0b},
+	{0x8aaf, 0x2b},
+	{0x8ab0, 0xe5},
+	{0x8ab1, 0x1e},
+	{0x8ab2, 0xf0},
+	{0x8ab3, 0xa2},
+	{0x8ab4, 0x31},
+	{0x8ab5, 0x92},
+	{0x8ab6, 0xaf},
+	{0x8ab7, 0x22},
+	{0x8ab8, 0xd2},
+	{0x8ab9, 0x01},
+	{0x8aba, 0xc2},
+	{0x8abb, 0x02},
+	{0x8abc, 0xe4},
+	{0x8abd, 0xf5},
+	{0x8abe, 0x40},
+	{0x8abf, 0xf5},
+	{0x8ac0, 0x3f},
+	{0x8ac1, 0xd2},
+	{0x8ac2, 0x34},
+	{0x8ac3, 0xd2},
+	{0x8ac4, 0x32},
+	{0x8ac5, 0xd2},
+	{0x8ac6, 0x35},
+	{0x8ac7, 0xd2},
+	{0x8ac8, 0x01},
+	{0x8ac9, 0xc2},
+	{0x8aca, 0x02},
+	{0x8acb, 0xf5},
+	{0x8acc, 0x40},
+	{0x8acd, 0xf5},
+	{0x8ace, 0x3f},
+	{0x8acf, 0xd2},
+	{0x8ad0, 0x34},
+	{0x8ad1, 0xd2},
+	{0x8ad2, 0x32},
+	{0x8ad3, 0x22},
+	{0x8ad4, 0x8f},
+	{0x8ad5, 0x30},
+	{0x8ad6, 0x8e},
+	{0x8ad7, 0x2f},
+	{0x8ad8, 0x8d},
+	{0x8ad9, 0x2e},
+	{0x8ada, 0x8c},
+	{0x8adb, 0x2d},
+	{0x8adc, 0x22},
+	{0x8add, 0x12},
+	{0x8ade, 0x04},
+	{0x8adf, 0x47},
+	{0x8ae0, 0x8f},
+	{0x8ae1, 0x1e},
+	{0x8ae2, 0x8e},
+	{0x8ae3, 0x1d},
+	{0x8ae4, 0x8d},
+	{0x8ae5, 0x1c},
+	{0x8ae6, 0x8c},
+	{0x8ae7, 0x1b},
+	{0x8ae8, 0x22},
+	{0x8ae9, 0x93},
+	{0x8aea, 0xf9},
+	{0x8aeb, 0xf8},
+	{0x8aec, 0x02},
+	{0x8aed, 0x04},
+	{0x8aee, 0x34},
+	{0x8aef, 0x90},
+	{0x8af0, 0x0e},
+	{0x8af1, 0x81},
+	{0x8af2, 0x12},
+	{0x8af3, 0x04},
+	{0x8af4, 0x47},
+	{0x8af5, 0x8f},
+	{0x8af6, 0x3b},
+	{0x8af7, 0x8e},
+	{0x8af8, 0x3a},
+	{0x8af9, 0x8d},
+	{0x8afa, 0x39},
+	{0x8afb, 0x8c},
+	{0x8afc, 0x38},
+	{0x8afd, 0xd2},
+	{0x8afe, 0x06},
+	{0x8aff, 0x30},
+	{0x8b00, 0x06},
+	{0x8b01, 0x03},
+	{0x8b02, 0xd3},
+	{0x8b03, 0x80},
+	{0x8b04, 0x01},
+	{0x8b05, 0xc3},
+	{0x8b06, 0x92},
+	{0x8b07, 0x0e},
+	{0x8b08, 0x22},
+	{0x8b09, 0xc0},
+	{0x8b0a, 0xe0},
+	{0x8b0b, 0xc0},
+	{0x8b0c, 0x83},
+	{0x8b0d, 0xc0},
+	{0x8b0e, 0x82},
+	{0x8b0f, 0x90},
+	{0x8b10, 0x3f},
+	{0x8b11, 0x0d},
+	{0x8b12, 0xe0},
+	{0x8b13, 0xf5},
+	{0x8b14, 0x28},
+	{0x8b15, 0xe5},
+	{0x8b16, 0x28},
+	{0x8b17, 0xf0},
+	{0x8b18, 0xd0},
+	{0x8b19, 0x82},
+	{0x8b1a, 0xd0},
+	{0x8b1b, 0x83},
+	{0x8b1c, 0xd0},
+	{0x8b1d, 0xe0},
+	{0x8b1e, 0x32},
+	{0x8b1f, 0x78},
+	{0x8b20, 0x7f},
+	{0x8b21, 0xe4},
+	{0x8b22, 0xf6},
+	{0x8b23, 0xd8},
+	{0x8b24, 0xfd},
+	{0x8b25, 0x75},
+	{0x8b26, 0x81},
+	{0x8b27, 0xc0},
+	{0x8b28, 0x02},
+	{0x8b29, 0x09},
+	{0x8b2a, 0xa5},
+	{0x8b2b, 0xe4},
+	{0x8b2c, 0x93},
+	{0x8b2d, 0xfe},
+	{0x8b2e, 0x74},
+	{0x8b2f, 0x01},
+	{0x8b30, 0x93},
+	{0x8b31, 0xf5},
+	{0x8b32, 0x82},
+	{0x8b33, 0x8e},
+	{0x8b34, 0x83},
+	{0x8b35, 0x22},
+	{0x8b36, 0x8f},
+	{0x8b37, 0x82},
+	{0x8b38, 0x8e},
+	{0x8b39, 0x83},
+	{0x8b3a, 0x75},
+	{0x8b3b, 0xf0},
+	{0x8b3c, 0x04},
+	{0x8b3d, 0xed},
+	{0x8b3e, 0x02},
+	{0x8b3f, 0x04},
+	{0x8b40, 0x89},
+	{0x8b41, 0x00},
+	{0x8b42, 0x00},
+	{0x8b43, 0x00},
+	{0x8b44, 0x00},
+	{0x8b45, 0x00},
+	{0x8b46, 0x00},
+	{0x8b47, 0x00},
+	{0x8b48, 0x00},
+	{0x8b49, 0x00},
+	{0x8b4a, 0x00},
+	{0x8b4b, 0x00},
+	{0x8b4c, 0x00},
+	{0x8b4d, 0x00},
+	{0x8b4e, 0x00},
+	{0x8b4f, 0x00},
+	{0x8b50, 0x00},
+	{0x8b51, 0x00},
+	{0x8b52, 0x00},
+	{0x8b53, 0x00},
+	{0x8b54, 0x00},
+	{0x8b55, 0x00},
+	{0x8b56, 0x00},
+	{0x8b57, 0x00},
+	{0x8b58, 0x00},
+	{0x8b59, 0x00},
+	{0x8b5a, 0x00},
+	{0x8b5b, 0x00},
+	{0x8b5c, 0x00},
+	{0x8b5d, 0x00},
+	{0x8b5e, 0x00},
+	{0x8b5f, 0x00},
+	{0x8b60, 0x00},
+	{0x8b61, 0x00},
+	{0x8b62, 0x00},
+	{0x8b63, 0x00},
+	{0x8b64, 0x00},
+	{0x8b65, 0x00},
+	{0x8b66, 0x00},
+	{0x8b67, 0x00},
+	{0x8b68, 0x00},
+	{0x8b69, 0x00},
+	{0x8b6a, 0x00},
+	{0x8b6b, 0x00},
+	{0x8b6c, 0x00},
+	{0x8b6d, 0x00},
+	{0x8b6e, 0x00},
+	{0x8b6f, 0x00},
+	{0x8b70, 0x00},
+	{0x8b71, 0x00},
+	{0x8b72, 0x00},
+	{0x8b73, 0x00},
+	{0x8b74, 0x00},
+	{0x8b75, 0x00},
+	{0x8b76, 0x00},
+	{0x8b77, 0x00},
+	{0x8b78, 0x00},
+	{0x8b79, 0x00},
+	{0x8b7a, 0x00},
+	{0x8b7b, 0x00},
+	{0x8b7c, 0x00},
+	{0x8b7d, 0x00},
+	{0x8b7e, 0x00},
+	{0x8b7f, 0x00},
+	{0x8b80, 0x00},
+	{0x8b81, 0x00},
+	{0x8b82, 0x00},
+	{0x8b83, 0x00},
+	{0x8b84, 0x00},
+	{0x8b85, 0x00},
+	{0x8b86, 0x00},
+	{0x8b87, 0x00},
+	{0x8b88, 0x00},
+	{0x8b89, 0x00},
+	{0x8b8a, 0x00},
+	{0x8b8b, 0x00},
+	{0x8b8c, 0x00},
+	{0x8b8d, 0x00},
+	{0x8b8e, 0x00},
+	{0x8b8f, 0x00},
+	{0x8b90, 0x00},
+	{0x8b91, 0x00},
+	{0x8b92, 0x00},
+	{0x8b93, 0x00},
+	{0x8b94, 0x00},
+	{0x8b95, 0x00},
+	{0x8b96, 0x00},
+	{0x8b97, 0x00},
+	{0x8b98, 0x00},
+	{0x8b99, 0x00},
+	{0x8b9a, 0x00},
+	{0x8b9b, 0x00},
+	{0x8b9c, 0x00},
+	{0x8b9d, 0x00},
+	{0x8b9e, 0x00},
+	{0x8b9f, 0x00},
+	{0x8ba0, 0x00},
+	{0x8ba1, 0x00},
+	{0x8ba2, 0x00},
+	{0x8ba3, 0x00},
+	{0x8ba4, 0x00},
+	{0x8ba5, 0x00},
+	{0x8ba6, 0x00},
+	{0x8ba7, 0x00},
+	{0x8ba8, 0x00},
+	{0x8ba9, 0x00},
+	{0x8baa, 0x00},
+	{0x8bab, 0x00},
+	{0x8bac, 0x00},
+	{0x8bad, 0x00},
+	{0x8bae, 0x00},
+	{0x8baf, 0x00},
+	{0x8bb0, 0x00},
+	{0x8bb1, 0x00},
+	{0x8bb2, 0x00},
+	{0x8bb3, 0x00},
+	{0x8bb4, 0x00},
+	{0x8bb5, 0x00},
+	{0x8bb6, 0x00},
+	{0x8bb7, 0x00},
+	{0x8bb8, 0x00},
+	{0x8bb9, 0x00},
+	{0x8bba, 0x00},
+	{0x8bbb, 0x00},
+	{0x8bbc, 0x00},
+	{0x8bbd, 0x00},
+	{0x8bbe, 0x00},
+	{0x8bbf, 0x00},
+	{0x8bc0, 0x00},
+	{0x8bc1, 0x00},
+	{0x8bc2, 0x00},
+	{0x8bc3, 0x00},
+	{0x8bc4, 0x00},
+	{0x8bc5, 0x00},
+	{0x8bc6, 0x00},
+	{0x8bc7, 0x00},
+	{0x8bc8, 0x00},
+	{0x8bc9, 0x00},
+	{0x8bca, 0x00},
+	{0x8bcb, 0x00},
+	{0x8bcc, 0x00},
+	{0x8bcd, 0x00},
+	{0x8bce, 0x00},
+	{0x8bcf, 0x00},
+	{0x8bd0, 0x00},
+	{0x8bd1, 0x00},
+	{0x8bd2, 0x00},
+	{0x8bd3, 0x00},
+	{0x8bd4, 0x00},
+	{0x8bd5, 0x00},
+	{0x8bd6, 0x00},
+	{0x8bd7, 0x00},
+	{0x8bd8, 0x00},
+	{0x8bd9, 0x00},
+	{0x8bda, 0x00},
+	{0x8bdb, 0x00},
+	{0x8bdc, 0x00},
+	{0x8bdd, 0x00},
+	{0x8bde, 0x00},
+	{0x8bdf, 0x00},
+	{0x8be0, 0x00},
+	{0x8be1, 0x00},
+	{0x8be2, 0x00},
+	{0x8be3, 0x00},
+	{0x8be4, 0x00},
+	{0x8be5, 0x00},
+	{0x8be6, 0x00},
+	{0x8be7, 0x00},
+	{0x8be8, 0x00},
+	{0x8be9, 0x00},
+	{0x8bea, 0x00},
+	{0x8beb, 0x00},
+	{0x8bec, 0x00},
+	{0x8bed, 0x00},
+	{0x8bee, 0x00},
+	{0x8bef, 0x00},
+	{0x8bf0, 0x00},
+	{0x8bf1, 0x00},
+	{0x8bf2, 0x00},
+	{0x8bf3, 0x00},
+	{0x8bf4, 0x00},
+	{0x8bf5, 0x00},
+	{0x8bf6, 0x00},
+	{0x8bf7, 0x00},
+	{0x8bf8, 0x00},
+	{0x8bf9, 0x00},
+	{0x8bfa, 0x00},
+	{0x8bfb, 0x00},
+	{0x8bfc, 0x00},
+	{0x8bfd, 0x00},
+	{0x8bfe, 0x00},
+	{0x8bff, 0x00},
+	{0x8c00, 0x00},
+	{0x8c01, 0x00},
+	{0x8c02, 0x00},
+	{0x8c03, 0x00},
+	{0x8c04, 0x00},
+	{0x8c05, 0x00},
+	{0x8c06, 0x00},
+	{0x8c07, 0x00},
+	{0x8c08, 0x00},
+	{0x8c09, 0x00},
+	{0x8c0a, 0x00},
+	{0x8c0b, 0x00},
+	{0x8c0c, 0x00},
+	{0x8c0d, 0x00},
+	{0x8c0e, 0x00},
+	{0x8c0f, 0x00},
+	{0x8c10, 0x00},
+	{0x8c11, 0x00},
+	{0x8c12, 0x00},
+	{0x8c13, 0x00},
+	{0x8c14, 0x00},
+	{0x8c15, 0x00},
+	{0x8c16, 0x00},
+	{0x8c17, 0x00},
+	{0x8c18, 0x00},
+	{0x8c19, 0x00},
+	{0x8c1a, 0x00},
+	{0x8c1b, 0x00},
+	{0x8c1c, 0x00},
+	{0x8c1d, 0x00},
+	{0x8c1e, 0x00},
+	{0x8c1f, 0x00},
+	{0x8c20, 0x00},
+	{0x8c21, 0x00},
+	{0x8c22, 0x00},
+	{0x8c23, 0x00},
+	{0x8c24, 0x00},
+	{0x8c25, 0x00},
+	{0x8c26, 0x00},
+	{0x8c27, 0x00},
+	{0x8c28, 0x00},
+	{0x8c29, 0x00},
+	{0x8c2a, 0x00},
+	{0x8c2b, 0x00},
+	{0x8c2c, 0x00},
+	{0x8c2d, 0x00},
+	{0x8c2e, 0x00},
+	{0x8c2f, 0x00},
+	{0x8c30, 0x00},
+	{0x8c31, 0x00},
+	{0x8c32, 0x00},
+	{0x8c33, 0x00},
+	{0x8c34, 0x00},
+	{0x8c35, 0x00},
+	{0x8c36, 0x00},
+	{0x8c37, 0x00},
+	{0x8c38, 0x00},
+	{0x8c39, 0x00},
+	{0x8c3a, 0x00},
+	{0x8c3b, 0x00},
+	{0x8c3c, 0x00},
+	{0x8c3d, 0x00},
+	{0x8c3e, 0x00},
+	{0x8c3f, 0x00},
+	{0x8c40, 0x00},
+	{0x8c41, 0x00},
+	{0x8c42, 0x00},
+	{0x8c43, 0x00},
+	{0x8c44, 0x00},
+	{0x8c45, 0x00},
+	{0x8c46, 0x00},
+	{0x8c47, 0x00},
+	{0x8c48, 0x00},
+	{0x8c49, 0x00},
+	{0x8c4a, 0x00},
+	{0x8c4b, 0x00},
+	{0x8c4c, 0x00},
+	{0x8c4d, 0x00},
+	{0x8c4e, 0x00},
+	{0x8c4f, 0x00},
+	{0x8c50, 0x00},
+	{0x8c51, 0x00},
+	{0x8c52, 0x00},
+	{0x8c53, 0x00},
+	{0x8c54, 0x00},
+	{0x8c55, 0x00},
+	{0x8c56, 0x00},
+	{0x8c57, 0x00},
+	{0x8c58, 0x00},
+	{0x8c59, 0x00},
+	{0x8c5a, 0x00},
+	{0x8c5b, 0x00},
+	{0x8c5c, 0x00},
+	{0x8c5d, 0x00},
+	{0x8c5e, 0x00},
+	{0x8c5f, 0x00},
+	{0x8c60, 0x00},
+	{0x8c61, 0x00},
+	{0x8c62, 0x00},
+	{0x8c63, 0x00},
+	{0x8c64, 0x00},
+	{0x8c65, 0x00},
+	{0x8c66, 0x00},
+	{0x8c67, 0x00},
+	{0x8c68, 0x00},
+	{0x8c69, 0x00},
+	{0x8c6a, 0x00},
+	{0x8c6b, 0x00},
+	{0x8c6c, 0x00},
+	{0x8c6d, 0x00},
+	{0x8c6e, 0x00},
+	{0x8c6f, 0x00},
+	{0x8c70, 0x00},
+	{0x8c71, 0x00},
+	{0x8c72, 0x00},
+	{0x8c73, 0x00},
+	{0x8c74, 0x00},
+	{0x8c75, 0x00},
+	{0x8c76, 0x00},
+	{0x8c77, 0x00},
+	{0x8c78, 0x00},
+	{0x8c79, 0x00},
+	{0x8c7a, 0x00},
+	{0x8c7b, 0x00},
+	{0x8c7c, 0x00},
+	{0x8c7d, 0x00},
+	{0x8c7e, 0x00},
+	{0x8c7f, 0x00},
+	{0x8c80, 0x00},
+	{0x8c81, 0x00},
+	{0x8c82, 0x00},
+	{0x8c83, 0x00},
+	{0x8c84, 0x00},
+	{0x8c85, 0x00},
+	{0x8c86, 0x00},
+	{0x8c87, 0x00},
+	{0x8c88, 0x00},
+	{0x8c89, 0x00},
+	{0x8c8a, 0x00},
+	{0x8c8b, 0x00},
+	{0x8c8c, 0x00},
+	{0x8c8d, 0x00},
+	{0x8c8e, 0x00},
+	{0x8c8f, 0x00},
+	{0x8c90, 0x00},
+	{0x8c91, 0x00},
+	{0x8c92, 0x00},
+	{0x8c93, 0x00},
+	{0x8c94, 0x00},
+	{0x8c95, 0x00},
+	{0x8c96, 0x00},
+	{0x8c97, 0x00},
+	{0x8c98, 0x00},
+	{0x8c99, 0x00},
+	{0x8c9a, 0x00},
+	{0x8c9b, 0x00},
+	{0x8c9c, 0x00},
+	{0x8c9d, 0x00},
+	{0x8c9e, 0x00},
+	{0x8c9f, 0x00},
+	{0x8ca0, 0x00},
+	{0x8ca1, 0x00},
+	{0x8ca2, 0x00},
+	{0x8ca3, 0x00},
+	{0x8ca4, 0x00},
+	{0x8ca5, 0x00},
+	{0x8ca6, 0x00},
+	{0x8ca7, 0x00},
+	{0x8ca8, 0x00},
+	{0x8ca9, 0x00},
+	{0x8caa, 0x00},
+	{0x8cab, 0x00},
+	{0x8cac, 0x00},
+	{0x8cad, 0x00},
+	{0x8cae, 0x00},
+	{0x8caf, 0x00},
+	{0x8cb0, 0x00},
+	{0x8cb1, 0x00},
+	{0x8cb2, 0x00},
+	{0x8cb3, 0x00},
+	{0x8cb4, 0x00},
+	{0x8cb5, 0x00},
+	{0x8cb6, 0x00},
+	{0x8cb7, 0x00},
+	{0x8cb8, 0x00},
+	{0x8cb9, 0x00},
+	{0x8cba, 0x00},
+	{0x8cbb, 0x00},
+	{0x8cbc, 0x00},
+	{0x8cbd, 0x00},
+	{0x8cbe, 0x00},
+	{0x8cbf, 0x00},
+	{0x8cc0, 0x00},
+	{0x8cc1, 0x00},
+	{0x8cc2, 0x00},
+	{0x8cc3, 0x00},
+	{0x8cc4, 0x00},
+	{0x8cc5, 0x00},
+	{0x8cc6, 0x00},
+	{0x8cc7, 0x00},
+	{0x8cc8, 0x00},
+	{0x8cc9, 0x00},
+	{0x8cca, 0x00},
+	{0x8ccb, 0x00},
+	{0x8ccc, 0x00},
+	{0x8ccd, 0x00},
+	{0x8cce, 0x00},
+	{0x8ccf, 0x00},
+	{0x8cd0, 0x00},
+	{0x8cd1, 0x00},
+	{0x8cd2, 0x00},
+	{0x8cd3, 0x00},
+	{0x8cd4, 0x00},
+	{0x8cd5, 0x00},
+	{0x8cd6, 0x00},
+	{0x8cd7, 0x00},
+	{0x8cd8, 0x00},
+	{0x8cd9, 0x00},
+	{0x8cda, 0x00},
+	{0x8cdb, 0x00},
+	{0x8cdc, 0x00},
+	{0x8cdd, 0x00},
+	{0x8cde, 0x00},
+	{0x8cdf, 0x00},
+	{0x8ce0, 0x00},
+	{0x8ce1, 0x00},
+	{0x8ce2, 0x00},
+	{0x8ce3, 0x00},
+	{0x8ce4, 0x00},
+	{0x8ce5, 0x00},
+	{0x8ce6, 0x00},
+	{0x8ce7, 0x00},
+	{0x8ce8, 0x00},
+	{0x8ce9, 0x00},
+	{0x8cea, 0x00},
+	{0x8ceb, 0x00},
+	{0x8cec, 0x00},
+	{0x8ced, 0x00},
+	{0x8cee, 0x00},
+	{0x8cef, 0x00},
+	{0x8cf0, 0x00},
+	{0x8cf1, 0x00},
+	{0x8cf2, 0x00},
+	{0x8cf3, 0x00},
+	{0x8cf4, 0x00},
+	{0x8cf5, 0x00},
+	{0x8cf6, 0x00},
+	{0x8cf7, 0x00},
+	{0x8cf8, 0x00},
+	{0x8cf9, 0x00},
+	{0x8cfa, 0x00},
+	{0x8cfb, 0x00},
+	{0x8cfc, 0x00},
+	{0x8cfd, 0x00},
+	{0x8cfe, 0x00},
+	{0x8cff, 0x00},
+	{0x8d00, 0x00},
+	{0x8d01, 0x00},
+	{0x8d02, 0x00},
+	{0x8d03, 0x00},
+	{0x8d04, 0x00},
+	{0x8d05, 0x00},
+	{0x8d06, 0x00},
+	{0x8d07, 0x00},
+	{0x8d08, 0x00},
+	{0x8d09, 0x00},
+	{0x8d0a, 0x00},
+	{0x8d0b, 0x00},
+	{0x8d0c, 0x00},
+	{0x8d0d, 0x00},
+	{0x8d0e, 0x00},
+	{0x8d0f, 0x00},
+	{0x8d10, 0x00},
+	{0x8d11, 0x00},
+	{0x8d12, 0x00},
+	{0x8d13, 0x00},
+	{0x8d14, 0x00},
+	{0x8d15, 0x00},
+	{0x8d16, 0x00},
+	{0x8d17, 0x00},
+	{0x8d18, 0x00},
+	{0x8d19, 0x00},
+	{0x8d1a, 0x00},
+	{0x8d1b, 0x00},
+	{0x8d1c, 0x00},
+	{0x8d1d, 0x00},
+	{0x8d1e, 0x00},
+	{0x8d1f, 0x00},
+	{0x8d20, 0x00},
+	{0x8d21, 0x00},
+	{0x8d22, 0x00},
+	{0x8d23, 0x00},
+	{0x8d24, 0x00},
+	{0x8d25, 0x00},
+	{0x8d26, 0x00},
+	{0x8d27, 0x00},
+	{0x8d28, 0x00},
+	{0x8d29, 0x00},
+	{0x8d2a, 0x00},
+	{0x8d2b, 0x00},
+	{0x8d2c, 0x00},
+	{0x8d2d, 0x00},
+	{0x8d2e, 0x00},
+	{0x8d2f, 0x00},
+	{0x8d30, 0x00},
+	{0x8d31, 0x00},
+	{0x8d32, 0x00},
+	{0x8d33, 0x00},
+	{0x8d34, 0x00},
+	{0x8d35, 0x00},
+	{0x8d36, 0x00},
+	{0x8d37, 0x00},
+	{0x8d38, 0x00},
+	{0x8d39, 0x00},
+	{0x8d3a, 0x00},
+	{0x8d3b, 0x00},
+	{0x8d3c, 0x00},
+	{0x8d3d, 0x00},
+	{0x8d3e, 0x00},
+	{0x8d3f, 0x00},
+	{0x8d40, 0x00},
+	{0x8d41, 0x00},
+	{0x8d42, 0x00},
+	{0x8d43, 0x00},
+	{0x8d44, 0x00},
+	{0x8d45, 0x00},
+	{0x8d46, 0x00},
+	{0x8d47, 0x00},
+	{0x8d48, 0x00},
+	{0x8d49, 0x00},
+	{0x8d4a, 0x00},
+	{0x8d4b, 0x00},
+	{0x8d4c, 0x00},
+	{0x8d4d, 0x00},
+	{0x8d4e, 0x00},
+	{0x8d4f, 0x00},
+	{0x8d50, 0x00},
+	{0x8d51, 0x00},
+	{0x8d52, 0x00},
+	{0x8d53, 0x00},
+	{0x8d54, 0x00},
+	{0x8d55, 0x00},
+	{0x8d56, 0x00},
+	{0x8d57, 0x00},
+	{0x8d58, 0x00},
+	{0x8d59, 0x00},
+	{0x8d5a, 0x00},
+	{0x8d5b, 0x00},
+	{0x8d5c, 0x00},
+	{0x8d5d, 0x00},
+	{0x8d5e, 0x00},
+	{0x8d5f, 0x00},
+	{0x8d60, 0x00},
+	{0x8d61, 0x00},
+	{0x8d62, 0x00},
+	{0x8d63, 0x00},
+	{0x8d64, 0x00},
+	{0x8d65, 0x00},
+	{0x8d66, 0x00},
+	{0x8d67, 0x00},
+	{0x8d68, 0x00},
+	{0x8d69, 0x00},
+	{0x8d6a, 0x00},
+	{0x8d6b, 0x00},
+	{0x8d6c, 0x00},
+	{0x8d6d, 0x00},
+	{0x8d6e, 0x00},
+	{0x8d6f, 0x00},
+	{0x8d70, 0x00},
+	{0x8d71, 0x00},
+	{0x8d72, 0x00},
+	{0x8d73, 0x00},
+	{0x8d74, 0x00},
+	{0x8d75, 0x00},
+	{0x8d76, 0x00},
+	{0x8d77, 0x00},
+	{0x8d78, 0x00},
+	{0x8d79, 0x00},
+	{0x8d7a, 0x00},
+	{0x8d7b, 0x00},
+	{0x8d7c, 0x00},
+	{0x8d7d, 0x00},
+	{0x8d7e, 0x00},
+	{0x8d7f, 0x00},
+	{0x8d80, 0x00},
+	{0x8d81, 0x00},
+	{0x8d82, 0x00},
+	{0x8d83, 0x00},
+	{0x8d84, 0x00},
+	{0x8d85, 0x00},
+	{0x8d86, 0x00},
+	{0x8d87, 0x00},
+	{0x8d88, 0x00},
+	{0x8d89, 0x00},
+	{0x8d8a, 0x00},
+	{0x8d8b, 0x00},
+	{0x8d8c, 0x00},
+	{0x8d8d, 0x00},
+	{0x8d8e, 0x00},
+	{0x8d8f, 0x00},
+	{0x8d90, 0x00},
+	{0x8d91, 0x00},
+	{0x8d92, 0x00},
+	{0x8d93, 0x00},
+	{0x8d94, 0x00},
+	{0x8d95, 0x00},
+	{0x8d96, 0x00},
+	{0x8d97, 0x00},
+	{0x8d98, 0x00},
+	{0x8d99, 0x00},
+	{0x8d9a, 0x00},
+	{0x8d9b, 0x00},
+	{0x8d9c, 0x00},
+	{0x8d9d, 0x00},
+	{0x8d9e, 0x00},
+	{0x8d9f, 0x00},
+	{0x8da0, 0x00},
+	{0x8da1, 0x00},
+	{0x8da2, 0x00},
+	{0x8da3, 0x00},
+	{0x8da4, 0x00},
+	{0x8da5, 0x00},
+	{0x8da6, 0x00},
+	{0x8da7, 0x00},
+	{0x8da8, 0x00},
+	{0x8da9, 0x00},
+	{0x8daa, 0x00},
+	{0x8dab, 0x00},
+	{0x8dac, 0x00},
+	{0x8dad, 0x00},
+	{0x8dae, 0x00},
+	{0x8daf, 0x00},
+	{0x8db0, 0x00},
+	{0x8db1, 0x00},
+	{0x8db2, 0x00},
+	{0x8db3, 0x00},
+	{0x8db4, 0x00},
+	{0x8db5, 0x00},
+	{0x8db6, 0x00},
+	{0x8db7, 0x00},
+	{0x8db8, 0x00},
+	{0x8db9, 0x00},
+	{0x8dba, 0x00},
+	{0x8dbb, 0x00},
+	{0x8dbc, 0x00},
+	{0x8dbd, 0x00},
+	{0x8dbe, 0x00},
+	{0x8dbf, 0x00},
+	{0x8dc0, 0x00},
+	{0x8dc1, 0x00},
+	{0x8dc2, 0x00},
+	{0x8dc3, 0x00},
+	{0x8dc4, 0x00},
+	{0x8dc5, 0x00},
+	{0x8dc6, 0x00},
+	{0x8dc7, 0x00},
+	{0x8dc8, 0x00},
+	{0x8dc9, 0x00},
+	{0x8dca, 0x00},
+	{0x8dcb, 0x00},
+	{0x8dcc, 0x00},
+	{0x8dcd, 0x00},
+	{0x8dce, 0x00},
+	{0x8dcf, 0x00},
+	{0x8dd0, 0x00},
+	{0x8dd1, 0x00},
+	{0x8dd2, 0x00},
+	{0x8dd3, 0x00},
+	{0x8dd4, 0x00},
+	{0x8dd5, 0x00},
+	{0x8dd6, 0x00},
+	{0x8dd7, 0x00},
+	{0x8dd8, 0x00},
+	{0x8dd9, 0x00},
+	{0x8dda, 0x00},
+	{0x8ddb, 0x00},
+	{0x8ddc, 0x00},
+	{0x8ddd, 0x00},
+	{0x8dde, 0x00},
+	{0x8ddf, 0x00},
+	{0x8de0, 0x00},
+	{0x8de1, 0x00},
+	{0x8de2, 0x00},
+	{0x8de3, 0x00},
+	{0x8de4, 0x00},
+	{0x8de5, 0x00},
+	{0x8de6, 0x00},
+	{0x8de7, 0x00},
+	{0x8de8, 0x00},
+	{0x8de9, 0x00},
+	{0x8dea, 0x00},
+	{0x8deb, 0x00},
+	{0x8dec, 0x00},
+	{0x8ded, 0x00},
+	{0x8dee, 0x00},
+	{0x8def, 0x00},
+	{0x8df0, 0x00},
+	{0x8df1, 0x00},
+	{0x8df2, 0x00},
+	{0x8df3, 0x00},
+	{0x8df4, 0x00},
+	{0x8df5, 0x00},
+	{0x8df6, 0x00},
+	{0x8df7, 0x00},
+	{0x8df8, 0x00},
+	{0x8df9, 0x00},
+	{0x8dfa, 0x00},
+	{0x8dfb, 0x00},
+	{0x8dfc, 0x00},
+	{0x8dfd, 0x00},
+	{0x8dfe, 0x00},
+	{0x8dff, 0x00},
+	{0x8e00, 0x11},
+	{0x8e01, 0x02},
+	{0x8e02, 0x28},
+	{0x8e03, 0x09},
+	{0x8e04, 0x00},
+	{0x8e05, 0x12},
+	{0x8e06, 0x4f},
+	{0x8e07, 0x56},
+	{0x8e08, 0x54},
+	{0x8e09, 0x20},
+	{0x8e0a, 0x20},
+	{0x8e0b, 0x20},
+	{0x8e0c, 0x20},
+	{0x8e0d, 0x20},
+	{0x8e0e, 0x20},
+	{0x8e0f, 0x01},
+	{0x8e10, 0x10},
+	{0x8e11, 0x00},
+	{0x8e12, 0x56},
+	{0x8e13, 0x40},
+	{0x8e14, 0x1a},
+	{0x8e15, 0x30},
+	{0x8e16, 0x29},
+	{0x8e17, 0x7e},
+	{0x8e18, 0x00},
+	{0x8e19, 0x30},
+	{0x8e1a, 0x04},
+	{0x8e1b, 0x20},
+	{0x8e1c, 0xdf},
+	{0x8e1d, 0x30},
+	{0x8e1e, 0x05},
+	{0x8e1f, 0x40},
+	{0x8e20, 0xbf},
+	{0x8e21, 0x50},
+	{0x8e22, 0x25},
+	{0x8e23, 0x04},
+	{0x8e24, 0xfb},
+	{0x8e25, 0x50},
+	{0x8e26, 0x03},
+	{0x8e27, 0x00},
+	{0x8e28, 0xfd},
+	{0x8e29, 0x50},
+	{0x8e2a, 0x27},
+	{0x8e2b, 0x01},
+	{0x8e2c, 0xfe},
+	{0x8e2d, 0x60},
+	{0x8e2e, 0x00},
+	{0x8e2f, 0x11},
+	{0x8e30, 0x00},
+	{0x8e31, 0x3f},
+	{0x8e32, 0x05},
+	{0x8e33, 0x30},
+	{0x8e34, 0x00},
+	{0x8e35, 0x3f},
+	{0x8e36, 0x06},
+	{0x8e37, 0x22},
+	{0x8e38, 0x00},
+	{0x8e39, 0x3f},
+	{0x8e3a, 0x01},
+	{0x8e3b, 0x29},
+	{0x8e3c, 0x00},
+	{0x8e3d, 0x3f},
+	{0x8e3e, 0x02},
+	{0x8e3f, 0x00},
+	{0x8e40, 0x00},
+	{0x8e41, 0x36},
+	{0x8e42, 0x06},
+	{0x8e43, 0x07},
+	{0x8e44, 0x00},
+	{0x8e45, 0x3f},
+	{0x8e46, 0x0b},
+	{0x8e47, 0x0f},
+	{0x8e48, 0xf0},
+	{0x8e49, 0x30},
+	{0x8e4a, 0x01},
+	{0x8e4b, 0x40},
+	{0x8e4c, 0xbf},
+	{0x8e4d, 0x30},
+	{0x8e4e, 0x01},
+	{0x8e4f, 0x00},
+	{0x8e50, 0xbf},
+	{0x8e51, 0x30},
+	{0x8e52, 0x29},
+	{0x8e53, 0x70},
+	{0x8e54, 0x00},
+	{0x8e55, 0x3a},
+	{0x8e56, 0x00},
+	{0x8e57, 0x01},
+	{0x8e58, 0xfe},
+	{0x8e59, 0x3a},
+	{0x8e5a, 0x00},
+	{0x8e5b, 0x00},
+	{0x8e5c, 0xfe},
+	{0x8e5d, 0x36},
+	{0x8e5e, 0x03},
+	{0x8e5f, 0x36},
+	{0x8e60, 0x02},
+	{0x8e61, 0x41},
+	{0x8e62, 0x44},
+	{0x8e63, 0x58},
+	{0x8e64, 0x20},
+	{0x8e65, 0x18},
+	{0x8e66, 0x10},
+	{0x8e67, 0x0a},
+	{0x8e68, 0x04},
+	{0x8e69, 0x04},
+	{0x8e6a, 0x00},
+	{0x8e6b, 0x03},
+	{0x8e6c, 0xff},
+	{0x8e6d, 0x64},
+	{0x8e6e, 0x00},
+	{0x8e6f, 0x00},
+	{0x8e70, 0x80},
+	{0x8e71, 0x00},
+	{0x8e72, 0x00},
+	{0x8e73, 0x00},
+	{0x8e74, 0x00},
+	{0x8e75, 0x00},
+	{0x8e76, 0x00},
+	{0x8e77, 0x02},
+	{0x8e78, 0x04},
+	{0x8e79, 0x06},
+	{0x8e7a, 0x00},
+	{0x8e7b, 0x03},
+	{0x8e7c, 0x98},
+	{0x8e7d, 0x00},
+	{0x8e7e, 0xcc},
+	{0x8e7f, 0x50},
+	{0x8e80, 0x3c},
+	{0x8e81, 0x28},
+	{0x8e82, 0x1e},
+	{0x8e83, 0x0c},
+	{0x8e84, 0x0c},
+	{0x8e85, 0x00},
+	{0x8e86, 0x00},
+	{0x8e87, 0x00},
+	{0x8e88, 0x6e},
+	{0x8e89, 0x05},
+	{0x8e8a, 0x05},
+	{0x8e8b, 0x00},
+	{0x8e8c, 0xa5},
+	{0x8e8d, 0x5a},
+	{0x3022, 0x00},
+	{0x3023, 0x00},
+	{0x3024, 0x00},
+	{0x3025, 0x00},
+	{0x3026, 0x00},
+	{0x3027, 0x00},
+	{0x3028, 0x00},
+	{0x3029, 0xFF},
+	{0x3000, 0x00},
+	{OV5640_TABLE_END, 0x0000}
+};
+
+static struct ov5640_reg tbl_single_focus[] = {
+	{0x3023, 0x01},
+	{0x3022, 0x03},
+	{OV5640_TABLE_END, 0x0000}
+};
+
+static struct ov5640_reg tbl_release_focus[] = {
+	{0x3023, 0x01},
+	{0x3022, 0x08},
+	{OV5640_TABLE_END, 0x0000}
+};
+
+#endif
diff --git a/drivers/media/video/tegra/ov5650.c b/drivers/media/video/tegra/ov5650.c
new file mode 100644
index 000000000000..7fae83c21fe2
--- /dev/null
+++ b/drivers/media/video/tegra/ov5650.c
@@ -0,0 +1,1586 @@
+/*
+ * ov5650.c - ov5650 sensor driver
+ *
+ * Copyright (C) 2011 Google Inc.
+ *
+ * Contributors:
+ *      Rebecca Schultz Zavin <rebecca@android.com>
+ *
+ * Leverage OV9640.c
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <media/ov5650.h>
+#include <media/tegra_camera.h>
+
+#define SIZEOF_I2C_TRANSBUF 32
+
+struct ov5650_reg {
+	u16 addr;
+	u16 val;
+};
+
+struct ov5650_sensor {
+	struct i2c_client *i2c_client;
+	struct ov5650_platform_data *pdata;
+};
+
+struct ov5650_info {
+	int mode;
+	enum StereoCameraMode camera_mode;
+	struct ov5650_sensor left;
+	struct ov5650_sensor right;
+	struct ov5650_sensordata sensor_data;
+	struct mutex mutex_le;
+	struct mutex mutex_ri;
+	int power_refcnt_le;
+	int power_refcnt_ri;
+	u8 i2c_trans_buf[SIZEOF_I2C_TRANSBUF];
+};
+
+static struct ov5650_info *stereo_ov5650_info;
+
+#define OV5650_TABLE_WAIT_MS 0
+#define OV5650_TABLE_END 1
+#define OV5650_MAX_RETRIES 3
+
+static struct ov5650_reg tp_none_seq[] = {
+	{0x5046, 0x00},
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg tp_cbars_seq[] = {
+	{0x503D, 0xC0},
+	{0x503E, 0x00},
+	{0x5046, 0x01},
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg tp_checker_seq[] = {
+	{0x503D, 0xC0},
+	{0x503E, 0x0A},
+	{0x5046, 0x01},
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg *test_pattern_modes[] = {
+	tp_none_seq,
+	tp_cbars_seq,
+	tp_checker_seq,
+};
+
+static struct ov5650_reg reset_seq[] = {
+	{0x3008, 0x82},
+	{OV5650_TABLE_WAIT_MS, 5},
+	{0x3008, 0x42},
+	{OV5650_TABLE_WAIT_MS, 5},
+	{OV5650_TABLE_END, 0x0000},
+};
+
+static struct ov5650_reg mode_start[] = {
+	{0x3103, 0x93},
+	{0x3017, 0xff},
+	{0x3018, 0xfc},
+
+	{0x3600, 0x50},
+	{0x3601, 0x0d},
+	{0x3604, 0x50},
+	{0x3605, 0x04},
+	{0x3606, 0x3f},
+	{0x3612, 0x1a},
+	{0x3630, 0x22},
+	{0x3631, 0x22},
+	{0x3702, 0x3a},
+	{0x3704, 0x18},
+	{0x3705, 0xda},
+	{0x3706, 0x41},
+	{0x370a, 0x80},
+	{0x370b, 0x40},
+	{0x370e, 0x00},
+	{0x3710, 0x28},
+	{0x3712, 0x13},
+	{0x3830, 0x50},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3a00, 0x38},
+
+
+	{0x3603, 0xa7},
+	{0x3615, 0x50},
+	{0x3620, 0x56},
+	{0x3810, 0x00},
+	{0x3836, 0x00},
+	{0x3a1a, 0x06},
+	{0x4000, 0x01},
+	{0x401c, 0x48},
+	{0x401d, 0x08},
+	{0x5000, 0x06},
+	{0x5001, 0x00},
+	{0x5002, 0x00},
+	{0x503d, 0x00},
+	{0x5046, 0x00},
+
+	{0x300f, 0x8f},
+
+	{0x3010, 0x10},
+	{0x3011, 0x14},
+	{0x3012, 0x02},
+	{0x3815, 0x82},
+	{0x3503, 0x33},
+	{0x3613, 0x44},
+	{OV5650_TABLE_END, 0x0},
+};
+
+static struct ov5650_reg mode_2592x1944[] = {
+	{0x3621, 0x2f},
+
+	{0x3632, 0x55},
+	{0x3703, 0xe6},
+	{0x370c, 0xa0},
+	{0x370d, 0x04},
+	{0x3713, 0x2f},
+	{0x3800, 0x02},
+	{0x3801, 0x58},
+	{0x3802, 0x00},
+	{0x3803, 0x0c},
+	{0x3804, 0x0a},
+	{0x3805, 0x20},
+	{0x3806, 0x07},
+	{0x3807, 0xa0},
+	{0x3808, 0x0a},
+
+	{0x3809, 0x20},
+
+	{0x380a, 0x07},
+
+	{0x380b, 0xa0},
+
+	{0x380c, 0x0c},
+
+	{0x380d, 0xb4},
+
+	{0x380e, 0x07},
+
+	{0x380f, 0xb0},
+
+	{0x3818, 0xc0},
+	{0x381a, 0x3c},
+	{0x3a0d, 0x06},
+	{0x3c01, 0x00},
+	{0x3007, 0x3f},
+	{0x5059, 0x80},
+	{0x3003, 0x03},
+	{0x3500, 0x00},
+	{0x3501, 0x7a},
+
+	{0x3502, 0xd0},
+
+	{0x350a, 0x00},
+	{0x350b, 0x00},
+	{0x401d, 0x08},
+	{0x4801, 0x0f},
+	{0x300e, 0x0c},
+	{0x4803, 0x50},
+	{0x4800, 0x34},
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_1296x972[] = {
+	{0x3621, 0xaf},
+
+	{0x3632, 0x5a},
+	{0x3703, 0xb0},
+	{0x370c, 0xc5},
+	{0x370d, 0x42},
+	{0x3713, 0x2f},
+	{0x3800, 0x03},
+	{0x3801, 0x3c},
+	{0x3802, 0x00},
+	{0x3803, 0x06},
+	{0x3804, 0x05},
+	{0x3805, 0x10},
+	{0x3806, 0x03},
+	{0x3807, 0xd0},
+	{0x3808, 0x05},
+
+	{0x3809, 0x10},
+
+	{0x380a, 0x03},
+
+	{0x380b, 0xd0},
+
+	{0x380c, 0x08},
+
+	{0x380d, 0xa8},
+
+	{0x380e, 0x05},
+
+	{0x380f, 0xa4},
+
+	{0x3818, 0xc1},
+	{0x381a, 0x00},
+	{0x3a0d, 0x08},
+	{0x3c01, 0x00},
+	{0x3007, 0x3b},
+	{0x5059, 0x80},
+	{0x3003, 0x03},
+	{0x3500, 0x00},
+
+	{0x3501, 0x5a},
+	{0x3502, 0x10},
+	{0x350a, 0x00},
+	{0x350b, 0x10},
+	{0x401d, 0x08},
+	{0x4801, 0x0f},
+	{0x300e, 0x0c},
+	{0x4803, 0x50},
+	{0x4800, 0x34},
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_2080x1164[] = {
+	{0x3103, 0x93},
+	{0x3007, 0x3b},
+	{0x3017, 0xff},
+	{0x3018, 0xfc},
+
+	{0x3600, 0x54},
+	{0x3601, 0x05},
+	{0x3603, 0xa7},
+	{0x3604, 0x40},
+	{0x3605, 0x04},
+	{0x3606, 0x3f},
+	{0x3612, 0x1a},
+	{0x3613, 0x44},
+	{0x3615, 0x52},
+	{0x3620, 0x56},
+	{0x3623, 0x01},
+	{0x3630, 0x22},
+	{0x3631, 0x36},
+	{0x3632, 0x5f},
+	{0x3633, 0x24},
+
+	{0x3702, 0x3a},
+	{0x3704, 0x18},
+	{0x3706, 0x41},
+	{0x370b, 0x40},
+	{0x370e, 0x00},
+	{0x3710, 0x28},
+	{0x3711, 0x24},
+	{0x3712, 0x13},
+
+	{0x3810, 0x00},
+	{0x3815, 0x82},
+	{0x3830, 0x50},
+	{0x3836, 0x00},
+
+	{0x3a1a, 0x06},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3a00, 0x38},
+
+	{0x3a0d, 0x06},
+	{0x3c01, 0x34},
+
+	{0x401f, 0x03},
+	{0x4000, 0x05},
+	{0x401d, 0x08},
+	{0x4001, 0x02},
+
+	{0x5001, 0x00},
+	{0x5002, 0x00},
+	{0x503d, 0x00},
+	{0x5046, 0x00},
+
+	{0x300f, 0x8f},
+
+	{0x3010, 0x10},
+	{0x3011, 0x14},
+	{0x3012, 0x02},
+	{0x3503, 0x33},
+
+
+	{0x3621, 0x2f},
+
+	{0x3703, 0xe6},
+	{0x370c, 0x00},
+	{0x370d, 0x04},
+	{0x3713, 0x22},
+	{0x3714, 0x27},
+	{0x3705, 0xda},
+	{0x370a, 0x80},
+
+	{0x3800, 0x02},
+	{0x3801, 0x12},
+	{0x3802, 0x00},
+	{0x3803, 0x0a},
+	{0x3804, 0x08},
+	{0x3805, 0x20},
+	{0x3806, 0x04},
+	{0x3807, 0x92},
+	{0x3808, 0x08},
+
+	{0x3809, 0x20},
+
+	{0x380a, 0x04},
+
+	{0x380b, 0x92},
+
+	{0x380c, 0x0a},
+
+	{0x380d, 0x96},
+
+	{0x380e, 0x04},
+
+	{0x380f, 0x9e},
+
+	{0x3818, 0xc0},
+	{0x381a, 0x3c},
+	{0x381c, 0x31},
+	{0x381d, 0x8e},
+	{0x381e, 0x04},
+	{0x381f, 0x92},
+	{0x3820, 0x04},
+	{0x3821, 0x19},
+	{0x3824, 0x01},
+	{0x3827, 0x0a},
+	{0x401c, 0x46},
+
+	{0x3003, 0x03},
+	{0x3500, 0x00},
+	{0x3501, 0x49},
+	{0x3502, 0xa0},
+	{0x350a, 0x00},
+	{0x350b, 0x00},
+	{0x4801, 0x0f},
+	{0x300e, 0x0c},
+	{0x4803, 0x50},
+	{0x4800, 0x34},
+
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_1920x1080[] = {
+	{0x3103, 0x93},
+	{0x3007, 0x3b},
+	{0x3017, 0xff},
+	{0x3018, 0xfc},
+
+	{0x3600, 0x54},
+	{0x3601, 0x05},
+	{0x3603, 0xa7},
+	{0x3604, 0x40},
+	{0x3605, 0x04},
+	{0x3606, 0x3f},
+	{0x3612, 0x1a},
+	{0x3613, 0x44},
+	{0x3615, 0x52},
+	{0x3620, 0x56},
+	{0x3623, 0x01},
+	{0x3630, 0x22},
+	{0x3631, 0x36},
+	{0x3632, 0x5f},
+	{0x3633, 0x24},
+
+	{0x3702, 0x3a},
+	{0x3704, 0x18},
+	{0x3706, 0x41},
+	{0x370b, 0x40},
+	{0x370e, 0x00},
+	{0x3710, 0x28},
+	{0x3711, 0x24},
+	{0x3712, 0x13},
+
+	{0x3810, 0x00},
+	{0x3815, 0x82},
+
+	{0x3830, 0x50},
+	{0x3836, 0x00},
+
+	{0x3a1a, 0x06},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3a00, 0x38},
+	{0x3a0d, 0x06},
+	{0x3c01, 0x34},
+
+	{0x401f, 0x03},
+	{0x4000, 0x05},
+	{0x401d, 0x08},
+	{0x4001, 0x02},
+
+	{0x5001, 0x00},
+	{0x5002, 0x00},
+	{0x503d, 0x00},
+	{0x5046, 0x00},
+
+	{0x300f, 0x8f},
+	{0x3010, 0x10},
+	{0x3011, 0x14},
+	{0x3012, 0x02},
+	{0x3503, 0x33},
+
+	{0x3621, 0x2f},
+	{0x3703, 0xe6},
+	{0x370c, 0x00},
+	{0x370d, 0x04},
+	{0x3713, 0x22},
+	{0x3714, 0x27},
+	{0x3705, 0xda},
+	{0x370a, 0x80},
+
+	{0x3800, 0x02},
+	{0x3801, 0x94},
+	{0x3802, 0x00},
+	{0x3803, 0x0c},
+	{0x3804, 0x07},
+	{0x3805, 0x80},
+	{0x3806, 0x04},
+	{0x3807, 0x40},
+	{0x3808, 0x07},
+	{0x3809, 0x80},
+	{0x380a, 0x04},
+	{0x380b, 0x40},
+	{0x380c, 0x0a},
+	{0x380d, 0x84},
+	{0x380e, 0x04},
+	{0x380f, 0xa4},
+	{0x3818, 0xc0},
+	{0x381a, 0x3c},
+	{0x381c, 0x31},
+	{0x381d, 0xa4},
+	{0x381e, 0x04},
+	{0x381f, 0x60},
+	{0x3820, 0x03},
+	{0x3821, 0x1a},
+	{0x3824, 0x01},
+	{0x3827, 0x0a},
+	{0x401c, 0x46},
+
+	{0x3003, 0x03},
+	{0x3500, 0x00},
+	{0x3501, 0x49},
+	{0x3502, 0xa0},
+	{0x350a, 0x00},
+	{0x350b, 0x00},
+	{0x4801, 0x0f},
+	{0x300e, 0x0c},
+	{0x4803, 0x50},
+	{0x4800, 0x34},
+
+	{OV5650_TABLE_END, 0x0000}
+};
+
+
+static struct ov5650_reg mode_1264x704[] = {
+	{0x3600, 0x54},
+	{0x3601, 0x05},
+	{0x3604, 0x40},
+	{0x3705, 0xdb},
+	{0x370a, 0x81},
+	{0x3615, 0x52},
+	{0x3810, 0x40},
+	{0x3836, 0x41},
+	{0x4000, 0x05},
+	{0x401c, 0x42},
+	{0x401d, 0x08},
+	{0x5046, 0x09},
+	{0x3010, 0x00},
+	{0x3503, 0x00},
+	{0x3613, 0xc4},
+
+	{0x3621, 0xaf},
+
+	{0x3632, 0x55},
+	{0x3703, 0x9a},
+	{0x370c, 0x00},
+	{0x370d, 0x42},
+	{0x3713, 0x22},
+	{0x3800, 0x02},
+	{0x3801, 0x54},
+	{0x3802, 0x00},
+	{0x3803, 0x0c},
+	{0x3804, 0x05},
+	{0x3805, 0x00},
+	{0x3806, 0x02},
+	{0x3807, 0xd0},
+	{0x3808, 0x05},
+
+	{0x3809, 0x00},
+
+	{0x380a, 0x02},
+
+	{0x380b, 0xd0},
+
+	{0x380c, 0x08},
+
+	{0x380d, 0x72},
+
+	{0x380e, 0x02},
+
+	{0x380f, 0xe4},
+
+	{0x3818, 0xc1},
+	{0x381a, 0x3c},
+	{0x3a0d, 0x06},
+	{0x3c01, 0x34},
+	{0x3007, 0x3b},
+	{0x5059, 0x80},
+	{0x3003, 0x03},
+	{0x3500, 0x04},
+	{0x3501, 0xa5},
+
+	{0x3502, 0x10},
+
+	{0x350a, 0x00},
+	{0x350b, 0x00},
+	{0x4801, 0x0f},
+	{0x300e, 0x0c},
+	{0x4803, 0x50},
+	{0x4800, 0x24},
+	{0x300f, 0x8b},
+
+	{0x3711, 0x24},
+	{0x3713, 0x92},
+	{0x3714, 0x17},
+	{0x381c, 0x10},
+	{0x381d, 0x82},
+	{0x381e, 0x05},
+	{0x381f, 0xc0},
+	{0x3821, 0x20},
+	{0x3824, 0x23},
+	{0x3825, 0x2c},
+	{0x3826, 0x00},
+	{0x3827, 0x0c},
+	{0x3623, 0x01},
+	{0x3633, 0x24},
+	{0x3632, 0x5f},
+	{0x401f, 0x03},
+
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_320x240[] = {
+	{0x3103, 0x93},
+	{0x3b07, 0x0c},
+	{0x3017, 0xff},
+	{0x3018, 0xfc},
+	{0x3706, 0x41},
+	{0x3613, 0xc4},
+	{0x370d, 0x42},
+	{0x3703, 0x9a},
+	{0x3630, 0x22},
+	{0x3605, 0x04},
+	{0x3606, 0x3f},
+	{0x3712, 0x13},
+	{0x370e, 0x00},
+	{0x370b, 0x40},
+	{0x3600, 0x54},
+	{0x3601, 0x05},
+	{0x3713, 0x22},
+	{0x3714, 0x27},
+	{0x3631, 0x22},
+	{0x3612, 0x1a},
+	{0x3604, 0x40},
+	{0x3705, 0xdc},
+	{0x370a, 0x83},
+	{0x370c, 0xc8},
+	{0x3710, 0x28},
+	{0x3702, 0x3a},
+	{0x3704, 0x18},
+	{0x3a18, 0x00},
+	{0x3a19, 0xf8},
+	{0x3a00, 0x38},
+	{0x3800, 0x02},
+	{0x3801, 0x54},
+	{0x3803, 0x0c},
+	{0x380c, 0x0c},
+	{0x380d, 0xb4},
+	{0x380e, 0x07},
+	{0x380f, 0xb0},
+	{0x3830, 0x50},
+	{0x3a08, 0x12},
+	{0x3a09, 0x70},
+	{0x3a0a, 0x0f},
+	{0x3a0b, 0x60},
+	{0x3a0d, 0x06},
+	{0x3a0e, 0x06},
+	{0x3a13, 0x54},
+	{0x3815, 0x82},
+	{0x5059, 0x80},
+	{0x3615, 0x52},
+	{0x505a, 0x0a},
+	{0x505b, 0x2e},
+	{0x3713, 0x92},
+	{0x3714, 0x17},
+	{0x3803, 0x0a},
+	{0x3804, 0x05},
+	{0x3805, 0x00},
+	{0x3806, 0x01},
+	{0x3807, 0x00},
+	{0x3808, 0x01},
+	{0x3809, 0x40},
+	{0x380a, 0x01},
+	{0x380b, 0x00},
+	{0x380c, 0x0a},
+
+	{0x380d, 0x04},
+
+	{0x380e, 0x01},
+
+	{0x380f, 0x38},
+
+	{0x3500, 0x00},
+	{0x3501, 0x13},
+	{0x3502, 0x80},
+	{0x350b, 0x7f},
+
+	{0x3815, 0x81},
+	{0x3824, 0x23},
+	{0x3825, 0x20},
+	{0x3826, 0x00},
+	{0x3827, 0x08},
+	{0x370d, 0xc2},
+	{0x3a08, 0x17},
+	{0x3a09, 0x64},
+	{0x3a0a, 0x13},
+	{0x3a0b, 0x80},
+	{0x3a00, 0x58},
+	{0x3a1a, 0x06},
+	{0x3503, 0x33},
+	{0x3623, 0x01},
+	{0x3633, 0x24},
+	{0x3c01, 0x34},
+	{0x3c04, 0x28},
+	{0x3c05, 0x98},
+	{0x3c07, 0x07},
+	{0x3c09, 0xc2},
+	{0x4000, 0x05},
+	{0x401d, 0x08},
+	{0x4001, 0x02},
+	{0x401c, 0x42},
+	{0x5046, 0x09},
+	{0x3810, 0x40},
+	{0x3836, 0x41},
+	{0x505f, 0x04},
+	{0x5001, 0x00},
+	{0x5002, 0x02},
+	{0x503d, 0x00},
+	{0x5901, 0x08},
+	{0x585a, 0x01},
+	{0x585b, 0x2c},
+	{0x585c, 0x01},
+	{0x585d, 0x93},
+	{0x585e, 0x01},
+	{0x585f, 0x90},
+	{0x5860, 0x01},
+	{0x5861, 0x0d},
+	{0x5180, 0xc0},
+	{0x5184, 0x00},
+	{0x470a, 0x00},
+	{0x470b, 0x00},
+	{0x470c, 0x00},
+	{0x300f, 0x8e},
+	{0x3603, 0xa7},
+	{0x3632, 0x55},
+	{0x3620, 0x56},
+	{0x3621, 0xaf},
+	{0x3818, 0xc3},
+	{0x3631, 0x36},
+	{0x3632, 0x5f},
+	{0x3711, 0x24},
+	{0x401f, 0x03},
+
+	{0x3011, 0x14},
+	{0x3007, 0x3B},
+	{0x300f, 0x8f},
+	{0x4801, 0x0f},
+	{0x3003, 0x03},
+	{0x300e, 0x0c},
+	{0x3010, 0x15},
+	{0x4803, 0x50},
+	{0x4800, 0x24},
+	{0x4837, 0x40},
+	{0x3815, 0x82},
+
+	{OV5650_TABLE_END, 0x0000}
+};
+
+static struct ov5650_reg mode_end[] = {
+	{0x3212, 0x00},
+	{0x3003, 0x01},
+	{0x3212, 0x10},
+	{0x3212, 0xa0},
+	{0x3008, 0x02},
+
+	{OV5650_TABLE_END, 0x0000}
+};
+
+enum {
+	OV5650_MODE_2592x1944,
+	OV5650_MODE_1296x972,
+	OV5650_MODE_2080x1164,
+	OV5650_MODE_1920x1080,
+	OV5650_MODE_1264x704,
+	OV5650_MODE_320x240,
+	OV5650_MODE_INVALID
+};
+
+static struct ov5650_reg *mode_table[] = {
+	[OV5650_MODE_2592x1944] = mode_2592x1944,
+	[OV5650_MODE_1296x972]  = mode_1296x972,
+	[OV5650_MODE_2080x1164] = mode_2080x1164,
+	[OV5650_MODE_1920x1080] = mode_1920x1080,
+	[OV5650_MODE_1264x704]  = mode_1264x704,
+	[OV5650_MODE_320x240]   = mode_320x240
+};
+
+static inline void ov5650_get_frame_length_regs(struct ov5650_reg *regs,
+						u32 frame_length)
+{
+	regs->addr = 0x380e;
+	regs->val = (frame_length >> 8) & 0xff;
+	(regs + 1)->addr = 0x380f;
+	(regs + 1)->val = (frame_length) & 0xff;
+}
+
+static inline void ov5650_get_coarse_time_regs(struct ov5650_reg *regs,
+						u32 coarse_time)
+{
+	regs->addr = 0x3500;
+	regs->val = (coarse_time >> 12) & 0xff;
+	(regs + 1)->addr = 0x3501;
+	(regs + 1)->val = (coarse_time >> 4) & 0xff;
+	(regs + 2)->addr = 0x3502;
+	(regs + 2)->val = (coarse_time & 0xf) << 4;
+}
+
+static inline void ov5650_get_gain_reg(struct ov5650_reg *regs, u16 gain)
+{
+	regs->addr = 0x350b;
+	regs->val = gain;
+}
+
+static int ov5650_read_reg(struct i2c_client *client, u16 addr, u8 *val)
+{
+	int err;
+	struct i2c_msg msg[2];
+	unsigned char data[3];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 2;
+	msg[0].buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8) (addr >> 8);
+	data[1] = (u8) (addr & 0xff);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = data + 2;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+
+	if (err != 2)
+		return -EINVAL;
+
+	*val = data[2];
+
+	return 0;
+}
+
+static int ov5650_read_reg_helper(struct ov5650_info *info,
+					u16 addr, u8 *val)
+{
+	int ret;
+	switch (info->camera_mode) {
+	case Main:
+	case StereoCameraMode_Left:
+		ret = ov5650_read_reg(info->left.i2c_client, addr, val);
+		break;
+	case StereoCameraMode_Stereo:
+		ret = ov5650_read_reg(info->left.i2c_client, addr, val);
+		if (ret)
+			break;
+		ret = ov5650_read_reg(info->right.i2c_client, addr, val);
+		break;
+	case StereoCameraMode_Right:
+		ret = ov5650_read_reg(info->right.i2c_client, addr, val);
+		break;
+	default:
+		return -1;
+	}
+	return ret;
+}
+
+static int ov5650_write_reg(struct i2c_client *client, u16 addr, u8 val)
+{
+	int err;
+	struct i2c_msg msg;
+	unsigned char data[3];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = (u8) (addr >> 8);
+	data[1] = (u8) (addr & 0xff);
+	data[2] = (u8) (val & 0xff);
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = data;
+
+	err = i2c_transfer(client->adapter, &msg, 1);
+	if (err == 1)
+		return 0;
+
+	pr_err("ov5650: i2c transfer failed, retrying %x %x\n",	addr, val);
+
+	return err;
+}
+
+static int ov5650_write_reg_helper(struct ov5650_info *info,
+					u16 addr, u8 val)
+{
+	int ret;
+	switch (info->camera_mode) {
+	case Main:
+	case StereoCameraMode_Left:
+		ret = ov5650_write_reg(info->left.i2c_client, addr, val);
+		break;
+	case StereoCameraMode_Stereo:
+		ret = ov5650_write_reg(info->left.i2c_client, addr, val);
+		if (ret)
+			break;
+		ret = ov5650_write_reg(info->right.i2c_client, addr, val);
+		break;
+	case StereoCameraMode_Right:
+		ret = ov5650_write_reg(info->right.i2c_client, addr, val);
+		break;
+	default:
+		return -1;
+	}
+	return ret;
+}
+
+static int ov5650_write_bulk_reg(struct i2c_client *client, u8 *data, int len)
+{
+	int err;
+	struct i2c_msg msg;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = len;
+	msg.buf = data;
+
+	err = i2c_transfer(client->adapter, &msg, 1);
+	if (err == 1)
+		return 0;
+
+	pr_err("ov5650: i2c bulk transfer failed at %x\n",
+		(int)data[0] << 8 | data[1]);
+
+	return err;
+}
+
+static int ov5650_write_bulk_reg_helper(struct ov5650_info *info, int len)
+{
+	int ret;
+	switch (info->camera_mode) {
+	case Main:
+	case StereoCameraMode_Left:
+		ret = ov5650_write_bulk_reg(info->left.i2c_client,
+					info->i2c_trans_buf, len);
+		break;
+	case StereoCameraMode_Stereo:
+		ret = ov5650_write_bulk_reg(info->left.i2c_client,
+					info->i2c_trans_buf, len);
+		if (ret)
+			break;
+		ret = ov5650_write_bulk_reg(info->right.i2c_client,
+					info->i2c_trans_buf, len);
+		break;
+	case StereoCameraMode_Right:
+		ret = ov5650_write_bulk_reg(info->right.i2c_client,
+					info->i2c_trans_buf, len);
+		break;
+	default:
+		return -1;
+	}
+	return ret;
+}
+
+static int ov5650_write_table(struct ov5650_info *info,
+				const struct ov5650_reg table[],
+				const struct ov5650_reg override_list[],
+				int num_override_regs)
+{
+	int err;
+	const struct ov5650_reg *next, *n_next;
+	u8 *b_ptr = info->i2c_trans_buf;
+	unsigned int buf_filled = 0;
+	unsigned int i;
+	u16 val;
+
+	for (next = table; next->addr != OV5650_TABLE_END; next++) {
+		if (next->addr == OV5650_TABLE_WAIT_MS) {
+			msleep(next->val);
+			continue;
+		}
+
+		val = next->val;
+		/* When an override list is passed in, replace the reg */
+		/* value to write if the reg is in the list            */
+		if (override_list) {
+			for (i = 0; i < num_override_regs; i++) {
+				if (next->addr == override_list[i].addr) {
+					val = override_list[i].val;
+					break;
+				}
+			}
+		}
+
+		if (!buf_filled) {
+			b_ptr = info->i2c_trans_buf;
+			*b_ptr++ = next->addr >> 8;
+			*b_ptr++ = next->addr & 0xff;
+			buf_filled = 2;
+		}
+		*b_ptr++ = val;
+		buf_filled++;
+
+		n_next = next + 1;
+		if (n_next->addr != OV5650_TABLE_END &&
+			n_next->addr != OV5650_TABLE_WAIT_MS &&
+			buf_filled < SIZEOF_I2C_TRANSBUF &&
+			n_next->addr == next->addr + 1) {
+			continue;
+		}
+
+		err = ov5650_write_bulk_reg_helper(info, buf_filled);
+		if (err)
+			return err;
+
+		buf_filled = 0;
+	}
+	return 0;
+}
+
+static int ov5650_set_mode(struct ov5650_info *info, struct ov5650_mode *mode)
+{
+	int sensor_mode;
+	int err;
+	struct ov5650_reg reg_list[6];
+
+	pr_info("%s: xres %u yres %u framelength %u coarsetime %u gain %u\n",
+		__func__, mode->xres, mode->yres, mode->frame_length,
+		mode->coarse_time, mode->gain);
+	if (mode->xres == 2592 && mode->yres == 1944)
+		sensor_mode = OV5650_MODE_2592x1944;
+	else if (mode->xres == 1296 && mode->yres == 972)
+		sensor_mode = OV5650_MODE_1296x972;
+	else if (mode->xres == 2080 && mode->yres == 1164)
+		sensor_mode = OV5650_MODE_2080x1164;
+	else if (mode->xres == 1920 && mode->yres == 1080)
+		sensor_mode = OV5650_MODE_1920x1080;
+	else if (mode->xres == 1264 && mode->yres == 704)
+		sensor_mode = OV5650_MODE_1264x704;
+	else if (mode->xres == 320 && mode->yres == 240)
+		sensor_mode = OV5650_MODE_320x240;
+	else {
+		pr_err("%s: invalid resolution supplied to set mode %d %d\n",
+		       __func__, mode->xres, mode->yres);
+		return -EINVAL;
+	}
+
+	/* get a list of override regs for the asking frame length, */
+	/* coarse integration time, and gain.                       */
+	ov5650_get_frame_length_regs(reg_list, mode->frame_length);
+	ov5650_get_coarse_time_regs(reg_list + 2, mode->coarse_time);
+	ov5650_get_gain_reg(reg_list + 5, mode->gain);
+
+	err = ov5650_write_table(info, reset_seq, NULL, 0);
+	if (err)
+		return err;
+
+	err = ov5650_write_table(info, mode_start, NULL, 0);
+	if (err)
+		return err;
+
+	err = ov5650_write_table(info, mode_table[sensor_mode],
+		reg_list, 6);
+	if (err)
+		return err;
+
+	err = ov5650_write_table(info, mode_end, NULL, 0);
+	if (err)
+		return err;
+
+	info->mode = sensor_mode;
+	return 0;
+}
+
+static int ov5650_set_frame_length(struct ov5650_info *info, u32 frame_length)
+{
+	int ret;
+	struct ov5650_reg reg_list[2];
+	u8 *b_ptr = info->i2c_trans_buf;
+
+	ov5650_get_frame_length_regs(reg_list, frame_length);
+
+	*b_ptr++ = reg_list[0].addr >> 8;
+	*b_ptr++ = reg_list[0].addr & 0xff;
+	*b_ptr++ = reg_list[0].val & 0xff;
+	*b_ptr++ = reg_list[1].val & 0xff;
+	ret = ov5650_write_bulk_reg_helper(info, 4);
+
+	return ret;
+}
+
+static int ov5650_set_coarse_time(struct ov5650_info *info, u32 coarse_time)
+{
+	int ret;
+	struct ov5650_reg reg_list[3];
+	u8 *b_ptr = info->i2c_trans_buf;
+
+	ov5650_get_coarse_time_regs(reg_list, coarse_time);
+
+	*b_ptr++ = reg_list[0].addr >> 8;
+	*b_ptr++ = reg_list[0].addr & 0xff;
+	*b_ptr++ = reg_list[0].val & 0xff;
+	*b_ptr++ = reg_list[1].val & 0xff;
+	*b_ptr++ = reg_list[2].val & 0xff;
+	ret = ov5650_write_bulk_reg_helper(info, 5);
+
+	return ret;
+}
+
+static int ov5650_set_gain(struct ov5650_info *info, u16 gain)
+{
+	int ret;
+	struct ov5650_reg reg_list;
+
+	ov5650_get_gain_reg(&reg_list, gain);
+	ret = ov5650_write_reg_helper(info, reg_list.addr, reg_list.val);
+
+	return ret;
+}
+
+static int ov5650_set_group_hold(struct ov5650_info *info, struct ov5650_ae *ae)
+{
+	int ret;
+	int count = 0;
+	bool groupHoldEnabled = false;
+
+	if (ae->gain_enable)
+		count++;
+	if (ae->coarse_time_enable)
+		count++;
+	if (ae->frame_length_enable)
+		count++;
+	if (count >= 2)
+		groupHoldEnabled = true;
+
+	if (groupHoldEnabled) {
+		ret = ov5650_write_reg_helper(info, 0x3212, 0x01);
+		if (ret)
+			return ret;
+	}
+
+	if (ae->gain_enable)
+		ov5650_set_gain(info, ae->gain);
+	if (ae->coarse_time_enable)
+		ov5650_set_coarse_time(info, ae->coarse_time);
+	if (ae->frame_length_enable)
+		ov5650_set_frame_length(info, ae->frame_length);
+
+	if (groupHoldEnabled) {
+		ret = ov5650_write_reg_helper(info, 0x3212, 0x11);
+		if (ret)
+			return ret;
+
+		ret = ov5650_write_reg_helper(info, 0x3212, 0xa1);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+
+static int ov5650_set_binning(struct ov5650_info *info, u8 enable)
+{
+	s32 ret;
+	u8  array_ctrl_reg, analog_ctrl_reg, timing_reg;
+	u32 val;
+
+	if (info->mode == OV5650_MODE_2592x1944
+	 || info->mode == OV5650_MODE_2080x1164
+	 || info->mode >= OV5650_MODE_INVALID) {
+		return -EINVAL;
+	}
+
+	ov5650_read_reg_helper(info, OV5650_ARRAY_CONTROL_01, &array_ctrl_reg);
+	ov5650_read_reg_helper(info, OV5650_ANALOG_CONTROL_D, &analog_ctrl_reg);
+	ov5650_read_reg_helper(info, OV5650_TIMING_TC_REG_18, &timing_reg);
+
+	ret = ov5650_write_reg_helper(info,
+			OV5650_SRM_GRUP_ACCESS,
+			OV5650_GROUP_ID(3));
+	if (ret < 0)
+		return -EIO;
+
+	if (!enable) {
+		ret = ov5650_write_reg_helper(info,
+			OV5650_ARRAY_CONTROL_01,
+			array_ctrl_reg |
+			(OV5650_H_BINNING_BIT | OV5650_H_SUBSAMPLING_BIT));
+
+		if (ret < 0)
+			goto exit;
+
+		ret = ov5650_write_reg_helper(info,
+			OV5650_ANALOG_CONTROL_D,
+			analog_ctrl_reg & ~OV5650_V_BINNING_BIT);
+
+		if (ret < 0)
+			goto exit;
+
+		ret = ov5650_write_reg_helper(info,
+			OV5650_TIMING_TC_REG_18,
+			timing_reg | OV5650_V_SUBSAMPLING_BIT);
+
+		if (ret < 0)
+			goto exit;
+
+		if (info->mode == OV5650_MODE_1296x972)
+			val = 0x1A2;
+		else
+			/* FIXME: this value is not verified yet. */
+			val = 0x1A8;
+
+		ret = ov5650_write_reg_helper(info,
+			OV5650_TIMING_CONTROL_HS_HIGH,
+			(val >> 8));
+
+		if (ret < 0)
+			goto exit;
+
+		ret = ov5650_write_reg_helper(info,
+			OV5650_TIMING_CONTROL_HS_LOW,
+			(val & 0xFF));
+	} else {
+		ret = ov5650_write_reg_helper(info,
+			OV5650_ARRAY_CONTROL_01,
+			(array_ctrl_reg | OV5650_H_BINNING_BIT)
+			& ~OV5650_H_SUBSAMPLING_BIT);
+
+		if (ret < 0)
+			goto exit;
+
+		ret = ov5650_write_reg_helper(info,
+			OV5650_ANALOG_CONTROL_D,
+			analog_ctrl_reg | OV5650_V_BINNING_BIT);
+
+		if (ret < 0)
+			goto exit;
+
+		ret = ov5650_write_reg_helper(info,
+			OV5650_TIMING_TC_REG_18,
+			timing_reg | OV5650_V_SUBSAMPLING_BIT);
+
+		if (ret < 0)
+			goto exit;
+
+		if (info->mode == OV5650_MODE_1296x972)
+			val = 0x33C;
+		else
+			val = 0x254;
+
+		ret = ov5650_write_reg_helper(info,
+			OV5650_TIMING_CONTROL_HS_HIGH,
+			(val >> 8));
+
+		if (ret < 0)
+			goto exit;
+
+		ret = ov5650_write_reg_helper(info,
+			OV5650_TIMING_CONTROL_HS_LOW,
+			(val & 0xFF));
+	}
+
+exit:
+	ret = ov5650_write_reg_helper(info,
+		OV5650_SRM_GRUP_ACCESS,
+		(OV5650_GROUP_HOLD_END_BIT | OV5650_GROUP_ID(3)));
+
+	ret |= ov5650_write_reg_helper(info,
+		OV5650_SRM_GRUP_ACCESS,
+		(OV5650_GROUP_HOLD_BIT | OV5650_GROUP_LAUNCH_BIT |
+		OV5650_GROUP_ID(3)));
+
+	return ret;
+}
+
+static int ov5650_test_pattern(struct ov5650_info *info,
+			       enum ov5650_test_pattern pattern)
+{
+	if (pattern >= ARRAY_SIZE(test_pattern_modes))
+		return -EINVAL;
+
+	return ov5650_write_table(info,
+				  test_pattern_modes[pattern],
+				  NULL, 0);
+}
+
+static int set_power_helper(struct ov5650_platform_data *pdata,
+				int powerLevel, int *ref_cnt)
+{
+	if (pdata) {
+		if (powerLevel && pdata->power_on) {
+			if (*ref_cnt == 0)
+				pdata->power_on();
+			*ref_cnt = *ref_cnt + 1;
+		}
+		else if (pdata->power_off) {
+			*ref_cnt = *ref_cnt - 1;
+			if (*ref_cnt <= 0)
+				pdata->power_off();
+		}
+	}
+	return 0;
+}
+
+static int ov5650_set_power(struct ov5650_info *info, int powerLevel)
+{
+	pr_info("%s: powerLevel=%d camera mode=%d\n", __func__, powerLevel,
+			info->camera_mode);
+
+	if (StereoCameraMode_Left & info->camera_mode) {
+		mutex_lock(&info->mutex_le);
+		set_power_helper(info->left.pdata, powerLevel,
+			&info->power_refcnt_le);
+		mutex_unlock(&info->mutex_le);
+	}
+
+	if (StereoCameraMode_Right & info->camera_mode) {
+		mutex_lock(&info->mutex_ri);
+		set_power_helper(info->right.pdata, powerLevel,
+			&info->power_refcnt_ri);
+		mutex_unlock(&info->mutex_ri);
+	}
+
+	return 0;
+}
+
+static int ov5650_get_sensor_id(struct ov5650_info *info)
+{
+	int ret = 0;
+	int i;
+	u8  bak;
+
+	pr_info("%s\n", __func__);
+	if (info->sensor_data.fuse_id_size)
+		return 0;
+
+	ov5650_set_power(info, 1);
+
+	for (i = 0; i < 5; i++) {
+		ret |= ov5650_write_reg_helper(info, 0x3d00, i);
+		ret |= ov5650_read_reg_helper(info, 0x3d04,
+				&bak);
+		info->sensor_data.fuse_id[i] = bak;
+	}
+
+	if (!ret)
+		info->sensor_data.fuse_id_size = i;
+
+	ov5650_set_power(info, 0);
+	return ret;
+}
+
+static long ov5650_ioctl(struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	int err;
+	struct ov5650_info *info = file->private_data;
+
+	switch (cmd) {
+	case OV5650_IOCTL_SET_CAMERA_MODE:
+	{
+		if (info->camera_mode != arg) {
+			err = ov5650_set_power(info, 0);
+			if (err) {
+				pr_info("%s %d\n", __func__, __LINE__);
+				return err;
+			}
+			info->camera_mode = arg;
+			err = ov5650_set_power(info, 1);
+			if (err)
+				return err;
+		}
+		return 0;
+	}
+	case OV5650_IOCTL_SYNC_SENSORS:
+		if (info->right.pdata->synchronize_sensors)
+			info->right.pdata->synchronize_sensors();
+		return 0;
+	case OV5650_IOCTL_SET_MODE:
+	{
+		struct ov5650_mode mode;
+		if (copy_from_user(&mode,
+				   (const void __user *)arg,
+				   sizeof(struct ov5650_mode))) {
+			pr_info("%s %d\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+
+		return ov5650_set_mode(info, &mode);
+	}
+	case OV5650_IOCTL_SET_FRAME_LENGTH:
+		return ov5650_set_frame_length(info, (u32)arg);
+	case OV5650_IOCTL_SET_COARSE_TIME:
+		return ov5650_set_coarse_time(info, (u32)arg);
+	case OV5650_IOCTL_SET_GAIN:
+		return ov5650_set_gain(info, (u16)arg);
+	case OV5650_IOCTL_SET_BINNING:
+		return ov5650_set_binning(info, (u8)arg);
+	case OV5650_IOCTL_GET_STATUS:
+	{
+		u16 status = 0;
+		if (copy_to_user((void __user *)arg, &status,
+				 2)) {
+			pr_info("%s %d\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+		return 0;
+	}
+	case OV5650_IOCTL_TEST_PATTERN:
+	{
+		err = ov5650_test_pattern(info, (enum ov5650_test_pattern) arg);
+		if (err)
+			pr_err("%s %d %d\n", __func__, __LINE__, err);
+		return err;
+	}
+	case OV5650_IOCTL_SET_GROUP_HOLD:
+	{
+		struct ov5650_ae ae;
+		if (copy_from_user(&ae,
+				(const void __user *)arg,
+				sizeof(struct ov5650_ae))) {
+			pr_info("%s %d\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+		return ov5650_set_group_hold(info, &ae);
+	}
+	case OV5650_IOCTL_GET_SENSORDATA:
+	{
+		err = ov5650_get_sensor_id(info);
+		if (err) {
+			pr_err("%s %d %d\n", __func__, __LINE__, err);
+			return err;
+		}
+		if (copy_to_user((void __user *)arg,
+				&info->sensor_data,
+				sizeof(struct ov5650_sensordata))) {
+			pr_info("%s %d\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+		return 0;
+	}
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int ov5650_open(struct inode *inode, struct file *file)
+{
+	pr_info("%s\n", __func__);
+	file->private_data = stereo_ov5650_info;
+	ov5650_set_power(stereo_ov5650_info, 1);
+	return 0;
+}
+
+int ov5650_release(struct inode *inode, struct file *file)
+{
+	struct ov5650_info *info = file->private_data;
+
+	ov5650_set_power(info, 0);
+	file->private_data = NULL;
+	return 0;
+}
+
+
+static const struct file_operations ov5650_fileops = {
+	.owner = THIS_MODULE,
+	.open = ov5650_open,
+	.unlocked_ioctl = ov5650_ioctl,
+	.release = ov5650_release,
+};
+
+static struct miscdevice ov5650_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "ov5650",
+	.fops = &ov5650_fileops,
+};
+
+static int ov5650_probe_common(void)
+{
+	int err;
+
+	if (!stereo_ov5650_info) {
+		stereo_ov5650_info = kzalloc(sizeof(struct ov5650_info),
+					GFP_KERNEL);
+		if (!stereo_ov5650_info) {
+			pr_err("ov5650: Unable to allocate memory!\n");
+			return -ENOMEM;
+		}
+
+		err = misc_register(&ov5650_device);
+		if (err) {
+			pr_err("ov5650: Unable to register misc device!\n");
+			kfree(stereo_ov5650_info);
+			return err;
+		}
+	}
+	return 0;
+}
+
+static int ov5650_remove_common(struct i2c_client *client)
+{
+	if (stereo_ov5650_info->left.i2c_client ||
+		stereo_ov5650_info->right.i2c_client)
+		return 0;
+
+	misc_deregister(&ov5650_device);
+	kfree(stereo_ov5650_info);
+	stereo_ov5650_info = NULL;
+
+	return 0;
+}
+
+static int left_ov5650_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int err;
+	pr_info("%s: probing sensor.\n", __func__);
+
+	err = ov5650_probe_common();
+	if (err)
+		return err;
+
+	stereo_ov5650_info->left.pdata = client->dev.platform_data;
+	stereo_ov5650_info->left.i2c_client = client;
+	mutex_init(&stereo_ov5650_info->mutex_le);
+	mutex_init(&stereo_ov5650_info->mutex_ri);
+
+	return 0;
+}
+
+static int left_ov5650_remove(struct i2c_client *client)
+{
+	if (stereo_ov5650_info) {
+		stereo_ov5650_info->left.i2c_client = NULL;
+		ov5650_remove_common(client);
+	}
+	return 0;
+}
+
+static const struct i2c_device_id left_ov5650_id[] = {
+	{ "ov5650", 0 },
+	{ "ov5650L", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, left_ov5650_id);
+
+static struct i2c_driver left_ov5650_i2c_driver = {
+	.driver = {
+		.name = "ov5650",
+		.owner = THIS_MODULE,
+	},
+	.probe = left_ov5650_probe,
+	.remove = left_ov5650_remove,
+	.id_table = left_ov5650_id,
+};
+
+static int right_ov5650_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int err;
+	pr_info("%s: probing sensor.\n", __func__);
+
+	err = ov5650_probe_common();
+	if (err)
+		return err;
+
+	stereo_ov5650_info->right.pdata = client->dev.platform_data;
+	stereo_ov5650_info->right.i2c_client = client;
+
+	return 0;
+}
+
+static int right_ov5650_remove(struct i2c_client *client)
+{
+	if (stereo_ov5650_info) {
+		stereo_ov5650_info->right.i2c_client = NULL;
+		ov5650_remove_common(client);
+	}
+	return 0;
+}
+
+static const struct i2c_device_id right_ov5650_id[] = {
+	{ "ov5650R", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, right_ov5650_id);
+
+static struct i2c_driver right_ov5650_i2c_driver = {
+	.driver = {
+		.name = "ov5650R",
+		.owner = THIS_MODULE,
+	},
+	.probe = right_ov5650_probe,
+	.remove = right_ov5650_remove,
+	.id_table = right_ov5650_id,
+};
+
+static int __init ov5650_init(void)
+{
+	int ret;
+	pr_info("ov5650 sensor driver loading\n");
+	ret = i2c_add_driver(&left_ov5650_i2c_driver);
+	if (ret)
+		return ret;
+	return i2c_add_driver(&right_ov5650_i2c_driver);
+}
+
+static void __exit ov5650_exit(void)
+{
+	i2c_del_driver(&right_ov5650_i2c_driver);
+	i2c_del_driver(&left_ov5650_i2c_driver);
+}
+
+module_init(ov5650_init);
+module_exit(ov5650_exit);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/video/tegra/ov9726.c b/drivers/media/video/tegra/ov9726.c
new file mode 100644
index 000000000000..45d6c85f0925
--- /dev/null
+++ b/drivers/media/video/tegra/ov9726.c
@@ -0,0 +1,893 @@
+/*
+ * ov9726.c - ov9726 sensor driver
+ *
+ * Copyright (c) 2011, NVIDIA, All Rights Reserved.
+ *
+ * Contributors:
+ *	  Charlie Huang <chahuang@nvidia.com>
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <mach/iomap.h>
+#include <linux/atomic.h>
+#include <mach/gpio.h>
+#include <linux/regulator/consumer.h>
+
+#include <media/ov9726.h>
+
+struct ov9726_power_rail {
+	struct regulator *sen_1v8_reg;
+	struct regulator *sen_2v8_reg;
+};
+
+struct ov9726_devinfo {
+	struct miscdevice		miscdev_info;
+	struct i2c_client		*i2c_client;
+	struct ov9726_platform_data	*pdata;
+	struct ov9726_power_rail	power_rail;
+	atomic_t			in_use;
+	__u32				mode;
+	struct ov9726_reg		grphold_temp[10];
+};
+
+static struct ov9726_reg mode_1280x720[] = {
+	/*
+	(1) clock setting
+	clock formula:	(Ref_clk / pre_pll_clk_div) * pll_multiplier /
+			vt_sys_clk_div / vt_pix_clk_div / divmip
+	input clk at        24Mhz
+	pre_pll_clk_div     0305[3:0] => 4
+	pll_multiplier      0307[6:0] => 100
+	vt_sys_clk_div      0303[3:0] => 1
+	vt_pix_clk_div      0301[3:0] => 10
+	divmip              3010[2:0] => 1
+
+	Overall timing:
+	line length:  1664 (reg 0x342/0x343)
+	frame length: 840  (reg 0x340/0x341)
+	coarse integration time: 835 lines (reg 0x202/0x203) => change to 836
+
+	visible pixels: (0,40) - (1280, 720+40) with size 1280x720
+	Output pixels (1280x720)
+
+	Frame rate if MCLK=24MHz:
+	24Mhz/4 *100/1/10/1 = 60 Mhz
+	60Mhz/1664/840 = 42.9 fps
+	*/
+
+	{0x0103, 0x01},
+
+	{OV9726_TABLE_WAIT_MS, 10},
+
+	{0x3026, 0x00},
+	{0x3027, 0x00},
+	{0x3705, 0x45},
+	{0x3603, 0xaa},
+	{0x3632, 0x2f},
+	{0x3620, 0x66},
+	{0x3621, 0xc0},
+	{0x0202, 0x03},
+	{0x0203, 0x13},
+	{0x3833, 0x04},
+	{0x3835, 0x02},
+	{0x4702, 0x04},
+	{0x4704, 0x00},
+	{0x4706, 0x08},
+	{0x5052, 0x01},
+	{0x3819, 0x6c},
+	{0x3817, 0x94},
+	{0x404e, 0x7e},
+	{0x3601, 0x40},
+	{0x3610, 0xa0},
+	{0x0344, 0x00},
+	{0x0345, 0x00},
+	{0x0346, 0x00},
+	{0x0347, 0x28},
+
+	{0x034c, 0x05},
+	{0x034d, 0x00},
+	{0x034e, 0x02},
+	{0x034f, 0xd8},
+	{0x3002, 0x00},
+	{0x3004, 0x00},
+	{0x3005, 0x00},
+	{0x4800, 0x44},
+	{0x4801, 0x0f},
+	{0x4803, 0x05},
+	{0x4601, 0x16},
+	{0x3014, 0x05},
+	{0x0101, 0x01},
+	{0x3707, 0x14},
+	{0x3622, 0x9f},
+	{0x4002, 0x45},
+	{0x5001, 0x00},
+	{0x3406, 0x01},
+	{0x3503, 0x17},
+	{0x0205, 0x3f},
+	{0x0100, 0x01},
+	{0x0112, 0x0a},
+	{0x0113, 0x0a},
+	{0x3013, 0x20},
+	{0x4837, 0x2f},
+	{0x3615, 0xf0},
+	{0x0340, 0x03},
+	{0x0341, 0x48},
+	{0x0342, 0x06},
+	{0x0343, 0x80},
+	{0x3702, 0x1e},
+	{0x3703, 0x3c},
+	{0x3704, 0x0e},
+
+	{0x3104, 0x20},
+	{0x0305, 0x04},
+	{0x0307, 0x46},
+	{0x0303, 0x01},
+	{0x0301, 0x0a},
+	{0x3010, 0x01},
+	{0x460e, 0x00},
+
+	{0x5000, 0x00},
+	{0x5002, 0x00},
+	{0x3017, 0xd2},
+	{0x3018, 0x69},
+	{0x3019, 0x96},
+	{0x5047, 0x61},
+	{0x3604, 0x1c},
+	{0x3602, 0x10},
+	{0x3612, 0x21},
+	{0x3630, 0x0a},
+	{0x3631, 0x53},
+	{0x3633, 0x70},
+	{0x4005, 0x1a},
+	{0x4009, 0x10},
+
+	{OV9726_TABLE_END, 0x0000}
+};
+
+static struct ov9726_reg mode_1280x800[] = {
+	{0x0103, 0x01},
+
+	{OV9726_TABLE_WAIT_MS, 10},
+
+	{0x3026, 0x00},
+	{0x3027, 0x00},
+	{0x3705, 0x45},
+	{0x3603, 0xaa},
+	{0x3632, 0x2f},
+	{0x3620, 0x66},
+	{0x3621, 0xc0},
+	{0x0202, 0x03},
+	{0x0203, 0x13},
+	{0x3833, 0x04},
+	{0x3835, 0x02},
+	{0x4702, 0x04},
+	{0x4704, 0x00},
+	{0x4706, 0x08},
+	{0x5052, 0x01},
+	{0x3819, 0x6c},
+	{0x3817, 0x94},
+	{0x404e, 0x7e},
+	{0x3601, 0x40},
+	{0x3610, 0xa0},
+
+	{0x0344, 0x00},
+	{0x0345, 0x00},
+	{0x0346, 0x00},
+	{0x0347, 0x00},
+	{0x034c, 0x05},
+	{0x034d, 0x10},
+	{0x034e, 0x03},
+	{0x034f, 0x28},
+
+	{0x3002, 0x00},
+	{0x3004, 0x00},
+	{0x3005, 0x00},
+	{0x4800, 0x44},
+	{0x4801, 0x0f},
+	{0x4803, 0x05},
+	{0x4601, 0x16},
+	{0x3014, 0x05},
+	{0x0101, 0x01},
+	{0x3707, 0x14},
+	{0x3622, 0x9f},
+	{0x4002, 0x45},
+	{0x5001, 0x00},
+	{0x3406, 0x01},
+	{0x3503, 0x17},
+	{0x0205, 0x3f},
+	{0x0100, 0x01},
+	{0x0112, 0x0a},
+	{0x0113, 0x0a},
+	{0x3013, 0x20},
+	{0x4837, 0x2f},
+	{0x3615, 0xf0},
+	{0x0340, 0x03},
+	{0x0341, 0x48},
+	{0x0342, 0x06},
+	{0x0343, 0x80},
+	{0x3702, 0x1e},
+	{0x3703, 0x3c},
+	{0x3704, 0x0e},
+
+	{0x3104, 0x20},
+	{0x0305, 0x04},
+	{0x0307, 0x46},
+	{0x0303, 0x01},
+	{0x0301, 0x0a},
+	{0x3010, 0x01},
+	{0x460e, 0x00},
+
+	{0x5000, 0x00},
+	{0x5002, 0x00},
+	{0x3017, 0xd2},
+	{0x3018, 0x69},
+	{0x3019, 0x96},
+	{0x5047, 0x61},
+	{0x3604, 0x1c},
+	{0x3602, 0x10},
+	{0x3612, 0x21},
+	{0x3630, 0x0a},
+	{0x3631, 0x53},
+	{0x3633, 0x70},
+	{0x4005, 0x1a},
+	{0x4009, 0x10},
+
+	{OV9726_TABLE_END, 0x0000}
+};
+
+enum {
+	OV9726_MODE_1280x720,
+	OV9726_MODE_1280x800,
+};
+
+static struct ov9726_reg *mode_table[] = {
+	[OV9726_MODE_1280x720] = mode_1280x720,
+	[OV9726_MODE_1280x800] = mode_1280x800,
+};
+
+static inline void
+msleep_range(unsigned int delay_base)
+{
+	usleep_range(delay_base*1000, delay_base*1000 + 500);
+}
+
+static inline int
+ov9726_power_init(struct ov9726_devinfo *dev)
+{
+	struct i2c_client *i2c_client = dev->i2c_client;
+	int err = 0;
+
+	dev->power_rail.sen_1v8_reg = regulator_get(&i2c_client->dev, "dovdd");
+	if (IS_ERR_OR_NULL(dev->power_rail.sen_1v8_reg)) {
+		dev_err(&i2c_client->dev, "%s: failed to get vdd\n",
+			__func__);
+		err = PTR_ERR(dev->power_rail.sen_1v8_reg);
+		goto ov9726_power_init_end;
+	}
+
+	dev->power_rail.sen_2v8_reg = regulator_get(&i2c_client->dev, "avdd");
+	if (IS_ERR_OR_NULL(dev->power_rail.sen_2v8_reg)) {
+		dev_err(&i2c_client->dev, "%s: failed to get vaa\n",
+			__func__);
+		err = PTR_ERR(dev->power_rail.sen_2v8_reg);
+
+		regulator_put(dev->power_rail.sen_1v8_reg);
+		dev->power_rail.sen_1v8_reg = NULL;
+	}
+
+ov9726_power_init_end:
+	return err;
+}
+
+static inline void
+ov9726_power_release(struct ov9726_devinfo *dev)
+{
+	regulator_put(dev->power_rail.sen_1v8_reg);
+	regulator_put(dev->power_rail.sen_2v8_reg);
+}
+
+static int
+ov9726_power(struct ov9726_devinfo *dev, bool pwr_on)
+{
+	struct i2c_client *i2c_client = dev->i2c_client;
+	int rst_active_state = dev->pdata->rst_low_active ? 0 : 1;
+	int pwdn_active_state = dev->pdata->pwdn_low_active ? 0 : 1;
+	int ret = 0;
+
+	dev_info(&i2c_client->dev, "%s %s\n", __func__, pwr_on ? "on" : "off");
+
+	if (pwr_on) {
+		/* pull low the RST pin of ov9726 first */
+		gpio_set_value(dev->pdata->gpio_rst, rst_active_state);
+		msleep_range(1);
+		/* Plug 1.8V and 2.8V power to sensor */
+		ret = regulator_enable(dev->power_rail.sen_1v8_reg);
+		if (ret) {
+			dev_err(&i2c_client->dev, "%s: failed to enable vdd\n",
+				__func__);
+			goto fail_regulator_1v8_reg;
+		}
+
+		msleep_range(20);
+
+		ret = regulator_enable(dev->power_rail.sen_2v8_reg);
+		if (ret) {
+			dev_err(&i2c_client->dev, "%s: failed to enable vaa\n",
+				__func__);
+			goto fail_regulator_2v8_reg;
+		}
+		msleep_range(1);
+		/* turn on ov9726 */
+		gpio_set_value(dev->pdata->gpio_pwdn, !pwdn_active_state);
+
+		msleep_range(5);
+		/* release RST pin */
+		gpio_set_value(dev->pdata->gpio_rst, !rst_active_state);
+		msleep_range(20);
+
+		/* Board specific power-on sequence */
+		dev->pdata->power_on();
+	} else {
+		/* pull low the RST pin of ov9726 */
+		gpio_set_value(dev->pdata->gpio_rst, rst_active_state);
+		msleep_range(1);
+		/* turn off ov9726 */
+		gpio_set_value(dev->pdata->gpio_pwdn, pwdn_active_state);
+		msleep_range(1);
+
+		/* Unplug 1.8V and 2.8V power from sensor */
+		regulator_disable(dev->power_rail.sen_2v8_reg);
+		regulator_disable(dev->power_rail.sen_1v8_reg);
+
+		/* Board specific power-down sequence */
+		dev->pdata->power_off();
+	}
+
+	return 0;
+
+fail_regulator_2v8_reg:
+	regulator_put(dev->power_rail.sen_2v8_reg);
+	dev->power_rail.sen_2v8_reg = NULL;
+	regulator_disable(dev->power_rail.sen_1v8_reg);
+fail_regulator_1v8_reg:
+	regulator_put(dev->power_rail.sen_1v8_reg);
+	dev->power_rail.sen_1v8_reg = NULL;
+	return ret;
+}
+
+static inline void
+ov9726_get_frame_length_regs(struct ov9726_reg *regs, u32 frame_length)
+{
+	regs->addr = OV9726_REG_FRAME_LENGTH_HI;
+	regs->val = (frame_length >> 8) & 0xff;
+	regs++;
+	regs->addr = OV9726_REG_FRAME_LENGTH_LO;
+	regs->val = frame_length & 0xff;
+}
+
+static inline void
+ov9726_get_coarse_time_regs(struct ov9726_reg *regs, u32 coarse_time)
+{
+	regs->addr = OV9726_REG_COARSE_TIME_HI;
+	regs->val = (coarse_time >> 8) & 0xff;
+	regs++;
+	regs->addr = OV9726_REG_COARSE_TIME_LO;
+	regs->val = coarse_time & 0xff;
+}
+
+static inline void
+ov9726_get_gain_reg(struct ov9726_reg *regs, u16 gain)
+{
+	regs->addr = OV9726_REG_GAIN_HI;
+	regs->val = (gain >> 8) & 0xff;
+	regs++;
+	regs->addr = OV9726_REG_GAIN_LO;
+	regs->val = gain & 0xff;
+}
+
+static int
+ov9726_read_reg8(struct i2c_client *client, u16 addr, u8 *val)
+{
+	int	err;
+	struct i2c_msg  msg[2];
+	unsigned char   data[3];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 2;
+	msg[0].buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8)(addr >> 8);
+	data[1] = (u8)(addr & 0xff);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = data + 2;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+
+	if (err != 2)
+		err = -EINVAL;
+	else {
+		*val = data[2];
+		err = 0;
+	}
+
+	return err;
+}
+
+static int
+ov9726_write_reg8(struct i2c_client *client, u16 addr, u8 val)
+{
+	int		err;
+	struct i2c_msg	msg;
+	unsigned char	data[3];
+	int		retry = 0;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = (u8)(addr >> 8);
+	data[1] = (u8)(addr & 0xff);
+	data[2] = (u8)(val & 0xff);
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = data;
+
+	do {
+		err = i2c_transfer(client->adapter, &msg, 1);
+		if (err == 1)
+			break;
+
+		retry++;
+		dev_err(&client->dev,
+			"ov9726: i2c transfer failed, retrying %x %x\n",
+			addr, val);
+		msleep_range(3);
+	} while (retry <= OV9726_MAX_RETRIES);
+
+	return (err != 1);
+}
+
+static int
+ov9726_write_reg16(struct i2c_client *client, u16 addr, u16 val)
+{
+	int		count;
+	struct i2c_msg  msg;
+	unsigned char   data[4];
+	int		retry = 0;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = (u8)(addr >> 8);
+	data[1] = (u8)(addr & 0xff);
+	data[2] = (u8)(val >> 8);
+	data[3] = (u8)(val & 0xff);
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 4;
+	msg.buf = data;
+
+	do {
+		count = i2c_transfer(client->adapter, &msg, 1);
+		if (count == 1)
+			return 0;
+
+		retry++;
+		dev_err(&client->dev,
+			"ov9726: i2c transfer failed, retrying %x %x %x\n",
+			addr, val, count);
+		msleep_range(3);
+	} while (retry <= OV9726_MAX_RETRIES);
+
+	return -EIO;
+}
+
+static int
+ov9726_write_table(
+	struct i2c_client *client,
+	struct ov9726_reg table[],
+	struct ov9726_reg override_list[],
+	int num_override_regs)
+{
+	const struct ov9726_reg	*next;
+	int			err = 0;
+	int			i;
+	u16			val;
+
+	dev_info(&client->dev, "ov9726_write_table\n");
+
+	for (next = table; next->addr != OV9726_TABLE_END; next++) {
+
+		if (next->addr == OV9726_TABLE_WAIT_MS) {
+			msleep_range(next->val);
+			continue;
+		}
+
+		val = next->val;
+
+		/* When an override list is passed in, replace the reg */
+		/* value to write if the reg is in the list */
+		if (override_list) {
+			for (i = 0; i < num_override_regs; i++) {
+				if (next->addr == override_list[i].addr) {
+					val = override_list[i].val;
+					break;
+				}
+			}
+		}
+
+		err = ov9726_write_reg8(client, next->addr, val);
+		if (err)
+			break;
+	}
+
+	return err;
+}
+
+static int
+ov9726_set_frame_length(struct i2c_client *i2c_client, u32 frame_length)
+{
+	int ret;
+
+	dev_info(&i2c_client->dev, "[%s] (0x%08x)\n", __func__,  frame_length);
+	/* hold register value */
+	ret = ov9726_write_reg8(i2c_client, 0x104, 0x01);
+	if (ret)
+		return ret;
+
+	ret = ov9726_write_reg16(i2c_client,
+				  OV9726_REG_FRAME_LENGTH_HI,
+				  frame_length);
+
+	/* release hold, update register value */
+	ret |= ov9726_write_reg8(i2c_client, 0x104, 0x00);
+
+	return ret;
+}
+
+static int
+ov9726_set_coarse_time(struct i2c_client *i2c_client, u32 coarse_time)
+{
+	int ret;
+
+	dev_info(&i2c_client->dev, "[%s] (0x%08x)\n", __func__,  coarse_time);
+	/* hold register value */
+	ret = ov9726_write_reg8(i2c_client, 0x104, 0x01);
+	if (ret)
+		return ret;
+
+	ret = ov9726_write_reg16(i2c_client,
+			  OV9726_REG_COARSE_TIME_HI,
+			  coarse_time);
+
+	/* release hold, update register value */
+	ret |= ov9726_write_reg8(i2c_client, 0x104, 0x00);
+
+	return ret;
+}
+
+static int ov9726_set_gain(struct i2c_client *i2c_client, u16 gain)
+{
+	int ret;
+
+	/* hold register value */
+	ret = ov9726_write_reg8(i2c_client, 0x104, 0x01);
+	if (ret)
+		return ret;
+
+	ret = ov9726_write_reg16(i2c_client, OV9726_REG_GAIN_HI, gain);
+
+	/* release hold, update register value */
+	ret |= ov9726_write_reg8(i2c_client, 0x104, 0x00);
+
+	return ret;
+}
+
+static int ov9726_set_group_hold(struct ov9726_devinfo *dev,
+			struct ov9726_ae *ae)
+{
+#define OV9726_REG_PUSH8(p, a, v) \
+	do { \
+		(p)->addr = (a); \
+		(p)->val = (v); \
+		(p)++; \
+	} while (0)
+
+#define OV9726_REG_PUSH16(ptr, addr, val) do { \
+		OV9726_REG_PUSH8(ptr, (addr), (val) >> 8); \
+		OV9726_REG_PUSH8(ptr, (addr) + 1, (val) & 0xff); \
+	} while (0)
+
+	struct ov9726_reg *gptr = &dev->grphold_temp[0];
+
+	if (!ae->gain_enable &&
+		!ae->coarse_time_enable &&
+		!ae->frame_length_enable)
+		return 0;
+
+	OV9726_REG_PUSH8(gptr, 0x0104, 0x01);
+	if (ae->gain_enable)
+		OV9726_REG_PUSH16(gptr,
+			OV9726_REG_GAIN_HI, ae->gain);
+	if (ae->coarse_time_enable)
+		OV9726_REG_PUSH16(gptr,
+			OV9726_REG_COARSE_TIME_HI, ae->coarse_time);
+	if (ae->frame_length_enable) {
+		OV9726_REG_PUSH16(gptr,
+			OV9726_REG_FRAME_LENGTH_HI, ae->frame_length);
+	}
+	OV9726_REG_PUSH8(gptr, 0x0104, 0x00);
+	OV9726_REG_PUSH8(gptr, OV9726_TABLE_END, 0x00);
+
+	return ov9726_write_table(dev->i2c_client,
+				dev->grphold_temp, NULL, 0);
+}
+
+static int ov9726_get_status(struct i2c_client *i2c_client, u8 *status)
+{
+	int err;
+
+	err = ov9726_read_reg8(i2c_client, 0x003, status);
+	*status = 0;
+	return err;
+}
+
+static int
+ov9726_set_mode(
+	struct ov9726_devinfo *dev,
+	struct ov9726_mode *mode)
+{
+	struct i2c_client	*i2c_client = dev->i2c_client;
+	struct ov9726_reg	reg_override[6];
+	int			err = 0;
+	int			sensor_mode;
+
+	dev_info(&i2c_client->dev, "%s.\n", __func__);
+
+	if (mode->xres == 1280 && mode->yres == 800)
+		sensor_mode = OV9726_MODE_1280x800;
+	else if (mode->xres == 1280 && mode->yres == 720)
+		sensor_mode = OV9726_MODE_1280x720;
+	else {
+		dev_err(&i2c_client->dev,
+			"%s: invalid resolution supplied to set mode %d %d\n",
+			__func__, mode->xres, mode->yres);
+		return -EINVAL;
+	}
+
+	ov9726_get_frame_length_regs(reg_override, mode->frame_length);
+	ov9726_get_coarse_time_regs(reg_override + 2, mode->coarse_time);
+	ov9726_get_gain_reg(reg_override + 4, mode->gain);
+
+	if (dev->mode != mode->mode_id) {
+		dev_info(&i2c_client->dev,
+			"%s: xres %u yres %u framelen %u coarse %u gain %u\n",
+			__func__, mode->xres, mode->yres, mode->frame_length,
+			mode->coarse_time, mode->gain);
+
+		err = ov9726_write_table(i2c_client,
+			mode_table[sensor_mode], reg_override,
+			sizeof(reg_override) / sizeof(reg_override[0]));
+		if (err)
+			goto ov9726_set_mode_exit;
+
+		dev->mode = mode->mode_id;
+	}
+
+ov9726_set_mode_exit:
+	return err;
+}
+
+static long
+ov9726_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct ov9726_devinfo	*dev = file->private_data;
+	struct i2c_client	*i2c_client = dev->i2c_client;
+	int			 err = 0;
+
+	switch (cmd) {
+	case OV9726_IOCTL_SET_MODE:
+	{
+		struct ov9726_mode	mode;
+
+		if (copy_from_user(&mode,
+				   (const void __user *)arg,
+				   sizeof(struct ov9726_mode))) {
+			err = -EFAULT;
+			break;
+		}
+
+		err = ov9726_set_mode(dev, &mode);
+
+		break;
+	}
+	case OV9726_IOCTL_SET_FRAME_LENGTH:
+		err = ov9726_set_frame_length(i2c_client, (u32)arg);
+		break;
+	case OV9726_IOCTL_SET_COARSE_TIME:
+		err = ov9726_set_coarse_time(i2c_client, (u32)arg);
+		break;
+	case OV9726_IOCTL_SET_GAIN:
+		err = ov9726_set_gain(i2c_client, (u16)arg);
+		break;
+	case OV9726_IOCTL_SET_GROUP_HOLD:
+	{
+		struct ov9726_ae ae;
+		if (copy_from_user(&ae,
+			(const void __user *)arg, sizeof(struct ov9726_ae))) {
+			pr_info("%s %d\n", __func__, __LINE__);
+			return -EFAULT;
+		}
+		err = ov9726_set_group_hold(dev, &ae);
+		break;
+	}
+	case OV9726_IOCTL_GET_STATUS:
+	{
+		u8 status;
+
+		err = ov9726_get_status(i2c_client, &status);
+		if (!err) {
+			if (copy_to_user((void __user *)arg,
+					&status, sizeof(status)))
+				err = -EFAULT;
+		}
+		break;
+	}
+	default:
+		err = -EINVAL;
+		break;
+	}
+
+	return err;
+}
+
+static int ov9726_open(struct inode *inode, struct file *file)
+{
+	struct miscdevice	*miscdev = file->private_data;
+	struct ov9726_devinfo	*dev;
+
+	dev = container_of(miscdev, struct ov9726_devinfo, miscdev_info);
+	/* check if device is in use */
+	if (atomic_xchg(&dev->in_use, 1))
+		return -EBUSY;
+	dev->mode = (__u32)-1;
+	file->private_data = dev;
+
+	ov9726_power(dev, true);
+
+	return 0;
+}
+
+int ov9726_release(struct inode *inode, struct file *file)
+{
+	struct ov9726_devinfo *dev;
+
+	dev = file->private_data;
+	file->private_data = NULL;
+
+	ov9726_power(dev, false);
+
+	/* warn if device already released */
+	WARN_ON(!atomic_xchg(&dev->in_use, 0));
+	return 0;
+}
+
+static const struct file_operations ov9726_fileops = {
+	.owner		= THIS_MODULE,
+	.open		= ov9726_open,
+	.unlocked_ioctl	= ov9726_ioctl,
+	.release	= ov9726_release,
+};
+
+static struct miscdevice ov9726_device = {
+	.name		= "ov9726",
+	.minor		= MISC_DYNAMIC_MINOR,
+	.fops		= &ov9726_fileops,
+};
+
+static int
+ov9726_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct ov9726_devinfo	*dev;
+	int			err = 0;
+
+	dev_info(&client->dev, "ov9726: probing sensor.\n");
+
+	dev = kzalloc(sizeof(struct ov9726_devinfo), GFP_KERNEL);
+	if (!dev) {
+		dev_err(&client->dev, "ov9726: Unable to allocate memory!\n");
+		err = -ENOMEM;
+		goto probe_end;
+	}
+
+	memcpy(&(dev->miscdev_info),
+		&ov9726_device,
+		sizeof(struct miscdevice));
+
+	err = misc_register(&(dev->miscdev_info));
+	if (err) {
+		dev_err(&client->dev, "ov9726: Unable to register misc device!\n");
+		goto probe_end;
+	}
+
+	dev->pdata = client->dev.platform_data;
+	dev->i2c_client = client;
+	atomic_set(&dev->in_use, 0);
+	i2c_set_clientdata(client, dev);
+
+	err = ov9726_power_init(dev);
+
+probe_end:
+	if (err) {
+		kfree(dev);
+		dev_err(&client->dev, "failed.\n");
+	}
+
+	return err;
+}
+
+static int ov9726_remove(struct i2c_client *client)
+{
+	struct ov9726_devinfo	*dev;
+
+	dev = i2c_get_clientdata(client);
+	i2c_set_clientdata(client, NULL);
+	misc_deregister(&ov9726_device);
+	ov9726_power_release(dev);
+	kfree(dev);
+
+	return 0;
+}
+
+static const struct i2c_device_id ov9726_id[] = {
+	{"ov9726", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, ov9726_id);
+
+static struct i2c_driver ov9726_i2c_driver = {
+	.driver	= {
+		.name   = "ov9726",
+		.owner  = THIS_MODULE,
+		},
+	.probe  = ov9726_probe,
+	.remove = ov9726_remove,
+	.id_table   = ov9726_id,
+};
+
+static int __init ov9726_init(void)
+{
+	pr_info("ov9726 sensor driver loading\n");
+	return i2c_add_driver(&ov9726_i2c_driver);
+}
+
+static void __exit ov9726_exit(void)
+{
+	i2c_del_driver(&ov9726_i2c_driver);
+}
+
+module_init(ov9726_init);
+module_exit(ov9726_exit);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/video/tegra/sh532u.c b/drivers/media/video/tegra/sh532u.c
new file mode 100644
index 000000000000..b9f9b91d5a45
--- /dev/null
+++ b/drivers/media/video/tegra/sh532u.c
@@ -0,0 +1,2029 @@
+/*
+ * SH532U focuser driver.
+ *
+ * Copyright (C) 2011-2013 NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms 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
+ */
+
+/* Implementation
+ * --------------
+ * The board level details about the device need to be provided in the board
+ * file with the <device>_platform_data structure.
+ * Standard among NVC kernel drivers in this structure is:
+ * .cfg = Use the NVC_CFG_ defines that are in nvc.h.
+ *        Descriptions of the configuration options are with the defines.
+ *        This value is typically 0.
+ * .num = The number of the instance of the device.  This should start at 1 and
+ *        and increment for each device on the board.  This number will be
+ *        appended to the MISC driver name, Example: /dev/focuser.1
+ *        If not used or 0, then nothing is appended to the name.
+ * .sync = If there is a need to synchronize two devices, then this value is
+ *         the number of the device instance (.num above) this device is to
+ *         sync to.  For example:
+ *         Device 1 platform entries =
+ *         .num = 1,
+ *         .sync = 2,
+ *         Device 2 platfrom entries =
+ *         .num = 2,
+ *         .sync = 1,
+ *         The above example sync's device 1 and 2.
+ *         To disable sync, set .sync = 0.  Note that the .num = 0 device is
+ *         not allowed to be synced to.
+ *         This is typically used for stereo applications.
+ * .dev_name = The MISC driver name the device registers as.  If not used,
+ *             then the part number of the device is used for the driver name.
+ *             If using the NVC user driver then use the name found in this
+ *             driver under _default_pdata.
+ * .gpio_count = The ARRAY_SIZE of the nvc_gpio_pdata table.
+ * .gpio = A pointer to the nvc_gpio_pdata structure's platform GPIO data.
+ *         The GPIO mechanism works by cross referencing the .gpio_type key
+ *         among the nvc_gpio_pdata GPIO data and the driver's nvc_gpio_init
+ *         GPIO data to build a GPIO table the driver can use.  The GPIO's
+ *         defined in the device header file's _gpio_type enum are the
+ *         gpio_type keys for the nvc_gpio_pdata and nvc_gpio_init structures.
+ *         These need to be present in the board file's nvc_gpio_pdata
+ *         structure for the GPIO's that are used.
+ *         The driver's GPIO logic uses assert/deassert throughout until the
+ *         low level _gpio_wr/rd calls where the .assert_high is used to
+ *         convert the value to the correct signal level.
+ *         See the GPIO notes in nvc.h for additional information.
+ *
+ * The following is specific to NVC kernel focus drivers:
+ * .nvc = Pointer to the nvc_focus_nvc structure.  This structure needs to
+ *        be defined and populated if overriding the driver defaults.
+ * .cap = Pointer to the nvc_focus_cap structure.  This structure needs to
+ *        be defined and populated if overriding the driver defaults.
+ *
+ * The following is specific to this NVC kernel focus driver:
+ * .info = Pointer to the sh532u_pdata_info structure.  This structure does
+ *         not need to be defined and populated unless overriding ROM data.
+.* .i2c_addr_rom = The I2C address of the onboard ROM.
+ *
+ * Power Requirements:
+ * The device's header file defines the voltage regulators needed with the
+ * enumeration <device>_vreg.  The order these are enumerated is the order
+ * the regulators will be enabled when powering on the device.  When the
+ * device is powered off the regulators are disabled in descending order.
+ * The <device>_vregs table in this driver uses the nvc_regulator_init
+ * structure to define the regulator ID strings that go with the regulators
+ * defined with <device>_vreg.  These regulator ID strings (or supply names)
+ * will be used in the regulator_get function in the _vreg_init function.
+ * The board power file and <device>_vregs regulator ID strings must match.
+ */
+
+
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+#include <linux/list.h>
+#include <linux/jiffies.h>
+#include <linux/gpio.h>
+#include <media/sh532u.h>
+
+#define SH532U_ID		0x0532
+#define SH532U_STARTUP_DELAY_MS	10
+/* defaults if no ROM data */
+#define SH532U_HYPERFOCAL_RATIO 1836 /* 41.2f/224.4f Ratio source: SEMCO */
+/* _HYPERFOCAL_RATIO is multiplied and _HYPERFOCAL_DIV divides for float */
+#define SH532U_HYPERFOCAL_DIV	10000
+#define SH532U_FOCAL_LENGTH	0x408D70A4
+#define SH532U_FNUMBER		0x40333333
+#define SH532U_MAX_APERATURE	0x3FCA0EA1
+#define SH532U_ACTUATOR_RANGE	1000
+#define SH532U_SETTLETIME	30
+#define SH532U_FOCUS_MACRO	950
+#define SH532U_FOCUS_HYPER	250
+#define SH532U_FOCUS_INFINITY	50
+#define SH532U_TIMEOUT_MS	200
+#define SH532U_POS_LOW_DEFAULT	0xA000
+#define SH532U_POS_HIGH_DEFAULT	0x6000
+#define SH532U_SLEW_RATE			1
+#define SH532U_POS_TRANSLATE		0
+#define SH532U_POS_SIGN_CHANGER		(-1)
+
+static u8 sh532u_ids[] = {
+	0xF0,
+};
+
+static struct nvc_gpio_init sh532u_gpios[] = {
+	{ SH532U_GPIO_RESET, GPIOF_OUT_INIT_LOW, "reset", false, true, },
+	{ SH532U_GPIO_I2CMUX, 0, "i2c_mux", 0, false, },
+	{ SH532U_GPIO_GP1, 0, "gp1", 0, false, },
+	{ SH532U_GPIO_GP2, 0, "gp2", 0, false, },
+	{ SH532U_GPIO_GP3, 0, "gp3", 0, false, },
+};
+
+static struct nvc_regulator_init sh532u_vregs[] = {
+	{ SH532U_VREG_AVDD, "avdd", },
+	{ SH532U_VREG_DVDD, "dvdd", },
+};
+
+struct sh532u_info {
+	atomic_t in_use;
+	struct i2c_client *i2c_client;
+	struct sh532u_platform_data *pdata;
+	struct miscdevice miscdev;
+	struct list_head list;
+	struct nvc_gpio gpio[ARRAY_SIZE(sh532u_gpios)];
+	struct nvc_regulator vreg[ARRAY_SIZE(sh532u_vregs)];
+	int pwr_api;
+	int pwr_dev;
+	u8 s_mode;
+	struct sh532u_info *s_info;
+	u8 id_minor;
+	unsigned i2c_addr_rom;
+	struct nvc_focus_nvc nvc;
+	struct nvc_focus_cap cap;
+	struct nv_focuser_config config;
+	enum nvc_focus_sts sts;
+	struct sh532u_pdata_info cfg;
+	bool reset_flag;
+	bool init_cal_flag;
+	s16 abs_base;
+	u32 abs_range;
+	u32 pos_rel;
+	s16 pos_abs;
+	long pos_time_wr;
+};
+
+static struct sh532u_pdata_info sh532u_default_info = {
+	.move_timeoutms	= SH532U_TIMEOUT_MS,
+	.focus_hyper_ratio = SH532U_HYPERFOCAL_RATIO,
+	.focus_hyper_div = SH532U_HYPERFOCAL_DIV,
+};
+
+static struct nvc_focus_cap sh532u_default_cap = {
+	.version	= NVC_FOCUS_CAP_VER2,
+	.actuator_range = SH532U_ACTUATOR_RANGE,
+	.settle_time	= SH532U_SETTLETIME,
+	.focus_macro	= SH532U_FOCUS_MACRO,
+	.focus_hyper	= SH532U_FOCUS_HYPER,
+	.focus_infinity	= SH532U_FOCUS_INFINITY,
+	.slew_rate	  = SH532U_SLEW_RATE,
+	.position_translate = SH532U_POS_TRANSLATE,
+};
+
+static struct nvc_focus_nvc sh532u_default_nvc = {
+	.focal_length	= SH532U_FOCAL_LENGTH,
+	.fnumber	= SH532U_FNUMBER,
+	.max_aperature	= SH532U_MAX_APERATURE,
+};
+
+static struct sh532u_platform_data sh532u_default_pdata = {
+	.cfg		= 0,
+	.num		= 0,
+	.sync		= 0,
+	.dev_name	= "focuser",
+	.i2c_addr_rom	= 0x50,
+};
+
+static u32 sh532u_a2buf[] = {
+	0x0018019c,
+	0x0018019d,
+	0x0000019e,
+	0x007f0192,
+	0x00000194,
+	0x00f00184,
+	0x00850187,
+	0x0000018a,
+	0x00fd7187,
+	0x007f7183,
+	0x0008025a,
+	0x05042218,
+	0x80010216,
+	0x000601a0,
+	0x00808183,
+	0xffffffff
+};
+
+static LIST_HEAD(sh532u_info_list);
+static DEFINE_SPINLOCK(sh532u_spinlock);
+
+
+static int sh532u_i2c_rd8(struct sh532u_info *info, u8 addr, u8 reg, u8 *val)
+{
+	struct i2c_msg msg[2];
+	u8 buf[2];
+
+	buf[0] = reg;
+	if (addr) {
+		msg[0].addr = addr;
+		msg[1].addr = addr;
+	} else {
+		msg[0].addr = info->i2c_client->addr;
+		msg[1].addr = info->i2c_client->addr;
+	}
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &buf[0];
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = &buf[1];
+	*val = 0;
+	if (i2c_transfer(info->i2c_client->adapter, msg, 2) != 2)
+		return -EIO;
+
+	*val = buf[1];
+	return 0;
+}
+
+static int sh532u_i2c_wr8(struct sh532u_info *info, u8 reg, u8 val)
+{
+	struct i2c_msg msg;
+	u8 buf[2];
+
+	buf[0] = reg;
+	buf[1] = val;
+	msg.addr = info->i2c_client->addr;
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = &buf[0];
+	if (i2c_transfer(info->i2c_client->adapter, &msg, 1) != 1)
+		return -EIO;
+
+	return 0;
+}
+
+static int sh532u_i2c_rd16(struct sh532u_info *info, u8 reg, u16 *val)
+{
+	struct i2c_msg msg[2];
+	u8 buf[3];
+
+	buf[0] = reg;
+	msg[0].addr = info->i2c_client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &buf[0];
+	msg[1].addr = info->i2c_client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 2;
+	msg[1].buf = &buf[1];
+	if (i2c_transfer(info->i2c_client->adapter, msg, 2) != 2)
+		return -EIO;
+
+	*val = (((u16)buf[1] << 8) | (u16)buf[2]);
+	return 0;
+}
+
+
+static int sh532u_i2c_wr16(struct sh532u_info *info, u8 reg, u16 val)
+{
+	struct i2c_msg msg;
+	u8 buf[3];
+
+	buf[0] = reg;
+	buf[1] = (u8)(val >> 8);
+	buf[2] = (u8)(val & 0xff);
+	msg.addr = info->i2c_client->addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = &buf[0];
+	if (i2c_transfer(info->i2c_client->adapter, &msg, 1) != 1)
+		return -EIO;
+
+	return 0;
+}
+
+static int sh532u_i2c_rd32(struct sh532u_info *info, u8 addr, u8 reg, u32 *val)
+{
+	struct i2c_msg msg[2];
+	u8 buf[5];
+
+	buf[0] = reg;
+	if (addr) {
+		msg[0].addr = addr;
+		msg[1].addr = addr;
+	} else {
+		msg[0].addr = info->i2c_client->addr;
+		msg[1].addr = info->i2c_client->addr;
+	}
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &buf[0];
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 4;
+	msg[1].buf = &buf[1];
+	if (i2c_transfer(info->i2c_client->adapter, msg, 2) != 2)
+		return -EIO;
+
+	*val = (((u32)buf[4] << 24) | ((u32)buf[3] << 16) |
+			((u32)buf[2] << 8) | ((u32)buf[1]));
+	return 0;
+}
+
+static int sh532u_gpio_wr(struct sh532u_info *info,
+			  enum sh532u_gpio i,
+			  int val) /* val: 0=deassert, 1=assert */
+{
+	int err = -EINVAL;
+
+	if (info->gpio[i].flag) {
+		if (val)
+			val = 1;
+		if (!info->gpio[i].active_high)
+			val = !val;
+		val &= 1;
+		err = val;
+		gpio_set_value_cansleep(info->gpio[i].gpio, val);
+		dev_dbg(&info->i2c_client->dev, "%s %u %d\n",
+			__func__, info->gpio[i].gpio, val);
+	}
+	return err; /* return value written or error */
+}
+
+static int sh532u_gpio_reset(struct sh532u_info *info, int val)
+{
+	int err = 0;
+
+	if (val) {
+		if (!info->reset_flag) {
+			info->reset_flag = true;
+			err = sh532u_gpio_wr(info, SH532U_GPIO_RESET, 1);
+			if (err < 0)
+				return 0; /* flag no reset */
+
+			mdelay(1);
+			sh532u_gpio_wr(info, SH532U_GPIO_RESET, 0);
+			mdelay(SH532U_STARTUP_DELAY_MS); /* startup delay */
+			err = 1; /* flag that a reset was done */
+		}
+	} else {
+		info->reset_flag = false;
+	}
+	return err;
+}
+
+static void sh532u_gpio_able(struct sh532u_info *info, int val)
+{
+	if (val) {
+		sh532u_gpio_wr(info, SH532U_GPIO_GP1, val);
+		sh532u_gpio_wr(info, SH532U_GPIO_GP2, val);
+		sh532u_gpio_wr(info, SH532U_GPIO_GP3, val);
+	} else {
+		sh532u_gpio_wr(info, SH532U_GPIO_GP3, val);
+		sh532u_gpio_wr(info, SH532U_GPIO_GP2, val);
+		sh532u_gpio_wr(info, SH532U_GPIO_GP1, val);
+	}
+}
+
+static void sh532u_gpio_exit(struct sh532u_info *info)
+{
+	unsigned i;
+
+	for (i = 0; i < ARRAY_SIZE(sh532u_gpios); i++) {
+		if (info->gpio[i].flag && info->gpio[i].own) {
+			gpio_free(info->gpio[i].gpio);
+			info->gpio[i].own = false;
+		}
+	}
+}
+
+static void sh532u_gpio_init(struct sh532u_info *info)
+{
+	char label[32];
+	unsigned long flags;
+	unsigned type;
+	unsigned i;
+	unsigned j;
+	int err;
+
+	for (i = 0; i < ARRAY_SIZE(sh532u_gpios); i++)
+		info->gpio[i].flag = false;
+	if (!info->pdata->gpio_count || !info->pdata->gpio)
+		return;
+
+	for (i = 0; i < ARRAY_SIZE(sh532u_gpios); i++) {
+		type = sh532u_gpios[i].gpio_type;
+		for (j = 0; j < info->pdata->gpio_count; j++) {
+			if (type == info->pdata->gpio[j].gpio_type)
+				break;
+		}
+		if (j == info->pdata->gpio_count)
+			continue;
+
+		info->gpio[type].gpio = info->pdata->gpio[j].gpio;
+		info->gpio[type].flag = true;
+		if (sh532u_gpios[i].use_flags) {
+			flags = sh532u_gpios[i].flags;
+			info->gpio[type].active_high =
+						   sh532u_gpios[i].active_high;
+		} else {
+			info->gpio[type].active_high =
+					      info->pdata->gpio[j].active_high;
+			if (info->gpio[type].active_high)
+				flags = GPIOF_OUT_INIT_LOW;
+			else
+				flags = GPIOF_OUT_INIT_HIGH;
+		}
+		if (!info->pdata->gpio[j].init_en)
+			continue;
+
+		snprintf(label, sizeof(label), "sh532u_%u_%s",
+			 info->pdata->num, sh532u_gpios[i].label);
+		err = gpio_request_one(info->gpio[type].gpio, flags, label);
+		if (err) {
+			dev_err(&info->i2c_client->dev, "%s ERR %s %u\n",
+				__func__, label, info->gpio[type].gpio);
+		} else {
+			info->gpio[type].own = true;
+			dev_dbg(&info->i2c_client->dev, "%s %s %u\n",
+				__func__, label, info->gpio[type].gpio);
+		}
+	}
+}
+
+static int sh532u_vreg_dis(struct sh532u_info *info,
+			   enum sh532u_vreg i)
+{
+	int err = 0;
+
+	if (info->vreg[i].vreg_flag && (info->vreg[i].vreg != NULL)) {
+		err = regulator_disable(info->vreg[i].vreg);
+		if (!err)
+			dev_dbg(&info->i2c_client->dev, "%s: %s\n",
+				__func__, info->vreg[i].vreg_name);
+		else
+			dev_err(&info->i2c_client->dev, "%s %s ERR\n",
+				__func__, info->vreg[i].vreg_name);
+	}
+	info->vreg[i].vreg_flag = false;
+	return err;
+}
+
+static int sh532u_vreg_dis_all(struct sh532u_info *info)
+{
+	unsigned i;
+	int err = 0;
+
+	for (i = ARRAY_SIZE(sh532u_vregs); i > 0; i--)
+		err |= sh532u_vreg_dis(info, (i - 1));
+	return err;
+}
+
+static int sh532u_vreg_en(struct sh532u_info *info,
+			  enum sh532u_vreg i)
+{
+	int err = 0;
+
+	if (!info->vreg[i].vreg_flag && (info->vreg[i].vreg != NULL)) {
+		err = regulator_enable(info->vreg[i].vreg);
+		if (!err) {
+			dev_dbg(&info->i2c_client->dev, "%s: %s\n",
+				__func__, info->vreg[i].vreg_name);
+			info->vreg[i].vreg_flag = true;
+			err = 1; /* flag regulator state change */
+		} else {
+			dev_err(&info->i2c_client->dev, "%s %s ERR\n",
+				__func__, info->vreg[i].vreg_name);
+		}
+	}
+	return err;
+}
+
+static int sh532u_vreg_en_all(struct sh532u_info *info)
+{
+	unsigned i;
+	int err = 0;
+
+	for (i = 0; i < ARRAY_SIZE(sh532u_vregs); i++)
+		err |= sh532u_vreg_en(info, i);
+	return err;
+}
+
+static void sh532u_vreg_exit(struct sh532u_info *info)
+{
+	unsigned i;
+
+	for (i = 0; i < ARRAY_SIZE(sh532u_vregs); i++) {
+		regulator_put(info->vreg[i].vreg);
+		info->vreg[i].vreg = NULL;
+	}
+}
+
+static int sh532u_vreg_init(struct sh532u_info *info)
+{
+	unsigned i;
+	unsigned j;
+	int err = 0;
+
+	for (i = 0; i < ARRAY_SIZE(sh532u_vregs); i++) {
+		j = sh532u_vregs[i].vreg_num;
+		info->vreg[j].vreg_name = sh532u_vregs[i].vreg_name;
+		info->vreg[j].vreg_flag = false;
+		info->vreg[j].vreg = regulator_get(&info->i2c_client->dev,
+						   info->vreg[j].vreg_name);
+		if (IS_ERR_OR_NULL(info->vreg[j].vreg)) {
+			if (PTR_ERR(info->vreg[j].vreg) != -ENODEV)
+				dev_dbg(&info->i2c_client->dev,
+					"%s %s ERR: %d\n",
+					__func__, info->vreg[j].vreg_name,
+					(int)info->vreg[j].vreg);
+			else
+				dev_info(&info->i2c_client->dev,
+					 "%s no regulator found for %s. "
+					 "This board may not have an "
+					 "independent %s regulator.\n",
+					 __func__, info->vreg[j].vreg_name,
+					 info->vreg[j].vreg_name);
+			err |= PTR_ERR(info->vreg[j].vreg);
+			info->vreg[j].vreg = NULL;
+		} else {
+			dev_dbg(&info->i2c_client->dev, "%s: %s\n",
+				__func__, info->vreg[j].vreg_name);
+		}
+	}
+	return err;
+}
+
+static int sh532u_pm_wr(struct sh532u_info *info, int pwr)
+{
+	int err = 0;
+
+	if ((info->pdata->cfg & (NVC_CFG_OFF2STDBY | NVC_CFG_BOOT_INIT)) &&
+			(pwr == NVC_PWR_OFF ||
+			 pwr == NVC_PWR_STDBY_OFF))
+		pwr = NVC_PWR_STDBY;
+
+	if (pwr == info->pwr_dev)
+		return 0;
+
+	switch (pwr) {
+	case NVC_PWR_OFF_FORCE:
+	case NVC_PWR_OFF:
+		err = sh532u_vreg_dis_all(info);
+		sh532u_gpio_able(info, 0);
+		sh532u_gpio_reset(info, 0);
+		break;
+
+	case NVC_PWR_STDBY_OFF:
+	case NVC_PWR_STDBY:
+		err = sh532u_vreg_en_all(info);
+		sh532u_gpio_able(info, 1);
+		sh532u_gpio_reset(info, 1);
+		err |= sh532u_i2c_wr8(info, STBY_211, 0x80);
+		err |= sh532u_i2c_wr8(info, CLKSEL_211, 0x38);
+		err |= sh532u_i2c_wr8(info, CLKSEL_211, 0x39);
+		break;
+
+	case NVC_PWR_COMM:
+	case NVC_PWR_ON:
+		err = sh532u_vreg_en_all(info);
+		sh532u_gpio_able(info, 1);
+		sh532u_gpio_reset(info, 1);
+		err |= sh532u_i2c_wr8(info, CLKSEL_211, 0x38);
+		err |= sh532u_i2c_wr8(info, CLKSEL_211, 0x34);
+		err |= sh532u_i2c_wr8(info, STBY_211, 0xF0);
+		break;
+
+	default:
+		err = -EINVAL;
+		break;
+	}
+
+	if (err < 0) {
+		dev_err(&info->i2c_client->dev, "%s err %d\n", __func__, err);
+		pwr = NVC_PWR_ERR;
+	}
+	info->pwr_dev = pwr;
+	dev_dbg(&info->i2c_client->dev, "%s pwr_dev=%d\n",
+		__func__, info->pwr_dev);
+	if (err > 0)
+		return 0;
+
+	return err;
+}
+
+static int sh532u_pm_wr_s(struct sh532u_info *info, int pwr)
+{
+	int err1 = 0;
+	int err2 = 0;
+
+	if ((info->s_mode == NVC_SYNC_OFF) ||
+			(info->s_mode == NVC_SYNC_MASTER) ||
+			(info->s_mode == NVC_SYNC_STEREO))
+		err1 = sh532u_pm_wr(info, pwr);
+	if ((info->s_mode == NVC_SYNC_SLAVE) ||
+			(info->s_mode == NVC_SYNC_STEREO))
+		err2 = sh532u_pm_wr(info->s_info, pwr);
+	return err1 | err2;
+}
+
+static int sh532u_pm_api_wr(struct sh532u_info *info, int pwr)
+{
+	int err = 0;
+
+	if (!pwr || (pwr > NVC_PWR_ON))
+		return 0;
+
+	if (pwr > info->pwr_dev)
+		err = sh532u_pm_wr_s(info, pwr);
+	if (!err)
+		info->pwr_api = pwr;
+	else
+		info->pwr_api = NVC_PWR_ERR;
+	if (info->pdata->cfg & NVC_CFG_NOERR)
+		return 0;
+
+	return err;
+}
+
+static int sh532u_pm_dev_wr(struct sh532u_info *info, int pwr)
+{
+	if (pwr < info->pwr_api)
+		pwr = info->pwr_api;
+	if (info->sts == NVC_FOCUS_STS_WAIT_FOR_MOVE_END)
+		pwr = NVC_PWR_ON;
+	return sh532u_pm_wr(info, pwr);
+}
+
+static void sh532u_pm_exit(struct sh532u_info *info)
+{
+	sh532u_pm_wr(info, NVC_PWR_OFF_FORCE);
+	sh532u_vreg_exit(info);
+	sh532u_gpio_exit(info);
+}
+
+static void sh532u_pm_init(struct sh532u_info *info)
+{
+	sh532u_gpio_init(info);
+	sh532u_vreg_init(info);
+}
+
+static int sh532u_reset(struct sh532u_info *info, u32 level)
+{
+	int err;
+
+	if (level == NVC_RESET_SOFT) {
+		err = sh532u_pm_wr(info, NVC_PWR_COMM);
+		err |= sh532u_i2c_wr8(info, SFTRST_211, 0xFF); /* SW reset */
+		mdelay(1);
+		err |= sh532u_i2c_wr8(info, SFTRST_211, 0);
+	} else {
+		err = sh532u_pm_wr(info, NVC_PWR_OFF_FORCE);
+	}
+	err |= sh532u_pm_wr(info, info->pwr_api);
+	return err;
+}
+
+static int sh532u_dev_id(struct sh532u_info *info)
+{
+	u8 val = 0;
+	unsigned i;
+	int err;
+
+	sh532u_pm_dev_wr(info, NVC_PWR_COMM);
+	err = sh532u_i2c_rd8(info, 0, HVCA_DEVICE_ID, &val);
+	if (!err) {
+		dev_dbg(&info->i2c_client->dev, "%s found devId: %x\n",
+			__func__, val);
+		info->id_minor = 0;
+		for (i = 0; i < ARRAY_SIZE(sh532u_ids); i++) {
+			if (val == sh532u_ids[i]) {
+				info->id_minor = val;
+				break;
+			}
+		}
+		if (!info->id_minor) {
+			err = -ENODEV;
+			dev_dbg(&info->i2c_client->dev, "%s No devId match\n",
+				__func__);
+		}
+	}
+	sh532u_pm_dev_wr(info, NVC_PWR_OFF);
+	return err;
+}
+
+static void sh532u_sts_rd(struct sh532u_info *info)
+{
+	u8 us_tmp;
+	u16 us_smv_fin;
+	int err;
+
+	if (info->sts == NVC_FOCUS_STS_INITIALIZING)
+		return;
+
+	info->sts = NVC_FOCUS_STS_NO_DEVICE; /* assume I2C err */
+	err = sh532u_i2c_rd8(info, 0, STMVEN_211, &us_tmp);
+	err |= sh532u_i2c_rd16(info, RZ_211H, &us_smv_fin);
+	if (err)
+		return;
+
+	/* StepMove Error Handling, Unexpected Position */
+	if ((us_smv_fin == 0x7FFF) || (us_smv_fin == 0x8001))
+		/* Stop StepMove Operation */
+		sh532u_i2c_wr8(info, STMVEN_211, us_tmp & 0xFE);
+	if (us_tmp & STMVEN_ON) {
+		err = sh532u_i2c_rd8(info, 0, MSSET_211, &us_tmp);
+		if (!err) {
+			if (us_tmp & CHTGST_ON)
+				info->sts = NVC_FOCUS_STS_WAIT_FOR_SETTLE;
+			else
+				info->sts = NVC_FOCUS_STS_LENS_SETTLED;
+		}
+	} else {
+		info->sts = NVC_FOCUS_STS_WAIT_FOR_MOVE_END;
+	}
+}
+
+static s16 sh532u_rel2abs(struct sh532u_info *info, s32 rel_position)
+{
+	s16 abs_pos;
+
+	if (rel_position > info->cap.actuator_range)
+		rel_position = info->cap.actuator_range;
+	if (info->cap.position_translate)  {
+		rel_position = info->cap.actuator_range - rel_position;
+		if (rel_position) {
+			rel_position *= info->abs_range;
+			rel_position /= info->cap.actuator_range;
+		}
+		abs_pos = (s16)(info->abs_base + rel_position);
+	} else {
+		abs_pos = rel_position * SH532U_POS_SIGN_CHANGER;
+	}
+
+	if (abs_pos < info->cfg.limit_low)
+		abs_pos = info->cfg.limit_low;
+	if (abs_pos > info->cfg.limit_high)
+		abs_pos = info->cfg.limit_high;
+
+	dev_dbg(&info->i2c_client->dev, "%s: rel_position %d returning abs_pos %d\n",
+			__func__, rel_position, abs_pos);
+
+	return abs_pos;
+}
+
+static u32 sh532u_abs2rel(struct sh532u_info *info, s16 abs_position)
+{
+	u32 rel_pos;
+
+	if (abs_position > info->cfg.limit_high)
+		abs_position = info->cfg.limit_high;
+	if (abs_position < info->abs_base)
+		abs_position = info->abs_base;
+
+	if (info->cap.position_translate) {
+		rel_pos = (u32)(abs_position - info->abs_base);
+		rel_pos *= info->cap.actuator_range;
+		rel_pos /= info->abs_range;
+
+		if (rel_pos > info->cap.actuator_range)
+			rel_pos = info->cap.actuator_range;
+		rel_pos = info->cap.actuator_range - rel_pos;
+	} else {
+		rel_pos = abs_position * SH532U_POS_SIGN_CHANGER;
+	}
+	dev_dbg(&info->i2c_client->dev, "%s: abs_position %d returning rel_pos %d",
+			__func__, abs_position, rel_pos);
+
+	return rel_pos;
+}
+
+static int sh532u_abs_pos_rd(struct sh532u_info *info, s16 *position)
+{
+	int err;
+	u16 abs_pos = 0;
+
+	err = sh532u_i2c_rd16(info, RZ_211H, &abs_pos);
+	*position = (s16)abs_pos;
+	return err;
+}
+
+static int sh532u_rel_pos_rd(struct sh532u_info *info, s32 *position)
+{
+	s16 abs_pos;
+	long msec;
+	int pos;
+	int err;
+
+	err = sh532u_abs_pos_rd(info, &abs_pos);
+	if (err)
+		return -EINVAL;
+
+	if ((abs_pos >= (info->pos_abs - STMV_SIZE)) &&
+			(abs_pos <= (info->pos_abs + STMV_SIZE))) {
+		pos = (int)info->pos_rel;
+	} else {
+		msec = jiffies;
+		msec -= info->pos_time_wr;
+		msec = msec * 1000 / HZ;
+		sh532u_sts_rd(info);
+		if ((info->sts == NVC_FOCUS_STS_LENS_SETTLED) ||
+				(msec > info->cfg.move_timeoutms)) {
+			pos = (int)info->pos_rel;
+		} else {
+			pos = (int)sh532u_abs2rel(info, abs_pos);
+			if ((pos == (info->pos_rel - 1)) ||
+					(pos == (info->pos_rel + 1)))
+				pos = (int)info->pos_rel;
+		}
+	}
+	if (info->cap.position_translate) {
+		if (pos < 0)
+			pos = 0;
+	}
+	*position = pos;
+	return 0;
+}
+
+static void sh532u_calibration_caps(struct sh532u_info *info)
+{
+	s16 abs_top;
+	u32 rel_range;
+	u32 rel_lo;
+	u32 rel_hi;
+	u32 step;
+	u32 loop_limit;
+	u32 i;
+
+	/*
+	 * calculate relative and absolute positions
+	 * Note that relative values, what upper SW uses, are the
+	 * abstraction of HW (absolute) values.
+	 * |<--limit_low                                  limit_high-->|
+	 * | |<-------------------_ACTUATOR_RANGE------------------->| |
+	 *              -focus_inf                        -focus_mac
+	 *   |<---RI--->|                                 |<---RM--->|
+	 *   -abs_base  -pos_low                          -pos_high  -abs_top
+	 *
+	 * The pos_low and pos_high are fixed absolute positions and correspond
+	 * to the relative focus_infinity and focus_macro, respectively.  We'd
+	 * like to have "wiggle" room (RI and RM) around these relative
+	 * positions so the loop below finds the best fit for RI and RM without
+	 * passing the absolute limits.
+	 * We want our _ACTUATOR_RANGE to be infinity on the 0 end and macro
+	 * on the max end.  However, the focuser HW is opposite this.
+	 * Therefore we use the rel(ative)_lo/hi variables in the calculation
+	 * loop and assign them the focus_infinity and focus_macro values.
+	 */
+	rel_lo = (info->cap.actuator_range - info->cap.focus_macro);
+	rel_hi = info->cap.focus_infinity;
+	info->abs_range = (u32)(info->cfg.pos_high - info->cfg.pos_low);
+	loop_limit = (rel_lo > rel_hi) ? rel_lo : rel_hi;
+	dev_dbg(&info->i2c_client->dev, "%s: rel_lo %d rel_hi %d loop_limit %d\n",
+				__func__, rel_lo, rel_hi, loop_limit);
+	for (i = 0; i <= loop_limit; i++) {
+		rel_range = info->cap.actuator_range - (rel_lo + rel_hi);
+		step = info->abs_range / rel_range;
+		info->abs_base = info->cfg.pos_low - (step * rel_lo);
+		abs_top = info->cfg.pos_high + (step * rel_hi);
+		if (info->abs_base < info->cfg.limit_low) {
+			if (rel_lo > 0)
+				rel_lo--;
+		}
+		if (abs_top > info->cfg.limit_high) {
+			if (rel_hi > 0)
+				rel_hi--;
+		}
+		if (info->abs_base >= info->cfg.limit_low &&
+					abs_top <= info->cfg.limit_high)
+			break;
+	}
+	dev_dbg(&info->i2c_client->dev, "%s: info->abs_range %d abs_base %d abs_top %d\n",
+			__func__, info->abs_range, info->abs_base, abs_top);
+
+	if (!info->cap.position_translate && info->abs_range)
+		info->cap.actuator_range = info->abs_range;
+
+	info->cap.focus_hyper = info->abs_range;
+	info->abs_range = (u32)(abs_top - info->abs_base);
+	/* calculate absolute hyperfocus position */
+	info->cap.focus_hyper *= info->cfg.focus_hyper_ratio;
+	info->cap.focus_hyper /= info->cfg.focus_hyper_div;
+	abs_top = (s16)(info->cfg.pos_high - info->cap.focus_hyper);
+
+	/* update actual relative positions */
+	info->cap.focus_hyper = sh532u_abs2rel(info, abs_top);
+	dev_dbg(&info->i2c_client->dev, "%s: focus_hyper abs %d rel %d\n",
+			__func__, abs_top, info->cap.focus_hyper);
+
+	info->cap.focus_infinity = sh532u_abs2rel(info, info->cfg.pos_high);
+	dev_dbg(&info->i2c_client->dev, "%s: focus_infinity abs %d rel %d\n",
+			__func__, info->cfg.pos_high, info->cap.focus_infinity);
+
+	info->cap.focus_macro = sh532u_abs2rel(info, info->cfg.pos_low);
+	dev_dbg(&info->i2c_client->dev, "%s: focus_macro abs %d rel %d\n",
+			__func__, info->cfg.pos_low, info->cap.focus_macro);
+
+	dev_dbg(&info->i2c_client->dev, "%s: Version %d actuator_range %d "
+			"settle_time %d position_traslate %d\n",
+			__func__, info->cap.version, info->cap.actuator_range,
+			info->cap.settle_time, info->cap.position_translate);
+}
+
+static int sh532u_calibration(struct sh532u_info *info, bool use_defaults)
+{
+	u8 reg;
+	int err;
+	int ret = 0;
+
+	if (info->init_cal_flag) {
+		dev_dbg(&info->i2c_client->dev, "%s: Already initialized"
+				"Returning\n", __func__);
+		return 0;
+	}
+
+	/*
+	 * Get Inf1, Mac1
+	 * Inf1 and Mac1 are the mechanical limit position.
+	 * Inf1: top limit.
+	 * Mac1: bottom limit.
+	 */
+	err = sh532u_i2c_rd8(info, info->i2c_addr_rom, addrMac1, &reg);
+	if (!err && (reg != 0) && (reg != 0xFF))
+		info->cfg.limit_low = (reg<<8) & 0xff00;
+	ret = err;
+	err = sh532u_i2c_rd8(info, info->i2c_addr_rom, addrInf1, &reg);
+	if (!err && (reg != 0) && (reg != 0xFF))
+		info->cfg.limit_high = (reg<<8) & 0xff00;
+	ret |= err;
+	/*
+	 * Get Inf2, Mac2
+	 * Inf2 and Mac2 are the calibration data for SEMCO AF lens.
+	 * Inf2: Best focus (lens position) when object distance is 1.2M.
+	 * Mac2: Best focus (lens position) when object distance is 10cm.
+	 */
+	err = sh532u_i2c_rd8(info, info->i2c_addr_rom, addrMac2, &reg);
+	if (!err && (reg != 0) && (reg != 0xFF))
+		info->cfg.pos_low = (reg << 8) & 0xff00;
+	ret |= err;
+	err = sh532u_i2c_rd8(info, info->i2c_addr_rom, addrInf2, &reg);
+	if (!err && (reg != 0) && (reg != 0xFF))
+		info->cfg.pos_high = (reg << 8) & 0xff00;
+	ret |= err;
+	/* set overrides */
+	if (info->pdata->info) {
+		if (info->pdata->info->pos_low)
+			info->cfg.pos_low = info->pdata->info->pos_low;
+		if (info->pdata->info->pos_high)
+			info->cfg.pos_high = info->pdata->info->pos_high;
+		if (info->pdata->info->limit_low)
+			info->cfg.limit_low = info->pdata->info->limit_low;
+		if (info->pdata->info->limit_high)
+			info->cfg.limit_high = info->pdata->info->limit_high;
+		if (info->pdata->info->move_timeoutms)
+			info->cfg.move_timeoutms =
+					info->pdata->info->move_timeoutms;
+		if (info->pdata->info->focus_hyper_ratio)
+			info->cfg.focus_hyper_ratio =
+					info->pdata->info->focus_hyper_ratio;
+		if (info->pdata->info->focus_hyper_div)
+			info->cfg.focus_hyper_div =
+					info->pdata->info->focus_hyper_div;
+	}
+	/*
+	 * There is known to be many sh532u devices with no EPROM data.
+	 * Using default data is known to reduce the sh532u performance since
+	 * the defaults may no where be close to the correct values that
+	 * should be used.  However, we don't want to prevent the camera from
+	 * starting due to the lack of the EPROM data.
+	 * The following truth table shows the action to take at this point:
+	 * DFLT = the use_defaults flag (used after multiple attempts)
+	 * I2C = the I2C transactions to get the data.
+	 * DATA = the needed data either from the EPROM or board file.
+	 * DFLT   I2C   DATA   Action
+	 * --------------------------
+	 *  0     FAIL  FAIL   Exit with -EIO
+	 *  0     FAIL  PASS   Continue to calculations
+	 *  0     PASS  FAIL   Use defaults
+	 *  0     PASS  PASS   Continue to calculations
+	 *  1     FAIL  FAIL   Use defaults
+	 *  1     FAIL  PASS   Continue to calculations
+	 *  1     PASS  FAIL   Use defaults
+	 *  1     PASS  PASS   Continue to calculations
+	 */
+	/* err = DATA where FAIL = 1 */
+	if (!info->cfg.pos_low || info->cfg.pos_high <= info->cfg.pos_low ||
+		!info->cfg.limit_low ||
+		info->cfg.limit_high <= info->cfg.limit_low)
+		err = 1;
+	else
+		err = 0;
+	/* Exit with -EIO */
+	if (!use_defaults && ret && err) {
+		dev_err(&info->i2c_client->dev, "%s ERR\n", __func__);
+		return -EIO;
+	}
+
+	/* Use defaults */
+	if (err) {
+		info->cfg.pos_low = SH532U_POS_LOW_DEFAULT;
+		info->cfg.pos_high = SH532U_POS_HIGH_DEFAULT;
+		info->cfg.limit_low = SH532U_POS_LOW_DEFAULT;
+		info->cfg.limit_high = SH532U_POS_HIGH_DEFAULT;
+		dev_err(&info->i2c_client->dev, "%s ERR: ERPOM data is void!  "
+			"Focuser will use defaults that will cause "
+			"reduced functionality!\n", __func__);
+	}
+	if (info->cfg.pos_low < info->cfg.limit_low)
+		info->cfg.pos_low = info->cfg.limit_low;
+	if (info->cfg.pos_high > info->cfg.limit_high)
+		info->cfg.pos_high = info->cfg.limit_high;
+	dev_dbg(&info->i2c_client->dev, "%s pos_low=%d\n",
+		__func__, (int)info->cfg.pos_low);
+	dev_dbg(&info->i2c_client->dev, "%s pos_high=%d\n",
+		__func__, (int)info->cfg.pos_high);
+	dev_dbg(&info->i2c_client->dev, "%s limit_low=%d\n",
+		__func__, (int)info->cfg.limit_low);
+	dev_dbg(&info->i2c_client->dev, "%s limit_high=%d\n",
+		__func__, (int)info->cfg.limit_high);
+
+	sh532u_calibration_caps(info);
+	info->init_cal_flag = 1;
+	dev_dbg(&info->i2c_client->dev, "%s complete\n", __func__);
+	return 0;
+}
+
+	/* Write 1 byte data to the HVCA Drive IC by data type */
+static int sh532u_hvca_wr1(struct sh532u_info *info,
+			   u8 ep_type, u8 ep_data1, u8 ep_addr)
+{
+	u8 us_data;
+	int err = 0;
+
+	switch (ep_type & 0xF0) {
+	case DIRECT_MODE:
+		us_data = ep_data1;
+		break;
+
+	case INDIRECT_EEPROM:
+		err = sh532u_i2c_rd8(info,
+				     info->i2c_addr_rom,
+				     ep_data1,
+				     &us_data);
+		break;
+
+	case INDIRECT_HVCA:
+		err = sh532u_i2c_rd8(info, 0, ep_data1, &us_data);
+		break;
+
+	case MASK_AND:
+		err = sh532u_i2c_rd8(info, 0, ep_addr, &us_data);
+		us_data &= ep_data1;
+		break;
+
+	case MASK_OR:
+		err = sh532u_i2c_rd8(info, 0, ep_addr, &us_data);
+		us_data |= ep_data1;
+		break;
+
+	default:
+		err = -EINVAL;
+	}
+	if (!err)
+		err = sh532u_i2c_wr8(info, ep_addr, us_data);
+	return err;
+}
+
+	/* Write 2 byte data to the HVCA Drive IC by data type */
+static int sh532u_hvca_wr2(struct sh532u_info *info, u8 ep_type,
+				u8 ep_data1, u8 ep_data2, u8 ep_addr)
+{
+	u8 uc_data1;
+	u8 uc_data2;
+	u16 us_data;
+	int err = 0;
+
+	switch (ep_type & 0xF0) {
+	case DIRECT_MODE:
+		us_data = (((u16)ep_data1 << 8) & 0xFF00) |
+			((u16)ep_data2 & 0x00FF);
+		break;
+
+	case INDIRECT_EEPROM:
+		err = sh532u_i2c_rd8(info,
+				     info->i2c_addr_rom,
+				     ep_data1,
+				     &uc_data1);
+		err |= sh532u_i2c_rd8(info,
+				      info->i2c_addr_rom,
+				      ep_data2,
+				      &uc_data2);
+		us_data = (((u16)uc_data1 << 8) & 0xFF00) |
+				((u16)uc_data2 & 0x00FF);
+		break;
+
+	case INDIRECT_HVCA:
+		err = sh532u_i2c_rd8(info, 0, ep_data1, &uc_data1);
+		err |= sh532u_i2c_rd8(info, 0, ep_data2, &uc_data2);
+		us_data = (((u16)uc_data1 << 8) & 0xFF00) |
+				((u16)uc_data2 & 0x00FF);
+		break;
+
+	case MASK_AND:
+		err = sh532u_i2c_rd16(info, ep_addr, &us_data);
+		us_data &= ((((u16)ep_data1 << 8) & 0xFF00) |
+			    ((u16)ep_data2 & 0x00FF));
+		break;
+
+	case MASK_OR:
+		err = sh532u_i2c_rd16(info, ep_addr, &us_data);
+		us_data |= ((((u16)ep_data1 << 8) & 0xFF00) |
+			    ((u16)ep_data2 & 0x00FF));
+		break;
+
+	default:
+		err = -EINVAL;
+	}
+	if (!err)
+		err = sh532u_i2c_wr16(info, ep_addr, us_data);
+	return err;
+}
+
+static int sh532u_dev_init(struct sh532u_info *info)
+{
+	int eeprom_reg;
+	unsigned eeprom_data = 0;
+	u8 ep_addr;
+	u8 ep_type;
+	u8 ep_data1;
+	u8 ep_data2;
+	int err;
+	int ret = 0;
+
+	err = sh532u_i2c_rd8(info, 0, SWTCH_211, &ep_data1);
+	if (err)
+		return err; /* exit if unable to communicate with device */
+
+	ep_data2 = ep_data1;
+	err |= sh532u_i2c_rd8(info, 0, ANA1_211, &ep_data1);
+	ep_data2 |= ep_data1;
+	if (!err && ep_data2)
+		return 0; /* Already initialized */
+
+	info->sts = NVC_FOCUS_STS_INITIALIZING;
+	for (eeprom_reg = 0x30; eeprom_reg <= 0x013C; eeprom_reg += 4) {
+		if (eeprom_reg > 0xFF) {
+			/* use hardcoded data instead */
+			eeprom_data = sh532u_a2buf[(eeprom_reg & 0xFF) / 4];
+		} else {
+			err = (sh532u_i2c_rd32(info,
+					    info->i2c_addr_rom,
+					    eeprom_reg & 0xFF,
+					    &eeprom_data));
+			if (err) {
+				ret |= err;
+				continue;
+			}
+		}
+
+		/* HVCA Address to write eeprom Data1,Data2 by the Data type */
+		ep_addr = (u8)(eeprom_data & 0x000000ff);
+		ep_type = (u8)((eeprom_data & 0x0000ff00) >> 8);
+		ep_data1 = (u8)((eeprom_data & 0x00ff0000) >> 16);
+		ep_data2 = (u8)((eeprom_data & 0xff000000) >> 24);
+		if (ep_addr == 0xFF)
+			break;
+
+		if (ep_addr == 0xDD) {
+			mdelay((unsigned int)((ep_data1 << 8) | ep_data2));
+		} else {
+			if ((ep_type & 0x0F) == DATA_1BYTE) {
+				err = sh532u_hvca_wr1(info,
+						      ep_type,
+						      ep_data1,
+						      ep_addr);
+			} else {
+				err = sh532u_hvca_wr2(info,
+						      ep_type,
+						      ep_data1,
+						      ep_data2,
+						      ep_addr);
+			}
+		}
+		ret |= err;
+	}
+
+	err = ret;
+	if (err)
+		dev_err(&info->i2c_client->dev, "%s programming err=%d\n",
+			__func__, err);
+	err |= sh532u_calibration(info, false);
+	info->sts = NVC_FOCUS_STS_LENS_SETTLED;
+	return err;
+}
+
+static int sh532u_pos_abs_wr(struct sh532u_info *info, s16 tar_pos)
+{
+	s16 cur_pos;
+	s16 move_step;
+	u16 move_distance;
+	int err;
+
+	sh532u_pm_dev_wr(info, NVC_PWR_ON);
+	err = sh532u_dev_init(info);
+	if (err)
+		return err;
+
+	/* Read Current Position */
+	err = sh532u_abs_pos_rd(info, &cur_pos);
+	if (err)
+		return err;
+
+	dev_dbg(&info->i2c_client->dev, "%s cur_pos=%d tar_pos=%d\n",
+		__func__, (int)cur_pos, (int)tar_pos);
+	info->sts = NVC_FOCUS_STS_WAIT_FOR_MOVE_END;
+	/* Check move distance to Target Position */
+	move_distance = abs((int)cur_pos - (int)tar_pos);
+	/* if move distance is shorter than MS1Z12(=Step width) */
+	if (move_distance <= STMV_SIZE) {
+		err = sh532u_i2c_wr8(info, MSSET_211,
+				     (INI_MSSET_211 | 0x01));
+		err |= sh532u_i2c_wr16(info, MS1Z22_211H, tar_pos);
+	} else {
+		if (cur_pos < tar_pos)
+			move_step = STMV_SIZE;
+		else
+			move_step = -STMV_SIZE;
+		/* Set StepMove Target Positon */
+		err = sh532u_i2c_wr16(info, MS1Z12_211H, move_step);
+		err |= sh532u_i2c_wr16(info, STMVENDH_211, tar_pos);
+		/* Start StepMove */
+		err |= sh532u_i2c_wr8(info, STMVEN_211,
+				      (STMCHTG_ON |
+				       STMSV_ON |
+				       STMLFF_OFF |
+				       STMVEN_ON));
+	}
+	dev_dbg(&info->i2c_client->dev, "%s: position %d\n", __func__, tar_pos);
+	return err;
+}
+
+static int sh532u_move_wait(struct sh532u_info *info)
+{
+	u16 us_smv_fin;
+	u8 moveTime;
+	u8 ucParMod;
+	u8 tmp;
+	int err;
+
+	moveTime = 0;
+	do {
+		mdelay(1);
+		err = sh532u_i2c_rd8(info, 0, STMVEN_211, &ucParMod);
+		err |= sh532u_i2c_rd16(info, RZ_211H, &us_smv_fin);
+		if (err)
+			return err;
+
+		/* StepMove Error Handling, Unexpected Position */
+		if ((us_smv_fin == 0x7FFF) || (us_smv_fin == 0x8001)) {
+			/* Stop StepMove Operation */
+			err = sh532u_i2c_wr8(info, STMVEN_211,
+					     ucParMod & 0xFE);
+			if (err)
+				return err;
+		}
+
+		moveTime++;
+		/* Wait StepMove operation end */
+	} while ((ucParMod & STMVEN_ON) && (moveTime < 50));
+
+	moveTime = 0;
+	if ((ucParMod & 0x08) == STMCHTG_ON) {
+		mdelay(5);
+		do {
+			mdelay(1);
+			moveTime++;
+			err = sh532u_i2c_rd8(info, 0, MSSET_211, &tmp);
+			if (err)
+				return err;
+
+		} while ((tmp & CHTGST_ON) && (moveTime < 15));
+	}
+	return err;
+}
+
+static int sh532u_move_pulse(struct sh532u_info *info, s16 position)
+{
+	int err;
+
+	err = sh532u_pos_abs_wr(info, position);
+	err |= sh532u_move_wait(info);
+	return err;
+}
+
+static int sh532u_hvca_pos_init(struct sh532u_info *info)
+{
+	s16 limit_bottom;
+	s16 limit_top;
+	int err;
+
+	limit_bottom = (((int)info->cfg.limit_low * 5) >> 3) & 0xFFC0;
+	if (limit_bottom < info->cfg.limit_low)
+		limit_bottom = info->cfg.limit_low;
+	limit_top = (((int)info->cfg.limit_high * 5) >> 3) & 0xFFC0;
+	if (limit_top > info->cfg.limit_high)
+		limit_top = info->cfg.limit_high;
+	err = sh532u_move_pulse(info, limit_bottom);
+	err |= sh532u_move_pulse(info, limit_top);
+	err |= sh532u_move_pulse(info, info->cfg.pos_high);
+	return err;
+}
+
+static int sh532u_pos_rel_wr(struct sh532u_info *info, s32 position)
+{
+	s16 abs_pos;
+
+	if (info->cap.position_translate) {
+		if (position > info->cap.actuator_range) {
+			dev_err(&info->i2c_client->dev, "%s invalid position %d\n",
+				__func__, position);
+			return -EINVAL;
+		}
+	}
+	abs_pos = sh532u_rel2abs(info, position);
+
+	info->pos_rel = position;
+	info->pos_abs = abs_pos;
+	info->pos_time_wr = jiffies;
+	return sh532u_pos_abs_wr(info, abs_pos);
+}
+
+static void sh532u_get_focuser_capabilities(struct sh532u_info *info)
+{
+	memset(&info->config, 0, sizeof(info->config));
+
+	info->config.focal_length = info->nvc.focal_length;
+	info->config.fnumber = info->nvc.fnumber;
+	info->config.max_aperture = info->nvc.fnumber;
+	info->config.range_ends_reversed = (SH532U_POS_SIGN_CHANGER == -1)
+								? 1 : 0;
+
+	info->config.settle_time = info->cap.settle_time;
+
+	/*
+	 * We do not use pos_working_low and pos_working_high
+	 * in the kernel driver.
+	 */
+	info->config.pos_working_low = AF_POS_INVALID_VALUE;
+	info->config.pos_working_high = AF_POS_INVALID_VALUE;
+
+	info->config.pos_actual_low = info->cfg.limit_high *
+							SH532U_POS_SIGN_CHANGER;
+	info->config.pos_actual_high = info->cfg.limit_low *
+							SH532U_POS_SIGN_CHANGER;
+	info->config.slew_rate = info->cap.slew_rate;
+	info->config.circle_of_confusion = -1;
+
+	/*
+	 * These need to be passed up once we have the EEPROM/OTP read
+	 * routines in teh kernel. These need to be passed up much earlier on.
+	 * Till we have these routines, we pass them up as part of the get call.
+	 */
+	info->config.num_focuser_sets = 1;
+	info->config.focuser_set[0].posture = 'S';
+	info->config.focuser_set[0].macro = info->cap.focus_macro;
+	info->config.focuser_set[0].hyper = info->cap.focus_hyper;
+	info->config.focuser_set[0].inf = info->cap.focus_infinity;
+	info->config.focuser_set[0].hysteresis = 0;
+	info->config.focuser_set[0].settle_time = info->cap.settle_time;
+	info->config.focuser_set[0].num_dist_pairs = 0;
+
+	dev_dbg(&info->i2c_client->dev, "%s: pos_actual_low %d pos_actual_high %d "
+		" settle_time %d\n", __func__, info->config.pos_actual_low,
+		info->config.pos_actual_high, info->cap.settle_time);
+
+}
+
+
+static int sh532u_set_focuser_capabilities(struct sh532u_info *info,
+					struct nvc_param *params)
+{
+	if (copy_from_user(&info->config, (const void __user *)params->p_value,
+		sizeof(struct nv_focuser_config))) {
+		dev_err(&info->i2c_client->dev, "%s Error: copy_from_user bytes %d\n",
+				__func__, sizeof(struct nv_focuser_config));
+		return -EFAULT;
+	}
+
+	/* info.config.focuser_set[0].posture, macro, hyper, infinity and
+	 * hysterisis can remain there only. We need only settle_time &
+	 * slew_rate for use here.
+	 */
+	info->cap.settle_time = info->config.focuser_set[0].settle_time;
+	info->config.slew_rate = info->config.slew_rate;
+
+	dev_dbg(&info->i2c_client->dev, "%s: copy_from_user bytes %d\n",
+			__func__,  sizeof(struct nv_focuser_config));
+	return 0;
+}
+
+
+static int sh532u_param_rd(struct sh532u_info *info, unsigned long arg)
+{
+	struct nvc_param params;
+	const void *data_ptr;
+	u32 data_size = 0;
+	s32 position;
+	int err;
+
+	if (copy_from_user(&params,
+			(const void __user *)arg,
+			sizeof(struct nvc_param))) {
+		dev_err(&info->i2c_client->dev, "%s %d copy_from_user err\n",
+			__func__, __LINE__);
+		return -EFAULT;
+	}
+
+	if (info->s_mode == NVC_SYNC_SLAVE)
+		info = info->s_info;
+	switch (params.param) {
+	case NVC_PARAM_LOCUS:
+		sh532u_pm_dev_wr(info, NVC_PWR_COMM);
+		err = sh532u_rel_pos_rd(info, &position);
+		if (err && !(info->pdata->cfg & NVC_CFG_NOERR))
+			return -EINVAL;
+
+		data_ptr = &position;
+		data_size = sizeof(position);
+		sh532u_pm_dev_wr(info, NVC_PWR_STDBY);
+		dev_dbg(&info->i2c_client->dev, "%s LOCUS: %d\n",
+			__func__, position);
+		break;
+
+	case NVC_PARAM_FOCAL_LEN:
+		data_ptr = &info->nvc.focal_length;
+		data_size = sizeof(info->nvc.focal_length);
+		dev_dbg(&info->i2c_client->dev, "%s FOCAL_LEN: %x\n",
+			__func__, info->nvc.focal_length);
+		break;
+
+	case NVC_PARAM_MAX_APERTURE:
+		data_ptr = &info->nvc.max_aperature;
+		data_size = sizeof(info->nvc.max_aperature);
+		dev_dbg(&info->i2c_client->dev, "%s MAX_APERTURE: %x\n",
+			__func__, info->nvc.max_aperature);
+		break;
+
+	case NVC_PARAM_FNUMBER:
+		data_ptr = &info->nvc.fnumber;
+		data_size = sizeof(info->nvc.fnumber);
+		dev_dbg(&info->i2c_client->dev, "%s FNUMBER: %u\n",
+			__func__, info->nvc.fnumber);
+		break;
+
+	case NVC_PARAM_CAPS:
+		sh532u_pm_dev_wr(info, NVC_PWR_COMM);
+		err = sh532u_calibration(info, true);
+		sh532u_pm_dev_wr(info, NVC_PWR_STDBY);
+		if (err)
+			return -EIO;
+		dev_dbg(&info->i2c_client->dev, "%s: NVC_PARAM_CAPS: params.param %d "
+				"params.sizeofvalue %d\n",
+				__func__, params.param, params.sizeofvalue);
+
+		sh532u_get_focuser_capabilities(info);
+
+		data_ptr = &info->config;
+		data_size = sizeof(info->config);
+		break;
+
+	case NVC_PARAM_STS:
+		data_ptr = &info->sts;
+		data_size = sizeof(info->sts);
+		dev_dbg(&info->i2c_client->dev, "%s STS: %d\n",
+			__func__, info->sts);
+		break;
+
+	case NVC_PARAM_STEREO:
+		data_ptr = &info->s_mode;
+		data_size = sizeof(info->s_mode);
+		dev_dbg(&info->i2c_client->dev, "%s STEREO: %d\n",
+			__func__, info->s_mode);
+		break;
+
+	default:
+		dev_dbg(&info->i2c_client->dev,
+			"%s unsupported parameter: %d\n",
+			__func__, params.param);
+		return -EINVAL;
+	}
+
+	if (params.sizeofvalue < data_size) {
+		dev_err(&info->i2c_client->dev,
+			"%s data size mismatch %d != %d Param: %d\n",
+			__func__, params.sizeofvalue, data_size, params.param);
+		return -EINVAL;
+	}
+
+	if (copy_to_user((void __user *)params.p_value,
+			 data_ptr,
+			 data_size)) {
+		dev_err(&info->i2c_client->dev,
+			"%s copy_to_user err line %d\n", __func__, __LINE__);
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int sh532u_param_wr_s(struct sh532u_info *info,
+			     struct nvc_param *params,
+			     u32 u32val)
+{
+	u8 u8val;
+	int err;
+
+	u8val = (u8)u32val;
+	switch (params->param) {
+	case NVC_PARAM_LOCUS:
+		dev_dbg(&info->i2c_client->dev, "%s LOCUS: %d\n",
+			__func__, (s32) u32val);
+		err = sh532u_pos_rel_wr(info, (s32) u32val);
+		return err;
+
+	case NVC_PARAM_RESET:
+		err = sh532u_reset(info, u32val);
+		dev_dbg(&info->i2c_client->dev, "%s RESET: %d\n",
+			__func__, err);
+		return err;
+
+	case NVC_PARAM_SELF_TEST:
+		err = sh532u_hvca_pos_init(info);
+		dev_dbg(&info->i2c_client->dev, "%s SELF_TEST: %d\n",
+			__func__, err);
+		return err;
+
+	case NVC_PARAM_CAPS:
+		dev_dbg(&info->i2c_client->dev, "%s CAPS. Error. sh532u_param_wr "
+				"should be called instead\n", __func__);
+		return -EFAULT;
+
+	default:
+		dev_dbg(&info->i2c_client->dev,
+			"%s unsupported parameter: %d\n",
+			__func__, params->param);
+		return -EINVAL;
+	}
+}
+
+static int sh532u_param_wr(struct sh532u_info *info, unsigned long arg)
+{
+	struct nvc_param params;
+	u8 u8val;
+	u32 u32val;
+	int err = 0;
+
+	if (copy_from_user(&params, (const void __user *)arg,
+			   sizeof(struct nvc_param))) {
+		dev_err(&info->i2c_client->dev,
+			"%s copy_from_user err line %d\n", __func__, __LINE__);
+		return -EFAULT;
+	}
+
+	if (copy_from_user(&u32val, (const void __user *)params.p_value,
+			   sizeof(u32val))) {
+		dev_err(&info->i2c_client->dev, "%s %d copy_from_user err\n",
+			__func__, __LINE__);
+		return -EFAULT;
+	}
+
+	u8val = (u8)u32val;
+	/* parameters independent of sync mode */
+	switch (params.param) {
+	case NVC_PARAM_STEREO:
+		dev_dbg(&info->i2c_client->dev, "%s STEREO: %d\n",
+			__func__, u8val);
+		if (u8val == info->s_mode)
+			return 0;
+
+		switch (u8val) {
+		case NVC_SYNC_OFF:
+			info->s_mode = u8val;
+			sh532u_gpio_wr(info, SH532U_GPIO_I2CMUX, 0);
+			if (info->s_info != NULL) {
+				info->s_info->s_mode = u8val;
+				sh532u_pm_wr(info->s_info, NVC_PWR_OFF);
+			}
+			break;
+
+		case NVC_SYNC_MASTER:
+			info->s_mode = u8val;
+			sh532u_gpio_wr(info, SH532U_GPIO_I2CMUX, 0);
+			if (info->s_info != NULL)
+				info->s_info->s_mode = u8val;
+			break;
+
+		case NVC_SYNC_SLAVE:
+			if (info->s_info != NULL) {
+				/* default slave lens position */
+				err = sh532u_pos_rel_wr(info->s_info,
+					     info->s_info->cap.focus_infinity);
+				if (!err) {
+					info->s_mode = u8val;
+					info->s_info->s_mode = u8val;
+					sh532u_gpio_wr(info,
+						       SH532U_GPIO_I2CMUX, 0);
+				} else {
+					if (info->s_mode != NVC_SYNC_STEREO)
+						sh532u_pm_wr(info->s_info,
+							     NVC_PWR_OFF);
+					err = -EIO;
+				}
+			} else {
+				err = -EINVAL;
+			}
+			break;
+
+		case NVC_SYNC_STEREO:
+			if (info->s_info != NULL) {
+				/* sync power */
+				info->s_info->pwr_api = info->pwr_api;
+				/* move slave lens to master position */
+				err = sh532u_pos_rel_wr(info->s_info,
+							 info->pos_rel);
+				if (!err) {
+					info->s_mode = u8val;
+					info->s_info->s_mode = u8val;
+					sh532u_gpio_wr(info,
+						       SH532U_GPIO_I2CMUX, 1);
+				} else {
+					if (info->s_mode != NVC_SYNC_SLAVE)
+						sh532u_pm_wr(info->s_info,
+							     NVC_PWR_OFF);
+					err = -EIO;
+				}
+			} else {
+				err = -EINVAL;
+			}
+			break;
+
+		default:
+			err = -EINVAL;
+		}
+		if (info->pdata->cfg & NVC_CFG_NOERR)
+			return 0;
+
+		return err;
+
+	case NVC_PARAM_CAPS:
+		if (sh532u_set_focuser_capabilities(info, &params)) {
+			dev_err(&info->i2c_client->dev, "%s: Error: copy_from_user bytes %d\n",
+					__func__, params.sizeofvalue);
+			return -EFAULT;
+		}
+		return 0;
+
+	default:
+	/* parameters dependent on sync mode */
+		switch (info->s_mode) {
+		case NVC_SYNC_OFF:
+		case NVC_SYNC_MASTER:
+			return sh532u_param_wr_s(info, &params, u32val);
+
+		case NVC_SYNC_SLAVE:
+			return sh532u_param_wr_s(info->s_info, &params,
+						 u32val);
+
+		case NVC_SYNC_STEREO:
+			err = sh532u_param_wr_s(info, &params, u32val);
+			if (!(info->pdata->cfg & NVC_CFG_SYNC_I2C_MUX))
+				err |= sh532u_param_wr_s(info->s_info,
+							 &params,
+							 u32val);
+			return err;
+
+		default:
+			dev_err(&info->i2c_client->dev, "%s %d internal err\n",
+				__func__, __LINE__);
+			return -EINVAL;
+		}
+	}
+}
+
+static long sh532u_ioctl(struct file *file,
+			 unsigned int cmd,
+			 unsigned long arg)
+{
+	struct sh532u_info *info = file->private_data;
+	int pwr;
+	int err;
+
+	switch (cmd) {
+	case NVC_IOCTL_PARAM_WR:
+		err = sh532u_param_wr(info, arg);
+		return err;
+
+	case NVC_IOCTL_PARAM_RD:
+		err = sh532u_param_rd(info, arg);
+		return err;
+
+	case NVC_IOCTL_PWR_WR:
+		/* This is a Guaranteed Level of Service (GLOS) call */
+		pwr = (int)arg * 2;
+		dev_dbg(&info->i2c_client->dev, "%s PWR_WR: %d\n",
+			__func__, pwr);
+		err = sh532u_pm_api_wr(info, pwr);
+		return err;
+
+	case NVC_IOCTL_PWR_RD:
+		if (info->s_mode == NVC_SYNC_SLAVE)
+			pwr = info->s_info->pwr_api / 2;
+		else
+			pwr = info->pwr_api / 2;
+		dev_dbg(&info->i2c_client->dev, "%s PWR_RD: %d\n",
+			__func__, pwr);
+		if (copy_to_user((void __user *)arg, (const void *)&pwr,
+				 sizeof(pwr))) {
+			dev_err(&info->i2c_client->dev,
+					"%s copy_to_user err line %d\n",
+					__func__, __LINE__);
+			return -EFAULT;
+		}
+
+		return 0;
+
+	default:
+		dev_dbg(&info->i2c_client->dev, "%s unsupported ioctl: %x\n",
+			__func__, cmd);
+	}
+
+	return -EINVAL;
+}
+
+static void sh532u_sdata_init(struct sh532u_info *info)
+{
+	/* set defaults */
+	memcpy(&info->cfg, &sh532u_default_info, sizeof(info->cfg));
+	memcpy(&info->nvc, &sh532u_default_nvc, sizeof(info->nvc));
+	memcpy(&info->cap, &sh532u_default_cap, sizeof(info->cap));
+	if (info->pdata->i2c_addr_rom)
+		info->i2c_addr_rom = info->pdata->i2c_addr_rom;
+	else
+		info->i2c_addr_rom = sh532u_default_pdata.i2c_addr_rom;
+	/* set overrides if any */
+	if (info->pdata->nvc) {
+		if (info->pdata->nvc->fnumber)
+			info->nvc.fnumber = info->pdata->nvc->fnumber;
+		if (info->pdata->nvc->focal_length)
+			info->nvc.focal_length =
+					info->pdata->nvc->focal_length;
+		if (info->pdata->nvc->max_aperature)
+			info->nvc.max_aperature =
+					info->pdata->nvc->max_aperature;
+	}
+	if (info->pdata->cap) {
+		if (info->pdata->cap->actuator_range)
+			info->cap.actuator_range =
+					info->pdata->cap->actuator_range;
+		if (info->pdata->cap->settle_time)
+			info->cap.settle_time = info->pdata->cap->settle_time;
+		if (info->pdata->cap->focus_macro)
+			info->cap.focus_macro = info->pdata->cap->focus_macro;
+		if (info->pdata->cap->focus_hyper)
+			info->cap.focus_hyper = info->pdata->cap->focus_hyper;
+		if (info->pdata->cap->focus_infinity)
+			info->cap.focus_infinity =
+					info->pdata->cap->focus_infinity;
+	}
+}
+
+static int sh532u_sync_en(unsigned num, unsigned sync)
+{
+	struct sh532u_info *master = NULL;
+	struct sh532u_info *slave = NULL;
+	struct sh532u_info *pos = NULL;
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(pos, &sh532u_info_list, list) {
+		if (pos->pdata->num == num) {
+			master = pos;
+			break;
+		}
+	}
+	pos = NULL;
+	list_for_each_entry_rcu(pos, &sh532u_info_list, list) {
+		if (pos->pdata->num == sync) {
+			slave = pos;
+			break;
+		}
+	}
+	rcu_read_unlock();
+	if (master != NULL)
+		master->s_info = NULL;
+	if (slave != NULL)
+		slave->s_info = NULL;
+	if (!sync)
+		return 0; /* no err if sync disabled */
+
+	if (num == sync)
+		return -EINVAL; /* err if sync instance is itself */
+
+	if ((master != NULL) && (slave != NULL)) {
+		master->s_info = slave;
+		slave->s_info = master;
+	}
+	return 0;
+}
+
+static int sh532u_sync_dis(struct sh532u_info *info)
+{
+	if (info->s_info != NULL) {
+		info->s_info->s_mode = 0;
+		info->s_info->s_info = NULL;
+		info->s_mode = 0;
+		info->s_info = NULL;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int sh532u_open(struct inode *inode, struct file *file)
+{
+	struct sh532u_info *info = NULL;
+	struct sh532u_info *pos = NULL;
+	int err;
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(pos, &sh532u_info_list, list) {
+		if (pos->miscdev.minor == iminor(inode)) {
+			info = pos;
+			break;
+		}
+	}
+	rcu_read_unlock();
+	if (!info)
+		return -ENODEV;
+
+	err = sh532u_sync_en(info->pdata->num, info->pdata->sync);
+	if (err == -EINVAL)
+		dev_err(&info->i2c_client->dev,
+			"%s err: invalid num (%u) and sync (%u) instance\n",
+			__func__, info->pdata->num, info->pdata->sync);
+	if (atomic_xchg(&info->in_use, 1))
+		return -EBUSY;
+
+	if (info->s_info != NULL) {
+		if (atomic_xchg(&info->s_info->in_use, 1))
+			return -EBUSY;
+	}
+
+	file->private_data = info;
+	dev_dbg(&info->i2c_client->dev, "%s\n", __func__);
+	sh532u_pos_rel_wr(info, info->cap.focus_infinity);
+	sh532u_pm_wr_s(info, NVC_PWR_OFF);
+	return 0;
+}
+
+static int sh532u_release(struct inode *inode, struct file *file)
+{
+	struct sh532u_info *info = file->private_data;
+
+	dev_dbg(&info->i2c_client->dev, "%s\n", __func__);
+	sh532u_pm_wr_s(info, NVC_PWR_OFF);
+	file->private_data = NULL;
+	WARN_ON(!atomic_xchg(&info->in_use, 0));
+	if (info->s_info != NULL)
+		WARN_ON(!atomic_xchg(&info->s_info->in_use, 0));
+	sh532u_sync_dis(info);
+	return 0;
+}
+
+static const struct file_operations sh532u_fileops = {
+	.owner = THIS_MODULE,
+	.open = sh532u_open,
+	.unlocked_ioctl = sh532u_ioctl,
+	.release = sh532u_release,
+};
+
+static void sh532u_del(struct sh532u_info *info)
+{
+	sh532u_pm_exit(info);
+	if ((info->s_mode == NVC_SYNC_SLAVE) ||
+					     (info->s_mode == NVC_SYNC_STEREO))
+		sh532u_pm_exit(info->s_info);
+	sh532u_sync_dis(info);
+	spin_lock(&sh532u_spinlock);
+	list_del_rcu(&info->list);
+	spin_unlock(&sh532u_spinlock);
+	synchronize_rcu();
+}
+
+static int sh532u_remove(struct i2c_client *client)
+{
+	struct sh532u_info *info = i2c_get_clientdata(client);
+
+	dev_dbg(&info->i2c_client->dev, "%s\n", __func__);
+	misc_deregister(&info->miscdev);
+	sh532u_del(info);
+	return 0;
+}
+
+static int sh532u_probe(
+	struct i2c_client *client,
+	const struct i2c_device_id *id)
+{
+	struct sh532u_info *info;
+	char dname[16];
+	int err;
+
+	info = devm_kzalloc(&client->dev, sizeof(*info), GFP_KERNEL);
+	if (info == NULL) {
+		dev_err(&client->dev, "%s: kzalloc error\n", __func__);
+		return -ENOMEM;
+	}
+
+	info->i2c_client = client;
+	if (client->dev.platform_data) {
+		info->pdata = client->dev.platform_data;
+	} else {
+		info->pdata = &sh532u_default_pdata;
+		dev_dbg(&client->dev,
+			"%s No platform data.  Using defaults.\n", __func__);
+	}
+	i2c_set_clientdata(client, info);
+	INIT_LIST_HEAD(&info->list);
+	spin_lock(&sh532u_spinlock);
+	list_add_rcu(&info->list, &sh532u_info_list);
+	spin_unlock(&sh532u_spinlock);
+	sh532u_pm_init(info);
+	sh532u_sdata_init(info);
+	if (info->pdata->cfg & (NVC_CFG_NODEV | NVC_CFG_BOOT_INIT)) {
+		err = sh532u_dev_id(info);
+		if (err < 0) {
+			if (info->pdata->cfg & NVC_CFG_NODEV) {
+				sh532u_del(info);
+				return -ENODEV;
+			} else {
+				dev_err(&client->dev, "%s dev %x not found\n",
+					__func__, SH532U_ID);
+			}
+		} else {
+			dev_dbg(&client->dev, "%s device found\n", __func__);
+			sh532u_pm_dev_wr(info, NVC_PWR_ON);
+			sh532u_calibration(info, false);
+			if (info->pdata->cfg & NVC_CFG_BOOT_INIT)
+				/* initial move causes full initialization */
+				sh532u_pos_rel_wr(info,
+						  info->cap.focus_infinity);
+		}
+		sh532u_pm_dev_wr(info, NVC_PWR_OFF);
+	}
+
+	if (info->pdata->dev_name != 0)
+		strcpy(dname, info->pdata->dev_name);
+	else
+		strcpy(dname, "sh532u");
+	if (info->pdata->num)
+		snprintf(dname, sizeof(dname), "%s.%u",
+			 dname, info->pdata->num);
+	info->miscdev.name = dname;
+	info->miscdev.fops = &sh532u_fileops;
+	info->miscdev.minor = MISC_DYNAMIC_MINOR;
+	if (misc_register(&info->miscdev)) {
+		dev_err(&client->dev, "%s unable to register misc device %s\n",
+			__func__, dname);
+		sh532u_del(info);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static const struct i2c_device_id sh532u_id[] = {
+	{ "sh532u", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, sh532u_id);
+
+static struct i2c_driver sh532u_i2c_driver = {
+	.driver = {
+		.name = "sh532u",
+		.owner = THIS_MODULE,
+	},
+	.id_table = sh532u_id,
+	.probe = sh532u_probe,
+	.remove = sh532u_remove,
+};
+
+static int __init sh532u_init(void)
+{
+	return i2c_add_driver(&sh532u_i2c_driver);
+}
+
+static void __exit sh532u_exit(void)
+{
+	i2c_del_driver(&sh532u_i2c_driver);
+}
+
+module_init(sh532u_init);
+module_exit(sh532u_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/tegra/soc380.c b/drivers/media/video/tegra/soc380.c
new file mode 100644
index 000000000000..e0022166fff6
--- /dev/null
+++ b/drivers/media/video/tegra/soc380.c
@@ -0,0 +1,474 @@
+/*
+ * soc380.c - soc380 sensor driver
+ *
+ * Copyright (c) 2011, NVIDIA, All Rights Reserved.
+ *
+ * Contributors:
+ *      Abhinav Sinha <absinha@nvidia.com>
+ *
+ * Leverage OV2710.c
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ */
+
+/**
+ * SetMode Sequence for 640x480. Phase 0. Sensor Dependent.
+ * This sequence should put sensor in streaming mode for 640x480
+ * This is usually given by the FAE or the sensor vendor.
+ */
+
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <media/soc380.h>
+
+struct soc380_reg {
+	u16 addr;
+	u16 val;
+};
+
+struct soc380_info {
+	int mode;
+	struct i2c_client *i2c_client;
+	struct soc380_platform_data *pdata;
+};
+
+#define SOC380_TABLE_WAIT_MS 0
+#define SOC380_TABLE_END 1
+#define SOC380_MAX_RETRIES 3
+
+static struct soc380_reg mode_640x480[] = {
+	{0x001A, 0x0011},
+
+	{SOC380_TABLE_WAIT_MS, 1},
+
+	{0x001A, 0x0010},
+
+	{SOC380_TABLE_WAIT_MS, 1},
+
+	{0x0018, 0x4028},
+	{0x001A, 0x0210},
+	{0x0010, 0x021c},
+	{0x0012, 0x0000},
+	{0x0014, 0x244B},
+
+	{SOC380_TABLE_WAIT_MS, 10},
+
+	{0x0014, 0x304B},
+
+	{SOC380_TABLE_WAIT_MS, 50},
+
+	{0x0014, 0xB04A},
+
+	{0x098C, 0x2703},
+	{0x0990, 0x0280},
+	{0x098C, 0x2705},
+	{0x0990, 0x01E0},
+	{0x098C, 0x2707},
+	{0x0990, 0x0280},
+	{0x098C, 0x2709},
+	{0x0990, 0x01E0},
+	{0x098C, 0x270D},
+	{0x0990, 0x0000},
+	{0x098C, 0x270F},
+	{0x0990, 0x0000},
+	{0x098C, 0x2711},
+	{0x0990, 0x01E7},
+	{0x098C, 0x2713},
+	{0x0990, 0x0287},
+	{0x098C, 0x2715},
+	{0x0990, 0x0001},
+	{0x098C, 0x2717},
+	{0x0990, 0x0026},
+	{0x098C, 0x2719},
+	{0x0990, 0x001A},
+	{0x098C, 0x271B},
+	{0x0990, 0x006B},
+	{0x098C, 0x271D},
+	{0x0990, 0x006B},
+	{0x098C, 0x271F},
+	{0x0990, 0x022A},
+	{0x098C, 0x2721},
+	{0x0990, 0x034A},
+	{0x098C, 0x2723},
+	{0x0990, 0x0000},
+	{0x098C, 0x2725},
+	{0x0990, 0x0000},
+	{0x098C, 0x2727},
+	{0x0990, 0x01E7},
+	{0x098C, 0x2729},
+	{0x0990, 0x0287},
+	{0x098C, 0x272B},
+	{0x0990, 0x0001},
+	{0x098C, 0x272D},
+	{0x0990, 0x0026},
+	{0x098C, 0x272F},
+	{0x0990, 0x001A},
+	{0x098C, 0x2731},
+	{0x0990, 0x006B},
+	{0x098C, 0x2733},
+	{0x0990, 0x006B},
+	{0x098C, 0x2735},
+	{0x0990, 0x022A},
+	{0x098C, 0x2737},
+	{0x0990, 0x034A},
+	{0x098C, 0x2739},
+	{0x0990, 0x0000},
+	{0x098C, 0x273B},
+	{0x0990, 0x027F},
+	{0x098C, 0x273D},
+	{0x0990, 0x0000},
+	{0x098C, 0x273F},
+	{0x0990, 0x01DF},
+	{0x098C, 0x2747},
+	{0x0990, 0x0000},
+	{0x098C, 0x2749},
+	{0x0990, 0x027F},
+	{0x098C, 0x274B},
+	{0x0990, 0x0000},
+	{0x098C, 0x274D},
+	{0x0990, 0x01DF},
+	{0x098C, 0x222D},
+	{0x0990, 0x008B},
+	{0x098C, 0xA408},
+	{0x0990, 0x0021},
+	{0x098C, 0xA409},
+	{0x0990, 0x0023},
+	{0x098C, 0xA40A},
+	{0x0990, 0x0028},
+	{0x098C, 0xA40B},
+	{0x0990, 0x002A},
+	{0x098C, 0x2411},
+	{0x0990, 0x008B},
+	{0x098C, 0x2413},
+	{0x0990, 0x00A6},
+	{0x098C, 0x2415},
+	{0x0990, 0x008B},
+	{0x098C, 0x2417},
+	{0x0990, 0x00A6},
+	{0x098C, 0xA404},
+	{0x0990, 0x0010},
+	{0x098C, 0xA40D},
+	{0x0990, 0x0002},
+	{0x098C, 0xA40E},
+	{0x0990, 0x0003},
+	{0x098C, 0xA410},
+	{0x0990, 0x000A},
+	{0x098C, 0xA215},
+	{0x0990, 0x0003},
+	{0x098C, 0xA20C},
+	{0x0990, 0x0003},
+
+	{0x098C, 0xA103},
+	{0x0990, 0x0006},
+	{SOC380_TABLE_WAIT_MS, 100},
+
+	{0x098C, 0xA103},
+	{0x0990, 0x0005},
+	{SOC380_TABLE_WAIT_MS, 50},
+
+	{SOC380_TABLE_END, 0x0000}
+};
+
+enum {
+	SOC380_MODE_680x480,
+};
+
+static struct soc380_reg *mode_table[] = {
+	[SOC380_MODE_680x480] = mode_640x480,
+};
+
+static int soc380_read_reg(struct i2c_client *client, u16 addr, u16 *val)
+{
+	int err;
+	struct i2c_msg msg[2];
+	unsigned char data[4];
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 2;
+	msg[0].buf = data;
+
+	/* high byte goes out first */
+	data[0] = (u8) (addr >> 8);
+	data[1] = (u8) (addr & 0xff);
+
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 2;
+	msg[1].buf = data + 2;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+
+	if (err != 2)
+		return -EINVAL;
+
+	*val = data[2] << 8 | data[3];
+
+	return 0;
+}
+
+static int soc380_write_reg(struct i2c_client *client, u16 addr, u16 val)
+{
+	int err;
+	struct i2c_msg msg;
+	unsigned char data[4];
+	int retry = 0;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	data[0] = (u8) (addr >> 8);
+	data[1] = (u8) (addr & 0xff);
+	data[2] = (u8) (val >> 8);
+	data[3] = (u8) (val & 0xff);
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 4;
+	msg.buf = data;
+
+	do {
+		err = i2c_transfer(client->adapter, &msg, 1);
+		if (err == 1)
+			return 0;
+		retry++;
+		pr_err("soc380: i2c transfer failed, retrying %x %x\n",
+		       addr, val);
+		msleep(3);
+	} while (retry <= SOC380_MAX_RETRIES);
+
+	return err;
+}
+
+static int soc380_write_table(struct i2c_client *client,
+			      const struct soc380_reg table[],
+			      const struct soc380_reg override_list[],
+			      int num_override_regs)
+{
+	int err;
+	const struct soc380_reg *next;
+	int i;
+	u16 val;
+
+	for (next = table; next->addr != SOC380_TABLE_END; next++) {
+		if (next->addr == SOC380_TABLE_WAIT_MS) {
+			msleep(next->val);
+			continue;
+		}
+
+		val = next->val;
+
+		/* When an override list is passed in, replace the reg */
+		/* value to write if the reg is in the list            */
+		if (override_list) {
+			for (i = 0; i < num_override_regs; i++) {
+				if (next->addr == override_list[i].addr) {
+					val = override_list[i].val;
+					break;
+				}
+			}
+		}
+
+		err = soc380_write_reg(client, next->addr, val);
+		if (err)
+			return err;
+	}
+	return 0;
+}
+
+static int soc380_set_mode(struct soc380_info *info, struct soc380_mode *mode)
+{
+	int sensor_mode;
+	int err;
+
+	pr_info("%s: xres %u yres %u\n", __func__, mode->xres, mode->yres);
+	if (mode->xres == 640 && mode->yres == 480)
+		sensor_mode = SOC380_MODE_680x480;
+	else {
+		pr_err("%s: invalid resolution supplied to set mode %d %d\n",
+		       __func__, mode->xres, mode->yres);
+		return -EINVAL;
+	}
+
+	err = soc380_write_table(info->i2c_client, mode_table[sensor_mode],
+		NULL, 0);
+	if (err)
+		return err;
+
+	info->mode = sensor_mode;
+	return 0;
+}
+
+static int soc380_get_status(struct soc380_info *info,
+		struct soc380_status *dev_status)
+{
+	int err;
+
+	err = soc380_write_reg(info->i2c_client, 0x98C, dev_status->data);
+	if (err)
+		return err;
+
+	err = soc380_read_reg(info->i2c_client, 0x0990,
+		(u16 *) &dev_status->status);
+	if (err)
+		return err;
+
+	return err;
+}
+
+static long soc380_ioctl(struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	int err;
+	struct soc380_info *info = file->private_data;
+
+	switch (cmd) {
+	case SOC380_IOCTL_SET_MODE:
+	{
+		struct soc380_mode mode;
+		if (copy_from_user(&mode,
+				   (const void __user *)arg,
+				   sizeof(struct soc380_mode))) {
+			return -EFAULT;
+		}
+
+		return soc380_set_mode(info, &mode);
+	}
+	case SOC380_IOCTL_GET_STATUS:
+	{
+		struct soc380_status dev_status;
+		if (copy_from_user(&dev_status,
+				   (const void __user *)arg,
+				   sizeof(struct soc380_status))) {
+			return -EFAULT;
+		}
+
+		err = soc380_get_status(info, &dev_status);
+		if (err)
+			return err;
+		if (copy_to_user((void __user *)arg, &dev_status,
+				 sizeof(struct soc380_status))) {
+			return -EFAULT;
+		}
+		return 0;
+	}
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static struct soc380_info *info;
+
+static int soc380_open(struct inode *inode, struct file *file)
+{
+	struct soc380_status dev_status;
+	int err;
+
+	file->private_data = info;
+	if (info->pdata && info->pdata->power_on)
+		info->pdata->power_on();
+
+	dev_status.data = 0;
+	dev_status.status = 0;
+	err = soc380_get_status(info, &dev_status);
+	return err;
+}
+
+int soc380_release(struct inode *inode, struct file *file)
+{
+	if (info->pdata && info->pdata->power_off)
+		info->pdata->power_off();
+	file->private_data = NULL;
+	return 0;
+}
+
+static const struct file_operations soc380_fileops = {
+	.owner = THIS_MODULE,
+	.open = soc380_open,
+	.unlocked_ioctl = soc380_ioctl,
+	.release = soc380_release,
+};
+
+static struct miscdevice soc380_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "soc380",
+	.fops = &soc380_fileops,
+};
+
+static int soc380_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int err;
+
+	pr_info("soc380: probing sensor.\n");
+
+	info = kzalloc(sizeof(struct soc380_info), GFP_KERNEL);
+	if (!info) {
+		pr_err("soc380: Unable to allocate memory!\n");
+		return -ENOMEM;
+	}
+
+	err = misc_register(&soc380_device);
+	if (err) {
+		pr_err("soc380: Unable to register misc device!\n");
+		kfree(info);
+		return err;
+	}
+
+	info->pdata = client->dev.platform_data;
+	info->i2c_client = client;
+
+	i2c_set_clientdata(client, info);
+	return 0;
+}
+
+static int soc380_remove(struct i2c_client *client)
+{
+	struct soc380_info *info;
+	info = i2c_get_clientdata(client);
+	misc_deregister(&soc380_device);
+	kfree(info);
+	return 0;
+}
+
+static const struct i2c_device_id soc380_id[] = {
+	{ "soc380", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, soc380_id);
+
+static struct i2c_driver soc380_i2c_driver = {
+	.driver = {
+		.name = "soc380",
+		.owner = THIS_MODULE,
+	},
+	.probe = soc380_probe,
+	.remove = soc380_remove,
+	.id_table = soc380_id,
+};
+
+static int __init soc380_init(void)
+{
+	pr_info("soc380 sensor driver loading\n");
+	return i2c_add_driver(&soc380_i2c_driver);
+}
+
+static void __exit soc380_exit(void)
+{
+	i2c_del_driver(&soc380_i2c_driver);
+}
+
+module_init(soc380_init);
+module_exit(soc380_exit);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/video/tegra/ssl3250a.c b/drivers/media/video/tegra/ssl3250a.c
new file mode 100644
index 000000000000..ef2c80a883c5
--- /dev/null
+++ b/drivers/media/video/tegra/ssl3250a.c
@@ -0,0 +1,986 @@
+/*
+ * ssl3250a.c - ssl3250a flash/torch kernel driver
+ *
+ * Copyright (C) 2011 NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms 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
+ */
+
+/* Implementation
+ * --------------
+ * The board level details about the device need to be provided in the board
+ * file with the ssl3250a_platform_data structure.
+ * Standard among NVC kernel drivers in this structure is:
+ * .cfg = Use the NVC_CFG_ defines that are in nvc_torch.h.
+ *        Descriptions of the configuration options are with the defines.
+ *        This value is typically 0.
+ * .num = The number of the instance of the device.  This should start at 1 and
+ *        and increment for each device on the board.  This number will be
+ *        appended to the MISC driver name, Example: /dev/ssl3250a.1
+ * .sync = If there is a need to synchronize two devices, then this value is
+ *         the number of the device instance this device is allowed to sync to.
+ *         This is typically used for stereo applications.
+ * .dev_name = The MISC driver name the device registers as.  If not used,
+ *             then the part number of the device is used for the driver name.
+ *             If using the NVC user driver then use the name found in this
+ *             driver under _default_pdata.
+ *
+ * The following is specific to NVC kernel flash/torch drivers:
+ * .pinstate = a pointer to the nvc_torch_pin_state structure.  This
+ *             structure gives the details of which VI GPIO to use to trigger
+ *             the flash.  The mask tells which pin and the values is the
+ *             level.  For example, if VI GPIO pin 6 is used, then
+ *             .mask = 0x0040
+ *             .values = 0x0040
+ *             If VI GPIO pin 0 is used, then
+ *             .mask = 0x0001
+ *             .values = 0x0001
+ *             This is typically just one pin but there is some legacy
+ *             here that insinuates more than one pin can be used.
+ *             When the flash level is set, then the driver will return the
+ *             value in values.  When the flash level is off, the driver will
+ *             return 0 for the values to deassert the signal.
+ *             If a VI GPIO is not used, then the mask and values must be set
+ *             to 0.  The flash may then be triggered via I2C instead.
+ *             However, a VI GPIO is strongly encouraged since it allows
+ *             tighter timing with the picture taken as well as reduced power
+ *             by asserting the trigger signal for only when needed.
+ * .max_amp_torch = Is the maximum torch value allowed.  The value is 0 to
+ *                  _MAX_TORCH_LEVEL.  This is to allow a limit to the amount
+ *                  of amps used.  If left blank then _MAX_TORCH_LEVEL will be
+ *                  used.
+ * .max_amp_flash = Is the maximum flash value allowed.  The value is 0 to
+ *                  _MAX_FLASH_LEVEL.  This is to allow a limit to the amount
+ *                  of amps used.  If left blank then _MAX_FLASH_LEVEL will be
+ *                  used.
+ *
+ * The following is specific to only this NVC kernel flash/torch driver:
+ * .gpio_act = Is the GPIO needed to control the ACT signal.  If tied high,
+ *             then this can be left blank.
+ *
+ * Power Requirements
+ * The board power file must contain the following labels for the power
+ * regulator(s) of this device:
+ * "vdd_i2c" = the power regulator for the I2C power.
+ * Note that this device is typically connected directly to the battery rail
+ * and does not need a source power regulator (vdd).
+ *
+ * The above values should be all that is needed to use the device with this
+ * driver.  Modifications of this driver should not be needed.
+ */
+
+
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+#include <linux/list.h>
+#include <linux/regulator/consumer.h>
+#include <linux/gpio.h>
+#include <media/nvc.h>
+#include <media/ssl3250a.h>
+
+#define SSL3250A_REG_AMP	0x00
+#define SSL3250A_REG_TMR	0x01
+#define SSL3250A_REG_STRB	0x02
+#define SSL3250A_REG_STS	0x03
+#define ssl3250a_flash_cap_size	(sizeof(ssl3250a_flash_cap.numberoflevels) \
+				+ (sizeof(ssl3250a_flash_cap.levels[0]) \
+				* (SSL3250A_MAX_FLASH_LEVEL + 1)))
+#define ssl3250a_torch_cap_size	(sizeof(ssl3250a_torch_cap.numberoflevels) \
+				+ (sizeof(ssl3250a_torch_cap.guidenum[0]) \
+				* (SSL3250A_MAX_TORCH_LEVEL + 1)))
+
+
+static struct nvc_torch_flash_capabilities ssl3250a_flash_cap = {
+	SSL3250A_MAX_FLASH_LEVEL + 1,
+	{
+		{ 0,   0xFFFFFFFF, 0 },
+		{ 215, 820,        20 },
+		{ 230, 820,        20 },
+		{ 245, 820,        20 },
+		{ 260, 820,        20 },
+		{ 275, 820,        20 },
+		{ 290, 820,        20 },
+		{ 305, 820,        20 },
+		{ 320, 820,        20 },
+		{ 335, 820,        20 },
+		{ 350, 820,        20 },
+		{ 365, 820,        20 },
+		{ 380, 820,        20 },
+		{ 395, 820,        20 },
+		{ 410, 820,        20 },
+		{ 425, 820,        20 },
+		{ 440, 820,        20 },
+		{ 455, 820,        20 },
+		{ 470, 820,        20 },
+		{ 485, 820,        20 },
+		{ 500, 820,        20 }
+	}
+};
+
+static struct nvc_torch_torch_capabilities ssl3250a_torch_cap = {
+	SSL3250A_MAX_TORCH_LEVEL + 1,
+	{
+		0,
+		50,
+		65,
+		80,
+		95,
+		110,
+		125,
+		140,
+		155,
+		170,
+		185,
+		200
+	}
+};
+
+struct ssl3250a_info {
+	atomic_t in_use;
+	struct i2c_client *i2c_client;
+	struct ssl3250a_platform_data *pdata;
+	struct miscdevice miscdev;
+	struct list_head list;
+	int pwr_api;
+	int pwr_dev;
+	struct nvc_regulator vreg_i2c;
+	u8 s_mode;
+	struct ssl3250a_info *s_info;
+};
+
+static struct nvc_torch_pin_state ssl3250a_default_pinstate = {
+	.mask		= 0x0000,
+	.values		= 0x0000,
+};
+
+static struct ssl3250a_platform_data ssl3250a_default_pdata = {
+	.cfg		= 0,
+	.num		= 0,
+	.sync		= 0,
+	.dev_name	= "torch",
+	.pinstate	= &ssl3250a_default_pinstate,
+	.max_amp_torch	= SSL3250A_MAX_TORCH_LEVEL,
+	.max_amp_flash	= SSL3250A_MAX_FLASH_LEVEL,
+};
+
+static LIST_HEAD(ssl3250a_info_list);
+static DEFINE_SPINLOCK(ssl3250a_spinlock);
+
+
+static int ssl3250a_i2c_rd(struct ssl3250a_info *info, u8 reg, u8 *val)
+{
+	struct i2c_msg msg[2];
+	u8 buf[2];
+
+	buf[0] = reg;
+	msg[0].addr = info->i2c_client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &buf[0];
+	msg[1].addr = info->i2c_client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = &buf[1];
+	*val = 0;
+	if (i2c_transfer(info->i2c_client->adapter, msg, 2) != 2)
+		return -EIO;
+
+	*val = buf[1];
+	return 0;
+}
+
+static int ssl3250a_i2c_wr(struct ssl3250a_info *info, u8 reg, u8 val)
+{
+	struct i2c_msg msg;
+	u8 buf[2];
+
+	buf[0] = reg;
+	buf[1] = val;
+	msg.addr = info->i2c_client->addr;
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = &buf[0];
+	if (i2c_transfer(info->i2c_client->adapter, &msg, 1) != 1)
+		return -EIO;
+
+	return 0;
+}
+
+static void ssl3250a_gpio_act(struct ssl3250a_info *info, int val)
+{
+	int prev_val;
+
+	if (info->pdata->gpio_act) {
+		prev_val = gpio_get_value(info->pdata->gpio_act);
+		if (val != prev_val) {
+			gpio_set_value(info->pdata->gpio_act, val);
+			if (val)
+				mdelay(1); /* delay for device startup */
+		}
+	}
+}
+
+static void ssl3250a_pm_regulator_put(struct nvc_regulator *sreg)
+{
+	regulator_put(sreg->vreg);
+	sreg->vreg = NULL;
+}
+
+static int ssl3250a_pm_regulator_get(struct ssl3250a_info *info,
+				     struct nvc_regulator *sreg,
+				     char vreg_name[])
+{
+	int err = 0;
+
+	sreg->vreg_flag = 0;
+	sreg->vreg = regulator_get(&info->i2c_client->dev, vreg_name);
+	if (WARN_ON(IS_ERR(sreg->vreg))) {
+		dev_err(&info->i2c_client->dev,
+			"%s err for regulator: %s err: %d\n",
+			__func__, vreg_name, (int)sreg->vreg);
+		err = PTR_ERR(sreg->vreg);
+		sreg->vreg = NULL;
+	} else {
+		sreg->vreg_name = vreg_name;
+		dev_dbg(&info->i2c_client->dev,
+				"%s vreg_name: %s\n",
+				__func__, sreg->vreg_name);
+	}
+	return err;
+}
+
+static int ssl3250a_pm_regulator_en(struct ssl3250a_info *info,
+				    struct nvc_regulator *sreg)
+{
+	int err = 0;
+
+	if (!sreg->vreg_flag && (sreg->vreg != NULL)) {
+		err = regulator_enable(sreg->vreg);
+		if (!err) {
+			dev_dbg(&info->i2c_client->dev,
+					"%s vreg_name: %s\n",
+					__func__, sreg->vreg_name);
+			sreg->vreg_flag = 1;
+			err = 1; /* flag regulator state change */
+		} else {
+			dev_err(&info->i2c_client->dev,
+					"%s err, regulator: %s\n",
+					__func__, sreg->vreg_name);
+		}
+	}
+	return err;
+}
+
+static int ssl3250a_pm_regulator_dis(struct ssl3250a_info *info,
+				     struct nvc_regulator *sreg)
+{
+	int err = 0;
+
+	if (sreg->vreg_flag && (sreg->vreg != NULL)) {
+		err = regulator_disable(sreg->vreg);
+		if (!err)
+			dev_dbg(&info->i2c_client->dev,
+					"%s vreg_name: %s\n",
+					__func__, sreg->vreg_name);
+		else
+			dev_err(&info->i2c_client->dev,
+					"%s err, regulator: %s\n",
+					__func__, sreg->vreg_name);
+	}
+	sreg->vreg_flag = 0;
+	return err;
+}
+
+static int ssl3250a_pm_wr(struct ssl3250a_info *info, int pwr)
+{
+	int err = 0;
+
+	if (pwr == info->pwr_dev)
+		return 0;
+
+	switch (pwr) {
+	case NVC_PWR_OFF:
+		if ((info->pdata->cfg & NVC_CFG_OFF2STDBY) ||
+			     (info->pdata->cfg & NVC_CFG_BOOT_INIT)) {
+			pwr = NVC_PWR_STDBY;
+		} else {
+			ssl3250a_gpio_act(info, 0);
+			err = ssl3250a_pm_regulator_dis(info, &info->vreg_i2c);
+			break;
+		}
+	case NVC_PWR_STDBY_OFF:
+		if ((info->pdata->cfg & NVC_CFG_OFF2STDBY) ||
+			     (info->pdata->cfg & NVC_CFG_BOOT_INIT)) {
+			pwr = NVC_PWR_STDBY;
+		} else {
+			ssl3250a_gpio_act(info, 0);
+			err = ssl3250a_pm_regulator_en(info, &info->vreg_i2c);
+			break;
+		}
+	case NVC_PWR_STDBY:
+		err = ssl3250a_pm_regulator_en(info, &info->vreg_i2c);
+		ssl3250a_gpio_act(info, 1);
+		err |= ssl3250a_i2c_wr(info, SSL3250A_REG_AMP, 0x00);
+		break;
+
+	case NVC_PWR_COMM:
+	case NVC_PWR_ON:
+		err = ssl3250a_pm_regulator_en(info, &info->vreg_i2c);
+		ssl3250a_gpio_act(info, 1);
+		break;
+
+	default:
+		err = -EINVAL;
+		break;
+	}
+
+	if (err < 0) {
+		dev_err(&info->i2c_client->dev, "%s error\n", __func__);
+		pwr = NVC_PWR_ERR;
+	}
+	info->pwr_dev = pwr;
+	if (err > 0)
+		return 0;
+
+	return err;
+}
+
+static int ssl3250a_pm_wr_s(struct ssl3250a_info *info, int pwr)
+{
+	int err1 = 0;
+	int err2 = 0;
+
+	if ((info->s_mode == NVC_SYNC_OFF) ||
+			(info->s_mode == NVC_SYNC_MASTER) ||
+			(info->s_mode == NVC_SYNC_STEREO))
+		err1 = ssl3250a_pm_wr(info, pwr);
+	if ((info->s_mode == NVC_SYNC_SLAVE) ||
+			(info->s_mode == NVC_SYNC_STEREO))
+		err2 = ssl3250a_pm_wr(info->s_info, pwr);
+	return err1 | err2;
+}
+
+static int ssl3250a_pm_api_wr(struct ssl3250a_info *info, int pwr)
+{
+	int err = 0;
+
+	if (!pwr || (pwr > NVC_PWR_ON))
+		return 0;
+
+	if (pwr > info->pwr_dev)
+		err = ssl3250a_pm_wr_s(info, pwr);
+	if (!err)
+		info->pwr_api = pwr;
+	else
+		info->pwr_api = NVC_PWR_ERR;
+	if (info->pdata->cfg & NVC_CFG_NOERR)
+		return 0;
+
+	return err;
+}
+
+static int ssl3250a_pm_dev_wr(struct ssl3250a_info *info, int pwr)
+{
+	if (pwr < info->pwr_api)
+		pwr = info->pwr_api;
+	return ssl3250a_pm_wr(info, pwr);
+}
+
+static void ssl3250a_pm_exit(struct ssl3250a_info *info)
+{
+	ssl3250a_pm_wr_s(info, NVC_PWR_OFF);
+	ssl3250a_pm_regulator_put(&info->vreg_i2c);
+}
+
+static void ssl3250a_pm_init(struct ssl3250a_info *info)
+{
+	ssl3250a_pm_regulator_get(info, &info->vreg_i2c, "vdd_i2c");
+}
+
+static int ssl3250a_dev_id(struct ssl3250a_info *info)
+{
+	u8 addr;
+	u8 reg;
+	int err;
+
+	ssl3250a_pm_dev_wr(info, NVC_PWR_COMM);
+	/* There isn't a device ID so we just check that all the registers
+	 * equal their startup defaults, which in this case, is 0.
+	 */
+	for (addr = 0; addr < SSL3250A_REG_STS; addr++) {
+		err = ssl3250a_i2c_rd(info, addr, &reg);
+		if (err) {
+			break;
+		} else {
+			if (reg) {
+				err = -ENODEV;
+				break;
+			}
+		}
+	}
+	ssl3250a_pm_dev_wr(info, NVC_PWR_OFF);
+	return err;
+}
+
+static int ssl3250a_param_rd(struct ssl3250a_info *info, long arg)
+{
+	struct nvc_param params;
+	struct nvc_torch_pin_state pinstate;
+	const void *data_ptr;
+	u32 data_size = 0;
+	int err;
+	u8 reg;
+
+	if (copy_from_user(&params,
+			(const void __user *)arg,
+			sizeof(struct nvc_param))) {
+		dev_err(&info->i2c_client->dev, "%s %d copy_from_user err\n",
+				__func__, __LINE__);
+		return -EINVAL;
+	}
+
+	if (info->s_mode == NVC_SYNC_SLAVE)
+		info = info->s_info;
+	switch (params.param) {
+	case NVC_PARAM_FLASH_CAPS:
+		dev_dbg(&info->i2c_client->dev, "%s FLASH_CAPS\n", __func__);
+		data_ptr = &ssl3250a_flash_cap;
+		data_size = ssl3250a_flash_cap_size;
+		break;
+
+	case NVC_PARAM_FLASH_LEVEL:
+		ssl3250a_pm_dev_wr(info, NVC_PWR_COMM);
+		err = ssl3250a_i2c_rd(info, SSL3250A_REG_AMP, &reg);
+		ssl3250a_pm_dev_wr(info, NVC_PWR_OFF);
+		if (err < 0)
+			return err;
+
+		reg >>= 3; /*7:3=flash amps*/
+		reg &= 0x1F; /*4:0=flash amps*/
+		if (reg < 12) /*flash starts at 12*/
+			reg = 0; /*<12=torch or off*/
+		else
+			reg -= 11; /*create flash index*/
+		dev_dbg(&info->i2c_client->dev, "%s FLASH_LEVEL: %u\n",
+			    __func__,
+			    (unsigned)ssl3250a_flash_cap.levels[reg].guidenum);
+		data_ptr = &ssl3250a_flash_cap.levels[reg].guidenum;
+		data_size = sizeof(ssl3250a_flash_cap.levels[reg].guidenum);
+		break;
+
+	case NVC_PARAM_TORCH_CAPS:
+		dev_dbg(&info->i2c_client->dev, "%s TORCH_CAPS\n", __func__);
+		data_ptr = &ssl3250a_torch_cap;
+		data_size = ssl3250a_torch_cap_size;
+		break;
+
+	case NVC_PARAM_TORCH_LEVEL:
+		ssl3250a_pm_dev_wr(info, NVC_PWR_COMM);
+		err = ssl3250a_i2c_rd(info, SSL3250A_REG_AMP, &reg);
+		ssl3250a_pm_dev_wr(info, NVC_PWR_OFF);
+		if (err < 0)
+			return err;
+
+		reg >>= 3; /*7:3=torch amps*/
+		reg &= 0x1F; /*4:0=torch amps*/
+		if (reg > 11) /*flash starts at 12*/
+			reg = 0; /*>11=flash mode (torch off)*/
+		dev_dbg(&info->i2c_client->dev, "%s TORCH_LEVEL: %u\n",
+				__func__,
+				(unsigned)ssl3250a_torch_cap.guidenum[reg]);
+		data_ptr = &ssl3250a_torch_cap.guidenum[reg];
+		data_size = sizeof(ssl3250a_torch_cap.guidenum[reg]);
+		break;
+
+	case NVC_PARAM_FLASH_PIN_STATE:
+		pinstate.mask = info->pdata->pinstate->mask;
+		ssl3250a_pm_dev_wr(info, NVC_PWR_COMM);
+		err = ssl3250a_i2c_rd(info, SSL3250A_REG_AMP, &reg);
+		ssl3250a_pm_dev_wr(info, NVC_PWR_OFF);
+		if (err < 0)
+			return err;
+
+		reg >>= 3; /*7:3=flash amps*/
+		reg &= 0x1F; /*4:0=flash amps*/
+		if (reg < 12) /*flash starts at 12*/
+			pinstate.values = 0; /*deassert strobe*/
+		else
+			/*assert strobe*/
+			pinstate.values = info->pdata->pinstate->values;
+		dev_dbg(&info->i2c_client->dev, "%s FLASH_PIN_STATE: %x&%x\n",
+				__func__, pinstate.mask, pinstate.values);
+		data_ptr = &pinstate;
+		data_size = sizeof(struct nvc_torch_pin_state);
+		break;
+
+	case NVC_PARAM_STEREO:
+		dev_dbg(&info->i2c_client->dev, "%s STEREO: %d\n",
+				__func__, info->s_mode);
+		data_ptr = &info->s_mode;
+		data_size = sizeof(info->s_mode);
+		break;
+
+	default:
+		dev_err(&info->i2c_client->dev,
+				"%s unsupported parameter: %d\n",
+				__func__, params.param);
+		return -EINVAL;
+	}
+
+	if (params.sizeofvalue < data_size) {
+		dev_err(&info->i2c_client->dev,
+				"%s data size mismatch %d != %d\n",
+				__func__, params.sizeofvalue, data_size);
+		return -EINVAL;
+	}
+
+	if (copy_to_user((void __user *)params.p_value,
+			 data_ptr,
+			 data_size)) {
+		dev_err(&info->i2c_client->dev,
+				"%s copy_to_user err line %d\n",
+				__func__, __LINE__);
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int ssl3250a_param_wr_s(struct ssl3250a_info *info,
+			       struct nvc_param *params,
+			       u8 val)
+{
+	int err;
+
+	switch (params->param) {
+	case NVC_PARAM_FLASH_LEVEL:
+		dev_dbg(&info->i2c_client->dev, "%s FLASH_LEVEL: %d\n",
+				__func__, val);
+		ssl3250a_pm_dev_wr(info, NVC_PWR_ON);
+		if (val > ssl3250a_default_pdata.max_amp_flash)
+			val = ssl3250a_default_pdata.max_amp_flash;
+		/*Amp limit values are in the board-sensors file.*/
+		if (info->pdata->max_amp_flash &&
+				(val > info->pdata->max_amp_flash))
+			val = info->pdata->max_amp_flash;
+		if (val) {
+			val += 11; /*flash starts at 12*/
+			val <<= 3; /*7:3=flash/torch amps*/
+		}
+		err = ssl3250a_i2c_wr(info, SSL3250A_REG_AMP, val);
+		if (!val) /*turn pwr off if no flash && no pwr_api*/
+			ssl3250a_pm_dev_wr(info, NVC_PWR_OFF);
+		return err;
+
+	case NVC_PARAM_TORCH_LEVEL:
+		dev_dbg(&info->i2c_client->dev, "%s TORCH_LEVEL: %d\n",
+				__func__, val);
+		ssl3250a_pm_dev_wr(info, NVC_PWR_ON);
+		if (val > ssl3250a_default_pdata.max_amp_torch)
+			val = ssl3250a_default_pdata.max_amp_torch;
+		/*Amp limit values are in the board-sensors file.*/
+		if (info->pdata->max_amp_torch &&
+				(val > info->pdata->max_amp_torch))
+			val = info->pdata->max_amp_torch;
+		if (val)
+			val <<= 3; /*7:3=flash/torch amps*/
+		err = ssl3250a_i2c_wr(info, SSL3250A_REG_AMP, val);
+		if (!val) /*turn pwr off if no torch && no pwr_api*/
+			ssl3250a_pm_dev_wr(info, NVC_PWR_OFF);
+		return err;
+
+	case NVC_PARAM_FLASH_PIN_STATE:
+		dev_dbg(&info->i2c_client->dev, "%s FLASH_PIN_STATE: %d\n",
+				__func__, val);
+		if (val)
+			val = 0x01; /*0:0=soft trigger*/
+		return ssl3250a_i2c_wr(info, SSL3250A_REG_STRB, val);
+
+	default:
+		dev_err(&info->i2c_client->dev,
+				"%s unsupported parameter: %d\n",
+				__func__, params->param);
+		return -EINVAL;
+	}
+}
+
+static int ssl3250a_param_wr(struct ssl3250a_info *info, long arg)
+{
+	struct nvc_param params;
+	u8 val;
+	int err = 0;
+
+	if (copy_from_user(&params,
+				(const void __user *)arg,
+				sizeof(struct nvc_param))) {
+		dev_err(&info->i2c_client->dev, "%s %d copy_from_user err\n",
+				__func__, __LINE__);
+		return -EINVAL;
+	}
+
+	if (copy_from_user(&val, (const void __user *)params.p_value,
+			   sizeof(val))) {
+		dev_err(&info->i2c_client->dev, "%s %d copy_from_user err\n",
+				__func__, __LINE__);
+		return -EINVAL;
+	}
+
+	/* parameters independent of sync mode */
+	switch (params.param) {
+	case NVC_PARAM_STEREO:
+		dev_dbg(&info->i2c_client->dev, "%s STEREO: %d\n",
+				__func__, (int)val);
+		if (val == info->s_mode)
+			return 0;
+
+		switch (val) {
+		case NVC_SYNC_OFF:
+			info->s_mode = val;
+			if (info->s_info != NULL) {
+				info->s_info->s_mode = val;
+				ssl3250a_pm_wr(info->s_info, NVC_PWR_OFF);
+			}
+			break;
+
+		case NVC_SYNC_MASTER:
+			info->s_mode = val;
+			if (info->s_info != NULL)
+				info->s_info->s_mode = val;
+			break;
+
+		case NVC_SYNC_SLAVE:
+		case NVC_SYNC_STEREO:
+			if (info->s_info != NULL) {
+				/* sync power */
+				info->s_info->pwr_api = info->pwr_api;
+				err = ssl3250a_pm_wr(info->s_info,
+						     info->pwr_dev);
+				if (!err) {
+					info->s_mode = val;
+					info->s_info->s_mode = val;
+				} else {
+					ssl3250a_pm_wr(info->s_info,
+						       NVC_PWR_OFF);
+					err = -EIO;
+				}
+			} else {
+				err = -EINVAL;
+			}
+			break;
+
+		default:
+			err = -EINVAL;
+		}
+		if (info->pdata->cfg & NVC_CFG_NOERR)
+			return 0;
+
+		return err;
+
+	default:
+	/* parameters dependent on sync mode */
+		switch (info->s_mode) {
+		case NVC_SYNC_OFF:
+		case NVC_SYNC_MASTER:
+			return ssl3250a_param_wr_s(info, &params, val);
+
+		case NVC_SYNC_SLAVE:
+			return ssl3250a_param_wr_s(info->s_info,
+						 &params,
+						 val);
+
+		case NVC_SYNC_STEREO:
+			err = ssl3250a_param_wr_s(info, &params, val);
+			if (!(info->pdata->cfg & NVC_CFG_SYNC_I2C_MUX))
+				err |= ssl3250a_param_wr_s(info->s_info,
+							 &params,
+							 val);
+			return err;
+
+		default:
+			dev_err(&info->i2c_client->dev, "%s %d internal err\n",
+					__func__, __LINE__);
+			return -EINVAL;
+		}
+	}
+}
+
+static long ssl3250a_ioctl(struct file *file,
+			   unsigned int cmd,
+			   unsigned long arg)
+{
+	struct ssl3250a_info *info = file->private_data;
+	int pwr;
+
+	switch (cmd) {
+	case NVC_IOCTL_PARAM_WR:
+		return ssl3250a_param_wr(info, arg);
+
+	case NVC_IOCTL_PARAM_RD:
+		return ssl3250a_param_rd(info, arg);
+
+	case NVC_IOCTL_PWR_WR:
+		/* This is a Guaranteed Level of Service (GLOS) call */
+		pwr = (int)arg * 2;
+		dev_dbg(&info->i2c_client->dev, "%s PWR_WR: %d\n",
+				__func__, pwr);
+		return ssl3250a_pm_api_wr(info, pwr);
+
+	case NVC_IOCTL_PWR_RD:
+		if (info->s_mode == NVC_SYNC_SLAVE)
+			pwr = info->s_info->pwr_api / 2;
+		else
+			pwr = info->pwr_api / 2;
+		dev_dbg(&info->i2c_client->dev, "%s PWR_RD: %d\n",
+				__func__, pwr);
+		if (copy_to_user((void __user *)arg, (const void *)&pwr,
+				 sizeof(pwr))) {
+			dev_err(&info->i2c_client->dev,
+					"%s copy_to_user err line %d\n",
+					__func__, __LINE__);
+			return -EFAULT;
+		}
+
+		return 0;
+
+	default:
+		dev_err(&info->i2c_client->dev, "%s unsupported ioctl: %x\n",
+				__func__, cmd);
+		return -EINVAL;
+	}
+}
+
+static int ssl3250a_sync_en(int dev1, int dev2)
+{
+	struct ssl3250a_info *sync1 = NULL;
+	struct ssl3250a_info *sync2 = NULL;
+	struct ssl3250a_info *pos = NULL;
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(pos, &ssl3250a_info_list, list) {
+		if (pos->pdata->num == dev1) {
+			sync1 = pos;
+			break;
+		}
+	}
+	pos = NULL;
+	list_for_each_entry_rcu(pos, &ssl3250a_info_list, list) {
+		if (pos->pdata->num == dev2) {
+			sync2 = pos;
+			break;
+		}
+	}
+	rcu_read_unlock();
+	if (sync1 != NULL)
+		sync1->s_info = NULL;
+	if (sync2 != NULL)
+		sync2->s_info = NULL;
+	if (!dev1 && !dev2)
+		return 0; /* no err if default instance 0's used */
+
+	if (dev1 == dev2)
+		return -EINVAL; /* err if sync instance is itself */
+
+	if ((sync1 != NULL) && (sync2 != NULL)) {
+		sync1->s_info = sync2;
+		sync2->s_info = sync1;
+	}
+
+	return 0;
+}
+
+static int ssl3250a_sync_dis(struct ssl3250a_info *info)
+{
+	if (info->s_info != NULL) {
+		info->s_info->s_mode = 0;
+		info->s_info->s_info = NULL;
+		info->s_mode = 0;
+		info->s_info = NULL;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int ssl3250a_open(struct inode *inode, struct file *file)
+{
+	struct ssl3250a_info *info = NULL;
+	struct ssl3250a_info *pos = NULL;
+	int err;
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(pos, &ssl3250a_info_list, list) {
+		if (pos->miscdev.minor == iminor(inode)) {
+			info = pos;
+			break;
+		}
+	}
+	rcu_read_unlock();
+	if (!info)
+		return -ENODEV;
+
+	err = ssl3250a_sync_en(info->pdata->num, info->pdata->sync);
+	if (err == -EINVAL)
+		dev_err(&info->i2c_client->dev,
+			 "%s err: invalid num (%u) and sync (%u) instance\n",
+			 __func__, info->pdata->num, info->pdata->sync);
+	if (atomic_xchg(&info->in_use, 1))
+		return -EBUSY;
+
+	if (info->s_info != NULL) {
+		if (atomic_xchg(&info->s_info->in_use, 1))
+			return -EBUSY;
+	}
+
+	file->private_data = info;
+	dev_dbg(&info->i2c_client->dev, "%s\n", __func__);
+	return 0;
+}
+
+static int ssl3250a_release(struct inode *inode, struct file *file)
+{
+	struct ssl3250a_info *info = file->private_data;
+
+	dev_dbg(&info->i2c_client->dev, "%s\n", __func__);
+	ssl3250a_pm_wr_s(info, NVC_PWR_OFF);
+	file->private_data = NULL;
+	WARN_ON(!atomic_xchg(&info->in_use, 0));
+	if (info->s_info != NULL)
+		WARN_ON(!atomic_xchg(&info->s_info->in_use, 0));
+	ssl3250a_sync_dis(info);
+	return 0;
+}
+
+static const struct file_operations ssl3250a_fileops = {
+	.owner = THIS_MODULE,
+	.open = ssl3250a_open,
+	.unlocked_ioctl = ssl3250a_ioctl,
+	.release = ssl3250a_release,
+};
+
+static void ssl3250a_del(struct ssl3250a_info *info)
+{
+	ssl3250a_pm_exit(info);
+	ssl3250a_sync_dis(info);
+	spin_lock(&ssl3250a_spinlock);
+	list_del_rcu(&info->list);
+	spin_unlock(&ssl3250a_spinlock);
+	synchronize_rcu();
+}
+
+static int ssl3250a_remove(struct i2c_client *client)
+{
+	struct ssl3250a_info *info = i2c_get_clientdata(client);
+
+	dev_dbg(&info->i2c_client->dev, "%s\n", __func__);
+	misc_deregister(&info->miscdev);
+	ssl3250a_del(info);
+	return 0;
+}
+
+static int ssl3250a_probe(
+	struct i2c_client *client,
+	const struct i2c_device_id *id)
+{
+	struct ssl3250a_info *info;
+	char dname[16];
+	int err;
+
+	dev_dbg(&client->dev, "%s\n", __func__);
+	info = devm_kzalloc(&client->dev, sizeof(*info), GFP_KERNEL);
+	if (info == NULL) {
+		dev_err(&client->dev, "%s: kzalloc error\n", __func__);
+		return -ENOMEM;
+	}
+
+	info->i2c_client = client;
+	if (client->dev.platform_data) {
+		info->pdata = client->dev.platform_data;
+	} else {
+		info->pdata = &ssl3250a_default_pdata;
+		dev_dbg(&client->dev,
+				"%s No platform data.  Using defaults.\n",
+				__func__);
+	}
+	i2c_set_clientdata(client, info);
+	INIT_LIST_HEAD(&info->list);
+	spin_lock(&ssl3250a_spinlock);
+	list_add_rcu(&info->list, &ssl3250a_info_list);
+	spin_unlock(&ssl3250a_spinlock);
+	ssl3250a_pm_init(info);
+	err = ssl3250a_dev_id(info);
+	if (err < 0) {
+		dev_err(&client->dev, "%s device not found\n", __func__);
+		if (info->pdata->cfg & NVC_CFG_NODEV) {
+			ssl3250a_del(info);
+			return -ENODEV;
+		}
+	} else {
+		dev_dbg(&client->dev, "%s device found\n", __func__);
+	}
+
+	if (info->pdata->dev_name != 0)
+		strcpy(dname, info->pdata->dev_name);
+	else
+		strcpy(dname, "ssl3250a");
+	if (info->pdata->num)
+		snprintf(dname, sizeof(dname), "%s.%u",
+			 dname, info->pdata->num);
+	info->miscdev.name = dname;
+	info->miscdev.fops = &ssl3250a_fileops;
+	info->miscdev.minor = MISC_DYNAMIC_MINOR;
+	if (misc_register(&info->miscdev)) {
+		dev_err(&client->dev, "%s unable to register misc device %s\n",
+				__func__, dname);
+		ssl3250a_del(info);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static const struct i2c_device_id ssl3250a_id[] = {
+	{ "ssl3250a", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, ssl3250a_id);
+
+static struct i2c_driver ssl3250a_i2c_driver = {
+	.driver = {
+		.name = "ssl3250a",
+		.owner = THIS_MODULE,
+	},
+	.id_table = ssl3250a_id,
+	.probe = ssl3250a_probe,
+	.remove = ssl3250a_remove,
+};
+
+static int __init ssl3250a_init(void)
+{
+	return i2c_add_driver(&ssl3250a_i2c_driver);
+}
+
+static void __exit ssl3250a_exit(void)
+{
+	i2c_del_driver(&ssl3250a_i2c_driver);
+}
+
+module_init(ssl3250a_init);
+module_exit(ssl3250a_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/tegra/tegra_camera.c b/drivers/media/video/tegra/tegra_camera.c
new file mode 100644
index 000000000000..2b0cd005096c
--- /dev/null
+++ b/drivers/media/video/tegra/tegra_camera.c
@@ -0,0 +1,580 @@
+/*
+ * drivers/media/video/tegra/tegra_camera.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (c) 2010-2012, NVIDIA Corporation.  All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/miscdevice.h>
+#include <linux/platform_device.h>
+#include <linux/ioctl.h>
+#include <linux/fs.h>
+#include <linux/device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <mach/iomap.h>
+#include <mach/clk.h>
+#include <mach/powergate.h>
+
+#include <media/tegra_camera.h>
+
+/* Eventually this should handle all clock and reset calls for the isp, vi,
+ * vi_sensor, and csi modules, replacing nvrm and nvos completely for camera
+ */
+#define TEGRA_CAMERA_NAME "tegra_camera"
+
+struct tegra_camera_dev {
+	struct device *dev;
+	struct miscdevice misc_dev;
+	struct clk *isp_clk;
+	struct clk *vi_clk;
+	struct clk *vi_sensor_clk;
+	struct clk *csus_clk;
+	struct clk *csi_clk;
+	struct clk *emc_clk;
+	struct regulator *reg;
+	struct tegra_camera_clk_info info;
+	struct mutex tegra_camera_lock;
+	atomic_t in_use;
+	int power_on;
+};
+
+struct tegra_camera_block {
+	int (*enable) (struct tegra_camera_dev *dev);
+	int (*disable) (struct tegra_camera_dev *dev);
+	bool is_enabled;
+};
+
+/*
+ * Declare and define two static variables to provide hint to
+ * gr3d module
+ */
+static int tegra_camera_on;
+static struct tegra_camera_platform_data *pdata;
+
+int is_tegra_camera_on(void)
+{
+	if (pdata) {
+		if (pdata->limit_3d_emc_clk)
+			return tegra_camera_on;
+		else
+			return 0;
+	} else {
+		return 0;
+	}
+}
+
+static int tegra_camera_enable_clk(struct tegra_camera_dev *dev)
+{
+	clk_enable(dev->vi_clk);
+	clk_enable(dev->vi_sensor_clk);
+	clk_enable(dev->csus_clk);
+
+	tegra_periph_reset_assert(dev->vi_clk);
+	udelay(2);
+	tegra_periph_reset_deassert(dev->vi_clk);
+
+	clk_enable(dev->isp_clk);
+	tegra_periph_reset_assert(dev->isp_clk);
+	udelay(2);
+	tegra_periph_reset_deassert(dev->isp_clk);
+
+	clk_enable(dev->csi_clk);
+	tegra_periph_reset_assert(dev->csi_clk);
+	udelay(2);
+	tegra_periph_reset_deassert(dev->csi_clk);
+	return 0;
+}
+
+static int tegra_camera_disable_clk(struct tegra_camera_dev *dev)
+{
+	clk_disable(dev->csi_clk);
+	tegra_periph_reset_assert(dev->csi_clk);
+	clk_disable(dev->isp_clk);
+	tegra_periph_reset_assert(dev->isp_clk);
+	clk_disable(dev->csus_clk);
+	clk_disable(dev->vi_sensor_clk);
+	clk_disable(dev->vi_clk);
+	tegra_periph_reset_assert(dev->vi_clk);
+
+	return 0;
+}
+
+static int tegra_camera_enable_emc(struct tegra_camera_dev *dev)
+{
+	int ret = tegra_emc_disable_eack();
+	clk_enable(dev->emc_clk);
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	clk_set_rate(dev->emc_clk, 300000000);
+#endif
+	return ret;
+}
+
+static int tegra_camera_disable_emc(struct tegra_camera_dev *dev)
+{
+	clk_disable(dev->emc_clk);
+	return tegra_emc_enable_eack();
+}
+
+static int tegra_camera_clk_set_rate(struct tegra_camera_dev *dev)
+{
+	struct clk *clk, *clk_parent;
+	struct tegra_camera_clk_info *info = &dev->info;
+	unsigned long parent_rate, parent_div_rate, parent_div_rate_pre;
+
+	if (!info) {
+		dev_err(dev->dev,
+				"%s: no clock info %d\n",
+				__func__, info->id);
+		return -EINVAL;
+	}
+
+	if (info->id != TEGRA_CAMERA_MODULE_VI &&
+		info->id != TEGRA_CAMERA_MODULE_EMC) {
+		dev_err(dev->dev,
+				"%s: set rate only aplies to vi module %d\n",
+				__func__, info->id);
+		return -EINVAL;
+	}
+
+	switch (info->clk_id) {
+	case TEGRA_CAMERA_VI_CLK:
+		clk = dev->vi_clk;
+		break;
+	case TEGRA_CAMERA_VI_SENSOR_CLK:
+		clk = dev->vi_sensor_clk;
+		break;
+	case TEGRA_CAMERA_EMC_CLK:
+		clk = dev->emc_clk;
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+		dev_dbg(dev->dev, "%s: emc_clk rate=%lu\n",
+			__func__, info->rate);
+		clk_set_rate(dev->emc_clk, info->rate);
+#endif
+		goto set_rate_end;
+	default:
+		dev_err(dev->dev,
+				"%s: invalid clk id for set rate %d\n",
+				__func__, info->clk_id);
+		return -EINVAL;
+	}
+
+	clk_parent = clk_get_parent(clk);
+	parent_rate = clk_get_rate(clk_parent);
+	dev_dbg(dev->dev, "%s: clk_id=%d, parent_rate=%lu, clk_rate=%lu\n",
+			__func__, info->clk_id, parent_rate, info->rate);
+	parent_div_rate = parent_rate;
+	parent_div_rate_pre = parent_rate;
+
+	/*
+	 * The requested clock rate from user space should be respected.
+	 * This loop is to search the clock rate that is higher than requested
+	 * clock.
+	 */
+	while (parent_div_rate >= info->rate) {
+		parent_div_rate_pre = parent_div_rate;
+		parent_div_rate = clk_round_rate(clk, parent_div_rate-1);
+	}
+
+	dev_dbg(dev->dev, "%s: set_rate=%lu",
+			__func__, parent_div_rate_pre);
+
+	clk_set_rate(clk, parent_div_rate_pre);
+
+	if (info->clk_id == TEGRA_CAMERA_VI_CLK) {
+		/*
+		 * bit 25: 0 = pd2vi_Clk, 1 = vi_sensor_clk
+		 * bit 24: 0 = internal clock, 1 = external clock(pd2vi_clk)
+		 */
+		if (info->flag == TEGRA_CAMERA_ENABLE_PD2VI_CLK)
+			tegra_clk_cfg_ex(clk, TEGRA_CLK_VI_INP_SEL, 2);
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+		{
+			u32 val;
+			void __iomem *apb_misc =
+				IO_ADDRESS(TEGRA_APB_MISC_BASE);
+			val = readl(apb_misc + 0x42c);
+			writel(val | 0x1, apb_misc + 0x42c);
+		}
+#endif
+	}
+
+set_rate_end:
+	info->rate = clk_get_rate(clk);
+	dev_dbg(dev->dev, "%s: get_rate=%lu",
+			__func__, info->rate);
+	return 0;
+
+}
+
+static int tegra_camera_power_on(struct tegra_camera_dev *dev)
+{
+	int ret = 0;
+
+	dev_dbg(dev->dev, "%s++\n", __func__);
+
+	/* Enable external power */
+	if (dev->reg) {
+		ret = regulator_enable(dev->reg);
+		if (ret) {
+			dev_err(dev->dev,
+				"%s: enable csi regulator failed.\n",
+				__func__);
+			return ret;
+		}
+	}
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+	/* Unpowergate VE */
+	ret = tegra_unpowergate_partition(TEGRA_POWERGATE_VENC);
+	if (ret)
+		dev_err(dev->dev,
+			"%s: unpowergate failed.\n",
+			__func__);
+#endif
+	dev->power_on = 1;
+	tegra_camera_on = dev->power_on;
+	return ret;
+}
+
+static int tegra_camera_power_off(struct tegra_camera_dev *dev)
+{
+	int ret = 0;
+
+	dev_dbg(dev->dev, "%s++\n", __func__);
+
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+	/* Powergate VE */
+	ret = tegra_powergate_partition(TEGRA_POWERGATE_VENC);
+	if (ret)
+		dev_err(dev->dev,
+			"%s: powergate failed.\n",
+			__func__);
+#endif
+	/* Disable external power */
+	if (dev->reg) {
+		ret = regulator_disable(dev->reg);
+		if (ret) {
+			dev_err(dev->dev,
+				"%s: disable csi regulator failed.\n",
+				__func__);
+			return ret;
+		}
+	}
+	dev->power_on = 0;
+	tegra_camera_on = dev->power_on;
+	return ret;
+}
+
+static long tegra_camera_ioctl(struct file *file,
+			       unsigned int cmd, unsigned long arg)
+{
+	uint id;
+	struct tegra_camera_dev *dev = file->private_data;
+
+	/* first element of arg must be u32 with id of module to talk to */
+	if (copy_from_user(&id, (const void __user *)arg, sizeof(uint))) {
+		dev_err(dev->dev,
+				"%s: Failed to copy arg from user", __func__);
+		return -EFAULT;
+	}
+
+	if (id >= TEGRA_CAMERA_MODULE_MAX) {
+		dev_err(dev->dev,
+				"%s: Invalid id to tegra isp ioctl%d\n",
+				__func__, id);
+		return -EINVAL;
+	}
+
+	switch (cmd) {
+	/*
+	 * Clock enable/disable and reset should be handled in kernel.
+	 * In order to support legacy code in user space, we don't remove
+	 * these IOCTL.
+	 */
+	case TEGRA_CAMERA_IOCTL_ENABLE:
+	case TEGRA_CAMERA_IOCTL_DISABLE:
+	case TEGRA_CAMERA_IOCTL_RESET:
+		return 0;
+	case TEGRA_CAMERA_IOCTL_CLK_SET_RATE:
+	{
+		int ret;
+
+		if (copy_from_user(&dev->info, (const void __user *)arg,
+				   sizeof(struct tegra_camera_clk_info))) {
+			dev_err(dev->dev,
+				"%s: Failed to copy arg from user\n", __func__);
+			return -EFAULT;
+		}
+		ret = tegra_camera_clk_set_rate(dev);
+		if (ret)
+			return ret;
+		if (copy_to_user((void __user *)arg, &dev->info,
+				 sizeof(struct tegra_camera_clk_info))) {
+			dev_err(dev->dev,
+				"%s: Failed to copy arg to user\n", __func__);
+			return -EFAULT;
+		}
+		return 0;
+	}
+	default:
+		dev_err(dev->dev,
+				"%s: Unknown tegra_camera ioctl.\n", __func__);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int tegra_camera_open(struct inode *inode, struct file *file)
+{
+	struct miscdevice *miscdev = file->private_data;
+	struct tegra_camera_dev *dev = container_of(miscdev,
+						struct tegra_camera_dev,
+						misc_dev);
+	int ret = 0;
+
+	dev_info(dev->dev, "%s\n", __func__);
+
+	if (atomic_xchg(&dev->in_use, 1))
+		return -EBUSY;
+
+	file->private_data = dev;
+
+	mutex_lock(&dev->tegra_camera_lock);
+	/* turn on CSI regulator */
+	ret = tegra_camera_power_on(dev);
+	if (ret)
+		goto open_exit;
+	/* set EMC request */
+	ret = tegra_camera_enable_emc(dev);
+	if (ret)
+		goto open_exit;
+	/* enable camera HW clock */
+	ret = tegra_camera_enable_clk(dev);
+	if (ret)
+		goto open_exit;
+open_exit:
+	mutex_unlock(&dev->tegra_camera_lock);
+	return ret;
+}
+
+static int tegra_camera_release(struct inode *inode, struct file *file)
+{
+	int ret = 0;
+	struct tegra_camera_dev *dev = file->private_data;
+
+	dev_info(dev->dev, "%s\n", __func__);
+
+
+	mutex_lock(&dev->tegra_camera_lock);
+	/* disable HW clock */
+	ret = tegra_camera_disable_clk(dev);
+	if (ret)
+		goto release_exit;
+	/* nullify EMC request */
+	ret = tegra_camera_disable_emc(dev);
+	if (ret)
+		goto release_exit;
+	/* turn off CSI regulator */
+	tegra_camera_power_off(dev);
+	if (ret)
+		goto release_exit;
+
+release_exit:
+	mutex_unlock(&dev->tegra_camera_lock);
+	WARN_ON(!atomic_xchg(&dev->in_use, 0));
+	return 0;
+}
+
+static const struct file_operations tegra_camera_fops = {
+	.owner = THIS_MODULE,
+	.open = tegra_camera_open,
+	.unlocked_ioctl = tegra_camera_ioctl,
+	.release = tegra_camera_release,
+};
+
+static int tegra_camera_clk_get(struct platform_device *pdev, const char *name,
+				struct clk **clk)
+{
+	*clk = clk_get(&pdev->dev, name);
+	if (IS_ERR_OR_NULL(*clk)) {
+		dev_err(&pdev->dev, "%s: unable to get clock for %s\n",
+			__func__, name);
+		*clk = NULL;
+		return PTR_ERR(*clk);
+	}
+	return 0;
+}
+
+static int tegra_camera_probe(struct platform_device *pdev)
+{
+	int err;
+	struct tegra_camera_dev *dev;
+
+	dev_info(&pdev->dev, "%s\n", __func__);
+	dev = devm_kzalloc(&pdev->dev, sizeof(struct tegra_camera_dev),
+			GFP_KERNEL);
+	if (!dev) {
+		err = -ENOMEM;
+		dev_err(&pdev->dev, "%s: unable to allocate memory\n",
+			__func__);
+		goto alloc_err;
+	}
+
+	mutex_init(&dev->tegra_camera_lock);
+
+	/* Powergate VE when boot */
+	mutex_lock(&dev->tegra_camera_lock);
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+	err = tegra_powergate_partition(TEGRA_POWERGATE_VENC);
+	if (err)
+		dev_err(&pdev->dev, "%s: powergate failed.\n", __func__);
+#endif
+	mutex_unlock(&dev->tegra_camera_lock);
+
+	dev->dev = &pdev->dev;
+	pdata = pdev->dev.platform_data;
+
+	/* Get regulator pointer */
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	dev->reg = regulator_get(&pdev->dev, "vcsi");
+#else
+	dev->reg = regulator_get(&pdev->dev, "avdd_dsi_csi");
+#endif
+	if (IS_ERR_OR_NULL(dev->reg)) {
+		if (dev->reg == ERR_PTR(-ENODEV)) {
+			dev->reg = NULL;
+			dev_info(&pdev->dev, "%s: no regulator device, overriding\n",
+							__func__);
+		} else {
+			dev_err(&pdev->dev, "%s: couldn't get regulator\n",
+							__func__);
+			return PTR_ERR(dev->reg);
+		}
+	}
+
+	dev->misc_dev.minor = MISC_DYNAMIC_MINOR;
+	dev->misc_dev.name = TEGRA_CAMERA_NAME;
+	dev->misc_dev.fops = &tegra_camera_fops;
+	dev->misc_dev.parent = &pdev->dev;
+
+	err = misc_register(&dev->misc_dev);
+	if (err) {
+		dev_err(&pdev->dev, "%s: Unable to register misc device!\n",
+		       TEGRA_CAMERA_NAME);
+		goto misc_register_err;
+	}
+
+	err = tegra_camera_clk_get(pdev, "isp", &dev->isp_clk);
+	if (err)
+		goto misc_register_err;
+	err = tegra_camera_clk_get(pdev, "vi", &dev->vi_clk);
+	if (err)
+		goto vi_clk_get_err;
+	err = tegra_camera_clk_get(pdev, "vi_sensor", &dev->vi_sensor_clk);
+	if (err)
+		goto vi_sensor_clk_get_err;
+	err = tegra_camera_clk_get(pdev, "csus", &dev->csus_clk);
+	if (err)
+		goto csus_clk_get_err;
+	err = tegra_camera_clk_get(pdev, "csi", &dev->csi_clk);
+	if (err)
+		goto csi_clk_get_err;
+	err = tegra_camera_clk_get(pdev, "emc", &dev->emc_clk);
+	if (err)
+		goto emc_clk_get_err;
+
+	/* dev is set in order to restore in _remove */
+	platform_set_drvdata(pdev, dev);
+
+	return 0;
+
+emc_clk_get_err:
+	clk_put(dev->emc_clk);
+csi_clk_get_err:
+	clk_put(dev->csus_clk);
+csus_clk_get_err:
+	clk_put(dev->vi_sensor_clk);
+vi_sensor_clk_get_err:
+	clk_put(dev->vi_clk);
+vi_clk_get_err:
+	clk_put(dev->isp_clk);
+misc_register_err:
+	regulator_put(dev->reg);
+alloc_err:
+	return err;
+}
+
+static int tegra_camera_remove(struct platform_device *pdev)
+{
+	struct tegra_camera_dev *dev = platform_get_drvdata(pdev);
+
+	clk_put(dev->isp_clk);
+	clk_put(dev->vi_clk);
+	clk_put(dev->vi_sensor_clk);
+	clk_put(dev->csus_clk);
+	clk_put(dev->csi_clk);
+
+	misc_deregister(&dev->misc_dev);
+	regulator_put(dev->reg);
+	mutex_destroy(&dev->tegra_camera_lock);
+
+	return 0;
+}
+
+static int tegra_camera_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct tegra_camera_dev *dev = platform_get_drvdata(pdev);
+	int ret = 0;
+
+	mutex_lock(&dev->tegra_camera_lock);
+	if (dev->power_on) {
+		ret = -EBUSY;
+		dev_err(&pdev->dev,
+		"tegra_camera cannot suspend, "
+		"application is holding on to camera. \n");
+	}
+	mutex_unlock(&dev->tegra_camera_lock);
+
+	return ret;
+}
+
+static int tegra_camera_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static struct platform_driver tegra_camera_driver = {
+	.probe = tegra_camera_probe,
+	.remove = tegra_camera_remove,
+	.suspend = tegra_camera_suspend,
+	.resume = tegra_camera_resume,
+	.driver = { .name = TEGRA_CAMERA_NAME }
+};
+
+static int __init tegra_camera_init(void)
+{
+	return platform_driver_register(&tegra_camera_driver);
+}
+
+static void __exit tegra_camera_exit(void)
+{
+	platform_driver_unregister(&tegra_camera_driver);
+}
+
+module_init(tegra_camera_init);
+module_exit(tegra_camera_exit);
+
diff --git a/drivers/media/video/tegra/tegra_dtv.c b/drivers/media/video/tegra/tegra_dtv.c
new file mode 100644
index 000000000000..f0b9a0a33a33
--- /dev/null
+++ b/drivers/media/video/tegra/tegra_dtv.c
@@ -0,0 +1,1087 @@
+/*
+ * tegra_dtv.c - Tegra DTV interface driver
+ *
+ * Author: Adam Jiang <chaoj@nvidia.com>
+ * Copyright (c) 2011, NVIDIA Corporation.
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms 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., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/fs.h>
+#include <linux/completion.h>
+#include <linux/dma-mapping.h>
+#include <linux/vmalloc.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+#include <linux/wakelock.h>
+#include <linux/platform_device.h>
+#include <linux/miscdevice.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/delay.h>
+
+#include <media/tegra_dtv.h>
+
+#include <linux/uaccess.h>
+#include <mach/iomap.h>
+#include <mach/dma.h>
+
+/* offsets from TEGRA_DTV_BASE */
+#define DTV_SPI_CONTROL         0x40
+#define DTV_MODE                0x44
+#define DTV_CTRL                0x48
+#define DTV_PACKET_COUNT        0x4c
+#define DTV_ERROR_COUNT         0x50
+#define DTV_INTERRUPT_STATUS    0x54
+#define DTV_STATUS              0x58
+#define DTV_RX_FIFO             0x5c
+
+/* DTV_SPI_CONTROL */
+#define DTV_SPI_CONTROL_ENABLE_DTV  1
+
+/* DTV_MODE_0 */
+#define DTV_MODE_BYTE_SWIZZLE_SHIFT 6
+#define DTV_MODE_BYTE_SWIZZLE       (1 << DTV_MODE_BYTE_SWIZZLE_SHIFT)
+#define DTV_MODE_BIT_SWIZZLE_SHIFT  5
+#define DTV_MODE_BIT_SWIZZLE        (1 << DTV_MODE_BIT_SWIZZLE_SHIFT)
+#define DTV_MODE_CLK_EDGE_SHIFT     4
+#define DTV_MODE_CLK_EDGE_MASK      1
+#define DTV_MODE_CLK_EDGE_NEG       (1 << DTV_MODE_CLK_EDGE_SHIFT)
+#define DTV_MODE_PRTL_SEL_SHIFT     2
+#define DTV_MODE_PRTL_SEL_MASK      (0x3 << DTV_MODE_PRTL_SEL_SHIFT)
+#define DTV_MODE_CLK_MODE_SHIFT     1
+#define DTV_MODE_CLK_MODE_MASK      (0x1 << DTV_MODE_CLK_MODE_SHIFT)
+#define DTV_MODE_PRTL_ENABLE        1
+
+/* DTV_CONTROL_0 */
+#define DTV_CTRL_FEC_SIZE_SHIFT         24
+#define DTV_CTRL_FEC_SIZE_MASK          (0x7F << DTV_CTRL_FEC_SIZE_SHIFT)
+#define DTV_CTRL_BODY_SIZE_SHIFT        16
+#define DTV_CTRL_BODY_SIZE_MASK         (0xFF << DTV_CTRL_BODY_SIZE_SHIFT)
+#define DTV_CTRL_FIFO_ATTN_LEVEL_SHIFT  8
+#define DTV_CTRL_FIFO_ATTN_LEVEL_MASK   (0x1F << DTV_CTRL_FIFO_ATTN_LEVEL_SHIFT)
+#define DTV_CTRL_FIFO_ATTN_ONE_WORD     (0 << DTV_CTRL_FIFO_ATTN_LEVEL_SHIFT)
+#define DTV_CTRL_FIFO_ATTN_TWO_WORD     (1 << DTV_CTRL_FIFO_ATTN_LEVEL_SHIFT)
+#define DTV_CTRL_FIFO_ATTN_THREE_WORD   (2 << DTV_CTRL_FIFO_ATTN_LEVEL_SHIFT)
+#define DTV_CTRL_FIFO_ATTN_FOUR_WORD    (3 << DTV_CTRL_FIFO_ATTN_LEVEL_SHIFT)
+#define DTV_CTRL_BODY_VALID_SEL_SHIFT   6
+#define DTV_CTRL_BODY_VALID_SEL_MASK    (1 << DTV_CTRL_BODY_VALID_SEL_SHIFT)
+#define DTV_CTRL_START_SEL_SHIFT        4
+#define DTV_CTRL_START_SEL_MASK         (1 << DTV_CTRL_START_SEL_SHIFT)
+#define DTV_CTRL_ERROR_POLARITY_SHIFT   2
+#define DTV_CTRL_ERROR_POLARITY_MASK    (1 << DTV_CTRL_ERROR_POLARITY_SHIFT)
+#define DTV_CTRL_PSYNC_POLARITY_SHIFT   1
+#define DTV_CTRL_PSYNC_POLARITY_MASK    (1 << DTV_CTRL_PSYNC_POLARITY_SHIFT)
+#define DTV_CTRL_VALID_POLARITY_SHIFT   0
+#define DTV_CTRL_VALID_POLARITY_MASK    (1 << DTV_CTRL_VALID_POLARITY_SHIFT)
+
+/* DTV_INTERRUPT_STATUS_0 */
+#define DTV_INTERRUPT_PACKET_UNDERRUN_ERR 8
+#define DTV_INTERRUPT_BODY_OVERRUN_ERR    4
+#define DTV_INTERRUPT_BODY_UNDERRUN_ERR   2
+#define DTV_INTERRUPT_UPSTREAM_ERR        1
+
+/* DTV_STATUS_0 */
+#define DTV_STATUS_RXF_UNDERRUN 4
+#define DTV_STATUS_RXF_EMPTY    2
+#define DTV_STATUS_RXF_FULL     1
+
+#define TEGRA_DTV_NAME "tegra_dtv"
+
+/* default sw config */
+#define DTV_BUF_SIZE_ORDER                PAGE_SHIFT
+#define DTV_MAX_NUM_BUFS                  4
+
+#define DTV_FIFO_ATN_LVL_LOW_GEAR         0
+#define DTV_FIFO_ATN_LVL_SECOND_GEAR      1
+#define DTV_FIFO_ATN_LVL_THIRD_GEAR       2
+#define DTV_FIFO_ATN_LVL_TOP_GEAR         3
+
+struct dtv_stream {
+	struct mutex	mtx;
+
+	bool			 xferring;	/* is DMA in progress */
+	unsigned		 num_bufs;
+	void			*buffer[DTV_MAX_NUM_BUFS];
+	dma_addr_t		 buf_phy[DTV_MAX_NUM_BUFS];
+	struct completion	 comp[DTV_MAX_NUM_BUFS];
+	struct tegra_dma_req	 dma_req[DTV_MAX_NUM_BUFS];
+	int			 last_queued;
+
+	int	fifo_atn_level;
+
+	struct tegra_dma_channel	*dma_chan;
+	bool				 stopped;
+	struct completion		 stop_completion;
+	spinlock_t			 dma_req_lock;
+	size_t				 buf_size;
+
+	struct work_struct	work;
+	struct wake_lock	wake_lock;
+	char			wake_lock_name[16];
+};
+
+struct tegra_dtv_context {
+	struct tegra_dtv_hw_config config;
+	struct clk                *clk;
+	int                        clk_enabled;
+
+	phys_addr_t                phys;
+	void * __iomem base;
+	unsigned long              dma_req_sel;
+
+	struct dtv_stream          stream;
+	/* debugfs */
+	struct dentry             *d;
+	/* for refer back */
+	struct platform_device    *pdev;
+	struct miscdevice          miscdev;
+};
+
+static inline struct tegra_dtv_context *to_ctx(struct dtv_stream *s)
+{
+	return container_of(s, struct tegra_dtv_context, stream);
+}
+
+/* access control */
+static atomic_t tegra_dtv_instance_nr = ATOMIC_INIT(1);
+
+static inline u32 tegra_dtv_readl(struct tegra_dtv_context *dtv,
+					    unsigned long reg)
+{
+	BUG_ON(!dtv->clk_enabled);
+	return readl(dtv->base + reg);
+}
+
+static inline void tegra_dtv_writel(struct tegra_dtv_context *dtv,
+				    u32 val, unsigned long reg)
+{
+	BUG_ON(!dtv->clk_enabled);
+	writel(val, dtv->base + reg);
+}
+
+/* process */
+static inline void prevent_suspend(struct dtv_stream *s)
+{
+	pr_debug("%s called.\n", __func__);
+	cancel_work_sync(&s->work);
+	wake_lock(&s->wake_lock);
+}
+
+static void tegra_dtv_worker(struct work_struct *w)
+{
+	struct dtv_stream *s = container_of(w, struct dtv_stream, work);
+	pr_debug("%s called.\n", __func__);
+	wake_unlock(&s->wake_lock);
+}
+
+static inline void wakeup_suspend(struct dtv_stream *s)
+{
+	schedule_work(&s->work);
+}
+
+static inline bool wait_till_stopped(struct dtv_stream *s)
+{
+	int ret;
+
+	pr_debug("%s: wait for completion\n", __func__);
+
+	ret = wait_for_completion_timeout(
+		&s->stop_completion, HZ);
+	if (!ret)
+		pr_err("%s: wait timed out", __func__);
+	if (ret < 0)
+		pr_err("%s: wait error %d\n", __func__, ret);
+
+	wakeup_suspend(s);
+
+	pr_debug("%s: done: %d\n", __func__, ret);
+
+	return true;
+}
+
+/* dma transfer */
+static inline bool are_xfers_pending(struct dtv_stream *s)
+{
+	int i;
+
+	pr_debug("%s called\n", __func__);
+
+	for (i = 0; i < s->num_bufs; i++)
+		if (!completion_done(&s->comp[i]))
+			return true;
+	return false;
+}
+
+static void tegra_dtv_rx_dma_complete(struct tegra_dma_req *req)
+{
+	unsigned long flags;
+	unsigned req_num;
+	struct dtv_stream *s = req->dev;
+
+	spin_lock_irqsave(&s->dma_req_lock, flags);
+
+	pr_debug("%s called.\n", __func__);
+
+	req_num = req - s->dma_req;
+	pr_debug("%s: complete buffer %d size %d bytes\n",
+		 __func__, req_num, req->bytes_transferred);
+	BUG_ON(req_num >= s->num_bufs);
+
+	complete(&s->comp[req_num]);
+
+	if (!are_xfers_pending(s))
+		pr_debug("%s: overflow.\n", __func__);
+
+	spin_unlock_irqrestore(&s->dma_req_lock, flags);
+}
+
+/* hw */
+static inline void _dtv_enable_protocol(struct tegra_dtv_context *dtv_ctx)
+{
+	u32 val;
+
+	val = tegra_dtv_readl(dtv_ctx, DTV_MODE);
+	val &= ~0x01;
+	val |= DTV_MODE_PRTL_ENABLE;
+	tegra_dtv_writel(dtv_ctx, val, DTV_MODE);
+}
+
+static inline void _dtv_disable_protocol(struct tegra_dtv_context *dtv_ctx)
+{
+	u32 val;
+
+	val = tegra_dtv_readl(dtv_ctx, DTV_MODE);
+	val &= ~DTV_MODE_PRTL_ENABLE;
+	tegra_dtv_writel(dtv_ctx, val, DTV_MODE);
+}
+
+static inline u32 _dtv_get_status(struct tegra_dtv_context *dtv_ctx)
+{
+	return tegra_dtv_readl(dtv_ctx, DTV_STATUS);
+}
+
+static inline void _dtv_set_attn_level(struct tegra_dtv_context *dtv_ctx)
+{
+	/* TODO: consider have this set to corresponding transfer request */
+	u32 val;
+
+	val = tegra_dtv_readl(dtv_ctx, DTV_CTRL);
+	val &= ~DTV_CTRL_FIFO_ATTN_LEVEL_MASK;
+	val |= DTV_CTRL_FIFO_ATTN_FOUR_WORD;
+	tegra_dtv_writel(dtv_ctx, val, DTV_CTRL);
+}
+
+/* ioctl */
+static inline void _dtv_set_hw_params(struct tegra_dtv_context *dtv_ctx)
+{
+	u32 val = 0;
+	u32 reg;
+	struct tegra_dtv_hw_config *cfg = &dtv_ctx->config;
+
+	val = (cfg->clk_edge << DTV_MODE_CLK_EDGE_SHIFT) |
+		(cfg->protocol_sel << DTV_MODE_PRTL_SEL_SHIFT) |
+		(cfg->clk_mode << DTV_MODE_CLK_MODE_SHIFT);
+	reg = tegra_dtv_readl(dtv_ctx, DTV_MODE);
+	reg &= ~(DTV_MODE_CLK_EDGE_MASK |
+		 DTV_MODE_PRTL_SEL_MASK |
+		 DTV_MODE_CLK_MODE_MASK);
+	reg |= val;
+	tegra_dtv_writel(dtv_ctx, reg, DTV_MODE);
+
+	val = 0;
+	reg = 0;
+	val = (cfg->fec_size << DTV_CTRL_FEC_SIZE_SHIFT) |
+		(cfg->body_size << DTV_CTRL_BODY_SIZE_SHIFT) |
+		(cfg->body_valid_sel << DTV_CTRL_BODY_VALID_SEL_SHIFT) |
+		(cfg->start_sel << DTV_CTRL_START_SEL_SHIFT) |
+		(cfg->err_pol << DTV_CTRL_ERROR_POLARITY_SHIFT) |
+		(cfg->psync_pol << DTV_CTRL_PSYNC_POLARITY_SHIFT) |
+		(cfg->valid_pol << DTV_CTRL_VALID_POLARITY_SHIFT);
+	reg = tegra_dtv_readl(dtv_ctx, DTV_CTRL);
+	reg &= ~(DTV_CTRL_FEC_SIZE_MASK |
+		 DTV_CTRL_BODY_SIZE_MASK |
+		 DTV_CTRL_BODY_VALID_SEL_MASK |
+		 DTV_CTRL_START_SEL_MASK |
+		 DTV_CTRL_ERROR_POLARITY_MASK |
+		 DTV_CTRL_PSYNC_POLARITY_MASK |
+		 DTV_CTRL_VALID_POLARITY_MASK);
+	reg |= val;
+	tegra_dtv_writel(dtv_ctx, reg, DTV_CTRL);
+}
+
+#define DTV_GET_REG_VAL(x, reg, seg)				\
+	((x & reg##_##seg##_MASK) >> reg##_##seg##_SHIFT)
+
+static inline void _dtv_get_hw_params(struct tegra_dtv_context *dtv_ctx,
+				      struct tegra_dtv_hw_config *cfg)
+{
+	u32 reg;
+
+	reg = tegra_dtv_readl(dtv_ctx, DTV_MODE);
+	cfg->clk_edge = DTV_GET_REG_VAL(reg, DTV_MODE, CLK_EDGE);
+	cfg->protocol_sel = DTV_GET_REG_VAL(reg, DTV_MODE, PRTL_SEL);
+	cfg->clk_mode = DTV_GET_REG_VAL(reg, DTV_MODE, CLK_MODE);
+
+	reg = tegra_dtv_readl(dtv_ctx, DTV_CTRL);
+	cfg->fec_size = DTV_GET_REG_VAL(reg, DTV_CTRL, FEC_SIZE);
+	cfg->body_size = DTV_GET_REG_VAL(reg, DTV_CTRL, BODY_SIZE);
+	cfg->body_valid_sel = DTV_GET_REG_VAL(reg, DTV_CTRL, BODY_VALID_SEL);
+	cfg->start_sel = DTV_GET_REG_VAL(reg, DTV_CTRL, START_SEL);
+	cfg->err_pol = DTV_GET_REG_VAL(reg, DTV_CTRL, ERROR_POLARITY);
+	cfg->psync_pol = DTV_GET_REG_VAL(reg, DTV_CTRL, PSYNC_POLARITY);
+	cfg->valid_pol = DTV_GET_REG_VAL(reg, DTV_CTRL, VALID_POLARITY);
+}
+
+/* must call with stream->dma_req_lock held. */
+static int stop_xfer_unsafe(struct dtv_stream *s)
+{
+	int spin = 0;
+	struct tegra_dtv_context *dtv_ctx = to_ctx(s);
+
+	pr_debug("%s called\n", __func__);
+	tegra_dma_cancel(s->dma_chan);
+	_dtv_disable_protocol(dtv_ctx);
+	while ((_dtv_get_status(dtv_ctx) & DTV_STATUS_RXF_FULL) &&
+	       spin < 100) {
+		udelay(10);
+		if (spin++ > 50)
+			pr_info("%s : spin %d\n", __func__, spin);
+	}
+	if (spin == 100)
+		pr_warn("%s : spinny\n", __func__);
+
+	return 0;
+}
+
+/* must call with stream->mtx held */
+static void __force_xfer_stop(struct dtv_stream *s)
+{
+	int i;
+
+	pr_debug("%s called.\n", __func__);
+
+	if (!s->stopped) {
+		s->stopped = true;
+		if (are_xfers_pending(s))
+			wait_till_stopped(s);
+		for (i = 0; i < s->num_bufs; i++) {
+			init_completion(&s->comp[i]);
+			complete(&s->comp[i]);
+		}
+	}
+
+	tegra_dma_cancel(s->dma_chan);
+	s->xferring = false;
+
+	pr_debug("%s: done\n", __func__);
+}
+
+static long tegra_dtv_ioctl(struct file *file, unsigned int cmd,
+			    unsigned long arg)
+{
+	int ret = 0;
+	struct tegra_dtv_context *dtv_ctx;
+	struct dtv_stream *s;
+
+	dtv_ctx = (struct tegra_dtv_context *) file->private_data;
+	s = &dtv_ctx->stream;
+
+	/* process may sleep on this */
+	mutex_lock(&s->mtx);
+
+	switch (cmd) {
+	case TEGRA_DTV_IOCTL_START:
+		pr_debug("%s: run serial ts handling.\n", __func__);
+		s->stopped = false;
+		break;
+	case TEGRA_DTV_IOCTL_STOP:
+		pr_debug("%s: stop serial ts handling.\n", __func__);
+		if (s->xferring) {
+			stop_xfer_unsafe(s);
+			complete(&s->stop_completion);
+			__force_xfer_stop(s);
+			s->stopped = true;
+		}
+		break;
+	case TEGRA_DTV_IOCTL_SET_HW_CONFIG:
+	{
+		struct tegra_dtv_hw_config cfg;
+
+		if (s->xferring) {
+			pr_err("%s: tranfering is in progress.\n", __func__);
+			ret = -EBUSY;
+			break;
+		}
+
+		if (copy_from_user(&cfg, (const void __user  *) arg,
+				   sizeof(cfg))) {
+			ret = -EFAULT;
+			break;
+		}
+
+		dtv_ctx->config = cfg;
+		_dtv_set_hw_params(dtv_ctx);
+		break;
+	}
+	case TEGRA_DTV_IOCTL_GET_HW_CONFIG:
+	{
+		struct tegra_dtv_hw_config cfg;
+
+		_dtv_get_hw_params(dtv_ctx, &cfg);
+
+		if (copy_to_user((void __user  *)arg, &cfg,
+				 sizeof(cfg)))
+			ret = -EFAULT;
+		break;
+	}
+	default:
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&s->mtx);
+
+	return ret;
+}
+
+/* must call with stream->dma_req_lock held. */
+static int start_xfer_unsafe(struct dtv_stream *s, size_t size)
+{
+	int i;
+	u32 reg;
+	struct tegra_dtv_context *dtv_ctx = to_ctx(s);
+
+	BUG_ON(are_xfers_pending(s));
+
+	pr_debug("%s called.\n", __func__);
+
+	for (i = 0; i < s->num_bufs; i++) {
+		init_completion(&s->comp[i]);
+		s->dma_req[i].dest_addr = s->buf_phy[i];
+		s->dma_req[i].size = size;
+		tegra_dma_enqueue_req(s->dma_chan, &s->dma_req[i]);
+	}
+
+	s->last_queued = s->num_bufs - 1;
+
+	/* too late ? */
+	_dtv_set_attn_level(dtv_ctx);
+	_dtv_enable_protocol(dtv_ctx);
+
+	reg = tegra_dtv_readl(dtv_ctx, DTV_MODE);
+	pr_debug("DTV_MODE = 0x%08x\n", reg);
+
+	return 0;
+}
+
+static int try_start_fill_buf(struct dtv_stream *s, size_t size)
+{
+	int ret = 0;
+	unsigned long flags;
+
+	pr_debug("%s called\n", __func__);
+
+	prevent_suspend(s);
+
+	spin_lock_irqsave(&s->dma_req_lock, flags);
+	if (!s->stopped && !are_xfers_pending(s)) {
+		ret = start_xfer_unsafe(s, size);
+		if (ret) {
+			pr_err("%s: start tranfer failed.\n", __func__);
+			/* let process not wait stupid */
+			wakeup_suspend(s);
+		}
+	}
+	spin_unlock_irqrestore(&s->dma_req_lock, flags);
+
+	return ret;
+}
+
+static ssize_t tegra_dtv_read(struct file *file, char __user *buf,
+			      size_t size, loff_t *off)
+{
+	ssize_t ret;
+	ssize_t xfer_size = 0;
+	int     buf_no;
+	struct tegra_dma_req *req;
+	struct tegra_dtv_context *dtv_ctx;
+
+	dtv_ctx = (struct tegra_dtv_context *) file->private_data;
+
+	mutex_lock(&dtv_ctx->stream.mtx);
+
+	if (!IS_ALIGNED(size, 4) || size < 4 ||
+	    size > dtv_ctx->stream.buf_size) {
+		pr_err("%s: invalid user size %d\n", __func__, size);
+		ret = -EINVAL;
+		mutex_unlock(&dtv_ctx->stream.mtx);
+		return ret;
+	}
+
+	pr_debug("%s: read %d bytes.\n", __func__, size);
+
+	if (dtv_ctx->stream.stopped) {
+		pr_debug("%s: tegra dtv transferring is stopped.\n",
+			 __func__);
+		ret = 0;
+		mutex_unlock(&dtv_ctx->stream.mtx);
+		return ret;
+	}
+
+	/* start dma transfer */
+	ret = try_start_fill_buf(&dtv_ctx->stream, size);
+	if (ret < 0 && ret != -EALREADY) {
+		pr_err("%s: could not start recording.\n", __func__);
+		mutex_unlock(&dtv_ctx->stream.mtx);
+		return ret;
+	}
+	dtv_ctx->stream.xferring = true;
+
+	buf_no = (dtv_ctx->stream.last_queued + 1) % dtv_ctx->stream.num_bufs;
+	pr_debug("%s: buf_no = %d\n", __func__, buf_no);
+
+	/* Wait for the buffers to be filled up. The maximum timeout
+	  *value should be caculated dynamically based on
+	 * buf_size(dtv_ctx->stream).buf_size. For isdb-t 1seg signal,
+	  *it bit rate is 300 - 456 kpbs, if buf_size = 4096 bytes, then
+	 * to fill up one buffer takes ~77ms.
+	 */
+	ret = wait_for_completion_interruptible_timeout(
+		&dtv_ctx->stream.comp[buf_no], HZ);
+	if (!ret) {
+		pr_err("%s: timeout", __func__);
+		ret = -ETIMEDOUT;
+		mutex_unlock(&dtv_ctx->stream.mtx);
+		return ret;
+	} else if (ret < 0) {
+		pr_err("%s: wait error %d", __func__, ret);
+		mutex_unlock(&dtv_ctx->stream.mtx);
+		return ret;
+	}
+
+	req = &dtv_ctx->stream.dma_req[buf_no];
+
+	/* xfer cannot exceed buffer size */
+	xfer_size = size > req->size ? req->size : size;
+	req->size = size;
+	dma_sync_single_for_cpu(NULL,
+				dtv_ctx->stream.dma_req[buf_no].dest_addr,
+				dtv_ctx->stream.dma_req[buf_no].size,
+				DMA_FROM_DEVICE);
+	ret = copy_to_user(buf, dtv_ctx->stream.buffer[buf_no], xfer_size);
+	if (ret) {
+		ret = -EFAULT;
+		mutex_unlock(&dtv_ctx->stream.mtx);
+		return ret;
+	}
+
+	/* not stopped, reinitial stop */
+	init_completion(&dtv_ctx->stream.stop_completion);
+
+	dtv_ctx->stream.last_queued = buf_no;
+
+	/* refill copied buffer */
+	ret = tegra_dma_enqueue_req(dtv_ctx->stream.dma_chan, req);
+	BUG_ON(ret);
+
+	ret = xfer_size;
+	*off += xfer_size;
+
+	mutex_unlock(&dtv_ctx->stream.mtx);
+
+	pr_debug("%s : done with ret = %d\n", __func__, ret);
+
+	return ret;
+}
+
+static int tegra_dtv_open(struct inode *inode, struct file *file)
+{
+	int i;
+	struct miscdevice *miscdev = file->private_data;
+	struct tegra_dtv_context *dtv_ctx =
+		container_of(miscdev, struct tegra_dtv_context, miscdev);
+	file->private_data = dtv_ctx;
+
+	dtv_ctx = (struct tegra_dtv_context *) file->private_data;
+
+	pr_debug("%s called\n", __func__);
+
+	/* can be opened once */
+	if (!atomic_dec_and_test(&tegra_dtv_instance_nr)) {
+		atomic_inc(&tegra_dtv_instance_nr);
+		pr_err("tegra_dtv device can only be opened once.\n");
+		return -EBUSY;
+	}
+
+	mutex_lock(&dtv_ctx->stream.mtx);
+
+	dtv_ctx->stream.stopped = false;
+
+	/* cleanup completion */
+	for (i = 0; i < dtv_ctx->stream.num_bufs; i++) {
+		init_completion(&dtv_ctx->stream.comp[i]);
+		/* complete all */
+		complete(&dtv_ctx->stream.comp[i]);
+	}
+
+	mutex_unlock(&dtv_ctx->stream.mtx);
+
+	return 0;
+}
+
+static int tegra_dtv_release(struct inode *inode, struct file *file)
+{
+	struct tegra_dtv_context *dtv_ctx =
+		(struct tegra_dtv_context *) file->private_data;
+
+	pr_debug("%s called\n", __func__);
+
+	atomic_inc(&tegra_dtv_instance_nr);
+
+	mutex_lock(&dtv_ctx->stream.mtx);
+	if (dtv_ctx->stream.xferring) {
+		stop_xfer_unsafe(&dtv_ctx->stream);
+		/* clean up stop condition */
+		complete(&dtv_ctx->stream.stop_completion);
+		__force_xfer_stop(&dtv_ctx->stream);
+	}
+	/* wakeup any pending process */
+	wakeup_suspend(&dtv_ctx->stream);
+	mutex_unlock(&dtv_ctx->stream.mtx);
+
+	pr_debug("%s : done\n", __func__);
+
+	return 0;
+}
+
+static const struct file_operations tegra_dtv_fops = {
+	.owner = THIS_MODULE,
+	.open = tegra_dtv_open,
+	.read = tegra_dtv_read,
+	.unlocked_ioctl = tegra_dtv_ioctl,
+	.release = tegra_dtv_release,
+};
+
+#ifdef CONFIG_DEBUG_FS
+static int dtv_reg_show(struct seq_file *s, void *unused)
+{
+	struct tegra_dtv_context *dtv_ctx = s->private;
+
+	seq_printf(s, "tegra_dtv register list\n");
+	seq_printf(s, "-------------------------------\n");
+	seq_printf(s, "DTV_SPI_CONTROL_0: 0x%08x\n",
+		   tegra_dtv_readl(dtv_ctx, DTV_SPI_CONTROL));
+	seq_printf(s, "DTV_MODE_0:        0x%08x\n",
+		   tegra_dtv_readl(dtv_ctx, DTV_MODE));
+	seq_printf(s, "DTV_CONTROL:       0x%08x\n",
+		   tegra_dtv_readl(dtv_ctx, DTV_CTRL));
+	seq_printf(s, "DTV_FIFO:          0x%08x\n",
+		   tegra_dtv_readl(dtv_ctx, DTV_RX_FIFO));
+
+	return 0;
+
+}
+
+static int dtv_debugfs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dtv_reg_show, inode->i_private);
+}
+
+static const struct file_operations dtv_debugfs_fops = {
+	.open = dtv_debugfs_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static int dtv_debugfs_init(struct tegra_dtv_context *dtv_ctx)
+{
+	struct dentry *d;
+
+	d = debugfs_create_file("tegra_dtv", S_IRUGO, NULL, dtv_ctx,
+				&dtv_debugfs_fops);
+	if (!d)
+		return -ENOMEM;
+
+	dtv_ctx->d = d;
+
+	return 0;
+}
+
+static void dtv_debugfs_exit(struct tegra_dtv_context *dtv_ctx)
+{
+	debugfs_remove(dtv_ctx->d);
+}
+#else
+static int dtv_debugfs_init(struct tegra_dtv_context *dtv_ctx) { return 0; }
+static void dtv_debugfs_exit(struct tegra_dtv_context *dtv_ctx) {};
+#endif
+
+static void setup_dma_rx_request(struct tegra_dma_req *req,
+			    struct dtv_stream *s)
+{
+	struct tegra_dtv_context *dtv_ctx;
+
+	pr_debug("%s before to_ctx\n", __func__);
+	dtv_ctx = to_ctx(s);
+
+	pr_debug("%s called\n", __func__);
+
+	memset(req, 0, sizeof(*req));
+
+	req->complete = tegra_dtv_rx_dma_complete;
+	req->dev = s;
+	req->to_memory = true;
+	req->req_sel = TEGRA_DMA_REQ_SEL_DTV;
+
+	req->source_addr = dtv_ctx->phys + DTV_RX_FIFO;
+	req->source_wrap = 4;
+	req->source_bus_width = 32;
+	req->fixed_burst_size = 1;
+
+	req->dest_wrap = 0;
+	req->dest_bus_width = 32;
+}
+
+static int setup_dma(struct tegra_dtv_context *dtv_ctx)
+{
+	int ret = 0;
+	int i;
+
+	pr_debug("%s called\n", __func__);
+
+	for (i = 0; i < dtv_ctx->stream.num_bufs; i++) {
+		dtv_ctx->stream.buf_phy[i] = dma_map_single(
+			&dtv_ctx->pdev->dev,
+			dtv_ctx->stream.buffer[i],
+			dtv_ctx->stream.buf_size,
+			DMA_FROM_DEVICE);
+		BUG_ON(!dtv_ctx->stream.buf_phy[i]);
+		setup_dma_rx_request(&dtv_ctx->stream.dma_req[i],
+				     &dtv_ctx->stream);
+		dtv_ctx->stream.dma_req[i].dest_addr =
+			dtv_ctx->stream.buf_phy[i];
+	}
+	dtv_ctx->stream.dma_chan = tegra_dma_allocate_channel(
+		TEGRA_DMA_MODE_CONTINUOUS_SINGLE,
+		"tegra_dtv_rx", dtv_ctx->dma_req_sel);
+	if (!dtv_ctx->stream.dma_chan) {
+		pr_err("%s : cannot allocate input DMA channel: %ld\n",
+		       __func__, PTR_ERR(dtv_ctx->stream.dma_chan));
+		ret = -ENODEV;
+		/* release */
+		for (i = 0; i < dtv_ctx->stream.num_bufs; i++) {
+			dma_unmap_single(&dtv_ctx->pdev->dev,
+					 dtv_ctx->stream.buf_phy[i],
+					 1 << DTV_BUF_SIZE_ORDER,
+					 DMA_FROM_DEVICE);
+			dtv_ctx->stream.buf_phy[i] = 0;
+		}
+		tegra_dma_free_channel(dtv_ctx->stream.dma_chan);
+		dtv_ctx->stream.dma_chan = 0;
+
+		return ret;
+	}
+
+	return ret;
+}
+
+static void tear_down_dma(struct tegra_dtv_context *dtv_ctx)
+{
+	int i;
+
+	pr_debug("%s called\n", __func__);
+
+	for (i = 0; i < dtv_ctx->stream.num_bufs; i++) {
+		dma_unmap_single(&dtv_ctx->pdev->dev,
+				 dtv_ctx->stream.buf_phy[i],
+				 1 << DTV_BUF_SIZE_ORDER,
+				 DMA_FROM_DEVICE);
+		dtv_ctx->stream.buf_phy[i] = 0;
+	}
+	tegra_dma_free_channel(dtv_ctx->stream.dma_chan);
+	dtv_ctx->stream.dma_chan = 0;
+}
+
+static int init_stream_buffer(struct dtv_stream *s, unsigned num)
+{
+	int ret;
+	int i, j;
+
+	pr_debug("%s (num %d)\n", __func__, num);
+
+	for (i = 0; i < num; i++) {
+		kfree(s->buffer[i]);
+		s->buffer[i] = kmalloc((1 << DTV_BUF_SIZE_ORDER),
+				       GFP_KERNEL | GFP_DMA);
+		if (!s->buffer[i]) {
+			pr_err("%s : cannot allocate buffer.\n", __func__);
+			for (j = i - 1; j >= 0; j--) {
+				kfree(s->buffer[j]);
+				s->buffer[j] = 0;
+			}
+			ret = -ENOMEM;
+			return ret;
+		}
+	}
+	return 0;
+}
+
+static void release_stream_buffer(struct dtv_stream *s, unsigned num)
+{
+	int i;
+
+	pr_debug("%s (num %d)\n", __func__, num);
+
+	for (i = 0; i < num; i++) {
+		kfree(s->buffer[i]);
+		s->buffer[i] = 0;
+	}
+}
+
+static int setup_stream(struct dtv_stream *stream)
+{
+	int ret = 0;
+	int i;
+
+	pr_debug("%s called\n", __func__);
+
+	stream->xferring = false;
+	mutex_init(&stream->mtx);
+	init_completion(&stream->stop_completion);
+	spin_lock_init(&stream->dma_req_lock);
+	stream->dma_chan = NULL;
+	stream->fifo_atn_level = DTV_FIFO_ATN_LVL_TOP_GEAR;
+	stream->buf_size = 1 << DTV_BUF_SIZE_ORDER;
+	stream->num_bufs = DTV_MAX_NUM_BUFS;
+	/* init each buffer */
+	for (i = 0; i < stream->num_bufs; i++) {
+		init_completion(&stream->comp[i]);
+		/* complete all at this moment */
+		complete(&stream->comp[i]);
+		stream->buffer[i] = 0;
+		stream->buf_phy[i] = 0;
+	}
+	stream->last_queued = 0;
+	ret = init_stream_buffer(stream, stream->num_bufs);
+	if (ret < 0)
+		return ret;
+
+	INIT_WORK(&stream->work, tegra_dtv_worker);
+	wake_lock_init(&stream->wake_lock, WAKE_LOCK_SUSPEND, "tegra_dtv");
+
+	return ret;
+}
+
+static int tegra_dtv_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct tegra_dtv_context *dtv_ctx;
+	struct clk *clk;
+	struct resource *res;
+
+	pr_info("%s: probing dtv.\n", __func__);
+
+	dtv_ctx = devm_kzalloc(&pdev->dev, sizeof(struct tegra_dtv_context),
+			       GFP_KERNEL);
+	if (!dtv_ctx) {
+		pr_err("%s: Failed to allocate memory for dtv context.\n",
+		       __func__);
+		ret = -ENOMEM;
+		return ret;
+	}
+	platform_set_drvdata(pdev, dtv_ctx);
+
+	/* for refer back */
+	dtv_ctx->pdev = pdev;
+
+	/* enable clk for dtv */
+	clk = clk_get(&pdev->dev, NULL);
+	if (!clk) {
+		dev_err(&pdev->dev, "cannot get clock for tegra_dtv.\n");
+		ret = -EIO;
+		goto fail_no_clk;
+	}
+	dtv_ctx->clk = clk;
+	ret = clk_enable(clk);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "cannot enable clk for tegra_dtv.\n");
+		goto fail_clk_enable;
+	}
+	dtv_ctx->clk_enabled = 1;
+
+	/* get resource */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (unlikely(!res)) {
+		pr_err("%s: Failed to get resource for dtv.\n",
+		       __func__);
+		ret = -ENODEV;
+		goto fail_no_res;
+	}
+
+	if (!devm_request_mem_region(&pdev->dev, res->start,
+			resource_size(res), dev_name(&pdev->dev))) {
+		ret = -EBUSY;
+		return ret;
+	}
+	dtv_ctx->phys = res->start;
+	dtv_ctx->base = devm_ioremap(&pdev->dev, res->start,
+				     resource_size(res));
+	if (!dtv_ctx->base) {
+		dev_err(&pdev->dev, "cannot ioremap iomem.\n");
+		ret = -ENOMEM;
+		return ret;
+	}
+
+	ret = setup_stream(&dtv_ctx->stream);
+	if (ret < 0)
+		goto fail_setup_stream;
+
+	ret = setup_dma(dtv_ctx);
+	if (ret < 0)
+		goto fail_setup_dma;
+
+	/* register as a misc device */
+	dtv_ctx->miscdev.minor = MISC_DYNAMIC_MINOR;
+	dtv_ctx->miscdev.name = TEGRA_DTV_NAME;
+	dtv_ctx->miscdev.fops = &tegra_dtv_fops;
+	ret = misc_register(&dtv_ctx->miscdev);
+	if (ret) {
+		pr_err("%s: Unable to register misc device.\n",
+		       __func__);
+		ret = -ENODEV;
+		goto fail_misc_reg;
+	}
+
+	ret = dtv_debugfs_init(dtv_ctx);
+	if (ret) {
+		pr_err("%s: Unable to register debugfs entry.\n",
+		       __func__);
+		goto fail_debugfs_reg;
+	}
+
+	return 0;
+
+fail_debugfs_reg:
+	dtv_debugfs_exit(dtv_ctx);
+fail_misc_reg:
+	misc_deregister(&dtv_ctx->miscdev);
+fail_setup_stream:
+fail_setup_dma:
+	tear_down_dma(dtv_ctx);
+fail_no_res:
+fail_clk_enable:
+fail_no_clk:
+	if (clk)
+		clk_put(clk);
+
+	return ret;
+}
+
+static int __devexit tegra_dtv_remove(struct platform_device *pdev)
+{
+	struct tegra_dtv_context *dtv_ctx;
+
+	pr_info("%s: remove dtv.\n", __func__);
+
+	dtv_ctx = platform_get_drvdata(pdev);
+
+	dtv_debugfs_exit(dtv_ctx);
+	tear_down_dma(dtv_ctx);
+	release_stream_buffer(&dtv_ctx->stream, dtv_ctx->stream.num_bufs);
+
+	clk_put(dtv_ctx->clk);
+
+	misc_deregister(&dtv_ctx->miscdev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int tegra_dtv_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct tegra_dtv_context *dtv_ctx;
+
+	pr_info("%s: suspend dtv.\n", __func__);
+
+	dtv_ctx = platform_get_drvdata(pdev);
+
+	/* stop xferring */
+	mutex_lock(&dtv_ctx->stream.mtx);
+	if (dtv_ctx->stream.xferring) {
+		stop_xfer_unsafe(&dtv_ctx->stream);
+		/* clean up stop condition */
+		complete(&dtv_ctx->stream.stop_completion);
+		__force_xfer_stop(&dtv_ctx->stream);
+	}
+	/* wakeup any pending process */
+	wakeup_suspend(&dtv_ctx->stream);
+	mutex_unlock(&dtv_ctx->stream.mtx);
+
+	clk_disable(dtv_ctx->clk);
+
+	return 0;
+}
+
+static int tegra_dtv_resume(struct platform_device *pdev)
+{
+	struct tegra_dtv_context *dtv_ctx;
+
+	pr_info("%s: resume dtv.\n", __func__);
+
+	dtv_ctx = platform_get_drvdata(pdev);
+	clk_enable(dtv_ctx->clk);
+
+	return 0;
+}
+#endif /* CONFIG_PM */
+
+static struct platform_driver tegra_dtv_driver = {
+	.driver = {
+		.name = TEGRA_DTV_NAME,
+		.owner = THIS_MODULE,
+	},
+	.probe = tegra_dtv_probe,
+	.remove = __devexit_p(tegra_dtv_remove),
+#ifdef CONFIG_PM
+	.suspend = tegra_dtv_suspend,
+	.resume = tegra_dtv_resume,
+#endif
+};
+
+static int __init tegra_dtv_init(void)
+{
+	return platform_driver_register(&tegra_dtv_driver);
+}
+
+static void __exit tegra_dtv_exit(void)
+{
+	platform_driver_unregister(&tegra_dtv_driver);
+}
+
+module_init(tegra_dtv_init);
+module_exit(tegra_dtv_exit);
+
+MODULE_AUTHOR("Adam Jiang <chaoj@nvidia.com>");
+MODULE_DESCRIPTION("Tegra DTV interface driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" TEGRA_DTV_NAME);
diff --git a/drivers/media/video/tegra/tps61050.c b/drivers/media/video/tegra/tps61050.c
new file mode 100644
index 000000000000..def353ca9c1a
--- /dev/null
+++ b/drivers/media/video/tegra/tps61050.c
@@ -0,0 +1,991 @@
+/*
+ * tps61050.c - tps61050 flash/torch kernel driver
+ *
+ * Copyright (C) 2011 NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms 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
+ */
+
+/* Implementation
+ * --------------
+ * The board level details about the device need to be provided in the board
+ * file with the tps61050_platform_data structure.
+ * Standard among NVC kernel drivers in this structure is:
+ * .cfg = Use the NVC_CFG_ defines that are in nvc_torch.h.
+ *        Descriptions of the configuration options are with the defines.
+ *        This value is typically 0.
+ * .num = The number of the instance of the device.  This should start at 1 and
+ *        and increment for each device on the board.  This number will be
+ *        appended to the MISC driver name, Example: /dev/tps61050.1
+ * .sync = If there is a need to synchronize two devices, then this value is
+ *         the number of the device instance this device is allowed to sync to.
+ *         This is typically used for stereo applications.
+ * .dev_name = The MISC driver name the device registers as.  If not used,
+ *             then the part number of the device is used for the driver name.
+ *             If using the NVC user driver then use the name found in this
+ *             driver under _default_pdata.
+ *
+ * The following is specific to NVC kernel flash/torch drivers:
+ * .pinstate = a pointer to the nvc_torch_pin_state structure.  This
+ *             structure gives the details of which VI GPIO to use to trigger
+ *             the flash.  The mask tells which pin and the values is the
+ *             level.  For example, if VI GPIO pin 6 is used, then
+ *             .mask = 0x0040
+ *             .values = 0x0040
+ *             If VI GPIO pin 0 is used, then
+ *             .mask = 0x0001
+ *             .values = 0x0001
+ *             This is typically just one pin but there is some legacy
+ *             here that insinuates more than one pin can be used.
+ *             When the flash level is set, then the driver will return the
+ *             value in values.  When the flash level is off, the driver will
+ *             return 0 for the values to deassert the signal.
+ *             If a VI GPIO is not used, then the mask and values must be set
+ *             to 0.  The flash may then be triggered via I2C instead.
+ *             However, a VI GPIO is strongly encouraged since it allows
+ *             tighter timing with the picture taken as well as reduced power
+ *             by asserting the trigger signal for only when needed.
+ * .max_amp_torch = Is the maximum torch value allowed.  The value is 0 to
+ *                  _MAX_TORCH_LEVEL.  This is to allow a limit to the amount
+ *                  of amps used.  If left blank then _MAX_TORCH_LEVEL will be
+ *                  used.
+ * .max_amp_flash = Is the maximum flash value allowed.  The value is 0 to
+ *                  _MAX_FLASH_LEVEL.  This is to allow a limit to the amount
+ *                  of amps used.  If left blank then _MAX_FLASH_LEVEL will be
+ *                  used.
+ *
+ * The following is specific to only this NVC kernel flash/torch driver:
+ * N/A
+ *
+ * Power Requirements
+ * The board power file must contain the following labels for the power
+ * regulator(s) of this device:
+ * "vdd_i2c" = the power regulator for the I2C power.
+ * Note that this device is typically connected directly to the battery rail
+ * and does not need a source power regulator (vdd).
+ *
+ * The above values should be all that is needed to use the device with this
+ * driver.  Modifications of this driver should not be needed.
+ */
+
+
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+#include <linux/list.h>
+#include <linux/regulator/consumer.h>
+#include <media/nvc.h>
+#include <media/tps61050.h>
+
+#define TPS61050_REG0		0x00
+#define TPS61050_REG1		0x01
+#define TPS61050_REG2		0x02
+#define TPS61050_REG3		0x03
+#define tps61050_flash_cap_size	(sizeof(tps61050_flash_cap.numberoflevels) \
+				+ (sizeof(tps61050_flash_cap.levels[0]) \
+				* (TPS61050_MAX_FLASH_LEVEL + 1)))
+#define tps61050_torch_cap_size	(sizeof(tps61050_torch_cap.numberoflevels) \
+				+ (sizeof(tps61050_torch_cap.guidenum[0]) \
+				* (TPS61050_MAX_TORCH_LEVEL + 1)))
+
+
+static struct nvc_torch_flash_capabilities tps61050_flash_cap = {
+	TPS61050_MAX_FLASH_LEVEL + 1,
+	{
+		{ 0,   0xFFFFFFFF, 0 },
+		{ 150, 558,        2 },
+		{ 200, 558,        2 },
+		{ 300, 558,        2 },
+		{ 400, 558,        2 },
+		{ 500, 558,        2 },
+		{ 700, 558,        2 },
+		{ 900, 558,        2 },
+		{ 900, 558,        2 }
+	}
+};
+
+static struct nvc_torch_torch_capabilities tps61050_torch_cap = {
+	TPS61050_MAX_TORCH_LEVEL + 1,
+	{
+		0,
+		50,
+		75,
+		100,
+		150,
+		200,
+		491,
+		491
+	}
+};
+
+struct tps61050_info {
+	atomic_t in_use;
+	struct i2c_client *i2c_client;
+	struct tps61050_platform_data *pdata;
+	struct miscdevice miscdev;
+	struct list_head list;
+	int pwr_api;
+	int pwr_dev;
+	struct nvc_regulator vreg_i2c;
+	u8 s_mode;
+	struct tps61050_info *s_info;
+};
+
+static struct nvc_torch_pin_state tps61050_default_pinstate = {
+	.mask		= 0x0000,
+	.values		= 0x0000,
+};
+
+static struct tps61050_platform_data tps61050_default_pdata = {
+	.cfg		= 0,
+	.num		= 0,
+	.sync		= 0,
+	.dev_name	= "torch",
+	.pinstate	= &tps61050_default_pinstate,
+	.max_amp_torch	= TPS61050_MAX_TORCH_LEVEL,
+	.max_amp_flash	= TPS61050_MAX_FLASH_LEVEL,
+};
+
+static LIST_HEAD(tps61050_info_list);
+static DEFINE_SPINLOCK(tps61050_spinlock);
+
+
+static int tps61050_i2c_rd(struct tps61050_info *info, u8 reg, u8 *val)
+{
+	struct i2c_msg msg[2];
+	u8 buf[2];
+
+	buf[0] = reg;
+	msg[0].addr = info->i2c_client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &buf[0];
+	msg[1].addr = info->i2c_client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = &buf[1];
+	*val = 0;
+	if (i2c_transfer(info->i2c_client->adapter, msg, 2) != 2)
+		return -EIO;
+
+	*val = buf[1];
+	return 0;
+}
+
+static int tps61050_i2c_wr(struct tps61050_info *info, u8 reg, u8 val)
+{
+	struct i2c_msg msg;
+	u8 buf[2];
+
+	buf[0] = reg;
+	buf[1] = val;
+	msg.addr = info->i2c_client->addr;
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = &buf[0];
+	if (i2c_transfer(info->i2c_client->adapter, &msg, 1) != 1)
+		return -EIO;
+
+	return 0;
+}
+
+static void tps61050_pm_regulator_put(struct nvc_regulator *sreg)
+{
+	regulator_put(sreg->vreg);
+	sreg->vreg = NULL;
+}
+
+static int tps61050_pm_regulator_get(struct tps61050_info *info,
+				     struct nvc_regulator *sreg,
+				     char vreg_name[])
+{
+	int err = 0;
+
+	sreg->vreg_flag = 0;
+	sreg->vreg = regulator_get(&info->i2c_client->dev, vreg_name);
+	if (IS_ERR_OR_NULL(sreg->vreg)) {
+		dev_err(&info->i2c_client->dev,
+				"%s err for regulator: %s err: %d\n",
+				__func__, vreg_name, (int)sreg->vreg);
+		err = PTR_ERR(sreg->vreg);
+		sreg->vreg = NULL;
+	} else {
+		sreg->vreg_name = vreg_name;
+		dev_dbg(&info->i2c_client->dev,
+				"%s vreg_name: %s\n",
+				__func__, sreg->vreg_name);
+	}
+	return err;
+}
+
+static int tps61050_pm_regulator_en(struct tps61050_info *info,
+				    struct nvc_regulator *sreg)
+{
+	int err = 0;
+
+	if (!sreg->vreg_flag && (sreg->vreg != NULL)) {
+		err = regulator_enable(sreg->vreg);
+		if (!err) {
+			dev_dbg(&info->i2c_client->dev,
+					"%s vreg_name: %s\n",
+					__func__, sreg->vreg_name);
+			sreg->vreg_flag = 1;
+			err = 1; /* flag regulator state change */
+			mdelay(5); /* device powerup delay */
+		} else {
+			dev_err(&info->i2c_client->dev,
+					"%s err, regulator: %s\n",
+					__func__, sreg->vreg_name);
+		}
+	}
+	return err;
+}
+
+static int tps61050_pm_regulator_dis(struct tps61050_info *info,
+				     struct nvc_regulator *sreg)
+{
+	int err = 0;
+
+	if (sreg->vreg_flag && (sreg->vreg != NULL)) {
+		err = regulator_disable(sreg->vreg);
+		if (!err)
+			dev_dbg(&info->i2c_client->dev,
+					"%s vreg_name: %s\n",
+					__func__, sreg->vreg_name);
+		else
+			dev_err(&info->i2c_client->dev,
+					"%s err, regulator: %s\n",
+					__func__, sreg->vreg_name);
+	}
+	sreg->vreg_flag = 0;
+	return err;
+}
+
+static int tps61050_pm_wr(struct tps61050_info *info, int pwr)
+{
+	int err = 0;
+	u8 reg;
+
+	if (pwr == info->pwr_dev)
+		return 0;
+
+	switch (pwr) {
+	case NVC_PWR_OFF:
+		if ((info->pdata->cfg & NVC_CFG_OFF2STDBY) ||
+			     (info->pdata->cfg & NVC_CFG_BOOT_INIT)) {
+			pwr = NVC_PWR_STDBY;
+		} else {
+			err = tps61050_pm_regulator_en(info, &info->vreg_i2c);
+			err |= tps61050_i2c_wr(info, TPS61050_REG0, 0x00);
+			err |= tps61050_i2c_wr(info, TPS61050_REG1, 0x00);
+			err |= tps61050_pm_regulator_dis(info, &info->vreg_i2c);
+			break;
+		}
+	case NVC_PWR_STDBY_OFF:
+		if ((info->pdata->cfg & NVC_CFG_OFF2STDBY) ||
+			     (info->pdata->cfg & NVC_CFG_BOOT_INIT)) {
+			pwr = NVC_PWR_STDBY;
+		} else {
+			err = tps61050_pm_regulator_en(info, &info->vreg_i2c);
+			err |= tps61050_i2c_wr(info, TPS61050_REG0, 0x00);
+			err |= tps61050_i2c_wr(info, TPS61050_REG1, 0x00);
+			break;
+		}
+	case NVC_PWR_STDBY:
+		err = tps61050_pm_regulator_en(info, &info->vreg_i2c);
+		err |= tps61050_i2c_rd(info, TPS61050_REG0, &reg);
+		reg &= 0x3F; /* 7:6 = mode */
+		err |= tps61050_i2c_wr(info, TPS61050_REG0, reg);
+		break;
+
+	case NVC_PWR_COMM:
+	case NVC_PWR_ON:
+		err = tps61050_pm_regulator_en(info, &info->vreg_i2c);
+		break;
+
+	default:
+		err = -EINVAL;
+		break;
+	}
+
+	if (err < 0) {
+		dev_err(&info->i2c_client->dev, "%s error\n", __func__);
+		pwr = NVC_PWR_ERR;
+	}
+	info->pwr_dev = pwr;
+	if (err > 0)
+		return 0;
+
+	return err;
+}
+
+static int tps61050_pm_wr_s(struct tps61050_info *info, int pwr)
+{
+	int err1 = 0;
+	int err2 = 0;
+
+	if ((info->s_mode == NVC_SYNC_OFF) ||
+			(info->s_mode == NVC_SYNC_MASTER) ||
+			(info->s_mode == NVC_SYNC_STEREO))
+		err1 = tps61050_pm_wr(info, pwr);
+	if ((info->s_mode == NVC_SYNC_SLAVE) ||
+			(info->s_mode == NVC_SYNC_STEREO))
+		err2 = tps61050_pm_wr(info->s_info, pwr);
+	return err1 | err2;
+}
+
+static int tps61050_pm_api_wr(struct tps61050_info *info, int pwr)
+{
+	int err = 0;
+
+	if (!pwr || (pwr > NVC_PWR_ON))
+		return 0;
+
+	if (pwr > info->pwr_dev)
+		err = tps61050_pm_wr_s(info, pwr);
+	if (!err)
+		info->pwr_api = pwr;
+	else
+		info->pwr_api = NVC_PWR_ERR;
+	if (info->pdata->cfg & NVC_CFG_NOERR)
+		return 0;
+
+	return err;
+}
+
+static int tps61050_pm_dev_wr(struct tps61050_info *info, int pwr)
+{
+	if (pwr < info->pwr_api)
+		pwr = info->pwr_api;
+	return tps61050_pm_wr(info, pwr);
+}
+
+static void tps61050_pm_exit(struct tps61050_info *info)
+{
+	tps61050_pm_wr_s(info, NVC_PWR_OFF);
+	tps61050_pm_regulator_put(&info->vreg_i2c);
+}
+
+static void tps61050_pm_init(struct tps61050_info *info)
+{
+	tps61050_pm_regulator_get(info, &info->vreg_i2c, "vdd_i2c");
+}
+
+struct tps61050_reg_init {
+	u8 mask;
+	u8 val;
+};
+
+static struct tps61050_reg_init tps61050_reg_init_id[] = {
+	{0xC0, 0x00},
+	{0xC0, 0x00},
+	{0x87, 0x00},
+	{0xFF, 0xD1},
+};
+
+static int tps61050_dev_id(struct tps61050_info *info)
+{
+	u8 reg;
+	u8 i;
+	int err;
+
+	tps61050_pm_dev_wr(info, NVC_PWR_COMM);
+	/* There isn't a device ID so we just check that all the registers
+	 * equal their startup defaults.
+	 */
+	for (i = TPS61050_REG0; i <= TPS61050_REG3; i++) {
+		err = tps61050_i2c_rd(info, i, &reg);
+		if (err) {
+			break;
+		} else {
+			reg &= tps61050_reg_init_id[i].mask;
+			if (reg != tps61050_reg_init_id[i].val) {
+				err = -ENODEV;
+				break;
+			}
+		}
+	}
+	tps61050_pm_dev_wr(info, NVC_PWR_OFF);
+	return err;
+}
+
+static int tps61050_param_rd(struct tps61050_info *info, long arg)
+{
+	struct nvc_param params;
+	struct nvc_torch_pin_state pinstate;
+	const void *data_ptr;
+	u8 reg;
+	u32 data_size = 0;
+	int err;
+
+	if (copy_from_user(&params,
+			(const void __user *)arg,
+			sizeof(struct nvc_param))) {
+		dev_err(&info->i2c_client->dev, "%s %d copy_from_user err\n",
+				__func__, __LINE__);
+		return -EINVAL;
+	}
+
+	if (info->s_mode == NVC_SYNC_SLAVE)
+		info = info->s_info;
+	switch (params.param) {
+	case NVC_PARAM_FLASH_CAPS:
+		dev_dbg(&info->i2c_client->dev, "%s FLASH_CAPS\n", __func__);
+		data_ptr = &tps61050_flash_cap;
+		data_size = tps61050_flash_cap_size;
+		break;
+
+	case NVC_PARAM_FLASH_LEVEL:
+		tps61050_pm_dev_wr(info, NVC_PWR_COMM);
+		err = tps61050_i2c_rd(info, TPS61050_REG1, &reg);
+		tps61050_pm_dev_wr(info, NVC_PWR_OFF);
+		if (err < 0)
+			return err;
+
+		if (reg & 0x80) { /* 7:7 flash on/off */
+			reg &= 0x07; /* 2:0 flash setting */
+			reg++; /* flash setting +1 if flash on */
+		} else {
+			reg = 0; /* flash is off */
+		}
+		dev_dbg(&info->i2c_client->dev, "%s FLASH_LEVEL: %u\n",
+			    __func__,
+			    (unsigned)tps61050_flash_cap.levels[reg].guidenum);
+		data_ptr = &tps61050_flash_cap.levels[reg].guidenum;
+		data_size = sizeof(tps61050_flash_cap.levels[reg].guidenum);
+		break;
+
+	case NVC_PARAM_TORCH_CAPS:
+		dev_dbg(&info->i2c_client->dev, "%s TORCH_CAPS\n", __func__);
+		data_ptr = &tps61050_torch_cap;
+		data_size = tps61050_torch_cap_size;
+		break;
+
+	case NVC_PARAM_TORCH_LEVEL:
+		tps61050_pm_dev_wr(info, NVC_PWR_COMM);
+		err = tps61050_i2c_rd(info, TPS61050_REG0, &reg);
+		tps61050_pm_dev_wr(info, NVC_PWR_OFF);
+		if (err < 0)
+			return err;
+
+		reg &= 0x07;
+		dev_dbg(&info->i2c_client->dev, "%s TORCH_LEVEL: %u\n",
+				__func__,
+				(unsigned)tps61050_torch_cap.guidenum[reg]);
+		data_ptr = &tps61050_torch_cap.guidenum[reg];
+		data_size = sizeof(tps61050_torch_cap.guidenum[reg]);
+		break;
+
+	case NVC_PARAM_FLASH_PIN_STATE:
+		pinstate.mask = info->pdata->pinstate->mask;
+		tps61050_pm_dev_wr(info, NVC_PWR_COMM);
+		err = tps61050_i2c_rd(info, TPS61050_REG1, &reg);
+		tps61050_pm_dev_wr(info, NVC_PWR_OFF);
+		if (err < 0)
+			return err;
+
+		reg &= 0x80; /* 7:7=flash enable */
+		if (reg)
+			/* assert strobe */
+			pinstate.values = info->pdata->pinstate->values;
+		else
+			pinstate.values = 0; /* deassert strobe */
+		dev_dbg(&info->i2c_client->dev, "%s FLASH_PIN_STATE: %x&%x\n",
+				__func__, pinstate.mask, pinstate.values);
+		data_ptr = &pinstate;
+		data_size = sizeof(struct nvc_torch_pin_state);
+		break;
+
+	case NVC_PARAM_STEREO:
+		dev_dbg(&info->i2c_client->dev, "%s STEREO: %d\n",
+				__func__, (int)info->s_mode);
+		data_ptr = &info->s_mode;
+		data_size = sizeof(info->s_mode);
+		break;
+
+	default:
+		dev_err(&info->i2c_client->dev,
+				"%s unsupported parameter: %d\n",
+				__func__, params.param);
+		return -EINVAL;
+	}
+
+	if (params.sizeofvalue < data_size) {
+		dev_err(&info->i2c_client->dev,
+				"%s data size mismatch %d != %d\n",
+				__func__, params.sizeofvalue, data_size);
+		return -EINVAL;
+	}
+
+	if (copy_to_user((void __user *)params.p_value,
+			 data_ptr,
+			 data_size)) {
+		dev_err(&info->i2c_client->dev,
+				"%s copy_to_user err line %d\n",
+				__func__, __LINE__);
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int tps61050_param_wr_s(struct tps61050_info *info,
+			       struct nvc_param *params,
+			       u8 val)
+{
+	u8 reg;
+	int err = 0;
+
+	/*
+	 * 7:6 flash/torch mode
+	 * 0 0 = off (power save)
+	 * 0 1 = torch only (torch power is 2:0 REG0 where 0 = off)
+	 * 1 0 = flash and torch (flash power is 2:0 REG1 (0 is a power level))
+	 * 1 1 = N/A
+	 * Note that 7:6 of REG0 and REG1 are shadowed with each other.
+	 * In the code below we want to turn on/off one
+	 * without affecting the other.
+	 */
+	switch (params->param) {
+	case NVC_PARAM_FLASH_LEVEL:
+		dev_dbg(&info->i2c_client->dev, "%s FLASH_LEVEL: %d\n",
+				__func__, val);
+		tps61050_pm_dev_wr(info, NVC_PWR_ON);
+		if (val) {
+			val--;
+			if (val > tps61050_default_pdata.max_amp_flash)
+				val = tps61050_default_pdata.max_amp_flash;
+			/* Amp limit values are in the board-sensors file. */
+			if (info->pdata->max_amp_flash &&
+					(val > info->pdata->max_amp_flash))
+				val = info->pdata->max_amp_flash;
+			val |= 0x80; /* 7:7=flash mode */
+		} else {
+			err = tps61050_i2c_rd(info, TPS61050_REG0, &reg);
+			if (reg & 0x07) /* 2:0=torch setting */
+				val = 0x40; /* 6:6 enable just torch */
+		}
+		err |= tps61050_i2c_wr(info, TPS61050_REG1, val);
+		val &= 0xC0; /* 7:6=flash/torch mode */
+		if (!val) /* turn pwr off if no flash && no pwr_api */
+			tps61050_pm_dev_wr(info, NVC_PWR_OFF);
+		return err;
+
+	case NVC_PARAM_TORCH_LEVEL:
+		dev_dbg(&info->i2c_client->dev, "%s TORCH_LEVEL: %d\n",
+				__func__, val);
+		tps61050_pm_dev_wr(info, NVC_PWR_ON);
+		err = tps61050_i2c_rd(info, TPS61050_REG1, &reg);
+		reg &= 0x80; /* 7:7=flash */
+		if (val) {
+			if (val > tps61050_default_pdata.max_amp_torch)
+				val = tps61050_default_pdata.max_amp_torch;
+			/* Amp limit values are in the board-sensors file. */
+			if (info->pdata->max_amp_torch &&
+					(val > info->pdata->max_amp_torch))
+				val = info->pdata->max_amp_torch;
+			if (!reg) /* test if flash/torch off */
+				val |= (0x40); /* 6:6=torch only mode */
+		} else {
+			val |= reg;
+		}
+		err |= tps61050_i2c_wr(info, TPS61050_REG0, val);
+		val &= 0xC0; /* 7:6=mode */
+		if (!val) /* turn pwr off if no torch && no pwr_api */
+			tps61050_pm_dev_wr(info, NVC_PWR_OFF);
+		return err;
+
+	case NVC_PARAM_FLASH_PIN_STATE:
+		dev_dbg(&info->i2c_client->dev, "%s FLASH_PIN_STATE: %d\n",
+				__func__, val);
+		if (val)
+			val = 0x08; /* 3:3=soft trigger */
+		err = tps61050_i2c_rd(info, TPS61050_REG1, &reg);
+		val |= reg;
+		err |= tps61050_i2c_wr(info, TPS61050_REG1, val);
+		return err;
+
+	default:
+		dev_err(&info->i2c_client->dev,
+				"%s unsupported parameter: %d\n",
+				__func__, params->param);
+		return -EINVAL;
+	}
+}
+
+static int tps61050_param_wr(struct tps61050_info *info, long arg)
+{
+	struct nvc_param params;
+	u8 val;
+	int err = 0;
+
+	if (copy_from_user(&params, (const void __user *)arg,
+			   sizeof(struct nvc_param))) {
+		dev_err(&info->i2c_client->dev, "%s %d copy_from_user err\n",
+				__func__, __LINE__);
+		return -EINVAL;
+	}
+
+	if (copy_from_user(&val, (const void __user *)params.p_value,
+			   sizeof(val))) {
+		dev_err(&info->i2c_client->dev, "%s %d copy_from_user err\n",
+				__func__, __LINE__);
+		return -EINVAL;
+	}
+
+	/* parameters independent of sync mode */
+	switch (params.param) {
+	case NVC_PARAM_STEREO:
+		dev_dbg(&info->i2c_client->dev, "%s STEREO: %d\n",
+				__func__, (int)val);
+		if (val == info->s_mode)
+			return 0;
+
+		switch (val) {
+		case NVC_SYNC_OFF:
+			info->s_mode = val;
+			if (info->s_info != NULL) {
+				info->s_info->s_mode = val;
+				tps61050_pm_wr(info->s_info, NVC_PWR_OFF);
+			}
+			break;
+
+		case NVC_SYNC_MASTER:
+			info->s_mode = val;
+			if (info->s_info != NULL)
+				info->s_info->s_mode = val;
+			break;
+
+		case NVC_SYNC_SLAVE:
+		case NVC_SYNC_STEREO:
+			if (info->s_info != NULL) {
+				/* sync power */
+				info->s_info->pwr_api = info->pwr_api;
+				err = tps61050_pm_wr(info->s_info,
+						     info->pwr_dev);
+				if (!err) {
+					info->s_mode = val;
+					info->s_info->s_mode = val;
+				} else {
+					tps61050_pm_wr(info->s_info,
+						       NVC_PWR_OFF);
+					err = -EIO;
+				}
+			} else {
+				err = -EINVAL;
+			}
+			break;
+
+		default:
+			err = -EINVAL;
+		}
+		if (info->pdata->cfg & NVC_CFG_NOERR)
+			return 0;
+
+		return err;
+
+	default:
+	/* parameters dependent on sync mode */
+		switch (info->s_mode) {
+		case NVC_SYNC_OFF:
+		case NVC_SYNC_MASTER:
+			return tps61050_param_wr_s(info, &params, val);
+
+		case NVC_SYNC_SLAVE:
+			return tps61050_param_wr_s(info->s_info,
+						 &params,
+						 val);
+
+		case NVC_SYNC_STEREO:
+			err = tps61050_param_wr_s(info, &params, val);
+			if (!(info->pdata->cfg & NVC_CFG_SYNC_I2C_MUX))
+				err |= tps61050_param_wr_s(info->s_info,
+							 &params,
+							 val);
+			return err;
+
+		default:
+			dev_err(&info->i2c_client->dev, "%s %d internal err\n",
+					__func__, __LINE__);
+			return -EINVAL;
+		}
+	}
+}
+
+static long tps61050_ioctl(struct file *file,
+		unsigned int cmd,
+		unsigned long arg)
+{
+	struct tps61050_info *info = file->private_data;
+	int pwr;
+
+	switch (cmd) {
+	case NVC_IOCTL_PARAM_WR:
+		return tps61050_param_wr(info, arg);
+
+	case NVC_IOCTL_PARAM_RD:
+		return tps61050_param_rd(info, arg);
+
+	case NVC_IOCTL_PWR_WR:
+		/* This is a Guaranteed Level of Service (GLOS) call */
+		pwr = (int)arg * 2;
+		dev_dbg(&info->i2c_client->dev, "%s PWR_WR: %d\n",
+				__func__, pwr);
+		return tps61050_pm_api_wr(info, pwr);
+
+	case NVC_IOCTL_PWR_RD:
+		if (info->s_mode == NVC_SYNC_SLAVE)
+			pwr = info->s_info->pwr_api / 2;
+		else
+			pwr = info->pwr_api / 2;
+		dev_dbg(&info->i2c_client->dev, "%s PWR_RD: %d\n",
+				__func__, pwr);
+		if (copy_to_user((void __user *)arg, (const void *)&pwr,
+				 sizeof(pwr))) {
+			dev_err(&info->i2c_client->dev,
+					"%s copy_to_user err line %d\n",
+					__func__, __LINE__);
+			return -EFAULT;
+		}
+		return 0;
+
+	default:
+		dev_err(&info->i2c_client->dev, "%s unsupported ioctl: %x\n",
+				__func__, cmd);
+		return -EINVAL;
+	}
+}
+
+static int tps61050_sync_en(int dev1, int dev2)
+{
+	struct tps61050_info *sync1 = NULL;
+	struct tps61050_info *sync2 = NULL;
+	struct tps61050_info *pos = NULL;
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(pos, &tps61050_info_list, list) {
+		if (pos->pdata->num == dev1) {
+			sync1 = pos;
+			break;
+		}
+	}
+	pos = NULL;
+	list_for_each_entry_rcu(pos, &tps61050_info_list, list) {
+		if (pos->pdata->num == dev2) {
+			sync2 = pos;
+			break;
+		}
+	}
+	rcu_read_unlock();
+	if (sync1 != NULL)
+		sync1->s_info = NULL;
+	if (sync2 != NULL)
+		sync2->s_info = NULL;
+	if (!dev1 && !dev2)
+		return 0; /* no err if default instance 0's used */
+
+	if (dev1 == dev2)
+		return -EINVAL; /* err if sync instance is itself */
+
+	if ((sync1 != NULL) && (sync2 != NULL)) {
+		sync1->s_info = sync2;
+		sync2->s_info = sync1;
+	}
+	return 0;
+}
+
+static int tps61050_sync_dis(struct tps61050_info *info)
+{
+	if (info->s_info != NULL) {
+		info->s_info->s_mode = 0;
+		info->s_info->s_info = NULL;
+		info->s_mode = 0;
+		info->s_info = NULL;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int tps61050_open(struct inode *inode, struct file *file)
+{
+	struct tps61050_info *info = NULL;
+	struct tps61050_info *pos = NULL;
+	int err;
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(pos, &tps61050_info_list, list) {
+		if (pos->miscdev.minor == iminor(inode)) {
+			info = pos;
+			break;
+		}
+	}
+	rcu_read_unlock();
+	if (!info)
+		return -ENODEV;
+
+	err = tps61050_sync_en(info->pdata->num, info->pdata->sync);
+	if (err == -EINVAL)
+		dev_err(&info->i2c_client->dev,
+			 "%s err: invalid num (%u) and sync (%u) instance\n",
+			 __func__, info->pdata->num, info->pdata->sync);
+	if (atomic_xchg(&info->in_use, 1))
+		return -EBUSY;
+
+	if (info->s_info != NULL) {
+		if (atomic_xchg(&info->s_info->in_use, 1))
+			return -EBUSY;
+	}
+
+	file->private_data = info;
+	dev_dbg(&info->i2c_client->dev, "%s\n", __func__);
+	return 0;
+}
+
+static int tps61050_release(struct inode *inode, struct file *file)
+{
+	struct tps61050_info *info = file->private_data;
+
+	dev_dbg(&info->i2c_client->dev, "%s\n", __func__);
+	tps61050_pm_wr_s(info, NVC_PWR_OFF);
+	file->private_data = NULL;
+	WARN_ON(!atomic_xchg(&info->in_use, 0));
+	if (info->s_info != NULL)
+		WARN_ON(!atomic_xchg(&info->s_info->in_use, 0));
+	tps61050_sync_dis(info);
+	return 0;
+}
+
+static const struct file_operations tps61050_fileops = {
+	.owner = THIS_MODULE,
+	.open = tps61050_open,
+	.unlocked_ioctl = tps61050_ioctl,
+	.release = tps61050_release,
+};
+
+static void tps61050_del(struct tps61050_info *info)
+{
+	tps61050_pm_exit(info);
+	tps61050_sync_dis(info);
+	spin_lock(&tps61050_spinlock);
+	list_del_rcu(&info->list);
+	spin_unlock(&tps61050_spinlock);
+	synchronize_rcu();
+}
+
+static int tps61050_remove(struct i2c_client *client)
+{
+	struct tps61050_info *info = i2c_get_clientdata(client);
+
+	dev_dbg(&info->i2c_client->dev, "%s\n", __func__);
+	misc_deregister(&info->miscdev);
+	tps61050_del(info);
+	return 0;
+}
+
+static int tps61050_probe(
+	struct i2c_client *client,
+	const struct i2c_device_id *id)
+{
+	struct tps61050_info *info;
+	char dname[16];
+	int err;
+
+	dev_dbg(&client->dev, "%s\n", __func__);
+	info = devm_kzalloc(&client->dev, sizeof(*info), GFP_KERNEL);
+	if (info == NULL) {
+		dev_err(&client->dev, "%s: kzalloc error\n", __func__);
+		return -ENOMEM;
+	}
+
+	info->i2c_client = client;
+	if (client->dev.platform_data) {
+		info->pdata = client->dev.platform_data;
+	} else {
+		info->pdata = &tps61050_default_pdata;
+		dev_dbg(&client->dev,
+			"%s No platform data.  Using defaults.\n",
+			__func__);
+	}
+	i2c_set_clientdata(client, info);
+	INIT_LIST_HEAD(&info->list);
+	spin_lock(&tps61050_spinlock);
+	list_add_rcu(&info->list, &tps61050_info_list);
+	spin_unlock(&tps61050_spinlock);
+	tps61050_pm_init(info);
+	err = tps61050_dev_id(info);
+	if (err < 0) {
+		dev_err(&client->dev, "%s device not found\n", __func__);
+		if (info->pdata->cfg & NVC_CFG_NODEV) {
+			tps61050_del(info);
+			return -ENODEV;
+		}
+	} else {
+		dev_dbg(&client->dev, "%s device found\n", __func__);
+	}
+
+	if (info->pdata->dev_name != 0)
+		strcpy(dname, info->pdata->dev_name);
+	else
+		strcpy(dname, "tps61050");
+	if (info->pdata->num)
+		snprintf(dname, sizeof(dname), "%s.%u",
+			 dname, info->pdata->num);
+	info->miscdev.name = dname;
+	info->miscdev.fops = &tps61050_fileops;
+	info->miscdev.minor = MISC_DYNAMIC_MINOR;
+	if (misc_register(&info->miscdev)) {
+		dev_err(&client->dev, "%s unable to register misc device %s\n",
+				__func__, dname);
+		tps61050_del(info);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static const struct i2c_device_id tps61050_id[] = {
+	{ "tps61050", 0 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(i2c, tps61050_id);
+
+static struct i2c_driver tps61050_i2c_driver = {
+	.driver = {
+		.name = "tps61050",
+		.owner = THIS_MODULE,
+	},
+	.id_table = tps61050_id,
+	.probe = tps61050_probe,
+	.remove = tps61050_remove,
+};
+
+static int __init tps61050_init(void)
+{
+	return i2c_add_driver(&tps61050_i2c_driver);
+}
+
+static void __exit tps61050_exit(void)
+{
+	i2c_del_driver(&tps61050_i2c_driver);
+}
+
+module_init(tps61050_init);
+module_exit(tps61050_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/tegra_v4l2_camera.c b/drivers/media/video/tegra_v4l2_camera.c
new file mode 100644
index 000000000000..935cdcf8d40c
--- /dev/null
+++ b/drivers/media/video/tegra_v4l2_camera.c
@@ -0,0 +1,2215 @@
+/*
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ * Copyright (c) 2013-2014 Antmicro Ltd <www.antmicro.com>
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/pm_runtime.h>
+#include <linux/nvhost.h>
+#include <linux/kthread.h>
+
+#include <mach/iomap.h>
+#include <mach/powergate.h>
+
+#include <media/soc_camera.h>
+#include <media/soc_mediabus.h>
+#include <media/videobuf2-dma-nvmap.h>
+#include <media/videobuf2-memops.h> /* for vb2_vmarea_handler */
+#include <media/tegra_v4l2_camera.h>
+
+#include "dev.h"
+#include "bus_client.h"
+#include "host1x/host1x_syncpt.h"
+
+#define TEGRA_CAM_DRV_NAME "vi"
+#define TEGRA_CAM_VERSION_CODE KERNEL_VERSION(0, 0, 5)
+
+#define TEGRA_SYNCPT_VI_WAIT_TIMEOUT                    200
+#define TEGRA_SYNCPT_CSI_WAIT_TIMEOUT                   200
+
+#define TEGRA_SYNCPT_RETRY_COUNT			10
+
+/* SYNCPTs 12-17 are reserved for VI. */
+#define TEGRA_VI_SYNCPT_VI                              NVSYNCPT_VI_ISP_2
+#define TEGRA_VI_SYNCPT_CSI_A                           NVSYNCPT_VI_ISP_3
+#define TEGRA_VI_SYNCPT_CSI_B                           NVSYNCPT_VI_ISP_4
+
+/* Tegra CSI-MIPI registers. */
+#define TEGRA_VI_OUT_1_INCR_SYNCPT			0x0000
+#define TEGRA_VI_OUT_1_INCR_SYNCPT_CNTRL		0x0004
+#define TEGRA_VI_OUT_1_INCR_SYNCPT_ERROR		0x0008
+#define TEGRA_VI_OUT_2_INCR_SYNCPT			0x0020
+#define TEGRA_VI_OUT_2_INCR_SYNCPT_CNTRL		0x0024
+#define TEGRA_VI_OUT_2_INCR_SYNCPT_ERROR		0x0028
+#define TEGRA_VI_MISC_INCR_SYNCPT			0x0040
+#define TEGRA_VI_MISC_INCR_SYNCPT_CNTRL			0x0044
+#define TEGRA_VI_MISC_INCR_SYNCPT_ERROR			0x0048
+#define TEGRA_VI_CONT_SYNCPT_OUT_1			0x0060
+#define TEGRA_VI_CONT_SYNCPT_OUT_2			0x0064
+#define TEGRA_VI_CONT_SYNCPT_VIP_VSYNC			0x0068
+#define TEGRA_VI_CONT_SYNCPT_VI2EPP			0x006c
+#define TEGRA_VI_CONT_SYNCPT_CSI_PPA_FRAME_START	0x0070
+#define TEGRA_VI_CONT_SYNCPT_CSI_PPA_FRAME_END		0x0074
+#define TEGRA_VI_CONT_SYNCPT_CSI_PPB_FRAME_START	0x0078
+#define TEGRA_VI_CONT_SYNCPT_CSI_PPB_FRAME_END		0x007c
+#define TEGRA_VI_CTXSW					0x0080
+#define TEGRA_VI_INTSTATUS				0x0084
+#define TEGRA_VI_VI_INPUT_CONTROL			0x0088
+#define TEGRA_VI_VI_CORE_CONTROL			0x008c
+#define TEGRA_VI_VI_FIRST_OUTPUT_CONTROL		0x0090
+#define TEGRA_VI_VI_SECOND_OUTPUT_CONTROL		0x0094
+#define TEGRA_VI_HOST_INPUT_FRAME_SIZE			0x0098
+#define TEGRA_VI_HOST_H_ACTIVE				0x009c
+#define TEGRA_VI_HOST_V_ACTIVE				0x00a0
+#define TEGRA_VI_VIP_H_ACTIVE				0x00a4
+#define TEGRA_VI_VIP_V_ACTIVE				0x00a8
+#define TEGRA_VI_VI_PEER_CONTROL			0x00ac
+#define TEGRA_VI_VI_DMA_SELECT				0x00b0
+#define TEGRA_VI_HOST_DMA_WRITE_BUFFER			0x00b4
+#define TEGRA_VI_HOST_DMA_BASE_ADDRESS			0x00b8
+#define TEGRA_VI_HOST_DMA_WRITE_BUFFER_STATUS		0x00bc
+#define TEGRA_VI_HOST_DMA_WRITE_PEND_BUFCOUNT		0x00c0
+#define TEGRA_VI_VB0_START_ADDRESS_FIRST		0x00c4
+#define TEGRA_VI_VB0_BASE_ADDRESS_FIRST			0x00c8
+#define TEGRA_VI_VB0_START_ADDRESS_U			0x00cc
+#define TEGRA_VI_VB0_BASE_ADDRESS_U			0x00d0
+#define TEGRA_VI_VB0_START_ADDRESS_V			0x00d4
+#define TEGRA_VI_VB0_BASE_ADDRESS_V			0x00d8
+#define TEGRA_VI_VB_SCRATCH_ADDRESS_UV			0x00dc
+#define TEGRA_VI_FIRST_OUTPUT_FRAME_SIZE		0x00e0
+#define TEGRA_VI_VB0_COUNT_FIRST			0x00e4
+#define TEGRA_VI_VB0_SIZE_FIRST				0x00e8
+#define TEGRA_VI_VB0_BUFFER_STRIDE_FIRST		0x00ec
+#define TEGRA_VI_VB0_START_ADDRESS_SECOND		0x00f0
+#define TEGRA_VI_VB0_BASE_ADDRESS_SECOND		0x00f4
+#define TEGRA_VI_SECOND_OUTPUT_FRAME_SIZE		0x00f8
+#define TEGRA_VI_VB0_COUNT_SECOND			0x00fc
+#define TEGRA_VI_VB0_SIZE_SECOND			0x0100
+#define TEGRA_VI_VB0_BUFFER_STRIDE_SECOND		0x0104
+#define TEGRA_VI_H_LPF_CONTROL				0x0108
+#define TEGRA_VI_H_DOWNSCALE_CONTROL			0x010c
+#define TEGRA_VI_V_DOWNSCALE_CONTROL			0x0110
+#define TEGRA_VI_CSC_Y					0x0114
+#define TEGRA_VI_CSC_UV_R				0x0118
+#define TEGRA_VI_CSC_UV_G				0x011c
+#define TEGRA_VI_CSC_UV_B				0x0120
+#define TEGRA_VI_CSC_ALPHA				0x0124
+#define TEGRA_VI_HOST_VSYNC				0x0128
+#define TEGRA_VI_COMMAND				0x012c
+#define TEGRA_VI_HOST_FIFO_STATUS			0x0130
+#define TEGRA_VI_INTERRUPT_MASK				0x0134
+#define TEGRA_VI_INTERRUPT_TYPE_SELECT			0x0138
+#define TEGRA_VI_INTERRUPT_POLARITY_SELECT		0x013c
+#define TEGRA_VI_INTERRUPT_STATUS			0x0140
+#define TEGRA_VI_VIP_INPUT_STATUS			0x0144
+#define TEGRA_VI_VIDEO_BUFFER_STATUS			0x0148
+#define TEGRA_VI_SYNC_OUTPUT				0x014c
+#define TEGRA_VI_VVS_OUTPUT_DELAY			0x0150
+#define TEGRA_VI_PWM_CONTROL				0x0154
+#define TEGRA_VI_PWM_SELECT_PULSE_A			0x0158
+#define TEGRA_VI_PWM_SELECT_PULSE_B			0x015c
+#define TEGRA_VI_PWM_SELECT_PULSE_C			0x0160
+#define TEGRA_VI_PWM_SELECT_PULSE_D			0x0164
+#define TEGRA_VI_VI_DATA_INPUT_CONTROL			0x0168
+#define TEGRA_VI_PIN_INPUT_ENABLE			0x016c
+#define TEGRA_VI_PIN_OUTPUT_ENABLE			0x0170
+#define TEGRA_VI_PIN_INVERSION				0x0174
+#define TEGRA_VI_PIN_INPUT_DATA				0x0178
+#define TEGRA_VI_PIN_OUTPUT_DATA			0x017c
+#define TEGRA_VI_PIN_OUTPUT_SELECT			0x0180
+#define TEGRA_VI_RAISE_VIP_BUFFER_FIRST_OUTPUT		0x0184
+#define TEGRA_VI_RAISE_VIP_FRAME_FIRST_OUTPUT		0x0188
+#define TEGRA_VI_RAISE_VIP_BUFFER_SECOND_OUTPUT		0x018c
+#define TEGRA_VI_RAISE_VIP_FRAME_SECOND_OUTPUT		0x0190
+#define TEGRA_VI_RAISE_HOST_FIRST_OUTPUT		0x0194
+#define TEGRA_VI_RAISE_HOST_SECOND_OUTPUT		0x0198
+#define TEGRA_VI_RAISE_EPP				0x019c
+#define TEGRA_VI_CAMERA_CONTROL				0x01a0
+#define TEGRA_VI_VI_ENABLE				0x01a4
+#define TEGRA_VI_VI_ENABLE_2				0x01a8
+#define TEGRA_VI_VI_RAISE				0x01ac
+#define TEGRA_VI_Y_FIFO_WRITE				0x01b0
+#define TEGRA_VI_U_FIFO_WRITE				0x01b4
+#define TEGRA_VI_V_FIFO_WRITE				0x01b8
+#define TEGRA_VI_VI_MCCIF_FIFOCTRL			0x01bc
+#define TEGRA_VI_TIMEOUT_WCOAL_VI			0x01c0
+#define TEGRA_VI_MCCIF_VIRUV_HP				0x01c4
+#define TEGRA_VI_MCCIF_VIWSB_HP				0x01c8
+#define TEGRA_VI_MCCIF_VIWU_HP				0x01cc
+#define TEGRA_VI_MCCIF_VIWV_HP				0x01d0
+#define TEGRA_VI_MCCIF_VIWY_HP				0x01d4
+#define TEGRA_VI_CSI_PPA_RAISE_FRAME_START		0x01d8
+#define TEGRA_VI_CSI_PPA_RAISE_FRAME_END		0x01dc
+#define TEGRA_VI_CSI_PPB_RAISE_FRAME_START		0x01e0
+#define TEGRA_VI_CSI_PBB_RAISE_FRAME_END		0x01e4
+#define TEGRA_VI_CSI_PPA_H_ACTIVE			0x01e8
+#define TEGRA_VI_CSI_PPA_V_ACTIVE			0x01ec
+#define TEGRA_VI_CSI_PPB_H_ACTIVE			0x01f0
+#define TEGRA_VI_CSI_PPB_V_ACTIVE			0x01f4
+#define TEGRA_VI_ISP_H_ACTIVE				0x01f8
+#define TEGRA_VI_ISP_V_ACTIVE				0x01fc
+#define TEGRA_VI_STREAM_1_RESOURCE_DEFINE		0x0200
+#define TEGRA_VI_STREAM_2_RESOURCE_DEFINE		0x0204
+#define TEGRA_VI_RAISE_STREAM_1_DONE			0x0208
+#define TEGRA_VI_RAISE_STREAM_2_DONE			0x020c
+#define TEGRA_VI_TS_MODE				0x0210
+#define TEGRA_VI_TS_CONTROL				0x0214
+#define TEGRA_VI_TS_PACKET_COUNT			0x0218
+#define TEGRA_VI_TS_ERROR_COUNT				0x021c
+#define TEGRA_VI_TS_CPU_FLOW_CTL			0x0220
+#define TEGRA_VI_VB0_CHROMA_BUFFER_STRIDE_FIRST		0x0224
+#define TEGRA_VI_VB0_CHROMA_LINE_STRIDE_FIRST		0x0228
+#define TEGRA_VI_EPP_LINES_PER_BUFFER			0x022c
+#define TEGRA_VI_BUFFER_RELEASE_OUTPUT1			0x0230
+#define TEGRA_VI_BUFFER_RELEASE_OUTPUT2			0x0234
+#define TEGRA_VI_DEBUG_FLOW_CONTROL_COUNTER_OUTPUT1	0x0238
+#define TEGRA_VI_DEBUG_FLOW_CONTROL_COUNTER_OUTPUT2	0x023c
+#define TEGRA_VI_TERMINATE_BW_FIRST			0x0240
+#define TEGRA_VI_TERMINATE_BW_SECOND			0x0244
+#define TEGRA_VI_VB0_FIRST_BUFFER_ADDR_MODE		0x0248
+#define TEGRA_VI_VB0_SECOND_BUFFER_ADDR_MODE		0x024c
+#define TEGRA_VI_RESERVE_0				0x0250
+#define TEGRA_VI_RESERVE_1				0x0254
+#define TEGRA_VI_RESERVE_2				0x0258
+#define TEGRA_VI_RESERVE_3				0x025c
+#define TEGRA_VI_RESERVE_4				0x0260
+#define TEGRA_VI_MCCIF_VIRUV_HYST			0x0264
+#define TEGRA_VI_MCCIF_VIWSB_HYST			0x0268
+#define TEGRA_VI_MCCIF_VIWU_HYST			0x026c
+#define TEGRA_VI_MCCIF_VIWV_HYST			0x0270
+#define TEGRA_VI_MCCIF_VIWY_HYST			0x0274
+
+#define TEGRA_CSI_VI_INPUT_STREAM_CONTROL		0x0800
+#define TEGRA_CSI_HOST_INPUT_STREAM_CONTROL		0x0808
+#define TEGRA_CSI_INPUT_STREAM_A_CONTROL		0x0810
+#define TEGRA_CSI_PIXEL_STREAM_A_CONTROL0		0x0818
+#define TEGRA_CSI_PIXEL_STREAM_A_CONTROL1		0x081c
+#define TEGRA_CSI_PIXEL_STREAM_A_WORD_COUNT		0x0820
+#define TEGRA_CSI_PIXEL_STREAM_A_GAP			0x0824
+#define TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND		0x0828
+#define TEGRA_CSI_INPUT_STREAM_B_CONTROL		0x083c
+#define TEGRA_CSI_PIXEL_STREAM_B_CONTROL0		0x0844
+#define TEGRA_CSI_PIXEL_STREAM_B_CONTROL1		0x0848
+#define TEGRA_CSI_PIXEL_STREAM_B_WORD_COUNT		0x084c
+#define TEGRA_CSI_PIXEL_STREAM_B_GAP			0x0850
+#define TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND		0x0854
+#define TEGRA_CSI_PHY_CIL_COMMAND			0x0868
+#define TEGRA_CSI_PHY_CILA_CONTROL0			0x086c
+#define TEGRA_CSI_PHY_CILB_CONTROL0			0x0870
+#define TEGRA_CSI_CSI_PIXEL_PARSER_STATUS		0x0878
+#define TEGRA_CSI_CSI_CIL_STATUS			0x087c
+#define TEGRA_CSI_CSI_PIXEL_PARSER_INTERRUPT_MASK	0x0880
+#define TEGRA_CSI_CSI_CIL_INTERRUPT_MASK		0x0884
+#define TEGRA_CSI_CSI_READONLY_STATUS			0x0888
+#define TEGRA_CSI_ESCAPE_MODE_COMMAND			0x088c
+#define TEGRA_CSI_ESCAPE_MODE_DATA			0x0890
+#define TEGRA_CSI_CILA_PAD_CONFIG0			0x0894
+#define TEGRA_CSI_CILA_PAD_CONFIG1			0x0898
+#define TEGRA_CSI_CILB_PAD_CONFIG0			0x089c
+#define TEGRA_CSI_CILB_PAD_CONFIG1			0x08a0
+#define TEGRA_CSI_CIL_PAD_CONFIG			0x08a4
+#define TEGRA_CSI_CILA_MIPI_CAL_CONFIG			0x08a8
+#define TEGRA_CSI_CILB_MIPI_CAL_CONFIG			0x08ac
+#define TEGRA_CSI_CIL_MIPI_CAL_STATUS			0x08b0
+#define TEGRA_CSI_CLKEN_OVERRIDE			0x08b4
+#define TEGRA_CSI_DEBUG_CONTROL				0x08b8
+#define TEGRA_CSI_DEBUG_COUNTER_0			0x08bc
+#define TEGRA_CSI_DEBUG_COUNTER_1			0x08c0
+#define TEGRA_CSI_DEBUG_COUNTER_2			0x08c4
+#define TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME		0x08c8
+#define TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME		0x08cc
+#define TEGRA_CSI_DSI_MIPI_CAL_CONFIG			0x08d0
+#define TEGRA_CSI_MIPIBIAS_PAD_CONFIG0			0x08d4
+
+#define IS_INTERLACED ((pcdev->field == V4L2_FIELD_INTERLACED)\
+		|| (pcdev->field == V4L2_FIELD_INTERLACED_BT)\
+		|| (pcdev->field == V4L2_FIELD_INTERLACED_TB))
+
+#define TC_VI_REG_RD(DEV, REG) readl(DEV->vi_base + REG)
+#define TC_VI_REG_WT(DEV, REG, VAL) writel(VAL, DEV->vi_base + REG)
+
+#define tegra_camera_port_is_valid(port) \
+	(((port) >= TEGRA_CAMERA_PORT_CSI_A) && \
+	 ((port) <= TEGRA_CAMERA_PORT_VIP))
+
+#define tegra_camera_port_is_csi(port) \
+	(((port) == TEGRA_CAMERA_PORT_CSI_A) || \
+	 ((port) == TEGRA_CAMERA_PORT_CSI_B))
+
+/*
+ * Structures
+ */
+
+/* buffer for one video frame */
+struct tegra_buffer {
+	struct vb2_buffer		vb; /* v4l buffer must be first */
+	struct list_head		queue;
+	struct soc_camera_device	*icd;
+	int				output_channel;
+
+	/*
+	 * Various buffer addresses shadowed so we don't have to recalculate
+	 * per frame.  These are calculated during videobuf_prepare.
+	 */
+	dma_addr_t			buffer_addr;
+	dma_addr_t			buffer_addr_u;
+	dma_addr_t			buffer_addr_v;
+	dma_addr_t			start_addr;
+	dma_addr_t			start_addr_u;
+	dma_addr_t			start_addr_v;
+	void				*virtual_addr;
+
+	dma_addr_t			internal_phys_addr;
+	void				*internal_virtual_addr;
+};
+
+struct tegra_camera_dev {
+	struct soc_camera_host		ici;
+	struct nvhost_device		*ndev;
+
+	struct clk			*clk_vi;
+	struct clk			*clk_vi_sensor;
+	struct clk			*clk_csi;
+	struct clk			*clk_isp;
+	struct clk			*clk_csus;
+	struct clk			*clk_sclk;
+	struct clk			*clk_emc;
+
+	struct regulator		*reg;
+
+	void __iomem			*vi_base;
+	spinlock_t			videobuf_queue_lock;
+	struct list_head		capture;
+	struct vb2_buffer		*active;
+	struct vb2_alloc_ctx		*alloc_ctx;
+	enum v4l2_field			field;
+	int				sequence_a;
+	int				sequence_b;
+
+	struct work_struct		work;
+	struct mutex			work_mutex;
+
+	struct soc_camera_device	*icd;
+
+	u32				syncpt_vi;
+	u32				syncpt_csi_a;
+	u32				syncpt_csi_b;
+	int				capturing;
+
+	/* private buffer for non-interlaced frame */
+	struct vb2_dc_buf *internal_vbuf;
+
+	/* Debug */
+	int num_frames;
+	int enable_refcnt;
+
+	/* CSI pad calibration flag */
+	int cal_done;
+};
+
+static const struct soc_mbus_pixelfmt tegra_camera_formats[] = {
+	{
+		.fourcc			= V4L2_PIX_FMT_UYVY,
+		.name			= "YUV422 (UYVY) packed",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_VYUY,
+		.name			= "YUV422 (VYUY) packed",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_YUYV,
+		.name			= "YUV422 (YUYV) packed",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_YVYU,
+		.name			= "YUV422 (YVYU) packed",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_YUV420,
+		.name			= "YUV420 (YU12) planar",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_YVU420,
+		.name			= "YVU420 (YV12) planar",
+		.bits_per_sample	= 12,
+		.packing		= SOC_MBUS_PACKING_NONE,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+
+	/* For RAW8 and RAW10 output, we always output 16-bit (2 bytes). */
+	{
+		.fourcc			= V4L2_PIX_FMT_SBGGR8,
+		.name			= "Bayer 8 BGBG.. GRGR..",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_SBGGR10,
+		.name			= "Bayer 10 BGBG.. GRGR..",
+		.bits_per_sample	= 16,
+		.packing		= SOC_MBUS_PACKING_EXTEND16,
+		.order			= SOC_MBUS_ORDER_LE,
+	},
+
+};
+
+/* interlace related stuff */
+struct thread_args {
+	struct tegra_camera_dev *pcdev;
+	struct tegra_buffer *tb;
+};
+
+void interlace_and_copy(void *dst, void *src, int width, int height)
+{
+	int l;
+
+	for (l = 0; l <= (height-1); l++) {
+		if (l < (height/2))
+			memcpy(dst + (width * (2 * l)), (void *) ((unsigned int)(src) + (width * l)), width);
+		else
+			memcpy(dst + (width * (2 * (l-(height/2)) + 1)), (void *) ((unsigned int)(src) + (width * l)), width);
+	}
+}
+
+int make_interlaced(void *arg)
+{
+	struct thread_args *ta = arg;
+	struct soc_camera_device *icd = ta->tb->icd;
+	void *src = ta->tb->internal_virtual_addr;
+	void *dst = ta->tb->virtual_addr;
+	int bytes_per_line;
+
+	if (icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_YUV420 || icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_YVU420) {
+		interlace_and_copy(dst, src, icd->user_width, icd->user_height); /* Y */
+		interlace_and_copy(dst + (icd->user_width * icd->user_height),
+				src + (icd->user_width * icd->user_height),
+				icd->user_width,
+				icd->user_height/4); /* U */
+		interlace_and_copy(dst + (icd->user_width * icd->user_height) + (icd->user_width * icd->user_height)/4,
+				src + (icd->user_width * icd->user_height) + (icd->user_width * icd->user_height)/4,
+				icd->user_width,
+				icd->user_height/4); /* V */
+	} else {
+		bytes_per_line = soc_mbus_bytes_per_line(icd->user_width, icd->current_fmt->host_fmt);
+		interlace_and_copy(dst, src, bytes_per_line, icd->user_height); /* Y, U, V */
+	}
+	do_exit(0);
+}
+
+static struct tegra_buffer *to_tegra_vb(struct vb2_buffer *vb)
+{
+	return container_of(vb, struct tegra_buffer, vb);
+}
+
+static void tegra_camera_save_syncpts(struct tegra_camera_dev *pcdev)
+{
+	pcdev->syncpt_csi_a =
+		nvhost_syncpt_read_ext(pcdev->ndev,
+				       TEGRA_VI_SYNCPT_CSI_A);
+
+	pcdev->syncpt_csi_b =
+		nvhost_syncpt_read_ext(pcdev->ndev,
+				       TEGRA_VI_SYNCPT_CSI_B);
+
+	pcdev->syncpt_vi =
+		nvhost_syncpt_read_ext(pcdev->ndev,
+				       TEGRA_VI_SYNCPT_VI);
+}
+
+static void tegra_camera_incr_syncpts(struct tegra_camera_dev *pcdev)
+{
+	nvhost_syncpt_cpu_incr_ext(pcdev->ndev,
+				   TEGRA_VI_SYNCPT_CSI_A);
+
+	nvhost_syncpt_cpu_incr_ext(pcdev->ndev,
+				   TEGRA_VI_SYNCPT_CSI_B);
+
+	nvhost_syncpt_cpu_incr_ext(pcdev->ndev,
+				   TEGRA_VI_SYNCPT_VI);
+}
+
+static void tegra_camera_capture_clean(struct tegra_camera_dev *pcdev)
+{
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_VI_INPUT_STREAM_CONTROL, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_HOST_INPUT_STREAM_CONTROL, 0x00000000);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_PIXEL_PARSER_STATUS, 0x0);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_CIL_STATUS, 0x0);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_PIXEL_PARSER_INTERRUPT_MASK, 0x0);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_CIL_INTERRUPT_MASK, 0x0);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_READONLY_STATUS, 0x0);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_ESCAPE_MODE_COMMAND, 0x0);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_ESCAPE_MODE_DATA, 0x0);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CIL_PAD_CONFIG, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CIL_MIPI_CAL_STATUS, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CLKEN_OVERRIDE, 0x00000000);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_DEBUG_CONTROL, 0x0);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_DEBUG_COUNTER_0, 0x0);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_DEBUG_COUNTER_1, 0x0);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_DEBUG_COUNTER_2, 0x0);
+}
+
+static void tegra_camera_capture_setup_csi_a(struct tegra_camera_dev *pcdev,
+					     struct soc_camera_device *icd,
+					     u32 hdr)
+{
+	struct tegra_camera_platform_data *pdata = icd->link->priv;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+			icd->current_fmt->host_fmt);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_A_CONTROL, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL0, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL1, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_WORD_COUNT, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_GAP, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILA_CONTROL0, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_PAD_CONFIG0, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_PAD_CONFIG1, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_MIPI_CAL_CONFIG, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME, 0x0);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_CORE_CONTROL, 0x02000000);
+
+	/* CSI-A H_ACTIVE and V_ACTIVE */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPA_H_ACTIVE,
+		     (icd->user_width << 16));
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPA_V_ACTIVE,
+		     (icd->user_height << 16));
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL1,
+		0x1); /* Frame # for top field detect for interlaced */
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_WORD_COUNT,
+		bytes_per_line);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_GAP, 0x00140000);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_EXPECTED_FRAME,
+		(icd->user_height << 16) |
+		(0x100 << 4) | /* Wait 0x100 vi clks for timeout */
+		0x1); /* Enable line timeout */
+
+	/* pad 0s enabled, virtual channel ID 00 */
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_A_CONTROL0,
+		(0x1 << 16) | /* Output 1 pixel per clock */
+		(hdr << 8) | /* If hdr shows wrong fmt, use right value */
+		(0x1 << 7) | /* Check header CRC */
+		(0x1 << 6) | /* Use word count field in the header */
+		(0x1 << 5) | /* Look at data identifier byte in hdr */
+		(0x1 << 4));  /* Expect packet header */
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_A_CONTROL,
+		     (0x3f << 16) | /* Skip packet threshold */
+		     (pdata->lanes - 1));
+
+	/* Use 0x00000022 for continuous clock mode. */
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILA_CONTROL0,
+		(pdata->continuous_clk << 5) |
+		0x5); /* Clock settle time */
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_CSI_PPA_FRAME_START,
+		(0x1 << 8) | /* Enable continuous syncpt */
+		TEGRA_VI_SYNCPT_CSI_A);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CIL_COMMAND, 0x00020001);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND, 0x0000f002);
+}
+
+static void tegra_camera_capture_setup_csi_b(struct tegra_camera_dev *pcdev,
+					     struct soc_camera_device *icd,
+					     u32 hdr)
+{
+	struct tegra_camera_platform_data *pdata = icd->link->priv;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_B_CONTROL, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL0, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL1, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_WORD_COUNT, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_GAP, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILB_CONTROL0, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILB_PAD_CONFIG0, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILB_PAD_CONFIG1, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILB_MIPI_CAL_CONFIG, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME, 0x0);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_CORE_CONTROL, 0x04000000);
+
+	/* CSI-B H_ACTIVE and V_ACTIVE */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPB_H_ACTIVE,
+		(icd->user_width << 16));
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CSI_PPB_V_ACTIVE,
+		(icd->user_height << 16));
+
+	/* pad 0s enabled, virtual channel ID 00 */
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL0,
+		(0x1 << 16) | /* Output 1 pixel per clock */
+		(hdr << 8) | /* If hdr shows wrong fmt, use right value */
+		(0x1 << 7) | /* Check header CRC */
+		(0x1 << 6) | /* Use word count field in the header */
+		(0x1 << 5) | /* Look at data identifier byte in hdr */
+		(0x1 << 4) | /* Expect packet header */
+		0x1); /* Set PPB stream source to CSI B */
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_CONTROL1,
+		0x1); /* Frame # for top field detect for interlaced */
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_WORD_COUNT,
+		bytes_per_line);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_GAP, 0x00140000);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_B_EXPECTED_FRAME,
+		(icd->user_height << 16) |
+		(0x100 << 4) | /* Wait 0x100 vi clks for timeout */
+		0x1); /* Enable line timeout */
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_INPUT_STREAM_B_CONTROL,
+		     (0x3f << 16) | /* Skip packet threshold */
+		     (pdata->lanes - 1));
+
+	/* Use 0x00000022 for continuous clock mode. */
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CILB_CONTROL0,
+		(pdata->continuous_clk << 5) |
+		0x5); /* Clock settle time */
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_CSI_PPB_FRAME_START,
+		(0x1 << 8) | /* Enable continuous syncpt */
+		TEGRA_VI_SYNCPT_CSI_B);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PHY_CIL_COMMAND, 0x00010002);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND, 0x0000f002);
+}
+
+static void tegra_camera_capture_setup_vip(struct tegra_camera_dev *pcdev,
+					   struct soc_camera_device *icd,
+					   u32 input_control)
+{
+	struct tegra_camera_platform_data *pdata = icd->link->priv;
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_CORE_CONTROL, 0x00000000);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_INPUT_CONTROL,
+		/* (1 << 27) | field detect */
+		(0 << 28) |  /* 1 == top field is even field, 00 == odd */
+		(((pdata->internal_sync == true) ? 1 : 0) << 25) |
+						/* 1 == hsync/vsync decoded
+						   internally from data
+						   (BT.656) */
+		(1 << 1) | /* VIP_INPUT_ENABLE */
+		input_control);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_H_DOWNSCALE_CONTROL, 0x00000000);
+	TC_VI_REG_WT(pcdev, TEGRA_VI_V_DOWNSCALE_CONTROL, 0x00000000);
+
+	/* VIP H_ACTIVE and V_ACTIVE */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VIP_H_ACTIVE,
+		(icd->user_width << 16) |
+		(pdata->vip_h_active_start - ((pdata->internal_sync == true) ? 1 : 0)));
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VIP_V_ACTIVE,
+		((IS_INTERLACED ? (icd->user_height/2) : (icd->user_height)) << 16) |
+		pdata->vip_v_active_start);
+
+	/*
+	 * For VIP, D9..D2 is mapped to the video decoder's P7..P0.
+	 * Disable/mask out the other Dn wires.
+	 */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_PIN_INPUT_ENABLE, 0x000003fc | 0x6000); /* D2..D9 + VSYNC + HSYNC */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_DATA_INPUT_CONTROL, 0x000003fc);
+	TC_VI_REG_WT(pcdev, TEGRA_VI_PIN_INVERSION, 0x00000000);
+
+	TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_OUT_1,
+		(0x1 << 8) | /* Enable continuous syncpt */
+		TEGRA_VI_SYNCPT_VI);
+
+	/* TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL, 0x00000004); */
+}
+
+struct vb2_dc_buf {
+	struct vb2_dc_conf		*conf;
+	void				*vaddr;
+	dma_addr_t			paddr;
+	unsigned long			size;
+	struct vm_area_struct		*vma;
+	atomic_t			refcount;
+	struct vb2_vmarea_handler	handler;
+
+	struct nvmap_handle_ref		*nvmap_ref;
+};
+
+static int tegra_camera_capture_output_channel_setup(
+		struct tegra_camera_dev *pcdev,
+		struct soc_camera_device *icd)
+{
+	struct tegra_camera_platform_data *pdata = icd->link->priv;
+	int port = pdata->port;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+	const struct soc_camera_format_xlate *current_fmt = icd->current_fmt;
+	u32 output_fourcc = current_fmt->host_fmt->fourcc;
+	u32 output_format, output_control;
+	int frame_count;
+	struct tegra_buffer *buf = to_tegra_vb(pcdev->active);
+
+	switch (output_fourcc) {
+	case V4L2_PIX_FMT_UYVY:
+		output_format = 0x3; /* Default to YUV422 */
+		break;
+	case V4L2_PIX_FMT_VYUY:
+		output_format = (0x1 << 17) | 0x3;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		output_format = (0x2 << 17) | 0x3;
+		break;
+	case V4L2_PIX_FMT_YVYU:
+		output_format = (0x3 << 17) | 0x3;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		output_format = 0x6; /* YUV420 planar */
+		break;
+	case V4L2_PIX_FMT_SBGGR8:
+	case V4L2_PIX_FMT_SBGGR10:
+		/* Use second output channel for RAW8/RAW10 */
+		buf->output_channel = 1;
+
+		if (port == TEGRA_CAMERA_PORT_CSI_A)
+			output_format = 0x7;
+		else if (port == TEGRA_CAMERA_PORT_CSI_B)
+			output_format = 0x8;
+		else
+			output_format = 0x9;
+		break;
+	default:
+		dev_err(&pcdev->ndev->dev, "Wrong output format %d\n",
+			output_fourcc);
+		return -EINVAL;
+	}
+
+	output_control = (pdata->flip_v ? (0x1 << 20) : 0) |
+			(pdata->flip_h ? (0x1 << 19) : 0) |
+			output_format;
+
+	/* if the video is interlaced, then take two frames */
+	frame_count = IS_INTERLACED ? 2 : 1;
+
+	if (buf->output_channel == 0) {
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VI_FIRST_OUTPUT_CONTROL,
+				output_control);
+		/*
+		 * Set up frame size.  Bits 31:16 are the number of lines, and
+		 * bits 15:0 are the number of pixels per line.
+		 */
+		TC_VI_REG_WT(pcdev, TEGRA_VI_FIRST_OUTPUT_FRAME_SIZE,
+				((icd->user_height/frame_count) << 16) | icd->user_width);
+
+		/* First output memory enabled */
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VI_ENABLE, 0x00000000);
+
+		/* Set the number of frames in the buffer. */
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_COUNT_FIRST, frame_count);
+
+		/* Set up buffer frame size. */
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_SIZE_FIRST,
+				((icd->user_height/frame_count) << 16) | icd->user_width);
+
+		if (output_fourcc == V4L2_PIX_FMT_YUV420 || output_fourcc == V4L2_PIX_FMT_YVU420) {
+			TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BUFFER_STRIDE_FIRST,
+					((icd->user_height/frame_count) * icd->user_width) | (2<<30));
+		} else {
+			TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BUFFER_STRIDE_FIRST,
+					((icd->user_height/frame_count) * bytes_per_line) | (2<<30));
+		}
+
+		TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_OUT_1,
+				(0x1 << 8) | /* Enable continuous syncpt */
+				TEGRA_VI_SYNCPT_VI);
+
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VI_ENABLE, 0x00000000);
+	} else if (buf->output_channel == 1) {
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VI_SECOND_OUTPUT_CONTROL,
+				output_control);
+
+		TC_VI_REG_WT(pcdev, TEGRA_VI_SECOND_OUTPUT_FRAME_SIZE,
+				(icd->user_height << 16) | icd->user_width);
+
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VI_ENABLE_2, 0x00000000);
+
+		/* Set the number of frames in the buffer. */
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_COUNT_SECOND, 0x00000001);
+
+		/* Set up buffer frame size. */
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_SIZE_SECOND,
+				(icd->user_height << 16) | icd->user_width);
+
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BUFFER_STRIDE_SECOND,
+				(icd->user_height * bytes_per_line));
+
+		TC_VI_REG_WT(pcdev, TEGRA_VI_CONT_SYNCPT_OUT_2,
+				(0x1 << 8) | /* Enable continuous syncpt */
+				TEGRA_VI_SYNCPT_VI);
+
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VI_ENABLE_2, 0x00000000);
+	} else {
+		dev_err(&pcdev->ndev->dev, "Wrong output channel %d\n",
+			buf->output_channel);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int tegra_camera_capture_setup(struct tegra_camera_dev *pcdev)
+{
+	struct vb2_buffer *vb = pcdev->active;
+	struct tegra_buffer *buf = to_tegra_vb(vb);
+	struct soc_camera_device *icd = buf->icd;
+	struct tegra_camera_platform_data *pdata = icd->link->priv;
+	int port = pdata->port;
+	const struct soc_camera_format_xlate *current_fmt = icd->current_fmt;
+	enum v4l2_mbus_pixelcode input_code = current_fmt->code;
+	u32 hdr, input_control = 0x0;
+
+	if (!pcdev->icd)
+		pcdev->icd = icd;
+
+	switch (input_code) {
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+		input_control |= 0x2 << 8;
+		hdr = 30;
+		break;
+	case V4L2_MBUS_FMT_VYUY8_2X8:
+		input_control |= 0x3 << 8;
+		hdr = 30;
+		break;
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+		input_control |= 0x0;
+		hdr = 30;
+		break;
+	case V4L2_MBUS_FMT_YVYU8_2X8:
+		input_control |= 0x1 << 8;
+		hdr = 30;
+		break;
+	case V4L2_MBUS_FMT_SBGGR8_1X8:
+		input_control |= 0x2 << 2;	/* Input Format = Bayer */
+		hdr = 42;
+		break;
+	case V4L2_MBUS_FMT_SBGGR10_1X10:
+		input_control |= 0x2 << 2;	/* Input Format = Bayer */
+		hdr = 43;
+		break;
+	default:
+		dev_err(&pcdev->ndev->dev, "Input format %d is not supported\n",
+			input_code);
+		return -EINVAL;
+	}
+
+	/*
+	 * Set up low pass filter.  Use 0x240 for chromaticity and 0x240
+	 * for luminance, which is the default and means not to touch
+	 * anything.
+	 */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_H_LPF_CONTROL, 0x02400240);
+
+	/* Set up raise-on-edge, so we get an interrupt on end of frame. */
+	TC_VI_REG_WT(pcdev, TEGRA_VI_VI_RAISE, 0x00000001);
+
+	if (!pdata->continuous_capture || !pcdev->capturing) {
+		/* Cleanup registers */
+		tegra_camera_capture_clean(pcdev);
+	}
+
+	/* Setup registers for CSI-A, CSI-B and VIP inputs */
+	if (port == TEGRA_CAMERA_PORT_CSI_A)
+		tegra_camera_capture_setup_csi_a(pcdev, icd, hdr);
+	else if (port == TEGRA_CAMERA_PORT_CSI_B)
+		tegra_camera_capture_setup_csi_b(pcdev, icd, hdr);
+	else
+		tegra_camera_capture_setup_vip(pcdev, icd, input_control);
+
+	/* Setup registers for output channels */
+	return tegra_camera_capture_output_channel_setup(pcdev, icd);
+}
+
+static int tegra_camera_capture_buffer_setup(struct tegra_camera_dev *pcdev,
+			struct tegra_buffer *buf)
+{
+	struct soc_camera_device *icd = buf->icd;
+
+	switch (icd->current_fmt->host_fmt->fourcc) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BASE_ADDRESS_U,
+			     buf->buffer_addr_u);
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_START_ADDRESS_U,
+			     buf->start_addr_u);
+
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BASE_ADDRESS_V,
+			     buf->buffer_addr_v);
+		TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_START_ADDRESS_V,
+			     buf->start_addr_v);
+
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_VYUY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_YVYU:
+	case V4L2_PIX_FMT_SGRBG8:
+	case V4L2_PIX_FMT_SGRBG10:
+		/* output 1 */
+		if (buf->output_channel == 0) {
+			TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BASE_ADDRESS_FIRST,
+					buf->buffer_addr);
+			TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_START_ADDRESS_FIRST,
+					buf->start_addr);
+		/* output 2 */
+		} else if (buf->output_channel == 1) {
+			TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_BASE_ADDRESS_SECOND,
+					buf->buffer_addr);
+			TC_VI_REG_WT(pcdev, TEGRA_VI_VB0_START_ADDRESS_SECOND,
+					buf->start_addr);
+		} else {
+			dev_err(&pcdev->ndev->dev, "Wrong output channel %d\n",
+				buf->output_channel);
+			return -EINVAL;
+		}
+	break;
+
+	default:
+		dev_err(&pcdev->ndev->dev, "Wrong host format %d\n",
+			icd->current_fmt->host_fmt->fourcc);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int tegra_camera_capture_start(struct tegra_camera_dev *pcdev,
+				      struct tegra_buffer *buf)
+{
+	struct soc_camera_device *icd = buf->icd;
+	struct tegra_camera_platform_data *pdata = icd->link->priv;
+	int port = pdata->port;
+	int err;
+	int count = 1;
+	struct task_struct *interlace_task;
+	struct thread_args ta;
+
+	err = tegra_camera_capture_buffer_setup(pcdev, buf);
+	if (err < 0)
+		return err;
+	/*
+	 * Only wait on CSI frame end syncpt if we're using CSI.  Otherwise,
+	 * wait on VIP VSYNC syncpt.
+	 */
+	if (port == TEGRA_CAMERA_PORT_CSI_A) {
+		pcdev->syncpt_csi_a = nvhost_syncpt_incr_max_ext(pcdev->ndev,
+						TEGRA_VI_SYNCPT_CSI_A, 1);
+		TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
+				pdata->continuous_capture?0x0000f001 :0x0000f005);
+
+		err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
+				TEGRA_VI_SYNCPT_CSI_A,
+				pcdev->syncpt_csi_a,
+				TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
+				NULL);
+	} else if (port == TEGRA_CAMERA_PORT_CSI_B) {
+		pcdev->syncpt_csi_b = nvhost_syncpt_incr_max_ext(pcdev->ndev,
+						TEGRA_VI_SYNCPT_CSI_B, 1);
+		TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
+				pdata->continuous_capture? 0x0000f001: 0x0000f005);
+
+		err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
+				TEGRA_VI_SYNCPT_CSI_B,
+				pcdev->syncpt_csi_b,
+				TEGRA_SYNCPT_CSI_WAIT_TIMEOUT,
+				NULL);
+	} else {
+		if (IS_INTERLACED) {
+			count = 2; /* if interlaced - grab two frames */
+			ta.tb = buf;
+			ta.pcdev = pcdev;
+			interlace_task = kthread_run(make_interlaced, &ta, "interlacing thread");
+		}
+		while (count-- && !err) {
+			pcdev->syncpt_vi = nvhost_syncpt_incr_max_ext(pcdev->ndev,
+							TEGRA_VI_SYNCPT_VI, 1);
+			TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL,
+					0x00000001);
+			err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
+					TEGRA_VI_SYNCPT_VI,
+					pcdev->syncpt_vi,
+					TEGRA_SYNCPT_VI_WAIT_TIMEOUT,
+					NULL);
+		}
+	}
+
+	pcdev->capturing = 1;
+
+	if (!err)
+		return 0;
+
+	if (tegra_camera_port_is_csi(port)) {
+		u32 ppstatus;
+		u32 cilstatus;
+		u32 rostatus;
+
+		dev_warn(&icd->vdev->dev, "Timeout on CSI syncpt\n");
+		dev_warn(&icd->vdev->dev, "buffer_addr = 0x%08x\n",
+			buf->buffer_addr);
+
+		ppstatus = TC_VI_REG_RD(pcdev,
+			TEGRA_CSI_CSI_PIXEL_PARSER_STATUS);
+		cilstatus = TC_VI_REG_RD(pcdev,
+			 TEGRA_CSI_CSI_CIL_STATUS);
+		rostatus = TC_VI_REG_RD(pcdev,
+			TEGRA_CSI_CSI_READONLY_STATUS);
+
+		dev_warn(&icd->vdev->dev,
+			"PPSTATUS = 0x%08x, "
+			"CILSTATUS = 0x%08x, "
+			"ROSTATUS = 0x%08x\n",
+			ppstatus, cilstatus, rostatus);
+	} else {
+		u32 vip_input_status;
+
+		dev_warn(&pcdev->ndev->dev, "Timeout on VI syncpt\n");
+		dev_warn(&pcdev->ndev->dev, "buffer_addr = 0x%08x\n",
+			buf->buffer_addr);
+
+		vip_input_status = TC_VI_REG_RD(pcdev,
+			TEGRA_VI_VIP_INPUT_STATUS);
+
+		dev_warn(&pcdev->ndev->dev,
+			"VIP_INPUT_STATUS = 0x%08x\n",
+			vip_input_status);
+	}
+
+	return err;
+}
+
+static int tegra_camera_capture_stop(struct tegra_camera_dev *pcdev, int port)
+{
+	int err;
+	struct tegra_buffer *buf = to_tegra_vb(pcdev->active);
+
+	if (port == TEGRA_CAMERA_PORT_CSI_A)
+		TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
+			     0x0000f002);
+	else if (port == TEGRA_CAMERA_PORT_CSI_B)
+		TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
+			     0x0000f002);
+	else
+		TC_VI_REG_WT(pcdev, TEGRA_VI_CAMERA_CONTROL,
+			     0x00000005);
+
+	pcdev->syncpt_vi = nvhost_syncpt_incr_max_ext(pcdev->ndev,
+					TEGRA_VI_SYNCPT_VI, 1);
+	if (tegra_camera_port_is_csi(port))
+		err = nvhost_syncpt_wait_timeout_ext(pcdev->ndev,
+			TEGRA_VI_SYNCPT_VI,
+			pcdev->syncpt_vi,
+			TEGRA_SYNCPT_VI_WAIT_TIMEOUT,
+			NULL);
+	else
+		err = 0;
+
+	if (err) {
+		u32 buffer_addr;
+		u32 ppstatus;
+		u32 cilstatus;
+
+		dev_warn(&pcdev->ndev->dev, "Timeout on VI syncpt\n");
+
+		if (buf->output_channel == 0)
+			buffer_addr = TC_VI_REG_RD(pcdev,
+					   TEGRA_VI_VB0_BASE_ADDRESS_FIRST);
+		else if (buf->output_channel == 1)
+			buffer_addr = TC_VI_REG_RD(pcdev,
+					   TEGRA_VI_VB0_BASE_ADDRESS_SECOND);
+		else {
+			dev_err(&pcdev->ndev->dev, "Wrong output channel %d\n",
+				buf->output_channel);
+			return -EINVAL;
+		}
+
+		dev_warn(&pcdev->ndev->dev, "buffer_addr = 0x%08x\n",
+			buffer_addr);
+
+		ppstatus = TC_VI_REG_RD(pcdev,
+					TEGRA_CSI_CSI_PIXEL_PARSER_STATUS);
+		cilstatus = TC_VI_REG_RD(pcdev,
+					 TEGRA_CSI_CSI_CIL_STATUS);
+		dev_warn(&pcdev->ndev->dev,
+			"PPSTATUS = 0x%08x, CILSTATUS = 0x%08x\n",
+			ppstatus, cilstatus);
+	}
+
+	return err;
+}
+
+static void tegra_camera_activate(struct tegra_camera_dev *pcdev)
+{
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	u32 val;
+	void __iomem *apb_misc;
+#endif
+
+	nvhost_module_busy_ext(pcdev->ndev);
+
+	/* Enable external power */
+	regulator_enable(pcdev->reg);
+
+	/* Unpowergate VE */
+	tegra_unpowergate_partition(TEGRA_POWERGATE_VENC);
+
+	/* Turn on relevant clocks. */
+	clk_set_rate(pcdev->clk_vi, 300000000);
+	clk_enable(pcdev->clk_vi);
+	clk_set_rate(pcdev->clk_vi_sensor, 24000000);
+	clk_enable(pcdev->clk_vi_sensor);
+	clk_enable(pcdev->clk_csi);
+	clk_enable(pcdev->clk_isp);
+	clk_enable(pcdev->clk_csus);
+	clk_set_rate(pcdev->clk_sclk, 80000000);
+	clk_enable(pcdev->clk_sclk);
+	clk_enable(pcdev->clk_emc);
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	apb_misc = IO_ADDRESS(TEGRA_APB_MISC_BASE);
+	val = readl(apb_misc + 0x42c);
+	writel(val | 0x1, apb_misc + 0x42c);
+#endif
+
+	/* Save current syncpt values. */
+	tegra_camera_save_syncpts(pcdev);
+}
+
+static void tegra_camera_deactivate(struct tegra_camera_dev *pcdev)
+{
+	struct soc_camera_device *icd = pcdev->icd;
+	if (icd) {
+		struct tegra_camera_platform_data *pdata = icd->link->priv;
+
+		if (pdata->continuous_capture) {
+			pcdev->capturing = 0;
+			TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
+						 0x0000f003);
+			TC_VI_REG_WT(pcdev, TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
+						 0x0000f003);
+			usleep_range(20000, 21000);
+		}
+	}
+
+	/* Turn off relevant clocks. */
+	clk_disable(pcdev->clk_vi);
+	clk_disable(pcdev->clk_vi_sensor);
+	clk_disable(pcdev->clk_csi);
+	clk_disable(pcdev->clk_isp);
+	clk_disable(pcdev->clk_csus);
+	clk_disable(pcdev->clk_sclk);
+	clk_disable(pcdev->clk_emc);
+
+	/* Powergate VE */
+	tegra_powergate_partition(TEGRA_POWERGATE_VENC);
+
+	/* Disable external power */
+	regulator_disable(pcdev->reg);
+
+	nvhost_module_idle_ext(pcdev->ndev);
+
+	pcdev->cal_done = 0;
+}
+
+static int tegra_camera_capture_frame(struct tegra_camera_dev *pcdev)
+{
+	struct vb2_buffer *vb = pcdev->active;
+	struct tegra_buffer *buf = to_tegra_vb(vb);
+	struct soc_camera_device *icd = buf->icd;
+	struct tegra_camera_platform_data *pdata = icd->link->priv;
+	int port = pdata->port;
+	int retry = TEGRA_SYNCPT_RETRY_COUNT;
+	int err;
+
+	while (retry) {
+		err = tegra_camera_capture_start(pcdev, buf);
+		/* Capturing succeed, stop capturing */
+		if (!err)
+			err = tegra_camera_capture_stop(pcdev, port);
+		/* Capturing failed, stop and retry */
+		else {
+			retry--;
+
+			/* Stop streaming. */
+			if (port == TEGRA_CAMERA_PORT_CSI_A) {
+				TC_VI_REG_WT(pcdev,
+					     TEGRA_CSI_PIXEL_STREAM_PPA_COMMAND,
+					     0x0000f002);
+				/* Clear status registers. */
+				TC_VI_REG_WT(pcdev,
+					     TEGRA_CSI_CSI_PIXEL_PARSER_STATUS,
+					     0xffffffff);
+				TC_VI_REG_WT(pcdev,
+					     TEGRA_CSI_CSI_CIL_STATUS,
+					     0xffffffff);
+			} else if (port == TEGRA_CAMERA_PORT_CSI_B) {
+				TC_VI_REG_WT(pcdev,
+					     TEGRA_CSI_PIXEL_STREAM_PPB_COMMAND,
+					     0x0000f002);
+				/* Clear status registers. */
+				TC_VI_REG_WT(pcdev,
+					     TEGRA_CSI_CSI_PIXEL_PARSER_STATUS,
+					     0xffffffff);
+				TC_VI_REG_WT(pcdev,
+					     TEGRA_CSI_CSI_CIL_STATUS,
+					     0xffffffff);
+			} else {
+				TC_VI_REG_WT(pcdev,
+					     TEGRA_VI_CAMERA_CONTROL,
+					     0x00000005);
+			}
+
+			tegra_camera_incr_syncpts(pcdev);
+			tegra_camera_save_syncpts(pcdev);
+
+			continue;
+		}
+
+		break;
+	}
+
+	/* Reset hardware for too many errors */
+	if (!retry) {
+		tegra_camera_deactivate(pcdev);
+		mdelay(5);
+		tegra_camera_activate(pcdev);
+		if (pcdev->active)
+			tegra_camera_capture_setup(pcdev);
+	}
+
+	/* drop the first interlaced frame */
+	if (IS_INTERLACED && (pcdev->num_frames == 0)) {
+		pcdev->num_frames++;
+		return err;
+	}
+
+	spin_lock_irq(&pcdev->videobuf_queue_lock);
+
+	do_gettimeofday(&vb->v4l2_buf.timestamp);
+	vb->v4l2_buf.field = pcdev->field;
+	if (port == TEGRA_CAMERA_PORT_CSI_A)
+		vb->v4l2_buf.sequence = pcdev->sequence_a++;
+	else if (port == TEGRA_CAMERA_PORT_CSI_B)
+		vb->v4l2_buf.sequence = pcdev->sequence_b++;
+
+	vb2_buffer_done(vb, (err != 0) ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+
+	list_del_init(&buf->queue);
+
+	pcdev->num_frames++;
+
+	spin_unlock_irq(&pcdev->videobuf_queue_lock);
+
+	return err;
+}
+
+static int tegra_camera_csi_pad_calibration(struct tegra_camera_dev *pcdev)
+{
+	struct vb2_buffer *vb = pcdev->active;
+	struct tegra_buffer *buf = to_tegra_vb(vb);
+	struct soc_camera_device *icd = buf->icd;
+	struct tegra_camera_platform_data *pdata = icd->link->priv;
+	int port = pdata->port;
+	int retry = 500;
+	u32 data;
+	int err = -EINVAL;
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_DSI_MIPI_CAL_CONFIG, 0x40300);
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_MIPIBIAS_PAD_CONFIG0, 0x50700);
+
+	if (port == TEGRA_CAMERA_PORT_CSI_B || pdata->lanes == 4)
+		TC_VI_REG_WT(pcdev, TEGRA_CSI_CILB_MIPI_CAL_CONFIG, 0x200004);
+
+	data = 0x2a000004;
+	if (port == TEGRA_CAMERA_PORT_CSI_A)
+		data |= 0x200000; /* MIPI_CAL_SELA */
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_MIPI_CAL_CONFIG, data);
+
+	/* Start calibration */
+	data |= 0x80000000;
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_MIPI_CAL_CONFIG, data);
+
+	while (retry--) {
+		data = TC_VI_REG_RD(pcdev, TEGRA_CSI_CSI_CIL_STATUS);
+		if ((data & 0x8000) == 0x8000)
+			break;
+		udelay(20);
+	}
+	if (!retry) {
+		dev_warn(&pcdev->ndev->dev, "MIPI calibration timeout!\n");
+		goto cal_out;
+	}
+
+	/* Clear status */
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_CIL_STATUS, data);
+	retry = 500;
+	while (retry--) {
+		data = TC_VI_REG_RD(pcdev, TEGRA_CSI_CSI_CIL_STATUS);
+		if ((data & 0x8000) == 0x0)
+			break;
+		udelay(20);
+	}
+
+	if (!retry) {
+		dev_warn(&pcdev->ndev->dev,
+			 "Clear MIPI calibration status timeout!\n");
+		goto cal_out;
+	}
+
+	data = TC_VI_REG_RD(pcdev, TEGRA_CSI_CSI_PIXEL_PARSER_STATUS);
+	err = TC_VI_REG_RD(pcdev, TEGRA_CSI_CSI_CIL_STATUS);
+	if (data | err) {
+		dev_warn(&pcdev->ndev->dev,
+			 "Calibration status not be cleared!\n");
+		err = -EINVAL;
+		goto cal_out;
+	}
+
+	/* Calibration succeed */
+	err = 0;
+
+cal_out:
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CSI_CIL_STATUS, data);
+
+	/* un-select to avoid interference with DSI */
+	if (port == TEGRA_CAMERA_PORT_CSI_B || pdata->lanes == 4)
+		TC_VI_REG_WT(pcdev, TEGRA_CSI_CILB_MIPI_CAL_CONFIG, 0x4);
+
+	TC_VI_REG_WT(pcdev, TEGRA_CSI_CILA_MIPI_CAL_CONFIG, 0x2a000004);
+
+	return err;
+}
+
+static void tegra_camera_work(struct work_struct *work)
+{
+	struct tegra_camera_dev *pcdev =
+		container_of(work, struct tegra_camera_dev, work);
+	struct tegra_buffer *buf;
+	struct soc_camera_device *icd;
+	struct tegra_camera_platform_data *pdata;
+
+	while (1) {
+		mutex_lock(&pcdev->work_mutex);
+
+		spin_lock_irq(&pcdev->videobuf_queue_lock);
+		if (list_empty(&pcdev->capture)) {
+			pcdev->active = NULL;
+			spin_unlock_irq(&pcdev->videobuf_queue_lock);
+			mutex_unlock(&pcdev->work_mutex);
+			return;
+		}
+
+		buf = list_entry(pcdev->capture.next, struct tegra_buffer,
+				queue);
+		pcdev->active = &buf->vb;
+		spin_unlock_irq(&pcdev->videobuf_queue_lock);
+
+		icd = buf->icd;
+		pdata = icd->link->priv;
+
+		if (!pdata->continuous_capture || !pcdev->capturing) {
+			tegra_camera_capture_setup(pcdev);
+			if (!pcdev->cal_done) {
+				tegra_camera_csi_pad_calibration(pcdev);
+				pcdev->cal_done = 1;
+			}
+		}
+		tegra_camera_capture_frame(pcdev);
+
+		mutex_unlock(&pcdev->work_mutex);
+	}
+}
+
+static int tegra_camera_init_buffer(struct tegra_buffer *buf)
+{
+	struct soc_camera_device *icd = buf->icd;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+	struct tegra_camera_platform_data *pdata = icd->link->priv;
+
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_camera_dev *pcdev = ici->priv;
+	if (IS_INTERLACED) {
+		buf->internal_phys_addr = pcdev->internal_vbuf->paddr; /* physical addr of internal buffer */
+		buf->internal_virtual_addr = pcdev->internal_vbuf->vaddr; /* virtual address of internal buffer */
+		buf->buffer_addr = buf->internal_phys_addr; /* internal buffer -> buffer for decoding */
+		buf->virtual_addr = vb2_plane_vaddr(&buf->vb, 0); /* save virtual addr for later interlace handling */
+	} else
+		buf->buffer_addr = vb2_dma_nvmap_plane_paddr(&buf->vb, 0); /* physical addr */
+
+	switch (icd->current_fmt->host_fmt->fourcc) {
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_VYUY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_YVYU:
+	case V4L2_PIX_FMT_SGRBG8:
+	case V4L2_PIX_FMT_SGRBG10:
+		buf->start_addr = buf->buffer_addr;
+
+		if (pdata->flip_v)
+			buf->start_addr += bytes_per_line *
+					   (icd->user_height-1);
+
+		if (pdata->flip_h)
+			buf->start_addr += bytes_per_line - 1;
+
+		break;
+
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		buf->buffer_addr_u = buf->buffer_addr +
+				     icd->user_width * icd->user_height;
+		buf->buffer_addr_v = buf->buffer_addr_u +
+				     (icd->user_width * icd->user_height) / 4;
+
+		/* For YVU420, we swap the locations of the U and V planes. */
+		if (icd->current_fmt->host_fmt->fourcc == V4L2_PIX_FMT_YVU420) {
+			dma_addr_t temp = buf->buffer_addr_u;
+			buf->buffer_addr_u = buf->buffer_addr_v;
+			buf->buffer_addr_v = temp;
+		}
+
+		buf->start_addr = buf->buffer_addr;
+		buf->start_addr_u = buf->buffer_addr_u;
+		buf->start_addr_v = buf->buffer_addr_v;
+
+		if (pdata->flip_v) {
+			buf->start_addr += icd->user_width *
+					   (icd->user_height - 1);
+
+			buf->start_addr_u += ((icd->user_width/2) *
+					      ((icd->user_height/2) - 1));
+
+			buf->start_addr_v += ((icd->user_width/2) *
+					      ((icd->user_height/2) - 1));
+		}
+
+		if (pdata->flip_h) {
+			buf->start_addr += icd->user_width - 1;
+
+			buf->start_addr_u += (icd->user_width/2) - 1;
+
+			buf->start_addr_v += (icd->user_width/2) - 1;
+		}
+
+		break;
+
+	default:
+		dev_err(icd->parent, "Wrong host format %d\n",
+			icd->current_fmt->host_fmt->fourcc);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ *  Videobuf operations
+ */
+static int tegra_camera_videobuf_setup(struct vb2_queue *vq,
+				       unsigned int *num_buffers,
+				       unsigned int *num_planes,
+				       unsigned long sizes[],
+				       void *alloc_ctxs[])
+{
+	struct soc_camera_device *icd = container_of(vq,
+						     struct soc_camera_device,
+						     vb2_vidq);
+	struct tegra_camera_platform_data *pdata = icd->link->priv;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_camera_dev *pcdev = ici->priv;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+
+	dev_dbg(icd->parent, "In tegra_camera_videobuf_setup()\n");
+
+	if (bytes_per_line < 0)
+		return bytes_per_line;
+
+	*num_planes = 1;
+
+	if (pdata->port == TEGRA_CAMERA_PORT_CSI_A)
+		pcdev->sequence_a = 0;
+	else if (pdata->port == TEGRA_CAMERA_PORT_CSI_B)
+		pcdev->sequence_b = 0;
+	sizes[0] = bytes_per_line * icd->user_height;
+	alloc_ctxs[0] = pcdev->alloc_ctx;
+
+	if (!*num_buffers)
+		*num_buffers = 2;
+
+	if (IS_INTERLACED)
+		pcdev->internal_vbuf = vb2_dma_nvmap_memops.alloc(pcdev->alloc_ctx, bytes_per_line * icd->user_height);
+
+	return 0;
+}
+
+static int tegra_camera_videobuf_prepare(struct vb2_buffer *vb)
+{
+	struct soc_camera_device *icd = container_of(vb->vb2_queue,
+						     struct soc_camera_device,
+						     vb2_vidq);
+	struct tegra_buffer *buf = to_tegra_vb(vb);
+	struct tegra_camera_platform_data *pdata = icd->link->priv;
+	int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
+						icd->current_fmt->host_fmt);
+	unsigned long size;
+
+	dev_dbg(icd->parent, "In tegra_camera_videobuf_prepare()\n");
+
+	if (bytes_per_line < 0)
+		return bytes_per_line;
+
+	buf->icd = icd;
+
+	if (!pdata) {
+		dev_err(icd->parent, "No platform data for this device!\n");
+		return -EINVAL;
+	}
+
+	if (!tegra_camera_port_is_valid(pdata->port)) {
+		dev_err(icd->parent,
+			"Invalid camera port %d in platform data\n",
+			pdata->port);
+		return -EINVAL;
+	}
+
+	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+		vb, vb2_plane_vaddr(vb, 0), vb2_plane_size(vb, 0));
+
+#ifdef PREFILL_BUFFER
+	/*
+	 * This can be useful if you want to see if we actually fill
+	 * the buffer with something
+	 */
+	if (vb2_plane_vaddr(vb, 0))
+		memset(vb2_plane_vaddr(vb, 0), 0xbd, vb2_plane_size(vb, 0));
+#endif
+
+	if (!icd->current_fmt) {
+		dev_err(icd->parent, "%s NULL format point\n", __func__);
+		return -EINVAL;
+	}
+
+	size = icd->user_height * bytes_per_line;
+
+	if (vb2_plane_size(vb, 0) < size) {
+		dev_err(icd->parent, "Buffer too small (%lu < %lu)\n",
+			vb2_plane_size(vb, 0), size);
+		return -ENOBUFS;
+	}
+
+	vb2_set_plane_payload(vb, 0, size);
+
+	return tegra_camera_init_buffer(buf);
+}
+
+static void tegra_camera_videobuf_queue(struct vb2_buffer *vb)
+{
+	struct soc_camera_device *icd = container_of(vb->vb2_queue,
+						     struct soc_camera_device,
+						     vb2_vidq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_camera_dev *pcdev = ici->priv;
+	struct tegra_buffer *buf = to_tegra_vb(vb);
+
+	dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__,
+		vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0));
+
+	spin_lock_irq(&pcdev->videobuf_queue_lock);
+	list_add_tail(&buf->queue, &pcdev->capture);
+	schedule_work(&pcdev->work);
+	spin_unlock_irq(&pcdev->videobuf_queue_lock);
+
+	dev_dbg(icd->parent, "Finished tegra_camera_videobuf_queue()\n");
+}
+
+static void tegra_camera_videobuf_release(struct vb2_buffer *vb)
+{
+	struct soc_camera_device *icd = container_of(vb->vb2_queue,
+						     struct soc_camera_device,
+						     vb2_vidq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_buffer *buf = to_tegra_vb(vb);
+	struct tegra_camera_dev *pcdev = ici->priv;
+
+	dev_dbg(icd->parent, "In tegra_camera_videobuf_release()\n");
+
+	if (IS_INTERLACED && pcdev->internal_vbuf != NULL) {
+		vb2_dma_nvmap_memops.put(pcdev->internal_vbuf);
+		pcdev->internal_vbuf = NULL;
+	}
+
+	mutex_lock(&pcdev->work_mutex);
+
+	spin_lock_irq(&pcdev->videobuf_queue_lock);
+
+	if (pcdev->active == vb)
+		pcdev->active = NULL;
+
+	/*
+	 * Doesn't hurt also if the list is empty, but it hurts, if queuing the
+	 * buffer failed, and .buf_init() hasn't been called
+	 */
+	if (buf->queue.next)
+		list_del_init(&buf->queue);
+
+	spin_unlock_irq(&pcdev->videobuf_queue_lock);
+
+	mutex_unlock(&pcdev->work_mutex);
+
+	dev_dbg(icd->parent, "Finished tegra_camera_videobuf_release()\n");
+}
+
+static int tegra_camera_videobuf_init(struct vb2_buffer *vb)
+{
+	/* This is for locking debugging only */
+	INIT_LIST_HEAD(&to_tegra_vb(vb)->queue);
+
+	return 0;
+}
+
+static int tegra_camera_stop_streaming(struct vb2_queue *q)
+{
+	struct soc_camera_device *icd = container_of(q,
+						     struct soc_camera_device,
+						     vb2_vidq);
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_camera_dev *pcdev = ici->priv;
+	struct list_head *buf_head, *tmp;
+
+
+	mutex_lock(&pcdev->work_mutex);
+
+	spin_lock_irq(&pcdev->videobuf_queue_lock);
+	list_for_each_safe(buf_head, tmp, &pcdev->capture) {
+		struct tegra_buffer *buf = container_of(buf_head,
+				struct tegra_buffer,
+				queue);
+		if (buf->icd == icd)
+			list_del_init(buf_head);
+	}
+	spin_unlock_irq(&pcdev->videobuf_queue_lock);
+
+	if (pcdev->active) {
+		struct tegra_buffer *buf = to_tegra_vb(pcdev->active);
+		if (buf->icd == icd)
+			pcdev->active = NULL;
+	}
+
+	mutex_unlock(&pcdev->work_mutex);
+
+	return 0;
+}
+
+static struct vb2_ops tegra_camera_videobuf_ops = {
+	.queue_setup	= tegra_camera_videobuf_setup,
+	.buf_prepare	= tegra_camera_videobuf_prepare,
+	.buf_queue	= tegra_camera_videobuf_queue,
+	.buf_cleanup	= tegra_camera_videobuf_release,
+	.buf_init	= tegra_camera_videobuf_init,
+	.wait_prepare	= soc_camera_unlock,
+	.wait_finish	= soc_camera_lock,
+	.stop_streaming	= tegra_camera_stop_streaming,
+};
+
+/*
+ *  SOC camera host operations
+ */
+static int tegra_camera_init_videobuf(struct vb2_queue *q,
+				      struct soc_camera_device *icd)
+{
+	dev_dbg(icd->parent, "In tegra_camera_init_videobuf()\n");
+
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR;
+	q->drv_priv = icd;
+	q->ops = &tegra_camera_videobuf_ops;
+	q->mem_ops = &vb2_dma_nvmap_memops;
+	q->buf_struct_size = sizeof(struct tegra_buffer);
+
+	dev_dbg(icd->parent, "Finished tegra_camera_init_videobuf()\n");
+
+	return vb2_queue_init(q);
+}
+
+/*
+ * Called with .video_lock held
+ */
+static int tegra_camera_add_device(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_camera_dev *pcdev = ici->priv;
+
+	if (!pcdev->enable_refcnt) {
+		pm_runtime_get_sync(ici->v4l2_dev.dev);
+		tegra_camera_activate(pcdev);
+		pcdev->num_frames = 0;
+	}
+	pcdev->enable_refcnt++;
+
+	dev_dbg(icd->parent, "TEGRA Camera host attached to camera %d\n",
+		icd->devnum);
+
+	return 0;
+}
+
+/* Called with .video_lock held */
+static void tegra_camera_remove_device(struct soc_camera_device *icd)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+	struct tegra_camera_dev *pcdev = ici->priv;
+
+	pcdev->enable_refcnt--;
+	if (!pcdev->enable_refcnt) {
+		cancel_work_sync(&pcdev->work);
+		tegra_camera_deactivate(pcdev);
+		pm_runtime_put_sync(ici->v4l2_dev.dev);
+	}
+
+	dev_dbg(icd->parent, "Frames captured: %d\n", pcdev->num_frames);
+
+	dev_dbg(icd->parent, "TEGRA camera host detached from camera %d\n",
+		icd->devnum);
+}
+
+static int tegra_camera_set_bus_param(struct soc_camera_device *icd,
+					__u32 pixfmt)
+{
+	return 0;
+}
+
+static int tegra_camera_get_formats(struct soc_camera_device *icd,
+				    unsigned int idx,
+				    struct soc_camera_format_xlate *xlate)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	struct device *dev = icd->parent;
+	int formats = 0;
+	int ret;
+	enum v4l2_mbus_pixelcode code;
+	const struct soc_mbus_pixelfmt *fmt;
+	int k;
+
+	ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
+	if (ret != 0)
+		/* No more formats */
+		return 0;
+
+	fmt = soc_mbus_get_fmtdesc(code);
+	if (!fmt) {
+		dev_err(dev, "Invalid format code #%u: %d\n", idx, code);
+		return 0;
+	}
+
+	switch (code) {
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+	case V4L2_MBUS_FMT_VYUY8_2X8:
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+	case V4L2_MBUS_FMT_YVYU8_2X8:
+	case V4L2_MBUS_FMT_SGRBG8_1X8:
+	case V4L2_MBUS_FMT_SGRBG10_1X10:
+		formats += ARRAY_SIZE(tegra_camera_formats);
+		for (k = 0;
+		     xlate && (k < ARRAY_SIZE(tegra_camera_formats));
+		     k++) {
+			xlate->host_fmt	= &tegra_camera_formats[k];
+			xlate->code	= code;
+			xlate++;
+
+			dev_info(dev, "Providing format %s using code %d\n",
+				 tegra_camera_formats[k].name, code);
+		}
+		break;
+	default:
+		dev_info(dev, "Not supporting %s\n", fmt->name);
+		return 0;
+	}
+
+	return formats;
+}
+
+static void tegra_camera_put_formats(struct soc_camera_device *icd)
+{
+	kfree(icd->host_priv);
+	icd->host_priv = NULL;
+}
+
+static int tegra_camera_set_fmt(struct soc_camera_device *icd,
+			      struct v4l2_format *f)
+{
+	struct device *dev = icd->parent;
+	struct soc_camera_host *ici = to_soc_camera_host(dev);
+	struct tegra_camera_dev *pcdev = ici->priv;
+
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate = NULL;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_mbus_framefmt mf;
+	int ret;
+
+	dev_dbg(dev, "In tegra_camera_set_fmt()\n");
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
+	if (!xlate) {
+		dev_warn(dev, "Format %x not found\n", pix->pixelformat);
+		return -EINVAL;
+	}
+
+	mf.width	= pix->width;
+	mf.height	= pix->height;
+	mf.field	= pix->field;
+	mf.colorspace	= pix->colorspace;
+	mf.code		= xlate->code;
+
+	ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
+	if (IS_ERR_VALUE(ret)) {
+		dev_warn(dev, "Failed to configure for format %x\n",
+			 pix->pixelformat);
+		return ret;
+	}
+
+	if (mf.code != xlate->code) {
+		dev_warn(dev, "mf.code = %d, xlate->code = %d, mismatch\n",
+			mf.code, xlate->code);
+		return -EINVAL;
+	}
+
+	icd->user_width		= mf.width;
+	icd->user_height	= mf.height;
+	icd->current_fmt	= xlate;
+
+	pcdev->field = pix->field;
+
+	dev_dbg(dev, "Finished tegra_camera_set_fmt(), returning %d\n", ret);
+
+	return ret;
+}
+
+static int tegra_camera_enum_fsizes(struct soc_camera_device *icd, struct v4l2_frmsizeenum * fsize) {
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	int ret;
+
+	//printk("%s (%d)\n", __func__, fsize->index);
+	ret = v4l2_subdev_call(sd, video, enum_framesizes, fsize);
+	if (IS_ERR_VALUE(ret))
+		return ret;
+
+	return 0;
+}
+
+static int tegra_camera_try_fmt(struct soc_camera_device *icd,
+				struct v4l2_format *f)
+{
+	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+	const struct soc_camera_format_xlate *xlate;
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct v4l2_mbus_framefmt mf;
+	__u32 pixfmt = pix->pixelformat;
+	int ret;
+
+	dev_dbg(icd->parent, "In tegra_camera_try_fmt()\n");
+
+	xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
+	if (!xlate) {
+		dev_warn(icd->parent, "Format %x not found\n", pixfmt);
+		return -EINVAL;
+	}
+
+	pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
+						    xlate->host_fmt);
+	if (pix->bytesperline < 0)
+		return pix->bytesperline;
+	pix->sizeimage = pix->height * pix->bytesperline;
+
+	/* limit to sensor capabilities */
+	mf.width	= pix->width;
+	mf.height	= pix->height;
+	mf.field	= pix->field;
+	mf.colorspace	= pix->colorspace;
+	mf.code		= xlate->code;
+
+	ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
+	if (IS_ERR_VALUE(ret))
+		return ret;
+
+	pix->width	= mf.width;
+	pix->height	= mf.height;
+	pix->colorspace	= mf.colorspace;
+	/*
+	 * width and height could have been changed, therefore update the
+	 * bytesperline and sizeimage here.
+	 */
+	pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
+						    xlate->host_fmt);
+	pix->sizeimage = pix->height * pix->bytesperline;
+
+	switch (mf.field) {
+	case V4L2_FIELD_ANY:
+	case V4L2_FIELD_NONE:
+		pix->field	= V4L2_FIELD_NONE;
+		break;
+	case V4L2_FIELD_INTERLACED_BT:
+		pix->field	= V4L2_FIELD_INTERLACED_BT;
+		break;
+	case V4L2_FIELD_INTERLACED_TB:
+		pix->field	= V4L2_FIELD_INTERLACED_TB;
+		break;
+	case V4L2_FIELD_INTERLACED:
+		pix->field	= V4L2_FIELD_INTERLACED;
+		break;
+	default:
+		/* TODO: support interlaced at least in pass-through mode */
+		dev_err(icd->parent, "Field type %d unsupported.\n",
+			mf.field);
+		return -EINVAL;
+	}
+
+	dev_dbg(icd->parent,
+		"Finished tegra_camera_try_fmt(), returning %d\n", ret);
+
+	return ret;
+}
+
+static int tegra_camera_reqbufs(struct soc_camera_device *icd,
+				struct v4l2_requestbuffers *p)
+{
+	return 0;
+}
+
+static unsigned int tegra_camera_poll(struct file *file, poll_table *pt)
+{
+	struct soc_camera_device *icd = file->private_data;
+
+	return vb2_poll(&icd->vb2_vidq, file, pt);
+}
+
+static int tegra_camera_querycap(struct soc_camera_host *ici,
+				 struct v4l2_capability *cap)
+{
+	strlcpy(cap->card, TEGRA_CAM_DRV_NAME, sizeof(cap->card));
+	cap->version = TEGRA_CAM_VERSION_CODE;
+	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+
+	return 0;
+}
+
+static struct soc_camera_host_ops tegra_soc_camera_host_ops = {
+	.owner		= THIS_MODULE,
+	.init_videobuf2	= tegra_camera_init_videobuf,
+	.add		= tegra_camera_add_device,
+	.remove		= tegra_camera_remove_device,
+	.set_bus_param	= tegra_camera_set_bus_param,
+	.get_formats	= tegra_camera_get_formats,
+	.put_formats	= tegra_camera_put_formats,
+	.set_fmt	= tegra_camera_set_fmt,
+	.try_fmt	= tegra_camera_try_fmt,
+	.reqbufs	= tegra_camera_reqbufs,
+	.poll		= tegra_camera_poll,
+	.querycap	= tegra_camera_querycap,
+	.enum_fsizes	= tegra_camera_enum_fsizes,
+};
+
+static int __devinit tegra_camera_probe(struct nvhost_device *ndev,
+					struct nvhost_device_id *id_table)
+{
+	struct tegra_camera_dev *pcdev;
+	int err = 0;
+
+	pcdev = kzalloc(sizeof(struct tegra_camera_dev), GFP_KERNEL);
+	if (!pcdev) {
+		dev_err(&ndev->dev, "Could not allocate pcdev\n");
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	pcdev->ndev = ndev;
+
+	pcdev->ici.priv		= pcdev;
+	pcdev->ici.v4l2_dev.dev	= &ndev->dev;
+	pcdev->ici.nr		= ndev->id;
+	pcdev->ici.drv_name	= dev_name(&ndev->dev);
+	pcdev->ici.ops		= &tegra_soc_camera_host_ops;
+
+	INIT_LIST_HEAD(&pcdev->capture);
+	INIT_WORK(&pcdev->work, tegra_camera_work);
+	spin_lock_init(&pcdev->videobuf_queue_lock);
+	mutex_init(&pcdev->work_mutex);
+
+	pcdev->clk_vi = clk_get_sys("tegra_camera", "vi");
+	if (IS_ERR_OR_NULL(pcdev->clk_vi)) {
+		dev_err(&ndev->dev, "Failed to get vi clock.\n");
+		goto exit_free_pcdev;
+	}
+
+	pcdev->clk_vi_sensor = clk_get_sys("tegra_camera", "vi_sensor");
+	if (IS_ERR_OR_NULL(pcdev->clk_vi_sensor)) {
+		dev_err(&ndev->dev, "Failed to get vi_sensor clock.\n");
+		goto exit_put_clk_vi;
+	}
+
+	pcdev->clk_csi = clk_get_sys("tegra_camera", "csi");
+	if (IS_ERR_OR_NULL(pcdev->clk_csi)) {
+		dev_err(&ndev->dev, "Failed to get csi clock.\n");
+		goto exit_put_clk_vi_sensor;
+	}
+
+	pcdev->clk_isp = clk_get_sys("tegra_camera", "isp");
+	if (IS_ERR_OR_NULL(pcdev->clk_isp)) {
+		dev_err(&ndev->dev, "Failed to get isp clock.\n");
+		goto exit_put_clk_csi;
+	}
+
+	pcdev->clk_csus = clk_get_sys("tegra_camera", "csus");
+	if (IS_ERR_OR_NULL(pcdev->clk_csus)) {
+		dev_err(&ndev->dev, "Failed to get csus clock.\n");
+		goto exit_put_clk_isp;
+	}
+
+	pcdev->clk_sclk = clk_get_sys("tegra_camera", "sclk");
+	if (IS_ERR_OR_NULL(pcdev->clk_sclk)) {
+		dev_err(&ndev->dev, "Failed to get sclk clock.\n");
+		goto exit_put_clk_csus;
+	}
+
+	pcdev->clk_emc = clk_get_sys("tegra_camera", "emc");
+	if (IS_ERR_OR_NULL(pcdev->clk_emc)) {
+		dev_err(&ndev->dev, "Failed to get emc clock.\n");
+		goto exit_put_clk_sclk;
+	}
+
+	clk_set_rate(pcdev->clk_vi, 300000000);
+	clk_set_rate(pcdev->clk_vi_sensor, 24000000);
+
+	/* Get regulator pointer */
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	pcdev->reg = regulator_get(&ndev->dev, "vcsi");
+#else
+	pcdev->reg = regulator_get(&ndev->dev, "avdd_dsi_csi");
+#endif
+	if (IS_ERR_OR_NULL(pcdev->reg)) {
+		dev_err(&ndev->dev, "%s: couldn't get regulator\n",
+				__func__);
+		goto exit_put_clk_emc;
+	}
+
+	nvhost_set_drvdata(ndev, pcdev);
+	err = nvhost_client_device_get_resources(ndev);
+	if (err) {
+		dev_err(&ndev->dev, "%s: nvhost get resources failed %d\n",
+				__func__, err);
+		goto exit_put_regulator;
+	}
+
+	/* initialize nvhost client device only the first time */
+	if (ndev->power_attrib == NULL) {
+		err = nvhost_client_device_init(ndev);
+		if (err) {
+			dev_err(&ndev->dev, "%s: nvhost init failed %d\n",
+					__func__, err);
+			goto exit_put_regulator;
+		}
+	}
+
+	pcdev->vi_base = ndev->aperture;
+
+	pm_suspend_ignore_children(&ndev->dev, true);
+	pm_runtime_enable(&ndev->dev);
+	pm_runtime_resume(&ndev->dev);
+
+	pcdev->alloc_ctx = vb2_dma_nvmap_init_ctx(&ndev->dev);
+	if (IS_ERR(pcdev->alloc_ctx)) {
+		err = PTR_ERR(pcdev->alloc_ctx);
+		goto exit_pm_disable;
+	}
+
+	err = soc_camera_host_register(&pcdev->ici);
+	if (IS_ERR_VALUE(err))
+		goto exit_cleanup_alloc_ctx;
+
+	dev_notice(&ndev->dev, "Tegra camera driver loaded.\n");
+
+	return err;
+
+exit_cleanup_alloc_ctx:
+	vb2_dma_nvmap_cleanup_ctx(pcdev->alloc_ctx);
+exit_pm_disable:
+	pm_runtime_disable(&ndev->dev);
+	nvhost_client_device_put_resources(ndev);
+exit_put_regulator:
+	regulator_put(pcdev->reg);
+exit_put_clk_emc:
+	clk_put(pcdev->clk_emc);
+exit_put_clk_sclk:
+	clk_put(pcdev->clk_sclk);
+exit_put_clk_csus:
+	clk_put(pcdev->clk_csus);
+exit_put_clk_isp:
+	clk_put(pcdev->clk_isp);
+exit_put_clk_csi:
+	clk_put(pcdev->clk_csi);
+exit_put_clk_vi_sensor:
+	clk_put(pcdev->clk_vi_sensor);
+exit_put_clk_vi:
+	clk_put(pcdev->clk_vi);
+exit_free_pcdev:
+	kfree(pcdev);
+exit:
+	return err;
+}
+
+static int __devexit tegra_camera_remove(struct nvhost_device *ndev)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(&ndev->dev);
+	struct tegra_camera_dev *pcdev = container_of(ici,
+					struct tegra_camera_dev, ici);
+	struct resource *res;
+
+	res = nvhost_get_resource_byname(ndev, IORESOURCE_MEM, "regs");
+	if (!res)
+		return -EBUSY;
+
+	soc_camera_host_unregister(ici);
+
+	vb2_dma_nvmap_cleanup_ctx(pcdev->alloc_ctx);
+
+	pm_runtime_disable(&ndev->dev);
+
+	regulator_put(pcdev->reg);
+
+	nvhost_client_device_put_resources(ndev);
+
+	clk_put(pcdev->clk_csus);
+	clk_put(pcdev->clk_isp);
+	clk_put(pcdev->clk_csi);
+	clk_put(pcdev->clk_vi_sensor);
+	clk_put(pcdev->clk_vi);
+
+	kfree(pcdev);
+
+	dev_notice(&ndev->dev, "Tegra camera host driver unloaded\n");
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_FISH
+static int tegra_camera_suspend(struct nvhost_device *ndev, pm_message_t state)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(&ndev->dev);
+	struct tegra_camera_dev *pcdev = container_of(ici,
+					struct tegra_camera_dev, ici);
+
+	mutex_lock(&pcdev->work_mutex);
+
+	/* We only need to do something if a camera sensor is attached. */
+	if (pcdev->icd) {
+		/* Suspend the camera sensor. */
+		WARN_ON(!pcdev->icd->ops->suspend);
+		pcdev->icd->ops->suspend(pcdev->icd, state);
+	}
+
+	return 0;
+}
+
+static int tegra_camera_resume(struct nvhost_device *ndev)
+{
+	struct soc_camera_host *ici = to_soc_camera_host(&ndev->dev);
+	struct tegra_camera_dev *pcdev = container_of(ici,
+					struct tegra_camera_dev, ici);
+
+	/* We only need to do something if a camera sensor is attached. */
+	if (pcdev->icd) {
+		/* Resume the camera host. */
+		tegra_camera_save_syncpts(pcdev);
+		if (pcdev->active)
+			tegra_camera_capture_setup(pcdev);
+
+		/* Resume the camera sensor. */
+		WARN_ON(!pcdev->icd->ops->resume);
+		pcdev->icd->ops->resume(pcdev->icd);
+	}
+
+	mutex_unlock(&pcdev->work_mutex);
+
+	return 0;
+}
+#endif
+
+static struct nvhost_driver tegra_camera_driver = {
+	.driver	= {
+		.name	= TEGRA_CAM_DRV_NAME,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= tegra_camera_probe,
+	.remove		= __devexit_p(tegra_camera_remove),
+#ifdef CONFIG_PM_FISH
+	.suspend	= tegra_camera_suspend,
+	.resume		= tegra_camera_resume,
+#endif
+};
+
+
+static int __init tegra_camera_init(void)
+{
+	return nvhost_driver_register(&tegra_camera_driver);
+}
+
+static void __exit tegra_camera_exit(void)
+{
+	nvhost_driver_unregister(&tegra_camera_driver);
+}
+
+module_init(tegra_camera_init);
+module_exit(tegra_camera_exit);
+
+MODULE_DESCRIPTION("TEGRA SoC Camera Host driver");
+MODULE_AUTHOR("Andrew Chew <achew@nvidia.com>");
+MODULE_AUTHOR("Bryan Wu <pengw@nvidia.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("nvhost:" TEGRA_CAM_DRV_NAME);
diff --git a/drivers/media/video/timblogiw.c b/drivers/media/video/timblogiw.c
index 84cd1b65b765..81f6c0501b25 100644
--- a/drivers/media/video/timblogiw.c
+++ b/drivers/media/video/timblogiw.c
@@ -563,8 +563,8 @@ static void buffer_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
 
 	spin_unlock_irq(&fh->queue_lock);
 
-	desc = fh->chan->device->device_prep_slave_sg(fh->chan,
-		buf->sg, sg_elems, DMA_FROM_DEVICE,
+	desc = dmaengine_prep_slave_sg(fh->chan,
+		buf->sg, sg_elems, DMA_DEV_TO_MEM,
 		DMA_PREP_INTERRUPT | DMA_COMPL_SKIP_SRC_UNMAP);
 	if (!desc) {
 		spin_lock_irq(&fh->queue_lock);
diff --git a/drivers/media/video/tvp5150_reg.h b/drivers/media/video/tvp5150_reg.h
index 4240043c0b2a..c4cdbadeead9 100644
--- a/drivers/media/video/tvp5150_reg.h
+++ b/drivers/media/video/tvp5150_reg.h
@@ -6,6 +6,9 @@
  */
 
 #define TVP5150_VD_IN_SRC_SEL_1      0x00 /* Video input source selection #1 */
+#define TVP5150_VIDEO_INPUT_SELECT_AIP1A        0x00
+#define TVP5150_VIDEO_INPUT_SELECT_AIP1B        0x02
+#define TVP5150_VIDEO_INPUT_SELECT_SVIDEO       0x01
 #define TVP5150_ANAL_CHL_CTL         0x01 /* Analog channel controls */
 #define TVP5150_OP_MODE_CTL          0x02 /* Operation mode controls */
 #define TVP5150_MISC_CTL             0x03 /* Miscellaneous controls */
diff --git a/drivers/media/video/tvp5150soc.c b/drivers/media/video/tvp5150soc.c
new file mode 100644
index 000000000000..3cb7cd6b4dc8
--- /dev/null
+++ b/drivers/media/video/tvp5150soc.c
@@ -0,0 +1,538 @@
+/*
+ * tvp5150 - Texas Instruments TVP5150A/AM1 video decoder driver
+ *
+ * Copyright (c) 2005,2006 Mauro Carvalho Chehab (mchehab@infradead.org)
+ * This code is placed under the terms of the GNU General Public License v2
+ */
+
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+
+#include <media/soc_camera.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-chip-ident.h>
+#include <media/v4l2-subdev.h>
+#include <media/v4l2-ioctl.h>
+
+#include <media/tvp5150.h>
+#include "tvp5150_reg.h"
+
+#define I2C_RETRY_COUNT                3
+#define LINE_PIXELS                        576
+#define FRAME_LINES                        520
+
+#define MODULE_NAME "tvp5150soc"
+
+static unsigned int debug = 0;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level");
+
+struct i2c_reg_value {
+        unsigned char reg;
+        unsigned char value;
+};
+
+static struct tvp5150soc_format_struct {
+        enum v4l2_mbus_pixelcode mbus_code;
+        enum v4l2_colorspace colorspace;
+} tvp5150soc_formats[] = {
+        {
+                .mbus_code        = V4L2_MBUS_FMT_YVYU8_2X8,
+                .colorspace        = V4L2_COLORSPACE_JPEG,
+        },
+};
+#define N_TVP5150_FMTS ARRAY_SIZE(tvp5150soc_formats)
+
+struct tvp5150soc_decoder {
+        struct v4l2_subdev sd;
+        const struct tvp5150soc_format_struct *fmt_list;
+        struct tvp5150soc_format_struct *fmt;
+        int num_fmts;
+        int active_input;
+};
+
+static const struct v4l2_queryctrl tvp5150soc_controls[] = {
+        {
+                .id                = V4L2_CID_BRIGHTNESS,
+                .type                = V4L2_CTRL_TYPE_INTEGER,
+                .name                = "brightness",
+                .minimum        = 0,
+                .maximum        = 255,
+                .step                = 1,
+                .default_value        = 128,
+        },
+        {
+                .id                = V4L2_CID_CONTRAST,
+                .type                = V4L2_CTRL_TYPE_INTEGER,
+                .name                = "contrast",
+                .minimum        = 0,
+                .maximum        = 207,
+                .step                = 1,
+                .default_value        = 128,
+        },
+        {
+                .id                = V4L2_CID_SATURATION,
+                .type                = V4L2_CTRL_TYPE_INTEGER,
+                .name                = "saturation",
+                .minimum        = 0,
+                .maximum        = 255,
+                .step                = 1,
+                .default_value        = 128,
+        },
+        {
+                .id                = V4L2_CID_HUE,
+                .type                = V4L2_CTRL_TYPE_INTEGER,
+                .name                = "hue",
+                .minimum        = -127, /*TODO: TVP5150 supports hue in range -180..180, which is equal to -127..127 reg value*/
+                .maximum        = 127,
+                .step                = 1,
+                .default_value        = 0,
+        },
+};
+
+static inline struct tvp5150soc_decoder *to_decoder(struct v4l2_subdev *sd)
+{
+        return container_of(sd, struct tvp5150soc_decoder, sd);
+}
+
+static int tvp5150soc_read_reg(struct v4l2_subdev *sd, unsigned char addr, unsigned char *val)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        int err, retry = I2C_RETRY_COUNT;
+
+    int ccc = client->addr;
+    int aaa = addr;
+    printk(KERN_ERR "tvp5150soc_read_reg: %04X %04X\n", ccc, aaa);
+
+        while(retry)
+        {
+                err = i2c_smbus_read_byte_data(client, addr);
+                if(err > 0)
+                        break;
+                retry--;
+                msleep_interruptible(10);
+        }
+
+        if(err < 0)
+                v4l2_dbg(0, debug, sd, "i2c i/o error: %d\n", err);
+        else
+        {
+                *val = (unsigned char)err;
+                v4l2_dbg(2, debug, sd, "tvp5150: read 0x%02x = 0x%02x\n", addr, *val);
+        }
+
+    printk(KERN_ERR "tvp5150soc_read_reg result: %d\n", err);
+    
+    return err;
+}
+
+static int tvp5150soc_write_reg(struct v4l2_subdev *sd, unsigned char addr, unsigned char val)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        int err, retry = I2C_RETRY_COUNT;
+
+        while(retry)
+        {
+                err = i2c_smbus_write_byte_data(client, addr, val);
+                if(err > 0)
+                        break;
+                retry--;
+                msleep_interruptible(10);
+        }
+
+        if(err < 0)
+                v4l2_dbg(0, debug, sd, "i2c i/o error: %d\n", err);
+        else
+                v4l2_dbg(2, debug, sd, "tvp5150: write 0x%02x = 0x%02x\n", addr, val);
+
+        return err;
+}
+
+/* Default values as sugested at TVP5150AM1 datasheet */
+static const struct i2c_reg_value tvp5150soc_init_default[] = {
+        {TVP5150_VD_IN_SRC_SEL_1,0x00},                /* 0x00 */
+        {TVP5150_ANAL_CHL_CTL,0x15},                /* 0x01 */
+        {TVP5150_OP_MODE_CTL,0x00},                        /* 0x02 */
+        {TVP5150_MISC_CTL,0x01},                        /* 0x03 */
+        {TVP5150_COLOR_KIL_THSH_CTL,0x10},        /* 0x06 */
+        {TVP5150_LUMA_PROC_CTL_1,0x60},                /* 0x07 */
+        {TVP5150_LUMA_PROC_CTL_2,0x00},                /* 0x08 */
+        {TVP5150_BRIGHT_CTL,0x80},                        /* 0x09 */
+        {TVP5150_SATURATION_CTL,0x80},                /* 0x0a */
+        {TVP5150_HUE_CTL,0x00},                                /* 0x0b */
+        {TVP5150_CONTRAST_CTL,0x80},                /* 0x0c */
+        {TVP5150_DATA_RATE_SEL,0x47},                /* 0x0d */
+        {TVP5150_LUMA_PROC_CTL_3,0x00},                /* 0x0e */
+        {TVP5150_CONF_SHARED_PIN,0x08},                /* 0x0f */
+        {TVP5150_ACT_VD_CROP_ST_MSB,0x00},        /* 0x11 */
+        {TVP5150_ACT_VD_CROP_ST_LSB,0x00},        /* 0x12 */
+        {TVP5150_ACT_VD_CROP_STP_MSB,0x00},        /* 0x13 */
+        {TVP5150_ACT_VD_CROP_STP_LSB,0x00},        /* 0x14 */
+        {TVP5150_GENLOCK,0x01},                                /* 0x15 */
+        {TVP5150_HORIZ_SYNC_START,0x80},        /* 0x16 */
+        {TVP5150_VERT_BLANKING_START,0x00},        /* 0x18 */
+        {TVP5150_VERT_BLANKING_STOP,0x00},        /* 0x19 */
+        {TVP5150_CHROMA_PROC_CTL_1,0x0c},        /* 0x1a */
+        {TVP5150_CHROMA_PROC_CTL_2,0x14},        /* 0x1b */
+        {TVP5150_INT_RESET_REG_B,0x00},                /* 0x1c */
+        {TVP5150_INT_ENABLE_REG_B,0x00},        /* 0x1d */
+        {TVP5150_INTT_CONFIG_REG_B,0x00},        /* 0x1e */
+        {TVP5150_VIDEO_STD,0x00},                        /* 0x28 */
+        {TVP5150_MACROVISION_ON_CTR,0x0f},        /* 0x2e */
+        {TVP5150_MACROVISION_OFF_CTR,0x01},        /* 0x2f */
+        {TVP5150_TELETEXT_FIL_ENA,0x00},        /* 0xbb */
+        {TVP5150_INT_STATUS_REG_A,0x00},        /* 0xc0 */
+        {TVP5150_INT_ENABLE_REG_A,0x00},        /* 0xc1 */
+        {TVP5150_INT_CONF,0x04},                        /* 0xc2 */
+        {TVP5150_FIFO_INT_THRESHOLD,0x80},        /* 0xc8 */
+        {TVP5150_FIFO_RESET,0x00},                        /* 0xc9 */
+        {TVP5150_LINE_NUMBER_INT,0x00},                /* 0xca */
+        {TVP5150_PIX_ALIGN_REG_LOW,0x4e},        /* 0xcb */
+        {TVP5150_PIX_ALIGN_REG_HIGH,0x00},        /* 0xcc */
+        {TVP5150_FIFO_OUT_CTRL,0x01},                /* 0xcd */
+        {TVP5150_FULL_FIELD_ENA,0x00},                /* 0xcf */
+        {TVP5150_LINE_MODE_INI,0x00},                /* 0xd0 */
+        {TVP5150_FULL_FIELD_MODE_REG,0x7f},        /* 0xfc */
+        { /* end of data */0xff,0xff}
+};
+
+/* Default values as sugested at TVP5150AM1 datasheet */
+static const struct i2c_reg_value tvp5150soc_init_enable[] = {
+        {TVP5150_VD_IN_SRC_SEL_1, 0x02},
+        {TVP5150_CONF_SHARED_PIN, 0x02},
+        {TVP5150_ANAL_CHL_CTL, 0x15}, /* Automatic offset and AGC enabled */
+        {TVP5150_MISC_CTL, 0x6f}, /* Activate YCrCb output 0x9 or 0xd ? */
+        {TVP5150_AUTOSW_MSK, 0x0}, /* Activates video std autodetection for all standards */
+        {TVP5150_DATA_RATE_SEL, 0x47},/* Default format: 0x47. For 4:2:2: 0x40 */
+        {TVP5150_CHROMA_PROC_CTL_1, 0x0c},
+        {TVP5150_CHROMA_PROC_CTL_2, 0x54},
+        {0x27, 0x20},/* Non documented, but initialized on WinTV USB2 */
+        {0xff, 0xff}
+};
+
+static int tvp5150soc_write_inittab(struct v4l2_subdev *sd,        const struct i2c_reg_value *regs)
+{
+        while (regs->reg != 0xff) {
+                tvp5150soc_write_reg(sd, regs->reg, regs->value);
+                regs++;
+        }
+        return 0;
+}
+
+static int tvp5150soc_s_input(struct file *file, void *priv, unsigned int i)
+{
+        struct soc_camera_device *icd = file->private_data;
+        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+        struct tvp5150soc_decoder *decoder = to_decoder(sd);
+        unsigned char val;
+
+        if (i < 3)
+        {
+                decoder->active_input = i;
+                switch (decoder->active_input) {
+                case 0:
+                        val = TVP5150_VIDEO_INPUT_SELECT_AIP1A;
+                        break;
+                case 1:
+                        val = TVP5150_VIDEO_INPUT_SELECT_AIP1B;
+                        break;
+                case 2:
+                        val = TVP5150_VIDEO_INPUT_SELECT_SVIDEO;
+                        break;
+                default:
+                        val = TVP5150_VIDEO_INPUT_SELECT_AIP1A;
+                }
+                return tvp5150soc_write_reg(sd, TVP5150_VD_IN_SRC_SEL_1, val);
+        }
+
+        return -EINVAL;
+}
+
+static int tvp5150soc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+        struct soc_camera_device *icd = file->private_data;
+        struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
+        struct tvp5150soc_decoder *decoder = to_decoder(sd);
+
+        *i = decoder->active_input;
+
+        return 0;
+}
+
+/********************************************************************************************************************************/
+
+static int tvp5150soc_set_bus_param(struct soc_camera_device *icd, unsigned long flags)
+{
+        return 0;
+}
+
+static unsigned long tvp5150soc_query_bus_param(struct soc_camera_device *icd)
+{
+        struct soc_camera_link *icl = to_soc_camera_link(icd);
+
+        unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
+                SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
+                SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8;
+
+        return soc_camera_apply_sensor_flags(icl, flags);
+}
+
+/********************************************************************************************************************************/
+
+static int tvp5150soc_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+        switch (ctrl->id) {
+        case V4L2_CID_BRIGHTNESS:
+                tvp5150soc_read_reg(sd, TVP5150_BRIGHT_CTL, (unsigned char*)&ctrl->value);
+                return 0;
+        case V4L2_CID_CONTRAST:
+                tvp5150soc_read_reg(sd, TVP5150_CONTRAST_CTL, (unsigned char*)&ctrl->value);
+                return 0;
+        case V4L2_CID_SATURATION:
+                tvp5150soc_read_reg(sd, TVP5150_SATURATION_CTL, (unsigned char*)&ctrl->value);
+                return 0;
+        case V4L2_CID_HUE:
+                tvp5150soc_read_reg(sd, TVP5150_HUE_CTL, (unsigned char*)&ctrl->value);
+                return 0;
+        }
+        return -EINVAL;
+}
+
+static int tvp5150soc_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
+{
+        switch (ctrl->id) {
+        case V4L2_CID_BRIGHTNESS:
+                tvp5150soc_write_reg(sd, TVP5150_BRIGHT_CTL, ctrl->value);
+                return 0;
+        case V4L2_CID_CONTRAST:
+                tvp5150soc_write_reg(sd, TVP5150_CONTRAST_CTL, ctrl->value);
+                return 0;
+        case V4L2_CID_SATURATION:
+                tvp5150soc_write_reg(sd, TVP5150_SATURATION_CTL, ctrl->value);
+                return 0;
+        case V4L2_CID_HUE:
+                tvp5150soc_write_reg(sd, TVP5150_HUE_CTL, ctrl->value);
+                return 0;
+        }
+        return -EINVAL;
+}
+
+static int tvp5150soc_reset(struct v4l2_subdev *sd, u32 val)
+{
+        /* Initializes TVP5150 to its default values */
+        /* TVP5150 has no ability to software reset */
+        tvp5150soc_write_inittab(sd, tvp5150soc_init_default);
+
+        /* Initializes TVP5150 to stream enabled values */
+        tvp5150soc_write_inittab(sd, tvp5150soc_init_enable);
+
+        return 0;
+}
+
+/********************************************************************************************************************************/
+
+static int tvp5150soc_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
+{
+        /*
+         * Set the image format. Currently we support only one format with
+         * fixed resolution, so we can set the format as it is on camera startup.
+         */
+        tvp5150soc_reset(sd, 0);
+
+        return 0;
+}
+
+static int tvp5150soc_try_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *mf)
+{
+        struct tvp5150soc_decoder *decoder = to_decoder(sd);
+        int index;
+
+    printk(KERN_ERR "tvp5150soc_probe try mbus format\n");
+
+        /* Check if we support queried image format. */
+        for (index = 0; index < N_TVP5150_FMTS; index++)
+                if (tvp5150soc_formats[index].mbus_code == mf->code)
+            {
+                printk(KERN_ERR "tvp5150soc_probe try mbus format found format\n");
+                break;
+            }
+        /* If not, set the only one which we support */
+        if (index >= N_TVP5150_FMTS) {
+                /* default to first format */
+                index = 0;
+        printk(KERN_ERR "tvp5150soc_probe try mbus format default format\n");
+                mf->code = tvp5150soc_formats[0].mbus_code;
+        }
+
+        /* Store the current format */
+        decoder->fmt = &tvp5150soc_formats[index];
+
+        /* Fixed value, move to tvp5150soc_formats */
+        mf->field = V4L2_FIELD_INTERLACED_TB;
+    mf->width = LINE_PIXELS;
+        mf->height = FRAME_LINES;
+        mf->colorspace = tvp5150soc_formats[index].colorspace;
+
+        return 0;
+}
+
+static int tvp5150soc_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned int index, enum v4l2_mbus_pixelcode *code)
+{
+        if (index >= ARRAY_SIZE(tvp5150soc_formats))
+                return -EINVAL;
+
+    *code = tvp5150soc_formats[index].mbus_code;
+
+        return 0;
+}
+/********************************************************************************************************************************/
+
+static struct soc_camera_ops tvp5150soc_soc_camera_ops = {
+        .set_bus_param                = tvp5150soc_set_bus_param,
+        .query_bus_param        = tvp5150soc_query_bus_param,
+        .controls                        = tvp5150soc_controls,
+        .num_controls                = ARRAY_SIZE(tvp5150soc_controls),
+};
+
+static const struct v4l2_subdev_core_ops tvp5150soc_core_ops = {
+        .g_ctrl = tvp5150soc_g_ctrl,
+        .s_ctrl = tvp5150soc_s_ctrl,
+        .reset = tvp5150soc_reset,
+};
+
+static const struct v4l2_subdev_video_ops tvp5150soc_video_ops = {
+        .s_mbus_fmt         = tvp5150soc_s_mbus_fmt,
+        .try_mbus_fmt         = tvp5150soc_try_mbus_fmt,
+        .enum_mbus_fmt         = tvp5150soc_enum_mbus_fmt,
+};
+
+static const struct v4l2_subdev_ops tvp5150soc_ops = {
+        .core = &tvp5150soc_core_ops,
+        .video = &tvp5150soc_video_ops,
+};
+
+
+/****************************************************************************
+                        I2C Client & Driver
+ ****************************************************************************/
+static int tvp5150soc_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+        struct soc_camera_device *icd = client->dev.platform_data;
+        struct soc_camera_link *icl;
+        struct tvp5150soc_decoder *decoder;
+        struct v4l2_subdev *sd;
+    struct v4l2_ioctl_ops *ops;
+        unsigned char msb_id, lsb_id, msb_rom, lsb_rom;
+
+    printk(KERN_ERR "tvp5150soc_probe start\n");
+
+        /* Check if the adapter supports the needed features */
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+        {
+            printk(KERN_ERR "tvp5150soc_probe error 1\n");
+            return -EIO;
+        }
+
+    icl = to_soc_camera_link(icd);
+        if (!icl)
+        {
+                dev_err(&client->dev, "No platform data!!\n");
+        printk(KERN_ERR "tvp5150soc_probe error 2\n");
+                return -ENODEV;
+        }
+
+        decoder = kzalloc(sizeof(struct tvp5150soc_decoder), GFP_KERNEL);
+        if (!decoder)
+        {
+                dev_err(&client->dev, "Failed to allocate memory for private data!\n");
+        printk(KERN_ERR "tvp5150soc_probe error 3\n");
+                return -ENOMEM;
+        }
+
+        /* TODO: init def settings of tvp5150soc_decoder */
+
+        sd = &decoder->sd;
+
+        /* Register with V4L2 layer as slave device */
+        v4l2_i2c_subdev_init(sd, client, &tvp5150soc_ops);
+
+        tvp5150soc_read_reg(sd, TVP5150_MSB_DEV_ID, &msb_id);
+        tvp5150soc_read_reg(sd, TVP5150_LSB_DEV_ID, &lsb_id);
+        tvp5150soc_read_reg(sd, TVP5150_ROM_MAJOR_VER, &msb_rom);
+        tvp5150soc_read_reg(sd, TVP5150_ROM_MINOR_VER, &lsb_rom);
+
+        if (msb_rom == 4 && lsb_rom == 0) { /* Is TVP5150AM1 */
+                v4l2_info(sd, "tvp%02x%02xam1 detected.\n", msb_id, lsb_id);
+                /* ITU-T BT.656.4 timing */
+                tvp5150soc_write_reg(sd, TVP5150_REV_SELECT, 0);
+        } else {
+                if (msb_rom == 3 || lsb_rom == 0x21) { /* Is TVP5150A */
+                        v4l2_info(sd, "tvp%02x%02xa detected.\n", msb_id, lsb_id);
+                } else {
+                        v4l2_info(sd, "*** unknown tvp%02x%02x chip detected.\n", msb_id, lsb_id);
+                        v4l2_info(sd, "*** Rom ver is %d.%d\n", msb_rom, lsb_rom);
+                }
+        }
+
+        icd->ops = &tvp5150soc_soc_camera_ops;
+
+        /*
+         * This is the only way to support more than one input as soc_camera
+         * assumes in its own vidioc_s(g)_input implementation that only one
+         * input is present we have to override that with our own handlers.
+         */
+        ops = (struct v4l2_ioctl_ops*)icd->vdev->ioctl_ops;
+        ops->vidioc_s_input = &tvp5150soc_s_input;
+        ops->vidioc_g_input = &tvp5150soc_g_input;
+
+    printk(KERN_ERR "tvp5150soc_probe return 0\n");
+        return 0;
+}
+
+static int tvp5150soc_remove(struct i2c_client *client)
+{
+        struct v4l2_subdev *sd = i2c_get_clientdata(client);
+        struct tvp5150soc_decoder *decoder = to_decoder(sd);
+
+        v4l2_device_unregister_subdev(sd);
+        kfree(decoder);
+
+        return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id tvp5150soc_id[] = {
+        { MODULE_NAME, 0 },
+        { }
+};
+
+static struct i2c_driver tvp5150soc_driver = {
+        .driver = {
+                .owner        = THIS_MODULE,
+                .name        = MODULE_NAME,
+        },
+        .probe                = tvp5150soc_probe,
+        .remove                = tvp5150soc_remove,
+        .id_table        = tvp5150soc_id,
+};
+
+static __init int init_tvp5150soc(void)
+{
+        return i2c_add_driver(&tvp5150soc_driver);
+}
+
+static __exit void exit_tvp5150soc(void)
+{
+        i2c_del_driver(&tvp5150soc_driver);
+}
+
+module_init(init_tvp5150soc);
+module_exit(exit_tvp5150soc);
+
+MODULE_DESCRIPTION("Texas Instruments TVP5150A video decoder driver for soc_camera interface");
+MODULE_AUTHOR("Antmicro Ltd.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/uvc/uvc_driver.c b/drivers/media/video/uvc/uvc_driver.c
index e4100b1f68df..d48ed7e7f3bc 100644
--- a/drivers/media/video/uvc/uvc_driver.c
+++ b/drivers/media/video/uvc/uvc_driver.c
@@ -114,6 +114,11 @@ static struct uvc_format_desc uvc_fmts[] = {
 		.guid		= UVC_GUID_FORMAT_RGBP,
 		.fcc		= V4L2_PIX_FMT_RGB565,
 	},
+	{
+		.name		= "H.264",
+		.guid		= UVC_GUID_FORMAT_H264,
+		.fcc		= V4L2_PIX_FMT_H264,
+	},
 };
 
 /* ------------------------------------------------------------------------
diff --git a/drivers/media/video/uvc/uvcvideo.h b/drivers/media/video/uvc/uvcvideo.h
index cbdd49bf8b67..281ef2aa3471 100644
--- a/drivers/media/video/uvc/uvcvideo.h
+++ b/drivers/media/video/uvc/uvcvideo.h
@@ -179,6 +179,10 @@ struct uvc_xu_control {
 	{ 'M',  '4',  '2',  '0', 0x00, 0x00, 0x10, 0x00, \
 	 0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71}
 
+#define UVC_GUID_FORMAT_H264 \
+	{ 'H',  '2',  '6',  '4', 0x00, 0x00, 0x10, 0x00, \
+	 0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71}
+
 /* ------------------------------------------------------------------------
  * Driver specific constants.
  */
diff --git a/drivers/media/video/videobuf2-dma-nvmap.c b/drivers/media/video/videobuf2-dma-nvmap.c
new file mode 100644
index 000000000000..4ba8fed0cc02
--- /dev/null
+++ b/drivers/media/video/videobuf2-dma-nvmap.c
@@ -0,0 +1,268 @@
+/*
+ * Copyright (c) 2012-2013, NVIDIA 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/dma-mapping.h>
+#include <linux/nvmap.h>
+
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-memops.h>
+
+struct vb2_dc_conf {
+	struct device		*dev;
+	struct nvmap_client	*nvmap_client;
+};
+
+struct vb2_dc_buf {
+	struct vb2_dc_conf		*conf;
+	void				*vaddr;
+	dma_addr_t			paddr;
+	unsigned long			size;
+	struct vm_area_struct		*vma;
+	atomic_t			refcount;
+	struct vb2_vmarea_handler	handler;
+
+	struct nvmap_handle_ref		*nvmap_ref;
+};
+
+static void vb2_dma_nvmap_put(void *buf_priv);
+
+static void *vb2_dma_nvmap_alloc(void *alloc_ctx, unsigned long size)
+{
+	struct vb2_dc_conf *conf = alloc_ctx;
+	struct vb2_dc_buf *buf;
+	int ret;
+
+	buf = kzalloc(sizeof *buf, GFP_KERNEL);
+	if (!buf) {
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	buf->nvmap_ref = nvmap_alloc(conf->nvmap_client, size, 32,
+				     NVMAP_HANDLE_UNCACHEABLE, 0);
+	if (IS_ERR(buf->nvmap_ref)) {
+		dev_err(conf->dev, "nvmap_alloc failed\n");
+		ret = -ENOMEM;
+		goto exit_free;
+	}
+
+	buf->paddr = nvmap_pin(conf->nvmap_client, buf->nvmap_ref);
+	if (IS_ERR_VALUE(buf->paddr)) {
+		dev_err(conf->dev, "nvmap_pin failed\n");
+		ret = -ENOMEM;
+		goto exit_dealloc;
+	}
+
+	buf->vaddr = nvmap_mmap(buf->nvmap_ref);
+	if (!buf->vaddr) {
+		dev_err(conf->dev, "nvmap_mmap failed\n");
+		ret = -ENOMEM;
+		goto exit_unpin;
+	}
+
+	buf->conf = conf;
+	buf->size = size;
+
+	buf->handler.refcount = &buf->refcount;
+	buf->handler.put = vb2_dma_nvmap_put;
+	buf->handler.arg = buf;
+
+	*((unsigned long *)buf->vaddr) = (unsigned long)buf->nvmap_ref->handle;
+
+	atomic_inc(&buf->refcount);
+
+	return buf;
+
+exit_unpin:
+	nvmap_unpin(conf->nvmap_client, buf->nvmap_ref);
+exit_dealloc:
+	nvmap_free(conf->nvmap_client, buf->nvmap_ref);
+exit_free:
+	kfree(buf);
+exit:
+	return ERR_PTR(ret);
+}
+
+static void vb2_dma_nvmap_put(void *buf_priv)
+{
+	struct vb2_dc_buf *buf = buf_priv;
+
+	if (atomic_dec_and_test(&buf->refcount)) {
+		nvmap_munmap(buf->nvmap_ref, buf->vaddr);
+		nvmap_unpin(buf->conf->nvmap_client, buf->nvmap_ref);
+		nvmap_free(buf->conf->nvmap_client, buf->nvmap_ref);
+		kfree(buf);
+	}
+}
+
+static void *vb2_dma_nvmap_cookie(void *buf_priv)
+{
+	struct vb2_dc_buf *buf = buf_priv;
+
+	return &buf->paddr;
+}
+
+static void *vb2_dma_nvmap_vaddr(void *buf_priv)
+{
+	struct vb2_dc_buf *buf = buf_priv;
+	if (!buf)
+		return 0;
+
+	return buf->vaddr;
+}
+
+static unsigned int vb2_dma_nvmap_num_users(void *buf_priv)
+{
+	struct vb2_dc_buf *buf = buf_priv;
+
+	return atomic_read(&buf->refcount);
+}
+
+static int vb2_dma_nvmap_mmap(void *buf_priv, struct vm_area_struct *vma)
+{
+	struct vb2_dc_buf *buf = buf_priv;
+	unsigned long vm_start, paddr;
+	void *vaddr;
+	int size;
+	int ret;
+
+	if (!buf) {
+		pr_err("No buffer to map\n");
+		return -EINVAL;
+	}
+
+	size = min_t(unsigned long, vma->vm_end - vma->vm_start, buf->size);
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	for (vaddr = buf->vaddr; vaddr < buf->vaddr + size;
+		vaddr += PAGE_SIZE) {
+		paddr = page_to_phys(vmalloc_to_page(vaddr));
+		vm_start = vma->vm_start + (unsigned long) (vaddr - buf->vaddr);
+		ret = remap_pfn_range(vma, vm_start, paddr >> PAGE_SHIFT,
+				PAGE_SIZE, vma->vm_page_prot);
+		if (ret) {
+			pr_err("Remapping memory failed, error: %d\n",
+				ret);
+			return ret;
+		}
+		pr_debug("%s: mapped paddr 0x%08lx at 0x%08lx, size %ld\n",
+			__func__, paddr, vm_start, PAGE_SIZE);
+	}
+
+	vma->vm_flags		|= VM_DONTEXPAND | VM_RESERVED;
+	vma->vm_private_data	= &buf->handler;
+	vma->vm_ops		= &vb2_common_vm_ops;
+
+	vma->vm_ops->open(vma);
+
+	return 0;
+}
+
+static void *vb2_dma_nvmap_get_userptr(void *alloc_ctx, unsigned long vaddr,
+					unsigned long size, int write)
+{
+	struct vb2_dc_buf *buf;
+	struct vm_area_struct *vma;
+	dma_addr_t paddr = 0;
+	int ret;
+
+	buf = kzalloc(sizeof *buf, GFP_KERNEL);
+	if (!buf)
+		return ERR_PTR(-ENOMEM);
+
+	ret = vb2_get_contig_userptr(vaddr, size, &vma, &paddr);
+	if (ret) {
+		pr_err("Failed acquiring VMA for vaddr 0x%08lx\n",
+			vaddr);
+		kfree(buf);
+		return ERR_PTR(ret);
+	}
+
+	buf->size = size;
+	buf->paddr = paddr;
+	buf->vma = vma;
+
+	return buf;
+}
+
+static void vb2_dma_nvmap_put_userptr(void *mem_priv)
+{
+	struct vb2_dc_buf *buf = mem_priv;
+
+	if (!buf)
+		return;
+
+	vb2_put_vma(buf->vma);
+	kfree(buf);
+}
+
+const struct vb2_mem_ops vb2_dma_nvmap_memops = {
+	.alloc		= vb2_dma_nvmap_alloc,
+	.put		= vb2_dma_nvmap_put,
+	.cookie		= vb2_dma_nvmap_cookie,
+	.vaddr		= vb2_dma_nvmap_vaddr,
+	.mmap		= vb2_dma_nvmap_mmap,
+	.get_userptr	= vb2_dma_nvmap_get_userptr,
+	.put_userptr	= vb2_dma_nvmap_put_userptr,
+	.num_users	= vb2_dma_nvmap_num_users,
+};
+EXPORT_SYMBOL_GPL(vb2_dma_nvmap_memops);
+
+void *vb2_dma_nvmap_init_ctx(struct device *dev)
+{
+	struct vb2_dc_conf *conf;
+	int ret;
+
+	conf = kzalloc(sizeof *conf, GFP_KERNEL);
+	if (!conf) {
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	conf->dev = dev;
+
+	conf->nvmap_client = nvmap_create_client(nvmap_dev,
+						 "videobuf2-dma-nvmap");
+	if (!conf->nvmap_client) {
+		ret = -ENOMEM;
+		goto exit_free;
+	}
+
+	return conf;
+
+exit_free:
+	kfree(conf);
+exit:
+	return ERR_PTR(ret);
+}
+EXPORT_SYMBOL_GPL(vb2_dma_nvmap_init_ctx);
+
+void vb2_dma_nvmap_cleanup_ctx(void *alloc_ctx)
+{
+	struct vb2_dc_conf *conf = alloc_ctx;
+
+	nvmap_client_put(conf->nvmap_client);
+
+	kfree(alloc_ctx);
+}
+EXPORT_SYMBOL_GPL(vb2_dma_nvmap_cleanup_ctx);
+
+MODULE_DESCRIPTION("DMA-nvmap memory handling routines for videobuf2");
+MODULE_AUTHOR("Andrew Chew <achew@nvidia.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index 21574bdf485f..1f131d797583 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -175,7 +175,7 @@ config MFD_TPS65910
 	bool "TPS65910 Power Management chip"
 	depends on I2C=y && GPIOLIB
 	select MFD_CORE
-	select GPIO_TPS65910
+	select REGMAP_I2C
 	help
 	  if you say yes here you get support for the TPS65910 series of
 	  Power Management chips.
@@ -270,6 +270,11 @@ config TWL6040_CORE
 	select MFD_CORE
 	default n
 
+config AIC3262_CODEC
+	bool
+	select MFD_CORE
+	default n
+
 config MFD_STMPE
 	bool "Support STMicroelectronics STMPE"
 	depends on I2C=y && GENERIC_HARDIRQS
@@ -292,6 +297,7 @@ config MFD_STMPE
 		GPIO: stmpe-gpio
 		Keypad: stmpe-keypad
 		Touchscreen: stmpe-ts
+		ADC: stmpe-adc
 
 config MFD_TC3589X
 	bool "Support Toshiba TC35892 and variants"
@@ -353,6 +359,19 @@ config PMIC_ADP5520
 	  individual components like LCD backlight, LEDs, GPIOs and Kepad
 	  under the corresponding menus.
 
+config MFD_MAX77665
+	bool "Maxim Semiconductor MAX77665 Companion PMIC Support"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select MFD_CORE
+	select REGMAP_I2C
+	help
+	  Say yes here to support for Maxim Semiconductor MAX77665.
+	  This is a Power Management IC with Flash, Fuel Gauge, Haptic,
+	  MUIC controls on chip.
+	  This driver provides common support for accessing the device;
+	  additional drivers must be enabled in order to use the functionality
+	  of the device.
+
 config MFD_MAX8925
 	bool "Maxim Semiconductor MAX8925 PMIC Support"
 	depends on I2C=y && GENERIC_HARDIRQS
@@ -386,6 +405,26 @@ config MFD_MAX8998
 	  additional drivers must be enabled in order to use the functionality
 	  of the device.
 
+config MFD_MAX8907C
+	tristate "Maxim Semiconductor MAX8907C PMIC Support"
+	select MFD_CORE
+	depends on I2C
+	help
+	  Say yes here to support for Maxim Semiconductor MAX8907C. This is
+	  a Power Management IC. This driver provies common support for
+	  accessing the device, additional drivers must be enabled in order
+	  to use the functionality of the device.
+
+config MFD_MAX77663
+	tristate "Maxim Semiconductor MAX77663 PMIC Support"
+	select MFD_CORE
+	depends on I2C
+	help
+	  Say yes here to support for Maxim Semiconductor MAX77663. This is
+	  a Power Management IC. This driver provies common support for
+	  accessing the device, additional drivers must be enabled in order
+	  to use the functionality of the device.
+
 config MFD_WM8400
 	tristate "Support Wolfson Microelectronics WM8400"
 	select MFD_CORE
@@ -404,6 +443,7 @@ config MFD_WM831X_I2C
 	bool "Support Wolfson Microelectronics WM831x/2x PMICs with I2C"
 	select MFD_CORE
 	select MFD_WM831X
+	select REGMAP_I2C
 	depends on I2C=y && GENERIC_HARDIRQS
 	help
 	  Support for the Wolfson Microelecronics WM831x and WM832x PMICs
@@ -415,6 +455,7 @@ config MFD_WM831X_SPI
 	bool "Support Wolfson Microelectronics WM831x/2x PMICs with SPI"
 	select MFD_CORE
 	select MFD_WM831X
+	select REGMAP_SPI
 	depends on SPI_MASTER && GENERIC_HARDIRQS
 	help
 	  Support for the Wolfson Microelecronics WM831x and WM832x PMICs
@@ -488,6 +529,7 @@ config MFD_WM8350_I2C
 config MFD_WM8994
 	bool "Support Wolfson Microelectronics WM8994"
 	select MFD_CORE
+	select REGMAP_I2C
 	depends on I2C=y && GENERIC_HARDIRQS
 	help
 	  The WM8994 is a highly integrated hi-fi CODEC designed for
@@ -760,6 +802,18 @@ config MFD_PM8XXX_IRQ
 config TPS65911_COMPARATOR
 	tristate
 
+config MFD_TPS65090
+	bool "TPS65090 Power Management chips"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select MFD_CORE
+	select REGMAP_I2C
+	help
+	  If you say yes here you get support for the TPS65090 series of
+	  Power Management chips.
+	  This driver provides common support for accessing the device,
+	  additional drivers must be enabled in order to use the
+	  functionality of the device.
+
 config MFD_AAT2870_CORE
 	bool "Support for the AnalogicTech AAT2870"
 	select MFD_CORE
@@ -770,6 +824,78 @@ config MFD_AAT2870_CORE
 	  additional drivers must be enabled in order to use the
 	  functionality of the device.
 
+config MFD_TPS6591X
+	bool "TPS6591x Power Management chips"
+	depends on I2C && GPIOLIB && GENERIC_HARDIRQS
+	select MFD_CORE
+	help
+	  If you say yes here you get support for the TPS6591X series of
+	  Power Management chips.
+	  This driver provides common support for accessing the device,
+	  additional drivers must be enabled in order to use the
+	  functionality of the device.
+
+config MFD_RC5T583
+	bool "Ricoh RC5T583 Power Management system device"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select MFD_CORE
+	select REGMAP_I2C
+	help
+	  Select this option to get support for the RICOH583 Power
+	  Management system device.
+	  This driver provides common support for accessing the device
+	  through i2c interface. The device supports multiple sub-devices
+	  like GPIO, interrupts, RTC, LDO and DCDC regulators, onkey.
+	  Additional drivers must be enabled in order to use the
+	  different functionality of the device.
+
+config MFD_AAT2870_CORE
+	bool "Support for the AnalogicTech AAT2870"
+	select MFD_CORE
+	depends on I2C=y && GPIOLIB
+	help
+	  If you say yes here you get support for the AAT2870.
+
+config MFD_TPS80031
+	bool "TI TPS80031 Power Management chips"
+	depends on I2C && GPIOLIB && GENERIC_HARDIRQS
+	select MFD_CORE
+	help
+	  If you say yes here you get support for the TPS80031 Power
+	  Management chips.
+	  This driver provides common support for accessing the device,
+	  additional drivers must be enabled in order to use the
+	  functionality of the device.
+
+config GPADC_TPS80031
+	bool "Support for TI TPS80031 Gpadc driver"
+	depends on  MFD_TPS80031
+	help
+	  If you say yes here you get support for the TPS80031 gpadc
+	  Module.
+
+config MFD_RICOH583
+	bool "Ricoh RC5T583 Power Management system device"
+	depends on I2C && GPIOLIB && GENERIC_HARDIRQS
+	select MFD_CORE
+	default n
+	help
+	  If you say yes here you get support for the RICOH583 Power
+	  Management system device.
+	  This driver provides common support for accessing the device,
+	  additional drivers must be enabled in order to use the
+	  functionality of the device.
+
+config MFD_PALMAS
+	bool "Support for the TI Palmas series chips"
+	select MFD_CORE
+	select REGMAP_I2C
+	select REGMAP_IRQ
+	depends on I2C=y
+	help
+	  If you say yes here you get support for the Palmas
+	  series of PMIC chips from Texas Instruments.
+
 endif # MFD_SUPPORT
 
 menu "Multimedia Capabilities Port drivers"
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index c58020303d18..c8dc50450219 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -49,6 +49,7 @@ obj-$(CONFIG_TWL4030_POWER)    += twl4030-power.o
 obj-$(CONFIG_MFD_TWL4030_AUDIO)	+= twl4030-audio.o
 obj-$(CONFIG_TWL6030_PWM)	+= twl6030-pwm.o
 obj-$(CONFIG_TWL6040_CORE)	+= twl6040-core.o twl6040-irq.o
+obj-$(CONFIG_AIC3262_CODEC)	+= tlv320aic3262-core.o tlv320aic3262-irq.o
 
 obj-$(CONFIG_MFD_MC13XXX)	+= mc13xxx-core.o
 
@@ -67,6 +68,7 @@ endif
 obj-$(CONFIG_UCB1400_CORE)	+= ucb1400_core.o
 
 obj-$(CONFIG_PMIC_DA903X)	+= da903x.o
+obj-$(CONFIG_MFD_MAX77665)	+= max77665.o
 max8925-objs			:= max8925-core.o max8925-i2c.o
 obj-$(CONFIG_MFD_MAX8925)	+= max8925.o
 obj-$(CONFIG_MFD_MAX8997)	+= max8997.o max8997-irq.o
@@ -101,4 +103,15 @@ obj-$(CONFIG_MFD_OMAP_USB_HOST)	+= omap-usb-host.o
 obj-$(CONFIG_MFD_PM8921_CORE) 	+= pm8921-core.o
 obj-$(CONFIG_MFD_PM8XXX_IRQ) 	+= pm8xxx-irq.o
 obj-$(CONFIG_TPS65911_COMPARATOR)	+= tps65911-comparator.o
+obj-$(CONFIG_MFD_TPS65090)	+= tps65090.o
 obj-$(CONFIG_MFD_AAT2870_CORE)	+= aat2870-core.o
+obj-$(CONFIG_MFD_TPS6591X)	+= tps6591x.o
+obj-$(CONFIG_MFD_TPS80031)	+= tps80031.o
+obj-$(CONFIG_GPADC_TPS80031)	+= tps8003x-gpadc.o
+obj-$(CONFIG_MFD_MAX8907C)	+= max8907c.o
+obj-$(CONFIG_MFD_MAX8907C)	+= max8907c-irq.o
+obj-$(CONFIG_MFD_MAX77663)	+= max77663-core.o
+obj-$(CONFIG_MFD_RICOH583)	+= ricoh583.o
+obj-$(CONFIG_MFD_RC5T583)	+= rc5t583.o rc5t583-irq.o
+obj-$(CONFIG_MFD_PALMAS)	+= palmas.o
+obj-$(CONFIG_MFD_LM3533)	+= lm3533-core.o lm3533-ctrlbank.o
diff --git a/drivers/mfd/aat2870-core.c b/drivers/mfd/aat2870-core.c
index 345dc658ef06..e7a4e2ab6cdc 100644
--- a/drivers/mfd/aat2870-core.c
+++ b/drivers/mfd/aat2870-core.c
@@ -79,27 +79,22 @@ static struct mfd_cell aat2870_devs[] = {
 	{
 		.name = "aat2870-backlight",
 		.id = AAT2870_ID_BL,
-		.pdata_size = sizeof(struct aat2870_bl_platform_data),
 	},
 	{
 		.name = "aat2870-regulator",
 		.id = AAT2870_ID_LDOA,
-		.pdata_size = sizeof(struct regulator_init_data),
 	},
 	{
 		.name = "aat2870-regulator",
 		.id = AAT2870_ID_LDOB,
-		.pdata_size = sizeof(struct regulator_init_data),
 	},
 	{
 		.name = "aat2870-regulator",
 		.id = AAT2870_ID_LDOC,
-		.pdata_size = sizeof(struct regulator_init_data),
 	},
 	{
 		.name = "aat2870-regulator",
 		.id = AAT2870_ID_LDOD,
-		.pdata_size = sizeof(struct regulator_init_data),
 	},
 };
 
@@ -423,7 +418,7 @@ static int aat2870_i2c_probe(struct i2c_client *client,
 			if ((pdata->subdevs[i].id == aat2870_devs[j].id) &&
 					!strcmp(pdata->subdevs[i].name,
 						aat2870_devs[j].name)) {
-				aat2870_devs[j].platform_data =
+				aat2870_devs[j].mfd_data =
 					pdata->subdevs[i].platform_data;
 				break;
 			}
diff --git a/drivers/mfd/max77663-core.c b/drivers/mfd/max77663-core.c
new file mode 100644
index 000000000000..46728c331f83
--- /dev/null
+++ b/drivers/mfd/max77663-core.c
@@ -0,0 +1,1453 @@
+/*
+ * drivers/mfd/max77663-core.c
+ * Max77663 mfd driver (I2C bus access)
+ *
+ * Copyright 2011 Maxim Integrated Products, Inc.
+ * Copyright (C) 2011-2012 NVIDIA Corporation
+ *
+ * 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.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+#include <linux/ratelimit.h>
+#include <linux/kthread.h>
+#include <linux/mfd/core.h>
+#include <linux/platform_device.h>
+#include <linux/seq_file.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+
+#include <linux/mfd/max77663-core.h>
+
+/* RTC i2c slave address */
+#define MAX77663_RTC_I2C_ADDR		0x48
+
+/* Registers */
+#define MAX77663_REG_IRQ_TOP		0x05
+#define MAX77663_REG_LBT_IRQ		0x06
+#define MAX77663_REG_SD_IRQ		0x07
+#define MAX77663_REG_LDOX_IRQ		0x08
+#define MAX77663_REG_LDO8_IRQ		0x09
+#define MAX77663_REG_GPIO_IRQ		0x0A
+#define MAX77663_REG_ONOFF_IRQ		0x0B
+#define MAX77663_REG_NVER		0x0C
+#define MAX77663_REG_IRQ_TOP_MASK	0x0D
+#define MAX77663_REG_LBT_IRQ_MASK	0x0E
+#define MAX77663_REG_SD_IRQ_MASK	0x0F
+#define MAX77663_REG_LDOX_IRQ_MASK	0x10
+#define MAX77663_REG_LDO8_IRQ_MASK	0x11
+#define MAX77663_REG_ONOFF_IRQ_MASK	0x12
+#define MAX77663_REG_GPIO_CTRL0		0x36
+#define MAX77663_REG_GPIO_CTRL1		0x37
+#define MAX77663_REG_GPIO_CTRL2		0x38
+#define MAX77663_REG_GPIO_CTRL3		0x39
+#define MAX77663_REG_GPIO_CTRL4		0x3A
+#define MAX77663_REG_GPIO_CTRL5		0x3B
+#define MAX77663_REG_GPIO_CTRL6		0x3C
+#define MAX77663_REG_GPIO_CTRL7		0x3D
+#define MAX77663_REG_GPIO_PU		0x3E
+#define MAX77663_REG_GPIO_PD		0x3F
+#define MAX77663_REG_GPIO_ALT		0x40
+#define MAX77663_REG_ONOFF_CFG1		0x41
+#define MAX77663_REG_ONOFF_CFG2		0x42
+
+#define IRQ_TOP_GLBL_MASK		(1 << 7)
+#define IRQ_TOP_GLBL_SHIFT		7
+#define IRQ_TOP_SD_MASK			(1 << 6)
+#define IRQ_TOP_SD_SHIFT		6
+#define IRQ_TOP_LDO_MASK		(1 << 5)
+#define IRQ_TOP_LDO_SHIFT		5
+#define IRQ_TOP_GPIO_MASK		(1 << 4)
+#define IRQ_TOP_GPIO_SHIFT		4
+#define IRQ_TOP_RTC_MASK		(1 << 3)
+#define IRQ_TOP_RTC_SHIFT		3
+#define IRQ_TOP_32K_MASK		(1 << 2)
+#define IRQ_TOP_32K_SHIFT		2
+#define IRQ_TOP_ONOFF_MASK		(1 << 1)
+#define IRQ_TOP_ONOFF_SHIFT		1
+#define IRQ_TOP_NVER_MASK		(1 << 0)
+#define IRQ_TOP_NVER_SHIFT		0
+
+#define IRQ_GLBL_MASK			(1 << 0)
+
+#define IRQ_LBT_BASE			MAX77663_IRQ_LBT_LB
+#define IRQ_LBT_END			MAX77663_IRQ_LBT_THERM_ALRM2
+
+#define IRQ_GPIO_BASE			MAX77663_IRQ_GPIO0
+#define IRQ_GPIO_END			MAX77663_IRQ_GPIO7
+
+#define IRQ_ONOFF_BASE			MAX77663_IRQ_ONOFF_HRDPOWRN
+#define IRQ_ONOFF_END			MAX77663_IRQ_ONOFF_ACOK_RISING
+
+#define GPIO_REG_ADDR(offset)		(MAX77663_REG_GPIO_CTRL0 + offset)
+
+#define GPIO_CTRL_DBNC_MASK		(3 << 6)
+#define GPIO_CTRL_DBNC_SHIFT		6
+#define GPIO_CTRL_REFE_IRQ_MASK		(3 << 4)
+#define GPIO_CTRL_REFE_IRQ_SHIFT	4
+#define GPIO_CTRL_DOUT_MASK		(1 << 3)
+#define GPIO_CTRL_DOUT_SHIFT		3
+#define GPIO_CTRL_DIN_MASK		(1 << 2)
+#define GPIO_CTRL_DIN_SHIFT		2
+#define GPIO_CTRL_DIR_MASK		(1 << 1)
+#define GPIO_CTRL_DIR_SHIFT		1
+#define GPIO_CTRL_OUT_DRV_MASK		(1 << 0)
+#define GPIO_CTRL_OUT_DRV_SHIFT		0
+
+#define GPIO_REFE_IRQ_NONE		0
+#define GPIO_REFE_IRQ_EDGE_FALLING	1
+#define GPIO_REFE_IRQ_EDGE_RISING	2
+#define GPIO_REFE_IRQ_EDGE_BOTH		3
+
+#define GPIO_DBNC_NONE			0
+#define GPIO_DBNC_8MS			1
+#define GPIO_DBNC_16MS			2
+#define GPIO_DBNC_32MS			3
+
+#define ONOFF_SFT_RST_MASK		(1 << 7)
+#define ONOFF_SLPEN_MASK		(1 << 2)
+#define ONOFF_PWR_OFF_MASK		(1 << 1)
+
+#define ONOFF_SLP_LPM_MASK		(1 << 5)
+
+#define ONOFF_IRQ_EN0_RISING		(1 << 3)
+
+enum {
+	CACHE_IRQ_LBT,
+	CACHE_IRQ_SD,
+	CACHE_IRQ_LDO,
+	CACHE_IRQ_ONOFF,
+	CACHE_IRQ_NR,
+};
+
+struct max77663_irq_data {
+	int mask_reg;
+	u16 mask;
+	u8 top_mask;
+	u8 top_shift;
+	int cache_idx;
+	bool is_rtc;
+	bool is_unmask;
+	u8 trigger_type;
+};
+
+struct max77663_chip {
+	struct device *dev;
+	struct i2c_client *i2c_power;
+	struct i2c_client *i2c_rtc;
+
+	struct max77663_platform_data *pdata;
+	struct mutex io_lock;
+
+	struct irq_chip irq;
+	struct mutex irq_lock;
+	int irq_base;
+	int irq_top_count[8];
+	u8 cache_irq_top_mask;
+	u16 cache_irq_mask[CACHE_IRQ_NR];
+
+	struct gpio_chip gpio;
+	int gpio_base;
+	u8 cache_gpio_ctrl[MAX77663_GPIO_NR];
+	u8 cache_gpio_pu;
+	u8 cache_gpio_pd;
+	u8 cache_gpio_alt;
+
+	u8 rtc_i2c_addr;
+};
+
+struct max77663_chip *max77663_chip;
+
+#define IRQ_DATA_LBT(_name, _shift)			\
+	[MAX77663_IRQ_LBT_##_name] = {			\
+		.mask_reg = MAX77663_REG_LBT_IRQ_MASK,	\
+		.mask = (1 << _shift),			\
+		.top_mask = IRQ_TOP_GLBL_MASK,		\
+		.top_shift = IRQ_TOP_GLBL_SHIFT,	\
+		.cache_idx = CACHE_IRQ_LBT,		\
+	}
+
+#define IRQ_DATA_GPIO(_name)				\
+	[MAX77663_IRQ_GPIO##_name] = {			\
+		.mask = (1 << _name),			\
+		.top_mask = IRQ_TOP_GPIO_MASK,		\
+		.top_shift = IRQ_TOP_GPIO_SHIFT,	\
+		.cache_idx = -1,			\
+	}
+
+#define IRQ_DATA_ONOFF(_name, _shift)			\
+	[MAX77663_IRQ_ONOFF_##_name] = {		\
+		.mask_reg = MAX77663_REG_ONOFF_IRQ_MASK,\
+		.mask = (1 << _shift),			\
+		.top_mask = IRQ_TOP_ONOFF_MASK,		\
+		.top_shift = IRQ_TOP_ONOFF_SHIFT,	\
+		.cache_idx = CACHE_IRQ_ONOFF,		\
+	}
+
+static struct max77663_irq_data max77663_irqs[MAX77663_IRQ_NR] = {
+	IRQ_DATA_LBT(LB, 3),
+	IRQ_DATA_LBT(THERM_ALRM1, 2),
+	IRQ_DATA_LBT(THERM_ALRM2, 1),
+	IRQ_DATA_GPIO(0),
+	IRQ_DATA_GPIO(1),
+	IRQ_DATA_GPIO(2),
+	IRQ_DATA_GPIO(3),
+	IRQ_DATA_GPIO(4),
+	IRQ_DATA_GPIO(5),
+	IRQ_DATA_GPIO(6),
+	IRQ_DATA_GPIO(7),
+	IRQ_DATA_ONOFF(HRDPOWRN,     0),
+	IRQ_DATA_ONOFF(EN0_1SEC,     1),
+	IRQ_DATA_ONOFF(EN0_FALLING,  2),
+	IRQ_DATA_ONOFF(EN0_RISING,   3),
+	IRQ_DATA_ONOFF(LID_FALLING,  4),
+	IRQ_DATA_ONOFF(LID_RISING,   5),
+	IRQ_DATA_ONOFF(ACOK_FALLING, 6),
+	IRQ_DATA_ONOFF(ACOK_RISING,  7),
+	[MAX77663_IRQ_RTC] = {
+		.top_mask = IRQ_TOP_RTC_MASK,
+		.top_shift = IRQ_TOP_RTC_SHIFT,
+		.cache_idx = -1,
+		.is_rtc = 1,
+	},
+	[MAX77663_IRQ_SD_PF] = {
+		.mask_reg = MAX77663_REG_SD_IRQ_MASK,
+		.mask = 0xF8,
+		.top_mask = IRQ_TOP_SD_MASK,
+		.top_shift = IRQ_TOP_SD_SHIFT,
+		.cache_idx = CACHE_IRQ_SD,
+	},
+	[MAX77663_IRQ_LDO_PF] = {
+		.mask_reg = MAX77663_REG_LDOX_IRQ_MASK,
+		.mask = 0x1FF,
+		.top_mask = IRQ_TOP_LDO_MASK,
+		.top_shift = IRQ_TOP_LDO_SHIFT,
+		.cache_idx = CACHE_IRQ_LDO,
+	},
+	[MAX77663_IRQ_32K] = {
+		.top_mask = IRQ_TOP_32K_MASK,
+		.top_shift = IRQ_TOP_32K_SHIFT,
+		.cache_idx = -1,
+	},
+	[MAX77663_IRQ_NVER] = {
+		.top_mask = IRQ_TOP_NVER_MASK,
+		.top_shift = IRQ_TOP_NVER_SHIFT,
+		.cache_idx = -1,
+	},
+};
+
+/* MAX77663 PMU doesn't allow PWR_OFF and SFT_RST setting in ONOFF_CFG1
+ * at the same time. So if it try to set PWR_OFF and SFT_RST to ONOFF_CFG1
+ * simultaneously, handle only SFT_RST and ignore PWR_OFF.
+ */
+#define CHECK_ONOFF_CFG1_MASK	(ONOFF_SFT_RST_MASK | ONOFF_PWR_OFF_MASK)
+#define CHECK_ONOFF_CFG1(_addr, _val)			\
+	unlikely((_addr == MAX77663_REG_ONOFF_CFG1) &&	\
+		 ((_val & CHECK_ONOFF_CFG1_MASK) == CHECK_ONOFF_CFG1_MASK))
+
+static inline int max77663_i2c_write(struct i2c_client *client, u8 addr,
+				     void *src, u32 bytes)
+{
+	u8 buf[bytes + 1];
+	int ret;
+
+	dev_dbg(&client->dev, "i2c_write: addr=0x%02x, src=0x%02x, bytes=%u\n",
+		addr, *((u8 *)src), bytes);
+
+	if (client->addr == max77663_chip->rtc_i2c_addr) {
+		/* RTC registers support sequential writing */
+		buf[0] = addr;
+		memcpy(&buf[1], src, bytes);
+	} else {
+		/* Power registers support register-data pair writing */
+		u8 *src8 = (u8 *)src;
+		int i;
+
+		for (i = 0; i < (bytes * 2); i++) {
+			if (i % 2) {
+				if (CHECK_ONOFF_CFG1(buf[i - 1], *src8))
+					buf[i] = *src8++ & ~ONOFF_PWR_OFF_MASK;
+				else
+					buf[i] = *src8++;
+			} else {
+				buf[i] = addr++;
+			}
+		}
+		bytes = (bytes * 2) - 1;
+	}
+
+	ret = i2c_master_send(client, buf, bytes + 1);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+
+static inline int max77663_i2c_read(struct i2c_client *client, u8 addr,
+				    void *dest, u32 bytes)
+{
+	int ret;
+
+	if (bytes > 1) {
+		ret = i2c_smbus_read_i2c_block_data(client, addr, bytes, dest);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = i2c_smbus_read_byte_data(client, addr);
+		if (ret < 0)
+			return ret;
+
+		*((u8 *)dest) = (u8)ret;
+	}
+
+	dev_dbg(&client->dev, "i2c_read: addr=0x%02x, dest=0x%02x, bytes=%u\n",
+		addr, *((u8 *)dest), bytes);
+	return 0;
+}
+
+int max77663_read(struct device *dev, u8 addr, void *values, u32 len,
+		  bool is_rtc)
+{
+	struct max77663_chip *chip = dev_get_drvdata(dev);
+	struct i2c_client *client = NULL;
+	int ret;
+
+	mutex_lock(&chip->io_lock);
+	if (!is_rtc)
+		client = chip->i2c_power;
+	else
+		client = chip->i2c_rtc;
+
+	ret = max77663_i2c_read(client, addr, values, len);
+	mutex_unlock(&chip->io_lock);
+	return ret;
+}
+EXPORT_SYMBOL(max77663_read);
+
+int max77663_write(struct device *dev, u8 addr, void *values, u32 len,
+		   bool is_rtc)
+{
+	struct max77663_chip *chip = dev_get_drvdata(dev);
+	struct i2c_client *client = NULL;
+	int ret;
+
+	mutex_lock(&chip->io_lock);
+	if (!is_rtc)
+		client = chip->i2c_power;
+	else
+		client = chip->i2c_rtc;
+
+	ret = max77663_i2c_write(client, addr, values, len);
+	mutex_unlock(&chip->io_lock);
+	return ret;
+}
+EXPORT_SYMBOL(max77663_write);
+
+int max77663_set_bits(struct device *dev, u8 addr, u8 mask, u8 value,
+		      bool is_rtc)
+{
+	struct max77663_chip *chip = dev_get_drvdata(dev);
+	struct i2c_client *client = NULL;
+	u8 tmp;
+	int ret;
+
+	mutex_lock(&chip->io_lock);
+	if (!is_rtc)
+		client = chip->i2c_power;
+	else
+		client = chip->i2c_rtc;
+
+	ret = max77663_i2c_read(client, addr, &tmp, 1);
+	if (ret == 0) {
+		value = (tmp & ~mask) | (value & mask);
+		ret = max77663_i2c_write(client, addr, &value, 1);
+	}
+	mutex_unlock(&chip->io_lock);
+	return ret;
+}
+EXPORT_SYMBOL(max77663_set_bits);
+
+static void max77663_power_off(void)
+{
+	struct max77663_chip *chip = max77663_chip;
+
+	if (!chip)
+		return;
+
+	dev_info(chip->dev, "%s: Global shutdown\n", __func__);
+	max77663_set_bits(chip->dev, MAX77663_REG_ONOFF_CFG1,
+			  ONOFF_SFT_RST_MASK, ONOFF_SFT_RST_MASK, 0);
+}
+
+static int max77663_sleep(struct max77663_chip *chip, bool on)
+{
+	int ret = 0;
+
+	if (chip->pdata->flags & SLP_LPM_ENABLE) {
+		/* Put the power rails into Low-Power mode during sleep mode,
+		 * if the power rail's power mode is GLPM. */
+		ret = max77663_set_bits(chip->dev, MAX77663_REG_ONOFF_CFG2,
+					ONOFF_SLP_LPM_MASK,
+					on ? ONOFF_SLP_LPM_MASK : 0, 0);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* Enable sleep that AP can be placed into sleep mode
+	 * by pulling EN1 low */
+	return max77663_set_bits(chip->dev, MAX77663_REG_ONOFF_CFG1,
+				 ONOFF_SLPEN_MASK,
+				 on ? ONOFF_SLPEN_MASK : 0, 0);
+}
+
+static inline int max77663_cache_write(struct device *dev, u8 addr, u8 mask,
+				       u8 val, u8 *cache)
+{
+	u8 new_val;
+	int ret;
+
+	new_val = (*cache & ~mask) | (val & mask);
+	if (*cache != new_val) {
+		ret = max77663_write(dev, addr, &new_val, 1, 0);
+		if (ret < 0)
+			return ret;
+		*cache = new_val;
+	}
+	return 0;
+}
+
+static inline
+struct max77663_chip *max77663_chip_from_gpio(struct gpio_chip *gpio)
+{
+	return container_of(gpio, struct max77663_chip, gpio);
+}
+
+static int max77663_gpio_set_pull_up(struct max77663_chip *chip, int offset,
+				     int pull_up)
+{
+	u8 val = 0;
+
+	if ((offset < MAX77663_GPIO0) || (MAX77663_GPIO7 < offset))
+		return -EINVAL;
+
+	if (pull_up == GPIO_PU_ENABLE)
+		val = (1 << offset);
+
+	return max77663_cache_write(chip->dev, MAX77663_REG_GPIO_PU,
+				    (1 << offset), val, &chip->cache_gpio_pu);
+}
+
+static int max77663_gpio_set_pull_down(struct max77663_chip *chip, int offset,
+				       int pull_down)
+{
+	u8 val = 0;
+
+	if ((offset < MAX77663_GPIO0) || (MAX77663_GPIO7 < offset))
+		return -EINVAL;
+
+	if (pull_down == GPIO_PD_ENABLE)
+		val = (1 << offset);
+
+	return max77663_cache_write(chip->dev, MAX77663_REG_GPIO_PD,
+				    (1 << offset), val, &chip->cache_gpio_pd);
+}
+
+static inline
+int max77663_gpio_is_alternate(struct max77663_chip *chip, int offset)
+{
+	return (chip->cache_gpio_alt & (1 << offset)) ? 1 : 0;
+}
+
+int max77663_gpio_set_alternate(int gpio, int alternate)
+{
+	struct max77663_chip *chip = max77663_chip;
+	u8 val = 0;
+	int ret = 0;
+
+	if (!chip)
+		return -ENXIO;
+
+	gpio -= chip->gpio_base;
+	if ((gpio < MAX77663_GPIO0) || (MAX77663_GPIO7 < gpio))
+		return -EINVAL;
+
+	if (alternate == GPIO_ALT_ENABLE) {
+		val = (1 << gpio);
+		if (gpio == MAX77663_GPIO7) {
+			ret = max77663_gpio_set_pull_up(chip, gpio, 0);
+			if (ret < 0)
+				return ret;
+
+			ret = max77663_gpio_set_pull_down(chip, gpio, 0);
+			if (ret < 0)
+				return ret;
+		}
+	}
+
+	return max77663_cache_write(chip->dev, MAX77663_REG_GPIO_ALT,
+				    (1 << gpio), val, &chip->cache_gpio_alt);
+}
+EXPORT_SYMBOL(max77663_gpio_set_alternate);
+
+static int max77663_gpio_dir_input(struct gpio_chip *gpio, unsigned offset)
+{
+	struct max77663_chip *chip = max77663_chip_from_gpio(gpio);
+
+	if (max77663_gpio_is_alternate(chip, offset)) {
+		dev_warn(chip->dev, "gpio_dir_input: "
+			"gpio%u is used as alternate mode\n", offset);
+		return 0;
+	}
+
+	return max77663_cache_write(chip->dev, GPIO_REG_ADDR(offset),
+				    GPIO_CTRL_DIR_MASK, GPIO_CTRL_DIR_MASK,
+				    &chip->cache_gpio_ctrl[offset]);
+}
+
+static int max77663_gpio_get(struct gpio_chip *gpio, unsigned offset)
+{
+	struct max77663_chip *chip = max77663_chip_from_gpio(gpio);
+	u8 val;
+	int ret;
+
+	if (max77663_gpio_is_alternate(chip, offset)) {
+		dev_warn(chip->dev, "gpio_get: "
+			"gpio%u is used as alternate mode\n", offset);
+		return 0;
+	}
+
+	ret = max77663_read(chip->dev, GPIO_REG_ADDR(offset), &val, 1, 0);
+	if (ret < 0)
+		return ret;
+
+	chip->cache_gpio_ctrl[offset] = val;
+	return (val & GPIO_CTRL_DIN_MASK) >> GPIO_CTRL_DIN_SHIFT;
+}
+
+static int max77663_gpio_dir_output(struct gpio_chip *gpio, unsigned offset,
+				    int value)
+{
+	struct max77663_chip *chip = max77663_chip_from_gpio(gpio);
+	u8 mask = GPIO_CTRL_DIR_MASK | GPIO_CTRL_DOUT_MASK;
+	u8 val = (value ? 1 : 0) << GPIO_CTRL_DOUT_SHIFT;
+
+	if (max77663_gpio_is_alternate(chip, offset)) {
+		dev_warn(chip->dev, "gpio_dir_output: "
+			"gpio%u is used as alternate mode\n", offset);
+		return 0;
+	}
+
+	return max77663_cache_write(chip->dev, GPIO_REG_ADDR(offset), mask, val,
+				    &chip->cache_gpio_ctrl[offset]);
+}
+
+static int max77663_gpio_set_debounce(struct gpio_chip *gpio, unsigned offset,
+				      unsigned debounce)
+{
+	struct max77663_chip *chip = max77663_chip_from_gpio(gpio);
+	u8 shift = GPIO_CTRL_DBNC_SHIFT;
+	u8 val = 0;
+
+	if (max77663_gpio_is_alternate(chip, offset)) {
+		dev_warn(chip->dev, "gpio_set_debounce: "
+			"gpio%u is used as alternate mode\n", offset);
+		return 0;
+	}
+
+	if (debounce == 0)
+		val = 0;
+	else if ((0 < debounce) && (debounce <= 8))
+		val = (GPIO_DBNC_8MS << shift);
+	else if ((8 < debounce) && (debounce <= 16))
+		val = (GPIO_DBNC_16MS << shift);
+	else if ((16 < debounce) && (debounce <= 32))
+		val = (GPIO_DBNC_32MS << shift);
+	else
+		return -EINVAL;
+
+	return max77663_cache_write(chip->dev, GPIO_REG_ADDR(offset),
+				    GPIO_CTRL_DBNC_MASK, val,
+				    &chip->cache_gpio_ctrl[offset]);
+}
+
+static void max77663_gpio_set(struct gpio_chip *gpio, unsigned offset,
+			      int value)
+{
+	struct max77663_chip *chip = max77663_chip_from_gpio(gpio);
+	u8 val = (value ? 1 : 0) << GPIO_CTRL_DOUT_SHIFT;
+
+	if (max77663_gpio_is_alternate(chip, offset)) {
+		dev_warn(chip->dev, "gpio_set: "
+			"gpio%u is used as alternate mode\n", offset);
+		return;
+	}
+
+	max77663_cache_write(chip->dev, GPIO_REG_ADDR(offset),
+			     GPIO_CTRL_DOUT_MASK, val,
+			     &chip->cache_gpio_ctrl[offset]);
+}
+
+static int max77663_gpio_to_irq(struct gpio_chip *gpio, unsigned offset)
+{
+	struct max77663_chip *chip = max77663_chip_from_gpio(gpio);
+
+	return chip->irq_base + IRQ_GPIO_BASE + offset;
+}
+
+#ifdef CONFIG_DEBUG_FS
+static void max77663_gpio_dbg_show(struct seq_file *s, struct gpio_chip *gpio)
+{
+	struct max77663_chip *chip = max77663_chip_from_gpio(gpio);
+	int i;
+
+	for (i = 0; i < gpio->ngpio; i++) {
+		u8 ctrl_val;
+		const char *label;
+		int is_out;
+		int ret;
+
+		label = gpiochip_is_requested(gpio, i);
+		if (!label)
+			label = "Unrequested";
+
+		seq_printf(s, " gpio-%-3d (%-20.20s) ", i + chip->gpio_base,
+			   label);
+
+		if (chip->cache_gpio_alt & (1 << i)) {
+			seq_printf(s, "alt\n");
+			continue;
+		}
+
+		ret = max77663_read(chip->dev, GPIO_REG_ADDR(i), &ctrl_val, 1,
+				    0);
+		if (ret < 0) {
+			seq_printf(s, "\n");
+			continue;
+		}
+
+		is_out = ctrl_val & GPIO_CTRL_DIR_MASK ? 0 : 1;
+		seq_printf(s, "%s %s", (is_out ? "out" : "in"), (is_out ?
+			   (ctrl_val & GPIO_CTRL_DOUT_MASK ? "hi" : "lo")
+			   : (ctrl_val & GPIO_CTRL_DIN_MASK ? "hi" : "lo")));
+
+		if (!is_out) {
+			int irq = gpio_to_irq(i + chip->gpio_base);
+			struct irq_desc *desc = irq_to_desc(irq);
+			u8 dbnc;
+
+			if (irq >= 0 && desc->action) {
+				u8 mask = GPIO_CTRL_REFE_IRQ_MASK;
+				u8 shift = GPIO_CTRL_REFE_IRQ_SHIFT;
+				char *trigger;
+
+				switch ((ctrl_val & mask) >> shift) {
+				case GPIO_REFE_IRQ_EDGE_FALLING:
+					trigger = "edge-falling";
+					break;
+				case GPIO_REFE_IRQ_EDGE_RISING:
+					trigger = "edge-rising";
+					break;
+				case GPIO_REFE_IRQ_EDGE_BOTH:
+					trigger = "edge-both";
+					break;
+				default:
+					trigger = "masked";
+					break;
+				}
+
+				seq_printf(s, " irq-%d %s", irq, trigger);
+			}
+
+			dbnc = (ctrl_val & GPIO_CTRL_DBNC_MASK)
+				>> GPIO_CTRL_DBNC_SHIFT;
+			seq_printf(s, " debounce-%s",
+				   dbnc ==  GPIO_DBNC_8MS ? "8ms" :
+				   dbnc ==  GPIO_DBNC_16MS ? "16ms" :
+				   dbnc ==  GPIO_DBNC_32MS ? "32ms" : "none");
+		} else {
+			seq_printf(s, " %s",
+				   (ctrl_val & GPIO_CTRL_OUT_DRV_MASK ?
+				   "output-drive" : "open-drain"));
+		}
+
+		seq_printf(s, "\n");
+	}
+}
+#else
+#define max77663_gpio_dbg_show		NULL
+#endif /* CONFIG_DEBUG_FS */
+
+static int max77663_gpio_set_config(struct max77663_chip *chip,
+				    struct max77663_gpio_config *gpio_cfg)
+{
+	int gpio = gpio_cfg->gpio;
+	u8 val = 0, mask = 0;
+	int ret = 0;
+
+	if ((gpio < MAX77663_GPIO0) || (MAX77663_GPIO7 < gpio))
+		return -EINVAL;
+
+	if (gpio_cfg->pull_up != GPIO_PU_DEF) {
+		ret = max77663_gpio_set_pull_up(chip, gpio, gpio_cfg->pull_up);
+		if (ret < 0) {
+			dev_err(chip->dev, "gpio_set_config: "
+				"Failed to set gpio%d pull-up\n", gpio);
+			return ret;
+		}
+	}
+
+	if (gpio_cfg->pull_down != GPIO_PD_DEF) {
+		ret = max77663_gpio_set_pull_down(chip, gpio,
+						  gpio_cfg->pull_down);
+		if (ret < 0) {
+			dev_err(chip->dev, "gpio_set_config: "
+				"Failed to set gpio%d pull-down\n", gpio);
+			return ret;
+		}
+	}
+
+	if (gpio_cfg->dir != GPIO_DIR_DEF) {
+		mask = GPIO_CTRL_DIR_MASK;
+		if (gpio_cfg->dir == GPIO_DIR_IN) {
+			val |= GPIO_CTRL_DIR_MASK;
+		} else {
+			if (gpio_cfg->dout != GPIO_DOUT_DEF) {
+				mask |= GPIO_CTRL_DOUT_MASK;
+				if (gpio_cfg->dout == GPIO_DOUT_HIGH)
+					val |= GPIO_CTRL_DOUT_MASK;
+			}
+
+			if (gpio_cfg->out_drv != GPIO_OUT_DRV_DEF) {
+				mask |= GPIO_CTRL_OUT_DRV_MASK;
+				if (gpio_cfg->out_drv == GPIO_OUT_DRV_PUSH_PULL)
+					val |= GPIO_CTRL_OUT_DRV_MASK;
+			}
+		}
+
+		ret = max77663_cache_write(chip->dev, GPIO_REG_ADDR(gpio), mask,
+					   val, &chip->cache_gpio_ctrl[gpio]);
+		if (ret < 0) {
+			dev_err(chip->dev, "gpio_set_config: "
+				"Failed to set gpio%d control\n", gpio);
+			return ret;
+		}
+	}
+
+	if (gpio_cfg->alternate != GPIO_ALT_DEF) {
+		ret = max77663_gpio_set_alternate(gpio + chip->gpio_base,
+						  gpio_cfg->alternate);
+		if (ret < 0) {
+			dev_err(chip->dev, "gpio_set_config: "
+				"Failed to set gpio%d alternate\n", gpio);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int max77663_gpio_init(struct max77663_chip *chip)
+{
+	int i;
+	int ret;
+
+	chip->gpio.label = chip->i2c_power->name;
+	chip->gpio.dev = chip->dev;
+	chip->gpio.owner = THIS_MODULE;
+	chip->gpio.direction_input = max77663_gpio_dir_input;
+	chip->gpio.get = max77663_gpio_get;
+	chip->gpio.direction_output = max77663_gpio_dir_output;
+	chip->gpio.set_debounce = max77663_gpio_set_debounce;
+	chip->gpio.set = max77663_gpio_set;
+	chip->gpio.to_irq = max77663_gpio_to_irq;
+	chip->gpio.dbg_show = max77663_gpio_dbg_show;
+	chip->gpio.ngpio = MAX77663_GPIO_NR;
+	chip->gpio.can_sleep = 1;
+	if (chip->gpio_base)
+		chip->gpio.base = chip->gpio_base;
+	else
+		chip->gpio.base = -1;
+
+	ret = max77663_read(chip->dev, MAX77663_REG_GPIO_CTRL0,
+			    chip->cache_gpio_ctrl, MAX77663_GPIO_NR, 0);
+	if (ret < 0) {
+		dev_err(chip->dev, "gpio_init: Failed to get gpio control\n");
+		return ret;
+	}
+
+	ret = max77663_read(chip->dev, MAX77663_REG_GPIO_PU,
+			    &chip->cache_gpio_pu, 1, 0);
+	if (ret < 0) {
+		dev_err(chip->dev, "gpio_init: Failed to get gpio pull-up\n");
+		return ret;
+	}
+
+	ret = max77663_read(chip->dev, MAX77663_REG_GPIO_PD,
+			    &chip->cache_gpio_pd, 1, 0);
+	if (ret < 0) {
+		dev_err(chip->dev, "gpio_init: Failed to get gpio pull-down\n");
+		return ret;
+	}
+
+	ret = max77663_read(chip->dev, MAX77663_REG_GPIO_ALT,
+			    &chip->cache_gpio_alt, 1, 0);
+	if (ret < 0) {
+		dev_err(chip->dev, "gpio_init: Failed to get gpio alternate\n");
+		return ret;
+	}
+
+	ret = gpiochip_add(&chip->gpio);
+	if (ret < 0) {
+		dev_err(chip->dev, "gpio_init: Failed to add gpiochip\n");
+		return ret;
+	}
+	chip->gpio_base = chip->gpio.base;
+
+	for (i = 0; i < chip->pdata->num_gpio_cfgs; i++) {
+		ret = max77663_gpio_set_config(chip,
+					       &chip->pdata->gpio_cfgs[i]);
+		if (ret < 0) {
+			dev_err(chip->dev,
+				"gpio_init: Failed to set gpio config\n");
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static void max77663_gpio_exit(struct max77663_chip *chip)
+{
+	if (gpiochip_remove(&chip->gpio) < 0)
+		dev_err(chip->dev, "gpio_exit: Failed to remove gpiochip\n");
+}
+
+static void max77663_irq_mask(struct irq_data *data)
+{
+	struct max77663_chip *chip = irq_data_get_irq_chip_data(data);
+
+	max77663_irqs[data->irq - chip->irq_base].is_unmask = 0;
+}
+
+static void max77663_irq_unmask(struct irq_data *data)
+{
+	struct max77663_chip *chip = irq_data_get_irq_chip_data(data);
+
+	max77663_irqs[data->irq - chip->irq_base].is_unmask = 1;
+}
+
+static void max77663_irq_lock(struct irq_data *data)
+{
+	struct max77663_chip *chip = irq_data_get_irq_chip_data(data);
+
+	mutex_lock(&chip->irq_lock);
+}
+
+static void max77663_irq_sync_unlock(struct irq_data *data)
+{
+	struct max77663_chip *chip = irq_data_get_irq_chip_data(data);
+	struct max77663_irq_data *irq_data =
+			&max77663_irqs[data->irq - chip->irq_base];
+	int idx = irq_data->cache_idx;
+	u8 irq_top_mask = chip->cache_irq_top_mask;
+	u16 irq_mask = chip->cache_irq_mask[idx];
+	int update_irq_top = 0;
+	u32 len = 1;
+	int ret;
+
+	if (irq_data->is_unmask) {
+		if (chip->irq_top_count[irq_data->top_shift] == 0)
+			update_irq_top = 1;
+		chip->irq_top_count[irq_data->top_shift]++;
+
+		if (irq_data->top_mask != IRQ_TOP_GLBL_MASK)
+			irq_top_mask &= ~irq_data->top_mask;
+
+		if (idx != -1)
+			irq_mask &= ~irq_data->mask;
+	} else {
+		if (chip->irq_top_count[irq_data->top_shift] == 1)
+			update_irq_top = 1;
+
+		if (--chip->irq_top_count[irq_data->top_shift] < 0)
+			chip->irq_top_count[irq_data->top_shift] = 0;
+
+		if (irq_data->top_mask != IRQ_TOP_GLBL_MASK)
+			irq_top_mask |= irq_data->top_mask;
+
+		if (idx != -1)
+			irq_mask |= irq_data->mask;
+	}
+
+	if ((idx != -1) && (irq_mask != chip->cache_irq_mask[idx])) {
+		if (irq_data->top_mask == IRQ_TOP_LDO_MASK)
+			len = 2;
+
+		ret = max77663_write(chip->dev, irq_data->mask_reg,
+				     &irq_mask, len, irq_data->is_rtc);
+		if (ret < 0)
+			goto out;
+
+		chip->cache_irq_mask[idx] = irq_mask;
+	} else if ((idx == -1) && (irq_data->top_mask == IRQ_TOP_GPIO_MASK)) {
+		unsigned offset = data->irq - chip->irq_base - IRQ_GPIO_BASE;
+		u8 shift = GPIO_CTRL_REFE_IRQ_SHIFT;
+
+		if (irq_data->is_unmask) {
+			if (irq_data->trigger_type)
+				irq_mask = irq_data->trigger_type;
+			else
+				irq_mask = GPIO_REFE_IRQ_EDGE_FALLING << shift;
+		} else {
+			irq_mask = GPIO_REFE_IRQ_NONE << shift;
+		}
+
+		ret = max77663_cache_write(chip->dev, GPIO_REG_ADDR(offset),
+					   GPIO_CTRL_REFE_IRQ_MASK, irq_mask,
+					   &chip->cache_gpio_ctrl[offset]);
+		if (ret < 0)
+			goto out;
+
+		if (irq_data->is_unmask)
+			irq_data->trigger_type = irq_mask;
+	}
+
+	if (update_irq_top && (irq_top_mask != chip->cache_irq_top_mask)) {
+		ret = max77663_cache_write(chip->dev, MAX77663_REG_IRQ_TOP_MASK,
+					   irq_data->top_mask, irq_top_mask,
+					   &chip->cache_irq_top_mask);
+		if (ret < 0)
+			goto out;
+	}
+
+out:
+	mutex_unlock(&chip->irq_lock);
+}
+
+static int max77663_irq_gpio_set_type(struct irq_data *data, unsigned int type)
+{
+	struct max77663_chip *chip = irq_data_get_irq_chip_data(data);
+	struct max77663_irq_data *irq_data =
+			&max77663_irqs[data->irq - chip->irq_base];
+	unsigned offset = data->irq - chip->irq_base - IRQ_GPIO_BASE;
+	u8 shift = GPIO_CTRL_REFE_IRQ_SHIFT;
+	u8 val;
+
+	switch (type) {
+	case IRQ_TYPE_NONE:
+	case IRQ_TYPE_EDGE_FALLING:
+		val = (GPIO_REFE_IRQ_EDGE_FALLING << shift);
+		break;
+	case IRQ_TYPE_EDGE_RISING:
+		val = (GPIO_REFE_IRQ_EDGE_RISING << shift);
+		break;
+	case IRQ_TYPE_EDGE_BOTH:
+		val = (GPIO_REFE_IRQ_EDGE_BOTH << shift);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	irq_data->trigger_type = val;
+	if (!(chip->cache_gpio_ctrl[offset] & GPIO_CTRL_REFE_IRQ_MASK))
+		return 0;
+
+	return max77663_cache_write(chip->dev, GPIO_REG_ADDR(offset),
+				    GPIO_CTRL_REFE_IRQ_MASK, val,
+				    &chip->cache_gpio_ctrl[offset]);
+}
+
+static inline int max77663_do_irq(struct max77663_chip *chip, u8 addr,
+				  int irq_base, int irq_end)
+{
+	struct max77663_irq_data *irq_data = NULL;
+	int irqs_to_handle[irq_end - irq_base + 1];
+	int handled = 0;
+	u16 val;
+	u32 len = 1;
+	int i;
+	int ret;
+
+	ret = max77663_read(chip->dev, addr, &val, len, 0);
+	if (ret < 0)
+		return ret;
+
+	for (i = irq_base; i <= irq_end; i++) {
+		irq_data = &max77663_irqs[i];
+		if (val & irq_data->mask) {
+			irqs_to_handle[handled] = i + chip->irq_base;
+			handled++;
+		}
+	}
+
+	for (i = 0; i < handled; i++)
+		handle_nested_irq(irqs_to_handle[i]);
+
+	return 0;
+}
+
+static irqreturn_t max77663_irq(int irq, void *data)
+{
+	struct max77663_chip *chip = data;
+	u8 irq_top;
+	int ret;
+
+	ret = max77663_read(chip->dev, MAX77663_REG_IRQ_TOP, &irq_top, 1, 0);
+	if (ret < 0) {
+		dev_err(chip->dev, "irq: Failed to get irq top status\n");
+		return IRQ_NONE;
+	}
+
+	if (irq_top & IRQ_TOP_GLBL_MASK) {
+		ret = max77663_do_irq(chip, MAX77663_REG_LBT_IRQ, IRQ_LBT_BASE,
+				      IRQ_LBT_END);
+		if (ret < 0)
+			return IRQ_NONE;
+	}
+
+	if (irq_top & IRQ_TOP_GPIO_MASK) {
+		ret = max77663_do_irq(chip, MAX77663_REG_GPIO_IRQ,
+				      IRQ_GPIO_BASE, IRQ_GPIO_END);
+		if (ret < 0)
+			return IRQ_NONE;
+	}
+
+	if (irq_top & IRQ_TOP_ONOFF_MASK) {
+		ret = max77663_do_irq(chip, MAX77663_REG_ONOFF_IRQ,
+				      IRQ_ONOFF_BASE, IRQ_ONOFF_END);
+		if (ret < 0)
+			return IRQ_NONE;
+	}
+
+	if (irq_top & IRQ_TOP_RTC_MASK)
+		handle_nested_irq(MAX77663_IRQ_RTC + chip->irq_base);
+
+	if (irq_top & IRQ_TOP_SD_MASK)
+		handle_nested_irq(MAX77663_IRQ_SD_PF + chip->irq_base);
+
+	if (irq_top & IRQ_TOP_LDO_MASK)
+		handle_nested_irq(MAX77663_IRQ_LDO_PF + chip->irq_base);
+
+	if (irq_top & IRQ_TOP_32K_MASK)
+		handle_nested_irq(MAX77663_IRQ_32K + chip->irq_base);
+
+	if (irq_top & IRQ_TOP_NVER_MASK)
+		handle_nested_irq(MAX77663_IRQ_NVER + chip->irq_base);
+
+	return IRQ_HANDLED;
+}
+
+static struct irq_chip max77663_irq_gpio_chip = {
+	.name = "max77663-irq",
+	.irq_mask = max77663_irq_mask,
+	.irq_unmask = max77663_irq_unmask,
+	.irq_set_type = max77663_irq_gpio_set_type,
+	.irq_bus_lock = max77663_irq_lock,
+	.irq_bus_sync_unlock = max77663_irq_sync_unlock,
+};
+
+static struct irq_chip max77663_irq_chip = {
+	.name = "max77663-irq",
+	.irq_mask = max77663_irq_mask,
+	.irq_unmask = max77663_irq_unmask,
+	.irq_bus_lock = max77663_irq_lock,
+	.irq_bus_sync_unlock = max77663_irq_sync_unlock,
+};
+
+static int max77663_irq_init(struct max77663_chip *chip)
+{
+	u32 temp;
+	int i, ret = 0;
+
+	mutex_init(&chip->irq_lock);
+
+	/* Mask all interrupts */
+	chip->cache_irq_top_mask = 0xFF;
+	chip->cache_irq_mask[CACHE_IRQ_LBT] = 0x0F;
+	chip->cache_irq_mask[CACHE_IRQ_SD] = 0xFF;
+	chip->cache_irq_mask[CACHE_IRQ_LDO] = 0xFFFF;
+	chip->cache_irq_mask[CACHE_IRQ_ONOFF] = 0xFF;
+
+	max77663_write(chip->dev, MAX77663_REG_IRQ_TOP_MASK,
+		       &chip->cache_irq_top_mask, 1, 0);
+	max77663_write(chip->dev, MAX77663_REG_LBT_IRQ_MASK,
+		       &chip->cache_irq_mask[CACHE_IRQ_LBT], 1, 0);
+	max77663_write(chip->dev, MAX77663_REG_SD_IRQ_MASK,
+		       &chip->cache_irq_mask[CACHE_IRQ_SD], 1, 0);
+	max77663_write(chip->dev, MAX77663_REG_LDOX_IRQ_MASK,
+		       &chip->cache_irq_mask[CACHE_IRQ_LDO], 2, 0);
+	max77663_write(chip->dev, MAX77663_REG_ONOFF_IRQ_MASK,
+		       &chip->cache_irq_mask[CACHE_IRQ_ONOFF], 1, 0);
+
+	/* Clear all interrups */
+	max77663_read(chip->dev, MAX77663_REG_LBT_IRQ, &temp, 1, 0);
+	max77663_read(chip->dev, MAX77663_REG_SD_IRQ, &temp, 1, 0);
+	max77663_read(chip->dev, MAX77663_REG_LDOX_IRQ, &temp, 2, 0);
+	max77663_read(chip->dev, MAX77663_REG_GPIO_IRQ, &temp, 1, 0);
+	max77663_read(chip->dev, MAX77663_REG_ONOFF_IRQ, &temp, 1, 0);
+
+	for (i = chip->irq_base; i < (MAX77663_IRQ_NR + chip->irq_base); i++) {
+		if (i >= NR_IRQS) {
+			dev_err(chip->dev,
+				"irq_init: Can't set irq chip for irq %d\n", i);
+			continue;
+		}
+
+		irq_set_chip_data(i, chip);
+
+		if ((IRQ_GPIO_BASE <= i - chip->irq_base) &&
+				(i - chip->irq_base <= IRQ_GPIO_END))
+			irq_set_chip_and_handler(i, &max77663_irq_gpio_chip,
+						 handle_edge_irq);
+		else
+			irq_set_chip_and_handler(i, &max77663_irq_chip,
+						 handle_edge_irq);
+#ifdef CONFIG_ARM
+		set_irq_flags(i, IRQF_VALID);
+#else
+		irq_set_noprobe(i);
+#endif
+		irq_set_nested_thread(i, 1);
+	}
+
+	ret = request_threaded_irq(chip->i2c_power->irq, NULL, max77663_irq,
+				   IRQF_ONESHOT, "max77663", chip);
+	if (ret) {
+		dev_err(chip->dev, "irq_init: Failed to request irq %d\n",
+			chip->i2c_power->irq);
+		return ret;
+	}
+
+	device_init_wakeup(chip->dev, 1);
+	enable_irq_wake(chip->i2c_power->irq);
+
+	chip->cache_irq_top_mask &= ~IRQ_TOP_GLBL_MASK;
+	max77663_write(chip->dev, MAX77663_REG_IRQ_TOP_MASK,
+		       &chip->cache_irq_top_mask, 1, 0);
+
+	chip->cache_irq_mask[CACHE_IRQ_LBT] &= ~IRQ_GLBL_MASK;
+	max77663_write(chip->dev, MAX77663_REG_LBT_IRQ_MASK,
+		       &chip->cache_irq_mask[CACHE_IRQ_LBT], 1, 0);
+
+	chip->cache_irq_mask[CACHE_IRQ_ONOFF] &= ~ONOFF_IRQ_EN0_RISING;
+	max77663_write(chip->dev, MAX77663_REG_ONOFF_IRQ_MASK,
+		       &chip->cache_irq_mask[CACHE_IRQ_ONOFF], 1, 0);
+
+	return 0;
+}
+
+static void max77663_irq_exit(struct max77663_chip *chip)
+{
+	if (chip->i2c_power->irq)
+		free_irq(chip->i2c_power->irq, chip);
+}
+
+#ifdef CONFIG_DEBUG_FS
+static struct dentry *max77663_dentry_regs;
+
+static int max77663_debugfs_dump_regs(struct max77663_chip *chip, char *label,
+				      u8 *addrs, int num_addrs, char *buf,
+				      ssize_t *len, int is_rtc)
+{
+	ssize_t count = *len;
+	u8 val;
+	int ret = 0;
+	int i;
+
+	count += sprintf(buf + count, "%s\n", label);
+	if (count >= PAGE_SIZE - 1)
+		return -ERANGE;
+
+	for (i = 0; i < num_addrs; i++) {
+		count += sprintf(buf + count, "0x%02x: ", addrs[i]);
+		if (count >= PAGE_SIZE - 1)
+			return -ERANGE;
+
+		ret = max77663_read(chip->dev, addrs[i], &val, 1, is_rtc);
+		if (ret == 0)
+			count += sprintf(buf + count, "0x%02x\n", val);
+		else
+			count += sprintf(buf + count, "<read fail: %d>\n", ret);
+
+		if (count >= PAGE_SIZE - 1)
+			return -ERANGE;
+	}
+
+	*len = count;
+	return 0;
+}
+
+static int max77663_debugfs_regs_open(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+
+static ssize_t max77663_debugfs_regs_read(struct file *file,
+					  char __user *user_buf,
+					  size_t count, loff_t *ppos)
+{
+	struct max77663_chip *chip = file->private_data;
+	char *buf;
+	size_t len = 0;
+	ssize_t ret;
+
+	/* Excluded interrupt status register to prevent register clear */
+	u8 global_regs[] = { 0x00, 0x01, 0x02, 0x05, 0x0D, 0x0E, 0x13 };
+	u8 sd_regs[] = {
+		0x07, 0x0F, 0x16, 0x17, 0x18, 0x19, 0x1A, 0x1B, 0x1C, 0x1D,
+		0x1E, 0x1F, 0x20, 0x21, 0x22
+	};
+	u8 ldo_regs[] = {
+		0x10, 0x11, 0x23, 0x24, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2A,
+		0x2B, 0x2C, 0x2D, 0x2E, 0x2F, 0x30, 0x31, 0x32, 0x33, 0x34,
+		0x35
+	};
+	u8 gpio_regs[] = {
+		0x36, 0x37, 0x38, 0x39, 0x3A, 0x3B, 0x3C, 0x3D, 0x3E, 0x3F,
+		0x40
+	};
+	u8 rtc_regs[] = {
+		0x01, 0x02, 0x03, 0x04, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B,
+		0x0C, 0x0D, 0x0E, 0x0F, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15,
+		0x16, 0x17, 0x18, 0x19, 0x1A, 0x1B
+	};
+	u8 osc_32k_regs[] = { 0x03 };
+	u8 bbc_regs[] = { 0x04 };
+	u8 onoff_regs[] = { 0x12, 0x15, 0x41, 0x42 };
+	u8 fps_regs[] = {
+		0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4A, 0x4B, 0x4C,
+		0x4D, 0x4E, 0x4F, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56,
+		0x57
+	};
+	u8 cid_regs[] = { 0x58, 0x59, 0x5A, 0x5B, 0x5C, 0x5D };
+
+	buf = kmalloc(PAGE_SIZE, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	len += sprintf(buf + len, "MAX77663 Registers\n");
+	max77663_debugfs_dump_regs(chip, "[Global]", global_regs,
+				   ARRAY_SIZE(global_regs), buf, &len, 0);
+	max77663_debugfs_dump_regs(chip, "[Step-Down]", sd_regs,
+				   ARRAY_SIZE(sd_regs), buf, &len, 0);
+	max77663_debugfs_dump_regs(chip, "[LDO]", ldo_regs,
+				   ARRAY_SIZE(ldo_regs), buf, &len, 0);
+	max77663_debugfs_dump_regs(chip, "[GPIO]", gpio_regs,
+				   ARRAY_SIZE(gpio_regs), buf, &len, 0);
+	max77663_debugfs_dump_regs(chip, "[RTC]", rtc_regs,
+				   ARRAY_SIZE(rtc_regs), buf, &len, 1);
+	max77663_debugfs_dump_regs(chip, "[32kHz Oscillator]", osc_32k_regs,
+				   ARRAY_SIZE(osc_32k_regs), buf, &len, 0);
+	max77663_debugfs_dump_regs(chip, "[Backup Battery Charger]", bbc_regs,
+				   ARRAY_SIZE(bbc_regs), buf, &len, 0);
+	max77663_debugfs_dump_regs(chip, "[On/OFF Controller]", onoff_regs,
+				   ARRAY_SIZE(onoff_regs), buf, &len, 0);
+	max77663_debugfs_dump_regs(chip, "[Flexible Power Sequencer]", fps_regs,
+				   ARRAY_SIZE(fps_regs), buf, &len, 0);
+	max77663_debugfs_dump_regs(chip, "[Chip Identification]", cid_regs,
+				   ARRAY_SIZE(cid_regs), buf, &len, 0);
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, len);
+	kfree(buf);
+
+	return ret;
+}
+
+static const struct file_operations max77663_debugfs_regs_fops = {
+	.open = max77663_debugfs_regs_open,
+	.read = max77663_debugfs_regs_read,
+};
+
+static void max77663_debugfs_init(struct max77663_chip *chip)
+{
+	max77663_dentry_regs = debugfs_create_file(chip->i2c_power->name,
+						   0444, 0, chip,
+						   &max77663_debugfs_regs_fops);
+	if (!max77663_dentry_regs)
+		dev_warn(chip->dev,
+			 "debugfs_init: Failed to create debugfs file\n");
+}
+
+static void max77663_debugfs_exit(struct max77663_chip *chip)
+{
+	debugfs_remove(max77663_dentry_regs);
+}
+#else
+static inline void max77663_debugfs_init(struct max77663_chip *chip)
+{
+}
+
+static inline void max77663_debugfs_exit(struct max77663_chip *chip)
+{
+}
+#endif /* CONFIG_DEBUG_FS */
+
+static int max77663_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
+{
+	struct max77663_platform_data *pdata = client->dev.platform_data;
+	struct max77663_chip *chip;
+	int ret = 0;
+
+	if (pdata == NULL) {
+		dev_err(&client->dev, "probe: Invalid platform_data\n");
+		ret = -ENODEV;
+		goto out;
+	}
+
+	chip = kzalloc(sizeof(struct max77663_chip), GFP_KERNEL);
+	if (chip == NULL) {
+		dev_err(&client->dev, "probe: kzalloc() failed\n");
+		ret = -ENOMEM;
+		goto out;
+	}
+	max77663_chip = chip;
+
+	chip->i2c_power = client;
+	i2c_set_clientdata(client, chip);
+
+	if (pdata->rtc_i2c_addr)
+		chip->rtc_i2c_addr = pdata->rtc_i2c_addr;
+	else
+		chip->rtc_i2c_addr = MAX77663_RTC_I2C_ADDR;
+
+	chip->i2c_rtc = i2c_new_dummy(client->adapter, chip->rtc_i2c_addr);
+	i2c_set_clientdata(chip->i2c_rtc, chip);
+
+	chip->dev = &client->dev;
+	chip->pdata = pdata;
+	chip->irq_base = pdata->irq_base;
+	chip->gpio_base = pdata->gpio_base;
+	mutex_init(&chip->io_lock);
+
+	max77663_gpio_init(chip);
+	max77663_irq_init(chip);
+	max77663_debugfs_init(chip);
+	ret = max77663_sleep(chip, false);
+	if (ret < 0) {
+		dev_err(&client->dev, "probe: Failed to disable sleep\n");
+		goto out_exit;
+	}
+
+	if (pdata->use_power_off && !pm_power_off)
+		pm_power_off = max77663_power_off;
+
+	ret = mfd_add_devices(&client->dev, 0, pdata->sub_devices,
+			      pdata->num_subdevs, NULL, 0);
+	if (ret != 0) {
+		dev_err(&client->dev, "probe: Failed to add subdev: %d\n", ret);
+		goto out_exit;
+	}
+
+	return 0;
+
+out_exit:
+	max77663_debugfs_exit(chip);
+	max77663_gpio_exit(chip);
+	max77663_irq_exit(chip);
+	mutex_destroy(&chip->io_lock);
+	max77663_chip = NULL;
+	kfree(chip);
+out:
+	return ret;
+}
+
+static int __devexit max77663_remove(struct i2c_client *client)
+{
+	struct max77663_chip *chip = i2c_get_clientdata(client);
+
+	mfd_remove_devices(chip->dev);
+	max77663_debugfs_exit(chip);
+	max77663_irq_exit(chip);
+	max77663_gpio_exit(chip);
+	mutex_destroy(&chip->io_lock);
+	max77663_chip = NULL;
+	kfree(chip);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int max77663_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct max77663_chip *chip = i2c_get_clientdata(client);
+	int ret;
+
+	if (client->irq)
+		disable_irq(client->irq);
+
+	ret = max77663_sleep(chip, true);
+	if (ret < 0)
+		dev_err(dev, "suspend: Failed to enable sleep\n");
+
+	return ret;
+}
+
+static int max77663_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct max77663_chip *chip = i2c_get_clientdata(client);
+	int ret;
+
+	ret = max77663_sleep(chip, false);
+	if (ret < 0) {
+		dev_err(dev, "resume: Failed to disable sleep\n");
+		return ret;
+	}
+
+	if (client->irq)
+		enable_irq(client->irq);
+
+	return 0;
+}
+#else
+#define max77663_suspend      NULL
+#define max77663_resume       NULL
+#endif /* CONFIG_PM */
+
+static const struct i2c_device_id max77663_id[] = {
+	{"max77663", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, max77663_id);
+
+static const struct dev_pm_ops max77663_pm = {
+	.suspend = max77663_suspend,
+	.resume = max77663_resume,
+};
+
+static struct i2c_driver max77663_driver = {
+	.driver = {
+		.name = "max77663",
+		.owner = THIS_MODULE,
+		.pm = &max77663_pm,
+	},
+	.probe = max77663_probe,
+	.remove = __devexit_p(max77663_remove),
+	.id_table = max77663_id,
+};
+
+static int __init max77663_init(void)
+{
+	return i2c_add_driver(&max77663_driver);
+}
+subsys_initcall(max77663_init);
+
+static void __exit max77663_exit(void)
+{
+	i2c_del_driver(&max77663_driver);
+}
+module_exit(max77663_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("MAX77663 Multi Function Device Core Driver");
+MODULE_VERSION("1.0");
diff --git a/drivers/mfd/max77665.c b/drivers/mfd/max77665.c
new file mode 100644
index 000000000000..b66712ba5737
--- /dev/null
+++ b/drivers/mfd/max77665.c
@@ -0,0 +1,371 @@
+/*
+ * Core driver for MAXIM MAX77665
+ *
+ * Copyright (c) 2012, NVIDIA 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/max77665.h>
+#include <linux/slab.h>
+
+#define MAX77665_INT_STS	0x22
+#define MAX77665_INT_MSK	0x23
+#define MAX77665_PMIC_FLASH	0x00 ... 0x10
+#define MAX77665_PMIC_PMIC	0x20 ... 0x2D
+#define MAX77665_PMIC_CHARGER	0xB0 ... 0xC6
+#define MAX77665_MUIC		0x00 ... 0x0E
+#define MAX77665_HAPTIC		0x00 ... 0x10
+
+static u8 max77665_i2c_slave_address[] = {
+	[MAX77665_I2C_SLAVE_PMIC] = 0x66,
+	[MAX77665_I2C_SLAVE_MUIC] = 0x25,
+	[MAX77665_I2C_SLAVE_HAPTIC] = 0x48,
+};
+
+struct max77665_irq_data {
+	int bit;
+};
+
+#define MAX77665_IRQ(_id, _bit_pos)				\
+	[MAX77665_IRQ_##_id] =	{				\
+					.bit	= (_bit_pos),	\
+				}
+
+static const struct max77665_irq_data max77665_irqs[] = {
+	MAX77665_IRQ(CHARGER, 0),
+	MAX77665_IRQ(TOP_SYS, 1),
+	MAX77665_IRQ(FLASH, 2),
+	MAX77665_IRQ(MUIC, 3),
+};
+
+static struct mfd_cell max77665s[] = {
+	{.name = "max77665-charger",},
+	{.name = "max77665-flash",},
+	{.name = "max77665-muic",},
+	{.name = "max77665-haptic",},
+};
+
+static void max77665_irq_lock(struct irq_data *data)
+{
+	struct max77665 *max77665 = irq_data_get_irq_chip_data(data);
+
+	mutex_lock(&max77665->irq_lock);
+}
+
+static void max77665_irq_mask(struct irq_data *irq_data)
+{
+	struct max77665 *max77665 = irq_data_get_irq_chip_data(irq_data);
+	unsigned int __irq = irq_data->irq - max77665->irq_base;
+	const struct max77665_irq_data *data = &max77665_irqs[__irq];
+	int ret;
+
+	ret = max77665_set_bits(max77665->dev, MAX77665_I2C_SLAVE_PMIC,
+				MAX77665_INT_MSK, data->bit);
+	if (ret < 0)
+		dev_err(max77665->dev,
+			"Clearing mask reg failed e = %d\n", ret);
+}
+
+static void max77665_irq_unmask(struct irq_data *irq_data)
+{
+	struct max77665 *max77665 = irq_data_get_irq_chip_data(irq_data);
+	unsigned int __irq = irq_data->irq - max77665->irq_base;
+	const struct max77665_irq_data *data = &max77665_irqs[__irq];
+	int ret;
+
+	ret = max77665_clr_bits(max77665->dev, MAX77665_I2C_SLAVE_PMIC,
+				MAX77665_INT_MSK, data->bit);
+	if (ret < 0)
+		dev_err(max77665->dev,
+			"Setting mask reg failed e = %d\n", ret);
+}
+
+static void max77665_irq_sync_unlock(struct irq_data *data)
+{
+	struct max77665 *max77665 = irq_data_get_irq_chip_data(data);
+
+	mutex_unlock(&max77665->irq_lock);
+}
+
+static irqreturn_t max77665_irq(int irq, void *data)
+{
+	struct max77665 *max77665 = data;
+	int ret = 0;
+	u8 status = 0;
+	unsigned long int acks = 0;
+	int i;
+
+	ret = max77665_read(max77665->dev, MAX77665_I2C_SLAVE_PMIC,
+					MAX77665_INT_STS, &status);
+	if (ret < 0) {
+		dev_err(max77665->dev,
+				"failed to read status regi, e %d\n", ret);
+		return IRQ_NONE;
+	}
+	acks = status;
+	for_each_set_bit(i, &acks, ARRAY_SIZE(max77665_irqs))
+		handle_nested_irq(max77665->irq_base + i);
+	return acks ? IRQ_HANDLED : IRQ_NONE;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int max77665_irq_set_wake(struct irq_data *data, unsigned int enable)
+{
+	struct max77665 *max77665 = irq_data_get_irq_chip_data(data);
+
+	return irq_set_irq_wake(max77665->irq_base, enable);
+}
+
+#else
+#define max77665_irq_set_wake NULL
+#endif
+
+static int __devinit max77665_irq_init(struct max77665 *max77665, int irq,
+	int irq_base)
+{
+	int i, ret;
+
+	if (irq_base <= 0) {
+		dev_err(max77665->dev, "IRQ base not set, int not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_init(&max77665->irq_lock);
+
+	ret = max77665_write(max77665->dev, MAX77665_I2C_SLAVE_PMIC,
+					MAX77665_INT_MSK, 0xFF);
+	if (ret < 0) {
+		dev_err(max77665->dev,
+			"Int mask reg write failed, e %d\n", ret);
+		return ret;
+	}
+
+	max77665->irq_base = irq_base;
+	max77665->irq_chip.name = "max77665";
+	max77665->irq_chip.irq_mask = max77665_irq_mask;
+	max77665->irq_chip.irq_unmask = max77665_irq_unmask;
+	max77665->irq_chip.irq_bus_lock = max77665_irq_lock;
+	max77665->irq_chip.irq_bus_sync_unlock = max77665_irq_sync_unlock;
+	max77665->irq_chip.irq_set_wake = max77665_irq_set_wake;
+
+	for (i = 0; i < ARRAY_SIZE(max77665_irqs); i++) {
+		int __irq = i + max77665->irq_base;
+		irq_set_chip_data(__irq, max77665);
+		irq_set_chip_and_handler(__irq, &max77665->irq_chip,
+					 handle_simple_irq);
+		irq_set_nested_thread(__irq, 1);
+#ifdef CONFIG_ARM
+		set_irq_flags(__irq, IRQF_VALID);
+#endif
+	}
+
+	ret = request_threaded_irq(irq, NULL, max77665_irq, IRQF_ONESHOT,
+				"max77665", max77665);
+	if (ret < 0) {
+		dev_err(max77665->dev, "Int registration failed, e %d\n", ret);
+		return ret;
+	}
+
+	device_init_wakeup(max77665->dev, 1);
+	return ret;
+}
+
+static bool rd_wr_reg_pmic(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case MAX77665_PMIC_FLASH:
+	case MAX77665_PMIC_PMIC:
+	case MAX77665_PMIC_CHARGER:
+		return true;
+	default:
+		dev_err(dev, "non-existing reg %s() reg 0x%x\n", __func__, reg);
+		return false;
+	}
+}
+
+static bool rd_wr_reg_muic(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case MAX77665_MUIC:
+		return true;
+	default:
+		dev_err(dev, "non-existing reg %s() reg 0x%x\n", __func__, reg);
+		return false;
+	}
+}
+
+static bool rd_wr_reg_haptic(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case MAX77665_HAPTIC:
+		return true;
+	default:
+		dev_err(dev, "non-existing reg %s() reg 0x%x\n", __func__, reg);
+		return false;
+	}
+}
+
+static const struct regmap_config max77665_regmap_config[] = {
+	{
+		.reg_bits = 8,
+		.val_bits = 8,
+		.max_register = 0xFF,
+		.writeable_reg = rd_wr_reg_pmic,
+		.readable_reg = rd_wr_reg_pmic,
+		.cache_type = REGCACHE_RBTREE,
+	}, {
+		.reg_bits = 8,
+		.val_bits = 8,
+		.max_register = 0x0E,
+		.writeable_reg = rd_wr_reg_muic,
+		.readable_reg = rd_wr_reg_muic,
+		.cache_type = REGCACHE_RBTREE,
+	}, {
+		.reg_bits = 8,
+		.val_bits = 8,
+		.max_register = 0x10,
+		.writeable_reg = rd_wr_reg_haptic,
+		.readable_reg = rd_wr_reg_haptic,
+		.cache_type = REGCACHE_RBTREE,
+	},
+};
+
+static int __devinit max77665_i2c_probe(struct i2c_client *client,
+					const struct i2c_device_id *id)
+{
+	struct max77665_platform_data *pdata = client->dev.platform_data;
+	struct max77665 *max77665;
+	struct i2c_client *slv_client;
+	int ret;
+	int i;
+
+	if (!pdata) {
+		dev_err(&client->dev, "max77665 requires platform data\n");
+		return -EINVAL;
+	}
+
+	max77665 = devm_kzalloc(&client->dev, sizeof(*max77665), GFP_KERNEL);
+	if (!max77665) {
+		dev_err(&client->dev, "mem alloc for max77665 failed\n");
+		return -ENOMEM;
+	}
+
+	max77665->dev = &client->dev;
+
+	for (i = 0; i < MAX77665_I2C_SLAVE_MAX; ++i) {
+		slv_client = max77665->client[i];
+		if (i == 0)
+			slv_client = client;
+		else
+			slv_client = i2c_new_dummy(client->adapter,
+						max77665_i2c_slave_address[i]);
+		if (!slv_client) {
+			dev_err(&client->dev, "can't attach client %d\n", i);
+			ret = -ENOMEM;
+			goto err_exit;
+		}
+		i2c_set_clientdata(slv_client, max77665);
+
+		max77665->regmap[i] = devm_regmap_init_i2c(slv_client,
+					&max77665_regmap_config[i]);
+		if (IS_ERR(max77665->regmap[i])) {
+			ret = PTR_ERR(max77665->regmap[i]);
+			dev_err(&client->dev,
+				"regmap %d init failed with err: %d\n",	i, ret);
+			goto err_exit;
+		}
+	}
+
+	if (client->irq > 0)
+		max77665_irq_init(max77665, client->irq, pdata->irq_base);
+
+	ret = mfd_add_devices(max77665->dev, -1, max77665s,
+		ARRAY_SIZE(max77665s), NULL, 0);
+	if (ret) {
+		dev_err(&client->dev, "add mfd devices failed with err: %d\n",
+			ret);
+		goto err_irq_exit;
+	}
+
+	return 0;
+
+err_irq_exit:
+	if (client->irq > 0)
+		free_irq(client->irq, max77665);
+err_exit:
+	for (i = 0; i < MAX77665_I2C_SLAVE_MAX; ++i) {
+		slv_client = max77665->client[i];
+		if (slv_client && slv_client != client)
+			i2c_unregister_device(slv_client);
+	}
+	return ret;
+}
+
+static int __devexit max77665_i2c_remove(struct i2c_client *client)
+{
+	struct max77665 *max77665 = i2c_get_clientdata(client);
+	int i;
+	struct i2c_client *slv_client;
+
+	mfd_remove_devices(max77665->dev);
+	if (client->irq > 0)
+		free_irq(client->irq, max77665);
+
+	for (i = 0; i < MAX77665_I2C_SLAVE_MAX; ++i) {
+		slv_client = max77665->client[i];
+		if (slv_client && slv_client != client)
+			i2c_unregister_device(slv_client);
+	}
+
+	return 0;
+}
+
+static const struct i2c_device_id max77665_id_table[] = {
+	{ "max77665", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, max77665_id_table);
+
+static struct i2c_driver max77665_driver = {
+	.driver	= {
+		.name	= "max77665",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= max77665_i2c_probe,
+	.remove		= __devexit_p(max77665_i2c_remove),
+	.id_table	= max77665_id_table,
+};
+
+static int __init max77665_init(void)
+{
+	return i2c_add_driver(&max77665_driver);
+}
+subsys_initcall(max77665_init);
+
+static void __exit max77665_exit(void)
+{
+	i2c_del_driver(&max77665_driver);
+}
+module_exit(max77665_exit);
+
+MODULE_DESCRIPTION("MAXIM MAX77665 core driver");
+MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/max8907c-irq.c b/drivers/mfd/max8907c-irq.c
new file mode 100644
index 000000000000..8d6e9600e7fe
--- /dev/null
+++ b/drivers/mfd/max8907c-irq.c
@@ -0,0 +1,425 @@
+/*
+ * Battery driver for Maxim MAX8907C
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ * Based on driver/mfd/max8925-core.c, Copyright (C) 2009-2010 Marvell International Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/max8907c.h>
+
+struct max8907c_irq_data {
+	int	reg;
+	int	mask_reg;
+	int	enable;		/* enable or not */
+	int	offs;		/* bit offset in mask register */
+	bool	is_rtc;
+	int	wake;
+};
+
+static struct max8907c_irq_data max8907c_irqs[] = {
+	[MAX8907C_IRQ_VCHG_DC_OVP] = {
+		.reg		= MAX8907C_REG_CHG_IRQ1,
+		.mask_reg	= MAX8907C_REG_CHG_IRQ1_MASK,
+		.offs		= 1 << 0,
+	},
+	[MAX8907C_IRQ_VCHG_DC_F] = {
+		.reg		= MAX8907C_REG_CHG_IRQ1,
+		.mask_reg	= MAX8907C_REG_CHG_IRQ1_MASK,
+		.offs		= 1 << 1,
+	},
+	[MAX8907C_IRQ_VCHG_DC_R] = {
+		.reg		= MAX8907C_REG_CHG_IRQ1,
+		.mask_reg	= MAX8907C_REG_CHG_IRQ1_MASK,
+		.offs		= 1 << 2,
+	},
+	[MAX8907C_IRQ_VCHG_THM_OK_R] = {
+		.reg		= MAX8907C_REG_CHG_IRQ2,
+		.mask_reg	= MAX8907C_REG_CHG_IRQ2_MASK,
+		.offs		= 1 << 0,
+	},
+	[MAX8907C_IRQ_VCHG_THM_OK_F] = {
+		.reg		= MAX8907C_REG_CHG_IRQ2,
+		.mask_reg	= MAX8907C_REG_CHG_IRQ2_MASK,
+		.offs		= 1 << 1,
+	},
+	[MAX8907C_IRQ_VCHG_MBATTLOW_F] = {
+		.reg		= MAX8907C_REG_CHG_IRQ2,
+		.mask_reg	= MAX8907C_REG_CHG_IRQ2_MASK,
+		.offs		= 1 << 2,
+	},
+	[MAX8907C_IRQ_VCHG_MBATTLOW_R] = {
+		.reg		= MAX8907C_REG_CHG_IRQ2,
+		.mask_reg	= MAX8907C_REG_CHG_IRQ2_MASK,
+		.offs		= 1 << 3,
+	},
+	[MAX8907C_IRQ_VCHG_RST] = {
+		.reg		= MAX8907C_REG_CHG_IRQ2,
+		.mask_reg	= MAX8907C_REG_CHG_IRQ2_MASK,
+		.offs		= 1 << 4,
+	},
+	[MAX8907C_IRQ_VCHG_DONE] = {
+		.reg		= MAX8907C_REG_CHG_IRQ2,
+		.mask_reg	= MAX8907C_REG_CHG_IRQ2_MASK,
+		.offs		= 1 << 5,
+	},
+	[MAX8907C_IRQ_VCHG_TOPOFF] = {
+		.reg		= MAX8907C_REG_CHG_IRQ2,
+		.mask_reg	= MAX8907C_REG_CHG_IRQ2_MASK,
+		.offs		= 1 << 6,
+	},
+	[MAX8907C_IRQ_VCHG_TMR_FAULT] = {
+		.reg		= MAX8907C_REG_CHG_IRQ2,
+		.mask_reg	= MAX8907C_REG_CHG_IRQ2_MASK,
+		.offs		= 1 << 7,
+	},
+	[MAX8907C_IRQ_GPM_RSTIN] = {
+		.reg		= MAX8907C_REG_ON_OFF_IRQ1,
+		.mask_reg	= MAX8907C_REG_ON_OFF_IRQ1_MASK,
+		.offs		= 1 << 0,
+	},
+	[MAX8907C_IRQ_GPM_MPL] = {
+		.reg		= MAX8907C_REG_ON_OFF_IRQ1,
+		.mask_reg	= MAX8907C_REG_ON_OFF_IRQ1_MASK,
+		.offs		= 1 << 1,
+	},
+	[MAX8907C_IRQ_GPM_SW_3SEC] = {
+		.reg		= MAX8907C_REG_ON_OFF_IRQ1,
+		.mask_reg	= MAX8907C_REG_ON_OFF_IRQ1_MASK,
+		.offs		= 1 << 2,
+	},
+	[MAX8907C_IRQ_GPM_EXTON_F] = {
+		.reg		= MAX8907C_REG_ON_OFF_IRQ1,
+		.mask_reg	= MAX8907C_REG_ON_OFF_IRQ1_MASK,
+		.offs		= 1 << 3,
+	},
+	[MAX8907C_IRQ_GPM_EXTON_R] = {
+		.reg		= MAX8907C_REG_ON_OFF_IRQ1,
+		.mask_reg	= MAX8907C_REG_ON_OFF_IRQ1_MASK,
+		.offs		= 1 << 4,
+	},
+	[MAX8907C_IRQ_GPM_SW_1SEC] = {
+		.reg		= MAX8907C_REG_ON_OFF_IRQ1,
+		.mask_reg	= MAX8907C_REG_ON_OFF_IRQ1_MASK,
+		.offs		= 1 << 5,
+	},
+	[MAX8907C_IRQ_GPM_SW_F] = {
+		.reg		= MAX8907C_REG_ON_OFF_IRQ1,
+		.mask_reg	= MAX8907C_REG_ON_OFF_IRQ1_MASK,
+		.offs		= 1 << 6,
+	},
+	[MAX8907C_IRQ_GPM_SW_R] = {
+		.reg		= MAX8907C_REG_ON_OFF_IRQ1,
+		.mask_reg	= MAX8907C_REG_ON_OFF_IRQ1_MASK,
+		.offs		= 1 << 7,
+	},
+	[MAX8907C_IRQ_GPM_SYSCKEN_F] = {
+		.reg		= MAX8907C_REG_ON_OFF_IRQ2,
+		.mask_reg	= MAX8907C_REG_ON_OFF_IRQ2_MASK,
+		.offs		= 1 << 0,
+	},
+	[MAX8907C_IRQ_GPM_SYSCKEN_R] = {
+		.reg		= MAX8907C_REG_ON_OFF_IRQ2,
+		.mask_reg	= MAX8907C_REG_ON_OFF_IRQ2_MASK,
+		.offs		= 1 << 1,
+	},
+	[MAX8907C_IRQ_RTC_ALARM1] = {
+		.reg		= MAX8907C_REG_RTC_IRQ,
+		.mask_reg	= MAX8907C_REG_RTC_IRQ_MASK,
+		.offs		= 1 << 2,
+		.is_rtc		= true,
+	},
+	[MAX8907C_IRQ_RTC_ALARM0] = {
+		.reg		= MAX8907C_REG_RTC_IRQ,
+		.mask_reg	= MAX8907C_REG_RTC_IRQ_MASK,
+		.offs		= 1 << 3,
+		.is_rtc		= true,
+	},
+};
+
+static inline struct max8907c_irq_data *irq_to_max8907c(struct max8907c *chip,
+						      int irq)
+{
+	return &max8907c_irqs[irq - chip->irq_base];
+}
+
+static irqreturn_t max8907c_irq(int irq, void *data)
+{
+	struct max8907c *chip = data;
+	struct max8907c_irq_data *irq_data;
+	struct i2c_client *i2c;
+	int read_reg = -1, value = 0;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(max8907c_irqs); i++) {
+		irq_data = &max8907c_irqs[i];
+
+		if (irq_data->is_rtc)
+			i2c = chip->i2c_rtc;
+		else
+			i2c = chip->i2c_power;
+
+		if (read_reg != irq_data->reg) {
+			read_reg = irq_data->reg;
+			value = max8907c_reg_read(i2c, irq_data->reg);
+		}
+
+		if (value & irq_data->enable)
+			handle_nested_irq(chip->irq_base + i);
+	}
+	return IRQ_HANDLED;
+}
+
+static void max8907c_irq_lock(struct irq_data *data)
+{
+	struct max8907c *chip = irq_data_get_irq_chip_data(data);
+
+	mutex_lock(&chip->irq_lock);
+}
+
+static void max8907c_irq_sync_unlock(struct irq_data *data)
+{
+	struct max8907c *chip = irq_data_get_irq_chip_data(data);
+	struct max8907c_irq_data *irq_data;
+	unsigned char irq_chg[2], irq_on[2];
+	unsigned char irq_rtc;
+	int i;
+
+	irq_chg[0] = irq_chg[1] = irq_on[0] = irq_on[1] = irq_rtc = 0xFF;
+
+	for (i = 0; i < ARRAY_SIZE(max8907c_irqs); i++) {
+		irq_data = &max8907c_irqs[i];
+		/* 1 -- disable, 0 -- enable */
+		switch (irq_data->mask_reg) {
+		case MAX8907C_REG_CHG_IRQ1_MASK:
+			irq_chg[0] &= ~irq_data->enable;
+			break;
+		case MAX8907C_REG_CHG_IRQ2_MASK:
+			irq_chg[1] &= ~irq_data->enable;
+			break;
+		case MAX8907C_REG_ON_OFF_IRQ1_MASK:
+			irq_on[0] &= ~irq_data->enable;
+			break;
+		case MAX8907C_REG_ON_OFF_IRQ2_MASK:
+			irq_on[1] &= ~irq_data->enable;
+			break;
+		case MAX8907C_REG_RTC_IRQ_MASK:
+			irq_rtc &= ~irq_data->enable;
+			break;
+		default:
+			dev_err(chip->dev, "wrong IRQ\n");
+			break;
+		}
+	}
+	/* update mask into registers */
+	if (chip->cache_chg[0] != irq_chg[0]) {
+		chip->cache_chg[0] = irq_chg[0];
+		max8907c_reg_write(chip->i2c_power, MAX8907C_REG_CHG_IRQ1_MASK,
+			irq_chg[0]);
+	}
+	if (chip->cache_chg[1] != irq_chg[1]) {
+		chip->cache_chg[1] = irq_chg[1];
+		max8907c_reg_write(chip->i2c_power, MAX8907C_REG_CHG_IRQ2_MASK,
+			irq_chg[1]);
+	}
+	if (chip->cache_on[0] != irq_on[0]) {
+		chip->cache_on[0] = irq_on[0];
+		max8907c_reg_write(chip->i2c_power, MAX8907C_REG_ON_OFF_IRQ1_MASK,
+				irq_on[0]);
+	}
+	if (chip->cache_on[1] != irq_on[1]) {
+		chip->cache_on[1] = irq_on[1];
+		max8907c_reg_write(chip->i2c_power, MAX8907C_REG_ON_OFF_IRQ2_MASK,
+				irq_on[1]);
+	}
+	if (chip->cache_rtc != irq_rtc) {
+		chip->cache_rtc = irq_rtc;
+		max8907c_reg_write(chip->i2c_rtc, MAX8907C_REG_RTC_IRQ_MASK,
+				   irq_rtc);
+	}
+
+	mutex_unlock(&chip->irq_lock);
+}
+
+static void max8907c_irq_enable(struct irq_data *data)
+{
+	struct max8907c *chip = irq_data_get_irq_chip_data(data);
+	max8907c_irqs[data->irq - chip->irq_base].enable
+		= max8907c_irqs[data->irq - chip->irq_base].offs;
+}
+
+static void max8907c_irq_disable(struct irq_data *data)
+{
+	struct max8907c *chip = irq_data_get_irq_chip_data(data);
+	max8907c_irqs[data->irq - chip->irq_base].enable = 0;
+}
+
+static int max8907c_irq_set_wake(struct irq_data *data, unsigned int on)
+{
+	struct max8907c *chip = irq_data_get_irq_chip_data(data);
+	if (on) {
+		max8907c_irqs[data->irq - chip->irq_base].wake
+			= max8907c_irqs[data->irq - chip->irq_base].enable;
+	} else {
+		max8907c_irqs[data->irq - chip->irq_base].wake = 0;
+	}
+	return 0;
+}
+
+static struct irq_chip max8907c_irq_chip = {
+	.name			= "max8907c",
+	.irq_bus_lock		= max8907c_irq_lock,
+	.irq_bus_sync_unlock	= max8907c_irq_sync_unlock,
+	.irq_enable		= max8907c_irq_enable,
+	.irq_disable		= max8907c_irq_disable,
+	.irq_set_wake		= max8907c_irq_set_wake,
+};
+
+int max8907c_irq_init(struct max8907c *chip, int irq, int irq_base)
+{
+	unsigned long flags = IRQF_ONESHOT;
+	struct irq_desc *desc;
+	int i, ret;
+	int __irq;
+
+	if (!irq_base || !irq) {
+		dev_warn(chip->dev, "No interrupt support\n");
+		return -EINVAL;
+	}
+	/* clear all interrupts */
+	max8907c_reg_read(chip->i2c_power, MAX8907C_REG_CHG_IRQ1);
+	max8907c_reg_read(chip->i2c_power, MAX8907C_REG_CHG_IRQ2);
+	max8907c_reg_read(chip->i2c_power, MAX8907C_REG_ON_OFF_IRQ1);
+	max8907c_reg_read(chip->i2c_power, MAX8907C_REG_ON_OFF_IRQ2);
+	max8907c_reg_read(chip->i2c_rtc, MAX8907C_REG_RTC_IRQ);
+	/* mask all interrupts */
+	max8907c_reg_write(chip->i2c_rtc, MAX8907C_REG_ALARM0_CNTL, 0);
+	max8907c_reg_write(chip->i2c_rtc, MAX8907C_REG_ALARM1_CNTL, 0);
+	max8907c_reg_write(chip->i2c_power, MAX8907C_REG_CHG_IRQ1_MASK, 0xff);
+	max8907c_reg_write(chip->i2c_power, MAX8907C_REG_CHG_IRQ2_MASK, 0xff);
+	max8907c_reg_write(chip->i2c_power, MAX8907C_REG_ON_OFF_IRQ1_MASK, 0xff);
+	max8907c_reg_write(chip->i2c_power, MAX8907C_REG_ON_OFF_IRQ2_MASK, 0xff);
+	max8907c_reg_write(chip->i2c_rtc, MAX8907C_REG_RTC_IRQ_MASK, 0xff);
+
+	chip->cache_chg[0] = chip->cache_chg[1] =
+		chip->cache_on[0] = chip->cache_on[1] =
+		chip->cache_rtc = 0xFF;
+
+	mutex_init(&chip->irq_lock);
+	chip->core_irq = irq;
+	chip->irq_base = irq_base;
+	desc = irq_to_desc(chip->core_irq);
+
+	/* register with genirq */
+	for (i = 0; i < ARRAY_SIZE(max8907c_irqs); i++) {
+		__irq = i + chip->irq_base;
+		irq_set_chip_data(__irq, chip);
+		irq_set_chip_and_handler(__irq, &max8907c_irq_chip,
+					 handle_edge_irq);
+		irq_set_nested_thread(__irq, 1);
+#ifdef CONFIG_ARM
+		/* ARM requires an extra step to clear IRQ_NOREQUEST, which it
+		 * sets on behalf of every irq_chip.
+		 */
+		set_irq_flags(__irq, IRQF_VALID);
+#else
+		irq_set_noprobe(__irq);
+#endif
+	}
+
+	ret = request_threaded_irq(irq, NULL, max8907c_irq, flags,
+				   "max8907c", chip);
+	if (ret) {
+		dev_err(chip->dev, "Failed to request core IRQ: %d\n", ret);
+		chip->core_irq = 0;
+	}
+
+	device_init_wakeup(chip->dev, 1);
+
+	return ret;
+}
+
+int max8907c_suspend(struct i2c_client *i2c, pm_message_t state)
+{
+	struct max8907c *chip = i2c_get_clientdata(i2c);
+
+	struct max8907c_irq_data *irq_data;
+	unsigned char irq_chg[2], irq_on[2];
+	unsigned char irq_rtc;
+	int i;
+
+	irq_chg[0] = irq_chg[1] = irq_on[0] = irq_on[1] = irq_rtc = 0xFF;
+
+	for (i = 0; i < ARRAY_SIZE(max8907c_irqs); i++) {
+		irq_data = &max8907c_irqs[i];
+		/* 1 -- disable, 0 -- enable */
+		switch (irq_data->mask_reg) {
+		case MAX8907C_REG_CHG_IRQ1_MASK:
+			irq_chg[0] &= ~irq_data->wake;
+			break;
+		case MAX8907C_REG_CHG_IRQ2_MASK:
+			irq_chg[1] &= ~irq_data->wake;
+			break;
+		case MAX8907C_REG_ON_OFF_IRQ1_MASK:
+			irq_on[0] &= ~irq_data->wake;
+			break;
+		case MAX8907C_REG_ON_OFF_IRQ2_MASK:
+			irq_on[1] &= ~irq_data->wake;
+			break;
+		case MAX8907C_REG_RTC_IRQ_MASK:
+			irq_rtc &= ~irq_data->wake;
+			break;
+		default:
+			dev_err(chip->dev, "wrong IRQ\n");
+			break;
+		}
+	}
+
+	max8907c_reg_write(chip->i2c_power, MAX8907C_REG_CHG_IRQ1_MASK, irq_chg[0]);
+	max8907c_reg_write(chip->i2c_power, MAX8907C_REG_CHG_IRQ2_MASK, irq_chg[1]);
+	max8907c_reg_write(chip->i2c_power, MAX8907C_REG_ON_OFF_IRQ1_MASK, irq_on[0]);
+	max8907c_reg_write(chip->i2c_power, MAX8907C_REG_ON_OFF_IRQ2_MASK, irq_on[1]);
+	max8907c_reg_write(chip->i2c_rtc, MAX8907C_REG_RTC_IRQ_MASK, irq_rtc);
+
+	if (device_may_wakeup(chip->dev))
+		enable_irq_wake(chip->core_irq);
+	else
+		disable_irq(chip->core_irq);
+
+	return 0;
+}
+
+int max8907c_resume(struct i2c_client *i2c)
+{
+	struct max8907c *chip = i2c_get_clientdata(i2c);
+
+	if (device_may_wakeup(chip->dev))
+		disable_irq_wake(chip->core_irq);
+	else
+		enable_irq(chip->core_irq);
+
+	max8907c_reg_write(chip->i2c_power, MAX8907C_REG_CHG_IRQ1_MASK, chip->cache_chg[0]);
+	max8907c_reg_write(chip->i2c_power, MAX8907C_REG_CHG_IRQ2_MASK, chip->cache_chg[1]);
+	max8907c_reg_write(chip->i2c_power, MAX8907C_REG_ON_OFF_IRQ1_MASK, chip->cache_on[0]);
+	max8907c_reg_write(chip->i2c_power, MAX8907C_REG_ON_OFF_IRQ2_MASK, chip->cache_on[1]);
+	max8907c_reg_write(chip->i2c_rtc, MAX8907C_REG_RTC_IRQ_MASK, chip->cache_rtc);
+
+	return 0;
+}
+
+void max8907c_irq_free(struct max8907c *chip)
+{
+	if (chip->core_irq)
+		free_irq(chip->core_irq, chip);
+}
+
diff --git a/drivers/mfd/max8907c.c b/drivers/mfd/max8907c.c
new file mode 100644
index 000000000000..f8964cb92965
--- /dev/null
+++ b/drivers/mfd/max8907c.c
@@ -0,0 +1,389 @@
+/*
+ * max8907c.c - mfd driver for MAX8907c
+ *
+ * Copyright (C) 2010 Gyungoh Yoo <jack.yoo@maxim-ic.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/max8907c.h>
+
+static struct mfd_cell cells[] = {
+	{.name = "max8907-regulator",},
+	{.name = "max8907c-rtc",},
+};
+
+static int max8907c_i2c_read(struct i2c_client *i2c, u8 reg, u8 count, u8 *dest)
+{
+	struct i2c_msg xfer[2];
+	int ret = 0;
+
+	xfer[0].addr = i2c->addr;
+	xfer[0].flags = 0;
+	xfer[0].len = 1;
+	xfer[0].buf = &reg;
+
+	xfer[1].addr = i2c->addr;
+	xfer[1].flags = I2C_M_RD;
+	xfer[1].len = count;
+	xfer[1].buf = dest;
+
+	ret = i2c_transfer(i2c->adapter, xfer, 2);
+	if (ret < 0)
+		return ret;
+	if (ret != 2)
+		return -EIO;
+
+	return 0;
+}
+
+static int max8907c_i2c_write(struct i2c_client *i2c, u8 reg, u8 count, const u8 *src)
+{
+	u8 msg[0x100 + 1];
+	int ret = 0;
+
+	msg[0] = reg;
+	memcpy(&msg[1], src, count);
+
+	ret = i2c_master_send(i2c, msg, count + 1);
+	if (ret < 0)
+		return ret;
+	if (ret != count + 1)
+		return -EIO;
+
+	return 0;
+}
+
+int max8907c_reg_read(struct i2c_client *i2c, u8 reg)
+{
+	int ret;
+	u8 val;
+
+	ret = max8907c_i2c_read(i2c, reg, 1, &val);
+
+	pr_debug("max8907c: reg read  reg=%x, val=%x\n",
+		 (unsigned int)reg, (unsigned int)val);
+
+	if (ret != 0)
+		pr_err("Failed to read max8907c I2C driver: %d\n", ret);
+	return val;
+}
+EXPORT_SYMBOL_GPL(max8907c_reg_read);
+
+int max8907c_reg_bulk_read(struct i2c_client *i2c, u8 reg, u8 count, u8 *val)
+{
+	int ret;
+
+	ret = max8907c_i2c_read(i2c, reg, count, val);
+
+	pr_debug("max8907c: reg read  reg=%x, val=%x\n",
+		 (unsigned int)reg, (unsigned int)*val);
+
+	if (ret != 0)
+		pr_err("Failed to read max8907c I2C driver: %d\n", ret);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(max8907c_reg_bulk_read);
+
+int max8907c_reg_write(struct i2c_client *i2c, u8 reg, u8 val)
+{
+	struct max8907c *max8907c = i2c_get_clientdata(i2c);
+	int ret;
+
+	pr_debug("max8907c: reg write  reg=%x, val=%x\n",
+		 (unsigned int)reg, (unsigned int)val);
+
+	mutex_lock(&max8907c->io_lock);
+	ret = max8907c_i2c_write(i2c, reg, 1, &val);
+	mutex_unlock(&max8907c->io_lock);
+
+	if (ret != 0)
+		pr_err("Failed to write max8907c I2C driver: %d\n", ret);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(max8907c_reg_write);
+
+int max8907c_reg_bulk_write(struct i2c_client *i2c, u8 reg, u8 count, u8 *val)
+{
+	struct max8907c *max8907c = i2c_get_clientdata(i2c);
+	int ret;
+
+	pr_debug("max8907c: reg write  reg=%x, val=%x\n",
+		 (unsigned int)reg, (unsigned int)*val);
+
+	mutex_lock(&max8907c->io_lock);
+	ret = max8907c_i2c_write(i2c, reg, count, val);
+	mutex_unlock(&max8907c->io_lock);
+
+	if (ret != 0)
+		pr_err("Failed to write max8907c I2C driver: %d\n", ret);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(max8907c_reg_bulk_write);
+
+int max8907c_set_bits(struct i2c_client *i2c, u8 reg, u8 mask, u8 val)
+{
+	struct max8907c *max8907c = i2c_get_clientdata(i2c);
+	u8 tmp;
+	int ret;
+
+	pr_debug("max8907c: reg write  reg=%02X, val=%02X, mask=%02X\n",
+		 (unsigned int)reg, (unsigned int)val, (unsigned int)mask);
+
+	mutex_lock(&max8907c->io_lock);
+	ret = max8907c_i2c_read(i2c, reg, 1, &tmp);
+	if (ret == 0) {
+		val = (tmp & ~mask) | (val & mask);
+		ret = max8907c_i2c_write(i2c, reg, 1, &val);
+	}
+	mutex_unlock(&max8907c->io_lock);
+
+	if (ret != 0)
+		pr_err("Failed to write max8907c I2C driver: %d\n", ret);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(max8907c_set_bits);
+
+static struct i2c_client *max8907c_client = NULL;
+static void max8907c_power_off(void)
+{
+	if (!max8907c_client)
+		return;
+
+	max8907c_set_bits(max8907c_client, MAX8907C_REG_RESET_CNFG,
+						MAX8907C_MASK_POWER_OFF, 0x40);
+}
+
+void max8907c_deep_sleep(int enter)
+{
+	if (!max8907c_client)
+		return;
+
+	if (enter) {
+		max8907c_reg_write(max8907c_client, MAX8907C_REG_SDSEQCNT1,
+						MAX8907C_POWER_UP_DELAY_CNT12);
+		max8907c_reg_write(max8907c_client, MAX8907C_REG_SDSEQCNT2,
+							MAX8907C_DELAY_CNT0);
+		max8907c_reg_write(max8907c_client, MAX8907C_REG_SDCTL2,
+							MAX8907C_SD_SEQ2);
+	} else {
+		max8907c_reg_write(max8907c_client, MAX8907C_REG_SDSEQCNT1,
+							MAX8907C_DELAY_CNT0);
+		max8907c_reg_write(max8907c_client, MAX8907C_REG_SDCTL2,
+							MAX8907C_SD_SEQ1);
+		max8907c_reg_write(max8907c_client, MAX8907C_REG_SDSEQCNT2,
+				MAX8907C_POWER_UP_DELAY_CNT1 | MAX8907C_POWER_DOWN_DELAY_CNT12);
+	}
+}
+
+static int max8907c_remove_subdev(struct device *dev, void *unused)
+{
+	platform_device_unregister(to_platform_device(dev));
+	return 0;
+}
+
+static int max8907c_remove_subdevs(struct max8907c *max8907c)
+{
+	return device_for_each_child(max8907c->dev, NULL,
+				     max8907c_remove_subdev);
+}
+
+static int max8097c_add_subdevs(struct max8907c *max8907c,
+				struct max8907c_platform_data *pdata)
+{
+	struct platform_device *pdev;
+	int ret;
+	int i;
+
+	for (i = 0; i < pdata->num_subdevs; i++) {
+		pdev = platform_device_alloc(pdata->subdevs[i]->name,
+					     pdata->subdevs[i]->id);
+
+		pdev->dev.parent = max8907c->dev;
+		pdev->dev.platform_data = pdata->subdevs[i]->dev.platform_data;
+
+		ret = platform_device_add(pdev);
+		if (ret)
+			goto error;
+	}
+	return 0;
+
+error:
+	max8907c_remove_subdevs(max8907c);
+	return ret;
+}
+
+int max8907c_pwr_en_config(void)
+{
+	int ret;
+	u8 data;
+
+	if (!max8907c_client)
+		return -EINVAL;
+
+	/*
+	 * Enable/disable PWREN h/w control mechanism (PWREN signal must be
+	 * inactive = high at this time)
+	 */
+	ret = max8907c_set_bits(max8907c_client, MAX8907C_REG_RESET_CNFG,
+					MAX8907C_MASK_PWR_EN, MAX8907C_PWR_EN);
+	if (ret != 0)
+		return ret;
+
+	/*
+	 * When enabled, connect PWREN to SEQ2 by clearing SEQ2 configuration
+	 * settings for silicon revision that requires s/w WAR. On other
+	 * MAX8907B revisions PWREN is always connected to SEQ2.
+	 */
+	data = max8907c_reg_read(max8907c_client, MAX8907C_REG_II2RR);
+
+	if (data == MAX8907B_II2RR_PWREN_WAR) {
+		data = 0x00;
+		ret = max8907c_reg_write(max8907c_client, MAX8907C_REG_SEQ2CNFG, data);
+	}
+	return ret;
+}
+
+int max8907c_pwr_en_attach(void)
+{
+	int ret;
+
+	if (!max8907c_client)
+		return -EINVAL;
+
+	/* No sequencer delay for CPU rail when it is attached */
+	ret = max8907c_reg_write(max8907c_client, MAX8907C_REG_SDSEQCNT1,
+							MAX8907C_DELAY_CNT0);
+	if (ret != 0)
+		return ret;
+
+	return max8907c_set_bits(max8907c_client, MAX8907C_REG_SDCTL1,
+					MAX8907C_MASK_CTL_SEQ, MAX8907C_CTL_SEQ);
+}
+
+static int max8907c_i2c_probe(struct i2c_client *i2c,
+			      const struct i2c_device_id *id)
+{
+	struct max8907c *max8907c;
+	struct max8907c_platform_data *pdata = i2c->dev.platform_data;
+	int ret;
+	int i;
+	u8 tmp;
+
+	max8907c = kzalloc(sizeof(struct max8907c), GFP_KERNEL);
+	if (max8907c == NULL)
+		return -ENOMEM;
+
+	max8907c->dev = &i2c->dev;
+	dev_set_drvdata(max8907c->dev, max8907c);
+
+	max8907c->i2c_power = i2c;
+	i2c_set_clientdata(i2c, max8907c);
+
+	max8907c->i2c_rtc = i2c_new_dummy(i2c->adapter, RTC_I2C_ADDR);
+	i2c_set_clientdata(max8907c->i2c_rtc, max8907c);
+
+	mutex_init(&max8907c->io_lock);
+
+	for (i = 0; i < ARRAY_SIZE(cells); i++) {
+		cells[i].platform_data = max8907c;
+		cells[i].pdata_size = sizeof(*max8907c);
+	}
+	ret = mfd_add_devices(max8907c->dev, -1, cells, ARRAY_SIZE(cells),
+			      NULL, 0);
+	if (ret != 0) {
+		i2c_unregister_device(max8907c->i2c_rtc);
+		kfree(max8907c);
+		pr_debug("max8907c: failed to add MFD devices   %X\n", ret);
+		return ret;
+	}
+
+	max8907c_client = i2c;
+
+	max8907c_irq_init(max8907c, i2c->irq, pdata->irq_base);
+
+	ret = max8097c_add_subdevs(max8907c, pdata);
+
+	if (pdata->use_power_off && !pm_power_off)
+		pm_power_off = max8907c_power_off;
+
+	ret = max8907c_i2c_read(i2c, MAX8907C_REG_SYSENSEL, 1, &tmp);
+	/*Mask HARD RESET, if enabled */
+	if (ret == 0) {
+		tmp &= ~(BIT(7));
+		ret = max8907c_i2c_write(i2c, MAX8907C_REG_SYSENSEL, 1, &tmp);
+	}
+
+	if (ret != 0) {
+		pr_err("Failed to write max8907c I2C driver: %d\n", ret);
+		return ret;
+	}
+
+	if (pdata->max8907c_setup)
+		return pdata->max8907c_setup();
+
+	return ret;
+}
+
+static int max8907c_i2c_remove(struct i2c_client *i2c)
+{
+	struct max8907c *max8907c = i2c_get_clientdata(i2c);
+
+	max8907c_remove_subdevs(max8907c);
+	i2c_unregister_device(max8907c->i2c_rtc);
+	mfd_remove_devices(max8907c->dev);
+	max8907c_irq_free(max8907c);
+	kfree(max8907c);
+
+	return 0;
+}
+
+static const struct i2c_device_id max8907c_i2c_id[] = {
+	{"max8907c", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, max8907c_i2c_id);
+
+static struct i2c_driver max8907c_i2c_driver = {
+	.driver = {
+		   .name = "max8907c",
+		   .owner = THIS_MODULE,
+		   },
+	.probe = max8907c_i2c_probe,
+	.remove = max8907c_i2c_remove,
+	.suspend = max8907c_suspend,
+	.resume = max8907c_resume,
+	.id_table = max8907c_i2c_id,
+};
+
+static int __init max8907c_i2c_init(void)
+{
+	int ret = -ENODEV;
+
+	ret = i2c_add_driver(&max8907c_i2c_driver);
+	if (ret != 0)
+		pr_err("Failed to register I2C driver: %d\n", ret);
+
+	return ret;
+}
+
+subsys_initcall(max8907c_i2c_init);
+
+static void __exit max8907c_i2c_exit(void)
+{
+	i2c_del_driver(&max8907c_i2c_driver);
+}
+
+module_exit(max8907c_i2c_exit);
+
+MODULE_DESCRIPTION("MAX8907C multi-function core driver");
+MODULE_AUTHOR("Gyungoh Yoo <jack.yoo@maxim-ic.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/palmas.c b/drivers/mfd/palmas.c
new file mode 100644
index 000000000000..00c0aba7eba0
--- /dev/null
+++ b/drivers/mfd/palmas.c
@@ -0,0 +1,509 @@
+/*
+ * TI Palmas MFD Driver
+ *
+ * Copyright 2011-2012 Texas Instruments Inc.
+ *
+ * Author: Graeme Gregory <gg@slimlogic.co.uk>
+ *
+ *  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.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/regmap.h>
+#include <linux/err.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/palmas.h>
+
+static const struct resource gpadc_resource[] = {
+	{
+		.name = "EOC_SW",
+		.start = PALMAS_GPADC_EOC_SW_IRQ,
+		.end = PALMAS_GPADC_EOC_SW_IRQ,
+		.flags = IORESOURCE_IRQ,
+	}
+};
+
+static const struct resource usb_resource[] = {
+	{
+		.name = "ID",
+		.start = PALMAS_ID_OTG_IRQ,
+		.end = PALMAS_ID_OTG_IRQ,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "ID_WAKEUP",
+		.start = PALMAS_ID_IRQ,
+		.end = PALMAS_ID_IRQ,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "VBUS",
+		.start = PALMAS_VBUS_OTG_IRQ,
+		.end = PALMAS_VBUS_OTG_IRQ,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.name = "VBUS_WAKEUP",
+		.start = PALMAS_VBUS_IRQ,
+		.end = PALMAS_VBUS_IRQ,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static const struct resource rtc_resource[] = {
+	{
+		.name = "RTC_ALARM",
+		.start = PALMAS_RTC_ALARM_IRQ,
+		.end = PALMAS_RTC_ALARM_IRQ,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static const struct resource pwron_resource[] = {
+	{
+		.name = "PWRON_BUTTON",
+		.start = PALMAS_PWRON_IRQ,
+		.end = PALMAS_PWRON_IRQ,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+enum palmas_ids {
+	PALMAS_PMIC_ID,
+	PALMAS_GPIO_ID,
+	PALMAS_LEDS_ID,
+	PALMAS_WDT_ID,
+	PALMAS_RTC_ID,
+	PALMAS_PWRBUTTON_ID,
+	PALMAS_GPADC_ID,
+	PALMAS_RESOURCE_ID,
+	PALMAS_CLK_ID,
+	PALMAS_PWM_ID,
+	PALMAS_USB_ID,
+};
+
+static const struct mfd_cell palmas_children[] = {
+	{
+		.name = "palmas-pmic",
+		.id = PALMAS_PMIC_ID,
+	},
+	{
+		.name = "palmas-gpio",
+		.id = PALMAS_GPIO_ID,
+	},
+	{
+		.name = "palmas-leds",
+		.id = PALMAS_LEDS_ID,
+	},
+	{
+		.name = "palmas-wdt",
+		.id = PALMAS_WDT_ID,
+	},
+	{
+		.name = "palmas-rtc",
+		.num_resources = ARRAY_SIZE(rtc_resource),
+		.resources = rtc_resource,
+		.id = PALMAS_RTC_ID,
+	},
+	{
+		.name = "palmas-pwrbutton",
+		.num_resources = ARRAY_SIZE(pwron_resource),
+		.resources = pwron_resource,
+		.id = PALMAS_PWRBUTTON_ID,
+	},
+	{
+		.name = "palmas-gpadc",
+		.num_resources = ARRAY_SIZE(gpadc_resource),
+		.resources = gpadc_resource,
+		.id = PALMAS_GPADC_ID,
+	},
+	{
+		.name = "palmas-resource",
+		.id = PALMAS_RESOURCE_ID,
+	},
+	{
+		.name = "palmas-clk",
+		.id = PALMAS_CLK_ID,
+	},
+	{
+		.name = "palmas-pwm",
+		.id = PALMAS_PWM_ID,
+	},
+	{
+		.name = "palmas-usb",
+		.num_resources = ARRAY_SIZE(usb_resource),
+		.resources = usb_resource,
+		.id = PALMAS_USB_ID,
+	}
+};
+
+static const struct regmap_config palmas_regmap_config[PALMAS_NUM_CLIENTS] = {
+	{
+		.reg_bits = 8,
+		.val_bits = 8,
+		.max_register = PALMAS_BASE_TO_REG(PALMAS_PU_PD_OD_BASE,
+					PALMAS_PRIMARY_SECONDARY_PAD3),
+	},
+	{
+		.reg_bits = 8,
+		.val_bits = 8,
+		.max_register = PALMAS_BASE_TO_REG(PALMAS_GPADC_BASE,
+					PALMAS_GPADC_SMPS_VSEL_MONITORING),
+	},
+	{
+		.reg_bits = 8,
+		.val_bits = 8,
+		.max_register = PALMAS_BASE_TO_REG(PALMAS_TRIM_GPADC_BASE,
+					PALMAS_GPADC_TRIM16),
+	},
+};
+
+static const struct regmap_irq palmas_irqs[] = {
+	/* INT1 IRQs */
+	[PALMAS_CHARG_DET_N_VBUS_OVV_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_CHARG_DET_N_VBUS_OVV,
+	},
+	[PALMAS_PWRON_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_PWRON,
+	},
+	[PALMAS_LONG_PRESS_KEY_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_LONG_PRESS_KEY,
+	},
+	[PALMAS_RPWRON_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_RPWRON,
+	},
+	[PALMAS_PWRDOWN_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_PWRDOWN,
+	},
+	[PALMAS_HOTDIE_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_HOTDIE,
+	},
+	[PALMAS_VSYS_MON_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_VSYS_MON,
+	},
+	[PALMAS_VBAT_MON_IRQ] = {
+		.mask = PALMAS_INT1_STATUS_VBAT_MON,
+	},
+	/* INT2 IRQs*/
+	[PALMAS_RTC_ALARM_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_RTC_ALARM,
+		.reg_offset = 1,
+	},
+	[PALMAS_RTC_TIMER_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_RTC_TIMER,
+		.reg_offset = 1,
+	},
+	[PALMAS_WDT_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_WDT,
+		.reg_offset = 1,
+	},
+	[PALMAS_BATREMOVAL_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_BATREMOVAL,
+		.reg_offset = 1,
+	},
+	[PALMAS_RESET_IN_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_RESET_IN,
+		.reg_offset = 1,
+	},
+	[PALMAS_FBI_BB_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_FBI_BB,
+		.reg_offset = 1,
+	},
+	[PALMAS_SHORT_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_SHORT,
+		.reg_offset = 1,
+	},
+	[PALMAS_VAC_ACOK_IRQ] = {
+		.mask = PALMAS_INT2_STATUS_VAC_ACOK,
+		.reg_offset = 1,
+	},
+	/* INT3 IRQs */
+	[PALMAS_GPADC_AUTO_0_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_GPADC_AUTO_0,
+		.reg_offset = 2,
+	},
+	[PALMAS_GPADC_AUTO_1_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_GPADC_AUTO_1,
+		.reg_offset = 2,
+	},
+	[PALMAS_GPADC_EOC_SW_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_GPADC_EOC_SW,
+		.reg_offset = 2,
+	},
+	[PALMAS_GPADC_EOC_RT_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_GPADC_EOC_RT,
+		.reg_offset = 2,
+	},
+	[PALMAS_ID_OTG_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_ID_OTG,
+		.reg_offset = 2,
+	},
+	[PALMAS_ID_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_ID,
+		.reg_offset = 2,
+	},
+	[PALMAS_VBUS_OTG_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_VBUS_OTG,
+		.reg_offset = 2,
+	},
+	[PALMAS_VBUS_IRQ] = {
+		.mask = PALMAS_INT3_STATUS_VBUS,
+		.reg_offset = 2,
+	},
+	/* INT4 IRQs */
+	[PALMAS_GPIO_0_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_0,
+		.reg_offset = 3,
+	},
+	[PALMAS_GPIO_1_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_1,
+		.reg_offset = 3,
+	},
+	[PALMAS_GPIO_2_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_2,
+		.reg_offset = 3,
+	},
+	[PALMAS_GPIO_3_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_3,
+		.reg_offset = 3,
+	},
+	[PALMAS_GPIO_4_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_4,
+		.reg_offset = 3,
+	},
+	[PALMAS_GPIO_5_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_5,
+		.reg_offset = 3,
+	},
+	[PALMAS_GPIO_6_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_6,
+		.reg_offset = 3,
+	},
+	[PALMAS_GPIO_7_IRQ] = {
+		.mask = PALMAS_INT4_STATUS_GPIO_7,
+		.reg_offset = 3,
+	},
+};
+
+static struct regmap_irq_chip palmas_irq_chip = {
+	.name = "palmas",
+	.irqs = palmas_irqs,
+	.num_irqs = ARRAY_SIZE(palmas_irqs),
+
+	.num_regs = 4,
+	.irq_reg_stride = 5,
+	.status_base = PALMAS_BASE_TO_REG(PALMAS_INTERRUPT_BASE,
+			PALMAS_INT1_STATUS),
+	.mask_base = PALMAS_BASE_TO_REG(PALMAS_INTERRUPT_BASE,
+			PALMAS_INT1_MASK),
+};
+
+static int __devinit palmas_i2c_probe(struct i2c_client *i2c,
+			    const struct i2c_device_id *id)
+{
+	struct palmas *palmas;
+	struct palmas_platform_data *pdata;
+	int ret = 0, i;
+	unsigned int reg, addr;
+	int slave;
+	struct mfd_cell *children;
+
+	pdata = dev_get_platdata(&i2c->dev);
+	if (!pdata)
+		return -EINVAL;
+
+	palmas = devm_kzalloc(&i2c->dev, sizeof(struct palmas), GFP_KERNEL);
+	if (palmas == NULL)
+		return -ENOMEM;
+
+	i2c_set_clientdata(i2c, palmas);
+	palmas->dev = &i2c->dev;
+	palmas->id = id->driver_data;
+	palmas->irq = i2c->irq;
+
+	for (i = 0; i < PALMAS_NUM_CLIENTS; i++) {
+		if (i == 0)
+			palmas->i2c_clients[i] = i2c;
+		else {
+			palmas->i2c_clients[i] =
+					i2c_new_dummy(i2c->adapter,
+							i2c->addr + i);
+			if (!palmas->i2c_clients[i]) {
+				dev_err(palmas->dev,
+					"can't attach client %d\n", i);
+				ret = -ENOMEM;
+				goto err;
+			}
+		}
+		palmas->regmap[i] = devm_regmap_init_i2c(palmas->i2c_clients[i],
+				&palmas_regmap_config[i]);
+		if (IS_ERR(palmas->regmap[i])) {
+			ret = PTR_ERR(palmas->regmap[i]);
+			dev_err(palmas->dev,
+				"Failed to allocate regmap %d, err: %d\n",
+				i, ret);
+			goto err;
+		}
+	}
+
+	ret = regmap_add_irq_chip(palmas->regmap[1], palmas->irq,
+			IRQF_ONESHOT | IRQF_TRIGGER_LOW, -1, &palmas_irq_chip,
+			&palmas->irq_data);
+	if (ret < 0)
+		goto err;
+
+	slave = PALMAS_BASE_TO_SLAVE(PALMAS_PU_PD_OD_BASE);
+	addr = PALMAS_BASE_TO_REG(PALMAS_PU_PD_OD_BASE,
+			PALMAS_PRIMARY_SECONDARY_PAD1);
+
+	if (pdata->mux_from_pdata) {
+		reg = pdata->pad1;
+		ret = regmap_write(palmas->regmap[slave], addr, reg);
+		if (ret)
+			goto err;
+	} else {
+		ret = regmap_read(palmas->regmap[slave], addr, &reg);
+		if (ret)
+			goto err;
+	}
+
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_0))
+		palmas->gpio_muxed |= PALMAS_GPIO_0_MUXED;
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_1_MASK))
+		palmas->gpio_muxed |= PALMAS_GPIO_1_MUXED;
+	else if ((reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_1_MASK) ==
+			(2 << PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_1_SHIFT))
+		palmas->led_muxed |= PALMAS_LED1_MUXED;
+	else if ((reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_1_MASK) ==
+			(3 << PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_1_SHIFT))
+		palmas->pwm_muxed |= PALMAS_PWM1_MUXED;
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_2_MASK))
+		palmas->gpio_muxed |= PALMAS_GPIO_2_MUXED;
+	else if ((reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_2_MASK) ==
+			(2 << PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_2_SHIFT))
+		palmas->led_muxed |= PALMAS_LED2_MUXED;
+	else if ((reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_2_MASK) ==
+			(3 << PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_2_SHIFT))
+		palmas->pwm_muxed |= PALMAS_PWM2_MUXED;
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD1_GPIO_3))
+		palmas->gpio_muxed |= PALMAS_GPIO_3_MUXED;
+
+	addr = PALMAS_BASE_TO_REG(PALMAS_PU_PD_OD_BASE,
+			PALMAS_PRIMARY_SECONDARY_PAD2);
+
+	if (pdata->mux_from_pdata) {
+		reg = pdata->pad2;
+		ret = regmap_write(palmas->regmap[slave], addr, reg);
+		if (ret)
+			goto err;
+	} else {
+		ret = regmap_read(palmas->regmap[slave], addr, &reg);
+		if (ret)
+			goto err;
+	}
+
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD2_GPIO_4))
+		palmas->gpio_muxed |= PALMAS_GPIO_4_MUXED;
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD2_GPIO_5_MASK))
+		palmas->gpio_muxed |= PALMAS_GPIO_5_MUXED;
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD2_GPIO_6))
+		palmas->gpio_muxed |= PALMAS_GPIO_6_MUXED;
+	if (!(reg & PALMAS_PRIMARY_SECONDARY_PAD2_GPIO_7_MASK))
+		palmas->gpio_muxed |= PALMAS_GPIO_7_MUXED;
+
+	dev_info(palmas->dev, "Muxing GPIO %x, PWM %x, LED %x\n",
+			palmas->gpio_muxed, palmas->pwm_muxed,
+			palmas->led_muxed);
+
+	reg = pdata->power_ctrl;
+
+	slave = PALMAS_BASE_TO_SLAVE(PALMAS_PMU_CONTROL_BASE);
+	addr = PALMAS_BASE_TO_REG(PALMAS_PMU_CONTROL_BASE, PALMAS_POWER_CTRL);
+
+	ret = regmap_write(palmas->regmap[slave], addr, reg);
+	if (ret)
+		goto err;
+
+	children = kmemdup(palmas_children, sizeof(palmas_children),
+			   GFP_KERNEL);
+	if (!children) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	ret = mfd_add_devices(palmas->dev, -1,
+			      children, ARRAY_SIZE(palmas_children),
+			      NULL, regmap_irq_chip_get_base(palmas->irq_data));
+	kfree(children);
+
+	if (ret < 0)
+		goto err;
+
+	return ret;
+
+err:
+	mfd_remove_devices(palmas->dev);
+	kfree(palmas);
+	return ret;
+}
+
+static int palmas_i2c_remove(struct i2c_client *i2c)
+{
+	struct palmas *palmas = i2c_get_clientdata(i2c);
+
+	mfd_remove_devices(palmas->dev);
+	regmap_del_irq_chip(palmas->irq, palmas->irq_data);
+
+	return 0;
+}
+
+static const struct i2c_device_id palmas_i2c_id[] = {
+	{ "palmas", },
+	{ "twl6035", },
+	{ "twl6037", },
+	{ "tps65913", },
+};
+MODULE_DEVICE_TABLE(i2c, palmas_i2c_id);
+
+static struct of_device_id __devinitdata of_palmas_match_tbl[] = {
+	{ .compatible = "ti,palmas", },
+	{ /* end */ }
+};
+
+static struct i2c_driver palmas_i2c_driver = {
+	.driver = {
+		   .name = "palmas",
+		   .of_match_table = of_palmas_match_tbl,
+		   .owner = THIS_MODULE,
+	},
+	.probe = palmas_i2c_probe,
+	.remove = palmas_i2c_remove,
+	.id_table = palmas_i2c_id,
+};
+
+static int __init palmas_i2c_init(void)
+{
+	return i2c_add_driver(&palmas_i2c_driver);
+}
+/* init early so consumer devices can complete system boot */
+subsys_initcall(palmas_i2c_init);
+
+static void __exit palmas_i2c_exit(void)
+{
+	i2c_del_driver(&palmas_i2c_driver);
+}
+module_exit(palmas_i2c_exit);
+
+MODULE_AUTHOR("Graeme Gregory <gg@slimlogic.co.uk>");
+MODULE_DESCRIPTION("Palmas chip family multi-function driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/rc5t583-irq.c b/drivers/mfd/rc5t583-irq.c
new file mode 100644
index 000000000000..fa6f80fad5f1
--- /dev/null
+++ b/drivers/mfd/rc5t583-irq.c
@@ -0,0 +1,408 @@
+/*
+ * Interrupt driver for RICOH583 power management chip.
+ *
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION.  All rights reserved.
+ * Author: Laxman dewangan <ldewangan@nvidia.com>
+ *
+ * based on code
+ *      Copyright (C) 2011 RICOH COMPANY,LTD
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ *
+ */
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/mfd/rc5t583.h>
+
+enum int_type {
+	SYS_INT  = 0x1,
+	DCDC_INT = 0x2,
+	RTC_INT  = 0x4,
+	ADC_INT  = 0x8,
+	GPIO_INT = 0x10,
+};
+
+static int gpedge_add[] = {
+	RC5T583_GPIO_GPEDGE2,
+	RC5T583_GPIO_GPEDGE2
+};
+
+static int irq_en_add[] = {
+	RC5T583_INT_EN_SYS1,
+	RC5T583_INT_EN_SYS2,
+	RC5T583_INT_EN_DCDC,
+	RC5T583_INT_EN_RTC,
+	RC5T583_INT_EN_ADC1,
+	RC5T583_INT_EN_ADC2,
+	RC5T583_INT_EN_ADC3,
+	RC5T583_GPIO_EN_INT
+};
+
+static int irq_mon_add[] = {
+	RC5T583_INT_MON_SYS1,
+	RC5T583_INT_MON_SYS2,
+	RC5T583_INT_MON_DCDC,
+	RC5T583_INT_MON_RTC,
+	RC5T583_INT_IR_ADCL,
+	RC5T583_INT_IR_ADCH,
+	RC5T583_INT_IR_ADCEND,
+	RC5T583_INT_IR_GPIOF,
+	RC5T583_INT_IR_GPIOR
+};
+
+static int irq_clr_add[] = {
+	RC5T583_INT_IR_SYS1,
+	RC5T583_INT_IR_SYS2,
+	RC5T583_INT_IR_DCDC,
+	RC5T583_INT_IR_RTC,
+	RC5T583_INT_IR_ADCL,
+	RC5T583_INT_IR_ADCH,
+	RC5T583_INT_IR_ADCEND,
+	RC5T583_INT_IR_GPIOF,
+	RC5T583_INT_IR_GPIOR
+};
+
+static int main_int_type[] = {
+	SYS_INT,
+	SYS_INT,
+	DCDC_INT,
+	RTC_INT,
+	ADC_INT,
+	ADC_INT,
+	ADC_INT,
+	GPIO_INT,
+	GPIO_INT,
+};
+
+struct rc5t583_irq_data {
+	u8	int_type;
+	u8	master_bit;
+	u8	int_en_bit;
+	u8	mask_reg_index;
+	int	grp_index;
+};
+
+#define RC5T583_IRQ(_int_type, _master_bit, _grp_index, \
+			_int_bit, _mask_ind)		\
+	{						\
+		.int_type	= _int_type,		\
+		.master_bit	= _master_bit,		\
+		.grp_index	= _grp_index,		\
+		.int_en_bit	= _int_bit,		\
+		.mask_reg_index	= _mask_ind,		\
+	}
+
+static const struct rc5t583_irq_data rc5t583_irqs[RC5T583_MAX_IRQS] = {
+	[RC5T583_IRQ_ONKEY]		= RC5T583_IRQ(SYS_INT,  0, 0, 0, 0),
+	[RC5T583_IRQ_ACOK]		= RC5T583_IRQ(SYS_INT,  0, 1, 1, 0),
+	[RC5T583_IRQ_LIDOPEN]		= RC5T583_IRQ(SYS_INT,  0, 2, 2, 0),
+	[RC5T583_IRQ_PREOT]		= RC5T583_IRQ(SYS_INT,  0, 3, 3, 0),
+	[RC5T583_IRQ_CLKSTP]		= RC5T583_IRQ(SYS_INT,  0, 4, 4, 0),
+	[RC5T583_IRQ_ONKEY_OFF]		= RC5T583_IRQ(SYS_INT,  0, 5, 5, 0),
+	[RC5T583_IRQ_WD]		= RC5T583_IRQ(SYS_INT,  0, 7, 7, 0),
+	[RC5T583_IRQ_EN_PWRREQ1]	= RC5T583_IRQ(SYS_INT,  0, 8, 0, 1),
+	[RC5T583_IRQ_EN_PWRREQ2]	= RC5T583_IRQ(SYS_INT,  0, 9, 1, 1),
+	[RC5T583_IRQ_PRE_VINDET]	= RC5T583_IRQ(SYS_INT,  0, 10, 2, 1),
+
+	[RC5T583_IRQ_DC0LIM]		= RC5T583_IRQ(DCDC_INT, 1, 0, 0, 2),
+	[RC5T583_IRQ_DC1LIM]		= RC5T583_IRQ(DCDC_INT, 1, 1, 1, 2),
+	[RC5T583_IRQ_DC2LIM]		= RC5T583_IRQ(DCDC_INT, 1, 2, 2, 2),
+	[RC5T583_IRQ_DC3LIM]		= RC5T583_IRQ(DCDC_INT, 1, 3, 3, 2),
+
+	[RC5T583_IRQ_CTC]		= RC5T583_IRQ(RTC_INT,  2, 0, 0, 3),
+	[RC5T583_IRQ_YALE]		= RC5T583_IRQ(RTC_INT,  2, 5, 5, 3),
+	[RC5T583_IRQ_DALE]		= RC5T583_IRQ(RTC_INT,  2, 6, 6, 3),
+	[RC5T583_IRQ_WALE]		= RC5T583_IRQ(RTC_INT,  2, 7, 7, 3),
+
+	[RC5T583_IRQ_AIN1L]		= RC5T583_IRQ(ADC_INT,  3, 0, 0, 4),
+	[RC5T583_IRQ_AIN2L]		= RC5T583_IRQ(ADC_INT,  3, 1, 1, 4),
+	[RC5T583_IRQ_AIN3L]		= RC5T583_IRQ(ADC_INT,  3, 2, 2, 4),
+	[RC5T583_IRQ_VBATL]		= RC5T583_IRQ(ADC_INT,  3, 3, 3, 4),
+	[RC5T583_IRQ_VIN3L]		= RC5T583_IRQ(ADC_INT,  3, 4, 4, 4),
+	[RC5T583_IRQ_VIN8L]		= RC5T583_IRQ(ADC_INT,  3, 5, 5, 4),
+	[RC5T583_IRQ_AIN1H]		= RC5T583_IRQ(ADC_INT,  3, 6, 0, 5),
+	[RC5T583_IRQ_AIN2H]		= RC5T583_IRQ(ADC_INT,  3, 7, 1, 5),
+	[RC5T583_IRQ_AIN3H]		= RC5T583_IRQ(ADC_INT,  3, 8, 2, 5),
+	[RC5T583_IRQ_VBATH]		= RC5T583_IRQ(ADC_INT,  3, 9, 3, 5),
+	[RC5T583_IRQ_VIN3H]		= RC5T583_IRQ(ADC_INT,  3, 10, 4, 5),
+	[RC5T583_IRQ_VIN8H]		= RC5T583_IRQ(ADC_INT,  3, 11, 5, 5),
+	[RC5T583_IRQ_ADCEND]		= RC5T583_IRQ(ADC_INT,  3, 12, 0, 6),
+
+	[RC5T583_IRQ_GPIO0]		= RC5T583_IRQ(GPIO_INT, 4, 0, 0, 7),
+	[RC5T583_IRQ_GPIO1]		= RC5T583_IRQ(GPIO_INT, 4, 1, 1, 7),
+	[RC5T583_IRQ_GPIO2]		= RC5T583_IRQ(GPIO_INT, 4, 2, 2, 7),
+	[RC5T583_IRQ_GPIO3]		= RC5T583_IRQ(GPIO_INT, 4, 3, 3, 7),
+	[RC5T583_IRQ_GPIO4]		= RC5T583_IRQ(GPIO_INT, 4, 4, 4, 7),
+	[RC5T583_IRQ_GPIO5]		= RC5T583_IRQ(GPIO_INT, 4, 5, 5, 7),
+	[RC5T583_IRQ_GPIO6]		= RC5T583_IRQ(GPIO_INT, 4, 6, 6, 7),
+	[RC5T583_IRQ_GPIO7]		= RC5T583_IRQ(GPIO_INT, 4, 7, 7, 7),
+};
+
+static void rc5t583_irq_lock(struct irq_data *irq_data)
+{
+	struct rc5t583 *rc5t583 = irq_data_get_irq_chip_data(irq_data);
+	mutex_lock(&rc5t583->irq_lock);
+}
+
+static void rc5t583_irq_unmask(struct irq_data *irq_data)
+{
+	struct rc5t583 *rc5t583 = irq_data_get_irq_chip_data(irq_data);
+	unsigned int __irq = irq_data->irq - rc5t583->irq_base;
+	const struct rc5t583_irq_data *data = &rc5t583_irqs[__irq];
+
+	rc5t583->group_irq_en[data->grp_index] |= 1 << data->grp_index;
+	rc5t583->intc_inten_reg |= 1 << data->master_bit;
+	rc5t583->irq_en_reg[data->mask_reg_index] |= 1 << data->int_en_bit;
+}
+
+static void rc5t583_irq_mask(struct irq_data *irq_data)
+{
+	struct rc5t583 *rc5t583 = irq_data_get_irq_chip_data(irq_data);
+	unsigned int __irq = irq_data->irq - rc5t583->irq_base;
+	const struct rc5t583_irq_data *data = &rc5t583_irqs[__irq];
+
+	rc5t583->group_irq_en[data->grp_index] &= ~(1 << data->grp_index);
+	if (!rc5t583->group_irq_en[data->grp_index])
+		rc5t583->intc_inten_reg &= ~(1 << data->master_bit);
+
+	rc5t583->irq_en_reg[data->mask_reg_index] &= ~(1 << data->int_en_bit);
+}
+
+static int rc5t583_irq_set_type(struct irq_data *irq_data, unsigned int type)
+{
+	struct rc5t583 *rc5t583 = irq_data_get_irq_chip_data(irq_data);
+	unsigned int __irq = irq_data->irq - rc5t583->irq_base;
+	const struct rc5t583_irq_data *data = &rc5t583_irqs[__irq];
+	int val = 0;
+	int gpedge_index;
+	int gpedge_bit_pos;
+
+	/* Supporting only trigger level inetrrupt */
+	if ((data->int_type & GPIO_INT) && (type & IRQ_TYPE_EDGE_BOTH)) {
+		gpedge_index = data->int_en_bit / 4;
+		gpedge_bit_pos = data->int_en_bit % 4;
+
+		if (type & IRQ_TYPE_EDGE_FALLING)
+			val |= 0x2;
+
+		if (type & IRQ_TYPE_EDGE_RISING)
+			val |= 0x1;
+
+		rc5t583->gpedge_reg[gpedge_index] &= ~(3 << gpedge_bit_pos);
+		rc5t583->gpedge_reg[gpedge_index] |= (val << gpedge_bit_pos);
+		rc5t583_irq_unmask(irq_data);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static void rc5t583_irq_sync_unlock(struct irq_data *irq_data)
+{
+	struct rc5t583 *rc5t583 = irq_data_get_irq_chip_data(irq_data);
+	int i;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(rc5t583->gpedge_reg); i++) {
+		ret = rc5t583_write(rc5t583->dev, gpedge_add[i],
+				rc5t583->gpedge_reg[i]);
+		if (ret < 0)
+			dev_warn(rc5t583->dev,
+				"Error in writing reg 0x%02x error: %d\n",
+				gpedge_add[i], ret);
+	}
+
+	for (i = 0; i < ARRAY_SIZE(rc5t583->irq_en_reg); i++) {
+		ret = rc5t583_write(rc5t583->dev, irq_en_add[i],
+					rc5t583->irq_en_reg[i]);
+		if (ret < 0)
+			dev_warn(rc5t583->dev,
+				"Error in writing reg 0x%02x error: %d\n",
+				irq_en_add[i], ret);
+	}
+
+	ret = rc5t583_write(rc5t583->dev, RC5T583_INTC_INTEN,
+				rc5t583->intc_inten_reg);
+	if (ret < 0)
+		dev_warn(rc5t583->dev,
+			"Error in writing reg 0x%02x error: %d\n",
+			RC5T583_INTC_INTEN, ret);
+
+	mutex_unlock(&rc5t583->irq_lock);
+}
+#ifdef CONFIG_PM_SLEEP
+static int rc5t583_irq_set_wake(struct irq_data *irq_data, unsigned int on)
+{
+	struct rc5t583 *rc5t583 = irq_data_get_irq_chip_data(irq_data);
+	return irq_set_irq_wake(rc5t583->chip_irq, on);
+}
+#else
+#define rc5t583_irq_set_wake NULL
+#endif
+
+static irqreturn_t rc5t583_irq(int irq, void *data)
+{
+	struct rc5t583 *rc5t583 = data;
+	uint8_t int_sts[RC5T583_MAX_INTERRUPT_MASK_REGS];
+	uint8_t master_int;
+	int i;
+	int ret;
+	unsigned int rtc_int_sts = 0;
+
+	/* Clear the status */
+	for (i = 0; i < RC5T583_MAX_INTERRUPT_MASK_REGS; i++)
+		int_sts[i] = 0;
+
+	ret  = rc5t583_read(rc5t583->dev, RC5T583_INTC_INTMON, &master_int);
+	if (ret < 0) {
+		dev_err(rc5t583->dev,
+			"Error in reading reg 0x%02x error: %d\n",
+			RC5T583_INTC_INTMON, ret);
+		return IRQ_HANDLED;
+	}
+
+	for (i = 0; i < RC5T583_MAX_INTERRUPT_MASK_REGS; ++i) {
+		if (!(master_int & main_int_type[i]))
+			continue;
+
+		ret = rc5t583_read(rc5t583->dev, irq_mon_add[i], &int_sts[i]);
+		if (ret < 0) {
+			dev_warn(rc5t583->dev,
+				"Error in reading reg 0x%02x error: %d\n",
+				irq_mon_add[i], ret);
+			int_sts[i] = 0;
+			continue;
+		}
+
+		if (main_int_type[i] & RTC_INT) {
+			rtc_int_sts = 0;
+			if (int_sts[i] & 0x1)
+				rtc_int_sts |= BIT(6);
+			if (int_sts[i] & 0x2)
+				rtc_int_sts |= BIT(7);
+			if (int_sts[i] & 0x4)
+				rtc_int_sts |= BIT(0);
+			if (int_sts[i] & 0x8)
+				rtc_int_sts |= BIT(5);
+		}
+
+		ret = rc5t583_write(rc5t583->dev, irq_clr_add[i],
+				~int_sts[i]);
+		if (ret < 0)
+			dev_warn(rc5t583->dev,
+				"Error in reading reg 0x%02x error: %d\n",
+				irq_clr_add[i], ret);
+
+		if (main_int_type[i] & RTC_INT)
+			int_sts[i] = rtc_int_sts;
+	}
+
+	/* Merge gpio interrupts for rising and falling case*/
+	int_sts[7] |= int_sts[8];
+
+	/* Call interrupt handler if enabled */
+	for (i = 0; i < RC5T583_MAX_IRQS; ++i) {
+		const struct rc5t583_irq_data *data = &rc5t583_irqs[i];
+		if ((int_sts[data->mask_reg_index] & (1 << data->int_en_bit)) &&
+			(rc5t583->group_irq_en[data->master_bit] &
+					(1 << data->grp_index)))
+			handle_nested_irq(rc5t583->irq_base + i);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static struct irq_chip rc5t583_irq_chip = {
+	.name = "rc5t583-irq",
+	.irq_mask = rc5t583_irq_mask,
+	.irq_unmask = rc5t583_irq_unmask,
+	.irq_bus_lock = rc5t583_irq_lock,
+	.irq_bus_sync_unlock = rc5t583_irq_sync_unlock,
+	.irq_set_type = rc5t583_irq_set_type,
+	.irq_set_wake = rc5t583_irq_set_wake,
+};
+
+int rc5t583_irq_init(struct rc5t583 *rc5t583, int irq, int irq_base)
+{
+	int i, ret;
+
+	if (!irq_base) {
+		dev_warn(rc5t583->dev, "No interrupt support on IRQ base\n");
+		return -EINVAL;
+	}
+
+	mutex_init(&rc5t583->irq_lock);
+
+	/* Initailize all int register to 0 */
+	for (i = 0; i < RC5T583_MAX_INTERRUPT_MASK_REGS; i++)  {
+		ret = rc5t583_write(rc5t583->dev, irq_en_add[i],
+				rc5t583->irq_en_reg[i]);
+		if (ret < 0)
+			dev_warn(rc5t583->dev,
+				"Error in writing reg 0x%02x error: %d\n",
+				irq_en_add[i], ret);
+	}
+
+	for (i = 0; i < RC5T583_MAX_GPEDGE_REG; i++)  {
+		ret = rc5t583_write(rc5t583->dev, gpedge_add[i],
+				rc5t583->gpedge_reg[i]);
+		if (ret < 0)
+			dev_warn(rc5t583->dev,
+				"Error in writing reg 0x%02x error: %d\n",
+				gpedge_add[i], ret);
+	}
+
+	ret = rc5t583_write(rc5t583->dev, RC5T583_INTC_INTEN, 0x0);
+	if (ret < 0)
+		dev_warn(rc5t583->dev,
+			"Error in writing reg 0x%02x error: %d\n",
+			RC5T583_INTC_INTEN, ret);
+
+	/* Clear all interrupts in case they woke up active. */
+	for (i = 0; i < RC5T583_MAX_INTERRUPT_MASK_REGS; i++)  {
+		ret = rc5t583_write(rc5t583->dev, irq_clr_add[i], 0);
+		if (ret < 0)
+			dev_warn(rc5t583->dev,
+				"Error in writing reg 0x%02x error: %d\n",
+				irq_clr_add[i], ret);
+	}
+
+	rc5t583->irq_base = irq_base;
+	rc5t583->chip_irq = irq;
+
+	for (i = 0; i < RC5T583_MAX_IRQS; i++) {
+		int __irq = i + rc5t583->irq_base;
+		irq_set_chip_data(__irq, rc5t583);
+		irq_set_chip_and_handler(__irq, &rc5t583_irq_chip,
+					 handle_simple_irq);
+		irq_set_nested_thread(__irq, 1);
+#ifdef CONFIG_ARM
+		set_irq_flags(__irq, IRQF_VALID);
+#endif
+	}
+
+	ret = request_threaded_irq(irq, NULL, rc5t583_irq, IRQF_ONESHOT,
+				"rc5t583", rc5t583);
+	if (ret < 0)
+		dev_err(rc5t583->dev,
+			"Error in registering interrupt error: %d\n", ret);
+	return ret;
+}
+
+int rc5t583_irq_exit(struct rc5t583 *rc5t583)
+{
+	if (rc5t583->chip_irq)
+		free_irq(rc5t583->chip_irq, rc5t583);
+	return 0;
+}
diff --git a/drivers/mfd/rc5t583.c b/drivers/mfd/rc5t583.c
new file mode 100644
index 000000000000..cdc1df7fa0e9
--- /dev/null
+++ b/drivers/mfd/rc5t583.c
@@ -0,0 +1,347 @@
+/*
+ * Core driver access RC5T583 power management chip.
+ *
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION.  All rights reserved.
+ * Author: Laxman dewangan <ldewangan@nvidia.com>
+ *
+ * Based on code
+ *	Copyright (C) 2011 RICOH COMPANY,LTD
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ *
+ */
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/rc5t583.h>
+#include <linux/regmap.h>
+
+#define RICOH_ONOFFSEL_REG	0x10
+#define RICOH_SWCTL_REG		0x5E
+
+struct deepsleep_control_data {
+	u8 reg_add;
+	u8 ds_pos_bit;
+};
+
+#define DEEPSLEEP_INIT(_id, _reg, _pos)		\
+	{					\
+		.reg_add = RC5T583_##_reg,	\
+		.ds_pos_bit = _pos,		\
+	}
+
+static struct deepsleep_control_data deepsleep_data[] = {
+	DEEPSLEEP_INIT(DC0, SLPSEQ1, 0),
+	DEEPSLEEP_INIT(DC1, SLPSEQ1, 4),
+	DEEPSLEEP_INIT(DC2, SLPSEQ2, 0),
+	DEEPSLEEP_INIT(DC3, SLPSEQ2, 4),
+	DEEPSLEEP_INIT(LDO0, SLPSEQ3, 0),
+	DEEPSLEEP_INIT(LDO1, SLPSEQ3, 4),
+	DEEPSLEEP_INIT(LDO2, SLPSEQ4, 0),
+	DEEPSLEEP_INIT(LDO3, SLPSEQ4, 4),
+	DEEPSLEEP_INIT(LDO4, SLPSEQ5, 0),
+	DEEPSLEEP_INIT(LDO5, SLPSEQ5, 4),
+	DEEPSLEEP_INIT(LDO6, SLPSEQ6, 0),
+	DEEPSLEEP_INIT(LDO7, SLPSEQ6, 4),
+	DEEPSLEEP_INIT(LDO8, SLPSEQ7, 0),
+	DEEPSLEEP_INIT(LDO9, SLPSEQ7, 4),
+	DEEPSLEEP_INIT(PSO0, SLPSEQ8, 0),
+	DEEPSLEEP_INIT(PSO1, SLPSEQ8, 4),
+	DEEPSLEEP_INIT(PSO2, SLPSEQ9, 0),
+	DEEPSLEEP_INIT(PSO3, SLPSEQ9, 4),
+	DEEPSLEEP_INIT(PSO4, SLPSEQ10, 0),
+	DEEPSLEEP_INIT(PSO5, SLPSEQ10, 4),
+	DEEPSLEEP_INIT(PSO6, SLPSEQ11, 0),
+	DEEPSLEEP_INIT(PSO7, SLPSEQ11, 4),
+};
+
+#define EXT_PWR_REQ		\
+	(RC5T583_EXT_PWRREQ1_CONTROL | RC5T583_EXT_PWRREQ2_CONTROL)
+
+static struct mfd_cell rc5t583_subdevs[] = {
+	{.name = "rc5t583-gpio",},
+	{.name = "rc5t583-regulator",},
+	{.name = "rc5t583-rtc",      },
+	{.name = "rc5t583-key",      }
+};
+
+static int __rc5t583_set_ext_pwrreq1_control(struct device *dev,
+	int id, int ext_pwr, int slots)
+{
+	int ret;
+	uint8_t sleepseq_val;
+	unsigned int en_bit;
+	unsigned int slot_bit;
+
+	if (id == RC5T583_DS_DC0) {
+		dev_err(dev, "PWRREQ1 is invalid control for rail %d\n", id);
+		return -EINVAL;
+	}
+
+	en_bit = deepsleep_data[id].ds_pos_bit;
+	slot_bit = en_bit + 1;
+	ret = rc5t583_read(dev, deepsleep_data[id].reg_add, &sleepseq_val);
+	if (ret < 0) {
+		dev_err(dev, "Error in reading reg 0x%x\n",
+				deepsleep_data[id].reg_add);
+		return ret;
+	}
+
+	sleepseq_val &= ~(0xF << en_bit);
+	sleepseq_val |= BIT(en_bit);
+	sleepseq_val |= ((slots & 0x7) << slot_bit);
+	ret = rc5t583_set_bits(dev, RICOH_ONOFFSEL_REG, BIT(1));
+	if (ret < 0) {
+		dev_err(dev, "Error in updating the 0x%02x register\n",
+				RICOH_ONOFFSEL_REG);
+		return ret;
+	}
+
+	ret = rc5t583_write(dev, deepsleep_data[id].reg_add, sleepseq_val);
+	if (ret < 0) {
+		dev_err(dev, "Error in writing reg 0x%x\n",
+				deepsleep_data[id].reg_add);
+		return ret;
+	}
+
+	if (id == RC5T583_DS_LDO4) {
+		ret = rc5t583_write(dev, RICOH_SWCTL_REG, 0x1);
+		if (ret < 0)
+			dev_err(dev, "Error in writing reg 0x%x\n",
+				RICOH_SWCTL_REG);
+	}
+	return ret;
+}
+
+static int __rc5t583_set_ext_pwrreq2_control(struct device *dev,
+	int id, int ext_pwr)
+{
+	int ret;
+
+	if (id != RC5T583_DS_DC0) {
+		dev_err(dev, "PWRREQ2 is invalid control for rail %d\n", id);
+		return -EINVAL;
+	}
+
+	ret = rc5t583_set_bits(dev, RICOH_ONOFFSEL_REG, BIT(2));
+	if (ret < 0)
+		dev_err(dev, "Error in updating the ONOFFSEL 0x10 register\n");
+	return ret;
+}
+
+int rc5t583_ext_power_req_config(struct device *dev, int ds_id,
+	int ext_pwr_req, int deepsleep_slot_nr)
+{
+	if ((ext_pwr_req & EXT_PWR_REQ) == EXT_PWR_REQ)
+		return -EINVAL;
+
+	if (ext_pwr_req & RC5T583_EXT_PWRREQ1_CONTROL)
+		return __rc5t583_set_ext_pwrreq1_control(dev, ds_id,
+				ext_pwr_req, deepsleep_slot_nr);
+
+	if (ext_pwr_req & RC5T583_EXT_PWRREQ2_CONTROL)
+		return __rc5t583_set_ext_pwrreq2_control(dev,
+			ds_id, ext_pwr_req);
+	return 0;
+}
+EXPORT_SYMBOL(rc5t583_ext_power_req_config);
+
+static int rc5t583_clear_ext_power_req(struct rc5t583 *rc5t583,
+	struct rc5t583_platform_data *pdata)
+{
+	int ret;
+	int i;
+	uint8_t on_off_val = 0;
+
+	/*  Clear ONOFFSEL register */
+	if (pdata->enable_shutdown)
+		on_off_val = 0x1;
+
+	ret = rc5t583_write(rc5t583->dev, RICOH_ONOFFSEL_REG, on_off_val);
+	if (ret < 0)
+		dev_warn(rc5t583->dev, "Error in writing reg %d error: %d\n",
+					RICOH_ONOFFSEL_REG, ret);
+
+	ret = rc5t583_write(rc5t583->dev, RICOH_SWCTL_REG, 0x0);
+	if (ret < 0)
+		dev_warn(rc5t583->dev, "Error in writing reg %d error: %d\n",
+					RICOH_SWCTL_REG, ret);
+
+	/* Clear sleep sequence register */
+	for (i = RC5T583_SLPSEQ1; i <= RC5T583_SLPSEQ11; ++i) {
+		ret = rc5t583_write(rc5t583->dev, i, 0x0);
+		if (ret < 0)
+			dev_warn(rc5t583->dev,
+				"Error in writing reg 0x%02x error: %d\n",
+				i, ret);
+	}
+	return 0;
+}
+
+static bool volatile_reg(struct device *dev, unsigned int reg)
+{
+	/* Enable caching in interrupt registers */
+	switch (reg) {
+	case RC5T583_INT_EN_SYS1:
+	case RC5T583_INT_EN_SYS2:
+	case RC5T583_INT_EN_DCDC:
+	case RC5T583_INT_EN_RTC:
+	case RC5T583_INT_EN_ADC1:
+	case RC5T583_INT_EN_ADC2:
+	case RC5T583_INT_EN_ADC3:
+	case RC5T583_GPIO_GPEDGE1:
+	case RC5T583_GPIO_GPEDGE2:
+	case RC5T583_GPIO_EN_INT:
+		return false;
+
+	case RC5T583_GPIO_MON_IOIN:
+		/* This is gpio input register */
+		return true;
+
+	default:
+		/* Enable caching in gpio registers */
+		if ((reg >= RC5T583_GPIO_IOSEL) &&
+				(reg <= RC5T583_GPIO_GPOFUNC))
+			return false;
+
+		/* Enable caching in sleep seq registers */
+		if ((reg >= RC5T583_SLPSEQ1) && (reg <= RC5T583_SLPSEQ11))
+			return false;
+
+		/* Enable caching of regulator registers */
+		if ((reg >= RC5T583_REG_DC0CTL) && (reg <= RC5T583_REG_SR3CTL))
+			return false;
+		if ((reg >= RC5T583_REG_LDOEN1) &&
+					(reg <= RC5T583_REG_LDO9DAC_DS))
+			return false;
+
+		break;
+	}
+
+	return true;
+}
+
+static const struct regmap_config rc5t583_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.volatile_reg = volatile_reg,
+	.max_register = RC5T583_MAX_REGS,
+	.num_reg_defaults_raw = RC5T583_MAX_REGS,
+	.cache_type = REGCACHE_RBTREE,
+};
+
+static int __devinit rc5t583_i2c_probe(struct i2c_client *i2c,
+			      const struct i2c_device_id *id)
+{
+	struct rc5t583 *rc5t583;
+	struct rc5t583_platform_data *pdata = i2c->dev.platform_data;
+	int ret;
+	bool irq_init_success = false;
+
+	if (!pdata) {
+		dev_err(&i2c->dev, "Err: Platform data not found\n");
+		return -EINVAL;
+	}
+
+	rc5t583 = devm_kzalloc(&i2c->dev, sizeof(struct rc5t583), GFP_KERNEL);
+	if (!rc5t583) {
+		dev_err(&i2c->dev, "Memory allocation failed\n");
+		return -ENOMEM;
+	}
+
+	rc5t583->dev = &i2c->dev;
+	i2c_set_clientdata(i2c, rc5t583);
+
+	rc5t583->regmap = devm_regmap_init_i2c(i2c, &rc5t583_regmap_config);
+	if (IS_ERR(rc5t583->regmap)) {
+		ret = PTR_ERR(rc5t583->regmap);
+		dev_err(&i2c->dev, "regmap initialization failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = rc5t583_clear_ext_power_req(rc5t583, pdata);
+	if (ret < 0)
+		return ret;
+
+	if (i2c->irq) {
+		ret = rc5t583_irq_init(rc5t583, i2c->irq, pdata->irq_base);
+		/* Still continue with waring if irq init fails */
+		if (ret)
+			dev_warn(&i2c->dev, "IRQ init failed: %d\n", ret);
+		else
+			irq_init_success = true;
+	}
+
+	ret = mfd_add_devices(rc5t583->dev, -1, rc5t583_subdevs,
+			ARRAY_SIZE(rc5t583_subdevs), NULL, 0);
+	if (ret) {
+		dev_err(&i2c->dev, "add mfd devices failed: %d\n", ret);
+		goto err_add_devs;
+	}
+
+	return 0;
+
+err_add_devs:
+	if (irq_init_success)
+		rc5t583_irq_exit(rc5t583);
+	return ret;
+}
+
+static int  __devexit rc5t583_i2c_remove(struct i2c_client *i2c)
+{
+	struct rc5t583 *rc5t583 = i2c_get_clientdata(i2c);
+
+	mfd_remove_devices(rc5t583->dev);
+	rc5t583_irq_exit(rc5t583);
+	return 0;
+}
+
+static const struct i2c_device_id rc5t583_i2c_id[] = {
+	{.name = "rc5t583", .driver_data = 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, rc5t583_i2c_id);
+
+static struct i2c_driver rc5t583_i2c_driver = {
+	.driver = {
+		   .name = "rc5t583",
+		   .owner = THIS_MODULE,
+		   },
+	.probe = rc5t583_i2c_probe,
+	.remove = __devexit_p(rc5t583_i2c_remove),
+	.id_table = rc5t583_i2c_id,
+};
+
+static int __init rc5t583_i2c_init(void)
+{
+	return i2c_add_driver(&rc5t583_i2c_driver);
+}
+subsys_initcall(rc5t583_i2c_init);
+
+static void __exit rc5t583_i2c_exit(void)
+{
+	i2c_del_driver(&rc5t583_i2c_driver);
+}
+
+module_exit(rc5t583_i2c_exit);
+
+MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
+MODULE_DESCRIPTION("RICOH RC5T583 power management system device driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/ricoh583.c b/drivers/mfd/ricoh583.c
new file mode 100644
index 000000000000..a29053ebaf86
--- /dev/null
+++ b/drivers/mfd/ricoh583.c
@@ -0,0 +1,1213 @@
+/*
+ * driver/mfd/ricoh583.c
+ *
+ * Core driver implementation to access RICOH583 power management chip.
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ *
+ * Copyright (C) 2011 RICOH COMPANY,LTD
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ */
+/*#define DEBUG			1*/
+/*#define VERBOSE_DEBUG		1*/
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/ricoh583.h>
+
+#define RICOH_ONOFFSEL_REG	0x10
+#define RICOH_SWCTL_REG		0x5E
+
+/* Interrupt enable register */
+#define RICOH583_INT_EN_SYS1	0x19
+#define RICOH583_INT_EN_SYS2	0x1D
+#define RICOH583_INT_EN_DCDC	0x41
+#define RICOH583_INT_EN_RTC	0xED
+#define RICOH583_INT_EN_ADC1	0x90
+#define RICOH583_INT_EN_ADC2	0x91
+#define RICOH583_INT_EN_ADC3	0x92
+#define RICOH583_INT_EN_GPIO	0xA8
+
+/* interrupt status registers (monitor regs in Ricoh)*/
+#define RICOH583_INTC_INTPOL	0xAD
+#define RICOH583_INTC_INTEN	0xAE
+#define RICOH583_INTC_INTMON	0xAF
+
+#define RICOH583_INT_MON_GRP	0xAF
+#define RICOH583_INT_MON_SYS1	0x1B
+#define RICOH583_INT_MON_SYS2	0x1F
+#define RICOH583_INT_MON_DCDC	0x43
+#define RICOH583_INT_MON_RTC	0xEE
+
+/* interrupt clearing registers */
+#define RICOH583_INT_IR_SYS1	0x1A
+#define RICOH583_INT_IR_SYS2	0x1E
+#define RICOH583_INT_IR_DCDC	0x42
+#define RICOH583_INT_IR_RTC	0xEE
+#define RICOH583_INT_IR_ADCL	0x94
+#define RICOH583_INT_IR_ADCH	0x95
+#define RICOH583_INT_IR_ADCEND	0x96
+#define RICOH583_INT_IR_GPIOR	0xA9
+#define RICOH583_INT_IR_GPIOF	0xAA
+
+/* GPIO register base address */
+#define RICOH583_GPIO_IOSEL	0xA0
+#define RICOH583_GPIO_PDEN	0xA1
+#define RICOH583_GPIO_IOOUT	0xA2
+#define RICOH583_GPIO_PGSEL	0xA3
+#define RICOH583_GPIO_GPINV	0xA4
+#define RICOH583_GPIO_GPDEB	0xA5
+#define RICOH583_GPIO_GPEDGE1	0xA6
+#define RICOH583_GPIO_GPEDGE2	0xA7
+#define RICOH583_GPIO_EN_GPIR	0xA8
+#define RICOH583_GPIO_MON_IOIN	0xAB
+#define RICOH583_GPIO_GPOFUNC	0xAC
+#define RICOH583_INTC_INTEN	0xAE
+
+enum int_type {
+	SYS_INT	 = 0x1,
+	DCDC_INT = 0x2,
+	RTC_INT  = 0x4,
+	ADC_INT  = 0x8,
+	GPIO_INT = 0x10,
+};
+
+struct ricoh583_irq_data {
+	u8	int_type;
+	u8	master_bit;
+	u8	int_en_bit;
+	u8	mask_reg_index;
+	int	grp_index;
+};
+
+struct deepsleep_control_data {
+	u8 reg_add;
+	u8 ds_pos_bit;
+};
+
+#define RICOH583_IRQ(_int_type, _master_bit, _grp_index, _int_bit, _mask_ind) \
+	{						\
+		.int_type	= _int_type,		\
+		.master_bit	= _master_bit,		\
+		.grp_index	= _grp_index,		\
+		.int_en_bit	= _int_bit,		\
+		.mask_reg_index	= _mask_ind,		\
+	}
+
+static const struct ricoh583_irq_data ricoh583_irqs[] = {
+	[RICOH583_IRQ_ONKEY]		= RICOH583_IRQ(SYS_INT,  0, 0, 0, 0),
+	[RICOH583_IRQ_ACOK]		= RICOH583_IRQ(SYS_INT,  0, 1, 1, 0),
+	[RICOH583_IRQ_LIDOPEN]		= RICOH583_IRQ(SYS_INT,  0, 2, 2, 0),
+	[RICOH583_IRQ_PREOT]		= RICOH583_IRQ(SYS_INT,  0, 3, 3, 0),
+	[RICOH583_IRQ_CLKSTP]		= RICOH583_IRQ(SYS_INT,  0, 4, 4, 0),
+	[RICOH583_IRQ_ONKEY_OFF]	= RICOH583_IRQ(SYS_INT,  0, 5, 5, 0),
+	[RICOH583_IRQ_WD]		= RICOH583_IRQ(SYS_INT,  0, 7, 7, 0),
+	[RICOH583_IRQ_EN_PWRREQ1]	= RICOH583_IRQ(SYS_INT,  0, 8, 0, 1),
+	[RICOH583_IRQ_EN_PWRREQ2]	= RICOH583_IRQ(SYS_INT,  0, 9, 1, 1),
+	[RICOH583_IRQ_PRE_VINDET]	= RICOH583_IRQ(SYS_INT,  0, 10, 2, 1),
+
+	[RICOH583_IRQ_DC0LIM]		= RICOH583_IRQ(DCDC_INT, 1, 0, 0, 2),
+	[RICOH583_IRQ_DC1LIM]		= RICOH583_IRQ(DCDC_INT, 1, 1, 1, 2),
+	[RICOH583_IRQ_DC2LIM]		= RICOH583_IRQ(DCDC_INT, 1, 2, 2, 2),
+	[RICOH583_IRQ_DC3LIM]		= RICOH583_IRQ(DCDC_INT, 1, 3, 3, 2),
+
+	[RICOH583_IRQ_CTC]		= RICOH583_IRQ(RTC_INT,  2, 0, 0, 3),
+	[RICOH583_IRQ_YALE]		= RICOH583_IRQ(RTC_INT,  2, 5, 5, 3),
+	[RICOH583_IRQ_DALE]		= RICOH583_IRQ(RTC_INT,  2, 6, 6, 3),
+	[RICOH583_IRQ_WALE]		= RICOH583_IRQ(RTC_INT,  2, 7, 7, 3),
+
+	[RICOH583_IRQ_AIN1L]		= RICOH583_IRQ(ADC_INT,  3, 0, 0, 4),
+	[RICOH583_IRQ_AIN2L]		= RICOH583_IRQ(ADC_INT,  3, 1, 1, 4),
+	[RICOH583_IRQ_AIN3L]		= RICOH583_IRQ(ADC_INT,  3, 2, 2, 4),
+	[RICOH583_IRQ_VBATL]		= RICOH583_IRQ(ADC_INT,  3, 3, 3, 4),
+	[RICOH583_IRQ_VIN3L]		= RICOH583_IRQ(ADC_INT,  3, 4, 4, 4),
+	[RICOH583_IRQ_VIN8L]		= RICOH583_IRQ(ADC_INT,  3, 5, 5, 4),
+	[RICOH583_IRQ_AIN1H]		= RICOH583_IRQ(ADC_INT,  3, 6, 0, 5),
+	[RICOH583_IRQ_AIN2H]		= RICOH583_IRQ(ADC_INT,  3, 7, 1, 5),
+	[RICOH583_IRQ_AIN3H]		= RICOH583_IRQ(ADC_INT,  3, 8, 2, 5),
+	[RICOH583_IRQ_VBATH]		= RICOH583_IRQ(ADC_INT,  3, 9, 3, 5),
+	[RICOH583_IRQ_VIN3H]		= RICOH583_IRQ(ADC_INT,  3, 10, 4, 5),
+	[RICOH583_IRQ_VIN8H]		= RICOH583_IRQ(ADC_INT,  3, 11, 5, 5),
+	[RICOH583_IRQ_ADCEND]		= RICOH583_IRQ(ADC_INT,  3, 12, 0, 6),
+
+	[RICOH583_IRQ_GPIO0]		= RICOH583_IRQ(GPIO_INT, 4, 0, 0, 7),
+	[RICOH583_IRQ_GPIO1]		= RICOH583_IRQ(GPIO_INT, 4, 1, 1, 7),
+	[RICOH583_IRQ_GPIO2]		= RICOH583_IRQ(GPIO_INT, 4, 2, 2, 7),
+	[RICOH583_IRQ_GPIO3]		= RICOH583_IRQ(GPIO_INT, 4, 3, 3, 7),
+	[RICOH583_IRQ_GPIO4]		= RICOH583_IRQ(GPIO_INT, 4, 4, 4, 7),
+	[RICOH583_IRQ_GPIO5]		= RICOH583_IRQ(GPIO_INT, 4, 5, 5, 7),
+	[RICOH583_IRQ_GPIO6]		= RICOH583_IRQ(GPIO_INT, 4, 6, 6, 7),
+	[RICOH583_IRQ_GPIO7]		= RICOH583_IRQ(GPIO_INT, 4, 7, 7, 7),
+	[RICOH583_NR_IRQS]		= RICOH583_IRQ(GPIO_INT, 4, 8, 8, 7),
+};
+
+#define DEEPSLEEP_INIT(_id, _reg, _pos)		\
+	[RICOH583_DS_##_id] = {.reg_add = _reg, .ds_pos_bit = _pos}
+
+static struct deepsleep_control_data deepsleep_data[] = {
+	DEEPSLEEP_INIT(DC1, 0x21, 4),
+	DEEPSLEEP_INIT(DC2, 0x22, 0),
+	DEEPSLEEP_INIT(DC3, 0x22, 4),
+	DEEPSLEEP_INIT(LDO0, 0x23, 0),
+	DEEPSLEEP_INIT(LDO1, 0x23, 4),
+	DEEPSLEEP_INIT(LDO2, 0x24, 0),
+	DEEPSLEEP_INIT(LDO3, 0x24, 4),
+	DEEPSLEEP_INIT(LDO4, 0x25, 0),
+	DEEPSLEEP_INIT(LDO5, 0x25, 4),
+	DEEPSLEEP_INIT(LDO6, 0x26, 0),
+	DEEPSLEEP_INIT(LDO7, 0x26, 4),
+	DEEPSLEEP_INIT(LDO8, 0x27, 0),
+	DEEPSLEEP_INIT(LDO9, 0x27, 4),
+	DEEPSLEEP_INIT(PSO0, 0x28, 0),
+	DEEPSLEEP_INIT(PSO1, 0x28, 4),
+	DEEPSLEEP_INIT(PSO2, 0x29, 0),
+	DEEPSLEEP_INIT(PSO3, 0x29, 4),
+	DEEPSLEEP_INIT(PSO4, 0x2A, 0),
+	DEEPSLEEP_INIT(PSO5, 0x2A, 4),
+	DEEPSLEEP_INIT(PSO6, 0x2B, 0),
+	DEEPSLEEP_INIT(PSO7, 0x2B, 4),
+};
+
+#define MAX_INTERRUPT_MASKS	8
+#define MAX_MAIN_INTERRUPT	5
+#define EXT_PWR_REQ		\
+	(RICOH583_EXT_PWRREQ1_CONTROL | RICOH583_EXT_PWRREQ2_CONTROL)
+
+struct ricoh583 {
+	struct device		*dev;
+	struct i2c_client	*client;
+	struct mutex		io_lock;
+	int			gpio_base;
+	struct gpio_chip	gpio;
+	int			irq_base;
+	struct irq_chip		irq_chip;
+	struct mutex		irq_lock;
+	unsigned long		group_irq_en[MAX_MAIN_INTERRUPT];
+
+	/* For main interrupt bits in INTC */
+	u8			intc_inten_cache;
+	u8			intc_inten_reg;
+
+	/* For group interrupt bits and address */
+	u8			irq_en_cache[MAX_INTERRUPT_MASKS];
+	u8			irq_en_reg[MAX_INTERRUPT_MASKS];
+	u8			irq_en_add[MAX_INTERRUPT_MASKS];
+
+	/* Interrupt monitor and clear register */
+	u8			irq_mon_add[MAX_INTERRUPT_MASKS + 1];
+	u8			irq_clr_add[MAX_INTERRUPT_MASKS + 1];
+	u8			main_int_type[MAX_INTERRUPT_MASKS + 1];
+
+	/* For gpio edge */
+	u8			gpedge_cache[2];
+	u8			gpedge_reg[2];
+	u8			gpedge_add[2];
+};
+
+static inline int __ricoh583_read(struct i2c_client *client,
+				  u8 reg, uint8_t *val)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, reg);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed reading at 0x%02x\n", reg);
+		return ret;
+	}
+
+	*val = (uint8_t)ret;
+	dev_dbg(&client->dev, "ricoh583: reg read  reg=%x, val=%x\n",
+				reg, *val);
+	return 0;
+}
+
+static inline int __ricoh583_bulk_reads(struct i2c_client *client, u8 reg,
+				int len, uint8_t *val)
+{
+	int ret;
+	int i;
+
+	ret = i2c_smbus_read_i2c_block_data(client, reg, len, val);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed reading from 0x%02x\n", reg);
+		return ret;
+	}
+	for (i = 0; i < len; ++i) {
+		dev_dbg(&client->dev, "ricoh583: reg read  reg=%x, val=%x\n",
+				reg + i, *(val + i));
+	}
+	return 0;
+}
+
+static inline int __ricoh583_write(struct i2c_client *client,
+				 u8 reg, uint8_t val)
+{
+	int ret;
+
+	dev_dbg(&client->dev, "ricoh583: reg write  reg=%x, val=%x\n",
+				reg, val);
+	ret = i2c_smbus_write_byte_data(client, reg, val);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed writing 0x%02x to 0x%02x\n",
+				val, reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+static inline int __ricoh583_bulk_writes(struct i2c_client *client, u8 reg,
+				  int len, uint8_t *val)
+{
+	int ret;
+	int i;
+
+	for (i = 0; i < len; ++i) {
+		dev_dbg(&client->dev, "ricoh583: reg write  reg=%x, val=%x\n",
+				reg + i, *(val + i));
+	}
+
+	ret = i2c_smbus_write_i2c_block_data(client, reg, len, val);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed writings to 0x%02x\n", reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+int ricoh583_write(struct device *dev, u8 reg, uint8_t val)
+{
+	struct ricoh583 *ricoh583 = dev_get_drvdata(dev);
+	int ret = 0;
+
+	mutex_lock(&ricoh583->io_lock);
+	ret = __ricoh583_write(to_i2c_client(dev), reg, val);
+	mutex_unlock(&ricoh583->io_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ricoh583_write);
+
+int ricoh583_bulk_writes(struct device *dev, u8 reg, u8 len, uint8_t *val)
+{
+	struct ricoh583 *ricoh583 = dev_get_drvdata(dev);
+	int ret = 0;
+
+	mutex_lock(&ricoh583->io_lock);
+	ret = __ricoh583_bulk_writes(to_i2c_client(dev), reg, len, val);
+	mutex_unlock(&ricoh583->io_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ricoh583_bulk_writes);
+
+int ricoh583_read(struct device *dev, u8 reg, uint8_t *val)
+{
+	return __ricoh583_read(to_i2c_client(dev), reg, val);
+}
+EXPORT_SYMBOL_GPL(ricoh583_read);
+
+int ricoh583_bulk_reads(struct device *dev, u8 reg, u8 len, uint8_t *val)
+{
+	return __ricoh583_bulk_reads(to_i2c_client(dev), reg, len, val);
+}
+EXPORT_SYMBOL_GPL(ricoh583_bulk_reads);
+
+int ricoh583_set_bits(struct device *dev, u8 reg, uint8_t bit_mask)
+{
+	struct ricoh583 *ricoh583 = dev_get_drvdata(dev);
+	uint8_t reg_val;
+	int ret = 0;
+
+	mutex_lock(&ricoh583->io_lock);
+
+	ret = __ricoh583_read(to_i2c_client(dev), reg, &reg_val);
+	if (ret)
+		goto out;
+
+	if ((reg_val & bit_mask) != bit_mask) {
+		reg_val |= bit_mask;
+		ret = __ricoh583_write(to_i2c_client(dev), reg, reg_val);
+	}
+out:
+	mutex_unlock(&ricoh583->io_lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ricoh583_set_bits);
+
+int ricoh583_clr_bits(struct device *dev, u8 reg, uint8_t bit_mask)
+{
+	struct ricoh583 *ricoh583 = dev_get_drvdata(dev);
+	uint8_t reg_val;
+	int ret = 0;
+
+	mutex_lock(&ricoh583->io_lock);
+
+	ret = __ricoh583_read(to_i2c_client(dev), reg, &reg_val);
+	if (ret)
+		goto out;
+
+	if (reg_val & bit_mask) {
+		reg_val &= ~bit_mask;
+		ret = __ricoh583_write(to_i2c_client(dev), reg, reg_val);
+	}
+out:
+	mutex_unlock(&ricoh583->io_lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ricoh583_clr_bits);
+
+int ricoh583_update(struct device *dev, u8 reg, uint8_t val, uint8_t mask)
+{
+	struct ricoh583 *ricoh583 = dev_get_drvdata(dev);
+	uint8_t reg_val;
+	int ret = 0;
+
+	mutex_lock(&ricoh583->io_lock);
+
+	ret = __ricoh583_read(ricoh583->client, reg, &reg_val);
+	if (ret)
+		goto out;
+
+	if ((reg_val & mask) != val) {
+		reg_val = (reg_val & ~mask) | (val & mask);
+		ret = __ricoh583_write(ricoh583->client, reg, reg_val);
+	}
+out:
+	mutex_unlock(&ricoh583->io_lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ricoh583_update);
+
+static int __ricoh583_set_ext_pwrreq1_control(struct device *dev,
+	enum ricoh583_deepsleep_control_id id,
+	enum ricoh583_ext_pwrreq_control ext_pwr, int slots)
+{
+	int ret;
+	uint8_t sleepseq_val;
+	u8 en_bit;
+	u8 slot_bit;
+
+	if (!(ext_pwr & RICOH583_EXT_PWRREQ1_CONTROL))
+		return 0;
+
+	if (id == RICOH583_DS_DC0) {
+		dev_err(dev, "PWRREQ1 is invalid control for rail %d\n", id);
+		return -EINVAL;
+	}
+
+	en_bit = deepsleep_data[id].ds_pos_bit;
+	slot_bit = en_bit + 1;
+	ret = ricoh583_read(dev, deepsleep_data[id].reg_add, &sleepseq_val);
+	if (ret < 0) {
+		dev_err(dev, "Error in reading reg 0x%x\n",
+				deepsleep_data[id].reg_add);
+		return ret;
+	}
+
+	sleepseq_val &= ~(0xF << en_bit);
+	sleepseq_val |= (1 << en_bit);
+	sleepseq_val |= ((slots & 0x7) << slot_bit);
+	ret = ricoh583_set_bits(dev, RICOH_ONOFFSEL_REG, (1 << 1));
+	if (ret < 0) {
+		dev_err(dev, "Error in updating the 0x%02x register\n",
+				RICOH_ONOFFSEL_REG);
+		return ret;
+	}
+
+	ret = ricoh583_write(dev, deepsleep_data[id].reg_add, sleepseq_val);
+	if (ret < 0) {
+		dev_err(dev, "Error in writing reg 0x%x\n",
+				deepsleep_data[id].reg_add);
+		return ret;
+	}
+
+	if (id == RICOH583_DS_LDO4) {
+		ret = ricoh583_write(dev, RICOH_SWCTL_REG, 0x1);
+		if (ret < 0)
+			dev_err(dev, "Error in writing reg 0x%x\n",
+				RICOH_SWCTL_REG);
+	}
+	return ret;
+}
+
+static int __ricoh583_set_ext_pwrreq2_control(struct device *dev,
+	enum ricoh583_deepsleep_control_id id,
+	enum ricoh583_ext_pwrreq_control ext_pwr)
+{
+	int ret;
+
+	if (!(ext_pwr & RICOH583_EXT_PWRREQ2_CONTROL))
+		return 0;
+
+	if (id != RICOH583_DS_DC0) {
+		dev_err(dev, "PWRREQ2 is invalid control for rail %d\n", id);
+		return -EINVAL;
+	}
+
+	ret = ricoh583_set_bits(dev, RICOH_ONOFFSEL_REG, (1 << 2));
+	if (ret < 0)
+		dev_err(dev, "Error in updating the ONOFFSEL 0x10 register\n");
+	return ret;
+}
+
+int ricoh583_ext_power_req_config(struct device *dev,
+	enum ricoh583_deepsleep_control_id id,
+	enum ricoh583_ext_pwrreq_control ext_pwr_req,
+	int deepsleep_slot_nr)
+{
+	if ((ext_pwr_req & EXT_PWR_REQ) == EXT_PWR_REQ)
+		return -EINVAL;
+
+	if (ext_pwr_req & RICOH583_EXT_PWRREQ1_CONTROL)
+		return __ricoh583_set_ext_pwrreq1_control(dev, id,
+				ext_pwr_req, deepsleep_slot_nr);
+
+	if (ext_pwr_req & RICOH583_EXT_PWRREQ2_CONTROL)
+		return __ricoh583_set_ext_pwrreq2_control(dev,
+			id, ext_pwr_req);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ricoh583_ext_power_req_config);
+
+static int __devinit ricoh583_ext_power_init(struct ricoh583 *ricoh583,
+	struct ricoh583_platform_data *pdata)
+{
+	int ret;
+	int i;
+	uint8_t on_off_val = 0;
+
+	/*  Clear ONOFFSEL register */
+	mutex_lock(&ricoh583->io_lock);
+	if (pdata->enable_shutdown_pin)
+		on_off_val |= 0x1;
+
+	ret = __ricoh583_write(ricoh583->client, RICOH_ONOFFSEL_REG,
+					on_off_val);
+	if (ret < 0)
+		dev_err(ricoh583->dev, "Error in writing reg %d error: "
+				"%d\n", RICOH_ONOFFSEL_REG, ret);
+
+	ret = __ricoh583_write(ricoh583->client, RICOH_SWCTL_REG, 0x0);
+	if (ret < 0)
+		dev_err(ricoh583->dev, "Error in writing reg %d error: "
+				"%d\n", RICOH_SWCTL_REG, ret);
+
+	/* Clear sleepseq register */
+	for (i = 0x21; i < 0x2B; ++i) {
+		ret = __ricoh583_write(ricoh583->client, i, 0x0);
+		if (ret < 0)
+			dev_err(ricoh583->dev, "Error in writing reg 0x%02x "
+				"error: %d\n", i, ret);
+	}
+	mutex_unlock(&ricoh583->io_lock);
+	return 0;
+}
+
+static struct i2c_client *ricoh583_i2c_client;
+int ricoh583_power_off(void)
+{
+	if (!ricoh583_i2c_client)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int ricoh583_gpio_get(struct gpio_chip *gc, unsigned offset)
+{
+	struct ricoh583 *ricoh583 = container_of(gc, struct ricoh583, gpio);
+	uint8_t val;
+	int ret;
+
+	ret = __ricoh583_read(ricoh583->client, RICOH583_GPIO_MON_IOIN, &val);
+	if (ret < 0)
+		return ret;
+
+	return ((val & (0x1 << offset)) != 0);
+}
+
+static void ricoh583_gpio_set(struct gpio_chip *chip, unsigned offset,
+			int value)
+{
+	struct ricoh583 *ricoh583 = container_of(chip, struct ricoh583, gpio);
+	if (value)
+		ricoh583_set_bits(ricoh583->dev, RICOH583_GPIO_IOOUT,
+						1 << offset);
+	else
+		ricoh583_clr_bits(ricoh583->dev, RICOH583_GPIO_IOOUT,
+						1 << offset);
+}
+
+static int ricoh583_gpio_input(struct gpio_chip *chip, unsigned offset)
+{
+	struct ricoh583 *ricoh583 = container_of(chip, struct ricoh583, gpio);
+
+	return ricoh583_clr_bits(ricoh583->dev, RICOH583_GPIO_IOSEL,
+						1 << offset);
+}
+
+static int ricoh583_gpio_output(struct gpio_chip *chip, unsigned offset,
+				int value)
+{
+	struct ricoh583 *ricoh583 = container_of(chip, struct ricoh583, gpio);
+
+	ricoh583_gpio_set(chip, offset, value);
+	return ricoh583_set_bits(ricoh583->dev, RICOH583_GPIO_IOSEL,
+						1 << offset);
+}
+
+static int ricoh583_gpio_to_irq(struct gpio_chip *chip, unsigned off)
+{
+	struct ricoh583 *ricoh583 = container_of(chip, struct ricoh583, gpio);
+
+	if ((off >= 0) && (off < 8))
+		return ricoh583->irq_base + RICOH583_IRQ_GPIO0 + off;
+
+	return -EIO;
+}
+
+static int ricoh583_gpio_request(struct gpio_chip *chip, unsigned offset)
+{
+	struct ricoh583 *ricoh583 = container_of(chip, struct ricoh583, gpio);
+	int ret;
+
+	ret = ricoh583_clr_bits(ricoh583->dev, RICOH583_GPIO_PGSEL,
+			1 << offset);
+	if (ret < 0)
+		dev_err(ricoh583->dev, "%s(): The error in writing register "
+			"0x%02x\n", __func__, RICOH583_GPIO_PGSEL);
+	return ret;
+}
+
+static void ricoh583_gpio_free(struct gpio_chip *chip, unsigned offset)
+{
+	struct ricoh583 *ricoh583 = container_of(chip, struct ricoh583, gpio);
+	int ret;
+
+	ret = ricoh583_set_bits(ricoh583->dev, RICOH583_GPIO_PGSEL,
+				1 << offset);
+	if (ret < 0)
+		dev_err(ricoh583->dev, "%s(): The error in writing register "
+			"0x%02x\n", __func__, RICOH583_GPIO_PGSEL);
+}
+
+static void __devinit ricoh583_gpio_init(struct ricoh583 *ricoh583,
+	struct ricoh583_platform_data *pdata)
+{
+	int ret;
+	int i;
+	struct ricoh583_gpio_init_data *ginit;
+
+	if (pdata->gpio_base  <= 0)
+		return;
+
+	ret = ricoh583_write(ricoh583->dev, RICOH583_GPIO_PGSEL, 0xEF);
+	if (ret < 0) {
+		dev_err(ricoh583->dev, "%s(): The error in writing register "
+			"0x%02x\n", __func__, RICOH583_GPIO_PGSEL);
+		return;
+	}
+
+	for (i = 0; i < pdata->num_gpioinit_data; ++i) {
+		ginit = &pdata->gpio_init_data[i];
+		if (!ginit->init_apply)
+			continue;
+		if (ginit->pulldn_en)
+			ret = ricoh583_set_bits(ricoh583->dev,
+					RICOH583_GPIO_PDEN, 1 << i);
+		else
+			ret = ricoh583_clr_bits(ricoh583->dev,
+					RICOH583_GPIO_PDEN, 1 << i);
+		if (ret < 0)
+			dev_err(ricoh583->dev, "Gpio %d init "
+				"pden configuration failed: %d\n", i, ret);
+
+		if (ginit->output_mode_en) {
+			if (ginit->output_val)
+				ret = ricoh583_set_bits(ricoh583->dev,
+					RICOH583_GPIO_IOOUT, 1 << i);
+			else
+				ret = ricoh583_clr_bits(ricoh583->dev,
+					RICOH583_GPIO_IOOUT, 1 << i);
+			if (!ret)
+				ret = ricoh583_set_bits(ricoh583->dev,
+					RICOH583_GPIO_IOSEL, 1 << i);
+		} else
+			ret = ricoh583_clr_bits(ricoh583->dev,
+					RICOH583_GPIO_IOSEL, 1 << i);
+
+		if (ret < 0)
+			dev_err(ricoh583->dev, "Gpio %d init "
+				"dir configuration failed: %d\n", i, ret);
+
+		ret = ricoh583_clr_bits(ricoh583->dev, RICOH583_GPIO_PGSEL,
+					1 << i);
+		if (ret < 0)
+			dev_err(ricoh583->dev, "%s(): The error in writing "
+				"register 0x%02x\n", __func__,
+				RICOH583_GPIO_PGSEL);
+	}
+
+	ricoh583->gpio.owner		= THIS_MODULE;
+	ricoh583->gpio.label		= ricoh583->client->name;
+	ricoh583->gpio.dev		= ricoh583->dev;
+	ricoh583->gpio.base		= pdata->gpio_base;
+	ricoh583->gpio.ngpio		= RICOH583_NR_GPIO;
+	ricoh583->gpio.can_sleep	= 1;
+
+	ricoh583->gpio.request	= ricoh583_gpio_request;
+	ricoh583->gpio.free	= ricoh583_gpio_free;
+	ricoh583->gpio.direction_input	= ricoh583_gpio_input;
+	ricoh583->gpio.direction_output	= ricoh583_gpio_output;
+	ricoh583->gpio.set		= ricoh583_gpio_set;
+	ricoh583->gpio.get		= ricoh583_gpio_get;
+	ricoh583->gpio.to_irq	   = ricoh583_gpio_to_irq;
+
+	ret = gpiochip_add(&ricoh583->gpio);
+	if (ret)
+		dev_warn(ricoh583->dev, "GPIO registration failed: %d\n", ret);
+}
+
+static void ricoh583_irq_lock(struct irq_data *irq_data)
+{
+	struct ricoh583 *ricoh583 = irq_data_get_irq_chip_data(irq_data);
+
+	mutex_lock(&ricoh583->irq_lock);
+}
+
+static void ricoh583_irq_unmask(struct irq_data *irq_data)
+{
+	struct ricoh583 *ricoh583 = irq_data_get_irq_chip_data(irq_data);
+	unsigned int __irq = irq_data->irq - ricoh583->irq_base;
+	const struct ricoh583_irq_data *data = &ricoh583_irqs[__irq];
+
+	ricoh583->group_irq_en[data->grp_index] |= (1 << data->grp_index);
+	if (ricoh583->group_irq_en[data->grp_index])
+		ricoh583->intc_inten_reg |= 1 << data->master_bit;
+
+	ricoh583->irq_en_reg[data->mask_reg_index] |= 1 << data->int_en_bit;
+}
+
+static void ricoh583_irq_mask(struct irq_data *irq_data)
+{
+	struct ricoh583 *ricoh583 = irq_data_get_irq_chip_data(irq_data);
+	unsigned int __irq = irq_data->irq - ricoh583->irq_base;
+	const struct ricoh583_irq_data *data = &ricoh583_irqs[__irq];
+
+	ricoh583->group_irq_en[data->grp_index] &= ~(1 << data->grp_index);
+	if (!ricoh583->group_irq_en[data->grp_index])
+		ricoh583->intc_inten_reg &= ~(1 << data->master_bit);
+
+	ricoh583->irq_en_reg[data->mask_reg_index] &= ~(1 << data->int_en_bit);
+}
+
+static void ricoh583_irq_sync_unlock(struct irq_data *irq_data)
+{
+	struct ricoh583 *ricoh583 = irq_data_get_irq_chip_data(irq_data);
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ricoh583->gpedge_reg); i++) {
+		if (ricoh583->gpedge_reg[i] != ricoh583->gpedge_cache[i]) {
+			if (!WARN_ON(__ricoh583_write(ricoh583->client,
+						    ricoh583->gpedge_add[i],
+						    ricoh583->gpedge_reg[i])))
+				ricoh583->gpedge_cache[i] =
+						ricoh583->gpedge_reg[i];
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ricoh583->irq_en_reg); i++) {
+		if (ricoh583->irq_en_reg[i] != ricoh583->irq_en_cache[i]) {
+			if (!WARN_ON(__ricoh583_write(ricoh583->client,
+					    ricoh583->irq_en_add[i],
+						    ricoh583->irq_en_reg[i])))
+				ricoh583->irq_en_cache[i] =
+						ricoh583->irq_en_reg[i];
+		}
+	}
+
+	if (ricoh583->intc_inten_reg != ricoh583->intc_inten_cache) {
+		if (!WARN_ON(__ricoh583_write(ricoh583->client,
+				RICOH583_INTC_INTEN, ricoh583->intc_inten_reg)))
+			ricoh583->intc_inten_cache = ricoh583->intc_inten_reg;
+	}
+
+	mutex_unlock(&ricoh583->irq_lock);
+}
+
+static int ricoh583_irq_set_type(struct irq_data *irq_data, unsigned int type)
+{
+	struct ricoh583 *ricoh583 = irq_data_get_irq_chip_data(irq_data);
+	unsigned int __irq = irq_data->irq - ricoh583->irq_base;
+	const struct ricoh583_irq_data *data = &ricoh583_irqs[__irq];
+	int val = 0;
+	int gpedge_index;
+	int gpedge_bit_pos;
+
+	if (data->int_type & GPIO_INT) {
+		gpedge_index = data->int_en_bit / 4;
+		gpedge_bit_pos = data->int_en_bit % 4;
+
+		if (type & IRQ_TYPE_EDGE_FALLING)
+			val |= 0x2;
+
+		if (type & IRQ_TYPE_EDGE_RISING)
+			val |= 0x1;
+
+		ricoh583->gpedge_reg[gpedge_index] &= ~(3 << gpedge_bit_pos);
+		ricoh583->gpedge_reg[gpedge_index] |= (val << gpedge_bit_pos);
+		ricoh583_irq_unmask(irq_data);
+	}
+	return 0;
+}
+
+static irqreturn_t ricoh583_irq(int irq, void *data)
+{
+	struct ricoh583 *ricoh583 = data;
+	u8 int_sts[9];
+	u8 master_int;
+	int i;
+	int ret;
+	u8 rtc_int_sts = 0;
+
+	/* Clear the status */
+	for (i = 0; i < 9; i++)
+		int_sts[i] = 0;
+
+	ret  = __ricoh583_read(ricoh583->client, RICOH583_INTC_INTMON,
+						&master_int);
+	if (ret < 0) {
+		dev_err(ricoh583->dev, "Error in reading reg 0x%02x "
+			"error: %d\n", RICOH583_INTC_INTMON, ret);
+		return IRQ_HANDLED;
+	}
+
+	for (i = 0; i < 9; ++i) {
+		if (!(master_int & ricoh583->main_int_type[i]))
+			continue;
+		ret = __ricoh583_read(ricoh583->client,
+				ricoh583->irq_mon_add[i], &int_sts[i]);
+		if (ret < 0) {
+			dev_err(ricoh583->dev, "Error in reading reg 0x%02x "
+				"error: %d\n", ricoh583->irq_mon_add[i], ret);
+			int_sts[i] = 0;
+			continue;
+		}
+
+		if (ricoh583->main_int_type[i] & RTC_INT) {
+			rtc_int_sts = 0;
+			if (int_sts[i] & 0x1)
+				rtc_int_sts |= BIT(6);
+			if (int_sts[i] & 0x2)
+				rtc_int_sts |= BIT(7);
+			if (int_sts[i] & 0x4)
+				rtc_int_sts |= BIT(0);
+			if (int_sts[i] & 0x8)
+				rtc_int_sts |= BIT(5);
+		}
+
+		ret = __ricoh583_write(ricoh583->client,
+				ricoh583->irq_clr_add[i], ~int_sts[i]);
+		if (ret < 0) {
+			dev_err(ricoh583->dev, "Error in reading reg 0x%02x "
+				"error: %d\n", ricoh583->irq_clr_add[i], ret);
+		}
+		if (ricoh583->main_int_type[i] & RTC_INT)
+			int_sts[i] = rtc_int_sts;
+	}
+
+	/* Merge gpio interrupts  for rising and falling case*/
+	int_sts[7] |= int_sts[8];
+
+	/* Call interrupt handler if enabled */
+	for (i = 0; i < RICOH583_NR_IRQS; ++i) {
+		const struct ricoh583_irq_data *data = &ricoh583_irqs[i];
+		if ((int_sts[data->mask_reg_index] & (1 << data->int_en_bit)) &&
+			(ricoh583->group_irq_en[data->master_bit] &
+					(1 << data->grp_index)))
+			handle_nested_irq(ricoh583->irq_base + i);
+	}
+	return IRQ_HANDLED;
+}
+
+static int __devinit ricoh583_irq_init(struct ricoh583 *ricoh583, int irq,
+				int irq_base)
+{
+	int i, ret;
+
+	if (!irq_base) {
+		dev_warn(ricoh583->dev, "No interrupt support on IRQ base\n");
+		return -EINVAL;
+	}
+
+	mutex_init(&ricoh583->irq_lock);
+
+	/* Initialize all locals to 0 */
+	for (i = 0; i < MAX_INTERRUPT_MASKS; i++) {
+		ricoh583->irq_en_cache[i] = 0;
+		ricoh583->irq_en_reg[i] = 0;
+	}
+	ricoh583->intc_inten_cache = 0;
+	ricoh583->intc_inten_reg = 0;
+	for (i = 0; i < 2; i++) {
+		ricoh583->gpedge_cache[i] = 0;
+		ricoh583->gpedge_reg[i] = 0;
+	}
+
+	/* Interrupt enable register */
+	ricoh583->gpedge_add[0] = RICOH583_GPIO_GPEDGE2;
+	ricoh583->gpedge_add[1] = RICOH583_GPIO_GPEDGE1;
+	ricoh583->irq_en_add[0] = RICOH583_INT_EN_SYS1;
+	ricoh583->irq_en_add[1] = RICOH583_INT_EN_SYS2;
+	ricoh583->irq_en_add[2] = RICOH583_INT_EN_DCDC;
+	ricoh583->irq_en_add[3] = RICOH583_INT_EN_RTC;
+	ricoh583->irq_en_add[4] = RICOH583_INT_EN_ADC1;
+	ricoh583->irq_en_add[5] = RICOH583_INT_EN_ADC2;
+	ricoh583->irq_en_add[6] = RICOH583_INT_EN_ADC3;
+	ricoh583->irq_en_add[7] = RICOH583_INT_EN_GPIO;
+
+	/* Interrupt status monitor register */
+	ricoh583->irq_mon_add[0] = RICOH583_INT_MON_SYS1;
+	ricoh583->irq_mon_add[1] = RICOH583_INT_MON_SYS2;
+	ricoh583->irq_mon_add[2] = RICOH583_INT_MON_DCDC;
+	ricoh583->irq_mon_add[3] = RICOH583_INT_MON_RTC;
+	ricoh583->irq_mon_add[4] = RICOH583_INT_IR_ADCL;
+	ricoh583->irq_mon_add[5] = RICOH583_INT_IR_ADCH;
+	ricoh583->irq_mon_add[6] = RICOH583_INT_IR_ADCEND;
+	ricoh583->irq_mon_add[7] = RICOH583_INT_IR_GPIOF;
+	ricoh583->irq_mon_add[8] = RICOH583_INT_IR_GPIOR;
+
+	/* Interrupt status clear register */
+	ricoh583->irq_clr_add[0] = RICOH583_INT_IR_SYS1;
+	ricoh583->irq_clr_add[1] = RICOH583_INT_IR_SYS2;
+	ricoh583->irq_clr_add[2] = RICOH583_INT_IR_DCDC;
+	ricoh583->irq_clr_add[3] = RICOH583_INT_IR_RTC;
+	ricoh583->irq_clr_add[4] = RICOH583_INT_IR_ADCL;
+	ricoh583->irq_clr_add[5] = RICOH583_INT_IR_ADCH;
+	ricoh583->irq_clr_add[6] = RICOH583_INT_IR_ADCEND;
+	ricoh583->irq_clr_add[7] = RICOH583_INT_IR_GPIOF;
+	ricoh583->irq_clr_add[8] = RICOH583_INT_IR_GPIOR;
+
+	ricoh583->main_int_type[0] = SYS_INT;
+	ricoh583->main_int_type[1] = SYS_INT;
+	ricoh583->main_int_type[2] = DCDC_INT;
+	ricoh583->main_int_type[3] = RTC_INT;
+	ricoh583->main_int_type[4] = ADC_INT;
+	ricoh583->main_int_type[5] = ADC_INT;
+	ricoh583->main_int_type[6] = ADC_INT;
+	ricoh583->main_int_type[7] = GPIO_INT;
+	ricoh583->main_int_type[8] = GPIO_INT;
+
+	/* Initailize all int register to 0 */
+	for (i = 0; i < MAX_INTERRUPT_MASKS; i++)  {
+		ret = __ricoh583_write(ricoh583->client,
+				ricoh583->irq_en_add[i],
+				ricoh583->irq_en_reg[i]);
+		if (ret < 0)
+			dev_err(ricoh583->dev, "Error in writing reg 0x%02x "
+				"error: %d\n", ricoh583->irq_en_add[i], ret);
+	}
+
+	for (i = 0; i < 2; i++)  {
+		ret = __ricoh583_write(ricoh583->client,
+				ricoh583->gpedge_add[i],
+				ricoh583->gpedge_reg[i]);
+		if (ret < 0)
+			dev_err(ricoh583->dev, "Error in writing reg 0x%02x "
+				"error: %d\n", ricoh583->gpedge_add[i], ret);
+	}
+
+	ret = __ricoh583_write(ricoh583->client, RICOH583_INTC_INTEN, 0x0);
+	if (ret < 0)
+		dev_err(ricoh583->dev, "Error in writing reg 0x%02x "
+				"error: %d\n", RICOH583_INTC_INTEN, ret);
+
+	/* Clear all interrupts in case they woke up active. */
+	for (i = 0; i < 9; i++)  {
+		ret = __ricoh583_write(ricoh583->client,
+					ricoh583->irq_clr_add[i], 0);
+		if (ret < 0)
+			dev_err(ricoh583->dev, "Error in writing reg 0x%02x "
+				"error: %d\n", ricoh583->irq_clr_add[i], ret);
+	}
+
+	ricoh583->irq_base = irq_base;
+	ricoh583->irq_chip.name = "ricoh583";
+	ricoh583->irq_chip.irq_mask = ricoh583_irq_mask;
+	ricoh583->irq_chip.irq_unmask = ricoh583_irq_unmask;
+	ricoh583->irq_chip.irq_bus_lock = ricoh583_irq_lock;
+	ricoh583->irq_chip.irq_bus_sync_unlock = ricoh583_irq_sync_unlock;
+	ricoh583->irq_chip.irq_set_type = ricoh583_irq_set_type;
+
+	for (i = 0; i < RICOH583_NR_IRQS; i++) {
+		int __irq = i + ricoh583->irq_base;
+		irq_set_chip_data(__irq, ricoh583);
+		irq_set_chip_and_handler(__irq, &ricoh583->irq_chip,
+					 handle_simple_irq);
+		irq_set_nested_thread(__irq, 1);
+#ifdef CONFIG_ARM
+		set_irq_flags(__irq, IRQF_VALID);
+#endif
+	}
+
+	ret = request_threaded_irq(irq, NULL, ricoh583_irq, IRQF_ONESHOT,
+				   "ricoh583", ricoh583);
+	if (ret < 0)
+		dev_err(ricoh583->dev, "Error in registering interrupt "
+				"error: %d\n", ret);
+	if (!ret) {
+		device_init_wakeup(ricoh583->dev, 1);
+		enable_irq_wake(irq);
+	}
+	return ret;
+}
+
+static int ricoh583_remove_subdev(struct device *dev, void *unused)
+{
+	platform_device_unregister(to_platform_device(dev));
+	return 0;
+}
+
+static int ricoh583_remove_subdevs(struct ricoh583 *ricoh583)
+{
+	return device_for_each_child(ricoh583->dev, NULL,
+				     ricoh583_remove_subdev);
+}
+
+static int __devinit ricoh583_add_subdevs(struct ricoh583 *ricoh583,
+				struct ricoh583_platform_data *pdata)
+{
+	struct ricoh583_subdev_info *subdev;
+	struct platform_device *pdev;
+	int i, ret = 0;
+
+	for (i = 0; i < pdata->num_subdevs; i++) {
+		subdev = &pdata->subdevs[i];
+
+		pdev = platform_device_alloc(subdev->name, subdev->id);
+
+		pdev->dev.parent = ricoh583->dev;
+		pdev->dev.platform_data = subdev->platform_data;
+
+		ret = platform_device_add(pdev);
+		if (ret)
+			goto failed;
+	}
+	return 0;
+
+failed:
+	ricoh583_remove_subdevs(ricoh583);
+	return ret;
+}
+
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+static void print_regs(const char *header, struct seq_file *s,
+		struct i2c_client *client, int start_offset,
+		int end_offset)
+{
+	uint8_t reg_val;
+	int i;
+	int ret;
+
+	seq_printf(s, "%s\n", header);
+	for (i = start_offset; i <= end_offset; ++i) {
+		ret = __ricoh583_read(client, i, &reg_val);
+		if (ret >= 0)
+			seq_printf(s, "Reg 0x%02x Value 0x%02x\n", i, reg_val);
+	}
+	seq_printf(s, "------------------\n");
+}
+
+static int dbg_tps_show(struct seq_file *s, void *unused)
+{
+	struct ricoh583 *tps = s->private;
+	struct i2c_client *client = tps->client;
+
+	seq_printf(s, "RICOH583 Registers\n");
+	seq_printf(s, "------------------\n");
+
+	print_regs("System Regs",	       s, client, 0x0, 0xF);
+	print_regs("Power Control Regs",	s, client, 0x10, 0x2B);
+	print_regs("DCDC1 Regs",		s, client, 0x30, 0x43);
+	print_regs("DCDC1 Regs",		s, client, 0x60, 0x63);
+	print_regs("LDO   Regs",		s, client, 0x50, 0x5F);
+	print_regs("LDO   Regs",		s, client, 0x64, 0x6D);
+	print_regs("ADC  Regs",		 s, client, 0x70, 0x72);
+	print_regs("ADC  Regs",		 s, client, 0x74, 0x8B);
+	print_regs("ADC  Regs",		 s, client, 0x90, 0x96);
+	print_regs("GPIO Regs",		 s, client, 0xA0, 0xAC);
+	print_regs("INTC Regs",		 s, client, 0xAD, 0xAF);
+	print_regs("RTC  Regs",		 s, client, 0xE0, 0xEE);
+	print_regs("RTC  Regs",		 s, client, 0xF0, 0xF4);
+	return 0;
+}
+
+static int dbg_tps_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dbg_tps_show, inode->i_private);
+}
+
+static const struct file_operations debug_fops = {
+	.open	   = dbg_tps_open,
+	.read	   = seq_read,
+	.llseek	 = seq_lseek,
+	.release	= single_release,
+};
+static void __init ricoh583_debuginit(struct ricoh583 *tps)
+{
+	(void)debugfs_create_file("ricoh583", S_IRUGO, NULL,
+			tps, &debug_fops);
+}
+#else
+static void __init ricoh583_debuginit(struct ricoh583 *tpsi)
+{
+	return;
+}
+#endif
+
+static int ricoh583_i2c_probe(struct i2c_client *i2c,
+			      const struct i2c_device_id *id)
+{
+	struct ricoh583 *ricoh583;
+	struct ricoh583_platform_data *pdata = i2c->dev.platform_data;
+	int ret;
+
+	ricoh583 = kzalloc(sizeof(struct ricoh583), GFP_KERNEL);
+	if (ricoh583 == NULL)
+		return -ENOMEM;
+
+	ricoh583->client = i2c;
+	ricoh583->dev = &i2c->dev;
+	i2c_set_clientdata(i2c, ricoh583);
+
+	mutex_init(&ricoh583->io_lock);
+
+	ret = ricoh583_ext_power_init(ricoh583, pdata);
+	if (ret < 0)
+		goto err_irq_init;
+
+	if (i2c->irq) {
+		ret = ricoh583_irq_init(ricoh583, i2c->irq, pdata->irq_base);
+		if (ret) {
+			dev_err(&i2c->dev, "IRQ init failed: %d\n", ret);
+			goto err_irq_init;
+		}
+	}
+
+	ret = ricoh583_add_subdevs(ricoh583, pdata);
+	if (ret) {
+		dev_err(&i2c->dev, "add devices failed: %d\n", ret);
+		goto err_add_devs;
+	}
+
+	ricoh583_gpio_init(ricoh583, pdata);
+
+	ricoh583_debuginit(ricoh583);
+
+	ricoh583_i2c_client = i2c;
+	return 0;
+
+err_add_devs:
+	if (i2c->irq)
+		free_irq(i2c->irq, ricoh583);
+err_irq_init:
+	kfree(ricoh583);
+	return ret;
+}
+
+static int  __devexit ricoh583_i2c_remove(struct i2c_client *i2c)
+{
+	struct ricoh583 *ricoh583 = i2c_get_clientdata(i2c);
+
+	if (i2c->irq)
+		free_irq(i2c->irq, ricoh583);
+
+	ricoh583_remove_subdevs(ricoh583);
+	kfree(ricoh583);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int ricoh583_i2c_suspend(struct i2c_client *i2c, pm_message_t state)
+{
+	if (i2c->irq)
+		disable_irq(i2c->irq);
+	return 0;
+}
+
+
+static int ricoh583_i2c_resume(struct i2c_client *i2c)
+{
+	if (i2c->irq)
+		enable_irq(i2c->irq);
+	return 0;
+}
+
+#endif
+
+static const struct i2c_device_id ricoh583_i2c_id[] = {
+	{"ricoh583", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, ricoh583_i2c_id);
+
+static struct i2c_driver ricoh583_i2c_driver = {
+	.driver = {
+		   .name = "ricoh583",
+		   .owner = THIS_MODULE,
+		   },
+	.probe = ricoh583_i2c_probe,
+	.remove = __devexit_p(ricoh583_i2c_remove),
+#ifdef CONFIG_PM
+	.suspend = ricoh583_i2c_suspend,
+	.resume = ricoh583_i2c_resume,
+#endif
+	.id_table = ricoh583_i2c_id,
+};
+
+
+static int __init ricoh583_i2c_init(void)
+{
+	int ret = -ENODEV;
+	ret = i2c_add_driver(&ricoh583_i2c_driver);
+	if (ret != 0)
+		pr_err("Failed to register I2C driver: %d\n", ret);
+
+	return ret;
+}
+
+subsys_initcall(ricoh583_i2c_init);
+
+static void __exit ricoh583_i2c_exit(void)
+{
+	i2c_del_driver(&ricoh583_i2c_driver);
+}
+
+module_exit(ricoh583_i2c_exit);
+
+MODULE_DESCRIPTION("RICOH583 multi-function core driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/stmpe.c b/drivers/mfd/stmpe.c
index 2963689cf45c..0c556fa65dd2 100644
--- a/drivers/mfd/stmpe.c
+++ b/drivers/mfd/stmpe.c
@@ -350,6 +350,25 @@ static struct mfd_cell stmpe_ts_cell = {
 };
 
 /*
+ * ADC (STMPE811)
+ */
+
+static struct resource stmpe_adc_resources[] = {
+	{
+		.name	= "STMPE_ADC",
+		.start	= 0,
+		.end	= 0,
+		.flags	= IORESOURCE_IRQ,
+	},
+};
+
+static struct mfd_cell stmpe_adc_cell = {
+	.name		= "stmpe-adc",
+	.resources	= stmpe_adc_resources,
+	.num_resources	= ARRAY_SIZE(stmpe_adc_resources),
+};
+
+/*
  * STMPE811
  */
 
@@ -381,6 +400,11 @@ static struct stmpe_variant_block stmpe811_blocks[] = {
 		.irq	= STMPE811_IRQ_TOUCH_DET,
 		.block	= STMPE_BLOCK_TOUCHSCREEN,
 	},
+	{
+		.cell	= &stmpe_adc_cell,
+		.irq	= STMPE811_IRQ_ADC,
+		.block	= STMPE_BLOCK_ADC,
+	},
 };
 
 static int stmpe811_enable(struct stmpe *stmpe, unsigned int blocks,
diff --git a/drivers/mfd/tlv320aic3262-core.c b/drivers/mfd/tlv320aic3262-core.c
new file mode 100644
index 000000000000..7b61c7497a45
--- /dev/null
+++ b/drivers/mfd/tlv320aic3262-core.c
@@ -0,0 +1,885 @@
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/spi/spi.h>
+#include <linux/delay.h>
+#include <linux/mfd/core.h>
+#include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <linux/regulator/machine.h>
+#include <linux/gpio.h>
+
+#include <linux/mfd/tlv320aic3262-core.h>
+#include <linux/mfd/tlv320aic3262-registers.h>
+#define DEBUG
+struct aic3262_gpio {
+	unsigned int reg;
+	u8 mask;
+	u8 shift;
+};
+struct aic3262_gpio  aic3262_gpio_control[] = {
+	{
+		.reg = AIC3262_GPIO1_IO_CNTL,
+		.mask = AIC3262_GPIO_D6_D2,
+		.shift = AIC3262_GPIO_D2_SHIFT,
+	},
+	{
+		.reg = AIC3262_GPIO2_IO_CNTL,
+		.mask = AIC3262_GPIO_D6_D2,
+		.shift = AIC3262_GPIO_D2_SHIFT,
+	},
+	{
+		.reg = AIC3262_GPI1_EN,
+		.mask = AIC3262_GPI1_D2_D1,
+		.shift = AIC3262_GPIO_D1_SHIFT,
+	},
+	{
+		.reg = AIC3262_GPI2_EN,
+		.mask = AIC3262_GPI2_D5_D4,
+		.shift = AIC3262_GPIO_D4_SHIFT,
+	},
+	{
+		.reg = AIC3262_GPO1_OUT_CNTL,
+		.mask = AIC3262_GPO1_D4_D1,
+		.shift = AIC3262_GPIO_D1_SHIFT,
+	},
+};
+static int aic3262_read(struct aic3262 *aic3262, unsigned int reg,
+		       int bytes, void *dest)
+{
+	int ret;
+	int i;
+	u8 *buf = dest;
+
+	BUG_ON(bytes <= 0);
+
+	ret = aic3262->read_dev(aic3262, reg, bytes, dest);
+	if (ret < 0)
+		return ret;
+
+	for (i = 0; i < bytes ; i++) {
+		dev_vdbg(aic3262->dev, "Read %04x from R%d(0x%x)\n",
+			 buf[i], reg + i, reg + i);
+	}
+
+	return ret;
+}
+
+/**
+ * aic3262_reg_read: Read a single TLV320AIC3262 register.
+ *
+ * @aic3262: Device to read from.
+ * @reg: Register to read.
+ */
+int aic3262_reg_read(struct aic3262 *aic3262, unsigned int reg)
+{
+	unsigned char val;
+	int ret;
+
+	mutex_lock(&aic3262->io_lock);
+
+	ret = aic3262_read(aic3262, reg, 1, &val);
+
+	mutex_unlock(&aic3262->io_lock);
+
+	if (ret < 0)
+		return ret;
+	else
+		return val;
+}
+EXPORT_SYMBOL_GPL(aic3262_reg_read);
+
+/**
+ * aic3262_bulk_read: Read multiple TLV320AIC3262 registers
+ *
+ * @aic3262: Device to read from
+ * @reg: First register
+ * @count: Number of registers
+ * @buf: Buffer to fill.  The data will be returned big endian.
+ */
+int aic3262_bulk_read(struct aic3262 *aic3262, unsigned int reg,
+		     int count, u8 *buf)
+{
+	int ret;
+
+	mutex_lock(&aic3262->io_lock);
+
+	ret = aic3262_read(aic3262, reg, count, buf);
+
+	mutex_unlock(&aic3262->io_lock);
+
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(aic3262_bulk_read);
+
+static int aic3262_write(struct aic3262 *aic3262, unsigned int reg,
+			int bytes, const void *src)
+{
+	const u8 *buf = src;
+	int i;
+
+	BUG_ON(bytes <= 0);
+
+	for (i = 0; i < bytes ; i++) {
+		dev_vdbg(aic3262->dev, "Write %04x to R%d(0x%x)\n",
+			buf[i], reg + i, reg + i);
+	}
+
+	return aic3262->write_dev(aic3262, reg, bytes, src);
+}
+
+/**
+ * aic3262_reg_write: Write a single TLV320AIC3262 register.
+ *
+ * @aic3262: Device to write to.
+ * @reg: Register to write to.
+ * @val: Value to write.
+ */
+int aic3262_reg_write(struct aic3262 *aic3262, unsigned int reg,
+		     unsigned char val)
+{
+	int ret;
+
+
+	mutex_lock(&aic3262->io_lock);
+
+	dev_dbg(aic3262->dev, "w 30 %x %x", reg, val);
+	ret = aic3262_write(aic3262, reg, 1, &val);
+
+	mutex_unlock(&aic3262->io_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(aic3262_reg_write);
+
+/**
+ * aic3262_bulk_write: Write multiple TLV320AIC3262 registers
+ *
+ * @aic3262: Device to write to
+ * @reg: First register
+ * @count: Number of registers
+ * @buf: Buffer to write from.  Data must be big-endian formatted.
+ */
+int aic3262_bulk_write(struct aic3262 *aic3262, unsigned int reg,
+		      int count, const u8 *buf)
+{
+	int ret;
+
+	mutex_lock(&aic3262->io_lock);
+
+	ret = aic3262_write(aic3262, reg, count, buf);
+
+	mutex_unlock(&aic3262->io_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(aic3262_bulk_write);
+
+/**
+ * aic3262_set_bits: Set the value of a bitfield in a TLV320AIC3262 register
+ *
+ * @aic3262: Device to write to.
+ * @reg: Register to write to.
+ * @mask: Mask of bits to set.
+ * @val: Value to set (unshifted)
+ */
+int aic3262_set_bits(struct aic3262 *aic3262, unsigned int reg,
+		    unsigned char mask, unsigned char val)
+{
+	int ret;
+	u8 r;
+
+	mutex_lock(&aic3262->io_lock);
+
+	ret = aic3262_read(aic3262, reg, 1, &r);
+	if (ret < 0)
+		goto out;
+
+
+	r &= ~mask;
+	r |= (val & mask);
+
+	dev_dbg(aic3262->dev, "w 30 %x %x", reg, r);
+	ret = aic3262_write(aic3262, reg, 1, &r);
+
+out:
+	mutex_unlock(&aic3262->io_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(aic3262_set_bits);
+
+/**
+ * aic3262_wait_bits: wait for a value of a bitfield in a TLV320AIC3262 register
+ *
+ * @aic3262: Device to write to.
+ * @reg: Register to write to.
+ * @mask: Mask of bits to set.
+ * @val: Value to set (unshifted)
+ * @sleep: mdelay value in each iteration in milliseconds
+ * @count: iteration count for timeout
+ */
+int aic3262_wait_bits(struct aic3262 *aic3262, unsigned int reg,
+		unsigned char mask, unsigned char val, int sleep, int counter)
+{
+	int status;
+	int timeout =  sleep*counter;
+
+	status = aic3262_reg_read(aic3262, reg);
+	while (((status & mask) != val) && counter) {
+		mdelay(sleep);
+		status = aic3262_reg_read(aic3262, reg);
+		counter--;
+	};
+	if (!counter)
+		dev_err(aic3262->dev,
+		 "wait_bits timedout (%d millisecs). lastval 0x%x\n",
+		 timeout, status);
+	return counter;
+}
+EXPORT_SYMBOL_GPL(aic3262_wait_bits);
+
+/* to be changed -- Mukund*/
+static struct resource aic3262_codec_resources[] = {
+	{
+		.start = AIC3262_IRQ_HEADSET_DETECT,
+		.end   = AIC3262_IRQ_SPEAKER_OVER_TEMP,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static struct resource aic3262_gpio_resources[] = {
+	{
+		.start = AIC3262_GPIO1,
+		.end   = AIC3262_GPO1,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static struct mfd_cell aic3262_devs[] = {
+	{
+		.name = "tlv320aic3262-codec",
+		.num_resources = ARRAY_SIZE(aic3262_codec_resources),
+		.resources = aic3262_codec_resources,
+	},
+
+	{
+		.name = "tlv320aic3262-gpio",
+		.num_resources = ARRAY_SIZE(aic3262_gpio_resources),
+		.resources = aic3262_gpio_resources,
+		.pm_runtime_no_callbacks = true,
+	},
+};
+
+
+#ifdef CONFIG_PM
+static int aic3262_suspend(struct device *dev)
+{
+	struct aic3262 *aic3262 = dev_get_drvdata(dev);
+
+	aic3262->suspended = true;
+
+	return 0;
+}
+
+static int aic3262_resume(struct device *dev)
+{
+	struct aic3262 *aic3262 = dev_get_drvdata(dev);
+
+
+	aic3262->suspended = false;
+
+	return 0;
+}
+
+static UNIVERSAL_DEV_PM_OPS(aic3262_pm_ops, aic3262_suspend, aic3262_resume,
+				NULL);
+#endif
+
+
+/*
+ * Instantiate the generic non-control parts of the device.
+ */
+static int aic3262_device_init(struct aic3262 *aic3262, int irq)
+{
+	struct aic3262_pdata *pdata = aic3262->dev->platform_data;
+	const char *devname;
+	int ret, i;
+	u8 revID, pgID;
+	unsigned int naudint = 0;
+	u8 resetVal = 1;
+
+	mutex_init(&aic3262->io_lock);
+	dev_set_drvdata(aic3262->dev, aic3262);
+	if (pdata) {
+		if (pdata->gpio_reset) {
+			ret = gpio_request(pdata->gpio_reset,
+				"aic3262-reset-pin");
+			if (ret != 0) {
+				dev_err(aic3262->dev,
+				"Failed to reset aic3262 using gpio %d\n",
+				pdata->gpio_reset);
+				goto err_return;
+			}
+			gpio_direction_output(pdata->gpio_reset, 1);
+			mdelay(5);
+			gpio_direction_output(pdata->gpio_reset, 0);
+			mdelay(5);
+			gpio_direction_output(pdata->gpio_reset, 1);
+			mdelay(5);
+		}
+	}
+
+
+	/* run the codec through software reset */
+	ret = aic3262_reg_write(aic3262, AIC3262_RESET_REG, resetVal);
+	if (ret < 0) {
+		dev_err(aic3262->dev, "Could not write to AIC3262 register\n");
+		goto err_return;
+	}
+
+	mdelay(10);
+
+	ret = aic3262_reg_read(aic3262, AIC3262_REV_PG_ID);
+	if (ret < 0) {
+		dev_err(aic3262->dev, "Failed to read ID register\n");
+		goto err_return;
+	}
+	revID = (ret & AIC3262_REV_MASK) >> AIC3262_REV_SHIFT;
+	pgID = (ret & AIC3262_PG_MASK) >> AIC3262_PG_SHIFT;
+	switch (revID) {
+	case 3:
+		devname = "TLV320AIC3262";
+		if (aic3262->type != TLV320AIC3262)
+			dev_warn(aic3262->dev, "Device registered as type %d\n",
+				 aic3262->type);
+		aic3262->type = TLV320AIC3262;
+		break;
+	case 1:
+		devname = "TLV320AIC3262";
+		if (aic3262->type != TLV320AIC3262)
+			dev_warn(aic3262->dev, "Device registered as type %d\n",
+				 aic3262->type);
+		aic3262->type = TLV320AIC3262;
+		break;
+
+	default:
+		dev_err(aic3262->dev, "Device is not a TLV320AIC3262, ID is %x\n",
+			ret);
+		ret = -EINVAL;
+		goto err_return;
+
+	}
+
+	dev_info(aic3262->dev, "%s revision %c\n", devname, 'D' + ret);
+
+
+	if (pdata) {
+		if (pdata->gpio_irq == 1) {
+			naudint = gpio_to_irq(pdata->naudint_irq);
+			gpio_request(pdata->naudint_irq, "aic3262-gpio-irq");
+			gpio_direction_input(pdata->naudint_irq);
+		} else
+			naudint = pdata->naudint_irq;
+
+		aic3262->irq = naudint;
+		aic3262->irq_base = pdata->irq_base;
+		for (i = 0; i < AIC3262_NUM_GPIO; i++) {
+			if (pdata->gpio[i].used) {
+				/* Direction is input */
+				if (pdata->gpio[i].in) {
+					/* set direction to input for GPIO,
+					and enable for GPI */
+					aic3262_set_bits(aic3262,
+						aic3262_gpio_control[i].reg,
+						aic3262_gpio_control[i].mask,
+						0x1 <<
+						aic3262_gpio_control[i].shift);
+
+					if (pdata->gpio[i].in_reg)
+						/* Some input modes, does not
+						need extra registers to be
+						written */
+						aic3262_set_bits(aic3262,
+							pdata->gpio[i].in_reg,
+							pdata->gpio[i].
+							in_reg_bitmask,
+							pdata->gpio[i].value <<
+							pdata->gpio[i].
+							in_reg_shift);
+				} else {
+					/* Direction si output */
+					aic3262_set_bits(aic3262,
+						aic3262_gpio_control[i].reg,
+						aic3262_gpio_control[i].mask,
+						pdata->gpio[i].value <<
+						aic3262_gpio_control[i].shift);
+				}
+			} else
+				aic3262_set_bits(aic3262,
+					aic3262_gpio_control[i].reg,
+					aic3262_gpio_control[i].mask, 0x0);
+		}
+	}
+
+	if (naudint) {
+		/* codec interrupt */
+		ret = aic3262_irq_init(aic3262);
+		if (ret)
+			goto err_irq;
+	}
+
+	ret = mfd_add_devices(aic3262->dev, -1,
+			      aic3262_devs, ARRAY_SIZE(aic3262_devs),
+			      NULL, 0);
+	if (ret != 0) {
+		dev_err(aic3262->dev, "Failed to add children: %d\n", ret);
+		goto err_irq;
+	}
+
+	pm_runtime_enable(aic3262->dev);
+	pm_runtime_resume(aic3262->dev);
+
+	return 0;
+
+err_irq:
+	aic3262_irq_exit(aic3262);
+err_return:
+	kfree(aic3262);
+	return ret;
+}
+
+static void aic3262_device_exit(struct aic3262 *aic3262)
+{
+	pm_runtime_disable(aic3262->dev);
+	mfd_remove_devices(aic3262->dev);
+	aic3262_irq_exit(aic3262);
+	kfree(aic3262);
+}
+
+#if defined(CONFIG_I2C) || defined(CONFIG_I2C_MODULE)
+
+
+static int aic3262_i2c_read_device(struct aic3262 *aic3262, unsigned int reg,
+				  int bytes, void *dest)
+{
+	struct i2c_client *i2c = aic3262->control_data;
+	union aic326x_reg_union *aic_reg = (union aic326x_reg_union *) &reg;
+	char *value;
+	int ret;
+	u8 buf[2];
+	u8 page, book, offset;
+	page = aic_reg->aic326x_register.page;
+	book = aic_reg->aic326x_register.book;
+	offset = aic_reg->aic326x_register.offset;
+	if (aic3262->book_no != book) {
+		/* We should change to page 0.
+		Change the book by writing to offset 127 of page 0
+		Change the page back to whatever was set before change page */
+		buf[0] = 0x0;
+		buf[1] = 0x0;
+		ret = i2c_master_send(i2c, (unsigned char *)buf, 2);
+		if (ret < 0)
+			return ret;
+		if (ret != 2)
+			return -EIO;
+		buf[0] = 127;
+		buf[1] = book;
+		ret = i2c_master_send(i2c, (unsigned char *)buf, 2);
+		if (ret < 0)
+			return ret;
+		if (ret != 2)
+			return -EIO;
+		aic3262->book_no = book;
+		aic3262->page_no = 0x0;
+	}
+
+	if (aic3262->page_no != page) {
+		buf[0] = 0x0;
+		buf[1] = page;
+		ret = i2c_master_send(i2c, (unsigned char *) buf, 2);
+
+		if (ret < 0)
+			return ret;
+		if (ret != 2)
+			return -EIO;
+		aic3262->page_no = page;
+	}
+
+	/* Send the required offset */
+	buf[0] = offset ;
+	ret = i2c_master_send(i2c, (unsigned char *)buf, 1);
+	if (ret < 0)
+		return ret;
+	if (ret != 1)
+		return -EIO;
+
+	ret = i2c_master_recv(i2c, dest, bytes);
+	value = dest;
+	if (ret < 0)
+		return ret;
+	if (ret != bytes)
+		return -EIO;
+	return ret;
+}
+
+static int aic3262_i2c_write_device(struct aic3262 *aic3262, unsigned int reg,
+				   int bytes, const void *src)
+{
+	struct i2c_client *i2c = aic3262->control_data;
+	int ret;
+
+	union aic326x_reg_union *aic_reg = (union aic326x_reg_union *) &reg;
+
+	u8 buf[2];
+	u8 write_buf[bytes + 1];
+	u8 page, book, offset;
+	page = aic_reg->aic326x_register.page;
+	book = aic_reg->aic326x_register.book;
+	offset = aic_reg->aic326x_register.offset;
+	if (aic3262->book_no != book) {
+		/* We should change to page 0.
+		Change the book by writing to offset 127 of page 0
+		Change the page back to whatever was set before change page*/
+		buf[0] = 0x0;
+		buf[1] = 0x0;
+		ret = i2c_master_send(i2c, (unsigned char *)buf, 2);
+
+		if (ret < 0)
+			return ret;
+		if (ret != 2)
+			return -EIO;
+		buf[0] = 127;
+		buf[1] = book;
+		ret = i2c_master_send(i2c, (unsigned char *)buf, 2);
+		if (ret < 0)
+			return ret;
+		if (ret != 2)
+			return -EIO;
+		aic3262->book_no = book;
+		aic3262->page_no = 0x0;
+	}
+
+	if (aic3262->page_no != page) {
+		buf[0] = 0x0;
+		buf[1] = page;
+		ret = i2c_master_send(i2c, (unsigned char *) buf, 2);
+		if (ret < 0)
+			return ret;
+		if (ret != 2)
+			return -EIO;
+		aic3262->page_no = page;
+	}
+	write_buf[0] = offset;
+	memcpy(&write_buf[1], src, bytes);
+	ret = i2c_master_send(i2c, write_buf, bytes + 1);
+	if (ret < 0)
+		return ret;
+	if (ret != (bytes + 1))
+		return -EIO;
+
+	return 0;
+}
+
+static int aic3262_i2c_probe(struct i2c_client *i2c,
+			    const struct i2c_device_id *id)
+{
+	struct aic3262 *aic3262;
+
+	aic3262 = kzalloc(sizeof(struct aic3262), GFP_KERNEL);
+	if (aic3262 == NULL)
+		return -ENOMEM;
+
+	i2c_set_clientdata(i2c, aic3262);
+	aic3262->dev = &i2c->dev;
+	aic3262->control_data = i2c;
+	aic3262->read_dev = aic3262_i2c_read_device;
+	aic3262->write_dev = aic3262_i2c_write_device;
+	aic3262->type = id->driver_data;
+	aic3262->book_no = 255;
+	aic3262->page_no = 255;
+
+	return aic3262_device_init(aic3262, i2c->irq);
+}
+
+static int aic3262_i2c_remove(struct i2c_client *i2c)
+{
+	struct aic3262 *aic3262 = i2c_get_clientdata(i2c);
+
+	aic3262_device_exit(aic3262);
+
+	return 0;
+}
+
+static const struct i2c_device_id aic3262_i2c_id[] = {
+	{ "tlv320aic3262", TLV320AIC3262 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, aic3262_i2c_id);
+
+
+static struct i2c_driver aic3262_i2c_driver = {
+	.driver = {
+		.name = "tlv320aic3262",
+		.owner = THIS_MODULE,
+		.pm = &aic3262_pm_ops,
+	},
+	.probe = aic3262_i2c_probe,
+	.remove = aic3262_i2c_remove,
+	.id_table = aic3262_i2c_id,
+};
+
+static int __init aic3262_i2c_init(void)
+{
+	int ret;
+	ret = i2c_add_driver(&aic3262_i2c_driver);
+	if (ret != 0)
+		pr_err("Failed to register aic3262 I2C driver: %d\n", ret);
+
+	return ret;
+}
+module_init(aic3262_i2c_init);
+
+static void __exit aic3262_i2c_exit(void)
+{
+	i2c_del_driver(&aic3262_i2c_driver);
+}
+module_exit(aic3262_i2c_exit);
+#endif
+#if defined(CONFIG_SPI_MASTER)
+/* TODO: UGLY
+ * NVidia's CS differs from what TI requires on the SPI bus. So before
+ * we do any write/read we pull down the CS gpio :(
+ * The problem is in spi_read.
+ * Can we set the flag spi_transfer.cs_change during read so that CS is
+ * pulled low until the next transaction occurs
+ * (spi_read requires a spi_write followed by spi_read)
+ */
+#include <linux/gpio.h>
+#include "../../../arch/arm/mach-tegra/gpio-names.h"
+#include <linux/delay.h>
+#define SPI_CS  TEGRA_GPIO_PX3
+#define CS(a)  gpio_set_value(SPI_CS, a)
+void nvidia_spi_cs_en(bool stop)
+{
+	if (stop) {
+		CS(1);
+		udelay(1);
+	} else {
+		CS(0);
+		udelay(1);
+	}
+	return;
+}
+static int aic3262_spi_read_device(struct aic3262 *aic3262, unsigned int reg,
+				  int bytes, void *dest)
+{
+	struct spi_device *spi = aic3262->control_data;
+	union aic326x_reg_union *aic_reg = (union aic326x_reg_union *) &reg;
+	u8 *write_read_buf;
+	unsigned int i;
+	unsigned int time;
+	unsigned int last_count;
+	unsigned int spi_read_bufsize = max(32, SMP_CACHE_BYTES)-1;
+	struct spi_message	message;
+	struct spi_transfer	x[2];
+	int ret;
+	u8 buf[2];
+	u8 page, book, offset;
+	page = aic_reg->aic326x_register.page;
+	book = aic_reg->aic326x_register.book;
+	offset = aic_reg->aic326x_register.offset;
+	if (aic3262->book_no != book) {
+		/* We should change to page 0.
+		Change the book by writing to offset 127 of page 0
+		Change the page back to whatever was set before change page */
+
+		buf[0] = 0x0;
+		buf[1] = 0x0;
+
+		nvidia_spi_cs_en(0);
+		ret = spi_write(spi, (unsigned char *)buf, 2);
+		nvidia_spi_cs_en(1);
+
+		if (ret < 0)
+			return ret;
+		buf[0] = (127 << 1) ;
+		buf[1] = book;
+
+		nvidia_spi_cs_en(0);
+		ret = spi_write(spi, (unsigned char *)buf, 2);
+		nvidia_spi_cs_en(1);
+
+		if (ret < 0)
+			return ret;
+		aic3262->book_no = book;
+		aic3262->page_no = 0x0;
+	}
+
+	if (aic3262->page_no != page) {
+		buf[0] = 0x0;
+		buf[1] = page;
+
+		nvidia_spi_cs_en(0);
+		ret = spi_write(spi, (unsigned char *)buf, 2);
+		nvidia_spi_cs_en(1);
+
+		if (ret < 0)
+			return ret;
+		aic3262->page_no = page;
+	}
+
+	buf[0] = (offset << 1) | (0x01) ;
+	memset(x, 0, sizeof x);
+	spi_message_init(&message);
+	x[0].len = 1;
+	x[0].tx_buf = buf;
+	x[1].len = bytes;
+	x[1].rx_buf = dest ;
+
+	spi_message_add_tail(&x[0], &message);
+	spi_message_add_tail(&x[1], &message);
+
+	nvidia_spi_cs_en(0);
+	ret = spi_sync(spi, &message);
+	nvidia_spi_cs_en(1);
+	if (ret < 0)
+		return ret;
+
+	return bytes;
+
+}
+/* NVidia's CS differs from what TI requires on the SPI bus. So before
+ * we do any write/read we pull down the CS gpio :(
+ */
+static int aic3262_spi_write_device(struct aic3262 *aic3262, unsigned int reg,
+				   int bytes, const void *src)
+{
+	struct spi_device *spi = aic3262->control_data;
+	int ret;
+
+	union aic326x_reg_union *aic_reg = (union aic326x_reg_union *) &reg;
+
+	u8 buf[2];
+	u8 write_buf[bytes + 1];
+	u8 page, book, offset;
+	page = aic_reg->aic326x_register.page;
+	book = aic_reg->aic326x_register.book;
+	offset = aic_reg->aic326x_register.offset;
+	if (aic3262->book_no != book) {
+		/* We should change to page 0.
+		Change the book by writing to offset 127 of page 0
+		Change the page back to whatever was set before change page */
+
+		buf[0] = 0x0;
+		buf[1] = 0x0;
+
+		nvidia_spi_cs_en(0);
+		ret = spi_write(spi, (unsigned char *)buf, 2);
+		nvidia_spi_cs_en(1);
+
+		if (ret < 0)
+			return ret;
+		buf[0] = (127 << 1) ;
+		buf[1] = book;
+
+		nvidia_spi_cs_en(0);
+		ret = spi_write(spi, (unsigned char *)buf, 2);
+		nvidia_spi_cs_en(1);
+
+		if (ret < 0)
+			return ret;
+		aic3262->book_no = book;
+		aic3262->page_no = 0x0;
+	}
+
+	if (aic3262->page_no != page) {
+		buf[0] = 0x0;
+		buf[1] = page;
+		nvidia_spi_cs_en(0);
+		ret = spi_write(spi, (unsigned char *) buf, 2);
+		nvidia_spi_cs_en(1);
+		if (ret < 0)
+			return ret;
+		aic3262->page_no = page;
+	}
+	write_buf[0] = offset << 1 ;
+	memcpy(&write_buf[1], src, bytes);
+	nvidia_spi_cs_en(0);
+	ret = spi_write(spi, write_buf, bytes + 1);
+	nvidia_spi_cs_en(1);
+	if (ret < 0)
+		return ret;
+
+	return bytes;
+}
+
+static int aic3262_spi_probe(struct spi_device *spi)
+{
+	struct aic3262 *aic3262;
+
+	aic3262 = kzalloc(sizeof(struct aic3262), GFP_KERNEL);
+	if (aic3262 == NULL)
+		return -ENOMEM;
+
+	spi_set_drvdata(spi, aic3262);
+	aic3262->dev = &spi->dev;
+	aic3262->control_data = spi;
+	aic3262->read_dev = aic3262_spi_read_device;
+	aic3262->write_dev = aic3262_spi_write_device;
+	spi->bits_per_word = 8;
+	spi->mode = SPI_MODE_1;
+	spi->max_speed_hz = 4000*1000;
+	spi_setup(spi);
+
+	if (strcmp(spi->modalias, "tlv320aic3262") == 0)
+		aic3262->type = TLV320AIC3262;
+	aic3262->book_no = 255;
+	aic3262->page_no = 255;
+
+	return aic3262_device_init(aic3262, spi->irq);
+}
+
+static int aic3262_spi_remove(struct spi_device *spi)
+{
+	struct aic3262 *aic3262 = spi_get_drvdata(spi);
+
+	aic3262_device_exit(aic3262);
+
+	return 0;
+}
+
+static struct spi_driver aic3262_spi_driver = {
+	.driver = {
+		.name = "tlv320aic3262",
+		.owner = THIS_MODULE,
+		.pm = &aic3262_pm_ops,
+	},
+	.probe = aic3262_spi_probe,
+	.remove = aic3262_spi_remove,
+};
+
+static int __init aic3262_spi_init(void)
+{
+	int ret;
+	ret = spi_register_driver(&aic3262_spi_driver);
+	if (ret != 0)
+		pr_err("Failed to register aic3262 SPI driver: %d\n", ret);
+
+	return ret;
+}
+module_init(aic3262_spi_init);
+
+static void __exit aic3262_spi_exit(void)
+{
+	spi_unregister_driver(&aic3262_spi_driver);
+}
+module_exit(aic3262_spi_exit);
+#endif
+
+MODULE_DESCRIPTION("Core support for the TLV320AIC3262 audio CODEC");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Mukund Navada <navada@ti.com>");
diff --git a/drivers/mfd/tlv320aic3262-irq.c b/drivers/mfd/tlv320aic3262-irq.c
new file mode 100644
index 000000000000..7e7a5499f3e5
--- /dev/null
+++ b/drivers/mfd/tlv320aic3262-irq.c
@@ -0,0 +1,204 @@
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/irq.h>
+#include <linux/mfd/core.h>
+#include <linux/interrupt.h>
+
+#include <linux/mfd/tlv320aic3262-core.h>
+#include <linux/mfd/tlv320aic3262-registers.h>
+
+#include <linux/delay.h>
+struct aic3262_irq_data {
+	int mask;
+	int status;
+};
+
+static struct aic3262_irq_data aic3262_irqs[] = {
+	{
+	 .mask = AIC3262_HEADSET_IN_MASK,
+	 .status = AIC3262_HEADSET_PLUG_UNPLUG_INT,
+	 },
+	{
+	 .mask = AIC3262_BUTTON_PRESS_MASK,
+	 .status = AIC3262_BUTTON_PRESS_INT,
+	 },
+	{
+	 .mask = AIC3262_DAC_DRC_THRES_MASK,
+	 .status = AIC3262_LEFT_DRC_THRES_INT | AIC3262_RIGHT_DRC_THRES_INT,
+	 },
+	{
+	 .mask = AIC3262_AGC_NOISE_MASK,
+	 .status = AIC3262_LEFT_AGC_NOISE_INT | AIC3262_RIGHT_AGC_NOISE_INT,
+	 },
+	{
+	 .mask = AIC3262_OVER_CURRENT_MASK,
+	 .status = AIC3262_LEFT_OUTPUT_DRIVER_OVERCURRENT_INT
+	 | AIC3262_RIGHT_OUTPUT_DRIVER_OVERCURRENT_INT,
+	 },
+	{
+	 .mask = AIC3262_OVERFLOW_MASK,
+	 .status =
+	 AIC3262_LEFT_DAC_OVERFLOW_INT | AIC3262_RIGHT_DAC_OVERFLOW_INT |
+	 AIC3262_MINIDSP_D_BARREL_SHIFT_OVERFLOW_INT |
+	 AIC3262_LEFT_ADC_OVERFLOW_INT | AIC3262_RIGHT_ADC_OVERFLOW_INT |
+	 AIC3262_MINIDSP_D_BARREL_SHIFT_OVERFLOW_INT,
+	 },
+	{
+	 .mask = AIC3262_SPK_OVERCURRENT_MASK,
+	 .status = AIC3262_SPK_OVER_CURRENT_INT,
+	 },
+
+};
+
+struct aic3262_gpio_data {
+
+};
+
+static inline struct aic3262_irq_data *irq_to_aic3262_irq(struct aic3262
+							  *aic3262, int irq)
+{
+	return &aic3262_irqs[irq - aic3262->irq_base];
+}
+
+static void aic3262_irq_lock(struct irq_data *data)
+{
+	struct aic3262 *aic3262 = irq_data_get_irq_chip_data(data);
+
+	mutex_lock(&aic3262->irq_lock);
+}
+
+static void aic3262_irq_sync_unlock(struct irq_data *data)
+{
+	struct aic3262 *aic3262 = irq_data_get_irq_chip_data(data);
+
+	/* write back to hardware any change in irq mask */
+	if (aic3262->irq_masks_cur != aic3262->irq_masks_cache) {
+		aic3262->irq_masks_cache = aic3262->irq_masks_cur;
+		aic3262_reg_write(aic3262, AIC3262_INT1_CNTL,
+				  aic3262->irq_masks_cur);
+	}
+
+	mutex_unlock(&aic3262->irq_lock);
+}
+
+static void aic3262_irq_unmask(struct irq_data *data)
+{
+	struct aic3262 *aic3262 = irq_data_get_irq_chip_data(data);
+	struct aic3262_irq_data *irq_data =
+	    irq_to_aic3262_irq(aic3262, data->irq);
+
+	aic3262->irq_masks_cur |= irq_data->mask;
+}
+
+static void aic3262_irq_mask(struct irq_data *data)
+{
+	struct aic3262 *aic3262 = irq_data_get_irq_chip_data(data);
+	struct aic3262_irq_data *irq_data =
+	    irq_to_aic3262_irq(aic3262, data->irq);
+
+	aic3262->irq_masks_cur &= ~irq_data->mask;
+}
+
+static struct irq_chip aic3262_irq_chip = {
+	.name = "tlv320aic3262",
+	.irq_bus_lock = aic3262_irq_lock,
+	.irq_bus_sync_unlock = aic3262_irq_sync_unlock,
+	.irq_mask = aic3262_irq_mask,
+	.irq_unmask = aic3262_irq_unmask,
+};
+
+static irqreturn_t aic3262_irq_thread(int irq, void *data)
+{
+	struct aic3262 *aic3262 = data;
+	u8 status[4];
+	int i = 0;
+	/* Reading the sticky bit registers acknowledges
+	the interrupt to the device */
+	aic3262_bulk_read(aic3262, AIC3262_INT_STICKY_FLAG1, 4, status);
+
+	/* report  */
+	if (status[2] & aic3262_irqs[AIC3262_IRQ_HEADSET_DETECT].status)
+		handle_nested_irq(aic3262->irq_base);
+
+	if (status[2] & aic3262_irqs[AIC3262_IRQ_BUTTON_PRESS].status)
+		handle_nested_irq(aic3262->irq_base + 1);
+	if (status[2] & aic3262_irqs[AIC3262_IRQ_DAC_DRC].status)
+		handle_nested_irq(aic3262->irq_base + 2);
+	if (status[3] & aic3262_irqs[AIC3262_IRQ_AGC_NOISE].status)
+		handle_nested_irq(aic3262->irq_base + 3);
+	if (status[2] & aic3262_irqs[AIC3262_IRQ_OVER_CURRENT].status)
+		handle_nested_irq(aic3262->irq_base + 4);
+	if (status[0] & aic3262_irqs[AIC3262_IRQ_OVERFLOW_EVENT].status)
+		handle_nested_irq(aic3262->irq_base + 5);
+	if (status[3] & aic3262_irqs[AIC3262_IRQ_SPEAKER_OVER_TEMP].status)
+		handle_nested_irq(aic3262->irq_base + 6);
+
+	/* ack unmasked irqs */
+	/* No need to acknowledge the interrupt on AIC3262 */
+
+	return IRQ_HANDLED;
+}
+
+int aic3262_irq_init(struct aic3262 *aic3262)
+{
+	int cur_irq, ret;
+
+	mutex_init(&aic3262->irq_lock);
+
+	/* mask the individual interrupt sources */
+	aic3262->irq_masks_cur = 0x0;
+	aic3262->irq_masks_cache = 0x0;
+	aic3262_reg_write(aic3262, AIC3262_INT1_CNTL, 0x0);
+
+	if (!aic3262->irq) {
+		dev_warn(aic3262->dev,
+			 "no interrupt specified, no interrupts\n");
+		aic3262->irq_base = 0;
+		return 0;
+	}
+
+	if (!aic3262->irq_base) {
+		dev_err(aic3262->dev,
+			"no interrupt base specified, no interrupts\n");
+		return 0;
+	}
+
+	/* Register them with genirq */
+	for (cur_irq = aic3262->irq_base;
+	     cur_irq < aic3262->irq_base + ARRAY_SIZE(aic3262_irqs);
+	     cur_irq++) {
+		irq_set_chip_data(cur_irq, aic3262);
+		irq_set_chip_and_handler(cur_irq, &aic3262_irq_chip,
+					 handle_edge_irq);
+		irq_set_nested_thread(cur_irq, 1);
+
+		/* ARM needs us to explicitly flag the IRQ as valid
+		 * and will set them noprobe when we do so. */
+#ifdef CONFIG_ARM
+		set_irq_flags(cur_irq, IRQF_VALID);
+#else
+		set_irq_noprobe(cur_irq);
+#endif
+	}
+
+	ret = request_threaded_irq(aic3262->irq, NULL, aic3262_irq_thread,
+				   IRQF_TRIGGER_RISING,
+				   "tlv320aic3262", aic3262);
+	if (ret) {
+		dev_err(aic3262->dev, "failed to request IRQ %d: %d\n",
+			aic3262->irq, ret);
+		return ret;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(aic3262_irq_init);
+
+void aic3262_irq_exit(struct aic3262 *aic3262)
+{
+	if (aic3262->irq)
+		free_irq(aic3262->irq, aic3262);
+}
+EXPORT_SYMBOL(aic3262_irq_exit);
diff --git a/drivers/mfd/tps65090.c b/drivers/mfd/tps65090.c
new file mode 100644
index 000000000000..25f463de1513
--- /dev/null
+++ b/drivers/mfd/tps65090.c
@@ -0,0 +1,333 @@
+/*
+ * Core driver for TI TPS65090 PMIC family
+ *
+ * Copyright (c) 2012, NVIDIA 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/tps65090.h>
+#include <linux/err.h>
+
+#define NUM_INT_REG 2
+#define TOTAL_NUM_REG 0x18
+
+/* interrupt status registers */
+#define TPS65090_INT_STS	0x0
+#define TPS65090_INT_STS2	0x1
+
+/* interrupt mask registers */
+#define TPS65090_INT_MSK	0x2
+#define TPS65090_INT_MSK2	0x3
+
+struct tps65090_irq_data {
+	u8		mask_reg;
+	u8		mask_pos;
+};
+
+#define TPS65090_IRQ(_reg, _mask_pos)		\
+	{					\
+		.mask_reg	= (_reg),	\
+		.mask_pos	= (_mask_pos),	\
+	}
+
+static const struct tps65090_irq_data tps65090_irqs[] = {
+	[0]		= TPS65090_IRQ(0, 0),
+	[1]		= TPS65090_IRQ(0, 1),
+	[2]		= TPS65090_IRQ(0, 2),
+	[3]		= TPS65090_IRQ(0, 3),
+	[4]		= TPS65090_IRQ(0, 4),
+	[5]		= TPS65090_IRQ(0, 5),
+	[6]		= TPS65090_IRQ(0, 6),
+	[7]		= TPS65090_IRQ(0, 7),
+	[8]		= TPS65090_IRQ(1, 0),
+	[9]		= TPS65090_IRQ(1, 1),
+	[10]		= TPS65090_IRQ(1, 2),
+	[11]		= TPS65090_IRQ(1, 3),
+	[12]		= TPS65090_IRQ(1, 4),
+	[13]		= TPS65090_IRQ(1, 5),
+	[14]		= TPS65090_IRQ(1, 6),
+	[15]		= TPS65090_IRQ(1, 7),
+};
+
+static struct mfd_cell tps65090s[] = {
+	{
+		.name = "tps65910-pmic",
+	},
+};
+
+static void tps65090_irq_lock(struct irq_data *data)
+{
+	struct tps65090 *tps65090 = irq_data_get_irq_chip_data(data);
+
+	mutex_lock(&tps65090->irq_lock);
+}
+
+static void tps65090_irq_mask(struct irq_data *irq_data)
+{
+	struct tps65090 *tps65090 = irq_data_get_irq_chip_data(irq_data);
+	unsigned int __irq = irq_data->hwirq;
+	const struct tps65090_irq_data *data = &tps65090_irqs[__irq];
+
+	tps65090_set_bits(tps65090->dev, (TPS65090_INT_MSK + data->mask_reg),
+		data->mask_pos);
+}
+
+static void tps65090_irq_unmask(struct irq_data *irq_data)
+{
+	struct tps65090 *tps65090 = irq_data_get_irq_chip_data(irq_data);
+	unsigned int __irq = irq_data->irq - tps65090->irq_base;
+	const struct tps65090_irq_data *data = &tps65090_irqs[__irq];
+
+	tps65090_clr_bits(tps65090->dev, (TPS65090_INT_MSK + data->mask_reg),
+		data->mask_pos);
+}
+
+static void tps65090_irq_sync_unlock(struct irq_data *data)
+{
+	struct tps65090 *tps65090 = irq_data_get_irq_chip_data(data);
+
+	mutex_unlock(&tps65090->irq_lock);
+}
+
+static irqreturn_t tps65090_irq(int irq, void *data)
+{
+	struct tps65090 *tps65090 = data;
+	int ret = 0;
+	u8 status = 0, mask = 0;
+	unsigned long int acks = 0;
+	int i;
+
+	for (i = 0; i < NUM_INT_REG; i++) {
+		ret = tps65090_read(tps65090->dev, TPS65090_INT_MSK + i, &mask);
+		if (ret < 0) {
+			dev_err(tps65090->dev,
+				"failed to read mask reg [addr:%d]\n",
+				TPS65090_INT_MSK + i);
+			return IRQ_NONE;
+		}
+		ret = tps65090_read(tps65090->dev, TPS65090_INT_STS + i,
+			&status);
+		if (ret < 0) {
+			dev_err(tps65090->dev,
+				"failed to read status reg [addr:%d]\n",
+				 TPS65090_INT_STS + i);
+			return IRQ_NONE;
+		}
+		if (status) {
+			/* Ack only those interrupts which are not masked */
+			status &= (~mask);
+			ret = tps65090_write(tps65090->dev,
+					TPS65090_INT_STS + i, status);
+			if (ret < 0) {
+				dev_err(tps65090->dev,
+					"failed to write interrupt status\n");
+				return IRQ_NONE;
+			}
+			acks |= (status << (i * 8));
+		}
+	}
+
+	for_each_set_bit(i, &acks, ARRAY_SIZE(tps65090_irqs))
+		handle_nested_irq(tps65090->irq_base + i);
+	return acks ? IRQ_HANDLED : IRQ_NONE;
+}
+
+static int __devinit tps65090_irq_init(struct tps65090 *tps65090, int irq,
+	int irq_base)
+{
+	int i, ret;
+
+	if (!irq_base) {
+		dev_err(tps65090->dev, "IRQ base not set\n");
+		return -EINVAL;
+	}
+
+	mutex_init(&tps65090->irq_lock);
+
+	for (i = 0; i < NUM_INT_REG; i++)
+		tps65090_write(tps65090->dev, TPS65090_INT_MSK + i, 0xFF);
+
+	for (i = 0; i < NUM_INT_REG; i++)
+		tps65090_write(tps65090->dev, TPS65090_INT_STS + i, 0xff);
+
+	tps65090->irq_base = irq_base;
+	tps65090->irq_chip.name = "tps65090";
+	tps65090->irq_chip.irq_mask = tps65090_irq_mask;
+	tps65090->irq_chip.irq_unmask = tps65090_irq_unmask;
+	tps65090->irq_chip.irq_bus_lock = tps65090_irq_lock;
+	tps65090->irq_chip.irq_bus_sync_unlock = tps65090_irq_sync_unlock;
+
+	for (i = 0; i < ARRAY_SIZE(tps65090_irqs); i++) {
+		int __irq = i + tps65090->irq_base;
+		irq_set_chip_data(__irq, tps65090);
+		irq_set_chip_and_handler(__irq, &tps65090->irq_chip,
+					 handle_simple_irq);
+		irq_set_nested_thread(__irq, 1);
+#ifdef CONFIG_ARM
+		set_irq_flags(__irq, IRQF_VALID);
+#endif
+	}
+
+	ret = request_threaded_irq(irq, NULL, tps65090_irq, IRQF_ONESHOT,
+				"tps65090", tps65090);
+	if (!ret) {
+		device_init_wakeup(tps65090->dev, 1);
+		enable_irq_wake(irq);
+	}
+
+	return ret;
+}
+
+static bool is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	if ((reg == TPS65090_INT_STS) || (reg == TPS65090_INT_STS))
+		return true;
+	else
+		return false;
+}
+
+static const struct regmap_config tps65090_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = TOTAL_NUM_REG,
+	.num_reg_defaults_raw = TOTAL_NUM_REG,
+	.cache_type = REGCACHE_RBTREE,
+	.volatile_reg = is_volatile_reg,
+};
+
+static int __devinit tps65090_i2c_probe(struct i2c_client *client,
+					const struct i2c_device_id *id)
+{
+	struct tps65090_platform_data *pdata = client->dev.platform_data;
+	struct tps65090 *tps65090;
+	int ret;
+
+	if (!pdata) {
+		dev_err(&client->dev, "tps65090 requires platform data\n");
+		return -EINVAL;
+	}
+
+	tps65090 = devm_kzalloc(&client->dev, sizeof(*tps65090), GFP_KERNEL);
+	if (!tps65090) {
+		dev_err(&client->dev, "mem alloc for tps65090 failed\n");
+		return -ENOMEM;
+	}
+
+	tps65090->dev = &client->dev;
+	i2c_set_clientdata(client, tps65090);
+
+	tps65090->rmap = devm_regmap_init_i2c(client, &tps65090_regmap_config);
+	if (IS_ERR(tps65090->rmap)) {
+		ret = PTR_ERR(tps65090->rmap);
+		dev_err(&client->dev, "regmap_init failed with err: %d\n", ret);
+		return ret;
+	}
+
+	if (client->irq) {
+		ret = tps65090_irq_init(tps65090, client->irq, pdata->irq_base);
+		if (ret) {
+			dev_err(&client->dev, "IRQ init failed with err: %d\n",
+				ret);
+			return ret;
+		}
+	}
+
+	ret = mfd_add_devices(tps65090->dev, -1, tps65090s,
+		ARRAY_SIZE(tps65090s), NULL, 0);
+	if (ret) {
+		dev_err(&client->dev, "add mfd devices failed with err: %d\n",
+			ret);
+		goto err_irq_exit;
+	}
+
+	return 0;
+
+err_irq_exit:
+	if (client->irq)
+		free_irq(client->irq, tps65090);
+	return ret;
+}
+
+static int __devexit tps65090_i2c_remove(struct i2c_client *client)
+{
+	struct tps65090 *tps65090 = i2c_get_clientdata(client);
+
+	mfd_remove_devices(tps65090->dev);
+	if (client->irq)
+		free_irq(client->irq, tps65090);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int tps65090_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	if (client->irq)
+		disable_irq(client->irq);
+	return 0;
+}
+
+static int tps65090_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	if (client->irq)
+		enable_irq(client->irq);
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops tps65090_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(tps65090_suspend, tps65090_resume)
+};
+
+static const struct i2c_device_id tps65090_id_table[] = {
+	{ "tps65090", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, tps65090_id_table);
+
+static struct i2c_driver tps65090_driver = {
+	.driver	= {
+		.name	= "tps65090",
+		.owner	= THIS_MODULE,
+		.pm	= &tps65090_pm_ops,
+	},
+	.probe		= tps65090_i2c_probe,
+	.remove		= __devexit_p(tps65090_i2c_remove),
+	.id_table	= tps65090_id_table,
+};
+
+static int __init tps65090_init(void)
+{
+	return i2c_add_driver(&tps65090_driver);
+}
+subsys_initcall(tps65090_init);
+
+static void __exit tps65090_exit(void)
+{
+	i2c_del_driver(&tps65090_driver);
+}
+module_exit(tps65090_exit);
+
+MODULE_DESCRIPTION("TPS65090 core driver");
+MODULE_AUTHOR("Venu Byravarasu <vbyravarasu@nvidia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/tps6586x.c b/drivers/mfd/tps6586x.c
index bba26d96c240..1c140e5ace4f 100644
--- a/drivers/mfd/tps6586x.c
+++ b/drivers/mfd/tps6586x.c
@@ -27,6 +27,10 @@
 #include <linux/mfd/core.h>
 #include <linux/mfd/tps6586x.h>
 
+#define TPS6586X_SUPPLYENE  0x14
+#define EXITSLREQ_BIT       BIT(1) /* Exit sleep mode request */
+#define SLEEP_MODE_BIT      BIT(3) /* Sleep mode */
+
 /* GPIO control registers */
 #define TPS6586X_GPIOSET1	0x5d
 #define TPS6586X_GPIOSET2	0x5e
@@ -92,6 +96,7 @@ struct tps6586x {
 	struct mutex		lock;
 	struct device		*dev;
 	struct i2c_client	*client;
+	enum tps6586x_type	type;
 
 	struct gpio_chip	gpio;
 	struct irq_chip		irq_chip;
@@ -251,6 +256,30 @@ out:
 }
 EXPORT_SYMBOL_GPL(tps6586x_update);
 
+enum tps6586x_type tps6586x_gettype(struct device *dev)
+{
+	struct tps6586x *tps6586x = dev_get_drvdata(dev);
+
+	return tps6586x->type;
+}
+EXPORT_SYMBOL_GPL(tps6586x_gettype);
+
+static struct i2c_client *tps6586x_i2c_client = NULL;
+static void tps6586x_power_off(void)
+{
+	struct device *dev = NULL;
+
+	if (!tps6586x_i2c_client)
+		return;
+
+	dev = &tps6586x_i2c_client->dev;
+
+	if (tps6586x_clr_bits(dev, TPS6586X_SUPPLYENE, EXITSLREQ_BIT))
+		return;
+
+	tps6586x_set_bits(dev, TPS6586X_SUPPLYENE, SLEEP_MODE_BIT);
+}
+
 static int tps6586x_gpio_get(struct gpio_chip *gc, unsigned offset)
 {
 	struct tps6586x *tps6586x = container_of(gc, struct tps6586x, gpio);
@@ -274,13 +303,24 @@ static void tps6586x_gpio_set(struct gpio_chip *chip, unsigned offset,
 			value << offset, 1 << offset);
 }
 
+static int tps6586x_gpio_input(struct gpio_chip *gc, unsigned offset)
+{
+	/* FIXME: add handling of GPIOs as dedicated inputs */
+	return -ENOSYS;
+}
+
 static int tps6586x_gpio_output(struct gpio_chip *gc, unsigned offset,
 				int value)
 {
 	struct tps6586x *tps6586x = container_of(gc, struct tps6586x, gpio);
 	uint8_t val, mask;
+	int ret;
 
-	tps6586x_gpio_set(gc, offset, value);
+	val = value << offset;
+	mask = 0x1 << offset;
+	ret = tps6586x_update(tps6586x->dev, TPS6586X_GPIOSET2, val, mask);
+	if (ret)
+		return ret;
 
 	val = 0x1 << (offset * 2);
 	mask = 0x3 << (offset * 2);
@@ -300,7 +340,7 @@ static int tps6586x_gpio_init(struct tps6586x *tps6586x, int gpio_base)
 	tps6586x->gpio.ngpio		= 4;
 	tps6586x->gpio.can_sleep	= 1;
 
-	/* FIXME: add handling of GPIOs as dedicated inputs */
+	tps6586x->gpio.direction_input	= tps6586x_gpio_input;
 	tps6586x->gpio.direction_output	= tps6586x_gpio_output;
 	tps6586x->gpio.set		= tps6586x_gpio_set;
 	tps6586x->gpio.get		= tps6586x_gpio_get;
@@ -486,17 +526,36 @@ static int __devinit tps6586x_i2c_probe(struct i2c_client *client,
 		return -ENOTSUPP;
 	}
 
+	tps6586x = kzalloc(sizeof(struct tps6586x), GFP_KERNEL);
+	if (tps6586x == NULL)
+		return -ENOMEM;
+
 	ret = i2c_smbus_read_byte_data(client, TPS6586X_VERSIONCRC);
 	if (ret < 0) {
 		dev_err(&client->dev, "Chip ID read failed: %d\n", ret);
-		return -EIO;
+		ret = -EIO;
+		goto err_irq_init;
+	}
+	tps6586x->type = (enum tps6586x_type)ret;
+	switch (ret) {
+	case TPS658621A:
+		dev_info(&client->dev, "found TPS658621A, ");
+		break;
+	case TPS658621D:
+		dev_info(&client->dev, "found TPS658621D, ");
+		break;
+	case TPS658623:
+		dev_info(&client->dev, "found TPS658623, ");
+		break;
+	case TPS658643:
+		dev_info(&client->dev, "found TPS658643, ");
+		break;
+	default:
+		dev_info(&client->dev, "unknown TPS6586X found, ");
+		tps6586x->type = TPS6586X_ANY;
 	}
 
-	dev_info(&client->dev, "VERSIONCRC is %02x\n", ret);
-
-	tps6586x = kzalloc(sizeof(struct tps6586x), GFP_KERNEL);
-	if (tps6586x == NULL)
-		return -ENOMEM;
+	printk("VERSIONCRC is %02x\n", ret);
 
 	tps6586x->client = client;
 	tps6586x->dev = &client->dev;
@@ -525,6 +584,11 @@ static int __devinit tps6586x_i2c_probe(struct i2c_client *client,
 		goto err_add_devs;
 	}
 
+	if (pdata->use_power_off && !pm_power_off)
+		pm_power_off = tps6586x_power_off;
+
+	tps6586x_i2c_client = client;
+
 	return 0;
 
 err_add_devs:
diff --git a/drivers/mfd/tps65910-irq.c b/drivers/mfd/tps65910-irq.c
index a56be931551c..0f1ff7fbdc74 100644
--- a/drivers/mfd/tps65910-irq.c
+++ b/drivers/mfd/tps65910-irq.c
@@ -41,28 +41,28 @@ static inline int irq_to_tps65910_irq(struct tps65910 *tps65910,
 static irqreturn_t tps65910_irq(int irq, void *irq_data)
 {
 	struct tps65910 *tps65910 = irq_data;
+	unsigned int reg;
 	u32 irq_sts;
 	u32 irq_mask;
-	u8 reg;
 	int i;
 
-	tps65910->read(tps65910, TPS65910_INT_STS, 1, &reg);
+	tps65910_reg_read(tps65910, TPS65910_INT_STS, &reg);
 	irq_sts = reg;
-	tps65910->read(tps65910, TPS65910_INT_STS2, 1, &reg);
+	tps65910_reg_read(tps65910, TPS65910_INT_STS2, &reg);
 	irq_sts |= reg << 8;
 	switch (tps65910_chip_id(tps65910)) {
 	case TPS65911:
-		tps65910->read(tps65910, TPS65910_INT_STS3, 1, &reg);
+		tps65910_reg_read(tps65910, TPS65910_INT_STS3, &reg);
 		irq_sts |= reg << 16;
 	}
 
-	tps65910->read(tps65910, TPS65910_INT_MSK, 1, &reg);
+	tps65910_reg_read(tps65910, TPS65910_INT_MSK, &reg);
 	irq_mask = reg;
-	tps65910->read(tps65910, TPS65910_INT_MSK2, 1, &reg);
+	tps65910_reg_read(tps65910, TPS65910_INT_MSK2, &reg);
 	irq_mask |= reg << 8;
 	switch (tps65910_chip_id(tps65910)) {
 	case TPS65911:
-		tps65910->read(tps65910, TPS65910_INT_MSK3, 1, &reg);
+		tps65910_reg_read(tps65910, TPS65910_INT_MSK3, &reg);
 		irq_mask |= reg << 16;
 	}
 
@@ -82,13 +82,13 @@ static irqreturn_t tps65910_irq(int irq, void *irq_data)
 	/* Write the STS register back to clear IRQs we handled */
 	reg = irq_sts & 0xFF;
 	irq_sts >>= 8;
-	tps65910->write(tps65910, TPS65910_INT_STS, 1, &reg);
+	tps65910_reg_write(tps65910, TPS65910_INT_STS, reg);
 	reg = irq_sts & 0xFF;
-	tps65910->write(tps65910, TPS65910_INT_STS2, 1, &reg);
+	tps65910_reg_write(tps65910, TPS65910_INT_STS2, reg);
 	switch (tps65910_chip_id(tps65910)) {
 	case TPS65911:
 		reg = irq_sts >> 8;
-		tps65910->write(tps65910, TPS65910_INT_STS3, 1, &reg);
+		tps65910_reg_write(tps65910, TPS65910_INT_STS3, reg);
 	}
 
 	return IRQ_HANDLED;
@@ -105,27 +105,27 @@ static void tps65910_irq_sync_unlock(struct irq_data *data)
 {
 	struct tps65910 *tps65910 = irq_data_get_irq_chip_data(data);
 	u32 reg_mask;
-	u8 reg;
+	unsigned int reg;
 
-	tps65910->read(tps65910, TPS65910_INT_MSK, 1, &reg);
+	tps65910_reg_read(tps65910, TPS65910_INT_MSK, &reg);
 	reg_mask = reg;
-	tps65910->read(tps65910, TPS65910_INT_MSK2, 1, &reg);
+	tps65910_reg_read(tps65910, TPS65910_INT_MSK2, &reg);
 	reg_mask |= reg << 8;
 	switch (tps65910_chip_id(tps65910)) {
 	case TPS65911:
-		tps65910->read(tps65910, TPS65910_INT_MSK3, 1, &reg);
+		tps65910_reg_read(tps65910, TPS65910_INT_MSK3, &reg);
 		reg_mask |= reg << 16;
 	}
 
 	if (tps65910->irq_mask != reg_mask) {
 		reg = tps65910->irq_mask & 0xFF;
-		tps65910->write(tps65910, TPS65910_INT_MSK, 1, &reg);
+		tps65910_reg_write(tps65910, TPS65910_INT_MSK, reg);
 		reg = tps65910->irq_mask >> 8 & 0xFF;
-		tps65910->write(tps65910, TPS65910_INT_MSK2, 1, &reg);
+		tps65910_reg_write(tps65910, TPS65910_INT_MSK2, reg);
 		switch (tps65910_chip_id(tps65910)) {
 		case TPS65911:
 			reg = tps65910->irq_mask >> 16;
-			tps65910->write(tps65910, TPS65910_INT_MSK3, 1, &reg);
+			tps65910_reg_write(tps65910, TPS65910_INT_MSK3, reg);
 		}
 	}
 	mutex_unlock(&tps65910->irq_lock);
@@ -145,12 +145,23 @@ static void tps65910_irq_disable(struct irq_data *data)
 	tps65910->irq_mask |= ( 1 << irq_to_tps65910_irq(tps65910, data->irq));
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int tps65910_irq_set_wake(struct irq_data *data, unsigned int enable)
+{
+	struct tps65910 *tps65910 = irq_data_get_irq_chip_data(data);
+	return irq_set_irq_wake(tps65910->chip_irq, enable);
+}
+#else
+#define tps65910_irq_set_wake NULL
+#endif
+
 static struct irq_chip tps65910_irq_chip = {
 	.name = "tps65910",
 	.irq_bus_lock = tps65910_irq_lock,
 	.irq_bus_sync_unlock = tps65910_irq_sync_unlock,
 	.irq_disable = tps65910_irq_disable,
 	.irq_enable = tps65910_irq_enable,
+	.irq_set_wake = tps65910_irq_set_wake,
 };
 
 int tps65910_irq_init(struct tps65910 *tps65910, int irq,
@@ -215,6 +226,7 @@ int tps65910_irq_init(struct tps65910 *tps65910, int irq,
 
 int tps65910_irq_exit(struct tps65910 *tps65910)
 {
-	free_irq(tps65910->chip_irq, tps65910);
+	if (tps65910->chip_irq)
+		free_irq(tps65910->chip_irq, tps65910);
 	return 0;
 }
diff --git a/drivers/mfd/tps65910.c b/drivers/mfd/tps65910.c
index 6f5b8cf2f652..18b30cf45e5b 100644
--- a/drivers/mfd/tps65910.c
+++ b/drivers/mfd/tps65910.c
@@ -16,14 +16,19 @@
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/init.h>
+#include <linux/err.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
-#include <linux/gpio.h>
 #include <linux/mfd/core.h>
+#include <linux/regmap.h>
 #include <linux/mfd/tps65910.h>
+#include <linux/of_device.h>
 
 static struct mfd_cell tps65910s[] = {
 	{
+		.name = "tps65910-gpio",
+	},
+	{
 		.name = "tps65910-pmic",
 	},
 	{
@@ -35,164 +40,232 @@ static struct mfd_cell tps65910s[] = {
 };
 
 
-static int tps65910_i2c_read(struct tps65910 *tps65910, u8 reg,
-				  int bytes, void *dest)
+static bool is_volatile_reg(struct device *dev, unsigned int reg)
 {
-	struct i2c_client *i2c = tps65910->i2c_client;
-	struct i2c_msg xfer[2];
-	int ret;
-
-	/* Write register */
-	xfer[0].addr = i2c->addr;
-	xfer[0].flags = 0;
-	xfer[0].len = 1;
-	xfer[0].buf = &reg;
-
-	/* Read data */
-	xfer[1].addr = i2c->addr;
-	xfer[1].flags = I2C_M_RD;
-	xfer[1].len = bytes;
-	xfer[1].buf = dest;
-
-	ret = i2c_transfer(i2c->adapter, xfer, 2);
-	if (ret == 2)
-		ret = 0;
-	else if (ret >= 0)
-		ret = -EIO;
-
-	return ret;
+	struct tps65910 *tps65910 = dev_get_drvdata(dev);
+
+	/*
+	 * Caching all regulator registers.
+	 * All regualator register address range is same for
+	 * TPS65910 and TPS65911
+	 */
+	if ((reg >= TPS65910_VIO) && (reg <= TPS65910_VDAC)) {
+		/* Check for non-existing register */
+		if (tps65910_chip_id(tps65910) == TPS65910)
+			if ((reg == TPS65911_VDDCTRL_OP) ||
+				(reg == TPS65911_VDDCTRL_SR))
+				return true;
+		return false;
+	}
+	return true;
 }
 
-static int tps65910_i2c_write(struct tps65910 *tps65910, u8 reg,
-				   int bytes, void *src)
+static const struct regmap_config tps65910_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.volatile_reg = is_volatile_reg,
+	.max_register = TPS65910_MAX_REGISTER - 1,
+	.cache_type = REGCACHE_RBTREE,
+};
+
+static int __devinit tps65910_sleepinit(struct tps65910 *tps65910,
+		struct tps65910_board *pmic_pdata)
 {
-	struct i2c_client *i2c = tps65910->i2c_client;
-	/* we add 1 byte for device register */
-	u8 msg[TPS65910_MAX_REGISTER + 1];
-	int ret;
+	struct device *dev = NULL;
+	int ret = 0;
 
-	if (bytes > TPS65910_MAX_REGISTER)
-		return -EINVAL;
+	dev = tps65910->dev;
 
-	msg[0] = reg;
-	memcpy(&msg[1], src, bytes);
+	if (!pmic_pdata->en_dev_slp)
+		return 0;
+
+	/* enabling SLEEP device state */
+	ret = tps65910_reg_set_bits(tps65910, TPS65910_DEVCTRL,
+				DEVCTRL_DEV_SLP_MASK);
+	if (ret < 0) {
+		dev_err(dev, "set dev_slp failed: %d\n", ret);
+		goto err_sleep_init;
+	}
+
+	/* Return if there is no sleep keepon data. */
+	if (!pmic_pdata->slp_keepon)
+		return 0;
+
+	if (pmic_pdata->slp_keepon->therm_keepon) {
+		ret = tps65910_reg_set_bits(tps65910,
+				TPS65910_SLEEP_KEEP_RES_ON,
+				SLEEP_KEEP_RES_ON_THERM_KEEPON_MASK);
+		if (ret < 0) {
+			dev_err(dev, "set therm_keepon failed: %d\n", ret);
+			goto disable_dev_slp;
+		}
+	}
+
+	if (pmic_pdata->slp_keepon->clkout32k_keepon) {
+		ret = tps65910_reg_set_bits(tps65910,
+				TPS65910_SLEEP_KEEP_RES_ON,
+				SLEEP_KEEP_RES_ON_CLKOUT32K_KEEPON_MASK);
+		if (ret < 0) {
+			dev_err(dev, "set clkout32k_keepon failed: %d\n", ret);
+			goto disable_dev_slp;
+		}
+	}
+
+	if (pmic_pdata->slp_keepon->i2chs_keepon) {
+		ret = tps65910_reg_set_bits(tps65910,
+				TPS65910_SLEEP_KEEP_RES_ON,
+				SLEEP_KEEP_RES_ON_I2CHS_KEEPON_MASK);
+		if (ret < 0) {
+			dev_err(dev, "set i2chs_keepon failed: %d\n", ret);
+			goto disable_dev_slp;
+		}
+	}
 
-	ret = i2c_master_send(i2c, msg, bytes + 1);
-	if (ret < 0)
-		return ret;
-	if (ret != bytes + 1)
-		return -EIO;
 	return 0;
+
+disable_dev_slp:
+	tps65910_reg_clear_bits(tps65910, TPS65910_DEVCTRL,
+				DEVCTRL_DEV_SLP_MASK);
+
+err_sleep_init:
+	return ret;
 }
 
-int tps65910_set_bits(struct tps65910 *tps65910, u8 reg, u8 mask)
+#ifdef CONFIG_OF
+static struct of_device_id tps65910_of_match[] = {
+	{ .compatible = "ti,tps65910", .data = (void *)TPS65910},
+	{ .compatible = "ti,tps65911", .data = (void *)TPS65911},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, tps65910_of_match);
+
+static struct tps65910_board *tps65910_parse_dt(struct i2c_client *client,
+						int *chip_id)
 {
-	u8 data;
-	int err;
-
-	mutex_lock(&tps65910->io_mutex);
-	err = tps65910_i2c_read(tps65910, reg, 1, &data);
-	if (err) {
-		dev_err(tps65910->dev, "read from reg %x failed\n", reg);
-		goto out;
+	struct device_node *np = client->dev.of_node;
+	struct tps65910_board *board_info;
+	unsigned int prop;
+	const struct of_device_id *match;
+	unsigned int prop_array[TPS6591X_MAX_NUM_GPIO];
+	int ret = 0;
+	int idx;
+
+	match = of_match_device(tps65910_of_match, &client->dev);
+	if (!match) {
+		dev_err(&client->dev, "Failed to find matching dt id\n");
+		return NULL;
 	}
 
-	data |= mask;
-	err = tps65910_i2c_write(tps65910, reg, 1, &data);
-	if (err)
-		dev_err(tps65910->dev, "write to reg %x failed\n", reg);
+	*chip_id  = (int)match->data;
 
-out:
-	mutex_unlock(&tps65910->io_mutex);
-	return err;
-}
-EXPORT_SYMBOL_GPL(tps65910_set_bits);
+	board_info = devm_kzalloc(&client->dev, sizeof(*board_info),
+			GFP_KERNEL);
+	if (!board_info) {
+		dev_err(&client->dev, "Failed to allocate pdata\n");
+		return NULL;
+	}
 
-int tps65910_clear_bits(struct tps65910 *tps65910, u8 reg, u8 mask)
-{
-	u8 data;
-	int err;
-
-	mutex_lock(&tps65910->io_mutex);
-	err = tps65910_i2c_read(tps65910, reg, 1, &data);
-	if (err) {
-		dev_err(tps65910->dev, "read from reg %x failed\n", reg);
-		goto out;
+	ret = of_property_read_u32(np, "ti,vmbch-threshold", &prop);
+	if (!ret)
+		board_info->vmbch_threshold = prop;
+	else if (*chip_id == TPS65911)
+		dev_warn(&client->dev, "VMBCH-Threshold not specified");
+
+	ret = of_property_read_u32(np, "ti,vmbch2-threshold", &prop);
+	if (!ret)
+		board_info->vmbch2_threshold = prop;
+	else if (*chip_id == TPS65911)
+		dev_warn(&client->dev, "VMBCH2-Threshold not specified");
+
+	ret = of_property_read_u32_array(np, "ti,en-gpio-sleep",
+				   prop_array, TPS6591X_MAX_NUM_GPIO);
+	if (!ret)
+		for (idx = 0; idx < ARRAY_SIZE(prop_array); idx++)
+			board_info->en_gpio_sleep[idx] = (prop_array[idx] != 0);
+	else if (ret != -EINVAL) {
+		dev_err(&client->dev,
+			"error reading property ti,en-gpio-sleep: %d\n.", ret);
+		return NULL;
 	}
 
-	data &= mask;
-	err = tps65910_i2c_write(tps65910, reg, 1, &data);
-	if (err)
-		dev_err(tps65910->dev, "write to reg %x failed\n", reg);
 
-out:
-	mutex_unlock(&tps65910->io_mutex);
-	return err;
+	board_info->irq = client->irq;
+	board_info->irq_base = -1;
+	board_info->gpio_base = -1;
+
+	return board_info;
+}
+#else
+static inline
+struct tps65910_board *tps65910_parse_dt(struct i2c_client *client,
+					 int *chip_id)
+{
+	return NULL;
 }
-EXPORT_SYMBOL_GPL(tps65910_clear_bits);
+#endif
 
-static int tps65910_i2c_probe(struct i2c_client *i2c,
-			    const struct i2c_device_id *id)
+static __devinit int tps65910_i2c_probe(struct i2c_client *i2c,
+					const struct i2c_device_id *id)
 {
 	struct tps65910 *tps65910;
 	struct tps65910_board *pmic_plat_data;
 	struct tps65910_platform_data *init_data;
 	int ret = 0;
+	int chip_id = id->driver_data;
 
 	pmic_plat_data = dev_get_platdata(&i2c->dev);
+
+	if (!pmic_plat_data && i2c->dev.of_node)
+		pmic_plat_data = tps65910_parse_dt(i2c, &chip_id);
+
 	if (!pmic_plat_data)
 		return -EINVAL;
 
-	init_data = kzalloc(sizeof(struct tps65910_platform_data), GFP_KERNEL);
+	init_data = devm_kzalloc(&i2c->dev, sizeof(*init_data), GFP_KERNEL);
 	if (init_data == NULL)
 		return -ENOMEM;
 
-	tps65910 = kzalloc(sizeof(struct tps65910), GFP_KERNEL);
-	if (tps65910 == NULL) {
-		kfree(init_data);
+	tps65910 = devm_kzalloc(&i2c->dev, sizeof(*tps65910), GFP_KERNEL);
+	if (tps65910 == NULL)
 		return -ENOMEM;
-	}
 
 	i2c_set_clientdata(i2c, tps65910);
 	tps65910->dev = &i2c->dev;
 	tps65910->i2c_client = i2c;
-	tps65910->id = id->driver_data;
-	tps65910->read = tps65910_i2c_read;
-	tps65910->write = tps65910_i2c_write;
+	tps65910->id = chip_id;
 	mutex_init(&tps65910->io_mutex);
 
+	tps65910->regmap = devm_regmap_init_i2c(i2c, &tps65910_regmap_config);
+	if (IS_ERR(tps65910->regmap)) {
+		ret = PTR_ERR(tps65910->regmap);
+		dev_err(&i2c->dev, "regmap initialization failed: %d\n", ret);
+		return ret;
+	}
+
 	ret = mfd_add_devices(tps65910->dev, -1,
 			      tps65910s, ARRAY_SIZE(tps65910s),
 			      NULL, 0);
-	if (ret < 0)
-		goto err;
+	if (ret < 0) {
+		dev_err(&i2c->dev, "mfd_add_devices failed: %d\n", ret);
+		return ret;
+	}
 
 	init_data->irq = pmic_plat_data->irq;
-	init_data->irq_base = pmic_plat_data->irq;
+	init_data->irq_base = pmic_plat_data->irq_base;
 
-	tps65910_gpio_init(tps65910, pmic_plat_data->gpio_base);
+	tps65910_irq_init(tps65910, init_data->irq, init_data);
 
-	ret = tps65910_irq_init(tps65910, init_data->irq, init_data);
-	if (ret < 0)
-		goto err;
+	tps65910_sleepinit(tps65910, pmic_plat_data);
 
-	kfree(init_data);
-	return ret;
-
-err:
-	mfd_remove_devices(tps65910->dev);
-	kfree(tps65910);
-	kfree(init_data);
 	return ret;
 }
 
-static int tps65910_i2c_remove(struct i2c_client *i2c)
+static __devexit int tps65910_i2c_remove(struct i2c_client *i2c)
 {
 	struct tps65910 *tps65910 = i2c_get_clientdata(i2c);
 
-	mfd_remove_devices(tps65910->dev);
 	tps65910_irq_exit(tps65910);
-	kfree(tps65910);
+	mfd_remove_devices(tps65910->dev);
 
 	return 0;
 }
@@ -209,9 +282,10 @@ static struct i2c_driver tps65910_i2c_driver = {
 	.driver = {
 		   .name = "tps65910",
 		   .owner = THIS_MODULE,
+		   .of_match_table = of_match_ptr(tps65910_of_match),
 	},
 	.probe = tps65910_i2c_probe,
-	.remove = tps65910_i2c_remove,
+	.remove = __devexit_p(tps65910_i2c_remove),
 	.id_table = tps65910_i2c_id,
 };
 
diff --git a/drivers/mfd/tps6591x.c b/drivers/mfd/tps6591x.c
new file mode 100644
index 000000000000..bd60e09c3168
--- /dev/null
+++ b/drivers/mfd/tps6591x.c
@@ -0,0 +1,930 @@
+/*
+ * driver/mfd/tps6591x.c
+ *
+ * Core driver for TI TPS6591x PMIC family
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+
+#include <linux/mfd/core.h>
+#include <linux/mfd/tps6591x.h>
+
+/* device control registers */
+#define TPS6591X_DEVCTRL	0x3F
+#define DEVCTRL_PWR_OFF_SEQ	(1 << 7)
+#define DEVCTRL_DEV_ON		(1 << 2)
+#define DEVCTRL_DEV_SLP		(1 << 1)
+#define TPS6591X_DEVCTRL2	0x40
+
+/* device sleep on registers */
+#define TPS6591X_SLEEP_KEEP_ON	0x42
+#define SLEEP_KEEP_ON_THERM	(1 << 7)
+#define SLEEP_KEEP_ON_CLKOUT32K	(1 << 6)
+#define SLEEP_KEEP_ON_VRTC	(1 << 5)
+#define SLEEP_KEEP_ON_I2CHS	(1 << 4)
+
+/* interrupt status registers */
+#define TPS6591X_INT_STS	0x50
+#define TPS6591X_INT_STS2	0x52
+#define TPS6591X_INT_STS3	0x54
+
+/* interrupt mask registers */
+#define TPS6591X_INT_MSK	0x51
+#define TPS6591X_INT_MSK2	0x53
+#define TPS6591X_INT_MSK3	0x55
+
+/* GPIO register base address */
+#define TPS6591X_GPIO_BASE_ADDR	0x60
+
+/* silicon version number */
+#define TPS6591X_VERNUM		0x80
+
+#define TPS6591X_GPIO_SLEEP	7
+#define TPS6591X_GPIO_PDEN	3
+#define TPS6591X_GPIO_DIR	2
+
+enum irq_type {
+	EVENT,
+	GPIO,
+};
+
+struct tps6591x_irq_data {
+	u8		mask_reg;
+	u8		mask_pos;
+	enum irq_type	type;
+};
+
+#define TPS6591X_IRQ(_reg, _mask_pos, _type)	\
+	{					\
+		.mask_reg	= (_reg),	\
+		.mask_pos	= (_mask_pos),	\
+		.type		= (_type),	\
+	}
+
+static const struct tps6591x_irq_data tps6591x_irqs[] = {
+	[TPS6591X_INT_PWRHOLD_F]	= TPS6591X_IRQ(0, 0, EVENT),
+	[TPS6591X_INT_VMBHI]		= TPS6591X_IRQ(0, 1, EVENT),
+	[TPS6591X_INT_PWRON]		= TPS6591X_IRQ(0, 2, EVENT),
+	[TPS6591X_INT_PWRON_LP]		= TPS6591X_IRQ(0, 3, EVENT),
+	[TPS6591X_INT_PWRHOLD_R]	= TPS6591X_IRQ(0, 4, EVENT),
+	[TPS6591X_INT_HOTDIE]		= TPS6591X_IRQ(0, 5, EVENT),
+	[TPS6591X_INT_RTC_ALARM]	= TPS6591X_IRQ(0, 6, EVENT),
+	[TPS6591X_INT_RTC_PERIOD]	= TPS6591X_IRQ(0, 7, EVENT),
+	[TPS6591X_INT_GPIO0]		= TPS6591X_IRQ(1, 0, GPIO),
+	[TPS6591X_INT_GPIO1]		= TPS6591X_IRQ(1, 2, GPIO),
+	[TPS6591X_INT_GPIO2]		= TPS6591X_IRQ(1, 4, GPIO),
+	[TPS6591X_INT_GPIO3]		= TPS6591X_IRQ(1, 6, GPIO),
+	[TPS6591X_INT_GPIO4]		= TPS6591X_IRQ(2, 0, GPIO),
+	[TPS6591X_INT_GPIO5]		= TPS6591X_IRQ(2, 2, GPIO),
+	[TPS6591X_INT_WTCHDG]		= TPS6591X_IRQ(2, 4, EVENT),
+	[TPS6591X_INT_VMBCH2_H]		= TPS6591X_IRQ(2, 5, EVENT),
+	[TPS6591X_INT_VMBCH2_L]		= TPS6591X_IRQ(2, 6, EVENT),
+	[TPS6591X_INT_PWRDN]		= TPS6591X_IRQ(2, 7, EVENT),
+};
+
+struct tps6591x {
+	struct mutex		lock;
+	struct device		*dev;
+	struct i2c_client	*client;
+
+	struct gpio_chip	gpio;
+	struct irq_chip		irq_chip;
+	struct mutex		irq_lock;
+	int			irq_base;
+	int			irq_main;
+	u32			irq_en;
+	u8			mask_cache[3];
+	u8			mask_reg[3];
+};
+
+static inline int __tps6591x_read(struct i2c_client *client,
+				  int reg, uint8_t *val)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, reg);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed reading at 0x%02x\n", reg);
+		return ret;
+	}
+
+	*val = (uint8_t)ret;
+
+	return 0;
+}
+
+static inline int __tps6591x_reads(struct i2c_client *client, int reg,
+				int len, uint8_t *val)
+{
+	int ret;
+
+	ret = i2c_smbus_read_i2c_block_data(client, reg, len, val);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed reading from 0x%02x\n", reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+static inline int __tps6591x_write(struct i2c_client *client,
+				 int reg, uint8_t val)
+{
+	int ret;
+	ret = i2c_smbus_write_byte_data(client, reg, val);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed writing 0x%02x to 0x%02x\n",
+				val, reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+static inline int __tps6591x_writes(struct i2c_client *client, int reg,
+				  int len, uint8_t *val)
+{
+	int ret;
+
+	ret = i2c_smbus_write_i2c_block_data(client, reg, len, val);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed writings to 0x%02x\n", reg);
+		return ret;
+	}
+
+	return 0;
+}
+
+int tps6591x_write(struct device *dev, int reg, uint8_t val)
+{
+	struct tps6591x *tps6591x = dev_get_drvdata(dev);
+	int ret = 0;
+
+	mutex_lock(&tps6591x->lock);
+	ret = __tps6591x_write(to_i2c_client(dev), reg, val);
+	mutex_unlock(&tps6591x->lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(tps6591x_write);
+
+int tps6591x_writes(struct device *dev, int reg, int len, uint8_t *val)
+{
+	struct tps6591x *tps6591x = dev_get_drvdata(dev);
+	int ret = 0;
+
+	mutex_lock(&tps6591x->lock);
+	ret = __tps6591x_writes(to_i2c_client(dev), reg, len, val);
+	mutex_unlock(&tps6591x->lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(tps6591x_writes);
+
+int tps6591x_read(struct device *dev, int reg, uint8_t *val)
+{
+	return __tps6591x_read(to_i2c_client(dev), reg, val);
+}
+EXPORT_SYMBOL_GPL(tps6591x_read);
+
+int tps6591x_reads(struct device *dev, int reg, int len, uint8_t *val)
+{
+	return __tps6591x_reads(to_i2c_client(dev), reg, len, val);
+}
+EXPORT_SYMBOL_GPL(tps6591x_reads);
+
+int tps6591x_set_bits(struct device *dev, int reg, uint8_t bit_mask)
+{
+	struct tps6591x *tps6591x = dev_get_drvdata(dev);
+	uint8_t reg_val;
+	int ret = 0;
+
+	mutex_lock(&tps6591x->lock);
+
+	ret = __tps6591x_read(to_i2c_client(dev), reg, &reg_val);
+	if (ret)
+		goto out;
+
+	if ((reg_val & bit_mask) != bit_mask) {
+		reg_val |= bit_mask;
+		ret = __tps6591x_write(to_i2c_client(dev), reg, reg_val);
+	}
+out:
+	mutex_unlock(&tps6591x->lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(tps6591x_set_bits);
+
+int tps6591x_clr_bits(struct device *dev, int reg, uint8_t bit_mask)
+{
+	struct tps6591x *tps6591x = dev_get_drvdata(dev);
+	uint8_t reg_val;
+	int ret = 0;
+
+	mutex_lock(&tps6591x->lock);
+
+	ret = __tps6591x_read(to_i2c_client(dev), reg, &reg_val);
+	if (ret)
+		goto out;
+
+	if (reg_val & bit_mask) {
+		reg_val &= ~bit_mask;
+		ret = __tps6591x_write(to_i2c_client(dev), reg, reg_val);
+	}
+out:
+	mutex_unlock(&tps6591x->lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(tps6591x_clr_bits);
+
+int tps6591x_update(struct device *dev, int reg, uint8_t val, uint8_t mask)
+{
+	struct tps6591x *tps6591x = dev_get_drvdata(dev);
+	uint8_t reg_val;
+	int ret = 0;
+
+	mutex_lock(&tps6591x->lock);
+
+	ret = __tps6591x_read(tps6591x->client, reg, &reg_val);
+	if (ret)
+		goto out;
+
+	if ((reg_val & mask) != val) {
+		reg_val = (reg_val & ~mask) | (val & mask);
+		ret = __tps6591x_write(tps6591x->client, reg, reg_val);
+	}
+out:
+	mutex_unlock(&tps6591x->lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(tps6591x_update);
+
+static struct i2c_client *tps6591x_i2c_client;
+static void tps6591x_power_off(void)
+{
+	struct device *dev = NULL;
+
+	if (!tps6591x_i2c_client)
+		return;
+
+	dev = &tps6591x_i2c_client->dev;
+
+	if (tps6591x_set_bits(dev, TPS6591X_DEVCTRL, DEVCTRL_PWR_OFF_SEQ) < 0)
+		return;
+
+	tps6591x_clr_bits(dev, TPS6591X_DEVCTRL, DEVCTRL_DEV_ON);
+}
+
+static int tps6591x_gpio_get(struct gpio_chip *gc, unsigned offset)
+{
+	struct tps6591x *tps6591x = container_of(gc, struct tps6591x, gpio);
+	uint8_t val;
+	int ret;
+
+	ret = __tps6591x_read(tps6591x->client, TPS6591X_GPIO_BASE_ADDR +
+			offset,	&val);
+	if (ret)
+		return ret;
+
+	if (val & 0x4)
+		return val & 0x1;
+	else
+		return (val & 0x2) ? 1 : 0;
+}
+
+static void tps6591x_gpio_set(struct gpio_chip *chip, unsigned offset,
+			int value)
+{
+
+	struct tps6591x *tps6591x = container_of(chip, struct tps6591x, gpio);
+
+	tps6591x_update(tps6591x->dev, TPS6591X_GPIO_BASE_ADDR + offset,
+			value, 0x1);
+}
+
+static int tps6591x_gpio_input(struct gpio_chip *gc, unsigned offset)
+{
+	struct tps6591x *tps6591x = container_of(gc, struct tps6591x, gpio);
+	uint8_t reg_val;
+	int ret;
+
+	ret = __tps6591x_read(tps6591x->client, TPS6591X_GPIO_BASE_ADDR +
+			offset,	&reg_val);
+	if (ret)
+		return ret;
+
+	reg_val &= ~0x4;
+	return __tps6591x_write(tps6591x->client, TPS6591X_GPIO_BASE_ADDR +
+			offset,	reg_val);
+}
+
+static int tps6591x_gpio_output(struct gpio_chip *gc, unsigned offset,
+				int value)
+{
+	struct tps6591x *tps6591x = container_of(gc, struct tps6591x, gpio);
+	uint8_t reg_val, val;
+	int ret;
+
+	ret = __tps6591x_read(tps6591x->client, TPS6591X_GPIO_BASE_ADDR +
+			offset,	&reg_val);
+	if (ret)
+		return ret;
+
+	reg_val &= ~0x1;
+	val = (value & 0x1) | 0x4;
+	reg_val = reg_val | val;
+	return __tps6591x_write(tps6591x->client, TPS6591X_GPIO_BASE_ADDR +
+			offset,	reg_val);
+}
+
+static int tps6591x_gpio_to_irq(struct gpio_chip *gc, unsigned off)
+{
+	struct tps6591x *tps6591x;
+	tps6591x = container_of(gc, struct tps6591x, gpio);
+
+	if ((off >= 0) && (off <= TPS6591X_INT_GPIO5 - TPS6591X_INT_GPIO0))
+		return tps6591x->irq_base + TPS6591X_INT_GPIO0 + off;
+
+	return -EIO;
+}
+
+static void tps6591x_gpio_init(struct tps6591x *tps6591x,
+			struct tps6591x_platform_data *pdata)
+{
+	int ret;
+	int gpio_base = pdata->gpio_base;
+	int i;
+	u8 gpio_reg;
+	struct tps6591x_gpio_init_data *ginit;
+
+	if (gpio_base <= 0)
+		return;
+
+	for (i = 0; i < pdata->num_gpioinit_data; ++i) {
+		ginit = &pdata->gpio_init_data[i];
+		if (!ginit->init_apply)
+			continue;
+		gpio_reg = (ginit->sleep_en << TPS6591X_GPIO_SLEEP) |
+				(ginit->pulldn_en << TPS6591X_GPIO_PDEN) |
+				(ginit->output_mode_en << TPS6591X_GPIO_DIR);
+
+		if (ginit->output_mode_en)
+			gpio_reg |= ginit->output_val;
+
+		ret =  __tps6591x_write(tps6591x->client,
+				TPS6591X_GPIO_BASE_ADDR + i, gpio_reg);
+		if (ret < 0)
+			dev_err(&tps6591x->client->dev, "Gpio %d init "
+				"configuration failed: %d\n", i, ret);
+	}
+
+	tps6591x->gpio.owner		= THIS_MODULE;
+	tps6591x->gpio.label		= tps6591x->client->name;
+	tps6591x->gpio.dev		= tps6591x->dev;
+	tps6591x->gpio.base		= gpio_base;
+	tps6591x->gpio.ngpio		= TPS6591X_GPIO_NR;
+	tps6591x->gpio.can_sleep	= 1;
+
+	tps6591x->gpio.direction_input	= tps6591x_gpio_input;
+	tps6591x->gpio.direction_output	= tps6591x_gpio_output;
+	tps6591x->gpio.set		= tps6591x_gpio_set;
+	tps6591x->gpio.get		= tps6591x_gpio_get;
+	tps6591x->gpio.to_irq		= tps6591x_gpio_to_irq;
+
+	ret = gpiochip_add(&tps6591x->gpio);
+	if (ret)
+		dev_warn(tps6591x->dev, "GPIO registration failed: %d\n", ret);
+}
+
+static int __remove_subdev(struct device *dev, void *unused)
+{
+	platform_device_unregister(to_platform_device(dev));
+	return 0;
+}
+
+static int tps6591x_remove_subdevs(struct tps6591x *tps6591x)
+{
+	return device_for_each_child(tps6591x->dev, NULL, __remove_subdev);
+}
+
+static void tps6591x_irq_lock(struct irq_data *data)
+{
+	struct tps6591x *tps6591x = irq_data_get_irq_chip_data(data);
+
+	mutex_lock(&tps6591x->irq_lock);
+}
+
+static void tps6591x_irq_mask(struct irq_data *irq_data)
+{
+	struct tps6591x *tps6591x = irq_data_get_irq_chip_data(irq_data);
+	unsigned int __irq = irq_data->irq - tps6591x->irq_base;
+	const struct tps6591x_irq_data *data = &tps6591x_irqs[__irq];
+
+	if (data->type == EVENT)
+		tps6591x->mask_reg[data->mask_reg] |= (1 << data->mask_pos);
+	else
+		tps6591x->mask_reg[data->mask_reg] |= (3 << data->mask_pos);
+
+	tps6591x->irq_en &= ~(1 << __irq);
+}
+
+static void tps6591x_irq_unmask(struct irq_data *irq_data)
+{
+	struct tps6591x *tps6591x = irq_data_get_irq_chip_data(irq_data);
+
+	unsigned int __irq = irq_data->irq - tps6591x->irq_base;
+	const struct tps6591x_irq_data *data = &tps6591x_irqs[__irq];
+
+	if (data->type == EVENT) {
+		tps6591x->mask_reg[data->mask_reg] &= ~(1 << data->mask_pos);
+		tps6591x->irq_en |= (1 << __irq);
+	}
+}
+
+static void tps6591x_irq_sync_unlock(struct irq_data *data)
+{
+	struct tps6591x *tps6591x = irq_data_get_irq_chip_data(data);
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(tps6591x->mask_reg); i++) {
+		if (tps6591x->mask_reg[i] != tps6591x->mask_cache[i]) {
+			if (!WARN_ON(tps6591x_write(tps6591x->dev,
+						TPS6591X_INT_MSK + 2*i,
+						tps6591x->mask_reg[i])))
+				tps6591x->mask_cache[i] = tps6591x->mask_reg[i];
+		}
+	}
+
+	mutex_unlock(&tps6591x->irq_lock);
+}
+
+static int tps6591x_irq_set_type(struct irq_data *irq_data, unsigned int type)
+{
+	struct tps6591x *tps6591x = irq_data_get_irq_chip_data(irq_data);
+
+	unsigned int __irq = irq_data->irq - tps6591x->irq_base;
+	const struct tps6591x_irq_data *data = &tps6591x_irqs[__irq];
+
+	if (data->type == GPIO) {
+		if (type & IRQ_TYPE_EDGE_FALLING)
+			tps6591x->mask_reg[data->mask_reg]
+				&= ~(1 << data->mask_pos);
+		else
+			tps6591x->mask_reg[data->mask_reg]
+				|= (1 << data->mask_pos);
+
+		if (type & IRQ_TYPE_EDGE_RISING)
+			tps6591x->mask_reg[data->mask_reg]
+				&= ~(2 << data->mask_pos);
+		else
+			tps6591x->mask_reg[data->mask_reg]
+				|= (2 << data->mask_pos);
+
+		tps6591x->irq_en |= (1 << __irq);
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int tps6591x_irq_set_wake(struct irq_data *irq_data, unsigned int on)
+{
+	struct tps6591x *tps6591x = irq_data_get_irq_chip_data(irq_data);
+	return irq_set_irq_wake(tps6591x->irq_main, on);
+}
+#else
+#define tps6591x_irq_set_wake NULL
+#endif
+
+
+static irqreturn_t tps6591x_irq(int irq, void *data)
+{
+	struct tps6591x *tps6591x = data;
+	int ret = 0;
+	u8 tmp[3];
+	u8 int_ack;
+	u32 acks, mask = 0;
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		ret = tps6591x_read(tps6591x->dev, TPS6591X_INT_STS + 2*i,
+				&tmp[i]);
+		if (ret < 0) {
+			dev_err(tps6591x->dev,
+				"failed to read interrupt status\n");
+			return IRQ_NONE;
+		}
+		if (tmp[i]) {
+			/* Ack only those interrupts which are enabled */
+			int_ack = tmp[i] & (~(tps6591x->mask_cache[i]));
+			ret = tps6591x_write(tps6591x->dev,
+					TPS6591X_INT_STS + 2*i,	int_ack);
+			if (ret < 0) {
+				dev_err(tps6591x->dev,
+					"failed to write interrupt status\n");
+				return IRQ_NONE;
+			}
+		}
+	}
+
+	acks = (tmp[2] << 16) | (tmp[1] << 8) | tmp[0];
+
+	for (i = 0; i < ARRAY_SIZE(tps6591x_irqs); i++) {
+		if (tps6591x_irqs[i].type == GPIO)
+			mask = (3 << (tps6591x_irqs[i].mask_pos
+					+ tps6591x_irqs[i].mask_reg*8));
+		else if (tps6591x_irqs[i].type == EVENT)
+			mask = (1 << (tps6591x_irqs[i].mask_pos
+					+ tps6591x_irqs[i].mask_reg*8));
+
+		if ((acks & mask) && (tps6591x->irq_en & (1 << i)))
+			handle_nested_irq(tps6591x->irq_base + i);
+	}
+	return IRQ_HANDLED;
+}
+
+static int __devinit tps6591x_irq_init(struct tps6591x *tps6591x, int irq,
+				int irq_base)
+{
+	int i, ret;
+
+	if (!irq_base) {
+		dev_warn(tps6591x->dev, "No interrupt support on IRQ base\n");
+		return -EINVAL;
+	}
+
+	mutex_init(&tps6591x->irq_lock);
+
+	tps6591x->mask_reg[0] = 0xFF;
+	tps6591x->mask_reg[1] = 0xFF;
+	tps6591x->mask_reg[2] = 0xFF;
+	for (i = 0; i < 3; i++) {
+		tps6591x->mask_cache[i] = tps6591x->mask_reg[i];
+		tps6591x_write(tps6591x->dev, TPS6591X_INT_MSK + 2*i,
+				 tps6591x->mask_cache[i]);
+	}
+
+	for (i = 0; i < 3; i++)
+		tps6591x_write(tps6591x->dev, TPS6591X_INT_STS + 2*i, 0xff);
+
+	tps6591x->irq_base = irq_base;
+	tps6591x->irq_main = irq;
+
+	tps6591x->irq_chip.name = "tps6591x";
+	tps6591x->irq_chip.irq_mask = tps6591x_irq_mask;
+	tps6591x->irq_chip.irq_unmask = tps6591x_irq_unmask;
+	tps6591x->irq_chip.irq_bus_lock = tps6591x_irq_lock;
+	tps6591x->irq_chip.irq_bus_sync_unlock = tps6591x_irq_sync_unlock;
+	tps6591x->irq_chip.irq_set_type = tps6591x_irq_set_type;
+	tps6591x->irq_chip.irq_set_wake = tps6591x_irq_set_wake;
+
+	for (i = 0; i < ARRAY_SIZE(tps6591x_irqs); i++) {
+		int __irq = i + tps6591x->irq_base;
+		irq_set_chip_data(__irq, tps6591x);
+		irq_set_chip_and_handler(__irq, &tps6591x->irq_chip,
+					 handle_simple_irq);
+		irq_set_nested_thread(__irq, 1);
+#ifdef CONFIG_ARM
+		set_irq_flags(__irq, IRQF_VALID);
+#endif
+	}
+
+	ret = request_threaded_irq(irq, NULL, tps6591x_irq, IRQF_ONESHOT,
+				"tps6591x", tps6591x);
+	if (!ret) {
+		device_init_wakeup(tps6591x->dev, 1);
+		enable_irq_wake(irq);
+	}
+
+	return ret;
+}
+
+static int __devinit tps6591x_add_subdevs(struct tps6591x *tps6591x,
+					  struct tps6591x_platform_data *pdata)
+{
+	struct tps6591x_subdev_info *subdev;
+	struct platform_device *pdev;
+	int i, ret = 0;
+
+	for (i = 0; i < pdata->num_subdevs; i++) {
+		subdev = &pdata->subdevs[i];
+
+		pdev = platform_device_alloc(subdev->name, subdev->id);
+
+		pdev->dev.parent = tps6591x->dev;
+		pdev->dev.platform_data = subdev->platform_data;
+
+		ret = platform_device_add(pdev);
+		if (ret)
+			goto failed;
+	}
+	return 0;
+
+failed:
+	tps6591x_remove_subdevs(tps6591x);
+	return ret;
+}
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+static void print_regs(const char *header, struct seq_file *s,
+		struct i2c_client *client, int start_offset,
+		int end_offset)
+{
+	uint8_t reg_val;
+	int i;
+	int ret;
+
+	seq_printf(s, "%s\n", header);
+	for (i = start_offset; i <= end_offset; ++i) {
+		ret = __tps6591x_read(client, i, &reg_val);
+		if (ret >= 0)
+			seq_printf(s, "Reg 0x%02x Value 0x%02x\n", i, reg_val);
+	}
+	seq_printf(s, "------------------\n");
+}
+
+static int dbg_tps_show(struct seq_file *s, void *unused)
+{
+	struct tps6591x *tps = s->private;
+	struct i2c_client *client = tps->client;
+
+	seq_printf(s, "TPS6591x Registers\n");
+	seq_printf(s, "------------------\n");
+
+	print_regs("Timing Regs",    s, client, 0x0, 0x6);
+	print_regs("Alarm Regs",     s, client, 0x8, 0xD);
+	print_regs("RTC Regs",       s, client, 0x10, 0x16);
+	print_regs("BCK Regs",       s, client, 0x17, 0x1B);
+	print_regs("PUADEN Regs",    s, client, 0x18, 0x18);
+	print_regs("REF Regs",       s, client, 0x1D, 0x1D);
+	print_regs("VDD Regs",       s, client, 0x1E, 0x29);
+	print_regs("LDO Regs",       s, client, 0x30, 0x37);
+	print_regs("THERM Regs",     s, client, 0x38, 0x38);
+	print_regs("BBCH Regs",      s, client, 0x39, 0x39);
+	print_regs("DCDCCNTRL Regs", s, client, 0x3E, 0x3E);
+	print_regs("DEV_CNTRL Regs", s, client, 0x3F, 0x40);
+	print_regs("SLEEP Regs",     s, client, 0x41, 0x44);
+	print_regs("EN1 Regs",       s, client, 0x45, 0x48);
+	print_regs("INT Regs",       s, client, 0x50, 0x55);
+	print_regs("GPIO Regs",      s, client, 0x60, 0x68);
+	print_regs("WATCHDOG Regs",  s, client, 0x69, 0x69);
+	print_regs("VMBCH Regs",     s, client, 0x6A, 0x6B);
+	print_regs("LED_CTRL Regs",  s, client, 0x6c, 0x6D);
+	print_regs("PWM_CTRL Regs",  s, client, 0x6E, 0x6F);
+	print_regs("SPARE Regs",     s, client, 0x70, 0x70);
+	print_regs("VERNUM Regs",    s, client, 0x80, 0x80);
+	return 0;
+}
+
+static int dbg_tps_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dbg_tps_show, inode->i_private);
+}
+
+static const struct file_operations debug_fops = {
+	.open		= dbg_tps_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static void __init tps6591x_debuginit(struct tps6591x *tps)
+{
+	(void)debugfs_create_file("tps6591x", S_IRUGO, NULL,
+			tps, &debug_fops);
+}
+#else
+static void __init tps6591x_debuginit(struct tps6591x *tpsi)
+{
+	return;
+}
+#endif
+
+static int __init tps6591x_sleepinit(struct tps6591x *tpsi,
+					struct tps6591x_platform_data *pdata)
+{
+	struct device *dev = NULL;
+	int ret = 0;
+
+	dev = tpsi->dev;
+
+	if (!pdata->dev_slp_en)
+		goto no_err_return;
+
+	/* pmu dev_slp_en is set. Make sure slp_keepon is available before
+	 * allowing SLEEP device state */
+	if (!pdata->slp_keepon) {
+		dev_err(dev, "slp_keepon_data required for slp_en\n");
+		goto err_sleep_init;
+	}
+
+	/* enabling SLEEP device state */
+	ret = tps6591x_set_bits(dev, TPS6591X_DEVCTRL, DEVCTRL_DEV_SLP);
+	if (ret < 0) {
+		dev_err(dev, "set dev_slp failed: %d\n", ret);
+		goto err_sleep_init;
+	}
+
+	if (pdata->slp_keepon->therm_keepon) {
+		ret = tps6591x_set_bits(dev, TPS6591X_SLEEP_KEEP_ON,
+						SLEEP_KEEP_ON_THERM);
+		if (ret < 0) {
+			dev_err(dev, "set therm_keepon failed: %d\n", ret);
+			goto disable_dev_slp;
+		}
+	}
+
+	if (pdata->slp_keepon->clkout32k_keepon) {
+		ret = tps6591x_set_bits(dev, TPS6591X_SLEEP_KEEP_ON,
+						SLEEP_KEEP_ON_CLKOUT32K);
+		if (ret < 0) {
+			dev_err(dev, "set clkout32k_keepon failed: %d\n", ret);
+			goto disable_dev_slp;
+		}
+	}
+
+
+	if (pdata->slp_keepon->vrtc_keepon) {
+		ret = tps6591x_set_bits(dev, TPS6591X_SLEEP_KEEP_ON,
+						SLEEP_KEEP_ON_VRTC);
+		if (ret < 0) {
+			dev_err(dev, "set vrtc_keepon failed: %d\n", ret);
+			goto disable_dev_slp;
+		}
+	}
+
+	if (pdata->slp_keepon->i2chs_keepon) {
+		ret = tps6591x_set_bits(dev, TPS6591X_SLEEP_KEEP_ON,
+						SLEEP_KEEP_ON_I2CHS);
+		if (ret < 0) {
+			dev_err(dev, "set i2chs_keepon failed: %d\n", ret);
+			goto disable_dev_slp;
+		}
+	}
+
+no_err_return:
+	return 0;
+
+disable_dev_slp:
+	tps6591x_clr_bits(dev, TPS6591X_DEVCTRL, DEVCTRL_DEV_SLP);
+
+err_sleep_init:
+	return ret;
+}
+
+static int __devinit tps6591x_i2c_probe(struct i2c_client *client,
+					const struct i2c_device_id *id)
+{
+	struct tps6591x_platform_data *pdata = client->dev.platform_data;
+	struct tps6591x *tps6591x;
+	int ret;
+
+	if (!pdata) {
+		dev_err(&client->dev, "tps6591x requires platform data\n");
+		return -ENOTSUPP;
+	}
+
+	ret = i2c_smbus_read_byte_data(client, TPS6591X_VERNUM);
+	if (ret < 0) {
+		dev_err(&client->dev, "Silicon version number read"
+				" failed: %d\n", ret);
+		return -EIO;
+	}
+
+	dev_info(&client->dev, "VERNUM is %02x\n", ret);
+
+	tps6591x = kzalloc(sizeof(struct tps6591x), GFP_KERNEL);
+	if (tps6591x == NULL)
+		return -ENOMEM;
+
+	tps6591x->client = client;
+	tps6591x->dev = &client->dev;
+	i2c_set_clientdata(client, tps6591x);
+
+	mutex_init(&tps6591x->lock);
+
+	if (client->irq) {
+		ret = tps6591x_irq_init(tps6591x, client->irq,
+					pdata->irq_base);
+		if (ret) {
+			dev_err(&client->dev, "IRQ init failed: %d\n", ret);
+			goto err_irq_init;
+		}
+	}
+
+	ret = tps6591x_add_subdevs(tps6591x, pdata);
+	if (ret) {
+		dev_err(&client->dev, "add devices failed: %d\n", ret);
+		goto err_add_devs;
+	}
+
+	tps6591x_gpio_init(tps6591x, pdata);
+
+	tps6591x_debuginit(tps6591x);
+
+	tps6591x_sleepinit(tps6591x, pdata);
+
+	if (pdata->use_power_off && !pm_power_off)
+		pm_power_off = tps6591x_power_off;
+
+	tps6591x_i2c_client = client;
+
+	return 0;
+
+err_add_devs:
+	if (client->irq)
+		free_irq(client->irq, tps6591x);
+err_irq_init:
+	kfree(tps6591x);
+	return ret;
+}
+
+static int __devexit tps6591x_i2c_remove(struct i2c_client *client)
+{
+	struct tps6591x *tps6591x = i2c_get_clientdata(client);
+
+	if (client->irq)
+		free_irq(client->irq, tps6591x);
+
+	if (gpiochip_remove(&tps6591x->gpio) < 0)
+		dev_err(&client->dev, "Error in removing the gpio driver\n");
+
+	kfree(tps6591x);
+	return 0;
+}
+#ifdef CONFIG_PM
+static int tps6591x_i2c_suspend(struct i2c_client *client, pm_message_t state)
+{
+	if (client->irq)
+		disable_irq(client->irq);
+	return 0;
+}
+
+static int tps6591x_i2c_resume(struct i2c_client *client)
+{
+	if (client->irq)
+		enable_irq(client->irq);
+	return 0;
+}
+#endif
+
+
+static const struct i2c_device_id tps6591x_id_table[] = {
+	{ "tps6591x", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, tps6591x_id_table);
+
+static struct i2c_driver tps6591x_driver = {
+	.driver	= {
+		.name	= "tps6591x",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= tps6591x_i2c_probe,
+	.remove		= __devexit_p(tps6591x_i2c_remove),
+#ifdef CONFIG_PM
+	.suspend	= tps6591x_i2c_suspend,
+	.resume		= tps6591x_i2c_resume,
+#endif
+	.id_table	= tps6591x_id_table,
+};
+
+static int __init tps6591x_init(void)
+{
+	return i2c_add_driver(&tps6591x_driver);
+}
+subsys_initcall(tps6591x_init);
+
+static void __exit tps6591x_exit(void)
+{
+	i2c_del_driver(&tps6591x_driver);
+}
+module_exit(tps6591x_exit);
+
+MODULE_DESCRIPTION("TPS6591X core driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/tps80031.c b/drivers/mfd/tps80031.c
new file mode 100644
index 000000000000..002b7d022d70
--- /dev/null
+++ b/drivers/mfd/tps80031.c
@@ -0,0 +1,1415 @@
+/*
+ * driver/mfd/tps80031.c
+ *
+ * Core driver for TI TPS80031
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/pm.h>
+#include <linux/regmap.h>
+
+#include <linux/mfd/core.h>
+#include <linux/mfd/tps80031.h>
+
+/* interrupt related registers */
+#define TPS80031_INT_STS_A		0xD0
+#define TPS80031_INT_STS_B		0xD1
+#define TPS80031_INT_STS_C		0xD2
+#define TPS80031_INT_MSK_LINE_A		0xD3
+#define TPS80031_INT_MSK_LINE_B		0xD4
+#define TPS80031_INT_MSK_LINE_C		0xD5
+#define TPS80031_INT_MSK_STS_A		0xD6
+#define TPS80031_INT_MSK_STS_B		0xD7
+#define TPS80031_INT_MSK_STS_C		0xD8
+
+#define TPS80031_CONTROLLER_STAT1		0xE3
+#define CONTROLLER_STAT1_BAT_TEMP		0
+#define CONTROLLER_STAT1_BAT_REMOVED		1
+#define CONTROLLER_STAT1_VBUS_DET		2
+#define CONTROLLER_STAT1_VAC_DET		3
+#define CONTROLLER_STAT1_FAULT_WDG		4
+#define CONTROLLER_STAT1_LINCH_GATED		6
+
+#define TPS80031_CONTROLLER_INT_MASK		0xE0
+#define CONTROLLER_INT_MASK_MVAC_DET		0
+#define CONTROLLER_INT_MASK_MVBUS_DET		1
+#define CONTROLLER_INT_MASK_MBAT_TEMP		2
+#define CONTROLLER_INT_MASK_MFAULT_WDG		3
+#define CONTROLLER_INT_MASK_MBAT_REMOVED	4
+#define CONTROLLER_INT_MASK_MLINCH_GATED	5
+
+#define CHARGE_CONTROL_SUB_INT_MASK		0x3F
+
+/* Version number related register */
+#define TPS80031_JTAGVERNUM		0x87
+/* Epprom version */
+#define TPS80031_EPROM_REV		0xDF
+
+/* External control register */
+#define REGEN1_BASE_ADD		0xAE
+#define REGEN2_BASE_ADD		0xB1
+#define SYSEN_BASE_ADD		0xB4
+
+/* device control registers */
+#define TPS80031_PHOENIX_DEV_ON	0x25
+#define DEVOFF	1
+
+#define CLK32KAO_BASE_ADD	0xBA
+#define CLK32KG_BASE_ADD	0xBD
+#define CLK32KAUDIO_BASE_ADD	0xC0
+
+#define EXT_CONTROL_CFG_TRANS 0
+#define EXT_CONTROL_CFG_STATE 1
+
+#define STATE_OFF	0x00
+#define STATE_ON	0x01
+#define STATE_MASK	0x03
+
+#define TRANS_SLEEP_OFF		0x00
+#define TRANS_SLEEP_ON		0x04
+#define TRANS_SLEEP_MASK	0x0C
+
+#define TPS_NUM_SLAVES	4
+#define EXT_PWR_REQ (PWR_REQ_INPUT_PREQ1 | PWR_REQ_INPUT_PREQ2 | \
+		PWR_REQ_INPUT_PREQ3)
+#define TPS80031_PREQ1_RES_ASS_A	0xD7
+#define TPS80031_PREQ2_RES_ASS_A	0xDA
+#define TPS80031_PREQ3_RES_ASS_A	0xDD
+#define TPS80031_PHOENIX_MSK_TRANSITION 0x20
+
+#define TPS80031_CFG_INPUT_PUPD1 0xF0
+#define TPS80031_CFG_INPUT_PUPD2 0xF1
+#define TPS80031_CFG_INPUT_PUPD3 0xF2
+#define TPS80031_CFG_INPUT_PUPD4 0xF3
+
+#define TPS80031_BBSPOR_CFG	0xE6
+#define TPS80031_BBSPOR_CHG_EN	0x8
+
+/* Valid Address ranges */
+#define TPS80031_ID0_PMIC_SLAVE_SMPS_DVS	0x55 ... 0x5C
+
+#define TPS80031_ID1_RTC			0x00 ... 0x16
+#define TPS80031_ID1_MEMORY			0x17 ... 0x1E
+#define TPS80031_ID1_PMC_MASTER			0x1F ... 0x2D
+#define TPS80031_ID1_PMC_SLAVE_MISC		0x31 ... 0x34
+#define TPS80031_ID1_PMC_SLAVE_SMPS		0x40 ... 0x68
+#define TPS80031_ID1_PMC_SLAVE_LDO		0x80 ... 0xA7
+#define TPS80031_ID1_PMC_SLAVE_REOSURCES	0XAD ... 0xD0
+#define TPS80031_ID1_PMC_PREQ_ASSIGN		0XD7 ... 0xDF
+#define TPS80031_ID1_PMC_MISC			0xE2 ... 0xEF
+#define TPS80031_ID1_PMC_PU_PD_HZ		0xF0 ... 0xF6
+#define TPS80031_ID1_PMC_BACKUP			0xFA
+
+#define TPS80031_ID2_USB			0x00 ... 0x1A
+#define TPS80031_ID2_GPADC_CONTROL		0x2E ... 0x36
+#define TPS80031_ID2_GPADC_RESULTS		0x37 ... 0x3C
+#define TPS80031_ID2_AUXILLIARIES		0x90 ... 0x9C
+#define TPS80031_ID2_CUSTOM			0xA0 ... 0xB9
+#define TPS80031_ID2_PWM			0xBA ... 0xBE
+#define TPS80031_ID2_FUEL_GAUSE			0xC0 ... 0xCB
+#define TPS80031_ID2_INTERFACE_INTERRUPTS	0xD0 ... 0xD8
+#define TPS80031_ID2_CHARGER			0xDA ... 0xF5
+
+#define TPS80031_ID3_TEST_LDO			0x00 ... 0x09
+#define TPS80031_ID3_TEST_SMPS			0x10 ... 0x2B
+#define TPS80031_ID3_TEST_POWER			0x30 ... 0x36
+#define TPS80031_ID3_TEST_CHARGER		0x40 ... 0x48
+#define TPS80031_ID3_TEST_AUXILIIARIES		0x50 ... 0xB1
+
+#define TPS80031_ID3_DIEID			0xC0 ... 0xC8
+#define TPS80031_ID3_TRIM_PHOENIX		0xCC ... 0xEA
+#define TPS80031_ID3_TRIM_CUSTOM		0xEC ... 0xED
+
+#define TPS80031_MAX_REGISTER	0x100
+
+struct tps80031_pupd_data {
+	u8	reg;
+	u8	pullup_bit;
+	u8	pulldown_bit;
+};
+
+static u8 pmc_ext_control_base[] = {
+	REGEN1_BASE_ADD,
+	REGEN2_BASE_ADD,
+	SYSEN_BASE_ADD,
+};
+
+static u8 pmc_clk32k_control_base[] = {
+	CLK32KAO_BASE_ADD,
+	CLK32KG_BASE_ADD,
+	CLK32KAUDIO_BASE_ADD,
+};
+struct tps80031_irq_data {
+	u8	mask_reg;
+	u8	mask_mask;
+	u8	is_sec_int;
+	u8	parent_int;
+	u8	mask_sec_int_reg;
+	u8	int_mask_bit;
+	u8	int_sec_sts_reg;
+	u8	int_sts_bit;
+};
+
+#define TPS80031_IRQ(_reg, _mask)	\
+	{							\
+		.mask_reg = (TPS80031_INT_MSK_LINE_##_reg) -	\
+				TPS80031_INT_MSK_LINE_A,	\
+		.mask_mask = (_mask),				\
+	}
+
+#define TPS80031_IRQ_SEC(_reg, _mask, _pint, _sint_mask_bit, _sint_sts_bit) \
+	{								\
+		.mask_reg = (TPS80031_INT_MSK_LINE_##_reg) -		\
+				TPS80031_INT_MSK_LINE_A,		\
+		.mask_mask = (_mask),					\
+		.is_sec_int = true,					\
+		.parent_int = TPS80031_INT_##_pint,			\
+		.mask_sec_int_reg = TPS80031_CONTROLLER_INT_MASK,	\
+		.int_mask_bit = CONTROLLER_INT_MASK_##_sint_mask_bit,	\
+		.int_sec_sts_reg = TPS80031_CONTROLLER_STAT1,		\
+		.int_sts_bit = CONTROLLER_STAT1_##_sint_sts_bit		\
+	}
+
+static const struct tps80031_irq_data tps80031_irqs[] = {
+
+	[TPS80031_INT_PWRON]		= TPS80031_IRQ(A, 0),
+	[TPS80031_INT_RPWRON]		= TPS80031_IRQ(A, 1),
+	[TPS80031_INT_SYS_VLOW]		= TPS80031_IRQ(A, 2),
+	[TPS80031_INT_RTC_ALARM]	= TPS80031_IRQ(A, 3),
+	[TPS80031_INT_RTC_PERIOD]	= TPS80031_IRQ(A, 4),
+	[TPS80031_INT_HOT_DIE]		= TPS80031_IRQ(A, 5),
+	[TPS80031_INT_VXX_SHORT]	= TPS80031_IRQ(A, 6),
+	[TPS80031_INT_SPDURATION]	= TPS80031_IRQ(A, 7),
+	[TPS80031_INT_WATCHDOG]		= TPS80031_IRQ(B, 0),
+	[TPS80031_INT_BAT]		= TPS80031_IRQ(B, 1),
+	[TPS80031_INT_SIM]		= TPS80031_IRQ(B, 2),
+	[TPS80031_INT_MMC]		= TPS80031_IRQ(B, 3),
+	[TPS80031_INT_RES]		= TPS80031_IRQ(B, 4),
+	[TPS80031_INT_GPADC_RT]		= TPS80031_IRQ(B, 5),
+	[TPS80031_INT_GPADC_SW2_EOC]	= TPS80031_IRQ(B, 6),
+	[TPS80031_INT_CC_AUTOCAL]	= TPS80031_IRQ(B, 7),
+	[TPS80031_INT_ID_WKUP]		= TPS80031_IRQ(C, 0),
+	[TPS80031_INT_VBUSS_WKUP]	= TPS80031_IRQ(C, 1),
+	[TPS80031_INT_ID]		= TPS80031_IRQ(C, 2),
+	[TPS80031_INT_VBUS]		= TPS80031_IRQ(C, 3),
+	[TPS80031_INT_CHRG_CTRL]	= TPS80031_IRQ(C, 4),
+	[TPS80031_INT_EXT_CHRG]		= TPS80031_IRQ(C, 5),
+	[TPS80031_INT_INT_CHRG]		= TPS80031_IRQ(C, 6),
+	[TPS80031_INT_RES2]		= TPS80031_IRQ(C, 7),
+	[TPS80031_INT_BAT_TEMP_OVRANGE]	= TPS80031_IRQ_SEC(C, 4, CHRG_CTRL,
+						MBAT_TEMP,	BAT_TEMP),
+	[TPS80031_INT_BAT_REMOVED]	= TPS80031_IRQ_SEC(C, 4, CHRG_CTRL,
+						MBAT_REMOVED,	BAT_REMOVED),
+	[TPS80031_INT_VBUS_DET]		= TPS80031_IRQ_SEC(C, 4, CHRG_CTRL,
+						MVBUS_DET,	VBUS_DET),
+	[TPS80031_INT_VAC_DET]		= TPS80031_IRQ_SEC(C, 4, CHRG_CTRL,
+						MVAC_DET,	VAC_DET),
+	[TPS80031_INT_FAULT_WDG]	= TPS80031_IRQ_SEC(C, 4, CHRG_CTRL,
+						MFAULT_WDG,	FAULT_WDG),
+	[TPS80031_INT_LINCH_GATED]	= TPS80031_IRQ_SEC(C, 4, CHRG_CTRL,
+						MLINCH_GATED,	LINCH_GATED),
+};
+
+#define PUPD_DATA(_reg, _pulldown_bit, _pullup_bit)	\
+	{						\
+		.reg = TPS80031_CFG_INPUT_PUPD##_reg,	\
+		.pulldown_bit = _pulldown_bit,		\
+		.pullup_bit = _pullup_bit,		\
+	}
+
+static const struct tps80031_pupd_data tps80031_pupds[] = {
+	[TPS80031_PREQ1]		= PUPD_DATA(1, 1 << 0,	1 << 1	),
+	[TPS80031_PREQ2A]		= PUPD_DATA(1, 1 << 2,	1 << 3	),
+	[TPS80031_PREQ2B]		= PUPD_DATA(1, 1 << 4,	1 << 5	),
+	[TPS80031_PREQ2C]		= PUPD_DATA(1, 1 << 6,	1 << 7	),
+	[TPS80031_PREQ3]		= PUPD_DATA(2, 1 << 0,	1 << 1	),
+	[TPS80031_NRES_WARM]		= PUPD_DATA(2, 0,	1 << 2	),
+	[TPS80031_PWM_FORCE]		= PUPD_DATA(2, 1 << 5,	0	),
+	[TPS80031_CHRG_EXT_CHRG_STATZ]	= PUPD_DATA(2, 0,	1 << 6	),
+	[TPS80031_SIM]			= PUPD_DATA(3, 1 << 0,	1 << 1	),
+	[TPS80031_MMC]			= PUPD_DATA(3, 1 << 2,	1 << 3	),
+	[TPS80031_GPADC_START]		= PUPD_DATA(3, 1 << 4,	0	),
+	[TPS80031_DVSI2C_SCL]		= PUPD_DATA(4, 0,	1 << 0	),
+	[TPS80031_DVSI2C_SDA]		= PUPD_DATA(4, 0,	1 << 1	),
+	[TPS80031_CTLI2C_SCL]		= PUPD_DATA(4, 0,	1 << 2	),
+	[TPS80031_CTLI2C_SDA]		= PUPD_DATA(4, 0,	1 << 3	),
+};
+
+static const int controller_stat1_irq_nr[] = {
+	TPS80031_INT_BAT_TEMP_OVRANGE,
+	TPS80031_INT_BAT_REMOVED,
+	TPS80031_INT_VBUS_DET,
+	TPS80031_INT_VAC_DET,
+	TPS80031_INT_FAULT_WDG,
+	0,
+	TPS80031_INT_LINCH_GATED,
+	0
+};
+
+/* Structure for TPS80031 Slaves */
+struct tps80031_client {
+	struct i2c_client *client;
+	struct mutex lock;
+	u8 addr;
+};
+
+struct tps80031 {
+	struct device		*dev;
+	unsigned long		chip_info;
+	int			es_version;
+
+	struct gpio_chip	gpio;
+	struct irq_chip		irq_chip;
+	struct mutex		irq_lock;
+	int			irq_base;
+	u32			irq_en;
+	u8			mask_cache[3];
+	u8			mask_reg[3];
+	u8			cont_int_mask_reg;
+	u8			cont_int_mask_cache;
+	u8			cont_int_en;
+	u8			prev_cont_stat1;
+	struct tps80031_client	tps_clients[TPS_NUM_SLAVES];
+	struct regmap		*regmap[TPS_NUM_SLAVES];
+};
+
+/* TPS80031 sub mfd devices */
+static struct mfd_cell tps80031_cell[] = {
+	{
+		.name = "tps80031-regulators",
+	},
+	{
+		.name = "tps80031-rtc",
+	},
+	{
+		.name = "tps80031-gpadc",
+	},
+	{
+		.name = "tps80031-battery-gauge",
+	},
+	{
+		.name = "tps80031-charger",
+	},
+};
+
+
+int tps80031_write(struct device *dev, int sid, int reg, uint8_t val)
+{
+	struct tps80031 *tps80031 = dev_get_drvdata(dev);
+
+	return regmap_write(tps80031->regmap[sid], reg, val);
+}
+EXPORT_SYMBOL_GPL(tps80031_write);
+
+int tps80031_writes(struct device *dev, int sid, int reg, int len, uint8_t *val)
+{
+	struct tps80031 *tps80031 = dev_get_drvdata(dev);
+
+	return regmap_bulk_write(tps80031->regmap[sid], reg, val, len);
+}
+EXPORT_SYMBOL_GPL(tps80031_writes);
+
+int tps80031_read(struct device *dev, int sid, int reg, uint8_t *val)
+{
+	struct tps80031 *tps80031 = dev_get_drvdata(dev);
+	unsigned int ival;
+	int ret;
+
+	ret = regmap_read(tps80031->regmap[sid], reg, &ival);
+	if (ret < 0) {
+		dev_err(dev, "failed reading from reg 0x%02x\n", reg);
+		return ret;
+	}
+
+	*val = ival;
+	return ret;
+}
+EXPORT_SYMBOL_GPL(tps80031_read);
+
+int tps80031_reads(struct device *dev, int sid, int reg, int len, uint8_t *val)
+{
+	struct tps80031 *tps80031 = dev_get_drvdata(dev);
+
+	return regmap_bulk_read(tps80031->regmap[sid], reg, val, len);
+}
+EXPORT_SYMBOL_GPL(tps80031_reads);
+
+int tps80031_set_bits(struct device *dev, int sid, int reg, uint8_t bit_mask)
+{
+	struct tps80031 *tps80031 = dev_get_drvdata(dev);
+
+	return regmap_update_bits(tps80031->regmap[sid], reg,
+				bit_mask, bit_mask);
+}
+EXPORT_SYMBOL_GPL(tps80031_set_bits);
+
+int tps80031_clr_bits(struct device *dev, int sid, int reg, uint8_t bit_mask)
+{
+	struct tps80031 *tps80031 = dev_get_drvdata(dev);
+
+	return regmap_update_bits(tps80031->regmap[sid], reg, bit_mask, 0);
+}
+EXPORT_SYMBOL_GPL(tps80031_clr_bits);
+
+int tps80031_update(struct device *dev, int sid, int reg, uint8_t val,
+		uint8_t mask)
+{
+	struct tps80031 *tps80031 = dev_get_drvdata(dev);
+
+	return regmap_update_bits(tps80031->regmap[sid], reg, mask, val);
+}
+EXPORT_SYMBOL_GPL(tps80031_update);
+
+int tps80031_force_update(struct device *dev, int sid, int reg, uint8_t val,
+			  uint8_t mask)
+{
+	struct tps80031 *tps80031 = dev_get_drvdata(dev);
+	struct tps80031_client *tps = &tps80031->tps_clients[sid];
+	uint8_t reg_val;
+	int ret = 0;
+
+	mutex_lock(&tps->lock);
+
+	ret = tps80031_read(dev, sid, reg, &reg_val);
+	if (ret)
+		goto out;
+
+	reg_val = (reg_val & ~mask) | (val & mask);
+	ret = tps80031_write(dev, sid, reg, reg_val);
+
+out:
+	mutex_unlock(&tps->lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(tps80031_force_update);
+
+int tps80031_ext_power_req_config(struct device *dev,
+		unsigned long ext_ctrl_flag, int preq_bit,
+		int state_reg_add, int trans_reg_add)
+{
+	u8 res_ass_reg = 0;
+	int preq_mask_bit = 0;
+	int ret;
+
+	if (!(ext_ctrl_flag & EXT_PWR_REQ))
+		return 0;
+
+	if (ext_ctrl_flag & PWR_REQ_INPUT_PREQ1) {
+		res_ass_reg = TPS80031_PREQ1_RES_ASS_A + (preq_bit >> 3);
+		preq_mask_bit = 5;
+	} else if (ext_ctrl_flag & PWR_REQ_INPUT_PREQ2) {
+		res_ass_reg = TPS80031_PREQ2_RES_ASS_A + (preq_bit >> 3);
+		preq_mask_bit = 6;
+	} else if (ext_ctrl_flag & PWR_REQ_INPUT_PREQ3) {
+		res_ass_reg = TPS80031_PREQ3_RES_ASS_A + (preq_bit >> 3);
+		preq_mask_bit = 7;
+	}
+
+	/* Configure REQ_ASS registers */
+	ret = tps80031_set_bits(dev, SLAVE_ID1, res_ass_reg,
+					BIT(preq_bit & 0x7));
+	if (ret < 0) {
+		dev_err(dev, "%s() Not able to set bit %d of "
+			"reg %d error %d\n",
+			__func__, preq_bit, res_ass_reg, ret);
+		return ret;
+	}
+
+	/* Unmask the PREQ */
+	ret = tps80031_clr_bits(dev, SLAVE_ID1,
+			TPS80031_PHOENIX_MSK_TRANSITION, BIT(preq_mask_bit));
+	if (ret < 0) {
+		dev_err(dev, "%s() Not able to clear bit %d of "
+			"reg %d error %d\n",
+			 __func__, preq_mask_bit,
+			TPS80031_PHOENIX_MSK_TRANSITION, ret);
+		return ret;
+	}
+
+	/* Switch regulator control to resource now */
+	if (ext_ctrl_flag & (PWR_REQ_INPUT_PREQ2 | PWR_REQ_INPUT_PREQ3)) {
+		ret = tps80031_update(dev, SLAVE_ID1, state_reg_add, 0x0,
+						STATE_MASK);
+		if (ret < 0)
+			dev_err(dev, "%s() Error in writing the STATE "
+				"register %d error %d\n", __func__,
+				state_reg_add, ret);
+	} else {
+		ret = tps80031_update(dev, SLAVE_ID1, trans_reg_add,
+				TRANS_SLEEP_OFF, TRANS_SLEEP_MASK);
+		if (ret < 0)
+			dev_err(dev, "%s() Error in writing the TRANS "
+				"register %d error %d\n", __func__,
+				trans_reg_add, ret);
+	}
+	return ret;
+}
+EXPORT_SYMBOL_GPL(tps80031_ext_power_req_config);
+
+unsigned long tps80031_get_chip_info(struct device *dev)
+{
+	struct tps80031 *tps80031 = dev_get_drvdata(dev);
+	return tps80031->chip_info;
+}
+EXPORT_SYMBOL_GPL(tps80031_get_chip_info);
+
+int tps80031_get_pmu_version(struct device *dev)
+{
+	struct tps80031 *tps80031 = dev_get_drvdata(dev);
+	return tps80031->es_version;
+}
+EXPORT_SYMBOL_GPL(tps80031_get_pmu_version);
+
+static struct tps80031 *tps80031_dev;
+static void tps80031_power_off(void)
+{
+	struct tps80031_client *tps = &tps80031_dev->tps_clients[SLAVE_ID1];
+
+	if (!tps->client)
+		return;
+	dev_info(&tps->client->dev, "switching off PMU\n");
+	tps80031_write(&tps->client->dev, SLAVE_ID1,
+				TPS80031_PHOENIX_DEV_ON, DEVOFF);
+}
+
+static void tps80031_pupd_init(struct tps80031 *tps80031,
+			       struct tps80031_platform_data *pdata)
+{
+	struct tps80031_pupd_init_data *pupd_init_data = pdata->pupd_init_data;
+	int data_size = pdata->pupd_init_data_size;
+	int i;
+
+	for (i = 0; i < data_size; ++i) {
+		struct tps80031_pupd_init_data *pupd_init = &pupd_init_data[i];
+		const struct tps80031_pupd_data *pupd =
+			&tps80031_pupds[pupd_init->input_pin];
+		u8 update_value = 0;
+		u8 update_mask = pupd->pulldown_bit | pupd->pullup_bit;
+
+		if (pupd_init->setting == TPS80031_PUPD_PULLDOWN)
+			update_value = pupd->pulldown_bit;
+		else if (pupd_init->setting == TPS80031_PUPD_PULLUP)
+			update_value = pupd->pullup_bit;
+
+		tps80031_update(tps80031->dev, SLAVE_ID1, pupd->reg,
+				update_value, update_mask);
+	}
+}
+
+static void tps80031_backup_battery_charger_control(struct tps80031 *tps80031,
+						    int enable)
+{
+	if (enable)
+		tps80031_update(tps80031->dev, SLAVE_ID1, TPS80031_BBSPOR_CFG,
+				TPS80031_BBSPOR_CHG_EN, TPS80031_BBSPOR_CHG_EN);
+	else
+		tps80031_update(tps80031->dev, SLAVE_ID1, TPS80031_BBSPOR_CFG,
+				0, TPS80031_BBSPOR_CHG_EN);
+}
+
+static void tps80031_init_ext_control(struct tps80031 *tps80031,
+			struct tps80031_platform_data *pdata) {
+	int ret;
+	int i;
+
+	/* Clear all external control for this rail */
+	for (i = 0; i < 9; ++i) {
+		ret = tps80031_write(tps80031->dev, SLAVE_ID1,
+				TPS80031_PREQ1_RES_ASS_A + i, 0);
+		if (ret < 0)
+			dev_err(tps80031->dev, "%s() Error in clearing "
+				"register %02x\n", __func__,
+				TPS80031_PREQ1_RES_ASS_A + i);
+	}
+
+	/* Mask the PREQ */
+	ret = tps80031_set_bits(tps80031->dev, SLAVE_ID1,
+			TPS80031_PHOENIX_MSK_TRANSITION, 0x7 << 5);
+	if (ret < 0)
+		dev_err(tps80031->dev, "%s() Not able to mask register "
+			"0x%02x\n", __func__, TPS80031_PHOENIX_MSK_TRANSITION);
+}
+
+static int tps80031_gpio_get(struct gpio_chip *gc, unsigned offset)
+{
+	struct tps80031 *tps80031 = container_of(gc, struct tps80031, gpio);
+	struct tps80031_client *tps = &tps80031->tps_clients[SLAVE_ID1];
+	uint8_t state;
+	uint8_t trans;
+	int ret;
+
+	ret = tps80031_read(&tps->client->dev, SLAVE_ID1,
+			pmc_ext_control_base[offset] +
+				EXT_CONTROL_CFG_STATE, &state);
+	if (ret)
+		return ret;
+
+	if (state != 0) {
+		ret = tps80031_read(&tps->client->dev, SLAVE_ID1,
+				pmc_ext_control_base[offset] +
+					EXT_CONTROL_CFG_TRANS, &trans);
+		if (ret)
+			return ret;
+		return trans & 0x1;
+	}
+	return 0;
+}
+
+static void tps80031_gpio_set(struct gpio_chip *gc, unsigned offset,
+			int value)
+{
+	struct tps80031 *tps80031 = container_of(gc, struct tps80031, gpio);
+
+	tps80031_update(tps80031->dev, SLAVE_ID1,
+		pmc_ext_control_base[offset] + EXT_CONTROL_CFG_TRANS,
+			value, 0x1);
+}
+
+static int tps80031_gpio_input(struct gpio_chip *gc, unsigned offset)
+{
+	return -EIO;
+}
+
+static int tps80031_gpio_output(struct gpio_chip *gc, unsigned offset,
+				int value)
+{
+	tps80031_gpio_set(gc, offset, value);
+	return 0;
+}
+
+static int tps80031_gpio_enable(struct gpio_chip *gc, unsigned offset)
+{
+	struct tps80031 *tps80031 = container_of(gc, struct tps80031, gpio);
+	int ret;
+
+	ret = tps80031_update(tps80031->dev, SLAVE_ID1,
+		pmc_ext_control_base[offset] + EXT_CONTROL_CFG_STATE,
+						STATE_ON, STATE_MASK);
+	if (ret)
+		return ret;
+
+	return tps80031_write(tps80031->dev, SLAVE_ID1,
+		pmc_ext_control_base[offset] + EXT_CONTROL_CFG_TRANS, 0x0);
+}
+
+static void tps80031_gpio_disable(struct gpio_chip *gc, unsigned offset)
+{
+	struct tps80031 *tps80031 = container_of(gc, struct tps80031, gpio);
+	tps80031_update(tps80031->dev, SLAVE_ID1,
+		pmc_ext_control_base[offset] + EXT_CONTROL_CFG_STATE,
+						STATE_OFF, STATE_MASK);
+}
+
+static void tps80031_gpio_init(struct tps80031 *tps80031,
+			struct tps80031_platform_data *pdata)
+{
+	int ret;
+	int gpio_base = pdata->gpio_base;
+	struct tps80031_client *tps = &tps80031->tps_clients[SLAVE_ID1];
+	struct tps80031_gpio_init_data *gpio_init_data = pdata->gpio_init_data;
+	int data_size = pdata->gpio_init_data_size;
+	static int preq_bit_pos[TPS80031_GPIO_NR] = {16, 17, 18};
+	int base_add;
+	int i;
+
+	if (gpio_base <= 0)
+		return;
+
+	/* Configure the external request mode */
+	for (i = 0; i < data_size; ++i) {
+		struct tps80031_gpio_init_data *gpio_pd = &gpio_init_data[i];
+		base_add = pmc_ext_control_base[gpio_pd->gpio_nr];
+
+		if (gpio_pd->ext_ctrl_flag & EXT_PWR_REQ) {
+			ret = tps80031_ext_power_req_config(tps80031->dev,
+				gpio_pd->ext_ctrl_flag,
+				preq_bit_pos[gpio_pd->gpio_nr],
+				base_add + EXT_CONTROL_CFG_STATE,
+				base_add + EXT_CONTROL_CFG_TRANS);
+			if (ret < 0)
+				dev_warn(tps80031->dev, "Ext pwrreq GPIO "
+					"sleep control fails\n");
+		}
+
+		if (gpio_pd->ext_ctrl_flag & PWR_OFF_ON_SLEEP) {
+			ret = tps80031_update(tps80031->dev, SLAVE_ID1,
+				base_add + EXT_CONTROL_CFG_TRANS, 0x0, 0xC);
+			if (ret < 0)
+				dev_warn(tps80031->dev, "GPIO OFF on sleep "
+					"control fails\n");
+		}
+
+		if (gpio_pd->ext_ctrl_flag & PWR_ON_ON_SLEEP) {
+			ret = tps80031_update(tps80031->dev, SLAVE_ID1,
+				base_add + EXT_CONTROL_CFG_TRANS, 0x4, 0xC);
+			if (ret < 0)
+				dev_warn(tps80031->dev, "GPIO ON on sleep "
+					"control fails\n");
+		}
+	}
+
+	tps80031->gpio.owner		= THIS_MODULE;
+	tps80031->gpio.label		= tps->client->name;
+	tps80031->gpio.dev		= tps80031->dev;
+	tps80031->gpio.base		= gpio_base;
+	tps80031->gpio.ngpio		= TPS80031_GPIO_NR;
+	tps80031->gpio.can_sleep	= 1;
+
+	tps80031->gpio.request		= tps80031_gpio_enable;
+	tps80031->gpio.free		= tps80031_gpio_disable;
+	tps80031->gpio.direction_input	= tps80031_gpio_input;
+	tps80031->gpio.direction_output	= tps80031_gpio_output;
+	tps80031->gpio.set		= tps80031_gpio_set;
+	tps80031->gpio.get		= tps80031_gpio_get;
+
+	ret = gpiochip_add(&tps80031->gpio);
+	if (ret)
+		dev_warn(tps80031->dev, "GPIO registration failed: %d\n", ret);
+}
+
+static void tps80031_irq_lock(struct irq_data *data)
+{
+	struct tps80031 *tps80031 = irq_data_get_irq_chip_data(data);
+
+	mutex_lock(&tps80031->irq_lock);
+}
+
+static void tps80031_irq_enable(struct irq_data *data)
+{
+	struct tps80031 *tps80031 = irq_data_get_irq_chip_data(data);
+	unsigned int __irq = data->irq - tps80031->irq_base;
+	const struct tps80031_irq_data *irq_data = &tps80031_irqs[__irq];
+
+	if (irq_data->is_sec_int) {
+		tps80031->cont_int_mask_reg &= ~(1 << irq_data->int_mask_bit);
+		tps80031->cont_int_en |= (1 << irq_data->int_mask_bit);
+		tps80031->mask_reg[irq_data->mask_reg] &= ~(1 << irq_data->mask_mask);
+		tps80031->irq_en |= (1 << irq_data->parent_int);
+	} else
+		tps80031->mask_reg[irq_data->mask_reg] &= ~(1 << irq_data->mask_mask);
+
+	tps80031->irq_en |= (1 << __irq);
+}
+
+static void tps80031_irq_disable(struct irq_data *data)
+{
+	struct tps80031 *tps80031 = irq_data_get_irq_chip_data(data);
+
+	unsigned int __irq = data->irq - tps80031->irq_base;
+	const struct tps80031_irq_data *irq_data = &tps80031_irqs[__irq];
+
+	if (irq_data->is_sec_int) {
+		tps80031->cont_int_mask_reg |= (1 << irq_data->int_mask_bit);
+		tps80031->cont_int_en &= ~(1 << irq_data->int_mask_bit);
+		if (!tps80031->cont_int_en) {
+			tps80031->mask_reg[irq_data->mask_reg] |=
+						(1 << irq_data->mask_mask);
+			tps80031->irq_en &= ~(1 << irq_data->parent_int);
+		}
+		tps80031->irq_en &= ~(1 << __irq);
+	} else
+		tps80031->mask_reg[irq_data->mask_reg] |= (1 << irq_data->mask_mask);
+
+	tps80031->irq_en &= ~(1 << __irq);
+}
+
+static void tps80031_irq_sync_unlock(struct irq_data *data)
+{
+	struct tps80031 *tps80031 = irq_data_get_irq_chip_data(data);
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(tps80031->mask_reg); i++) {
+		if (tps80031->mask_reg[i] != tps80031->mask_cache[i]) {
+			if (!WARN_ON(tps80031_write(tps80031->dev, SLAVE_ID2,
+						TPS80031_INT_MSK_LINE_A + i,
+						tps80031->mask_reg[i])))
+				if (!WARN_ON(tps80031_write(tps80031->dev,
+						SLAVE_ID2,
+						TPS80031_INT_MSK_STS_A + i,
+						tps80031->mask_reg[i])))
+					tps80031->mask_cache[i] =
+							tps80031->mask_reg[i];
+		}
+	}
+
+	if (tps80031->cont_int_mask_reg != tps80031->cont_int_mask_cache) {
+		if (!WARN_ON(tps80031_write(tps80031->dev, SLAVE_ID2,
+				TPS80031_CONTROLLER_INT_MASK,
+				tps80031->cont_int_mask_reg)))
+			tps80031->cont_int_mask_cache =
+						tps80031->cont_int_mask_reg;
+	}
+
+	mutex_unlock(&tps80031->irq_lock);
+}
+
+static irqreturn_t tps80031_charge_control_irq(int irq, void *data)
+{
+	struct tps80031 *tps80031 = data;
+	int ret = 0;
+	int i;
+	u8 cont_sts;
+	u8 org_sts;
+	if (irq != (tps80031->irq_base + TPS80031_INT_CHRG_CTRL)) {
+		dev_err(tps80031->dev, "%s() Got the illegal interrupt %d\n",
+					__func__, irq);
+		return IRQ_NONE;
+	}
+
+	ret = tps80031_read(tps80031->dev, SLAVE_ID2,
+			TPS80031_CONTROLLER_STAT1, &org_sts);
+	if (ret < 0) {
+		dev_err(tps80031->dev, "%s(): failed to read controller state1 "
+				"status %d\n", __func__, ret);
+		return IRQ_NONE;
+	}
+
+	/* Get change from last interrupt and mask for interested interrupt
+	 * for charge control interrupt */
+	cont_sts = org_sts ^ tps80031->prev_cont_stat1;
+	tps80031->prev_cont_stat1 = org_sts;
+	/* Clear watchdog timer state */
+	tps80031->prev_cont_stat1 &= ~(1 << 4);
+	cont_sts &= 0x5F;
+
+	for (i = 0; i < 8; ++i) {
+		if (!controller_stat1_irq_nr[i])
+			continue;
+
+		if ((cont_sts & BIT(i)) &&
+			(tps80031->irq_en & BIT(controller_stat1_irq_nr[i])))
+			handle_nested_irq(tps80031->irq_base +
+						controller_stat1_irq_nr[i]);
+		cont_sts &= ~BIT(i);
+	}
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t tps80031_irq(int irq, void *data)
+{
+	struct tps80031 *tps80031 = data;
+	int ret = 0;
+	u32 acks;
+	int i;
+	uint8_t tmp[3];
+
+	ret = tps80031_reads(tps80031->dev, SLAVE_ID2,
+			     TPS80031_INT_STS_A, 3, tmp);
+	if (ret < 0) {
+		dev_err(tps80031->dev, "failed to read interrupt status\n");
+		return IRQ_NONE;
+	}
+	acks = (tmp[2] << 16) | (tmp[1] << 8) | tmp[0];
+
+	if (acks) {
+		/*
+		 * Hardware behavior: hardware have the shadow register for
+		 * interrupt status register which is updated if interrupt
+		 * comes just after the interrupt status read. This shadow
+		 * register gets written to main status register and cleared
+		 * if any byte write happens in any of status register like
+		 * STS_A, STS_B or STS_C.
+		 * Hence here to clear the original interrupt status and
+		 * updating the STS register with the shadow register, it is
+		 * require to write only one byte in any of STS register.
+		 * Having multiple register write can cause the STS register
+		 * to clear without handling those interrupt and can cause
+		 * interrupt miss.
+		 */
+		ret = tps80031_write(tps80031->dev, SLAVE_ID2,
+				      TPS80031_INT_STS_A, 0);
+		if (ret < 0) {
+			dev_err(tps80031->dev, "failed to write "
+						"interrupt status\n");
+			return IRQ_NONE;
+		}
+
+		while (acks) {
+			i = __ffs(acks);
+			if (tps80031->irq_en & (1 << i))
+				handle_nested_irq(tps80031->irq_base + i);
+			acks &= ~(1 << i);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int __devinit tps80031_irq_init(struct tps80031 *tps80031, int irq,
+				int irq_base)
+{
+	int i, ret;
+
+	if (!irq_base) {
+		dev_warn(tps80031->dev, "No interrupt support on IRQ base\n");
+		return -EINVAL;
+	}
+
+	mutex_init(&tps80031->irq_lock);
+
+	for (i = 0; i < 3; i++) {
+		tps80031->mask_reg[i] = 0xFF;
+		tps80031->mask_cache[i] = tps80031->mask_reg[i];
+		tps80031_write(tps80031->dev, SLAVE_ID2,
+					TPS80031_INT_MSK_LINE_A + i,
+					tps80031->mask_cache[i]);
+		tps80031_write(tps80031->dev, SLAVE_ID2,
+					TPS80031_INT_MSK_STS_A + i, 0xFF);
+		tps80031_write(tps80031->dev, SLAVE_ID2,
+					TPS80031_INT_STS_A + i, 0xFF);
+	}
+
+	ret = tps80031_read(tps80031->dev, SLAVE_ID2,
+				TPS80031_CONTROLLER_INT_MASK,
+				&tps80031->cont_int_mask_reg);
+	if (ret < 0) {
+		dev_err(tps80031->dev, "Error in reading the controller_mask "
+					"register %d\n", ret);
+		return ret;
+	}
+
+	tps80031->cont_int_mask_reg |= CHARGE_CONTROL_SUB_INT_MASK;
+	tps80031->cont_int_mask_cache = tps80031->cont_int_mask_reg;
+	tps80031->cont_int_en = 0;
+	ret = tps80031_write(tps80031->dev, SLAVE_ID2,
+				TPS80031_CONTROLLER_INT_MASK,
+				tps80031->cont_int_mask_reg);
+	if (ret < 0) {
+		dev_err(tps80031->dev, "Error in writing the controller_mask "
+					"register %d\n", ret);
+		return ret;
+	}
+
+	ret = tps80031_read(tps80031->dev, SLAVE_ID2,
+			TPS80031_CONTROLLER_STAT1, &tps80031->prev_cont_stat1);
+	if (ret < 0) {
+		dev_err(tps80031->dev, "%s(): failed to read controller state1 "
+				"status %d\n", __func__, ret);
+		return ret;
+	}
+
+	/* Clear watch dog interrupt status in status */
+	tps80031->prev_cont_stat1 &= ~(1 << 4);
+
+	tps80031->irq_base = irq_base;
+
+	tps80031->irq_chip.name = "tps80031";
+	tps80031->irq_chip.irq_enable = tps80031_irq_enable;
+	tps80031->irq_chip.irq_disable = tps80031_irq_disable;
+	tps80031->irq_chip.irq_bus_lock = tps80031_irq_lock;
+	tps80031->irq_chip.irq_bus_sync_unlock = tps80031_irq_sync_unlock;
+
+	for (i = 0; i < TPS80031_INT_NR; i++) {
+		int __irq = i + tps80031->irq_base;
+		irq_set_chip_data(__irq, tps80031);
+		irq_set_chip_and_handler(__irq, &tps80031->irq_chip,
+					 handle_simple_irq);
+		irq_set_nested_thread(__irq, 1);
+#ifdef CONFIG_ARM
+		set_irq_flags(__irq, IRQF_VALID);
+#endif
+	}
+
+	ret = request_threaded_irq(irq, NULL, tps80031_irq, IRQF_ONESHOT,
+				"tps80031", tps80031);
+	/* register the isr for the secondary interrupt */
+	if (!ret)
+		ret = request_threaded_irq(irq_base + TPS80031_INT_CHRG_CTRL,
+				NULL, tps80031_charge_control_irq,
+				IRQF_ONESHOT, "80031_chg_ctl", tps80031);
+	if (!ret) {
+
+		device_init_wakeup(tps80031->dev, 1);
+		enable_irq_wake(irq);
+	}
+
+	return ret;
+}
+
+static void tps80031_clk32k_enable(struct tps80031 *tps80031, int base_add)
+{
+	int ret;
+	ret = tps80031_update(tps80031->dev, SLAVE_ID1,
+			base_add + EXT_CONTROL_CFG_STATE, STATE_ON, STATE_MASK);
+	if (!ret)
+		ret = tps80031_update(tps80031->dev, SLAVE_ID1,
+				base_add + EXT_CONTROL_CFG_TRANS,
+				STATE_ON, STATE_MASK);
+	if (ret < 0)
+		dev_err(tps80031->dev, "Error in updating clock register\n");
+}
+
+static void tps80031_clk32k_init(struct tps80031 *tps80031,
+			struct tps80031_platform_data *pdata)
+{
+	int ret;
+	struct tps80031_clk32k_init_data *clk32_idata = pdata->clk32k_init_data;
+	int data_size = pdata->clk32k_init_data_size;
+	static int clk32k_preq_bit_pos[TPS80031_CLOCK32K_NR] = {-1, 20, 19};
+	int base_add;
+	int i;
+
+	if (!clk32_idata || !data_size)
+		return;
+
+	/* Configure the external request mode */
+	for (i = 0; i < data_size; ++i) {
+		struct tps80031_clk32k_init_data *clk32_pd =  &clk32_idata[i];
+		base_add = pmc_clk32k_control_base[clk32_pd->clk32k_nr];
+		if (clk32_pd->enable)
+			tps80031_clk32k_enable(tps80031, base_add);
+
+		if ((clk32_pd->ext_ctrl_flag & EXT_PWR_REQ) &&
+			 (clk32k_preq_bit_pos[clk32_pd->clk32k_nr] != -1)) {
+			ret = tps80031_ext_power_req_config(tps80031->dev,
+				clk32_pd->ext_ctrl_flag,
+				clk32k_preq_bit_pos[clk32_pd->clk32k_nr],
+				base_add + EXT_CONTROL_CFG_STATE,
+				base_add + EXT_CONTROL_CFG_TRANS);
+			if (ret < 0)
+				dev_warn(tps80031->dev, "Clk32 ext control "
+					"fails\n");
+		}
+
+		if (clk32_pd->ext_ctrl_flag & PWR_OFF_ON_SLEEP) {
+			ret = tps80031_update(tps80031->dev, SLAVE_ID1,
+				base_add + EXT_CONTROL_CFG_TRANS, 0x0, 0xC);
+			if (ret < 0)
+				dev_warn(tps80031->dev, "clk OFF on sleep "
+					"control fails\n");
+		}
+
+		if (clk32_pd->ext_ctrl_flag & PWR_ON_ON_SLEEP) {
+			ret = tps80031_update(tps80031->dev, SLAVE_ID1,
+				base_add + EXT_CONTROL_CFG_TRANS, 0x4, 0xC);
+			if (ret < 0)
+				dev_warn(tps80031->dev, "clk ON sleep "
+					"control fails\n");
+		}
+	}
+}
+
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+static void print_regs(const char *header, struct seq_file *s,
+		int sid, int start_offset, int end_offset)
+{
+	struct tps80031 *tps80031 = s->private;
+	struct tps80031_client *tps = &tps80031->tps_clients[sid];
+	uint8_t reg_val;
+	int i;
+	int ret;
+
+	seq_printf(s, "%s\n", header);
+	for (i = start_offset; i <= end_offset; ++i) {
+		ret = tps80031_read(&tps->client->dev, sid, i, &reg_val);
+		if (ret >= 0)
+			seq_printf(s, "Addr = 0x%02x Reg 0x%02x Value 0x%02x\n",
+						tps->client->addr, i, reg_val);
+	}
+	seq_printf(s, "------------------\n");
+}
+
+static int dbg_tps_show(struct seq_file *s, void *unused)
+{
+	seq_printf(s, "TPS80031 Registers\n");
+	seq_printf(s, "------------------\n");
+	print_regs("VIO Regs",       s, SLAVE_ID1, 0x47, 0x49);
+	print_regs("VIO Regs",       s, SLAVE_ID0, 0x49, 0x4A);
+	print_regs("SMPS1 Regs",     s, SLAVE_ID1, 0x53, 0x54);
+	print_regs("SMPS1 Regs",     s, SLAVE_ID0, 0x55, 0x56);
+	print_regs("SMPS1 Regs",     s, SLAVE_ID1, 0x57, 0x57);
+	print_regs("SMPS2 Regs",     s, SLAVE_ID1, 0x59, 0x5B);
+	print_regs("SMPS2 Regs",     s, SLAVE_ID0, 0x5B, 0x5C);
+	print_regs("SMPS2 Regs",     s, SLAVE_ID1, 0x5C, 0x5D);
+	print_regs("SMPS3 Regs",     s, SLAVE_ID1, 0x65, 0x68);
+	print_regs("SMPS4 Regs",     s, SLAVE_ID1, 0x41, 0x44);
+	print_regs("VANA Regs",      s, SLAVE_ID1, 0x81, 0x83);
+	print_regs("VRTC Regs",      s, SLAVE_ID1, 0xC3, 0xC4);
+	print_regs("LDO1 Regs",      s, SLAVE_ID1, 0x9D, 0x9F);
+	print_regs("LDO2 Regs",      s, SLAVE_ID1, 0x85, 0x87);
+	print_regs("LDO3 Regs",      s, SLAVE_ID1, 0x8D, 0x8F);
+	print_regs("LDO4 Regs",      s, SLAVE_ID1, 0x89, 0x8B);
+	print_regs("LDO5 Regs",      s, SLAVE_ID1, 0x99, 0x9B);
+	print_regs("LDO6 Regs",      s, SLAVE_ID1, 0x91, 0x93);
+	print_regs("LDO7 Regs",      s, SLAVE_ID1, 0xA5, 0xA7);
+	print_regs("LDOUSB Regs",    s, SLAVE_ID1, 0xA1, 0xA3);
+	print_regs("LDOLN Regs",     s, SLAVE_ID1, 0x95, 0x97);
+	print_regs("REGEN1 Regs",    s, SLAVE_ID1, 0xAE, 0xAF);
+	print_regs("REGEN2 Regs",    s, SLAVE_ID1, 0xB1, 0xB2);
+	print_regs("SYSEN Regs",     s, SLAVE_ID1, 0xB4, 0xB5);
+	print_regs("CLK32KAO Regs",  s, SLAVE_ID1, 0xBA, 0xBB);
+	print_regs("CLK32KG Regs",   s, SLAVE_ID1, 0xBD, 0xBE);
+	print_regs("CLK32KAUD Regs", s, SLAVE_ID1, 0xC0, 0xC1);
+	print_regs("INT Regs",       s, SLAVE_ID2, 0xD0, 0xD8);
+	print_regs("PREQ Regs",      s, SLAVE_ID1, 0xD7, 0xDF);
+	print_regs("MASK_PH Regs",   s, SLAVE_ID1, 0x20, 0x21);
+	print_regs("PMC MISC Regs",  s, SLAVE_ID1, 0xE0, 0xEF);
+	print_regs("CONT_STATE",     s, SLAVE_ID2, 0xE0, 0xE4);
+	print_regs("VERNUM Regs",    s, SLAVE_ID3, 0x87, 0x87);
+	print_regs("EEPROM Regs",    s, SLAVE_ID3, 0xDF, 0xDF);
+	print_regs("CHARGE Regs",    s, SLAVE_ID2, 0xDA, 0xF5);
+	return 0;
+}
+
+static int dbg_tps_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dbg_tps_show, inode->i_private);
+}
+
+static const struct file_operations debug_fops = {
+	.open		= dbg_tps_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static void __init tps80031_debuginit(struct tps80031 *tps)
+{
+	(void)debugfs_create_file("tps80031", S_IRUGO, NULL,
+			tps, &debug_fops);
+}
+#else
+static void __init tps80031_debuginit(struct tps80031 *tpsi)
+{
+	return;
+}
+#endif
+
+static bool rd_wr_reg_id0(struct device *dev, unsigned int reg)
+{
+	switch(reg) {
+	case TPS80031_ID0_PMIC_SLAVE_SMPS_DVS:
+		return true;
+	default:
+		pr_err("non-existing reg %s() %d reg %x\n", __func__, __LINE__, reg);
+		BUG();
+		return false;
+	}
+}
+
+static bool rd_wr_reg_id1(struct device *dev, unsigned int reg)
+{
+	switch(reg) {
+	case TPS80031_ID1_RTC:
+	case TPS80031_ID1_MEMORY:
+	case TPS80031_ID1_PMC_MASTER:
+	case TPS80031_ID1_PMC_SLAVE_SMPS:
+	case TPS80031_ID1_PMC_SLAVE_MISC:
+	case TPS80031_ID1_PMC_SLAVE_LDO:
+	case TPS80031_ID1_PMC_SLAVE_REOSURCES:
+	case TPS80031_ID1_PMC_PREQ_ASSIGN:
+	case TPS80031_ID1_PMC_MISC:
+	case TPS80031_ID1_PMC_PU_PD_HZ:
+	case TPS80031_ID1_PMC_BACKUP:
+		return true;
+	default:
+		pr_err("non-existing reg %s() %d reg %x\n", __func__, __LINE__, reg);
+		BUG();
+		return false;
+	}
+}
+
+static bool rd_wr_reg_id2(struct device *dev, unsigned int reg)
+{
+	switch(reg) {
+	case TPS80031_ID2_USB:
+	case TPS80031_ID2_GPADC_CONTROL:
+	case TPS80031_ID2_GPADC_RESULTS:
+	case TPS80031_ID2_AUXILLIARIES:
+	case TPS80031_ID2_CUSTOM:
+	case TPS80031_ID2_PWM:
+	case TPS80031_ID2_FUEL_GAUSE:
+	case TPS80031_ID2_INTERFACE_INTERRUPTS:
+	case TPS80031_ID2_CHARGER:
+		return true;
+	default:
+		pr_err("non-existing reg %s() %d reg %x\n", __func__, __LINE__, reg);
+		BUG();
+		return false;
+	}
+}
+static bool rd_wr_reg_id3(struct device *dev, unsigned int reg)
+{
+	switch(reg) {
+	case TPS80031_ID3_TEST_LDO:
+	case TPS80031_ID3_TEST_SMPS:
+	case TPS80031_ID3_TEST_POWER:
+	case TPS80031_ID3_TEST_CHARGER:
+	case TPS80031_ID3_TEST_AUXILIIARIES:
+	case TPS80031_ID3_DIEID:
+	case TPS80031_ID3_TRIM_PHOENIX:
+	case TPS80031_ID3_TRIM_CUSTOM:
+		return true;
+	default:
+		pr_err("non-existing reg %s() %d reg %x\n", __func__, __LINE__, reg);
+		BUG();
+		return false;
+	}
+}
+
+static const struct regmap_config tps80031_regmap_configs[] = {
+	{
+		.reg_bits = 8,
+		.val_bits = 8,
+		.writeable_reg = rd_wr_reg_id0,
+		.readable_reg = rd_wr_reg_id0,
+		.max_register = TPS80031_MAX_REGISTER - 1,
+	},
+	{
+		.reg_bits = 8,
+		.val_bits = 8,
+		.writeable_reg = rd_wr_reg_id1,
+		.readable_reg = rd_wr_reg_id1,
+		.max_register = TPS80031_MAX_REGISTER - 1,
+	},
+	{
+		.reg_bits = 8,
+		.val_bits = 8,
+		.writeable_reg = rd_wr_reg_id2,
+		.readable_reg = rd_wr_reg_id2,
+		.max_register = TPS80031_MAX_REGISTER - 1,
+	},
+	{
+		.reg_bits = 8,
+		.val_bits = 8,
+		.writeable_reg = rd_wr_reg_id3,
+		.readable_reg = rd_wr_reg_id3,
+		.max_register = TPS80031_MAX_REGISTER - 1,
+	},
+};
+
+static int __devexit tps80031_i2c_remove(struct i2c_client *client)
+{
+	struct tps80031 *tps80031 = i2c_get_clientdata(client);
+	int i;
+
+	mfd_remove_devices(tps80031->dev);
+
+	if (client->irq)
+		free_irq(client->irq, tps80031);
+
+	if (tps80031->gpio.owner != NULL)
+		if (gpiochip_remove(&tps80031->gpio) < 0)
+			dev_err(&client->dev, "Error in removing the gpio driver\n");
+
+	for (i = 0; i < TPS_NUM_SLAVES; i++) {
+		struct tps80031_client *tps = &tps80031->tps_clients[i];
+		if (tps->client && tps->client != client)
+			i2c_unregister_device(tps->client);
+		tps80031->tps_clients[i].client = NULL;
+		mutex_destroy(&tps->lock);
+	}
+
+	return 0;
+}
+
+static int __devinit tps80031_i2c_probe(struct i2c_client *client,
+					const struct i2c_device_id *id)
+{
+	struct tps80031_platform_data *pdata = client->dev.platform_data;
+	struct tps80031 *tps80031;
+	struct tps80031_client *tps;
+	int ret;
+	int jtag_ver;
+	int ep_ver;
+	int i;
+
+	if (!pdata) {
+		dev_err(&client->dev, "tps80031 requires platform data\n");
+		return -ENOTSUPP;
+	}
+
+	jtag_ver = i2c_smbus_read_byte_data(client, TPS80031_JTAGVERNUM);
+	if (jtag_ver < 0) {
+		dev_err(&client->dev, "Silicon version number read"
+				" failed: %d\n", jtag_ver);
+		return -EIO;
+	}
+
+	ep_ver = i2c_smbus_read_byte_data(client, TPS80031_EPROM_REV);
+	if (ep_ver < 0) {
+		dev_err(&client->dev, "Silicon eeprom version read"
+				" failed: %d\n", ep_ver);
+		return -EIO;
+	}
+
+	dev_info(&client->dev, "Jtag version 0x%02x and Eeprom version 0x%02x\n",
+						jtag_ver, ep_ver);
+
+	tps80031 = devm_kzalloc(&client->dev, sizeof(*tps80031), GFP_KERNEL);
+	if (!tps80031) {
+		dev_err(&client->dev, "Memory alloc for tps80031 failed\n");
+		return -ENOMEM;
+	}
+
+	tps80031->es_version = jtag_ver;
+	tps80031->dev = &client->dev;
+	i2c_set_clientdata(client, tps80031);
+	tps80031->chip_info = id->driver_data;
+
+	/* Set up slaves */
+	tps80031->tps_clients[SLAVE_ID0].addr = I2C_ID0_ADDR;
+	tps80031->tps_clients[SLAVE_ID1].addr = I2C_ID1_ADDR;
+	tps80031->tps_clients[SLAVE_ID2].addr = I2C_ID2_ADDR;
+	tps80031->tps_clients[SLAVE_ID3].addr = I2C_ID3_ADDR;
+	for (i = 0; i < TPS_NUM_SLAVES; i++) {
+		tps = &tps80031->tps_clients[i];
+		if (tps->addr == client->addr)
+			tps->client = client;
+		else
+			tps->client = i2c_new_dummy(client->adapter,
+						tps->addr);
+		if (!tps->client) {
+			dev_err(&client->dev, "can't attach client %d\n", i);
+			ret = -ENOMEM;
+			goto fail_client_reg;
+		}
+		i2c_set_clientdata(tps->client, tps80031);
+		mutex_init(&tps->lock);
+
+		tps80031->regmap[i] = devm_regmap_init_i2c(tps->client,
+					&tps80031_regmap_configs[i]);
+		if (IS_ERR(tps80031->regmap[i])) {
+			ret = PTR_ERR(tps80031->regmap[i]);
+			dev_err(&client->dev,
+				"regmap %d init failed, err %d\n", i, ret);
+			goto fail_client_reg;
+		}
+	}
+
+	if (client->irq) {
+		ret = tps80031_irq_init(tps80031, client->irq,
+					pdata->irq_base);
+		if (ret) {
+			dev_err(&client->dev, "IRQ init failed: %d\n", ret);
+			goto fail_client_reg;
+		}
+	}
+
+	tps80031_pupd_init(tps80031, pdata);
+
+	tps80031_init_ext_control(tps80031, pdata);
+
+	ret = mfd_add_devices(tps80031->dev, -1,
+			tps80031_cell, ARRAY_SIZE(tps80031_cell), NULL, 0);
+	if (ret < 0) {
+		dev_err(&client->dev, "mfd_add_devices failed: %d\n", ret);
+		goto fail_mfd_add;
+	}
+
+	tps80031_gpio_init(tps80031, pdata);
+
+	tps80031_clk32k_init(tps80031, pdata);
+
+	tps80031_debuginit(tps80031);
+
+	tps80031_backup_battery_charger_control(tps80031, 1);
+
+	if (pdata->use_power_off && !pm_power_off)
+		pm_power_off = tps80031_power_off;
+
+	tps80031_dev = tps80031;
+
+	return 0;
+
+fail_mfd_add:
+	if (client->irq)
+		free_irq(client->irq, tps80031);
+fail_client_reg:
+	for (i = 0; i < TPS_NUM_SLAVES; i++) {
+		struct tps80031_client *tps = &tps80031->tps_clients[i];
+		if (tps->client && tps->client != client)
+			i2c_unregister_device(tps->client);
+		tps80031->tps_clients[i].client = NULL;
+		mutex_destroy(&tps->lock);
+	}
+	return ret;
+}
+
+#ifdef CONFIG_PM
+static int tps80031_i2c_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct tps80031 *tps80031 = i2c_get_clientdata(client);
+	if (client->irq)
+		disable_irq(client->irq);
+	tps80031_backup_battery_charger_control(tps80031, 0);
+	return 0;
+}
+
+static int tps80031_i2c_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct tps80031 *tps80031 = i2c_get_clientdata(client);
+	tps80031_backup_battery_charger_control(tps80031, 1);
+	if (client->irq)
+		enable_irq(client->irq);
+	return 0;
+}
+static const struct dev_pm_ops tps80031_dev_pm_ops = {
+	.suspend	= tps80031_i2c_suspend,
+	.resume		= tps80031_i2c_resume,
+};
+#define TPS80031_DEV_PM (&tps80031_dev_pm_ops)
+#else
+#define TPS80031_DEV_PM NULL
+#endif
+
+
+static const struct i2c_device_id tps80031_id_table[] = {
+	{ "tps80031", TPS80031 },
+	{ "tps80032", TPS80032 },
+};
+MODULE_DEVICE_TABLE(i2c, tps80031_id_table);
+
+static struct i2c_driver tps80031_driver = {
+	.driver	= {
+		.name	= "tps80031",
+		.owner	= THIS_MODULE,
+		.pm	= TPS80031_DEV_PM,
+	},
+	.probe		= tps80031_i2c_probe,
+	.remove		= __devexit_p(tps80031_i2c_remove),
+	.id_table	= tps80031_id_table,
+};
+
+static int __init tps80031_init(void)
+{
+	return i2c_add_driver(&tps80031_driver);
+}
+subsys_initcall(tps80031_init);
+
+static void __exit tps80031_exit(void)
+{
+	i2c_del_driver(&tps80031_driver);
+}
+module_exit(tps80031_exit);
+
+MODULE_DESCRIPTION("TPS80031 core driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/tps8003x-gpadc.c b/drivers/mfd/tps8003x-gpadc.c
new file mode 100644
index 000000000000..5db912cc01fd
--- /dev/null
+++ b/drivers/mfd/tps8003x-gpadc.c
@@ -0,0 +1,650 @@
+/*
+ * drivers/mfd/tps8003x-gpadc.c
+ *
+ * Gpadc for TI's tps80031
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms 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., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/platform_device.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/i2c/twl.h>
+#include <linux/mfd/tps80031.h>
+#include <linux/uaccess.h>
+#include <linux/spinlock.h>
+
+#define GPADC_CTRL	0x2e
+#define GPSELECT_ISB	0x35
+#define GPCH0_LSB	0x3b
+#define GPCH0_MSB	0x3c
+#define CTRL_P1		0x36
+#define TOGGLE1		0x90
+#define MISC1		0xe4
+
+#define CTRL_P1_SP1	BIT(3)
+#define TOGGLE1_GPADCR	BIT(1)
+#define GPADC_BUSY	(1 << 0)
+#define GPADC_EOC_SW	(1 << 1)
+#define SCALE		(1 << 15)
+
+#define TPS80031_GPADC_MAX_CHANNELS 17
+#define TPS80031_GPADC_IOC_MAGIC '`'
+#define TPS80031_GPADC_IOCX_ADC_RAW_READ _IO(TPS80031_GPADC_IOC_MAGIC, 0)
+
+struct tps80031_gpadc_user_parms {
+	int channel;
+	int status;
+	u16 result;
+};
+
+struct tps80031_calibration {
+	s32 gain_error;
+	s32 offset_error;
+};
+
+struct tps80031_ideal_code {
+	s16 code1;
+	s16 code2;
+};
+
+struct tps80031_scalar_channel {
+	uint8_t delta1_addr;
+	uint8_t delta1_mask;
+	uint8_t delta2_addr;
+	uint8_t delta2_mask;
+};
+
+static struct tps80031_calibration
+	tps80031_calib_tbl[TPS80031_GPADC_MAX_CHANNELS];
+static const uint32_t calibration_bit_map = 0x47FF;
+static const uint32_t scalar_bit_map = 0x4785;
+
+#define TPS80031_GPADC_TRIM1	0xCD
+#define TPS80031_GPADC_TRIM2	0xCE
+#define TPS80031_GPADC_TRIM3	0xCF
+#define TPS80031_GPADC_TRIM4	0xD0
+#define TPS80031_GPADC_TRIM5	0xD1
+#define TPS80031_GPADC_TRIM6	0xD2
+#define TPS80031_GPADC_TRIM7	0xD3
+#define TPS80031_GPADC_TRIM8	0xD4
+#define TPS80031_GPADC_TRIM9	0xD5
+#define TPS80031_GPADC_TRIM10	0xD6
+#define TPS80031_GPADC_TRIM11	0xD7
+#define TPS80031_GPADC_TRIM12	0xD8
+#define TPS80031_GPADC_TRIM13	0xD9
+#define TPS80031_GPADC_TRIM14	0xDA
+#define TPS80031_GPADC_TRIM15	0xDB
+#define TPS80031_GPADC_TRIM16	0xDC
+#define TPS80031_GPADC_TRIM19	0xFD
+
+static const struct tps80031_scalar_channel
+		tps80031_trim[TPS80031_GPADC_MAX_CHANNELS] = {
+	{ TPS80031_GPADC_TRIM1, 0x7, TPS80031_GPADC_TRIM2, 0x07},
+	{ 0x00, },
+	{ TPS80031_GPADC_TRIM3, 0x1F, TPS80031_GPADC_TRIM4, 0x3F},
+	{ 0x00, },
+	{ 0x00, },
+	{ 0x00, },
+	{ 0x00, },
+	{ TPS80031_GPADC_TRIM7, 0x1F, TPS80031_GPADC_TRIM8, 0x1F },
+	{ TPS80031_GPADC_TRIM9, 0x0F, TPS80031_GPADC_TRIM10, 0x1F },
+	{ TPS80031_GPADC_TRIM11, 0x0F, TPS80031_GPADC_TRIM12, 0x1F },
+	{ TPS80031_GPADC_TRIM13, 0x0F, TPS80031_GPADC_TRIM14, 0x1F },
+	{ 0x00, },
+	{ 0x00, },
+	{ 0x00, },
+	{ TPS80031_GPADC_TRIM15, 0x0f, TPS80031_GPADC_TRIM16, 0x1F },
+	{ 0x00, },
+	{ 0x00 ,},
+};
+
+/*
+* actual scaler gain is multiplied by 8 for fixed point operation
+* 1.875 * 8 = 15
+*/
+static const uint16_t tps80031_gain[TPS80031_GPADC_MAX_CHANNELS] = {
+	1142,   /* CHANNEL 0 */
+	8,      /* CHANNEL 1 */
+	/* 1.875 */
+	15,     /* CHANNEL 2 */
+	8,      /* CHANNEL 3 */
+	8,      /* CHANNEL 4 */
+	8,      /* CHANNEL 5 */
+	8,      /* CHANNEL 6 */
+	/* 5 */
+	40,     /* CHANNEL 7 */
+	/* 6.25 */
+	50,     /* CHANNEL 8 */
+	/* 11.25 */
+	90,     /* CHANNEL 9 */
+	/* 6.875 */
+	55,     /* CHANNEL 10 */
+	/* 1.875 */
+	15,     /* CHANNEL 11 */
+	8,      /* CHANNEL 12 */
+	8,      /* CHANNEL 13 */
+	/* 6.875 */
+	55,     /* CHANNEL 14 */
+	8,      /* CHANNEL 15 */
+	8,      /* CHANNEL 16 */
+};
+
+/*
+* calibration not needed for channel 11, 12, 13, 15 and 16
+* calibration offset is same for channel 1, 3, 4, 5
+*/
+static const struct tps80031_ideal_code
+	tps80031_ideal[TPS80031_GPADC_MAX_CHANNELS] = {
+	{463,   2982},  /* CHANNEL 0 */
+	{328,   3604},  /* CHANNEL 1 */
+	{221,   3274},  /* CHANNEL 2 */
+	{328,   3604},  /* CHANNEL 3 */
+	{328,   3604},  /* CHANNEL 4 */
+	{328,   3604},  /* CHANNEL 5 */
+	{328,   3604},  /* CHANNEL 6 */
+	{1966,  3013},  /* CHANNEL 7 */
+	{328,   2754},  /* CHANNEL 8 */
+	{728,   3275},  /* CHANNEL 9 */
+	{596,   3274},  /* CHANNEL 10 */
+	{0,     0},     /* CHANNEL 11 */
+	{0,     0},     /* CHANNEL 12 */
+	{0,     0},     /* CHANNEL 13 */
+	{193,   2859},  /* CHANNEL 14 */
+	{0,     0},     /* CHANNEL 15 */
+	{0,     0},     /* CHANNEL 16 */
+};
+
+struct tps80031_gpadc_data {
+	struct device		*dev;
+	struct mutex		lock;
+};
+
+static struct tps80031_gpadc_data *the_gpadc;
+
+static ssize_t show_gain(struct device *dev,
+		struct device_attribute *devattr, char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int value;
+	int status;
+
+	value = tps80031_calib_tbl[attr->index].gain_error;
+	status = sprintf(buf, "%d\n", value);
+	return status;
+}
+
+static ssize_t set_gain(struct device *dev,
+	struct device_attribute *devattr, const char *buf, size_t count)
+{
+	long val;
+	int status = count;
+
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	if ((strict_strtol(buf, 10, &val) < 0) || (val < 15000)
+				     || (val > 60000))
+		return -EINVAL;
+	tps80031_calib_tbl[attr->index].gain_error = val;
+	return status;
+}
+
+static ssize_t show_offset(struct device *dev,
+		struct device_attribute *devattr, char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int value;
+	int status;
+
+	value = tps80031_calib_tbl[attr->index].offset_error;
+	status = sprintf(buf, "%d\n", value);
+	return status;
+}
+
+static ssize_t set_offset(struct device *dev,
+	struct device_attribute *devattr, const char *buf, size_t count)
+{
+	long val;
+	int status = count;
+
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	if ((strict_strtol(buf, 10, &val) < 0) || (val < 15000)
+							|| (val > 60000))
+		return -EINVAL;
+	tps80031_calib_tbl[attr->index].offset_error = val;
+	return status;
+}
+
+static int tps80031_reg_read(struct tps80031_gpadc_data *gpadc, int sid,
+				int reg, uint8_t *val)
+{
+	int ret;
+
+	ret = tps80031_read(gpadc->dev->parent, sid, reg, val);
+	if (ret < 0)
+		dev_err(gpadc->dev, "Failed read register 0x%02x\n", reg);
+	return ret;
+}
+
+static int tps80031_reg_write(struct tps80031_gpadc_data *gpadc, int sid,
+				int reg, uint8_t val)
+{
+	int ret;
+
+	ret = tps80031_write(gpadc->dev->parent, sid, reg, val);
+	if (ret < 0)
+		dev_err(gpadc->dev, "Failed write register 0x%02x\n", reg);
+	return ret;
+}
+
+static int tps80031_gpadc_channel_raw_read(struct tps80031_gpadc_data *gpadc)
+{
+	uint8_t msb, lsb;
+	int ret;
+	ret = tps80031_reg_read(gpadc, SLAVE_ID2, GPCH0_LSB, &lsb);
+	if (ret < 0)
+		return ret;
+	ret = tps80031_reg_read(gpadc, SLAVE_ID2, GPCH0_MSB, &msb);
+	if (ret < 0)
+		return ret;
+
+	return (int)((msb << 8) | lsb);
+}
+
+static int tps80031_gpadc_read_channels(struct tps80031_gpadc_data *gpadc,
+						uint32_t channel)
+{
+	uint8_t bits;
+	int gain_error;
+	int offset_error;
+	int raw_code;
+	int corrected_code;
+	int channel_value;
+	int raw_channel_value;
+
+	/* TPS80031 has 12bit ADC */
+	bits = 12;
+	raw_code = tps80031_gpadc_channel_raw_read(gpadc);
+	if (raw_code < 0)
+		return raw_code;
+	/*
+	 * Channels 0,2,7,8,9,10,14 offst and gain cannot
+	 * be fully compensated by software
+	 */
+	if (channel == 7)
+		return raw_code;
+	/*
+	 * multiply by 1000 to convert the unit to milli
+	 * division by 1024 (>> bits) for 10/12 bit ADC
+	 * division by 8 (>> 3) for actual scaler gain
+	 */
+	raw_channel_value =
+		(raw_code * tps80031_gain[channel] * 1000) >> (bits + 3);
+
+	gain_error = tps80031_calib_tbl[channel].gain_error;
+	offset_error = tps80031_calib_tbl[channel].offset_error;
+	corrected_code = (raw_code * SCALE - offset_error) / gain_error;
+	channel_value =
+		(corrected_code * tps80031_gain[channel] * 1000) >> (bits + 3);
+	return channel_value;
+}
+
+static int tps80031_gpadc_wait_conversion_ready(
+	struct tps80031_gpadc_data *gpadc,
+		unsigned int timeout_ms)
+{
+	int ret;
+	unsigned long timeout;
+	timeout = jiffies + msecs_to_jiffies(timeout_ms);
+	do {
+		uint8_t reg;
+		ret = tps80031_reg_read(gpadc, SLAVE_ID2, CTRL_P1, &reg);
+		if (ret < 0)
+			return ret;
+		if (!(reg & GPADC_BUSY) &&
+				(reg & GPADC_EOC_SW))
+			return 0;
+	} while (!time_after(jiffies, timeout));
+	return -EAGAIN;
+}
+
+static inline int tps80031_gpadc_config
+	(struct tps80031_gpadc_data *gpadc, int channel_no)
+{
+	int ret = 0;
+
+	ret = tps80031_reg_write(gpadc, SLAVE_ID2, TOGGLE1, TOGGLE1_GPADCR);
+	if (ret < 0)
+		return ret;
+
+	ret = tps80031_reg_write(gpadc, SLAVE_ID2, GPSELECT_ISB, channel_no);
+	if (ret < 0)
+		return ret;
+
+	ret = tps80031_reg_write(gpadc, SLAVE_ID2, GPADC_CTRL, 0xef);
+	if (ret < 0)
+		return ret;
+
+	ret = tps80031_reg_write(gpadc, SLAVE_ID1, MISC1, 0x02);
+	if (ret < 0)
+		return ret;
+
+	return ret;
+}
+
+int tps80031_gpadc_conversion(int channel_no)
+{
+	int ret = 0;
+	int read_value;
+
+	mutex_lock(&the_gpadc->lock);
+
+	ret = tps80031_gpadc_config(the_gpadc, channel_no);
+	if (ret < 0)
+		goto err;
+
+	/* start ADC conversion */
+	ret = tps80031_reg_write(the_gpadc, SLAVE_ID2, CTRL_P1, CTRL_P1_SP1);
+	if (ret < 0)
+		goto err;
+
+	/* Wait until conversion is ready (ctrl register returns EOC) */
+	ret = tps80031_gpadc_wait_conversion_ready(the_gpadc, 5);
+	if (ret) {
+		dev_dbg(the_gpadc->dev, "conversion timeout!\n");
+		goto err;
+	}
+
+	read_value = tps80031_gpadc_read_channels(the_gpadc, channel_no);
+	mutex_unlock(&the_gpadc->lock);
+	return read_value;
+err:
+	mutex_unlock(&the_gpadc->lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(tps80031_gpadc_conversion);
+
+static SENSOR_DEVICE_ATTR(in0_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 0);
+static SENSOR_DEVICE_ATTR(in0_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 0);
+static SENSOR_DEVICE_ATTR(in1_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 1);
+static SENSOR_DEVICE_ATTR(in1_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 1);
+static SENSOR_DEVICE_ATTR(in2_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 2);
+static SENSOR_DEVICE_ATTR(in2_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 2);
+static SENSOR_DEVICE_ATTR(in3_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 3);
+static SENSOR_DEVICE_ATTR(in3_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 3);
+static SENSOR_DEVICE_ATTR(in4_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 4);
+static SENSOR_DEVICE_ATTR(in4_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 4);
+static SENSOR_DEVICE_ATTR(in5_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 5);
+static SENSOR_DEVICE_ATTR(in5_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 5);
+static SENSOR_DEVICE_ATTR(in6_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 6);
+static SENSOR_DEVICE_ATTR(in6_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 6);
+static SENSOR_DEVICE_ATTR(in7_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 7);
+static SENSOR_DEVICE_ATTR(in7_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 7);
+static SENSOR_DEVICE_ATTR(in8_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 8);
+static SENSOR_DEVICE_ATTR(in8_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 8);
+static SENSOR_DEVICE_ATTR(in9_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 9);
+static SENSOR_DEVICE_ATTR(in9_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 9);
+static SENSOR_DEVICE_ATTR(in10_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 10);
+static SENSOR_DEVICE_ATTR(in10_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 10);
+static SENSOR_DEVICE_ATTR(in11_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 11);
+static SENSOR_DEVICE_ATTR(in11_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 11);
+static SENSOR_DEVICE_ATTR(in12_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 12);
+static SENSOR_DEVICE_ATTR(in12_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 12);
+static SENSOR_DEVICE_ATTR(in13_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 13);
+static SENSOR_DEVICE_ATTR(in13_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 13);
+static SENSOR_DEVICE_ATTR(in14_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 14);
+static SENSOR_DEVICE_ATTR(in14_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 14);
+static SENSOR_DEVICE_ATTR(in15_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 15);
+static SENSOR_DEVICE_ATTR(in15_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 15);
+static SENSOR_DEVICE_ATTR(in16_gain, S_IRUGO|S_IWUSR, show_gain, set_gain, 16);
+static SENSOR_DEVICE_ATTR(in16_offset, S_IRUGO|S_IWUSR,
+						show_offset, set_offset, 16);
+
+#define IN_ATTRS(X)\
+	&sensor_dev_attr_in##X##_gain.dev_attr.attr,    \
+	&sensor_dev_attr_in##X##_offset.dev_attr.attr   \
+
+static struct attribute *tps80031_gpadc_attributes[] = {
+	IN_ATTRS(0),
+	IN_ATTRS(1),
+	IN_ATTRS(2),
+	IN_ATTRS(3),
+	IN_ATTRS(4),
+	IN_ATTRS(5),
+	IN_ATTRS(6),
+	IN_ATTRS(7),
+	IN_ATTRS(8),
+	IN_ATTRS(9),
+	IN_ATTRS(10),
+	IN_ATTRS(11),
+	IN_ATTRS(12),
+	IN_ATTRS(13),
+	IN_ATTRS(14),
+	IN_ATTRS(15),
+	IN_ATTRS(16),
+	NULL
+};
+
+static const struct attribute_group tps80031_gpadc_group = {
+	.attrs = tps80031_gpadc_attributes,
+};
+
+static long tps80031_gpadc_ioctl(struct file *filp, unsigned int cmd,
+						unsigned long arg)
+{
+	struct tps80031_gpadc_user_parms par;
+	int val, ret, channel_no;
+
+	ret = copy_from_user(&par, (void __user *) arg, sizeof(par));
+	if (ret) {
+		dev_dbg(the_gpadc->dev, "copy_from_user: %d\n", ret);
+		return -EACCES;
+	}
+	switch (cmd) {
+	case TPS80031_GPADC_IOCX_ADC_RAW_READ:
+		channel_no = par.channel;
+		val = tps80031_gpadc_conversion(channel_no);
+		if (likely(val > 0)) {
+			par.status = 0;
+			par.result = val;
+		} else if (val == 0) {
+			par.status = -ENODATA;
+		} else {
+			par.status = val;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+	ret = copy_to_user((void __user *) arg, &par, sizeof(par));
+	if (ret) {
+		dev_dbg(the_gpadc->dev, "copy_to_user: %d\n", ret);
+		return -EACCES;
+	}
+	return 0;
+}
+
+static const struct file_operations tps80031_gpadc_fileops = {
+	.owner = THIS_MODULE,
+	.unlocked_ioctl = tps80031_gpadc_ioctl,
+};
+
+static struct miscdevice tps80031_gpadc_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "tps80031-gpadc",
+	.fops = &tps80031_gpadc_fileops
+};
+
+static int __devinit tps80031_gpadc_probe(struct platform_device *pdev)
+{
+	struct tps80031_gpadc_data *gpadc;
+
+	s16 delta_error1 = 0, delta_error2 = 0;
+	s16 ideal_code1, ideal_code2;
+	s16 scalar_delta1 = 0, scalar_delta2 = 0;
+	s32 gain_error_1;
+	s32 offset_error;
+	uint8_t l_delta1, l_delta2, h_delta2;
+	uint8_t l_scalar1, l_scalar2;
+	uint8_t sign;
+	uint8_t index;
+	int ret;
+
+	gpadc = devm_kzalloc(&pdev->dev, sizeof *gpadc, GFP_KERNEL);
+	if (!gpadc)
+		return -ENOMEM;
+
+	gpadc->dev = &pdev->dev;
+	ret = misc_register(&tps80031_gpadc_device);
+	if (ret) {
+		dev_dbg(&pdev->dev, "could not register misc_device\n");
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, gpadc);
+	mutex_init(&gpadc->lock);
+
+	for (index = 0; index < TPS80031_GPADC_MAX_CHANNELS; index++) {
+		if (~calibration_bit_map & (1 << index))
+			continue;
+
+		if (~scalar_bit_map & (1 << index)) {
+			ret = tps80031_reg_read(gpadc, SLAVE_ID2,
+				tps80031_trim[index].delta1_addr, &l_scalar1);
+			if (ret < 0)
+				goto err;
+			ret = tps80031_reg_read(gpadc, SLAVE_ID2,
+				tps80031_trim[index].delta2_addr, &l_scalar2);
+			if (ret < 0)
+				goto err;
+
+			l_scalar1 &= tps80031_trim[index].delta1_mask;
+			sign = l_scalar1 & 1;
+			scalar_delta1 = l_scalar1 >> 1;
+			if (sign)
+				scalar_delta1 = 0 - scalar_delta1;
+			l_scalar2 &= tps80031_trim[index].delta2_mask;
+			sign = l_scalar2 & 1;
+			scalar_delta2 = l_scalar2 >> 1;
+			if (sign)
+				scalar_delta2 = 0 - scalar_delta2;
+		} else {
+			scalar_delta1 = 0;
+			scalar_delta2 = 0;
+		}
+		ret = tps80031_reg_read(gpadc, SLAVE_ID2, TPS80031_GPADC_TRIM5,
+							&l_delta1);
+		if (ret < 0)
+			goto err;
+		ret = tps80031_reg_read(gpadc, SLAVE_ID2, TPS80031_GPADC_TRIM6,
+							&l_delta2);
+		if (ret < 0)
+			goto err;
+		ret = tps80031_reg_read(gpadc, SLAVE_ID2, TPS80031_GPADC_TRIM19,
+							&h_delta2);
+		if (ret < 0)
+			goto err;
+
+		sign = l_delta1 & 1;
+
+		delta_error1 = l_delta1 >> 1;
+		if (sign)
+			delta_error1 = (0 - delta_error1);
+		sign = l_delta2 & 1;
+
+		delta_error2 = (l_delta2 >> 1) | (h_delta2 << 7);
+		if (sign)
+			delta_error2 = (0 - delta_error2);
+		ideal_code1 = tps80031_ideal[index].code1 * 4;
+		ideal_code2 = tps80031_ideal[index].code2 * 4;
+
+		gain_error_1 = ((delta_error2 + scalar_delta2) -
+				(delta_error1 - scalar_delta1)) *
+				SCALE / (ideal_code2 - ideal_code1);
+		offset_error = (delta_error1 + scalar_delta1) *
+				SCALE - gain_error_1 *  ideal_code1;
+
+		tps80031_calib_tbl[index].gain_error = gain_error_1 + SCALE;
+		tps80031_calib_tbl[index].offset_error = offset_error;
+	}
+
+	the_gpadc = gpadc;
+	ret = sysfs_create_group(&pdev->dev.kobj, &tps80031_gpadc_group);
+	if (ret) {
+		dev_err(&pdev->dev, "could not create sysfs files\n");
+		goto err;
+	}
+	return 0;
+err:
+	misc_deregister(&tps80031_gpadc_device);
+	return ret;
+}
+
+static int __devexit tps80031_gpadc_remove(struct platform_device *pdev)
+{
+	sysfs_remove_group(&pdev->dev.kobj, &tps80031_gpadc_group);
+	misc_deregister(&tps80031_gpadc_device);
+	return 0;
+}
+
+static struct platform_driver tps80031_gpadc_driver = {
+	.probe		= tps80031_gpadc_probe,
+	.remove		= __devexit_p(tps80031_gpadc_remove),
+	.driver		= {
+		.name	= "tps80031-gpadc",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init tps80031_gpadc_init(void)
+{
+	return platform_driver_register(&tps80031_gpadc_driver);
+}
+
+module_init(tps80031_gpadc_init);
+
+static void __exit tps80031_gpadc_exit(void)
+{
+	platform_driver_unregister(&tps80031_gpadc_driver);
+}
+
+module_exit(tps80031_gpadc_exit);
+MODULE_ALIAS("platform:tps80031-gpadc");
+MODULE_DESCRIPTION("tps80031 ADC driver");
diff --git a/drivers/mfd/twl-core.c b/drivers/mfd/twl-core.c
index 01ecfeee6524..630ebb1e2f92 100644
--- a/drivers/mfd/twl-core.c
+++ b/drivers/mfd/twl-core.c
@@ -58,6 +58,14 @@
 
 #define DRIVER_NAME			"twl"
 
+#if defined(CONFIG_TWL4030_BCI_BATTERY) || \
+	defined(CONFIG_TWL4030_BCI_BATTERY_MODULE) || \
+	defined(CONFIG_TWL6030_BCI_BATTERY) || \
+	defined(CONFIG_TWL6030_BCI_BATTERY_MODULE)
+#define twl_has_bci()		true
+#else
+#define twl_has_bci()		false
+#endif
 #if defined(CONFIG_KEYBOARD_TWL4030) || defined(CONFIG_KEYBOARD_TWL4030_MODULE)
 #define twl_has_keypad()	true
 #else
@@ -77,7 +85,8 @@
 #define twl_has_regulator()	false
 #endif
 
-#if defined(CONFIG_TWL4030_MADC) || defined(CONFIG_TWL4030_MADC_MODULE)
+#if defined(CONFIG_TWL4030_MADC) || defined(CONFIG_TWL4030_MADC_MODULE) ||\
+    defined(CONFIG_TWL6030_GPADC) || defined(CONFIG_TWL6030_GPADC_MODULE)
 #define twl_has_madc()	true
 #else
 #define twl_has_madc()	false
@@ -109,7 +118,7 @@
 #define twl_has_watchdog()        false
 #endif
 
-#if defined(CONFIG_TWL4030_CODEC) || defined(CONFIG_TWL4030_CODEC_MODULE) ||\
+#if defined(CONFIG_MFD_TWL4030_AUDIO) || defined(CONFIG_MFD_TWL4030_AUDIO_MODULE) ||\
 	defined(CONFIG_TWL6040_CORE) || defined(CONFIG_TWL6040_CORE_MODULE)
 #define twl_has_codec()	true
 #else
@@ -125,7 +134,7 @@
 /* Triton Core internal information (BEGIN) */
 
 /* Last - for index max*/
-#define TWL4030_MODULE_LAST		TWL4030_MODULE_SECURED_REG
+#define TWL4030_MODULE_LAST		TWL6025_MODULE_CHARGER
 
 #define TWL_NUM_SLAVES		4
 
@@ -208,6 +217,11 @@
 #define TWL6030_BASEADD_RSV		0x0000
 #define TWL6030_BASEADD_ZERO		0x0000
 
+/* twl6030 SMPS EPROM values */
+#define TWL6030_SMPS_OFFSET		0xB0
+#define TWL6030_SMPS_MULT		0xB3
+
+
 /* Few power values */
 #define R_CFG_BOOT			0x05
 
@@ -218,6 +232,9 @@
 #define HIGH_PERF_SQ			(1 << 3)
 #define CK32K_LOWPWR_EN			(1 << 7)
 
+/* MPU80031 specific clock32 generation register */
+#define REG_CLK32KG_CFG_TRANS 0x8D
+#define REG_CLK32KG_CFG_STATE 0x8E
 
 /* chip-specific feature flags, for i2c_device_id.driver_data */
 #define TWL4030_VAUX2		BIT(0)	/* pre-5030 voltage ranges */
@@ -240,6 +257,33 @@ unsigned int twl_rev(void)
 }
 EXPORT_SYMBOL(twl_rev);
 
+static unsigned int twl_feat;
+unsigned int twl_features(void)
+{
+	return twl_feat;
+}
+EXPORT_SYMBOL(twl_features);
+
+u8 twl_get_smps_offset(void)
+{
+	u8 value;
+
+	twl_i2c_read_u8(TWL_MODULE_PM_RECEIVER, &value,
+			TWL6030_SMPS_OFFSET);
+	return value;
+}
+EXPORT_SYMBOL(twl_get_smps_offset);
+
+u8 twl_get_smps_mult(void)
+{
+	u8 value;
+
+	twl_i2c_read_u8(TWL_MODULE_PM_RECEIVER, &value,
+			TWL6030_SMPS_MULT);
+	return value;
+}
+EXPORT_SYMBOL(twl_get_smps_mult);
+
 /* Structure for each TWL4030/TWL6030 Slave */
 struct twl_client {
 	struct i2c_client *client;
@@ -362,13 +406,13 @@ int twl_i2c_write(u8 mod_no, u8 *value, u8 reg, unsigned num_bytes)
 		pr_err("%s: invalid module number %d\n", DRIVER_NAME, mod_no);
 		return -EPERM;
 	}
-	sid = twl_map[mod_no].sid;
-	twl = &twl_modules[sid];
-
 	if (unlikely(!inuse)) {
-		pr_err("%s: client %d is not initialized\n", DRIVER_NAME, sid);
+		pr_err("%s: not initialized\n", DRIVER_NAME);
 		return -EPERM;
 	}
+	sid = twl_map[mod_no].sid;
+	twl = &twl_modules[sid];
+
 	mutex_lock(&twl->xfer_lock);
 	/*
 	 * [MSG1]: fill the register address data
@@ -419,13 +463,13 @@ int twl_i2c_read(u8 mod_no, u8 *value, u8 reg, unsigned num_bytes)
 		pr_err("%s: invalid module number %d\n", DRIVER_NAME, mod_no);
 		return -EPERM;
 	}
-	sid = twl_map[mod_no].sid;
-	twl = &twl_modules[sid];
-
 	if (unlikely(!inuse)) {
-		pr_err("%s: client %d is not initialized\n", DRIVER_NAME, sid);
+		pr_err("%s: not initialized\n", DRIVER_NAME);
 		return -EPERM;
 	}
+	sid = twl_map[mod_no].sid;
+	twl = &twl_modules[sid];
+
 	mutex_lock(&twl->xfer_lock);
 	/* [MSG1] fill the register address data */
 	msg = &twl->xfer_msg[0];
@@ -490,6 +534,19 @@ int twl_i2c_read_u8(u8 mod_no, u8 *value, u8 reg)
 }
 EXPORT_SYMBOL(twl_i2c_read_u8);
 
+
+void twl_reg_dump(int module, int start, int end)
+{
+	int i;
+	u8 val;
+
+	for (i = start; i < end; i++) {
+		twl_i2c_read_u8(module, &val, i);
+		printk(KERN_ERR "reg 0x%2x val 0x%2x\n", i, val);
+	}
+}
+EXPORT_SYMBOL(twl_reg_dump);
+
 /*----------------------------------------------------------------------*/
 
 /**
@@ -660,8 +717,16 @@ add_children(struct twl4030_platform_data *pdata, unsigned long features)
 		if (IS_ERR(child))
 			return PTR_ERR(child);
 	}
+	if (twl_has_bci() && pdata->bci &&
+	    (features & TWL6030_CLASS)) {
+		child = add_child(1, "twl6030_bci",
+				pdata->bci, sizeof(*pdata->bci),
+				false,
+				pdata->irq_base + CHARGER_INTR_OFFSET,
+				pdata->irq_base + CHARGERFAULT_INTR_OFFSET);
+	}
 
-	if (twl_has_madc() && pdata->madc) {
+	if (twl_has_madc() && pdata->madc && twl_class_is_4030()) {
 		child = add_child(2, "twl4030_madc",
 				pdata->madc, sizeof(*pdata->madc),
 				true, pdata->irq_base + MADC_INTR_OFFSET, 0);
@@ -669,6 +734,15 @@ add_children(struct twl4030_platform_data *pdata, unsigned long features)
 			return PTR_ERR(child);
 	}
 
+	if (twl_has_madc() && pdata->madc && twl_class_is_6030()) {
+		child = add_child(1, "twl6030_gpadc",
+				pdata->madc, sizeof(*pdata->madc),
+				true, pdata->irq_base + MADC_INTR_OFFSET,
+				pdata->irq_base + GPADCSW_INTR_OFFSET);
+		if (IS_ERR(child))
+			return PTR_ERR(child);
+	}
+
 	if (twl_has_rtc()) {
 		/*
 		 * REVISIT platform_data here currently might expose the
@@ -1084,13 +1158,29 @@ static inline int __init unprotect_pm_master(void)
 }
 
 static void clocks_init(struct device *dev,
-			struct twl4030_clock_init_data *clock)
+			struct twl4030_clock_init_data *clock,
+			unsigned long features)
 {
 	int e = 0;
 	struct clk *osc;
 	u32 rate;
 	u8 ctrl = HFCLK_FREQ_26_MHZ;
 
+	if (features & MPU80031_SUBCLASS) {
+		if (clock && clock->clk32_active_state_on) {
+			e = twl_i2c_write_u8(TWL_MODULE_PM_RECEIVER, 0x1,
+					REG_CLK32KG_CFG_TRANS);
+			if (!e)
+				e = twl_i2c_write_u8(TWL_MODULE_PM_RECEIVER, 0x1,
+					REG_CLK32KG_CFG_STATE);
+			if (e) {
+				dev_err(dev, "Error in initialization"
+						" of 32K output\n");
+				return;
+			}
+		}
+	}
+
 #if defined(CONFIG_ARCH_OMAP2) || defined(CONFIG_ARCH_OMAP3)
 	if (cpu_is_omap2430())
 		osc = clk_get(dev, "osc_ck");
@@ -1228,7 +1318,7 @@ twl_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	}
 
 	/* setup clock framework */
-	clocks_init(&client->dev, pdata->clock);
+	clocks_init(&client->dev, pdata->clock, id->driver_data);
 
 	/* read TWL IDCODE Register */
 	if (twl_id == TWL4030_CLASS_ID) {
@@ -1269,7 +1359,16 @@ twl_probe(struct i2c_client *client, const struct i2c_device_id *id)
 		twl_i2c_write_u8(TWL4030_MODULE_INTBR, temp, REG_GPPUPDCTR1);
 	}
 
+	twl_feat = id->driver_data;
+
 	status = add_children(pdata, id->driver_data);
+	if (status < 0 )
+		goto fail;
+
+	/* Board Specific Init Callback */
+	if(pdata->init)
+		status = pdata->init();
+
 fail:
 	if (status < 0)
 		twl_remove(client);
@@ -1287,6 +1386,7 @@ static const struct i2c_device_id twl_ids[] = {
 					   and vibrator. Charger in USB module*/
 	{ "twl6030", TWL6030_CLASS },	/* "Phoenix power chip" */
 	{ "twl6025", TWL6030_CLASS | TWL6025_SUBCLASS }, /* "Phoenix lite" */
+	{ "mpu80031", TWL6030_CLASS | TWL6025_SUBCLASS | MPU80031_SUBCLASS},
 	{ /* end of list */ },
 };
 MODULE_DEVICE_TABLE(i2c, twl_ids);
diff --git a/drivers/mfd/twl4030-madc.c b/drivers/mfd/twl4030-madc.c
index 7cbf2aa9e64f..834f824d3c11 100644
--- a/drivers/mfd/twl4030-madc.c
+++ b/drivers/mfd/twl4030-madc.c
@@ -740,6 +740,28 @@ static int __devinit twl4030_madc_probe(struct platform_device *pdev)
 			TWL4030_BCI_BCICTL1);
 		goto err_i2c;
 	}
+
+	/* Check that MADC clock is on */
+	ret = twl_i2c_read_u8(TWL4030_MODULE_INTBR, &regval, TWL4030_REG_GPBR1);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to read reg GPBR1 0x%X\n",
+				TWL4030_REG_GPBR1);
+		goto err_i2c;
+	}
+
+	/* If MADC clk is not on, turn it on */
+	if (!(regval & TWL4030_GPBR1_MADC_HFCLK_EN)) {
+		dev_info(&pdev->dev, "clk disabled, enabling\n");
+		regval |= TWL4030_GPBR1_MADC_HFCLK_EN;
+		ret = twl_i2c_write_u8(TWL4030_MODULE_INTBR, regval,
+				       TWL4030_REG_GPBR1);
+		if (ret) {
+			dev_err(&pdev->dev, "unable to write reg GPBR1 0x%X\n",
+					TWL4030_REG_GPBR1);
+			goto err_i2c;
+		}
+	}
+
 	platform_set_drvdata(pdev, madc);
 	mutex_init(&madc->lock);
 	ret = request_threaded_irq(platform_get_irq(pdev, 0), NULL,
diff --git a/drivers/mfd/wm831x-core.c b/drivers/mfd/wm831x-core.c
index 282e76ab678f..0a2b8d41a702 100644
--- a/drivers/mfd/wm831x-core.c
+++ b/drivers/mfd/wm831x-core.c
@@ -18,12 +18,14 @@
 #include <linux/delay.h>
 #include <linux/mfd/core.h>
 #include <linux/slab.h>
+#include <linux/err.h>
 
 #include <linux/mfd/wm831x/core.h>
 #include <linux/mfd/wm831x/pdata.h>
 #include <linux/mfd/wm831x/irq.h>
 #include <linux/mfd/wm831x/auxadc.h>
 #include <linux/mfd/wm831x/otp.h>
+#include <linux/mfd/wm831x/pmu.h>
 #include <linux/mfd/wm831x/regulator.h>
 
 /* Current settings - values are 2*2^(reg_val/4) microamps.  These are
@@ -160,27 +162,350 @@ int wm831x_reg_unlock(struct wm831x *wm831x)
 }
 EXPORT_SYMBOL_GPL(wm831x_reg_unlock);
 
-static int wm831x_read(struct wm831x *wm831x, unsigned short reg,
-		       int bytes, void *dest)
+static bool wm831x_reg_readable(struct device *dev, unsigned int reg)
 {
-	int ret, i;
-	u16 *buf = dest;
-
-	BUG_ON(bytes % 2);
-	BUG_ON(bytes <= 0);
+	switch (reg) {
+	case WM831X_RESET_ID:
+	case WM831X_REVISION:
+	case WM831X_PARENT_ID:
+	case WM831X_SYSVDD_CONTROL:
+	case WM831X_THERMAL_MONITORING:
+	case WM831X_POWER_STATE:
+	case WM831X_WATCHDOG:
+	case WM831X_ON_PIN_CONTROL:
+	case WM831X_RESET_CONTROL:
+	case WM831X_CONTROL_INTERFACE:
+	case WM831X_SECURITY_KEY:
+	case WM831X_SOFTWARE_SCRATCH:
+	case WM831X_OTP_CONTROL:
+	case WM831X_GPIO_LEVEL:
+	case WM831X_SYSTEM_STATUS:
+	case WM831X_ON_SOURCE:
+	case WM831X_OFF_SOURCE:
+	case WM831X_SYSTEM_INTERRUPTS:
+	case WM831X_INTERRUPT_STATUS_1:
+	case WM831X_INTERRUPT_STATUS_2:
+	case WM831X_INTERRUPT_STATUS_3:
+	case WM831X_INTERRUPT_STATUS_4:
+	case WM831X_INTERRUPT_STATUS_5:
+	case WM831X_IRQ_CONFIG:
+	case WM831X_SYSTEM_INTERRUPTS_MASK:
+	case WM831X_INTERRUPT_STATUS_1_MASK:
+	case WM831X_INTERRUPT_STATUS_2_MASK:
+	case WM831X_INTERRUPT_STATUS_3_MASK:
+	case WM831X_INTERRUPT_STATUS_4_MASK:
+	case WM831X_INTERRUPT_STATUS_5_MASK:
+	case WM831X_RTC_WRITE_COUNTER:
+	case WM831X_RTC_TIME_1:
+	case WM831X_RTC_TIME_2:
+	case WM831X_RTC_ALARM_1:
+	case WM831X_RTC_ALARM_2:
+	case WM831X_RTC_CONTROL:
+	case WM831X_RTC_TRIM:
+	case WM831X_TOUCH_CONTROL_1:
+	case WM831X_TOUCH_CONTROL_2:
+	case WM831X_TOUCH_DATA_X:
+	case WM831X_TOUCH_DATA_Y:
+	case WM831X_TOUCH_DATA_Z:
+	case WM831X_AUXADC_DATA:
+	case WM831X_AUXADC_CONTROL:
+	case WM831X_AUXADC_SOURCE:
+	case WM831X_COMPARATOR_CONTROL:
+	case WM831X_COMPARATOR_1:
+	case WM831X_COMPARATOR_2:
+	case WM831X_COMPARATOR_3:
+	case WM831X_COMPARATOR_4:
+	case WM831X_GPIO1_CONTROL:
+	case WM831X_GPIO2_CONTROL:
+	case WM831X_GPIO3_CONTROL:
+	case WM831X_GPIO4_CONTROL:
+	case WM831X_GPIO5_CONTROL:
+	case WM831X_GPIO6_CONTROL:
+	case WM831X_GPIO7_CONTROL:
+	case WM831X_GPIO8_CONTROL:
+	case WM831X_GPIO9_CONTROL:
+	case WM831X_GPIO10_CONTROL:
+	case WM831X_GPIO11_CONTROL:
+	case WM831X_GPIO12_CONTROL:
+	case WM831X_GPIO13_CONTROL:
+	case WM831X_GPIO14_CONTROL:
+	case WM831X_GPIO15_CONTROL:
+	case WM831X_GPIO16_CONTROL:
+	case WM831X_CHARGER_CONTROL_1:
+	case WM831X_CHARGER_CONTROL_2:
+	case WM831X_CHARGER_STATUS:
+	case WM831X_BACKUP_CHARGER_CONTROL:
+	case WM831X_STATUS_LED_1:
+	case WM831X_STATUS_LED_2:
+	case WM831X_CURRENT_SINK_1:
+	case WM831X_CURRENT_SINK_2:
+	case WM831X_DCDC_ENABLE:
+	case WM831X_LDO_ENABLE:
+	case WM831X_DCDC_STATUS:
+	case WM831X_LDO_STATUS:
+	case WM831X_DCDC_UV_STATUS:
+	case WM831X_LDO_UV_STATUS:
+	case WM831X_DC1_CONTROL_1:
+	case WM831X_DC1_CONTROL_2:
+	case WM831X_DC1_ON_CONFIG:
+	case WM831X_DC1_SLEEP_CONTROL:
+	case WM831X_DC1_DVS_CONTROL:
+	case WM831X_DC2_CONTROL_1:
+	case WM831X_DC2_CONTROL_2:
+	case WM831X_DC2_ON_CONFIG:
+	case WM831X_DC2_SLEEP_CONTROL:
+	case WM831X_DC2_DVS_CONTROL:
+	case WM831X_DC3_CONTROL_1:
+	case WM831X_DC3_CONTROL_2:
+	case WM831X_DC3_ON_CONFIG:
+	case WM831X_DC3_SLEEP_CONTROL:
+	case WM831X_DC4_CONTROL:
+	case WM831X_DC4_SLEEP_CONTROL:
+	case WM831X_EPE1_CONTROL:
+	case WM831X_EPE2_CONTROL:
+	case WM831X_LDO1_CONTROL:
+	case WM831X_LDO1_ON_CONTROL:
+	case WM831X_LDO1_SLEEP_CONTROL:
+	case WM831X_LDO2_CONTROL:
+	case WM831X_LDO2_ON_CONTROL:
+	case WM831X_LDO2_SLEEP_CONTROL:
+	case WM831X_LDO3_CONTROL:
+	case WM831X_LDO3_ON_CONTROL:
+	case WM831X_LDO3_SLEEP_CONTROL:
+	case WM831X_LDO4_CONTROL:
+	case WM831X_LDO4_ON_CONTROL:
+	case WM831X_LDO4_SLEEP_CONTROL:
+	case WM831X_LDO5_CONTROL:
+	case WM831X_LDO5_ON_CONTROL:
+	case WM831X_LDO5_SLEEP_CONTROL:
+	case WM831X_LDO6_CONTROL:
+	case WM831X_LDO6_ON_CONTROL:
+	case WM831X_LDO6_SLEEP_CONTROL:
+	case WM831X_LDO7_CONTROL:
+	case WM831X_LDO7_ON_CONTROL:
+	case WM831X_LDO7_SLEEP_CONTROL:
+	case WM831X_LDO8_CONTROL:
+	case WM831X_LDO8_ON_CONTROL:
+	case WM831X_LDO8_SLEEP_CONTROL:
+	case WM831X_LDO9_CONTROL:
+	case WM831X_LDO9_ON_CONTROL:
+	case WM831X_LDO9_SLEEP_CONTROL:
+	case WM831X_LDO10_CONTROL:
+	case WM831X_LDO10_ON_CONTROL:
+	case WM831X_LDO10_SLEEP_CONTROL:
+	case WM831X_LDO11_ON_CONTROL:
+	case WM831X_LDO11_SLEEP_CONTROL:
+	case WM831X_POWER_GOOD_SOURCE_1:
+	case WM831X_POWER_GOOD_SOURCE_2:
+	case WM831X_CLOCK_CONTROL_1:
+	case WM831X_CLOCK_CONTROL_2:
+	case WM831X_FLL_CONTROL_1:
+	case WM831X_FLL_CONTROL_2:
+	case WM831X_FLL_CONTROL_3:
+	case WM831X_FLL_CONTROL_4:
+	case WM831X_FLL_CONTROL_5:
+	case WM831X_UNIQUE_ID_1:
+	case WM831X_UNIQUE_ID_2:
+	case WM831X_UNIQUE_ID_3:
+	case WM831X_UNIQUE_ID_4:
+	case WM831X_UNIQUE_ID_5:
+	case WM831X_UNIQUE_ID_6:
+	case WM831X_UNIQUE_ID_7:
+	case WM831X_UNIQUE_ID_8:
+	case WM831X_FACTORY_OTP_ID:
+	case WM831X_FACTORY_OTP_1:
+	case WM831X_FACTORY_OTP_2:
+	case WM831X_FACTORY_OTP_3:
+	case WM831X_FACTORY_OTP_4:
+	case WM831X_FACTORY_OTP_5:
+	case WM831X_CUSTOMER_OTP_ID:
+	case WM831X_DC1_OTP_CONTROL:
+	case WM831X_DC2_OTP_CONTROL:
+	case WM831X_DC3_OTP_CONTROL:
+	case WM831X_LDO1_2_OTP_CONTROL:
+	case WM831X_LDO3_4_OTP_CONTROL:
+	case WM831X_LDO5_6_OTP_CONTROL:
+	case WM831X_LDO7_8_OTP_CONTROL:
+	case WM831X_LDO9_10_OTP_CONTROL:
+	case WM831X_LDO11_EPE_CONTROL:
+	case WM831X_GPIO1_OTP_CONTROL:
+	case WM831X_GPIO2_OTP_CONTROL:
+	case WM831X_GPIO3_OTP_CONTROL:
+	case WM831X_GPIO4_OTP_CONTROL:
+	case WM831X_GPIO5_OTP_CONTROL:
+	case WM831X_GPIO6_OTP_CONTROL:
+	case WM831X_DBE_CHECK_DATA:
+		return true;
+	default:
+		return false;
+	}
+}
 
-	ret = wm831x->read_dev(wm831x, reg, bytes, dest);
-	if (ret < 0)
-		return ret;
+static bool wm831x_reg_writeable(struct device *dev, unsigned int reg)
+{
+	struct wm831x *wm831x = dev_get_drvdata(dev);
 
-	for (i = 0; i < bytes / 2; i++) {
-		buf[i] = be16_to_cpu(buf[i]);
+	if (wm831x_reg_locked(wm831x, reg))
+		return false;
 
-		dev_vdbg(wm831x->dev, "Read %04x from R%d(0x%x)\n",
-			 buf[i], reg + i, reg + i);
+	switch (reg) {
+	case WM831X_SYSVDD_CONTROL:
+	case WM831X_THERMAL_MONITORING:
+	case WM831X_POWER_STATE:
+	case WM831X_WATCHDOG:
+	case WM831X_ON_PIN_CONTROL:
+	case WM831X_RESET_CONTROL:
+	case WM831X_CONTROL_INTERFACE:
+	case WM831X_SECURITY_KEY:
+	case WM831X_SOFTWARE_SCRATCH:
+	case WM831X_OTP_CONTROL:
+	case WM831X_GPIO_LEVEL:
+	case WM831X_INTERRUPT_STATUS_1:
+	case WM831X_INTERRUPT_STATUS_2:
+	case WM831X_INTERRUPT_STATUS_3:
+	case WM831X_INTERRUPT_STATUS_4:
+	case WM831X_INTERRUPT_STATUS_5:
+	case WM831X_IRQ_CONFIG:
+	case WM831X_SYSTEM_INTERRUPTS_MASK:
+	case WM831X_INTERRUPT_STATUS_1_MASK:
+	case WM831X_INTERRUPT_STATUS_2_MASK:
+	case WM831X_INTERRUPT_STATUS_3_MASK:
+	case WM831X_INTERRUPT_STATUS_4_MASK:
+	case WM831X_INTERRUPT_STATUS_5_MASK:
+	case WM831X_RTC_TIME_1:
+	case WM831X_RTC_TIME_2:
+	case WM831X_RTC_ALARM_1:
+	case WM831X_RTC_ALARM_2:
+	case WM831X_RTC_CONTROL:
+	case WM831X_RTC_TRIM:
+	case WM831X_TOUCH_CONTROL_1:
+	case WM831X_TOUCH_CONTROL_2:
+	case WM831X_AUXADC_CONTROL:
+	case WM831X_AUXADC_SOURCE:
+	case WM831X_COMPARATOR_CONTROL:
+	case WM831X_COMPARATOR_1:
+	case WM831X_COMPARATOR_2:
+	case WM831X_COMPARATOR_3:
+	case WM831X_COMPARATOR_4:
+	case WM831X_GPIO1_CONTROL:
+	case WM831X_GPIO2_CONTROL:
+	case WM831X_GPIO3_CONTROL:
+	case WM831X_GPIO4_CONTROL:
+	case WM831X_GPIO5_CONTROL:
+	case WM831X_GPIO6_CONTROL:
+	case WM831X_GPIO7_CONTROL:
+	case WM831X_GPIO8_CONTROL:
+	case WM831X_GPIO9_CONTROL:
+	case WM831X_GPIO10_CONTROL:
+	case WM831X_GPIO11_CONTROL:
+	case WM831X_GPIO12_CONTROL:
+	case WM831X_GPIO13_CONTROL:
+	case WM831X_GPIO14_CONTROL:
+	case WM831X_GPIO15_CONTROL:
+	case WM831X_GPIO16_CONTROL:
+	case WM831X_CHARGER_CONTROL_1:
+	case WM831X_CHARGER_CONTROL_2:
+	case WM831X_CHARGER_STATUS:
+	case WM831X_BACKUP_CHARGER_CONTROL:
+	case WM831X_STATUS_LED_1:
+	case WM831X_STATUS_LED_2:
+	case WM831X_CURRENT_SINK_1:
+	case WM831X_CURRENT_SINK_2:
+	case WM831X_DCDC_ENABLE:
+	case WM831X_LDO_ENABLE:
+	case WM831X_DC1_CONTROL_1:
+	case WM831X_DC1_CONTROL_2:
+	case WM831X_DC1_ON_CONFIG:
+	case WM831X_DC1_SLEEP_CONTROL:
+	case WM831X_DC1_DVS_CONTROL:
+	case WM831X_DC2_CONTROL_1:
+	case WM831X_DC2_CONTROL_2:
+	case WM831X_DC2_ON_CONFIG:
+	case WM831X_DC2_SLEEP_CONTROL:
+	case WM831X_DC2_DVS_CONTROL:
+	case WM831X_DC3_CONTROL_1:
+	case WM831X_DC3_CONTROL_2:
+	case WM831X_DC3_ON_CONFIG:
+	case WM831X_DC3_SLEEP_CONTROL:
+	case WM831X_DC4_CONTROL:
+	case WM831X_DC4_SLEEP_CONTROL:
+	case WM831X_EPE1_CONTROL:
+	case WM831X_EPE2_CONTROL:
+	case WM831X_LDO1_CONTROL:
+	case WM831X_LDO1_ON_CONTROL:
+	case WM831X_LDO1_SLEEP_CONTROL:
+	case WM831X_LDO2_CONTROL:
+	case WM831X_LDO2_ON_CONTROL:
+	case WM831X_LDO2_SLEEP_CONTROL:
+	case WM831X_LDO3_CONTROL:
+	case WM831X_LDO3_ON_CONTROL:
+	case WM831X_LDO3_SLEEP_CONTROL:
+	case WM831X_LDO4_CONTROL:
+	case WM831X_LDO4_ON_CONTROL:
+	case WM831X_LDO4_SLEEP_CONTROL:
+	case WM831X_LDO5_CONTROL:
+	case WM831X_LDO5_ON_CONTROL:
+	case WM831X_LDO5_SLEEP_CONTROL:
+	case WM831X_LDO6_CONTROL:
+	case WM831X_LDO6_ON_CONTROL:
+	case WM831X_LDO6_SLEEP_CONTROL:
+	case WM831X_LDO7_CONTROL:
+	case WM831X_LDO7_ON_CONTROL:
+	case WM831X_LDO7_SLEEP_CONTROL:
+	case WM831X_LDO8_CONTROL:
+	case WM831X_LDO8_ON_CONTROL:
+	case WM831X_LDO8_SLEEP_CONTROL:
+	case WM831X_LDO9_CONTROL:
+	case WM831X_LDO9_ON_CONTROL:
+	case WM831X_LDO9_SLEEP_CONTROL:
+	case WM831X_LDO10_CONTROL:
+	case WM831X_LDO10_ON_CONTROL:
+	case WM831X_LDO10_SLEEP_CONTROL:
+	case WM831X_LDO11_ON_CONTROL:
+	case WM831X_LDO11_SLEEP_CONTROL:
+	case WM831X_POWER_GOOD_SOURCE_1:
+	case WM831X_POWER_GOOD_SOURCE_2:
+	case WM831X_CLOCK_CONTROL_1:
+	case WM831X_CLOCK_CONTROL_2:
+	case WM831X_FLL_CONTROL_1:
+	case WM831X_FLL_CONTROL_2:
+	case WM831X_FLL_CONTROL_3:
+	case WM831X_FLL_CONTROL_4:
+	case WM831X_FLL_CONTROL_5:
+		return true;
+	default:
+		return false;
 	}
+}
 
-	return 0;
+static bool wm831x_reg_volatile(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case WM831X_SYSTEM_STATUS:
+	case WM831X_ON_SOURCE:
+	case WM831X_OFF_SOURCE:
+	case WM831X_GPIO_LEVEL:
+	case WM831X_SYSTEM_INTERRUPTS:
+	case WM831X_INTERRUPT_STATUS_1:
+	case WM831X_INTERRUPT_STATUS_2:
+	case WM831X_INTERRUPT_STATUS_3:
+	case WM831X_INTERRUPT_STATUS_4:
+	case WM831X_INTERRUPT_STATUS_5:
+	case WM831X_RTC_TIME_1:
+	case WM831X_RTC_TIME_2:
+	case WM831X_TOUCH_DATA_X:
+	case WM831X_TOUCH_DATA_Y:
+	case WM831X_TOUCH_DATA_Z:
+	case WM831X_AUXADC_DATA:
+	case WM831X_CHARGER_STATUS:
+	case WM831X_DCDC_STATUS:
+	case WM831X_LDO_STATUS:
+	case WM831X_DCDC_UV_STATUS:
+	case WM831X_LDO_UV_STATUS:
+		return true;
+	default:
+		return false;
+	}
 }
 
 /**
@@ -191,14 +516,10 @@ static int wm831x_read(struct wm831x *wm831x, unsigned short reg,
  */
 int wm831x_reg_read(struct wm831x *wm831x, unsigned short reg)
 {
-	unsigned short val;
+	unsigned int val;
 	int ret;
 
-	mutex_lock(&wm831x->io_lock);
-
-	ret = wm831x_read(wm831x, reg, 2, &val);
-
-	mutex_unlock(&wm831x->io_lock);
+	ret = regmap_read(wm831x->regmap, reg, &val);
 
 	if (ret < 0)
 		return ret;
@@ -218,15 +539,7 @@ EXPORT_SYMBOL_GPL(wm831x_reg_read);
 int wm831x_bulk_read(struct wm831x *wm831x, unsigned short reg,
 		     int count, u16 *buf)
 {
-	int ret;
-
-	mutex_lock(&wm831x->io_lock);
-
-	ret = wm831x_read(wm831x, reg, count * 2, buf);
-
-	mutex_unlock(&wm831x->io_lock);
-
-	return ret;
+	return regmap_bulk_read(wm831x->regmap, reg, buf, count);
 }
 EXPORT_SYMBOL_GPL(wm831x_bulk_read);
 
@@ -234,7 +547,7 @@ static int wm831x_write(struct wm831x *wm831x, unsigned short reg,
 			int bytes, void *src)
 {
 	u16 *buf = src;
-	int i;
+	int i, ret;
 
 	BUG_ON(bytes % 2);
 	BUG_ON(bytes <= 0);
@@ -245,11 +558,10 @@ static int wm831x_write(struct wm831x *wm831x, unsigned short reg,
 
 		dev_vdbg(wm831x->dev, "Write %04x to R%d(0x%x)\n",
 			 buf[i], reg + i, reg + i);
-
-		buf[i] = cpu_to_be16(buf[i]);
+		ret = regmap_write(wm831x->regmap, reg + i, buf[i]);
 	}
 
-	return wm831x->write_dev(wm831x, reg, bytes, src);
+	return 0;
 }
 
 /**
@@ -286,20 +598,14 @@ int wm831x_set_bits(struct wm831x *wm831x, unsigned short reg,
 		    unsigned short mask, unsigned short val)
 {
 	int ret;
-	u16 r;
 
 	mutex_lock(&wm831x->io_lock);
 
-	ret = wm831x_read(wm831x, reg, 2, &r);
-	if (ret < 0)
-		goto out;
-
-	r &= ~mask;
-	r |= val & mask;
-
-	ret = wm831x_write(wm831x, reg, 2, &r);
+	if (!wm831x_reg_locked(wm831x, reg))
+		ret = regmap_update_bits(wm831x->regmap, reg, mask, val);
+	else
+		ret = -EPERM;
 
-out:
 	mutex_unlock(&wm831x->io_lock);
 
 	return ret;
@@ -1292,6 +1598,19 @@ static struct mfd_cell backlight_devs[] = {
 	},
 };
 
+struct regmap_config wm831x_regmap_config = {
+	.reg_bits = 16,
+	.val_bits = 16,
+
+	.cache_type = REGCACHE_RBTREE,
+
+	.max_register = WM831X_DBE_CHECK_DATA,
+	.readable_reg = wm831x_reg_readable,
+	.writeable_reg = wm831x_reg_writeable,
+	.volatile_reg = wm831x_reg_volatile,
+};
+EXPORT_SYMBOL_GPL(wm831x_regmap_config);
+
 /*
  * Instantiate the generic non-control parts of the device.
  */
@@ -1305,11 +1624,12 @@ int wm831x_device_init(struct wm831x *wm831x, unsigned long id, int irq)
 	mutex_init(&wm831x->io_lock);
 	mutex_init(&wm831x->key_lock);
 	dev_set_drvdata(wm831x->dev, wm831x);
+	wm831x->soft_shutdown = pdata->soft_shutdown;
 
 	ret = wm831x_reg_read(wm831x, WM831X_PARENT_ID);
 	if (ret < 0) {
 		dev_err(wm831x->dev, "Failed to read parent ID: %d\n", ret);
-		goto err;
+		goto err_regmap;
 	}
 	switch (ret) {
 	case 0x6204:
@@ -1318,20 +1638,20 @@ int wm831x_device_init(struct wm831x *wm831x, unsigned long id, int irq)
 	default:
 		dev_err(wm831x->dev, "Device is not a WM831x: ID %x\n", ret);
 		ret = -EINVAL;
-		goto err;
+		goto err_regmap;
 	}
 
 	ret = wm831x_reg_read(wm831x, WM831X_REVISION);
 	if (ret < 0) {
 		dev_err(wm831x->dev, "Failed to read revision: %d\n", ret);
-		goto err;
+		goto err_regmap;
 	}
 	rev = (ret & WM831X_PARENT_REV_MASK) >> WM831X_PARENT_REV_SHIFT;
 
 	ret = wm831x_reg_read(wm831x, WM831X_RESET_ID);
 	if (ret < 0) {
 		dev_err(wm831x->dev, "Failed to read device ID: %d\n", ret);
-		goto err;
+		goto err_regmap;
 	}
 
 	/* Some engineering samples do not have the ID set, rely on
@@ -1406,7 +1726,7 @@ int wm831x_device_init(struct wm831x *wm831x, unsigned long id, int irq)
 	default:
 		dev_err(wm831x->dev, "Unknown WM831x device %04x\n", ret);
 		ret = -EINVAL;
-		goto err;
+		goto err_regmap;
 	}
 
 	/* This will need revisiting in future but is OK for all
@@ -1420,7 +1740,7 @@ int wm831x_device_init(struct wm831x *wm831x, unsigned long id, int irq)
 	ret = wm831x_reg_read(wm831x, WM831X_SECURITY_KEY);
 	if (ret < 0) {
 		dev_err(wm831x->dev, "Failed to read security key: %d\n", ret);
-		goto err;
+		goto err_regmap;
 	}
 	if (ret != 0) {
 		dev_warn(wm831x->dev, "Security key had non-zero value %x\n",
@@ -1433,7 +1753,7 @@ int wm831x_device_init(struct wm831x *wm831x, unsigned long id, int irq)
 		ret = pdata->pre_init(wm831x);
 		if (ret != 0) {
 			dev_err(wm831x->dev, "pre_init() failed: %d\n", ret);
-			goto err;
+			goto err_regmap;
 		}
 	}
 
@@ -1456,7 +1776,7 @@ int wm831x_device_init(struct wm831x *wm831x, unsigned long id, int irq)
 
 	ret = wm831x_irq_init(wm831x, irq);
 	if (ret != 0)
-		goto err;
+		goto err_regmap;
 
 	wm831x_auxadc_init(wm831x);
 
@@ -1552,8 +1872,9 @@ int wm831x_device_init(struct wm831x *wm831x, unsigned long id, int irq)
 
 err_irq:
 	wm831x_irq_exit(wm831x);
-err:
+err_regmap:
 	mfd_remove_devices(wm831x->dev);
+	regmap_exit(wm831x->regmap);
 	kfree(wm831x);
 	return ret;
 }
@@ -1565,6 +1886,7 @@ void wm831x_device_exit(struct wm831x *wm831x)
 	if (wm831x->irq_base)
 		free_irq(wm831x->irq_base + WM831X_IRQ_AUXADC_DATA, wm831x);
 	wm831x_irq_exit(wm831x);
+	regmap_exit(wm831x->regmap);
 	kfree(wm831x);
 }
 
@@ -1604,6 +1926,15 @@ int wm831x_device_suspend(struct wm831x *wm831x)
 	return 0;
 }
 
+void wm831x_device_shutdown(struct wm831x *wm831x)
+{
+	if (wm831x->soft_shutdown) {
+		dev_info(wm831x->dev, "Initiating shutdown...\n");
+		wm831x_set_bits(wm831x, WM831X_POWER_STATE, WM831X_CHIP_ON, 0);
+	}
+}
+EXPORT_SYMBOL_GPL(wm831x_device_shutdown);
+
 MODULE_DESCRIPTION("Core support for the WM831X AudioPlus PMIC");
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Mark Brown");
diff --git a/drivers/mfd/wm831x-i2c.c b/drivers/mfd/wm831x-i2c.c
index a06cbc739716..ac8da1d439da 100644
--- a/drivers/mfd/wm831x-i2c.c
+++ b/drivers/mfd/wm831x-i2c.c
@@ -18,67 +18,17 @@
 #include <linux/delay.h>
 #include <linux/mfd/core.h>
 #include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/regmap.h>
 
 #include <linux/mfd/wm831x/core.h>
 #include <linux/mfd/wm831x/pdata.h>
 
-static int wm831x_i2c_read_device(struct wm831x *wm831x, unsigned short reg,
-				  int bytes, void *dest)
-{
-	struct i2c_client *i2c = wm831x->control_data;
-	int ret;
-	u16 r = cpu_to_be16(reg);
-
-	ret = i2c_master_send(i2c, (unsigned char *)&r, 2);
-	if (ret < 0)
-		return ret;
-	if (ret != 2)
-		return -EIO;
-
-	ret = i2c_master_recv(i2c, dest, bytes);
-	if (ret < 0)
-		return ret;
-	if (ret != bytes)
-		return -EIO;
-	return 0;
-}
-
-/* Currently we allocate the write buffer on the stack; this is OK for
- * small writes - if we need to do large writes this will need to be
- * revised.
- */
-static int wm831x_i2c_write_device(struct wm831x *wm831x, unsigned short reg,
-				   int bytes, void *src)
-{
-	struct i2c_client *i2c = wm831x->control_data;
-	struct i2c_msg xfer[2];
-	int ret;
-
-	reg = cpu_to_be16(reg);
-
-	xfer[0].addr = i2c->addr;
-	xfer[0].flags = 0;
-	xfer[0].len = 2;
-	xfer[0].buf = (char *)&reg;
-
-	xfer[1].addr = i2c->addr;
-	xfer[1].flags = I2C_M_NOSTART;
-	xfer[1].len = bytes;
-	xfer[1].buf = (char *)src;
-
-	ret = i2c_transfer(i2c->adapter, xfer, 2);
-	if (ret < 0)
-		return ret;
-	if (ret != 2)
-		return -EIO;
-
-	return 0;
-}
-
 static int wm831x_i2c_probe(struct i2c_client *i2c,
 			    const struct i2c_device_id *id)
 {
 	struct wm831x *wm831x;
+	int ret;
 
 	wm831x = kzalloc(sizeof(struct wm831x), GFP_KERNEL);
 	if (wm831x == NULL)
@@ -86,9 +36,15 @@ static int wm831x_i2c_probe(struct i2c_client *i2c,
 
 	i2c_set_clientdata(i2c, wm831x);
 	wm831x->dev = &i2c->dev;
-	wm831x->control_data = i2c;
-	wm831x->read_dev = wm831x_i2c_read_device;
-	wm831x->write_dev = wm831x_i2c_write_device;
+
+	wm831x->regmap = regmap_init_i2c(i2c, &wm831x_regmap_config);
+	if (IS_ERR(wm831x->regmap)) {
+		ret = PTR_ERR(wm831x->regmap);
+		dev_err(wm831x->dev, "Failed to allocate register map: %d\n",
+			ret);
+		kfree(wm831x);
+		return ret;
+	}
 
 	return wm831x_device_init(wm831x, id->driver_data, i2c->irq);
 }
@@ -109,6 +65,13 @@ static int wm831x_i2c_suspend(struct device *dev)
 	return wm831x_device_suspend(wm831x);
 }
 
+static void wm831x_i2c_shutdown(struct i2c_client *i2c)
+{
+	struct wm831x *wm831x = i2c_get_clientdata(i2c);
+
+	wm831x_device_shutdown(wm831x);
+}
+
 static const struct i2c_device_id wm831x_i2c_id[] = {
 	{ "wm8310", WM8310 },
 	{ "wm8311", WM8311 },
@@ -133,6 +96,7 @@ static struct i2c_driver wm831x_i2c_driver = {
 	},
 	.probe = wm831x_i2c_probe,
 	.remove = wm831x_i2c_remove,
+	.shutdown = wm831x_i2c_shutdown,
 	.id_table = wm831x_i2c_id,
 };
 
diff --git a/drivers/mfd/wm831x-spi.c b/drivers/mfd/wm831x-spi.c
index eed8e4f7a5a1..8d6a9a969dbc 100644
--- a/drivers/mfd/wm831x-spi.c
+++ b/drivers/mfd/wm831x-spi.c
@@ -16,78 +16,19 @@
 #include <linux/module.h>
 #include <linux/pm.h>
 #include <linux/spi/spi.h>
+#include <linux/regmap.h>
+#include <linux/err.h>
 
 #include <linux/mfd/wm831x/core.h>
 
-static int wm831x_spi_read_device(struct wm831x *wm831x, unsigned short reg,
-				  int bytes, void *dest)
-{
-	u16 tx_val;
-	u16 *d = dest;
-	int r, ret;
-
-	/* Go register at a time */
-	for (r = reg; r < reg + (bytes / 2); r++) {
-		tx_val = r | 0x8000;
-
-		ret = spi_write_then_read(wm831x->control_data,
-					  (u8 *)&tx_val, 2, (u8 *)d, 2);
-		if (ret != 0)
-			return ret;
-
-		*d = be16_to_cpu(*d);
-
-		d++;
-	}
-
-	return 0;
-}
-
-static int wm831x_spi_write_device(struct wm831x *wm831x, unsigned short reg,
-				   int bytes, void *src)
-{
-	struct spi_device *spi = wm831x->control_data;
-	u16 *s = src;
-	u16 data[2];
-	int ret, r;
-
-	/* Go register at a time */
-	for (r = reg; r < reg + (bytes / 2); r++) {
-		data[0] = r;
-		data[1] = *s++;
-
-		ret = spi_write(spi, (char *)&data, sizeof(data));
-		if (ret != 0)
-			return ret;
-	}
-
-	return 0;
-}
-
 static int __devinit wm831x_spi_probe(struct spi_device *spi)
 {
+	const struct spi_device_id *id = spi_get_device_id(spi);
 	struct wm831x *wm831x;
 	enum wm831x_parent type;
+	int ret;
 
-	/* Currently SPI support for ID tables is unmerged, we're faking it */
-	if (strcmp(spi->modalias, "wm8310") == 0)
-		type = WM8310;
-	else if (strcmp(spi->modalias, "wm8311") == 0)
-		type = WM8311;
-	else if (strcmp(spi->modalias, "wm8312") == 0)
-		type = WM8312;
-	else if (strcmp(spi->modalias, "wm8320") == 0)
-		type = WM8320;
-	else if (strcmp(spi->modalias, "wm8321") == 0)
-		type = WM8321;
-	else if (strcmp(spi->modalias, "wm8325") == 0)
-		type = WM8325;
-	else if (strcmp(spi->modalias, "wm8326") == 0)
-		type = WM8326;
-	else {
-		dev_err(&spi->dev, "Unknown device type\n");
-		return -EINVAL;
-	}
+	type = (enum wm831x_parent)id->driver_data;
 
 	wm831x = kzalloc(sizeof(struct wm831x), GFP_KERNEL);
 	if (wm831x == NULL)
@@ -98,9 +39,15 @@ static int __devinit wm831x_spi_probe(struct spi_device *spi)
 
 	dev_set_drvdata(&spi->dev, wm831x);
 	wm831x->dev = &spi->dev;
-	wm831x->control_data = spi;
-	wm831x->read_dev = wm831x_spi_read_device;
-	wm831x->write_dev = wm831x_spi_write_device;
+
+	wm831x->regmap = regmap_init_spi(spi, &wm831x_regmap_config);
+	if (IS_ERR(wm831x->regmap)) {
+		ret = PTR_ERR(wm831x->regmap);
+		dev_err(wm831x->dev, "Failed to allocate register map: %d\n",
+			ret);
+		kfree(wm831x);
+		return ret;
+	}
 
 	return wm831x_device_init(wm831x, type, spi->irq);
 }
@@ -121,119 +68,50 @@ static int wm831x_spi_suspend(struct device *dev)
 	return wm831x_device_suspend(wm831x);
 }
 
+static void wm831x_spi_shutdown(struct spi_device *spi)
+{
+	struct wm831x *wm831x = dev_get_drvdata(&spi->dev);
+
+	wm831x_device_shutdown(wm831x);
+}
+
 static const struct dev_pm_ops wm831x_spi_pm = {
 	.freeze = wm831x_spi_suspend,
 	.suspend = wm831x_spi_suspend,
 };
 
-static struct spi_driver wm8310_spi_driver = {
-	.driver = {
-		.name	= "wm8310",
-		.bus	= &spi_bus_type,
-		.owner	= THIS_MODULE,
-		.pm	= &wm831x_spi_pm,
-	},
-	.probe		= wm831x_spi_probe,
-	.remove		= __devexit_p(wm831x_spi_remove),
+static const struct spi_device_id wm831x_spi_ids[] = {
+	{ "wm8310", WM8310 },
+	{ "wm8311", WM8311 },
+	{ "wm8312", WM8312 },
+	{ "wm8320", WM8320 },
+	{ "wm8321", WM8321 },
+	{ "wm8325", WM8325 },
+	{ "wm8326", WM8326 },
+	{ },
 };
+MODULE_DEVICE_TABLE(spi, wm831x_spi_id);
 
-static struct spi_driver wm8311_spi_driver = {
+static struct spi_driver wm831x_spi_driver = {
 	.driver = {
-		.name	= "wm8311",
-		.bus	= &spi_bus_type,
-		.owner	= THIS_MODULE,
-		.pm	= &wm831x_spi_pm,
-	},
-	.probe		= wm831x_spi_probe,
-	.remove		= __devexit_p(wm831x_spi_remove),
-};
-
-static struct spi_driver wm8312_spi_driver = {
-	.driver = {
-		.name	= "wm8312",
-		.bus	= &spi_bus_type,
-		.owner	= THIS_MODULE,
-		.pm	= &wm831x_spi_pm,
-	},
-	.probe		= wm831x_spi_probe,
-	.remove		= __devexit_p(wm831x_spi_remove),
-};
-
-static struct spi_driver wm8320_spi_driver = {
-	.driver = {
-		.name	= "wm8320",
-		.bus	= &spi_bus_type,
-		.owner	= THIS_MODULE,
-		.pm	= &wm831x_spi_pm,
-	},
-	.probe		= wm831x_spi_probe,
-	.remove		= __devexit_p(wm831x_spi_remove),
-};
-
-static struct spi_driver wm8321_spi_driver = {
-	.driver = {
-		.name	= "wm8321",
-		.bus	= &spi_bus_type,
-		.owner	= THIS_MODULE,
-		.pm	= &wm831x_spi_pm,
-	},
-	.probe		= wm831x_spi_probe,
-	.remove		= __devexit_p(wm831x_spi_remove),
-};
-
-static struct spi_driver wm8325_spi_driver = {
-	.driver = {
-		.name	= "wm8325",
-		.bus	= &spi_bus_type,
-		.owner	= THIS_MODULE,
-		.pm	= &wm831x_spi_pm,
-	},
-	.probe		= wm831x_spi_probe,
-	.remove		= __devexit_p(wm831x_spi_remove),
-};
-
-static struct spi_driver wm8326_spi_driver = {
-	.driver = {
-		.name	= "wm8326",
+		.name	= "wm831x",
 		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 		.pm	= &wm831x_spi_pm,
 	},
+	.id_table	= wm831x_spi_ids,
 	.probe		= wm831x_spi_probe,
 	.remove		= __devexit_p(wm831x_spi_remove),
+	.shutdown	= wm831x_spi_shutdown,
 };
 
 static int __init wm831x_spi_init(void)
 {
 	int ret;
 
-	ret = spi_register_driver(&wm8310_spi_driver);
-	if (ret != 0)
-		pr_err("Failed to register WM8310 SPI driver: %d\n", ret);
-
-	ret = spi_register_driver(&wm8311_spi_driver);
-	if (ret != 0)
-		pr_err("Failed to register WM8311 SPI driver: %d\n", ret);
-
-	ret = spi_register_driver(&wm8312_spi_driver);
-	if (ret != 0)
-		pr_err("Failed to register WM8312 SPI driver: %d\n", ret);
-
-	ret = spi_register_driver(&wm8320_spi_driver);
-	if (ret != 0)
-		pr_err("Failed to register WM8320 SPI driver: %d\n", ret);
-
-	ret = spi_register_driver(&wm8321_spi_driver);
-	if (ret != 0)
-		pr_err("Failed to register WM8321 SPI driver: %d\n", ret);
-
-	ret = spi_register_driver(&wm8325_spi_driver);
-	if (ret != 0)
-		pr_err("Failed to register WM8325 SPI driver: %d\n", ret);
-
-	ret = spi_register_driver(&wm8326_spi_driver);
+	ret = spi_register_driver(&wm831x_spi_driver);
 	if (ret != 0)
-		pr_err("Failed to register WM8326 SPI driver: %d\n", ret);
+		pr_err("Failed to register WM831x SPI driver: %d\n", ret);
 
 	return 0;
 }
@@ -241,13 +119,7 @@ subsys_initcall(wm831x_spi_init);
 
 static void __exit wm831x_spi_exit(void)
 {
-	spi_unregister_driver(&wm8326_spi_driver);
-	spi_unregister_driver(&wm8325_spi_driver);
-	spi_unregister_driver(&wm8321_spi_driver);
-	spi_unregister_driver(&wm8320_spi_driver);
-	spi_unregister_driver(&wm8312_spi_driver);
-	spi_unregister_driver(&wm8311_spi_driver);
-	spi_unregister_driver(&wm8310_spi_driver);
+	spi_unregister_driver(&wm831x_spi_driver);
 }
 module_exit(wm831x_spi_exit);
 
diff --git a/drivers/mfd/wm8400-core.c b/drivers/mfd/wm8400-core.c
index 597f82edacaa..e06ba9440cdb 100644
--- a/drivers/mfd/wm8400-core.c
+++ b/drivers/mfd/wm8400-core.c
@@ -13,11 +13,13 @@
  */
 
 #include <linux/bug.h>
+#include <linux/err.h>
 #include <linux/i2c.h>
 #include <linux/kernel.h>
 #include <linux/mfd/core.h>
 #include <linux/mfd/wm8400-private.h>
 #include <linux/mfd/wm8400-audio.h>
+#include <linux/regmap.h>
 #include <linux/slab.h>
 
 static struct {
@@ -123,14 +125,9 @@ static int wm8400_read(struct wm8400 *wm8400, u8 reg, int num_regs, u16 *dest)
 	/* If there are any volatile reads then read back the entire block */
 	for (i = reg; i < reg + num_regs; i++)
 		if (reg_data[i].vol) {
-			ret = wm8400->read_dev(wm8400->io_data, reg,
-					       num_regs, dest);
-			if (ret != 0)
-				return ret;
-			for (i = 0; i < num_regs; i++)
-				dest[i] = be16_to_cpu(dest[i]);
-
-			return 0;
+			ret = regmap_bulk_read(wm8400->regmap, reg, dest,
+					       num_regs);
+			return ret;
 		}
 
 	/* Otherwise use the cache */
@@ -149,14 +146,11 @@ static int wm8400_write(struct wm8400 *wm8400, u8 reg, int num_regs,
 	for (i = 0; i < num_regs; i++) {
 		BUG_ON(!reg_data[reg + i].writable);
 		wm8400->reg_cache[reg + i] = src[i];
-		src[i] = cpu_to_be16(src[i]);
+		ret = regmap_write(wm8400->regmap, reg, src[i]);
+		if (ret != 0)
+			return ret;
 	}
 
-	/* Do the actual I/O */
-	ret = wm8400->write_dev(wm8400->io_data, reg, num_regs, src);
-	if (ret != 0)
-		return -EIO;
-
 	return 0;
 }
 
@@ -270,14 +264,14 @@ static int wm8400_init(struct wm8400 *wm8400,
 	dev_set_drvdata(wm8400->dev, wm8400);
 
 	/* Check that this is actually a WM8400 */
-	ret = wm8400->read_dev(wm8400->io_data, WM8400_RESET_ID, 1, &reg);
+	ret = regmap_read(wm8400->regmap, WM8400_RESET_ID, &i);
 	if (ret != 0) {
 		dev_err(wm8400->dev, "Chip ID register read failed\n");
 		return -EIO;
 	}
-	if (be16_to_cpu(reg) != reg_data[WM8400_RESET_ID].default_val) {
+	if (i != reg_data[WM8400_RESET_ID].default_val) {
 		dev_err(wm8400->dev, "Device is not a WM8400, ID is %x\n",
-			be16_to_cpu(reg));
+			reg);
 		return -ENODEV;
 	}
 
@@ -285,9 +279,8 @@ static int wm8400_init(struct wm8400 *wm8400,
 	 * is a PMIC we can't reset it safely so initialise the register
 	 * cache from the hardware.
 	 */
-	ret = wm8400->read_dev(wm8400->io_data, 0,
-			       ARRAY_SIZE(wm8400->reg_cache),
-			       wm8400->reg_cache);
+	ret = regmap_raw_read(wm8400->regmap, 0, wm8400->reg_cache,
+			      ARRAY_SIZE(wm8400->reg_cache));
 	if (ret != 0) {
 		dev_err(wm8400->dev, "Register cache read failed\n");
 		return -EIO;
@@ -337,60 +330,13 @@ static void wm8400_release(struct wm8400 *wm8400)
 	mfd_remove_devices(wm8400->dev);
 }
 
-#if defined(CONFIG_I2C) || defined(CONFIG_I2C_MODULE)
-static int wm8400_i2c_read(void *io_data, char reg, int count, u16 *dest)
-{
-	struct i2c_client *i2c = io_data;
-	struct i2c_msg xfer[2];
-	int ret;
-
-	/* Write register */
-	xfer[0].addr = i2c->addr;
-	xfer[0].flags = 0;
-	xfer[0].len = 1;
-	xfer[0].buf = &reg;
-
-	/* Read data */
-	xfer[1].addr = i2c->addr;
-	xfer[1].flags = I2C_M_RD;
-	xfer[1].len = count * sizeof(u16);
-	xfer[1].buf = (u8 *)dest;
-
-	ret = i2c_transfer(i2c->adapter, xfer, 2);
-	if (ret == 2)
-		ret = 0;
-	else if (ret >= 0)
-		ret = -EIO;
-
-	return ret;
-}
-
-static int wm8400_i2c_write(void *io_data, char reg, int count, const u16 *src)
-{
-	struct i2c_client *i2c = io_data;
-	u8 *msg;
-	int ret;
-
-	/* We add 1 byte for device register - ideally I2C would gather. */
-	msg = kmalloc((count * sizeof(u16)) + 1, GFP_KERNEL);
-	if (msg == NULL)
-		return -ENOMEM;
-
-	msg[0] = reg;
-	memcpy(&msg[1], src, count * sizeof(u16));
-
-	ret = i2c_master_send(i2c, msg, (count * sizeof(u16)) + 1);
-
-	if (ret == (count * 2) + 1)
-		ret = 0;
-	else if (ret >= 0)
-		ret = -EIO;
-
-	kfree(msg);
-
-	return ret;
-}
+static const struct regmap_config wm8400_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 16,
+	.max_register = WM8400_REGISTER_COUNT - 1,
+};
 
+#if defined(CONFIG_I2C) || defined(CONFIG_I2C_MODULE)
 static int wm8400_i2c_probe(struct i2c_client *i2c,
 			    const struct i2c_device_id *id)
 {
@@ -403,18 +349,23 @@ static int wm8400_i2c_probe(struct i2c_client *i2c,
 		goto err;
 	}
 
-	wm8400->io_data = i2c;
-	wm8400->read_dev = wm8400_i2c_read;
-	wm8400->write_dev = wm8400_i2c_write;
+	wm8400->regmap = regmap_init_i2c(i2c, &wm8400_regmap_config);
+	if (IS_ERR(wm8400->regmap)) {
+		ret = PTR_ERR(wm8400->regmap);
+		goto struct_err;
+	}
+
 	wm8400->dev = &i2c->dev;
 	i2c_set_clientdata(i2c, wm8400);
 
 	ret = wm8400_init(wm8400, i2c->dev.platform_data);
 	if (ret != 0)
-		goto struct_err;
+		goto map_err;
 
 	return 0;
 
+map_err:
+	regmap_exit(wm8400->regmap);
 struct_err:
 	kfree(wm8400);
 err:
@@ -426,6 +377,7 @@ static int wm8400_i2c_remove(struct i2c_client *i2c)
 	struct wm8400 *wm8400 = i2c_get_clientdata(i2c);
 
 	wm8400_release(wm8400);
+	regmap_exit(wm8400->regmap);
 	kfree(wm8400);
 
 	return 0;
diff --git a/drivers/mfd/wm8994-core.c b/drivers/mfd/wm8994-core.c
index 96479c9b1728..bfde4e8ec638 100644
--- a/drivers/mfd/wm8994-core.c
+++ b/drivers/mfd/wm8994-core.c
@@ -16,9 +16,11 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
+#include <linux/err.h>
 #include <linux/delay.h>
 #include <linux/mfd/core.h>
 #include <linux/pm_runtime.h>
+#include <linux/regmap.h>
 #include <linux/regulator/consumer.h>
 #include <linux/regulator/machine.h>
 
@@ -29,22 +31,7 @@
 static int wm8994_read(struct wm8994 *wm8994, unsigned short reg,
 		       int bytes, void *dest)
 {
-	int ret, i;
-	u16 *buf = dest;
-
-	BUG_ON(bytes % 2);
-	BUG_ON(bytes <= 0);
-
-	ret = wm8994->read_dev(wm8994, reg, bytes, dest);
-	if (ret < 0)
-		return ret;
-
-	for (i = 0; i < bytes / 2; i++) {
-		dev_vdbg(wm8994->dev, "Read %04x from R%d(0x%x)\n",
-			 be16_to_cpu(buf[i]), reg + i, reg + i);
-	}
-
-	return 0;
+	return regmap_raw_read(wm8994->regmap, reg, dest, bytes);
 }
 
 /**
@@ -55,19 +42,15 @@ static int wm8994_read(struct wm8994 *wm8994, unsigned short reg,
  */
 int wm8994_reg_read(struct wm8994 *wm8994, unsigned short reg)
 {
-	unsigned short val;
+	unsigned int val;
 	int ret;
 
-	mutex_lock(&wm8994->io_lock);
-
-	ret = wm8994_read(wm8994, reg, 2, &val);
-
-	mutex_unlock(&wm8994->io_lock);
+	ret = regmap_read(wm8994->regmap, reg, &val);
 
 	if (ret < 0)
 		return ret;
 	else
-		return be16_to_cpu(val);
+		return val;
 }
 EXPORT_SYMBOL_GPL(wm8994_reg_read);
 
@@ -82,33 +65,13 @@ EXPORT_SYMBOL_GPL(wm8994_reg_read);
 int wm8994_bulk_read(struct wm8994 *wm8994, unsigned short reg,
 		     int count, u16 *buf)
 {
-	int ret;
-
-	mutex_lock(&wm8994->io_lock);
-
-	ret = wm8994_read(wm8994, reg, count * 2, buf);
-
-	mutex_unlock(&wm8994->io_lock);
-
-	return ret;
+	return regmap_bulk_read(wm8994->regmap, reg, buf, count);
 }
-EXPORT_SYMBOL_GPL(wm8994_bulk_read);
 
 static int wm8994_write(struct wm8994 *wm8994, unsigned short reg,
 			int bytes, const void *src)
 {
-	const u16 *buf = src;
-	int i;
-
-	BUG_ON(bytes % 2);
-	BUG_ON(bytes <= 0);
-
-	for (i = 0; i < bytes / 2; i++) {
-		dev_vdbg(wm8994->dev, "Write %04x to R%d(0x%x)\n",
-			 be16_to_cpu(buf[i]), reg + i, reg + i);
-	}
-
-	return wm8994->write_dev(wm8994, reg, bytes, src);
+	return regmap_raw_write(wm8994->regmap, reg, src, bytes);
 }
 
 /**
@@ -121,17 +84,7 @@ static int wm8994_write(struct wm8994 *wm8994, unsigned short reg,
 int wm8994_reg_write(struct wm8994 *wm8994, unsigned short reg,
 		     unsigned short val)
 {
-	int ret;
-
-	val = cpu_to_be16(val);
-
-	mutex_lock(&wm8994->io_lock);
-
-	ret = wm8994_write(wm8994, reg, 2, &val);
-
-	mutex_unlock(&wm8994->io_lock);
-
-	return ret;
+	return regmap_write(wm8994->regmap, reg, val);
 }
 EXPORT_SYMBOL_GPL(wm8994_reg_write);
 
@@ -146,15 +99,7 @@ EXPORT_SYMBOL_GPL(wm8994_reg_write);
 int wm8994_bulk_write(struct wm8994 *wm8994, unsigned short reg,
 		      int count, const u16 *buf)
 {
-	int ret;
-
-	mutex_lock(&wm8994->io_lock);
-
-	ret = wm8994_write(wm8994, reg, count * 2, buf);
-
-	mutex_unlock(&wm8994->io_lock);
-
-	return ret;
+	return regmap_raw_write(wm8994->regmap, reg, buf, count * sizeof(u16));
 }
 EXPORT_SYMBOL_GPL(wm8994_bulk_write);
 
@@ -169,28 +114,7 @@ EXPORT_SYMBOL_GPL(wm8994_bulk_write);
 int wm8994_set_bits(struct wm8994 *wm8994, unsigned short reg,
 		    unsigned short mask, unsigned short val)
 {
-	int ret;
-	u16 r;
-
-	mutex_lock(&wm8994->io_lock);
-
-	ret = wm8994_read(wm8994, reg, 2, &r);
-	if (ret < 0)
-		goto out;
-
-	r = be16_to_cpu(r);
-
-	r &= ~mask;
-	r |= val;
-
-	r = cpu_to_be16(r);
-
-	ret = wm8994_write(wm8994, reg, 2, &r);
-
-out:
-	mutex_unlock(&wm8994->io_lock);
-
-	return ret;
+	return regmap_update_bits(wm8994->regmap, reg, mask, val);
 }
 EXPORT_SYMBOL_GPL(wm8994_set_bits);
 
@@ -378,6 +302,11 @@ static int wm8994_ldo_in_use(struct wm8994_pdata *pdata, int ldo)
 }
 #endif
 
+static struct regmap_config wm8994_regmap_config = {
+	.reg_bits = 16,
+	.val_bits = 16,
+};
+
 /*
  * Instantiate the generic non-control parts of the device.
  */
@@ -387,7 +316,6 @@ static int wm8994_device_init(struct wm8994 *wm8994, int irq)
 	const char *devname;
 	int ret, i;
 
-	mutex_init(&wm8994->io_lock);
 	dev_set_drvdata(wm8994->dev, wm8994);
 
 	/* Add the on-chip regulators first for bootstrapping */
@@ -397,7 +325,7 @@ static int wm8994_device_init(struct wm8994 *wm8994, int irq)
 			      NULL, 0);
 	if (ret != 0) {
 		dev_err(wm8994->dev, "Failed to add children: %d\n", ret);
-		goto err;
+		goto err_regmap;
 	}
 
 	switch (wm8994->type) {
@@ -409,7 +337,7 @@ static int wm8994_device_init(struct wm8994 *wm8994, int irq)
 		break;
 	default:
 		BUG();
-		goto err;
+		goto err_regmap;
 	}
 
 	wm8994->supplies = kzalloc(sizeof(struct regulator_bulk_data) *
@@ -417,7 +345,7 @@ static int wm8994_device_init(struct wm8994 *wm8994, int irq)
 				   GFP_KERNEL);
 	if (!wm8994->supplies) {
 		ret = -ENOMEM;
-		goto err;
+		goto err_regmap;
 	}
 
 	switch (wm8994->type) {
@@ -431,7 +359,7 @@ static int wm8994_device_init(struct wm8994 *wm8994, int irq)
 		break;
 	default:
 		BUG();
-		goto err;
+		goto err_regmap;
 	}
 		
 	ret = regulator_bulk_get(wm8994->dev, wm8994->num_supplies,
@@ -554,7 +482,8 @@ err_get:
 	regulator_bulk_free(wm8994->num_supplies, wm8994->supplies);
 err_supplies:
 	kfree(wm8994->supplies);
-err:
+err_regmap:
+	regmap_exit(wm8994->regmap);
 	mfd_remove_devices(wm8994->dev);
 	kfree(wm8994);
 	return ret;
@@ -569,62 +498,15 @@ static void wm8994_device_exit(struct wm8994 *wm8994)
 			       wm8994->supplies);
 	regulator_bulk_free(wm8994->num_supplies, wm8994->supplies);
 	kfree(wm8994->supplies);
+	regmap_exit(wm8994->regmap);
 	kfree(wm8994);
 }
 
-static int wm8994_i2c_read_device(struct wm8994 *wm8994, unsigned short reg,
-				  int bytes, void *dest)
-{
-	struct i2c_client *i2c = wm8994->control_data;
-	int ret;
-	u16 r = cpu_to_be16(reg);
-
-	ret = i2c_master_send(i2c, (unsigned char *)&r, 2);
-	if (ret < 0)
-		return ret;
-	if (ret != 2)
-		return -EIO;
-
-	ret = i2c_master_recv(i2c, dest, bytes);
-	if (ret < 0)
-		return ret;
-	if (ret != bytes)
-		return -EIO;
-	return 0;
-}
-
-static int wm8994_i2c_write_device(struct wm8994 *wm8994, unsigned short reg,
-				   int bytes, const void *src)
-{
-	struct i2c_client *i2c = wm8994->control_data;
-	struct i2c_msg xfer[2];
-	int ret;
-
-	reg = cpu_to_be16(reg);
-
-	xfer[0].addr = i2c->addr;
-	xfer[0].flags = 0;
-	xfer[0].len = 2;
-	xfer[0].buf = (char *)&reg;
-
-	xfer[1].addr = i2c->addr;
-	xfer[1].flags = I2C_M_NOSTART;
-	xfer[1].len = bytes;
-	xfer[1].buf = (char *)src;
-
-	ret = i2c_transfer(i2c->adapter, xfer, 2);
-	if (ret < 0)
-		return ret;
-	if (ret != 2)
-		return -EIO;
-
-	return 0;
-}
-
 static int wm8994_i2c_probe(struct i2c_client *i2c,
 			    const struct i2c_device_id *id)
 {
 	struct wm8994 *wm8994;
+	int ret;
 
 	wm8994 = kzalloc(sizeof(struct wm8994), GFP_KERNEL);
 	if (wm8994 == NULL)
@@ -632,12 +514,18 @@ static int wm8994_i2c_probe(struct i2c_client *i2c,
 
 	i2c_set_clientdata(i2c, wm8994);
 	wm8994->dev = &i2c->dev;
-	wm8994->control_data = i2c;
-	wm8994->read_dev = wm8994_i2c_read_device;
-	wm8994->write_dev = wm8994_i2c_write_device;
 	wm8994->irq = i2c->irq;
 	wm8994->type = id->driver_data;
 
+	wm8994->regmap = regmap_init_i2c(i2c, &wm8994_regmap_config);
+	if (IS_ERR(wm8994->regmap)) {
+		ret = PTR_ERR(wm8994->regmap);
+		dev_err(wm8994->dev, "Failed to allocate register map: %d\n",
+			ret);
+		kfree(wm8994);
+		return ret;
+	}
+
 	return wm8994_device_init(wm8994, i2c->irq);
 }
 
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 2d6423c2d193..0d6210e50e43 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -392,6 +392,22 @@ config HMC6352
 	  This driver provides support for the Honeywell HMC6352 compass,
 	  providing configuration and heading data via sysfs.
 
+config SENSORS_AK8975
+	tristate "AK8975 compass support"
+	default n
+	depends on I2C
+	help
+	  If you say yes here you get support for Asahi Kasei's
+	  orientation sensor AK8975.
+
+config SENSORS_NCT1008
+	tristate "ON Semiconductor Temperature Sensor"
+	default n
+	depends on I2C
+	help
+	 Say yes here if you wish to include the ON Semiconductor
+	 NCT1008 Temperature sensor.
+
 config EP93XX_PWM
 	tristate "EP93xx PWM support"
 	depends on ARCH_EP93XX
@@ -435,6 +451,10 @@ config TI_DAC7512
 	  This driver can also be built as a module. If so, the module
 	  will be called ti_dac7512.
 
+config UID_STAT
+	bool "UID based statistics tracking exported to /proc/uid_stat"
+	default n
+
 config VMWARE_BALLOON
 	tristate "VMware Balloon Driver"
 	depends on X86
@@ -472,7 +492,7 @@ config BMP085
 	  module will be called bmp085.
 
 config PCH_PHUB
-	tristate "Intel EG20T PCH / OKI SEMICONDUCTOR IOH(ML7213/ML7223) PHUB"
+	tristate "Intel EG20T PCH/LAPIS Semicon IOH(ML7213/ML7223/ML7831) PHUB"
 	depends on PCI
 	help
 	  This driver is for PCH(Platform controller Hub) PHUB(Packet Hub) of
@@ -480,12 +500,13 @@ config PCH_PHUB
 	  processor. The Topcliff has MAC address and Option ROM data in SROM.
 	  This driver can access MAC address and Option ROM data in SROM.
 
-	  This driver also can be used for OKI SEMICONDUCTOR IOH(Input/
-	  Output Hub), ML7213 and ML7223.
-	  ML7213 IOH is for IVI(In-Vehicle Infotainment) use and ML7223 IOH is
-	  for MP(Media Phone) use.
-	  ML7213/ML7223 is companion chip for Intel Atom E6xx series.
-	  ML7213/ML7223 is completely compatible for Intel EG20T PCH.
+	  This driver also can be used for LAPIS Semiconductor's IOH,
+	  ML7213/ML7223/ML7831.
+	  ML7213 which is for IVI(In-Vehicle Infotainment) use.
+	  ML7223 IOH is for MP(Media Phone) use.
+	  ML7831 IOH is for general purpose use.
+	  ML7213/ML7223/ML7831 is companion chip for Intel Atom E6xx series.
+	  ML7213/ML7223/ML7831 is completely compatible for Intel EG20T PCH.
 
 	  To compile this driver as a module, choose M here: the module will
 	  be called pch_phub.
@@ -499,6 +520,61 @@ config USB_SWITCH_FSA9480
 	  stereo and mono audio, video, microphone and UART data to use
 	  a common connector port.
 
+config WL127X_RFKILL
+	tristate "Bluetooth power control driver for TI wl127x"
+	depends on RFKILL
+	default n
+	---help---
+	 Creates an rfkill entry in sysfs for power control of Bluetooth
+	 TI wl127x chips.
+
+config APANIC
+	bool "Android kernel panic diagnostics driver"
+	default n
+	---help---
+	 Driver which handles kernel panics and attempts to write
+	 critical debugging data to flash.
+
+config APANIC_PLABEL
+	string "Android panic dump flash partition label"
+	depends on APANIC
+	default "kpanic"
+	---help---
+	 If your platform uses a different flash partition label for storing
+ 	 crashdumps, enter it here.
+
+config BCM4329_RFKILL
+	bool "Enable BCM4329 RFKILL driver"
+	default n
+	---help---
+	Adds BCM4329 RFKILL driver for Broadcom BCM4329 chipset
+
+config TEGRA_CRYPTO_DEV
+	bool "Device node to access tegra aes hardware"
+	---help---
+	Dev node /dev/tegra-crypto in order to get access to tegra aes
+	hardware from user space
+
+config MAX1749_VIBRATOR
+	bool "MAX1749 vibrator device driver"
+	depends on ANDROID_TIMED_OUTPUT
+	default n
+	---help---
+	Adds a timed output vibrator device node for MAX1749 vibrator motor
+
+config THERM_EST
+	bool "Thermal estimator driver"
+	default n
+	---help---
+	Thermal driver which estimates temperature based of other sensors.
+
+config TEGRA_THROUGHPUT
+	bool "Device node to set throughput target"
+	depends on TEGRA_DC && TEGRA_DC_EXTENSIONS
+	default y
+	---help---
+	Dev node /dev/tegra-throughput used to set a throughput target.
+
 source "drivers/misc/c2port/Kconfig"
 source "drivers/misc/eeprom/Kconfig"
 source "drivers/misc/cb710/Kconfig"
@@ -506,5 +582,8 @@ source "drivers/misc/iwmc3200top/Kconfig"
 source "drivers/misc/ti-st/Kconfig"
 source "drivers/misc/lis3lv02d/Kconfig"
 source "drivers/misc/carma/Kconfig"
+source "drivers/misc/inv_mpu/Kconfig"
+source "drivers/misc/tegra-baseband/Kconfig"
+source "drivers/misc/tegra-cec/Kconfig"
 
 endif # MISC_DEVICES
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 8f3efb68a141..0912d6d2ea53 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -1,6 +1,7 @@
 #
 # Makefile for misc devices that really don't fit anywhere else.
 #
+GCOV_PROFILE_tegra-cryptodev.o := y
 
 obj-$(CONFIG_IBM_ASM)		+= ibmasm/
 obj-$(CONFIG_AD525X_DPOT)	+= ad525x_dpot.o
@@ -13,8 +14,8 @@ obj-$(CONFIG_ATMEL_TCLIB)	+= atmel_tclib.o
 obj-$(CONFIG_BMP085)		+= bmp085.o
 obj-$(CONFIG_ICS932S401)	+= ics932s401.o
 obj-$(CONFIG_LKDTM)		+= lkdtm.o
-obj-$(CONFIG_TIFM_CORE)       	+= tifm_core.o
-obj-$(CONFIG_TIFM_7XX1)       	+= tifm_7xx1.o
+obj-$(CONFIG_TIFM_CORE)		+= tifm_core.o
+obj-$(CONFIG_TIFM_7XX1)		+= tifm_7xx1.o
 obj-$(CONFIG_PHANTOM)		+= phantom.o
 obj-$(CONFIG_SENSORS_BH1780)	+= bh1780gli.o
 obj-$(CONFIG_SENSORS_BH1770)	+= bh1770glc.o
@@ -33,6 +34,7 @@ obj-$(CONFIG_SENSORS_TSL2550)	+= tsl2550.o
 obj-$(CONFIG_EP93XX_PWM)	+= ep93xx_pwm.o
 obj-$(CONFIG_DS1682)		+= ds1682.o
 obj-$(CONFIG_TI_DAC7512)	+= ti_dac7512.o
+obj-$(CONFIG_UID_STAT)		+= uid_stat.o
 obj-$(CONFIG_C2PORT)		+= c2port/
 obj-$(CONFIG_IWMC3200TOP)      += iwmc3200top/
 obj-$(CONFIG_HMC6352)		+= hmc6352.o
@@ -47,3 +49,16 @@ obj-$(CONFIG_AB8500_PWM)	+= ab8500-pwm.o
 obj-y				+= lis3lv02d/
 obj-y				+= carma/
 obj-$(CONFIG_USB_SWITCH_FSA9480) += fsa9480.o
+obj-$(CONFIG_WL127X_RFKILL)	+= wl127x-rfkill.o
+obj-$(CONFIG_APANIC)		+= apanic.o
+obj-$(CONFIG_SENSORS_AK8975)	+= akm8975.o
+obj-$(CONFIG_SENSORS_NCT1008)	+= nct1008.o
+obj-$(CONFIG_BCM4329_RFKILL)	+= bcm4329_rfkill.o
+obj-$(CONFIG_INV_SENSORS)      += inv_mpu/
+CFLAGS_tegra-cryptodev.o	 = -Werror
+obj-$(CONFIG_TEGRA_CRYPTO_DEV)	+= tegra-cryptodev.o
+obj-$(CONFIG_TEGRA_BB_SUPPORT)	+= tegra-baseband/
+obj-$(CONFIG_TEGRA_CEC_SUPPORT)	+= tegra-cec/
+obj-$(CONFIG_MAX1749_VIBRATOR)	+= max1749.o
+obj-$(CONFIG_THERM_EST)		+= therm_est.o
+obj-$(CONFIG_TEGRA_THROUGHPUT)	+= tegra-throughput.o
diff --git a/drivers/misc/akm8975.c b/drivers/misc/akm8975.c
new file mode 100644
index 000000000000..aef7985d4ce4
--- /dev/null
+++ b/drivers/misc/akm8975.c
@@ -0,0 +1,732 @@
+/* drivers/misc/akm8975.c - akm8975 compass driver
+ *
+ * Copyright (C) 2007-2008 HTC Corporation.
+ * Author: Hou-Kun Chen <houkun.chen@gmail.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+/*
+ * Revised by AKM 2009/04/02
+ * Revised by Motorola 2010/05/27
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <linux/gpio.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/freezer.h>
+#include <linux/akm8975.h>
+#include <linux/earlysuspend.h>
+
+#define AK8975DRV_CALL_DBG 0
+#if AK8975DRV_CALL_DBG
+#define FUNCDBG(msg)	pr_err("%s:%s\n", __func__, msg);
+#else
+#define FUNCDBG(msg)
+#endif
+
+#define AK8975DRV_DATA_DBG 0
+#define MAX_FAILURE_COUNT 10
+
+struct akm8975_data {
+	struct i2c_client *this_client;
+	struct akm8975_platform_data *pdata;
+	struct input_dev *input_dev;
+	struct work_struct work;
+	struct mutex flags_lock;
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	struct early_suspend early_suspend;
+#endif
+};
+
+/*
+* Because misc devices can not carry a pointer from driver register to
+* open, we keep this global. This limits the driver to a single instance.
+*/
+struct akm8975_data *akmd_data;
+
+static DECLARE_WAIT_QUEUE_HEAD(open_wq);
+
+static atomic_t open_flag;
+
+static short m_flag;
+static short a_flag;
+static short t_flag;
+static short mv_flag;
+
+static short akmd_delay;
+
+static ssize_t akm8975_show(struct device *dev, struct device_attribute *attr,
+				 char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	return sprintf(buf, "%u\n", i2c_smbus_read_byte_data(client,
+							     AK8975_REG_CNTL));
+}
+static ssize_t akm8975_store(struct device *dev, struct device_attribute *attr,
+			    const char *buf, size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	unsigned long val;
+	strict_strtoul(buf, 10, &val);
+	if (val > 0xff)
+		return -EINVAL;
+	i2c_smbus_write_byte_data(client, AK8975_REG_CNTL, val);
+	return count;
+}
+static DEVICE_ATTR(akm_ms1, S_IWUSR | S_IRUGO, akm8975_show, akm8975_store);
+
+static int akm8975_i2c_rxdata(struct akm8975_data *akm, char *buf, int length)
+{
+	struct i2c_msg msgs[] = {
+		{
+			.addr = akm->this_client->addr,
+			.flags = 0,
+			.len = 1,
+			.buf = buf,
+		},
+		{
+			.addr = akm->this_client->addr,
+			.flags = I2C_M_RD,
+			.len = length,
+			.buf = buf,
+		},
+	};
+
+	FUNCDBG("called");
+
+	if (i2c_transfer(akm->this_client->adapter, msgs, 2) < 0) {
+		pr_err("akm8975_i2c_rxdata: transfer error\n");
+		return EIO;
+	} else
+		return 0;
+}
+
+static int akm8975_i2c_txdata(struct akm8975_data *akm, char *buf, int length)
+{
+	struct i2c_msg msgs[] = {
+		{
+			.addr = akm->this_client->addr,
+			.flags = 0,
+			.len = length,
+			.buf = buf,
+		},
+	};
+
+	FUNCDBG("called");
+
+	if (i2c_transfer(akm->this_client->adapter, msgs, 1) < 0) {
+		pr_err("akm8975_i2c_txdata: transfer error\n");
+		return -EIO;
+	} else
+		return 0;
+}
+
+static void akm8975_ecs_report_value(struct akm8975_data *akm, short *rbuf)
+{
+	struct akm8975_data *data = i2c_get_clientdata(akm->this_client);
+
+	FUNCDBG("called");
+
+#if AK8975DRV_DATA_DBG
+	pr_info("akm8975_ecs_report_value: yaw = %d, pitch = %d, roll = %d\n",
+				 rbuf[0], rbuf[1], rbuf[2]);
+	pr_info("tmp = %d, m_stat= %d, g_stat=%d\n", rbuf[3], rbuf[4], rbuf[5]);
+	pr_info("Acceleration:	 x = %d LSB, y = %d LSB, z = %d LSB\n",
+				 rbuf[6], rbuf[7], rbuf[8]);
+	pr_info("Magnetic:	 x = %d LSB, y = %d LSB, z = %d LSB\n\n",
+				 rbuf[9], rbuf[10], rbuf[11]);
+#endif
+	mutex_lock(&akm->flags_lock);
+	/* Report magnetic sensor information */
+	if (m_flag) {
+		input_report_abs(data->input_dev, ABS_RX, rbuf[0]);
+		input_report_abs(data->input_dev, ABS_RY, rbuf[1]);
+		input_report_abs(data->input_dev, ABS_RZ, rbuf[2]);
+		input_report_abs(data->input_dev, ABS_RUDDER, rbuf[4]);
+	}
+
+	/* Report acceleration sensor information */
+	if (a_flag) {
+		input_report_abs(data->input_dev, ABS_X, rbuf[6]);
+		input_report_abs(data->input_dev, ABS_Y, rbuf[7]);
+		input_report_abs(data->input_dev, ABS_Z, rbuf[8]);
+		input_report_abs(data->input_dev, ABS_WHEEL, rbuf[5]);
+	}
+
+	/* Report temperature information */
+	if (t_flag)
+		input_report_abs(data->input_dev, ABS_THROTTLE, rbuf[3]);
+
+	if (mv_flag) {
+		input_report_abs(data->input_dev, ABS_HAT0X, rbuf[9]);
+		input_report_abs(data->input_dev, ABS_HAT0Y, rbuf[10]);
+		input_report_abs(data->input_dev, ABS_BRAKE, rbuf[11]);
+	}
+	mutex_unlock(&akm->flags_lock);
+
+	input_sync(data->input_dev);
+}
+
+static void akm8975_ecs_close_done(struct akm8975_data *akm)
+{
+	FUNCDBG("called");
+	mutex_lock(&akm->flags_lock);
+	m_flag = 1;
+	a_flag = 1;
+	t_flag = 1;
+	mv_flag = 1;
+	mutex_unlock(&akm->flags_lock);
+}
+
+static int akm_aot_open(struct inode *inode, struct file *file)
+{
+	int ret = -1;
+
+	FUNCDBG("called");
+	if (atomic_cmpxchg(&open_flag, 0, 1) == 0) {
+		wake_up(&open_wq);
+		ret = 0;
+	}
+
+	ret = nonseekable_open(inode, file);
+	if (ret)
+		return ret;
+
+	file->private_data = akmd_data;
+
+	return ret;
+}
+
+static int akm_aot_release(struct inode *inode, struct file *file)
+{
+	FUNCDBG("called");
+	atomic_set(&open_flag, 0);
+	wake_up(&open_wq);
+	return 0;
+}
+
+static int akm_aot_ioctl(struct inode *inode, struct file *file,
+	      unsigned int cmd, unsigned long arg)
+{
+	void __user *argp = (void __user *) arg;
+	short flag;
+	struct akm8975_data *akm = file->private_data;
+
+	FUNCDBG("called");
+
+	switch (cmd) {
+	case ECS_IOCTL_APP_SET_MFLAG:
+	case ECS_IOCTL_APP_SET_AFLAG:
+	case ECS_IOCTL_APP_SET_MVFLAG:
+		if (copy_from_user(&flag, argp, sizeof(flag)))
+			return -EFAULT;
+		if (flag < 0 || flag > 1)
+			return -EINVAL;
+		break;
+	case ECS_IOCTL_APP_SET_DELAY:
+		if (copy_from_user(&flag, argp, sizeof(flag)))
+			return -EFAULT;
+		break;
+	default:
+		break;
+	}
+
+	mutex_lock(&akm->flags_lock);
+	switch (cmd) {
+	case ECS_IOCTL_APP_SET_MFLAG:
+	  m_flag = flag;
+		break;
+	case ECS_IOCTL_APP_GET_MFLAG:
+		flag = m_flag;
+		break;
+	case ECS_IOCTL_APP_SET_AFLAG:
+		a_flag = flag;
+		break;
+	case ECS_IOCTL_APP_GET_AFLAG:
+		flag = a_flag;
+		break;
+	case ECS_IOCTL_APP_SET_MVFLAG:
+		mv_flag = flag;
+		break;
+	case ECS_IOCTL_APP_GET_MVFLAG:
+		flag = mv_flag;
+		break;
+	case ECS_IOCTL_APP_SET_DELAY:
+		akmd_delay = flag;
+		break;
+	case ECS_IOCTL_APP_GET_DELAY:
+		flag = akmd_delay;
+		break;
+	default:
+		return -ENOTTY;
+	}
+	mutex_unlock(&akm->flags_lock);
+
+	switch (cmd) {
+	case ECS_IOCTL_APP_GET_MFLAG:
+	case ECS_IOCTL_APP_GET_AFLAG:
+	case ECS_IOCTL_APP_GET_MVFLAG:
+	case ECS_IOCTL_APP_GET_DELAY:
+		if (copy_to_user(argp, &flag, sizeof(flag)))
+			return -EFAULT;
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int akmd_open(struct inode *inode, struct file *file)
+{
+	int err = 0;
+
+	FUNCDBG("called");
+	err = nonseekable_open(inode, file);
+	if (err)
+		return err;
+
+	file->private_data = akmd_data;
+	return 0;
+}
+
+static int akmd_release(struct inode *inode, struct file *file)
+{
+	struct akm8975_data *akm = file->private_data;
+
+	FUNCDBG("called");
+	akm8975_ecs_close_done(akm);
+	return 0;
+}
+
+static int akmd_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+		      unsigned long arg)
+{
+	void __user *argp = (void __user *) arg;
+
+	char rwbuf[16];
+	int ret = -1;
+	int status;
+	short value[12];
+	short delay;
+	struct akm8975_data *akm = file->private_data;
+
+	FUNCDBG("called");
+
+	switch (cmd) {
+	case ECS_IOCTL_READ:
+	case ECS_IOCTL_WRITE:
+		if (copy_from_user(&rwbuf, argp, sizeof(rwbuf)))
+			return -EFAULT;
+		break;
+
+	case ECS_IOCTL_SET_YPR:
+		if (copy_from_user(&value, argp, sizeof(value)))
+			return -EFAULT;
+		break;
+
+	default:
+		break;
+	}
+
+	switch (cmd) {
+	case ECS_IOCTL_READ:
+		if (rwbuf[0] < 1)
+			return -EINVAL;
+
+		ret = akm8975_i2c_rxdata(akm, &rwbuf[1], rwbuf[0]);
+		if (ret < 0)
+			return ret;
+		break;
+
+	case ECS_IOCTL_WRITE:
+		if (rwbuf[0] < 2)
+			return -EINVAL;
+
+		ret = akm8975_i2c_txdata(akm, &rwbuf[1], rwbuf[0]);
+		if (ret < 0)
+			return ret;
+		break;
+	case ECS_IOCTL_SET_YPR:
+		akm8975_ecs_report_value(akm, value);
+		break;
+
+	case ECS_IOCTL_GET_OPEN_STATUS:
+		wait_event_interruptible(open_wq,
+					 (atomic_read(&open_flag) != 0));
+		status = atomic_read(&open_flag);
+		break;
+	case ECS_IOCTL_GET_CLOSE_STATUS:
+		wait_event_interruptible(open_wq,
+					 (atomic_read(&open_flag) == 0));
+		status = atomic_read(&open_flag);
+		break;
+
+	case ECS_IOCTL_GET_DELAY:
+		delay = akmd_delay;
+		break;
+
+	default:
+		FUNCDBG("Unknown cmd\n");
+		return -ENOTTY;
+	}
+
+	switch (cmd) {
+	case ECS_IOCTL_READ:
+		if (copy_to_user(argp, &rwbuf, sizeof(rwbuf)))
+			return -EFAULT;
+		break;
+	case ECS_IOCTL_GET_OPEN_STATUS:
+	case ECS_IOCTL_GET_CLOSE_STATUS:
+		if (copy_to_user(argp, &status, sizeof(status)))
+			return -EFAULT;
+		break;
+	case ECS_IOCTL_GET_DELAY:
+		if (copy_to_user(argp, &delay, sizeof(delay)))
+			return -EFAULT;
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+/* needed to clear the int. pin */
+static void akm_work_func(struct work_struct *work)
+{
+	struct akm8975_data *akm =
+	    container_of(work, struct akm8975_data, work);
+
+	FUNCDBG("called");
+	enable_irq(akm->this_client->irq);
+}
+
+static irqreturn_t akm8975_interrupt(int irq, void *dev_id)
+{
+	struct akm8975_data *akm = dev_id;
+	FUNCDBG("called");
+
+	disable_irq_nosync(akm->this_client->irq);
+	schedule_work(&akm->work);
+	return IRQ_HANDLED;
+}
+
+static int akm8975_power_off(struct akm8975_data *akm)
+{
+#if AK8975DRV_CALL_DBG
+	pr_info("%s\n", __func__);
+#endif
+	if (akm->pdata->power_off)
+		akm->pdata->power_off();
+
+	return 0;
+}
+
+static int akm8975_power_on(struct akm8975_data *akm)
+{
+	int err;
+
+#if AK8975DRV_CALL_DBG
+	pr_info("%s\n", __func__);
+#endif
+	if (akm->pdata->power_on) {
+		err = akm->pdata->power_on();
+		if (err < 0)
+			return err;
+	}
+	return 0;
+}
+
+static int akm8975_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+	struct akm8975_data *akm = i2c_get_clientdata(client);
+
+#if AK8975DRV_CALL_DBG
+	pr_info("%s\n", __func__);
+#endif
+	/* TO DO: might need more work after power mgmt
+	   is enabled */
+	return akm8975_power_off(akm);
+}
+
+static int akm8975_resume(struct i2c_client *client)
+{
+	struct akm8975_data *akm = i2c_get_clientdata(client);
+
+#if AK8975DRV_CALL_DBG
+	pr_info("%s\n", __func__);
+#endif
+	/* TO DO: might need more work after power mgmt
+	   is enabled */
+	return akm8975_power_on(akm);
+}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void akm8975_early_suspend(struct early_suspend *handler)
+{
+	struct akm8975_data *akm;
+	akm = container_of(handler, struct akm8975_data, early_suspend);
+
+#if AK8975DRV_CALL_DBG
+	pr_info("%s\n", __func__);
+#endif
+	akm8975_suspend(akm->this_client, PMSG_SUSPEND);
+}
+
+static void akm8975_early_resume(struct early_suspend *handler)
+{
+	struct akm8975_data *akm;
+	akm = container_of(handler, struct akm8975_data, early_suspend);
+
+#if AK8975DRV_CALL_DBG
+	pr_info("%s\n", __func__);
+#endif
+	akm8975_resume(akm->this_client);
+}
+#endif
+
+
+static int akm8975_init_client(struct i2c_client *client)
+{
+	struct akm8975_data *data;
+	int ret;
+
+	data = i2c_get_clientdata(client);
+
+	ret = request_irq(client->irq, akm8975_interrupt, IRQF_TRIGGER_RISING,
+				"akm8975", data);
+
+	if (ret < 0) {
+		pr_err("akm8975_init_client: request irq failed\n");
+		goto err;
+	}
+
+	init_waitqueue_head(&open_wq);
+
+	mutex_lock(&data->flags_lock);
+	m_flag = 1;
+	a_flag = 1;
+	t_flag = 1;
+	mv_flag = 1;
+	mutex_unlock(&data->flags_lock);
+
+	return 0;
+err:
+  return ret;
+}
+
+static const struct file_operations akmd_fops = {
+	.owner = THIS_MODULE,
+	.open = akmd_open,
+	.release = akmd_release,
+	.unlocked_ioctl = akmd_ioctl,
+};
+
+static const struct file_operations akm_aot_fops = {
+	.owner = THIS_MODULE,
+	.open = akm_aot_open,
+	.release = akm_aot_release,
+	.unlocked_ioctl = akm_aot_ioctl,
+};
+
+static struct miscdevice akm_aot_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "akm8975_aot",
+	.fops = &akm_aot_fops,
+};
+
+static struct miscdevice akmd_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "akm8975_dev",
+	.fops = &akmd_fops,
+};
+
+int akm8975_probe(struct i2c_client *client,
+		  const struct i2c_device_id *devid)
+{
+	struct akm8975_data *akm;
+	int err;
+	FUNCDBG("called");
+
+	if (client->dev.platform_data == NULL) {
+		dev_err(&client->dev, "platform data is NULL. exiting.\n");
+		err = -ENODEV;
+		goto exit_platform_data_null;
+	}
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "platform data is NULL. exiting.\n");
+		err = -ENODEV;
+		goto exit_check_functionality_failed;
+	}
+
+	akm = kzalloc(sizeof(struct akm8975_data), GFP_KERNEL);
+	if (!akm) {
+		dev_err(&client->dev,
+			"failed to allocate memory for module data\n");
+		err = -ENOMEM;
+		goto exit_alloc_data_failed;
+	}
+
+	akm->pdata = client->dev.platform_data;
+
+	mutex_init(&akm->flags_lock);
+	INIT_WORK(&akm->work, akm_work_func);
+	i2c_set_clientdata(client, akm);
+
+	err = akm8975_power_on(akm);
+	if (err < 0)
+		goto exit_power_on_failed;
+
+	akm8975_init_client(client);
+	akm->this_client = client;
+	akmd_data = akm;
+
+	akm->input_dev = input_allocate_device();
+	if (!akm->input_dev) {
+		err = -ENOMEM;
+		dev_err(&akm->this_client->dev,
+			"input device allocate failed\n");
+		goto exit_input_dev_alloc_failed;
+	}
+
+	set_bit(EV_ABS, akm->input_dev->evbit);
+
+	/* yaw */
+	input_set_abs_params(akm->input_dev, ABS_RX, 0, 23040, 0, 0);
+	/* pitch */
+	input_set_abs_params(akm->input_dev, ABS_RY, -11520, 11520, 0, 0);
+	/* roll */
+	input_set_abs_params(akm->input_dev, ABS_RZ, -5760, 5760, 0, 0);
+	/* x-axis acceleration */
+	input_set_abs_params(akm->input_dev, ABS_X, -5760, 5760, 0, 0);
+	/* y-axis acceleration */
+	input_set_abs_params(akm->input_dev, ABS_Y, -5760, 5760, 0, 0);
+	/* z-axis acceleration */
+	input_set_abs_params(akm->input_dev, ABS_Z, -5760, 5760, 0, 0);
+	/* temparature */
+	input_set_abs_params(akm->input_dev, ABS_THROTTLE, -30, 85, 0, 0);
+	/* status of magnetic sensor */
+	input_set_abs_params(akm->input_dev, ABS_RUDDER, 0, 3, 0, 0);
+	/* status of acceleration sensor */
+	input_set_abs_params(akm->input_dev, ABS_WHEEL, 0, 3, 0, 0);
+	/* x-axis of raw magnetic vector */
+	input_set_abs_params(akm->input_dev, ABS_HAT0X, -20480, 20479, 0, 0);
+	/* y-axis of raw magnetic vector */
+	input_set_abs_params(akm->input_dev, ABS_HAT0Y, -20480, 20479, 0, 0);
+	/* z-axis of raw magnetic vector */
+	input_set_abs_params(akm->input_dev, ABS_BRAKE, -20480, 20479, 0, 0);
+
+	akm->input_dev->name = "compass";
+
+	err = input_register_device(akm->input_dev);
+	if (err) {
+		pr_err("akm8975_probe: Unable to register input device: %s\n",
+					 akm->input_dev->name);
+		goto exit_input_register_device_failed;
+	}
+
+	err = misc_register(&akmd_device);
+	if (err) {
+		pr_err("akm8975_probe: akmd_device register failed\n");
+		goto exit_misc_device_register_failed;
+	}
+
+	err = misc_register(&akm_aot_device);
+	if (err) {
+		pr_err("akm8975_probe: akm_aot_device register failed\n");
+		goto exit_misc_device_register_failed;
+	}
+
+	err = device_create_file(&client->dev, &dev_attr_akm_ms1);
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	akm->early_suspend.suspend = akm8975_early_suspend;
+	akm->early_suspend.resume = akm8975_early_resume;
+	register_early_suspend(&akm->early_suspend);
+#endif
+	return 0;
+
+exit_misc_device_register_failed:
+exit_input_register_device_failed:
+	input_free_device(akm->input_dev);
+exit_input_dev_alloc_failed:
+	akm8975_power_off(akm);
+exit_power_on_failed:
+	kfree(akm);
+exit_alloc_data_failed:
+exit_check_functionality_failed:
+exit_platform_data_null:
+	return err;
+}
+
+static int __devexit akm8975_remove(struct i2c_client *client)
+{
+	struct akm8975_data *akm = i2c_get_clientdata(client);
+	FUNCDBG("called");
+	free_irq(client->irq, NULL);
+	input_unregister_device(akm->input_dev);
+	misc_deregister(&akmd_device);
+	misc_deregister(&akm_aot_device);
+	akm8975_power_off(akm);
+	kfree(akm);
+	return 0;
+}
+
+static const struct i2c_device_id akm8975_id[] = {
+	{ "akm8975", 0 },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(i2c, akm8975_id);
+
+static struct i2c_driver akm8975_driver = {
+	.probe = akm8975_probe,
+	.remove = akm8975_remove,
+#ifndef CONFIG_HAS_EARLYSUSPEND
+	.resume = akm8975_resume,
+	.suspend = akm8975_suspend,
+#endif
+	.id_table = akm8975_id,
+	.driver = {
+		.name = "akm8975",
+	},
+};
+
+static int __init akm8975_init(void)
+{
+	pr_info("AK8975 compass driver: init\n");
+	FUNCDBG("AK8975 compass driver: init\n");
+	return i2c_add_driver(&akm8975_driver);
+}
+
+static void __exit akm8975_exit(void)
+{
+	FUNCDBG("AK8975 compass driver: exit\n");
+	i2c_del_driver(&akm8975_driver);
+}
+
+module_init(akm8975_init);
+module_exit(akm8975_exit);
+
+MODULE_AUTHOR("Hou-Kun Chen <hk_chen@htc.com>");
+MODULE_DESCRIPTION("AK8975 compass driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/apanic.c b/drivers/misc/apanic.c
new file mode 100644
index 000000000000..ca875f89da7a
--- /dev/null
+++ b/drivers/misc/apanic.c
@@ -0,0 +1,606 @@
+/* drivers/misc/apanic.c
+ *
+ * Copyright (C) 2009 Google, Inc.
+ * Author: San Mehat <san@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/wakelock.h>
+#include <linux/platform_device.h>
+#include <linux/uaccess.h>
+#include <linux/mtd/mtd.h>
+#include <linux/notifier.h>
+#include <linux/mtd/mtd.h>
+#include <linux/debugfs.h>
+#include <linux/fs.h>
+#include <linux/proc_fs.h>
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+#include <linux/preempt.h>
+
+extern void ram_console_enable_console(int);
+
+struct panic_header {
+	u32 magic;
+#define PANIC_MAGIC 0xdeadf00d
+
+	u32 version;
+#define PHDR_VERSION   0x01
+
+	u32 console_offset;
+	u32 console_length;
+
+	u32 threads_offset;
+	u32 threads_length;
+};
+
+struct apanic_data {
+	struct mtd_info		*mtd;
+	struct panic_header	curr;
+	void			*bounce;
+	struct proc_dir_entry	*apanic_console;
+	struct proc_dir_entry	*apanic_threads;
+};
+
+static struct apanic_data drv_ctx;
+static struct work_struct proc_removal_work;
+static DEFINE_MUTEX(drv_mutex);
+
+static unsigned int *apanic_bbt;
+static unsigned int apanic_erase_blocks;
+static unsigned int apanic_good_blocks;
+
+static void set_bb(unsigned int block, unsigned int *bbt)
+{
+	unsigned int flag = 1;
+
+	BUG_ON(block >= apanic_erase_blocks);
+
+	flag = flag << (block%32);
+	apanic_bbt[block/32] |= flag;
+	apanic_good_blocks--;
+}
+
+static unsigned int get_bb(unsigned int block, unsigned int *bbt)
+{
+	unsigned int flag;
+
+	BUG_ON(block >= apanic_erase_blocks);
+
+	flag = 1 << (block%32);
+	return apanic_bbt[block/32] & flag;
+}
+
+static void alloc_bbt(struct mtd_info *mtd, unsigned int *bbt)
+{
+	int bbt_size;
+	apanic_erase_blocks = (mtd->size)>>(mtd->erasesize_shift);
+	bbt_size = (apanic_erase_blocks+32)/32;
+
+	apanic_bbt = kmalloc(bbt_size*4, GFP_KERNEL);
+	memset(apanic_bbt, 0, bbt_size*4);
+	apanic_good_blocks = apanic_erase_blocks;
+}
+static void scan_bbt(struct mtd_info *mtd, unsigned int *bbt)
+{
+	int i;
+
+	for (i = 0; i < apanic_erase_blocks; i++) {
+		if (mtd->block_isbad(mtd, i*mtd->erasesize))
+			set_bb(i, apanic_bbt);
+	}
+}
+
+#define APANIC_INVALID_OFFSET 0xFFFFFFFF
+
+static unsigned int phy_offset(struct mtd_info *mtd, unsigned int offset)
+{
+	unsigned int logic_block = offset>>(mtd->erasesize_shift);
+	unsigned int phy_block;
+	unsigned good_block = 0;
+
+	for (phy_block = 0; phy_block < apanic_erase_blocks; phy_block++) {
+		if (!get_bb(phy_block, apanic_bbt))
+			good_block++;
+		if (good_block == (logic_block + 1))
+			break;
+	}
+
+	if (good_block != (logic_block + 1))
+		return APANIC_INVALID_OFFSET;
+
+	return offset + ((phy_block-logic_block)<<mtd->erasesize_shift);
+}
+
+static void apanic_erase_callback(struct erase_info *done)
+{
+	wait_queue_head_t *wait_q = (wait_queue_head_t *) done->priv;
+	wake_up(wait_q);
+}
+
+static int apanic_proc_read(char *buffer, char **start, off_t offset,
+			       int count, int *peof, void *dat)
+{
+	struct apanic_data *ctx = &drv_ctx;
+	size_t file_length;
+	off_t file_offset;
+	unsigned int page_no;
+	off_t page_offset;
+	int rc;
+	size_t len;
+
+	if (!count)
+		return 0;
+
+	mutex_lock(&drv_mutex);
+
+	switch ((int) dat) {
+	case 1:	/* apanic_console */
+		file_length = ctx->curr.console_length;
+		file_offset = ctx->curr.console_offset;
+		break;
+	case 2:	/* apanic_threads */
+		file_length = ctx->curr.threads_length;
+		file_offset = ctx->curr.threads_offset;
+		break;
+	default:
+		pr_err("Bad dat (%d)\n", (int) dat);
+		mutex_unlock(&drv_mutex);
+		return -EINVAL;
+	}
+
+	if ((offset + count) > file_length) {
+		mutex_unlock(&drv_mutex);
+		return 0;
+	}
+
+	/* We only support reading a maximum of a flash page */
+	if (count > ctx->mtd->writesize)
+		count = ctx->mtd->writesize;
+
+	page_no = (file_offset + offset) / ctx->mtd->writesize;
+	page_offset = (file_offset + offset) % ctx->mtd->writesize;
+
+
+	if (phy_offset(ctx->mtd, (page_no * ctx->mtd->writesize))
+		== APANIC_INVALID_OFFSET) {
+		pr_err("apanic: reading an invalid address\n");
+		mutex_unlock(&drv_mutex);
+		return -EINVAL;
+	}
+	rc = ctx->mtd->read(ctx->mtd,
+		phy_offset(ctx->mtd, (page_no * ctx->mtd->writesize)),
+		ctx->mtd->writesize,
+		&len, ctx->bounce);
+
+	if (page_offset)
+		count -= page_offset;
+	memcpy(buffer, ctx->bounce + page_offset, count);
+
+	*start = count;
+
+	if ((offset + count) == file_length)
+		*peof = 1;
+
+	mutex_unlock(&drv_mutex);
+	return count;
+}
+
+static void mtd_panic_erase(void)
+{
+	struct apanic_data *ctx = &drv_ctx;
+	struct erase_info erase;
+	DECLARE_WAITQUEUE(wait, current);
+	wait_queue_head_t wait_q;
+	int rc, i;
+
+	init_waitqueue_head(&wait_q);
+	erase.mtd = ctx->mtd;
+	erase.callback = apanic_erase_callback;
+	erase.len = ctx->mtd->erasesize;
+	erase.priv = (u_long)&wait_q;
+	for (i = 0; i < ctx->mtd->size; i += ctx->mtd->erasesize) {
+		erase.addr = i;
+		set_current_state(TASK_INTERRUPTIBLE);
+		add_wait_queue(&wait_q, &wait);
+
+		if (get_bb(erase.addr>>ctx->mtd->erasesize_shift, apanic_bbt)) {
+			printk(KERN_WARNING
+			       "apanic: Skipping erase of bad "
+			       "block @%llx\n", erase.addr);
+			set_current_state(TASK_RUNNING);
+			remove_wait_queue(&wait_q, &wait);
+			continue;
+		}
+
+		rc = ctx->mtd->erase(ctx->mtd, &erase);
+		if (rc) {
+			set_current_state(TASK_RUNNING);
+			remove_wait_queue(&wait_q, &wait);
+			printk(KERN_ERR
+			       "apanic: Erase of 0x%llx, 0x%llx failed\n",
+			       (unsigned long long) erase.addr,
+			       (unsigned long long) erase.len);
+			if (rc == -EIO) {
+				if (ctx->mtd->block_markbad(ctx->mtd,
+							    erase.addr)) {
+					printk(KERN_ERR
+					       "apanic: Err marking blk bad\n");
+					goto out;
+				}
+				printk(KERN_INFO
+				       "apanic: Marked a bad block"
+				       " @%llx\n", erase.addr);
+				set_bb(erase.addr>>ctx->mtd->erasesize_shift,
+					apanic_bbt);
+				continue;
+			}
+			goto out;
+		}
+		schedule();
+		remove_wait_queue(&wait_q, &wait);
+	}
+	printk(KERN_DEBUG "apanic: %s partition erased\n",
+	       CONFIG_APANIC_PLABEL);
+out:
+	return;
+}
+
+static void apanic_remove_proc_work(struct work_struct *work)
+{
+	struct apanic_data *ctx = &drv_ctx;
+
+	mutex_lock(&drv_mutex);
+	mtd_panic_erase();
+	memset(&ctx->curr, 0, sizeof(struct panic_header));
+	if (ctx->apanic_console) {
+		remove_proc_entry("apanic_console", NULL);
+		ctx->apanic_console = NULL;
+	}
+	if (ctx->apanic_threads) {
+		remove_proc_entry("apanic_threads", NULL);
+		ctx->apanic_threads = NULL;
+	}
+	mutex_unlock(&drv_mutex);
+}
+
+static int apanic_proc_write(struct file *file, const char __user *buffer,
+				unsigned long count, void *data)
+{
+	schedule_work(&proc_removal_work);
+	return count;
+}
+
+static void mtd_panic_notify_add(struct mtd_info *mtd)
+{
+	struct apanic_data *ctx = &drv_ctx;
+	struct panic_header *hdr = ctx->bounce;
+	size_t len;
+	int rc;
+	int    proc_entry_created = 0;
+
+	if (strcmp(mtd->name, CONFIG_APANIC_PLABEL))
+		return;
+
+	ctx->mtd = mtd;
+
+	alloc_bbt(mtd, apanic_bbt);
+	scan_bbt(mtd, apanic_bbt);
+
+	if (apanic_good_blocks == 0) {
+		printk(KERN_ERR "apanic: no any good blocks?!\n");
+		goto out_err;
+	}
+
+	rc = mtd->read(mtd, phy_offset(mtd, 0), mtd->writesize,
+			&len, ctx->bounce);
+	if (rc && rc == -EBADMSG) {
+		printk(KERN_WARNING
+		       "apanic: Bad ECC on block 0 (ignored)\n");
+	} else if (rc && rc != -EUCLEAN) {
+		printk(KERN_ERR "apanic: Error reading block 0 (%d)\n", rc);
+		goto out_err;
+	}
+
+	if (len != mtd->writesize) {
+		printk(KERN_ERR "apanic: Bad read size (%d)\n", rc);
+		goto out_err;
+	}
+
+	printk(KERN_INFO "apanic: Bound to mtd partition '%s'\n", mtd->name);
+
+	if (hdr->magic != PANIC_MAGIC) {
+		printk(KERN_INFO "apanic: No panic data available\n");
+		mtd_panic_erase();
+		return;
+	}
+
+	if (hdr->version != PHDR_VERSION) {
+		printk(KERN_INFO "apanic: Version mismatch (%d != %d)\n",
+		       hdr->version, PHDR_VERSION);
+		mtd_panic_erase();
+		return;
+	}
+
+	memcpy(&ctx->curr, hdr, sizeof(struct panic_header));
+
+	printk(KERN_INFO "apanic: c(%u, %u) t(%u, %u)\n",
+	       hdr->console_offset, hdr->console_length,
+	       hdr->threads_offset, hdr->threads_length);
+
+	if (hdr->console_length) {
+		ctx->apanic_console = create_proc_entry("apanic_console",
+						      S_IFREG | S_IRUGO, NULL);
+		if (!ctx->apanic_console)
+			printk(KERN_ERR "%s: failed creating procfile\n",
+			       __func__);
+		else {
+			ctx->apanic_console->read_proc = apanic_proc_read;
+			ctx->apanic_console->write_proc = apanic_proc_write;
+			ctx->apanic_console->size = hdr->console_length;
+			ctx->apanic_console->data = (void *) 1;
+			proc_entry_created = 1;
+		}
+	}
+
+	if (hdr->threads_length) {
+		ctx->apanic_threads = create_proc_entry("apanic_threads",
+						       S_IFREG | S_IRUGO, NULL);
+		if (!ctx->apanic_threads)
+			printk(KERN_ERR "%s: failed creating procfile\n",
+			       __func__);
+		else {
+			ctx->apanic_threads->read_proc = apanic_proc_read;
+			ctx->apanic_threads->write_proc = apanic_proc_write;
+			ctx->apanic_threads->size = hdr->threads_length;
+			ctx->apanic_threads->data = (void *) 2;
+			proc_entry_created = 1;
+		}
+	}
+
+	if (!proc_entry_created)
+		mtd_panic_erase();
+
+	return;
+out_err:
+	ctx->mtd = NULL;
+}
+
+static void mtd_panic_notify_remove(struct mtd_info *mtd)
+{
+	struct apanic_data *ctx = &drv_ctx;
+	if (mtd == ctx->mtd) {
+		ctx->mtd = NULL;
+		printk(KERN_INFO "apanic: Unbound from %s\n", mtd->name);
+	}
+}
+
+static struct mtd_notifier mtd_panic_notifier = {
+	.add	= mtd_panic_notify_add,
+	.remove	= mtd_panic_notify_remove,
+};
+
+static int in_panic = 0;
+
+static int apanic_writeflashpage(struct mtd_info *mtd, loff_t to,
+				 const u_char *buf)
+{
+	int rc;
+	size_t wlen;
+	int panic = in_interrupt() | in_atomic();
+
+	if (panic && !mtd->panic_write) {
+		printk(KERN_EMERG "%s: No panic_write available\n", __func__);
+		return 0;
+	} else if (!panic && !mtd->write) {
+		printk(KERN_EMERG "%s: No write available\n", __func__);
+		return 0;
+	}
+
+	to = phy_offset(mtd, to);
+	if (to == APANIC_INVALID_OFFSET) {
+		printk(KERN_EMERG "apanic: write to invalid address\n");
+		return 0;
+	}
+
+	if (panic)
+		rc = mtd->panic_write(mtd, to, mtd->writesize, &wlen, buf);
+	else
+		rc = mtd->write(mtd, to, mtd->writesize, &wlen, buf);
+
+	if (rc) {
+		printk(KERN_EMERG
+		       "%s: Error writing data to flash (%d)\n",
+		       __func__, rc);
+		return rc;
+	}
+
+	return wlen;
+}
+
+extern int log_buf_copy(char *dest, int idx, int len);
+extern void log_buf_clear(void);
+
+/*
+ * Writes the contents of the console to the specified offset in flash.
+ * Returns number of bytes written
+ */
+static int apanic_write_console(struct mtd_info *mtd, unsigned int off)
+{
+	struct apanic_data *ctx = &drv_ctx;
+	int saved_oip;
+	int idx = 0;
+	int rc, rc2;
+	unsigned int last_chunk = 0;
+
+	while (!last_chunk) {
+		saved_oip = oops_in_progress;
+		oops_in_progress = 1;
+		rc = log_buf_copy(ctx->bounce, idx, mtd->writesize);
+		if (rc < 0)
+			break;
+
+		if (rc != mtd->writesize)
+			last_chunk = rc;
+
+		oops_in_progress = saved_oip;
+		if (rc <= 0)
+			break;
+		if (rc != mtd->writesize)
+			memset(ctx->bounce + rc, 0, mtd->writesize - rc);
+
+		rc2 = apanic_writeflashpage(mtd, off, ctx->bounce);
+		if (rc2 <= 0) {
+			printk(KERN_EMERG
+			       "apanic: Flash write failed (%d)\n", rc2);
+			return idx;
+		}
+		if (!last_chunk)
+			idx += rc2;
+		else
+			idx += last_chunk;
+		off += rc2;
+	}
+	return idx;
+}
+
+static int apanic(struct notifier_block *this, unsigned long event,
+			void *ptr)
+{
+	struct apanic_data *ctx = &drv_ctx;
+	struct panic_header *hdr = (struct panic_header *) ctx->bounce;
+	int console_offset = 0;
+	int console_len = 0;
+	int threads_offset = 0;
+	int threads_len = 0;
+	int rc;
+
+	if (in_panic)
+		return NOTIFY_DONE;
+	in_panic = 1;
+#ifdef CONFIG_PREEMPT
+	/* Ensure that cond_resched() won't try to preempt anybody */
+	add_preempt_count(PREEMPT_ACTIVE);
+#endif
+	touch_softlockup_watchdog();
+
+	if (!ctx->mtd)
+		goto out;
+
+	if (ctx->curr.magic) {
+		printk(KERN_EMERG "Crash partition in use!\n");
+		goto out;
+	}
+	console_offset = ctx->mtd->writesize;
+
+	/*
+	 * Write out the console
+	 */
+	console_len = apanic_write_console(ctx->mtd, console_offset);
+	if (console_len < 0) {
+		printk(KERN_EMERG "Error writing console to panic log! (%d)\n",
+		       console_len);
+		console_len = 0;
+	}
+
+	/*
+	 * Write out all threads
+	 */
+	threads_offset = ALIGN(console_offset + console_len,
+			       ctx->mtd->writesize);
+	if (!threads_offset)
+		threads_offset = ctx->mtd->writesize;
+
+	ram_console_enable_console(0);
+
+	log_buf_clear();
+	show_state_filter(0);
+	threads_len = apanic_write_console(ctx->mtd, threads_offset);
+	if (threads_len < 0) {
+		printk(KERN_EMERG "Error writing threads to panic log! (%d)\n",
+		       threads_len);
+		threads_len = 0;
+	}
+
+	/*
+	 * Finally write the panic header
+	 */
+	memset(ctx->bounce, 0, PAGE_SIZE);
+	hdr->magic = PANIC_MAGIC;
+	hdr->version = PHDR_VERSION;
+
+	hdr->console_offset = console_offset;
+	hdr->console_length = console_len;
+
+	hdr->threads_offset = threads_offset;
+	hdr->threads_length = threads_len;
+
+	rc = apanic_writeflashpage(ctx->mtd, 0, ctx->bounce);
+	if (rc <= 0) {
+		printk(KERN_EMERG "apanic: Header write failed (%d)\n",
+		       rc);
+		goto out;
+	}
+
+	printk(KERN_EMERG "apanic: Panic dump sucessfully written to flash\n");
+
+ out:
+#ifdef CONFIG_PREEMPT
+	sub_preempt_count(PREEMPT_ACTIVE);
+#endif
+	in_panic = 0;
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block panic_blk = {
+	.notifier_call	= apanic,
+};
+
+static int panic_dbg_get(void *data, u64 *val)
+{
+	apanic(NULL, 0, NULL);
+	return 0;
+}
+
+static int panic_dbg_set(void *data, u64 val)
+{
+	BUG();
+	return -1;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(panic_dbg_fops, panic_dbg_get, panic_dbg_set, "%llu\n");
+
+int __init apanic_init(void)
+{
+	register_mtd_user(&mtd_panic_notifier);
+	atomic_notifier_chain_register(&panic_notifier_list, &panic_blk);
+	debugfs_create_file("apanic", 0644, NULL, NULL, &panic_dbg_fops);
+	memset(&drv_ctx, 0, sizeof(drv_ctx));
+	drv_ctx.bounce = (void *) __get_free_page(GFP_KERNEL);
+	INIT_WORK(&proc_removal_work, apanic_remove_proc_work);
+	printk(KERN_INFO "Android kernel panic handler initialized (bind=%s)\n",
+	       CONFIG_APANIC_PLABEL);
+	return 0;
+}
+
+module_init(apanic_init);
diff --git a/drivers/misc/bcm4329_rfkill.c b/drivers/misc/bcm4329_rfkill.c
new file mode 100644
index 000000000000..a2d5ca6699cc
--- /dev/null
+++ b/drivers/misc/bcm4329_rfkill.c
@@ -0,0 +1,216 @@
+/*
+ * drivers/misc/bcm4329_rfkill.c
+ *
+ * Copyright (c) 2010, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/err.h>
+#include <linux/types.h>
+#include <linux/uaccess.h>
+#include <linux/fs.h>
+#include <linux/gpio.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/rfkill.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+
+struct bcm4329_rfkill_data {
+	int gpio_reset;
+	int gpio_shutdown;
+	int delay;
+	struct clk *bt_32k_clk;
+};
+
+static struct bcm4329_rfkill_data *bcm4329_rfkill;
+
+static int bcm4329_bt_rfkill_set_power(void *data, bool blocked)
+{
+	/*
+	 * check if BT gpio_shutdown line status and current request are same.
+	 * If same, then return, else perform requested operation.
+	 */
+	if (gpio_get_value(bcm4329_rfkill->gpio_shutdown) && !blocked)
+		return 0;
+
+	if (blocked) {
+		if (bcm4329_rfkill->gpio_shutdown)
+			gpio_direction_output(bcm4329_rfkill->gpio_shutdown, 0);
+		if (bcm4329_rfkill->gpio_reset)
+			gpio_direction_output(bcm4329_rfkill->gpio_reset, 0);
+		if (bcm4329_rfkill->bt_32k_clk)
+			clk_disable(bcm4329_rfkill->bt_32k_clk);
+	} else {
+		if (bcm4329_rfkill->bt_32k_clk)
+			clk_enable(bcm4329_rfkill->bt_32k_clk);
+		if (bcm4329_rfkill->gpio_shutdown)
+		{
+			gpio_direction_output(bcm4329_rfkill->gpio_shutdown, 0);
+			msleep(100);
+			gpio_direction_output(bcm4329_rfkill->gpio_shutdown, 1);
+			msleep(100);
+		}
+		if (bcm4329_rfkill->gpio_reset)
+		{
+			gpio_direction_output(bcm4329_rfkill->gpio_reset, 0);
+			msleep(100);
+			gpio_direction_output(bcm4329_rfkill->gpio_reset, 1);
+			msleep(100);
+		}
+	}
+
+	return 0;
+}
+
+static const struct rfkill_ops bcm4329_bt_rfkill_ops = {
+	.set_block = bcm4329_bt_rfkill_set_power,
+};
+
+static int bcm4329_rfkill_probe(struct platform_device *pdev)
+{
+	struct rfkill *bt_rfkill;
+	struct resource *res;
+	int ret;
+	bool enable = false;  /* off */
+	bool default_sw_block_state;
+
+	bcm4329_rfkill = kzalloc(sizeof(*bcm4329_rfkill), GFP_KERNEL);
+	if (!bcm4329_rfkill)
+		return -ENOMEM;
+
+	bcm4329_rfkill->bt_32k_clk = clk_get(&pdev->dev, "bcm4329_32k_clk");
+	if (IS_ERR(bcm4329_rfkill->bt_32k_clk)) {
+		pr_warn("%s: can't find bcm4329_32k_clk.\
+				assuming 32k clock to chip\n", __func__);
+		bcm4329_rfkill->bt_32k_clk = NULL;
+	}
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_IO,
+						"bcm4329_nreset_gpio");
+	if (res) {
+		bcm4329_rfkill->gpio_reset = res->start;
+		ret = gpio_request(bcm4329_rfkill->gpio_reset,
+						"bcm4329_nreset_gpio");
+	} else {
+		pr_warn("%s : can't find reset gpio. "
+			"reset gpio may not be defined for "
+			"this platform \n", __func__);
+		bcm4329_rfkill->gpio_reset = 0;
+	}
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_IO,
+						"bcm4329_nshutdown_gpio");
+	if (res) {
+		bcm4329_rfkill->gpio_shutdown = res->start;
+		ret = gpio_request(bcm4329_rfkill->gpio_shutdown,
+						"bcm4329_nshutdown_gpio");
+	} else {
+		pr_warn("%s : can't find shutdown gpio "
+			"shutdown gpio may not be defined for "
+			"this platform \n", __func__);
+		bcm4329_rfkill->gpio_shutdown = 0;
+	}
+
+	/* make sure at-least one of the GPIO is defined */
+	if (!bcm4329_rfkill->gpio_reset && !bcm4329_rfkill->gpio_shutdown)
+		goto free_bcm_res;
+
+	if (bcm4329_rfkill->bt_32k_clk && enable)
+		clk_enable(bcm4329_rfkill->bt_32k_clk);
+	if (bcm4329_rfkill->gpio_shutdown)
+		gpio_direction_output(bcm4329_rfkill->gpio_shutdown, enable);
+	if (bcm4329_rfkill->gpio_reset)
+		gpio_direction_output(bcm4329_rfkill->gpio_reset, enable);
+
+	bt_rfkill = rfkill_alloc("bcm4329 Bluetooth", &pdev->dev,
+				RFKILL_TYPE_BLUETOOTH, &bcm4329_bt_rfkill_ops,
+				NULL);
+
+	if (unlikely(!bt_rfkill))
+		goto free_bcm_res;
+
+	default_sw_block_state = !enable;
+	rfkill_set_states(bt_rfkill, default_sw_block_state, false);
+
+	ret = rfkill_register(bt_rfkill);
+
+	if (unlikely(ret)) {
+		rfkill_destroy(bt_rfkill);
+		goto free_bcm_res;
+	}
+
+	return 0;
+
+free_bcm_res:
+	if (bcm4329_rfkill->gpio_shutdown)
+		gpio_free(bcm4329_rfkill->gpio_shutdown);
+	if (bcm4329_rfkill->gpio_reset)
+		gpio_free(bcm4329_rfkill->gpio_reset);
+	if (bcm4329_rfkill->bt_32k_clk && enable)
+		clk_disable(bcm4329_rfkill->bt_32k_clk);
+	if (bcm4329_rfkill->bt_32k_clk)
+		clk_put(bcm4329_rfkill->bt_32k_clk);
+	kfree(bcm4329_rfkill);
+	return -ENODEV;
+}
+
+static int bcm4329_rfkill_remove(struct platform_device *pdev)
+{
+	struct rfkill *bt_rfkill = platform_get_drvdata(pdev);
+
+	if (bcm4329_rfkill->bt_32k_clk)
+		clk_put(bcm4329_rfkill->bt_32k_clk);
+	rfkill_unregister(bt_rfkill);
+	rfkill_destroy(bt_rfkill);
+	if (bcm4329_rfkill->gpio_shutdown)
+		gpio_free(bcm4329_rfkill->gpio_shutdown);
+	if (bcm4329_rfkill->gpio_reset)
+		gpio_free(bcm4329_rfkill->gpio_reset);
+	kfree(bcm4329_rfkill);
+
+	return 0;
+}
+
+static struct platform_driver bcm4329_rfkill_driver = {
+	.probe = bcm4329_rfkill_probe,
+	.remove = bcm4329_rfkill_remove,
+	.driver = {
+		   .name = "bcm4329_rfkill",
+		   .owner = THIS_MODULE,
+	},
+};
+
+static int __init bcm4329_rfkill_init(void)
+{
+	return platform_driver_register(&bcm4329_rfkill_driver);
+}
+
+static void __exit bcm4329_rfkill_exit(void)
+{
+	platform_driver_unregister(&bcm4329_rfkill_driver);
+}
+
+module_init(bcm4329_rfkill_init);
+module_exit(bcm4329_rfkill_exit);
+
+MODULE_DESCRIPTION("BCM4329 rfkill");
+MODULE_AUTHOR("NVIDIA");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/inv_mpu/Kconfig b/drivers/misc/inv_mpu/Kconfig
new file mode 100644
index 000000000000..4c231b576f68
--- /dev/null
+++ b/drivers/misc/inv_mpu/Kconfig
@@ -0,0 +1,70 @@
+config MPU_SENSORS_TIMERIRQ
+	tristate "MPU Timer IRQ"
+	help
+	  If you say yes here you get access to the timerirq device handle which
+	  can be used to select on.  This can be used instead of IRQ's, sleeping,
+	  or timer threads.  Reading from this device returns the same type of
+	  information as reading from the MPU and slave IRQ's.
+
+menuconfig INV_SENSORS
+	tristate "Motion Processing Unit"
+	depends on I2C
+	default y
+
+if INV_SENSORS
+
+choice
+	tristate "MPU Master"
+	depends on I2C && INV_SENSORS
+	default MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_MPU3050
+	tristate "MPU3050"
+	depends on I2C
+	select MPU_SENSORS_MPU3050_GYRO
+	help
+	  If you say yes here you get support for the MPU3050 Gyroscope driver
+	  This driver can also be built as a module.  If so, the module
+	  will be called mpu3050.
+
+config MPU_SENSORS_MPU6050A2
+	tristate "MPU6050A2"
+	depends on I2C
+	select MPU_SENSORS_MPU6050_GYRO
+	help
+	  If you say yes here you get support for the MPU6050A2 Gyroscope driver
+	  This driver can also be built as a module.  If so, the module
+	  will be called mpu6050a2.
+
+config MPU_SENSORS_MPU6050B1
+	tristate "MPU6050B1"
+	depends on I2C
+	select MPU_SENSORS_MPU6050_GYRO
+	help
+	  If you say yes here you get support for the MPU6050 Gyroscope driver
+	  This driver can also be built as a module.  If so, the module
+	  will be called mpu6050b1.
+
+endchoice
+
+config MPU_SENSORS_MPU3050_GYRO
+	bool "MPU3050 built in gyroscope"
+	depends on MPU_SENSORS_MPU3050
+
+config MPU_SENSORS_MPU6050_GYRO
+	bool "MPU6050 built in gyroscope"
+	depends on MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+
+source "drivers/misc/inv_mpu/accel/Kconfig"
+source "drivers/misc/inv_mpu/compass/Kconfig"
+source "drivers/misc/inv_mpu/pressure/Kconfig"
+
+config MPU_USERSPACE_DEBUG
+	bool "MPU Userspace debugging ioctls"
+	help
+	  Allows the ioctls MPU_SET_MPU_PLATFORM_DATA and
+	  MPU_SET_EXT_SLAVE_PLATFORM_DATA, which allows userspace applications
+	  to override the slave address and orientation.  This is dangerous
+	  and could cause the devices not to work.
+
+endif #INV_SENSORS
diff --git a/drivers/misc/inv_mpu/Makefile b/drivers/misc/inv_mpu/Makefile
new file mode 100644
index 000000000000..f10c6844a087
--- /dev/null
+++ b/drivers/misc/inv_mpu/Makefile
@@ -0,0 +1,18 @@
+
+# Kernel makefile for motions sensors
+#
+#
+
+EXTRA_CFLAGS += -Idrivers/misc/inv_mpu
+EXTRA_CFLAGS += -D__C99_DESIGNATED_INITIALIZER
+EXTRA_CFLAGS += -DINV_CACHE_DMP=1
+
+obj-$(CONFIG_MPU_SENSORS_TIMERIRQ)+= timerirq.o
+obj-$(CONFIG_INV_SENSORS) += mpuirq.o slaveirq.o
+
+obj-$(CONFIG_MPU_SENSORS_MPU3050) += mpu3050/
+obj-$(CONFIG_MPU_SENSORS_MPU6050B1) += mpu6050/
+
+obj-y += accel/
+obj-y += compass/
+obj-y += pressure/
diff --git a/drivers/misc/inv_mpu/accel/Kconfig b/drivers/misc/inv_mpu/accel/Kconfig
new file mode 100644
index 000000000000..4e280bd876bc
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/Kconfig
@@ -0,0 +1,133 @@
+menuconfig INV_SENSORS_ACCELEROMETERS
+	bool "Accelerometer Slave Sensors"
+	default y
+	---help---
+	  Say Y here to get to see options for device drivers for various
+	  accelerometrs for integration with the MPU3050 or MPU6050 driver.
+	  This option alone does not add any kernel code.
+
+	  If you say N, all options in this submenu will be skipped and disabled.
+
+if INV_SENSORS_ACCELEROMETERS
+
+config MPU_SENSORS_ADXL34X
+	tristate "ADI adxl34x"
+	depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+	help
+	  This enables support for the ADI adxl345 or adxl346 accelerometers.
+	  This support is for integration with the MPU3050 gyroscope device
+	  driver.  Only one accelerometer can be registered at a time.
+	  Specifying more that one accelerometer in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_BMA222
+	tristate "Bosch BMA222"
+	depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+	help
+	  This enables support for the Bosch BMA222 accelerometer
+	  This support is for integration with the MPU3050 gyroscope device
+	  driver.  Only one accelerometer can be registered at a time.
+	  Specifying more that one accelerometer in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_BMA150
+	tristate "Bosch BMA150"
+	depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+	help
+	  This enables support for the Bosch BMA150 accelerometer
+	  This support is for integration with the MPU3050 gyroscope device
+	  driver.  Only one accelerometer can be registered at a time.
+	  Specifying more that one accelerometer in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_BMA250
+	tristate "Bosch BMA250"
+	depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+	help
+	  This enables support for the Bosch BMA250 accelerometer
+	  This support is for integration with the MPU3050 gyroscope device
+	  driver.  Only one accelerometer can be registered at a time.
+	  Specifying more that one accelerometer in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_KXSD9
+	tristate "Kionix KXSD9"
+	depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+	help
+	  This enables support for the Kionix KXSD9 accelerometer
+	  This support is for integration with the MPU3050 gyroscope device
+	  driver.  Only one accelerometer can be registered at a time.
+	  Specifying more that one accelerometer in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_KXTF9
+	tristate "Kionix KXTF9"
+	depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+	help
+	  This enables support for the Kionix KXFT9 accelerometer
+	  This support is for integration with the MPU3050 gyroscope device
+	  driver.  Only one accelerometer can be registered at a time.
+	  Specifying more that one accelerometer in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_LIS331DLH
+	tristate "ST lis331dlh"
+	depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+	help
+	  This enables support for the ST lis331dlh accelerometer
+	  This support is for integration with the MPU3050 gyroscope device
+	  driver.  Only one accelerometer can be registered at a time.
+	  Specifying more that one accelerometer in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_LIS3DH
+	tristate "ST lis3dh"
+	depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+	help
+	  This enables support for the ST lis3dh accelerometer
+	  This support is for integration with the MPU3050 gyroscope device
+	  driver.  Only one accelerometer can be registered at a time.
+	  Specifying more that one accelerometer in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_LSM303DLX_A
+	tristate "ST lsm303dlx"
+	depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+	help
+	  This enables support for the ST lsm303dlh and lsm303dlm accelerometers
+	  This support is for integration with the MPU3050 gyroscope device
+	  driver.  Only one accelerometer can be registered at a time.
+	  Specifying more that one accelerometer in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_MMA8450
+	tristate "Freescale mma8450"
+	depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+	help
+	  This enables support for the Freescale mma8450 accelerometer
+	  This support is for integration with the MPU3050 gyroscope device
+	  driver.  Only one accelerometer can be registered at a time.
+	  Specifying more that one accelerometer in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_MMA845X
+	tristate "Freescale mma8451/8452/8453"
+	depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+	help
+	  This enables support for the Freescale mma8451/8452/8453 accelerometer
+	  This support is for integration with the MPU3050 gyroscope device
+	  driver.  Only one accelerometer can be registered at a time.
+	  Specifying more that one accelerometer in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_MPU6050_ACCEL
+	tristate "MPU6050 built in accelerometer"
+	depends on MPU_SENSORS_MPU6050B1 || MPU_SENSORS_MPU6050A2
+	help
+	  This enables support for the MPU6050 built in accelerometer.
+	  This the built in support for integration with the MPU6050 gyroscope
+	  device driver.  This is the only accelerometer supported with the
+	  MPU6050.  Specifying another accelerometer in the board file will
+	  result in runtime errors.
+
+endif
diff --git a/drivers/misc/inv_mpu/accel/Makefile b/drivers/misc/inv_mpu/accel/Makefile
new file mode 100644
index 000000000000..1f0f5bec6774
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/Makefile
@@ -0,0 +1,38 @@
+#
+# Accel Slaves to MPUxxxx
+#
+obj-$(CONFIG_MPU_SENSORS_ADXL34X) += inv_mpu_adxl34x.o
+inv_mpu_adxl34x-objs += adxl34x.o
+
+obj-$(CONFIG_MPU_SENSORS_BMA150) += inv_mpu_bma150.o
+inv_mpu_bma150-objs += bma150.o
+
+obj-$(CONFIG_MPU_SENSORS_KXTF9) += inv_mpu_kxtf9.o
+inv_mpu_kxtf9-objs += kxtf9.o
+
+obj-$(CONFIG_MPU_SENSORS_BMA222) += inv_mpu_bma222.o
+inv_mpu_bma222-objs += bma222.o
+
+obj-$(CONFIG_MPU_SENSORS_BMA250) += inv_mpu_bma250.o
+inv_mpu_bma250-objs += bma250.o
+
+obj-$(CONFIG_MPU_SENSORS_KXSD9) += inv_mpu_kxsd9.o
+inv_mpu_kxsd9-objs += kxsd9.o
+
+obj-$(CONFIG_MPU_SENSORS_LIS331DLH) += inv_mpu_lis331.o
+inv_mpu_lis331-objs += lis331.o
+
+obj-$(CONFIG_MPU_SENSORS_LIS3DH) += inv_mpu_lis3dh.o
+inv_mpu_lis3dh-objs += lis3dh.o
+
+obj-$(CONFIG_MPU_SENSORS_LSM303DLX_A) += inv_mpu_lsm303dlx_a.o
+inv_mpu_lsm303dlx_a-objs += lsm303dlx_a.o
+
+obj-$(CONFIG_MPU_SENSORS_MMA8450) += inv_mpu_mma8450.o
+inv_mpu_mma8450-objs += mma8450.o
+
+obj-$(CONFIG_MPU_SENSORS_MMA845X) += inv_mpu_mma845x.o
+inv_mpu_mma845x-objs += mma845x.o
+
+EXTRA_CFLAGS += -Idrivers/misc/inv_mpu
+EXTRA_CFLAGS += -D__C99_DESIGNATED_INITIALIZER
diff --git a/drivers/misc/inv_mpu/accel/adxl34x.c b/drivers/misc/inv_mpu/accel/adxl34x.c
new file mode 100644
index 000000000000..f2bff8a75925
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/adxl34x.c
@@ -0,0 +1,728 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   adxl34x.c
+ *      @brief  Accelerometer setup and handling methods for AD adxl345 and
+ *              adxl346.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* -------------------------------------------------------------------------- */
+
+/* registers */
+#define ADXL34X_ODR_REG			(0x2C)
+#define ADXL34X_PWR_REG			(0x2D)
+#define ADXL34X_DATAFORMAT_REG		(0x31)
+
+/* masks */
+#define ADXL34X_ODR_MASK		(0x0F)
+#define ADXL34X_PWR_SLEEP_MASK		(0x04)
+#define ADXL34X_PWR_MEAS_MASK		(0x08)
+#define ADXL34X_DATAFORMAT_JUSTIFY_MASK	(0x04)
+#define ADXL34X_DATAFORMAT_FSR_MASK	(0x03)
+
+/* -------------------------------------------------------------------------- */
+
+struct adxl34x_config {
+	unsigned int odr;		/** < output data rate in mHz */
+	unsigned int fsr;		/** < full scale range mg */
+	unsigned int fsr_reg_mask;	/** < register setting for fsr */
+};
+
+struct adxl34x_private_data {
+	struct adxl34x_config suspend;	/** < suspend configuration */
+	struct adxl34x_config resume;	/** < resume configuration */
+};
+
+/**
+ *  @brief Set the output data rate for the particular configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             Config to modify with new ODR.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param odr
+ *             Output data rate in units of 1/1000Hz (mHz).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_set_odr(void *mlsl_handle,
+			   struct ext_slave_platform_data *pdata,
+			   struct adxl34x_config *config,
+			   int apply,
+			   long odr)
+{
+	int result = INV_SUCCESS;
+	unsigned char new_odr_mask;
+
+	/* ADXL346 (Rev. A) pages 13, 24 */
+	if (odr >= 3200000) {
+		new_odr_mask = 0x0F;
+		config->odr = 3200000;
+	} else if (odr >= 1600000) {
+		new_odr_mask = 0x0E;
+		config->odr = 1600000;
+	} else if (odr >= 800000) {
+		new_odr_mask = 0x0D;
+		config->odr = 800000;
+	} else if (odr >= 400000) {
+		new_odr_mask = 0x0C;
+		config->odr = 400000;
+	} else if (odr >= 200000) {
+		new_odr_mask = 0x0B;
+		config->odr = 200000;
+	} else if (odr >= 100000) {
+		new_odr_mask = 0x0A;
+		config->odr = 100000;
+	} else if (odr >= 50000) {
+		new_odr_mask = 0x09;
+		config->odr = 50000;
+	} else if (odr >= 25000) {
+		new_odr_mask = 0x08;
+		config->odr = 25000;
+	} else if (odr >= 12500) {
+		new_odr_mask = 0x07;
+		config->odr = 12500;
+	} else if (odr >= 6250) {
+		new_odr_mask = 0x06;
+		config->odr = 6250;
+	} else if (odr >= 3130) {
+		new_odr_mask = 0x05;
+		config->odr = 3130;
+	} else if (odr >= 1560) {
+		new_odr_mask = 0x04;
+		config->odr = 1560;
+	} else if (odr >= 780) {
+		new_odr_mask = 0x03;
+		config->odr = 780;
+	} else if (odr >= 390) {
+		new_odr_mask = 0x02;
+		config->odr = 390;
+	} else if (odr >= 200) {
+		new_odr_mask = 0x01;
+		config->odr = 200;
+	} else {
+		new_odr_mask = 0x00;
+		config->odr = 100;
+	}
+
+	if (apply) {
+		unsigned char reg_odr;
+		result = inv_serial_read(mlsl_handle, pdata->address,
+				ADXL34X_ODR_REG, 1, &reg_odr);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		reg_odr &= ~ADXL34X_ODR_MASK;
+		reg_odr |= new_odr_mask;
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				ADXL34X_ODR_REG, reg_odr);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		MPL_LOGV("ODR: %d mHz\n", config->odr);
+	}
+	return result;
+}
+
+/**
+ *  @brief Set the full scale range of the accels
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             pointer to configuration.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param fsr
+ *             requested full scale range in milli gees (mg).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_set_fsr(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct adxl34x_config *config,
+			  int apply,
+			  long fsr)
+{
+	int result = INV_SUCCESS;
+
+	if (fsr <= 2000) {
+		config->fsr_reg_mask = 0x00;
+		config->fsr = 2000;
+	} else if (fsr <= 4000) {
+		config->fsr_reg_mask = 0x01;
+		config->fsr = 4000;
+	} else if (fsr <= 8000) {
+		config->fsr_reg_mask = 0x02;
+		config->fsr = 8000;
+	} else { /* 8001 -> oo */
+		config->fsr_reg_mask = 0x03;
+		config->fsr = 16000;
+	}
+
+	if (apply) {
+		unsigned char reg_df;
+		result = inv_serial_read(mlsl_handle, pdata->address,
+				ADXL34X_DATAFORMAT_REG, 1, &reg_df);
+		reg_df &= ~ADXL34X_DATAFORMAT_FSR_MASK;
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				ADXL34X_DATAFORMAT_REG,
+				reg_df | config->fsr_reg_mask);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		MPL_LOGV("FSR: %d mg\n", config->fsr);
+	}
+	return result;
+}
+
+/**
+ *  @brief facility to retrieve the device configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to store the returned configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_get_config(void *mlsl_handle,
+		    struct ext_slave_descr *slave,
+				struct ext_slave_platform_data *pdata,
+				struct ext_slave_config *data)
+{
+	struct adxl34x_private_data *private_data =
+			(struct adxl34x_private_data *)(pdata->private_data);
+
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.odr;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.odr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief device configuration facility.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to the configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_config(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata,
+			 struct ext_slave_config *data)
+{
+	struct adxl34x_private_data *private_data =
+			(struct adxl34x_private_data *)(pdata->private_data);
+
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		return adxl34x_set_odr(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		return adxl34x_set_odr(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		return adxl34x_set_fsr(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		return adxl34x_set_fsr(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief suspends the device to put it in its lowest power mode.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_suspend(void *mlsl_handle,
+			   struct ext_slave_descr *slave,
+			   struct ext_slave_platform_data *pdata)
+{
+	int result;
+
+	/*
+	struct adxl34x_config *suspend_config =
+		&((struct adxl34x_private_data *)pdata->private_data)->suspend;
+
+	result = adxl34x_set_odr(mlsl_handle, pdata, suspend_config,
+				true, suspend_config->odr);
+	if (result) {
+	LOG_RESULT_LOCATION(result);
+	return result;
+}
+	result = adxl34x_set_fsr(mlsl_handle, pdata, suspend_config,
+				true, suspend_config->fsr);
+	if (result) {
+	LOG_RESULT_LOCATION(result);
+	return result;
+}
+	*/
+
+	/*
+	  Page 25
+	  When clearing the sleep bit, it is recommended that the part
+	  be placed into standby mode and then set back to measurement mode
+	  with a subsequent write.
+	  This is done to ensure that the device is properly biased if sleep
+	  mode is manually disabled; otherwise, the first few samples of data
+	  after the sleep bit is cleared may have additional noise,
+	  especially if the device was asleep when the bit was cleared. */
+
+	/* go in standy-by mode (suspends measurements) */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			ADXL34X_PWR_REG, ADXL34X_PWR_MEAS_MASK);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* and then in sleep */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			ADXL34X_PWR_REG,
+			ADXL34X_PWR_MEAS_MASK | ADXL34X_PWR_SLEEP_MASK);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+/**
+ *  @brief resume the device in the proper power state given the configuration
+ *         chosen.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_resume(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	struct adxl34x_config *resume_config =
+		&((struct adxl34x_private_data *)pdata->private_data)->resume;
+	unsigned char reg;
+
+	/*
+	  Page 25
+	  When clearing the sleep bit, it is recommended that the part
+	  be placed into standby mode and then set back to measurement mode
+	  with a subsequent write.
+	  This is done to ensure that the device is properly biased if sleep
+	  mode is manually disabled; otherwise, the first few samples of data
+	  after the sleep bit is cleared may have additional noise,
+	  especially if the device was asleep when the bit was cleared. */
+
+	/* remove sleep, but leave in stand-by */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			ADXL34X_PWR_REG, ADXL34X_PWR_MEAS_MASK);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = adxl34x_set_odr(mlsl_handle, pdata, resume_config,
+				true, resume_config->odr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/*
+	  -> FSR
+	  -> Justiy bit for Big endianess
+	  -> resulution to 10 bits
+	*/
+	reg = ADXL34X_DATAFORMAT_JUSTIFY_MASK;
+	reg |= resume_config->fsr_reg_mask;
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			ADXL34X_DATAFORMAT_REG, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* go in measurement mode */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			ADXL34X_PWR_REG, 0x00);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* DATA_FORMAT: full resolution of +/-2g; data is left justified */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 0x31, reg);
+
+	return result;
+}
+
+/**
+ *  @brief one-time device driver initialization function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is loaded in the kernel.
+ *         If the driver is built-in in the kernel, this function will be
+ *         called at boot time.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_init(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata)
+{
+	int result;
+	long range;
+
+	struct adxl34x_private_data *private_data;
+	private_data = (struct adxl34x_private_data *)
+	    kzalloc(sizeof(struct adxl34x_private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	pdata->private_data = private_data;
+
+	result = adxl34x_set_odr(mlsl_handle, pdata, &private_data->suspend,
+				false, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = adxl34x_set_odr(mlsl_handle, pdata, &private_data->resume,
+				false, 200000);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	range = range_fixedpoint_to_long_mg(slave->range);
+	result = adxl34x_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+				false, range);
+	result = adxl34x_set_fsr(mlsl_handle, pdata, &private_data->resume,
+				false, range);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = adxl34x_suspend(mlsl_handle, slave, pdata);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+/**
+ *  @brief one-time device driver exit function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is removed from the kernel.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_exit(void *mlsl_handle,
+		 struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief read the sensor data from the device.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a buffer to store the data read.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int adxl34x_read(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			unsigned char *data)
+{
+	int result;
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 slave->read_reg, slave->read_len, data);
+	return result;
+}
+
+static struct ext_slave_descr adxl34x_descr = {
+	.init             = adxl34x_init,
+	.exit             = adxl34x_exit,
+	.suspend          = adxl34x_suspend,
+	.resume           = adxl34x_resume,
+	.read             = adxl34x_read,
+	.config           = adxl34x_config,
+	.get_config       = adxl34x_get_config,
+	.name             = "adxl34x",	/* 5 or 6 */
+	.type             = EXT_SLAVE_TYPE_ACCEL,
+	.id               = ACCEL_ID_ADXL34X,
+	.read_reg         = 0x32,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_LITTLE_ENDIAN,
+	.range            = {2, 0},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *adxl34x_get_slave_descr(void)
+{
+	return &adxl34x_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct adxl34x_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int adxl34x_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct adxl34x_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					adxl34x_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int adxl34x_mod_remove(struct i2c_client *client)
+{
+	struct adxl34x_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				adxl34x_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id adxl34x_mod_id[] = {
+	{ "adxl34x", ACCEL_ID_ADXL34X },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, adxl34x_mod_id);
+
+static struct i2c_driver adxl34x_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = adxl34x_mod_probe,
+	.remove = adxl34x_mod_remove,
+	.id_table = adxl34x_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "adxl34x_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init adxl34x_mod_init(void)
+{
+	int res = i2c_add_driver(&adxl34x_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "adxl34x_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit adxl34x_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&adxl34x_mod_driver);
+}
+
+module_init(adxl34x_mod_init);
+module_exit(adxl34x_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate ADXL34X sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("adxl34x_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/bma150.c b/drivers/misc/inv_mpu/accel/bma150.c
new file mode 100644
index 000000000000..745d90a6744f
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/bma150.c
@@ -0,0 +1,777 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   bma150.c
+ *      @brief  Accelerometer setup and handling methods for Bosch BMA150.
+ */
+
+/* -------------------------------------------------------------------------- */
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+
+/* -------------------------------------------------------------------------- */
+/* registers */
+#define BMA150_CTRL_REG			(0x14)
+#define BMA150_INT_REG			(0x15)
+#define BMA150_PWR_REG			(0x0A)
+
+/* masks */
+#define BMA150_CTRL_MASK		(0x18)
+#define BMA150_CTRL_MASK_ODR		(0xF8)
+#define BMA150_CTRL_MASK_FSR		(0xE7)
+#define BMA150_INT_MASK_WUP		(0xF8)
+#define BMA150_INT_MASK_IRQ		(0xDF)
+#define BMA150_PWR_MASK_SLEEP		(0x01)
+#define BMA150_PWR_MASK_SOFT_RESET	(0x02)
+
+/* -------------------------------------------------------------------------- */
+struct bma150_config {
+	unsigned int odr;	/** < output data rate mHz */
+	unsigned int fsr;	/** < full scale range mgees */
+	unsigned int irq_type;	/** < type of IRQ, see bma150_set_irq */
+	unsigned char ctrl_reg;	/** < control register value */
+	unsigned char int_reg;	/** < interrupt control register value */
+};
+
+struct bma150_private_data {
+	struct bma150_config suspend;	/** < suspend configuration */
+	struct bma150_config resume;	/** < resume configuration */
+};
+
+/**
+ *  @brief Simply disables the IRQ since it is not usable on BMA150 devices.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *              configuration to apply to, suspend or resume
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param irq_type
+ *              the type of IRQ.  Valid values are
+ *              - MPU_SLAVE_IRQ_TYPE_NONE
+ *              - MPU_SLAVE_IRQ_TYPE_MOTION
+ *              - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ *              The only supported IRQ type is MPU_SLAVE_IRQ_TYPE_NONE which
+ *              corresponds to disabling the IRQ completely.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_set_irq(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct bma150_config *config,
+			  int apply,
+			  long irq_type)
+{
+	int result = INV_SUCCESS;
+
+	if (irq_type != MPU_SLAVE_IRQ_TYPE_NONE)
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+
+	config->irq_type = MPU_SLAVE_IRQ_TYPE_NONE;
+	config->int_reg = 0x00;
+
+	if (apply) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+			 BMA150_CTRL_REG, config->ctrl_reg);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+			 BMA150_INT_REG, config->int_reg);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	return result;
+}
+
+/**
+ *  @brief Set the output data rate for the particular configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             Config to modify with new ODR.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param odr
+ *             Output data rate in units of 1/1000Hz (mHz).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_set_odr(void *mlsl_handle,
+			struct ext_slave_platform_data *pdata,
+			struct bma150_config *config,
+			int apply,
+			long odr)
+{
+	unsigned char odr_bits = 0;
+	unsigned char wup_bits = 0;
+	int result = INV_SUCCESS;
+
+	if (odr > 100000) {
+		config->odr = 190000;
+		odr_bits = 0x03;
+	} else if (odr > 50000) {
+		config->odr = 100000;
+		odr_bits = 0x02;
+	} else if (odr > 25000) {
+		config->odr = 50000;
+		odr_bits = 0x01;
+	} else if (odr > 0) {
+		config->odr = 25000;
+		odr_bits = 0x00;
+	} else {
+		config->odr = 0;
+		wup_bits = 0x00;
+	}
+
+	config->int_reg &= BMA150_INT_MASK_WUP;
+	config->ctrl_reg &= BMA150_CTRL_MASK_ODR;
+	config->ctrl_reg |= odr_bits;
+
+	MPL_LOGV("ODR: %d\n", config->odr);
+	if (apply) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					BMA150_CTRL_REG, config->ctrl_reg);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					BMA150_INT_REG, config->int_reg);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	return result;
+}
+
+/**
+ *  @brief Set the full scale range of the accels
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             pointer to configuration.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param fsr
+ *             requested full scale range.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_set_fsr(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct bma150_config *config,
+			  int apply,
+			  long fsr)
+{
+	unsigned char fsr_bits;
+	int result = INV_SUCCESS;
+
+	if (fsr <= 2048) {
+		fsr_bits = 0x00;
+		config->fsr = 2048;
+	} else if (fsr <= 4096) {
+		fsr_bits = 0x08;
+		config->fsr = 4096;
+	} else {
+		fsr_bits = 0x10;
+		config->fsr = 8192;
+	}
+
+	config->ctrl_reg &= BMA150_CTRL_MASK_FSR;
+	config->ctrl_reg |= fsr_bits;
+
+	MPL_LOGV("FSR: %d\n", config->fsr);
+	if (apply) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA150_CTRL_REG, config->ctrl_reg);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA150_CTRL_REG, config->ctrl_reg);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	return result;
+}
+
+/**
+ *  @brief one-time device driver initialization function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is loaded in the kernel.
+ *         If the driver is built-in in the kernel, this function will be
+ *         called at boot time.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_init(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char reg;
+	long range;
+
+	struct bma150_private_data *private_data;
+	private_data = (struct bma150_private_data *)
+	    kzalloc(sizeof(struct bma150_private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	pdata->private_data = private_data;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			BMA150_PWR_REG, BMA150_PWR_MASK_SOFT_RESET);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(1);
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 BMA150_CTRL_REG, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	private_data->resume.ctrl_reg = reg;
+	private_data->suspend.ctrl_reg = reg;
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 BMA150_INT_REG, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	private_data->resume.int_reg = reg;
+	private_data->suspend.int_reg = reg;
+
+	result = bma150_set_odr(mlsl_handle, pdata, &private_data->suspend,
+				false, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = bma150_set_odr(mlsl_handle, pdata, &private_data->resume,
+				false, 200000);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	range = range_fixedpoint_to_long_mg(slave->range);
+	result = bma150_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+				false, range);
+	result = bma150_set_fsr(mlsl_handle, pdata, &private_data->resume,
+				false, range);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = bma150_set_irq(mlsl_handle, pdata, &private_data->suspend,
+				false, MPU_SLAVE_IRQ_TYPE_NONE);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = bma150_set_irq(mlsl_handle, pdata, &private_data->resume,
+				false, MPU_SLAVE_IRQ_TYPE_NONE);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       BMA150_PWR_REG, BMA150_PWR_MASK_SLEEP);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+/**
+ *  @brief one-time device driver exit function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is removed from the kernel.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_exit(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief device configuration facility.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to the configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_config(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata,
+			 struct ext_slave_config *data)
+{
+	struct bma150_private_data *private_data =
+			(struct bma150_private_data *)(pdata->private_data);
+
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		return bma150_set_odr(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		return bma150_set_odr(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		return bma150_set_fsr(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		return bma150_set_fsr(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		return bma150_set_irq(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		return bma150_set_irq(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply,
+				      *((long *)data->data));
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief facility to retrieve the device configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to store the returned configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_get_config(void *mlsl_handle,
+				struct ext_slave_descr *slave,
+				struct ext_slave_platform_data *pdata,
+				struct ext_slave_config *data)
+{
+	struct bma150_private_data *private_data =
+			(struct bma150_private_data *)(pdata->private_data);
+
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.odr;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.odr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.irq_type;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.irq_type;
+		break;
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief suspends the device to put it in its lowest power mode.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_suspend(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char ctrl_reg;
+	unsigned char int_reg;
+
+	struct bma150_private_data *private_data =
+			(struct bma150_private_data *)(pdata->private_data);
+
+	ctrl_reg = private_data->suspend.ctrl_reg;
+	int_reg = private_data->suspend.int_reg;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			BMA150_PWR_REG, BMA150_PWR_MASK_SOFT_RESET);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(1);
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			BMA150_CTRL_REG, ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			BMA150_INT_REG, int_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+		BMA150_PWR_REG, BMA150_PWR_MASK_SLEEP);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+/**
+ *  @brief resume the device in the proper power state given the configuration
+ *         chosen.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_resume(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char ctrl_reg;
+	unsigned char int_reg;
+
+	struct bma150_private_data *private_data =
+			(struct bma150_private_data *)(pdata->private_data);
+
+	ctrl_reg = private_data->resume.ctrl_reg;
+	int_reg = private_data->resume.int_reg;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA150_PWR_REG, BMA150_PWR_MASK_SOFT_RESET);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(1);
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA150_CTRL_REG, ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA150_INT_REG, int_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA150_PWR_REG, 0x00);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+/**
+ *  @brief read the sensor data from the device.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a buffer to store the data read.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma150_read(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data)
+{
+	return inv_serial_read(mlsl_handle, pdata->address,
+			       slave->read_reg, slave->read_len, data);
+}
+
+static struct ext_slave_descr bma150_descr = {
+	.init             = bma150_init,
+	.exit             = bma150_exit,
+	.suspend          = bma150_suspend,
+	.resume           = bma150_resume,
+	.read             = bma150_read,
+	.config           = bma150_config,
+	.get_config       = bma150_get_config,
+	.name             = "bma150",
+	.type             = EXT_SLAVE_TYPE_ACCEL,
+	.id               = ACCEL_ID_BMA150,
+	.read_reg         = 0x02,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_LITTLE_ENDIAN,
+	.range            = {2, 0},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *bma150_get_slave_descr(void)
+{
+	return &bma150_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+
+/* Platform data for the MPU */
+struct bma150_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int bma150_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct bma150_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					bma150_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int bma150_mod_remove(struct i2c_client *client)
+{
+	struct bma150_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				bma150_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id bma150_mod_id[] = {
+	{ "bma150", ACCEL_ID_BMA150 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, bma150_mod_id);
+
+static struct i2c_driver bma150_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = bma150_mod_probe,
+	.remove = bma150_mod_remove,
+	.id_table = bma150_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "bma150_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init bma150_mod_init(void)
+{
+	int res = i2c_add_driver(&bma150_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "bma150_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit bma150_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&bma150_mod_driver);
+}
+
+module_init(bma150_mod_init);
+module_exit(bma150_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate BMA150 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("bma150_mod");
+
+/**
+ *  @}
+ */
+
diff --git a/drivers/misc/inv_mpu/accel/bma222.c b/drivers/misc/inv_mpu/accel/bma222.c
new file mode 100644
index 000000000000..e9fc99b1a62d
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/bma222.c
@@ -0,0 +1,654 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/*
+ *  @addtogroup ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   bma222.c
+ *      @brief  Accelerometer setup and handling methods for Bosch BMA222.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+
+/* -------------------------------------------------------------------------- */
+
+#define BMA222_STATUS_REG	(0x0A)
+#define BMA222_FSR_REG		(0x0F)
+#define ADXL34X_ODR_REG		(0x10)
+#define BMA222_PWR_REG		(0x11)
+#define BMA222_SOFTRESET_REG	(0x14)
+
+#define BMA222_STATUS_RDY_MASK	(0x80)
+#define BMA222_FSR_MASK		(0x0F)
+#define BMA222_ODR_MASK		(0x1F)
+#define BMA222_PWR_SLEEP_MASK	(0x80)
+#define BMA222_PWR_AWAKE_MASK	(0x00)
+#define BMA222_SOFTRESET_MASK	(0xB6)
+#define BMA222_SOFTRESET_MASK	(0xB6)
+
+/* -------------------------------------------------------------------------- */
+
+struct bma222_config {
+	unsigned int odr;		/** < output data rate in mHz */
+	unsigned int fsr;		/** < full scale range mg */
+};
+
+struct bma222_private_data {
+	struct bma222_config suspend;	/** < suspend configuration */
+	struct bma222_config resume;	/** < resume configuration */
+};
+
+
+/* -------------------------------------------------------------------------- */
+
+/**
+ *  @brief Set the output data rate for the particular configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             Config to modify with new ODR.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param odr
+ *             Output data rate in units of 1/1000Hz (mHz).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_set_odr(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct bma222_config *config,
+			  int apply,
+			  long odr)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg_odr;
+
+	if (odr >= 1000000) {
+		reg_odr = 0x0F;
+		config->odr = 1000000;
+	} else if (odr >= 500000) {
+		reg_odr = 0x0E;
+		config->odr = 500000;
+	} else if (odr >= 250000) {
+		reg_odr = 0x0D;
+		config->odr = 250000;
+	} else if (odr >= 125000) {
+		reg_odr = 0x0C;
+		config->odr = 125000;
+	} else if (odr >= 62500) {
+		reg_odr = 0x0B;
+		config->odr = 62500;
+	} else if (odr >= 32000) {
+		reg_odr = 0x0A;
+		config->odr = 32000;
+	} else if (odr >= 16000) {
+		reg_odr = 0x09;
+		config->odr = 16000;
+	} else {
+		reg_odr = 0x08;
+		config->odr = 8000;
+	}
+
+	if (apply) {
+		MPL_LOGV("ODR: %d\n", config->odr);
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					ADXL34X_ODR_REG, reg_odr);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	return result;
+}
+
+/**
+ *  @brief Set the full scale range of the accels
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             pointer to configuration.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param fsr
+ *             requested full scale range.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_set_fsr(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct bma222_config *config,
+			  int apply,
+			  long fsr)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg_fsr_mask;
+
+	if (fsr <= 2000) {
+		reg_fsr_mask = 0x03;
+		config->fsr = 2000;
+	} else if (fsr <= 4000) {
+		reg_fsr_mask = 0x05;
+		config->fsr = 4000;
+	} else if (fsr <= 8000) {
+		reg_fsr_mask = 0x08;
+		config->fsr = 8000;
+	} else { /* 8001 -> oo */
+		reg_fsr_mask = 0x0C;
+		config->fsr = 16000;
+	}
+
+	if (apply) {
+		MPL_LOGV("FSR: %d\n", config->fsr);
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA222_FSR_REG, reg_fsr_mask);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	return result;
+}
+
+/**
+ *  @brief one-time device driver initialization function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is loaded in the kernel.
+ *         If the driver is built-in in the kernel, this function will be
+ *         called at boot time.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_init(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	int result;
+
+	struct bma222_private_data *private_data;
+	private_data = (struct bma222_private_data *)
+	    kzalloc(sizeof(struct bma222_private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	pdata->private_data = private_data;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			BMA222_SOFTRESET_REG, BMA222_SOFTRESET_MASK);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(1);
+
+	result = bma222_set_odr(mlsl_handle, pdata, &private_data->suspend,
+				false, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = bma222_set_odr(mlsl_handle, pdata, &private_data->resume,
+				false, 200000);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = bma222_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+				false, 2000);
+	result = bma222_set_fsr(mlsl_handle, pdata, &private_data->resume,
+				false, 2000);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA222_PWR_REG, BMA222_PWR_SLEEP_MASK);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+/**
+ *  @brief one-time device driver exit function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is removed from the kernel.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_exit(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	return INV_SUCCESS;
+}
+
+
+/**
+ *  @brief facility to retrieve the device configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to store the returned configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_get_config(void *mlsl_handle,
+				struct ext_slave_descr *slave,
+				struct ext_slave_platform_data *pdata,
+				struct ext_slave_config *data)
+{
+	struct bma222_private_data *private_data =
+			(struct bma222_private_data *)(pdata->private_data);
+
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.odr;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.odr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief device configuration facility.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to the configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_config(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata,
+			 struct ext_slave_config *data)
+{
+	struct bma222_private_data *private_data =
+			(struct bma222_private_data *)(pdata->private_data);
+
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		return bma222_set_odr(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		return bma222_set_odr(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		return bma222_set_fsr(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		return bma222_set_fsr(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief suspends the device to put it in its lowest power mode.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_suspend(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	int result;
+	struct bma222_config *suspend_config =
+		&((struct bma222_private_data *)pdata->private_data)->suspend;
+
+	result = bma222_set_odr(mlsl_handle, pdata, suspend_config,
+				true, suspend_config->odr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = bma222_set_fsr(mlsl_handle, pdata, suspend_config,
+				true, suspend_config->fsr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA222_PWR_REG, BMA222_PWR_SLEEP_MASK);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	msleep(3); /* 3 ms powerup time maximum */
+	return result;
+}
+
+/**
+ *  @brief resume the device in the proper power state given the configuration
+ *         chosen.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_resume(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	struct bma222_config *resume_config =
+		&((struct bma222_private_data *)pdata->private_data)->resume;
+
+	/* Soft reset */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			BMA222_SOFTRESET_REG, BMA222_SOFTRESET_MASK);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(10);
+
+	result = bma222_set_odr(mlsl_handle, pdata, resume_config,
+				true, resume_config->odr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = bma222_set_fsr(mlsl_handle, pdata, resume_config,
+				true, resume_config->fsr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+/**
+ *  @brief read the sensor data from the device.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a buffer to store the data read.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma222_read(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data)
+{
+	int result = INV_SUCCESS;
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				BMA222_STATUS_REG, 1, data);
+	if (data[0] & BMA222_STATUS_RDY_MASK) {
+		result = inv_serial_read(mlsl_handle, pdata->address,
+				 slave->read_reg, slave->read_len, data);
+	return result;
+	} else
+		return INV_ERROR_ACCEL_DATA_NOT_READY;
+}
+
+static struct ext_slave_descr bma222_descr = {
+	.init             = bma222_init,
+	.exit             = bma222_exit,
+	.suspend          = bma222_suspend,
+	.resume           = bma222_resume,
+	.read             = bma222_read,
+	.config           = bma222_config,
+	.get_config       = bma222_get_config,
+	.name             = "bma222",
+	.type             = EXT_SLAVE_TYPE_ACCEL,
+	.id               = ACCEL_ID_BMA222,
+	.read_reg         = 0x02,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_LITTLE_ENDIAN,
+	.range            = {2, 0},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *bma222_get_slave_descr(void)
+{
+	return &bma222_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+
+struct bma222_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int bma222_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct bma222_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					bma222_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int bma222_mod_remove(struct i2c_client *client)
+{
+	struct bma222_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				bma222_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id bma222_mod_id[] = {
+	{ "bma222", ACCEL_ID_BMA222 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, bma222_mod_id);
+
+static struct i2c_driver bma222_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = bma222_mod_probe,
+	.remove = bma222_mod_remove,
+	.id_table = bma222_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "bma222_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init bma222_mod_init(void)
+{
+	int res = i2c_add_driver(&bma222_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "bma222_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit bma222_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&bma222_mod_driver);
+}
+
+module_init(bma222_mod_init);
+module_exit(bma222_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate BMA222 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("bma222_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/bma250.c b/drivers/misc/inv_mpu/accel/bma250.c
new file mode 100644
index 000000000000..6a245f4566aa
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/bma250.c
@@ -0,0 +1,787 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   bma250.c
+ *      @brief  Accelerometer setup and handling methods for Bosch BMA250.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+
+/* -------------------------------------------------------------------------- */
+
+/* registers */
+#define BMA250_STATUS_REG	(0x0A)
+#define BMA250_FSR_REG		(0x0F)
+#define BMA250_ODR_REG		(0x10)
+#define BMA250_PWR_REG		(0x11)
+#define BMA250_SOFTRESET_REG	(0x14)
+#define BMA250_INT_TYPE_REG	(0x17)
+#define BMA250_INT_DST_REG	(0x1A)
+#define BMA250_INT_SRC_REG	(0x1E)
+
+/* masks */
+#define BMA250_STATUS_RDY_MASK	(0x80)
+#define BMA250_FSR_MASK		(0x0F)
+#define BMA250_ODR_MASK		(0x1F)
+#define BMA250_PWR_SLEEP_MASK	(0x80)
+#define BMA250_PWR_AWAKE_MASK	(0x00)
+#define BMA250_SOFTRESET_MASK   (0xB6)
+#define BMA250_INT_TYPE_MASK	(0x10)
+#define BMA250_INT_DST_1_MASK	(0x01)
+#define BMA250_INT_DST_2_MASK	(0x80)
+#define BMA250_INT_SRC_MASK	(0x00)
+
+/* -------------------------------------------------------------------------- */
+
+struct bma250_config {
+	unsigned int odr;		/** < output data rate in mHz */
+	unsigned int fsr;		/** < full scale range mg */
+	unsigned char irq_type;
+};
+
+struct bma250_private_data {
+	struct bma250_config suspend;	/** < suspend configuration */
+	struct bma250_config resume;	/** < resume configuration */
+};
+
+/* -------------------------------------------------------------------------- */
+/**
+ *  @brief Sets the IRQ to fire when one of the IRQ events occur.
+ *         Threshold and duration will not be used unless the type is MOT or
+ *         NMOT.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *              configuration to apply to, suspend or resume
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param irq_type
+ *              the type of IRQ.  Valid values are
+ * - MPU_SLAVE_IRQ_TYPE_NONE
+ * - MPU_SLAVE_IRQ_TYPE_MOTION
+ * - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_set_irq(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct bma250_config *config,
+			  int apply, long irq_type)
+{
+	int result = INV_SUCCESS;
+	unsigned char irqtype_reg;
+	unsigned char irqdst_reg;
+	unsigned char irqsrc_reg;
+
+	switch (irq_type) {
+	case MPU_SLAVE_IRQ_TYPE_DATA_READY:
+		/* data ready int. */
+		irqtype_reg = BMA250_INT_TYPE_MASK;
+		/* routed to interrupt pin 1 */
+		irqdst_reg = BMA250_INT_DST_1_MASK;
+		/* from filtered data */
+		irqsrc_reg = BMA250_INT_SRC_MASK;
+		break;
+	/* unfinished
+	case MPU_SLAVE_IRQ_TYPE_MOTION:
+		reg1 = 0x00;
+		reg2 = config->mot_int1_cfg;
+		reg3 = ;
+		break;
+	*/
+	case MPU_SLAVE_IRQ_TYPE_NONE:
+		irqtype_reg = 0x00;
+		irqdst_reg = 0x00;
+		irqsrc_reg = 0x00;
+		break;
+	default:
+		return INV_ERROR_INVALID_PARAMETER;
+		break;
+	}
+
+	config->irq_type = (unsigned char)irq_type;
+
+	if (apply) {
+		/* select the type of interrupt to use */
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA250_INT_TYPE_REG, irqtype_reg);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		/* select to which interrupt pin to route it to */
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA250_INT_DST_REG, irqdst_reg);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		/* select whether the interrupt works off filtered or
+		   unfiltered data */
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA250_INT_SRC_REG, irqsrc_reg);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	return result;
+}
+
+/**
+ *  @brief Set the output data rate for the particular configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             Config to modify with new ODR.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param odr
+ *             Output data rate in units of 1/1000Hz (mHz).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_set_odr(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct bma250_config *config,
+			  int apply,
+			  long odr)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg_odr;
+
+	/* Table uses bandwidth which is half the sample rate */
+	odr = odr >> 1;
+	if (odr >= 1000000) {
+		reg_odr = 0x0F;
+		config->odr = 2000000;
+	} else if (odr >= 500000) {
+		reg_odr = 0x0E;
+		config->odr = 1000000;
+	} else if (odr >= 250000) {
+		reg_odr = 0x0D;
+		config->odr = 500000;
+	} else if (odr >= 125000) {
+		reg_odr = 0x0C;
+		config->odr = 250000;
+	} else if (odr >= 62500) {
+		reg_odr = 0x0B;
+		config->odr = 125000;
+	} else if (odr >= 31250) {
+		reg_odr = 0x0A;
+		config->odr = 62500;
+	} else if (odr >= 15630) {
+		reg_odr = 0x09;
+		config->odr = 31250;
+	} else {
+		reg_odr = 0x08;
+		config->odr = 15630;
+	}
+
+	if (apply) {
+		MPL_LOGV("ODR: %d\n", config->odr);
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA250_ODR_REG, reg_odr);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	return result;
+}
+
+/**
+ *  @brief Set the full scale range of the accels
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             pointer to configuration.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param fsr
+ *             requested full scale range.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_set_fsr(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct bma250_config *config,
+			  int apply,
+			  long fsr)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg_fsr_mask;
+
+	if (fsr <= 2000) {
+		reg_fsr_mask = 0x03;
+		config->fsr = 2000;
+	} else if (fsr <= 4000) {
+		reg_fsr_mask = 0x05;
+		config->fsr = 4000;
+	} else if (fsr <= 8000) {
+		reg_fsr_mask = 0x08;
+		config->fsr = 8000;
+	} else { /* 8001 -> oo */
+		reg_fsr_mask = 0x0C;
+		config->fsr = 16000;
+	}
+
+	if (apply) {
+		MPL_LOGV("FSR: %d\n", config->fsr);
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA250_FSR_REG, reg_fsr_mask);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	return result;
+}
+
+/**
+ *  @brief one-time device driver initialization function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is loaded in the kernel.
+ *         If the driver is built-in in the kernel, this function will be
+ *         called at boot time.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_init(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	long range;
+
+	struct bma250_private_data *private_data;
+	private_data = kzalloc(sizeof(struct bma250_private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	pdata->private_data = private_data;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			BMA250_SOFTRESET_REG, BMA250_SOFTRESET_MASK);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(1);
+
+	result = bma250_set_odr(mlsl_handle, pdata, &private_data->suspend,
+				false, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = bma250_set_odr(mlsl_handle, pdata, &private_data->resume,
+				false, 200000);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	range = range_fixedpoint_to_long_mg(slave->range);
+	result = bma250_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+				false, range);
+	result = bma250_set_fsr(mlsl_handle, pdata, &private_data->resume,
+				false, range);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = bma250_set_irq(mlsl_handle, pdata, &private_data->suspend,
+				false, MPU_SLAVE_IRQ_TYPE_NONE);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = bma250_set_irq(mlsl_handle, pdata, &private_data->resume,
+				false, MPU_SLAVE_IRQ_TYPE_NONE);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA250_PWR_REG, BMA250_PWR_SLEEP_MASK);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+/**
+ *  @brief one-time device driver exit function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is removed from the kernel.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_exit(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief device configuration facility.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to the configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_config(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata,
+			 struct ext_slave_config *data)
+{
+	struct bma250_private_data *private_data =
+			(struct bma250_private_data *)(pdata->private_data);
+
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		return bma250_set_odr(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		return bma250_set_odr(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		return bma250_set_fsr(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		return bma250_set_fsr(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		return bma250_set_irq(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply,
+				      *((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		return bma250_set_irq(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply,
+				      *((long *)data->data));
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief facility to retrieve the device configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to store the returned configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_get_config(void *mlsl_handle,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata,
+			     struct ext_slave_config *data)
+{
+	struct bma250_private_data *private_data =
+			(struct bma250_private_data *)(pdata->private_data);
+
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.odr;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.odr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.irq_type;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.irq_type;
+		break;
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief suspends the device to put it in its lowest power mode.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_suspend(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	int result;
+	struct bma250_config *suspend_config =
+		&((struct bma250_private_data *)pdata->private_data)->suspend;
+
+	result = bma250_set_odr(mlsl_handle, pdata, suspend_config,
+				true, suspend_config->odr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = bma250_set_fsr(mlsl_handle, pdata, suspend_config,
+				true, suspend_config->fsr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = bma250_set_irq(mlsl_handle, pdata, suspend_config,
+				true, suspend_config->irq_type);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				BMA250_PWR_REG, BMA250_PWR_SLEEP_MASK);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	msleep(3); /* 3 ms powerup time maximum */
+	return result;
+}
+
+/**
+ *  @brief resume the device in the proper power state given the configuration
+ *         chosen.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_resume(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	struct bma250_config *resume_config =
+		&((struct bma250_private_data *)pdata->private_data)->resume;
+
+	result = bma250_set_odr(mlsl_handle, pdata, resume_config,
+				true, resume_config->odr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = bma250_set_fsr(mlsl_handle, pdata, resume_config,
+				true, resume_config->fsr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = bma250_set_irq(mlsl_handle, pdata, resume_config,
+				true, resume_config->irq_type);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			BMA250_PWR_REG, BMA250_PWR_AWAKE_MASK);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+/**
+ *  @brief read the sensor data from the device.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a buffer to store the data read.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int bma250_read(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data)
+{
+	int result = INV_SUCCESS;
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				BMA250_STATUS_REG, 1, data);
+	if (1) { /* KLP - workaroud for small data ready window */
+		result = inv_serial_read(mlsl_handle, pdata->address,
+				slave->read_reg, slave->read_len, data);
+		return result;
+	} else
+		return INV_ERROR_ACCEL_DATA_NOT_READY;
+}
+
+static struct ext_slave_descr bma250_descr = {
+	.init             = bma250_init,
+	.exit             = bma250_exit,
+	.suspend          = bma250_suspend,
+	.resume           = bma250_resume,
+	.read             = bma250_read,
+	.config           = bma250_config,
+	.get_config       = bma250_get_config,
+	.name             = "bma250",
+	.type             = EXT_SLAVE_TYPE_ACCEL,
+	.id               = ACCEL_ID_BMA250,
+	.read_reg         = 0x02,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_LITTLE_ENDIAN,
+	.range            = {2, 0},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *bma250_get_slave_descr(void)
+{
+	return &bma250_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+
+/* Platform data for the MPU */
+struct bma250_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int bma250_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct bma250_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					bma250_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int bma250_mod_remove(struct i2c_client *client)
+{
+	struct bma250_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				bma250_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id bma250_mod_id[] = {
+	{ "bma250", ACCEL_ID_BMA250 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, bma250_mod_id);
+
+static struct i2c_driver bma250_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = bma250_mod_probe,
+	.remove = bma250_mod_remove,
+	.id_table = bma250_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "bma250_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init bma250_mod_init(void)
+{
+	int res = i2c_add_driver(&bma250_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "bma250_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit bma250_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&bma250_mod_driver);
+}
+
+module_init(bma250_mod_init);
+module_exit(bma250_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate BMA250 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("bma250_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/cma3000.c b/drivers/misc/inv_mpu/accel/cma3000.c
new file mode 100644
index 000000000000..496d1f29bdc0
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/cma3000.c
@@ -0,0 +1,222 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/*
+ *  @addtogroup ACCELDL
+ *  @brief      Accelerometer setup and handling methods for VTI CMA3000.
+ *
+ *  @{
+ *      @file   cma3000.c
+ *      @brief  Accelerometer setup and handling methods for VTI CMA3000
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* -------------------------------------------------------------------------- */
+
+static int cma3000_suspend(void *mlsl_handle,
+			   struct ext_slave_descr *slave,
+			   struct ext_slave_platform_data *pdata)
+{
+	int result;
+	/* RAM reset */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address, 0x1d, 0xcd);
+	return result;
+}
+
+static int cma3000_resume(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+
+	return INV_SUCCESS;
+}
+
+static int cma3000_read(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			unsigned char *data)
+{
+	int result;
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 slave->reg, slave->len, data);
+	return result;
+}
+
+static struct ext_slave_descr cma3000_descr = {
+	.init             = NULL,
+	.exit             = NULL,
+	.suspend          = cma3000_suspend,
+	.resume           = cma3000_resume,
+	.read             = cma3000_read,
+	.config           = NULL,
+	.get_config       = NULL,
+	.name             = "cma3000",
+	.type             = EXT_SLAVE_TYPE_ACCEL,
+	.id               = ID_INVALID,
+	.read_reg         = 0x06,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_LITTLE_ENDIAN,
+	.range            = {2, 0},
+	.trigger          = NULL,
+
+};
+
+static
+struct ext_slave_descr *cma3000_get_slave_descr(void)
+{
+	return &cma3000_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+
+struct cma3000_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int cma3000_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct cma3000_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					cma3000_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int cma3000_mod_remove(struct i2c_client *client)
+{
+	struct cma3000_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				cma3000_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id cma3000_mod_id[] = {
+	{ "cma3000", ACCEL_ID_CMA3000 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, cma3000_mod_id);
+
+static struct i2c_driver cma3000_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = cma3000_mod_probe,
+	.remove = cma3000_mod_remove,
+	.id_table = cma3000_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "cma3000_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init cma3000_mod_init(void)
+{
+	int res = i2c_add_driver(&cma3000_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "cma3000_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit cma3000_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&cma3000_mod_driver);
+}
+
+module_init(cma3000_mod_init);
+module_exit(cma3000_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate CMA3000 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("cma3000_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/kxsd9.c b/drivers/misc/inv_mpu/accel/kxsd9.c
new file mode 100644
index 000000000000..5cb4eaf6b4ab
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/kxsd9.c
@@ -0,0 +1,264 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup ACCELDL
+ *  @brief      Accelerometer setup and handling methods for Kionix KXSD9.
+ *
+ *  @{
+ *      @file   kxsd9.c
+ *      @brief  Accelerometer setup and handling methods for Kionix KXSD9.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* -------------------------------------------------------------------------- */
+
+static int kxsd9_suspend(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	/* CTRL_REGB: low-power standby mode */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address, 0x0d, 0x0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+/* full scale setting - register and mask */
+#define ACCEL_KIONIX_CTRL_REG      (0x0C)
+#define ACCEL_KIONIX_CTRL_MASK     (0x3)
+
+static int kxsd9_resume(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg;
+
+	/* Full Scale */
+	reg = 0x0;
+	reg &= ~ACCEL_KIONIX_CTRL_MASK;
+	reg |= 0x00;
+	if (slave->range.mantissa == 4) {		/* 4g scale = 4.9951 */
+		reg |= 0x2;
+		slave->range.fraction = 9951;
+	} else if (slave->range.mantissa == 7) {	/* 6g scale = 7.5018 */
+		reg |= 0x1;
+		slave->range.fraction = 5018;
+	} else if (slave->range.mantissa == 9) {	/* 8g scale = 9.9902 */
+		reg |= 0x0;
+		slave->range.fraction = 9902;
+	} else {
+		slave->range.mantissa = 2;		/* 2g scale = 2.5006 */
+		slave->range.fraction = 5006;
+		reg |= 0x3;
+	}
+	reg |= 0xC0;		/* 100Hz LPF */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    ACCEL_KIONIX_CTRL_REG, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* normal operation */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address, 0x0d, 0x40);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return INV_SUCCESS;
+}
+
+static int kxsd9_read(void *mlsl_handle,
+		      struct ext_slave_descr *slave,
+		      struct ext_slave_platform_data *pdata,
+		      unsigned char *data)
+{
+	int result;
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 slave->read_reg, slave->read_len, data);
+	return result;
+}
+
+static struct ext_slave_descr kxsd9_descr = {
+	.init             = NULL,
+	.exit             = NULL,
+	.suspend          = kxsd9_suspend,
+	.resume           = kxsd9_resume,
+	.read             = kxsd9_read,
+	.config           = NULL,
+	.get_config       = NULL,
+	.name             = "kxsd9",
+	.type             = EXT_SLAVE_TYPE_ACCEL,
+	.id               = ACCEL_ID_KXSD9,
+	.read_reg         = 0x00,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_BIG_ENDIAN,
+	.range            = {2, 5006},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *kxsd9_get_slave_descr(void)
+{
+	return &kxsd9_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct kxsd9_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int kxsd9_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct kxsd9_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					kxsd9_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int kxsd9_mod_remove(struct i2c_client *client)
+{
+	struct kxsd9_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				kxsd9_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id kxsd9_mod_id[] = {
+	{ "kxsd9", ACCEL_ID_KXSD9 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, kxsd9_mod_id);
+
+static struct i2c_driver kxsd9_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = kxsd9_mod_probe,
+	.remove = kxsd9_mod_remove,
+	.id_table = kxsd9_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "kxsd9_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init kxsd9_mod_init(void)
+{
+	int res = i2c_add_driver(&kxsd9_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "kxsd9_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit kxsd9_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&kxsd9_mod_driver);
+}
+
+module_init(kxsd9_mod_init);
+module_exit(kxsd9_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate KXSD9 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("kxsd9_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/kxtf9.c b/drivers/misc/inv_mpu/accel/kxtf9.c
new file mode 100644
index 000000000000..80776f249c63
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/kxtf9.c
@@ -0,0 +1,841 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup ACCELDL
+ *  @brief      Accelerometer setup and handling methods for Kionix KXTF9.
+ *
+ *  @{
+ *      @file   kxtf9.c
+ *      @brief  Accelerometer setup and handling methods for Kionix KXTF9.
+*/
+
+/* -------------------------------------------------------------------------- */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 1
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+#define KXTF9_XOUT_HPF_L                (0x00)	/* 0000 0000 */
+#define KXTF9_XOUT_HPF_H                (0x01)	/* 0000 0001 */
+#define KXTF9_YOUT_HPF_L                (0x02)	/* 0000 0010 */
+#define KXTF9_YOUT_HPF_H                (0x03)	/* 0000 0011 */
+#define KXTF9_ZOUT_HPF_L                (0x04)	/* 0001 0100 */
+#define KXTF9_ZOUT_HPF_H                (0x05)	/* 0001 0101 */
+#define KXTF9_XOUT_L                    (0x06)	/* 0000 0110 */
+#define KXTF9_XOUT_H                    (0x07)	/* 0000 0111 */
+#define KXTF9_YOUT_L                    (0x08)	/* 0000 1000 */
+#define KXTF9_YOUT_H                    (0x09)	/* 0000 1001 */
+#define KXTF9_ZOUT_L                    (0x0A)	/* 0001 1010 */
+#define KXTF9_ZOUT_H                    (0x0B)	/* 0001 1011 */
+#define KXTF9_ST_RESP                   (0x0C)	/* 0000 1100 */
+#define KXTF9_WHO_AM_I                  (0x0F)	/* 0000 1111 */
+#define KXTF9_TILT_POS_CUR              (0x10)	/* 0001 0000 */
+#define KXTF9_TILT_POS_PRE              (0x11)	/* 0001 0001 */
+#define KXTF9_INT_SRC_REG1              (0x15)	/* 0001 0101 */
+#define KXTF9_INT_SRC_REG2              (0x16)	/* 0001 0110 */
+#define KXTF9_STATUS_REG                (0x18)	/* 0001 1000 */
+#define KXTF9_INT_REL                   (0x1A)	/* 0001 1010 */
+#define KXTF9_CTRL_REG1                 (0x1B)	/* 0001 1011 */
+#define KXTF9_CTRL_REG2                 (0x1C)	/* 0001 1100 */
+#define KXTF9_CTRL_REG3                 (0x1D)	/* 0001 1101 */
+#define KXTF9_INT_CTRL_REG1             (0x1E)	/* 0001 1110 */
+#define KXTF9_INT_CTRL_REG2             (0x1F)	/* 0001 1111 */
+#define KXTF9_INT_CTRL_REG3             (0x20)	/* 0010 0000 */
+#define KXTF9_DATA_CTRL_REG             (0x21)	/* 0010 0001 */
+#define KXTF9_TILT_TIMER                (0x28)	/* 0010 1000 */
+#define KXTF9_WUF_TIMER                 (0x29)	/* 0010 1001 */
+#define KXTF9_TDT_TIMER                 (0x2B)	/* 0010 1011 */
+#define KXTF9_TDT_H_THRESH              (0x2C)	/* 0010 1100 */
+#define KXTF9_TDT_L_THRESH              (0x2D)	/* 0010 1101 */
+#define KXTF9_TDT_TAP_TIMER             (0x2E)	/* 0010 1110 */
+#define KXTF9_TDT_TOTAL_TIMER           (0x2F)	/* 0010 1111 */
+#define KXTF9_TDT_LATENCY_TIMER         (0x30)	/* 0011 0000 */
+#define KXTF9_TDT_WINDOW_TIMER          (0x31)	/* 0011 0001 */
+#define KXTF9_WUF_THRESH                (0x5A)	/* 0101 1010 */
+#define KXTF9_TILT_ANGLE                (0x5C)	/* 0101 1100 */
+#define KXTF9_HYST_SET                  (0x5F)	/* 0101 1111 */
+
+#define KXTF9_MAX_DUR (0xFF)
+#define KXTF9_MAX_THS (0xFF)
+#define KXTF9_THS_COUNTS_P_G (32)
+
+/* -------------------------------------------------------------------------- */
+
+struct kxtf9_config {
+	unsigned long odr;	/* Output data rate mHz */
+	unsigned int fsr;	/* full scale range mg */
+	unsigned int ths;	/* Motion no-motion thseshold mg */
+	unsigned int dur;	/* Motion no-motion duration ms */
+	unsigned int irq_type;
+	unsigned char reg_ths;
+	unsigned char reg_dur;
+	unsigned char reg_odr;
+	unsigned char reg_int_cfg1;
+	unsigned char reg_int_cfg2;
+	unsigned char ctrl_reg1;
+};
+
+struct kxtf9_private_data {
+	struct kxtf9_config suspend;
+	struct kxtf9_config resume;
+};
+
+static int kxtf9_set_ths(void *mlsl_handle,
+			 struct ext_slave_platform_data *pdata,
+			 struct kxtf9_config *config, int apply, long ths)
+{
+	int result = INV_SUCCESS;
+	if ((ths * KXTF9_THS_COUNTS_P_G / 1000) > KXTF9_MAX_THS)
+		ths = (long)(KXTF9_MAX_THS * 1000) / KXTF9_THS_COUNTS_P_G;
+
+	if (ths < 0)
+		ths = 0;
+
+	config->ths = ths;
+	config->reg_ths = (unsigned char)
+	    ((long)(ths * KXTF9_THS_COUNTS_P_G) / 1000);
+	MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths);
+	if (apply)
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 KXTF9_WUF_THRESH,
+						 config->reg_ths);
+	return result;
+}
+
+static int kxtf9_set_dur(void *mlsl_handle,
+			 struct ext_slave_platform_data *pdata,
+			 struct kxtf9_config *config, int apply, long dur)
+{
+	int result = INV_SUCCESS;
+	long reg_dur = (dur * config->odr) / 1000000L;
+	config->dur = dur;
+
+	if (reg_dur > KXTF9_MAX_DUR)
+		reg_dur = KXTF9_MAX_DUR;
+
+	config->reg_dur = (unsigned char)reg_dur;
+	MPL_LOGV("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur);
+	if (apply)
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 KXTF9_WUF_TIMER,
+						 (unsigned char)reg_dur);
+	return result;
+}
+
+/**
+ * Sets the IRQ to fire when one of the IRQ events occur.  Threshold and
+ * duration will not be used uless the type is MOT or NMOT.
+ *
+ * @param config configuration to apply to, suspend or resume
+ * @param irq_type The type of IRQ.  Valid values are
+ * - MPU_SLAVE_IRQ_TYPE_NONE
+ * - MPU_SLAVE_IRQ_TYPE_MOTION
+ * - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ */
+static int kxtf9_set_irq(void *mlsl_handle,
+			 struct ext_slave_platform_data *pdata,
+			 struct kxtf9_config *config, int apply, long irq_type)
+{
+	int result = INV_SUCCESS;
+	struct kxtf9_private_data *private_data = pdata->private_data;
+
+	config->irq_type = (unsigned char)irq_type;
+	config->ctrl_reg1 &= ~0x22;
+	if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+		config->ctrl_reg1 |= 0x20;
+		config->reg_int_cfg1 = 0x38;
+		config->reg_int_cfg2 = 0x00;
+	} else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
+		config->ctrl_reg1 |= 0x02;
+		if ((unsigned long)config ==
+		    (unsigned long)&private_data->suspend)
+			config->reg_int_cfg1 = 0x34;
+		else
+			config->reg_int_cfg1 = 0x24;
+		config->reg_int_cfg2 = 0xE0;
+	} else {
+		config->reg_int_cfg1 = 0x00;
+		config->reg_int_cfg2 = 0x00;
+	}
+
+	if (apply) {
+		/* Must clear bit 7 before writing new configuration */
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 KXTF9_CTRL_REG1, 0x40);
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 KXTF9_INT_CTRL_REG1,
+						 config->reg_int_cfg1);
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 KXTF9_INT_CTRL_REG2,
+						 config->reg_int_cfg2);
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 KXTF9_CTRL_REG1,
+						 config->ctrl_reg1);
+	}
+	MPL_LOGV("CTRL_REG1: %lx, INT_CFG1: %lx, INT_CFG2: %lx\n",
+		 (unsigned long)config->ctrl_reg1,
+		 (unsigned long)config->reg_int_cfg1,
+		 (unsigned long)config->reg_int_cfg2);
+
+	return result;
+}
+
+/**
+ * Set the Output data rate for the particular configuration
+ *
+ * @param config Config to modify with new ODR
+ * @param odr Output data rate in units of 1/1000Hz
+ */
+static int kxtf9_set_odr(void *mlsl_handle,
+			 struct ext_slave_platform_data *pdata,
+			 struct kxtf9_config *config, int apply, long odr)
+{
+	unsigned char bits;
+	int result = INV_SUCCESS;
+
+	/* Data sheet says there is 12.5 hz, but that seems to produce a single
+	 * correct data value, thus we remove it from the table */
+	if (odr > 400000L) {
+		config->odr = 800000L;
+		bits = 0x06;
+	} else if (odr > 200000L) {
+		config->odr = 400000L;
+		bits = 0x05;
+	} else if (odr > 100000L) {
+		config->odr = 200000L;
+		bits = 0x04;
+	} else if (odr > 50000) {
+		config->odr = 100000L;
+		bits = 0x03;
+	} else if (odr > 25000) {
+		config->odr = 50000;
+		bits = 0x02;
+	} else if (odr != 0) {
+		config->odr = 25000;
+		bits = 0x01;
+	} else {
+		config->odr = 0;
+		bits = 0;
+	}
+
+	if (odr != 0)
+		config->ctrl_reg1 |= 0x80;
+	else
+		config->ctrl_reg1 &= ~0x80;
+
+	config->reg_odr = bits;
+	kxtf9_set_dur(mlsl_handle, pdata, config, apply, config->dur);
+	MPL_LOGV("ODR: %ld, 0x%02x\n", config->odr, (int)config->ctrl_reg1);
+	if (apply) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 KXTF9_DATA_CTRL_REG,
+						 config->reg_odr);
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 KXTF9_CTRL_REG1, 0x40);
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 KXTF9_CTRL_REG1,
+						 config->ctrl_reg1);
+	}
+	return result;
+}
+
+/**
+ * Set the full scale range of the accels
+ *
+ * @param config pointer to configuration
+ * @param fsr requested full scale range
+ */
+static int kxtf9_set_fsr(void *mlsl_handle,
+			 struct ext_slave_platform_data *pdata,
+			 struct kxtf9_config *config, int apply, long fsr)
+{
+	int result = INV_SUCCESS;
+
+	config->ctrl_reg1 = (config->ctrl_reg1 & 0xE7);
+	if (fsr <= 2000) {
+		config->fsr = 2000;
+		config->ctrl_reg1 |= 0x00;
+	} else if (fsr <= 4000) {
+		config->fsr = 4000;
+		config->ctrl_reg1 |= 0x08;
+	} else {
+		config->fsr = 8000;
+		config->ctrl_reg1 |= 0x10;
+	}
+
+	MPL_LOGV("FSR: %d\n", config->fsr);
+	if (apply) {
+		/* Must clear bit 7 before writing new configuration */
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 KXTF9_CTRL_REG1, 0x40);
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 KXTF9_CTRL_REG1,
+						 config->ctrl_reg1);
+	}
+	return result;
+}
+
+static int kxtf9_suspend(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char data;
+	struct kxtf9_private_data *private_data = pdata->private_data;
+
+	/* Wake up */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 KXTF9_CTRL_REG1, 0x40);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* INT_CTRL_REG1: */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 KXTF9_INT_CTRL_REG1,
+					 private_data->suspend.reg_int_cfg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* WUF_THRESH: */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 KXTF9_WUF_THRESH,
+					 private_data->suspend.reg_ths);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* DATA_CTRL_REG */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 KXTF9_DATA_CTRL_REG,
+					 private_data->suspend.reg_odr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* WUF_TIMER */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 KXTF9_WUF_TIMER,
+					 private_data->suspend.reg_dur);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Normal operation  */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 KXTF9_CTRL_REG1,
+					 private_data->suspend.ctrl_reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 KXTF9_INT_REL, 1, &data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+/* full scale setting - register and mask */
+#define ACCEL_KIONIX_CTRL_REG      (0x1b)
+#define ACCEL_KIONIX_CTRL_MASK     (0x18)
+
+static int kxtf9_resume(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	unsigned char data;
+	struct kxtf9_private_data *private_data = pdata->private_data;
+
+	/* Wake up */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 KXTF9_CTRL_REG1, 0x40);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* INT_CTRL_REG1: */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 KXTF9_INT_CTRL_REG1,
+					 private_data->resume.reg_int_cfg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* WUF_THRESH: */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 KXTF9_WUF_THRESH,
+					 private_data->resume.reg_ths);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* DATA_CTRL_REG */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 KXTF9_DATA_CTRL_REG,
+					 private_data->resume.reg_odr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* WUF_TIMER */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 KXTF9_WUF_TIMER,
+					 private_data->resume.reg_dur);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Normal operation  */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 KXTF9_CTRL_REG1,
+					 private_data->resume.ctrl_reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 KXTF9_INT_REL, 1, &data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return INV_SUCCESS;
+}
+
+static int kxtf9_init(void *mlsl_handle,
+		      struct ext_slave_descr *slave,
+		      struct ext_slave_platform_data *pdata)
+{
+
+	struct kxtf9_private_data *private_data;
+	int result = INV_SUCCESS;
+
+	private_data = (struct kxtf9_private_data *)
+	    kzalloc(sizeof(struct kxtf9_private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	/* RAM reset */
+	/* Fastest Reset */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 KXTF9_CTRL_REG1, 0x40);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Fastest Reset */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 KXTF9_DATA_CTRL_REG, 0x36);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Reset */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 KXTF9_CTRL_REG3, 0xcd);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(2);
+
+	pdata->private_data = private_data;
+
+	private_data->resume.ctrl_reg1 = 0xC0;
+	private_data->suspend.ctrl_reg1 = 0x40;
+
+	result = kxtf9_set_dur(mlsl_handle, pdata, &private_data->suspend,
+			       false, 1000);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = kxtf9_set_dur(mlsl_handle, pdata, &private_data->resume,
+			       false, 2540);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = kxtf9_set_odr(mlsl_handle, pdata, &private_data->suspend,
+			       false, 50000);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = kxtf9_set_odr(mlsl_handle, pdata, &private_data->resume,
+			       false, 200000L);
+
+	result = kxtf9_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+			       false, 2000);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = kxtf9_set_fsr(mlsl_handle, pdata, &private_data->resume,
+			       false, 2000);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = kxtf9_set_ths(mlsl_handle, pdata, &private_data->suspend,
+			       false, 80);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = kxtf9_set_ths(mlsl_handle, pdata, &private_data->resume,
+			       false, 40);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = kxtf9_set_irq(mlsl_handle, pdata, &private_data->suspend,
+			       false, MPU_SLAVE_IRQ_TYPE_NONE);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = kxtf9_set_irq(mlsl_handle, pdata, &private_data->resume,
+			       false, MPU_SLAVE_IRQ_TYPE_NONE);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+static int kxtf9_exit(void *mlsl_handle,
+		      struct ext_slave_descr *slave,
+		      struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	return INV_SUCCESS;
+}
+
+static int kxtf9_config(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			struct ext_slave_config *data)
+{
+	struct kxtf9_private_data *private_data = pdata->private_data;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		return kxtf9_set_odr(mlsl_handle, pdata,
+				     &private_data->suspend,
+				     data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		return kxtf9_set_odr(mlsl_handle, pdata,
+				     &private_data->resume,
+				     data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		return kxtf9_set_fsr(mlsl_handle, pdata,
+				     &private_data->suspend,
+				     data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		return kxtf9_set_fsr(mlsl_handle, pdata,
+				     &private_data->resume,
+				     data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		return kxtf9_set_ths(mlsl_handle, pdata,
+				     &private_data->suspend,
+				     data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		return kxtf9_set_ths(mlsl_handle, pdata,
+				     &private_data->resume,
+				     data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		return kxtf9_set_dur(mlsl_handle, pdata,
+				     &private_data->suspend,
+				     data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		return kxtf9_set_dur(mlsl_handle, pdata,
+				     &private_data->resume,
+				     data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		return kxtf9_set_irq(mlsl_handle, pdata,
+				     &private_data->suspend,
+				     data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		return kxtf9_set_irq(mlsl_handle, pdata,
+				     &private_data->resume,
+				     data->apply, *((long *)data->data));
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static int kxtf9_get_config(void *mlsl_handle,
+			    struct ext_slave_descr *slave,
+			    struct ext_slave_platform_data *pdata,
+			    struct ext_slave_config *data)
+{
+	struct kxtf9_private_data *private_data = pdata->private_data;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.odr;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.odr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.ths;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.ths;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.dur;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.dur;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.irq_type;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.irq_type;
+		break;
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static int kxtf9_read(void *mlsl_handle,
+		      struct ext_slave_descr *slave,
+		      struct ext_slave_platform_data *pdata,
+		      unsigned char *data)
+{
+	int result;
+	unsigned char reg;
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 KXTF9_INT_SRC_REG2, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (!(reg & 0x10))
+		return INV_ERROR_ACCEL_DATA_NOT_READY;
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 slave->read_reg, slave->read_len, data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+static struct ext_slave_descr kxtf9_descr = {
+	.init             = kxtf9_init,
+	.exit             = kxtf9_exit,
+	.suspend          = kxtf9_suspend,
+	.resume           = kxtf9_resume,
+	.read             = kxtf9_read,
+	.config           = kxtf9_config,
+	.get_config       = kxtf9_get_config,
+	.name             = "kxtf9",
+	.type             = EXT_SLAVE_TYPE_ACCEL,
+	.id               = ACCEL_ID_KXTF9,
+	.read_reg         = 0x06,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_LITTLE_ENDIAN,
+	.range            = {2, 0},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *kxtf9_get_slave_descr(void)
+{
+	return &kxtf9_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct kxtf9_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int kxtf9_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct kxtf9_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					kxtf9_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int kxtf9_mod_remove(struct i2c_client *client)
+{
+	struct kxtf9_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				kxtf9_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id kxtf9_mod_id[] = {
+	{ "kxtf9", ACCEL_ID_KXTF9 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, kxtf9_mod_id);
+
+static struct i2c_driver kxtf9_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = kxtf9_mod_probe,
+	.remove = kxtf9_mod_remove,
+	.id_table = kxtf9_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "kxtf9_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init kxtf9_mod_init(void)
+{
+	int res = i2c_add_driver(&kxtf9_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "kxtf9_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit kxtf9_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&kxtf9_mod_driver);
+}
+
+module_init(kxtf9_mod_init);
+module_exit(kxtf9_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate KXTF9 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("kxtf9_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/lis331.c b/drivers/misc/inv_mpu/accel/lis331.c
new file mode 100644
index 000000000000..bcbec252af97
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/lis331.c
@@ -0,0 +1,745 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   lis331.c
+ *      @brief  Accelerometer setup and handling methods for ST LIS331DLH.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 1
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* full scale setting - register & mask */
+#define LIS331DLH_CTRL_REG1         (0x20)
+#define LIS331DLH_CTRL_REG2         (0x21)
+#define LIS331DLH_CTRL_REG3         (0x22)
+#define LIS331DLH_CTRL_REG4         (0x23)
+#define LIS331DLH_CTRL_REG5         (0x24)
+#define LIS331DLH_HP_FILTER_RESET   (0x25)
+#define LIS331DLH_REFERENCE         (0x26)
+#define LIS331DLH_STATUS_REG        (0x27)
+#define LIS331DLH_OUT_X_L           (0x28)
+#define LIS331DLH_OUT_X_H           (0x29)
+#define LIS331DLH_OUT_Y_L           (0x2a)
+#define LIS331DLH_OUT_Y_H           (0x2b)
+#define LIS331DLH_OUT_Z_L           (0x2b)
+#define LIS331DLH_OUT_Z_H           (0x2d)
+
+#define LIS331DLH_INT1_CFG          (0x30)
+#define LIS331DLH_INT1_SRC          (0x31)
+#define LIS331DLH_INT1_THS          (0x32)
+#define LIS331DLH_INT1_DURATION     (0x33)
+
+#define LIS331DLH_INT2_CFG          (0x34)
+#define LIS331DLH_INT2_SRC          (0x35)
+#define LIS331DLH_INT2_THS          (0x36)
+#define LIS331DLH_INT2_DURATION     (0x37)
+
+/* CTRL_REG1 */
+#define LIS331DLH_CTRL_MASK         (0x30)
+#define LIS331DLH_SLEEP_MASK        (0x20)
+#define LIS331DLH_PWR_MODE_NORMAL   (0x20)
+
+#define LIS331DLH_MAX_DUR           (0x7F)
+
+
+/* -------------------------------------------------------------------------- */
+
+struct lis331dlh_config {
+	unsigned int odr;
+	unsigned int fsr;	/* full scale range mg */
+	unsigned int ths;	/* Motion no-motion thseshold mg */
+	unsigned int dur;	/* Motion no-motion duration ms */
+	unsigned char reg_ths;
+	unsigned char reg_dur;
+	unsigned char ctrl_reg1;
+	unsigned char irq_type;
+	unsigned char mot_int1_cfg;
+};
+
+struct lis331dlh_private_data {
+	struct lis331dlh_config suspend;
+	struct lis331dlh_config resume;
+};
+
+/* -------------------------------------------------------------------------- */
+static int lis331dlh_set_ths(void *mlsl_handle,
+			     struct ext_slave_platform_data *pdata,
+			     struct lis331dlh_config *config,
+			     int apply, long ths)
+{
+	int result = INV_SUCCESS;
+	if ((unsigned int)ths >= config->fsr)
+		ths = (long)config->fsr - 1;
+
+	if (ths < 0)
+		ths = 0;
+
+	config->ths = ths;
+	config->reg_ths = (unsigned char)(long)((ths * 128L) / (config->fsr));
+	MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths);
+	if (apply)
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 LIS331DLH_INT1_THS,
+						 config->reg_ths);
+	return result;
+}
+
+static int lis331dlh_set_dur(void *mlsl_handle,
+			     struct ext_slave_platform_data *pdata,
+			     struct lis331dlh_config *config,
+			     int apply, long dur)
+{
+	int result = INV_SUCCESS;
+	long reg_dur = (dur * config->odr) / 1000000L;
+	config->dur = dur;
+
+	if (reg_dur > LIS331DLH_MAX_DUR)
+		reg_dur = LIS331DLH_MAX_DUR;
+
+	config->reg_dur = (unsigned char)reg_dur;
+	MPL_LOGV("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur);
+	if (apply)
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 LIS331DLH_INT1_DURATION,
+						 (unsigned char)reg_dur);
+	return result;
+}
+
+/**
+ * Sets the IRQ to fire when one of the IRQ events occur.  Threshold and
+ * duration will not be used uless the type is MOT or NMOT.
+ *
+ * @param config configuration to apply to, suspend or resume
+ * @param irq_type The type of IRQ.  Valid values are
+ * - MPU_SLAVE_IRQ_TYPE_NONE
+ * - MPU_SLAVE_IRQ_TYPE_MOTION
+ * - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ */
+static int lis331dlh_set_irq(void *mlsl_handle,
+			     struct ext_slave_platform_data *pdata,
+			     struct lis331dlh_config *config,
+			     int apply, long irq_type)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg1;
+	unsigned char reg2;
+
+	config->irq_type = (unsigned char)irq_type;
+	if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+		reg1 = 0x02;
+		reg2 = 0x00;
+	} else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
+		reg1 = 0x00;
+		reg2 = config->mot_int1_cfg;
+	} else {
+		reg1 = 0x00;
+		reg2 = 0x00;
+	}
+
+	if (apply) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 LIS331DLH_CTRL_REG3, reg1);
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 LIS331DLH_INT1_CFG, reg2);
+	}
+
+	return result;
+}
+
+/**
+ * Set the Output data rate for the particular configuration
+ *
+ * @param config Config to modify with new ODR
+ * @param odr Output data rate in units of 1/1000Hz
+ */
+static int lis331dlh_set_odr(void *mlsl_handle,
+			     struct ext_slave_platform_data *pdata,
+			     struct lis331dlh_config *config,
+			     int apply, long odr)
+{
+	unsigned char bits;
+	int result = INV_SUCCESS;
+
+	/* normal power modes */
+	if (odr > 400000) {
+		config->odr = 1000000;
+		bits = LIS331DLH_PWR_MODE_NORMAL | 0x18;
+	} else if (odr > 100000) {
+		config->odr = 400000;
+		bits = LIS331DLH_PWR_MODE_NORMAL | 0x10;
+	} else if (odr > 50000) {
+		config->odr = 100000;
+		bits = LIS331DLH_PWR_MODE_NORMAL | 0x08;
+	} else if (odr > 10000) {
+		config->odr = 50000;
+		bits = LIS331DLH_PWR_MODE_NORMAL | 0x00;
+	/* low power modes */
+	} else if (odr > 5000) {
+		config->odr = 10000;
+		bits = 0xC0;
+	} else if (odr > 2000) {
+		config->odr = 5000;
+		bits = 0xA0;
+	} else if (odr > 1000) {
+		config->odr = 2000;
+		bits = 0x80;
+	} else if (odr > 500) {
+		config->odr = 1000;
+		bits = 0x60;
+	} else if (odr > 0) {
+		config->odr = 500;
+		bits = 0x40;
+	} else {
+		config->odr = 0;
+		bits = 0;
+	}
+
+	config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0x7);
+	lis331dlh_set_dur(mlsl_handle, pdata, config, apply, config->dur);
+	MPL_LOGV("ODR: %d, 0x%02x\n", config->odr, (int)config->ctrl_reg1);
+	if (apply)
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 LIS331DLH_CTRL_REG1,
+						 config->ctrl_reg1);
+	return result;
+}
+
+/**
+ * Set the full scale range of the accels
+ *
+ * @param config pointer to configuration
+ * @param fsr requested full scale range
+ */
+static int lis331dlh_set_fsr(void *mlsl_handle,
+			     struct ext_slave_platform_data *pdata,
+			     struct lis331dlh_config *config,
+			     int apply, long fsr)
+{
+	unsigned char reg1 = 0x40;
+	int result = INV_SUCCESS;
+
+	if (fsr <= 2048) {
+		config->fsr = 2048;
+	} else if (fsr <= 4096) {
+		reg1 |= 0x30;
+		config->fsr = 4096;
+	} else {
+		reg1 |= 0x10;
+		config->fsr = 8192;
+	}
+
+	lis331dlh_set_ths(mlsl_handle, pdata, config, apply, config->ths);
+	MPL_LOGV("FSR: %d\n", config->fsr);
+	if (apply)
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 LIS331DLH_CTRL_REG4, reg1);
+
+	return result;
+}
+
+static int lis331dlh_suspend(void *mlsl_handle,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg1;
+	unsigned char reg2;
+	struct lis331dlh_private_data *private_data =
+		(struct lis331dlh_private_data *)(pdata->private_data);
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS331DLH_CTRL_REG1,
+					 private_data->suspend.ctrl_reg1);
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS331DLH_CTRL_REG2, 0x0f);
+	reg1 = 0x40;
+	if (private_data->suspend.fsr == 8192)
+		reg1 |= 0x30;
+	else if (private_data->suspend.fsr == 4096)
+		reg1 |= 0x10;
+	/* else bits [4..5] are already zero */
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS331DLH_CTRL_REG4, reg1);
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS331DLH_INT1_THS,
+					 private_data->suspend.reg_ths);
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS331DLH_INT1_DURATION,
+					 private_data->suspend.reg_dur);
+
+	if (private_data->suspend.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+		reg1 = 0x02;
+		reg2 = 0x00;
+	} else if (private_data->suspend.irq_type ==
+		   MPU_SLAVE_IRQ_TYPE_MOTION) {
+		reg1 = 0x00;
+		reg2 = private_data->suspend.mot_int1_cfg;
+	} else {
+		reg1 = 0x00;
+		reg2 = 0x00;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS331DLH_CTRL_REG3, reg1);
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS331DLH_INT1_CFG, reg2);
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 LIS331DLH_HP_FILTER_RESET, 1, &reg1);
+	return result;
+}
+
+static int lis331dlh_resume(void *mlsl_handle,
+			    struct ext_slave_descr *slave,
+			    struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg1;
+	unsigned char reg2;
+	struct lis331dlh_private_data *private_data =
+		(struct lis331dlh_private_data *)(pdata->private_data);
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS331DLH_CTRL_REG1,
+					 private_data->resume.ctrl_reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(6);
+
+	/* Full Scale */
+	reg1 = 0x40;
+	if (private_data->resume.fsr == 8192)
+		reg1 |= 0x30;
+	else if (private_data->resume.fsr == 4096)
+		reg1 |= 0x10;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS331DLH_CTRL_REG4, reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Configure high pass filter */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS331DLH_CTRL_REG2, 0x0F);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+		reg1 = 0x02;
+		reg2 = 0x00;
+	} else if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
+		reg1 = 0x00;
+		reg2 = private_data->resume.mot_int1_cfg;
+	} else {
+		reg1 = 0x00;
+		reg2 = 0x00;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS331DLH_CTRL_REG3, reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS331DLH_INT1_THS,
+					 private_data->resume.reg_ths);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS331DLH_INT1_DURATION,
+					 private_data->resume.reg_dur);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS331DLH_INT1_CFG, reg2);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 LIS331DLH_HP_FILTER_RESET, 1, &reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+static int lis331dlh_read(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata,
+			  unsigned char *data)
+{
+	int result = INV_SUCCESS;
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 LIS331DLH_STATUS_REG, 1, data);
+	if (data[0] & 0x0F) {
+		result = inv_serial_read(mlsl_handle, pdata->address,
+					 slave->read_reg, slave->read_len,
+					 data);
+		return result;
+	} else
+		return INV_ERROR_ACCEL_DATA_NOT_READY;
+}
+
+static int lis331dlh_init(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	struct lis331dlh_private_data *private_data;
+	long range;
+	private_data = (struct lis331dlh_private_data *)
+	    kzalloc(sizeof(struct lis331dlh_private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	pdata->private_data = private_data;
+
+	private_data->resume.ctrl_reg1 = 0x37;
+	private_data->suspend.ctrl_reg1 = 0x47;
+	private_data->resume.mot_int1_cfg = 0x95;
+	private_data->suspend.mot_int1_cfg = 0x2a;
+
+	lis331dlh_set_odr(mlsl_handle, pdata, &private_data->suspend, false, 0);
+	lis331dlh_set_odr(mlsl_handle, pdata, &private_data->resume,
+			  false, 200000);
+
+	range = range_fixedpoint_to_long_mg(slave->range);
+	lis331dlh_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+			false, range);
+	lis331dlh_set_fsr(mlsl_handle, pdata, &private_data->resume,
+			false, range);
+
+	lis331dlh_set_ths(mlsl_handle, pdata, &private_data->suspend,
+			  false, 80);
+	lis331dlh_set_ths(mlsl_handle, pdata, &private_data->resume, false, 40);
+
+
+	lis331dlh_set_dur(mlsl_handle, pdata, &private_data->suspend,
+			  false, 1000);
+	lis331dlh_set_dur(mlsl_handle, pdata, &private_data->resume,
+			  false, 2540);
+
+	lis331dlh_set_irq(mlsl_handle, pdata, &private_data->suspend,
+			  false, MPU_SLAVE_IRQ_TYPE_NONE);
+	lis331dlh_set_irq(mlsl_handle, pdata, &private_data->resume,
+			  false, MPU_SLAVE_IRQ_TYPE_NONE);
+	return INV_SUCCESS;
+}
+
+static int lis331dlh_exit(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	return INV_SUCCESS;
+}
+
+static int lis331dlh_config(void *mlsl_handle,
+			    struct ext_slave_descr *slave,
+			    struct ext_slave_platform_data *pdata,
+			    struct ext_slave_config *data)
+{
+	struct lis331dlh_private_data *private_data = pdata->private_data;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		return lis331dlh_set_odr(mlsl_handle, pdata,
+					 &private_data->suspend,
+					 data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		return lis331dlh_set_odr(mlsl_handle, pdata,
+					 &private_data->resume,
+					 data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		return lis331dlh_set_fsr(mlsl_handle, pdata,
+					 &private_data->suspend,
+					 data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		return lis331dlh_set_fsr(mlsl_handle, pdata,
+					 &private_data->resume,
+					 data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		return lis331dlh_set_ths(mlsl_handle, pdata,
+					 &private_data->suspend,
+					 data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		return lis331dlh_set_ths(mlsl_handle, pdata,
+					 &private_data->resume,
+					 data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		return lis331dlh_set_dur(mlsl_handle, pdata,
+					 &private_data->suspend,
+					 data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		return lis331dlh_set_dur(mlsl_handle, pdata,
+					 &private_data->resume,
+					 data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		return lis331dlh_set_irq(mlsl_handle, pdata,
+					 &private_data->suspend,
+					 data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		return lis331dlh_set_irq(mlsl_handle, pdata,
+					 &private_data->resume,
+					 data->apply, *((long *)data->data));
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static int lis331dlh_get_config(void *mlsl_handle,
+				struct ext_slave_descr *slave,
+				struct ext_slave_platform_data *pdata,
+				struct ext_slave_config *data)
+{
+	struct lis331dlh_private_data *private_data = pdata->private_data;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.odr;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.odr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.ths;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.ths;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.dur;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.dur;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.irq_type;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.irq_type;
+		break;
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static struct ext_slave_descr lis331dlh_descr = {
+	.init             = lis331dlh_init,
+	.exit             = lis331dlh_exit,
+	.suspend          = lis331dlh_suspend,
+	.resume           = lis331dlh_resume,
+	.read             = lis331dlh_read,
+	.config           = lis331dlh_config,
+	.get_config       = lis331dlh_get_config,
+	.name             = "lis331dlh",
+	.type             = EXT_SLAVE_TYPE_ACCEL,
+	.id               = ACCEL_ID_LIS331,
+	.read_reg         = (0x28 | 0x80), /* 0x80 for burst reads */
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_BIG_ENDIAN,
+	.range            = {2, 480},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *lis331_get_slave_descr(void)
+{
+	return &lis331dlh_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct lis331_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int lis331_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct lis331_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					lis331_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int lis331_mod_remove(struct i2c_client *client)
+{
+	struct lis331_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				lis331_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id lis331_mod_id[] = {
+	{ "lis331", ACCEL_ID_LIS331 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, lis331_mod_id);
+
+static struct i2c_driver lis331_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = lis331_mod_probe,
+	.remove = lis331_mod_remove,
+	.id_table = lis331_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "lis331_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init lis331_mod_init(void)
+{
+	int res = i2c_add_driver(&lis331_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "lis331_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit lis331_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&lis331_mod_driver);
+}
+
+module_init(lis331_mod_init);
+module_exit(lis331_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate LIS331 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("lis331_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/lis3dh.c b/drivers/misc/inv_mpu/accel/lis3dh.c
new file mode 100644
index 000000000000..27206e4b847c
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/lis3dh.c
@@ -0,0 +1,728 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   lis3dh.c
+ *      @brief  Accelerometer setup and handling methods for ST LIS3DH.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 0
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* full scale setting - register & mask */
+#define LIS3DH_CTRL_REG1         (0x20)
+#define LIS3DH_CTRL_REG2         (0x21)
+#define LIS3DH_CTRL_REG3         (0x22)
+#define LIS3DH_CTRL_REG4         (0x23)
+#define LIS3DH_CTRL_REG5         (0x24)
+#define LIS3DH_CTRL_REG6         (0x25)
+#define LIS3DH_REFERENCE         (0x26)
+#define LIS3DH_STATUS_REG        (0x27)
+#define LIS3DH_OUT_X_L           (0x28)
+#define LIS3DH_OUT_X_H           (0x29)
+#define LIS3DH_OUT_Y_L           (0x2a)
+#define LIS3DH_OUT_Y_H           (0x2b)
+#define LIS3DH_OUT_Z_L           (0x2c)
+#define LIS3DH_OUT_Z_H           (0x2d)
+
+#define LIS3DH_INT1_CFG          (0x30)
+#define LIS3DH_INT1_SRC          (0x31)
+#define LIS3DH_INT1_THS          (0x32)
+#define LIS3DH_INT1_DURATION     (0x33)
+
+#define LIS3DH_MAX_DUR (0x7F)
+
+/* -------------------------------------------------------------------------- */
+
+struct lis3dh_config {
+	unsigned long odr;
+	unsigned int fsr;	/* full scale range mg */
+	unsigned int ths;	/* Motion no-motion thseshold mg */
+	unsigned int dur;	/* Motion no-motion duration ms */
+	unsigned char reg_ths;
+	unsigned char reg_dur;
+	unsigned char ctrl_reg1;
+	unsigned char irq_type;
+	unsigned char mot_int1_cfg;
+};
+
+struct lis3dh_private_data {
+	struct lis3dh_config suspend;
+	struct lis3dh_config resume;
+};
+
+/* -------------------------------------------------------------------------- */
+
+static int lis3dh_set_ths(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct lis3dh_config *config, int apply, long ths)
+{
+	int result = INV_SUCCESS;
+	if ((unsigned int)ths > 1000 * config->fsr)
+		ths = (long)1000 * config->fsr;
+
+	if (ths < 0)
+		ths = 0;
+
+	config->ths = ths;
+	config->reg_ths = (unsigned char)(long)((ths * 128L) / (config->fsr));
+	MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths);
+	if (apply)
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 LIS3DH_INT1_THS,
+						 config->reg_ths);
+	return result;
+}
+
+static int lis3dh_set_dur(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct lis3dh_config *config, int apply, long dur)
+{
+	int result = INV_SUCCESS;
+	long reg_dur = (dur * config->odr) / 1000000L;
+	config->dur = dur;
+
+	if (reg_dur > LIS3DH_MAX_DUR)
+		reg_dur = LIS3DH_MAX_DUR;
+
+	config->reg_dur = (unsigned char)reg_dur;
+	MPL_LOGV("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur);
+	if (apply)
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 LIS3DH_INT1_DURATION,
+						 (unsigned char)reg_dur);
+	return result;
+}
+
+/**
+ * Sets the IRQ to fire when one of the IRQ events occur.  Threshold and
+ * duration will not be used uless the type is MOT or NMOT.
+ *
+ * @param config configuration to apply to, suspend or resume
+ * @param irq_type The type of IRQ.  Valid values are
+ * - MPU_SLAVE_IRQ_TYPE_NONE
+ * - MPU_SLAVE_IRQ_TYPE_MOTION
+ * - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ */
+static int lis3dh_set_irq(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct lis3dh_config *config,
+			  int apply, long irq_type)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg1;
+	unsigned char reg2;
+
+	config->irq_type = (unsigned char)irq_type;
+	if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+		reg1 = 0x10;
+		reg2 = 0x00;
+	} else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
+		reg1 = 0x40;
+		reg2 = config->mot_int1_cfg;
+	} else {
+		reg1 = 0x00;
+		reg2 = 0x00;
+	}
+
+	if (apply) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 LIS3DH_CTRL_REG3, reg1);
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 LIS3DH_INT1_CFG, reg2);
+	}
+
+	return result;
+}
+
+/**
+ * Set the Output data rate for the particular configuration
+ *
+ * @param config Config to modify with new ODR
+ * @param odr Output data rate in units of 1/1000Hz
+ */
+static int lis3dh_set_odr(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct lis3dh_config *config, int apply, long odr)
+{
+	unsigned char bits;
+	int result = INV_SUCCESS;
+
+	if (odr > 400000L) {
+		config->odr = 1250000L;
+		bits = 0x90;
+	} else if (odr > 200000L) {
+		config->odr = 400000L;
+		bits = 0x70;
+	} else if (odr > 100000L) {
+		config->odr = 200000L;
+		bits = 0x60;
+	} else if (odr > 50000) {
+		config->odr = 100000L;
+		bits = 0x50;
+	} else if (odr > 25000) {
+		config->odr = 50000;
+		bits = 0x40;
+	} else if (odr > 10000) {
+		config->odr = 25000;
+		bits = 0x30;
+	} else if (odr > 1000) {
+		config->odr = 10000;
+		bits = 0x20;
+	} else if (odr > 500) {
+		config->odr = 1000;
+		bits = 0x10;
+	} else {
+		config->odr = 0;
+		bits = 0;
+	}
+
+	config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0xf);
+	lis3dh_set_dur(mlsl_handle, pdata, config, apply, config->dur);
+	MPL_LOGV("ODR: %ld, 0x%02x\n", config->odr, (int)config->ctrl_reg1);
+	if (apply)
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 LIS3DH_CTRL_REG1,
+						 config->ctrl_reg1);
+	return result;
+}
+
+/**
+ * Set the full scale range of the accels
+ *
+ * @param config pointer to configuration
+ * @param fsr requested full scale range
+ */
+static int lis3dh_set_fsr(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct lis3dh_config *config, int apply, long fsr)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg1 = 0x48;
+
+	if (fsr <= 2048) {
+		config->fsr = 2048;
+	} else if (fsr <= 4096) {
+		reg1 |= 0x10;
+		config->fsr = 4096;
+	} else if (fsr <= 8192) {
+		reg1 |= 0x20;
+		config->fsr = 8192;
+	} else {
+		reg1 |= 0x30;
+		config->fsr = 16348;
+	}
+
+	lis3dh_set_ths(mlsl_handle, pdata, config, apply, config->ths);
+	MPL_LOGV("FSR: %d\n", config->fsr);
+	if (apply)
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 LIS3DH_CTRL_REG4, reg1);
+
+	return result;
+}
+
+static int lis3dh_suspend(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg1;
+	unsigned char reg2;
+	struct lis3dh_private_data *private_data = pdata->private_data;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS3DH_CTRL_REG1,
+					 private_data->suspend.ctrl_reg1);
+
+	reg1 = 0x48;
+	if (private_data->suspend.fsr == 16384)
+		reg1 |= 0x30;
+	else if (private_data->suspend.fsr == 8192)
+		reg1 |= 0x20;
+	else if (private_data->suspend.fsr == 4096)
+		reg1 |= 0x10;
+	else if (private_data->suspend.fsr == 2048)
+		reg1 |= 0x00;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS3DH_CTRL_REG4, reg1);
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS3DH_INT1_THS,
+					 private_data->suspend.reg_ths);
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS3DH_INT1_DURATION,
+					 private_data->suspend.reg_dur);
+
+	if (private_data->suspend.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+		reg1 = 0x10;
+		reg2 = 0x00;
+	} else if (private_data->suspend.irq_type ==
+		   MPU_SLAVE_IRQ_TYPE_MOTION) {
+		reg1 = 0x40;
+		reg2 = private_data->suspend.mot_int1_cfg;
+	} else {
+		reg1 = 0x00;
+		reg2 = 0x00;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS3DH_CTRL_REG3, reg1);
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS3DH_INT1_CFG, reg2);
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 LIS3DH_CTRL_REG6, 1, &reg1);
+
+	return result;
+}
+
+static int lis3dh_resume(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char reg1;
+	unsigned char reg2;
+	struct lis3dh_private_data *private_data = pdata->private_data;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS3DH_CTRL_REG1,
+					 private_data->resume.ctrl_reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(6);
+
+	/* Full Scale */
+	reg1 = 0x48;
+	if (private_data->suspend.fsr == 16384)
+		reg1 |= 0x30;
+	else if (private_data->suspend.fsr == 8192)
+		reg1 |= 0x20;
+	else if (private_data->suspend.fsr == 4096)
+		reg1 |= 0x10;
+	else if (private_data->suspend.fsr == 2048)
+		reg1 |= 0x00;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS3DH_CTRL_REG4, reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+		reg1 = 0x10;
+		reg2 = 0x00;
+	} else if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
+		reg1 = 0x40;
+		reg2 = private_data->resume.mot_int1_cfg;
+	} else {
+		reg1 = 0x00;
+		reg2 = 0x00;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS3DH_CTRL_REG3, reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS3DH_INT1_THS,
+					 private_data->resume.reg_ths);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS3DH_INT1_DURATION,
+					 private_data->resume.reg_dur);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS3DH_INT1_CFG, reg2);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 LIS3DH_CTRL_REG6, 1, &reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+static int lis3dh_read(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data)
+{
+	int result = INV_SUCCESS;
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 LIS3DH_STATUS_REG, 1, data);
+	if (data[0] & 0x0F) {
+		result = inv_serial_read(mlsl_handle, pdata->address,
+					 slave->read_reg, slave->read_len,
+					 data);
+		return result;
+	} else
+		return INV_ERROR_ACCEL_DATA_NOT_READY;
+}
+
+static int lis3dh_init(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	int result;
+	long range;
+	struct lis3dh_private_data *private_data;
+	private_data = (struct lis3dh_private_data *)
+	    kzalloc(sizeof(struct lis3dh_private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	pdata->private_data = private_data;
+
+	private_data->resume.ctrl_reg1 = 0x67;
+	private_data->suspend.ctrl_reg1 = 0x18;
+	private_data->resume.mot_int1_cfg = 0x95;
+	private_data->suspend.mot_int1_cfg = 0x2a;
+
+	lis3dh_set_odr(mlsl_handle, pdata, &private_data->suspend, false, 0);
+	lis3dh_set_odr(mlsl_handle, pdata, &private_data->resume,
+		       false, 200000L);
+
+	range = range_fixedpoint_to_long_mg(slave->range);
+	lis3dh_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+			false, range);
+	lis3dh_set_fsr(mlsl_handle, pdata, &private_data->resume,
+			false, range);
+
+	lis3dh_set_ths(mlsl_handle, pdata, &private_data->suspend,
+			false, 80);
+	lis3dh_set_ths(mlsl_handle, pdata, &private_data->resume,
+			false, 40);
+
+	lis3dh_set_dur(mlsl_handle, pdata, &private_data->suspend,
+			false, 1000);
+	lis3dh_set_dur(mlsl_handle, pdata, &private_data->resume,
+			false, 2540);
+
+	lis3dh_set_irq(mlsl_handle, pdata, &private_data->suspend,
+			false, MPU_SLAVE_IRQ_TYPE_NONE);
+	lis3dh_set_irq(mlsl_handle, pdata, &private_data->resume,
+			false, MPU_SLAVE_IRQ_TYPE_NONE);
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 LIS3DH_CTRL_REG1, 0x07);
+	msleep(6);
+
+	return INV_SUCCESS;
+}
+
+static int lis3dh_exit(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	return INV_SUCCESS;
+}
+
+static int lis3dh_config(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata,
+			 struct ext_slave_config *data)
+{
+	struct lis3dh_private_data *private_data = pdata->private_data;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		return lis3dh_set_odr(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		return lis3dh_set_odr(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		return lis3dh_set_fsr(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		return lis3dh_set_fsr(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		return lis3dh_set_ths(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		return lis3dh_set_ths(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		return lis3dh_set_dur(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		return lis3dh_set_dur(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		return lis3dh_set_irq(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		return lis3dh_set_irq(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply, *((long *)data->data));
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+	return INV_SUCCESS;
+}
+
+static int lis3dh_get_config(void *mlsl_handle,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata,
+			     struct ext_slave_config *data)
+{
+	struct lis3dh_private_data *private_data = pdata->private_data;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.odr;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.odr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.ths;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.ths;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.dur;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.dur;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.irq_type;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.irq_type;
+		break;
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static struct ext_slave_descr lis3dh_descr = {
+	.init             = lis3dh_init,
+	.exit             = lis3dh_exit,
+	.suspend          = lis3dh_suspend,
+	.resume           = lis3dh_resume,
+	.read             = lis3dh_read,
+	.config           = lis3dh_config,
+	.get_config       = lis3dh_get_config,
+	.name             = "lis3dh",
+	.type             = EXT_SLAVE_TYPE_ACCEL,
+	.id               = ACCEL_ID_LIS3DH,
+	.read_reg         = 0x28 | 0x80, /* 0x80 for burst reads */
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_BIG_ENDIAN,
+	.range            = {2, 480},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *lis3dh_get_slave_descr(void)
+{
+	return &lis3dh_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct lis3dh_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int lis3dh_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct lis3dh_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					lis3dh_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int lis3dh_mod_remove(struct i2c_client *client)
+{
+	struct lis3dh_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				lis3dh_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id lis3dh_mod_id[] = {
+	{ "lis3dh", ACCEL_ID_LIS3DH },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, lis3dh_mod_id);
+
+static struct i2c_driver lis3dh_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = lis3dh_mod_probe,
+	.remove = lis3dh_mod_remove,
+	.id_table = lis3dh_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "lis3dh_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init lis3dh_mod_init(void)
+{
+	int res = i2c_add_driver(&lis3dh_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "lis3dh_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit lis3dh_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&lis3dh_mod_driver);
+}
+
+module_init(lis3dh_mod_init);
+module_exit(lis3dh_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate LIS3DH sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("lis3dh_mod");
+
+/*
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/lsm303dlx_a.c b/drivers/misc/inv_mpu/accel/lsm303dlx_a.c
new file mode 100644
index 000000000000..576282a0fb16
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/lsm303dlx_a.c
@@ -0,0 +1,881 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   lsm303dlx_a.c
+ *      @brief  Accelerometer setup and handling methods for ST LSM303DLH
+ *              or LSM303DLM accel.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* -------------------------------------------------------------------------- */
+
+/* full scale setting - register & mask */
+#define LSM303DLx_CTRL_REG1         (0x20)
+#define LSM303DLx_CTRL_REG2         (0x21)
+#define LSM303DLx_CTRL_REG3         (0x22)
+#define LSM303DLx_CTRL_REG4         (0x23)
+#define LSM303DLx_CTRL_REG5         (0x24)
+#define LSM303DLx_HP_FILTER_RESET   (0x25)
+#define LSM303DLx_REFERENCE         (0x26)
+#define LSM303DLx_STATUS_REG        (0x27)
+#define LSM303DLx_OUT_X_L           (0x28)
+#define LSM303DLx_OUT_X_H           (0x29)
+#define LSM303DLx_OUT_Y_L           (0x2a)
+#define LSM303DLx_OUT_Y_H           (0x2b)
+#define LSM303DLx_OUT_Z_L           (0x2b)
+#define LSM303DLx_OUT_Z_H           (0x2d)
+
+#define LSM303DLx_INT1_CFG          (0x30)
+#define LSM303DLx_INT1_SRC          (0x31)
+#define LSM303DLx_INT1_THS          (0x32)
+#define LSM303DLx_INT1_DURATION     (0x33)
+
+#define LSM303DLx_INT2_CFG          (0x34)
+#define LSM303DLx_INT2_SRC          (0x35)
+#define LSM303DLx_INT2_THS          (0x36)
+#define LSM303DLx_INT2_DURATION     (0x37)
+
+#define LSM303DLx_CTRL_MASK         (0x30)
+#define LSM303DLx_SLEEP_MASK        (0x20)
+#define LSM303DLx_PWR_MODE_NORMAL   (0x20)
+
+#define LSM303DLx_MAX_DUR           (0x7F)
+
+/* -------------------------------------------------------------------------- */
+
+struct lsm303dlx_a_config {
+	unsigned int odr;
+	unsigned int fsr; /** < full scale range mg */
+	unsigned int ths; /** < Motion no-motion thseshold mg */
+	unsigned int dur; /** < Motion no-motion duration ms */
+	unsigned char reg_ths;
+	unsigned char reg_dur;
+	unsigned char ctrl_reg1;
+	unsigned char irq_type;
+	unsigned char mot_int1_cfg;
+};
+
+struct lsm303dlx_a_private_data {
+	struct lsm303dlx_a_config suspend;
+	struct lsm303dlx_a_config resume;
+};
+
+/* -------------------------------------------------------------------------- */
+
+static int lsm303dlx_a_set_ths(void *mlsl_handle,
+			       struct ext_slave_platform_data *pdata,
+			       struct lsm303dlx_a_config *config,
+			       int apply,
+			       long ths)
+{
+	int result = INV_SUCCESS;
+	if ((unsigned int) ths >= config->fsr)
+		ths = (long) config->fsr - 1;
+
+	if (ths < 0)
+		ths = 0;
+
+	config->ths = ths;
+	config->reg_ths = (unsigned char)(long)((ths * 128L) / (config->fsr));
+	MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths);
+	if (apply)
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					LSM303DLx_INT1_THS,
+					config->reg_ths);
+	return result;
+}
+
+static int lsm303dlx_a_set_dur(void *mlsl_handle,
+			       struct ext_slave_platform_data *pdata,
+			       struct lsm303dlx_a_config *config,
+			       int apply,
+			       long dur)
+{
+	int result = INV_SUCCESS;
+	long reg_dur = (dur * config->odr) / 1000000L;
+	config->dur = dur;
+
+	if (reg_dur > LSM303DLx_MAX_DUR)
+		reg_dur = LSM303DLx_MAX_DUR;
+
+	config->reg_dur = (unsigned char) reg_dur;
+	MPL_LOGV("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur);
+	if (apply)
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					LSM303DLx_INT1_DURATION,
+					(unsigned char)reg_dur);
+	return result;
+}
+
+/**
+ * Sets the IRQ to fire when one of the IRQ events occur.  Threshold and
+ * duration will not be used uless the type is MOT or NMOT.
+ *
+ * @param config configuration to apply to, suspend or resume
+ * @param irq_type The type of IRQ.  Valid values are
+ * - MPU_SLAVE_IRQ_TYPE_NONE
+ * - MPU_SLAVE_IRQ_TYPE_MOTION
+ * - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ */
+static int lsm303dlx_a_set_irq(void *mlsl_handle,
+			       struct ext_slave_platform_data *pdata,
+			       struct lsm303dlx_a_config *config,
+			       int apply,
+			       long irq_type)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg1;
+	unsigned char reg2;
+
+	config->irq_type = (unsigned char)irq_type;
+	if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+		reg1 = 0x02;
+		reg2 = 0x00;
+	} else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
+		reg1 = 0x00;
+		reg2 = config->mot_int1_cfg;
+	} else {
+		reg1 = 0x00;
+		reg2 = 0x00;
+	}
+
+	if (apply) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					LSM303DLx_CTRL_REG3, reg1);
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					LSM303DLx_INT1_CFG, reg2);
+	}
+
+	return result;
+}
+
+/**
+ *  @brief Set the output data rate for the particular configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             Config to modify with new ODR.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param odr
+ *             Output data rate in units of 1/1000Hz (mHz).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_set_odr(void *mlsl_handle,
+			       struct ext_slave_platform_data *pdata,
+			       struct lsm303dlx_a_config *config,
+			       int apply,
+			       long odr)
+{
+	unsigned char bits;
+	int result = INV_SUCCESS;
+
+	/* normal power modes */
+	if (odr > 400000) {
+		config->odr = 1000000;
+		bits = LSM303DLx_PWR_MODE_NORMAL | 0x18;
+	} else if (odr > 100000) {
+		config->odr = 400000;
+		bits = LSM303DLx_PWR_MODE_NORMAL | 0x10;
+	} else if (odr > 50000) {
+		config->odr = 100000;
+		bits = LSM303DLx_PWR_MODE_NORMAL | 0x08;
+	} else if (odr > 10000) {
+		config->odr = 50000;
+		bits = LSM303DLx_PWR_MODE_NORMAL | 0x00;
+	/* low power modes */
+	} else if (odr > 5000) {
+		config->odr = 10000;
+		bits = 0xC0;
+	} else if (odr > 2000) {
+		config->odr = 5000;
+		bits = 0xA0;
+	} else if (odr > 1000) {
+		config->odr = 2000;
+		bits = 0x80;
+	} else if (odr > 500) {
+		config->odr = 1000;
+		bits = 0x60;
+	} else if (odr > 0) {
+		config->odr = 500;
+		bits = 0x40;
+	} else {
+		config->odr = 0;
+		bits = 0;
+	}
+
+	config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0x7);
+	lsm303dlx_a_set_dur(mlsl_handle, pdata, config, apply, config->dur);
+	MPL_LOGV("ODR: %d, 0x%02x\n", config->odr, (int)config->ctrl_reg1);
+	if (apply)
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					LSM303DLx_CTRL_REG1,
+					config->ctrl_reg1);
+	return result;
+}
+
+/**
+ *  @brief Set the full scale range of the accels
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             pointer to configuration.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param fsr
+ *             requested full scale range.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_set_fsr(void *mlsl_handle,
+			       struct ext_slave_platform_data *pdata,
+			       struct lsm303dlx_a_config *config,
+			       int apply,
+			       long fsr)
+{
+	unsigned char reg1 = 0x40;
+	int result = INV_SUCCESS;
+
+	if (fsr <= 2048) {
+		config->fsr = 2048;
+	} else if (fsr <= 4096) {
+		reg1 |= 0x30;
+		config->fsr = 4096;
+	} else {
+		reg1 |= 0x10;
+		config->fsr = 8192;
+	}
+
+	lsm303dlx_a_set_ths(mlsl_handle, pdata,
+			config, apply, config->ths);
+	MPL_LOGV("FSR: %d\n", config->fsr);
+	if (apply)
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					LSM303DLx_CTRL_REG4, reg1);
+
+	return result;
+}
+
+/**
+ *  @brief suspends the device to put it in its lowest power mode.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_suspend(void *mlsl_handle,
+			       struct ext_slave_descr *slave,
+			       struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg1;
+	unsigned char reg2;
+	struct lsm303dlx_a_private_data *private_data =
+		(struct lsm303dlx_a_private_data *)(pdata->private_data);
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       LSM303DLx_CTRL_REG1,
+				       private_data->suspend.ctrl_reg1);
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       LSM303DLx_CTRL_REG2, 0x0f);
+	reg1 = 0x40;
+	if (private_data->suspend.fsr == 8192)
+		reg1 |= 0x30;
+	else if (private_data->suspend.fsr == 4096)
+		reg1 |= 0x10;
+	/* else bits [4..5] are already zero */
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       LSM303DLx_CTRL_REG4, reg1);
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       LSM303DLx_INT1_THS,
+				       private_data->suspend.reg_ths);
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       LSM303DLx_INT1_DURATION,
+				       private_data->suspend.reg_dur);
+
+	if (private_data->suspend.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+		reg1 = 0x02;
+		reg2 = 0x00;
+	} else if (private_data->suspend.irq_type ==
+		   MPU_SLAVE_IRQ_TYPE_MOTION) {
+		reg1 = 0x00;
+		reg2 = private_data->suspend.mot_int1_cfg;
+	} else {
+		reg1 = 0x00;
+		reg2 = 0x00;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       LSM303DLx_CTRL_REG3, reg1);
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       LSM303DLx_INT1_CFG, reg2);
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				LSM303DLx_HP_FILTER_RESET, 1, &reg1);
+	return result;
+}
+
+/**
+ *  @brief resume the device in the proper power state given the configuration
+ *         chosen.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_resume(void *mlsl_handle,
+			      struct ext_slave_descr *slave,
+			      struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg1;
+	unsigned char reg2;
+	struct lsm303dlx_a_private_data *private_data =
+		(struct lsm303dlx_a_private_data *)(pdata->private_data);
+
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       LSM303DLx_CTRL_REG1,
+				       private_data->resume.ctrl_reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(6);
+
+	/* Full Scale */
+	reg1 = 0x40;
+	if (private_data->resume.fsr == 8192)
+		reg1 |= 0x30;
+	else if (private_data->resume.fsr == 4096)
+		reg1 |= 0x10;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       LSM303DLx_CTRL_REG4, reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Configure high pass filter */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       LSM303DLx_CTRL_REG2, 0x0F);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+		reg1 = 0x02;
+		reg2 = 0x00;
+	} else if (private_data->resume.irq_type ==
+		   MPU_SLAVE_IRQ_TYPE_MOTION) {
+		reg1 = 0x00;
+		reg2 = private_data->resume.mot_int1_cfg;
+	} else {
+		reg1 = 0x00;
+		reg2 = 0x00;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       LSM303DLx_CTRL_REG3, reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       LSM303DLx_INT1_THS,
+				       private_data->resume.reg_ths);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       LSM303DLx_INT1_DURATION,
+				       private_data->resume.reg_dur);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       LSM303DLx_INT1_CFG, reg2);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				LSM303DLx_HP_FILTER_RESET, 1, &reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+/**
+ *  @brief read the sensor data from the device.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a buffer to store the data read.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_read(void *mlsl_handle,
+			    struct ext_slave_descr *slave,
+			    struct ext_slave_platform_data *pdata,
+			    unsigned char *data)
+{
+	int result = INV_SUCCESS;
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				LSM303DLx_STATUS_REG, 1, data);
+	if (data[0] & 0x0F) {
+		result = inv_serial_read(mlsl_handle, pdata->address,
+					slave->read_reg, slave->read_len, data);
+	return result;
+	} else
+		return INV_ERROR_ACCEL_DATA_NOT_READY;
+}
+
+/**
+ *  @brief one-time device driver initialization function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is loaded in the kernel.
+ *         If the driver is built-in in the kernel, this function will be
+ *         called at boot time.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_init(void *mlsl_handle,
+			    struct ext_slave_descr *slave,
+			    struct ext_slave_platform_data *pdata)
+{
+	long range;
+	struct lsm303dlx_a_private_data *private_data;
+	private_data = (struct lsm303dlx_a_private_data *)
+	    kzalloc(sizeof(struct lsm303dlx_a_private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	pdata->private_data = private_data;
+
+	private_data->resume.ctrl_reg1 = 0x37;
+	private_data->suspend.ctrl_reg1 = 0x47;
+	private_data->resume.mot_int1_cfg = 0x95;
+	private_data->suspend.mot_int1_cfg = 0x2a;
+
+	lsm303dlx_a_set_odr(mlsl_handle, pdata, &private_data->suspend,
+			false, 0);
+	lsm303dlx_a_set_odr(mlsl_handle, pdata, &private_data->resume,
+			false, 200000);
+
+	range = range_fixedpoint_to_long_mg(slave->range);
+	lsm303dlx_a_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+			false, range);
+	lsm303dlx_a_set_fsr(mlsl_handle, pdata, &private_data->resume,
+			false, range);
+
+	lsm303dlx_a_set_ths(mlsl_handle, pdata, &private_data->suspend,
+			false, 80);
+	lsm303dlx_a_set_ths(mlsl_handle, pdata, &private_data->resume,
+			false, 40);
+
+	lsm303dlx_a_set_dur(mlsl_handle, pdata, &private_data->suspend,
+			false, 1000);
+	lsm303dlx_a_set_dur(mlsl_handle, pdata, &private_data->resume,
+			false, 2540);
+
+	lsm303dlx_a_set_irq(mlsl_handle, pdata, &private_data->suspend,
+			false, MPU_SLAVE_IRQ_TYPE_NONE);
+	lsm303dlx_a_set_irq(mlsl_handle, pdata, &private_data->resume,
+			false, MPU_SLAVE_IRQ_TYPE_NONE);
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief one-time device driver exit function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is removed from the kernel.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_exit(void *mlsl_handle,
+			    struct ext_slave_descr *slave,
+			    struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief device configuration facility.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to the configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_config(void *mlsl_handle,
+			      struct ext_slave_descr *slave,
+			      struct ext_slave_platform_data *pdata,
+			      struct ext_slave_config *data)
+{
+	struct lsm303dlx_a_private_data *private_data = pdata->private_data;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		return lsm303dlx_a_set_odr(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		return lsm303dlx_a_set_odr(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		return lsm303dlx_a_set_fsr(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		return lsm303dlx_a_set_fsr(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		return lsm303dlx_a_set_ths(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		return lsm303dlx_a_set_ths(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		return lsm303dlx_a_set_dur(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		return lsm303dlx_a_set_dur(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		return lsm303dlx_a_set_irq(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		return lsm303dlx_a_set_irq(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief facility to retrieve the device configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to store the returned configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int lsm303dlx_a_get_config(void *mlsl_handle,
+				  struct ext_slave_descr *slave,
+				  struct ext_slave_platform_data *pdata,
+				  struct ext_slave_config *data)
+{
+	struct lsm303dlx_a_private_data *private_data = pdata->private_data;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.odr;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.odr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.ths;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.ths;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.dur;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.dur;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.irq_type;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.irq_type;
+		break;
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static struct ext_slave_descr lsm303dlx_a_descr = {
+	.init             = lsm303dlx_a_init,
+	.exit             = lsm303dlx_a_exit,
+	.suspend          = lsm303dlx_a_suspend,
+	.resume           = lsm303dlx_a_resume,
+	.read             = lsm303dlx_a_read,
+	.config           = lsm303dlx_a_config,
+	.get_config       = lsm303dlx_a_get_config,
+	.name             = "lsm303dlx_a",
+	.type             = EXT_SLAVE_TYPE_ACCEL,
+	.id               = ACCEL_ID_LSM303DLX,
+	.read_reg         = (0x28 | 0x80), /* 0x80 for burst reads */
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_BIG_ENDIAN,
+	.range            = {2, 480},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *lsm303dlx_a_get_slave_descr(void)
+{
+	return &lsm303dlx_a_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct lsm303dlx_a_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int lsm303dlx_a_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct lsm303dlx_a_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					lsm303dlx_a_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int lsm303dlx_a_mod_remove(struct i2c_client *client)
+{
+	struct lsm303dlx_a_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				lsm303dlx_a_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id lsm303dlx_a_mod_id[] = {
+	{ "lsm303dlx", ACCEL_ID_LSM303DLX },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, lsm303dlx_a_mod_id);
+
+static struct i2c_driver lsm303dlx_a_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = lsm303dlx_a_mod_probe,
+	.remove = lsm303dlx_a_mod_remove,
+	.id_table = lsm303dlx_a_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "lsm303dlx_a_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init lsm303dlx_a_mod_init(void)
+{
+	int res = i2c_add_driver(&lsm303dlx_a_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "lsm303dlx_a_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit lsm303dlx_a_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&lsm303dlx_a_mod_driver);
+}
+
+module_init(lsm303dlx_a_mod_init);
+module_exit(lsm303dlx_a_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate LSM303DLX_A sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("lsm303dlx_a_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/mma8450.c b/drivers/misc/inv_mpu/accel/mma8450.c
new file mode 100644
index 000000000000..f698ee98bf50
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/mma8450.c
@@ -0,0 +1,804 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   mma8450.c
+ *      @brief  Accelerometer setup and handling methods for Freescale MMA8450.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* full scale setting - register & mask */
+#define ACCEL_MMA8450_XYZ_DATA_CFG	(0x16)
+
+#define ACCEL_MMA8450_CTRL_REG1		(0x38)
+#define ACCEL_MMA8450_CTRL_REG2     (0x39)
+#define ACCEL_MMA8450_CTRL_REG4		(0x3B)
+#define ACCEL_MMA8450_CTRL_REG5		(0x3C)
+
+#define ACCEL_MMA8450_CTRL_REG		(0x38)
+#define ACCEL_MMA8450_CTRL_MASK		(0x03)
+
+#define ACCEL_MMA8450_SLEEP_MASK	(0x03)
+
+/* -------------------------------------------------------------------------- */
+
+struct mma8450_config {
+	unsigned int odr;
+	unsigned int fsr;	/** < full scale range mg */
+	unsigned int ths;	/** < Motion no-motion thseshold mg */
+	unsigned int dur;	/** < Motion no-motion duration ms */
+	unsigned char reg_ths;
+	unsigned char reg_dur;
+	unsigned char ctrl_reg1;
+	unsigned char irq_type;
+	unsigned char mot_int1_cfg;
+};
+
+struct mma8450_private_data {
+	struct mma8450_config suspend;
+	struct mma8450_config resume;
+};
+
+
+/* -------------------------------------------------------------------------- */
+
+static int mma8450_set_ths(void *mlsl_handle,
+			struct ext_slave_platform_data *pdata,
+			struct mma8450_config *config,
+			int apply,
+			long ths)
+{
+	return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+static int mma8450_set_dur(void *mlsl_handle,
+			struct ext_slave_platform_data *pdata,
+			struct mma8450_config *config,
+			int apply,
+			long dur)
+{
+	return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+/**
+ *  @brief Sets the IRQ to fire when one of the IRQ events occur.
+ *         Threshold and duration will not be used unless the type is MOT or
+ *         NMOT.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *              configuration to apply to, suspend or resume
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param irq_type
+ *              the type of IRQ.  Valid values are
+ *              - MPU_SLAVE_IRQ_TYPE_NONE
+ *              - MPU_SLAVE_IRQ_TYPE_MOTION
+ *              - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_set_irq(void *mlsl_handle,
+		struct ext_slave_platform_data *pdata,
+		struct mma8450_config *config,
+		int apply,
+		long irq_type)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg1;
+	unsigned char reg2;
+	unsigned char reg3;
+
+	config->irq_type = (unsigned char)irq_type;
+	if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+		reg1 = 0x01;
+		reg2 = 0x01;
+		reg3 = 0x07;
+	} else if (irq_type == MPU_SLAVE_IRQ_TYPE_NONE) {
+		reg1 = 0x00;
+		reg2 = 0x00;
+		reg3 = 0x00;
+	} else {
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	}
+
+	if (apply) {
+		/* XYZ_DATA_CFG: event flag enabled on Z axis */
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					ACCEL_MMA8450_XYZ_DATA_CFG, reg3);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					ACCEL_MMA8450_CTRL_REG4, reg1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					ACCEL_MMA8450_CTRL_REG5, reg2);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	return result;
+}
+
+/**
+ *  @brief Set the output data rate for the particular configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             Config to modify with new ODR.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param odr
+ *             Output data rate in units of 1/1000Hz (mHz).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_set_odr(void *mlsl_handle,
+		struct ext_slave_platform_data *pdata,
+		struct mma8450_config *config,
+		int apply,
+		long odr)
+{
+	unsigned char bits;
+	int result = INV_SUCCESS;
+
+	if (odr > 200000) {
+		config->odr = 400000;
+		bits = 0x00;
+	} else if (odr > 100000) {
+		config->odr = 200000;
+		bits = 0x04;
+	} else if (odr > 50000) {
+		config->odr = 100000;
+		bits = 0x08;
+	} else if (odr > 25000) {
+		config->odr = 50000;
+		bits = 0x0C;
+	} else if (odr > 12500) {
+		config->odr = 25000;
+		bits = 0x40; /* Sleep -> Auto wake mode */
+	} else if (odr > 1563) {
+		config->odr = 12500;
+		bits = 0x10;
+	} else if (odr > 0) {
+		config->odr = 1563;
+		bits = 0x14;
+	} else {
+		config->ctrl_reg1 = 0; /* Set FS1.FS2 to Standby */
+		config->odr = 0;
+		bits = 0;
+	}
+
+	config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0x3);
+	if (apply) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				ACCEL_MMA8450_CTRL_REG1, 0);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				ACCEL_MMA8450_CTRL_REG1, config->ctrl_reg1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		MPL_LOGV("ODR: %d mHz, 0x%02x\n",
+			config->odr, (int)config->ctrl_reg1);
+	}
+	return result;
+}
+
+/**
+ *  @brief Set the full scale range of the accels
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             pointer to configuration.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param fsr
+ *             requested full scale range.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_set_fsr(void *mlsl_handle,
+		struct ext_slave_platform_data *pdata,
+		struct mma8450_config *config,
+		int apply,
+		long fsr)
+{
+	unsigned char bits;
+	int result = INV_SUCCESS;
+
+	if (fsr <= 2000) {
+		bits = 0x01;
+		config->fsr = 2000;
+	} else if (fsr <= 4000) {
+		bits = 0x02;
+		config->fsr = 4000;
+	} else {
+		bits = 0x03;
+		config->fsr = 8000;
+	}
+
+	config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0xFC);
+	if (apply) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				ACCEL_MMA8450_CTRL_REG1, config->ctrl_reg1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		MPL_LOGV("FSR: %d mg\n", config->fsr);
+	}
+	return result;
+}
+
+/**
+ *  @brief suspends the device to put it in its lowest power mode.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_suspend(void *mlsl_handle,
+		    struct ext_slave_descr *slave,
+		    struct ext_slave_platform_data *pdata)
+{
+	int result;
+	struct mma8450_private_data *private_data = pdata->private_data;
+
+	if (private_data->suspend.fsr == 4000)
+		slave->range.mantissa = 4;
+	else if (private_data->suspend.fsr == 8000)
+		slave->range.mantissa = 8;
+	else
+		slave->range.mantissa = 2;
+	slave->range.fraction = 0;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			ACCEL_MMA8450_CTRL_REG1, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	if (private_data->suspend.ctrl_reg1) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				ACCEL_MMA8450_CTRL_REG1,
+				private_data->suspend.ctrl_reg1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	result = mma8450_set_irq(mlsl_handle, pdata,
+				&private_data->suspend,
+				true, private_data->suspend.irq_type);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+/**
+ *  @brief resume the device in the proper power state given the configuration
+ *         chosen.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_resume(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	struct mma8450_private_data *private_data = pdata->private_data;
+
+	/* Full Scale */
+	if (private_data->resume.fsr == 4000)
+		slave->range.mantissa = 4;
+	else if (private_data->resume.fsr == 8000)
+		slave->range.mantissa = 8;
+	else
+		slave->range.mantissa = 2;
+	slave->range.fraction = 0;
+
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			ACCEL_MMA8450_CTRL_REG1, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	if (private_data->resume.ctrl_reg1) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				ACCEL_MMA8450_CTRL_REG1,
+				private_data->resume.ctrl_reg1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	result = mma8450_set_irq(mlsl_handle, pdata,
+			&private_data->resume,
+			true, private_data->resume.irq_type);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+/**
+ *  @brief read the sensor data from the device.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a buffer to store the data read.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_read(void *mlsl_handle,
+		 struct ext_slave_descr *slave,
+		 struct ext_slave_platform_data *pdata, unsigned char *data)
+{
+	int result;
+	unsigned char local_data[4];	/* Status register + 3 bytes data */
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				0x00, sizeof(local_data), local_data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	memcpy(data, &local_data[1], (slave->read_len) - 1);
+
+	MPL_LOGV("Data Not Ready: %02x %02x %02x %02x\n",
+		 local_data[0], local_data[1],
+		 local_data[2], local_data[3]);
+
+	return result;
+}
+
+/**
+ *  @brief one-time device driver initialization function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is loaded in the kernel.
+ *         If the driver is built-in in the kernel, this function will be
+ *         called at boot time.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_init(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	struct mma8450_private_data *private_data;
+	private_data = (struct mma8450_private_data *)
+	    kzalloc(sizeof(struct mma8450_private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	pdata->private_data = private_data;
+
+	mma8450_set_odr(mlsl_handle, pdata, &private_data->suspend,
+			false, 0);
+	mma8450_set_odr(mlsl_handle, pdata, &private_data->resume,
+			false, 200000);
+	mma8450_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+			false, 2000);
+	mma8450_set_fsr(mlsl_handle, pdata, &private_data->resume,
+			false, 2000);
+	mma8450_set_irq(mlsl_handle, pdata, &private_data->suspend,
+			false,
+			MPU_SLAVE_IRQ_TYPE_NONE);
+	mma8450_set_irq(mlsl_handle, pdata, &private_data->resume,
+			false,
+			MPU_SLAVE_IRQ_TYPE_NONE);
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief one-time device driver exit function.
+ *         If the driver is built as a kernel module, this function will be
+ *         called when the module is removed from the kernel.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_exit(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief device configuration facility.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to the configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_config(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			struct ext_slave_config *data)
+{
+	struct mma8450_private_data *private_data = pdata->private_data;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		return mma8450_set_odr(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		return mma8450_set_odr(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		return mma8450_set_fsr(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		return mma8450_set_fsr(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		return mma8450_set_ths(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		return mma8450_set_ths(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		return mma8450_set_dur(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		return mma8450_set_dur(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		return mma8450_set_irq(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		return mma8450_set_irq(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+/**
+ *  @brief facility to retrieve the device configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a pointer to store the returned configuration data structure.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma8450_get_config(void *mlsl_handle,
+				struct ext_slave_descr *slave,
+				struct ext_slave_platform_data *pdata,
+				struct ext_slave_config *data)
+{
+	struct mma8450_private_data *private_data = pdata->private_data;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.odr;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.odr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.ths;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.ths;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.dur;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.dur;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.irq_type;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.irq_type;
+		break;
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static struct ext_slave_descr mma8450_descr = {
+	.init             = mma8450_init,
+	.exit             = mma8450_exit,
+	.suspend          = mma8450_suspend,
+	.resume           = mma8450_resume,
+	.read             = mma8450_read,
+	.config           = mma8450_config,
+	.get_config       = mma8450_get_config,
+	.name             = "mma8450",
+	.type             = EXT_SLAVE_TYPE_ACCEL,
+	.id               = ACCEL_ID_MMA8450,
+	.read_reg         = 0x00,
+	.read_len         = 4,
+	.endian           = EXT_SLAVE_FS8_BIG_ENDIAN,
+	.range            = {2, 0},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *mma8450_get_slave_descr(void)
+{
+	return &mma8450_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct mma8450_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int mma8450_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct mma8450_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					mma8450_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int mma8450_mod_remove(struct i2c_client *client)
+{
+	struct mma8450_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				mma8450_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id mma8450_mod_id[] = {
+	{ "mma8450", ACCEL_ID_MMA8450 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, mma8450_mod_id);
+
+static struct i2c_driver mma8450_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = mma8450_mod_probe,
+	.remove = mma8450_mod_remove,
+	.id_table = mma8450_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "mma8450_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init mma8450_mod_init(void)
+{
+	int res = i2c_add_driver(&mma8450_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "mma8450_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit mma8450_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&mma8450_mod_driver);
+}
+
+module_init(mma8450_mod_init);
+module_exit(mma8450_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate MMA8450 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("mma8450_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/mma845x.c b/drivers/misc/inv_mpu/accel/mma845x.c
new file mode 100644
index 000000000000..5f62b22388b1
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/mma845x.c
@@ -0,0 +1,713 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   mma845x.c
+ *      @brief  Accelerometer setup and handling methods for Freescale MMA845X
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+#define ACCEL_MMA845X_XYZ_DATA_CFG	(0x0E)
+#define ACCEL_MMA845X_CTRL_REG1      (0x2A)
+#define ACCEL_MMA845X_CTRL_REG4		(0x2D)
+#define ACCEL_MMA845X_CTRL_REG5		(0x2E)
+
+#define ACCEL_MMA845X_SLEEP_MASK     (0x01)
+
+/* full scale setting - register & mask */
+#define ACCEL_MMA845X_CFG_REG       (0x0E)
+#define ACCEL_MMA845X_CTRL_MASK     (0x03)
+
+/* -------------------------------------------------------------------------- */
+
+struct mma845x_config {
+	unsigned int odr;
+	unsigned int fsr;		/** < full scale range mg */
+	unsigned int ths;		/** < Motion no-motion thseshold mg */
+	unsigned int dur;		/** < Motion no-motion duration ms */
+	unsigned char reg_ths;
+	unsigned char reg_dur;
+	unsigned char ctrl_reg1;
+	unsigned char irq_type;
+	unsigned char mot_int1_cfg;
+};
+
+struct mma845x_private_data {
+	struct mma845x_config suspend;
+	struct mma845x_config resume;
+};
+
+/* -------------------------------------------------------------------------- */
+
+static int mma845x_set_ths(void *mlsl_handle,
+		struct ext_slave_platform_data *pdata,
+		struct mma845x_config *config,
+		int apply,
+		long ths)
+{
+	return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+static int mma845x_set_dur(void *mlsl_handle,
+		struct ext_slave_platform_data *pdata,
+		struct mma845x_config *config,
+		int apply,
+		long dur)
+{
+	return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+/**
+ *  @brief Sets the IRQ to fire when one of the IRQ events occur.
+ *         Threshold and duration will not be used unless the type is MOT or
+ *         NMOT.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *              configuration to apply to, suspend or resume
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param irq_type
+ *              the type of IRQ.  Valid values are
+ *              - MPU_SLAVE_IRQ_TYPE_NONE
+ *              - MPU_SLAVE_IRQ_TYPE_MOTION
+ *              - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma845x_set_irq(void *mlsl_handle,
+		struct ext_slave_platform_data *pdata,
+		struct mma845x_config *config,
+		int apply,
+		long irq_type)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg1;
+	unsigned char reg2;
+
+	config->irq_type = (unsigned char)irq_type;
+	if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+		reg1 = 0x01;
+		reg2 = 0x01;
+	} else if (irq_type == MPU_SLAVE_IRQ_TYPE_NONE) {
+		reg1 = 0x00;
+		reg2 = 0x00;
+	} else {
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	}
+
+	if (apply) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					ACCEL_MMA845X_CTRL_REG4, reg1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					ACCEL_MMA845X_CTRL_REG5, reg2);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	return result;
+}
+
+/**
+ *  @brief Set the output data rate for the particular configuration.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             Config to modify with new ODR.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param odr
+ *             Output data rate in units of 1/1000Hz (mHz).
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma845x_set_odr(void *mlsl_handle,
+			struct ext_slave_platform_data *pdata,
+			struct mma845x_config *config,
+			int apply,
+			long odr)
+{
+	unsigned char bits;
+	int result = INV_SUCCESS;
+
+	if (odr > 400000) {
+		config->odr = 800000;
+		bits = 0x01;
+	} else if (odr > 200000) {
+		config->odr = 400000;
+		bits = 0x09;
+	} else if (odr > 100000) {
+		config->odr = 200000;
+		bits = 0x11;
+	} else if (odr > 50000) {
+		config->odr = 100000;
+		bits = 0x19;
+	} else if (odr > 12500) {
+		config->odr = 50000;
+		bits = 0x21;
+	} else if (odr > 6250) {
+		config->odr = 12500;
+		bits = 0x29;
+	} else if (odr > 1560) {
+		config->odr = 6250;
+		bits = 0x31;
+	} else if (odr > 0) {
+		config->odr = 1560;
+		bits = 0x39;
+	} else {
+		config->ctrl_reg1 = 0; /* Set FS1.FS2 to Standby */
+		config->odr = 0;
+		bits = 0;
+	}
+
+	config->ctrl_reg1 = bits;
+	if (apply) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				ACCEL_MMA845X_CTRL_REG1,
+				config->ctrl_reg1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		MPL_LOGV("ODR: %d mHz, 0x%02x\n", config->odr,
+			 (int)config->ctrl_reg1);
+	}
+	return result;
+}
+
+/**
+ *  @brief Set the full scale range of the accels
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param config
+ *             pointer to configuration.
+ *  @param apply
+ *             whether to apply immediately or save the settings to be applied
+ *             at the next resume.
+ *  @param fsr
+ *             requested full scale range.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma845x_set_fsr(void *mlsl_handle,
+		struct ext_slave_platform_data *pdata,
+		struct mma845x_config *config,
+		int apply,
+		long fsr)
+{
+	unsigned char bits;
+	int result = INV_SUCCESS;
+
+	if (fsr <= 2000) {
+		bits = 0x00;
+		config->fsr = 2000;
+	} else if (fsr <= 4000) {
+		bits = 0x01;
+		config->fsr = 4000;
+	} else {
+		bits = 0x02;
+		config->fsr = 8000;
+	}
+
+	if (apply) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+				ACCEL_MMA845X_XYZ_DATA_CFG,
+				bits);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		MPL_LOGV("FSR: %d mg\n", config->fsr);
+	}
+	return result;
+}
+
+/**
+ *  @brief suspends the device to put it in its lowest power mode.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma845x_suspend(void *mlsl_handle,
+		    struct ext_slave_descr *slave,
+		    struct ext_slave_platform_data *pdata)
+{
+	int result;
+	struct mma845x_private_data *private_data = pdata->private_data;
+
+	/* Full Scale */
+	if (private_data->suspend.fsr == 4000)
+		slave->range.mantissa = 4;
+	else if (private_data->suspend.fsr == 8000)
+		slave->range.mantissa = 8;
+	else
+		slave->range.mantissa = 2;
+
+	slave->range.fraction = 0;
+
+	result = mma845x_set_fsr(mlsl_handle, pdata,
+				&private_data->suspend,
+				true, private_data->suspend.fsr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					ACCEL_MMA845X_CTRL_REG1,
+					private_data->suspend.ctrl_reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+/**
+ *  @brief resume the device in the proper power state given the configuration
+ *         chosen.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma845x_resume(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	struct mma845x_private_data *private_data = pdata->private_data;
+
+	/* Full Scale */
+	if (private_data->resume.fsr == 4000)
+		slave->range.mantissa = 4;
+	else if (private_data->resume.fsr == 8000)
+		slave->range.mantissa = 8;
+	else
+		slave->range.mantissa = 2;
+
+	slave->range.fraction = 0;
+
+	result = mma845x_set_fsr(mlsl_handle, pdata,
+			&private_data->resume,
+			true, private_data->resume.fsr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+			ACCEL_MMA845X_CTRL_REG1,
+			private_data->resume.ctrl_reg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+/**
+ *  @brief read the sensor data from the device.
+ *
+ *  @param mlsl_handle
+ *             the handle to the serial channel the device is connected to.
+ *  @param slave
+ *             a pointer to the slave descriptor data structure.
+ *  @param pdata
+ *             a pointer to the slave platform data.
+ *  @param data
+ *             a buffer to store the data read.
+ *
+ *  @return INV_SUCCESS if successful or a non-zero error code.
+ */
+static int mma845x_read(void *mlsl_handle,
+		 struct ext_slave_descr *slave,
+		 struct ext_slave_platform_data *pdata, unsigned char *data)
+{
+	int result;
+	unsigned char local_data[7];	/* Status register + 6 bytes data */
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 slave->read_reg, sizeof(local_data),
+				 local_data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	memcpy(data, &local_data[1], slave->read_len);
+	return result;
+}
+
+static int mma845x_init(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	long range;
+	struct mma845x_private_data *private_data;
+	private_data = (struct mma845x_private_data *)
+	    kzalloc(sizeof(struct mma845x_private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	pdata->private_data = private_data;
+
+	mma845x_set_odr(mlsl_handle, pdata, &private_data->suspend,
+			false, 0);
+	mma845x_set_odr(mlsl_handle, pdata, &private_data->resume,
+			false, 200000);
+
+	range = range_fixedpoint_to_long_mg(slave->range);
+	mma845x_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+			false, range);
+	mma845x_set_fsr(mlsl_handle, pdata, &private_data->resume,
+			false, range);
+
+	mma845x_set_irq(mlsl_handle, pdata, &private_data->suspend,
+			false, MPU_SLAVE_IRQ_TYPE_NONE);
+	mma845x_set_irq(mlsl_handle, pdata, &private_data->resume,
+			false, MPU_SLAVE_IRQ_TYPE_NONE);
+	return INV_SUCCESS;
+}
+
+static int mma845x_exit(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	return INV_SUCCESS;
+}
+
+static int mma845x_config(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			struct ext_slave_config *data)
+{
+	struct mma845x_private_data *private_data = pdata->private_data;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		return mma845x_set_odr(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		return mma845x_set_odr(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		return mma845x_set_fsr(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		return mma845x_set_fsr(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		return mma845x_set_ths(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		return mma845x_set_ths(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		return mma845x_set_dur(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		return mma845x_set_dur(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		return mma845x_set_irq(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		return mma845x_set_irq(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static int mma845x_get_config(void *mlsl_handle,
+				struct ext_slave_descr *slave,
+				struct ext_slave_platform_data *pdata,
+				struct ext_slave_config *data)
+{
+	struct mma845x_private_data *private_data = pdata->private_data;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.odr;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.odr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.ths;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.ths;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.dur;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.dur;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.irq_type;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.irq_type;
+		break;
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static struct ext_slave_descr mma845x_descr = {
+	.init             = mma845x_init,
+	.exit             = mma845x_exit,
+	.suspend          = mma845x_suspend,
+	.resume           = mma845x_resume,
+	.read             = mma845x_read,
+	.config           = mma845x_config,
+	.get_config       = mma845x_get_config,
+	.name             = "mma845x",
+	.type             = EXT_SLAVE_TYPE_ACCEL,
+	.id               = ACCEL_ID_MMA845X,
+	.read_reg         = 0x00,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_FS16_BIG_ENDIAN,
+	.range            = {2, 0},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *mma845x_get_slave_descr(void)
+{
+	return &mma845x_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct mma845x_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int mma845x_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct mma845x_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					mma845x_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int mma845x_mod_remove(struct i2c_client *client)
+{
+	struct mma845x_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				mma845x_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id mma845x_mod_id[] = {
+	{ "mma845x", ACCEL_ID_MMA845X },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, mma845x_mod_id);
+
+static struct i2c_driver mma845x_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = mma845x_mod_probe,
+	.remove = mma845x_mod_remove,
+	.id_table = mma845x_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "mma845x_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init mma845x_mod_init(void)
+{
+	int res = i2c_add_driver(&mma845x_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "mma845x_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit mma845x_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&mma845x_mod_driver);
+}
+
+module_init(mma845x_mod_init);
+module_exit(mma845x_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate MMA845X sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("mma845x_mod");
+
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/mpu6050.c b/drivers/misc/inv_mpu/accel/mpu6050.c
new file mode 100644
index 000000000000..c5bb6784a411
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/mpu6050.c
@@ -0,0 +1,695 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup ACCELDL
+ *  @brief      Provides the interface to setup and handle an accelerometer.
+ *
+ *  @{
+ *      @file   mpu6050.c
+ *      @brief  Accelerometer setup and handling methods for Invensense MPU6050
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mpu6050b1.h"
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/* -------------------------------------------------------------------------- */
+
+struct mpu6050_config {
+	unsigned int odr;		/**< output data rate 1/1000 Hz */
+	unsigned int fsr;		/**< full scale range mg */
+	unsigned int ths;		/**< mot/no-mot thseshold mg */
+	unsigned int dur;		/**< mot/no-mot duration ms */
+	unsigned int irq_type;		/**< irq type */
+};
+
+struct mpu6050_private_data {
+	struct mpu6050_config suspend;
+	struct mpu6050_config resume;
+	struct mldl_cfg *mldl_cfg_ref;
+};
+
+/* -------------------------------------------------------------------------- */
+
+static int mpu6050_set_mldl_cfg_ref(void *mlsl_handle,
+			struct ext_slave_platform_data *pdata,
+			struct mldl_cfg *mldl_cfg_ref)
+{
+	struct mpu6050_private_data *private_data =
+			(struct mpu6050_private_data *)pdata->private_data;
+	private_data->mldl_cfg_ref = mldl_cfg_ref;
+	return 0;
+}
+static int mpu6050_set_lp_mode(void *mlsl_handle,
+			struct ext_slave_platform_data *pdata,
+			unsigned char lpa_freq)
+{
+	unsigned char b = 0;
+	/* Reducing the duration setting for lp mode */
+	b = 1;
+	inv_serial_single_write(mlsl_handle, pdata->address,
+				MPUREG_ACCEL_MOT_DUR, b);
+	/* Setting the cycle bit and LPA wake up freq */
+	inv_serial_read(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, 1,
+			&b);
+	b |= BIT_CYCLE | BIT_PD_PTAT;
+	inv_serial_single_write(mlsl_handle, pdata->address,
+				MPUREG_PWR_MGMT_1,
+				b);
+	inv_serial_read(mlsl_handle, pdata->address,
+			MPUREG_PWR_MGMT_2, 1, &b);
+	b |= lpa_freq & BITS_LPA_WAKE_CTRL;
+	inv_serial_single_write(mlsl_handle, pdata->address,
+				MPUREG_PWR_MGMT_2, b);
+
+	return INV_SUCCESS;
+}
+
+static int mpu6050_set_fp_mode(void *mlsl_handle,
+				struct ext_slave_platform_data *pdata)
+{
+	unsigned char b;
+	struct mpu6050_private_data *private_data =
+			(struct mpu6050_private_data *)pdata->private_data;
+	/* Resetting the cycle bit and LPA wake up freq */
+	inv_serial_read(mlsl_handle, pdata->address,
+			MPUREG_PWR_MGMT_1, 1, &b);
+	b &= ~BIT_CYCLE & ~BIT_PD_PTAT;
+	inv_serial_single_write(mlsl_handle, pdata->address,
+				MPUREG_PWR_MGMT_1, b);
+	inv_serial_read(mlsl_handle, pdata->address,
+			MPUREG_PWR_MGMT_2, 1, &b);
+	b &= ~BITS_LPA_WAKE_CTRL;
+	inv_serial_single_write(mlsl_handle, pdata->address,
+				MPUREG_PWR_MGMT_2, b);
+	/* Resetting the duration setting for fp mode */
+	b = (unsigned char)private_data->suspend.ths / ACCEL_MOT_DUR_LSB;
+	inv_serial_single_write(mlsl_handle, pdata->address,
+				MPUREG_ACCEL_MOT_DUR, b);
+
+	return INV_SUCCESS;
+}
+/**
+ * Record the odr for use in computing duration values.
+ *
+ * @param config Config to set, suspend or resume structure
+ * @param odr output data rate in 1/1000 hz
+ */
+static int mpu6050_set_odr(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct mpu6050_config *config, long apply, long odr)
+{
+	int result;
+	unsigned char b;
+	unsigned char lpa_freq = 1; /* Default value */
+	long base;
+	int total_divider;
+	struct mpu6050_private_data *private_data =
+			(struct mpu6050_private_data *)pdata->private_data;
+	struct mldl_cfg *mldl_cfg_ref =
+			(struct mldl_cfg *)private_data->mldl_cfg_ref;
+
+	if (mldl_cfg_ref) {
+		base = 1000 *
+			inv_mpu_get_sampling_rate_hz(mldl_cfg_ref->mpu_gyro_cfg)
+			* (mldl_cfg_ref->mpu_gyro_cfg->divider + 1);
+	} else {
+		/* have no reference to mldl_cfg => assume base rate is 1000 */
+		base = 1000000L;
+	}
+
+	if (odr != 0) {
+		total_divider = (base / odr) - 1;
+		/* final odr MAY be different from requested odr due to
+		   integer truncation */
+		config->odr = base / (total_divider + 1);
+	} else {
+		config->odr = 0;
+		return 0;
+	}
+
+	/* if the DMP and/or gyros are on, don't set the ODR =>
+	   the DMP/gyro mldl_cfg->divider setting will handle it */
+	if (apply
+	    && (mldl_cfg_ref &&
+	    !(mldl_cfg_ref->inv_mpu_cfg->requested_sensors &
+		    INV_DMP_PROCESSOR))) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					MPUREG_SMPLRT_DIV,
+					(unsigned char)total_divider);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		MPL_LOGI("ODR : %d mHz\n", config->odr);
+	}
+	/* Decide whether to put accel in LP mode or pull out of LP mode
+	   based on the odr. */
+	switch (odr) {
+	case 1000:
+		lpa_freq = BITS_LPA_WAKE_1HZ;
+		break;
+	case 2000:
+		lpa_freq = BITS_LPA_WAKE_2HZ;
+		break;
+	case 10000:
+		lpa_freq = BITS_LPA_WAKE_10HZ;
+		break;
+	case 40000:
+		lpa_freq = BITS_LPA_WAKE_40HZ;
+		break;
+	default:
+		inv_serial_read(mlsl_handle, pdata->address,
+				MPUREG_PWR_MGMT_1, 1, &b);
+		b &= BIT_CYCLE;
+		if (b == BIT_CYCLE) {
+			MPL_LOGI(" Accel LP - > FP mode. \n ");
+			mpu6050_set_fp_mode(mlsl_handle, pdata);
+		}
+	}
+	/* If lpa_freq default value was changed, set into LP mode */
+	if (lpa_freq != 1) {
+		MPL_LOGI(" Accel FP - > LP mode. \n ");
+		mpu6050_set_lp_mode(mlsl_handle, pdata, lpa_freq);
+	}
+	return 0;
+}
+
+static int mpu6050_set_fsr(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct mpu6050_config *config, long apply, long fsr)
+{
+	unsigned char fsr_mask;
+	int result;
+
+	if (fsr <= 2000) {
+		config->fsr = 2000;
+		fsr_mask = 0x00;
+	} else if (fsr <= 4000) {
+		config->fsr = 4000;
+		fsr_mask = 0x08;
+	} else if (fsr <= 8000) {
+		config->fsr = 8000;
+		fsr_mask = 0x10;
+	} else { /* fsr = [8001, oo) */
+		config->fsr = 16000;
+		fsr_mask = 0x18;
+	}
+
+	if (apply) {
+		unsigned char reg;
+		result = inv_serial_read(mlsl_handle, pdata->address,
+					 MPUREG_ACCEL_CONFIG, 1, &reg);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 MPUREG_ACCEL_CONFIG,
+						 reg | fsr_mask);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		MPL_LOGV("FSR: %d\n", config->fsr);
+	}
+	return 0;
+}
+
+static int mpu6050_set_irq(void *mlsl_handle,
+			  struct ext_slave_platform_data *pdata,
+			  struct mpu6050_config *config, long apply,
+			  long irq_type)
+{
+	int result;
+	unsigned char reg_int_cfg;
+
+	switch (irq_type) {
+	case MPU_SLAVE_IRQ_TYPE_DATA_READY:
+		config->irq_type = irq_type;
+		reg_int_cfg = BIT_RAW_RDY_EN;
+		break;
+	/* todo: add MOTION, NO_MOTION, and FREEFALL */
+	case MPU_SLAVE_IRQ_TYPE_NONE:
+		/* Do nothing, not even set the interrupt because it is
+		   shared with the gyro */
+		config->irq_type = irq_type;
+		return 0;
+	default:
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	if (apply) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+						 MPUREG_INT_ENABLE,
+						 reg_int_cfg);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		MPL_LOGV("irq_type: %d\n", config->irq_type);
+	}
+
+	return 0;
+}
+
+static int mpu6050_set_ths(void *mlsl_handle,
+			  struct ext_slave_platform_data *slave,
+			  struct mpu6050_config *config, long apply, long ths)
+{
+	if (ths < 0)
+		ths = 0;
+
+	config->ths = ths;
+	MPL_LOGV("THS: %d\n", config->ths);
+	return 0;
+}
+
+static int mpu6050_set_dur(void *mlsl_handle,
+			  struct ext_slave_platform_data *slave,
+			  struct mpu6050_config *config, long apply, long dur)
+{
+	if (dur < 0)
+		dur = 0;
+
+	config->dur = dur;
+	MPL_LOGV("DUR: %d\n", config->dur);
+	return 0;
+}
+
+
+static int mpu6050_init(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	int result;
+	struct mpu6050_private_data *private_data;
+
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	pdata->private_data = private_data;
+
+	result = mpu6050_set_odr(mlsl_handle, pdata, &private_data->suspend,
+				 false, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = mpu6050_set_odr(mlsl_handle, pdata, &private_data->resume,
+				 false, 200000);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = mpu6050_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+				 false, 2000);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = mpu6050_set_fsr(mlsl_handle, pdata, &private_data->resume,
+				 false, 2000);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = mpu6050_set_irq(mlsl_handle, pdata, &private_data->suspend,
+				 false, MPU_SLAVE_IRQ_TYPE_NONE);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = mpu6050_set_irq(mlsl_handle, pdata, &private_data->resume,
+				 false, MPU_SLAVE_IRQ_TYPE_NONE);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = mpu6050_set_ths(mlsl_handle, pdata, &private_data->suspend,
+				 false, 80);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = mpu6050_set_ths(mlsl_handle, pdata, &private_data->resume,
+				 false, 40);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = mpu6050_set_dur(mlsl_handle, pdata, &private_data->suspend,
+				 false, 1000);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = mpu6050_set_dur(mlsl_handle, pdata, &private_data->resume,
+				 false, 2540);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return 0;
+}
+
+static int mpu6050_exit(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	pdata->private_data = NULL;
+	return 0;
+}
+
+static int mpu6050_suspend(void *mlsl_handle,
+			   struct ext_slave_descr *slave,
+			   struct ext_slave_platform_data *pdata)
+{
+	unsigned char reg;
+	int result;
+	struct mpu6050_private_data *private_data =
+			(struct mpu6050_private_data *)pdata->private_data;
+
+	result = mpu6050_set_odr(mlsl_handle, pdata, &private_data->suspend,
+				true, private_data->suspend.odr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = mpu6050_set_irq(mlsl_handle, pdata, &private_data->suspend,
+				true, private_data->suspend.irq_type);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 MPUREG_PWR_MGMT_2, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	reg |= (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 MPUREG_PWR_MGMT_2, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return 0;
+}
+
+static int mpu6050_resume(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char reg;
+	struct mpu6050_private_data *private_data =
+		(struct mpu6050_private_data *)pdata->private_data;
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 MPUREG_PWR_MGMT_1, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (reg & BIT_SLEEP) {
+		result = inv_serial_single_write(mlsl_handle, pdata->address,
+					MPUREG_PWR_MGMT_1, reg & ~BIT_SLEEP);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	msleep(2);
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+			MPUREG_PWR_MGMT_2, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	reg &= ~(BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       MPUREG_PWR_MGMT_2, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* settings */
+
+	result = mpu6050_set_fsr(mlsl_handle, pdata, &private_data->resume,
+				 true, private_data->resume.fsr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = mpu6050_set_odr(mlsl_handle, pdata, &private_data->resume,
+				 true, private_data->resume.odr);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = mpu6050_set_irq(mlsl_handle, pdata, &private_data->resume,
+				 true, private_data->resume.irq_type);
+
+	/* motion, no_motion */
+	/* TODO : port these in their respective _set_thrs and _set_dur
+		  functions and use the APPLY paremeter to apply just like
+		  _set_odr, _set_irq, and _set_fsr. */
+	reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_THR_LSB;
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 MPUREG_ACCEL_MOT_THR, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	reg = (unsigned char)
+	    ACCEL_ZRMOT_THR_LSB_CONVERSION(private_data->resume.ths);
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 MPUREG_ACCEL_ZRMOT_THR, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_DUR_LSB;
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 MPUREG_ACCEL_MOT_DUR, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	reg = (unsigned char)private_data->resume.ths / ACCEL_ZRMOT_DUR_LSB;
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 MPUREG_ACCEL_ZRMOT_DUR, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return 0;
+}
+
+static int mpu6050_read(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			unsigned char *data)
+{
+	int result;
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 slave->read_reg, slave->read_len, data);
+	return result;
+}
+
+static int mpu6050_config(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata,
+			 struct ext_slave_config *data)
+{
+	struct mpu6050_private_data *private_data =
+		(struct mpu6050_private_data *)pdata->private_data;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		return mpu6050_set_odr(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		return mpu6050_set_odr(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		return mpu6050_set_fsr(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		return mpu6050_set_fsr(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		return mpu6050_set_ths(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		return mpu6050_set_ths(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		return mpu6050_set_dur(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		return mpu6050_set_dur(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		return mpu6050_set_irq(mlsl_handle, pdata,
+				      &private_data->suspend,
+				      data->apply, *((long *)data->data));
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		return mpu6050_set_irq(mlsl_handle, pdata,
+				      &private_data->resume,
+				      data->apply, *((long *)data->data));
+	case MPU_SLAVE_CONFIG_INTERNAL_REFERENCE:
+		return mpu6050_set_mldl_cfg_ref(mlsl_handle, pdata,
+					       (struct mldl_cfg *)data->data);
+		break;
+
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return 0;
+}
+
+static int mpu6050_get_config(void *mlsl_handle,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata,
+			     struct ext_slave_config *data)
+{
+	struct mpu6050_private_data *private_data =
+		(struct mpu6050_private_data *)pdata->private_data;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.odr;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.odr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.ths;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.ths;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.dur;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.dur;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->suspend.irq_type;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		(*(unsigned long *)data->data) =
+		    (unsigned long)private_data->resume.irq_type;
+		break;
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return 0;
+}
+
+static struct ext_slave_descr mpu6050_descr = {
+	.init             = mpu6050_init,
+	.exit             = mpu6050_exit,
+	.suspend          = mpu6050_suspend,
+	.resume           = mpu6050_resume,
+	.read             = mpu6050_read,
+	.config           = mpu6050_config,
+	.get_config       = mpu6050_get_config,
+	.name             = "mpu6050",
+	.type             = EXT_SLAVE_TYPE_ACCEL,
+	.id               = ACCEL_ID_MPU6050,
+	.read_reg         = 0x3B,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_BIG_ENDIAN,
+	.range            = {2, 0},
+	.trigger          = NULL,
+};
+
+struct ext_slave_descr *mpu6050_get_slave_descr(void)
+{
+	return &mpu6050_descr;
+}
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/accel/mpu6050.h b/drivers/misc/inv_mpu/accel/mpu6050.h
new file mode 100644
index 000000000000..c347bcb4d773
--- /dev/null
+++ b/drivers/misc/inv_mpu/accel/mpu6050.h
@@ -0,0 +1,28 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+
+#ifndef __MPU6050_H__
+#define __MPU6050_H__
+
+#include <linux/mpu.h>
+
+struct ext_slave_descr *mpu6050_get_slave_descr(void);
+
+#endif
diff --git a/drivers/misc/inv_mpu/compass/Kconfig b/drivers/misc/inv_mpu/compass/Kconfig
new file mode 100644
index 000000000000..7e6bac87d31e
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/Kconfig
@@ -0,0 +1,130 @@
+menuconfig INV_SENSORS_COMPASS
+	bool "Compass Slave Sensors"
+	default y
+	---help---
+	  Say Y here to get to see options for device drivers for various
+	  compasses. This option alone does not add any kernel code.
+
+	  If you say N, all options in this submenu will be skipped and disabled.
+
+if INV_SENSORS_COMPASS
+
+config MPU_SENSORS_AK8963
+        tristate "AKM ak8963"
+        help
+          This enables support for the AKM ak8963 compass
+          This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one compass can be registered at a time.
+          Specifying more that one compass in the board file will result
+          in runtime errors.
+
+config MPU_SENSORS_AK8975
+	tristate "AKM ak8975"
+	help
+	  This enables support for the AKM ak8975 compass
+	  This support is for integration with the MPU3050 or MPU6050 gyroscope
+	  device driver.  Only one compass can be registered at a time.
+	  Specifying more that one compass in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_AK8972
+	tristate "AKM ak8972"
+	help
+	  This enables support for the AKM ak8972 compass
+	  This support is for integration with the MPU3050 or MPU6050 gyroscope
+	  device driver.  Only one compass can be registered at a time.
+	  Specifying more that one compass in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_MMC314X
+	tristate "MEMSIC mmc314x"
+	help
+	  This enables support for the MEMSIC mmc314x compass
+	  This support is for integration with the MPU3050 or MPU6050 gyroscope
+	  device driver.  Only one compass can be registered at a time.
+	  Specifying more that one compass in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_AMI30X
+	tristate "Aichi Steel ami30X"
+	help
+	  This enables support for the Aichi Steel ami304/ami305 compass
+	  This support is for integration with the MPU3050 or MPU6050 gyroscope
+	  device driver.  Only one compass can be registered at a time.
+	  Specifying more that one compass in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_AMI306
+	tristate "Aichi Steel ami306"
+	help
+	  This enables support for the Aichi Steel ami306 compass
+	  This support is for integration with the MPU3050 or MPU6050 gyroscope
+	  device driver.  Only one compass can be registered at a time.
+	  Specifying more that one compass in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_HMC5883
+	tristate "Honeywell hmc5883"
+	help
+	  This enables support for the Honeywell hmc5883 compass
+	  This support is for integration with the MPU3050 or MPU6050 gyroscope
+	  device driver.  Only one compass can be registered at a time.
+	  Specifying more that one compass in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_LSM303DLX_M
+	tristate "ST lsm303dlx"
+	help
+	  This enables support for the ST lsm303dlh and lsm303dlm compasses
+	  This support is for integration with the MPU3050 or MPU6050 gyroscope
+	  device driver.  Only one compass can be registered at a time.
+	  Specifying more that one compass in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_MMC314XMS
+	tristate "MEMSIC mmc314xMS"
+	help
+	  This enables support for the MEMSIC mmc314xMS compass
+	  This support is for integration with the MPU3050 or MPU6050 gyroscope
+	  device driver.  Only one compass can be registered at a time.
+	  Specifying more that one compass in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_YAS529
+	tristate "Yamaha yas529"
+	depends on INPUT_YAS_MAGNETOMETER
+	help
+	  This enables support for the Yamaha yas529 compass
+	  This support is for integration with the MPU3050 or MPU6050 gyroscope
+	  device driver.  Only one compass can be registered at a time.
+	  Specifying more that one compass in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_YAS530
+	tristate "Yamaha yas530"
+	help
+	  This enables support for the Yamaha yas530 compass
+	  This support is for integration with the MPU3050 or MPU6050 gyroscope
+	  device driver.  Only one compass can be registered at a time.
+	  Specifying more that one compass in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_HSCDTD002B
+	tristate "Alps hscdtd002b"
+	help
+	  This enables support for the Alps hscdtd002b compass
+	  This support is for integration with the MPU3050 or MPU6050 gyroscope
+	  device driver.  Only one compass can be registered at a time.
+	  Specifying more that one compass in the board file will result
+	  in runtime errors.
+
+config MPU_SENSORS_HSCDTD004A
+	tristate "Alps hscdtd004a"
+	help
+	  This enables support for the Alps hscdtd004a compass
+	  This support is for integration with the MPU3050 or MPU6050 gyroscope
+	  device driver.  Only one compass can be registered at a time.
+	  Specifying more that one compass in the board file will result
+	  in runtime errors.
+
+endif
diff --git a/drivers/misc/inv_mpu/compass/Makefile b/drivers/misc/inv_mpu/compass/Makefile
new file mode 100644
index 000000000000..d76ce1b06f31
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/Makefile
@@ -0,0 +1,41 @@
+#
+# Compass Slaves MPUxxxx
+#
+obj-$(CONFIG_MPU_SENSORS_AMI30X) += inv_mpu_ami30x.o
+inv_mpu_ami30x-objs +=	ami30x.o
+
+obj-$(CONFIG_MPU_SENSORS_AMI306) += inv_mpu_ami306.o
+inv_mpu_ami306-objs +=	ami306.o
+
+obj-$(CONFIG_MPU_SENSORS_HMC5883) += inv_mpu_hmc5883.o
+inv_mpu_hmc5883-objs +=	hmc5883.o
+
+obj-$(CONFIG_MPU_SENSORS_LSM303DLX_M) += inv_mpu_lsm303dlx_m.o
+inv_mpu_lsm303dlx_m-objs +=	lsm303dlx_m.o
+
+obj-$(CONFIG_MPU_SENSORS_MMC314X) += inv_mpu_mmc314x.o
+inv_mpu_mmc314x-objs +=	mmc314x.o
+
+obj-$(CONFIG_MPU_SENSORS_YAS529) += inv_mpu_yas529.o
+inv_mpu_yas529-objs +=	yas529-kernel.o
+
+obj-$(CONFIG_MPU_SENSORS_YAS530) += inv_mpu_yas530.o
+inv_mpu_yas530-objs +=	yas530.o
+
+obj-$(CONFIG_MPU_SENSORS_HSCDTD002B) += inv_mpu_hscdtd002b.o
+inv_mpu_hscdtd002b-objs +=	hscdtd002b.o
+
+obj-$(CONFIG_MPU_SENSORS_HSCDTD004A) += inv_mpu_hscdtd004a.o
+inv_mpu_hscdtd004a-objs +=	hscdtd004a.o
+
+obj-$(CONFIG_MPU_SENSORS_AK8963) += inv_mpu_ak89xx.o
+inv_mpu_ak89xx-objs +=  ak89xx.o
+
+obj-$(CONFIG_MPU_SENSORS_AK8975) += inv_mpu_ak8975.o
+inv_mpu_ak8975-objs +=	ak8975.o
+
+obj-$(CONFIG_MPU_SENSORS_AK8972) += inv_mpu_ak8972.o
+inv_mpu_ak8972-objs +=	ak8972.o
+
+EXTRA_CFLAGS += -Idrivers/misc/inv_mpu
+EXTRA_CFLAGS += -D__C99_DESIGNATED_INITIALIZER
diff --git a/drivers/misc/inv_mpu/compass/ak8972.c b/drivers/misc/inv_mpu/compass/ak8972.c
new file mode 100644
index 000000000000..7eb15b44039d
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/ak8972.c
@@ -0,0 +1,499 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup COMPASSDL
+ *
+ *  @{
+ *      @file   ak8972.c
+ *      @brief  Magnetometer setup and handling methods for the AKM AK8972 compass device.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define AK8972_REG_ST1  (0x02)
+#define AK8972_REG_HXL  (0x03)
+#define AK8972_REG_ST2  (0x09)
+
+#define AK8972_REG_CNTL (0x0A)
+#define AK8972_REG_ASAX (0x10)
+#define AK8972_REG_ASAY (0x11)
+#define AK8972_REG_ASAZ (0x12)
+
+#define AK8972_CNTL_MODE_POWER_DOWN         (0x00)
+#define AK8972_CNTL_MODE_SINGLE_MEASUREMENT (0x01)
+#define AK8972_CNTL_MODE_FUSE_ROM_ACCESS    (0x0f)
+
+/* -------------------------------------------------------------------------- */
+struct ak8972_config {
+	char asa[COMPASS_NUM_AXES];	/* axis sensitivity adjustment */
+};
+
+struct ak8972_private_data {
+	struct ak8972_config init;
+};
+
+/* -------------------------------------------------------------------------- */
+static int ak8972_init(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char serial_data[COMPASS_NUM_AXES];
+
+	struct ak8972_private_data *private_data;
+	private_data = (struct ak8972_private_data *)
+	    kzalloc(sizeof(struct ak8972_private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 AK8972_REG_CNTL,
+					 AK8972_CNTL_MODE_POWER_DOWN);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Wait at least 100us */
+	udelay(100);
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 AK8972_REG_CNTL,
+					 AK8972_CNTL_MODE_FUSE_ROM_ACCESS);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Wait at least 200us */
+	udelay(200);
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 AK8972_REG_ASAX,
+				 COMPASS_NUM_AXES, serial_data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	pdata->private_data = private_data;
+
+	private_data->init.asa[0] = serial_data[0];
+	private_data->init.asa[1] = serial_data[1];
+	private_data->init.asa[2] = serial_data[2];
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 AK8972_REG_CNTL,
+					 AK8972_CNTL_MODE_POWER_DOWN);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	udelay(100);
+	return INV_SUCCESS;
+}
+
+static int ak8972_exit(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	return INV_SUCCESS;
+}
+
+static int ak8972_suspend(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    AK8972_REG_CNTL,
+				    AK8972_CNTL_MODE_POWER_DOWN);
+	msleep(1);		/* wait at least 100us */
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+static int ak8972_resume(void *mlsl_handle,
+		  struct ext_slave_descr *slave,
+		  struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    AK8972_REG_CNTL,
+				    AK8972_CNTL_MODE_SINGLE_MEASUREMENT);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+static int ak8972_read(void *mlsl_handle,
+		struct ext_slave_descr *slave,
+		struct ext_slave_platform_data *pdata, unsigned char *data)
+{
+	unsigned char regs[8];
+	unsigned char *stat = &regs[0];
+	unsigned char *stat2 = &regs[7];
+	int result = INV_SUCCESS;
+	int status = INV_SUCCESS;
+
+	result =
+	    inv_serial_read(mlsl_handle, pdata->address, AK8972_REG_ST1,
+			    8, regs);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Always return the data and the status registers */
+	memcpy(data, &regs[1], 6);
+	data[6] = regs[0];
+	data[7] = regs[7];
+
+	/*
+	 * ST : data ready -
+	 * Measurement has been completed and data is ready to be read.
+	 */
+	if (*stat & 0x01)
+		status = INV_SUCCESS;
+
+	/*
+	 * ST2 : data error -
+	 * occurs when data read is started outside of a readable period;
+	 * data read would not be correct.
+	 * Valid in continuous measurement mode only.
+	 * In single measurement mode this error should not occour but we
+	 * stil account for it and return an error, since the data would be
+	 * corrupted.
+	 * DERR bit is self-clearing when ST2 register is read.
+	 */
+	if (*stat2 & 0x04)
+		status = INV_ERROR_COMPASS_DATA_ERROR;
+	/*
+	 * ST2 : overflow -
+	 * the sum of the absolute values of all axis |X|+|Y|+|Z| < 2400uT.
+	 * This is likely to happen in presence of an external magnetic
+	 * disturbance; it indicates, the sensor data is incorrect and should
+	 * be ignored.
+	 * An error is returned.
+	 * HOFL bit clears when a new measurement starts.
+	 */
+	if (*stat2 & 0x08)
+		status = INV_ERROR_COMPASS_DATA_OVERFLOW;
+	/*
+	 * ST : overrun -
+	 * the previous sample was not fetched and lost.
+	 * Valid in continuous measurement mode only.
+	 * In single measurement mode this error should not occour and we
+	 * don't consider this condition an error.
+	 * DOR bit is self-clearing when ST2 or any meas. data register is
+	 * read.
+	 */
+	if (*stat & 0x02) {
+		/* status = INV_ERROR_COMPASS_DATA_UNDERFLOW; */
+		status = INV_SUCCESS;
+	}
+
+	/*
+	 * trigger next measurement if:
+	 *    - stat is non zero;
+	 *    - if stat is zero and stat2 is non zero.
+	 * Won't trigger if data is not ready and there was no error.
+	 */
+	if (*stat != 0x00 || *stat2 != 0x00) {
+		result = inv_serial_single_write(
+		    mlsl_handle, pdata->address,
+		    AK8972_REG_CNTL, AK8972_CNTL_MODE_SINGLE_MEASUREMENT);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	return status;
+}
+
+static int ak8972_config(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata,
+			 struct ext_slave_config *data)
+{
+	int result;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_WRITE_REGISTERS:
+		result = inv_serial_write(mlsl_handle, pdata->address,
+					  data->len,
+					  (unsigned char *)data->data);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		break;
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+	case MPU_SLAVE_CONFIG_MOT_THS:
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static int ak8972_get_config(void *mlsl_handle,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata,
+			     struct ext_slave_config *data)
+{
+	struct ak8972_private_data *private_data = pdata->private_data;
+	int result;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_READ_REGISTERS:
+		{
+			unsigned char *serial_data =
+			    (unsigned char *)data->data;
+			result =
+			    inv_serial_read(mlsl_handle, pdata->address,
+					    serial_data[0], data->len - 1,
+					    &serial_data[1]);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			break;
+		}
+	case MPU_SLAVE_READ_SCALE:
+		{
+			unsigned char *serial_data =
+			    (unsigned char *)data->data;
+			serial_data[0] = private_data->init.asa[0];
+			serial_data[1] = private_data->init.asa[1];
+			serial_data[2] = private_data->init.asa[2];
+			result = INV_SUCCESS;
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			break;
+		}
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) = 0;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) = 8000;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+	case MPU_SLAVE_CONFIG_MOT_THS:
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static struct ext_slave_read_trigger ak8972_read_trigger = {
+	/*.reg              = */ 0x0A,
+	/*.value            = */ 0x01
+};
+
+static struct ext_slave_descr ak8972_descr = {
+	.init             = ak8972_init,
+	.exit             = ak8972_exit,
+	.suspend          = ak8972_suspend,
+	.resume           = ak8972_resume,
+	.read             = ak8972_read,
+	.config           = ak8972_config,
+	.get_config       = ak8972_get_config,
+	.name             = "ak8972",
+	.type             = EXT_SLAVE_TYPE_COMPASS,
+	.id               = COMPASS_ID_AK8972,
+	.read_reg         = 0x01,
+	.read_len         = 9,
+	.endian           = EXT_SLAVE_LITTLE_ENDIAN,
+	.range            = {39321, 6000},
+	.trigger          = &ak8972_read_trigger,
+};
+
+static
+struct ext_slave_descr *ak8972_get_slave_descr(void)
+{
+	return &ak8972_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct ak8972_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int ak8972_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct ak8972_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					ak8972_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int ak8972_mod_remove(struct i2c_client *client)
+{
+	struct ak8972_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				ak8972_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id ak8972_mod_id[] = {
+	{ "ak8972", COMPASS_ID_AK8972 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, ak8972_mod_id);
+
+static struct i2c_driver ak8972_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = ak8972_mod_probe,
+	.remove = ak8972_mod_remove,
+	.id_table = ak8972_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "ak8972_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init ak8972_mod_init(void)
+{
+	int res = i2c_add_driver(&ak8972_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "ak8972_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit ak8972_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&ak8972_mod_driver);
+}
+
+module_init(ak8972_mod_init);
+module_exit(ak8972_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate AK8972 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("ak8972_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/ak8975.c b/drivers/misc/inv_mpu/compass/ak8975.c
new file mode 100644
index 000000000000..3642e29e89a7
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/ak8975.c
@@ -0,0 +1,500 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup COMPASSDL
+ *
+ *  @{
+ *      @file   ak8975.c
+ *      @brief  Magnetometer setup and handling methods for the AKM AK8975,
+ *              AKM AK8975B, and AKM AK8975C compass devices.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define AK8975_REG_ST1  (0x02)
+#define AK8975_REG_HXL  (0x03)
+#define AK8975_REG_ST2  (0x09)
+
+#define AK8975_REG_CNTL (0x0A)
+#define AK8975_REG_ASAX (0x10)
+#define AK8975_REG_ASAY (0x11)
+#define AK8975_REG_ASAZ (0x12)
+
+#define AK8975_CNTL_MODE_POWER_DOWN         (0x00)
+#define AK8975_CNTL_MODE_SINGLE_MEASUREMENT (0x01)
+#define AK8975_CNTL_MODE_FUSE_ROM_ACCESS    (0x0f)
+
+/* -------------------------------------------------------------------------- */
+struct ak8975_config {
+	char asa[COMPASS_NUM_AXES];	/* axis sensitivity adjustment */
+};
+
+struct ak8975_private_data {
+	struct ak8975_config init;
+};
+
+/* -------------------------------------------------------------------------- */
+static int ak8975_init(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char serial_data[COMPASS_NUM_AXES];
+
+	struct ak8975_private_data *private_data;
+	private_data = (struct ak8975_private_data *)
+	    kzalloc(sizeof(struct ak8975_private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 AK8975_REG_CNTL,
+					 AK8975_CNTL_MODE_POWER_DOWN);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Wait at least 100us */
+	udelay(100);
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 AK8975_REG_CNTL,
+					 AK8975_CNTL_MODE_FUSE_ROM_ACCESS);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Wait at least 200us */
+	udelay(200);
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 AK8975_REG_ASAX,
+				 COMPASS_NUM_AXES, serial_data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	pdata->private_data = private_data;
+
+	private_data->init.asa[0] = serial_data[0];
+	private_data->init.asa[1] = serial_data[1];
+	private_data->init.asa[2] = serial_data[2];
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 AK8975_REG_CNTL,
+					 AK8975_CNTL_MODE_POWER_DOWN);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	udelay(100);
+	return INV_SUCCESS;
+}
+
+static int ak8975_exit(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	return INV_SUCCESS;
+}
+
+static int ak8975_suspend(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    AK8975_REG_CNTL,
+				    AK8975_CNTL_MODE_POWER_DOWN);
+	msleep(1);		/* wait at least 100us */
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+static int ak8975_resume(void *mlsl_handle,
+		  struct ext_slave_descr *slave,
+		  struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    AK8975_REG_CNTL,
+				    AK8975_CNTL_MODE_SINGLE_MEASUREMENT);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+static int ak8975_read(void *mlsl_handle,
+		struct ext_slave_descr *slave,
+		struct ext_slave_platform_data *pdata, unsigned char *data)
+{
+	unsigned char regs[8];
+	unsigned char *stat = &regs[0];
+	unsigned char *stat2 = &regs[7];
+	int result = INV_SUCCESS;
+	int status = INV_SUCCESS;
+
+	result =
+	    inv_serial_read(mlsl_handle, pdata->address, AK8975_REG_ST1,
+			    8, regs);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Always return the data and the status registers */
+	memcpy(data, &regs[1], 6);
+	data[6] = regs[0];
+	data[7] = regs[7];
+
+	/*
+	 * ST : data ready -
+	 * Measurement has been completed and data is ready to be read.
+	 */
+	if (*stat & 0x01)
+		status = INV_SUCCESS;
+
+	/*
+	 * ST2 : data error -
+	 * occurs when data read is started outside of a readable period;
+	 * data read would not be correct.
+	 * Valid in continuous measurement mode only.
+	 * In single measurement mode this error should not occour but we
+	 * stil account for it and return an error, since the data would be
+	 * corrupted.
+	 * DERR bit is self-clearing when ST2 register is read.
+	 */
+	if (*stat2 & 0x04)
+		status = INV_ERROR_COMPASS_DATA_ERROR;
+	/*
+	 * ST2 : overflow -
+	 * the sum of the absolute values of all axis |X|+|Y|+|Z| < 2400uT.
+	 * This is likely to happen in presence of an external magnetic
+	 * disturbance; it indicates, the sensor data is incorrect and should
+	 * be ignored.
+	 * An error is returned.
+	 * HOFL bit clears when a new measurement starts.
+	 */
+	if (*stat2 & 0x08)
+		status = INV_ERROR_COMPASS_DATA_OVERFLOW;
+	/*
+	 * ST : overrun -
+	 * the previous sample was not fetched and lost.
+	 * Valid in continuous measurement mode only.
+	 * In single measurement mode this error should not occour and we
+	 * don't consider this condition an error.
+	 * DOR bit is self-clearing when ST2 or any meas. data register is
+	 * read.
+	 */
+	if (*stat & 0x02) {
+		/* status = INV_ERROR_COMPASS_DATA_UNDERFLOW; */
+		status = INV_SUCCESS;
+	}
+
+	/*
+	 * trigger next measurement if:
+	 *    - stat is non zero;
+	 *    - if stat is zero and stat2 is non zero.
+	 * Won't trigger if data is not ready and there was no error.
+	 */
+	if (*stat != 0x00 || *stat2 != 0x00) {
+		result = inv_serial_single_write(
+		    mlsl_handle, pdata->address,
+		    AK8975_REG_CNTL, AK8975_CNTL_MODE_SINGLE_MEASUREMENT);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	return status;
+}
+
+static int ak8975_config(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata,
+			 struct ext_slave_config *data)
+{
+	int result;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_WRITE_REGISTERS:
+		result = inv_serial_write(mlsl_handle, pdata->address,
+					  data->len,
+					  (unsigned char *)data->data);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		break;
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+	case MPU_SLAVE_CONFIG_MOT_THS:
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static int ak8975_get_config(void *mlsl_handle,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata,
+			     struct ext_slave_config *data)
+{
+	struct ak8975_private_data *private_data = pdata->private_data;
+	int result;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_READ_REGISTERS:
+		{
+			unsigned char *serial_data =
+			    (unsigned char *)data->data;
+			result =
+			    inv_serial_read(mlsl_handle, pdata->address,
+					    serial_data[0], data->len - 1,
+					    &serial_data[1]);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			break;
+		}
+	case MPU_SLAVE_READ_SCALE:
+		{
+			unsigned char *serial_data =
+			    (unsigned char *)data->data;
+			serial_data[0] = private_data->init.asa[0];
+			serial_data[1] = private_data->init.asa[1];
+			serial_data[2] = private_data->init.asa[2];
+			result = INV_SUCCESS;
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			break;
+		}
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) = 0;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) = 8000;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+	case MPU_SLAVE_CONFIG_MOT_THS:
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static struct ext_slave_read_trigger ak8975_read_trigger = {
+	/*.reg              = */ 0x0A,
+	/*.value            = */ 0x01
+};
+
+static struct ext_slave_descr ak8975_descr = {
+	.init             = ak8975_init,
+	.exit             = ak8975_exit,
+	.suspend          = ak8975_suspend,
+	.resume           = ak8975_resume,
+	.read             = ak8975_read,
+	.config           = ak8975_config,
+	.get_config       = ak8975_get_config,
+	.name             = "ak8975",
+	.type             = EXT_SLAVE_TYPE_COMPASS,
+	.id               = COMPASS_ID_AK8975,
+	.read_reg         = 0x01,
+	.read_len         = 10,
+	.endian           = EXT_SLAVE_LITTLE_ENDIAN,
+	.range            = {9830, 4000},
+	.trigger          = &ak8975_read_trigger,
+};
+
+static
+struct ext_slave_descr *ak8975_get_slave_descr(void)
+{
+	return &ak8975_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct ak8975_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int ak8975_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct ak8975_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					ak8975_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int ak8975_mod_remove(struct i2c_client *client)
+{
+	struct ak8975_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				ak8975_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id ak8975_mod_id[] = {
+	{ "ak8975", COMPASS_ID_AK8975 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, ak8975_mod_id);
+
+static struct i2c_driver ak8975_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = ak8975_mod_probe,
+	.remove = ak8975_mod_remove,
+	.id_table = ak8975_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "ak8975_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init ak8975_mod_init(void)
+{
+	int res = i2c_add_driver(&ak8975_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "ak8975_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit ak8975_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&ak8975_mod_driver);
+}
+
+module_init(ak8975_mod_init);
+module_exit(ak8975_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate AK8975 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("ak8975_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/ak89xx.c b/drivers/misc/inv_mpu/compass/ak89xx.c
new file mode 100644
index 000000000000..d15b0b8dbe68
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/ak89xx.c
@@ -0,0 +1,522 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup COMPASSDL
+ *
+ *  @{
+ *      @file   ak89xx.c
+ *      @brief  Magnetometer setup and handling methods for the AKM
+ *              compass devices.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define AK89XX_REG_ST1     (0x02)
+#define AK89XX_REG_HXL     (0x03)
+#define AK89XX_REG_ST2     (0x09)
+
+#define AK89XX_REG_CNTL    (0x0A)
+#define AK89XX_REG_ASAX    (0x10)
+#define AK89XX_REG_ASAY    (0x11)
+#define AK89XX_REG_ASAZ    (0x12)
+
+#define AK89XX_CNTL_MODE_POWER_DOWN	    (0x00)
+#define AK89XX_CNTL_MODE_SINGLE_MEASUREMENT (0x01)
+#define AK89XX_CNTL_MODE_FUSE_ROM_ACCESS    (0x0f)
+
+/* -------------------------------------------------------------------------- */
+struct ak89xx_config {
+	char asa[COMPASS_NUM_AXES];	/* axis sensitivity adjustment */
+};
+
+struct ak89xx_private_data {
+	struct ak89xx_config init;
+};
+
+/* -------------------------------------------------------------------------- */
+static int ak89xx_init(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char serial_data[COMPASS_NUM_AXES];
+
+	struct ak89xx_private_data *private_data;
+	private_data = (struct ak89xx_private_data *)
+	    kzalloc(sizeof(struct ak89xx_private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 AK89XX_REG_CNTL,
+					 AK89XX_CNTL_MODE_POWER_DOWN);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Wait at least 100us */
+	udelay(100);
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 AK89XX_REG_CNTL,
+					 AK89XX_CNTL_MODE_FUSE_ROM_ACCESS);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Wait at least 200us */
+	udelay(200);
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 AK89XX_REG_ASAX,
+				 COMPASS_NUM_AXES, serial_data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	pdata->private_data = private_data;
+
+	private_data->init.asa[0] = serial_data[0];
+	private_data->init.asa[1] = serial_data[1];
+	private_data->init.asa[2] = serial_data[2];
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 AK89XX_REG_CNTL,
+					 AK89XX_CNTL_MODE_POWER_DOWN);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	udelay(100);
+	return INV_SUCCESS;
+}
+
+static int ak89xx_exit(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	return INV_SUCCESS;
+}
+
+static int ak89xx_suspend(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    AK89XX_REG_CNTL,
+				    AK89XX_CNTL_MODE_POWER_DOWN);
+	msleep(1);		/* wait at least 100us */
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+static int ak89xx_resume(void *mlsl_handle,
+		  struct ext_slave_descr *slave,
+		  struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    AK89XX_REG_CNTL,
+				    AK89XX_CNTL_MODE_SINGLE_MEASUREMENT);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+static int ak89xx_read(void *mlsl_handle,
+		struct ext_slave_descr *slave,
+		struct ext_slave_platform_data *pdata, unsigned char *data)
+{
+	unsigned char regs[8];
+	unsigned char *stat = &regs[0];
+	unsigned char *stat2 = &regs[7];
+	int result = INV_SUCCESS;
+	int status = INV_SUCCESS;
+
+	result =
+	    inv_serial_read(mlsl_handle, pdata->address, AK89XX_REG_ST1,
+			    8, regs);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Always return the data and the status registers */
+	memcpy(data, &regs[1], 6);
+	data[6] = regs[0];
+	data[7] = regs[7];
+
+	/*
+	 * ST : data ready -
+	 * Measurement has been completed and data is ready to be read.
+	 */
+	if (*stat & 0x01)
+		status = INV_SUCCESS;
+
+	/*
+	 * ST2 : data error -
+	 * occurs when data read is started outside of a readable period;
+	 * data read would not be correct.
+	 * Valid in continuous measurement mode only.
+	 * In single measurement mode this error should not occour but we
+	 * stil account for it and return an error, since the data would be
+	 * corrupted.
+	 * DERR bit is self-clearing when ST2 register is read.
+	 *
+	 * This bit is always zero on the AK8963.
+	 */
+	if (*stat2 & 0x04)
+		status = INV_ERROR_COMPASS_DATA_ERROR;
+	/*
+	 * ST2 : overflow -
+	 * the sum of the absolute values of all axis |X|+|Y|+|Z| < 2400uT.
+	 * This is likely to happen in presence of an external magnetic
+	 * disturbance; it indicates, the sensor data is incorrect and should
+	 * be ignored.
+	 * An error is returned.
+	 * HOFL bit clears when a new measurement starts.
+	 */
+	if (*stat2 & 0x08)
+		status = INV_ERROR_COMPASS_DATA_OVERFLOW;
+	/*
+	 * ST : overrun -
+	 * the previous sample was not fetched and lost.
+	 * Valid in continuous measurement mode only.
+	 * In single measurement mode this error should not occour and we
+	 * don't consider this condition an error.
+	 * DOR bit is self-clearing when ST2 or any meas. data register is
+	 * read.
+	 */
+	if (*stat & 0x02) {
+		/* status = INV_ERROR_COMPASS_DATA_UNDERFLOW; */
+		status = INV_SUCCESS;
+	}
+
+	/*
+	 * trigger next measurement if:
+	 *    - stat is non zero;
+	 *    - if stat is zero and stat2 is non zero.
+	 * Won't trigger if data is not ready and there was no error.
+	 */
+	if (*stat != 0x00 || *stat2 != 0x00) {
+		result = inv_serial_single_write(
+		    mlsl_handle, pdata->address,
+		    AK89XX_REG_CNTL, AK89XX_CNTL_MODE_SINGLE_MEASUREMENT);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	return status;
+}
+
+static int ak89xx_config(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata,
+			 struct ext_slave_config *data)
+{
+	int result;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_WRITE_REGISTERS:
+		result = inv_serial_write(mlsl_handle, pdata->address,
+					  data->len,
+					  (unsigned char *)data->data);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		break;
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+	case MPU_SLAVE_CONFIG_MOT_THS:
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static int ak89xx_get_config(void *mlsl_handle,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata,
+			     struct ext_slave_config *data)
+{
+	struct ak89xx_private_data *private_data = pdata->private_data;
+	int result;
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_READ_REGISTERS:
+		{
+			unsigned char *serial_data =
+			    (unsigned char *)data->data;
+			result =
+			    inv_serial_read(mlsl_handle, pdata->address,
+					    serial_data[0], data->len - 1,
+					    &serial_data[1]);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			break;
+		}
+	case MPU_SLAVE_READ_SCALE:
+		{
+			unsigned char *serial_data =
+			    (unsigned char *)data->data;
+			serial_data[0] = private_data->init.asa[0];
+			serial_data[1] = private_data->init.asa[1];
+			serial_data[2] = private_data->init.asa[2];
+			result = INV_SUCCESS;
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			break;
+		}
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) = 0;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) = 8000;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+	case MPU_SLAVE_CONFIG_MOT_THS:
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+	default:
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static struct ext_slave_read_trigger ak89xx_read_trigger = {
+	/*.reg              = */ 0x0A,
+	/*.value            = */ 0x01
+};
+
+static struct ext_slave_descr ak89xx_descr = {
+	.init             = ak89xx_init,
+	.exit             = ak89xx_exit,
+	.suspend          = ak89xx_suspend,
+	.resume           = ak89xx_resume,
+	.read             = ak89xx_read,
+	.config           = ak89xx_config,
+	.get_config       = ak89xx_get_config,
+	.name             = "ak89xx",
+	.type             = EXT_SLAVE_TYPE_COMPASS,
+	.id               = COMPASS_ID_AK8963,
+	.read_reg         = 0x01,
+	.read_len         = 10,
+	.endian           = EXT_SLAVE_LITTLE_ENDIAN,
+	.range            = {9830, 4000},
+	.trigger          = &ak89xx_read_trigger,
+};
+
+static
+struct ext_slave_descr *ak89xx_get_slave_descr(void)
+{
+	return &ak89xx_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct ak89xx_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int ak89xx_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct ak89xx_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	/* Override default values based on model. */
+	ak89xx_descr.id = devid->driver_data;
+	/* ak89xx_descr.name = devid->name; */
+	switch (ak89xx_descr.id) {
+	case COMPASS_ID_AK8963:
+		break;
+	case COMPASS_ID_AK8972:
+		ak89xx_descr.read_len = 9;
+		ak89xx_descr.range.mantissa = 39321;
+		ak89xx_descr.range.fraction = 6000;
+		break;
+	case COMPASS_ID_AK8975:
+		break;
+	default:
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					ak89xx_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int ak89xx_mod_remove(struct i2c_client *client)
+{
+	struct ak89xx_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				ak89xx_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id ak89xx_mod_id[] = {
+	{ "ak8963", COMPASS_ID_AK8963 },
+	{ "ak8972", COMPASS_ID_AK8972 },
+	{ "ak8975", COMPASS_ID_AK8975 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, ak89xx_mod_id);
+
+static struct i2c_driver ak89xx_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = ak89xx_mod_probe,
+	.remove = ak89xx_mod_remove,
+	.id_table = ak89xx_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "ak89xx_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init ak89xx_mod_init(void)
+{
+	int res = i2c_add_driver(&ak89xx_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "ak89xx_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit ak89xx_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&ak89xx_mod_driver);
+}
+
+module_init(ak89xx_mod_init);
+module_exit(ak89xx_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate AKM AK89XX sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("ak89xx_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/ami306.c b/drivers/misc/inv_mpu/compass/ami306.c
new file mode 100644
index 000000000000..f645457d1612
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/ami306.c
@@ -0,0 +1,1020 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup COMPASSDL
+ *
+ *  @{
+ *      @file   ami306.c
+ *      @brief  Magnetometer setup and handling methods for Aichi AMI306
+ *              compass.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include "ami_hw.h"
+#include "ami_sensor_def.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define AMI306_REG_DATAX		(0x10)
+#define AMI306_REG_STAT1		(0x18)
+#define AMI306_REG_CNTL1		(0x1B)
+#define AMI306_REG_CNTL2		(0x1C)
+#define AMI306_REG_CNTL3		(0x1D)
+#define AMI306_REG_CNTL4_1		(0x5C)
+#define AMI306_REG_CNTL4_2		(0x5D)
+
+#define AMI306_BIT_CNTL1_PC1		(0x80)
+#define AMI306_BIT_CNTL1_ODR1		(0x10)
+#define AMI306_BIT_CNTL1_FS1		(0x02)
+
+#define AMI306_BIT_CNTL2_IEN		(0x10)
+#define AMI306_BIT_CNTL2_DREN		(0x08)
+#define AMI306_BIT_CNTL2_DRP		(0x04)
+#define AMI306_BIT_CNTL3_F0RCE		(0x40)
+
+#define AMI_FINE_MAX			(96)
+#define AMI_STANDARD_OFFSET		(0x800)
+#define AMI_GAIN_COR_DEFAULT		(1000)
+
+/* -------------------------------------------------------------------------- */
+struct ami306_private_data {
+	int isstandby;
+	unsigned char fine[3];
+	struct ami_sensor_parametor param;
+	struct ami_win_parameter win;
+};
+
+/* -------------------------------------------------------------------------- */
+static inline unsigned short little_u8_to_u16(unsigned char *p_u8)
+{
+	return p_u8[0] | (p_u8[1] << 8);
+}
+
+static int ami306_set_bits8(void *mlsl_handle,
+			    struct ext_slave_platform_data *pdata,
+			    unsigned char reg, unsigned char bits)
+{
+	int result;
+	unsigned char buf;
+
+	result = inv_serial_read(mlsl_handle, pdata->address, reg, 1, &buf);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	buf |= bits;
+	result = inv_serial_single_write(mlsl_handle, pdata->address, reg, buf);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+static int ami306_wait_data_ready(void *mlsl_handle,
+				  struct ext_slave_platform_data *pdata,
+				  unsigned long usecs, unsigned long times)
+{
+	int result = 0;
+	unsigned char buf;
+
+	for (; 0 < times; --times) {
+		udelay(usecs);
+		result = inv_serial_read(mlsl_handle, pdata->address,
+					 AMI_REG_STA1, 1, &buf);
+		if (buf & AMI_STA1_DRDY_BIT)
+			return 0;
+		else if (buf & AMI_STA1_DOR_BIT)
+			return INV_ERROR_COMPASS_DATA_OVERFLOW;
+	}
+	return INV_ERROR_COMPASS_DATA_NOT_READY;
+}
+
+static int ami306_read_raw_data(void *mlsl_handle,
+				struct ext_slave_platform_data *pdata,
+				short dat[3])
+{
+	int result;
+	unsigned char buf[6];
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 AMI_REG_DATAX, sizeof(buf), buf);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dat[0] = little_u8_to_u16(&buf[0]);
+	dat[1] = little_u8_to_u16(&buf[2]);
+	dat[2] = little_u8_to_u16(&buf[4]);
+	return result;
+}
+
+#define AMI_WAIT_DATAREADY_RETRY		3	/* retry times */
+#define AMI_DRDYWAIT				800	/* u(micro) sec */
+static int ami306_force_mesurement(void *mlsl_handle,
+				   struct ext_slave_platform_data *pdata,
+				   short ver[3])
+{
+	int result;
+	int status;
+	result = ami306_set_bits8(mlsl_handle, pdata,
+				  AMI_REG_CTRL3, AMI_CTRL3_FORCE_BIT);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = ami306_wait_data_ready(mlsl_handle, pdata,
+					AMI_DRDYWAIT, AMI_WAIT_DATAREADY_RETRY);
+	if (result && result != INV_ERROR_COMPASS_DATA_OVERFLOW) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/*  READ DATA X,Y,Z */
+	status = ami306_read_raw_data(mlsl_handle, pdata, ver);
+	if (status) {
+		LOG_RESULT_LOCATION(status);
+		return status;
+	}
+
+	return result;
+}
+
+static int ami306_mea(void *mlsl_handle,
+		      struct ext_slave_platform_data *pdata, short val[3])
+{
+	int result = ami306_force_mesurement(mlsl_handle, pdata, val);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	val[0] += AMI_STANDARD_OFFSET;
+	val[1] += AMI_STANDARD_OFFSET;
+	val[2] += AMI_STANDARD_OFFSET;
+	return result;
+}
+
+static int ami306_write_offset(void *mlsl_handle,
+			       struct ext_slave_platform_data *pdata,
+			       unsigned char *fine)
+{
+	int result = 0;
+	unsigned char dat[3];
+	dat[0] = AMI_REG_OFFX;
+	dat[1] = 0x7f & fine[0];
+	dat[2] = 0;
+	result = inv_serial_write(mlsl_handle, pdata->address,
+				  sizeof(dat), dat);
+	dat[0] = AMI_REG_OFFY;
+	dat[1] = 0x7f & fine[1];
+	dat[2] = 0;
+	result = inv_serial_write(mlsl_handle, pdata->address,
+				  sizeof(dat), dat);
+	dat[0] = AMI_REG_OFFZ;
+	dat[1] = 0x7f & fine[2];
+	dat[2] = 0;
+	result = inv_serial_write(mlsl_handle, pdata->address,
+				  sizeof(dat), dat);
+	return result;
+}
+
+static int ami306_start_sensor(void *mlsl_handle,
+			       struct ext_slave_platform_data *pdata)
+{
+	int result = 0;
+	unsigned char buf[3];
+	struct ami306_private_data *private_data = pdata->private_data;
+
+	/* Step 1 */
+	result = ami306_set_bits8(mlsl_handle, pdata,
+				  AMI_REG_CTRL1,
+				  AMI_CTRL1_PC1 | AMI_CTRL1_FS1_FORCE);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Step 2 */
+	result = ami306_set_bits8(mlsl_handle, pdata,
+				  AMI_REG_CTRL2, AMI_CTRL2_DREN);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Step 3 */
+	buf[0] = AMI_REG_CTRL4;
+	buf[1] = AMI_CTRL4_HS & 0xFF;
+	buf[2] = (AMI_CTRL4_HS >> 8) & 0xFF;
+	result = inv_serial_write(mlsl_handle, pdata->address,
+				  sizeof(buf), buf);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Step 4 */
+	result = ami306_write_offset(mlsl_handle, pdata, private_data->fine);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+/**
+ * This function does this.
+ *
+ * @param mlsl_handle this param is this.
+ * @param slave
+ * @param pdata
+ *
+ * @return INV_SUCCESS or non-zero error code.
+ */
+static int ami306_read_param(void *mlsl_handle,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata)
+{
+	int result = 0;
+	unsigned char regs[12];
+	struct ami306_private_data *private_data = pdata->private_data;
+	struct ami_sensor_parametor *param = &private_data->param;
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 AMI_REG_SENX, sizeof(regs), regs);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Little endian 16 bit registers */
+	param->m_gain.x = little_u8_to_u16(&regs[0]);
+	param->m_gain.y = little_u8_to_u16(&regs[2]);
+	param->m_gain.z = little_u8_to_u16(&regs[4]);
+
+	param->m_interference.xy = regs[7];
+	param->m_interference.xz = regs[6];
+	param->m_interference.yx = regs[9];
+	param->m_interference.yz = regs[8];
+	param->m_interference.zx = regs[11];
+	param->m_interference.zy = regs[10];
+
+	param->m_offset.x = AMI_STANDARD_OFFSET;
+	param->m_offset.y = AMI_STANDARD_OFFSET;
+	param->m_offset.z = AMI_STANDARD_OFFSET;
+
+	param->m_gain_cor.x = AMI_GAIN_COR_DEFAULT;
+	param->m_gain_cor.y = AMI_GAIN_COR_DEFAULT;
+	param->m_gain_cor.z = AMI_GAIN_COR_DEFAULT;
+
+	return result;
+}
+
+static int ami306_initial_b0_adjust(void *mlsl_handle,
+				    struct ext_slave_descr *slave,
+				    struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char fine[3] = { 0 };
+	short data[3];
+	int diff[3] = { 0x7fff, 0x7fff, 0x7fff };
+	int fn = 0;
+	int ax = 0;
+	unsigned char buf[3];
+	struct ami306_private_data *private_data = pdata->private_data;
+
+	result = ami306_set_bits8(mlsl_handle, pdata,
+				  AMI_REG_CTRL2, AMI_CTRL2_DREN);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	buf[0] = AMI_REG_CTRL4;
+	buf[1] = AMI_CTRL4_HS & 0xFF;
+	buf[2] = (AMI_CTRL4_HS >> 8) & 0xFF;
+	result = inv_serial_write(mlsl_handle, pdata->address,
+				  sizeof(buf), buf);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	for (fn = 0; fn < AMI_FINE_MAX; ++fn) {	/* fine 0 -> 95 */
+		fine[0] = fine[1] = fine[2] = fn;
+		result = ami306_write_offset(mlsl_handle, pdata, fine);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+		result = ami306_force_mesurement(mlsl_handle, pdata, data);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		MPL_LOGV("[%d] x:%-5d y:%-5d z:%-5d\n",
+			 fn, data[0], data[1], data[2]);
+
+		for (ax = 0; ax < 3; ax++) {
+			/* search point most close to zero. */
+			if (diff[ax] > abs(data[ax])) {
+				private_data->fine[ax] = fn;
+				diff[ax] = abs(data[ax]);
+			}
+		}
+	}
+	MPL_LOGV("fine x:%-5d y:%-5d z:%-5d\n",
+		 private_data->fine[0], private_data->fine[1],
+		 private_data->fine[2]);
+
+	result = ami306_write_offset(mlsl_handle, pdata, private_data->fine);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Software Reset */
+	result = ami306_set_bits8(mlsl_handle, pdata,
+				  AMI_REG_CTRL3, AMI_CTRL3_SRST_BIT);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+}
+
+#define SEH_RANGE_MIN 100
+#define SEH_RANGE_MAX 3950
+static int ami306_search_offset(void *mlsl_handle,
+				struct ext_slave_descr *slave,
+				struct ext_slave_platform_data *pdata)
+{
+	int result;
+	int axis;
+	unsigned char regs[6];
+	unsigned char run_flg[3] = { 1, 1, 1 };
+	unsigned char fine[3];
+	unsigned char win_range_fine[3];
+	unsigned short fine_output[3];
+	short val[3];
+	unsigned short cnt[3] = { 0 };
+	struct ami306_private_data *private_data = pdata->private_data;
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 AMI_FINEOUTPUT_X, sizeof(regs), regs);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	fine_output[0] = little_u8_to_u16(&regs[0]);
+	fine_output[1] = little_u8_to_u16(&regs[2]);
+	fine_output[2] = little_u8_to_u16(&regs[4]);
+
+	for (axis = 0; axis < 3; ++axis) {
+		if (fine_output[axis] == 0) {
+			MPL_LOGV("error fine_output %d  axis:%d\n",
+				 __LINE__, axis);
+			return -1;
+		}
+		/*  fines per a window */
+		win_range_fine[axis] = (SEH_RANGE_MAX - SEH_RANGE_MIN)
+		    / fine_output[axis];
+	}
+
+	/* get current fine */
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 AMI_REG_OFFX, 2, &regs[0]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 AMI_REG_OFFY, 2, &regs[2]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 AMI_REG_OFFZ, 2, &regs[4]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	fine[0] = (unsigned char)(regs[0] & 0x7f);
+	fine[1] = (unsigned char)(regs[2] & 0x7f);
+	fine[2] = (unsigned char)(regs[4] & 0x7f);
+
+	while (run_flg[0] == 1 || run_flg[1] == 1 || run_flg[2] == 1) {
+
+		result = ami306_mea(mlsl_handle, pdata, val);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		MPL_LOGV("val  x:%-5d y:%-5d z:%-5d\n", val[0], val[1], val[2]);
+		MPL_LOGV("now fine x:%-5d y:%-5d z:%-5d\n",
+			 fine[0], fine[1], fine[2]);
+
+		for (axis = 0; axis < 3; ++axis) {
+			if (axis == 0) {	/* X-axis is reversed */
+				val[axis] = 0x0FFF & ~val[axis];
+			}
+			if (val[axis] < SEH_RANGE_MIN) {
+				/* At the case of less low limmit. */
+				fine[axis] -= win_range_fine[axis];
+				MPL_LOGV("min : fine=%d diff=%d\n",
+					 fine[axis], win_range_fine[axis]);
+			}
+			if (val[axis] > SEH_RANGE_MAX) {
+				/* At the case of over high limmit. */
+				fine[axis] += win_range_fine[axis];
+				MPL_LOGV("max : fine=%d diff=%d\n",
+					 fine[axis], win_range_fine[axis]);
+			}
+			if (SEH_RANGE_MIN <= val[axis] &&
+			    val[axis] <= SEH_RANGE_MAX) {
+				/* In the current window. */
+				int diff_fine =
+				    (val[axis] - AMI_STANDARD_OFFSET) /
+				    fine_output[axis];
+				fine[axis] += diff_fine;
+				run_flg[axis] = 0;
+				MPL_LOGV("mid : fine=%d diff=%d\n",
+					 fine[axis], diff_fine);
+			}
+
+			if (!(0 <= fine[axis] && fine[axis] < AMI_FINE_MAX)) {
+				MPL_LOGE("fine err :%d\n", cnt[axis]);
+				goto out;
+			}
+			if (cnt[axis] > 3) {
+				MPL_LOGE("cnt err :%d\n", cnt[axis]);
+				goto out;
+			}
+			cnt[axis]++;
+		}
+		MPL_LOGV("new fine x:%-5d y:%-5d z:%-5d\n",
+			 fine[0], fine[1], fine[2]);
+
+		/* set current fine */
+		result = ami306_write_offset(mlsl_handle, pdata, fine);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	memcpy(private_data->fine, fine, sizeof(fine));
+out:
+	result = ami306_set_bits8(mlsl_handle, pdata,
+				  AMI_REG_CTRL3, AMI_CTRL3_SRST_BIT);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	udelay(250 + 50);
+	return 0;
+}
+
+static int ami306_read_win(void *mlsl_handle,
+			   struct ext_slave_descr *slave,
+			   struct ext_slave_platform_data *pdata)
+{
+	int result = 0;
+	unsigned char regs[6];
+	struct ami306_private_data *private_data = pdata->private_data;
+	struct ami_win_parameter *win = &private_data->win;
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 AMI_REG_OFFOTPX, sizeof(regs), regs);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	win->m_0Gauss_fine.x = (unsigned char)(regs[0] & 0x7f);
+	win->m_0Gauss_fine.y = (unsigned char)(regs[2] & 0x7f);
+	win->m_0Gauss_fine.z = (unsigned char)(regs[4] & 0x7f);
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 AMI_REG_OFFX, 2, &regs[0]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 AMI_REG_OFFY, 2, &regs[2]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 AMI_REG_OFFZ, 2, &regs[4]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	win->m_fine.x = (unsigned char)(regs[0] & 0x7f);
+	win->m_fine.y = (unsigned char)(regs[2] & 0x7f);
+	win->m_fine.z = (unsigned char)(regs[4] & 0x7f);
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 AMI_FINEOUTPUT_X, sizeof(regs), regs);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	win->m_fine_output.x = little_u8_to_u16(&regs[0]);
+	win->m_fine_output.y = little_u8_to_u16(&regs[2]);
+	win->m_fine_output.z = little_u8_to_u16(&regs[4]);
+
+	return result;
+}
+
+static int ami306_suspend(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char reg;
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 AMI306_REG_CNTL1, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	reg &= ~(AMI306_BIT_CNTL1_PC1 | AMI306_BIT_CNTL1_FS1);
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 AMI306_REG_CNTL1, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+static int ami306_resume(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	unsigned char regs[] = {
+		AMI306_REG_CNTL4_1,
+		0x7E,
+		0xA0
+	};
+	/* Step1. Set CNTL1 reg to power model active (Write CNTL1:PC1=1) */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 AMI306_REG_CNTL1,
+					 AMI306_BIT_CNTL1_PC1 |
+					 AMI306_BIT_CNTL1_FS1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Step2. Set CNTL2 reg to DRDY active high and enabled
+	   (Write CNTL2:DREN=1) */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 AMI306_REG_CNTL2,
+					 AMI306_BIT_CNTL2_DREN |
+					 AMI306_BIT_CNTL2_DRP);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Step3. Set CNTL4 reg to for measurement speed: Write CNTL4, 0xA07E */
+	result = inv_serial_write(mlsl_handle, pdata->address,
+				ARRAY_SIZE(regs), regs);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Step4. skipped */
+
+	/* Step5. Set CNTL3 reg to forced measurement period
+	   (Write CNTL3:FORCE=1) */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 AMI306_REG_CNTL3,
+					 AMI306_BIT_CNTL3_F0RCE);
+
+	return result;
+}
+
+static int ami306_read(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data)
+{
+	int result = INV_SUCCESS;
+	int ii;
+	short val[COMPASS_NUM_AXES];
+
+	result = ami306_mea(mlsl_handle, pdata, val);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	for (ii = 0; ii < COMPASS_NUM_AXES; ii++) {
+		val[ii] -= AMI_STANDARD_OFFSET;
+		data[2 * ii] = val[ii] & 0xFF;
+		data[(2 * ii) + 1] = (val[ii] >> 8) & 0xFF;
+	}
+	return result;
+}
+
+static int ami306_init(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	int result;
+	struct ami306_private_data *private_data;
+	private_data = (struct ami306_private_data *)
+	    kzalloc(sizeof(struct ami306_private_data), GFP_KERNEL);
+
+	if (!private_data)
+		return INV_ERROR_MEMORY_EXAUSTED;
+
+	pdata->private_data = private_data;
+	result = ami306_set_bits8(mlsl_handle, pdata,
+				  AMI_REG_CTRL1,
+				  AMI_CTRL1_PC1 | AMI_CTRL1_FS1_FORCE);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Read Parameters */
+	result = ami306_read_param(mlsl_handle, slave, pdata);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Read Window */
+	result = ami306_initial_b0_adjust(mlsl_handle, slave, pdata);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = ami306_start_sensor(mlsl_handle, pdata);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = ami306_read_win(mlsl_handle, slave, pdata);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 AMI306_REG_CNTL1, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return INV_SUCCESS;
+}
+
+static int ami306_exit(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata)
+{
+	kfree(pdata->private_data);
+	return INV_SUCCESS;
+}
+
+static int ami306_config(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata,
+			 struct ext_slave_config *data)
+{
+	if (!data->data) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	switch (data->key) {
+	case MPU_SLAVE_PARAM:
+	case MPU_SLAVE_WINDOW:
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+	case MPU_SLAVE_CONFIG_MOT_THS:
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static int ami306_get_config(void *mlsl_handle,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata,
+			     struct ext_slave_config *data)
+{
+	int result;
+	struct ami306_private_data *private_data = pdata->private_data;
+	if (!data->data) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	switch (data->key) {
+	case MPU_SLAVE_PARAM:
+		if (sizeof(struct ami_sensor_parametor) > data->len) {
+			LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+			return INV_ERROR_INVALID_PARAMETER;
+		}
+		if (data->apply) {
+			result = ami306_read_param(mlsl_handle, slave, pdata);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+		memcpy(data->data, &private_data->param,
+		       sizeof(struct ami_sensor_parametor));
+		break;
+	case MPU_SLAVE_WINDOW:
+		if (sizeof(struct ami_win_parameter) > data->len) {
+			LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+			return INV_ERROR_INVALID_PARAMETER;
+		}
+		if (data->apply) {
+			result = ami306_read_win(mlsl_handle, slave, pdata);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+		memcpy(data->data, &private_data->win,
+		       sizeof(struct ami_win_parameter));
+		break;
+	case MPU_SLAVE_SEARCHOFFSET:
+		if (sizeof(struct ami_win_parameter) > data->len) {
+			LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+			return INV_ERROR_INVALID_PARAMETER;
+		}
+		if (data->apply) {
+			result = ami306_search_offset(mlsl_handle,
+						      slave, pdata);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			/* Start sensor */
+			result = ami306_start_sensor(mlsl_handle, pdata);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			result = ami306_read_win(mlsl_handle, slave, pdata);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+		memcpy(data->data, &private_data->win,
+		       sizeof(struct ami_win_parameter));
+		break;
+	case MPU_SLAVE_READWINPARAMS:
+		if (sizeof(struct ami_win_parameter) > data->len) {
+			LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+			return INV_ERROR_INVALID_PARAMETER;
+		}
+		if (data->apply) {
+			result = ami306_initial_b0_adjust(mlsl_handle,
+							  slave, pdata);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			/* Start sensor */
+			result = ami306_start_sensor(mlsl_handle, pdata);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			result = ami306_read_win(mlsl_handle, slave, pdata);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+		memcpy(data->data, &private_data->win,
+		       sizeof(struct ami_win_parameter));
+		break;
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) = 0;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) = 50000;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+	case MPU_SLAVE_CONFIG_MOT_THS:
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+	case MPU_SLAVE_READ_SCALE:
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+static struct ext_slave_read_trigger ami306_read_trigger = {
+	/*.reg              = */ AMI_REG_CTRL3,
+	/*.value            = */ AMI_CTRL3_FORCE_BIT
+};
+
+static struct ext_slave_descr ami306_descr = {
+	.init             = ami306_init,
+	.exit             = ami306_exit,
+	.suspend          = ami306_suspend,
+	.resume           = ami306_resume,
+	.read             = ami306_read,
+	.config           = ami306_config,
+	.get_config       = ami306_get_config,
+	.name             = "ami306",
+	.type             = EXT_SLAVE_TYPE_COMPASS,
+	.id               = COMPASS_ID_AMI306,
+	.read_reg         = 0x0E,
+	.read_len         = 13,
+	.endian           = EXT_SLAVE_LITTLE_ENDIAN,
+	.range            = {5461, 3333},
+	.trigger          = &ami306_read_trigger,
+};
+
+static
+struct ext_slave_descr *ami306_get_slave_descr(void)
+{
+	return &ami306_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct ami306_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int ami306_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct ami306_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					ami306_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int ami306_mod_remove(struct i2c_client *client)
+{
+	struct ami306_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				ami306_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id ami306_mod_id[] = {
+	{ "ami306", COMPASS_ID_AMI306 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, ami306_mod_id);
+
+static struct i2c_driver ami306_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = ami306_mod_probe,
+	.remove = ami306_mod_remove,
+	.id_table = ami306_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "ami306_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init ami306_mod_init(void)
+{
+	int res = i2c_add_driver(&ami306_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "ami306_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit ami306_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&ami306_mod_driver);
+}
+
+module_init(ami306_mod_init);
+module_exit(ami306_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate AMI306 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("ami306_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/ami30x.c b/drivers/misc/inv_mpu/compass/ami30x.c
new file mode 100644
index 000000000000..0c4937c44263
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/ami30x.c
@@ -0,0 +1,308 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup COMPASSDL
+ *
+ *  @{
+ *      @file   ami30x.c
+ *      @brief  Magnetometer setup and handling methods for Aichi AMI304
+ *              and AMI305 compass devices.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define AMI30X_REG_DATAX (0x10)
+#define AMI30X_REG_STAT1 (0x18)
+#define AMI30X_REG_CNTL1 (0x1B)
+#define AMI30X_REG_CNTL2 (0x1C)
+#define AMI30X_REG_CNTL3 (0x1D)
+
+#define AMI30X_BIT_CNTL1_PC1  (0x80)
+#define AMI30X_BIT_CNTL1_ODR1 (0x10)
+#define AMI30X_BIT_CNTL1_FS1  (0x02)
+
+#define AMI30X_BIT_CNTL2_IEN  (0x10)
+#define AMI30X_BIT_CNTL2_DREN (0x08)
+#define AMI30X_BIT_CNTL2_DRP  (0x04)
+#define AMI30X_BIT_CNTL3_F0RCE (0x40)
+
+/* -------------------------------------------------------------------------- */
+static int ami30x_suspend(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char reg;
+	result =
+	    inv_serial_read(mlsl_handle, pdata->address, AMI30X_REG_CNTL1,
+			    1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	reg &= ~(AMI30X_BIT_CNTL1_PC1 | AMI30X_BIT_CNTL1_FS1);
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    AMI30X_REG_CNTL1, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+static int ami30x_resume(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	/* Set CNTL1 reg to power model active */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    AMI30X_REG_CNTL1,
+				    AMI30X_BIT_CNTL1_PC1 |
+				    AMI30X_BIT_CNTL1_FS1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Set CNTL2 reg to DRDY active high and enabled */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    AMI30X_REG_CNTL2,
+				    AMI30X_BIT_CNTL2_DREN |
+				    AMI30X_BIT_CNTL2_DRP);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Set CNTL3 reg to forced measurement period */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    AMI30X_REG_CNTL3, AMI30X_BIT_CNTL3_F0RCE);
+
+	return result;
+}
+
+static int ami30x_read(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data)
+{
+	unsigned char stat;
+	int result = INV_SUCCESS;
+
+	/* Read status reg and check if data ready (DRDY) */
+	result =
+	    inv_serial_read(mlsl_handle, pdata->address, AMI30X_REG_STAT1,
+			    1, &stat);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (stat & 0x40) {
+		result =
+		    inv_serial_read(mlsl_handle, pdata->address,
+				    AMI30X_REG_DATAX, 6, (unsigned char *)data);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		/* start another measurement */
+		result =
+		    inv_serial_single_write(mlsl_handle, pdata->address,
+					    AMI30X_REG_CNTL3,
+					    AMI30X_BIT_CNTL3_F0RCE);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+		return INV_SUCCESS;
+	}
+
+	return INV_ERROR_COMPASS_DATA_NOT_READY;
+}
+
+
+/* For AMI305,the range field needs to be modified to {9830.4f} */
+static struct ext_slave_descr ami30x_descr = {
+	.init             = NULL,
+	.exit             = NULL,
+	.suspend          = ami30x_suspend,
+	.resume           = ami30x_resume,
+	.read             = ami30x_read,
+	.config           = NULL,
+	.get_config       = NULL,
+	.name             = "ami30x",
+	.type             = EXT_SLAVE_TYPE_COMPASS,
+	.id               = COMPASS_ID_AMI30X,
+	.read_reg         = 0x06,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_LITTLE_ENDIAN,
+	.range            = {5461, 3333},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *ami30x_get_slave_descr(void)
+{
+	return &ami30x_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct ami30x_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int ami30x_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct ami30x_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					ami30x_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int ami30x_mod_remove(struct i2c_client *client)
+{
+	struct ami30x_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				ami30x_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id ami30x_mod_id[] = {
+	{ "ami30x", COMPASS_ID_AMI30X },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, ami30x_mod_id);
+
+static struct i2c_driver ami30x_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = ami30x_mod_probe,
+	.remove = ami30x_mod_remove,
+	.id_table = ami30x_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "ami30x_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init ami30x_mod_init(void)
+{
+	int res = i2c_add_driver(&ami30x_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "ami30x_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit ami30x_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&ami30x_mod_driver);
+}
+
+module_init(ami30x_mod_init);
+module_exit(ami30x_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate AMI30X sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("ami30x_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/ami_hw.h b/drivers/misc/inv_mpu/compass/ami_hw.h
new file mode 100644
index 000000000000..32a04e91cdc1
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/ami_hw.h
@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) 2010 Information System Products Co.,Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef AMI_HW_H
+#define AMI_HW_H
+
+#define	AMI_I2C_BUS_NUM			2
+
+#ifdef	AMI304_MODEL
+#define AMI_I2C_ADDRESS			0x0F
+#else
+#define AMI_I2C_ADDRESS			0x0E
+#endif
+
+#define AMI_GPIO_INT			152
+#define AMI_GPIO_DRDY			153
+
+/* AMI-Sensor Internal Register Address
+ *(Please refer to AMI-Sensor Specifications)
+ */
+#define AMI_MOREINFO_CMDCODE		0x0d
+#define AMI_WHOIAM_CMDCODE		0x0f
+#define AMI_REG_DATAX			0x10
+#define AMI_REG_DATAY			0x12
+#define AMI_REG_DATAZ			0x14
+#define AMI_REG_STA1			0x18
+#define AMI_REG_CTRL1			0x1b
+#define AMI_REG_CTRL2			0x1c
+#define AMI_REG_CTRL3			0x1d
+#define AMI_REG_B0X			0x20
+#define AMI_REG_B0Y			0x22
+#define AMI_REG_B0Z			0x24
+#define AMI_REG_CTRL5			0x40
+#define AMI_REG_CTRL4			0x5c
+#define AMI_REG_TEMP			0x60
+#define AMI_REG_DELAYX			0x68
+#define AMI_REG_DELAYY			0x6e
+#define AMI_REG_DELAYZ			0x74
+#define AMI_REG_OFFX			0x6c
+#define AMI_REG_OFFY			0x72
+#define AMI_REG_OFFZ			0x78
+#define AMI_FINEOUTPUT_X		0x90
+#define AMI_FINEOUTPUT_Y		0x92
+#define AMI_FINEOUTPUT_Z		0x94
+#define AMI_REG_SENX			0x96
+#define AMI_REG_SENY			0x98
+#define AMI_REG_SENZ			0x9a
+#define AMI_REG_GAINX			0x9c
+#define AMI_REG_GAINY			0x9e
+#define AMI_REG_GAINZ			0xa0
+#define AMI_GETVERSION_CMDCODE		0xe8
+#define AMI_SERIALNUMBER_CMDCODE	0xea
+#define AMI_REG_B0OTPX			0xa2
+#define AMI_REG_B0OTPY			0xb8
+#define AMI_REG_B0OTPZ			0xce
+#define AMI_REG_OFFOTPX			0xf8
+#define AMI_REG_OFFOTPY			0xfa
+#define AMI_REG_OFFOTPZ			0xfc
+
+/* AMI-Sensor Control Bit  (Please refer to AMI-Sensor Specifications) */
+#define AMI_CTRL1_PC1			0x80
+#define AMI_CTRL1_FS1_FORCE		0x02
+#define AMI_CTRL1_ODR1			0x10
+#define AMI_CTRL2_DREN			0x08
+#define AMI_CTRL2_DRP			0x04
+#define AMI_CTRL3_FORCE_BIT		0x40
+#define AMI_CTRL3_B0_LO_BIT		0x10
+#define AMI_CTRL3_SRST_BIT		0x80
+#define AMI_CTRL4_HS			0xa07e
+#define AMI_CTRL4_AB			0x0001
+#define AMI_STA1_DRDY_BIT		0x40
+#define AMI_STA1_DOR_BIT		0x20
+
+#endif
diff --git a/drivers/misc/inv_mpu/compass/ami_sensor_def.h b/drivers/misc/inv_mpu/compass/ami_sensor_def.h
new file mode 100644
index 000000000000..64032e2bf1fb
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/ami_sensor_def.h
@@ -0,0 +1,144 @@
+/*
+ * Copyright (C) 2010 Information System Products Co.,Ltd.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ * Definitions for ami306 compass chip.
+ */
+#ifndef AMI_SENSOR_DEF_H
+#define AMI_SENSOR_DEF_H
+
+/*********************************************************************
+ Constant
+ *********************************************************************/
+#define	AMI_OK		0x00			/**< Normal */
+#define	AMI_PARAM_ERR	0x01			/**< Parameter Error  */
+#define	AMI_SEQ_ERR	0x02			/**< Squence Error  */
+#define	AMI_SYSTEM_ERR	0x10			/**< System Error  */
+#define AMI_BLOCK_ERR	0x20			/**< Block Error */
+#define	AMI_ERROR	0x99			/**< other Error  */
+
+/*********************************************************************
+ Struct definition
+ *********************************************************************/
+/** axis sensitivity(gain) calibration parameter information  */
+struct ami_vector3d {
+	signed short x;			/**< X-axis  */
+	signed short y;			/**< Y-axis  */
+	signed short z;			/**< Z-axis  */
+};
+
+/** axis interference information  */
+struct ami_interference {
+	/**< Y-axis magnetic field for X-axis correction value  */
+	signed short xy;
+	/**< Z-axis magnetic field for X-axis correction value  */
+	signed short xz;
+	/**< X-axis magnetic field for Y-axis correction value  */
+	signed short yx;
+	/**< Z-axis magnetic field for Y-axis correction value  */
+	signed short yz;
+	/**< X-axis magnetic field for Z-axis correction value  */
+	signed short zx;
+	/**< Y-axis magnetic field for Z-axis correction value  */
+	signed short zy;
+};
+
+/** sensor calibration Parameter information  */
+struct ami_sensor_parametor {
+	/**< geomagnetic field sensor gain  */
+	struct ami_vector3d m_gain;
+	/**< geomagnetic field sensor gain correction parameter  */
+	struct ami_vector3d m_gain_cor;
+	/**< geomagnetic field sensor offset  */
+	struct ami_vector3d m_offset;
+	/**< geomagnetic field sensor axis interference parameter */
+	struct ami_interference m_interference;
+#ifdef AMI_6AXIS
+	/**< acceleration sensor gain  */
+	struct ami_vector3d a_gain;
+	/**< acceleration sensor offset  */
+	struct ami_vector3d a_offset;
+	/**< acceleration sensor deviation  */
+	signed short a_deviation;
+#endif
+};
+
+/** G2-Sensor measurement value (voltage ADC value ) */
+struct ami_sensor_rawvalue {
+	/**< geomagnetic field sensor measurement X-axis value
+	(mounted position/direction reference) */
+	unsigned short mx;
+	/**< geomagnetic field sensor measurement Y-axis value
+	(mounted position/direction reference) */
+	unsigned short my;
+	/**< geomagnetic field sensor measurement Z-axis value
+	(mounted position/direction reference) */
+	unsigned short mz;
+#ifdef AMI_6AXIS
+	/**< acceleration sensor measurement X-axis value
+	(mounted position/direction reference) */
+	unsigned short ax;
+	/**< acceleration sensor measurement Y-axis value
+	(mounted position/direction reference) */
+	unsigned short ay;
+	/**< acceleration sensor measurement Z-axis value
+	(mounted position/direction reference) */
+	unsigned short az;
+#endif
+	/**< temperature sensor measurement value  */
+	unsigned short temperature;
+};
+
+/** Window function Parameter information  */
+struct ami_win_parameter {
+	/**< current fine value  */
+	struct ami_vector3d m_fine;
+	/**< change per 1coarse */
+	struct ami_vector3d m_fine_output;
+	/**< fine value at zero gauss */
+	struct ami_vector3d m_0Gauss_fine;
+#ifdef AMI304
+	/**< current b0 value  */
+	struct ami_vector3d m_b0;
+	/**< current coarse value  */
+	struct ami_vector3d m_coar;
+	/**< change per 1fine */
+	struct ami_vector3d m_coar_output;
+	/**< coarse value at zero gauss */
+	struct ami_vector3d m_0Gauss_coar;
+	/**< delay value  */
+	struct ami_vector3d m_delay;
+#endif
+};
+
+/** AMI chip information ex) 1)model 2)s/n 3)ver 4)more info in the chip */
+struct ami_chipinfo {
+	unsigned short info;	/* INFO 0x0d/0x0e reg.  */
+	unsigned short ver;	/* VER  0xe8/0xe9 reg.  */
+	unsigned short sn;	/* SN   0xea/0xeb reg.  */
+	unsigned char wia;	/* WIA  0x0f      reg.  */
+};
+
+/** AMI Driver Information  */
+struct ami_driverinfo {
+	unsigned char remarks[40];	/* Some Information   */
+	unsigned char datetime[30];	/* compiled date&time */
+	unsigned char ver_major;	/* major version */
+	unsigned char ver_middle;	/* middle.. */
+	unsigned char ver_minor;	/* minor .. */
+};
+
+#endif
diff --git a/drivers/misc/inv_mpu/compass/hmc5883.c b/drivers/misc/inv_mpu/compass/hmc5883.c
new file mode 100644
index 000000000000..fdf2ac00565a
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/hmc5883.c
@@ -0,0 +1,391 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup COMPASSDL
+ *
+ *  @{
+ *      @file   hmc5883.c
+ *      @brief  Magnetometer setup and handling methods for Honeywell
+ *              HMC5883 compass.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+enum HMC_REG {
+	HMC_REG_CONF_A = 0x0,
+	HMC_REG_CONF_B = 0x1,
+	HMC_REG_MODE = 0x2,
+	HMC_REG_X_M = 0x3,
+	HMC_REG_X_L = 0x4,
+	HMC_REG_Z_M = 0x5,
+	HMC_REG_Z_L = 0x6,
+	HMC_REG_Y_M = 0x7,
+	HMC_REG_Y_L = 0x8,
+	HMC_REG_STATUS = 0x9,
+	HMC_REG_ID_A = 0xA,
+	HMC_REG_ID_B = 0xB,
+	HMC_REG_ID_C = 0xC
+};
+
+enum HMC_CONF_A {
+	HMC_CONF_A_DRATE_MASK = 0x1C,
+	HMC_CONF_A_DRATE_0_75 = 0x00,
+	HMC_CONF_A_DRATE_1_5 = 0x04,
+	HMC_CONF_A_DRATE_3 = 0x08,
+	HMC_CONF_A_DRATE_7_5 = 0x0C,
+	HMC_CONF_A_DRATE_15 = 0x10,
+	HMC_CONF_A_DRATE_30 = 0x14,
+	HMC_CONF_A_DRATE_75 = 0x18,
+	HMC_CONF_A_MEAS_MASK = 0x3,
+	HMC_CONF_A_MEAS_NORM = 0x0,
+	HMC_CONF_A_MEAS_POS = 0x1,
+	HMC_CONF_A_MEAS_NEG = 0x2
+};
+
+enum HMC_CONF_B {
+	HMC_CONF_B_GAIN_MASK = 0xE0,
+	HMC_CONF_B_GAIN_0_9 = 0x00,
+	HMC_CONF_B_GAIN_1_2 = 0x20,
+	HMC_CONF_B_GAIN_1_9 = 0x40,
+	HMC_CONF_B_GAIN_2_5 = 0x60,
+	HMC_CONF_B_GAIN_4_0 = 0x80,
+	HMC_CONF_B_GAIN_4_6 = 0xA0,
+	HMC_CONF_B_GAIN_5_5 = 0xC0,
+	HMC_CONF_B_GAIN_7_9 = 0xE0
+};
+
+enum HMC_MODE {
+	HMC_MODE_MASK = 0x3,
+	HMC_MODE_CONT = 0x0,
+	HMC_MODE_SINGLE = 0x1,
+	HMC_MODE_IDLE = 0x2,
+	HMC_MODE_SLEEP = 0x3
+};
+
+/* -------------------------------------------------------------------------- */
+static int hmc5883_suspend(void *mlsl_handle,
+			   struct ext_slave_descr *slave,
+			   struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    HMC_REG_MODE, HMC_MODE_SLEEP);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(3);
+
+	return result;
+}
+
+static int hmc5883_resume(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	/* Use single measurement mode. Start at sleep state. */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    HMC_REG_MODE, HMC_MODE_SLEEP);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Config normal measurement */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    HMC_REG_CONF_A, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Adjust gain to 307 LSB/Gauss */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    HMC_REG_CONF_B, HMC_CONF_B_GAIN_5_5);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+static int hmc5883_read(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			unsigned char *data)
+{
+	unsigned char stat;
+	int result = INV_SUCCESS;
+	unsigned char tmp;
+	short axisFixed;
+
+	/* Read status reg. to check if data is ready */
+	result =
+	    inv_serial_read(mlsl_handle, pdata->address, HMC_REG_STATUS, 1,
+			    &stat);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	if (stat & 0x01) {
+		result =
+		    inv_serial_read(mlsl_handle, pdata->address,
+				    HMC_REG_X_M, 6, (unsigned char *)data);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+		/* switch YZ axis to proper position */
+		tmp = data[2];
+		data[2] = data[4];
+		data[4] = tmp;
+		tmp = data[3];
+		data[3] = data[5];
+		data[5] = tmp;
+
+		/*drop data if overflows */
+		if ((data[0] == 0xf0) || (data[2] == 0xf0)
+		    || (data[4] == 0xf0)) {
+			/* trigger next measurement read */
+			result =
+			    inv_serial_single_write(mlsl_handle,
+						    pdata->address,
+						    HMC_REG_MODE,
+						    HMC_MODE_SINGLE);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			return INV_ERROR_COMPASS_DATA_OVERFLOW;
+		}
+		/* convert to fixed point and apply sensitivity correction for
+		   Z-axis */
+		axisFixed =
+		    (short)((unsigned short)data[5] +
+			    (unsigned short)data[4] * 256);
+		/* scale up by 1.125 (36/32) */
+		axisFixed = (short)(axisFixed * 36);
+		data[4] = axisFixed >> 8;
+		data[5] = axisFixed & 0xFF;
+
+		axisFixed =
+		    (short)((unsigned short)data[3] +
+			    (unsigned short)data[2] * 256);
+		axisFixed = (short)(axisFixed * 32);
+		data[2] = axisFixed >> 8;
+		data[3] = axisFixed & 0xFF;
+
+		axisFixed =
+		    (short)((unsigned short)data[1] +
+			    (unsigned short)data[0] * 256);
+		axisFixed = (short)(axisFixed * 32);
+		data[0] = axisFixed >> 8;
+		data[1] = axisFixed & 0xFF;
+
+		/* trigger next measurement read */
+		result =
+		    inv_serial_single_write(mlsl_handle, pdata->address,
+					    HMC_REG_MODE, HMC_MODE_SINGLE);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+		return INV_SUCCESS;
+	} else {
+		/* trigger next measurement read */
+		result =
+		    inv_serial_single_write(mlsl_handle, pdata->address,
+					    HMC_REG_MODE, HMC_MODE_SINGLE);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+		return INV_ERROR_COMPASS_DATA_NOT_READY;
+	}
+}
+
+static struct ext_slave_descr hmc5883_descr = {
+	.init             = NULL,
+	.exit             = NULL,
+	.suspend          = hmc5883_suspend,
+	.resume           = hmc5883_resume,
+	.read             = hmc5883_read,
+	.config           = NULL,
+	.get_config       = NULL,
+	.name             = "hmc5883",
+	.type             = EXT_SLAVE_TYPE_COMPASS,
+	.id               = COMPASS_ID_HMC5883,
+	.read_reg         = 0x06,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_BIG_ENDIAN,
+	.range            = {10673, 6156},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *hmc5883_get_slave_descr(void)
+{
+	return &hmc5883_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct hmc5883_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int hmc5883_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct hmc5883_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					hmc5883_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int hmc5883_mod_remove(struct i2c_client *client)
+{
+	struct hmc5883_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				hmc5883_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id hmc5883_mod_id[] = {
+	{ "hmc5883", COMPASS_ID_HMC5883 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, hmc5883_mod_id);
+
+static struct i2c_driver hmc5883_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = hmc5883_mod_probe,
+	.remove = hmc5883_mod_remove,
+	.id_table = hmc5883_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "hmc5883_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init hmc5883_mod_init(void)
+{
+	int res = i2c_add_driver(&hmc5883_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "hmc5883_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit hmc5883_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&hmc5883_mod_driver);
+}
+
+module_init(hmc5883_mod_init);
+module_exit(hmc5883_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate HMC5883 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("hmc5883_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/hscdtd002b.c b/drivers/misc/inv_mpu/compass/hscdtd002b.c
new file mode 100644
index 000000000000..4f6013cbe3dc
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/hscdtd002b.c
@@ -0,0 +1,294 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup COMPASSDL
+ *
+ *  @{
+ *      @file   hscdtd002b.c
+ *      @brief  Magnetometer setup and handling methods for Alps HSCDTD002B
+ *              compass.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define COMPASS_HSCDTD002B_STAT          (0x18)
+#define COMPASS_HSCDTD002B_CTRL1         (0x1B)
+#define COMPASS_HSCDTD002B_CTRL2         (0x1C)
+#define COMPASS_HSCDTD002B_CTRL3         (0x1D)
+#define COMPASS_HSCDTD002B_DATAX         (0x10)
+
+/* -------------------------------------------------------------------------- */
+static int hscdtd002b_suspend(void *mlsl_handle,
+			      struct ext_slave_descr *slave,
+			      struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	/* Power mode: stand-by */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    COMPASS_HSCDTD002B_CTRL1, 0x00);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(1);		/* turn-off time */
+
+	return result;
+}
+
+static int hscdtd002b_resume(void *mlsl_handle,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	/* Soft reset */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    COMPASS_HSCDTD002B_CTRL3, 0x80);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Force state; Power mode: active */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    COMPASS_HSCDTD002B_CTRL1, 0x82);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Data ready enable */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    COMPASS_HSCDTD002B_CTRL2, 0x08);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(1);		/* turn-on time */
+
+	return result;
+}
+
+static int hscdtd002b_read(void *mlsl_handle,
+			   struct ext_slave_descr *slave,
+			   struct ext_slave_platform_data *pdata,
+			   unsigned char *data)
+{
+	unsigned char stat;
+	int result = INV_SUCCESS;
+	int status = INV_SUCCESS;
+
+	/* Read status reg. to check if data is ready */
+	result =
+	    inv_serial_read(mlsl_handle, pdata->address,
+			    COMPASS_HSCDTD002B_STAT, 1, &stat);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	if (stat & 0x40) {
+		result =
+		    inv_serial_read(mlsl_handle, pdata->address,
+				    COMPASS_HSCDTD002B_DATAX, 6,
+				    (unsigned char *)data);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		status = INV_SUCCESS;
+	} else if (stat & 0x20) {
+		status = INV_ERROR_COMPASS_DATA_OVERFLOW;
+	} else {
+		status = INV_ERROR_COMPASS_DATA_NOT_READY;
+	}
+	/* trigger next measurement read */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    COMPASS_HSCDTD002B_CTRL3, 0x40);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return status;
+}
+
+static struct ext_slave_descr hscdtd002b_descr = {
+	.init             = NULL,
+	.exit             = NULL,
+	.suspend          = hscdtd002b_suspend,
+	.resume           = hscdtd002b_resume,
+	.read             = hscdtd002b_read,
+	.config           = NULL,
+	.get_config       = NULL,
+	.name             = "hscdtd002b",
+	.type             = EXT_SLAVE_TYPE_COMPASS,
+	.id               = COMPASS_ID_HSCDTD002B,
+	.read_reg         = 0x10,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_LITTLE_ENDIAN,
+	.range            = {9830, 4000},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *hscdtd002b_get_slave_descr(void)
+{
+	return &hscdtd002b_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct hscdtd002b_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int hscdtd002b_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct hscdtd002b_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					hscdtd002b_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int hscdtd002b_mod_remove(struct i2c_client *client)
+{
+	struct hscdtd002b_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				hscdtd002b_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id hscdtd002b_mod_id[] = {
+	{ "hscdtd002b", COMPASS_ID_HSCDTD002B },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, hscdtd002b_mod_id);
+
+static struct i2c_driver hscdtd002b_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = hscdtd002b_mod_probe,
+	.remove = hscdtd002b_mod_remove,
+	.id_table = hscdtd002b_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "hscdtd002b_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init hscdtd002b_mod_init(void)
+{
+	int res = i2c_add_driver(&hscdtd002b_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "hscdtd002b_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit hscdtd002b_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&hscdtd002b_mod_driver);
+}
+
+module_init(hscdtd002b_mod_init);
+module_exit(hscdtd002b_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate HSCDTD002B sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("hscdtd002b_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/hscdtd004a.c b/drivers/misc/inv_mpu/compass/hscdtd004a.c
new file mode 100644
index 000000000000..f0915599bd2f
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/hscdtd004a.c
@@ -0,0 +1,318 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup COMPASSDL
+ *
+ *  @{
+ *      @file   hscdtd004a.c
+ *      @brief  Magnetometer setup and handling methods for Alps HSCDTD004A
+ *              compass.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define COMPASS_HSCDTD004A_STAT          (0x18)
+#define COMPASS_HSCDTD004A_CTRL1         (0x1B)
+#define COMPASS_HSCDTD004A_CTRL2         (0x1C)
+#define COMPASS_HSCDTD004A_CTRL3         (0x1D)
+#define COMPASS_HSCDTD004A_DATAX         (0x10)
+
+/* -------------------------------------------------------------------------- */
+
+static int hscdtd004a_suspend(void *mlsl_handle,
+			      struct ext_slave_descr *slave,
+			      struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	/* Power mode: stand-by */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    COMPASS_HSCDTD004A_CTRL1, 0x00);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(1);		/* turn-off time */
+
+	return result;
+}
+
+static int hscdtd004a_init(void *mlsl_handle,
+			   struct ext_slave_descr *slave,
+			   struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char data1, data2[2];
+
+	result = inv_serial_read(mlsl_handle, pdata->address, 0xf, 1, &data1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(mlsl_handle, pdata->address, 0xd, 2, data2);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	if (data1 != 0x49 || data2[0] != 0x45 || data2[1] != 0x54) {
+		LOG_RESULT_LOCATION(INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED);
+		return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
+	}
+	return result;
+}
+
+static int hscdtd004a_resume(void *mlsl_handle,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	/* Soft reset */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    COMPASS_HSCDTD004A_CTRL3, 0x80);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Normal state; Power mode: active */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    COMPASS_HSCDTD004A_CTRL1, 0x82);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Data ready enable */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    COMPASS_HSCDTD004A_CTRL2, 0x7C);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(1);		/* turn-on time */
+	return result;
+}
+
+static int hscdtd004a_read(void *mlsl_handle,
+			   struct ext_slave_descr *slave,
+			   struct ext_slave_platform_data *pdata,
+			   unsigned char *data)
+{
+	unsigned char stat;
+	int result = INV_SUCCESS;
+	int status = INV_SUCCESS;
+
+	/* Read status reg. to check if data is ready */
+	result =
+	    inv_serial_read(mlsl_handle, pdata->address,
+			    COMPASS_HSCDTD004A_STAT, 1, &stat);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	if (stat & 0x48) {
+		result =
+		    inv_serial_read(mlsl_handle, pdata->address,
+				    COMPASS_HSCDTD004A_DATAX, 6,
+				    (unsigned char *)data);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		status = INV_SUCCESS;
+	} else if (stat & 0x68) {
+		status = INV_ERROR_COMPASS_DATA_OVERFLOW;
+	} else {
+		status = INV_ERROR_COMPASS_DATA_NOT_READY;
+	}
+	/* trigger next measurement read */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    COMPASS_HSCDTD004A_CTRL3, 0x40);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return status;
+
+}
+
+static struct ext_slave_descr hscdtd004a_descr = {
+	.init             = hscdtd004a_init,
+	.exit             = NULL,
+	.suspend          = hscdtd004a_suspend,
+	.resume           = hscdtd004a_resume,
+	.read             = hscdtd004a_read,
+	.config           = NULL,
+	.get_config       = NULL,
+	.name             = "hscdtd004a",
+	.type             = EXT_SLAVE_TYPE_COMPASS,
+	.id               = COMPASS_ID_HSCDTD004A,
+	.read_reg         = 0x10,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_LITTLE_ENDIAN,
+	.range            = {9830, 4000},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *hscdtd004a_get_slave_descr(void)
+{
+	return &hscdtd004a_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct hscdtd004a_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int hscdtd004a_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct hscdtd004a_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					hscdtd004a_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int hscdtd004a_mod_remove(struct i2c_client *client)
+{
+	struct hscdtd004a_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				hscdtd004a_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id hscdtd004a_mod_id[] = {
+	{ "hscdtd004a", COMPASS_ID_HSCDTD004A },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, hscdtd004a_mod_id);
+
+static struct i2c_driver hscdtd004a_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = hscdtd004a_mod_probe,
+	.remove = hscdtd004a_mod_remove,
+	.id_table = hscdtd004a_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "hscdtd004a_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init hscdtd004a_mod_init(void)
+{
+	int res = i2c_add_driver(&hscdtd004a_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "hscdtd004a_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit hscdtd004a_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&hscdtd004a_mod_driver);
+}
+
+module_init(hscdtd004a_mod_init);
+module_exit(hscdtd004a_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate HSCDTD004A sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("hscdtd004a_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/lsm303dlx_m.c b/drivers/misc/inv_mpu/compass/lsm303dlx_m.c
new file mode 100644
index 000000000000..32f8cdddb00b
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/lsm303dlx_m.c
@@ -0,0 +1,395 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup COMPASSDL
+ *
+ *  @{
+ *      @file   lsm303dlx_m.c
+ *      @brief  Magnetometer setup and handling methods for ST LSM303
+ *              compass.
+ *              This magnetometer device is part of a combo chip with the
+ *              ST LIS331DLH accelerometer and the logic in entirely based
+ *              on the Honeywell HMC5883 magnetometer.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+enum LSM_REG {
+	LSM_REG_CONF_A = 0x0,
+	LSM_REG_CONF_B = 0x1,
+	LSM_REG_MODE = 0x2,
+	LSM_REG_X_M = 0x3,
+	LSM_REG_X_L = 0x4,
+	LSM_REG_Z_M = 0x5,
+	LSM_REG_Z_L = 0x6,
+	LSM_REG_Y_M = 0x7,
+	LSM_REG_Y_L = 0x8,
+	LSM_REG_STATUS = 0x9,
+	LSM_REG_ID_A = 0xA,
+	LSM_REG_ID_B = 0xB,
+	LSM_REG_ID_C = 0xC
+};
+
+enum LSM_CONF_A {
+	LSM_CONF_A_DRATE_MASK = 0x1C,
+	LSM_CONF_A_DRATE_0_75 = 0x00,
+	LSM_CONF_A_DRATE_1_5 = 0x04,
+	LSM_CONF_A_DRATE_3 = 0x08,
+	LSM_CONF_A_DRATE_7_5 = 0x0C,
+	LSM_CONF_A_DRATE_15 = 0x10,
+	LSM_CONF_A_DRATE_30 = 0x14,
+	LSM_CONF_A_DRATE_75 = 0x18,
+	LSM_CONF_A_MEAS_MASK = 0x3,
+	LSM_CONF_A_MEAS_NORM = 0x0,
+	LSM_CONF_A_MEAS_POS = 0x1,
+	LSM_CONF_A_MEAS_NEG = 0x2
+};
+
+enum LSM_CONF_B {
+	LSM_CONF_B_GAIN_MASK = 0xE0,
+	LSM_CONF_B_GAIN_0_9 = 0x00,
+	LSM_CONF_B_GAIN_1_2 = 0x20,
+	LSM_CONF_B_GAIN_1_9 = 0x40,
+	LSM_CONF_B_GAIN_2_5 = 0x60,
+	LSM_CONF_B_GAIN_4_0 = 0x80,
+	LSM_CONF_B_GAIN_4_6 = 0xA0,
+	LSM_CONF_B_GAIN_5_5 = 0xC0,
+	LSM_CONF_B_GAIN_7_9 = 0xE0
+};
+
+enum LSM_MODE {
+	LSM_MODE_MASK = 0x3,
+	LSM_MODE_CONT = 0x0,
+	LSM_MODE_SINGLE = 0x1,
+	LSM_MODE_IDLE = 0x2,
+	LSM_MODE_SLEEP = 0x3
+};
+
+/* -------------------------------------------------------------------------- */
+
+static int lsm303dlx_m_suspend(void *mlsl_handle,
+			      struct ext_slave_descr *slave,
+			      struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    LSM_REG_MODE, LSM_MODE_SLEEP);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(3);
+
+	return result;
+}
+
+static int lsm303dlx_m_resume(void *mlsl_handle,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	/* Use single measurement mode. Start at sleep state. */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    LSM_REG_MODE, LSM_MODE_SLEEP);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Config normal measurement */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    LSM_REG_CONF_A, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Adjust gain to 320 LSB/Gauss */
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    LSM_REG_CONF_B, LSM_CONF_B_GAIN_5_5);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+static int lsm303dlx_m_read(void *mlsl_handle,
+			   struct ext_slave_descr *slave,
+			   struct ext_slave_platform_data *pdata,
+			   unsigned char *data)
+{
+	unsigned char stat;
+	int result = INV_SUCCESS;
+	short axis_fixed;
+
+	/* Read status reg. to check if data is ready */
+	result =
+	    inv_serial_read(mlsl_handle, pdata->address, LSM_REG_STATUS, 1,
+			    &stat);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	if (stat & 0x01) {
+		result =
+		    inv_serial_read(mlsl_handle, pdata->address,
+				    LSM_REG_X_M, 6, (unsigned char *)data);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+		/*drop data if overflows */
+		if ((data[0] == 0xf0) || (data[2] == 0xf0)
+		    || (data[4] == 0xf0)) {
+			/* trigger next measurement read */
+			result =
+			    inv_serial_single_write(mlsl_handle,
+						    pdata->address,
+						    LSM_REG_MODE,
+						    LSM_MODE_SINGLE);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			return INV_ERROR_COMPASS_DATA_OVERFLOW;
+		}
+		/* convert to fixed point and apply sensitivity correction for
+		   Z-axis */
+		axis_fixed =
+		    (short)((unsigned short)data[5] +
+			    (unsigned short)data[4] * 256);
+		/* scale up by 1.125 (36/32) approximate of 1.122 (320/285) */
+		if (slave->id == COMPASS_ID_LSM303DLM) {
+			/* NOTE/IMPORTANT:
+			   lsm303dlm compass axis definition doesn't
+			   respect the right hand rule. We invert
+			   the sign of the Z axis to fix that. */
+			axis_fixed = (short)(-1 * axis_fixed * 36);
+		} else {
+			axis_fixed = (short)(axis_fixed * 36);
+		}
+		data[4] = axis_fixed >> 8;
+		data[5] = axis_fixed & 0xFF;
+
+		axis_fixed =
+		    (short)((unsigned short)data[3] +
+			    (unsigned short)data[2] * 256);
+		axis_fixed = (short)(axis_fixed * 32);
+		data[2] = axis_fixed >> 8;
+		data[3] = axis_fixed & 0xFF;
+
+		axis_fixed =
+		    (short)((unsigned short)data[1] +
+			    (unsigned short)data[0] * 256);
+		axis_fixed = (short)(axis_fixed * 32);
+		data[0] = axis_fixed >> 8;
+		data[1] = axis_fixed & 0xFF;
+
+		/* trigger next measurement read */
+		result =
+		    inv_serial_single_write(mlsl_handle, pdata->address,
+					    LSM_REG_MODE, LSM_MODE_SINGLE);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+		return INV_SUCCESS;
+	} else {
+		/* trigger next measurement read */
+		result =
+		    inv_serial_single_write(mlsl_handle, pdata->address,
+					    LSM_REG_MODE, LSM_MODE_SINGLE);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+		return INV_ERROR_COMPASS_DATA_NOT_READY;
+	}
+}
+
+static struct ext_slave_descr lsm303dlx_m_descr = {
+	.init             = NULL,
+	.exit             = NULL,
+	.suspend          = lsm303dlx_m_suspend,
+	.resume           = lsm303dlx_m_resume,
+	.read             = lsm303dlx_m_read,
+	.config           = NULL,
+	.get_config       = NULL,
+	.name             = "lsm303dlx_m",
+	.type             = EXT_SLAVE_TYPE_COMPASS,
+	.id               = ID_INVALID,
+	.read_reg         = 0x06,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_BIG_ENDIAN,
+	.range            = {10240, 0},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *lsm303dlx_m_get_slave_descr(void)
+{
+	return &lsm303dlx_m_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct lsm303dlx_m_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static const struct i2c_device_id lsm303dlx_m_mod_id[] = {
+	{ "lsm303dlh", COMPASS_ID_LSM303DLH },
+	{ "lsm303dlm", COMPASS_ID_LSM303DLM },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, lsm303dlx_m_mod_id);
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int lsm303dlx_m_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct lsm303dlx_m_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+	lsm303dlx_m_descr.id = devid->driver_data;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					lsm303dlx_m_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int lsm303dlx_m_mod_remove(struct i2c_client *client)
+{
+	struct lsm303dlx_m_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				lsm303dlx_m_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static struct i2c_driver lsm303dlx_m_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = lsm303dlx_m_mod_probe,
+	.remove = lsm303dlx_m_mod_remove,
+	.id_table = lsm303dlx_m_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "lsm303dlx_m_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init lsm303dlx_m_mod_init(void)
+{
+	int res = i2c_add_driver(&lsm303dlx_m_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "lsm303dlx_m_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit lsm303dlx_m_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&lsm303dlx_m_mod_driver);
+}
+
+module_init(lsm303dlx_m_mod_init);
+module_exit(lsm303dlx_m_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate lsm303dlx_m sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("lsm303dlx_m_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/mmc314x.c b/drivers/misc/inv_mpu/compass/mmc314x.c
new file mode 100644
index 000000000000..786fadcc3e48
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/mmc314x.c
@@ -0,0 +1,313 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup COMPASSDL
+ *
+ *  @{
+ *      @file   mmc314x.c
+ *      @brief  Magnetometer setup and handling methods for the
+ *              MEMSIC MMC314x compass.
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+
+static int reset_int = 1000;
+static int read_count = 1;
+static char reset_mode;		/* in Z-init section */
+
+/* -------------------------------------------------------------------------- */
+#define MMC314X_REG_ST (0x00)
+#define MMC314X_REG_X_MSB (0x01)
+
+#define MMC314X_CNTL_MODE_WAKE_UP (0x01)
+#define MMC314X_CNTL_MODE_SET (0x02)
+#define MMC314X_CNTL_MODE_RESET (0x04)
+
+/* -------------------------------------------------------------------------- */
+
+static int mmc314x_suspend(void *mlsl_handle,
+			   struct ext_slave_descr *slave,
+			   struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	return result;
+}
+
+static int mmc314x_resume(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+
+	int result;
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    MMC314X_REG_ST, MMC314X_CNTL_MODE_RESET);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(10);
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    MMC314X_REG_ST, MMC314X_CNTL_MODE_SET);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(10);
+	read_count = 1;
+	return INV_SUCCESS;
+}
+
+static int mmc314x_read(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			unsigned char *data)
+{
+	int result, ii;
+	short tmp[3];
+	unsigned char tmpdata[6];
+
+	if (read_count > 1000)
+		read_count = 1;
+
+	result =
+	    inv_serial_read(mlsl_handle, pdata->address, MMC314X_REG_X_MSB,
+			    6, (unsigned char *)data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	for (ii = 0; ii < 6; ii++)
+		tmpdata[ii] = data[ii];
+
+	for (ii = 0; ii < 3; ii++) {
+		tmp[ii] = (short)((tmpdata[2 * ii] << 8) + tmpdata[2 * ii + 1]);
+		tmp[ii] = tmp[ii] - 4096;
+		tmp[ii] = tmp[ii] * 16;
+	}
+
+	for (ii = 0; ii < 3; ii++) {
+		data[2 * ii] = (unsigned char)(tmp[ii] >> 8);
+		data[2 * ii + 1] = (unsigned char)(tmp[ii]);
+	}
+
+	if (read_count % reset_int == 0) {
+		if (reset_mode) {
+			result =
+			    inv_serial_single_write(mlsl_handle,
+						    pdata->address,
+						    MMC314X_REG_ST,
+						    MMC314X_CNTL_MODE_RESET);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			reset_mode = 0;
+			return INV_ERROR_COMPASS_DATA_NOT_READY;
+		} else {
+			result =
+			    inv_serial_single_write(mlsl_handle,
+						    pdata->address,
+						    MMC314X_REG_ST,
+						    MMC314X_CNTL_MODE_SET);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			reset_mode = 1;
+			read_count++;
+			return INV_ERROR_COMPASS_DATA_NOT_READY;
+		}
+	}
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    MMC314X_REG_ST, MMC314X_CNTL_MODE_WAKE_UP);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	read_count++;
+
+	return INV_SUCCESS;
+}
+
+static struct ext_slave_descr mmc314x_descr = {
+	.init             = NULL,
+	.exit             = NULL,
+	.suspend          = mmc314x_suspend,
+	.resume           = mmc314x_resume,
+	.read             = mmc314x_read,
+	.config           = NULL,
+	.get_config       = NULL,
+	.name             = "mmc314x",
+	.type             = EXT_SLAVE_TYPE_COMPASS,
+	.id               = COMPASS_ID_MMC314X,
+	.read_reg         = 0x01,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_BIG_ENDIAN,
+	.range            = {400, 0},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *mmc314x_get_slave_descr(void)
+{
+	return &mmc314x_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct mmc314x_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int mmc314x_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct mmc314x_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					mmc314x_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int mmc314x_mod_remove(struct i2c_client *client)
+{
+	struct mmc314x_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				mmc314x_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id mmc314x_mod_id[] = {
+	{ "mmc314x", COMPASS_ID_MMC314X },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, mmc314x_mod_id);
+
+static struct i2c_driver mmc314x_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = mmc314x_mod_probe,
+	.remove = mmc314x_mod_remove,
+	.id_table = mmc314x_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "mmc314x_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init mmc314x_mod_init(void)
+{
+	int res = i2c_add_driver(&mmc314x_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "mmc314x_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit mmc314x_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&mmc314x_mod_driver);
+}
+
+module_init(mmc314x_mod_init);
+module_exit(mmc314x_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate MMC314X sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("mmc314x_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/yas529-kernel.c b/drivers/misc/inv_mpu/compass/yas529-kernel.c
new file mode 100644
index 000000000000..f53223fba641
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/yas529-kernel.c
@@ -0,0 +1,611 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <log.h>
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+/*----- YAMAHA YAS529 Registers ------*/
+enum YAS_REG {
+	YAS_REG_CMDR		= 0x00,	/* 000 < 5 */
+	YAS_REG_XOFFSETR	= 0x20,	/* 001 < 5 */
+	YAS_REG_Y1OFFSETR	= 0x40,	/* 010 < 5 */
+	YAS_REG_Y2OFFSETR	= 0x60,	/* 011 < 5 */
+	YAS_REG_ICOILR		= 0x80,	/* 100 < 5 */
+	YAS_REG_CAL		= 0xA0,	/* 101 < 5 */
+	YAS_REG_CONFR		= 0xC0,	/* 110 < 5 */
+	YAS_REG_DOUTR		= 0xE0	/* 111 < 5 */
+};
+
+/* -------------------------------------------------------------------------- */
+
+static long a1;
+static long a2;
+static long a3;
+static long a4;
+static long a5;
+static long a6;
+static long a7;
+static long a8;
+static long a9;
+
+/* -------------------------------------------------------------------------- */
+static int yas529_sensor_i2c_write(struct i2c_adapter *i2c_adap,
+				   unsigned char address,
+				   unsigned int len, unsigned char *data)
+{
+	struct i2c_msg msgs[1];
+	int res;
+
+	if (NULL == data || NULL == i2c_adap)
+		return -EINVAL;
+
+	msgs[0].addr = address;
+	msgs[0].flags = 0;	/* write */
+	msgs[0].buf = (unsigned char *)data;
+	msgs[0].len = len;
+
+	res = i2c_transfer(i2c_adap, msgs, 1);
+	if (res < 1)
+		return res;
+	else
+		return 0;
+}
+
+static int yas529_sensor_i2c_read(struct i2c_adapter *i2c_adap,
+				  unsigned char address,
+				  unsigned char reg,
+				  unsigned int len, unsigned char *data)
+{
+	struct i2c_msg msgs[2];
+	int res;
+
+	if (NULL == data || NULL == i2c_adap)
+		return -EINVAL;
+
+	msgs[0].addr = address;
+	msgs[0].flags = I2C_M_RD;
+	msgs[0].buf = data;
+	msgs[0].len = len;
+
+	res = i2c_transfer(i2c_adap, msgs, 1);
+	if (res < 1)
+		return res;
+	else
+		return 0;
+}
+
+static int yas529_suspend(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	return result;
+}
+
+static int yas529_resume(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	unsigned char dummyData[1] = { 0 };
+	unsigned char dummyRegister = 0;
+	unsigned char rawData[6];
+	unsigned char calData[9];
+
+	short xoffset, y1offset, y2offset;
+	short d2, d3, d4, d5, d6, d7, d8, d9;
+
+	/* YAS529 Application Manual MS-3C - Section 4.4.5 */
+	/* =============================================== */
+	/* Step 1 - register initialization */
+	/* zero initialization coil register - "100 00 000" */
+	dummyData[0] = YAS_REG_ICOILR | 0x00;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* zero config register - "110 00 000" */
+	dummyData[0] = YAS_REG_CONFR | 0x00;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Step 2 - initialization coil operation */
+	dummyData[0] = YAS_REG_ICOILR | 0x11;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_ICOILR | 0x01;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_ICOILR | 0x12;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_ICOILR | 0x02;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_ICOILR | 0x13;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_ICOILR | 0x03;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_ICOILR | 0x14;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_ICOILR | 0x04;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_ICOILR | 0x15;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_ICOILR | 0x05;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_ICOILR | 0x16;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_ICOILR | 0x06;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_ICOILR | 0x17;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_ICOILR | 0x07;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_ICOILR | 0x10;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_ICOILR | 0x00;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Step 3 - rough offset measurement */
+	/* Config register - Measurements results - "110 00 000" */
+	dummyData[0] = YAS_REG_CONFR | 0x00;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Measurements command register - Rough offset measurement -
+	   "000 00001" */
+	dummyData[0] = YAS_REG_CMDR | 0x01;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(2);		/* wait at least 1.5ms */
+
+	/* Measurement data read */
+	result =
+	    yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+				   dummyRegister, 6, rawData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	xoffset =
+	    (short)((unsigned short)rawData[5] +
+		    ((unsigned short)rawData[4] & 0x7) * 256) - 5;
+	if (xoffset < 0)
+		xoffset = 0;
+	y1offset =
+	    (short)((unsigned short)rawData[3] +
+		    ((unsigned short)rawData[2] & 0x7) * 256) - 5;
+	if (y1offset < 0)
+		y1offset = 0;
+	y2offset =
+	    (short)((unsigned short)rawData[1] +
+		    ((unsigned short)rawData[0] & 0x7) * 256) - 5;
+	if (y2offset < 0)
+		y2offset = 0;
+
+	/* Step 4 - rough offset setting */
+	/* Set rough offset register values */
+	dummyData[0] = YAS_REG_XOFFSETR | xoffset;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_Y1OFFSETR | y1offset;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	dummyData[0] = YAS_REG_Y2OFFSETR | y2offset;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* CAL matrix read (first read is invalid) */
+	/* Config register - CAL register read - "110 01 000" */
+	dummyData[0] = YAS_REG_CONFR | 0x08;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* CAL data read */
+	result =
+	    yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+				   dummyRegister, 9, calData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Config register - CAL register read - "110 01 000" */
+	dummyData[0] = YAS_REG_CONFR | 0x08;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* CAL data read */
+	result =
+	    yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+				   dummyRegister, 9, calData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Calculate coefficients of the sensitivity correction matrix */
+	a1 = 100;
+	d2 = (calData[0] & 0xFC) >> 2;	/* [71..66] 6bit */
+	a2 = (short)(d2 - 32);
+	/* [65..62] 4bit */
+	d3 = ((calData[0] & 0x03) << 2) | ((calData[1] & 0xC0) >> 6);
+	a3 = (short)(d3 - 8);
+	d4 = (calData[1] & 0x3F);	/* [61..56] 6bit */
+	a4 = (short)(d4 - 32);
+	d5 = (calData[2] & 0xFC) >> 2;	/* [55..50] 6bit */
+	a5 = (short)(d5 - 32) + 70;
+	/* [49..44] 6bit */
+	d6 = ((calData[2] & 0x03) << 4) | ((calData[3] & 0xF0) >> 4);
+	a6 = (short)(d6 - 32);
+	/* [43..38] 6bit */
+	d7 = ((calData[3] & 0x0F) << 2) | ((calData[4] & 0xC0) >> 6);
+	a7 = (short)(d7 - 32);
+	d8 = (calData[4] & 0x3F);	/* [37..32] 6bit */
+	a8 = (short)(d8 - 32);
+	d9 = (calData[5] & 0xFE) >> 1;	/* [31..25] 7bit */
+	a9 = (short)(d9 - 64) + 130;
+
+	return result;
+}
+
+static int yas529_read(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data)
+{
+	unsigned char stat;
+	unsigned char rawData[6];
+	unsigned char dummyData[1] = { 0 };
+	unsigned char dummyRegister = 0;
+	int result = INV_SUCCESS;
+	short SX, SY1, SY2, SY, SZ;
+	short row1fixed, row2fixed, row3fixed;
+
+	/* Config register - Measurements results - "110 00 000" */
+	dummyData[0] = YAS_REG_CONFR | 0x00;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Measurements command register - Normal magnetic field measurement -
+	   "000 00000" */
+	dummyData[0] = YAS_REG_CMDR | 0x00;
+	result =
+	    yas529_sensor_i2c_write(mlsl_handle, pdata->address, 1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	msleep(10);
+	/* Measurement data read */
+	result =
+	    yas529_sensor_i2c_read(mlsl_handle, pdata->address,
+				   dummyRegister, 6, (unsigned char *)&rawData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	stat = rawData[0] & 0x80;
+	if (stat == 0x00) {
+		/* Extract raw data */
+		SX = (short)((unsigned short)rawData[5] +
+			     ((unsigned short)rawData[4] & 0x7) * 256);
+		SY1 =
+		    (short)((unsigned short)rawData[3] +
+			    ((unsigned short)rawData[2] & 0x7) * 256);
+		SY2 =
+		    (short)((unsigned short)rawData[1] +
+			    ((unsigned short)rawData[0] & 0x7) * 256);
+		if ((SX <= 1) || (SY1 <= 1) || (SY2 <= 1))
+			return INV_ERROR_COMPASS_DATA_UNDERFLOW;
+		if ((SX >= 1024) || (SY1 >= 1024) || (SY2 >= 1024))
+			return INV_ERROR_COMPASS_DATA_OVERFLOW;
+		/* Convert to XYZ axis */
+		SX = -1 * SX;
+		SY = SY2 - SY1;
+		SZ = SY1 + SY2;
+
+		/* Apply sensitivity correction matrix */
+		row1fixed = (short)((a1 * SX + a2 * SY + a3 * SZ) >> 7) * 41;
+		row2fixed = (short)((a4 * SX + a5 * SY + a6 * SZ) >> 7) * 41;
+		row3fixed = (short)((a7 * SX + a8 * SY + a9 * SZ) >> 7) * 41;
+
+		data[0] = row1fixed >> 8;
+		data[1] = row1fixed & 0xFF;
+		data[2] = row2fixed >> 8;
+		data[3] = row2fixed & 0xFF;
+		data[4] = row3fixed >> 8;
+		data[5] = row3fixed & 0xFF;
+
+		return INV_SUCCESS;
+	} else {
+		return INV_ERROR_COMPASS_DATA_NOT_READY;
+	}
+}
+
+static struct ext_slave_descr yas529_descr = {
+	.init             = NULL,
+	.exit             = NULL,
+	.suspend          = yas529_suspend,
+	.resume           = yas529_resume,
+	.read             = yas529_read,
+	.config           = NULL,
+	.get_config       = NULL,
+	.name             = "yas529",
+	.type             = EXT_SLAVE_TYPE_COMPASS,
+	.id               = COMPASS_ID_YAS529,
+	.read_reg         = 0x06,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_BIG_ENDIAN,
+	.range            = {19660, 8000},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *yas529_get_slave_descr(void)
+{
+	return &yas529_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct yas529_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int yas529_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct yas529_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					yas529_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int yas529_mod_remove(struct i2c_client *client)
+{
+	struct yas529_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				yas529_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id yas529_mod_id[] = {
+	{ "yas529", COMPASS_ID_YAS529 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, yas529_mod_id);
+
+static struct i2c_driver yas529_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = yas529_mod_probe,
+	.remove = yas529_mod_remove,
+	.id_table = yas529_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "yas529_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init yas529_mod_init(void)
+{
+	int res = i2c_add_driver(&yas529_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "yas529_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit yas529_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&yas529_mod_driver);
+}
+
+module_init(yas529_mod_init);
+module_exit(yas529_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate YAS529 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("yas529_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/compass/yas530.c b/drivers/misc/inv_mpu/compass/yas530.c
new file mode 100644
index 000000000000..fdca05ba8e5c
--- /dev/null
+++ b/drivers/misc/inv_mpu/compass/yas530.c
@@ -0,0 +1,580 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+  *  @addtogroup COMPASSDL
+  *
+  *  @{
+  *      @file   yas530.c
+  *      @brief  Magnetometer setup and handling methods for Yamaha YAS530
+  *              compass when used in a user-space solution (no kernel driver).
+  */
+
+/* -------------------------------------------------------------------------- */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include "log.h"
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+/* -------------------------------------------------------------------------- */
+#define YAS530_REGADDR_DEVICE_ID          (0x80)
+#define YAS530_REGADDR_ACTUATE_INIT_COIL  (0x81)
+#define YAS530_REGADDR_MEASURE_COMMAND    (0x82)
+#define YAS530_REGADDR_CONFIG             (0x83)
+#define YAS530_REGADDR_MEASURE_INTERVAL   (0x84)
+#define YAS530_REGADDR_OFFSET_X           (0x85)
+#define YAS530_REGADDR_OFFSET_Y1          (0x86)
+#define YAS530_REGADDR_OFFSET_Y2          (0x87)
+#define YAS530_REGADDR_TEST1              (0x88)
+#define YAS530_REGADDR_TEST2              (0x89)
+#define YAS530_REGADDR_CAL                (0x90)
+#define YAS530_REGADDR_MEASURE_DATA       (0xb0)
+
+/* -------------------------------------------------------------------------- */
+static int Cx, Cy1, Cy2;
+static int /*a1, */ a2, a3, a4, a5, a6, a7, a8, a9;
+static int k;
+
+static unsigned char dx, dy1, dy2;
+static unsigned char d2, d3, d4, d5, d6, d7, d8, d9, d0;
+static unsigned char dck;
+
+/* -------------------------------------------------------------------------- */
+
+static int set_hardware_offset(void *mlsl_handle,
+			       struct ext_slave_descr *slave,
+			       struct ext_slave_platform_data *pdata,
+			       char offset_x, char offset_y1, char offset_y2)
+{
+	char data;
+	int result = INV_SUCCESS;
+
+	data = offset_x & 0x3f;
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 YAS530_REGADDR_OFFSET_X, data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	data = offset_y1 & 0x3f;
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 YAS530_REGADDR_OFFSET_Y1, data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	data = offset_y2 & 0x3f;
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 YAS530_REGADDR_OFFSET_Y2, data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+static int set_measure_command(void *mlsl_handle,
+			       struct ext_slave_descr *slave,
+			       struct ext_slave_platform_data *pdata,
+			       int ldtc, int fors, int dlymes)
+{
+	int result = INV_SUCCESS;
+
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 YAS530_REGADDR_MEASURE_COMMAND, 0x01);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+static int measure_normal(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata,
+			  int *busy, unsigned short *t,
+			  unsigned short *x, unsigned short *y1,
+			  unsigned short *y2)
+{
+	unsigned char data[8];
+	unsigned short b, to, xo, y1o, y2o;
+	int result;
+	ktime_t sleeptime;
+	result = set_measure_command(mlsl_handle, slave, pdata, 0, 0, 0);
+	sleeptime = ktime_set(0, 2 * NSEC_PER_MSEC);
+	set_current_state(TASK_UNINTERRUPTIBLE);
+	schedule_hrtimeout(&sleeptime, HRTIMER_MODE_REL);
+
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 YAS530_REGADDR_MEASURE_DATA, 8, data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	b = (data[0] >> 7) & 0x01;
+	to = ((data[0] << 2) & 0x1fc) | ((data[1] >> 6) & 0x03);
+	xo = ((data[2] << 5) & 0xfe0) | ((data[3] >> 3) & 0x1f);
+	y1o = ((data[4] << 5) & 0xfe0) | ((data[5] >> 3) & 0x1f);
+	y2o = ((data[6] << 5) & 0xfe0) | ((data[7] >> 3) & 0x1f);
+
+	*busy = b;
+	*t = to;
+	*x = xo;
+	*y1 = y1o;
+	*y2 = y2o;
+
+	return result;
+}
+
+static int check_offset(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			char offset_x, char offset_y1, char offset_y2,
+			int *flag_x, int *flag_y1, int *flag_y2)
+{
+	int result;
+	int busy;
+	short t, x, y1, y2;
+
+	result = set_hardware_offset(mlsl_handle, slave, pdata,
+				     offset_x, offset_y1, offset_y2);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = measure_normal(mlsl_handle, slave, pdata,
+				&busy, &t, &x, &y1, &y2);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	*flag_x = 0;
+	*flag_y1 = 0;
+	*flag_y2 = 0;
+
+	if (x > 2048)
+		*flag_x = 1;
+	if (y1 > 2048)
+		*flag_y1 = 1;
+	if (y2 > 2048)
+		*flag_y2 = 1;
+	if (x < 2048)
+		*flag_x = -1;
+	if (y1 < 2048)
+		*flag_y1 = -1;
+	if (y2 < 2048)
+		*flag_y2 = -1;
+
+	return result;
+}
+
+static int measure_and_set_offset(void *mlsl_handle,
+				  struct ext_slave_descr *slave,
+				  struct ext_slave_platform_data *pdata,
+				  char *offset)
+{
+	int i;
+	int result = INV_SUCCESS;
+	char offset_x = 0, offset_y1 = 0, offset_y2 = 0;
+	int flag_x = 0, flag_y1 = 0, flag_y2 = 0;
+	static const int correct[5] = { 16, 8, 4, 2, 1 };
+
+	for (i = 0; i < 5; i++) {
+		result = check_offset(mlsl_handle, slave, pdata,
+				      offset_x, offset_y1, offset_y2,
+				      &flag_x, &flag_y1, &flag_y2);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+		if (flag_x)
+			offset_x += flag_x * correct[i];
+		if (flag_y1)
+			offset_y1 += flag_y1 * correct[i];
+		if (flag_y2)
+			offset_y2 += flag_y2 * correct[i];
+	}
+
+	result = set_hardware_offset(mlsl_handle, slave, pdata,
+				     offset_x, offset_y1, offset_y2);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	offset[0] = offset_x;
+	offset[1] = offset_y1;
+	offset[2] = offset_y2;
+
+	return result;
+}
+
+static void coordinate_conversion(short x, short y1, short y2, short t,
+				  int32_t *xo, int32_t *yo, int32_t *zo)
+{
+	int32_t sx, sy1, sy2, sy, sz;
+	int32_t hx, hy, hz;
+
+	sx = x - (Cx * t) / 100;
+	sy1 = y1 - (Cy1 * t) / 100;
+	sy2 = y2 - (Cy2 * t) / 100;
+
+	sy = sy1 - sy2;
+	sz = -sy1 - sy2;
+
+	hx = k * ((100 * sx + a2 * sy + a3 * sz) / 10);
+	hy = k * ((a4 * sx + a5 * sy + a6 * sz) / 10);
+	hz = k * ((a7 * sx + a8 * sy + a9 * sz) / 10);
+
+	*xo = hx;
+	*yo = hy;
+	*zo = hz;
+}
+
+static int yas530_suspend(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	return result;
+}
+
+static int yas530_resume(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+
+	unsigned char dummyData = 0x00;
+	char offset[3] = { 0, 0, 0 };
+	unsigned char data[16];
+	unsigned char read_reg[1];
+
+	/* =============================================== */
+
+	/* Step 1 - Test register initialization */
+	dummyData = 0x00;
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 YAS530_REGADDR_TEST1, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result =
+	    inv_serial_single_write(mlsl_handle, pdata->address,
+				    YAS530_REGADDR_TEST2, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Device ID read  */
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 YAS530_REGADDR_DEVICE_ID, 1, read_reg);
+
+	/*Step 2 Read the CAL register */
+	/* CAL data read */
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 YAS530_REGADDR_CAL, 16, data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* CAL data Second Read */
+	result = inv_serial_read(mlsl_handle, pdata->address,
+				 YAS530_REGADDR_CAL, 16, data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/*Cal data */
+	dx = data[0];
+	dy1 = data[1];
+	dy2 = data[2];
+	d2 = (data[3] >> 2) & 0x03f;
+	d3 = ((data[3] << 2) & 0x0c) | ((data[4] >> 6) & 0x03);
+	d4 = data[4] & 0x3f;
+	d5 = (data[5] >> 2) & 0x3f;
+	d6 = ((data[5] << 4) & 0x30) | ((data[6] >> 4) & 0x0f);
+	d7 = ((data[6] << 3) & 0x78) | ((data[7] >> 5) & 0x07);
+	d8 = ((data[7] << 1) & 0x3e) | ((data[8] >> 7) & 0x01);
+	d9 = ((data[8] << 1) & 0xfe) | ((data[9] >> 7) & 0x01);
+	d0 = (data[9] >> 2) & 0x1f;
+	dck = ((data[9] << 1) & 0x06) | ((data[10] >> 7) & 0x01);
+
+	/*Correction Data */
+	Cx = (int)dx * 6 - 768;
+	Cy1 = (int)dy1 * 6 - 768;
+	Cy2 = (int)dy2 * 6 - 768;
+	a2 = (int)d2 - 32;
+	a3 = (int)d3 - 8;
+	a4 = (int)d4 - 32;
+	a5 = (int)d5 + 38;
+	a6 = (int)d6 - 32;
+	a7 = (int)d7 - 64;
+	a8 = (int)d8 - 32;
+	a9 = (int)d9;
+	k = (int)d0 + 10;
+
+	/*Obtain the [49:47] bits */
+	dck &= 0x07;
+
+	/*Step 3 : Storing the CONFIG with the CLK value */
+	dummyData = 0x00 | (dck << 2);
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 YAS530_REGADDR_CONFIG, dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/*Step 4 : Set Acquisition Interval Register */
+	dummyData = 0x00;
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 YAS530_REGADDR_MEASURE_INTERVAL,
+					 dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/*Step 5 : Reset Coil */
+	dummyData = 0x00;
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+					 YAS530_REGADDR_ACTUATE_INIT_COIL,
+					 dummyData);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Offset Measurement and Set */
+	result = measure_and_set_offset(mlsl_handle, slave, pdata, offset);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+static int yas530_read(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data)
+{
+	int result = INV_SUCCESS;
+
+	int busy;
+	short t, x, y1, y2;
+	int32_t xyz[3];
+	short rawfixed[3];
+
+	result = measure_normal(mlsl_handle, slave, pdata,
+				&busy, &t, &x, &y1, &y2);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	coordinate_conversion(x, y1, y2, t, &xyz[0], &xyz[1], &xyz[2]);
+
+	rawfixed[0] = (short)(xyz[0] / 100);
+	rawfixed[1] = (short)(xyz[1] / 100);
+	rawfixed[2] = (short)(xyz[2] / 100);
+
+	data[0] = rawfixed[0] >> 8;
+	data[1] = rawfixed[0] & 0xFF;
+	data[2] = rawfixed[1] >> 8;
+	data[3] = rawfixed[1] & 0xFF;
+	data[4] = rawfixed[2] >> 8;
+	data[5] = rawfixed[2] & 0xFF;
+
+	if (busy)
+		return INV_ERROR_COMPASS_DATA_NOT_READY;
+	return result;
+}
+
+static struct ext_slave_descr yas530_descr = {
+	.init             = NULL,
+	.exit             = NULL,
+	.suspend          = yas530_suspend,
+	.resume           = yas530_resume,
+	.read             = yas530_read,
+	.config           = NULL,
+	.get_config       = NULL,
+	.name             = "yas530",
+	.type             = EXT_SLAVE_TYPE_COMPASS,
+	.id               = COMPASS_ID_YAS530,
+	.read_reg         = 0x06,
+	.read_len         = 6,
+	.endian           = EXT_SLAVE_BIG_ENDIAN,
+	.range            = {3276, 8001},
+	.trigger          = NULL,
+};
+
+static
+struct ext_slave_descr *yas530_get_slave_descr(void)
+{
+	return &yas530_descr;
+}
+
+/* -------------------------------------------------------------------------- */
+struct yas530_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int yas530_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct yas530_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					yas530_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int yas530_mod_remove(struct i2c_client *client)
+{
+	struct yas530_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				yas530_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id yas530_mod_id[] = {
+	{ "yas530", COMPASS_ID_YAS530 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, yas530_mod_id);
+
+static struct i2c_driver yas530_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = yas530_mod_probe,
+	.remove = yas530_mod_remove,
+	.id_table = yas530_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "yas530_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init yas530_mod_init(void)
+{
+	int res = i2c_add_driver(&yas530_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "yas530_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit yas530_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&yas530_mod_driver);
+}
+
+module_init(yas530_mod_init);
+module_exit(yas530_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate YAS530 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("yas530_mod");
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/log.h b/drivers/misc/inv_mpu/log.h
new file mode 100644
index 000000000000..5630602e3efa
--- /dev/null
+++ b/drivers/misc/inv_mpu/log.h
@@ -0,0 +1,287 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/*
+ * This file incorporates work covered by the following copyright and
+ * permission notice:
+ *
+ * Copyright (C) 2005 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ * C/C++ logging functions.  See the logging documentation for API details.
+ *
+ * We'd like these to be available from C code (in case we import some from
+ * somewhere), so this has a C interface.
+ *
+ * The output will be correct when the log file is shared between multiple
+ * threads and/or multiple processes so long as the operating system
+ * supports O_APPEND.  These calls have mutex-protected data structures
+ * and so are NOT reentrant.  Do not use MPL_LOG in a signal handler.
+ */
+#ifndef _LIBS_CUTILS_MPL_LOG_H
+#define _LIBS_CUTILS_MPL_LOG_H
+
+#include "mltypes.h"
+#include <stdarg.h>
+
+
+#include <linux/kernel.h>
+
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Normally we strip MPL_LOGV (VERBOSE messages) from release builds.
+ * You can modify this (for example with "#define MPL_LOG_NDEBUG 0"
+ * at the top of your source file) to change that behavior.
+ */
+#ifndef MPL_LOG_NDEBUG
+#ifdef NDEBUG
+#define MPL_LOG_NDEBUG 1
+#else
+#define MPL_LOG_NDEBUG 0
+#endif
+#endif
+
+#define MPL_LOG_UNKNOWN MPL_LOG_VERBOSE
+#define MPL_LOG_DEFAULT KERN_DEFAULT
+#define MPL_LOG_VERBOSE KERN_CONT
+#define MPL_LOG_DEBUG   KERN_NOTICE
+#define MPL_LOG_INFO    KERN_INFO
+#define MPL_LOG_WARN    KERN_WARNING
+#define MPL_LOG_ERROR   KERN_ERR
+#define MPL_LOG_SILENT  MPL_LOG_VERBOSE
+
+
+
+/*
+ * This is the local tag used for the following simplified
+ * logging macros.  You can change this preprocessor definition
+ * before using the other macros to change the tag.
+ */
+#ifndef MPL_LOG_TAG
+#define MPL_LOG_TAG
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Simplified macro to send a verbose log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGV
+#if MPL_LOG_NDEBUG
+#define MPL_LOGV(fmt, ...)						\
+	do {								\
+		if (0)							\
+			MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
+	} while (0)
+#else
+#define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef CONDITION
+#define CONDITION(cond)     ((cond) != 0)
+#endif
+
+#ifndef MPL_LOGV_IF
+#if MPL_LOG_NDEBUG
+#define MPL_LOGV_IF(cond, fmt, ...)  \
+	do { if (0) MPL_LOG(fmt, ##__VA_ARGS__); } while (0)
+#else
+#define MPL_LOGV_IF(cond, fmt, ...) \
+	((CONDITION(cond))						\
+		? MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
+		: (void)0)
+#endif
+#endif
+
+/*
+ * Simplified macro to send a debug log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGD
+#define MPL_LOGD(fmt, ...) MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGD_IF
+#define MPL_LOGD_IF(cond, fmt, ...) \
+	((CONDITION(cond))					       \
+		? MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)  \
+		: (void)0)
+#endif
+
+/*
+ * Simplified macro to send an info log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGI
+#define MPL_LOGI(fmt, ...) pr_info(KERN_INFO MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGI_IF
+#define MPL_LOGI_IF(cond, fmt, ...) \
+	((CONDITION(cond))                                              \
+		? MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)   \
+		: (void)0)
+#endif
+
+/*
+ * Simplified macro to send a warning log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGW
+#define MPL_LOGW(fmt, ...) printk(KERN_WARNING MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGW_IF
+#define MPL_LOGW_IF(cond, fmt, ...) \
+	((CONDITION(cond))					       \
+		? MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__)   \
+		: (void)0)
+#endif
+
+/*
+ * Simplified macro to send an error log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGE
+#define MPL_LOGE(fmt, ...) printk(KERN_ERR MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGE_IF
+#define MPL_LOGE_IF(cond, fmt, ...) \
+	((CONDITION(cond))					       \
+		? MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__)  \
+		: (void)0)
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Log a fatal error.  If the given condition fails, this stops program
+ * execution like a normal assertion, but also generating the given message.
+ * It is NOT stripped from release builds.  Note that the condition test
+ * is -inverted- from the normal assert() semantics.
+ */
+#define MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ...) \
+	((CONDITION(cond))					   \
+		? ((void)android_printAssert(#cond, MPL_LOG_TAG,   \
+						fmt, ##__VA_ARGS__))	\
+		: (void)0)
+
+#define MPL_LOG_ALWAYS_FATAL(fmt, ...) \
+	(((void)android_printAssert(NULL, MPL_LOG_TAG, fmt, ##__VA_ARGS__)))
+
+/*
+ * Versions of MPL_LOG_ALWAYS_FATAL_IF and MPL_LOG_ALWAYS_FATAL that
+ * are stripped out of release builds.
+ */
+#if MPL_LOG_NDEBUG
+#define MPL_LOG_FATAL_IF(cond, fmt, ...)				\
+	do {								\
+		if (0)							\
+			MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__); \
+	} while (0)
+#define MPL_LOG_FATAL(fmt, ...)						\
+	do {								\
+		if (0)							\
+			MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)	\
+	} while (0)
+#else
+#define MPL_LOG_FATAL_IF(cond, fmt, ...) \
+	MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__)
+#define MPL_LOG_FATAL(fmt, ...) \
+	MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Assertion that generates a log message when the assertion fails.
+ * Stripped out of release builds.  Uses the current MPL_LOG_TAG.
+ */
+#define MPL_LOG_ASSERT(cond, fmt, ...)			\
+	MPL_LOG_FATAL_IF(!(cond), fmt, ##__VA_ARGS__)
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Basic log message macro.
+ *
+ * Example:
+ *  MPL_LOG(MPL_LOG_WARN, NULL, "Failed with error %d", errno);
+ *
+ * The second argument may be NULL or "" to indicate the "global" tag.
+ */
+#ifndef MPL_LOG
+#define MPL_LOG(priority, tag, fmt, ...)		\
+	MPL_LOG_PRI(priority, tag, fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Log macro that allows you to specify a number for the priority.
+ */
+#ifndef MPL_LOG_PRI
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+	pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Log macro that allows you to pass in a varargs ("args" is a va_list).
+ */
+#ifndef MPL_LOG_PRI_VA
+/* not allowed in the Kernel because there is no dev_dbg that takes a va_list */
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * ===========================================================================
+ *
+ * The stuff in the rest of this file should not be used directly.
+ */
+
+int _MLPrintLog(int priority, const char *tag, const char *fmt,	...);
+int _MLPrintVaLog(int priority, const char *tag, const char *fmt, va_list args);
+/* Final implementation of actual writing to a character device */
+int _MLWriteLog(const char *buf, int buflen);
+
+static inline void __print_result_location(int result,
+					   const char *file,
+					   const char *func, int line)
+{
+	MPL_LOGE("%s|%s|%d returning %d\n", file, func, line, result);
+}
+
+#define LOG_RESULT_LOCATION(condition) \
+	do {								\
+		__print_result_location((int)(condition), __FILE__,	\
+					__func__, __LINE__);		\
+	} while (0)
+
+
+#endif				/* _LIBS_CUTILS_MPL_LOG_H */
diff --git a/drivers/misc/inv_mpu/mldl_cfg.h b/drivers/misc/inv_mpu/mldl_cfg.h
new file mode 100644
index 000000000000..2b81b89179d5
--- /dev/null
+++ b/drivers/misc/inv_mpu/mldl_cfg.h
@@ -0,0 +1,384 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup MLDL
+ *
+ *  @{
+ *      @file   mldl_cfg.h
+ *      @brief  The Motion Library Driver Layer Configuration header file.
+ */
+
+#ifndef __MLDL_CFG_H__
+#define __MLDL_CFG_H__
+
+#include "mltypes.h"
+#include "mlsl.h"
+#include <linux/mpu.h>
+#ifdef MPU_CURRENT_BUILD_MPU6050B1
+#include "mpu6050b1.h"
+#elif defined(MPU_CURRENT_BUILD_MPU3050)
+#include "mpu3050.h"
+#endif
+
+#include "log.h"
+
+/*************************************************************************
+ *  Sensors Bit definitions
+ *************************************************************************/
+
+#define INV_X_GYRO			(0x0001)
+#define INV_Y_GYRO			(0x0002)
+#define INV_Z_GYRO			(0x0004)
+#define INV_DMP_PROCESSOR		(0x0008)
+
+#define INV_X_ACCEL			(0x0010)
+#define INV_Y_ACCEL			(0x0020)
+#define INV_Z_ACCEL			(0x0040)
+
+#define INV_X_COMPASS			(0x0080)
+#define INV_Y_COMPASS			(0x0100)
+#define INV_Z_COMPASS			(0x0200)
+
+#define INV_X_PRESSURE			(0x0300)
+#define INV_Y_PRESSURE			(0x0800)
+#define INV_Z_PRESSURE			(0x1000)
+
+#define INV_TEMPERATURE			(0x2000)
+#define INV_TIME			(0x4000)
+
+#define INV_THREE_AXIS_GYRO		(0x000F)
+#define INV_THREE_AXIS_ACCEL		(0x0070)
+#define INV_THREE_AXIS_COMPASS		(0x0380)
+#define INV_THREE_AXIS_PRESSURE		(0x1C00)
+
+#define INV_FIVE_AXIS			(0x007B)
+#define INV_SIX_AXIS_GYRO_ACCEL		(0x007F)
+#define INV_SIX_AXIS_ACCEL_COMPASS	(0x03F0)
+#define INV_NINE_AXIS			(0x03FF)
+#define INV_ALL_SENSORS			(0x7FFF)
+
+#define MPL_PROD_KEY(ver, rev) (ver * 100 + rev)
+
+/* -------------------------------------------------------------------------- */
+struct mpu_ram {
+	__u16 length;
+	__u8 *ram;
+};
+
+struct mpu_gyro_cfg {
+	__u8 int_config;
+	__u8 ext_sync;
+	__u8 full_scale;
+	__u8 lpf;
+	__u8 clk_src;
+	__u8 divider;
+	__u8 dmp_enable;
+	__u8 fifo_enable;
+	__u8 dmp_cfg1;
+	__u8 dmp_cfg2;
+};
+
+/* Offset registers that can be calibrated */
+struct mpu_offsets {
+	__u8	tc[GYRO_NUM_AXES];
+	__u16	gyro[GYRO_NUM_AXES];
+};
+
+/* Chip related information that can be read and verified */
+struct mpu_chip_info {
+	__u8 addr;
+	__u8 product_revision;
+	__u8 silicon_revision;
+	__u8 product_id;
+	__u16 gyro_sens_trim;
+	/* Only used for MPU6050 */
+	__u16 accel_sens_trim;
+};
+
+
+struct inv_mpu_cfg {
+	__u32 requested_sensors;
+	__u8 ignore_system_suspend;
+};
+
+/* Driver related state information */
+struct inv_mpu_state {
+#define MPU_GYRO_IS_SUSPENDED		(0x01 << EXT_SLAVE_TYPE_GYROSCOPE)
+#define MPU_ACCEL_IS_SUSPENDED		(0x01 << EXT_SLAVE_TYPE_ACCEL)
+#define MPU_COMPASS_IS_SUSPENDED	(0x01 << EXT_SLAVE_TYPE_COMPASS)
+#define MPU_PRESSURE_IS_SUSPENDED	(0x01 << EXT_SLAVE_TYPE_PRESSURE)
+#define MPU_GYRO_IS_BYPASSED		(0x10)
+#define MPU_DMP_IS_SUSPENDED		(0x20)
+#define MPU_GYRO_NEEDS_CONFIG		(0x40)
+#define MPU_DEVICE_IS_SUSPENDED		(0x80)
+	__u8 status;
+	/* 0-1 for 3050, bitfield of BIT_SLVx_DLY_EN, x = [0..4] */
+	__u8 i2c_slaves_enabled;
+};
+
+/* Platform data for the MPU */
+struct mldl_cfg {
+	struct mpu_ram			*mpu_ram;
+	struct mpu_gyro_cfg		*mpu_gyro_cfg;
+	struct mpu_offsets		*mpu_offsets;
+	struct mpu_chip_info		*mpu_chip_info;
+
+	/* MPU Related stored status and info */
+	struct inv_mpu_cfg		*inv_mpu_cfg;
+	struct inv_mpu_state		*inv_mpu_state;
+
+	/* Slave related information */
+	struct ext_slave_descr		*slave[EXT_SLAVE_NUM_TYPES];
+	/* Platform Data */
+	struct mpu_platform_data	*pdata;
+	struct ext_slave_platform_data	*pdata_slave[EXT_SLAVE_NUM_TYPES];
+};
+
+/* -------------------------------------------------------------------------- */
+
+int inv_mpu_open(struct mldl_cfg *mldl_cfg,
+		 void *mlsl_handle,
+		 void *accel_handle,
+		 void *compass_handle,
+		 void *pressure_handle);
+int inv_mpu_close(struct mldl_cfg *mldl_cfg,
+		  void *mlsl_handle,
+		  void *accel_handle,
+		  void *compass_handle,
+		  void *pressure_handle);
+int inv_mpu_resume(struct mldl_cfg *mldl_cfg,
+		   void *gyro_handle,
+		   void *accel_handle,
+		   void *compass_handle,
+		   void *pressure_handle,
+		   unsigned long sensors);
+int inv_mpu_suspend(struct mldl_cfg *mldl_cfg,
+		    void *gyro_handle,
+		    void *accel_handle,
+		    void *compass_handle,
+		    void *pressure_handle,
+		    unsigned long sensors);
+int inv_mpu_set_firmware(struct mldl_cfg *mldl_cfg,
+			 void *mlsl_handle,
+			 const unsigned char *data,
+			 int size);
+
+/* -------------------------------------------------------------------------- */
+/* Slave Read functions */
+int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg,
+		       void *gyro_handle,
+		       void *slave_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data);
+static inline int inv_mpu_read_accel(struct mldl_cfg *mldl_cfg,
+				     void *gyro_handle,
+				     void *accel_handle, unsigned char *data)
+{
+	if (!mldl_cfg) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_slave_read(
+		mldl_cfg, gyro_handle, accel_handle,
+		mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL],
+		mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+		data);
+}
+
+static inline int inv_mpu_read_compass(struct mldl_cfg *mldl_cfg,
+				       void *gyro_handle,
+				       void *compass_handle,
+				       unsigned char *data)
+{
+	if (!mldl_cfg) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_slave_read(
+		mldl_cfg, gyro_handle, compass_handle,
+		mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS],
+		mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS],
+		data);
+}
+
+static inline int inv_mpu_read_pressure(struct mldl_cfg *mldl_cfg,
+					void *gyro_handle,
+					void *pressure_handle,
+					unsigned char *data)
+{
+	if (!mldl_cfg) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_slave_read(
+		mldl_cfg, gyro_handle, pressure_handle,
+		mldl_cfg->slave[EXT_SLAVE_TYPE_PRESSURE],
+		mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
+		data);
+}
+
+int gyro_config(void *mlsl_handle,
+		struct mldl_cfg *mldl_cfg,
+		struct ext_slave_config *data);
+
+/* Slave Config functions */
+int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg,
+			 void *gyro_handle,
+			 void *slave_handle,
+			 struct ext_slave_config *data,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata);
+static inline int inv_mpu_config_accel(struct mldl_cfg *mldl_cfg,
+				       void *gyro_handle,
+				       void *accel_handle,
+				       struct ext_slave_config *data)
+{
+	if (!mldl_cfg) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_slave_config(
+		mldl_cfg, gyro_handle, accel_handle, data,
+		mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL],
+		mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL]);
+}
+
+static inline int inv_mpu_config_compass(struct mldl_cfg *mldl_cfg,
+					 void *gyro_handle,
+					 void *compass_handle,
+					 struct ext_slave_config *data)
+{
+	if (!mldl_cfg) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_slave_config(
+		mldl_cfg, gyro_handle, compass_handle, data,
+		mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS],
+		mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]);
+}
+
+static inline int inv_mpu_config_pressure(struct mldl_cfg *mldl_cfg,
+					  void *gyro_handle,
+					  void *pressure_handle,
+					  struct ext_slave_config *data)
+{
+	if (!mldl_cfg) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_slave_config(
+		mldl_cfg, gyro_handle, pressure_handle, data,
+		mldl_cfg->slave[EXT_SLAVE_TYPE_PRESSURE],
+		mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_PRESSURE]);
+}
+
+int gyro_get_config(void *mlsl_handle,
+		struct mldl_cfg *mldl_cfg,
+		struct ext_slave_config *data);
+
+/* Slave get config functions */
+int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg,
+			     void *gyro_handle,
+			     void *slave_handle,
+			     struct ext_slave_config *data,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata);
+
+static inline int inv_mpu_get_accel_config(struct mldl_cfg *mldl_cfg,
+					   void *gyro_handle,
+					   void *accel_handle,
+					   struct ext_slave_config *data)
+{
+	if (!mldl_cfg) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_get_slave_config(
+		mldl_cfg, gyro_handle, accel_handle, data,
+		mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL],
+		mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL]);
+}
+
+static inline int inv_mpu_get_compass_config(struct mldl_cfg *mldl_cfg,
+					     void *gyro_handle,
+					     void *compass_handle,
+					     struct ext_slave_config *data)
+{
+	if (!mldl_cfg || !(mldl_cfg->pdata)) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_get_slave_config(
+		mldl_cfg, gyro_handle, compass_handle, data,
+		mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS],
+		mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]);
+}
+
+static inline int inv_mpu_get_pressure_config(struct mldl_cfg *mldl_cfg,
+					      void *gyro_handle,
+					      void *pressure_handle,
+					      struct ext_slave_config *data)
+{
+	if (!mldl_cfg || !(mldl_cfg->pdata)) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	return inv_mpu_get_slave_config(
+		mldl_cfg, gyro_handle, pressure_handle, data,
+		mldl_cfg->slave[EXT_SLAVE_TYPE_PRESSURE],
+		mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_PRESSURE]);
+}
+
+/* -------------------------------------------------------------------------- */
+
+static inline
+long inv_mpu_get_sampling_rate_hz(struct mpu_gyro_cfg *gyro_cfg)
+{
+	if (((gyro_cfg->lpf) == 0) || ((gyro_cfg->lpf) == 7))
+		return 8000L / (gyro_cfg->divider + 1);
+	else
+		return 1000L / (gyro_cfg->divider + 1);
+}
+
+static inline
+long inv_mpu_get_sampling_period_us(struct mpu_gyro_cfg *gyro_cfg)
+{
+	if (((gyro_cfg->lpf) == 0) || ((gyro_cfg->lpf) == 7))
+		return (long) (1000000L * (gyro_cfg->divider + 1)) / 8000L;
+	else
+		return (long) (1000000L * (gyro_cfg->divider + 1)) / 1000L;
+}
+
+#endif				/* __MLDL_CFG_H__ */
+
+/**
+ * @}
+ */
diff --git a/drivers/misc/inv_mpu/mldl_print_cfg.h b/drivers/misc/inv_mpu/mldl_print_cfg.h
new file mode 100644
index 000000000000..2e1911440cca
--- /dev/null
+++ b/drivers/misc/inv_mpu/mldl_print_cfg.h
@@ -0,0 +1,38 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ * @defgroup
+ * @brief
+ *
+ * @{
+ * @file     mldl_print_cfg.h
+ * @brief
+ *
+ *
+ */
+#ifndef __MLDL_PRINT_CFG__
+#define __MLDL_PRINT_CFG__
+
+#include "mldl_cfg.h"
+
+
+void mldl_print_cfg(struct mldl_cfg *mldl_cfg);
+
+#endif /* __MLDL_PRINT_CFG__ */
diff --git a/drivers/misc/inv_mpu/mlsl.h b/drivers/misc/inv_mpu/mlsl.h
new file mode 100644
index 000000000000..204baedc1e20
--- /dev/null
+++ b/drivers/misc/inv_mpu/mlsl.h
@@ -0,0 +1,186 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+#ifndef __MLSL_H__
+#define __MLSL_H__
+
+/**
+ *  @defgroup   MLSL
+ *  @brief      Motion Library - Serial Layer.
+ *              The Motion Library System Layer provides the Motion Library
+ *              with the communication interface to the hardware.
+ *
+ *              The communication interface is assumed to support serial
+ *              transfers in burst of variable length up to
+ *              SERIAL_MAX_TRANSFER_SIZE.
+ *              The default value for SERIAL_MAX_TRANSFER_SIZE is 128 bytes.
+ *              Transfers of length greater than SERIAL_MAX_TRANSFER_SIZE, will
+ *              be subdivided in smaller transfers of length <=
+ *              SERIAL_MAX_TRANSFER_SIZE.
+ *              The SERIAL_MAX_TRANSFER_SIZE definition can be modified to
+ *              overcome any host processor transfer size limitation down to
+ *              1 B, the minimum.
+ *              An higher value for SERIAL_MAX_TRANSFER_SIZE will favor
+ *              performance and efficiency while requiring higher resource usage
+ *              (mostly buffering). A smaller value will increase overhead and
+ *              decrease efficiency but allows to operate with more resource
+ *              constrained processor and master serial controllers.
+ *              The SERIAL_MAX_TRANSFER_SIZE definition can be found in the
+ *              mlsl.h header file and master serial controllers.
+ *              The SERIAL_MAX_TRANSFER_SIZE definition can be found in the
+ *              mlsl.h header file.
+ *
+ *  @{
+ *      @file   mlsl.h
+ *      @brief  The Motion Library System Layer.
+ *
+ */
+
+#include "mltypes.h"
+#include <linux/mpu.h>
+
+
+/*
+ * NOTE : to properly support Yamaha compass reads,
+ *	  the max transfer size should be at least 9 B.
+ *	  Length in bytes, typically a power of 2 >= 2
+ */
+#define SERIAL_MAX_TRANSFER_SIZE 128
+
+
+/**
+ *  inv_serial_single_write() - used to write a single byte of data.
+ *  @sl_handle		pointer to the serial device used for the communication.
+ *  @slave_addr		I2C slave address of device.
+ *  @register_addr	Register address to write.
+ *  @data		Single byte of data to write.
+ *
+ *	It is called by the MPL to write a single byte of data to the MPU.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int inv_serial_single_write(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned char register_addr,
+	unsigned char data);
+
+/**
+ *  inv_serial_write() - used to write multiple bytes of data to registers.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @register_addr	Register address to write.
+ *  @length	Length of burst of data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int inv_serial_write(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short length,
+	unsigned char const *data);
+
+/**
+ *  inv_serial_read() - used to read multiple bytes of data from registers.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @register_addr	Register address to read.
+ *  @length	Length of burst of data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+int inv_serial_read(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned char register_addr,
+	unsigned short length,
+	unsigned char *data);
+
+/**
+ *  inv_serial_read_mem() - used to read multiple bytes of data from the memory.
+ *	    This should be sent by I2C or SPI.
+ *
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @mem_addr	The location in the memory to read from.
+ *  @length	Length of burst data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+int inv_serial_read_mem(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short mem_addr,
+	unsigned short length,
+	unsigned char *data);
+
+/**
+ *  inv_serial_write_mem() - used to write multiple bytes of data to the memory.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @mem_addr	The location in the memory to write to.
+ *  @length	Length of burst data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+int inv_serial_write_mem(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short mem_addr,
+	unsigned short length,
+	unsigned char const *data);
+
+/**
+ *  inv_serial_read_fifo() - used to read multiple bytes of data from the fifo.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @length	Length of burst of data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+int inv_serial_read_fifo(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short length,
+	unsigned char *data);
+
+/**
+ *  inv_serial_write_fifo() - used to write multiple bytes of data to the fifo.
+ *  @sl_handle	a file handle to the serial device used for the communication.
+ *  @slave_addr	I2C slave address of device.
+ *  @length	Length of burst of data.
+ *  @data	Pointer to block of data.
+ *
+ *  returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise.
+ */
+int inv_serial_write_fifo(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short length,
+	unsigned char const *data);
+
+/**
+ * @}
+ */
+#endif				/* __MLSL_H__ */
diff --git a/drivers/misc/inv_mpu/mltypes.h b/drivers/misc/inv_mpu/mltypes.h
new file mode 100644
index 000000000000..a249f93be3ee
--- /dev/null
+++ b/drivers/misc/inv_mpu/mltypes.h
@@ -0,0 +1,234 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @defgroup MLERROR
+ *  @brief  Definition of the error codes used within the MPL and
+ *          returned to the user.
+ *          Every function tries to return a meaningful error code basing
+ *          on the occuring error condition. The error code is numeric.
+ *
+ *          The available error codes and their associated values are:
+ *          - (0)               INV_SUCCESS
+ *          - (32)              INV_ERROR
+ *          - (22 / EINVAL)     INV_ERROR_INVALID_PARAMETER
+ *          - (1  / EPERM)      INV_ERROR_FEATURE_NOT_ENABLED
+ *          - (36)              INV_ERROR_FEATURE_NOT_IMPLEMENTED
+ *          - (38)              INV_ERROR_DMP_NOT_STARTED
+ *          - (39)              INV_ERROR_DMP_STARTED
+ *          - (40)              INV_ERROR_NOT_OPENED
+ *          - (41)              INV_ERROR_OPENED
+ *          - (19 / ENODEV)     INV_ERROR_INVALID_MODULE
+ *          - (12 / ENOMEM)     INV_ERROR_MEMORY_EXAUSTED
+ *          - (44)              INV_ERROR_DIVIDE_BY_ZERO
+ *          - (45)              INV_ERROR_ASSERTION_FAILURE
+ *          - (46)              INV_ERROR_FILE_OPEN
+ *          - (47)              INV_ERROR_FILE_READ
+ *          - (48)              INV_ERROR_FILE_WRITE
+ *          - (49)              INV_ERROR_INVALID_CONFIGURATION
+ *          - (52)              INV_ERROR_SERIAL_CLOSED
+ *          - (53)              INV_ERROR_SERIAL_OPEN_ERROR
+ *          - (54)              INV_ERROR_SERIAL_READ
+ *          - (55)              INV_ERROR_SERIAL_WRITE
+ *          - (56)              INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED
+ *          - (57)              INV_ERROR_SM_TRANSITION
+ *          - (58)              INV_ERROR_SM_IMPROPER_STATE
+ *          - (62)              INV_ERROR_FIFO_OVERFLOW
+ *          - (63)              INV_ERROR_FIFO_FOOTER
+ *          - (64)              INV_ERROR_FIFO_READ_COUNT
+ *          - (65)              INV_ERROR_FIFO_READ_DATA
+ *          - (72)              INV_ERROR_MEMORY_SET
+ *          - (82)              INV_ERROR_LOG_MEMORY_ERROR
+ *          - (83)              INV_ERROR_LOG_OUTPUT_ERROR
+ *          - (92)              INV_ERROR_OS_BAD_PTR
+ *          - (93)              INV_ERROR_OS_BAD_HANDLE
+ *          - (94)              INV_ERROR_OS_CREATE_FAILED
+ *          - (95)              INV_ERROR_OS_LOCK_FAILED
+ *          - (102)             INV_ERROR_COMPASS_DATA_OVERFLOW
+ *          - (103)             INV_ERROR_COMPASS_DATA_UNDERFLOW
+ *          - (104)             INV_ERROR_COMPASS_DATA_NOT_READY
+ *          - (105)             INV_ERROR_COMPASS_DATA_ERROR
+ *          - (107)             INV_ERROR_CALIBRATION_LOAD
+ *          - (108)             INV_ERROR_CALIBRATION_STORE
+ *          - (109)             INV_ERROR_CALIBRATION_LEN
+ *          - (110)             INV_ERROR_CALIBRATION_CHECKSUM
+ *          - (111)             INV_ERROR_ACCEL_DATA_OVERFLOW
+ *          - (112)             INV_ERROR_ACCEL_DATA_UNDERFLOW
+ *          - (113)             INV_ERROR_ACCEL_DATA_NOT_READY
+ *          - (114)             INV_ERROR_ACCEL_DATA_ERROR
+ *
+ *          The available warning codes and their associated values are:
+ *          - (115)             INV_WARNING_MOTION_RACE
+ *          - (116)             INV_WARNING_QUAT_TRASHED
+ *
+ *  @{
+ *      @file mltypes.h
+ *  @}
+ */
+
+#ifndef MLTYPES_H
+#define MLTYPES_H
+
+#include <linux/types.h>
+#include <asm-generic/errno-base.h>
+
+
+
+
+/*---------------------------
+ *    ML Defines
+ *--------------------------*/
+
+#ifndef NULL
+#define NULL 0
+#endif
+
+/* - ML Errors. - */
+#define ERROR_NAME(x)   (#x)
+#define ERROR_CHECK_FIRST(first, x) \
+	{ if (INV_SUCCESS == first) first = x; }
+
+#define INV_SUCCESS                       (0)
+/* Generic Error code.  Proprietary Error Codes only */
+#define INV_ERROR_BASE                    (0x20)
+#define INV_ERROR                         (INV_ERROR_BASE)
+
+/* Compatibility and other generic error codes */
+#define INV_ERROR_INVALID_PARAMETER             (EINVAL)
+#define INV_ERROR_FEATURE_NOT_ENABLED           (EPERM)
+#define INV_ERROR_FEATURE_NOT_IMPLEMENTED       (INV_ERROR_BASE + 4)
+#define INV_ERROR_DMP_NOT_STARTED               (INV_ERROR_BASE + 6)
+#define INV_ERROR_DMP_STARTED                   (INV_ERROR_BASE + 7)
+#define INV_ERROR_NOT_OPENED                    (INV_ERROR_BASE + 8)
+#define INV_ERROR_OPENED                        (INV_ERROR_BASE + 9)
+#define INV_ERROR_INVALID_MODULE                (ENODEV)
+#define INV_ERROR_MEMORY_EXAUSTED               (ENOMEM)
+#define INV_ERROR_DIVIDE_BY_ZERO                (INV_ERROR_BASE + 12)
+#define INV_ERROR_ASSERTION_FAILURE             (INV_ERROR_BASE + 13)
+#define INV_ERROR_FILE_OPEN                     (INV_ERROR_BASE + 14)
+#define INV_ERROR_FILE_READ                     (INV_ERROR_BASE + 15)
+#define INV_ERROR_FILE_WRITE                    (INV_ERROR_BASE + 16)
+#define INV_ERROR_INVALID_CONFIGURATION         (INV_ERROR_BASE + 17)
+
+/* Serial Communication */
+#define INV_ERROR_SERIAL_CLOSED                 (INV_ERROR_BASE + 20)
+#define INV_ERROR_SERIAL_OPEN_ERROR             (INV_ERROR_BASE + 21)
+#define INV_ERROR_SERIAL_READ                   (INV_ERROR_BASE + 22)
+#define INV_ERROR_SERIAL_WRITE                  (INV_ERROR_BASE + 23)
+#define INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED  (INV_ERROR_BASE + 24)
+
+/* SM = State Machine */
+#define INV_ERROR_SM_TRANSITION                 (INV_ERROR_BASE + 25)
+#define INV_ERROR_SM_IMPROPER_STATE             (INV_ERROR_BASE + 26)
+
+/* Fifo */
+#define INV_ERROR_FIFO_OVERFLOW                 (INV_ERROR_BASE + 30)
+#define INV_ERROR_FIFO_FOOTER                   (INV_ERROR_BASE + 31)
+#define INV_ERROR_FIFO_READ_COUNT               (INV_ERROR_BASE + 32)
+#define INV_ERROR_FIFO_READ_DATA                (INV_ERROR_BASE + 33)
+
+/* Memory & Registers, Set & Get */
+#define INV_ERROR_MEMORY_SET                    (INV_ERROR_BASE + 40)
+
+#define INV_ERROR_LOG_MEMORY_ERROR              (INV_ERROR_BASE + 50)
+#define INV_ERROR_LOG_OUTPUT_ERROR              (INV_ERROR_BASE + 51)
+
+/* OS interface errors */
+#define INV_ERROR_OS_BAD_PTR                    (INV_ERROR_BASE + 60)
+#define INV_ERROR_OS_BAD_HANDLE                 (INV_ERROR_BASE + 61)
+#define INV_ERROR_OS_CREATE_FAILED              (INV_ERROR_BASE + 62)
+#define INV_ERROR_OS_LOCK_FAILED                (INV_ERROR_BASE + 63)
+
+/* Compass errors */
+#define INV_ERROR_COMPASS_DATA_OVERFLOW         (INV_ERROR_BASE + 70)
+#define INV_ERROR_COMPASS_DATA_UNDERFLOW        (INV_ERROR_BASE + 71)
+#define INV_ERROR_COMPASS_DATA_NOT_READY        (INV_ERROR_BASE + 72)
+#define INV_ERROR_COMPASS_DATA_ERROR            (INV_ERROR_BASE + 73)
+
+/* Load/Store calibration */
+#define INV_ERROR_CALIBRATION_LOAD              (INV_ERROR_BASE + 75)
+#define INV_ERROR_CALIBRATION_STORE             (INV_ERROR_BASE + 76)
+#define INV_ERROR_CALIBRATION_LEN               (INV_ERROR_BASE + 77)
+#define INV_ERROR_CALIBRATION_CHECKSUM          (INV_ERROR_BASE + 78)
+
+/* Accel errors */
+#define INV_ERROR_ACCEL_DATA_OVERFLOW           (INV_ERROR_BASE + 79)
+#define INV_ERROR_ACCEL_DATA_UNDERFLOW          (INV_ERROR_BASE + 80)
+#define INV_ERROR_ACCEL_DATA_NOT_READY          (INV_ERROR_BASE + 81)
+#define INV_ERROR_ACCEL_DATA_ERROR              (INV_ERROR_BASE + 82)
+
+/* No Motion Warning States */
+#define INV_WARNING_MOTION_RACE                 (INV_ERROR_BASE + 83)
+#define INV_WARNING_QUAT_TRASHED                (INV_ERROR_BASE + 84)
+#define INV_WARNING_GYRO_MAG                    (INV_ERROR_BASE + 85)
+
+#ifdef INV_USE_LEGACY_NAMES
+#define ML_SUCCESS                        INV_SUCCESS
+#define ML_ERROR                          INV_ERROR
+#define ML_ERROR_INVALID_PARAMETER        INV_ERROR_INVALID_PARAMETER
+#define ML_ERROR_FEATURE_NOT_ENABLED      INV_ERROR_FEATURE_NOT_ENABLED
+#define ML_ERROR_FEATURE_NOT_IMPLEMENTED  INV_ERROR_FEATURE_NOT_IMPLEMENTED
+#define ML_ERROR_DMP_NOT_STARTED          INV_ERROR_DMP_NOT_STARTED
+#define ML_ERROR_DMP_STARTED              INV_ERROR_DMP_STARTED
+#define ML_ERROR_NOT_OPENED               INV_ERROR_NOT_OPENED
+#define ML_ERROR_OPENED                   INV_ERROR_OPENED
+#define ML_ERROR_INVALID_MODULE           INV_ERROR_INVALID_MODULE
+#define ML_ERROR_MEMORY_EXAUSTED          INV_ERROR_MEMORY_EXAUSTED
+#define ML_ERROR_DIVIDE_BY_ZERO           INV_ERROR_DIVIDE_BY_ZERO
+#define ML_ERROR_ASSERTION_FAILURE        INV_ERROR_ASSERTION_FAILURE
+#define ML_ERROR_FILE_OPEN                INV_ERROR_FILE_OPEN
+#define ML_ERROR_FILE_READ                INV_ERROR_FILE_READ
+#define ML_ERROR_FILE_WRITE               INV_ERROR_FILE_WRITE
+#define ML_ERROR_INVALID_CONFIGURATION    INV_ERROR_INVALID_CONFIGURATION
+#define ML_ERROR_SERIAL_CLOSED            INV_ERROR_SERIAL_CLOSED
+#define ML_ERROR_SERIAL_OPEN_ERROR        INV_ERROR_SERIAL_OPEN_ERROR
+#define ML_ERROR_SERIAL_READ              INV_ERROR_SERIAL_READ
+#define ML_ERROR_SERIAL_WRITE             INV_ERROR_SERIAL_WRITE
+#define ML_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED  \
+	INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED
+#define ML_ERROR_SM_TRANSITION            INV_ERROR_SM_TRANSITION
+#define ML_ERROR_SM_IMPROPER_STATE        INV_ERROR_SM_IMPROPER_STATE
+#define ML_ERROR_FIFO_OVERFLOW            INV_ERROR_FIFO_OVERFLOW
+#define ML_ERROR_FIFO_FOOTER              INV_ERROR_FIFO_FOOTER
+#define ML_ERROR_FIFO_READ_COUNT          INV_ERROR_FIFO_READ_COUNT
+#define ML_ERROR_FIFO_READ_DATA           INV_ERROR_FIFO_READ_DATA
+#define ML_ERROR_MEMORY_SET               INV_ERROR_MEMORY_SET
+#define ML_ERROR_LOG_MEMORY_ERROR         INV_ERROR_LOG_MEMORY_ERROR
+#define ML_ERROR_LOG_OUTPUT_ERROR         INV_ERROR_LOG_OUTPUT_ERROR
+#define ML_ERROR_OS_BAD_PTR               INV_ERROR_OS_BAD_PTR
+#define ML_ERROR_OS_BAD_HANDLE            INV_ERROR_OS_BAD_HANDLE
+#define ML_ERROR_OS_CREATE_FAILED         INV_ERROR_OS_CREATE_FAILED
+#define ML_ERROR_OS_LOCK_FAILED           INV_ERROR_OS_LOCK_FAILED
+#define ML_ERROR_COMPASS_DATA_OVERFLOW    INV_ERROR_COMPASS_DATA_OVERFLOW
+#define ML_ERROR_COMPASS_DATA_UNDERFLOW   INV_ERROR_COMPASS_DATA_UNDERFLOW
+#define ML_ERROR_COMPASS_DATA_NOT_READY   INV_ERROR_COMPASS_DATA_NOT_READY
+#define ML_ERROR_COMPASS_DATA_ERROR       INV_ERROR_COMPASS_DATA_ERROR
+#define ML_ERROR_CALIBRATION_LOAD         INV_ERROR_CALIBRATION_LOAD
+#define ML_ERROR_CALIBRATION_STORE        INV_ERROR_CALIBRATION_STORE
+#define ML_ERROR_CALIBRATION_LEN          INV_ERROR_CALIBRATION_LEN
+#define ML_ERROR_CALIBRATION_CHECKSUM     INV_ERROR_CALIBRATION_CHECKSUM
+#define ML_ERROR_ACCEL_DATA_OVERFLOW      INV_ERROR_ACCEL_DATA_OVERFLOW
+#define ML_ERROR_ACCEL_DATA_UNDERFLOW     INV_ERROR_ACCEL_DATA_UNDERFLOW
+#define ML_ERROR_ACCEL_DATA_NOT_READY     INV_ERROR_ACCEL_DATA_NOT_READY
+#define ML_ERROR_ACCEL_DATA_ERROR         INV_ERROR_ACCEL_DATA_ERROR
+#endif
+
+/* For Linux coding compliance */
+
+#endif				/* MLTYPES_H */
diff --git a/drivers/misc/inv_mpu/mpu-dev.h b/drivers/misc/inv_mpu/mpu-dev.h
new file mode 100644
index 000000000000..b6a4fcfac586
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu-dev.h
@@ -0,0 +1,36 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+
+#ifndef __MPU_DEV_H__
+#define __MPU_DEV_H__
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/mpu.h>
+
+int inv_mpu_register_slave(struct module *slave_module,
+			struct i2c_client *client,
+			struct ext_slave_platform_data *pdata,
+			struct ext_slave_descr *(*slave_descr)(void));
+
+void inv_mpu_unregister_slave(struct i2c_client *client,
+			struct ext_slave_platform_data *pdata,
+			struct ext_slave_descr *(*slave_descr)(void));
+#endif
diff --git a/drivers/misc/inv_mpu/mpu3050.h b/drivers/misc/inv_mpu/mpu3050.h
new file mode 100644
index 000000000000..02af16ed1216
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050.h
@@ -0,0 +1,251 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+#ifndef __MPU_H_
+#error Do not include this file directly.  Include mpu.h instead.
+#endif
+
+#ifndef __MPU3050_H_
+#define __MPU3050_H_
+
+#include <linux/types.h>
+
+
+#define MPU_NAME "mpu3050"
+#define DEFAULT_MPU_SLAVEADDR       0x68
+
+/*==== MPU REGISTER SET ====*/
+enum mpu_register {
+	MPUREG_WHO_AM_I = 0,	/* 00 0x00 */
+	MPUREG_PRODUCT_ID,	/* 01 0x01 */
+	MPUREG_02_RSVD,		/* 02 0x02 */
+	MPUREG_03_RSVD,		/* 03 0x03 */
+	MPUREG_04_RSVD,		/* 04 0x04 */
+	MPUREG_XG_OFFS_TC,	/* 05 0x05 */
+	MPUREG_06_RSVD,		/* 06 0x06 */
+	MPUREG_07_RSVD,		/* 07 0x07 */
+	MPUREG_YG_OFFS_TC,	/* 08 0x08 */
+	MPUREG_09_RSVD,		/* 09 0x09 */
+	MPUREG_0A_RSVD,		/* 10 0x0a */
+	MPUREG_ZG_OFFS_TC,	/* 11 0x0b */
+	MPUREG_X_OFFS_USRH,	/* 12 0x0c */
+	MPUREG_X_OFFS_USRL,	/* 13 0x0d */
+	MPUREG_Y_OFFS_USRH,	/* 14 0x0e */
+	MPUREG_Y_OFFS_USRL,	/* 15 0x0f */
+	MPUREG_Z_OFFS_USRH,	/* 16 0x10 */
+	MPUREG_Z_OFFS_USRL,	/* 17 0x11 */
+	MPUREG_FIFO_EN1,	/* 18 0x12 */
+	MPUREG_FIFO_EN2,	/* 19 0x13 */
+	MPUREG_AUX_SLV_ADDR,	/* 20 0x14 */
+	MPUREG_SMPLRT_DIV,	/* 21 0x15 */
+	MPUREG_DLPF_FS_SYNC,	/* 22 0x16 */
+	MPUREG_INT_CFG,		/* 23 0x17 */
+	MPUREG_ACCEL_BURST_ADDR,/* 24 0x18 */
+	MPUREG_19_RSVD,		/* 25 0x19 */
+	MPUREG_INT_STATUS,	/* 26 0x1a */
+	MPUREG_TEMP_OUT_H,	/* 27 0x1b */
+	MPUREG_TEMP_OUT_L,	/* 28 0x1c */
+	MPUREG_GYRO_XOUT_H,	/* 29 0x1d */
+	MPUREG_GYRO_XOUT_L,	/* 30 0x1e */
+	MPUREG_GYRO_YOUT_H,	/* 31 0x1f */
+	MPUREG_GYRO_YOUT_L,	/* 32 0x20 */
+	MPUREG_GYRO_ZOUT_H,	/* 33 0x21 */
+	MPUREG_GYRO_ZOUT_L,	/* 34 0x22 */
+	MPUREG_23_RSVD,		/* 35 0x23 */
+	MPUREG_24_RSVD,		/* 36 0x24 */
+	MPUREG_25_RSVD,		/* 37 0x25 */
+	MPUREG_26_RSVD,		/* 38 0x26 */
+	MPUREG_27_RSVD,		/* 39 0x27 */
+	MPUREG_28_RSVD,		/* 40 0x28 */
+	MPUREG_29_RSVD,		/* 41 0x29 */
+	MPUREG_2A_RSVD,		/* 42 0x2a */
+	MPUREG_2B_RSVD,		/* 43 0x2b */
+	MPUREG_2C_RSVD,		/* 44 0x2c */
+	MPUREG_2D_RSVD,		/* 45 0x2d */
+	MPUREG_2E_RSVD,		/* 46 0x2e */
+	MPUREG_2F_RSVD,		/* 47 0x2f */
+	MPUREG_30_RSVD,		/* 48 0x30 */
+	MPUREG_31_RSVD,		/* 49 0x31 */
+	MPUREG_32_RSVD,		/* 50 0x32 */
+	MPUREG_33_RSVD,		/* 51 0x33 */
+	MPUREG_34_RSVD,		/* 52 0x34 */
+	MPUREG_DMP_CFG_1,	/* 53 0x35 */
+	MPUREG_DMP_CFG_2,	/* 54 0x36 */
+	MPUREG_BANK_SEL,	/* 55 0x37 */
+	MPUREG_MEM_START_ADDR,	/* 56 0x38 */
+	MPUREG_MEM_R_W,		/* 57 0x39 */
+	MPUREG_FIFO_COUNTH,	/* 58 0x3a */
+	MPUREG_FIFO_COUNTL,	/* 59 0x3b */
+	MPUREG_FIFO_R_W,	/* 60 0x3c */
+	MPUREG_USER_CTRL,	/* 61 0x3d */
+	MPUREG_PWR_MGM,		/* 62 0x3e */
+	MPUREG_3F_RSVD,		/* 63 0x3f */
+	NUM_OF_MPU_REGISTERS	/* 64 0x40 */
+};
+
+/*==== BITS FOR MPU ====*/
+
+/*---- MPU 'FIFO_EN1' register (12) ----*/
+#define BIT_TEMP_OUT                0x80
+#define BIT_GYRO_XOUT               0x40
+#define BIT_GYRO_YOUT               0x20
+#define BIT_GYRO_ZOUT               0x10
+#define BIT_ACCEL_XOUT              0x08
+#define BIT_ACCEL_YOUT              0x04
+#define BIT_ACCEL_ZOUT              0x02
+#define BIT_AUX_1OUT                0x01
+/*---- MPU 'FIFO_EN2' register (13) ----*/
+#define BIT_AUX_2OUT                0x02
+#define BIT_AUX_3OUT                0x01
+/*---- MPU 'DLPF_FS_SYNC' register (16) ----*/
+#define BITS_EXT_SYNC_NONE          0x00
+#define BITS_EXT_SYNC_TEMP          0x20
+#define BITS_EXT_SYNC_GYROX         0x40
+#define BITS_EXT_SYNC_GYROY         0x60
+#define BITS_EXT_SYNC_GYROZ         0x80
+#define BITS_EXT_SYNC_ACCELX        0xA0
+#define BITS_EXT_SYNC_ACCELY        0xC0
+#define BITS_EXT_SYNC_ACCELZ        0xE0
+#define BITS_EXT_SYNC_MASK          0xE0
+#define BITS_FS_250DPS              0x00
+#define BITS_FS_500DPS              0x08
+#define BITS_FS_1000DPS             0x10
+#define BITS_FS_2000DPS             0x18
+#define BITS_FS_MASK                0x18
+#define BITS_DLPF_CFG_256HZ_NOLPF2  0x00
+#define BITS_DLPF_CFG_188HZ         0x01
+#define BITS_DLPF_CFG_98HZ          0x02
+#define BITS_DLPF_CFG_42HZ          0x03
+#define BITS_DLPF_CFG_20HZ          0x04
+#define BITS_DLPF_CFG_10HZ          0x05
+#define BITS_DLPF_CFG_5HZ           0x06
+#define BITS_DLPF_CFG_2100HZ_NOLPF  0x07
+#define BITS_DLPF_CFG_MASK          0x07
+/*---- MPU 'INT_CFG' register (17) ----*/
+#define BIT_ACTL                    0x80
+#define BIT_ACTL_LOW                0x80
+#define BIT_ACTL_HIGH               0x00
+#define BIT_OPEN                    0x40
+#define BIT_OPEN_DRAIN              0x40
+#define BIT_PUSH_PULL               0x00
+#define BIT_LATCH_INT_EN            0x20
+#define BIT_INT_PULSE_WIDTH_50US    0x00
+#define BIT_INT_ANYRD_2CLEAR        0x10
+#define BIT_INT_STAT_READ_2CLEAR    0x00
+#define BIT_MPU_RDY_EN              0x04
+#define BIT_DMP_INT_EN              0x02
+#define BIT_RAW_RDY_EN              0x01
+/*---- MPU 'INT_STATUS' register (1A) ----*/
+#define BIT_INT_STATUS_FIFO_OVERLOW 0x80
+#define BIT_MPU_RDY                 0x04
+#define BIT_DMP_INT                 0x02
+#define BIT_RAW_RDY                 0x01
+/*---- MPU 'BANK_SEL' register (37) ----*/
+#define BIT_PRFTCH_EN               0x20
+#define BIT_CFG_USER_BANK           0x10
+#define BITS_MEM_SEL                0x0f
+/*---- MPU 'USER_CTRL' register (3D) ----*/
+#define BIT_DMP_EN                  0x80
+#define BIT_FIFO_EN                 0x40
+#define BIT_AUX_IF_EN               0x20
+#define BIT_AUX_RD_LENG             0x10
+#define BIT_AUX_IF_RST              0x08
+#define BIT_DMP_RST                 0x04
+#define BIT_FIFO_RST                0x02
+#define BIT_GYRO_RST                0x01
+/*---- MPU 'PWR_MGM' register (3E) ----*/
+#define BIT_H_RESET                 0x80
+#define BIT_SLEEP                   0x40
+#define BIT_STBY_XG                 0x20
+#define BIT_STBY_YG                 0x10
+#define BIT_STBY_ZG                 0x08
+#define BITS_CLKSEL                 0x07
+
+/*---- MPU Silicon Revision ----*/
+#define MPU_SILICON_REV_A4           1	/* MPU A4 Device */
+#define MPU_SILICON_REV_B1           2	/* MPU B1 Device */
+#define MPU_SILICON_REV_B4           3	/* MPU B4 Device */
+#define MPU_SILICON_REV_B6           4	/* MPU B6 Device */
+
+/*---- MPU Memory ----*/
+#define MPU_MEM_BANK_SIZE           (256)
+#define FIFO_HW_SIZE                (512)
+
+enum MPU_MEMORY_BANKS {
+	MPU_MEM_RAM_BANK_0 = 0,
+	MPU_MEM_RAM_BANK_1,
+	MPU_MEM_RAM_BANK_2,
+	MPU_MEM_RAM_BANK_3,
+	MPU_MEM_NUM_RAM_BANKS,
+	MPU_MEM_OTP_BANK_0 = MPU_MEM_NUM_RAM_BANKS,
+	/* This one is always last */
+	MPU_MEM_NUM_BANKS
+};
+
+/*---- structure containing control variables used by MLDL ----*/
+/*---- MPU clock source settings ----*/
+/*---- MPU filter selections ----*/
+enum mpu_filter {
+	MPU_FILTER_256HZ_NOLPF2 = 0,
+	MPU_FILTER_188HZ,
+	MPU_FILTER_98HZ,
+	MPU_FILTER_42HZ,
+	MPU_FILTER_20HZ,
+	MPU_FILTER_10HZ,
+	MPU_FILTER_5HZ,
+	MPU_FILTER_2100HZ_NOLPF,
+	NUM_MPU_FILTER
+};
+
+enum mpu_fullscale {
+	MPU_FS_250DPS = 0,
+	MPU_FS_500DPS,
+	MPU_FS_1000DPS,
+	MPU_FS_2000DPS,
+	NUM_MPU_FS
+};
+
+enum mpu_clock_sel {
+	MPU_CLK_SEL_INTERNAL = 0,
+	MPU_CLK_SEL_PLLGYROX,
+	MPU_CLK_SEL_PLLGYROY,
+	MPU_CLK_SEL_PLLGYROZ,
+	MPU_CLK_SEL_PLLEXT32K,
+	MPU_CLK_SEL_PLLEXT19M,
+	MPU_CLK_SEL_RESERVED,
+	MPU_CLK_SEL_STOP,
+	NUM_CLK_SEL
+};
+
+enum mpu_ext_sync {
+	MPU_EXT_SYNC_NONE = 0,
+	MPU_EXT_SYNC_TEMP,
+	MPU_EXT_SYNC_GYROX,
+	MPU_EXT_SYNC_GYROY,
+	MPU_EXT_SYNC_GYROZ,
+	MPU_EXT_SYNC_ACCELX,
+	MPU_EXT_SYNC_ACCELY,
+	MPU_EXT_SYNC_ACCELZ,
+	NUM_MPU_EXT_SYNC
+};
+
+#define DLPF_FS_SYNC_VALUE(ext_sync, full_scale, lpf) \
+	((ext_sync << 5) | (full_scale << 3) | lpf)
+
+#endif				/* __MPU3050_H_ */
diff --git a/drivers/misc/inv_mpu/mpu3050/Makefile b/drivers/misc/inv_mpu/mpu3050/Makefile
new file mode 100644
index 000000000000..9e5739302386
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050/Makefile
@@ -0,0 +1,17 @@
+
+# Kernel makefile for motions sensors
+#
+#
+
+obj-$(CONFIG_MPU_SENSORS_MPU3050) += mpu3050.o
+
+ccflags-y := -DMPU_CURRENT_BUILD_MPU3050
+
+mpu3050-objs += mldl_cfg.o
+mpu3050-objs += mldl_print_cfg.o
+mpu3050-objs += mlsl-kernel.o
+mpu3050-objs += mpu-dev.o
+
+EXTRA_CFLAGS += -Idrivers/misc/inv_mpu
+EXTRA_CFLAGS += -D__C99_DESIGNATED_INITIALIZER
+EXTRA_CFLAGS += -DINV_CACHE_DMP=1
diff --git a/drivers/misc/inv_mpu/mpu3050/mldl_cfg.c b/drivers/misc/inv_mpu/mpu3050/mldl_cfg.c
new file mode 100644
index 000000000000..ccacc8ec0b56
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050/mldl_cfg.c
@@ -0,0 +1,1765 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup MLDL
+ *
+ *  @{
+ *      @file   mldl_cfg.c
+ *      @brief  The Motion Library Driver Layer.
+ */
+
+/* -------------------------------------------------------------------------- */
+#include <linux/delay.h>
+#include <linux/slab.h>
+
+#include <stddef.h>
+
+#include "mldl_cfg.h"
+#include <linux/mpu.h>
+#include "mpu3050.h"
+
+#include "mlsl.h"
+#include "mldl_print_cfg.h"
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "mldl_cfg:"
+
+/* -------------------------------------------------------------------------- */
+
+#define SLEEP   1
+#define WAKE_UP 0
+#define RESET   1
+#define STANDBY 1
+
+/* -------------------------------------------------------------------------- */
+
+/**
+ * @brief Stop the DMP running
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+static int dmp_stop(struct mldl_cfg *mldl_cfg, void *gyro_handle)
+{
+	unsigned char user_ctrl_reg;
+	int result;
+
+	if (mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED)
+		return INV_SUCCESS;
+
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_USER_CTRL, 1, &user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	user_ctrl_reg = (user_ctrl_reg & (~BIT_FIFO_EN)) | BIT_FIFO_RST;
+	user_ctrl_reg = (user_ctrl_reg & (~BIT_DMP_EN)) | BIT_DMP_RST;
+
+	result = inv_serial_single_write(gyro_handle,
+					 mldl_cfg->mpu_chip_info->addr,
+					 MPUREG_USER_CTRL, user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	mldl_cfg->inv_mpu_state->status |= MPU_DMP_IS_SUSPENDED;
+
+	return result;
+}
+
+/**
+ * @brief Starts the DMP running
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+static int dmp_start(struct mldl_cfg *mldl_cfg, void *mlsl_handle)
+{
+	unsigned char user_ctrl_reg;
+	int result;
+
+	if ((!(mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED) &&
+	    mldl_cfg->mpu_gyro_cfg->dmp_enable)
+		||
+	   ((mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED) &&
+		   !mldl_cfg->mpu_gyro_cfg->dmp_enable))
+		return INV_SUCCESS;
+
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_USER_CTRL, 1, &user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_single_write(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_USER_CTRL,
+		((user_ctrl_reg & (~BIT_FIFO_EN))
+			| BIT_FIFO_RST));
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_single_write(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_USER_CTRL, user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_USER_CTRL, 1, &user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	user_ctrl_reg |= BIT_DMP_EN;
+
+	if (mldl_cfg->mpu_gyro_cfg->fifo_enable)
+		user_ctrl_reg |= BIT_FIFO_EN;
+	else
+		user_ctrl_reg &= ~BIT_FIFO_EN;
+
+	user_ctrl_reg |= BIT_DMP_RST;
+
+	result = inv_serial_single_write(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_USER_CTRL, user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	mldl_cfg->inv_mpu_state->status &= ~MPU_DMP_IS_SUSPENDED;
+
+	return result;
+}
+
+
+
+static int mpu3050_set_i2c_bypass(struct mldl_cfg *mldl_cfg,
+				  void *mlsl_handle, unsigned char enable)
+{
+	unsigned char b;
+	int result;
+	unsigned char status = mldl_cfg->inv_mpu_state->status;
+
+	if ((status & MPU_GYRO_IS_BYPASSED && enable) ||
+	    (!(status & MPU_GYRO_IS_BYPASSED) && !enable))
+		return INV_SUCCESS;
+
+	/*---- get current 'USER_CTRL' into b ----*/
+	result = inv_serial_read(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_USER_CTRL, 1, &b);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	b &= ~BIT_AUX_IF_EN;
+
+	if (!enable) {
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_USER_CTRL,
+			(b | BIT_AUX_IF_EN));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	} else {
+		/* Coming out of I2C is tricky due to several erratta.  Do not
+		 * modify this algorithm
+		 */
+		/*
+		 * 1) wait for the right time and send the command to change
+		 * the aux i2c slave address to an invalid address that will
+		 * get nack'ed
+		 *
+		 * 0x00 is broadcast.  0x7F is unlikely to be used by any aux.
+		 */
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_AUX_SLV_ADDR, 0x7F);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		/*
+		 * 2) wait enough time for a nack to occur, then go into
+		 *    bypass mode:
+		 */
+		msleep(2);
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_USER_CTRL, (b));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		/*
+		 * 3) wait for up to one MPU cycle then restore the slave
+		 *    address
+		 */
+		msleep(inv_mpu_get_sampling_period_us(mldl_cfg->mpu_gyro_cfg)
+			/ 1000);
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_AUX_SLV_ADDR,
+			mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL]
+				->address);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+		/*
+		 * 4) reset the ime interface
+		 */
+
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_USER_CTRL,
+			(b | BIT_AUX_IF_RST));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		msleep(2);
+	}
+	if (enable)
+		mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_BYPASSED;
+	else
+		mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED;
+
+	return result;
+}
+
+
+/**
+ *  @brief  enables/disables the I2C bypass to an external device
+ *          connected to MPU's secondary I2C bus.
+ *  @param  enable
+ *              Non-zero to enable pass through.
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+static int mpu_set_i2c_bypass(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
+			      unsigned char enable)
+{
+	return mpu3050_set_i2c_bypass(mldl_cfg, mlsl_handle, enable);
+}
+
+
+#define NUM_OF_PROD_REVS (ARRAY_SIZE(prod_rev_map))
+
+/* NOTE : when not indicated, product revision
+	  is considered an 'npp'; non production part */
+
+struct prod_rev_map_t {
+	unsigned char silicon_rev;
+	unsigned short gyro_trim;
+};
+
+#define OLDEST_PROD_REV_SUPPORTED	11
+static struct prod_rev_map_t prod_rev_map[] = {
+	{0, 0},
+	{MPU_SILICON_REV_A4, 131},	/* 1  A? OBSOLETED */
+	{MPU_SILICON_REV_A4, 131},	/* 2  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 3  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 4  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 5  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 6  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 7  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 8  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 9  |  */
+	{MPU_SILICON_REV_A4, 131},	/* 10 V  */
+	{MPU_SILICON_REV_B1, 131},	/* 11 B1 */
+	{MPU_SILICON_REV_B1, 131},	/* 12 |  */
+	{MPU_SILICON_REV_B1, 131},	/* 13 |  */
+	{MPU_SILICON_REV_B1, 131},	/* 14 V  */
+	{MPU_SILICON_REV_B4, 131},	/* 15 B4 */
+	{MPU_SILICON_REV_B4, 131},	/* 16 |  */
+	{MPU_SILICON_REV_B4, 131},	/* 17 |  */
+	{MPU_SILICON_REV_B4, 131},	/* 18 |  */
+	{MPU_SILICON_REV_B4, 115},	/* 19 |  */
+	{MPU_SILICON_REV_B4, 115},	/* 20 V  */
+	{MPU_SILICON_REV_B6, 131},	/* 21 B6 (B6/A9)  */
+	{MPU_SILICON_REV_B4, 115},	/* 22 B4 (B7/A10) */
+	{MPU_SILICON_REV_B6, 0},	/* 23 B6 */
+	{MPU_SILICON_REV_B6, 0},	/* 24 |  */
+	{MPU_SILICON_REV_B6, 0},	/* 25 |  */
+	{MPU_SILICON_REV_B6, 131},	/* 26 V  (B6/A11) */
+};
+
+/**
+ *  @internal
+ *  @brief  Get the silicon revision ID from OTP for MPU3050.
+ *          The silicon revision number is in read from OTP bank 0,
+ *          ADDR6[7:2].  The corresponding ID is retrieved by lookup
+ *          in a map.
+ *
+ *  @param  mldl_cfg
+ *              a pointer to the mldl config data structure.
+ *  @param  mlsl_handle
+ *              an file handle to the serial communication device the
+ *              device is connected to.
+ *
+ *  @return 0 on success, a non-zero error code otherwise.
+ */
+static int inv_get_silicon_rev_mpu3050(
+		struct mldl_cfg *mldl_cfg, void *mlsl_handle)
+{
+	int result;
+	unsigned char index = 0x00;
+	unsigned char bank =
+	    (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0);
+	unsigned short mem_addr = ((bank << 8) | 0x06);
+	struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
+
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_PRODUCT_ID, 1,
+				 &mpu_chip_info->product_id);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_read_mem(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		mem_addr, 1, &index);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	index >>= 2;
+
+	/* clean the prefetch and cfg user bank bits */
+	result = inv_serial_single_write(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_BANK_SEL, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (index < OLDEST_PROD_REV_SUPPORTED || index >= NUM_OF_PROD_REVS) {
+		mpu_chip_info->silicon_revision = 0;
+		mpu_chip_info->gyro_sens_trim = 0;
+		MPL_LOGE("Unsupported Product Revision Detected : %d\n", index);
+		return INV_ERROR_INVALID_MODULE;
+	}
+
+	mpu_chip_info->product_revision = index;
+	mpu_chip_info->silicon_revision = prod_rev_map[index].silicon_rev;
+	mpu_chip_info->gyro_sens_trim = prod_rev_map[index].gyro_trim;
+	if (mpu_chip_info->gyro_sens_trim == 0) {
+		MPL_LOGE("gyro sensitivity trim is 0"
+			 " - unsupported non production part.\n");
+		return INV_ERROR_INVALID_MODULE;
+	}
+
+	return result;
+}
+#define inv_get_silicon_rev inv_get_silicon_rev_mpu3050
+
+
+/**
+ *  @brief  Enable / Disable the use MPU's secondary I2C interface level
+ *          shifters.
+ *          When enabled the secondary I2C interface to which the external
+ *          device is connected runs at VDD voltage (main supply).
+ *          When disabled the 2nd interface runs at VDDIO voltage.
+ *          See the device specification for more details.
+ *
+ *  @note   using this API may produce unpredictable results, depending on how
+ *          the MPU and slave device are setup on the target platform.
+ *          Use of this API should entirely be restricted to system
+ *          integrators. Once the correct value is found, there should be no
+ *          need to change the level shifter at runtime.
+ *
+ *  @pre    Must be called after inv_serial_start().
+ *  @note   Typically called before inv_dmp_open().
+ *
+ *  @param[in]  enable:
+ *                  0 to run at VDDIO (default),
+ *                  1 to run at VDD.
+ *
+ *  @return INV_SUCCESS if successfull, a non-zero error code otherwise.
+ */
+static int inv_mpu_set_level_shifter_bit(struct mldl_cfg *mldl_cfg,
+				  void *mlsl_handle, unsigned char enable)
+{
+	int result;
+	unsigned char regval;
+
+	unsigned char reg;
+	unsigned char mask;
+
+	if (0 == mldl_cfg->mpu_chip_info->silicon_revision)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	/*-- on parts before B6 the VDDIO bit is bit 7 of ACCEL_BURST_ADDR --
+	NOTE: this is incompatible with ST accelerometers where the VDDIO
+	bit MUST be set to enable ST's internal logic to autoincrement
+	the register address on burst reads --*/
+	if ((mldl_cfg->mpu_chip_info->silicon_revision & 0xf)
+		< MPU_SILICON_REV_B6) {
+		reg = MPUREG_ACCEL_BURST_ADDR;
+		mask = 0x80;
+	} else {
+		/*-- on B6 parts the VDDIO bit was moved to FIFO_EN2 =>
+		  the mask is always 0x04 --*/
+		reg = MPUREG_FIFO_EN2;
+		mask = 0x04;
+	}
+
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 reg, 1, &regval);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (enable)
+		regval |= mask;
+	else
+		regval &= ~mask;
+
+	result = inv_serial_single_write(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr, reg, regval);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return result;
+	return INV_SUCCESS;
+}
+
+
+/**
+ * @internal
+ * @brief   This function controls the power management on the MPU device.
+ *          The entire chip can be put to low power sleep mode, or individual
+ *          gyros can be turned on/off.
+ *
+ *          Putting the device into sleep mode depending upon the changing needs
+ *          of the associated applications is a recommended method for reducing
+ *          power consuption.  It is a safe opearation in that sleep/wake up of
+ *          gyros while running will not result in any interruption of data.
+ *
+ *          Although it is entirely allowed to put the device into full sleep
+ *          while running the DMP, it is not recomended because it will disrupt
+ *          the ongoing calculations carried on inside the DMP and consequently
+ *          the sensor fusion algorithm. Furthermore, while in sleep mode
+ *          read & write operation from the app processor on both registers and
+ *          memory are disabled and can only regained by restoring the MPU in
+ *          normal power mode.
+ *          Disabling any of the gyro axis will reduce the associated power
+ *          consuption from the PLL but will not stop the DMP from running
+ *          state.
+ *
+ * @param   reset
+ *              Non-zero to reset the device. Note that this setting
+ *              is volatile and the corresponding register bit will
+ *              clear itself right after being applied.
+ * @param   sleep
+ *              Non-zero to put device into full sleep.
+ * @param   disable_gx
+ *              Non-zero to disable gyro X.
+ * @param   disable_gy
+ *              Non-zero to disable gyro Y.
+ * @param   disable_gz
+ *              Non-zero to disable gyro Z.
+ *
+ * @return  INV_SUCCESS if successfull; a non-zero error code otherwise.
+ */
+static int mpu3050_pwr_mgmt(struct mldl_cfg *mldl_cfg,
+			    void *mlsl_handle,
+			    unsigned char reset,
+			    unsigned char sleep,
+			    unsigned char disable_gx,
+			    unsigned char disable_gy,
+			    unsigned char disable_gz)
+{
+	unsigned char b;
+	int result;
+
+	result =
+	    inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			    MPUREG_PWR_MGM, 1, &b);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* If we are awake, we need to put it in bypass before resetting */
+	if ((!(b & BIT_SLEEP)) && reset)
+		result = mpu_set_i2c_bypass(mldl_cfg, mlsl_handle, 1);
+
+	/* Reset if requested */
+	if (reset) {
+		MPL_LOGV("Reset MPU3050\n");
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_PWR_MGM, b | BIT_H_RESET);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		msleep(5);
+		/* Some chips are awake after reset and some are asleep,
+		 * check the status */
+		result = inv_serial_read(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_PWR_MGM, 1, &b);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	/* Update the suspended state just in case we return early */
+	if (b & BIT_SLEEP) {
+		mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_SUSPENDED;
+		mldl_cfg->inv_mpu_state->status |= MPU_DEVICE_IS_SUSPENDED;
+	} else {
+		mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_SUSPENDED;
+		mldl_cfg->inv_mpu_state->status &= ~MPU_DEVICE_IS_SUSPENDED;
+	}
+
+	/* if power status match requested, nothing else's left to do */
+	if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) ==
+	    (((sleep != 0) * BIT_SLEEP) |
+	     ((disable_gx != 0) * BIT_STBY_XG) |
+	     ((disable_gy != 0) * BIT_STBY_YG) |
+	     ((disable_gz != 0) * BIT_STBY_ZG))) {
+		return INV_SUCCESS;
+	}
+
+	/*
+	 * This specific transition between states needs to be reinterpreted:
+	 *    (1,1,1,1) -> (0,1,1,1) has to become
+	 *    (1,1,1,1) -> (1,0,0,0) -> (0,1,1,1)
+	 * where
+	 *    (1,1,1,1) is (sleep=1,disable_gx=1,disable_gy=1,disable_gz=1)
+	 */
+	if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) ==
+	    (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
+	    && ((!sleep) && disable_gx && disable_gy && disable_gz)) {
+		result = mpu3050_pwr_mgmt(mldl_cfg, mlsl_handle, 0, 1, 0, 0, 0);
+		if (result)
+			return result;
+		b |= BIT_SLEEP;
+		b &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
+	}
+
+	if ((b & BIT_SLEEP) != ((sleep != 0) * BIT_SLEEP)) {
+		if (sleep) {
+			result = mpu_set_i2c_bypass(mldl_cfg, mlsl_handle, 1);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			b |= BIT_SLEEP;
+			result =
+			    inv_serial_single_write(
+				    mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				    MPUREG_PWR_MGM, b);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			mldl_cfg->inv_mpu_state->status |=
+				MPU_GYRO_IS_SUSPENDED;
+			mldl_cfg->inv_mpu_state->status |=
+				MPU_DEVICE_IS_SUSPENDED;
+		} else {
+			b &= ~BIT_SLEEP;
+			result =
+			    inv_serial_single_write(
+				    mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				    MPUREG_PWR_MGM, b);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			mldl_cfg->inv_mpu_state->status &=
+				~MPU_GYRO_IS_SUSPENDED;
+			mldl_cfg->inv_mpu_state->status &=
+				~MPU_DEVICE_IS_SUSPENDED;
+			msleep(5);
+		}
+	}
+	/*---
+	  WORKAROUND FOR PUTTING GYRO AXIS in STAND-BY MODE
+	  1) put one axis at a time in stand-by
+	  ---*/
+	if ((b & BIT_STBY_XG) != ((disable_gx != 0) * BIT_STBY_XG)) {
+		b ^= BIT_STBY_XG;
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_PWR_MGM, b);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	if ((b & BIT_STBY_YG) != ((disable_gy != 0) * BIT_STBY_YG)) {
+		b ^= BIT_STBY_YG;
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_PWR_MGM, b);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	if ((b & BIT_STBY_ZG) != ((disable_gz != 0) * BIT_STBY_ZG)) {
+		b ^= BIT_STBY_ZG;
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_PWR_MGM, b);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	return INV_SUCCESS;
+}
+
+
+/**
+ *  @brief  sets the clock source for the gyros.
+ *  @param  mldl_cfg
+ *              a pointer to the struct mldl_cfg data structure.
+ *  @param  gyro_handle
+ *              an handle to the serial device the gyro is assigned to.
+ *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+static int mpu_set_clock_source(void *gyro_handle, struct mldl_cfg *mldl_cfg)
+{
+	int result;
+	unsigned char cur_clk_src;
+	unsigned char reg;
+
+	/* clock source selection */
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_PWR_MGM, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	cur_clk_src = reg & BITS_CLKSEL;
+	reg &= ~BITS_CLKSEL;
+
+
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_PWR_MGM, mldl_cfg->mpu_gyro_cfg->clk_src | reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* TODO : workarounds to be determined and implemented */
+
+	return result;
+}
+
+/**
+ * Configures the MPU I2C Master
+ *
+ * @mldl_cfg Handle to the configuration data
+ * @gyro_handle handle to the gyro communictation interface
+ * @slave Can be Null if turning off the slave
+ * @slave_pdata Can be null if turning off the slave
+ * @slave_id enum ext_slave_type to determine which index to use
+ *
+ *
+ * This fucntion configures the slaves by:
+ * 1) Setting up the read
+ *    a) Read Register
+ *    b) Read Length
+ * 2) Set up the data trigger (MPU6050 only)
+ *    a) Set trigger write register
+ *    b) Set Trigger write value
+ * 3) Set up the divider (MPU6050 only)
+ * 4) Set the slave bypass mode depending on slave
+ *
+ * returns INV_SUCCESS or non-zero error code
+ */
+static int mpu_set_slave_mpu3050(struct mldl_cfg *mldl_cfg,
+				 void *gyro_handle,
+				 struct ext_slave_descr *slave,
+				 struct ext_slave_platform_data *slave_pdata,
+				 int slave_id)
+{
+	int result;
+	unsigned char reg;
+	unsigned char slave_reg;
+	unsigned char slave_len;
+	unsigned char slave_endian;
+	unsigned char slave_address;
+
+	if (slave_id != EXT_SLAVE_TYPE_ACCEL)
+		return 0;
+
+	result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, true);
+
+	if (NULL == slave || NULL == slave_pdata) {
+		slave_reg = 0;
+		slave_len = 0;
+		slave_endian = 0;
+		slave_address = 0;
+		mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 0;
+	} else {
+		slave_reg = slave->read_reg;
+		slave_len = slave->read_len;
+		slave_endian = slave->endian;
+		slave_address = slave_pdata->address;
+		mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 1;
+	}
+
+	/* Address */
+	result = inv_serial_single_write(gyro_handle,
+					 mldl_cfg->mpu_chip_info->addr,
+					 MPUREG_AUX_SLV_ADDR, slave_address);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Register */
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_ACCEL_BURST_ADDR, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	reg = ((reg & 0x80) | slave_reg);
+	result = inv_serial_single_write(gyro_handle,
+					 mldl_cfg->mpu_chip_info->addr,
+					 MPUREG_ACCEL_BURST_ADDR, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Length */
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_USER_CTRL, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	reg = (reg & ~BIT_AUX_RD_LENG);
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_USER_CTRL, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	return result;
+}
+
+
+static int mpu_set_slave(struct mldl_cfg *mldl_cfg,
+			 void *gyro_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *slave_pdata,
+			 int slave_id)
+{
+	return mpu_set_slave_mpu3050(mldl_cfg, gyro_handle, slave,
+				     slave_pdata, slave_id);
+}
+/**
+ * Check to see if the gyro was reset by testing a couple of registers known
+ * to change on reset.
+ *
+ * @mldl_cfg mldl configuration structure
+ * @gyro_handle handle used to communicate with the gyro
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+static int mpu_was_reset(struct mldl_cfg *mldl_cfg, void *gyro_handle)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg;
+
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_DMP_CFG_2, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (mldl_cfg->mpu_gyro_cfg->dmp_cfg2 != reg)
+		return true;
+
+	if (0 != mldl_cfg->mpu_gyro_cfg->dmp_cfg1)
+		return false;
+
+	/* Inconclusive assume it was reset */
+	return true;
+}
+
+
+int inv_mpu_set_firmware(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
+			 const unsigned char *data, int size)
+{
+	int bank, offset, write_size;
+	int result;
+	unsigned char read[MPU_MEM_BANK_SIZE];
+
+	if (mldl_cfg->inv_mpu_state->status & MPU_DEVICE_IS_SUSPENDED) {
+#if INV_CACHE_DMP == 1
+		memcpy(mldl_cfg->mpu_ram->ram, data, size);
+		return INV_SUCCESS;
+#else
+		LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET);
+		return INV_ERROR_MEMORY_SET;
+#endif
+	}
+
+	if (!(mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED)) {
+		LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET);
+		return INV_ERROR_MEMORY_SET;
+	}
+	/* Write and verify memory */
+	for (bank = 0; size > 0; bank++,
+			size -= write_size,
+			data += write_size) {
+		if (size > MPU_MEM_BANK_SIZE)
+			write_size = MPU_MEM_BANK_SIZE;
+		else
+			write_size = size;
+
+		result = inv_serial_write_mem(mlsl_handle,
+				mldl_cfg->mpu_chip_info->addr,
+				((bank << 8) | 0x00),
+				write_size,
+				data);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			MPL_LOGE("Write mem error in bank %d\n", bank);
+			return result;
+		}
+		result = inv_serial_read_mem(mlsl_handle,
+				mldl_cfg->mpu_chip_info->addr,
+				((bank << 8) | 0x00),
+				write_size,
+				read);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			MPL_LOGE("Read mem error in bank %d\n", bank);
+			return result;
+		}
+
+#define ML_SKIP_CHECK 20
+		for (offset = 0; offset < write_size; offset++) {
+			/* skip the register memory locations */
+			if (bank == 0 && offset < ML_SKIP_CHECK)
+				continue;
+			if (data[offset] != read[offset]) {
+				result = INV_ERROR_SERIAL_WRITE;
+				break;
+			}
+		}
+		if (result != INV_SUCCESS) {
+			LOG_RESULT_LOCATION(result);
+			MPL_LOGE("Read data mismatch at bank %d, offset %d\n",
+				bank, offset);
+			return result;
+		}
+	}
+	return INV_SUCCESS;
+}
+
+static int gyro_resume(struct mldl_cfg *mldl_cfg, void *gyro_handle,
+		       unsigned long sensors)
+{
+	int result;
+	int ii;
+	unsigned char reg;
+	unsigned char regs[7];
+
+	/* Wake up the part */
+	result = mpu3050_pwr_mgmt(mldl_cfg, gyro_handle, false, false,
+				  !(sensors & INV_X_GYRO),
+				  !(sensors & INV_Y_GYRO),
+				  !(sensors & INV_Z_GYRO));
+
+	if (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_CONFIG) &&
+	    !mpu_was_reset(mldl_cfg, gyro_handle)) {
+		return INV_SUCCESS;
+	}
+
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_INT_CFG,
+		(mldl_cfg->mpu_gyro_cfg->int_config |
+			mldl_cfg->pdata->int_config));
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_SMPLRT_DIV, mldl_cfg->mpu_gyro_cfg->divider);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = mpu_set_clock_source(gyro_handle, mldl_cfg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	reg = DLPF_FS_SYNC_VALUE(mldl_cfg->mpu_gyro_cfg->ext_sync,
+				 mldl_cfg->mpu_gyro_cfg->full_scale,
+				 mldl_cfg->mpu_gyro_cfg->lpf);
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_DLPF_FS_SYNC, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_DMP_CFG_1, mldl_cfg->mpu_gyro_cfg->dmp_cfg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_DMP_CFG_2, mldl_cfg->mpu_gyro_cfg->dmp_cfg2);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Write and verify memory */
+#if INV_CACHE_DMP != 0
+	inv_mpu_set_firmware(mldl_cfg, gyro_handle,
+		mldl_cfg->mpu_ram->ram, mldl_cfg->mpu_ram->length);
+#endif
+
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_XG_OFFS_TC, mldl_cfg->mpu_offsets->tc[0]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_YG_OFFS_TC, mldl_cfg->mpu_offsets->tc[1]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_ZG_OFFS_TC, mldl_cfg->mpu_offsets->tc[2]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	regs[0] = MPUREG_X_OFFS_USRH;
+	for (ii = 0; ii < ARRAY_SIZE(mldl_cfg->mpu_offsets->gyro); ii++) {
+		regs[1 + ii * 2] =
+			(unsigned char)(mldl_cfg->mpu_offsets->gyro[ii] >> 8)
+			& 0xff;
+		regs[1 + ii * 2 + 1] =
+			(unsigned char)(mldl_cfg->mpu_offsets->gyro[ii] & 0xff);
+	}
+	result = inv_serial_write(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				  7, regs);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Configure slaves */
+	result = inv_mpu_set_level_shifter_bit(mldl_cfg, gyro_handle,
+					       mldl_cfg->pdata->level_shifter);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_NEEDS_CONFIG;
+
+	return result;
+}
+
+int gyro_config(void *mlsl_handle,
+		struct mldl_cfg *mldl_cfg,
+		struct ext_slave_config *data)
+{
+	struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg;
+	struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
+	struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets;
+	int ii;
+
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_INT_CONFIG:
+		mpu_gyro_cfg->int_config = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_EXT_SYNC:
+		mpu_gyro_cfg->ext_sync = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_FULL_SCALE:
+		mpu_gyro_cfg->full_scale = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_LPF:
+		mpu_gyro_cfg->lpf = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_CLK_SRC:
+		mpu_gyro_cfg->clk_src = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_DIVIDER:
+		mpu_gyro_cfg->divider = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_DMP_ENABLE:
+		mpu_gyro_cfg->dmp_enable = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_FIFO_ENABLE:
+		mpu_gyro_cfg->fifo_enable = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_DMP_CFG1:
+		mpu_gyro_cfg->dmp_cfg1 = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_DMP_CFG2:
+		mpu_gyro_cfg->dmp_cfg2 = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_TC:
+		for (ii = 0; ii < GYRO_NUM_AXES; ii++)
+			mpu_offsets->tc[ii] = ((__u8 *)data->data)[ii];
+		break;
+	case MPU_SLAVE_GYRO:
+		for (ii = 0; ii < GYRO_NUM_AXES; ii++)
+			mpu_offsets->gyro[ii] = ((__u16 *)data->data)[ii];
+		break;
+	case MPU_SLAVE_ADDR:
+		mpu_chip_info->addr = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_PRODUCT_REVISION:
+		mpu_chip_info->product_revision = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_SILICON_REVISION:
+		mpu_chip_info->silicon_revision = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_PRODUCT_ID:
+		mpu_chip_info->product_id = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_GYRO_SENS_TRIM:
+		mpu_chip_info->gyro_sens_trim = *((__u16 *)data->data);
+		break;
+	case MPU_SLAVE_ACCEL_SENS_TRIM:
+		mpu_chip_info->accel_sens_trim = *((__u16 *)data->data);
+		break;
+	case MPU_SLAVE_RAM:
+		if (data->len != mldl_cfg->mpu_ram->length)
+			return INV_ERROR_INVALID_PARAMETER;
+
+		memcpy(mldl_cfg->mpu_ram->ram, data->data, data->len);
+		break;
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+	mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG;
+	return INV_SUCCESS;
+}
+
+int gyro_get_config(void *mlsl_handle,
+		struct mldl_cfg *mldl_cfg,
+		struct ext_slave_config *data)
+{
+	struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg;
+	struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
+	struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets;
+	int ii;
+
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_INT_CONFIG:
+		*((__u8 *)data->data) = mpu_gyro_cfg->int_config;
+		break;
+	case MPU_SLAVE_EXT_SYNC:
+		*((__u8 *)data->data) = mpu_gyro_cfg->ext_sync;
+		break;
+	case MPU_SLAVE_FULL_SCALE:
+		*((__u8 *)data->data) = mpu_gyro_cfg->full_scale;
+		break;
+	case MPU_SLAVE_LPF:
+		*((__u8 *)data->data) = mpu_gyro_cfg->lpf;
+		break;
+	case MPU_SLAVE_CLK_SRC:
+		*((__u8 *)data->data) = mpu_gyro_cfg->clk_src;
+		break;
+	case MPU_SLAVE_DIVIDER:
+		*((__u8 *)data->data) = mpu_gyro_cfg->divider;
+		break;
+	case MPU_SLAVE_DMP_ENABLE:
+		*((__u8 *)data->data) = mpu_gyro_cfg->dmp_enable;
+		break;
+	case MPU_SLAVE_FIFO_ENABLE:
+		*((__u8 *)data->data) = mpu_gyro_cfg->fifo_enable;
+		break;
+	case MPU_SLAVE_DMP_CFG1:
+		*((__u8 *)data->data) = mpu_gyro_cfg->dmp_cfg1;
+		break;
+	case MPU_SLAVE_DMP_CFG2:
+		*((__u8 *)data->data) = mpu_gyro_cfg->dmp_cfg2;
+		break;
+	case MPU_SLAVE_TC:
+		for (ii = 0; ii < GYRO_NUM_AXES; ii++)
+			((__u8 *)data->data)[ii] = mpu_offsets->tc[ii];
+		break;
+	case MPU_SLAVE_GYRO:
+		for (ii = 0; ii < GYRO_NUM_AXES; ii++)
+			((__u16 *)data->data)[ii] = mpu_offsets->gyro[ii];
+		break;
+	case MPU_SLAVE_ADDR:
+		*((__u8 *)data->data) = mpu_chip_info->addr;
+		break;
+	case MPU_SLAVE_PRODUCT_REVISION:
+		*((__u8 *)data->data) = mpu_chip_info->product_revision;
+		break;
+	case MPU_SLAVE_SILICON_REVISION:
+		*((__u8 *)data->data) = mpu_chip_info->silicon_revision;
+		break;
+	case MPU_SLAVE_PRODUCT_ID:
+		*((__u8 *)data->data) = mpu_chip_info->product_id;
+		break;
+	case MPU_SLAVE_GYRO_SENS_TRIM:
+		*((__u16 *)data->data) = mpu_chip_info->gyro_sens_trim;
+		break;
+	case MPU_SLAVE_ACCEL_SENS_TRIM:
+		*((__u16 *)data->data) = mpu_chip_info->accel_sens_trim;
+		break;
+	case MPU_SLAVE_RAM:
+		if (data->len != mldl_cfg->mpu_ram->length)
+			return INV_ERROR_INVALID_PARAMETER;
+
+		memcpy(data->data, mldl_cfg->mpu_ram->ram, data->len);
+		break;
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+
+/*******************************************************************************
+ *******************************************************************************
+ * Exported functions
+ *******************************************************************************
+ ******************************************************************************/
+
+/**
+ * Initializes the pdata structure to defaults.
+ *
+ * Opens the device to read silicon revision, product id and whoami.
+ *
+ * @mldl_cfg
+ *          The internal device configuration data structure.
+ * @mlsl_handle
+ *          The serial communication handle.
+ *
+ * @return INV_SUCCESS if silicon revision, product id and woami are supported
+ *         by this software.
+ */
+int inv_mpu_open(struct mldl_cfg *mldl_cfg,
+		 void *gyro_handle,
+		 void *accel_handle,
+		 void *compass_handle, void *pressure_handle)
+{
+	int result;
+	void *slave_handle[EXT_SLAVE_NUM_TYPES];
+	int ii;
+
+	/* Default is Logic HIGH, pushpull, latch disabled, anyread to clear */
+	ii = 0;
+	mldl_cfg->inv_mpu_cfg->ignore_system_suspend = false;
+	mldl_cfg->mpu_gyro_cfg->int_config = BIT_DMP_INT_EN;
+	mldl_cfg->mpu_gyro_cfg->clk_src = MPU_CLK_SEL_PLLGYROZ;
+	mldl_cfg->mpu_gyro_cfg->lpf = MPU_FILTER_42HZ;
+	mldl_cfg->mpu_gyro_cfg->full_scale = MPU_FS_2000DPS;
+	mldl_cfg->mpu_gyro_cfg->divider = 4;
+	mldl_cfg->mpu_gyro_cfg->dmp_enable = 1;
+	mldl_cfg->mpu_gyro_cfg->fifo_enable = 1;
+	mldl_cfg->mpu_gyro_cfg->ext_sync = 0;
+	mldl_cfg->mpu_gyro_cfg->dmp_cfg1 = 0;
+	mldl_cfg->mpu_gyro_cfg->dmp_cfg2 = 0;
+	mldl_cfg->inv_mpu_state->status =
+		MPU_DMP_IS_SUSPENDED |
+		MPU_GYRO_IS_SUSPENDED |
+		MPU_ACCEL_IS_SUSPENDED |
+		MPU_COMPASS_IS_SUSPENDED |
+		MPU_PRESSURE_IS_SUSPENDED |
+		MPU_DEVICE_IS_SUSPENDED;
+	mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 0;
+
+	slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
+	slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
+	slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
+	slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
+
+	if (mldl_cfg->mpu_chip_info->addr == 0) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	/*
+	 * Reset,
+	 * Take the DMP out of sleep, and
+	 * read the product_id, sillicon rev and whoami
+	 */
+	mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_BYPASSED;
+	result = mpu3050_pwr_mgmt(mldl_cfg, gyro_handle, RESET, 0, 0, 0, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_get_silicon_rev(mldl_cfg, gyro_handle);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Get the factory temperature compensation offsets */
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_XG_OFFS_TC, 1,
+				 &mldl_cfg->mpu_offsets->tc[0]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_YG_OFFS_TC, 1,
+				 &mldl_cfg->mpu_offsets->tc[1]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_ZG_OFFS_TC, 1,
+				 &mldl_cfg->mpu_offsets->tc[2]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Into bypass mode before sleeping and calling the slaves init */
+	result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, true);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_mpu_set_level_shifter_bit(mldl_cfg, gyro_handle,
+			mldl_cfg->pdata->level_shifter);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+
+#if INV_CACHE_DMP != 0
+	result = mpu3050_pwr_mgmt(mldl_cfg, gyro_handle, 0, SLEEP, 0, 0, 0);
+#endif
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+
+	return result;
+
+}
+
+/**
+ * Close the mpu interface
+ *
+ * @mldl_cfg pointer to the configuration structure
+ * @mlsl_handle pointer to the serial layer handle
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+int inv_mpu_close(struct mldl_cfg *mldl_cfg,
+		  void *gyro_handle,
+		  void *accel_handle,
+		  void *compass_handle,
+		  void *pressure_handle)
+{
+	return 0;
+}
+
+/**
+ *  @brief  resume the MPU device and all the other sensor
+ *          devices from their low power state.
+ *
+ *  @mldl_cfg
+ *              pointer to the configuration structure
+ *  @gyro_handle
+ *              the main file handle to the MPU device.
+ *  @accel_handle
+ *              an handle to the accelerometer device, if sitting
+ *              onto a separate bus. Can match mlsl_handle if
+ *              the accelerometer device operates on the same
+ *              primary bus of MPU.
+ *  @compass_handle
+ *              an handle to the compass device, if sitting
+ *              onto a separate bus. Can match mlsl_handle if
+ *              the compass device operates on the same
+ *              primary bus of MPU.
+ *  @pressure_handle
+ *              an handle to the pressure sensor device, if sitting
+ *              onto a separate bus. Can match mlsl_handle if
+ *              the pressure sensor device operates on the same
+ *              primary bus of MPU.
+ *  @resume_gyro
+ *              whether resuming the gyroscope device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @resume_accel
+ *              whether resuming the accelerometer device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @resume_compass
+ *              whether resuming the compass device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @resume_pressure
+ *              whether resuming the pressure sensor device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @return  INV_SUCCESS or a non-zero error code.
+ */
+int inv_mpu_resume(struct mldl_cfg *mldl_cfg,
+		   void *gyro_handle,
+		   void *accel_handle,
+		   void *compass_handle,
+		   void *pressure_handle,
+		   unsigned long sensors)
+{
+	int result = INV_SUCCESS;
+	int ii;
+	bool resume_slave[EXT_SLAVE_NUM_TYPES];
+	bool resume_dmp = sensors & INV_DMP_PROCESSOR;
+	void *slave_handle[EXT_SLAVE_NUM_TYPES];
+	resume_slave[EXT_SLAVE_TYPE_GYROSCOPE] =
+		(sensors & (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO));
+	resume_slave[EXT_SLAVE_TYPE_ACCEL] =
+		sensors & INV_THREE_AXIS_ACCEL;
+	resume_slave[EXT_SLAVE_TYPE_COMPASS] =
+		sensors & INV_THREE_AXIS_COMPASS;
+	resume_slave[EXT_SLAVE_TYPE_PRESSURE] =
+		sensors & INV_THREE_AXIS_PRESSURE;
+
+	slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
+	slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
+	slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
+	slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
+
+
+	mldl_print_cfg(mldl_cfg);
+
+	/* Skip the Gyro since slave[EXT_SLAVE_TYPE_GYROSCOPE] is NULL */
+	for (ii = EXT_SLAVE_TYPE_ACCEL; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (resume_slave[ii] &&
+		    ((!mldl_cfg->slave[ii]) ||
+			(!mldl_cfg->slave[ii]->resume))) {
+			LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+			return INV_ERROR_INVALID_PARAMETER;
+		}
+	}
+
+	if ((resume_slave[EXT_SLAVE_TYPE_GYROSCOPE] || resume_dmp)
+	    && ((mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_SUSPENDED) ||
+		(mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_CONFIG))) {
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = dmp_stop(mldl_cfg, gyro_handle);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = gyro_resume(mldl_cfg, gyro_handle, sensors);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!mldl_cfg->slave[ii] ||
+		    !mldl_cfg->pdata_slave[ii] ||
+		    !resume_slave[ii] ||
+		    !(mldl_cfg->inv_mpu_state->status & (1 << ii)))
+			continue;
+
+		if (EXT_SLAVE_BUS_SECONDARY ==
+		    mldl_cfg->pdata_slave[ii]->bus) {
+			result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle,
+						    true);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+		result = mldl_cfg->slave[ii]->resume(slave_handle[ii],
+						mldl_cfg->slave[ii],
+						mldl_cfg->pdata_slave[ii]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		mldl_cfg->inv_mpu_state->status &= ~(1 << ii);
+	}
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (resume_dmp &&
+		    !(mldl_cfg->inv_mpu_state->status & (1 << ii)) &&
+		    mldl_cfg->pdata_slave[ii] &&
+		    EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata_slave[ii]->bus) {
+			result = mpu_set_slave(mldl_cfg,
+					gyro_handle,
+					mldl_cfg->slave[ii],
+					mldl_cfg->pdata_slave[ii],
+					mldl_cfg->slave[ii]->type);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+	}
+
+	/* Turn on the master i2c iterface if necessary */
+	if (resume_dmp) {
+		result = mpu_set_i2c_bypass(
+			mldl_cfg, gyro_handle,
+			!(mldl_cfg->inv_mpu_state->i2c_slaves_enabled));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+		/* Now start */
+		result = dmp_start(mldl_cfg, gyro_handle);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	mldl_cfg->inv_mpu_cfg->requested_sensors = sensors;
+
+	return result;
+}
+
+/**
+ *  @brief  suspend the MPU device and all the other sensor
+ *          devices into their low power state.
+ *  @mldl_cfg
+ *              a pointer to the struct mldl_cfg internal data
+ *              structure.
+ *  @gyro_handle
+ *              the main file handle to the MPU device.
+ *  @accel_handle
+ *              an handle to the accelerometer device, if sitting
+ *              onto a separate bus. Can match gyro_handle if
+ *              the accelerometer device operates on the same
+ *              primary bus of MPU.
+ *  @compass_handle
+ *              an handle to the compass device, if sitting
+ *              onto a separate bus. Can match gyro_handle if
+ *              the compass device operates on the same
+ *              primary bus of MPU.
+ *  @pressure_handle
+ *              an handle to the pressure sensor device, if sitting
+ *              onto a separate bus. Can match gyro_handle if
+ *              the pressure sensor device operates on the same
+ *              primary bus of MPU.
+ *  @accel
+ *              whether suspending the accelerometer device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @compass
+ *              whether suspending the compass device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @pressure
+ *              whether suspending the pressure sensor device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @return  INV_SUCCESS or a non-zero error code.
+ */
+int inv_mpu_suspend(struct mldl_cfg *mldl_cfg,
+		    void *gyro_handle,
+		    void *accel_handle,
+		    void *compass_handle,
+		    void *pressure_handle,
+		    unsigned long sensors)
+{
+	int result = INV_SUCCESS;
+	int ii;
+	struct ext_slave_descr **slave = mldl_cfg->slave;
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	bool suspend_dmp = ((sensors & INV_DMP_PROCESSOR) == INV_DMP_PROCESSOR);
+	bool suspend_slave[EXT_SLAVE_NUM_TYPES];
+	void *slave_handle[EXT_SLAVE_NUM_TYPES];
+
+	suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE] =
+		((sensors & (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO))
+			== (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO));
+	suspend_slave[EXT_SLAVE_TYPE_ACCEL] =
+		((sensors & INV_THREE_AXIS_ACCEL) == INV_THREE_AXIS_ACCEL);
+	suspend_slave[EXT_SLAVE_TYPE_COMPASS] =
+		((sensors & INV_THREE_AXIS_COMPASS) == INV_THREE_AXIS_COMPASS);
+	suspend_slave[EXT_SLAVE_TYPE_PRESSURE] =
+		((sensors & INV_THREE_AXIS_PRESSURE) ==
+			INV_THREE_AXIS_PRESSURE);
+
+	slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
+	slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
+	slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
+	slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
+
+	if (suspend_dmp) {
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = dmp_stop(mldl_cfg, gyro_handle);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	/* Gyro */
+	if (suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE] &&
+	    !(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_SUSPENDED)) {
+		result = mpu3050_pwr_mgmt(
+			mldl_cfg, gyro_handle, 0,
+			suspend_dmp && suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE],
+			(unsigned char)(sensors & INV_X_GYRO),
+			(unsigned char)(sensors & INV_Y_GYRO),
+			(unsigned char)(sensors & INV_Z_GYRO));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		bool is_suspended = mldl_cfg->inv_mpu_state->status & (1 << ii);
+		if (!slave[ii]   || !pdata_slave[ii] ||
+		    is_suspended || !suspend_slave[ii])
+			continue;
+
+		if (EXT_SLAVE_BUS_SECONDARY == pdata_slave[ii]->bus) {
+			result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+		result = slave[ii]->suspend(slave_handle[ii],
+						  slave[ii],
+						  pdata_slave[ii]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		if (EXT_SLAVE_BUS_SECONDARY == pdata_slave[ii]->bus) {
+			result = mpu_set_slave(mldl_cfg, gyro_handle,
+					       NULL, NULL,
+					       slave[ii]->type);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+		mldl_cfg->inv_mpu_state->status |= (1 << ii);
+	}
+
+	/* Re-enable the i2c master if there are configured slaves and DMP */
+	if (!suspend_dmp) {
+		result = mpu_set_i2c_bypass(
+			mldl_cfg, gyro_handle,
+			!(mldl_cfg->inv_mpu_state->i2c_slaves_enabled));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	mldl_cfg->inv_mpu_cfg->requested_sensors = (~sensors) & INV_ALL_SENSORS;
+
+	return result;
+}
+
+int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg,
+		       void *gyro_handle,
+		       void *slave_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data)
+{
+	int result;
+	int bypass_result;
+	int remain_bypassed = true;
+
+	if (NULL == slave || NULL == slave->read) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
+		return INV_ERROR_INVALID_CONFIGURATION;
+	}
+
+	if ((EXT_SLAVE_BUS_SECONDARY == pdata->bus)
+	    && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
+		remain_bypassed = false;
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	result = slave->read(slave_handle, slave, pdata, data);
+
+	if (!remain_bypassed) {
+		bypass_result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
+		if (bypass_result) {
+			LOG_RESULT_LOCATION(bypass_result);
+			return bypass_result;
+		}
+	}
+	return result;
+}
+
+int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg,
+			 void *gyro_handle,
+			 void *slave_handle,
+			 struct ext_slave_config *data,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	int remain_bypassed = true;
+
+	if (NULL == slave || NULL == slave->config) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
+		return INV_ERROR_INVALID_CONFIGURATION;
+	}
+
+	if (data->apply && (EXT_SLAVE_BUS_SECONDARY == pdata->bus)
+	    && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
+		remain_bypassed = false;
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	result = slave->config(slave_handle, slave, pdata, data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (!remain_bypassed) {
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	return result;
+}
+
+int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg,
+			     void *gyro_handle,
+			     void *slave_handle,
+			     struct ext_slave_config *data,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata)
+{
+	int result;
+	int remain_bypassed = true;
+
+	if (NULL == slave || NULL == slave->get_config) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
+		return INV_ERROR_INVALID_CONFIGURATION;
+	}
+
+	if (data->apply && (EXT_SLAVE_BUS_SECONDARY == pdata->bus)
+	    && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
+		remain_bypassed = false;
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	result = slave->get_config(slave_handle, slave, pdata, data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (!remain_bypassed) {
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	return result;
+}
+
+/**
+ * @}
+ */
diff --git a/drivers/misc/inv_mpu/mpu3050/mldl_print_cfg.c b/drivers/misc/inv_mpu/mpu3050/mldl_print_cfg.c
new file mode 100644
index 000000000000..e2b8d30cebaa
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050/mldl_print_cfg.c
@@ -0,0 +1,137 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup MLDL
+ *
+ *  @{
+ *      @file   mldl_print_cfg.c
+ *      @brief  The Motion Library Driver Layer.
+ */
+
+#include <stddef.h>
+#include "mldl_cfg.h"
+#include "mlsl.h"
+#include "linux/mpu.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "mldl_print_cfg:"
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 1
+
+void mldl_print_cfg(struct mldl_cfg *mldl_cfg)
+{
+	struct mpu_gyro_cfg	*mpu_gyro_cfg	= mldl_cfg->mpu_gyro_cfg;
+	struct mpu_offsets	*mpu_offsets	= mldl_cfg->mpu_offsets;
+	struct mpu_chip_info	*mpu_chip_info	= mldl_cfg->mpu_chip_info;
+	struct inv_mpu_cfg	*inv_mpu_cfg	= mldl_cfg->inv_mpu_cfg;
+	struct inv_mpu_state	*inv_mpu_state	= mldl_cfg->inv_mpu_state;
+	struct ext_slave_descr	**slave		= mldl_cfg->slave;
+	struct mpu_platform_data *pdata		= mldl_cfg->pdata;
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	int ii;
+
+	/* mpu_gyro_cfg */
+	MPL_LOGV("int_config     = %02x\n", mpu_gyro_cfg->int_config);
+	MPL_LOGV("ext_sync       = %02x\n", mpu_gyro_cfg->ext_sync);
+	MPL_LOGV("full_scale     = %02x\n", mpu_gyro_cfg->full_scale);
+	MPL_LOGV("lpf            = %02x\n", mpu_gyro_cfg->lpf);
+	MPL_LOGV("clk_src        = %02x\n", mpu_gyro_cfg->clk_src);
+	MPL_LOGV("divider        = %02x\n", mpu_gyro_cfg->divider);
+	MPL_LOGV("dmp_enable     = %02x\n", mpu_gyro_cfg->dmp_enable);
+	MPL_LOGV("fifo_enable    = %02x\n", mpu_gyro_cfg->fifo_enable);
+	MPL_LOGV("dmp_cfg1       = %02x\n", mpu_gyro_cfg->dmp_cfg1);
+	MPL_LOGV("dmp_cfg2       = %02x\n", mpu_gyro_cfg->dmp_cfg2);
+	/* mpu_offsets */
+	MPL_LOGV("tc[0]      = %02x\n", mpu_offsets->tc[0]);
+	MPL_LOGV("tc[1]      = %02x\n", mpu_offsets->tc[1]);
+	MPL_LOGV("tc[2]      = %02x\n", mpu_offsets->tc[2]);
+	MPL_LOGV("gyro[0]    = %04x\n", mpu_offsets->gyro[0]);
+	MPL_LOGV("gyro[1]    = %04x\n", mpu_offsets->gyro[1]);
+	MPL_LOGV("gyro[2]    = %04x\n", mpu_offsets->gyro[2]);
+
+	/* mpu_chip_info */
+	MPL_LOGV("addr            = %02x\n", mldl_cfg->mpu_chip_info->addr);
+
+	MPL_LOGV("silicon_revision = %02x\n", mpu_chip_info->silicon_revision);
+	MPL_LOGV("product_revision = %02x\n", mpu_chip_info->product_revision);
+	MPL_LOGV("product_id       = %02x\n", mpu_chip_info->product_id);
+	MPL_LOGV("gyro_sens_trim   = %02x\n", mpu_chip_info->gyro_sens_trim);
+
+	MPL_LOGV("requested_sensors = %04x\n", inv_mpu_cfg->requested_sensors);
+	MPL_LOGV("ignore_system_suspend= %04x\n",
+		inv_mpu_cfg->ignore_system_suspend);
+	MPL_LOGV("status = %04x\n", inv_mpu_state->status);
+	MPL_LOGV("i2c_slaves_enabled= %04x\n",
+		inv_mpu_state->i2c_slaves_enabled);
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!slave[ii])
+			continue;
+		MPL_LOGV("SLAVE %d:\n", ii);
+		MPL_LOGV("    suspend  = %02x\n", (int)slave[ii]->suspend);
+		MPL_LOGV("    resume   = %02x\n", (int)slave[ii]->resume);
+		MPL_LOGV("    read     = %02x\n", (int)slave[ii]->read);
+		MPL_LOGV("    type     = %02x\n", slave[ii]->type);
+		MPL_LOGV("    reg      = %02x\n", slave[ii]->read_reg);
+		MPL_LOGV("    len      = %02x\n", slave[ii]->read_len);
+		MPL_LOGV("    endian   = %02x\n", slave[ii]->endian);
+		MPL_LOGV("    range.mantissa= %02x\n",
+			slave[ii]->range.mantissa);
+		MPL_LOGV("    range.fraction= %02x\n",
+			slave[ii]->range.fraction);
+	}
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!pdata_slave[ii])
+			continue;
+		MPL_LOGV("PDATA_SLAVE[%d]\n", ii);
+		MPL_LOGV("    irq        = %02x\n", pdata_slave[ii]->irq);
+		MPL_LOGV("    adapt_num  = %02x\n", pdata_slave[ii]->adapt_num);
+		MPL_LOGV("    bus        = %02x\n", pdata_slave[ii]->bus);
+		MPL_LOGV("    address    = %02x\n", pdata_slave[ii]->address);
+		MPL_LOGV("    orientation=\n"
+			"                            %2d %2d %2d\n"
+			"                            %2d %2d %2d\n"
+			"                            %2d %2d %2d\n",
+			pdata_slave[ii]->orientation[0],
+			pdata_slave[ii]->orientation[1],
+			pdata_slave[ii]->orientation[2],
+			pdata_slave[ii]->orientation[3],
+			pdata_slave[ii]->orientation[4],
+			pdata_slave[ii]->orientation[5],
+			pdata_slave[ii]->orientation[6],
+			pdata_slave[ii]->orientation[7],
+			pdata_slave[ii]->orientation[8]);
+	}
+
+	MPL_LOGV("pdata->int_config         = %02x\n", pdata->int_config);
+	MPL_LOGV("pdata->level_shifter      = %02x\n", pdata->level_shifter);
+	MPL_LOGV("pdata->orientation        =\n"
+		 "                            %2d %2d %2d\n"
+		 "                            %2d %2d %2d\n"
+		 "                            %2d %2d %2d\n",
+		 pdata->orientation[0], pdata->orientation[1],
+		 pdata->orientation[2], pdata->orientation[3],
+		 pdata->orientation[4], pdata->orientation[5],
+		 pdata->orientation[6], pdata->orientation[7],
+		 pdata->orientation[8]);
+}
diff --git a/drivers/misc/inv_mpu/mpu3050/mlsl-kernel.c b/drivers/misc/inv_mpu/mpu3050/mlsl-kernel.c
new file mode 100644
index 000000000000..19adf5182c00
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050/mlsl-kernel.c
@@ -0,0 +1,420 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+#include "mlsl.h"
+#include <linux/i2c.h>
+#include "log.h"
+#include "mpu3050.h"
+
+static int inv_i2c_write(struct i2c_adapter *i2c_adap,
+			    unsigned char address,
+			    unsigned int len, unsigned char const *data)
+{
+	struct i2c_msg msgs[1];
+	int res;
+
+	if (!data || !i2c_adap) {
+		LOG_RESULT_LOCATION(-EINVAL);
+		return -EINVAL;
+	}
+
+	msgs[0].addr = address;
+	msgs[0].flags = 0;	/* write */
+	msgs[0].buf = (unsigned char *)data;
+	msgs[0].len = len;
+
+	res = i2c_transfer(i2c_adap, msgs, 1);
+	if (res < 1) {
+		if (res == 0)
+			res = -EIO;
+		LOG_RESULT_LOCATION(res);
+		return res;
+	} else
+		return 0;
+}
+
+static int inv_i2c_write_register(struct i2c_adapter *i2c_adap,
+				     unsigned char address,
+				     unsigned char reg, unsigned char value)
+{
+	unsigned char data[2];
+
+	data[0] = reg;
+	data[1] = value;
+	return inv_i2c_write(i2c_adap, address, 2, data);
+}
+
+static int inv_i2c_read(struct i2c_adapter *i2c_adap,
+			   unsigned char address, unsigned char reg,
+			   unsigned int len, unsigned char *data)
+{
+	struct i2c_msg msgs[2];
+	int res;
+
+	if (!data || !i2c_adap) {
+		LOG_RESULT_LOCATION(-EINVAL);
+		return -EINVAL;
+	}
+
+	msgs[0].addr = address;
+	msgs[0].flags = 0;	/* write */
+	msgs[0].buf = &reg;
+	msgs[0].len = 1;
+
+	msgs[1].addr = address;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].buf = data;
+	msgs[1].len = len;
+
+	res = i2c_transfer(i2c_adap, msgs, 2);
+	if (res < 2) {
+		if (res >= 0)
+			res = -EIO;
+		LOG_RESULT_LOCATION(res);
+		return res;
+	} else
+		return 0;
+}
+
+static int mpu_memory_read(struct i2c_adapter *i2c_adap,
+			   unsigned char mpu_addr,
+			   unsigned short mem_addr,
+			   unsigned int len, unsigned char *data)
+{
+	unsigned char bank[2];
+	unsigned char addr[2];
+	unsigned char buf;
+
+	struct i2c_msg msgs[4];
+	int res;
+
+	if (!data || !i2c_adap) {
+		LOG_RESULT_LOCATION(-EINVAL);
+		return -EINVAL;
+	}
+
+	bank[0] = MPUREG_BANK_SEL;
+	bank[1] = mem_addr >> 8;
+
+	addr[0] = MPUREG_MEM_START_ADDR;
+	addr[1] = mem_addr & 0xFF;
+
+	buf = MPUREG_MEM_R_W;
+
+	/* write message */
+	msgs[0].addr = mpu_addr;
+	msgs[0].flags = 0;
+	msgs[0].buf = bank;
+	msgs[0].len = sizeof(bank);
+
+	msgs[1].addr = mpu_addr;
+	msgs[1].flags = 0;
+	msgs[1].buf = addr;
+	msgs[1].len = sizeof(addr);
+
+	msgs[2].addr = mpu_addr;
+	msgs[2].flags = 0;
+	msgs[2].buf = &buf;
+	msgs[2].len = 1;
+
+	msgs[3].addr = mpu_addr;
+	msgs[3].flags = I2C_M_RD;
+	msgs[3].buf = data;
+	msgs[3].len = len;
+
+	res = i2c_transfer(i2c_adap, msgs, 4);
+	if (res != 4) {
+		if (res >= 0)
+			res = -EIO;
+		LOG_RESULT_LOCATION(res);
+		return res;
+	} else
+		return 0;
+}
+
+static int mpu_memory_write(struct i2c_adapter *i2c_adap,
+			    unsigned char mpu_addr,
+			    unsigned short mem_addr,
+			    unsigned int len, unsigned char const *data)
+{
+	unsigned char bank[2];
+	unsigned char addr[2];
+	unsigned char buf[513];
+
+	struct i2c_msg msgs[3];
+	int res;
+
+	if (!data || !i2c_adap) {
+		LOG_RESULT_LOCATION(-EINVAL);
+		return -EINVAL;
+	}
+	if (len >= (sizeof(buf) - 1)) {
+		LOG_RESULT_LOCATION(-ENOMEM);
+		return -ENOMEM;
+	}
+
+	bank[0] = MPUREG_BANK_SEL;
+	bank[1] = mem_addr >> 8;
+
+	addr[0] = MPUREG_MEM_START_ADDR;
+	addr[1] = mem_addr & 0xFF;
+
+	buf[0] = MPUREG_MEM_R_W;
+	memcpy(buf + 1, data, len);
+
+	/* write message */
+	msgs[0].addr = mpu_addr;
+	msgs[0].flags = 0;
+	msgs[0].buf = bank;
+	msgs[0].len = sizeof(bank);
+
+	msgs[1].addr = mpu_addr;
+	msgs[1].flags = 0;
+	msgs[1].buf = addr;
+	msgs[1].len = sizeof(addr);
+
+	msgs[2].addr = mpu_addr;
+	msgs[2].flags = 0;
+	msgs[2].buf = (unsigned char *)buf;
+	msgs[2].len = len + 1;
+
+	res = i2c_transfer(i2c_adap, msgs, 3);
+	if (res != 3) {
+		if (res >= 0)
+			res = -EIO;
+		LOG_RESULT_LOCATION(res);
+		return res;
+	} else
+		return 0;
+}
+
+int inv_serial_single_write(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned char register_addr,
+	unsigned char data)
+{
+	return inv_i2c_write_register((struct i2c_adapter *)sl_handle,
+				      slave_addr, register_addr, data);
+}
+EXPORT_SYMBOL(inv_serial_single_write);
+
+int inv_serial_write(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short length,
+	unsigned char const *data)
+{
+	int result;
+	const unsigned short data_length = length - 1;
+	const unsigned char start_reg_addr = data[0];
+	unsigned char i2c_write[SERIAL_MAX_TRANSFER_SIZE + 1];
+	unsigned short bytes_written = 0;
+
+	while (bytes_written < data_length) {
+		unsigned short this_len = min(SERIAL_MAX_TRANSFER_SIZE,
+					     data_length - bytes_written);
+		if (bytes_written == 0) {
+			result = inv_i2c_write((struct i2c_adapter *)
+					       sl_handle, slave_addr,
+					       1 + this_len, data);
+		} else {
+			/* manually increment register addr between chunks */
+			i2c_write[0] = start_reg_addr + bytes_written;
+			memcpy(&i2c_write[1], &data[1 + bytes_written],
+				this_len);
+			result = inv_i2c_write((struct i2c_adapter *)
+					       sl_handle, slave_addr,
+					       1 + this_len, i2c_write);
+		}
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		bytes_written += this_len;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(inv_serial_write);
+
+int inv_serial_read(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned char register_addr,
+	unsigned short length,
+	unsigned char *data)
+{
+	int result;
+	unsigned short bytes_read = 0;
+
+	if ((slave_addr & 0x7E) == DEFAULT_MPU_SLAVEADDR
+		&& (register_addr == MPUREG_FIFO_R_W ||
+		    register_addr == MPUREG_MEM_R_W)) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	while (bytes_read < length) {
+		unsigned short this_len =
+		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
+		result = inv_i2c_read((struct i2c_adapter *)sl_handle,
+				      slave_addr, register_addr + bytes_read,
+				      this_len, &data[bytes_read]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		bytes_read += this_len;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(inv_serial_read);
+
+int inv_serial_write_mem(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short mem_addr,
+	unsigned short length,
+	unsigned char const *data)
+{
+	int result;
+	unsigned short bytes_written = 0;
+
+	if ((mem_addr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
+		pr_err("memory read length (%d B) extends beyond its"
+		       " limits (%d) if started at location %d\n", length,
+		       MPU_MEM_BANK_SIZE, mem_addr & 0xFF);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+	while (bytes_written < length) {
+		unsigned short this_len =
+		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_written);
+		result = mpu_memory_write((struct i2c_adapter *)sl_handle,
+					  slave_addr, mem_addr + bytes_written,
+					  this_len, &data[bytes_written]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		bytes_written += this_len;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(inv_serial_write_mem);
+
+int inv_serial_read_mem(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short mem_addr,
+	unsigned short length,
+	unsigned char *data)
+{
+	int result;
+	unsigned short bytes_read = 0;
+
+	if ((mem_addr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
+		printk
+		    ("memory read length (%d B) extends beyond its limits (%d) "
+		     "if started at location %d\n", length,
+		     MPU_MEM_BANK_SIZE, mem_addr & 0xFF);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+	while (bytes_read < length) {
+		unsigned short this_len =
+		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
+		result =
+		    mpu_memory_read((struct i2c_adapter *)sl_handle,
+				    slave_addr, mem_addr + bytes_read,
+				    this_len, &data[bytes_read]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		bytes_read += this_len;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(inv_serial_read_mem);
+
+int inv_serial_write_fifo(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short length,
+	unsigned char const *data)
+{
+	int result;
+	unsigned char i2c_write[SERIAL_MAX_TRANSFER_SIZE + 1];
+	unsigned short bytes_written = 0;
+
+	if (length > FIFO_HW_SIZE) {
+		printk(KERN_ERR
+		       "maximum fifo write length is %d\n", FIFO_HW_SIZE);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+	while (bytes_written < length) {
+		unsigned short this_len =
+		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_written);
+		i2c_write[0] = MPUREG_FIFO_R_W;
+		memcpy(&i2c_write[1], &data[bytes_written], this_len);
+		result = inv_i2c_write((struct i2c_adapter *)sl_handle,
+				       slave_addr, this_len + 1, i2c_write);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		bytes_written += this_len;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(inv_serial_write_fifo);
+
+int inv_serial_read_fifo(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short length,
+	unsigned char *data)
+{
+	int result;
+	unsigned short bytes_read = 0;
+
+	if (length > FIFO_HW_SIZE) {
+		printk(KERN_ERR
+		       "maximum fifo read length is %d\n", FIFO_HW_SIZE);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+	while (bytes_read < length) {
+		unsigned short this_len =
+		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
+		result = inv_i2c_read((struct i2c_adapter *)sl_handle,
+				      slave_addr, MPUREG_FIFO_R_W, this_len,
+				      &data[bytes_read]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		bytes_read += this_len;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(inv_serial_read_fifo);
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/mpu3050/mpu-dev.c b/drivers/misc/inv_mpu/mpu3050/mpu-dev.c
new file mode 100644
index 000000000000..c98ed3b4a80e
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu3050/mpu-dev.c
@@ -0,0 +1,1250 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include <linux/pm.h>
+#include <linux/mutex.h>
+#include <linux/suspend.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+
+#include "mpuirq.h"
+#include "slaveirq.h"
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#include <linux/mpu.h>
+
+
+/* Platform data for the MPU */
+struct mpu_private_data {
+	struct miscdevice dev;
+	struct i2c_client *client;
+
+	/* mldl_cfg data */
+	struct mldl_cfg mldl_cfg;
+	struct mpu_ram		mpu_ram;
+	struct mpu_gyro_cfg	mpu_gyro_cfg;
+	struct mpu_offsets	mpu_offsets;
+	struct mpu_chip_info	mpu_chip_info;
+	struct inv_mpu_cfg	inv_mpu_cfg;
+	struct inv_mpu_state	inv_mpu_state;
+
+	struct mutex mutex;
+	wait_queue_head_t mpu_event_wait;
+	struct completion completion;
+	struct timer_list timeout;
+	struct notifier_block nb;
+	struct mpuirq_data mpu_pm_event;
+	int response_timeout;	/* In seconds */
+	unsigned long event;
+	int pid;
+	struct module *slave_modules[EXT_SLAVE_NUM_TYPES];
+};
+
+struct mpu_private_data *mpu_private_data;
+
+static void mpu_pm_timeout(u_long data)
+{
+	struct mpu_private_data *mpu = (struct mpu_private_data *)data;
+	struct i2c_client *client = mpu->client;
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+	complete(&mpu->completion);
+}
+
+static int mpu_pm_notifier_callback(struct notifier_block *nb,
+				    unsigned long event, void *unused)
+{
+	struct mpu_private_data *mpu =
+	    container_of(nb, struct mpu_private_data, nb);
+	struct i2c_client *client = mpu->client;
+	struct timeval event_time;
+	dev_dbg(&client->adapter->dev, "%s: %ld\n", __func__, event);
+
+	/* Prevent the file handle from being closed before we initialize
+	   the completion event */
+	mutex_lock(&mpu->mutex);
+	if (!(mpu->pid) ||
+	    (event != PM_SUSPEND_PREPARE && event != PM_POST_SUSPEND)) {
+		mutex_unlock(&mpu->mutex);
+		return NOTIFY_OK;
+	}
+
+	if (event == PM_SUSPEND_PREPARE)
+		mpu->event = MPU_PM_EVENT_SUSPEND_PREPARE;
+	if (event == PM_POST_SUSPEND)
+		mpu->event = MPU_PM_EVENT_POST_SUSPEND;
+
+	do_gettimeofday(&event_time);
+	mpu->mpu_pm_event.interruptcount++;
+	mpu->mpu_pm_event.irqtime =
+	    (((long long)event_time.tv_sec) << 32) + event_time.tv_usec;
+	mpu->mpu_pm_event.data_type = MPUIRQ_DATA_TYPE_PM_EVENT;
+	mpu->mpu_pm_event.data = mpu->event;
+
+	if (mpu->response_timeout > 0) {
+		mpu->timeout.expires = jiffies + mpu->response_timeout * HZ;
+		add_timer(&mpu->timeout);
+	}
+	INIT_COMPLETION(mpu->completion);
+	mutex_unlock(&mpu->mutex);
+
+	wake_up_interruptible(&mpu->mpu_event_wait);
+	wait_for_completion(&mpu->completion);
+	del_timer_sync(&mpu->timeout);
+	dev_dbg(&client->adapter->dev, "%s: %ld DONE\n", __func__, event);
+	return NOTIFY_OK;
+}
+
+static int mpu_dev_open(struct inode *inode, struct file *file)
+{
+	struct mpu_private_data *mpu =
+	    container_of(file->private_data, struct mpu_private_data, dev);
+	struct i2c_client *client = mpu->client;
+	int result;
+	int ii;
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+	dev_dbg(&client->adapter->dev, "current->pid %d\n", current->pid);
+
+	result = mutex_lock_interruptible(&mpu->mutex);
+	if (mpu->pid) {
+		mutex_unlock(&mpu->mutex);
+		return -EBUSY;
+	}
+	mpu->pid = current->pid;
+
+	/* Reset the sensors to the default */
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"%s: mutex_lock_interruptible returned %d\n",
+			__func__, result);
+		return result;
+	}
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++)
+		__module_get(mpu->slave_modules[ii]);
+
+	mutex_unlock(&mpu->mutex);
+	return 0;
+}
+
+/* close function - called when the "file" /dev/mpu is closed in userspace   */
+static int mpu_release(struct inode *inode, struct file *file)
+{
+	struct mpu_private_data *mpu =
+	    container_of(file->private_data, struct mpu_private_data, dev);
+	struct i2c_client *client = mpu->client;
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	int result = 0;
+	int ii;
+	struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!pdata_slave[ii])
+			slave_adapter[ii] = NULL;
+		else
+			slave_adapter[ii] =
+				i2c_get_adapter(pdata_slave[ii]->adapt_num);
+	}
+	slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+	mutex_lock(&mpu->mutex);
+	mldl_cfg->inv_mpu_cfg->requested_sensors = 0;
+	result = inv_mpu_suspend(mldl_cfg,
+				slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+				slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+				slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+				slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+				INV_ALL_SENSORS);
+	mpu->pid = 0;
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++)
+		module_put(mpu->slave_modules[ii]);
+
+	mutex_unlock(&mpu->mutex);
+	complete(&mpu->completion);
+	dev_dbg(&client->adapter->dev, "mpu_release\n");
+
+	return result;
+}
+
+/* read function called when from /dev/mpu is read.  Read from the FIFO */
+static ssize_t mpu_read(struct file *file,
+			char __user *buf, size_t count, loff_t *offset)
+{
+	struct mpu_private_data *mpu =
+	    container_of(file->private_data, struct mpu_private_data, dev);
+	struct i2c_client *client = mpu->client;
+	size_t len = sizeof(mpu->mpu_pm_event) + sizeof(unsigned long);
+	int err;
+
+	if (!mpu->event && (!(file->f_flags & O_NONBLOCK)))
+		wait_event_interruptible(mpu->mpu_event_wait, mpu->event);
+
+	if (!mpu->event || !buf
+	    || count < sizeof(mpu->mpu_pm_event))
+		return 0;
+
+	err = copy_to_user(buf, &mpu->mpu_pm_event, sizeof(mpu->mpu_pm_event));
+	if (err) {
+		dev_err(&client->adapter->dev,
+			"Copy to user returned %d\n", err);
+		return -EFAULT;
+	}
+	mpu->event = 0;
+	return len;
+}
+
+static unsigned int mpu_poll(struct file *file, struct poll_table_struct *poll)
+{
+	struct mpu_private_data *mpu =
+	    container_of(file->private_data, struct mpu_private_data, dev);
+	int mask = 0;
+
+	poll_wait(file, &mpu->mpu_event_wait, poll);
+	if (mpu->event)
+		mask |= POLLIN | POLLRDNORM;
+	return mask;
+}
+
+static int mpu_dev_ioctl_get_ext_slave_platform_data(
+	struct i2c_client *client,
+	struct ext_slave_platform_data __user *arg)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(client);
+	struct ext_slave_platform_data *pdata_slave;
+	struct ext_slave_platform_data local_pdata_slave;
+
+	if (copy_from_user(&local_pdata_slave, arg, sizeof(local_pdata_slave)))
+		return -EFAULT;
+
+	if (local_pdata_slave.type >= EXT_SLAVE_NUM_TYPES)
+		return -EINVAL;
+
+	pdata_slave = mpu->mldl_cfg.pdata_slave[local_pdata_slave.type];
+	/* All but private data and irq_data */
+	if (!pdata_slave)
+		return -ENODEV;
+	if (copy_to_user(arg, pdata_slave, sizeof(*pdata_slave)))
+		return -EFAULT;
+	return 0;
+}
+
+static int mpu_dev_ioctl_get_mpu_platform_data(
+	struct i2c_client *client,
+	struct mpu_platform_data __user *arg)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(client);
+	struct mpu_platform_data *pdata = mpu->mldl_cfg.pdata;
+
+	if (copy_to_user(arg, pdata, sizeof(*pdata)))
+		return -EFAULT;
+	return 0;
+}
+
+static int mpu_dev_ioctl_get_ext_slave_descr(
+	struct i2c_client *client,
+	struct ext_slave_descr __user *arg)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(client);
+	struct ext_slave_descr *slave;
+	struct ext_slave_descr local_slave;
+
+	if (copy_from_user(&local_slave, arg, sizeof(local_slave)))
+		return -EFAULT;
+
+	if (local_slave.type >= EXT_SLAVE_NUM_TYPES)
+		return -EINVAL;
+
+	slave = mpu->mldl_cfg.slave[local_slave.type];
+	/* All but private data and irq_data */
+	if (!slave)
+		return -ENODEV;
+	if (copy_to_user(arg, slave, sizeof(*slave)))
+		return -EFAULT;
+	return 0;
+}
+
+
+/**
+ * slave_config() - Pass a requested slave configuration to the slave sensor
+ *
+ * @adapter the adaptor to use to communicate with the slave
+ * @mldl_cfg the mldl configuration structuer
+ * @slave pointer to the slave descriptor
+ * @usr_config The configuration to pass to the slave sensor
+ *
+ * returns 0 or non-zero error code
+ */
+static int inv_mpu_config(struct mldl_cfg *mldl_cfg,
+			void *gyro_adapter,
+			struct ext_slave_config __user *usr_config)
+{
+	int retval = 0;
+	struct ext_slave_config config;
+
+	retval = copy_from_user(&config, usr_config, sizeof(config));
+	if (retval)
+		return -EFAULT;
+
+	if (config.len && config.data) {
+		void *data;
+		data = kmalloc(config.len, GFP_KERNEL);
+		if (!data)
+			return -ENOMEM;
+
+		retval = copy_from_user(data,
+					(void __user *)config.data, config.len);
+		if (retval) {
+			retval = -EFAULT;
+			kfree(data);
+			return retval;
+		}
+		config.data = data;
+	}
+	retval = gyro_config(gyro_adapter, mldl_cfg, &config);
+	kfree(config.data);
+	return retval;
+}
+
+static int inv_mpu_get_config(struct mldl_cfg *mldl_cfg,
+			    void *gyro_adapter,
+			    struct ext_slave_config __user *usr_config)
+{
+	int retval = 0;
+	struct ext_slave_config config;
+	void *user_data;
+
+	retval = copy_from_user(&config, usr_config, sizeof(config));
+	if (retval)
+		return -EFAULT;
+
+	user_data = config.data;
+	if (config.len && config.data) {
+		void *data;
+		data = kmalloc(config.len, GFP_KERNEL);
+		if (!data)
+			return -ENOMEM;
+
+		retval = copy_from_user(data,
+					(void __user *)config.data, config.len);
+		if (retval) {
+			retval = -EFAULT;
+			kfree(data);
+			return retval;
+		}
+		config.data = data;
+	}
+	retval = gyro_get_config(gyro_adapter, mldl_cfg, &config);
+	if (!retval)
+		retval = copy_to_user((unsigned char __user *)user_data,
+				config.data, config.len);
+	kfree(config.data);
+	return retval;
+}
+
+static int slave_config(struct mldl_cfg *mldl_cfg,
+			void *gyro_adapter,
+			void *slave_adapter,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			struct ext_slave_config __user *usr_config)
+{
+	int retval = 0;
+	struct ext_slave_config config;
+	if ((!slave) || (!slave->config))
+		return -ENODEV;
+
+	retval = copy_from_user(&config, usr_config, sizeof(config));
+	if (retval)
+		return -EFAULT;
+
+	if (config.len && config.data) {
+		void *data;
+		data = kmalloc(config.len, GFP_KERNEL);
+		if (!data)
+			return -ENOMEM;
+
+		retval = copy_from_user(data,
+					(void __user *)config.data, config.len);
+		if (retval) {
+			retval = -EFAULT;
+			kfree(data);
+			return retval;
+		}
+		config.data = data;
+	}
+	retval = inv_mpu_slave_config(mldl_cfg, gyro_adapter, slave_adapter,
+				      &config, slave, pdata);
+	kfree(config.data);
+	return retval;
+}
+
+static int slave_get_config(struct mldl_cfg *mldl_cfg,
+			    void *gyro_adapter,
+			    void *slave_adapter,
+			    struct ext_slave_descr *slave,
+			    struct ext_slave_platform_data *pdata,
+			    struct ext_slave_config __user *usr_config)
+{
+	int retval = 0;
+	struct ext_slave_config config;
+	void *user_data;
+	if (!(slave) || !(slave->get_config))
+		return -ENODEV;
+
+	retval = copy_from_user(&config, usr_config, sizeof(config));
+	if (retval)
+		return -EFAULT;
+
+	user_data = config.data;
+	if (config.len && config.data) {
+		void *data;
+		data = kmalloc(config.len, GFP_KERNEL);
+		if (!data)
+			return -ENOMEM;
+
+		retval = copy_from_user(data,
+					(void __user *)config.data, config.len);
+		if (retval) {
+			retval = -EFAULT;
+			kfree(data);
+			return retval;
+		}
+		config.data = data;
+	}
+	retval = inv_mpu_get_slave_config(mldl_cfg, gyro_adapter,
+					  slave_adapter, &config, slave, pdata);
+	if (retval) {
+		kfree(config.data);
+		return retval;
+	}
+	retval = copy_to_user((unsigned char __user *)user_data,
+			      config.data, config.len);
+	kfree(config.data);
+	return retval;
+}
+
+static int inv_slave_read(struct mldl_cfg *mldl_cfg,
+			  void *gyro_adapter,
+			  void *slave_adapter,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata,
+			  void __user *usr_data)
+{
+	int retval;
+	unsigned char *data;
+	data = kzalloc(slave->read_len, GFP_KERNEL);
+	if (!data)
+		return -EFAULT;
+
+	retval = inv_mpu_slave_read(mldl_cfg, gyro_adapter, slave_adapter,
+				    slave, pdata, data);
+
+	if ((!retval) &&
+	    (copy_to_user((unsigned char __user *)usr_data,
+			  data, slave->read_len)))
+		retval = -EFAULT;
+
+	kfree(data);
+	return retval;
+}
+
+static int mpu_handle_mlsl(void *sl_handle,
+			   unsigned char addr,
+			   unsigned int cmd,
+			   struct mpu_read_write __user *usr_msg)
+{
+	int retval = 0;
+	struct mpu_read_write msg;
+	unsigned char *user_data;
+	retval = copy_from_user(&msg, usr_msg, sizeof(msg));
+	if (retval)
+		return -EFAULT;
+
+	user_data = msg.data;
+	if (msg.length && msg.data) {
+		unsigned char *data;
+		data = kmalloc(msg.length, GFP_KERNEL);
+		if (!data)
+			return -ENOMEM;
+
+		retval = copy_from_user(data,
+					(void __user *)msg.data, msg.length);
+		if (retval) {
+			retval = -EFAULT;
+			kfree(data);
+			return retval;
+		}
+		msg.data = data;
+	} else {
+		return -EPERM;
+	}
+
+	switch (cmd) {
+	case MPU_READ:
+		retval = inv_serial_read(sl_handle, addr,
+					 msg.address, msg.length, msg.data);
+		break;
+	case MPU_WRITE:
+		retval = inv_serial_write(sl_handle, addr,
+					  msg.length, msg.data);
+		break;
+	case MPU_READ_MEM:
+		retval = inv_serial_read_mem(sl_handle, addr,
+					     msg.address, msg.length, msg.data);
+		break;
+	case MPU_WRITE_MEM:
+		retval = inv_serial_write_mem(sl_handle, addr,
+					      msg.address, msg.length,
+					      msg.data);
+		break;
+	case MPU_READ_FIFO:
+		retval = inv_serial_read_fifo(sl_handle, addr,
+					      msg.length, msg.data);
+		break;
+	case MPU_WRITE_FIFO:
+		retval = inv_serial_write_fifo(sl_handle, addr,
+					       msg.length, msg.data);
+		break;
+
+	};
+	if (retval) {
+		dev_err(&((struct i2c_adapter *)sl_handle)->dev,
+			"%s: i2c %d error %d\n",
+			__func__, cmd, retval);
+		kfree(msg.data);
+		return retval;
+	}
+	retval = copy_to_user((unsigned char __user *)user_data,
+			      msg.data, msg.length);
+	kfree(msg.data);
+	return retval;
+}
+
+/* ioctl - I/O control */
+static long mpu_dev_ioctl(struct file *file,
+			  unsigned int cmd, unsigned long arg)
+{
+	struct mpu_private_data *mpu =
+	    container_of(file->private_data, struct mpu_private_data, dev);
+	struct i2c_client *client = mpu->client;
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	int retval = 0;
+	struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+	struct ext_slave_descr **slave = mldl_cfg->slave;
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	int ii;
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!pdata_slave[ii])
+			slave_adapter[ii] = NULL;
+		else
+			slave_adapter[ii] =
+				i2c_get_adapter(pdata_slave[ii]->adapt_num);
+	}
+	slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+	retval = mutex_lock_interruptible(&mpu->mutex);
+	if (retval) {
+		dev_err(&client->adapter->dev,
+			"%s: mutex_lock_interruptible returned %d\n",
+			__func__, retval);
+		return retval;
+	}
+
+	switch (cmd) {
+	case MPU_GET_EXT_SLAVE_PLATFORM_DATA:
+		retval = mpu_dev_ioctl_get_ext_slave_platform_data(
+			client,
+			(struct ext_slave_platform_data __user *)arg);
+		break;
+	case MPU_GET_MPU_PLATFORM_DATA:
+		retval = mpu_dev_ioctl_get_mpu_platform_data(
+			client,
+			(struct mpu_platform_data __user *)arg);
+		break;
+	case MPU_GET_EXT_SLAVE_DESCR:
+		retval = mpu_dev_ioctl_get_ext_slave_descr(
+			client,
+			(struct ext_slave_descr __user *)arg);
+		break;
+	case MPU_READ:
+	case MPU_WRITE:
+	case MPU_READ_MEM:
+	case MPU_WRITE_MEM:
+	case MPU_READ_FIFO:
+	case MPU_WRITE_FIFO:
+		retval = mpu_handle_mlsl(
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			mldl_cfg->mpu_chip_info->addr, cmd,
+			(struct mpu_read_write __user *)arg);
+		break;
+	case MPU_CONFIG_GYRO:
+		retval = inv_mpu_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_CONFIG_ACCEL:
+		retval = slave_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+			slave[EXT_SLAVE_TYPE_ACCEL],
+			pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_CONFIG_COMPASS:
+		retval = slave_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+			slave[EXT_SLAVE_TYPE_COMPASS],
+			pdata_slave[EXT_SLAVE_TYPE_COMPASS],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_CONFIG_PRESSURE:
+		retval = slave_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+			slave[EXT_SLAVE_TYPE_PRESSURE],
+			pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_GET_CONFIG_GYRO:
+		retval = inv_mpu_get_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_GET_CONFIG_ACCEL:
+		retval = slave_get_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+			slave[EXT_SLAVE_TYPE_ACCEL],
+			pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_GET_CONFIG_COMPASS:
+		retval = slave_get_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+			slave[EXT_SLAVE_TYPE_COMPASS],
+			pdata_slave[EXT_SLAVE_TYPE_COMPASS],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_GET_CONFIG_PRESSURE:
+		retval = slave_get_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+			slave[EXT_SLAVE_TYPE_PRESSURE],
+			pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_SUSPEND:
+		retval = inv_mpu_suspend(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+			slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+			slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+			arg);
+		break;
+	case MPU_RESUME:
+		retval = inv_mpu_resume(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+			slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+			slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+			arg);
+		break;
+	case MPU_PM_EVENT_HANDLED:
+		dev_dbg(&client->adapter->dev, "%s: %d\n", __func__, cmd);
+		complete(&mpu->completion);
+		break;
+	case MPU_READ_ACCEL:
+		retval = inv_slave_read(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+			slave[EXT_SLAVE_TYPE_ACCEL],
+			pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+			(unsigned char __user *)arg);
+		break;
+	case MPU_READ_COMPASS:
+		retval = inv_slave_read(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+			slave[EXT_SLAVE_TYPE_COMPASS],
+			pdata_slave[EXT_SLAVE_TYPE_COMPASS],
+			(unsigned char __user *)arg);
+		break;
+	case MPU_READ_PRESSURE:
+		retval = inv_slave_read(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+			slave[EXT_SLAVE_TYPE_PRESSURE],
+			pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
+			(unsigned char __user *)arg);
+		break;
+	case MPU_GET_REQUESTED_SENSORS:
+		if (copy_to_user(
+			   (__u32 __user *)arg,
+			   &mldl_cfg->inv_mpu_cfg->requested_sensors,
+			   sizeof(mldl_cfg->inv_mpu_cfg->requested_sensors)))
+			retval = -EFAULT;
+		break;
+	case MPU_SET_REQUESTED_SENSORS:
+		mldl_cfg->inv_mpu_cfg->requested_sensors = arg;
+		break;
+	case MPU_GET_IGNORE_SYSTEM_SUSPEND:
+		if (copy_to_user(
+			(unsigned char __user *)arg,
+			&mldl_cfg->inv_mpu_cfg->ignore_system_suspend,
+			sizeof(mldl_cfg->inv_mpu_cfg->ignore_system_suspend)))
+			retval = -EFAULT;
+		break;
+	case MPU_SET_IGNORE_SYSTEM_SUSPEND:
+		mldl_cfg->inv_mpu_cfg->ignore_system_suspend = arg;
+		break;
+	case MPU_GET_MLDL_STATUS:
+		if (copy_to_user(
+			(unsigned char __user *)arg,
+			&mldl_cfg->inv_mpu_state->status,
+			sizeof(mldl_cfg->inv_mpu_state->status)))
+			retval = -EFAULT;
+		break;
+	case MPU_GET_I2C_SLAVES_ENABLED:
+		if (copy_to_user(
+			(unsigned char __user *)arg,
+			&mldl_cfg->inv_mpu_state->i2c_slaves_enabled,
+			sizeof(mldl_cfg->inv_mpu_state->i2c_slaves_enabled)))
+			retval = -EFAULT;
+		break;
+	default:
+		dev_err(&client->adapter->dev,
+			"%s: Unknown cmd %x, arg %lu\n",
+			__func__, cmd, arg);
+		retval = -EINVAL;
+	};
+
+	mutex_unlock(&mpu->mutex);
+	dev_dbg(&client->adapter->dev, "%s: %08x, %08lx, %d\n",
+		__func__, cmd, arg, retval);
+
+	if (retval > 0)
+		retval = -retval;
+
+	return retval;
+}
+
+void mpu_shutdown(struct i2c_client *client)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	int ii;
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!pdata_slave[ii])
+			slave_adapter[ii] = NULL;
+		else
+			slave_adapter[ii] =
+				i2c_get_adapter(pdata_slave[ii]->adapt_num);
+	}
+	slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+	mutex_lock(&mpu->mutex);
+	(void)inv_mpu_suspend(mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+			slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+			slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+			INV_ALL_SENSORS);
+	mutex_unlock(&mpu->mutex);
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+}
+
+int mpu_dev_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	int ii;
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!pdata_slave[ii])
+			slave_adapter[ii] = NULL;
+		else
+			slave_adapter[ii] =
+				i2c_get_adapter(pdata_slave[ii]->adapt_num);
+	}
+	slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+	mutex_lock(&mpu->mutex);
+	if (!mldl_cfg->inv_mpu_cfg->ignore_system_suspend) {
+		dev_dbg(&client->adapter->dev,
+			"%s: suspending on event %d\n", __func__, mesg.event);
+		(void)inv_mpu_suspend(mldl_cfg,
+				slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+				slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+				slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+				slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+				INV_ALL_SENSORS);
+	} else {
+		dev_dbg(&client->adapter->dev,
+			"%s: Already suspended %d\n", __func__, mesg.event);
+	}
+	mutex_unlock(&mpu->mutex);
+	return 0;
+}
+
+int mpu_dev_resume(struct i2c_client *client)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	int ii;
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!pdata_slave[ii])
+			slave_adapter[ii] = NULL;
+		else
+			slave_adapter[ii] =
+				i2c_get_adapter(pdata_slave[ii]->adapt_num);
+	}
+	slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+	mutex_lock(&mpu->mutex);
+	if (mpu->pid && !mldl_cfg->inv_mpu_cfg->ignore_system_suspend) {
+		(void)inv_mpu_resume(mldl_cfg,
+				slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+				slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+				slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+				slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+				mldl_cfg->inv_mpu_cfg->requested_sensors);
+		dev_dbg(&client->adapter->dev,
+			"%s for pid %d\n", __func__, mpu->pid);
+	}
+	mutex_unlock(&mpu->mutex);
+	return 0;
+}
+
+/* define which file operations are supported */
+static const struct file_operations mpu_fops = {
+	.owner = THIS_MODULE,
+	.read = mpu_read,
+	.poll = mpu_poll,
+	.unlocked_ioctl = mpu_dev_ioctl,
+	.open = mpu_dev_open,
+	.release = mpu_release,
+};
+
+int inv_mpu_register_slave(struct module *slave_module,
+			struct i2c_client *slave_client,
+			struct ext_slave_platform_data *slave_pdata,
+			struct ext_slave_descr *(*get_slave_descr)(void))
+{
+	struct mpu_private_data *mpu = mpu_private_data;
+	struct mldl_cfg *mldl_cfg;
+	struct ext_slave_descr *slave_descr;
+	struct ext_slave_platform_data **pdata_slave;
+	char *irq_name = NULL;
+	int result = 0;
+
+	if (!slave_client || !slave_pdata || !get_slave_descr)
+		return -EINVAL;
+
+	if (!mpu) {
+		dev_err(&slave_client->adapter->dev,
+			"%s: Null mpu_private_data\n", __func__);
+		return -EINVAL;
+	}
+	mldl_cfg    = &mpu->mldl_cfg;
+	pdata_slave = mldl_cfg->pdata_slave;
+	slave_descr = get_slave_descr();
+
+	if (!slave_descr) {
+		dev_err(&slave_client->adapter->dev,
+			"%s: Null ext_slave_descr\n", __func__);
+		return -EINVAL;
+	}
+
+	mutex_lock(&mpu->mutex);
+	if (mpu->pid) {
+		mutex_unlock(&mpu->mutex);
+		return -EBUSY;
+	}
+
+	if (pdata_slave[slave_descr->type]) {
+		result = -EBUSY;
+		goto out_unlock_mutex;
+	}
+
+	slave_pdata->address	= slave_client->addr;
+	slave_pdata->irq	= slave_client->irq;
+	slave_pdata->adapt_num	= i2c_adapter_id(slave_client->adapter);
+
+	dev_info(&slave_client->adapter->dev,
+		"%s: +%s Type %d: Addr: %2x IRQ: %2d, Adapt: %2d\n",
+		__func__,
+		slave_descr->name,
+		slave_descr->type,
+		slave_pdata->address,
+		slave_pdata->irq,
+		slave_pdata->adapt_num);
+
+	switch (slave_descr->type) {
+	case EXT_SLAVE_TYPE_ACCEL:
+		irq_name = "accelirq";
+		break;
+	case EXT_SLAVE_TYPE_COMPASS:
+		irq_name = "compassirq";
+		break;
+	case EXT_SLAVE_TYPE_PRESSURE:
+		irq_name = "pressureirq";
+		break;
+	default:
+		irq_name = "none";
+	};
+	if (slave_descr->init) {
+		result = slave_descr->init(slave_client->adapter,
+					slave_descr,
+					slave_pdata);
+		if (result) {
+			dev_err(&slave_client->adapter->dev,
+				"%s init failed %d\n",
+				slave_descr->name, result);
+			goto out_unlock_mutex;
+		}
+	}
+
+	pdata_slave[slave_descr->type] = slave_pdata;
+	mpu->slave_modules[slave_descr->type] = slave_module;
+	mldl_cfg->slave[slave_descr->type] = slave_descr;
+
+	goto out_unlock_mutex;
+
+out_unlock_mutex:
+	mutex_unlock(&mpu->mutex);
+
+	if (!result && irq_name && (slave_pdata->irq > 0)) {
+		int warn_result;
+		dev_info(&slave_client->adapter->dev,
+			"Installing %s irq using %d\n",
+			irq_name,
+			slave_pdata->irq);
+		warn_result = slaveirq_init(slave_client->adapter,
+					slave_pdata, irq_name);
+		if (result)
+			dev_WARN(&slave_client->adapter->dev,
+				"%s irq assigned error: %d\n",
+				slave_descr->name, warn_result);
+	} else {
+		dev_info(&slave_client->adapter->dev,
+			"%s irq not assigned: %d %d %d\n",
+			slave_descr->name,
+			result, (int)irq_name, slave_pdata->irq);
+	}
+
+	return result;
+}
+EXPORT_SYMBOL(inv_mpu_register_slave);
+
+void inv_mpu_unregister_slave(struct i2c_client *slave_client,
+			struct ext_slave_platform_data *slave_pdata,
+			struct ext_slave_descr *(*get_slave_descr)(void))
+{
+	struct mpu_private_data *mpu = mpu_private_data;
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct ext_slave_descr *slave_descr;
+	int result;
+
+	dev_info(&slave_client->adapter->dev, "%s\n", __func__);
+
+	if (!slave_client || !slave_pdata || !get_slave_descr)
+		return;
+
+	if (slave_pdata->irq)
+		slaveirq_exit(slave_pdata);
+
+	slave_descr = get_slave_descr();
+	if (!slave_descr)
+		return;
+
+	mutex_lock(&mpu->mutex);
+
+	if (slave_descr->exit) {
+		result = slave_descr->exit(slave_client->adapter,
+					slave_descr,
+					slave_pdata);
+		if (result)
+			dev_err(&slave_client->adapter->dev,
+				"Accel exit failed %d\n", result);
+	}
+	mldl_cfg->slave[slave_descr->type] = NULL;
+	mldl_cfg->pdata_slave[slave_descr->type] = NULL;
+	mpu->slave_modules[slave_descr->type] = NULL;
+
+	mutex_unlock(&mpu->mutex);
+
+}
+EXPORT_SYMBOL(inv_mpu_unregister_slave);
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static const struct i2c_device_id mpu_id[] = {
+	{"mpu3050", 0},
+	{"mpu6050", 0},
+	{"mpu6050_no_accel", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, mpu_id);
+
+int mpu_probe(struct i2c_client *client, const struct i2c_device_id *devid)
+{
+	struct mpu_platform_data *pdata;
+	struct mpu_private_data *mpu;
+	struct mldl_cfg *mldl_cfg;
+	int res = 0;
+	int ii = 0;
+	unsigned long irq_flags;
+
+	dev_info(&client->adapter->dev, "%s: %d\n", __func__, ii++);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		res = -ENODEV;
+		goto out_check_functionality_failed;
+	}
+
+	mpu = kzalloc(sizeof(struct mpu_private_data), GFP_KERNEL);
+	if (!mpu) {
+		res = -ENOMEM;
+		goto out_alloc_data_failed;
+	}
+	mldl_cfg = &mpu->mldl_cfg;
+	mldl_cfg->mpu_ram	= &mpu->mpu_ram;
+	mldl_cfg->mpu_gyro_cfg	= &mpu->mpu_gyro_cfg;
+	mldl_cfg->mpu_offsets	= &mpu->mpu_offsets;
+	mldl_cfg->mpu_chip_info	= &mpu->mpu_chip_info;
+	mldl_cfg->inv_mpu_cfg	= &mpu->inv_mpu_cfg;
+	mldl_cfg->inv_mpu_state	= &mpu->inv_mpu_state;
+
+	mldl_cfg->mpu_ram->length = MPU_MEM_NUM_RAM_BANKS * MPU_MEM_BANK_SIZE;
+	mldl_cfg->mpu_ram->ram = kzalloc(mldl_cfg->mpu_ram->length, GFP_KERNEL);
+	if (!mldl_cfg->mpu_ram->ram) {
+		res = -ENOMEM;
+		goto out_alloc_ram_failed;
+	}
+	mpu_private_data = mpu;
+	i2c_set_clientdata(client, mpu);
+	mpu->client = client;
+
+	init_waitqueue_head(&mpu->mpu_event_wait);
+	mutex_init(&mpu->mutex);
+	init_completion(&mpu->completion);
+
+	mpu->response_timeout = 60;	/* Seconds */
+	mpu->timeout.function = mpu_pm_timeout;
+	mpu->timeout.data = (u_long) mpu;
+	init_timer(&mpu->timeout);
+
+	mpu->nb.notifier_call = mpu_pm_notifier_callback;
+	mpu->nb.priority = 0;
+	res = register_pm_notifier(&mpu->nb);
+	if (res) {
+		dev_err(&client->adapter->dev,
+			"Unable to register pm_notifier %d\n", res);
+		goto out_register_pm_notifier_failed;
+	}
+
+	pdata = (struct mpu_platform_data *)client->dev.platform_data;
+	if (!pdata) {
+		dev_WARN(&client->adapter->dev,
+			 "Missing platform data for mpu\n");
+	}
+	mldl_cfg->pdata = pdata;
+
+	mldl_cfg->mpu_chip_info->addr = client->addr;
+	res = inv_mpu_open(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL);
+
+	if (res) {
+		dev_err(&client->adapter->dev,
+			"Unable to open %s %d\n", MPU_NAME, res);
+		res = -ENODEV;
+		goto out_whoami_failed;
+	}
+
+	mpu->dev.minor = MISC_DYNAMIC_MINOR;
+	mpu->dev.name = "mpu";
+	mpu->dev.fops = &mpu_fops;
+	res = misc_register(&mpu->dev);
+	if (res < 0) {
+		dev_err(&client->adapter->dev,
+			"ERROR: misc_register returned %d\n", res);
+		goto out_misc_register_failed;
+	}
+
+	if (client->irq) {
+		dev_info(&client->adapter->dev,
+			 "Installing irq using %d\n", client->irq);
+		if (BIT_ACTL_LOW == ((mldl_cfg->pdata->int_config) & BIT_ACTL))
+			irq_flags = IRQF_TRIGGER_FALLING;
+		else
+			irq_flags = IRQF_TRIGGER_RISING;
+		res = mpuirq_init(client, mldl_cfg, irq_flags);
+
+		if (res)
+			goto out_mpuirq_failed;
+	} else {
+		dev_WARN(&client->adapter->dev,
+			 "Missing %s IRQ\n", MPU_NAME);
+	}
+	return res;
+
+out_mpuirq_failed:
+	misc_deregister(&mpu->dev);
+out_misc_register_failed:
+	inv_mpu_close(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL);
+out_whoami_failed:
+	unregister_pm_notifier(&mpu->nb);
+out_register_pm_notifier_failed:
+	kfree(mldl_cfg->mpu_ram->ram);
+	mpu_private_data = NULL;
+out_alloc_ram_failed:
+	kfree(mpu);
+out_alloc_data_failed:
+out_check_functionality_failed:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, res);
+	return res;
+
+}
+
+static int mpu_remove(struct i2c_client *client)
+{
+	struct mpu_private_data *mpu = i2c_get_clientdata(client);
+	struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	int ii;
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!pdata_slave[ii])
+			slave_adapter[ii] = NULL;
+		else
+			slave_adapter[ii] =
+				i2c_get_adapter(pdata_slave[ii]->adapt_num);
+	}
+
+	slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_close(mldl_cfg,
+		slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+		slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+		slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+		slave_adapter[EXT_SLAVE_TYPE_PRESSURE]);
+
+
+	if (client->irq)
+		mpuirq_exit();
+
+	misc_deregister(&mpu->dev);
+
+	unregister_pm_notifier(&mpu->nb);
+
+	kfree(mpu->mldl_cfg.mpu_ram->ram);
+	kfree(mpu);
+
+	return 0;
+}
+
+static struct i2c_driver mpu_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = mpu_probe,
+	.remove = mpu_remove,
+	.id_table = mpu_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = MPU_NAME,
+		   },
+	.address_list = normal_i2c,
+	.shutdown = mpu_shutdown,	/* optional */
+	.suspend = mpu_dev_suspend,	/* optional */
+	.resume = mpu_dev_resume,	/* optional */
+
+};
+
+static int __init mpu_init(void)
+{
+	int res = i2c_add_driver(&mpu_driver);
+	pr_info("%s: Probe name %s\n", __func__, MPU_NAME);
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit mpu_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&mpu_driver);
+}
+
+module_init(mpu_init);
+module_exit(mpu_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("User space character device interface for MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS(MPU_NAME);
diff --git a/drivers/misc/inv_mpu/mpu6050/Makefile b/drivers/misc/inv_mpu/mpu6050/Makefile
new file mode 100644
index 000000000000..a93aa97a6997
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/Makefile
@@ -0,0 +1,18 @@
+
+# Kernel makefile for motions sensors
+#
+#
+
+obj-$(CONFIG_MPU_SENSORS_MPU6050B1) += mpu6050b1.o
+
+ccflags-y := -DMPU_CURRENT_BUILD_MPU6050B1
+
+mpu6050b1-objs += mldl_cfg.o
+mpu6050b1-objs += mldl_print_cfg.o
+mpu6050b1-objs += mlsl-kernel.o
+mpu6050b1-objs += mpu-dev.o
+mpu6050b1-objs += ../accel/mpu6050.o
+
+EXTRA_CFLAGS += -Idrivers/misc/inv_mpu
+EXTRA_CFLAGS += -D__C99_DESIGNATED_INITIALIZER
+EXTRA_CFLAGS += -DINV_CACHE_DMP=1
diff --git a/drivers/misc/inv_mpu/mpu6050/mldl_cfg.c b/drivers/misc/inv_mpu/mpu6050/mldl_cfg.c
new file mode 100644
index 000000000000..920f4ae8a4b7
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/mldl_cfg.c
@@ -0,0 +1,1926 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup MLDL
+ *
+ *  @{
+ *      @file   mldl_cfg.c
+ *      @brief  The Motion Library Driver Layer.
+ */
+
+/* -------------------------------------------------------------------------- */
+#include <linux/delay.h>
+#include <linux/slab.h>
+
+#include <stddef.h>
+
+#include "mldl_cfg.h"
+#include <linux/mpu.h>
+#include "mpu6050b1.h"
+
+#include "mlsl.h"
+#include "mldl_print_cfg.h"
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "mldl_cfg:"
+
+/* -------------------------------------------------------------------------- */
+
+#define SLEEP   0
+#define WAKE_UP 7
+#define RESET   1
+#define STANDBY 1
+
+/* -------------------------------------------------------------------------- */
+
+/**
+ * @brief Stop the DMP running
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+static int dmp_stop(struct mldl_cfg *mldl_cfg, void *gyro_handle)
+{
+	unsigned char user_ctrl_reg;
+	int result;
+
+	if (mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED)
+		return INV_SUCCESS;
+
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_USER_CTRL, 1, &user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	user_ctrl_reg = (user_ctrl_reg & (~BIT_FIFO_EN)) | BIT_FIFO_RST;
+	user_ctrl_reg = (user_ctrl_reg & (~BIT_DMP_EN)) | BIT_DMP_RST;
+
+	result = inv_serial_single_write(gyro_handle,
+					 mldl_cfg->mpu_chip_info->addr,
+					 MPUREG_USER_CTRL, user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	mldl_cfg->inv_mpu_state->status |= MPU_DMP_IS_SUSPENDED;
+
+	return result;
+}
+
+/**
+ * @brief Starts the DMP running
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+static int dmp_start(struct mldl_cfg *mldl_cfg, void *mlsl_handle)
+{
+	unsigned char user_ctrl_reg;
+	int result;
+
+	if ((!(mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED) &&
+	    mldl_cfg->mpu_gyro_cfg->dmp_enable)
+		||
+	   ((mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED) &&
+		   !mldl_cfg->mpu_gyro_cfg->dmp_enable))
+		return INV_SUCCESS;
+
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_USER_CTRL, 1, &user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_single_write(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_USER_CTRL,
+		((user_ctrl_reg & (~BIT_FIFO_EN))
+			| BIT_FIFO_RST));
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_single_write(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_USER_CTRL, user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_USER_CTRL, 1, &user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	user_ctrl_reg |= BIT_DMP_EN;
+
+	if (mldl_cfg->mpu_gyro_cfg->fifo_enable)
+		user_ctrl_reg |= BIT_FIFO_EN;
+	else
+		user_ctrl_reg &= ~BIT_FIFO_EN;
+
+	user_ctrl_reg |= BIT_DMP_RST;
+
+	result = inv_serial_single_write(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_USER_CTRL, user_ctrl_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	mldl_cfg->inv_mpu_state->status &= ~MPU_DMP_IS_SUSPENDED;
+
+	return result;
+}
+
+/**
+ *  @brief  enables/disables the I2C bypass to an external device
+ *          connected to MPU's secondary I2C bus.
+ *  @param  enable
+ *              Non-zero to enable pass through.
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+static int mpu6050b1_set_i2c_bypass(struct mldl_cfg *mldl_cfg,
+				    void *mlsl_handle, unsigned char enable)
+{
+	unsigned char reg;
+	int result;
+	unsigned char status = mldl_cfg->inv_mpu_state->status;
+	if ((status & MPU_GYRO_IS_BYPASSED && enable) ||
+	    (!(status & MPU_GYRO_IS_BYPASSED) && !enable))
+		return INV_SUCCESS;
+
+	/*---- get current 'USER_CTRL' into b ----*/
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_USER_CTRL, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (!enable) {
+		/* setting int_config with the property flag BIT_BYPASS_EN
+		   should be done by the setup functions */
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_INT_PIN_CFG,
+			(mldl_cfg->pdata->int_config & ~(BIT_BYPASS_EN)));
+		if (!(reg & BIT_I2C_MST_EN)) {
+			result =
+			    inv_serial_single_write(
+				    mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				    MPUREG_USER_CTRL,
+				    (reg | BIT_I2C_MST_EN));
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+	} else if (enable) {
+		if (reg & BIT_AUX_IF_EN) {
+			result =
+			    inv_serial_single_write(
+				    mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				    MPUREG_USER_CTRL,
+				    (reg & (~BIT_I2C_MST_EN)));
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+			/*****************************************
+			 * To avoid hanging the bus we must sleep until all
+			 * slave transactions have been completed.
+			 *  24 bytes max slave reads
+			 *  +1 byte possible extra write
+			 *  +4 max slave address
+			 * ---
+			 *  33 Maximum bytes
+			 *  x9 Approximate bits per byte
+			 * ---
+			 * 297 bits.
+			 * 2.97 ms minimum @ 100kbps
+			 * 0.75 ms minimum @ 400kbps.
+			 *****************************************/
+			msleep(3);
+		}
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_INT_PIN_CFG,
+			(mldl_cfg->pdata->int_config | BIT_BYPASS_EN));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	if (enable)
+		mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_BYPASSED;
+	else
+		mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED;
+
+	return result;
+}
+
+
+
+
+/**
+ *  @brief  enables/disables the I2C bypass to an external device
+ *          connected to MPU's secondary I2C bus.
+ *  @param  enable
+ *              Non-zero to enable pass through.
+ *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
+ */
+static int mpu_set_i2c_bypass(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
+			      unsigned char enable)
+{
+	return mpu6050b1_set_i2c_bypass(mldl_cfg, mlsl_handle, enable);
+}
+
+
+#define NUM_OF_PROD_REVS (ARRAY_SIZE(prod_rev_map))
+
+/* NOTE : when not indicated, product revision
+	  is considered an 'npp'; non production part */
+
+/* produces an unique identifier for each device based on the
+   combination of product version and product revision */
+struct prod_rev_map_t {
+	unsigned short mpl_product_key;
+	unsigned char silicon_rev;
+	unsigned short gyro_trim;
+	unsigned short accel_trim;
+};
+
+/* NOTE: product entries are in chronological order */
+static struct prod_rev_map_t prod_rev_map[] = {
+	/* prod_ver = 0 */
+	{MPL_PROD_KEY(0,  1), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,  2), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,  3), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,  4), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,  5), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,  6), MPU_SILICON_REV_A2, 131, 16384},	/* (A2/C2-1) */
+	/* prod_ver = 1, forced to 0 for MPU6050 A2 */
+	{MPL_PROD_KEY(0,  7), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,  8), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,  9), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0, 10), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0, 11), MPU_SILICON_REV_A2, 131, 16384},	/* (A2/D2-1) */
+	{MPL_PROD_KEY(0, 12), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0, 13), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0, 14), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0, 15), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0, 27), MPU_SILICON_REV_A2, 131, 16384},	/* (A2/D4)   */
+	/* prod_ver = 1 */
+	{MPL_PROD_KEY(1, 16), MPU_SILICON_REV_B1, 131, 16384},	/* (B1/D2-1) */
+	{MPL_PROD_KEY(1, 17), MPU_SILICON_REV_B1, 131, 16384},	/* (B1/D2-2) */
+	{MPL_PROD_KEY(1, 18), MPU_SILICON_REV_B1, 131, 16384},	/* (B1/D2-3) */
+	{MPL_PROD_KEY(1, 19), MPU_SILICON_REV_B1, 131, 16384},	/* (B1/D2-4) */
+	{MPL_PROD_KEY(1, 20), MPU_SILICON_REV_B1, 131, 16384},	/* (B1/D2-5) */
+	{MPL_PROD_KEY(1, 28), MPU_SILICON_REV_B1, 131, 16384},	/* (B1/D4)   */
+	{MPL_PROD_KEY(1,  1), MPU_SILICON_REV_B1, 131, 16384},	/* (B1/E1-1) */
+	{MPL_PROD_KEY(1,  2), MPU_SILICON_REV_B1, 131, 16384},	/* (B1/E1-2) */
+	{MPL_PROD_KEY(1,  3), MPU_SILICON_REV_B1, 131, 16384},	/* (B1/E1-3) */
+	{MPL_PROD_KEY(1,  4), MPU_SILICON_REV_B1, 131, 16384},	/* (B1/E1-4) */
+	{MPL_PROD_KEY(1,  5), MPU_SILICON_REV_B1, 131, 16384},	/* (B1/E1-5) */
+	{MPL_PROD_KEY(1,  6), MPU_SILICON_REV_B1, 131, 16384},	/* (B1/E1-6) */
+	/* prod_ver = 2 */
+	{MPL_PROD_KEY(2,  7), MPU_SILICON_REV_B1, 131, 16384},	/* (B2/E1-1) */
+	{MPL_PROD_KEY(2,  8), MPU_SILICON_REV_B1, 131, 16384},	/* (B2/E1-2) */
+	{MPL_PROD_KEY(2,  9), MPU_SILICON_REV_B1, 131, 16384},	/* (B2/E1-3) */
+	{MPL_PROD_KEY(2, 10), MPU_SILICON_REV_B1, 131, 16384},	/* (B2/E1-4) */
+	{MPL_PROD_KEY(2, 11), MPU_SILICON_REV_B1, 131, 16384},	/* (B2/E1-5) */
+	{MPL_PROD_KEY(2, 12), MPU_SILICON_REV_B1, 131, 16384},	/* (B2/E1-6) */
+	{MPL_PROD_KEY(2, 29), MPU_SILICON_REV_B1, 131, 16384},	/* (B2/D4)   */
+	/* prod_ver = 3 */
+	{MPL_PROD_KEY(3, 30), MPU_SILICON_REV_B1, 131, 16384},	/* (B2/E2)   */
+	/* prod_ver = 4 */
+	{MPL_PROD_KEY(4, 31), MPU_SILICON_REV_B1, 131,  8192},	/* (B2/F1)   */
+	{MPL_PROD_KEY(4,  1), MPU_SILICON_REV_B1, 131,  8192},	/* (B3/F1)   */
+	{MPL_PROD_KEY(4,  3), MPU_SILICON_REV_B1, 131,  8192},	/* (B4/F1)   */
+	/* prod_ver = 5 */
+	{MPL_PROD_KEY(6, 19), MPU_SILICON_REV_B1, 131, 16384},	/* (B5/E2)   */
+	/* prod_ver = 7 */
+	{MPL_PROD_KEY(7, 19), MPU_SILICON_REV_B1, 131, 16384},	/* (B5/E2)   */
+	/* prod_ver = 8 */
+	{MPL_PROD_KEY(8, 19), MPU_SILICON_REV_B1, 131, 16384},	/* (B5/E2)   */
+	/* prod_ver = 9 */
+	{MPL_PROD_KEY(9, 19), MPU_SILICON_REV_B1, 131, 16384},	/* (B5/E2)   */
+	/* prod_ver = 10 */
+	{MPL_PROD_KEY(10, 19), MPU_SILICON_REV_B1, 131, 16384}	/* (B5/E2)   */
+};
+
+/**
+ *  @internal
+ *  @brief  Inverse lookup of the index of an MPL product key .
+ *  @param  key
+ *              the MPL product indentifier also referred to as 'key'.
+ *  @return the index position of the key in the array, -1 if not found.
+ */
+short index_of_key(unsigned short key)
+{
+	int i;
+	for (i = 0; i < NUM_OF_PROD_REVS; i++)
+		if (prod_rev_map[i].mpl_product_key == key)
+			return (short)i;
+	return -1;
+}
+
+/**
+ *  @internal
+ *  @brief  Get the product revision and version for MPU6050 and
+ *          extract all per-part specific information.
+ *          The product version number is read from the PRODUCT_ID register in
+ *          user space register map.
+ *          The product revision number is in read from OTP bank 0, ADDR6[7:2].
+ *          These 2 numbers, combined, provide an unique key to be used to
+ *          retrieve some per-device information such as the silicon revision
+ *          and the gyro and accel sensitivity trim values.
+ *
+ *  @param  mldl_cfg
+ *              a pointer to the mldl config data structure.
+ *  @param  mlsl_handle
+ *              an file handle to the serial communication device the
+ *              device is connected to.
+ *
+ *  @return 0 on success, a non-zero error code otherwise.
+ */
+static int inv_get_silicon_rev_mpu6050(
+		struct mldl_cfg *mldl_cfg, void *mlsl_handle)
+{
+	int result;
+	unsigned char prod_ver = 0x00, prod_rev = 0x00;
+	unsigned char bank =
+	    (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0);
+	unsigned short memAddr = ((bank << 8) | 0x06);
+	unsigned short key;
+	short index;
+	struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
+
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_PRODUCT_ID, 1, &prod_ver);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	prod_ver &= 0xF;
+
+	result = inv_serial_read_mem(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				     memAddr, 1, &prod_rev);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	prod_rev >>= 2;
+
+	/* clean the prefetch and cfg user bank bits */
+	result = inv_serial_single_write(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_BANK_SEL, 0);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	key = MPL_PROD_KEY(prod_ver, prod_rev);
+	if (key == 0) {
+		MPL_LOGE("Product id read as 0 "
+			 "indicates device is either "
+			 "incompatible or an MPU3050\n");
+		return INV_ERROR_INVALID_MODULE;
+	}
+	index = index_of_key(key);
+	if (index == -1 || index >= NUM_OF_PROD_REVS) {
+		MPL_LOGE("Unsupported product key %d in MPL\n", key);
+		return INV_ERROR_INVALID_MODULE;
+	}
+	/* check MPL is compiled for this device */
+	if (prod_rev_map[index].silicon_rev != MPU_SILICON_REV_B1) {
+		MPL_LOGE("MPL compiled for MPU6050B1 support "
+			 "but device is not MPU6050B1 (%d)\n", key);
+		return INV_ERROR_INVALID_MODULE;
+	}
+
+	mpu_chip_info->product_id = prod_ver;
+	mpu_chip_info->product_revision = prod_rev;
+	mpu_chip_info->silicon_revision = prod_rev_map[index].silicon_rev;
+	mpu_chip_info->gyro_sens_trim = prod_rev_map[index].gyro_trim;
+	mpu_chip_info->accel_sens_trim = prod_rev_map[index].accel_trim;
+
+	return result;
+}
+#define inv_get_silicon_rev inv_get_silicon_rev_mpu6050
+
+
+/**
+ *  @brief  Enable / Disable the use MPU's secondary I2C interface level
+ *          shifters.
+ *          When enabled the secondary I2C interface to which the external
+ *          device is connected runs at VDD voltage (main supply).
+ *          When disabled the 2nd interface runs at VDDIO voltage.
+ *          See the device specification for more details.
+ *
+ *  @note   using this API may produce unpredictable results, depending on how
+ *          the MPU and slave device are setup on the target platform.
+ *          Use of this API should entirely be restricted to system
+ *          integrators. Once the correct value is found, there should be no
+ *          need to change the level shifter at runtime.
+ *
+ *  @pre    Must be called after inv_serial_start().
+ *  @note   Typically called before inv_dmp_open().
+ *
+ *  @param[in]  enable:
+ *                  0 to run at VDDIO (default),
+ *                  1 to run at VDD.
+ *
+ *  @return INV_SUCCESS if successfull, a non-zero error code otherwise.
+ */
+static int inv_mpu_set_level_shifter_bit(struct mldl_cfg *mldl_cfg,
+				  void *mlsl_handle, unsigned char enable)
+{
+	int result;
+	unsigned char regval;
+
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_YG_OFFS_TC, 1, &regval);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	if (enable)
+		regval |= BIT_I2C_MST_VDDIO;
+
+	result = inv_serial_single_write(
+		mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_YG_OFFS_TC, regval);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	return INV_SUCCESS;
+}
+
+
+/**
+ * @internal
+ * @brief MPU6050 B1 power management functions.
+ * @param mldl_cfg
+ *          a pointer to the internal mldl_cfg data structure.
+ * @param mlsl_handle
+ *          a file handle to the serial device used to communicate
+ *          with the MPU6050 B1 device.
+ * @param reset
+ *          1 to reset hardware.
+ * @param sensors
+ *          Bitfield of sensors to leave on
+ *
+ * @return 0 on success, a non-zero error code on error.
+ */
+static int mpu60xx_pwr_mgmt(struct mldl_cfg *mldl_cfg,
+				    void *mlsl_handle,
+				    unsigned int reset, unsigned long sensors)
+{
+	unsigned char pwr_mgmt[2];
+	unsigned char pwr_mgmt_prev[2];
+	int result;
+	int sleep = !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL
+				| INV_DMP_PROCESSOR));
+
+	if (reset) {
+		MPL_LOGI("Reset MPU6050 B1\n");
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_PWR_MGMT_1, BIT_H_RESET);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED;
+		msleep(15);
+	}
+
+	/* NOTE : reading both PWR_MGMT_1 and PWR_MGMT_2 for efficiency because
+		  they are accessible even when the device is powered off */
+	result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_PWR_MGMT_1, 2, pwr_mgmt_prev);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	pwr_mgmt[0] = pwr_mgmt_prev[0];
+	pwr_mgmt[1] = pwr_mgmt_prev[1];
+
+	if (sleep) {
+		mldl_cfg->inv_mpu_state->status |= MPU_DEVICE_IS_SUSPENDED;
+		pwr_mgmt[0] |= BIT_SLEEP;
+	} else {
+		mldl_cfg->inv_mpu_state->status &= ~MPU_DEVICE_IS_SUSPENDED;
+		pwr_mgmt[0] &= ~BIT_SLEEP;
+	}
+	if (pwr_mgmt[0] != pwr_mgmt_prev[0]) {
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_PWR_MGMT_1, pwr_mgmt[0]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	pwr_mgmt[1] &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
+	if (!(sensors & INV_X_GYRO))
+		pwr_mgmt[1] |= BIT_STBY_XG;
+	if (!(sensors & INV_Y_GYRO))
+		pwr_mgmt[1] |= BIT_STBY_YG;
+	if (!(sensors & INV_Z_GYRO))
+		pwr_mgmt[1] |= BIT_STBY_ZG;
+
+	if (pwr_mgmt[1] != pwr_mgmt_prev[1]) {
+		result = inv_serial_single_write(
+			mlsl_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_PWR_MGMT_2, pwr_mgmt[1]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	if ((pwr_mgmt[1] & (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) ==
+	    (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) {
+		mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_SUSPENDED;
+	} else {
+		mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_SUSPENDED;
+	}
+
+	return INV_SUCCESS;
+}
+
+
+/**
+ *  @brief  sets the clock source for the gyros.
+ *  @param  mldl_cfg
+ *              a pointer to the struct mldl_cfg data structure.
+ *  @param  gyro_handle
+ *              an handle to the serial device the gyro is assigned to.
+ *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+static int mpu_set_clock_source(void *gyro_handle, struct mldl_cfg *mldl_cfg)
+{
+	int result;
+	unsigned char cur_clk_src;
+	unsigned char reg;
+
+	/* clock source selection */
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_PWR_MGM, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	cur_clk_src = reg & BITS_CLKSEL;
+	reg &= ~BITS_CLKSEL;
+
+
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_PWR_MGM, mldl_cfg->mpu_gyro_cfg->clk_src | reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* ERRATA:
+	   workaroud to switch from any MPU_CLK_SEL_PLLGYROx to
+	   MPU_CLK_SEL_INTERNAL and XGyro is powered up:
+	   1) Select INT_OSC
+	   2) PD XGyro
+	   3) PU XGyro
+	 */
+	if ((cur_clk_src == MPU_CLK_SEL_PLLGYROX
+		 || cur_clk_src == MPU_CLK_SEL_PLLGYROY
+		 || cur_clk_src == MPU_CLK_SEL_PLLGYROZ)
+	    && mldl_cfg->mpu_gyro_cfg->clk_src == MPU_CLK_SEL_INTERNAL
+	    && mldl_cfg->inv_mpu_cfg->requested_sensors & INV_X_GYRO) {
+		unsigned char first_result = INV_SUCCESS;
+		mldl_cfg->inv_mpu_cfg->requested_sensors &= ~INV_X_GYRO;
+		result = mpu60xx_pwr_mgmt(
+			mldl_cfg, gyro_handle,
+			false, mldl_cfg->inv_mpu_cfg->requested_sensors);
+		ERROR_CHECK_FIRST(first_result, result);
+		mldl_cfg->inv_mpu_cfg->requested_sensors |= INV_X_GYRO;
+		result = mpu60xx_pwr_mgmt(
+			mldl_cfg, gyro_handle,
+			false, mldl_cfg->inv_mpu_cfg->requested_sensors);
+		ERROR_CHECK_FIRST(first_result, result);
+		result = first_result;
+	}
+	return result;
+}
+
+/**
+ * Configures the MPU I2C Master
+ *
+ * @mldl_cfg Handle to the configuration data
+ * @gyro_handle handle to the gyro communictation interface
+ * @slave Can be Null if turning off the slave
+ * @slave_pdata Can be null if turning off the slave
+ * @slave_id enum ext_slave_type to determine which index to use
+ *
+ *
+ * This fucntion configures the slaves by:
+ * 1) Setting up the read
+ *    a) Read Register
+ *    b) Read Length
+ * 2) Set up the data trigger (MPU6050 only)
+ *    a) Set trigger write register
+ *    b) Set Trigger write value
+ * 3) Set up the divider (MPU6050 only)
+ * 4) Set the slave bypass mode depending on slave
+ *
+ * returns INV_SUCCESS or non-zero error code
+ */
+
+static int mpu_set_slave_mpu60xx(struct mldl_cfg *mldl_cfg,
+				 void *gyro_handle,
+				 struct ext_slave_descr *slave,
+				 struct ext_slave_platform_data *slave_pdata,
+				 int slave_id)
+{
+	int result;
+	unsigned char reg;
+	/* Slave values */
+	unsigned char slave_reg;
+	unsigned char slave_len;
+	unsigned char slave_endian;
+	unsigned char slave_address;
+	/* Which MPU6050 registers to use */
+	unsigned char addr_reg;
+	unsigned char reg_reg;
+	unsigned char ctrl_reg;
+	/* Which MPU6050 registers to use for the trigger */
+	unsigned char addr_trig_reg;
+	unsigned char reg_trig_reg;
+	unsigned char ctrl_trig_reg;
+
+	unsigned char bits_slave_delay = 0;
+	/* Divide down rate for the Slave, from the mpu rate */
+	unsigned char d0_trig_reg;
+	unsigned char delay_ctrl_orig;
+	unsigned char delay_ctrl;
+	long divider;
+
+	if (NULL == slave || NULL == slave_pdata) {
+		slave_reg = 0;
+		slave_len = 0;
+		slave_endian = 0;
+		slave_address = 0;
+	} else {
+		slave_reg = slave->read_reg;
+		slave_len = slave->read_len;
+		slave_endian = slave->endian;
+		slave_address = slave_pdata->address;
+		slave_address |= BIT_I2C_READ;
+	}
+
+	switch (slave_id) {
+	case EXT_SLAVE_TYPE_ACCEL:
+		addr_reg = MPUREG_I2C_SLV1_ADDR;
+		reg_reg  = MPUREG_I2C_SLV1_REG;
+		ctrl_reg = MPUREG_I2C_SLV1_CTRL;
+		addr_trig_reg = 0;
+		reg_trig_reg  = 0;
+		ctrl_trig_reg = 0;
+		bits_slave_delay = BIT_SLV1_DLY_EN;
+		break;
+	case EXT_SLAVE_TYPE_COMPASS:
+		addr_reg = MPUREG_I2C_SLV0_ADDR;
+		reg_reg  = MPUREG_I2C_SLV0_REG;
+		ctrl_reg = MPUREG_I2C_SLV0_CTRL;
+		addr_trig_reg = MPUREG_I2C_SLV2_ADDR;
+		reg_trig_reg  = MPUREG_I2C_SLV2_REG;
+		ctrl_trig_reg = MPUREG_I2C_SLV2_CTRL;
+		d0_trig_reg   = MPUREG_I2C_SLV2_DO;
+		bits_slave_delay = BIT_SLV2_DLY_EN | BIT_SLV0_DLY_EN;
+		break;
+	case EXT_SLAVE_TYPE_PRESSURE:
+		addr_reg = MPUREG_I2C_SLV3_ADDR;
+		reg_reg  = MPUREG_I2C_SLV3_REG;
+		ctrl_reg = MPUREG_I2C_SLV3_CTRL;
+		addr_trig_reg = MPUREG_I2C_SLV4_ADDR;
+		reg_trig_reg  = MPUREG_I2C_SLV4_REG;
+		ctrl_trig_reg = MPUREG_I2C_SLV4_CTRL;
+		bits_slave_delay = BIT_SLV4_DLY_EN | BIT_SLV3_DLY_EN;
+		break;
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	};
+
+	/* return if this slave has already been set */
+	if ((slave_address &&
+	     ((mldl_cfg->inv_mpu_state->i2c_slaves_enabled & bits_slave_delay)
+		    == bits_slave_delay)) ||
+	    (!slave_address &&
+	     (mldl_cfg->inv_mpu_state->i2c_slaves_enabled & bits_slave_delay) ==
+		    0))
+		return 0;
+
+	result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, true);
+
+	/* Address */
+	result = inv_serial_single_write(gyro_handle,
+					 mldl_cfg->mpu_chip_info->addr,
+					 addr_reg, slave_address);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	/* Register */
+	result = inv_serial_single_write(gyro_handle,
+					 mldl_cfg->mpu_chip_info->addr,
+					 reg_reg, slave_reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Length, byte swapping, grouping & enable */
+	if (slave_len > BITS_SLV_LENG) {
+		MPL_LOGW("Limiting slave burst read length to "
+			 "the allowed maximum (15B, req. %d)\n", slave_len);
+		slave_len = BITS_SLV_LENG;
+		return INV_ERROR_INVALID_CONFIGURATION;
+	}
+	reg = slave_len;
+	if (slave_endian == EXT_SLAVE_LITTLE_ENDIAN) {
+		reg |= BIT_SLV_BYTE_SW;
+		if (slave_reg & 1)
+			reg |= BIT_SLV_GRP;
+	}
+	if (slave_address)
+		reg |= BIT_SLV_ENABLE;
+
+	result = inv_serial_single_write(gyro_handle,
+					 mldl_cfg->mpu_chip_info->addr,
+					 ctrl_reg, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Trigger */
+	if (addr_trig_reg) {
+		/* If slave address is 0 this clears the trigger */
+		result = inv_serial_single_write(gyro_handle,
+						 mldl_cfg->mpu_chip_info->addr,
+						 addr_trig_reg,
+						 slave_address & ~BIT_I2C_READ);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	if (slave && slave->trigger && reg_trig_reg) {
+		result = inv_serial_single_write(gyro_handle,
+						 mldl_cfg->mpu_chip_info->addr,
+						 reg_trig_reg,
+						 slave->trigger->reg);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = inv_serial_single_write(gyro_handle,
+						 mldl_cfg->mpu_chip_info->addr,
+						 ctrl_trig_reg,
+						 BIT_SLV_ENABLE | 0x01);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = inv_serial_single_write(gyro_handle,
+						 mldl_cfg->mpu_chip_info->addr,
+						 d0_trig_reg,
+						 slave->trigger->value);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	} else if (ctrl_trig_reg) {
+		result = inv_serial_single_write(gyro_handle,
+						 mldl_cfg->mpu_chip_info->addr,
+						 ctrl_trig_reg, 0x00);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	/* Data rate */
+	if (slave) {
+		struct ext_slave_config config;
+		long data;
+		config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
+		config.len = sizeof(long);
+		config.apply = false;
+		config.data = &data;
+		if (!(slave->get_config))
+			return INV_ERROR_INVALID_CONFIGURATION;
+
+		result = slave->get_config(NULL, slave, slave_pdata, &config);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		MPL_LOGI("Slave %d ODR: %ld Hz\n", slave_id, data / 1000);
+		divider = ((1000 * inv_mpu_get_sampling_rate_hz(
+				    mldl_cfg->mpu_gyro_cfg))
+			/ data) - 1;
+	} else {
+		divider = 0;
+	}
+
+	result = inv_serial_read(gyro_handle,
+				mldl_cfg->mpu_chip_info->addr,
+				MPUREG_I2C_MST_DELAY_CTRL,
+				1, &delay_ctrl_orig);
+	delay_ctrl = delay_ctrl_orig;
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (divider > 0 && divider <= MASK_I2C_MST_DLY) {
+		result = inv_serial_read(gyro_handle,
+					 mldl_cfg->mpu_chip_info->addr,
+					 MPUREG_I2C_SLV4_CTRL, 1, &reg);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		if ((reg & MASK_I2C_MST_DLY) &&
+			((long)(reg & MASK_I2C_MST_DLY) !=
+				(divider & MASK_I2C_MST_DLY))) {
+			MPL_LOGW("Changing slave divider: %ld to %ld\n",
+				 (long)(reg & MASK_I2C_MST_DLY),
+				 (divider & MASK_I2C_MST_DLY));
+
+		}
+		reg |= (unsigned char)(divider & MASK_I2C_MST_DLY);
+		result = inv_serial_single_write(gyro_handle,
+						 mldl_cfg->mpu_chip_info->addr,
+						 MPUREG_I2C_SLV4_CTRL,
+						 reg);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+		delay_ctrl |= bits_slave_delay;
+	} else {
+		delay_ctrl &= ~(bits_slave_delay);
+	}
+	if (delay_ctrl != delay_ctrl_orig) {
+		result = inv_serial_single_write(
+			gyro_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_I2C_MST_DELAY_CTRL,
+			delay_ctrl);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	if (slave_address)
+		mldl_cfg->inv_mpu_state->i2c_slaves_enabled |=
+			bits_slave_delay;
+	else
+		mldl_cfg->inv_mpu_state->i2c_slaves_enabled &=
+			~bits_slave_delay;
+
+	return result;
+}
+
+static int mpu_set_slave(struct mldl_cfg *mldl_cfg,
+			 void *gyro_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *slave_pdata,
+			 int slave_id)
+{
+	return mpu_set_slave_mpu60xx(mldl_cfg, gyro_handle, slave,
+				     slave_pdata, slave_id);
+}
+/**
+ * Check to see if the gyro was reset by testing a couple of registers known
+ * to change on reset.
+ *
+ * @mldl_cfg mldl configuration structure
+ * @gyro_handle handle used to communicate with the gyro
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+static int mpu_was_reset(struct mldl_cfg *mldl_cfg, void *gyro_handle)
+{
+	int result = INV_SUCCESS;
+	unsigned char reg;
+
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_DMP_CFG_2, 1, &reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (mldl_cfg->mpu_gyro_cfg->dmp_cfg2 != reg)
+		return true;
+
+	if (0 != mldl_cfg->mpu_gyro_cfg->dmp_cfg1)
+		return false;
+
+	/* Inconclusive assume it was reset */
+	return true;
+}
+
+
+int inv_mpu_set_firmware(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
+			 const unsigned char *data, int size)
+{
+	int bank, offset, write_size;
+	int result;
+	unsigned char read[MPU_MEM_BANK_SIZE];
+
+	if (mldl_cfg->inv_mpu_state->status & MPU_DEVICE_IS_SUSPENDED) {
+#if INV_CACHE_DMP == 1
+		memcpy(mldl_cfg->mpu_ram->ram, data, size);
+		return INV_SUCCESS;
+#else
+		LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET);
+		return INV_ERROR_MEMORY_SET;
+#endif
+	}
+
+	if (!(mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED)) {
+		LOG_RESULT_LOCATION(INV_ERROR_MEMORY_SET);
+		return INV_ERROR_MEMORY_SET;
+	}
+	/* Write and verify memory */
+	for (bank = 0; size > 0; bank++,
+			size -= write_size,
+			data += write_size) {
+		if (size > MPU_MEM_BANK_SIZE)
+			write_size = MPU_MEM_BANK_SIZE;
+		else
+			write_size = size;
+
+		result = inv_serial_write_mem(mlsl_handle,
+				mldl_cfg->mpu_chip_info->addr,
+				((bank << 8) | 0x00),
+				write_size,
+				data);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			MPL_LOGE("Write mem error in bank %d\n", bank);
+			return result;
+		}
+		result = inv_serial_read_mem(mlsl_handle,
+				mldl_cfg->mpu_chip_info->addr,
+				((bank << 8) | 0x00),
+				write_size,
+				read);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			MPL_LOGE("Read mem error in bank %d\n", bank);
+			return result;
+		}
+
+#define ML_SKIP_CHECK 38
+		for (offset = 0; offset < write_size; offset++) {
+			/* skip the register memory locations */
+			if (bank == 0 && offset < ML_SKIP_CHECK)
+				continue;
+			if (data[offset] != read[offset]) {
+				result = INV_ERROR_SERIAL_WRITE;
+				break;
+			}
+		}
+		if (result != INV_SUCCESS) {
+			LOG_RESULT_LOCATION(result);
+			MPL_LOGE("Read data mismatch at bank %d, offset %d\n",
+				bank, offset);
+			return result;
+		}
+	}
+	return INV_SUCCESS;
+}
+
+static int gyro_resume(struct mldl_cfg *mldl_cfg, void *gyro_handle,
+		       unsigned long sensors)
+{
+	int result;
+	int ii;
+	unsigned char reg;
+	unsigned char regs[7];
+
+	/* Wake up the part */
+	result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, false, sensors);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Always set the INT_ENABLE and DIVIDER as the Accel Only mode for 6050
+	   can set these too */
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_INT_ENABLE, (mldl_cfg->mpu_gyro_cfg->int_config));
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_SMPLRT_DIV, mldl_cfg->mpu_gyro_cfg->divider);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_CONFIG) &&
+	    !mpu_was_reset(mldl_cfg, gyro_handle)) {
+		return INV_SUCCESS;
+	}
+
+	/* Configure the MPU */
+	result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = mpu_set_clock_source(gyro_handle, mldl_cfg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	reg = MPUREG_GYRO_CONFIG_VALUE(0, 0, 0,
+				       mldl_cfg->mpu_gyro_cfg->full_scale);
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_GYRO_CONFIG, reg);
+	reg = MPUREG_CONFIG_VALUE(mldl_cfg->mpu_gyro_cfg->ext_sync,
+				  mldl_cfg->mpu_gyro_cfg->lpf);
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_CONFIG, reg);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_DMP_CFG_1, mldl_cfg->mpu_gyro_cfg->dmp_cfg1);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_DMP_CFG_2, mldl_cfg->mpu_gyro_cfg->dmp_cfg2);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Write and verify memory */
+#if INV_CACHE_DMP != 0
+	inv_mpu_set_firmware(mldl_cfg, gyro_handle,
+		mldl_cfg->mpu_ram->ram, mldl_cfg->mpu_ram->length);
+#endif
+
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_XG_OFFS_TC,
+		((mldl_cfg->mpu_offsets->tc[0] << 1) & BITS_XG_OFFS_TC));
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	regs[0] = ((mldl_cfg->mpu_offsets->tc[1] << 1) & BITS_YG_OFFS_TC);
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_YG_OFFS_TC, regs[0]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_single_write(
+		gyro_handle, mldl_cfg->mpu_chip_info->addr,
+		MPUREG_ZG_OFFS_TC,
+		((mldl_cfg->mpu_offsets->tc[2] << 1) & BITS_ZG_OFFS_TC));
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	regs[0] = MPUREG_X_OFFS_USRH;
+	for (ii = 0; ii < ARRAY_SIZE(mldl_cfg->mpu_offsets->gyro); ii++) {
+		regs[1 + ii * 2] =
+			(unsigned char)(mldl_cfg->mpu_offsets->gyro[ii] >> 8)
+			& 0xff;
+		regs[1 + ii * 2 + 1] =
+			(unsigned char)(mldl_cfg->mpu_offsets->gyro[ii] & 0xff);
+	}
+	result = inv_serial_write(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				  7, regs);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Configure slaves */
+	result = inv_mpu_set_level_shifter_bit(mldl_cfg, gyro_handle,
+					       mldl_cfg->pdata->level_shifter);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_NEEDS_CONFIG;
+
+	return result;
+}
+
+int gyro_config(void *mlsl_handle,
+		struct mldl_cfg *mldl_cfg,
+		struct ext_slave_config *data)
+{
+	struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg;
+	struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
+	struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets;
+	int ii;
+
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_INT_CONFIG:
+		mpu_gyro_cfg->int_config = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_EXT_SYNC:
+		mpu_gyro_cfg->ext_sync = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_FULL_SCALE:
+		mpu_gyro_cfg->full_scale = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_LPF:
+		mpu_gyro_cfg->lpf = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_CLK_SRC:
+		mpu_gyro_cfg->clk_src = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_DIVIDER:
+		mpu_gyro_cfg->divider = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_DMP_ENABLE:
+		mpu_gyro_cfg->dmp_enable = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_FIFO_ENABLE:
+		mpu_gyro_cfg->fifo_enable = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_DMP_CFG1:
+		mpu_gyro_cfg->dmp_cfg1 = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_DMP_CFG2:
+		mpu_gyro_cfg->dmp_cfg2 = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_TC:
+		for (ii = 0; ii < GYRO_NUM_AXES; ii++)
+			mpu_offsets->tc[ii] = ((__u8 *)data->data)[ii];
+		break;
+	case MPU_SLAVE_GYRO:
+		for (ii = 0; ii < GYRO_NUM_AXES; ii++)
+			mpu_offsets->gyro[ii] = ((__u16 *)data->data)[ii];
+		break;
+	case MPU_SLAVE_ADDR:
+		mpu_chip_info->addr = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_PRODUCT_REVISION:
+		mpu_chip_info->product_revision = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_SILICON_REVISION:
+		mpu_chip_info->silicon_revision = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_PRODUCT_ID:
+		mpu_chip_info->product_id = *((__u8 *)data->data);
+		break;
+	case MPU_SLAVE_GYRO_SENS_TRIM:
+		mpu_chip_info->gyro_sens_trim = *((__u16 *)data->data);
+		break;
+	case MPU_SLAVE_ACCEL_SENS_TRIM:
+		mpu_chip_info->accel_sens_trim = *((__u16 *)data->data);
+		break;
+	case MPU_SLAVE_RAM:
+		if (data->len != mldl_cfg->mpu_ram->length)
+			return INV_ERROR_INVALID_PARAMETER;
+
+		memcpy(mldl_cfg->mpu_ram->ram, data->data, data->len);
+		break;
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+	mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG;
+	return INV_SUCCESS;
+}
+
+int gyro_get_config(void *mlsl_handle,
+		struct mldl_cfg *mldl_cfg,
+		struct ext_slave_config *data)
+{
+	struct mpu_gyro_cfg *mpu_gyro_cfg = mldl_cfg->mpu_gyro_cfg;
+	struct mpu_chip_info *mpu_chip_info = mldl_cfg->mpu_chip_info;
+	struct mpu_offsets *mpu_offsets = mldl_cfg->mpu_offsets;
+	int ii;
+
+	if (!data->data)
+		return INV_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_INT_CONFIG:
+		*((__u8 *)data->data) = mpu_gyro_cfg->int_config;
+		break;
+	case MPU_SLAVE_EXT_SYNC:
+		*((__u8 *)data->data) = mpu_gyro_cfg->ext_sync;
+		break;
+	case MPU_SLAVE_FULL_SCALE:
+		*((__u8 *)data->data) = mpu_gyro_cfg->full_scale;
+		break;
+	case MPU_SLAVE_LPF:
+		*((__u8 *)data->data) = mpu_gyro_cfg->lpf;
+		break;
+	case MPU_SLAVE_CLK_SRC:
+		*((__u8 *)data->data) = mpu_gyro_cfg->clk_src;
+		break;
+	case MPU_SLAVE_DIVIDER:
+		*((__u8 *)data->data) = mpu_gyro_cfg->divider;
+		break;
+	case MPU_SLAVE_DMP_ENABLE:
+		*((__u8 *)data->data) = mpu_gyro_cfg->dmp_enable;
+		break;
+	case MPU_SLAVE_FIFO_ENABLE:
+		*((__u8 *)data->data) = mpu_gyro_cfg->fifo_enable;
+		break;
+	case MPU_SLAVE_DMP_CFG1:
+		*((__u8 *)data->data) = mpu_gyro_cfg->dmp_cfg1;
+		break;
+	case MPU_SLAVE_DMP_CFG2:
+		*((__u8 *)data->data) = mpu_gyro_cfg->dmp_cfg2;
+		break;
+	case MPU_SLAVE_TC:
+		for (ii = 0; ii < GYRO_NUM_AXES; ii++)
+			((__u8 *)data->data)[ii] = mpu_offsets->tc[ii];
+		break;
+	case MPU_SLAVE_GYRO:
+		for (ii = 0; ii < GYRO_NUM_AXES; ii++)
+			((__u16 *)data->data)[ii] = mpu_offsets->gyro[ii];
+		break;
+	case MPU_SLAVE_ADDR:
+		*((__u8 *)data->data) = mpu_chip_info->addr;
+		break;
+	case MPU_SLAVE_PRODUCT_REVISION:
+		*((__u8 *)data->data) = mpu_chip_info->product_revision;
+		break;
+	case MPU_SLAVE_SILICON_REVISION:
+		*((__u8 *)data->data) = mpu_chip_info->silicon_revision;
+		break;
+	case MPU_SLAVE_PRODUCT_ID:
+		*((__u8 *)data->data) = mpu_chip_info->product_id;
+		break;
+	case MPU_SLAVE_GYRO_SENS_TRIM:
+		*((__u16 *)data->data) = mpu_chip_info->gyro_sens_trim;
+		break;
+	case MPU_SLAVE_ACCEL_SENS_TRIM:
+		*((__u16 *)data->data) = mpu_chip_info->accel_sens_trim;
+		break;
+	case MPU_SLAVE_RAM:
+		if (data->len != mldl_cfg->mpu_ram->length)
+			return INV_ERROR_INVALID_PARAMETER;
+
+		memcpy(data->data, mldl_cfg->mpu_ram->ram, data->len);
+		break;
+	default:
+		LOG_RESULT_LOCATION(INV_ERROR_FEATURE_NOT_IMPLEMENTED);
+		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return INV_SUCCESS;
+}
+
+
+/*******************************************************************************
+ *******************************************************************************
+ * Exported functions
+ *******************************************************************************
+ ******************************************************************************/
+
+/**
+ * Initializes the pdata structure to defaults.
+ *
+ * Opens the device to read silicon revision, product id and whoami.
+ *
+ * @mldl_cfg
+ *          The internal device configuration data structure.
+ * @mlsl_handle
+ *          The serial communication handle.
+ *
+ * @return INV_SUCCESS if silicon revision, product id and woami are supported
+ *         by this software.
+ */
+int inv_mpu_open(struct mldl_cfg *mldl_cfg,
+		 void *gyro_handle,
+		 void *accel_handle,
+		 void *compass_handle, void *pressure_handle)
+{
+	int result;
+	void *slave_handle[EXT_SLAVE_NUM_TYPES];
+	int ii;
+
+	/* Default is Logic HIGH, pushpull, latch disabled, anyread to clear */
+	ii = 0;
+	mldl_cfg->inv_mpu_cfg->ignore_system_suspend = false;
+	mldl_cfg->mpu_gyro_cfg->int_config = BIT_DMP_INT_EN;
+	mldl_cfg->mpu_gyro_cfg->clk_src = MPU_CLK_SEL_PLLGYROZ;
+	mldl_cfg->mpu_gyro_cfg->lpf = MPU_FILTER_42HZ;
+	mldl_cfg->mpu_gyro_cfg->full_scale = MPU_FS_2000DPS;
+	mldl_cfg->mpu_gyro_cfg->divider = 4;
+	mldl_cfg->mpu_gyro_cfg->dmp_enable = 1;
+	mldl_cfg->mpu_gyro_cfg->fifo_enable = 1;
+	mldl_cfg->mpu_gyro_cfg->ext_sync = 0;
+	mldl_cfg->mpu_gyro_cfg->dmp_cfg1 = 0;
+	mldl_cfg->mpu_gyro_cfg->dmp_cfg2 = 0;
+	mldl_cfg->inv_mpu_state->status =
+		MPU_DMP_IS_SUSPENDED |
+		MPU_GYRO_IS_SUSPENDED |
+		MPU_ACCEL_IS_SUSPENDED |
+		MPU_COMPASS_IS_SUSPENDED |
+		MPU_PRESSURE_IS_SUSPENDED |
+		MPU_DEVICE_IS_SUSPENDED;
+	mldl_cfg->inv_mpu_state->i2c_slaves_enabled = 0;
+
+	slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
+	slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
+	slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
+	slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
+
+	if (mldl_cfg->mpu_chip_info->addr == 0) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	/*
+	 * Reset,
+	 * Take the DMP out of sleep, and
+	 * read the product_id, sillicon rev and whoami
+	 */
+	mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED;
+	result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, true,
+				  INV_THREE_AXIS_GYRO);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	result = inv_get_silicon_rev(mldl_cfg, gyro_handle);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Get the factory temperature compensation offsets */
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_XG_OFFS_TC, 1,
+				 &mldl_cfg->mpu_offsets->tc[0]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_YG_OFFS_TC, 1,
+				 &mldl_cfg->mpu_offsets->tc[1]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
+				 MPUREG_ZG_OFFS_TC, 1,
+				 &mldl_cfg->mpu_offsets->tc[2]);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* Into bypass mode before sleeping and calling the slaves init */
+	result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, true);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	result = inv_mpu_set_level_shifter_bit(mldl_cfg, gyro_handle,
+			mldl_cfg->pdata->level_shifter);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	for (ii = 0; ii < GYRO_NUM_AXES; ii++) {
+		mldl_cfg->mpu_offsets->tc[ii] =
+		    (mldl_cfg->mpu_offsets->tc[ii] & BITS_XG_OFFS_TC) >> 1;
+	}
+
+#if INV_CACHE_DMP != 0
+	result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, false, 0);
+#endif
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+
+	return result;
+
+}
+
+/**
+ * Close the mpu interface
+ *
+ * @mldl_cfg pointer to the configuration structure
+ * @mlsl_handle pointer to the serial layer handle
+ *
+ * @return INV_SUCCESS or non-zero error code
+ */
+int inv_mpu_close(struct mldl_cfg *mldl_cfg,
+		  void *gyro_handle,
+		  void *accel_handle,
+		  void *compass_handle,
+		  void *pressure_handle)
+{
+	return 0;
+}
+
+/**
+ *  @brief  resume the MPU device and all the other sensor
+ *          devices from their low power state.
+ *
+ *  @mldl_cfg
+ *              pointer to the configuration structure
+ *  @gyro_handle
+ *              the main file handle to the MPU device.
+ *  @accel_handle
+ *              an handle to the accelerometer device, if sitting
+ *              onto a separate bus. Can match mlsl_handle if
+ *              the accelerometer device operates on the same
+ *              primary bus of MPU.
+ *  @compass_handle
+ *              an handle to the compass device, if sitting
+ *              onto a separate bus. Can match mlsl_handle if
+ *              the compass device operates on the same
+ *              primary bus of MPU.
+ *  @pressure_handle
+ *              an handle to the pressure sensor device, if sitting
+ *              onto a separate bus. Can match mlsl_handle if
+ *              the pressure sensor device operates on the same
+ *              primary bus of MPU.
+ *  @resume_gyro
+ *              whether resuming the gyroscope device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @resume_accel
+ *              whether resuming the accelerometer device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @resume_compass
+ *              whether resuming the compass device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @resume_pressure
+ *              whether resuming the pressure sensor device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @return  INV_SUCCESS or a non-zero error code.
+ */
+int inv_mpu_resume(struct mldl_cfg *mldl_cfg,
+		   void *gyro_handle,
+		   void *accel_handle,
+		   void *compass_handle,
+		   void *pressure_handle,
+		   unsigned long sensors)
+{
+	int result = INV_SUCCESS;
+	int ii;
+	bool resume_slave[EXT_SLAVE_NUM_TYPES];
+	bool resume_dmp = sensors & INV_DMP_PROCESSOR;
+	void *slave_handle[EXT_SLAVE_NUM_TYPES];
+	resume_slave[EXT_SLAVE_TYPE_GYROSCOPE] =
+		(sensors & (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO));
+	resume_slave[EXT_SLAVE_TYPE_ACCEL] =
+		sensors & INV_THREE_AXIS_ACCEL;
+	resume_slave[EXT_SLAVE_TYPE_COMPASS] =
+		sensors & INV_THREE_AXIS_COMPASS;
+	resume_slave[EXT_SLAVE_TYPE_PRESSURE] =
+		sensors & INV_THREE_AXIS_PRESSURE;
+
+	slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
+	slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
+	slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
+	slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
+
+
+	mldl_print_cfg(mldl_cfg);
+
+	/* Skip the Gyro since slave[EXT_SLAVE_TYPE_GYROSCOPE] is NULL */
+	for (ii = EXT_SLAVE_TYPE_ACCEL; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (resume_slave[ii] &&
+		    ((!mldl_cfg->slave[ii]) ||
+			(!mldl_cfg->slave[ii]->resume))) {
+			LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+			return INV_ERROR_INVALID_PARAMETER;
+		}
+	}
+
+	if ((resume_slave[EXT_SLAVE_TYPE_GYROSCOPE] || resume_dmp)
+	    && ((mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_SUSPENDED) ||
+		(mldl_cfg->inv_mpu_state->status & MPU_GYRO_NEEDS_CONFIG))) {
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = dmp_stop(mldl_cfg, gyro_handle);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = gyro_resume(mldl_cfg, gyro_handle, sensors);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!mldl_cfg->slave[ii] ||
+		    !mldl_cfg->pdata_slave[ii] ||
+		    !resume_slave[ii] ||
+		    !(mldl_cfg->inv_mpu_state->status & (1 << ii)))
+			continue;
+
+		if (EXT_SLAVE_BUS_SECONDARY ==
+		    mldl_cfg->pdata_slave[ii]->bus) {
+			result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle,
+						    true);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+		result = mldl_cfg->slave[ii]->resume(slave_handle[ii],
+						mldl_cfg->slave[ii],
+						mldl_cfg->pdata_slave[ii]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		mldl_cfg->inv_mpu_state->status &= ~(1 << ii);
+	}
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (resume_dmp &&
+		    !(mldl_cfg->inv_mpu_state->status & (1 << ii)) &&
+		    mldl_cfg->pdata_slave[ii] &&
+		    EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata_slave[ii]->bus) {
+			result = mpu_set_slave(mldl_cfg,
+					gyro_handle,
+					mldl_cfg->slave[ii],
+					mldl_cfg->pdata_slave[ii],
+					mldl_cfg->slave[ii]->type);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+	}
+
+	/* Turn on the master i2c iterface if necessary */
+	if (resume_dmp) {
+		result = mpu_set_i2c_bypass(
+			mldl_cfg, gyro_handle,
+			!(mldl_cfg->inv_mpu_state->i2c_slaves_enabled));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+
+		/* Now start */
+		result = dmp_start(mldl_cfg, gyro_handle);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	mldl_cfg->inv_mpu_cfg->requested_sensors = sensors;
+
+	return result;
+}
+
+/**
+ *  @brief  suspend the MPU device and all the other sensor
+ *          devices into their low power state.
+ *  @mldl_cfg
+ *              a pointer to the struct mldl_cfg internal data
+ *              structure.
+ *  @gyro_handle
+ *              the main file handle to the MPU device.
+ *  @accel_handle
+ *              an handle to the accelerometer device, if sitting
+ *              onto a separate bus. Can match gyro_handle if
+ *              the accelerometer device operates on the same
+ *              primary bus of MPU.
+ *  @compass_handle
+ *              an handle to the compass device, if sitting
+ *              onto a separate bus. Can match gyro_handle if
+ *              the compass device operates on the same
+ *              primary bus of MPU.
+ *  @pressure_handle
+ *              an handle to the pressure sensor device, if sitting
+ *              onto a separate bus. Can match gyro_handle if
+ *              the pressure sensor device operates on the same
+ *              primary bus of MPU.
+ *  @accel
+ *              whether suspending the accelerometer device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @compass
+ *              whether suspending the compass device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @pressure
+ *              whether suspending the pressure sensor device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @return  INV_SUCCESS or a non-zero error code.
+ */
+int inv_mpu_suspend(struct mldl_cfg *mldl_cfg,
+		    void *gyro_handle,
+		    void *accel_handle,
+		    void *compass_handle,
+		    void *pressure_handle,
+		    unsigned long sensors)
+{
+	int result = INV_SUCCESS;
+	int ii;
+	struct ext_slave_descr **slave = mldl_cfg->slave;
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	bool suspend_dmp = ((sensors & INV_DMP_PROCESSOR) == INV_DMP_PROCESSOR);
+	bool suspend_slave[EXT_SLAVE_NUM_TYPES];
+	void *slave_handle[EXT_SLAVE_NUM_TYPES];
+
+	suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE] =
+		((sensors & (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO))
+			== (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO));
+	suspend_slave[EXT_SLAVE_TYPE_ACCEL] =
+		((sensors & INV_THREE_AXIS_ACCEL) == INV_THREE_AXIS_ACCEL);
+	suspend_slave[EXT_SLAVE_TYPE_COMPASS] =
+		((sensors & INV_THREE_AXIS_COMPASS) == INV_THREE_AXIS_COMPASS);
+	suspend_slave[EXT_SLAVE_TYPE_PRESSURE] =
+		((sensors & INV_THREE_AXIS_PRESSURE) ==
+			INV_THREE_AXIS_PRESSURE);
+
+	slave_handle[EXT_SLAVE_TYPE_GYROSCOPE] = gyro_handle;
+	slave_handle[EXT_SLAVE_TYPE_ACCEL] = accel_handle;
+	slave_handle[EXT_SLAVE_TYPE_COMPASS] = compass_handle;
+	slave_handle[EXT_SLAVE_TYPE_PRESSURE] = pressure_handle;
+
+	if (suspend_dmp) {
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		result = dmp_stop(mldl_cfg, gyro_handle);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	/* Gyro */
+	if (suspend_slave[EXT_SLAVE_TYPE_GYROSCOPE] &&
+	    !(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_SUSPENDED)) {
+		result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, false,
+					((~sensors) & INV_ALL_SENSORS));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		bool is_suspended = mldl_cfg->inv_mpu_state->status & (1 << ii);
+		if (!slave[ii]   || !pdata_slave[ii] ||
+		    is_suspended || !suspend_slave[ii])
+			continue;
+
+		if (EXT_SLAVE_BUS_SECONDARY == pdata_slave[ii]->bus) {
+			result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+		result = slave[ii]->suspend(slave_handle[ii],
+						  slave[ii],
+						  pdata_slave[ii]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		if (EXT_SLAVE_BUS_SECONDARY == pdata_slave[ii]->bus) {
+			result = mpu_set_slave(mldl_cfg, gyro_handle,
+					       NULL, NULL,
+					       slave[ii]->type);
+			if (result) {
+				LOG_RESULT_LOCATION(result);
+				return result;
+			}
+		}
+		mldl_cfg->inv_mpu_state->status |= (1 << ii);
+	}
+
+	/* Re-enable the i2c master if there are configured slaves and DMP */
+	if (!suspend_dmp) {
+		result = mpu_set_i2c_bypass(
+			mldl_cfg, gyro_handle,
+			!(mldl_cfg->inv_mpu_state->i2c_slaves_enabled));
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	/* Disable irq when suspend all sensors */
+	if (sensors == INV_ALL_SENSORS) {
+		result = inv_serial_single_write(
+			gyro_handle, mldl_cfg->mpu_chip_info->addr,
+			MPUREG_INT_ENABLE, 0);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	mldl_cfg->inv_mpu_cfg->requested_sensors = (~sensors) & INV_ALL_SENSORS;
+
+	return result;
+}
+
+int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg,
+		       void *gyro_handle,
+		       void *slave_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data)
+{
+	int result;
+	int bypass_result;
+	int remain_bypassed = true;
+
+	if (NULL == slave || NULL == slave->read) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
+		return INV_ERROR_INVALID_CONFIGURATION;
+	}
+
+	if ((EXT_SLAVE_BUS_SECONDARY == pdata->bus)
+	    && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
+		remain_bypassed = false;
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	result = slave->read(slave_handle, slave, pdata, data);
+
+	if (!remain_bypassed) {
+		bypass_result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
+		if (bypass_result) {
+			LOG_RESULT_LOCATION(bypass_result);
+			return bypass_result;
+		}
+	}
+	return result;
+}
+
+int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg,
+			 void *gyro_handle,
+			 void *slave_handle,
+			 struct ext_slave_config *data,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	int remain_bypassed = true;
+
+	if (NULL == slave || NULL == slave->config) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
+		return INV_ERROR_INVALID_CONFIGURATION;
+	}
+
+	if (data->apply && (EXT_SLAVE_BUS_SECONDARY == pdata->bus)
+	    && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
+		remain_bypassed = false;
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	result = slave->config(slave_handle, slave, pdata, data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (!remain_bypassed) {
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	return result;
+}
+
+int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg,
+			     void *gyro_handle,
+			     void *slave_handle,
+			     struct ext_slave_config *data,
+			     struct ext_slave_descr *slave,
+			     struct ext_slave_platform_data *pdata)
+{
+	int result;
+	int remain_bypassed = true;
+
+	if (NULL == slave || NULL == slave->get_config) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
+		return INV_ERROR_INVALID_CONFIGURATION;
+	}
+
+	if (data->apply && (EXT_SLAVE_BUS_SECONDARY == pdata->bus)
+	    && (!(mldl_cfg->inv_mpu_state->status & MPU_GYRO_IS_BYPASSED))) {
+		remain_bypassed = false;
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 1);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+
+	result = slave->get_config(slave_handle, slave, pdata, data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	if (!remain_bypassed) {
+		result = mpu_set_i2c_bypass(mldl_cfg, gyro_handle, 0);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+	}
+	return result;
+}
+
+/**
+ * @}
+ */
diff --git a/drivers/misc/inv_mpu/mpu6050/mldl_print_cfg.c b/drivers/misc/inv_mpu/mpu6050/mldl_print_cfg.c
new file mode 100644
index 000000000000..7379a1707610
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/mldl_print_cfg.c
@@ -0,0 +1,138 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @addtogroup MLDL
+ *
+ *  @{
+ *      @file   mldl_print_cfg.c
+ *      @brief  The Motion Library Driver Layer.
+ */
+
+#include <stddef.h>
+#include "mldl_cfg.h"
+#include "mlsl.h"
+#include "linux/mpu.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "mldl_print_cfg:"
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 1
+
+void mldl_print_cfg(struct mldl_cfg *mldl_cfg)
+{
+	struct mpu_gyro_cfg	*mpu_gyro_cfg	= mldl_cfg->mpu_gyro_cfg;
+	struct mpu_offsets	*mpu_offsets	= mldl_cfg->mpu_offsets;
+	struct mpu_chip_info	*mpu_chip_info	= mldl_cfg->mpu_chip_info;
+	struct inv_mpu_cfg	*inv_mpu_cfg	= mldl_cfg->inv_mpu_cfg;
+	struct inv_mpu_state	*inv_mpu_state	= mldl_cfg->inv_mpu_state;
+	struct ext_slave_descr	**slave		= mldl_cfg->slave;
+	struct mpu_platform_data *pdata		= mldl_cfg->pdata;
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	int ii;
+
+	/* mpu_gyro_cfg */
+	MPL_LOGV("int_config     = %02x\n", mpu_gyro_cfg->int_config);
+	MPL_LOGV("ext_sync       = %02x\n", mpu_gyro_cfg->ext_sync);
+	MPL_LOGV("full_scale     = %02x\n", mpu_gyro_cfg->full_scale);
+	MPL_LOGV("lpf            = %02x\n", mpu_gyro_cfg->lpf);
+	MPL_LOGV("clk_src        = %02x\n", mpu_gyro_cfg->clk_src);
+	MPL_LOGV("divider        = %02x\n", mpu_gyro_cfg->divider);
+	MPL_LOGV("dmp_enable     = %02x\n", mpu_gyro_cfg->dmp_enable);
+	MPL_LOGV("fifo_enable    = %02x\n", mpu_gyro_cfg->fifo_enable);
+	MPL_LOGV("dmp_cfg1       = %02x\n", mpu_gyro_cfg->dmp_cfg1);
+	MPL_LOGV("dmp_cfg2       = %02x\n", mpu_gyro_cfg->dmp_cfg2);
+	/* mpu_offsets */
+	MPL_LOGV("tc[0]      = %02x\n", mpu_offsets->tc[0]);
+	MPL_LOGV("tc[1]      = %02x\n", mpu_offsets->tc[1]);
+	MPL_LOGV("tc[2]      = %02x\n", mpu_offsets->tc[2]);
+	MPL_LOGV("gyro[0]    = %04x\n", mpu_offsets->gyro[0]);
+	MPL_LOGV("gyro[1]    = %04x\n", mpu_offsets->gyro[1]);
+	MPL_LOGV("gyro[2]    = %04x\n", mpu_offsets->gyro[2]);
+
+	/* mpu_chip_info */
+	MPL_LOGV("addr            = %02x\n", mldl_cfg->mpu_chip_info->addr);
+
+	MPL_LOGV("silicon_revision = %02x\n", mpu_chip_info->silicon_revision);
+	MPL_LOGV("product_revision = %02x\n", mpu_chip_info->product_revision);
+	MPL_LOGV("product_id       = %02x\n", mpu_chip_info->product_id);
+	MPL_LOGV("gyro_sens_trim   = %02x\n", mpu_chip_info->gyro_sens_trim);
+	MPL_LOGV("accel_sens_trim  = %02x\n", mpu_chip_info->accel_sens_trim);
+
+	MPL_LOGV("requested_sensors = %04x\n", inv_mpu_cfg->requested_sensors);
+	MPL_LOGV("ignore_system_suspend= %04x\n",
+		inv_mpu_cfg->ignore_system_suspend);
+	MPL_LOGV("status = %04x\n", inv_mpu_state->status);
+	MPL_LOGV("i2c_slaves_enabled= %04x\n",
+		inv_mpu_state->i2c_slaves_enabled);
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!slave[ii])
+			continue;
+		MPL_LOGV("SLAVE %d:\n", ii);
+		MPL_LOGV("    suspend  = %02x\n", (int)slave[ii]->suspend);
+		MPL_LOGV("    resume   = %02x\n", (int)slave[ii]->resume);
+		MPL_LOGV("    read     = %02x\n", (int)slave[ii]->read);
+		MPL_LOGV("    type     = %02x\n", slave[ii]->type);
+		MPL_LOGV("    reg      = %02x\n", slave[ii]->read_reg);
+		MPL_LOGV("    len      = %02x\n", slave[ii]->read_len);
+		MPL_LOGV("    endian   = %02x\n", slave[ii]->endian);
+		MPL_LOGV("    range.mantissa= %02x\n",
+			slave[ii]->range.mantissa);
+		MPL_LOGV("    range.fraction= %02x\n",
+			slave[ii]->range.fraction);
+	}
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!pdata_slave[ii])
+			continue;
+		MPL_LOGV("PDATA_SLAVE[%d]\n", ii);
+		MPL_LOGV("    irq        = %02x\n", pdata_slave[ii]->irq);
+		MPL_LOGV("    adapt_num  = %02x\n", pdata_slave[ii]->adapt_num);
+		MPL_LOGV("    bus        = %02x\n", pdata_slave[ii]->bus);
+		MPL_LOGV("    address    = %02x\n", pdata_slave[ii]->address);
+		MPL_LOGV("    orientation=\n"
+			"                            %2d %2d %2d\n"
+			"                            %2d %2d %2d\n"
+			"                            %2d %2d %2d\n",
+			pdata_slave[ii]->orientation[0],
+			pdata_slave[ii]->orientation[1],
+			pdata_slave[ii]->orientation[2],
+			pdata_slave[ii]->orientation[3],
+			pdata_slave[ii]->orientation[4],
+			pdata_slave[ii]->orientation[5],
+			pdata_slave[ii]->orientation[6],
+			pdata_slave[ii]->orientation[7],
+			pdata_slave[ii]->orientation[8]);
+	}
+
+	MPL_LOGV("pdata->int_config         = %02x\n", pdata->int_config);
+	MPL_LOGV("pdata->level_shifter      = %02x\n", pdata->level_shifter);
+	MPL_LOGV("pdata->orientation        =\n"
+		 "                            %2d %2d %2d\n"
+		 "                            %2d %2d %2d\n"
+		 "                            %2d %2d %2d\n",
+		 pdata->orientation[0], pdata->orientation[1],
+		 pdata->orientation[2], pdata->orientation[3],
+		 pdata->orientation[4], pdata->orientation[5],
+		 pdata->orientation[6], pdata->orientation[7],
+		 pdata->orientation[8]);
+}
diff --git a/drivers/misc/inv_mpu/mpu6050/mlsl-kernel.c b/drivers/misc/inv_mpu/mpu6050/mlsl-kernel.c
new file mode 100644
index 000000000000..f1c228f48fe3
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/mlsl-kernel.c
@@ -0,0 +1,420 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+#include "mlsl.h"
+#include <linux/i2c.h>
+#include "log.h"
+#include "mpu6050b1.h"
+
+static int inv_i2c_write(struct i2c_adapter *i2c_adap,
+			    unsigned char address,
+			    unsigned int len, unsigned char const *data)
+{
+	struct i2c_msg msgs[1];
+	int res;
+
+	if (!data || !i2c_adap) {
+		LOG_RESULT_LOCATION(-EINVAL);
+		return -EINVAL;
+	}
+
+	msgs[0].addr = address;
+	msgs[0].flags = 0;	/* write */
+	msgs[0].buf = (unsigned char *)data;
+	msgs[0].len = len;
+
+	res = i2c_transfer(i2c_adap, msgs, 1);
+	if (res < 1) {
+		if (res == 0)
+			res = -EIO;
+		LOG_RESULT_LOCATION(res);
+		return res;
+	} else
+		return 0;
+}
+
+static int inv_i2c_write_register(struct i2c_adapter *i2c_adap,
+				     unsigned char address,
+				     unsigned char reg, unsigned char value)
+{
+	unsigned char data[2];
+
+	data[0] = reg;
+	data[1] = value;
+	return inv_i2c_write(i2c_adap, address, 2, data);
+}
+
+static int inv_i2c_read(struct i2c_adapter *i2c_adap,
+			   unsigned char address, unsigned char reg,
+			   unsigned int len, unsigned char *data)
+{
+	struct i2c_msg msgs[2];
+	int res;
+
+	if (!data || !i2c_adap) {
+		LOG_RESULT_LOCATION(-EINVAL);
+		return -EINVAL;
+	}
+
+	msgs[0].addr = address;
+	msgs[0].flags = 0;	/* write */
+	msgs[0].buf = &reg;
+	msgs[0].len = 1;
+
+	msgs[1].addr = address;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].buf = data;
+	msgs[1].len = len;
+
+	res = i2c_transfer(i2c_adap, msgs, 2);
+	if (res < 2) {
+		if (res >= 0)
+			res = -EIO;
+		LOG_RESULT_LOCATION(res);
+		return res;
+	} else
+		return 0;
+}
+
+static int mpu_memory_read(struct i2c_adapter *i2c_adap,
+			   unsigned char mpu_addr,
+			   unsigned short mem_addr,
+			   unsigned int len, unsigned char *data)
+{
+	unsigned char bank[2];
+	unsigned char addr[2];
+	unsigned char buf;
+
+	struct i2c_msg msgs[4];
+	int res;
+
+	if (!data || !i2c_adap) {
+		LOG_RESULT_LOCATION(-EINVAL);
+		return -EINVAL;
+	}
+
+	bank[0] = MPUREG_BANK_SEL;
+	bank[1] = mem_addr >> 8;
+
+	addr[0] = MPUREG_MEM_START_ADDR;
+	addr[1] = mem_addr & 0xFF;
+
+	buf = MPUREG_MEM_R_W;
+
+	/* write message */
+	msgs[0].addr = mpu_addr;
+	msgs[0].flags = 0;
+	msgs[0].buf = bank;
+	msgs[0].len = sizeof(bank);
+
+	msgs[1].addr = mpu_addr;
+	msgs[1].flags = 0;
+	msgs[1].buf = addr;
+	msgs[1].len = sizeof(addr);
+
+	msgs[2].addr = mpu_addr;
+	msgs[2].flags = 0;
+	msgs[2].buf = &buf;
+	msgs[2].len = 1;
+
+	msgs[3].addr = mpu_addr;
+	msgs[3].flags = I2C_M_RD;
+	msgs[3].buf = data;
+	msgs[3].len = len;
+
+	res = i2c_transfer(i2c_adap, msgs, 4);
+	if (res != 4) {
+		if (res >= 0)
+			res = -EIO;
+		LOG_RESULT_LOCATION(res);
+		return res;
+	} else
+		return 0;
+}
+
+static int mpu_memory_write(struct i2c_adapter *i2c_adap,
+			    unsigned char mpu_addr,
+			    unsigned short mem_addr,
+			    unsigned int len, unsigned char const *data)
+{
+	unsigned char bank[2];
+	unsigned char addr[2];
+	unsigned char buf[513];
+
+	struct i2c_msg msgs[3];
+	int res;
+
+	if (!data || !i2c_adap) {
+		LOG_RESULT_LOCATION(-EINVAL);
+		return -EINVAL;
+	}
+	if (len >= (sizeof(buf) - 1)) {
+		LOG_RESULT_LOCATION(-ENOMEM);
+		return -ENOMEM;
+	}
+
+	bank[0] = MPUREG_BANK_SEL;
+	bank[1] = mem_addr >> 8;
+
+	addr[0] = MPUREG_MEM_START_ADDR;
+	addr[1] = mem_addr & 0xFF;
+
+	buf[0] = MPUREG_MEM_R_W;
+	memcpy(buf + 1, data, len);
+
+	/* write message */
+	msgs[0].addr = mpu_addr;
+	msgs[0].flags = 0;
+	msgs[0].buf = bank;
+	msgs[0].len = sizeof(bank);
+
+	msgs[1].addr = mpu_addr;
+	msgs[1].flags = 0;
+	msgs[1].buf = addr;
+	msgs[1].len = sizeof(addr);
+
+	msgs[2].addr = mpu_addr;
+	msgs[2].flags = 0;
+	msgs[2].buf = (unsigned char *)buf;
+	msgs[2].len = len + 1;
+
+	res = i2c_transfer(i2c_adap, msgs, 3);
+	if (res != 3) {
+		if (res >= 0)
+			res = -EIO;
+		LOG_RESULT_LOCATION(res);
+		return res;
+	} else
+		return 0;
+}
+
+int inv_serial_single_write(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned char register_addr,
+	unsigned char data)
+{
+	return inv_i2c_write_register((struct i2c_adapter *)sl_handle,
+				      slave_addr, register_addr, data);
+}
+EXPORT_SYMBOL(inv_serial_single_write);
+
+int inv_serial_write(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short length,
+	unsigned char const *data)
+{
+	int result;
+	const unsigned short data_length = length - 1;
+	const unsigned char start_reg_addr = data[0];
+	unsigned char i2c_write[SERIAL_MAX_TRANSFER_SIZE + 1];
+	unsigned short bytes_written = 0;
+
+	while (bytes_written < data_length) {
+		unsigned short this_len = min(SERIAL_MAX_TRANSFER_SIZE,
+					     data_length - bytes_written);
+		if (bytes_written == 0) {
+			result = inv_i2c_write((struct i2c_adapter *)
+					       sl_handle, slave_addr,
+					       1 + this_len, data);
+		} else {
+			/* manually increment register addr between chunks */
+			i2c_write[0] = start_reg_addr + bytes_written;
+			memcpy(&i2c_write[1], &data[1 + bytes_written],
+				this_len);
+			result = inv_i2c_write((struct i2c_adapter *)
+					       sl_handle, slave_addr,
+					       1 + this_len, i2c_write);
+		}
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		bytes_written += this_len;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(inv_serial_write);
+
+int inv_serial_read(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned char register_addr,
+	unsigned short length,
+	unsigned char *data)
+{
+	int result;
+	unsigned short bytes_read = 0;
+
+	if ((slave_addr & 0x7E) == DEFAULT_MPU_SLAVEADDR
+		&& (register_addr == MPUREG_FIFO_R_W ||
+		    register_addr == MPUREG_MEM_R_W)) {
+		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+
+	while (bytes_read < length) {
+		unsigned short this_len =
+		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
+		result = inv_i2c_read((struct i2c_adapter *)sl_handle,
+				      slave_addr, register_addr + bytes_read,
+				      this_len, &data[bytes_read]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		bytes_read += this_len;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(inv_serial_read);
+
+int inv_serial_write_mem(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short mem_addr,
+	unsigned short length,
+	unsigned char const *data)
+{
+	int result;
+	unsigned short bytes_written = 0;
+
+	if ((mem_addr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
+		pr_err("memory read length (%d B) extends beyond its"
+		       " limits (%d) if started at location %d\n", length,
+		       MPU_MEM_BANK_SIZE, mem_addr & 0xFF);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+	while (bytes_written < length) {
+		unsigned short this_len =
+		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_written);
+		result = mpu_memory_write((struct i2c_adapter *)sl_handle,
+					  slave_addr, mem_addr + bytes_written,
+					  this_len, &data[bytes_written]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		bytes_written += this_len;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(inv_serial_write_mem);
+
+int inv_serial_read_mem(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short mem_addr,
+	unsigned short length,
+	unsigned char *data)
+{
+	int result;
+	unsigned short bytes_read = 0;
+
+	if ((mem_addr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
+		printk
+		    ("memory read length (%d B) extends beyond its limits (%d) "
+		     "if started at location %d\n", length,
+		     MPU_MEM_BANK_SIZE, mem_addr & 0xFF);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+	while (bytes_read < length) {
+		unsigned short this_len =
+		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
+		result =
+		    mpu_memory_read((struct i2c_adapter *)sl_handle,
+				    slave_addr, mem_addr + bytes_read,
+				    this_len, &data[bytes_read]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		bytes_read += this_len;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(inv_serial_read_mem);
+
+int inv_serial_write_fifo(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short length,
+	unsigned char const *data)
+{
+	int result;
+	unsigned char i2c_write[SERIAL_MAX_TRANSFER_SIZE + 1];
+	unsigned short bytes_written = 0;
+
+	if (length > FIFO_HW_SIZE) {
+		printk(KERN_ERR
+		       "maximum fifo write length is %d\n", FIFO_HW_SIZE);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+	while (bytes_written < length) {
+		unsigned short this_len =
+		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_written);
+		i2c_write[0] = MPUREG_FIFO_R_W;
+		memcpy(&i2c_write[1], &data[bytes_written], this_len);
+		result = inv_i2c_write((struct i2c_adapter *)sl_handle,
+				       slave_addr, this_len + 1, i2c_write);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		bytes_written += this_len;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(inv_serial_write_fifo);
+
+int inv_serial_read_fifo(
+	void *sl_handle,
+	unsigned char slave_addr,
+	unsigned short length,
+	unsigned char *data)
+{
+	int result;
+	unsigned short bytes_read = 0;
+
+	if (length > FIFO_HW_SIZE) {
+		printk(KERN_ERR
+		       "maximum fifo read length is %d\n", FIFO_HW_SIZE);
+		return INV_ERROR_INVALID_PARAMETER;
+	}
+	while (bytes_read < length) {
+		unsigned short this_len =
+		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
+		result = inv_i2c_read((struct i2c_adapter *)sl_handle,
+				      slave_addr, MPUREG_FIFO_R_W, this_len,
+				      &data[bytes_read]);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			return result;
+		}
+		bytes_read += this_len;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(inv_serial_read_fifo);
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/inv_mpu/mpu6050/mpu-dev.c b/drivers/misc/inv_mpu/mpu6050/mpu-dev.c
new file mode 100644
index 000000000000..e171dc2a7eff
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050/mpu-dev.c
@@ -0,0 +1,1309 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include <linux/pm.h>
+#include <linux/mutex.h>
+#include <linux/suspend.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+
+#include "mpuirq.h"
+#include "slaveirq.h"
+#include "mlsl.h"
+#include "mldl_cfg.h"
+#include <linux/mpu.h>
+
+#include "accel/mpu6050.h"
+
+/* Platform data for the MPU */
+struct mpu_private_data {
+	struct miscdevice dev;
+	struct i2c_client *client;
+
+	/* mldl_cfg data */
+	struct mldl_cfg mldl_cfg;
+	struct mpu_ram		mpu_ram;
+	struct mpu_gyro_cfg	mpu_gyro_cfg;
+	struct mpu_offsets	mpu_offsets;
+	struct mpu_chip_info	mpu_chip_info;
+	struct inv_mpu_cfg	inv_mpu_cfg;
+	struct inv_mpu_state	inv_mpu_state;
+
+	struct mutex mutex;
+	wait_queue_head_t mpu_event_wait;
+	struct completion completion;
+	struct timer_list timeout;
+	struct notifier_block nb;
+	struct mpuirq_data mpu_pm_event;
+	int response_timeout;	/* In seconds */
+	unsigned long event;
+	int pid;
+	struct module *slave_modules[EXT_SLAVE_NUM_TYPES];
+};
+
+struct mpu_private_data *mpu_private_data;
+
+static void mpu_pm_timeout(u_long data)
+{
+	struct mpu_private_data *mpu = (struct mpu_private_data *)data;
+	struct i2c_client *client = mpu->client;
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+	complete(&mpu->completion);
+}
+
+static int mpu_pm_notifier_callback(struct notifier_block *nb,
+				    unsigned long event, void *unused)
+{
+	struct mpu_private_data *mpu =
+	    container_of(nb, struct mpu_private_data, nb);
+	struct i2c_client *client = mpu->client;
+	struct timeval event_time;
+	dev_dbg(&client->adapter->dev, "%s: %ld\n", __func__, event);
+
+	/* Prevent the file handle from being closed before we initialize
+	   the completion event */
+	mutex_lock(&mpu->mutex);
+	if (!(mpu->pid) ||
+	    (event != PM_SUSPEND_PREPARE && event != PM_POST_SUSPEND)) {
+		mutex_unlock(&mpu->mutex);
+		return NOTIFY_OK;
+	}
+
+	if (event == PM_SUSPEND_PREPARE)
+		mpu->event = MPU_PM_EVENT_SUSPEND_PREPARE;
+	if (event == PM_POST_SUSPEND)
+		mpu->event = MPU_PM_EVENT_POST_SUSPEND;
+
+	do_gettimeofday(&event_time);
+	mpu->mpu_pm_event.interruptcount++;
+	mpu->mpu_pm_event.irqtime =
+	    (((long long)event_time.tv_sec) << 32) + event_time.tv_usec;
+	mpu->mpu_pm_event.data_type = MPUIRQ_DATA_TYPE_PM_EVENT;
+	mpu->mpu_pm_event.data = mpu->event;
+
+	if (mpu->response_timeout > 0) {
+		mpu->timeout.expires = jiffies + mpu->response_timeout * HZ;
+		add_timer(&mpu->timeout);
+	}
+	INIT_COMPLETION(mpu->completion);
+	mutex_unlock(&mpu->mutex);
+
+	wake_up_interruptible(&mpu->mpu_event_wait);
+	wait_for_completion(&mpu->completion);
+	del_timer_sync(&mpu->timeout);
+	dev_dbg(&client->adapter->dev, "%s: %ld DONE\n", __func__, event);
+	return NOTIFY_OK;
+}
+
+static int mpu_dev_open(struct inode *inode, struct file *file)
+{
+	struct mpu_private_data *mpu =
+	    container_of(file->private_data, struct mpu_private_data, dev);
+	struct i2c_client *client = mpu->client;
+	int result;
+	int ii;
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+	dev_dbg(&client->adapter->dev, "current->pid %d\n", current->pid);
+
+	result = mutex_lock_interruptible(&mpu->mutex);
+	if (mpu->pid) {
+		mutex_unlock(&mpu->mutex);
+		return -EBUSY;
+	}
+	mpu->pid = current->pid;
+
+	/* Reset the sensors to the default */
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"%s: mutex_lock_interruptible returned %d\n",
+			__func__, result);
+		return result;
+	}
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++)
+		__module_get(mpu->slave_modules[ii]);
+
+	mutex_unlock(&mpu->mutex);
+	return 0;
+}
+
+/* close function - called when the "file" /dev/mpu is closed in userspace   */
+static int mpu_release(struct inode *inode, struct file *file)
+{
+	struct mpu_private_data *mpu =
+	    container_of(file->private_data, struct mpu_private_data, dev);
+	struct i2c_client *client = mpu->client;
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	int result = 0;
+	int ii;
+	struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!pdata_slave[ii])
+			slave_adapter[ii] = NULL;
+		else
+			slave_adapter[ii] =
+				i2c_get_adapter(pdata_slave[ii]->adapt_num);
+	}
+	slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+	mutex_lock(&mpu->mutex);
+	mldl_cfg->inv_mpu_cfg->requested_sensors = 0;
+	result = inv_mpu_suspend(mldl_cfg,
+				slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+				slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+				slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+				slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+				INV_ALL_SENSORS);
+	mpu->pid = 0;
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++)
+		module_put(mpu->slave_modules[ii]);
+
+	mutex_unlock(&mpu->mutex);
+	complete(&mpu->completion);
+	dev_dbg(&client->adapter->dev, "mpu_release\n");
+
+	return result;
+}
+
+/* read function called when from /dev/mpu is read.  Read from the FIFO */
+static ssize_t mpu_read(struct file *file,
+			char __user *buf, size_t count, loff_t *offset)
+{
+	struct mpu_private_data *mpu =
+	    container_of(file->private_data, struct mpu_private_data, dev);
+	struct i2c_client *client = mpu->client;
+	size_t len = sizeof(mpu->mpu_pm_event) + sizeof(unsigned long);
+	int err;
+
+	if (!mpu->event && (!(file->f_flags & O_NONBLOCK)))
+		wait_event_interruptible(mpu->mpu_event_wait, mpu->event);
+
+	if (!mpu->event || !buf
+	    || count < sizeof(mpu->mpu_pm_event))
+		return 0;
+
+	err = copy_to_user(buf, &mpu->mpu_pm_event, sizeof(mpu->mpu_pm_event));
+	if (err) {
+		dev_err(&client->adapter->dev,
+			"Copy to user returned %d\n", err);
+		return -EFAULT;
+	}
+	mpu->event = 0;
+	return len;
+}
+
+static unsigned int mpu_poll(struct file *file, struct poll_table_struct *poll)
+{
+	struct mpu_private_data *mpu =
+	    container_of(file->private_data, struct mpu_private_data, dev);
+	int mask = 0;
+
+	poll_wait(file, &mpu->mpu_event_wait, poll);
+	if (mpu->event)
+		mask |= POLLIN | POLLRDNORM;
+	return mask;
+}
+
+static int mpu_dev_ioctl_get_ext_slave_platform_data(
+	struct i2c_client *client,
+	struct ext_slave_platform_data __user *arg)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(client);
+	struct ext_slave_platform_data *pdata_slave;
+	struct ext_slave_platform_data local_pdata_slave;
+
+	if (copy_from_user(&local_pdata_slave, arg, sizeof(local_pdata_slave)))
+		return -EFAULT;
+
+	if (local_pdata_slave.type >= EXT_SLAVE_NUM_TYPES)
+		return -EINVAL;
+
+	pdata_slave = mpu->mldl_cfg.pdata_slave[local_pdata_slave.type];
+	/* All but private data and irq_data */
+	if (!pdata_slave)
+		return -ENODEV;
+	if (copy_to_user(arg, pdata_slave, sizeof(*pdata_slave)))
+		return -EFAULT;
+	return 0;
+}
+
+static int mpu_dev_ioctl_get_mpu_platform_data(
+	struct i2c_client *client,
+	struct mpu_platform_data __user *arg)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(client);
+	struct mpu_platform_data *pdata = mpu->mldl_cfg.pdata;
+
+	if (copy_to_user(arg, pdata, sizeof(*pdata)))
+		return -EFAULT;
+	return 0;
+}
+
+static int mpu_dev_ioctl_get_ext_slave_descr(
+	struct i2c_client *client,
+	struct ext_slave_descr __user *arg)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(client);
+	struct ext_slave_descr *slave;
+	struct ext_slave_descr local_slave;
+
+	if (copy_from_user(&local_slave, arg, sizeof(local_slave)))
+		return -EFAULT;
+
+	if (local_slave.type >= EXT_SLAVE_NUM_TYPES)
+		return -EINVAL;
+
+	slave = mpu->mldl_cfg.slave[local_slave.type];
+	/* All but private data and irq_data */
+	if (!slave)
+		return -ENODEV;
+	if (copy_to_user(arg, slave, sizeof(*slave)))
+		return -EFAULT;
+	return 0;
+}
+
+
+/**
+ * slave_config() - Pass a requested slave configuration to the slave sensor
+ *
+ * @adapter the adaptor to use to communicate with the slave
+ * @mldl_cfg the mldl configuration structuer
+ * @slave pointer to the slave descriptor
+ * @usr_config The configuration to pass to the slave sensor
+ *
+ * returns 0 or non-zero error code
+ */
+static int inv_mpu_config(struct mldl_cfg *mldl_cfg,
+			void *gyro_adapter,
+			struct ext_slave_config __user *usr_config)
+{
+	int retval = 0;
+	struct ext_slave_config config;
+
+	retval = copy_from_user(&config, usr_config, sizeof(config));
+	if (retval)
+		return -EFAULT;
+
+	if (config.len && config.data) {
+		void *data;
+		data = kmalloc(config.len, GFP_KERNEL);
+		if (!data)
+			return -ENOMEM;
+
+		retval = copy_from_user(data,
+					(void __user *)config.data, config.len);
+		if (retval) {
+			retval = -EFAULT;
+			kfree(data);
+			return retval;
+		}
+		config.data = data;
+	}
+	retval = gyro_config(gyro_adapter, mldl_cfg, &config);
+	kfree(config.data);
+	return retval;
+}
+
+static int inv_mpu_get_config(struct mldl_cfg *mldl_cfg,
+			    void *gyro_adapter,
+			    struct ext_slave_config __user *usr_config)
+{
+	int retval = 0;
+	struct ext_slave_config config;
+	void *user_data;
+
+	retval = copy_from_user(&config, usr_config, sizeof(config));
+	if (retval)
+		return -EFAULT;
+
+	user_data = config.data;
+	if (config.len && config.data) {
+		void *data;
+		data = kmalloc(config.len, GFP_KERNEL);
+		if (!data)
+			return -ENOMEM;
+
+		retval = copy_from_user(data,
+					(void __user *)config.data, config.len);
+		if (retval) {
+			retval = -EFAULT;
+			kfree(data);
+			return retval;
+		}
+		config.data = data;
+	}
+	retval = gyro_get_config(gyro_adapter, mldl_cfg, &config);
+	if (!retval)
+		retval = copy_to_user((unsigned char __user *)user_data,
+				config.data, config.len);
+	kfree(config.data);
+	return retval;
+}
+
+static int slave_config(struct mldl_cfg *mldl_cfg,
+			void *gyro_adapter,
+			void *slave_adapter,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			struct ext_slave_config __user *usr_config)
+{
+	int retval = 0;
+	struct ext_slave_config config;
+	if ((!slave) || (!slave->config))
+		return -ENODEV;
+
+	retval = copy_from_user(&config, usr_config, sizeof(config));
+	if (retval)
+		return -EFAULT;
+
+	if (config.len && config.data) {
+		void *data;
+		data = kmalloc(config.len, GFP_KERNEL);
+		if (!data)
+			return -ENOMEM;
+
+		retval = copy_from_user(data,
+					(void __user *)config.data, config.len);
+		if (retval) {
+			retval = -EFAULT;
+			kfree(data);
+			return retval;
+		}
+		config.data = data;
+	}
+	retval = inv_mpu_slave_config(mldl_cfg, gyro_adapter, slave_adapter,
+				      &config, slave, pdata);
+	kfree(config.data);
+	return retval;
+}
+
+static int slave_get_config(struct mldl_cfg *mldl_cfg,
+			    void *gyro_adapter,
+			    void *slave_adapter,
+			    struct ext_slave_descr *slave,
+			    struct ext_slave_platform_data *pdata,
+			    struct ext_slave_config __user *usr_config)
+{
+	int retval = 0;
+	struct ext_slave_config config;
+	void *user_data;
+	if (!(slave) || !(slave->get_config))
+		return -ENODEV;
+
+	retval = copy_from_user(&config, usr_config, sizeof(config));
+	if (retval)
+		return -EFAULT;
+
+	user_data = config.data;
+	if (config.len && config.data) {
+		void *data;
+		data = kmalloc(config.len, GFP_KERNEL);
+		if (!data)
+			return -ENOMEM;
+
+		retval = copy_from_user(data,
+					(void __user *)config.data, config.len);
+		if (retval) {
+			retval = -EFAULT;
+			kfree(data);
+			return retval;
+		}
+		config.data = data;
+	}
+	retval = inv_mpu_get_slave_config(mldl_cfg, gyro_adapter,
+					  slave_adapter, &config, slave, pdata);
+	if (retval) {
+		kfree(config.data);
+		return retval;
+	}
+	retval = copy_to_user((unsigned char __user *)user_data,
+			      config.data, config.len);
+	kfree(config.data);
+	return retval;
+}
+
+static int inv_slave_read(struct mldl_cfg *mldl_cfg,
+			  void *gyro_adapter,
+			  void *slave_adapter,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata,
+			  void __user *usr_data)
+{
+	int retval;
+	unsigned char *data;
+	data = kzalloc(slave->read_len, GFP_KERNEL);
+	if (!data)
+		return -EFAULT;
+
+	retval = inv_mpu_slave_read(mldl_cfg, gyro_adapter, slave_adapter,
+				    slave, pdata, data);
+
+	if ((!retval) &&
+	    (copy_to_user((unsigned char __user *)usr_data,
+			  data, slave->read_len)))
+		retval = -EFAULT;
+
+	kfree(data);
+	return retval;
+}
+
+static int mpu_handle_mlsl(void *sl_handle,
+			   unsigned char addr,
+			   unsigned int cmd,
+			   struct mpu_read_write __user *usr_msg)
+{
+	int retval = 0;
+	struct mpu_read_write msg;
+	unsigned char *user_data;
+	retval = copy_from_user(&msg, usr_msg, sizeof(msg));
+	if (retval)
+		return -EFAULT;
+
+	user_data = msg.data;
+	if (msg.length && msg.data) {
+		unsigned char *data;
+		data = kmalloc(msg.length, GFP_KERNEL);
+		if (!data)
+			return -ENOMEM;
+
+		retval = copy_from_user(data,
+					(void __user *)msg.data, msg.length);
+		if (retval) {
+			retval = -EFAULT;
+			kfree(data);
+			return retval;
+		}
+		msg.data = data;
+	} else {
+		return -EPERM;
+	}
+
+	switch (cmd) {
+	case MPU_READ:
+		retval = inv_serial_read(sl_handle, addr,
+					 msg.address, msg.length, msg.data);
+		break;
+	case MPU_WRITE:
+		retval = inv_serial_write(sl_handle, addr,
+					  msg.length, msg.data);
+		break;
+	case MPU_READ_MEM:
+		retval = inv_serial_read_mem(sl_handle, addr,
+					     msg.address, msg.length, msg.data);
+		break;
+	case MPU_WRITE_MEM:
+		retval = inv_serial_write_mem(sl_handle, addr,
+					      msg.address, msg.length,
+					      msg.data);
+		break;
+	case MPU_READ_FIFO:
+		retval = inv_serial_read_fifo(sl_handle, addr,
+					      msg.length, msg.data);
+		break;
+	case MPU_WRITE_FIFO:
+		retval = inv_serial_write_fifo(sl_handle, addr,
+					       msg.length, msg.data);
+		break;
+
+	};
+	if (retval) {
+		dev_err(&((struct i2c_adapter *)sl_handle)->dev,
+			"%s: i2c %d error %d\n",
+			__func__, cmd, retval);
+		kfree(msg.data);
+		return retval;
+	}
+	retval = copy_to_user((unsigned char __user *)user_data,
+			      msg.data, msg.length);
+	kfree(msg.data);
+	return retval;
+}
+
+/* ioctl - I/O control */
+static long mpu_dev_ioctl(struct file *file,
+			  unsigned int cmd, unsigned long arg)
+{
+	struct mpu_private_data *mpu =
+	    container_of(file->private_data, struct mpu_private_data, dev);
+	struct i2c_client *client = mpu->client;
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	int retval = 0;
+	struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+	struct ext_slave_descr **slave = mldl_cfg->slave;
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	int ii;
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!pdata_slave[ii])
+			slave_adapter[ii] = NULL;
+		else
+			slave_adapter[ii] =
+				i2c_get_adapter(pdata_slave[ii]->adapt_num);
+	}
+	slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+	retval = mutex_lock_interruptible(&mpu->mutex);
+	if (retval) {
+		dev_err(&client->adapter->dev,
+			"%s: mutex_lock_interruptible returned %d\n",
+			__func__, retval);
+		return retval;
+	}
+
+	switch (cmd) {
+	case MPU_GET_EXT_SLAVE_PLATFORM_DATA:
+		retval = mpu_dev_ioctl_get_ext_slave_platform_data(
+			client,
+			(struct ext_slave_platform_data __user *)arg);
+		break;
+	case MPU_GET_MPU_PLATFORM_DATA:
+		retval = mpu_dev_ioctl_get_mpu_platform_data(
+			client,
+			(struct mpu_platform_data __user *)arg);
+		break;
+	case MPU_GET_EXT_SLAVE_DESCR:
+		retval = mpu_dev_ioctl_get_ext_slave_descr(
+			client,
+			(struct ext_slave_descr __user *)arg);
+		break;
+	case MPU_READ:
+	case MPU_WRITE:
+	case MPU_READ_MEM:
+	case MPU_WRITE_MEM:
+	case MPU_READ_FIFO:
+	case MPU_WRITE_FIFO:
+		retval = mpu_handle_mlsl(
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			mldl_cfg->mpu_chip_info->addr, cmd,
+			(struct mpu_read_write __user *)arg);
+		break;
+	case MPU_CONFIG_GYRO:
+		retval = inv_mpu_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_CONFIG_ACCEL:
+		retval = slave_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+			slave[EXT_SLAVE_TYPE_ACCEL],
+			pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_CONFIG_COMPASS:
+		retval = slave_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+			slave[EXT_SLAVE_TYPE_COMPASS],
+			pdata_slave[EXT_SLAVE_TYPE_COMPASS],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_CONFIG_PRESSURE:
+		retval = slave_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+			slave[EXT_SLAVE_TYPE_PRESSURE],
+			pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_GET_CONFIG_GYRO:
+		retval = inv_mpu_get_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_GET_CONFIG_ACCEL:
+		retval = slave_get_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+			slave[EXT_SLAVE_TYPE_ACCEL],
+			pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_GET_CONFIG_COMPASS:
+		retval = slave_get_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+			slave[EXT_SLAVE_TYPE_COMPASS],
+			pdata_slave[EXT_SLAVE_TYPE_COMPASS],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_GET_CONFIG_PRESSURE:
+		retval = slave_get_config(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+			slave[EXT_SLAVE_TYPE_PRESSURE],
+			pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
+			(struct ext_slave_config __user *)arg);
+		break;
+	case MPU_SUSPEND:
+		retval = inv_mpu_suspend(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+			slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+			slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+			arg);
+		break;
+	case MPU_RESUME:
+		retval = inv_mpu_resume(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+			slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+			slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+			arg);
+		break;
+	case MPU_PM_EVENT_HANDLED:
+		dev_dbg(&client->adapter->dev, "%s: %d\n", __func__, cmd);
+		complete(&mpu->completion);
+		break;
+	case MPU_READ_ACCEL:
+		retval = inv_slave_read(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+			slave[EXT_SLAVE_TYPE_ACCEL],
+			pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+			(unsigned char __user *)arg);
+		break;
+	case MPU_READ_COMPASS:
+		retval = inv_slave_read(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+			slave[EXT_SLAVE_TYPE_COMPASS],
+			pdata_slave[EXT_SLAVE_TYPE_COMPASS],
+			(unsigned char __user *)arg);
+		break;
+	case MPU_READ_PRESSURE:
+		retval = inv_slave_read(
+			mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+			slave[EXT_SLAVE_TYPE_PRESSURE],
+			pdata_slave[EXT_SLAVE_TYPE_PRESSURE],
+			(unsigned char __user *)arg);
+		break;
+	case MPU_GET_REQUESTED_SENSORS:
+		if (copy_to_user(
+			   (__u32 __user *)arg,
+			   &mldl_cfg->inv_mpu_cfg->requested_sensors,
+			   sizeof(mldl_cfg->inv_mpu_cfg->requested_sensors)))
+			retval = -EFAULT;
+		break;
+	case MPU_SET_REQUESTED_SENSORS:
+		mldl_cfg->inv_mpu_cfg->requested_sensors = arg;
+		break;
+	case MPU_GET_IGNORE_SYSTEM_SUSPEND:
+		if (copy_to_user(
+			(unsigned char __user *)arg,
+			&mldl_cfg->inv_mpu_cfg->ignore_system_suspend,
+			sizeof(mldl_cfg->inv_mpu_cfg->ignore_system_suspend)))
+			retval = -EFAULT;
+		break;
+	case MPU_SET_IGNORE_SYSTEM_SUSPEND:
+		mldl_cfg->inv_mpu_cfg->ignore_system_suspend = arg;
+		break;
+	case MPU_GET_MLDL_STATUS:
+		if (copy_to_user(
+			(unsigned char __user *)arg,
+			&mldl_cfg->inv_mpu_state->status,
+			sizeof(mldl_cfg->inv_mpu_state->status)))
+			retval = -EFAULT;
+		break;
+	case MPU_GET_I2C_SLAVES_ENABLED:
+		if (copy_to_user(
+			(unsigned char __user *)arg,
+			&mldl_cfg->inv_mpu_state->i2c_slaves_enabled,
+			sizeof(mldl_cfg->inv_mpu_state->i2c_slaves_enabled)))
+			retval = -EFAULT;
+		break;
+	default:
+		dev_err(&client->adapter->dev,
+			"%s: Unknown cmd %x, arg %lu\n",
+			__func__, cmd, arg);
+		retval = -EINVAL;
+	};
+
+	mutex_unlock(&mpu->mutex);
+	dev_dbg(&client->adapter->dev, "%s: %08x, %08lx, %d\n",
+		__func__, cmd, arg, retval);
+
+	if (retval > 0)
+		retval = -retval;
+
+	return retval;
+}
+
+void mpu_shutdown(struct i2c_client *client)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	int ii;
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!pdata_slave[ii])
+			slave_adapter[ii] = NULL;
+		else
+			slave_adapter[ii] =
+				i2c_get_adapter(pdata_slave[ii]->adapt_num);
+	}
+	slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+	mutex_lock(&mpu->mutex);
+	(void)inv_mpu_suspend(mldl_cfg,
+			slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+			slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+			slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+			slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+			INV_ALL_SENSORS);
+	mutex_unlock(&mpu->mutex);
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+}
+
+int mpu_dev_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	int ii;
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!pdata_slave[ii])
+			slave_adapter[ii] = NULL;
+		else
+			slave_adapter[ii] =
+				i2c_get_adapter(pdata_slave[ii]->adapt_num);
+	}
+	slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+	mutex_lock(&mpu->mutex);
+	if (!mldl_cfg->inv_mpu_cfg->ignore_system_suspend) {
+		dev_dbg(&client->adapter->dev,
+			"%s: suspending on event %d\n", __func__, mesg.event);
+		(void)inv_mpu_suspend(mldl_cfg,
+				slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+				slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+				slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+				slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+				INV_ALL_SENSORS);
+	} else {
+		dev_dbg(&client->adapter->dev,
+			"%s: Already suspended %d\n", __func__, mesg.event);
+	}
+	mutex_unlock(&mpu->mutex);
+	return 0;
+}
+
+int mpu_dev_resume(struct i2c_client *client)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *)i2c_get_clientdata(client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	int ii;
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!pdata_slave[ii])
+			slave_adapter[ii] = NULL;
+		else
+			slave_adapter[ii] =
+				i2c_get_adapter(pdata_slave[ii]->adapt_num);
+	}
+	slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+
+	mutex_lock(&mpu->mutex);
+	if (mpu->pid && !mldl_cfg->inv_mpu_cfg->ignore_system_suspend) {
+		(void)inv_mpu_resume(mldl_cfg,
+				slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+				slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+				slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+				slave_adapter[EXT_SLAVE_TYPE_PRESSURE],
+				mldl_cfg->inv_mpu_cfg->requested_sensors);
+		dev_dbg(&client->adapter->dev,
+			"%s for pid %d\n", __func__, mpu->pid);
+	}
+	mutex_unlock(&mpu->mutex);
+	return 0;
+}
+
+/* define which file operations are supported */
+static const struct file_operations mpu_fops = {
+	.owner = THIS_MODULE,
+	.read = mpu_read,
+	.poll = mpu_poll,
+	.unlocked_ioctl = mpu_dev_ioctl,
+	.open = mpu_dev_open,
+	.release = mpu_release,
+};
+
+int inv_mpu_register_slave(struct module *slave_module,
+			struct i2c_client *slave_client,
+			struct ext_slave_platform_data *slave_pdata,
+			struct ext_slave_descr *(*get_slave_descr)(void))
+{
+	struct mpu_private_data *mpu = mpu_private_data;
+	struct mldl_cfg *mldl_cfg;
+	struct ext_slave_descr *slave_descr;
+	struct ext_slave_platform_data **pdata_slave;
+	char *irq_name = NULL;
+	int result = 0;
+
+	if (!slave_client || !slave_pdata || !get_slave_descr)
+		return -EINVAL;
+
+	if (!mpu) {
+		dev_err(&slave_client->adapter->dev,
+			"%s: Null mpu_private_data\n", __func__);
+		return -EINVAL;
+	}
+	mldl_cfg    = &mpu->mldl_cfg;
+	pdata_slave = mldl_cfg->pdata_slave;
+	slave_descr = get_slave_descr();
+
+	if (!slave_descr) {
+		dev_err(&slave_client->adapter->dev,
+			"%s: Null ext_slave_descr\n", __func__);
+		return -EINVAL;
+	}
+
+	mutex_lock(&mpu->mutex);
+	if (mpu->pid) {
+		mutex_unlock(&mpu->mutex);
+		return -EBUSY;
+	}
+
+	if (pdata_slave[slave_descr->type]) {
+		result = -EBUSY;
+		goto out_unlock_mutex;
+	}
+
+	slave_pdata->address	= slave_client->addr;
+	slave_pdata->irq	= slave_client->irq;
+	slave_pdata->adapt_num	= i2c_adapter_id(slave_client->adapter);
+
+	dev_info(&slave_client->adapter->dev,
+		"%s: +%s Type %d: Addr: %2x IRQ: %2d, Adapt: %2d\n",
+		__func__,
+		slave_descr->name,
+		slave_descr->type,
+		slave_pdata->address,
+		slave_pdata->irq,
+		slave_pdata->adapt_num);
+
+	switch (slave_descr->type) {
+	case EXT_SLAVE_TYPE_ACCEL:
+		irq_name = "accelirq";
+		break;
+	case EXT_SLAVE_TYPE_COMPASS:
+		irq_name = "compassirq";
+		break;
+	case EXT_SLAVE_TYPE_PRESSURE:
+		irq_name = "pressureirq";
+		break;
+	default:
+		irq_name = "none";
+	};
+	if (slave_descr->init) {
+		result = slave_descr->init(slave_client->adapter,
+					slave_descr,
+					slave_pdata);
+		if (result) {
+			dev_err(&slave_client->adapter->dev,
+				"%s init failed %d\n",
+				slave_descr->name, result);
+			goto out_unlock_mutex;
+		}
+	}
+
+	if (slave_descr->type == EXT_SLAVE_TYPE_ACCEL &&
+	    slave_descr->id == ACCEL_ID_MPU6050 &&
+	    slave_descr->config) {
+		/* pass a reference to the mldl_cfg data
+		   structure to the mpu6050 accel "class" */
+		struct ext_slave_config config;
+		config.key = MPU_SLAVE_CONFIG_INTERNAL_REFERENCE;
+		config.len = sizeof(struct mldl_cfg *);
+		config.apply = true;
+		config.data = mldl_cfg;
+		result = slave_descr->config(
+			slave_client->adapter, slave_descr,
+			slave_pdata, &config);
+		if (result) {
+			LOG_RESULT_LOCATION(result);
+			goto out_slavedescr_exit;
+		}
+	}
+	pdata_slave[slave_descr->type] = slave_pdata;
+	mpu->slave_modules[slave_descr->type] = slave_module;
+	mldl_cfg->slave[slave_descr->type] = slave_descr;
+
+	goto out_unlock_mutex;
+
+out_slavedescr_exit:
+	if (slave_descr->exit)
+		slave_descr->exit(slave_client->adapter,
+				  slave_descr, slave_pdata);
+out_unlock_mutex:
+	mutex_unlock(&mpu->mutex);
+
+	if (!result && irq_name && (slave_pdata->irq > 0)) {
+		int warn_result;
+		dev_info(&slave_client->adapter->dev,
+			"Installing %s irq using %d\n",
+			irq_name,
+			slave_pdata->irq);
+		warn_result = slaveirq_init(slave_client->adapter,
+					slave_pdata, irq_name);
+		if (result)
+			dev_WARN(&slave_client->adapter->dev,
+				"%s irq assigned error: %d\n",
+				slave_descr->name, warn_result);
+	} else {
+		dev_WARN(&slave_client->adapter->dev,
+			"%s irq not assigned: %d %d %d\n",
+			slave_descr->name,
+			result, (int)irq_name, slave_pdata->irq);
+	}
+
+	return result;
+}
+EXPORT_SYMBOL(inv_mpu_register_slave);
+
+void inv_mpu_unregister_slave(struct i2c_client *slave_client,
+			struct ext_slave_platform_data *slave_pdata,
+			struct ext_slave_descr *(*get_slave_descr)(void))
+{
+	struct mpu_private_data *mpu = mpu_private_data;
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct ext_slave_descr *slave_descr;
+	int result;
+
+	dev_info(&slave_client->adapter->dev, "%s\n", __func__);
+
+	if (!slave_client || !slave_pdata || !get_slave_descr)
+		return;
+
+	if (slave_pdata->irq)
+		slaveirq_exit(slave_pdata);
+
+	slave_descr = get_slave_descr();
+	if (!slave_descr)
+		return;
+
+	mutex_lock(&mpu->mutex);
+
+	if (slave_descr->exit) {
+		result = slave_descr->exit(slave_client->adapter,
+					slave_descr,
+					slave_pdata);
+		if (result)
+			dev_err(&slave_client->adapter->dev,
+				"Accel exit failed %d\n", result);
+	}
+	mldl_cfg->slave[slave_descr->type] = NULL;
+	mldl_cfg->pdata_slave[slave_descr->type] = NULL;
+	mpu->slave_modules[slave_descr->type] = NULL;
+
+	mutex_unlock(&mpu->mutex);
+
+}
+EXPORT_SYMBOL(inv_mpu_unregister_slave);
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static const struct i2c_device_id mpu_id[] = {
+	{"mpu3050", 0},
+	{"mpu6050", 0},
+	{"mpu6050_no_accel", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, mpu_id);
+
+int mpu_probe(struct i2c_client *client, const struct i2c_device_id *devid)
+{
+	struct mpu_platform_data *pdata;
+	struct mpu_private_data *mpu;
+	struct mldl_cfg *mldl_cfg;
+	int res = 0;
+	int ii = 0;
+	unsigned long irq_flags;
+
+	dev_info(&client->adapter->dev, "%s: %d\n", __func__, ii++);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		res = -ENODEV;
+		goto out_check_functionality_failed;
+	}
+
+	mpu = kzalloc(sizeof(struct mpu_private_data), GFP_KERNEL);
+	if (!mpu) {
+		res = -ENOMEM;
+		goto out_alloc_data_failed;
+	}
+	mldl_cfg = &mpu->mldl_cfg;
+	mldl_cfg->mpu_ram	= &mpu->mpu_ram;
+	mldl_cfg->mpu_gyro_cfg	= &mpu->mpu_gyro_cfg;
+	mldl_cfg->mpu_offsets	= &mpu->mpu_offsets;
+	mldl_cfg->mpu_chip_info	= &mpu->mpu_chip_info;
+	mldl_cfg->inv_mpu_cfg	= &mpu->inv_mpu_cfg;
+	mldl_cfg->inv_mpu_state	= &mpu->inv_mpu_state;
+
+	mldl_cfg->mpu_ram->length = MPU_MEM_NUM_RAM_BANKS * MPU_MEM_BANK_SIZE;
+	mldl_cfg->mpu_ram->ram = kzalloc(mldl_cfg->mpu_ram->length, GFP_KERNEL);
+	if (!mldl_cfg->mpu_ram->ram) {
+		res = -ENOMEM;
+		goto out_alloc_ram_failed;
+	}
+	mpu_private_data = mpu;
+	i2c_set_clientdata(client, mpu);
+	mpu->client = client;
+
+	init_waitqueue_head(&mpu->mpu_event_wait);
+	mutex_init(&mpu->mutex);
+	init_completion(&mpu->completion);
+
+	mpu->response_timeout = 60;	/* Seconds */
+	mpu->timeout.function = mpu_pm_timeout;
+	mpu->timeout.data = (u_long) mpu;
+	init_timer(&mpu->timeout);
+
+	mpu->nb.notifier_call = mpu_pm_notifier_callback;
+	mpu->nb.priority = 0;
+	res = register_pm_notifier(&mpu->nb);
+	if (res) {
+		dev_err(&client->adapter->dev,
+			"Unable to register pm_notifier %d\n", res);
+		goto out_register_pm_notifier_failed;
+	}
+
+	pdata = (struct mpu_platform_data *)client->dev.platform_data;
+	if (!pdata) {
+		dev_WARN(&client->adapter->dev,
+			 "Missing platform data for mpu\n");
+	}
+	mldl_cfg->pdata = pdata;
+
+	mldl_cfg->mpu_chip_info->addr = client->addr;
+	res = inv_mpu_open(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL);
+
+	if (res) {
+		dev_err(&client->adapter->dev,
+			"Unable to open %s %d\n", MPU_NAME, res);
+		res = -ENODEV;
+		goto out_whoami_failed;
+	}
+
+	mpu->dev.minor = MISC_DYNAMIC_MINOR;
+	mpu->dev.name = "mpu";
+	mpu->dev.fops = &mpu_fops;
+	res = misc_register(&mpu->dev);
+	if (res < 0) {
+		dev_err(&client->adapter->dev,
+			"ERROR: misc_register returned %d\n", res);
+		goto out_misc_register_failed;
+	}
+
+	if (client->irq) {
+		dev_info(&client->adapter->dev,
+			 "Installing irq using %d\n", client->irq);
+		if (BIT_ACTL_LOW == ((mldl_cfg->pdata->int_config) & BIT_ACTL))
+			irq_flags = IRQF_TRIGGER_FALLING;
+		else
+			irq_flags = IRQF_TRIGGER_RISING;
+		res = mpuirq_init(client, mldl_cfg, irq_flags);
+
+		if (res)
+			goto out_mpuirq_failed;
+	} else {
+		dev_WARN(&client->adapter->dev,
+			 "Missing %s IRQ\n", MPU_NAME);
+	}
+	if (!strcmp(mpu_id[1].name, devid->name)) {
+		/* Special case to re-use the inv_mpu_register_slave */
+		struct ext_slave_platform_data *slave_pdata;
+		slave_pdata = kzalloc(sizeof(*slave_pdata), GFP_KERNEL);
+		if (!slave_pdata) {
+			res = -ENOMEM;
+			goto out_slave_pdata_kzalloc_failed;
+		}
+		slave_pdata->bus = EXT_SLAVE_BUS_PRIMARY;
+		for (ii = 0; ii < 9; ii++)
+			slave_pdata->orientation[ii] = pdata->orientation[ii];
+		res = inv_mpu_register_slave(
+			NULL, client,
+			slave_pdata,
+			mpu6050_get_slave_descr);
+		if (res) {
+			/* if inv_mpu_register_slave fails there are no pointer
+			   references to the memory allocated to slave_pdata */
+			kfree(slave_pdata);
+			goto out_slave_pdata_kzalloc_failed;
+		}
+	}
+	return res;
+
+out_slave_pdata_kzalloc_failed:
+	if (client->irq)
+		mpuirq_exit();
+out_mpuirq_failed:
+	misc_deregister(&mpu->dev);
+out_misc_register_failed:
+	inv_mpu_close(&mpu->mldl_cfg, client->adapter, NULL, NULL, NULL);
+out_whoami_failed:
+	unregister_pm_notifier(&mpu->nb);
+out_register_pm_notifier_failed:
+	kfree(mldl_cfg->mpu_ram->ram);
+	mpu_private_data = NULL;
+out_alloc_ram_failed:
+	kfree(mpu);
+out_alloc_data_failed:
+out_check_functionality_failed:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, res);
+	return res;
+
+}
+
+static int mpu_remove(struct i2c_client *client)
+{
+	struct mpu_private_data *mpu = i2c_get_clientdata(client);
+	struct i2c_adapter *slave_adapter[EXT_SLAVE_NUM_TYPES];
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
+	int ii;
+
+	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
+		if (!pdata_slave[ii])
+			slave_adapter[ii] = NULL;
+		else
+			slave_adapter[ii] =
+				i2c_get_adapter(pdata_slave[ii]->adapt_num);
+	}
+
+	slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE] = client->adapter;
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_close(mldl_cfg,
+		slave_adapter[EXT_SLAVE_TYPE_GYROSCOPE],
+		slave_adapter[EXT_SLAVE_TYPE_ACCEL],
+		slave_adapter[EXT_SLAVE_TYPE_COMPASS],
+		slave_adapter[EXT_SLAVE_TYPE_PRESSURE]);
+
+	if (mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL] &&
+		(mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->id ==
+			ACCEL_ID_MPU6050)) {
+		struct ext_slave_platform_data *slave_pdata =
+			mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL];
+		inv_mpu_unregister_slave(
+			client,
+			mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL],
+			mpu6050_get_slave_descr);
+		kfree(slave_pdata);
+	}
+
+	if (client->irq)
+		mpuirq_exit();
+
+	misc_deregister(&mpu->dev);
+
+	unregister_pm_notifier(&mpu->nb);
+
+	kfree(mpu->mldl_cfg.mpu_ram->ram);
+	kfree(mpu);
+
+	return 0;
+}
+
+static struct i2c_driver mpu_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = mpu_probe,
+	.remove = mpu_remove,
+	.id_table = mpu_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = MPU_NAME,
+		   },
+	.address_list = normal_i2c,
+	.shutdown = mpu_shutdown,	/* optional */
+	.suspend = mpu_dev_suspend,	/* optional */
+	.resume = mpu_dev_resume,	/* optional */
+
+};
+
+static int __init mpu_init(void)
+{
+	int res = i2c_add_driver(&mpu_driver);
+	pr_info("%s: Probe name %s\n", __func__, MPU_NAME);
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit mpu_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&mpu_driver);
+}
+
+module_init(mpu_init);
+module_exit(mpu_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("User space character device interface for MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS(MPU_NAME);
diff --git a/drivers/misc/inv_mpu/mpu6050b1.h b/drivers/misc/inv_mpu/mpu6050b1.h
new file mode 100644
index 000000000000..686f6e5d86a5
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpu6050b1.h
@@ -0,0 +1,435 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ * @defgroup
+ * @brief
+ *
+ * @{
+ *      @file     mpu6050.h
+ *      @brief
+ */
+
+#ifndef __MPU_H_
+#error Do not include this file directly.  Include mpu.h instead.
+#endif
+
+#ifndef __MPU6050B1_H_
+#define __MPU6050B1_H_
+
+
+#define MPU_NAME "mpu6050B1"
+#define DEFAULT_MPU_SLAVEADDR		0x68
+
+/*==== MPU6050B1 REGISTER SET ====*/
+enum {
+	MPUREG_XG_OFFS_TC = 0,			/* 0x00,   0 */
+	MPUREG_YG_OFFS_TC,			/* 0x01,   1 */
+	MPUREG_ZG_OFFS_TC,			/* 0x02,   2 */
+	MPUREG_X_FINE_GAIN,			/* 0x03,   3 */
+	MPUREG_Y_FINE_GAIN,			/* 0x04,   4 */
+	MPUREG_Z_FINE_GAIN,			/* 0x05,   5 */
+	MPUREG_XA_OFFS_H,			/* 0x06,   6 */
+	MPUREG_XA_OFFS_L,			/* 0x07,   7 */
+	MPUREG_YA_OFFS_H,			/* 0x08,   8 */
+	MPUREG_YA_OFFS_L,			/* 0x09,   9 */
+	MPUREG_ZA_OFFS_H,			/* 0x0a,  10 */
+	MPUREG_ZA_OFFS_L,			/* 0x0B,  11 */
+	MPUREG_PRODUCT_ID,			/* 0x0c,  12 */
+	MPUREG_0D_RSVD,				/* 0x0d,  13 */
+	MPUREG_0E_RSVD,				/* 0x0e,  14 */
+	MPUREG_0F_RSVD,				/* 0x0f,  15 */
+	MPUREG_10_RSVD,				/* 0x00,  16 */
+	MPUREG_11_RSVD,				/* 0x11,  17 */
+	MPUREG_12_RSVD,				/* 0x12,  18 */
+	MPUREG_XG_OFFS_USRH,			/* 0x13,  19 */
+	MPUREG_XG_OFFS_USRL,			/* 0x14,  20 */
+	MPUREG_YG_OFFS_USRH,			/* 0x15,  21 */
+	MPUREG_YG_OFFS_USRL,			/* 0x16,  22 */
+	MPUREG_ZG_OFFS_USRH,			/* 0x17,  23 */
+	MPUREG_ZG_OFFS_USRL,			/* 0x18,  24 */
+	MPUREG_SMPLRT_DIV,			/* 0x19,  25 */
+	MPUREG_CONFIG,				/* 0x1A,  26 */
+	MPUREG_GYRO_CONFIG,			/* 0x1b,  27 */
+	MPUREG_ACCEL_CONFIG,			/* 0x1c,  28 */
+	MPUREG_ACCEL_FF_THR,			/* 0x1d,  29 */
+	MPUREG_ACCEL_FF_DUR,			/* 0x1e,  30 */
+	MPUREG_ACCEL_MOT_THR,			/* 0x1f,  31 */
+	MPUREG_ACCEL_MOT_DUR,			/* 0x20,  32 */
+	MPUREG_ACCEL_ZRMOT_THR,			/* 0x21,  33 */
+	MPUREG_ACCEL_ZRMOT_DUR,			/* 0x22,  34 */
+	MPUREG_FIFO_EN,				/* 0x23,  35 */
+	MPUREG_I2C_MST_CTRL,			/* 0x24,  36 */
+	MPUREG_I2C_SLV0_ADDR,			/* 0x25,  37 */
+	MPUREG_I2C_SLV0_REG,			/* 0x26,  38 */
+	MPUREG_I2C_SLV0_CTRL,			/* 0x27,  39 */
+	MPUREG_I2C_SLV1_ADDR,			/* 0x28,  40 */
+	MPUREG_I2C_SLV1_REG,			/* 0x29,  41 */
+	MPUREG_I2C_SLV1_CTRL,			/* 0x2a,  42 */
+	MPUREG_I2C_SLV2_ADDR,			/* 0x2B,  43 */
+	MPUREG_I2C_SLV2_REG,			/* 0x2c,  44 */
+	MPUREG_I2C_SLV2_CTRL,			/* 0x2d,  45 */
+	MPUREG_I2C_SLV3_ADDR,			/* 0x2E,  46 */
+	MPUREG_I2C_SLV3_REG,			/* 0x2f,  47 */
+	MPUREG_I2C_SLV3_CTRL,			/* 0x30,  48 */
+	MPUREG_I2C_SLV4_ADDR,			/* 0x31,  49 */
+	MPUREG_I2C_SLV4_REG,			/* 0x32,  50 */
+	MPUREG_I2C_SLV4_DO,			/* 0x33,  51 */
+	MPUREG_I2C_SLV4_CTRL,			/* 0x34,  52 */
+	MPUREG_I2C_SLV4_DI,			/* 0x35,  53 */
+	MPUREG_I2C_MST_STATUS,			/* 0x36,  54 */
+	MPUREG_INT_PIN_CFG,			/* 0x37,  55 */
+	MPUREG_INT_ENABLE,			/* 0x38,  56 */
+	MPUREG_DMP_INT_STATUS,			/* 0x39,  57 */
+	MPUREG_INT_STATUS,			/* 0x3A,  58 */
+	MPUREG_ACCEL_XOUT_H,			/* 0x3B,  59 */
+	MPUREG_ACCEL_XOUT_L,			/* 0x3c,  60 */
+	MPUREG_ACCEL_YOUT_H,			/* 0x3d,  61 */
+	MPUREG_ACCEL_YOUT_L,			/* 0x3e,  62 */
+	MPUREG_ACCEL_ZOUT_H,			/* 0x3f,  63 */
+	MPUREG_ACCEL_ZOUT_L,			/* 0x40,  64 */
+	MPUREG_TEMP_OUT_H,			/* 0x41,  65 */
+	MPUREG_TEMP_OUT_L,			/* 0x42,  66 */
+	MPUREG_GYRO_XOUT_H,			/* 0x43,  67 */
+	MPUREG_GYRO_XOUT_L,			/* 0x44,  68 */
+	MPUREG_GYRO_YOUT_H,			/* 0x45,  69 */
+	MPUREG_GYRO_YOUT_L,			/* 0x46,  70 */
+	MPUREG_GYRO_ZOUT_H,			/* 0x47,  71 */
+	MPUREG_GYRO_ZOUT_L,			/* 0x48,  72 */
+	MPUREG_EXT_SLV_SENS_DATA_00,		/* 0x49,  73 */
+	MPUREG_EXT_SLV_SENS_DATA_01,		/* 0x4a,  74 */
+	MPUREG_EXT_SLV_SENS_DATA_02,		/* 0x4b,  75 */
+	MPUREG_EXT_SLV_SENS_DATA_03,		/* 0x4c,  76 */
+	MPUREG_EXT_SLV_SENS_DATA_04,		/* 0x4d,  77 */
+	MPUREG_EXT_SLV_SENS_DATA_05,		/* 0x4e,  78 */
+	MPUREG_EXT_SLV_SENS_DATA_06,		/* 0x4F,  79 */
+	MPUREG_EXT_SLV_SENS_DATA_07,		/* 0x50,  80 */
+	MPUREG_EXT_SLV_SENS_DATA_08,		/* 0x51,  81 */
+	MPUREG_EXT_SLV_SENS_DATA_09,		/* 0x52,  82 */
+	MPUREG_EXT_SLV_SENS_DATA_10,		/* 0x53,  83 */
+	MPUREG_EXT_SLV_SENS_DATA_11,		/* 0x54,  84 */
+	MPUREG_EXT_SLV_SENS_DATA_12,		/* 0x55,  85 */
+	MPUREG_EXT_SLV_SENS_DATA_13,		/* 0x56,  86 */
+	MPUREG_EXT_SLV_SENS_DATA_14,		/* 0x57,  87 */
+	MPUREG_EXT_SLV_SENS_DATA_15,		/* 0x58,  88 */
+	MPUREG_EXT_SLV_SENS_DATA_16,		/* 0x59,  89 */
+	MPUREG_EXT_SLV_SENS_DATA_17,		/* 0x5a,  90 */
+	MPUREG_EXT_SLV_SENS_DATA_18,		/* 0x5B,  91 */
+	MPUREG_EXT_SLV_SENS_DATA_19,		/* 0x5c,  92 */
+	MPUREG_EXT_SLV_SENS_DATA_20,		/* 0x5d,  93 */
+	MPUREG_EXT_SLV_SENS_DATA_21,		/* 0x5e,  94 */
+	MPUREG_EXT_SLV_SENS_DATA_22,		/* 0x5f,  95 */
+	MPUREG_EXT_SLV_SENS_DATA_23,		/* 0x60,  96 */
+	MPUREG_ACCEL_INTEL_STATUS,		/* 0x61,  97 */
+	MPUREG_62_RSVD,				/* 0x62,  98 */
+	MPUREG_I2C_SLV0_DO,			/* 0x63,  99 */
+	MPUREG_I2C_SLV1_DO,			/* 0x64, 100 */
+	MPUREG_I2C_SLV2_DO,			/* 0x65, 101 */
+	MPUREG_I2C_SLV3_DO,			/* 0x66, 102 */
+	MPUREG_I2C_MST_DELAY_CTRL,		/* 0x67, 103 */
+	MPUREG_SIGNAL_PATH_RESET,		/* 0x68, 104 */
+	MPUREG_ACCEL_INTEL_CTRL,		/* 0x69, 105 */
+	MPUREG_USER_CTRL,			/* 0x6A, 106 */
+	MPUREG_PWR_MGMT_1,			/* 0x6B, 107 */
+	MPUREG_PWR_MGMT_2,			/* 0x6C, 108 */
+	MPUREG_BANK_SEL,			/* 0x6D, 109 */
+	MPUREG_MEM_START_ADDR,			/* 0x6E, 100 */
+	MPUREG_MEM_R_W,				/* 0x6F, 111 */
+	MPUREG_DMP_CFG_1,			/* 0x70, 112 */
+	MPUREG_DMP_CFG_2,			/* 0x71, 113 */
+	MPUREG_FIFO_COUNTH,			/* 0x72, 114 */
+	MPUREG_FIFO_COUNTL,			/* 0x73, 115 */
+	MPUREG_FIFO_R_W,			/* 0x74, 116 */
+	MPUREG_WHOAMI,				/* 0x75, 117 */
+
+	NUM_OF_MPU_REGISTERS			/* = 0x76, 118 */
+};
+
+/*==== MPU6050B1 MEMORY ====*/
+enum MPU_MEMORY_BANKS {
+	MEM_RAM_BANK_0 = 0,
+	MEM_RAM_BANK_1,
+	MEM_RAM_BANK_2,
+	MEM_RAM_BANK_3,
+	MEM_RAM_BANK_4,
+	MEM_RAM_BANK_5,
+	MEM_RAM_BANK_6,
+	MEM_RAM_BANK_7,
+	MEM_RAM_BANK_8,
+	MEM_RAM_BANK_9,
+	MEM_RAM_BANK_10,
+	MEM_RAM_BANK_11,
+	MPU_MEM_NUM_RAM_BANKS,
+	MPU_MEM_OTP_BANK_0 = 16
+};
+
+
+/*==== MPU6050B1 parameters ====*/
+
+#define NUM_REGS		(NUM_OF_MPU_REGISTERS)
+#define START_SENS_REGS		(0x3B)
+#define NUM_SENS_REGS		(0x60 - START_SENS_REGS + 1)
+
+/*---- MPU Memory ----*/
+#define NUM_BANKS		(MPU_MEM_NUM_RAM_BANKS)
+#define BANK_SIZE		(256)
+#define MEM_SIZE		(NUM_BANKS * BANK_SIZE)
+#define MPU_MEM_BANK_SIZE	(BANK_SIZE)	/*alternative name */
+
+#define FIFO_HW_SIZE		(1024)
+
+#define NUM_EXT_SLAVES		(4)
+
+
+/*==== BITS FOR MPU6050B1 ====*/
+/*---- MPU6050B1 'XG_OFFS_TC' register (0, 1, 2) ----*/
+#define BIT_PU_SLEEP_MODE			0x80
+#define BITS_XG_OFFS_TC				0x7E
+#define BIT_OTP_BNK_VLD				0x01
+
+#define BIT_I2C_MST_VDDIO			0x80
+#define BITS_YG_OFFS_TC				0x7E
+#define BITS_ZG_OFFS_TC				0x7E
+/*---- MPU6050B1 'FIFO_EN' register (23) ----*/
+#define	BIT_TEMP_OUT				0x80
+#define	BIT_GYRO_XOUT				0x40
+#define	BIT_GYRO_YOUT				0x20
+#define	BIT_GYRO_ZOUT				0x10
+#define	BIT_ACCEL				0x08
+#define	BIT_SLV_2				0x04
+#define	BIT_SLV_1				0x02
+#define	BIT_SLV_0				0x01
+/*---- MPU6050B1 'CONFIG' register (1A) ----*/
+/*NONE						0xC0 */
+#define	BITS_EXT_SYNC_SET			0x38
+#define	BITS_DLPF_CFG				0x07
+/*---- MPU6050B1 'GYRO_CONFIG' register (1B) ----*/
+/* voluntarily modified label from BITS_FS_SEL to
+ * BITS_GYRO_FS_SEL to avoid confusion with MPU
+ */
+#define BITS_GYRO_FS_SEL			0x18
+/*NONE						0x07 */
+/*---- MPU6050B1 'ACCEL_CONFIG' register (1C) ----*/
+#define BITS_ACCEL_FS_SEL			0x18
+#define BITS_ACCEL_HPF				0x07
+/*---- MPU6050B1 'I2C_MST_CTRL' register (24) ----*/
+#define BIT_MULT_MST_EN				0x80
+#define BIT_WAIT_FOR_ES				0x40
+#define BIT_SLV_3_FIFO_EN			0x20
+#define BIT_I2C_MST_PSR				0x10
+#define BITS_I2C_MST_CLK			0x0F
+/*---- MPU6050B1 'I2C_SLV?_ADDR' register (27,2A,2D,30) ----*/
+#define BIT_I2C_READ				0x80
+#define BIT_I2C_WRITE				0x00
+#define BITS_I2C_ADDR				0x7F
+/*---- MPU6050B1 'I2C_SLV?_CTRL' register (27,2A,2D,30) ----*/
+#define BIT_SLV_ENABLE				0x80
+#define BIT_SLV_BYTE_SW				0x40
+#define BIT_SLV_REG_DIS				0x20
+#define BIT_SLV_GRP				0x10
+#define BITS_SLV_LENG				0x0F
+/*---- MPU6050B1 'I2C_SLV4_ADDR' register (31) ----*/
+#define BIT_I2C_SLV4_RNW			0x80
+/*---- MPU6050B1 'I2C_SLV4_CTRL' register (34) ----*/
+#define BIT_I2C_SLV4_EN				0x80
+#define BIT_SLV4_DONE_INT_EN			0x40
+#define BIT_SLV4_REG_DIS			0x20
+#define MASK_I2C_MST_DLY			0x1F
+/*---- MPU6050B1 'I2C_MST_STATUS' register (36) ----*/
+#define BIT_PASS_THROUGH			0x80
+#define BIT_I2C_SLV4_DONE			0x40
+#define BIT_I2C_LOST_ARB			0x20
+#define BIT_I2C_SLV4_NACK			0x10
+#define BIT_I2C_SLV3_NACK			0x08
+#define BIT_I2C_SLV2_NACK			0x04
+#define BIT_I2C_SLV1_NACK			0x02
+#define BIT_I2C_SLV0_NACK			0x01
+/*---- MPU6050B1 'INT_PIN_CFG' register (37) ----*/
+#define	BIT_ACTL				0x80
+#define BIT_ACTL_LOW				0x80
+#define BIT_ACTL_HIGH				0x00
+#define	BIT_OPEN				0x40
+#define	BIT_LATCH_INT_EN			0x20
+#define	BIT_INT_ANYRD_2CLEAR			0x10
+#define	BIT_ACTL_FSYNC				0x08
+#define	BIT_FSYNC_INT_EN			0x04
+#define	BIT_BYPASS_EN				0x02
+#define	BIT_CLKOUT_EN				0x01
+/*---- MPU6050B1 'INT_ENABLE' register (38) ----*/
+#define	BIT_FF_EN				0x80
+#define	BIT_MOT_EN				0x40
+#define	BIT_ZMOT_EN				0x20
+#define	BIT_FIFO_OVERFLOW_EN			0x10
+#define BIT_I2C_MST_INT_EN			0x08
+#define	BIT_PLL_RDY_EN				0x04
+#define BIT_DMP_INT_EN				0x02
+#define	BIT_RAW_RDY_EN				0x01
+/*---- MPU6050B1 'DMP_INT_STATUS' register (39) ----*/
+/*NONE						0x80 */
+/*NONE						0x40 */
+#define	BIT_DMP_INT_5				0x20
+#define	BIT_DMP_INT_4				0x10
+#define	BIT_DMP_INT_3				0x08
+#define	BIT_DMP_INT_2				0x04
+#define	BIT_DMP_INT_1				0x02
+#define	BIT_DMP_INT_0				0x01
+/*---- MPU6050B1 'INT_STATUS' register (3A) ----*/
+#define	BIT_FF_INT				0x80
+#define	BIT_MOT_INT				0x40
+#define	BIT_ZMOT_INT				0x20
+#define	BIT_FIFO_OVERFLOW_INT			0x10
+#define	BIT_I2C_MST_INT				0x08
+#define	BIT_PLL_RDY_INT				0x04
+#define BIT_DMP_INT				0x02
+#define	BIT_RAW_DATA_RDY_INT			0x01
+/*---- MPU6050B1 'MPUREG_I2C_MST_DELAY_CTRL' register (0x67) ----*/
+#define	BIT_DELAY_ES_SHADOW			0x80
+#define	BIT_SLV4_DLY_EN				0x10
+#define	BIT_SLV3_DLY_EN				0x08
+#define	BIT_SLV2_DLY_EN				0x04
+#define	BIT_SLV1_DLY_EN				0x02
+#define	BIT_SLV0_DLY_EN				0x01
+/*---- MPU6050B1 'BANK_SEL' register (6D) ----*/
+#define	BIT_PRFTCH_EN				0x40
+#define	BIT_CFG_USER_BANK			0x20
+#define	BITS_MEM_SEL				0x1f
+/*---- MPU6050B1 'USER_CTRL' register (6A) ----*/
+#define	BIT_DMP_EN				0x80
+#define	BIT_FIFO_EN				0x40
+#define	BIT_I2C_MST_EN				0x20
+#define	BIT_I2C_IF_DIS				0x10
+#define	BIT_DMP_RST				0x08
+#define	BIT_FIFO_RST				0x04
+#define	BIT_I2C_MST_RST				0x02
+#define	BIT_SIG_COND_RST			0x01
+/*---- MPU6050B1 'PWR_MGMT_1' register (6B) ----*/
+#define	BIT_H_RESET				0x80
+#define	BIT_SLEEP				0x40
+#define	BIT_CYCLE				0x20
+#define BIT_PD_PTAT				0x08
+#define BITS_CLKSEL				0x07
+/*---- MPU6050B1 'PWR_MGMT_2' register (6C) ----*/
+#define	BITS_LPA_WAKE_CTRL			0xC0
+#define	BITS_LPA_WAKE_1HZ			0x00
+#define	BITS_LPA_WAKE_2HZ			0x40
+#define	BITS_LPA_WAKE_10HZ			0x80
+#define	BITS_LPA_WAKE_40HZ			0xC0
+#define	BIT_STBY_XA				0x20
+#define	BIT_STBY_YA				0x10
+#define	BIT_STBY_ZA				0x08
+#define	BIT_STBY_XG				0x04
+#define	BIT_STBY_YG				0x02
+#define	BIT_STBY_ZG				0x01
+
+#define ACCEL_MOT_THR_LSB (32) /* mg */
+#define ACCEL_MOT_DUR_LSB (1)
+#define ACCEL_ZRMOT_THR_LSB_CONVERSION(mg) ((mg * 1000) / 255)
+#define ACCEL_ZRMOT_DUR_LSB (64)
+
+/*----------------------------------------------------------------------------*/
+/*---- Alternative names to take care of conflicts with current mpu3050.h ----*/
+/*----------------------------------------------------------------------------*/
+
+/*-- registers --*/
+#define MPUREG_DLPF_FS_SYNC	MPUREG_CONFIG			/* 0x1A */
+
+#define MPUREG_PWR_MGM		MPUREG_PWR_MGMT_1		/* 0x6B */
+#define MPUREG_FIFO_EN1		MPUREG_FIFO_EN			/* 0x23 */
+#define MPUREG_INT_CFG		MPUREG_INT_ENABLE		/* 0x38 */
+#define MPUREG_X_OFFS_USRH	MPUREG_XG_OFFS_USRH		/* 0x13 */
+#define MPUREG_WHO_AM_I		MPUREG_WHOAMI			/* 0x75 */
+#define MPUREG_23_RSVD		MPUREG_EXT_SLV_SENS_DATA_00	/* 0x49 */
+
+/*-- bits --*/
+/* 'USER_CTRL' register */
+#define BIT_AUX_IF_EN		BIT_I2C_MST_EN
+#define BIT_AUX_RD_LENG		BIT_I2C_MST_EN
+#define BIT_IME_IF_RST		BIT_I2C_MST_RST
+#define BIT_GYRO_RST		BIT_SIG_COND_RST
+/* 'INT_ENABLE' register */
+#define BIT_RAW_RDY		BIT_RAW_DATA_RDY_INT
+#define BIT_MPU_RDY_EN		BIT_PLL_RDY_EN
+/* 'INT_STATUS' register */
+#define BIT_INT_STATUS_FIFO_OVERLOW BIT_FIFO_OVERFLOW_INT
+
+/*---- MPU6050 Silicon Revisions ----*/
+#define MPU_SILICON_REV_A2		1	/* MPU6050A2 Device */
+#define MPU_SILICON_REV_B1		2	/* MPU6050B1 Device */
+
+/*---- MPU6050 notable product revisions ----*/
+#define MPU_PRODUCT_KEY_B1_E1_5		105
+#define MPU_PRODUCT_KEY_B2_F1		431
+
+/*---- structure containing control variables used by MLDL ----*/
+/*---- MPU clock source settings ----*/
+/*---- MPU filter selections ----*/
+enum mpu_filter {
+	MPU_FILTER_256HZ_NOLPF2 = 0,
+	MPU_FILTER_188HZ,
+	MPU_FILTER_98HZ,
+	MPU_FILTER_42HZ,
+	MPU_FILTER_20HZ,
+	MPU_FILTER_10HZ,
+	MPU_FILTER_5HZ,
+	MPU_FILTER_2100HZ_NOLPF,
+	NUM_MPU_FILTER
+};
+
+enum mpu_fullscale {
+	MPU_FS_250DPS = 0,
+	MPU_FS_500DPS,
+	MPU_FS_1000DPS,
+	MPU_FS_2000DPS,
+	NUM_MPU_FS
+};
+
+enum mpu_clock_sel {
+	MPU_CLK_SEL_INTERNAL = 0,
+	MPU_CLK_SEL_PLLGYROX,
+	MPU_CLK_SEL_PLLGYROY,
+	MPU_CLK_SEL_PLLGYROZ,
+	MPU_CLK_SEL_PLLEXT32K,
+	MPU_CLK_SEL_PLLEXT19M,
+	MPU_CLK_SEL_RESERVED,
+	MPU_CLK_SEL_STOP,
+	NUM_CLK_SEL
+};
+
+enum mpu_ext_sync {
+	MPU_EXT_SYNC_NONE = 0,
+	MPU_EXT_SYNC_TEMP,
+	MPU_EXT_SYNC_GYROX,
+	MPU_EXT_SYNC_GYROY,
+	MPU_EXT_SYNC_GYROZ,
+	MPU_EXT_SYNC_ACCELX,
+	MPU_EXT_SYNC_ACCELY,
+	MPU_EXT_SYNC_ACCELZ,
+	NUM_MPU_EXT_SYNC
+};
+
+#define MPUREG_CONFIG_VALUE(ext_sync, lpf) \
+	((ext_sync << 3) | lpf)
+
+#define MPUREG_GYRO_CONFIG_VALUE(x_st, y_st, z_st, full_scale)	\
+	((x_st ? 0x80 : 0) |				\
+	 (y_st ? 0x70 : 0) |				\
+	 (z_st ? 0x60 : 0) |				\
+	 (full_scale << 3))
+
+#endif				/* __MPU6050_H_ */
diff --git a/drivers/misc/inv_mpu/mpuirq.c b/drivers/misc/inv_mpu/mpuirq.c
new file mode 100644
index 000000000000..d8b721e43468
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpuirq.c
@@ -0,0 +1,254 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/irq.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+
+#include <linux/mpu.h>
+#include "mpuirq.h"
+#include "mldl_cfg.h"
+
+#define MPUIRQ_NAME "mpuirq"
+
+/* function which gets accel data and sends it to MPU */
+
+DECLARE_WAIT_QUEUE_HEAD(mpuirq_wait);
+
+struct mpuirq_dev_data {
+	struct i2c_client *mpu_client;
+	struct miscdevice *dev;
+	int irq;
+	int pid;
+	int accel_divider;
+	int data_ready;
+	int timeout;
+};
+
+static struct mpuirq_dev_data mpuirq_dev_data;
+static struct mpuirq_data mpuirq_data;
+static char *interface = MPUIRQ_NAME;
+
+static int mpuirq_open(struct inode *inode, struct file *file)
+{
+	dev_dbg(mpuirq_dev_data.dev->this_device,
+		"%s current->pid %d\n", __func__, current->pid);
+	mpuirq_dev_data.pid = current->pid;
+	file->private_data = &mpuirq_dev_data;
+	return 0;
+}
+
+/* close function - called when the "file" /dev/mpuirq is closed in userspace */
+static int mpuirq_release(struct inode *inode, struct file *file)
+{
+	dev_dbg(mpuirq_dev_data.dev->this_device, "mpuirq_release\n");
+	return 0;
+}
+
+/* read function called when from /dev/mpuirq is read */
+static ssize_t mpuirq_read(struct file *file,
+			   char *buf, size_t count, loff_t *ppos)
+{
+	int len, err;
+	struct mpuirq_dev_data *p_mpuirq_dev_data = file->private_data;
+
+	if (!mpuirq_dev_data.data_ready &&
+	    mpuirq_dev_data.timeout && (!(file->f_flags & O_NONBLOCK))) {
+		wait_event_interruptible_timeout(mpuirq_wait,
+						 mpuirq_dev_data.data_ready,
+						 mpuirq_dev_data.timeout);
+	}
+
+	if (mpuirq_dev_data.data_ready && NULL != buf
+	    && count >= sizeof(mpuirq_data)) {
+		err = copy_to_user(buf, &mpuirq_data, sizeof(mpuirq_data));
+		mpuirq_data.data_type = 0;
+	} else {
+		return 0;
+	}
+	if (err != 0) {
+		dev_err(p_mpuirq_dev_data->dev->this_device,
+			"Copy to user returned %d\n", err);
+		return -EFAULT;
+	}
+	mpuirq_dev_data.data_ready = 0;
+	len = sizeof(mpuirq_data);
+	return len;
+}
+
+unsigned int mpuirq_poll(struct file *file, struct poll_table_struct *poll)
+{
+	int mask = 0;
+
+	poll_wait(file, &mpuirq_wait, poll);
+	if (mpuirq_dev_data.data_ready)
+		mask |= POLLIN | POLLRDNORM;
+	return mask;
+}
+
+/* ioctl - I/O control */
+static long mpuirq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	int data;
+
+	switch (cmd) {
+	case MPUIRQ_SET_TIMEOUT:
+		mpuirq_dev_data.timeout = arg;
+		break;
+
+	case MPUIRQ_GET_INTERRUPT_CNT:
+		data = mpuirq_data.interruptcount - 1;
+		if (mpuirq_data.interruptcount > 1)
+			mpuirq_data.interruptcount = 1;
+
+		if (copy_to_user((int *)arg, &data, sizeof(int)))
+			return -EFAULT;
+		break;
+	case MPUIRQ_GET_IRQ_TIME:
+		if (copy_to_user((int *)arg, &mpuirq_data.irqtime,
+				 sizeof(mpuirq_data.irqtime)))
+			return -EFAULT;
+		mpuirq_data.irqtime = 0;
+		break;
+	case MPUIRQ_SET_FREQUENCY_DIVIDER:
+		mpuirq_dev_data.accel_divider = arg;
+		break;
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+static irqreturn_t mpuirq_handler(int irq, void *dev_id)
+{
+	static int mycount;
+	struct timeval irqtime;
+	mycount++;
+
+	mpuirq_data.interruptcount++;
+
+	/* wake up (unblock) for reading data from userspace */
+	/* and ignore first interrupt generated in module init */
+	mpuirq_dev_data.data_ready = 1;
+
+	do_gettimeofday(&irqtime);
+	mpuirq_data.irqtime = (((long long)irqtime.tv_sec) << 32);
+	mpuirq_data.irqtime += irqtime.tv_usec;
+	mpuirq_data.data_type = MPUIRQ_DATA_TYPE_MPU_IRQ;
+	mpuirq_data.data = 0;
+
+	wake_up_interruptible(&mpuirq_wait);
+
+	return IRQ_HANDLED;
+
+}
+
+/* define which file operations are supported */
+const struct file_operations mpuirq_fops = {
+	.owner = THIS_MODULE,
+	.read = mpuirq_read,
+	.poll = mpuirq_poll,
+
+	.unlocked_ioctl = mpuirq_ioctl,
+	.open = mpuirq_open,
+	.release = mpuirq_release,
+};
+
+static struct miscdevice mpuirq_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = MPUIRQ_NAME,
+	.fops = &mpuirq_fops,
+};
+
+int mpuirq_init(struct i2c_client *mpu_client, struct mldl_cfg *mldl_cfg,
+		unsigned long irq_flags)
+{
+
+	int res;
+
+	mpuirq_dev_data.mpu_client = mpu_client;
+
+	dev_info(&mpu_client->adapter->dev,
+		 "Module Param interface = %s\n", interface);
+
+	mpuirq_dev_data.irq = mpu_client->irq;
+	mpuirq_dev_data.pid = 0;
+	mpuirq_dev_data.accel_divider = -1;
+	mpuirq_dev_data.data_ready = 0;
+	mpuirq_dev_data.timeout = 0;
+	mpuirq_dev_data.dev = &mpuirq_device;
+
+	if (mpuirq_dev_data.irq) {
+		irq_flags |= IRQF_SHARED;
+		res =
+		    request_irq(mpuirq_dev_data.irq, mpuirq_handler, irq_flags,
+				interface, &mpuirq_dev_data.irq);
+		if (res) {
+			dev_err(&mpu_client->adapter->dev,
+				"myirqtest: cannot register IRQ %d\n",
+				mpuirq_dev_data.irq);
+		} else {
+			res = misc_register(&mpuirq_device);
+			if (res < 0) {
+				dev_err(&mpu_client->adapter->dev,
+					"misc_register returned %d\n", res);
+				free_irq(mpuirq_dev_data.irq,
+					 &mpuirq_dev_data.irq);
+			}
+		}
+
+	} else {
+		res = 0;
+	}
+
+	return res;
+}
+EXPORT_SYMBOL(mpuirq_init);
+
+void mpuirq_exit(void)
+{
+	if (mpuirq_dev_data.irq > 0)
+		free_irq(mpuirq_dev_data.irq, &mpuirq_dev_data.irq);
+
+	dev_info(mpuirq_device.this_device, "Unregistering %s\n", MPUIRQ_NAME);
+	misc_deregister(&mpuirq_device);
+
+	return;
+}
+EXPORT_SYMBOL(mpuirq_exit);
+
+module_param(interface, charp, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(interface, "The Interface name");
diff --git a/drivers/misc/inv_mpu/mpuirq.h b/drivers/misc/inv_mpu/mpuirq.h
new file mode 100644
index 000000000000..073867d830c5
--- /dev/null
+++ b/drivers/misc/inv_mpu/mpuirq.h
@@ -0,0 +1,37 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+#ifndef __MPUIRQ__
+#define __MPUIRQ__
+
+#include <linux/i2c-dev.h>
+#include <linux/time.h>
+#include <linux/ioctl.h>
+#include "mldl_cfg.h"
+
+#define MPUIRQ_SET_TIMEOUT           _IOW(MPU_IOCTL, 0x40, unsigned long)
+#define MPUIRQ_GET_INTERRUPT_CNT     _IOR(MPU_IOCTL, 0x41, unsigned long)
+#define MPUIRQ_GET_IRQ_TIME          _IOR(MPU_IOCTL, 0x42, struct timeval)
+#define MPUIRQ_SET_FREQUENCY_DIVIDER _IOW(MPU_IOCTL, 0x43, unsigned long)
+
+void mpuirq_exit(void);
+int mpuirq_init(struct i2c_client *mpu_client, struct mldl_cfg *mldl_cfg,
+		unsigned long irq_flags);
+
+#endif
diff --git a/drivers/misc/inv_mpu/pressure/Kconfig b/drivers/misc/inv_mpu/pressure/Kconfig
new file mode 100644
index 000000000000..f1c021e8f126
--- /dev/null
+++ b/drivers/misc/inv_mpu/pressure/Kconfig
@@ -0,0 +1,20 @@
+menuconfig: INV_SENSORS_PRESSURE
+	bool "Pressure Sensor Slaves"
+	depends on INV_SENSORS
+	default y
+	help
+	  Select y to see a list of supported pressure sensors that can be
+	  integrated with the MPUxxxx set of motion processors.
+
+if INV_SENSORS_PRESSURE
+
+config MPU_SENSORS_BMA085
+	tristate "Bosch BMA085"
+	help
+	  This enables support for the Bosch bma085 pressure sensor
+	  This support is for integration with the MPU3050 or MPU6050 gyroscope
+          device driver.  Only one accelerometer can be registered at a time.
+	  Specifying more that one accelerometer in the board file will result
+	  in runtime errors.
+
+endif
diff --git a/drivers/misc/inv_mpu/pressure/Makefile b/drivers/misc/inv_mpu/pressure/Makefile
new file mode 100644
index 000000000000..595923d809dc
--- /dev/null
+++ b/drivers/misc/inv_mpu/pressure/Makefile
@@ -0,0 +1,8 @@
+#
+# Pressure Slaves to MPUxxxx
+#
+obj-$(CONFIG_MPU_SENSORS_BMA085) += inv_mpu_bma085.o
+inv_mpu_bma085-objs +=	bma085.o
+
+EXTRA_CFLAGS += -Idrivers/misc/inv_mpu
+EXTRA_CFLAGS += -D__C99_DESIGNATED_INITIALIZER
diff --git a/drivers/misc/inv_mpu/pressure/bma085.c b/drivers/misc/inv_mpu/pressure/bma085.c
new file mode 100644
index 000000000000..696d2b6e183c
--- /dev/null
+++ b/drivers/misc/inv_mpu/pressure/bma085.c
@@ -0,0 +1,367 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+/**
+ *  @defgroup   ACCELDL (Motion Library - Pressure Driver Layer)
+ *  @brief      Provides the interface to setup and handle a pressure
+ *              connected to the secondary I2C interface of the gyroscope.
+ *
+ *  @{
+ *      @file   bma085.c
+ *      @brief  Pressure setup and handling methods.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include "mpu-dev.h"
+
+#include <linux/mpu.h>
+#include "mlsl.h"
+#include "log.h"
+
+/*
+ * this structure holds all device specific calibration parameters
+ */
+struct bmp085_calibration_param_t {
+	short ac1;
+	short ac2;
+	short ac3;
+	unsigned short ac4;
+	unsigned short ac5;
+	unsigned short ac6;
+	short b1;
+	short b2;
+	short mb;
+	short mc;
+	short md;
+	long param_b5;
+};
+
+struct bmp085_calibration_param_t cal_param;
+
+#define PRESSURE_BMA085_PARAM_MG      3038        /* calibration parameter */
+#define PRESSURE_BMA085_PARAM_MH     -7357        /* calibration parameter */
+#define PRESSURE_BMA085_PARAM_MI      3791        /* calibration parameter */
+
+/*********************************************
+ *    Pressure Initialization Functions
+ *********************************************/
+
+static int bma085_suspend(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	int result = INV_SUCCESS;
+	return result;
+}
+
+#define PRESSURE_BMA085_PROM_START_ADDR  (0xAA)
+#define PRESSURE_BMA085_PROM_DATA_LEN    (22)
+#define PRESSURE_BMP085_CTRL_MEAS_REG    (0xF4)
+/* temperature measurent */
+#define PRESSURE_BMP085_T_MEAS           (0x2E)
+/* pressure measurement; oversampling_setting */
+#define PRESSURE_BMP085_P_MEAS_OSS_0     (0x34)
+#define PRESSURE_BMP085_P_MEAS_OSS_1     (0x74)
+#define PRESSURE_BMP085_P_MEAS_OSS_2     (0xB4)
+#define PRESSURE_BMP085_P_MEAS_OSS_3     (0xF4)
+#define PRESSURE_BMP085_ADC_OUT_MSB_REG  (0xF6)
+#define PRESSURE_BMP085_ADC_OUT_LSB_REG  (0xF7)
+
+static int bma085_resume(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char data[PRESSURE_BMA085_PROM_DATA_LEN];
+
+	result =
+	    inv_serial_read(mlsl_handle, pdata->address,
+			   PRESSURE_BMA085_PROM_START_ADDR,
+			   PRESSURE_BMA085_PROM_DATA_LEN, data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+
+	/* parameters AC1-AC6 */
+	cal_param.ac1 = (data[0] << 8) | data[1];
+	cal_param.ac2 = (data[2] << 8) | data[3];
+	cal_param.ac3 = (data[4] << 8) | data[5];
+	cal_param.ac4 = (data[6] << 8) | data[7];
+	cal_param.ac5 = (data[8] << 8) | data[9];
+	cal_param.ac6 = (data[10] << 8) | data[11];
+
+	/* parameters B1,B2 */
+	cal_param.b1 = (data[12] << 8) | data[13];
+	cal_param.b2 = (data[14] << 8) | data[15];
+
+	/* parameters MB,MC,MD */
+	cal_param.mb = (data[16] << 8) | data[17];
+	cal_param.mc = (data[18] << 8) | data[19];
+	cal_param.md = (data[20] << 8) | data[21];
+
+	return result;
+}
+
+static int bma085_read(void *mlsl_handle,
+		       struct ext_slave_descr *slave,
+		       struct ext_slave_platform_data *pdata,
+		       unsigned char *data)
+{
+	int result;
+	long pressure, x1, x2, x3, b3, b6;
+	unsigned long b4, b7;
+	unsigned long up;
+	unsigned short ut;
+	short oversampling_setting = 0;
+	short temperature;
+	long divisor;
+
+	/* get temprature */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       PRESSURE_BMP085_CTRL_MEAS_REG,
+				       PRESSURE_BMP085_T_MEAS);
+	msleep(5);
+	result =
+	    inv_serial_read(mlsl_handle, pdata->address,
+			   PRESSURE_BMP085_ADC_OUT_MSB_REG, 2,
+			   (unsigned char *)data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	ut = (data[0] << 8) | data[1];
+
+	x1 = (((long) ut - (long)cal_param.ac6) * (long)cal_param.ac5) >> 15;
+	divisor = x1 + cal_param.md;
+	if (!divisor)
+		return INV_ERROR_DIVIDE_BY_ZERO;
+
+	x2 = ((long)cal_param.mc << 11) / (x1 + cal_param.md);
+	cal_param.param_b5 = x1 + x2;
+	/* temperature in 0.1 degree C */
+	temperature = (short)((cal_param.param_b5 + 8) >> 4);
+
+	/* get pressure */
+	result = inv_serial_single_write(mlsl_handle, pdata->address,
+				       PRESSURE_BMP085_CTRL_MEAS_REG,
+				       PRESSURE_BMP085_P_MEAS_OSS_0);
+	msleep(5);
+	result =
+	    inv_serial_read(mlsl_handle, pdata->address,
+			   PRESSURE_BMP085_ADC_OUT_MSB_REG, 2,
+			   (unsigned char *)data);
+	if (result) {
+		LOG_RESULT_LOCATION(result);
+		return result;
+	}
+	up = (((unsigned long) data[0] << 8) | ((unsigned long) data[1]));
+
+	b6 = cal_param.param_b5 - 4000;
+	/* calculate B3 */
+	x1 = (b6*b6) >> 12;
+	x1 *= cal_param.b2;
+	x1 >>= 11;
+
+	x2 = (cal_param.ac2*b6);
+	x2 >>= 11;
+
+	x3 = x1 + x2;
+
+	b3 = (((((long)cal_param.ac1) * 4 + x3)
+	    << oversampling_setting) + 2) >> 2;
+
+	/* calculate B4 */
+	x1 = (cal_param.ac3 * b6) >> 13;
+	x2 = (cal_param.b1 * ((b6*b6) >> 12)) >> 16;
+	x3 = ((x1 + x2) + 2) >> 2;
+	b4 = (cal_param.ac4 * (unsigned long) (x3 + 32768)) >> 15;
+	if (!b4)
+		return INV_ERROR;
+
+	b7 = ((unsigned long)(up - b3) * (50000>>oversampling_setting));
+	if (b7 < 0x80000000)
+		pressure = (b7 << 1) / b4;
+	else
+		pressure = (b7 / b4) << 1;
+
+	x1 = pressure >> 8;
+	x1 *= x1;
+	x1 = (x1 * PRESSURE_BMA085_PARAM_MG) >> 16;
+	x2 = (pressure * PRESSURE_BMA085_PARAM_MH) >> 16;
+	/* pressure in Pa */
+	pressure += (x1 + x2 + PRESSURE_BMA085_PARAM_MI) >> 4;
+
+	data[0] = (unsigned char)(pressure >> 16);
+	data[1] = (unsigned char)(pressure >> 8);
+	data[2] = (unsigned char)(pressure & 0xFF);
+
+	return result;
+}
+
+static struct ext_slave_descr bma085_descr = {
+	.init             = NULL,
+	.exit             = NULL,
+	.suspend          = bma085_suspend,
+	.resume           = bma085_resume,
+	.read             = bma085_read,
+	.config           = NULL,
+	.get_config       = NULL,
+	.name             = "bma085",
+	.type             = EXT_SLAVE_TYPE_PRESSURE,
+	.id               = PRESSURE_ID_BMA085,
+	.read_reg         = 0xF6,
+	.read_len         = 3,
+	.endian           = EXT_SLAVE_BIG_ENDIAN,
+	.range            = {0, 0},
+};
+
+static
+struct ext_slave_descr *bma085_get_slave_descr(void)
+{
+	return &bma085_descr;
+}
+
+/* Platform data for the MPU */
+struct bma085_mod_private_data {
+	struct i2c_client *client;
+	struct ext_slave_platform_data *pdata;
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+static int bma085_mod_probe(struct i2c_client *client,
+			   const struct i2c_device_id *devid)
+{
+	struct ext_slave_platform_data *pdata;
+	struct bma085_mod_private_data *private_data;
+	int result = 0;
+
+	dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENODEV;
+		goto out_no_free;
+	}
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->adapter->dev,
+			"Missing platform data for slave %s\n", devid->name);
+		result = -EFAULT;
+		goto out_no_free;
+	}
+
+	private_data = kzalloc(sizeof(*private_data), GFP_KERNEL);
+	if (!private_data) {
+		result = -ENOMEM;
+		goto out_no_free;
+	}
+
+	i2c_set_clientdata(client, private_data);
+	private_data->client = client;
+	private_data->pdata = pdata;
+
+	result = inv_mpu_register_slave(THIS_MODULE, client, pdata,
+					bma085_get_slave_descr);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Slave registration failed: %s, %d\n",
+			devid->name, result);
+		goto out_free_memory;
+	}
+
+	return result;
+
+out_free_memory:
+	kfree(private_data);
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+	return result;
+
+}
+
+static int bma085_mod_remove(struct i2c_client *client)
+{
+	struct bma085_mod_private_data *private_data =
+		i2c_get_clientdata(client);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	inv_mpu_unregister_slave(client, private_data->pdata,
+				bma085_get_slave_descr);
+
+	kfree(private_data);
+	return 0;
+}
+
+static const struct i2c_device_id bma085_mod_id[] = {
+	{ "bma085", PRESSURE_ID_BMA085 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, bma085_mod_id);
+
+static struct i2c_driver bma085_mod_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = bma085_mod_probe,
+	.remove = bma085_mod_remove,
+	.id_table = bma085_mod_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "bma085_mod",
+		   },
+	.address_list = normal_i2c,
+};
+
+static int __init bma085_mod_init(void)
+{
+	int res = i2c_add_driver(&bma085_mod_driver);
+	pr_info("%s: Probe name %s\n", __func__, "bma085_mod");
+	if (res)
+		pr_err("%s failed\n", __func__);
+	return res;
+}
+
+static void __exit bma085_mod_exit(void)
+{
+	pr_info("%s\n", __func__);
+	i2c_del_driver(&bma085_mod_driver);
+}
+
+module_init(bma085_mod_init);
+module_exit(bma085_mod_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Driver to integrate BMA085 sensor with the MPU");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("bma085_mod");
+/**
+ *  @}
+**/
diff --git a/drivers/misc/inv_mpu/slaveirq.c b/drivers/misc/inv_mpu/slaveirq.c
new file mode 100644
index 000000000000..22cfa4e458e8
--- /dev/null
+++ b/drivers/misc/inv_mpu/slaveirq.c
@@ -0,0 +1,268 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/irq.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/wait.h>
+#include <linux/slab.h>
+
+#include <linux/mpu.h>
+#include "slaveirq.h"
+#include "mldl_cfg.h"
+
+/* function which gets slave data and sends it to SLAVE */
+
+struct slaveirq_dev_data {
+	struct miscdevice dev;
+	struct i2c_client *slave_client;
+	struct mpuirq_data data;
+	wait_queue_head_t slaveirq_wait;
+	int irq;
+	int pid;
+	int data_ready;
+	int timeout;
+};
+
+/* The following depends on patch fa1f68db6ca7ebb6fc4487ac215bffba06c01c28
+ * drivers: misc: pass miscdevice pointer via file private data
+ */
+static int slaveirq_open(struct inode *inode, struct file *file)
+{
+	/* Device node is availabe in the file->private_data, this is
+	 * exactly what we want so we leave it there */
+	struct slaveirq_dev_data *data =
+	    container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+	dev_dbg(data->dev.this_device,
+		"%s current->pid %d\n", __func__, current->pid);
+	data->pid = current->pid;
+	return 0;
+}
+
+static int slaveirq_release(struct inode *inode, struct file *file)
+{
+	struct slaveirq_dev_data *data =
+	    container_of(file->private_data, struct slaveirq_dev_data, dev);
+	dev_dbg(data->dev.this_device, "slaveirq_release\n");
+	return 0;
+}
+
+/* read function called when from /dev/slaveirq is read */
+static ssize_t slaveirq_read(struct file *file,
+			     char *buf, size_t count, loff_t *ppos)
+{
+	int len, err;
+	struct slaveirq_dev_data *data =
+	    container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+	if (!data->data_ready && data->timeout &&
+	    !(file->f_flags & O_NONBLOCK)) {
+		wait_event_interruptible_timeout(data->slaveirq_wait,
+						 data->data_ready,
+						 data->timeout);
+	}
+
+	if (data->data_ready && NULL != buf && count >= sizeof(data->data)) {
+		err = copy_to_user(buf, &data->data, sizeof(data->data));
+		data->data.data_type = 0;
+	} else {
+		return 0;
+	}
+	if (err != 0) {
+		dev_err(data->dev.this_device,
+			"Copy to user returned %d\n", err);
+		return -EFAULT;
+	}
+	data->data_ready = 0;
+	len = sizeof(data->data);
+	return len;
+}
+
+static unsigned int slaveirq_poll(struct file *file,
+				  struct poll_table_struct *poll)
+{
+	int mask = 0;
+	struct slaveirq_dev_data *data =
+	    container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+	poll_wait(file, &data->slaveirq_wait, poll);
+	if (data->data_ready)
+		mask |= POLLIN | POLLRDNORM;
+	return mask;
+}
+
+/* ioctl - I/O control */
+static long slaveirq_ioctl(struct file *file,
+			   unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	int tmp;
+	struct slaveirq_dev_data *data =
+	    container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+	switch (cmd) {
+	case SLAVEIRQ_SET_TIMEOUT:
+		data->timeout = arg;
+		break;
+
+	case SLAVEIRQ_GET_INTERRUPT_CNT:
+		tmp = data->data.interruptcount - 1;
+		if (data->data.interruptcount > 1)
+			data->data.interruptcount = 1;
+
+		if (copy_to_user((int *)arg, &tmp, sizeof(int)))
+			return -EFAULT;
+		break;
+	case SLAVEIRQ_GET_IRQ_TIME:
+		if (copy_to_user((int *)arg, &data->data.irqtime,
+				 sizeof(data->data.irqtime)))
+			return -EFAULT;
+		data->data.irqtime = 0;
+		break;
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+static irqreturn_t slaveirq_handler(int irq, void *dev_id)
+{
+	struct slaveirq_dev_data *data = (struct slaveirq_dev_data *)dev_id;
+	static int mycount;
+	struct timeval irqtime;
+	mycount++;
+
+	data->data.interruptcount++;
+
+	/* wake up (unblock) for reading data from userspace */
+	data->data_ready = 1;
+
+	do_gettimeofday(&irqtime);
+	data->data.irqtime = (((long long)irqtime.tv_sec) << 32);
+	data->data.irqtime += irqtime.tv_usec;
+	data->data.data_type |= 1;
+
+	wake_up_interruptible(&data->slaveirq_wait);
+
+	return IRQ_HANDLED;
+
+}
+
+/* define which file operations are supported */
+static const struct file_operations slaveirq_fops = {
+	.owner = THIS_MODULE,
+	.read = slaveirq_read,
+	.poll = slaveirq_poll,
+
+#if HAVE_COMPAT_IOCTL
+	.compat_ioctl = slaveirq_ioctl,
+#endif
+#if HAVE_UNLOCKED_IOCTL
+	.unlocked_ioctl = slaveirq_ioctl,
+#endif
+	.open = slaveirq_open,
+	.release = slaveirq_release,
+};
+
+int slaveirq_init(struct i2c_adapter *slave_adapter,
+		  struct ext_slave_platform_data *pdata, char *name)
+{
+
+	int res;
+	struct slaveirq_dev_data *data;
+
+	if (!pdata->irq)
+		return -EINVAL;
+
+	pdata->irq_data = kzalloc(sizeof(*data), GFP_KERNEL);
+	data = (struct slaveirq_dev_data *)pdata->irq_data;
+	if (!data)
+		return -ENOMEM;
+
+	data->dev.minor = MISC_DYNAMIC_MINOR;
+	data->dev.name = name;
+	data->dev.fops = &slaveirq_fops;
+	data->irq = pdata->irq;
+	data->pid = 0;
+	data->data_ready = 0;
+	data->timeout = 0;
+
+	init_waitqueue_head(&data->slaveirq_wait);
+
+	res = request_irq(data->irq, slaveirq_handler,
+			IRQF_TRIGGER_RISING | IRQF_SHARED,
+			  data->dev.name, data);
+
+	if (res) {
+		dev_err(&slave_adapter->dev,
+			"myirqtest: cannot register IRQ %d\n", data->irq);
+		goto out_request_irq;
+	}
+
+	res = misc_register(&data->dev);
+	if (res < 0) {
+		dev_err(&slave_adapter->dev,
+			"misc_register returned %d\n", res);
+		goto out_misc_register;
+	}
+
+	return res;
+
+out_misc_register:
+	free_irq(data->irq, data);
+out_request_irq:
+	kfree(pdata->irq_data);
+	pdata->irq_data = NULL;
+
+	return res;
+}
+EXPORT_SYMBOL(slaveirq_init);
+
+void slaveirq_exit(struct ext_slave_platform_data *pdata)
+{
+	struct slaveirq_dev_data *data = pdata->irq_data;
+
+	if (!pdata->irq_data || data->irq <= 0)
+		return;
+
+	dev_info(data->dev.this_device, "Unregistering %s\n", data->dev.name);
+
+	free_irq(data->irq, data);
+	misc_deregister(&data->dev);
+	kfree(pdata->irq_data);
+	pdata->irq_data = NULL;
+}
+EXPORT_SYMBOL(slaveirq_exit);
diff --git a/drivers/misc/inv_mpu/slaveirq.h b/drivers/misc/inv_mpu/slaveirq.h
new file mode 100644
index 000000000000..6926634ff94c
--- /dev/null
+++ b/drivers/misc/inv_mpu/slaveirq.h
@@ -0,0 +1,36 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+#ifndef __SLAVEIRQ__
+#define __SLAVEIRQ__
+
+#include <linux/i2c-dev.h>
+
+#include <linux/mpu.h>
+#include "mpuirq.h"
+
+#define SLAVEIRQ_SET_TIMEOUT           _IOW(MPU_IOCTL, 0x50, unsigned long)
+#define SLAVEIRQ_GET_INTERRUPT_CNT     _IOR(MPU_IOCTL, 0x51, unsigned long)
+#define SLAVEIRQ_GET_IRQ_TIME          _IOR(MPU_IOCTL, 0x52, unsigned long)
+
+void slaveirq_exit(struct ext_slave_platform_data *pdata);
+int slaveirq_init(struct i2c_adapter *slave_adapter,
+		  struct ext_slave_platform_data *pdata, char *name);
+
+#endif
diff --git a/drivers/misc/inv_mpu/timerirq.c b/drivers/misc/inv_mpu/timerirq.c
new file mode 100644
index 000000000000..601858f9c4d5
--- /dev/null
+++ b/drivers/misc/inv_mpu/timerirq.c
@@ -0,0 +1,296 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/timer.h>
+#include <linux/slab.h>
+
+#include <linux/mpu.h>
+#include "mltypes.h"
+#include "timerirq.h"
+
+/* function which gets timer data and sends it to TIMER */
+struct timerirq_data {
+	int pid;
+	int data_ready;
+	int run;
+	int timeout;
+	unsigned long period;
+	struct mpuirq_data data;
+	struct completion timer_done;
+	wait_queue_head_t timerirq_wait;
+	struct timer_list timer;
+	struct miscdevice *dev;
+};
+
+static struct miscdevice *timerirq_dev_data;
+
+static void timerirq_handler(unsigned long arg)
+{
+	struct timerirq_data *data = (struct timerirq_data *)arg;
+	struct timeval irqtime;
+
+	data->data.interruptcount++;
+
+	data->data_ready = 1;
+
+	do_gettimeofday(&irqtime);
+	data->data.irqtime = (((long long)irqtime.tv_sec) << 32);
+	data->data.irqtime += irqtime.tv_usec;
+	data->data.data_type |= 1;
+
+	dev_dbg(data->dev->this_device,
+		"%s, %lld, %ld\n", __func__, data->data.irqtime,
+		(unsigned long)data);
+
+	wake_up_interruptible(&data->timerirq_wait);
+
+	if (data->run)
+		mod_timer(&data->timer,
+			  jiffies + msecs_to_jiffies(data->period));
+	else
+		complete(&data->timer_done);
+}
+
+static int start_timerirq(struct timerirq_data *data)
+{
+	dev_dbg(data->dev->this_device,
+		"%s current->pid %d\n", __func__, current->pid);
+
+	/* Timer already running... success */
+	if (data->run)
+		return 0;
+
+	/* Don't allow a period of 0 since this would fire constantly */
+	if (!data->period)
+		return -EINVAL;
+
+	data->run = true;
+	data->data_ready = false;
+
+	init_completion(&data->timer_done);
+	setup_timer(&data->timer, timerirq_handler, (unsigned long)data);
+
+	return mod_timer(&data->timer,
+			 jiffies + msecs_to_jiffies(data->period));
+}
+
+static int stop_timerirq(struct timerirq_data *data)
+{
+	dev_dbg(data->dev->this_device,
+		"%s current->pid %lx\n", __func__, (unsigned long)data);
+
+	if (data->run) {
+		data->run = false;
+		mod_timer(&data->timer, jiffies + 1);
+		wait_for_completion(&data->timer_done);
+	}
+	return 0;
+}
+
+/* The following depends on patch fa1f68db6ca7ebb6fc4487ac215bffba06c01c28
+ * drivers: misc: pass miscdevice pointer via file private data
+ */
+static int timerirq_open(struct inode *inode, struct file *file)
+{
+	/* Device node is availabe in the file->private_data, this is
+	 * exactly what we want so we leave it there */
+	struct miscdevice *dev_data = file->private_data;
+	struct timerirq_data *data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->dev = dev_data;
+	file->private_data = data;
+	data->pid = current->pid;
+	init_waitqueue_head(&data->timerirq_wait);
+
+	dev_dbg(data->dev->this_device,
+		"%s current->pid %d\n", __func__, current->pid);
+	return 0;
+}
+
+static int timerirq_release(struct inode *inode, struct file *file)
+{
+	struct timerirq_data *data = file->private_data;
+	dev_dbg(data->dev->this_device, "timerirq_release\n");
+	if (data->run)
+		stop_timerirq(data);
+	kfree(data);
+	return 0;
+}
+
+/* read function called when from /dev/timerirq is read */
+static ssize_t timerirq_read(struct file *file,
+			     char *buf, size_t count, loff_t *ppos)
+{
+	int len, err;
+	struct timerirq_data *data = file->private_data;
+
+	if (!data->data_ready && data->timeout &&
+	    !(file->f_flags & O_NONBLOCK)) {
+		wait_event_interruptible_timeout(data->timerirq_wait,
+						 data->data_ready,
+						 data->timeout);
+	}
+
+	if (data->data_ready && NULL != buf && count >= sizeof(data->data)) {
+		err = copy_to_user(buf, &data->data, sizeof(data->data));
+		data->data.data_type = 0;
+	} else {
+		return 0;
+	}
+	if (err != 0) {
+		dev_err(data->dev->this_device,
+			"Copy to user returned %d\n", err);
+		return -EFAULT;
+	}
+	data->data_ready = 0;
+	len = sizeof(data->data);
+	return len;
+}
+
+static unsigned int timerirq_poll(struct file *file,
+				  struct poll_table_struct *poll)
+{
+	int mask = 0;
+	struct timerirq_data *data = file->private_data;
+
+	poll_wait(file, &data->timerirq_wait, poll);
+	if (data->data_ready)
+		mask |= POLLIN | POLLRDNORM;
+	return mask;
+}
+
+/* ioctl - I/O control */
+static long timerirq_ioctl(struct file *file,
+			   unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	int tmp;
+	struct timerirq_data *data = file->private_data;
+
+	dev_dbg(data->dev->this_device,
+		"%s current->pid %d, %d, %ld\n",
+		__func__, current->pid, cmd, arg);
+
+	if (!data)
+		return -EFAULT;
+
+	switch (cmd) {
+	case TIMERIRQ_SET_TIMEOUT:
+		data->timeout = arg;
+		break;
+	case TIMERIRQ_GET_INTERRUPT_CNT:
+		tmp = data->data.interruptcount - 1;
+		if (data->data.interruptcount > 1)
+			data->data.interruptcount = 1;
+
+		if (copy_to_user((int *)arg, &tmp, sizeof(int)))
+			return -EFAULT;
+		break;
+	case TIMERIRQ_START:
+		data->period = arg;
+		retval = start_timerirq(data);
+		break;
+	case TIMERIRQ_STOP:
+		retval = stop_timerirq(data);
+		break;
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+/* define which file operations are supported */
+static const struct file_operations timerirq_fops = {
+	.owner = THIS_MODULE,
+	.read = timerirq_read,
+	.poll = timerirq_poll,
+
+#if HAVE_COMPAT_IOCTL
+	.compat_ioctl = timerirq_ioctl,
+#endif
+#if HAVE_UNLOCKED_IOCTL
+	.unlocked_ioctl = timerirq_ioctl,
+#endif
+	.open = timerirq_open,
+	.release = timerirq_release,
+};
+
+static int __init timerirq_init(void)
+{
+
+	int res;
+	static struct miscdevice *data;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+	timerirq_dev_data = data;
+	data->minor = MISC_DYNAMIC_MINOR;
+	data->name = "timerirq";
+	data->fops = &timerirq_fops;
+
+	res = misc_register(data);
+	if (res < 0) {
+		dev_err(data->this_device, "misc_register returned %d\n", res);
+		return res;
+	}
+
+	return res;
+}
+
+module_init(timerirq_init);
+
+static void __exit timerirq_exit(void)
+{
+	struct miscdevice *data = timerirq_dev_data;
+
+	dev_info(data->this_device, "Unregistering %s\n", data->name);
+
+	misc_deregister(data);
+	kfree(data);
+
+	timerirq_dev_data = NULL;
+}
+
+module_exit(timerirq_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Timer IRQ device driver.");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("timerirq");
diff --git a/drivers/misc/inv_mpu/timerirq.h b/drivers/misc/inv_mpu/timerirq.h
new file mode 100644
index 000000000000..f69f07a45a3b
--- /dev/null
+++ b/drivers/misc/inv_mpu/timerirq.h
@@ -0,0 +1,30 @@
+/*
+	$License:
+	Copyright (C) 2011 InvenSense 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, see <http://www.gnu.org/licenses/>.
+	$
+ */
+
+#ifndef __TIMERIRQ__
+#define __TIMERIRQ__
+
+#include <linux/mpu.h>
+
+#define TIMERIRQ_SET_TIMEOUT           _IOW(MPU_IOCTL, 0x60, unsigned long)
+#define TIMERIRQ_GET_INTERRUPT_CNT     _IOW(MPU_IOCTL, 0x61, unsigned long)
+#define TIMERIRQ_START                 _IOW(MPU_IOCTL, 0x62, unsigned long)
+#define TIMERIRQ_STOP                  _IO(MPU_IOCTL, 0x63)
+
+#endif
diff --git a/drivers/misc/max1749.c b/drivers/misc/max1749.c
new file mode 100644
index 000000000000..e98964289002
--- /dev/null
+++ b/drivers/misc/max1749.c
@@ -0,0 +1,118 @@
+/*
+ * drivers/misc/max1749.c
+ *
+ * Driver for MAX1749, vibrator motor driver.
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/regulator/consumer.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/hrtimer.h>
+#include <linux/delay.h>
+
+#include "../staging/android/timed_output.h"
+
+static struct regulator *regulator;
+static int timeout;
+
+static void vibrator_start(void)
+{
+	regulator_enable(regulator);
+}
+
+static void vibrator_stop(void)
+{
+	int ret;
+
+	ret = regulator_is_enabled(regulator);
+	if (ret > 0)
+		regulator_disable(regulator);
+}
+
+/*
+ * Timeout value can be changed from sysfs entry
+ * created by timed_output_dev.
+ * echo 100 > /sys/class/timed_output/vibrator/enable
+ */
+static void vibrator_enable(struct timed_output_dev *dev, int value)
+{
+	timeout = value;
+	if (!regulator)
+		return;
+
+	if (value) {
+		vibrator_start();
+		msleep(value);
+		vibrator_stop();
+	} else {
+		vibrator_stop();
+	}
+}
+
+/*
+ * Timeout value can be read from sysfs entry
+ * created by timed_output_dev.
+ * cat /sys/class/timed_output/vibrator/enable
+ */
+static int vibrator_get_time(struct timed_output_dev *dev)
+{
+	return timeout;
+}
+
+static struct timed_output_dev vibrator_dev = {
+	.name		= "vibrator",
+	.get_time	= vibrator_get_time,
+	.enable		= vibrator_enable,
+};
+
+static int __init vibrator_init(void)
+{
+	int status;
+
+	regulator = regulator_get(NULL, "vdd_vbrtr");
+	if (IS_ERR_OR_NULL(regulator)) {
+		pr_err("vibrator_init:Couldn't get regulator vdd_vbrtr\n");
+		regulator = NULL;
+		return PTR_ERR(regulator);
+	}
+
+	status = timed_output_dev_register(&vibrator_dev);
+
+	if (status) {
+		regulator_put(regulator);
+		regulator = NULL;
+	}
+	return status;
+}
+
+static void __exit vibrator_exit(void)
+{
+	if (regulator) {
+		timed_output_dev_unregister(&vibrator_dev);
+		regulator_put(regulator);
+		regulator = NULL;
+	}
+}
+
+MODULE_DESCRIPTION("timed output vibrator device");
+MODULE_AUTHOR("GPL");
+
+module_init(vibrator_init);
+module_exit(vibrator_exit);
diff --git a/drivers/misc/mpu3050/Kconfig b/drivers/misc/mpu3050/Kconfig
new file mode 100644
index 000000000000..de240fa0ad83
--- /dev/null
+++ b/drivers/misc/mpu3050/Kconfig
@@ -0,0 +1,65 @@
+
+menu "Motion Sensors Support"
+
+choice
+    tristate "Motion Processing Unit"
+    depends on I2C
+    optional
+
+config MPU_SENSORS_MPU3050
+    tristate "MPU3050"
+    help
+      If you say yes here you get support for the MPU3050 Gyroscope driver
+      This driver can also be built as a module.  If so, the module
+      will be called mpu3050.
+
+config MPU_SENSORS_MPU6000
+    tristate "MPU6000"
+    help
+      If you say yes here you get support for the MPU6000 Gyroscope driver
+      This driver can also be built as a module.  If so, the module
+      will be called mpu6000.
+
+endchoice
+
+choice
+    prompt "Accelerometer Type"
+    depends on MPU_SENSORS_MPU3050
+    optional
+
+config MPU_SENSORS_KXTF9
+    bool "Kionix KXTF9"
+    help
+      This enables support for the Kionix KXFT9 accelerometer
+
+endchoice
+
+choice
+    prompt "Compass Type"
+    depends on MPU_SENSORS_MPU6000 || MPU_SENSORS_MPU3050
+    optional
+
+config MPU_SENSORS_AK8975
+    bool "AKM ak8975"
+    help
+      This enables support for the AKM ak8975 compass
+
+endchoice
+
+config MPU_SENSORS_TIMERIRQ
+    tristate "Timer IRQ"
+    help
+    If you say yes here you get access to the timerirq device handle which
+    can be used to select on. This can be used instead of IRQ's, sleeping,
+    or timer threads. Reading from this device returns the same type of
+    information as reading from the MPU and slave IRQ's.
+
+config MPU_SENSORS_DEBUG
+    bool "MPU debug"
+    depends on MPU_SENSORS_MPU3050 || MPU_SENSORS_MPU6000 || MPU_SENSORS_TIMERIRQ
+    help
+      If you say yes here you get extra debug messages from the MPU3050
+      and other slave sensors.
+
+endmenu
+
diff --git a/drivers/misc/mpu3050/Makefile b/drivers/misc/mpu3050/Makefile
new file mode 100644
index 000000000000..89ac46fdac5b
--- /dev/null
+++ b/drivers/misc/mpu3050/Makefile
@@ -0,0 +1,132 @@
+
+# Kernel makefile for motions sensors
+#
+#
+
+# MPU
+obj-$(CONFIG_MPU_SENSORS_MPU3050)	+= mpu3050.o
+mpu3050-objs += mpuirq.o \
+	slaveirq.o \
+	mpu-dev.o \
+	mpu-i2c.o \
+	mlsl-kernel.o \
+	mlos-kernel.o \
+	$(MLLITE_DIR)mldl_cfg.o
+
+#
+# Accel options
+#
+ifdef CONFIG_MPU_SENSORS_ADXL346
+mpu3050-objs += $(MLLITE_DIR)accel/adxl346.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_BMA150
+mpu3050-objs += $(MLLITE_DIR)accel/bma150.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_BMA222
+mpu3050-objs += $(MLLITE_DIR)accel/bma222.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_KXSD9
+mpu3050-objs += $(MLLITE_DIR)accel/kxsd9.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_KXTF9
+mpu3050-objs += $(MLLITE_DIR)accel/kxtf9.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_LIS331DLH
+mpu3050-objs += $(MLLITE_DIR)accel/lis331.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_LIS3DH
+mpu3050-objs += $(MLLITE_DIR)accel/lis3dh.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_LSM303DLHA
+mpu3050-objs += $(MLLITE_DIR)accel/lsm303a.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_MMA8450
+mpu3050-objs += $(MLLITE_DIR)accel/mma8450.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_MMA845X
+mpu3050-objs += $(MLLITE_DIR)accel/mma845x.o
+endif
+
+#
+# Compass options
+#
+ifdef CONFIG_MPU_SENSORS_AK8975
+mpu3050-objs += $(MLLITE_DIR)compass/ak8975.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_AMI30X
+mpu3050-objs += $(MLLITE_DIR)compass/ami30x.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_AMI306
+mpu3050-objs += $(MLLITE_DIR)compass/ami306.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_HMC5883
+mpu3050-objs += $(MLLITE_DIR)compass/hmc5883.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_LSM303DLHM
+mpu3050-objs += $(MLLITE_DIR)compass/lsm303m.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_MMC314X
+mpu3050-objs += $(MLLITE_DIR)compass/mmc314x.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_YAS529
+mpu3050-objs += $(MLLITE_DIR)compass/yas529-kernel.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_YAS530
+mpu3050-objs += $(MLLITE_DIR)compass/yas530.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_HSCDTD002B
+mpu3050-objs += $(MLLITE_DIR)compass/hscdtd002b.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_HSCDTD004A
+mpu3050-objs += $(MLLITE_DIR)compass/hscdtd004a.o
+endif
+#
+# Pressure options
+#
+ifdef CONFIG_MPU_SENSORS_BMA085
+mpu3050-objs += $(MLLITE_DIR)pressure/bma085.o
+endif
+
+EXTRA_CFLAGS += -I$(M)/$(MLLITE_DIR) \
+                -I$(M)/../../include \
+		-Idrivers/misc/mpu3050 \
+                -Iinclude/linux
+
+obj-$(CONFIG_MPU_SENSORS_MPU6000)+= mpu6000.o
+mpu6000-objs += mpuirq.o \
+	slaveirq.o \
+	mpu-dev.o \
+	mpu-i2c.o \
+	mlsl-kernel.o \
+	mlos-kernel.o \
+	$(MLLITE_DIR)mldl_cfg.o \
+	$(MLLITE_DIR)accel/mantis.o
+
+ifdef CONFIG_MPU_SENSORS_AK8975
+mpu6000-objs += $(MLLITE_DIR)compass/ak8975.o
+endif
+
+ifdef CONFIG_MPU_SENSORS_MPU6000
+EXTRA_CFLAGS += -DM_HW
+endif
+
+obj-$(CONFIG_MPU_SENSORS_TIMERIRQ)+= timerirq.o
+
diff --git a/drivers/misc/mpu3050/accel/kxtf9.c b/drivers/misc/mpu3050/accel/kxtf9.c
new file mode 100644
index 000000000000..938cd572a8fd
--- /dev/null
+++ b/drivers/misc/mpu3050/accel/kxtf9.c
@@ -0,0 +1,669 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+
+/**
+ *  @defgroup   ACCELDL (Motion Library - Accelerometer Driver Layer)
+ *  @brief      Provides the interface to setup and handle an accelerometers
+ *              connected to the secondary I2C interface of the gyroscope.
+ *
+ *  @{
+ *      @file   kxtf9.c
+ *      @brief  Accelerometer setup and handling methods.
+*/
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#undef MPL_LOG_NDEBUG
+#define MPL_LOG_NDEBUG 1
+
+#ifdef __KERNEL__
+#include <linux/module.h>
+#endif
+
+#include "mpu.h"
+#include "mlsl.h"
+#include "mlos.h"
+
+#include <log.h>
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-acc"
+
+#define KXTF9_XOUT_HPF_L                (0x00) /* 0000 0000 */
+#define KXTF9_XOUT_HPF_H                (0x01) /* 0000 0001 */
+#define KXTF9_YOUT_HPF_L                (0x02) /* 0000 0010 */
+#define KXTF9_YOUT_HPF_H                (0x03) /* 0000 0011 */
+#define KXTF9_ZOUT_HPF_L                (0x04) /* 0001 0100 */
+#define KXTF9_ZOUT_HPF_H                (0x05) /* 0001 0101 */
+#define KXTF9_XOUT_L                    (0x06) /* 0000 0110 */
+#define KXTF9_XOUT_H                    (0x07) /* 0000 0111 */
+#define KXTF9_YOUT_L                    (0x08) /* 0000 1000 */
+#define KXTF9_YOUT_H                    (0x09) /* 0000 1001 */
+#define KXTF9_ZOUT_L                    (0x0A) /* 0001 1010 */
+#define KXTF9_ZOUT_H                    (0x0B) /* 0001 1011 */
+#define KXTF9_ST_RESP                   (0x0C) /* 0000 1100 */
+#define KXTF9_WHO_AM_I                  (0x0F) /* 0000 1111 */
+#define KXTF9_TILT_POS_CUR              (0x10) /* 0001 0000 */
+#define KXTF9_TILT_POS_PRE              (0x11) /* 0001 0001 */
+#define KXTF9_INT_SRC_REG1              (0x15) /* 0001 0101 */
+#define KXTF9_INT_SRC_REG2              (0x16) /* 0001 0110 */
+#define KXTF9_STATUS_REG                (0x18) /* 0001 1000 */
+#define KXTF9_INT_REL                   (0x1A) /* 0001 1010 */
+#define KXTF9_CTRL_REG1                 (0x1B) /* 0001 1011 */
+#define KXTF9_CTRL_REG2                 (0x1C) /* 0001 1100 */
+#define KXTF9_CTRL_REG3                 (0x1D) /* 0001 1101 */
+#define KXTF9_INT_CTRL_REG1             (0x1E) /* 0001 1110 */
+#define KXTF9_INT_CTRL_REG2             (0x1F) /* 0001 1111 */
+#define KXTF9_INT_CTRL_REG3             (0x20) /* 0010 0000 */
+#define KXTF9_DATA_CTRL_REG             (0x21) /* 0010 0001 */
+#define KXTF9_TILT_TIMER                (0x28) /* 0010 1000 */
+#define KXTF9_WUF_TIMER                 (0x29) /* 0010 1001 */
+#define KXTF9_TDT_TIMER                 (0x2B) /* 0010 1011 */
+#define KXTF9_TDT_H_THRESH              (0x2C) /* 0010 1100 */
+#define KXTF9_TDT_L_THRESH              (0x2D) /* 0010 1101 */
+#define KXTF9_TDT_TAP_TIMER             (0x2E) /* 0010 1110 */
+#define KXTF9_TDT_TOTAL_TIMER           (0x2F) /* 0010 1111 */
+#define KXTF9_TDT_LATENCY_TIMER         (0x30) /* 0011 0000 */
+#define KXTF9_TDT_WINDOW_TIMER          (0x31) /* 0011 0001 */
+#define KXTF9_WUF_THRESH                (0x5A) /* 0101 1010 */
+#define KXTF9_TILT_ANGLE                (0x5C) /* 0101 1100 */
+#define KXTF9_HYST_SET                  (0x5F) /* 0101 1111 */
+
+#define KXTF9_MAX_DUR (0xFF)
+#define KXTF9_MAX_THS (0xFF)
+#define KXTF9_THS_COUNTS_P_G (32)
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+struct kxtf9_config {
+	unsigned int odr; /* Output data rate mHz */
+	unsigned int fsr; /* full scale range mg */
+	unsigned int ths; /* Motion no-motion thseshold mg */
+	unsigned int dur; /* Motion no-motion duration ms */
+	unsigned int irq_type;
+	unsigned char reg_ths;
+	unsigned char reg_dur;
+	unsigned char reg_odr;
+	unsigned char reg_int_cfg1;
+	unsigned char reg_int_cfg2;
+	unsigned char ctrl_reg1;
+};
+
+struct kxtf9_private_data {
+	struct kxtf9_config suspend;
+	struct kxtf9_config resume;
+};
+
+/*****************************************
+    Accelerometer Initialization Functions
+*****************************************/
+
+static int kxtf9_set_ths(void *mlsl_handle,
+			struct ext_slave_platform_data *pdata,
+			struct kxtf9_config *config,
+			int apply,
+			long ths)
+{
+	int result = ML_SUCCESS;
+	if ((ths * KXTF9_THS_COUNTS_P_G / 1000) > KXTF9_MAX_THS)
+		ths = (KXTF9_MAX_THS * 1000) / KXTF9_THS_COUNTS_P_G;
+
+	if (ths < 0)
+		ths = 0;
+
+	config->ths = ths;
+	config->reg_ths = (unsigned char)
+		((long)(ths * KXTF9_THS_COUNTS_P_G) / 1000);
+	MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths);
+	if (apply)
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					KXTF9_WUF_THRESH,
+					config->reg_ths);
+	return result;
+}
+
+static int kxtf9_set_dur(void *mlsl_handle,
+			struct ext_slave_platform_data *pdata,
+			struct kxtf9_config *config,
+			int apply,
+			long dur)
+{
+	int result = ML_SUCCESS;
+	long reg_dur = (dur * config->odr) / 1000000;
+	config->dur = dur;
+
+	if (reg_dur > KXTF9_MAX_DUR)
+		reg_dur = KXTF9_MAX_DUR;
+
+	config->reg_dur = (unsigned char) reg_dur;
+	MPL_LOGV("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur);
+	if (apply)
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					KXTF9_WUF_TIMER,
+					(unsigned char)reg_dur);
+	return result;
+}
+
+/**
+ * Sets the IRQ to fire when one of the IRQ events occur.  Threshold and
+ * duration will not be used uless the type is MOT or NMOT.
+ *
+ * @param config configuration to apply to, suspend or resume
+ * @param irq_type The type of IRQ.  Valid values are
+ * - MPU_SLAVE_IRQ_TYPE_NONE
+ * - MPU_SLAVE_IRQ_TYPE_MOTION
+ * - MPU_SLAVE_IRQ_TYPE_DATA_READY
+ */
+static int kxtf9_set_irq(void *mlsl_handle,
+			struct ext_slave_platform_data *pdata,
+			struct kxtf9_config *config,
+			int apply,
+			long irq_type)
+{
+	int result = ML_SUCCESS;
+	struct kxtf9_private_data *private_data = pdata->private_data;
+
+	config->irq_type = (unsigned char)irq_type;
+	config->ctrl_reg1 &= ~0x22;
+	if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
+		config->ctrl_reg1 |= 0x20;
+		config->reg_int_cfg1 = 0x38;
+		config->reg_int_cfg2 = 0x00;
+	} else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
+		config->ctrl_reg1 |= 0x02;
+		if ((unsigned long) config ==
+			(unsigned long) &private_data->suspend)
+			config->reg_int_cfg1 = 0x34;
+		else
+			config->reg_int_cfg1 = 0x24;
+		config->reg_int_cfg2 = 0xE0;
+	} else {
+		config->reg_int_cfg1 = 0x00;
+		config->reg_int_cfg2 = 0x00;
+	}
+
+	if (apply) {
+		/* Must clear bit 7 before writing new configuration */
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					KXTF9_CTRL_REG1, 0x40);
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					KXTF9_INT_CTRL_REG1,
+					config->reg_int_cfg1);
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					KXTF9_INT_CTRL_REG2,
+					config->reg_int_cfg2);
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					KXTF9_CTRL_REG1,
+					config->ctrl_reg1);
+	}
+	MPL_LOGV("CTRL_REG1: %lx, INT_CFG1: %lx, INT_CFG2: %lx\n",
+		(unsigned long)config->ctrl_reg1,
+		(unsigned long)config->reg_int_cfg1,
+		(unsigned long)config->reg_int_cfg2);
+
+	return result;
+}
+
+/**
+ * Set the Output data rate for the particular configuration
+ *
+ * @param config Config to modify with new ODR
+ * @param odr Output data rate in units of 1/1000Hz
+ */
+static int kxtf9_set_odr(void *mlsl_handle,
+			struct ext_slave_platform_data *pdata,
+			struct kxtf9_config *config,
+			int apply,
+			long odr)
+{
+	unsigned char bits;
+	int result = ML_SUCCESS;
+
+	/* Data sheet says there is 12.5 hz, but that seems to produce a single
+	 * correct data value, thus we remove it from the table */
+	if (odr > 400000) {
+		config->odr = 800000;
+		bits = 0x06;
+	} else if (odr > 200000) {
+		config->odr = 400000;
+		bits = 0x05;
+	} else if (odr > 100000) {
+		config->odr = 200000;
+		bits = 0x04;
+	} else if (odr > 50000) {
+		config->odr = 100000;
+		bits = 0x03;
+	} else if (odr > 25000) {
+		config->odr = 50000;
+		bits = 0x02;
+	} else if (odr != 0) {
+		config->odr = 25000;
+		bits = 0x01;
+	} else {
+		config->odr = 0;
+		bits = 0;
+	}
+
+	config->reg_odr = bits;
+	kxtf9_set_dur(mlsl_handle, pdata,
+		config, apply, config->dur);
+	MPL_LOGV("ODR: %d, 0x%02x\n", config->odr, (int)config->ctrl_reg1);
+	if (apply) {
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					KXTF9_DATA_CTRL_REG,
+					config->reg_odr);
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					KXTF9_CTRL_REG1,
+					0x40);
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					KXTF9_CTRL_REG1,
+					config->ctrl_reg1);
+	}
+	return result;
+}
+
+/**
+ * Set the full scale range of the accels
+ *
+ * @param config pointer to configuration
+ * @param fsr requested full scale range
+ */
+static int kxtf9_set_fsr(void *mlsl_handle,
+			struct ext_slave_platform_data *pdata,
+			struct kxtf9_config *config,
+			int apply,
+			long fsr)
+{
+	int result = ML_SUCCESS;
+
+	config->ctrl_reg1 = (config->ctrl_reg1 & 0xE7);
+	if (fsr <= 2000) {
+		config->fsr = 2000;
+		config->ctrl_reg1 |= 0x00;
+	} else if (fsr <= 4000) {
+		config->fsr = 4000;
+		config->ctrl_reg1 |= 0x08;
+	} else {
+		config->fsr = 8000;
+		config->ctrl_reg1 |= 0x10;
+	}
+
+	MPL_LOGV("FSR: %d\n", config->fsr);
+	if (apply) {
+		/* Must clear bit 7 before writing new configuration */
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					KXTF9_CTRL_REG1, 0x40);
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					KXTF9_CTRL_REG1, config->ctrl_reg1);
+	}
+	return result;
+}
+
+static int kxtf9_suspend(void *mlsl_handle,
+			 struct ext_slave_descr *slave,
+			 struct ext_slave_platform_data *pdata)
+{
+	int result;
+	unsigned char data;
+	struct kxtf9_private_data *private_data = pdata->private_data;
+
+	/* Wake up */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				KXTF9_CTRL_REG1, 0x40);
+	ERROR_CHECK(result);
+	/* INT_CTRL_REG1: */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				KXTF9_INT_CTRL_REG1,
+				private_data->suspend.reg_int_cfg1);
+	ERROR_CHECK(result);
+	/* WUF_THRESH: */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				KXTF9_WUF_THRESH,
+				private_data->suspend.reg_ths);
+	ERROR_CHECK(result);
+	/* DATA_CTRL_REG */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				KXTF9_DATA_CTRL_REG,
+				private_data->suspend.reg_odr);
+	ERROR_CHECK(result);
+	/* WUF_TIMER */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				KXTF9_WUF_TIMER, private_data->suspend.reg_dur);
+	ERROR_CHECK(result);
+
+	/* Normal operation  */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				KXTF9_CTRL_REG1,
+				private_data->suspend.ctrl_reg1);
+	ERROR_CHECK(result);
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				KXTF9_INT_REL, 1, &data);
+	ERROR_CHECK(result);
+
+	return result;
+}
+
+/* full scale setting - register and mask */
+#define ACCEL_KIONIX_CTRL_REG      (0x1b)
+#define ACCEL_KIONIX_CTRL_MASK     (0x18)
+
+static int kxtf9_resume(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+	unsigned char data;
+	struct kxtf9_private_data *private_data = pdata->private_data;
+
+	/* Wake up */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				KXTF9_CTRL_REG1, 0x40);
+	ERROR_CHECK(result);
+	/* INT_CTRL_REG1: */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				KXTF9_INT_CTRL_REG1,
+				private_data->resume.reg_int_cfg1);
+	ERROR_CHECK(result);
+	/* WUF_THRESH: */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				KXTF9_WUF_THRESH, private_data->resume.reg_ths);
+	ERROR_CHECK(result);
+	/* DATA_CTRL_REG */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				KXTF9_DATA_CTRL_REG,
+				private_data->resume.reg_odr);
+	ERROR_CHECK(result);
+	/* WUF_TIMER */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				KXTF9_WUF_TIMER, private_data->resume.reg_dur);
+	ERROR_CHECK(result);
+
+	/* Normal operation  */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				KXTF9_CTRL_REG1,
+				private_data->resume.ctrl_reg1);
+	ERROR_CHECK(result);
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				KXTF9_INT_REL, 1, &data);
+	ERROR_CHECK(result);
+
+	return ML_SUCCESS;
+}
+
+static int kxtf9_init(void *mlsl_handle,
+		struct ext_slave_descr *slave,
+		struct ext_slave_platform_data *pdata)
+{
+
+	struct kxtf9_private_data *private_data;
+	int result = ML_SUCCESS;
+
+	private_data = (struct kxtf9_private_data *)
+		MLOSMalloc(sizeof(struct kxtf9_private_data));
+
+	if (!private_data)
+		return ML_ERROR_MEMORY_EXAUSTED;
+
+	/* RAM reset */
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				KXTF9_CTRL_REG1,
+				0x40); /* Fastest Reset */
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				KXTF9_DATA_CTRL_REG,
+				0x36); /* Fastest Reset */
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				KXTF9_CTRL_REG3, 0xcd); /* Reset */
+	ERROR_CHECK(result);
+	MLOSSleep(2);
+
+	pdata->private_data = private_data;
+
+	private_data->resume.ctrl_reg1 = 0xC0;
+	private_data->suspend.ctrl_reg1 = 0x40;
+
+	result = kxtf9_set_dur(mlsl_handle, pdata, &private_data->suspend,
+			FALSE, 1000);
+	ERROR_CHECK(result);
+	result = kxtf9_set_dur(mlsl_handle, pdata, &private_data->resume,
+			FALSE,  2540);
+	ERROR_CHECK(result);
+
+	result = kxtf9_set_odr(mlsl_handle, pdata, &private_data->suspend,
+			FALSE, 50000);
+	ERROR_CHECK(result);
+	result = kxtf9_set_odr(mlsl_handle, pdata, &private_data->resume,
+			FALSE, 200000);
+
+	result = kxtf9_set_fsr(mlsl_handle, pdata, &private_data->suspend,
+			FALSE, 2000);
+	ERROR_CHECK(result);
+	result = kxtf9_set_fsr(mlsl_handle, pdata, &private_data->resume,
+			FALSE, 2000);
+	ERROR_CHECK(result);
+
+	result = kxtf9_set_ths(mlsl_handle, pdata, &private_data->suspend,
+			FALSE, 80);
+	ERROR_CHECK(result);
+	result = kxtf9_set_ths(mlsl_handle, pdata, &private_data->resume,
+			FALSE, 40);
+	ERROR_CHECK(result);
+
+	result = kxtf9_set_irq(mlsl_handle, pdata, &private_data->suspend,
+			FALSE,
+			MPU_SLAVE_IRQ_TYPE_NONE);
+	ERROR_CHECK(result);
+	result = kxtf9_set_irq(mlsl_handle, pdata, &private_data->resume,
+			FALSE,
+			MPU_SLAVE_IRQ_TYPE_NONE);
+	ERROR_CHECK(result);
+	return result;
+}
+
+static int kxtf9_exit(void *mlsl_handle,
+			  struct ext_slave_descr *slave,
+			  struct ext_slave_platform_data *pdata)
+{
+	if (pdata->private_data)
+		return MLOSFree(pdata->private_data);
+	else
+		return ML_SUCCESS;
+}
+
+static int kxtf9_config(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			struct ext_slave_config *data)
+{
+	int retval;
+	long odr;
+	struct kxtf9_private_data *private_data = pdata->private_data;
+	if (!data->data)
+		return ML_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		return kxtf9_set_odr(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		odr = *((long *)data->data);
+		if (odr != 0)
+			private_data->resume.ctrl_reg1 |= 0x80;
+
+		retval = kxtf9_set_odr(mlsl_handle, pdata,
+				&private_data->resume,
+				data->apply,
+				odr);
+		return retval;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		return kxtf9_set_fsr(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		return kxtf9_set_fsr(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		return kxtf9_set_ths(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		return kxtf9_set_ths(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		return kxtf9_set_dur(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		return kxtf9_set_dur(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		return kxtf9_set_irq(mlsl_handle, pdata,
+					&private_data->suspend,
+					data->apply,
+					*((long *)data->data));
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		return kxtf9_set_irq(mlsl_handle, pdata,
+					&private_data->resume,
+					data->apply,
+					*((long *)data->data));
+	default:
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return ML_SUCCESS;
+}
+
+static int kxtf9_get_config(void *mlsl_handle,
+				struct ext_slave_descr *slave,
+				struct ext_slave_platform_data *pdata,
+				struct ext_slave_config *data)
+{
+	struct kxtf9_private_data *private_data = pdata->private_data;
+	if (!data->data)
+		return ML_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.odr;
+		break;
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.odr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.fsr;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_THS:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.ths;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.ths;
+		break;
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.dur;
+		break;
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.dur;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->suspend.irq_type;
+		break;
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+		(*(unsigned long *)data->data) =
+			(unsigned long) private_data->resume.irq_type;
+		break;
+	default:
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return ML_SUCCESS;
+}
+
+static int kxtf9_read(void *mlsl_handle,
+		      struct ext_slave_descr *slave,
+		      struct ext_slave_platform_data *pdata,
+		      unsigned char *data)
+{
+	int result;
+	unsigned char reg;
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				KXTF9_INT_SRC_REG2, 1, &reg);
+	ERROR_CHECK(result);
+
+	if (!(reg & 0x10))
+		return ML_ERROR_ACCEL_DATA_NOT_READY;
+
+	result = MLSLSerialRead(mlsl_handle, pdata->address,
+				slave->reg, slave->len, data);
+	ERROR_CHECK(result);
+	return result;
+}
+
+static struct ext_slave_descr kxtf9_descr = {
+	/*.init             = */ kxtf9_init,
+	/*.exit             = */ kxtf9_exit,
+	/*.suspend          = */ kxtf9_suspend,
+	/*.resume           = */ kxtf9_resume,
+	/*.read             = */ kxtf9_read,
+	/*.config           = */ kxtf9_config,
+	/*.get_config       = */ kxtf9_get_config,
+	/*.name             = */ "kxtf9",
+	/*.type             = */ EXT_SLAVE_TYPE_ACCELEROMETER,
+	/*.id               = */ ACCEL_ID_KXTF9,
+	/*.reg              = */ 0x06,
+	/*.len              = */ 6,
+	/*.endian           = */ EXT_SLAVE_LITTLE_ENDIAN,
+	/*.range            = */ {2, 0},
+};
+
+struct ext_slave_descr *kxtf9_get_slave_descr(void)
+{
+	return &kxtf9_descr;
+}
+EXPORT_SYMBOL(kxtf9_get_slave_descr);
+
+/**
+ *  @}
+**/
diff --git a/drivers/misc/mpu3050/compass/ak8975.c b/drivers/misc/mpu3050/compass/ak8975.c
new file mode 100644
index 000000000000..b8aed30ba39b
--- /dev/null
+++ b/drivers/misc/mpu3050/compass/ak8975.c
@@ -0,0 +1,258 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+
+/**
+ *  @defgroup   COMPASSDL (Motion Library - Accelerometer Driver Layer)
+ *  @brief      Provides the interface to setup and handle an accelerometers
+ *              connected to the secondary I2C interface of the gyroscope.
+ *
+ *  @{
+ *      @file   AK8975.c
+ *      @brief  Magnetometer setup and handling methods for AKM 8975 compass.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#include <string.h>
+
+#ifdef __KERNEL__
+#include <linux/module.h>
+#endif
+
+#include "mpu.h"
+#include "mlsl.h"
+#include "mlos.h"
+
+#include <log.h>
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+
+#define AK8975_REG_ST1  (0x02)
+#define AK8975_REG_HXL  (0x03)
+#define AK8975_REG_ST2  (0x09)
+
+#define AK8975_REG_CNTL (0x0A)
+
+#define AK8975_CNTL_MODE_POWER_DOWN         (0x00)
+#define AK8975_CNTL_MODE_SINGLE_MEASUREMENT (0x01)
+
+int ak8975_suspend(void *mlsl_handle,
+		   struct ext_slave_descr *slave,
+		   struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  AK8975_REG_CNTL,
+				  AK8975_CNTL_MODE_POWER_DOWN);
+	MLOSSleep(1);		/* wait at least 100us */
+	ERROR_CHECK(result);
+	return result;
+}
+
+int ak8975_resume(void *mlsl_handle,
+		  struct ext_slave_descr *slave,
+		  struct ext_slave_platform_data *pdata)
+{
+	int result = ML_SUCCESS;
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+				  AK8975_REG_CNTL,
+				  AK8975_CNTL_MODE_SINGLE_MEASUREMENT);
+	ERROR_CHECK(result);
+	return result;
+}
+
+int ak8975_read(void *mlsl_handle,
+		struct ext_slave_descr *slave,
+		struct ext_slave_platform_data *pdata, unsigned char *data)
+{
+	unsigned char regs[8];
+	unsigned char *stat = &regs[0];
+	unsigned char *stat2 = &regs[7];
+	int result = ML_SUCCESS;
+	int status = ML_SUCCESS;
+
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->address, AK8975_REG_ST1,
+			   8, regs);
+	ERROR_CHECK(result);
+
+	/*
+	 * ST : data ready -
+	 * Measurement has been completed and data is ready to be read.
+	 */
+	if (*stat & 0x01) {
+		memcpy(data, &regs[1], 6);
+		status = ML_SUCCESS;
+	}
+
+	/*
+	 * ST2 : data error -
+	 * occurs when data read is started outside of a readable period;
+	 * data read would not be correct.
+	 * Valid in continuous measurement mode only.
+	 * In single measurement mode this error should not occour but we
+	 * stil account for it and return an error, since the data would be
+	 * corrupted.
+	 * DERR bit is self-clearing when ST2 register is read.
+	 */
+	if (*stat2 & 0x04)
+		status = ML_ERROR_COMPASS_DATA_ERROR;
+	/*
+	 * ST2 : overflow -
+	 * the sum of the absolute values of all axis |X|+|Y|+|Z| < 2400uT.
+	 * This is likely to happen in presence of an external magnetic
+	 * disturbance; it indicates, the sensor data is incorrect and should
+	 * be ignored.
+	 * An error is returned.
+	 * HOFL bit clears when a new measurement starts.
+	 */
+	if (*stat2 & 0x08)
+		status = ML_ERROR_COMPASS_DATA_OVERFLOW;
+	/*
+	 * ST : overrun -
+	 * the previous sample was not fetched and lost.
+	 * Valid in continuous measurement mode only.
+	 * In single measurement mode this error should not occour and we
+	 * don't consider this condition an error.
+	 * DOR bit is self-clearing when ST2 or any meas. data register is
+	 * read.
+	 */
+	if (*stat & 0x02) {
+		/* status = ML_ERROR_COMPASS_DATA_UNDERFLOW; */
+		status = ML_SUCCESS;
+	}
+
+	/*
+	 * trigger next measurement if:
+	 *    - stat is non zero;
+	 *    - if stat is zero and stat2 is non zero.
+	 * Won't trigger if data is not ready and there was no error.
+	 */
+	if (*stat != 0x00 || *stat2 != 0x00) {
+		result =
+		    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
+					  AK8975_REG_CNTL,
+					  AK8975_CNTL_MODE_SINGLE_MEASUREMENT);
+		ERROR_CHECK(result);
+	}
+
+	return status;
+}
+
+static int ak8975_config(void *mlsl_handle,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			struct ext_slave_config *data)
+{
+	int result;
+	if (!data->data)
+		return ML_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_WRITE_REGISTERS:
+		result = MLSLSerialWrite(mlsl_handle, pdata->address,
+					data->len,
+					(unsigned char *)data->data);
+		ERROR_CHECK(result);
+		break;
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+	case MPU_SLAVE_CONFIG_MOT_THS:
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+	default:
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return ML_SUCCESS;
+}
+
+static int ak8975_get_config(void *mlsl_handle,
+				struct ext_slave_descr *slave,
+				struct ext_slave_platform_data *pdata,
+				struct ext_slave_config *data)
+{
+	int result;
+	if (!data->data)
+		return ML_ERROR_INVALID_PARAMETER;
+
+	switch (data->key) {
+	case MPU_SLAVE_READ_REGISTERS:
+	{
+		unsigned char *serial_data = (unsigned char *)data->data;
+		result = MLSLSerialRead(mlsl_handle, pdata->address,
+					serial_data[0],
+					data->len - 1,
+					&serial_data[1]);
+		ERROR_CHECK(result);
+		break;
+	}
+	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
+	case MPU_SLAVE_CONFIG_ODR_RESUME:
+	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
+	case MPU_SLAVE_CONFIG_FSR_RESUME:
+	case MPU_SLAVE_CONFIG_MOT_THS:
+	case MPU_SLAVE_CONFIG_NMOT_THS:
+	case MPU_SLAVE_CONFIG_MOT_DUR:
+	case MPU_SLAVE_CONFIG_NMOT_DUR:
+	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
+	case MPU_SLAVE_CONFIG_IRQ_RESUME:
+	default:
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+	};
+
+	return ML_SUCCESS;
+}
+
+struct ext_slave_descr ak8975_descr = {
+	/*.init             = */ NULL,
+	/*.exit             = */ NULL,
+	/*.suspend          = */ ak8975_suspend,
+	/*.resume           = */ ak8975_resume,
+	/*.read             = */ ak8975_read,
+	/*.config           = */ ak8975_config,
+	/*.get_config       = */ ak8975_get_config,
+	/*.name             = */ "ak8975",
+	/*.type             = */ EXT_SLAVE_TYPE_COMPASS,
+	/*.id               = */ COMPASS_ID_AKM,
+	/*.reg              = */ 0x01,
+	/*.len              = */ 9,
+	/*.endian           = */ EXT_SLAVE_LITTLE_ENDIAN,
+	/*.range            = */ {9830, 4000}
+};
+
+struct ext_slave_descr *ak8975_get_slave_descr(void)
+{
+	return &ak8975_descr;
+}
+EXPORT_SYMBOL(ak8975_get_slave_descr);
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/mpu3050/log.h b/drivers/misc/mpu3050/log.h
new file mode 100644
index 000000000000..f2f9ea7ece8e
--- /dev/null
+++ b/drivers/misc/mpu3050/log.h
@@ -0,0 +1,306 @@
+/*
+ * Copyright (C) 2010 InvenSense Inc
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ * C/C++ logging functions.  See the logging documentation for API details.
+ *
+ * We'd like these to be available from C code (in case we import some from
+ * somewhere), so this has a C interface.
+ *
+ * The output will be correct when the log file is shared between multiple
+ * threads and/or multiple processes so long as the operating system
+ * supports O_APPEND.  These calls have mutex-protected data structures
+ * and so are NOT reentrant.  Do not use MPL_LOG in a signal handler.
+ */
+#ifndef _LIBS_CUTILS_MPL_LOG_H
+#define _LIBS_CUTILS_MPL_LOG_H
+
+#include <stdarg.h>
+
+#ifdef ANDROID
+#include <utils/Log.h>		/* For the LOG macro */
+#endif
+
+#ifdef __KERNEL__
+#include <linux/kernel.h>
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Normally we strip MPL_LOGV (VERBOSE messages) from release builds.
+ * You can modify this (for example with "#define MPL_LOG_NDEBUG 0"
+ * at the top of your source file) to change that behavior.
+ */
+#ifndef MPL_LOG_NDEBUG
+#ifdef NDEBUG
+#define MPL_LOG_NDEBUG 1
+#else
+#define MPL_LOG_NDEBUG 0
+#endif
+#endif
+
+#ifdef __KERNEL__
+#define MPL_LOG_UNKNOWN MPL_LOG_VERBOSE
+#define MPL_LOG_DEFAULT KERN_DEFAULT
+#define MPL_LOG_VERBOSE KERN_CONT
+#define MPL_LOG_DEBUG   KERN_NOTICE
+#define MPL_LOG_INFO    KERN_INFO
+#define MPL_LOG_WARN    KERN_WARNING
+#define MPL_LOG_ERROR   KERN_ERR
+#define MPL_LOG_SILENT  MPL_LOG_VERBOSE
+
+#else
+	/* Based off the log priorities in android
+	   /system/core/include/android/log.h */
+#define MPL_LOG_UNKNOWN		(0)
+#define MPL_LOG_DEFAULT		(1)
+#define MPL_LOG_VERBOSE		(2)
+#define MPL_LOG_DEBUG		(3)
+#define MPL_LOG_INFO		(4)
+#define MPL_LOG_WARN		(5)
+#define MPL_LOG_ERROR		(6)
+#define MPL_LOG_SILENT		(8)
+#endif
+
+
+/*
+ * This is the local tag used for the following simplified
+ * logging macros.  You can change this preprocessor definition
+ * before using the other macros to change the tag.
+ */
+#ifndef MPL_LOG_TAG
+#ifdef __KERNEL__
+#define MPL_LOG_TAG
+#else
+#define MPL_LOG_TAG NULL
+#endif
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Simplified macro to send a verbose log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGV
+#if MPL_LOG_NDEBUG
+#define MPL_LOGV(fmt, ...)						\
+	do {								\
+		if (0)							\
+			MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\
+	} while (0)
+#else
+#define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef CONDITION
+#define CONDITION(cond)     ((cond) != 0)
+#endif
+
+#ifndef MPL_LOGV_IF
+#if MPL_LOG_NDEBUG
+#define MPL_LOGV_IF(cond, fmt, ...)  \
+	do { if (0) MPL_LOG(fmt, ##__VA_ARGS__); } while (0)
+#else
+#define MPL_LOGV_IF(cond, fmt, ...) \
+	((CONDITION(cond))						\
+		? MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \
+		: (void)0)
+#endif
+#endif
+
+/*
+ * Simplified macro to send a debug log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGD
+#define MPL_LOGD(fmt, ...) MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGD_IF
+#define MPL_LOGD_IF(cond, fmt, ...) \
+	((CONDITION(cond))					       \
+		? MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__)  \
+		: (void)0)
+#endif
+
+/*
+ * Simplified macro to send an info log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGI
+#define MPL_LOGI(fmt, ...) MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+
+#ifndef MPL_LOGI_IF
+#define MPL_LOGI_IF(cond, fmt, ...) \
+	((CONDITION(cond))                                              \
+		? MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__)   \
+		: (void)0)
+#endif
+
+/*
+ * Simplified macro to send a warning log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGW
+#ifdef __KERNEL__
+#define MPL_LOGW(fmt, ...) printk(KERN_WARNING MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOGW(fmt, ...) MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef MPL_LOGW_IF
+#define MPL_LOGW_IF(cond, fmt, ...) \
+	((CONDITION(cond))					       \
+		? MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__)   \
+		: (void)0)
+#endif
+
+/*
+ * Simplified macro to send an error log message using the current MPL_LOG_TAG.
+ */
+#ifndef MPL_LOGE
+#ifdef __KERNEL__
+#define MPL_LOGE(fmt, ...) printk(KERN_ERR MPL_LOG_TAG fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOGE(fmt, ...) MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+#ifndef MPL_LOGE_IF
+#define MPL_LOGE_IF(cond, fmt, ...) \
+	((CONDITION(cond))					       \
+		? MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__)  \
+		: (void)0)
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Log a fatal error.  If the given condition fails, this stops program
+ * execution like a normal assertion, but also generating the given message.
+ * It is NOT stripped from release builds.  Note that the condition test
+ * is -inverted- from the normal assert() semantics.
+ */
+#define MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ...) \
+	((CONDITION(cond))					   \
+		? ((void)android_printAssert(#cond, MPL_LOG_TAG,   \
+						fmt, ##__VA_ARGS__))	\
+		: (void)0)
+
+#define MPL_LOG_ALWAYS_FATAL(fmt, ...) \
+	(((void)android_printAssert(NULL, MPL_LOG_TAG, fmt, ##__VA_ARGS__)))
+
+/*
+ * Versions of MPL_LOG_ALWAYS_FATAL_IF and MPL_LOG_ALWAYS_FATAL that
+ * are stripped out of release builds.
+ */
+#if MPL_LOG_NDEBUG
+#define MPL_LOG_FATAL_IF(cond, fmt, ...)				\
+	do {								\
+		if (0)							\
+			MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__); \
+	} while (0)
+#define MPL_LOG_FATAL(fmt, ...)						\
+	do {								\
+		if (0)							\
+			MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)	\
+	} while (0)
+#else
+#define MPL_LOG_FATAL_IF(cond, fmt, ...) \
+	MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__)
+#define MPL_LOG_FATAL(fmt, ...) \
+	MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Assertion that generates a log message when the assertion fails.
+ * Stripped out of release builds.  Uses the current MPL_LOG_TAG.
+ */
+#define MPL_LOG_ASSERT(cond, fmt, ...)			\
+	MPL_LOG_FATAL_IF(!(cond), fmt, ##__VA_ARGS__)
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Basic log message macro.
+ *
+ * Example:
+ *  MPL_LOG(MPL_LOG_WARN, NULL, "Failed with error %d", errno);
+ *
+ * The second argument may be NULL or "" to indicate the "global" tag.
+ */
+#ifndef MPL_LOG
+#define MPL_LOG(priority, tag, fmt, ...)		\
+	MPL_LOG_PRI(priority, tag, fmt, ##__VA_ARGS__)
+#endif
+
+/*
+ * Log macro that allows you to specify a number for the priority.
+ */
+#ifndef MPL_LOG_PRI
+#ifdef ANDROID
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+	LOG(priority, tag, fmt, ##__VA_ARGS__)
+#elif defined __KERNEL__
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+	pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__)
+#else
+#define MPL_LOG_PRI(priority, tag, fmt, ...) \
+	_MLPrintLog(MPL_##priority, tag, fmt, ##__VA_ARGS__)
+#endif
+#endif
+
+/*
+ * Log macro that allows you to pass in a varargs ("args" is a va_list).
+ */
+#ifndef MPL_LOG_PRI_VA
+#ifdef ANDROID
+#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
+    android_vprintLog(priority, NULL, tag, fmt, args)
+#elif defined __KERNEL__
+/* not allowed in the Kernel because there is no dev_dbg that takes a va_list */
+#else
+#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \
+    _MLPrintVaLog(priority, NULL, tag, fmt, args)
+#endif
+#endif
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * ===========================================================================
+ *
+ * The stuff in the rest of this file should not be used directly.
+ */
+
+#ifndef ANDROID
+	int _MLPrintLog(int priority, const char *tag, const char *fmt,
+			...);
+	int _MLPrintVaLog(int priority, const char *tag, const char *fmt,
+			  va_list args);
+/* Final implementation of actual writing to a character device */
+	int _MLWriteLog(const char *buf, int buflen);
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+#endif				/* _LIBS_CUTILS_MPL_LOG_H */
diff --git a/drivers/misc/mpu3050/mldl_cfg.c b/drivers/misc/mpu3050/mldl_cfg.c
new file mode 100644
index 000000000000..9cc4cf690386
--- /dev/null
+++ b/drivers/misc/mpu3050/mldl_cfg.c
@@ -0,0 +1,1739 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+
+/**
+ *  @addtogroup MLDL
+ *
+ *  @{
+ *      @file   mldl_cfg.c
+ *      @brief  The Motion Library Driver Layer.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#include <stddef.h>
+
+#include "mldl_cfg.h"
+#include "mpu.h"
+
+#include "mlsl.h"
+#include "mlos.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "mldl_cfg:"
+
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+#ifdef M_HW
+#define SLEEP   0
+#define WAKE_UP 7
+#define RESET   1
+#define STANDBY 1
+#else
+/* licteral significance of all parameters used in MLDLPowerMgmtMPU */
+#define SLEEP   1
+#define WAKE_UP 0
+#define RESET   1
+#define STANDBY 1
+#endif
+
+/*---------------------*/
+/*-    Prototypes.    -*/
+/*---------------------*/
+
+/*----------------------*/
+/*-  Static Functions. -*/
+/*----------------------*/
+
+static int dmp_stop(struct mldl_cfg *mldl_cfg, void *gyro_handle)
+{
+	unsigned char userCtrlReg;
+	int result;
+
+	if (!mldl_cfg->dmp_is_running)
+		return ML_SUCCESS;
+
+	result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
+				MPUREG_USER_CTRL, 1, &userCtrlReg);
+	ERROR_CHECK(result);
+	userCtrlReg = (userCtrlReg & (~BIT_FIFO_EN)) | BIT_FIFO_RST;
+	userCtrlReg = (userCtrlReg & (~BIT_DMP_EN)) | BIT_DMP_RST;
+
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				       MPUREG_USER_CTRL, userCtrlReg);
+	ERROR_CHECK(result);
+	mldl_cfg->dmp_is_running = 0;
+
+	return result;
+
+}
+/**
+ * @brief Starts the DMP running
+ *
+ * @return ML_SUCCESS or non-zero error code
+ */
+static int dmp_start(struct mldl_cfg *pdata, void *mlsl_handle)
+{
+	unsigned char userCtrlReg;
+	int result;
+
+	if (pdata->dmp_is_running == pdata->dmp_enable)
+		return ML_SUCCESS;
+
+	result = MLSLSerialRead(mlsl_handle, pdata->addr,
+				MPUREG_USER_CTRL, 1, &userCtrlReg);
+	ERROR_CHECK(result);
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+				       MPUREG_USER_CTRL,
+				       ((userCtrlReg & (~BIT_FIFO_EN))
+						|   BIT_FIFO_RST));
+	ERROR_CHECK(result);
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+				       MPUREG_USER_CTRL, userCtrlReg);
+	ERROR_CHECK(result);
+
+	result = MLSLSerialRead(mlsl_handle, pdata->addr,
+				MPUREG_USER_CTRL, 1, &userCtrlReg);
+	ERROR_CHECK(result);
+
+	if (pdata->dmp_enable)
+		userCtrlReg |= BIT_DMP_EN;
+	else
+		userCtrlReg &= ~BIT_DMP_EN;
+
+	if (pdata->fifo_enable)
+		userCtrlReg |= BIT_FIFO_EN;
+	else
+		userCtrlReg &= ~BIT_FIFO_EN;
+
+	userCtrlReg |= BIT_DMP_RST;
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+				       MPUREG_USER_CTRL, userCtrlReg);
+	ERROR_CHECK(result);
+	pdata->dmp_is_running = pdata->dmp_enable;
+
+	return result;
+}
+
+/**
+ *  @brief  enables/disables the I2C bypass to an external device
+ *          connected to MPU's secondary I2C bus.
+ *  @param  enable
+ *              Non-zero to enable pass through.
+ *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+static int MLDLSetI2CBypass(struct mldl_cfg *mldl_cfg,
+			    void *mlsl_handle,
+			    unsigned char enable)
+{
+	unsigned char b;
+	int result;
+
+	if ((mldl_cfg->gyro_is_bypassed && enable) ||
+	    (!mldl_cfg->gyro_is_bypassed && !enable))
+		return ML_SUCCESS;
+
+	/*---- get current 'USER_CTRL' into b ----*/
+	result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
+				MPUREG_USER_CTRL, 1, &b);
+	ERROR_CHECK(result);
+
+	b &= ~BIT_AUX_IF_EN;
+
+	if (!enable) {
+		result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
+					       MPUREG_USER_CTRL,
+					       (b | BIT_AUX_IF_EN));
+		ERROR_CHECK(result);
+	} else {
+		/* Coming out of I2C is tricky due to several erratta.  Do not
+		 * modify this algorithm
+		 */
+		/*
+		 * 1) wait for the right time and send the command to change
+		 * the aux i2c slave address to an invalid address that will
+		 * get nack'ed
+		 *
+		 * 0x00 is broadcast.  0x7F is unlikely to be used by any aux.
+		 */
+		result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
+					       MPUREG_AUX_SLV_ADDR, 0x7F);
+		ERROR_CHECK(result);
+		/*
+		 * 2) wait enough time for a nack to occur, then go into
+		 *    bypass mode:
+		 */
+		MLOSSleep(2);
+		result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
+					       MPUREG_USER_CTRL, (b));
+		ERROR_CHECK(result);
+		/*
+		 * 3) wait for up to one MPU cycle then restore the slave
+		 *    address
+		 */
+		MLOSSleep(SAMPLING_PERIOD_US(mldl_cfg) / 1000);
+		result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
+					       MPUREG_AUX_SLV_ADDR,
+					       mldl_cfg->pdata->
+					       accel.address);
+		ERROR_CHECK(result);
+
+		/*
+		 * 4) reset the ime interface
+		 */
+#ifdef M_HW
+		result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
+					       MPUREG_USER_CTRL,
+					       (b | BIT_I2C_MST_RST));
+
+#else
+		result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
+					       MPUREG_USER_CTRL,
+					       (b | BIT_AUX_IF_RST));
+#endif
+		ERROR_CHECK(result);
+		MLOSSleep(2);
+	}
+	mldl_cfg->gyro_is_bypassed = enable;
+
+	return result;
+}
+
+struct tsProdRevMap {
+	unsigned char siliconRev;
+	unsigned short sensTrim;
+};
+
+#define NUM_OF_PROD_REVS (DIM(prodRevsMap))
+
+/* NOTE : 'npp' is a non production part */
+#ifdef M_HW
+#define OLDEST_PROD_REV_SUPPORTED 1
+static struct tsProdRevMap prodRevsMap[] = {
+	{0, 0},
+	{MPU_SILICON_REV_A1, 131},	/* 1 A1 (npp) */
+	{MPU_SILICON_REV_A1, 131},	/* 2 A1 (npp) */
+	{MPU_SILICON_REV_A1, 131},	/* 3 A1 (npp) */
+	{MPU_SILICON_REV_A1, 131},	/* 4 A1 (npp) */
+	{MPU_SILICON_REV_A1, 131},	/* 5 A1 (npp) */
+	{MPU_SILICON_REV_A1, 131},	/* 6 A1 (npp) */
+	{MPU_SILICON_REV_A1, 131},	/* 7 A1 (npp) */
+	{MPU_SILICON_REV_A1, 131},	/* 8 A1 (npp) */
+};
+
+#else				/* !M_HW */
+#define OLDEST_PROD_REV_SUPPORTED 11
+
+static struct tsProdRevMap prodRevsMap[] = {
+	{0, 0},
+	{MPU_SILICON_REV_A4, 131},	/* 1 A? OBSOLETED */
+	{MPU_SILICON_REV_A4, 131},	/* 2 | */
+	{MPU_SILICON_REV_A4, 131},	/* 3 V */
+	{MPU_SILICON_REV_A4, 131},	/* 4 */
+	{MPU_SILICON_REV_A4, 131},	/* 5 */
+	{MPU_SILICON_REV_A4, 131},	/* 6 */
+	{MPU_SILICON_REV_A4, 131},	/* 7 */
+	{MPU_SILICON_REV_A4, 131},	/* 8 */
+	{MPU_SILICON_REV_A4, 131},	/* 9 */
+	{MPU_SILICON_REV_A4, 131},	/* 10 */
+	{MPU_SILICON_REV_B1, 131},	/* 11 B1 */
+	{MPU_SILICON_REV_B1, 131},	/* 12 | */
+	{MPU_SILICON_REV_B1, 131},	/* 13 V */
+	{MPU_SILICON_REV_B1, 131},	/* 14 B4 */
+	{MPU_SILICON_REV_B4, 131},	/* 15 | */
+	{MPU_SILICON_REV_B4, 131},	/* 16 V */
+	{MPU_SILICON_REV_B4, 131},	/* 17 */
+	{MPU_SILICON_REV_B4, 131},	/* 18 */
+	{MPU_SILICON_REV_B4, 115},	/* 19  */
+	{MPU_SILICON_REV_B4, 115},	/* 20 */
+	{MPU_SILICON_REV_B6, 131},	/* 21 B6 (B6/A9)  */
+	{MPU_SILICON_REV_B4, 115},	/* 22 B4 (B7/A10) */
+	{MPU_SILICON_REV_B6, 0},	/* 23 B6 (npp)    */
+	{MPU_SILICON_REV_B6, 0},	/* 24 |  (npp)    */
+	{MPU_SILICON_REV_B6, 0},	/* 25 V  (npp)    */
+	{MPU_SILICON_REV_B6, 131},	/* 26    (B6/A11) */
+};
+#endif				/* !M_HW */
+
+/**
+ *  @internal
+ *  @brief  Get the silicon revision ID from OTP.
+ *          The silicon revision number is in read from OTP bank 0,
+ *          ADDR6[7:2].  The corresponding ID is retrieved by lookup
+ *          in a map.
+ *  @return The silicon revision ID (0 on error).
+ */
+static int MLDLGetSiliconRev(struct mldl_cfg *pdata,
+			     void *mlsl_handle)
+{
+	int result;
+	unsigned char index = 0x00;
+	unsigned char bank =
+	    (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0);
+	unsigned short memAddr = ((bank << 8) | 0x06);
+
+	result = MLSLSerialReadMem(mlsl_handle, pdata->addr,
+				   memAddr, 1, &index);
+	ERROR_CHECK(result);
+	if (result)
+		return result;
+	index >>= 2;
+
+	/* clean the prefetch and cfg user bank bits */
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+				  MPUREG_BANK_SEL, 0);
+	ERROR_CHECK(result);
+	if (result)
+		return result;
+
+	if (index < OLDEST_PROD_REV_SUPPORTED || NUM_OF_PROD_REVS <= index) {
+		pdata->silicon_revision = 0;
+		pdata->trim = 0;
+		MPL_LOGE("Unsupported Product Revision Detected : %d\n", index);
+		return ML_ERROR_INVALID_MODULE;
+	}
+
+	pdata->silicon_revision = prodRevsMap[index].siliconRev;
+	pdata->trim = prodRevsMap[index].sensTrim;
+
+	if (pdata->trim == 0) {
+		MPL_LOGE("sensitivity trim is 0"
+			 " - unsupported non production part.\n");
+		return ML_ERROR_INVALID_MODULE;
+	}
+
+	return result;
+}
+
+/**
+ *  @brief  Enable / Disable the use MPU's secondary I2C interface level
+ *          shifters.
+ *          When enabled the secondary I2C interface to which the external
+ *          device is connected runs at VDD voltage (main supply).
+ *          When disabled the 2nd interface runs at VDDIO voltage.
+ *          See the device specification for more details.
+ *
+ *  @note   using this API may produce unpredictable results, depending on how
+ *          the MPU and slave device are setup on the target platform.
+ *          Use of this API should entirely be restricted to system
+ *          integrators. Once the correct value is found, there should be no
+ *          need to change the level shifter at runtime.
+ *
+ *  @pre    Must be called after MLSerialOpen().
+ *  @note   Typically called before MLDmpOpen().
+ *
+ *  @param[in]  enable:
+ *                  0 to run at VDDIO (default),
+ *                  1 to run at VDD.
+ *
+ *  @return ML_SUCCESS if successfull, a non-zero error code otherwise.
+ */
+static int MLDLSetLevelShifterBit(struct mldl_cfg *pdata,
+				  void *mlsl_handle,
+				  unsigned char enable)
+{
+#ifndef M_HW
+	int result;
+	unsigned char reg;
+	unsigned char mask;
+	unsigned char regval;
+
+	if (0 == pdata->silicon_revision)
+		return ML_ERROR_INVALID_PARAMETER;
+
+	/*-- on parts before B6 the VDDIO bit is bit 7 of ACCEL_BURST_ADDR --
+	NOTE: this is incompatible with ST accelerometers where the VDDIO
+	bit MUST be set to enable ST's internal logic to autoincrement
+	the register address on burst reads --*/
+	if ((pdata->silicon_revision & 0xf) < MPU_SILICON_REV_B6) {
+		reg = MPUREG_ACCEL_BURST_ADDR;
+		mask = 0x80;
+	} else {
+		/*-- on B6 parts the VDDIO bit was moved to FIFO_EN2 =>
+		  the mask is always 0x04 --*/
+		reg = MPUREG_FIFO_EN2;
+		mask = 0x04;
+	}
+
+	result = MLSLSerialRead(mlsl_handle, pdata->addr, reg, 1, &regval);
+	if (result)
+		return result;
+
+	if (enable)
+		regval |= mask;
+	else
+		regval &= ~mask;
+
+	result =
+	    MLSLSerialWriteSingle(mlsl_handle, pdata->addr, reg, regval);
+
+	return result;
+#else
+	return ML_SUCCESS;
+#endif
+}
+
+
+#ifdef M_HW
+/**
+ *  @internal
+ * @param reset 1 to reset hardware
+ */
+static tMLError mpu60xx_pwr_mgmt(struct mldl_cfg *pdata,
+				 void *mlsl_handle,
+				 unsigned char reset,
+				 unsigned char powerselection)
+{
+	unsigned char b;
+	tMLError result;
+
+	if (powerselection < 0 || powerselection > 7)
+		return ML_ERROR_INVALID_PARAMETER;
+
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGMT_1, 1,
+			   &b);
+	ERROR_CHECK(result);
+
+	b &= ~(BITS_PWRSEL);
+
+	if (reset) {
+		/* Current sillicon has an errata where the reset will get
+		 * nacked.  Ignore the error code for now. */
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+					       MPUREG_PWR_MGM, b | BIT_H_RESET);
+#define M_HW_RESET_ERRATTA
+#ifndef M_HW_RESET_ERRATTA
+		ERROR_CHECK(result);
+#else
+		MLOSSleep(50);
+#endif
+	}
+
+	b |= (powerselection << 4);
+
+	if (b & BITS_PWRSEL)
+		pdata->gyro_is_suspended = FALSE;
+	else
+		pdata->gyro_is_suspended = TRUE;
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+				       MPUREG_PWR_MGM, b);
+	ERROR_CHECK(result);
+
+	return ML_SUCCESS;
+}
+
+/**
+ *  @internal
+ */
+static tMLError MLDLStandByGyros(struct mldl_cfg *pdata,
+				 void *mlsl_handle,
+				 unsigned char disable_gx,
+				 unsigned char disable_gy,
+				 unsigned char disable_gz)
+{
+	unsigned char b;
+	tMLError result;
+
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGMT_2, 1,
+			   &b);
+	ERROR_CHECK(result);
+
+	b &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
+	b |= (disable_gx << 2 | disable_gy << 1 | disable_gz);
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+				       MPUREG_PWR_MGMT_2, b);
+	ERROR_CHECK(result);
+
+	return ML_SUCCESS;
+}
+
+/**
+ *  @internal
+ */
+static tMLError MLDLStandByAccels(struct mldl_cfg *pdata,
+				  void *mlsl_handle,
+				  unsigned char disable_ax,
+				  unsigned char disable_ay,
+				  unsigned char disable_az)
+{
+	unsigned char b;
+	tMLError result;
+
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGMT_2, 1,
+			   &b);
+	ERROR_CHECK(result);
+
+	b &= ~(BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
+	b |= (disable_ax << 2 | disable_ay << 1 | disable_az);
+
+	result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+				       MPUREG_PWR_MGMT_2, b);
+	ERROR_CHECK(result);
+
+	return ML_SUCCESS;
+}
+
+#else				/* ! M_HW */
+
+/**
+ * @internal
+ * @brief   This function controls the power management on the MPU device.
+ *          The entire chip can be put to low power sleep mode, or individual
+ *          gyros can be turned on/off.
+ *
+ *          Putting the device into sleep mode depending upon the changing needs
+ *          of the associated applications is a recommended method for reducing
+ *          power consuption.  It is a safe opearation in that sleep/wake up of
+ *          gyros while running will not result in any interruption of data.
+ *
+ *          Although it is entirely allowed to put the device into full sleep
+ *          while running the DMP, it is not recomended because it will disrupt
+ *          the ongoing calculations carried on inside the DMP and consequently
+ *          the sensor fusion algorithm. Furthermore, while in sleep mode
+ *          read & write operation from the app processor on both registers and
+ *          memory are disabled and can only regained by restoring the MPU in
+ *          normal power mode.
+ *          Disabling any of the gyro axis will reduce the associated power
+ *          consuption from the PLL but will not stop the DMP from running
+ *          state.
+ *
+ * @param   reset
+ *              Non-zero to reset the device. Note that this setting
+ *              is volatile and the corresponding register bit will
+ *              clear itself right after being applied.
+ * @param   sleep
+ *              Non-zero to put device into full sleep.
+ * @param   disable_gx
+ *              Non-zero to disable gyro X.
+ * @param   disable_gy
+ *              Non-zero to disable gyro Y.
+ * @param   disable_gz
+ *              Non-zero to disable gyro Z.
+ *
+ * @return  ML_SUCCESS if successfull; a non-zero error code otherwise.
+ */
+static int MLDLPowerMgmtMPU(struct mldl_cfg *pdata,
+			    void *mlsl_handle,
+			    unsigned char reset,
+			    unsigned char sleep,
+			    unsigned char disable_gx,
+			    unsigned char disable_gy,
+			    unsigned char disable_gz)
+{
+	unsigned char b;
+	int result;
+
+	result =
+	    MLSLSerialRead(mlsl_handle, pdata->addr, MPUREG_PWR_MGM, 1,
+			   &b);
+	ERROR_CHECK(result);
+
+	/* If we are awake, we need to put it in bypass before resetting */
+	if ((!(b & BIT_SLEEP)) && reset)
+		result = MLDLSetI2CBypass(pdata, mlsl_handle, 1);
+
+	/* If we are awake, we need stop the dmp sleeping */
+	if ((!(b & BIT_SLEEP)) && sleep)
+		dmp_stop(pdata, mlsl_handle);
+
+	/* Reset if requested */
+	if (reset) {
+		MPL_LOGV("Reset MPU3050\n");
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+					MPUREG_PWR_MGM, b | BIT_H_RESET);
+		ERROR_CHECK(result);
+		MLOSSleep(5);
+		pdata->gyro_needs_reset = FALSE;
+		/* Some chips are awake after reset and some are asleep,
+		 * check the status */
+		result = MLSLSerialRead(mlsl_handle, pdata->addr,
+					MPUREG_PWR_MGM, 1, &b);
+		ERROR_CHECK(result);
+	}
+
+	/* Update the suspended state just in case we return early */
+	if (b & BIT_SLEEP)
+		pdata->gyro_is_suspended = TRUE;
+	else
+		pdata->gyro_is_suspended = FALSE;
+
+	/* if power status match requested, nothing else's left to do */
+	if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) ==
+		(((sleep != 0) * BIT_SLEEP) |
+		((disable_gx != 0) * BIT_STBY_XG) |
+		((disable_gy != 0) * BIT_STBY_YG) |
+		((disable_gz != 0) * BIT_STBY_ZG))) {
+		return ML_SUCCESS;
+	}
+
+	/*
+	 * This specific transition between states needs to be reinterpreted:
+	 *    (1,1,1,1) -> (0,1,1,1) has to become
+	 *    (1,1,1,1) -> (1,0,0,0) -> (0,1,1,1)
+	 * where
+	 *    (1,1,1,1) is (sleep=1,disable_gx=1,disable_gy=1,disable_gz=1)
+	 */
+	if ((b & (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)) ==
+		 (BIT_SLEEP | BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
+		&& ((!sleep) && disable_gx && disable_gy && disable_gz)) {
+		result = MLDLPowerMgmtMPU(pdata, mlsl_handle, 0, 1, 0, 0, 0);
+		if (result)
+			return result;
+		b |= BIT_SLEEP;
+		b &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
+	}
+
+	if ((b & BIT_SLEEP) != ((sleep != 0) * BIT_SLEEP)) {
+		if (sleep) {
+			result = MLDLSetI2CBypass(pdata, mlsl_handle, 1);
+			ERROR_CHECK(result);
+			b |= BIT_SLEEP;
+			result =
+			    MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+						  MPUREG_PWR_MGM, b);
+			ERROR_CHECK(result);
+			pdata->gyro_is_suspended = TRUE;
+		} else {
+			b &= ~BIT_SLEEP;
+			result =
+			    MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+						  MPUREG_PWR_MGM, b);
+			ERROR_CHECK(result);
+			pdata->gyro_is_suspended = FALSE;
+			MLOSSleep(5);
+		}
+	}
+	/*---
+	  WORKAROUND FOR PUTTING GYRO AXIS in STAND-BY MODE
+	  1) put one axis at a time in stand-by
+	  ---*/
+	if ((b & BIT_STBY_XG) != ((disable_gx != 0) * BIT_STBY_XG)) {
+		b ^= BIT_STBY_XG;
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+						MPUREG_PWR_MGM, b);
+		ERROR_CHECK(result);
+	}
+	if ((b & BIT_STBY_YG) != ((disable_gy != 0) * BIT_STBY_YG)) {
+		b ^= BIT_STBY_YG;
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+					       MPUREG_PWR_MGM, b);
+		ERROR_CHECK(result);
+	}
+	if ((b & BIT_STBY_ZG) != ((disable_gz != 0) * BIT_STBY_ZG)) {
+		b ^= BIT_STBY_ZG;
+		result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
+					       MPUREG_PWR_MGM, b);
+		ERROR_CHECK(result);
+	}
+
+	return ML_SUCCESS;
+}
+#endif				/* M_HW */
+
+
+void mpu_print_cfg(struct mldl_cfg *mldl_cfg)
+{
+	struct mpu3050_platform_data *pdata = mldl_cfg->pdata;
+	struct ext_slave_platform_data *accel = &mldl_cfg->pdata->accel;
+	struct ext_slave_platform_data *compass =
+	    &mldl_cfg->pdata->compass;
+	struct ext_slave_platform_data *pressure =
+	    &mldl_cfg->pdata->pressure;
+
+	MPL_LOGD("mldl_cfg.addr             = %02x\n", mldl_cfg->addr);
+	MPL_LOGD("mldl_cfg.int_config       = %02x\n",
+		 mldl_cfg->int_config);
+	MPL_LOGD("mldl_cfg.ext_sync         = %02x\n", mldl_cfg->ext_sync);
+	MPL_LOGD("mldl_cfg.full_scale       = %02x\n",
+		 mldl_cfg->full_scale);
+	MPL_LOGD("mldl_cfg.lpf              = %02x\n", mldl_cfg->lpf);
+	MPL_LOGD("mldl_cfg.clk_src          = %02x\n", mldl_cfg->clk_src);
+	MPL_LOGD("mldl_cfg.divider          = %02x\n", mldl_cfg->divider);
+	MPL_LOGD("mldl_cfg.dmp_enable       = %02x\n",
+		 mldl_cfg->dmp_enable);
+	MPL_LOGD("mldl_cfg.fifo_enable      = %02x\n",
+		 mldl_cfg->fifo_enable);
+	MPL_LOGD("mldl_cfg.dmp_cfg1         = %02x\n", mldl_cfg->dmp_cfg1);
+	MPL_LOGD("mldl_cfg.dmp_cfg2         = %02x\n", mldl_cfg->dmp_cfg2);
+	MPL_LOGD("mldl_cfg.offset_tc[0]     = %02x\n",
+		 mldl_cfg->offset_tc[0]);
+	MPL_LOGD("mldl_cfg.offset_tc[1]     = %02x\n",
+		 mldl_cfg->offset_tc[1]);
+	MPL_LOGD("mldl_cfg.offset_tc[2]     = %02x\n",
+		 mldl_cfg->offset_tc[2]);
+	MPL_LOGD("mldl_cfg.silicon_revision = %02x\n",
+		 mldl_cfg->silicon_revision);
+	MPL_LOGD("mldl_cfg.product_id       = %02x\n",
+		 mldl_cfg->product_id);
+	MPL_LOGD("mldl_cfg.trim             = %02x\n", mldl_cfg->trim);
+	MPL_LOGD("mldl_cfg.requested_sensors= %04lx\n",
+		 mldl_cfg->requested_sensors);
+
+	if (mldl_cfg->accel) {
+		MPL_LOGD("slave_accel->suspend      = %02x\n",
+			 (int) mldl_cfg->accel->suspend);
+		MPL_LOGD("slave_accel->resume       = %02x\n",
+			 (int) mldl_cfg->accel->resume);
+		MPL_LOGD("slave_accel->read         = %02x\n",
+			 (int) mldl_cfg->accel->read);
+		MPL_LOGD("slave_accel->type         = %02x\n",
+			 mldl_cfg->accel->type);
+		MPL_LOGD("slave_accel->reg          = %02x\n",
+			 mldl_cfg->accel->reg);
+		MPL_LOGD("slave_accel->len          = %02x\n",
+			 mldl_cfg->accel->len);
+		MPL_LOGD("slave_accel->endian       = %02x\n",
+			 mldl_cfg->accel->endian);
+		MPL_LOGD("slave_accel->range.mantissa= %02lx\n",
+			 mldl_cfg->accel->range.mantissa);
+		MPL_LOGD("slave_accel->range.fraction= %02lx\n",
+			 mldl_cfg->accel->range.fraction);
+	} else {
+		MPL_LOGD("slave_accel               = NULL\n");
+	}
+
+	if (mldl_cfg->compass) {
+		MPL_LOGD("slave_compass->suspend    = %02x\n",
+			 (int) mldl_cfg->compass->suspend);
+		MPL_LOGD("slave_compass->resume     = %02x\n",
+			 (int) mldl_cfg->compass->resume);
+		MPL_LOGD("slave_compass->read       = %02x\n",
+			 (int) mldl_cfg->compass->read);
+		MPL_LOGD("slave_compass->type       = %02x\n",
+			 mldl_cfg->compass->type);
+		MPL_LOGD("slave_compass->reg        = %02x\n",
+			 mldl_cfg->compass->reg);
+		MPL_LOGD("slave_compass->len        = %02x\n",
+			 mldl_cfg->compass->len);
+		MPL_LOGD("slave_compass->endian     = %02x\n",
+			 mldl_cfg->compass->endian);
+		MPL_LOGD("slave_compass->range.mantissa= %02lx\n",
+			 mldl_cfg->compass->range.mantissa);
+		MPL_LOGD("slave_compass->range.fraction= %02lx\n",
+			 mldl_cfg->compass->range.fraction);
+
+	} else {
+		MPL_LOGD("slave_compass             = NULL\n");
+	}
+
+	if (mldl_cfg->pressure) {
+		MPL_LOGD("slave_pressure->suspend    = %02x\n",
+			 (int) mldl_cfg->pressure->suspend);
+		MPL_LOGD("slave_pressure->resume     = %02x\n",
+			 (int) mldl_cfg->pressure->resume);
+		MPL_LOGD("slave_pressure->read       = %02x\n",
+			 (int) mldl_cfg->pressure->read);
+		MPL_LOGD("slave_pressure->type       = %02x\n",
+			 mldl_cfg->pressure->type);
+		MPL_LOGD("slave_pressure->reg        = %02x\n",
+			 mldl_cfg->pressure->reg);
+		MPL_LOGD("slave_pressure->len        = %02x\n",
+			 mldl_cfg->pressure->len);
+		MPL_LOGD("slave_pressure->endian     = %02x\n",
+			 mldl_cfg->pressure->endian);
+		MPL_LOGD("slave_pressure->range.mantissa= %02lx\n",
+			 mldl_cfg->pressure->range.mantissa);
+		MPL_LOGD("slave_pressure->range.fraction= %02lx\n",
+			 mldl_cfg->pressure->range.fraction);
+
+	} else {
+		MPL_LOGD("slave_pressure             = NULL\n");
+	}
+	MPL_LOGD("accel->get_slave_descr    = %x\n",
+		 (unsigned int) accel->get_slave_descr);
+	MPL_LOGD("accel->irq                = %02x\n", accel->irq);
+	MPL_LOGD("accel->adapt_num          = %02x\n", accel->adapt_num);
+	MPL_LOGD("accel->bus                = %02x\n", accel->bus);
+	MPL_LOGD("accel->address            = %02x\n", accel->address);
+	MPL_LOGD("accel->orientation        =\n"
+		 "                            %2d %2d %2d\n"
+		 "                            %2d %2d %2d\n"
+		 "                            %2d %2d %2d\n",
+		 accel->orientation[0], accel->orientation[1],
+		 accel->orientation[2], accel->orientation[3],
+		 accel->orientation[4], accel->orientation[5],
+		 accel->orientation[6], accel->orientation[7],
+		 accel->orientation[8]);
+	MPL_LOGD("compass->get_slave_descr  = %x\n",
+		 (unsigned int) compass->get_slave_descr);
+	MPL_LOGD("compass->irq              = %02x\n", compass->irq);
+	MPL_LOGD("compass->adapt_num        = %02x\n", compass->adapt_num);
+	MPL_LOGD("compass->bus              = %02x\n", compass->bus);
+	MPL_LOGD("compass->address          = %02x\n", compass->address);
+	MPL_LOGD("compass->orientation      =\n"
+		 "                            %2d %2d %2d\n"
+		 "                            %2d %2d %2d\n"
+		 "                            %2d %2d %2d\n",
+		 compass->orientation[0], compass->orientation[1],
+		 compass->orientation[2], compass->orientation[3],
+		 compass->orientation[4], compass->orientation[5],
+		 compass->orientation[6], compass->orientation[7],
+		 compass->orientation[8]);
+	MPL_LOGD("pressure->get_slave_descr  = %x\n",
+		 (unsigned int) pressure->get_slave_descr);
+	MPL_LOGD("pressure->irq             = %02x\n", pressure->irq);
+	MPL_LOGD("pressure->adapt_num       = %02x\n", pressure->adapt_num);
+	MPL_LOGD("pressure->bus             = %02x\n", pressure->bus);
+	MPL_LOGD("pressure->address         = %02x\n", pressure->address);
+	MPL_LOGD("pressure->orientation     =\n"
+		 "                            %2d %2d %2d\n"
+		 "                            %2d %2d %2d\n"
+		 "                            %2d %2d %2d\n",
+		 pressure->orientation[0], pressure->orientation[1],
+		 pressure->orientation[2], pressure->orientation[3],
+		 pressure->orientation[4], pressure->orientation[5],
+		 pressure->orientation[6], pressure->orientation[7],
+		 pressure->orientation[8]);
+
+	MPL_LOGD("pdata->int_config         = %02x\n", pdata->int_config);
+	MPL_LOGD("pdata->level_shifter      = %02x\n",
+		 pdata->level_shifter);
+	MPL_LOGD("pdata->orientation        =\n"
+		 "                            %2d %2d %2d\n"
+		 "                            %2d %2d %2d\n"
+		 "                            %2d %2d %2d\n",
+		 pdata->orientation[0], pdata->orientation[1],
+		 pdata->orientation[2], pdata->orientation[3],
+		 pdata->orientation[4], pdata->orientation[5],
+		 pdata->orientation[6], pdata->orientation[7],
+		 pdata->orientation[8]);
+
+	MPL_LOGD("Struct sizes: mldl_cfg: %d, "
+		 "ext_slave_descr:%d, "
+		 "mpu3050_platform_data:%d: RamOffset: %d\n",
+		 sizeof(struct mldl_cfg), sizeof(struct ext_slave_descr),
+		 sizeof(struct mpu3050_platform_data),
+		 offsetof(struct mldl_cfg, ram));
+}
+
+int mpu_set_slave(struct mldl_cfg *mldl_cfg,
+		void *gyro_handle,
+		struct ext_slave_descr *slave,
+		struct ext_slave_platform_data *slave_pdata)
+{
+	int result;
+	unsigned char reg;
+	unsigned char slave_reg;
+	unsigned char slave_len;
+	unsigned char slave_endian;
+	unsigned char slave_address;
+
+	result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE);
+
+	if (NULL == slave || NULL == slave_pdata) {
+		slave_reg = 0;
+		slave_len = 0;
+		slave_endian = 0;
+		slave_address = 0;
+	} else {
+		slave_reg = slave->reg;
+		slave_len = slave->len;
+		slave_endian = slave->endian;
+		slave_address = slave_pdata->address;
+	}
+
+	/* Address */
+	result = MLSLSerialWriteSingle(gyro_handle,
+				mldl_cfg->addr,
+				MPUREG_AUX_SLV_ADDR,
+				slave_address);
+	ERROR_CHECK(result);
+	/* Register */
+	result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
+				MPUREG_ACCEL_BURST_ADDR, 1,
+				&reg);
+	ERROR_CHECK(result);
+	reg = ((reg & 0x80) | slave_reg);
+	result = MLSLSerialWriteSingle(gyro_handle,
+				mldl_cfg->addr,
+				MPUREG_ACCEL_BURST_ADDR,
+				reg);
+	ERROR_CHECK(result);
+
+#ifdef M_HW
+	/* Length, byte swapping, grouping & enable */
+	if (slave_len > BITS_SLV_LENG) {
+		MPL_LOGW("Limiting slave burst read length to "
+			"the allowed maximum (15B, req. %d)\n",
+			slave_len);
+		slave_len = BITS_SLV_LENG;
+	}
+	reg = slave_len;
+	if (slave_endian == EXT_SLAVE_LITTLE_ENDIAN)
+		reg |= BIT_SLV_BYTE_SW;
+	reg |= BIT_SLV_GRP;
+	reg |= BIT_SLV_ENABLE;
+
+	result = MLSLSerialWriteSingle(gyro_handle,
+				mldl_cfg->addr,
+				MPUREG_I2C_SLV0_CTRL,
+				reg);
+#else
+	/* Length */
+	result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
+				MPUREG_USER_CTRL, 1, &reg);
+	ERROR_CHECK(result);
+	reg = (reg & ~BIT_AUX_RD_LENG);
+	result = MLSLSerialWriteSingle(gyro_handle,
+				mldl_cfg->addr,
+				MPUREG_USER_CTRL, reg);
+	ERROR_CHECK(result);
+#endif
+
+	if (slave_address) {
+		result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, FALSE);
+		ERROR_CHECK(result);
+	}
+	return result;
+}
+
+/**
+ * Check to see if the gyro was reset by testing a couple of registers known
+ * to change on reset.
+ *
+ * @param mldl_cfg mldl configuration structure
+ * @param gyro_handle handle used to communicate with the gyro
+ *
+ * @return ML_SUCCESS or non-zero error code
+ */
+static int mpu_was_reset(struct mldl_cfg *mldl_cfg, void *gyro_handle)
+{
+	int result = ML_SUCCESS;
+	unsigned char reg;
+
+	result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
+				MPUREG_DMP_CFG_2, 1, &reg);
+	ERROR_CHECK(result);
+
+	if (mldl_cfg->dmp_cfg2 != reg)
+		return TRUE;
+
+	if (0 != mldl_cfg->dmp_cfg1)
+		return FALSE;
+
+	result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
+				MPUREG_SMPLRT_DIV, 1, &reg);
+	ERROR_CHECK(result);
+	if (reg != mldl_cfg->divider)
+		return TRUE;
+
+	if (0 != mldl_cfg->divider)
+		return FALSE;
+
+	/* Inconclusive assume it was reset */
+	return TRUE;
+}
+
+static int gyro_resume(struct mldl_cfg *mldl_cfg, void *gyro_handle)
+{
+	int result;
+	int ii;
+	int jj;
+	unsigned char reg;
+	unsigned char regs[7];
+
+	/* Wake up the part */
+#ifdef M_HW
+	result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, RESET,
+				WAKE_UP);
+	ERROR_CHECK(result);
+
+	/* Configure the MPU */
+	result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, 1);
+	ERROR_CHECK(result);
+	/* setting int_config with the propert flag BIT_BYPASS_EN
+	   should be done by the setup functions */
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_INT_PIN_CFG,
+				(mldl_cfg->pdata->int_config |
+					BIT_BYPASS_EN));
+	ERROR_CHECK(result);
+	/* temporary: masking out higher bits to avoid switching
+	   intelligence */
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_INT_ENABLE,
+				(mldl_cfg->int_config));
+	ERROR_CHECK(result);
+#else
+	result = MLDLPowerMgmtMPU(mldl_cfg, gyro_handle, 0, 0,
+				mldl_cfg->gyro_power & BIT_STBY_XG,
+				mldl_cfg->gyro_power & BIT_STBY_YG,
+				mldl_cfg->gyro_power & BIT_STBY_ZG);
+
+	if (!mldl_cfg->gyro_needs_reset &&
+	    !mpu_was_reset(mldl_cfg, gyro_handle)) {
+		return ML_SUCCESS;
+	}
+
+	result = MLDLPowerMgmtMPU(mldl_cfg, gyro_handle, 1, 0,
+				mldl_cfg->gyro_power & BIT_STBY_XG,
+				mldl_cfg->gyro_power & BIT_STBY_YG,
+				mldl_cfg->gyro_power & BIT_STBY_ZG);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_INT_CFG,
+				(mldl_cfg->int_config |
+					mldl_cfg->pdata->int_config));
+	ERROR_CHECK(result);
+#endif
+
+	result = MLSLSerialRead(gyro_handle, mldl_cfg->addr,
+				MPUREG_PWR_MGM, 1, &reg);
+	ERROR_CHECK(result);
+	reg &= ~BITS_CLKSEL;
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_PWR_MGM,
+				mldl_cfg->clk_src | reg);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_SMPLRT_DIV,
+				mldl_cfg->divider);
+	ERROR_CHECK(result);
+
+#ifdef M_HW
+	reg = DLPF_FS_SYNC_VALUE(0, mldl_cfg->full_scale, 0);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_GYRO_CONFIG, reg);
+	reg = DLPF_FS_SYNC_VALUE(mldl_cfg->ext_sync, 0, mldl_cfg->lpf);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_CONFIG, reg);
+#else
+	reg = DLPF_FS_SYNC_VALUE(mldl_cfg->ext_sync,
+				mldl_cfg->full_scale, mldl_cfg->lpf);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_DLPF_FS_SYNC, reg);
+#endif
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_DMP_CFG_1,
+				mldl_cfg->dmp_cfg1);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_DMP_CFG_2,
+				mldl_cfg->dmp_cfg2);
+	ERROR_CHECK(result);
+
+	/* Write and verify memory */
+	for (ii = 0; ii < MPU_MEM_NUM_RAM_BANKS; ii++) {
+		unsigned char read[MPU_MEM_BANK_SIZE];
+
+		result = MLSLSerialWriteMem(gyro_handle,
+					mldl_cfg->addr,
+					((ii << 8) | 0x00),
+					MPU_MEM_BANK_SIZE,
+					mldl_cfg->ram[ii]);
+		ERROR_CHECK(result);
+		result = MLSLSerialReadMem(gyro_handle, mldl_cfg->addr,
+					((ii << 8) | 0x00),
+					MPU_MEM_BANK_SIZE, read);
+		ERROR_CHECK(result);
+
+#ifdef M_HW
+#define ML_SKIP_CHECK 38
+#else
+#define ML_SKIP_CHECK 20
+#endif
+		for (jj = 0; jj < MPU_MEM_BANK_SIZE; jj++) {
+			/* skip the register memory locations */
+			if (ii == 0 && jj < ML_SKIP_CHECK)
+				continue;
+			if (mldl_cfg->ram[ii][jj] != read[jj]) {
+				result = ML_ERROR_SERIAL_WRITE;
+				break;
+			}
+		}
+		ERROR_CHECK(result);
+	}
+
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_XG_OFFS_TC,
+				mldl_cfg->offset_tc[0]);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_YG_OFFS_TC,
+				mldl_cfg->offset_tc[1]);
+	ERROR_CHECK(result);
+	result = MLSLSerialWriteSingle(gyro_handle, mldl_cfg->addr,
+				MPUREG_ZG_OFFS_TC,
+				mldl_cfg->offset_tc[2]);
+	ERROR_CHECK(result);
+
+	regs[0] = MPUREG_X_OFFS_USRH;
+	for (ii = 0; ii < DIM(mldl_cfg->offset); ii++) {
+		regs[1 + ii * 2] =
+			(unsigned char)(mldl_cfg->offset[ii] >> 8)
+			& 0xff;
+		regs[1 + ii * 2 + 1] =
+			(unsigned char)(mldl_cfg->offset[ii] & 0xff);
+	}
+	result = MLSLSerialWrite(gyro_handle, mldl_cfg->addr, 7, regs);
+	ERROR_CHECK(result);
+
+	/* Configure slaves */
+	result = MLDLSetLevelShifterBit(mldl_cfg, gyro_handle,
+					mldl_cfg->pdata->level_shifter);
+	ERROR_CHECK(result);
+	return result;
+}
+/*******************************************************************************
+ *******************************************************************************
+ * Exported functions
+ *******************************************************************************
+ ******************************************************************************/
+
+/**
+ * Initializes the pdata structure to defaults.
+ *
+ * Opens the device to read silicon revision, product id and whoami.
+ *
+ * @param mldl_cfg
+ *          The internal device configuration data structure.
+ * @param mlsl_handle
+ *          The serial communication handle.
+ *
+ * @return ML_SUCCESS if silicon revision, product id and woami are supported
+ *         by this software.
+ */
+int mpu3050_open(struct mldl_cfg *mldl_cfg,
+		 void *mlsl_handle,
+		 void *accel_handle,
+		 void *compass_handle,
+		 void *pressure_handle)
+{
+	int result;
+	/* Default is Logic HIGH, pushpull, latch disabled, anyread to clear */
+	mldl_cfg->ignore_system_suspend = FALSE;
+	mldl_cfg->int_config = BIT_INT_ANYRD_2CLEAR | BIT_DMP_INT_EN;
+	mldl_cfg->clk_src = MPU_CLK_SEL_PLLGYROZ;
+	mldl_cfg->lpf = MPU_FILTER_42HZ;
+	mldl_cfg->full_scale = MPU_FS_2000DPS;
+	mldl_cfg->divider = 4;
+	mldl_cfg->dmp_enable = 1;
+	mldl_cfg->fifo_enable = 1;
+	mldl_cfg->ext_sync = 0;
+	mldl_cfg->dmp_cfg1 = 0;
+	mldl_cfg->dmp_cfg2 = 0;
+	mldl_cfg->gyro_power = 0;
+	mldl_cfg->gyro_is_bypassed = TRUE;
+	mldl_cfg->dmp_is_running = FALSE;
+	mldl_cfg->gyro_is_suspended = TRUE;
+	mldl_cfg->accel_is_suspended = TRUE;
+	mldl_cfg->compass_is_suspended = TRUE;
+	mldl_cfg->pressure_is_suspended = TRUE;
+	mldl_cfg->gyro_needs_reset = FALSE;
+	if (mldl_cfg->addr == 0) {
+#ifdef __KERNEL__
+		return ML_ERROR_INVALID_PARAMETER;
+#else
+		mldl_cfg->addr = 0x68;
+#endif
+	}
+
+	/*
+	 * Reset,
+	 * Take the DMP out of sleep, and
+	 * read the product_id, sillicon rev and whoami
+	 */
+#ifdef M_HW
+	result = mpu60xx_pwr_mgmt(mldl_cfg, mlsl_handle,
+				  RESET, WAKE_UP);
+#else
+	result = MLDLPowerMgmtMPU(mldl_cfg, mlsl_handle, RESET, 0, 0, 0, 0);
+#endif
+	ERROR_CHECK(result);
+
+	result = MLDLGetSiliconRev(mldl_cfg, mlsl_handle);
+	ERROR_CHECK(result);
+#ifndef M_HW
+	result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
+				MPUREG_PRODUCT_ID, 1,
+				&mldl_cfg->product_id);
+	ERROR_CHECK(result);
+#endif
+
+	/* Get the factory temperature compensation offsets */
+	result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
+				MPUREG_XG_OFFS_TC, 1,
+				&mldl_cfg->offset_tc[0]);
+	ERROR_CHECK(result);
+	result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
+				MPUREG_YG_OFFS_TC, 1,
+				&mldl_cfg->offset_tc[1]);
+	ERROR_CHECK(result);
+	result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
+				MPUREG_ZG_OFFS_TC, 1,
+				&mldl_cfg->offset_tc[2]);
+	ERROR_CHECK(result);
+
+	/* Configure the MPU */
+#ifdef M_HW
+	result = mpu60xx_pwr_mgmt(mldl_cfg, mlsl_handle,
+				  FALSE, SLEEP);
+#else
+	result =
+	    MLDLPowerMgmtMPU(mldl_cfg, mlsl_handle, 0, SLEEP, 0, 0, 0);
+#endif
+	ERROR_CHECK(result);
+
+	if (mldl_cfg->accel && mldl_cfg->accel->init) {
+		result = mldl_cfg->accel->init(accel_handle,
+					       mldl_cfg->accel,
+					       &mldl_cfg->pdata->accel);
+		ERROR_CHECK(result);
+	}
+
+	if (mldl_cfg->compass && mldl_cfg->compass->init) {
+		result = mldl_cfg->compass->init(compass_handle,
+						 mldl_cfg->compass,
+						 &mldl_cfg->pdata->compass);
+		if (ML_SUCCESS != result) {
+			MPL_LOGE("mldl_cfg->compass->init returned %d\n",
+				result);
+			goto out_accel;
+		}
+	}
+	if (mldl_cfg->pressure && mldl_cfg->pressure->init) {
+		result = mldl_cfg->pressure->init(pressure_handle,
+						  mldl_cfg->pressure,
+						  &mldl_cfg->pdata->pressure);
+		if (ML_SUCCESS != result) {
+			MPL_LOGE("mldl_cfg->pressure->init returned %d\n",
+				result);
+			goto out_compass;
+		}
+	}
+
+	mldl_cfg->requested_sensors = ML_THREE_AXIS_GYRO;
+	if (mldl_cfg->accel && mldl_cfg->accel->resume)
+		mldl_cfg->requested_sensors |= ML_THREE_AXIS_ACCEL;
+
+	if (mldl_cfg->compass && mldl_cfg->compass->resume)
+		mldl_cfg->requested_sensors |= ML_THREE_AXIS_COMPASS;
+
+	if (mldl_cfg->pressure && mldl_cfg->pressure->resume)
+		mldl_cfg->requested_sensors |= ML_THREE_AXIS_PRESSURE;
+
+	return result;
+
+out_compass:
+	if (mldl_cfg->compass->init)
+		mldl_cfg->compass->exit(compass_handle,
+				mldl_cfg->compass,
+				&mldl_cfg->pdata->compass);
+out_accel:
+	if (mldl_cfg->accel->init)
+		mldl_cfg->accel->exit(accel_handle,
+				mldl_cfg->accel,
+				&mldl_cfg->pdata->accel);
+	return result;
+
+}
+
+/**
+ * Close the mpu3050 interface
+ *
+ * @param mldl_cfg pointer to the configuration structure
+ * @param mlsl_handle pointer to the serial layer handle
+ *
+ * @return ML_SUCCESS or non-zero error code
+ */
+int mpu3050_close(struct mldl_cfg *mldl_cfg,
+		  void *mlsl_handle,
+		  void *accel_handle,
+		  void *compass_handle,
+		  void *pressure_handle)
+{
+	int result = ML_SUCCESS;
+	int ret_result = ML_SUCCESS;
+
+	if (mldl_cfg->accel && mldl_cfg->accel->exit) {
+		result = mldl_cfg->accel->exit(accel_handle,
+					mldl_cfg->accel,
+					&mldl_cfg->pdata->accel);
+		if (ML_SUCCESS != result)
+			MPL_LOGE("Accel exit failed %d\n", result);
+		ret_result = result;
+	}
+	if (ML_SUCCESS == ret_result)
+		ret_result = result;
+
+	if (mldl_cfg->compass && mldl_cfg->compass->exit) {
+		result = mldl_cfg->compass->exit(compass_handle,
+						mldl_cfg->compass,
+						&mldl_cfg->pdata->compass);
+		if (ML_SUCCESS != result)
+			MPL_LOGE("Compass exit failed %d\n", result);
+	}
+	if (ML_SUCCESS == ret_result)
+		ret_result = result;
+
+	if (mldl_cfg->pressure && mldl_cfg->pressure->exit) {
+		result = mldl_cfg->pressure->exit(pressure_handle,
+						mldl_cfg->pressure,
+						&mldl_cfg->pdata->pressure);
+		if (ML_SUCCESS != result)
+			MPL_LOGE("Pressure exit failed %d\n", result);
+	}
+	if (ML_SUCCESS == ret_result)
+		ret_result = result;
+
+	return ret_result;
+}
+
+/**
+ *  @brief  resume the MPU3050 device and all the other sensor
+ *          devices from their low power state.
+ *
+ *  @param  mldl_cfg
+ *              pointer to the configuration structure
+ *  @param  gyro_handle
+ *              the main file handle to the MPU3050 device.
+ *  @param  accel_handle
+ *              an handle to the accelerometer device, if sitting
+ *              onto a separate bus. Can match mlsl_handle if
+ *              the accelerometer device operates on the same
+ *              primary bus of MPU.
+ *  @param  compass_handle
+ *              an handle to the compass device, if sitting
+ *              onto a separate bus. Can match mlsl_handle if
+ *              the compass device operates on the same
+ *              primary bus of MPU.
+ *  @param  pressure_handle
+ *              an handle to the pressure sensor device, if sitting
+ *              onto a separate bus. Can match mlsl_handle if
+ *              the pressure sensor device operates on the same
+ *              primary bus of MPU.
+ *  @param  resume_gyro
+ *              whether resuming the gyroscope device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @param  resume_accel
+ *              whether resuming the accelerometer device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @param  resume_compass
+ *              whether resuming the compass device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @param  resume_pressure
+ *              whether resuming the pressure sensor device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @return  ML_SUCCESS or a non-zero error code.
+ */
+int mpu3050_resume(struct mldl_cfg *mldl_cfg,
+		   void *gyro_handle,
+		   void *accel_handle,
+		   void *compass_handle,
+		   void *pressure_handle,
+		   bool resume_gyro,
+		   bool resume_accel,
+		   bool resume_compass,
+		   bool resume_pressure)
+{
+	int result = ML_SUCCESS;
+
+#ifdef CONFIG_MPU_SENSORS_DEBUG
+	mpu_print_cfg(mldl_cfg);
+#endif
+
+	if (resume_accel &&
+	    ((!mldl_cfg->accel) || (!mldl_cfg->accel->resume)))
+		return ML_ERROR_INVALID_PARAMETER;
+	if (resume_compass &&
+	    ((!mldl_cfg->compass) || (!mldl_cfg->compass->resume)))
+		return ML_ERROR_INVALID_PARAMETER;
+	if (resume_pressure &&
+	    ((!mldl_cfg->pressure) || (!mldl_cfg->pressure->resume)))
+		return ML_ERROR_INVALID_PARAMETER;
+
+	if (resume_gyro && mldl_cfg->gyro_is_suspended) {
+		result = gyro_resume(mldl_cfg, gyro_handle);
+		ERROR_CHECK(result);
+	}
+
+	if (resume_accel && mldl_cfg->accel_is_suspended) {
+		if (!mldl_cfg->gyro_is_suspended &&
+		    EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus) {
+			result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE);
+			ERROR_CHECK(result);
+		}
+		result = mldl_cfg->accel->resume(accel_handle,
+						 mldl_cfg->accel,
+						 &mldl_cfg->pdata->accel);
+		ERROR_CHECK(result);
+		mldl_cfg->accel_is_suspended = FALSE;
+	}
+
+	if (!mldl_cfg->gyro_is_suspended && !mldl_cfg->accel_is_suspended &&
+		EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus) {
+		result = mpu_set_slave(mldl_cfg,
+				gyro_handle,
+				mldl_cfg->accel,
+				&mldl_cfg->pdata->accel);
+		ERROR_CHECK(result);
+	}
+
+	if (resume_compass && mldl_cfg->compass_is_suspended) {
+		if (!mldl_cfg->gyro_is_suspended &&
+		    EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus) {
+			result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE);
+			ERROR_CHECK(result);
+		}
+		result = mldl_cfg->compass->resume(compass_handle,
+						   mldl_cfg->compass,
+						   &mldl_cfg->pdata->
+						   compass);
+		ERROR_CHECK(result);
+		mldl_cfg->compass_is_suspended = FALSE;
+	}
+
+	if (!mldl_cfg->gyro_is_suspended && !mldl_cfg->compass_is_suspended &&
+		EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus) {
+		result = mpu_set_slave(mldl_cfg,
+				gyro_handle,
+				mldl_cfg->compass,
+				&mldl_cfg->pdata->compass);
+		ERROR_CHECK(result);
+	}
+
+	if (resume_pressure && mldl_cfg->pressure_is_suspended) {
+		if (!mldl_cfg->gyro_is_suspended &&
+		    EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus) {
+			result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, TRUE);
+			ERROR_CHECK(result);
+		}
+		result = mldl_cfg->pressure->resume(pressure_handle,
+						    mldl_cfg->pressure,
+						    &mldl_cfg->pdata->
+						    pressure);
+		ERROR_CHECK(result);
+		mldl_cfg->pressure_is_suspended = FALSE;
+	}
+
+	if (!mldl_cfg->gyro_is_suspended && !mldl_cfg->pressure_is_suspended &&
+		EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus) {
+		result = mpu_set_slave(mldl_cfg,
+				gyro_handle,
+				mldl_cfg->pressure,
+				&mldl_cfg->pdata->pressure);
+		ERROR_CHECK(result);
+	}
+
+	/* Now start */
+	if (resume_gyro) {
+		result = dmp_start(mldl_cfg, gyro_handle);
+		ERROR_CHECK(result);
+	}
+
+	return result;
+}
+
+/**
+ *  @brief  suspend the MPU3050 device and all the other sensor
+ *          devices into their low power state.
+ *  @param  gyro_handle
+ *              the main file handle to the MPU3050 device.
+ *  @param  accel_handle
+ *              an handle to the accelerometer device, if sitting
+ *              onto a separate bus. Can match gyro_handle if
+ *              the accelerometer device operates on the same
+ *              primary bus of MPU.
+ *  @param  compass_handle
+ *              an handle to the compass device, if sitting
+ *              onto a separate bus. Can match gyro_handle if
+ *              the compass device operates on the same
+ *              primary bus of MPU.
+ *  @param  pressure_handle
+ *              an handle to the pressure sensor device, if sitting
+ *              onto a separate bus. Can match gyro_handle if
+ *              the pressure sensor device operates on the same
+ *              primary bus of MPU.
+ *  @param  accel
+ *              whether suspending the accelerometer device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @param  compass
+ *              whether suspending the compass device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @param  pressure
+ *              whether suspending the pressure sensor device is
+ *              actually needed (if the device supports low power
+ *              mode of some sort).
+ *  @return  ML_SUCCESS or a non-zero error code.
+ */
+int mpu3050_suspend(struct mldl_cfg *mldl_cfg,
+		    void *gyro_handle,
+		    void *accel_handle,
+		    void *compass_handle,
+		    void *pressure_handle,
+		    bool suspend_gyro,
+		    bool suspend_accel,
+		    bool suspend_compass,
+		    bool suspend_pressure)
+{
+	int result = ML_SUCCESS;
+
+	if (suspend_gyro && !mldl_cfg->gyro_is_suspended) {
+#ifdef M_HW
+		return ML_SUCCESS;
+		/* This puts the bus into bypass mode */
+		result = MLDLSetI2CBypass(mldl_cfg, gyro_handle, 1);
+		ERROR_CHECK(result);
+		result = mpu60xx_pwr_mgmt(mldl_cfg, gyro_handle, 0, SLEEP);
+#else
+		result = MLDLPowerMgmtMPU(mldl_cfg, gyro_handle,
+					0, SLEEP, 0, 0, 0);
+#endif
+		ERROR_CHECK(result);
+	}
+
+	if (!mldl_cfg->accel_is_suspended && suspend_accel &&
+	    mldl_cfg->accel && mldl_cfg->accel->suspend) {
+		if (!mldl_cfg->gyro_is_suspended &&
+		    EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus) {
+			result = mpu_set_slave(mldl_cfg, gyro_handle,
+					       NULL, NULL);
+			ERROR_CHECK(result);
+		}
+		result = mldl_cfg->accel->suspend(accel_handle,
+						  mldl_cfg->accel,
+						  &mldl_cfg->pdata->accel);
+		ERROR_CHECK(result);
+		mldl_cfg->accel_is_suspended = TRUE;
+	}
+
+	if (!mldl_cfg->compass_is_suspended && suspend_compass &&
+	    mldl_cfg->compass && mldl_cfg->compass->suspend) {
+		if (!mldl_cfg->gyro_is_suspended &&
+		    EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus) {
+			result = mpu_set_slave(mldl_cfg, gyro_handle,
+					       NULL, NULL);
+			ERROR_CHECK(result);
+		}
+		result = mldl_cfg->compass->suspend(compass_handle,
+						    mldl_cfg->compass,
+						    &mldl_cfg->
+						    pdata->compass);
+		ERROR_CHECK(result);
+		mldl_cfg->compass_is_suspended = TRUE;
+	}
+
+	if (!mldl_cfg->pressure_is_suspended && suspend_pressure &&
+	    mldl_cfg->pressure && mldl_cfg->pressure->suspend) {
+		if (!mldl_cfg->gyro_is_suspended &&
+		    EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus) {
+			result = mpu_set_slave(mldl_cfg, gyro_handle,
+					       NULL, NULL);
+			ERROR_CHECK(result);
+		}
+		result = mldl_cfg->pressure->suspend(pressure_handle,
+						    mldl_cfg->pressure,
+						    &mldl_cfg->
+						    pdata->pressure);
+		ERROR_CHECK(result);
+		mldl_cfg->pressure_is_suspended = TRUE;
+	}
+	return result;
+}
+
+
+/**
+ *  @brief  read raw sensor data from the accelerometer device
+ *          in use.
+ *  @param  mldl_cfg
+ *              A pointer to the struct mldl_cfg data structure.
+ *  @param  accel_handle
+ *              The handle to the device the accelerometer is connected to.
+ *  @param  data
+ *              a buffer to store the raw sensor data.
+ *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int mpu3050_read_accel(struct mldl_cfg *mldl_cfg,
+		       void *accel_handle, unsigned char *data)
+{
+	if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->read)
+		if ((EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->accel.bus)
+			&& (!mldl_cfg->gyro_is_bypassed))
+			return ML_ERROR_FEATURE_NOT_ENABLED;
+		else
+			return mldl_cfg->accel->read(accel_handle,
+						     mldl_cfg->accel,
+						     &mldl_cfg->pdata->accel,
+						     data);
+	else
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+/**
+ *  @brief  read raw sensor data from the compass device
+ *          in use.
+ *  @param  mldl_cfg
+ *              A pointer to the struct mldl_cfg data structure.
+ *  @param  compass_handle
+ *              The handle to the device the compass is connected to.
+ *  @param  data
+ *              a buffer to store the raw sensor data.
+ *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int mpu3050_read_compass(struct mldl_cfg *mldl_cfg,
+			 void *compass_handle, unsigned char *data)
+{
+	if (NULL != mldl_cfg->compass && NULL != mldl_cfg->compass->read)
+		if ((EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->compass.bus)
+			&& (!mldl_cfg->gyro_is_bypassed))
+			return ML_ERROR_FEATURE_NOT_ENABLED;
+		else
+			return mldl_cfg->compass->read(compass_handle,
+						mldl_cfg->compass,
+						&mldl_cfg->pdata->compass,
+						data);
+	else
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+/**
+ *  @brief  read raw sensor data from the pressure device
+ *          in use.
+ *  @param  mldl_cfg
+ *              A pointer to the struct mldl_cfg data structure.
+ *  @param  pressure_handle
+ *              The handle to the device the pressure sensor is connected to.
+ *  @param  data
+ *              a buffer to store the raw sensor data.
+ *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+int mpu3050_read_pressure(struct mldl_cfg *mldl_cfg,
+			 void *pressure_handle, unsigned char *data)
+{
+	if (NULL != mldl_cfg->pressure && NULL != mldl_cfg->pressure->read)
+		if ((EXT_SLAVE_BUS_SECONDARY == mldl_cfg->pdata->pressure.bus)
+			&& (!mldl_cfg->gyro_is_bypassed))
+			return ML_ERROR_FEATURE_NOT_ENABLED;
+		else
+			return mldl_cfg->pressure->read(
+				pressure_handle,
+				mldl_cfg->pressure,
+				&mldl_cfg->pdata->pressure,
+				data);
+	else
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+int mpu3050_config_accel(struct mldl_cfg *mldl_cfg,
+			void *accel_handle,
+			struct ext_slave_config *data)
+{
+	if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->config)
+		return mldl_cfg->accel->config(accel_handle,
+					       mldl_cfg->accel,
+					       &mldl_cfg->pdata->accel,
+					       data);
+	else
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+
+}
+
+int mpu3050_config_compass(struct mldl_cfg *mldl_cfg,
+			void *compass_handle,
+			struct ext_slave_config *data)
+{
+	if (NULL != mldl_cfg->compass && NULL != mldl_cfg->compass->config)
+		return mldl_cfg->compass->config(compass_handle,
+						 mldl_cfg->compass,
+						 &mldl_cfg->pdata->compass,
+						 data);
+	else
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+
+}
+
+int mpu3050_config_pressure(struct mldl_cfg *mldl_cfg,
+			void *pressure_handle,
+			struct ext_slave_config *data)
+{
+	if (NULL != mldl_cfg->pressure && NULL != mldl_cfg->pressure->config)
+		return mldl_cfg->pressure->config(pressure_handle,
+						  mldl_cfg->pressure,
+						  &mldl_cfg->pdata->pressure,
+						  data);
+	else
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+int mpu3050_get_config_accel(struct mldl_cfg *mldl_cfg,
+			void *accel_handle,
+			struct ext_slave_config *data)
+{
+	if (NULL != mldl_cfg->accel && NULL != mldl_cfg->accel->get_config)
+		return mldl_cfg->accel->get_config(accel_handle,
+						mldl_cfg->accel,
+						&mldl_cfg->pdata->accel,
+						data);
+	else
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+
+}
+
+int mpu3050_get_config_compass(struct mldl_cfg *mldl_cfg,
+			void *compass_handle,
+			struct ext_slave_config *data)
+{
+	if (NULL != mldl_cfg->compass && NULL != mldl_cfg->compass->get_config)
+		return mldl_cfg->compass->get_config(compass_handle,
+						mldl_cfg->compass,
+						&mldl_cfg->pdata->compass,
+						data);
+	else
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+
+}
+
+int mpu3050_get_config_pressure(struct mldl_cfg *mldl_cfg,
+				void *pressure_handle,
+				struct ext_slave_config *data)
+{
+	if (NULL != mldl_cfg->pressure &&
+	    NULL != mldl_cfg->pressure->get_config)
+		return mldl_cfg->pressure->get_config(pressure_handle,
+						mldl_cfg->pressure,
+						&mldl_cfg->pdata->pressure,
+						data);
+	else
+		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+
+/**
+ *@}
+ */
diff --git a/drivers/misc/mpu3050/mldl_cfg.h b/drivers/misc/mpu3050/mldl_cfg.h
new file mode 100644
index 000000000000..ad6a211c5d86
--- /dev/null
+++ b/drivers/misc/mpu3050/mldl_cfg.h
@@ -0,0 +1,199 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+
+/**
+ *  @addtogroup MLDL
+ *
+ *  @{
+ *      @file   mldl_cfg.h
+ *      @brief  The Motion Library Driver Layer Configuration header file.
+ */
+
+#ifndef __MLDL_CFG_H__
+#define __MLDL_CFG_H__
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#include "mlsl.h"
+#include "mpu.h"
+
+/* --------------------- */
+/* -    Defines.       - */
+/* --------------------- */
+
+    /*************************************************************************/
+    /*  Sensors                                                              */
+    /*************************************************************************/
+
+#define ML_X_GYRO			(0x0001)
+#define ML_Y_GYRO			(0x0002)
+#define ML_Z_GYRO			(0x0004)
+#define ML_DMP_PROCESSOR		(0x0008)
+
+#define ML_X_ACCEL			(0x0010)
+#define ML_Y_ACCEL			(0x0020)
+#define ML_Z_ACCEL			(0x0040)
+
+#define ML_X_COMPASS			(0x0080)
+#define ML_Y_COMPASS			(0x0100)
+#define ML_Z_COMPASS			(0x0200)
+
+#define ML_X_PRESSURE			(0x0300)
+#define ML_Y_PRESSURE			(0x0800)
+#define ML_Z_PRESSURE			(0x1000)
+
+#define ML_TEMPERATURE			(0x2000)
+#define ML_TIME				(0x4000)
+
+#define ML_THREE_AXIS_GYRO		(0x000F)
+#define ML_THREE_AXIS_ACCEL		(0x0070)
+#define ML_THREE_AXIS_COMPASS		(0x0380)
+#define ML_THREE_AXIS_PRESSURE		(0x1C00)
+
+#define ML_FIVE_AXIS			(0x007B)
+#define ML_SIX_AXIS_GYRO_ACCEL		(0x007F)
+#define ML_SIX_AXIS_ACCEL_COMPASS	(0x03F0)
+#define ML_NINE_AXIS			(0x03FF)
+#define ML_ALL_SENSORS			(0x7FFF)
+
+#define SAMPLING_RATE_HZ(mldl_cfg)					\
+	((((((mldl_cfg)->lpf) == 0) || (((mldl_cfg)->lpf) == 7))	\
+		? (8000)						\
+		: (1000))						\
+		/ ((mldl_cfg)->divider + 1))
+
+#define SAMPLING_PERIOD_US(mldl_cfg)					\
+	((1000000L * ((mldl_cfg)->divider + 1)) /			\
+	(((((mldl_cfg)->lpf) == 0) || (((mldl_cfg)->lpf) == 7))		\
+		? (8000)						\
+		: (1000)))
+/* --------------------- */
+/* -    Variables.     - */
+/* --------------------- */
+
+/* Platform data for the MPU */
+struct mldl_cfg {
+	/* MPU related configuration */
+	unsigned long requested_sensors;
+	unsigned char ignore_system_suspend;
+	unsigned char addr;
+	unsigned char int_config;
+	unsigned char ext_sync;
+	unsigned char full_scale;
+	unsigned char lpf;
+	unsigned char clk_src;
+	unsigned char divider;
+	unsigned char dmp_enable;
+	unsigned char fifo_enable;
+	unsigned char dmp_cfg1;
+	unsigned char dmp_cfg2;
+	unsigned char gyro_power;
+	unsigned char offset_tc[MPU_NUM_AXES];
+	unsigned short offset[MPU_NUM_AXES];
+	unsigned char ram[MPU_MEM_NUM_RAM_BANKS][MPU_MEM_BANK_SIZE];
+
+	/* MPU Related stored status and info */
+	unsigned char silicon_revision;
+	unsigned char product_id;
+	unsigned short trim;
+
+	/* Driver/Kernel related state information */
+	int gyro_is_bypassed;
+	int dmp_is_running;
+	int gyro_is_suspended;
+	int accel_is_suspended;
+	int compass_is_suspended;
+	int pressure_is_suspended;
+	int gyro_needs_reset;
+
+	/* Slave related information */
+	struct ext_slave_descr *accel;
+	struct ext_slave_descr *compass;
+	struct ext_slave_descr *pressure;
+
+	/* Platform Data */
+	struct mpu3050_platform_data *pdata;
+};
+
+
+int mpu3050_open(struct mldl_cfg *mldl_cfg,
+		 void *mlsl_handle,
+		 void *accel_handle,
+		 void *compass_handle,
+		 void *pressure_handle);
+int mpu3050_close(struct mldl_cfg *mldl_cfg,
+		  void *mlsl_handle,
+		  void *accel_handle,
+		  void *compass_handle,
+		  void *pressure_handle);
+int mpu3050_resume(struct mldl_cfg *mldl_cfg,
+		   void *gyro_handle,
+		   void *accel_handle,
+		   void *compass_handle,
+		   void *pressure_handle,
+		   bool resume_gyro,
+		   bool resume_accel,
+		   bool resume_compass,
+		   bool resume_pressure);
+int mpu3050_suspend(struct mldl_cfg *mldl_cfg,
+		    void *gyro_handle,
+		    void *accel_handle,
+		    void *compass_handle,
+		    void *pressure_handle,
+		    bool suspend_gyro,
+		    bool suspend_accel,
+		    bool suspend_compass,
+		    bool suspend_pressure);
+int mpu3050_read_accel(struct mldl_cfg *mldl_cfg,
+		       void *accel_handle,
+		       unsigned char *data);
+int mpu3050_read_compass(struct mldl_cfg *mldl_cfg,
+			 void *compass_handle,
+			 unsigned char *data);
+int mpu3050_read_pressure(struct mldl_cfg *mldl_cfg, void *mlsl_handle,
+			  unsigned char *data);
+
+int mpu3050_config_accel(struct mldl_cfg *mldl_cfg,
+			 void *accel_handle,
+			 struct ext_slave_config *data);
+int mpu3050_config_compass(struct mldl_cfg *mldl_cfg,
+			   void *compass_handle,
+			   struct ext_slave_config *data);
+int mpu3050_config_pressure(struct mldl_cfg *mldl_cfg,
+			    void *pressure_handle,
+			    struct ext_slave_config *data);
+
+int mpu3050_get_config_accel(struct mldl_cfg *mldl_cfg,
+			     void *accel_handle,
+			     struct ext_slave_config *data);
+int mpu3050_get_config_compass(struct mldl_cfg *mldl_cfg,
+			       void *compass_handle,
+			       struct ext_slave_config *data);
+int mpu3050_get_config_pressure(struct mldl_cfg *mldl_cfg,
+				void *pressure_handle,
+				struct ext_slave_config *data);
+
+
+#endif				/* __MLDL_CFG_H__ */
+
+/**
+ *@}
+ */
diff --git a/drivers/misc/mpu3050/mlos-kernel.c b/drivers/misc/mpu3050/mlos-kernel.c
new file mode 100644
index 000000000000..ced9ba4f6f3c
--- /dev/null
+++ b/drivers/misc/mpu3050/mlos-kernel.c
@@ -0,0 +1,89 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+/**
+ * @defgroup
+ * @brief
+ *
+ * @{
+ * @file     mlos-kernel.c
+ * @brief
+ *
+ *
+ */
+
+#include "mlos.h"
+#include <linux/delay.h>
+#include <linux/slab.h>
+
+void *MLOSMalloc(unsigned int numBytes)
+{
+	return kmalloc(numBytes, GFP_KERNEL);
+}
+
+tMLError MLOSFree(void *ptr)
+{
+	kfree(ptr);
+	return ML_SUCCESS;
+}
+
+tMLError MLOSCreateMutex(HANDLE *mutex)
+{
+	/* @todo implement if needed */
+	return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+tMLError MLOSLockMutex(HANDLE mutex)
+{
+	/* @todo implement if needed */
+	return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+tMLError MLOSUnlockMutex(HANDLE mutex)
+{
+	/* @todo implement if needed */
+	return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+tMLError MLOSDestroyMutex(HANDLE handle)
+{
+	/* @todo implement if needed */
+	return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
+
+FILE *MLOSFOpen(char *filename)
+{
+	/* @todo implement if needed */
+	return NULL;
+}
+
+void MLOSFClose(FILE *fp)
+{
+	/* @todo implement if needed */
+}
+
+void MLOSSleep(int mSecs)
+{
+	msleep(mSecs);
+}
+
+unsigned long MLOSGetTickCount(void)
+{
+	/* @todo implement if needed */
+	return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
+}
diff --git a/drivers/misc/mpu3050/mlos.h b/drivers/misc/mpu3050/mlos.h
new file mode 100644
index 000000000000..4ebb86c9fa5c
--- /dev/null
+++ b/drivers/misc/mpu3050/mlos.h
@@ -0,0 +1,73 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+
+#ifndef _MLOS_H
+#define _MLOS_H
+
+#ifndef __KERNEL__
+#include <stdio.h>
+#endif
+
+#include "mltypes.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+	/* ------------ */
+	/* - Defines. - */
+	/* ------------ */
+
+	/* - MLOSCreateFile defines. - */
+
+#define MLOS_GENERIC_READ         ((unsigned int)0x80000000)
+#define MLOS_GENERIC_WRITE        ((unsigned int)0x40000000)
+#define MLOS_FILE_SHARE_READ      ((unsigned int)0x00000001)
+#define MLOS_FILE_SHARE_WRITE     ((unsigned int)0x00000002)
+#define MLOS_OPEN_EXISTING        ((unsigned int)0x00000003)
+
+	/* ---------- */
+	/* - Enums. - */
+	/* ---------- */
+
+	/* --------------- */
+	/* - Structures. - */
+	/* --------------- */
+
+	/* --------------------- */
+	/* - Function p-types. - */
+	/* --------------------- */
+
+	void *MLOSMalloc(unsigned int numBytes);
+	tMLError MLOSFree(void *ptr);
+	tMLError MLOSCreateMutex(HANDLE *mutex);
+	tMLError MLOSLockMutex(HANDLE mutex);
+	tMLError MLOSUnlockMutex(HANDLE mutex);
+	FILE *MLOSFOpen(char *filename);
+	void MLOSFClose(FILE *fp);
+
+	tMLError MLOSDestroyMutex(HANDLE handle);
+
+	void MLOSSleep(int mSecs);
+	unsigned long MLOSGetTickCount(void);
+
+#ifdef __cplusplus
+}
+#endif
+#endif				/* _MLOS_H */
diff --git a/drivers/misc/mpu3050/mlsl-kernel.c b/drivers/misc/mpu3050/mlsl-kernel.c
new file mode 100644
index 000000000000..cb1605131cbf
--- /dev/null
+++ b/drivers/misc/mpu3050/mlsl-kernel.c
@@ -0,0 +1,331 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+
+#include "mlsl.h"
+#include "mpu-i2c.h"
+
+/* ------------ */
+/* - Defines. - */
+/* ------------ */
+
+/* ---------------------- */
+/* - Types definitions. - */
+/* ---------------------- */
+
+/* --------------------- */
+/* - Function p-types. - */
+/* --------------------- */
+
+/**
+ *  @brief  used to open the I2C or SPI serial port.
+ *          This port is used to send and receive data to the MPU device.
+ *  @param  portNum
+ *              The COM port number associated with the device in use.
+ *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+tMLError MLSLSerialOpen(char const *port, void **sl_handle)
+{
+	return ML_SUCCESS;
+}
+
+/**
+ *  @brief  used to reset any buffering the driver may be doing
+ *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+tMLError MLSLSerialReset(void *sl_handle)
+{
+	return ML_SUCCESS;
+}
+
+/**
+ *  @brief  used to close the I2C or SPI serial port.
+ *          This port is used to send and receive data to the MPU device.
+ *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+tMLError MLSLSerialClose(void *sl_handle)
+{
+	return ML_SUCCESS;
+}
+
+/**
+ *  @brief  used to read a single byte of data.
+ *          This should be sent by I2C or SPI.
+ *
+ *  @param  slaveAddr       I2C slave address of device.
+ *  @param  registerAddr    Register address to read.
+ *  @param  data            Single byte of data to read.
+ *
+ *  @return ML_SUCCESS if the command is successful, an error code otherwise.
+ */
+tMLError MLSLSerialWriteSingle(void *sl_handle,
+			       unsigned char slaveAddr,
+			       unsigned char registerAddr,
+			       unsigned char data)
+{
+	return sensor_i2c_write_register((struct i2c_adapter *) sl_handle,
+					 slaveAddr, registerAddr, data);
+}
+
+
+/**
+ *  @brief  used to write multiple bytes of data from registers.
+ *          This should be sent by I2C.
+ *
+ *  @param  slaveAddr       I2C slave address of device.
+ *  @param  registerAddr    Register address to write.
+ *  @param  length          Length of burst of data.
+ *  @param  data            Pointer to block of data.
+ *
+ *  @return ML_SUCCESS if successful, a non-zero error code otherwise.
+ */
+tMLError MLSLSerialWrite(void *sl_handle,
+			 unsigned char slaveAddr,
+			 unsigned short length,
+			 unsigned char const *data)
+{
+	tMLError result;
+	const unsigned short dataLength = length - 1;
+	const unsigned char startRegAddr = data[0];
+	unsigned char i2cWrite[SERIAL_MAX_TRANSFER_SIZE + 1];
+	unsigned short bytesWritten = 0;
+
+	while (bytesWritten < dataLength) {
+		unsigned short thisLen = min(SERIAL_MAX_TRANSFER_SIZE,
+					     dataLength - bytesWritten);
+		if (bytesWritten == 0) {
+			result = sensor_i2c_write((struct i2c_adapter *)
+						  sl_handle, slaveAddr,
+						  1 + thisLen, data);
+		} else {
+			/* manually increment register addr between chunks */
+			i2cWrite[0] = startRegAddr + bytesWritten;
+			memcpy(&i2cWrite[1], &data[1 + bytesWritten],
+			       thisLen);
+			result = sensor_i2c_write((struct i2c_adapter *)
+						  sl_handle, slaveAddr,
+						  1 + thisLen, i2cWrite);
+		}
+		if (ML_SUCCESS != result)
+			return result;
+		bytesWritten += thisLen;
+	}
+	return ML_SUCCESS;
+}
+
+
+/**
+ *  @brief  used to read multiple bytes of data from registers.
+ *          This should be sent by I2C.
+ *
+ *  @param  slaveAddr       I2C slave address of device.
+ *  @param  registerAddr    Register address to read.
+ *  @param  length          Length of burst of data.
+ *  @param  data            Pointer to block of data.
+ *
+ *  @return Zero if successful; an error code otherwise
+ */
+tMLError MLSLSerialRead(void *sl_handle,
+			unsigned char slaveAddr,
+			unsigned char registerAddr,
+			unsigned short length, unsigned char *data)
+{
+	tMLError result;
+	unsigned short bytesRead = 0;
+
+	if (registerAddr == MPUREG_FIFO_R_W
+	    || registerAddr == MPUREG_MEM_R_W) {
+		return ML_ERROR_INVALID_PARAMETER;
+	}
+	while (bytesRead < length) {
+		unsigned short thisLen =
+		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytesRead);
+		result =
+		    sensor_i2c_read((struct i2c_adapter *) sl_handle,
+				    slaveAddr, registerAddr + bytesRead,
+				    thisLen, &data[bytesRead]);
+		if (ML_SUCCESS != result)
+			return result;
+		bytesRead += thisLen;
+	}
+	return ML_SUCCESS;
+}
+
+
+/**
+ *  @brief  used to write multiple bytes of data to the memory.
+ *          This should be sent by I2C.
+ *
+ *  @param  slaveAddr       I2C slave address of device.
+ *  @param  memAddr         The location in the memory to write to.
+ *  @param  length          Length of burst data.
+ *  @param  data            Pointer to block of data.
+ *
+ *  @return Zero if successful; an error code otherwise
+ */
+tMLError MLSLSerialWriteMem(void *sl_handle,
+			    unsigned char slaveAddr,
+			    unsigned short memAddr,
+			    unsigned short length,
+			    unsigned char const *data)
+{
+	tMLError result;
+	unsigned short bytesWritten = 0;
+
+	if ((memAddr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
+		pr_err("memory read length (%d B) extends beyond its"
+			" limits (%d) if started at location %d\n", length,
+			MPU_MEM_BANK_SIZE, memAddr & 0xFF);
+		return ML_ERROR_INVALID_PARAMETER;
+	}
+	while (bytesWritten < length) {
+		unsigned short thisLen =
+		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytesWritten);
+		result =
+		    mpu_memory_write((struct i2c_adapter *) sl_handle,
+				     slaveAddr, memAddr + bytesWritten,
+				     thisLen, &data[bytesWritten]);
+		if (ML_SUCCESS != result)
+			return result;
+		bytesWritten += thisLen;
+	}
+	return ML_SUCCESS;
+}
+
+
+/**
+ *  @brief  used to read multiple bytes of data from the memory.
+ *          This should be sent by I2C.
+ *
+ *  @param  slaveAddr       I2C slave address of device.
+ *  @param  memAddr         The location in the memory to read from.
+ *  @param  length          Length of burst data.
+ *  @param  data            Pointer to block of data.
+ *
+ *  @return Zero if successful; an error code otherwise
+ */
+tMLError MLSLSerialReadMem(void *sl_handle,
+			   unsigned char slaveAddr,
+			   unsigned short memAddr,
+			   unsigned short length, unsigned char *data)
+{
+	tMLError result;
+	unsigned short bytesRead = 0;
+
+	if ((memAddr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
+		printk
+		    ("memory read length (%d B) extends beyond its limits (%d) "
+		     "if started at location %d\n", length,
+		     MPU_MEM_BANK_SIZE, memAddr & 0xFF);
+		return ML_ERROR_INVALID_PARAMETER;
+	}
+	while (bytesRead < length) {
+		unsigned short thisLen =
+		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytesRead);
+		result =
+		    mpu_memory_read((struct i2c_adapter *) sl_handle,
+				    slaveAddr, memAddr + bytesRead,
+				    thisLen, &data[bytesRead]);
+		if (ML_SUCCESS != result)
+			return result;
+		bytesRead += thisLen;
+	}
+	return ML_SUCCESS;
+}
+
+
+/**
+ *  @brief  used to write multiple bytes of data to the fifo.
+ *          This should be sent by I2C.
+ *
+ *  @param  slaveAddr       I2C slave address of device.
+ *  @param  length          Length of burst of data.
+ *  @param  data            Pointer to block of data.
+ *
+ *  @return Zero if successful; an error code otherwise
+ */
+tMLError MLSLSerialWriteFifo(void *sl_handle,
+			     unsigned char slaveAddr,
+			     unsigned short length,
+			     unsigned char const *data)
+{
+	tMLError result;
+	unsigned char i2cWrite[SERIAL_MAX_TRANSFER_SIZE + 1];
+	unsigned short bytesWritten = 0;
+
+	if (length > FIFO_HW_SIZE) {
+		printk(KERN_ERR
+		       "maximum fifo write length is %d\n", FIFO_HW_SIZE);
+		return ML_ERROR_INVALID_PARAMETER;
+	}
+	while (bytesWritten < length) {
+		unsigned short thisLen =
+		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytesWritten);
+		i2cWrite[0] = MPUREG_FIFO_R_W;
+		memcpy(&i2cWrite[1], &data[bytesWritten], thisLen);
+		result = sensor_i2c_write((struct i2c_adapter *) sl_handle,
+					  slaveAddr, thisLen + 1,
+					  i2cWrite);
+		if (ML_SUCCESS != result)
+			return result;
+		bytesWritten += thisLen;
+	}
+	return ML_SUCCESS;
+}
+
+
+/**
+ *  @brief  used to read multiple bytes of data from the fifo.
+ *          This should be sent by I2C.
+ *
+ *  @param  slaveAddr       I2C slave address of device.
+ *  @param  length          Length of burst of data.
+ *  @param  data            Pointer to block of data.
+ *
+ *  @return Zero if successful; an error code otherwise
+ */
+tMLError MLSLSerialReadFifo(void *sl_handle,
+			    unsigned char slaveAddr,
+			    unsigned short length, unsigned char *data)
+{
+	tMLError result;
+	unsigned short bytesRead = 0;
+
+	if (length > FIFO_HW_SIZE) {
+		printk(KERN_ERR
+		       "maximum fifo read length is %d\n", FIFO_HW_SIZE);
+		return ML_ERROR_INVALID_PARAMETER;
+	}
+	while (bytesRead < length) {
+		unsigned short thisLen =
+		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytesRead);
+		result =
+		    sensor_i2c_read((struct i2c_adapter *) sl_handle,
+				    slaveAddr, MPUREG_FIFO_R_W, thisLen,
+				    &data[bytesRead]);
+		if (ML_SUCCESS != result)
+			return result;
+		bytesRead += thisLen;
+	}
+
+	return ML_SUCCESS;
+}
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/mpu3050/mlsl.h b/drivers/misc/mpu3050/mlsl.h
new file mode 100644
index 000000000000..76f69c77ba98
--- /dev/null
+++ b/drivers/misc/mpu3050/mlsl.h
@@ -0,0 +1,103 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+
+#ifndef __MSSL_H__
+#define __MSSL_H__
+
+#include "mltypes.h"
+#include "mpu.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* ------------ */
+/* - Defines. - */
+/* ------------ */
+
+/*
+ * NOTE : to properly support Yamaha compass reads,
+ * the max transfer size should be at least 9 B.
+ * Length in bytes, typically a power of 2 >= 2
+ */
+#define SERIAL_MAX_TRANSFER_SIZE 128
+
+/* ---------------------- */
+/* - Types definitions. - */
+/* ---------------------- */
+
+/* --------------------- */
+/* - Function p-types. - */
+/* --------------------- */
+
+	tMLError MLSLSerialOpen(char const *port,
+				void **sl_handle);
+	tMLError MLSLSerialReset(void *sl_handle);
+	tMLError MLSLSerialClose(void *sl_handle);
+
+	tMLError MLSLSerialWriteSingle(void *sl_handle,
+				       unsigned char slaveAddr,
+				       unsigned char registerAddr,
+				       unsigned char data);
+
+	tMLError MLSLSerialRead(void *sl_handle,
+				unsigned char slaveAddr,
+				unsigned char registerAddr,
+				unsigned short length,
+				unsigned char *data);
+
+	tMLError MLSLSerialWrite(void *sl_handle,
+				 unsigned char slaveAddr,
+				 unsigned short length,
+				 unsigned char const *data);
+
+	tMLError MLSLSerialReadMem(void *sl_handle,
+				   unsigned char slaveAddr,
+				   unsigned short memAddr,
+				   unsigned short length,
+				   unsigned char *data);
+
+	tMLError MLSLSerialWriteMem(void *sl_handle,
+				    unsigned char slaveAddr,
+				    unsigned short memAddr,
+				    unsigned short length,
+				    unsigned char const *data);
+
+	tMLError MLSLSerialReadFifo(void *sl_handle,
+				    unsigned char slaveAddr,
+				    unsigned short length,
+				    unsigned char *data);
+
+	tMLError MLSLSerialWriteFifo(void *sl_handle,
+				     unsigned char slaveAddr,
+				     unsigned short length,
+				     unsigned char const *data);
+
+	tMLError MLSLWriteCal(unsigned char *cal, unsigned int len);
+	tMLError MLSLReadCal(unsigned char *cal, unsigned int len);
+	tMLError MLSLGetCalLength(unsigned int *len);
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+#endif				/* MLSL_H */
diff --git a/drivers/misc/mpu3050/mltypes.h b/drivers/misc/mpu3050/mltypes.h
new file mode 100644
index 000000000000..d0b27fa89e78
--- /dev/null
+++ b/drivers/misc/mpu3050/mltypes.h
@@ -0,0 +1,227 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+
+/**
+ *  @defgroup MLERROR
+ *  @brief  Motion Library - Error definitions.
+ *          Definition of the error codes used within the MPL and returned
+ *          to the user.
+ *          Every function tries to return a meaningful error code basing
+ *          on the occuring error condition. The error code is numeric.
+ *
+ *          The available error codes and their associated values are:
+ *          - (0)       ML_SUCCESS
+ *          - (1)       ML_ERROR
+ *          - (2)       ML_ERROR_INVALID_PARAMETER
+ *          - (3)       ML_ERROR_FEATURE_NOT_ENABLED
+ *          - (4)       ML_ERROR_FEATURE_NOT_IMPLEMENTED
+ *          - (6)       ML_ERROR_DMP_NOT_STARTED
+ *          - (7)       ML_ERROR_DMP_STARTED
+ *          - (8)       ML_ERROR_NOT_OPENED
+ *          - (9)       ML_ERROR_OPENED
+ *          - (10)      ML_ERROR_INVALID_MODULE
+ *          - (11)      ML_ERROR_MEMORY_EXAUSTED
+ *          - (12)      ML_ERROR_DIVIDE_BY_ZERO
+ *          - (13)      ML_ERROR_ASSERTION_FAILURE
+ *          - (14)      ML_ERROR_FILE_OPEN
+ *          - (15)      ML_ERROR_FILE_READ
+ *          - (16)      ML_ERROR_FILE_WRITE
+ *          - (20)      ML_ERROR_SERIAL_CLOSED
+ *          - (21)      ML_ERROR_SERIAL_OPEN_ERROR
+ *          - (22)      ML_ERROR_SERIAL_READ
+ *          - (23)      ML_ERROR_SERIAL_WRITE
+ *          - (24)      ML_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED
+ *          - (25)      ML_ERROR_SM_TRANSITION
+ *          - (26)      ML_ERROR_SM_IMPROPER_STATE
+ *          - (30)      ML_ERROR_FIFO_OVERFLOW
+ *          - (31)      ML_ERROR_FIFO_FOOTER
+ *          - (32)      ML_ERROR_FIFO_READ_COUNT
+ *          - (33)      ML_ERROR_FIFO_READ_DATA
+ *          - (40)      ML_ERROR_MEMORY_SET
+ *          - (50)      ML_ERROR_LOG_MEMORY_ERROR
+ *          - (51)      ML_ERROR_LOG_OUTPUT_ERROR
+ *          - (60)      ML_ERROR_OS_BAD_PTR
+ *          - (61)      ML_ERROR_OS_BAD_HANDLE
+ *          - (62)      ML_ERROR_OS_CREATE_FAILED
+ *          - (63)      ML_ERROR_OS_LOCK_FAILED
+ *          - (70)      ML_ERROR_COMPASS_DATA_OVERFLOW
+ *          - (71)      ML_ERROR_COMPASS_DATA_UNDERFLOW
+ *          - (72)      ML_ERROR_COMPASS_DATA_NOT_READY
+ *          - (73)      ML_ERROR_COMPASS_DATA_ERROR
+ *          - (75)      ML_ERROR_CALIBRATION_LOAD
+ *          - (76)      ML_ERROR_CALIBRATION_STORE
+ *          - (77)      ML_ERROR_CALIBRATION_LEN
+ *          - (78)      ML_ERROR_CALIBRATION_CHECKSUM
+ *
+ *  @{
+ *      @file mltypes.h
+ *  @}
+ */
+
+#ifndef MLTYPES_H
+#define MLTYPES_H
+
+#ifdef __KERNEL__
+#include <linux/types.h>
+#else
+#include "stdint_invensense.h"
+#endif
+#include "log.h"
+
+/*---------------------------
+    ML Types
+---------------------------*/
+
+/**
+ *  @struct tMLError mltypes.h "mltypes"
+ *  @brief  The MPL Error Code return type.
+ *
+ *  @code
+ *      typedef unsigned char tMLError;
+ *  @endcode
+ */
+typedef unsigned char tMLError;
+
+#if defined(LINUX) || defined(__KERNEL__)
+typedef unsigned int HANDLE;
+#endif
+
+#ifdef __KERNEL__
+typedef HANDLE FILE;
+#endif
+
+#ifndef __cplusplus
+#ifndef __KERNEL__
+typedef int_fast8_t bool;
+#endif
+#endif
+
+/*---------------------------
+    ML Defines
+---------------------------*/
+
+#ifndef NULL
+#define NULL 0
+#endif
+
+#ifndef TRUE
+#define TRUE 1
+#endif
+
+#ifndef FALSE
+#define FALSE 0
+#endif
+
+/* Dimension of an array */
+#ifndef DIM
+#define DIM(array) (sizeof(array)/sizeof((array)[0]))
+#endif
+
+/* - ML Errors. - */
+#define ERROR_NAME(x)   (#x)
+#define ERROR_CHECK(x)                                                  \
+	do {								\
+		if (ML_SUCCESS != x) {					\
+			MPL_LOGE("%s|%s|%d returning %d\n",		\
+				__FILE__, __func__, __LINE__, x);	\
+			return x;					\
+		}							\
+	} while (0)
+
+#define ERROR_CHECK_FIRST(first, x)                                     \
+	{ if (ML_SUCCESS == first) first = x; }
+
+#define ML_SUCCESS                       (0)
+/* Generic Error code.  Proprietary Error Codes only */
+#define ML_ERROR                         (1)
+
+/* Compatibility and other generic error codes */
+#define ML_ERROR_INVALID_PARAMETER       (2)
+#define ML_ERROR_FEATURE_NOT_ENABLED     (3)
+#define ML_ERROR_FEATURE_NOT_IMPLEMENTED (4)
+#define ML_ERROR_DMP_NOT_STARTED         (6)
+#define ML_ERROR_DMP_STARTED             (7)
+#define ML_ERROR_NOT_OPENED              (8)
+#define ML_ERROR_OPENED                  (9)
+#define ML_ERROR_INVALID_MODULE         (10)
+#define ML_ERROR_MEMORY_EXAUSTED        (11)
+#define ML_ERROR_DIVIDE_BY_ZERO         (12)
+#define ML_ERROR_ASSERTION_FAILURE      (13)
+#define ML_ERROR_FILE_OPEN              (14)
+#define ML_ERROR_FILE_READ              (15)
+#define ML_ERROR_FILE_WRITE             (16)
+#define ML_ERROR_INVALID_CONFIGURATION  (17)
+
+/* Serial Communication */
+#define ML_ERROR_SERIAL_CLOSED          (20)
+#define ML_ERROR_SERIAL_OPEN_ERROR      (21)
+#define ML_ERROR_SERIAL_READ            (22)
+#define ML_ERROR_SERIAL_WRITE           (23)
+#define ML_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED  (24)
+
+/* SM = State Machine */
+#define ML_ERROR_SM_TRANSITION          (25)
+#define ML_ERROR_SM_IMPROPER_STATE      (26)
+
+/* Fifo */
+#define ML_ERROR_FIFO_OVERFLOW          (30)
+#define ML_ERROR_FIFO_FOOTER            (31)
+#define ML_ERROR_FIFO_READ_COUNT        (32)
+#define ML_ERROR_FIFO_READ_DATA         (33)
+
+/* Memory & Registers, Set & Get */
+#define ML_ERROR_MEMORY_SET             (40)
+
+#define ML_ERROR_LOG_MEMORY_ERROR       (50)
+#define ML_ERROR_LOG_OUTPUT_ERROR       (51)
+
+/* OS interface errors */
+#define ML_ERROR_OS_BAD_PTR             (60)
+#define ML_ERROR_OS_BAD_HANDLE          (61)
+#define ML_ERROR_OS_CREATE_FAILED       (62)
+#define ML_ERROR_OS_LOCK_FAILED         (63)
+
+/* Compass errors */
+#define ML_ERROR_COMPASS_DATA_OVERFLOW  (70)
+#define ML_ERROR_COMPASS_DATA_UNDERFLOW (71)
+#define ML_ERROR_COMPASS_DATA_NOT_READY (72)
+#define ML_ERROR_COMPASS_DATA_ERROR     (73)
+
+/* Load/Store calibration */
+#define ML_ERROR_CALIBRATION_LOAD       (75)
+#define ML_ERROR_CALIBRATION_STORE      (76)
+#define ML_ERROR_CALIBRATION_LEN        (77)
+#define ML_ERROR_CALIBRATION_CHECKSUM   (78)
+
+/* Accel errors */
+#define ML_ERROR_ACCEL_DATA_OVERFLOW    (79)
+#define ML_ERROR_ACCEL_DATA_UNDERFLOW   (80)
+#define ML_ERROR_ACCEL_DATA_NOT_READY   (81)
+#define ML_ERROR_ACCEL_DATA_ERROR       (82)
+
+/* For Linux coding compliance */
+#ifndef __KERNEL__
+#define EXPORT_SYMBOL(x)
+#endif
+
+/*---------------------------
+    p-Types
+---------------------------*/
+
+#endif				/* MLTYPES_H */
diff --git a/drivers/misc/mpu3050/mpu-dev.c b/drivers/misc/mpu3050/mpu-dev.c
new file mode 100644
index 000000000000..4dba44d45483
--- /dev/null
+++ b/drivers/misc/mpu3050/mpu-dev.c
@@ -0,0 +1,1310 @@
+/*
+    mpu-dev.c - mpu3050 char device interface  2  $License:
+
+    Copyright (C) 1995-97 Simon G. Vogl
+    Copyright (C) 1998-99 Frodo Looijaard <frodol@dds.nl>
+    Copyright (C) 2003 Greg Kroah-Hartman <greg@kroah.com>
+
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+ */
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include <linux/pm.h>
+#include <linux/mutex.h>
+#include <linux/suspend.h>
+#include <linux/poll.h>
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+
+#include "mpuirq.h"
+#include "slaveirq.h"
+#include "mlsl.h"
+#include "mpu-i2c.h"
+#include "mldl_cfg.h"
+#include "mpu.h"
+
+#define MPU3050_EARLY_SUSPEND_IN_DRIVER 0
+
+/* Platform data for the MPU */
+struct mpu_private_data {
+	struct mldl_cfg mldl_cfg;
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	struct early_suspend early_suspend;
+#endif
+	struct mutex mutex;
+	wait_queue_head_t mpu_event_wait;
+	struct completion completion;
+	struct timer_list timeout;
+	struct notifier_block nb;
+	struct mpuirq_data mpu_pm_event;
+	int response_timeout;	/* In seconds */
+	unsigned long event;
+	int pid;
+};
+
+static struct i2c_client *this_client;
+
+
+static void
+mpu_pm_timeout(u_long data)
+{
+	struct mpu_private_data *mpu = (struct mpu_private_data *) data;
+	dev_dbg(&this_client->adapter->dev, "%s\n", __func__);
+	complete(&mpu->completion);
+}
+
+static int mpu_pm_notifier_callback(struct notifier_block *nb,
+				    unsigned long event,
+				    void *unused)
+{
+	struct mpu_private_data *mpu =
+		container_of(nb, struct mpu_private_data, nb);
+	struct timeval event_time;
+	dev_dbg(&this_client->adapter->dev, "%s: %ld\n", __func__, event);
+
+	/* Prevent the file handle from being closed before we initialize
+	   the completion event */
+	mutex_lock(&mpu->mutex);
+	if (!(mpu->pid) ||
+		(event != PM_SUSPEND_PREPARE && event != PM_POST_SUSPEND)) {
+		mutex_unlock(&mpu->mutex);
+		return NOTIFY_OK;
+	}
+
+	do_gettimeofday(&event_time);
+	mpu->mpu_pm_event.interruptcount++;
+	mpu->mpu_pm_event.irqtime =
+		(((long long) event_time.tv_sec) << 32) +
+		event_time.tv_usec;
+	mpu->mpu_pm_event.data_type = MPUIRQ_DATA_TYPE_PM_EVENT;
+	mpu->mpu_pm_event.data_size = sizeof(unsigned long);
+	mpu->mpu_pm_event.data = &mpu->event;
+
+	if (event == PM_SUSPEND_PREPARE)
+		mpu->event = MPU_PM_EVENT_SUSPEND_PREPARE;
+	if (event == PM_POST_SUSPEND)
+		mpu->event = MPU_PM_EVENT_POST_SUSPEND;
+
+	if (mpu->response_timeout > 0) {
+		mpu->timeout.expires = jiffies + mpu->response_timeout * HZ;
+		add_timer(&mpu->timeout);
+	}
+	INIT_COMPLETION(mpu->completion);
+	mutex_unlock(&mpu->mutex);
+
+	wake_up_interruptible(&mpu->mpu_event_wait);
+	wait_for_completion(&mpu->completion);
+	del_timer_sync(&mpu->timeout);
+	dev_dbg(&this_client->adapter->dev, "%s: %ld DONE\n", __func__, event);
+	return NOTIFY_OK;
+}
+
+static int mpu_open(struct inode *inode, struct file *file)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(this_client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	int result;
+	dev_dbg(&this_client->adapter->dev, "mpu_open\n");
+	dev_dbg(&this_client->adapter->dev, "current->pid %d\n",
+		current->pid);
+	mpu->pid = current->pid;
+	file->private_data = this_client;
+	/* we could do some checking on the flags supplied by "open" */
+	/* i.e. O_NONBLOCK */
+	/* -> set some flag to disable interruptible_sleep_on in mpu_read */
+
+	/* Reset the sensors to the default */
+	result = mutex_lock_interruptible(&mpu->mutex);
+	if (result) {
+		dev_err(&this_client->adapter->dev,
+			"%s: mutex_lock_interruptible returned %d\n",
+			__func__, result);
+		return result;
+	}
+	mldl_cfg->requested_sensors = ML_THREE_AXIS_GYRO;
+	if (mldl_cfg->accel && mldl_cfg->accel->resume)
+		mldl_cfg->requested_sensors |= ML_THREE_AXIS_ACCEL;
+
+	if (mldl_cfg->compass && mldl_cfg->compass->resume)
+		mldl_cfg->requested_sensors |= ML_THREE_AXIS_COMPASS;
+
+	if (mldl_cfg->pressure && mldl_cfg->pressure->resume)
+		mldl_cfg->requested_sensors |= ML_THREE_AXIS_PRESSURE;
+	mutex_unlock(&mpu->mutex);
+	return 0;
+}
+
+/* close function - called when the "file" /dev/mpu is closed in userspace   */
+static int mpu_release(struct inode *inode, struct file *file)
+{
+	struct i2c_client *client =
+	    (struct i2c_client *) file->private_data;
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct i2c_adapter *accel_adapter;
+	struct i2c_adapter *compass_adapter;
+	struct i2c_adapter *pressure_adapter;
+	int result = 0;
+
+	accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+	compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+	pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+	mutex_lock(&mpu->mutex);
+	result = mpu3050_suspend(mldl_cfg, client->adapter,
+				 accel_adapter, compass_adapter,
+				 pressure_adapter,
+				 TRUE, TRUE, TRUE, TRUE);
+	mpu->pid = 0;
+	mutex_unlock(&mpu->mutex);
+	complete(&mpu->completion);
+	dev_dbg(&this_client->adapter->dev, "mpu_release\n");
+	return result;
+}
+
+/* read function called when from /dev/mpu is read.  Read from the FIFO */
+static ssize_t mpu_read(struct file *file,
+			char __user *buf, size_t count, loff_t *offset)
+{
+	struct mpuirq_data local_mpu_pm_event;
+	struct i2c_client *client =
+	    (struct i2c_client *) file->private_data;
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(client);
+	size_t len = sizeof(mpu->mpu_pm_event) + sizeof(unsigned long);
+	int err;
+
+	if (!mpu->event && (!(file->f_flags & O_NONBLOCK)))
+		wait_event_interruptible(mpu->mpu_event_wait, mpu->event);
+
+	if (!mpu->event || NULL == buf
+		|| count < sizeof(mpu->mpu_pm_event) + sizeof(unsigned long))
+		return 0;
+
+	err = copy_from_user(&local_mpu_pm_event, buf,
+			sizeof(mpu->mpu_pm_event));
+	if (err != 0) {
+		dev_err(&this_client->adapter->dev,
+			"Copy from user returned %d\n", err);
+		return -EFAULT;
+	}
+
+	mpu->mpu_pm_event.data = local_mpu_pm_event.data;
+	err = copy_to_user((unsigned long __user *)local_mpu_pm_event.data,
+			&mpu->event,
+			sizeof(mpu->event));
+	if (err != 0) {
+		dev_err(&this_client->adapter->dev,
+			"Copy to user returned %d\n", err);
+		return -EFAULT;
+	}
+	err = copy_to_user(buf, &mpu->mpu_pm_event, sizeof(mpu->mpu_pm_event));
+	if (err != 0) {
+		dev_err(&this_client->adapter->dev,
+			"Copy to user returned %d\n", err);
+		return -EFAULT;
+	}
+	mpu->event = 0;
+	return len;
+}
+
+static unsigned int mpu_poll(struct file *file, struct poll_table_struct *poll)
+{
+	struct i2c_client *client =
+	    (struct i2c_client *) file->private_data;
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(client);
+	int mask = 0;
+
+	poll_wait(file, &mpu->mpu_event_wait, poll);
+	if (mpu->event)
+		mask |= POLLIN | POLLRDNORM;
+	return mask;
+}
+
+static int
+mpu_ioctl_set_mpu_pdata(struct i2c_client *client, unsigned long arg)
+{
+	int ii;
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(client);
+	struct mpu3050_platform_data *pdata = mpu->mldl_cfg.pdata;
+	struct mpu3050_platform_data local_pdata;
+
+	if (copy_from_user(&local_pdata, (unsigned char __user *) arg,
+				sizeof(local_pdata)))
+		return -EFAULT;
+
+	pdata->int_config = local_pdata.int_config;
+	for (ii = 0; ii < DIM(pdata->orientation); ii++)
+		pdata->orientation[ii] = local_pdata.orientation[ii];
+	pdata->level_shifter = local_pdata.level_shifter;
+
+	pdata->accel.address = local_pdata.accel.address;
+	for (ii = 0; ii < DIM(pdata->accel.orientation); ii++)
+		pdata->accel.orientation[ii] =
+			local_pdata.accel.orientation[ii];
+
+	pdata->compass.address = local_pdata.compass.address;
+	for (ii = 0; ii < DIM(pdata->compass.orientation); ii++)
+		pdata->compass.orientation[ii] =
+			local_pdata.compass.orientation[ii];
+
+	pdata->pressure.address = local_pdata.pressure.address;
+	for (ii = 0; ii < DIM(pdata->pressure.orientation); ii++)
+		pdata->pressure.orientation[ii] =
+			local_pdata.pressure.orientation[ii];
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	return ML_SUCCESS;
+}
+
+static int
+mpu_ioctl_set_mpu_config(struct i2c_client *client, unsigned long arg)
+{
+	int ii;
+	int result = ML_SUCCESS;
+	struct mpu_private_data *mpu =
+		(struct mpu_private_data *) i2c_get_clientdata(client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct mldl_cfg *temp_mldl_cfg;
+
+	dev_dbg(&this_client->adapter->dev, "%s\n", __func__);
+
+	temp_mldl_cfg = kzalloc(sizeof(struct mldl_cfg), GFP_KERNEL);
+	if (NULL == temp_mldl_cfg)
+		return -ENOMEM;
+
+	/*
+	 * User space is not allowed to modify accel compass pressure or
+	 * pdata structs, as well as silicon_revision product_id or trim
+	 */
+	if (copy_from_user(temp_mldl_cfg, (struct mldl_cfg __user *) arg,
+				offsetof(struct mldl_cfg, silicon_revision))) {
+		result = -EFAULT;
+		goto out;
+	}
+
+	if (mldl_cfg->gyro_is_suspended) {
+		if (mldl_cfg->addr != temp_mldl_cfg->addr)
+			mldl_cfg->gyro_needs_reset = TRUE;
+
+		if (mldl_cfg->int_config != temp_mldl_cfg->int_config)
+			mldl_cfg->gyro_needs_reset = TRUE;
+
+		if (mldl_cfg->ext_sync != temp_mldl_cfg->ext_sync)
+			mldl_cfg->gyro_needs_reset = TRUE;
+
+		if (mldl_cfg->full_scale != temp_mldl_cfg->full_scale)
+			mldl_cfg->gyro_needs_reset = TRUE;
+
+		if (mldl_cfg->lpf != temp_mldl_cfg->lpf)
+			mldl_cfg->gyro_needs_reset = TRUE;
+
+		if (mldl_cfg->clk_src != temp_mldl_cfg->clk_src)
+			mldl_cfg->gyro_needs_reset = TRUE;
+
+		if (mldl_cfg->divider != temp_mldl_cfg->divider)
+			mldl_cfg->gyro_needs_reset = TRUE;
+
+		if (mldl_cfg->dmp_enable != temp_mldl_cfg->dmp_enable)
+			mldl_cfg->gyro_needs_reset = TRUE;
+
+		if (mldl_cfg->fifo_enable != temp_mldl_cfg->fifo_enable)
+			mldl_cfg->gyro_needs_reset = TRUE;
+
+		if (mldl_cfg->dmp_cfg1 != temp_mldl_cfg->dmp_cfg1)
+			mldl_cfg->gyro_needs_reset = TRUE;
+
+		if (mldl_cfg->dmp_cfg2 != temp_mldl_cfg->dmp_cfg2)
+			mldl_cfg->gyro_needs_reset = TRUE;
+
+		if (mldl_cfg->gyro_power != temp_mldl_cfg->gyro_power)
+			mldl_cfg->gyro_needs_reset = TRUE;
+
+		for (ii = 0; ii < MPU_NUM_AXES; ii++)
+			if (mldl_cfg->offset_tc[ii] !=
+			    temp_mldl_cfg->offset_tc[ii])
+				mldl_cfg->gyro_needs_reset = TRUE;
+
+		for (ii = 0; ii < MPU_NUM_AXES; ii++)
+			if (mldl_cfg->offset[ii] != temp_mldl_cfg->offset[ii])
+				mldl_cfg->gyro_needs_reset = TRUE;
+
+		if (memcmp(mldl_cfg->ram, temp_mldl_cfg->ram,
+				MPU_MEM_NUM_RAM_BANKS * MPU_MEM_BANK_SIZE *
+				sizeof(unsigned char)))
+			mldl_cfg->gyro_needs_reset = TRUE;
+	}
+
+	memcpy(mldl_cfg, temp_mldl_cfg,
+		offsetof(struct mldl_cfg, silicon_revision));
+
+out:
+	kfree(temp_mldl_cfg);
+	return result;
+}
+
+static int
+mpu_ioctl_get_mpu_config(struct i2c_client *client, unsigned long arg)
+{
+	/* Have to be careful as there are 3 pointers in the mldl_cfg
+	 * structure */
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct mldl_cfg *local_mldl_cfg;
+	int retval = 0;
+
+	local_mldl_cfg = kzalloc(sizeof(struct mldl_cfg), GFP_KERNEL);
+	if (NULL == local_mldl_cfg)
+		return -ENOMEM;
+
+	retval =
+	    copy_from_user(local_mldl_cfg, (struct mldl_cfg __user *) arg,
+			   sizeof(struct mldl_cfg));
+	if (retval) {
+		dev_err(&this_client->adapter->dev,
+			"%s|%s:%d: EFAULT on arg\n",
+			__FILE__, __func__, __LINE__);
+		retval = -EFAULT;
+		goto out;
+	}
+
+	/* Fill in the accel, compass, pressure and pdata pointers */
+	if (mldl_cfg->accel) {
+		retval = copy_to_user((void __user *)local_mldl_cfg->accel,
+				      mldl_cfg->accel,
+				      sizeof(*mldl_cfg->accel));
+		if (retval) {
+			dev_err(&this_client->adapter->dev,
+				"%s|%s:%d: EFAULT on accel\n",
+				__FILE__, __func__, __LINE__);
+			retval = -EFAULT;
+			goto out;
+		}
+	}
+
+	if (mldl_cfg->compass) {
+		retval = copy_to_user((void __user *)local_mldl_cfg->compass,
+				      mldl_cfg->compass,
+				      sizeof(*mldl_cfg->compass));
+		if (retval) {
+			dev_err(&this_client->adapter->dev,
+				"%s|%s:%d: EFAULT on compass\n",
+				__FILE__, __func__, __LINE__);
+			retval = -EFAULT;
+			goto out;
+		}
+	}
+
+	if (mldl_cfg->pressure) {
+		retval = copy_to_user((void __user *)local_mldl_cfg->pressure,
+				      mldl_cfg->pressure,
+				      sizeof(*mldl_cfg->pressure));
+		if (retval) {
+			dev_err(&this_client->adapter->dev,
+				"%s|%s:%d: EFAULT on pressure\n",
+				__FILE__, __func__, __LINE__);
+			retval = -EFAULT;
+			goto out;
+		}
+	}
+
+	if (mldl_cfg->pdata) {
+		retval = copy_to_user((void __user *)local_mldl_cfg->pdata,
+				      mldl_cfg->pdata,
+				      sizeof(*mldl_cfg->pdata));
+		if (retval) {
+			dev_err(&this_client->adapter->dev,
+				"%s|%s:%d: EFAULT on pdata\n",
+				__FILE__, __func__, __LINE__);
+			retval = -EFAULT;
+			goto out;
+		}
+	}
+
+	/* Do not modify the accel, compass, pressure and pdata pointers */
+	retval = copy_to_user((struct mldl_cfg __user *) arg,
+			      mldl_cfg, offsetof(struct mldl_cfg, accel));
+
+	if (retval)
+		retval = -EFAULT;
+out:
+	kfree(local_mldl_cfg);
+	return retval;
+}
+
+/**
+ * Pass a requested slave configuration to the slave sensor
+ *
+ * @param adapter the adaptor to use to communicate with the slave
+ * @param mldl_cfg the mldl configuration structuer
+ * @param slave pointer to the slave descriptor
+ * @param usr_config The configuration to pass to the slave sensor
+ *
+ * @return 0 or non-zero error code
+ */
+static int slave_config(void *adapter,
+			struct mldl_cfg *mldl_cfg,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			struct ext_slave_config __user *usr_config)
+{
+	int retval = ML_SUCCESS;
+	struct ext_slave_config config;
+	if ((!slave) || (!slave->config))
+		return retval;
+
+	retval = copy_from_user(&config, usr_config, sizeof(config));
+	if (retval)
+		return -EFAULT;
+
+	if (config.len && config.data) {
+		int *data;
+		data = kzalloc(config.len, GFP_KERNEL);
+		if (!data)
+			return ML_ERROR_MEMORY_EXAUSTED;
+
+		retval = copy_from_user(data,
+					(void __user *)config.data,
+					config.len);
+		if (retval) {
+			retval = -EFAULT;
+			kfree(data);
+			return retval;
+		}
+		config.data = data;
+	}
+	retval = slave->config(adapter, slave, pdata, &config);
+	kfree(config.data);
+	return retval;
+}
+
+/**
+ * Get a requested slave configuration from the slave sensor
+ *
+ * @param adapter the adaptor to use to communicate with the slave
+ * @param mldl_cfg the mldl configuration structuer
+ * @param slave pointer to the slave descriptor
+ * @param usr_config The configuration for the slave to fill out
+ *
+ * @return 0 or non-zero error code
+ */
+static int slave_get_config(void *adapter,
+			struct mldl_cfg *mldl_cfg,
+			struct ext_slave_descr *slave,
+			struct ext_slave_platform_data *pdata,
+			struct ext_slave_config __user *usr_config)
+{
+	int retval = ML_SUCCESS;
+	struct ext_slave_config config;
+	void *user_data;
+	if (!(slave) || !(slave->get_config))
+		return ML_SUCCESS;
+
+	retval = copy_from_user(&config, usr_config, sizeof(config));
+	if (retval)
+		return -EFAULT;
+
+	user_data = config.data;
+	if (config.len && config.data) {
+		int *data;
+		data = kzalloc(config.len, GFP_KERNEL);
+		if (!data)
+			return ML_ERROR_MEMORY_EXAUSTED;
+
+		retval = copy_from_user(data,
+					(void __user *)config.data,
+					config.len);
+		if (retval) {
+			retval = -EFAULT;
+			kfree(data);
+			return retval;
+		}
+		config.data = data;
+	}
+	retval = slave->get_config(adapter, slave, pdata, &config);
+	if (retval) {
+		kfree(config.data);
+		return retval;
+	}
+	retval = copy_to_user((unsigned char __user *) user_data,
+			config.data,
+			config.len);
+	kfree(config.data);
+	return retval;
+}
+
+static int mpu_handle_mlsl(void *sl_handle,
+			unsigned char addr,
+			unsigned int cmd,
+			struct mpu_read_write __user *usr_msg)
+{
+	int retval = ML_SUCCESS;
+	struct mpu_read_write msg;
+	unsigned char *user_data;
+	retval = copy_from_user(&msg, usr_msg, sizeof(msg));
+	if (retval)
+		return -EFAULT;
+
+	user_data = msg.data;
+	if (msg.length && msg.data) {
+		unsigned char *data;
+		data = kzalloc(msg.length, GFP_KERNEL);
+		if (!data)
+			return ML_ERROR_MEMORY_EXAUSTED;
+
+		retval = copy_from_user(data,
+					(void __user *)msg.data,
+					msg.length);
+		if (retval) {
+			retval = -EFAULT;
+			kfree(data);
+			return retval;
+		}
+		msg.data = data;
+	} else {
+		return ML_ERROR_INVALID_PARAMETER;
+	}
+
+	switch (cmd) {
+	case MPU_READ:
+		retval = MLSLSerialRead(sl_handle, addr,
+					msg.address, msg.length, msg.data);
+		break;
+	case MPU_WRITE:
+		retval = MLSLSerialWrite(sl_handle, addr,
+					msg.length, msg.data);
+		break;
+	case MPU_READ_MEM:
+		retval = MLSLSerialReadMem(sl_handle, addr,
+					msg.address, msg.length, msg.data);
+		break;
+	case MPU_WRITE_MEM:
+		retval = MLSLSerialWriteMem(sl_handle, addr,
+					msg.address, msg.length, msg.data);
+		break;
+	case MPU_READ_FIFO:
+		retval = MLSLSerialReadFifo(sl_handle, addr,
+					msg.length, msg.data);
+		break;
+	case MPU_WRITE_FIFO:
+		retval = MLSLSerialWriteFifo(sl_handle, addr,
+					msg.length, msg.data);
+		break;
+
+	};
+	retval = copy_to_user((unsigned char __user *) user_data,
+			msg.data,
+			msg.length);
+	kfree(msg.data);
+	return retval;
+}
+
+/* ioctl - I/O control */
+static long mpu_ioctl(struct file *file,
+		      unsigned int cmd, unsigned long arg)
+{
+	struct i2c_client *client =
+	    (struct i2c_client *) file->private_data;
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	int retval = 0;
+	struct i2c_adapter *accel_adapter;
+	struct i2c_adapter *compass_adapter;
+	struct i2c_adapter *pressure_adapter;
+
+	accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+	compass_adapter =
+	    i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+	pressure_adapter =
+	    i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+	retval = mutex_lock_interruptible(&mpu->mutex);
+	if (retval) {
+		dev_err(&this_client->adapter->dev,
+			"%s: mutex_lock_interruptible returned %d\n",
+			__func__, retval);
+		return retval;
+	}
+
+	switch (cmd) {
+	case MPU_SET_MPU_CONFIG:
+		retval = mpu_ioctl_set_mpu_config(client, arg);
+		break;
+	case MPU_SET_PLATFORM_DATA:
+		retval = mpu_ioctl_set_mpu_pdata(client, arg);
+		break;
+	case MPU_GET_MPU_CONFIG:
+		retval = mpu_ioctl_get_mpu_config(client, arg);
+		break;
+	case MPU_READ:
+	case MPU_WRITE:
+	case MPU_READ_MEM:
+	case MPU_WRITE_MEM:
+	case MPU_READ_FIFO:
+	case MPU_WRITE_FIFO:
+		retval = mpu_handle_mlsl(client->adapter, mldl_cfg->addr, cmd,
+					(struct mpu_read_write __user *) arg);
+		break;
+	case MPU_CONFIG_ACCEL:
+		retval = slave_config(accel_adapter, mldl_cfg,
+				mldl_cfg->accel,
+				&mldl_cfg->pdata->accel,
+				(struct ext_slave_config __user *) arg);
+		break;
+	case MPU_CONFIG_COMPASS:
+		retval = slave_config(compass_adapter, mldl_cfg,
+				mldl_cfg->compass,
+				&mldl_cfg->pdata->compass,
+				(struct ext_slave_config __user *) arg);
+		break;
+	case MPU_CONFIG_PRESSURE:
+		retval = slave_config(pressure_adapter, mldl_cfg,
+				mldl_cfg->pressure,
+				&mldl_cfg->pdata->pressure,
+				(struct ext_slave_config __user *) arg);
+		break;
+	case MPU_GET_CONFIG_ACCEL:
+		retval = slave_get_config(accel_adapter, mldl_cfg,
+					mldl_cfg->accel,
+					&mldl_cfg->pdata->accel,
+					(struct ext_slave_config __user *) arg);
+		break;
+	case MPU_GET_CONFIG_COMPASS:
+		retval = slave_get_config(compass_adapter, mldl_cfg,
+					mldl_cfg->compass,
+					&mldl_cfg->pdata->compass,
+					(struct ext_slave_config __user *) arg);
+		break;
+	case MPU_GET_CONFIG_PRESSURE:
+		retval = slave_get_config(pressure_adapter, mldl_cfg,
+					mldl_cfg->pressure,
+					&mldl_cfg->pdata->pressure,
+					(struct ext_slave_config __user *) arg);
+		break;
+	case MPU_SUSPEND:
+	{
+		unsigned long sensors;
+		sensors = ~(mldl_cfg->requested_sensors);
+		retval = mpu3050_suspend(mldl_cfg,
+					client->adapter,
+					accel_adapter,
+					compass_adapter,
+					pressure_adapter,
+					((sensors & ML_THREE_AXIS_GYRO)
+						== ML_THREE_AXIS_GYRO),
+					((sensors & ML_THREE_AXIS_ACCEL)
+						== ML_THREE_AXIS_ACCEL),
+					((sensors & ML_THREE_AXIS_COMPASS)
+						== ML_THREE_AXIS_COMPASS),
+					((sensors & ML_THREE_AXIS_PRESSURE)
+						== ML_THREE_AXIS_PRESSURE));
+	}
+	break;
+	case MPU_RESUME:
+	{
+		unsigned long sensors;
+		sensors = mldl_cfg->requested_sensors;
+		retval = mpu3050_resume(mldl_cfg,
+					client->adapter,
+					accel_adapter,
+					compass_adapter,
+					pressure_adapter,
+					sensors & ML_THREE_AXIS_GYRO,
+					sensors & ML_THREE_AXIS_ACCEL,
+					sensors & ML_THREE_AXIS_COMPASS,
+					sensors & ML_THREE_AXIS_PRESSURE);
+	}
+	break;
+	case MPU_PM_EVENT_HANDLED:
+		dev_dbg(&this_client->adapter->dev,
+			"%s: %d\n", __func__, cmd);
+		complete(&mpu->completion);
+		break;
+	case MPU_READ_ACCEL:
+	{
+		unsigned char data[6];
+		retval = mpu3050_read_accel(mldl_cfg, client->adapter,
+					    data);
+		if ((ML_SUCCESS == retval) &&
+		    (copy_to_user((unsigned char __user *) arg,
+			    data, sizeof(data))))
+			retval = -EFAULT;
+	}
+	break;
+	case MPU_READ_COMPASS:
+	{
+		unsigned char data[6];
+		struct i2c_adapter *compass_adapt =
+			i2c_get_adapter(mldl_cfg->pdata->compass.
+					adapt_num);
+		retval = mpu3050_read_compass(mldl_cfg, compass_adapt,
+						 data);
+		if ((ML_SUCCESS == retval) &&
+			(copy_to_user((unsigned char *) arg,
+				data, sizeof(data))))
+			retval = -EFAULT;
+	}
+	break;
+	case MPU_READ_PRESSURE:
+	{
+		unsigned char data[3];
+		struct i2c_adapter *pressure_adapt =
+			i2c_get_adapter(mldl_cfg->pdata->pressure.
+					adapt_num);
+		retval =
+			mpu3050_read_pressure(mldl_cfg, pressure_adapt,
+					data);
+		if ((ML_SUCCESS == retval) &&
+		    (copy_to_user((unsigned char __user *) arg,
+			    data, sizeof(data))))
+			retval = -EFAULT;
+	}
+	break;
+	default:
+		dev_err(&this_client->adapter->dev,
+			"%s: Unknown cmd %x, arg %lu\n", __func__, cmd,
+			arg);
+		retval = -EINVAL;
+	}
+
+	mutex_unlock(&mpu->mutex);
+	return retval;
+}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+void mpu3050_early_suspend(struct early_suspend *h)
+{
+	struct mpu_private_data *mpu = container_of(h,
+						    struct
+						    mpu_private_data,
+						    early_suspend);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct i2c_adapter *accel_adapter;
+	struct i2c_adapter *compass_adapter;
+	struct i2c_adapter *pressure_adapter;
+
+	accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+	compass_adapter =
+	    i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+	pressure_adapter =
+	    i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+	dev_dbg(&this_client->adapter->dev, "%s: %d, %d\n", __func__,
+		h->level, mpu->mldl_cfg.gyro_is_suspended);
+	if (MPU3050_EARLY_SUSPEND_IN_DRIVER) {
+		mutex_lock(&mpu->mutex);
+		(void) mpu3050_suspend(mldl_cfg, this_client->adapter,
+				accel_adapter, compass_adapter,
+				pressure_adapter, TRUE, TRUE, TRUE, TRUE);
+		mutex_unlock(&mpu->mutex);
+	}
+}
+
+void mpu3050_early_resume(struct early_suspend *h)
+{
+	struct mpu_private_data *mpu = container_of(h,
+						    struct
+						    mpu_private_data,
+						    early_suspend);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct i2c_adapter *accel_adapter;
+	struct i2c_adapter *compass_adapter;
+	struct i2c_adapter *pressure_adapter;
+
+	accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+	compass_adapter =
+	    i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+	pressure_adapter =
+	    i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+	if (MPU3050_EARLY_SUSPEND_IN_DRIVER) {
+		if (mpu->pid) {
+			unsigned long sensors = mldl_cfg->requested_sensors;
+			mutex_lock(&mpu->mutex);
+			(void) mpu3050_resume(mldl_cfg,
+					this_client->adapter,
+					accel_adapter,
+					compass_adapter,
+					pressure_adapter,
+					sensors & ML_THREE_AXIS_GYRO,
+					sensors & ML_THREE_AXIS_ACCEL,
+					sensors & ML_THREE_AXIS_COMPASS,
+					sensors & ML_THREE_AXIS_PRESSURE);
+			mutex_unlock(&mpu->mutex);
+			dev_dbg(&this_client->adapter->dev,
+				"%s for pid %d\n", __func__, mpu->pid);
+		}
+	}
+	dev_dbg(&this_client->adapter->dev, "%s: %d\n", __func__, h->level);
+}
+#endif
+
+void mpu_shutdown(struct i2c_client *client)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct i2c_adapter *accel_adapter;
+	struct i2c_adapter *compass_adapter;
+	struct i2c_adapter *pressure_adapter;
+
+	accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+	compass_adapter =
+	    i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+	pressure_adapter =
+	    i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+	mutex_lock(&mpu->mutex);
+	(void) mpu3050_suspend(mldl_cfg, this_client->adapter,
+			       accel_adapter, compass_adapter, pressure_adapter,
+			       TRUE, TRUE, TRUE, TRUE);
+	mutex_unlock(&mpu->mutex);
+	dev_dbg(&this_client->adapter->dev, "%s\n", __func__);
+}
+
+int mpu_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct i2c_adapter *accel_adapter;
+	struct i2c_adapter *compass_adapter;
+	struct i2c_adapter *pressure_adapter;
+
+	accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+	compass_adapter =
+	    i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+	pressure_adapter =
+	    i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+	mutex_lock(&mpu->mutex);
+	if (!mldl_cfg->ignore_system_suspend) {
+		dev_dbg(&this_client->adapter->dev,
+			"%s: suspending on event %d\n", __func__,
+			mesg.event);
+		(void) mpu3050_suspend(mldl_cfg, this_client->adapter,
+				       accel_adapter, compass_adapter,
+				       pressure_adapter,
+				       TRUE, TRUE, TRUE, TRUE);
+	} else {
+		dev_dbg(&this_client->adapter->dev,
+			"%s: Already suspended %d\n", __func__,
+			mesg.event);
+	}
+	mutex_unlock(&mpu->mutex);
+	return 0;
+}
+
+int mpu_resume(struct i2c_client *client)
+{
+	struct mpu_private_data *mpu =
+	    (struct mpu_private_data *) i2c_get_clientdata(client);
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct i2c_adapter *accel_adapter;
+	struct i2c_adapter *compass_adapter;
+	struct i2c_adapter *pressure_adapter;
+
+	accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+	compass_adapter =
+	    i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+	pressure_adapter =
+	    i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+	mutex_lock(&mpu->mutex);
+	if (mpu->pid && !mldl_cfg->ignore_system_suspend) {
+		unsigned long sensors = mldl_cfg->requested_sensors;
+		(void) mpu3050_resume(mldl_cfg, this_client->adapter,
+				      accel_adapter,
+				      compass_adapter,
+				      pressure_adapter,
+				      sensors & ML_THREE_AXIS_GYRO,
+				      sensors & ML_THREE_AXIS_ACCEL,
+				      sensors & ML_THREE_AXIS_COMPASS,
+				      sensors & ML_THREE_AXIS_PRESSURE);
+		dev_dbg(&this_client->adapter->dev,
+			"%s for pid %d\n", __func__, mpu->pid);
+	}
+	mutex_unlock(&mpu->mutex);
+	return 0;
+}
+
+/* define which file operations are supported */
+static const struct file_operations mpu_fops = {
+	.owner = THIS_MODULE,
+	.read = mpu_read,
+	.poll = mpu_poll,
+
+#if HAVE_COMPAT_IOCTL
+	.compat_ioctl = mpu_ioctl,
+#endif
+#if HAVE_UNLOCKED_IOCTL
+	.unlocked_ioctl = mpu_ioctl,
+#endif
+	.open = mpu_open,
+	.release = mpu_release,
+};
+
+static unsigned short normal_i2c[] = { I2C_CLIENT_END };
+
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 32)
+I2C_CLIENT_INSMOD;
+#endif
+
+static struct miscdevice i2c_mpu_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "mpu", /* Same for both 3050 and 6000 */
+	.fops = &mpu_fops,
+};
+
+
+int mpu3050_probe(struct i2c_client *client,
+		  const struct i2c_device_id *devid)
+{
+	struct mpu3050_platform_data *pdata;
+	struct mpu_private_data *mpu;
+	struct mldl_cfg *mldl_cfg;
+	int res = 0;
+	struct i2c_adapter *accel_adapter = NULL;
+	struct i2c_adapter *compass_adapter = NULL;
+	struct i2c_adapter *pressure_adapter = NULL;
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		res = -ENODEV;
+		goto out_check_functionality_failed;
+	}
+
+	mpu = kzalloc(sizeof(struct mpu_private_data), GFP_KERNEL);
+	if (!mpu) {
+		res = -ENOMEM;
+		goto out_alloc_data_failed;
+	}
+
+	i2c_set_clientdata(client, mpu);
+	this_client = client;
+	mldl_cfg = &mpu->mldl_cfg;
+
+	init_waitqueue_head(&mpu->mpu_event_wait);
+
+	mutex_init(&mpu->mutex);
+	init_completion(&mpu->completion);
+
+	mpu->response_timeout = 60; /* Seconds */
+	mpu->timeout.function = mpu_pm_timeout;
+	mpu->timeout.data = (u_long) mpu;
+	init_timer(&mpu->timeout);
+
+	/* FIXME:
+	 * Do not register the pm_notifier as it causes
+	 * issues with resume sequence as there is no response
+	 * from user-space for power notifications for approx
+	 * 60 sec. Refer NV bug 858630 for more details.
+	 */
+#if 0
+	mpu->nb.notifier_call = mpu_pm_notifier_callback;
+	mpu->nb.priority = 0;
+	register_pm_notifier(&mpu->nb);
+#endif
+
+	pdata = (struct mpu3050_platform_data *) client->dev.platform_data;
+	if (!pdata) {
+		dev_warn(&this_client->adapter->dev,
+			 "Missing platform data for mpu3050\n");
+	} else {
+		mldl_cfg->pdata = pdata;
+
+#if defined(CONFIG_MPU_SENSORS_MPU3050_MODULE) || \
+    defined(CONFIG_MPU_SENSORS_MPU6000_MODULE)
+		pdata->accel.get_slave_descr = get_accel_slave_descr;
+		pdata->compass.get_slave_descr = get_compass_slave_descr;
+		pdata->pressure.get_slave_descr = get_pressure_slave_descr;
+#endif
+
+		if (pdata->accel.get_slave_descr) {
+			mldl_cfg->accel =
+			    pdata->accel.get_slave_descr();
+			dev_info(&this_client->adapter->dev,
+				 "%s: +%s\n", MPU_NAME,
+				 mldl_cfg->accel->name);
+			accel_adapter =
+				i2c_get_adapter(pdata->accel.adapt_num);
+			if (pdata->accel.irq > 0) {
+				dev_info(&this_client->adapter->dev,
+					"Installing Accel irq using %d\n",
+					pdata->accel.irq);
+				res = slaveirq_init(accel_adapter,
+						&pdata->accel,
+						"accelirq");
+				if (res)
+					goto out_accelirq_failed;
+			} else {
+				dev_warn(&this_client->adapter->dev,
+					"WARNING: Accel irq not assigned\n");
+			}
+		} else {
+			dev_warn(&this_client->adapter->dev,
+				 "%s: No Accel Present\n", MPU_NAME);
+		}
+
+		if (pdata->compass.get_slave_descr) {
+			mldl_cfg->compass =
+			    pdata->compass.get_slave_descr();
+			dev_info(&this_client->adapter->dev,
+				 "%s: +%s\n", MPU_NAME,
+				 mldl_cfg->compass->name);
+			compass_adapter =
+				i2c_get_adapter(pdata->compass.adapt_num);
+			if (pdata->compass.irq > 0) {
+				dev_info(&this_client->adapter->dev,
+					"Installing Compass irq using %d\n",
+					pdata->compass.irq);
+				res = slaveirq_init(compass_adapter,
+						&pdata->compass,
+						"compassirq");
+				if (res)
+					goto out_compassirq_failed;
+			} else {
+				dev_warn(&this_client->adapter->dev,
+					"WARNING: Compass irq not assigned\n");
+			}
+		} else {
+			dev_warn(&this_client->adapter->dev,
+				 "%s: No Compass Present\n", MPU_NAME);
+		}
+
+		if (pdata->pressure.get_slave_descr) {
+			mldl_cfg->pressure =
+			    pdata->pressure.get_slave_descr();
+			dev_info(&this_client->adapter->dev,
+				 "%s: +%s\n", MPU_NAME,
+				 mldl_cfg->pressure->name);
+			pressure_adapter =
+				i2c_get_adapter(pdata->pressure.adapt_num);
+
+			if (pdata->pressure.irq > 0) {
+				dev_info(&this_client->adapter->dev,
+					"Installing Pressure irq using %d\n",
+					pdata->pressure.irq);
+				res = slaveirq_init(pressure_adapter,
+						&pdata->pressure,
+						"pressureirq");
+				if (res)
+					goto out_pressureirq_failed;
+			} else {
+				dev_warn(&this_client->adapter->dev,
+					"WARNING: Pressure irq not assigned\n");
+			}
+		} else {
+			dev_warn(&this_client->adapter->dev,
+				 "%s: No Pressure Present\n", MPU_NAME);
+		}
+	}
+
+	mldl_cfg->addr = client->addr;
+	res = mpu3050_open(&mpu->mldl_cfg, client->adapter,
+			accel_adapter, compass_adapter, pressure_adapter);
+
+	if (res) {
+		dev_err(&this_client->adapter->dev,
+			"Unable to open %s %d\n", MPU_NAME, res);
+		res = -ENODEV;
+		goto out_whoami_failed;
+	}
+
+	res = misc_register(&i2c_mpu_device);
+	if (res < 0) {
+		dev_err(&this_client->adapter->dev,
+			"ERROR: misc_register returned %d\n", res);
+		goto out_misc_register_failed;
+	}
+
+	if (this_client->irq > 0) {
+		dev_info(&this_client->adapter->dev,
+			 "Installing irq using %d\n", this_client->irq);
+		res = mpuirq_init(this_client);
+		if (res)
+			goto out_mpuirq_failed;
+	} else {
+		dev_warn(&this_client->adapter->dev,
+			"Missing %s IRQ\n", MPU_NAME);
+	}
+
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	mpu->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	mpu->early_suspend.suspend = mpu3050_early_suspend;
+	mpu->early_suspend.resume = mpu3050_early_resume;
+	register_early_suspend(&mpu->early_suspend);
+#endif
+	return res;
+
+out_mpuirq_failed:
+	misc_deregister(&i2c_mpu_device);
+out_misc_register_failed:
+	mpu3050_close(&mpu->mldl_cfg, client->adapter,
+		accel_adapter, compass_adapter, pressure_adapter);
+out_whoami_failed:
+	if (pdata &&
+	    pdata->pressure.get_slave_descr &&
+	    pdata->pressure.irq)
+		slaveirq_exit(&pdata->pressure);
+out_pressureirq_failed:
+	if (pdata &&
+	    pdata->compass.get_slave_descr &&
+	    pdata->compass.irq)
+		slaveirq_exit(&pdata->compass);
+out_compassirq_failed:
+	if (pdata &&
+	    pdata->accel.get_slave_descr &&
+	    pdata->accel.irq)
+		slaveirq_exit(&pdata->accel);
+out_accelirq_failed:
+	kfree(mpu);
+out_alloc_data_failed:
+out_check_functionality_failed:
+	dev_err(&this_client->adapter->dev, "%s failed %d\n", __func__,
+		res);
+	return res;
+
+}
+
+static int mpu3050_remove(struct i2c_client *client)
+{
+	struct mpu_private_data *mpu = i2c_get_clientdata(client);
+	struct i2c_adapter *accel_adapter;
+	struct i2c_adapter *compass_adapter;
+	struct i2c_adapter *pressure_adapter;
+	struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
+	struct mpu3050_platform_data *pdata = mldl_cfg->pdata;
+
+	accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+	compass_adapter =
+	    i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num);
+	pressure_adapter =
+	    i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num);
+
+	dev_dbg(&client->adapter->dev, "%s\n", __func__);
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	unregister_early_suspend(&mpu->early_suspend);
+#endif
+	mpu3050_close(mldl_cfg, client->adapter,
+		accel_adapter, compass_adapter, pressure_adapter);
+
+	if (client->irq)
+		mpuirq_exit();
+
+	if (pdata &&
+	    pdata->pressure.get_slave_descr &&
+	    pdata->pressure.irq)
+		slaveirq_exit(&pdata->pressure);
+
+	if (pdata &&
+	    pdata->compass.get_slave_descr &&
+	    pdata->compass.irq)
+		slaveirq_exit(&pdata->compass);
+
+	if (pdata &&
+	    pdata->accel.get_slave_descr &&
+	    pdata->accel.irq)
+		slaveirq_exit(&pdata->accel);
+
+	misc_deregister(&i2c_mpu_device);
+	kfree(mpu);
+
+	return 0;
+}
+
+static const struct i2c_device_id mpu3050_id[] = {
+	{MPU_NAME, 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, mpu3050_id);
+
+static struct i2c_driver mpu3050_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe = mpu3050_probe,
+	.remove = mpu3050_remove,
+	.id_table = mpu3050_id,
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = MPU_NAME,
+		   },
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 32)
+	.address_data = &addr_data,
+#else
+	.address_list = normal_i2c,
+#endif
+
+	.shutdown = mpu_shutdown,	/* optional */
+	.suspend = mpu_suspend,	/* optional */
+	.resume = mpu_resume,	/* optional */
+
+};
+
+static int __init mpu_init(void)
+{
+	int res = i2c_add_driver(&mpu3050_driver);
+	pr_debug("%s\n", __func__);
+	if (res)
+		pr_err("%s failed\n",
+			__func__);
+	return res;
+}
+
+static void __exit mpu_exit(void)
+{
+	pr_debug("%s\n", __func__);
+	i2c_del_driver(&mpu3050_driver);
+}
+
+module_init(mpu_init);
+module_exit(mpu_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("User space character device interface for MPU3050");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS(MPU_NAME);
diff --git a/drivers/misc/mpu3050/mpu-i2c.c b/drivers/misc/mpu3050/mpu-i2c.c
new file mode 100644
index 000000000000..b1298d313abf
--- /dev/null
+++ b/drivers/misc/mpu3050/mpu-i2c.c
@@ -0,0 +1,196 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+
+/**
+ *  @defgroup
+ *  @brief
+ *
+ *  @{
+ *      @file   mpu-i2c.c
+ *      @brief
+ *
+ */
+
+#include <linux/i2c.h>
+#include "mpu.h"
+
+int sensor_i2c_write(struct i2c_adapter *i2c_adap,
+		     unsigned char address,
+		     unsigned int len, unsigned char const *data)
+{
+	struct i2c_msg msgs[1];
+	int res;
+
+	if (NULL == data || NULL == i2c_adap)
+		return -EINVAL;
+
+	msgs[0].addr = address;
+	msgs[0].flags = 0;	/* write */
+	msgs[0].buf = (unsigned char *) data;
+	msgs[0].len = len;
+
+	res = i2c_transfer(i2c_adap, msgs, 1);
+	if (res < 1)
+		return res;
+	else
+		return 0;
+}
+
+int sensor_i2c_write_register(struct i2c_adapter *i2c_adap,
+			      unsigned char address,
+			      unsigned char reg, unsigned char value)
+{
+	unsigned char data[2];
+
+	data[0] = reg;
+	data[1] = value;
+	return sensor_i2c_write(i2c_adap, address, 2, data);
+}
+
+int sensor_i2c_read(struct i2c_adapter *i2c_adap,
+		    unsigned char address,
+		    unsigned char reg,
+		    unsigned int len, unsigned char *data)
+{
+	struct i2c_msg msgs[2];
+	int res;
+
+	if (NULL == data || NULL == i2c_adap)
+		return -EINVAL;
+
+	msgs[0].addr = address;
+	msgs[0].flags = 0;	/* write */
+	msgs[0].buf = &reg;
+	msgs[0].len = 1;
+
+	msgs[1].addr = address;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].buf = data;
+	msgs[1].len = len;
+
+	res = i2c_transfer(i2c_adap, msgs, 2);
+	if (res < 2)
+		return res;
+	else
+		return 0;
+}
+
+int mpu_memory_read(struct i2c_adapter *i2c_adap,
+		    unsigned char mpu_addr,
+		    unsigned short mem_addr,
+		    unsigned int len, unsigned char *data)
+{
+	unsigned char bank[2];
+	unsigned char addr[2];
+	unsigned char buf;
+
+	struct i2c_msg msgs[4];
+	int ret;
+
+	if (NULL == data || NULL == i2c_adap)
+		return -EINVAL;
+
+	bank[0] = MPUREG_BANK_SEL;
+	bank[1] = mem_addr >> 8;
+
+	addr[0] = MPUREG_MEM_START_ADDR;
+	addr[1] = mem_addr & 0xFF;
+
+	buf = MPUREG_MEM_R_W;
+
+	/* Write Message */
+	msgs[0].addr = mpu_addr;
+	msgs[0].flags = 0;
+	msgs[0].buf = bank;
+	msgs[0].len = sizeof(bank);
+
+	msgs[1].addr = mpu_addr;
+	msgs[1].flags = 0;
+	msgs[1].buf = addr;
+	msgs[1].len = sizeof(addr);
+
+	msgs[2].addr = mpu_addr;
+	msgs[2].flags = 0;
+	msgs[2].buf = &buf;
+	msgs[2].len = 1;
+
+	msgs[3].addr = mpu_addr;
+	msgs[3].flags = I2C_M_RD;
+	msgs[3].buf = data;
+	msgs[3].len = len;
+
+	ret = i2c_transfer(i2c_adap, msgs, 4);
+	if (ret != 4)
+		return ret;
+	else
+		return 0;
+}
+
+int mpu_memory_write(struct i2c_adapter *i2c_adap,
+		     unsigned char mpu_addr,
+		     unsigned short mem_addr,
+		     unsigned int len, unsigned char const *data)
+{
+	unsigned char bank[2];
+	unsigned char addr[2];
+	unsigned char buf[513];
+
+	struct i2c_msg msgs[3];
+	int ret;
+
+	if (NULL == data || NULL == i2c_adap)
+		return -EINVAL;
+	if (len >= (sizeof(buf) - 1))
+		return -ENOMEM;
+
+	bank[0] = MPUREG_BANK_SEL;
+	bank[1] = mem_addr >> 8;
+
+	addr[0] = MPUREG_MEM_START_ADDR;
+	addr[1] = mem_addr & 0xFF;
+
+	buf[0] = MPUREG_MEM_R_W;
+	memcpy(buf + 1, data, len);
+
+	/* Write Message */
+	msgs[0].addr = mpu_addr;
+	msgs[0].flags = 0;
+	msgs[0].buf = bank;
+	msgs[0].len = sizeof(bank);
+
+	msgs[1].addr = mpu_addr;
+	msgs[1].flags = 0;
+	msgs[1].buf = addr;
+	msgs[1].len = sizeof(addr);
+
+	msgs[2].addr = mpu_addr;
+	msgs[2].flags = 0;
+	msgs[2].buf = (unsigned char *) buf;
+	msgs[2].len = len + 1;
+
+	ret = i2c_transfer(i2c_adap, msgs, 3);
+	if (ret != 3)
+		return ret;
+	else
+		return 0;
+}
+
+/**
+ *  @}
+ */
diff --git a/drivers/misc/mpu3050/mpu-i2c.h b/drivers/misc/mpu3050/mpu-i2c.h
new file mode 100644
index 000000000000..0bbc8c64594e
--- /dev/null
+++ b/drivers/misc/mpu3050/mpu-i2c.h
@@ -0,0 +1,58 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+/**
+ * @defgroup
+ * @brief
+ *
+ * @{
+ * @file     mpu-i2c.c
+ * @brief
+ *
+ *
+ */
+
+#ifndef __MPU_I2C_H__
+#define __MPU_I2C_H__
+
+#include <linux/i2c.h>
+
+int sensor_i2c_write(struct i2c_adapter *i2c_adap,
+		     unsigned char address,
+		     unsigned int len, unsigned char const *data);
+
+int sensor_i2c_write_register(struct i2c_adapter *i2c_adap,
+			      unsigned char address,
+			      unsigned char reg, unsigned char value);
+
+int sensor_i2c_read(struct i2c_adapter *i2c_adap,
+		    unsigned char address,
+		    unsigned char reg,
+		    unsigned int len, unsigned char *data);
+
+int mpu_memory_read(struct i2c_adapter *i2c_adap,
+		    unsigned char mpu_addr,
+		    unsigned short mem_addr,
+		    unsigned int len, unsigned char *data);
+
+int mpu_memory_write(struct i2c_adapter *i2c_adap,
+		     unsigned char mpu_addr,
+		     unsigned short mem_addr,
+		     unsigned int len, unsigned char const *data);
+
+#endif	/* __MPU_I2C_H__ */
diff --git a/drivers/misc/mpu3050/mpuirq.c b/drivers/misc/mpu3050/mpuirq.c
new file mode 100644
index 000000000000..ce1ad409cbf4
--- /dev/null
+++ b/drivers/misc/mpu3050/mpuirq.c
@@ -0,0 +1,319 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/irq.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/workqueue.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+
+#include "mpu.h"
+#include "mpuirq.h"
+#include "mldl_cfg.h"
+#include "mpu-i2c.h"
+
+#define MPUIRQ_NAME "mpuirq"
+
+/* function which gets accel data and sends it to MPU */
+
+DECLARE_WAIT_QUEUE_HEAD(mpuirq_wait);
+
+struct mpuirq_dev_data {
+	struct work_struct work;
+	struct i2c_client *mpu_client;
+	struct miscdevice *dev;
+	int irq;
+	int pid;
+	int accel_divider;
+	int data_ready;
+	int timeout;
+};
+
+static struct mpuirq_dev_data mpuirq_dev_data;
+static struct mpuirq_data mpuirq_data;
+static char *interface = MPUIRQ_NAME;
+
+static void mpu_accel_data_work_fcn(struct work_struct *work);
+
+static int mpuirq_open(struct inode *inode, struct file *file)
+{
+	dev_dbg(mpuirq_dev_data.dev->this_device,
+		"%s current->pid %d\n", __func__, current->pid);
+	mpuirq_dev_data.pid = current->pid;
+	file->private_data = &mpuirq_dev_data;
+	return 0;
+}
+
+/* close function - called when the "file" /dev/mpuirq is closed in userspace */
+static int mpuirq_release(struct inode *inode, struct file *file)
+{
+	dev_dbg(mpuirq_dev_data.dev->this_device, "mpuirq_release\n");
+	return 0;
+}
+
+/* read function called when from /dev/mpuirq is read */
+static ssize_t mpuirq_read(struct file *file,
+			   char *buf, size_t count, loff_t *ppos)
+{
+	int len, err;
+	struct mpuirq_dev_data *p_mpuirq_dev_data = file->private_data;
+
+	if (!mpuirq_dev_data.data_ready &&
+		mpuirq_dev_data.timeout &&
+		(!(file->f_flags & O_NONBLOCK))) {
+		wait_event_interruptible_timeout(mpuirq_wait,
+						 mpuirq_dev_data.
+						 data_ready,
+						 mpuirq_dev_data.timeout);
+	}
+
+	if (mpuirq_dev_data.data_ready && NULL != buf
+	    && count >= sizeof(mpuirq_data)) {
+		err = copy_to_user(buf, &mpuirq_data, sizeof(mpuirq_data));
+		mpuirq_data.data_type = 0;
+	} else {
+		return 0;
+	}
+	if (err != 0) {
+		dev_err(p_mpuirq_dev_data->dev->this_device,
+			"Copy to user returned %d\n", err);
+		return -EFAULT;
+	}
+	mpuirq_dev_data.data_ready = 0;
+	len = sizeof(mpuirq_data);
+	return len;
+}
+
+unsigned int mpuirq_poll(struct file *file, struct poll_table_struct *poll)
+{
+	int mask = 0;
+
+	poll_wait(file, &mpuirq_wait, poll);
+	if (mpuirq_dev_data.data_ready)
+		mask |= POLLIN | POLLRDNORM;
+	return mask;
+}
+
+/* ioctl - I/O control */
+static long mpuirq_ioctl(struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	int data;
+
+	switch (cmd) {
+	case MPUIRQ_SET_TIMEOUT:
+		mpuirq_dev_data.timeout = arg;
+		break;
+
+	case MPUIRQ_GET_INTERRUPT_CNT:
+		data = mpuirq_data.interruptcount - 1;
+		if (mpuirq_data.interruptcount > 1)
+			mpuirq_data.interruptcount = 1;
+
+		if (copy_to_user((int *) arg, &data, sizeof(int)))
+			return -EFAULT;
+		break;
+	case MPUIRQ_GET_IRQ_TIME:
+		if (copy_to_user((int *) arg, &mpuirq_data.irqtime,
+				 sizeof(mpuirq_data.irqtime)))
+			return -EFAULT;
+		mpuirq_data.irqtime = 0;
+		break;
+	case MPUIRQ_SET_FREQUENCY_DIVIDER:
+		mpuirq_dev_data.accel_divider = arg;
+		break;
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+static void mpu_accel_data_work_fcn(struct work_struct *work)
+{
+	struct mpuirq_dev_data *mpuirq_dev_data =
+	    (struct mpuirq_dev_data *) work;
+	struct mldl_cfg *mldl_cfg =
+	    (struct mldl_cfg *)
+	    i2c_get_clientdata(mpuirq_dev_data->mpu_client);
+	struct i2c_adapter *accel_adapter;
+	unsigned char wbuff[16];
+	unsigned char rbuff[16];
+	int ii;
+
+	accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num);
+	mldl_cfg->accel->read(accel_adapter,
+			      mldl_cfg->accel,
+			      &mldl_cfg->pdata->accel, rbuff);
+
+
+	/* @todo add other data formats here as well */
+	if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->accel->endian) {
+		for (ii = 0; ii < 3; ii++) {
+			wbuff[2 * ii + 1] = rbuff[2 * ii + 1];
+			wbuff[2 * ii + 2] = rbuff[2 * ii + 0];
+		}
+	} else {
+		memcpy(wbuff + 1, rbuff, mldl_cfg->accel->len);
+	}
+
+	wbuff[7] = 0;
+	wbuff[8] = 1;		/*set semaphore */
+
+	mpu_memory_write(mpuirq_dev_data->mpu_client->adapter,
+			 mldl_cfg->addr, 0x0108, 8, wbuff);
+}
+
+static irqreturn_t mpuirq_handler(int irq, void *dev_id)
+{
+	static int mycount;
+	struct timeval irqtime;
+	mycount++;
+
+	mpuirq_data.interruptcount++;
+
+	/* wake up (unblock) for reading data from userspace */
+	/* and ignore first interrupt generated in module init */
+	mpuirq_dev_data.data_ready = 1;
+
+	do_gettimeofday(&irqtime);
+	mpuirq_data.irqtime = (((long long) irqtime.tv_sec) << 32);
+	mpuirq_data.irqtime += irqtime.tv_usec;
+
+	if ((mpuirq_dev_data.accel_divider >= 0) &&
+		(0 == (mycount % (mpuirq_dev_data.accel_divider + 1)))) {
+		schedule_work((struct work_struct
+				*) (&mpuirq_dev_data));
+	}
+
+	wake_up_interruptible(&mpuirq_wait);
+
+	return IRQ_HANDLED;
+
+}
+
+/* define which file operations are supported */
+const struct file_operations mpuirq_fops = {
+	.owner = THIS_MODULE,
+	.read = mpuirq_read,
+	.poll = mpuirq_poll,
+
+#if HAVE_COMPAT_IOCTL
+	.compat_ioctl = mpuirq_ioctl,
+#endif
+#if HAVE_UNLOCKED_IOCTL
+	.unlocked_ioctl = mpuirq_ioctl,
+#endif
+	.open = mpuirq_open,
+	.release = mpuirq_release,
+};
+
+static struct miscdevice mpuirq_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = MPUIRQ_NAME,
+	.fops = &mpuirq_fops,
+};
+
+int mpuirq_init(struct i2c_client *mpu_client)
+{
+
+	int res;
+	struct mldl_cfg *mldl_cfg =
+	    (struct mldl_cfg *) i2c_get_clientdata(mpu_client);
+
+	/* work_struct initialization */
+	INIT_WORK((struct work_struct *) &mpuirq_dev_data,
+		  mpu_accel_data_work_fcn);
+	mpuirq_dev_data.mpu_client = mpu_client;
+
+	dev_info(&mpu_client->adapter->dev,
+		 "Module Param interface = %s\n", interface);
+
+	mpuirq_dev_data.irq = mpu_client->irq;
+	mpuirq_dev_data.pid = 0;
+	mpuirq_dev_data.accel_divider = -1;
+	mpuirq_dev_data.data_ready = 0;
+	mpuirq_dev_data.timeout = 0;
+	mpuirq_dev_data.dev = &mpuirq_device;
+
+	if (mpuirq_dev_data.irq) {
+		unsigned long flags;
+		if (BIT_ACTL_LOW ==
+		    ((mldl_cfg->pdata->int_config) & BIT_ACTL))
+			flags = IRQF_TRIGGER_FALLING;
+		else
+			flags = IRQF_TRIGGER_RISING;
+
+		res =
+		    request_irq(mpuirq_dev_data.irq, mpuirq_handler, flags,
+				interface, &mpuirq_dev_data.irq);
+		if (res) {
+			dev_err(&mpu_client->adapter->dev,
+				"myirqtest: cannot register IRQ %d\n",
+				mpuirq_dev_data.irq);
+		} else {
+			res = misc_register(&mpuirq_device);
+			if (res < 0) {
+				dev_err(&mpu_client->adapter->dev,
+					"misc_register returned %d\n",
+					res);
+				free_irq(mpuirq_dev_data.irq,
+					 &mpuirq_dev_data.irq);
+			}
+		}
+
+	} else {
+		res = 0;
+	}
+
+	return res;
+}
+
+void mpuirq_exit(void)
+{
+	/* Free the IRQ first before flushing the work */
+	if (mpuirq_dev_data.irq > 0)
+		free_irq(mpuirq_dev_data.irq, &mpuirq_dev_data.irq);
+
+	flush_scheduled_work();
+
+	dev_info(mpuirq_device.this_device, "Unregistering %s\n",
+		 MPUIRQ_NAME);
+	misc_deregister(&mpuirq_device);
+
+	return;
+}
+
+module_param(interface, charp, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(interface, "The Interface name");
diff --git a/drivers/misc/mpu3050/mpuirq.h b/drivers/misc/mpu3050/mpuirq.h
new file mode 100644
index 000000000000..a71c79c75e8c
--- /dev/null
+++ b/drivers/misc/mpu3050/mpuirq.h
@@ -0,0 +1,42 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+
+#ifndef __MPUIRQ__
+#define __MPUIRQ__
+
+#ifdef __KERNEL__
+#include <linux/i2c-dev.h>
+#include <linux/time.h>
+#else
+#include <sys/time.h>
+#endif
+
+#define MPUIRQ_SET_TIMEOUT           _IOW(MPU_IOCTL, 0x40, unsigned long)
+#define MPUIRQ_GET_INTERRUPT_CNT     _IOR(MPU_IOCTL, 0x41, unsigned long)
+#define MPUIRQ_GET_IRQ_TIME          _IOR(MPU_IOCTL, 0x42, struct timeval)
+#define MPUIRQ_SET_FREQUENCY_DIVIDER _IOW(MPU_IOCTL, 0x43, unsigned long)
+
+#ifdef __KERNEL__
+
+void mpuirq_exit(void);
+int mpuirq_init(struct i2c_client *mpu_client);
+
+#endif
+
+#endif
diff --git a/drivers/misc/mpu3050/slaveirq.c b/drivers/misc/mpu3050/slaveirq.c
new file mode 100644
index 000000000000..a3c7bfec4b4b
--- /dev/null
+++ b/drivers/misc/mpu3050/slaveirq.c
@@ -0,0 +1,273 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/irq.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/wait.h>
+#include <linux/slab.h>
+
+#include "mpu.h"
+#include "slaveirq.h"
+#include "mldl_cfg.h"
+#include "mpu-i2c.h"
+
+/* function which gets slave data and sends it to SLAVE */
+
+struct slaveirq_dev_data {
+	struct miscdevice dev;
+	struct i2c_client *slave_client;
+	struct mpuirq_data data;
+	wait_queue_head_t slaveirq_wait;
+	int irq;
+	int pid;
+	int data_ready;
+	int timeout;
+};
+
+/* The following depends on patch fa1f68db6ca7ebb6fc4487ac215bffba06c01c28
+ * drivers: misc: pass miscdevice pointer via file private data
+ */
+static int slaveirq_open(struct inode *inode, struct file *file)
+{
+	/* Device node is availabe in the file->private_data, this is
+	 * exactly what we want so we leave it there */
+	struct slaveirq_dev_data *data =
+		container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+	dev_dbg(data->dev.this_device,
+		"%s current->pid %d\n", __func__, current->pid);
+	data->pid = current->pid;
+	return 0;
+}
+
+static int slaveirq_release(struct inode *inode, struct file *file)
+{
+	struct slaveirq_dev_data *data =
+		container_of(file->private_data, struct slaveirq_dev_data, dev);
+	dev_dbg(data->dev.this_device, "slaveirq_release\n");
+	return 0;
+}
+
+/* read function called when from /dev/slaveirq is read */
+static ssize_t slaveirq_read(struct file *file,
+			   char *buf, size_t count, loff_t *ppos)
+{
+	int len, err;
+	struct slaveirq_dev_data *data =
+		container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+	if (!data->data_ready &&
+		data->timeout &&
+		!(file->f_flags & O_NONBLOCK)) {
+		wait_event_interruptible_timeout(data->slaveirq_wait,
+						 data->data_ready,
+						 data->timeout);
+	}
+
+	if (data->data_ready && NULL != buf
+	    && count >= sizeof(data->data)) {
+		err = copy_to_user(buf, &data->data, sizeof(data->data));
+		data->data.data_type = 0;
+	} else {
+		return 0;
+	}
+	if (err != 0) {
+		dev_err(data->dev.this_device,
+			"Copy to user returned %d\n", err);
+		return -EFAULT;
+	}
+	data->data_ready = 0;
+	len = sizeof(data->data);
+	return len;
+}
+
+static unsigned int slaveirq_poll(struct file *file,
+				struct poll_table_struct *poll)
+{
+	int mask = 0;
+	struct slaveirq_dev_data *data =
+		container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+	poll_wait(file, &data->slaveirq_wait, poll);
+	if (data->data_ready)
+		mask |= POLLIN | POLLRDNORM;
+	return mask;
+}
+
+/* ioctl - I/O control */
+static long slaveirq_ioctl(struct file *file,
+			   unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	int tmp;
+	struct slaveirq_dev_data *data =
+		container_of(file->private_data, struct slaveirq_dev_data, dev);
+
+	switch (cmd) {
+	case SLAVEIRQ_SET_TIMEOUT:
+		data->timeout = arg;
+		break;
+
+	case SLAVEIRQ_GET_INTERRUPT_CNT:
+		tmp = data->data.interruptcount - 1;
+		if (data->data.interruptcount > 1)
+			data->data.interruptcount = 1;
+
+		if (copy_to_user((int *) arg, &tmp, sizeof(int)))
+			return -EFAULT;
+		break;
+	case SLAVEIRQ_GET_IRQ_TIME:
+		if (copy_to_user((int *) arg, &data->data.irqtime,
+				 sizeof(data->data.irqtime)))
+			return -EFAULT;
+		data->data.irqtime = 0;
+		break;
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+static irqreturn_t slaveirq_handler(int irq, void *dev_id)
+{
+	struct slaveirq_dev_data *data = (struct slaveirq_dev_data *)dev_id;
+	static int mycount;
+	struct timeval irqtime;
+	mycount++;
+
+	data->data.interruptcount++;
+
+	/* wake up (unblock) for reading data from userspace */
+	data->data_ready = 1;
+
+	do_gettimeofday(&irqtime);
+	data->data.irqtime = (((long long) irqtime.tv_sec) << 32);
+	data->data.irqtime += irqtime.tv_usec;
+	data->data.data_type |= 1;
+
+	wake_up_interruptible(&data->slaveirq_wait);
+
+	return IRQ_HANDLED;
+
+}
+
+/* define which file operations are supported */
+static const struct file_operations slaveirq_fops = {
+	.owner = THIS_MODULE,
+	.read = slaveirq_read,
+	.poll = slaveirq_poll,
+
+#if HAVE_COMPAT_IOCTL
+	.compat_ioctl = slaveirq_ioctl,
+#endif
+#if HAVE_UNLOCKED_IOCTL
+	.unlocked_ioctl = slaveirq_ioctl,
+#endif
+	.open = slaveirq_open,
+	.release = slaveirq_release,
+};
+
+int slaveirq_init(struct i2c_adapter *slave_adapter,
+		  struct ext_slave_platform_data *pdata,
+		  char *name)
+{
+
+	int res;
+	struct slaveirq_dev_data *data;
+
+	if (!pdata->irq)
+		return -EINVAL;
+
+	pdata->irq_data = kzalloc(sizeof(*data),
+				GFP_KERNEL);
+	data = (struct slaveirq_dev_data *) pdata->irq_data;
+	if (!data)
+		return -ENOMEM;
+
+	data->dev.minor = MISC_DYNAMIC_MINOR;
+	data->dev.name = name;
+	data->dev.fops = &slaveirq_fops;
+	data->irq = pdata->irq;
+	data->pid = 0;
+	data->data_ready = 0;
+	data->timeout = 0;
+
+	init_waitqueue_head(&data->slaveirq_wait);
+
+	res = request_irq(data->irq, slaveirq_handler, IRQF_TRIGGER_RISING,
+			  data->dev.name, data);
+
+	if (res) {
+		dev_err(&slave_adapter->dev,
+			"myirqtest: cannot register IRQ %d\n",
+			data->irq);
+		goto out_request_irq;
+	}
+
+	res = misc_register(&data->dev);
+	if (res < 0) {
+		dev_err(&slave_adapter->dev,
+			"misc_register returned %d\n",
+			res);
+		goto out_misc_register;
+	}
+
+	return res;
+
+out_misc_register:
+	free_irq(data->irq, data);
+out_request_irq:
+	kfree(pdata->irq_data);
+	pdata->irq_data = NULL;
+
+	return res;
+}
+
+void slaveirq_exit(struct ext_slave_platform_data *pdata)
+{
+	struct slaveirq_dev_data *data = pdata->irq_data;
+
+	if (!pdata->irq_data || data->irq <= 0)
+		return;
+
+	dev_info(data->dev.this_device, "Unregistering %s\n",
+		 data->dev.name);
+
+	free_irq(data->irq, data);
+	misc_deregister(&data->dev);
+	kfree(pdata->irq_data);
+	pdata->irq_data = NULL;
+}
diff --git a/drivers/misc/mpu3050/slaveirq.h b/drivers/misc/mpu3050/slaveirq.h
new file mode 100644
index 000000000000..b4e1115f1b0a
--- /dev/null
+++ b/drivers/misc/mpu3050/slaveirq.h
@@ -0,0 +1,43 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+
+#ifndef __SLAVEIRQ__
+#define __SLAVEIRQ__
+
+#ifdef __KERNEL__
+#include <linux/i2c-dev.h>
+#endif
+
+#include "mpu.h"
+#include "mpuirq.h"
+
+#define SLAVEIRQ_SET_TIMEOUT           _IOW(MPU_IOCTL, 0x50, unsigned long)
+#define SLAVEIRQ_GET_INTERRUPT_CNT     _IOR(MPU_IOCTL, 0x51, unsigned long)
+#define SLAVEIRQ_GET_IRQ_TIME          _IOR(MPU_IOCTL, 0x52, unsigned long)
+
+#ifdef __KERNEL__
+
+void slaveirq_exit(struct ext_slave_platform_data *pdata);
+int slaveirq_init(struct i2c_adapter *slave_adapter,
+		struct ext_slave_platform_data *pdata,
+		char *name);
+
+#endif
+
+#endif
diff --git a/drivers/misc/mpu3050/timerirq.c b/drivers/misc/mpu3050/timerirq.c
new file mode 100644
index 000000000000..41c3ac981016
--- /dev/null
+++ b/drivers/misc/mpu3050/timerirq.c
@@ -0,0 +1,299 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/stat.h>
+#include <linux/signal.h>
+#include <linux/miscdevice.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/poll.h>
+
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/timer.h>
+#include <linux/slab.h>
+
+#include "mpu.h"
+#include "mltypes.h"
+#include "timerirq.h"
+
+/* function which gets timer data and sends it to TIMER */
+struct timerirq_data {
+	int pid;
+	int data_ready;
+	int run;
+	int timeout;
+	unsigned long period;
+	struct mpuirq_data data;
+	struct completion timer_done;
+	wait_queue_head_t timerirq_wait;
+	struct timer_list timer;
+	struct miscdevice *dev;
+};
+
+static struct miscdevice *timerirq_dev_data;
+
+static void timerirq_handler(unsigned long arg)
+{
+	struct timerirq_data *data = (struct timerirq_data *)arg;
+	struct timeval irqtime;
+
+	data->data.interruptcount++;
+
+	data->data_ready = 1;
+
+	do_gettimeofday(&irqtime);
+	data->data.irqtime = (((long long) irqtime.tv_sec) << 32);
+	data->data.irqtime += irqtime.tv_usec;
+	data->data.data_type |= 1;
+
+	dev_dbg(data->dev->this_device,
+		"%s, %lld, %ld\n", __func__, data->data.irqtime,
+		(unsigned long)data);
+
+	wake_up_interruptible(&data->timerirq_wait);
+
+	if (data->run)
+		mod_timer(&data->timer,
+			jiffies + msecs_to_jiffies(data->period));
+	else
+		complete(&data->timer_done);
+}
+
+static int start_timerirq(struct timerirq_data *data)
+{
+	dev_dbg(data->dev->this_device,
+		"%s current->pid %d\n", __func__, current->pid);
+
+	/* Timer already running... success */
+	if (data->run)
+		return 0;
+
+	/* Don't allow a period of 0 since this would fire constantly */
+	if (!data->period)
+		return -EINVAL;
+
+	data->run = TRUE;
+	data->data_ready = FALSE;
+
+	init_completion(&data->timer_done);
+	setup_timer(&data->timer, timerirq_handler, (unsigned long)data);
+
+	return mod_timer(&data->timer,
+			jiffies + msecs_to_jiffies(data->period));
+}
+
+static int stop_timerirq(struct timerirq_data *data)
+{
+	dev_dbg(data->dev->this_device,
+		"%s current->pid %lx\n", __func__, (unsigned long)data);
+
+	if (data->run) {
+		data->run = FALSE;
+		mod_timer(&data->timer, jiffies + 1);
+		wait_for_completion(&data->timer_done);
+	}
+	return 0;
+}
+
+/* The following depends on patch fa1f68db6ca7ebb6fc4487ac215bffba06c01c28
+ * drivers: misc: pass miscdevice pointer via file private data
+ */
+static int timerirq_open(struct inode *inode, struct file *file)
+{
+	/* Device node is availabe in the file->private_data, this is
+	 * exactly what we want so we leave it there */
+	struct miscdevice *dev_data = file->private_data;
+	struct timerirq_data *data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->dev = dev_data;
+	file->private_data = data;
+	data->pid = current->pid;
+	init_waitqueue_head(&data->timerirq_wait);
+
+	dev_dbg(data->dev->this_device,
+		"%s current->pid %d\n", __func__, current->pid);
+	return 0;
+}
+
+static int timerirq_release(struct inode *inode, struct file *file)
+{
+	struct timerirq_data *data = file->private_data;
+	dev_dbg(data->dev->this_device, "timerirq_release\n");
+	if (data->run)
+		stop_timerirq(data);
+	kfree(data);
+	return 0;
+}
+
+/* read function called when from /dev/timerirq is read */
+static ssize_t timerirq_read(struct file *file,
+			   char *buf, size_t count, loff_t *ppos)
+{
+	int len, err;
+	struct timerirq_data *data = file->private_data;
+
+	if (!data->data_ready &&
+		data->timeout &&
+		!(file->f_flags & O_NONBLOCK)) {
+		wait_event_interruptible_timeout(data->timerirq_wait,
+						 data->data_ready,
+						 data->timeout);
+	}
+
+	if (data->data_ready && NULL != buf
+	    && count >= sizeof(data->data)) {
+		err = copy_to_user(buf, &data->data, sizeof(data->data));
+		data->data.data_type = 0;
+	} else {
+		return 0;
+	}
+	if (err != 0) {
+		dev_err(data->dev->this_device,
+			"Copy to user returned %d\n", err);
+		return -EFAULT;
+	}
+	data->data_ready = 0;
+	len = sizeof(data->data);
+	return len;
+}
+
+static unsigned int timerirq_poll(struct file *file,
+				struct poll_table_struct *poll)
+{
+	int mask = 0;
+	struct timerirq_data *data = file->private_data;
+
+	poll_wait(file, &data->timerirq_wait, poll);
+	if (data->data_ready)
+		mask |= POLLIN | POLLRDNORM;
+	return mask;
+}
+
+/* ioctl - I/O control */
+static long timerirq_ioctl(struct file *file,
+			   unsigned int cmd, unsigned long arg)
+{
+	int retval = 0;
+	int tmp;
+	struct timerirq_data *data = file->private_data;
+
+	dev_dbg(data->dev->this_device,
+		"%s current->pid %d, %d, %ld\n",
+		__func__, current->pid, cmd, arg);
+
+	if (!data)
+		return -EFAULT;
+
+	switch (cmd) {
+	case TIMERIRQ_SET_TIMEOUT:
+		data->timeout = arg;
+		break;
+	case TIMERIRQ_GET_INTERRUPT_CNT:
+		tmp = data->data.interruptcount - 1;
+		if (data->data.interruptcount > 1)
+			data->data.interruptcount = 1;
+
+		if (copy_to_user((int *) arg, &tmp, sizeof(int)))
+			return -EFAULT;
+		break;
+	case TIMERIRQ_START:
+		data->period = arg;
+		retval = start_timerirq(data);
+		break;
+	case TIMERIRQ_STOP:
+		retval = stop_timerirq(data);
+		break;
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+/* define which file operations are supported */
+static const struct file_operations timerirq_fops = {
+	.owner = THIS_MODULE,
+	.read = timerirq_read,
+	.poll = timerirq_poll,
+
+#if HAVE_COMPAT_IOCTL
+	.compat_ioctl = timerirq_ioctl,
+#endif
+#if HAVE_UNLOCKED_IOCTL
+	.unlocked_ioctl = timerirq_ioctl,
+#endif
+	.open = timerirq_open,
+	.release = timerirq_release,
+};
+
+static int __init timerirq_init(void)
+{
+
+	int res;
+	static struct miscdevice *data;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+	timerirq_dev_data = data;
+	data->minor = MISC_DYNAMIC_MINOR;
+	data->name = "timerirq";
+	data->fops = &timerirq_fops;
+
+	res = misc_register(data);
+	if (res < 0) {
+		dev_err(data->this_device,
+			"misc_register returned %d\n",
+			res);
+		return res;
+	}
+
+	return res;
+}
+module_init(timerirq_init);
+
+static void __exit timerirq_exit(void)
+{
+	struct miscdevice *data = timerirq_dev_data;
+
+	dev_info(data->this_device, "Unregistering %s\n",
+		 data->name);
+
+	misc_deregister(data);
+	kfree(data);
+
+	timerirq_dev_data = NULL;
+}
+module_exit(timerirq_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Timer IRQ device driver.");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("timerirq");
diff --git a/drivers/misc/mpu3050/timerirq.h b/drivers/misc/mpu3050/timerirq.h
new file mode 100644
index 000000000000..ec2c1e29f080
--- /dev/null
+++ b/drivers/misc/mpu3050/timerirq.h
@@ -0,0 +1,30 @@
+/*
+ $License:
+    Copyright (C) 2010 InvenSense 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, see <http://www.gnu.org/licenses/>.
+  $
+ */
+
+#ifndef __TIMERIRQ__
+#define __TIMERIRQ__
+
+#include "mpu.h"
+
+#define TIMERIRQ_SET_TIMEOUT           _IOW(MPU_IOCTL, 0x60, unsigned long)
+#define TIMERIRQ_GET_INTERRUPT_CNT     _IOW(MPU_IOCTL, 0x61, unsigned long)
+#define TIMERIRQ_START                 _IOW(MPU_IOCTL, 0x62, unsigned long)
+#define TIMERIRQ_STOP                  _IO(MPU_IOCTL, 0x63)
+
+#endif
diff --git a/drivers/misc/nct1008.c b/drivers/misc/nct1008.c
new file mode 100644
index 000000000000..a2714698d00d
--- /dev/null
+++ b/drivers/misc/nct1008.c
@@ -0,0 +1,963 @@
+/*
+ * drivers/misc/nct1008.c
+ *
+ * Driver for NCT1008, temperature monitoring device from ON Semiconductors
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/device.h>
+#include <linux/nct1008.h>
+#include <linux/delay.h>
+#include <linux/regulator/consumer.h>
+
+/* Register Addresses */
+#define LOCAL_TEMP_RD			0x00
+#define EXT_TEMP_RD_HI			0x01
+#define EXT_TEMP_RD_LO			0x10
+#define STATUS_RD			0x02
+#define CONFIG_RD			0x03
+
+#define LOCAL_TEMP_HI_LIMIT_RD		0x05
+#define LOCAL_TEMP_LO_LIMIT_RD		0x06
+
+#define EXT_TEMP_HI_LIMIT_HI_BYTE_RD	0x07
+#define EXT_TEMP_LO_LIMIT_HI_BYTE_RD	0x08
+
+#define CONFIG_WR			0x09
+#define CONV_RATE_WR			0x0A
+#define LOCAL_TEMP_HI_LIMIT_WR		0x0B
+#define LOCAL_TEMP_LO_LIMIT_WR		0x0C
+#define EXT_TEMP_HI_LIMIT_HI_BYTE_WR	0x0D
+#define EXT_TEMP_LO_LIMIT_HI_BYTE_WR	0x0E
+#define ONE_SHOT			0x0F
+#define OFFSET_WR			0x11
+#define OFFSET_QUARTER_WR		0x12
+#define EXT_THERM_LIMIT_WR		0x19
+#define LOCAL_THERM_LIMIT_WR		0x20
+#define THERM_HYSTERESIS_WR		0x21
+
+/* Configuration Register Bits */
+#define EXTENDED_RANGE_BIT		BIT(2)
+#define THERM2_BIT			BIT(5)
+#define STANDBY_BIT			BIT(6)
+#define ALERT_BIT			BIT(7)
+
+/* Max Temperature Measurements */
+#define EXTENDED_RANGE_OFFSET		64U
+#define STANDARD_RANGE_MAX		127U
+#define EXTENDED_RANGE_MAX		(150U + EXTENDED_RANGE_OFFSET)
+
+#define NCT1008_MIN_TEMP -64
+#define NCT1008_MAX_TEMP 191
+
+#define MAX_STR_PRINT 50
+
+#define MAX_CONV_TIME_ONESHOT_MS (52)
+#define CELSIUS_TO_MILLICELSIUS(x) ((x)*1000)
+#define MILLICELSIUS_TO_CELSIUS(x) ((x)/1000)
+
+
+static int conv_period_ms_table[] =
+	{16000, 8000, 4000, 2000, 1000, 500, 250, 125, 63, 32, 16};
+
+static inline s8 value_to_temperature(bool extended, u8 value)
+{
+	return extended ? (s8)(value - EXTENDED_RANGE_OFFSET) : (s8)value;
+}
+
+static inline u8 temperature_to_value(bool extended, s8 temp)
+{
+	return extended ? (u8)(temp + EXTENDED_RANGE_OFFSET) : (u8)temp;
+}
+
+static int nct1008_get_temp(struct device *dev, long *etemp, long *itemp)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct nct1008_platform_data *pdata = client->dev.platform_data;
+	s8 temp_local;
+	u8 temp_ext_lo;
+	s8 temp_ext_hi;
+	long temp_ext_milli;
+	long temp_local_milli;
+	u8 value;
+
+	/* Read Local Temp */
+	if (itemp) {
+		value = i2c_smbus_read_byte_data(client, LOCAL_TEMP_RD);
+		if (value < 0)
+			goto error;
+		temp_local = value_to_temperature(pdata->ext_range, value);
+		temp_local_milli = CELSIUS_TO_MILLICELSIUS(temp_local);
+
+		*itemp = temp_local_milli;
+	}
+
+	/* Read External Temp */
+	if (etemp) {
+		value = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_LO);
+		if (value < 0)
+			goto error;
+		temp_ext_lo = (value >> 6);
+
+		value = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_HI);
+		if (value < 0)
+			goto error;
+		temp_ext_hi = value_to_temperature(pdata->ext_range, value);
+
+		temp_ext_milli = CELSIUS_TO_MILLICELSIUS(temp_ext_hi) +
+					temp_ext_lo * 250;
+
+		*etemp = temp_ext_milli;
+	}
+
+	return 0;
+error:
+	dev_err(&client->dev, "\n error in file=: %s %s() line=%d: "
+		"error=%d ", __FILE__, __func__, __LINE__, value);
+	return value;
+}
+
+static ssize_t nct1008_show_temp(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct nct1008_platform_data *pdata = client->dev.platform_data;
+	s8 temp1 = 0;
+	s8 temp = 0;
+	u8 temp2 = 0;
+	int value = 0;
+
+	if (!dev || !buf || !attr)
+		return -EINVAL;
+
+	value = i2c_smbus_read_byte_data(client, LOCAL_TEMP_RD);
+	if (value < 0)
+		goto error;
+	temp1 = value_to_temperature(pdata->ext_range, value);
+
+	value = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_LO);
+	if (value < 0)
+		goto error;
+	temp2 = (value >> 6);
+	value = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_HI);
+	if (value < 0)
+		goto error;
+	temp = value_to_temperature(pdata->ext_range, value);
+
+	return snprintf(buf, MAX_STR_PRINT, "%d %d.%d\n",
+		temp1, temp, temp2 * 25);
+
+error:
+	return snprintf(buf, MAX_STR_PRINT,
+		"Error read local/ext temperature\n");
+}
+
+static ssize_t nct1008_show_temp_overheat(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct nct1008_platform_data *pdata = client->dev.platform_data;
+	int value;
+	s8 temp, temp2;
+
+	/* Local temperature h/w shutdown limit */
+	value = i2c_smbus_read_byte_data(client, LOCAL_THERM_LIMIT_WR);
+	if (value < 0)
+		goto error;
+	temp = value_to_temperature(pdata->ext_range, value);
+
+	/* External temperature h/w shutdown limit */
+	value = i2c_smbus_read_byte_data(client, EXT_THERM_LIMIT_WR);
+	if (value < 0)
+		goto error;
+	temp2 = value_to_temperature(pdata->ext_range, value);
+
+	return snprintf(buf, MAX_STR_PRINT, "%d %d\n", temp, temp2);
+error:
+	dev_err(dev, "%s: failed to read temperature-overheat "
+		"\n", __func__);
+	return snprintf(buf, MAX_STR_PRINT, " Rd overheat Error\n");
+}
+
+static ssize_t nct1008_set_temp_overheat(struct device *dev,
+			struct device_attribute *attr,
+			const char *buf, size_t count)
+{
+	long int num;
+	int err;
+	u8 temp;
+	long currTemp;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct nct1008_platform_data *pdata = client->dev.platform_data;
+	char bufTemp[MAX_STR_PRINT];
+	char bufOverheat[MAX_STR_PRINT];
+	unsigned int ret;
+
+	if (strict_strtol(buf, 0, &num)) {
+		dev_err(dev, "\n file: %s, line=%d return %s() ", __FILE__,
+			__LINE__, __func__);
+		return -EINVAL;
+	}
+	if (((int)num < NCT1008_MIN_TEMP) || ((int)num >= NCT1008_MAX_TEMP)) {
+		dev_err(dev, "\n file: %s, line=%d return %s() ", __FILE__,
+			__LINE__, __func__);
+		return -EINVAL;
+	}
+	/* check for system power down */
+	err = nct1008_get_temp(dev, &currTemp, NULL);
+	if (err)
+		goto error;
+
+	currTemp = MILLICELSIUS_TO_CELSIUS(currTemp);
+
+	if (currTemp >= (int)num) {
+		ret = nct1008_show_temp(dev, attr, bufTemp);
+		ret = nct1008_show_temp_overheat(dev, attr, bufOverheat);
+		dev_err(dev, "\nCurrent temp: %s ", bufTemp);
+		dev_err(dev, "\nOld overheat limit: %s ", bufOverheat);
+		dev_err(dev, "\nReset from overheat: curr temp=%ld, "
+			"new overheat temp=%d\n\n", currTemp, (int)num);
+	}
+
+	/* External temperature h/w shutdown limit */
+	temp = temperature_to_value(pdata->ext_range, (s8)num);
+	err = i2c_smbus_write_byte_data(client, EXT_THERM_LIMIT_WR, temp);
+	if (err < 0)
+		goto error;
+
+	/* Local temperature h/w shutdown limit */
+	temp = temperature_to_value(pdata->ext_range, (s8)num);
+	err = i2c_smbus_write_byte_data(client, LOCAL_THERM_LIMIT_WR, temp);
+	if (err < 0)
+		goto error;
+	return count;
+error:
+	dev_err(dev, " %s: failed to set temperature-overheat\n", __func__);
+	return err;
+}
+
+static ssize_t nct1008_show_temp_alert(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct nct1008_platform_data *pdata = client->dev.platform_data;
+	int value;
+	s8 temp_hi, temp_lo;
+	/* External Temperature Throttling hi-limit */
+	value = i2c_smbus_read_byte_data(client, EXT_TEMP_HI_LIMIT_HI_BYTE_RD);
+	if (value < 0)
+		goto error;
+	temp_hi = value_to_temperature(pdata->ext_range, value);
+
+	/* External Temperature Throttling lo-limit */
+	value = i2c_smbus_read_byte_data(client, EXT_TEMP_LO_LIMIT_HI_BYTE_RD);
+	if (value < 0)
+		goto error;
+	temp_lo = value_to_temperature(pdata->ext_range, value);
+
+	return snprintf(buf, MAX_STR_PRINT, "lo:%d hi:%d\n", temp_lo, temp_hi);
+error:
+	dev_err(dev, "%s: failed to read temperature-alert\n", __func__);
+	return snprintf(buf, MAX_STR_PRINT, " Rd alert Error\n");
+}
+
+static ssize_t nct1008_set_temp_alert(struct device *dev,
+			struct device_attribute *attr,
+			const char *buf, size_t count)
+{
+	long int num;
+	int value;
+	int err;
+	struct i2c_client *client = to_i2c_client(dev);
+	struct nct1008_platform_data *pdata = client->dev.platform_data;
+
+	if (strict_strtol(buf, 0, &num)) {
+		dev_err(dev, "\n file: %s, line=%d return %s() ", __FILE__,
+			__LINE__, __func__);
+		return -EINVAL;
+	}
+	if (((int)num < NCT1008_MIN_TEMP) || ((int)num >= NCT1008_MAX_TEMP)) {
+		dev_err(dev, "\n file: %s, line=%d return %s() ", __FILE__,
+			__LINE__, __func__);
+		return -EINVAL;
+	}
+
+	/* External Temperature Throttling limit */
+	value = temperature_to_value(pdata->ext_range, (s8)num);
+	err = i2c_smbus_write_byte_data(client, EXT_TEMP_HI_LIMIT_HI_BYTE_WR,
+		value);
+	if (err < 0)
+		goto error;
+
+	/* Local Temperature Throttling limit */
+	err = i2c_smbus_write_byte_data(client, LOCAL_TEMP_HI_LIMIT_WR,
+		value);
+	if (err < 0)
+		goto error;
+
+	return count;
+error:
+	dev_err(dev, "%s: failed to set temperature-alert "
+		"\n", __func__);
+	return err;
+}
+
+static ssize_t nct1008_show_ext_temp(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct nct1008_platform_data *pdata = client->dev.platform_data;
+	s8 temp_value;
+	int data = 0;
+	int data_lo;
+
+	if (!dev || !buf || !attr)
+		return -EINVAL;
+
+	/* When reading the full external temperature value, read the
+	 * LSB first. This causes the MSB to be locked (that is, the
+	 * ADC does not write to it) until it is read */
+	data_lo = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_LO);
+	if (data_lo < 0) {
+		dev_err(&client->dev, "%s: failed to read "
+			"ext_temperature, i2c error=%d\n", __func__, data_lo);
+		goto error;
+	}
+
+	data = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_HI);
+	if (data < 0) {
+		dev_err(&client->dev, "%s: failed to read "
+			"ext_temperature, i2c error=%d\n", __func__, data);
+		goto error;
+	}
+
+	temp_value = value_to_temperature(pdata->ext_range, data);
+
+	return snprintf(buf, MAX_STR_PRINT, "%d.%d\n", temp_value,
+		(25 * (data_lo >> 6)));
+error:
+	return snprintf(buf, MAX_STR_PRINT, "Error read ext temperature\n");
+}
+
+static DEVICE_ATTR(temperature, S_IRUGO, nct1008_show_temp, NULL);
+static DEVICE_ATTR(temperature_overheat, (S_IRUGO | (S_IWUSR | S_IWGRP)),
+		nct1008_show_temp_overheat, nct1008_set_temp_overheat);
+static DEVICE_ATTR(temperature_alert, (S_IRUGO | (S_IWUSR | S_IWGRP)),
+		nct1008_show_temp_alert, nct1008_set_temp_alert);
+static DEVICE_ATTR(ext_temperature, S_IRUGO, nct1008_show_ext_temp, NULL);
+
+static struct attribute *nct1008_attributes[] = {
+	&dev_attr_temperature.attr,
+	&dev_attr_temperature_overheat.attr,
+	&dev_attr_temperature_alert.attr,
+	&dev_attr_ext_temperature.attr,
+	NULL
+};
+
+static const struct attribute_group nct1008_attr_group = {
+	.attrs = nct1008_attributes,
+};
+
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+static void print_reg(const char *reg_name, struct seq_file *s,
+		int offset)
+{
+	struct nct1008_data *nct_data = s->private;
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(nct_data->client,
+		offset);
+	if (ret >= 0)
+		seq_printf(s, "Reg %s Addr = 0x%02x Reg 0x%02x "
+		"Value 0x%02x\n", reg_name,
+		nct_data->client->addr,
+			offset, ret);
+	else
+		seq_printf(s, "%s: line=%d, i2c read error=%d\n",
+		__func__, __LINE__, ret);
+}
+
+static int dbg_nct1008_show(struct seq_file *s, void *unused)
+{
+	seq_printf(s, "nct1008 nct72 Registers\n");
+	seq_printf(s, "------------------\n");
+	print_reg("Local Temp Value    ",     s, 0x00);
+	print_reg("Ext Temp Value Hi   ",     s, 0x01);
+	print_reg("Status              ",     s, 0x02);
+	print_reg("Configuration       ",     s, 0x03);
+	print_reg("Conversion Rate     ",     s, 0x04);
+	print_reg("Local Temp Hi Limit ",     s, 0x05);
+	print_reg("Local Temp Lo Limit ",     s, 0x06);
+	print_reg("Ext Temp Hi Limit Hi",     s, 0x07);
+	print_reg("Ext Temp Hi Limit Lo",     s, 0x13);
+	print_reg("Ext Temp Lo Limit Hi",     s, 0x08);
+	print_reg("Ext Temp Lo Limit Lo",     s, 0x14);
+	print_reg("Ext Temp Value Lo   ",     s, 0x10);
+	print_reg("Ext Temp Offset Hi  ",     s, 0x11);
+	print_reg("Ext Temp Offset Lo  ",     s, 0x12);
+	print_reg("Ext THERM Limit     ",     s, 0x19);
+	print_reg("Local THERM Limit   ",     s, 0x20);
+	print_reg("THERM Hysteresis    ",     s, 0x21);
+	print_reg("Consecutive ALERT   ",     s, 0x22);
+	return 0;
+}
+
+static int dbg_nct1008_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dbg_nct1008_show, inode->i_private);
+}
+
+static const struct file_operations debug_fops = {
+	.open		= dbg_nct1008_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int __init nct1008_debuginit(struct nct1008_data *nct)
+{
+	int err = 0;
+	struct dentry *d;
+	if (nct->chip == NCT72)
+		d = debugfs_create_file("nct72", S_IRUGO, NULL,
+				(void *)nct, &debug_fops);
+	else
+		d = debugfs_create_file("nct1008", S_IRUGO, NULL,
+				(void *)nct, &debug_fops);
+	if ((!d) || IS_ERR(d)) {
+		dev_err(&nct->client->dev, "Error: %s debugfs_create_file"
+			" returned an error\n", __func__);
+		err = -ENOENT;
+		goto end;
+	}
+	if (d == ERR_PTR(-ENODEV)) {
+		dev_err(&nct->client->dev, "Error: %s debugfs not supported "
+			"error=-ENODEV\n", __func__);
+		err = -ENODEV;
+	} else {
+		nct->dent = d;
+	}
+end:
+	return err;
+}
+#else
+static int __init nct1008_debuginit(struct nct1008_data *nct)
+{
+	return 0;
+}
+#endif
+
+static int nct1008_enable(struct i2c_client *client)
+{
+	struct nct1008_data *data = i2c_get_clientdata(client);
+	int err;
+
+	err = i2c_smbus_write_byte_data(client, CONFIG_WR,
+				  data->config & ~STANDBY_BIT);
+	if (err < 0)
+		dev_err(&client->dev, "%s, line=%d, i2c write error=%d\n",
+		__func__, __LINE__, err);
+	return err;
+}
+
+static int nct1008_disable(struct i2c_client *client)
+{
+	struct nct1008_data *data = i2c_get_clientdata(client);
+	int err;
+
+	err = i2c_smbus_write_byte_data(client, CONFIG_WR,
+				  data->config | STANDBY_BIT);
+	if (err < 0)
+		dev_err(&client->dev, "%s, line=%d, i2c write error=%d\n",
+		__func__, __LINE__, err);
+	return err;
+}
+
+static int nct1008_within_limits(struct nct1008_data *data)
+{
+	int intr_status;
+
+	intr_status = i2c_smbus_read_byte_data(data->client, STATUS_RD);
+
+	return !(intr_status & (BIT(3) | BIT(4)));
+}
+
+static void nct1008_work_func(struct work_struct *work)
+{
+	struct nct1008_data *data = container_of(work, struct nct1008_data,
+						work);
+	int intr_status;
+	struct timespec ts;
+
+	nct1008_disable(data->client);
+
+	if (data->alert_func)
+		if (!nct1008_within_limits(data))
+			data->alert_func(data->alert_data);
+
+	/* Initiate one-shot conversion */
+	i2c_smbus_write_byte_data(data->client, ONE_SHOT, 0x1);
+
+	/* Give hardware necessary time to finish conversion */
+	ts = ns_to_timespec(MAX_CONV_TIME_ONESHOT_MS * 1000 * 1000);
+	hrtimer_nanosleep(&ts, NULL, HRTIMER_MODE_REL, CLOCK_MONOTONIC);
+
+	intr_status = i2c_smbus_read_byte_data(data->client, STATUS_RD);
+
+	nct1008_enable(data->client);
+
+	enable_irq(data->client->irq);
+}
+
+static irqreturn_t nct1008_irq(int irq, void *dev_id)
+{
+	struct nct1008_data *data = dev_id;
+
+	disable_irq_nosync(irq);
+	queue_work(data->workqueue, &data->work);
+
+	return IRQ_HANDLED;
+}
+
+static void nct1008_power_control(struct nct1008_data *data, bool is_enable)
+{
+	int ret;
+	if (!data->nct_reg) {
+		data->nct_reg = regulator_get(&data->client->dev, "vdd");
+		if (IS_ERR_OR_NULL(data->nct_reg)) {
+			if (PTR_ERR(data->nct_reg) == -ENODEV)
+				dev_info(&data->client->dev,
+					"no regulator found for vdd."
+					" Assuming vdd is always powered");
+			else
+				dev_warn(&data->client->dev, "Error [%ld] in "
+					"getting the regulator handle for"
+					" vdd\n", PTR_ERR(data->nct_reg));
+			data->nct_reg = NULL;
+			return;
+		}
+	}
+	if (is_enable)
+		ret = regulator_enable(data->nct_reg);
+	else
+		ret = regulator_disable(data->nct_reg);
+
+	if (ret < 0)
+		dev_err(&data->client->dev, "Error in %s rail vdd_nct%s, "
+			"error %d\n", (is_enable) ? "enabling" : "disabling",
+			(data->chip == NCT72) ? "72" : "1008",
+			ret);
+	else
+		dev_info(&data->client->dev, "success in %s rail vdd_nct%s\n",
+			(is_enable) ? "enabling" : "disabling",
+			(data->chip == NCT72) ? "72" : "1008");
+}
+
+static int __devinit nct1008_configure_sensor(struct nct1008_data* data)
+{
+	struct i2c_client *client = data->client;
+	struct nct1008_platform_data *pdata = client->dev.platform_data;
+	u8 value;
+	s8 temp;
+	u8 temp2;
+	int err;
+
+	if (!pdata || !pdata->supported_hwrev)
+		return -ENODEV;
+
+	/* Place in Standby */
+	data->config = STANDBY_BIT;
+	err = i2c_smbus_write_byte_data(client, CONFIG_WR, data->config);
+	if (err)
+		goto error;
+
+	/* External temperature h/w shutdown limit */
+	value = temperature_to_value(pdata->ext_range, NCT1008_MAX_TEMP);
+	err = i2c_smbus_write_byte_data(client, EXT_THERM_LIMIT_WR, value);
+	if (err)
+		goto error;
+
+	/* Local temperature h/w shutdown limit */
+	value = temperature_to_value(pdata->ext_range, NCT1008_MAX_TEMP);
+	err = i2c_smbus_write_byte_data(client, LOCAL_THERM_LIMIT_WR, value);
+	if (err)
+		goto error;
+
+	/* set extended range mode if needed */
+	if (pdata->ext_range)
+		data->config |= EXTENDED_RANGE_BIT;
+	data->config &= ~(THERM2_BIT | ALERT_BIT);
+
+	err = i2c_smbus_write_byte_data(client, CONFIG_WR, data->config);
+	if (err)
+		goto error;
+
+	/* Temperature conversion rate */
+	err = i2c_smbus_write_byte_data(client, CONV_RATE_WR, pdata->conv_rate);
+	if (err)
+		goto error;
+
+	data->conv_period_ms = conv_period_ms_table[pdata->conv_rate];
+
+	/* Setup local hi and lo limits */
+	err = i2c_smbus_write_byte_data(client,
+		LOCAL_TEMP_HI_LIMIT_WR, NCT1008_MAX_TEMP);
+	if (err)
+		goto error;
+
+	err = i2c_smbus_write_byte_data(client,
+		LOCAL_TEMP_LO_LIMIT_WR, 0);
+	if (err)
+		goto error;
+
+	/* Setup external hi and lo limits */
+	err = i2c_smbus_write_byte_data(client,
+		EXT_TEMP_LO_LIMIT_HI_BYTE_WR, 0);
+	if (err)
+		goto error;
+	err = i2c_smbus_write_byte_data(client, EXT_TEMP_HI_LIMIT_HI_BYTE_WR,
+			NCT1008_MAX_TEMP);
+	if (err)
+		goto error;
+
+	/* read initial temperature */
+	value = i2c_smbus_read_byte_data(client, LOCAL_TEMP_RD);
+	if (value < 0) {
+		err = value;
+		goto error;
+	}
+	temp = value_to_temperature(pdata->ext_range, value);
+	dev_dbg(&client->dev, "\n initial local temp = %d ", temp);
+
+	value = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_LO);
+	if (value < 0) {
+		err = value;
+		goto error;
+	}
+	temp2 = (value >> 6);
+	value = i2c_smbus_read_byte_data(client, EXT_TEMP_RD_HI);
+	if (value < 0) {
+		err = value;
+		goto error;
+	}
+	temp = value_to_temperature(pdata->ext_range, value);
+
+	if (temp2 > 0)
+		dev_dbg(&client->dev, "\n initial ext temp = %d.%d deg",
+				temp, temp2 * 25);
+	else
+		dev_dbg(&client->dev, "\n initial ext temp = %d.0 deg", temp);
+
+	/* Remote channel offset */
+	err = i2c_smbus_write_byte_data(client, OFFSET_WR, pdata->offset / 4);
+	if (err < 0)
+		goto error;
+
+	/* Remote channel offset fraction (quarters) */
+	err = i2c_smbus_write_byte_data(client, OFFSET_QUARTER_WR,
+					(pdata->offset % 4) << 6);
+	if (err < 0)
+		goto error;
+
+	/* register sysfs hooks */
+	err = sysfs_create_group(&client->dev.kobj, &nct1008_attr_group);
+	if (err < 0) {
+		dev_err(&client->dev, "\n sysfs create err=%d ", err);
+		goto error;
+	}
+
+	return 0;
+error:
+	dev_err(&client->dev, "\n exit %s, err=%d ", __func__, err);
+	return err;
+}
+
+static int __devinit nct1008_configure_irq(struct nct1008_data *data)
+{
+	data->workqueue = create_singlethread_workqueue((data->chip == NCT72) \
+							? "nct72" : "nct1008");
+
+	INIT_WORK(&data->work, nct1008_work_func);
+
+	if (data->client->irq < 0)
+		return 0;
+	else
+		return request_irq(data->client->irq, nct1008_irq,
+			IRQF_TRIGGER_LOW,
+			(data->chip == NCT72) ? "nct72" : "nct1008",
+			data);
+}
+
+int nct1008_thermal_get_temp(struct nct1008_data *data, long *temp)
+{
+	return nct1008_get_temp(&data->client->dev, temp, NULL);
+}
+
+int nct1008_thermal_get_temps(struct nct1008_data *data, long *etemp, long *itemp)
+{
+	return nct1008_get_temp(&data->client->dev, etemp, itemp);
+}
+
+int nct1008_thermal_get_temp_low(struct nct1008_data *data, long *temp)
+{
+	*temp = 0;
+	return 0;
+}
+
+int nct1008_thermal_set_limits(struct nct1008_data *data,
+				long lo_limit_milli,
+				long hi_limit_milli)
+{
+	int err;
+	u8 value;
+	bool extended_range = data->plat_data.ext_range;
+	long lo_limit = MILLICELSIUS_TO_CELSIUS(lo_limit_milli);
+	long hi_limit = MILLICELSIUS_TO_CELSIUS(hi_limit_milli);
+
+	if (lo_limit >= hi_limit)
+		return -EINVAL;
+
+	if (data->current_lo_limit != lo_limit) {
+		value = temperature_to_value(extended_range, lo_limit);
+		pr_debug("%s: set lo_limit %ld\n", __func__, lo_limit);
+		err = i2c_smbus_write_byte_data(data->client,
+				EXT_TEMP_LO_LIMIT_HI_BYTE_WR, value);
+		if (err)
+			return err;
+
+		data->current_lo_limit = lo_limit;
+	}
+
+	if (data->current_hi_limit != hi_limit) {
+		value = temperature_to_value(extended_range, hi_limit);
+		pr_debug("%s: set hi_limit %ld\n", __func__, hi_limit);
+		err = i2c_smbus_write_byte_data(data->client,
+				EXT_TEMP_HI_LIMIT_HI_BYTE_WR, value);
+		if (err)
+			return err;
+
+		data->current_hi_limit = hi_limit;
+	}
+
+	return 0;
+}
+
+int nct1008_thermal_set_alert(struct nct1008_data *data,
+				void (*alert_func)(void *),
+				void *alert_data)
+{
+	data->alert_data = alert_data;
+	data->alert_func = alert_func;
+
+	return 0;
+}
+
+int nct1008_thermal_set_shutdown_temp(struct nct1008_data *data,
+					long shutdown_temp_milli)
+{
+	struct i2c_client *client = data->client;
+	struct nct1008_platform_data *pdata = client->dev.platform_data;
+	int err;
+	u8 value;
+	long shutdown_temp;
+
+	shutdown_temp = MILLICELSIUS_TO_CELSIUS(shutdown_temp_milli);
+
+	/* External temperature h/w shutdown limit */
+	value = temperature_to_value(pdata->ext_range, shutdown_temp);
+	err = i2c_smbus_write_byte_data(client, EXT_THERM_LIMIT_WR, value);
+	if (err)
+		return err;
+
+	/* Local temperature h/w shutdown limit */
+	value = temperature_to_value(pdata->ext_range, shutdown_temp);
+	err = i2c_smbus_write_byte_data(client, LOCAL_THERM_LIMIT_WR, value);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+/*
+ * Manufacturer(OnSemi) recommended sequence for
+ * Extended Range mode is as follows
+ * 1. Place in Standby
+ * 2. Scale the THERM and ALERT limits
+ *	appropriately(for Extended Range mode).
+ * 3. Enable Extended Range mode.
+ *	ALERT mask/THERM2 mode may be done here
+ *	as these are not critical
+ * 4. Set Conversion Rate as required
+ * 5. Take device out of Standby
+ */
+
+/*
+ * function nct1008_probe takes care of initial configuration
+ */
+static int __devinit nct1008_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
+{
+	struct nct1008_data *data;
+	int err;
+
+	data = kzalloc(sizeof(struct nct1008_data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->client = client;
+	data->chip = id->driver_data;
+	memcpy(&data->plat_data, client->dev.platform_data,
+		sizeof(struct nct1008_platform_data));
+	i2c_set_clientdata(client, data);
+
+	nct1008_power_control(data, true);
+	/* extended range recommended steps 1 through 4 taken care
+	 * in nct1008_configure_sensor function */
+	err = nct1008_configure_sensor(data);	/* sensor is in standby */
+	if (err < 0) {
+		dev_err(&client->dev, "\n error file: %s : %s(), line=%d ",
+			__FILE__, __func__, __LINE__);
+		goto error;
+	}
+
+	err = nct1008_configure_irq(data);
+	if (err < 0) {
+		dev_err(&client->dev, "\n error file: %s : %s(), line=%d ",
+			__FILE__, __func__, __LINE__);
+		goto error;
+	}
+	dev_info(&client->dev, "%s: initialized\n", __func__);
+
+	/* extended range recommended step 5 is in nct1008_enable function */
+	err = nct1008_enable(client);		/* sensor is running */
+	if (err < 0) {
+		dev_err(&client->dev, "Error: %s, line=%d, error=%d\n",
+			__func__, __LINE__, err);
+		goto error;
+	}
+
+	err = nct1008_debuginit(data);
+	if (err < 0)
+		err = 0; /* without debugfs we may continue */
+
+	/* notify callback that probe is done */
+	if (data->plat_data.probe_callback)
+		data->plat_data.probe_callback(data);
+
+	return 0;
+
+error:
+	dev_err(&client->dev, "\n exit %s, err=%d ", __func__, err);
+	nct1008_power_control(data, false);
+	if (data->nct_reg)
+		regulator_put(data->nct_reg);
+	kfree(data);
+	return err;
+}
+
+static int __devexit nct1008_remove(struct i2c_client *client)
+{
+	struct nct1008_data *data = i2c_get_clientdata(client);
+
+	if (data->dent)
+		debugfs_remove(data->dent);
+
+	free_irq(data->client->irq, data);
+	cancel_work_sync(&data->work);
+	sysfs_remove_group(&client->dev.kobj, &nct1008_attr_group);
+	nct1008_power_control(data, false);
+	if (data->nct_reg)
+		regulator_put(data->nct_reg);
+
+	kfree(data);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int nct1008_suspend(struct i2c_client *client, pm_message_t state)
+{
+	int err;
+
+	disable_irq(client->irq);
+	err = nct1008_disable(client);
+	return err;
+}
+
+static int nct1008_resume(struct i2c_client *client)
+{
+	int err;
+
+	err = nct1008_enable(client);
+	if (err < 0) {
+		dev_err(&client->dev, "Error: %s, error=%d\n",
+			__func__, err);
+		return err;
+	}
+	enable_irq(client->irq);
+
+	return 0;
+}
+#endif
+
+static const struct i2c_device_id nct1008_id[] = {
+	{ "nct1008", NCT1008 },
+	{ "nct72", NCT72},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, nct1008_id);
+
+static struct i2c_driver nct1008_driver = {
+	.driver = {
+		.name	= "nct1008_nct72",
+	},
+	.probe		= nct1008_probe,
+	.remove		= __devexit_p(nct1008_remove),
+	.id_table	= nct1008_id,
+#ifdef CONFIG_PM
+	.suspend	= nct1008_suspend,
+	.resume		= nct1008_resume,
+#endif
+};
+
+static int __init nct1008_init(void)
+{
+	return i2c_add_driver(&nct1008_driver);
+}
+
+static void __exit nct1008_exit(void)
+{
+	i2c_del_driver(&nct1008_driver);
+}
+
+MODULE_DESCRIPTION("Temperature sensor driver for OnSemi NCT1008/NCT72");
+MODULE_LICENSE("GPL");
+
+module_init(nct1008_init);
+module_exit(nct1008_exit);
diff --git a/drivers/misc/pch_phub.c b/drivers/misc/pch_phub.c
index 0fd7e77bee29..ba276ba5a824 100644
--- a/drivers/misc/pch_phub.c
+++ b/drivers/misc/pch_phub.c
@@ -73,6 +73,9 @@
 #define PCI_DEVICE_ID_ROHM_ML7223_mPHUB	0x8012 /* for Bus-m */
 #define PCI_DEVICE_ID_ROHM_ML7223_nPHUB	0x8002 /* for Bus-n */
 
+/* Macros for ML7831 */
+#define PCI_DEVICE_ID_ROHM_ML7831_PHUB 0x8801
+
 /* SROM ACCESS Macro */
 #define PCH_WORD_ADDR_MASK (~((1 << 2) - 1))
 
@@ -464,7 +467,7 @@ static int pch_phub_write_gbe_mac_addr(struct pch_phub_reg *chip, u8 *data)
 	int retval;
 	int i;
 
-	if (chip->ioh_type == 1) /* EG20T */
+	if ((chip->ioh_type == 1) || (chip->ioh_type == 5)) /* EG20T or ML7831*/
 		retval = pch_phub_gbe_serial_rom_conf(chip);
 	else	/* ML7223 */
 		retval = pch_phub_gbe_serial_rom_conf_mp(chip);
@@ -757,6 +760,22 @@ static int __devinit pch_phub_probe(struct pci_dev *pdev,
 		chip->pch_opt_rom_start_address =\
 						 PCH_PHUB_ROM_START_ADDR_ML7223;
 		chip->pch_mac_start_address = PCH_PHUB_MAC_START_ADDR_ML7223;
+	} else if (id->driver_data == 5) { /* ML7831 */
+		retval = sysfs_create_file(&pdev->dev.kobj,
+					   &dev_attr_pch_mac.attr);
+		if (retval)
+			goto err_sysfs_create;
+
+		retval = sysfs_create_bin_file(&pdev->dev.kobj, &pch_bin_attr);
+		if (retval)
+			goto exit_bin_attr;
+
+		/* set the prefech value */
+		iowrite32(0x000affaa, chip->pch_phub_base_address + 0x14);
+		/* set the interrupt delay value */
+		iowrite32(0x25, chip->pch_phub_base_address + 0x44);
+		chip->pch_opt_rom_start_address = PCH_PHUB_ROM_START_ADDR_EG20T;
+		chip->pch_mac_start_address = PCH_PHUB_MAC_START_ADDR_EG20T;
 	}
 
 	chip->ioh_type = id->driver_data;
@@ -841,6 +860,7 @@ static struct pci_device_id pch_phub_pcidev_id[] = {
 	{ PCI_VDEVICE(ROHM, PCI_DEVICE_ID_ROHM_ML7213_PHUB), 2,  },
 	{ PCI_VDEVICE(ROHM, PCI_DEVICE_ID_ROHM_ML7223_mPHUB), 3,  },
 	{ PCI_VDEVICE(ROHM, PCI_DEVICE_ID_ROHM_ML7223_nPHUB), 4,  },
+	{ PCI_VDEVICE(ROHM, PCI_DEVICE_ID_ROHM_ML7831_PHUB), 5,  },
 	{ }
 };
 MODULE_DEVICE_TABLE(pci, pch_phub_pcidev_id);
diff --git a/drivers/misc/spear13xx_pcie_gadget.c b/drivers/misc/spear13xx_pcie_gadget.c
index cfbddbef11de..43d073bc1d9c 100644
--- a/drivers/misc/spear13xx_pcie_gadget.c
+++ b/drivers/misc/spear13xx_pcie_gadget.c
@@ -903,6 +903,6 @@ static void __exit spear_pcie_gadget_exit(void)
 }
 module_exit(spear_pcie_gadget_exit);
 
-MODULE_ALIAS("pcie-gadget-spear");
+MODULE_ALIAS("platform:pcie-gadget-spear");
 MODULE_AUTHOR("Pratyush Anand");
 MODULE_LICENSE("GPL");
diff --git a/drivers/misc/tegra-baseband/Kconfig b/drivers/misc/tegra-baseband/Kconfig
new file mode 100644
index 000000000000..1f116918296f
--- /dev/null
+++ b/drivers/misc/tegra-baseband/Kconfig
@@ -0,0 +1,32 @@
+menuconfig TEGRA_BB_SUPPORT
+	bool "Tegra baseband support"
+	depends on ARCH_TEGRA
+	---help---
+	  Say Y here to get to see options for tegra baseband support.
+	  This option alone does not add any kernel code.
+
+	  If you say N, all options in this submenu will be skipped and disabled.
+
+if TEGRA_BB_SUPPORT
+
+config TEGRA_BB_POWER
+	bool "Enable tegra baseband power driver"
+	---help---
+	 Adds power management driver for managing different baseband
+	 modems with tegra processor.
+
+	 This driver should work with at least the following devices:
+
+            * STE M7400
+            * ...
+
+	 Disabled by default. Choose Y here if you want to build the driver.
+
+config TEGRA_BB_M7400
+	bool "Enable driver for M7400 modem"
+	---help---
+	 Enables driver for M7400 modem.
+
+	 Disabled by default. Choose Y here if you want to build the driver.
+
+endif # TEGRA_BB_SUPPORT
diff --git a/drivers/misc/tegra-baseband/Makefile b/drivers/misc/tegra-baseband/Makefile
new file mode 100644
index 000000000000..68ec4fe83ac1
--- /dev/null
+++ b/drivers/misc/tegra-baseband/Makefile
@@ -0,0 +1,8 @@
+#
+# Makefile for tegra baseband support.
+#
+
+subdir-ccflags-y := -Werror
+
+obj-$(CONFIG_TEGRA_BB_POWER)	+= bb-power.o
+obj-$(CONFIG_TEGRA_BB_M7400)	+= bb-m7400.o
diff --git a/drivers/misc/tegra-baseband/bb-m7400.c b/drivers/misc/tegra-baseband/bb-m7400.c
new file mode 100644
index 000000000000..edde7d1786ed
--- /dev/null
+++ b/drivers/misc/tegra-baseband/bb-m7400.c
@@ -0,0 +1,337 @@
+/*
+ * drivers/misc/tegra-baseband/bb-m7400.c
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/resource.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/err.h>
+#include <linux/device.h>
+#include <linux/usb.h>
+#include <linux/wakelock.h>
+#include <linux/platform_data/tegra_usb.h>
+#include <asm/mach-types.h>
+#include <asm/mach/arch.h>
+#include <mach/tegra-bb-power.h>
+#include <mach/usb_phy.h>
+
+#include "bb-power.h"
+
+static struct tegra_bb_gpio_data m7400_gpios[] = {
+	{ { GPIO_INVALID, GPIOF_OUT_INIT_LOW, "MDM_PWR_ON" }, true },
+	{ { GPIO_INVALID, GPIOF_IN, "MDM_PWRSTATUS" }, true },
+	{ { GPIO_INVALID, GPIOF_OUT_INIT_HIGH, "MDM_SERVICE" }, true },
+	{ { GPIO_INVALID, GPIOF_OUT_INIT_LOW, "MDM_USB_AWR" }, false },
+	{ { GPIO_INVALID, GPIOF_IN, "MDM_USB_CWR" }, false },
+	{ { GPIO_INVALID, GPIOF_IN, "MDM_RESOUT2" }, true },
+	{ { GPIO_INVALID, GPIOF_OUT_INIT_LOW, "MDM_USB_ARR" }, false },
+	{ { GPIO_INVALID, 0, NULL }, false },	/* End of table */
+};
+static bool ehci_registered;
+static int modem_status;
+static int gpio_awr;
+static int gpio_cwr;
+static int gpio_arr;
+static struct usb_device *m7400_usb_device;
+
+static int gpio_wait_timeout(int gpio, int value, int timeout_msec)
+{
+	int count;
+	for (count = 0; count < timeout_msec; ++count) {
+		if (gpio_get_value(gpio) == value)
+			return 0;
+		mdelay(1);
+	}
+	return -1;
+}
+
+static int m7400_enum_handshake(void)
+{
+	int retval = 0;
+
+	/* Wait for CP to indicate ready - by driving CWR high. */
+	if (gpio_wait_timeout(gpio_cwr, 1, 10) != 0) {
+			pr_info("%s: Error: timeout waiting for modem resume.\n",
+							__func__);
+			retval = -1;
+	}
+
+	/* Signal AP ready - Drive AWR and ARR high. */
+	gpio_set_value(gpio_awr, 1);
+	gpio_set_value(gpio_arr, 1);
+
+	return retval;
+}
+
+static int m7400_apup_handshake(bool checkresponse)
+{
+	int retval = 0;
+
+	/* Signal AP ready - Drive AWR and ARR high. */
+	gpio_set_value(gpio_awr, 1);
+	gpio_set_value(gpio_arr, 1);
+
+	if (checkresponse) {
+		/* Wait for CP ack - by driving CWR high. */
+		if (gpio_wait_timeout(gpio_cwr, 1, 10) != 0) {
+			pr_info("%s: Error: timeout waiting for modem ack.\n",
+							__func__);
+			retval = -1;
+		}
+	}
+	return retval;
+}
+
+static void m7400_apdown_handshake(void)
+{
+	/* Signal AP going down to modem - Drive AWR low. */
+	/* No need to wait for a CP response */
+	gpio_set_value(gpio_awr, 0);
+}
+
+static void m7400_l2_suspend(void)
+{
+	/* Gets called for two cases :
+		a) Port suspend.
+		b) Bus suspend. */
+	if (modem_status == BBSTATE_L2)
+		return;
+
+	/* Post bus suspend: Drive ARR low. */
+	gpio_set_value(gpio_arr, 0);
+	modem_status = BBSTATE_L2;
+}
+
+static void m7400_l2_resume(void)
+{
+	/* Gets called for two cases :
+		a) L2 resume.
+		b) bus resume phase of L3 resume. */
+	if (modem_status == BBSTATE_L0)
+		return;
+
+	/* Pre bus resume: Drive ARR high. */
+	gpio_set_value(gpio_arr, 1);
+
+	/* If host initiated resume - Wait for CP ack (CWR goes high). */
+	/* If device initiated resume - CWR will be already high. */
+	if (gpio_wait_timeout(gpio_cwr, 1, 10) != 0) {
+		pr_info("%s: Error: timeout waiting for modem ack.\n",
+						__func__);
+		return;
+	}
+	modem_status = BBSTATE_L0;
+}
+
+static void m7400_l3_suspend(void)
+{
+	m7400_apdown_handshake();
+	modem_status = BBSTATE_L3;
+}
+
+static void m7400_l3_resume(void)
+{
+	m7400_apup_handshake(true);
+	modem_status = BBSTATE_L0;
+}
+
+static irqreturn_t m7400_wake_irq(int irq, void *dev_id)
+{
+	struct usb_interface *intf;
+
+	switch (modem_status) {
+	case BBSTATE_L2:
+		/* Resume usb host activity. */
+		if (m7400_usb_device) {
+			usb_lock_device(m7400_usb_device);
+			intf = usb_ifnum_to_if(m7400_usb_device, 0);
+			usb_autopm_get_interface(intf);
+			usb_autopm_put_interface(intf);
+			usb_unlock_device(m7400_usb_device);
+		}
+		break;
+	default:
+		break;
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int m7400_power(int code)
+{
+	switch (code) {
+	case PWRSTATE_L2L3:
+	m7400_l3_suspend();
+	break;
+	case PWRSTATE_L3L0:
+	m7400_l3_resume();
+	break;
+	default:
+	break;
+	}
+	return 0;
+}
+
+static void m7400_ehci_customize(struct platform_device *pdev)
+{
+	struct tegra_usb_platform_data *ehci_pdata;
+
+	ehci_pdata = (struct tegra_usb_platform_data *)
+			pdev->dev.platform_data;
+
+	/* Register PHY callbacks */
+	ehci_pdata->ops->post_suspend = m7400_l2_suspend;
+	ehci_pdata->ops->pre_resume = m7400_l2_resume;
+
+	/* Override required settings */
+	ehci_pdata->u_data.host.power_off_on_suspend = false;
+}
+
+static int m7400_attrib_write(struct device *dev, int value)
+{
+	struct tegra_bb_pdata *pdata;
+	static struct platform_device *ehci_device;
+	static bool first_enum = true;
+
+	if (value > 1 || (!ehci_registered && !value)) {
+		/* Supported values are 0/1. */
+		return -1;
+	}
+
+	pdata = (struct tegra_bb_pdata *) dev->platform_data;
+	if (value) {
+
+		/* Check readiness for enumeration */
+		if (first_enum)
+			first_enum = false;
+		else
+			m7400_enum_handshake();
+
+		/* Register ehci controller */
+		ehci_device = pdata->ehci_register();
+		if (ehci_device == NULL) {
+			pr_info("%s - Error: ehci register failed.\n",
+							 __func__);
+			return -1;
+		}
+
+		/* Customize PHY setup/callbacks */
+		m7400_ehci_customize(ehci_device);
+
+		ehci_registered = true;
+	} else {
+		/* Unregister ehci controller */
+		if (ehci_device != NULL)
+			pdata->ehci_unregister(&ehci_device);
+
+		/* Signal AP going down */
+		m7400_apdown_handshake();
+		ehci_registered = false;
+	}
+
+	return 0;
+}
+
+static int m7400_registered(struct usb_device *udev)
+{
+	m7400_usb_device = udev;
+	modem_status = BBSTATE_L0;
+	return 0;
+}
+
+static struct tegra_bb_gpio_irqdata m7400_gpioirqs[] = {
+	{ GPIO_INVALID, "tegra_bb_wake", m7400_wake_irq,
+				IRQF_TRIGGER_RISING, true, NULL },
+	{ GPIO_INVALID, NULL, NULL, 0, NULL },	/* End of table */
+};
+
+static struct tegra_bb_power_gdata m7400_gdata = {
+	.gpio = m7400_gpios,
+	.gpioirq = m7400_gpioirqs,
+};
+
+static struct tegra_bb_power_mdata m7400_mdata = {
+	.vid = 0x04cc,
+	.pid = 0x230f,
+	.wake_capable = true,
+	.autosuspend_ready = true,
+	.reg_cb = m7400_registered,
+};
+
+static struct tegra_bb_power_data m7400_data = {
+	.gpio_data = &m7400_gdata,
+	.modem_data = &m7400_mdata,
+};
+
+static void *m7400_init(void *pdata)
+{
+	struct tegra_bb_pdata *platdata = (struct tegra_bb_pdata *) pdata;
+	union tegra_bb_gpio_id *id = platdata->id;
+
+	/* Fill the gpio ids allocated by hardware */
+	m7400_gpios[0].data.gpio = id->m7400.pwr_on;
+	m7400_gpios[1].data.gpio = id->m7400.pwr_status;
+	m7400_gpios[2].data.gpio = id->m7400.service;
+	m7400_gpios[3].data.gpio = id->m7400.usb_awr;
+	m7400_gpios[4].data.gpio = id->m7400.usb_cwr;
+	m7400_gpios[5].data.gpio = id->m7400.resout2;
+	m7400_gpios[6].data.gpio = id->m7400.uart_awr;
+	m7400_gpioirqs[0].id = id->m7400.usb_cwr;
+
+	if (!platdata->ehci_register || !platdata->ehci_unregister) {
+		pr_info("%s - Error: ehci reg/unreg functions missing.\n"
+							, __func__);
+		return 0;
+	}
+
+	gpio_awr = m7400_gpios[3].data.gpio;
+	gpio_cwr = m7400_gpios[4].data.gpio;
+	gpio_arr = m7400_gpios[6].data.gpio;
+	if (gpio_awr == GPIO_INVALID || gpio_cwr == GPIO_INVALID
+			|| gpio_arr == GPIO_INVALID) {
+		pr_info("%s - Error: Invalid gpio data.\n", __func__);
+		return 0;
+	}
+
+	ehci_registered = false;
+	modem_status = BBSTATE_UNKNOWN;
+	return (void *) &m7400_data;
+}
+
+static void *m7400_deinit(void)
+{
+	return (void *) &m7400_data;
+}
+
+static struct tegra_bb_callback m7400_callbacks = {
+	.init = m7400_init,
+	.deinit = m7400_deinit,
+	.attrib = m7400_attrib_write,
+#ifdef CONFIG_PM
+	.power = m7400_power,
+#endif
+};
+
+void *m7400_get_cblist(void)
+{
+	return (void *) &m7400_callbacks;
+}
diff --git a/drivers/misc/tegra-baseband/bb-power.c b/drivers/misc/tegra-baseband/bb-power.c
new file mode 100644
index 000000000000..c070fd45160e
--- /dev/null
+++ b/drivers/misc/tegra-baseband/bb-power.c
@@ -0,0 +1,334 @@
+/*
+ * drivers/misc/tegra-baseband/bb-power.c
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/platform_device.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/usb.h>
+#include <linux/uaccess.h>
+#include <linux/platform_data/tegra_usb.h>
+#include <mach/usb_phy.h>
+#include <mach/tegra-bb-power.h>
+#include "bb-power.h"
+
+static struct tegra_bb_callback *callback;
+static int attr_load_val;
+static struct tegra_bb_power_mdata *mdata;
+static bb_get_cblist get_cblist[] = {
+	NULL,
+	NULL,
+	NULL,
+	M7400_CB,
+};
+
+static int tegra_bb_power_gpio_init(struct tegra_bb_power_gdata *gdata)
+{
+	int ret;
+	int irq;
+	unsigned gpio_id;
+	const char *gpio_label;
+	unsigned long gpio_flags;
+	struct tegra_bb_gpio_data *gpiolist;
+	struct tegra_bb_gpio_irqdata *gpioirq;
+
+	gpiolist = gdata->gpio;
+	for (; gpiolist->data.gpio != GPIO_INVALID; ++gpiolist) {
+		gpio_id = (gpiolist->data.gpio);
+		gpio_label = (gpiolist->data.label);
+		gpio_flags = (gpiolist->data.flags);
+
+		/* Request the gpio */
+		ret = gpio_request(gpio_id, gpio_label);
+		if (ret) {
+			pr_err("%s: Error: gpio_request for gpio %d failed.\n",
+							 __func__, gpio_id);
+			return ret;
+		}
+
+		/* Set gpio direction, as requested */
+		if (gpio_flags == GPIOF_IN)
+			gpio_direction_input(gpio_id);
+		else
+			gpio_direction_output(gpio_id, (!gpio_flags ? 0 : 1));
+
+		/* Create a sysfs node, if requested */
+		if (gpiolist->doexport)
+			gpio_export(gpio_id, false);
+	}
+
+	gpioirq = gdata->gpioirq;
+	for (; gpioirq->id != GPIO_INVALID; ++gpioirq) {
+
+		/* Create interrupt handler, if requested */
+		if (gpioirq->handler != NULL) {
+			irq = gpio_to_irq(gpioirq->id);
+			ret = request_threaded_irq(irq, NULL, gpioirq->handler,
+				gpioirq->flags, gpioirq->name, gpioirq->cookie);
+			if (ret < 0) {
+				pr_err("%s: Error: threaded_irq req fail.\n"
+								, __func__);
+				return ret;
+			}
+
+			if (gpioirq->wake_capable) {
+				ret = enable_irq_wake(irq);
+				if (ret) {
+					pr_err("%s: Error: irqwake req fail.\n",
+								__func__);
+					return ret;
+				}
+			}
+		}
+	}
+	return 0;
+}
+
+static int tegra_bb_power_gpio_deinit(struct tegra_bb_power_gdata *gdata)
+{
+	struct tegra_bb_gpio_data *gpiolist;
+	struct tegra_bb_gpio_irqdata *gpioirq;
+
+	gpiolist = gdata->gpio;
+	for (; gpiolist->data.gpio != GPIO_INVALID; ++gpiolist) {
+
+		/* Free the gpio */
+		gpio_free(gpiolist->data.gpio);
+	}
+
+	gpioirq = gdata->gpioirq;
+	for (; gpioirq->id != GPIO_INVALID; ++gpioirq) {
+
+		/* Free the irq */
+		free_irq(gpio_to_irq(gpioirq->id), gpioirq->cookie);
+	}
+	return 0;
+}
+
+static ssize_t tegra_bb_attr_write(struct device *dev,
+			struct device_attribute *attr,
+			const char *buf, size_t count)
+{
+	int val;
+
+	if (sscanf(buf, "%d", &val) != 1)
+		return -EINVAL;
+
+	if (callback && callback->attrib) {
+		if (!callback->attrib(dev, val))
+			attr_load_val = val;
+	}
+	return count;
+}
+
+static ssize_t tegra_bb_attr_read(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%d", attr_load_val);
+}
+
+static DEVICE_ATTR(load, S_IRUSR | S_IWUSR | S_IRGRP,
+		tegra_bb_attr_read, tegra_bb_attr_write);
+
+static void tegra_usbdevice_added(struct usb_device *udev)
+{
+	const struct usb_device_descriptor *desc = &udev->descriptor;
+
+	if (desc->idVendor == mdata->vid &&
+	    desc->idProduct == mdata->pid) {
+		pr_debug("%s: Device %s added.\n", udev->product, __func__);
+
+		if (mdata->wake_capable)
+			device_set_wakeup_enable(&udev->dev, true);
+		if (mdata->autosuspend_ready)
+			usb_enable_autosuspend(udev);
+		if (mdata->reg_cb)
+			mdata->reg_cb(udev);
+	}
+}
+
+static void tegra_usbdevice_removed(struct usb_device *udev)
+{
+	const struct usb_device_descriptor *desc = &udev->descriptor;
+
+	if (desc->idVendor == mdata->vid &&
+	    desc->idProduct == mdata->pid) {
+		pr_debug("%s: Device %s removed.\n", udev->product, __func__);
+	}
+}
+
+static int tegra_usb_notify(struct notifier_block *self, unsigned long action,
+		      void *dev)
+{
+	switch (action) {
+	case USB_DEVICE_ADD:
+		tegra_usbdevice_added((struct usb_device *)dev);
+		break;
+	case USB_DEVICE_REMOVE:
+		tegra_usbdevice_removed((struct usb_device *)dev);
+		break;
+	}
+	return NOTIFY_OK;
+}
+
+static struct notifier_block tegra_usb_nb = {
+	.notifier_call = tegra_usb_notify,
+};
+
+static int tegra_bb_power_probe(struct platform_device *device)
+{
+	struct device *dev = &device->dev;
+	struct tegra_bb_pdata *pdata;
+	struct tegra_bb_power_data *data;
+	struct tegra_bb_power_gdata *gdata;
+	int err;
+	unsigned int bb_id;
+
+	pdata = (struct tegra_bb_pdata *) dev->platform_data;
+	if (!pdata) {
+		pr_err("%s - Error: platform data is empty.\n", __func__);
+		return -ENODEV;
+	}
+
+	/* Obtain BB specific callback list */
+	bb_id = pdata->bb_id;
+	if (get_cblist[bb_id] != NULL) {
+		callback = (struct tegra_bb_callback *) get_cblist[bb_id]();
+		if (callback && callback->init) {
+			data = (struct tegra_bb_power_data *)
+			callback->init((void *)pdata);
+
+			gdata = data->gpio_data;
+			if (!gdata) {
+				pr_err("%s - Error: Gpio data is empty.\n",
+								__func__);
+				return -ENODEV;
+			}
+
+			/* Initialize gpio as required */
+			tegra_bb_power_gpio_init(gdata);
+
+			mdata = data->modem_data;
+			if (mdata && mdata->vid && mdata->pid)
+				/* Register to notifications from usb core */
+				usb_register_notify(&tegra_usb_nb);
+		} else {
+			pr_err("%s - Error: init callback is empty.\n",
+								__func__);
+			return -ENODEV;
+		}
+	} else {
+		pr_err("%s - Error: callback data is empty.\n", __func__);
+		return -ENODEV;
+	}
+
+	/* Create the control sysfs node */
+	err = device_create_file(dev, &dev_attr_load);
+	if (err < 0) {
+		pr_err("%s - Error: device_create_file failed.\n", __func__);
+		return -ENODEV;
+	}
+	attr_load_val = 0;
+
+	return 0;
+}
+
+static int tegra_bb_power_remove(struct platform_device *device)
+{
+	struct device *dev = &device->dev;
+	struct tegra_bb_power_data *data;
+	struct tegra_bb_power_gdata *gdata;
+
+	/* BB specific callback */
+	if (callback && callback->deinit) {
+		data = (struct tegra_bb_power_data *)
+		callback->deinit();
+
+		/* Deinitialize gpios */
+		gdata = data->gpio_data;
+		if (gdata)
+			tegra_bb_power_gpio_deinit(gdata);
+		else {
+			pr_err("%s - Error: Gpio data is empty.\n", __func__);
+			return -ENODEV;
+		}
+
+		mdata = data->modem_data;
+		if (mdata && mdata->vid && mdata->pid)
+			/* Register to notifications from usb core */
+			usb_unregister_notify(&tegra_usb_nb);
+	}
+
+	/* Remove the control sysfs node */
+	device_remove_file(dev, &dev_attr_load);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int tegra_bb_power_suspend(struct platform_device *device,
+	pm_message_t state)
+{
+	/* BB specific callback */
+	if (callback && callback->power)
+		callback->power(PWRSTATE_L2L3);
+	return 0;
+}
+
+static int tegra_bb_power_resume(struct platform_device *device)
+{
+	/* BB specific callback */
+	if (callback && callback->power)
+		callback->power(PWRSTATE_L3L0);
+	return 0;
+}
+#endif
+
+static struct platform_driver tegra_bb_power_driver = {
+	.probe = tegra_bb_power_probe,
+	.remove = tegra_bb_power_remove,
+#ifdef CONFIG_PM
+	.suspend = tegra_bb_power_suspend,
+	.resume = tegra_bb_power_resume,
+#endif
+	.driver = {
+		.name = "tegra_baseband_power",
+	},
+};
+
+static int __init tegra_baseband_power_init(void)
+{
+	pr_debug("%s\n", __func__);
+	return platform_driver_register(&tegra_bb_power_driver);
+}
+
+static void __exit tegra_baseband_power_exit(void)
+{
+	pr_debug("%s\n", __func__);
+	platform_driver_unregister(&tegra_bb_power_driver);
+}
+
+module_init(tegra_baseband_power_init)
+module_exit(tegra_baseband_power_exit)
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_DESCRIPTION("Tegra modem power management driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/tegra-baseband/bb-power.h b/drivers/misc/tegra-baseband/bb-power.h
new file mode 100644
index 000000000000..cdd693802031
--- /dev/null
+++ b/drivers/misc/tegra-baseband/bb-power.h
@@ -0,0 +1,99 @@
+/*
+ * drivers/misc/tegra-baseband/bb-power.h
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+enum tegra_bb_state {
+	BBSTATE_UNKNOWN,
+	/* Baseband state L0 - Running */
+	BBSTATE_L0,
+	/* Baseband state L2 - Suspended */
+	BBSTATE_L2,
+	/* Baseband state L3 - Suspended and detached */
+	BBSTATE_L3,
+};
+
+enum tegra_bb_pwrstate {
+	/* System power state - Entering suspend */
+	PWRSTATE_L2L3,
+	/* System power state - Resuming from suspend */
+	PWRSTATE_L3L0,
+	PWRSTATE_INVALID,
+};
+
+struct tegra_bb_gpio_data {
+	/* Baseband gpio data */
+	struct gpio data;
+	/* Baseband gpio - Should it be exported to sysfs ? */
+	bool doexport;
+};
+
+struct tegra_bb_gpio_irqdata {
+	/* Baseband gpio IRQ - Id */
+	int id;
+	/* Baseband gpio IRQ - Friendly name */
+	const char *name;
+	/* Baseband gpio IRQ - IRQ handler */
+	irq_handler_t handler;
+	/* Baseband gpio IRQ - IRQ trigger flags */
+	int flags;
+	/* Baseband gpio IRQ - Can the gpio wake system from sleep ? */
+	bool wake_capable;
+	void *cookie;
+};
+
+typedef void* (*bb_get_cblist)(void);
+typedef void* (*bb_init_cb)(void *pdata);
+typedef void* (*bb_deinit_cb)(void);
+typedef int (*bb_power_cb)(int code);
+typedef int (*bb_attrib_cb)(struct device *dev, int value);
+typedef int (*modem_register_cb)(struct usb_device *udev);
+
+struct tegra_bb_power_gdata {
+	struct tegra_bb_gpio_data *gpio;
+	struct tegra_bb_gpio_irqdata *gpioirq;
+};
+
+struct tegra_bb_power_mdata {
+	/* Baseband USB vendor ID */
+	int vid;
+	/* Baseband USB product ID */
+	int pid;
+	/* Baseband capability - Can it generate a wakeup ? */
+	bool wake_capable;
+	/* Baseband capability - Can it be auto/runtime suspended ? */
+	bool autosuspend_ready;
+	/* Baseband callback after a successful registration */
+	modem_register_cb reg_cb;
+};
+
+struct tegra_bb_power_data {
+	struct tegra_bb_power_gdata *gpio_data;
+	struct tegra_bb_power_mdata *modem_data;
+};
+
+struct tegra_bb_callback {
+	bb_init_cb init;
+	bb_deinit_cb deinit;
+	bb_power_cb power;
+	bb_attrib_cb attrib;
+	bool valid;
+};
+
+#ifdef CONFIG_TEGRA_BB_M7400
+extern void *m7400_get_cblist(void);
+#define M7400_CB m7400_get_cblist
+#else
+#define M7400_CB NULL
+#endif
diff --git a/drivers/misc/tegra-cec/Kconfig b/drivers/misc/tegra-cec/Kconfig
new file mode 100644
index 000000000000..11fa9bc9ce17
--- /dev/null
+++ b/drivers/misc/tegra-cec/Kconfig
@@ -0,0 +1,19 @@
+menuconfig TEGRA_CEC_SUPPORT
+	bool "Tegra CEC support"
+	depends on ARCH_TEGRA
+	---help---
+	  Say Y here to get to see options for tegra CEC support.
+	  This option alone does not add any kernel code.
+
+	  If you say N, all options in this submenu will be skipped and disabled.
+
+if TEGRA_CEC_SUPPORT
+
+config TEGRA_CEC_T30
+	bool "Enable T30 CEC support"
+	---help---
+	 Adds HDMI-CEC driver for T30.
+
+	 Disabled by default. Choose Y here if you want to build the driver.
+
+endif # TEGRA_CEC_SUPPORT
diff --git a/drivers/misc/tegra-cec/Makefile b/drivers/misc/tegra-cec/Makefile
new file mode 100644
index 000000000000..8bac2a519046
--- /dev/null
+++ b/drivers/misc/tegra-cec/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for tegra cec support.
+#
+
+subdir-ccflags-y	= -Werror
+
+obj-$(CONFIG_TEGRA_CEC_T30)	+= tegra_cec.o
diff --git a/drivers/misc/tegra-cec/tegra_cec.c b/drivers/misc/tegra-cec/tegra_cec.c
new file mode 100644
index 000000000000..a876d9d60712
--- /dev/null
+++ b/drivers/misc/tegra-cec/tegra_cec.c
@@ -0,0 +1,383 @@
+/*
+ * drivers/misc/tegra-cec/tegra_cec.c
+ *
+ * Copyright (c) 2012, NVIDIA 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/workqueue.h>
+#include <linux/ktime.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+
+#include <linux/platform_device.h>
+#include <linux/miscdevice.h>
+
+#include <mach/clk.h>
+
+#include "tegra_cec.h"
+
+
+int tegra_cec_open(struct inode *inode, struct file *file)
+{
+	struct miscdevice *miscdev = file->private_data;
+	struct tegra_cec *cec = container_of(miscdev,
+		struct tegra_cec, misc_dev);
+	dev_dbg(cec->dev, "%s\n", __func__);
+	file->private_data = cec;
+
+	return 0;
+}
+
+int tegra_cec_release(struct inode *inode, struct file *file)
+{
+	struct tegra_cec *cec = file->private_data;
+
+	dev_dbg(cec->dev, "%s\n", __func__);
+
+	return 0;
+}
+
+ssize_t tegra_cec_write(struct file *file, const char __user *buffer,
+	size_t count, loff_t *ppos)
+{
+	struct tegra_cec *cec = file->private_data;
+	unsigned long write_buff;
+
+	count = 4;
+
+	if (copy_from_user(&write_buff, buffer, count))
+		return -EFAULT;
+
+	writel((TEGRA_CEC_INT_MASK_TX_REGISTER_EMPTY |
+		TEGRA_CEC_INT_MASK_TX_REGISTER_UNDERRUN |
+	    TEGRA_CEC_INT_MASK_TX_FRAME_OR_BLOCK_NAKD |
+	    TEGRA_CEC_INT_MASK_TX_ARBITRATION_FAILED |
+	    TEGRA_CEC_INT_MASK_TX_BUS_ANOMALY_DETECTED |
+	    TEGRA_CEC_INT_MASK_RX_REGISTER_FULL |
+	    TEGRA_CEC_INT_MASK_RX_REGISTER_OVERRUN),
+	    cec->cec_base + TEGRA_CEC_INT_MASK);
+
+	wait_event_interruptible(cec->tx_waitq, cec->tx_wake == 1);
+	writel(write_buff, cec->cec_base + TEGRA_CEC_TX_REGISTER);
+	cec->tx_wake = 0;
+
+	writel((TEGRA_CEC_INT_MASK_TX_REGISTER_UNDERRUN |
+		TEGRA_CEC_INT_MASK_TX_FRAME_OR_BLOCK_NAKD |
+	    TEGRA_CEC_INT_MASK_TX_ARBITRATION_FAILED |
+	    TEGRA_CEC_INT_MASK_TX_BUS_ANOMALY_DETECTED |
+	    TEGRA_CEC_INT_MASK_RX_REGISTER_FULL |
+	    TEGRA_CEC_INT_MASK_RX_REGISTER_OVERRUN),
+	    cec->cec_base + TEGRA_CEC_INT_MASK);
+
+	write_buff = 0x00;
+	return count;
+}
+
+ssize_t tegra_cec_read(struct file *file, char  __user *buffer,
+	size_t count, loff_t *ppos)
+{
+	struct tegra_cec *cec = file->private_data;
+	unsigned short rx_buffer;
+	count = 2;
+
+	if (cec->rx_wake == 0)
+		if (file->f_flags & O_NONBLOCK)
+			return -EAGAIN;
+
+	wait_event_interruptible(cec->rx_waitq, cec->rx_wake == 1);
+
+	rx_buffer = readw(cec->cec_base + TEGRA_CEC_RX_REGISTER);
+
+	if (copy_to_user(buffer, &rx_buffer, count))
+		return -EFAULT;
+
+	rx_buffer = 0x0;
+	cec->rx_wake = 0;
+	return count;
+}
+
+static irqreturn_t tegra_cec_irq_handler(int irq, void *data)
+{
+	struct device *dev = data;
+	struct tegra_cec *cec = dev_get_drvdata(dev);
+	unsigned long status;
+
+	status = readl(cec->cec_base + TEGRA_CEC_INT_STAT);
+
+	if (!status)
+		return IRQ_HANDLED;
+
+	if ((status & TEGRA_CEC_INT_STAT_RX_REGISTER_OVERRUN) ||
+		(status & TEGRA_CEC_INT_STAT_RX_BUS_ANOMALY_DETECTED) ||
+		(status & TEGRA_CEC_INT_STAT_RX_START_BIT_DETECTED) ||
+		(status & TEGRA_CEC_INT_STAT_RX_BUS_ERROR_DETECTED)) {
+		writel((TEGRA_CEC_INT_STAT_RX_REGISTER_OVERRUN |
+			TEGRA_CEC_INT_STAT_RX_BUS_ANOMALY_DETECTED |
+			TEGRA_CEC_INT_STAT_RX_START_BIT_DETECTED |
+			TEGRA_CEC_INT_STAT_RX_BUS_ERROR_DETECTED),
+			cec->cec_base + TEGRA_CEC_INT_STAT);
+	} else if (status & TEGRA_CEC_INT_STAT_RX_REGISTER_FULL) {
+		writel((TEGRA_CEC_INT_STAT_RX_REGISTER_FULL),
+			cec->cec_base + TEGRA_CEC_INT_STAT);
+		cec->rx_wake = 1;
+		wake_up_interruptible(&cec->rx_waitq);
+	} else if ((status & TEGRA_CEC_INT_STAT_TX_REGISTER_UNDERRUN) ||
+		(status & TEGRA_CEC_INT_STAT_TX_FRAME_OR_BLOCK_NAKD) ||
+		(status & TEGRA_CEC_INT_STAT_TX_ARBITRATION_FAILED) ||
+		(status & TEGRA_CEC_INT_STAT_TX_BUS_ANOMALY_DETECTED)) {
+		writel((TEGRA_CEC_INT_STAT_TX_REGISTER_UNDERRUN |
+			TEGRA_CEC_INT_STAT_TX_FRAME_OR_BLOCK_NAKD |
+			TEGRA_CEC_INT_STAT_TX_REGISTER_EMPTY |
+			TEGRA_CEC_INT_STAT_TX_ARBITRATION_FAILED |
+			TEGRA_CEC_INT_STAT_TX_BUS_ANOMALY_DETECTED),
+			cec->cec_base + TEGRA_CEC_INT_STAT);
+	} else if (status & TEGRA_CEC_INT_STAT_TX_REGISTER_EMPTY) {
+		cec->tx_wake = 1;
+		wake_up_interruptible(&cec->tx_waitq);
+		writel((TEGRA_CEC_INT_STAT_TX_REGISTER_EMPTY),
+		   cec->cec_base + TEGRA_CEC_INT_STAT);
+	} else if (status & TEGRA_CEC_INT_STAT_TX_FRAME_TRANSMITTED) {
+		writel((TEGRA_CEC_INT_STAT_TX_FRAME_TRANSMITTED),
+		   cec->cec_base + TEGRA_CEC_INT_STAT);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static const struct file_operations tegra_cec_fops = {
+	.owner = THIS_MODULE,
+	.open = tegra_cec_open,
+	.release = tegra_cec_release,
+	.read = tegra_cec_read,
+	.write = tegra_cec_write,
+};
+
+static void tegra_cec_init(struct tegra_cec *cec)
+{
+
+	writel(0x00, cec->cec_base + TEGRA_CEC_HW_CONTROL);
+	writel(0x00, cec->cec_base + TEGRA_CEC_INT_MASK);
+	writel(0xffffffff, cec->cec_base + TEGRA_CEC_INT_STAT);
+	msleep(1000);
+
+	writel(0x00, cec->cec_base + TEGRA_CEC_SW_CONTROL);
+
+	writel((TEGRA_CEC_LOGICAL_ADDR<<TEGRA_CEC_HW_CONTROL_RX_LOGICAL_ADDRS_MASK)&
+	   (~TEGRA_CEC_HW_CONTROL_RX_SNOOP) &
+	   (~TEGRA_CEC_HW_CONTROL_RX_NAK_MODE) &
+	   (~TEGRA_CEC_HW_CONTROL_TX_NAK_MODE) &
+	   (~TEGRA_CEC_HW_CONTROL_FAST_SIM_MODE) |
+	   (TEGRA_CEC_HW_CONTROL_TX_RX_MODE),
+	   cec->cec_base + TEGRA_CEC_HW_CONTROL);
+
+	writel(0x00, cec->cec_base + TEGRA_CEC_INPUT_FILTER);
+
+	writel((0x7a << TEGRA_CEC_RX_TIMING_0_RX_START_BIT_MAX_LO_TIME_MASK) |
+	   (0x6d << TEGRA_CEC_RX_TIMING_0_RX_START_BIT_MIN_LO_TIME_MASK) |
+	   (0x93 << TEGRA_CEC_RX_TIMING_0_RX_START_BIT_MAX_DURATION_MASK) |
+	   (0x86 << TEGRA_CEC_RX_TIMING_0_RX_START_BIT_MIN_DURATION_MASK),
+	   cec->cec_base + TEGRA_CEC_RX_TIMING_0);
+
+	writel((0x35 << TEGRA_CEC_RX_TIMING_1_RX_DATA_BIT_MAX_LO_TIME_MASK) |
+	   (0x21 << TEGRA_CEC_RX_TIMING_1_RX_DATA_BIT_SAMPLE_TIME_MASK) |
+	   (0x56 << TEGRA_CEC_RX_TIMING_1_RX_DATA_BIT_MAX_DURATION_MASK) |
+	   (0x40 << TEGRA_CEC_RX_TIMING_1_RX_DATA_BIT_MIN_DURATION_MASK),
+	   cec->cec_base + TEGRA_CEC_RX_TIMING_1);
+
+	writel((0x50 << TEGRA_CEC_RX_TIMING_2_RX_END_OF_BLOCK_TIME_MASK),
+	   cec->cec_base + TEGRA_CEC_RX_TIMING_2);
+
+	writel((0x74 << TEGRA_CEC_TX_TIMING_0_TX_START_BIT_LO_TIME_MASK) |
+	   (0x8d << TEGRA_CEC_TX_TIMING_0_TX_START_BIT_DURATION_MASK) |
+	   (0x08 << TEGRA_CEC_TX_TIMING_0_TX_BUS_XITION_TIME_MASK) |
+	   (0x71 << TEGRA_CEC_TX_TIMING_0_TX_BUS_ERROR_LO_TIME_MASK),
+	   cec->cec_base + TEGRA_CEC_TX_TIMING_0);
+
+	writel((0x2f << TEGRA_CEC_TX_TIMING_1_TX_LO_DATA_BIT_LO_TIME_MASK) |
+	   (0x13 << TEGRA_CEC_TX_TIMING_1_TX_HI_DATA_BIT_LO_TIME_MASK) |
+	   (0x4b << TEGRA_CEC_TX_TIMING_1_TX_DATA_BIT_DURATION_MASK) |
+	   (0x21 << TEGRA_CEC_TX_TIMING_1_TX_ACK_NAK_BIT_SAMPLE_TIME_MASK),
+	   cec->cec_base + TEGRA_CEC_TX_TIMING_1);
+
+	writel((0x07 << TEGRA_CEC_TX_TIMING_2_BUS_IDLE_TIME_ADDITIONAL_FRAME_MASK) |
+	   (0x05 << TEGRA_CEC_TX_TIMING_2_TX_BUS_IDLE_TIME_NEW_FRAME_MASK) |
+	   (0x03 << TEGRA_CEC_TX_TIMING_2_TX_BUS_IDLE_TIME_RETRY_FRAME_MASK),
+	   cec->cec_base + TEGRA_CEC_TX_TIMING_2);
+
+	writel((TEGRA_CEC_INT_MASK_TX_REGISTER_UNDERRUN |
+	    TEGRA_CEC_INT_MASK_TX_FRAME_OR_BLOCK_NAKD |
+	    TEGRA_CEC_INT_MASK_TX_ARBITRATION_FAILED |
+	    TEGRA_CEC_INT_MASK_TX_BUS_ANOMALY_DETECTED |
+	    TEGRA_CEC_INT_MASK_RX_REGISTER_FULL |
+	    TEGRA_CEC_INT_MASK_RX_REGISTER_OVERRUN),
+	   cec->cec_base + TEGRA_CEC_INT_MASK);
+}
+
+static int __devinit tegra_cec_probe(struct platform_device *pdev)
+{
+	struct tegra_cec *cec;
+	struct resource *res;
+	int ret = 0;
+
+	cec = devm_kzalloc(&pdev->dev, sizeof(struct tegra_cec), GFP_KERNEL);
+
+	if (!cec)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	if (!res) {
+		dev_err(&pdev->dev,
+		    "Unable to allocate resources for device.\n");
+		ret = -EBUSY;
+		goto cec_error;
+	}
+
+	if (!devm_request_mem_region(&pdev->dev, res->start, resource_size(res),
+		pdev->name)) {
+		dev_err(&pdev->dev,
+			"Unable to request mem region for device.\n");
+		ret = -EBUSY;
+		goto cec_error;
+	}
+
+	cec->tegra_cec_irq = platform_get_irq(pdev, 0);
+
+	if (cec->tegra_cec_irq <= 0) {
+		ret = -EBUSY;
+		goto cec_error;
+	}
+
+	cec->cec_base = devm_ioremap_nocache(&pdev->dev, res->start,
+		resource_size(res));
+
+	if (!cec->cec_base) {
+		dev_err(&pdev->dev, "Unable to grab IOs for device.\n");
+		ret = -EBUSY;
+		goto cec_error;
+	}
+
+	cec->clk = clk_get(&pdev->dev, "cec");
+
+	if (IS_ERR_OR_NULL(cec->clk)) {
+		dev_err(&pdev->dev, "can't get clock for CEC\n");
+		ret = -ENOENT;
+		goto clk_error;
+	}
+
+	clk_enable(cec->clk);
+
+	/* set context info. */
+	cec->dev = &pdev->dev;
+	cec->rx_wake = 0;
+	cec->tx_wake = 0;
+	init_waitqueue_head(&cec->rx_waitq);
+	init_waitqueue_head(&cec->tx_waitq);
+
+	platform_set_drvdata(pdev, cec);
+	/* clear out the hardware. */
+
+	tegra_cec_init(cec);
+
+	device_init_wakeup(&pdev->dev, 1);
+
+	cec->misc_dev.minor = MISC_DYNAMIC_MINOR;
+	cec->misc_dev.name = TEGRA_CEC_NAME;
+	cec->misc_dev.fops = &tegra_cec_fops;
+	cec->misc_dev.parent = &pdev->dev;
+
+	if (misc_register(&cec->misc_dev)) {
+		printk(KERN_WARNING "Couldn't register device , %s.\n", TEGRA_CEC_NAME);
+		goto cec_error;
+	}
+
+	ret = devm_request_irq(&pdev->dev, cec->tegra_cec_irq,
+		tegra_cec_irq_handler, IRQF_DISABLED, "cec_irq", &pdev->dev);
+
+	if (ret) {
+		dev_err(&pdev->dev,
+			"Unable to request interrupt for device (err=%d).\n", ret);
+		goto cec_error;
+	}
+
+	dev_notice(&pdev->dev, "probed\n");
+
+	return 0;
+
+cec_error:
+	clk_disable(cec->clk)
+	clk_put(cec->clk);
+clk_error:
+	return ret;
+}
+
+static int tegra_cec_remove(struct platform_device *pdev)
+{
+	struct tegra_cec *cec = platform_get_drvdata(pdev);
+
+	clk_disable(cec->clk)
+	clk_put(cec->clk);
+
+	misc_deregister(&cec->misc_dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int tegra_cec_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct tegra_cec *cec = platform_get_drvdata(pdev);
+
+	clk_disable(cec->clk);
+
+	return 0;
+}
+
+static int tegra_cec_resume(struct platform_device *pdev)
+{
+
+	struct tegra_cec *cec = platform_get_drvdata(pdev);
+	clk_enable(cec->clk);
+	tegra_cec_init(cec);
+	return 0;
+}
+#endif
+
+static struct platform_driver tegra_cec_driver = {
+	.driver = {
+		.name = TEGRA_CEC_NAME,
+		.owner = THIS_MODULE,
+	},
+	.probe = tegra_cec_probe,
+	.remove = tegra_cec_remove,
+
+#ifdef CONFIG_PM
+	.suspend = tegra_cec_suspend,
+	.resume = tegra_cec_resume,
+#endif
+};
+
+module_platform_driver(tegra_cec_driver);
diff --git a/drivers/misc/tegra-cec/tegra_cec.h b/drivers/misc/tegra-cec/tegra_cec.h
new file mode 100644
index 000000000000..acc94dc61965
--- /dev/null
+++ b/drivers/misc/tegra-cec/tegra_cec.h
@@ -0,0 +1,134 @@
+/*
+ * drivers/misc/tegra-cec/tegra_cec.h
+ *
+ * Copyright (c) 2012, NVIDIA 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/pm.h>
+
+
+struct tegra_cec {
+	struct device		*dev;
+	struct miscdevice 	misc_dev;
+	struct clk		*clk;
+	void __iomem		*cec_base;
+	int			tegra_cec_irq;
+	wait_queue_head_t	rx_waitq;
+	wait_queue_head_t	tx_waitq;
+	unsigned int		rx_wake;
+	unsigned int		tx_wake;
+};
+static int tegra_cec_remove(struct platform_device *pdev);
+
+/*CEC Timing registers*/
+#define TEGRA_CEC_SW_CONTROL 	 0X000
+#define TEGRA_CEC_HW_CONTROL	 0X004
+#define TEGRA_CEC_INPUT_FILTER	 0X008
+#define TEGRA_CEC_TX_REGISTER	 0X010
+#define TEGRA_CEC_RX_REGISTER	 0X014
+#define TEGRA_CEC_RX_TIMING_0	 0X018
+#define TEGRA_CEC_RX_TIMING_1	 0X01C
+#define TEGRA_CEC_RX_TIMING_2	 0X020
+#define TEGRA_CEC_TX_TIMING_0	 0X024
+#define TEGRA_CEC_TX_TIMING_1	 0X028
+#define TEGRA_CEC_TX_TIMING_2	 0X02C
+#define TEGRA_CEC_INT_STAT	 0X030
+#define TEGRA_CEC_INT_MASK	 0X034
+#define TEGRA_CEC_HW_DEBUG_RX	 0X038
+#define TEGRA_CEC_HW_DEBUG_TX	 0X03C
+
+#define TEGRA_CEC_LOGICAL_ADDR	0x10
+
+#define TEGRA_CEC_HW_CONTROL_RX_LOGICAL_ADDRS_MASK	0
+#define TEGRA_CEC_HW_CONTROL_RX_SNOOP 			(1<<15)
+#define TEGRA_CEC_HW_CONTROL_RX_NAK_MODE 		(1<<16)
+#define TEGRA_CEC_HW_CONTROL_TX_NAK_MODE		(1<<24)
+#define TEGRA_CEC_HW_CONTROL_FAST_SIM_MODE 		(1<<30)
+#define TEGRA_CEC_HW_CONTROL_TX_RX_MODE			(1<<31)
+
+#define TEGRA_CEC_INPUT_FILTER_MODE		(1<<31)
+#define TEGRA_CEC_INPUT_FILTER_FIFO_LENGTH_MASK	0
+
+#define TEGRA_CEC_TX_REGISTER_DATA_MASK	 		0
+#define TEGRA_CEC_TX_REGISTER_EOM_MASK	 		8
+#define TEGRA_CEC_TX_REGISTER_ADDRESS_MODE_MASK	 	12
+#define TEGRA_CEC_TX_REGISTER_GENERATE_START_BIT_MASK	16
+#define TEGRA_CEC_TX_REGISTER_RETRY_FRAME_MASK	 	17
+
+#define TEGRA_CEC_RX_REGISTER_MASK	 0
+#define TEGRA_CEC_RX_REGISTER_EOM	 (1<<8)
+#define TEGRA_CEC_RX_REGISTER_ACK	 (1<<9)
+
+#define TEGRA_CEC_RX_TIMING_0_RX_START_BIT_MAX_LO_TIME_MASK	 0
+#define TEGRA_CEC_RX_TIMING_0_RX_START_BIT_MIN_LO_TIME_MASK	 8
+#define TEGRA_CEC_RX_TIMING_0_RX_START_BIT_MAX_DURATION_MASK	 16
+#define TEGRA_CEC_RX_TIMING_0_RX_START_BIT_MIN_DURATION_MASK	 24
+
+#define TEGRA_CEC_RX_TIMING_1_RX_DATA_BIT_MAX_LO_TIME_MASK	 0
+#define TEGRA_CEC_RX_TIMING_1_RX_DATA_BIT_SAMPLE_TIME_MASK	 8
+#define TEGRA_CEC_RX_TIMING_1_RX_DATA_BIT_MAX_DURATION_MASK	 16
+#define TEGRA_CEC_RX_TIMING_1_RX_DATA_BIT_MIN_DURATION_MASK	 24
+
+#define TEGRA_CEC_RX_TIMING_2_RX_END_OF_BLOCK_TIME_MASK	 0
+
+#define TEGRA_CEC_TX_TIMING_0_TX_START_BIT_LO_TIME_MASK	 	0
+#define TEGRA_CEC_TX_TIMING_0_TX_START_BIT_DURATION_MASK	8
+#define TEGRA_CEC_TX_TIMING_0_TX_BUS_XITION_TIME_MASK	 	16
+#define TEGRA_CEC_TX_TIMING_0_TX_BUS_ERROR_LO_TIME_MASK	 	24
+
+#define TEGRA_CEC_TX_TIMING_1_TX_LO_DATA_BIT_LO_TIME_MASK	0
+#define TEGRA_CEC_TX_TIMING_1_TX_HI_DATA_BIT_LO_TIME_MASK	8
+#define TEGRA_CEC_TX_TIMING_1_TX_DATA_BIT_DURATION_MASK	 	16
+#define TEGRA_CEC_TX_TIMING_1_TX_ACK_NAK_BIT_SAMPLE_TIME_MASK	24
+
+#define TEGRA_CEC_TX_TIMING_2_BUS_IDLE_TIME_ADDITIONAL_FRAME_MASK	0
+#define TEGRA_CEC_TX_TIMING_2_TX_BUS_IDLE_TIME_NEW_FRAME_MASK		4
+#define TEGRA_CEC_TX_TIMING_2_TX_BUS_IDLE_TIME_RETRY_FRAME_MASK	 	8
+
+#define TEGRA_CEC_INT_STAT_TX_REGISTER_EMPTY	 		(1<<0)
+#define TEGRA_CEC_INT_STAT_TX_REGISTER_UNDERRUN	 		(1<<1)
+#define TEGRA_CEC_INT_STAT_TX_FRAME_OR_BLOCK_NAKD		(1<<2)
+#define TEGRA_CEC_INT_STAT_TX_ARBITRATION_FAILED		(1<<3)
+#define TEGRA_CEC_INT_STAT_TX_BUS_ANOMALY_DETECTED		(1<<4)
+#define TEGRA_CEC_INT_STAT_TX_FRAME_TRANSMITTED			(1<<5)
+#define TEGRA_CEC_INT_STAT_RX_REGISTER_FULL			(1<<8)
+#define TEGRA_CEC_INT_STAT_RX_REGISTER_OVERRUN			(1<<9)
+#define TEGRA_CEC_INT_STAT_RX_START_BIT_DETECTED		(1<<10)
+#define TEGRA_CEC_INT_STAT_RX_BUS_ANOMALY_DETECTED		(1<<11)
+#define TEGRA_CEC_INT_STAT_RX_BUS_ERROR_DETECTED		(1<<12)
+#define TEGRA_CEC_INT_STAT_FILTERED_RX_DATA_PIN_TRANSITION_H2L	(1<<13)
+#define TEGRA_CEC_INT_STAT_FILTERED_RX_DATA_PIN_TRANSITION_L2H	(1<<14)
+
+#define TEGRA_CEC_INT_MASK_TX_REGISTER_EMPTY	 		(1<<0)
+#define TEGRA_CEC_INT_MASK_TX_REGISTER_UNDERRUN	 		(1<<1)
+#define TEGRA_CEC_INT_MASK_TX_FRAME_OR_BLOCK_NAKD		(1<<2)
+#define TEGRA_CEC_INT_MASK_TX_ARBITRATION_FAILED		(1<<3)
+#define TEGRA_CEC_INT_MASK_TX_BUS_ANOMALY_DETECTED		(1<<4)
+#define TEGRA_CEC_INT_MASK_TX_FRAME_TRANSMITTED			(1<<5)
+#define TEGRA_CEC_INT_MASK_RX_REGISTER_FULL			(1<<8)
+#define TEGRA_CEC_INT_MASK_RX_REGISTER_OVERRUN			(1<<9)
+#define TEGRA_CEC_INT_MASK_RX_START_BIT_DETECTED		(1<<10)
+#define TEGRA_CEC_INT_MASK_RX_BUS_ANOMALY_DETECTED		(1<<11)
+#define TEGRA_CEC_INT_MASK_RX_BUS_ERROR_DETECTED		(1<<12)
+#define TEGRA_CEC_INT_MASK_FILTERED_RX_DATA_PIN_TRANSITION_H2L	(1<<13)
+#define TEGRA_CEC_INT_MASK_FILTERED_RX_DATA_PIN_TRANSITION_L2H	(1<<14)
+
+#define TEGRA_CEC_HW_DEBUG_TX_DURATION_COUNT_MASK	0
+#define TEGRA_CEC_HW_DEBUG_TX_TXBIT_COUNT_MASK		17
+#define TEGRA_CEC_HW_DEBUG_TX_STATE_MASK		21
+#define TEGRA_CEC_HW_DEBUG_TX_FORCELOOUT		(1<<25)
+#define TEGRA_CEC_HW_DEBUG_TX_TXDATABIT_SAMPLE_TIMER	(1<<26)
+
+#define TEGRA_CEC_NAME "tegra_cec"
diff --git a/drivers/misc/tegra-cryptodev.c b/drivers/misc/tegra-cryptodev.c
new file mode 100644
index 000000000000..d767ea05d6eb
--- /dev/null
+++ b/drivers/misc/tegra-cryptodev.c
@@ -0,0 +1,518 @@
+/*
+ * drivers/misc/tegra-cryptodev.c
+ *
+ * crypto dev node for NVIDIA tegra aes hardware
+ *
+ * Copyright (c) 2010, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/crypto.h>
+#include <linux/scatterlist.h>
+#include <linux/uaccess.h>
+#include <crypto/rng.h>
+#include <crypto/hash.h>
+#include <mach/hardware.h>
+
+#include "tegra-cryptodev.h"
+
+#define NBUFS 2
+#define XBUFSIZE 8
+
+struct tegra_crypto_ctx {
+	struct crypto_ablkcipher *ecb_tfm;
+	struct crypto_ablkcipher *cbc_tfm;
+	struct crypto_ablkcipher *ofb_tfm;
+	struct crypto_ablkcipher *ctr_tfm;
+	struct crypto_ablkcipher *cmac_tfm;
+	struct crypto_rng *rng;
+	u8 seed[TEGRA_CRYPTO_RNG_SEED_SIZE];
+	int use_ssk;
+};
+
+struct tegra_crypto_completion {
+	struct completion restart;
+	int req_err;
+};
+
+static int alloc_bufs(unsigned long *buf[NBUFS])
+{
+	int i;
+
+	for (i = 0; i < NBUFS; i++) {
+		buf[i] = (void *)__get_free_page(GFP_KERNEL);
+		if (!buf[i])
+			goto err_free_buf;
+	}
+
+	return 0;
+
+err_free_buf:
+	while (i-- > 0)
+		free_page((unsigned long)buf[i]);
+
+	return -ENOMEM;
+}
+
+static void free_bufs(unsigned long *buf[NBUFS])
+{
+	int i;
+
+	for (i = 0; i < NBUFS; i++)
+		free_page((unsigned long)buf[i]);
+}
+
+static int tegra_crypto_dev_open(struct inode *inode, struct file *filp)
+{
+	struct tegra_crypto_ctx *ctx;
+	int ret = 0;
+
+	ctx = kzalloc(sizeof(struct tegra_crypto_ctx), GFP_KERNEL);
+	if (!ctx) {
+		pr_err("no memory for context\n");
+		return -ENOMEM;
+	}
+
+	ctx->ecb_tfm = crypto_alloc_ablkcipher("ecb-aes-tegra",
+		CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC, 0);
+	if (IS_ERR(ctx->ecb_tfm)) {
+		pr_err("Failed to load transform for ecb-aes-tegra: %ld\n",
+			PTR_ERR(ctx->ecb_tfm));
+		ret = PTR_ERR(ctx->ecb_tfm);
+		goto fail_ecb;
+	}
+
+	ctx->cbc_tfm = crypto_alloc_ablkcipher("cbc-aes-tegra",
+		CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC, 0);
+	if (IS_ERR(ctx->cbc_tfm)) {
+		pr_err("Failed to load transform for cbc-aes-tegra: %ld\n",
+			PTR_ERR(ctx->cbc_tfm));
+		ret = PTR_ERR(ctx->cbc_tfm);
+		goto fail_cbc;
+	}
+
+	if (tegra_get_chipid() != TEGRA_CHIPID_TEGRA2) {
+		ctx->ofb_tfm = crypto_alloc_ablkcipher("ofb-aes-tegra",
+			CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC, 0);
+		if (IS_ERR(ctx->ofb_tfm)) {
+			pr_err("Failed to load transform for ofb-aes-tegra: %ld\n",
+				PTR_ERR(ctx->ofb_tfm));
+			ret = PTR_ERR(ctx->ofb_tfm);
+			goto fail_ofb;
+		}
+
+		ctx->ctr_tfm = crypto_alloc_ablkcipher("ctr-aes-tegra",
+			CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC, 0);
+		if (IS_ERR(ctx->ctr_tfm)) {
+			pr_err("Failed to load transform for ctr-aes-tegra: %ld\n",
+				PTR_ERR(ctx->ctr_tfm));
+			ret = PTR_ERR(ctx->ctr_tfm);
+			goto fail_ctr;
+		}
+	}
+
+	ctx->rng = crypto_alloc_rng("rng-aes-tegra", CRYPTO_ALG_TYPE_RNG, 0);
+	if (IS_ERR(ctx->rng)) {
+		pr_err("Failed to load transform for tegra rng: %ld\n",
+			PTR_ERR(ctx->rng));
+		ret = PTR_ERR(ctx->rng);
+		goto fail_rng;
+	}
+
+	filp->private_data = ctx;
+	return ret;
+
+fail_rng:
+	if (tegra_get_chipid() != TEGRA_CHIPID_TEGRA2)
+		crypto_free_ablkcipher(ctx->ctr_tfm);
+fail_ctr:
+	if (tegra_get_chipid() != TEGRA_CHIPID_TEGRA2)
+		crypto_free_ablkcipher(ctx->ofb_tfm);
+fail_ofb:
+	crypto_free_ablkcipher(ctx->cbc_tfm);
+
+fail_cbc:
+	crypto_free_ablkcipher(ctx->ecb_tfm);
+
+fail_ecb:
+	kfree(ctx);
+	return ret;
+}
+
+static int tegra_crypto_dev_release(struct inode *inode, struct file *filp)
+{
+	struct tegra_crypto_ctx *ctx = filp->private_data;
+
+	crypto_free_ablkcipher(ctx->ecb_tfm);
+	crypto_free_ablkcipher(ctx->cbc_tfm);
+
+	if (tegra_get_chipid() != TEGRA_CHIPID_TEGRA2) {
+		crypto_free_ablkcipher(ctx->ofb_tfm);
+		crypto_free_ablkcipher(ctx->ctr_tfm);
+	}
+
+	crypto_free_rng(ctx->rng);
+	kfree(ctx);
+	filp->private_data = NULL;
+	return 0;
+}
+
+static void tegra_crypt_complete(struct crypto_async_request *req, int err)
+{
+	struct tegra_crypto_completion *done = req->data;
+
+	if (err != -EINPROGRESS) {
+		done->req_err = err;
+		complete(&done->restart);
+	}
+}
+
+static int process_crypt_req(struct tegra_crypto_ctx *ctx, struct tegra_crypt_req *crypt_req)
+{
+	struct crypto_ablkcipher *tfm;
+	struct ablkcipher_request *req = NULL;
+	struct scatterlist in_sg;
+	struct scatterlist out_sg;
+	unsigned long *xbuf[NBUFS];
+	int ret = 0, size = 0;
+	unsigned long total = 0;
+	const u8 *key = NULL;
+	struct tegra_crypto_completion tcrypt_complete;
+
+	if (crypt_req->op & TEGRA_CRYPTO_ECB) {
+		req = ablkcipher_request_alloc(ctx->ecb_tfm, GFP_KERNEL);
+		tfm = ctx->ecb_tfm;
+	} else if (crypt_req->op & TEGRA_CRYPTO_CBC) {
+		req = ablkcipher_request_alloc(ctx->cbc_tfm, GFP_KERNEL);
+		tfm = ctx->cbc_tfm;
+	} else if ((crypt_req->op & TEGRA_CRYPTO_OFB) &&
+			(tegra_get_chipid() != TEGRA_CHIPID_TEGRA2)) {
+
+		req = ablkcipher_request_alloc(ctx->ofb_tfm, GFP_KERNEL);
+		tfm = ctx->ofb_tfm;
+	} else if ((crypt_req->op & TEGRA_CRYPTO_CTR) &&
+			(tegra_get_chipid() != TEGRA_CHIPID_TEGRA2)) {
+
+		req = ablkcipher_request_alloc(ctx->ctr_tfm, GFP_KERNEL);
+		tfm = ctx->ctr_tfm;
+	}
+
+	if (!req) {
+		pr_err("%s: Failed to allocate request\n", __func__);
+		return -ENOMEM;
+	}
+
+	if ((crypt_req->keylen < 0) || (crypt_req->keylen > AES_MAX_KEY_SIZE))
+		return -EINVAL;
+
+	crypto_ablkcipher_clear_flags(tfm, ~0);
+
+	if (!ctx->use_ssk)
+		key = crypt_req->key;
+
+	ret = crypto_ablkcipher_setkey(tfm, key, crypt_req->keylen);
+	if (ret < 0) {
+		pr_err("setkey failed");
+		goto process_req_out;
+	}
+
+	ret = alloc_bufs(xbuf);
+	if (ret < 0) {
+		pr_err("alloc_bufs failed");
+		goto process_req_out;
+	}
+
+	init_completion(&tcrypt_complete.restart);
+
+	ablkcipher_request_set_callback(req, CRYPTO_TFM_REQ_MAY_BACKLOG,
+		tegra_crypt_complete, &tcrypt_complete);
+
+	total = crypt_req->plaintext_sz;
+	while (total > 0) {
+		size = min(total, PAGE_SIZE);
+		ret = copy_from_user((void *)xbuf[0],
+			(void __user *)crypt_req->plaintext, size);
+		if (ret < 0) {
+			pr_debug("%s: copy_from_user failed (%d)\n", __func__, ret);
+			goto process_req_buf_out;
+		}
+		sg_init_one(&in_sg, xbuf[0], size);
+		sg_init_one(&out_sg, xbuf[1], size);
+
+		ablkcipher_request_set_crypt(req, &in_sg,
+			&out_sg, size, crypt_req->iv);
+
+		INIT_COMPLETION(tcrypt_complete.restart);
+		tcrypt_complete.req_err = 0;
+		ret = crypt_req->encrypt ?
+			crypto_ablkcipher_encrypt(req) :
+			crypto_ablkcipher_decrypt(req);
+
+		if ((ret == -EINPROGRESS) || (ret == -EBUSY)) {
+			/* crypto driver is asynchronous */
+			ret = wait_for_completion_interruptible(&tcrypt_complete.restart);
+
+			if (ret < 0)
+				goto process_req_buf_out;
+
+			if (tcrypt_complete.req_err < 0) {
+				ret = tcrypt_complete.req_err;
+				goto process_req_buf_out;
+			}
+		} else if (ret < 0) {
+			pr_debug("%scrypt failed (%d)\n",
+				crypt_req->encrypt ? "en" : "de", ret);
+			goto process_req_buf_out;
+		}
+
+		ret = copy_to_user((void __user *)crypt_req->result,
+			(const void *)xbuf[1], size);
+		if (ret < 0)
+			goto process_req_buf_out;
+
+		total -= size;
+		crypt_req->result += size;
+		crypt_req->plaintext += size;
+	}
+
+process_req_buf_out:
+	free_bufs(xbuf);
+process_req_out:
+	ablkcipher_request_free(req);
+
+	return ret;
+}
+
+static int sha_async_hash_op(struct ahash_request *req,
+				struct tegra_crypto_completion *tr,
+				int ret)
+{
+	if (ret == -EINPROGRESS || ret == -EBUSY) {
+		ret = wait_for_completion_interruptible(&tr->restart);
+		if (!ret)
+			ret = tr->req_err;
+		INIT_COMPLETION(tr->restart);
+	}
+	return ret;
+}
+
+static int tegra_crypto_sha(struct tegra_sha_req *sha_req)
+{
+
+	struct crypto_ahash *tfm;
+	struct scatterlist sg[1];
+	char result[64];
+	struct ahash_request *req;
+	struct tegra_crypto_completion sha_complete;
+	void *hash_buff;
+	unsigned long *xbuf[XBUFSIZE];
+	int ret = -ENOMEM;
+
+	tfm = crypto_alloc_ahash(sha_req->algo, 0, 0);
+	if (IS_ERR(tfm)) {
+		printk(KERN_ERR "alg: hash: Failed to load transform for %s: "
+		       "%ld\n", sha_req->algo, PTR_ERR(tfm));
+		goto out_alloc;
+	}
+
+	req = ahash_request_alloc(tfm, GFP_KERNEL);
+	if (!req) {
+		printk(KERN_ERR "alg: hash: Failed to allocate request for "
+		       "%s\n", sha_req->algo);
+		goto out_noreq;
+	}
+
+	ret = alloc_bufs(xbuf);
+	if (ret < 0) {
+		pr_err("alloc_bufs failed");
+		goto out_buf;
+	}
+
+	init_completion(&sha_complete.restart);
+
+	memset(result, 0, 64);
+
+	hash_buff = xbuf[0];
+
+	memcpy(hash_buff, sha_req->plaintext, sha_req->plaintext_sz);
+	sg_init_one(&sg[0], hash_buff, sha_req->plaintext_sz);
+
+	if (sha_req->keylen) {
+		crypto_ahash_clear_flags(tfm, ~0);
+		ret = crypto_ahash_setkey(tfm, sha_req->key,
+					  sha_req->keylen);
+		if (ret) {
+			printk(KERN_ERR "alg: hash: setkey failed on "
+			       " %s: ret=%d\n", sha_req->algo,
+			       -ret);
+			goto out;
+		}
+	}
+
+	ahash_request_set_crypt(req, sg, result, sha_req->plaintext_sz);
+
+	ret = sha_async_hash_op(req, &sha_complete, crypto_ahash_init(req));
+	if (ret) {
+		pr_err("alg: hash: init failed on "
+		       "for %s: ret=%d\n", sha_req->algo, -ret);
+		goto out;
+	}
+
+	ret = sha_async_hash_op(req, &sha_complete, crypto_ahash_update(req));
+	if (ret) {
+		pr_err("alg: hash: update failed on "
+		       "for %s: ret=%d\n", sha_req->algo, -ret);
+		goto out;
+	}
+
+	ret = sha_async_hash_op(req, &sha_complete, crypto_ahash_final(req));
+	if (ret) {
+		pr_err("alg: hash: final failed on "
+		       "for %s: ret=%d\n", sha_req->algo, -ret);
+		goto out;
+	}
+
+	ret = copy_to_user((void __user *)sha_req->result,
+		(const void *)result, crypto_ahash_digestsize(tfm));
+	if (ret < 0)
+		goto out;
+
+out:
+	free_bufs(xbuf);
+
+out_buf:
+	ahash_request_free(req);
+
+out_noreq:
+	crypto_free_ahash(tfm);
+
+out_alloc:
+	return ret;
+
+}
+
+static long tegra_crypto_dev_ioctl(struct file *filp,
+	unsigned int ioctl_num, unsigned long arg)
+{
+	struct tegra_crypto_ctx *ctx = filp->private_data;
+	struct tegra_crypt_req crypt_req;
+	struct tegra_rng_req rng_req;
+	struct tegra_sha_req sha_req;
+	char *rng;
+	int ret = 0;
+
+	switch (ioctl_num) {
+	case TEGRA_CRYPTO_IOCTL_NEED_SSK:
+		ctx->use_ssk = (int)arg;
+		break;
+
+	case TEGRA_CRYPTO_IOCTL_PROCESS_REQ:
+		ret = copy_from_user(&crypt_req, (void __user *)arg, sizeof(crypt_req));
+		if (ret < 0) {
+			pr_err("%s: copy_from_user fail(%d)\n", __func__, ret);
+			break;
+		}
+
+		ret = process_crypt_req(ctx, &crypt_req);
+		break;
+
+	case TEGRA_CRYPTO_IOCTL_SET_SEED:
+		if (copy_from_user(&rng_req, (void __user *)arg, sizeof(rng_req)))
+			return -EFAULT;
+
+		memcpy(ctx->seed, rng_req.seed, TEGRA_CRYPTO_RNG_SEED_SIZE);
+
+		ret = crypto_rng_reset(ctx->rng, ctx->seed,
+			crypto_rng_seedsize(ctx->rng));
+		break;
+
+	case TEGRA_CRYPTO_IOCTL_GET_RANDOM:
+		if (copy_from_user(&rng_req, (void __user *)arg, sizeof(rng_req)))
+			return -EFAULT;
+
+		rng = kzalloc(rng_req.nbytes, GFP_KERNEL);
+		if (!rng) {
+			pr_err("mem alloc for rng fail");
+			ret = -ENODATA;
+			goto rng_out;
+		}
+
+		ret = crypto_rng_get_bytes(ctx->rng, rng,
+			rng_req.nbytes);
+
+		if (ret != rng_req.nbytes) {
+			pr_err("rng failed");
+			ret = -ENODATA;
+			goto rng_out;
+		}
+
+		ret = copy_to_user((void __user *)rng_req.rdata,
+			(const void *)rng, rng_req.nbytes);
+		ret = (ret < 0) ? -ENODATA : 0;
+rng_out:
+		if (rng)
+			kfree(rng);
+		break;
+
+	case TEGRA_CRYPTO_IOCTL_GET_SHA:
+		if (tegra_get_chipid() != TEGRA_CHIPID_TEGRA2) {
+			if (copy_from_user(&sha_req, (void __user *)arg,
+				sizeof(sha_req)))
+				return -EFAULT;
+			ret = tegra_crypto_sha(&sha_req);
+		} else {
+			ret = -EINVAL;
+		}
+		break;
+
+	default:
+		pr_debug("invalid ioctl code(%d)", ioctl_num);
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+struct file_operations tegra_crypto_fops = {
+	.owner = THIS_MODULE,
+	.open = tegra_crypto_dev_open,
+	.release = tegra_crypto_dev_release,
+	.unlocked_ioctl = tegra_crypto_dev_ioctl,
+};
+
+struct miscdevice tegra_crypto_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "tegra-crypto",
+	.fops = &tegra_crypto_fops,
+};
+
+static int __init tegra_crypto_dev_init(void)
+{
+	return misc_register(&tegra_crypto_device);
+}
+
+late_initcall(tegra_crypto_dev_init);
+
+MODULE_DESCRIPTION("Tegra AES hw device node.");
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_LICENSE("GPLv2");
diff --git a/drivers/misc/tegra-cryptodev.h b/drivers/misc/tegra-cryptodev.h
new file mode 100644
index 000000000000..34ec9c16aac2
--- /dev/null
+++ b/drivers/misc/tegra-cryptodev.h
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2010, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __TEGRA_CRYPTODEV_H
+#define __TEGRA_CRYPTODEV_H
+
+#include <crypto/aes.h>
+
+#include <asm-generic/ioctl.h>
+
+/* ioctl arg = 1 if you want to use ssk. arg = 0 to use normal key */
+#define TEGRA_CRYPTO_IOCTL_NEED_SSK	_IOWR(0x98, 100, int)
+#define TEGRA_CRYPTO_IOCTL_PROCESS_REQ	_IOWR(0x98, 101, int*)
+#define TEGRA_CRYPTO_IOCTL_SET_SEED	_IOWR(0x98, 102, int*)
+#define TEGRA_CRYPTO_IOCTL_GET_RANDOM	_IOWR(0x98, 103, int*)
+#define TEGRA_CRYPTO_IOCTL_GET_SHA	_IOWR(0x98, 104, int*)
+
+#define TEGRA_CRYPTO_MAX_KEY_SIZE	AES_MAX_KEY_SIZE
+#define TEGRA_CRYPTO_IV_SIZE	AES_BLOCK_SIZE
+#define DEFAULT_RNG_BLK_SZ	16
+
+/* the seed consists of 16 bytes of key + 16 bytes of init vector */
+#define TEGRA_CRYPTO_RNG_SEED_SIZE	AES_KEYSIZE_128 + DEFAULT_RNG_BLK_SZ
+#define TEGRA_CRYPTO_RNG_SIZE	SZ_16
+
+/* encrypt/decrypt operations */
+#define TEGRA_CRYPTO_ECB	BIT(0)
+#define TEGRA_CRYPTO_CBC	BIT(1)
+#define TEGRA_CRYPTO_OFB	BIT(2)
+#define TEGRA_CRYPTO_CTR	BIT(3)
+#define TEGRA_CRYPTO_CMAC	BIT(4)
+#define TEGRA_CRYPTO_RNG	BIT(5)
+
+/* a pointer to this struct needs to be passed to:
+ * TEGRA_CRYPTO_IOCTL_PROCESS_REQ
+ */
+struct tegra_crypt_req {
+	int op; /* e.g. TEGRA_CRYPTO_ECB */
+	bool encrypt;
+	char key[TEGRA_CRYPTO_MAX_KEY_SIZE];
+	int keylen;
+	char iv[TEGRA_CRYPTO_IV_SIZE];
+	int ivlen;
+	u8 *plaintext;
+	int plaintext_sz;
+	u8 *result;
+};
+
+/* pointer to this struct should be passed to:
+ * TEGRA_CRYPTO_IOCTL_SET_SEED
+ * TEGRA_CRYPTO_IOCTL_GET_RANDOM
+ */
+struct tegra_rng_req {
+	u8 seed[TEGRA_CRYPTO_RNG_SEED_SIZE];
+	u8 *rdata; /* random generated data */
+	int nbytes; /* random data length */
+};
+
+struct tegra_sha_req {
+	char key[TEGRA_CRYPTO_MAX_KEY_SIZE];
+	int keylen;
+	unsigned char *algo;
+	unsigned char *plaintext;
+	int plaintext_sz;
+	unsigned char *result;
+};
+
+#endif
diff --git a/drivers/misc/tegra-throughput.c b/drivers/misc/tegra-throughput.c
new file mode 100644
index 000000000000..d491ef8c06cf
--- /dev/null
+++ b/drivers/misc/tegra-throughput.c
@@ -0,0 +1,242 @@
+/*
+ * drivers/misc/throughput.c
+ *
+ * Copyright (C) 2012, NVIDIA 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, version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/kthread.h>
+#include <linux/ktime.h>
+#include <linux/miscdevice.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/spinlock.h>
+#include <linux/throughput_ioctl.h>
+#include <linux/nvhost.h>
+#include <mach/dc.h>
+
+#define DEFAULT_SYNC_RATE 60000 /* 60 Hz */
+
+static unsigned short target_frame_time;
+static unsigned short last_frame_time;
+static ktime_t last_flip;
+static unsigned int multiple_app_disable;
+static spinlock_t lock;
+
+static struct work_struct work;
+static int throughput_hint;
+
+static void set_throughput_hint(struct work_struct *work)
+{
+	/* notify throughput hint clients here */
+	nvhost_scale3d_set_throughput_hint(throughput_hint);
+}
+
+static int throughput_flip_callback(void)
+{
+	long timediff;
+	ktime_t now;
+
+	/* only register flips when a single app is active */
+	if (multiple_app_disable)
+		return NOTIFY_DONE;
+
+	now = ktime_get();
+	if (last_flip.tv64 != 0) {
+		timediff = (long) ktime_us_delta(now, last_flip);
+		if (timediff > (long) USHRT_MAX)
+			last_frame_time = USHRT_MAX;
+		else
+			last_frame_time = (unsigned short) timediff;
+
+		if (last_frame_time == 0) {
+			pr_warn("%s: flips %lld nsec apart\n",
+				__func__, now.tv64 - last_flip.tv64);
+			return NOTIFY_DONE;
+		}
+
+		throughput_hint =
+			((int) target_frame_time * 1000) / last_frame_time;
+
+		if (!work_pending(&work))
+			schedule_work(&work);
+	}
+	last_flip = now;
+
+	return NOTIFY_OK;
+}
+
+static int sync_rate;
+static int throughput_active_app_count;
+
+static void reset_target_frame_time(void)
+{
+	if (sync_rate == 0) {
+		sync_rate = tegra_dc_get_panel_sync_rate();
+
+		if (sync_rate == 0)
+			sync_rate = DEFAULT_SYNC_RATE;
+	}
+
+	target_frame_time = (unsigned short) (1000000000 / sync_rate);
+
+	pr_debug("%s: panel sync rate %d, target frame time %u\n",
+		__func__, sync_rate, target_frame_time);
+}
+
+static int callback_initialized;
+
+static int throughput_open(struct inode *inode, struct file *file)
+{
+	spin_lock(&lock);
+
+	if (!callback_initialized) {
+		callback_initialized = 1;
+		tegra_dc_set_flip_callback(throughput_flip_callback);
+	}
+
+	throughput_active_app_count++;
+	if (throughput_active_app_count > 1)
+		multiple_app_disable = 1;
+
+	spin_unlock(&lock);
+
+
+	pr_debug("throughput_open node %p file %p\n", inode, file);
+
+	return 0;
+}
+
+static int throughput_release(struct inode *inode, struct file *file)
+{
+	spin_lock(&lock);
+
+	throughput_active_app_count--;
+	if (throughput_active_app_count == 0) {
+		reset_target_frame_time();
+		multiple_app_disable = 0;
+		callback_initialized = 0;
+		tegra_dc_unset_flip_callback();
+	}
+
+	spin_unlock(&lock);
+
+	pr_debug("throughput_release node %p file %p\n", inode, file);
+
+	return 0;
+}
+
+static int throughput_set_target_fps(unsigned long arg)
+{
+	int disable;
+
+	pr_debug("%s: target fps %lu requested\n", __func__, arg);
+
+	disable = multiple_app_disable;
+
+	if (disable) {
+		pr_debug("%s: %d active apps, disabling fps usage\n",
+			__func__, throughput_active_app_count);
+		return 0;
+	}
+
+	if (arg == 0)
+		reset_target_frame_time();
+	else {
+		unsigned long frame_time = (1000000 / arg);
+
+		if (frame_time > USHRT_MAX)
+			frame_time = USHRT_MAX;
+
+		target_frame_time = (unsigned short) frame_time;
+	}
+
+	return 0;
+}
+
+static long
+throughput_ioctl(struct file *file,
+			  unsigned int cmd,
+			  unsigned long arg)
+{
+	int err = 0;
+
+	if ((_IOC_TYPE(cmd) != TEGRA_THROUGHPUT_MAGIC) ||
+		(_IOC_NR(cmd) == 0) ||
+		(_IOC_NR(cmd) > TEGRA_THROUGHPUT_IOCTL_MAXNR))
+		return -EFAULT;
+
+	switch (cmd) {
+	case TEGRA_THROUGHPUT_IOCTL_TARGET_FPS:
+		pr_debug("%s: TEGRA_THROUGHPUT_IOCTL_TARGET_FPS %lu\n",
+			__func__, arg);
+		err = throughput_set_target_fps(arg);
+		break;
+
+	default:
+		err = -ENOTTY;
+	}
+
+	return err;
+}
+
+static const struct file_operations throughput_user_fops = {
+	.owner			= THIS_MODULE,
+	.open			= throughput_open,
+	.release		= throughput_release,
+	.unlocked_ioctl		= throughput_ioctl,
+};
+
+#define TEGRA_THROUGHPUT_MINOR 1
+
+static struct miscdevice throughput_miscdev = {
+	.minor  = TEGRA_THROUGHPUT_MINOR,
+	.name   = "tegra-throughput",
+	.fops   = &throughput_user_fops,
+	.mode   = 0666,
+};
+
+int __init throughput_init_miscdev(void)
+{
+	int ret;
+
+	pr_debug("%s: initializing\n", __func__);
+
+	spin_lock_init(&lock);
+	INIT_WORK(&work, set_throughput_hint);
+
+	ret = misc_register(&throughput_miscdev);
+	if (ret) {
+		pr_err("can\'t reigster throughput miscdev"
+		       " (minor %d err %d)\n", TEGRA_THROUGHPUT_MINOR, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+module_init(throughput_init_miscdev);
+
+void __exit throughput_exit_miscdev(void)
+{
+	pr_debug("%s: exiting\n", __func__);
+
+	cancel_work_sync(&work);
+
+	misc_deregister(&throughput_miscdev);
+}
+
+module_exit(throughput_exit_miscdev);
+
diff --git a/drivers/misc/therm_est.c b/drivers/misc/therm_est.c
new file mode 100644
index 000000000000..e67fc664c7af
--- /dev/null
+++ b/drivers/misc/therm_est.c
@@ -0,0 +1,139 @@
+/*
+ * drivers/misc/therm_est.c
+ *
+ * Copyright (C) 2010-2012 NVIDIA Corporation.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/kernel.h>
+#include <linux/cpufreq.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+#include <linux/slab.h>
+#include <linux/syscalls.h>
+#include <linux/therm_est.h>
+
+int therm_est_get_temp(struct therm_estimator *est, long *temp)
+{
+	*temp = est->cur_temp;
+	return 0;
+}
+
+int therm_est_set_limits(struct therm_estimator *est,
+				long lo_limit,
+				long hi_limit)
+{
+	est->therm_est_lo_limit = lo_limit;
+	est->therm_est_hi_limit = hi_limit;
+	return 0;
+}
+
+int therm_est_set_alert(struct therm_estimator *est,
+			void (*cb)(void *),
+			void *cb_data)
+{
+	if ((!cb) || est->callback)
+		BUG();
+
+	est->callback = cb;
+	est->callback_data = cb_data;
+
+	return 0;
+}
+
+static void therm_est_work_func(struct work_struct *work)
+{
+	int i, j, index, sum = 0;
+	long temp;
+	struct delayed_work *dwork = container_of (work,
+					struct delayed_work, work);
+	struct therm_estimator *est = container_of(
+					dwork,
+					struct therm_estimator,
+					therm_est_work);
+
+	for (i = 0; i < est->ndevs; i++) {
+		if (est->devs[i]->get_temp(est->devs[i]->dev_data, &temp))
+			continue;
+		est->devs[i]->hist[(est->ntemp % HIST_LEN)] = temp;
+	}
+
+	for (i = 0; i < est->ndevs; i++) {
+		for (j = 0; j < HIST_LEN; j++) {
+			index = (est->ntemp - j + HIST_LEN) % HIST_LEN;
+			sum += est->devs[i]->hist[index] *
+				est->devs[i]->coeffs[j];
+		}
+	}
+
+	est->cur_temp = sum / 100 + est->toffset;
+
+	est->ntemp++;
+
+	if (est->callback && ((est->cur_temp >= est->therm_est_hi_limit) ||
+			 (est->cur_temp <= est->therm_est_lo_limit)))
+		est->callback(est->callback_data);
+
+	queue_delayed_work(est->workqueue, &est->therm_est_work,
+				msecs_to_jiffies(est->polling_period));
+}
+
+struct therm_estimator *therm_est_register(
+			struct therm_est_subdevice **devs,
+			int ndevs,
+			long toffset,
+			long polling_period)
+{
+	int i, j;
+	long temp;
+	struct therm_estimator *est;
+	struct therm_est_subdevice *dev;
+
+	est = kzalloc(sizeof(struct therm_estimator), GFP_KERNEL);
+	if (IS_ERR_OR_NULL(est))
+		return ERR_PTR(-ENOMEM);
+
+	est->devs = devs;
+	est->ndevs = ndevs;
+	est->toffset = toffset;
+	est->polling_period = polling_period;
+
+	/* initialize history */
+	for (i = 0; i < ndevs; i++) {
+		dev = est->devs[i];
+
+		if (dev->get_temp(dev->dev_data, &temp)) {
+			kfree(est);
+			return ERR_PTR(-EINVAL);
+		}
+
+		for (j = 0; j < HIST_LEN; j++) {
+			dev->hist[j] = temp;
+		}
+	}
+
+	est->workqueue = alloc_workqueue("therm_est",
+				    WQ_HIGHPRI | WQ_UNBOUND | WQ_RESCUER, 1);
+	INIT_DELAYED_WORK(&est->therm_est_work, therm_est_work_func);
+
+	queue_delayed_work(est->workqueue,
+				&est->therm_est_work,
+				msecs_to_jiffies(est->polling_period));
+
+	return est;
+}
diff --git a/drivers/misc/ti-st/Kconfig b/drivers/misc/ti-st/Kconfig
index abb5de1afce3..8f20ddaf0949 100644
--- a/drivers/misc/ti-st/Kconfig
+++ b/drivers/misc/ti-st/Kconfig
@@ -14,4 +14,11 @@ config TI_ST
 	  are returned to relevant protocol drivers based on their
 	  packet types.
 
+config ST_GPS
+	tristate "gps driver for ST"
+	select TI_ST
+	help
+	  This enables the GPS driver for TI WL128x BT/FM/GPS combo devices.
+	  It will provide a character device for the TI GPS host software to
+	  access the GPS core on the WL128x.
 endmenu
diff --git a/drivers/misc/ti-st/Makefile b/drivers/misc/ti-st/Makefile
index 78d7ebb14749..3f6d990912c2 100644
--- a/drivers/misc/ti-st/Makefile
+++ b/drivers/misc/ti-st/Makefile
@@ -4,3 +4,4 @@
 #
 obj-$(CONFIG_TI_ST) 		+= st_drv.o
 st_drv-objs			:= st_core.o st_kim.o st_ll.o
+obj-$(CONFIG_ST_GPS)            += gps_drv.o
diff --git a/drivers/misc/ti-st/gps_drv.c b/drivers/misc/ti-st/gps_drv.c
new file mode 100644
index 000000000000..17ca92dd177b
--- /dev/null
+++ b/drivers/misc/ti-st/gps_drv.c
@@ -0,0 +1,804 @@
+/*
+ *   GPS Char Driver for Texas Instrument's Connectivity Chip.
+ *   Copyright (C) 2009 Texas Instruments
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms 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/cdev.h>
+#include <linux/fs.h>
+#include <linux/device.h>
+
+#include <linux/uaccess.h>
+#include <linux/tty.h>
+#include <linux/sched.h>
+
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/platform_device.h>
+#include <linux/poll.h>
+#include <linux/skbuff.h>
+#include <linux/interrupt.h>
+
+#include <linux/ti_wilink_st.h>
+
+#undef VERBOSE
+#undef DEBUG
+
+/* Debug macros*/
+#if defined(DEBUG)		/* limited debug messages */
+#define GPSDRV_DBG(fmt, arg...)  \
+	printk(KERN_INFO "[GPS] (gpsdrv):"fmt"\n" , ## arg)
+#define GPSDRV_VER(fmt, arg...)
+#elif defined(VERBOSE)		/* very verbose */
+#define GPSDRV_DBG(fmt, arg...)  \
+	printk(KERN_INFO "[GPS] (gpsdrv):"fmt"\n" , ## arg)
+#define GPSDRV_VER(fmt, arg...)  \
+	printk(KERN_INFO "[GPS] (gpsdrv):"fmt"\n" , ## arg)
+#define GPSDRV_ERR(fmt, arg...)  \
+	printk(KERN_ERR "[GPS] (gpsdrv):"fmt"\n" , ## arg)
+#else /* Error msgs only */
+#define GPSDRV_ERR(fmt, arg...)  \
+	printk(KERN_ERR "[GPS] (gpsdrv):"fmt"\n" , ## arg)
+#define GPSDRV_VER(fmt, arg...)
+#define GPSDRV_DBG(fmt, arg...)
+#endif
+
+static void gpsdrv_tsklet_write(unsigned long data);
+
+/* List of error codes returned by the gps driver*/
+enum {
+	GPS_ERR_FAILURE = -1,	/* check struct */
+	GPS_SUCCESS,
+	GPS_ERR_CLASS = -15,
+	GPS_ERR_CPY_TO_USR,
+	GPS_ERR_CPY_FRM_USR,
+	GPS_ERR_UNKNOWN,
+};
+
+/* Channel-9 details for GPS */
+#define GPS_CH9_PKT_HDR_SIZE		4
+#define GPS_CH9_PKT_NUMBER		0x9
+#define GPS_CH9_OP_WRITE		0x1
+#define GPS_CH9_OP_READ			0x2
+#define GPS_CH9_OP_COMPLETED_EVT	0x3
+
+/* Macros for Syncronising GPS registration and other R/W/ICTL operations */
+#define GPS_ST_REGISTERED	0
+#define GPS_ST_RUNNING		1
+
+/* Read time out defined to 10 seconds */
+#define GPSDRV_READ_TIMEOUT	10000
+/* Reg time out defined to 6 seconds */
+#define GPSDRV_REG_TIMEOUT	6000
+
+
+struct gpsdrv_event_hdr {
+	uint8_t opcode;
+	uint16_t plen;
+} __packed;
+
+/*
+ * struct gpsdrv_data - gps internal driver data
+ * @gpsdrv_reg_completed - completion to wait for registration
+ * @streg_cbdata - registration feedback
+ * @state - driver state
+ * @tx_count - TX throttling/unthrottling
+ * @st_write - write ptr from ST
+ * @rx_list - Rx data SKB queue
+ * @tx_list - Tx data SKB queue
+ * @gpsdrv_data_q - dataq checked up on poll/receive
+ * @lock - spin lock
+ * @gpsdrv_tx_tsklet - gps write task
+ */
+
+struct gpsdrv_data {
+	struct completion gpsdrv_reg_completed;
+	char streg_cbdata;
+	unsigned long state;
+	unsigned char tx_count;
+	long (*st_write) (struct sk_buff *skb);
+	struct sk_buff_head rx_list;
+	struct sk_buff_head tx_list;
+	wait_queue_head_t gpsdrv_data_q;
+	spinlock_t lock;
+	struct tasklet_struct gpsdrv_tx_tsklet;
+};
+
+#define DEVICE_NAME     "tigps"
+
+/***********Functions called from ST driver**********************************/
+
+/*  gpsdrv_st_recv Function
+ *  This is Called in from -- ST Core when a data is received
+ *  This is a registered callback with ST core when the gps driver registers
+ *  with ST.
+ *
+ *  Parameters:
+ *  @skb    : SKB buffer pointer which contains the incoming Ch-9 GPS data.
+ *  Returns:
+ *          GPS_SUCCESS - On Success
+ *          else suitable error code
+ */
+long gpsdrv_st_recv(void *arg, struct sk_buff *skb)
+{
+	struct gpsdrv_event_hdr gpsdrv_hdr = { 0x00, 0x0000 };
+	struct gpsdrv_data *hgps = (struct gpsdrv_data *)arg;
+
+	/* SKB is NULL */
+	if (NULL == skb) {
+		GPSDRV_ERR("Input SKB is NULL");
+		return GPS_ERR_FAILURE;
+	}
+
+	/* Sanity Check - To Check if the Rx Pkt is Channel -9 or not */
+	if (0x09 != skb->cb[0]) {
+		GPSDRV_ERR("Input SKB is not a Channel-9 packet");
+		return GPS_ERR_FAILURE;
+	}
+	/* Copy Ch-9 info to local structure */
+	memcpy(&gpsdrv_hdr, skb->data, GPS_CH9_PKT_HDR_SIZE - 1);
+	skb_pull(skb, GPS_CH9_PKT_HDR_SIZE - 1);
+
+	/* check if skb->len and gpsdrv_hdr.plen are equal */
+	if (skb->len != gpsdrv_hdr.plen) {
+		GPSDRV_ERR("Received corrupted packet - Length Mismatch");
+		return -EINVAL;
+	}
+#ifdef VERBOSE
+	printk(KERN_INFO"data start >>\n");
+	print_hex_dump(KERN_INFO, ">in>", DUMP_PREFIX_NONE,
+			16, 1, skb->data, skb->len, 0);
+	printk(KERN_INFO"\n<< end\n");
+#endif
+	/* Check the Opcode */
+	if ((gpsdrv_hdr.opcode != GPS_CH9_OP_READ) && (gpsdrv_hdr.opcode != \
+				GPS_CH9_OP_COMPLETED_EVT)) {
+		GPSDRV_ERR("Rec corrupt pkt opcode %x", gpsdrv_hdr.opcode);
+		return -EINVAL;
+	}
+
+	/* Strip Channel 9 packet information from SKB only
+	 * if the opcode is GPS_CH9_OP_READ and get AI2 packet
+	 */
+	if (GPS_CH9_OP_READ == gpsdrv_hdr.opcode) {
+		skb_queue_tail(&hgps->rx_list, skb);
+		wake_up_interruptible(&hgps->gpsdrv_data_q);
+	} else {
+		spin_lock(&hgps->lock);
+		/* The no. of Completed Packets is always 1
+		 * in case of Channel 9 as per spec.
+		 * Forcing it to 1 for precaution.
+		 */
+		hgps->tx_count = 1;
+		/* Check if Tx queue and Tx count not empty */
+		if (!skb_queue_empty(&hgps->tx_list)) {
+			/* Schedule the Tx-task let */
+			spin_unlock(&hgps->lock);
+			GPSDRV_VER(" Scheduling tasklet to write");
+			tasklet_schedule(&hgps->gpsdrv_tx_tsklet);
+		} else {
+			spin_unlock(&hgps->lock);
+		}
+
+		GPSDRV_VER(" Tx count = %x", hgps->tx_count);
+		/* Free the received command complete SKB */
+		kfree_skb(skb);
+	}
+
+	return GPS_SUCCESS;
+
+}
+
+/*  gpsdrv_st_cb Function
+ *  This is Called in from -- ST Core when the state is pending during
+ *  st_register. This is a registered callback with ST core when the gps
+ *  driver registers with ST.
+ *
+ *  Parameters:
+ *  @data   Status update of GPS registration
+ *  Returns: NULL
+ */
+void gpsdrv_st_cb(void *arg, char data)
+{
+	struct gpsdrv_data *hgps = (struct gpsdrv_data *)arg;
+
+	GPSDRV_DBG(" Inside %s", __func__);
+	hgps->streg_cbdata = data;	/* ST registration callback  status */
+	complete_all(&hgps->gpsdrv_reg_completed);
+	return;
+}
+
+static struct st_proto_s gpsdrv_proto = {
+	.chnl_id = 0x09,
+	.max_frame_size = 1024,
+	.hdr_len = 3,
+	.offset_len_in_hdr = 1,
+	.len_size = 2,
+	.reserve = 1,
+	.recv = gpsdrv_st_recv,
+	.reg_complete_cb = gpsdrv_st_cb,
+};
+
+/** gpsdrv_tsklet_write Function
+ *  This tasklet function will be scheduled when there is a data in Tx queue
+ *  and GPS chip sent an command completion packet with non zero value.
+ *
+ *  Parameters :
+ *  @data  : data passed to tasklet function
+ *  Returns : NULL
+ */
+void gpsdrv_tsklet_write(unsigned long data)
+{
+	struct sk_buff *skb = NULL;
+	struct gpsdrv_data *hgps = (struct gpsdrv_data *)data;
+
+	GPSDRV_DBG(" Inside %s", __func__);
+
+	spin_lock(&hgps->lock);
+
+	/* Perform sanity check of verifying the status
+	   to perform an st_write */
+	if (((!hgps->st_write) || (0 == hgps->tx_count))
+			|| ((skb_queue_empty(&hgps->tx_list)))) {
+		spin_unlock(&hgps->lock);
+		GPSDRV_ERR("Sanity check Failed exiting %s", __func__);
+		return;
+	}
+	/* hgps->tx_list not empty skb already present
+	 * dequeue the tx-data and perform a st_write
+	 */
+	hgps->tx_count--;
+	spin_unlock(&hgps->lock);
+	GPSDRV_VER(" Tx count in gpsdrv_tsklet_write = %x", hgps->tx_count);
+	skb = skb_dequeue(&hgps->tx_list);
+	hgps->st_write(skb);
+
+	return;
+}
+
+/*********Functions Called from GPS host***************************************/
+
+/** gpsdrv_open Function
+ *  This function will perform an register on ST driver.
+ *
+ *  Parameters :
+ *  @file  : File pointer for GPS char driver
+ *  @inod  :
+ *  Returns  GPS_SUCCESS -  on success
+ *           else suitable error code
+ */
+int gpsdrv_open(struct inode *inod, struct file *file)
+{
+	int ret = 0;
+	unsigned long timeout = GPSDRV_REG_TIMEOUT;
+	struct gpsdrv_data *hgps;
+
+	GPSDRV_DBG(" Inside %s", __func__);
+
+	/* Allocate local resource memory */
+	hgps = kzalloc(sizeof(struct gpsdrv_data), GFP_KERNEL);
+	if (!(hgps)) {
+		GPSDRV_ERR("Can't allocate GPS data structure");
+		return -ENOMEM;
+	}
+
+	/* Initialize wait queue, skb queue head and
+	 * registration complete strucuture
+	 */
+	skb_queue_head_init(&hgps->rx_list);
+	skb_queue_head_init(&hgps->tx_list);
+	init_completion(&hgps->gpsdrv_reg_completed);
+	init_waitqueue_head(&hgps->gpsdrv_data_q);
+	spin_lock_init(&hgps->lock);
+
+	/* Check if GPS is already registered with ST */
+	if (test_and_set_bit(GPS_ST_REGISTERED, &hgps->state)) {
+		GPSDRV_ERR("GPS Registered/Registration in progress with ST"
+				" ,open called again?");
+		kfree(hgps);
+		return -EAGAIN;
+	}
+
+	/* Initialize  gpsdrv_reg_completed so as to wait for completion
+	 * on the same
+	 * if st_register returns with a PENDING status
+	 */
+	INIT_COMPLETION(hgps->gpsdrv_reg_completed);
+
+	gpsdrv_proto.priv_data = hgps;
+	/* Resgister GPS with ST */
+	ret = st_register(&gpsdrv_proto);
+	GPSDRV_VER(" st_register returned %d", ret);
+
+	/* If GPS Registration returned with error, then clear GPS_ST_REGISTERED
+	 * for future open calls and return the appropriate error code
+	 */
+	if (ret < 0 && ret != -EINPROGRESS) {
+		GPSDRV_ERR(" st_register failed");
+		clear_bit(GPS_ST_REGISTERED, &hgps->state);
+		if (ret == -EINPROGRESS)
+			return -EAGAIN;
+		return GPS_ERR_FAILURE;
+	}
+
+	/* if returned status is pending, wait for the completion */
+	if (ret == -EINPROGRESS) {
+		GPSDRV_VER(" GPS Register waiting for completion ");
+		timeout = wait_for_completion_timeout \
+		(&hgps->gpsdrv_reg_completed, msecs_to_jiffies(timeout));
+		/* Check for timed out condition */
+		if (0 == timeout) {
+			GPSDRV_ERR("GPS Device registration timed out");
+			clear_bit(GPS_ST_REGISTERED, &hgps->state);
+			return -ETIMEDOUT;
+		} else if (0 > hgps->streg_cbdata) {
+			GPSDRV_ERR("GPS Device Registration Failed-ST\n");
+			GPSDRV_ERR("RegCB called with ");
+			GPSDRV_ERR("Invalid value %d\n", hgps->streg_cbdata);
+			clear_bit(GPS_ST_REGISTERED, &hgps->state);
+			return -EAGAIN;
+		}
+	}
+	GPSDRV_DBG(" gps registration complete ");
+
+	/* Assign the write callback pointer */
+	hgps->st_write = gpsdrv_proto.write;
+	hgps->tx_count = 1;
+	file->private_data = hgps;	/* set drv data */
+	tasklet_init(&hgps->gpsdrv_tx_tsklet, (void *)gpsdrv_tsklet_write,
+			(unsigned long)hgps);
+	set_bit(GPS_ST_RUNNING, &hgps->state);
+
+	return GPS_SUCCESS;
+}
+
+/** gpsdrv_release Function
+ *  This function will un-registers from the ST driver.
+ *
+ *  Parameters :
+ *  @file  : File pointer for GPS char driver
+ *  @inod  :
+ *  Returns  GPS_SUCCESS -  on success
+ *           else suitable error code
+ */
+int gpsdrv_release(struct inode *inod, struct file *file)
+{
+	struct gpsdrv_data *hgps = file->private_data;
+
+	GPSDRV_DBG(" Inside %s", __func__);
+
+	/* Disabling task-let 1st & then un-reg to avoid
+	 * tasklet getting scheduled
+	 */
+	tasklet_disable(&hgps->gpsdrv_tx_tsklet);
+	tasklet_kill(&hgps->gpsdrv_tx_tsklet);
+	/* Cleat registered bit if already registered */
+	if (test_and_clear_bit(GPS_ST_REGISTERED, &hgps->state)) {
+		if (st_unregister(&gpsdrv_proto) < 0) {
+			GPSDRV_ERR(" st_unregister failed");
+			/* Re-Enable the task-let if un-register fails */
+			tasklet_enable(&hgps->gpsdrv_tx_tsklet);
+			return GPS_ERR_FAILURE;
+		}
+	}
+
+	/* Reset Tx count value and st_write function pointer */
+	hgps->tx_count = 0;
+	hgps->st_write = NULL;
+	clear_bit(GPS_ST_RUNNING, &hgps->state);
+	GPSDRV_VER(" st_unregister success");
+
+	skb_queue_purge(&hgps->rx_list);
+	skb_queue_purge(&hgps->tx_list);
+	kfree(hgps);
+	file->private_data = NULL;
+
+	return GPS_SUCCESS;
+}
+
+/** gpsdrv_read Function
+ *  This function will wait till the data received from the ST driver
+ *  and then strips the GPS-Channel-9 header from the
+ *  incoming AI2 packet and then send it to GPS host application.
+ *
+ *  Parameters :
+ *  @file  : File pointer for GPS char driver
+ *  @data  : Data which needs to be passed to APP
+ *  @size  : Length of the data passesd
+ *  offset :
+ *  Returns  Size of AI2 packet received -  on success
+ *           else suitable error code
+ */
+ssize_t gpsdrv_read(struct file *file, char __user *data, size_t size,
+		loff_t *offset)
+{
+	int len = 0, error = 0;
+	struct sk_buff *skb = NULL;
+	unsigned long timeout = GPSDRV_READ_TIMEOUT;
+	struct gpsdrv_data *hgps;
+
+	GPSDRV_DBG(" Inside %s", __func__);
+
+	/* Validate input parameters */
+	if ((NULL == file) || (((NULL == data) || (0 == size)))) {
+		GPSDRV_ERR("Invalid input params passed to %s", __func__);
+		return -EINVAL;
+	}
+
+	hgps = file->private_data;
+	/* Check if GPS is registered to perform read operation */
+	if (!test_bit(GPS_ST_RUNNING, &hgps->state)) {
+		GPSDRV_ERR("GPS Device is not running");
+		return -EINVAL;
+	}
+
+	/* cannot come here if poll-ed before reading
+	 * if not poll-ed wait on the same wait_q
+	 */
+	timeout = wait_event_interruptible_timeout(hgps->gpsdrv_data_q,
+		!skb_queue_empty(&hgps->rx_list), msecs_to_jiffies(timeout));
+	/* Check for timed out condition */
+	if (0 == timeout) {
+		GPSDRV_ERR("GPS Device Read timed out");
+		return -ETIMEDOUT;
+	}
+
+
+	/* hgps->rx_list not empty skb already present */
+	skb = skb_dequeue(&hgps->rx_list);
+
+	if (!skb) {
+		GPSDRV_ERR("Dequed SKB is NULL?");
+		return GPS_ERR_UNKNOWN;
+	} else if (skb->len > size) {
+		GPSDRV_DBG("SKB length is Greater than requested size ");
+		GPSDRV_DBG("Returning the available length of SKB\n");
+
+		error = copy_to_user(data, skb->data, size);
+		skb_pull(skb, size);
+
+		if (skb->len != 0)
+			skb_queue_head(&hgps->rx_list, skb);
+
+		/* printk(KERN_DEBUG  "gpsdrv: total size read= %d", size);*/
+		return size;
+	}
+
+#ifdef VERBOSE
+	print_hex_dump(KERN_INFO, ">in>", DUMP_PREFIX_NONE,
+			16, 1, skb->data, skb->len, 0);
+#endif
+
+	/* Forward the data to the user */
+	if (skb->len <= size) {
+		if (copy_to_user(data, skb->data, skb->len)) {
+			GPSDRV_ERR(" Unable to copy to user space");
+			/* Queue the skb back to head */
+			skb_queue_head(&hgps->rx_list, skb);
+			return GPS_ERR_CPY_TO_USR;
+		}
+	}
+
+	len = skb->len;
+	kfree_skb(skb);
+	/* printk(KERN_DEBUG  "gpsdrv: total size read= %d", len); */
+	return len;
+}
+/* gpsdrv_write Function
+ *  This function will pre-pend the GPS-Channel-9 header to the
+ *  incoming AI2 packet sent from the GPS host application.
+ *
+ *  Parameters :
+ *  @file   : File pointer for GPS char driver
+ *  @data   : AI2 packet data from GPS application
+ *  @size   : Size of the AI2 packet data
+ *  @offset :
+ *  Returns  Size of AI2 packet on success
+ *           else suitable error code
+ */
+ssize_t gpsdrv_write(struct file *file, const char __user *data,
+		size_t size, loff_t *offset)
+{
+	unsigned char channel = GPS_CH9_PKT_NUMBER; /* GPS Channel number */
+	/* Initialize gpsdrv_event_hdr with write opcode */
+	struct gpsdrv_event_hdr gpsdrv_hdr = { GPS_CH9_OP_WRITE, 0x0000 };
+	struct sk_buff *skb = NULL;
+	struct gpsdrv_data *hgps;
+
+	GPSDRV_DBG(" Inside %s", __func__);
+	/* Validate input parameters */
+	if ((NULL == file) || (((NULL == data) || (0 == size)))) {
+		GPSDRV_ERR("Invalid input params passed to %s", __func__);
+		return -EINVAL;
+	}
+
+	hgps = file->private_data;
+
+	/* Check if GPS is registered to perform write operation */
+	if (!test_bit(GPS_ST_RUNNING, &hgps->state)) {
+		GPSDRV_ERR("GPS Device is not running");
+		return -EINVAL;
+	}
+
+	if (!hgps->st_write) {
+		GPSDRV_ERR(" Can't write to ST, hgps->st_write null ?");
+		return -EINVAL;
+	}
+
+
+	skb = alloc_skb(size + GPS_CH9_PKT_HDR_SIZE, GFP_ATOMIC);
+	/* Validate Created SKB */
+	if (NULL == skb) {
+		GPSDRV_ERR("Error aaloacting SKB");
+		return -ENOMEM;
+	}
+
+	/* Update chnl-9 information with plen=AI2 pckt size which is "size"*/
+	gpsdrv_hdr.plen = size;
+
+	/* PrePend Channel-9 header to AI2 packet and write to SKB */
+	memcpy(skb_put(skb, 1), &channel, 1);
+	memcpy(skb_put(skb, GPS_CH9_PKT_HDR_SIZE - 1), &gpsdrv_hdr,
+			GPS_CH9_PKT_HDR_SIZE - 1);
+
+	/* Forward the data from the user space to ST core */
+	if (copy_from_user(skb_put(skb, size), data, size)) {
+		GPSDRV_ERR(" Unable to copy from user space");
+		kfree_skb(skb);
+		return GPS_ERR_CPY_FRM_USR;
+	}
+
+#ifdef VERBOSE
+	GPSDRV_VER("start data..");
+	print_hex_dump(KERN_INFO, "<out<", DUMP_PREFIX_NONE,
+			16, 1, skb->data, size, 0);
+	GPSDRV_VER("\n..end data");
+#endif
+
+	/* Check if data can be sent to GPS chip
+	 * If not, add it to queue and that can be sent later
+	 */
+	spin_lock(&hgps->lock);
+	if (0 != hgps->tx_count) {
+		/* If TX Q is empty send current SKB;
+		 *  else, queue current SKB at end of tx_list queue and
+		 *  send first SKB in tx_list queue.
+		 */
+		hgps->tx_count--;
+		spin_unlock(&hgps->lock);
+
+		GPSDRV_VER(" Tx count in gpsdrv_write = %x", hgps->tx_count);
+
+		if (skb_queue_empty(&hgps->tx_list)) {
+			hgps->st_write(skb);
+		} else {
+			skb_queue_tail(&hgps->tx_list, skb);
+			hgps->st_write(skb_dequeue(&hgps->tx_list));
+		}
+	} else {
+		/* Add it to TX queue */
+		spin_unlock(&hgps->lock);
+		GPSDRV_VER(" SKB added to Tx queue ");
+		GPSDRV_VER("Tx count = %x ", hgps->tx_count);
+		skb_queue_tail(&hgps->tx_list, skb);
+		/* This is added for precaution in the case that there is a
+		 * context switch during the execution of the above lines.
+		 * Redundant otherwise.
+		 */
+		if ((0 != hgps->tx_count) && \
+				(!skb_queue_empty(&hgps->tx_list))) {
+			/* Schedule the Tx-task let */
+			GPSDRV_VER("Scheduling tasklet to write\n");
+			tasklet_schedule(&hgps->gpsdrv_tx_tsklet);
+		}
+	}
+
+	return size;
+}
+
+/** gpsdrv_ioctl Function
+ *  This will peform the functions as directed by the command and command
+ *  argument.
+ *
+ *  Parameters :
+ *  @file  : File pointer for GPS char driver
+ *  @cmd   : IOCTL Command
+ *  @arg   : Command argument for IOCTL command
+ *  Returns  GPS_SUCCESS on success
+ *           else suitable error code
+ */
+static long gpsdrv_ioctl(struct file *file,
+		unsigned int cmd, unsigned long arg)
+{
+	struct sk_buff *skb = NULL;
+	int		retCode = GPS_SUCCESS;
+	struct gpsdrv_data *hgps;
+
+	GPSDRV_DBG(" Inside %s", __func__);
+
+	/* Validate input parameters */
+	if ((NULL == file) || (0 == cmd)) {
+		GPSDRV_ERR("Invalid input parameters passed to %s", __func__);
+		return -EINVAL;
+	}
+
+	hgps = file->private_data;
+
+	/* Check if GPS is registered to perform IOCTL operation */
+	if (!test_bit(GPS_ST_RUNNING, &hgps->state)) {
+		GPSDRV_ERR("GPS Device is not running");
+		return -EINVAL;
+	}
+
+	switch (cmd) {
+	case TCFLSH:
+		GPSDRV_VER(" IOCTL TCFLSH invoked with %ld argument", arg);
+		switch (arg) {
+			/* purge Rx/Tx SKB list queues depending on arg value */
+		case TCIFLUSH:
+			skb_queue_purge(&hgps->rx_list);
+			break;
+		case TCOFLUSH:
+			skb_queue_purge(&hgps->tx_list);
+			break;
+		case TCIOFLUSH:
+			skb_queue_purge(&hgps->rx_list);
+			skb_queue_purge(&hgps->tx_list);
+			break;
+		default:
+			GPSDRV_ERR("Invalid Command passed for tcflush");
+			retCode = 0;
+			break;
+		}
+		break;
+	case FIONREAD:
+		/* Deque the SKB from the head if rx_list is not empty
+		 * And update the argument with skb->len to provide
+		 * the amount of data available in the available SKB
+		 */
+		skb = skb_dequeue(&hgps->rx_list);
+		if (skb != NULL) {
+			*(unsigned int *)arg = skb->len;
+			/* Re-Store the SKB for furtur Read operations */
+			skb_queue_head(&hgps->rx_list, skb);
+		} else {
+			*(unsigned int *)arg = 0;
+		}
+		GPSDRV_DBG("returning %d\n", *(unsigned int *)arg);
+
+		break;
+	default:
+		GPSDRV_DBG("Un-Identified IOCTL %d", cmd);
+		retCode = 0;
+		break;
+	}
+
+	return retCode;
+}
+
+/** gpsdrv_poll Function
+ *  This function will wait till some data is received to the gps driver from ST
+ *
+ *  Parameters :
+ *  @file  : File pointer for GPS char driver
+ *  @wait  : POLL wait information
+ *  Returns  status of POLL on success
+ *           else suitable error code
+ */
+static unsigned int gpsdrv_poll(struct file *file, poll_table *wait)
+{
+	unsigned long mask = 0;
+	struct gpsdrv_data *hgps = file->private_data;
+
+	/* Check if GPS is registered to perform read operation */
+	if (!test_bit(GPS_ST_RUNNING, &hgps->state)) {
+		GPSDRV_ERR("GPS Device is not running");
+		return -EINVAL;
+	}
+
+	/* Wait till data is signalled from gpsdrv_st_recv function
+	 *  with AI2 packet
+	 */
+	poll_wait(file, &hgps->gpsdrv_data_q, wait);
+
+	if (!skb_queue_empty(&hgps->rx_list))
+		mask |= POLLIN;	/* TODO: check app for mask */
+
+	return mask;
+}
+
+/* GPS Char driver function pointers
+ * These functions are called from USER space by pefroming File Operations
+ * on /dev/gps node exposed by this driver during init
+ */
+const struct file_operations gpsdrv_chrdev_ops = {
+	.owner = THIS_MODULE,
+	.open = gpsdrv_open,
+	.read = gpsdrv_read,
+	.write = gpsdrv_write,
+	.unlocked_ioctl = gpsdrv_ioctl,
+	.poll = gpsdrv_poll,
+	.release = gpsdrv_release,
+};
+
+/*********Functions called during insmod and delmod****************************/
+
+static int gpsdrv_major;		/* GPS major number */
+static struct class *gpsdrv_class;	/* GPS class during class_create */
+static struct device *gpsdrv_dev;	/* GPS dev during device_create */
+/** gpsdrv_init Function
+ *  This function Initializes the gps driver parametes and exposes
+ *  /dev/gps node to user space
+ *
+ *  Parameters : NULL
+ *  Returns  GPS_SUCCESS on success
+ *           else suitable error code
+ */
+static int __init gpsdrv_init(void)
+{
+
+	GPSDRV_DBG(" Inside %s", __func__);
+
+	/* Expose the device DEVICE_NAME to user space
+	 * And obtain the major number for the device
+	 */
+	gpsdrv_major = register_chrdev(0, DEVICE_NAME, \
+			&gpsdrv_chrdev_ops);
+	if (0 > gpsdrv_major) {
+		GPSDRV_ERR("Error when registering to char dev");
+		return GPS_ERR_FAILURE;
+	}
+	GPSDRV_VER("allocated %d, %d", gpsdrv_major, 0);
+
+	/*  udev */
+	gpsdrv_class = class_create(THIS_MODULE, DEVICE_NAME);
+	if (IS_ERR(gpsdrv_class)) {
+		GPSDRV_ERR(" Something went wrong in class_create");
+		unregister_chrdev(gpsdrv_major, DEVICE_NAME);
+		return GPS_ERR_CLASS;
+	}
+
+	gpsdrv_dev =
+		device_create(gpsdrv_class, NULL, MKDEV(gpsdrv_major, 0),
+				NULL, DEVICE_NAME);
+	if (IS_ERR(gpsdrv_dev)) {
+		GPSDRV_ERR(" Error in class_create");
+		unregister_chrdev(gpsdrv_major, DEVICE_NAME);
+		class_destroy(gpsdrv_class);
+		return GPS_ERR_CLASS;
+	}
+
+	return GPS_SUCCESS;
+}
+
+/** gpsdrv_exit Function
+ *  This function Destroys the gps driver parametes and /dev/gps node
+ *
+ *  Parameters : NULL
+ *  Returns   NULL
+ */
+static void __exit gpsdrv_exit(void)
+{
+	GPSDRV_DBG(" Inside %s", __func__);
+	GPSDRV_VER(" Bye.. freeing up %d", gpsdrv_major);
+
+	device_destroy(gpsdrv_class, MKDEV(gpsdrv_major, 0));
+	class_destroy(gpsdrv_class);
+	unregister_chrdev(gpsdrv_major, DEVICE_NAME);
+}
+
+module_init(gpsdrv_init);
+module_exit(gpsdrv_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/ti-st/st_core.c b/drivers/misc/ti-st/st_core.c
index ba168a7d54d4..dde6d57dea55 100644
--- a/drivers/misc/ti-st/st_core.c
+++ b/drivers/misc/ti-st/st_core.c
@@ -50,7 +50,6 @@ static void remove_channel_from_table(struct st_data_s *st_gdata,
 		struct st_proto_s *proto)
 {
 	pr_info("%s: id %d\n", __func__, proto->chnl_id);
-/*	st_gdata->list[proto->chnl_id] = NULL; */
 	st_gdata->is_registered[proto->chnl_id] = false;
 }
 
@@ -123,7 +122,6 @@ void st_send_frame(unsigned char chnl_id, struct st_data_s *st_gdata)
 			(st_gdata->list[chnl_id]->priv_data, st_gdata->rx_skb)
 			     != 0)) {
 			pr_err(" proto stack %d's ->recv failed", chnl_id);
-			kfree_skb(st_gdata->rx_skb);
 			return;
 		}
 	} else {
@@ -338,15 +336,31 @@ void st_int_recv(void *disc_data,
 			/* Unknow packet? */
 		default:
 			type = *ptr;
-			if (st_gdata->list[type] == NULL) {
-				pr_err("chip/interface misbehavior dropping"
-					" frame starting with 0x%02x", type);
-				goto done;
 
+			/* Default case means non-HCILL packets,
+			 * possibilities are packets for:
+			 * (a) valid protocol -  Supported Protocols within
+			 *     the ST_MAX_CHANNELS.
+			 * (b) registered protocol - Checked by
+			 *     "st_gdata->list[type] == NULL)" are supported
+			 *     protocols only.
+			 *  Rules out any invalid protocol and
+			 *  unregistered protocols with channel ID < 16.
+			 */
+
+			if ((type >= ST_MAX_CHANNELS) ||
+					(st_gdata->list[type] == NULL)) {
+				pr_err("chip/interface misbehavior "
+						"dropping frame starting "
+						"with 0x%02x", type);
+				goto done;
 			}
 			st_gdata->rx_skb = alloc_skb(
 					st_gdata->list[type]->max_frame_size,
 					GFP_ATOMIC);
+			if (!st_gdata->rx_skb)
+				goto done;
+
 			skb_reserve(st_gdata->rx_skb,
 					st_gdata->list[type]->reserve);
 			/* next 2 required for BT only */
diff --git a/drivers/misc/ti-st/st_kim.c b/drivers/misc/ti-st/st_kim.c
index 3a3580566dfc..addb6c5a3a3b 100644
--- a/drivers/misc/ti-st/st_kim.c
+++ b/drivers/misc/ti-st/st_kim.c
@@ -35,6 +35,7 @@
 
 #include <linux/skbuff.h>
 #include <linux/ti_wilink_st.h>
+#include <linux/serial_core.h>
 
 
 #define MAX_ST_DEVICES	3	/* Imagine 1 on each UART for now */
@@ -450,9 +451,7 @@ long st_kim_start(void *kim_data)
 
 	do {
 		/* platform specific enabling code here */
-		if (pdata->chip_enable)
-			pdata->chip_enable(kim_gdata);
-
+		wake_lock(&kim_gdata->core_data->st_wk_lock);
 		/* Configure BT nShutdown to HIGH state */
 		gpio_set_value(kim_gdata->nshutdown, GPIO_LOW);
 		mdelay(5);	/* FIXME: a proper toggle */
@@ -516,8 +515,6 @@ long st_kim_stop(void *kim_data)
 {
 	long err = 0;
 	struct kim_data_s	*kim_gdata = (struct kim_data_s *)kim_data;
-	struct ti_st_plat_data	*pdata =
-		kim_gdata->kim_pdev->dev.platform_data;
 
 	INIT_COMPLETION(kim_gdata->ldisc_installed);
 
@@ -546,8 +543,7 @@ long st_kim_stop(void *kim_data)
 	gpio_set_value(kim_gdata->nshutdown, GPIO_LOW);
 
 	/* platform specific disable */
-	if (pdata->chip_disable)
-		pdata->chip_disable(kim_gdata);
+	wake_unlock(&kim_gdata->core_data->st_wk_lock);
 	return err;
 }
 
@@ -706,6 +702,8 @@ static int kim_probe(struct platform_device *pdev)
 	/* refer to itself */
 	kim_gdata->core_data->kim_data = kim_gdata;
 
+	wake_lock_init(&kim_gdata->core_data->st_wk_lock, WAKE_LOCK_SUSPEND,
+				"st_wake_lock");
 	/* Claim the chip enable nShutdown gpio from the system */
 	kim_gdata->nshutdown = pdata->nshutdown_gpio;
 	status = gpio_request(kim_gdata->nshutdown, "kim");
@@ -771,32 +769,41 @@ static int kim_remove(struct platform_device *pdev)
 	sysfs_remove_group(&pdev->dev.kobj, &uim_attr_grp);
 	pr_info("sysfs entries removed");
 
+	wake_lock_destroy(&kim_gdata->core_data->st_wk_lock);
 	kim_gdata->kim_pdev = NULL;
 	st_core_exit(kim_gdata->core_data);
 
 	kfree(kim_gdata);
 	kim_gdata = NULL;
+
 	return 0;
 }
 
 int kim_suspend(struct platform_device *pdev, pm_message_t state)
 {
-	struct ti_st_plat_data	*pdata = pdev->dev.platform_data;
+	struct kim_data_s *kim_gdata;
+	struct st_data_s *core_data;
+	struct uart_state *uart_state;
+	struct uart_port *uport;
 
-	if (pdata->suspend)
-		return pdata->suspend(pdev, state);
+	kim_gdata = dev_get_drvdata(&pdev->dev);
+	core_data = kim_gdata->core_data;
+
+	if (st_ll_getstate(core_data) != ST_LL_INVALID) {
+		uart_state = core_data->tty->driver_data;
+		uport = uart_state->uart_port;
+#ifdef CONFIG_BT_TIBLUESLEEP
+		pr_info(" Bluesleep Start");
+		bluesleep_start(uport);
+#endif
+	}
 
-	return -EOPNOTSUPP;
+	return 0;
 }
 
 int kim_resume(struct platform_device *pdev)
 {
-	struct ti_st_plat_data	*pdata = pdev->dev.platform_data;
-
-	if (pdata->resume)
-		return pdata->resume(pdev);
-
-	return -EOPNOTSUPP;
+	return 0;
 }
 
 /**********************************************************************/
diff --git a/drivers/misc/ti-st/st_ll.c b/drivers/misc/ti-st/st_ll.c
index 1ff460a8e9c7..82558a36ceb2 100644
--- a/drivers/misc/ti-st/st_ll.c
+++ b/drivers/misc/ti-st/st_ll.c
@@ -38,9 +38,6 @@ static void send_ll_cmd(struct st_data_s *st_data,
 
 static void ll_device_want_to_sleep(struct st_data_s *st_data)
 {
-	struct kim_data_s	*kim_data;
-	struct ti_st_plat_data	*pdata;
-
 	pr_debug("%s", __func__);
 	/* sanity check */
 	if (st_data->ll_state != ST_LL_AWAKE)
@@ -52,20 +49,20 @@ static void ll_device_want_to_sleep(struct st_data_s *st_data)
 	st_data->ll_state = ST_LL_ASLEEP;
 
 	/* communicate to platform about chip asleep */
-	kim_data = st_data->kim_data;
-	pdata = kim_data->kim_pdev->dev.platform_data;
-	if (pdata->chip_asleep)
-		pdata->chip_asleep(NULL);
+#ifdef CONFIG_WAKELOCK
+	wake_unlock(&st_data->st_wk_lock);
+#endif
 }
 
 static void ll_device_want_to_wakeup(struct st_data_s *st_data)
 {
-	struct kim_data_s	*kim_data;
-	struct ti_st_plat_data	*pdata;
-
 	/* diff actions in diff states */
 	switch (st_data->ll_state) {
 	case ST_LL_ASLEEP:
+		/* communicate to platform about chip wakeup */
+#ifdef CONFIG_WAKELOCK
+		wake_lock(&st_data->st_wk_lock);
+#endif
 		send_ll_cmd(st_data, LL_WAKE_UP_ACK);	/* send wake_ack */
 		break;
 	case ST_LL_ASLEEP_TO_AWAKE:
@@ -81,14 +78,9 @@ static void ll_device_want_to_wakeup(struct st_data_s *st_data)
 		pr_err("duplicate wake_ind");
 		break;
 	}
+
 	/* update state */
 	st_data->ll_state = ST_LL_AWAKE;
-
-	/* communicate to platform about chip wakeup */
-	kim_data = st_data->kim_data;
-	pdata = kim_data->kim_pdev->dev.platform_data;
-	if (pdata->chip_asleep)
-		pdata->chip_awake(NULL);
 }
 
 /**********************************************************************/
@@ -98,6 +90,10 @@ static void ll_device_want_to_wakeup(struct st_data_s *st_data)
  * enable ST LL */
 void st_ll_enable(struct st_data_s *ll)
 {
+        /* communicate to platform about chip enable */
+#ifdef CONFIG_WAKELOCK
+	wake_lock(&ll->st_wk_lock);
+#endif
 	ll->ll_state = ST_LL_AWAKE;
 }
 
@@ -105,13 +101,22 @@ void st_ll_enable(struct st_data_s *ll)
  * disable ST LL */
 void st_ll_disable(struct st_data_s *ll)
 {
+        /* communicate to platform about chip disable */
+#ifdef CONFIG_WAKELOCK
+	wake_unlock(&ll->st_wk_lock);
+#endif
 	ll->ll_state = ST_LL_INVALID;
 }
 
 /* called when ST Core wants to update the state */
 void st_ll_wakeup(struct st_data_s *ll)
 {
+
 	if (likely(ll->ll_state != ST_LL_AWAKE)) {
+		/* communicate to platform about chip wakeup */
+#ifdef CONFIG_WAKELOCK
+	wake_lock(&ll->st_wk_lock);
+#endif
 		send_ll_cmd(ll, LL_WAKE_UP_IND);	/* WAKE_IND */
 		ll->ll_state = ST_LL_ASLEEP_TO_AWAKE;
 	} else {
diff --git a/drivers/misc/uid_stat.c b/drivers/misc/uid_stat.c
new file mode 100644
index 000000000000..2141124a6c12
--- /dev/null
+++ b/drivers/misc/uid_stat.c
@@ -0,0 +1,156 @@
+/* drivers/misc/uid_stat.c
+ *
+ * Copyright (C) 2008 - 2009 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <asm/atomic.h>
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/proc_fs.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/stat.h>
+#include <linux/uid_stat.h>
+#include <net/activity_stats.h>
+
+static DEFINE_SPINLOCK(uid_lock);
+static LIST_HEAD(uid_list);
+static struct proc_dir_entry *parent;
+
+struct uid_stat {
+	struct list_head link;
+	uid_t uid;
+	atomic_t tcp_rcv;
+	atomic_t tcp_snd;
+};
+
+static struct uid_stat *find_uid_stat(uid_t uid) {
+	unsigned long flags;
+	struct uid_stat *entry;
+
+	spin_lock_irqsave(&uid_lock, flags);
+	list_for_each_entry(entry, &uid_list, link) {
+		if (entry->uid == uid) {
+			spin_unlock_irqrestore(&uid_lock, flags);
+			return entry;
+		}
+	}
+	spin_unlock_irqrestore(&uid_lock, flags);
+	return NULL;
+}
+
+static int tcp_snd_read_proc(char *page, char **start, off_t off,
+				int count, int *eof, void *data)
+{
+	int len;
+	unsigned int bytes;
+	char *p = page;
+	struct uid_stat *uid_entry = (struct uid_stat *) data;
+	if (!data)
+		return 0;
+
+	bytes = (unsigned int) (atomic_read(&uid_entry->tcp_snd) + INT_MIN);
+	p += sprintf(p, "%u\n", bytes);
+	len = (p - page) - off;
+	*eof = (len <= count) ? 1 : 0;
+	*start = page + off;
+	return len;
+}
+
+static int tcp_rcv_read_proc(char *page, char **start, off_t off,
+				int count, int *eof, void *data)
+{
+	int len;
+	unsigned int bytes;
+	char *p = page;
+	struct uid_stat *uid_entry = (struct uid_stat *) data;
+	if (!data)
+		return 0;
+
+	bytes = (unsigned int) (atomic_read(&uid_entry->tcp_rcv) + INT_MIN);
+	p += sprintf(p, "%u\n", bytes);
+	len = (p - page) - off;
+	*eof = (len <= count) ? 1 : 0;
+	*start = page + off;
+	return len;
+}
+
+/* Create a new entry for tracking the specified uid. */
+static struct uid_stat *create_stat(uid_t uid) {
+	unsigned long flags;
+	char uid_s[32];
+	struct uid_stat *new_uid;
+	struct proc_dir_entry *entry;
+
+	/* Create the uid stat struct and append it to the list. */
+	if ((new_uid = kmalloc(sizeof(struct uid_stat), GFP_KERNEL)) == NULL)
+		return NULL;
+
+	new_uid->uid = uid;
+	/* Counters start at INT_MIN, so we can track 4GB of network traffic. */
+	atomic_set(&new_uid->tcp_rcv, INT_MIN);
+	atomic_set(&new_uid->tcp_snd, INT_MIN);
+
+	spin_lock_irqsave(&uid_lock, flags);
+	list_add_tail(&new_uid->link, &uid_list);
+	spin_unlock_irqrestore(&uid_lock, flags);
+
+	sprintf(uid_s, "%d", uid);
+	entry = proc_mkdir(uid_s, parent);
+
+	/* Keep reference to uid_stat so we know what uid to read stats from. */
+	create_proc_read_entry("tcp_snd", S_IRUGO, entry , tcp_snd_read_proc,
+		(void *) new_uid);
+
+	create_proc_read_entry("tcp_rcv", S_IRUGO, entry, tcp_rcv_read_proc,
+		(void *) new_uid);
+
+	return new_uid;
+}
+
+int uid_stat_tcp_snd(uid_t uid, int size) {
+	struct uid_stat *entry;
+	activity_stats_update();
+	if ((entry = find_uid_stat(uid)) == NULL &&
+		((entry = create_stat(uid)) == NULL)) {
+			return -1;
+	}
+	atomic_add(size, &entry->tcp_snd);
+	return 0;
+}
+
+int uid_stat_tcp_rcv(uid_t uid, int size) {
+	struct uid_stat *entry;
+	activity_stats_update();
+	if ((entry = find_uid_stat(uid)) == NULL &&
+		((entry = create_stat(uid)) == NULL)) {
+			return -1;
+	}
+	atomic_add(size, &entry->tcp_rcv);
+	return 0;
+}
+
+static int __init uid_stat_init(void)
+{
+	parent = proc_mkdir("uid_stat", NULL);
+	if (!parent) {
+		pr_err("uid_stat: failed to create proc entry\n");
+		return -1;
+	}
+	return 0;
+}
+
+__initcall(uid_stat_init);
diff --git a/drivers/misc/wl127x-rfkill.c b/drivers/misc/wl127x-rfkill.c
new file mode 100644
index 000000000000..f5b95152948b
--- /dev/null
+++ b/drivers/misc/wl127x-rfkill.c
@@ -0,0 +1,121 @@
+/*
+ * Bluetooth TI wl127x rfkill power control via GPIO
+ *
+ * Copyright (C) 2009 Motorola, Inc.
+ * Copyright (C) 2008 Texas Instruments
+ * Initial code: Pavan Savoy <pavan.savoy@gmail.com> (wl127x_power.c)
+ *
+ *  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
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/gpio.h>
+#include <linux/rfkill.h>
+#include <linux/platform_device.h>
+#include <linux/wl127x-rfkill.h>
+
+static int wl127x_rfkill_set_power(void *data, enum rfkill_state state)
+{
+	int nshutdown_gpio = (int) data;
+
+	switch (state) {
+	case RFKILL_STATE_UNBLOCKED:
+		gpio_set_value(nshutdown_gpio, 1);
+		break;
+	case RFKILL_STATE_SOFT_BLOCKED:
+		gpio_set_value(nshutdown_gpio, 0);
+		break;
+	default:
+		printk(KERN_ERR "invalid bluetooth rfkill state %d\n", state);
+	}
+	return 0;
+}
+
+static int wl127x_rfkill_probe(struct platform_device *pdev)
+{
+	int rc = 0;
+	struct wl127x_rfkill_platform_data *pdata = pdev->dev.platform_data;
+	enum rfkill_state default_state = RFKILL_STATE_SOFT_BLOCKED;  /* off */
+
+	rc = gpio_request(pdata->nshutdown_gpio, "wl127x_nshutdown_gpio");
+	if (unlikely(rc))
+		return rc;
+
+	rc = gpio_direction_output(pdata->nshutdown_gpio, 0);
+	if (unlikely(rc))
+		return rc;
+
+	rfkill_set_default(RFKILL_TYPE_BLUETOOTH, default_state);
+	wl127x_rfkill_set_power(NULL, default_state);
+
+	pdata->rfkill = rfkill_allocate(&pdev->dev, RFKILL_TYPE_BLUETOOTH);
+	if (unlikely(!pdata->rfkill))
+		return -ENOMEM;
+
+	pdata->rfkill->name = "wl127x";
+	pdata->rfkill->state = default_state;
+	/* userspace cannot take exclusive control */
+	pdata->rfkill->user_claim_unsupported = 1;
+	pdata->rfkill->user_claim = 0;
+	pdata->rfkill->data = (void *) pdata->nshutdown_gpio;
+	pdata->rfkill->toggle_radio = wl127x_rfkill_set_power;
+
+	rc = rfkill_register(pdata->rfkill);
+
+	if (unlikely(rc))
+		rfkill_free(pdata->rfkill);
+
+	return 0;
+}
+
+static int wl127x_rfkill_remove(struct platform_device *pdev)
+{
+	struct wl127x_rfkill_platform_data *pdata = pdev->dev.platform_data;
+
+	rfkill_unregister(pdata->rfkill);
+	rfkill_free(pdata->rfkill);
+	gpio_free(pdata->nshutdown_gpio);
+
+	return 0;
+}
+
+static struct platform_driver wl127x_rfkill_platform_driver = {
+	.probe = wl127x_rfkill_probe,
+	.remove = wl127x_rfkill_remove,
+	.driver = {
+		   .name = "wl127x-rfkill",
+		   .owner = THIS_MODULE,
+		   },
+};
+
+static int __init wl127x_rfkill_init(void)
+{
+	return platform_driver_register(&wl127x_rfkill_platform_driver);
+}
+
+static void __exit wl127x_rfkill_exit(void)
+{
+	platform_driver_unregister(&wl127x_rfkill_platform_driver);
+}
+
+module_init(wl127x_rfkill_init);
+module_exit(wl127x_rfkill_exit);
+
+MODULE_ALIAS("platform:wl127x");
+MODULE_DESCRIPTION("wl127x-rfkill");
+MODULE_AUTHOR("Motorola");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mmc/card/Kconfig b/drivers/mmc/card/Kconfig
index 3b1f783bf924..3875c21e04fa 100644
--- a/drivers/mmc/card/Kconfig
+++ b/drivers/mmc/card/Kconfig
@@ -50,6 +50,15 @@ config MMC_BLOCK_BOUNCE
 
 	  If unsure, say Y here.
 
+config MMC_BLOCK_DEFERRED_RESUME
+	bool "Deferr MMC layer resume until I/O is requested"
+	depends on MMC_BLOCK
+	default n
+	help
+	  Say Y here to enable deferred MMC resume until I/O
+	  is requested. This will reduce overall resume latency and
+	  save power when theres an SD card inserted but not being used.
+
 config SDIO_UART
 	tristate "SDIO UART/GPS class support"
 	help
@@ -67,3 +76,13 @@ config MMC_TEST
 
 	  This driver is only of interest to those developing or
 	  testing a host driver. Most people should say N here.
+
+config MMC_BKOPS
+	bool "Enable background ops"
+	default n
+	help
+	  Say Y here to enable background ops in driver. This will result
+	  in issuing of MMC_SWITCH command to write byte 164 of EXT_CSD,
+	  in order to trigger background ops in the MMC device's
+	  firmware, whenever URGENT_BKOPS flag is found to be set in a
+	  read/write command's response.
diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index 4c1a648d00fc..8dcca3a55a8c 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -35,6 +35,10 @@
 #include <linux/capability.h>
 #include <linux/compat.h>
 
+#include <linux/interrupt.h>
+#define CREATE_TRACE_POINTS
+#include <trace/events/mmc.h>
+
 #include <linux/mmc/ioctl.h>
 #include <linux/mmc/card.h>
 #include <linux/mmc/host.h>
@@ -46,6 +50,8 @@
 
 #include "queue.h"
 
+#include "../debug_mmc.h"
+
 MODULE_ALIAS("mmc:block");
 #ifdef MODULE_PARAM_PREFIX
 #undef MODULE_PARAM_PREFIX
@@ -59,6 +65,8 @@ MODULE_ALIAS("mmc:block");
 #define INAND_CMD38_ARG_SECTRIM1 0x81
 #define INAND_CMD38_ARG_SECTRIM2 0x88
 
+#define MMC_CMD_RETRIES 	10
+
 static DEFINE_MUTEX(block_mutex);
 
 /*
@@ -136,11 +144,7 @@ static struct mmc_blk_data *mmc_blk_get(struct gendisk *disk)
 
 static inline int mmc_get_devidx(struct gendisk *disk)
 {
-	int devmaj = MAJOR(disk_devt(disk));
-	int devidx = MINOR(disk_devt(disk)) / perdev_minors;
-
-	if (!devmaj)
-		devidx = disk->first_minor / perdev_minors;
+	int devidx = disk->first_minor / perdev_minors;
 	return devidx;
 }
 
@@ -575,18 +579,22 @@ static int mmc_blk_cmd_error(struct request *req, const char *name, int error,
 			req->rq_disk->disk_name, "timed out", name, status);
 
 		/* If the status cmd initially failed, retry the r/w cmd */
-		if (!status_valid)
+		if (!status_valid) {
+			pr_err("%s: status not valid, retrying timeout\n", req->rq_disk->disk_name);
 			return ERR_RETRY;
-
+		}
 		/*
 		 * If it was a r/w cmd crc error, or illegal command
 		 * (eg, issued in wrong state) then retry - we should
 		 * have corrected the state problem above.
 		 */
-		if (status & (R1_COM_CRC_ERROR | R1_ILLEGAL_COMMAND))
+		if (status & (R1_COM_CRC_ERROR | R1_ILLEGAL_COMMAND)) {
+			pr_err("%s: command error, retrying timeout\n", req->rq_disk->disk_name);
 			return ERR_RETRY;
+		}
 
 		/* Otherwise abort the command */
+		pr_err("%s: not retrying timeout\n", req->rq_disk->disk_name);
 		return ERR_ABORT;
 
 	default:
@@ -701,7 +709,9 @@ static int mmc_blk_issue_discard_rq(struct mmc_queue *mq, struct request *req)
 	from = blk_rq_pos(req);
 	nr = blk_rq_sectors(req);
 
-	if (mmc_can_trim(card))
+	if (mmc_can_discard(card))
+		arg = MMC_DISCARD_ARG;
+	else if (mmc_can_trim(card))
 		arg = MMC_TRIM_ARG;
 	else
 		arg = MMC_ERASE_ARG;
@@ -725,57 +735,6 @@ out:
 	return err ? 0 : 1;
 }
 
-static int mmc_blk_issue_secdiscard_rq(struct mmc_queue *mq,
-				       struct request *req)
-{
-	struct mmc_blk_data *md = mq->data;
-	struct mmc_card *card = md->queue.card;
-	unsigned int from, nr, arg;
-	int err = 0;
-
-	if (!mmc_can_secure_erase_trim(card)) {
-		err = -EOPNOTSUPP;
-		goto out;
-	}
-
-	from = blk_rq_pos(req);
-	nr = blk_rq_sectors(req);
-
-	if (mmc_can_trim(card) && !mmc_erase_group_aligned(card, from, nr))
-		arg = MMC_SECURE_TRIM1_ARG;
-	else
-		arg = MMC_SECURE_ERASE_ARG;
-
-	if (card->quirks & MMC_QUIRK_INAND_CMD38) {
-		err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-				 INAND_CMD38_ARG_EXT_CSD,
-				 arg == MMC_SECURE_TRIM1_ARG ?
-				 INAND_CMD38_ARG_SECTRIM1 :
-				 INAND_CMD38_ARG_SECERASE,
-				 0);
-		if (err)
-			goto out;
-	}
-	err = mmc_erase(card, from, nr, arg);
-	if (!err && arg == MMC_SECURE_TRIM1_ARG) {
-		if (card->quirks & MMC_QUIRK_INAND_CMD38) {
-			err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-					 INAND_CMD38_ARG_EXT_CSD,
-					 INAND_CMD38_ARG_SECTRIM2,
-					 0);
-			if (err)
-				goto out;
-		}
-		err = mmc_erase(card, from, nr, MMC_SECURE_TRIM2_ARG);
-	}
-out:
-	spin_lock_irq(&md->lock);
-	__blk_end_request(req, err, blk_rq_bytes(req));
-	spin_unlock_irq(&md->lock);
-
-	return err ? 0 : 1;
-}
-
 static int mmc_blk_issue_flush(struct mmc_queue *mq, struct request *req)
 {
 	struct mmc_blk_data *md = mq->data;
@@ -943,6 +902,7 @@ static void mmc_blk_rw_rq_prep(struct mmc_queue_req *mqrq,
 	if (!mmc_card_blockaddr(card))
 		brq->cmd.arg <<= 9;
 	brq->cmd.flags = MMC_RSP_SPI_R1 | MMC_RSP_R1 | MMC_CMD_ADTC;
+	brq->cmd.retries = MMC_CMD_RETRIES;
 	brq->data.blksz = 512;
 	brq->stop.opcode = MMC_STOP_TRANSMISSION;
 	brq->stop.arg = 0;
@@ -1137,6 +1097,9 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *rqc)
 		}
 	} while (ret);
 
+	if (brq->cmd.resp[0] & R1_URGENT_BKOPS)
+		mmc_card_set_need_bkops(card);
+
 	return 1;
 
  cmd_err:
@@ -1178,12 +1141,22 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *rqc)
 	return 0;
 }
 
+static int
+mmc_blk_set_blksize(struct mmc_blk_data *md, struct mmc_card *card);
+
 static int mmc_blk_issue_rq(struct mmc_queue *mq, struct request *req)
 {
 	int ret;
 	struct mmc_blk_data *md = mq->data;
 	struct mmc_card *card = md->queue.card;
 
+#ifdef CONFIG_MMC_BLOCK_DEFERRED_RESUME
+	if (mmc_bus_needs_resume(card->host)) {
+		mmc_resume_bus(card->host);
+		mmc_blk_set_blksize(md, card);
+	}
+#endif
+
 	if (req && !mq->mqrq_prev->req)
 		/* claim host only for the first request */
 		mmc_claim_host(card->host);
@@ -1198,16 +1171,17 @@ static int mmc_blk_issue_rq(struct mmc_queue *mq, struct request *req)
 		/* complete ongoing async transfer before issuing discard */
 		if (card->host->areq)
 			mmc_blk_issue_rw_rq(mq, NULL);
-		if (req->cmd_flags & REQ_SECURE)
-			ret = mmc_blk_issue_secdiscard_rq(mq, req);
-		else
-			ret = mmc_blk_issue_discard_rq(mq, req);
+		ret = mmc_blk_issue_discard_rq(mq, req);
 	} else if (req && req->cmd_flags & REQ_FLUSH) {
 		/* complete ongoing async transfer before issuing flush */
 		if (card->host->areq)
 			mmc_blk_issue_rw_rq(mq, NULL);
 		ret = mmc_blk_issue_flush(mq, req);
 	} else {
+		/* Abort any current bk ops of eMMC card by issuing HPI */
+		if (mmc_card_mmc(mq->card) && mmc_card_doing_bkops(mq->card))
+			mmc_interrupt_hpi(mq->card);
+
 		ret = mmc_blk_issue_rw_rq(mq, req);
 	}
 
@@ -1288,6 +1262,7 @@ static struct mmc_blk_data *mmc_blk_alloc_req(struct mmc_card *card,
 	md->disk->queue = md->queue.queue;
 	md->disk->driverfs_dev = parent;
 	set_disk_ro(md->disk, md->read_only || default_ro);
+	md->disk->flags = GENHD_FL_EXT_DEVT;
 
 	/*
 	 * As discussed on lkml, GENHD_FL_REMOVABLE should:
@@ -1527,6 +1502,9 @@ static int mmc_blk_probe(struct mmc_card *card)
 	mmc_set_drvdata(card, md);
 	mmc_fixup_device(card, blk_fixups);
 
+#ifdef CONFIG_MMC_BLOCK_DEFERRED_RESUME
+	mmc_set_bus_resume_policy(card->host, 1);
+#endif
 	if (mmc_add_disk(md))
 		goto out;
 
@@ -1552,6 +1530,11 @@ static void mmc_blk_remove(struct mmc_card *card)
 	mmc_release_host(card->host);
 	mmc_blk_remove_req(md);
 	mmc_set_drvdata(card, NULL);
+#ifdef CONFIG_MMC_BLOCK_DEFERRED_RESUME
+	mmc_set_bus_resume_policy(card->host, 0);
+	card->host->bus_resume_flags &= ~MMC_BUSRESUME_NEEDS_RESUME;
+	MMC_printk("%s: bus_resume_flags 0x%x", mmc_hostname(card->host), card->host->bus_resume_flags);
+#endif
 }
 
 #ifdef CONFIG_PM
@@ -1575,7 +1558,9 @@ static int mmc_blk_resume(struct mmc_card *card)
 	struct mmc_blk_data *md = mmc_get_drvdata(card);
 
 	if (md) {
+#ifndef CONFIG_MMC_BLOCK_DEFERRED_RESUME
 		mmc_blk_set_blksize(md, card);
+#endif
 
 		/*
 		 * Resume involves the card going into idle state,
diff --git a/drivers/mmc/card/mmc_test.c b/drivers/mmc/card/mmc_test.c
index 2bf229acd3b8..86817085aec7 100644
--- a/drivers/mmc/card/mmc_test.c
+++ b/drivers/mmc/card/mmc_test.c
@@ -169,6 +169,96 @@ struct mmc_test_async_req {
 	struct mmc_test_card *test;
 };
 
+struct mmc_test_parameter {
+	const char *name;
+	long value;
+	long (*exec)(struct mmc_test_card *);
+	const char *input;
+};
+
+static long mmc_test_set_testcase(struct mmc_test_card *test);
+static long mmc_test_set_clock(struct mmc_test_card *test);
+static long mmc_test_set_bus_width(struct mmc_test_card *test);
+static long mmc_test_set_timing(struct mmc_test_card *test);
+
+
+static struct mmc_test_parameter mmc_test_parameter[] = {
+	{
+		.name = "Testcase Number",
+		.value = 1,
+		.exec = mmc_test_set_testcase,
+		.input = "-n",
+	},
+	{
+		.name = "Clock Rate",
+		.value = -1,
+		.exec = mmc_test_set_clock,
+		.input = "-c",
+	},
+	{
+		.name = "Bus Width",
+		.value = -1,
+		.exec = mmc_test_set_bus_width,
+		.input = "-b",
+	},
+	{
+		.name = "Timing",
+		.value = -1,
+		.exec = mmc_test_set_timing,
+		.input = "-t",
+	},
+};
+
+static long mmc_test_set_testcase(struct mmc_test_card *test)
+{
+	return mmc_test_parameter[0].value;
+}
+
+static long mmc_test_set_clock(struct mmc_test_card *test)
+{
+	long clock = mmc_test_parameter[1].value;
+	if (-1 == clock)
+		return test->card->host->ios.clock;
+	WARN_ON(clock < test->card->host->f_min);
+	if (clock > test->card->host->f_max)
+		clock = test->card->host->f_max;
+
+	test->card->host->ios.clock = clock;
+
+	return test->card->host->ios.clock;
+}
+
+static long mmc_test_set_bus_width(struct mmc_test_card *test)
+{
+	long bus_width = mmc_test_parameter[2].value;
+	if (-1 == bus_width)
+		return test->card->host->ios.bus_width;
+
+	test->card->host->ios.bus_width = bus_width;
+
+	return test->card->host->ios.bus_width = bus_width;
+}
+
+static long mmc_test_set_timing(struct mmc_test_card *test)
+{
+	long timing = mmc_test_parameter[3].value;
+	if (-1 == timing)
+		return test->card->host->ios.timing;
+	test->card->host->ios.timing = timing;
+
+	return test->card->host->ios.timing;
+}
+
+static void mmc_test_set_parameters(struct mmc_test_card *test)
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(mmc_test_parameter); i++) {
+		printk(KERN_INFO "Parameter[%s] set to [%ld]\n",
+			mmc_test_parameter[i].name,
+			mmc_test_parameter[i].exec(test));
+	}
+}
+
 /*******************************************************************/
 /*  General helper functions                                       */
 /*******************************************************************/
@@ -2665,6 +2755,8 @@ static void mmc_test_run(struct mmc_test_card *test, int testcase)
 
 	mmc_claim_host(test->card->host);
 
+	mmc_test_set_parameters(test);
+
 	for (i = 0;i < ARRAY_SIZE(mmc_test_cases);i++) {
 		struct mmc_test_general_result *gr;
 
@@ -2777,6 +2869,23 @@ static void mmc_test_free_result(struct mmc_card *card)
 
 static LIST_HEAD(mmc_test_file_test);
 
+static void mmc_test_usage(struct seq_file *sf)
+{
+	int i = 0;
+
+	seq_printf(sf, "\nHow to run test:"
+			"\necho <testcase> [[param1 value1].... ] > test"
+			"\nExample:: echo 1 -b 4 -c 2500000 -t 2"
+			"\n\nSupported parameters in sequence\n");
+
+	for (i = 0; i < ARRAY_SIZE(mmc_test_parameter); i++) {
+		seq_printf(sf, "Parameter%d Name:[%s] option:[%s]\n",
+			i + 1, mmc_test_parameter[i].name,
+			mmc_test_parameter[i].input);
+	}
+	seq_printf(sf, "\'-1\' passed to take default value\n\n\n");
+}
+
 static int mtf_test_show(struct seq_file *sf, void *data)
 {
 	struct mmc_card *card = (struct mmc_card *)sf->private;
@@ -2811,24 +2920,92 @@ static int mtf_test_open(struct inode *inode, struct file *file)
 	return single_open(file, mtf_test_show, inode->i_private);
 }
 
+static int mmc_test_extract_parameters(char *data_buf)
+{
+	char *running = NULL;
+	char *token = NULL;
+	const char delimiters[] = " ";
+	long value;
+	int i;
+	int set = 0;
+
+	running = data_buf;
+
+	/*Example:
+	 * echo <testcasenumber> [[param1 value1] [param1 value1]] > test
+	 * $] echo 1 > test | Execute testcase 1
+	 * $] echo 1 -c 2500000 | execute tesecase 1 and set clock to 2500000
+	 * $] echo 1 -b 4 -c 2500000 -t 2 |
+	 *	execute tesecase 1, set clock to 2500000, set bus_width 4,
+	 *	and set timing to 2
+	*/
+
+	while ((token = strsep(&running, delimiters))) {
+		if (strict_strtol(token, 10, &value)) {
+			/* [Param1 value1] combination
+			 * Compare with available param list
+			 */
+			for (i = 0; i < ARRAY_SIZE(mmc_test_parameter); i++) {
+				if (!strcmp(mmc_test_parameter[i].input,
+						token)) {
+					/* Valid Option, extract following
+					 * value and save it
+					 */
+					token = strsep(&running, delimiters);
+					if (strict_strtol(token, 10,
+					    &(mmc_test_parameter[i].value))) {
+
+						printk(KERN_ERR "wrong parameter value\n");
+						return -EINVAL;
+					} else {
+						break;
+					}
+				}
+			}
+			if (i == ARRAY_SIZE(mmc_test_parameter)) {
+				printk(KERN_ERR "uknown mmc_test option\n");
+				return -EINVAL;
+			}
+		} else {
+			/* Testcase number */
+			if (!set) {
+				mmc_test_parameter[0].value = value;
+				set = 1;
+			} else {
+				printk(KERN_ERR "invalid options");
+				return -EINVAL;
+			}
+		}
+	}
+	return 0;
+}
+
 static ssize_t mtf_test_write(struct file *file, const char __user *buf,
 	size_t count, loff_t *pos)
 {
 	struct seq_file *sf = (struct seq_file *)file->private_data;
 	struct mmc_card *card = (struct mmc_card *)sf->private;
 	struct mmc_test_card *test;
-	char lbuf[12];
+	char *data_buf = NULL;
 	long testcase;
 
-	if (count >= sizeof(lbuf))
-		return -EINVAL;
+	data_buf = kzalloc(count, GFP_KERNEL);
+	if (data_buf == NULL)
+		return -ENOMEM;
 
-	if (copy_from_user(lbuf, buf, count))
+	if (copy_from_user(data_buf, buf, count)) {
+		kfree(data_buf);
+		return -EFAULT;
+	}
+	data_buf[strlen(data_buf) - 1] = '\0';
+	if (mmc_test_extract_parameters(data_buf)) {
+		mmc_test_usage(sf);
 		return -EFAULT;
-	lbuf[count] = '\0';
+	}
 
-	if (strict_strtol(lbuf, 10, &testcase))
-		return -EINVAL;
+	kfree(data_buf);
+
+	testcase = mmc_test_parameter[0].value;
 
 	test = kzalloc(sizeof(struct mmc_test_card), GFP_KERNEL);
 	if (!test)
diff --git a/drivers/mmc/card/queue.c b/drivers/mmc/card/queue.c
index 45fb362e3f01..ba38a2fe12f5 100644
--- a/drivers/mmc/card/queue.c
+++ b/drivers/mmc/card/queue.c
@@ -20,6 +20,8 @@
 #include <linux/mmc/host.h>
 #include "queue.h"
 
+#include "../debug_mmc.h"
+
 #define MMC_QUEUE_BOUNCESZ	65536
 
 #define MMC_QUEUE_SUSPENDED	(1 << 0)
@@ -64,6 +66,12 @@ static int mmc_queue_thread(void *d)
 			set_current_state(TASK_RUNNING);
 			mq->issue_fn(mq, req);
 		} else {
+			/*
+			 * Since the queue is empty, start synchronous
+			 * background ops if there is a request for it.
+			 */
+			if (mmc_card_need_bkops(mq->card))
+				mmc_bkops_start(mq->card, true);
 			if (kthread_should_stop()) {
 				set_current_state(TASK_RUNNING);
 				break;
@@ -134,7 +142,7 @@ static void mmc_queue_setup_discard(struct request_queue *q,
 
 	queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, q);
 	q->limits.max_discard_sectors = max_discard;
-	if (card->erased_byte == 0)
+	if (card->erased_byte == 0 && !mmc_can_discard(card))
 		q->limits.discard_zeroes_data = 1;
 	q->limits.discard_granularity = card->pref_erase << 9;
 	/* granularity must not be greater than max. discard */
@@ -343,6 +351,7 @@ void mmc_queue_suspend(struct mmc_queue *mq)
 
 	if (!(mq->flags & MMC_QUEUE_SUSPENDED)) {
 		mq->flags |= MMC_QUEUE_SUSPENDED;
+		MMC_printk("%s: blk_stop_queue start", mmc_hostname(mq->card->host));
 
 		spin_lock_irqsave(q->queue_lock, flags);
 		blk_stop_queue(q);
@@ -363,6 +372,7 @@ void mmc_queue_resume(struct mmc_queue *mq)
 
 	if (mq->flags & MMC_QUEUE_SUSPENDED) {
 		mq->flags &= ~MMC_QUEUE_SUSPENDED;
+		MMC_printk("%s: blk_start_queue start", mmc_hostname(mq->card->host));
 
 		up(&mq->thread_sem);
 
diff --git a/drivers/mmc/core/Kconfig b/drivers/mmc/core/Kconfig
index ef103871517f..85c2e1acd156 100644
--- a/drivers/mmc/core/Kconfig
+++ b/drivers/mmc/core/Kconfig
@@ -27,3 +27,20 @@ config MMC_CLKGATE
 	  support handling this in order for it to be of any use.
 
 	  If unsure, say N.
+
+config MMC_EMBEDDED_SDIO
+	boolean "MMC embedded SDIO device support (EXPERIMENTAL)"
+	depends on EXPERIMENTAL
+	help
+	  If you say Y here, support will be added for embedded SDIO
+	  devices which do not contain the necessary enumeration
+	  support in hardware to be properly detected.
+
+config MMC_PARANOID_SD_INIT
+	bool "Enable paranoid SD card initialization (EXPERIMENTAL)"
+	depends on EXPERIMENTAL
+	help
+	  If you say Y here, the MMC layer will be extra paranoid
+	  about re-trying SD init requests. This can be a useful
+	  work-around for buggy controllers and hardware. Enable
+	  if you are experiencing issues with SD detection.
diff --git a/drivers/mmc/core/bus.c b/drivers/mmc/core/bus.c
index 393d817ed040..f4bdbe6982c9 100644
--- a/drivers/mmc/core/bus.c
+++ b/drivers/mmc/core/bus.c
@@ -25,6 +25,10 @@
 
 #define to_mmc_driver(d)	container_of(d, struct mmc_driver, drv)
 
+#ifdef CONFIG_MMC_TEST
+static struct mmc_driver *mmc_test_drv;
+#endif
+
 static ssize_t mmc_type_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
@@ -107,6 +111,13 @@ static int mmc_bus_probe(struct device *dev)
 	struct mmc_driver *drv = to_mmc_driver(dev->driver);
 	struct mmc_card *card = mmc_dev_to_card(dev);
 
+#ifdef CONFIG_MMC_TEST
+	/*
+	 * Hack: Explicitly invoking mmc_test probe to co-exist with mmcblk driver.
+	 */
+	mmc_test_drv->probe(card);
+#endif
+
 	return drv->probe(card);
 }
 
@@ -120,18 +131,19 @@ static int mmc_bus_remove(struct device *dev)
 	return 0;
 }
 
-static int mmc_bus_suspend(struct device *dev, pm_message_t state)
+static int mmc_bus_pm_suspend(struct device *dev)
 {
 	struct mmc_driver *drv = to_mmc_driver(dev->driver);
 	struct mmc_card *card = mmc_dev_to_card(dev);
 	int ret = 0;
+	pm_message_t state = { PM_EVENT_SUSPEND };
 
 	if (dev->driver && drv->suspend)
 		ret = drv->suspend(card, state);
 	return ret;
 }
 
-static int mmc_bus_resume(struct device *dev)
+static int mmc_bus_pm_resume(struct device *dev)
 {
 	struct mmc_driver *drv = to_mmc_driver(dev->driver);
 	struct mmc_card *card = mmc_dev_to_card(dev);
@@ -143,7 +155,6 @@ static int mmc_bus_resume(struct device *dev)
 }
 
 #ifdef CONFIG_PM_RUNTIME
-
 static int mmc_runtime_suspend(struct device *dev)
 {
 	struct mmc_card *card = mmc_dev_to_card(dev);
@@ -162,21 +173,13 @@ static int mmc_runtime_idle(struct device *dev)
 {
 	return pm_runtime_suspend(dev);
 }
+#endif /* CONFIG_PM_RUNTIME */
 
 static const struct dev_pm_ops mmc_bus_pm_ops = {
-	.runtime_suspend	= mmc_runtime_suspend,
-	.runtime_resume		= mmc_runtime_resume,
-	.runtime_idle		= mmc_runtime_idle,
+	SET_SYSTEM_SLEEP_PM_OPS(mmc_bus_pm_suspend, mmc_bus_pm_resume)
+	SET_RUNTIME_PM_OPS(mmc_runtime_suspend, mmc_runtime_resume, mmc_runtime_idle)
 };
 
-#define MMC_PM_OPS_PTR	(&mmc_bus_pm_ops)
-
-#else /* !CONFIG_PM_RUNTIME */
-
-#define MMC_PM_OPS_PTR	NULL
-
-#endif /* !CONFIG_PM_RUNTIME */
-
 static struct bus_type mmc_bus_type = {
 	.name		= "mmc",
 	.dev_attrs	= mmc_dev_attrs,
@@ -184,9 +187,7 @@ static struct bus_type mmc_bus_type = {
 	.uevent		= mmc_bus_uevent,
 	.probe		= mmc_bus_probe,
 	.remove		= mmc_bus_remove,
-	.suspend	= mmc_bus_suspend,
-	.resume		= mmc_bus_resume,
-	.pm		= MMC_PM_OPS_PTR,
+	.pm		= &mmc_bus_pm_ops,
 };
 
 int mmc_register_bus(void)
@@ -206,6 +207,10 @@ void mmc_unregister_bus(void)
 int mmc_register_driver(struct mmc_driver *drv)
 {
 	drv->drv.bus = &mmc_bus_type;
+#ifdef CONFIG_MMC_TEST
+	if (!strcmp(drv->drv.name, "mmc_test"))
+		mmc_test_drv = drv;
+#endif
 	return driver_register(&drv->drv);
 }
 
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index b27b94078c21..5cbeb1bbf267 100644..100755
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -5,6 +5,7 @@
  *  SD support Copyright (C) 2004 Ian Molton, All Rights Reserved.
  *  Copyright (C) 2005-2008 Pierre Ossman, All Rights Reserved.
  *  MMCv4 support Copyright (C) 2006 Philip Langdale, All Rights Reserved.
+ *  Copyright (c) 2012 NVIDIA 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 version 2 as
@@ -24,6 +25,9 @@
 #include <linux/regulator/consumer.h>
 #include <linux/pm_runtime.h>
 #include <linux/suspend.h>
+#include <linux/wakelock.h>
+
+#include <trace/events/mmc.h>
 
 #include <linux/mmc/card.h>
 #include <linux/mmc/host.h>
@@ -38,6 +42,7 @@
 #include "mmc_ops.h"
 #include "sd_ops.h"
 #include "sdio_ops.h"
+#include "../debug_mmc.h"
 
 static struct workqueue_struct *workqueue;
 
@@ -107,6 +112,11 @@ void mmc_request_done(struct mmc_host *host, struct mmc_request *mrq)
 
 		cmd->retries--;
 		cmd->error = 0;
+		if (mrq->data) {
+			mrq->data->error = 0;
+			if (mrq->stop)
+				mrq->stop->error = 0;
+		}
 		host->ops->request(host, mrq);
 	} else {
 		led_trigger_event(host->led, LED_OFF);
@@ -120,6 +130,7 @@ void mmc_request_done(struct mmc_host *host, struct mmc_request *mrq)
 			pr_debug("%s:     %d bytes transferred: %d\n",
 				mmc_hostname(host),
 				mrq->data->bytes_xfered, mrq->data->error);
+			trace_mmc_blk_rw_end(cmd->opcode, cmd->arg, mrq->data);
 		}
 
 		if (mrq->stop) {
@@ -270,13 +281,44 @@ struct mmc_async_req *mmc_start_req(struct mmc_host *host,
 {
 	int err = 0;
 	struct mmc_async_req *data = host->areq;
+	struct mmc_card *card = host->card;
+	struct timeval before_time, after_time;
 
 	/* Prepare a new request */
 	if (areq)
 		mmc_pre_req(host, areq->mrq, !host->areq);
 
 	if (host->areq) {
+		if (card->ext_csd.refresh &&
+			(host->areq->mrq->data->flags & MMC_DATA_WRITE))
+				do_gettimeofday(&before_time);
 		mmc_wait_for_req_done(host, host->areq->mrq);
+		if (card->ext_csd.refresh &&
+			(host->areq->mrq->data->flags & MMC_DATA_WRITE)) {
+			do_gettimeofday(&after_time);
+			switch (after_time.tv_sec - before_time.tv_sec) {
+				case 0:
+					if (after_time.tv_usec -
+						before_time.tv_usec >=
+							MMC_SLOW_WRITE_TIME) {
+						card->ext_csd.last_tv_sec =
+							after_time.tv_sec;
+						card->ext_csd.last_bkops_tv_sec =
+							after_time.tv_sec;
+					}
+					break;
+				case 1:
+					if (after_time.tv_usec -
+						before_time.tv_usec <
+							MMC_SLOW_WRITE_TIME - 1000000)
+						break;
+				default:
+					card->ext_csd.last_tv_sec =
+						after_time.tv_sec;
+					card->ext_csd.last_bkops_tv_sec =
+						after_time.tv_sec;
+			}
+		}
 		err = host->areq->err_check(host->card, host->areq);
 		if (err) {
 			mmc_post_req(host, host->areq->mrq, 0);
@@ -288,8 +330,12 @@ struct mmc_async_req *mmc_start_req(struct mmc_host *host,
 		}
 	}
 
-	if (areq)
+	if (areq) {
+		trace_mmc_blk_rw_start(areq->mrq->cmd->opcode,
+				       areq->mrq->cmd->arg,
+				       areq->mrq->data);
 		__mmc_start_req(host, areq->mrq);
+	}
 
 	if (host->areq)
 		mmc_post_req(host, host->areq->mrq, 0);
@@ -319,6 +365,181 @@ void mmc_wait_for_req(struct mmc_host *host, struct mmc_request *mrq)
 EXPORT_SYMBOL(mmc_wait_for_req);
 
 /**
+ *	mmc_bkops_start - Issue start for mmc background ops
+ *	@card: the MMC card associated with bkops
+ *	@is_synchronous: is the backops synchronous
+ *
+ *	Issued background ops without the busy wait.
+ */
+int mmc_bkops_start(struct mmc_card *card, bool is_synchronous)
+{
+	int err;
+	unsigned long flags;
+	struct timeval before_time, after_time;
+
+	BUG_ON(!card);
+
+	if (!card->ext_csd.bk_ops_en || mmc_card_doing_bkops(card))
+		return 1;
+
+	mmc_claim_host(card->host);
+	if (card->ext_csd.refresh)
+		do_gettimeofday(&before_time);
+	err = mmc_send_bk_ops_cmd(card, is_synchronous);
+	if (err)
+		pr_err("%s: abort bk ops (%d error)\n",
+			mmc_hostname(card->host), err);
+	if (card->ext_csd.refresh) {
+		do_gettimeofday(&after_time);
+		switch (after_time.tv_sec - before_time.tv_sec) {
+			case 0:
+				if (after_time.tv_usec - before_time.tv_usec >=
+					MMC_SLOW_WRITE_TIME)
+					card->ext_csd.last_tv_sec = after_time.tv_sec;
+				break;
+			case 1:
+				if (after_time.tv_usec - before_time.tv_usec <
+					MMC_SLOW_WRITE_TIME - 1000000)
+					break;
+			default:
+				card->ext_csd.last_tv_sec = after_time.tv_sec;
+		}
+		card->ext_csd.last_bkops_tv_sec = after_time.tv_sec;
+	}
+
+	/*
+	 * Incase of asynchronous backops, set card state
+	 * to doing bk ops to ensure that HPI is issued before
+	 * handling any new request in the queue.
+	 */
+		spin_lock_irqsave(&card->host->lock, flags);
+		mmc_card_clr_need_bkops(card);
+		if (!is_synchronous)
+			mmc_card_set_doing_bkops(card);
+		spin_unlock_irqrestore(&card->host->lock, flags);
+
+	mmc_release_host(card->host);
+
+	return err;
+}
+EXPORT_SYMBOL(mmc_bkops_start);
+
+static void mmc_bkops_work(struct work_struct *work)
+{
+	struct mmc_card *card = container_of(work, struct mmc_card, bkops);
+	mmc_bkops_start(card, true);
+}
+
+static void mmc_refresh_work(struct work_struct *work)
+{
+	struct mmc_card *card = container_of(work, struct mmc_card, refresh);
+	char buf[512];
+	mmc_gen_cmd(card, buf, 0x44, 0x1, 0x0, 0x1);
+}
+
+void mmc_refresh(unsigned long data)
+{
+	struct mmc_card *card = (struct mmc_card *) data;
+	struct timeval cur_time;
+	__kernel_time_t timeout, timeout1, timeout2;
+
+	if ((!card) || (!card->ext_csd.refresh))
+		return;
+
+	INIT_WORK(&card->bkops, (work_func_t) mmc_bkops_work);
+	INIT_WORK(&card->refresh, (work_func_t) mmc_refresh_work);
+
+	do_gettimeofday(&cur_time);
+	timeout1 = MMC_REFRESH_INTERVAL - (cur_time.tv_sec -
+		card->ext_csd.last_tv_sec);
+	if ((cur_time.tv_sec < card->ext_csd.last_tv_sec) ||
+		(timeout1 <= 0)) {
+		queue_work(workqueue, &card->refresh);
+		card->ext_csd.last_tv_sec = cur_time.tv_sec;
+		card->ext_csd.last_bkops_tv_sec = cur_time.tv_sec;
+		timeout1 = MMC_REFRESH_INTERVAL;
+	}
+
+	timeout2 = MMC_BKOPS_INTERVAL - (cur_time.tv_sec -
+		card->ext_csd.last_bkops_tv_sec);
+	if ((cur_time.tv_sec < card->ext_csd.last_bkops_tv_sec) ||
+		(timeout2 <= 0)) {
+		mmc_card_set_need_bkops(card);
+		queue_work(workqueue, &card->bkops);
+		timeout2 = MMC_BKOPS_INTERVAL;
+	}
+
+	timeout = timeout1 < timeout2 ? timeout1 : timeout2;
+	card->timer.expires = jiffies + timeout*HZ;
+	add_timer(&card->timer);
+}
+EXPORT_SYMBOL(mmc_refresh);
+
+/**
+ *	mmc_interrupt_hpi - Issue for High priority Interrupt
+ *	@card: the MMC card associated with the HPI transfer
+ *
+ *	Issued High Priority Interrupt, and check for card status
+ *	util out-of prg-state.
+ */
+int mmc_interrupt_hpi(struct mmc_card *card)
+{
+	int err;
+	u32 status;
+	unsigned long flags;
+
+	BUG_ON(!card);
+
+	if (!mmc_card_mmc(card))
+		return 1;
+
+	if (!card->ext_csd.hpi_en) {
+		pr_info("%s: HPI enable bit unset\n", mmc_hostname(card->host));
+		return 1;
+	}
+
+	mmc_claim_host(card->host);
+	err = mmc_send_status(card, &status);
+	if (err) {
+		pr_err("%s: Get card status fail\n", mmc_hostname(card->host));
+		goto out;
+	}
+
+	/*
+	 * If the card status is in PRG-state, we can send the HPI command.
+	 */
+	if (R1_CURRENT_STATE(status) == R1_STATE_PRG) {
+		do {
+			/*
+			 * We don't know when the HPI command will finish
+			 * processing, so we need to resend HPI until out
+			 * of prg-state, and keep checking the card status
+			 * with SEND_STATUS.  If a timeout error occurs when
+			 * sending the HPI command, we are already out of
+			 * prg-state.
+			 */
+			err = mmc_send_hpi_cmd(card, &status);
+			if (err)
+				pr_debug("%s: abort HPI (%d error)\n",
+					 mmc_hostname(card->host), err);
+
+			err = mmc_send_status(card, &status);
+			if (err)
+				break;
+		} while (R1_CURRENT_STATE(status) == R1_STATE_PRG);
+	} else
+		pr_debug("%s: Left prg-state\n", mmc_hostname(card->host));
+
+out:
+	spin_lock_irqsave(&card->host->lock, flags);
+	mmc_card_clr_doing_bkops(card);
+	spin_unlock_irqrestore(&card->host->lock, flags);
+	mmc_release_host(card->host);
+	return err;
+}
+EXPORT_SYMBOL(mmc_interrupt_hpi);
+
+/**
  *	mmc_wait_for_cmd - start a command and wait for completion
  *	@host: MMC host to start command
  *	@cmd: MMC command to start
@@ -1151,7 +1372,7 @@ static void mmc_power_up(struct mmc_host *host)
 	mmc_host_clk_release(host);
 }
 
-static void mmc_power_off(struct mmc_host *host)
+void mmc_power_off(struct mmc_host *host)
 {
 	mmc_host_clk_hold(host);
 
@@ -1215,6 +1436,36 @@ static inline void mmc_bus_put(struct mmc_host *host)
 	spin_unlock_irqrestore(&host->lock, flags);
 }
 
+int mmc_resume_bus(struct mmc_host *host)
+{
+	unsigned long flags;
+
+	if (!mmc_bus_needs_resume(host))
+		return -EINVAL;
+
+	printk("%s: Starting deferred resume\n", mmc_hostname(host));
+	spin_lock_irqsave(&host->lock, flags);
+	host->bus_resume_flags &= ~MMC_BUSRESUME_NEEDS_RESUME;
+	host->rescan_disable = 0;
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	mmc_bus_get(host);
+	if (host->bus_ops && !host->bus_dead) {
+		mmc_power_up(host);
+		BUG_ON(!host->bus_ops->resume);
+		host->bus_ops->resume(host);
+	}
+
+	if (host->bus_ops->detect && !host->bus_dead)
+		host->bus_ops->detect(host);
+
+	mmc_bus_put(host);
+	printk("%s: Deferred resume completed\n", mmc_hostname(host));
+	return 0;
+}
+
+EXPORT_SYMBOL(mmc_resume_bus);
+
 /*
  * Assign a mmc bus handler to a host. Only one bus handler may control a
  * host at any given time.
@@ -1241,8 +1492,7 @@ void mmc_attach_bus(struct mmc_host *host, const struct mmc_bus_ops *ops)
 }
 
 /*
- * Remove the current bus handler from a host. Assumes that there are
- * no interesting cards left, so the bus is powered down.
+ * Remove the current bus handler from a host.
  */
 void mmc_detach_bus(struct mmc_host *host)
 {
@@ -1259,8 +1509,6 @@ void mmc_detach_bus(struct mmc_host *host)
 
 	spin_unlock_irqrestore(&host->lock, flags);
 
-	mmc_power_off(host);
-
 	mmc_bus_put(host);
 }
 
@@ -1283,6 +1531,7 @@ void mmc_detect_change(struct mmc_host *host, unsigned long delay)
 	spin_unlock_irqrestore(&host->lock, flags);
 #endif
 
+	wake_lock(&host->detect_wake_lock);
 	mmc_schedule_delayed_work(&host->detect, delay);
 }
 
@@ -1342,7 +1591,10 @@ static unsigned int mmc_mmc_erase_timeout(struct mmc_card *card,
 {
 	unsigned int erase_timeout;
 
-	if (card->ext_csd.erase_group_def & 1) {
+	if (arg == MMC_DISCARD_ARG ||
+		(arg == MMC_TRIM_ARG && card->ext_csd.rev >= 6)) {
+		erase_timeout = card->ext_csd.trim_timeout;
+	} else if (card->ext_csd.erase_group_def & 1) {
 		/* High Capacity Erase Group Size uses HC timeouts */
 		if (arg == MMC_TRIM_ARG)
 			erase_timeout = card->ext_csd.trim_timeout;
@@ -1438,8 +1690,13 @@ static int mmc_do_erase(struct mmc_card *card, unsigned int from,
 {
 	struct mmc_command cmd = {0};
 	unsigned int qty = 0;
+	unsigned int fr, nr;
 	int err;
 
+	fr = from;
+	nr = to - from + 1;
+	trace_mmc_blk_erase_start(arg, fr, nr);
+
 	/*
 	 * qty is used to calculate the erase timeout which depends on how many
 	 * erase groups (or allocation units in SD terminology) are affected.
@@ -1531,6 +1788,8 @@ static int mmc_do_erase(struct mmc_card *card, unsigned int from,
 	} while (!(cmd.resp[0] & R1_READY_FOR_DATA) ||
 		 R1_CURRENT_STATE(cmd.resp[0]) == R1_STATE_PRG);
 out:
+
+	trace_mmc_blk_erase_end(arg, fr, nr);
 	return err;
 }
 
@@ -1618,6 +1877,14 @@ int mmc_can_trim(struct mmc_card *card)
 }
 EXPORT_SYMBOL(mmc_can_trim);
 
+int mmc_can_discard(struct mmc_card *card)
+{
+	if (card->ext_csd.feature_support & MMC_DISCARD_FEATURE)
+		return 1;
+	return 0;
+}
+EXPORT_SYMBOL( mmc_can_discard);
+
 int mmc_can_secure_erase_trim(struct mmc_card *card)
 {
 	if (card->ext_csd.sec_feature_support & EXT_CSD_SEC_ER_EN)
@@ -1748,12 +2015,16 @@ static int mmc_rescan_try_freq(struct mmc_host *host, unsigned freq)
 	mmc_send_if_cond(host, host->ocr_avail);
 
 	/* Order's important: probe SDIO, then SD, then MMC */
-	if (!mmc_attach_sdio(host))
+	if (!mmc_attach_sdio(host)) {
+		MMC_printk("%s: sdio completed", mmc_hostname(host));
 		return 0;
+	}
 	if (!mmc_attach_sd(host))
 		return 0;
-	if (!mmc_attach_mmc(host))
+	if (!mmc_attach_mmc(host)) {
+		MMC_printk("%s: eMMC completed", mmc_hostname(host));
 		return 0;
+	}
 
 	mmc_power_off(host);
 	return -EIO;
@@ -1765,6 +2036,7 @@ void mmc_rescan(struct work_struct *work)
 	struct mmc_host *host =
 		container_of(work, struct mmc_host, detect.work);
 	int i;
+	bool extend_wakelock = false;
 
 	if (host->rescan_disable)
 		return;
@@ -1779,6 +2051,12 @@ void mmc_rescan(struct work_struct *work)
 	    && !(host->caps & MMC_CAP_NONREMOVABLE))
 		host->bus_ops->detect(host);
 
+	/* If the card was removed the bus will be marked
+	 * as dead - extend the wakelock so userspace
+	 * can respond */
+	if (host->bus_dead)
+		extend_wakelock = 1;
+
 	/*
 	 * Let mmc_bus_put() free the bus/bus_ops if we've found that
 	 * the card is no longer present.
@@ -1803,16 +2081,24 @@ void mmc_rescan(struct work_struct *work)
 
 	mmc_claim_host(host);
 	for (i = 0; i < ARRAY_SIZE(freqs); i++) {
-		if (!mmc_rescan_try_freq(host, max(freqs[i], host->f_min)))
+		if (!mmc_rescan_try_freq(host, max(freqs[i], host->f_min))) {
+			extend_wakelock = true;
 			break;
+		}
 		if (freqs[i] <= host->f_min)
 			break;
 	}
 	mmc_release_host(host);
 
  out:
-	if (host->caps & MMC_CAP_NEEDS_POLL)
+	if (extend_wakelock)
+		wake_lock_timeout(&host->detect_wake_lock, HZ / 2);
+	else
+		wake_unlock(&host->detect_wake_lock);
+	if (host->caps & MMC_CAP_NEEDS_POLL) {
+		wake_lock(&host->detect_wake_lock);
 		mmc_schedule_delayed_work(&host->detect, HZ);
+	}
 }
 
 void mmc_start_host(struct mmc_host *host)
@@ -1832,7 +2118,8 @@ void mmc_stop_host(struct mmc_host *host)
 
 	if (host->caps & MMC_CAP_DISABLE)
 		cancel_delayed_work(&host->disable);
-	cancel_delayed_work_sync(&host->detect);
+	if (cancel_delayed_work_sync(&host->detect))
+		wake_unlock(&host->detect_wake_lock);
 	mmc_flush_scheduled_work();
 
 	/* clear pm flags now and let card drivers set them as needed */
@@ -1845,6 +2132,7 @@ void mmc_stop_host(struct mmc_host *host)
 
 		mmc_claim_host(host);
 		mmc_detach_bus(host);
+		mmc_power_off(host);
 		mmc_release_host(host);
 		mmc_bus_put(host);
 		return;
@@ -1856,6 +2144,26 @@ void mmc_stop_host(struct mmc_host *host)
 	mmc_power_off(host);
 }
 
+int mmc_speed_class_control(struct mmc_host *host,
+	unsigned int speed_class_ctrl_arg)
+{
+	int err = -ENOSYS;
+	u32 status;
+
+	err = mmc_send_speed_class_ctrl(host, speed_class_ctrl_arg);
+	if (err)
+		return err;
+
+	/* Issue CMD13 to check for any errors during the busy period of CMD20 */
+	err = mmc_send_status(host->card, &status);
+	if (!err) {
+		if (status & R1_ERROR)
+			err = -EINVAL;
+	}
+	return err;
+}
+EXPORT_SYMBOL(mmc_speed_class_control);
+
 int mmc_power_save_host(struct mmc_host *host)
 {
 	int ret = 0;
@@ -1910,6 +2218,9 @@ int mmc_card_awake(struct mmc_host *host)
 {
 	int err = -ENOSYS;
 
+	if (host->caps2 & MMC_CAP2_NO_SLEEP_CMD)
+		return 0;
+
 	mmc_bus_get(host);
 
 	if (host->bus_ops && !host->bus_dead && host->bus_ops->awake)
@@ -1925,6 +2236,9 @@ int mmc_card_sleep(struct mmc_host *host)
 {
 	int err = -ENOSYS;
 
+	if (host->caps2 & MMC_CAP2_NO_SLEEP_CMD)
+		return 0;
+
 	mmc_bus_get(host);
 
 	if (host->bus_ops && !host->bus_dead && host->bus_ops->awake)
@@ -1956,9 +2270,17 @@ int mmc_suspend_host(struct mmc_host *host)
 {
 	int err = 0;
 
+	if (mmc_bus_needs_resume(host))
+		return 0;
+
+	if (mmc_card_mmc(host->card) && mmc_card_doing_bkops(host->card))
+		mmc_interrupt_hpi(host->card);
+	mmc_card_clr_need_bkops(host->card);
+
 	if (host->caps & MMC_CAP_DISABLE)
 		cancel_delayed_work(&host->disable);
-	cancel_delayed_work(&host->detect);
+	if (cancel_delayed_work(&host->detect))
+		wake_unlock(&host->detect_wake_lock);
 	mmc_flush_scheduled_work();
 
 	mmc_bus_get(host);
@@ -1974,6 +2296,7 @@ int mmc_suspend_host(struct mmc_host *host)
 				host->bus_ops->remove(host);
 			mmc_claim_host(host);
 			mmc_detach_bus(host);
+			mmc_power_off(host);
 			mmc_release_host(host);
 			host->pm_flags = 0;
 			err = 0;
@@ -1998,6 +2321,12 @@ int mmc_resume_host(struct mmc_host *host)
 	int err = 0;
 
 	mmc_bus_get(host);
+	if (mmc_bus_manual_resume(host)) {
+		host->bus_resume_flags |= MMC_BUSRESUME_NEEDS_RESUME;
+		mmc_bus_put(host);
+		return 0;
+	}
+
 	if (host->bus_ops && !host->bus_dead) {
 		if (!mmc_card_keep_power(host)) {
 			mmc_power_up(host);
@@ -2042,15 +2371,22 @@ int mmc_pm_notify(struct notifier_block *notify_block,
 		notify_block, struct mmc_host, pm_notify);
 	unsigned long flags;
 
+	printk("[mmc]mmc_pm_notify start\n");
+	MMC_printk("%s: mode %ld, bus_resume_flags %d rescan_disable %d", mmc_hostname(host), mode, host->bus_resume_flags, host->rescan_disable);
 
 	switch (mode) {
 	case PM_HIBERNATION_PREPARE:
 	case PM_SUSPEND_PREPARE:
 
 		spin_lock_irqsave(&host->lock, flags);
+		if (mmc_bus_needs_resume(host)) {
+			spin_unlock_irqrestore(&host->lock, flags);
+			break;
+		}
 		host->rescan_disable = 1;
 		spin_unlock_irqrestore(&host->lock, flags);
-		cancel_delayed_work_sync(&host->detect);
+		if (cancel_delayed_work_sync(&host->detect))
+			wake_unlock(&host->detect_wake_lock);
 
 		if (!host->bus_ops || host->bus_ops->suspend)
 			break;
@@ -2061,8 +2397,10 @@ int mmc_pm_notify(struct notifier_block *notify_block,
 			host->bus_ops->remove(host);
 
 		mmc_detach_bus(host);
+		mmc_power_off(host);
 		mmc_release_host(host);
 		host->pm_flags = 0;
+		MMC_printk("mode %ld ended", mode);
 		break;
 
 	case PM_POST_SUSPEND:
@@ -2070,16 +2408,38 @@ int mmc_pm_notify(struct notifier_block *notify_block,
 	case PM_POST_RESTORE:
 
 		spin_lock_irqsave(&host->lock, flags);
+		if (mmc_bus_manual_resume(host)) {
+			spin_unlock_irqrestore(&host->lock, flags);
+			break;
+		}
 		host->rescan_disable = 0;
 		spin_unlock_irqrestore(&host->lock, flags);
 		mmc_detect_change(host, 0);
 
 	}
 
+	MMC_printk("%s finished", mmc_hostname(host));
+
 	return 0;
 }
 #endif
 
+#ifdef CONFIG_MMC_EMBEDDED_SDIO
+void mmc_set_embedded_sdio_data(struct mmc_host *host,
+				struct sdio_cis *cis,
+				struct sdio_cccr *cccr,
+				struct sdio_embedded_func *funcs,
+				int num_funcs)
+{
+	host->embedded_sdio_data.cis = cis;
+	host->embedded_sdio_data.cccr = cccr;
+	host->embedded_sdio_data.funcs = funcs;
+	host->embedded_sdio_data.num_funcs = num_funcs;
+}
+
+EXPORT_SYMBOL(mmc_set_embedded_sdio_data);
+#endif
+
 static int __init mmc_init(void)
 {
 	int ret;
diff --git a/drivers/mmc/core/core.h b/drivers/mmc/core/core.h
index d9411ed2a39b..14664f1fb16f 100644
--- a/drivers/mmc/core/core.h
+++ b/drivers/mmc/core/core.h
@@ -43,6 +43,7 @@ int mmc_set_signal_voltage(struct mmc_host *host, int signal_voltage,
 			   bool cmd11);
 void mmc_set_timing(struct mmc_host *host, unsigned int timing);
 void mmc_set_driver_type(struct mmc_host *host, unsigned int drv_type);
+void mmc_power_off(struct mmc_host *host);
 
 static inline void mmc_delay(unsigned int ms)
 {
diff --git a/drivers/mmc/core/host.c b/drivers/mmc/core/host.c
index 793d0a0dad8d..e09f0a7eb652 100644
--- a/drivers/mmc/core/host.c
+++ b/drivers/mmc/core/host.c
@@ -284,6 +284,8 @@ struct mmc_host *mmc_alloc_host(int extra, struct device *dev)
 
 	spin_lock_init(&host->lock);
 	init_waitqueue_head(&host->wq);
+	wake_lock_init(&host->detect_wake_lock, WAKE_LOCK_SUSPEND,
+		kasprintf(GFP_KERNEL, "%s_detect", mmc_hostname(host)));
 	INIT_DELAYED_WORK(&host->detect, mmc_rescan);
 	INIT_DELAYED_WORK_DEFERRABLE(&host->disable, mmc_host_deeper_disable);
 #ifdef CONFIG_PM
@@ -336,7 +338,8 @@ int mmc_add_host(struct mmc_host *host)
 #endif
 
 	mmc_start_host(host);
-	register_pm_notifier(&host->pm_notify);
+	if (!(host->pm_flags & MMC_PM_IGNORE_PM_NOTIFY))
+		register_pm_notifier(&host->pm_notify);
 
 	return 0;
 }
@@ -353,7 +356,9 @@ EXPORT_SYMBOL(mmc_add_host);
  */
 void mmc_remove_host(struct mmc_host *host)
 {
-	unregister_pm_notifier(&host->pm_notify);
+	if (!(host->pm_flags & MMC_PM_IGNORE_PM_NOTIFY))
+		unregister_pm_notifier(&host->pm_notify);
+
 	mmc_stop_host(host);
 
 #ifdef CONFIG_DEBUG_FS
@@ -380,6 +385,7 @@ void mmc_free_host(struct mmc_host *host)
 	spin_lock(&mmc_host_lock);
 	idr_remove(&mmc_host_idr, host->index);
 	spin_unlock(&mmc_host_lock);
+	wake_lock_destroy(&host->detect_wake_lock);
 
 	put_device(&host->class_dev);
 }
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index 5700b1cbdfec..9af23fe2ae8f 100644..100755
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -4,6 +4,7 @@
  *  Copyright (C) 2003-2004 Russell King, All Rights Reserved.
  *  Copyright (C) 2005-2007 Pierre Ossman, All Rights Reserved.
  *  MMCv4 support Copyright (C) 2006 Philip Langdale, All Rights Reserved.
+ *  Copyright (c) 2012 NVIDIA 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 version 2 as
@@ -22,6 +23,8 @@
 #include "mmc_ops.h"
 #include "sd_ops.h"
 
+#include "../debug_mmc.h"
+
 static const unsigned int tran_exp[] = {
 	10000,		100000,		1000000,	10000000,
 	0,		0,		0,		0
@@ -95,6 +98,7 @@ static int mmc_decode_cid(struct mmc_card *card)
 		card->cid.prod_name[3]	= UNSTUFF_BITS(resp, 72, 8);
 		card->cid.prod_name[4]	= UNSTUFF_BITS(resp, 64, 8);
 		card->cid.prod_name[5]	= UNSTUFF_BITS(resp, 56, 8);
+		card->cid.prv		= UNSTUFF_BITS(resp, 48, 8);
 		card->cid.serial	= UNSTUFF_BITS(resp, 16, 32);
 		card->cid.month		= UNSTUFF_BITS(resp, 12, 4);
 		card->cid.year		= UNSTUFF_BITS(resp, 8, 4) + 1997;
@@ -106,6 +110,8 @@ static int mmc_decode_cid(struct mmc_card *card)
 		return -EINVAL;
 	}
 
+	MMC_printk("cid.prv 0x%x", card->cid.prv);
+
 	return 0;
 }
 
@@ -258,13 +264,12 @@ static int mmc_read_ext_csd(struct mmc_card *card, u8 *ext_csd)
 		}
 	}
 
+	/*
+	 * The EXT_CSD format is meant to be forward compatible. As long
+	 * as CSD_STRUCTURE does not change, all values for EXT_CSD_REV
+	 * are authorized, see JEDEC JESD84-B50 section B.8.
+	 */
 	card->ext_csd.rev = ext_csd[EXT_CSD_REV];
-	if (card->ext_csd.rev > 6) {
-		printk(KERN_ERR "%s: unrecognised EXT_CSD revision %d\n",
-			mmc_hostname(card->host), card->ext_csd.rev);
-		err = -EINVAL;
-		goto out;
-	}
 
 	card->ext_csd.raw_sectors[0] = ext_csd[EXT_CSD_SEC_CNT + 0];
 	card->ext_csd.raw_sectors[1] = ext_csd[EXT_CSD_SEC_CNT + 1];
@@ -276,6 +281,8 @@ static int mmc_read_ext_csd(struct mmc_card *card, u8 *ext_csd)
 			ext_csd[EXT_CSD_SEC_CNT + 1] << 8 |
 			ext_csd[EXT_CSD_SEC_CNT + 2] << 16 |
 			ext_csd[EXT_CSD_SEC_CNT + 3] << 24;
+		MMC_printk("ext_csd.sectors 0x%x prod_name %s BOOT_MULTI 0x%x", card->ext_csd.sectors, card->cid.prod_name, ext_csd[EXT_CSD_BOOT_MULT]);
+		card->ext_csd.sec_count = card->ext_csd.sectors;
 
 		/* Cards with density > 2GiB are sector addressed */
 		if (card->ext_csd.sectors > (2u * 1024 * 1024 * 1024) / 512)
@@ -359,6 +366,7 @@ static int mmc_read_ext_csd(struct mmc_card *card, u8 *ext_csd)
 		 * card has the Enhanced area enabled.  If so, export enhanced
 		 * area offset and size to user by adding sysfs interface.
 		 */
+		card->ext_csd.raw_partition_support = ext_csd[EXT_CSD_PARTITION_SUPPORT];
 		if ((ext_csd[EXT_CSD_PARTITION_SUPPORT] & 0x2) &&
 		    (ext_csd[EXT_CSD_PARTITION_ATTRIBUTE] & 0x1)) {
 			u8 hc_erase_grp_sz =
@@ -402,14 +410,42 @@ static int mmc_read_ext_csd(struct mmc_card *card, u8 *ext_csd)
 			ext_csd[EXT_CSD_TRIM_MULT];
 	}
 
-	if (card->ext_csd.rev >= 5)
+	card->ext_csd.raw_erased_mem_count = ext_csd[EXT_CSD_ERASED_MEM_CONT];
+	if (card->ext_csd.rev >= 5) {
 		card->ext_csd.rel_param = ext_csd[EXT_CSD_WR_REL_PARAM];
+		/* check whether the eMMC card supports HPI */
+		if (ext_csd[EXT_CSD_HPI_FEATURES] & 0x1) {
+			card->ext_csd.hpi = 1;
+			if (ext_csd[EXT_CSD_HPI_FEATURES] & 0x2)
+				card->ext_csd.hpi_cmd = MMC_STOP_TRANSMISSION;
+			else
+				card->ext_csd.hpi_cmd = MMC_SEND_STATUS;
+			/*
+			 * Indicate the maximum timeout to close
+			 * a command interrupted by HPI
+			 */
+			card->ext_csd.out_of_int_time =
+				ext_csd[EXT_CSD_OUT_OF_INTERRUPT_TIME] * 10;
+		}
+
+		/* Check whether the eMMC card supports background ops */
+		if (ext_csd[EXT_CSD_BKOPS_SUPPORT] & 0x1)
+			card->ext_csd.bk_ops = 1;
+
+		/* Check whether the eMMC card needs proactive refresh */
+		if ((card->cid.manfid == 0x90) && ((card->cid.prv == 0x73)
+			|| (card->cid.prv == 0x7b)))
+			card->ext_csd.refresh = 1;
+	}
 
 	if (ext_csd[EXT_CSD_ERASED_MEM_CONT])
 		card->erased_byte = 0xFF;
 	else
 		card->erased_byte = 0x0;
 
+	if ((card->cid.manfid == 0x15) && (ext_csd[64] & 0x01))
+		card->ext_csd.feature_support |= MMC_DISCARD_FEATURE;
+
 out:
 	return err;
 }
@@ -494,10 +530,12 @@ MMC_DEV_ATTR(hwrev, "0x%x\n", card->cid.hwrev);
 MMC_DEV_ATTR(manfid, "0x%06x\n", card->cid.manfid);
 MMC_DEV_ATTR(name, "%s\n", card->cid.prod_name);
 MMC_DEV_ATTR(oemid, "0x%04x\n", card->cid.oemid);
+MMC_DEV_ATTR(prv, "0x%x\n", card->cid.prv);
 MMC_DEV_ATTR(serial, "0x%08x\n", card->cid.serial);
 MMC_DEV_ATTR(enhanced_area_offset, "%llu\n",
 		card->ext_csd.enhanced_area_offset);
 MMC_DEV_ATTR(enhanced_area_size, "%u\n", card->ext_csd.enhanced_area_size);
+MMC_DEV_ATTR(sec_count, "0x%x\n", card->ext_csd.sec_count);
 
 static struct attribute *mmc_std_attrs[] = {
 	&dev_attr_cid.attr,
@@ -510,9 +548,11 @@ static struct attribute *mmc_std_attrs[] = {
 	&dev_attr_manfid.attr,
 	&dev_attr_name.attr,
 	&dev_attr_oemid.attr,
+	&dev_attr_prv.attr,
 	&dev_attr_serial.attr,
 	&dev_attr_enhanced_area_offset.attr,
 	&dev_attr_enhanced_area_size.attr,
+	&dev_attr_sec_count.attr,
 	NULL,
 };
 
@@ -638,29 +678,38 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 			goto free_card;
 	}
 
-	if (!oldcard) {
-		/*
-		 * Fetch and process extended CSD.
-		 */
-
-		err = mmc_get_ext_csd(card, &ext_csd);
-		if (err)
-			goto free_card;
-		err = mmc_read_ext_csd(card, ext_csd);
-		if (err)
-			goto free_card;
-
-		/* If doing byte addressing, check if required to do sector
-		 * addressing.  Handle the case of <2GB cards needing sector
-		 * addressing.  See section 8.1 JEDEC Standard JED84-A441;
-		 * ocr register has bit 30 set for sector addressing.
-		 */
-		if (!(mmc_card_blockaddr(card)) && (rocr & (1<<30)))
-			mmc_card_set_blockaddr(card);
+	/*
+	 * Fetch and process extended CSD.
+	 */
 
-		/* Erase size depends on CSD and Extended CSD */
-		mmc_set_erase_size(card);
+	err = mmc_get_ext_csd(card, &ext_csd);
+	if (err)
+		goto free_card;
+	err = mmc_read_ext_csd(card, ext_csd);
+	if (err)
+		goto free_card;
+
+	if (card->ext_csd.refresh) {
+		init_timer(&card->timer);
+		card->timer.data = (unsigned long) card;
+		card->timer.function = mmc_refresh;
+		card->timer.expires = MMC_BKOPS_INTERVAL <
+			MMC_REFRESH_INTERVAL ? MMC_BKOPS_INTERVAL :
+			MMC_REFRESH_INTERVAL;
+		card->timer.expires *= HZ;
+		card->timer.expires += jiffies;
+		add_timer(&card->timer);
 	}
+	/* If doing byte addressing, check if required to do sector
+	 * addressing.  Handle the case of <2GB cards needing sector
+	 * addressing.  See section 8.1 JEDEC Standard JED84-A441;
+	 * ocr register has bit 30 set for sector addressing.
+	 */
+	if (!(mmc_card_blockaddr(card)) && (rocr & (1<<30)))
+		mmc_card_set_blockaddr(card);
+
+	/* Erase size depends on CSD and Extended CSD */
+	mmc_set_erase_size(card);
 
 	/*
 	 * If enhanced_area_en is TRUE, host needs to enable ERASE_GRP_DEF
@@ -726,6 +775,40 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 	}
 
 	/*
+	 * Enable HPI feature (if supported)
+	 */
+	if (card->ext_csd.hpi && (card->host->caps & MMC_CAP_BKOPS)) {
+		err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+			EXT_CSD_HPI_MGMT, 1, 0);
+		if (err && err != -EBADMSG)
+			goto free_card;
+		if (err) {
+			pr_warning("%s: Enabling HPI failed\n",
+				   mmc_hostname(card->host));
+			err = 0;
+		} else {
+			card->ext_csd.hpi_en = 1;
+		}
+	}
+
+	/*
+	 * Enable Background ops feature (if supported)
+	 */
+	if (card->ext_csd.bk_ops && (card->host->caps & MMC_CAP_BKOPS)) {
+		err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+			EXT_CSD_BKOPS_EN, 1, 0);
+		if (err && err != -EBADMSG)
+			goto free_card;
+		if (err) {
+			pr_warning("%s: Enabling BK ops failed\n",
+				   mmc_hostname(card->host));
+			err = 0;
+		} else {
+			card->ext_csd.bk_ops_en = 1;
+		}
+	}
+
+	/*
 	 * Compute bus speed.
 	 */
 	max_dtr = (unsigned int)-1;
@@ -891,6 +974,7 @@ static void mmc_detect(struct mmc_host *host)
 
 		mmc_claim_host(host);
 		mmc_detach_bus(host);
+		mmc_power_off(host);
 		mmc_release_host(host);
 	}
 }
@@ -900,13 +984,19 @@ static void mmc_detect(struct mmc_host *host)
  */
 static int mmc_suspend(struct mmc_host *host)
 {
+	int err;
+
 	BUG_ON(!host);
 	BUG_ON(!host->card);
 
 	mmc_claim_host(host);
-	if (!mmc_host_is_spi(host))
+	if (mmc_card_can_sleep(host)) {
+		err = mmc_card_sleep(host);
+		if (!err)
+			mmc_card_set_sleep(host->card);
+	} else if (!mmc_host_is_spi(host))
 		mmc_deselect_cards(host);
-	host->card->state &= ~MMC_STATE_HIGHSPEED;
+	host->card->state &= ~(MMC_STATE_HIGHSPEED | MMC_STATE_HIGHSPEED_200);
 	mmc_release_host(host);
 
 	return 0;
@@ -926,7 +1016,11 @@ static int mmc_resume(struct mmc_host *host)
 	BUG_ON(!host->card);
 
 	mmc_claim_host(host);
-	err = mmc_init_card(host, host->ocr, host->card);
+	if (mmc_card_is_sleep(host->card)) {
+		err = mmc_card_awake(host);
+		mmc_card_clr_sleep(host->card);
+	} else
+		err = mmc_init_card(host, host->ocr, host->card);
 	mmc_release_host(host);
 
 	return err;
@@ -936,7 +1030,8 @@ static int mmc_power_restore(struct mmc_host *host)
 {
 	int ret;
 
-	host->card->state &= ~MMC_STATE_HIGHSPEED;
+	host->card->state &= ~(MMC_STATE_HIGHSPEED | MMC_STATE_HIGHSPEED_200);
+	mmc_card_clr_sleep(host->card);
 	mmc_claim_host(host);
 	ret = mmc_init_card(host, host->ocr, host->card);
 	mmc_release_host(host);
diff --git a/drivers/mmc/core/mmc_ops.c b/drivers/mmc/core/mmc_ops.c
index 770c3d06f5dc..c85c58aca3e2 100644
--- a/drivers/mmc/core/mmc_ops.c
+++ b/drivers/mmc/core/mmc_ops.c
@@ -547,3 +547,136 @@ int mmc_bus_test(struct mmc_card *card, u8 bus_width)
 	err = mmc_send_bus_test(card, card->host, MMC_BUS_TEST_R, width);
 	return err;
 }
+
+int mmc_send_hpi_cmd(struct mmc_card *card, u32 *status)
+{
+	struct mmc_command cmd = {0};
+	unsigned int opcode;
+	unsigned int flags;
+	int err;
+
+	opcode = card->ext_csd.hpi_cmd;
+	flags = MMC_RSP_R1 | MMC_CMD_AC;
+
+	cmd.opcode = opcode;
+	cmd.arg = card->rca << 16 | 1;
+	cmd.flags = flags;
+
+	err = mmc_wait_for_cmd(card->host, &cmd, 0);
+	if (err) {
+		pr_warn("%s: error %d interrupting operation. "
+			"HPI command response %#x\n", mmc_hostname(card->host),
+			err, cmd.resp[0]);
+		return err;
+	}
+	if (status)
+		*status = cmd.resp[0];
+
+	return 0;
+}
+
+int mmc_send_bk_ops_cmd(struct mmc_card *card, bool is_synchronous)
+{
+	int err;
+	struct mmc_command cmd;
+	u32 status;
+
+	BUG_ON(!card);
+	BUG_ON(!card->host);
+
+	memset(&cmd, 0, sizeof(struct mmc_command));
+
+	cmd.opcode = MMC_SWITCH;
+	cmd.arg = (MMC_SWITCH_MODE_WRITE_BYTE << 24) |
+		(EXT_CSD_BKOPS_START << 16) |
+		(1 << 8) |
+		EXT_CSD_CMD_SET_NORMAL;
+	if (is_synchronous)
+		cmd.flags = MMC_RSP_R1B | MMC_CMD_AC;
+	else
+		cmd.flags = MMC_RSP_R1 | MMC_CMD_AC;
+
+	err = mmc_wait_for_cmd(card->host, &cmd, MMC_CMD_RETRIES);
+	if (err)
+		return err;
+
+	/* Must check status to be sure of no errors */
+	do {
+		err = mmc_send_status(card, &status);
+		if (err)
+			return err;
+		if (card->host->caps & MMC_CAP_WAIT_WHILE_BUSY)
+			break;
+	} while (R1_CURRENT_STATE(status) == 7);
+
+	if (status & 0xFDFFA000)
+		printk(KERN_ERR "%s: unexpected status %#x after "
+			   "switch", mmc_hostname(card->host), status);
+	if (status & R1_SWITCH_ERROR)
+		return -EBADMSG;
+
+	return 0;
+}
+
+int mmc_gen_cmd(struct mmc_card *card, void *buf, u8 index, u8 arg1, u8 arg2, u8 mode)
+{
+	struct mmc_request mrq;
+	struct mmc_command cmd;
+	struct mmc_data data;
+	struct mmc_command stop;
+	struct scatterlist sg;
+	void *data_buf;
+
+	mmc_set_blocklen(card, 512);
+
+	data_buf = kmalloc(512, GFP_KERNEL);
+	if (data_buf == NULL)
+		return -ENOMEM;
+
+	memset(&mrq, 0, sizeof(struct mmc_request));
+	memset(&cmd, 0, sizeof(struct mmc_command));
+	memset(&data, 0, sizeof(struct mmc_data));
+	memset(&stop, 0, sizeof(struct mmc_command));
+
+	mrq.cmd = &cmd;
+	mrq.data = &data;
+	mrq.stop = &stop;
+
+	cmd.opcode = MMC_GEN_CMD;
+	cmd.arg = (arg2 << 16) |
+		  (arg1 << 8) |
+		  (index << 1) |
+		  mode;
+
+	cmd.flags = MMC_RSP_SPI_R1 | MMC_RSP_R1 | MMC_CMD_ADTC;
+
+	data.blksz = 512;
+	data.blocks = 1;
+	data.flags = MMC_DATA_READ;
+	data.sg = &sg;
+	data.sg_len = 1;
+
+	stop.opcode = MMC_STOP_TRANSMISSION;
+	stop.arg = 0;
+	stop.flags = MMC_RSP_SPI_R1B | MMC_RSP_R1B | MMC_CMD_AC;
+
+	sg_init_one(&sg, data_buf, 512);
+
+	mmc_set_data_timeout(&data, card);
+
+	mmc_claim_host(card->host);
+	mmc_wait_for_req(card->host, &mrq);
+	mmc_release_host(card->host);
+
+	memcpy(buf, data_buf, 512);
+	kfree(data_buf);
+
+	if (cmd.error)
+		return cmd.error;
+	if (data.error)
+		return data.error;
+	if (stop.error)
+		return stop.error;
+
+	return 0;
+}
diff --git a/drivers/mmc/core/mmc_ops.h b/drivers/mmc/core/mmc_ops.h
index 9276946fa5b7..a453531b46b6 100644
--- a/drivers/mmc/core/mmc_ops.h
+++ b/drivers/mmc/core/mmc_ops.h
@@ -2,6 +2,7 @@
  *  linux/drivers/mmc/core/mmc_ops.h
  *
  *  Copyright 2006-2007 Pierre Ossman
+ *  Copyright (c) 2012 NVIDIA 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
@@ -26,6 +27,9 @@ int mmc_spi_read_ocr(struct mmc_host *host, int highcap, u32 *ocrp);
 int mmc_spi_set_crc(struct mmc_host *host, int use_crc);
 int mmc_card_sleepawake(struct mmc_host *host, int sleep);
 int mmc_bus_test(struct mmc_card *card, u8 bus_width);
+int mmc_send_hpi_cmd(struct mmc_card *card, u32 *status);
+int mmc_send_bk_ops_cmd(struct mmc_card *card, bool is_synchronous);
+int mmc_gen_cmd(struct mmc_card *card, void *buf, u8 index, u8 arg1, u8 arg2, u8 mode);
 
 #endif
 
diff --git a/drivers/mmc/core/sd.c b/drivers/mmc/core/sd.c
index 0370e03e3142..cb2a9d4d4515 100644
--- a/drivers/mmc/core/sd.c
+++ b/drivers/mmc/core/sd.c
@@ -348,11 +348,11 @@ static int mmc_read_switch(struct mmc_card *card)
 		}
 
 		card->sw_caps.sd3_curr_limit = status[7];
-	} else {
-		if (status[13] & 0x02)
-			card->sw_caps.hs_max_dtr = 50000000;
 	}
 
+	if (status[13] & 0x02)
+		card->sw_caps.hs_max_dtr = 50000000;
+
 out:
 	kfree(status);
 
@@ -542,6 +542,8 @@ static int sd_set_bus_speed_mode(struct mmc_card *card, u8 *status)
 			mmc_hostname(card->host));
 	else {
 		mmc_set_timing(card->host, timing);
+		if (timing == MMC_TIMING_UHS_DDR50)
+			mmc_card_set_ddr_mode(card);
 		mmc_set_clock(card->host, card->sw_caps.uhs_max_dtr);
 	}
 
@@ -799,6 +801,9 @@ int mmc_sd_setup_card(struct mmc_host *host, struct mmc_card *card,
 	bool reinit)
 {
 	int err;
+#ifdef CONFIG_MMC_PARANOID_SD_INIT
+	int retries;
+#endif
 
 	if (!reinit) {
 		/*
@@ -825,7 +830,26 @@ int mmc_sd_setup_card(struct mmc_host *host, struct mmc_card *card,
 		/*
 		 * Fetch switch information from card.
 		 */
+#ifdef CONFIG_MMC_PARANOID_SD_INIT
+		for (retries = 1; retries <= 3; retries++) {
+			err = mmc_read_switch(card);
+			if (!err) {
+				if (retries > 1) {
+					printk(KERN_WARNING
+					       "%s: recovered\n", 
+					       mmc_hostname(host));
+				}
+				break;
+			} else {
+				printk(KERN_WARNING
+				       "%s: read switch failed (attempt %d)\n",
+				       mmc_hostname(host), retries);
+			}
+		}
+#else
 		err = mmc_read_switch(card);
+#endif
+
 		if (err)
 			return err;
 	}
@@ -1024,18 +1048,36 @@ static void mmc_sd_remove(struct mmc_host *host)
  */
 static void mmc_sd_detect(struct mmc_host *host)
 {
-	int err;
+	int err = 0;
+#ifdef CONFIG_MMC_PARANOID_SD_INIT
+        int retries = 5;
+#endif
 
 	BUG_ON(!host);
 	BUG_ON(!host->card);
-
+       
 	mmc_claim_host(host);
 
 	/*
 	 * Just check if our card has been removed.
 	 */
+#ifdef CONFIG_MMC_PARANOID_SD_INIT
+	while(retries) {
+		err = mmc_send_status(host->card, NULL);
+		if (err) {
+			retries--;
+			udelay(5);
+			continue;
+		}
+		break;
+	}
+	if (!retries) {
+		printk(KERN_ERR "%s(%s): Unable to re-detect card (%d)\n",
+		       __func__, mmc_hostname(host), err);
+	}
+#else
 	err = mmc_send_status(host->card, NULL);
-
+#endif
 	mmc_release_host(host);
 
 	if (err) {
@@ -1043,6 +1085,7 @@ static void mmc_sd_detect(struct mmc_host *host)
 
 		mmc_claim_host(host);
 		mmc_detach_bus(host);
+		mmc_power_off(host);
 		mmc_release_host(host);
 	}
 }
@@ -1073,12 +1116,31 @@ static int mmc_sd_suspend(struct mmc_host *host)
 static int mmc_sd_resume(struct mmc_host *host)
 {
 	int err;
+#ifdef CONFIG_MMC_PARANOID_SD_INIT
+	int retries;
+#endif
 
 	BUG_ON(!host);
 	BUG_ON(!host->card);
 
 	mmc_claim_host(host);
+#ifdef CONFIG_MMC_PARANOID_SD_INIT
+	retries = 5;
+	while (retries) {
+		err = mmc_sd_init_card(host, host->ocr, host->card);
+
+		if (err) {
+			printk(KERN_ERR "%s: Re-init card rc = %d (retries = %d)\n",
+			       mmc_hostname(host), err, retries);
+			mdelay(5);
+			retries--;
+			continue;
+		}
+		break;
+	}
+#else
 	err = mmc_sd_init_card(host, host->ocr, host->card);
+#endif
 	mmc_release_host(host);
 
 	return err;
@@ -1130,6 +1192,9 @@ int mmc_attach_sd(struct mmc_host *host)
 {
 	int err;
 	u32 ocr;
+#ifdef CONFIG_MMC_PARANOID_SD_INIT
+	int retries;
+#endif
 
 	BUG_ON(!host);
 	WARN_ON(!host->claimed);
@@ -1194,9 +1259,27 @@ int mmc_attach_sd(struct mmc_host *host)
 	/*
 	 * Detect and init the card.
 	 */
+#ifdef CONFIG_MMC_PARANOID_SD_INIT
+	retries = 5;
+	while (retries) {
+		err = mmc_sd_init_card(host, host->ocr, NULL);
+		if (err) {
+			retries--;
+			continue;
+		}
+		break;
+	}
+
+	if (!retries) {
+		printk(KERN_ERR "%s: mmc_sd_init_card() failure (err = %d)\n",
+		       mmc_hostname(host), err);
+		goto err;
+	}
+#else
 	err = mmc_sd_init_card(host, host->ocr, NULL);
 	if (err)
 		goto err;
+#endif
 
 	mmc_release_host(host);
 	err = mmc_add_card(host->card);
diff --git a/drivers/mmc/core/sd_ops.c b/drivers/mmc/core/sd_ops.c
index 021fed153804..b06781e69ce3 100644
--- a/drivers/mmc/core/sd_ops.c
+++ b/drivers/mmc/core/sd_ops.c
@@ -389,3 +389,29 @@ int mmc_app_sd_status(struct mmc_card *card, void *ssr)
 
 	return 0;
 }
+
+int mmc_send_speed_class_ctrl(struct mmc_host *host,
+	unsigned int speed_class_ctrl_arg)
+{
+	int err = 0;
+	struct mmc_command cmd = {
+			.opcode = SD_SPEED_CLASS_CONTROL,
+			.arg = (speed_class_ctrl_arg << 28),
+			.flags = MMC_RSP_R1B | MMC_CMD_AC | MMC_RSP_BUSY,
+		};
+
+	BUG_ON(!host);
+	BUG_ON(speed_class_ctrl_arg > 3);
+	err = mmc_wait_for_cmd(host, &cmd, MMC_CMD_RETRIES);
+	if (err)
+		return err;
+
+	/*
+	 * If the host does not wait while the card signals busy, then we will
+	 * will have to wait the max busy indication timeout.
+	 */
+	if (!(host->caps & MMC_CAP_WAIT_WHILE_BUSY))
+		mmc_delay(1000);
+	return err;
+}
+
diff --git a/drivers/mmc/core/sd_ops.h b/drivers/mmc/core/sd_ops.h
index ffc2305d905f..a77b8facceb4 100644
--- a/drivers/mmc/core/sd_ops.h
+++ b/drivers/mmc/core/sd_ops.h
@@ -20,6 +20,7 @@ int mmc_app_send_scr(struct mmc_card *card, u32 *scr);
 int mmc_sd_switch(struct mmc_card *card, int mode, int group,
 	u8 value, u8 *resp);
 int mmc_app_sd_status(struct mmc_card *card, void *ssr);
-
+int mmc_send_speed_class_ctrl(struct mmc_host *host,
+	unsigned int speed_class_ctrl_arg);
 #endif
 
diff --git a/drivers/mmc/core/sdio.c b/drivers/mmc/core/sdio.c
index 262fff019177..3d8a5e41a488 100644
--- a/drivers/mmc/core/sdio.c
+++ b/drivers/mmc/core/sdio.c
@@ -27,6 +27,10 @@
 #include "sdio_ops.h"
 #include "sdio_cis.h"
 
+#ifdef CONFIG_MMC_EMBEDDED_SDIO
+#include <linux/mmc/sdio_ids.h>
+#endif
+
 static int sdio_read_fbr(struct sdio_func *func)
 {
 	int ret;
@@ -102,6 +106,7 @@ static int sdio_read_cccr(struct mmc_card *card)
 	int ret;
 	int cccr_vsn;
 	unsigned char data;
+	unsigned char speed;
 
 	memset(&card->cccr, 0, sizeof(struct sdio_cccr));
 
@@ -111,8 +116,8 @@ static int sdio_read_cccr(struct mmc_card *card)
 
 	cccr_vsn = data & 0x0f;
 
-	if (cccr_vsn > SDIO_CCCR_REV_1_20) {
-		printk(KERN_ERR "%s: unrecognised CCCR structure version %d\n",
+	if (cccr_vsn > SDIO_CCCR_REV_3_00) {
+		pr_err("%s: unrecognised CCCR structure version %d\n",
 			mmc_hostname(card->host), cccr_vsn);
 		return -EINVAL;
 	}
@@ -140,12 +145,60 @@ static int sdio_read_cccr(struct mmc_card *card)
 	}
 
 	if (cccr_vsn >= SDIO_CCCR_REV_1_20) {
-		ret = mmc_io_rw_direct(card, 0, 0, SDIO_CCCR_SPEED, 0, &data);
+		ret = mmc_io_rw_direct(card, 0, 0, SDIO_CCCR_SPEED, 0, &speed);
 		if (ret)
 			goto out;
 
-		if (data & SDIO_SPEED_SHS)
-			card->cccr.high_speed = 1;
+		card->scr.sda_spec3 = 0;
+		card->sw_caps.sd3_bus_mode = 0;
+		card->sw_caps.sd3_drv_type = 0;
+		if (cccr_vsn >= SDIO_CCCR_REV_3_00) {
+			card->scr.sda_spec3 = 1;
+			ret = mmc_io_rw_direct(card, 0, 0,
+				SDIO_CCCR_UHS, 0, &data);
+			if (ret)
+				goto out;
+
+			if (card->host->caps &
+				(MMC_CAP_UHS_SDR12 | MMC_CAP_UHS_SDR25 |
+				 MMC_CAP_UHS_SDR50 | MMC_CAP_UHS_SDR104 |
+				 MMC_CAP_UHS_DDR50)) {
+				if (data & SDIO_UHS_DDR50)
+					card->sw_caps.sd3_bus_mode
+						|= SD_MODE_UHS_DDR50;
+
+				if (data & SDIO_UHS_SDR50)
+					card->sw_caps.sd3_bus_mode
+						|= SD_MODE_UHS_SDR50;
+
+				if (data & SDIO_UHS_SDR104)
+					card->sw_caps.sd3_bus_mode
+						|= SD_MODE_UHS_SDR104;
+			}
+
+			ret = mmc_io_rw_direct(card, 0, 0,
+				SDIO_CCCR_DRIVE_STRENGTH, 0, &data);
+			if (ret)
+				goto out;
+
+			if (data & SDIO_DRIVE_SDTA)
+				card->sw_caps.sd3_drv_type |= SD_DRIVER_TYPE_A;
+			if (data & SDIO_DRIVE_SDTC)
+				card->sw_caps.sd3_drv_type |= SD_DRIVER_TYPE_C;
+			if (data & SDIO_DRIVE_SDTD)
+				card->sw_caps.sd3_drv_type |= SD_DRIVER_TYPE_D;
+		}
+
+		/* if no uhs mode ensure we check for high speed */
+		if (!card->sw_caps.sd3_bus_mode) {
+			if (speed & SDIO_SPEED_SHS) {
+				card->cccr.high_speed = 1;
+				card->sw_caps.hs_max_dtr = 50000000;
+			} else {
+				card->cccr.high_speed = 0;
+				card->sw_caps.hs_max_dtr = 25000000;
+			}
+		}
 	}
 
 out:
@@ -327,6 +380,193 @@ static unsigned mmc_sdio_get_max_clock(struct mmc_card *card)
 	return max_dtr;
 }
 
+static unsigned char host_drive_to_sdio_drive(int host_strength)
+{
+	switch (host_strength) {
+	case MMC_SET_DRIVER_TYPE_A:
+		return SDIO_DTSx_SET_TYPE_A;
+	case MMC_SET_DRIVER_TYPE_B:
+		return SDIO_DTSx_SET_TYPE_B;
+	case MMC_SET_DRIVER_TYPE_C:
+		return SDIO_DTSx_SET_TYPE_C;
+	case MMC_SET_DRIVER_TYPE_D:
+		return SDIO_DTSx_SET_TYPE_D;
+	default:
+		return SDIO_DTSx_SET_TYPE_B;
+	}
+}
+
+static void sdio_select_driver_type(struct mmc_card *card)
+{
+	int host_drv_type = SD_DRIVER_TYPE_B;
+	int card_drv_type = SD_DRIVER_TYPE_B;
+	int drive_strength;
+	unsigned char card_strength;
+	int err;
+
+	/*
+	 * If the host doesn't support any of the Driver Types A,C or D,
+	 * or there is no board specific handler then default Driver
+	 * Type B is used.
+	 */
+	if (!(card->host->caps &
+		(MMC_CAP_DRIVER_TYPE_A |
+		 MMC_CAP_DRIVER_TYPE_C |
+		 MMC_CAP_DRIVER_TYPE_D)))
+		return;
+
+	if (!card->host->ops->select_drive_strength)
+		return;
+
+	if (card->host->caps & MMC_CAP_DRIVER_TYPE_A)
+		host_drv_type |= SD_DRIVER_TYPE_A;
+
+	if (card->host->caps & MMC_CAP_DRIVER_TYPE_C)
+		host_drv_type |= SD_DRIVER_TYPE_C;
+
+	if (card->host->caps & MMC_CAP_DRIVER_TYPE_D)
+		host_drv_type |= SD_DRIVER_TYPE_D;
+
+	if (card->sw_caps.sd3_drv_type & SD_DRIVER_TYPE_A)
+		card_drv_type |= SD_DRIVER_TYPE_A;
+
+	if (card->sw_caps.sd3_drv_type & SD_DRIVER_TYPE_C)
+		card_drv_type |= SD_DRIVER_TYPE_C;
+
+	if (card->sw_caps.sd3_drv_type & SD_DRIVER_TYPE_D)
+		card_drv_type |= SD_DRIVER_TYPE_D;
+
+	/*
+	 * The drive strength that the hardware can support
+	 * depends on the board design.  Pass the appropriate
+	 * information and let the hardware specific code
+	 * return what is possible given the options
+	 */
+	drive_strength = card->host->ops->select_drive_strength(
+		card->sw_caps.uhs_max_dtr,
+		host_drv_type, card_drv_type);
+
+	/* if error just use default for drive strength B */
+	err = mmc_io_rw_direct(card, 0, 0, SDIO_CCCR_DRIVE_STRENGTH, 0,
+		&card_strength);
+	if (err)
+		return;
+
+	card_strength &= ~(SDIO_DRIVE_DTSx_MASK<<SDIO_DRIVE_DTSx_SHIFT);
+	card_strength |= host_drive_to_sdio_drive(drive_strength);
+
+	err = mmc_io_rw_direct(card, 1, 0, SDIO_CCCR_DRIVE_STRENGTH,
+		card_strength, NULL);
+
+	/* if error default to drive strength B */
+	if (!err)
+		mmc_set_driver_type(card->host, drive_strength);
+}
+
+
+static int sdio_set_bus_speed_mode(struct mmc_card *card)
+{
+	unsigned int bus_speed, timing;
+	int err;
+	unsigned char speed;
+
+	/*
+	 * If the host doesn't support any of the UHS-I modes, fallback on
+	 * default speed.
+	 */
+	if (!(card->host->caps & (MMC_CAP_UHS_SDR12 | MMC_CAP_UHS_SDR25 |
+	    MMC_CAP_UHS_SDR50 | MMC_CAP_UHS_SDR104 | MMC_CAP_UHS_DDR50)))
+		return 0;
+
+	bus_speed = SDIO_SPEED_SDR12;
+	timing = MMC_TIMING_UHS_SDR12;
+	if ((card->host->caps & MMC_CAP_UHS_SDR104) &&
+	    (card->sw_caps.sd3_bus_mode & SD_MODE_UHS_SDR104)) {
+			bus_speed = SDIO_SPEED_SDR104;
+			timing = MMC_TIMING_UHS_SDR104;
+			card->sw_caps.uhs_max_dtr = UHS_SDR104_MAX_DTR;
+	} else if ((card->host->caps & MMC_CAP_UHS_DDR50) &&
+		   (card->sw_caps.sd3_bus_mode & SD_MODE_UHS_DDR50)) {
+			bus_speed = SDIO_SPEED_DDR50;
+			timing = MMC_TIMING_UHS_DDR50;
+			card->sw_caps.uhs_max_dtr = UHS_DDR50_MAX_DTR;
+	} else if ((card->host->caps & (MMC_CAP_UHS_SDR104 |
+		    MMC_CAP_UHS_SDR50)) && (card->sw_caps.sd3_bus_mode &
+		    SD_MODE_UHS_SDR50)) {
+			bus_speed = SDIO_SPEED_SDR50;
+			timing = MMC_TIMING_UHS_SDR50;
+			card->sw_caps.uhs_max_dtr = UHS_SDR50_MAX_DTR;
+	} else if ((card->host->caps & (MMC_CAP_UHS_SDR104 |
+		    MMC_CAP_UHS_SDR50 | MMC_CAP_UHS_SDR25)) &&
+		   (card->sw_caps.sd3_bus_mode & SD_MODE_UHS_SDR25)) {
+			bus_speed = SDIO_SPEED_SDR25;
+			timing = MMC_TIMING_UHS_SDR25;
+			card->sw_caps.uhs_max_dtr = UHS_SDR25_MAX_DTR;
+	} else if ((card->host->caps & (MMC_CAP_UHS_SDR104 |
+		    MMC_CAP_UHS_SDR50 | MMC_CAP_UHS_SDR25 |
+		    MMC_CAP_UHS_SDR12)) && (card->sw_caps.sd3_bus_mode &
+		    SD_MODE_UHS_SDR12)) {
+			bus_speed = SDIO_SPEED_SDR12;
+			timing = MMC_TIMING_UHS_SDR12;
+			card->sw_caps.uhs_max_dtr = UHS_SDR12_MAX_DTR;
+	}
+
+	err = mmc_io_rw_direct(card, 0, 0, SDIO_CCCR_SPEED, 0, &speed);
+	if (err)
+		return err;
+
+	speed &= ~SDIO_SPEED_BSS_MASK;
+	speed |= bus_speed;
+	err = mmc_io_rw_direct(card, 1, 0, SDIO_CCCR_SPEED, speed, NULL);
+	if (err)
+		return err;
+
+	if (bus_speed) {
+		mmc_set_timing(card->host, timing);
+		mmc_set_clock(card->host, card->sw_caps.uhs_max_dtr);
+	}
+
+	return 0;
+}
+
+/*
+ * UHS-I specific initialization procedure
+ */
+static int mmc_sdio_init_uhs_card(struct mmc_card *card)
+{
+	int err;
+
+	if (!card->scr.sda_spec3)
+		return 0;
+
+	/*
+	 * Switch to wider bus (if supported).
+	 */
+	if (card->host->caps & MMC_CAP_4_BIT_DATA) {
+		err = sdio_enable_4bit_bus(card);
+		if (err > 0) {
+			mmc_set_bus_width(card->host, MMC_BUS_WIDTH_4);
+			err = 0;
+		}
+	}
+
+	/* Set the driver strength for the card */
+	sdio_select_driver_type(card);
+
+	/* Set bus speed mode of the card */
+	err = sdio_set_bus_speed_mode(card);
+	if (err)
+		goto out;
+
+	/* Initialize and start re-tuning timer */
+	if (!mmc_host_is_spi(card->host) && card->host->ops->execute_tuning)
+		err = card->host->ops->execute_tuning(card->host);
+
+out:
+
+	return err;
+}
+
 /*
  * Handle the detection and initialisation of a card.
  *
@@ -394,6 +634,29 @@ static int mmc_sdio_init_card(struct mmc_host *host, u32 ocr,
 		host->ops->init_card(host, card);
 
 	/*
+	 * If the host and card support UHS-I mode request the card
+	 * to switch to 1.8V signaling level.  No 1.8v signalling if
+	 * UHS mode is not enabled to maintain compatibilty and some
+	 * systems that claim 1.8v signalling in fact do not support
+	 * it.
+	 */
+	if ((ocr & R4_18V_PRESENT) &&
+		(host->caps &
+			(MMC_CAP_UHS_SDR12 | MMC_CAP_UHS_SDR25 |
+			 MMC_CAP_UHS_SDR50 | MMC_CAP_UHS_SDR104 |
+			 MMC_CAP_UHS_DDR50))) {
+		err = mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_180, true);
+		if (err) {
+			ocr &= ~R4_18V_PRESENT;
+			host->ocr &= ~R4_18V_PRESENT;
+		}
+		err = 0;
+	} else {
+		ocr &= ~R4_18V_PRESENT;
+		host->ocr &= ~R4_18V_PRESENT;
+	}
+
+	/*
 	 * For native busses:  set card RCA and quit open drain mode.
 	 */
 	if (!powered_resume && !mmc_host_is_spi(host)) {
@@ -449,19 +712,35 @@ static int mmc_sdio_init_card(struct mmc_host *host, u32 ocr,
 		goto finish;
 	}
 
-	/*
-	 * Read the common registers.
-	 */
-	err = sdio_read_cccr(card);
-	if (err)
-		goto remove;
+#ifdef CONFIG_MMC_EMBEDDED_SDIO
+	if (host->embedded_sdio_data.cccr)
+		memcpy(&card->cccr, host->embedded_sdio_data.cccr, sizeof(struct sdio_cccr));
+	else {
+#endif
+		/*
+		 * Read the common registers.
+		 */
+		err = sdio_read_cccr(card);
+		if (err)
+			goto remove;
+#ifdef CONFIG_MMC_EMBEDDED_SDIO
+	}
+#endif
 
-	/*
-	 * Read the common CIS tuples.
-	 */
-	err = sdio_read_common_cis(card);
-	if (err)
-		goto remove;
+#ifdef CONFIG_MMC_EMBEDDED_SDIO
+	if (host->embedded_sdio_data.cis)
+		memcpy(&card->cis, host->embedded_sdio_data.cis, sizeof(struct sdio_cis));
+	else {
+#endif
+		/*
+		 * Read the common CIS tuples.
+		 */
+		err = sdio_read_common_cis(card);
+		if (err)
+			goto remove;
+#ifdef CONFIG_MMC_EMBEDDED_SDIO
+	}
+#endif
 
 	if (oldcard) {
 		int same = (card->cis.vendor == oldcard->cis.vendor &&
@@ -494,29 +773,39 @@ static int mmc_sdio_init_card(struct mmc_host *host, u32 ocr,
 	if (err)
 		goto remove;
 
-	/*
-	 * Switch to high-speed (if supported).
-	 */
-	err = sdio_enable_hs(card);
-	if (err > 0)
-		mmc_sd_go_highspeed(card);
-	else if (err)
-		goto remove;
+	/* Initialization sequence for UHS-I cards */
+	/* Only if card supports 1.8v and UHS signaling */
+	if ((ocr & R4_18V_PRESENT) && card->sw_caps.sd3_bus_mode) {
+		err = mmc_sdio_init_uhs_card(card);
+		if (err)
+			goto remove;
 
-	/*
-	 * Change to the card's maximum speed.
-	 */
-	mmc_set_clock(host, mmc_sdio_get_max_clock(card));
+		/* Card is an ultra-high-speed card */
+		mmc_sd_card_set_uhs(card);
+	} else {
+		/*
+		 * Switch to high-speed (if supported).
+		 */
+		err = sdio_enable_hs(card);
+		if (err > 0)
+			mmc_sd_go_highspeed(card);
+		else if (err)
+			goto remove;
 
-	/*
-	 * Switch to wider bus (if supported).
-	 */
-	err = sdio_enable_4bit_bus(card);
-	if (err > 0)
-		mmc_set_bus_width(card->host, MMC_BUS_WIDTH_4);
-	else if (err)
-		goto remove;
+		/*
+		 * Change to the card's maximum speed.
+		 */
+		mmc_set_clock(host, mmc_sdio_get_max_clock(card));
 
+		/*
+		 * Switch to wider bus (if supported).
+		 */
+		err = sdio_enable_4bit_bus(card);
+		if (err > 0)
+			mmc_set_bus_width(card->host, MMC_BUS_WIDTH_4);
+		else if (err)
+			goto remove;
+	}
 finish:
 	if (!oldcard)
 		host->card = card;
@@ -597,6 +886,7 @@ out:
 
 		mmc_claim_host(host);
 		mmc_detach_bus(host);
+		mmc_power_off(host);
 		mmc_release_host(host);
 	}
 }
@@ -798,8 +1088,17 @@ int mmc_attach_sdio(struct mmc_host *host)
 	 * Detect and init the card.
 	 */
 	err = mmc_sdio_init_card(host, host->ocr, NULL, 0);
-	if (err)
-		goto err;
+	if (err) {
+		if (err == -EAGAIN) {
+			/*
+			 * Retry initialization with S18R set to 0.
+			 */
+			host->ocr &= ~R4_18V_PRESENT;
+			err = mmc_sdio_init_card(host, host->ocr, NULL, 0);
+		}
+		if (err)
+			goto err;
+	}
 	card = host->card;
 
 	/*
@@ -826,14 +1125,36 @@ int mmc_attach_sdio(struct mmc_host *host)
 	funcs = (ocr & 0x70000000) >> 28;
 	card->sdio_funcs = 0;
 
+#ifdef CONFIG_MMC_EMBEDDED_SDIO
+	if (host->embedded_sdio_data.funcs)
+		card->sdio_funcs = funcs = host->embedded_sdio_data.num_funcs;
+#endif
+
 	/*
 	 * Initialize (but don't add) all present functions.
 	 */
 	for (i = 0; i < funcs; i++, card->sdio_funcs++) {
-		err = sdio_init_func(host->card, i + 1);
-		if (err)
-			goto remove;
-
+#ifdef CONFIG_MMC_EMBEDDED_SDIO
+		if (host->embedded_sdio_data.funcs) {
+			struct sdio_func *tmp;
+
+			tmp = sdio_alloc_func(host->card);
+			if (IS_ERR(tmp))
+				goto remove;
+			tmp->num = (i + 1);
+			card->sdio_func[i] = tmp;
+			tmp->class = host->embedded_sdio_data.funcs[i].f_class;
+			tmp->max_blksize = host->embedded_sdio_data.funcs[i].f_maxblksize;
+			tmp->vendor = card->cis.vendor;
+			tmp->device = card->cis.device;
+		} else {
+#endif
+			err = sdio_init_func(host->card, i + 1);
+			if (err)
+				goto remove;
+#ifdef CONFIG_MMC_EMBEDDED_SDIO
+		}
+#endif
 		/*
 		 * Enable Runtime PM for this func (if supported)
 		 */
@@ -881,3 +1202,77 @@ err:
 	return err;
 }
 
+int sdio_reset_comm(struct mmc_card *card)
+{
+	struct mmc_host *host = card->host;
+	u32 ocr;
+	int err;
+
+	printk("%s():\n", __func__);
+	mmc_claim_host(host);
+
+	mmc_go_idle(host);
+
+	mmc_set_clock(host, host->f_min);
+
+	err = mmc_send_io_op_cond(host, 0, &ocr);
+	if (err)
+		goto err;
+
+	host->ocr = mmc_select_voltage(host, ocr);
+	if (!host->ocr) {
+		err = -EINVAL;
+		goto err;
+	}
+
+	err = mmc_send_io_op_cond(host, host->ocr, &ocr);
+	if (err)
+		goto err;
+
+	if (mmc_host_is_spi(host)) {
+		err = mmc_spi_set_crc(host, use_spi_crc);
+		if (err)
+			goto err;
+	}
+
+	if (!mmc_host_is_spi(host)) {
+		err = mmc_send_relative_addr(host, &card->rca);
+		if (err)
+			goto err;
+		mmc_set_bus_mode(host, MMC_BUSMODE_PUSHPULL);
+	}
+	if (!mmc_host_is_spi(host)) {
+		err = mmc_select_card(card);
+		if (err)
+			goto err;
+	}
+
+	/*
+	 * Switch to high-speed (if supported).
+	 */
+	err = sdio_enable_hs(card);
+	if (err > 0)
+		mmc_sd_go_highspeed(card);
+	else if (err)
+		goto err;
+
+	/*
+	 * Change to the card's maximum speed.
+	 */
+	mmc_set_clock(host, mmc_sdio_get_max_clock(card));
+
+	err = sdio_enable_4bit_bus(card);
+	if (err > 0)
+		mmc_set_bus_width(host, MMC_BUS_WIDTH_4);
+	else if (err)
+		goto err;
+
+	mmc_release_host(host);
+	return 0;
+err:
+	printk("%s: Error resetting SDIO communications (%d)\n",
+	       mmc_hostname(host), err);
+	mmc_release_host(host);
+	return err;
+}
+EXPORT_SYMBOL(sdio_reset_comm);
diff --git a/drivers/mmc/core/sdio_bus.c b/drivers/mmc/core/sdio_bus.c
index e4e6822d09e3..ca58c307a129 100644
--- a/drivers/mmc/core/sdio_bus.c
+++ b/drivers/mmc/core/sdio_bus.c
@@ -23,6 +23,10 @@
 #include "sdio_cis.h"
 #include "sdio_bus.h"
 
+#ifdef CONFIG_MMC_EMBEDDED_SDIO
+#include <linux/mmc/host.h>
+#endif
+
 /* show configuration fields */
 #define sdio_config_attr(field, format_string)				\
 static ssize_t								\
@@ -256,7 +260,14 @@ static void sdio_release_func(struct device *dev)
 {
 	struct sdio_func *func = dev_to_sdio_func(dev);
 
-	sdio_free_func_cis(func);
+#ifdef CONFIG_MMC_EMBEDDED_SDIO
+	/*
+	 * If this device is embedded then we never allocated
+	 * cis tables for this func
+	 */
+	if (!func->card->host->embedded_sdio_data.funcs)
+#endif
+		sdio_free_func_cis(func);
 
 	if (func->info)
 		kfree(func->info);
diff --git a/drivers/mmc/core/sdio_io.c b/drivers/mmc/core/sdio_io.c
index 0f687cdeb064..549a34144646 100644
--- a/drivers/mmc/core/sdio_io.c
+++ b/drivers/mmc/core/sdio_io.c
@@ -383,6 +383,39 @@ u8 sdio_readb(struct sdio_func *func, unsigned int addr, int *err_ret)
 EXPORT_SYMBOL_GPL(sdio_readb);
 
 /**
+ *	sdio_readb_ext - read a single byte from a SDIO function
+ *	@func: SDIO function to access
+ *	@addr: address to read
+ *	@err_ret: optional status value from transfer
+ *	@in: value to add to argument
+ *
+ *	Reads a single byte from the address space of a given SDIO
+ *	function. If there is a problem reading the address, 0xff
+ *	is returned and @err_ret will contain the error code.
+ */
+unsigned char sdio_readb_ext(struct sdio_func *func, unsigned int addr,
+	int *err_ret, unsigned in)
+{
+	int ret;
+	unsigned char val;
+
+	BUG_ON(!func);
+
+	if (err_ret)
+		*err_ret = 0;
+
+	ret = mmc_io_rw_direct(func->card, 0, func->num, addr, (u8)in, &val);
+	if (ret) {
+		if (err_ret)
+			*err_ret = ret;
+		return 0xFF;
+	}
+
+	return val;
+}
+EXPORT_SYMBOL_GPL(sdio_readb_ext);
+
+/**
  *	sdio_writeb - write a single byte to a SDIO function
  *	@func: SDIO function to access
  *	@b: byte to write
diff --git a/drivers/mmc/debug_mmc.h b/drivers/mmc/debug_mmc.h
new file mode 100644
index 000000000000..592c919aa371
--- /dev/null
+++ b/drivers/mmc/debug_mmc.h
@@ -0,0 +1,17 @@
+#define MMC_DEBUG 1
+#if MMC_DEBUG
+#define MMC_DBG(fmt,args...) \
+	do { printk(KERN_DEBUG "[mmc_debug]:%s:%d "fmt"\n", __func__, __LINE__, ##args); } \
+	while (0)
+#else
+#define MMC_DBG(x...) do {} while (0)
+#endif
+
+
+#if 1
+#define MMC_printk(fmt,args...) \
+	do { printk(KERN_INFO "[mmc]:%s:%d "fmt"\n", __func__, __LINE__, ##args); } \
+	while (0)
+#else
+#define MMC_prink(x...) do {} while (0)
+#endif
diff --git a/drivers/mmc/host/Makefile b/drivers/mmc/host/Makefile
index b4b83f302e32..2f9d6f4f43e1 100644
--- a/drivers/mmc/host/Makefile
+++ b/drivers/mmc/host/Makefile
@@ -1,6 +1,7 @@
 #
 # Makefile for MMC/SD host controller drivers
 #
+GCOV_PROFILE_sdhci-tegra.o := y
 
 obj-$(CONFIG_MMC_ARMMMCI)	+= mmci.o
 obj-$(CONFIG_MMC_PXA)		+= pxamci.o
@@ -47,6 +48,7 @@ obj-$(CONFIG_MMC_SDHCI_PLTFM)		+= sdhci-pltfm.o
 obj-$(CONFIG_MMC_SDHCI_CNS3XXX)		+= sdhci-cns3xxx.o
 obj-$(CONFIG_MMC_SDHCI_ESDHC_IMX)	+= sdhci-esdhc-imx.o
 obj-$(CONFIG_MMC_SDHCI_DOVE)		+= sdhci-dove.o
+CFLAGS_sdhci-tegra.o			 = -Werror
 obj-$(CONFIG_MMC_SDHCI_TEGRA)		+= sdhci-tegra.o
 obj-$(CONFIG_MMC_SDHCI_OF_ESDHC)	+= sdhci-of-esdhc.o
 obj-$(CONFIG_MMC_SDHCI_OF_HLWD)		+= sdhci-of-hlwd.o
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index fa8cae1d7005..bcc5728ee3f7 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -689,8 +689,9 @@ atmci_prepare_data_dma(struct atmel_mci *host, struct mmc_data *data)
 	sglen = dma_map_sg(chan->device->dev, data->sg,
 			   data->sg_len, direction);
 
-	desc = chan->device->device_prep_slave_sg(chan,
-			data->sg, sglen, direction,
+	dmaengine_slave_config(chan, &host->dma_conf);
+	desc = dmaengine_prep_slave_sg(chan,
+			data->sg, sglen, slave_dirn,
 			DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	if (!desc)
 		goto unmap_exit;
diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index 56e9a4168264..b66b466ef3bd 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -406,7 +406,7 @@ static int mmci_dma_prep_data(struct mmci_host *host, struct mmc_data *data,
 		return -EINVAL;
 
 	dmaengine_slave_config(chan, &conf);
-	desc = device->device_prep_slave_sg(chan, data->sg, nr_sg,
+	desc = dmaengine_prep_slave_sg(chan, data->sg, nr_sg,
 					    conf.direction, DMA_CTRL_ACK);
 	if (!desc)
 		goto unmap_exit;
@@ -673,7 +673,8 @@ mmci_data_irq(struct mmci_host *host, struct mmc_data *data,
 	      unsigned int status)
 {
 	/* First check for errors */
-	if (status & (MCI_DATACRCFAIL|MCI_DATATIMEOUT|MCI_TXUNDERRUN|MCI_RXOVERRUN)) {
+	if (status & (MCI_DATACRCFAIL|MCI_DATATIMEOUT|MCI_STARTBITERR|
+		      MCI_TXUNDERRUN|MCI_RXOVERRUN)) {
 		u32 remain, success;
 
 		/* Terminate the DMA transfer */
@@ -752,8 +753,12 @@ mmci_cmd_irq(struct mmci_host *host, struct mmc_command *cmd,
 	}
 
 	if (!cmd->data || cmd->error) {
-		if (host->data)
+		if (host->data) {
+			/* Terminate the DMA transfer */
+			if (dma_inprogress(host))
+				mmci_dma_data_error(host);
 			mmci_stop_data(host);
+		}
 		mmci_request_end(host, cmd->mrq);
 	} else if (!(cmd->data->flags & MMC_DATA_READ)) {
 		mmci_start_data(host, cmd->data);
@@ -953,8 +958,9 @@ static irqreturn_t mmci_irq(int irq, void *dev_id)
 		dev_dbg(mmc_dev(host->mmc), "irq0 (data+cmd) %08x\n", status);
 
 		data = host->data;
-		if (status & (MCI_DATACRCFAIL|MCI_DATATIMEOUT|MCI_TXUNDERRUN|
-			      MCI_RXOVERRUN|MCI_DATAEND|MCI_DATABLOCKEND) && data)
+		if (status & (MCI_DATACRCFAIL|MCI_DATATIMEOUT|MCI_STARTBITERR|
+			      MCI_TXUNDERRUN|MCI_RXOVERRUN|MCI_DATAEND|
+			      MCI_DATABLOCKEND) && data)
 			mmci_data_irq(host, data, status);
 
 		cmd = host->cmd;
diff --git a/drivers/mmc/host/mxcmmc.c b/drivers/mmc/host/mxcmmc.c
index 14aa213b00da..1365e7a20906 100644
--- a/drivers/mmc/host/mxcmmc.c
+++ b/drivers/mmc/host/mxcmmc.c
@@ -249,8 +249,8 @@ static int mxcmci_setup_data(struct mxcmci_host *host, struct mmc_data *data)
 	if (nents != data->sg_len)
 		return -EINVAL;
 
-	host->desc = host->dma->device->device_prep_slave_sg(host->dma,
-		data->sg, data->sg_len, host->dma_dir,
+	host->desc = dmaengine_prep_slave_sg(host->dma,
+		data->sg, data->sg_len, slave_dirn,
 		DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 
 	if (!host->desc) {
@@ -731,6 +731,7 @@ static void mxcmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 				"failed to config DMA channel. Falling back to PIO\n");
 			dma_release_channel(host->dma);
 			host->do_dma = 0;
+			host->dma = NULL;
 		}
 	}
 
diff --git a/drivers/mmc/host/mxs-mmc.c b/drivers/mmc/host/mxs-mmc.c
index d513d47364d0..a31b33881697 100644
--- a/drivers/mmc/host/mxs-mmc.c
+++ b/drivers/mmc/host/mxs-mmc.c
@@ -322,8 +322,8 @@ static struct dma_async_tx_descriptor *mxs_mmc_prep_dma(
 		sg_len = SSP_PIO_NUM;
 	}
 
-	desc = host->dmach->device->device_prep_slave_sg(host->dmach,
-				sgl, sg_len, host->dma_dir, append);
+	desc = dmaengine_prep_slave_sg(host->dmach,
+				sgl, sg_len, host->slave_dirn, append);
 	if (desc) {
 		desc->callback = mxs_mmc_dma_irq_callback;
 		desc->callback_param = host;
diff --git a/drivers/mmc/host/sdhci-pltfm.c b/drivers/mmc/host/sdhci-pltfm.c
index 6414efeddca0..6c62a733bf15 100644
--- a/drivers/mmc/host/sdhci-pltfm.c
+++ b/drivers/mmc/host/sdhci-pltfm.c
@@ -35,6 +35,8 @@
 #endif
 #include "sdhci-pltfm.h"
 
+#include "../debug_mmc.h"
+
 static struct sdhci_ops sdhci_pltfm_ops = {
 };
 
@@ -196,16 +198,50 @@ EXPORT_SYMBOL_GPL(sdhci_pltfm_unregister);
 int sdhci_pltfm_suspend(struct platform_device *dev, pm_message_t state)
 {
 	struct sdhci_host *host = platform_get_drvdata(dev);
+	int ret;
+
+	MMC_printk("%s: ++", mmc_hostname(host->mmc));
+
+	ret = sdhci_suspend_host(host, state);
+	if (ret) {
+		dev_err(&dev->dev, "suspend failed, error = %d\n", ret);
+		return ret;
+	}
 
-	return sdhci_suspend_host(host, state);
+	if (host->ops && host->ops->suspend)
+		ret = host->ops->suspend(host, state);
+	if (ret) {
+		dev_err(&dev->dev, "suspend hook failed, error = %d\n", ret);
+		sdhci_resume_host(host);
+	}
+
+	MMC_printk("%s: --", mmc_hostname(host->mmc));
+
+	return ret;
 }
 EXPORT_SYMBOL_GPL(sdhci_pltfm_suspend);
 
 int sdhci_pltfm_resume(struct platform_device *dev)
 {
 	struct sdhci_host *host = platform_get_drvdata(dev);
+	int ret = 0;
+
+	MMC_printk("%s: ++", mmc_hostname(host->mmc));
+
+	if (host->ops && host->ops->resume)
+		ret = host->ops->resume(host);
+	if (ret) {
+		dev_err(&dev->dev, "resume hook failed, error = %d\n", ret);
+		return ret;
+	}
 
-	return sdhci_resume_host(host);
+	ret = sdhci_resume_host(host);
+	if (ret)
+		dev_err(&dev->dev, "resume failed, error = %d\n", ret);
+
+	MMC_printk("%s: --", mmc_hostname(host->mmc));
+
+	return ret;
 }
 EXPORT_SYMBOL_GPL(sdhci_pltfm_resume);
 #endif	/* CONFIG_PM */
diff --git a/drivers/mmc/host/sdhci-pltfm.h b/drivers/mmc/host/sdhci-pltfm.h
index 3a9fc3f40840..b92c7f29a4e7 100644
--- a/drivers/mmc/host/sdhci-pltfm.h
+++ b/drivers/mmc/host/sdhci-pltfm.h
@@ -17,7 +17,7 @@
 
 struct sdhci_pltfm_data {
 	struct sdhci_ops *ops;
-	unsigned int quirks;
+	u64 quirks;
 };
 
 struct sdhci_pltfm_host {
diff --git a/drivers/mmc/host/sdhci-tegra.c b/drivers/mmc/host/sdhci-tegra.c
index 18b0bd31de78..ae4f1c1ac501 100644
--- a/drivers/mmc/host/sdhci-tegra.c
+++ b/drivers/mmc/host/sdhci-tegra.c
@@ -1,6 +1,8 @@
 /*
  * Copyright (C) 2010 Google, Inc.
  *
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ *
  * This software is licensed under the terms of the GNU General Public
  * License version 2, as published by the Free Software Foundation, and
  * may be copied, distributed, and modified under those terms.
@@ -18,13 +20,110 @@
 #include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/gpio.h>
+#include <linux/slab.h>
 #include <linux/mmc/card.h>
 #include <linux/mmc/host.h>
+#include <linux/mmc/sd.h>
+#include <linux/regulator/consumer.h>
+#include <linux/delay.h>
 
 #include <mach/gpio.h>
 #include <mach/sdhci.h>
+#include <mach/io_dpd.h>
 
 #include "sdhci-pltfm.h"
+#include <../gpio-names.h>
+#include "../debug_mmc.h"
+
+#define SDHCI_VENDOR_CLOCK_CNTRL	0x100
+#define SDHCI_VENDOR_CLOCK_CNTRL_SDMMC_CLK	0x1
+#define SDHCI_VENDOR_CLOCK_CNTRL_PADPIPE_CLKEN_OVERRIDE	0x8
+#define SDHCI_VENDOR_CLOCK_CNTRL_SPI_MODE_CLKEN_OVERRIDE	0x4
+#define SDHCI_VENDOR_CLOCK_CNTRL_BASE_CLK_FREQ_SHIFT	8
+#define SDHCI_VENDOR_CLOCK_CNTRL_TAP_VALUE_SHIFT	16
+#define SDHCI_VENDOR_CLOCK_CNTRL_SDR50_TUNING		0x20
+
+#define SDHCI_VENDOR_MISC_CNTRL		0x120
+#define SDHCI_VENDOR_MISC_CNTRL_ENABLE_SDR104_SUPPORT	0x8
+#define SDHCI_VENDOR_MISC_CNTRL_ENABLE_SDR50_SUPPORT	0x10
+#define SDHCI_VENDOR_MISC_CNTRL_ENABLE_SD_3_0	0x20
+
+#define SDMMC_SDMEMCOMPPADCTRL	0x1E0
+#define SDMMC_SDMEMCOMPPADCTRL_VREF_SEL_MASK	0xF
+
+#define SDMMC_AUTO_CAL_CONFIG	0x1E4
+#define SDMMC_AUTO_CAL_CONFIG_AUTO_CAL_ENABLE	0x20000000
+#define SDMMC_AUTO_CAL_CONFIG_AUTO_CAL_PD_OFFSET_SHIFT	0x8
+#define SDMMC_AUTO_CAL_CONFIG_AUTO_CAL_PD_OFFSET	0x70
+#define SDMMC_AUTO_CAL_CONFIG_AUTO_CAL_PU_OFFSET	0x62
+
+#define SDHOST_1V8_OCR_MASK	0x8
+#define SDHOST_HIGH_VOLT_MIN	2700000
+#define SDHOST_HIGH_VOLT_MAX	3600000
+#define SDHOST_LOW_VOLT_MIN	1800000
+#define SDHOST_LOW_VOLT_MAX	1800000
+
+#define TEGRA_SDHOST_MIN_FREQ	50000000
+#define TEGRA2_SDHOST_STD_FREQ	50000000
+#define TEGRA3_SDHOST_STD_FREQ	104000000
+
+#define SD_SEND_TUNING_PATTERN	19
+#define MAX_TAP_VALUES	256
+
+static unsigned int tegra_sdhost_min_freq;
+static unsigned int tegra_sdhost_std_freq;
+
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+static void tegra_3x_sdhci_set_card_clock(struct sdhci_host *sdhci, unsigned int clock);
+static void tegra3_sdhci_post_reset_init(struct sdhci_host *sdhci);
+#endif
+
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+static unsigned int tegra3_sdhost_max_clk[4] = {
+	208000000,	104000000,	208000000,	104000000 };
+#endif
+
+struct tegra_sdhci_hw_ops{
+	/* Set the internal clk and card clk.*/
+	void	(*set_card_clock)(struct sdhci_host *sdhci, unsigned int clock);
+	/* Post reset vendor registers configuration */
+	void	(*sdhost_init)(struct sdhci_host *sdhci);
+};
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+static struct tegra_sdhci_hw_ops tegra_2x_sdhci_ops = {
+};
+#endif
+
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+static struct tegra_sdhci_hw_ops tegra_3x_sdhci_ops = {
+	.set_card_clock = tegra_3x_sdhci_set_card_clock,
+	.sdhost_init = tegra3_sdhci_post_reset_init,
+};
+#endif
+
+struct tegra_sdhci_host {
+	bool	clk_enabled;
+	struct regulator *vdd_io_reg;
+	struct regulator *vdd_slot_reg;
+	/* Pointer to the chip specific HW ops */
+	struct tegra_sdhci_hw_ops *hw_ops;
+	/* Host controller instance */
+	unsigned int instance;
+	/* vddio_min */
+	unsigned int vddio_min_uv;
+	/* vddio_max */
+	unsigned int vddio_max_uv;
+	/* max clk supported by the platform */
+	unsigned int max_clk_limit;
+	/* max ddr clk supported by the platform */
+	unsigned int ddr_clk_limit;
+	struct tegra_io_dpd *dpd;
+	bool card_present;
+	bool is_rail_enabled;
+	struct clk *emc_clk;
+	unsigned int emc_max_clk;
+};
 
 static u32 tegra_sdhci_readl(struct sdhci_host *host, int reg)
 {
@@ -41,11 +140,12 @@ static u32 tegra_sdhci_readl(struct sdhci_host *host, int reg)
 
 static u16 tegra_sdhci_readw(struct sdhci_host *host, int reg)
 {
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	if (unlikely(reg == SDHCI_HOST_VERSION)) {
 		/* Erratum: Version register is invalid in HW. */
 		return SDHCI_SPEC_200;
 	}
-
+#endif
 	return readw(host->ioaddr + reg);
 }
 
@@ -60,6 +160,7 @@ static void tegra_sdhci_writel(struct sdhci_host *host, u32 val, int reg)
 
 	writel(val, host->ioaddr + reg);
 
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
 	if (unlikely(reg == SDHCI_INT_ENABLE)) {
 		/* Erratum: Must enable block gap interrupt detection */
 		u8 gap_ctrl = readb(host->ioaddr + SDHCI_BLOCK_GAP_CONTROL);
@@ -69,6 +170,15 @@ static void tegra_sdhci_writel(struct sdhci_host *host, u32 val, int reg)
 			gap_ctrl &= ~0x8;
 		writeb(gap_ctrl, host->ioaddr + SDHCI_BLOCK_GAP_CONTROL);
 	}
+#endif
+}
+
+static unsigned int tegra_sdhci_get_cd(struct sdhci_host *sdhci)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(sdhci);
+	struct tegra_sdhci_host *tegra_host = pltfm_host->priv;
+
+	return tegra_host->card_present;
 }
 
 static unsigned int tegra_sdhci_get_ro(struct sdhci_host *sdhci)
@@ -84,9 +194,152 @@ static unsigned int tegra_sdhci_get_ro(struct sdhci_host *sdhci)
 	return gpio_get_value(plat->wp_gpio);
 }
 
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+static void tegra3_sdhci_post_reset_init(struct sdhci_host *sdhci)
+{
+	u16 misc_ctrl;
+	u32 vendor_ctrl;
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(sdhci);
+	struct tegra_sdhci_host *tegra_host = pltfm_host->priv;
+	struct platform_device *pdev = to_platform_device(mmc_dev(sdhci->mmc));
+	struct tegra_sdhci_platform_data *plat;
+
+	plat = pdev->dev.platform_data;
+	/* Set the base clock frequency */
+	vendor_ctrl = sdhci_readl(sdhci, SDHCI_VENDOR_CLOCK_CNTRL);
+	vendor_ctrl &= ~(0xFF << SDHCI_VENDOR_CLOCK_CNTRL_BASE_CLK_FREQ_SHIFT);
+	vendor_ctrl |= (tegra3_sdhost_max_clk[tegra_host->instance] / 1000000) <<
+		SDHCI_VENDOR_CLOCK_CNTRL_BASE_CLK_FREQ_SHIFT;
+	vendor_ctrl |= SDHCI_VENDOR_CLOCK_CNTRL_PADPIPE_CLKEN_OVERRIDE;
+	vendor_ctrl &= ~SDHCI_VENDOR_CLOCK_CNTRL_SPI_MODE_CLKEN_OVERRIDE;
+
+	/* Set tap delay */
+	if (plat->tap_delay) {
+		vendor_ctrl &= ~(0xFF <<
+			SDHCI_VENDOR_CLOCK_CNTRL_TAP_VALUE_SHIFT);
+		vendor_ctrl |= (plat->tap_delay <<
+			SDHCI_VENDOR_CLOCK_CNTRL_TAP_VALUE_SHIFT);
+	}
+	/* Enable frequency tuning for SDR50 mode */
+	vendor_ctrl |= SDHCI_VENDOR_CLOCK_CNTRL_SDR50_TUNING;
+	sdhci_writel(sdhci, vendor_ctrl, SDHCI_VENDOR_CLOCK_CNTRL);
+
+	/* Enable SDHOST v3.0 support */
+	misc_ctrl = sdhci_readw(sdhci, SDHCI_VENDOR_MISC_CNTRL);
+	misc_ctrl |= SDHCI_VENDOR_MISC_CNTRL_ENABLE_SD_3_0 |
+		SDHCI_VENDOR_MISC_CNTRL_ENABLE_SDR104_SUPPORT |
+		SDHCI_VENDOR_MISC_CNTRL_ENABLE_SDR50_SUPPORT;
+	sdhci_writew(sdhci, misc_ctrl, SDHCI_VENDOR_MISC_CNTRL);
+}
+#endif /*	#ifdef CONFIG_ARCH_TEGRA_3x_SOC */
+
+static int tegra_sdhci_set_uhs_signaling(struct sdhci_host *host,
+		unsigned int uhs)
+{
+	u16 clk, ctrl_2;
+	ctrl_2 = sdhci_readw(host, SDHCI_HOST_CONTROL2);
+
+	/* Select Bus Speed Mode for host */
+	ctrl_2 &= ~SDHCI_CTRL_UHS_MASK;
+	switch (uhs) {
+	case MMC_TIMING_UHS_SDR12:
+		ctrl_2 |= SDHCI_CTRL_UHS_SDR12;
+		break;
+	case MMC_TIMING_UHS_SDR25:
+		ctrl_2 |= SDHCI_CTRL_UHS_SDR25;
+		break;
+	case MMC_TIMING_UHS_SDR50:
+		ctrl_2 |= SDHCI_CTRL_UHS_SDR50;
+		break;
+	case MMC_TIMING_UHS_SDR104:
+		ctrl_2 |= SDHCI_CTRL_UHS_SDR104;
+		break;
+	case MMC_TIMING_UHS_DDR50:
+		ctrl_2 |= SDHCI_CTRL_UHS_DDR50;
+		break;
+	}
+
+	sdhci_writew(host, ctrl_2, SDHCI_HOST_CONTROL2);
+
+	if (uhs == MMC_TIMING_UHS_DDR50) {
+		clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
+		clk &= ~(0xFF << SDHCI_DIVIDER_SHIFT);
+		clk |= 1 << SDHCI_DIVIDER_SHIFT;
+		sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
+	}
+	return 0;
+}
+
+static void tegra_sdhci_reset_exit(struct sdhci_host *sdhci, u8 mask)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(sdhci);
+	struct tegra_sdhci_host *tegra_host = pltfm_host->priv;
+
+	if (mask & SDHCI_RESET_ALL) {
+		if (tegra_host->hw_ops->sdhost_init)
+			tegra_host->hw_ops->sdhost_init(sdhci);
+	}
+}
+
+static void sdhci_status_notify_cb(int card_present, void *dev_id)
+{
+	struct sdhci_host *sdhci = (struct sdhci_host *)dev_id;
+	struct platform_device *pdev = to_platform_device(mmc_dev(sdhci->mmc));
+	struct tegra_sdhci_platform_data *plat;
+	unsigned int status, oldstat;
+
+	pr_debug("%s: card_present %d\n", mmc_hostname(sdhci->mmc),
+		card_present);
+
+	plat = pdev->dev.platform_data;
+	if (!plat->mmc_data.status) {
+		mmc_detect_change(sdhci->mmc, 0);
+		return;
+	}
+
+	status = plat->mmc_data.status(mmc_dev(sdhci->mmc));
+
+	oldstat = plat->mmc_data.card_present;
+	plat->mmc_data.card_present = status;
+	if (status ^ oldstat) {
+		pr_debug("%s: Slot status change detected (%d -> %d)\n",
+			mmc_hostname(sdhci->mmc), oldstat, status);
+		if (status && !plat->mmc_data.built_in)
+			mmc_detect_change(sdhci->mmc, (5 * HZ) / 2);
+		else
+			mmc_detect_change(sdhci->mmc, 0);
+	}
+}
+
 static irqreturn_t carddetect_irq(int irq, void *data)
 {
 	struct sdhci_host *sdhost = (struct sdhci_host *)data;
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(sdhost);
+	struct tegra_sdhci_host *tegra_host = pltfm_host->priv;
+	struct platform_device *pdev = to_platform_device(mmc_dev(sdhost->mmc));
+	struct tegra_sdhci_platform_data *plat;
+
+	plat = pdev->dev.platform_data;
+
+	tegra_host->card_present = (gpio_get_value(plat->cd_gpio) == 0);
+
+	if (tegra_host->card_present) {
+		if (!tegra_host->is_rail_enabled) {
+			if (tegra_host->vdd_slot_reg)
+				regulator_enable(tegra_host->vdd_slot_reg);
+			if (tegra_host->vdd_io_reg)
+				regulator_enable(tegra_host->vdd_io_reg);
+			tegra_host->is_rail_enabled = 1;
+		}
+	} else {
+		if (tegra_host->is_rail_enabled) {
+			if (tegra_host->vdd_io_reg)
+				regulator_disable(tegra_host->vdd_io_reg);
+			if (tegra_host->vdd_slot_reg)
+				regulator_disable(tegra_host->vdd_slot_reg);
+			tegra_host->is_rail_enabled = 0;
+                }
+	}
 
 	tasklet_schedule(&sdhost->card_tasklet);
 	return IRQ_HANDLED;
@@ -115,19 +368,628 @@ static int tegra_sdhci_8bit(struct sdhci_host *host, int bus_width)
 	return 0;
 }
 
+static void tegra_sdhci_set_clk_rate(struct sdhci_host *sdhci,
+	unsigned int clock)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(sdhci);
+	struct tegra_sdhci_host *tegra_host = pltfm_host->priv;
+	unsigned int clk_rate;
+	unsigned int emc_clk;
+
+	if (sdhci->mmc->ios.timing == MMC_TIMING_UHS_DDR50) {
+		/*
+		 * In ddr mode, tegra sdmmc controller clock frequency
+		 * should be double the card clock frequency.
+		 */
+		if (tegra_host->ddr_clk_limit) {
+			clk_rate = tegra_host->ddr_clk_limit * 2;
+			if (tegra_host->emc_clk) {
+				emc_clk = clk_get_rate(tegra_host->emc_clk);
+				if (emc_clk == tegra_host->emc_max_clk)
+					clk_rate = clock * 2;
+			}
+		} else {
+			clk_rate = clock * 2;
+		}
+	} else 	if (sdhci->mmc->ios.timing == MMC_TIMING_UHS_SDR50) {
+		/*
+		 * In SDR50 mode, run the sdmmc controller at freq greater than
+		 * 104MHz to ensure the core voltage is at 1.2V. If the core voltage
+		 * is below 1.2V, CRC errors would occur during data transfers.
+		 */
+		clk_rate = clock * 2;
+	} else {
+		if (clock <= tegra_sdhost_min_freq)
+			clk_rate = tegra_sdhost_min_freq;
+		else if (clock <= tegra_sdhost_std_freq)
+			clk_rate = tegra_sdhost_std_freq;
+		else
+			clk_rate = clock;
+	}
+
+	if (tegra_host->max_clk_limit &&
+		(clk_rate > tegra_host->max_clk_limit))
+		clk_rate = tegra_host->max_clk_limit;
+
+	clk_set_rate(pltfm_host->clk, clk_rate);
+	sdhci->max_clk = clk_get_rate(pltfm_host->clk);
+}
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+static void tegra_3x_sdhci_set_card_clock(struct sdhci_host *sdhci, unsigned int clock)
+{
+	int div;
+	u16 clk;
+	unsigned long timeout;
+	u8 ctrl;
+
+	if (clock && clock == sdhci->clock)
+		return;
+
+	/*
+	 * Disable the card clock before disabling the internal
+	 * clock to avoid abnormal clock waveforms.
+	 */
+	clk = sdhci_readw(sdhci, SDHCI_CLOCK_CONTROL);
+	clk &= ~SDHCI_CLOCK_CARD_EN;
+	sdhci_writew(sdhci, clk, SDHCI_CLOCK_CONTROL);
+	sdhci_writew(sdhci, 0, SDHCI_CLOCK_CONTROL);
+
+	if (clock == 0)
+		goto out;
+	if (sdhci->mmc->ios.timing == MMC_TIMING_UHS_DDR50) {
+		div = 1;
+		goto set_clk;
+	}
+
+	if (sdhci->version >= SDHCI_SPEC_300) {
+		/* Version 3.00 divisors must be a multiple of 2. */
+		if (sdhci->max_clk <= clock) {
+			div = 1;
+		} else {
+			for (div = 2; div < SDHCI_MAX_DIV_SPEC_300; div += 2) {
+				if ((sdhci->max_clk / div) <= clock)
+					break;
+			}
+		}
+	} else {
+		/* Version 2.00 divisors must be a power of 2. */
+		for (div = 1; div < SDHCI_MAX_DIV_SPEC_200; div *= 2) {
+			if ((sdhci->max_clk / div) <= clock)
+				break;
+		}
+	}
+	div >>= 1;
+
+	/*
+	 * Tegra3 sdmmc controller internal clock will not be stabilized when
+	 * we use a clock divider value greater than 4. The WAR is as follows.
+	 * - Enable internal clock.
+	 * - Wait for 5 usec and do a dummy write.
+	 * - Poll for clk stable.
+	 */
+set_clk:
+	clk = (div & SDHCI_DIV_MASK) << SDHCI_DIVIDER_SHIFT;
+	clk |= ((div & SDHCI_DIV_HI_MASK) >> SDHCI_DIV_MASK_LEN)
+		<< SDHCI_DIVIDER_HI_SHIFT;
+	clk |= SDHCI_CLOCK_INT_EN;
+	sdhci_writew(sdhci, clk, SDHCI_CLOCK_CONTROL);
+
+	/* Wait for 5 usec */
+	udelay(5);
+
+	/* Do a dummy write */
+	ctrl = sdhci_readb(sdhci, SDHCI_CAPABILITIES);
+	ctrl |= 1;
+	sdhci_writeb(sdhci, ctrl, SDHCI_CAPABILITIES);
+
+	/* Wait max 20 ms */
+	timeout = 20;
+	while (!((clk = sdhci_readw(sdhci, SDHCI_CLOCK_CONTROL))
+		& SDHCI_CLOCK_INT_STABLE)) {
+		if (timeout == 0) {
+			dev_err(mmc_dev(sdhci->mmc), "Internal clock never stabilised\n");
+			return;
+		}
+		timeout--;
+		mdelay(1);
+	}
+
+	clk |= SDHCI_CLOCK_CARD_EN;
+	sdhci_writew(sdhci, clk, SDHCI_CLOCK_CONTROL);
+out:
+	sdhci->clock = clock;
+}
+#endif /*	#ifdef CONFIG_ARCH_TEGRA_3x_SOC */
+
+static void tegra_sdhci_set_clock(struct sdhci_host *sdhci, unsigned int clock)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(sdhci);
+	struct tegra_sdhci_host *tegra_host = pltfm_host->priv;
+	u8 ctrl;
+
+	pr_debug("%s %s %u enabled=%u\n", __func__,
+		mmc_hostname(sdhci->mmc), clock, tegra_host->clk_enabled);
+
+	if (clock) {
+		/* bring out sd instance from io dpd mode */
+		if (tegra_host->dpd) {
+			mutex_lock(&tegra_host->dpd->delay_lock);
+			cancel_delayed_work_sync(&tegra_host->dpd->delay_dpd);
+			tegra_io_dpd_disable(tegra_host->dpd);
+			mutex_unlock(&tegra_host->dpd->delay_lock);
+		}
+
+		if (!tegra_host->clk_enabled) {
+			clk_enable(pltfm_host->clk);
+			ctrl = sdhci_readb(sdhci, SDHCI_VENDOR_CLOCK_CNTRL);
+			ctrl |= SDHCI_VENDOR_CLOCK_CNTRL_SDMMC_CLK;
+			sdhci_writeb(sdhci, ctrl, SDHCI_VENDOR_CLOCK_CNTRL);
+			tegra_host->clk_enabled = true;
+		}
+		tegra_sdhci_set_clk_rate(sdhci, clock);
+		if (tegra_host->hw_ops->set_card_clock)
+			tegra_host->hw_ops->set_card_clock(sdhci, clock);
+	} else if (!clock && tegra_host->clk_enabled) {
+		if (tegra_host->hw_ops->set_card_clock)
+			tegra_host->hw_ops->set_card_clock(sdhci, clock);
+		ctrl = sdhci_readb(sdhci, SDHCI_VENDOR_CLOCK_CNTRL);
+		ctrl &= ~SDHCI_VENDOR_CLOCK_CNTRL_SDMMC_CLK;
+		sdhci_writeb(sdhci, ctrl, SDHCI_VENDOR_CLOCK_CNTRL);
+		clk_disable(pltfm_host->clk);
+		tegra_host->clk_enabled = false;
+		/* io dpd enable call for sd instance */
+
+		if (tegra_host->dpd) {
+			mutex_lock(&tegra_host->dpd->delay_lock);
+			if (tegra_host->dpd->need_delay_dpd) {
+				schedule_delayed_work(
+					&tegra_host->dpd->delay_dpd,
+					msecs_to_jiffies(100));
+			} else {
+				tegra_io_dpd_enable(tegra_host->dpd);
+			}
+			mutex_unlock(&tegra_host->dpd->delay_lock);
+		}
+	}
+}
+
+static int tegra_sdhci_signal_voltage_switch(struct sdhci_host *sdhci,
+	unsigned int signal_voltage)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(sdhci);
+	struct tegra_sdhci_host *tegra_host = pltfm_host->priv;
+	unsigned int min_uV = SDHOST_HIGH_VOLT_MIN;
+	unsigned int max_uV = SDHOST_HIGH_VOLT_MAX;
+	unsigned int rc = 0;
+	u16 clk, ctrl;
+	unsigned int val;
+
+	ctrl = sdhci_readw(sdhci, SDHCI_HOST_CONTROL2);
+	if (signal_voltage == MMC_SIGNAL_VOLTAGE_180) {
+		ctrl |= SDHCI_CTRL_VDD_180;
+		min_uV = SDHOST_LOW_VOLT_MIN;
+		max_uV = SDHOST_LOW_VOLT_MAX;
+	} else if (signal_voltage == MMC_SIGNAL_VOLTAGE_330) {
+		if (ctrl & SDHCI_CTRL_VDD_180)
+			ctrl &= ~SDHCI_CTRL_VDD_180;
+	}
+
+	/* Check if the slot can support the required voltage */
+	if (min_uV > tegra_host->vddio_max_uv)
+		return 0;
+
+	/* Switch OFF the card clock to prevent glitches on the clock line */
+	clk = sdhci_readw(sdhci, SDHCI_CLOCK_CONTROL);
+	clk &= ~SDHCI_CLOCK_CARD_EN;
+	sdhci_writew(sdhci, clk, SDHCI_CLOCK_CONTROL);
+
+	/* Set/clear the 1.8V signalling */
+	sdhci_writew(sdhci, ctrl, SDHCI_HOST_CONTROL2);
+
+	/* Switch the I/O rail voltage */
+	if (tegra_host->vdd_io_reg) {
+		rc = regulator_set_voltage(tegra_host->vdd_io_reg,
+			min_uV, max_uV);
+		if (rc) {
+			dev_err(mmc_dev(sdhci->mmc), "switching to 1.8V"
+			"failed . Switching back to 3.3V\n");
+			regulator_set_voltage(tegra_host->vdd_io_reg,
+				SDHOST_HIGH_VOLT_MIN,
+				SDHOST_HIGH_VOLT_MAX);
+			goto out;
+		}
+	}
+
+	/* Wait for 10 msec for the voltage to be switched */
+	mdelay(10);
+
+	/* Enable the card clock */
+	clk |= SDHCI_CLOCK_CARD_EN;
+	sdhci_writew(sdhci, clk, SDHCI_CLOCK_CONTROL);
+
+	/* Wait for 1 msec after enabling clock */
+	mdelay(1);
+
+	if (signal_voltage == MMC_SIGNAL_VOLTAGE_180) {
+		/* Do Auto Calibration for 1.8V signal voltage */
+		val = sdhci_readl(sdhci, SDMMC_AUTO_CAL_CONFIG);
+		val |= SDMMC_AUTO_CAL_CONFIG_AUTO_CAL_ENABLE;
+		/* Program Auto cal PD offset(bits 8:14) */
+		val &= ~(0x7F <<
+			SDMMC_AUTO_CAL_CONFIG_AUTO_CAL_PD_OFFSET_SHIFT);
+		val |= (SDMMC_AUTO_CAL_CONFIG_AUTO_CAL_PD_OFFSET <<
+			SDMMC_AUTO_CAL_CONFIG_AUTO_CAL_PD_OFFSET_SHIFT);
+		/* Program Auto cal PU offset(bits 0:6) */
+		val &= ~0x7F;
+		val |= SDMMC_AUTO_CAL_CONFIG_AUTO_CAL_PU_OFFSET;
+		sdhci_writel(sdhci, val, SDMMC_AUTO_CAL_CONFIG);
+
+		val = sdhci_readl(sdhci, SDMMC_SDMEMCOMPPADCTRL);
+		val &= ~SDMMC_SDMEMCOMPPADCTRL_VREF_SEL_MASK;
+		val |= 0x7;
+		sdhci_writel(sdhci, val, SDMMC_SDMEMCOMPPADCTRL);
+	}
+
+	return rc;
+out:
+	/* Enable the card clock */
+	clk |= SDHCI_CLOCK_CARD_EN;
+	sdhci_writew(sdhci, clk, SDHCI_CLOCK_CONTROL);
+
+	/* Wait for 1 msec for the clock to stabilize */
+	mdelay(1);
+
+	return rc;
+}
+
+static void tegra_sdhci_reset(struct sdhci_host *sdhci, u8 mask)
+{
+	unsigned long timeout;
+
+	sdhci_writeb(sdhci, mask, SDHCI_SOFTWARE_RESET);
+
+	/* Wait max 100 ms */
+	timeout = 100;
+
+	/* hw clears the bit when it's done */
+	while (sdhci_readb(sdhci, SDHCI_SOFTWARE_RESET) & mask) {
+		if (timeout == 0) {
+			dev_err(mmc_dev(sdhci->mmc), "Reset 0x%x never"
+				"completed.\n", (int)mask);
+			return;
+		}
+		timeout--;
+		mdelay(1);
+	}
+}
+
+static void sdhci_tegra_set_tap_delay(struct sdhci_host *sdhci,
+	unsigned int tap_delay)
+{
+	u32 vendor_ctrl;
+
+	/* Max tap delay value is 255 */
+	BUG_ON(tap_delay > MAX_TAP_VALUES);
+
+	vendor_ctrl = sdhci_readl(sdhci, SDHCI_VENDOR_CLOCK_CNTRL);
+	vendor_ctrl &= ~(0xFF << SDHCI_VENDOR_CLOCK_CNTRL_TAP_VALUE_SHIFT);
+	vendor_ctrl |= (tap_delay << SDHCI_VENDOR_CLOCK_CNTRL_TAP_VALUE_SHIFT);
+	sdhci_writel(sdhci, vendor_ctrl, SDHCI_VENDOR_CLOCK_CNTRL);
+}
+
+static int sdhci_tegra_run_frequency_tuning(struct sdhci_host *sdhci)
+{
+	int err = 0;
+	u8 ctrl;
+	u32 mask;
+	unsigned int timeout = 10;
+	int flags;
+	u32 intstatus;
+
+	mask = SDHCI_CMD_INHIBIT | SDHCI_DATA_INHIBIT;
+	while (sdhci_readl(sdhci, SDHCI_PRESENT_STATE) & mask) {
+		if (timeout == 0) {
+			dev_err(mmc_dev(sdhci->mmc), "Controller never"
+				"released inhibit bit(s).\n");
+			err = -ETIMEDOUT;
+			goto out;
+		}
+		timeout--;
+		mdelay(1);
+	}
+
+	ctrl = sdhci_readb(sdhci, SDHCI_HOST_CONTROL2);
+	ctrl &= ~SDHCI_CTRL_TUNED_CLK;
+	sdhci_writeb(sdhci, ctrl, SDHCI_HOST_CONTROL2);
+
+	ctrl = sdhci_readb(sdhci, SDHCI_HOST_CONTROL2);
+	ctrl |= SDHCI_CTRL_EXEC_TUNING;
+	sdhci_writeb(sdhci, ctrl, SDHCI_HOST_CONTROL2);
+
+	/*
+	 * In response to CMD19, the card sends 64 bytes of tuning
+	 * block to the Host Controller. So we set the block size
+	 * to 64 here.
+	 */
+	sdhci_writew(sdhci, SDHCI_MAKE_BLKSZ(7, 64), SDHCI_BLOCK_SIZE);
+
+	sdhci_writeb(sdhci, 0xE, SDHCI_TIMEOUT_CONTROL);
+
+	sdhci_writeb(sdhci, SDHCI_TRNS_READ, SDHCI_TRANSFER_MODE);
+
+	sdhci_writel(sdhci, 0x0, SDHCI_ARGUMENT);
+
+	/* Set the cmd flags */
+	flags = SDHCI_CMD_RESP_SHORT | SDHCI_CMD_CRC | SDHCI_CMD_DATA;
+	/* Issue the command */
+	sdhci_writew(sdhci, SDHCI_MAKE_CMD(
+		SD_SEND_TUNING_PATTERN, flags), SDHCI_COMMAND);
+
+	timeout = 5;
+	do {
+		timeout--;
+		mdelay(1);
+		intstatus = sdhci_readl(sdhci, SDHCI_INT_STATUS);
+		if (intstatus) {
+			sdhci_writel(sdhci, intstatus, SDHCI_INT_STATUS);
+			break;
+		}
+	} while(timeout);
+
+	if ((intstatus & SDHCI_INT_DATA_AVAIL) &&
+		!(intstatus & SDHCI_INT_DATA_CRC)) {
+		err = 0;
+		sdhci->tuning_done = 1;
+	} else {
+		tegra_sdhci_reset(sdhci, SDHCI_RESET_CMD);
+		tegra_sdhci_reset(sdhci, SDHCI_RESET_DATA);
+		err = -EIO;
+	}
+
+	if (sdhci->tuning_done) {
+		sdhci->tuning_done = 0;
+		ctrl = sdhci_readb(sdhci, SDHCI_HOST_CONTROL2);
+		if (!(ctrl & SDHCI_CTRL_EXEC_TUNING) &&
+			(ctrl & SDHCI_CTRL_TUNED_CLK))
+			err = 0;
+		else
+			err = -EIO;
+	}
+	mdelay(1);
+out:
+	return err;
+}
+
+static int sdhci_tegra_execute_tuning(struct sdhci_host *sdhci)
+{
+	int err;
+	u16 ctrl_2;
+	u8 *tap_delay_status;
+	unsigned int i = 0;
+	unsigned int temp_low_pass_tap = 0;
+	unsigned int temp_pass_window = 0;
+	unsigned int best_low_pass_tap = 0;
+	unsigned int best_pass_window = 0;
+	u32 ier;
+
+	/* Tuning is valid only in SDR104 and SDR50 modes */
+	ctrl_2 = sdhci_readw(sdhci, SDHCI_HOST_CONTROL2);
+	if (!(((ctrl_2 & SDHCI_CTRL_UHS_MASK) == SDHCI_CTRL_UHS_SDR104) ||
+		(((ctrl_2 & SDHCI_CTRL_UHS_MASK) == SDHCI_CTRL_UHS_SDR50) &&
+		(sdhci->flags & SDHCI_SDR50_NEEDS_TUNING))))
+			return 0;
+
+	tap_delay_status = kzalloc(MAX_TAP_VALUES, GFP_KERNEL);
+	if (tap_delay_status == NULL) {
+		dev_err(mmc_dev(sdhci->mmc), "failed to allocate memory"
+			"for storing tap_delay_status\n");
+		return -ENOMEM;
+	}
+
+	/*
+	 * Disable all interrupts signalling.Enable interrupt status
+	 * detection for buffer read ready and data crc. We use
+	 * polling for tuning as it involves less overhead.
+	 */
+	ier = sdhci_readl(sdhci, SDHCI_INT_ENABLE);
+	sdhci_writel(sdhci, 0, SDHCI_SIGNAL_ENABLE);
+	sdhci_writel(sdhci, SDHCI_INT_DATA_AVAIL |
+		SDHCI_INT_DATA_CRC, SDHCI_INT_ENABLE);
+
+	/*
+	 * Set each tap delay value and run frequency tuning. After each
+	 * run, update the tap delay status as working or not working.
+	 */
+	do {
+		/* Set the tap delay */
+		sdhci_tegra_set_tap_delay(sdhci, i);
+
+		/* Run frequency tuning */
+		err = sdhci_tegra_run_frequency_tuning(sdhci);
+
+		/* Update whether the tap delay worked or not */
+		tap_delay_status[i] = (err) ? 0: 1;
+		i++;
+	} while (i < 0xFF);
+
+	/* Find the best possible tap range */
+	for (i = 0; i < 0xFF; i++) {
+		temp_pass_window = 0;
+
+		/* Find the first passing tap in the current window */
+		if (tap_delay_status[i]) {
+			temp_low_pass_tap = i;
+
+			/* Find the pass window */
+			do {
+				temp_pass_window++;
+				i++;
+				if (i > 0xFF)
+					break;
+			} while (tap_delay_status[i]);
+
+			if ((temp_pass_window > best_pass_window) && (temp_pass_window > 1)){
+				best_low_pass_tap = temp_low_pass_tap;
+				best_pass_window = temp_pass_window;
+			}
+		}
+	}
+
+
+	pr_debug("%s: best pass tap window: start %d, end %d\n",
+		mmc_hostname(sdhci->mmc), best_low_pass_tap,
+		(best_low_pass_tap + best_pass_window));
+
+	/* Set the best tap */
+	sdhci_tegra_set_tap_delay(sdhci,
+		(best_low_pass_tap + ((best_pass_window * 3) / 4)));
+
+	/* Run frequency tuning */
+	err = sdhci_tegra_run_frequency_tuning(sdhci);
+
+	/* Enable the normal interrupts signalling */
+	sdhci_writel(sdhci, ier, SDHCI_INT_ENABLE);
+	sdhci_writel(sdhci, ier, SDHCI_SIGNAL_ENABLE);
+
+	if (tap_delay_status)
+		kfree(tap_delay_status);
+
+	return err;
+}
+
+static int tegra_sdhci_suspend(struct sdhci_host *sdhci, pm_message_t state)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(sdhci);
+	struct tegra_sdhci_host *tegra_host = pltfm_host->priv;
+
+	tegra_sdhci_set_clock(sdhci, 0);
+
+	/* Disable the power rails if any */
+	if (tegra_host->card_present) {
+		if (tegra_host->is_rail_enabled) {
+			if (tegra_host->vdd_io_reg)
+				regulator_disable(tegra_host->vdd_io_reg);
+			if (tegra_host->vdd_slot_reg)
+				regulator_disable(tegra_host->vdd_slot_reg);
+			tegra_host->is_rail_enabled = 0;
+		}
+	}
+
+	if (tegra_host->dpd) {
+		mutex_lock(&tegra_host->dpd->delay_lock);
+		tegra_host->dpd->need_delay_dpd = 1;
+		mutex_unlock(&tegra_host->dpd->delay_lock);
+	}
+
+	if (!strcmp(mmc_hostname(sdhci->mmc), "mmc0")) {
+                MMC_printk("%s: pull up data pin", mmc_hostname(sdhci->mmc));
+
+                tegra_gpio_enable(TEGRA_GPIO_PAA0);
+                tegra_gpio_enable(TEGRA_GPIO_PAA1);
+                tegra_gpio_enable(TEGRA_GPIO_PAA2);
+                tegra_gpio_enable(TEGRA_GPIO_PAA3);
+                tegra_gpio_enable(TEGRA_GPIO_PAA4);
+                tegra_gpio_enable(TEGRA_GPIO_PAA5);
+                tegra_gpio_enable(TEGRA_GPIO_PAA6);
+                tegra_gpio_enable(TEGRA_GPIO_PAA7);
+
+                gpio_request(TEGRA_GPIO_PAA0, "PAA0");
+                gpio_request(TEGRA_GPIO_PAA1, "PAA1");
+                gpio_request(TEGRA_GPIO_PAA2, "PAA2");
+                gpio_request(TEGRA_GPIO_PAA3, "PAA3");
+                gpio_request(TEGRA_GPIO_PAA4, "PAA4");
+                gpio_request(TEGRA_GPIO_PAA5, "PAA5");
+                gpio_request(TEGRA_GPIO_PAA6, "PAA6");
+                gpio_request(TEGRA_GPIO_PAA7, "PAA7");
+
+                gpio_direction_output(TEGRA_GPIO_PAA0, 1);
+                gpio_direction_output(TEGRA_GPIO_PAA1, 1);
+                gpio_direction_output(TEGRA_GPIO_PAA2, 1);
+                gpio_direction_output(TEGRA_GPIO_PAA3, 1);
+                gpio_direction_output(TEGRA_GPIO_PAA4, 1);
+                gpio_direction_output(TEGRA_GPIO_PAA5, 1);
+                gpio_direction_output(TEGRA_GPIO_PAA6, 1);
+                gpio_direction_output(TEGRA_GPIO_PAA7, 1);
+        }
+
+	return 0;
+}
+
+static int tegra_sdhci_resume(struct sdhci_host *sdhci)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(sdhci);
+	struct tegra_sdhci_host *tegra_host = pltfm_host->priv;
+
+	/* Enable the power rails if any */
+	if (tegra_host->card_present) {
+		if (!tegra_host->is_rail_enabled) {
+			if (tegra_host->vdd_slot_reg)
+				regulator_enable(tegra_host->vdd_slot_reg);
+			if (tegra_host->vdd_io_reg) {
+				regulator_enable(tegra_host->vdd_io_reg);
+				tegra_sdhci_signal_voltage_switch(sdhci, MMC_SIGNAL_VOLTAGE_330);
+			}
+			tegra_host->is_rail_enabled = 1;
+		}
+	}
+	/* Setting the min identification clock of freq 400KHz */
+	tegra_sdhci_set_clock(sdhci, 400000);
+
+	/* Reset the controller and power on if MMC_KEEP_POWER flag is set*/
+	if (sdhci->mmc->pm_flags & MMC_PM_KEEP_POWER) {
+		tegra_sdhci_reset(sdhci, SDHCI_RESET_ALL);
+		sdhci_writeb(sdhci, SDHCI_POWER_ON, SDHCI_POWER_CONTROL);
+		sdhci->pwr = 0;
+	}
+
+	if (!strcmp(mmc_hostname(sdhci->mmc), "mmc0")) {
+                MMC_printk("%s: disable data pin", mmc_hostname(sdhci->mmc));
+
+                tegra_gpio_disable(TEGRA_GPIO_PAA0);
+                tegra_gpio_disable(TEGRA_GPIO_PAA1);
+                tegra_gpio_disable(TEGRA_GPIO_PAA2);
+                tegra_gpio_disable(TEGRA_GPIO_PAA3);
+                tegra_gpio_disable(TEGRA_GPIO_PAA4);
+                tegra_gpio_disable(TEGRA_GPIO_PAA5);
+                tegra_gpio_disable(TEGRA_GPIO_PAA6);
+                tegra_gpio_disable(TEGRA_GPIO_PAA7);
+        }
+
+	return 0;
+}
+
 static struct sdhci_ops tegra_sdhci_ops = {
 	.get_ro     = tegra_sdhci_get_ro,
+	.get_cd     = tegra_sdhci_get_cd,
 	.read_l     = tegra_sdhci_readl,
 	.read_w     = tegra_sdhci_readw,
 	.write_l    = tegra_sdhci_writel,
 	.platform_8bit_width = tegra_sdhci_8bit,
+	.set_clock  = tegra_sdhci_set_clock,
+	.suspend    = tegra_sdhci_suspend,
+	.resume     = tegra_sdhci_resume,
+	.platform_reset_exit = tegra_sdhci_reset_exit,
+	.set_uhs_signaling = tegra_sdhci_set_uhs_signaling,
+	.switch_signal_voltage = tegra_sdhci_signal_voltage_switch,
+	.execute_freq_tuning = sdhci_tegra_execute_tuning,
 };
 
 static struct sdhci_pltfm_data sdhci_tegra_pdata = {
 	.quirks = SDHCI_QUIRK_BROKEN_TIMEOUT_VAL |
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+		  SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK |
+		  SDHCI_QUIRK_NON_STD_VOLTAGE_SWITCHING |
+#endif
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+		  SDHCI_QUIRK_NONSTANDARD_CLOCK |
+		  SDHCI_QUIRK_NON_STANDARD_TUNING |
+#endif
 		  SDHCI_QUIRK_SINGLE_POWER_WRITE |
 		  SDHCI_QUIRK_NO_HISPD_BIT |
-		  SDHCI_QUIRK_BROKEN_ADMA_ZEROLEN_DESC,
+		  SDHCI_QUIRK_BROKEN_ADMA_ZEROLEN_DESC |
+		  SDHCI_QUIRK_NO_CALC_MAX_DISCARD_TO |
+		  SDHCI_QUIRK_BROKEN_CARD_DETECTION |
+		  0,
 	.ops  = &tegra_sdhci_ops,
 };
 
@@ -136,6 +998,7 @@ static int __devinit sdhci_tegra_probe(struct platform_device *pdev)
 	struct sdhci_pltfm_host *pltfm_host;
 	struct tegra_sdhci_platform_data *plat;
 	struct sdhci_host *host;
+	struct tegra_sdhci_host *tegra_host;
 	struct clk *clk;
 	int rc;
 
@@ -153,6 +1016,29 @@ static int __devinit sdhci_tegra_probe(struct platform_device *pdev)
 		goto err_no_plat;
 	}
 
+	tegra_host = kzalloc(sizeof(struct tegra_sdhci_host), GFP_KERNEL);
+	if (tegra_host == NULL) {
+		dev_err(mmc_dev(host->mmc), "failed to allocate tegra host\n");
+		rc = -ENOMEM;
+		goto err_no_mem;
+	}
+
+	/*
+	 * Hack: SDR12, SDR25, SDR50, SDR104 and DDR50 all require 1.8V
+	 * signalling which our current T30 designs can't do.
+	 */
+	if (plat->no_1v8)
+		host->quirks |= SDHCI_QUIRK2_NO_1_8_V;
+
+#ifdef CONFIG_MMC_EMBEDDED_SDIO
+	if (plat->mmc_data.embedded_sdio)
+		mmc_set_embedded_sdio_data(host->mmc,
+			&plat->mmc_data.embedded_sdio->cis,
+			&plat->mmc_data.embedded_sdio->cccr,
+			plat->mmc_data.embedded_sdio->funcs,
+			plat->mmc_data.embedded_sdio->num_funcs);
+#endif
+
 	if (gpio_is_valid(plat->power_gpio)) {
 		rc = gpio_request(plat->power_gpio, "sdhci_power");
 		if (rc) {
@@ -160,7 +1046,6 @@ static int __devinit sdhci_tegra_probe(struct platform_device *pdev)
 				"failed to allocate power gpio\n");
 			goto err_power_req;
 		}
-		tegra_gpio_enable(plat->power_gpio);
 		gpio_direction_output(plat->power_gpio, 1);
 	}
 
@@ -171,10 +1056,12 @@ static int __devinit sdhci_tegra_probe(struct platform_device *pdev)
 				"failed to allocate cd gpio\n");
 			goto err_cd_req;
 		}
-		tegra_gpio_enable(plat->cd_gpio);
 		gpio_direction_input(plat->cd_gpio);
 
-		rc = request_irq(gpio_to_irq(plat->cd_gpio), carddetect_irq,
+		tegra_host->card_present = (gpio_get_value(plat->cd_gpio) == 0);
+
+		rc = request_threaded_irq(gpio_to_irq(plat->cd_gpio), NULL,
+				 carddetect_irq,
 				 IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
 				 mmc_hostname(host->mmc), host);
 
@@ -182,7 +1069,18 @@ static int __devinit sdhci_tegra_probe(struct platform_device *pdev)
 			dev_err(mmc_dev(host->mmc), "request irq error\n");
 			goto err_cd_irq_req;
 		}
+		rc = enable_irq_wake(gpio_to_irq(plat->cd_gpio));
+		if (rc < 0)
+			dev_err(mmc_dev(host->mmc),
+				"SD card wake-up event registration"
+					"failed with eroor: %d\n", rc);
+
+	} else if (plat->mmc_data.register_status_notify) {
+		plat->mmc_data.register_status_notify(sdhci_status_notify_cb, host);
+	}
 
+	if (plat->mmc_data.status) {
+		plat->mmc_data.card_present = plat->mmc_data.status(mmc_dev(host->mmc));
 	}
 
 	if (gpio_is_valid(plat->wp_gpio)) {
@@ -192,23 +1090,120 @@ static int __devinit sdhci_tegra_probe(struct platform_device *pdev)
 				"failed to allocate wp gpio\n");
 			goto err_wp_req;
 		}
-		tegra_gpio_enable(plat->wp_gpio);
 		gpio_direction_input(plat->wp_gpio);
 	}
 
+	/*
+	 * If there is no card detect gpio, assume that the
+	 * card is always present.
+	 */
+	if (!gpio_is_valid(plat->cd_gpio))
+		tegra_host->card_present = 1;
+
+	if (!plat->mmc_data.built_in) {
+		if (plat->mmc_data.ocr_mask & SDHOST_1V8_OCR_MASK) {
+			tegra_host->vddio_min_uv = SDHOST_LOW_VOLT_MIN;
+			tegra_host->vddio_max_uv = SDHOST_LOW_VOLT_MAX;
+		} else {
+			/*
+			 * Set the minV and maxV to default
+			 * voltage range of 2.7V - 3.6V
+			 */
+			tegra_host->vddio_min_uv = SDHOST_HIGH_VOLT_MIN;
+			tegra_host->vddio_max_uv = SDHOST_HIGH_VOLT_MAX;
+		}
+		tegra_host->vdd_io_reg = regulator_get(mmc_dev(host->mmc), "vddio_sdmmc");
+		if (IS_ERR_OR_NULL(tegra_host->vdd_io_reg)) {
+			dev_info(mmc_dev(host->mmc), "%s regulator not found: %ld."
+				"Assuming vddio_sdmmc is not required.\n",
+					"vddio_sdmmc", PTR_ERR(tegra_host->vdd_io_reg));
+			tegra_host->vdd_io_reg = NULL;
+		} else {
+			rc = regulator_set_voltage(tegra_host->vdd_io_reg,
+				tegra_host->vddio_min_uv,
+				tegra_host->vddio_max_uv);
+			if (rc) {
+				dev_err(mmc_dev(host->mmc), "%s regulator_set_voltage failed: %d",
+					"vddio_sdmmc", rc);
+			}
+		}
+
+		tegra_host->vdd_slot_reg = regulator_get(mmc_dev(host->mmc), "vddio_sd_slot");
+		if (IS_ERR_OR_NULL(tegra_host->vdd_slot_reg)) {
+			dev_info(mmc_dev(host->mmc), "%s regulator not found: %ld."
+				" Assuming vddio_sd_slot is not required.\n",
+					"vddio_sd_slot", PTR_ERR(tegra_host->vdd_slot_reg));
+			tegra_host->vdd_slot_reg = NULL;
+		}
+
+		if (tegra_host->card_present) {
+			if (tegra_host->vdd_slot_reg)
+				regulator_enable(tegra_host->vdd_slot_reg);
+			if (tegra_host->vdd_io_reg)
+				regulator_enable(tegra_host->vdd_io_reg);
+			tegra_host->is_rail_enabled = 1;
+		}
+	}
+
 	clk = clk_get(mmc_dev(host->mmc), NULL);
 	if (IS_ERR(clk)) {
 		dev_err(mmc_dev(host->mmc), "clk err\n");
 		rc = PTR_ERR(clk);
 		goto err_clk_get;
 	}
-	clk_enable(clk);
+	rc = clk_enable(clk);
+	if (rc != 0)
+		goto err_clk_put;
+
+	if (!strcmp(dev_name(mmc_dev(host->mmc)), "sdhci-tegra.3")) {
+		tegra_host->emc_clk = clk_get(mmc_dev(host->mmc), "emc");
+		if (IS_ERR(tegra_host->emc_clk)) {
+			dev_err(mmc_dev(host->mmc), "clk err\n");
+			rc = PTR_ERR(tegra_host->emc_clk);
+			goto err_clk_put;
+		}
+		tegra_host->emc_max_clk =
+			clk_round_rate(tegra_host->emc_clk, ULONG_MAX);
+	}
+
 	pltfm_host->clk = clk;
+	pltfm_host->priv = tegra_host;
+	tegra_host->clk_enabled = true;
+	tegra_host->max_clk_limit = plat->max_clk_limit;
+	tegra_host->ddr_clk_limit = plat->ddr_clk_limit;
+	tegra_host->instance = pdev->id;
+	tegra_host->dpd = tegra_io_dpd_get(mmc_dev(host->mmc));
 
-	host->mmc->pm_caps = plat->pm_flags;
+	host->mmc->pm_caps |= plat->pm_caps;
+	host->mmc->pm_flags |= plat->pm_flags;
 
+	host->mmc->caps |= MMC_CAP_ERASE;
+	host->mmc->caps |= MMC_CAP_DISABLE;
+	host->mmc->caps |= MMC_CAP_CMD23;
+	/* enable 1/8V DDR capable */
+	host->mmc->caps |= MMC_CAP_1_8V_DDR;
 	if (plat->is_8bit)
 		host->mmc->caps |= MMC_CAP_8_BIT_DATA;
+	host->mmc->caps |= MMC_CAP_SDIO_IRQ;
+
+	host->mmc->pm_caps |= MMC_PM_KEEP_POWER | MMC_PM_IGNORE_PM_NOTIFY;
+	if (plat->mmc_data.built_in) {
+		host->mmc->caps |= MMC_CAP_NONREMOVABLE;
+	}
+	host->mmc->pm_flags |= MMC_PM_IGNORE_PM_NOTIFY;
+
+#ifdef CONFIG_MMC_BKOPS
+	host->mmc->caps |= MMC_CAP_BKOPS;
+#endif
+
+	tegra_sdhost_min_freq = TEGRA_SDHOST_MIN_FREQ;
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	tegra_host->hw_ops = &tegra_2x_sdhci_ops;
+	tegra_sdhost_std_freq = TEGRA2_SDHOST_STD_FREQ;
+#else
+	tegra_host->hw_ops = &tegra_3x_sdhci_ops;
+	tegra_sdhost_std_freq = TEGRA3_SDHOST_STD_FREQ;
+#endif
 
 	rc = sdhci_add_host(host);
 	if (rc)
@@ -217,27 +1212,25 @@ static int __devinit sdhci_tegra_probe(struct platform_device *pdev)
 	return 0;
 
 err_add_host:
+	clk_put(tegra_host->emc_clk);
 	clk_disable(pltfm_host->clk);
+err_clk_put:
 	clk_put(pltfm_host->clk);
 err_clk_get:
-	if (gpio_is_valid(plat->wp_gpio)) {
-		tegra_gpio_disable(plat->wp_gpio);
+	if (gpio_is_valid(plat->wp_gpio))
 		gpio_free(plat->wp_gpio);
-	}
 err_wp_req:
 	if (gpio_is_valid(plat->cd_gpio))
 		free_irq(gpio_to_irq(plat->cd_gpio), host);
 err_cd_irq_req:
-	if (gpio_is_valid(plat->cd_gpio)) {
-		tegra_gpio_disable(plat->cd_gpio);
+	if (gpio_is_valid(plat->cd_gpio))
 		gpio_free(plat->cd_gpio);
-	}
 err_cd_req:
-	if (gpio_is_valid(plat->power_gpio)) {
-		tegra_gpio_disable(plat->power_gpio);
+	if (gpio_is_valid(plat->power_gpio))
 		gpio_free(plat->power_gpio);
-	}
 err_power_req:
+err_no_mem:
+	kfree(tegra_host);
 err_no_plat:
 	sdhci_pltfm_free(pdev);
 	return rc;
@@ -247,6 +1240,7 @@ static int __devexit sdhci_tegra_remove(struct platform_device *pdev)
 {
 	struct sdhci_host *host = platform_get_drvdata(pdev);
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct tegra_sdhci_host *tegra_host = pltfm_host->priv;
 	struct tegra_sdhci_platform_data *plat;
 	int dead = (readl(host->ioaddr + SDHCI_INT_STATUS) == 0xffffffff);
 
@@ -254,26 +1248,35 @@ static int __devexit sdhci_tegra_remove(struct platform_device *pdev)
 
 	plat = pdev->dev.platform_data;
 
-	if (gpio_is_valid(plat->wp_gpio)) {
-		tegra_gpio_disable(plat->wp_gpio);
-		gpio_free(plat->wp_gpio);
+	disable_irq_wake(gpio_to_irq(plat->cd_gpio));
+
+	if (tegra_host->vdd_slot_reg) {
+		regulator_disable(tegra_host->vdd_slot_reg);
+		regulator_put(tegra_host->vdd_slot_reg);
+	}
+
+	if (tegra_host->vdd_io_reg) {
+		regulator_disable(tegra_host->vdd_io_reg);
+		regulator_put(tegra_host->vdd_io_reg);
 	}
 
+	if (gpio_is_valid(plat->wp_gpio))
+		gpio_free(plat->wp_gpio);
+
 	if (gpio_is_valid(plat->cd_gpio)) {
 		free_irq(gpio_to_irq(plat->cd_gpio), host);
-		tegra_gpio_disable(plat->cd_gpio);
 		gpio_free(plat->cd_gpio);
 	}
 
-	if (gpio_is_valid(plat->power_gpio)) {
-		tegra_gpio_disable(plat->power_gpio);
+	if (gpio_is_valid(plat->power_gpio))
 		gpio_free(plat->power_gpio);
-	}
 
-	clk_disable(pltfm_host->clk);
+	if (tegra_host->clk_enabled)
+		clk_disable(pltfm_host->clk);
 	clk_put(pltfm_host->clk);
 
 	sdhci_pltfm_free(pdev);
+	kfree(tegra_host);
 
 	return 0;
 }
@@ -293,7 +1296,11 @@ static struct platform_driver sdhci_tegra_driver = {
 
 static int __init sdhci_tegra_init(void)
 {
-	return platform_driver_register(&sdhci_tegra_driver);
+	int ret = 0;
+	printk(KERN_INFO "%s+ #####\n", __func__);
+	ret = platform_driver_register(&sdhci_tegra_driver);
+	printk(KERN_INFO "%s- #####\n", __func__);
+	return ret;
 }
 module_init(sdhci_tegra_init);
 
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index 0e02cc1df12e..5b0e1196dd97 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -25,6 +25,7 @@
 
 #include <linux/mmc/mmc.h>
 #include <linux/mmc/host.h>
+#include <linux/mmc/card.h>
 
 #include "sdhci.h"
 
@@ -485,7 +486,10 @@ static int sdhci_adma_table_pre(struct sdhci_host *host,
 		if (offset) {
 			if (data->flags & MMC_DATA_WRITE) {
 				buffer = sdhci_kmap_atomic(sg, &flags);
+/* Hack to avoid extensive warning messages from Redpine Signals LiteFi driver */
+#ifndef CONFIG_MACH_COLIBRI_T20
 				WARN_ON(((long)buffer & PAGE_MASK) > (PAGE_SIZE - 3));
+#endif
 				memcpy(align, buffer, offset);
 				sdhci_kunmap_atomic(buffer, &flags);
 			}
@@ -1046,14 +1050,11 @@ static void sdhci_set_clock(struct sdhci_host *host, unsigned int clock)
 	u16 clk = 0;
 	unsigned long timeout;
 
-	if (clock == host->clock)
+	if (clock && clock == host->clock)
 		return;
 
-	if (host->ops->set_clock) {
-		host->ops->set_clock(host, clock);
-		if (host->quirks & SDHCI_QUIRK_NONSTANDARD_CLOCK)
-			return;
-	}
+	if (host->quirks & SDHCI_QUIRK_NONSTANDARD_CLOCK)
+		return;
 
 	sdhci_writew(host, 0, SDHCI_CLOCK_CONTROL);
 
@@ -1231,11 +1232,15 @@ static void sdhci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 	host->mrq = mrq;
 
 	/* If polling, assume that the card is always present. */
-	if (host->quirks & SDHCI_QUIRK_BROKEN_CARD_DETECTION)
-		present = true;
-	else
+	if (host->quirks & SDHCI_QUIRK_BROKEN_CARD_DETECTION) {
+		if (host->ops->get_cd)
+			present = host->ops->get_cd(host);
+		else
+			present = true;
+	} else {
 		present = sdhci_readl(host, SDHCI_PRESENT_STATE) &
 				SDHCI_CARD_PRESENT;
+	}
 
 	if (!present || host->flags & SDHCI_DEVICE_DEAD) {
 		host->mrq->cmd->error = -ENOMEDIUM;
@@ -1277,6 +1282,20 @@ static void sdhci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 
 	host = mmc_priv(mmc);
 
+	/*
+	 * Controller registers should not be updated without the
+	 * controller clock enabled. Set the minimum controller
+	 * clock if there is no clock.
+	 */
+	if (host->ops->set_clock) {
+		if (!host->clock && !ios->clock) {
+			host->ops->set_clock(host, host->mmc->f_min);
+			host->clock = host->mmc->f_min;
+		} else if (ios->clock && (ios->clock != host->clock)) {
+			host->ops->set_clock(host, ios->clock);
+		}
+	}
+
 	spin_lock_irqsave(&host->lock, flags);
 
 	if (host->flags & SDHCI_DEVICE_DEAD)
@@ -1291,13 +1310,13 @@ static void sdhci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		sdhci_reinit(host);
 	}
 
-	sdhci_set_clock(host, ios->clock);
-
 	if (ios->power_mode == MMC_POWER_OFF)
 		sdhci_set_power(host, -1);
 	else
 		sdhci_set_power(host, ios->vdd);
 
+	sdhci_set_clock(host, ios->clock);
+
 	if (host->ops->platform_send_init_74_clocks)
 		host->ops->platform_send_init_74_clocks(host, ios->power_mode);
 
@@ -1336,14 +1355,14 @@ static void sdhci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 
 	if (host->version >= SDHCI_SPEC_300) {
 		u16 clk, ctrl_2;
-		unsigned int clock;
 
 		/* In case of UHS-I modes, set High Speed Enable */
-		if ((ios->timing == MMC_TIMING_UHS_SDR50) ||
+		if (((ios->timing == MMC_TIMING_UHS_SDR50) ||
 		    (ios->timing == MMC_TIMING_UHS_SDR104) ||
 		    (ios->timing == MMC_TIMING_UHS_DDR50) ||
 		    (ios->timing == MMC_TIMING_UHS_SDR25) ||
 		    (ios->timing == MMC_TIMING_UHS_SDR12))
+		    && !(host->quirks & SDHCI_QUIRK_NO_HISPD_BIT))
 			ctrl |= SDHCI_CTRL_HISPD;
 
 		ctrl_2 = sdhci_readw(host, SDHCI_HOST_CONTROL2);
@@ -1376,9 +1395,9 @@ static void sdhci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 			sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
 
 			/* Re-enable SD Clock */
-			clock = host->clock;
-			host->clock = 0;
-			sdhci_set_clock(host, clock);
+			clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
+			clk |= SDHCI_CLOCK_CARD_EN;
+			sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
 		}
 
 
@@ -1407,9 +1426,9 @@ static void sdhci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		}
 
 		/* Re-enable SD Clock */
-		clock = host->clock;
-		host->clock = 0;
-		sdhci_set_clock(host, clock);
+		clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
+		clk |= SDHCI_CLOCK_CARD_EN;
+		sdhci_writew(host, clk, SDHCI_CLOCK_CONTROL);
 	} else
 		sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
 
@@ -1424,6 +1443,12 @@ static void sdhci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 out:
 	mmiowb();
 	spin_unlock_irqrestore(&host->lock, flags);
+	/*
+	 * Controller clock should only be disabled after all the register
+	 * writes are done.
+	 */
+	if (!ios->clock && host->ops->set_clock)
+		host->ops->set_clock(host, ios->clock);
 }
 
 static int check_ro(struct sdhci_host *host)
@@ -1510,6 +1535,12 @@ static int sdhci_start_signal_voltage_switch(struct mmc_host *mmc,
 	if (host->version < SDHCI_SPEC_300)
 		return 0;
 
+	if (host->quirks & SDHCI_QUIRK_NON_STD_VOLTAGE_SWITCHING) {
+		if (host->ops->switch_signal_voltage)
+			return host->ops->switch_signal_voltage(
+				host, ios->signal_voltage);
+	}
+
 	/*
 	 * We first check whether the request is to set signalling voltage
 	 * to 3.3V. If so, we change the voltage to 3.3V and return quickly.
@@ -1552,7 +1583,6 @@ static int sdhci_start_signal_voltage_switch(struct mmc_host *mmc,
 
 			/* Wait for 5ms */
 			usleep_range(5000, 5500);
-
 			ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
 			if (ctrl & SDHCI_CTRL_VDD_180) {
 				/* Provide SDCLK again and wait for 1ms*/
@@ -1609,6 +1639,14 @@ static int sdhci_execute_tuning(struct mmc_host *mmc)
 	disable_irq(host->irq);
 	spin_lock(&host->lock);
 
+	if ((host->quirks & SDHCI_QUIRK_NON_STANDARD_TUNING) &&
+		host->ops->execute_freq_tuning) {
+		err = host->ops->execute_freq_tuning(host);
+		spin_unlock(&host->lock);
+		enable_irq(host->irq);
+		return err;
+	}
+
 	ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
 
 	/*
@@ -1782,6 +1820,16 @@ static void sdhci_enable_preset_value(struct mmc_host *mmc, bool enable)
 	if (host->version < SDHCI_SPEC_300)
 		return;
 
+	/*
+	 * Enabling preset value would make programming clock
+	 * divider ineffective. The controller would use the
+	 * values present in the preset value registers. In
+	 * case of non-standard clock, let the platform driver
+	 * decide whether to enable preset or not.
+	 */
+	if (host->quirks & SDHCI_QUIRK_NONSTANDARD_CLOCK)
+		return;
+
 	spin_lock_irqsave(&host->lock, flags);
 
 	ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
@@ -1801,10 +1849,42 @@ static void sdhci_enable_preset_value(struct mmc_host *mmc, bool enable)
 	spin_unlock_irqrestore(&host->lock, flags);
 }
 
+int sdhci_enable(struct mmc_host *mmc)
+{
+	struct sdhci_host *host = mmc_priv(mmc);
+
+	if (!mmc->card || mmc->card->type == MMC_TYPE_SDIO)
+		return 0;
+
+	if (mmc->ios.clock) {
+		if (host->ops->set_clock)
+			host->ops->set_clock(host, mmc->ios.clock);
+		sdhci_set_clock(host, mmc->ios.clock);
+	}
+
+	return 0;
+}
+
+int sdhci_disable(struct mmc_host *mmc, int lazy)
+{
+	struct sdhci_host *host = mmc_priv(mmc);
+
+	if (!mmc->card || mmc->card->type == MMC_TYPE_SDIO)
+		return 0;
+
+	sdhci_set_clock(host, 0);
+	if (host->ops->set_clock)
+		host->ops->set_clock(host, 0);
+
+	return 0;
+}
+
 static const struct mmc_host_ops sdhci_ops = {
 	.request	= sdhci_request,
 	.set_ios	= sdhci_set_ios,
 	.get_ro		= sdhci_get_ro,
+	.enable		= sdhci_enable,
+	.disable	= sdhci_disable,
 	.enable_sdio_irq = sdhci_enable_sdio_irq,
 	.start_signal_voltage_switch	= sdhci_start_signal_voltage_switch,
 	.execute_tuning			= sdhci_execute_tuning,
@@ -1884,6 +1964,8 @@ static void sdhci_tasklet_finish(unsigned long param)
 			/* This is to force an update */
 			clock = host->clock;
 			host->clock = 0;
+			if (host->ops->set_clock)
+				host->ops->set_clock(host, clock);
 			sdhci_set_clock(host, clock);
 		}
 
@@ -2239,26 +2321,51 @@ out:
 
 int sdhci_suspend_host(struct sdhci_host *host, pm_message_t state)
 {
-	int ret;
+	int ret = 0;
+	bool has_tuning_timer;
+	struct mmc_host *mmc = host->mmc;
 
 	sdhci_disable_card_detection(host);
 
 	/* Disable tuning since we are suspending */
-	if (host->version >= SDHCI_SPEC_300 && host->tuning_count &&
-	    host->tuning_mode == SDHCI_TUNING_MODE_1) {
+	has_tuning_timer = host->version >= SDHCI_SPEC_300 &&
+		host->tuning_count && host->tuning_mode == SDHCI_TUNING_MODE_1;
+	if (has_tuning_timer) {
 		host->flags &= ~SDHCI_NEEDS_RETUNING;
 		mod_timer(&host->tuning_timer, jiffies +
 			host->tuning_count * HZ);
 	}
 
-	ret = mmc_suspend_host(host->mmc);
-	if (ret)
-		return ret;
+	if (mmc->card) {
+		ret = mmc_suspend_host(host->mmc);
+		if (ret)
+			goto err_suspend_host;
+	}
 
-	free_irq(host->irq, host);
+	if (mmc->pm_flags & MMC_PM_KEEP_POWER)
+		host->card_int_set = sdhci_readl(host, SDHCI_INT_ENABLE) &
+			SDHCI_INT_CARD_INT;
+
+	sdhci_mask_irqs(host, SDHCI_INT_ALL_MASK);
 
-	if (host->vmmc)
+	if (host->vmmc) {
 		ret = regulator_disable(host->vmmc);
+		if (ret)
+			pr_err("%s: failed to disable regulator\n", __func__);
+	}
+
+	if (host->irq)
+		disable_irq(host->irq);
+
+	return 0;
+
+err_suspend_host:
+	/* Set the re-tuning expiration flag */
+	if ((host->version >= SDHCI_SPEC_300) && host->tuning_count &&
+	    (host->tuning_mode == SDHCI_TUNING_MODE_1))
+		host->flags |= SDHCI_NEEDS_RETUNING;
+
+	sdhci_enable_card_detection(host);
 
 	return ret;
 }
@@ -2267,7 +2374,8 @@ EXPORT_SYMBOL_GPL(sdhci_suspend_host);
 
 int sdhci_resume_host(struct sdhci_host *host)
 {
-	int ret;
+	int ret = 0;
+	struct mmc_host *mmc = host->mmc;
 
 	if (host->vmmc) {
 		int ret = regulator_enable(host->vmmc);
@@ -2281,15 +2389,21 @@ int sdhci_resume_host(struct sdhci_host *host)
 			host->ops->enable_dma(host);
 	}
 
-	ret = request_irq(host->irq, sdhci_irq, IRQF_SHARED,
-			  mmc_hostname(host->mmc), host);
-	if (ret)
-		return ret;
+	if (host->irq)
+		enable_irq(host->irq);
 
 	sdhci_init(host, (host->mmc->pm_flags & MMC_PM_KEEP_POWER));
 	mmiowb();
 
-	ret = mmc_resume_host(host->mmc);
+	if (mmc->card) {
+		ret = mmc_resume_host(host->mmc);
+		/* Enable card interrupt as it is overwritten in sdhci_init */
+		if ((mmc->caps & MMC_CAP_SDIO_IRQ) &&
+			(mmc->pm_flags & MMC_PM_KEEP_POWER))
+				if (host->card_int_set)
+					mmc->ops->enable_sdio_irq(mmc, true);
+	}
+
 	sdhci_enable_card_detection(host);
 
 	/* Set the re-tuning expiration flag */
@@ -2507,9 +2621,8 @@ int sdhci_add_host(struct sdhci_host *host)
 	if (host->quirks & SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK)
 		host->timeout_clk = mmc->f_max / 1000;
 
-	mmc->max_discard_to = (1 << 27) / host->timeout_clk;
-
-	mmc->caps |= MMC_CAP_SDIO_IRQ | MMC_CAP_ERASE | MMC_CAP_CMD23;
+	if (!(host->quirks & SDHCI_QUIRK_NO_CALC_MAX_DISCARD_TO))
+		mmc->max_discard_to = (1 << 27) / host->timeout_clk;
 
 	if (host->quirks & SDHCI_QUIRK_MULTIBLOCK_READ_ACMD12)
 		host->flags |= SDHCI_AUTO_CMD12;
@@ -2538,11 +2651,16 @@ int sdhci_add_host(struct sdhci_host *host)
 		mmc->caps |= MMC_CAP_SD_HIGHSPEED | MMC_CAP_MMC_HIGHSPEED;
 
 	if ((host->quirks & SDHCI_QUIRK_BROKEN_CARD_DETECTION) &&
-	    mmc_card_is_removable(mmc))
+	    mmc_card_is_removable(mmc) && !(host->ops->get_cd))
 		mmc->caps |= MMC_CAP_NEEDS_POLL;
 
-	/* UHS-I mode(s) supported by the host controller. */
-	if (host->version >= SDHCI_SPEC_300)
+	if (host->quirks & SDHCI_QUIRK2_NO_1_8_V)
+		caps[1] &= ~(SDHCI_SUPPORT_SDR104 | SDHCI_SUPPORT_SDR50 |
+			     SDHCI_SUPPORT_DDR50);
+
+	/* Any UHS-I mode in caps implies SDR12 and SDR25 support. */
+	if (caps[1] & (SDHCI_SUPPORT_SDR104 | SDHCI_SUPPORT_SDR50 |
+		       SDHCI_SUPPORT_DDR50))
 		mmc->caps |= MMC_CAP_UHS_SDR12 | MMC_CAP_UHS_SDR25;
 
 	/* SDR104 supports also implies SDR50 support */
diff --git a/drivers/mmc/host/sdhci.h b/drivers/mmc/host/sdhci.h
index 745c42fa41ed..c00833de19da 100644
--- a/drivers/mmc/host/sdhci.h
+++ b/drivers/mmc/host/sdhci.h
@@ -270,10 +270,15 @@ struct sdhci_ops {
 	void (*platform_send_init_74_clocks)(struct sdhci_host *host,
 					     u8 power_mode);
 	unsigned int    (*get_ro)(struct sdhci_host *host);
+	unsigned int    (*get_cd)(struct sdhci_host *host);
 	void	(*platform_reset_enter)(struct sdhci_host *host, u8 mask);
 	void	(*platform_reset_exit)(struct sdhci_host *host, u8 mask);
 	int	(*set_uhs_signaling)(struct sdhci_host *host, unsigned int uhs);
-
+	int	(*suspend)(struct sdhci_host *host, pm_message_t state);
+	int	(*resume)(struct sdhci_host *host);
+	int	(*switch_signal_voltage)(struct sdhci_host *host,
+				unsigned int signal_voltage);
+	int	(*execute_freq_tuning)(struct sdhci_host *sdhci);
 };
 
 #ifdef CONFIG_MMC_SDHCI_IO_ACCESSORS
diff --git a/drivers/mmc/host/sh_mmcif.c b/drivers/mmc/host/sh_mmcif.c
index 557886bee9ce..dc44c5c33e01 100644
--- a/drivers/mmc/host/sh_mmcif.c
+++ b/drivers/mmc/host/sh_mmcif.c
@@ -229,8 +229,8 @@ static void sh_mmcif_start_dma_rx(struct sh_mmcif_host *host)
 			 DMA_FROM_DEVICE);
 	if (ret > 0) {
 		host->dma_active = true;
-		desc = chan->device->device_prep_slave_sg(chan, sg, ret,
-			DMA_FROM_DEVICE, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		desc = dmaengine_prep_slave_sg(chan, sg, ret,
+			DMA_DEV_TO_MEM, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	}
 
 	if (desc) {
@@ -277,8 +277,8 @@ static void sh_mmcif_start_dma_tx(struct sh_mmcif_host *host)
 			 DMA_TO_DEVICE);
 	if (ret > 0) {
 		host->dma_active = true;
-		desc = chan->device->device_prep_slave_sg(chan, sg, ret,
-			DMA_TO_DEVICE, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		desc = dmaengine_prep_slave_sg(chan, sg, ret,
+			DMA_MEM_TO_DEV, DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	}
 
 	if (desc) {
diff --git a/drivers/mmc/host/tmio_mmc_dma.c b/drivers/mmc/host/tmio_mmc_dma.c
index 86f259cdfcbc..def9c54f73f5 100644
--- a/drivers/mmc/host/tmio_mmc_dma.c
+++ b/drivers/mmc/host/tmio_mmc_dma.c
@@ -76,8 +76,8 @@ static void tmio_mmc_start_dma_rx(struct tmio_mmc_host *host)
 
 	ret = dma_map_sg(chan->device->dev, sg, host->sg_len, DMA_FROM_DEVICE);
 	if (ret > 0)
-		desc = chan->device->device_prep_slave_sg(chan, sg, ret,
-			DMA_FROM_DEVICE, DMA_CTRL_ACK);
+		desc = dmaengine_prep_slave_sg(chan, sg, ret,
+			DMA_DEV_TO_MEM, DMA_CTRL_ACK);
 
 	if (desc) {
 		cookie = dmaengine_submit(desc);
@@ -157,8 +157,8 @@ static void tmio_mmc_start_dma_tx(struct tmio_mmc_host *host)
 
 	ret = dma_map_sg(chan->device->dev, sg, host->sg_len, DMA_TO_DEVICE);
 	if (ret > 0)
-		desc = chan->device->device_prep_slave_sg(chan, sg, ret,
-			DMA_TO_DEVICE, DMA_CTRL_ACK);
+		desc = dmaengine_prep_slave_sg(chan, sg, ret,
+			DMA_MEM_TO_DEV, DMA_CTRL_ACK);
 
 	if (desc) {
 		cookie = dmaengine_submit(desc);
diff --git a/drivers/mmc/host/vub300.c b/drivers/mmc/host/vub300.c
index e8f6e65183d7..2ec978bc32ba 100644
--- a/drivers/mmc/host/vub300.c
+++ b/drivers/mmc/host/vub300.c
@@ -259,7 +259,7 @@ static int firmware_rom_wait_states = 0x04;
 static int firmware_rom_wait_states = 0x1C;
 #endif
 
-module_param(firmware_rom_wait_states, bool, 0644);
+module_param(firmware_rom_wait_states, int, 0644);
 MODULE_PARM_DESC(firmware_rom_wait_states,
 		 "ROM wait states byte=RRRIIEEE (Reserved Internal External)");
 
diff --git a/drivers/mtd/chips/cfi_cmdset_0002.c b/drivers/mtd/chips/cfi_cmdset_0002.c
index 23175edd5634..51d79223bc71 100644
--- a/drivers/mtd/chips/cfi_cmdset_0002.c
+++ b/drivers/mtd/chips/cfi_cmdset_0002.c
@@ -190,9 +190,17 @@ static void fixup_use_write_buffers(struct mtd_info *mtd)
 {
 	struct map_info *map = mtd->priv;
 	struct cfi_private *cfi = map->fldrv_priv;
+	struct cfi_pri_amdstd *extp = cfi->cmdset_priv;
+
 	if (cfi->cfiq->BufWriteTimeoutTyp) {
 		DEBUG(MTD_DEBUG_LEVEL1, "Using buffer write method\n" );
 		mtd->write = cfi_amdstd_write_buffers;
+
+		if (extp->SiliconRevision >= 0x1C) {
+			mtd->writesize = 512;
+			mtd->flags &= ~MTD_BIT_WRITEABLE;
+			printk(KERN_INFO "Enabling Spansion 65nm mode, writesize = 512 bytes\n");
+		}
 	}
 }
 
@@ -1372,21 +1380,18 @@ static int cfi_amdstd_write_words(struct mtd_info *mtd, loff_t to, size_t len,
 }
 
 
-/*
- * FIXME: interleaved mode not tested, and probably not supported!
- */
 static int __xipram do_write_buffer(struct map_info *map, struct flchip *chip,
 				    unsigned long adr, const u_char *buf,
 				    int len)
 {
 	struct cfi_private *cfi = map->fldrv_priv;
 	unsigned long timeo = jiffies + HZ;
-	/* see comments in do_write_oneword() regarding uWriteTimeo. */
-	unsigned long uWriteTimeout = ( HZ / 1000 ) + 1;
+	/* see comments in do_write_oneword() regarding uWriteTimeout, 20ms */
+	unsigned long uWriteTimeout = (HZ / 50) + 1;
 	int ret = -EIO;
 	unsigned long cmd_adr;
-	int z, words;
-	map_word datum;
+	int z, words, prolog, epilog, buflen = len;
+	map_word datum, pdat, edat;
 
 	adr += chip->start;
 	cmd_adr = adr;
@@ -1407,6 +1412,21 @@ static int __xipram do_write_buffer(struct map_info *map, struct flchip *chip,
 	ENABLE_VPP(map);
 	xip_disable(map, chip, cmd_adr);
 
+	/* If start is not bus-aligned, prepend old contents of flash */
+	prolog = (adr & (map_bankwidth(map)-1));
+	if (prolog) {
+		adr -= prolog;
+		cmd_adr -= prolog;
+		len += prolog;
+		pdat = map_read(map, adr);
+	}
+	/* If end is not bus-aligned, append old contents of flash */
+	epilog = ((adr + len) & (map_bankwidth(map)-1));
+	if (epilog) {
+		len += map_bankwidth(map)-epilog;
+		edat = map_read(map, adr + len - map_bankwidth(map));
+	}
+
 	cfi_send_gen_cmd(0xAA, cfi->addr_unlock1, chip->start, map, cfi, cfi->device_type, NULL);
 	cfi_send_gen_cmd(0x55, cfi->addr_unlock2, chip->start, map, cfi, cfi->device_type, NULL);
 
@@ -1420,8 +1440,21 @@ static int __xipram do_write_buffer(struct map_info *map, struct flchip *chip,
 	map_write(map, CMD(words - 1), cmd_adr);
 	/* Write data */
 	z = 0;
+	if (prolog) {
+		datum = map_word_load_partial(map, pdat, buf, prolog,
+				min_t(int, buflen,
+					map_bankwidth(map) - prolog));
+		map_write(map, datum, adr);
+
+		z += map_bankwidth(map);
+		buf += map_bankwidth(map) - prolog;
+	}
 	while(z < words * map_bankwidth(map)) {
-		datum = map_word_load(map, buf);
+		if (epilog && z >= (words-1) * map_bankwidth(map))
+			datum = map_word_load_partial(map, edat,
+					buf, 0, epilog);
+		else
+			datum = map_word_load(map, buf);
 		map_write(map, datum, adr + z);
 
 		z += map_bankwidth(map);
@@ -1470,8 +1503,13 @@ static int __xipram do_write_buffer(struct map_info *map, struct flchip *chip,
 
 	/* reset on all failures. */
 	map_write( map, CMD(0xF0), chip->start );
+	cfi_send_gen_cmd(0xAA, cfi->addr_unlock1, chip->start, map,
+				cfi, cfi->device_type, NULL);
+	cfi_send_gen_cmd(0x55, cfi->addr_unlock2, chip->start, map,
+				cfi, cfi->device_type, NULL);
+	cfi_send_gen_cmd(0xF0, cfi->addr_unlock1, chip->start, map,
+				cfi, cfi->device_type, NULL);
 	xip_enable(map, chip, adr);
-	/* FIXME - should have reset delay before continuing */
 
 	printk(KERN_WARNING "MTD %s(): software timeout\n",
 	       __func__ );
@@ -1503,36 +1541,12 @@ static int cfi_amdstd_write_buffers(struct mtd_info *mtd, loff_t to, size_t len,
 	chipnum = to >> cfi->chipshift;
 	ofs = to  - (chipnum << cfi->chipshift);
 
-	/* If it's not bus-aligned, do the first word write */
-	if (ofs & (map_bankwidth(map)-1)) {
-		size_t local_len = (-ofs)&(map_bankwidth(map)-1);
-		if (local_len > len)
-			local_len = len;
-		ret = cfi_amdstd_write_words(mtd, ofs + (chipnum<<cfi->chipshift),
-					     local_len, retlen, buf);
-		if (ret)
-			return ret;
-		ofs += local_len;
-		buf += local_len;
-		len -= local_len;
-
-		if (ofs >> cfi->chipshift) {
-			chipnum ++;
-			ofs = 0;
-			if (chipnum == cfi->numchips)
-				return 0;
-		}
-	}
-
-	/* Write buffer is worth it only if more than one word to write... */
-	while (len >= map_bankwidth(map) * 2) {
+	while (len) {
 		/* We must not cross write block boundaries */
 		int size = wbufsize - (ofs & (wbufsize-1));
 
 		if (size > len)
 			size = len;
-		if (size % map_bankwidth(map))
-			size -= size % map_bankwidth(map);
 
 		ret = do_write_buffer(map, &cfi->chips[chipnum],
 				      ofs, buf, size);
@@ -1552,16 +1566,6 @@ static int cfi_amdstd_write_buffers(struct mtd_info *mtd, loff_t to, size_t len,
 		}
 	}
 
-	if (len) {
-		size_t retlen_dregs = 0;
-
-		ret = cfi_amdstd_write_words(mtd, ofs + (chipnum<<cfi->chipshift),
-					     len, &retlen_dregs, buf);
-
-		*retlen += retlen_dregs;
-		return ret;
-	}
-
 	return 0;
 }
 
diff --git a/drivers/mtd/devices/Kconfig b/drivers/mtd/devices/Kconfig
index 35081ce77fbd..943d90f08c08 100644
--- a/drivers/mtd/devices/Kconfig
+++ b/drivers/mtd/devices/Kconfig
@@ -78,6 +78,12 @@ config MTD_DATAFLASH_OTP
 	  other key product data.  The second half is programmed with a
 	  unique-to-each-chip bit pattern at the factory.
 
+config MTD_NAND_TEGRA
+	tristate "Support for NAND Controller on NVIDIA Tegra"
+	depends on ARCH_TEGRA
+	help
+	  Enables NAND flash support for NVIDIA's Tegra family of chips.
+
 config MTD_M25P80
 	tristate "Support most SPI Flash chips (AT26DF, M25P, W25X, ...)"
 	depends on SPI_MASTER && EXPERIMENTAL
diff --git a/drivers/mtd/devices/Makefile b/drivers/mtd/devices/Makefile
index f3226b1d38fc..b9337a4f0c06 100644
--- a/drivers/mtd/devices/Makefile
+++ b/drivers/mtd/devices/Makefile
@@ -1,6 +1,7 @@
 #
 # linux/drivers/mtd/devices/Makefile
 #
+GCOV_PROFILE := y
 
 obj-$(CONFIG_MTD_DOC2000)	+= doc2000.o
 obj-$(CONFIG_MTD_DOC2001)	+= doc2001.o
@@ -17,3 +18,5 @@ obj-$(CONFIG_MTD_BLOCK2MTD)	+= block2mtd.o
 obj-$(CONFIG_MTD_DATAFLASH)	+= mtd_dataflash.o
 obj-$(CONFIG_MTD_M25P80)	+= m25p80.o
 obj-$(CONFIG_MTD_SST25L)	+= sst25l.o
+CFLAGS_tegra_nand.o		= -Werror
+obj-$(CONFIG_MTD_NAND_TEGRA)	+= tegra_nand.o
diff --git a/drivers/mtd/devices/tegra_nand.c b/drivers/mtd/devices/tegra_nand.c
new file mode 100644
index 000000000000..6034f0b2f239
--- /dev/null
+++ b/drivers/mtd/devices/tegra_nand.c
@@ -0,0 +1,1840 @@
+/*
+ * drivers/mtd/devices/tegra_nand.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Dima Zavin <dima@android.com>
+ *         Colin Cross <ccross@android.com>
+ *
+ * Copyright (C) 2010-2011 Nvidia Graphics Pvt. Ltd.
+ * http://www.nvidia.com
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ * Derived from: drivers/mtd/nand/nand_base.c
+ *               drivers/mtd/nand/pxa3xx.c
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+#include <linux/platform_device.h>
+#include <linux/types.h>
+#include <linux/clk.h>
+#include <linux/slab.h>
+#include <linux/gpio.h>
+
+#include <mach/nand.h>
+
+#include "tegra_nand.h"
+
+#define DRIVER_NAME	"tegra_nand"
+#define DRIVER_DESC	"Nvidia Tegra NAND Flash Controller driver"
+
+#define MAX_DMA_SZ			SZ_64K
+#define ECC_BUF_SZ			SZ_1K
+
+/* FIXME: is this right?!
+ * NvRM code says it should be 128 bytes, but that seems awfully small
+ */
+
+/*#define TEGRA_NAND_DEBUG
+#define TEGRA_NAND_DEBUG_PEDANTIC*/
+
+#ifdef TEGRA_NAND_DEBUG
+#define TEGRA_DBG(fmt, args...) \
+	do { pr_info(fmt, ##args); } while (0)
+#else
+#define TEGRA_DBG(fmt, args...)
+#endif
+
+/* TODO: will vary with devices, move into appropriate device spcific header */
+#define SCAN_TIMING_VAL		0x3f0bd214
+#define SCAN_TIMING2_VAL	0xb
+
+#define TIMEOUT (2 * HZ)
+/* TODO: pull in the register defs (fields, masks, etc) from Nvidia files
+ * so we don't have to redefine them */
+
+static const char *part_probes[] = { "cmdlinepart", NULL, };
+
+struct tegra_nand_chip {
+	spinlock_t lock;
+	uint32_t chipsize;
+	int num_chips;
+	int curr_chip;
+
+	/* addr >> chip_shift == chip number */
+	uint32_t chip_shift;
+	/* (addr >> page_shift) & page_mask == page number within chip */
+	uint32_t page_shift;
+	uint32_t page_mask;
+	/* column within page */
+	uint32_t column_mask;
+	/* addr >> block_shift == block number (across the whole mtd dev, not
+	 * just a single chip. */
+	uint32_t block_shift;
+
+	void *priv;
+};
+
+struct tegra_nand_info {
+	struct tegra_nand_chip chip;
+	struct mtd_info mtd;
+	struct tegra_nand_platform *plat;
+	struct device *dev;
+	struct mtd_partition *parts;
+
+	/* synchronizes access to accessing the actual NAND controller */
+	struct mutex lock;
+	/* partial_unaligned_rw_buffer is temporary buffer used during
+	   reading of unaligned data from nand pages or if data to be read
+	   is less than nand page size.
+	 */
+	uint8_t *partial_unaligned_rw_buffer;
+
+	void *oob_dma_buf;
+	dma_addr_t oob_dma_addr;
+	/* ecc error vector info (offset into page and data mask to apply */
+	void *ecc_buf;
+	dma_addr_t ecc_addr;
+	/* ecc error status (page number, err_cnt) */
+	uint32_t *ecc_errs;
+	uint32_t num_ecc_errs;
+	uint32_t max_ecc_errs;
+	spinlock_t ecc_lock;
+
+	uint32_t command_reg;
+	uint32_t config_reg;
+	uint32_t dmactrl_reg;
+
+	struct completion cmd_complete;
+	struct completion dma_complete;
+
+	/* bad block bitmap: 1 == good, 0 == bad/unknown */
+	unsigned long *bb_bitmap;
+
+	struct clk *clk;
+	uint32_t is_data_bus_width_16;
+	uint32_t device_id;
+	uint32_t vendor_id;
+	uint32_t dev_parms;
+	uint32_t num_bad_blocks;
+};
+#define MTD_TO_INFO(mtd)	container_of((mtd), struct tegra_nand_info, mtd)
+
+/* 64 byte oob block info for large page (== 2KB) device
+ *
+ * OOB flash layout for Tegra with Reed-Solomon 4 symbol correct ECC:
+ *      Skipped bytes(4)
+ *      Main area Ecc(36)
+ *      Tag data(20)
+ *      Tag data Ecc(4)
+ *
+ * Yaffs2 will use 16 tag bytes.
+ */
+
+static struct nand_ecclayout tegra_nand_oob_64 = {
+	.eccbytes = 36,
+	.eccpos = {
+		   4, 5, 6, 7, 8, 9, 10, 11, 12,
+		   13, 14, 15, 16, 17, 18, 19, 20, 21,
+		   22, 23, 24, 25, 26, 27, 28, 29, 30,
+		   31, 32, 33, 34, 35, 36, 37, 38, 39,
+		   },
+	.oobavail = 20,
+	.oobfree = {
+		    {.offset = 40,
+		     .length = 20,
+		     },
+		    },
+};
+
+static struct nand_ecclayout tegra_nand_oob_128 = {
+	.eccbytes = 72,
+	.eccpos = {
+		   4, 5, 6, 7, 8, 9, 10, 11, 12,
+		   13, 14, 15, 16, 17, 18, 19, 20, 21,
+		   22, 23, 24, 25, 26, 27, 28, 29, 30,
+		   31, 32, 33, 34, 35, 36, 37, 38, 39,
+		   40, 41, 42, 43, 44, 45, 46, 47, 48,
+		   49, 50, 51, 52, 53, 54, 55, 56, 57,
+		   58, 59, 60, 61, 62, 63, 64, 65, 66,
+		   /* ECC POS is only of size 64 bytes  so commenting the remaining
+		    * bytes here. As driver uses the Hardware ECC so it there is
+		    * no issue with it
+		    */
+		   /*67, 68, 69, 70, 71, 72, 73, 74, 75, */
+		   },
+	.oobavail = 48,
+	.oobfree = {
+		    {.offset = 76,
+		     .length = 48,
+		     },
+		    },
+};
+
+static struct nand_flash_dev *find_nand_flash_device(int dev_id)
+{
+	struct nand_flash_dev *dev = &nand_flash_ids[0];
+
+	while (dev->name && dev->id != dev_id)
+		dev++;
+	return dev->name ? dev : NULL;
+}
+
+static struct nand_manufacturers *find_nand_flash_vendor(int vendor_id)
+{
+	struct nand_manufacturers *vendor = &nand_manuf_ids[0];
+
+	while (vendor->id && vendor->id != vendor_id)
+		vendor++;
+	return vendor->id ? vendor : NULL;
+}
+
+#define REG_NAME(name)			{ name, #name }
+static struct {
+	uint32_t addr;
+	char *name;
+} reg_names[] = {
+	REG_NAME(COMMAND_REG),
+	REG_NAME(STATUS_REG),
+	REG_NAME(ISR_REG),
+	REG_NAME(IER_REG),
+	REG_NAME(CONFIG_REG),
+	REG_NAME(TIMING_REG),
+	REG_NAME(RESP_REG),
+	REG_NAME(TIMING2_REG),
+	REG_NAME(CMD_REG1),
+	REG_NAME(CMD_REG2),
+	REG_NAME(ADDR_REG1),
+	REG_NAME(ADDR_REG2),
+	REG_NAME(DMA_MST_CTRL_REG),
+	REG_NAME(DMA_CFG_A_REG),
+	REG_NAME(DMA_CFG_B_REG),
+	REG_NAME(FIFO_CTRL_REG),
+	REG_NAME(DATA_BLOCK_PTR_REG),
+	REG_NAME(TAG_PTR_REG),
+	REG_NAME(ECC_PTR_REG),
+	REG_NAME(DEC_STATUS_REG),
+	REG_NAME(HWSTATUS_CMD_REG),
+	REG_NAME(HWSTATUS_MASK_REG),
+	{0, NULL},
+};
+
+#undef REG_NAME
+
+static int dump_nand_regs(void)
+{
+	int i = 0;
+
+	TEGRA_DBG("%s: dumping registers\n", __func__);
+	while (reg_names[i].name != NULL) {
+		TEGRA_DBG("%s = 0x%08x\n", reg_names[i].name,
+			  readl(reg_names[i].addr));
+		i++;
+	}
+	TEGRA_DBG("%s: end of reg dump\n", __func__);
+	return 1;
+}
+
+static inline void enable_ints(struct tegra_nand_info *info, uint32_t mask)
+{
+	(void)info;
+	writel(readl(IER_REG) | mask, IER_REG);
+}
+
+static inline void disable_ints(struct tegra_nand_info *info, uint32_t mask)
+{
+	(void)info;
+	writel(readl(IER_REG) & ~mask, IER_REG);
+}
+
+static inline void
+split_addr(struct tegra_nand_info *info, loff_t offset, int *chipnr,
+	   uint32_t *page, uint32_t *column)
+{
+	*chipnr = (int)(offset >> info->chip.chip_shift);
+	*page = (offset >> info->chip.page_shift) & info->chip.page_mask;
+	*column = offset & info->chip.column_mask;
+}
+
+static irqreturn_t tegra_nand_irq(int irq, void *dev_id)
+{
+	struct tegra_nand_info *info = dev_id;
+	uint32_t isr;
+	uint32_t ier;
+	uint32_t dma_ctrl;
+	uint32_t tmp;
+
+	isr = readl(ISR_REG);
+	ier = readl(IER_REG);
+	dma_ctrl = readl(DMA_MST_CTRL_REG);
+#ifdef DEBUG_DUMP_IRQ
+	pr_info("IRQ: ISR=0x%08x IER=0x%08x DMA_IS=%d DMA_IE=%d\n",
+		isr, ier, !!(dma_ctrl & (1 << 20)), !!(dma_ctrl & (1 << 28)));
+#endif
+	if (isr & ISR_CMD_DONE) {
+		if (likely(!(readl(COMMAND_REG) & COMMAND_GO)))
+			complete(&info->cmd_complete);
+		else
+			pr_err("tegra_nand_irq: Spurious cmd done irq!\n");
+	}
+
+	if (isr & ISR_ECC_ERR) {
+		/* always want to read the decode status so xfers don't stall. */
+		tmp = readl(DEC_STATUS_REG);
+
+		/* was ECC check actually enabled */
+		if ((ier & IER_ECC_ERR)) {
+			unsigned long flags;
+			spin_lock_irqsave(&info->ecc_lock, flags);
+			info->ecc_errs[info->num_ecc_errs++] = tmp;
+			spin_unlock_irqrestore(&info->ecc_lock, flags);
+		}
+	}
+
+	if ((dma_ctrl & DMA_CTRL_IS_DMA_DONE) &&
+	    (dma_ctrl & DMA_CTRL_IE_DMA_DONE)) {
+		complete(&info->dma_complete);
+		writel(dma_ctrl, DMA_MST_CTRL_REG);
+	}
+
+	if ((isr & ISR_UND) && (ier & IER_UND))
+		pr_err("%s: fifo underrun.\n", __func__);
+
+	if ((isr & ISR_OVR) && (ier & IER_OVR))
+		pr_err("%s: fifo overrun.\n", __func__);
+
+	/* clear ALL interrupts?! */
+	writel(isr & 0xfffc, ISR_REG);
+
+	return IRQ_HANDLED;
+}
+
+static inline int tegra_nand_is_cmd_done(struct tegra_nand_info *info)
+{
+	return (readl(COMMAND_REG) & COMMAND_GO) ? 0 : 1;
+}
+
+static int tegra_nand_wait_cmd_done(struct tegra_nand_info *info)
+{
+	uint32_t timeout = TIMEOUT; /* TODO: make this realistic */
+	int ret;
+
+	ret = wait_for_completion_timeout(&info->cmd_complete, timeout);
+
+#ifdef TEGRA_NAND_DEBUG_PEDANTIC
+	BUG_ON(!ret && dump_nand_regs());
+#endif
+
+	return ret ? 0 : ret;
+}
+
+static inline void select_chip(struct tegra_nand_info *info, int chipnr)
+{
+	BUG_ON(chipnr != -1 && chipnr >= info->plat->max_chips);
+	info->chip.curr_chip = chipnr;
+}
+
+static void cfg_hwstatus_mon(struct tegra_nand_info *info)
+{
+	uint32_t val;
+
+	val = (HWSTATUS_RDSTATUS_MASK(1) |
+	       HWSTATUS_RDSTATUS_EXP_VAL(0) |
+	       HWSTATUS_RBSY_MASK(NAND_STATUS_READY) |
+	       HWSTATUS_RBSY_EXP_VAL(NAND_STATUS_READY));
+	writel(NAND_CMD_STATUS, HWSTATUS_CMD_REG);
+	writel(val, HWSTATUS_MASK_REG);
+}
+
+/* Tells the NAND controller to initiate the command. */
+static int tegra_nand_go(struct tegra_nand_info *info)
+{
+	BUG_ON(!tegra_nand_is_cmd_done(info));
+
+	INIT_COMPLETION(info->cmd_complete);
+	writel(info->command_reg | COMMAND_GO, COMMAND_REG);
+
+	if (unlikely(tegra_nand_wait_cmd_done(info))) {
+		/* TODO: abort command if needed? */
+		pr_err("%s: Timeout while waiting for command\n", __func__);
+		return -ETIMEDOUT;
+	}
+
+	/* TODO: maybe wait for dma here? */
+	return 0;
+}
+
+static void tegra_nand_prep_readid(struct tegra_nand_info *info)
+{
+	info->command_reg =
+	    (COMMAND_CLE | COMMAND_ALE | COMMAND_PIO | COMMAND_RX |
+	     COMMAND_ALE_BYTE_SIZE(0) | COMMAND_TRANS_SIZE(3) |
+	     (COMMAND_CE(info->chip.curr_chip)));
+	writel(NAND_CMD_READID, CMD_REG1);
+	writel(0, CMD_REG2);
+	writel(0, ADDR_REG1);
+	writel(0, ADDR_REG2);
+	writel(0, CONFIG_REG);
+}
+
+static int
+tegra_nand_cmd_readid(struct tegra_nand_info *info, uint32_t *chip_id)
+{
+	int err;
+
+#ifdef TEGRA_NAND_DEBUG_PEDANTIC
+	BUG_ON(info->chip.curr_chip == -1);
+#endif
+
+	tegra_nand_prep_readid(info);
+	err = tegra_nand_go(info);
+	if (err != 0)
+		return err;
+
+	*chip_id = readl(RESP_REG);
+	return 0;
+}
+
+/* assumes right locks are held */
+static int nand_cmd_get_status(struct tegra_nand_info *info, uint32_t *status)
+{
+	int err;
+
+	info->command_reg = (COMMAND_CLE | COMMAND_PIO | COMMAND_RX |
+			     COMMAND_RBSY_CHK |
+			     (COMMAND_CE(info->chip.curr_chip)));
+	writel(NAND_CMD_STATUS, CMD_REG1);
+	writel(0, CMD_REG2);
+	writel(0, ADDR_REG1);
+	writel(0, ADDR_REG2);
+	writel(CONFIG_COM_BSY, CONFIG_REG);
+
+	err = tegra_nand_go(info);
+	if (err != 0)
+		return err;
+
+	*status = readl(RESP_REG) & 0xff;
+	return 0;
+}
+
+static int tegra_nand_cmd_reset(struct tegra_nand_info *info,
+				uint32_t *chip_id)
+{
+	int err;
+	uint32_t status = 0;
+
+#ifdef TEGRA_NAND_DEBUG_PEDANTIC
+	BUG_ON(info->chip.curr_chip == -1);
+#endif
+
+	info->command_reg = (COMMAND_CLE |
+			     (COMMAND_CE(info->chip.curr_chip)));
+	writel(NAND_CMD_RESET, CMD_REG1);
+	writel(0, CMD_REG2);
+	writel(0, ADDR_REG1);
+	writel(0, ADDR_REG2);
+	writel(0, CONFIG_REG);
+
+	err = tegra_nand_go(info);
+	if (err != 0)
+		return err;
+
+	err = nand_cmd_get_status(info, &status);
+	if (err != 0)
+		return err;
+
+	return 0;
+}
+
+/* must be called with lock held */
+static int check_block_isbad(struct mtd_info *mtd, loff_t offs)
+{
+	struct tegra_nand_info *info = MTD_TO_INFO(mtd);
+	uint32_t block = offs >> info->chip.block_shift;
+	int chipnr;
+	uint32_t page;
+	uint32_t column;
+	int ret = 0;
+	int i;
+
+	if (info->bb_bitmap[BIT_WORD(block)] & BIT_MASK(block))
+		return 0;
+
+	offs &= ~(mtd->erasesize - 1);
+
+	if (info->is_data_bus_width_16)
+		writel(CONFIG_COM_BSY | CONFIG_BUS_WIDTH, CONFIG_REG);
+	else
+		writel(CONFIG_COM_BSY, CONFIG_REG);
+
+	split_addr(info, offs, &chipnr, &page, &column);
+	select_chip(info, chipnr);
+
+	column = mtd->writesize & 0xffff; /* force to be the offset of OOB */
+
+	/* check fist two pages of the block */
+	if (info->is_data_bus_width_16)
+		column = column >> 1;
+	for (i = 0; i < 2; ++i) {
+		info->command_reg =
+		    COMMAND_CE(info->chip.curr_chip) | COMMAND_CLE |
+		    COMMAND_ALE | COMMAND_ALE_BYTE_SIZE(4) | COMMAND_RX |
+		    COMMAND_PIO | COMMAND_TRANS_SIZE(1) | COMMAND_A_VALID |
+		    COMMAND_RBSY_CHK | COMMAND_SEC_CMD;
+		writel(NAND_CMD_READ0, CMD_REG1);
+		writel(NAND_CMD_READSTART, CMD_REG2);
+
+		writel(column | ((page & 0xffff) << 16), ADDR_REG1);
+		writel((page >> 16) & 0xff, ADDR_REG2);
+
+		/* ... poison me ... */
+		writel(0xaa55aa55, RESP_REG);
+		ret = tegra_nand_go(info);
+		if (ret != 0) {
+			pr_info("baaaaaad\n");
+			goto out;
+		}
+
+		if ((readl(RESP_REG) & 0xffff) != 0xffff) {
+			ret = 1;
+			goto out;
+		}
+
+		/* Note: The assumption here is that we cannot cross chip
+		 * boundary since the we are only looking at the first 2 pages in
+		 * a block, i.e. erasesize > writesize ALWAYS */
+		page++;
+	}
+
+out:
+	/* update the bitmap if the block is good */
+	if (ret == 0)
+		set_bit(block, info->bb_bitmap);
+	return ret;
+}
+
+static int tegra_nand_block_isbad(struct mtd_info *mtd, loff_t offs)
+{
+	struct tegra_nand_info *info = MTD_TO_INFO(mtd);
+	int ret;
+
+	if (offs >= mtd->size)
+		return -EINVAL;
+
+	mutex_lock(&info->lock);
+	ret = check_block_isbad(mtd, offs);
+	mutex_unlock(&info->lock);
+
+#if 0
+	if (ret > 0)
+		pr_info("block @ 0x%llx is bad.\n", offs);
+	else if (ret < 0)
+		pr_err("error checking block @ 0x%llx for badness.\n", offs);
+#endif
+
+	return ret;
+}
+
+static int tegra_nand_block_markbad(struct mtd_info *mtd, loff_t offs)
+{
+	struct tegra_nand_info *info = MTD_TO_INFO(mtd);
+	uint32_t block = offs >> info->chip.block_shift;
+	int chipnr;
+	uint32_t page;
+	uint32_t column;
+	int ret = 0;
+	int i;
+
+	if (offs >= mtd->size)
+		return -EINVAL;
+
+	pr_info("tegra_nand: setting block %d bad\n", block);
+
+	mutex_lock(&info->lock);
+	offs &= ~(mtd->erasesize - 1);
+
+	/* mark the block bad in our bitmap */
+	clear_bit(block, info->bb_bitmap);
+	mtd->ecc_stats.badblocks++;
+
+	if (info->is_data_bus_width_16)
+		writel(CONFIG_COM_BSY | CONFIG_BUS_WIDTH, CONFIG_REG);
+	else
+		writel(CONFIG_COM_BSY, CONFIG_REG);
+
+	split_addr(info, offs, &chipnr, &page, &column);
+	select_chip(info, chipnr);
+
+	column = mtd->writesize & 0xffff; /* force to be the offset of OOB */
+	if (info->is_data_bus_width_16)
+		column = column >> 1;
+	/* write to fist two pages in the block */
+	for (i = 0; i < 2; ++i) {
+		info->command_reg =
+		    COMMAND_CE(info->chip.curr_chip) | COMMAND_CLE |
+		    COMMAND_ALE | COMMAND_ALE_BYTE_SIZE(4) | COMMAND_TX |
+		    COMMAND_PIO | COMMAND_TRANS_SIZE(1) | COMMAND_A_VALID |
+		    COMMAND_RBSY_CHK | COMMAND_AFT_DAT | COMMAND_SEC_CMD;
+		writel(NAND_CMD_SEQIN, CMD_REG1);
+		writel(NAND_CMD_PAGEPROG, CMD_REG2);
+
+		writel(column | ((page & 0xffff) << 16), ADDR_REG1);
+		writel((page >> 16) & 0xff, ADDR_REG2);
+
+		writel(0x0, RESP_REG);
+		ret = tegra_nand_go(info);
+		if (ret != 0)
+			goto out;
+
+		/* TODO: check if the program op worked? */
+		page++;
+	}
+
+out:
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+static int tegra_nand_erase(struct mtd_info *mtd, struct erase_info *instr)
+{
+	struct tegra_nand_info *info = MTD_TO_INFO(mtd);
+	uint32_t num_blocks;
+	uint32_t offs;
+	int chipnr;
+	uint32_t page;
+	uint32_t column;
+	uint32_t status = 0;
+
+	TEGRA_DBG("tegra_nand_erase: addr=0x%08llx len=%lld\n", instr->addr,
+		  instr->len);
+
+	if ((instr->addr + instr->len) > mtd->size) {
+		pr_err("tegra_nand_erase: Can't erase past end of device\n");
+		instr->state = MTD_ERASE_FAILED;
+		return -EINVAL;
+	}
+
+	if (instr->addr & (mtd->erasesize - 1)) {
+		pr_err("tegra_nand_erase: addr=0x%08llx not block-aligned\n",
+		       instr->addr);
+		instr->state = MTD_ERASE_FAILED;
+		return -EINVAL;
+	}
+
+	if (instr->len & (mtd->erasesize - 1)) {
+		pr_err("tegra_nand_erase: len=%lld not block-aligned\n",
+		       instr->len);
+		instr->state = MTD_ERASE_FAILED;
+		return -EINVAL;
+	}
+
+	instr->fail_addr = 0xffffffff;
+
+	mutex_lock(&info->lock);
+
+	instr->state = MTD_ERASING;
+
+	offs = instr->addr;
+	num_blocks = instr->len >> info->chip.block_shift;
+
+	select_chip(info, -1);
+
+	while (num_blocks--) {
+		split_addr(info, offs, &chipnr, &page, &column);
+		if (chipnr != info->chip.curr_chip)
+			select_chip(info, chipnr);
+		TEGRA_DBG("tegra_nand_erase: addr=0x%08x, page=0x%08x\n", offs,
+			  page);
+
+		if (check_block_isbad(mtd, offs)) {
+			pr_info("%s: skipping bad block @ 0x%08x\n", __func__,
+				offs);
+			goto next_block;
+		}
+
+		info->command_reg =
+		    COMMAND_CE(info->chip.curr_chip) | COMMAND_CLE |
+		    COMMAND_ALE | COMMAND_ALE_BYTE_SIZE(2) |
+		    COMMAND_RBSY_CHK | COMMAND_SEC_CMD;
+		writel(NAND_CMD_ERASE1, CMD_REG1);
+		writel(NAND_CMD_ERASE2, CMD_REG2);
+
+		writel(page & 0xffffff, ADDR_REG1);
+		writel(0, ADDR_REG2);
+		writel(CONFIG_COM_BSY, CONFIG_REG);
+
+		if (tegra_nand_go(info) != 0) {
+			instr->fail_addr = offs;
+			goto out_err;
+		}
+
+		/* TODO: do we want a timeout here? */
+		if ((nand_cmd_get_status(info, &status) != 0) ||
+		    (status & NAND_STATUS_FAIL) ||
+		    ((status & NAND_STATUS_READY) != NAND_STATUS_READY)) {
+			instr->fail_addr = offs;
+			pr_info("%s: erase failed @ 0x%08x (stat=0x%08x)\n",
+				__func__, offs, status);
+			goto out_err;
+		}
+next_block:
+		offs += mtd->erasesize;
+	}
+
+	instr->state = MTD_ERASE_DONE;
+	mutex_unlock(&info->lock);
+	mtd_erase_callback(instr);
+	return 0;
+
+out_err:
+	instr->state = MTD_ERASE_FAILED;
+	mutex_unlock(&info->lock);
+	return -EIO;
+}
+
+static inline void dump_mtd_oob_ops(struct mtd_oob_ops *ops)
+{
+	pr_info("%s: oob_ops: mode=%s len=0x%x ooblen=0x%x "
+		"ooboffs=0x%x dat=0x%p oob=0x%p\n", __func__,
+		(ops->mode == MTD_OOB_AUTO ? "MTD_OOB_AUTO" :
+		 (ops->mode ==
+		  MTD_OOB_PLACE ? "MTD_OOB_PLACE" : "MTD_OOB_RAW")), ops->len,
+		ops->ooblen, ops->ooboffs, ops->datbuf, ops->oobbuf);
+}
+
+static int
+tegra_nand_read(struct mtd_info *mtd, loff_t from, size_t len,
+		size_t *retlen, uint8_t *buf)
+{
+	struct mtd_oob_ops ops;
+	int ret;
+
+	pr_debug("%s: read: from=0x%llx len=0x%x\n", __func__, from, len);
+	ops.mode = MTD_OOB_AUTO;
+	ops.len = len;
+	ops.datbuf = buf;
+	ops.oobbuf = NULL;
+	ret = mtd->read_oob(mtd, from, &ops);
+	*retlen = ops.retlen;
+	return ret;
+}
+
+static void
+correct_ecc_errors_on_blank_page(struct tegra_nand_info *info, u8 *datbuf,
+				 u8 *oobbuf, unsigned int a_len,
+				 unsigned int b_len)
+{
+	int i;
+	int all_ff = 1;
+	unsigned long flags;
+
+	spin_lock_irqsave(&info->ecc_lock, flags);
+	if (info->num_ecc_errs) {
+		if (datbuf) {
+			for (i = 0; i < a_len; i++)
+				if (datbuf[i] != 0xFF)
+					all_ff = 0;
+		}
+		if (oobbuf) {
+			for (i = 0; i < b_len; i++)
+				if (oobbuf[i] != 0xFF)
+					all_ff = 0;
+		}
+		if (all_ff)
+			info->num_ecc_errs = 0;
+	}
+	spin_unlock_irqrestore(&info->ecc_lock, flags);
+}
+
+static void update_ecc_counts(struct tegra_nand_info *info, int check_oob)
+{
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&info->ecc_lock, flags);
+	for (i = 0; i < info->num_ecc_errs; ++i) {
+		/* correctable */
+		info->mtd.ecc_stats.corrected +=
+		    DEC_STATUS_ERR_CNT(info->ecc_errs[i]);
+
+		/* uncorrectable */
+		if (info->ecc_errs[i] & DEC_STATUS_ECC_FAIL_A)
+			info->mtd.ecc_stats.failed++;
+		if (check_oob && (info->ecc_errs[i] & DEC_STATUS_ECC_FAIL_B))
+			info->mtd.ecc_stats.failed++;
+	}
+	info->num_ecc_errs = 0;
+	spin_unlock_irqrestore(&info->ecc_lock, flags);
+}
+
+static inline void clear_regs(struct tegra_nand_info *info)
+{
+	info->command_reg = 0;
+	info->config_reg = 0;
+	info->dmactrl_reg = 0;
+}
+
+static void
+prep_transfer_dma(struct tegra_nand_info *info, int rx, int do_ecc,
+		  uint32_t page, uint32_t column, dma_addr_t data_dma,
+		  uint32_t data_len, dma_addr_t oob_dma, uint32_t oob_len)
+{
+	uint32_t tag_sz = oob_len;
+
+	uint32_t page_size_sel = (info->mtd.writesize >> 11) + 2;
+#if 0
+	pr_info("%s: rx=%d ecc=%d  page=%d col=%d data_dma=0x%x "
+		"data_len=0x%08x oob_dma=0x%x ooblen=%d\n", __func__,
+		rx, do_ecc, page, column, data_dma, data_len, oob_dma, oob_len);
+#endif
+
+	info->command_reg =
+	    COMMAND_CE(info->chip.curr_chip) | COMMAND_CLE | COMMAND_ALE |
+	    COMMAND_ALE_BYTE_SIZE(4) | COMMAND_SEC_CMD | COMMAND_RBSY_CHK |
+	    COMMAND_TRANS_SIZE(8);
+
+	info->config_reg = (CONFIG_PIPELINE_EN | CONFIG_EDO_MODE |
+			    CONFIG_COM_BSY);
+	if (info->is_data_bus_width_16)
+		info->config_reg |= CONFIG_BUS_WIDTH;
+	info->dmactrl_reg = (DMA_CTRL_DMA_GO |
+			     DMA_CTRL_DMA_PERF_EN | DMA_CTRL_IE_DMA_DONE |
+			     DMA_CTRL_IS_DMA_DONE | DMA_CTRL_BURST_SIZE(4));
+
+	if (rx) {
+		if (do_ecc)
+			info->config_reg |= CONFIG_HW_ERR_CORRECTION;
+		info->command_reg |= COMMAND_RX;
+		info->dmactrl_reg |= DMA_CTRL_REUSE_BUFFER;
+		writel(NAND_CMD_READ0, CMD_REG1);
+		writel(NAND_CMD_READSTART, CMD_REG2);
+	} else {
+		info->command_reg |= (COMMAND_TX | COMMAND_AFT_DAT);
+		info->dmactrl_reg |= DMA_CTRL_DIR; /* DMA_RD == TX */
+		writel(NAND_CMD_SEQIN, CMD_REG1);
+		writel(NAND_CMD_PAGEPROG, CMD_REG2);
+	}
+
+	if (data_len) {
+		if (do_ecc)
+			info->config_reg |= CONFIG_HW_ECC | CONFIG_ECC_SEL;
+		info->config_reg |=
+		    CONFIG_PAGE_SIZE_SEL(page_size_sel) | CONFIG_TVALUE(0) |
+		    CONFIG_SKIP_SPARE | CONFIG_SKIP_SPARE_SEL(0);
+		info->command_reg |= COMMAND_A_VALID;
+		info->dmactrl_reg |= DMA_CTRL_DMA_EN_A;
+		writel(DMA_CFG_BLOCK_SIZE(data_len - 1), DMA_CFG_A_REG);
+		writel(data_dma, DATA_BLOCK_PTR_REG);
+	} else {
+		column = info->mtd.writesize;
+		if (do_ecc)
+			column += info->mtd.ecclayout->oobfree[0].offset;
+		writel(0, DMA_CFG_A_REG);
+		writel(0, DATA_BLOCK_PTR_REG);
+	}
+
+	if (oob_len) {
+		if (do_ecc) {
+			oob_len = info->mtd.oobavail;
+			tag_sz = info->mtd.oobavail;
+			tag_sz += 4;	/* size of tag ecc */
+			if (rx)
+				oob_len += 4; /* size of tag ecc */
+			info->config_reg |= CONFIG_ECC_EN_TAG;
+		}
+		if (data_len && rx)
+			oob_len += 4; /* num of skipped bytes */
+
+		info->command_reg |= COMMAND_B_VALID;
+		info->config_reg |= CONFIG_TAG_BYTE_SIZE(tag_sz - 1);
+		info->dmactrl_reg |= DMA_CTRL_DMA_EN_B;
+		writel(DMA_CFG_BLOCK_SIZE(oob_len - 1), DMA_CFG_B_REG);
+		writel(oob_dma, TAG_PTR_REG);
+	} else {
+		writel(0, DMA_CFG_B_REG);
+		writel(0, TAG_PTR_REG);
+	}
+	/* For x16 bit we needs to divide the column number by 2 */
+	if (info->is_data_bus_width_16)
+		column = column >> 1;
+	writel((column & 0xffff) | ((page & 0xffff) << 16), ADDR_REG1);
+	writel((page >> 16) & 0xff, ADDR_REG2);
+}
+
+static dma_addr_t
+tegra_nand_dma_map(struct device *dev, void *addr, size_t size,
+		   enum dma_data_direction dir)
+{
+	struct page *page;
+	unsigned long offset = (unsigned long)addr & ~PAGE_MASK;
+	if (virt_addr_valid(addr))
+		page = virt_to_page(addr);
+	else {
+		if (WARN_ON(size + offset > PAGE_SIZE))
+			return ~0;
+		page = vmalloc_to_page(addr);
+	}
+	return dma_map_page(dev, page, offset, size, dir);
+}
+
+static ssize_t show_vendor_id(struct device *dev, struct device_attribute *attr,
+			      char *buf)
+{
+	struct tegra_nand_info *info = dev_get_drvdata(dev);
+	return sprintf(buf, "0x%x\n", info->vendor_id);
+}
+
+static DEVICE_ATTR(vendor_id, S_IRUSR, show_vendor_id, NULL);
+
+static ssize_t show_device_id(struct device *dev, struct device_attribute *attr,
+			      char *buf)
+{
+	struct tegra_nand_info *info = dev_get_drvdata(dev);
+	return sprintf(buf, "0x%x\n", info->device_id);
+}
+
+static DEVICE_ATTR(device_id, S_IRUSR, show_device_id, NULL);
+
+static ssize_t show_flash_size(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	struct tegra_nand_info *info = dev_get_drvdata(dev);
+	struct mtd_info *mtd = &info->mtd;
+	return sprintf(buf, "%llu bytes\n", mtd->size);
+}
+
+static DEVICE_ATTR(flash_size, S_IRUSR, show_flash_size, NULL);
+
+static ssize_t show_num_bad_blocks(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	struct tegra_nand_info *info = dev_get_drvdata(dev);
+	return sprintf(buf, "%d\n", info->num_bad_blocks);
+}
+
+static DEVICE_ATTR(num_bad_blocks, S_IRUSR, show_num_bad_blocks, NULL);
+
+static ssize_t show_bb_bitmap(struct device *dev, struct device_attribute *attr,
+			      char *buf)
+{
+	struct tegra_nand_info *info = dev_get_drvdata(dev);
+	struct mtd_info *mtd = &info->mtd;
+	int num_blocks = mtd->size >> info->chip.block_shift, i, ret = 0, size =
+	    0;
+
+	for (i = 0; i < num_blocks / (8 * sizeof(unsigned long)); i++) {
+		size = sprintf(buf, "0x%lx\n", info->bb_bitmap[i]);
+		ret += size;
+		buf += size;
+	}
+	return ret;
+}
+
+static DEVICE_ATTR(bb_bitmap, S_IRUSR, show_bb_bitmap, NULL);
+
+/*
+ * Independent of Mode, we read main data and the OOB data from the oobfree areas as
+ * specified nand_ecclayout
+ * This function also checks buffer pool partial_unaligned_rw_buffer
+ * if the address is already present and is not 'unused' then it will use
+ * data in buffer else it will go for DMA.
+ */
+static int
+do_read_oob(struct mtd_info *mtd, loff_t from, struct mtd_oob_ops *ops)
+{
+	struct tegra_nand_info *info = MTD_TO_INFO(mtd);
+	struct mtd_ecc_stats old_ecc_stats;
+	int chipnr;
+	uint32_t page;
+	uint32_t column;
+	uint8_t *datbuf = ops->datbuf;
+	uint8_t *oobbuf = ops->oobbuf;
+	uint32_t ooblen = oobbuf ? ops->ooblen : 0;
+	uint32_t oobsz;
+	uint32_t page_count;
+	int err;
+	int unaligned = from & info->chip.column_mask;
+	uint32_t len = datbuf ? ((ops->len) + unaligned) : 0;
+	int do_ecc = 1;
+	dma_addr_t datbuf_dma_addr = 0;
+
+#if 0
+	dump_mtd_oob_ops(ops);
+#endif
+	ops->retlen = 0;
+	ops->oobretlen = 0;
+	from = from - unaligned;
+
+	/* Don't care about the MTD_OOB_  value field always use oobavail and ecc. */
+	oobsz = mtd->oobavail;
+	if (unlikely(ops->oobbuf && ops->ooblen > oobsz)) {
+		pr_err("%s: can't read OOB from multiple pages (%d > %d)\n",
+		       __func__, ops->ooblen, oobsz);
+		return -EINVAL;
+	} else if (ops->oobbuf && !len) {
+		page_count = 1;
+	} else {
+		page_count =
+		    (uint32_t) ((len + mtd->writesize - 1) / mtd->writesize);
+	}
+
+	mutex_lock(&info->lock);
+
+	memcpy(&old_ecc_stats, &mtd->ecc_stats, sizeof(old_ecc_stats));
+
+	if (do_ecc) {
+		enable_ints(info, IER_ECC_ERR);
+		writel(info->ecc_addr, ECC_PTR_REG);
+	} else
+		disable_ints(info, IER_ECC_ERR);
+
+	split_addr(info, from, &chipnr, &page, &column);
+	select_chip(info, chipnr);
+
+	/* reset it to point back to beginning of page */
+	from -= column;
+
+	while (page_count--) {
+		int a_len = min(mtd->writesize - column, len);
+		int b_len = min(oobsz, ooblen);
+		int temp_len = 0;
+		char *temp_buf = NULL;
+		/* Take care when read is of less than page size.
+		 * Otherwise there will be kernel Panic due to DMA timeout */
+		if (((a_len < mtd->writesize) && len) || unaligned) {
+			temp_len = a_len;
+			a_len = mtd->writesize;
+			temp_buf = datbuf;
+			datbuf = info->partial_unaligned_rw_buffer;
+		}
+#if 0
+		pr_info("%s: chip:=%d page=%d col=%d\n", __func__, chipnr,
+			page, column);
+#endif
+
+		clear_regs(info);
+		if (datbuf)
+			datbuf_dma_addr =
+			    tegra_nand_dma_map(info->dev, datbuf, a_len,
+					       DMA_FROM_DEVICE);
+
+		prep_transfer_dma(info, 1, do_ecc, page, column,
+				  datbuf_dma_addr, a_len, info->oob_dma_addr,
+				  b_len);
+		writel(info->config_reg, CONFIG_REG);
+		writel(info->dmactrl_reg, DMA_MST_CTRL_REG);
+
+		INIT_COMPLETION(info->dma_complete);
+		err = tegra_nand_go(info);
+		if (err != 0)
+			goto out_err;
+
+		if (!wait_for_completion_timeout(&info->dma_complete, TIMEOUT)) {
+			pr_err("%s: dma completion timeout\n", __func__);
+			dump_nand_regs();
+			err = -ETIMEDOUT;
+			goto out_err;
+		}
+
+		/*pr_info("tegra_read_oob: DMA complete\n"); */
+
+		/* if we are here, transfer is done */
+		if (datbuf)
+			dma_unmap_page(info->dev, datbuf_dma_addr, a_len,
+				       DMA_FROM_DEVICE);
+
+		if (oobbuf) {
+			uint32_t ofs = datbuf && oobbuf ? 4 : 0; /* skipped bytes */
+			memcpy(oobbuf, info->oob_dma_buf + ofs, b_len);
+		}
+
+		correct_ecc_errors_on_blank_page(info, datbuf, oobbuf, a_len,
+						 b_len);
+		/* Take care when read is of less than page size */
+		if (temp_len) {
+			memcpy(temp_buf, datbuf + unaligned,
+			       temp_len - unaligned);
+			a_len = temp_len;
+			datbuf = temp_buf;
+		}
+		if (datbuf) {
+			len -= a_len;
+			datbuf += a_len - unaligned;
+			ops->retlen += a_len - unaligned;
+		}
+
+		if (oobbuf) {
+			ooblen -= b_len;
+			oobbuf += b_len;
+			ops->oobretlen += b_len;
+		}
+
+		unaligned = 0;
+		update_ecc_counts(info, oobbuf != NULL);
+
+		if (!page_count)
+			break;
+
+		from += mtd->writesize;
+		column = 0;
+
+		split_addr(info, from, &chipnr, &page, &column);
+		if (chipnr != info->chip.curr_chip)
+			select_chip(info, chipnr);
+	}
+
+	disable_ints(info, IER_ECC_ERR);
+
+	if (mtd->ecc_stats.failed != old_ecc_stats.failed)
+		err = -EBADMSG;
+	else if (mtd->ecc_stats.corrected != old_ecc_stats.corrected)
+		err = -EUCLEAN;
+	else
+		err = 0;
+
+	mutex_unlock(&info->lock);
+	return err;
+
+out_err:
+	ops->retlen = 0;
+	ops->oobretlen = 0;
+
+	disable_ints(info, IER_ECC_ERR);
+	mutex_unlock(&info->lock);
+	return err;
+}
+
+/* just does some parameter checking and calls do_read_oob */
+static int
+tegra_nand_read_oob(struct mtd_info *mtd, loff_t from, struct mtd_oob_ops *ops)
+{
+	if (ops->datbuf && unlikely((from + ops->len) > mtd->size)) {
+		pr_err("%s: Can't read past end of device.\n", __func__);
+		return -EINVAL;
+	}
+
+	if (unlikely(ops->oobbuf && !ops->ooblen)) {
+		pr_err("%s: Reading 0 bytes from OOB is meaningless\n",
+		       __func__);
+		return -EINVAL;
+	}
+
+	if (unlikely(ops->mode != MTD_OOB_AUTO)) {
+		if (ops->oobbuf && ops->datbuf) {
+			pr_err("%s: can't read OOB + Data in non-AUTO mode.\n",
+			       __func__);
+			return -EINVAL;
+		}
+		if ((ops->mode == MTD_OOB_RAW) && !ops->datbuf) {
+			pr_err("%s: Raw mode only supports reading data area.\n",
+			     __func__);
+			return -EINVAL;
+		}
+	}
+
+	return do_read_oob(mtd, from, ops);
+}
+
+static int
+tegra_nand_write(struct mtd_info *mtd, loff_t to, size_t len,
+		 size_t *retlen, const uint8_t *buf)
+{
+	struct mtd_oob_ops ops;
+	int ret;
+
+	pr_debug("%s: write: to=0x%llx len=0x%x\n", __func__, to, len);
+	ops.mode = MTD_OOB_AUTO;
+	ops.len = len;
+	ops.datbuf = (uint8_t *) buf;
+	ops.oobbuf = NULL;
+	ret = mtd->write_oob(mtd, to, &ops);
+	*retlen = ops.retlen;
+	return ret;
+}
+
+static int
+do_write_oob(struct mtd_info *mtd, loff_t to, struct mtd_oob_ops *ops)
+{
+	struct tegra_nand_info *info = MTD_TO_INFO(mtd);
+	int chipnr;
+	uint32_t page;
+	uint32_t column;
+	uint8_t *datbuf = ops->datbuf;
+	uint8_t *oobbuf = ops->oobbuf;
+	uint32_t len = datbuf ? ops->len : 0;
+	uint32_t ooblen = oobbuf ? ops->ooblen : 0;
+	uint32_t oobsz;
+	uint32_t page_count;
+	int err;
+	int do_ecc = 1;
+	dma_addr_t datbuf_dma_addr = 0;
+
+#if 0
+	dump_mtd_oob_ops(ops);
+#endif
+
+	ops->retlen = 0;
+	ops->oobretlen = 0;
+
+	if (!ops->len)
+		return 0;
+
+	oobsz = mtd->oobavail;
+
+	if (unlikely(ops->oobbuf && ops->ooblen > oobsz)) {
+		pr_err("%s: can't write OOB to multiple pages (%d > %d)\n",
+		       __func__, ops->ooblen, oobsz);
+		return -EINVAL;
+	} else if (ops->oobbuf && !len) {
+		page_count = 1;
+	} else
+		page_count =
+		    max((uint32_t) (ops->len / mtd->writesize), (uint32_t) 1);
+
+	mutex_lock(&info->lock);
+
+	split_addr(info, to, &chipnr, &page, &column);
+	select_chip(info, chipnr);
+
+	while (page_count--) {
+		int a_len = min(mtd->writesize, len);
+		int b_len = min(oobsz, ooblen);
+		int temp_len = 0;
+		char *temp_buf = NULL;
+		/* Take care when write is of less than page size. Otherwise
+		 * there will be kernel panic due to dma timeout */
+		if ((a_len < mtd->writesize) && len) {
+			temp_len = a_len;
+			a_len = mtd->writesize;
+			temp_buf = datbuf;
+			datbuf = info->partial_unaligned_rw_buffer;
+			memset(datbuf, 0xff, a_len);
+			memcpy(datbuf, temp_buf, temp_len);
+		}
+
+		if (datbuf)
+			datbuf_dma_addr =
+			    tegra_nand_dma_map(info->dev, datbuf, a_len,
+					       DMA_TO_DEVICE);
+		if (oobbuf)
+			memcpy(info->oob_dma_buf, oobbuf, b_len);
+
+		clear_regs(info);
+		prep_transfer_dma(info, 0, do_ecc, page, column,
+				  datbuf_dma_addr, a_len, info->oob_dma_addr,
+				  b_len);
+
+		writel(info->config_reg, CONFIG_REG);
+		writel(info->dmactrl_reg, DMA_MST_CTRL_REG);
+
+		INIT_COMPLETION(info->dma_complete);
+		err = tegra_nand_go(info);
+		if (err != 0)
+			goto out_err;
+
+		if (!wait_for_completion_timeout(&info->dma_complete, TIMEOUT)) {
+			pr_err("%s: dma completion timeout\n", __func__);
+			dump_nand_regs();
+			goto out_err;
+		}
+		if (temp_len) {
+			a_len = temp_len;
+			datbuf = temp_buf;
+		}
+
+		if (datbuf) {
+			dma_unmap_page(info->dev, datbuf_dma_addr, a_len,
+				       DMA_TO_DEVICE);
+			len -= a_len;
+			datbuf += a_len;
+			ops->retlen += a_len;
+		}
+		if (oobbuf) {
+			ooblen -= b_len;
+			oobbuf += b_len;
+			ops->oobretlen += b_len;
+		}
+
+		if (!page_count)
+			break;
+
+		to += mtd->writesize;
+		column = 0;
+
+		split_addr(info, to, &chipnr, &page, &column);
+		if (chipnr != info->chip.curr_chip)
+			select_chip(info, chipnr);
+	}
+
+	mutex_unlock(&info->lock);
+	return err;
+
+out_err:
+	ops->retlen = 0;
+	ops->oobretlen = 0;
+
+	mutex_unlock(&info->lock);
+	return err;
+}
+
+static int
+tegra_nand_write_oob(struct mtd_info *mtd, loff_t to, struct mtd_oob_ops *ops)
+{
+	struct tegra_nand_info *info = MTD_TO_INFO(mtd);
+
+	if (unlikely(to & info->chip.column_mask)) {
+		pr_err("%s: Unaligned write (to 0x%llx) not supported\n",
+		       __func__, to);
+		return -EINVAL;
+	}
+
+	if (unlikely(ops->oobbuf && !ops->ooblen)) {
+		pr_err("%s: Writing 0 bytes to OOB is meaningless\n", __func__);
+		return -EINVAL;
+	}
+
+	return do_write_oob(mtd, to, ops);
+}
+
+static int tegra_nand_suspend(struct mtd_info *mtd)
+{
+	return 0;
+}
+
+static void
+set_chip_timing(struct tegra_nand_info *info, uint32_t vendor_id,
+		uint32_t dev_id, uint32_t fourth_id_field)
+{
+	struct tegra_nand_chip_parms *chip_parms = NULL;
+	uint32_t tmp;
+	int i = 0;
+	unsigned long nand_clk_freq_khz = clk_get_rate(info->clk) / 1000;
+	for (i = 0; i < info->plat->nr_chip_parms; i++)
+		if (info->plat->chip_parms[i].vendor_id == vendor_id &&
+		    info->plat->chip_parms[i].device_id == dev_id &&
+		    info->plat->chip_parms[i].read_id_fourth_byte ==
+		    fourth_id_field)
+			chip_parms = &info->plat->chip_parms[i];
+
+	if (!chip_parms) {
+		pr_warn("WARNING:tegra_nand: timing for vendor-id: "
+			"%x device-id: %x fourth-id-field: %x not found. Using Bootloader timing",
+			vendor_id, dev_id, fourth_id_field);
+		return;
+	}
+	/* TODO: Handle the change of frequency if DVFS is enabled */
+#define CNT(t)	(((((t) * nand_clk_freq_khz) + 1000000 - 1) / 1000000) - 1)
+	tmp = (TIMING_TRP_RESP(CNT(chip_parms->timing.trp_resp)) |
+	       TIMING_TWB(CNT(chip_parms->timing.twb)) |
+	       TIMING_TCR_TAR_TRR(CNT(chip_parms->timing.tcr_tar_trr)) |
+	       TIMING_TWHR(CNT(chip_parms->timing.twhr)) |
+	       TIMING_TCS(CNT(chip_parms->timing.tcs)) |
+	       TIMING_TWH(CNT(chip_parms->timing.twh)) |
+	       TIMING_TWP(CNT(chip_parms->timing.twp)) |
+	       TIMING_TRH(CNT(chip_parms->timing.trh)) |
+	       TIMING_TRP(CNT(chip_parms->timing.trp)));
+	writel(tmp, TIMING_REG);
+	writel(TIMING2_TADL(CNT(chip_parms->timing.tadl)), TIMING2_REG);
+#undef CNT
+}
+
+static void tegra_nand_resume(struct mtd_info *mtd)
+{
+	struct tegra_nand_info *info = MTD_TO_INFO(mtd);
+
+	cfg_hwstatus_mon(info);
+
+	/* clear all pending interrupts */
+	writel(readl(ISR_REG), ISR_REG);
+
+	/* clear dma interrupt */
+	writel(DMA_CTRL_IS_DMA_DONE, DMA_MST_CTRL_REG);
+
+	/* enable interrupts */
+	disable_ints(info, 0xffffffff);
+	enable_ints(info,
+		    IER_ERR_TRIG_VAL(4) | IER_UND | IER_OVR | IER_CMD_DONE |
+		    IER_ECC_ERR | IER_GIE);
+
+	writel(0, CONFIG_REG);
+
+	set_chip_timing(info, info->vendor_id,
+				info->device_id, info->dev_parms);
+
+	return;
+}
+
+static int scan_bad_blocks(struct tegra_nand_info *info)
+{
+	struct mtd_info *mtd = &info->mtd;
+	int num_blocks = mtd->size >> info->chip.block_shift;
+	uint32_t block;
+	int is_bad = 0;
+	info->num_bad_blocks = 0;
+
+	for (block = 0; block < num_blocks; ++block) {
+		/* make sure the bit is cleared, meaning it's bad/unknown before
+		 * we check. */
+		clear_bit(block, info->bb_bitmap);
+		is_bad = mtd->block_isbad(mtd, block << info->chip.block_shift);
+
+		if (is_bad == 0)
+			set_bit(block, info->bb_bitmap);
+		else if (is_bad > 0) {
+			info->num_bad_blocks++;
+			pr_debug("block 0x%08x is bad.\n", block);
+		} else {
+			pr_err("Fatal error (%d) while scanning for "
+			       "bad blocks\n", is_bad);
+			return is_bad;
+		}
+	}
+	return 0;
+}
+
+/* Scans for nand flash devices, identifies them, and fills in the
+ * device info. */
+static int tegra_nand_scan(struct mtd_info *mtd, int maxchips)
+{
+	struct tegra_nand_info *info = MTD_TO_INFO(mtd);
+	struct nand_flash_dev *dev_info;
+	struct nand_manufacturers *vendor_info;
+	uint32_t tmp;
+	uint32_t dev_id;
+	uint32_t vendor_id;
+	uint32_t dev_parms;
+	uint32_t mlc_parms;
+	int cnt;
+	int err = 0;
+
+	writel(SCAN_TIMING_VAL, TIMING_REG);
+	writel(SCAN_TIMING2_VAL, TIMING2_REG);
+	writel(0, CONFIG_REG);
+
+	select_chip(info, 0);
+
+	err = tegra_nand_cmd_reset(info, &tmp);
+	if (err != 0)
+		goto out_error;
+
+	err = tegra_nand_cmd_readid(info, &tmp);
+	if (err != 0)
+		goto out_error;
+
+	vendor_id = tmp & 0xff;
+	dev_id = (tmp >> 8) & 0xff;
+	mlc_parms = (tmp >> 16) & 0xff;
+	dev_parms = (tmp >> 24) & 0xff;
+
+	dev_info = find_nand_flash_device(dev_id);
+	if (dev_info == NULL) {
+		pr_err("%s: unknown flash device id (0x%02x) found.\n",
+		       __func__, dev_id);
+		err = -ENODEV;
+		goto out_error;
+	}
+
+	vendor_info = find_nand_flash_vendor(vendor_id);
+	if (vendor_info == NULL) {
+		pr_err("%s: unknown flash vendor id (0x%02x) found.\n",
+		       __func__, vendor_id);
+		err = -ENODEV;
+		goto out_error;
+	}
+
+	/* loop through and see if we can find more devices */
+	for (cnt = 1; cnt < info->plat->max_chips; ++cnt) {
+		select_chip(info, cnt);
+		/* TODO: figure out what to do about errors here */
+		err = tegra_nand_cmd_readid(info, &tmp);
+		if (err != 0)
+			goto out_error;
+		if ((dev_id != ((tmp >> 8) & 0xff)) ||
+		    (vendor_id != (tmp & 0xff)))
+			break;
+	}
+
+	pr_info("%s: %d NAND chip(s) found (vend=0x%02x, dev=0x%02x) (%s %s)\n",
+		DRIVER_NAME, cnt, vendor_id, dev_id, vendor_info->name,
+		dev_info->name);
+	info->vendor_id = vendor_id;
+	info->device_id = dev_id;
+	info->dev_parms = dev_parms;
+	info->chip.num_chips = cnt;
+	info->chip.chipsize = dev_info->chipsize << 20;
+	mtd->size = info->chip.num_chips * info->chip.chipsize;
+
+	/* format of 4th id byte returned by READ ID
+	 *   bit     7 = rsvd
+	 *   bit     6 = bus width. 1 == 16bit, 0 == 8bit
+	 *   bits  5:4 = data block size. 64kb * (2^val)
+	 *   bit     3 = rsvd
+	 *   bit     2 = spare area size / 512 bytes. 0 == 8bytes, 1 == 16bytes
+	 *   bits  1:0 = page size. 1kb * (2^val)
+	 */
+
+	/* page_size */
+	tmp = dev_parms & 0x3;
+	mtd->writesize = mtd->writebufsize = 1024 << tmp;
+	info->chip.column_mask = mtd->writesize - 1;
+
+	if (mtd->writesize > 4096) {
+		pr_err("%s: Large page devices with pagesize > 4kb are NOT "
+		       "supported\n", __func__);
+		goto out_error;
+	} else if (mtd->writesize < 2048) {
+		pr_err("%s: Small page devices are NOT supported\n", __func__);
+		goto out_error;
+	}
+
+	/* spare area, must be at least 64 bytes */
+	tmp = (dev_parms >> 2) & 0x1;
+	tmp = (8 << tmp) * (mtd->writesize / 512);
+	if (tmp < 64) {
+		pr_err("%s: Spare area (%d bytes) too small\n", __func__, tmp);
+		goto out_error;
+	}
+	mtd->oobsize = tmp;
+
+	/* data block size (erase size) (w/o spare) */
+	tmp = (dev_parms >> 4) & 0x3;
+	/* work around wrong block size identified for our device
+	   Note: ONFI would really be the way to go but has not been supported
+	   in Linux prior to version 2.6.37. */
+	if ((vendor_id == 0x2C) && (dev_id == 0x38))
+		tmp += 1;
+	mtd->erasesize = (64 * 1024) << tmp;
+	info->chip.block_shift = ffs(mtd->erasesize) - 1;
+	/* bus width of the nand chip 8/16 */
+	tmp = (dev_parms >> 6) & 0x1;
+	info->is_data_bus_width_16 = tmp;
+	/* used to select the appropriate chip/page in case multiple devices
+	 * are connected */
+	info->chip.chip_shift = ffs(info->chip.chipsize) - 1;
+	info->chip.page_shift = ffs(mtd->writesize) - 1;
+	info->chip.page_mask =
+	    (info->chip.chipsize >> info->chip.page_shift) - 1;
+
+	/* now fill in the rest of the mtd fields */
+	if (mtd->oobsize == 64)
+		mtd->ecclayout = &tegra_nand_oob_64;
+	else
+		mtd->ecclayout = &tegra_nand_oob_128;
+
+	mtd->oobavail = mtd->ecclayout->oobavail;
+	mtd->type = MTD_NANDFLASH;
+	mtd->flags = MTD_CAP_NANDFLASH;
+
+	mtd->erase = tegra_nand_erase;
+	mtd->lock = NULL;
+	mtd->point = NULL;
+	mtd->unpoint = NULL;
+	mtd->read = tegra_nand_read;
+	mtd->write = tegra_nand_write;
+	mtd->read_oob = tegra_nand_read_oob;
+	mtd->write_oob = tegra_nand_write_oob;
+
+	mtd->resume = tegra_nand_resume;
+	mtd->suspend = tegra_nand_suspend;
+	mtd->block_isbad = tegra_nand_block_isbad;
+	mtd->block_markbad = tegra_nand_block_markbad;
+
+	set_chip_timing(info, vendor_id, dev_id, dev_parms);
+
+	return 0;
+
+out_error:
+	pr_err("%s: NAND device scan aborted due to error(s).\n", __func__);
+	return err;
+}
+
+static int __devinit tegra_nand_probe(struct platform_device *pdev)
+{
+	struct tegra_nand_platform *plat = pdev->dev.platform_data;
+	struct tegra_nand_info *info = NULL;
+	struct tegra_nand_chip *chip = NULL;
+	struct mtd_info *mtd = NULL;
+	int err = 0;
+	uint64_t num_erase_blocks;
+
+	pr_debug("%s: probing (%p)\n", __func__, pdev);
+
+	if (!plat) {
+		pr_err("%s: no platform device info\n", __func__);
+		return -EINVAL;
+	} else if (!plat->chip_parms) {
+		pr_err("%s: no platform nand parms\n", __func__);
+		return -EINVAL;
+	}
+
+	info = kzalloc(sizeof(struct tegra_nand_info), GFP_KERNEL);
+	if (!info) {
+		pr_err("%s: no memory for flash info\n", __func__);
+		return -ENOMEM;
+	}
+
+	info->dev = &pdev->dev;
+	info->plat = plat;
+
+	platform_set_drvdata(pdev, info);
+
+	init_completion(&info->cmd_complete);
+	init_completion(&info->dma_complete);
+
+	mutex_init(&info->lock);
+	spin_lock_init(&info->ecc_lock);
+
+	chip = &info->chip;
+	chip->priv = &info->mtd;
+	chip->curr_chip = -1;
+
+	mtd = &info->mtd;
+	mtd->name = dev_name(&pdev->dev);
+	mtd->priv = &info->chip;
+	mtd->owner = THIS_MODULE;
+
+	/* HACK: allocate a dma buffer to hold 1 page oob data */
+	info->oob_dma_buf = dma_alloc_coherent(NULL, 128,
+					       &info->oob_dma_addr, GFP_KERNEL);
+	if (!info->oob_dma_buf) {
+		err = -ENOMEM;
+		goto out_free_info;
+	}
+
+	/* this will store the ecc error vector info */
+	info->ecc_buf = dma_alloc_coherent(NULL, ECC_BUF_SZ, &info->ecc_addr,
+					   GFP_KERNEL);
+	if (!info->ecc_buf) {
+		err = -ENOMEM;
+		goto out_free_dma_buf;
+	}
+
+	/* grab the irq */
+	if (!(pdev->resource[0].flags & IORESOURCE_IRQ)) {
+		pr_err("NAND IRQ resource not defined\n");
+		err = -EINVAL;
+		goto out_free_ecc_buf;
+	}
+
+	err = request_irq(pdev->resource[0].start, tegra_nand_irq,
+			  IRQF_SHARED, DRIVER_NAME, info);
+	if (err) {
+		pr_err("Unable to request IRQ %d (%d)\n",
+		       pdev->resource[0].start, err);
+		goto out_free_ecc_buf;
+	}
+
+	/* TODO: configure pinmux here?? */
+	info->clk = clk_get(&pdev->dev, NULL);
+
+	if (IS_ERR(info->clk)) {
+		err = PTR_ERR(info->clk);
+		goto out_free_ecc_buf;
+	}
+	err = clk_enable(info->clk);
+	if (err != 0)
+		goto out_free_ecc_buf;
+
+	if (plat->wp_gpio) {
+		gpio_request(plat->wp_gpio, "nand_wp");
+		gpio_direction_output(plat->wp_gpio, 1);
+	}
+
+	cfg_hwstatus_mon(info);
+
+	/* clear all pending interrupts */
+	writel(readl(ISR_REG), ISR_REG);
+
+	/* clear dma interrupt */
+	writel(DMA_CTRL_IS_DMA_DONE, DMA_MST_CTRL_REG);
+
+	/* enable interrupts */
+	disable_ints(info, 0xffffffff);
+	enable_ints(info,
+		    IER_ERR_TRIG_VAL(4) | IER_UND | IER_OVR | IER_CMD_DONE |
+		    IER_ECC_ERR | IER_GIE);
+
+	if (tegra_nand_scan(mtd, plat->max_chips)) {
+		err = -ENXIO;
+		goto out_dis_irq;
+	}
+	pr_info("%s: NVIDIA Tegra NAND controller @ base=0x%08x irq=%d.\n",
+		DRIVER_NAME, TEGRA_NAND_PHYS, pdev->resource[0].start);
+
+	/* allocate memory to hold the ecc error info */
+	info->max_ecc_errs = MAX_DMA_SZ / mtd->writesize;
+	info->ecc_errs = kmalloc(info->max_ecc_errs * sizeof(uint32_t),
+				 GFP_KERNEL);
+	if (!info->ecc_errs) {
+		err = -ENOMEM;
+		goto out_dis_irq;
+	}
+
+	/* alloc the bad block bitmap */
+	num_erase_blocks = mtd->size;
+	do_div(num_erase_blocks, mtd->erasesize);
+	info->bb_bitmap = kzalloc(BITS_TO_LONGS(num_erase_blocks) *
+				  sizeof(unsigned long), GFP_KERNEL);
+	if (!info->bb_bitmap) {
+		err = -ENOMEM;
+		goto out_free_ecc;
+	}
+
+	err = scan_bad_blocks(info);
+	if (err != 0)
+		goto out_free_bbbmap;
+
+#if 0
+	dump_nand_regs();
+#endif
+
+	err = parse_mtd_partitions(mtd, part_probes, &info->parts, 0);
+	if (err > 0) {
+		err = mtd_device_register(mtd, info->parts, err);
+	} else if (err <= 0 && plat->parts) {
+		err = mtd_device_register(mtd, plat->parts, plat->nr_parts);
+	} else
+		err = mtd_device_register(mtd, NULL, 0);
+	if (err != 0)
+		goto out_free_bbbmap;
+
+	dev_set_drvdata(&pdev->dev, info);
+
+	info->partial_unaligned_rw_buffer = kzalloc(mtd->writesize, GFP_KERNEL);
+	if (!info->partial_unaligned_rw_buffer) {
+		err = -ENOMEM;
+		goto out_free_bbbmap;
+	}
+
+	err = device_create_file(&pdev->dev, &dev_attr_device_id);
+	if (err != 0)
+		goto out_free_rw_buffer;
+
+	err = device_create_file(&pdev->dev, &dev_attr_vendor_id);
+	if (err != 0)
+		goto err_nand_sysfs_vendorid_failed;
+
+	err = device_create_file(&pdev->dev, &dev_attr_flash_size);
+	if (err != 0)
+		goto err_nand_sysfs_flash_size_failed;
+
+	err = device_create_file(&pdev->dev, &dev_attr_num_bad_blocks);
+	if (err != 0)
+		goto err_nand_sysfs_num_bad_blocks_failed;
+
+	err = device_create_file(&pdev->dev, &dev_attr_bb_bitmap);
+	if (err != 0)
+		goto err_nand_sysfs_bb_bitmap_failed;
+
+	pr_debug("%s: probe done.\n", __func__);
+	return 0;
+
+err_nand_sysfs_bb_bitmap_failed:
+	device_remove_file(&pdev->dev, &dev_attr_num_bad_blocks);
+
+err_nand_sysfs_num_bad_blocks_failed:
+	device_remove_file(&pdev->dev, &dev_attr_flash_size);
+
+err_nand_sysfs_flash_size_failed:
+	device_remove_file(&pdev->dev, &dev_attr_vendor_id);
+
+err_nand_sysfs_vendorid_failed:
+	device_remove_file(&pdev->dev, &dev_attr_device_id);
+
+out_free_rw_buffer:
+	kfree(info->partial_unaligned_rw_buffer);
+
+out_free_bbbmap:
+	kfree(info->bb_bitmap);
+
+out_free_ecc:
+	kfree(info->ecc_errs);
+
+out_dis_irq:
+	disable_ints(info, 0xffffffff);
+	free_irq(pdev->resource[0].start, info);
+
+out_free_ecc_buf:
+	dma_free_coherent(NULL, ECC_BUF_SZ, info->ecc_buf, info->ecc_addr);
+
+out_free_dma_buf:
+	dma_free_coherent(NULL, 128, info->oob_dma_buf, info->oob_dma_addr);
+
+out_free_info:
+	platform_set_drvdata(pdev, NULL);
+	kfree(info);
+
+	return err;
+}
+
+static int __devexit tegra_nand_remove(struct platform_device *pdev)
+{
+	struct tegra_nand_info *info = dev_get_drvdata(&pdev->dev);
+
+	dev_set_drvdata(&pdev->dev, NULL);
+
+	if (info) {
+		free_irq(pdev->resource[0].start, info);
+		kfree(info->bb_bitmap);
+		kfree(info->ecc_errs);
+		kfree(info->partial_unaligned_rw_buffer);
+
+		device_remove_file(&pdev->dev, &dev_attr_device_id);
+		device_remove_file(&pdev->dev, &dev_attr_vendor_id);
+		device_remove_file(&pdev->dev, &dev_attr_flash_size);
+		device_remove_file(&pdev->dev, &dev_attr_num_bad_blocks);
+		device_remove_file(&pdev->dev, &dev_attr_bb_bitmap);
+
+		dma_free_coherent(NULL, ECC_BUF_SZ, info->ecc_buf,
+				  info->ecc_addr);
+		dma_free_coherent(NULL, info->mtd.writesize + info->mtd.oobsize,
+				  info->oob_dma_buf, info->oob_dma_addr);
+		kfree(info);
+	}
+
+	return 0;
+}
+
+static struct platform_driver tegra_nand_driver = {
+	.probe = tegra_nand_probe,
+	.remove = __devexit_p(tegra_nand_remove),
+	.suspend = NULL,
+	.resume = NULL,
+	.driver = {
+		   .name = "tegra_nand",
+		   .owner = THIS_MODULE,
+		   },
+};
+
+static int __init tegra_nand_init(void)
+{
+	return platform_driver_register(&tegra_nand_driver);
+}
+
+static void __exit tegra_nand_exit(void)
+{
+	platform_driver_unregister(&tegra_nand_driver);
+}
+
+module_init(tegra_nand_init);
+module_exit(tegra_nand_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/mtd/devices/tegra_nand.h b/drivers/mtd/devices/tegra_nand.h
new file mode 100644
index 000000000000..339d6cc7330c
--- /dev/null
+++ b/drivers/mtd/devices/tegra_nand.h
@@ -0,0 +1,148 @@
+/*
+ * drivers/mtd/devices/tegra_nand.h
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Dima Zavin <dima@android.com>
+ *         Colin Cross <ccross@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 __MTD_DEV_TEGRA_NAND_H
+#define __MTD_DEV_TEGRA_NAND_H
+
+#include <mach/io.h>
+
+#define __BITMASK0(len)				((1 << (len)) - 1)
+#define __BITMASK(start, len)			(__BITMASK0(len) << (start))
+#define REG_BIT(bit)				(1 << (bit))
+#define REG_FIELD(val, start, len)		(((val) & __BITMASK0(len)) << (start))
+#define REG_FIELD_MASK(start, len)		(~(__BITMASK((start), (len))))
+#define REG_GET_FIELD(val, start, len)		(((val) >> (start)) & __BITMASK0(len))
+
+/* tegra nand registers... */
+#define TEGRA_NAND_PHYS				0x70008000
+#define TEGRA_NAND_BASE				IO_TO_VIRT(TEGRA_NAND_PHYS)
+#define COMMAND_REG				(TEGRA_NAND_BASE + 0x00)
+#define STATUS_REG				(TEGRA_NAND_BASE + 0x04)
+#define ISR_REG					(TEGRA_NAND_BASE + 0x08)
+#define IER_REG					(TEGRA_NAND_BASE + 0x0c)
+#define CONFIG_REG				(TEGRA_NAND_BASE + 0x10)
+#define TIMING_REG				(TEGRA_NAND_BASE + 0x14)
+#define RESP_REG				(TEGRA_NAND_BASE + 0x18)
+#define TIMING2_REG				(TEGRA_NAND_BASE + 0x1c)
+#define CMD_REG1				(TEGRA_NAND_BASE + 0x20)
+#define CMD_REG2				(TEGRA_NAND_BASE + 0x24)
+#define ADDR_REG1				(TEGRA_NAND_BASE + 0x28)
+#define ADDR_REG2				(TEGRA_NAND_BASE + 0x2c)
+#define DMA_MST_CTRL_REG			(TEGRA_NAND_BASE + 0x30)
+#define DMA_CFG_A_REG				(TEGRA_NAND_BASE + 0x34)
+#define DMA_CFG_B_REG				(TEGRA_NAND_BASE + 0x38)
+#define FIFO_CTRL_REG				(TEGRA_NAND_BASE + 0x3c)
+#define DATA_BLOCK_PTR_REG			(TEGRA_NAND_BASE + 0x40)
+#define TAG_PTR_REG				(TEGRA_NAND_BASE + 0x44)
+#define ECC_PTR_REG				(TEGRA_NAND_BASE + 0x48)
+#define DEC_STATUS_REG				(TEGRA_NAND_BASE + 0x4c)
+#define HWSTATUS_CMD_REG			(TEGRA_NAND_BASE + 0x50)
+#define HWSTATUS_MASK_REG			(TEGRA_NAND_BASE + 0x54)
+#define LL_CONFIG_REG				(TEGRA_NAND_BASE + 0x58)
+#define LL_PTR_REG				(TEGRA_NAND_BASE + 0x5c)
+#define LL_STATUS_REG				(TEGRA_NAND_BASE + 0x60)
+
+/* nand_command bits */
+#define COMMAND_GO				REG_BIT(31)
+#define COMMAND_CLE				REG_BIT(30)
+#define COMMAND_ALE				REG_BIT(29)
+#define COMMAND_PIO				REG_BIT(28)
+#define COMMAND_TX				REG_BIT(27)
+#define COMMAND_RX				REG_BIT(26)
+#define COMMAND_SEC_CMD				REG_BIT(25)
+#define COMMAND_AFT_DAT				REG_BIT(24)
+#define COMMAND_TRANS_SIZE(val)			REG_FIELD((val), 20, 4)
+#define COMMAND_A_VALID				REG_BIT(19)
+#define COMMAND_B_VALID				REG_BIT(18)
+#define COMMAND_RD_STATUS_CHK			REG_BIT(17)
+#define COMMAND_RBSY_CHK			REG_BIT(16)
+#define COMMAND_CE(val)				REG_BIT(8 + ((val) & 0x7))
+#define COMMAND_CLE_BYTE_SIZE(val)		REG_FIELD((val), 4, 2)
+#define COMMAND_ALE_BYTE_SIZE(val)		REG_FIELD((val), 0, 4)
+
+/* nand isr bits */
+#define ISR_UND					REG_BIT(7)
+#define ISR_OVR					REG_BIT(6)
+#define ISR_CMD_DONE				REG_BIT(5)
+#define ISR_ECC_ERR				REG_BIT(4)
+
+/* nand ier bits */
+#define IER_ERR_TRIG_VAL(val)			REG_FIELD((val), 16, 4)
+#define IER_UND					REG_BIT(7)
+#define IER_OVR					REG_BIT(6)
+#define IER_CMD_DONE				REG_BIT(5)
+#define IER_ECC_ERR				REG_BIT(4)
+#define IER_GIE					REG_BIT(0)
+
+/* nand config bits */
+#define CONFIG_HW_ECC				REG_BIT(31)
+#define CONFIG_ECC_SEL				REG_BIT(30)
+#define CONFIG_HW_ERR_CORRECTION		REG_BIT(29)
+#define CONFIG_PIPELINE_EN			REG_BIT(28)
+#define CONFIG_ECC_EN_TAG			REG_BIT(27)
+#define CONFIG_TVALUE(val)			REG_FIELD((val), 24, 2)
+#define CONFIG_SKIP_SPARE			REG_BIT(23)
+#define CONFIG_COM_BSY				REG_BIT(22)
+#define CONFIG_BUS_WIDTH			REG_BIT(21)
+#define CONFIG_EDO_MODE				REG_BIT(19)
+#define CONFIG_PAGE_SIZE_SEL(val)		REG_FIELD((val), 16, 3)
+#define CONFIG_SKIP_SPARE_SEL(val)		REG_FIELD((val), 14, 2)
+#define CONFIG_TAG_BYTE_SIZE(val)		REG_FIELD((val), 0, 8)
+
+/* nand timing bits */
+#define TIMING_TRP_RESP(val)			REG_FIELD((val), 28, 4)
+#define TIMING_TWB(val)				REG_FIELD((val), 24, 4)
+#define TIMING_TCR_TAR_TRR(val)			REG_FIELD((val), 20, 4)
+#define TIMING_TWHR(val)			REG_FIELD((val), 16, 4)
+#define TIMING_TCS(val)				REG_FIELD((val), 14, 2)
+#define TIMING_TWH(val)				REG_FIELD((val), 12, 2)
+#define TIMING_TWP(val)				REG_FIELD((val), 8, 4)
+#define TIMING_TRH(val)				REG_FIELD((val), 4, 2)
+#define TIMING_TRP(val)				REG_FIELD((val), 0, 4)
+
+/* nand timing2 bits */
+#define TIMING2_TADL(val)			REG_FIELD((val), 0, 4)
+
+/* nand dma_mst_ctrl bits */
+#define DMA_CTRL_DMA_GO				REG_BIT(31)
+#define DMA_CTRL_DIR				REG_BIT(30)
+#define DMA_CTRL_DMA_PERF_EN			REG_BIT(29)
+#define DMA_CTRL_IE_DMA_DONE			REG_BIT(28)
+#define DMA_CTRL_REUSE_BUFFER			REG_BIT(27)
+#define DMA_CTRL_BURST_SIZE(val)		REG_FIELD((val), 24, 3)
+#define DMA_CTRL_IS_DMA_DONE			REG_BIT(20)
+#define DMA_CTRL_DMA_EN_A			REG_BIT(2)
+#define DMA_CTRL_DMA_EN_B			REG_BIT(1)
+
+/* nand dma_cfg_a/cfg_b bits */
+#define DMA_CFG_BLOCK_SIZE(val)			REG_FIELD((val), 0, 16)
+
+/* nand dec_status bits */
+#define DEC_STATUS_ERR_PAGE_NUM(val)		REG_GET_FIELD((val), 24, 8)
+#define DEC_STATUS_ERR_CNT(val)			REG_GET_FIELD((val), 16, 8)
+#define DEC_STATUS_ECC_FAIL_A			REG_BIT(1)
+#define DEC_STATUS_ECC_FAIL_B			REG_BIT(0)
+
+/* nand hwstatus_mask bits */
+#define HWSTATUS_RDSTATUS_MASK(val)		REG_FIELD((val), 24, 8)
+#define HWSTATUS_RDSTATUS_EXP_VAL(val)		REG_FIELD((val), 16, 8)
+#define HWSTATUS_RBSY_MASK(val)			REG_FIELD((val), 8, 8)
+#define HWSTATUS_RBSY_EXP_VAL(val)		REG_FIELD((val), 0, 8)
+
+#endif
+
diff --git a/drivers/mtd/maps/Kconfig b/drivers/mtd/maps/Kconfig
index c0c328c5b133..299e67c039ff 100644
--- a/drivers/mtd/maps/Kconfig
+++ b/drivers/mtd/maps/Kconfig
@@ -561,4 +561,11 @@ config MTD_LATCH_ADDR
 
           If compiled as a module, it will be called latch-addr-flash.
 
+config MTD_NOR_TEGRA
+	bool "NOR Flash mapping driver for NVIDIA Tegra based boards"
+	depends on MTD_COMPLEX_MAPPINGS && ARCH_TEGRA
+	help
+	  This enables access routines for the flash chips on the NVIDIA Tegra
+	  based boards.
+
 endmenu
diff --git a/drivers/mtd/maps/Makefile b/drivers/mtd/maps/Makefile
index cb48b11affff..dbc68e0e87cb 100644
--- a/drivers/mtd/maps/Makefile
+++ b/drivers/mtd/maps/Makefile
@@ -60,3 +60,5 @@ obj-$(CONFIG_MTD_GPIO_ADDR)	+= gpio-addr-flash.o
 obj-$(CONFIG_MTD_BCM963XX)	+= bcm963xx-flash.o
 obj-$(CONFIG_MTD_LATCH_ADDR)	+= latch-addr-flash.o
 obj-$(CONFIG_MTD_LANTIQ)	+= lantiq-flash.o
+CFLAGS_tegra_nor.o		= -Werror
+obj-$(CONFIG_MTD_NOR_TEGRA)	+= tegra_nor.o
diff --git a/drivers/mtd/maps/tegra_nor.c b/drivers/mtd/maps/tegra_nor.c
new file mode 100644
index 000000000000..41adb1e40382
--- /dev/null
+++ b/drivers/mtd/maps/tegra_nor.c
@@ -0,0 +1,550 @@
+/*
+ * Copyright (C) 2009-2012, NVIDIA Corporation.  All rights reserved.
+ *
+ * Author:
+ *	Raghavendra VK <rvk@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ * drivers/mtd/maps/tegra_nor.c
+ *
+ * MTD mapping driver for the internal SNOR controller in Tegra SoCs
+ *
+ */
+
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/mutex.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/map.h>
+#include <linux/mtd/partitions.h>
+#include <linux/dma-mapping.h>
+#include <linux/proc_fs.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/clk.h>
+#include <linux/platform_data/tegra_nor.h>
+#include <asm/cacheflush.h>
+
+#define __BITMASK0(len)			(BIT(len) - 1)
+#define REG_FIELD(val, start, len)	(((val) & __BITMASK0(len)) << (start))
+#define REG_GET_FIELD(val, start, len)	(((val) >> (start)) & __BITMASK0(len))
+
+/* tegra gmi registers... */
+#define TEGRA_SNOR_CONFIG_REG			0x00
+#define TEGRA_SNOR_NOR_ADDR_PTR_REG		0x08
+#define TEGRA_SNOR_AHB_ADDR_PTR_REG		0x0C
+#define TEGRA_SNOR_TIMING0_REG			0x10
+#define TEGRA_SNOR_TIMING1_REG			0x14
+#define TEGRA_SNOR_DMA_CFG_REG			0x20
+
+/* config register */
+#define TEGRA_SNOR_CONFIG_GO			BIT(31)
+#define TEGRA_SNOR_CONFIG_WORDWIDE		BIT(30)
+#define TEGRA_SNOR_CONFIG_DEVICE_TYPE		BIT(29)
+#define TEGRA_SNOR_CONFIG_MUX_MODE		BIT(28)
+#define TEGRA_SNOR_CONFIG_BURST_LEN(val)	REG_FIELD((val), 26, 2)
+#define TEGRA_SNOR_CONFIG_RDY_ACTIVE		BIT(24)
+#define TEGRA_SNOR_CONFIG_RDY_POLARITY		BIT(23)
+#define TEGRA_SNOR_CONFIG_ADV_POLARITY		BIT(22)
+#define TEGRA_SNOR_CONFIG_OE_WE_POLARITY	BIT(21)
+#define TEGRA_SNOR_CONFIG_CS_POLARITY		BIT(20)
+#define TEGRA_SNOR_CONFIG_NOR_DPD		BIT(19)
+#define TEGRA_SNOR_CONFIG_WP			BIT(15)
+#define TEGRA_SNOR_CONFIG_PAGE_SZ(val)		REG_FIELD((val), 8, 2)
+#define TEGRA_SNOR_CONFIG_MST_ENB		BIT(7)
+#define TEGRA_SNOR_CONFIG_SNOR_CS(val)		REG_FIELD((val), 4, 2)
+#define TEGRA_SNOR_CONFIG_CE_LAST		REG_FIELD(3)
+#define TEGRA_SNOR_CONFIG_CE_FIRST		REG_FIELD(2)
+#define TEGRA_SNOR_CONFIG_DEVICE_MODE(val)	REG_FIELD((val), 0, 2)
+
+/* dma config register */
+#define TEGRA_SNOR_DMA_CFG_GO			BIT(31)
+#define TEGRA_SNOR_DMA_CFG_BSY			BIT(30)
+#define TEGRA_SNOR_DMA_CFG_DIR			BIT(29)
+#define TEGRA_SNOR_DMA_CFG_INT_ENB		BIT(28)
+#define TEGRA_SNOR_DMA_CFG_INT_STA		BIT(27)
+#define TEGRA_SNOR_DMA_CFG_BRST_SZ(val)		REG_FIELD((val), 24, 3)
+#define TEGRA_SNOR_DMA_CFG_WRD_CNT(val)		REG_FIELD((val), 2, 14)
+
+/* timing 0 register */
+#define TEGRA_SNOR_TIMING0_PG_RDY(val)		REG_FIELD((val), 28, 4)
+#define TEGRA_SNOR_TIMING0_PG_SEQ(val)		REG_FIELD((val), 20, 4)
+#define TEGRA_SNOR_TIMING0_MUX(val)		REG_FIELD((val), 12, 4)
+#define TEGRA_SNOR_TIMING0_HOLD(val)            REG_FIELD((val), 8, 4)
+#define TEGRA_SNOR_TIMING0_ADV(val)		REG_FIELD((val), 4, 4)
+#define TEGRA_SNOR_TIMING0_CE(val)		REG_FIELD((val), 0, 4)
+
+/* timing 1 register */
+#define TEGRA_SNOR_TIMING1_WE(val)		REG_FIELD((val), 16, 8)
+#define TEGRA_SNOR_TIMING1_OE(val)		REG_FIELD((val), 8, 8)
+#define TEGRA_SNOR_TIMING1_WAIT(val)		REG_FIELD((val), 0, 8)
+
+/* SNOR DMA supports 2^14 AHB (32-bit words)
+ * Maximum data in one transfer = 2^16 bytes
+ */
+#define TEGRA_SNOR_DMA_LIMIT           0x10000
+#define TEGRA_SNOR_DMA_LIMIT_WORDS     (TEGRA_SNOR_DMA_LIMIT >> 2)
+
+/* Even if BW is 1 MB/s, maximum time to
+ * transfer SNOR_DMA_LIMIT bytes is 66 ms
+ */
+#define TEGRA_SNOR_DMA_TIMEOUT_MS       67
+
+struct tegra_nor_info {
+	struct tegra_nor_platform_data *plat;
+	struct device *dev;
+	struct clk *clk;
+	struct mtd_partition *parts;
+	struct mtd_info *mtd;
+	struct map_info map;
+	struct completion dma_complete;
+	void __iomem *base;
+	void *dma_virt_buffer;
+	dma_addr_t dma_phys_buffer;
+	u32 init_config;
+	u32 timing0_default, timing1_default;
+	u32 timing0_read, timing1_read;
+};
+
+static inline unsigned long snor_tegra_readl(struct tegra_nor_info *tnor,
+					     unsigned long reg)
+{
+	return readl(tnor->base + reg);
+}
+
+static inline void snor_tegra_writel(struct tegra_nor_info *tnor,
+				     unsigned long val, unsigned long reg)
+{
+	writel(val, tnor->base + reg);
+}
+
+#define DRV_NAME "tegra-nor"
+
+static const char *part_probes[] = { "cmdlinepart", NULL };
+
+static int wait_for_dma_completion(struct tegra_nor_info *info)
+{
+	unsigned long dma_timeout;
+	int ret;
+
+	dma_timeout = msecs_to_jiffies(TEGRA_SNOR_DMA_TIMEOUT_MS);
+	ret = wait_for_completion_timeout(&info->dma_complete, dma_timeout);
+	return ret ? 0 : -ETIMEDOUT;
+}
+
+static void tegra_flash_dma(struct map_info *map,
+			    void *to, unsigned long from, ssize_t len)
+{
+	u32 snor_config, dma_config = 0;
+	int dma_transfer_count = 0, word32_count = 0;
+	u32 nor_address, current_transfer = 0;
+	u32 copy_to = (u32)to;
+	struct tegra_nor_info *c =
+	    container_of(map, struct tegra_nor_info, map);
+	struct tegra_nor_chip_parms *chip_parm = &c->plat->chip_parms;
+	unsigned int bytes_remaining = len;
+
+	snor_config = c->init_config;
+	snor_tegra_writel(c, c->timing0_read, TEGRA_SNOR_TIMING0_REG);
+	snor_tegra_writel(c, c->timing1_read, TEGRA_SNOR_TIMING1_REG);
+
+	if (len > 32) {
+		word32_count = len >> 2;
+		bytes_remaining = len & 0x00000003;
+		/*
+		 * The parameters can be setup in any order since we write to
+		 * controller register only after all parameters are set.
+		 */
+		/* SNOR CONFIGURATION SETUP */
+		switch(chip_parm->ReadMode)
+		{
+			case NorReadMode_Async:
+				snor_config |= TEGRA_SNOR_CONFIG_DEVICE_MODE(0);
+				break;
+
+			case NorReadMode_Page:
+				switch(chip_parm->PageLength)
+				{
+					case NorPageLength_Unsupported :
+						snor_config |= TEGRA_SNOR_CONFIG_DEVICE_MODE(0);
+						break;
+
+					case NorPageLength_4Word :
+						snor_config |= TEGRA_SNOR_CONFIG_DEVICE_MODE(1);
+						snor_config |= TEGRA_SNOR_CONFIG_PAGE_SZ(1);
+						break;
+
+					case NorPageLength_8Word :
+						snor_config |= TEGRA_SNOR_CONFIG_DEVICE_MODE(1);
+						snor_config |= TEGRA_SNOR_CONFIG_PAGE_SZ(2);
+						break;
+				}
+				break;
+
+			case NorReadMode_Burst:
+				snor_config |= TEGRA_SNOR_CONFIG_DEVICE_MODE(2);
+				switch(chip_parm->BurstLength)
+				{
+					case NorBurstLength_CntBurst :
+						snor_config |= TEGRA_SNOR_CONFIG_BURST_LEN(0);
+						break;
+					case NorBurstLength_8Word :
+						snor_config |= TEGRA_SNOR_CONFIG_BURST_LEN(1);
+						break;
+
+					case NorBurstLength_16Word :
+						snor_config |= TEGRA_SNOR_CONFIG_BURST_LEN(2);
+						break;
+
+					case NorBurstLength_32Word :
+						snor_config |= TEGRA_SNOR_CONFIG_BURST_LEN(3);
+						break;
+				}
+				break;
+		}
+		snor_config |= TEGRA_SNOR_CONFIG_MST_ENB;
+		/* SNOR DMA CONFIGURATION SETUP */
+		/* NOR -> AHB */
+		dma_config &= ~TEGRA_SNOR_DMA_CFG_DIR;
+		/* One word burst */
+		dma_config |= TEGRA_SNOR_DMA_CFG_BRST_SZ(4);
+
+		for (nor_address = (unsigned int)(map->phys + from);
+		     word32_count > 0;
+		     word32_count -= current_transfer,
+		     dma_transfer_count += current_transfer,
+		     nor_address += (current_transfer * 4),
+		     copy_to += (current_transfer * 4)) {
+
+			current_transfer =
+			    (word32_count > TEGRA_SNOR_DMA_LIMIT_WORDS)
+			    ? (TEGRA_SNOR_DMA_LIMIT_WORDS) : word32_count;
+			/* Start NOR operation */
+			snor_config |= TEGRA_SNOR_CONFIG_GO;
+			dma_config |= TEGRA_SNOR_DMA_CFG_GO;
+			/* Enable interrupt before every transaction since the
+			 * interrupt handler disables it */
+			dma_config |= TEGRA_SNOR_DMA_CFG_INT_ENB;
+			/* Num of AHB (32-bit) words to transferred minus 1 */
+			dma_config |=
+			    TEGRA_SNOR_DMA_CFG_WRD_CNT(current_transfer - 1);
+			snor_tegra_writel(c, c->dma_phys_buffer,
+					  TEGRA_SNOR_AHB_ADDR_PTR_REG);
+			snor_tegra_writel(c, nor_address,
+					  TEGRA_SNOR_NOR_ADDR_PTR_REG);
+			snor_tegra_writel(c, snor_config,
+					  TEGRA_SNOR_CONFIG_REG);
+			snor_tegra_writel(c, dma_config,
+					  TEGRA_SNOR_DMA_CFG_REG);
+			if (wait_for_dma_completion(c)) {
+				dev_err(c->dev, "timout waiting for DMA\n");
+				/* Transfer the remaining words by memcpy */
+				bytes_remaining += (word32_count << 2);
+				break;
+			}
+			dma_sync_single_for_cpu(c->dev, c->dma_phys_buffer,
+				(current_transfer << 2), DMA_FROM_DEVICE);
+			memcpy((char *)(copy_to), (char *)(c->dma_virt_buffer),
+				(current_transfer << 2));
+
+		}
+	}
+	/* Put the controller back into slave mode. */
+	snor_config = snor_tegra_readl(c, TEGRA_SNOR_CONFIG_REG);
+	snor_config &= ~TEGRA_SNOR_CONFIG_MST_ENB;
+	snor_config |= TEGRA_SNOR_CONFIG_DEVICE_MODE(0);
+	snor_tegra_writel(c, snor_config, TEGRA_SNOR_CONFIG_REG);
+
+	memcpy_fromio(((char *)to + (dma_transfer_count << 2)),
+		      ((char *)(map->virt + from) + (dma_transfer_count << 2)),
+		      bytes_remaining);
+
+	snor_tegra_writel(c, c->timing0_default, TEGRA_SNOR_TIMING0_REG);
+	snor_tegra_writel(c, c->timing1_default, TEGRA_SNOR_TIMING1_REG);
+}
+
+static irqreturn_t tegra_nor_isr(int flag, void *dev_id)
+{
+	struct tegra_nor_info *info = (struct tegra_nor_info *)dev_id;
+	u32 dma_config = snor_tegra_readl(info, TEGRA_SNOR_DMA_CFG_REG);
+	if (dma_config & TEGRA_SNOR_DMA_CFG_INT_STA) {
+		/* Disable interrupts. WAR for BUG:821560 */
+		dma_config &= ~TEGRA_SNOR_DMA_CFG_INT_ENB;
+		snor_tegra_writel(info, dma_config, TEGRA_SNOR_DMA_CFG_REG);
+		complete(&info->dma_complete);
+	} else {
+		pr_err("%s: Spurious interrupt\n", __func__);
+	}
+	return IRQ_HANDLED;
+}
+
+static int tegra_snor_controller_init(struct tegra_nor_info *info)
+{
+	struct tegra_nor_chip_parms *chip_parm = &info->plat->chip_parms;
+	u32 width = info->plat->flash.width;
+	u32 config = 0;
+
+	config |= TEGRA_SNOR_CONFIG_DEVICE_MODE(0);
+	config |= TEGRA_SNOR_CONFIG_SNOR_CS(0);
+	config &= ~TEGRA_SNOR_CONFIG_DEVICE_TYPE;	/* Select NOR */
+	config |= TEGRA_SNOR_CONFIG_WP;	/* Enable writes */
+	switch (width) {
+	case 2:
+		config &= ~TEGRA_SNOR_CONFIG_WORDWIDE;	/* 16 bit */
+		break;
+	case 4:
+		config |= TEGRA_SNOR_CONFIG_WORDWIDE;	/* 32 bit */
+		break;
+	default:
+		return -EINVAL;
+	}
+	switch (chip_parm->MuxMode)
+	{
+		case NorMuxMode_ADNonMux:
+			config &= ~TEGRA_SNOR_CONFIG_MUX_MODE;
+			break;
+		case NorMuxMode_ADMux:
+			config |= TEGRA_SNOR_CONFIG_MUX_MODE;
+			break;
+		default:
+			return -EINVAL;
+	}
+	switch (chip_parm->ReadyActive)
+	{
+		case NorReadyActive_WithData:
+			config &= ~TEGRA_SNOR_CONFIG_RDY_ACTIVE;
+			break;
+		case NorReadyActive_BeforeData:
+			config |= TEGRA_SNOR_CONFIG_RDY_ACTIVE;
+			break;
+		default:
+			return -EINVAL;
+	}
+	snor_tegra_writel(info, config, TEGRA_SNOR_CONFIG_REG);
+	info->init_config = config;
+
+	info->timing0_default = chip_parm->timing_default.timing0;
+	info->timing0_read = chip_parm->timing_read.timing0;
+	info->timing1_default = chip_parm->timing_default.timing1;
+	info->timing1_read = chip_parm->timing_read.timing1;
+
+	snor_tegra_writel(info, info->timing1_default, TEGRA_SNOR_TIMING1_REG);
+	snor_tegra_writel(info, info->timing0_default, TEGRA_SNOR_TIMING0_REG);
+	return 0;
+}
+
+static int tegra_nor_probe(struct platform_device *pdev)
+{
+	int err = 0;
+	struct tegra_nor_platform_data *plat = pdev->dev.platform_data;
+	struct tegra_nor_info *info = NULL;
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	int irq;
+
+	if (!plat) {
+		pr_err("%s: no platform device info\n", __func__);
+		err = -EINVAL;
+		goto fail;
+	}
+
+	info = devm_kzalloc(dev, sizeof(struct tegra_nor_info),
+			    GFP_KERNEL);
+	if (!info) {
+		err = -ENOMEM;
+		goto fail;
+	}
+
+	/* Get NOR controller & map the same */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(dev, "no mem resource?\n");
+		err = -ENODEV;
+		goto fail;
+	}
+
+	if (!devm_request_mem_region(dev, res->start, resource_size(res),
+				     dev_name(&pdev->dev))) {
+		dev_err(dev, "NOR region already claimed\n");
+		err = -EBUSY;
+		goto fail;
+	}
+
+	info->base = devm_ioremap(dev, res->start, resource_size(res));
+	if (!info->base) {
+		dev_err(dev, "Can't ioremap NOR region\n");
+		err = -ENOMEM;
+		goto fail;
+	}
+
+	/* Get NOR flash aperture & map the same */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!res) {
+		dev_err(dev, "no mem resource?\n");
+		err = -ENODEV;
+		goto fail;
+	}
+
+	if (!devm_request_mem_region(dev, res->start, resource_size(res),
+				     dev_name(dev))) {
+		dev_err(dev, "NOR region already claimed\n");
+		err = -EBUSY;
+		goto fail;
+	}
+
+	info->map.virt = devm_ioremap(dev, res->start,
+				      resource_size(res));
+	if (!info->map.virt) {
+		dev_err(dev, "Can't ioremap NOR region\n");
+		err = -ENOMEM;
+		goto fail;
+	}
+
+	info->plat = plat;
+	info->dev = dev;
+	info->map.bankwidth = plat->flash.width;
+	info->map.name = dev_name(dev);
+	info->map.phys = res->start;
+	info->map.size = resource_size(res);
+
+	info->clk = clk_get(dev, NULL);
+	if (IS_ERR(info->clk)) {
+		err = PTR_ERR(info->clk);
+		goto fail;
+	}
+
+	err = clk_enable(info->clk);
+	if (err != 0)
+		goto out_clk_put;
+
+	simple_map_init(&info->map);
+	info->map.copy_from = tegra_flash_dma;
+
+	/* Intialise the SNOR controller before probe */
+	err = tegra_snor_controller_init(info);
+	if (err) {
+		dev_err(dev, "Error initializing controller\n");
+		goto out_clk_disable;
+	}
+
+	init_completion(&info->dma_complete);
+
+	irq = platform_get_irq(pdev, 0);
+	if (!irq) {
+		dev_err(dev, "no irq resource?\n");
+		err = -ENODEV;
+		goto out_clk_disable;
+	}
+
+	/* Register SNOR DMA completion interrupt */
+	err = devm_request_irq(dev, irq, tegra_nor_isr, IRQF_DISABLED,
+			dev_name(dev), info);
+	if (err) {
+		dev_err(dev, "Failed to request irq %i\n", irq);
+		goto out_clk_disable;
+	}
+	info->dma_virt_buffer = dma_alloc_coherent(dev,
+							TEGRA_SNOR_DMA_LIMIT,
+							&info->dma_phys_buffer,
+							GFP_KERNEL);
+	if (info->dma_virt_buffer == NULL) {
+		dev_err(&pdev->dev, "Could not allocate buffer for DMA");
+		err = -ENOMEM;
+		goto out_clk_disable;
+	}
+
+	info->mtd = do_map_probe(plat->flash.map_name, &info->map);
+	if (!info->mtd) {
+		err = -EIO;
+		goto out_dma_free_coherent;
+	}
+	info->mtd->owner = THIS_MODULE;
+	info->parts = NULL;
+
+	platform_set_drvdata(pdev, info);
+	err = parse_mtd_partitions(info->mtd, part_probes, &info->parts, 0);
+	if (err > 0)
+		err = mtd_device_register(info->mtd, info->parts, err);
+	else if (err <= 0 && plat->flash.parts)
+		err =
+		    mtd_device_register(info->mtd, plat->flash.parts,
+				       plat->flash.nr_parts);
+	else
+		mtd_device_register(info->mtd, NULL, 0);
+
+	return 0;
+
+out_dma_free_coherent:
+	dma_free_coherent(dev, TEGRA_SNOR_DMA_LIMIT,
+				info->dma_virt_buffer, info->dma_phys_buffer);
+out_clk_disable:
+	clk_disable(info->clk);
+out_clk_put:
+	clk_put(info->clk);
+fail:
+	pr_err("Tegra NOR probe failed\n");
+	return err;
+}
+
+static int tegra_nor_remove(struct platform_device *pdev)
+{
+	struct tegra_nor_info *info = platform_get_drvdata(pdev);
+
+	mtd_device_unregister(info->mtd);
+	if (info->parts)
+		kfree(info->parts);
+	dma_free_coherent(&pdev->dev, TEGRA_SNOR_DMA_LIMIT,
+				info->dma_virt_buffer, info->dma_phys_buffer);
+	map_destroy(info->mtd);
+	clk_disable(info->clk);
+	clk_put(info->clk);
+
+	return 0;
+}
+
+static struct platform_driver __refdata tegra_nor_driver = {
+	.probe = tegra_nor_probe,
+	.remove = __devexit_p(tegra_nor_remove),
+	.driver = {
+		   .name = DRV_NAME,
+		   .owner = THIS_MODULE,
+		   },
+};
+
+static int __init tegra_nor_init(void)
+{
+	return platform_driver_register(&tegra_nor_driver);
+}
+
+static void __exit tegra_nor_exit(void)
+{
+	platform_driver_unregister(&tegra_nor_driver);
+}
+
+module_init(tegra_nor_init);
+module_exit(tegra_nor_exit);
+
+MODULE_AUTHOR("Raghavendra VK <rvk@nvidia.com>");
+MODULE_DESCRIPTION("NOR Flash mapping driver for NVIDIA Tegra based boards");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/mtd/mtd_blkdevs.c b/drivers/mtd/mtd_blkdevs.c
index ca385697446e..bff8d4671ad6 100644
--- a/drivers/mtd/mtd_blkdevs.c
+++ b/drivers/mtd/mtd_blkdevs.c
@@ -215,7 +215,7 @@ static int blktrans_open(struct block_device *bdev, fmode_t mode)
 
 	mutex_lock(&dev->lock);
 
-	if (dev->open++)
+	if (dev->open)
 		goto unlock;
 
 	kref_get(&dev->ref);
@@ -235,6 +235,7 @@ static int blktrans_open(struct block_device *bdev, fmode_t mode)
 		goto error_release;
 
 unlock:
+	dev->open++;
 	mutex_unlock(&dev->lock);
 	blktrans_dev_put(dev);
 	return ret;
diff --git a/drivers/mtd/mtdchar.c b/drivers/mtd/mtdchar.c
index f1af2228a1b1..49e20a497084 100644
--- a/drivers/mtd/mtdchar.c
+++ b/drivers/mtd/mtdchar.c
@@ -320,6 +320,7 @@ static ssize_t mtd_write(struct file *file, const char __user *buf, size_t count
 			ops.mode = MTD_OOB_RAW;
 			ops.datbuf = kbuf;
 			ops.oobbuf = NULL;
+			ops.ooboffs = 0;
 			ops.len = len;
 
 			ret = mtd->write_oob(mtd, *ppos, &ops);
diff --git a/drivers/mtd/mtdoops.c b/drivers/mtd/mtdoops.c
index e3e40f440323..43130e8aceac 100644
--- a/drivers/mtd/mtdoops.c
+++ b/drivers/mtd/mtdoops.c
@@ -253,6 +253,9 @@ static void find_next_position(struct mtdoops_context *cxt)
 	size_t retlen;
 
 	for (page = 0; page < cxt->oops_pages; page++) {
+		if (mtd->block_isbad &&
+		    mtd->block_isbad(mtd, page * record_size))
+			continue;
 		/* Assume the page is used */
 		mark_page_used(cxt, page);
 		ret = mtd->read(mtd, page * record_size, MTDOOPS_HEADER_SIZE,
@@ -369,7 +372,7 @@ static void mtdoops_notify_add(struct mtd_info *mtd)
 
 	/* oops_page_used is a bit field */
 	cxt->oops_page_used = vmalloc(DIV_ROUND_UP(mtdoops_pages,
-			BITS_PER_LONG));
+			BITS_PER_LONG) * sizeof(unsigned long));
 	if (!cxt->oops_page_used) {
 		printk(KERN_ERR "mtdoops: could not allocate page array\n");
 		return;
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index 4c3425235adc..43173a335e49 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -1,3 +1,10 @@
+config MTD_NAND_IDS
+	tristate "Include chip ids for known NAND devices."
+	depends on MTD
+	help
+	  Useful for NAND drivers that do not use the NAND subsystem but
+	  still like to take advantage of the known chip information.
+
 config MTD_NAND_ECC
 	tristate
 
@@ -121,6 +128,23 @@ config MTD_NAND_OMAP2
 	help
           Support for NAND flash on Texas Instruments OMAP2 and OMAP3 platforms.
 
+config MTD_NAND_OMAP_PREFETCH
+	bool "GPMC prefetch support for NAND Flash device"
+	depends on MTD_NAND_OMAP2
+	default y
+	help
+	 The NAND device can be accessed for Read/Write using GPMC PREFETCH engine
+	 to improve the performance.
+
+config MTD_NAND_OMAP_PREFETCH_DMA
+	depends on MTD_NAND_OMAP_PREFETCH
+	bool "DMA mode"
+	default n
+	help
+	 The GPMC PREFETCH engine can be configured eigther in MPU interrupt mode
+	 or in DMA interrupt mode.
+	 Say y for DMA mode or MPU mode will be used
+
 config MTD_NAND_IDS
 	tristate
 
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index a46e9bb847bd..15d71658b4f1 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -2097,14 +2097,22 @@ static int nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 
 /**
  * nand_fill_oob - [Internal] Transfer client buffer to oob
- * @chip:	nand chip structure
+ * @mtd:	MTD device structure
  * @oob:	oob data buffer
  * @len:	oob data write length
  * @ops:	oob ops structure
  */
-static uint8_t *nand_fill_oob(struct nand_chip *chip, uint8_t *oob, size_t len,
-						struct mtd_oob_ops *ops)
+static uint8_t *nand_fill_oob(struct mtd_info *mtd, uint8_t *oob, size_t len,
+			      struct mtd_oob_ops *ops)
 {
+	struct nand_chip *chip = mtd->priv;
+
+	/*
+	 * Initialise to all 0xFF, to avoid the possibility of left over OOB
+	 * data from a previous OOB read.
+	 */
+	memset(chip->oob_poi, 0xff, mtd->oobsize);
+
 	switch (ops->mode) {
 
 	case MTD_OOB_PLACE:
@@ -2201,10 +2209,6 @@ static int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 	    (chip->pagebuf << chip->page_shift) < (to + ops->len))
 		chip->pagebuf = -1;
 
-	/* If we're not given explicit OOB data, let it be 0xFF */
-	if (likely(!oob))
-		memset(chip->oob_poi, 0xff, mtd->oobsize);
-
 	/* Don't allow multipage oob writes with offset */
 	if (oob && ops->ooboffs && (ops->ooboffs + ops->ooblen > oobmaxlen))
 		return -EINVAL;
@@ -2226,8 +2230,11 @@ static int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 
 		if (unlikely(oob)) {
 			size_t len = min(oobwritelen, oobmaxlen);
-			oob = nand_fill_oob(chip, oob, len, ops);
+			oob = nand_fill_oob(mtd, oob, len, ops);
 			oobwritelen -= len;
+		} else {
+			/* We still need to erase leftover OOB data */
+			memset(chip->oob_poi, 0xff, mtd->oobsize);
 		}
 
 		ret = chip->write_page(mtd, chip, wbuf, page, cached,
@@ -2401,10 +2408,8 @@ static int nand_do_write_oob(struct mtd_info *mtd, loff_t to,
 	if (page == chip->pagebuf)
 		chip->pagebuf = -1;
 
-	memset(chip->oob_poi, 0xff, mtd->oobsize);
-	nand_fill_oob(chip, ops->oobbuf, ops->ooblen, ops);
+	nand_fill_oob(mtd, ops->oobbuf, ops->ooblen, ops);
 	status = chip->ecc.write_oob(mtd, chip, page & chip->pagemask);
-	memset(chip->oob_poi, 0xff, mtd->oobsize);
 
 	if (status)
 		return status;
@@ -3217,6 +3222,44 @@ int nand_scan_ident(struct mtd_info *mtd, int maxchips,
 }
 EXPORT_SYMBOL(nand_scan_ident);
 
+static void nand_panic_wait(struct mtd_info *mtd)
+{
+	struct nand_chip *chip = mtd->priv;
+	int i;
+
+	if (chip->state != FL_READY)
+		for (i = 0; i < 40; i++) {
+			if (chip->dev_ready(mtd))
+				break;
+			mdelay(10);
+		}
+	chip->state = FL_READY;
+}
+
+static int nand_panic_write(struct mtd_info *mtd, loff_t to, size_t len,
+			    size_t *retlen, const u_char *buf)
+{
+	struct nand_chip *chip = mtd->priv;
+	int ret;
+
+	/* Do not allow reads past end of device */
+	if ((to + len) > mtd->size)
+		return -EINVAL;
+	if (!len)
+		return 0;
+
+	nand_panic_wait(mtd);
+
+	chip->ops.len = len;
+	chip->ops.datbuf = (uint8_t *)buf;
+	chip->ops.oobbuf = NULL;
+
+	ret = nand_do_write_ops(mtd, to, &chip->ops);
+
+	*retlen = chip->ops.retlen;
+	return ret;
+}
+
 
 /**
  * nand_scan_tail - [NAND Interface] Scan for the NAND device
@@ -3460,6 +3503,7 @@ int nand_scan_tail(struct mtd_info *mtd)
 	mtd->panic_write = panic_nand_write;
 	mtd->read_oob = nand_read_oob;
 	mtd->write_oob = nand_write_oob;
+	mtd->panic_write = nand_panic_write;
 	mtd->sync = nand_sync;
 	mtd->lock = NULL;
 	mtd->unlock = NULL;
diff --git a/drivers/mtd/nand/nand_ids.c b/drivers/mtd/nand/nand_ids.c
index 00cf1b0d6053..a56bf52acf5e 100644
--- a/drivers/mtd/nand/nand_ids.c
+++ b/drivers/mtd/nand/nand_ids.c
@@ -104,6 +104,7 @@ struct nand_flash_dev nand_flash_ids[] = {
 	{"NAND 512MiB 3,3V 16-bit",	0xCC, 0, 512, 0, LP_OPTIONS16},
 
 	/* 8 Gigabit */
+	{"NAND 1GiB 1,8V 8-bit",	0x38, 0, 1024, 0, LP_OPTIONS},
 	{"NAND 1GiB 1,8V 8-bit",	0xA3, 0, 1024, 0, LP_OPTIONS},
 	{"NAND 1GiB 3,3V 8-bit",	0xD3, 0, 1024, 0, LP_OPTIONS},
 	{"NAND 1GiB 1,8V 16-bit",	0xB3, 0, 1024, 0, LP_OPTIONS16},
diff --git a/drivers/mtd/nand/pxa3xx_nand.c b/drivers/mtd/nand/pxa3xx_nand.c
index 1fb3b3a80581..30689cc2b3c7 100644
--- a/drivers/mtd/nand/pxa3xx_nand.c
+++ b/drivers/mtd/nand/pxa3xx_nand.c
@@ -685,6 +685,8 @@ static int pxa3xx_nand_read_page_hwecc(struct mtd_info *mtd,
 		 * OOB, ignore such double bit errors
 		 */
 		if (is_buf_blank(buf, mtd->writesize))
+			info->retcode = ERR_NONE;
+		else
 			mtd->ecc_stats.failed++;
 	}
 
@@ -813,7 +815,7 @@ static int pxa3xx_nand_detect_config(struct pxa3xx_nand_info *info)
 	info->page_size = ndcr & NDCR_PAGE_SZ ? 2048 : 512;
 	/* set info fields needed to read id */
 	info->read_id_bytes = (info->page_size == 2048) ? 4 : 2;
-	info->reg_ndcr = ndcr;
+	info->reg_ndcr = ndcr & ~NDCR_INT_MASK;
 	info->cmdset = &default_cmdset;
 
 	info->ndtr0cs0 = nand_readl(info, NDTR0CS0);
@@ -882,7 +884,7 @@ static int pxa3xx_nand_scan(struct mtd_info *mtd)
 	struct pxa3xx_nand_info *info = mtd->priv;
 	struct platform_device *pdev = info->pdev;
 	struct pxa3xx_nand_platform_data *pdata = pdev->dev.platform_data;
-	struct nand_flash_dev pxa3xx_flash_ids[2] = { {NULL,}, {NULL,} };
+	struct nand_flash_dev pxa3xx_flash_ids[2], *def = NULL;
 	const struct pxa3xx_nand_flash *f = NULL;
 	struct nand_chip *chip = mtd->priv;
 	uint32_t id = -1;
@@ -942,8 +944,10 @@ static int pxa3xx_nand_scan(struct mtd_info *mtd)
 	pxa3xx_flash_ids[0].erasesize = f->page_size * f->page_per_block;
 	if (f->flash_width == 16)
 		pxa3xx_flash_ids[0].options = NAND_BUSWIDTH_16;
+	pxa3xx_flash_ids[1].name = NULL;
+	def = pxa3xx_flash_ids;
 KEEP_CONFIG:
-	if (nand_scan_ident(mtd, 1, pxa3xx_flash_ids))
+	if (nand_scan_ident(mtd, 1, def))
 		return -ENODEV;
 	/* calculate addressing information */
 	info->col_addr_cycles = (mtd->writesize >= 2048) ? 2 : 1;
@@ -954,9 +958,9 @@ KEEP_CONFIG:
 		info->row_addr_cycles = 2;
 	mtd->name = mtd_names[0];
 	chip->ecc.mode = NAND_ECC_HW;
-	chip->ecc.size = f->page_size;
+	chip->ecc.size = info->page_size;
 
-	chip->options = (f->flash_width == 16) ? NAND_BUSWIDTH_16 : 0;
+	chip->options = (info->reg_ndcr & NDCR_DWIDTH_M) ? NAND_BUSWIDTH_16 : 0;
 	chip->options |= NAND_NO_AUTOINCR;
 	chip->options |= NAND_NO_READRDY;
 
diff --git a/drivers/mtd/redboot.c b/drivers/mtd/redboot.c
index 7a87d07cd79f..4938bd0b024a 100644
--- a/drivers/mtd/redboot.c
+++ b/drivers/mtd/redboot.c
@@ -297,6 +297,9 @@ static struct mtd_part_parser redboot_parser = {
 	.name = "RedBoot",
 };
 
+/* mtd parsers will request the module by parser name */
+MODULE_ALIAS("RedBoot");
+
 static int __init redboot_parser_init(void)
 {
 	return register_mtd_parser(&redboot_parser);
diff --git a/drivers/mtd/tests/mtd_stresstest.c b/drivers/mtd/tests/mtd_stresstest.c
index 531625fc9259..129bad2e4080 100644
--- a/drivers/mtd/tests/mtd_stresstest.c
+++ b/drivers/mtd/tests/mtd_stresstest.c
@@ -277,6 +277,12 @@ static int __init mtd_stresstest_init(void)
 	       (unsigned long long)mtd->size, mtd->erasesize,
 	       pgsize, ebcnt, pgcnt, mtd->oobsize);
 
+	if (ebcnt < 2) {
+		printk(PRINT_PREF "error: need at least 2 eraseblocks\n");
+		err = -ENOSPC;
+		goto out_put_mtd;
+	}
+
 	/* Read or write up 2 eraseblocks at a time */
 	bufsize = mtd->erasesize * 2;
 
@@ -315,6 +321,7 @@ out:
 	kfree(bbt);
 	vfree(writebuf);
 	vfree(readbuf);
+out_put_mtd:
 	put_mtd_device(mtd);
 	if (err)
 		printk(PRINT_PREF "error %d occurred\n", err);
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index a44874e24f2a..bcd010078fbb 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -2139,6 +2139,8 @@ config IP1000
 config IGB
 	tristate "Intel(R) 82575/82576 PCI-Express Gigabit Ethernet support"
 	depends on PCI
+	select I2C
+	select I2C_ALGOBIT
 	---help---
 	  This driver supports Intel(R) 82575/82576 gigabit ethernet family of
 	  adapters.  For more information on how to identify your adapter, go
@@ -2157,6 +2159,29 @@ config IGB
 	  To compile this driver as a module, choose M here. The module
 	  will be called igb.
 
+config IGB_PTP
+	bool "PTP Hardware Clock (PHC)"
+	default n
+	depends on IGB
+	select PTP_1588_CLOCK
+	---help---
+	  Say Y here if you want to use PTP Hardware Clock (PHC) in the
+	  driver.  Only the basic clock operations have been implemented.
+
+	  Every timestamp and clock read operations must consult the
+	  overflow counter to form a correct time value.
+
+config IGB_HWMON
+	bool "Intel(R) PCI-Express Gigabit adapters HWMON support"
+	default y
+	depends on IGB && HWMON && !(IGB=y && HWMON=m)
+	---help---
+	  Say Y if you want to expose thermal sensor data on Intel devices.
+
+	  Some of our devices contain thermal sensors, both external and internal.
+	  This data is available via the hwmon sysfs interface and exposes
+	  the onboard sensors.
+
 config IGB_DCA
 	bool "Direct Cache Access (DCA) Support"
 	default y
@@ -2237,18 +2262,27 @@ config R8169
 	  To compile this driver as a module, choose M here: the module
 	  will be called r8169.  This is recommended.
 
-config SB1250_MAC
-	tristate "SB1250 Gigabit Ethernet support"
-	depends on SIBYTE_SB1xxx_SOC
-	select PHYLIB
+config R8169
+	tristate "Realtek 8169 gigabit ethernet support"
+	depends on PCI
+	select CRC32
+	select MII
 	---help---
-	  This driver supports Gigabit Ethernet interfaces based on the
-	  Broadcom SiByte family of System-On-a-Chip parts.  They include
-	  the BCM1120, BCM1125, BCM1125H, BCM1250, BCM1255, BCM1280, BCM1455
-	  and BCM1480 chips.
+	  Say Y here if you have a Realtek 8169 PCI Gigabit Ethernet adapter.
 
 	  To compile this driver as a module, choose M here: the module
-	  will be called sb1250-mac.
+	  will be called r8169.  This is recommended.
+
+config R8169_FW_LOAD
+	bool "Load firmware for Realtek 8169"
+	depends on R8169
+	select FW_LOADER
+	default y
+	---help---
+	  This enables runtime loading of optional firmware by the Realtek 8169
+	  driver. Some environments do not require firmware, or prefer not to delay
+	  the boot process when firmware is not available. Say N here to disable
+	  firmware loading. If unsure, say Y.
 
 config SIS190
 	tristate "SiS190/SiS191 gigabit ethernet support"
@@ -3325,6 +3359,23 @@ config PPPOL2TP
 	  used by ISPs and enterprises to tunnel PPP traffic over UDP
 	  tunnels. L2TP is replacing PPTP for VPN uses.
 
+config PPPOLAC
+	tristate "PPP on L2TP Access Concentrator"
+	depends on PPP && INET
+	help
+	  L2TP (RFC 2661) is a tunneling protocol widely used in virtual private
+	  networks. This driver handles L2TP data packets between a UDP socket
+	  and a PPP channel, but only permits one session per socket. Thus it is
+	  fairly simple and suited for clients.
+
+config PPPOPNS
+	tristate "PPP on PPTP Network Server"
+	depends on PPP && INET
+	help
+	  PPTP (RFC 2637) is a tunneling protocol widely used in virtual private
+	  networks. This driver handles PPTP data packets between a RAW socket
+	  and a PPP channel. It is fairly simple and easy to use.
+
 config SLIP
 	tristate "SLIP (serial line) support"
 	---help---
diff --git a/drivers/net/Makefile b/drivers/net/Makefile
index e1eca2ab505e..e4a90ac66d09 100644
--- a/drivers/net/Makefile
+++ b/drivers/net/Makefile
@@ -167,6 +167,8 @@ obj-$(CONFIG_PPP_MPPE) += ppp_mppe.o
 obj-$(CONFIG_PPPOE) += pppox.o pppoe.o
 obj-$(CONFIG_PPPOL2TP) += pppox.o
 obj-$(CONFIG_PPTP) += pppox.o pptp.o
+obj-$(CONFIG_PPPOLAC) += pppox.o pppolac.o
+obj-$(CONFIG_PPPOPNS) += pppox.o pppopns.o
 
 obj-$(CONFIG_SLIP) += slip.o
 obj-$(CONFIG_SLHC) += slhc.o
@@ -283,6 +285,7 @@ obj-$(CONFIG_USB_USBNET)        += usb/
 obj-$(CONFIG_USB_ZD1201)        += usb/
 obj-$(CONFIG_USB_IPHETH)        += usb/
 obj-$(CONFIG_USB_CDC_PHONET)   += usb/
+obj-$(CONFIG_USB_NET_RAW_IP)    += usb/
 
 obj-$(CONFIG_WLAN) += wireless/
 obj-$(CONFIG_NET_TULIP) += tulip/
diff --git a/drivers/net/bnx2x/bnx2x_ethtool.c b/drivers/net/bnx2x/bnx2x_ethtool.c
index cf3e47914dd7..a91a5972d89e 100644
--- a/drivers/net/bnx2x/bnx2x_ethtool.c
+++ b/drivers/net/bnx2x/bnx2x_ethtool.c
@@ -2286,18 +2286,20 @@ static int bnx2x_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *info,
 	}
 }
 
-static int bnx2x_get_rxfh_indir(struct net_device *dev,
-				struct ethtool_rxfh_indir *indir)
+static u32 bnx2x_get_rxfh_indir_size(struct net_device *dev)
+{
+	struct bnx2x *bp = netdev_priv(dev);
+
+	return (bp->multi_mode == ETH_RSS_MODE_DISABLED ?
+		0 : T_ETH_INDIRECTION_TABLE_SIZE);
+}
+
+static int bnx2x_get_rxfh_indir(struct net_device *dev, u32 *indir)
 {
 	struct bnx2x *bp = netdev_priv(dev);
-	size_t copy_size =
-		min_t(size_t, indir->size, T_ETH_INDIRECTION_TABLE_SIZE);
 	u8 ind_table[T_ETH_INDIRECTION_TABLE_SIZE] = {0};
 	size_t i;
 
-	if (bp->multi_mode == ETH_RSS_MODE_DISABLED)
-		return -EOPNOTSUPP;
-
 	/* Get the current configuration of the RSS indirection table */
 	bnx2x_get_rss_ind_table(&bp->rss_conf_obj, ind_table);
 
@@ -2310,33 +2312,19 @@ static int bnx2x_get_rxfh_indir(struct net_device *dev,
 	 * align the returned table to the Client ID of the leading RSS
 	 * queue.
 	 */
-	for (i = 0; i < copy_size; i++)
-		indir->ring_index[i] = ind_table[i] - bp->fp->cl_id;
-
-	indir->size = T_ETH_INDIRECTION_TABLE_SIZE;
+	for (i = 0; i < T_ETH_INDIRECTION_TABLE_SIZE; i++)
+		indir[i] = ind_table[i] - bp->fp->cl_id;
 
 	return 0;
 }
 
-static int bnx2x_set_rxfh_indir(struct net_device *dev,
-				const struct ethtool_rxfh_indir *indir)
+static int bnx2x_set_rxfh_indir(struct net_device *dev, const u32 *indir)
 {
 	struct bnx2x *bp = netdev_priv(dev);
 	size_t i;
 	u8 ind_table[T_ETH_INDIRECTION_TABLE_SIZE] = {0};
-	u32 num_eth_queues = BNX2X_NUM_ETH_QUEUES(bp);
-
-	if (bp->multi_mode == ETH_RSS_MODE_DISABLED)
-		return -EOPNOTSUPP;
-
-	/* validate the size */
-	if (indir->size != T_ETH_INDIRECTION_TABLE_SIZE)
-		return -EINVAL;
 
 	for (i = 0; i < T_ETH_INDIRECTION_TABLE_SIZE; i++) {
-		/* validate the indices */
-		if (indir->ring_index[i] >= num_eth_queues)
-			return -EINVAL;
 		/*
 		 * The same as in bnx2x_get_rxfh_indir: we can't use a memcpy()
 		 * as an internal storage of an indirection table is a u8 array
@@ -2346,7 +2334,7 @@ static int bnx2x_set_rxfh_indir(struct net_device *dev,
 		 * align the received table to the Client ID of the leading RSS
 		 * queue
 		 */
-		ind_table[i] = indir->ring_index[i] + bp->fp->cl_id;
+		ind_table[i] = indir[i] + bp->fp->cl_id;
 	}
 
 	return bnx2x_config_rss_pf(bp, ind_table, false);
@@ -2379,6 +2367,7 @@ static const struct ethtool_ops bnx2x_ethtool_ops = {
 	.set_phys_id		= bnx2x_set_phys_id,
 	.get_ethtool_stats	= bnx2x_get_ethtool_stats,
 	.get_rxnfc		= bnx2x_get_rxnfc,
+	.get_rxfh_indir_size	= bnx2x_get_rxfh_indir_size,
 	.get_rxfh_indir		= bnx2x_get_rxfh_indir,
 	.set_rxfh_indir		= bnx2x_set_rxfh_indir,
 };
diff --git a/drivers/net/bonding/bond_alb.c b/drivers/net/bonding/bond_alb.c
index d4fbd2e62616..f1abd4bb9de3 100644
--- a/drivers/net/bonding/bond_alb.c
+++ b/drivers/net/bonding/bond_alb.c
@@ -871,16 +871,12 @@ static void alb_send_learning_packets(struct slave *slave, u8 mac_addr[])
 	}
 }
 
-/* hw is a boolean parameter that determines whether we should try and
- * set the hw address of the device as well as the hw address of the
- * net_device
- */
-static int alb_set_slave_mac_addr(struct slave *slave, u8 addr[], int hw)
+static int alb_set_slave_mac_addr(struct slave *slave, u8 addr[])
 {
 	struct net_device *dev = slave->dev;
 	struct sockaddr s_addr;
 
-	if (!hw) {
+	if (slave->bond->params.mode == BOND_MODE_TLB) {
 		memcpy(dev->dev_addr, addr, dev->addr_len);
 		return 0;
 	}
@@ -910,8 +906,8 @@ static void alb_swap_mac_addr(struct bonding *bond, struct slave *slave1, struct
 	u8 tmp_mac_addr[ETH_ALEN];
 
 	memcpy(tmp_mac_addr, slave1->dev->dev_addr, ETH_ALEN);
-	alb_set_slave_mac_addr(slave1, slave2->dev->dev_addr, bond->alb_info.rlb_enabled);
-	alb_set_slave_mac_addr(slave2, tmp_mac_addr, bond->alb_info.rlb_enabled);
+	alb_set_slave_mac_addr(slave1, slave2->dev->dev_addr);
+	alb_set_slave_mac_addr(slave2, tmp_mac_addr);
 
 }
 
@@ -1058,8 +1054,7 @@ static int alb_handle_addr_collision_on_attach(struct bonding *bond, struct slav
 
 		/* Try setting slave mac to bond address and fall-through
 		   to code handling that situation below... */
-		alb_set_slave_mac_addr(slave, bond->dev->dev_addr,
-				       bond->alb_info.rlb_enabled);
+		alb_set_slave_mac_addr(slave, bond->dev->dev_addr);
 	}
 
 	/* The slave's address is equal to the address of the bond.
@@ -1095,8 +1090,7 @@ static int alb_handle_addr_collision_on_attach(struct bonding *bond, struct slav
 	}
 
 	if (free_mac_slave) {
-		alb_set_slave_mac_addr(slave, free_mac_slave->perm_hwaddr,
-				       bond->alb_info.rlb_enabled);
+		alb_set_slave_mac_addr(slave, free_mac_slave->perm_hwaddr);
 
 		pr_warning("%s: Warning: the hw address of slave %s is in use by the bond; giving it the hw address of %s\n",
 			   bond->dev->name, slave->dev->name,
@@ -1453,8 +1447,7 @@ int bond_alb_init_slave(struct bonding *bond, struct slave *slave)
 {
 	int res;
 
-	res = alb_set_slave_mac_addr(slave, slave->perm_hwaddr,
-				     bond->alb_info.rlb_enabled);
+	res = alb_set_slave_mac_addr(slave, slave->perm_hwaddr);
 	if (res) {
 		return res;
 	}
@@ -1605,8 +1598,7 @@ void bond_alb_handle_active_change(struct bonding *bond, struct slave *new_slave
 		alb_swap_mac_addr(bond, swap_slave, new_slave);
 	} else {
 		/* set the new_slave to the bond mac address */
-		alb_set_slave_mac_addr(new_slave, bond->dev->dev_addr,
-				       bond->alb_info.rlb_enabled);
+		alb_set_slave_mac_addr(new_slave, bond->dev->dev_addr);
 	}
 
 	if (swap_slave) {
@@ -1666,8 +1658,7 @@ int bond_alb_set_mac_address(struct net_device *bond_dev, void *addr)
 		alb_swap_mac_addr(bond, swap_slave, bond->curr_active_slave);
 		alb_fasten_mac_swap(bond, swap_slave, bond->curr_active_slave);
 	} else {
-		alb_set_slave_mac_addr(bond->curr_active_slave, bond_dev->dev_addr,
-				       bond->alb_info.rlb_enabled);
+		alb_set_slave_mac_addr(bond->curr_active_slave, bond_dev->dev_addr);
 
 		read_lock(&bond->lock);
 		alb_send_learning_packets(bond->curr_active_slave, bond_dev->dev_addr);
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index de3d351ccb6b..ba0e065a63ce 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -1839,7 +1839,7 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 				 "but new slave device does not support netpoll.\n",
 				 bond_dev->name);
 			res = -EBUSY;
-			goto err_close;
+			goto err_detach;
 		}
 	}
 #endif
@@ -1848,7 +1848,7 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 
 	res = bond_create_slave_symlinks(bond_dev, slave_dev);
 	if (res)
-		goto err_close;
+		goto err_detach;
 
 	res = netdev_rx_handler_register(slave_dev, bond_handle_frame,
 					 new_slave);
@@ -1869,6 +1869,11 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 err_dest_symlinks:
 	bond_destroy_slave_symlinks(bond_dev, slave_dev);
 
+err_detach:
+	write_lock_bh(&bond->lock);
+	bond_detach_slave(bond, new_slave);
+	write_unlock_bh(&bond->lock);
+
 err_close:
 	dev_close(slave_dev);
 
diff --git a/drivers/net/caif/Kconfig b/drivers/net/caif/Kconfig
index abf4d7a9dcce..3013a74bbfdd 100644
--- a/drivers/net/caif/Kconfig
+++ b/drivers/net/caif/Kconfig
@@ -47,3 +47,12 @@ config CAIF_HSI
        The caif low level driver for CAIF over HSI.
        Be aware that if you enable this then you also need to
        enable a low-level HSI driver.
+
+config TEGRA_SPI_CAIF
+	tristate "TEGRA specific CAIF SPI transport driver for slave interface"
+	depends on CAIF_SPI_SLAVE && TEGRA_SPI_SLAVE
+	default n
+	---help---
+	The CAIF Link layer SPI Protocol driver for Tegra Slave SPI interface.
+	This driver implements a platform driver to accommodate for a
+	Tegra Slave SPI device.
diff --git a/drivers/net/caif/Makefile b/drivers/net/caif/Makefile
index 91dff861560f..5f85fe79c595 100644
--- a/drivers/net/caif/Makefile
+++ b/drivers/net/caif/Makefile
@@ -13,3 +13,8 @@ obj-$(CONFIG_CAIF_SHM) += caif_shm.o
 
 # HSI interface
 obj-$(CONFIG_CAIF_HSI) += caif_hsi.o
+
+# Tegra specific SPI slave physical interfaces module
+CFLAGS_tegra_caif_sspi.o	= -Werror
+tegra_cfspi_slave-objs := tegra_caif_sspi.o
+obj-$(CONFIG_TEGRA_SPI_CAIF) += tegra_cfspi_slave.o
diff --git a/drivers/net/caif/tegra_caif_sspi.c b/drivers/net/caif/tegra_caif_sspi.c
new file mode 100644
index 000000000000..c2c15c18f2e4
--- /dev/null
+++ b/drivers/net/caif/tegra_caif_sspi.c
@@ -0,0 +1,426 @@
+/*
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/wait.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/spi/spi.h>
+#include <linux/tegra_caif.h>
+#include <mach/spi.h>
+#include <net/caif/caif_spi.h>
+
+MODULE_LICENSE("GPL");
+
+#define SPI_CAIF_PAD_TRANSACTION_SIZE(x) \
+	(((x) > 4) ? ((((x) + 15) / 16) * 16) : (x))
+
+struct sspi_struct {
+	struct cfspi_dev sdev;
+	struct cfspi_xfer *xfer;
+};
+
+static struct sspi_struct slave;
+static struct platform_device slave_device;
+static struct spi_device *tegra_caif_spi_slave_device;
+int tegra_caif_sspi_gpio_spi_int;
+int tegra_caif_sspi_gpio_spi_ss;
+int tegra_caif_sspi_gpio_reset;
+int tegra_caif_sspi_gpio_power;
+int tegra_caif_sspi_gpio_awr;
+int tegra_caif_sspi_gpio_cwr;
+
+
+static int __devinit tegra_caif_spi_slave_probe(struct spi_device *spi);
+
+static int tegra_caif_spi_slave_remove(struct spi_device *spi)
+{
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int tegra_caif_spi_slave_suspend(struct spi_device *spi
+			, pm_message_t mesg)
+{
+	return 0;
+}
+#endif /* CONFIG_PM */
+
+#ifdef CONFIG_PM
+static int tegra_caif_spi_slave_resume(struct spi_device *spi)
+{
+	return 0;
+}
+#endif /* CONFIG_PM */
+
+static struct spi_driver tegra_caif_spi_slave_driver = {
+	.driver = {
+		.name = "baseband_spi_slave0.0",
+		.owner = THIS_MODULE,
+	},
+	.probe = tegra_caif_spi_slave_probe,
+	.remove = __devexit_p(tegra_caif_spi_slave_remove),
+#ifdef CONFIG_PM
+	.suspend = tegra_caif_spi_slave_suspend,
+	.resume = tegra_caif_spi_slave_resume,
+#endif /* CONFIG_PM */
+};
+
+void tegra_caif_modem_power(int on)
+{
+	static int u3xx_on;
+	int err;
+	int cnt = 0;
+	int val = 0;
+
+	if (u3xx_on == on)
+		return;
+	u3xx_on = on;
+
+	if (u3xx_on) {
+		/* turn on u3xx modem */
+		err = gpio_request(tegra_caif_sspi_gpio_reset
+				, "caif_sspi_reset");
+		if (err < 0)
+			goto err1;
+
+		err = gpio_request(tegra_caif_sspi_gpio_power
+				, "caif_sspi_power");
+		if (err < 0)
+			goto err2;
+
+		err = gpio_request(tegra_caif_sspi_gpio_awr
+				, "caif_sspi_awr");
+		if (err < 0)
+			goto err3;
+
+		err = gpio_request(tegra_caif_sspi_gpio_cwr
+				, "caif_sspi_cwr");
+		if (err < 0)
+			goto err4;
+
+		err = gpio_direction_output(tegra_caif_sspi_gpio_reset
+						, 0 /* asserted */);
+		if (err < 0)
+			goto err5;
+
+		err = gpio_direction_output(tegra_caif_sspi_gpio_power
+						, 0 /* off */);
+		if (err < 0)
+			goto err6;
+
+		err = gpio_direction_output(tegra_caif_sspi_gpio_awr
+						, 0);
+		if (err < 0)
+			goto err7;
+
+		err = gpio_direction_input(tegra_caif_sspi_gpio_cwr);
+		if (err < 0)
+			goto err8;
+
+		gpio_set_value(tegra_caif_sspi_gpio_power, 0);
+		gpio_set_value(tegra_caif_sspi_gpio_reset, 0);
+
+		msleep(800);
+
+		/* pulse modem power on for 300 ms */
+		gpio_set_value(tegra_caif_sspi_gpio_reset
+				, 1 /* deasserted */);
+		msleep(300);
+		gpio_set_value(tegra_caif_sspi_gpio_power, 1);
+		msleep(300);
+		gpio_set_value(tegra_caif_sspi_gpio_power, 0);
+		msleep(100);
+
+		/* set awr high */
+		gpio_set_value(tegra_caif_sspi_gpio_awr, 1);
+		val = gpio_get_value(tegra_caif_sspi_gpio_cwr);
+		while (!val) {
+			/* wait for cwr to go high */
+			val = gpio_get_value(tegra_caif_sspi_gpio_cwr);
+			pr_info(".");
+			msleep(100);
+			cnt++;
+			if (cnt > 200) {
+				pr_err("\nWaiting for CWR timed out - ERROR\n");
+				break;
+			}
+		}
+	}
+	return;
+err8:
+err7:
+err6:
+err5:
+	gpio_free(tegra_caif_sspi_gpio_cwr);
+err4:
+	gpio_free(tegra_caif_sspi_gpio_awr);
+err3:
+	gpio_free(tegra_caif_sspi_gpio_power);
+err2:
+	gpio_free(tegra_caif_sspi_gpio_reset);
+err1:
+	return;
+}
+
+static irqreturn_t sspi_irq(int irq, void *arg)
+{
+	/* You only need to trigger on an edge to the active state of the
+	 * SS signal. Once a edge is detected, the ss_cb() function should
+	 * be called with the parameter assert set to true. It is OK
+	 * (and even advised) to call the ss_cb() function in IRQ context
+	 * in order not to add any delay.
+	 */
+	int val;
+	struct cfspi_dev *sdev = (struct cfspi_dev *)arg;
+	val = gpio_get_value(tegra_caif_sspi_gpio_spi_ss);
+	if (val)
+		return IRQ_HANDLED;
+	sdev->ifc->ss_cb(true, sdev->ifc);
+	return IRQ_HANDLED;
+}
+
+static int sspi_callback(void *arg)
+{
+	/* for each spi_sync() call
+	 * - sspi_callback() called before spi transfer
+	 * - sspi_complete() called after spi transfer
+	 */
+
+	/* set master interrupt gpio pin active (tells master to
+	 * start spi clock)
+	 */
+	udelay(MIN_TRANSITION_TIME_USEC);
+	gpio_set_value(tegra_caif_sspi_gpio_spi_int, 1);
+	return 0;
+}
+
+static void sspi_complete(void *context)
+{
+	/* Normally the DMA or the SPI framework will call you back
+	 * in something similar to this. The only thing you need to
+	 * do is to call the xfer_done_cb() function, providing the pointer
+	 * to the CAIF SPI interface. It is OK to call this function
+	 * from IRQ context.
+	 */
+
+	struct cfspi_dev *sdev = (struct cfspi_dev *)context;
+	sdev->ifc->xfer_done_cb(sdev->ifc);
+}
+
+static void swap_byte(unsigned char *buf, unsigned int bufsiz)
+{
+	unsigned int i;
+	unsigned char tmp;
+	for (i = 0; i < bufsiz; i += 2) {
+		tmp = buf[i];
+		buf[i] = buf[i+1];
+		buf[i+1] = tmp;
+	}
+}
+
+static int sspi_init_xfer(struct cfspi_xfer *xfer, struct cfspi_dev *dev)
+{
+	/* Store transfer info. For a normal implementation you should
+	 * set up your DMA here and make sure that you are ready to
+	 * receive the data from the master SPI.
+	 */
+
+	struct sspi_struct *sspi = (struct sspi_struct *)dev->priv;
+	struct spi_message m;
+	struct spi_transfer t;
+	int err;
+
+	sspi->xfer = xfer;
+
+	if (!tegra_caif_spi_slave_device)
+		return -ENODEV;
+
+	err = spi_tegra_register_callback(tegra_caif_spi_slave_device,
+		sspi_callback, sspi);
+	if (err < 0) {
+		pr_err("\nspi_tegra_register_callback() failed\n");
+		return -ENODEV;
+	}
+	memset(&t, 0, sizeof(t));
+	t.tx_buf = xfer->va_tx;
+	swap_byte(xfer->va_tx, xfer->tx_dma_len);
+	t.rx_buf = xfer->va_rx;
+	t.len = max(xfer->tx_dma_len, xfer->rx_dma_len);
+	t.len = SPI_CAIF_PAD_TRANSACTION_SIZE(t.len);
+	t.bits_per_word = 16;
+	/* SPI controller clock should be 4 times the spi_clk */
+	t.speed_hz = (SPI_MASTER_CLK_MHZ * 4 * 1000000);
+	spi_message_init(&m);
+	spi_message_add_tail(&t, &m);
+
+	dmb();
+	err = spi_sync(tegra_caif_spi_slave_device, &m);
+	dmb();
+	swap_byte(xfer->va_tx, xfer->tx_dma_len);
+	swap_byte(xfer->va_rx, xfer->rx_dma_len);
+	sspi_complete(&sspi->sdev);
+	if (err < 0) {
+		pr_err("spi_init_xfer - spi_sync() err %d\n", err);
+		return err;
+	}
+	return 0;
+}
+
+void sspi_sig_xfer(bool xfer, struct cfspi_dev *dev)
+{
+	/* If xfer is true then you should assert the SPI_INT to indicate to
+	 * the master that you are ready to recieve the data from the master
+	 * SPI. If xfer is false then you should de-assert SPI_INT to indicate
+	 * that the transfer is done.
+	 */
+	if (xfer)
+		gpio_set_value(tegra_caif_sspi_gpio_spi_int, 1);
+	else
+		gpio_set_value(tegra_caif_sspi_gpio_spi_int, 0);
+}
+
+static void sspi_release(struct device *dev)
+{
+	/*
+	 * Here you should release your SPI device resources.
+	 */
+}
+
+static int __init sspi_init(void)
+{
+	/* Here you should initialize your SPI device by providing the
+	 * necessary functions, clock speed, name and private data. Once
+	 * done, you can register your device with the
+	 * platform_device_register() function. This function will return
+	 * with the CAIF SPI interface initialized. This is probably also
+	 * the place where you should set up your GPIOs, interrupts and SPI
+	 * resources.
+	 */
+
+	int res = 0;
+
+	/* Register Tegra SPI protocol driver. */
+	res = spi_register_driver(&tegra_caif_spi_slave_driver);
+	if (res < 0)
+		return res;
+
+	/* Initialize slave device. */
+	slave.sdev.init_xfer = sspi_init_xfer;
+	slave.sdev.sig_xfer = sspi_sig_xfer;
+	slave.sdev.clk_mhz = SPI_MASTER_CLK_MHZ;
+	slave.sdev.priv = &slave;
+	slave.sdev.name = "spi_sspi";
+	slave_device.dev.release = sspi_release;
+
+	/* Initialize platform device. */
+	slave_device.name = "cfspi_sspi";
+	slave_device.dev.platform_data = &slave.sdev;
+
+	/* Register platform device. */
+	res = platform_device_register(&slave_device);
+	if (res)
+		return -ENODEV;
+
+	return res;
+}
+
+static void __exit sspi_exit(void)
+{
+	/* Delete platfrom device. */
+	platform_device_del(&slave_device);
+
+	/* Free Tegra SPI protocol driver. */
+	spi_unregister_driver(&tegra_caif_spi_slave_driver);
+
+	/* Free Tegra GPIO interrupts. */
+	disable_irq(gpio_to_irq(tegra_caif_sspi_gpio_spi_ss));
+	free_irq(gpio_to_irq(tegra_caif_sspi_gpio_spi_ss), &slave_device);
+
+	/* Free Tegra GPIOs. */
+	gpio_free(tegra_caif_sspi_gpio_spi_ss);
+	gpio_free(tegra_caif_sspi_gpio_spi_int);
+}
+
+static int __devinit tegra_caif_spi_slave_probe(struct spi_device *spi)
+{
+	struct tegra_caif_platform_data *pdata;
+	int res;
+
+	if (!spi)
+		return -ENODEV;
+
+	pdata = spi->dev.platform_data;
+	if (!pdata)
+		return -ENODEV;
+
+	tegra_caif_sspi_gpio_spi_int = pdata->spi_int;
+	tegra_caif_sspi_gpio_spi_ss = pdata->spi_ss;
+	tegra_caif_sspi_gpio_reset = pdata->reset;
+	tegra_caif_sspi_gpio_power = pdata->power;
+	tegra_caif_sspi_gpio_awr = pdata->awr;
+	tegra_caif_sspi_gpio_cwr = pdata->cwr;
+
+	tegra_caif_spi_slave_device = spi;
+
+	/* Initialize Tegra GPIOs. */
+	res = gpio_request(tegra_caif_sspi_gpio_spi_int, "caif_sspi_spi_int");
+	if (res < 0)
+		goto err1;
+
+	res = gpio_request(tegra_caif_sspi_gpio_spi_ss, "caif_sspi_ss");
+	if (res < 0)
+		goto err2;
+
+	res = gpio_direction_output(tegra_caif_sspi_gpio_spi_int, 0);
+	if (res < 0)
+		goto err3;
+
+	res = gpio_direction_input(tegra_caif_sspi_gpio_spi_ss);
+	if (res < 0)
+		goto err4;
+
+	tegra_caif_modem_power(1);
+	msleep(2000);
+
+	/* Initialize Tegra GPIO interrupts. */
+	res = request_irq(gpio_to_irq(tegra_caif_sspi_gpio_spi_ss),
+		sspi_irq, IRQF_TRIGGER_FALLING, "caif_sspi_ss_irq",
+		&slave.sdev);
+	if (res < 0)
+		goto err5;
+
+	return 0;
+err5:
+	free_irq(gpio_to_irq(tegra_caif_sspi_gpio_spi_ss), &slave_device);
+err4:
+err3:
+	gpio_free(tegra_caif_sspi_gpio_spi_ss);
+err2:
+	gpio_free(tegra_caif_sspi_gpio_spi_int);
+err1:
+	return res;
+}
+
+module_init(sspi_init);
+module_exit(sspi_exit);
diff --git a/drivers/net/can/mcp251x.c b/drivers/net/can/mcp251x.c
index 330140ee266d..899b152ee8ae 100644
--- a/drivers/net/can/mcp251x.c
+++ b/drivers/net/can/mcp251x.c
@@ -593,6 +593,25 @@ static int mcp251x_do_set_bittiming(struct net_device *net)
 	return 0;
 }
 
+static int mcp251x_get_berr_counter(const struct net_device *dev,
+				    struct can_berr_counter *bec)
+{
+	struct mcp251x_priv *priv = netdev_priv(dev);
+	struct spi_device *spi = priv->spi;
+	uint8_t tec,rec;
+
+	mutex_lock(&priv->mcp_lock);
+
+	mcp251x_read_2regs(spi, TEC, &tec, &rec);
+
+	mutex_unlock(&priv->mcp_lock);
+
+	bec->txerr = tec;
+	bec->rxerr = rec;
+
+	return 0;
+}
+
 static int mcp251x_setup(struct net_device *net, struct mcp251x_priv *priv,
 			 struct spi_device *spi)
 {
@@ -997,6 +1016,7 @@ static int __devinit mcp251x_can_probe(struct spi_device *spi)
 	priv = netdev_priv(net);
 	priv->can.bittiming_const = &mcp251x_bittiming_const;
 	priv->can.do_set_mode = mcp251x_do_set_mode;
+	priv->can.do_get_berr_counter = mcp251x_get_berr_counter;
 	priv->can.clock.freq = pdata->oscillator_frequency / 2;
 	priv->can.ctrlmode_supported = CAN_CTRLMODE_3_SAMPLES |
 		CAN_CTRLMODE_LOOPBACK | CAN_CTRLMODE_LISTENONLY;
diff --git a/drivers/net/can/sja1000/sja1000_platform.c b/drivers/net/can/sja1000/sja1000_platform.c
index d9fadc489b32..c8428f25d914 100644
--- a/drivers/net/can/sja1000/sja1000_platform.c
+++ b/drivers/net/can/sja1000/sja1000_platform.c
@@ -38,12 +38,24 @@ MODULE_LICENSE("GPL v2");
 
 static u8 sp_read_reg8(const struct sja1000_priv *priv, int reg)
 {
+#if !defined(CONFIG_MACH_COLIBRI_T20) && !defined(CONFIG_MACH_COLIBRI_T30)
 	return ioread8(priv->reg_base + reg);
+#else
+	u8 value;
+	iowrite8(reg, priv->reg_base);
+	value = ioread8(priv->reg_base + 8);
+	return value;
+#endif
 }
 
 static void sp_write_reg8(const struct sja1000_priv *priv, int reg, u8 val)
 {
+#if !defined(CONFIG_MACH_COLIBRI_T20) && !defined(CONFIG_MACH_COLIBRI_T30)
 	iowrite8(val, priv->reg_base + reg);
+#else
+	iowrite8(reg, priv->reg_base);
+	iowrite8(val, priv->reg_base + 8);
+#endif
 }
 
 static u8 sp_read_reg16(const struct sja1000_priv *priv, int reg)
diff --git a/drivers/net/cxgb4/cxgb4_main.c b/drivers/net/cxgb4/cxgb4_main.c
index b4efa292fd6f..b6a3ed69512c 100644
--- a/drivers/net/cxgb4/cxgb4_main.c
+++ b/drivers/net/cxgb4/cxgb4_main.c
@@ -1871,30 +1871,30 @@ static int cxgb_set_features(struct net_device *dev, u32 features)
 	return err;
 }
 
-static int get_rss_table(struct net_device *dev, struct ethtool_rxfh_indir *p)
+static u32 get_rss_table_size(struct net_device *dev)
 {
 	const struct port_info *pi = netdev_priv(dev);
-	unsigned int n = min_t(unsigned int, p->size, pi->rss_size);
 
-	p->size = pi->rss_size;
+	return pi->rss_size;
+}
+
+static int get_rss_table(struct net_device *dev, u32 *p)
+{
+	const struct port_info *pi = netdev_priv(dev);
+	unsigned int n = pi->rss_size;
+
 	while (n--)
-		p->ring_index[n] = pi->rss[n];
+		p[n] = pi->rss[n];
 	return 0;
 }
 
-static int set_rss_table(struct net_device *dev,
-			 const struct ethtool_rxfh_indir *p)
+static int set_rss_table(struct net_device *dev, const u32 *p)
 {
 	unsigned int i;
 	struct port_info *pi = netdev_priv(dev);
 
-	if (p->size != pi->rss_size)
-		return -EINVAL;
-	for (i = 0; i < p->size; i++)
-		if (p->ring_index[i] >= pi->nqsets)
-			return -EINVAL;
-	for (i = 0; i < p->size; i++)
-		pi->rss[i] = p->ring_index[i];
+	for (i = 0; i < pi->rss_size; i++)
+		pi->rss[i] = p[i];
 	if (pi->adapter->flags & FULL_INIT_DONE)
 		return write_rss(pi, pi->rss);
 	return 0;
@@ -1989,6 +1989,7 @@ static struct ethtool_ops cxgb_ethtool_ops = {
 	.get_wol           = get_wol,
 	.set_wol           = set_wol,
 	.get_rxnfc         = get_rxnfc,
+	.get_rxfh_indir_size = get_rss_table_size,
 	.get_rxfh_indir    = get_rss_table,
 	.set_rxfh_indir    = set_rss_table,
 	.flash_device      = set_flash,
diff --git a/drivers/net/igb/Makefile b/drivers/net/igb/Makefile
index c6e4621b6262..38de6c07ecb5 100644
--- a/drivers/net/igb/Makefile
+++ b/drivers/net/igb/Makefile
@@ -1,7 +1,7 @@
 ################################################################################
 #
 # Intel 82575 PCI-Express Ethernet Linux driver
-# Copyright(c) 1999 - 2011 Intel Corporation.
+# Copyright(c) 1999 - 2013 Intel Corporation.
 #
 # This program is free software; you can redistribute it and/or modify it
 # under the terms and conditions of the GNU General Public License,
@@ -33,5 +33,8 @@
 obj-$(CONFIG_IGB) += igb.o
 
 igb-objs := igb_main.o igb_ethtool.o e1000_82575.o \
-	    e1000_mac.o e1000_nvm.o e1000_phy.o e1000_mbx.o
-
+	    e1000_mac.o e1000_nvm.o e1000_phy.o e1000_mbx.o \
+	    e1000_i210.o \
+	    e1000_manage.o igb_param.o kcompat.o e1000_api.o \
+	    igb_vmdq.o igb_procfs.o igb_hwmon.o igb_debugfs.o
+igb-$(CONFIG_IGB_PTP) += igb_ptp.o
diff --git a/drivers/net/igb/e1000_82575.c b/drivers/net/igb/e1000_82575.c
index c0857bdfb03a..6130005fd4f6 100644
--- a/drivers/net/igb/e1000_82575.c
+++ b/drivers/net/igb/e1000_82575.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2007-2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -25,80 +25,123 @@
 
 *******************************************************************************/
 
-/* e1000_82575
- * e1000_82576
+/*
+ * 82575EB Gigabit Network Connection
+ * 82575EB Gigabit Backplane Connection
+ * 82575GB Gigabit Network Connection
+ * 82576 Gigabit Network Connection
+ * 82576 Quad Port Gigabit Mezzanine Adapter
+ * 82580 Gigabit Network Connection
+ * I350 Gigabit Network Connection
  */
 
-#include <linux/types.h>
-#include <linux/if_ether.h>
-
-#include "e1000_mac.h"
-#include "e1000_82575.h"
-
-static s32  igb_get_invariants_82575(struct e1000_hw *);
-static s32  igb_acquire_phy_82575(struct e1000_hw *);
-static void igb_release_phy_82575(struct e1000_hw *);
-static s32  igb_acquire_nvm_82575(struct e1000_hw *);
-static void igb_release_nvm_82575(struct e1000_hw *);
-static s32  igb_check_for_link_82575(struct e1000_hw *);
-static s32  igb_get_cfg_done_82575(struct e1000_hw *);
-static s32  igb_init_hw_82575(struct e1000_hw *);
-static s32  igb_phy_hw_reset_sgmii_82575(struct e1000_hw *);
-static s32  igb_read_phy_reg_sgmii_82575(struct e1000_hw *, u32, u16 *);
-static s32  igb_read_phy_reg_82580(struct e1000_hw *, u32, u16 *);
-static s32  igb_write_phy_reg_82580(struct e1000_hw *, u32, u16);
-static s32  igb_reset_hw_82575(struct e1000_hw *);
-static s32  igb_reset_hw_82580(struct e1000_hw *);
-static s32  igb_set_d0_lplu_state_82575(struct e1000_hw *, bool);
-static s32  igb_setup_copper_link_82575(struct e1000_hw *);
-static s32  igb_setup_serdes_link_82575(struct e1000_hw *);
-static s32  igb_write_phy_reg_sgmii_82575(struct e1000_hw *, u32, u16);
-static void igb_clear_hw_cntrs_82575(struct e1000_hw *);
-static s32  igb_acquire_swfw_sync_82575(struct e1000_hw *, u16);
-static s32  igb_get_pcs_speed_and_duplex_82575(struct e1000_hw *, u16 *,
-						 u16 *);
-static s32  igb_get_phy_id_82575(struct e1000_hw *);
-static void igb_release_swfw_sync_82575(struct e1000_hw *, u16);
-static bool igb_sgmii_active_82575(struct e1000_hw *);
-static s32  igb_reset_init_script_82575(struct e1000_hw *);
-static s32  igb_read_mac_addr_82575(struct e1000_hw *);
-static s32  igb_set_pcie_completion_timeout(struct e1000_hw *hw);
-static s32  igb_reset_mdicnfg_82580(struct e1000_hw *hw);
-static s32  igb_validate_nvm_checksum_82580(struct e1000_hw *hw);
-static s32  igb_update_nvm_checksum_82580(struct e1000_hw *hw);
-static s32  igb_update_nvm_checksum_with_offset(struct e1000_hw *hw,
-						u16 offset);
-static s32 igb_validate_nvm_checksum_with_offset(struct e1000_hw *hw,
-						u16 offset);
-static s32 igb_validate_nvm_checksum_i350(struct e1000_hw *hw);
-static s32 igb_update_nvm_checksum_i350(struct e1000_hw *hw);
-static const u16 e1000_82580_rxpbs_table[] =
-	{ 36, 72, 144, 1, 2, 4, 8, 16,
-	  35, 70, 140 };
+#include "e1000_api.h"
+#include "e1000_i210.h"
+
+static s32  e1000_init_phy_params_82575(struct e1000_hw *hw);
+static s32  e1000_init_mac_params_82575(struct e1000_hw *hw);
+static s32  e1000_acquire_phy_82575(struct e1000_hw *hw);
+static void e1000_release_phy_82575(struct e1000_hw *hw);
+static s32  e1000_acquire_nvm_82575(struct e1000_hw *hw);
+static void e1000_release_nvm_82575(struct e1000_hw *hw);
+static s32  e1000_check_for_link_82575(struct e1000_hw *hw);
+static s32  e1000_check_for_link_media_swap(struct e1000_hw *hw);
+static s32  e1000_get_cfg_done_82575(struct e1000_hw *hw);
+static s32  e1000_get_link_up_info_82575(struct e1000_hw *hw, u16 *speed,
+					 u16 *duplex);
+static s32  e1000_init_hw_82575(struct e1000_hw *hw);
+static s32  e1000_phy_hw_reset_sgmii_82575(struct e1000_hw *hw);
+static s32  e1000_read_phy_reg_sgmii_82575(struct e1000_hw *hw, u32 offset,
+					   u16 *data);
+static s32  e1000_reset_hw_82575(struct e1000_hw *hw);
+static s32  e1000_reset_hw_82580(struct e1000_hw *hw);
+static s32  e1000_read_phy_reg_82580(struct e1000_hw *hw,
+				     u32 offset, u16 *data);
+static s32  e1000_write_phy_reg_82580(struct e1000_hw *hw,
+				      u32 offset, u16 data);
+static s32  e1000_set_d0_lplu_state_82580(struct e1000_hw *hw,
+					  bool active);
+static s32  e1000_set_d3_lplu_state_82580(struct e1000_hw *hw,
+					  bool active);
+static s32  e1000_set_d0_lplu_state_82575(struct e1000_hw *hw,
+					  bool active);
+static s32  e1000_setup_copper_link_82575(struct e1000_hw *hw);
+static s32  e1000_setup_serdes_link_82575(struct e1000_hw *hw);
+static s32  e1000_get_media_type_82575(struct e1000_hw *hw);
+static s32  e1000_set_sfp_media_type_82575(struct e1000_hw *hw);
+static s32  e1000_valid_led_default_82575(struct e1000_hw *hw, u16 *data);
+static s32  e1000_write_phy_reg_sgmii_82575(struct e1000_hw *hw,
+					    u32 offset, u16 data);
+static void e1000_clear_hw_cntrs_82575(struct e1000_hw *hw);
+static s32  e1000_acquire_swfw_sync_82575(struct e1000_hw *hw, u16 mask);
+static s32  e1000_get_pcs_speed_and_duplex_82575(struct e1000_hw *hw,
+						 u16 *speed, u16 *duplex);
+static s32  e1000_get_phy_id_82575(struct e1000_hw *hw);
+static void e1000_release_swfw_sync_82575(struct e1000_hw *hw, u16 mask);
+static bool e1000_sgmii_active_82575(struct e1000_hw *hw);
+static s32  e1000_reset_init_script_82575(struct e1000_hw *hw);
+static s32  e1000_read_mac_addr_82575(struct e1000_hw *hw);
+static void e1000_config_collision_dist_82575(struct e1000_hw *hw);
+static void e1000_power_down_phy_copper_82575(struct e1000_hw *hw);
+static void e1000_shutdown_serdes_link_82575(struct e1000_hw *hw);
+static void e1000_power_up_serdes_link_82575(struct e1000_hw *hw);
+static s32 e1000_set_pcie_completion_timeout(struct e1000_hw *hw);
+static s32 e1000_reset_mdicnfg_82580(struct e1000_hw *hw);
+static s32 e1000_validate_nvm_checksum_82580(struct e1000_hw *hw);
+static s32 e1000_update_nvm_checksum_82580(struct e1000_hw *hw);
+static s32 e1000_update_nvm_checksum_with_offset(struct e1000_hw *hw,
+						 u16 offset);
+static s32 e1000_validate_nvm_checksum_with_offset(struct e1000_hw *hw,
+						   u16 offset);
+static s32 e1000_validate_nvm_checksum_i350(struct e1000_hw *hw);
+static s32 e1000_update_nvm_checksum_i350(struct e1000_hw *hw);
+static void e1000_write_vfta_i350(struct e1000_hw *hw, u32 offset, u32 value);
+static void e1000_clear_vfta_i350(struct e1000_hw *hw);
+
+static void e1000_i2c_start(struct e1000_hw *hw);
+static void e1000_i2c_stop(struct e1000_hw *hw);
+static s32 e1000_clock_in_i2c_byte(struct e1000_hw *hw, u8 *data);
+static s32 e1000_clock_out_i2c_byte(struct e1000_hw *hw, u8 data);
+static s32 e1000_get_i2c_ack(struct e1000_hw *hw);
+static s32 e1000_clock_in_i2c_bit(struct e1000_hw *hw, bool *data);
+static s32 e1000_clock_out_i2c_bit(struct e1000_hw *hw, bool data);
+static void e1000_raise_i2c_clk(struct e1000_hw *hw, u32 *i2cctl);
+static void e1000_lower_i2c_clk(struct e1000_hw *hw, u32 *i2cctl);
+static s32 e1000_set_i2c_data(struct e1000_hw *hw, u32 *i2cctl, bool data);
+static bool e1000_get_i2c_data(u32 *i2cctl);
+
+static const u16 e1000_82580_rxpbs_table[] = {
+	36, 72, 144, 1, 2, 4, 8, 16, 35, 70, 140 };
 #define E1000_82580_RXPBS_TABLE_SIZE \
 	(sizeof(e1000_82580_rxpbs_table)/sizeof(u16))
 
+
 /**
- *  igb_sgmii_uses_mdio_82575 - Determine if I2C pins are for external MDIO
+ *  e1000_sgmii_uses_mdio_82575 - Determine if I2C pins are for external MDIO
  *  @hw: pointer to the HW structure
  *
  *  Called to determine if the I2C pins are being used for I2C or as an
  *  external MDIO interface since the two options are mutually exclusive.
  **/
-static bool igb_sgmii_uses_mdio_82575(struct e1000_hw *hw)
+static bool e1000_sgmii_uses_mdio_82575(struct e1000_hw *hw)
 {
 	u32 reg = 0;
 	bool ext_mdio = false;
 
+	DEBUGFUNC("e1000_sgmii_uses_mdio_82575");
+
 	switch (hw->mac.type) {
 	case e1000_82575:
 	case e1000_82576:
-		reg = rd32(E1000_MDIC);
+		reg = E1000_READ_REG(hw, E1000_MDIC);
 		ext_mdio = !!(reg & E1000_MDIC_DEST);
 		break;
 	case e1000_82580:
 	case e1000_i350:
-		reg = rd32(E1000_MDICNFG);
+	case e1000_i354:
+	case e1000_i210:
+	case e1000_i211:
+		reg = E1000_READ_REG(hw, E1000_MDICNFG);
 		ext_mdio = !!(reg & E1000_MDICNFG_EXT_MDIO);
 		break;
 	default:
@@ -107,280 +150,399 @@ static bool igb_sgmii_uses_mdio_82575(struct e1000_hw *hw)
 	return ext_mdio;
 }
 
-static s32 igb_get_invariants_82575(struct e1000_hw *hw)
+/**
+ *  e1000_init_phy_params_82575 - Init PHY func ptrs.
+ *  @hw: pointer to the HW structure
+ **/
+static s32 e1000_init_phy_params_82575(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
-	struct e1000_nvm_info *nvm = &hw->nvm;
-	struct e1000_mac_info *mac = &hw->mac;
-	struct e1000_dev_spec_82575 * dev_spec = &hw->dev_spec._82575;
-	u32 eecd;
-	s32 ret_val;
-	u16 size;
-	u32 ctrl_ext = 0;
+	s32 ret_val = E1000_SUCCESS;
+	u32 ctrl_ext;
 
-	switch (hw->device_id) {
-	case E1000_DEV_ID_82575EB_COPPER:
-	case E1000_DEV_ID_82575EB_FIBER_SERDES:
-	case E1000_DEV_ID_82575GB_QUAD_COPPER:
-		mac->type = e1000_82575;
-		break;
-	case E1000_DEV_ID_82576:
-	case E1000_DEV_ID_82576_NS:
-	case E1000_DEV_ID_82576_NS_SERDES:
-	case E1000_DEV_ID_82576_FIBER:
-	case E1000_DEV_ID_82576_SERDES:
-	case E1000_DEV_ID_82576_QUAD_COPPER:
-	case E1000_DEV_ID_82576_QUAD_COPPER_ET2:
-	case E1000_DEV_ID_82576_SERDES_QUAD:
-		mac->type = e1000_82576;
-		break;
-	case E1000_DEV_ID_82580_COPPER:
-	case E1000_DEV_ID_82580_FIBER:
-	case E1000_DEV_ID_82580_QUAD_FIBER:
-	case E1000_DEV_ID_82580_SERDES:
-	case E1000_DEV_ID_82580_SGMII:
-	case E1000_DEV_ID_82580_COPPER_DUAL:
-	case E1000_DEV_ID_DH89XXCC_SGMII:
-	case E1000_DEV_ID_DH89XXCC_SERDES:
-	case E1000_DEV_ID_DH89XXCC_BACKPLANE:
-	case E1000_DEV_ID_DH89XXCC_SFP:
-		mac->type = e1000_82580;
-		break;
-	case E1000_DEV_ID_I350_COPPER:
-	case E1000_DEV_ID_I350_FIBER:
-	case E1000_DEV_ID_I350_SERDES:
-	case E1000_DEV_ID_I350_SGMII:
-		mac->type = e1000_i350;
-		break;
-	default:
-		return -E1000_ERR_MAC_INIT;
-		break;
+	DEBUGFUNC("e1000_init_phy_params_82575");
+
+	phy->ops.read_i2c_byte = e1000_read_i2c_byte_generic;
+	phy->ops.write_i2c_byte = e1000_write_i2c_byte_generic;
+
+	if (hw->phy.media_type != e1000_media_type_copper) {
+		phy->type = e1000_phy_none;
+		goto out;
 	}
 
-	/* Set media type */
-	/*
-	 * The 82575 uses bits 22:23 for link mode. The mode can be changed
-	 * based on the EEPROM. We cannot rely upon device ID. There
-	 * is no distinguishable difference between fiber and internal
-	 * SerDes mode on the 82575. There can be an external PHY attached
-	 * on the SGMII interface. For this, we'll set sgmii_active to true.
-	 */
-	phy->media_type = e1000_media_type_copper;
-	dev_spec->sgmii_active = false;
+	phy->ops.power_up   = e1000_power_up_phy_copper;
+	phy->ops.power_down = e1000_power_down_phy_copper_82575;
 
-	ctrl_ext = rd32(E1000_CTRL_EXT);
-	switch (ctrl_ext & E1000_CTRL_EXT_LINK_MODE_MASK) {
-	case E1000_CTRL_EXT_LINK_MODE_SGMII:
-		dev_spec->sgmii_active = true;
-		break;
-	case E1000_CTRL_EXT_LINK_MODE_1000BASE_KX:
-	case E1000_CTRL_EXT_LINK_MODE_PCIE_SERDES:
-		hw->phy.media_type = e1000_media_type_internal_serdes;
-		break;
-	default:
-		break;
+	phy->autoneg_mask	= AUTONEG_ADVERTISE_SPEED_DEFAULT;
+	phy->reset_delay_us	= 100;
+
+	phy->ops.acquire	= e1000_acquire_phy_82575;
+	phy->ops.check_reset_block = e1000_check_reset_block_generic;
+	phy->ops.commit		= e1000_phy_sw_reset_generic;
+	phy->ops.get_cfg_done	= e1000_get_cfg_done_82575;
+	phy->ops.release	= e1000_release_phy_82575;
+
+	ctrl_ext = E1000_READ_REG(hw, E1000_CTRL_EXT);
+
+	if (e1000_sgmii_active_82575(hw)) {
+		phy->ops.reset = e1000_phy_hw_reset_sgmii_82575;
+		ctrl_ext |= E1000_CTRL_I2C_ENA;
+	} else {
+		phy->ops.reset = e1000_phy_hw_reset_generic;
+		ctrl_ext &= ~E1000_CTRL_I2C_ENA;
 	}
 
-	/* Set mta register count */
-	mac->mta_reg_count = 128;
-	/* Set rar entry count */
-	mac->rar_entry_count = E1000_RAR_ENTRIES_82575;
-	if (mac->type == e1000_82576)
-		mac->rar_entry_count = E1000_RAR_ENTRIES_82576;
-	if (mac->type == e1000_82580)
-		mac->rar_entry_count = E1000_RAR_ENTRIES_82580;
-	if (mac->type == e1000_i350)
-		mac->rar_entry_count = E1000_RAR_ENTRIES_I350;
-	/* reset */
-	if (mac->type >= e1000_82580)
-		mac->ops.reset_hw = igb_reset_hw_82580;
-	else
-		mac->ops.reset_hw = igb_reset_hw_82575;
-	/* Set if part includes ASF firmware */
-	mac->asf_firmware_present = true;
-	/* Set if manageability features are enabled. */
-	mac->arc_subsystem_valid =
-		(rd32(E1000_FWSM) & E1000_FWSM_MODE_MASK)
-			? true : false;
-	/* enable EEE on i350 parts */
-	if (mac->type == e1000_i350)
-		dev_spec->eee_disable = false;
-	else
-		dev_spec->eee_disable = true;
-	/* physical interface link setup */
-	mac->ops.setup_physical_interface =
-		(hw->phy.media_type == e1000_media_type_copper)
-			? igb_setup_copper_link_82575
-			: igb_setup_serdes_link_82575;
-
-	/* NVM initialization */
-	eecd = rd32(E1000_EECD);
-
-	nvm->opcode_bits        = 8;
-	nvm->delay_usec         = 1;
-	switch (nvm->override) {
-	case e1000_nvm_override_spi_large:
-		nvm->page_size    = 32;
-		nvm->address_bits = 16;
+	E1000_WRITE_REG(hw, E1000_CTRL_EXT, ctrl_ext);
+	e1000_reset_mdicnfg_82580(hw);
+
+	if (e1000_sgmii_active_82575(hw) && !e1000_sgmii_uses_mdio_82575(hw)) {
+		phy->ops.read_reg = e1000_read_phy_reg_sgmii_82575;
+		phy->ops.write_reg = e1000_write_phy_reg_sgmii_82575;
+	} else {
+		switch (hw->mac.type) {
+		case e1000_82580:
+		case e1000_i350:
+		case e1000_i354:
+			phy->ops.read_reg = e1000_read_phy_reg_82580;
+			phy->ops.write_reg = e1000_write_phy_reg_82580;
+			break;
+		case e1000_i210:
+		case e1000_i211:
+			phy->ops.read_reg = e1000_read_phy_reg_gs40g;
+			phy->ops.write_reg = e1000_write_phy_reg_gs40g;
+			break;
+		default:
+			phy->ops.read_reg = e1000_read_phy_reg_igp;
+			phy->ops.write_reg = e1000_write_phy_reg_igp;
+		}
+	}
+
+	/* Set phy->phy_addr and phy->id. */
+	ret_val = e1000_get_phy_id_82575(hw);
+
+	/* Verify phy id and set remaining function pointers */
+	switch (phy->id) {
+	case M88E1543_E_PHY_ID:
+	case M88E1512_E_PHY_ID:
+	case I347AT4_E_PHY_ID:
+	case M88E1112_E_PHY_ID:
+	case M88E1340M_E_PHY_ID:
+	case M88E1111_I_PHY_ID:
+		phy->type		= e1000_phy_m88;
+		phy->ops.check_polarity	= e1000_check_polarity_m88;
+		phy->ops.get_info	= e1000_get_phy_info_m88;
+		if (phy->id == I347AT4_E_PHY_ID ||
+		    phy->id == M88E1112_E_PHY_ID ||
+		    phy->id == M88E1340M_E_PHY_ID)
+			phy->ops.get_cable_length =
+					 e1000_get_cable_length_m88_gen2;
+		else if (phy->id == M88E1543_E_PHY_ID ||
+			 phy->id == M88E1512_E_PHY_ID)
+			phy->ops.get_cable_length =
+					 e1000_get_cable_length_m88_gen2;
+		else
+			phy->ops.get_cable_length = e1000_get_cable_length_m88;
+		phy->ops.force_speed_duplex = e1000_phy_force_speed_duplex_m88;
+		/* Check if this PHY is confgured for media swap. */
+		if (phy->id == M88E1112_E_PHY_ID) {
+			u16 data;
+
+			ret_val = phy->ops.write_reg(hw,
+						     E1000_M88E1112_PAGE_ADDR,
+						     2);
+			if (ret_val)
+				goto out;
+
+			ret_val = phy->ops.read_reg(hw,
+						    E1000_M88E1112_MAC_CTRL_1,
+						    &data);
+			if (ret_val)
+				goto out;
+
+			data = (data & E1000_M88E1112_MAC_CTRL_1_MODE_MASK) >>
+			       E1000_M88E1112_MAC_CTRL_1_MODE_SHIFT;
+			if (data == E1000_M88E1112_AUTO_COPPER_SGMII ||
+			    data == E1000_M88E1112_AUTO_COPPER_BASEX)
+				hw->mac.ops.check_for_link =
+						e1000_check_for_link_media_swap;
+		}
+		if (phy->id == M88E1512_E_PHY_ID) {
+			ret_val = e1000_initialize_M88E1512_phy(hw);
+			if (ret_val)
+				goto out;
+		}
 		break;
-	case e1000_nvm_override_spi_small:
-		nvm->page_size    = 8;
-		nvm->address_bits = 8;
+	case IGP03E1000_E_PHY_ID:
+	case IGP04E1000_E_PHY_ID:
+		phy->type = e1000_phy_igp_3;
+		phy->ops.check_polarity = e1000_check_polarity_igp;
+		phy->ops.get_info = e1000_get_phy_info_igp;
+		phy->ops.get_cable_length = e1000_get_cable_length_igp_2;
+		phy->ops.force_speed_duplex = e1000_phy_force_speed_duplex_igp;
+		phy->ops.set_d0_lplu_state = e1000_set_d0_lplu_state_82575;
+		phy->ops.set_d3_lplu_state = e1000_set_d3_lplu_state_generic;
 		break;
-	default:
-		nvm->page_size    = eecd & E1000_EECD_ADDR_BITS ? 32 : 8;
-		nvm->address_bits = eecd & E1000_EECD_ADDR_BITS ? 16 : 8;
+	case I82580_I_PHY_ID:
+	case I350_I_PHY_ID:
+		phy->type = e1000_phy_82580;
+		phy->ops.check_polarity = e1000_check_polarity_82577;
+		phy->ops.force_speed_duplex =
+					 e1000_phy_force_speed_duplex_82577;
+		phy->ops.get_cable_length = e1000_get_cable_length_82577;
+		phy->ops.get_info = e1000_get_phy_info_82577;
+		phy->ops.set_d0_lplu_state = e1000_set_d0_lplu_state_82580;
+		phy->ops.set_d3_lplu_state = e1000_set_d3_lplu_state_82580;
+		break;
+	case I210_I_PHY_ID:
+		phy->type		= e1000_phy_i210;
+		phy->ops.check_polarity	= e1000_check_polarity_m88;
+		phy->ops.get_info	= e1000_get_phy_info_m88;
+		phy->ops.get_cable_length = e1000_get_cable_length_m88_gen2;
+		phy->ops.set_d0_lplu_state = e1000_set_d0_lplu_state_82580;
+		phy->ops.set_d3_lplu_state = e1000_set_d3_lplu_state_82580;
+		phy->ops.force_speed_duplex = e1000_phy_force_speed_duplex_m88;
 		break;
+	default:
+		ret_val = -E1000_ERR_PHY;
+		goto out;
 	}
 
-	nvm->type = e1000_nvm_eeprom_spi;
+out:
+	return ret_val;
+}
+
+/**
+ *  e1000_init_nvm_params_82575 - Init NVM func ptrs.
+ *  @hw: pointer to the HW structure
+ **/
+s32 e1000_init_nvm_params_82575(struct e1000_hw *hw)
+{
+	struct e1000_nvm_info *nvm = &hw->nvm;
+	u32 eecd = E1000_READ_REG(hw, E1000_EECD);
+	u16 size;
+
+	DEBUGFUNC("e1000_init_nvm_params_82575");
 
 	size = (u16)((eecd & E1000_EECD_SIZE_EX_MASK) >>
 		     E1000_EECD_SIZE_EX_SHIFT);
-
 	/*
 	 * Added to a constant, "size" becomes the left-shift value
 	 * for setting word_size.
 	 */
 	size += NVM_WORD_SIZE_BASE_SHIFT;
 
-	/*
-	 * Check for invalid size
+	/* Just in case size is out of range, cap it to the largest
+	 * EEPROM size supported
 	 */
-	if ((hw->mac.type == e1000_82576) && (size > 15)) {
-		printk("igb: The NVM size is not valid, "
-			"defaulting to 32K.\n");
+	if (size > 15)
 		size = 15;
-	}
+
 	nvm->word_size = 1 << size;
-	if (nvm->word_size == (1 << 15))
-		nvm->page_size = 128;
+	if (hw->mac.type < e1000_i210) {
+		nvm->opcode_bits = 8;
+		nvm->delay_usec = 1;
+
+		switch (nvm->override) {
+		case e1000_nvm_override_spi_large:
+			nvm->page_size = 32;
+			nvm->address_bits = 16;
+			break;
+		case e1000_nvm_override_spi_small:
+			nvm->page_size = 8;
+			nvm->address_bits = 8;
+			break;
+		default:
+			nvm->page_size = eecd & E1000_EECD_ADDR_BITS ? 32 : 8;
+			nvm->address_bits = eecd & E1000_EECD_ADDR_BITS ?
+					    16 : 8;
+			break;
+		}
+		if (nvm->word_size == (1 << 15))
+			nvm->page_size = 128;
 
-	/* NVM Function Pointers */
-	nvm->ops.acquire = igb_acquire_nvm_82575;
+		nvm->type = e1000_nvm_eeprom_spi;
+	} else {
+		nvm->type = e1000_nvm_flash_hw;
+	}
+
+	/* Function Pointers */
+	nvm->ops.acquire = e1000_acquire_nvm_82575;
+	nvm->ops.release = e1000_release_nvm_82575;
 	if (nvm->word_size < (1 << 15))
-		nvm->ops.read = igb_read_nvm_eerd;
+		nvm->ops.read = e1000_read_nvm_eerd;
 	else
-		nvm->ops.read = igb_read_nvm_spi;
+		nvm->ops.read = e1000_read_nvm_spi;
+
+	nvm->ops.write = e1000_write_nvm_spi;
+	nvm->ops.validate = e1000_validate_nvm_checksum_generic;
+	nvm->ops.update = e1000_update_nvm_checksum_generic;
+	nvm->ops.valid_led_default = e1000_valid_led_default_82575;
 
-	nvm->ops.release = igb_release_nvm_82575;
+	/* override generic family function pointers for specific descendants */
 	switch (hw->mac.type) {
 	case e1000_82580:
-		nvm->ops.validate = igb_validate_nvm_checksum_82580;
-		nvm->ops.update = igb_update_nvm_checksum_82580;
+		nvm->ops.validate = e1000_validate_nvm_checksum_82580;
+		nvm->ops.update = e1000_update_nvm_checksum_82580;
 		break;
 	case e1000_i350:
-		nvm->ops.validate = igb_validate_nvm_checksum_i350;
-		nvm->ops.update = igb_update_nvm_checksum_i350;
-		break;
-	default:
-		nvm->ops.validate = igb_validate_nvm_checksum;
-		nvm->ops.update = igb_update_nvm_checksum;
-	}
-	nvm->ops.write = igb_write_nvm_spi;
-
-	/* if part supports SR-IOV then initialize mailbox parameters */
-	switch (mac->type) {
-	case e1000_82576:
-	case e1000_i350:
-		igb_init_mbx_params_pf(hw);
+	case e1000_i354:
+		nvm->ops.validate = e1000_validate_nvm_checksum_i350;
+		nvm->ops.update = e1000_update_nvm_checksum_i350;
 		break;
 	default:
 		break;
 	}
 
-	/* setup PHY parameters */
-	if (phy->media_type != e1000_media_type_copper) {
-		phy->type = e1000_phy_none;
-		return 0;
-	}
+	return E1000_SUCCESS;
+}
 
-	phy->autoneg_mask        = AUTONEG_ADVERTISE_SPEED_DEFAULT;
-	phy->reset_delay_us      = 100;
+/**
+ *  e1000_init_mac_params_82575 - Init MAC func ptrs.
+ *  @hw: pointer to the HW structure
+ **/
+static s32 e1000_init_mac_params_82575(struct e1000_hw *hw)
+{
+	struct e1000_mac_info *mac = &hw->mac;
+	struct e1000_dev_spec_82575 *dev_spec = &hw->dev_spec._82575;
 
-	ctrl_ext = rd32(E1000_CTRL_EXT);
+	DEBUGFUNC("e1000_init_mac_params_82575");
 
-	/* PHY function pointers */
-	if (igb_sgmii_active_82575(hw)) {
-		phy->ops.reset      = igb_phy_hw_reset_sgmii_82575;
-		ctrl_ext |= E1000_CTRL_I2C_ENA;
-	} else {
-		phy->ops.reset      = igb_phy_hw_reset;
-		ctrl_ext &= ~E1000_CTRL_I2C_ENA;
-	}
+	/* Derives media type */
+	e1000_get_media_type_82575(hw);
+	/* Set mta register count */
+	mac->mta_reg_count = 128;
+	/* Set uta register count */
+	mac->uta_reg_count = (hw->mac.type == e1000_82575) ? 0 : 128;
+	/* Set rar entry count */
+	mac->rar_entry_count = E1000_RAR_ENTRIES_82575;
+	if (mac->type == e1000_82576)
+		mac->rar_entry_count = E1000_RAR_ENTRIES_82576;
+	if (mac->type == e1000_82580)
+		mac->rar_entry_count = E1000_RAR_ENTRIES_82580;
+	if (mac->type == e1000_i350 || mac->type == e1000_i354)
+		mac->rar_entry_count = E1000_RAR_ENTRIES_I350;
 
-	wr32(E1000_CTRL_EXT, ctrl_ext);
-	igb_reset_mdicnfg_82580(hw);
+	/* Enable EEE default settings for EEE supported devices */
+	if (mac->type >= e1000_i350)
+		dev_spec->eee_disable = false;
 
-	if (igb_sgmii_active_82575(hw) && !igb_sgmii_uses_mdio_82575(hw)) {
-		phy->ops.read_reg   = igb_read_phy_reg_sgmii_82575;
-		phy->ops.write_reg  = igb_write_phy_reg_sgmii_82575;
-	} else if (hw->mac.type >= e1000_82580) {
-		phy->ops.read_reg   = igb_read_phy_reg_82580;
-		phy->ops.write_reg  = igb_write_phy_reg_82580;
-	} else {
-		phy->ops.read_reg   = igb_read_phy_reg_igp;
-		phy->ops.write_reg  = igb_write_phy_reg_igp;
-	}
+	/* Allow a single clear of the SW semaphore on I210 and newer */
+	if (mac->type >= e1000_i210)
+		dev_spec->clear_semaphore_once = true;
 
-	/* set lan id */
-	hw->bus.func = (rd32(E1000_STATUS) & E1000_STATUS_FUNC_MASK) >>
-	               E1000_STATUS_FUNC_SHIFT;
+	/* Set if part includes ASF firmware */
+	mac->asf_firmware_present = true;
+	/* FWSM register */
+	mac->has_fwsm = true;
+	/* ARC supported; valid only if manageability features are enabled. */
+	mac->arc_subsystem_valid =
+		!!(E1000_READ_REG(hw, E1000_FWSM) & E1000_FWSM_MODE_MASK);
 
-	/* Set phy->phy_addr and phy->id. */
-	ret_val = igb_get_phy_id_82575(hw);
-	if (ret_val)
-		return ret_val;
+	/* Function pointers */
 
-	/* Verify phy id and set remaining function pointers */
-	switch (phy->id) {
-	case I347AT4_E_PHY_ID:
-	case M88E1112_E_PHY_ID:
-	case M88E1111_I_PHY_ID:
-		phy->type                   = e1000_phy_m88;
-		phy->ops.get_phy_info       = igb_get_phy_info_m88;
+	/* bus type/speed/width */
+	mac->ops.get_bus_info = e1000_get_bus_info_pcie_generic;
+	/* reset */
+	if (mac->type >= e1000_82580)
+		mac->ops.reset_hw = e1000_reset_hw_82580;
+	else
+	mac->ops.reset_hw = e1000_reset_hw_82575;
+	/* hw initialization */
+	mac->ops.init_hw = e1000_init_hw_82575;
+	/* link setup */
+	mac->ops.setup_link = e1000_setup_link_generic;
+	/* physical interface link setup */
+	mac->ops.setup_physical_interface =
+		(hw->phy.media_type == e1000_media_type_copper)
+		? e1000_setup_copper_link_82575 : e1000_setup_serdes_link_82575;
+	/* physical interface shutdown */
+	mac->ops.shutdown_serdes = e1000_shutdown_serdes_link_82575;
+	/* physical interface power up */
+	mac->ops.power_up_serdes = e1000_power_up_serdes_link_82575;
+	/* check for link */
+	mac->ops.check_for_link = e1000_check_for_link_82575;
+	/* read mac address */
+	mac->ops.read_mac_addr = e1000_read_mac_addr_82575;
+	/* configure collision distance */
+	mac->ops.config_collision_dist = e1000_config_collision_dist_82575;
+	/* multicast address update */
+	mac->ops.update_mc_addr_list = e1000_update_mc_addr_list_generic;
+	if (hw->mac.type == e1000_i350 || mac->type == e1000_i354) {
+		/* writing VFTA */
+		mac->ops.write_vfta = e1000_write_vfta_i350;
+		/* clearing VFTA */
+		mac->ops.clear_vfta = e1000_clear_vfta_i350;
+	} else {
+		/* writing VFTA */
+		mac->ops.write_vfta = e1000_write_vfta_generic;
+		/* clearing VFTA */
+		mac->ops.clear_vfta = e1000_clear_vfta_generic;
+	}
+	if (hw->mac.type >= e1000_82580)
+		mac->ops.validate_mdi_setting =
+				e1000_validate_mdi_setting_crossover_generic;
+	/* ID LED init */
+	mac->ops.id_led_init = e1000_id_led_init_generic;
+	/* blink LED */
+	mac->ops.blink_led = e1000_blink_led_generic;
+	/* setup LED */
+	mac->ops.setup_led = e1000_setup_led_generic;
+	/* cleanup LED */
+	mac->ops.cleanup_led = e1000_cleanup_led_generic;
+	/* turn on/off LED */
+	mac->ops.led_on = e1000_led_on_generic;
+	mac->ops.led_off = e1000_led_off_generic;
+	/* clear hardware counters */
+	mac->ops.clear_hw_cntrs = e1000_clear_hw_cntrs_82575;
+	/* link info */
+	mac->ops.get_link_up_info = e1000_get_link_up_info_82575;
+	/* get thermal sensor data */
+	mac->ops.get_thermal_sensor_data =
+				e1000_get_thermal_sensor_data_generic;
+	mac->ops.init_thermal_sensor_thresh =
+				e1000_init_thermal_sensor_thresh_generic;
+	/* acquire SW_FW sync */
+	mac->ops.acquire_swfw_sync = e1000_acquire_swfw_sync_82575;
+	mac->ops.release_swfw_sync = e1000_release_swfw_sync_82575;
+	if (mac->type >= e1000_i210) {
+		mac->ops.acquire_swfw_sync = e1000_acquire_swfw_sync_i210;
+		mac->ops.release_swfw_sync = e1000_release_swfw_sync_i210;
+	}
 
-		if (phy->id == I347AT4_E_PHY_ID ||
-		    phy->id == M88E1112_E_PHY_ID)
-			phy->ops.get_cable_length = igb_get_cable_length_m88_gen2;
-		else
-			phy->ops.get_cable_length = igb_get_cable_length_m88;
+	/* set lan id for port to determine which phy lock to use */
+	hw->mac.ops.set_lan_id(hw);
 
-		phy->ops.force_speed_duplex = igb_phy_force_speed_duplex_m88;
-		break;
-	case IGP03E1000_E_PHY_ID:
-		phy->type                   = e1000_phy_igp_3;
-		phy->ops.get_phy_info       = igb_get_phy_info_igp;
-		phy->ops.get_cable_length   = igb_get_cable_length_igp_2;
-		phy->ops.force_speed_duplex = igb_phy_force_speed_duplex_igp;
-		phy->ops.set_d0_lplu_state  = igb_set_d0_lplu_state_82575;
-		phy->ops.set_d3_lplu_state  = igb_set_d3_lplu_state;
-		break;
-	case I82580_I_PHY_ID:
-	case I350_I_PHY_ID:
-		phy->type                   = e1000_phy_82580;
-		phy->ops.force_speed_duplex = igb_phy_force_speed_duplex_82580;
-		phy->ops.get_cable_length   = igb_get_cable_length_82580;
-		phy->ops.get_phy_info       = igb_get_phy_info_82580;
-		break;
-	default:
-		return -E1000_ERR_PHY;
-	}
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_init_function_pointers_82575 - Init func ptrs.
+ *  @hw: pointer to the HW structure
+ *
+ *  Called to initialize all function pointers and parameters.
+ **/
+void e1000_init_function_pointers_82575(struct e1000_hw *hw)
+{
+	DEBUGFUNC("e1000_init_function_pointers_82575");
 
-	return 0;
+	hw->mac.ops.init_params = e1000_init_mac_params_82575;
+	hw->nvm.ops.init_params = e1000_init_nvm_params_82575;
+	hw->phy.ops.init_params = e1000_init_phy_params_82575;
+	hw->mbx.ops.init_params = e1000_init_mbx_params_pf;
 }
 
 /**
- *  igb_acquire_phy_82575 - Acquire rights to access PHY
+ *  e1000_acquire_phy_82575 - Acquire rights to access PHY
  *  @hw: pointer to the HW structure
  *
- *  Acquire access rights to the correct PHY.  This is a
- *  function pointer entry point called by the api module.
+ *  Acquire access rights to the correct PHY.
  **/
-static s32 igb_acquire_phy_82575(struct e1000_hw *hw)
+static s32 e1000_acquire_phy_82575(struct e1000_hw *hw)
 {
 	u16 mask = E1000_SWFW_PHY0_SM;
 
+	DEBUGFUNC("e1000_acquire_phy_82575");
+
 	if (hw->bus.func == E1000_FUNC_1)
 		mask = E1000_SWFW_PHY1_SM;
 	else if (hw->bus.func == E1000_FUNC_2)
@@ -388,20 +550,21 @@ static s32 igb_acquire_phy_82575(struct e1000_hw *hw)
 	else if (hw->bus.func == E1000_FUNC_3)
 		mask = E1000_SWFW_PHY3_SM;
 
-	return igb_acquire_swfw_sync_82575(hw, mask);
+	return hw->mac.ops.acquire_swfw_sync(hw, mask);
 }
 
 /**
- *  igb_release_phy_82575 - Release rights to access PHY
+ *  e1000_release_phy_82575 - Release rights to access PHY
  *  @hw: pointer to the HW structure
  *
- *  A wrapper to release access rights to the correct PHY.  This is a
- *  function pointer entry point called by the api module.
+ *  A wrapper to release access rights to the correct PHY.
  **/
-static void igb_release_phy_82575(struct e1000_hw *hw)
+static void e1000_release_phy_82575(struct e1000_hw *hw)
 {
 	u16 mask = E1000_SWFW_PHY0_SM;
 
+	DEBUGFUNC("e1000_release_phy_82575");
+
 	if (hw->bus.func == E1000_FUNC_1)
 		mask = E1000_SWFW_PHY1_SM;
 	else if (hw->bus.func == E1000_FUNC_2)
@@ -409,11 +572,11 @@ static void igb_release_phy_82575(struct e1000_hw *hw)
 	else if (hw->bus.func == E1000_FUNC_3)
 		mask = E1000_SWFW_PHY3_SM;
 
-	igb_release_swfw_sync_82575(hw, mask);
+	hw->mac.ops.release_swfw_sync(hw, mask);
 }
 
 /**
- *  igb_read_phy_reg_sgmii_82575 - Read PHY register using sgmii
+ *  e1000_read_phy_reg_sgmii_82575 - Read PHY register using sgmii
  *  @hw: pointer to the HW structure
  *  @offset: register offset to be read
  *  @data: pointer to the read data
@@ -421,13 +584,15 @@ static void igb_release_phy_82575(struct e1000_hw *hw)
  *  Reads the PHY register at offset using the serial gigabit media independent
  *  interface and stores the retrieved information in data.
  **/
-static s32 igb_read_phy_reg_sgmii_82575(struct e1000_hw *hw, u32 offset,
+static s32 e1000_read_phy_reg_sgmii_82575(struct e1000_hw *hw, u32 offset,
 					  u16 *data)
 {
 	s32 ret_val = -E1000_ERR_PARAM;
 
+	DEBUGFUNC("e1000_read_phy_reg_sgmii_82575");
+
 	if (offset > E1000_MAX_SGMII_PHY_REG_ADDR) {
-		hw_dbg("PHY Address %u is out of range\n", offset);
+		DEBUGOUT1("PHY Address %u is out of range\n", offset);
 		goto out;
 	}
 
@@ -435,7 +600,7 @@ static s32 igb_read_phy_reg_sgmii_82575(struct e1000_hw *hw, u32 offset,
 	if (ret_val)
 		goto out;
 
-	ret_val = igb_read_phy_reg_i2c(hw, offset, data);
+	ret_val = e1000_read_phy_reg_i2c(hw, offset, data);
 
 	hw->phy.ops.release(hw);
 
@@ -444,7 +609,7 @@ out:
 }
 
 /**
- *  igb_write_phy_reg_sgmii_82575 - Write PHY register using sgmii
+ *  e1000_write_phy_reg_sgmii_82575 - Write PHY register using sgmii
  *  @hw: pointer to the HW structure
  *  @offset: register offset to write to
  *  @data: data to write at register offset
@@ -452,14 +617,15 @@ out:
  *  Writes the data to PHY register at the offset using the serial gigabit
  *  media independent interface.
  **/
-static s32 igb_write_phy_reg_sgmii_82575(struct e1000_hw *hw, u32 offset,
+static s32 e1000_write_phy_reg_sgmii_82575(struct e1000_hw *hw, u32 offset,
 					   u16 data)
 {
 	s32 ret_val = -E1000_ERR_PARAM;
 
+	DEBUGFUNC("e1000_write_phy_reg_sgmii_82575");
 
 	if (offset > E1000_MAX_SGMII_PHY_REG_ADDR) {
-		hw_dbg("PHY Address %d is out of range\n", offset);
+		DEBUGOUT1("PHY Address %d is out of range\n", offset);
 		goto out;
 	}
 
@@ -467,7 +633,7 @@ static s32 igb_write_phy_reg_sgmii_82575(struct e1000_hw *hw, u32 offset,
 	if (ret_val)
 		goto out;
 
-	ret_val = igb_write_phy_reg_i2c(hw, offset, data);
+	ret_val = e1000_write_phy_reg_i2c(hw, offset, data);
 
 	hw->phy.ops.release(hw);
 
@@ -476,20 +642,26 @@ out:
 }
 
 /**
- *  igb_get_phy_id_82575 - Retrieve PHY addr and id
+ *  e1000_get_phy_id_82575 - Retrieve PHY addr and id
  *  @hw: pointer to the HW structure
  *
  *  Retrieves the PHY address and ID for both PHY's which do and do not use
  *  sgmi interface.
  **/
-static s32 igb_get_phy_id_82575(struct e1000_hw *hw)
+static s32 e1000_get_phy_id_82575(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
-	s32  ret_val = 0;
+	s32  ret_val = E1000_SUCCESS;
 	u16 phy_id;
 	u32 ctrl_ext;
 	u32 mdic;
 
+	DEBUGFUNC("e1000_get_phy_id_82575");
+
+	/* some i354 devices need an extra read for phy id */
+	if (hw->mac.type == e1000_i354)
+		e1000_get_phy_id(hw);
+
 	/*
 	 * For SGMII PHYs, we try the list of possible addresses until
 	 * we find one that works.  For non-SGMII PHYs
@@ -497,23 +669,26 @@ static s32 igb_get_phy_id_82575(struct e1000_hw *hw)
 	 * work.  The result of this function should mean phy->phy_addr
 	 * and phy->id are set correctly.
 	 */
-	if (!(igb_sgmii_active_82575(hw))) {
+	if (!e1000_sgmii_active_82575(hw)) {
 		phy->addr = 1;
-		ret_val = igb_get_phy_id(hw);
+		ret_val = e1000_get_phy_id(hw);
 		goto out;
 	}
 
-	if (igb_sgmii_uses_mdio_82575(hw)) {
+	if (e1000_sgmii_uses_mdio_82575(hw)) {
 		switch (hw->mac.type) {
 		case e1000_82575:
 		case e1000_82576:
-			mdic = rd32(E1000_MDIC);
+			mdic = E1000_READ_REG(hw, E1000_MDIC);
 			mdic &= E1000_MDIC_PHY_MASK;
 			phy->addr = mdic >> E1000_MDIC_PHY_SHIFT;
 			break;
 		case e1000_82580:
 		case e1000_i350:
-			mdic = rd32(E1000_MDICNFG);
+		case e1000_i354:
+		case e1000_i210:
+		case e1000_i211:
+			mdic = E1000_READ_REG(hw, E1000_MDICNFG);
 			mdic &= E1000_MDICNFG_PHY_MASK;
 			phy->addr = mdic >> E1000_MDICNFG_PHY_SHIFT;
 			break;
@@ -522,25 +697,26 @@ static s32 igb_get_phy_id_82575(struct e1000_hw *hw)
 			goto out;
 			break;
 		}
-		ret_val = igb_get_phy_id(hw);
+		ret_val = e1000_get_phy_id(hw);
 		goto out;
 	}
 
 	/* Power on sgmii phy if it is disabled */
-	ctrl_ext = rd32(E1000_CTRL_EXT);
-	wr32(E1000_CTRL_EXT, ctrl_ext & ~E1000_CTRL_EXT_SDP3_DATA);
-	wrfl();
-	msleep(300);
+	ctrl_ext = E1000_READ_REG(hw, E1000_CTRL_EXT);
+	E1000_WRITE_REG(hw, E1000_CTRL_EXT,
+			ctrl_ext & ~E1000_CTRL_EXT_SDP3_DATA);
+	E1000_WRITE_FLUSH(hw);
+	msec_delay(300);
 
 	/*
 	 * The address field in the I2CCMD register is 3 bits and 0 is invalid.
 	 * Therefore, we need to test 1-7
 	 */
 	for (phy->addr = 1; phy->addr < 8; phy->addr++) {
-		ret_val = igb_read_phy_reg_sgmii_82575(hw, PHY_ID1, &phy_id);
-		if (ret_val == 0) {
-			hw_dbg("Vendor ID 0x%08X read at address %u\n",
-			       phy_id, phy->addr);
+		ret_val = e1000_read_phy_reg_sgmii_82575(hw, PHY_ID1, &phy_id);
+		if (ret_val == E1000_SUCCESS) {
+			DEBUGOUT2("Vendor ID 0x%08X read at address %u\n",
+				  phy_id, phy->addr);
 			/*
 			 * At the time of this writing, The M88 part is
 			 * the only supported SGMII PHY product.
@@ -548,7 +724,8 @@ static s32 igb_get_phy_id_82575(struct e1000_hw *hw)
 			if (phy_id == M88_VENDOR)
 				break;
 		} else {
-			hw_dbg("PHY address %u was unreadable\n", phy->addr);
+			DEBUGOUT1("PHY address %u was unreadable\n",
+				  phy->addr);
 		}
 	}
 
@@ -556,34 +733,39 @@ static s32 igb_get_phy_id_82575(struct e1000_hw *hw)
 	if (phy->addr == 8) {
 		phy->addr = 0;
 		ret_val = -E1000_ERR_PHY;
-		goto out;
 	} else {
-		ret_val = igb_get_phy_id(hw);
+		ret_val = e1000_get_phy_id(hw);
 	}
 
 	/* restore previous sfp cage power state */
-	wr32(E1000_CTRL_EXT, ctrl_ext);
+	E1000_WRITE_REG(hw, E1000_CTRL_EXT, ctrl_ext);
 
 out:
 	return ret_val;
 }
 
 /**
- *  igb_phy_hw_reset_sgmii_82575 - Performs a PHY reset
+ *  e1000_phy_hw_reset_sgmii_82575 - Performs a PHY reset
  *  @hw: pointer to the HW structure
  *
  *  Resets the PHY using the serial gigabit media independent interface.
  **/
-static s32 igb_phy_hw_reset_sgmii_82575(struct e1000_hw *hw)
+static s32 e1000_phy_hw_reset_sgmii_82575(struct e1000_hw *hw)
 {
-	s32 ret_val;
+	s32 ret_val = E1000_SUCCESS;
+	struct e1000_phy_info *phy = &hw->phy;
+
+	DEBUGFUNC("e1000_phy_hw_reset_sgmii_82575");
 
 	/*
 	 * This isn't a true "hard" reset, but is the only reset
 	 * available to us at this time.
 	 */
 
-	hw_dbg("Soft resetting SGMII attached PHY...\n");
+	DEBUGOUT("Soft resetting SGMII attached PHY...\n");
+
+	if (!(hw->phy.ops.write_reg))
+		goto out;
 
 	/*
 	 * SFP documentation requires the following to configure the SPF module
@@ -593,14 +775,18 @@ static s32 igb_phy_hw_reset_sgmii_82575(struct e1000_hw *hw)
 	if (ret_val)
 		goto out;
 
-	ret_val = igb_phy_sw_reset(hw);
+	ret_val = hw->phy.ops.commit(hw);
+	if (ret_val)
+		goto out;
 
+	if (phy->id == M88E1512_E_PHY_ID)
+		ret_val = e1000_initialize_M88E1512_phy(hw);
 out:
 	return ret_val;
 }
 
 /**
- *  igb_set_d0_lplu_state_82575 - Set Low Power Linkup D0 state
+ *  e1000_set_d0_lplu_state_82575 - Set Low Power Linkup D0 state
  *  @hw: pointer to the HW structure
  *  @active: true to enable LPLU, false to disable
  *
@@ -612,12 +798,17 @@ out:
  *  This is a function pointer entry point only called by
  *  PHY setup routines.
  **/
-static s32 igb_set_d0_lplu_state_82575(struct e1000_hw *hw, bool active)
+static s32 e1000_set_d0_lplu_state_82575(struct e1000_hw *hw, bool active)
 {
 	struct e1000_phy_info *phy = &hw->phy;
-	s32 ret_val;
+	s32 ret_val = E1000_SUCCESS;
 	u16 data;
 
+	DEBUGFUNC("e1000_set_d0_lplu_state_82575");
+
+	if (!(hw->phy.ops.read_reg))
+		goto out;
+
 	ret_val = phy->ops.read_reg(hw, IGP02E1000_PHY_POWER_MGMT, &data);
 	if (ret_val)
 		goto out;
@@ -625,22 +816,22 @@ static s32 igb_set_d0_lplu_state_82575(struct e1000_hw *hw, bool active)
 	if (active) {
 		data |= IGP02E1000_PM_D0_LPLU;
 		ret_val = phy->ops.write_reg(hw, IGP02E1000_PHY_POWER_MGMT,
-						 data);
+					     data);
 		if (ret_val)
 			goto out;
 
 		/* When LPLU is enabled, we should disable SmartSpeed */
 		ret_val = phy->ops.read_reg(hw, IGP01E1000_PHY_PORT_CONFIG,
-						&data);
+					    &data);
 		data &= ~IGP01E1000_PSCFR_SMART_SPEED;
 		ret_val = phy->ops.write_reg(hw, IGP01E1000_PHY_PORT_CONFIG,
-						 data);
+					     data);
 		if (ret_val)
 			goto out;
 	} else {
 		data &= ~IGP02E1000_PM_D0_LPLU;
 		ret_val = phy->ops.write_reg(hw, IGP02E1000_PHY_POWER_MGMT,
-						 data);
+					     data);
 		/*
 		 * LPLU and SmartSpeed are mutually exclusive.  LPLU is used
 		 * during Dx states where the power conservation is most
@@ -649,24 +840,28 @@ static s32 igb_set_d0_lplu_state_82575(struct e1000_hw *hw, bool active)
 		 */
 		if (phy->smart_speed == e1000_smart_speed_on) {
 			ret_val = phy->ops.read_reg(hw,
-					IGP01E1000_PHY_PORT_CONFIG, &data);
+						    IGP01E1000_PHY_PORT_CONFIG,
+						    &data);
 			if (ret_val)
 				goto out;
 
 			data |= IGP01E1000_PSCFR_SMART_SPEED;
 			ret_val = phy->ops.write_reg(hw,
-					IGP01E1000_PHY_PORT_CONFIG, data);
+						     IGP01E1000_PHY_PORT_CONFIG,
+						     data);
 			if (ret_val)
 				goto out;
 		} else if (phy->smart_speed == e1000_smart_speed_off) {
 			ret_val = phy->ops.read_reg(hw,
-					IGP01E1000_PHY_PORT_CONFIG, &data);
+						    IGP01E1000_PHY_PORT_CONFIG,
+						    &data);
 			if (ret_val)
 				goto out;
 
 			data &= ~IGP01E1000_PSCFR_SMART_SPEED;
 			ret_val = phy->ops.write_reg(hw,
-					IGP01E1000_PHY_PORT_CONFIG, data);
+						     IGP01E1000_PHY_PORT_CONFIG,
+						     data);
 			if (ret_val)
 				goto out;
 		}
@@ -677,7 +872,102 @@ out:
 }
 
 /**
- *  igb_acquire_nvm_82575 - Request for access to EEPROM
+ *  e1000_set_d0_lplu_state_82580 - Set Low Power Linkup D0 state
+ *  @hw: pointer to the HW structure
+ *  @active: true to enable LPLU, false to disable
+ *
+ *  Sets the LPLU D0 state according to the active flag.  When
+ *  activating LPLU this function also disables smart speed
+ *  and vice versa.  LPLU will not be activated unless the
+ *  device autonegotiation advertisement meets standards of
+ *  either 10 or 10/100 or 10/100/1000 at all duplexes.
+ *  This is a function pointer entry point only called by
+ *  PHY setup routines.
+ **/
+static s32 e1000_set_d0_lplu_state_82580(struct e1000_hw *hw, bool active)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	s32 ret_val = E1000_SUCCESS;
+	u32 data;
+
+	DEBUGFUNC("e1000_set_d0_lplu_state_82580");
+
+	data = E1000_READ_REG(hw, E1000_82580_PHY_POWER_MGMT);
+
+	if (active) {
+		data |= E1000_82580_PM_D0_LPLU;
+
+		/* When LPLU is enabled, we should disable SmartSpeed */
+		data &= ~E1000_82580_PM_SPD;
+	} else {
+		data &= ~E1000_82580_PM_D0_LPLU;
+
+		/*
+		 * LPLU and SmartSpeed are mutually exclusive.  LPLU is used
+		 * during Dx states where the power conservation is most
+		 * important.  During driver activity we should enable
+		 * SmartSpeed, so performance is maintained.
+		 */
+		if (phy->smart_speed == e1000_smart_speed_on)
+			data |= E1000_82580_PM_SPD;
+		else if (phy->smart_speed == e1000_smart_speed_off)
+			data &= ~E1000_82580_PM_SPD;
+	}
+
+	E1000_WRITE_REG(hw, E1000_82580_PHY_POWER_MGMT, data);
+	return ret_val;
+}
+
+/**
+ *  e1000_set_d3_lplu_state_82580 - Sets low power link up state for D3
+ *  @hw: pointer to the HW structure
+ *  @active: boolean used to enable/disable lplu
+ *
+ *  Success returns 0, Failure returns 1
+ *
+ *  The low power link up (lplu) state is set to the power management level D3
+ *  and SmartSpeed is disabled when active is true, else clear lplu for D3
+ *  and enable Smartspeed.  LPLU and Smartspeed are mutually exclusive.  LPLU
+ *  is used during Dx states where the power conservation is most important.
+ *  During driver activity, SmartSpeed should be enabled so performance is
+ *  maintained.
+ **/
+s32 e1000_set_d3_lplu_state_82580(struct e1000_hw *hw, bool active)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	s32 ret_val = E1000_SUCCESS;
+	u32 data;
+
+	DEBUGFUNC("e1000_set_d3_lplu_state_82580");
+
+	data = E1000_READ_REG(hw, E1000_82580_PHY_POWER_MGMT);
+
+	if (!active) {
+		data &= ~E1000_82580_PM_D3_LPLU;
+		/*
+		 * LPLU and SmartSpeed are mutually exclusive.  LPLU is used
+		 * during Dx states where the power conservation is most
+		 * important.  During driver activity we should enable
+		 * SmartSpeed, so performance is maintained.
+		 */
+		if (phy->smart_speed == e1000_smart_speed_on)
+			data |= E1000_82580_PM_SPD;
+		else if (phy->smart_speed == e1000_smart_speed_off)
+			data &= ~E1000_82580_PM_SPD;
+	} else if ((phy->autoneg_advertised == E1000_ALL_SPEED_DUPLEX) ||
+		   (phy->autoneg_advertised == E1000_ALL_NOT_GIG) ||
+		   (phy->autoneg_advertised == E1000_ALL_10_SPEED)) {
+		data |= E1000_82580_PM_D3_LPLU;
+		/* When LPLU is enabled, we should disable SmartSpeed */
+		data &= ~E1000_82580_PM_SPD;
+	}
+
+	E1000_WRITE_REG(hw, E1000_82580_PHY_POWER_MGMT, data);
+	return ret_val;
+}
+
+/**
+ *  e1000_acquire_nvm_82575 - Request for access to EEPROM
  *  @hw: pointer to the HW structure
  *
  *  Acquire the necessary semaphores for exclusive access to the EEPROM.
@@ -685,59 +975,90 @@ out:
  *  Return successful if access grant bit set, else clear the request for
  *  EEPROM access and return -E1000_ERR_NVM (-1).
  **/
-static s32 igb_acquire_nvm_82575(struct e1000_hw *hw)
+static s32 e1000_acquire_nvm_82575(struct e1000_hw *hw)
 {
 	s32 ret_val;
 
-	ret_val = igb_acquire_swfw_sync_82575(hw, E1000_SWFW_EEP_SM);
+	DEBUGFUNC("e1000_acquire_nvm_82575");
+
+	ret_val = e1000_acquire_swfw_sync_82575(hw, E1000_SWFW_EEP_SM);
 	if (ret_val)
 		goto out;
 
-	ret_val = igb_acquire_nvm(hw);
+	/*
+	 * Check if there is some access
+	 * error this access may hook on
+	 */
+	if (hw->mac.type == e1000_i350) {
+		u32 eecd = E1000_READ_REG(hw, E1000_EECD);
+		if (eecd & (E1000_EECD_BLOCKED | E1000_EECD_ABORT |
+		    E1000_EECD_TIMEOUT)) {
+			/* Clear all access error flags */
+			E1000_WRITE_REG(hw, E1000_EECD, eecd |
+					E1000_EECD_ERROR_CLR);
+			DEBUGOUT("Nvm bit banging access error detected and cleared.\n");
+		}
+	}
+	if (hw->mac.type == e1000_82580) {
+		u32 eecd = E1000_READ_REG(hw, E1000_EECD);
+		if (eecd & E1000_EECD_BLOCKED) {
+			/* Clear access error flag */
+			E1000_WRITE_REG(hw, E1000_EECD, eecd |
+					E1000_EECD_BLOCKED);
+			DEBUGOUT("Nvm bit banging access error detected and cleared.\n");
+		}
+	}
+
 
+	ret_val = e1000_acquire_nvm_generic(hw);
 	if (ret_val)
-		igb_release_swfw_sync_82575(hw, E1000_SWFW_EEP_SM);
+		e1000_release_swfw_sync_82575(hw, E1000_SWFW_EEP_SM);
 
 out:
 	return ret_val;
 }
 
 /**
- *  igb_release_nvm_82575 - Release exclusive access to EEPROM
+ *  e1000_release_nvm_82575 - Release exclusive access to EEPROM
  *  @hw: pointer to the HW structure
  *
  *  Stop any current commands to the EEPROM and clear the EEPROM request bit,
  *  then release the semaphores acquired.
  **/
-static void igb_release_nvm_82575(struct e1000_hw *hw)
+static void e1000_release_nvm_82575(struct e1000_hw *hw)
 {
-	igb_release_nvm(hw);
-	igb_release_swfw_sync_82575(hw, E1000_SWFW_EEP_SM);
+	DEBUGFUNC("e1000_release_nvm_82575");
+
+	e1000_release_nvm_generic(hw);
+
+	e1000_release_swfw_sync_82575(hw, E1000_SWFW_EEP_SM);
 }
 
 /**
- *  igb_acquire_swfw_sync_82575 - Acquire SW/FW semaphore
+ *  e1000_acquire_swfw_sync_82575 - Acquire SW/FW semaphore
  *  @hw: pointer to the HW structure
  *  @mask: specifies which semaphore to acquire
  *
  *  Acquire the SW/FW semaphore to access the PHY or NVM.  The mask
  *  will also specify which port we're acquiring the lock for.
  **/
-static s32 igb_acquire_swfw_sync_82575(struct e1000_hw *hw, u16 mask)
+static s32 e1000_acquire_swfw_sync_82575(struct e1000_hw *hw, u16 mask)
 {
 	u32 swfw_sync;
 	u32 swmask = mask;
 	u32 fwmask = mask << 16;
-	s32 ret_val = 0;
+	s32 ret_val = E1000_SUCCESS;
 	s32 i = 0, timeout = 200; /* FIXME: find real value to use here */
 
+	DEBUGFUNC("e1000_acquire_swfw_sync_82575");
+
 	while (i < timeout) {
-		if (igb_get_hw_semaphore(hw)) {
+		if (e1000_get_hw_semaphore_generic(hw)) {
 			ret_val = -E1000_ERR_SWFW_SYNC;
 			goto out;
 		}
 
-		swfw_sync = rd32(E1000_SW_FW_SYNC);
+		swfw_sync = E1000_READ_REG(hw, E1000_SW_FW_SYNC);
 		if (!(swfw_sync & (fwmask | swmask)))
 			break;
 
@@ -745,146 +1066,244 @@ static s32 igb_acquire_swfw_sync_82575(struct e1000_hw *hw, u16 mask)
 		 * Firmware currently using resource (fwmask)
 		 * or other software thread using resource (swmask)
 		 */
-		igb_put_hw_semaphore(hw);
-		mdelay(5);
+		e1000_put_hw_semaphore_generic(hw);
+		msec_delay_irq(5);
 		i++;
 	}
 
 	if (i == timeout) {
-		hw_dbg("Driver can't access resource, SW_FW_SYNC timeout.\n");
+		DEBUGOUT("Driver can't access resource, SW_FW_SYNC timeout.\n");
 		ret_val = -E1000_ERR_SWFW_SYNC;
 		goto out;
 	}
 
 	swfw_sync |= swmask;
-	wr32(E1000_SW_FW_SYNC, swfw_sync);
+	E1000_WRITE_REG(hw, E1000_SW_FW_SYNC, swfw_sync);
 
-	igb_put_hw_semaphore(hw);
+	e1000_put_hw_semaphore_generic(hw);
 
 out:
 	return ret_val;
 }
 
 /**
- *  igb_release_swfw_sync_82575 - Release SW/FW semaphore
+ *  e1000_release_swfw_sync_82575 - Release SW/FW semaphore
  *  @hw: pointer to the HW structure
  *  @mask: specifies which semaphore to acquire
  *
  *  Release the SW/FW semaphore used to access the PHY or NVM.  The mask
  *  will also specify which port we're releasing the lock for.
  **/
-static void igb_release_swfw_sync_82575(struct e1000_hw *hw, u16 mask)
+static void e1000_release_swfw_sync_82575(struct e1000_hw *hw, u16 mask)
 {
 	u32 swfw_sync;
 
-	while (igb_get_hw_semaphore(hw) != 0);
-	/* Empty */
+	DEBUGFUNC("e1000_release_swfw_sync_82575");
+
+	while (e1000_get_hw_semaphore_generic(hw) != E1000_SUCCESS)
+		; /* Empty */
 
-	swfw_sync = rd32(E1000_SW_FW_SYNC);
+	swfw_sync = E1000_READ_REG(hw, E1000_SW_FW_SYNC);
 	swfw_sync &= ~mask;
-	wr32(E1000_SW_FW_SYNC, swfw_sync);
+	E1000_WRITE_REG(hw, E1000_SW_FW_SYNC, swfw_sync);
 
-	igb_put_hw_semaphore(hw);
+	e1000_put_hw_semaphore_generic(hw);
 }
 
 /**
- *  igb_get_cfg_done_82575 - Read config done bit
+ *  e1000_get_cfg_done_82575 - Read config done bit
  *  @hw: pointer to the HW structure
  *
  *  Read the management control register for the config done bit for
  *  completion status.  NOTE: silicon which is EEPROM-less will fail trying
  *  to read the config done bit, so an error is *ONLY* logged and returns
- *  0.  If we were to return with error, EEPROM-less silicon
+ *  E1000_SUCCESS.  If we were to return with error, EEPROM-less silicon
  *  would not be able to be reset or change link.
  **/
-static s32 igb_get_cfg_done_82575(struct e1000_hw *hw)
+static s32 e1000_get_cfg_done_82575(struct e1000_hw *hw)
 {
 	s32 timeout = PHY_CFG_TIMEOUT;
-	s32 ret_val = 0;
+	s32 ret_val = E1000_SUCCESS;
 	u32 mask = E1000_NVM_CFG_DONE_PORT_0;
 
-	if (hw->bus.func == 1)
+	DEBUGFUNC("e1000_get_cfg_done_82575");
+
+	if (hw->bus.func == E1000_FUNC_1)
 		mask = E1000_NVM_CFG_DONE_PORT_1;
 	else if (hw->bus.func == E1000_FUNC_2)
 		mask = E1000_NVM_CFG_DONE_PORT_2;
 	else if (hw->bus.func == E1000_FUNC_3)
 		mask = E1000_NVM_CFG_DONE_PORT_3;
-
 	while (timeout) {
-		if (rd32(E1000_EEMNGCTL) & mask)
+		if (E1000_READ_REG(hw, E1000_EEMNGCTL) & mask)
 			break;
-		msleep(1);
+		msec_delay(1);
 		timeout--;
 	}
 	if (!timeout)
-		hw_dbg("MNG configuration cycle has not completed.\n");
+		DEBUGOUT("MNG configuration cycle has not completed.\n");
 
 	/* If EEPROM is not marked present, init the PHY manually */
-	if (((rd32(E1000_EECD) & E1000_EECD_PRES) == 0) &&
+	if (!(E1000_READ_REG(hw, E1000_EECD) & E1000_EECD_PRES) &&
 	    (hw->phy.type == e1000_phy_igp_3))
-		igb_phy_init_script_igp3(hw);
+		e1000_phy_init_script_igp3(hw);
+
+	return ret_val;
+}
+
+/**
+ *  e1000_get_link_up_info_82575 - Get link speed/duplex info
+ *  @hw: pointer to the HW structure
+ *  @speed: stores the current speed
+ *  @duplex: stores the current duplex
+ *
+ *  This is a wrapper function, if using the serial gigabit media independent
+ *  interface, use PCS to retrieve the link speed and duplex information.
+ *  Otherwise, use the generic function to get the link speed and duplex info.
+ **/
+static s32 e1000_get_link_up_info_82575(struct e1000_hw *hw, u16 *speed,
+					u16 *duplex)
+{
+	s32 ret_val;
+
+	DEBUGFUNC("e1000_get_link_up_info_82575");
+
+	if (hw->phy.media_type != e1000_media_type_copper)
+		ret_val = e1000_get_pcs_speed_and_duplex_82575(hw, speed,
+							       duplex);
+	else
+		ret_val = e1000_get_speed_and_duplex_copper_generic(hw, speed,
+								    duplex);
 
 	return ret_val;
 }
 
 /**
- *  igb_check_for_link_82575 - Check for link
+ *  e1000_check_for_link_82575 - Check for link
  *  @hw: pointer to the HW structure
  *
  *  If sgmii is enabled, then use the pcs register to determine link, otherwise
  *  use the generic interface for determining link.
  **/
-static s32 igb_check_for_link_82575(struct e1000_hw *hw)
+static s32 e1000_check_for_link_82575(struct e1000_hw *hw)
 {
 	s32 ret_val;
 	u16 speed, duplex;
 
+	DEBUGFUNC("e1000_check_for_link_82575");
+
 	if (hw->phy.media_type != e1000_media_type_copper) {
-		ret_val = igb_get_pcs_speed_and_duplex_82575(hw, &speed,
-		                                             &duplex);
+		ret_val = e1000_get_pcs_speed_and_duplex_82575(hw, &speed,
+							       &duplex);
 		/*
 		 * Use this flag to determine if link needs to be checked or
-		 * not.  If  we have link clear the flag so that we do not
+		 * not.  If we have link clear the flag so that we do not
 		 * continue to check for link.
 		 */
 		hw->mac.get_link_status = !hw->mac.serdes_has_link;
+
+		/*
+		 * Configure Flow Control now that Auto-Neg has completed.
+		 * First, we need to restore the desired flow control
+		 * settings because we may have had to re-autoneg with a
+		 * different link partner.
+		 */
+		ret_val = e1000_config_fc_after_link_up_generic(hw);
+		if (ret_val)
+			DEBUGOUT("Error configuring flow control\n");
 	} else {
-		ret_val = igb_check_for_copper_link(hw);
+		ret_val = e1000_check_for_copper_link_generic(hw);
 	}
 
 	return ret_val;
 }
 
 /**
- *  igb_power_up_serdes_link_82575 - Power up the serdes link after shutdown
+ *  e1000_check_for_link_media_swap - Check which M88E1112 interface linked
+ *  @hw: pointer to the HW structure
+ *
+ *  Poll the M88E1112 interfaces to see which interface achieved link.
+ */
+static s32 e1000_check_for_link_media_swap(struct e1000_hw *hw)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	s32 ret_val;
+	u16 data;
+	u8 port = 0;
+
+	DEBUGFUNC("e1000_check_for_link_media_swap");
+
+	/* Check the copper medium. */
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1112_PAGE_ADDR, 0);
+	if (ret_val)
+		return ret_val;
+
+	ret_val = phy->ops.read_reg(hw, E1000_M88E1112_STATUS, &data);
+	if (ret_val)
+		return ret_val;
+
+	if (data & E1000_M88E1112_STATUS_LINK)
+		port = E1000_MEDIA_PORT_COPPER;
+
+	/* Check the other medium. */
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1112_PAGE_ADDR, 1);
+	if (ret_val)
+		return ret_val;
+
+	ret_val = phy->ops.read_reg(hw, E1000_M88E1112_STATUS, &data);
+	if (ret_val)
+		return ret_val;
+
+	/* reset page to 0 */
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1112_PAGE_ADDR, 0);
+	if (ret_val)
+		return ret_val;
+
+	if (data & E1000_M88E1112_STATUS_LINK)
+		port = E1000_MEDIA_PORT_OTHER;
+
+	/* Determine if a swap needs to happen. */
+	if (port && (hw->dev_spec._82575.media_port != port)) {
+		hw->dev_spec._82575.media_port = port;
+		hw->dev_spec._82575.media_changed = true;
+	} else {
+		ret_val = e1000_check_for_link_82575(hw);
+	}
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_power_up_serdes_link_82575 - Power up the serdes link after shutdown
  *  @hw: pointer to the HW structure
  **/
-void igb_power_up_serdes_link_82575(struct e1000_hw *hw)
+static void e1000_power_up_serdes_link_82575(struct e1000_hw *hw)
 {
 	u32 reg;
 
+	DEBUGFUNC("e1000_power_up_serdes_link_82575");
 
 	if ((hw->phy.media_type != e1000_media_type_internal_serdes) &&
-	    !igb_sgmii_active_82575(hw))
+	    !e1000_sgmii_active_82575(hw))
 		return;
 
 	/* Enable PCS to turn on link */
-	reg = rd32(E1000_PCS_CFG0);
+	reg = E1000_READ_REG(hw, E1000_PCS_CFG0);
 	reg |= E1000_PCS_CFG_PCS_EN;
-	wr32(E1000_PCS_CFG0, reg);
+	E1000_WRITE_REG(hw, E1000_PCS_CFG0, reg);
 
 	/* Power up the laser */
-	reg = rd32(E1000_CTRL_EXT);
+	reg = E1000_READ_REG(hw, E1000_CTRL_EXT);
 	reg &= ~E1000_CTRL_EXT_SDP3_DATA;
-	wr32(E1000_CTRL_EXT, reg);
+	E1000_WRITE_REG(hw, E1000_CTRL_EXT, reg);
 
 	/* flush the write to verify completion */
-	wrfl();
-	msleep(1);
+	E1000_WRITE_FLUSH(hw);
+	msec_delay(1);
 }
 
 /**
- *  igb_get_pcs_speed_and_duplex_82575 - Retrieve current speed/duplex
+ *  e1000_get_pcs_speed_and_duplex_82575 - Retrieve current speed/duplex
  *  @hw: pointer to the HW structure
  *  @speed: stores the current speed
  *  @duplex: stores the current duplex
@@ -892,186 +1311,206 @@ void igb_power_up_serdes_link_82575(struct e1000_hw *hw)
  *  Using the physical coding sub-layer (PCS), retrieve the current speed and
  *  duplex, then store the values in the pointers provided.
  **/
-static s32 igb_get_pcs_speed_and_duplex_82575(struct e1000_hw *hw, u16 *speed,
-						u16 *duplex)
+static s32 e1000_get_pcs_speed_and_duplex_82575(struct e1000_hw *hw,
+						u16 *speed, u16 *duplex)
 {
 	struct e1000_mac_info *mac = &hw->mac;
 	u32 pcs;
+	u32 status;
 
-	/* Set up defaults for the return values of this function */
-	mac->serdes_has_link = false;
-	*speed = 0;
-	*duplex = 0;
+	DEBUGFUNC("e1000_get_pcs_speed_and_duplex_82575");
 
 	/*
 	 * Read the PCS Status register for link state. For non-copper mode,
 	 * the status register is not accurate. The PCS status register is
 	 * used instead.
 	 */
-	pcs = rd32(E1000_PCS_LSTAT);
+	pcs = E1000_READ_REG(hw, E1000_PCS_LSTAT);
 
 	/*
-	 * The link up bit determines when link is up on autoneg. The sync ok
-	 * gets set once both sides sync up and agree upon link. Stable link
-	 * can be determined by checking for both link up and link sync ok
+	 * The link up bit determines when link is up on autoneg.
 	 */
-	if ((pcs & E1000_PCS_LSTS_LINK_OK) && (pcs & E1000_PCS_LSTS_SYNK_OK)) {
+	if (pcs & E1000_PCS_LSTS_LINK_OK) {
 		mac->serdes_has_link = true;
 
 		/* Detect and store PCS speed */
-		if (pcs & E1000_PCS_LSTS_SPEED_1000) {
+		if (pcs & E1000_PCS_LSTS_SPEED_1000)
 			*speed = SPEED_1000;
-		} else if (pcs & E1000_PCS_LSTS_SPEED_100) {
+		else if (pcs & E1000_PCS_LSTS_SPEED_100)
 			*speed = SPEED_100;
-		} else {
+		else
 			*speed = SPEED_10;
-		}
 
 		/* Detect and store PCS duplex */
-		if (pcs & E1000_PCS_LSTS_DUPLEX_FULL) {
+		if (pcs & E1000_PCS_LSTS_DUPLEX_FULL)
 			*duplex = FULL_DUPLEX;
-		} else {
+		else
 			*duplex = HALF_DUPLEX;
+
+		/* Check if it is an I354 2.5Gb backplane connection. */
+		if (mac->type == e1000_i354) {
+			status = E1000_READ_REG(hw, E1000_STATUS);
+			if ((status & E1000_STATUS_2P5_SKU) &&
+			    !(status & E1000_STATUS_2P5_SKU_OVER)) {
+				*speed = SPEED_2500;
+				*duplex = FULL_DUPLEX;
+				DEBUGOUT("2500 Mbs, ");
+				DEBUGOUT("Full Duplex\n");
+			}
 		}
+
+	} else {
+		mac->serdes_has_link = false;
+		*speed = 0;
+		*duplex = 0;
 	}
 
-	return 0;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_shutdown_serdes_link_82575 - Remove link during power down
+ *  e1000_shutdown_serdes_link_82575 - Remove link during power down
  *  @hw: pointer to the HW structure
  *
- *  In the case of fiber serdes, shut down optics and PCS on driver unload
+ *  In the case of serdes shut down sfp and PCS on driver unload
  *  when management pass thru is not enabled.
  **/
-void igb_shutdown_serdes_link_82575(struct e1000_hw *hw)
+void e1000_shutdown_serdes_link_82575(struct e1000_hw *hw)
 {
 	u32 reg;
 
-	if (hw->phy.media_type != e1000_media_type_internal_serdes &&
-	    igb_sgmii_active_82575(hw))
+	DEBUGFUNC("e1000_shutdown_serdes_link_82575");
+
+	if ((hw->phy.media_type != e1000_media_type_internal_serdes) &&
+	    !e1000_sgmii_active_82575(hw))
 		return;
 
-	if (!igb_enable_mng_pass_thru(hw)) {
+	if (!e1000_enable_mng_pass_thru(hw)) {
 		/* Disable PCS to turn off link */
-		reg = rd32(E1000_PCS_CFG0);
+		reg = E1000_READ_REG(hw, E1000_PCS_CFG0);
 		reg &= ~E1000_PCS_CFG_PCS_EN;
-		wr32(E1000_PCS_CFG0, reg);
+		E1000_WRITE_REG(hw, E1000_PCS_CFG0, reg);
 
 		/* shutdown the laser */
-		reg = rd32(E1000_CTRL_EXT);
+		reg = E1000_READ_REG(hw, E1000_CTRL_EXT);
 		reg |= E1000_CTRL_EXT_SDP3_DATA;
-		wr32(E1000_CTRL_EXT, reg);
+		E1000_WRITE_REG(hw, E1000_CTRL_EXT, reg);
 
 		/* flush the write to verify completion */
-		wrfl();
-		msleep(1);
+		E1000_WRITE_FLUSH(hw);
+		msec_delay(1);
 	}
+
+	return;
 }
 
 /**
- *  igb_reset_hw_82575 - Reset hardware
+ *  e1000_reset_hw_82575 - Reset hardware
  *  @hw: pointer to the HW structure
  *
- *  This resets the hardware into a known state.  This is a
- *  function pointer entry point called by the api module.
+ *  This resets the hardware into a known state.
  **/
-static s32 igb_reset_hw_82575(struct e1000_hw *hw)
+static s32 e1000_reset_hw_82575(struct e1000_hw *hw)
 {
-	u32 ctrl, icr;
+	u32 ctrl;
 	s32 ret_val;
 
+	DEBUGFUNC("e1000_reset_hw_82575");
+
 	/*
 	 * Prevent the PCI-E bus from sticking if there is no TLP connection
 	 * on the last TLP read/write transaction when MAC is reset.
 	 */
-	ret_val = igb_disable_pcie_master(hw);
+	ret_val = e1000_disable_pcie_master_generic(hw);
 	if (ret_val)
-		hw_dbg("PCI-E Master disable polling has failed.\n");
+		DEBUGOUT("PCI-E Master disable polling has failed.\n");
 
 	/* set the completion timeout for interface */
-	ret_val = igb_set_pcie_completion_timeout(hw);
-	if (ret_val) {
-		hw_dbg("PCI-E Set completion timeout has failed.\n");
-	}
+	ret_val = e1000_set_pcie_completion_timeout(hw);
+	if (ret_val)
+		DEBUGOUT("PCI-E Set completion timeout has failed.\n");
 
-	hw_dbg("Masking off all interrupts\n");
-	wr32(E1000_IMC, 0xffffffff);
+	DEBUGOUT("Masking off all interrupts\n");
+	E1000_WRITE_REG(hw, E1000_IMC, 0xffffffff);
 
-	wr32(E1000_RCTL, 0);
-	wr32(E1000_TCTL, E1000_TCTL_PSP);
-	wrfl();
+	E1000_WRITE_REG(hw, E1000_RCTL, 0);
+	E1000_WRITE_REG(hw, E1000_TCTL, E1000_TCTL_PSP);
+	E1000_WRITE_FLUSH(hw);
 
-	msleep(10);
+	msec_delay(10);
 
-	ctrl = rd32(E1000_CTRL);
+	ctrl = E1000_READ_REG(hw, E1000_CTRL);
 
-	hw_dbg("Issuing a global reset to MAC\n");
-	wr32(E1000_CTRL, ctrl | E1000_CTRL_RST);
+	DEBUGOUT("Issuing a global reset to MAC\n");
+	E1000_WRITE_REG(hw, E1000_CTRL, ctrl | E1000_CTRL_RST);
 
-	ret_val = igb_get_auto_rd_done(hw);
+	ret_val = e1000_get_auto_rd_done_generic(hw);
 	if (ret_val) {
 		/*
 		 * When auto config read does not complete, do not
 		 * return with an error. This can happen in situations
 		 * where there is no eeprom and prevents getting link.
 		 */
-		hw_dbg("Auto Read Done did not complete\n");
+		DEBUGOUT("Auto Read Done did not complete\n");
 	}
 
 	/* If EEPROM is not present, run manual init scripts */
-	if ((rd32(E1000_EECD) & E1000_EECD_PRES) == 0)
-		igb_reset_init_script_82575(hw);
+	if (!(E1000_READ_REG(hw, E1000_EECD) & E1000_EECD_PRES))
+		e1000_reset_init_script_82575(hw);
 
 	/* Clear any pending interrupt events. */
-	wr32(E1000_IMC, 0xffffffff);
-	icr = rd32(E1000_ICR);
+	E1000_WRITE_REG(hw, E1000_IMC, 0xffffffff);
+	E1000_READ_REG(hw, E1000_ICR);
 
 	/* Install any alternate MAC address into RAR0 */
-	ret_val = igb_check_alt_mac_addr(hw);
+	ret_val = e1000_check_alt_mac_addr_generic(hw);
 
 	return ret_val;
 }
 
 /**
- *  igb_init_hw_82575 - Initialize hardware
+ *  e1000_init_hw_82575 - Initialize hardware
  *  @hw: pointer to the HW structure
  *
  *  This inits the hardware readying it for operation.
  **/
-static s32 igb_init_hw_82575(struct e1000_hw *hw)
+static s32 e1000_init_hw_82575(struct e1000_hw *hw)
 {
 	struct e1000_mac_info *mac = &hw->mac;
 	s32 ret_val;
 	u16 i, rar_count = mac->rar_entry_count;
 
+	DEBUGFUNC("e1000_init_hw_82575");
+
 	/* Initialize identification LED */
-	ret_val = igb_id_led_init(hw);
+	ret_val = mac->ops.id_led_init(hw);
 	if (ret_val) {
-		hw_dbg("Error initializing identification LED\n");
+		DEBUGOUT("Error initializing identification LED\n");
 		/* This is not fatal and we should not stop init due to this */
 	}
 
 	/* Disabling VLAN filtering */
-	hw_dbg("Initializing the IEEE VLAN\n");
-	igb_clear_vfta(hw);
+	DEBUGOUT("Initializing the IEEE VLAN\n");
+	mac->ops.clear_vfta(hw);
 
 	/* Setup the receive address */
-	igb_init_rx_addrs(hw, rar_count);
+	e1000_init_rx_addrs_generic(hw, rar_count);
 
 	/* Zero out the Multicast HASH table */
-	hw_dbg("Zeroing the MTA\n");
+	DEBUGOUT("Zeroing the MTA\n");
 	for (i = 0; i < mac->mta_reg_count; i++)
-		array_wr32(E1000_MTA, i, 0);
+		E1000_WRITE_REG_ARRAY(hw, E1000_MTA, i, 0);
 
 	/* Zero out the Unicast HASH table */
-	hw_dbg("Zeroing the UTA\n");
+	DEBUGOUT("Zeroing the UTA\n");
 	for (i = 0; i < mac->uta_reg_count; i++)
-		array_wr32(E1000_UTA, i, 0);
+		E1000_WRITE_REG_ARRAY(hw, E1000_UTA, i, 0);
 
 	/* Setup link and flow control */
-	ret_val = igb_setup_link(hw);
+	ret_val = mac->ops.setup_link(hw);
+
+	/* Set the default MTU size */
+	hw->dev_spec._82575.mtu = 1500;
 
 	/*
 	 * Clear all of the statistics registers (clear on read).  It is
@@ -1079,56 +1518,82 @@ static s32 igb_init_hw_82575(struct e1000_hw *hw)
 	 * because the symbol error count will increment wildly if there
 	 * is no link.
 	 */
-	igb_clear_hw_cntrs_82575(hw);
+	e1000_clear_hw_cntrs_82575(hw);
 
 	return ret_val;
 }
 
 /**
- *  igb_setup_copper_link_82575 - Configure copper link settings
+ *  e1000_setup_copper_link_82575 - Configure copper link settings
  *  @hw: pointer to the HW structure
  *
  *  Configures the link for auto-neg or forced speed and duplex.  Then we check
  *  for link, once link is established calls to configure collision distance
  *  and flow control are called.
  **/
-static s32 igb_setup_copper_link_82575(struct e1000_hw *hw)
+static s32 e1000_setup_copper_link_82575(struct e1000_hw *hw)
 {
 	u32 ctrl;
-	s32  ret_val;
+	s32 ret_val;
+	u32 phpm_reg;
+
+	DEBUGFUNC("e1000_setup_copper_link_82575");
 
-	ctrl = rd32(E1000_CTRL);
+	ctrl = E1000_READ_REG(hw, E1000_CTRL);
 	ctrl |= E1000_CTRL_SLU;
 	ctrl &= ~(E1000_CTRL_FRCSPD | E1000_CTRL_FRCDPX);
-	wr32(E1000_CTRL, ctrl);
+	E1000_WRITE_REG(hw, E1000_CTRL, ctrl);
 
-	ret_val = igb_setup_serdes_link_82575(hw);
+	/* Clear Go Link Disconnect bit on supported devices */
+	switch (hw->mac.type) {
+	case e1000_82580:
+	case e1000_i350:
+	case e1000_i210:
+	case e1000_i211:
+		phpm_reg = E1000_READ_REG(hw, E1000_82580_PHY_POWER_MGMT);
+		phpm_reg &= ~E1000_82580_PM_GO_LINKD;
+		E1000_WRITE_REG(hw, E1000_82580_PHY_POWER_MGMT, phpm_reg);
+		break;
+	default:
+		break;
+	}
+
+	ret_val = e1000_setup_serdes_link_82575(hw);
 	if (ret_val)
 		goto out;
 
-	if (igb_sgmii_active_82575(hw) && !hw->phy.reset_disable) {
+	if (e1000_sgmii_active_82575(hw) && !hw->phy.reset_disable) {
 		/* allow time for SFP cage time to power up phy */
-		msleep(300);
+		msec_delay(300);
 
 		ret_val = hw->phy.ops.reset(hw);
 		if (ret_val) {
-			hw_dbg("Error resetting the PHY.\n");
+			DEBUGOUT("Error resetting the PHY.\n");
 			goto out;
 		}
 	}
 	switch (hw->phy.type) {
+	case e1000_phy_i210:
 	case e1000_phy_m88:
-		if (hw->phy.id == I347AT4_E_PHY_ID ||
-		    hw->phy.id == M88E1112_E_PHY_ID)
-			ret_val = igb_copper_link_setup_m88_gen2(hw);
-		else
-			ret_val = igb_copper_link_setup_m88(hw);
+		switch (hw->phy.id) {
+		case I347AT4_E_PHY_ID:
+		case M88E1112_E_PHY_ID:
+		case M88E1340M_E_PHY_ID:
+		case M88E1543_E_PHY_ID:
+		case M88E1512_E_PHY_ID:
+		case I210_I_PHY_ID:
+			ret_val = e1000_copper_link_setup_m88_gen2(hw);
+			break;
+		default:
+			ret_val = e1000_copper_link_setup_m88(hw);
+			break;
+		}
 		break;
 	case e1000_phy_igp_3:
-		ret_val = igb_copper_link_setup_igp(hw);
+		ret_val = e1000_copper_link_setup_igp(hw);
 		break;
 	case e1000_phy_82580:
-		ret_val = igb_copper_link_setup_82580(hw);
+		ret_val = e1000_copper_link_setup_82577(hw);
 		break;
 	default:
 		ret_val = -E1000_ERR_PHY;
@@ -1138,13 +1603,13 @@ static s32 igb_setup_copper_link_82575(struct e1000_hw *hw)
 	if (ret_val)
 		goto out;
 
-	ret_val = igb_setup_copper_link(hw);
+	ret_val = e1000_setup_copper_link_generic(hw);
 out:
 	return ret_val;
 }
 
 /**
- *  igb_setup_serdes_link_82575 - Setup link for serdes
+ *  e1000_setup_serdes_link_82575 - Setup link for serdes
  *  @hw: pointer to the HW structure
  *
  *  Configure the physical coding sub-layer (PCS) link.  The PCS link is
@@ -1152,45 +1617,40 @@ out:
  *  interface (sgmii), or serdes fiber is being used.  Configures the link
  *  for auto-negotiation or forces speed/duplex.
  **/
-static s32 igb_setup_serdes_link_82575(struct e1000_hw *hw)
+static s32 e1000_setup_serdes_link_82575(struct e1000_hw *hw)
 {
-	u32 ctrl_ext, ctrl_reg, reg;
+	u32 ctrl_ext, ctrl_reg, reg, anadv_reg;
 	bool pcs_autoneg;
 	s32 ret_val = E1000_SUCCESS;
 	u16 data;
 
+	DEBUGFUNC("e1000_setup_serdes_link_82575");
+
 	if ((hw->phy.media_type != e1000_media_type_internal_serdes) &&
-	    !igb_sgmii_active_82575(hw))
+	    !e1000_sgmii_active_82575(hw))
 		return ret_val;
 
-
 	/*
 	 * On the 82575, SerDes loopback mode persists until it is
 	 * explicitly turned off or a power cycle is performed.  A read to
 	 * the register does not indicate its status.  Therefore, we ensure
 	 * loopback mode is disabled during initialization.
 	 */
-	wr32(E1000_SCTL, E1000_SCTL_DISABLE_SERDES_LOOPBACK);
+	E1000_WRITE_REG(hw, E1000_SCTL, E1000_SCTL_DISABLE_SERDES_LOOPBACK);
 
 	/* power on the sfp cage if present */
-	ctrl_ext = rd32(E1000_CTRL_EXT);
+	ctrl_ext = E1000_READ_REG(hw, E1000_CTRL_EXT);
 	ctrl_ext &= ~E1000_CTRL_EXT_SDP3_DATA;
-	wr32(E1000_CTRL_EXT, ctrl_ext);
+	E1000_WRITE_REG(hw, E1000_CTRL_EXT, ctrl_ext);
 
-	ctrl_reg = rd32(E1000_CTRL);
+	ctrl_reg = E1000_READ_REG(hw, E1000_CTRL);
 	ctrl_reg |= E1000_CTRL_SLU;
 
-	if (hw->mac.type == e1000_82575 || hw->mac.type == e1000_82576) {
-		/* set both sw defined pins */
+	/* set both sw defined pins on 82575/82576*/
+	if (hw->mac.type == e1000_82575 || hw->mac.type == e1000_82576)
 		ctrl_reg |= E1000_CTRL_SWDPIN0 | E1000_CTRL_SWDPIN1;
 
-		/* Set switch control to serdes energy detect */
-		reg = rd32(E1000_CONNSW);
-		reg |= E1000_CONNSW_ENRGSRC;
-		wr32(E1000_CONNSW, reg);
-	}
-
-	reg = rd32(E1000_PCS_LCTL);
+	reg = E1000_READ_REG(hw, E1000_PCS_LCTL);
 
 	/* default pcs_autoneg to the same setting as mac autoneg */
 	pcs_autoneg = hw->mac.autoneg;
@@ -1205,12 +1665,13 @@ static s32 igb_setup_serdes_link_82575(struct e1000_hw *hw)
 	case E1000_CTRL_EXT_LINK_MODE_1000BASE_KX:
 		/* disable PCS autoneg and support parallel detect only */
 		pcs_autoneg = false;
+		/* fall through to default case */
 	default:
 		if (hw->mac.type == e1000_82575 ||
 		    hw->mac.type == e1000_82576) {
 			ret_val = hw->nvm.ops.read(hw, NVM_COMPAT, 1, &data);
 			if (ret_val) {
-				printk(KERN_DEBUG "NVM Read Error\n\n");
+				DEBUGOUT("NVM Read Error\n");
 				return ret_val;
 			}
 
@@ -1224,14 +1685,14 @@ static s32 igb_setup_serdes_link_82575(struct e1000_hw *hw)
 		 * link either autoneg or be forced to 1000/Full
 		 */
 		ctrl_reg |= E1000_CTRL_SPD_1000 | E1000_CTRL_FRCSPD |
-		            E1000_CTRL_FD | E1000_CTRL_FRCDPX;
+			    E1000_CTRL_FD | E1000_CTRL_FRCDPX;
 
 		/* set speed of 1000/Full if speed/duplex is forced */
 		reg |= E1000_PCS_LCTL_FSV_1000 | E1000_PCS_LCTL_FDV_FULL;
 		break;
 	}
 
-	wr32(E1000_CTRL, ctrl_reg);
+	E1000_WRITE_REG(hw, E1000_CTRL, ctrl_reg);
 
 	/*
 	 * New SerDes mode allows for forcing speed or autonegotiating speed
@@ -1240,186 +1701,427 @@ static s32 igb_setup_serdes_link_82575(struct e1000_hw *hw)
 	 * However, both are supported by the hardware and some drivers/tools.
 	 */
 	reg &= ~(E1000_PCS_LCTL_AN_ENABLE | E1000_PCS_LCTL_FLV_LINK_UP |
-		E1000_PCS_LCTL_FSD | E1000_PCS_LCTL_FORCE_LINK);
-
-	/*
-	 * We force flow control to prevent the CTRL register values from being
-	 * overwritten by the autonegotiated flow control values
-	 */
-	reg |= E1000_PCS_LCTL_FORCE_FCTRL;
+		 E1000_PCS_LCTL_FSD | E1000_PCS_LCTL_FORCE_LINK);
 
 	if (pcs_autoneg) {
 		/* Set PCS register for autoneg */
 		reg |= E1000_PCS_LCTL_AN_ENABLE | /* Enable Autoneg */
 		       E1000_PCS_LCTL_AN_RESTART; /* Restart autoneg */
-		hw_dbg("Configuring Autoneg:PCS_LCTL=0x%08X\n", reg);
+
+		/* Disable force flow control for autoneg */
+		reg &= ~E1000_PCS_LCTL_FORCE_FCTRL;
+
+		/* Configure flow control advertisement for autoneg */
+		anadv_reg = E1000_READ_REG(hw, E1000_PCS_ANADV);
+		anadv_reg &= ~(E1000_TXCW_ASM_DIR | E1000_TXCW_PAUSE);
+
+		switch (hw->fc.requested_mode) {
+		case e1000_fc_full:
+		case e1000_fc_rx_pause:
+			anadv_reg |= E1000_TXCW_ASM_DIR;
+			anadv_reg |= E1000_TXCW_PAUSE;
+			break;
+		case e1000_fc_tx_pause:
+			anadv_reg |= E1000_TXCW_ASM_DIR;
+			break;
+		default:
+			break;
+		}
+
+		E1000_WRITE_REG(hw, E1000_PCS_ANADV, anadv_reg);
+
+		DEBUGOUT1("Configuring Autoneg:PCS_LCTL=0x%08X\n", reg);
 	} else {
 		/* Set PCS register for forced link */
-		reg |= E1000_PCS_LCTL_FSD;        /* Force Speed */
+		reg |= E1000_PCS_LCTL_FSD;	/* Force Speed */
 
-		hw_dbg("Configuring Forced Link:PCS_LCTL=0x%08X\n", reg);
+		/* Force flow control for forced link */
+		reg |= E1000_PCS_LCTL_FORCE_FCTRL;
+
+		DEBUGOUT1("Configuring Forced Link:PCS_LCTL=0x%08X\n", reg);
 	}
 
-	wr32(E1000_PCS_LCTL, reg);
+	E1000_WRITE_REG(hw, E1000_PCS_LCTL, reg);
+
+	if (!pcs_autoneg && !e1000_sgmii_active_82575(hw))
+		e1000_force_mac_fc_generic(hw);
+
+	return ret_val;
+}
+
+/**
+ *  e1000_get_media_type_82575 - derives current media type.
+ *  @hw: pointer to the HW structure
+ *
+ *  The media type is chosen reflecting few settings.
+ *  The following are taken into account:
+ *  - link mode set in the current port Init Control Word #3
+ *  - current link mode settings in CSR register
+ *  - MDIO vs. I2C PHY control interface chosen
+ *  - SFP module media type
+ **/
+static s32 e1000_get_media_type_82575(struct e1000_hw *hw)
+{
+	struct e1000_dev_spec_82575 *dev_spec = &hw->dev_spec._82575;
+	s32 ret_val = E1000_SUCCESS;
+	u32 ctrl_ext = 0;
+	u32 link_mode = 0;
+
+	/* Set internal phy as default */
+	dev_spec->sgmii_active = false;
+	dev_spec->module_plugged = false;
+
+	/* Get CSR setting */
+	ctrl_ext = E1000_READ_REG(hw, E1000_CTRL_EXT);
+
+	/* extract link mode setting */
+	link_mode = ctrl_ext & E1000_CTRL_EXT_LINK_MODE_MASK;
+
+	switch (link_mode) {
+	case E1000_CTRL_EXT_LINK_MODE_1000BASE_KX:
+		hw->phy.media_type = e1000_media_type_internal_serdes;
+		break;
+	case E1000_CTRL_EXT_LINK_MODE_GMII:
+		hw->phy.media_type = e1000_media_type_copper;
+		break;
+	case E1000_CTRL_EXT_LINK_MODE_SGMII:
+		/* Get phy control interface type set (MDIO vs. I2C)*/
+		if (e1000_sgmii_uses_mdio_82575(hw)) {
+			hw->phy.media_type = e1000_media_type_copper;
+			dev_spec->sgmii_active = true;
+			break;
+		}
+		/* fall through for I2C based SGMII */
+	case E1000_CTRL_EXT_LINK_MODE_PCIE_SERDES:
+		/* read media type from SFP EEPROM */
+		ret_val = e1000_set_sfp_media_type_82575(hw);
+		if ((ret_val != E1000_SUCCESS) ||
+		    (hw->phy.media_type == e1000_media_type_unknown)) {
+			/*
+			 * If media type was not identified then return media
+			 * type defined by the CTRL_EXT settings.
+			 */
+			hw->phy.media_type = e1000_media_type_internal_serdes;
+
+			if (link_mode == E1000_CTRL_EXT_LINK_MODE_SGMII) {
+				hw->phy.media_type = e1000_media_type_copper;
+				dev_spec->sgmii_active = true;
+			}
+
+			break;
+		}
+
+		/* do not change link mode for 100BaseFX */
+		if (dev_spec->eth_flags.e100_base_fx)
+			break;
+
+		/* change current link mode setting */
+		ctrl_ext &= ~E1000_CTRL_EXT_LINK_MODE_MASK;
+
+		if (hw->phy.media_type == e1000_media_type_copper)
+			ctrl_ext |= E1000_CTRL_EXT_LINK_MODE_SGMII;
+		else
+			ctrl_ext |= E1000_CTRL_EXT_LINK_MODE_PCIE_SERDES;
+
+		E1000_WRITE_REG(hw, E1000_CTRL_EXT, ctrl_ext);
+
+		break;
+	}
+
+	return ret_val;
+}
+
+/**
+ *  e1000_set_sfp_media_type_82575 - derives SFP module media type.
+ *  @hw: pointer to the HW structure
+ *
+ *  The media type is chosen based on SFP module.
+ *  compatibility flags retrieved from SFP ID EEPROM.
+ **/
+static s32 e1000_set_sfp_media_type_82575(struct e1000_hw *hw)
+{
+	s32 ret_val = E1000_ERR_CONFIG;
+	u32 ctrl_ext = 0;
+	struct e1000_dev_spec_82575 *dev_spec = &hw->dev_spec._82575;
+	struct sfp_e1000_flags *eth_flags = &dev_spec->eth_flags;
+	u8 tranceiver_type = 0;
+	s32 timeout = 3;
+
+	/* Turn I2C interface ON and power on sfp cage */
+	ctrl_ext = E1000_READ_REG(hw, E1000_CTRL_EXT);
+	ctrl_ext &= ~E1000_CTRL_EXT_SDP3_DATA;
+	E1000_WRITE_REG(hw, E1000_CTRL_EXT, ctrl_ext | E1000_CTRL_I2C_ENA);
+
+	E1000_WRITE_FLUSH(hw);
+
+	/* Read SFP module data */
+	while (timeout) {
+		ret_val = e1000_read_sfp_data_byte(hw,
+			E1000_I2CCMD_SFP_DATA_ADDR(E1000_SFF_IDENTIFIER_OFFSET),
+			&tranceiver_type);
+		if (ret_val == E1000_SUCCESS)
+			break;
+		msec_delay(100);
+		timeout--;
+	}
+	if (ret_val != E1000_SUCCESS)
+		goto out;
+
+	ret_val = e1000_read_sfp_data_byte(hw,
+			E1000_I2CCMD_SFP_DATA_ADDR(E1000_SFF_ETH_FLAGS_OFFSET),
+			(u8 *)eth_flags);
+	if (ret_val != E1000_SUCCESS)
+		goto out;
+
+	/* Check if there is some SFP module plugged and powered */
+	if ((tranceiver_type == E1000_SFF_IDENTIFIER_SFP) ||
+	    (tranceiver_type == E1000_SFF_IDENTIFIER_SFF)) {
+		dev_spec->module_plugged = true;
+		if (eth_flags->e1000_base_lx || eth_flags->e1000_base_sx) {
+			hw->phy.media_type = e1000_media_type_internal_serdes;
+		} else if (eth_flags->e100_base_fx) {
+			dev_spec->sgmii_active = true;
+			hw->phy.media_type = e1000_media_type_internal_serdes;
+		} else if (eth_flags->e1000_base_t) {
+			dev_spec->sgmii_active = true;
+			hw->phy.media_type = e1000_media_type_copper;
+		} else {
+			hw->phy.media_type = e1000_media_type_unknown;
+			DEBUGOUT("PHY module has not been recognized\n");
+			goto out;
+		}
+	} else {
+		hw->phy.media_type = e1000_media_type_unknown;
+	}
+	ret_val = E1000_SUCCESS;
+out:
+	/* Restore I2C interface setting */
+	E1000_WRITE_REG(hw, E1000_CTRL_EXT, ctrl_ext);
+	return ret_val;
+}
+
+/**
+ *  e1000_valid_led_default_82575 - Verify a valid default LED config
+ *  @hw: pointer to the HW structure
+ *  @data: pointer to the NVM (EEPROM)
+ *
+ *  Read the EEPROM for the current default LED configuration.  If the
+ *  LED configuration is not valid, set to a valid LED configuration.
+ **/
+static s32 e1000_valid_led_default_82575(struct e1000_hw *hw, u16 *data)
+{
+	s32 ret_val;
+
+	DEBUGFUNC("e1000_valid_led_default_82575");
 
-	if (!igb_sgmii_active_82575(hw))
-		igb_force_mac_fc(hw);
+	ret_val = hw->nvm.ops.read(hw, NVM_ID_LED_SETTINGS, 1, data);
+	if (ret_val) {
+		DEBUGOUT("NVM Read Error\n");
+		goto out;
+	}
 
+	if (*data == ID_LED_RESERVED_0000 || *data == ID_LED_RESERVED_FFFF) {
+		switch (hw->phy.media_type) {
+		case e1000_media_type_internal_serdes:
+			*data = ID_LED_DEFAULT_82575_SERDES;
+			break;
+		case e1000_media_type_copper:
+		default:
+			*data = ID_LED_DEFAULT;
+			break;
+		}
+	}
+out:
 	return ret_val;
 }
 
 /**
- *  igb_sgmii_active_82575 - Return sgmii state
+ *  e1000_sgmii_active_82575 - Return sgmii state
  *  @hw: pointer to the HW structure
  *
  *  82575 silicon has a serialized gigabit media independent interface (sgmii)
  *  which can be enabled for use in the embedded applications.  Simply
  *  return the current state of the sgmii interface.
  **/
-static bool igb_sgmii_active_82575(struct e1000_hw *hw)
+static bool e1000_sgmii_active_82575(struct e1000_hw *hw)
 {
 	struct e1000_dev_spec_82575 *dev_spec = &hw->dev_spec._82575;
 	return dev_spec->sgmii_active;
 }
 
 /**
- *  igb_reset_init_script_82575 - Inits HW defaults after reset
+ *  e1000_reset_init_script_82575 - Inits HW defaults after reset
  *  @hw: pointer to the HW structure
  *
  *  Inits recommended HW defaults after a reset when there is no EEPROM
  *  detected. This is only for the 82575.
  **/
-static s32 igb_reset_init_script_82575(struct e1000_hw *hw)
+static s32 e1000_reset_init_script_82575(struct e1000_hw *hw)
 {
+	DEBUGFUNC("e1000_reset_init_script_82575");
+
 	if (hw->mac.type == e1000_82575) {
-		hw_dbg("Running reset init script for 82575\n");
+		DEBUGOUT("Running reset init script for 82575\n");
 		/* SerDes configuration via SERDESCTRL */
-		igb_write_8bit_ctrl_reg(hw, E1000_SCTL, 0x00, 0x0C);
-		igb_write_8bit_ctrl_reg(hw, E1000_SCTL, 0x01, 0x78);
-		igb_write_8bit_ctrl_reg(hw, E1000_SCTL, 0x1B, 0x23);
-		igb_write_8bit_ctrl_reg(hw, E1000_SCTL, 0x23, 0x15);
+		e1000_write_8bit_ctrl_reg_generic(hw, E1000_SCTL, 0x00, 0x0C);
+		e1000_write_8bit_ctrl_reg_generic(hw, E1000_SCTL, 0x01, 0x78);
+		e1000_write_8bit_ctrl_reg_generic(hw, E1000_SCTL, 0x1B, 0x23);
+		e1000_write_8bit_ctrl_reg_generic(hw, E1000_SCTL, 0x23, 0x15);
 
 		/* CCM configuration via CCMCTL register */
-		igb_write_8bit_ctrl_reg(hw, E1000_CCMCTL, 0x14, 0x00);
-		igb_write_8bit_ctrl_reg(hw, E1000_CCMCTL, 0x10, 0x00);
+		e1000_write_8bit_ctrl_reg_generic(hw, E1000_CCMCTL, 0x14, 0x00);
+		e1000_write_8bit_ctrl_reg_generic(hw, E1000_CCMCTL, 0x10, 0x00);
 
 		/* PCIe lanes configuration */
-		igb_write_8bit_ctrl_reg(hw, E1000_GIOCTL, 0x00, 0xEC);
-		igb_write_8bit_ctrl_reg(hw, E1000_GIOCTL, 0x61, 0xDF);
-		igb_write_8bit_ctrl_reg(hw, E1000_GIOCTL, 0x34, 0x05);
-		igb_write_8bit_ctrl_reg(hw, E1000_GIOCTL, 0x2F, 0x81);
+		e1000_write_8bit_ctrl_reg_generic(hw, E1000_GIOCTL, 0x00, 0xEC);
+		e1000_write_8bit_ctrl_reg_generic(hw, E1000_GIOCTL, 0x61, 0xDF);
+		e1000_write_8bit_ctrl_reg_generic(hw, E1000_GIOCTL, 0x34, 0x05);
+		e1000_write_8bit_ctrl_reg_generic(hw, E1000_GIOCTL, 0x2F, 0x81);
 
 		/* PCIe PLL Configuration */
-		igb_write_8bit_ctrl_reg(hw, E1000_SCCTL, 0x02, 0x47);
-		igb_write_8bit_ctrl_reg(hw, E1000_SCCTL, 0x14, 0x00);
-		igb_write_8bit_ctrl_reg(hw, E1000_SCCTL, 0x10, 0x00);
+		e1000_write_8bit_ctrl_reg_generic(hw, E1000_SCCTL, 0x02, 0x47);
+		e1000_write_8bit_ctrl_reg_generic(hw, E1000_SCCTL, 0x14, 0x00);
+		e1000_write_8bit_ctrl_reg_generic(hw, E1000_SCCTL, 0x10, 0x00);
 	}
 
-	return 0;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_read_mac_addr_82575 - Read device MAC address
+ *  e1000_read_mac_addr_82575 - Read device MAC address
  *  @hw: pointer to the HW structure
  **/
-static s32 igb_read_mac_addr_82575(struct e1000_hw *hw)
+static s32 e1000_read_mac_addr_82575(struct e1000_hw *hw)
 {
-	s32 ret_val = 0;
+	s32 ret_val = E1000_SUCCESS;
+
+	DEBUGFUNC("e1000_read_mac_addr_82575");
 
 	/*
 	 * If there's an alternate MAC address place it in RAR0
 	 * so that it will override the Si installed default perm
 	 * address.
 	 */
-	ret_val = igb_check_alt_mac_addr(hw);
+	ret_val = e1000_check_alt_mac_addr_generic(hw);
 	if (ret_val)
 		goto out;
 
-	ret_val = igb_read_mac_addr(hw);
+	ret_val = e1000_read_mac_addr_generic(hw);
 
 out:
 	return ret_val;
 }
 
 /**
- * igb_power_down_phy_copper_82575 - Remove link during PHY power down
+ *  e1000_config_collision_dist_82575 - Configure collision distance
+ *  @hw: pointer to the HW structure
+ *
+ *  Configures the collision distance to the default value and is used
+ *  during link setup.
+ **/
+static void e1000_config_collision_dist_82575(struct e1000_hw *hw)
+{
+	u32 tctl_ext;
+
+	DEBUGFUNC("e1000_config_collision_dist_82575");
+
+	tctl_ext = E1000_READ_REG(hw, E1000_TCTL_EXT);
+
+	tctl_ext &= ~E1000_TCTL_EXT_COLD;
+	tctl_ext |= E1000_COLLISION_DISTANCE << E1000_TCTL_EXT_COLD_SHIFT;
+
+	E1000_WRITE_REG(hw, E1000_TCTL_EXT, tctl_ext);
+	E1000_WRITE_FLUSH(hw);
+}
+
+/**
+ * e1000_power_down_phy_copper_82575 - Remove link during PHY power down
  * @hw: pointer to the HW structure
  *
  * In the case of a PHY power down to save power, or to turn off link during a
  * driver unload, or wake on lan is not enabled, remove the link.
  **/
-void igb_power_down_phy_copper_82575(struct e1000_hw *hw)
+static void e1000_power_down_phy_copper_82575(struct e1000_hw *hw)
 {
+	struct e1000_phy_info *phy = &hw->phy;
+
+	if (!(phy->ops.check_reset_block))
+		return;
+
 	/* If the management interface is not enabled, then power down */
-	if (!(igb_enable_mng_pass_thru(hw) || igb_check_reset_block(hw)))
-		igb_power_down_phy_copper(hw);
+	if (!(e1000_enable_mng_pass_thru(hw) || phy->ops.check_reset_block(hw)))
+		e1000_power_down_phy_copper(hw);
+
+	return;
 }
 
 /**
- *  igb_clear_hw_cntrs_82575 - Clear device specific hardware counters
+ *  e1000_clear_hw_cntrs_82575 - Clear device specific hardware counters
  *  @hw: pointer to the HW structure
  *
  *  Clears the hardware counters by reading the counter registers.
  **/
-static void igb_clear_hw_cntrs_82575(struct e1000_hw *hw)
-{
-	igb_clear_hw_cntrs_base(hw);
-
-	rd32(E1000_PRC64);
-	rd32(E1000_PRC127);
-	rd32(E1000_PRC255);
-	rd32(E1000_PRC511);
-	rd32(E1000_PRC1023);
-	rd32(E1000_PRC1522);
-	rd32(E1000_PTC64);
-	rd32(E1000_PTC127);
-	rd32(E1000_PTC255);
-	rd32(E1000_PTC511);
-	rd32(E1000_PTC1023);
-	rd32(E1000_PTC1522);
-
-	rd32(E1000_ALGNERRC);
-	rd32(E1000_RXERRC);
-	rd32(E1000_TNCRS);
-	rd32(E1000_CEXTERR);
-	rd32(E1000_TSCTC);
-	rd32(E1000_TSCTFC);
-
-	rd32(E1000_MGTPRC);
-	rd32(E1000_MGTPDC);
-	rd32(E1000_MGTPTC);
-
-	rd32(E1000_IAC);
-	rd32(E1000_ICRXOC);
-
-	rd32(E1000_ICRXPTC);
-	rd32(E1000_ICRXATC);
-	rd32(E1000_ICTXPTC);
-	rd32(E1000_ICTXATC);
-	rd32(E1000_ICTXQEC);
-	rd32(E1000_ICTXQMTC);
-	rd32(E1000_ICRXDMTC);
-
-	rd32(E1000_CBTMPC);
-	rd32(E1000_HTDPMC);
-	rd32(E1000_CBRMPC);
-	rd32(E1000_RPTHC);
-	rd32(E1000_HGPTC);
-	rd32(E1000_HTCBDPC);
-	rd32(E1000_HGORCL);
-	rd32(E1000_HGORCH);
-	rd32(E1000_HGOTCL);
-	rd32(E1000_HGOTCH);
-	rd32(E1000_LENERRS);
+static void e1000_clear_hw_cntrs_82575(struct e1000_hw *hw)
+{
+	DEBUGFUNC("e1000_clear_hw_cntrs_82575");
+
+	e1000_clear_hw_cntrs_base_generic(hw);
+
+	E1000_READ_REG(hw, E1000_PRC64);
+	E1000_READ_REG(hw, E1000_PRC127);
+	E1000_READ_REG(hw, E1000_PRC255);
+	E1000_READ_REG(hw, E1000_PRC511);
+	E1000_READ_REG(hw, E1000_PRC1023);
+	E1000_READ_REG(hw, E1000_PRC1522);
+	E1000_READ_REG(hw, E1000_PTC64);
+	E1000_READ_REG(hw, E1000_PTC127);
+	E1000_READ_REG(hw, E1000_PTC255);
+	E1000_READ_REG(hw, E1000_PTC511);
+	E1000_READ_REG(hw, E1000_PTC1023);
+	E1000_READ_REG(hw, E1000_PTC1522);
+
+	E1000_READ_REG(hw, E1000_ALGNERRC);
+	E1000_READ_REG(hw, E1000_RXERRC);
+	E1000_READ_REG(hw, E1000_TNCRS);
+	E1000_READ_REG(hw, E1000_CEXTERR);
+	E1000_READ_REG(hw, E1000_TSCTC);
+	E1000_READ_REG(hw, E1000_TSCTFC);
+
+	E1000_READ_REG(hw, E1000_MGTPRC);
+	E1000_READ_REG(hw, E1000_MGTPDC);
+	E1000_READ_REG(hw, E1000_MGTPTC);
+
+	E1000_READ_REG(hw, E1000_IAC);
+	E1000_READ_REG(hw, E1000_ICRXOC);
+
+	E1000_READ_REG(hw, E1000_ICRXPTC);
+	E1000_READ_REG(hw, E1000_ICRXATC);
+	E1000_READ_REG(hw, E1000_ICTXPTC);
+	E1000_READ_REG(hw, E1000_ICTXATC);
+	E1000_READ_REG(hw, E1000_ICTXQEC);
+	E1000_READ_REG(hw, E1000_ICTXQMTC);
+	E1000_READ_REG(hw, E1000_ICRXDMTC);
+
+	E1000_READ_REG(hw, E1000_CBTMPC);
+	E1000_READ_REG(hw, E1000_HTDPMC);
+	E1000_READ_REG(hw, E1000_CBRMPC);
+	E1000_READ_REG(hw, E1000_RPTHC);
+	E1000_READ_REG(hw, E1000_HGPTC);
+	E1000_READ_REG(hw, E1000_HTCBDPC);
+	E1000_READ_REG(hw, E1000_HGORCL);
+	E1000_READ_REG(hw, E1000_HGORCH);
+	E1000_READ_REG(hw, E1000_HGOTCL);
+	E1000_READ_REG(hw, E1000_HGOTCH);
+	E1000_READ_REG(hw, E1000_LENERRS);
 
 	/* This register should not be read in copper configurations */
-	if (hw->phy.media_type == e1000_media_type_internal_serdes ||
-	    igb_sgmii_active_82575(hw))
-		rd32(E1000_SCVPC);
+	if ((hw->phy.media_type == e1000_media_type_internal_serdes) ||
+	    e1000_sgmii_active_82575(hw))
+		E1000_READ_REG(hw, E1000_SCVPC);
 }
 
 /**
- *  igb_rx_fifo_flush_82575 - Clean rx fifo after RX enable
+ *  e1000_rx_fifo_flush_82575 - Clean rx fifo after Rx enable
  *  @hw: pointer to the HW structure
  *
  *  After rx enable if managability is enabled then there is likely some
@@ -1427,72 +2129,73 @@ static void igb_clear_hw_cntrs_82575(struct e1000_hw *hw)
  *  function clears the fifos and flushes any packets that came in as rx was
  *  being enabled.
  **/
-void igb_rx_fifo_flush_82575(struct e1000_hw *hw)
+void e1000_rx_fifo_flush_82575(struct e1000_hw *hw)
 {
 	u32 rctl, rlpml, rxdctl[4], rfctl, temp_rctl, rx_enabled;
 	int i, ms_wait;
 
+	DEBUGFUNC("e1000_rx_fifo_workaround_82575");
 	if (hw->mac.type != e1000_82575 ||
-	    !(rd32(E1000_MANC) & E1000_MANC_RCV_TCO_EN))
+	    !(E1000_READ_REG(hw, E1000_MANC) & E1000_MANC_RCV_TCO_EN))
 		return;
 
-	/* Disable all RX queues */
+	/* Disable all Rx queues */
 	for (i = 0; i < 4; i++) {
-		rxdctl[i] = rd32(E1000_RXDCTL(i));
-		wr32(E1000_RXDCTL(i),
-		     rxdctl[i] & ~E1000_RXDCTL_QUEUE_ENABLE);
+		rxdctl[i] = E1000_READ_REG(hw, E1000_RXDCTL(i));
+		E1000_WRITE_REG(hw, E1000_RXDCTL(i),
+				rxdctl[i] & ~E1000_RXDCTL_QUEUE_ENABLE);
 	}
 	/* Poll all queues to verify they have shut down */
 	for (ms_wait = 0; ms_wait < 10; ms_wait++) {
-		msleep(1);
+		msec_delay(1);
 		rx_enabled = 0;
 		for (i = 0; i < 4; i++)
-			rx_enabled |= rd32(E1000_RXDCTL(i));
+			rx_enabled |= E1000_READ_REG(hw, E1000_RXDCTL(i));
 		if (!(rx_enabled & E1000_RXDCTL_QUEUE_ENABLE))
 			break;
 	}
 
 	if (ms_wait == 10)
-		hw_dbg("Queue disable timed out after 10ms\n");
+		DEBUGOUT("Queue disable timed out after 10ms\n");
 
 	/* Clear RLPML, RCTL.SBP, RFCTL.LEF, and set RCTL.LPE so that all
 	 * incoming packets are rejected.  Set enable and wait 2ms so that
 	 * any packet that was coming in as RCTL.EN was set is flushed
 	 */
-	rfctl = rd32(E1000_RFCTL);
-	wr32(E1000_RFCTL, rfctl & ~E1000_RFCTL_LEF);
+	rfctl = E1000_READ_REG(hw, E1000_RFCTL);
+	E1000_WRITE_REG(hw, E1000_RFCTL, rfctl & ~E1000_RFCTL_LEF);
 
-	rlpml = rd32(E1000_RLPML);
-	wr32(E1000_RLPML, 0);
+	rlpml = E1000_READ_REG(hw, E1000_RLPML);
+	E1000_WRITE_REG(hw, E1000_RLPML, 0);
 
-	rctl = rd32(E1000_RCTL);
+	rctl = E1000_READ_REG(hw, E1000_RCTL);
 	temp_rctl = rctl & ~(E1000_RCTL_EN | E1000_RCTL_SBP);
 	temp_rctl |= E1000_RCTL_LPE;
 
-	wr32(E1000_RCTL, temp_rctl);
-	wr32(E1000_RCTL, temp_rctl | E1000_RCTL_EN);
-	wrfl();
-	msleep(2);
+	E1000_WRITE_REG(hw, E1000_RCTL, temp_rctl);
+	E1000_WRITE_REG(hw, E1000_RCTL, temp_rctl | E1000_RCTL_EN);
+	E1000_WRITE_FLUSH(hw);
+	msec_delay(2);
 
-	/* Enable RX queues that were previously enabled and restore our
+	/* Enable Rx queues that were previously enabled and restore our
 	 * previous state
 	 */
 	for (i = 0; i < 4; i++)
-		wr32(E1000_RXDCTL(i), rxdctl[i]);
-	wr32(E1000_RCTL, rctl);
-	wrfl();
+		E1000_WRITE_REG(hw, E1000_RXDCTL(i), rxdctl[i]);
+	E1000_WRITE_REG(hw, E1000_RCTL, rctl);
+	E1000_WRITE_FLUSH(hw);
 
-	wr32(E1000_RLPML, rlpml);
-	wr32(E1000_RFCTL, rfctl);
+	E1000_WRITE_REG(hw, E1000_RLPML, rlpml);
+	E1000_WRITE_REG(hw, E1000_RFCTL, rfctl);
 
 	/* Flush receive errors generated by workaround */
-	rd32(E1000_ROC);
-	rd32(E1000_RNBC);
-	rd32(E1000_MPC);
+	E1000_READ_REG(hw, E1000_ROC);
+	E1000_READ_REG(hw, E1000_RNBC);
+	E1000_READ_REG(hw, E1000_MPC);
 }
 
 /**
- *  igb_set_pcie_completion_timeout - set pci-e completion timeout
+ *  e1000_set_pcie_completion_timeout - set pci-e completion timeout
  *  @hw: pointer to the HW structure
  *
  *  The defaults for 82575 and 82576 should be in the range of 50us to 50ms,
@@ -1501,10 +2204,10 @@ void igb_rx_fifo_flush_82575(struct e1000_hw *hw)
  *  increase the value to either 10ms to 200ms for capability version 1 config,
  *  or 16ms to 55ms for version 2.
  **/
-static s32 igb_set_pcie_completion_timeout(struct e1000_hw *hw)
+static s32 e1000_set_pcie_completion_timeout(struct e1000_hw *hw)
 {
-	u32 gcr = rd32(E1000_GCR);
-	s32 ret_val = 0;
+	u32 gcr = E1000_READ_REG(hw, E1000_GCR);
+	s32 ret_val = E1000_SUCCESS;
 	u16 pcie_devctl2;
 
 	/* only take action if timeout value is defaulted to 0 */
@@ -1525,96 +2228,120 @@ static s32 igb_set_pcie_completion_timeout(struct e1000_hw *hw)
 	 * directly in order to set the completion timeout value for
 	 * 16ms to 55ms
 	 */
-	ret_val = igb_read_pcie_cap_reg(hw, PCIE_DEVICE_CONTROL2,
-	                                &pcie_devctl2);
+	ret_val = e1000_read_pcie_cap_reg(hw, PCIE_DEVICE_CONTROL2,
+					  &pcie_devctl2);
 	if (ret_val)
 		goto out;
 
 	pcie_devctl2 |= PCIE_DEVICE_CONTROL2_16ms;
 
-	ret_val = igb_write_pcie_cap_reg(hw, PCIE_DEVICE_CONTROL2,
-	                                 &pcie_devctl2);
+	ret_val = e1000_write_pcie_cap_reg(hw, PCIE_DEVICE_CONTROL2,
+					   &pcie_devctl2);
 out:
 	/* disable completion timeout resend */
 	gcr &= ~E1000_GCR_CMPL_TMOUT_RESEND;
 
-	wr32(E1000_GCR, gcr);
+	E1000_WRITE_REG(hw, E1000_GCR, gcr);
 	return ret_val;
 }
 
 /**
- *  igb_vmdq_set_anti_spoofing_pf - enable or disable anti-spoofing
+ *  e1000_vmdq_set_anti_spoofing_pf - enable or disable anti-spoofing
  *  @hw: pointer to the hardware struct
  *  @enable: state to enter, either enabled or disabled
  *  @pf: Physical Function pool - do not set anti-spoofing for the PF
  *
  *  enables/disables L2 switch anti-spoofing functionality.
  **/
-void igb_vmdq_set_anti_spoofing_pf(struct e1000_hw *hw, bool enable, int pf)
+void e1000_vmdq_set_anti_spoofing_pf(struct e1000_hw *hw, bool enable, int pf)
 {
-	u32 dtxswc;
+	u32 reg_val, reg_offset;
 
 	switch (hw->mac.type) {
 	case e1000_82576:
+		reg_offset = E1000_DTXSWC;
+		break;
 	case e1000_i350:
-		dtxswc = rd32(E1000_DTXSWC);
-		if (enable) {
-			dtxswc |= (E1000_DTXSWC_MAC_SPOOF_MASK |
-				   E1000_DTXSWC_VLAN_SPOOF_MASK);
-			/* The PF can spoof - it has to in order to
-			 * support emulation mode NICs */
-			dtxswc ^= (1 << pf | 1 << (pf + MAX_NUM_VFS));
-		} else {
-			dtxswc &= ~(E1000_DTXSWC_MAC_SPOOF_MASK |
-				    E1000_DTXSWC_VLAN_SPOOF_MASK);
-		}
-		wr32(E1000_DTXSWC, dtxswc);
+	case e1000_i354:
+		reg_offset = E1000_TXSWC;
 		break;
 	default:
-		break;
+		return;
 	}
+
+	reg_val = E1000_READ_REG(hw, reg_offset);
+	if (enable) {
+		reg_val |= (E1000_DTXSWC_MAC_SPOOF_MASK |
+			     E1000_DTXSWC_VLAN_SPOOF_MASK);
+		/* The PF can spoof - it has to in order to
+		 * support emulation mode NICs
+		 */
+		reg_val ^= (1 << pf | 1 << (pf + MAX_NUM_VFS));
+	} else {
+		reg_val &= ~(E1000_DTXSWC_MAC_SPOOF_MASK |
+			     E1000_DTXSWC_VLAN_SPOOF_MASK);
+	}
+	E1000_WRITE_REG(hw, reg_offset, reg_val);
 }
 
 /**
- *  igb_vmdq_set_loopback_pf - enable or disable vmdq loopback
+ *  e1000_vmdq_set_loopback_pf - enable or disable vmdq loopback
  *  @hw: pointer to the hardware struct
  *  @enable: state to enter, either enabled or disabled
  *
  *  enables/disables L2 switch loopback functionality.
  **/
-void igb_vmdq_set_loopback_pf(struct e1000_hw *hw, bool enable)
+void e1000_vmdq_set_loopback_pf(struct e1000_hw *hw, bool enable)
 {
-	u32 dtxswc = rd32(E1000_DTXSWC);
+	u32 dtxswc;
+
+	switch (hw->mac.type) {
+	case e1000_82576:
+		dtxswc = E1000_READ_REG(hw, E1000_DTXSWC);
+		if (enable)
+			dtxswc |= E1000_DTXSWC_VMDQ_LOOPBACK_EN;
+		else
+			dtxswc &= ~E1000_DTXSWC_VMDQ_LOOPBACK_EN;
+		E1000_WRITE_REG(hw, E1000_DTXSWC, dtxswc);
+		break;
+	case e1000_i350:
+	case e1000_i354:
+		dtxswc = E1000_READ_REG(hw, E1000_TXSWC);
+		if (enable)
+			dtxswc |= E1000_DTXSWC_VMDQ_LOOPBACK_EN;
+		else
+			dtxswc &= ~E1000_DTXSWC_VMDQ_LOOPBACK_EN;
+		E1000_WRITE_REG(hw, E1000_TXSWC, dtxswc);
+		break;
+	default:
+		/* Currently no other hardware supports loopback */
+		break;
+	}
 
-	if (enable)
-		dtxswc |= E1000_DTXSWC_VMDQ_LOOPBACK_EN;
-	else
-		dtxswc &= ~E1000_DTXSWC_VMDQ_LOOPBACK_EN;
 
-	wr32(E1000_DTXSWC, dtxswc);
 }
 
 /**
- *  igb_vmdq_set_replication_pf - enable or disable vmdq replication
+ *  e1000_vmdq_set_replication_pf - enable or disable vmdq replication
  *  @hw: pointer to the hardware struct
  *  @enable: state to enter, either enabled or disabled
  *
  *  enables/disables replication of packets across multiple pools.
  **/
-void igb_vmdq_set_replication_pf(struct e1000_hw *hw, bool enable)
+void e1000_vmdq_set_replication_pf(struct e1000_hw *hw, bool enable)
 {
-	u32 vt_ctl = rd32(E1000_VT_CTL);
+	u32 vt_ctl = E1000_READ_REG(hw, E1000_VT_CTL);
 
 	if (enable)
 		vt_ctl |= E1000_VT_CTL_VM_REPL_EN;
 	else
 		vt_ctl &= ~E1000_VT_CTL_VM_REPL_EN;
 
-	wr32(E1000_VT_CTL, vt_ctl);
+	E1000_WRITE_REG(hw, E1000_VT_CTL, vt_ctl);
 }
 
 /**
- *  igb_read_phy_reg_82580 - Read 82580 MDI control register
+ *  e1000_read_phy_reg_82580 - Read 82580 MDI control register
  *  @hw: pointer to the HW structure
  *  @offset: register offset to be read
  *  @data: pointer to the read data
@@ -1622,16 +2349,17 @@ void igb_vmdq_set_replication_pf(struct e1000_hw *hw, bool enable)
  *  Reads the MDI control register in the PHY at offset and stores the
  *  information read to data.
  **/
-static s32 igb_read_phy_reg_82580(struct e1000_hw *hw, u32 offset, u16 *data)
+static s32 e1000_read_phy_reg_82580(struct e1000_hw *hw, u32 offset, u16 *data)
 {
 	s32 ret_val;
 
+	DEBUGFUNC("e1000_read_phy_reg_82580");
 
 	ret_val = hw->phy.ops.acquire(hw);
 	if (ret_val)
 		goto out;
 
-	ret_val = igb_read_phy_reg_mdic(hw, offset, data);
+	ret_val = e1000_read_phy_reg_mdic(hw, offset, data);
 
 	hw->phy.ops.release(hw);
 
@@ -1640,23 +2368,24 @@ out:
 }
 
 /**
- *  igb_write_phy_reg_82580 - Write 82580 MDI control register
+ *  e1000_write_phy_reg_82580 - Write 82580 MDI control register
  *  @hw: pointer to the HW structure
  *  @offset: register offset to write to
  *  @data: data to write to register at offset
  *
  *  Writes data to MDI control register in the PHY at offset.
  **/
-static s32 igb_write_phy_reg_82580(struct e1000_hw *hw, u32 offset, u16 data)
+static s32 e1000_write_phy_reg_82580(struct e1000_hw *hw, u32 offset, u16 data)
 {
 	s32 ret_val;
 
+	DEBUGFUNC("e1000_write_phy_reg_82580");
 
 	ret_val = hw->phy.ops.acquire(hw);
 	if (ret_val)
 		goto out;
 
-	ret_val = igb_write_phy_reg_mdic(hw, offset, data);
+	ret_val = e1000_write_phy_reg_mdic(hw, offset, data);
 
 	hw->phy.ops.release(hw);
 
@@ -1665,134 +2394,137 @@ out:
 }
 
 /**
- *  igb_reset_mdicnfg_82580 - Reset MDICNFG destination and com_mdio bits
+ *  e1000_reset_mdicnfg_82580 - Reset MDICNFG destination and com_mdio bits
  *  @hw: pointer to the HW structure
  *
  *  This resets the the MDICNFG.Destination and MDICNFG.Com_MDIO bits based on
  *  the values found in the EEPROM.  This addresses an issue in which these
  *  bits are not restored from EEPROM after reset.
  **/
-static s32 igb_reset_mdicnfg_82580(struct e1000_hw *hw)
+static s32 e1000_reset_mdicnfg_82580(struct e1000_hw *hw)
 {
-	s32 ret_val = 0;
+	s32 ret_val = E1000_SUCCESS;
 	u32 mdicnfg;
 	u16 nvm_data = 0;
 
+	DEBUGFUNC("e1000_reset_mdicnfg_82580");
+
 	if (hw->mac.type != e1000_82580)
 		goto out;
-	if (!igb_sgmii_active_82575(hw))
+	if (!e1000_sgmii_active_82575(hw))
 		goto out;
 
 	ret_val = hw->nvm.ops.read(hw, NVM_INIT_CONTROL3_PORT_A +
 				   NVM_82580_LAN_FUNC_OFFSET(hw->bus.func), 1,
 				   &nvm_data);
 	if (ret_val) {
-		hw_dbg("NVM Read Error\n");
+		DEBUGOUT("NVM Read Error\n");
 		goto out;
 	}
 
-	mdicnfg = rd32(E1000_MDICNFG);
+	mdicnfg = E1000_READ_REG(hw, E1000_MDICNFG);
 	if (nvm_data & NVM_WORD24_EXT_MDIO)
 		mdicnfg |= E1000_MDICNFG_EXT_MDIO;
 	if (nvm_data & NVM_WORD24_COM_MDIO)
 		mdicnfg |= E1000_MDICNFG_COM_MDIO;
-	wr32(E1000_MDICNFG, mdicnfg);
+	E1000_WRITE_REG(hw, E1000_MDICNFG, mdicnfg);
 out:
 	return ret_val;
 }
 
 /**
- *  igb_reset_hw_82580 - Reset hardware
+ *  e1000_reset_hw_82580 - Reset hardware
  *  @hw: pointer to the HW structure
  *
  *  This resets function or entire device (all ports, etc.)
  *  to a known state.
  **/
-static s32 igb_reset_hw_82580(struct e1000_hw *hw)
+static s32 e1000_reset_hw_82580(struct e1000_hw *hw)
 {
-	s32 ret_val = 0;
+	s32 ret_val = E1000_SUCCESS;
 	/* BH SW mailbox bit in SW_FW_SYNC */
 	u16 swmbsw_mask = E1000_SW_SYNCH_MB;
-	u32 ctrl, icr;
+	u32 ctrl;
 	bool global_device_reset = hw->dev_spec._82575.global_device_reset;
 
+	DEBUGFUNC("e1000_reset_hw_82580");
 
 	hw->dev_spec._82575.global_device_reset = false;
 
+	/* 82580 does not reliably do global_device_reset due to hw errata */
+	if (hw->mac.type == e1000_82580)
+		global_device_reset = false;
+
 	/* Get current control state. */
-	ctrl = rd32(E1000_CTRL);
+	ctrl = E1000_READ_REG(hw, E1000_CTRL);
 
 	/*
 	 * Prevent the PCI-E bus from sticking if there is no TLP connection
 	 * on the last TLP read/write transaction when MAC is reset.
 	 */
-	ret_val = igb_disable_pcie_master(hw);
+	ret_val = e1000_disable_pcie_master_generic(hw);
 	if (ret_val)
-		hw_dbg("PCI-E Master disable polling has failed.\n");
+		DEBUGOUT("PCI-E Master disable polling has failed.\n");
 
-	hw_dbg("Masking off all interrupts\n");
-	wr32(E1000_IMC, 0xffffffff);
-	wr32(E1000_RCTL, 0);
-	wr32(E1000_TCTL, E1000_TCTL_PSP);
-	wrfl();
+	DEBUGOUT("Masking off all interrupts\n");
+	E1000_WRITE_REG(hw, E1000_IMC, 0xffffffff);
+	E1000_WRITE_REG(hw, E1000_RCTL, 0);
+	E1000_WRITE_REG(hw, E1000_TCTL, E1000_TCTL_PSP);
+	E1000_WRITE_FLUSH(hw);
 
-	msleep(10);
+	msec_delay(10);
 
 	/* Determine whether or not a global dev reset is requested */
-	if (global_device_reset &&
-		igb_acquire_swfw_sync_82575(hw, swmbsw_mask))
+	if (global_device_reset && hw->mac.ops.acquire_swfw_sync(hw,
+	    swmbsw_mask))
 			global_device_reset = false;
 
-	if (global_device_reset &&
-		!(rd32(E1000_STATUS) & E1000_STAT_DEV_RST_SET))
+	if (global_device_reset && !(E1000_READ_REG(hw, E1000_STATUS) &
+	    E1000_STAT_DEV_RST_SET))
 		ctrl |= E1000_CTRL_DEV_RST;
 	else
 		ctrl |= E1000_CTRL_RST;
 
-	wr32(E1000_CTRL, ctrl);
-	wrfl();
+	E1000_WRITE_REG(hw, E1000_CTRL, ctrl);
+	E1000_WRITE_FLUSH(hw);
 
 	/* Add delay to insure DEV_RST has time to complete */
 	if (global_device_reset)
-		msleep(5);
+		msec_delay(5);
 
-	ret_val = igb_get_auto_rd_done(hw);
+	ret_val = e1000_get_auto_rd_done_generic(hw);
 	if (ret_val) {
 		/*
 		 * When auto config read does not complete, do not
 		 * return with an error. This can happen in situations
 		 * where there is no eeprom and prevents getting link.
 		 */
-		hw_dbg("Auto Read Done did not complete\n");
+		DEBUGOUT("Auto Read Done did not complete\n");
 	}
 
-	/* If EEPROM is not present, run manual init scripts */
-	if ((rd32(E1000_EECD) & E1000_EECD_PRES) == 0)
-		igb_reset_init_script_82575(hw);
-
 	/* clear global device reset status bit */
-	wr32(E1000_STATUS, E1000_STAT_DEV_RST_SET);
+	E1000_WRITE_REG(hw, E1000_STATUS, E1000_STAT_DEV_RST_SET);
 
 	/* Clear any pending interrupt events. */
-	wr32(E1000_IMC, 0xffffffff);
-	icr = rd32(E1000_ICR);
+	E1000_WRITE_REG(hw, E1000_IMC, 0xffffffff);
+	E1000_READ_REG(hw, E1000_ICR);
 
-	ret_val = igb_reset_mdicnfg_82580(hw);
+	ret_val = e1000_reset_mdicnfg_82580(hw);
 	if (ret_val)
-		hw_dbg("Could not reset MDICNFG based on EEPROM\n");
+		DEBUGOUT("Could not reset MDICNFG based on EEPROM\n");
 
 	/* Install any alternate MAC address into RAR0 */
-	ret_val = igb_check_alt_mac_addr(hw);
+	ret_val = e1000_check_alt_mac_addr_generic(hw);
 
 	/* Release semaphore */
 	if (global_device_reset)
-		igb_release_swfw_sync_82575(hw, swmbsw_mask);
+		hw->mac.ops.release_swfw_sync(hw, swmbsw_mask);
 
 	return ret_val;
 }
 
 /**
- *  igb_rxpbs_adjust_82580 - adjust RXPBS value to reflect actual RX PBA size
+ *  e1000_rxpbs_adjust_82580 - adjust RXPBS value to reflect actual Rx PBA size
  *  @data: data received by reading RXPBS register
  *
  *  The 82580 uses a table based approach for packet buffer allocation sizes.
@@ -1801,7 +2533,7 @@ static s32 igb_reset_hw_82580(struct e1000_hw *hw)
  *  0x0 36  72 144   1   2   4   8  16
  *  0x8 35  70 140 rsv rsv rsv rsv rsv
  */
-u16 igb_rxpbs_adjust_82580(u32 data)
+u16 e1000_rxpbs_adjust_82580(u32 data)
 {
 	u16 ret_val = 0;
 
@@ -1812,7 +2544,7 @@ u16 igb_rxpbs_adjust_82580(u32 data)
 }
 
 /**
- *  igb_validate_nvm_checksum_with_offset - Validate EEPROM
+ *  e1000_validate_nvm_checksum_with_offset - Validate EEPROM
  *  checksum
  *  @hw: pointer to the HW structure
  *  @offset: offset in words of the checksum protected region
@@ -1820,23 +2552,25 @@ u16 igb_rxpbs_adjust_82580(u32 data)
  *  Calculates the EEPROM checksum by reading/adding each word of the EEPROM
  *  and then verifies that the sum of the EEPROM is equal to 0xBABA.
  **/
-s32 igb_validate_nvm_checksum_with_offset(struct e1000_hw *hw, u16 offset)
+s32 e1000_validate_nvm_checksum_with_offset(struct e1000_hw *hw, u16 offset)
 {
-	s32 ret_val = 0;
+	s32 ret_val = E1000_SUCCESS;
 	u16 checksum = 0;
 	u16 i, nvm_data;
 
+	DEBUGFUNC("e1000_validate_nvm_checksum_with_offset");
+
 	for (i = offset; i < ((NVM_CHECKSUM_REG + offset) + 1); i++) {
 		ret_val = hw->nvm.ops.read(hw, i, 1, &nvm_data);
 		if (ret_val) {
-			hw_dbg("NVM Read Error\n");
+			DEBUGOUT("NVM Read Error\n");
 			goto out;
 		}
 		checksum += nvm_data;
 	}
 
 	if (checksum != (u16) NVM_SUM) {
-		hw_dbg("NVM Checksum Invalid\n");
+		DEBUGOUT("NVM Checksum Invalid\n");
 		ret_val = -E1000_ERR_NVM;
 		goto out;
 	}
@@ -1846,7 +2580,7 @@ out:
 }
 
 /**
- *  igb_update_nvm_checksum_with_offset - Update EEPROM
+ *  e1000_update_nvm_checksum_with_offset - Update EEPROM
  *  checksum
  *  @hw: pointer to the HW structure
  *  @offset: offset in words of the checksum protected region
@@ -1855,62 +2589,66 @@ out:
  *  up to the checksum.  Then calculates the EEPROM checksum and writes the
  *  value to the EEPROM.
  **/
-s32 igb_update_nvm_checksum_with_offset(struct e1000_hw *hw, u16 offset)
+s32 e1000_update_nvm_checksum_with_offset(struct e1000_hw *hw, u16 offset)
 {
 	s32 ret_val;
 	u16 checksum = 0;
 	u16 i, nvm_data;
 
+	DEBUGFUNC("e1000_update_nvm_checksum_with_offset");
+
 	for (i = offset; i < (NVM_CHECKSUM_REG + offset); i++) {
 		ret_val = hw->nvm.ops.read(hw, i, 1, &nvm_data);
 		if (ret_val) {
-			hw_dbg("NVM Read Error while updating checksum.\n");
+			DEBUGOUT("NVM Read Error while updating checksum.\n");
 			goto out;
 		}
 		checksum += nvm_data;
 	}
 	checksum = (u16) NVM_SUM - checksum;
 	ret_val = hw->nvm.ops.write(hw, (NVM_CHECKSUM_REG + offset), 1,
-				&checksum);
+				    &checksum);
 	if (ret_val)
-		hw_dbg("NVM Write Error while updating checksum.\n");
+		DEBUGOUT("NVM Write Error while updating checksum.\n");
 
 out:
 	return ret_val;
 }
 
 /**
- *  igb_validate_nvm_checksum_82580 - Validate EEPROM checksum
+ *  e1000_validate_nvm_checksum_82580 - Validate EEPROM checksum
  *  @hw: pointer to the HW structure
  *
  *  Calculates the EEPROM section checksum by reading/adding each word of
  *  the EEPROM and then verifies that the sum of the EEPROM is
  *  equal to 0xBABA.
  **/
-static s32 igb_validate_nvm_checksum_82580(struct e1000_hw *hw)
+static s32 e1000_validate_nvm_checksum_82580(struct e1000_hw *hw)
 {
-	s32 ret_val = 0;
+	s32 ret_val = E1000_SUCCESS;
 	u16 eeprom_regions_count = 1;
 	u16 j, nvm_data;
 	u16 nvm_offset;
 
+	DEBUGFUNC("e1000_validate_nvm_checksum_82580");
+
 	ret_val = hw->nvm.ops.read(hw, NVM_COMPATIBILITY_REG_3, 1, &nvm_data);
 	if (ret_val) {
-		hw_dbg("NVM Read Error\n");
+		DEBUGOUT("NVM Read Error\n");
 		goto out;
 	}
 
 	if (nvm_data & NVM_COMPATIBILITY_BIT_MASK) {
-		/* if checksums compatibility bit is set validate checksums
+		/* if chekcsums compatibility bit is set validate checksums
 		 * for all 4 ports. */
 		eeprom_regions_count = 4;
 	}
 
 	for (j = 0; j < eeprom_regions_count; j++) {
 		nvm_offset = NVM_82580_LAN_FUNC_OFFSET(j);
-		ret_val = igb_validate_nvm_checksum_with_offset(hw,
-								nvm_offset);
-		if (ret_val != 0)
+		ret_val = e1000_validate_nvm_checksum_with_offset(hw,
+								  nvm_offset);
+		if (ret_val != E1000_SUCCESS)
 			goto out;
 	}
 
@@ -1919,41 +2657,41 @@ out:
 }
 
 /**
- *  igb_update_nvm_checksum_82580 - Update EEPROM checksum
+ *  e1000_update_nvm_checksum_82580 - Update EEPROM checksum
  *  @hw: pointer to the HW structure
  *
  *  Updates the EEPROM section checksums for all 4 ports by reading/adding
  *  each word of the EEPROM up to the checksum.  Then calculates the EEPROM
  *  checksum and writes the value to the EEPROM.
  **/
-static s32 igb_update_nvm_checksum_82580(struct e1000_hw *hw)
+static s32 e1000_update_nvm_checksum_82580(struct e1000_hw *hw)
 {
 	s32 ret_val;
 	u16 j, nvm_data;
 	u16 nvm_offset;
 
+	DEBUGFUNC("e1000_update_nvm_checksum_82580");
+
 	ret_val = hw->nvm.ops.read(hw, NVM_COMPATIBILITY_REG_3, 1, &nvm_data);
 	if (ret_val) {
-		hw_dbg("NVM Read Error while updating checksum"
-			" compatibility bit.\n");
+		DEBUGOUT("NVM Read Error while updating checksum compatibility bit.\n");
 		goto out;
 	}
 
-	if ((nvm_data & NVM_COMPATIBILITY_BIT_MASK) == 0) {
+	if (!(nvm_data & NVM_COMPATIBILITY_BIT_MASK)) {
 		/* set compatibility bit to validate checksums appropriately */
 		nvm_data = nvm_data | NVM_COMPATIBILITY_BIT_MASK;
 		ret_val = hw->nvm.ops.write(hw, NVM_COMPATIBILITY_REG_3, 1,
-					&nvm_data);
+					    &nvm_data);
 		if (ret_val) {
-			hw_dbg("NVM Write Error while updating checksum"
-				" compatibility bit.\n");
+			DEBUGOUT("NVM Write Error while updating checksum compatibility bit.\n");
 			goto out;
 		}
 	}
 
 	for (j = 0; j < 4; j++) {
 		nvm_offset = NVM_82580_LAN_FUNC_OFFSET(j);
-		ret_val = igb_update_nvm_checksum_with_offset(hw, nvm_offset);
+		ret_val = e1000_update_nvm_checksum_with_offset(hw, nvm_offset);
 		if (ret_val)
 			goto out;
 	}
@@ -1963,24 +2701,26 @@ out:
 }
 
 /**
- *  igb_validate_nvm_checksum_i350 - Validate EEPROM checksum
+ *  e1000_validate_nvm_checksum_i350 - Validate EEPROM checksum
  *  @hw: pointer to the HW structure
  *
  *  Calculates the EEPROM section checksum by reading/adding each word of
  *  the EEPROM and then verifies that the sum of the EEPROM is
  *  equal to 0xBABA.
  **/
-static s32 igb_validate_nvm_checksum_i350(struct e1000_hw *hw)
+static s32 e1000_validate_nvm_checksum_i350(struct e1000_hw *hw)
 {
-	s32 ret_val = 0;
+	s32 ret_val = E1000_SUCCESS;
 	u16 j;
 	u16 nvm_offset;
 
+	DEBUGFUNC("e1000_validate_nvm_checksum_i350");
+
 	for (j = 0; j < 4; j++) {
 		nvm_offset = NVM_82580_LAN_FUNC_OFFSET(j);
-		ret_val = igb_validate_nvm_checksum_with_offset(hw,
-								nvm_offset);
-		if (ret_val != 0)
+		ret_val = e1000_validate_nvm_checksum_with_offset(hw,
+								  nvm_offset);
+		if (ret_val != E1000_SUCCESS)
 			goto out;
 	}
 
@@ -1989,23 +2729,25 @@ out:
 }
 
 /**
- *  igb_update_nvm_checksum_i350 - Update EEPROM checksum
+ *  e1000_update_nvm_checksum_i350 - Update EEPROM checksum
  *  @hw: pointer to the HW structure
  *
  *  Updates the EEPROM section checksums for all 4 ports by reading/adding
  *  each word of the EEPROM up to the checksum.  Then calculates the EEPROM
  *  checksum and writes the value to the EEPROM.
  **/
-static s32 igb_update_nvm_checksum_i350(struct e1000_hw *hw)
+static s32 e1000_update_nvm_checksum_i350(struct e1000_hw *hw)
 {
-	s32 ret_val = 0;
+	s32 ret_val = E1000_SUCCESS;
 	u16 j;
 	u16 nvm_offset;
 
+	DEBUGFUNC("e1000_update_nvm_checksum_i350");
+
 	for (j = 0; j < 4; j++) {
 		nvm_offset = NVM_82580_LAN_FUNC_OFFSET(j);
-		ret_val = igb_update_nvm_checksum_with_offset(hw, nvm_offset);
-		if (ret_val != 0)
+		ret_val = e1000_update_nvm_checksum_with_offset(hw, nvm_offset);
+		if (ret_val != E1000_SUCCESS)
 			goto out;
 	}
 
@@ -2014,71 +2756,1018 @@ out:
 }
 
 /**
- *  igb_set_eee_i350 - Enable/disable EEE support
+ *  __e1000_access_emi_reg - Read/write EMI register
+ *  @hw: pointer to the HW structure
+ *  @addr: EMI address to program
+ *  @data: pointer to value to read/write from/to the EMI address
+ *  @read: boolean flag to indicate read or write
+ **/
+static s32 __e1000_access_emi_reg(struct e1000_hw *hw, u16 address,
+				  u16 *data, bool read)
+{
+	s32 ret_val = E1000_SUCCESS;
+
+	DEBUGFUNC("__e1000_access_emi_reg");
+
+	ret_val = hw->phy.ops.write_reg(hw, E1000_EMIADD, address);
+	if (ret_val)
+		return ret_val;
+
+	if (read)
+		ret_val = hw->phy.ops.read_reg(hw, E1000_EMIDATA, data);
+	else
+		ret_val = hw->phy.ops.write_reg(hw, E1000_EMIDATA, *data);
+
+	return ret_val;
+}
+
+/**
+ *  e1000_read_emi_reg - Read Extended Management Interface register
+ *  @hw: pointer to the HW structure
+ *  @addr: EMI address to program
+ *  @data: value to be read from the EMI address
+ **/
+s32 e1000_read_emi_reg(struct e1000_hw *hw, u16 addr, u16 *data)
+{
+	DEBUGFUNC("e1000_read_emi_reg");
+
+	return __e1000_access_emi_reg(hw, addr, data, true);
+}
+
+/**
+ *  e1000_initialize_M88E1512_phy - Initialize M88E1512 PHY
+ *  @hw: pointer to the HW structure
+ *
+ *  Initialize Marverl 1512 to work correctly with Avoton.
+ **/
+s32 e1000_initialize_M88E1512_phy(struct e1000_hw *hw)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	s32 ret_val = E1000_SUCCESS;
+
+	DEBUGFUNC("e1000_initialize_M88E1512_phy");
+
+	/* Check if this is correct PHY. */
+	if (phy->id != M88E1512_E_PHY_ID)
+		goto out;
+
+	/* Switch to PHY page 0xFF. */
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1543_PAGE_ADDR, 0x00FF);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_2, 0x214B);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_1, 0x2144);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_2, 0x0C28);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_1, 0x2146);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_2, 0xB233);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_1, 0x214D);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_2, 0xCC0C);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_1, 0x2159);
+	if (ret_val)
+		goto out;
+
+	/* Switch to PHY page 0xFB. */
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1543_PAGE_ADDR, 0x00FB);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_3, 0x000D);
+	if (ret_val)
+		goto out;
+
+	/* Switch to PHY page 0x12. */
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1543_PAGE_ADDR, 0x12);
+	if (ret_val)
+		goto out;
+
+	/* Change mode to SGMII-to-Copper */
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_MODE, 0x8001);
+	if (ret_val)
+		goto out;
+
+	/* Return the PHY to page 0. */
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1543_PAGE_ADDR, 0);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.commit(hw);
+	if (ret_val) {
+		DEBUGOUT("Error committing the PHY changes\n");
+		return ret_val;
+	}
+
+	msec_delay(1000);
+out:
+	return ret_val;
+}
+
+/**
+ *  e1000_set_eee_i350 - Enable/disable EEE support
  *  @hw: pointer to the HW structure
  *
  *  Enable/disable EEE based on setting in dev_spec structure.
  *
  **/
-s32 igb_set_eee_i350(struct e1000_hw *hw)
+s32 e1000_set_eee_i350(struct e1000_hw *hw)
 {
-	s32 ret_val = 0;
-	u32 ipcnfg, eeer, ctrl_ext;
+	s32 ret_val = E1000_SUCCESS;
+	u32 ipcnfg, eeer;
 
-	ctrl_ext = rd32(E1000_CTRL_EXT);
-	if ((hw->mac.type != e1000_i350) ||
-	    (ctrl_ext & E1000_CTRL_EXT_LINK_MODE_MASK))
+	DEBUGFUNC("e1000_set_eee_i350");
+
+	if ((hw->mac.type < e1000_i350) ||
+	    (hw->phy.media_type != e1000_media_type_copper))
 		goto out;
-	ipcnfg = rd32(E1000_IPCNFG);
-	eeer = rd32(E1000_EEER);
+	ipcnfg = E1000_READ_REG(hw, E1000_IPCNFG);
+	eeer = E1000_READ_REG(hw, E1000_EEER);
 
 	/* enable or disable per user setting */
 	if (!(hw->dev_spec._82575.eee_disable)) {
-		ipcnfg |= (E1000_IPCNFG_EEE_1G_AN |
-			E1000_IPCNFG_EEE_100M_AN);
-		eeer |= (E1000_EEER_TX_LPI_EN |
-			E1000_EEER_RX_LPI_EN |
-			E1000_EEER_LPI_FC);
+		u32 eee_su = E1000_READ_REG(hw, E1000_EEE_SU);
+
+		ipcnfg |= (E1000_IPCNFG_EEE_1G_AN | E1000_IPCNFG_EEE_100M_AN);
+		eeer |= (E1000_EEER_TX_LPI_EN | E1000_EEER_RX_LPI_EN |
+			 E1000_EEER_LPI_FC);
 
+		/* This bit should not be set in normal operation. */
+		if (eee_su & E1000_EEE_SU_LPI_CLK_STP)
+			DEBUGOUT("LPI Clock Stop Bit should not be set!\n");
 	} else {
-		ipcnfg &= ~(E1000_IPCNFG_EEE_1G_AN |
-			E1000_IPCNFG_EEE_100M_AN);
-		eeer &= ~(E1000_EEER_TX_LPI_EN |
-			E1000_EEER_RX_LPI_EN |
-			E1000_EEER_LPI_FC);
-	}
-	wr32(E1000_IPCNFG, ipcnfg);
-	wr32(E1000_EEER, eeer);
+		ipcnfg &= ~(E1000_IPCNFG_EEE_1G_AN | E1000_IPCNFG_EEE_100M_AN);
+		eeer &= ~(E1000_EEER_TX_LPI_EN | E1000_EEER_RX_LPI_EN |
+			  E1000_EEER_LPI_FC);
+	}
+	E1000_WRITE_REG(hw, E1000_IPCNFG, ipcnfg);
+	E1000_WRITE_REG(hw, E1000_EEER, eeer);
+	E1000_READ_REG(hw, E1000_IPCNFG);
+	E1000_READ_REG(hw, E1000_EEER);
 out:
 
 	return ret_val;
 }
 
-static struct e1000_mac_operations e1000_mac_ops_82575 = {
-	.init_hw              = igb_init_hw_82575,
-	.check_for_link       = igb_check_for_link_82575,
-	.rar_set              = igb_rar_set,
-	.read_mac_addr        = igb_read_mac_addr_82575,
-	.get_speed_and_duplex = igb_get_speed_and_duplex_copper,
-};
+/**
+ *  e1000_set_eee_i354 - Enable/disable EEE support
+ *  @hw: pointer to the HW structure
+ *
+ *  Enable/disable EEE legacy mode based on setting in dev_spec structure.
+ *
+ **/
+s32 e1000_set_eee_i354(struct e1000_hw *hw)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	s32 ret_val = E1000_SUCCESS;
+	u16 phy_data;
 
-static struct e1000_phy_operations e1000_phy_ops_82575 = {
-	.acquire              = igb_acquire_phy_82575,
-	.get_cfg_done         = igb_get_cfg_done_82575,
-	.release              = igb_release_phy_82575,
-};
+	DEBUGFUNC("e1000_set_eee_i354");
 
-static struct e1000_nvm_operations e1000_nvm_ops_82575 = {
-	.acquire              = igb_acquire_nvm_82575,
-	.read                 = igb_read_nvm_eerd,
-	.release              = igb_release_nvm_82575,
-	.write                = igb_write_nvm_spi,
-};
+	if ((hw->phy.media_type != e1000_media_type_copper) ||
+	    ((phy->id != M88E1543_E_PHY_ID) &&
+	    (phy->id != M88E1512_E_PHY_ID)))
+		goto out;
+
+	if (!hw->dev_spec._82575.eee_disable) {
+		/* Switch to PHY page 18. */
+		ret_val = phy->ops.write_reg(hw, E1000_M88E1543_PAGE_ADDR, 18);
+		if (ret_val)
+			goto out;
+
+		ret_val = phy->ops.read_reg(hw, E1000_M88E1543_EEE_CTRL_1,
+					    &phy_data);
+		if (ret_val)
+			goto out;
+
+		phy_data |= E1000_M88E1543_EEE_CTRL_1_MS;
+		ret_val = phy->ops.write_reg(hw, E1000_M88E1543_EEE_CTRL_1,
+					     phy_data);
+		if (ret_val)
+			goto out;
+
+		/* Return the PHY to page 0. */
+		ret_val = phy->ops.write_reg(hw, E1000_M88E1543_PAGE_ADDR, 0);
+		if (ret_val)
+			goto out;
+
+		/* Turn on EEE advertisement. */
+		ret_val = e1000_read_xmdio_reg(hw, E1000_EEE_ADV_ADDR_I354,
+					       E1000_EEE_ADV_DEV_I354,
+					       &phy_data);
+		if (ret_val)
+			goto out;
+
+		phy_data |= E1000_EEE_ADV_100_SUPPORTED |
+			    E1000_EEE_ADV_1000_SUPPORTED;
+		ret_val = e1000_write_xmdio_reg(hw, E1000_EEE_ADV_ADDR_I354,
+						E1000_EEE_ADV_DEV_I354,
+						phy_data);
+	} else {
+		/* Turn off EEE advertisement. */
+		ret_val = e1000_read_xmdio_reg(hw, E1000_EEE_ADV_ADDR_I354,
+					       E1000_EEE_ADV_DEV_I354,
+					       &phy_data);
+		if (ret_val)
+			goto out;
+
+		phy_data &= ~(E1000_EEE_ADV_100_SUPPORTED |
+			      E1000_EEE_ADV_1000_SUPPORTED);
+		ret_val = e1000_write_xmdio_reg(hw, E1000_EEE_ADV_ADDR_I354,
+						E1000_EEE_ADV_DEV_I354,
+						phy_data);
+	}
+
+out:
+	return ret_val;
+}
+
+/**
+ *  e1000_get_eee_status_i354 - Get EEE status
+ *  @hw: pointer to the HW structure
+ *  @status: EEE status
+ *
+ *  Get EEE status by guessing based on whether Tx or Rx LPI indications have
+ *  been received.
+ **/
+s32 e1000_get_eee_status_i354(struct e1000_hw *hw, bool *status)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	s32 ret_val = E1000_SUCCESS;
+	u16 phy_data;
+
+	DEBUGFUNC("e1000_get_eee_status_i354");
+
+	/* Check if EEE is supported on this device. */
+	if ((hw->phy.media_type != e1000_media_type_copper) ||
+	    ((phy->id != M88E1543_E_PHY_ID) &&
+	    (phy->id != M88E1512_E_PHY_ID)))
+		goto out;
+
+	ret_val = e1000_read_xmdio_reg(hw, E1000_PCS_STATUS_ADDR_I354,
+				       E1000_PCS_STATUS_DEV_I354,
+				       &phy_data);
+	if (ret_val)
+		goto out;
+
+	*status = phy_data & (E1000_PCS_STATUS_TX_LPI_RCVD |
+			      E1000_PCS_STATUS_RX_LPI_RCVD) ? true : false;
+
+out:
+	return ret_val;
+}
+
+/* Due to a hw errata, if the host tries to  configure the VFTA register
+ * while performing queries from the BMC or DMA, then the VFTA in some
+ * cases won't be written.
+ */
+
+/**
+ *  e1000_clear_vfta_i350 - Clear VLAN filter table
+ *  @hw: pointer to the HW structure
+ *
+ *  Clears the register array which contains the VLAN filter table by
+ *  setting all the values to 0.
+ **/
+void e1000_clear_vfta_i350(struct e1000_hw *hw)
+{
+	u32 offset;
+	int i;
+
+	DEBUGFUNC("e1000_clear_vfta_350");
+
+	for (offset = 0; offset < E1000_VLAN_FILTER_TBL_SIZE; offset++) {
+		for (i = 0; i < 10; i++)
+			E1000_WRITE_REG_ARRAY(hw, E1000_VFTA, offset, 0);
+
+		E1000_WRITE_FLUSH(hw);
+	}
+}
+
+/**
+ *  e1000_write_vfta_i350 - Write value to VLAN filter table
+ *  @hw: pointer to the HW structure
+ *  @offset: register offset in VLAN filter table
+ *  @value: register value written to VLAN filter table
+ *
+ *  Writes value at the given offset in the register array which stores
+ *  the VLAN filter table.
+ **/
+void e1000_write_vfta_i350(struct e1000_hw *hw, u32 offset, u32 value)
+{
+	int i;
+
+	DEBUGFUNC("e1000_write_vfta_350");
+
+	for (i = 0; i < 10; i++)
+		E1000_WRITE_REG_ARRAY(hw, E1000_VFTA, offset, value);
 
-const struct e1000_info e1000_82575_info = {
-	.get_invariants = igb_get_invariants_82575,
-	.mac_ops = &e1000_mac_ops_82575,
-	.phy_ops = &e1000_phy_ops_82575,
-	.nvm_ops = &e1000_nvm_ops_82575,
+	E1000_WRITE_FLUSH(hw);
+}
+
+
+/**
+ *  e1000_set_i2c_bb - Enable I2C bit-bang
+ *  @hw: pointer to the HW structure
+ *
+ *  Enable I2C bit-bang interface
+ *
+ **/
+s32 e1000_set_i2c_bb(struct e1000_hw *hw)
+{
+	s32 ret_val = E1000_SUCCESS;
+	u32 ctrl_ext, i2cparams;
+
+	DEBUGFUNC("e1000_set_i2c_bb");
+
+	ctrl_ext = E1000_READ_REG(hw, E1000_CTRL_EXT);
+	ctrl_ext |= E1000_CTRL_I2C_ENA;
+	E1000_WRITE_REG(hw, E1000_CTRL_EXT, ctrl_ext);
+	E1000_WRITE_FLUSH(hw);
+
+	i2cparams = E1000_READ_REG(hw, E1000_I2CPARAMS);
+	i2cparams |= E1000_I2CBB_EN;
+	i2cparams |= E1000_I2C_DATA_OE_N;
+	i2cparams |= E1000_I2C_CLK_OE_N;
+	E1000_WRITE_REG(hw, E1000_I2CPARAMS, i2cparams);
+	E1000_WRITE_FLUSH(hw);
+
+	return ret_val;
+}
+
+/**
+ *  e1000_read_i2c_byte_generic - Reads 8 bit word over I2C
+ *  @hw: pointer to hardware structure
+ *  @byte_offset: byte offset to read
+ *  @dev_addr: device address
+ *  @data: value read
+ *
+ *  Performs byte read operation over I2C interface at
+ *  a specified device address.
+ **/
+s32 e1000_read_i2c_byte_generic(struct e1000_hw *hw, u8 byte_offset,
+				u8 dev_addr, u8 *data)
+{
+	s32 status = E1000_SUCCESS;
+	u32 max_retry = 10;
+	u32 retry = 1;
+	u16 swfw_mask = 0;
+
+	bool nack = true;
+
+	DEBUGFUNC("e1000_read_i2c_byte_generic");
+
+	swfw_mask = E1000_SWFW_PHY0_SM;
+
+	do {
+		if (hw->mac.ops.acquire_swfw_sync(hw, swfw_mask)
+		    != E1000_SUCCESS) {
+			status = E1000_ERR_SWFW_SYNC;
+			goto read_byte_out;
+		}
+
+		e1000_i2c_start(hw);
+
+		/* Device Address and write indication */
+		status = e1000_clock_out_i2c_byte(hw, dev_addr);
+		if (status != E1000_SUCCESS)
+			goto fail;
+
+		status = e1000_get_i2c_ack(hw);
+		if (status != E1000_SUCCESS)
+			goto fail;
+
+		status = e1000_clock_out_i2c_byte(hw, byte_offset);
+		if (status != E1000_SUCCESS)
+			goto fail;
+
+		status = e1000_get_i2c_ack(hw);
+		if (status != E1000_SUCCESS)
+			goto fail;
+
+		e1000_i2c_start(hw);
+
+		/* Device Address and read indication */
+		status = e1000_clock_out_i2c_byte(hw, (dev_addr | 0x1));
+		if (status != E1000_SUCCESS)
+			goto fail;
+
+		status = e1000_get_i2c_ack(hw);
+		if (status != E1000_SUCCESS)
+			goto fail;
+
+		status = e1000_clock_in_i2c_byte(hw, data);
+		if (status != E1000_SUCCESS)
+			goto fail;
+
+		status = e1000_clock_out_i2c_bit(hw, nack);
+		if (status != E1000_SUCCESS)
+			goto fail;
+
+		e1000_i2c_stop(hw);
+		break;
+
+fail:
+		hw->mac.ops.release_swfw_sync(hw, swfw_mask);
+		msec_delay(100);
+		e1000_i2c_bus_clear(hw);
+		retry++;
+		if (retry < max_retry)
+			DEBUGOUT("I2C byte read error - Retrying.\n");
+		else
+			DEBUGOUT("I2C byte read error.\n");
+
+	} while (retry < max_retry);
+
+	hw->mac.ops.release_swfw_sync(hw, swfw_mask);
+
+read_byte_out:
+
+	return status;
+}
+
+/**
+ *  e1000_write_i2c_byte_generic - Writes 8 bit word over I2C
+ *  @hw: pointer to hardware structure
+ *  @byte_offset: byte offset to write
+ *  @dev_addr: device address
+ *  @data: value to write
+ *
+ *  Performs byte write operation over I2C interface at
+ *  a specified device address.
+ **/
+s32 e1000_write_i2c_byte_generic(struct e1000_hw *hw, u8 byte_offset,
+				 u8 dev_addr, u8 data)
+{
+	s32 status = E1000_SUCCESS;
+	u32 max_retry = 1;
+	u32 retry = 0;
+	u16 swfw_mask = 0;
+
+	DEBUGFUNC("e1000_write_i2c_byte_generic");
+
+	swfw_mask = E1000_SWFW_PHY0_SM;
+
+	if (hw->mac.ops.acquire_swfw_sync(hw, swfw_mask) != E1000_SUCCESS) {
+		status = E1000_ERR_SWFW_SYNC;
+		goto write_byte_out;
+	}
+
+	do {
+		e1000_i2c_start(hw);
+
+		status = e1000_clock_out_i2c_byte(hw, dev_addr);
+		if (status != E1000_SUCCESS)
+			goto fail;
+
+		status = e1000_get_i2c_ack(hw);
+		if (status != E1000_SUCCESS)
+			goto fail;
+
+		status = e1000_clock_out_i2c_byte(hw, byte_offset);
+		if (status != E1000_SUCCESS)
+			goto fail;
+
+		status = e1000_get_i2c_ack(hw);
+		if (status != E1000_SUCCESS)
+			goto fail;
+
+		status = e1000_clock_out_i2c_byte(hw, data);
+		if (status != E1000_SUCCESS)
+			goto fail;
+
+		status = e1000_get_i2c_ack(hw);
+		if (status != E1000_SUCCESS)
+			goto fail;
+
+		e1000_i2c_stop(hw);
+		break;
+
+fail:
+		e1000_i2c_bus_clear(hw);
+		retry++;
+		if (retry < max_retry)
+			DEBUGOUT("I2C byte write error - Retrying.\n");
+		else
+			DEBUGOUT("I2C byte write error.\n");
+	} while (retry < max_retry);
+
+	hw->mac.ops.release_swfw_sync(hw, swfw_mask);
+
+write_byte_out:
+
+	return status;
+}
+
+/**
+ *  e1000_i2c_start - Sets I2C start condition
+ *  @hw: pointer to hardware structure
+ *
+ *  Sets I2C start condition (High -> Low on SDA while SCL is High)
+ **/
+static void e1000_i2c_start(struct e1000_hw *hw)
+{
+	u32 i2cctl = E1000_READ_REG(hw, E1000_I2CPARAMS);
+
+	DEBUGFUNC("e1000_i2c_start");
+
+	/* Start condition must begin with data and clock high */
+	e1000_set_i2c_data(hw, &i2cctl, 1);
+	e1000_raise_i2c_clk(hw, &i2cctl);
+
+	/* Setup time for start condition (4.7us) */
+	usec_delay(E1000_I2C_T_SU_STA);
+
+	e1000_set_i2c_data(hw, &i2cctl, 0);
+
+	/* Hold time for start condition (4us) */
+	usec_delay(E1000_I2C_T_HD_STA);
+
+	e1000_lower_i2c_clk(hw, &i2cctl);
+
+	/* Minimum low period of clock is 4.7 us */
+	usec_delay(E1000_I2C_T_LOW);
+
+}
+
+/**
+ *  e1000_i2c_stop - Sets I2C stop condition
+ *  @hw: pointer to hardware structure
+ *
+ *  Sets I2C stop condition (Low -> High on SDA while SCL is High)
+ **/
+static void e1000_i2c_stop(struct e1000_hw *hw)
+{
+	u32 i2cctl = E1000_READ_REG(hw, E1000_I2CPARAMS);
+
+	DEBUGFUNC("e1000_i2c_stop");
+
+	/* Stop condition must begin with data low and clock high */
+	e1000_set_i2c_data(hw, &i2cctl, 0);
+	e1000_raise_i2c_clk(hw, &i2cctl);
+
+	/* Setup time for stop condition (4us) */
+	usec_delay(E1000_I2C_T_SU_STO);
+
+	e1000_set_i2c_data(hw, &i2cctl, 1);
+
+	/* bus free time between stop and start (4.7us)*/
+	usec_delay(E1000_I2C_T_BUF);
+}
+
+/**
+ *  e1000_clock_in_i2c_byte - Clocks in one byte via I2C
+ *  @hw: pointer to hardware structure
+ *  @data: data byte to clock in
+ *
+ *  Clocks in one byte data via I2C data/clock
+ **/
+static s32 e1000_clock_in_i2c_byte(struct e1000_hw *hw, u8 *data)
+{
+	s32 i;
+	bool bit = 0;
+
+	DEBUGFUNC("e1000_clock_in_i2c_byte");
+
+	*data = 0;
+	for (i = 7; i >= 0; i--) {
+		e1000_clock_in_i2c_bit(hw, &bit);
+		*data |= bit << i;
+	}
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_clock_out_i2c_byte - Clocks out one byte via I2C
+ *  @hw: pointer to hardware structure
+ *  @data: data byte clocked out
+ *
+ *  Clocks out one byte data via I2C data/clock
+ **/
+static s32 e1000_clock_out_i2c_byte(struct e1000_hw *hw, u8 data)
+{
+	s32 status = E1000_SUCCESS;
+	s32 i;
+	u32 i2cctl;
+	bool bit = 0;
+
+	DEBUGFUNC("e1000_clock_out_i2c_byte");
+
+	for (i = 7; i >= 0; i--) {
+		bit = (data >> i) & 0x1;
+		status = e1000_clock_out_i2c_bit(hw, bit);
+
+		if (status != E1000_SUCCESS)
+			break;
+	}
+
+	/* Release SDA line (set high) */
+	i2cctl = E1000_READ_REG(hw, E1000_I2CPARAMS);
+
+	i2cctl |= E1000_I2C_DATA_OE_N;
+	E1000_WRITE_REG(hw, E1000_I2CPARAMS, i2cctl);
+	E1000_WRITE_FLUSH(hw);
+
+	return status;
+}
+
+/**
+ *  e1000_get_i2c_ack - Polls for I2C ACK
+ *  @hw: pointer to hardware structure
+ *
+ *  Clocks in/out one bit via I2C data/clock
+ **/
+static s32 e1000_get_i2c_ack(struct e1000_hw *hw)
+{
+	s32 status = E1000_SUCCESS;
+	u32 i = 0;
+	u32 i2cctl = E1000_READ_REG(hw, E1000_I2CPARAMS);
+	u32 timeout = 10;
+	bool ack = true;
+
+	DEBUGFUNC("e1000_get_i2c_ack");
+
+	e1000_raise_i2c_clk(hw, &i2cctl);
+
+	/* Minimum high period of clock is 4us */
+	usec_delay(E1000_I2C_T_HIGH);
+
+	/* Wait until SCL returns high */
+	for (i = 0; i < timeout; i++) {
+		usec_delay(1);
+		i2cctl = E1000_READ_REG(hw, E1000_I2CPARAMS);
+		if (i2cctl & E1000_I2C_CLK_IN)
+			break;
+	}
+	if (!(i2cctl & E1000_I2C_CLK_IN))
+		return E1000_ERR_I2C;
+
+	ack = e1000_get_i2c_data(&i2cctl);
+	if (ack) {
+		DEBUGOUT("I2C ack was not received.\n");
+		status = E1000_ERR_I2C;
+	}
+
+	e1000_lower_i2c_clk(hw, &i2cctl);
+
+	/* Minimum low period of clock is 4.7 us */
+	usec_delay(E1000_I2C_T_LOW);
+
+	return status;
+}
+
+/**
+ *  e1000_clock_in_i2c_bit - Clocks in one bit via I2C data/clock
+ *  @hw: pointer to hardware structure
+ *  @data: read data value
+ *
+ *  Clocks in one bit via I2C data/clock
+ **/
+static s32 e1000_clock_in_i2c_bit(struct e1000_hw *hw, bool *data)
+{
+	u32 i2cctl = E1000_READ_REG(hw, E1000_I2CPARAMS);
+
+	DEBUGFUNC("e1000_clock_in_i2c_bit");
+
+	e1000_raise_i2c_clk(hw, &i2cctl);
+
+	/* Minimum high period of clock is 4us */
+	usec_delay(E1000_I2C_T_HIGH);
+
+	i2cctl = E1000_READ_REG(hw, E1000_I2CPARAMS);
+	*data = e1000_get_i2c_data(&i2cctl);
+
+	e1000_lower_i2c_clk(hw, &i2cctl);
+
+	/* Minimum low period of clock is 4.7 us */
+	usec_delay(E1000_I2C_T_LOW);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_clock_out_i2c_bit - Clocks in/out one bit via I2C data/clock
+ *  @hw: pointer to hardware structure
+ *  @data: data value to write
+ *
+ *  Clocks out one bit via I2C data/clock
+ **/
+static s32 e1000_clock_out_i2c_bit(struct e1000_hw *hw, bool data)
+{
+	s32 status;
+	u32 i2cctl = E1000_READ_REG(hw, E1000_I2CPARAMS);
+
+	DEBUGFUNC("e1000_clock_out_i2c_bit");
+
+	status = e1000_set_i2c_data(hw, &i2cctl, data);
+	if (status == E1000_SUCCESS) {
+		e1000_raise_i2c_clk(hw, &i2cctl);
+
+		/* Minimum high period of clock is 4us */
+		usec_delay(E1000_I2C_T_HIGH);
+
+		e1000_lower_i2c_clk(hw, &i2cctl);
+
+		/* Minimum low period of clock is 4.7 us.
+		 * This also takes care of the data hold time.
+		 */
+		usec_delay(E1000_I2C_T_LOW);
+	} else {
+		status = E1000_ERR_I2C;
+		DEBUGOUT1("I2C data was not set to %X\n", data);
+	}
+
+	return status;
+}
+/**
+ *  e1000_raise_i2c_clk - Raises the I2C SCL clock
+ *  @hw: pointer to hardware structure
+ *  @i2cctl: Current value of I2CCTL register
+ *
+ *  Raises the I2C clock line '0'->'1'
+ **/
+static void e1000_raise_i2c_clk(struct e1000_hw *hw, u32 *i2cctl)
+{
+	DEBUGFUNC("e1000_raise_i2c_clk");
+
+	*i2cctl |= E1000_I2C_CLK_OUT;
+	*i2cctl &= ~E1000_I2C_CLK_OE_N;
+	E1000_WRITE_REG(hw, E1000_I2CPARAMS, *i2cctl);
+	E1000_WRITE_FLUSH(hw);
+
+	/* SCL rise time (1000ns) */
+	usec_delay(E1000_I2C_T_RISE);
+}
+
+/**
+ *  e1000_lower_i2c_clk - Lowers the I2C SCL clock
+ *  @hw: pointer to hardware structure
+ *  @i2cctl: Current value of I2CCTL register
+ *
+ *  Lowers the I2C clock line '1'->'0'
+ **/
+static void e1000_lower_i2c_clk(struct e1000_hw *hw, u32 *i2cctl)
+{
+
+	DEBUGFUNC("e1000_lower_i2c_clk");
+
+	*i2cctl &= ~E1000_I2C_CLK_OUT;
+	*i2cctl &= ~E1000_I2C_CLK_OE_N;
+	E1000_WRITE_REG(hw, E1000_I2CPARAMS, *i2cctl);
+	E1000_WRITE_FLUSH(hw);
+
+	/* SCL fall time (300ns) */
+	usec_delay(E1000_I2C_T_FALL);
+}
+
+/**
+ *  e1000_set_i2c_data - Sets the I2C data bit
+ *  @hw: pointer to hardware structure
+ *  @i2cctl: Current value of I2CCTL register
+ *  @data: I2C data value (0 or 1) to set
+ *
+ *  Sets the I2C data bit
+ **/
+static s32 e1000_set_i2c_data(struct e1000_hw *hw, u32 *i2cctl, bool data)
+{
+	s32 status = E1000_SUCCESS;
+
+	DEBUGFUNC("e1000_set_i2c_data");
+
+	if (data)
+		*i2cctl |= E1000_I2C_DATA_OUT;
+	else
+		*i2cctl &= ~E1000_I2C_DATA_OUT;
+
+	*i2cctl &= ~E1000_I2C_DATA_OE_N;
+	*i2cctl |= E1000_I2C_CLK_OE_N;
+	E1000_WRITE_REG(hw, E1000_I2CPARAMS, *i2cctl);
+	E1000_WRITE_FLUSH(hw);
+
+	/* Data rise/fall (1000ns/300ns) and set-up time (250ns) */
+	usec_delay(E1000_I2C_T_RISE + E1000_I2C_T_FALL + E1000_I2C_T_SU_DATA);
+
+	*i2cctl = E1000_READ_REG(hw, E1000_I2CPARAMS);
+	if (data != e1000_get_i2c_data(i2cctl)) {
+		status = E1000_ERR_I2C;
+		DEBUGOUT1("Error - I2C data was not set to %X.\n", data);
+	}
+
+	return status;
+}
+
+/**
+ *  e1000_get_i2c_data - Reads the I2C SDA data bit
+ *  @hw: pointer to hardware structure
+ *  @i2cctl: Current value of I2CCTL register
+ *
+ *  Returns the I2C data bit value
+ **/
+static bool e1000_get_i2c_data(u32 *i2cctl)
+{
+	bool data;
+
+	DEBUGFUNC("e1000_get_i2c_data");
+
+	if (*i2cctl & E1000_I2C_DATA_IN)
+		data = 1;
+	else
+		data = 0;
+
+	return data;
+}
+
+/**
+ *  e1000_i2c_bus_clear - Clears the I2C bus
+ *  @hw: pointer to hardware structure
+ *
+ *  Clears the I2C bus by sending nine clock pulses.
+ *  Used when data line is stuck low.
+ **/
+void e1000_i2c_bus_clear(struct e1000_hw *hw)
+{
+	u32 i2cctl = E1000_READ_REG(hw, E1000_I2CPARAMS);
+	u32 i;
+
+	DEBUGFUNC("e1000_i2c_bus_clear");
+
+	e1000_i2c_start(hw);
+
+	e1000_set_i2c_data(hw, &i2cctl, 1);
+
+	for (i = 0; i < 9; i++) {
+		e1000_raise_i2c_clk(hw, &i2cctl);
+
+		/* Min high period of clock is 4us */
+		usec_delay(E1000_I2C_T_HIGH);
+
+		e1000_lower_i2c_clk(hw, &i2cctl);
+
+		/* Min low period of clock is 4.7us*/
+		usec_delay(E1000_I2C_T_LOW);
+	}
+
+	e1000_i2c_start(hw);
+
+	/* Put the i2c bus back to default state */
+	e1000_i2c_stop(hw);
+}
+
+static const u8 e1000_emc_temp_data[4] = {
+	E1000_EMC_INTERNAL_DATA,
+	E1000_EMC_DIODE1_DATA,
+	E1000_EMC_DIODE2_DATA,
+	E1000_EMC_DIODE3_DATA
+};
+static const u8 e1000_emc_therm_limit[4] = {
+	E1000_EMC_INTERNAL_THERM_LIMIT,
+	E1000_EMC_DIODE1_THERM_LIMIT,
+	E1000_EMC_DIODE2_THERM_LIMIT,
+	E1000_EMC_DIODE3_THERM_LIMIT
 };
 
+/**
+ *  e1000_get_thermal_sensor_data_generic - Gathers thermal sensor data
+ *  @hw: pointer to hardware structure
+ *
+ *  Updates the temperatures in mac.thermal_sensor_data
+ **/
+s32 e1000_get_thermal_sensor_data_generic(struct e1000_hw *hw)
+{
+	s32 status = E1000_SUCCESS;
+	u16 ets_offset;
+	u16 ets_cfg;
+	u16 ets_sensor;
+	u8  num_sensors;
+	u8  sensor_index;
+	u8  sensor_location;
+	u8  i;
+	struct e1000_thermal_sensor_data *data = &hw->mac.thermal_sensor_data;
+
+	DEBUGFUNC("e1000_get_thermal_sensor_data_generic");
+
+	if ((hw->mac.type != e1000_i350) || (hw->bus.func != 0))
+		return E1000_NOT_IMPLEMENTED;
+
+	data->sensor[0].temp = (E1000_READ_REG(hw, E1000_THMJT) & 0xFF);
+
+	/* Return the internal sensor only if ETS is unsupported */
+	e1000_read_nvm(hw, NVM_ETS_CFG, 1, &ets_offset);
+	if ((ets_offset == 0x0000) || (ets_offset == 0xFFFF))
+		return status;
+
+	e1000_read_nvm(hw, ets_offset, 1, &ets_cfg);
+	if (((ets_cfg & NVM_ETS_TYPE_MASK) >> NVM_ETS_TYPE_SHIFT)
+	    != NVM_ETS_TYPE_EMC)
+		return E1000_NOT_IMPLEMENTED;
+
+	num_sensors = (ets_cfg & NVM_ETS_NUM_SENSORS_MASK);
+	if (num_sensors > E1000_MAX_SENSORS)
+		num_sensors = E1000_MAX_SENSORS;
+
+	for (i = 1; i < num_sensors; i++) {
+		e1000_read_nvm(hw, (ets_offset + i), 1, &ets_sensor);
+		sensor_index = ((ets_sensor & NVM_ETS_DATA_INDEX_MASK) >>
+				NVM_ETS_DATA_INDEX_SHIFT);
+		sensor_location = ((ets_sensor & NVM_ETS_DATA_LOC_MASK) >>
+				   NVM_ETS_DATA_LOC_SHIFT);
+
+		if (sensor_location != 0)
+			hw->phy.ops.read_i2c_byte(hw,
+					e1000_emc_temp_data[sensor_index],
+					E1000_I2C_THERMAL_SENSOR_ADDR,
+					&data->sensor[i].temp);
+	}
+	return status;
+}
+
+/**
+ *  e1000_init_thermal_sensor_thresh_generic - Sets thermal sensor thresholds
+ *  @hw: pointer to hardware structure
+ *
+ *  Sets the thermal sensor thresholds according to the NVM map
+ *  and save off the threshold and location values into mac.thermal_sensor_data
+ **/
+s32 e1000_init_thermal_sensor_thresh_generic(struct e1000_hw *hw)
+{
+	s32 status = E1000_SUCCESS;
+	u16 ets_offset;
+	u16 ets_cfg;
+	u16 ets_sensor;
+	u8  low_thresh_delta;
+	u8  num_sensors;
+	u8  sensor_index;
+	u8  sensor_location;
+	u8  therm_limit;
+	u8  i;
+	struct e1000_thermal_sensor_data *data = &hw->mac.thermal_sensor_data;
+
+	DEBUGFUNC("e1000_init_thermal_sensor_thresh_generic");
+
+	if ((hw->mac.type != e1000_i350) || (hw->bus.func != 0))
+		return E1000_NOT_IMPLEMENTED;
+
+	memset(data, 0, sizeof(struct e1000_thermal_sensor_data));
+
+	data->sensor[0].location = 0x1;
+	data->sensor[0].caution_thresh =
+		(E1000_READ_REG(hw, E1000_THHIGHTC) & 0xFF);
+	data->sensor[0].max_op_thresh =
+		(E1000_READ_REG(hw, E1000_THLOWTC) & 0xFF);
+
+	/* Return the internal sensor only if ETS is unsupported */
+	e1000_read_nvm(hw, NVM_ETS_CFG, 1, &ets_offset);
+	if ((ets_offset == 0x0000) || (ets_offset == 0xFFFF))
+		return status;
+
+	e1000_read_nvm(hw, ets_offset, 1, &ets_cfg);
+	if (((ets_cfg & NVM_ETS_TYPE_MASK) >> NVM_ETS_TYPE_SHIFT)
+	    != NVM_ETS_TYPE_EMC)
+		return E1000_NOT_IMPLEMENTED;
+
+	low_thresh_delta = ((ets_cfg & NVM_ETS_LTHRES_DELTA_MASK) >>
+			    NVM_ETS_LTHRES_DELTA_SHIFT);
+	num_sensors = (ets_cfg & NVM_ETS_NUM_SENSORS_MASK);
+
+	for (i = 1; i <= num_sensors; i++) {
+		e1000_read_nvm(hw, (ets_offset + i), 1, &ets_sensor);
+		sensor_index = ((ets_sensor & NVM_ETS_DATA_INDEX_MASK) >>
+				NVM_ETS_DATA_INDEX_SHIFT);
+		sensor_location = ((ets_sensor & NVM_ETS_DATA_LOC_MASK) >>
+				   NVM_ETS_DATA_LOC_SHIFT);
+		therm_limit = ets_sensor & NVM_ETS_DATA_HTHRESH_MASK;
+
+		hw->phy.ops.write_i2c_byte(hw,
+			e1000_emc_therm_limit[sensor_index],
+			E1000_I2C_THERMAL_SENSOR_ADDR,
+			therm_limit);
+
+		if ((i < E1000_MAX_SENSORS) && (sensor_location != 0)) {
+			data->sensor[i].location = sensor_location;
+			data->sensor[i].caution_thresh = therm_limit;
+			data->sensor[i].max_op_thresh = therm_limit -
+							low_thresh_delta;
+		}
+	}
+	return status;
+}
diff --git a/drivers/net/igb/e1000_82575.h b/drivers/net/igb/e1000_82575.h
index 786e110011a3..947015767605 100644
--- a/drivers/net/igb/e1000_82575.h
+++ b/drivers/net/igb/e1000_82575.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2007-2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -28,89 +28,246 @@
 #ifndef _E1000_82575_H_
 #define _E1000_82575_H_
 
-extern void igb_shutdown_serdes_link_82575(struct e1000_hw *hw);
-extern void igb_power_up_serdes_link_82575(struct e1000_hw *hw);
-extern void igb_power_down_phy_copper_82575(struct e1000_hw *hw);
-extern void igb_rx_fifo_flush_82575(struct e1000_hw *hw);
-
-#define ID_LED_DEFAULT_82575_SERDES ((ID_LED_DEF1_DEF2 << 12) | \
-                                     (ID_LED_DEF1_DEF2 <<  8) | \
-                                     (ID_LED_DEF1_DEF2 <<  4) | \
-                                     (ID_LED_OFF1_ON2))
-
-#define E1000_RAR_ENTRIES_82575        16
-#define E1000_RAR_ENTRIES_82576        24
-#define E1000_RAR_ENTRIES_82580        24
-#define E1000_RAR_ENTRIES_I350         32
+#define ID_LED_DEFAULT_82575_SERDES	((ID_LED_DEF1_DEF2 << 12) | \
+					 (ID_LED_DEF1_DEF2 <<  8) | \
+					 (ID_LED_DEF1_DEF2 <<  4) | \
+					 (ID_LED_OFF1_ON2))
+/*
+ * Receive Address Register Count
+ * Number of high/low register pairs in the RAR.  The RAR (Receive Address
+ * Registers) holds the directed and multicast addresses that we monitor.
+ * These entries are also used for MAC-based filtering.
+ */
+/*
+ * For 82576, there are an additional set of RARs that begin at an offset
+ * separate from the first set of RARs.
+ */
+#define E1000_RAR_ENTRIES_82575	16
+#define E1000_RAR_ENTRIES_82576	24
+#define E1000_RAR_ENTRIES_82580	24
+#define E1000_RAR_ENTRIES_I350	32
+#define E1000_SW_SYNCH_MB	0x00000100
+#define E1000_STAT_DEV_RST_SET	0x00100000
+#define E1000_CTRL_DEV_RST	0x20000000
+
+struct e1000_adv_data_desc {
+	__le64 buffer_addr;    /* Address of the descriptor's data buffer */
+	union {
+		u32 data;
+		struct {
+			u32 datalen:16; /* Data buffer length */
+			u32 rsvd:4;
+			u32 dtyp:4;  /* Descriptor type */
+			u32 dcmd:8;  /* Descriptor command */
+		} config;
+	} lower;
+	union {
+		u32 data;
+		struct {
+			u32 status:4;  /* Descriptor status */
+			u32 idx:4;
+			u32 popts:6;  /* Packet Options */
+			u32 paylen:18; /* Payload length */
+		} options;
+	} upper;
+};
 
-#define E1000_SW_SYNCH_MB              0x00000100
-#define E1000_STAT_DEV_RST_SET         0x00100000
-#define E1000_CTRL_DEV_RST             0x20000000
+#define E1000_TXD_DTYP_ADV_C	0x2  /* Advanced Context Descriptor */
+#define E1000_TXD_DTYP_ADV_D	0x3  /* Advanced Data Descriptor */
+#define E1000_ADV_TXD_CMD_DEXT	0x20 /* Descriptor extension (0 = legacy) */
+#define E1000_ADV_TUCMD_IPV4	0x2  /* IP Packet Type: 1=IPv4 */
+#define E1000_ADV_TUCMD_IPV6	0x0  /* IP Packet Type: 0=IPv6 */
+#define E1000_ADV_TUCMD_L4T_UDP	0x0  /* L4 Packet TYPE of UDP */
+#define E1000_ADV_TUCMD_L4T_TCP	0x4  /* L4 Packet TYPE of TCP */
+#define E1000_ADV_TUCMD_MKRREQ	0x10 /* Indicates markers are required */
+#define E1000_ADV_DCMD_EOP	0x1  /* End of Packet */
+#define E1000_ADV_DCMD_IFCS	0x2  /* Insert FCS (Ethernet CRC) */
+#define E1000_ADV_DCMD_RS	0x8  /* Report Status */
+#define E1000_ADV_DCMD_VLE	0x40 /* Add VLAN tag */
+#define E1000_ADV_DCMD_TSE	0x80 /* TCP Seg enable */
+/* Extended Device Control */
+#define E1000_CTRL_EXT_NSICR	0x00000001 /* Disable Intr Clear all on read */
+
+struct e1000_adv_context_desc {
+	union {
+		u32 ip_config;
+		struct {
+			u32 iplen:9;
+			u32 maclen:7;
+			u32 vlan_tag:16;
+		} fields;
+	} ip_setup;
+	u32 seq_num;
+	union {
+		u64 l4_config;
+		struct {
+			u32 mkrloc:9;
+			u32 tucmd:11;
+			u32 dtyp:4;
+			u32 adv:8;
+			u32 rsvd:4;
+			u32 idx:4;
+			u32 l4len:8;
+			u32 mss:16;
+		} fields;
+	} l4_setup;
+};
 
 /* SRRCTL bit definitions */
-#define E1000_SRRCTL_BSIZEPKT_SHIFT                     10 /* Shift _right_ */
-#define E1000_SRRCTL_BSIZEHDRSIZE_SHIFT                 2  /* Shift _left_ */
-#define E1000_SRRCTL_DESCTYPE_ADV_ONEBUF                0x02000000
-#define E1000_SRRCTL_DESCTYPE_HDR_SPLIT_ALWAYS          0x0A000000
-#define E1000_SRRCTL_DROP_EN                            0x80000000
-#define E1000_SRRCTL_TIMESTAMP                          0x40000000
-
-#define E1000_MRQC_ENABLE_RSS_4Q            0x00000002
-#define E1000_MRQC_ENABLE_VMDQ              0x00000003
-#define E1000_MRQC_ENABLE_VMDQ_RSS_2Q       0x00000005
-#define E1000_MRQC_RSS_FIELD_IPV4_UDP       0x00400000
-#define E1000_MRQC_RSS_FIELD_IPV6_UDP       0x00800000
-#define E1000_MRQC_RSS_FIELD_IPV6_UDP_EX    0x01000000
+#define E1000_SRRCTL_BSIZEPKT_SHIFT		10 /* Shift _right_ */
+#define E1000_SRRCTL_BSIZEHDRSIZE_MASK		0x00000F00
+#define E1000_SRRCTL_BSIZEHDRSIZE_SHIFT		2  /* Shift _left_ */
+#define E1000_SRRCTL_DESCTYPE_LEGACY		0x00000000
+#define E1000_SRRCTL_DESCTYPE_ADV_ONEBUF	0x02000000
+#define E1000_SRRCTL_DESCTYPE_HDR_SPLIT		0x04000000
+#define E1000_SRRCTL_DESCTYPE_HDR_SPLIT_ALWAYS	0x0A000000
+#define E1000_SRRCTL_DESCTYPE_HDR_REPLICATION	0x06000000
+#define E1000_SRRCTL_DESCTYPE_HDR_REPLICATION_LARGE_PKT 0x08000000
+#define E1000_SRRCTL_DESCTYPE_MASK		0x0E000000
+#define E1000_SRRCTL_TIMESTAMP			0x40000000
+#define E1000_SRRCTL_DROP_EN			0x80000000
+
+#define E1000_SRRCTL_BSIZEPKT_MASK		0x0000007F
+#define E1000_SRRCTL_BSIZEHDR_MASK		0x00003F00
+
+#define E1000_TX_HEAD_WB_ENABLE		0x1
+#define E1000_TX_SEQNUM_WB_ENABLE	0x2
+
+#define E1000_MRQC_ENABLE_RSS_4Q		0x00000002
+#define E1000_MRQC_ENABLE_VMDQ			0x00000003
+#define E1000_MRQC_ENABLE_VMDQ_RSS_2Q		0x00000005
+#define E1000_MRQC_RSS_FIELD_IPV4_UDP		0x00400000
+#define E1000_MRQC_RSS_FIELD_IPV6_UDP		0x00800000
+#define E1000_MRQC_RSS_FIELD_IPV6_UDP_EX	0x01000000
+#define E1000_MRQC_ENABLE_RSS_8Q		0x00000002
+
+#define E1000_VMRCTL_MIRROR_PORT_SHIFT		8
+#define E1000_VMRCTL_MIRROR_DSTPORT_MASK	(7 << \
+						 E1000_VMRCTL_MIRROR_PORT_SHIFT)
+#define E1000_VMRCTL_POOL_MIRROR_ENABLE		(1 << 0)
+#define E1000_VMRCTL_UPLINK_MIRROR_ENABLE	(1 << 1)
+#define E1000_VMRCTL_DOWNLINK_MIRROR_ENABLE	(1 << 2)
 
 #define E1000_EICR_TX_QUEUE ( \
-    E1000_EICR_TX_QUEUE0 |    \
-    E1000_EICR_TX_QUEUE1 |    \
-    E1000_EICR_TX_QUEUE2 |    \
-    E1000_EICR_TX_QUEUE3)
+	E1000_EICR_TX_QUEUE0 |    \
+	E1000_EICR_TX_QUEUE1 |    \
+	E1000_EICR_TX_QUEUE2 |    \
+	E1000_EICR_TX_QUEUE3)
 
 #define E1000_EICR_RX_QUEUE ( \
-    E1000_EICR_RX_QUEUE0 |    \
-    E1000_EICR_RX_QUEUE1 |    \
-    E1000_EICR_RX_QUEUE2 |    \
-    E1000_EICR_RX_QUEUE3)
+	E1000_EICR_RX_QUEUE0 |    \
+	E1000_EICR_RX_QUEUE1 |    \
+	E1000_EICR_RX_QUEUE2 |    \
+	E1000_EICR_RX_QUEUE3)
+
+#define E1000_EIMS_RX_QUEUE	E1000_EICR_RX_QUEUE
+#define E1000_EIMS_TX_QUEUE	E1000_EICR_TX_QUEUE
+
+#define EIMS_ENABLE_MASK ( \
+	E1000_EIMS_RX_QUEUE  | \
+	E1000_EIMS_TX_QUEUE  | \
+	E1000_EIMS_TCP_TIMER | \
+	E1000_EIMS_OTHER)
 
 /* Immediate Interrupt Rx (A.K.A. Low Latency Interrupt) */
-#define E1000_IMIREXT_SIZE_BP     0x00001000  /* Packet size bypass */
-#define E1000_IMIREXT_CTRL_BP     0x00080000  /* Bypass check of ctrl bits */
+#define E1000_IMIR_PORT_IM_EN	0x00010000  /* TCP port enable */
+#define E1000_IMIR_PORT_BP	0x00020000  /* TCP port check bypass */
+#define E1000_IMIREXT_SIZE_BP	0x00001000  /* Packet size bypass */
+#define E1000_IMIREXT_CTRL_URG	0x00002000  /* Check URG bit in header */
+#define E1000_IMIREXT_CTRL_ACK	0x00004000  /* Check ACK bit in header */
+#define E1000_IMIREXT_CTRL_PSH	0x00008000  /* Check PSH bit in header */
+#define E1000_IMIREXT_CTRL_RST	0x00010000  /* Check RST bit in header */
+#define E1000_IMIREXT_CTRL_SYN	0x00020000  /* Check SYN bit in header */
+#define E1000_IMIREXT_CTRL_FIN	0x00040000  /* Check FIN bit in header */
+#define E1000_IMIREXT_CTRL_BP	0x00080000  /* Bypass check of ctrl bits */
 
 /* Receive Descriptor - Advanced */
 union e1000_adv_rx_desc {
 	struct {
-		__le64 pkt_addr;             /* Packet buffer address */
-		__le64 hdr_addr;             /* Header buffer address */
+		__le64 pkt_addr; /* Packet buffer address */
+		__le64 hdr_addr; /* Header buffer address */
 	} read;
 	struct {
 		struct {
-			struct {
-				__le16 pkt_info;   /* RSS type, Packet type */
-				__le16 hdr_info;   /* Split Header,
-						    * header buffer length */
+			union {
+				__le32 data;
+				struct {
+					__le16 pkt_info; /*RSS type, Pkt type*/
+					/* Split Header, header buffer len */
+					__le16 hdr_info;
+				} hs_rss;
 			} lo_dword;
 			union {
-				__le32 rss;          /* RSS Hash */
+				__le32 rss; /* RSS Hash */
 				struct {
-					__le16 ip_id;    /* IP id */
-					__le16 csum;     /* Packet Checksum */
+					__le16 ip_id; /* IP id */
+					__le16 csum; /* Packet Checksum */
 				} csum_ip;
 			} hi_dword;
 		} lower;
 		struct {
-			__le32 status_error;     /* ext status/error */
-			__le16 length;           /* Packet length */
-			__le16 vlan;             /* VLAN tag */
+			__le32 status_error; /* ext status/error */
+			__le16 length; /* Packet length */
+			__le16 vlan; /* VLAN tag */
 		} upper;
 	} wb;  /* writeback */
 };
 
-#define E1000_RXDADV_HDRBUFLEN_MASK      0x7FE0
-#define E1000_RXDADV_HDRBUFLEN_SHIFT     5
-#define E1000_RXDADV_STAT_TS             0x10000 /* Pkt was time stamped */
-#define E1000_RXDADV_STAT_TSIP           0x08000 /* timestamp in packet */
+#define E1000_RXDADV_RSSTYPE_MASK	0x0000000F
+#define E1000_RXDADV_RSSTYPE_SHIFT	12
+#define E1000_RXDADV_HDRBUFLEN_MASK	0x7FE0
+#define E1000_RXDADV_HDRBUFLEN_SHIFT	5
+#define E1000_RXDADV_SPLITHEADER_EN	0x00001000
+#define E1000_RXDADV_SPH		0x8000
+#define E1000_RXDADV_STAT_TS		0x10000 /* Pkt was time stamped */
+#define E1000_RXDADV_STAT_TSIP		0x08000 /* timestamp in packet */
+#define E1000_RXDADV_ERR_HBO		0x00800000
+
+/* RSS Hash results */
+#define E1000_RXDADV_RSSTYPE_NONE	0x00000000
+#define E1000_RXDADV_RSSTYPE_IPV4_TCP	0x00000001
+#define E1000_RXDADV_RSSTYPE_IPV4	0x00000002
+#define E1000_RXDADV_RSSTYPE_IPV6_TCP	0x00000003
+#define E1000_RXDADV_RSSTYPE_IPV6_EX	0x00000004
+#define E1000_RXDADV_RSSTYPE_IPV6	0x00000005
+#define E1000_RXDADV_RSSTYPE_IPV6_TCP_EX 0x00000006
+#define E1000_RXDADV_RSSTYPE_IPV4_UDP	0x00000007
+#define E1000_RXDADV_RSSTYPE_IPV6_UDP	0x00000008
+#define E1000_RXDADV_RSSTYPE_IPV6_UDP_EX 0x00000009
+
+/* RSS Packet Types as indicated in the receive descriptor */
+#define E1000_RXDADV_PKTTYPE_ILMASK	0x000000F0
+#define E1000_RXDADV_PKTTYPE_TLMASK	0x00000F00
+#define E1000_RXDADV_PKTTYPE_NONE	0x00000000
+#define E1000_RXDADV_PKTTYPE_IPV4	0x00000010 /* IPV4 hdr present */
+#define E1000_RXDADV_PKTTYPE_IPV4_EX	0x00000020 /* IPV4 hdr + extensions */
+#define E1000_RXDADV_PKTTYPE_IPV6	0x00000040 /* IPV6 hdr present */
+#define E1000_RXDADV_PKTTYPE_IPV6_EX	0x00000080 /* IPV6 hdr + extensions */
+#define E1000_RXDADV_PKTTYPE_TCP	0x00000100 /* TCP hdr present */
+#define E1000_RXDADV_PKTTYPE_UDP	0x00000200 /* UDP hdr present */
+#define E1000_RXDADV_PKTTYPE_SCTP	0x00000400 /* SCTP hdr present */
+#define E1000_RXDADV_PKTTYPE_NFS	0x00000800 /* NFS hdr present */
+
+#define E1000_RXDADV_PKTTYPE_IPSEC_ESP	0x00001000 /* IPSec ESP */
+#define E1000_RXDADV_PKTTYPE_IPSEC_AH	0x00002000 /* IPSec AH */
+#define E1000_RXDADV_PKTTYPE_LINKSEC	0x00004000 /* LinkSec Encap */
+#define E1000_RXDADV_PKTTYPE_ETQF	0x00008000 /* PKTTYPE is ETQF index */
+#define E1000_RXDADV_PKTTYPE_ETQF_MASK	0x00000070 /* ETQF has 8 indices */
+#define E1000_RXDADV_PKTTYPE_ETQF_SHIFT	4 /* Right-shift 4 bits */
+
+/* LinkSec results */
+/* Security Processing bit Indication */
+#define E1000_RXDADV_LNKSEC_STATUS_SECP		0x00020000
+#define E1000_RXDADV_LNKSEC_ERROR_BIT_MASK	0x18000000
+#define E1000_RXDADV_LNKSEC_ERROR_NO_SA_MATCH	0x08000000
+#define E1000_RXDADV_LNKSEC_ERROR_REPLAY_ERROR	0x10000000
+#define E1000_RXDADV_LNKSEC_ERROR_BAD_SIG	0x18000000
+
+#define E1000_RXDADV_IPSEC_STATUS_SECP			0x00020000
+#define E1000_RXDADV_IPSEC_ERROR_BIT_MASK		0x18000000
+#define E1000_RXDADV_IPSEC_ERROR_INVALID_PROTOCOL	0x08000000
+#define E1000_RXDADV_IPSEC_ERROR_INVALID_LENGTH		0x10000000
+#define E1000_RXDADV_IPSEC_ERROR_AUTHENTICATION_FAILED	0x18000000
 
 /* Transmit Descriptor - Advanced */
 union e1000_adv_tx_desc {
@@ -127,14 +284,26 @@ union e1000_adv_tx_desc {
 };
 
 /* Adv Transmit Descriptor Config Masks */
-#define E1000_ADVTXD_MAC_TSTAMP   0x00080000 /* IEEE1588 Timestamp packet */
-#define E1000_ADVTXD_DTYP_CTXT    0x00200000 /* Advanced Context Descriptor */
-#define E1000_ADVTXD_DTYP_DATA    0x00300000 /* Advanced Data Descriptor */
-#define E1000_ADVTXD_DCMD_IFCS    0x02000000 /* Insert FCS (Ethernet CRC) */
-#define E1000_ADVTXD_DCMD_DEXT    0x20000000 /* Descriptor extension (1=Adv) */
-#define E1000_ADVTXD_DCMD_VLE     0x40000000 /* VLAN pkt enable */
-#define E1000_ADVTXD_DCMD_TSE     0x80000000 /* TCP Seg enable */
-#define E1000_ADVTXD_PAYLEN_SHIFT    14 /* Adv desc PAYLEN shift */
+#define E1000_ADVTXD_DTYP_CTXT	0x00200000 /* Advanced Context Descriptor */
+#define E1000_ADVTXD_DTYP_DATA	0x00300000 /* Advanced Data Descriptor */
+#define E1000_ADVTXD_DCMD_EOP	0x01000000 /* End of Packet */
+#define E1000_ADVTXD_DCMD_IFCS	0x02000000 /* Insert FCS (Ethernet CRC) */
+#define E1000_ADVTXD_DCMD_RS	0x08000000 /* Report Status */
+#define E1000_ADVTXD_DCMD_DDTYP_ISCSI	0x10000000 /* DDP hdr type or iSCSI */
+#define E1000_ADVTXD_DCMD_DEXT	0x20000000 /* Descriptor extension (1=Adv) */
+#define E1000_ADVTXD_DCMD_VLE	0x40000000 /* VLAN pkt enable */
+#define E1000_ADVTXD_DCMD_TSE	0x80000000 /* TCP Seg enable */
+#define E1000_ADVTXD_MAC_LINKSEC	0x00040000 /* Apply LinkSec on pkt */
+#define E1000_ADVTXD_MAC_TSTAMP		0x00080000 /* IEEE1588 Timestamp pkt */
+#define E1000_ADVTXD_STAT_SN_CRC	0x00000002 /* NXTSEQ/SEED prsnt in WB */
+#define E1000_ADVTXD_IDX_SHIFT		4  /* Adv desc Index shift */
+#define E1000_ADVTXD_POPTS_ISCO_1ST	0x00000000 /* 1st TSO of iSCSI PDU */
+#define E1000_ADVTXD_POPTS_ISCO_MDL	0x00000800 /* Middle TSO of iSCSI PDU */
+#define E1000_ADVTXD_POPTS_ISCO_LAST	0x00001000 /* Last TSO of iSCSI PDU */
+/* 1st & Last TSO-full iSCSI PDU*/
+#define E1000_ADVTXD_POPTS_ISCO_FULL	0x00001800
+#define E1000_ADVTXD_POPTS_IPSEC	0x00000400 /* IPSec offload request */
+#define E1000_ADVTXD_PAYLEN_SHIFT	14 /* Adv desc PAYLEN shift */
 
 /* Context descriptors */
 struct e1000_adv_tx_context_desc {
@@ -144,115 +313,200 @@ struct e1000_adv_tx_context_desc {
 	__le32 mss_l4len_idx;
 };
 
-#define E1000_ADVTXD_MACLEN_SHIFT    9  /* Adv ctxt desc mac len shift */
-#define E1000_ADVTXD_TUCMD_IPV4    0x00000400  /* IP Packet Type: 1=IPv4 */
-#define E1000_ADVTXD_TUCMD_L4T_TCP 0x00000800  /* L4 Packet TYPE of TCP */
-#define E1000_ADVTXD_TUCMD_L4T_SCTP 0x00001000 /* L4 packet TYPE of SCTP */
+#define E1000_ADVTXD_MACLEN_SHIFT	9  /* Adv ctxt desc mac len shift */
+#define E1000_ADVTXD_VLAN_SHIFT		16  /* Adv ctxt vlan tag shift */
+#define E1000_ADVTXD_TUCMD_IPV4		0x00000400  /* IP Packet Type: 1=IPv4 */
+#define E1000_ADVTXD_TUCMD_IPV6		0x00000000  /* IP Packet Type: 0=IPv6 */
+#define E1000_ADVTXD_TUCMD_L4T_UDP	0x00000000  /* L4 Packet TYPE of UDP */
+#define E1000_ADVTXD_TUCMD_L4T_TCP	0x00000800  /* L4 Packet TYPE of TCP */
+#define E1000_ADVTXD_TUCMD_L4T_SCTP	0x00001000  /* L4 Packet TYPE of SCTP */
+#define E1000_ADVTXD_TUCMD_IPSEC_TYPE_ESP	0x00002000 /* IPSec Type ESP */
 /* IPSec Encrypt Enable for ESP */
-#define E1000_ADVTXD_L4LEN_SHIFT     8  /* Adv ctxt L4LEN shift */
-#define E1000_ADVTXD_MSS_SHIFT      16  /* Adv ctxt MSS shift */
+#define E1000_ADVTXD_TUCMD_IPSEC_ENCRYPT_EN	0x00004000
+/* Req requires Markers and CRC */
+#define E1000_ADVTXD_TUCMD_MKRREQ	0x00002000
+#define E1000_ADVTXD_L4LEN_SHIFT	8  /* Adv ctxt L4LEN shift */
+#define E1000_ADVTXD_MSS_SHIFT		16  /* Adv ctxt MSS shift */
 /* Adv ctxt IPSec SA IDX mask */
+#define E1000_ADVTXD_IPSEC_SA_INDEX_MASK	0x000000FF
 /* Adv ctxt IPSec ESP len mask */
+#define E1000_ADVTXD_IPSEC_ESP_LEN_MASK		0x000000FF
 
 /* Additional Transmit Descriptor Control definitions */
-#define E1000_TXDCTL_QUEUE_ENABLE  0x02000000 /* Enable specific Tx Queue */
+#define E1000_TXDCTL_QUEUE_ENABLE	0x02000000 /* Ena specific Tx Queue */
+#define E1000_TXDCTL_SWFLSH		0x04000000 /* Tx Desc. wbk flushing */
 /* Tx Queue Arbitration Priority 0=low, 1=high */
+#define E1000_TXDCTL_PRIORITY		0x08000000
 
 /* Additional Receive Descriptor Control definitions */
-#define E1000_RXDCTL_QUEUE_ENABLE  0x02000000 /* Enable specific Rx Queue */
+#define E1000_RXDCTL_QUEUE_ENABLE	0x02000000 /* Ena specific Rx Queue */
+#define E1000_RXDCTL_SWFLSH		0x04000000 /* Rx Desc. wbk flushing */
 
 /* Direct Cache Access (DCA) definitions */
-#define E1000_DCA_CTRL_DCA_MODE_DISABLE 0x01 /* DCA Disable */
-#define E1000_DCA_CTRL_DCA_MODE_CB2     0x02 /* DCA Mode CB2 */
-
-#define E1000_DCA_RXCTRL_CPUID_MASK 0x0000001F /* Rx CPUID Mask */
-#define E1000_DCA_RXCTRL_DESC_DCA_EN (1 << 5) /* DCA Rx Desc enable */
-#define E1000_DCA_RXCTRL_HEAD_DCA_EN (1 << 6) /* DCA Rx Desc header enable */
-#define E1000_DCA_RXCTRL_DATA_DCA_EN (1 << 7) /* DCA Rx Desc payload enable */
-
-#define E1000_DCA_TXCTRL_CPUID_MASK 0x0000001F /* Tx CPUID Mask */
-#define E1000_DCA_TXCTRL_DESC_DCA_EN (1 << 5) /* DCA Tx Desc enable */
-#define E1000_DCA_TXCTRL_TX_WB_RO_EN (1 << 11) /* Tx Desc writeback RO bit */
-
-/* Additional DCA related definitions, note change in position of CPUID */
-#define E1000_DCA_TXCTRL_CPUID_MASK_82576 0xFF000000 /* Tx CPUID Mask */
-#define E1000_DCA_RXCTRL_CPUID_MASK_82576 0xFF000000 /* Rx CPUID Mask */
-#define E1000_DCA_TXCTRL_CPUID_SHIFT 24 /* Tx CPUID now in the last byte */
-#define E1000_DCA_RXCTRL_CPUID_SHIFT 24 /* Rx CPUID now in the last byte */
+#define E1000_DCA_CTRL_DCA_ENABLE	0x00000000 /* DCA Enable */
+#define E1000_DCA_CTRL_DCA_DISABLE	0x00000001 /* DCA Disable */
+
+#define E1000_DCA_CTRL_DCA_MODE_CB1	0x00 /* DCA Mode CB1 */
+#define E1000_DCA_CTRL_DCA_MODE_CB2	0x02 /* DCA Mode CB2 */
+
+#define E1000_DCA_RXCTRL_CPUID_MASK	0x0000001F /* Rx CPUID Mask */
+#define E1000_DCA_RXCTRL_DESC_DCA_EN	(1 << 5) /* DCA Rx Desc enable */
+#define E1000_DCA_RXCTRL_HEAD_DCA_EN	(1 << 6) /* DCA Rx Desc header ena */
+#define E1000_DCA_RXCTRL_DATA_DCA_EN	(1 << 7) /* DCA Rx Desc payload ena */
+#define E1000_DCA_RXCTRL_DESC_RRO_EN	(1 << 9) /* DCA Rx Desc Relax Order */
+
+#define E1000_DCA_TXCTRL_CPUID_MASK	0x0000001F /* Tx CPUID Mask */
+#define E1000_DCA_TXCTRL_DESC_DCA_EN	(1 << 5) /* DCA Tx Desc enable */
+#define E1000_DCA_TXCTRL_DESC_RRO_EN	(1 << 9) /* Tx rd Desc Relax Order */
+#define E1000_DCA_TXCTRL_TX_WB_RO_EN	(1 << 11) /* Tx Desc writeback RO bit */
+#define E1000_DCA_TXCTRL_DATA_RRO_EN	(1 << 13) /* Tx rd data Relax Order */
+
+#define E1000_DCA_TXCTRL_CPUID_MASK_82576	0xFF000000 /* Tx CPUID Mask */
+#define E1000_DCA_RXCTRL_CPUID_MASK_82576	0xFF000000 /* Rx CPUID Mask */
+#define E1000_DCA_TXCTRL_CPUID_SHIFT_82576	24 /* Tx CPUID */
+#define E1000_DCA_RXCTRL_CPUID_SHIFT_82576	24 /* Rx CPUID */
+
+/* Additional interrupt register bit definitions */
+#define E1000_ICR_LSECPNS	0x00000020 /* PN threshold - server */
+#define E1000_IMS_LSECPNS	E1000_ICR_LSECPNS /* PN threshold - server */
+#define E1000_ICS_LSECPNS	E1000_ICR_LSECPNS /* PN threshold - server */
 
 /* ETQF register bit definitions */
-#define E1000_ETQF_FILTER_ENABLE   (1 << 26)
-#define E1000_ETQF_1588            (1 << 30)
-
-/* FTQF register bit definitions */
-#define E1000_FTQF_VF_BP               0x00008000
-#define E1000_FTQF_1588_TIME_STAMP     0x08000000
-#define E1000_FTQF_MASK                0xF0000000
-#define E1000_FTQF_MASK_PROTO_BP       0x10000000
-#define E1000_FTQF_MASK_SOURCE_PORT_BP 0x80000000
-
-#define E1000_NVM_APME_82575          0x0400
-#define MAX_NUM_VFS                   8
-
-#define E1000_DTXSWC_MAC_SPOOF_MASK   0x000000FF /* Per VF MAC spoof control */
-#define E1000_DTXSWC_VLAN_SPOOF_MASK  0x0000FF00 /* Per VF VLAN spoof control */
-#define E1000_DTXSWC_LLE_MASK         0x00FF0000 /* Per VF Local LB enables */
-#define E1000_DTXSWC_VLAN_SPOOF_SHIFT 8
-#define E1000_DTXSWC_VMDQ_LOOPBACK_EN (1 << 31)  /* global VF LB enable */
+#define E1000_ETQF_FILTER_ENABLE	(1 << 26)
+#define E1000_ETQF_IMM_INT		(1 << 29)
+#define E1000_ETQF_1588			(1 << 30)
+#define E1000_ETQF_QUEUE_ENABLE		(1 << 31)
+/*
+ * ETQF filter list: one static filter per filter consumer. This is
+ *                   to avoid filter collisions later. Add new filters
+ *                   here!!
+ *
+ * Current filters:
+ *    EAPOL 802.1x (0x888e): Filter 0
+ */
+#define E1000_ETQF_FILTER_EAPOL		0
+
+#define E1000_FTQF_VF_BP		0x00008000
+#define E1000_FTQF_1588_TIME_STAMP	0x08000000
+#define E1000_FTQF_MASK			0xF0000000
+#define E1000_FTQF_MASK_PROTO_BP	0x10000000
+#define E1000_FTQF_MASK_SOURCE_ADDR_BP	0x20000000
+#define E1000_FTQF_MASK_DEST_ADDR_BP	0x40000000
+#define E1000_FTQF_MASK_SOURCE_PORT_BP	0x80000000
+
+#define E1000_NVM_APME_82575		0x0400
+#define MAX_NUM_VFS			7
+
+#define E1000_DTXSWC_MAC_SPOOF_MASK	0x000000FF /* Per VF MAC spoof cntrl */
+#define E1000_DTXSWC_VLAN_SPOOF_MASK	0x0000FF00 /* Per VF VLAN spoof cntrl */
+#define E1000_DTXSWC_LLE_MASK		0x00FF0000 /* Per VF Local LB enables */
+#define E1000_DTXSWC_VLAN_SPOOF_SHIFT	8
+#define E1000_DTXSWC_LLE_SHIFT		16
+#define E1000_DTXSWC_VMDQ_LOOPBACK_EN	(1 << 31)  /* global VF LB enable */
 
 /* Easy defines for setting default pool, would normally be left a zero */
-#define E1000_VT_CTL_DEFAULT_POOL_SHIFT 7
-#define E1000_VT_CTL_DEFAULT_POOL_MASK  (0x7 << E1000_VT_CTL_DEFAULT_POOL_SHIFT)
+#define E1000_VT_CTL_DEFAULT_POOL_SHIFT	7
+#define E1000_VT_CTL_DEFAULT_POOL_MASK	(0x7 << E1000_VT_CTL_DEFAULT_POOL_SHIFT)
 
 /* Other useful VMD_CTL register defines */
-#define E1000_VT_CTL_IGNORE_MAC         (1 << 28)
-#define E1000_VT_CTL_DISABLE_DEF_POOL   (1 << 29)
-#define E1000_VT_CTL_VM_REPL_EN         (1 << 30)
+#define E1000_VT_CTL_IGNORE_MAC		(1 << 28)
+#define E1000_VT_CTL_DISABLE_DEF_POOL	(1 << 29)
+#define E1000_VT_CTL_VM_REPL_EN		(1 << 30)
 
 /* Per VM Offload register setup */
-#define E1000_VMOLR_RLPML_MASK 0x00003FFF /* Long Packet Maximum Length mask */
-#define E1000_VMOLR_LPE        0x00010000 /* Accept Long packet */
-#define E1000_VMOLR_RSSE       0x00020000 /* Enable RSS */
-#define E1000_VMOLR_AUPE       0x01000000 /* Accept untagged packets */
-#define E1000_VMOLR_ROMPE      0x02000000 /* Accept overflow multicast */
-#define E1000_VMOLR_ROPE       0x04000000 /* Accept overflow unicast */
-#define E1000_VMOLR_BAM        0x08000000 /* Accept Broadcast packets */
-#define E1000_VMOLR_MPME       0x10000000 /* Multicast promiscuous mode */
-#define E1000_VMOLR_STRVLAN    0x40000000 /* Vlan stripping enable */
-#define E1000_VMOLR_STRCRC     0x80000000 /* CRC stripping enable */
-
-#define E1000_VLVF_ARRAY_SIZE     32
-#define E1000_VLVF_VLANID_MASK    0x00000FFF
-#define E1000_VLVF_POOLSEL_SHIFT  12
-#define E1000_VLVF_POOLSEL_MASK   (0xFF << E1000_VLVF_POOLSEL_SHIFT)
-#define E1000_VLVF_LVLAN          0x00100000
-#define E1000_VLVF_VLANID_ENABLE  0x80000000
-
-#define E1000_VMVIR_VLANA_DEFAULT      0x40000000 /* Always use default VLAN */
-#define E1000_VMVIR_VLANA_NEVER        0x80000000 /* Never insert VLAN tag */
-
-#define E1000_IOVCTL 0x05BBC
-#define E1000_IOVCTL_REUSE_VFQ 0x00000001
-
-#define E1000_RPLOLR_STRVLAN   0x40000000
-#define E1000_RPLOLR_STRCRC    0x80000000
-
-#define E1000_DTXCTL_8023LL     0x0004
-#define E1000_DTXCTL_VLAN_ADDED 0x0008
-#define E1000_DTXCTL_OOS_ENABLE 0x0010
-#define E1000_DTXCTL_MDP_EN     0x0020
-#define E1000_DTXCTL_SPOOF_INT  0x0040
+#define E1000_VMOLR_RLPML_MASK	0x00003FFF /* Long Packet Maximum Length mask */
+#define E1000_VMOLR_LPE		0x00010000 /* Accept Long packet */
+#define E1000_VMOLR_RSSE	0x00020000 /* Enable RSS */
+#define E1000_VMOLR_AUPE	0x01000000 /* Accept untagged packets */
+#define E1000_VMOLR_ROMPE	0x02000000 /* Accept overflow multicast */
+#define E1000_VMOLR_ROPE	0x04000000 /* Accept overflow unicast */
+#define E1000_VMOLR_BAM		0x08000000 /* Accept Broadcast packets */
+#define E1000_VMOLR_MPME	0x10000000 /* Multicast promiscuous mode */
+#define E1000_VMOLR_STRVLAN	0x40000000 /* Vlan stripping enable */
+#define E1000_VMOLR_STRCRC	0x80000000 /* CRC stripping enable */
+
+#define E1000_VMOLR_VPE		0x00800000 /* VLAN promiscuous enable */
+#define E1000_VMOLR_UPE		0x20000000 /* Unicast promisuous enable */
+#define E1000_DVMOLR_HIDVLAN	0x20000000 /* Vlan hiding enable */
+#define E1000_DVMOLR_STRVLAN	0x40000000 /* Vlan stripping enable */
+#define E1000_DVMOLR_STRCRC	0x80000000 /* CRC stripping enable */
+
+#define E1000_PBRWAC_WALPB	0x00000007 /* Wrap around event on LAN Rx PB */
+#define E1000_PBRWAC_PBE	0x00000008 /* Rx packet buffer empty */
+
+#define E1000_VLVF_ARRAY_SIZE		32
+#define E1000_VLVF_VLANID_MASK		0x00000FFF
+#define E1000_VLVF_POOLSEL_SHIFT	12
+#define E1000_VLVF_POOLSEL_MASK		(0xFF << E1000_VLVF_POOLSEL_SHIFT)
+#define E1000_VLVF_LVLAN		0x00100000
+#define E1000_VLVF_VLANID_ENABLE	0x80000000
+
+#define E1000_VMVIR_VLANA_DEFAULT	0x40000000 /* Always use default VLAN */
+#define E1000_VMVIR_VLANA_NEVER		0x80000000 /* Never insert VLAN tag */
+
+#define E1000_VF_INIT_TIMEOUT	200 /* Number of retries to clear RSTI */
+
+#define E1000_IOVCTL		0x05BBC
+#define E1000_IOVCTL_REUSE_VFQ	0x00000001
+
+#define E1000_RPLOLR_STRVLAN	0x40000000
+#define E1000_RPLOLR_STRCRC	0x80000000
+
+#define E1000_TCTL_EXT_COLD	0x000FFC00
+#define E1000_TCTL_EXT_COLD_SHIFT	10
+
+#define E1000_DTXCTL_8023LL	0x0004
+#define E1000_DTXCTL_VLAN_ADDED	0x0008
+#define E1000_DTXCTL_OOS_ENABLE	0x0010
+#define E1000_DTXCTL_MDP_EN	0x0020
+#define E1000_DTXCTL_SPOOF_INT	0x0040
 
 #define E1000_EEPROM_PCS_AUTONEG_DISABLE_BIT	(1 << 14)
 
-#define ALL_QUEUES   0xFFFF
-
-/* RX packet buffer size defines */
-#define E1000_RXPBS_SIZE_MASK_82576  0x0000007F
-void igb_vmdq_set_anti_spoofing_pf(struct e1000_hw *, bool, int);
-void igb_vmdq_set_loopback_pf(struct e1000_hw *, bool);
-void igb_vmdq_set_replication_pf(struct e1000_hw *, bool);
-u16 igb_rxpbs_adjust_82580(u32 data);
-s32 igb_set_eee_i350(struct e1000_hw *);
-
-#endif
+#define ALL_QUEUES		0xFFFF
+
+/* Rx packet buffer size defines */
+#define E1000_RXPBS_SIZE_MASK_82576	0x0000007F
+void e1000_vmdq_set_loopback_pf(struct e1000_hw *hw, bool enable);
+void e1000_vmdq_set_anti_spoofing_pf(struct e1000_hw *hw, bool enable, int pf);
+void e1000_vmdq_set_replication_pf(struct e1000_hw *hw, bool enable);
+s32 e1000_init_nvm_params_82575(struct e1000_hw *hw);
+
+u16 e1000_rxpbs_adjust_82580(u32 data);
+s32 e1000_read_emi_reg(struct e1000_hw *hw, u16 addr, u16 *data);
+s32 e1000_set_eee_i350(struct e1000_hw *);
+s32 e1000_set_eee_i354(struct e1000_hw *);
+s32 e1000_get_eee_status_i354(struct e1000_hw *, bool *);
+s32 e1000_initialize_M88E1512_phy(struct e1000_hw *hw);
+#define E1000_I2C_THERMAL_SENSOR_ADDR	0xF8
+#define E1000_EMC_INTERNAL_DATA		0x00
+#define E1000_EMC_INTERNAL_THERM_LIMIT	0x20
+#define E1000_EMC_DIODE1_DATA		0x01
+#define E1000_EMC_DIODE1_THERM_LIMIT	0x19
+#define E1000_EMC_DIODE2_DATA		0x23
+#define E1000_EMC_DIODE2_THERM_LIMIT	0x1A
+#define E1000_EMC_DIODE3_DATA		0x2A
+#define E1000_EMC_DIODE3_THERM_LIMIT	0x30
+
+s32 e1000_get_thermal_sensor_data_generic(struct e1000_hw *hw);
+s32 e1000_init_thermal_sensor_thresh_generic(struct e1000_hw *hw);
+
+/* I2C SDA and SCL timing parameters for standard mode */
+#define E1000_I2C_T_HD_STA	4
+#define E1000_I2C_T_LOW		5
+#define E1000_I2C_T_HIGH	4
+#define E1000_I2C_T_SU_STA	5
+#define E1000_I2C_T_HD_DATA	5
+#define E1000_I2C_T_SU_DATA	1
+#define E1000_I2C_T_RISE	1
+#define E1000_I2C_T_FALL	1
+#define E1000_I2C_T_SU_STO	4
+#define E1000_I2C_T_BUF		5
+
+s32 e1000_set_i2c_bb(struct e1000_hw *hw);
+s32 e1000_read_i2c_byte_generic(struct e1000_hw *hw, u8 byte_offset,
+				u8 dev_addr, u8 *data);
+s32 e1000_write_i2c_byte_generic(struct e1000_hw *hw, u8 byte_offset,
+				 u8 dev_addr, u8 data);
+void e1000_i2c_bus_clear(struct e1000_hw *hw);
+#endif /* _E1000_82575_H_ */
diff --git a/drivers/net/igb/e1000_api.c b/drivers/net/igb/e1000_api.c
new file mode 100644
index 000000000000..12e66a7e3817
--- /dev/null
+++ b/drivers/net/igb/e1000_api.c
@@ -0,0 +1,1162 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+#include "e1000_api.h"
+
+/**
+ *  e1000_init_mac_params - Initialize MAC function pointers
+ *  @hw: pointer to the HW structure
+ *
+ *  This function initializes the function pointers for the MAC
+ *  set of functions.  Called by drivers or by e1000_setup_init_funcs.
+ **/
+s32 e1000_init_mac_params(struct e1000_hw *hw)
+{
+	s32 ret_val = E1000_SUCCESS;
+
+	if (hw->mac.ops.init_params) {
+		ret_val = hw->mac.ops.init_params(hw);
+		if (ret_val) {
+			DEBUGOUT("MAC Initialization Error\n");
+			goto out;
+		}
+	} else {
+		DEBUGOUT("mac.init_mac_params was NULL\n");
+		ret_val = -E1000_ERR_CONFIG;
+	}
+
+out:
+	return ret_val;
+}
+
+/**
+ *  e1000_init_nvm_params - Initialize NVM function pointers
+ *  @hw: pointer to the HW structure
+ *
+ *  This function initializes the function pointers for the NVM
+ *  set of functions.  Called by drivers or by e1000_setup_init_funcs.
+ **/
+s32 e1000_init_nvm_params(struct e1000_hw *hw)
+{
+	s32 ret_val = E1000_SUCCESS;
+
+	if (hw->nvm.ops.init_params) {
+		ret_val = hw->nvm.ops.init_params(hw);
+		if (ret_val) {
+			DEBUGOUT("NVM Initialization Error\n");
+			goto out;
+		}
+	} else {
+		DEBUGOUT("nvm.init_nvm_params was NULL\n");
+		ret_val = -E1000_ERR_CONFIG;
+	}
+
+out:
+	return ret_val;
+}
+
+/**
+ *  e1000_init_phy_params - Initialize PHY function pointers
+ *  @hw: pointer to the HW structure
+ *
+ *  This function initializes the function pointers for the PHY
+ *  set of functions.  Called by drivers or by e1000_setup_init_funcs.
+ **/
+s32 e1000_init_phy_params(struct e1000_hw *hw)
+{
+	s32 ret_val = E1000_SUCCESS;
+
+	if (hw->phy.ops.init_params) {
+		ret_val = hw->phy.ops.init_params(hw);
+		if (ret_val) {
+			DEBUGOUT("PHY Initialization Error\n");
+			goto out;
+		}
+	} else {
+		DEBUGOUT("phy.init_phy_params was NULL\n");
+		ret_val =  -E1000_ERR_CONFIG;
+	}
+
+out:
+	return ret_val;
+}
+
+/**
+ *  e1000_init_mbx_params - Initialize mailbox function pointers
+ *  @hw: pointer to the HW structure
+ *
+ *  This function initializes the function pointers for the PHY
+ *  set of functions.  Called by drivers or by e1000_setup_init_funcs.
+ **/
+s32 e1000_init_mbx_params(struct e1000_hw *hw)
+{
+	s32 ret_val = E1000_SUCCESS;
+
+	if (hw->mbx.ops.init_params) {
+		ret_val = hw->mbx.ops.init_params(hw);
+		if (ret_val) {
+			DEBUGOUT("Mailbox Initialization Error\n");
+			goto out;
+		}
+	} else {
+		DEBUGOUT("mbx.init_mbx_params was NULL\n");
+		ret_val =  -E1000_ERR_CONFIG;
+	}
+
+out:
+	return ret_val;
+}
+
+/**
+ *  e1000_set_mac_type - Sets MAC type
+ *  @hw: pointer to the HW structure
+ *
+ *  This function sets the mac type of the adapter based on the
+ *  device ID stored in the hw structure.
+ *  MUST BE FIRST FUNCTION CALLED (explicitly or through
+ *  e1000_setup_init_funcs()).
+ **/
+s32 e1000_set_mac_type(struct e1000_hw *hw)
+{
+	struct e1000_mac_info *mac = &hw->mac;
+	s32 ret_val = E1000_SUCCESS;
+
+	DEBUGFUNC("e1000_set_mac_type");
+
+	switch (hw->device_id) {
+	case E1000_DEV_ID_82575EB_COPPER:
+	case E1000_DEV_ID_82575EB_FIBER_SERDES:
+	case E1000_DEV_ID_82575GB_QUAD_COPPER:
+		mac->type = e1000_82575;
+		break;
+	case E1000_DEV_ID_82576:
+	case E1000_DEV_ID_82576_FIBER:
+	case E1000_DEV_ID_82576_SERDES:
+	case E1000_DEV_ID_82576_QUAD_COPPER:
+	case E1000_DEV_ID_82576_QUAD_COPPER_ET2:
+	case E1000_DEV_ID_82576_NS:
+	case E1000_DEV_ID_82576_NS_SERDES:
+	case E1000_DEV_ID_82576_SERDES_QUAD:
+		mac->type = e1000_82576;
+		break;
+	case E1000_DEV_ID_82580_COPPER:
+	case E1000_DEV_ID_82580_FIBER:
+	case E1000_DEV_ID_82580_SERDES:
+	case E1000_DEV_ID_82580_SGMII:
+	case E1000_DEV_ID_82580_COPPER_DUAL:
+	case E1000_DEV_ID_82580_QUAD_FIBER:
+	case E1000_DEV_ID_DH89XXCC_SGMII:
+	case E1000_DEV_ID_DH89XXCC_SERDES:
+	case E1000_DEV_ID_DH89XXCC_BACKPLANE:
+	case E1000_DEV_ID_DH89XXCC_SFP:
+		mac->type = e1000_82580;
+		break;
+	case E1000_DEV_ID_I350_COPPER:
+	case E1000_DEV_ID_I350_FIBER:
+	case E1000_DEV_ID_I350_SERDES:
+	case E1000_DEV_ID_I350_SGMII:
+	case E1000_DEV_ID_I350_DA4:
+		mac->type = e1000_i350;
+		break;
+	case E1000_DEV_ID_I210_COPPER_FLASHLESS:
+	case E1000_DEV_ID_I210_SERDES_FLASHLESS:
+	case E1000_DEV_ID_I210_TOOLS_ONLY:
+	case E1000_DEV_ID_I210_COPPER:
+	case E1000_DEV_ID_I210_COPPER_OEM1:
+	case E1000_DEV_ID_I210_COPPER_IT:
+	case E1000_DEV_ID_I210_FIBER:
+	case E1000_DEV_ID_I210_SERDES:
+	case E1000_DEV_ID_I210_SGMII:
+		mac->type = e1000_i210;
+		break;
+	case E1000_DEV_ID_I211_TOOLS_ONLY:
+	case E1000_DEV_ID_I211_COPPER:
+		mac->type = e1000_i211;
+		break;
+
+	case E1000_DEV_ID_I354_BACKPLANE_1GBPS:
+	case E1000_DEV_ID_I354_SGMII:
+	case E1000_DEV_ID_I354_BACKPLANE_2_5GBPS:
+		mac->type = e1000_i354;
+		break;
+	default:
+		/* Should never have loaded on this device */
+		ret_val = -E1000_ERR_MAC_INIT;
+		break;
+	}
+
+	return ret_val;
+}
+
+/**
+ *  e1000_setup_init_funcs - Initializes function pointers
+ *  @hw: pointer to the HW structure
+ *  @init_device: true will initialize the rest of the function pointers
+ *		  getting the device ready for use.  false will only set
+ *		  MAC type and the function pointers for the other init
+ *		  functions.  Passing false will not generate any hardware
+ *		  reads or writes.
+ *
+ *  This function must be called by a driver in order to use the rest
+ *  of the 'shared' code files. Called by drivers only.
+ **/
+s32 e1000_setup_init_funcs(struct e1000_hw *hw, bool init_device)
+{
+	s32 ret_val;
+
+	/* Can't do much good without knowing the MAC type. */
+	ret_val = e1000_set_mac_type(hw);
+	if (ret_val) {
+		DEBUGOUT("ERROR: MAC type could not be set properly.\n");
+		goto out;
+	}
+
+	if (!hw->hw_addr) {
+		DEBUGOUT("ERROR: Registers not mapped\n");
+		ret_val = -E1000_ERR_CONFIG;
+		goto out;
+	}
+
+	/*
+	 * Init function pointers to generic implementations. We do this first
+	 * allowing a driver module to override it afterward.
+	 */
+	e1000_init_mac_ops_generic(hw);
+	e1000_init_phy_ops_generic(hw);
+	e1000_init_nvm_ops_generic(hw);
+	e1000_init_mbx_ops_generic(hw);
+
+	/*
+	 * Set up the init function pointers. These are functions within the
+	 * adapter family file that sets up function pointers for the rest of
+	 * the functions in that family.
+	 */
+	switch (hw->mac.type) {
+	case e1000_82575:
+	case e1000_82576:
+	case e1000_82580:
+	case e1000_i350:
+	case e1000_i354:
+		e1000_init_function_pointers_82575(hw);
+		break;
+	case e1000_i210:
+	case e1000_i211:
+		e1000_init_function_pointers_i210(hw);
+		break;
+	default:
+		DEBUGOUT("Hardware not supported\n");
+		ret_val = -E1000_ERR_CONFIG;
+		break;
+	}
+
+	/*
+	 * Initialize the rest of the function pointers. These require some
+	 * register reads/writes in some cases.
+	 */
+	if (!(ret_val) && init_device) {
+		ret_val = e1000_init_mac_params(hw);
+		if (ret_val)
+			goto out;
+
+		ret_val = e1000_init_nvm_params(hw);
+		if (ret_val)
+			goto out;
+
+		ret_val = e1000_init_phy_params(hw);
+		if (ret_val)
+			goto out;
+
+		ret_val = e1000_init_mbx_params(hw);
+		if (ret_val)
+			goto out;
+	}
+
+out:
+	return ret_val;
+}
+
+/**
+ *  e1000_get_bus_info - Obtain bus information for adapter
+ *  @hw: pointer to the HW structure
+ *
+ *  This will obtain information about the HW bus for which the
+ *  adapter is attached and stores it in the hw structure. This is a
+ *  function pointer entry point called by drivers.
+ **/
+s32 e1000_get_bus_info(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.get_bus_info)
+		return hw->mac.ops.get_bus_info(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_clear_vfta - Clear VLAN filter table
+ *  @hw: pointer to the HW structure
+ *
+ *  This clears the VLAN filter table on the adapter. This is a function
+ *  pointer entry point called by drivers.
+ **/
+void e1000_clear_vfta(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.clear_vfta)
+		hw->mac.ops.clear_vfta(hw);
+}
+
+/**
+ *  e1000_write_vfta - Write value to VLAN filter table
+ *  @hw: pointer to the HW structure
+ *  @offset: the 32-bit offset in which to write the value to.
+ *  @value: the 32-bit value to write at location offset.
+ *
+ *  This writes a 32-bit value to a 32-bit offset in the VLAN filter
+ *  table. This is a function pointer entry point called by drivers.
+ **/
+void e1000_write_vfta(struct e1000_hw *hw, u32 offset, u32 value)
+{
+	if (hw->mac.ops.write_vfta)
+		hw->mac.ops.write_vfta(hw, offset, value);
+}
+
+/**
+ *  e1000_update_mc_addr_list - Update Multicast addresses
+ *  @hw: pointer to the HW structure
+ *  @mc_addr_list: array of multicast addresses to program
+ *  @mc_addr_count: number of multicast addresses to program
+ *
+ *  Updates the Multicast Table Array.
+ *  The caller must have a packed mc_addr_list of multicast addresses.
+ **/
+void e1000_update_mc_addr_list(struct e1000_hw *hw, u8 *mc_addr_list,
+			       u32 mc_addr_count)
+{
+	if (hw->mac.ops.update_mc_addr_list)
+		hw->mac.ops.update_mc_addr_list(hw, mc_addr_list,
+						mc_addr_count);
+}
+
+/**
+ *  e1000_force_mac_fc - Force MAC flow control
+ *  @hw: pointer to the HW structure
+ *
+ *  Force the MAC's flow control settings. Currently no func pointer exists
+ *  and all implementations are handled in the generic version of this
+ *  function.
+ **/
+s32 e1000_force_mac_fc(struct e1000_hw *hw)
+{
+	return e1000_force_mac_fc_generic(hw);
+}
+
+/**
+ *  e1000_check_for_link - Check/Store link connection
+ *  @hw: pointer to the HW structure
+ *
+ *  This checks the link condition of the adapter and stores the
+ *  results in the hw->mac structure. This is a function pointer entry
+ *  point called by drivers.
+ **/
+s32 e1000_check_for_link(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.check_for_link)
+		return hw->mac.ops.check_for_link(hw);
+
+	return -E1000_ERR_CONFIG;
+}
+
+/**
+ *  e1000_check_mng_mode - Check management mode
+ *  @hw: pointer to the HW structure
+ *
+ *  This checks if the adapter has manageability enabled.
+ *  This is a function pointer entry point called by drivers.
+ **/
+bool e1000_check_mng_mode(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.check_mng_mode)
+		return hw->mac.ops.check_mng_mode(hw);
+
+	return false;
+}
+
+/**
+ *  e1000_mng_write_dhcp_info - Writes DHCP info to host interface
+ *  @hw: pointer to the HW structure
+ *  @buffer: pointer to the host interface
+ *  @length: size of the buffer
+ *
+ *  Writes the DHCP information to the host interface.
+ **/
+s32 e1000_mng_write_dhcp_info(struct e1000_hw *hw, u8 *buffer, u16 length)
+{
+	return e1000_mng_write_dhcp_info_generic(hw, buffer, length);
+}
+
+/**
+ *  e1000_reset_hw - Reset hardware
+ *  @hw: pointer to the HW structure
+ *
+ *  This resets the hardware into a known state. This is a function pointer
+ *  entry point called by drivers.
+ **/
+s32 e1000_reset_hw(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.reset_hw)
+		return hw->mac.ops.reset_hw(hw);
+
+	return -E1000_ERR_CONFIG;
+}
+
+/**
+ *  e1000_init_hw - Initialize hardware
+ *  @hw: pointer to the HW structure
+ *
+ *  This inits the hardware readying it for operation. This is a function
+ *  pointer entry point called by drivers.
+ **/
+s32 e1000_init_hw(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.init_hw)
+		return hw->mac.ops.init_hw(hw);
+
+	return -E1000_ERR_CONFIG;
+}
+
+/**
+ *  e1000_setup_link - Configures link and flow control
+ *  @hw: pointer to the HW structure
+ *
+ *  This configures link and flow control settings for the adapter. This
+ *  is a function pointer entry point called by drivers. While modules can
+ *  also call this, they probably call their own version of this function.
+ **/
+s32 e1000_setup_link(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.setup_link)
+		return hw->mac.ops.setup_link(hw);
+
+	return -E1000_ERR_CONFIG;
+}
+
+/**
+ *  e1000_get_speed_and_duplex - Returns current speed and duplex
+ *  @hw: pointer to the HW structure
+ *  @speed: pointer to a 16-bit value to store the speed
+ *  @duplex: pointer to a 16-bit value to store the duplex.
+ *
+ *  This returns the speed and duplex of the adapter in the two 'out'
+ *  variables passed in. This is a function pointer entry point called
+ *  by drivers.
+ **/
+s32 e1000_get_speed_and_duplex(struct e1000_hw *hw, u16 *speed, u16 *duplex)
+{
+	if (hw->mac.ops.get_link_up_info)
+		return hw->mac.ops.get_link_up_info(hw, speed, duplex);
+
+	return -E1000_ERR_CONFIG;
+}
+
+/**
+ *  e1000_setup_led - Configures SW controllable LED
+ *  @hw: pointer to the HW structure
+ *
+ *  This prepares the SW controllable LED for use and saves the current state
+ *  of the LED so it can be later restored. This is a function pointer entry
+ *  point called by drivers.
+ **/
+s32 e1000_setup_led(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.setup_led)
+		return hw->mac.ops.setup_led(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_cleanup_led - Restores SW controllable LED
+ *  @hw: pointer to the HW structure
+ *
+ *  This restores the SW controllable LED to the value saved off by
+ *  e1000_setup_led. This is a function pointer entry point called by drivers.
+ **/
+s32 e1000_cleanup_led(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.cleanup_led)
+		return hw->mac.ops.cleanup_led(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_blink_led - Blink SW controllable LED
+ *  @hw: pointer to the HW structure
+ *
+ *  This starts the adapter LED blinking. Request the LED to be setup first
+ *  and cleaned up after. This is a function pointer entry point called by
+ *  drivers.
+ **/
+s32 e1000_blink_led(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.blink_led)
+		return hw->mac.ops.blink_led(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_id_led_init - store LED configurations in SW
+ *  @hw: pointer to the HW structure
+ *
+ *  Initializes the LED config in SW. This is a function pointer entry point
+ *  called by drivers.
+ **/
+s32 e1000_id_led_init(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.id_led_init)
+		return hw->mac.ops.id_led_init(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_led_on - Turn on SW controllable LED
+ *  @hw: pointer to the HW structure
+ *
+ *  Turns the SW defined LED on. This is a function pointer entry point
+ *  called by drivers.
+ **/
+s32 e1000_led_on(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.led_on)
+		return hw->mac.ops.led_on(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_led_off - Turn off SW controllable LED
+ *  @hw: pointer to the HW structure
+ *
+ *  Turns the SW defined LED off. This is a function pointer entry point
+ *  called by drivers.
+ **/
+s32 e1000_led_off(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.led_off)
+		return hw->mac.ops.led_off(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_reset_adaptive - Reset adaptive IFS
+ *  @hw: pointer to the HW structure
+ *
+ *  Resets the adaptive IFS. Currently no func pointer exists and all
+ *  implementations are handled in the generic version of this function.
+ **/
+void e1000_reset_adaptive(struct e1000_hw *hw)
+{
+	e1000_reset_adaptive_generic(hw);
+}
+
+/**
+ *  e1000_update_adaptive - Update adaptive IFS
+ *  @hw: pointer to the HW structure
+ *
+ *  Updates adapter IFS. Currently no func pointer exists and all
+ *  implementations are handled in the generic version of this function.
+ **/
+void e1000_update_adaptive(struct e1000_hw *hw)
+{
+	e1000_update_adaptive_generic(hw);
+}
+
+/**
+ *  e1000_disable_pcie_master - Disable PCI-Express master access
+ *  @hw: pointer to the HW structure
+ *
+ *  Disables PCI-Express master access and verifies there are no pending
+ *  requests. Currently no func pointer exists and all implementations are
+ *  handled in the generic version of this function.
+ **/
+s32 e1000_disable_pcie_master(struct e1000_hw *hw)
+{
+	return e1000_disable_pcie_master_generic(hw);
+}
+
+/**
+ *  e1000_config_collision_dist - Configure collision distance
+ *  @hw: pointer to the HW structure
+ *
+ *  Configures the collision distance to the default value and is used
+ *  during link setup.
+ **/
+void e1000_config_collision_dist(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.config_collision_dist)
+		hw->mac.ops.config_collision_dist(hw);
+}
+
+/**
+ *  e1000_rar_set - Sets a receive address register
+ *  @hw: pointer to the HW structure
+ *  @addr: address to set the RAR to
+ *  @index: the RAR to set
+ *
+ *  Sets a Receive Address Register (RAR) to the specified address.
+ **/
+void e1000_rar_set(struct e1000_hw *hw, u8 *addr, u32 index)
+{
+	if (hw->mac.ops.rar_set)
+		hw->mac.ops.rar_set(hw, addr, index);
+}
+
+/**
+ *  e1000_validate_mdi_setting - Ensures valid MDI/MDIX SW state
+ *  @hw: pointer to the HW structure
+ *
+ *  Ensures that the MDI/MDIX SW state is valid.
+ **/
+s32 e1000_validate_mdi_setting(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.validate_mdi_setting)
+		return hw->mac.ops.validate_mdi_setting(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_hash_mc_addr - Determines address location in multicast table
+ *  @hw: pointer to the HW structure
+ *  @mc_addr: Multicast address to hash.
+ *
+ *  This hashes an address to determine its location in the multicast
+ *  table. Currently no func pointer exists and all implementations
+ *  are handled in the generic version of this function.
+ **/
+u32 e1000_hash_mc_addr(struct e1000_hw *hw, u8 *mc_addr)
+{
+	return e1000_hash_mc_addr_generic(hw, mc_addr);
+}
+
+/**
+ *  e1000_enable_tx_pkt_filtering - Enable packet filtering on TX
+ *  @hw: pointer to the HW structure
+ *
+ *  Enables packet filtering on transmit packets if manageability is enabled
+ *  and host interface is enabled.
+ *  Currently no func pointer exists and all implementations are handled in the
+ *  generic version of this function.
+ **/
+bool e1000_enable_tx_pkt_filtering(struct e1000_hw *hw)
+{
+	return e1000_enable_tx_pkt_filtering_generic(hw);
+}
+
+/**
+ *  e1000_mng_host_if_write - Writes to the manageability host interface
+ *  @hw: pointer to the HW structure
+ *  @buffer: pointer to the host interface buffer
+ *  @length: size of the buffer
+ *  @offset: location in the buffer to write to
+ *  @sum: sum of the data (not checksum)
+ *
+ *  This function writes the buffer content at the offset given on the host if.
+ *  It also does alignment considerations to do the writes in most efficient
+ *  way.  Also fills up the sum of the buffer in *buffer parameter.
+ **/
+s32 e1000_mng_host_if_write(struct e1000_hw *hw, u8 *buffer, u16 length,
+			    u16 offset, u8 *sum)
+{
+	return e1000_mng_host_if_write_generic(hw, buffer, length, offset, sum);
+}
+
+/**
+ *  e1000_mng_write_cmd_header - Writes manageability command header
+ *  @hw: pointer to the HW structure
+ *  @hdr: pointer to the host interface command header
+ *
+ *  Writes the command header after does the checksum calculation.
+ **/
+s32 e1000_mng_write_cmd_header(struct e1000_hw *hw,
+			       struct e1000_host_mng_command_header *hdr)
+{
+	return e1000_mng_write_cmd_header_generic(hw, hdr);
+}
+
+/**
+ *  e1000_mng_enable_host_if - Checks host interface is enabled
+ *  @hw: pointer to the HW structure
+ *
+ *  Returns E1000_success upon success, else E1000_ERR_HOST_INTERFACE_COMMAND
+ *
+ *  This function checks whether the HOST IF is enabled for command operation
+ *  and also checks whether the previous command is completed.  It busy waits
+ *  in case of previous command is not completed.
+ **/
+s32 e1000_mng_enable_host_if(struct e1000_hw *hw)
+{
+	return e1000_mng_enable_host_if_generic(hw);
+}
+
+/**
+ *  e1000_check_reset_block - Verifies PHY can be reset
+ *  @hw: pointer to the HW structure
+ *
+ *  Checks if the PHY is in a state that can be reset or if manageability
+ *  has it tied up. This is a function pointer entry point called by drivers.
+ **/
+s32 e1000_check_reset_block(struct e1000_hw *hw)
+{
+	if (hw->phy.ops.check_reset_block)
+		return hw->phy.ops.check_reset_block(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_read_phy_reg - Reads PHY register
+ *  @hw: pointer to the HW structure
+ *  @offset: the register to read
+ *  @data: the buffer to store the 16-bit read.
+ *
+ *  Reads the PHY register and returns the value in data.
+ *  This is a function pointer entry point called by drivers.
+ **/
+s32 e1000_read_phy_reg(struct e1000_hw *hw, u32 offset, u16 *data)
+{
+	if (hw->phy.ops.read_reg)
+		return hw->phy.ops.read_reg(hw, offset, data);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_write_phy_reg - Writes PHY register
+ *  @hw: pointer to the HW structure
+ *  @offset: the register to write
+ *  @data: the value to write.
+ *
+ *  Writes the PHY register at offset with the value in data.
+ *  This is a function pointer entry point called by drivers.
+ **/
+s32 e1000_write_phy_reg(struct e1000_hw *hw, u32 offset, u16 data)
+{
+	if (hw->phy.ops.write_reg)
+		return hw->phy.ops.write_reg(hw, offset, data);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_release_phy - Generic release PHY
+ *  @hw: pointer to the HW structure
+ *
+ *  Return if silicon family does not require a semaphore when accessing the
+ *  PHY.
+ **/
+void e1000_release_phy(struct e1000_hw *hw)
+{
+	if (hw->phy.ops.release)
+		hw->phy.ops.release(hw);
+}
+
+/**
+ *  e1000_acquire_phy - Generic acquire PHY
+ *  @hw: pointer to the HW structure
+ *
+ *  Return success if silicon family does not require a semaphore when
+ *  accessing the PHY.
+ **/
+s32 e1000_acquire_phy(struct e1000_hw *hw)
+{
+	if (hw->phy.ops.acquire)
+		return hw->phy.ops.acquire(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_read_kmrn_reg - Reads register using Kumeran interface
+ *  @hw: pointer to the HW structure
+ *  @offset: the register to read
+ *  @data: the location to store the 16-bit value read.
+ *
+ *  Reads a register out of the Kumeran interface. Currently no func pointer
+ *  exists and all implementations are handled in the generic version of
+ *  this function.
+ **/
+s32 e1000_read_kmrn_reg(struct e1000_hw *hw, u32 offset, u16 *data)
+{
+	return e1000_read_kmrn_reg_generic(hw, offset, data);
+}
+
+/**
+ *  e1000_write_kmrn_reg - Writes register using Kumeran interface
+ *  @hw: pointer to the HW structure
+ *  @offset: the register to write
+ *  @data: the value to write.
+ *
+ *  Writes a register to the Kumeran interface. Currently no func pointer
+ *  exists and all implementations are handled in the generic version of
+ *  this function.
+ **/
+s32 e1000_write_kmrn_reg(struct e1000_hw *hw, u32 offset, u16 data)
+{
+	return e1000_write_kmrn_reg_generic(hw, offset, data);
+}
+
+/**
+ *  e1000_get_cable_length - Retrieves cable length estimation
+ *  @hw: pointer to the HW structure
+ *
+ *  This function estimates the cable length and stores them in
+ *  hw->phy.min_length and hw->phy.max_length. This is a function pointer
+ *  entry point called by drivers.
+ **/
+s32 e1000_get_cable_length(struct e1000_hw *hw)
+{
+	if (hw->phy.ops.get_cable_length)
+		return hw->phy.ops.get_cable_length(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_get_phy_info - Retrieves PHY information from registers
+ *  @hw: pointer to the HW structure
+ *
+ *  This function gets some information from various PHY registers and
+ *  populates hw->phy values with it. This is a function pointer entry
+ *  point called by drivers.
+ **/
+s32 e1000_get_phy_info(struct e1000_hw *hw)
+{
+	if (hw->phy.ops.get_info)
+		return hw->phy.ops.get_info(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_phy_hw_reset - Hard PHY reset
+ *  @hw: pointer to the HW structure
+ *
+ *  Performs a hard PHY reset. This is a function pointer entry point called
+ *  by drivers.
+ **/
+s32 e1000_phy_hw_reset(struct e1000_hw *hw)
+{
+	if (hw->phy.ops.reset)
+		return hw->phy.ops.reset(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_phy_commit - Soft PHY reset
+ *  @hw: pointer to the HW structure
+ *
+ *  Performs a soft PHY reset on those that apply. This is a function pointer
+ *  entry point called by drivers.
+ **/
+s32 e1000_phy_commit(struct e1000_hw *hw)
+{
+	if (hw->phy.ops.commit)
+		return hw->phy.ops.commit(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_set_d0_lplu_state - Sets low power link up state for D0
+ *  @hw: pointer to the HW structure
+ *  @active: boolean used to enable/disable lplu
+ *
+ *  Success returns 0, Failure returns 1
+ *
+ *  The low power link up (lplu) state is set to the power management level D0
+ *  and SmartSpeed is disabled when active is true, else clear lplu for D0
+ *  and enable Smartspeed.  LPLU and Smartspeed are mutually exclusive.  LPLU
+ *  is used during Dx states where the power conservation is most important.
+ *  During driver activity, SmartSpeed should be enabled so performance is
+ *  maintained.  This is a function pointer entry point called by drivers.
+ **/
+s32 e1000_set_d0_lplu_state(struct e1000_hw *hw, bool active)
+{
+	if (hw->phy.ops.set_d0_lplu_state)
+		return hw->phy.ops.set_d0_lplu_state(hw, active);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_set_d3_lplu_state - Sets low power link up state for D3
+ *  @hw: pointer to the HW structure
+ *  @active: boolean used to enable/disable lplu
+ *
+ *  Success returns 0, Failure returns 1
+ *
+ *  The low power link up (lplu) state is set to the power management level D3
+ *  and SmartSpeed is disabled when active is true, else clear lplu for D3
+ *  and enable Smartspeed.  LPLU and Smartspeed are mutually exclusive.  LPLU
+ *  is used during Dx states where the power conservation is most important.
+ *  During driver activity, SmartSpeed should be enabled so performance is
+ *  maintained.  This is a function pointer entry point called by drivers.
+ **/
+s32 e1000_set_d3_lplu_state(struct e1000_hw *hw, bool active)
+{
+	if (hw->phy.ops.set_d3_lplu_state)
+		return hw->phy.ops.set_d3_lplu_state(hw, active);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_read_mac_addr - Reads MAC address
+ *  @hw: pointer to the HW structure
+ *
+ *  Reads the MAC address out of the adapter and stores it in the HW structure.
+ *  Currently no func pointer exists and all implementations are handled in the
+ *  generic version of this function.
+ **/
+s32 e1000_read_mac_addr(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.read_mac_addr)
+		return hw->mac.ops.read_mac_addr(hw);
+
+	return e1000_read_mac_addr_generic(hw);
+}
+
+/**
+ *  e1000_read_pba_string - Read device part number string
+ *  @hw: pointer to the HW structure
+ *  @pba_num: pointer to device part number
+ *  @pba_num_size: size of part number buffer
+ *
+ *  Reads the product board assembly (PBA) number from the EEPROM and stores
+ *  the value in pba_num.
+ *  Currently no func pointer exists and all implementations are handled in the
+ *  generic version of this function.
+ **/
+s32 e1000_read_pba_string(struct e1000_hw *hw, u8 *pba_num, u32 pba_num_size)
+{
+	return e1000_read_pba_string_generic(hw, pba_num, pba_num_size);
+}
+
+/**
+ *  e1000_read_pba_length - Read device part number string length
+ *  @hw: pointer to the HW structure
+ *  @pba_num_size: size of part number buffer
+ *
+ *  Reads the product board assembly (PBA) number length from the EEPROM and
+ *  stores the value in pba_num.
+ *  Currently no func pointer exists and all implementations are handled in the
+ *  generic version of this function.
+ **/
+s32 e1000_read_pba_length(struct e1000_hw *hw, u32 *pba_num_size)
+{
+	return e1000_read_pba_length_generic(hw, pba_num_size);
+}
+
+/**
+ *  e1000_validate_nvm_checksum - Verifies NVM (EEPROM) checksum
+ *  @hw: pointer to the HW structure
+ *
+ *  Validates the NVM checksum is correct. This is a function pointer entry
+ *  point called by drivers.
+ **/
+s32 e1000_validate_nvm_checksum(struct e1000_hw *hw)
+{
+	if (hw->nvm.ops.validate)
+		return hw->nvm.ops.validate(hw);
+
+	return -E1000_ERR_CONFIG;
+}
+
+/**
+ *  e1000_update_nvm_checksum - Updates NVM (EEPROM) checksum
+ *  @hw: pointer to the HW structure
+ *
+ *  Updates the NVM checksum. Currently no func pointer exists and all
+ *  implementations are handled in the generic version of this function.
+ **/
+s32 e1000_update_nvm_checksum(struct e1000_hw *hw)
+{
+	if (hw->nvm.ops.update)
+		return hw->nvm.ops.update(hw);
+
+	return -E1000_ERR_CONFIG;
+}
+
+/**
+ *  e1000_reload_nvm - Reloads EEPROM
+ *  @hw: pointer to the HW structure
+ *
+ *  Reloads the EEPROM by setting the "Reinitialize from EEPROM" bit in the
+ *  extended control register.
+ **/
+void e1000_reload_nvm(struct e1000_hw *hw)
+{
+	if (hw->nvm.ops.reload)
+		hw->nvm.ops.reload(hw);
+}
+
+/**
+ *  e1000_read_nvm - Reads NVM (EEPROM)
+ *  @hw: pointer to the HW structure
+ *  @offset: the word offset to read
+ *  @words: number of 16-bit words to read
+ *  @data: pointer to the properly sized buffer for the data.
+ *
+ *  Reads 16-bit chunks of data from the NVM (EEPROM). This is a function
+ *  pointer entry point called by drivers.
+ **/
+s32 e1000_read_nvm(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
+{
+	if (hw->nvm.ops.read)
+		return hw->nvm.ops.read(hw, offset, words, data);
+
+	return -E1000_ERR_CONFIG;
+}
+
+/**
+ *  e1000_write_nvm - Writes to NVM (EEPROM)
+ *  @hw: pointer to the HW structure
+ *  @offset: the word offset to read
+ *  @words: number of 16-bit words to write
+ *  @data: pointer to the properly sized buffer for the data.
+ *
+ *  Writes 16-bit chunks of data to the NVM (EEPROM). This is a function
+ *  pointer entry point called by drivers.
+ **/
+s32 e1000_write_nvm(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
+{
+	if (hw->nvm.ops.write)
+		return hw->nvm.ops.write(hw, offset, words, data);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_write_8bit_ctrl_reg - Writes 8bit Control register
+ *  @hw: pointer to the HW structure
+ *  @reg: 32bit register offset
+ *  @offset: the register to write
+ *  @data: the value to write.
+ *
+ *  Writes the PHY register at offset with the value in data.
+ *  This is a function pointer entry point called by drivers.
+ **/
+s32 e1000_write_8bit_ctrl_reg(struct e1000_hw *hw, u32 reg, u32 offset,
+			      u8 data)
+{
+	return e1000_write_8bit_ctrl_reg_generic(hw, reg, offset, data);
+}
+
+/**
+ * e1000_power_up_phy - Restores link in case of PHY power down
+ * @hw: pointer to the HW structure
+ *
+ * The phy may be powered down to save power, to turn off link when the
+ * driver is unloaded, or wake on lan is not enabled (among others).
+ **/
+void e1000_power_up_phy(struct e1000_hw *hw)
+{
+	if (hw->phy.ops.power_up)
+		hw->phy.ops.power_up(hw);
+
+	e1000_setup_link(hw);
+}
+
+/**
+ * e1000_power_down_phy - Power down PHY
+ * @hw: pointer to the HW structure
+ *
+ * The phy may be powered down to save power, to turn off link when the
+ * driver is unloaded, or wake on lan is not enabled (among others).
+ **/
+void e1000_power_down_phy(struct e1000_hw *hw)
+{
+	if (hw->phy.ops.power_down)
+		hw->phy.ops.power_down(hw);
+}
+
+/**
+ *  e1000_power_up_fiber_serdes_link - Power up serdes link
+ *  @hw: pointer to the HW structure
+ *
+ *  Power on the optics and PCS.
+ **/
+void e1000_power_up_fiber_serdes_link(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.power_up_serdes)
+		hw->mac.ops.power_up_serdes(hw);
+}
+
+/**
+ *  e1000_shutdown_fiber_serdes_link - Remove link during power down
+ *  @hw: pointer to the HW structure
+ *
+ *  Shutdown the optics and PCS on driver unload.
+ **/
+void e1000_shutdown_fiber_serdes_link(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.shutdown_serdes)
+		hw->mac.ops.shutdown_serdes(hw);
+}
+
+/**
+ *  e1000_get_thermal_sensor_data - Gathers thermal sensor data
+ *  @hw: pointer to hardware structure
+ *
+ *  Updates the temperatures in mac.thermal_sensor_data
+ **/
+s32 e1000_get_thermal_sensor_data(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.get_thermal_sensor_data)
+		return hw->mac.ops.get_thermal_sensor_data(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_init_thermal_sensor_thresh - Sets thermal sensor thresholds
+ *  @hw: pointer to hardware structure
+ *
+ *  Sets the thermal sensor thresholds according to the NVM map
+ **/
+s32 e1000_init_thermal_sensor_thresh(struct e1000_hw *hw)
+{
+	if (hw->mac.ops.init_thermal_sensor_thresh)
+		return hw->mac.ops.init_thermal_sensor_thresh(hw);
+
+	return E1000_SUCCESS;
+}
+
diff --git a/drivers/net/igb/e1000_api.h b/drivers/net/igb/e1000_api.h
new file mode 100644
index 000000000000..b21294ec9e18
--- /dev/null
+++ b/drivers/net/igb/e1000_api.h
@@ -0,0 +1,157 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+#ifndef _E1000_API_H_
+#define _E1000_API_H_
+
+#include "e1000_hw.h"
+
+extern void e1000_init_function_pointers_82575(struct e1000_hw *hw);
+extern void e1000_rx_fifo_flush_82575(struct e1000_hw *hw);
+extern void e1000_init_function_pointers_vf(struct e1000_hw *hw);
+extern void e1000_power_up_fiber_serdes_link(struct e1000_hw *hw);
+extern void e1000_shutdown_fiber_serdes_link(struct e1000_hw *hw);
+extern void e1000_init_function_pointers_i210(struct e1000_hw *hw);
+
+s32 e1000_set_obff_timer(struct e1000_hw *hw, u32 itr);
+s32 e1000_set_mac_type(struct e1000_hw *hw);
+s32 e1000_setup_init_funcs(struct e1000_hw *hw, bool init_device);
+s32 e1000_init_mac_params(struct e1000_hw *hw);
+s32 e1000_init_nvm_params(struct e1000_hw *hw);
+s32 e1000_init_phy_params(struct e1000_hw *hw);
+s32 e1000_init_mbx_params(struct e1000_hw *hw);
+s32 e1000_get_bus_info(struct e1000_hw *hw);
+void e1000_clear_vfta(struct e1000_hw *hw);
+void e1000_write_vfta(struct e1000_hw *hw, u32 offset, u32 value);
+s32 e1000_force_mac_fc(struct e1000_hw *hw);
+s32 e1000_check_for_link(struct e1000_hw *hw);
+s32 e1000_reset_hw(struct e1000_hw *hw);
+s32 e1000_init_hw(struct e1000_hw *hw);
+s32 e1000_setup_link(struct e1000_hw *hw);
+s32 e1000_get_speed_and_duplex(struct e1000_hw *hw, u16 *speed, u16 *duplex);
+s32 e1000_disable_pcie_master(struct e1000_hw *hw);
+void e1000_config_collision_dist(struct e1000_hw *hw);
+void e1000_rar_set(struct e1000_hw *hw, u8 *addr, u32 index);
+u32 e1000_hash_mc_addr(struct e1000_hw *hw, u8 *mc_addr);
+void e1000_update_mc_addr_list(struct e1000_hw *hw, u8 *mc_addr_list,
+			       u32 mc_addr_count);
+s32 e1000_setup_led(struct e1000_hw *hw);
+s32 e1000_cleanup_led(struct e1000_hw *hw);
+s32 e1000_check_reset_block(struct e1000_hw *hw);
+s32 e1000_blink_led(struct e1000_hw *hw);
+s32 e1000_led_on(struct e1000_hw *hw);
+s32 e1000_led_off(struct e1000_hw *hw);
+s32 e1000_id_led_init(struct e1000_hw *hw);
+void e1000_reset_adaptive(struct e1000_hw *hw);
+void e1000_update_adaptive(struct e1000_hw *hw);
+s32 e1000_get_cable_length(struct e1000_hw *hw);
+s32 e1000_validate_mdi_setting(struct e1000_hw *hw);
+s32 e1000_read_phy_reg(struct e1000_hw *hw, u32 offset, u16 *data);
+s32 e1000_write_phy_reg(struct e1000_hw *hw, u32 offset, u16 data);
+s32 e1000_write_8bit_ctrl_reg(struct e1000_hw *hw, u32 reg, u32 offset,
+			      u8 data);
+s32 e1000_get_phy_info(struct e1000_hw *hw);
+void e1000_release_phy(struct e1000_hw *hw);
+s32 e1000_acquire_phy(struct e1000_hw *hw);
+s32 e1000_phy_hw_reset(struct e1000_hw *hw);
+s32 e1000_phy_commit(struct e1000_hw *hw);
+void e1000_power_up_phy(struct e1000_hw *hw);
+void e1000_power_down_phy(struct e1000_hw *hw);
+s32 e1000_read_mac_addr(struct e1000_hw *hw);
+s32 e1000_read_pba_string(struct e1000_hw *hw, u8 *pba_num, u32 pba_num_size);
+s32 e1000_read_pba_length(struct e1000_hw *hw, u32 *pba_num_size);
+void e1000_reload_nvm(struct e1000_hw *hw);
+s32 e1000_update_nvm_checksum(struct e1000_hw *hw);
+s32 e1000_validate_nvm_checksum(struct e1000_hw *hw);
+s32 e1000_read_nvm(struct e1000_hw *hw, u16 offset, u16 words, u16 *data);
+s32 e1000_read_kmrn_reg(struct e1000_hw *hw, u32 offset, u16 *data);
+s32 e1000_write_kmrn_reg(struct e1000_hw *hw, u32 offset, u16 data);
+s32 e1000_write_nvm(struct e1000_hw *hw, u16 offset, u16 words, u16 *data);
+s32 e1000_set_d3_lplu_state(struct e1000_hw *hw, bool active);
+s32 e1000_set_d0_lplu_state(struct e1000_hw *hw, bool active);
+bool e1000_check_mng_mode(struct e1000_hw *hw);
+bool e1000_enable_tx_pkt_filtering(struct e1000_hw *hw);
+s32 e1000_mng_enable_host_if(struct e1000_hw *hw);
+s32 e1000_mng_host_if_write(struct e1000_hw *hw, u8 *buffer, u16 length,
+			    u16 offset, u8 *sum);
+s32 e1000_mng_write_cmd_header(struct e1000_hw *hw,
+			       struct e1000_host_mng_command_header *hdr);
+s32 e1000_mng_write_dhcp_info(struct e1000_hw *hw, u8 *buffer, u16 length);
+s32 e1000_get_thermal_sensor_data(struct e1000_hw *hw);
+s32 e1000_init_thermal_sensor_thresh(struct e1000_hw *hw);
+
+
+
+/*
+ * TBI_ACCEPT macro definition:
+ *
+ * This macro requires:
+ *      adapter = a pointer to struct e1000_hw
+ *      status = the 8 bit status field of the Rx descriptor with EOP set
+ *      error = the 8 bit error field of the Rx descriptor with EOP set
+ *      length = the sum of all the length fields of the Rx descriptors that
+ *               make up the current frame
+ *      last_byte = the last byte of the frame DMAed by the hardware
+ *      max_frame_length = the maximum frame length we want to accept.
+ *      min_frame_length = the minimum frame length we want to accept.
+ *
+ * This macro is a conditional that should be used in the interrupt
+ * handler's Rx processing routine when RxErrors have been detected.
+ *
+ * Typical use:
+ *  ...
+ *  if (TBI_ACCEPT) {
+ *      accept_frame = true;
+ *      e1000_tbi_adjust_stats(adapter, MacAddress);
+ *      frame_length--;
+ *  } else {
+ *      accept_frame = false;
+ *  }
+ *  ...
+ */
+
+/* The carrier extension symbol, as received by the NIC. */
+#define CARRIER_EXTENSION   0x0F
+
+#define TBI_ACCEPT(a, status, errors, length, last_byte, \
+		   min_frame_size, max_frame_size) \
+	(e1000_tbi_sbp_enabled_82543(a) && \
+	 (((errors) & E1000_RXD_ERR_FRAME_ERR_MASK) == E1000_RXD_ERR_CE) && \
+	 ((last_byte) == CARRIER_EXTENSION) && \
+	 (((status) & E1000_RXD_STAT_VP) ? \
+	  (((length) > (min_frame_size - VLAN_TAG_SIZE)) && \
+	  ((length) <= (max_frame_size + 1))) : \
+	  (((length) > min_frame_size) && \
+	  ((length) <= (max_frame_size + VLAN_TAG_SIZE + 1)))))
+
+#ifndef E1000_MAX
+#define E1000_MAX(a, b) ((a) > (b) ? (a) : (b))
+#endif
+#ifndef E1000_DIVIDE_ROUND_UP
+#define E1000_DIVIDE_ROUND_UP(a, b)	(((a) + (b) - 1) / (b)) /* ceil(a/b) */
+#endif
+#endif /* _E1000_API_H_ */
diff --git a/drivers/net/igb/e1000_defines.h b/drivers/net/igb/e1000_defines.h
index 7b8ddd830f19..e6c920938b16 100644
--- a/drivers/net/igb/e1000_defines.h
+++ b/drivers/net/igb/e1000_defines.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2007-2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -34,347 +34,496 @@
 
 /* Definitions for power management and wakeup registers */
 /* Wake Up Control */
-#define E1000_WUC_PME_EN     0x00000002 /* PME Enable */
+#define E1000_WUC_APME		0x00000001 /* APM Enable */
+#define E1000_WUC_PME_EN	0x00000002 /* PME Enable */
+#define E1000_WUC_PME_STATUS	0x00000004 /* PME Status */
+#define E1000_WUC_APMPME	0x00000008 /* Assert PME on APM Wakeup */
+#define E1000_WUC_PHY_WAKE	0x00000100 /* if PHY supports wakeup */
 
 /* Wake Up Filter Control */
-#define E1000_WUFC_LNKC 0x00000001 /* Link Status Change Wakeup Enable */
-#define E1000_WUFC_MAG  0x00000002 /* Magic Packet Wakeup Enable */
-#define E1000_WUFC_EX   0x00000004 /* Directed Exact Wakeup Enable */
-#define E1000_WUFC_MC   0x00000008 /* Directed Multicast Wakeup Enable */
-#define E1000_WUFC_BC   0x00000010 /* Broadcast Wakeup Enable */
+#define E1000_WUFC_LNKC	0x00000001 /* Link Status Change Wakeup Enable */
+#define E1000_WUFC_MAG	0x00000002 /* Magic Packet Wakeup Enable */
+#define E1000_WUFC_EX	0x00000004 /* Directed Exact Wakeup Enable */
+#define E1000_WUFC_MC	0x00000008 /* Directed Multicast Wakeup Enable */
+#define E1000_WUFC_BC	0x00000010 /* Broadcast Wakeup Enable */
+#define E1000_WUFC_ARP	0x00000020 /* ARP Request Packet Wakeup Enable */
+#define E1000_WUFC_IPV4	0x00000040 /* Directed IPv4 Packet Wakeup Enable */
+#define E1000_WUFC_FLX0		0x00010000 /* Flexible Filter 0 Enable */
+
+/* Wake Up Status */
+#define E1000_WUS_LNKC		E1000_WUFC_LNKC
+#define E1000_WUS_MAG		E1000_WUFC_MAG
+#define E1000_WUS_EX		E1000_WUFC_EX
+#define E1000_WUS_MC		E1000_WUFC_MC
+#define E1000_WUS_BC		E1000_WUFC_BC
 
 /* Extended Device Control */
-#define E1000_CTRL_EXT_SDP3_DATA 0x00000080 /* Value of SW Defineable Pin 3 */
+#define E1000_CTRL_EXT_SDP4_DATA	0x00000010 /* SW Definable Pin 4 data */
+#define E1000_CTRL_EXT_SDP6_DATA	0x00000040 /* SW Definable Pin 6 data */
+#define E1000_CTRL_EXT_SDP3_DATA	0x00000080 /* SW Definable Pin 3 data */
+#define E1000_CTRL_EXT_SDP6_DIR	0x00000400 /* Direction of SDP6 0=in 1=out */
+#define E1000_CTRL_EXT_SDP3_DIR	0x00000800 /* Direction of SDP3 0=in 1=out */
+#define E1000_CTRL_EXT_EE_RST	0x00002000 /* Reinitialize from EEPROM */
 /* Physical Func Reset Done Indication */
-#define E1000_CTRL_EXT_PFRSTD    0x00004000
-#define E1000_CTRL_EXT_LINK_MODE_MASK 0x00C00000
-#define E1000_CTRL_EXT_LINK_MODE_PCIE_SERDES  0x00C00000
-#define E1000_CTRL_EXT_LINK_MODE_1000BASE_KX  0x00400000
-#define E1000_CTRL_EXT_LINK_MODE_SGMII   0x00800000
-#define E1000_CTRL_EXT_LINK_MODE_GMII   0x00000000
-#define E1000_CTRL_EXT_EIAME          0x01000000
-#define E1000_CTRL_EXT_IRCA           0x00000001
-/* Interrupt delay cancellation */
-/* Driver loaded bit for FW */
-#define E1000_CTRL_EXT_DRV_LOAD       0x10000000
-/* Interrupt acknowledge Auto-mask */
-/* Clear Interrupt timers after IMS clear */
-/* packet buffer parity error detection enabled */
-/* descriptor FIFO parity error detection enable */
-#define E1000_CTRL_EXT_PBA_CLR        0x80000000 /* PBA Clear */
-#define E1000_I2CCMD_REG_ADDR_SHIFT   16
-#define E1000_I2CCMD_PHY_ADDR_SHIFT   24
-#define E1000_I2CCMD_OPCODE_READ      0x08000000
-#define E1000_I2CCMD_OPCODE_WRITE     0x00000000
-#define E1000_I2CCMD_READY            0x20000000
-#define E1000_I2CCMD_ERROR            0x80000000
-#define E1000_MAX_SGMII_PHY_REG_ADDR  255
-#define E1000_I2CCMD_PHY_TIMEOUT      200
-#define E1000_IVAR_VALID              0x80
-#define E1000_GPIE_NSICR              0x00000001
-#define E1000_GPIE_MSIX_MODE          0x00000010
-#define E1000_GPIE_EIAME              0x40000000
-#define E1000_GPIE_PBA                0x80000000
+#define E1000_CTRL_EXT_PFRSTD	0x00004000
+#define E1000_CTRL_EXT_SPD_BYPS	0x00008000 /* Speed Select Bypass */
+#define E1000_CTRL_EXT_RO_DIS	0x00020000 /* Relaxed Ordering disable */
+#define E1000_CTRL_EXT_DMA_DYN_CLK_EN	0x00080000 /* DMA Dynamic Clk Gating */
+#define E1000_CTRL_EXT_LINK_MODE_MASK	0x00C00000
+/* Offset of the link mode field in Ctrl Ext register */
+#define E1000_CTRL_EXT_LINK_MODE_OFFSET	22
+#define E1000_CTRL_EXT_LINK_MODE_1000BASE_KX	0x00400000
+#define E1000_CTRL_EXT_LINK_MODE_GMII	0x00000000
+#define E1000_CTRL_EXT_LINK_MODE_PCIE_SERDES	0x00C00000
+#define E1000_CTRL_EXT_LINK_MODE_SGMII	0x00800000
+#define E1000_CTRL_EXT_EIAME		0x01000000
+#define E1000_CTRL_EXT_IRCA		0x00000001
+#define E1000_CTRL_EXT_DRV_LOAD		0x10000000 /* Drv loaded bit for FW */
+#define E1000_CTRL_EXT_IAME		0x08000000 /* Int ACK Auto-mask */
+#define E1000_CTRL_EXT_PBA_CLR		0x80000000 /* PBA Clear */
+#define E1000_I2CCMD_REG_ADDR_SHIFT	16
+#define E1000_I2CCMD_PHY_ADDR_SHIFT	24
+#define E1000_I2CCMD_OPCODE_READ	0x08000000
+#define E1000_I2CCMD_OPCODE_WRITE	0x00000000
+#define E1000_I2CCMD_READY		0x20000000
+#define E1000_I2CCMD_ERROR		0x80000000
+#define E1000_I2CCMD_SFP_DATA_ADDR(a)	(0x0000 + (a))
+#define E1000_I2CCMD_SFP_DIAG_ADDR(a)	(0x0100 + (a))
+#define E1000_MAX_SGMII_PHY_REG_ADDR	255
+#define E1000_I2CCMD_PHY_TIMEOUT	200
+#define E1000_IVAR_VALID	0x80
+#define E1000_GPIE_NSICR	0x00000001
+#define E1000_GPIE_MSIX_MODE	0x00000010
+#define E1000_GPIE_EIAME	0x40000000
+#define E1000_GPIE_PBA		0x80000000
 
 /* Receive Descriptor bit definitions */
-#define E1000_RXD_STAT_DD       0x01    /* Descriptor Done */
-#define E1000_RXD_STAT_EOP      0x02    /* End of Packet */
-#define E1000_RXD_STAT_IXSM     0x04    /* Ignore checksum */
-#define E1000_RXD_STAT_VP       0x08    /* IEEE VLAN Packet */
-#define E1000_RXD_STAT_UDPCS    0x10    /* UDP xsum calculated */
-#define E1000_RXD_STAT_TCPCS    0x20    /* TCP xsum calculated */
-#define E1000_RXD_STAT_TS       0x10000 /* Pkt was time stamped */
-
-#define E1000_RXDEXT_STATERR_CE    0x01000000
-#define E1000_RXDEXT_STATERR_SE    0x02000000
-#define E1000_RXDEXT_STATERR_SEQ   0x04000000
-#define E1000_RXDEXT_STATERR_CXE   0x10000000
-#define E1000_RXDEXT_STATERR_TCPE  0x20000000
-#define E1000_RXDEXT_STATERR_IPE   0x40000000
-#define E1000_RXDEXT_STATERR_RXE   0x80000000
+#define E1000_RXD_STAT_DD	0x01    /* Descriptor Done */
+#define E1000_RXD_STAT_EOP	0x02    /* End of Packet */
+#define E1000_RXD_STAT_IXSM	0x04    /* Ignore checksum */
+#define E1000_RXD_STAT_VP	0x08    /* IEEE VLAN Packet */
+#define E1000_RXD_STAT_UDPCS	0x10    /* UDP xsum calculated */
+#define E1000_RXD_STAT_TCPCS	0x20    /* TCP xsum calculated */
+#define E1000_RXD_STAT_IPCS	0x40    /* IP xsum calculated */
+#define E1000_RXD_STAT_PIF	0x80    /* passed in-exact filter */
+#define E1000_RXD_STAT_IPIDV	0x200   /* IP identification valid */
+#define E1000_RXD_STAT_UDPV	0x400   /* Valid UDP checksum */
+#define E1000_RXD_STAT_DYNINT	0x800   /* Pkt caused INT via DYNINT */
+#define E1000_RXD_ERR_CE	0x01    /* CRC Error */
+#define E1000_RXD_ERR_SE	0x02    /* Symbol Error */
+#define E1000_RXD_ERR_SEQ	0x04    /* Sequence Error */
+#define E1000_RXD_ERR_CXE	0x10    /* Carrier Extension Error */
+#define E1000_RXD_ERR_TCPE	0x20    /* TCP/UDP Checksum Error */
+#define E1000_RXD_ERR_IPE	0x40    /* IP Checksum Error */
+#define E1000_RXD_ERR_RXE	0x80    /* Rx Data Error */
+#define E1000_RXD_SPC_VLAN_MASK	0x0FFF  /* VLAN ID is in lower 12 bits */
+
+#define E1000_RXDEXT_STATERR_TST	0x00000100 /* Time Stamp taken */
+#define E1000_RXDEXT_STATERR_LB		0x00040000
+#define E1000_RXDEXT_STATERR_CE		0x01000000
+#define E1000_RXDEXT_STATERR_SE		0x02000000
+#define E1000_RXDEXT_STATERR_SEQ	0x04000000
+#define E1000_RXDEXT_STATERR_CXE	0x10000000
+#define E1000_RXDEXT_STATERR_TCPE	0x20000000
+#define E1000_RXDEXT_STATERR_IPE	0x40000000
+#define E1000_RXDEXT_STATERR_RXE	0x80000000
+
+/* mask to determine if packets should be dropped due to frame errors */
+#define E1000_RXD_ERR_FRAME_ERR_MASK ( \
+	E1000_RXD_ERR_CE  |		\
+	E1000_RXD_ERR_SE  |		\
+	E1000_RXD_ERR_SEQ |		\
+	E1000_RXD_ERR_CXE |		\
+	E1000_RXD_ERR_RXE)
 
 /* Same mask, but for extended and packet split descriptors */
 #define E1000_RXDEXT_ERR_FRAME_ERR_MASK ( \
-    E1000_RXDEXT_STATERR_CE  |            \
-    E1000_RXDEXT_STATERR_SE  |            \
-    E1000_RXDEXT_STATERR_SEQ |            \
-    E1000_RXDEXT_STATERR_CXE |            \
-    E1000_RXDEXT_STATERR_RXE)
+	E1000_RXDEXT_STATERR_CE  |	\
+	E1000_RXDEXT_STATERR_SE  |	\
+	E1000_RXDEXT_STATERR_SEQ |	\
+	E1000_RXDEXT_STATERR_CXE |	\
+	E1000_RXDEXT_STATERR_RXE)
 
-#define E1000_MRQC_RSS_FIELD_IPV4_TCP          0x00010000
-#define E1000_MRQC_RSS_FIELD_IPV4              0x00020000
-#define E1000_MRQC_RSS_FIELD_IPV6_TCP_EX       0x00040000
-#define E1000_MRQC_RSS_FIELD_IPV6              0x00100000
-#define E1000_MRQC_RSS_FIELD_IPV6_TCP          0x00200000
+#define E1000_MRQC_RSS_FIELD_MASK		0xFFFF0000
+#define E1000_MRQC_RSS_FIELD_IPV4_TCP		0x00010000
+#define E1000_MRQC_RSS_FIELD_IPV4		0x00020000
+#define E1000_MRQC_RSS_FIELD_IPV6_TCP_EX	0x00040000
+#define E1000_MRQC_RSS_FIELD_IPV6		0x00100000
+#define E1000_MRQC_RSS_FIELD_IPV6_TCP		0x00200000
 
+#define E1000_RXDPS_HDRSTAT_HDRSP		0x00008000
 
 /* Management Control */
-#define E1000_MANC_SMBUS_EN      0x00000001 /* SMBus Enabled - RO */
-#define E1000_MANC_ASF_EN        0x00000002 /* ASF Enabled - RO */
-#define E1000_MANC_EN_BMC2OS     0x10000000 /* OSBMC is Enabled or not */
-/* Enable Neighbor Discovery Filtering */
-#define E1000_MANC_RCV_TCO_EN    0x00020000 /* Receive TCO Packets Enabled */
-#define E1000_MANC_BLK_PHY_RST_ON_IDE   0x00040000 /* Block phy resets */
+#define E1000_MANC_SMBUS_EN	0x00000001 /* SMBus Enabled - RO */
+#define E1000_MANC_ASF_EN	0x00000002 /* ASF Enabled - RO */
+#define E1000_MANC_ARP_EN	0x00002000 /* Enable ARP Request Filtering */
+#define E1000_MANC_RCV_TCO_EN	0x00020000 /* Receive TCO Packets Enabled */
+#define E1000_MANC_BLK_PHY_RST_ON_IDE	0x00040000 /* Block phy resets */
 /* Enable MAC address filtering */
-#define E1000_MANC_EN_MAC_ADDR_FILTER   0x00100000
+#define E1000_MANC_EN_MAC_ADDR_FILTER	0x00100000
+/* Enable MNG packets to host memory */
+#define E1000_MANC_EN_MNG2HOST		0x00200000
+
+#define E1000_MANC2H_PORT_623		0x00000020 /* Port 0x26f */
+#define E1000_MANC2H_PORT_664		0x00000040 /* Port 0x298 */
+#define E1000_MDEF_PORT_623		0x00000800 /* Port 0x26f */
+#define E1000_MDEF_PORT_664		0x00000400 /* Port 0x298 */
 
 /* Receive Control */
-#define E1000_RCTL_EN             0x00000002    /* enable */
-#define E1000_RCTL_SBP            0x00000004    /* store bad packet */
-#define E1000_RCTL_UPE            0x00000008    /* unicast promiscuous enable */
-#define E1000_RCTL_MPE            0x00000010    /* multicast promiscuous enab */
-#define E1000_RCTL_LPE            0x00000020    /* long packet enable */
-#define E1000_RCTL_LBM_MAC        0x00000040    /* MAC loopback mode */
-#define E1000_RCTL_LBM_TCVR       0x000000C0    /* tcvr loopback mode */
-#define E1000_RCTL_RDMTS_HALF     0x00000000    /* rx desc min threshold size */
-#define E1000_RCTL_MO_SHIFT       12            /* multicast offset shift */
-#define E1000_RCTL_BAM            0x00008000    /* broadcast enable */
-#define E1000_RCTL_SZ_512         0x00020000    /* rx buffer size 512 */
-#define E1000_RCTL_SZ_256         0x00030000    /* rx buffer size 256 */
-#define E1000_RCTL_VFE            0x00040000    /* vlan filter enable */
-#define E1000_RCTL_CFIEN          0x00080000    /* canonical form enable */
-#define E1000_RCTL_SECRC          0x04000000    /* Strip Ethernet CRC */
-
-/*
- * Use byte values for the following shift parameters
+#define E1000_RCTL_RST		0x00000001 /* Software reset */
+#define E1000_RCTL_EN		0x00000002 /* enable */
+#define E1000_RCTL_SBP		0x00000004 /* store bad packet */
+#define E1000_RCTL_UPE		0x00000008 /* unicast promisc enable */
+#define E1000_RCTL_MPE		0x00000010 /* multicast promisc enable */
+#define E1000_RCTL_LPE		0x00000020 /* long packet enable */
+#define E1000_RCTL_LBM_NO	0x00000000 /* no loopback mode */
+#define E1000_RCTL_LBM_MAC	0x00000040 /* MAC loopback mode */
+#define E1000_RCTL_LBM_TCVR	0x000000C0 /* tcvr loopback mode */
+#define E1000_RCTL_DTYP_PS	0x00000400 /* Packet Split descriptor */
+#define E1000_RCTL_RDMTS_HALF	0x00000000 /* Rx desc min thresh size */
+#define E1000_RCTL_MO_SHIFT	12 /* multicast offset shift */
+#define E1000_RCTL_MO_3		0x00003000 /* multicast offset 15:4 */
+#define E1000_RCTL_BAM		0x00008000 /* broadcast enable */
+/* these buffer sizes are valid if E1000_RCTL_BSEX is 0 */
+#define E1000_RCTL_SZ_2048	0x00000000 /* Rx buffer size 2048 */
+#define E1000_RCTL_SZ_1024	0x00010000 /* Rx buffer size 1024 */
+#define E1000_RCTL_SZ_512	0x00020000 /* Rx buffer size 512 */
+#define E1000_RCTL_SZ_256	0x00030000 /* Rx buffer size 256 */
+/* these buffer sizes are valid if E1000_RCTL_BSEX is 1 */
+#define E1000_RCTL_SZ_16384	0x00010000 /* Rx buffer size 16384 */
+#define E1000_RCTL_SZ_8192	0x00020000 /* Rx buffer size 8192 */
+#define E1000_RCTL_SZ_4096	0x00030000 /* Rx buffer size 4096 */
+#define E1000_RCTL_VFE		0x00040000 /* vlan filter enable */
+#define E1000_RCTL_CFIEN	0x00080000 /* canonical form enable */
+#define E1000_RCTL_CFI		0x00100000 /* canonical form indicator */
+#define E1000_RCTL_DPF		0x00400000 /* discard pause frames */
+#define E1000_RCTL_PMCF		0x00800000 /* pass MAC control frames */
+#define E1000_RCTL_BSEX		0x02000000 /* Buffer size extension */
+#define E1000_RCTL_SECRC	0x04000000 /* Strip Ethernet CRC */
+
+/* Use byte values for the following shift parameters
  * Usage:
  *     psrctl |= (((ROUNDUP(value0, 128) >> E1000_PSRCTL_BSIZE0_SHIFT) &
- *                  E1000_PSRCTL_BSIZE0_MASK) |
- *                ((ROUNDUP(value1, 1024) >> E1000_PSRCTL_BSIZE1_SHIFT) &
- *                  E1000_PSRCTL_BSIZE1_MASK) |
- *                ((ROUNDUP(value2, 1024) << E1000_PSRCTL_BSIZE2_SHIFT) &
- *                  E1000_PSRCTL_BSIZE2_MASK) |
- *                ((ROUNDUP(value3, 1024) << E1000_PSRCTL_BSIZE3_SHIFT) |;
- *                  E1000_PSRCTL_BSIZE3_MASK))
+ *		  E1000_PSRCTL_BSIZE0_MASK) |
+ *		((ROUNDUP(value1, 1024) >> E1000_PSRCTL_BSIZE1_SHIFT) &
+ *		  E1000_PSRCTL_BSIZE1_MASK) |
+ *		((ROUNDUP(value2, 1024) << E1000_PSRCTL_BSIZE2_SHIFT) &
+ *		  E1000_PSRCTL_BSIZE2_MASK) |
+ *		((ROUNDUP(value3, 1024) << E1000_PSRCTL_BSIZE3_SHIFT) |;
+ *		  E1000_PSRCTL_BSIZE3_MASK))
  * where value0 = [128..16256],  default=256
  *       value1 = [1024..64512], default=4096
  *       value2 = [0..64512],    default=4096
  *       value3 = [0..64512],    default=0
  */
 
-#define E1000_PSRCTL_BSIZE0_MASK   0x0000007F
-#define E1000_PSRCTL_BSIZE1_MASK   0x00003F00
-#define E1000_PSRCTL_BSIZE2_MASK   0x003F0000
-#define E1000_PSRCTL_BSIZE3_MASK   0x3F000000
+#define E1000_PSRCTL_BSIZE0_MASK	0x0000007F
+#define E1000_PSRCTL_BSIZE1_MASK	0x00003F00
+#define E1000_PSRCTL_BSIZE2_MASK	0x003F0000
+#define E1000_PSRCTL_BSIZE3_MASK	0x3F000000
 
-#define E1000_PSRCTL_BSIZE0_SHIFT  7            /* Shift _right_ 7 */
-#define E1000_PSRCTL_BSIZE1_SHIFT  2            /* Shift _right_ 2 */
-#define E1000_PSRCTL_BSIZE2_SHIFT  6            /* Shift _left_ 6 */
-#define E1000_PSRCTL_BSIZE3_SHIFT 14            /* Shift _left_ 14 */
+#define E1000_PSRCTL_BSIZE0_SHIFT	7    /* Shift _right_ 7 */
+#define E1000_PSRCTL_BSIZE1_SHIFT	2    /* Shift _right_ 2 */
+#define E1000_PSRCTL_BSIZE2_SHIFT	6    /* Shift _left_ 6 */
+#define E1000_PSRCTL_BSIZE3_SHIFT	14   /* Shift _left_ 14 */
 
 /* SWFW_SYNC Definitions */
-#define E1000_SWFW_EEP_SM   0x1
-#define E1000_SWFW_PHY0_SM  0x2
-#define E1000_SWFW_PHY1_SM  0x4
-#define E1000_SWFW_PHY2_SM  0x20
-#define E1000_SWFW_PHY3_SM  0x40
+#define E1000_SWFW_EEP_SM	0x01
+#define E1000_SWFW_PHY0_SM	0x02
+#define E1000_SWFW_PHY1_SM	0x04
+#define E1000_SWFW_CSR_SM	0x08
+#define E1000_SWFW_PHY2_SM	0x20
+#define E1000_SWFW_PHY3_SM	0x40
+#define E1000_SWFW_SW_MNG_SM	0x400
 
-/* FACTPS Definitions */
 /* Device Control */
-#define E1000_CTRL_FD       0x00000001  /* Full duplex.0=half; 1=full */
-#define E1000_CTRL_GIO_MASTER_DISABLE 0x00000004 /*Blocks new Master requests */
-#define E1000_CTRL_LRST     0x00000008  /* Link reset. 0=normal,1=reset */
-#define E1000_CTRL_ASDE     0x00000020  /* Auto-speed detect enable */
-#define E1000_CTRL_SLU      0x00000040  /* Set link up (Force Link) */
-#define E1000_CTRL_ILOS     0x00000080  /* Invert Loss-Of Signal */
-#define E1000_CTRL_SPD_SEL  0x00000300  /* Speed Select Mask */
-#define E1000_CTRL_SPD_100  0x00000100  /* Force 100Mb */
-#define E1000_CTRL_SPD_1000 0x00000200  /* Force 1Gb */
-#define E1000_CTRL_FRCSPD   0x00000800  /* Force Speed */
-#define E1000_CTRL_FRCDPX   0x00001000  /* Force Duplex */
-/* Defined polarity of Dock/Undock indication in SDP[0] */
-/* Reset both PHY ports, through PHYRST_N pin */
-/* enable link status from external LINK_0 and LINK_1 pins */
-#define E1000_CTRL_SWDPIN0  0x00040000  /* SWDPIN 0 value */
-#define E1000_CTRL_SWDPIN1  0x00080000  /* SWDPIN 1 value */
-#define E1000_CTRL_SWDPIO0  0x00400000  /* SWDPIN 0 Input or output */
-#define E1000_CTRL_RST      0x04000000  /* Global reset */
-#define E1000_CTRL_RFCE     0x08000000  /* Receive Flow Control enable */
-#define E1000_CTRL_TFCE     0x10000000  /* Transmit flow control enable */
-#define E1000_CTRL_VME      0x40000000  /* IEEE VLAN mode enable */
-#define E1000_CTRL_PHY_RST  0x80000000  /* PHY Reset */
-/* Initiate an interrupt to manageability engine */
-#define E1000_CTRL_I2C_ENA  0x02000000  /* I2C enable */
-
-/* Bit definitions for the Management Data IO (MDIO) and Management Data
- * Clock (MDC) pins in the Device Control Register.
- */
-
-#define E1000_CONNSW_ENRGSRC             0x4
-#define E1000_PCS_CFG_PCS_EN             8
-#define E1000_PCS_LCTL_FLV_LINK_UP       1
-#define E1000_PCS_LCTL_FSV_100           2
-#define E1000_PCS_LCTL_FSV_1000          4
-#define E1000_PCS_LCTL_FDV_FULL          8
-#define E1000_PCS_LCTL_FSD               0x10
-#define E1000_PCS_LCTL_FORCE_LINK        0x20
-#define E1000_PCS_LCTL_FORCE_FCTRL       0x80
-#define E1000_PCS_LCTL_AN_ENABLE         0x10000
-#define E1000_PCS_LCTL_AN_RESTART        0x20000
-#define E1000_PCS_LCTL_AN_TIMEOUT        0x40000
-#define E1000_ENABLE_SERDES_LOOPBACK     0x0410
-
-#define E1000_PCS_LSTS_LINK_OK           1
-#define E1000_PCS_LSTS_SPEED_100         2
-#define E1000_PCS_LSTS_SPEED_1000        4
-#define E1000_PCS_LSTS_DUPLEX_FULL       8
-#define E1000_PCS_LSTS_SYNK_OK           0x10
+#define E1000_CTRL_FD		0x00000001  /* Full duplex.0=half; 1=full */
+#define E1000_CTRL_PRIOR	0x00000004  /* Priority on PCI. 0=rx,1=fair */
+#define E1000_CTRL_GIO_MASTER_DISABLE 0x00000004 /*Blocks new Master reqs */
+#define E1000_CTRL_LRST		0x00000008  /* Link reset. 0=normal,1=reset */
+#define E1000_CTRL_ASDE		0x00000020  /* Auto-speed detect enable */
+#define E1000_CTRL_SLU		0x00000040  /* Set link up (Force Link) */
+#define E1000_CTRL_ILOS		0x00000080  /* Invert Loss-Of Signal */
+#define E1000_CTRL_SPD_SEL	0x00000300  /* Speed Select Mask */
+#define E1000_CTRL_SPD_10	0x00000000  /* Force 10Mb */
+#define E1000_CTRL_SPD_100	0x00000100  /* Force 100Mb */
+#define E1000_CTRL_SPD_1000	0x00000200  /* Force 1Gb */
+#define E1000_CTRL_FRCSPD	0x00000800  /* Force Speed */
+#define E1000_CTRL_FRCDPX	0x00001000  /* Force Duplex */
+#define E1000_CTRL_SWDPIN0	0x00040000 /* SWDPIN 0 value */
+#define E1000_CTRL_SWDPIN1	0x00080000 /* SWDPIN 1 value */
+#define E1000_CTRL_SWDPIN2	0x00100000 /* SWDPIN 2 value */
+#define E1000_CTRL_ADVD3WUC	0x00100000 /* D3 WUC */
+#define E1000_CTRL_SWDPIN3	0x00200000 /* SWDPIN 3 value */
+#define E1000_CTRL_SWDPIO0	0x00400000 /* SWDPIN 0 Input or output */
+#define E1000_CTRL_RST		0x04000000 /* Global reset */
+#define E1000_CTRL_RFCE		0x08000000 /* Receive Flow Control enable */
+#define E1000_CTRL_TFCE		0x10000000 /* Transmit flow control enable */
+#define E1000_CTRL_VME		0x40000000 /* IEEE VLAN mode enable */
+#define E1000_CTRL_PHY_RST	0x80000000 /* PHY Reset */
+#define E1000_CTRL_I2C_ENA	0x02000000 /* I2C enable */
+
+
+#define E1000_CONNSW_ENRGSRC		0x4
+#define E1000_CONNSW_PHYSD		0x400
+#define E1000_CONNSW_PHY_PDN		0x800
+#define E1000_CONNSW_SERDESD		0x200
+#define E1000_CONNSW_AUTOSENSE_CONF	0x2
+#define E1000_CONNSW_AUTOSENSE_EN	0x1
+#define E1000_PCS_CFG_PCS_EN		8
+#define E1000_PCS_LCTL_FLV_LINK_UP	1
+#define E1000_PCS_LCTL_FSV_10		0
+#define E1000_PCS_LCTL_FSV_100		2
+#define E1000_PCS_LCTL_FSV_1000		4
+#define E1000_PCS_LCTL_FDV_FULL		8
+#define E1000_PCS_LCTL_FSD		0x10
+#define E1000_PCS_LCTL_FORCE_LINK	0x20
+#define E1000_PCS_LCTL_FORCE_FCTRL	0x80
+#define E1000_PCS_LCTL_AN_ENABLE	0x10000
+#define E1000_PCS_LCTL_AN_RESTART	0x20000
+#define E1000_PCS_LCTL_AN_TIMEOUT	0x40000
+#define E1000_ENABLE_SERDES_LOOPBACK	0x0410
+
+#define E1000_PCS_LSTS_LINK_OK		1
+#define E1000_PCS_LSTS_SPEED_100	2
+#define E1000_PCS_LSTS_SPEED_1000	4
+#define E1000_PCS_LSTS_DUPLEX_FULL	8
+#define E1000_PCS_LSTS_SYNK_OK		0x10
+#define E1000_PCS_LSTS_AN_COMPLETE	0x10000
 
 /* Device Status */
-#define E1000_STATUS_FD         0x00000001      /* Full duplex.0=half,1=full */
-#define E1000_STATUS_LU         0x00000002      /* Link up.0=no,1=link */
-#define E1000_STATUS_FUNC_MASK  0x0000000C      /* PCI Function Mask */
-#define E1000_STATUS_FUNC_SHIFT 2
-#define E1000_STATUS_FUNC_1     0x00000004      /* Function 1 */
-#define E1000_STATUS_TXOFF      0x00000010      /* transmission paused */
-#define E1000_STATUS_SPEED_100  0x00000040      /* Speed 100Mb/s */
-#define E1000_STATUS_SPEED_1000 0x00000080      /* Speed 1000Mb/s */
-/* Change in Dock/Undock state. Clear on write '0'. */
-/* Status of Master requests. */
-#define E1000_STATUS_GIO_MASTER_ENABLE 0x00080000
-/* BMC external code execution disabled */
-
-/* Constants used to intrepret the masked PCI-X bus speed. */
-
-#define SPEED_10    10
-#define SPEED_100   100
-#define SPEED_1000  1000
-#define HALF_DUPLEX 1
-#define FULL_DUPLEX 2
-
-
-#define ADVERTISE_10_HALF                 0x0001
-#define ADVERTISE_10_FULL                 0x0002
-#define ADVERTISE_100_HALF                0x0004
-#define ADVERTISE_100_FULL                0x0008
-#define ADVERTISE_1000_HALF               0x0010 /* Not used, just FYI */
-#define ADVERTISE_1000_FULL               0x0020
+#define E1000_STATUS_FD			0x00000001 /* Duplex 0=half 1=full */
+#define E1000_STATUS_LU			0x00000002 /* Link up.0=no,1=link */
+#define E1000_STATUS_FUNC_MASK		0x0000000C /* PCI Function Mask */
+#define E1000_STATUS_FUNC_SHIFT		2
+#define E1000_STATUS_FUNC_1		0x00000004 /* Function 1 */
+#define E1000_STATUS_TXOFF		0x00000010 /* transmission paused */
+#define E1000_STATUS_SPEED_MASK	0x000000C0
+#define E1000_STATUS_SPEED_10		0x00000000 /* Speed 10Mb/s */
+#define E1000_STATUS_SPEED_100		0x00000040 /* Speed 100Mb/s */
+#define E1000_STATUS_SPEED_1000		0x00000080 /* Speed 1000Mb/s */
+#define E1000_STATUS_LAN_INIT_DONE	0x00000200 /* Lan Init Compltn by NVM */
+#define E1000_STATUS_PHYRA		0x00000400 /* PHY Reset Asserted */
+#define E1000_STATUS_GIO_MASTER_ENABLE	0x00080000 /* Master request status */
+#define E1000_STATUS_2P5_SKU		0x00001000 /* Val of 2.5GBE SKU strap */
+#define E1000_STATUS_2P5_SKU_OVER	0x00002000 /* Val of 2.5GBE SKU Over */
+
+#define SPEED_10	10
+#define SPEED_100	100
+#define SPEED_1000	1000
+#define SPEED_2500	2500
+#define HALF_DUPLEX	1
+#define FULL_DUPLEX	2
+
+
+#define ADVERTISE_10_HALF		0x0001
+#define ADVERTISE_10_FULL		0x0002
+#define ADVERTISE_100_HALF		0x0004
+#define ADVERTISE_100_FULL		0x0008
+#define ADVERTISE_1000_HALF		0x0010 /* Not used, just FYI */
+#define ADVERTISE_1000_FULL		0x0020
 
 /* 1000/H is not supported, nor spec-compliant. */
-#define E1000_ALL_SPEED_DUPLEX (ADVERTISE_10_HALF  |  ADVERTISE_10_FULL | \
-				ADVERTISE_100_HALF |  ADVERTISE_100_FULL | \
-						      ADVERTISE_1000_FULL)
-#define E1000_ALL_NOT_GIG      (ADVERTISE_10_HALF  |  ADVERTISE_10_FULL | \
-				ADVERTISE_100_HALF |  ADVERTISE_100_FULL)
-#define E1000_ALL_100_SPEED    (ADVERTISE_100_HALF |  ADVERTISE_100_FULL)
-#define E1000_ALL_10_SPEED     (ADVERTISE_10_HALF  |  ADVERTISE_10_FULL)
-#define E1000_ALL_FULL_DUPLEX  (ADVERTISE_10_FULL  |  ADVERTISE_100_FULL | \
-						      ADVERTISE_1000_FULL)
-#define E1000_ALL_HALF_DUPLEX  (ADVERTISE_10_HALF  |  ADVERTISE_100_HALF)
-
-#define AUTONEG_ADVERTISE_SPEED_DEFAULT   E1000_ALL_SPEED_DUPLEX
+#define E1000_ALL_SPEED_DUPLEX	( \
+	ADVERTISE_10_HALF | ADVERTISE_10_FULL | ADVERTISE_100_HALF | \
+	ADVERTISE_100_FULL | ADVERTISE_1000_FULL)
+#define E1000_ALL_NOT_GIG	( \
+	ADVERTISE_10_HALF | ADVERTISE_10_FULL | ADVERTISE_100_HALF | \
+	ADVERTISE_100_FULL)
+#define E1000_ALL_100_SPEED	(ADVERTISE_100_HALF | ADVERTISE_100_FULL)
+#define E1000_ALL_10_SPEED	(ADVERTISE_10_HALF | ADVERTISE_10_FULL)
+#define E1000_ALL_HALF_DUPLEX	(ADVERTISE_10_HALF | ADVERTISE_100_HALF)
+
+#define AUTONEG_ADVERTISE_SPEED_DEFAULT		E1000_ALL_SPEED_DUPLEX
 
 /* LED Control */
-#define E1000_LEDCTL_LED0_MODE_SHIFT      0
-#define E1000_LEDCTL_LED0_BLINK           0x00000080
+#define E1000_LEDCTL_LED0_MODE_MASK	0x0000000F
+#define E1000_LEDCTL_LED0_MODE_SHIFT	0
+#define E1000_LEDCTL_LED0_IVRT		0x00000040
+#define E1000_LEDCTL_LED0_BLINK		0x00000080
 
-#define E1000_LEDCTL_MODE_LED_ON        0xE
-#define E1000_LEDCTL_MODE_LED_OFF       0xF
+#define E1000_LEDCTL_MODE_LED_ON	0xE
+#define E1000_LEDCTL_MODE_LED_OFF	0xF
 
 /* Transmit Descriptor bit definitions */
-#define E1000_TXD_POPTS_IXSM 0x01       /* Insert IP checksum */
-#define E1000_TXD_POPTS_TXSM 0x02       /* Insert TCP/UDP checksum */
-#define E1000_TXD_CMD_EOP    0x01000000 /* End of Packet */
-#define E1000_TXD_CMD_IFCS   0x02000000 /* Insert FCS (Ethernet CRC) */
-#define E1000_TXD_CMD_RS     0x08000000 /* Report Status */
-#define E1000_TXD_CMD_DEXT   0x20000000 /* Descriptor extension (0 = legacy) */
-#define E1000_TXD_STAT_DD    0x00000001 /* Descriptor Done */
-/* Extended desc bits for Linksec and timesync */
+#define E1000_TXD_DTYP_D	0x00100000 /* Data Descriptor */
+#define E1000_TXD_DTYP_C	0x00000000 /* Context Descriptor */
+#define E1000_TXD_POPTS_IXSM	0x01       /* Insert IP checksum */
+#define E1000_TXD_POPTS_TXSM	0x02       /* Insert TCP/UDP checksum */
+#define E1000_TXD_CMD_EOP	0x01000000 /* End of Packet */
+#define E1000_TXD_CMD_IFCS	0x02000000 /* Insert FCS (Ethernet CRC) */
+#define E1000_TXD_CMD_IC	0x04000000 /* Insert Checksum */
+#define E1000_TXD_CMD_RS	0x08000000 /* Report Status */
+#define E1000_TXD_CMD_RPS	0x10000000 /* Report Packet Sent */
+#define E1000_TXD_CMD_DEXT	0x20000000 /* Desc extension (0 = legacy) */
+#define E1000_TXD_CMD_VLE	0x40000000 /* Add VLAN tag */
+#define E1000_TXD_CMD_IDE	0x80000000 /* Enable Tidv register */
+#define E1000_TXD_STAT_DD	0x00000001 /* Descriptor Done */
+#define E1000_TXD_STAT_EC	0x00000002 /* Excess Collisions */
+#define E1000_TXD_STAT_LC	0x00000004 /* Late Collisions */
+#define E1000_TXD_STAT_TU	0x00000008 /* Transmit underrun */
+#define E1000_TXD_CMD_TCP	0x01000000 /* TCP packet */
+#define E1000_TXD_CMD_IP	0x02000000 /* IP packet */
+#define E1000_TXD_CMD_TSE	0x04000000 /* TCP Seg enable */
+#define E1000_TXD_STAT_TC	0x00000004 /* Tx Underrun */
+#define E1000_TXD_EXTCMD_TSTAMP	0x00000010 /* IEEE1588 Timestamp packet */
 
 /* Transmit Control */
-#define E1000_TCTL_EN     0x00000002    /* enable tx */
-#define E1000_TCTL_PSP    0x00000008    /* pad short packets */
-#define E1000_TCTL_CT     0x00000ff0    /* collision threshold */
-#define E1000_TCTL_COLD   0x003ff000    /* collision distance */
-#define E1000_TCTL_RTLC   0x01000000    /* Re-transmit on late collision */
+#define E1000_TCTL_EN		0x00000002 /* enable Tx */
+#define E1000_TCTL_PSP		0x00000008 /* pad short packets */
+#define E1000_TCTL_CT		0x00000ff0 /* collision threshold */
+#define E1000_TCTL_COLD		0x003ff000 /* collision distance */
+#define E1000_TCTL_RTLC		0x01000000 /* Re-transmit on late collision */
+#define E1000_TCTL_MULR		0x10000000 /* Multiple request support */
 
-/* DMA Coalescing register fields */
-#define E1000_DMACR_DMACWT_MASK         0x00003FFF /* DMA Coalescing
-							* Watchdog Timer */
-#define E1000_DMACR_DMACTHR_MASK        0x00FF0000 /* DMA Coalescing Receive
-							* Threshold */
-#define E1000_DMACR_DMACTHR_SHIFT       16
-#define E1000_DMACR_DMAC_LX_MASK        0x30000000 /* Lx when no PCIe
-							* transactions */
-#define E1000_DMACR_DMAC_LX_SHIFT       28
-#define E1000_DMACR_DMAC_EN             0x80000000 /* Enable DMA Coalescing */
-
-#define E1000_DMCTXTH_DMCTTHR_MASK      0x00000FFF /* DMA Coalescing Transmit
-							* Threshold */
-
-#define E1000_DMCTLX_TTLX_MASK          0x00000FFF /* Time to LX request */
-
-#define E1000_DMCRTRH_UTRESH_MASK       0x0007FFFF /* Receive Traffic Rate
-							* Threshold */
-#define E1000_DMCRTRH_LRPRCW            0x80000000 /* Rcv packet rate in
-							* current window */
-
-#define E1000_DMCCNT_CCOUNT_MASK        0x01FFFFFF /* DMA Coal Rcv Traffic
-							* Current Cnt */
-
-#define E1000_FCRTC_RTH_COAL_MASK       0x0003FFF0 /* Flow ctrl Rcv Threshold
-							* High val */
-#define E1000_FCRTC_RTH_COAL_SHIFT      4
-#define E1000_PCIEMISC_LX_DECISION      0x00000080 /* Lx power decision */
+/* Transmit Arbitration Count */
+#define E1000_TARC0_ENABLE	0x00000400 /* Enable Tx Queue 0 */
 
 /* SerDes Control */
-#define E1000_SCTL_DISABLE_SERDES_LOOPBACK 0x0400
+#define E1000_SCTL_DISABLE_SERDES_LOOPBACK	0x0400
+#define E1000_SCTL_ENABLE_SERDES_LOOPBACK	0x0410
 
 /* Receive Checksum Control */
-#define E1000_RXCSUM_IPOFL     0x00000100   /* IPv4 checksum offload */
-#define E1000_RXCSUM_TUOFL     0x00000200   /* TCP / UDP checksum offload */
-#define E1000_RXCSUM_CRCOFL    0x00000800   /* CRC32 offload enable */
-#define E1000_RXCSUM_PCSD      0x00002000   /* packet checksum disabled */
+#define E1000_RXCSUM_IPOFL	0x00000100 /* IPv4 checksum offload */
+#define E1000_RXCSUM_TUOFL	0x00000200 /* TCP / UDP checksum offload */
+#define E1000_RXCSUM_CRCOFL	0x00000800 /* CRC32 offload enable */
+#define E1000_RXCSUM_IPPCSE	0x00001000 /* IP payload checksum enable */
+#define E1000_RXCSUM_PCSD	0x00002000 /* packet checksum disabled */
 
 /* Header split receive */
-#define E1000_RFCTL_LEF        0x00040000
+#define E1000_RFCTL_NFSW_DIS		0x00000040
+#define E1000_RFCTL_NFSR_DIS		0x00000080
+#define E1000_RFCTL_ACK_DIS		0x00001000
+#define E1000_RFCTL_EXTEN		0x00008000
+#define E1000_RFCTL_IPV6_EX_DIS		0x00010000
+#define E1000_RFCTL_NEW_IPV6_EXT_DIS	0x00020000
+#define E1000_RFCTL_LEF			0x00040000
 
 /* Collision related configuration parameters */
-#define E1000_COLLISION_THRESHOLD       15
-#define E1000_CT_SHIFT                  4
-#define E1000_COLLISION_DISTANCE        63
-#define E1000_COLD_SHIFT                12
+#define E1000_COLLISION_THRESHOLD	15
+#define E1000_CT_SHIFT			4
+#define E1000_COLLISION_DISTANCE	63
+#define E1000_COLD_SHIFT		12
+
+/* Default values for the transmit IPG register */
+#define DEFAULT_82543_TIPG_IPGT_FIBER	9
+#define DEFAULT_82543_TIPG_IPGT_COPPER	8
+
+#define E1000_TIPG_IPGT_MASK		0x000003FF
+
+#define DEFAULT_82543_TIPG_IPGR1	8
+#define E1000_TIPG_IPGR1_SHIFT		10
+
+#define DEFAULT_82543_TIPG_IPGR2	6
+#define DEFAULT_80003ES2LAN_TIPG_IPGR2	7
+#define E1000_TIPG_IPGR2_SHIFT		20
 
 /* Ethertype field values */
-#define ETHERNET_IEEE_VLAN_TYPE 0x8100  /* 802.3ac packet */
+#define ETHERNET_IEEE_VLAN_TYPE		0x8100  /* 802.3ac packet */
+
+#define ETHERNET_FCS_SIZE		4
+#define MAX_JUMBO_FRAME_SIZE		0x3F00
+
+/* Extended Configuration Control and Size */
+#define E1000_EXTCNF_CTRL_MDIO_SW_OWNERSHIP	0x00000020
+#define E1000_EXTCNF_CTRL_LCD_WRITE_ENABLE	0x00000001
+#define E1000_EXTCNF_CTRL_OEM_WRITE_ENABLE	0x00000008
+#define E1000_EXTCNF_CTRL_SWFLAG		0x00000020
+#define E1000_EXTCNF_CTRL_GATE_PHY_CFG		0x00000080
+#define E1000_EXTCNF_SIZE_EXT_PCIE_LENGTH_MASK	0x00FF0000
+#define E1000_EXTCNF_SIZE_EXT_PCIE_LENGTH_SHIFT	16
+#define E1000_EXTCNF_CTRL_EXT_CNF_POINTER_MASK	0x0FFF0000
+#define E1000_EXTCNF_CTRL_EXT_CNF_POINTER_SHIFT	16
 
-#define MAX_JUMBO_FRAME_SIZE    0x3F00
+#define E1000_PHY_CTRL_D0A_LPLU			0x00000002
+#define E1000_PHY_CTRL_NOND0A_LPLU		0x00000004
+#define E1000_PHY_CTRL_NOND0A_GBE_DISABLE	0x00000008
+#define E1000_PHY_CTRL_GBE_DISABLE		0x00000040
+
+#define E1000_KABGTXD_BGSQLBIAS			0x00050000
 
 /* PBA constants */
-#define E1000_PBA_34K 0x0022
-#define E1000_PBA_64K 0x0040    /* 64KB */
+#define E1000_PBA_8K		0x0008    /* 8KB */
+#define E1000_PBA_10K		0x000A    /* 10KB */
+#define E1000_PBA_12K		0x000C    /* 12KB */
+#define E1000_PBA_14K		0x000E    /* 14KB */
+#define E1000_PBA_16K		0x0010    /* 16KB */
+#define E1000_PBA_18K		0x0012
+#define E1000_PBA_20K		0x0014
+#define E1000_PBA_22K		0x0016
+#define E1000_PBA_24K		0x0018
+#define E1000_PBA_26K		0x001A
+#define E1000_PBA_30K		0x001E
+#define E1000_PBA_32K		0x0020
+#define E1000_PBA_34K		0x0022
+#define E1000_PBA_35K		0x0023
+#define E1000_PBA_38K		0x0026
+#define E1000_PBA_40K		0x0028
+#define E1000_PBA_48K		0x0030    /* 48KB */
+#define E1000_PBA_64K		0x0040    /* 64KB */
+
+#define E1000_PBA_RXA_MASK	0xFFFF
+
+#define E1000_PBS_16K		E1000_PBA_16K
+
+#define IFS_MAX			80
+#define IFS_MIN			40
+#define IFS_RATIO		4
+#define IFS_STEP		10
+#define MIN_NUM_XMITS		1000
 
 /* SW Semaphore Register */
-#define E1000_SWSM_SMBI         0x00000001 /* Driver Semaphore bit */
-#define E1000_SWSM_SWESMBI      0x00000002 /* FW Semaphore bit */
+#define E1000_SWSM_SMBI		0x00000001 /* Driver Semaphore bit */
+#define E1000_SWSM_SWESMBI	0x00000002 /* FW Semaphore bit */
+#define E1000_SWSM_DRV_LOAD	0x00000008 /* Driver Loaded Bit */
+
+#define E1000_SWSM2_LOCK	0x00000002 /* Secondary driver semaphore bit */
 
 /* Interrupt Cause Read */
-#define E1000_ICR_TXDW          0x00000001 /* Transmit desc written back */
-#define E1000_ICR_LSC           0x00000004 /* Link Status Change */
-#define E1000_ICR_RXSEQ         0x00000008 /* rx sequence error */
-#define E1000_ICR_RXDMT0        0x00000010 /* rx desc min. threshold (0) */
-#define E1000_ICR_RXT0          0x00000080 /* rx timer intr (ring 0) */
-#define E1000_ICR_VMMB          0x00000100 /* VM MB event */
-#define E1000_ICR_DRSTA         0x40000000 /* Device Reset Asserted */
+#define E1000_ICR_TXDW		0x00000001 /* Transmit desc written back */
+#define E1000_ICR_TXQE		0x00000002 /* Transmit Queue empty */
+#define E1000_ICR_LSC		0x00000004 /* Link Status Change */
+#define E1000_ICR_RXSEQ		0x00000008 /* Rx sequence error */
+#define E1000_ICR_RXDMT0	0x00000010 /* Rx desc min. threshold (0) */
+#define E1000_ICR_RXO		0x00000040 /* Rx overrun */
+#define E1000_ICR_RXT0		0x00000080 /* Rx timer intr (ring 0) */
+#define E1000_ICR_VMMB		0x00000100 /* VM MB event */
+#define E1000_ICR_RXCFG		0x00000400 /* Rx /c/ ordered set */
+#define E1000_ICR_GPI_EN0	0x00000800 /* GP Int 0 */
+#define E1000_ICR_GPI_EN1	0x00001000 /* GP Int 1 */
+#define E1000_ICR_GPI_EN2	0x00002000 /* GP Int 2 */
+#define E1000_ICR_GPI_EN3	0x00004000 /* GP Int 3 */
+#define E1000_ICR_TXD_LOW	0x00008000
+#define E1000_ICR_MNG		0x00040000 /* Manageability event */
+#define E1000_ICR_TS		0x00080000 /* Time Sync Interrupt */
+#define E1000_ICR_DRSTA		0x40000000 /* Device Reset Asserted */
 /* If this bit asserted, the driver should claim the interrupt */
-#define E1000_ICR_INT_ASSERTED  0x80000000
-/* LAN connected device generates an interrupt */
-#define E1000_ICR_DOUTSYNC      0x10000000 /* NIC DMA out of sync */
+#define E1000_ICR_INT_ASSERTED	0x80000000
+#define E1000_ICR_DOUTSYNC	0x10000000 /* NIC DMA out of sync */
+#define E1000_ICR_FER		0x00400000 /* Fatal Error */
+
+#define E1000_ICR_THS		0x00800000 /* ICR.THS: Thermal Sensor Event*/
+#define E1000_ICR_MDDET		0x10000000 /* Malicious Driver Detect */
+
 
 /* Extended Interrupt Cause Read */
-#define E1000_EICR_RX_QUEUE0    0x00000001 /* Rx Queue 0 Interrupt */
-#define E1000_EICR_RX_QUEUE1    0x00000002 /* Rx Queue 1 Interrupt */
-#define E1000_EICR_RX_QUEUE2    0x00000004 /* Rx Queue 2 Interrupt */
-#define E1000_EICR_RX_QUEUE3    0x00000008 /* Rx Queue 3 Interrupt */
-#define E1000_EICR_TX_QUEUE0    0x00000100 /* Tx Queue 0 Interrupt */
-#define E1000_EICR_TX_QUEUE1    0x00000200 /* Tx Queue 1 Interrupt */
-#define E1000_EICR_TX_QUEUE2    0x00000400 /* Tx Queue 2 Interrupt */
-#define E1000_EICR_TX_QUEUE3    0x00000800 /* Tx Queue 3 Interrupt */
-#define E1000_EICR_OTHER        0x80000000 /* Interrupt Cause Active */
+#define E1000_EICR_RX_QUEUE0	0x00000001 /* Rx Queue 0 Interrupt */
+#define E1000_EICR_RX_QUEUE1	0x00000002 /* Rx Queue 1 Interrupt */
+#define E1000_EICR_RX_QUEUE2	0x00000004 /* Rx Queue 2 Interrupt */
+#define E1000_EICR_RX_QUEUE3	0x00000008 /* Rx Queue 3 Interrupt */
+#define E1000_EICR_TX_QUEUE0	0x00000100 /* Tx Queue 0 Interrupt */
+#define E1000_EICR_TX_QUEUE1	0x00000200 /* Tx Queue 1 Interrupt */
+#define E1000_EICR_TX_QUEUE2	0x00000400 /* Tx Queue 2 Interrupt */
+#define E1000_EICR_TX_QUEUE3	0x00000800 /* Tx Queue 3 Interrupt */
+#define E1000_EICR_TCP_TIMER	0x40000000 /* TCP Timer */
+#define E1000_EICR_OTHER	0x80000000 /* Interrupt Cause Active */
 /* TCP Timer */
+#define E1000_TCPTIMER_KS	0x00000100 /* KickStart */
+#define E1000_TCPTIMER_COUNT_ENABLE	0x00000200 /* Count Enable */
+#define E1000_TCPTIMER_COUNT_FINISH	0x00000400 /* Count finish */
+#define E1000_TCPTIMER_LOOP	0x00000800 /* Loop */
 
-/*
- * This defines the bits that are set in the Interrupt Mask
+/* This defines the bits that are set in the Interrupt Mask
  * Set/Read Register.  Each bit is documented below:
  *   o RXT0   = Receiver Timer Interrupt (ring 0)
  *   o TXDW   = Transmit Descriptor Written Back
@@ -383,446 +532,787 @@
  *   o LSC    = Link Status Change
  */
 #define IMS_ENABLE_MASK ( \
-    E1000_IMS_RXT0   |    \
-    E1000_IMS_TXDW   |    \
-    E1000_IMS_RXDMT0 |    \
-    E1000_IMS_RXSEQ  |    \
-    E1000_IMS_LSC    |    \
-    E1000_IMS_DOUTSYNC)
+	E1000_IMS_RXT0   |    \
+	E1000_IMS_TXDW   |    \
+	E1000_IMS_RXDMT0 |    \
+	E1000_IMS_RXSEQ  |    \
+	E1000_IMS_LSC)
 
 /* Interrupt Mask Set */
-#define E1000_IMS_TXDW      E1000_ICR_TXDW      /* Transmit desc written back */
-#define E1000_IMS_LSC       E1000_ICR_LSC       /* Link Status Change */
-#define E1000_IMS_VMMB      E1000_ICR_VMMB      /* Mail box activity */
-#define E1000_IMS_RXSEQ     E1000_ICR_RXSEQ     /* rx sequence error */
-#define E1000_IMS_RXDMT0    E1000_ICR_RXDMT0    /* rx desc min. threshold */
-#define E1000_IMS_RXT0      E1000_ICR_RXT0      /* rx timer intr */
-#define E1000_IMS_DRSTA     E1000_ICR_DRSTA     /* Device Reset Asserted */
-#define E1000_IMS_DOUTSYNC  E1000_ICR_DOUTSYNC /* NIC DMA out of sync */
-
+#define E1000_IMS_TXDW		E1000_ICR_TXDW    /* Tx desc written back */
+#define E1000_IMS_TXQE		E1000_ICR_TXQE    /* Transmit Queue empty */
+#define E1000_IMS_LSC		E1000_ICR_LSC     /* Link Status Change */
+#define E1000_IMS_VMMB		E1000_ICR_VMMB    /* Mail box activity */
+#define E1000_IMS_RXSEQ		E1000_ICR_RXSEQ   /* Rx sequence error */
+#define E1000_IMS_RXDMT0	E1000_ICR_RXDMT0  /* Rx desc min. threshold */
+#define E1000_IMS_RXO		E1000_ICR_RXO     /* Rx overrun */
+#define E1000_IMS_RXT0		E1000_ICR_RXT0    /* Rx timer intr */
+#define E1000_IMS_TXD_LOW	E1000_ICR_TXD_LOW
+#define E1000_IMS_TS		E1000_ICR_TS      /* Time Sync Interrupt */
+#define E1000_IMS_DRSTA		E1000_ICR_DRSTA   /* Device Reset Asserted */
+#define E1000_IMS_DOUTSYNC	E1000_ICR_DOUTSYNC /* NIC DMA out of sync */
+#define E1000_IMS_FER		E1000_ICR_FER /* Fatal Error */
+
+#define E1000_IMS_THS		E1000_ICR_THS /* ICR.TS: Thermal Sensor Event*/
+#define E1000_IMS_MDDET		E1000_ICR_MDDET /* Malicious Driver Detect */
 /* Extended Interrupt Mask Set */
-#define E1000_EIMS_OTHER        E1000_EICR_OTHER   /* Interrupt Cause Active */
+#define E1000_EIMS_RX_QUEUE0	E1000_EICR_RX_QUEUE0 /* Rx Queue 0 Interrupt */
+#define E1000_EIMS_RX_QUEUE1	E1000_EICR_RX_QUEUE1 /* Rx Queue 1 Interrupt */
+#define E1000_EIMS_RX_QUEUE2	E1000_EICR_RX_QUEUE2 /* Rx Queue 2 Interrupt */
+#define E1000_EIMS_RX_QUEUE3	E1000_EICR_RX_QUEUE3 /* Rx Queue 3 Interrupt */
+#define E1000_EIMS_TX_QUEUE0	E1000_EICR_TX_QUEUE0 /* Tx Queue 0 Interrupt */
+#define E1000_EIMS_TX_QUEUE1	E1000_EICR_TX_QUEUE1 /* Tx Queue 1 Interrupt */
+#define E1000_EIMS_TX_QUEUE2	E1000_EICR_TX_QUEUE2 /* Tx Queue 2 Interrupt */
+#define E1000_EIMS_TX_QUEUE3	E1000_EICR_TX_QUEUE3 /* Tx Queue 3 Interrupt */
+#define E1000_EIMS_TCP_TIMER	E1000_EICR_TCP_TIMER /* TCP Timer */
+#define E1000_EIMS_OTHER	E1000_EICR_OTHER   /* Interrupt Cause Active */
 
 /* Interrupt Cause Set */
-#define E1000_ICS_LSC       E1000_ICR_LSC       /* Link Status Change */
-#define E1000_ICS_RXDMT0    E1000_ICR_RXDMT0    /* rx desc min. threshold */
-#define E1000_ICS_DRSTA     E1000_ICR_DRSTA     /* Device Reset Aserted */
+#define E1000_ICS_LSC		E1000_ICR_LSC       /* Link Status Change */
+#define E1000_ICS_RXSEQ		E1000_ICR_RXSEQ     /* Rx sequence error */
+#define E1000_ICS_RXDMT0	E1000_ICR_RXDMT0    /* Rx desc min. threshold */
 
 /* Extended Interrupt Cause Set */
+#define E1000_EICS_RX_QUEUE0	E1000_EICR_RX_QUEUE0 /* Rx Queue 0 Interrupt */
+#define E1000_EICS_RX_QUEUE1	E1000_EICR_RX_QUEUE1 /* Rx Queue 1 Interrupt */
+#define E1000_EICS_RX_QUEUE2	E1000_EICR_RX_QUEUE2 /* Rx Queue 2 Interrupt */
+#define E1000_EICS_RX_QUEUE3	E1000_EICR_RX_QUEUE3 /* Rx Queue 3 Interrupt */
+#define E1000_EICS_TX_QUEUE0	E1000_EICR_TX_QUEUE0 /* Tx Queue 0 Interrupt */
+#define E1000_EICS_TX_QUEUE1	E1000_EICR_TX_QUEUE1 /* Tx Queue 1 Interrupt */
+#define E1000_EICS_TX_QUEUE2	E1000_EICR_TX_QUEUE2 /* Tx Queue 2 Interrupt */
+#define E1000_EICS_TX_QUEUE3	E1000_EICR_TX_QUEUE3 /* Tx Queue 3 Interrupt */
+#define E1000_EICS_TCP_TIMER	E1000_EICR_TCP_TIMER /* TCP Timer */
+#define E1000_EICS_OTHER	E1000_EICR_OTHER   /* Interrupt Cause Active */
+
+#define E1000_EITR_ITR_INT_MASK	0x0000FFFF
+/* E1000_EITR_CNT_IGNR is only for 82576 and newer */
+#define E1000_EITR_CNT_IGNR	0x80000000 /* Don't reset counters on write */
+#define E1000_EITR_INTERVAL 0x00007FFC
 
 /* Transmit Descriptor Control */
+#define E1000_TXDCTL_PTHRESH	0x0000003F /* TXDCTL Prefetch Threshold */
+#define E1000_TXDCTL_HTHRESH	0x00003F00 /* TXDCTL Host Threshold */
+#define E1000_TXDCTL_WTHRESH	0x003F0000 /* TXDCTL Writeback Threshold */
+#define E1000_TXDCTL_GRAN	0x01000000 /* TXDCTL Granularity */
+#define E1000_TXDCTL_FULL_TX_DESC_WB	0x01010000 /* GRAN=1, WTHRESH=1 */
+#define E1000_TXDCTL_MAX_TX_DESC_PREFETCH 0x0100001F /* GRAN=1, PTHRESH=31 */
 /* Enable the counting of descriptors still to be processed. */
+#define E1000_TXDCTL_COUNT_DESC	0x00400000
 
 /* Flow Control Constants */
-#define FLOW_CONTROL_ADDRESS_LOW  0x00C28001
-#define FLOW_CONTROL_ADDRESS_HIGH 0x00000100
-#define FLOW_CONTROL_TYPE         0x8808
+#define FLOW_CONTROL_ADDRESS_LOW	0x00C28001
+#define FLOW_CONTROL_ADDRESS_HIGH	0x00000100
+#define FLOW_CONTROL_TYPE		0x8808
 
 /* 802.1q VLAN Packet Size */
-#define VLAN_TAG_SIZE              4    /* 802.3ac tag (not DMA'd) */
-#define E1000_VLAN_FILTER_TBL_SIZE 128  /* VLAN Filter Table (4096 bits) */
+#define VLAN_TAG_SIZE			4    /* 802.3ac tag (not DMA'd) */
+#define E1000_VLAN_FILTER_TBL_SIZE	128  /* VLAN Filter Table (4096 bits) */
 
-/* Receive Address */
-/*
+/* Receive Address
  * Number of high/low register pairs in the RAR. The RAR (Receive Address
  * Registers) holds the directed and multicast addresses that we monitor.
  * Technically, we have 16 spots.  However, we reserve one of these spots
  * (RAR[15]) for our directed address used by controllers with
  * manageability enabled, allowing us room for 15 multicast addresses.
  */
-#define E1000_RAH_AV  0x80000000        /* Receive descriptor valid */
-#define E1000_RAL_MAC_ADDR_LEN 4
-#define E1000_RAH_MAC_ADDR_LEN 2
-#define E1000_RAH_POOL_MASK 0x03FC0000
-#define E1000_RAH_POOL_1 0x00040000
+#define E1000_RAR_ENTRIES	15
+#define E1000_RAH_AV		0x80000000 /* Receive descriptor valid */
+#define E1000_RAL_MAC_ADDR_LEN	4
+#define E1000_RAH_MAC_ADDR_LEN	2
+#define E1000_RAH_QUEUE_MASK_82575	0x000C0000
+#define E1000_RAH_POOL_1	0x00040000
 
 /* Error Codes */
-#define E1000_SUCCESS      0
-#define E1000_ERR_NVM      1
-#define E1000_ERR_PHY      2
-#define E1000_ERR_CONFIG   3
-#define E1000_ERR_PARAM    4
-#define E1000_ERR_MAC_INIT 5
-#define E1000_ERR_RESET   9
-#define E1000_ERR_MASTER_REQUESTS_PENDING 10
-#define E1000_BLK_PHY_RESET   12
-#define E1000_ERR_SWFW_SYNC 13
-#define E1000_NOT_IMPLEMENTED 14
-#define E1000_ERR_MBX      15
-#define E1000_ERR_INVALID_ARGUMENT  16
-#define E1000_ERR_NO_SPACE          17
-#define E1000_ERR_NVM_PBA_SECTION   18
+#define E1000_SUCCESS			0
+#define E1000_ERR_NVM			1
+#define E1000_ERR_PHY			2
+#define E1000_ERR_CONFIG		3
+#define E1000_ERR_PARAM			4
+#define E1000_ERR_MAC_INIT		5
+#define E1000_ERR_PHY_TYPE		6
+#define E1000_ERR_RESET			9
+#define E1000_ERR_MASTER_REQUESTS_PENDING	10
+#define E1000_ERR_HOST_INTERFACE_COMMAND	11
+#define E1000_BLK_PHY_RESET		12
+#define E1000_ERR_SWFW_SYNC		13
+#define E1000_NOT_IMPLEMENTED		14
+#define E1000_ERR_MBX			15
+#define E1000_ERR_INVALID_ARGUMENT	16
+#define E1000_ERR_NO_SPACE		17
+#define E1000_ERR_NVM_PBA_SECTION	18
+#define E1000_ERR_I2C			19
+#define E1000_ERR_INVM_VALUE_NOT_FOUND	20
 
 /* Loop limit on how long we wait for auto-negotiation to complete */
-#define COPPER_LINK_UP_LIMIT              10
-#define PHY_AUTO_NEG_LIMIT                45
-#define PHY_FORCE_LIMIT                   20
+#define FIBER_LINK_UP_LIMIT		50
+#define COPPER_LINK_UP_LIMIT		10
+#define PHY_AUTO_NEG_LIMIT		45
+#define PHY_FORCE_LIMIT			20
 /* Number of 100 microseconds we wait for PCI Express master disable */
-#define MASTER_DISABLE_TIMEOUT      800
+#define MASTER_DISABLE_TIMEOUT		800
 /* Number of milliseconds we wait for PHY configuration done after MAC reset */
-#define PHY_CFG_TIMEOUT             100
+#define PHY_CFG_TIMEOUT			100
 /* Number of 2 milliseconds we wait for acquiring MDIO ownership. */
+#define MDIO_OWNERSHIP_TIMEOUT		10
 /* Number of milliseconds for NVM auto read done after MAC reset. */
-#define AUTO_READ_DONE_TIMEOUT      10
+#define AUTO_READ_DONE_TIMEOUT		10
 
 /* Flow Control */
-#define E1000_FCRTL_XONE 0x80000000     /* Enable XON frame transmission */
-
-#define E1000_TSYNCTXCTL_VALID    0x00000001 /* tx timestamp valid */
-#define E1000_TSYNCTXCTL_ENABLED  0x00000010 /* enable tx timestampping */
-
-#define E1000_TSYNCRXCTL_VALID      0x00000001 /* rx timestamp valid */
-#define E1000_TSYNCRXCTL_TYPE_MASK  0x0000000E /* rx type mask */
-#define E1000_TSYNCRXCTL_TYPE_L2_V2       0x00
-#define E1000_TSYNCRXCTL_TYPE_L4_V1       0x02
-#define E1000_TSYNCRXCTL_TYPE_L2_L4_V2    0x04
-#define E1000_TSYNCRXCTL_TYPE_ALL         0x08
-#define E1000_TSYNCRXCTL_TYPE_EVENT_V2    0x0A
-#define E1000_TSYNCRXCTL_ENABLED    0x00000010 /* enable rx timestampping */
-
-#define E1000_TSYNCRXCFG_PTP_V1_CTRLT_MASK   0x000000FF
-#define E1000_TSYNCRXCFG_PTP_V1_SYNC_MESSAGE       0x00
-#define E1000_TSYNCRXCFG_PTP_V1_DELAY_REQ_MESSAGE  0x01
-#define E1000_TSYNCRXCFG_PTP_V1_FOLLOWUP_MESSAGE   0x02
-#define E1000_TSYNCRXCFG_PTP_V1_DELAY_RESP_MESSAGE 0x03
-#define E1000_TSYNCRXCFG_PTP_V1_MANAGEMENT_MESSAGE 0x04
-
-#define E1000_TSYNCRXCFG_PTP_V2_MSGID_MASK               0x00000F00
-#define E1000_TSYNCRXCFG_PTP_V2_SYNC_MESSAGE                 0x0000
-#define E1000_TSYNCRXCFG_PTP_V2_DELAY_REQ_MESSAGE            0x0100
-#define E1000_TSYNCRXCFG_PTP_V2_PATH_DELAY_REQ_MESSAGE       0x0200
-#define E1000_TSYNCRXCFG_PTP_V2_PATH_DELAY_RESP_MESSAGE      0x0300
-#define E1000_TSYNCRXCFG_PTP_V2_FOLLOWUP_MESSAGE             0x0800
-#define E1000_TSYNCRXCFG_PTP_V2_DELAY_RESP_MESSAGE           0x0900
-#define E1000_TSYNCRXCFG_PTP_V2_PATH_DELAY_FOLLOWUP_MESSAGE  0x0A00
-#define E1000_TSYNCRXCFG_PTP_V2_ANNOUNCE_MESSAGE             0x0B00
-#define E1000_TSYNCRXCFG_PTP_V2_SIGNALLING_MESSAGE           0x0C00
-#define E1000_TSYNCRXCFG_PTP_V2_MANAGEMENT_MESSAGE           0x0D00
-
-#define E1000_TIMINCA_16NS_SHIFT 24
-
-#define E1000_MDICNFG_EXT_MDIO    0x80000000      /* MDI ext/int destination */
-#define E1000_MDICNFG_COM_MDIO    0x40000000      /* MDI shared w/ lan 0 */
-#define E1000_MDICNFG_PHY_MASK    0x03E00000
-#define E1000_MDICNFG_PHY_SHIFT   21
-
+#define E1000_FCRTH_RTH		0x0000FFF8 /* Mask Bits[15:3] for RTH */
+#define E1000_FCRTL_RTL		0x0000FFF8 /* Mask Bits[15:3] for RTL */
+#define E1000_FCRTL_XONE	0x80000000 /* Enable XON frame transmission */
+
+/* Transmit Configuration Word */
+#define E1000_TXCW_FD		0x00000020 /* TXCW full duplex */
+#define E1000_TXCW_PAUSE	0x00000080 /* TXCW sym pause request */
+#define E1000_TXCW_ASM_DIR	0x00000100 /* TXCW astm pause direction */
+#define E1000_TXCW_PAUSE_MASK	0x00000180 /* TXCW pause request mask */
+#define E1000_TXCW_ANE		0x80000000 /* Auto-neg enable */
+
+/* Receive Configuration Word */
+#define E1000_RXCW_CW		0x0000ffff /* RxConfigWord mask */
+#define E1000_RXCW_IV		0x08000000 /* Receive config invalid */
+#define E1000_RXCW_C		0x20000000 /* Receive config */
+#define E1000_RXCW_SYNCH	0x40000000 /* Receive config synch */
+
+#define E1000_TSYNCTXCTL_VALID		0x00000001 /* Tx timestamp valid */
+#define E1000_TSYNCTXCTL_ENABLED	0x00000010 /* enable Tx timestamping */
+
+#define E1000_TSYNCRXCTL_VALID		0x00000001 /* Rx timestamp valid */
+#define E1000_TSYNCRXCTL_TYPE_MASK	0x0000000E /* Rx type mask */
+#define E1000_TSYNCRXCTL_TYPE_L2_V2	0x00
+#define E1000_TSYNCRXCTL_TYPE_L4_V1	0x02
+#define E1000_TSYNCRXCTL_TYPE_L2_L4_V2	0x04
+#define E1000_TSYNCRXCTL_TYPE_ALL	0x08
+#define E1000_TSYNCRXCTL_TYPE_EVENT_V2	0x0A
+#define E1000_TSYNCRXCTL_ENABLED	0x00000010 /* enable Rx timestamping */
+#define E1000_TSYNCRXCTL_SYSCFI		0x00000020 /* Sys clock frequency */
+
+#define E1000_TSYNCRXCFG_PTP_V1_CTRLT_MASK		0x000000FF
+#define E1000_TSYNCRXCFG_PTP_V1_SYNC_MESSAGE		0x00
+#define E1000_TSYNCRXCFG_PTP_V1_DELAY_REQ_MESSAGE	0x01
+#define E1000_TSYNCRXCFG_PTP_V1_FOLLOWUP_MESSAGE	0x02
+#define E1000_TSYNCRXCFG_PTP_V1_DELAY_RESP_MESSAGE	0x03
+#define E1000_TSYNCRXCFG_PTP_V1_MANAGEMENT_MESSAGE	0x04
+
+#define E1000_TSYNCRXCFG_PTP_V2_MSGID_MASK		0x00000F00
+#define E1000_TSYNCRXCFG_PTP_V2_SYNC_MESSAGE		0x0000
+#define E1000_TSYNCRXCFG_PTP_V2_DELAY_REQ_MESSAGE	0x0100
+#define E1000_TSYNCRXCFG_PTP_V2_PATH_DELAY_REQ_MESSAGE	0x0200
+#define E1000_TSYNCRXCFG_PTP_V2_PATH_DELAY_RESP_MESSAGE	0x0300
+#define E1000_TSYNCRXCFG_PTP_V2_FOLLOWUP_MESSAGE	0x0800
+#define E1000_TSYNCRXCFG_PTP_V2_DELAY_RESP_MESSAGE	0x0900
+#define E1000_TSYNCRXCFG_PTP_V2_PATH_DELAY_FOLLOWUP_MESSAGE 0x0A00
+#define E1000_TSYNCRXCFG_PTP_V2_ANNOUNCE_MESSAGE	0x0B00
+#define E1000_TSYNCRXCFG_PTP_V2_SIGNALLING_MESSAGE	0x0C00
+#define E1000_TSYNCRXCFG_PTP_V2_MANAGEMENT_MESSAGE	0x0D00
+
+#define E1000_TIMINCA_16NS_SHIFT	24
+#define E1000_TIMINCA_INCPERIOD_SHIFT	24
+#define E1000_TIMINCA_INCVALUE_MASK	0x00FFFFFF
+
+#define E1000_TSICR_TXTS		0x00000002
+#define E1000_TSIM_TXTS			0x00000002
+/* TUPLE Filtering Configuration */
+#define E1000_TTQF_DISABLE_MASK		0xF0008000 /* TTQF Disable Mask */
+#define E1000_TTQF_QUEUE_ENABLE		0x100   /* TTQF Queue Enable Bit */
+#define E1000_TTQF_PROTOCOL_MASK	0xFF    /* TTQF Protocol Mask */
+/* TTQF TCP Bit, shift with E1000_TTQF_PROTOCOL SHIFT */
+#define E1000_TTQF_PROTOCOL_TCP		0x0
+/* TTQF UDP Bit, shift with E1000_TTQF_PROTOCOL_SHIFT */
+#define E1000_TTQF_PROTOCOL_UDP		0x1
+/* TTQF SCTP Bit, shift with E1000_TTQF_PROTOCOL_SHIFT */
+#define E1000_TTQF_PROTOCOL_SCTP	0x2
+#define E1000_TTQF_PROTOCOL_SHIFT	5       /* TTQF Protocol Shift */
+#define E1000_TTQF_QUEUE_SHIFT		16      /* TTQF Queue Shfit */
+#define E1000_TTQF_RX_QUEUE_MASK	0x70000 /* TTQF Queue Mask */
+#define E1000_TTQF_MASK_ENABLE		0x10000000 /* TTQF Mask Enable Bit */
+#define E1000_IMIR_CLEAR_MASK		0xF001FFFF /* IMIR Reg Clear Mask */
+#define E1000_IMIR_PORT_BYPASS		0x20000 /* IMIR Port Bypass Bit */
+#define E1000_IMIR_PRIORITY_SHIFT	29 /* IMIR Priority Shift */
+#define E1000_IMIREXT_CLEAR_MASK	0x7FFFF /* IMIREXT Reg Clear Mask */
+
+#define E1000_MDICNFG_EXT_MDIO		0x80000000 /* MDI ext/int destination */
+#define E1000_MDICNFG_COM_MDIO		0x40000000 /* MDI shared w/ lan 0 */
+#define E1000_MDICNFG_PHY_MASK		0x03E00000
+#define E1000_MDICNFG_PHY_SHIFT		21
+
+#define E1000_MEDIA_PORT_COPPER			1
+#define E1000_MEDIA_PORT_OTHER			2
+#define E1000_M88E1112_AUTO_COPPER_SGMII	0x2
+#define E1000_M88E1112_AUTO_COPPER_BASEX	0x3
+#define E1000_M88E1112_STATUS_LINK		0x0004 /* Interface Link Bit */
+#define E1000_M88E1112_MAC_CTRL_1		0x10
+#define E1000_M88E1112_MAC_CTRL_1_MODE_MASK	0x0380 /* Mode Select */
+#define E1000_M88E1112_MAC_CTRL_1_MODE_SHIFT	7
+#define E1000_M88E1112_PAGE_ADDR		0x16
+#define E1000_M88E1112_STATUS			0x01
+
+#define E1000_THSTAT_LOW_EVENT		0x20000000 /* Low thermal threshold */
+#define E1000_THSTAT_MID_EVENT		0x00200000 /* Mid thermal threshold */
+#define E1000_THSTAT_HIGH_EVENT		0x00002000 /* High thermal threshold */
+#define E1000_THSTAT_PWR_DOWN		0x00000001 /* Power Down Event */
+#define E1000_THSTAT_LINK_THROTTLE	0x00000002 /* Link Spd Throttle Event */
+
+/* I350 EEE defines */
+#define E1000_IPCNFG_EEE_1G_AN		0x00000008 /* IPCNFG EEE Ena 1G AN */
+#define E1000_IPCNFG_EEE_100M_AN	0x00000004 /* IPCNFG EEE Ena 100M AN */
+#define E1000_EEER_TX_LPI_EN		0x00010000 /* EEER Tx LPI Enable */
+#define E1000_EEER_RX_LPI_EN		0x00020000 /* EEER Rx LPI Enable */
+#define E1000_EEER_LPI_FC		0x00040000 /* EEER Ena on Flow Cntrl */
+/* EEE status */
+#define E1000_EEER_EEE_NEG		0x20000000 /* EEE capability nego */
+#define E1000_EEER_RX_LPI_STATUS	0x40000000 /* Rx in LPI state */
+#define E1000_EEER_TX_LPI_STATUS	0x80000000 /* Tx in LPI state */
+#define E1000_EEE_LP_ADV_ADDR_I350	0x040F     /* EEE LP Advertisement */
+#define E1000_M88E1543_PAGE_ADDR	0x16       /* Page Offset Register */
+#define E1000_M88E1543_EEE_CTRL_1	0x0
+#define E1000_M88E1543_EEE_CTRL_1_MS	0x0001     /* EEE Master/Slave */
+#define E1000_EEE_ADV_DEV_I354		7
+#define E1000_EEE_ADV_ADDR_I354		60
+#define E1000_EEE_ADV_100_SUPPORTED	(1 << 1)   /* 100BaseTx EEE Supported */
+#define E1000_EEE_ADV_1000_SUPPORTED	(1 << 2)   /* 1000BaseT EEE Supported */
+#define E1000_PCS_STATUS_DEV_I354	3
+#define E1000_PCS_STATUS_ADDR_I354	1
+#define E1000_PCS_STATUS_RX_LPI_RCVD	0x0400
+#define E1000_PCS_STATUS_TX_LPI_RCVD	0x0800
+#define E1000_M88E1512_CFG_REG_1	0x0010
+#define E1000_M88E1512_CFG_REG_2	0x0011
+#define E1000_M88E1512_CFG_REG_3	0x0007
+#define E1000_M88E1512_MODE		0x0014
+#define E1000_EEE_SU_LPI_CLK_STP	0x00800000 /* EEE LPI Clock Stop */
+#define E1000_EEE_LP_ADV_DEV_I210	7          /* EEE LP Adv Device */
+#define E1000_EEE_LP_ADV_ADDR_I210	61         /* EEE LP Adv Register */
 /* PCI Express Control */
-#define E1000_GCR_CMPL_TMOUT_MASK       0x0000F000
-#define E1000_GCR_CMPL_TMOUT_10ms       0x00001000
-#define E1000_GCR_CMPL_TMOUT_RESEND     0x00010000
-#define E1000_GCR_CAP_VER2              0x00040000
-
-/* mPHY Address Control and Data Registers */
-#define E1000_MPHY_ADDR_CTL          0x0024 /* mPHY Address Control Register */
-#define E1000_MPHY_ADDR_CTL_OFFSET_MASK 0xFFFF0000
-#define E1000_MPHY_DATA                 0x0E10 /* mPHY Data Register */
-
-/* mPHY PCS CLK Register */
-#define E1000_MPHY_PCS_CLK_REG_OFFSET  0x0004 /* mPHY PCS CLK AFE CSR Offset */
-/* mPHY Near End Digital Loopback Override Bit */
-#define E1000_MPHY_PCS_CLK_REG_DIGINELBEN 0x10
+#define E1000_GCR_RXD_NO_SNOOP		0x00000001
+#define E1000_GCR_RXDSCW_NO_SNOOP	0x00000002
+#define E1000_GCR_RXDSCR_NO_SNOOP	0x00000004
+#define E1000_GCR_TXD_NO_SNOOP		0x00000008
+#define E1000_GCR_TXDSCW_NO_SNOOP	0x00000010
+#define E1000_GCR_TXDSCR_NO_SNOOP	0x00000020
+#define E1000_GCR_CMPL_TMOUT_MASK	0x0000F000
+#define E1000_GCR_CMPL_TMOUT_10ms	0x00001000
+#define E1000_GCR_CMPL_TMOUT_RESEND	0x00010000
+#define E1000_GCR_CAP_VER2		0x00040000
+
+#define PCIE_NO_SNOOP_ALL	(E1000_GCR_RXD_NO_SNOOP | \
+				 E1000_GCR_RXDSCW_NO_SNOOP | \
+				 E1000_GCR_RXDSCR_NO_SNOOP | \
+				 E1000_GCR_TXD_NO_SNOOP    | \
+				 E1000_GCR_TXDSCW_NO_SNOOP | \
+				 E1000_GCR_TXDSCR_NO_SNOOP)
+
+#define E1000_MMDAC_FUNC_DATA	0x4000 /* Data, no post increment */
+
+/* mPHY address control and data registers */
+#define E1000_MPHY_ADDR_CTL		0x0024 /* Address Control Reg */
+#define E1000_MPHY_ADDR_CTL_OFFSET_MASK	0xFFFF0000
+#define E1000_MPHY_DATA			0x0E10 /* Data Register */
+
+/* AFE CSR Offset for PCS CLK */
+#define E1000_MPHY_PCS_CLK_REG_OFFSET	0x0004
+/* Override for near end digital loopback. */
+#define E1000_MPHY_PCS_CLK_REG_DIGINELBEN	0x10
 
 /* PHY Control Register */
-#define MII_CR_FULL_DUPLEX      0x0100  /* FDX =1, half duplex =0 */
-#define MII_CR_RESTART_AUTO_NEG 0x0200  /* Restart auto negotiation */
-#define MII_CR_POWER_DOWN       0x0800  /* Power down */
-#define MII_CR_AUTO_NEG_EN      0x1000  /* Auto Neg Enable */
-#define MII_CR_LOOPBACK         0x4000  /* 0 = normal, 1 = loopback */
-#define MII_CR_RESET            0x8000  /* 0 = normal, 1 = PHY reset */
-#define MII_CR_SPEED_1000       0x0040
-#define MII_CR_SPEED_100        0x2000
-#define MII_CR_SPEED_10         0x0000
+#define MII_CR_SPEED_SELECT_MSB	0x0040  /* bits 6,13: 10=1000, 01=100, 00=10 */
+#define MII_CR_COLL_TEST_ENABLE	0x0080  /* Collision test enable */
+#define MII_CR_FULL_DUPLEX	0x0100  /* FDX =1, half duplex =0 */
+#define MII_CR_RESTART_AUTO_NEG	0x0200  /* Restart auto negotiation */
+#define MII_CR_ISOLATE		0x0400  /* Isolate PHY from MII */
+#define MII_CR_POWER_DOWN	0x0800  /* Power down */
+#define MII_CR_AUTO_NEG_EN	0x1000  /* Auto Neg Enable */
+#define MII_CR_SPEED_SELECT_LSB	0x2000  /* bits 6,13: 10=1000, 01=100, 00=10 */
+#define MII_CR_LOOPBACK		0x4000  /* 0 = normal, 1 = loopback */
+#define MII_CR_RESET		0x8000  /* 0 = normal, 1 = PHY reset */
+#define MII_CR_SPEED_1000	0x0040
+#define MII_CR_SPEED_100	0x2000
+#define MII_CR_SPEED_10		0x0000
 
 /* PHY Status Register */
-#define MII_SR_LINK_STATUS       0x0004 /* Link Status 1 = link */
-#define MII_SR_AUTONEG_COMPLETE  0x0020 /* Auto Neg Complete */
+#define MII_SR_EXTENDED_CAPS	0x0001 /* Extended register capabilities */
+#define MII_SR_JABBER_DETECT	0x0002 /* Jabber Detected */
+#define MII_SR_LINK_STATUS	0x0004 /* Link Status 1 = link */
+#define MII_SR_AUTONEG_CAPS	0x0008 /* Auto Neg Capable */
+#define MII_SR_REMOTE_FAULT	0x0010 /* Remote Fault Detect */
+#define MII_SR_AUTONEG_COMPLETE	0x0020 /* Auto Neg Complete */
+#define MII_SR_PREAMBLE_SUPPRESS 0x0040 /* Preamble may be suppressed */
+#define MII_SR_EXTENDED_STATUS	0x0100 /* Ext. status info in Reg 0x0F */
+#define MII_SR_100T2_HD_CAPS	0x0200 /* 100T2 Half Duplex Capable */
+#define MII_SR_100T2_FD_CAPS	0x0400 /* 100T2 Full Duplex Capable */
+#define MII_SR_10T_HD_CAPS	0x0800 /* 10T   Half Duplex Capable */
+#define MII_SR_10T_FD_CAPS	0x1000 /* 10T   Full Duplex Capable */
+#define MII_SR_100X_HD_CAPS	0x2000 /* 100X  Half Duplex Capable */
+#define MII_SR_100X_FD_CAPS	0x4000 /* 100X  Full Duplex Capable */
+#define MII_SR_100T4_CAPS	0x8000 /* 100T4 Capable */
 
 /* Autoneg Advertisement Register */
-#define NWAY_AR_10T_HD_CAPS      0x0020   /* 10T   Half Duplex Capable */
-#define NWAY_AR_10T_FD_CAPS      0x0040   /* 10T   Full Duplex Capable */
-#define NWAY_AR_100TX_HD_CAPS    0x0080   /* 100TX Half Duplex Capable */
-#define NWAY_AR_100TX_FD_CAPS    0x0100   /* 100TX Full Duplex Capable */
-#define NWAY_AR_PAUSE            0x0400   /* Pause operation desired */
-#define NWAY_AR_ASM_DIR          0x0800   /* Asymmetric Pause Direction bit */
+#define NWAY_AR_SELECTOR_FIELD	0x0001   /* indicates IEEE 802.3 CSMA/CD */
+#define NWAY_AR_10T_HD_CAPS	0x0020   /* 10T   Half Duplex Capable */
+#define NWAY_AR_10T_FD_CAPS	0x0040   /* 10T   Full Duplex Capable */
+#define NWAY_AR_100TX_HD_CAPS	0x0080   /* 100TX Half Duplex Capable */
+#define NWAY_AR_100TX_FD_CAPS	0x0100   /* 100TX Full Duplex Capable */
+#define NWAY_AR_100T4_CAPS	0x0200   /* 100T4 Capable */
+#define NWAY_AR_PAUSE		0x0400   /* Pause operation desired */
+#define NWAY_AR_ASM_DIR		0x0800   /* Asymmetric Pause Direction bit */
+#define NWAY_AR_REMOTE_FAULT	0x2000   /* Remote Fault detected */
+#define NWAY_AR_NEXT_PAGE	0x8000   /* Next Page ability supported */
 
 /* Link Partner Ability Register (Base Page) */
-#define NWAY_LPAR_PAUSE          0x0400 /* LP Pause operation desired */
-#define NWAY_LPAR_ASM_DIR        0x0800 /* LP Asymmetric Pause Direction bit */
+#define NWAY_LPAR_SELECTOR_FIELD	0x0000 /* LP protocol selector field */
+#define NWAY_LPAR_10T_HD_CAPS		0x0020 /* LP 10T Half Dplx Capable */
+#define NWAY_LPAR_10T_FD_CAPS		0x0040 /* LP 10T Full Dplx Capable */
+#define NWAY_LPAR_100TX_HD_CAPS		0x0080 /* LP 100TX Half Dplx Capable */
+#define NWAY_LPAR_100TX_FD_CAPS		0x0100 /* LP 100TX Full Dplx Capable */
+#define NWAY_LPAR_100T4_CAPS		0x0200 /* LP is 100T4 Capable */
+#define NWAY_LPAR_PAUSE			0x0400 /* LP Pause operation desired */
+#define NWAY_LPAR_ASM_DIR		0x0800 /* LP Asym Pause Direction bit */
+#define NWAY_LPAR_REMOTE_FAULT		0x2000 /* LP detected Remote Fault */
+#define NWAY_LPAR_ACKNOWLEDGE		0x4000 /* LP rx'd link code word */
+#define NWAY_LPAR_NEXT_PAGE		0x8000 /* Next Page ability supported */
 
 /* Autoneg Expansion Register */
+#define NWAY_ER_LP_NWAY_CAPS		0x0001 /* LP has Auto Neg Capability */
+#define NWAY_ER_PAGE_RXD		0x0002 /* LP 10T Half Dplx Capable */
+#define NWAY_ER_NEXT_PAGE_CAPS		0x0004 /* LP 10T Full Dplx Capable */
+#define NWAY_ER_LP_NEXT_PAGE_CAPS	0x0008 /* LP 100TX Half Dplx Capable */
+#define NWAY_ER_PAR_DETECT_FAULT	0x0010 /* LP 100TX Full Dplx Capable */
 
 /* 1000BASE-T Control Register */
-#define CR_1000T_HD_CAPS         0x0100 /* Advertise 1000T HD capability */
-#define CR_1000T_FD_CAPS         0x0200 /* Advertise 1000T FD capability  */
-#define CR_1000T_MS_VALUE        0x0800 /* 1=Configure PHY as Master */
-					/* 0=Configure PHY as Slave */
-#define CR_1000T_MS_ENABLE       0x1000 /* 1=Master/Slave manual config value */
-					/* 0=Automatic Master/Slave config */
+#define CR_1000T_ASYM_PAUSE	0x0080 /* Advertise asymmetric pause bit */
+#define CR_1000T_HD_CAPS	0x0100 /* Advertise 1000T HD capability */
+#define CR_1000T_FD_CAPS	0x0200 /* Advertise 1000T FD capability  */
+/* 1=Repeater/switch device port 0=DTE device */
+#define CR_1000T_REPEATER_DTE	0x0400
+/* 1=Configure PHY as Master 0=Configure PHY as Slave */
+#define CR_1000T_MS_VALUE	0x0800
+/* 1=Master/Slave manual config value 0=Automatic Master/Slave config */
+#define CR_1000T_MS_ENABLE	0x1000
+#define CR_1000T_TEST_MODE_NORMAL 0x0000 /* Normal Operation */
+#define CR_1000T_TEST_MODE_1	0x2000 /* Transmit Waveform test */
+#define CR_1000T_TEST_MODE_2	0x4000 /* Master Transmit Jitter test */
+#define CR_1000T_TEST_MODE_3	0x6000 /* Slave Transmit Jitter test */
+#define CR_1000T_TEST_MODE_4	0x8000 /* Transmitter Distortion test */
 
 /* 1000BASE-T Status Register */
-#define SR_1000T_REMOTE_RX_STATUS 0x1000 /* Remote receiver OK */
-#define SR_1000T_LOCAL_RX_STATUS  0x2000 /* Local receiver OK */
+#define SR_1000T_IDLE_ERROR_CNT		0x00FF /* Num idle err since last rd */
+#define SR_1000T_ASYM_PAUSE_DIR		0x0100 /* LP asym pause direction bit */
+#define SR_1000T_LP_HD_CAPS		0x0400 /* LP is 1000T HD capable */
+#define SR_1000T_LP_FD_CAPS		0x0800 /* LP is 1000T FD capable */
+#define SR_1000T_REMOTE_RX_STATUS	0x1000 /* Remote receiver OK */
+#define SR_1000T_LOCAL_RX_STATUS	0x2000 /* Local receiver OK */
+#define SR_1000T_MS_CONFIG_RES		0x4000 /* 1=Local Tx Master, 0=Slave */
+#define SR_1000T_MS_CONFIG_FAULT	0x8000 /* Master/Slave config fault */
 
+#define SR_1000T_PHY_EXCESSIVE_IDLE_ERR_COUNT	5
 
 /* PHY 1000 MII Register/Bit Definitions */
 /* PHY Registers defined by IEEE */
-#define PHY_CONTROL      0x00 /* Control Register */
-#define PHY_STATUS       0x01 /* Status Register */
-#define PHY_ID1          0x02 /* Phy Id Reg (word 1) */
-#define PHY_ID2          0x03 /* Phy Id Reg (word 2) */
-#define PHY_AUTONEG_ADV  0x04 /* Autoneg Advertisement */
-#define PHY_LP_ABILITY   0x05 /* Link Partner Ability (Base Page) */
-#define PHY_1000T_CTRL   0x09 /* 1000Base-T Control Reg */
-#define PHY_1000T_STATUS 0x0A /* 1000Base-T Status Reg */
+#define PHY_CONTROL		0x00 /* Control Register */
+#define PHY_STATUS		0x01 /* Status Register */
+#define PHY_ID1			0x02 /* Phy Id Reg (word 1) */
+#define PHY_ID2			0x03 /* Phy Id Reg (word 2) */
+#define PHY_AUTONEG_ADV		0x04 /* Autoneg Advertisement */
+#define PHY_LP_ABILITY		0x05 /* Link Partner Ability (Base Page) */
+#define PHY_AUTONEG_EXP		0x06 /* Autoneg Expansion Reg */
+#define PHY_NEXT_PAGE_TX	0x07 /* Next Page Tx */
+#define PHY_LP_NEXT_PAGE	0x08 /* Link Partner Next Page */
+#define PHY_1000T_CTRL		0x09 /* 1000Base-T Control Reg */
+#define PHY_1000T_STATUS	0x0A /* 1000Base-T Status Reg */
+#define PHY_EXT_STATUS		0x0F /* Extended Status Reg */
+
+#define PHY_CONTROL_LB		0x4000 /* PHY Loopback bit */
 
 /* NVM Control */
-#define E1000_EECD_SK        0x00000001 /* NVM Clock */
-#define E1000_EECD_CS        0x00000002 /* NVM Chip Select */
-#define E1000_EECD_DI        0x00000004 /* NVM Data In */
-#define E1000_EECD_DO        0x00000008 /* NVM Data Out */
-#define E1000_EECD_REQ       0x00000040 /* NVM Access Request */
-#define E1000_EECD_GNT       0x00000080 /* NVM Access Grant */
-#define E1000_EECD_PRES      0x00000100 /* NVM Present */
+#define E1000_EECD_SK		0x00000001 /* NVM Clock */
+#define E1000_EECD_CS		0x00000002 /* NVM Chip Select */
+#define E1000_EECD_DI		0x00000004 /* NVM Data In */
+#define E1000_EECD_DO		0x00000008 /* NVM Data Out */
+#define E1000_EECD_REQ		0x00000040 /* NVM Access Request */
+#define E1000_EECD_GNT		0x00000080 /* NVM Access Grant */
+#define E1000_EECD_PRES		0x00000100 /* NVM Present */
+#define E1000_EECD_SIZE		0x00000200 /* NVM Size (0=64 word 1=256 word) */
+#define E1000_EECD_BLOCKED	0x00008000 /* Bit banging access blocked flag */
+#define E1000_EECD_ABORT	0x00010000 /* NVM operation aborted flag */
+#define E1000_EECD_TIMEOUT	0x00020000 /* NVM read operation timeout flag */
+#define E1000_EECD_ERROR_CLR	0x00040000 /* NVM error status clear bit */
 /* NVM Addressing bits based on type 0=small, 1=large */
-#define E1000_EECD_ADDR_BITS 0x00000400
-#define E1000_NVM_GRANT_ATTEMPTS   1000 /* NVM # attempts to gain grant */
-#define E1000_EECD_AUTO_RD          0x00000200  /* NVM Auto Read done */
-#define E1000_EECD_SIZE_EX_MASK     0x00007800  /* NVM Size */
-#define E1000_EECD_SIZE_EX_SHIFT     11
-
-/* Offset to data in NVM read/write registers */
-#define E1000_NVM_RW_REG_DATA   16
-#define E1000_NVM_RW_REG_DONE   2    /* Offset to READ/WRITE done bit */
-#define E1000_NVM_RW_REG_START  1    /* Start operation */
-#define E1000_NVM_RW_ADDR_SHIFT 2    /* Shift to the address bits */
-#define E1000_NVM_POLL_READ     0    /* Flag for polling for read complete */
+#define E1000_EECD_ADDR_BITS	0x00000400
+#define E1000_NVM_GRANT_ATTEMPTS	1000 /* NVM # attempts to gain grant */
+#define E1000_EECD_AUTO_RD		0x00000200  /* NVM Auto Read done */
+#define E1000_EECD_SIZE_EX_MASK		0x00007800  /* NVM Size */
+#define E1000_EECD_SIZE_EX_SHIFT	11
+#define E1000_EECD_FLUPD		0x00080000 /* Update FLASH */
+#define E1000_EECD_AUPDEN		0x00100000 /* Ena Auto FLASH update */
+#define E1000_EECD_SEC1VAL		0x00400000 /* Sector One Valid */
+#define E1000_EECD_SEC1VAL_VALID_MASK	(E1000_EECD_AUTO_RD | E1000_EECD_PRES)
+#define E1000_EECD_FLUPD_I210		0x00800000 /* Update FLASH */
+#define E1000_EECD_FLUDONE_I210		0x04000000 /* Update FLASH done */
+#define E1000_EECD_FLASH_DETECTED_I210	0x00080000 /* FLASH detected */
+#define E1000_EECD_SEC1VAL_I210		0x02000000 /* Sector One Valid */
+#define E1000_FLUDONE_ATTEMPTS		20000
+#define E1000_EERD_EEWR_MAX_COUNT	512 /* buffered EEPROM words rw */
+#define E1000_I210_FIFO_SEL_RX		0x00
+#define E1000_I210_FIFO_SEL_TX_QAV(_i)	(0x02 + (_i))
+#define E1000_I210_FIFO_SEL_TX_LEGACY	E1000_I210_FIFO_SEL_TX_QAV(0)
+#define E1000_I210_FIFO_SEL_BMC2OS_TX	0x06
+#define E1000_I210_FIFO_SEL_BMC2OS_RX	0x01
+
+#define E1000_I210_FLASH_SECTOR_SIZE	0x1000 /* 4KB FLASH sector unit size */
+/* Secure FLASH mode requires removing MSb */
+#define E1000_I210_FW_PTR_MASK		0x7FFF
+/* Firmware code revision field word offset*/
+#define E1000_I210_FW_VER_OFFSET	328
+
+#define E1000_NVM_RW_REG_DATA	16  /* Offset to data in NVM read/write regs */
+#define E1000_NVM_RW_REG_DONE	2   /* Offset to READ/WRITE done bit */
+#define E1000_NVM_RW_REG_START	1   /* Start operation */
+#define E1000_NVM_RW_ADDR_SHIFT	2   /* Shift to the address bits */
+#define E1000_NVM_POLL_WRITE	1   /* Flag for polling for write complete */
+#define E1000_NVM_POLL_READ	0   /* Flag for polling for read complete */
+#define E1000_FLASH_UPDATES	2000
 
 /* NVM Word Offsets */
-#define NVM_COMPAT                 0x0003
-#define NVM_ID_LED_SETTINGS        0x0004 /* SERDES output amplitude */
-#define NVM_INIT_CONTROL2_REG      0x000F
-#define NVM_INIT_CONTROL3_PORT_B   0x0014
-#define NVM_INIT_CONTROL3_PORT_A   0x0024
-#define NVM_ALT_MAC_ADDR_PTR       0x0037
-#define NVM_CHECKSUM_REG           0x003F
-#define NVM_COMPATIBILITY_REG_3    0x0003
-#define NVM_COMPATIBILITY_BIT_MASK 0x8000
-
-#define E1000_NVM_CFG_DONE_PORT_0  0x040000 /* MNG config cycle done */
-#define E1000_NVM_CFG_DONE_PORT_1  0x080000 /* ...for second port */
-#define E1000_NVM_CFG_DONE_PORT_2  0x100000 /* ...for third port */
-#define E1000_NVM_CFG_DONE_PORT_3  0x200000 /* ...for fourth port */
-
-#define NVM_82580_LAN_FUNC_OFFSET(a) (a ? (0x40 + (0x40 * a)) : 0)
+#define NVM_COMPAT			0x0003
+#define NVM_ID_LED_SETTINGS		0x0004
+#define NVM_VERSION			0x0005
+#define E1000_I210_NVM_FW_MODULE_PTR	0x0010
+#define E1000_I350_NVM_FW_MODULE_PTR	0x0051
+#define NVM_FUTURE_INIT_WORD1		0x0019
+#define NVM_ETRACK_WORD			0x0042
+#define NVM_ETRACK_HIWORD		0x0043
+#define NVM_COMB_VER_OFF		0x0083
+#define NVM_COMB_VER_PTR		0x003d
+
+/* NVM version defines */
+#define NVM_MAJOR_MASK			0xF000
+#define NVM_MINOR_MASK			0x0FF0
+#define NVM_IMAGE_ID_MASK		0x000F
+#define NVM_COMB_VER_MASK		0x00FF
+#define NVM_MAJOR_SHIFT			12
+#define NVM_MINOR_SHIFT			4
+#define NVM_COMB_VER_SHFT		8
+#define NVM_VER_INVALID			0xFFFF
+#define NVM_ETRACK_SHIFT		16
+#define NVM_ETRACK_VALID		0x8000
+#define NVM_NEW_DEC_MASK		0x0F00
+#define NVM_HEX_CONV			16
+#define NVM_HEX_TENS			10
+
+/* FW version defines */
+/* Offset of "Loader patch ptr" in Firmware Header */
+#define E1000_I350_NVM_FW_LOADER_PATCH_PTR_OFFSET	0x01
+/* Patch generation hour & minutes */
+#define E1000_I350_NVM_FW_VER_WORD1_OFFSET		0x04
+/* Patch generation month & day */
+#define E1000_I350_NVM_FW_VER_WORD2_OFFSET		0x05
+/* Patch generation year */
+#define E1000_I350_NVM_FW_VER_WORD3_OFFSET		0x06
+/* Patch major & minor numbers */
+#define E1000_I350_NVM_FW_VER_WORD4_OFFSET		0x07
+
+#define NVM_MAC_ADDR			0x0000
+#define NVM_SUB_DEV_ID			0x000B
+#define NVM_SUB_VEN_ID			0x000C
+#define NVM_DEV_ID			0x000D
+#define NVM_VEN_ID			0x000E
+#define NVM_INIT_CTRL_2			0x000F
+#define NVM_INIT_CTRL_4			0x0013
+#define NVM_LED_1_CFG			0x001C
+#define NVM_LED_0_2_CFG			0x001F
+
+#define NVM_COMPAT_VALID_CSUM		0x0001
+#define NVM_FUTURE_INIT_WORD1_VALID_CSUM	0x0040
+
+#define NVM_ETS_CFG			0x003E
+#define NVM_ETS_LTHRES_DELTA_MASK	0x07C0
+#define NVM_ETS_LTHRES_DELTA_SHIFT	6
+#define NVM_ETS_TYPE_MASK		0x0038
+#define NVM_ETS_TYPE_SHIFT		3
+#define NVM_ETS_TYPE_EMC		0x000
+#define NVM_ETS_NUM_SENSORS_MASK	0x0007
+#define NVM_ETS_DATA_LOC_MASK		0x3C00
+#define NVM_ETS_DATA_LOC_SHIFT		10
+#define NVM_ETS_DATA_INDEX_MASK		0x0300
+#define NVM_ETS_DATA_INDEX_SHIFT	8
+#define NVM_ETS_DATA_HTHRESH_MASK	0x00FF
+#define NVM_INIT_CONTROL2_REG		0x000F
+#define NVM_INIT_CONTROL3_PORT_B	0x0014
+#define NVM_INIT_3GIO_3			0x001A
+#define NVM_SWDEF_PINS_CTRL_PORT_0	0x0020
+#define NVM_INIT_CONTROL3_PORT_A	0x0024
+#define NVM_CFG				0x0012
+#define NVM_ALT_MAC_ADDR_PTR		0x0037
+#define NVM_CHECKSUM_REG		0x003F
+#define NVM_COMPATIBILITY_REG_3		0x0003
+#define NVM_COMPATIBILITY_BIT_MASK	0x8000
+
+#define E1000_NVM_CFG_DONE_PORT_0	0x040000 /* MNG config cycle done */
+#define E1000_NVM_CFG_DONE_PORT_1	0x080000 /* ...for second port */
+#define E1000_NVM_CFG_DONE_PORT_2	0x100000 /* ...for third port */
+#define E1000_NVM_CFG_DONE_PORT_3	0x200000 /* ...for fourth port */
+
+#define NVM_82580_LAN_FUNC_OFFSET(a)	((a) ? (0x40 + (0x40 * (a))) : 0)
 
 /* Mask bits for fields in Word 0x24 of the NVM */
-#define NVM_WORD24_COM_MDIO         0x0008 /* MDIO interface shared */
-#define NVM_WORD24_EXT_MDIO         0x0004 /* MDIO accesses routed external */
+#define NVM_WORD24_COM_MDIO		0x0008 /* MDIO interface shared */
+#define NVM_WORD24_EXT_MDIO		0x0004 /* MDIO accesses routed extrnl */
+/* Offset of Link Mode bits for 82575/82576 */
+#define NVM_WORD24_LNK_MODE_OFFSET	8
+/* Offset of Link Mode bits for 82580 up */
+#define NVM_WORD24_82580_LNK_MODE_OFFSET	4
+
 
 /* Mask bits for fields in Word 0x0f of the NVM */
-#define NVM_WORD0F_PAUSE_MASK       0x3000
-#define NVM_WORD0F_ASM_DIR          0x2000
+#define NVM_WORD0F_PAUSE_MASK		0x3000
+#define NVM_WORD0F_PAUSE		0x1000
+#define NVM_WORD0F_ASM_DIR		0x2000
 
 /* Mask bits for fields in Word 0x1a of the NVM */
+#define NVM_WORD1A_ASPM_MASK		0x000C
 
-/* length of string needed to store part num */
-#define E1000_PBANUM_LENGTH         11
+/* Mask bits for fields in Word 0x03 of the EEPROM */
+#define NVM_COMPAT_LOM			0x0800
 
-/* For checksumming, the sum of all words in the NVM should equal 0xBABA. */
-#define NVM_SUM                    0xBABA
+/* length of string needed to store PBA number */
+#define E1000_PBANUM_LENGTH		11
 
-#define NVM_PBA_OFFSET_0           8
-#define NVM_PBA_OFFSET_1           9
-#define NVM_PBA_PTR_GUARD          0xFAFA
-#define NVM_WORD_SIZE_BASE_SHIFT   6
+/* For checksumming, the sum of all words in the NVM should equal 0xBABA. */
+#define NVM_SUM				0xBABA
 
-/* NVM Commands - Microwire */
+/* PBA (printed board assembly) number words */
+#define NVM_PBA_OFFSET_0		8
+#define NVM_PBA_OFFSET_1		9
+#define NVM_PBA_PTR_GUARD		0xFAFA
+#define NVM_RESERVED_WORD		0xFFFF
+#define NVM_WORD_SIZE_BASE_SHIFT	6
 
 /* NVM Commands - SPI */
-#define NVM_MAX_RETRY_SPI          5000 /* Max wait of 5ms, for RDY signal */
-#define NVM_WRITE_OPCODE_SPI       0x02 /* NVM write opcode */
-#define NVM_READ_OPCODE_SPI        0x03 /* NVM read opcode */
-#define NVM_A8_OPCODE_SPI          0x08 /* opcode bit-3 = address bit-8 */
-#define NVM_WREN_OPCODE_SPI        0x06 /* NVM set Write Enable latch */
-#define NVM_RDSR_OPCODE_SPI        0x05 /* NVM read Status register */
+#define NVM_MAX_RETRY_SPI	5000 /* Max wait of 5ms, for RDY signal */
+#define NVM_READ_OPCODE_SPI	0x03 /* NVM read opcode */
+#define NVM_WRITE_OPCODE_SPI	0x02 /* NVM write opcode */
+#define NVM_A8_OPCODE_SPI	0x08 /* opcode bit-3 = address bit-8 */
+#define NVM_WREN_OPCODE_SPI	0x06 /* NVM set Write Enable latch */
+#define NVM_RDSR_OPCODE_SPI	0x05 /* NVM read Status register */
 
 /* SPI NVM Status Register */
-#define NVM_STATUS_RDY_SPI         0x01
+#define NVM_STATUS_RDY_SPI	0x01
 
 /* Word definitions for ID LED Settings */
-#define ID_LED_RESERVED_0000 0x0000
-#define ID_LED_RESERVED_FFFF 0xFFFF
-#define ID_LED_DEFAULT       ((ID_LED_OFF1_ON2  << 12) | \
-			      (ID_LED_OFF1_OFF2 <<  8) | \
-			      (ID_LED_DEF1_DEF2 <<  4) | \
-			      (ID_LED_DEF1_DEF2))
-#define ID_LED_DEF1_DEF2     0x1
-#define ID_LED_DEF1_ON2      0x2
-#define ID_LED_DEF1_OFF2     0x3
-#define ID_LED_ON1_DEF2      0x4
-#define ID_LED_ON1_ON2       0x5
-#define ID_LED_ON1_OFF2      0x6
-#define ID_LED_OFF1_DEF2     0x7
-#define ID_LED_OFF1_ON2      0x8
-#define ID_LED_OFF1_OFF2     0x9
-
-#define IGP_ACTIVITY_LED_MASK   0xFFFFF0FF
-#define IGP_ACTIVITY_LED_ENABLE 0x0300
-#define IGP_LED3_MODE           0x07000000
+#define ID_LED_RESERVED_0000	0x0000
+#define ID_LED_RESERVED_FFFF	0xFFFF
+#define ID_LED_DEFAULT		((ID_LED_OFF1_ON2  << 12) | \
+				 (ID_LED_OFF1_OFF2 <<  8) | \
+				 (ID_LED_DEF1_DEF2 <<  4) | \
+				 (ID_LED_DEF1_DEF2))
+#define ID_LED_DEF1_DEF2	0x1
+#define ID_LED_DEF1_ON2		0x2
+#define ID_LED_DEF1_OFF2	0x3
+#define ID_LED_ON1_DEF2		0x4
+#define ID_LED_ON1_ON2		0x5
+#define ID_LED_ON1_OFF2		0x6
+#define ID_LED_OFF1_DEF2	0x7
+#define ID_LED_OFF1_ON2		0x8
+#define ID_LED_OFF1_OFF2	0x9
+
+#define IGP_ACTIVITY_LED_MASK	0xFFFFF0FF
+#define IGP_ACTIVITY_LED_ENABLE	0x0300
+#define IGP_LED3_MODE		0x07000000
 
 /* PCI/PCI-X/PCI-EX Config space */
-#define PCIE_DEVICE_CONTROL2         0x28
-#define PCIE_DEVICE_CONTROL2_16ms    0x0005
+#define PCI_HEADER_TYPE_REGISTER	0x0E
+#define PCIE_LINK_STATUS		0x12
+#define PCIE_DEVICE_CONTROL2		0x28
+
+#define PCI_HEADER_TYPE_MULTIFUNC	0x80
+#define PCIE_LINK_WIDTH_MASK		0x3F0
+#define PCIE_LINK_WIDTH_SHIFT		4
+#define PCIE_LINK_SPEED_MASK		0x0F
+#define PCIE_LINK_SPEED_2500		0x01
+#define PCIE_LINK_SPEED_5000		0x02
+#define PCIE_DEVICE_CONTROL2_16ms	0x0005
+
+#ifndef ETH_ADDR_LEN
+#define ETH_ADDR_LEN			6
+#endif
 
-#define PHY_REVISION_MASK      0xFFFFFFF0
-#define MAX_PHY_REG_ADDRESS    0x1F  /* 5 bit address bus (0-0x1F) */
-#define MAX_PHY_MULTI_PAGE_REG 0xF
+#define PHY_REVISION_MASK		0xFFFFFFF0
+#define MAX_PHY_REG_ADDRESS		0x1F  /* 5 bit address bus (0-0x1F) */
+#define MAX_PHY_MULTI_PAGE_REG		0xF
 
-/* Bit definitions for valid PHY IDs. */
-/*
+/* Bit definitions for valid PHY IDs.
  * I = Integrated
  * E = External
  */
-#define M88E1111_I_PHY_ID    0x01410CC0
-#define M88E1112_E_PHY_ID    0x01410C90
-#define I347AT4_E_PHY_ID     0x01410DC0
-#define IGP03E1000_E_PHY_ID  0x02A80390
-#define I82580_I_PHY_ID      0x015403A0
-#define I350_I_PHY_ID        0x015403B0
-#define M88_VENDOR           0x0141
+#define M88E1000_E_PHY_ID	0x01410C50
+#define M88E1000_I_PHY_ID	0x01410C30
+#define M88E1011_I_PHY_ID	0x01410C20
+#define IGP01E1000_I_PHY_ID	0x02A80380
+#define M88E1111_I_PHY_ID	0x01410CC0
+#define M88E1543_E_PHY_ID	0x01410EA0
+#define M88E1512_E_PHY_ID	0x01410DD0
+#define M88E1112_E_PHY_ID	0x01410C90
+#define I347AT4_E_PHY_ID	0x01410DC0
+#define M88E1340M_E_PHY_ID	0x01410DF0
+#define GG82563_E_PHY_ID	0x01410CA0
+#define IGP03E1000_E_PHY_ID	0x02A80390
+#define IFE_E_PHY_ID		0x02A80330
+#define IFE_PLUS_E_PHY_ID	0x02A80320
+#define IFE_C_E_PHY_ID		0x02A80310
+#define I82580_I_PHY_ID		0x015403A0
+#define I350_I_PHY_ID		0x015403B0
+#define I210_I_PHY_ID		0x01410C00
+#define IGP04E1000_E_PHY_ID	0x02A80391
+#define M88_VENDOR		0x0141
 
 /* M88E1000 Specific Registers */
-#define M88E1000_PHY_SPEC_CTRL     0x10  /* PHY Specific Control Register */
-#define M88E1000_PHY_SPEC_STATUS   0x11  /* PHY Specific Status Register */
-#define M88E1000_EXT_PHY_SPEC_CTRL 0x14  /* Extended PHY Specific Control */
+#define M88E1000_PHY_SPEC_CTRL		0x10  /* PHY Specific Control Reg */
+#define M88E1000_PHY_SPEC_STATUS	0x11  /* PHY Specific Status Reg */
+#define M88E1000_EXT_PHY_SPEC_CTRL	0x14  /* Extended PHY Specific Cntrl */
+#define M88E1000_RX_ERR_CNTR		0x15  /* Receive Error Counter */
 
-#define M88E1000_PHY_PAGE_SELECT   0x1D  /* Reg 29 for page number setting */
-#define M88E1000_PHY_GEN_CONTROL   0x1E  /* Its meaning depends on reg 29 */
+#define M88E1000_PHY_PAGE_SELECT	0x1D  /* Reg 29 for pg number setting */
+#define M88E1000_PHY_GEN_CONTROL	0x1E  /* meaning depends on reg 29 */
 
 /* M88E1000 PHY Specific Control Register */
-#define M88E1000_PSCR_POLARITY_REVERSAL 0x0002 /* 1=Polarity Reversal enabled */
-/* 1=CLK125 low, 0=CLK125 toggling */
-#define M88E1000_PSCR_MDI_MANUAL_MODE  0x0000  /* MDI Crossover Mode bits 6:5 */
-					       /* Manual MDI configuration */
-#define M88E1000_PSCR_MDIX_MANUAL_MODE 0x0020  /* Manual MDIX configuration */
+#define M88E1000_PSCR_POLARITY_REVERSAL	0x0002 /* 1=Polarity Reverse enabled */
+/* MDI Crossover Mode bits 6:5 Manual MDI configuration */
+#define M88E1000_PSCR_MDI_MANUAL_MODE	0x0000
+#define M88E1000_PSCR_MDIX_MANUAL_MODE	0x0020  /* Manual MDIX configuration */
 /* 1000BASE-T: Auto crossover, 100BASE-TX/10BASE-T: MDI Mode */
-#define M88E1000_PSCR_AUTO_X_1000T     0x0040
+#define M88E1000_PSCR_AUTO_X_1000T	0x0040
 /* Auto crossover enabled all speeds */
-#define M88E1000_PSCR_AUTO_X_MODE      0x0060
-/*
- * 1=Enable Extended 10BASE-T distance (Lower 10BASE-T Rx Threshold
- * 0=Normal 10BASE-T Rx Threshold
- */
-/* 1=5-bit interface in 100BASE-TX, 0=MII interface in 100BASE-TX */
-#define M88E1000_PSCR_ASSERT_CRS_ON_TX     0x0800 /* 1=Assert CRS on Transmit */
+#define M88E1000_PSCR_AUTO_X_MODE	0x0060
+#define M88E1000_PSCR_ASSERT_CRS_ON_TX	0x0800 /* 1=Assert CRS on Tx */
 
 /* M88E1000 PHY Specific Status Register */
-#define M88E1000_PSSR_REV_POLARITY       0x0002 /* 1=Polarity reversed */
-#define M88E1000_PSSR_DOWNSHIFT          0x0020 /* 1=Downshifted */
-#define M88E1000_PSSR_MDIX               0x0040 /* 1=MDIX; 0=MDI */
-/*
- * 0 = <50M
+#define M88E1000_PSSR_REV_POLARITY	0x0002 /* 1=Polarity reversed */
+#define M88E1000_PSSR_DOWNSHIFT		0x0020 /* 1=Downshifted */
+#define M88E1000_PSSR_MDIX		0x0040 /* 1=MDIX; 0=MDI */
+/* 0 = <50M
  * 1 = 50-80M
  * 2 = 80-110M
  * 3 = 110-140M
  * 4 = >140M
  */
-#define M88E1000_PSSR_CABLE_LENGTH       0x0380
-#define M88E1000_PSSR_SPEED              0xC000 /* Speed, bits 14:15 */
-#define M88E1000_PSSR_1000MBS            0x8000 /* 10=1000Mbs */
-
-#define M88E1000_PSSR_CABLE_LENGTH_SHIFT 7
-
-/* M88E1000 Extended PHY Specific Control Register */
-/*
- * 1 = Lost lock detect enabled.
- * Will assert lost lock and bring
- * link down if idle not seen
- * within 1ms in 1000BASE-T
- */
-/*
- * Number of times we will attempt to autonegotiate before downshifting if we
+#define M88E1000_PSSR_CABLE_LENGTH	0x0380
+#define M88E1000_PSSR_LINK		0x0400 /* 1=Link up, 0=Link down */
+#define M88E1000_PSSR_SPD_DPLX_RESOLVED	0x0800 /* 1=Speed & Duplex resolved */
+#define M88E1000_PSSR_SPEED		0xC000 /* Speed, bits 14:15 */
+#define M88E1000_PSSR_1000MBS		0x8000 /* 10=1000Mbs */
+
+#define M88E1000_PSSR_CABLE_LENGTH_SHIFT	7
+
+/* Number of times we will attempt to autonegotiate before downshifting if we
  * are the master
  */
-#define M88E1000_EPSCR_MASTER_DOWNSHIFT_MASK 0x0C00
-#define M88E1000_EPSCR_MASTER_DOWNSHIFT_1X   0x0000
-/*
- * Number of times we will attempt to autonegotiate before downshifting if we
+#define M88E1000_EPSCR_MASTER_DOWNSHIFT_MASK	0x0C00
+#define M88E1000_EPSCR_MASTER_DOWNSHIFT_1X	0x0000
+/* Number of times we will attempt to autonegotiate before downshifting if we
  * are the slave
  */
-#define M88E1000_EPSCR_SLAVE_DOWNSHIFT_MASK  0x0300
-#define M88E1000_EPSCR_SLAVE_DOWNSHIFT_1X    0x0100
-#define M88E1000_EPSCR_TX_CLK_25      0x0070 /* 25  MHz TX_CLK */
-
-/* Intel i347-AT4 Registers */
+#define M88E1000_EPSCR_SLAVE_DOWNSHIFT_MASK	0x0300
+#define M88E1000_EPSCR_SLAVE_DOWNSHIFT_1X	0x0100
+#define M88E1000_EPSCR_TX_CLK_25	0x0070 /* 25  MHz TX_CLK */
 
-#define I347AT4_PCDL                   0x10 /* PHY Cable Diagnostics Length */
-#define I347AT4_PCDC                   0x15 /* PHY Cable Diagnostics Control */
-#define I347AT4_PAGE_SELECT            0x16
+/* Intel I347AT4 Registers */
+#define I347AT4_PCDL		0x10 /* PHY Cable Diagnostics Length */
+#define I347AT4_PCDC		0x15 /* PHY Cable Diagnostics Control */
+#define I347AT4_PAGE_SELECT	0x16
 
-/* i347-AT4 Extended PHY Specific Control Register */
+/* I347AT4 Extended PHY Specific Control Register */
 
-/*
- *  Number of times we will attempt to autonegotiate before downshifting if we
- *  are the master
+/* Number of times we will attempt to autonegotiate before downshifting if we
+ * are the master
  */
-#define I347AT4_PSCR_DOWNSHIFT_ENABLE 0x0800
-#define I347AT4_PSCR_DOWNSHIFT_MASK   0x7000
-#define I347AT4_PSCR_DOWNSHIFT_1X     0x0000
-#define I347AT4_PSCR_DOWNSHIFT_2X     0x1000
-#define I347AT4_PSCR_DOWNSHIFT_3X     0x2000
-#define I347AT4_PSCR_DOWNSHIFT_4X     0x3000
-#define I347AT4_PSCR_DOWNSHIFT_5X     0x4000
-#define I347AT4_PSCR_DOWNSHIFT_6X     0x5000
-#define I347AT4_PSCR_DOWNSHIFT_7X     0x6000
-#define I347AT4_PSCR_DOWNSHIFT_8X     0x7000
-
-/* i347-AT4 PHY Cable Diagnostics Control */
-#define I347AT4_PCDC_CABLE_LENGTH_UNIT 0x0400 /* 0=cm 1=meters */
-
-/* Marvell 1112 only registers */
-#define M88E1112_VCT_DSP_DISTANCE       0x001A
+#define I347AT4_PSCR_DOWNSHIFT_ENABLE	0x0800
+#define I347AT4_PSCR_DOWNSHIFT_MASK	0x7000
+#define I347AT4_PSCR_DOWNSHIFT_1X	0x0000
+#define I347AT4_PSCR_DOWNSHIFT_2X	0x1000
+#define I347AT4_PSCR_DOWNSHIFT_3X	0x2000
+#define I347AT4_PSCR_DOWNSHIFT_4X	0x3000
+#define I347AT4_PSCR_DOWNSHIFT_5X	0x4000
+#define I347AT4_PSCR_DOWNSHIFT_6X	0x5000
+#define I347AT4_PSCR_DOWNSHIFT_7X	0x6000
+#define I347AT4_PSCR_DOWNSHIFT_8X	0x7000
+
+/* I347AT4 PHY Cable Diagnostics Control */
+#define I347AT4_PCDC_CABLE_LENGTH_UNIT	0x0400 /* 0=cm 1=meters */
+
+/* M88E1112 only registers */
+#define M88E1112_VCT_DSP_DISTANCE	0x001A
 
 /* M88EC018 Rev 2 specific DownShift settings */
-#define M88EC018_EPSCR_DOWNSHIFT_COUNTER_MASK  0x0E00
-#define M88EC018_EPSCR_DOWNSHIFT_COUNTER_5X    0x0800
+#define M88EC018_EPSCR_DOWNSHIFT_COUNTER_MASK	0x0E00
+#define M88EC018_EPSCR_DOWNSHIFT_COUNTER_5X	0x0800
 
-/* MDI Control */
-#define E1000_MDIC_DATA_MASK 0x0000FFFF
-#define E1000_MDIC_REG_MASK  0x001F0000
-#define E1000_MDIC_REG_SHIFT 16
-#define E1000_MDIC_PHY_MASK  0x03E00000
-#define E1000_MDIC_PHY_SHIFT 21
-#define E1000_MDIC_OP_WRITE  0x04000000
-#define E1000_MDIC_OP_READ   0x08000000
-#define E1000_MDIC_READY     0x10000000
-#define E1000_MDIC_INT_EN    0x20000000
-#define E1000_MDIC_ERROR     0x40000000
-#define E1000_MDIC_DEST      0x80000000
-
-/* Thermal Sensor */
-#define E1000_THSTAT_PWR_DOWN       0x00000001 /* Power Down Event */
-#define E1000_THSTAT_LINK_THROTTLE  0x00000002 /* Link Speed Throttle Event */
-
-/* Energy Efficient Ethernet */
-#define E1000_IPCNFG_EEE_1G_AN       0x00000008  /* EEE Enable 1G AN */
-#define E1000_IPCNFG_EEE_100M_AN     0x00000004  /* EEE Enable 100M AN */
-#define E1000_EEER_TX_LPI_EN         0x00010000  /* EEE Tx LPI Enable */
-#define E1000_EEER_RX_LPI_EN         0x00020000  /* EEE Rx LPI Enable */
-#define E1000_EEER_LPI_FC            0x00040000  /* EEE Enable on FC */
+/* Bits...
+ * 15-5: page
+ * 4-0: register offset
+ */
+#define GG82563_PAGE_SHIFT	5
+#define GG82563_REG(page, reg)	\
+	(((page) << GG82563_PAGE_SHIFT) | ((reg) & MAX_PHY_REG_ADDRESS))
+#define GG82563_MIN_ALT_REG	30
 
-/* SerDes Control */
-#define E1000_GEN_CTL_READY             0x80000000
-#define E1000_GEN_CTL_ADDRESS_SHIFT     8
-#define E1000_GEN_POLL_TIMEOUT          640
+/* GG82563 Specific Registers */
+#define GG82563_PHY_SPEC_CTRL		GG82563_REG(0, 16) /* PHY Spec Cntrl */
+#define GG82563_PHY_PAGE_SELECT		GG82563_REG(0, 22) /* Page Select */
+#define GG82563_PHY_SPEC_CTRL_2		GG82563_REG(0, 26) /* PHY Spec Cntrl2 */
+#define GG82563_PHY_PAGE_SELECT_ALT	GG82563_REG(0, 29) /* Alt Page Select */
 
-#define E1000_VFTA_ENTRY_SHIFT               5
-#define E1000_VFTA_ENTRY_MASK                0x7F
-#define E1000_VFTA_ENTRY_BIT_SHIFT_MASK      0x1F
+/* MAC Specific Control Register */
+#define GG82563_PHY_MAC_SPEC_CTRL	GG82563_REG(2, 21)
 
-/* DMA Coalescing register fields */
-#define E1000_PCIEMISC_LX_DECISION      0x00000080 /* Lx power decision based
-                                                      on DMA coal */
+#define GG82563_PHY_DSP_DISTANCE	GG82563_REG(5, 26) /* DSP Distance */
+
+/* Page 193 - Port Control Registers */
+/* Kumeran Mode Control */
+#define GG82563_PHY_KMRN_MODE_CTRL	GG82563_REG(193, 16)
+#define GG82563_PHY_PWR_MGMT_CTRL	GG82563_REG(193, 20) /* Pwr Mgt Ctrl */
+
+/* Page 194 - KMRN Registers */
+#define GG82563_PHY_INBAND_CTRL		GG82563_REG(194, 18) /* Inband Ctrl */
+
+/* MDI Control */
+#define E1000_MDIC_REG_MASK	0x001F0000
+#define E1000_MDIC_REG_SHIFT	16
+#define E1000_MDIC_PHY_MASK	0x03E00000
+#define E1000_MDIC_PHY_SHIFT	21
+#define E1000_MDIC_OP_WRITE	0x04000000
+#define E1000_MDIC_OP_READ	0x08000000
+#define E1000_MDIC_READY	0x10000000
+#define E1000_MDIC_ERROR	0x40000000
+#define E1000_MDIC_DEST		0x80000000
+
+/* SerDes Control */
+#define E1000_GEN_CTL_READY		0x80000000
+#define E1000_GEN_CTL_ADDRESS_SHIFT	8
+#define E1000_GEN_POLL_TIMEOUT		640
+
+/* LinkSec register fields */
+#define E1000_LSECTXCAP_SUM_MASK	0x00FF0000
+#define E1000_LSECTXCAP_SUM_SHIFT	16
+#define E1000_LSECRXCAP_SUM_MASK	0x00FF0000
+#define E1000_LSECRXCAP_SUM_SHIFT	16
+
+#define E1000_LSECTXCTRL_EN_MASK	0x00000003
+#define E1000_LSECTXCTRL_DISABLE	0x0
+#define E1000_LSECTXCTRL_AUTH		0x1
+#define E1000_LSECTXCTRL_AUTH_ENCRYPT	0x2
+#define E1000_LSECTXCTRL_AISCI		0x00000020
+#define E1000_LSECTXCTRL_PNTHRSH_MASK	0xFFFFFF00
+#define E1000_LSECTXCTRL_RSV_MASK	0x000000D8
+
+#define E1000_LSECRXCTRL_EN_MASK	0x0000000C
+#define E1000_LSECRXCTRL_EN_SHIFT	2
+#define E1000_LSECRXCTRL_DISABLE	0x0
+#define E1000_LSECRXCTRL_CHECK		0x1
+#define E1000_LSECRXCTRL_STRICT		0x2
+#define E1000_LSECRXCTRL_DROP		0x3
+#define E1000_LSECRXCTRL_PLSH		0x00000040
+#define E1000_LSECRXCTRL_RP		0x00000080
+#define E1000_LSECRXCTRL_RSV_MASK	0xFFFFFF33
 
 /* Tx Rate-Scheduler Config fields */
 #define E1000_RTTBCNRC_RS_ENA		0x80000000
@@ -831,4 +1321,68 @@
 #define E1000_RTTBCNRC_RF_INT_MASK	\
 	(E1000_RTTBCNRC_RF_DEC_MASK << E1000_RTTBCNRC_RF_INT_SHIFT)
 
-#endif
+/* DMA Coalescing register fields */
+/* DMA Coalescing Watchdog Timer */
+#define E1000_DMACR_DMACWT_MASK		0x00003FFF
+/* DMA Coalescing Rx Threshold */
+#define E1000_DMACR_DMACTHR_MASK	0x00FF0000
+#define E1000_DMACR_DMACTHR_SHIFT	16
+/* Lx when no PCIe transactions */
+#define E1000_DMACR_DMAC_LX_MASK	0x30000000
+#define E1000_DMACR_DMAC_LX_SHIFT	28
+#define E1000_DMACR_DMAC_EN		0x80000000 /* Enable DMA Coalescing */
+/* DMA Coalescing BMC-to-OS Watchdog Enable */
+#define E1000_DMACR_DC_BMC2OSW_EN	0x00008000
+
+/* DMA Coalescing Transmit Threshold */
+#define E1000_DMCTXTH_DMCTTHR_MASK	0x00000FFF
+
+#define E1000_DMCTLX_TTLX_MASK		0x00000FFF /* Time to LX request */
+
+/* Rx Traffic Rate Threshold */
+#define E1000_DMCRTRH_UTRESH_MASK	0x0007FFFF
+/* Rx packet rate in current window */
+#define E1000_DMCRTRH_LRPRCW		0x80000000
+
+/* DMA Coal Rx Traffic Current Count */
+#define E1000_DMCCNT_CCOUNT_MASK	0x01FFFFFF
+
+/* Flow ctrl Rx Threshold High val */
+#define E1000_FCRTC_RTH_COAL_MASK	0x0003FFF0
+#define E1000_FCRTC_RTH_COAL_SHIFT	4
+/* Lx power decision based on DMA coal */
+#define E1000_PCIEMISC_LX_DECISION	0x00000080
+
+#define E1000_RXPBS_CFG_TS_EN		0x80000000 /* Timestamp in Rx buffer */
+#define E1000_RXPBS_SIZE_I210_MASK	0x0000003F /* Rx packet buffer size */
+#define E1000_TXPB0S_SIZE_I210_MASK	0x0000003F /* Tx packet buffer 0 size */
+
+/* Proxy Filter Control */
+#define E1000_PROXYFC_D0		0x00000001 /* Enable offload in D0 */
+#define E1000_PROXYFC_EX		0x00000004 /* Directed exact proxy */
+#define E1000_PROXYFC_MC		0x00000008 /* Directed MC Proxy */
+#define E1000_PROXYFC_BC		0x00000010 /* Broadcast Proxy Enable */
+#define E1000_PROXYFC_ARP_DIRECTED	0x00000020 /* Directed ARP Proxy Ena */
+#define E1000_PROXYFC_IPV4		0x00000040 /* Directed IPv4 Enable */
+#define E1000_PROXYFC_IPV6		0x00000080 /* Directed IPv6 Enable */
+#define E1000_PROXYFC_NS		0x00000200 /* IPv6 Neighbor Solicitation */
+#define E1000_PROXYFC_ARP		0x00000800 /* ARP Request Proxy Ena */
+/* Proxy Status */
+#define E1000_PROXYS_CLEAR		0xFFFFFFFF /* Clear */
+
+/* Firmware Status */
+#define E1000_FWSTS_FWRI		0x80000000 /* FW Reset Indication */
+/* VF Control */
+#define E1000_VTCTRL_RST		0x04000000 /* Reset VF */
+
+#define E1000_STATUS_LAN_ID_MASK	0x00000000C /* Mask for Lan ID field */
+/* Lan ID bit field offset in status register */
+#define E1000_STATUS_LAN_ID_OFFSET	2
+#define E1000_VFTA_ENTRIES		128
+#ifndef E1000_UNUSEDARG
+#define E1000_UNUSEDARG
+#endif /* E1000_UNUSEDARG */
+#ifndef ERROR_REPORT
+#define ERROR_REPORT(fmt)	do { } while (0)
+#endif /* ERROR_REPORT */
+#endif /* _E1000_DEFINES_H_ */
diff --git a/drivers/net/igb/e1000_hw.h b/drivers/net/igb/e1000_hw.h
index 4519a1367170..0c4c1fb49dac 100644
--- a/drivers/net/igb/e1000_hw.h
+++ b/drivers/net/igb/e1000_hw.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2007-2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -28,54 +28,68 @@
 #ifndef _E1000_HW_H_
 #define _E1000_HW_H_
 
-#include <linux/types.h>
-#include <linux/delay.h>
-#include <linux/io.h>
-#include <linux/netdevice.h>
-
+#include "e1000_osdep.h"
 #include "e1000_regs.h"
 #include "e1000_defines.h"
 
 struct e1000_hw;
 
-#define E1000_DEV_ID_82576                    0x10C9
-#define E1000_DEV_ID_82576_FIBER              0x10E6
-#define E1000_DEV_ID_82576_SERDES             0x10E7
-#define E1000_DEV_ID_82576_QUAD_COPPER        0x10E8
-#define E1000_DEV_ID_82576_QUAD_COPPER_ET2    0x1526
-#define E1000_DEV_ID_82576_NS                 0x150A
-#define E1000_DEV_ID_82576_NS_SERDES          0x1518
-#define E1000_DEV_ID_82576_SERDES_QUAD        0x150D
-#define E1000_DEV_ID_82575EB_COPPER           0x10A7
-#define E1000_DEV_ID_82575EB_FIBER_SERDES     0x10A9
-#define E1000_DEV_ID_82575GB_QUAD_COPPER      0x10D6
-#define E1000_DEV_ID_82580_COPPER             0x150E
-#define E1000_DEV_ID_82580_FIBER              0x150F
-#define E1000_DEV_ID_82580_SERDES             0x1510
-#define E1000_DEV_ID_82580_SGMII              0x1511
-#define E1000_DEV_ID_82580_COPPER_DUAL        0x1516
-#define E1000_DEV_ID_82580_QUAD_FIBER         0x1527
-#define E1000_DEV_ID_DH89XXCC_SGMII           0x0438
-#define E1000_DEV_ID_DH89XXCC_SERDES          0x043A
-#define E1000_DEV_ID_DH89XXCC_BACKPLANE       0x043C
-#define E1000_DEV_ID_DH89XXCC_SFP             0x0440
-#define E1000_DEV_ID_I350_COPPER              0x1521
-#define E1000_DEV_ID_I350_FIBER               0x1522
-#define E1000_DEV_ID_I350_SERDES              0x1523
-#define E1000_DEV_ID_I350_SGMII               0x1524
-
-#define E1000_REVISION_2 2
-#define E1000_REVISION_4 4
-
-#define E1000_FUNC_0     0
-#define E1000_FUNC_1     1
-#define E1000_FUNC_2     2
-#define E1000_FUNC_3     3
-
-#define E1000_ALT_MAC_ADDRESS_OFFSET_LAN0   0
-#define E1000_ALT_MAC_ADDRESS_OFFSET_LAN1   3
-#define E1000_ALT_MAC_ADDRESS_OFFSET_LAN2   6
-#define E1000_ALT_MAC_ADDRESS_OFFSET_LAN3   9
+#define E1000_DEV_ID_82576			0x10C9
+#define E1000_DEV_ID_82576_FIBER		0x10E6
+#define E1000_DEV_ID_82576_SERDES		0x10E7
+#define E1000_DEV_ID_82576_QUAD_COPPER		0x10E8
+#define E1000_DEV_ID_82576_QUAD_COPPER_ET2	0x1526
+#define E1000_DEV_ID_82576_NS			0x150A
+#define E1000_DEV_ID_82576_NS_SERDES		0x1518
+#define E1000_DEV_ID_82576_SERDES_QUAD		0x150D
+#define E1000_DEV_ID_82575EB_COPPER		0x10A7
+#define E1000_DEV_ID_82575EB_FIBER_SERDES	0x10A9
+#define E1000_DEV_ID_82575GB_QUAD_COPPER	0x10D6
+#define E1000_DEV_ID_82580_COPPER		0x150E
+#define E1000_DEV_ID_82580_FIBER		0x150F
+#define E1000_DEV_ID_82580_SERDES		0x1510
+#define E1000_DEV_ID_82580_SGMII		0x1511
+#define E1000_DEV_ID_82580_COPPER_DUAL		0x1516
+#define E1000_DEV_ID_82580_QUAD_FIBER		0x1527
+#define E1000_DEV_ID_I350_COPPER		0x1521
+#define E1000_DEV_ID_I350_FIBER			0x1522
+#define E1000_DEV_ID_I350_SERDES		0x1523
+#define E1000_DEV_ID_I350_SGMII			0x1524
+#define E1000_DEV_ID_I350_DA4			0x1546
+#define E1000_DEV_ID_I210_TOOLS_ONLY		0x1531
+#define E1000_DEV_ID_I211_TOOLS_ONLY		0x1532
+#define E1000_DEV_ID_I210_COPPER		0x1533
+#define E1000_DEV_ID_I210_COPPER_OEM1		0x1534
+#define E1000_DEV_ID_I210_COPPER_IT		0x1535
+#define E1000_DEV_ID_I210_FIBER			0x1536
+#define E1000_DEV_ID_I210_SERDES		0x1537
+#define E1000_DEV_ID_I210_SGMII			0x1538
+#define E1000_DEV_ID_I210_COPPER_FLASHLESS	0x157B
+#define E1000_DEV_ID_I210_SERDES_FLASHLESS	0x157C
+#define E1000_DEV_ID_I211_COPPER		0x1539
+#define E1000_DEV_ID_I354_BACKPLANE_1GBPS	0x1F40
+#define E1000_DEV_ID_I354_SGMII			0x1F41
+#define E1000_DEV_ID_I354_BACKPLANE_2_5GBPS	0x1F45
+#define E1000_DEV_ID_DH89XXCC_SGMII		0x0438
+#define E1000_DEV_ID_DH89XXCC_SERDES		0x043A
+#define E1000_DEV_ID_DH89XXCC_BACKPLANE		0x043C
+#define E1000_DEV_ID_DH89XXCC_SFP		0x0440
+
+#define E1000_REVISION_0	0
+#define E1000_REVISION_1	1
+#define E1000_REVISION_2	2
+#define E1000_REVISION_3	3
+#define E1000_REVISION_4	4
+
+#define E1000_FUNC_0		0
+#define E1000_FUNC_1		1
+#define E1000_FUNC_2		2
+#define E1000_FUNC_3		3
+
+#define E1000_ALT_MAC_ADDRESS_OFFSET_LAN0	0
+#define E1000_ALT_MAC_ADDRESS_OFFSET_LAN1	3
+#define E1000_ALT_MAC_ADDRESS_OFFSET_LAN2	6
+#define E1000_ALT_MAC_ADDRESS_OFFSET_LAN3	9
 
 enum e1000_mac_type {
 	e1000_undefined = 0,
@@ -83,13 +97,17 @@ enum e1000_mac_type {
 	e1000_82576,
 	e1000_82580,
 	e1000_i350,
+	e1000_i354,
+	e1000_i210,
+	e1000_i211,
 	e1000_num_macs  /* List is 1-based, so subtract 1 for true count. */
 };
 
 enum e1000_media_type {
 	e1000_media_type_unknown = 0,
 	e1000_media_type_copper = 1,
-	e1000_media_type_internal_serdes = 2,
+	e1000_media_type_fiber = 2,
+	e1000_media_type_internal_serdes = 3,
 	e1000_num_media_types
 };
 
@@ -98,6 +116,7 @@ enum e1000_nvm_type {
 	e1000_nvm_none,
 	e1000_nvm_eeprom_spi,
 	e1000_nvm_flash_hw,
+	e1000_nvm_invm,
 	e1000_nvm_flash_sw
 };
 
@@ -117,6 +136,8 @@ enum e1000_phy_type {
 	e1000_phy_igp_3,
 	e1000_phy_ife,
 	e1000_phy_82580,
+	e1000_phy_vf,
+	e1000_phy_i210,
 };
 
 enum e1000_bus_type {
@@ -170,6 +191,177 @@ enum e1000_fc_mode {
 	e1000_fc_default = 0xFF
 };
 
+enum e1000_ms_type {
+	e1000_ms_hw_default = 0,
+	e1000_ms_force_master,
+	e1000_ms_force_slave,
+	e1000_ms_auto
+};
+
+enum e1000_smart_speed {
+	e1000_smart_speed_default = 0,
+	e1000_smart_speed_on,
+	e1000_smart_speed_off
+};
+
+enum e1000_serdes_link_state {
+	e1000_serdes_link_down = 0,
+	e1000_serdes_link_autoneg_progress,
+	e1000_serdes_link_autoneg_complete,
+	e1000_serdes_link_forced_up
+};
+
+#ifndef __le16
+#define __le16 u16
+#endif
+#ifndef __le32
+#define __le32 u32
+#endif
+#ifndef __le64
+#define __le64 u64
+#endif
+/* Receive Descriptor */
+struct e1000_rx_desc {
+	__le64 buffer_addr; /* Address of the descriptor's data buffer */
+	__le16 length;      /* Length of data DMAed into data buffer */
+	__le16 csum; /* Packet checksum */
+	u8  status;  /* Descriptor status */
+	u8  errors;  /* Descriptor Errors */
+	__le16 special;
+};
+
+/* Receive Descriptor - Extended */
+union e1000_rx_desc_extended {
+	struct {
+		__le64 buffer_addr;
+		__le64 reserved;
+	} read;
+	struct {
+		struct {
+			__le32 mrq; /* Multiple Rx Queues */
+			union {
+				__le32 rss; /* RSS Hash */
+				struct {
+					__le16 ip_id;  /* IP id */
+					__le16 csum;   /* Packet Checksum */
+				} csum_ip;
+			} hi_dword;
+		} lower;
+		struct {
+			__le32 status_error;  /* ext status/error */
+			__le16 length;
+			__le16 vlan; /* VLAN tag */
+		} upper;
+	} wb;  /* writeback */
+};
+
+#define MAX_PS_BUFFERS 4
+
+/* Number of packet split data buffers (not including the header buffer) */
+#define PS_PAGE_BUFFERS	(MAX_PS_BUFFERS - 1)
+
+/* Receive Descriptor - Packet Split */
+union e1000_rx_desc_packet_split {
+	struct {
+		/* one buffer for protocol header(s), three data buffers */
+		__le64 buffer_addr[MAX_PS_BUFFERS];
+	} read;
+	struct {
+		struct {
+			__le32 mrq;  /* Multiple Rx Queues */
+			union {
+				__le32 rss; /* RSS Hash */
+				struct {
+					__le16 ip_id;    /* IP id */
+					__le16 csum;     /* Packet Checksum */
+				} csum_ip;
+			} hi_dword;
+		} lower;
+		struct {
+			__le32 status_error;  /* ext status/error */
+			__le16 length0;  /* length of buffer 0 */
+			__le16 vlan;  /* VLAN tag */
+		} middle;
+		struct {
+			__le16 header_status;
+			/* length of buffers 1-3 */
+			__le16 length[PS_PAGE_BUFFERS];
+		} upper;
+		__le64 reserved;
+	} wb; /* writeback */
+};
+
+/* Transmit Descriptor */
+struct e1000_tx_desc {
+	__le64 buffer_addr;   /* Address of the descriptor's data buffer */
+	union {
+		__le32 data;
+		struct {
+			__le16 length;  /* Data buffer length */
+			u8 cso;  /* Checksum offset */
+			u8 cmd;  /* Descriptor control */
+		} flags;
+	} lower;
+	union {
+		__le32 data;
+		struct {
+			u8 status; /* Descriptor status */
+			u8 css;  /* Checksum start */
+			__le16 special;
+		} fields;
+	} upper;
+};
+
+/* Offload Context Descriptor */
+struct e1000_context_desc {
+	union {
+		__le32 ip_config;
+		struct {
+			u8 ipcss;  /* IP checksum start */
+			u8 ipcso;  /* IP checksum offset */
+			__le16 ipcse;  /* IP checksum end */
+		} ip_fields;
+	} lower_setup;
+	union {
+		__le32 tcp_config;
+		struct {
+			u8 tucss;  /* TCP checksum start */
+			u8 tucso;  /* TCP checksum offset */
+			__le16 tucse;  /* TCP checksum end */
+		} tcp_fields;
+	} upper_setup;
+	__le32 cmd_and_length;
+	union {
+		__le32 data;
+		struct {
+			u8 status;  /* Descriptor status */
+			u8 hdr_len;  /* Header length */
+			__le16 mss;  /* Maximum segment size */
+		} fields;
+	} tcp_seg_setup;
+};
+
+/* Offload data descriptor */
+struct e1000_data_desc {
+	__le64 buffer_addr;  /* Address of the descriptor's buffer address */
+	union {
+		__le32 data;
+		struct {
+			__le16 length;  /* Data buffer length */
+			u8 typ_len_ext;
+			u8 cmd;
+		} flags;
+	} lower;
+	union {
+		__le32 data;
+		struct {
+			u8 status;  /* Descriptor status */
+			u8 popts;  /* Packet Options */
+			__le16 special;
+		} fields;
+	} upper;
+};
+
 /* Statistics counters collected by the MAC */
 struct e1000_hw_stats {
 	u64 crcerrs;
@@ -254,6 +446,7 @@ struct e1000_hw_stats {
 	u64 b2ogprc;
 };
 
+
 struct e1000_phy_stats {
 	u32 idle_errors;
 	u32 receive_errors;
@@ -278,7 +471,7 @@ struct e1000_host_command_header {
 	u8 checksum;
 };
 
-#define E1000_HI_MAX_DATA_LENGTH     252
+#define E1000_HI_MAX_DATA_LENGTH	252
 struct e1000_host_command_info {
 	struct e1000_host_command_header command_header;
 	u8 command_data[E1000_HI_MAX_DATA_LENGTH];
@@ -293,7 +486,7 @@ struct e1000_host_mng_command_header {
 	u16 command_length;
 };
 
-#define E1000_HI_MAX_MNG_DATA_LENGTH 0x6F8
+#define E1000_HI_MAX_MNG_DATA_LENGTH	0x6F8
 struct e1000_host_mng_command_info {
 	struct e1000_host_mng_command_header command_header;
 	u8 command_data[E1000_HI_MAX_MNG_DATA_LENGTH];
@@ -302,67 +495,130 @@ struct e1000_host_mng_command_info {
 #include "e1000_mac.h"
 #include "e1000_phy.h"
 #include "e1000_nvm.h"
+#include "e1000_manage.h"
 #include "e1000_mbx.h"
 
+/* Function pointers for the MAC. */
 struct e1000_mac_operations {
+	s32  (*init_params)(struct e1000_hw *);
+	s32  (*id_led_init)(struct e1000_hw *);
+	s32  (*blink_led)(struct e1000_hw *);
+	bool (*check_mng_mode)(struct e1000_hw *);
 	s32  (*check_for_link)(struct e1000_hw *);
+	s32  (*cleanup_led)(struct e1000_hw *);
+	void (*clear_hw_cntrs)(struct e1000_hw *);
+	void (*clear_vfta)(struct e1000_hw *);
+	s32  (*get_bus_info)(struct e1000_hw *);
+	void (*set_lan_id)(struct e1000_hw *);
+	s32  (*get_link_up_info)(struct e1000_hw *, u16 *, u16 *);
+	s32  (*led_on)(struct e1000_hw *);
+	s32  (*led_off)(struct e1000_hw *);
+	void (*update_mc_addr_list)(struct e1000_hw *, u8 *, u32);
 	s32  (*reset_hw)(struct e1000_hw *);
 	s32  (*init_hw)(struct e1000_hw *);
-	bool (*check_mng_mode)(struct e1000_hw *);
+	void (*shutdown_serdes)(struct e1000_hw *);
+	void (*power_up_serdes)(struct e1000_hw *);
+	s32  (*setup_link)(struct e1000_hw *);
 	s32  (*setup_physical_interface)(struct e1000_hw *);
-	void (*rar_set)(struct e1000_hw *, u8 *, u32);
+	s32  (*setup_led)(struct e1000_hw *);
+	void (*write_vfta)(struct e1000_hw *, u32, u32);
+	void (*config_collision_dist)(struct e1000_hw *);
+	void (*rar_set)(struct e1000_hw *, u8*, u32);
 	s32  (*read_mac_addr)(struct e1000_hw *);
-	s32  (*get_speed_and_duplex)(struct e1000_hw *, u16 *, u16 *);
-};
-
+	s32  (*validate_mdi_setting)(struct e1000_hw *);
+	s32 (*get_thermal_sensor_data)(struct e1000_hw *);
+	s32 (*init_thermal_sensor_thresh)(struct e1000_hw *);
+	s32  (*acquire_swfw_sync)(struct e1000_hw *, u16);
+	void (*release_swfw_sync)(struct e1000_hw *, u16);
+};
+
+/* When to use various PHY register access functions:
+ *
+ *                 Func   Caller
+ *   Function      Does   Does    When to use
+ *   ~~~~~~~~~~~~  ~~~~~  ~~~~~~  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ *   X_reg         L,P,A  n/a     for simple PHY reg accesses
+ *   X_reg_locked  P,A    L       for multiple accesses of different regs
+ *                                on different pages
+ *   X_reg_page    A      L,P     for multiple accesses of different regs
+ *                                on the same page
+ *
+ * Where X=[read|write], L=locking, P=sets page, A=register access
+ *
+ */
 struct e1000_phy_operations {
+	s32  (*init_params)(struct e1000_hw *);
 	s32  (*acquire)(struct e1000_hw *);
 	s32  (*check_polarity)(struct e1000_hw *);
 	s32  (*check_reset_block)(struct e1000_hw *);
+	s32  (*commit)(struct e1000_hw *);
 	s32  (*force_speed_duplex)(struct e1000_hw *);
 	s32  (*get_cfg_done)(struct e1000_hw *hw);
 	s32  (*get_cable_length)(struct e1000_hw *);
-	s32  (*get_phy_info)(struct e1000_hw *);
+	s32  (*get_info)(struct e1000_hw *);
+	s32  (*set_page)(struct e1000_hw *, u16);
 	s32  (*read_reg)(struct e1000_hw *, u32, u16 *);
+	s32  (*read_reg_locked)(struct e1000_hw *, u32, u16 *);
+	s32  (*read_reg_page)(struct e1000_hw *, u32, u16 *);
 	void (*release)(struct e1000_hw *);
 	s32  (*reset)(struct e1000_hw *);
 	s32  (*set_d0_lplu_state)(struct e1000_hw *, bool);
 	s32  (*set_d3_lplu_state)(struct e1000_hw *, bool);
 	s32  (*write_reg)(struct e1000_hw *, u32, u16);
+	s32  (*write_reg_locked)(struct e1000_hw *, u32, u16);
+	s32  (*write_reg_page)(struct e1000_hw *, u32, u16);
+	void (*power_up)(struct e1000_hw *);
+	void (*power_down)(struct e1000_hw *);
+	s32 (*read_i2c_byte)(struct e1000_hw *, u8, u8, u8 *);
+	s32 (*write_i2c_byte)(struct e1000_hw *, u8, u8, u8);
 };
 
+/* Function pointers for the NVM. */
 struct e1000_nvm_operations {
+	s32  (*init_params)(struct e1000_hw *);
 	s32  (*acquire)(struct e1000_hw *);
 	s32  (*read)(struct e1000_hw *, u16, u16, u16 *);
 	void (*release)(struct e1000_hw *);
-	s32  (*write)(struct e1000_hw *, u16, u16, u16 *);
+	void (*reload)(struct e1000_hw *);
 	s32  (*update)(struct e1000_hw *);
+	s32  (*valid_led_default)(struct e1000_hw *, u16 *);
 	s32  (*validate)(struct e1000_hw *);
+	s32  (*write)(struct e1000_hw *, u16, u16, u16 *);
 };
 
-struct e1000_info {
-	s32 (*get_invariants)(struct e1000_hw *);
-	struct e1000_mac_operations *mac_ops;
-	struct e1000_phy_operations *phy_ops;
-	struct e1000_nvm_operations *nvm_ops;
+#define E1000_MAX_SENSORS		3
+
+struct e1000_thermal_diode_data {
+	u8 location;
+	u8 temp;
+	u8 caution_thresh;
+	u8 max_op_thresh;
 };
 
-extern const struct e1000_info e1000_82575_info;
+struct e1000_thermal_sensor_data {
+	struct e1000_thermal_diode_data sensor[E1000_MAX_SENSORS];
+};
 
 struct e1000_mac_info {
 	struct e1000_mac_operations ops;
-
-	u8 addr[6];
-	u8 perm_addr[6];
+	u8 addr[ETH_ADDR_LEN];
+	u8 perm_addr[ETH_ADDR_LEN];
 
 	enum e1000_mac_type type;
 
+	u32 collision_delta;
 	u32 ledctl_default;
 	u32 ledctl_mode1;
 	u32 ledctl_mode2;
 	u32 mc_filter_type;
+	u32 tx_packet_delta;
 	u32 txcw;
 
+	u16 current_ifs_val;
+	u16 ifs_max_val;
+	u16 ifs_min_val;
+	u16 ifs_ratio;
+	u16 ifs_step_size;
 	u16 mta_reg_count;
 	u16 uta_reg_count;
 
@@ -374,22 +630,21 @@ struct e1000_mac_info {
 	u8  forced_speed_duplex;
 
 	bool adaptive_ifs;
+	bool has_fwsm;
 	bool arc_subsystem_valid;
 	bool asf_firmware_present;
 	bool autoneg;
 	bool autoneg_failed;
-	bool disable_hw_init_bits;
 	bool get_link_status;
-	bool ifs_params_forced;
 	bool in_ifs_mode;
-	bool report_tx_early;
+	enum e1000_serdes_link_state serdes_link_state;
 	bool serdes_has_link;
 	bool tx_pkt_filtering;
+	struct e1000_thermal_sensor_data thermal_sensor_data;
 };
 
 struct e1000_phy_info {
 	struct e1000_phy_operations ops;
-
 	enum e1000_phy_type type;
 
 	enum e1000_1000t_rx_status local_rx;
@@ -442,20 +697,19 @@ struct e1000_bus_info {
 	enum e1000_bus_speed speed;
 	enum e1000_bus_width width;
 
-	u32 snoop;
-
 	u16 func;
 	u16 pci_cmd_word;
 };
 
 struct e1000_fc_info {
-	u32 high_water;     /* Flow control high-water mark */
-	u32 low_water;      /* Flow control low-water mark */
-	u16 pause_time;     /* Flow control pause timer */
-	bool send_xon;      /* Flow control send XON */
-	bool strict_ieee;   /* Strict IEEE mode */
-	enum e1000_fc_mode current_mode; /* Type of flow control */
-	enum e1000_fc_mode requested_mode;
+	u32 high_water;  /* Flow control high-water mark */
+	u32 low_water;  /* Flow control low-water mark */
+	u16 pause_time;  /* Flow control pause timer */
+	u16 refresh_time;  /* Flow control refresh timer */
+	bool send_xon;  /* Flow control send XON */
+	bool strict_ieee;  /* Strict IEEE mode */
+	enum e1000_fc_mode current_mode;  /* FC mode in effect */
+	enum e1000_fc_mode requested_mode;  /* FC mode requested by caller */
 };
 
 struct e1000_mbx_operations {
@@ -490,6 +744,17 @@ struct e1000_dev_spec_82575 {
 	bool sgmii_active;
 	bool global_device_reset;
 	bool eee_disable;
+	bool module_plugged;
+	bool clear_semaphore_once;
+	u32 mtu;
+	struct sfp_e1000_flags eth_flags;
+	u8 media_port;
+	bool media_changed;
+};
+
+struct e1000_dev_spec_vf {
+	u32 vf_number;
+	u32 v2p_mailbox;
 };
 
 struct e1000_hw {
@@ -508,7 +773,8 @@ struct e1000_hw {
 	struct e1000_host_mng_dhcp_cookie mng_cookie;
 
 	union {
-		struct e1000_dev_spec_82575	_82575;
+		struct e1000_dev_spec_82575 _82575;
+		struct e1000_dev_spec_vf vf;
 	} dev_spec;
 
 	u16 device_id;
@@ -519,11 +785,11 @@ struct e1000_hw {
 	u8  revision_id;
 };
 
-extern struct net_device *igb_get_hw_dev(struct e1000_hw *hw);
-#define hw_dbg(format, arg...) \
-	netdev_dbg(igb_get_hw_dev(hw), format, ##arg)
+#include "e1000_82575.h"
+#include "e1000_i210.h"
 
 /* These functions must be implemented by drivers */
-s32  igb_read_pcie_cap_reg(struct e1000_hw *hw, u32 reg, u16 *value);
-s32  igb_write_pcie_cap_reg(struct e1000_hw *hw, u32 reg, u16 *value);
-#endif /* _E1000_HW_H_ */
+s32  e1000_read_pcie_cap_reg(struct e1000_hw *hw, u32 reg, u16 *value);
+s32  e1000_write_pcie_cap_reg(struct e1000_hw *hw, u32 reg, u16 *value);
+
+#endif
diff --git a/drivers/net/igb/e1000_i210.c b/drivers/net/igb/e1000_i210.c
new file mode 100644
index 000000000000..1e9f3e6e6b42
--- /dev/null
+++ b/drivers/net/igb/e1000_i210.c
@@ -0,0 +1,909 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+#include "e1000_api.h"
+
+
+static s32 e1000_acquire_nvm_i210(struct e1000_hw *hw);
+static void e1000_release_nvm_i210(struct e1000_hw *hw);
+static s32 e1000_get_hw_semaphore_i210(struct e1000_hw *hw);
+static s32 e1000_write_nvm_srwr(struct e1000_hw *hw, u16 offset, u16 words,
+				u16 *data);
+static s32 e1000_pool_flash_update_done_i210(struct e1000_hw *hw);
+static s32 e1000_valid_led_default_i210(struct e1000_hw *hw, u16 *data);
+
+/**
+ *  e1000_acquire_nvm_i210 - Request for access to EEPROM
+ *  @hw: pointer to the HW structure
+ *
+ *  Acquire the necessary semaphores for exclusive access to the EEPROM.
+ *  Set the EEPROM access request bit and wait for EEPROM access grant bit.
+ *  Return successful if access grant bit set, else clear the request for
+ *  EEPROM access and return -E1000_ERR_NVM (-1).
+ **/
+static s32 e1000_acquire_nvm_i210(struct e1000_hw *hw)
+{
+	s32 ret_val;
+
+	DEBUGFUNC("e1000_acquire_nvm_i210");
+
+	ret_val = e1000_acquire_swfw_sync_i210(hw, E1000_SWFW_EEP_SM);
+
+	return ret_val;
+}
+
+/**
+ *  e1000_release_nvm_i210 - Release exclusive access to EEPROM
+ *  @hw: pointer to the HW structure
+ *
+ *  Stop any current commands to the EEPROM and clear the EEPROM request bit,
+ *  then release the semaphores acquired.
+ **/
+static void e1000_release_nvm_i210(struct e1000_hw *hw)
+{
+	DEBUGFUNC("e1000_release_nvm_i210");
+
+	e1000_release_swfw_sync_i210(hw, E1000_SWFW_EEP_SM);
+}
+
+/**
+ *  e1000_acquire_swfw_sync_i210 - Acquire SW/FW semaphore
+ *  @hw: pointer to the HW structure
+ *  @mask: specifies which semaphore to acquire
+ *
+ *  Acquire the SW/FW semaphore to access the PHY or NVM.  The mask
+ *  will also specify which port we're acquiring the lock for.
+ **/
+s32 e1000_acquire_swfw_sync_i210(struct e1000_hw *hw, u16 mask)
+{
+	u32 swfw_sync;
+	u32 swmask = mask;
+	u32 fwmask = mask << 16;
+	s32 ret_val = E1000_SUCCESS;
+	s32 i = 0, timeout = 200; /* FIXME: find real value to use here */
+
+	DEBUGFUNC("e1000_acquire_swfw_sync_i210");
+
+	while (i < timeout) {
+		if (e1000_get_hw_semaphore_i210(hw)) {
+			ret_val = -E1000_ERR_SWFW_SYNC;
+			goto out;
+		}
+
+		swfw_sync = E1000_READ_REG(hw, E1000_SW_FW_SYNC);
+		if (!(swfw_sync & (fwmask | swmask)))
+			break;
+
+		/*
+		 * Firmware currently using resource (fwmask)
+		 * or other software thread using resource (swmask)
+		 */
+		e1000_put_hw_semaphore_generic(hw);
+		msec_delay_irq(5);
+		i++;
+	}
+
+	if (i == timeout) {
+		DEBUGOUT("Driver can't access resource, SW_FW_SYNC timeout.\n");
+		ret_val = -E1000_ERR_SWFW_SYNC;
+		goto out;
+	}
+
+	swfw_sync |= swmask;
+	E1000_WRITE_REG(hw, E1000_SW_FW_SYNC, swfw_sync);
+
+	e1000_put_hw_semaphore_generic(hw);
+
+out:
+	return ret_val;
+}
+
+/**
+ *  e1000_release_swfw_sync_i210 - Release SW/FW semaphore
+ *  @hw: pointer to the HW structure
+ *  @mask: specifies which semaphore to acquire
+ *
+ *  Release the SW/FW semaphore used to access the PHY or NVM.  The mask
+ *  will also specify which port we're releasing the lock for.
+ **/
+void e1000_release_swfw_sync_i210(struct e1000_hw *hw, u16 mask)
+{
+	u32 swfw_sync;
+
+	DEBUGFUNC("e1000_release_swfw_sync_i210");
+
+	while (e1000_get_hw_semaphore_i210(hw) != E1000_SUCCESS)
+		; /* Empty */
+
+	swfw_sync = E1000_READ_REG(hw, E1000_SW_FW_SYNC);
+	swfw_sync &= ~mask;
+	E1000_WRITE_REG(hw, E1000_SW_FW_SYNC, swfw_sync);
+
+	e1000_put_hw_semaphore_generic(hw);
+}
+
+/**
+ *  e1000_get_hw_semaphore_i210 - Acquire hardware semaphore
+ *  @hw: pointer to the HW structure
+ *
+ *  Acquire the HW semaphore to access the PHY or NVM
+ **/
+static s32 e1000_get_hw_semaphore_i210(struct e1000_hw *hw)
+{
+	u32 swsm;
+	s32 timeout = hw->nvm.word_size + 1;
+	s32 i = 0;
+
+	DEBUGFUNC("e1000_get_hw_semaphore_i210");
+
+	/* Get the SW semaphore */
+	while (i < timeout) {
+		swsm = E1000_READ_REG(hw, E1000_SWSM);
+		if (!(swsm & E1000_SWSM_SMBI))
+			break;
+
+		usec_delay(50);
+		i++;
+	}
+
+	if (i == timeout) {
+		/* In rare circumstances, the SW semaphore may already be held
+		 * unintentionally. Clear the semaphore once before giving up.
+		 */
+		if (hw->dev_spec._82575.clear_semaphore_once) {
+			hw->dev_spec._82575.clear_semaphore_once = false;
+			e1000_put_hw_semaphore_generic(hw);
+			for (i = 0; i < timeout; i++) {
+				swsm = E1000_READ_REG(hw, E1000_SWSM);
+				if (!(swsm & E1000_SWSM_SMBI))
+					break;
+
+				usec_delay(50);
+			}
+		}
+
+		/* If we do not have the semaphore here, we have to give up. */
+		if (i == timeout) {
+			DEBUGOUT("Driver can't access device - SMBI bit is set.\n");
+			return -E1000_ERR_NVM;
+		}
+	}
+
+	/* Get the FW semaphore. */
+	for (i = 0; i < timeout; i++) {
+		swsm = E1000_READ_REG(hw, E1000_SWSM);
+		E1000_WRITE_REG(hw, E1000_SWSM, swsm | E1000_SWSM_SWESMBI);
+
+		/* Semaphore acquired if bit latched */
+		if (E1000_READ_REG(hw, E1000_SWSM) & E1000_SWSM_SWESMBI)
+			break;
+
+		usec_delay(50);
+	}
+
+	if (i == timeout) {
+		/* Release semaphores */
+		e1000_put_hw_semaphore_generic(hw);
+		DEBUGOUT("Driver can't access the NVM\n");
+		return -E1000_ERR_NVM;
+	}
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_read_nvm_srrd_i210 - Reads Shadow Ram using EERD register
+ *  @hw: pointer to the HW structure
+ *  @offset: offset of word in the Shadow Ram to read
+ *  @words: number of words to read
+ *  @data: word read from the Shadow Ram
+ *
+ *  Reads a 16 bit word from the Shadow Ram using the EERD register.
+ *  Uses necessary synchronization semaphores.
+ **/
+s32 e1000_read_nvm_srrd_i210(struct e1000_hw *hw, u16 offset, u16 words,
+			     u16 *data)
+{
+	s32 status = E1000_SUCCESS;
+	u16 i, count;
+
+	DEBUGFUNC("e1000_read_nvm_srrd_i210");
+
+	/* We cannot hold synchronization semaphores for too long,
+	 * because of forceful takeover procedure. However it is more efficient
+	 * to read in bursts than synchronizing access for each word. */
+	for (i = 0; i < words; i += E1000_EERD_EEWR_MAX_COUNT) {
+		count = (words - i) / E1000_EERD_EEWR_MAX_COUNT > 0 ?
+			E1000_EERD_EEWR_MAX_COUNT : (words - i);
+		if (hw->nvm.ops.acquire(hw) == E1000_SUCCESS) {
+			status = e1000_read_nvm_eerd(hw, offset, count,
+						     data + i);
+			hw->nvm.ops.release(hw);
+		} else {
+			status = E1000_ERR_SWFW_SYNC;
+		}
+
+		if (status != E1000_SUCCESS)
+			break;
+	}
+
+	return status;
+}
+
+/**
+ *  e1000_write_nvm_srwr_i210 - Write to Shadow RAM using EEWR
+ *  @hw: pointer to the HW structure
+ *  @offset: offset within the Shadow RAM to be written to
+ *  @words: number of words to write
+ *  @data: 16 bit word(s) to be written to the Shadow RAM
+ *
+ *  Writes data to Shadow RAM at offset using EEWR register.
+ *
+ *  If e1000_update_nvm_checksum is not called after this function , the
+ *  data will not be committed to FLASH and also Shadow RAM will most likely
+ *  contain an invalid checksum.
+ *
+ *  If error code is returned, data and Shadow RAM may be inconsistent - buffer
+ *  partially written.
+ **/
+s32 e1000_write_nvm_srwr_i210(struct e1000_hw *hw, u16 offset, u16 words,
+			      u16 *data)
+{
+	s32 status = E1000_SUCCESS;
+	u16 i, count;
+
+	DEBUGFUNC("e1000_write_nvm_srwr_i210");
+
+	/* We cannot hold synchronization semaphores for too long,
+	 * because of forceful takeover procedure. However it is more efficient
+	 * to write in bursts than synchronizing access for each word. */
+	for (i = 0; i < words; i += E1000_EERD_EEWR_MAX_COUNT) {
+		count = (words - i) / E1000_EERD_EEWR_MAX_COUNT > 0 ?
+			E1000_EERD_EEWR_MAX_COUNT : (words - i);
+		if (hw->nvm.ops.acquire(hw) == E1000_SUCCESS) {
+			status = e1000_write_nvm_srwr(hw, offset, count,
+						      data + i);
+			hw->nvm.ops.release(hw);
+		} else {
+			status = E1000_ERR_SWFW_SYNC;
+		}
+
+		if (status != E1000_SUCCESS)
+			break;
+	}
+
+	return status;
+}
+
+/**
+ *  e1000_write_nvm_srwr - Write to Shadow Ram using EEWR
+ *  @hw: pointer to the HW structure
+ *  @offset: offset within the Shadow Ram to be written to
+ *  @words: number of words to write
+ *  @data: 16 bit word(s) to be written to the Shadow Ram
+ *
+ *  Writes data to Shadow Ram at offset using EEWR register.
+ *
+ *  If e1000_update_nvm_checksum is not called after this function , the
+ *  Shadow Ram will most likely contain an invalid checksum.
+ **/
+static s32 e1000_write_nvm_srwr(struct e1000_hw *hw, u16 offset, u16 words,
+				u16 *data)
+{
+	struct e1000_nvm_info *nvm = &hw->nvm;
+	u32 i, k, eewr = 0;
+	u32 attempts = 100000;
+	s32 ret_val = E1000_SUCCESS;
+
+	DEBUGFUNC("e1000_write_nvm_srwr");
+
+	/*
+	 * A check for invalid values:  offset too large, too many words,
+	 * too many words for the offset, and not enough words.
+	 */
+	if ((offset >= nvm->word_size) || (words > (nvm->word_size - offset)) ||
+	    (words == 0)) {
+		DEBUGOUT("nvm parameter(s) out of bounds\n");
+		ret_val = -E1000_ERR_NVM;
+		goto out;
+	}
+
+	for (i = 0; i < words; i++) {
+		eewr = ((offset+i) << E1000_NVM_RW_ADDR_SHIFT) |
+			(data[i] << E1000_NVM_RW_REG_DATA) |
+			E1000_NVM_RW_REG_START;
+
+		E1000_WRITE_REG(hw, E1000_SRWR, eewr);
+
+		for (k = 0; k < attempts; k++) {
+			if (E1000_NVM_RW_REG_DONE &
+			    E1000_READ_REG(hw, E1000_SRWR)) {
+				ret_val = E1000_SUCCESS;
+				break;
+			}
+			usec_delay(5);
+		}
+
+		if (ret_val != E1000_SUCCESS) {
+			DEBUGOUT("Shadow RAM write EEWR timed out\n");
+			break;
+		}
+	}
+
+out:
+	return ret_val;
+}
+
+/** e1000_read_invm_word_i210 - Reads OTP
+ *  @hw: pointer to the HW structure
+ *  @address: the word address (aka eeprom offset) to read
+ *  @data: pointer to the data read
+ *
+ *  Reads 16-bit words from the OTP. Return error when the word is not
+ *  stored in OTP.
+ **/
+static s32 e1000_read_invm_word_i210(struct e1000_hw *hw, u8 address, u16 *data)
+{
+	s32 status = -E1000_ERR_INVM_VALUE_NOT_FOUND;
+	u32 invm_dword;
+	u16 i;
+	u8 record_type, word_address;
+
+	DEBUGFUNC("e1000_read_invm_word_i210");
+
+	for (i = 0; i < E1000_INVM_SIZE; i++) {
+		invm_dword = E1000_READ_REG(hw, E1000_INVM_DATA_REG(i));
+		/* Get record type */
+		record_type = INVM_DWORD_TO_RECORD_TYPE(invm_dword);
+		if (record_type == E1000_INVM_UNINITIALIZED_STRUCTURE)
+			break;
+		if (record_type == E1000_INVM_CSR_AUTOLOAD_STRUCTURE)
+			i += E1000_INVM_CSR_AUTOLOAD_DATA_SIZE_IN_DWORDS;
+		if (record_type == E1000_INVM_RSA_KEY_SHA256_STRUCTURE)
+			i += E1000_INVM_RSA_KEY_SHA256_DATA_SIZE_IN_DWORDS;
+		if (record_type == E1000_INVM_WORD_AUTOLOAD_STRUCTURE) {
+			word_address = INVM_DWORD_TO_WORD_ADDRESS(invm_dword);
+			if (word_address == address) {
+				*data = INVM_DWORD_TO_WORD_DATA(invm_dword);
+				DEBUGOUT2("Read INVM Word 0x%02x = %x",
+					  address, *data);
+				status = E1000_SUCCESS;
+				break;
+			}
+		}
+	}
+	if (status != E1000_SUCCESS)
+		DEBUGOUT1("Requested word 0x%02x not found in OTP\n", address);
+	return status;
+}
+
+/** e1000_read_invm_i210 - Read invm wrapper function for I210/I211
+ *  @hw: pointer to the HW structure
+ *  @address: the word address (aka eeprom offset) to read
+ *  @data: pointer to the data read
+ *
+ *  Wrapper function to return data formerly found in the NVM.
+ **/
+static s32 e1000_read_invm_i210(struct e1000_hw *hw, u16 offset,
+				u16 E1000_UNUSEDARG words, u16 *data)
+{
+	s32 ret_val = E1000_SUCCESS;
+
+	DEBUGFUNC("e1000_read_invm_i210");
+
+	/* Only the MAC addr is required to be present in the iNVM */
+	switch (offset) {
+	case NVM_MAC_ADDR:
+		ret_val = e1000_read_invm_word_i210(hw, (u8)offset, &data[0]);
+		ret_val |= e1000_read_invm_word_i210(hw, (u8)offset+1,
+						     &data[1]);
+		ret_val |= e1000_read_invm_word_i210(hw, (u8)offset+2,
+						     &data[2]);
+		if (ret_val != E1000_SUCCESS)
+			DEBUGOUT("MAC Addr not found in iNVM\n");
+		break;
+	case NVM_INIT_CTRL_2:
+		ret_val = e1000_read_invm_word_i210(hw, (u8)offset, data);
+		if (ret_val != E1000_SUCCESS) {
+			*data = NVM_INIT_CTRL_2_DEFAULT_I211;
+			ret_val = E1000_SUCCESS;
+		}
+		break;
+	case NVM_INIT_CTRL_4:
+		ret_val = e1000_read_invm_word_i210(hw, (u8)offset, data);
+		if (ret_val != E1000_SUCCESS) {
+			*data = NVM_INIT_CTRL_4_DEFAULT_I211;
+			ret_val = E1000_SUCCESS;
+		}
+		break;
+	case NVM_LED_1_CFG:
+		ret_val = e1000_read_invm_word_i210(hw, (u8)offset, data);
+		if (ret_val != E1000_SUCCESS) {
+			*data = NVM_LED_1_CFG_DEFAULT_I211;
+			ret_val = E1000_SUCCESS;
+		}
+		break;
+	case NVM_LED_0_2_CFG:
+		ret_val = e1000_read_invm_word_i210(hw, (u8)offset, data);
+		if (ret_val != E1000_SUCCESS) {
+			*data = NVM_LED_0_2_CFG_DEFAULT_I211;
+			ret_val = E1000_SUCCESS;
+		}
+		break;
+	case NVM_ID_LED_SETTINGS:
+		ret_val = e1000_read_invm_word_i210(hw, (u8)offset, data);
+		if (ret_val != E1000_SUCCESS) {
+			*data = ID_LED_RESERVED_FFFF;
+			ret_val = E1000_SUCCESS;
+		}
+		break;
+	case NVM_SUB_DEV_ID:
+		*data = hw->subsystem_device_id;
+		break;
+	case NVM_SUB_VEN_ID:
+		*data = hw->subsystem_vendor_id;
+		break;
+	case NVM_DEV_ID:
+		*data = hw->device_id;
+		break;
+	case NVM_VEN_ID:
+		*data = hw->vendor_id;
+		break;
+	default:
+		DEBUGOUT1("NVM word 0x%02x is not mapped.\n", offset);
+		*data = NVM_RESERVED_WORD;
+		break;
+	}
+	return ret_val;
+}
+
+/**
+ *  e1000_read_invm_version - Reads iNVM version and image type
+ *  @hw: pointer to the HW structure
+ *  @invm_ver: version structure for the version read
+ *
+ *  Reads iNVM version and image type.
+ **/
+s32 e1000_read_invm_version(struct e1000_hw *hw,
+			    struct e1000_fw_version *invm_ver)
+{
+	u32 *record = NULL;
+	u32 *next_record = NULL;
+	u32 i = 0;
+	u32 invm_dword = 0;
+	u32 invm_blocks = E1000_INVM_SIZE - (E1000_INVM_ULT_BYTES_SIZE /
+					     E1000_INVM_RECORD_SIZE_IN_BYTES);
+	u32 buffer[E1000_INVM_SIZE];
+	s32 status = -E1000_ERR_INVM_VALUE_NOT_FOUND;
+	u16 version = 0;
+
+	DEBUGFUNC("e1000_read_invm_version");
+
+	/* Read iNVM memory */
+	for (i = 0; i < E1000_INVM_SIZE; i++) {
+		invm_dword = E1000_READ_REG(hw, E1000_INVM_DATA_REG(i));
+		buffer[i] = invm_dword;
+	}
+
+	/* Read version number */
+	for (i = 1; i < invm_blocks; i++) {
+		record = &buffer[invm_blocks - i];
+		next_record = &buffer[invm_blocks - i + 1];
+
+		/* Check if we have first version location used */
+		if ((i == 1) && ((*record & E1000_INVM_VER_FIELD_ONE) == 0)) {
+			version = 0;
+			status = E1000_SUCCESS;
+			break;
+		}
+		/* Check if we have second version location used */
+		else if ((i == 1) &&
+			 ((*record & E1000_INVM_VER_FIELD_TWO) == 0)) {
+			version = (*record & E1000_INVM_VER_FIELD_ONE) >> 3;
+			status = E1000_SUCCESS;
+			break;
+		}
+		/*
+		 * Check if we have odd version location
+		 * used and it is the last one used
+		 */
+		else if ((((*record & E1000_INVM_VER_FIELD_ONE) == 0) &&
+			 ((*record & 0x3) == 0)) || (((*record & 0x3) != 0) &&
+			 (i != 1))) {
+			version = (*next_record & E1000_INVM_VER_FIELD_TWO)
+				  >> 13;
+			status = E1000_SUCCESS;
+			break;
+		}
+		/*
+		 * Check if we have even version location
+		 * used and it is the last one used
+		 */
+		else if (((*record & E1000_INVM_VER_FIELD_TWO) == 0) &&
+			 ((*record & 0x3) == 0)) {
+			version = (*record & E1000_INVM_VER_FIELD_ONE) >> 3;
+			status = E1000_SUCCESS;
+			break;
+		}
+	}
+
+	if (status == E1000_SUCCESS) {
+		invm_ver->invm_major = (version & E1000_INVM_MAJOR_MASK)
+					>> E1000_INVM_MAJOR_SHIFT;
+		invm_ver->invm_minor = version & E1000_INVM_MINOR_MASK;
+	}
+	/* Read Image Type */
+	for (i = 1; i < invm_blocks; i++) {
+		record = &buffer[invm_blocks - i];
+		next_record = &buffer[invm_blocks - i + 1];
+
+		/* Check if we have image type in first location used */
+		if ((i == 1) && ((*record & E1000_INVM_IMGTYPE_FIELD) == 0)) {
+			invm_ver->invm_img_type = 0;
+			status = E1000_SUCCESS;
+			break;
+		}
+		/* Check if we have image type in first location used */
+		else if ((((*record & 0x3) == 0) &&
+			 ((*record & E1000_INVM_IMGTYPE_FIELD) == 0)) ||
+			 ((((*record & 0x3) != 0) && (i != 1)))) {
+			invm_ver->invm_img_type =
+				(*next_record & E1000_INVM_IMGTYPE_FIELD) >> 23;
+			status = E1000_SUCCESS;
+			break;
+		}
+	}
+	return status;
+}
+
+/**
+ *  e1000_validate_nvm_checksum_i210 - Validate EEPROM checksum
+ *  @hw: pointer to the HW structure
+ *
+ *  Calculates the EEPROM checksum by reading/adding each word of the EEPROM
+ *  and then verifies that the sum of the EEPROM is equal to 0xBABA.
+ **/
+s32 e1000_validate_nvm_checksum_i210(struct e1000_hw *hw)
+{
+	s32 status = E1000_SUCCESS;
+	s32 (*read_op_ptr)(struct e1000_hw *, u16, u16, u16 *);
+
+	DEBUGFUNC("e1000_validate_nvm_checksum_i210");
+
+	if (hw->nvm.ops.acquire(hw) == E1000_SUCCESS) {
+
+		/*
+		 * Replace the read function with semaphore grabbing with
+		 * the one that skips this for a while.
+		 * We have semaphore taken already here.
+		 */
+		read_op_ptr = hw->nvm.ops.read;
+		hw->nvm.ops.read = e1000_read_nvm_eerd;
+
+		status = e1000_validate_nvm_checksum_generic(hw);
+
+		/* Revert original read operation. */
+		hw->nvm.ops.read = read_op_ptr;
+
+		hw->nvm.ops.release(hw);
+	} else {
+		status = E1000_ERR_SWFW_SYNC;
+	}
+
+	return status;
+}
+
+
+/**
+ *  e1000_update_nvm_checksum_i210 - Update EEPROM checksum
+ *  @hw: pointer to the HW structure
+ *
+ *  Updates the EEPROM checksum by reading/adding each word of the EEPROM
+ *  up to the checksum.  Then calculates the EEPROM checksum and writes the
+ *  value to the EEPROM. Next commit EEPROM data onto the Flash.
+ **/
+s32 e1000_update_nvm_checksum_i210(struct e1000_hw *hw)
+{
+	s32 ret_val = E1000_SUCCESS;
+	u16 checksum = 0;
+	u16 i, nvm_data;
+
+	DEBUGFUNC("e1000_update_nvm_checksum_i210");
+
+	/*
+	 * Read the first word from the EEPROM. If this times out or fails, do
+	 * not continue or we could be in for a very long wait while every
+	 * EEPROM read fails
+	 */
+	ret_val = e1000_read_nvm_eerd(hw, 0, 1, &nvm_data);
+	if (ret_val != E1000_SUCCESS) {
+		DEBUGOUT("EEPROM read failed\n");
+		goto out;
+	}
+
+	if (hw->nvm.ops.acquire(hw) == E1000_SUCCESS) {
+		/*
+		 * Do not use hw->nvm.ops.write, hw->nvm.ops.read
+		 * because we do not want to take the synchronization
+		 * semaphores twice here.
+		 */
+
+		for (i = 0; i < NVM_CHECKSUM_REG; i++) {
+			ret_val = e1000_read_nvm_eerd(hw, i, 1, &nvm_data);
+			if (ret_val) {
+				hw->nvm.ops.release(hw);
+				DEBUGOUT("NVM Read Error while updating checksum.\n");
+				goto out;
+			}
+			checksum += nvm_data;
+		}
+		checksum = (u16) NVM_SUM - checksum;
+		ret_val = e1000_write_nvm_srwr(hw, NVM_CHECKSUM_REG, 1,
+						&checksum);
+		if (ret_val != E1000_SUCCESS) {
+			hw->nvm.ops.release(hw);
+			DEBUGOUT("NVM Write Error while updating checksum.\n");
+			goto out;
+		}
+
+		hw->nvm.ops.release(hw);
+
+		ret_val = e1000_update_flash_i210(hw);
+	} else {
+		ret_val = E1000_ERR_SWFW_SYNC;
+	}
+out:
+	return ret_val;
+}
+
+/**
+ *  e1000_get_flash_presence_i210 - Check if flash device is detected.
+ *  @hw: pointer to the HW structure
+ *
+ **/
+bool e1000_get_flash_presence_i210(struct e1000_hw *hw)
+{
+	u32 eec = 0;
+	bool ret_val = false;
+
+	DEBUGFUNC("e1000_get_flash_presence_i210");
+
+	eec = E1000_READ_REG(hw, E1000_EECD);
+
+	if (eec & E1000_EECD_FLASH_DETECTED_I210)
+		ret_val = true;
+
+	return ret_val;
+}
+
+/**
+ *  e1000_update_flash_i210 - Commit EEPROM to the flash
+ *  @hw: pointer to the HW structure
+ *
+ **/
+s32 e1000_update_flash_i210(struct e1000_hw *hw)
+{
+	s32 ret_val = E1000_SUCCESS;
+	u32 flup;
+
+	DEBUGFUNC("e1000_update_flash_i210");
+
+	ret_val = e1000_pool_flash_update_done_i210(hw);
+	if (ret_val == -E1000_ERR_NVM) {
+		DEBUGOUT("Flash update time out\n");
+		goto out;
+	}
+
+	flup = E1000_READ_REG(hw, E1000_EECD) | E1000_EECD_FLUPD_I210;
+	E1000_WRITE_REG(hw, E1000_EECD, flup);
+
+	ret_val = e1000_pool_flash_update_done_i210(hw);
+	if (ret_val == E1000_SUCCESS)
+		DEBUGOUT("Flash update complete\n");
+	else
+		DEBUGOUT("Flash update time out\n");
+
+out:
+	return ret_val;
+}
+
+/**
+ *  e1000_pool_flash_update_done_i210 - Pool FLUDONE status.
+ *  @hw: pointer to the HW structure
+ *
+ **/
+s32 e1000_pool_flash_update_done_i210(struct e1000_hw *hw)
+{
+	s32 ret_val = -E1000_ERR_NVM;
+	u32 i, reg;
+
+	DEBUGFUNC("e1000_pool_flash_update_done_i210");
+
+	for (i = 0; i < E1000_FLUDONE_ATTEMPTS; i++) {
+		reg = E1000_READ_REG(hw, E1000_EECD);
+		if (reg & E1000_EECD_FLUDONE_I210) {
+			ret_val = E1000_SUCCESS;
+			break;
+		}
+		usec_delay(5);
+	}
+
+	return ret_val;
+}
+
+/**
+ *  e1000_init_nvm_params_i210 - Initialize i210 NVM function pointers
+ *  @hw: pointer to the HW structure
+ *
+ *  Initialize the i210/i211 NVM parameters and function pointers.
+ **/
+static s32 e1000_init_nvm_params_i210(struct e1000_hw *hw)
+{
+	s32 ret_val = E1000_SUCCESS;
+	struct e1000_nvm_info *nvm = &hw->nvm;
+
+	DEBUGFUNC("e1000_init_nvm_params_i210");
+
+	ret_val = e1000_init_nvm_params_82575(hw);
+	nvm->ops.acquire = e1000_acquire_nvm_i210;
+	nvm->ops.release = e1000_release_nvm_i210;
+	nvm->ops.valid_led_default = e1000_valid_led_default_i210;
+	if (e1000_get_flash_presence_i210(hw)) {
+		hw->nvm.type = e1000_nvm_flash_hw;
+		nvm->ops.read    = e1000_read_nvm_srrd_i210;
+		nvm->ops.write   = e1000_write_nvm_srwr_i210;
+		nvm->ops.validate = e1000_validate_nvm_checksum_i210;
+		nvm->ops.update   = e1000_update_nvm_checksum_i210;
+	} else {
+		hw->nvm.type = e1000_nvm_invm;
+		nvm->ops.read     = e1000_read_invm_i210;
+		nvm->ops.write    = e1000_null_write_nvm;
+		nvm->ops.validate = e1000_null_ops_generic;
+		nvm->ops.update   = e1000_null_ops_generic;
+	}
+	return ret_val;
+}
+
+/**
+ *  e1000_init_function_pointers_i210 - Init func ptrs.
+ *  @hw: pointer to the HW structure
+ *
+ *  Called to initialize all function pointers and parameters.
+ **/
+void e1000_init_function_pointers_i210(struct e1000_hw *hw)
+{
+	e1000_init_function_pointers_82575(hw);
+	hw->nvm.ops.init_params = e1000_init_nvm_params_i210;
+
+	return;
+}
+
+/**
+ *  e1000_valid_led_default_i210 - Verify a valid default LED config
+ *  @hw: pointer to the HW structure
+ *  @data: pointer to the NVM (EEPROM)
+ *
+ *  Read the EEPROM for the current default LED configuration.  If the
+ *  LED configuration is not valid, set to a valid LED configuration.
+ **/
+static s32 e1000_valid_led_default_i210(struct e1000_hw *hw, u16 *data)
+{
+	s32 ret_val;
+
+	DEBUGFUNC("e1000_valid_led_default_i210");
+
+	ret_val = hw->nvm.ops.read(hw, NVM_ID_LED_SETTINGS, 1, data);
+	if (ret_val) {
+		DEBUGOUT("NVM Read Error\n");
+		goto out;
+	}
+
+	if (*data == ID_LED_RESERVED_0000 || *data == ID_LED_RESERVED_FFFF) {
+		switch (hw->phy.media_type) {
+		case e1000_media_type_internal_serdes:
+			*data = ID_LED_DEFAULT_I210_SERDES;
+			break;
+		case e1000_media_type_copper:
+		default:
+			*data = ID_LED_DEFAULT_I210;
+			break;
+		}
+	}
+out:
+	return ret_val;
+}
+
+/**
+ *  __e1000_access_xmdio_reg - Read/write XMDIO register
+ *  @hw: pointer to the HW structure
+ *  @address: XMDIO address to program
+ *  @dev_addr: device address to program
+ *  @data: pointer to value to read/write from/to the XMDIO address
+ *  @read: boolean flag to indicate read or write
+ **/
+static s32 __e1000_access_xmdio_reg(struct e1000_hw *hw, u16 address,
+				    u8 dev_addr, u16 *data, bool read)
+{
+	s32 ret_val = E1000_SUCCESS;
+
+	DEBUGFUNC("__e1000_access_xmdio_reg");
+
+	ret_val = hw->phy.ops.write_reg(hw, E1000_MMDAC, dev_addr);
+	if (ret_val)
+		return ret_val;
+
+	ret_val = hw->phy.ops.write_reg(hw, E1000_MMDAAD, address);
+	if (ret_val)
+		return ret_val;
+
+	ret_val = hw->phy.ops.write_reg(hw, E1000_MMDAC, E1000_MMDAC_FUNC_DATA |
+							 dev_addr);
+	if (ret_val)
+		return ret_val;
+
+	if (read)
+		ret_val = hw->phy.ops.read_reg(hw, E1000_MMDAAD, data);
+	else
+		ret_val = hw->phy.ops.write_reg(hw, E1000_MMDAAD, *data);
+	if (ret_val)
+		return ret_val;
+
+	/* Recalibrate the device back to 0 */
+	ret_val = hw->phy.ops.write_reg(hw, E1000_MMDAC, 0);
+	if (ret_val)
+		return ret_val;
+
+	return ret_val;
+}
+
+/**
+ *  e1000_read_xmdio_reg - Read XMDIO register
+ *  @hw: pointer to the HW structure
+ *  @addr: XMDIO address to program
+ *  @dev_addr: device address to program
+ *  @data: value to be read from the EMI address
+ **/
+s32 e1000_read_xmdio_reg(struct e1000_hw *hw, u16 addr, u8 dev_addr, u16 *data)
+{
+	DEBUGFUNC("e1000_read_xmdio_reg");
+
+	return __e1000_access_xmdio_reg(hw, addr, dev_addr, data, true);
+}
+
+/**
+ *  e1000_write_xmdio_reg - Write XMDIO register
+ *  @hw: pointer to the HW structure
+ *  @addr: XMDIO address to program
+ *  @dev_addr: device address to program
+ *  @data: value to be written to the XMDIO address
+ **/
+s32 e1000_write_xmdio_reg(struct e1000_hw *hw, u16 addr, u8 dev_addr, u16 data)
+{
+	DEBUGFUNC("e1000_read_xmdio_reg");
+
+	return __e1000_access_xmdio_reg(hw, addr, dev_addr, &data, false);
+}
diff --git a/drivers/net/igb/e1000_i210.h b/drivers/net/igb/e1000_i210.h
new file mode 100644
index 000000000000..57b2eb5602c2
--- /dev/null
+++ b/drivers/net/igb/e1000_i210.h
@@ -0,0 +1,91 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+#ifndef _E1000_I210_H_
+#define _E1000_I210_H_
+
+bool e1000_get_flash_presence_i210(struct e1000_hw *hw);
+s32 e1000_update_flash_i210(struct e1000_hw *hw);
+s32 e1000_update_nvm_checksum_i210(struct e1000_hw *hw);
+s32 e1000_validate_nvm_checksum_i210(struct e1000_hw *hw);
+s32 e1000_write_nvm_srwr_i210(struct e1000_hw *hw, u16 offset,
+			      u16 words, u16 *data);
+s32 e1000_read_nvm_srrd_i210(struct e1000_hw *hw, u16 offset,
+			     u16 words, u16 *data);
+s32 e1000_read_invm_version(struct e1000_hw *hw,
+			    struct e1000_fw_version *invm_ver);
+s32 e1000_acquire_swfw_sync_i210(struct e1000_hw *hw, u16 mask);
+void e1000_release_swfw_sync_i210(struct e1000_hw *hw, u16 mask);
+s32 e1000_read_xmdio_reg(struct e1000_hw *hw, u16 addr, u8 dev_addr,
+			 u16 *data);
+s32 e1000_write_xmdio_reg(struct e1000_hw *hw, u16 addr, u8 dev_addr,
+			  u16 data);
+
+#define E1000_STM_OPCODE		0xDB00
+#define E1000_EEPROM_FLASH_SIZE_WORD	0x11
+
+#define INVM_DWORD_TO_RECORD_TYPE(invm_dword) \
+	(u8)((invm_dword) & 0x7)
+#define INVM_DWORD_TO_WORD_ADDRESS(invm_dword) \
+	(u8)(((invm_dword) & 0x0000FE00) >> 9)
+#define INVM_DWORD_TO_WORD_DATA(invm_dword) \
+	(u16)(((invm_dword) & 0xFFFF0000) >> 16)
+
+enum E1000_INVM_STRUCTURE_TYPE {
+	E1000_INVM_UNINITIALIZED_STRUCTURE		= 0x00,
+	E1000_INVM_WORD_AUTOLOAD_STRUCTURE		= 0x01,
+	E1000_INVM_CSR_AUTOLOAD_STRUCTURE		= 0x02,
+	E1000_INVM_PHY_REGISTER_AUTOLOAD_STRUCTURE	= 0x03,
+	E1000_INVM_RSA_KEY_SHA256_STRUCTURE		= 0x04,
+	E1000_INVM_INVALIDATED_STRUCTURE		= 0x0F,
+};
+
+#define E1000_INVM_RSA_KEY_SHA256_DATA_SIZE_IN_DWORDS	8
+#define E1000_INVM_CSR_AUTOLOAD_DATA_SIZE_IN_DWORDS	1
+#define E1000_INVM_ULT_BYTES_SIZE	8
+#define E1000_INVM_RECORD_SIZE_IN_BYTES	4
+#define E1000_INVM_VER_FIELD_ONE	0x1FF8
+#define E1000_INVM_VER_FIELD_TWO	0x7FE000
+#define E1000_INVM_IMGTYPE_FIELD	0x1F800000
+
+#define E1000_INVM_MAJOR_MASK	0x3F0
+#define E1000_INVM_MINOR_MASK	0xF
+#define E1000_INVM_MAJOR_SHIFT	4
+
+#define ID_LED_DEFAULT_I210		((ID_LED_OFF1_ON2  << 8) | \
+					 (ID_LED_DEF1_DEF2 <<  4) | \
+					 (ID_LED_OFF1_OFF2))
+#define ID_LED_DEFAULT_I210_SERDES	((ID_LED_DEF1_DEF2 << 8) | \
+					 (ID_LED_DEF1_DEF2 <<  4) | \
+					 (ID_LED_OFF1_ON2))
+
+/* NVM offset defaults for I211 devices */
+#define NVM_INIT_CTRL_2_DEFAULT_I211	0X7243
+#define NVM_INIT_CTRL_4_DEFAULT_I211	0x00C1
+#define NVM_LED_1_CFG_DEFAULT_I211	0x0184
+#define NVM_LED_0_2_CFG_DEFAULT_I211	0x200C
+#endif
diff --git a/drivers/net/igb/e1000_mac.c b/drivers/net/igb/e1000_mac.c
index 2b5ef761d2ab..28b4e97d775d 100644
--- a/drivers/net/igb/e1000_mac.c
+++ b/drivers/net/igb/e1000_mac.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2007-2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -25,48 +25,158 @@
 
 *******************************************************************************/
 
-#include <linux/if_ether.h>
-#include <linux/delay.h>
-#include <linux/pci.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
+#include "e1000_api.h"
 
-#include "e1000_mac.h"
+static s32 e1000_validate_mdi_setting_generic(struct e1000_hw *hw);
+static void e1000_set_lan_id_multi_port_pcie(struct e1000_hw *hw);
+static void e1000_config_collision_dist_generic(struct e1000_hw *hw);
+static void e1000_rar_set_generic(struct e1000_hw *hw, u8 *addr, u32 index);
 
-#include "igb.h"
+/**
+ *  e1000_init_mac_ops_generic - Initialize MAC function pointers
+ *  @hw: pointer to the HW structure
+ *
+ *  Setups up the function pointers to no-op functions
+ **/
+void e1000_init_mac_ops_generic(struct e1000_hw *hw)
+{
+	struct e1000_mac_info *mac = &hw->mac;
+	DEBUGFUNC("e1000_init_mac_ops_generic");
+
+	/* General Setup */
+	mac->ops.init_params = e1000_null_ops_generic;
+	mac->ops.init_hw = e1000_null_ops_generic;
+	mac->ops.reset_hw = e1000_null_ops_generic;
+	mac->ops.setup_physical_interface = e1000_null_ops_generic;
+	mac->ops.get_bus_info = e1000_null_ops_generic;
+	mac->ops.set_lan_id = e1000_set_lan_id_multi_port_pcie;
+	mac->ops.read_mac_addr = e1000_read_mac_addr_generic;
+	mac->ops.config_collision_dist = e1000_config_collision_dist_generic;
+	mac->ops.clear_hw_cntrs = e1000_null_mac_generic;
+	/* LED */
+	mac->ops.cleanup_led = e1000_null_ops_generic;
+	mac->ops.setup_led = e1000_null_ops_generic;
+	mac->ops.blink_led = e1000_null_ops_generic;
+	mac->ops.led_on = e1000_null_ops_generic;
+	mac->ops.led_off = e1000_null_ops_generic;
+	/* LINK */
+	mac->ops.setup_link = e1000_null_ops_generic;
+	mac->ops.get_link_up_info = e1000_null_link_info;
+	mac->ops.check_for_link = e1000_null_ops_generic;
+	/* Management */
+	mac->ops.check_mng_mode = e1000_null_mng_mode;
+	/* VLAN, MC, etc. */
+	mac->ops.update_mc_addr_list = e1000_null_update_mc;
+	mac->ops.clear_vfta = e1000_null_mac_generic;
+	mac->ops.write_vfta = e1000_null_write_vfta;
+	mac->ops.rar_set = e1000_rar_set_generic;
+	mac->ops.validate_mdi_setting = e1000_validate_mdi_setting_generic;
+}
+
+/**
+ *  e1000_null_ops_generic - No-op function, returns 0
+ *  @hw: pointer to the HW structure
+ **/
+s32 e1000_null_ops_generic(struct e1000_hw E1000_UNUSEDARG *hw)
+{
+	DEBUGFUNC("e1000_null_ops_generic");
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_null_mac_generic - No-op function, return void
+ *  @hw: pointer to the HW structure
+ **/
+void e1000_null_mac_generic(struct e1000_hw E1000_UNUSEDARG *hw)
+{
+	DEBUGFUNC("e1000_null_mac_generic");
+	return;
+}
+
+/**
+ *  e1000_null_link_info - No-op function, return 0
+ *  @hw: pointer to the HW structure
+ **/
+s32 e1000_null_link_info(struct e1000_hw E1000_UNUSEDARG *hw,
+			 u16 E1000_UNUSEDARG *s, u16 E1000_UNUSEDARG *d)
+{
+	DEBUGFUNC("e1000_null_link_info");
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_null_mng_mode - No-op function, return false
+ *  @hw: pointer to the HW structure
+ **/
+bool e1000_null_mng_mode(struct e1000_hw E1000_UNUSEDARG *hw)
+{
+	DEBUGFUNC("e1000_null_mng_mode");
+	return false;
+}
+
+/**
+ *  e1000_null_update_mc - No-op function, return void
+ *  @hw: pointer to the HW structure
+ **/
+void e1000_null_update_mc(struct e1000_hw E1000_UNUSEDARG *hw,
+			  u8 E1000_UNUSEDARG *h, u32 E1000_UNUSEDARG a)
+{
+	DEBUGFUNC("e1000_null_update_mc");
+	return;
+}
 
-static s32 igb_set_default_fc(struct e1000_hw *hw);
-static s32 igb_set_fc_watermarks(struct e1000_hw *hw);
+/**
+ *  e1000_null_write_vfta - No-op function, return void
+ *  @hw: pointer to the HW structure
+ **/
+void e1000_null_write_vfta(struct e1000_hw E1000_UNUSEDARG *hw,
+			   u32 E1000_UNUSEDARG a, u32 E1000_UNUSEDARG b)
+{
+	DEBUGFUNC("e1000_null_write_vfta");
+	return;
+}
 
 /**
- *  igb_get_bus_info_pcie - Get PCIe bus information
+ *  e1000_null_rar_set - No-op function, return void
+ *  @hw: pointer to the HW structure
+ **/
+void e1000_null_rar_set(struct e1000_hw E1000_UNUSEDARG *hw,
+			u8 E1000_UNUSEDARG *h, u32 E1000_UNUSEDARG a)
+{
+	DEBUGFUNC("e1000_null_rar_set");
+	return;
+}
+
+/**
+ *  e1000_get_bus_info_pcie_generic - Get PCIe bus information
  *  @hw: pointer to the HW structure
  *
  *  Determines and stores the system bus information for a particular
  *  network interface.  The following bus information is determined and stored:
  *  bus speed, bus width, type (PCIe), and PCIe function.
  **/
-s32 igb_get_bus_info_pcie(struct e1000_hw *hw)
+s32 e1000_get_bus_info_pcie_generic(struct e1000_hw *hw)
 {
+	struct e1000_mac_info *mac = &hw->mac;
 	struct e1000_bus_info *bus = &hw->bus;
 	s32 ret_val;
-	u32 reg;
 	u16 pcie_link_status;
 
+	DEBUGFUNC("e1000_get_bus_info_pcie_generic");
+
 	bus->type = e1000_bus_type_pci_express;
 
-	ret_val = igb_read_pcie_cap_reg(hw,
-					PCI_EXP_LNKSTA,
-					&pcie_link_status);
+	ret_val = e1000_read_pcie_cap_reg(hw, PCIE_LINK_STATUS,
+					  &pcie_link_status);
 	if (ret_val) {
 		bus->width = e1000_bus_width_unknown;
 		bus->speed = e1000_bus_speed_unknown;
 	} else {
-		switch (pcie_link_status & PCI_EXP_LNKSTA_CLS) {
-		case PCI_EXP_LNKSTA_CLS_2_5GB:
+		switch (pcie_link_status & PCIE_LINK_SPEED_MASK) {
+		case PCIE_LINK_SPEED_2500:
 			bus->speed = e1000_bus_speed_2500;
 			break;
-		case PCI_EXP_LNKSTA_CLS_5_0GB:
+		case PCIE_LINK_SPEED_5000:
 			bus->speed = e1000_bus_speed_5000;
 			break;
 		default:
@@ -75,35 +185,68 @@ s32 igb_get_bus_info_pcie(struct e1000_hw *hw)
 		}
 
 		bus->width = (enum e1000_bus_width)((pcie_link_status &
-						     PCI_EXP_LNKSTA_NLW) >>
-						     PCI_EXP_LNKSTA_NLW_SHIFT);
+			      PCIE_LINK_WIDTH_MASK) >> PCIE_LINK_WIDTH_SHIFT);
 	}
 
-	reg = rd32(E1000_STATUS);
+	mac->ops.set_lan_id(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_set_lan_id_multi_port_pcie - Set LAN id for PCIe multiple port devices
+ *
+ *  @hw: pointer to the HW structure
+ *
+ *  Determines the LAN function id by reading memory-mapped registers
+ *  and swaps the port value if requested.
+ **/
+static void e1000_set_lan_id_multi_port_pcie(struct e1000_hw *hw)
+{
+	struct e1000_bus_info *bus = &hw->bus;
+	u32 reg;
+
+	/* The status register reports the correct function number
+	 * for the device regardless of function swap state.
+	 */
+	reg = E1000_READ_REG(hw, E1000_STATUS);
 	bus->func = (reg & E1000_STATUS_FUNC_MASK) >> E1000_STATUS_FUNC_SHIFT;
+}
 
-	return 0;
+/**
+ *  e1000_set_lan_id_single_port - Set LAN id for a single port device
+ *  @hw: pointer to the HW structure
+ *
+ *  Sets the LAN function id to zero for a single port device.
+ **/
+void e1000_set_lan_id_single_port(struct e1000_hw *hw)
+{
+	struct e1000_bus_info *bus = &hw->bus;
+
+	bus->func = 0;
 }
 
 /**
- *  igb_clear_vfta - Clear VLAN filter table
+ *  e1000_clear_vfta_generic - Clear VLAN filter table
  *  @hw: pointer to the HW structure
  *
  *  Clears the register array which contains the VLAN filter table by
  *  setting all the values to 0.
  **/
-void igb_clear_vfta(struct e1000_hw *hw)
+void e1000_clear_vfta_generic(struct e1000_hw *hw)
 {
 	u32 offset;
 
+	DEBUGFUNC("e1000_clear_vfta_generic");
+
 	for (offset = 0; offset < E1000_VLAN_FILTER_TBL_SIZE; offset++) {
-		array_wr32(E1000_VFTA, offset, 0);
-		wrfl();
+		E1000_WRITE_REG_ARRAY(hw, E1000_VFTA, offset, 0);
+		E1000_WRITE_FLUSH(hw);
 	}
 }
 
 /**
- *  igb_write_vfta - Write value to VLAN filter table
+ *  e1000_write_vfta_generic - Write value to VLAN filter table
  *  @hw: pointer to the HW structure
  *  @offset: register offset in VLAN filter table
  *  @value: register value written to VLAN filter table
@@ -111,106 +254,98 @@ void igb_clear_vfta(struct e1000_hw *hw)
  *  Writes value at the given offset in the register array which stores
  *  the VLAN filter table.
  **/
-static void igb_write_vfta(struct e1000_hw *hw, u32 offset, u32 value)
+void e1000_write_vfta_generic(struct e1000_hw *hw, u32 offset, u32 value)
 {
-	array_wr32(E1000_VFTA, offset, value);
-	wrfl();
+	DEBUGFUNC("e1000_write_vfta_generic");
+
+	E1000_WRITE_REG_ARRAY(hw, E1000_VFTA, offset, value);
+	E1000_WRITE_FLUSH(hw);
 }
 
 /**
- *  igb_init_rx_addrs - Initialize receive address's
+ *  e1000_init_rx_addrs_generic - Initialize receive address's
  *  @hw: pointer to the HW structure
  *  @rar_count: receive address registers
  *
- *  Setups the receive address registers by setting the base receive address
+ *  Setup the receive address registers by setting the base receive address
  *  register to the devices MAC address and clearing all the other receive
  *  address registers to 0.
  **/
-void igb_init_rx_addrs(struct e1000_hw *hw, u16 rar_count)
+void e1000_init_rx_addrs_generic(struct e1000_hw *hw, u16 rar_count)
 {
 	u32 i;
-	u8 mac_addr[ETH_ALEN] = {0};
+	u8 mac_addr[ETH_ADDR_LEN] = {0};
+
+	DEBUGFUNC("e1000_init_rx_addrs_generic");
 
 	/* Setup the receive address */
-	hw_dbg("Programming MAC Address into RAR[0]\n");
+	DEBUGOUT("Programming MAC Address into RAR[0]\n");
 
 	hw->mac.ops.rar_set(hw, hw->mac.addr, 0);
 
 	/* Zero out the other (rar_entry_count - 1) receive addresses */
-	hw_dbg("Clearing RAR[1-%u]\n", rar_count-1);
+	DEBUGOUT1("Clearing RAR[1-%u]\n", rar_count-1);
 	for (i = 1; i < rar_count; i++)
 		hw->mac.ops.rar_set(hw, mac_addr, i);
 }
 
 /**
- *  igb_vfta_set - enable or disable vlan in VLAN filter table
- *  @hw: pointer to the HW structure
- *  @vid: VLAN id to add or remove
- *  @add: if true add filter, if false remove
- *
- *  Sets or clears a bit in the VLAN filter table array based on VLAN id
- *  and if we are adding or removing the filter
- **/
-s32 igb_vfta_set(struct e1000_hw *hw, u32 vid, bool add)
-{
-	u32 index = (vid >> E1000_VFTA_ENTRY_SHIFT) & E1000_VFTA_ENTRY_MASK;
-	u32 mask = 1 << (vid & E1000_VFTA_ENTRY_BIT_SHIFT_MASK);
-	u32 vfta = array_rd32(E1000_VFTA, index);
-	s32 ret_val = 0;
-
-	/* bit was set/cleared before we started */
-	if ((!!(vfta & mask)) == add) {
-		ret_val = -E1000_ERR_CONFIG;
-	} else {
-		if (add)
-			vfta |= mask;
-		else
-			vfta &= ~mask;
-	}
-
-	igb_write_vfta(hw, index, vfta);
-
-	return ret_val;
-}
-
-/**
- *  igb_check_alt_mac_addr - Check for alternate MAC addr
+ *  e1000_check_alt_mac_addr_generic - Check for alternate MAC addr
  *  @hw: pointer to the HW structure
  *
  *  Checks the nvm for an alternate MAC address.  An alternate MAC address
  *  can be setup by pre-boot software and must be treated like a permanent
- *  address and must override the actual permanent MAC address.  If an
- *  alternate MAC address is fopund it is saved in the hw struct and
- *  prgrammed into RAR0 and the cuntion returns success, otherwise the
- *  function returns an error.
+ *  address and must override the actual permanent MAC address. If an
+ *  alternate MAC address is found it is programmed into RAR0, replacing
+ *  the permanent address that was installed into RAR0 by the Si on reset.
+ *  This function will return SUCCESS unless it encounters an error while
+ *  reading the EEPROM.
  **/
-s32 igb_check_alt_mac_addr(struct e1000_hw *hw)
+s32 e1000_check_alt_mac_addr_generic(struct e1000_hw *hw)
 {
 	u32 i;
-	s32 ret_val = 0;
+	s32 ret_val;
 	u16 offset, nvm_alt_mac_addr_offset, nvm_data;
-	u8 alt_mac_addr[ETH_ALEN];
+	u8 alt_mac_addr[ETH_ADDR_LEN];
+
+	DEBUGFUNC("e1000_check_alt_mac_addr_generic");
+
+	ret_val = hw->nvm.ops.read(hw, NVM_COMPAT, 1, &nvm_data);
+	if (ret_val)
+		return ret_val;
+
+
+	/* Alternate MAC address is handled by the option ROM for 82580
+	 * and newer. SW support not required.
+	 */
+	if (hw->mac.type >= e1000_82580)
+		return E1000_SUCCESS;
 
 	ret_val = hw->nvm.ops.read(hw, NVM_ALT_MAC_ADDR_PTR, 1,
-				 &nvm_alt_mac_addr_offset);
+				   &nvm_alt_mac_addr_offset);
 	if (ret_val) {
-		hw_dbg("NVM Read Error\n");
-		goto out;
+		DEBUGOUT("NVM Read Error\n");
+		return ret_val;
 	}
 
-	if (nvm_alt_mac_addr_offset == 0xFFFF) {
+	if ((nvm_alt_mac_addr_offset == 0xFFFF) ||
+	    (nvm_alt_mac_addr_offset == 0x0000))
 		/* There is no Alternate MAC Address */
-		goto out;
-	}
+		return E1000_SUCCESS;
 
 	if (hw->bus.func == E1000_FUNC_1)
 		nvm_alt_mac_addr_offset += E1000_ALT_MAC_ADDRESS_OFFSET_LAN1;
-	for (i = 0; i < ETH_ALEN; i += 2) {
+	if (hw->bus.func == E1000_FUNC_2)
+		nvm_alt_mac_addr_offset += E1000_ALT_MAC_ADDRESS_OFFSET_LAN2;
+
+	if (hw->bus.func == E1000_FUNC_3)
+		nvm_alt_mac_addr_offset += E1000_ALT_MAC_ADDRESS_OFFSET_LAN3;
+	for (i = 0; i < ETH_ADDR_LEN; i += 2) {
 		offset = nvm_alt_mac_addr_offset + (i >> 1);
 		ret_val = hw->nvm.ops.read(hw, offset, 1, &nvm_data);
 		if (ret_val) {
-			hw_dbg("NVM Read Error\n");
-			goto out;
+			DEBUGOUT("NVM Read Error\n");
+			return ret_val;
 		}
 
 		alt_mac_addr[i] = (u8)(nvm_data & 0xFF);
@@ -218,24 +353,22 @@ s32 igb_check_alt_mac_addr(struct e1000_hw *hw)
 	}
 
 	/* if multicast bit is set, the alternate address will not be used */
-	if (is_multicast_ether_addr(alt_mac_addr)) {
-		hw_dbg("Ignoring Alternate Mac Address with MC bit set\n");
-		goto out;
+	if (alt_mac_addr[0] & 0x01) {
+		DEBUGOUT("Ignoring Alternate Mac Address with MC bit set\n");
+		return E1000_SUCCESS;
 	}
 
-	/*
-	 * We have a valid alternate MAC address, and we want to treat it the
+	/* We have a valid alternate MAC address, and we want to treat it the
 	 * same as the normal permanent MAC address stored by the HW into the
 	 * RAR. Do this by mapping this address into RAR0.
 	 */
 	hw->mac.ops.rar_set(hw, alt_mac_addr, 0);
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_rar_set - Set receive address register
+ *  e1000_rar_set_generic - Set receive address register
  *  @hw: pointer to the HW structure
  *  @addr: pointer to the receive address
  *  @index: receive address array register
@@ -243,17 +376,17 @@ out:
  *  Sets the receive address array register at index to the address passed
  *  in by addr.
  **/
-void igb_rar_set(struct e1000_hw *hw, u8 *addr, u32 index)
+static void e1000_rar_set_generic(struct e1000_hw *hw, u8 *addr, u32 index)
 {
 	u32 rar_low, rar_high;
 
-	/*
-	 * HW expects these in little endian so we reverse the byte order
+	DEBUGFUNC("e1000_rar_set_generic");
+
+	/* HW expects these in little endian so we reverse the byte order
 	 * from network order (big endian) to little endian
 	 */
-	rar_low = ((u32) addr[0] |
-		   ((u32) addr[1] << 8) |
-		    ((u32) addr[2] << 16) | ((u32) addr[3] << 24));
+	rar_low = ((u32) addr[0] | ((u32) addr[1] << 8) |
+		   ((u32) addr[2] << 16) | ((u32) addr[3] << 24));
 
 	rar_high = ((u32) addr[4] | ((u32) addr[5] << 8));
 
@@ -261,78 +394,41 @@ void igb_rar_set(struct e1000_hw *hw, u8 *addr, u32 index)
 	if (rar_low || rar_high)
 		rar_high |= E1000_RAH_AV;
 
-	/*
-	 * Some bridges will combine consecutive 32-bit writes into
+	/* Some bridges will combine consecutive 32-bit writes into
 	 * a single burst write, which will malfunction on some parts.
 	 * The flushes avoid this.
 	 */
-	wr32(E1000_RAL(index), rar_low);
-	wrfl();
-	wr32(E1000_RAH(index), rar_high);
-	wrfl();
+	E1000_WRITE_REG(hw, E1000_RAL(index), rar_low);
+	E1000_WRITE_FLUSH(hw);
+	E1000_WRITE_REG(hw, E1000_RAH(index), rar_high);
+	E1000_WRITE_FLUSH(hw);
 }
 
 /**
- *  igb_mta_set - Set multicast filter table address
- *  @hw: pointer to the HW structure
- *  @hash_value: determines the MTA register and bit to set
- *
- *  The multicast table address is a register array of 32-bit registers.
- *  The hash_value is used to determine what register the bit is in, the
- *  current value is read, the new bit is OR'd in and the new value is
- *  written back into the register.
- **/
-void igb_mta_set(struct e1000_hw *hw, u32 hash_value)
-{
-	u32 hash_bit, hash_reg, mta;
-
-	/*
-	 * The MTA is a register array of 32-bit registers. It is
-	 * treated like an array of (32*mta_reg_count) bits.  We want to
-	 * set bit BitArray[hash_value]. So we figure out what register
-	 * the bit is in, read it, OR in the new bit, then write
-	 * back the new value.  The (hw->mac.mta_reg_count - 1) serves as a
-	 * mask to bits 31:5 of the hash value which gives us the
-	 * register we're modifying.  The hash bit within that register
-	 * is determined by the lower 5 bits of the hash value.
-	 */
-	hash_reg = (hash_value >> 5) & (hw->mac.mta_reg_count - 1);
-	hash_bit = hash_value & 0x1F;
-
-	mta = array_rd32(E1000_MTA, hash_reg);
-
-	mta |= (1 << hash_bit);
-
-	array_wr32(E1000_MTA, hash_reg, mta);
-	wrfl();
-}
-
-/**
- *  igb_hash_mc_addr - Generate a multicast hash value
+ *  e1000_hash_mc_addr_generic - Generate a multicast hash value
  *  @hw: pointer to the HW structure
  *  @mc_addr: pointer to a multicast address
  *
  *  Generates a multicast address hash value which is used to determine
- *  the multicast filter table array address and new table value.  See
- *  igb_mta_set()
+ *  the multicast filter table array address and new table value.
  **/
-static u32 igb_hash_mc_addr(struct e1000_hw *hw, u8 *mc_addr)
+u32 e1000_hash_mc_addr_generic(struct e1000_hw *hw, u8 *mc_addr)
 {
 	u32 hash_value, hash_mask;
 	u8 bit_shift = 0;
 
+	DEBUGFUNC("e1000_hash_mc_addr_generic");
+
 	/* Register count multiplied by bits per register */
 	hash_mask = (hw->mac.mta_reg_count * 32) - 1;
 
-	/*
-	 * For a mc_filter_type of 0, bit_shift is the number of left-shifts
+	/* For a mc_filter_type of 0, bit_shift is the number of left-shifts
 	 * where 0xFF would still fall within the hash mask.
 	 */
 	while (hash_mask >> bit_shift != 0xFF)
 		bit_shift++;
 
-	/*
-	 * The portion of the address that is used for the hash table
+	/* The portion of the address that is used for the hash table
 	 * is determined by the mc_filter_type setting.
 	 * The algorithm is such that there is a total of 8 bits of shifting.
 	 * The bit_shift for a mc_filter_type of 0 represents the number of
@@ -350,7 +446,7 @@ static u32 igb_hash_mc_addr(struct e1000_hw *hw, u8 *mc_addr)
 	 * values resulting from each mc_filter_type...
 	 * [0] [1] [2] [3] [4] [5]
 	 * 01  AA  00  12  34  56
-	 * LSB                 MSB
+	 * LSB		 MSB
 	 *
 	 * case 0: hash_value = ((0x34 >> 4) | (0x56 << 4)) & 0xFFF = 0x563
 	 * case 1: hash_value = ((0x34 >> 3) | (0x56 << 5)) & 0xFFF = 0xAC6
@@ -379,7 +475,7 @@ static u32 igb_hash_mc_addr(struct e1000_hw *hw, u8 *mc_addr)
 }
 
 /**
- *  igb_update_mc_addr_list - Update Multicast addresses
+ *  e1000_update_mc_addr_list_generic - Update Multicast addresses
  *  @hw: pointer to the HW structure
  *  @mc_addr_list: array of multicast addresses to program
  *  @mc_addr_count: number of multicast addresses to program
@@ -387,156 +483,376 @@ static u32 igb_hash_mc_addr(struct e1000_hw *hw, u8 *mc_addr)
  *  Updates entire Multicast Table Array.
  *  The caller must have a packed mc_addr_list of multicast addresses.
  **/
-void igb_update_mc_addr_list(struct e1000_hw *hw,
-                             u8 *mc_addr_list, u32 mc_addr_count)
+void e1000_update_mc_addr_list_generic(struct e1000_hw *hw,
+				       u8 *mc_addr_list, u32 mc_addr_count)
 {
 	u32 hash_value, hash_bit, hash_reg;
 	int i;
 
+	DEBUGFUNC("e1000_update_mc_addr_list_generic");
+
 	/* clear mta_shadow */
 	memset(&hw->mac.mta_shadow, 0, sizeof(hw->mac.mta_shadow));
 
 	/* update mta_shadow from mc_addr_list */
 	for (i = 0; (u32) i < mc_addr_count; i++) {
-		hash_value = igb_hash_mc_addr(hw, mc_addr_list);
+		hash_value = e1000_hash_mc_addr_generic(hw, mc_addr_list);
 
 		hash_reg = (hash_value >> 5) & (hw->mac.mta_reg_count - 1);
 		hash_bit = hash_value & 0x1F;
 
 		hw->mac.mta_shadow[hash_reg] |= (1 << hash_bit);
-		mc_addr_list += (ETH_ALEN);
+		mc_addr_list += (ETH_ADDR_LEN);
 	}
 
 	/* replace the entire MTA table */
 	for (i = hw->mac.mta_reg_count - 1; i >= 0; i--)
-		array_wr32(E1000_MTA, i, hw->mac.mta_shadow[i]);
-	wrfl();
+		E1000_WRITE_REG_ARRAY(hw, E1000_MTA, i, hw->mac.mta_shadow[i]);
+	E1000_WRITE_FLUSH(hw);
 }
 
 /**
- *  igb_clear_hw_cntrs_base - Clear base hardware counters
+ *  e1000_clear_hw_cntrs_base_generic - Clear base hardware counters
  *  @hw: pointer to the HW structure
  *
  *  Clears the base hardware counters by reading the counter registers.
  **/
-void igb_clear_hw_cntrs_base(struct e1000_hw *hw)
+void e1000_clear_hw_cntrs_base_generic(struct e1000_hw *hw)
 {
-	rd32(E1000_CRCERRS);
-	rd32(E1000_SYMERRS);
-	rd32(E1000_MPC);
-	rd32(E1000_SCC);
-	rd32(E1000_ECOL);
-	rd32(E1000_MCC);
-	rd32(E1000_LATECOL);
-	rd32(E1000_COLC);
-	rd32(E1000_DC);
-	rd32(E1000_SEC);
-	rd32(E1000_RLEC);
-	rd32(E1000_XONRXC);
-	rd32(E1000_XONTXC);
-	rd32(E1000_XOFFRXC);
-	rd32(E1000_XOFFTXC);
-	rd32(E1000_FCRUC);
-	rd32(E1000_GPRC);
-	rd32(E1000_BPRC);
-	rd32(E1000_MPRC);
-	rd32(E1000_GPTC);
-	rd32(E1000_GORCL);
-	rd32(E1000_GORCH);
-	rd32(E1000_GOTCL);
-	rd32(E1000_GOTCH);
-	rd32(E1000_RNBC);
-	rd32(E1000_RUC);
-	rd32(E1000_RFC);
-	rd32(E1000_ROC);
-	rd32(E1000_RJC);
-	rd32(E1000_TORL);
-	rd32(E1000_TORH);
-	rd32(E1000_TOTL);
-	rd32(E1000_TOTH);
-	rd32(E1000_TPR);
-	rd32(E1000_TPT);
-	rd32(E1000_MPTC);
-	rd32(E1000_BPTC);
+	DEBUGFUNC("e1000_clear_hw_cntrs_base_generic");
+
+	E1000_READ_REG(hw, E1000_CRCERRS);
+	E1000_READ_REG(hw, E1000_SYMERRS);
+	E1000_READ_REG(hw, E1000_MPC);
+	E1000_READ_REG(hw, E1000_SCC);
+	E1000_READ_REG(hw, E1000_ECOL);
+	E1000_READ_REG(hw, E1000_MCC);
+	E1000_READ_REG(hw, E1000_LATECOL);
+	E1000_READ_REG(hw, E1000_COLC);
+	E1000_READ_REG(hw, E1000_DC);
+	E1000_READ_REG(hw, E1000_SEC);
+	E1000_READ_REG(hw, E1000_RLEC);
+	E1000_READ_REG(hw, E1000_XONRXC);
+	E1000_READ_REG(hw, E1000_XONTXC);
+	E1000_READ_REG(hw, E1000_XOFFRXC);
+	E1000_READ_REG(hw, E1000_XOFFTXC);
+	E1000_READ_REG(hw, E1000_FCRUC);
+	E1000_READ_REG(hw, E1000_GPRC);
+	E1000_READ_REG(hw, E1000_BPRC);
+	E1000_READ_REG(hw, E1000_MPRC);
+	E1000_READ_REG(hw, E1000_GPTC);
+	E1000_READ_REG(hw, E1000_GORCL);
+	E1000_READ_REG(hw, E1000_GORCH);
+	E1000_READ_REG(hw, E1000_GOTCL);
+	E1000_READ_REG(hw, E1000_GOTCH);
+	E1000_READ_REG(hw, E1000_RNBC);
+	E1000_READ_REG(hw, E1000_RUC);
+	E1000_READ_REG(hw, E1000_RFC);
+	E1000_READ_REG(hw, E1000_ROC);
+	E1000_READ_REG(hw, E1000_RJC);
+	E1000_READ_REG(hw, E1000_TORL);
+	E1000_READ_REG(hw, E1000_TORH);
+	E1000_READ_REG(hw, E1000_TOTL);
+	E1000_READ_REG(hw, E1000_TOTH);
+	E1000_READ_REG(hw, E1000_TPR);
+	E1000_READ_REG(hw, E1000_TPT);
+	E1000_READ_REG(hw, E1000_MPTC);
+	E1000_READ_REG(hw, E1000_BPTC);
 }
 
 /**
- *  igb_check_for_copper_link - Check for link (Copper)
+ *  e1000_check_for_copper_link_generic - Check for link (Copper)
  *  @hw: pointer to the HW structure
  *
  *  Checks to see of the link status of the hardware has changed.  If a
  *  change in link status has been detected, then we read the PHY registers
  *  to get the current speed/duplex if link exists.
  **/
-s32 igb_check_for_copper_link(struct e1000_hw *hw)
+s32 e1000_check_for_copper_link_generic(struct e1000_hw *hw)
 {
 	struct e1000_mac_info *mac = &hw->mac;
 	s32 ret_val;
 	bool link;
 
-	/*
-	 * We only want to go out to the PHY registers to see if Auto-Neg
+	DEBUGFUNC("e1000_check_for_copper_link");
+
+	/* We only want to go out to the PHY registers to see if Auto-Neg
 	 * has completed and/or if our link status has changed.  The
 	 * get_link_status flag is set upon receiving a Link Status
 	 * Change or Rx Sequence Error interrupt.
 	 */
-	if (!mac->get_link_status) {
-		ret_val = 0;
-		goto out;
-	}
+	if (!mac->get_link_status)
+		return E1000_SUCCESS;
 
-	/*
-	 * First we want to see if the MII Status Register reports
+	/* First we want to see if the MII Status Register reports
 	 * link.  If so, then we want to get the current speed/duplex
 	 * of the PHY.
 	 */
-	ret_val = igb_phy_has_link(hw, 1, 0, &link);
+	ret_val = e1000_phy_has_link_generic(hw, 1, 0, &link);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	if (!link)
-		goto out; /* No link detected */
+		return E1000_SUCCESS; /* No link detected */
 
 	mac->get_link_status = false;
 
-	/*
-	 * Check if there was DownShift, must be checked
+	/* Check if there was DownShift, must be checked
 	 * immediately after link-up
 	 */
-	igb_check_downshift(hw);
+	e1000_check_downshift_generic(hw);
 
-	/*
-	 * If we are forcing speed/duplex, then we simply return since
+	/* If we are forcing speed/duplex, then we simply return since
 	 * we have already determined whether we have link or not.
 	 */
-	if (!mac->autoneg) {
-		ret_val = -E1000_ERR_CONFIG;
-		goto out;
-	}
+	if (!mac->autoneg)
+		return -E1000_ERR_CONFIG;
 
-	/*
-	 * Auto-Neg is enabled.  Auto Speed Detection takes care
+	/* Auto-Neg is enabled.  Auto Speed Detection takes care
 	 * of MAC speed/duplex configuration.  So we only need to
 	 * configure Collision Distance in the MAC.
 	 */
-	igb_config_collision_dist(hw);
+	mac->ops.config_collision_dist(hw);
 
-	/*
-	 * Configure Flow Control now that Auto-Neg has completed.
+	/* Configure Flow Control now that Auto-Neg has completed.
 	 * First, we need to restore the desired flow control
 	 * settings because we may have had to re-autoneg with a
 	 * different link partner.
 	 */
-	ret_val = igb_config_fc_after_link_up(hw);
+	ret_val = e1000_config_fc_after_link_up_generic(hw);
 	if (ret_val)
-		hw_dbg("Error configuring flow control\n");
+		DEBUGOUT("Error configuring flow control\n");
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_setup_link - Setup flow control and link settings
+ *  e1000_check_for_fiber_link_generic - Check for link (Fiber)
+ *  @hw: pointer to the HW structure
+ *
+ *  Checks for link up on the hardware.  If link is not up and we have
+ *  a signal, then we need to force link up.
+ **/
+s32 e1000_check_for_fiber_link_generic(struct e1000_hw *hw)
+{
+	struct e1000_mac_info *mac = &hw->mac;
+	u32 rxcw;
+	u32 ctrl;
+	u32 status;
+	s32 ret_val;
+
+	DEBUGFUNC("e1000_check_for_fiber_link_generic");
+
+	ctrl = E1000_READ_REG(hw, E1000_CTRL);
+	status = E1000_READ_REG(hw, E1000_STATUS);
+	rxcw = E1000_READ_REG(hw, E1000_RXCW);
+
+	/* If we don't have link (auto-negotiation failed or link partner
+	 * cannot auto-negotiate), the cable is plugged in (we have signal),
+	 * and our link partner is not trying to auto-negotiate with us (we
+	 * are receiving idles or data), we need to force link up. We also
+	 * need to give auto-negotiation time to complete, in case the cable
+	 * was just plugged in. The autoneg_failed flag does this.
+	 */
+	/* (ctrl & E1000_CTRL_SWDPIN1) == 1 == have signal */
+	if ((ctrl & E1000_CTRL_SWDPIN1) && !(status & E1000_STATUS_LU) &&
+	    !(rxcw & E1000_RXCW_C)) {
+		if (!mac->autoneg_failed) {
+			mac->autoneg_failed = true;
+			return E1000_SUCCESS;
+		}
+		DEBUGOUT("NOT Rx'ing /C/, disable AutoNeg and force link.\n");
+
+		/* Disable auto-negotiation in the TXCW register */
+		E1000_WRITE_REG(hw, E1000_TXCW, (mac->txcw & ~E1000_TXCW_ANE));
+
+		/* Force link-up and also force full-duplex. */
+		ctrl = E1000_READ_REG(hw, E1000_CTRL);
+		ctrl |= (E1000_CTRL_SLU | E1000_CTRL_FD);
+		E1000_WRITE_REG(hw, E1000_CTRL, ctrl);
+
+		/* Configure Flow Control after forcing link up. */
+		ret_val = e1000_config_fc_after_link_up_generic(hw);
+		if (ret_val) {
+			DEBUGOUT("Error configuring flow control\n");
+			return ret_val;
+		}
+	} else if ((ctrl & E1000_CTRL_SLU) && (rxcw & E1000_RXCW_C)) {
+		/* If we are forcing link and we are receiving /C/ ordered
+		 * sets, re-enable auto-negotiation in the TXCW register
+		 * and disable forced link in the Device Control register
+		 * in an attempt to auto-negotiate with our link partner.
+		 */
+		DEBUGOUT("Rx'ing /C/, enable AutoNeg and stop forcing link.\n");
+		E1000_WRITE_REG(hw, E1000_TXCW, mac->txcw);
+		E1000_WRITE_REG(hw, E1000_CTRL, (ctrl & ~E1000_CTRL_SLU));
+
+		mac->serdes_has_link = true;
+	}
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_check_for_serdes_link_generic - Check for link (Serdes)
+ *  @hw: pointer to the HW structure
+ *
+ *  Checks for link up on the hardware.  If link is not up and we have
+ *  a signal, then we need to force link up.
+ **/
+s32 e1000_check_for_serdes_link_generic(struct e1000_hw *hw)
+{
+	struct e1000_mac_info *mac = &hw->mac;
+	u32 rxcw;
+	u32 ctrl;
+	u32 status;
+	s32 ret_val;
+
+	DEBUGFUNC("e1000_check_for_serdes_link_generic");
+
+	ctrl = E1000_READ_REG(hw, E1000_CTRL);
+	status = E1000_READ_REG(hw, E1000_STATUS);
+	rxcw = E1000_READ_REG(hw, E1000_RXCW);
+
+	/* If we don't have link (auto-negotiation failed or link partner
+	 * cannot auto-negotiate), and our link partner is not trying to
+	 * auto-negotiate with us (we are receiving idles or data),
+	 * we need to force link up. We also need to give auto-negotiation
+	 * time to complete.
+	 */
+	/* (ctrl & E1000_CTRL_SWDPIN1) == 1 == have signal */
+	if (!(status & E1000_STATUS_LU) && !(rxcw & E1000_RXCW_C)) {
+		if (!mac->autoneg_failed) {
+			mac->autoneg_failed = true;
+			return E1000_SUCCESS;
+		}
+		DEBUGOUT("NOT Rx'ing /C/, disable AutoNeg and force link.\n");
+
+		/* Disable auto-negotiation in the TXCW register */
+		E1000_WRITE_REG(hw, E1000_TXCW, (mac->txcw & ~E1000_TXCW_ANE));
+
+		/* Force link-up and also force full-duplex. */
+		ctrl = E1000_READ_REG(hw, E1000_CTRL);
+		ctrl |= (E1000_CTRL_SLU | E1000_CTRL_FD);
+		E1000_WRITE_REG(hw, E1000_CTRL, ctrl);
+
+		/* Configure Flow Control after forcing link up. */
+		ret_val = e1000_config_fc_after_link_up_generic(hw);
+		if (ret_val) {
+			DEBUGOUT("Error configuring flow control\n");
+			return ret_val;
+		}
+	} else if ((ctrl & E1000_CTRL_SLU) && (rxcw & E1000_RXCW_C)) {
+		/* If we are forcing link and we are receiving /C/ ordered
+		 * sets, re-enable auto-negotiation in the TXCW register
+		 * and disable forced link in the Device Control register
+		 * in an attempt to auto-negotiate with our link partner.
+		 */
+		DEBUGOUT("Rx'ing /C/, enable AutoNeg and stop forcing link.\n");
+		E1000_WRITE_REG(hw, E1000_TXCW, mac->txcw);
+		E1000_WRITE_REG(hw, E1000_CTRL, (ctrl & ~E1000_CTRL_SLU));
+
+		mac->serdes_has_link = true;
+	} else if (!(E1000_TXCW_ANE & E1000_READ_REG(hw, E1000_TXCW))) {
+		/* If we force link for non-auto-negotiation switch, check
+		 * link status based on MAC synchronization for internal
+		 * serdes media type.
+		 */
+		/* SYNCH bit and IV bit are sticky. */
+		usec_delay(10);
+		rxcw = E1000_READ_REG(hw, E1000_RXCW);
+		if (rxcw & E1000_RXCW_SYNCH) {
+			if (!(rxcw & E1000_RXCW_IV)) {
+				mac->serdes_has_link = true;
+				DEBUGOUT("SERDES: Link up - forced.\n");
+			}
+		} else {
+			mac->serdes_has_link = false;
+			DEBUGOUT("SERDES: Link down - force failed.\n");
+		}
+	}
+
+	if (E1000_TXCW_ANE & E1000_READ_REG(hw, E1000_TXCW)) {
+		status = E1000_READ_REG(hw, E1000_STATUS);
+		if (status & E1000_STATUS_LU) {
+			/* SYNCH bit and IV bit are sticky, so reread rxcw. */
+			usec_delay(10);
+			rxcw = E1000_READ_REG(hw, E1000_RXCW);
+			if (rxcw & E1000_RXCW_SYNCH) {
+				if (!(rxcw & E1000_RXCW_IV)) {
+					mac->serdes_has_link = true;
+					DEBUGOUT("SERDES: Link up - autoneg completed successfully.\n");
+				} else {
+					mac->serdes_has_link = false;
+					DEBUGOUT("SERDES: Link down - invalid codewords detected in autoneg.\n");
+				}
+			} else {
+				mac->serdes_has_link = false;
+				DEBUGOUT("SERDES: Link down - no sync.\n");
+			}
+		} else {
+			mac->serdes_has_link = false;
+			DEBUGOUT("SERDES: Link down - autoneg failed\n");
+		}
+	}
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_set_default_fc_generic - Set flow control default values
+ *  @hw: pointer to the HW structure
+ *
+ *  Read the EEPROM for the default values for flow control and store the
+ *  values.
+ **/
+static s32 e1000_set_default_fc_generic(struct e1000_hw *hw)
+{
+	s32 ret_val;
+	u16 nvm_data;
+	u16 nvm_offset = 0;
+
+	DEBUGFUNC("e1000_set_default_fc_generic");
+
+	/* Read and store word 0x0F of the EEPROM. This word contains bits
+	 * that determine the hardware's default PAUSE (flow control) mode,
+	 * a bit that determines whether the HW defaults to enabling or
+	 * disabling auto-negotiation, and the direction of the
+	 * SW defined pins. If there is no SW over-ride of the flow
+	 * control setting, then the variable hw->fc will
+	 * be initialized based on a value in the EEPROM.
+	 */
+	if (hw->mac.type == e1000_i350) {
+		nvm_offset = NVM_82580_LAN_FUNC_OFFSET(hw->bus.func);
+		ret_val = hw->nvm.ops.read(hw,
+					   NVM_INIT_CONTROL2_REG +
+					   nvm_offset,
+					   1, &nvm_data);
+	} else {
+		ret_val = hw->nvm.ops.read(hw,
+					   NVM_INIT_CONTROL2_REG,
+					   1, &nvm_data);
+	}
+
+
+	if (ret_val) {
+		DEBUGOUT("NVM Read Error\n");
+		return ret_val;
+	}
+
+	if (!(nvm_data & NVM_WORD0F_PAUSE_MASK))
+		hw->fc.requested_mode = e1000_fc_none;
+	else if ((nvm_data & NVM_WORD0F_PAUSE_MASK) ==
+		 NVM_WORD0F_ASM_DIR)
+		hw->fc.requested_mode = e1000_fc_tx_pause;
+	else
+		hw->fc.requested_mode = e1000_fc_full;
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_setup_link_generic - Setup flow control and link settings
  *  @hw: pointer to the HW structure
  *
  *  Determines which flow control settings to use, then configures flow
@@ -545,104 +861,268 @@ out:
  *  should be established.  Assumes the hardware has previously been reset
  *  and the transmitter and receiver are not enabled.
  **/
-s32 igb_setup_link(struct e1000_hw *hw)
+s32 e1000_setup_link_generic(struct e1000_hw *hw)
 {
-	s32 ret_val = 0;
+	s32 ret_val;
+
+	DEBUGFUNC("e1000_setup_link_generic");
 
-	/*
-	 * In the case of the phy reset being blocked, we already have a link.
+	/* In the case of the phy reset being blocked, we already have a link.
 	 * We do not need to set it up again.
 	 */
-	if (igb_check_reset_block(hw))
-		goto out;
+	if (hw->phy.ops.check_reset_block && hw->phy.ops.check_reset_block(hw))
+		return E1000_SUCCESS;
 
-	/*
-	 * If requested flow control is set to default, set flow control
+	/* If requested flow control is set to default, set flow control
 	 * based on the EEPROM flow control settings.
 	 */
 	if (hw->fc.requested_mode == e1000_fc_default) {
-		ret_val = igb_set_default_fc(hw);
+		ret_val = e1000_set_default_fc_generic(hw);
 		if (ret_val)
-			goto out;
+			return ret_val;
 	}
 
-	/*
-	 * We want to save off the original Flow Control configuration just
-	 * in case we get disconnected and then reconnected into a different
-	 * hub or switch with different Flow Control capabilities.
+	/* Save off the requested flow control mode for use later.  Depending
+	 * on the link partner's capabilities, we may or may not use this mode.
 	 */
 	hw->fc.current_mode = hw->fc.requested_mode;
 
-	hw_dbg("After fix-ups FlowControl is now = %x\n", hw->fc.current_mode);
+	DEBUGOUT1("After fix-ups FlowControl is now = %x\n",
+		hw->fc.current_mode);
 
 	/* Call the necessary media_type subroutine to configure the link. */
 	ret_val = hw->mac.ops.setup_physical_interface(hw);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	/*
-	 * Initialize the flow control address, type, and PAUSE timer
+	/* Initialize the flow control address, type, and PAUSE timer
 	 * registers to their default values.  This is done even if flow
 	 * control is disabled, because it does not hurt anything to
 	 * initialize these registers.
 	 */
-	hw_dbg("Initializing the Flow Control address, type and timer regs\n");
-	wr32(E1000_FCT, FLOW_CONTROL_TYPE);
-	wr32(E1000_FCAH, FLOW_CONTROL_ADDRESS_HIGH);
-	wr32(E1000_FCAL, FLOW_CONTROL_ADDRESS_LOW);
+	DEBUGOUT("Initializing the Flow Control address, type and timer regs\n");
+	E1000_WRITE_REG(hw, E1000_FCT, FLOW_CONTROL_TYPE);
+	E1000_WRITE_REG(hw, E1000_FCAH, FLOW_CONTROL_ADDRESS_HIGH);
+	E1000_WRITE_REG(hw, E1000_FCAL, FLOW_CONTROL_ADDRESS_LOW);
+
+	E1000_WRITE_REG(hw, E1000_FCTTV, hw->fc.pause_time);
+
+	return e1000_set_fc_watermarks_generic(hw);
+}
+
+/**
+ *  e1000_commit_fc_settings_generic - Configure flow control
+ *  @hw: pointer to the HW structure
+ *
+ *  Write the flow control settings to the Transmit Config Word Register (TXCW)
+ *  base on the flow control settings in e1000_mac_info.
+ **/
+static s32 e1000_commit_fc_settings_generic(struct e1000_hw *hw)
+{
+	struct e1000_mac_info *mac = &hw->mac;
+	u32 txcw;
+
+	DEBUGFUNC("e1000_commit_fc_settings_generic");
+
+	/* Check for a software override of the flow control settings, and
+	 * setup the device accordingly.  If auto-negotiation is enabled, then
+	 * software will have to set the "PAUSE" bits to the correct value in
+	 * the Transmit Config Word Register (TXCW) and re-start auto-
+	 * negotiation.  However, if auto-negotiation is disabled, then
+	 * software will have to manually configure the two flow control enable
+	 * bits in the CTRL register.
+	 *
+	 * The possible values of the "fc" parameter are:
+	 *      0:  Flow control is completely disabled
+	 *      1:  Rx flow control is enabled (we can receive pause frames,
+	 *          but not send pause frames).
+	 *      2:  Tx flow control is enabled (we can send pause frames but we
+	 *          do not support receiving pause frames).
+	 *      3:  Both Rx and Tx flow control (symmetric) are enabled.
+	 */
+	switch (hw->fc.current_mode) {
+	case e1000_fc_none:
+		/* Flow control completely disabled by a software over-ride. */
+		txcw = (E1000_TXCW_ANE | E1000_TXCW_FD);
+		break;
+	case e1000_fc_rx_pause:
+		/* Rx Flow control is enabled and Tx Flow control is disabled
+		 * by a software over-ride. Since there really isn't a way to
+		 * advertise that we are capable of Rx Pause ONLY, we will
+		 * advertise that we support both symmetric and asymmetric Rx
+		 * PAUSE.  Later, we will disable the adapter's ability to send
+		 * PAUSE frames.
+		 */
+		txcw = (E1000_TXCW_ANE | E1000_TXCW_FD | E1000_TXCW_PAUSE_MASK);
+		break;
+	case e1000_fc_tx_pause:
+		/* Tx Flow control is enabled, and Rx Flow control is disabled,
+		 * by a software over-ride.
+		 */
+		txcw = (E1000_TXCW_ANE | E1000_TXCW_FD | E1000_TXCW_ASM_DIR);
+		break;
+	case e1000_fc_full:
+		/* Flow control (both Rx and Tx) is enabled by a software
+		 * over-ride.
+		 */
+		txcw = (E1000_TXCW_ANE | E1000_TXCW_FD | E1000_TXCW_PAUSE_MASK);
+		break;
+	default:
+		DEBUGOUT("Flow control param set incorrectly\n");
+		return -E1000_ERR_CONFIG;
+		break;
+	}
+
+	E1000_WRITE_REG(hw, E1000_TXCW, txcw);
+	mac->txcw = txcw;
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_poll_fiber_serdes_link_generic - Poll for link up
+ *  @hw: pointer to the HW structure
+ *
+ *  Polls for link up by reading the status register, if link fails to come
+ *  up with auto-negotiation, then the link is forced if a signal is detected.
+ **/
+static s32 e1000_poll_fiber_serdes_link_generic(struct e1000_hw *hw)
+{
+	struct e1000_mac_info *mac = &hw->mac;
+	u32 i, status;
+	s32 ret_val;
+
+	DEBUGFUNC("e1000_poll_fiber_serdes_link_generic");
+
+	/* If we have a signal (the cable is plugged in, or assumed true for
+	 * serdes media) then poll for a "Link-Up" indication in the Device
+	 * Status Register.  Time-out if a link isn't seen in 500 milliseconds
+	 * seconds (Auto-negotiation should complete in less than 500
+	 * milliseconds even if the other end is doing it in SW).
+	 */
+	for (i = 0; i < FIBER_LINK_UP_LIMIT; i++) {
+		msec_delay(10);
+		status = E1000_READ_REG(hw, E1000_STATUS);
+		if (status & E1000_STATUS_LU)
+			break;
+	}
+	if (i == FIBER_LINK_UP_LIMIT) {
+		DEBUGOUT("Never got a valid link from auto-neg!!!\n");
+		mac->autoneg_failed = true;
+		/* AutoNeg failed to achieve a link, so we'll call
+		 * mac->check_for_link. This routine will force the
+		 * link up if we detect a signal. This will allow us to
+		 * communicate with non-autonegotiating link partners.
+		 */
+		ret_val = mac->ops.check_for_link(hw);
+		if (ret_val) {
+			DEBUGOUT("Error while checking for link\n");
+			return ret_val;
+		}
+		mac->autoneg_failed = false;
+	} else {
+		mac->autoneg_failed = false;
+		DEBUGOUT("Valid Link Found\n");
+	}
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_setup_fiber_serdes_link_generic - Setup link for fiber/serdes
+ *  @hw: pointer to the HW structure
+ *
+ *  Configures collision distance and flow control for fiber and serdes
+ *  links.  Upon successful setup, poll for link.
+ **/
+s32 e1000_setup_fiber_serdes_link_generic(struct e1000_hw *hw)
+{
+	u32 ctrl;
+	s32 ret_val;
+
+	DEBUGFUNC("e1000_setup_fiber_serdes_link_generic");
+
+	ctrl = E1000_READ_REG(hw, E1000_CTRL);
+
+	/* Take the link out of reset */
+	ctrl &= ~E1000_CTRL_LRST;
+
+	hw->mac.ops.config_collision_dist(hw);
+
+	ret_val = e1000_commit_fc_settings_generic(hw);
+	if (ret_val)
+		return ret_val;
+
+	/* Since auto-negotiation is enabled, take the link out of reset (the
+	 * link will be in reset, because we previously reset the chip). This
+	 * will restart auto-negotiation.  If auto-negotiation is successful
+	 * then the link-up status bit will be set and the flow control enable
+	 * bits (RFCE and TFCE) will be set according to their negotiated value.
+	 */
+	DEBUGOUT("Auto-negotiation enabled\n");
 
-	wr32(E1000_FCTTV, hw->fc.pause_time);
+	E1000_WRITE_REG(hw, E1000_CTRL, ctrl);
+	E1000_WRITE_FLUSH(hw);
+	msec_delay(1);
 
-	ret_val = igb_set_fc_watermarks(hw);
+	/* For these adapters, the SW definable pin 1 is set when the optics
+	 * detect a signal.  If we have a signal, then poll for a "Link-Up"
+	 * indication.
+	 */
+	if (hw->phy.media_type == e1000_media_type_internal_serdes ||
+	    (E1000_READ_REG(hw, E1000_CTRL) & E1000_CTRL_SWDPIN1)) {
+		ret_val = e1000_poll_fiber_serdes_link_generic(hw);
+	} else {
+		DEBUGOUT("No signal detected\n");
+	}
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_config_collision_dist - Configure collision distance
+ *  e1000_config_collision_dist_generic - Configure collision distance
  *  @hw: pointer to the HW structure
  *
  *  Configures the collision distance to the default value and is used
- *  during link setup. Currently no func pointer exists and all
- *  implementations are handled in the generic version of this function.
+ *  during link setup.
  **/
-void igb_config_collision_dist(struct e1000_hw *hw)
+static void e1000_config_collision_dist_generic(struct e1000_hw *hw)
 {
 	u32 tctl;
 
-	tctl = rd32(E1000_TCTL);
+	DEBUGFUNC("e1000_config_collision_dist_generic");
+
+	tctl = E1000_READ_REG(hw, E1000_TCTL);
 
 	tctl &= ~E1000_TCTL_COLD;
 	tctl |= E1000_COLLISION_DISTANCE << E1000_COLD_SHIFT;
 
-	wr32(E1000_TCTL, tctl);
-	wrfl();
+	E1000_WRITE_REG(hw, E1000_TCTL, tctl);
+	E1000_WRITE_FLUSH(hw);
 }
 
 /**
- *  igb_set_fc_watermarks - Set flow control high/low watermarks
+ *  e1000_set_fc_watermarks_generic - Set flow control high/low watermarks
  *  @hw: pointer to the HW structure
  *
  *  Sets the flow control high/low threshold (watermark) registers.  If
  *  flow control XON frame transmission is enabled, then set XON frame
- *  tansmission as well.
+ *  transmission as well.
  **/
-static s32 igb_set_fc_watermarks(struct e1000_hw *hw)
+s32 e1000_set_fc_watermarks_generic(struct e1000_hw *hw)
 {
-	s32 ret_val = 0;
 	u32 fcrtl = 0, fcrth = 0;
 
-	/*
-	 * Set the flow control receive threshold registers.  Normally,
+	DEBUGFUNC("e1000_set_fc_watermarks_generic");
+
+	/* Set the flow control receive threshold registers.  Normally,
 	 * these registers will be set to a default threshold that may be
 	 * adjusted later by the driver's runtime code.  However, if the
 	 * ability to transmit pause frames is not enabled, then these
 	 * registers will be set to 0.
 	 */
 	if (hw->fc.current_mode & e1000_fc_tx_pause) {
-		/*
-		 * We need to set up the Receive Threshold high and low water
+		/* We need to set up the Receive Threshold high and low water
 		 * marks as well as (optionally) enabling the transmission of
 		 * XON frames.
 		 */
@@ -652,54 +1132,14 @@ static s32 igb_set_fc_watermarks(struct e1000_hw *hw)
 
 		fcrth = hw->fc.high_water;
 	}
-	wr32(E1000_FCRTL, fcrtl);
-	wr32(E1000_FCRTH, fcrth);
-
-	return ret_val;
-}
-
-/**
- *  igb_set_default_fc - Set flow control default values
- *  @hw: pointer to the HW structure
- *
- *  Read the EEPROM for the default values for flow control and store the
- *  values.
- **/
-static s32 igb_set_default_fc(struct e1000_hw *hw)
-{
-	s32 ret_val = 0;
-	u16 nvm_data;
-
-	/*
-	 * Read and store word 0x0F of the EEPROM. This word contains bits
-	 * that determine the hardware's default PAUSE (flow control) mode,
-	 * a bit that determines whether the HW defaults to enabling or
-	 * disabling auto-negotiation, and the direction of the
-	 * SW defined pins. If there is no SW over-ride of the flow
-	 * control setting, then the variable hw->fc will
-	 * be initialized based on a value in the EEPROM.
-	 */
-	ret_val = hw->nvm.ops.read(hw, NVM_INIT_CONTROL2_REG, 1, &nvm_data);
-
-	if (ret_val) {
-		hw_dbg("NVM Read Error\n");
-		goto out;
-	}
+	E1000_WRITE_REG(hw, E1000_FCRTL, fcrtl);
+	E1000_WRITE_REG(hw, E1000_FCRTH, fcrth);
 
-	if ((nvm_data & NVM_WORD0F_PAUSE_MASK) == 0)
-		hw->fc.requested_mode = e1000_fc_none;
-	else if ((nvm_data & NVM_WORD0F_PAUSE_MASK) ==
-		 NVM_WORD0F_ASM_DIR)
-		hw->fc.requested_mode = e1000_fc_tx_pause;
-	else
-		hw->fc.requested_mode = e1000_fc_full;
-
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_force_mac_fc - Force the MAC's flow control settings
+ *  e1000_force_mac_fc_generic - Force the MAC's flow control settings
  *  @hw: pointer to the HW structure
  *
  *  Force the MAC's flow control settings.  Sets the TFCE and RFCE bits in the
@@ -708,15 +1148,15 @@ out:
  *  autonegotiation is managed by the PHY rather than the MAC.  Software must
  *  also configure these bits when link is forced on a fiber connection.
  **/
-s32 igb_force_mac_fc(struct e1000_hw *hw)
+s32 e1000_force_mac_fc_generic(struct e1000_hw *hw)
 {
 	u32 ctrl;
-	s32 ret_val = 0;
 
-	ctrl = rd32(E1000_CTRL);
+	DEBUGFUNC("e1000_force_mac_fc_generic");
+
+	ctrl = E1000_READ_REG(hw, E1000_CTRL);
 
-	/*
-	 * Because we didn't get link via the internal auto-negotiation
+	/* Because we didn't get link via the internal auto-negotiation
 	 * mechanism (we either forced link or we got link via PHY
 	 * auto-neg), we have to manually enable/disable transmit an
 	 * receive flow control.
@@ -730,10 +1170,10 @@ s32 igb_force_mac_fc(struct e1000_hw *hw)
 	 *          frames but not send pause frames).
 	 *      2:  Tx flow control is enabled (we can send pause frames
 	 *          frames but we do not receive pause frames).
-	 *      3:  Both Rx and TX flow control (symmetric) is enabled.
+	 *      3:  Both Rx and Tx flow control (symmetric) is enabled.
 	 *  other:  No other values should be possible at this point.
 	 */
-	hw_dbg("hw->fc.current_mode = %u\n", hw->fc.current_mode);
+	DEBUGOUT1("hw->fc.current_mode = %u\n", hw->fc.current_mode);
 
 	switch (hw->fc.current_mode) {
 	case e1000_fc_none:
@@ -751,19 +1191,17 @@ s32 igb_force_mac_fc(struct e1000_hw *hw)
 		ctrl |= (E1000_CTRL_TFCE | E1000_CTRL_RFCE);
 		break;
 	default:
-		hw_dbg("Flow control param set incorrectly\n");
-		ret_val = -E1000_ERR_CONFIG;
-		goto out;
+		DEBUGOUT("Flow control param set incorrectly\n");
+		return -E1000_ERR_CONFIG;
 	}
 
-	wr32(E1000_CTRL, ctrl);
+	E1000_WRITE_REG(hw, E1000_CTRL, ctrl);
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_config_fc_after_link_up - Configures flow control after link
+ *  e1000_config_fc_after_link_up_generic - Configures flow control after link
  *  @hw: pointer to the HW structure
  *
  *  Checks the status of auto-negotiation after link up to ensure that the
@@ -772,76 +1210,72 @@ out:
  *  and did not fail, then we configure flow control based on our link
  *  partner.
  **/
-s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
+s32 e1000_config_fc_after_link_up_generic(struct e1000_hw *hw)
 {
 	struct e1000_mac_info *mac = &hw->mac;
-	s32 ret_val = 0;
+	s32 ret_val = E1000_SUCCESS;
+	u32 pcs_status_reg, pcs_adv_reg, pcs_lp_ability_reg, pcs_ctrl_reg;
 	u16 mii_status_reg, mii_nway_adv_reg, mii_nway_lp_ability_reg;
 	u16 speed, duplex;
 
-	/*
-	 * Check for the case where we have fiber media and auto-neg failed
+	DEBUGFUNC("e1000_config_fc_after_link_up_generic");
+
+	/* Check for the case where we have fiber media and auto-neg failed
 	 * so we had to force link.  In this case, we need to force the
 	 * configuration of the MAC to match the "fc" parameter.
 	 */
 	if (mac->autoneg_failed) {
-		if (hw->phy.media_type == e1000_media_type_internal_serdes)
-			ret_val = igb_force_mac_fc(hw);
+		if (hw->phy.media_type == e1000_media_type_fiber ||
+		    hw->phy.media_type == e1000_media_type_internal_serdes)
+			ret_val = e1000_force_mac_fc_generic(hw);
 	} else {
 		if (hw->phy.media_type == e1000_media_type_copper)
-			ret_val = igb_force_mac_fc(hw);
+			ret_val = e1000_force_mac_fc_generic(hw);
 	}
 
 	if (ret_val) {
-		hw_dbg("Error forcing flow control settings\n");
-		goto out;
+		DEBUGOUT("Error forcing flow control settings\n");
+		return ret_val;
 	}
 
-	/*
-	 * Check for the case where we have copper media and auto-neg is
+	/* Check for the case where we have copper media and auto-neg is
 	 * enabled.  In this case, we need to check and see if Auto-Neg
 	 * has completed, and if so, how the PHY and link partner has
 	 * flow control configured.
 	 */
 	if ((hw->phy.media_type == e1000_media_type_copper) && mac->autoneg) {
-		/*
-		 * Read the MII Status Register and check to see if AutoNeg
+		/* Read the MII Status Register and check to see if AutoNeg
 		 * has completed.  We read this twice because this reg has
 		 * some "sticky" (latched) bits.
 		 */
-		ret_val = hw->phy.ops.read_reg(hw, PHY_STATUS,
-						   &mii_status_reg);
+		ret_val = hw->phy.ops.read_reg(hw, PHY_STATUS, &mii_status_reg);
 		if (ret_val)
-			goto out;
-		ret_val = hw->phy.ops.read_reg(hw, PHY_STATUS,
-						   &mii_status_reg);
+			return ret_val;
+		ret_val = hw->phy.ops.read_reg(hw, PHY_STATUS, &mii_status_reg);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		if (!(mii_status_reg & MII_SR_AUTONEG_COMPLETE)) {
-			hw_dbg("Copper PHY and Auto Neg "
-				 "has not completed.\n");
-			goto out;
+			DEBUGOUT("Copper PHY and Auto Neg has not completed.\n");
+			return ret_val;
 		}
 
-		/*
-		 * The AutoNeg process has completed, so we now need to
+		/* The AutoNeg process has completed, so we now need to
 		 * read both the Auto Negotiation Advertisement
 		 * Register (Address 4) and the Auto_Negotiation Base
 		 * Page Ability Register (Address 5) to determine how
 		 * flow control was negotiated.
 		 */
 		ret_val = hw->phy.ops.read_reg(hw, PHY_AUTONEG_ADV,
-					    &mii_nway_adv_reg);
+					       &mii_nway_adv_reg);
 		if (ret_val)
-			goto out;
+			return ret_val;
 		ret_val = hw->phy.ops.read_reg(hw, PHY_LP_ABILITY,
-					    &mii_nway_lp_ability_reg);
+					       &mii_nway_lp_ability_reg);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
-		/*
-		 * Two bits in the Auto Negotiation Advertisement Register
+		/* Two bits in the Auto Negotiation Advertisement Register
 		 * (Address 4) and two bits in the Auto Negotiation Base
 		 * Page Ability Register (Address 5) determine flow control
 		 * for both the PHY and the link partner.  The following
@@ -876,24 +1310,21 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 		 */
 		if ((mii_nway_adv_reg & NWAY_AR_PAUSE) &&
 		    (mii_nway_lp_ability_reg & NWAY_LPAR_PAUSE)) {
-			/*
-			 * Now we need to check if the user selected RX ONLY
+			/* Now we need to check if the user selected Rx ONLY
 			 * of pause frames.  In this case, we had to advertise
-			 * FULL flow control because we could not advertise RX
+			 * FULL flow control because we could not advertise Rx
 			 * ONLY. Hence, we must now check to see if we need to
-			 * turn OFF  the TRANSMISSION of PAUSE frames.
+			 * turn OFF the TRANSMISSION of PAUSE frames.
 			 */
 			if (hw->fc.requested_mode == e1000_fc_full) {
 				hw->fc.current_mode = e1000_fc_full;
-				hw_dbg("Flow Control = FULL.\r\n");
+				DEBUGOUT("Flow Control = FULL.\n");
 			} else {
 				hw->fc.current_mode = e1000_fc_rx_pause;
-				hw_dbg("Flow Control = "
-				       "RX PAUSE frames only.\r\n");
+				DEBUGOUT("Flow Control = Rx PAUSE frames only.\n");
 			}
 		}
-		/*
-		 * For receiving PAUSE frames ONLY.
+		/* For receiving PAUSE frames ONLY.
 		 *
 		 *   LOCAL DEVICE  |   LINK PARTNER
 		 * PAUSE | ASM_DIR | PAUSE | ASM_DIR | Result
@@ -905,10 +1336,9 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 			  (mii_nway_lp_ability_reg & NWAY_LPAR_PAUSE) &&
 			  (mii_nway_lp_ability_reg & NWAY_LPAR_ASM_DIR)) {
 			hw->fc.current_mode = e1000_fc_tx_pause;
-			hw_dbg("Flow Control = TX PAUSE frames only.\r\n");
+			DEBUGOUT("Flow Control = Tx PAUSE frames only.\n");
 		}
-		/*
-		 * For transmitting PAUSE frames ONLY.
+		/* For transmitting PAUSE frames ONLY.
 		 *
 		 *   LOCAL DEVICE  |   LINK PARTNER
 		 * PAUSE | ASM_DIR | PAUSE | ASM_DIR | Result
@@ -920,70 +1350,167 @@ s32 igb_config_fc_after_link_up(struct e1000_hw *hw)
 			 !(mii_nway_lp_ability_reg & NWAY_LPAR_PAUSE) &&
 			 (mii_nway_lp_ability_reg & NWAY_LPAR_ASM_DIR)) {
 			hw->fc.current_mode = e1000_fc_rx_pause;
-			hw_dbg("Flow Control = RX PAUSE frames only.\r\n");
-		}
-		/*
-		 * Per the IEEE spec, at this point flow control should be
-		 * disabled.  However, we want to consider that we could
-		 * be connected to a legacy switch that doesn't advertise
-		 * desired flow control, but can be forced on the link
-		 * partner.  So if we advertised no flow control, that is
-		 * what we will resolve to.  If we advertised some kind of
-		 * receive capability (Rx Pause Only or Full Flow Control)
-		 * and the link partner advertised none, we will configure
-		 * ourselves to enable Rx Flow Control only.  We can do
-		 * this safely for two reasons:  If the link partner really
-		 * didn't want flow control enabled, and we enable Rx, no
-		 * harm done since we won't be receiving any PAUSE frames
-		 * anyway.  If the intent on the link partner was to have
-		 * flow control enabled, then by us enabling RX only, we
-		 * can at least receive pause frames and process them.
-		 * This is a good idea because in most cases, since we are
-		 * predominantly a server NIC, more times than not we will
-		 * be asked to delay transmission of packets than asking
-		 * our link partner to pause transmission of frames.
-		 */
-		else if ((hw->fc.requested_mode == e1000_fc_none ||
-			  hw->fc.requested_mode == e1000_fc_tx_pause) ||
-			 hw->fc.strict_ieee) {
-			hw->fc.current_mode = e1000_fc_none;
-			hw_dbg("Flow Control = NONE.\r\n");
+			DEBUGOUT("Flow Control = Rx PAUSE frames only.\n");
 		} else {
-			hw->fc.current_mode = e1000_fc_rx_pause;
-			hw_dbg("Flow Control = RX PAUSE frames only.\r\n");
+			/* Per the IEEE spec, at this point flow control
+			 * should be disabled.
+			 */
+			hw->fc.current_mode = e1000_fc_none;
+			DEBUGOUT("Flow Control = NONE.\n");
 		}
 
-		/*
-		 * Now we need to do one last check...  If we auto-
+		/* Now we need to do one last check...  If we auto-
 		 * negotiated to HALF DUPLEX, flow control should not be
 		 * enabled per IEEE 802.3 spec.
 		 */
-		ret_val = hw->mac.ops.get_speed_and_duplex(hw, &speed, &duplex);
+		ret_val = mac->ops.get_link_up_info(hw, &speed, &duplex);
 		if (ret_val) {
-			hw_dbg("Error getting link speed and duplex\n");
-			goto out;
+			DEBUGOUT("Error getting link speed and duplex\n");
+			return ret_val;
 		}
 
 		if (duplex == HALF_DUPLEX)
 			hw->fc.current_mode = e1000_fc_none;
 
-		/*
-		 * Now we call a subroutine to actually force the MAC
+		/* Now we call a subroutine to actually force the MAC
 		 * controller to use the correct flow control settings.
 		 */
-		ret_val = igb_force_mac_fc(hw);
+		ret_val = e1000_force_mac_fc_generic(hw);
 		if (ret_val) {
-			hw_dbg("Error forcing flow control settings\n");
-			goto out;
+			DEBUGOUT("Error forcing flow control settings\n");
+			return ret_val;
 		}
 	}
 
-out:
-	return ret_val;
+	/* Check for the case where we have SerDes media and auto-neg is
+	 * enabled.  In this case, we need to check and see if Auto-Neg
+	 * has completed, and if so, how the PHY and link partner has
+	 * flow control configured.
+	 */
+	if ((hw->phy.media_type == e1000_media_type_internal_serdes) &&
+	    mac->autoneg) {
+		/* Read the PCS_LSTS and check to see if AutoNeg
+		 * has completed.
+		 */
+		pcs_status_reg = E1000_READ_REG(hw, E1000_PCS_LSTAT);
+
+		if (!(pcs_status_reg & E1000_PCS_LSTS_AN_COMPLETE)) {
+			DEBUGOUT("PCS Auto Neg has not completed.\n");
+			return ret_val;
+		}
+
+		/* The AutoNeg process has completed, so we now need to
+		 * read both the Auto Negotiation Advertisement
+		 * Register (PCS_ANADV) and the Auto_Negotiation Base
+		 * Page Ability Register (PCS_LPAB) to determine how
+		 * flow control was negotiated.
+		 */
+		pcs_adv_reg = E1000_READ_REG(hw, E1000_PCS_ANADV);
+		pcs_lp_ability_reg = E1000_READ_REG(hw, E1000_PCS_LPAB);
+
+		/* Two bits in the Auto Negotiation Advertisement Register
+		 * (PCS_ANADV) and two bits in the Auto Negotiation Base
+		 * Page Ability Register (PCS_LPAB) determine flow control
+		 * for both the PHY and the link partner.  The following
+		 * table, taken out of the IEEE 802.3ab/D6.0 dated March 25,
+		 * 1999, describes these PAUSE resolution bits and how flow
+		 * control is determined based upon these settings.
+		 * NOTE:  DC = Don't Care
+		 *
+		 *   LOCAL DEVICE  |   LINK PARTNER
+		 * PAUSE | ASM_DIR | PAUSE | ASM_DIR | NIC Resolution
+		 *-------|---------|-------|---------|--------------------
+		 *   0   |    0    |  DC   |   DC    | e1000_fc_none
+		 *   0   |    1    |   0   |   DC    | e1000_fc_none
+		 *   0   |    1    |   1   |    0    | e1000_fc_none
+		 *   0   |    1    |   1   |    1    | e1000_fc_tx_pause
+		 *   1   |    0    |   0   |   DC    | e1000_fc_none
+		 *   1   |   DC    |   1   |   DC    | e1000_fc_full
+		 *   1   |    1    |   0   |    0    | e1000_fc_none
+		 *   1   |    1    |   0   |    1    | e1000_fc_rx_pause
+		 *
+		 * Are both PAUSE bits set to 1?  If so, this implies
+		 * Symmetric Flow Control is enabled at both ends.  The
+		 * ASM_DIR bits are irrelevant per the spec.
+		 *
+		 * For Symmetric Flow Control:
+		 *
+		 *   LOCAL DEVICE  |   LINK PARTNER
+		 * PAUSE | ASM_DIR | PAUSE | ASM_DIR | Result
+		 *-------|---------|-------|---------|--------------------
+		 *   1   |   DC    |   1   |   DC    | e1000_fc_full
+		 *
+		 */
+		if ((pcs_adv_reg & E1000_TXCW_PAUSE) &&
+		    (pcs_lp_ability_reg & E1000_TXCW_PAUSE)) {
+			/* Now we need to check if the user selected Rx ONLY
+			 * of pause frames.  In this case, we had to advertise
+			 * FULL flow control because we could not advertise Rx
+			 * ONLY. Hence, we must now check to see if we need to
+			 * turn OFF the TRANSMISSION of PAUSE frames.
+			 */
+			if (hw->fc.requested_mode == e1000_fc_full) {
+				hw->fc.current_mode = e1000_fc_full;
+				DEBUGOUT("Flow Control = FULL.\n");
+			} else {
+				hw->fc.current_mode = e1000_fc_rx_pause;
+				DEBUGOUT("Flow Control = Rx PAUSE frames only.\n");
+			}
+		}
+		/* For receiving PAUSE frames ONLY.
+		 *
+		 *   LOCAL DEVICE  |   LINK PARTNER
+		 * PAUSE | ASM_DIR | PAUSE | ASM_DIR | Result
+		 *-------|---------|-------|---------|--------------------
+		 *   0   |    1    |   1   |    1    | e1000_fc_tx_pause
+		 */
+		else if (!(pcs_adv_reg & E1000_TXCW_PAUSE) &&
+			  (pcs_adv_reg & E1000_TXCW_ASM_DIR) &&
+			  (pcs_lp_ability_reg & E1000_TXCW_PAUSE) &&
+			  (pcs_lp_ability_reg & E1000_TXCW_ASM_DIR)) {
+			hw->fc.current_mode = e1000_fc_tx_pause;
+			DEBUGOUT("Flow Control = Tx PAUSE frames only.\n");
+		}
+		/* For transmitting PAUSE frames ONLY.
+		 *
+		 *   LOCAL DEVICE  |   LINK PARTNER
+		 * PAUSE | ASM_DIR | PAUSE | ASM_DIR | Result
+		 *-------|---------|-------|---------|--------------------
+		 *   1   |    1    |   0   |    1    | e1000_fc_rx_pause
+		 */
+		else if ((pcs_adv_reg & E1000_TXCW_PAUSE) &&
+			 (pcs_adv_reg & E1000_TXCW_ASM_DIR) &&
+			 !(pcs_lp_ability_reg & E1000_TXCW_PAUSE) &&
+			 (pcs_lp_ability_reg & E1000_TXCW_ASM_DIR)) {
+			hw->fc.current_mode = e1000_fc_rx_pause;
+			DEBUGOUT("Flow Control = Rx PAUSE frames only.\n");
+		} else {
+			/* Per the IEEE spec, at this point flow control
+			 * should be disabled.
+			 */
+			hw->fc.current_mode = e1000_fc_none;
+			DEBUGOUT("Flow Control = NONE.\n");
+		}
+
+		/* Now we call a subroutine to actually force the MAC
+		 * controller to use the correct flow control settings.
+		 */
+		pcs_ctrl_reg = E1000_READ_REG(hw, E1000_PCS_LCTL);
+		pcs_ctrl_reg |= E1000_PCS_LCTL_FORCE_FCTRL;
+		E1000_WRITE_REG(hw, E1000_PCS_LCTL, pcs_ctrl_reg);
+
+		ret_val = e1000_force_mac_fc_generic(hw);
+		if (ret_val) {
+			DEBUGOUT("Error forcing flow control settings\n");
+			return ret_val;
+		}
+	}
+
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_get_speed_and_duplex_copper - Retrieve current speed/duplex
+ *  e1000_get_speed_and_duplex_copper_generic - Retrieve current speed/duplex
  *  @hw: pointer to the HW structure
  *  @speed: stores the current speed
  *  @duplex: stores the current duplex
@@ -991,172 +1518,185 @@ out:
  *  Read the status register for the current speed/duplex and store the current
  *  speed and duplex for copper connections.
  **/
-s32 igb_get_speed_and_duplex_copper(struct e1000_hw *hw, u16 *speed,
-				      u16 *duplex)
+s32 e1000_get_speed_and_duplex_copper_generic(struct e1000_hw *hw, u16 *speed,
+					      u16 *duplex)
 {
 	u32 status;
 
-	status = rd32(E1000_STATUS);
+	DEBUGFUNC("e1000_get_speed_and_duplex_copper_generic");
+
+	status = E1000_READ_REG(hw, E1000_STATUS);
 	if (status & E1000_STATUS_SPEED_1000) {
 		*speed = SPEED_1000;
-		hw_dbg("1000 Mbs, ");
+		DEBUGOUT("1000 Mbs, ");
 	} else if (status & E1000_STATUS_SPEED_100) {
 		*speed = SPEED_100;
-		hw_dbg("100 Mbs, ");
+		DEBUGOUT("100 Mbs, ");
 	} else {
 		*speed = SPEED_10;
-		hw_dbg("10 Mbs, ");
+		DEBUGOUT("10 Mbs, ");
 	}
 
 	if (status & E1000_STATUS_FD) {
 		*duplex = FULL_DUPLEX;
-		hw_dbg("Full Duplex\n");
+		DEBUGOUT("Full Duplex\n");
 	} else {
 		*duplex = HALF_DUPLEX;
-		hw_dbg("Half Duplex\n");
+		DEBUGOUT("Half Duplex\n");
 	}
 
-	return 0;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_get_hw_semaphore - Acquire hardware semaphore
+ *  e1000_get_speed_and_duplex_fiber_generic - Retrieve current speed/duplex
+ *  @hw: pointer to the HW structure
+ *  @speed: stores the current speed
+ *  @duplex: stores the current duplex
+ *
+ *  Sets the speed and duplex to gigabit full duplex (the only possible option)
+ *  for fiber/serdes links.
+ **/
+s32 e1000_get_speed_and_duplex_fiber_serdes_generic(struct e1000_hw E1000_UNUSEDARG *hw,
+						    u16 *speed, u16 *duplex)
+{
+	DEBUGFUNC("e1000_get_speed_and_duplex_fiber_serdes_generic");
+
+	*speed = SPEED_1000;
+	*duplex = FULL_DUPLEX;
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_get_hw_semaphore_generic - Acquire hardware semaphore
  *  @hw: pointer to the HW structure
  *
  *  Acquire the HW semaphore to access the PHY or NVM
  **/
-s32 igb_get_hw_semaphore(struct e1000_hw *hw)
+s32 e1000_get_hw_semaphore_generic(struct e1000_hw *hw)
 {
 	u32 swsm;
-	s32 ret_val = 0;
 	s32 timeout = hw->nvm.word_size + 1;
 	s32 i = 0;
 
+	DEBUGFUNC("e1000_get_hw_semaphore_generic");
+
 	/* Get the SW semaphore */
 	while (i < timeout) {
-		swsm = rd32(E1000_SWSM);
+		swsm = E1000_READ_REG(hw, E1000_SWSM);
 		if (!(swsm & E1000_SWSM_SMBI))
 			break;
 
-		udelay(50);
+		usec_delay(50);
 		i++;
 	}
 
 	if (i == timeout) {
-		hw_dbg("Driver can't access device - SMBI bit is set.\n");
-		ret_val = -E1000_ERR_NVM;
-		goto out;
+		DEBUGOUT("Driver can't access device - SMBI bit is set.\n");
+		return -E1000_ERR_NVM;
 	}
 
 	/* Get the FW semaphore. */
 	for (i = 0; i < timeout; i++) {
-		swsm = rd32(E1000_SWSM);
-		wr32(E1000_SWSM, swsm | E1000_SWSM_SWESMBI);
+		swsm = E1000_READ_REG(hw, E1000_SWSM);
+		E1000_WRITE_REG(hw, E1000_SWSM, swsm | E1000_SWSM_SWESMBI);
 
 		/* Semaphore acquired if bit latched */
-		if (rd32(E1000_SWSM) & E1000_SWSM_SWESMBI)
+		if (E1000_READ_REG(hw, E1000_SWSM) & E1000_SWSM_SWESMBI)
 			break;
 
-		udelay(50);
+		usec_delay(50);
 	}
 
 	if (i == timeout) {
 		/* Release semaphores */
-		igb_put_hw_semaphore(hw);
-		hw_dbg("Driver can't access the NVM\n");
-		ret_val = -E1000_ERR_NVM;
-		goto out;
+		e1000_put_hw_semaphore_generic(hw);
+		DEBUGOUT("Driver can't access the NVM\n");
+		return -E1000_ERR_NVM;
 	}
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_put_hw_semaphore - Release hardware semaphore
+ *  e1000_put_hw_semaphore_generic - Release hardware semaphore
  *  @hw: pointer to the HW structure
  *
  *  Release hardware semaphore used to access the PHY or NVM
  **/
-void igb_put_hw_semaphore(struct e1000_hw *hw)
+void e1000_put_hw_semaphore_generic(struct e1000_hw *hw)
 {
 	u32 swsm;
 
-	swsm = rd32(E1000_SWSM);
+	DEBUGFUNC("e1000_put_hw_semaphore_generic");
+
+	swsm = E1000_READ_REG(hw, E1000_SWSM);
 
 	swsm &= ~(E1000_SWSM_SMBI | E1000_SWSM_SWESMBI);
 
-	wr32(E1000_SWSM, swsm);
+	E1000_WRITE_REG(hw, E1000_SWSM, swsm);
 }
 
 /**
- *  igb_get_auto_rd_done - Check for auto read completion
+ *  e1000_get_auto_rd_done_generic - Check for auto read completion
  *  @hw: pointer to the HW structure
  *
  *  Check EEPROM for Auto Read done bit.
  **/
-s32 igb_get_auto_rd_done(struct e1000_hw *hw)
+s32 e1000_get_auto_rd_done_generic(struct e1000_hw *hw)
 {
 	s32 i = 0;
-	s32 ret_val = 0;
 
+	DEBUGFUNC("e1000_get_auto_rd_done_generic");
 
 	while (i < AUTO_READ_DONE_TIMEOUT) {
-		if (rd32(E1000_EECD) & E1000_EECD_AUTO_RD)
+		if (E1000_READ_REG(hw, E1000_EECD) & E1000_EECD_AUTO_RD)
 			break;
-		msleep(1);
+		msec_delay(1);
 		i++;
 	}
 
 	if (i == AUTO_READ_DONE_TIMEOUT) {
-		hw_dbg("Auto read by HW from NVM has not completed.\n");
-		ret_val = -E1000_ERR_RESET;
-		goto out;
+		DEBUGOUT("Auto read by HW from NVM has not completed.\n");
+		return -E1000_ERR_RESET;
 	}
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_valid_led_default - Verify a valid default LED config
+ *  e1000_valid_led_default_generic - Verify a valid default LED config
  *  @hw: pointer to the HW structure
  *  @data: pointer to the NVM (EEPROM)
  *
  *  Read the EEPROM for the current default LED configuration.  If the
  *  LED configuration is not valid, set to a valid LED configuration.
  **/
-static s32 igb_valid_led_default(struct e1000_hw *hw, u16 *data)
+s32 e1000_valid_led_default_generic(struct e1000_hw *hw, u16 *data)
 {
 	s32 ret_val;
 
+	DEBUGFUNC("e1000_valid_led_default_generic");
+
 	ret_val = hw->nvm.ops.read(hw, NVM_ID_LED_SETTINGS, 1, data);
 	if (ret_val) {
-		hw_dbg("NVM Read Error\n");
-		goto out;
+		DEBUGOUT("NVM Read Error\n");
+		return ret_val;
 	}
 
-	if (*data == ID_LED_RESERVED_0000 || *data == ID_LED_RESERVED_FFFF) {
-		switch(hw->phy.media_type) {
-		case e1000_media_type_internal_serdes:
-			*data = ID_LED_DEFAULT_82575_SERDES;
-			break;
-		case e1000_media_type_copper:
-		default:
-			*data = ID_LED_DEFAULT;
-			break;
-		}
-	}
-out:
-	return ret_val;
+	if (*data == ID_LED_RESERVED_0000 || *data == ID_LED_RESERVED_FFFF)
+		*data = ID_LED_DEFAULT;
+
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_id_led_init -
+ *  e1000_id_led_init_generic -
  *  @hw: pointer to the HW structure
  *
  **/
-s32 igb_id_led_init(struct e1000_hw *hw)
+s32 e1000_id_led_init_generic(struct e1000_hw *hw)
 {
 	struct e1000_mac_info *mac = &hw->mac;
 	s32 ret_val;
@@ -1166,11 +1706,13 @@ s32 igb_id_led_init(struct e1000_hw *hw)
 	u16 data, i, temp;
 	const u16 led_mask = 0x0F;
 
-	ret_val = igb_valid_led_default(hw, &data);
+	DEBUGFUNC("e1000_id_led_init_generic");
+
+	ret_val = hw->nvm.ops.valid_led_default(hw, &data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	mac->ledctl_default = rd32(E1000_LEDCTL);
+	mac->ledctl_default = E1000_READ_REG(hw, E1000_LEDCTL);
 	mac->ledctl_mode1 = mac->ledctl_default;
 	mac->ledctl_mode2 = mac->ledctl_default;
 
@@ -1212,135 +1754,324 @@ s32 igb_id_led_init(struct e1000_hw *hw)
 		}
 	}
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_setup_led_generic - Configures SW controllable LED
+ *  @hw: pointer to the HW structure
+ *
+ *  This prepares the SW controllable LED for use and saves the current state
+ *  of the LED so it can be later restored.
+ **/
+s32 e1000_setup_led_generic(struct e1000_hw *hw)
+{
+	u32 ledctl;
+
+	DEBUGFUNC("e1000_setup_led_generic");
+
+	if (hw->mac.ops.setup_led != e1000_setup_led_generic)
+		return -E1000_ERR_CONFIG;
+
+	if (hw->phy.media_type == e1000_media_type_fiber) {
+		ledctl = E1000_READ_REG(hw, E1000_LEDCTL);
+		hw->mac.ledctl_default = ledctl;
+		/* Turn off LED0 */
+		ledctl &= ~(E1000_LEDCTL_LED0_IVRT | E1000_LEDCTL_LED0_BLINK |
+			    E1000_LEDCTL_LED0_MODE_MASK);
+		ledctl |= (E1000_LEDCTL_MODE_LED_OFF <<
+			   E1000_LEDCTL_LED0_MODE_SHIFT);
+		E1000_WRITE_REG(hw, E1000_LEDCTL, ledctl);
+	} else if (hw->phy.media_type == e1000_media_type_copper) {
+		E1000_WRITE_REG(hw, E1000_LEDCTL, hw->mac.ledctl_mode1);
+	}
+
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_cleanup_led - Set LED config to default operation
+ *  e1000_cleanup_led_generic - Set LED config to default operation
  *  @hw: pointer to the HW structure
  *
  *  Remove the current LED configuration and set the LED configuration
  *  to the default value, saved from the EEPROM.
  **/
-s32 igb_cleanup_led(struct e1000_hw *hw)
+s32 e1000_cleanup_led_generic(struct e1000_hw *hw)
 {
-	wr32(E1000_LEDCTL, hw->mac.ledctl_default);
-	return 0;
+	DEBUGFUNC("e1000_cleanup_led_generic");
+
+	E1000_WRITE_REG(hw, E1000_LEDCTL, hw->mac.ledctl_default);
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_blink_led - Blink LED
+ *  e1000_blink_led_generic - Blink LED
  *  @hw: pointer to the HW structure
  *
- *  Blink the led's which are set to be on.
+ *  Blink the LEDs which are set to be on.
  **/
-s32 igb_blink_led(struct e1000_hw *hw)
+s32 e1000_blink_led_generic(struct e1000_hw *hw)
 {
 	u32 ledctl_blink = 0;
 	u32 i;
 
-	/*
-	 * set the blink bit for each LED that's "on" (0x0E)
-	 * in ledctl_mode2
-	 */
-	ledctl_blink = hw->mac.ledctl_mode2;
-	for (i = 0; i < 4; i++)
-		if (((hw->mac.ledctl_mode2 >> (i * 8)) & 0xFF) ==
-		    E1000_LEDCTL_MODE_LED_ON)
-			ledctl_blink |= (E1000_LEDCTL_LED0_BLINK <<
-					 (i * 8));
+	DEBUGFUNC("e1000_blink_led_generic");
 
-	wr32(E1000_LEDCTL, ledctl_blink);
+	if (hw->phy.media_type == e1000_media_type_fiber) {
+		/* always blink LED0 for PCI-E fiber */
+		ledctl_blink = E1000_LEDCTL_LED0_BLINK |
+		     (E1000_LEDCTL_MODE_LED_ON << E1000_LEDCTL_LED0_MODE_SHIFT);
+	} else {
+		/* Set the blink bit for each LED that's "on" (0x0E)
+		 * (or "off" if inverted) in ledctl_mode2.  The blink
+		 * logic in hardware only works when mode is set to "on"
+		 * so it must be changed accordingly when the mode is
+		 * "off" and inverted.
+		 */
+		ledctl_blink = hw->mac.ledctl_mode2;
+		for (i = 0; i < 32; i += 8) {
+			u32 mode = (hw->mac.ledctl_mode2 >> i) &
+			    E1000_LEDCTL_LED0_MODE_MASK;
+			u32 led_default = hw->mac.ledctl_default >> i;
+
+			if ((!(led_default & E1000_LEDCTL_LED0_IVRT) &&
+			     (mode == E1000_LEDCTL_MODE_LED_ON)) ||
+			    ((led_default & E1000_LEDCTL_LED0_IVRT) &&
+			     (mode == E1000_LEDCTL_MODE_LED_OFF))) {
+				ledctl_blink &=
+				    ~(E1000_LEDCTL_LED0_MODE_MASK << i);
+				ledctl_blink |= (E1000_LEDCTL_LED0_BLINK |
+						 E1000_LEDCTL_MODE_LED_ON) << i;
+			}
+		}
+	}
 
-	return 0;
+	E1000_WRITE_REG(hw, E1000_LEDCTL, ledctl_blink);
+
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_led_off - Turn LED off
+ *  e1000_led_on_generic - Turn LED on
+ *  @hw: pointer to the HW structure
+ *
+ *  Turn LED on.
+ **/
+s32 e1000_led_on_generic(struct e1000_hw *hw)
+{
+	u32 ctrl;
+
+	DEBUGFUNC("e1000_led_on_generic");
+
+	switch (hw->phy.media_type) {
+	case e1000_media_type_fiber:
+		ctrl = E1000_READ_REG(hw, E1000_CTRL);
+		ctrl &= ~E1000_CTRL_SWDPIN0;
+		ctrl |= E1000_CTRL_SWDPIO0;
+		E1000_WRITE_REG(hw, E1000_CTRL, ctrl);
+		break;
+	case e1000_media_type_copper:
+		E1000_WRITE_REG(hw, E1000_LEDCTL, hw->mac.ledctl_mode2);
+		break;
+	default:
+		break;
+	}
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_led_off_generic - Turn LED off
  *  @hw: pointer to the HW structure
  *
  *  Turn LED off.
  **/
-s32 igb_led_off(struct e1000_hw *hw)
+s32 e1000_led_off_generic(struct e1000_hw *hw)
 {
+	u32 ctrl;
+
+	DEBUGFUNC("e1000_led_off_generic");
+
 	switch (hw->phy.media_type) {
+	case e1000_media_type_fiber:
+		ctrl = E1000_READ_REG(hw, E1000_CTRL);
+		ctrl |= E1000_CTRL_SWDPIN0;
+		ctrl |= E1000_CTRL_SWDPIO0;
+		E1000_WRITE_REG(hw, E1000_CTRL, ctrl);
+		break;
 	case e1000_media_type_copper:
-		wr32(E1000_LEDCTL, hw->mac.ledctl_mode1);
+		E1000_WRITE_REG(hw, E1000_LEDCTL, hw->mac.ledctl_mode1);
 		break;
 	default:
 		break;
 	}
 
-	return 0;
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_set_pcie_no_snoop_generic - Set PCI-express capabilities
+ *  @hw: pointer to the HW structure
+ *  @no_snoop: bitmap of snoop events
+ *
+ *  Set the PCI-express register to snoop for events enabled in 'no_snoop'.
+ **/
+void e1000_set_pcie_no_snoop_generic(struct e1000_hw *hw, u32 no_snoop)
+{
+	u32 gcr;
+
+	DEBUGFUNC("e1000_set_pcie_no_snoop_generic");
+
+	if (no_snoop) {
+		gcr = E1000_READ_REG(hw, E1000_GCR);
+		gcr &= ~(PCIE_NO_SNOOP_ALL);
+		gcr |= no_snoop;
+		E1000_WRITE_REG(hw, E1000_GCR, gcr);
+	}
 }
 
 /**
- *  igb_disable_pcie_master - Disables PCI-express master access
+ *  e1000_disable_pcie_master_generic - Disables PCI-express master access
  *  @hw: pointer to the HW structure
  *
- *  Returns 0 (0) if successful, else returns -10
- *  (-E1000_ERR_MASTER_REQUESTS_PENDING) if master disable bit has not casued
+ *  Returns E1000_SUCCESS if successful, else returns -10
+ *  (-E1000_ERR_MASTER_REQUESTS_PENDING) if master disable bit has not caused
  *  the master requests to be disabled.
  *
  *  Disables PCI-Express master access and verifies there are no pending
  *  requests.
  **/
-s32 igb_disable_pcie_master(struct e1000_hw *hw)
+s32 e1000_disable_pcie_master_generic(struct e1000_hw *hw)
 {
 	u32 ctrl;
 	s32 timeout = MASTER_DISABLE_TIMEOUT;
-	s32 ret_val = 0;
 
-	if (hw->bus.type != e1000_bus_type_pci_express)
-		goto out;
+	DEBUGFUNC("e1000_disable_pcie_master_generic");
 
-	ctrl = rd32(E1000_CTRL);
+	ctrl = E1000_READ_REG(hw, E1000_CTRL);
 	ctrl |= E1000_CTRL_GIO_MASTER_DISABLE;
-	wr32(E1000_CTRL, ctrl);
+	E1000_WRITE_REG(hw, E1000_CTRL, ctrl);
 
 	while (timeout) {
-		if (!(rd32(E1000_STATUS) &
+		if (!(E1000_READ_REG(hw, E1000_STATUS) &
 		      E1000_STATUS_GIO_MASTER_ENABLE))
 			break;
-		udelay(100);
+		usec_delay(100);
 		timeout--;
 	}
 
 	if (!timeout) {
-		hw_dbg("Master requests are pending.\n");
-		ret_val = -E1000_ERR_MASTER_REQUESTS_PENDING;
-		goto out;
+		DEBUGOUT("Master requests are pending.\n");
+		return -E1000_ERR_MASTER_REQUESTS_PENDING;
 	}
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_reset_adaptive_generic - Reset Adaptive Interframe Spacing
+ *  @hw: pointer to the HW structure
+ *
+ *  Reset the Adaptive Interframe Spacing throttle to default values.
+ **/
+void e1000_reset_adaptive_generic(struct e1000_hw *hw)
+{
+	struct e1000_mac_info *mac = &hw->mac;
+
+	DEBUGFUNC("e1000_reset_adaptive_generic");
+
+	if (!mac->adaptive_ifs) {
+		DEBUGOUT("Not in Adaptive IFS mode!\n");
+		return;
+	}
+
+	mac->current_ifs_val = 0;
+	mac->ifs_min_val = IFS_MIN;
+	mac->ifs_max_val = IFS_MAX;
+	mac->ifs_step_size = IFS_STEP;
+	mac->ifs_ratio = IFS_RATIO;
+
+	mac->in_ifs_mode = false;
+	E1000_WRITE_REG(hw, E1000_AIT, 0);
 }
 
 /**
- *  igb_validate_mdi_setting - Verify MDI/MDIx settings
+ *  e1000_update_adaptive_generic - Update Adaptive Interframe Spacing
  *  @hw: pointer to the HW structure
  *
- *  Verify that when not using auto-negotitation that MDI/MDIx is correctly
+ *  Update the Adaptive Interframe Spacing Throttle value based on the
+ *  time between transmitted packets and time between collisions.
+ **/
+void e1000_update_adaptive_generic(struct e1000_hw *hw)
+{
+	struct e1000_mac_info *mac = &hw->mac;
+
+	DEBUGFUNC("e1000_update_adaptive_generic");
+
+	if (!mac->adaptive_ifs) {
+		DEBUGOUT("Not in Adaptive IFS mode!\n");
+		return;
+	}
+
+	if ((mac->collision_delta * mac->ifs_ratio) > mac->tx_packet_delta) {
+		if (mac->tx_packet_delta > MIN_NUM_XMITS) {
+			mac->in_ifs_mode = true;
+			if (mac->current_ifs_val < mac->ifs_max_val) {
+				if (!mac->current_ifs_val)
+					mac->current_ifs_val = mac->ifs_min_val;
+				else
+					mac->current_ifs_val +=
+						mac->ifs_step_size;
+				E1000_WRITE_REG(hw, E1000_AIT,
+						mac->current_ifs_val);
+			}
+		}
+	} else {
+		if (mac->in_ifs_mode &&
+		    (mac->tx_packet_delta <= MIN_NUM_XMITS)) {
+			mac->current_ifs_val = 0;
+			mac->in_ifs_mode = false;
+			E1000_WRITE_REG(hw, E1000_AIT, 0);
+		}
+	}
+}
+
+/**
+ *  e1000_validate_mdi_setting_generic - Verify MDI/MDIx settings
+ *  @hw: pointer to the HW structure
+ *
+ *  Verify that when not using auto-negotiation that MDI/MDIx is correctly
  *  set, which is forced to MDI mode only.
  **/
-s32 igb_validate_mdi_setting(struct e1000_hw *hw)
+static s32 e1000_validate_mdi_setting_generic(struct e1000_hw *hw)
 {
-	s32 ret_val = 0;
+	DEBUGFUNC("e1000_validate_mdi_setting_generic");
 
 	if (!hw->mac.autoneg && (hw->phy.mdix == 0 || hw->phy.mdix == 3)) {
-		hw_dbg("Invalid MDI setting detected\n");
+		DEBUGOUT("Invalid MDI setting detected\n");
 		hw->phy.mdix = 1;
-		ret_val = -E1000_ERR_CONFIG;
-		goto out;
+		return -E1000_ERR_CONFIG;
 	}
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_write_8bit_ctrl_reg - Write a 8bit CTRL register
+ *  e1000_validate_mdi_setting_crossover_generic - Verify MDI/MDIx settings
+ *  @hw: pointer to the HW structure
+ *
+ *  Validate the MDI/MDIx setting, allowing for auto-crossover during forced
+ *  operation.
+ **/
+s32 e1000_validate_mdi_setting_crossover_generic(struct e1000_hw E1000_UNUSEDARG *hw)
+{
+	DEBUGFUNC("e1000_validate_mdi_setting_crossover_generic");
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_write_8bit_ctrl_reg_generic - Write a 8bit CTRL register
  *  @hw: pointer to the HW structure
  *  @reg: 32bit register offset such as E1000_SCTL
  *  @offset: register offset to write to
@@ -1350,72 +2081,28 @@ out:
  *  and they all have the format address << 8 | data and bit 31 is polled for
  *  completion.
  **/
-s32 igb_write_8bit_ctrl_reg(struct e1000_hw *hw, u32 reg,
-			      u32 offset, u8 data)
+s32 e1000_write_8bit_ctrl_reg_generic(struct e1000_hw *hw, u32 reg,
+				      u32 offset, u8 data)
 {
 	u32 i, regvalue = 0;
-	s32 ret_val = 0;
+
+	DEBUGFUNC("e1000_write_8bit_ctrl_reg_generic");
 
 	/* Set up the address and data */
 	regvalue = ((u32)data) | (offset << E1000_GEN_CTL_ADDRESS_SHIFT);
-	wr32(reg, regvalue);
+	E1000_WRITE_REG(hw, reg, regvalue);
 
 	/* Poll the ready bit to see if the MDI read completed */
 	for (i = 0; i < E1000_GEN_POLL_TIMEOUT; i++) {
-		udelay(5);
-		regvalue = rd32(reg);
+		usec_delay(5);
+		regvalue = E1000_READ_REG(hw, reg);
 		if (regvalue & E1000_GEN_CTL_READY)
 			break;
 	}
 	if (!(regvalue & E1000_GEN_CTL_READY)) {
-		hw_dbg("Reg %08x did not indicate ready\n", reg);
-		ret_val = -E1000_ERR_PHY;
-		goto out;
+		DEBUGOUT1("Reg %08x did not indicate ready\n", reg);
+		return -E1000_ERR_PHY;
 	}
 
-out:
-	return ret_val;
-}
-
-/**
- *  igb_enable_mng_pass_thru - Enable processing of ARP's
- *  @hw: pointer to the HW structure
- *
- *  Verifies the hardware needs to leave interface enabled so that frames can
- *  be directed to and from the management interface.
- **/
-bool igb_enable_mng_pass_thru(struct e1000_hw *hw)
-{
-	u32 manc;
-	u32 fwsm, factps;
-	bool ret_val = false;
-
-	if (!hw->mac.asf_firmware_present)
-		goto out;
-
-	manc = rd32(E1000_MANC);
-
-	if (!(manc & E1000_MANC_RCV_TCO_EN))
-		goto out;
-
-	if (hw->mac.arc_subsystem_valid) {
-		fwsm = rd32(E1000_FWSM);
-		factps = rd32(E1000_FACTPS);
-
-		if (!(factps & E1000_FACTPS_MNGCG) &&
-		    ((fwsm & E1000_FWSM_MODE_MASK) ==
-		     (e1000_mng_mode_pt << E1000_FWSM_MODE_SHIFT))) {
-			ret_val = true;
-			goto out;
-		}
-	} else {
-		if ((manc & E1000_MANC_SMBUS_EN) &&
-		    !(manc & E1000_MANC_ASF_EN)) {
-			ret_val = true;
-			goto out;
-		}
-	}
-
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
diff --git a/drivers/net/igb/e1000_mac.h b/drivers/net/igb/e1000_mac.h
index 4927f61fbbc8..6a1b0f52f4a7 100644
--- a/drivers/net/igb/e1000_mac.h
+++ b/drivers/net/igb/e1000_mac.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2007-2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -28,63 +28,53 @@
 #ifndef _E1000_MAC_H_
 #define _E1000_MAC_H_
 
-#include "e1000_hw.h"
-
-#include "e1000_phy.h"
-#include "e1000_nvm.h"
-#include "e1000_defines.h"
-
-/*
- * Functions that should not be called directly from drivers but can be used
- * by other files in this 'shared code'
- */
-s32  igb_blink_led(struct e1000_hw *hw);
-s32  igb_check_for_copper_link(struct e1000_hw *hw);
-s32  igb_cleanup_led(struct e1000_hw *hw);
-s32  igb_config_fc_after_link_up(struct e1000_hw *hw);
-s32  igb_disable_pcie_master(struct e1000_hw *hw);
-s32  igb_force_mac_fc(struct e1000_hw *hw);
-s32  igb_get_auto_rd_done(struct e1000_hw *hw);
-s32  igb_get_bus_info_pcie(struct e1000_hw *hw);
-s32  igb_get_hw_semaphore(struct e1000_hw *hw);
-s32  igb_get_speed_and_duplex_copper(struct e1000_hw *hw, u16 *speed,
-				       u16 *duplex);
-s32  igb_id_led_init(struct e1000_hw *hw);
-s32  igb_led_off(struct e1000_hw *hw);
-void igb_update_mc_addr_list(struct e1000_hw *hw,
-	                     u8 *mc_addr_list, u32 mc_addr_count);
-s32  igb_setup_link(struct e1000_hw *hw);
-s32  igb_validate_mdi_setting(struct e1000_hw *hw);
-s32  igb_write_8bit_ctrl_reg(struct e1000_hw *hw, u32 reg,
-			       u32 offset, u8 data);
-
-void igb_clear_hw_cntrs_base(struct e1000_hw *hw);
-void igb_clear_vfta(struct e1000_hw *hw);
-s32  igb_vfta_set(struct e1000_hw *hw, u32 vid, bool add);
-void igb_config_collision_dist(struct e1000_hw *hw);
-void igb_init_rx_addrs(struct e1000_hw *hw, u16 rar_count);
-void igb_mta_set(struct e1000_hw *hw, u32 hash_value);
-void igb_put_hw_semaphore(struct e1000_hw *hw);
-void igb_rar_set(struct e1000_hw *hw, u8 *addr, u32 index);
-s32  igb_check_alt_mac_addr(struct e1000_hw *hw);
-
-bool igb_enable_mng_pass_thru(struct e1000_hw *hw);
-
-enum e1000_mng_mode {
-	e1000_mng_mode_none = 0,
-	e1000_mng_mode_asf,
-	e1000_mng_mode_pt,
-	e1000_mng_mode_ipmi,
-	e1000_mng_mode_host_if_only
-};
-
-#define E1000_FACTPS_MNGCG    0x20000000
-
-#define E1000_FWSM_MODE_MASK  0xE
-#define E1000_FWSM_MODE_SHIFT 1
-
-#define E1000_MNG_DHCP_COOKIE_STATUS_VLAN    0x2
-
-extern void e1000_init_function_pointers_82575(struct e1000_hw *hw);
+void e1000_init_mac_ops_generic(struct e1000_hw *hw);
+void e1000_null_mac_generic(struct e1000_hw *hw);
+s32  e1000_null_ops_generic(struct e1000_hw *hw);
+s32  e1000_null_link_info(struct e1000_hw *hw, u16 *s, u16 *d);
+bool e1000_null_mng_mode(struct e1000_hw *hw);
+void e1000_null_update_mc(struct e1000_hw *hw, u8 *h, u32 a);
+void e1000_null_write_vfta(struct e1000_hw *hw, u32 a, u32 b);
+void e1000_null_rar_set(struct e1000_hw *hw, u8 *h, u32 a);
+s32  e1000_blink_led_generic(struct e1000_hw *hw);
+s32  e1000_check_for_copper_link_generic(struct e1000_hw *hw);
+s32  e1000_check_for_fiber_link_generic(struct e1000_hw *hw);
+s32  e1000_check_for_serdes_link_generic(struct e1000_hw *hw);
+s32  e1000_cleanup_led_generic(struct e1000_hw *hw);
+s32  e1000_config_fc_after_link_up_generic(struct e1000_hw *hw);
+s32  e1000_disable_pcie_master_generic(struct e1000_hw *hw);
+s32  e1000_force_mac_fc_generic(struct e1000_hw *hw);
+s32  e1000_get_auto_rd_done_generic(struct e1000_hw *hw);
+s32  e1000_get_bus_info_pcie_generic(struct e1000_hw *hw);
+void e1000_set_lan_id_single_port(struct e1000_hw *hw);
+s32  e1000_get_hw_semaphore_generic(struct e1000_hw *hw);
+s32  e1000_get_speed_and_duplex_copper_generic(struct e1000_hw *hw, u16 *speed,
+					       u16 *duplex);
+s32  e1000_get_speed_and_duplex_fiber_serdes_generic(struct e1000_hw *hw,
+						     u16 *speed, u16 *duplex);
+s32  e1000_id_led_init_generic(struct e1000_hw *hw);
+s32  e1000_led_on_generic(struct e1000_hw *hw);
+s32  e1000_led_off_generic(struct e1000_hw *hw);
+void e1000_update_mc_addr_list_generic(struct e1000_hw *hw,
+				       u8 *mc_addr_list, u32 mc_addr_count);
+s32  e1000_set_fc_watermarks_generic(struct e1000_hw *hw);
+s32  e1000_setup_fiber_serdes_link_generic(struct e1000_hw *hw);
+s32  e1000_setup_led_generic(struct e1000_hw *hw);
+s32  e1000_setup_link_generic(struct e1000_hw *hw);
+s32  e1000_validate_mdi_setting_crossover_generic(struct e1000_hw *hw);
+s32  e1000_write_8bit_ctrl_reg_generic(struct e1000_hw *hw, u32 reg,
+				       u32 offset, u8 data);
+
+u32  e1000_hash_mc_addr_generic(struct e1000_hw *hw, u8 *mc_addr);
+
+void e1000_clear_hw_cntrs_base_generic(struct e1000_hw *hw);
+void e1000_clear_vfta_generic(struct e1000_hw *hw);
+void e1000_init_rx_addrs_generic(struct e1000_hw *hw, u16 rar_count);
+void e1000_put_hw_semaphore_generic(struct e1000_hw *hw);
+s32  e1000_check_alt_mac_addr_generic(struct e1000_hw *hw);
+void e1000_reset_adaptive_generic(struct e1000_hw *hw);
+void e1000_set_pcie_no_snoop_generic(struct e1000_hw *hw, u32 no_snoop);
+void e1000_update_adaptive_generic(struct e1000_hw *hw);
+void e1000_write_vfta_generic(struct e1000_hw *hw, u32 offset, u32 value);
 
 #endif
diff --git a/drivers/net/igb/e1000_manage.c b/drivers/net/igb/e1000_manage.c
new file mode 100644
index 000000000000..e1a2abe08dd2
--- /dev/null
+++ b/drivers/net/igb/e1000_manage.c
@@ -0,0 +1,556 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+#include "e1000_api.h"
+
+/**
+ *  e1000_calculate_checksum - Calculate checksum for buffer
+ *  @buffer: pointer to EEPROM
+ *  @length: size of EEPROM to calculate a checksum for
+ *
+ *  Calculates the checksum for some buffer on a specified length.  The
+ *  checksum calculated is returned.
+ **/
+u8 e1000_calculate_checksum(u8 *buffer, u32 length)
+{
+	u32 i;
+	u8 sum = 0;
+
+	DEBUGFUNC("e1000_calculate_checksum");
+
+	if (!buffer)
+		return 0;
+
+	for (i = 0; i < length; i++)
+		sum += buffer[i];
+
+	return (u8) (0 - sum);
+}
+
+/**
+ *  e1000_mng_enable_host_if_generic - Checks host interface is enabled
+ *  @hw: pointer to the HW structure
+ *
+ *  Returns E1000_success upon success, else E1000_ERR_HOST_INTERFACE_COMMAND
+ *
+ *  This function checks whether the HOST IF is enabled for command operation
+ *  and also checks whether the previous command is completed.  It busy waits
+ *  in case of previous command is not completed.
+ **/
+s32 e1000_mng_enable_host_if_generic(struct e1000_hw *hw)
+{
+	u32 hicr;
+	u8 i;
+
+	DEBUGFUNC("e1000_mng_enable_host_if_generic");
+
+	if (!hw->mac.arc_subsystem_valid) {
+		DEBUGOUT("ARC subsystem not valid.\n");
+		return -E1000_ERR_HOST_INTERFACE_COMMAND;
+	}
+
+	/* Check that the host interface is enabled. */
+	hicr = E1000_READ_REG(hw, E1000_HICR);
+	if (!(hicr & E1000_HICR_EN)) {
+		DEBUGOUT("E1000_HOST_EN bit disabled.\n");
+		return -E1000_ERR_HOST_INTERFACE_COMMAND;
+	}
+	/* check the previous command is completed */
+	for (i = 0; i < E1000_MNG_DHCP_COMMAND_TIMEOUT; i++) {
+		hicr = E1000_READ_REG(hw, E1000_HICR);
+		if (!(hicr & E1000_HICR_C))
+			break;
+		msec_delay_irq(1);
+	}
+
+	if (i == E1000_MNG_DHCP_COMMAND_TIMEOUT) {
+		DEBUGOUT("Previous command timeout failed .\n");
+		return -E1000_ERR_HOST_INTERFACE_COMMAND;
+	}
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_check_mng_mode_generic - Generic check management mode
+ *  @hw: pointer to the HW structure
+ *
+ *  Reads the firmware semaphore register and returns true (>0) if
+ *  manageability is enabled, else false (0).
+ **/
+bool e1000_check_mng_mode_generic(struct e1000_hw *hw)
+{
+	u32 fwsm = E1000_READ_REG(hw, E1000_FWSM);
+
+	DEBUGFUNC("e1000_check_mng_mode_generic");
+
+
+	return (fwsm & E1000_FWSM_MODE_MASK) ==
+		(E1000_MNG_IAMT_MODE << E1000_FWSM_MODE_SHIFT);
+}
+
+/**
+ *  e1000_enable_tx_pkt_filtering_generic - Enable packet filtering on Tx
+ *  @hw: pointer to the HW structure
+ *
+ *  Enables packet filtering on transmit packets if manageability is enabled
+ *  and host interface is enabled.
+ **/
+bool e1000_enable_tx_pkt_filtering_generic(struct e1000_hw *hw)
+{
+	struct e1000_host_mng_dhcp_cookie *hdr = &hw->mng_cookie;
+	u32 *buffer = (u32 *)&hw->mng_cookie;
+	u32 offset;
+	s32 ret_val, hdr_csum, csum;
+	u8 i, len;
+
+	DEBUGFUNC("e1000_enable_tx_pkt_filtering_generic");
+
+	hw->mac.tx_pkt_filtering = true;
+
+	/* No manageability, no filtering */
+	if (!hw->mac.ops.check_mng_mode(hw)) {
+		hw->mac.tx_pkt_filtering = false;
+		return hw->mac.tx_pkt_filtering;
+	}
+
+	/* If we can't read from the host interface for whatever
+	 * reason, disable filtering.
+	 */
+	ret_val = e1000_mng_enable_host_if_generic(hw);
+	if (ret_val != E1000_SUCCESS) {
+		hw->mac.tx_pkt_filtering = false;
+		return hw->mac.tx_pkt_filtering;
+	}
+
+	/* Read in the header.  Length and offset are in dwords. */
+	len    = E1000_MNG_DHCP_COOKIE_LENGTH >> 2;
+	offset = E1000_MNG_DHCP_COOKIE_OFFSET >> 2;
+	for (i = 0; i < len; i++)
+		*(buffer + i) = E1000_READ_REG_ARRAY_DWORD(hw, E1000_HOST_IF,
+							   offset + i);
+	hdr_csum = hdr->checksum;
+	hdr->checksum = 0;
+	csum = e1000_calculate_checksum((u8 *)hdr,
+					E1000_MNG_DHCP_COOKIE_LENGTH);
+	/* If either the checksums or signature don't match, then
+	 * the cookie area isn't considered valid, in which case we
+	 * take the safe route of assuming Tx filtering is enabled.
+	 */
+	if ((hdr_csum != csum) || (hdr->signature != E1000_IAMT_SIGNATURE)) {
+		hw->mac.tx_pkt_filtering = true;
+		return hw->mac.tx_pkt_filtering;
+	}
+
+	/* Cookie area is valid, make the final check for filtering. */
+	if (!(hdr->status & E1000_MNG_DHCP_COOKIE_STATUS_PARSING))
+		hw->mac.tx_pkt_filtering = false;
+
+	return hw->mac.tx_pkt_filtering;
+}
+
+/**
+ *  e1000_mng_write_cmd_header_generic - Writes manageability command header
+ *  @hw: pointer to the HW structure
+ *  @hdr: pointer to the host interface command header
+ *
+ *  Writes the command header after does the checksum calculation.
+ **/
+s32 e1000_mng_write_cmd_header_generic(struct e1000_hw *hw,
+				      struct e1000_host_mng_command_header *hdr)
+{
+	u16 i, length = sizeof(struct e1000_host_mng_command_header);
+
+	DEBUGFUNC("e1000_mng_write_cmd_header_generic");
+
+	/* Write the whole command header structure with new checksum. */
+
+	hdr->checksum = e1000_calculate_checksum((u8 *)hdr, length);
+
+	length >>= 2;
+	/* Write the relevant command block into the ram area. */
+	for (i = 0; i < length; i++) {
+		E1000_WRITE_REG_ARRAY_DWORD(hw, E1000_HOST_IF, i,
+					    *((u32 *) hdr + i));
+		E1000_WRITE_FLUSH(hw);
+	}
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_mng_host_if_write_generic - Write to the manageability host interface
+ *  @hw: pointer to the HW structure
+ *  @buffer: pointer to the host interface buffer
+ *  @length: size of the buffer
+ *  @offset: location in the buffer to write to
+ *  @sum: sum of the data (not checksum)
+ *
+ *  This function writes the buffer content at the offset given on the host if.
+ *  It also does alignment considerations to do the writes in most efficient
+ *  way.  Also fills up the sum of the buffer in *buffer parameter.
+ **/
+s32 e1000_mng_host_if_write_generic(struct e1000_hw *hw, u8 *buffer,
+				    u16 length, u16 offset, u8 *sum)
+{
+	u8 *tmp;
+	u8 *bufptr = buffer;
+	u32 data = 0;
+	u16 remaining, i, j, prev_bytes;
+
+	DEBUGFUNC("e1000_mng_host_if_write_generic");
+
+	/* sum = only sum of the data and it is not checksum */
+
+	if (length == 0 || offset + length > E1000_HI_MAX_MNG_DATA_LENGTH)
+		return -E1000_ERR_PARAM;
+
+	tmp = (u8 *)&data;
+	prev_bytes = offset & 0x3;
+	offset >>= 2;
+
+	if (prev_bytes) {
+		data = E1000_READ_REG_ARRAY_DWORD(hw, E1000_HOST_IF, offset);
+		for (j = prev_bytes; j < sizeof(u32); j++) {
+			*(tmp + j) = *bufptr++;
+			*sum += *(tmp + j);
+		}
+		E1000_WRITE_REG_ARRAY_DWORD(hw, E1000_HOST_IF, offset, data);
+		length -= j - prev_bytes;
+		offset++;
+	}
+
+	remaining = length & 0x3;
+	length -= remaining;
+
+	/* Calculate length in DWORDs */
+	length >>= 2;
+
+	/* The device driver writes the relevant command block into the
+	 * ram area.
+	 */
+	for (i = 0; i < length; i++) {
+		for (j = 0; j < sizeof(u32); j++) {
+			*(tmp + j) = *bufptr++;
+			*sum += *(tmp + j);
+		}
+
+		E1000_WRITE_REG_ARRAY_DWORD(hw, E1000_HOST_IF, offset + i,
+					    data);
+	}
+	if (remaining) {
+		for (j = 0; j < sizeof(u32); j++) {
+			if (j < remaining)
+				*(tmp + j) = *bufptr++;
+			else
+				*(tmp + j) = 0;
+
+			*sum += *(tmp + j);
+		}
+		E1000_WRITE_REG_ARRAY_DWORD(hw, E1000_HOST_IF, offset + i,
+					    data);
+	}
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_mng_write_dhcp_info_generic - Writes DHCP info to host interface
+ *  @hw: pointer to the HW structure
+ *  @buffer: pointer to the host interface
+ *  @length: size of the buffer
+ *
+ *  Writes the DHCP information to the host interface.
+ **/
+s32 e1000_mng_write_dhcp_info_generic(struct e1000_hw *hw, u8 *buffer,
+				      u16 length)
+{
+	struct e1000_host_mng_command_header hdr;
+	s32 ret_val;
+	u32 hicr;
+
+	DEBUGFUNC("e1000_mng_write_dhcp_info_generic");
+
+	hdr.command_id = E1000_MNG_DHCP_TX_PAYLOAD_CMD;
+	hdr.command_length = length;
+	hdr.reserved1 = 0;
+	hdr.reserved2 = 0;
+	hdr.checksum = 0;
+
+	/* Enable the host interface */
+	ret_val = e1000_mng_enable_host_if_generic(hw);
+	if (ret_val)
+		return ret_val;
+
+	/* Populate the host interface with the contents of "buffer". */
+	ret_val = e1000_mng_host_if_write_generic(hw, buffer, length,
+						  sizeof(hdr), &(hdr.checksum));
+	if (ret_val)
+		return ret_val;
+
+	/* Write the manageability command header */
+	ret_val = e1000_mng_write_cmd_header_generic(hw, &hdr);
+	if (ret_val)
+		return ret_val;
+
+	/* Tell the ARC a new command is pending. */
+	hicr = E1000_READ_REG(hw, E1000_HICR);
+	E1000_WRITE_REG(hw, E1000_HICR, hicr | E1000_HICR_C);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_enable_mng_pass_thru - Check if management passthrough is needed
+ *  @hw: pointer to the HW structure
+ *
+ *  Verifies the hardware needs to leave interface enabled so that frames can
+ *  be directed to and from the management interface.
+ **/
+bool e1000_enable_mng_pass_thru(struct e1000_hw *hw)
+{
+	u32 manc;
+	u32 fwsm, factps;
+
+	DEBUGFUNC("e1000_enable_mng_pass_thru");
+
+	if (!hw->mac.asf_firmware_present)
+		return false;
+
+	manc = E1000_READ_REG(hw, E1000_MANC);
+
+	if (!(manc & E1000_MANC_RCV_TCO_EN))
+		return false;
+
+	if (hw->mac.has_fwsm) {
+		fwsm = E1000_READ_REG(hw, E1000_FWSM);
+		factps = E1000_READ_REG(hw, E1000_FACTPS);
+
+		if (!(factps & E1000_FACTPS_MNGCG) &&
+		    ((fwsm & E1000_FWSM_MODE_MASK) ==
+		     (e1000_mng_mode_pt << E1000_FWSM_MODE_SHIFT)))
+			return true;
+	} else if ((manc & E1000_MANC_SMBUS_EN) &&
+		   !(manc & E1000_MANC_ASF_EN)) {
+		return true;
+	}
+
+	return false;
+}
+
+/**
+ *  e1000_host_interface_command - Writes buffer to host interface
+ *  @hw: pointer to the HW structure
+ *  @buffer: contains a command to write
+ *  @length: the byte length of the buffer, must be multiple of 4 bytes
+ *
+ *  Writes a buffer to the Host Interface.  Upon success, returns E1000_SUCCESS
+ *  else returns E1000_ERR_HOST_INTERFACE_COMMAND.
+ **/
+s32 e1000_host_interface_command(struct e1000_hw *hw, u8 *buffer, u32 length)
+{
+	u32 hicr, i;
+
+	DEBUGFUNC("e1000_host_interface_command");
+
+	if (!(hw->mac.arc_subsystem_valid)) {
+		DEBUGOUT("Hardware doesn't support host interface command.\n");
+		return E1000_SUCCESS;
+	}
+
+	if (!hw->mac.asf_firmware_present) {
+		DEBUGOUT("Firmware is not present.\n");
+		return E1000_SUCCESS;
+	}
+
+	if (length == 0 || length & 0x3 ||
+	    length > E1000_HI_MAX_BLOCK_BYTE_LENGTH) {
+		DEBUGOUT("Buffer length failure.\n");
+		return -E1000_ERR_HOST_INTERFACE_COMMAND;
+	}
+
+	/* Check that the host interface is enabled. */
+	hicr = E1000_READ_REG(hw, E1000_HICR);
+	if (!(hicr & E1000_HICR_EN)) {
+		DEBUGOUT("E1000_HOST_EN bit disabled.\n");
+		return -E1000_ERR_HOST_INTERFACE_COMMAND;
+	}
+
+	/* Calculate length in DWORDs */
+	length >>= 2;
+
+	/* The device driver writes the relevant command block
+	 * into the ram area.
+	 */
+	for (i = 0; i < length; i++)
+		E1000_WRITE_REG_ARRAY_DWORD(hw, E1000_HOST_IF, i,
+					    *((u32 *)buffer + i));
+
+	/* Setting this bit tells the ARC that a new command is pending. */
+	E1000_WRITE_REG(hw, E1000_HICR, hicr | E1000_HICR_C);
+
+	for (i = 0; i < E1000_HI_COMMAND_TIMEOUT; i++) {
+		hicr = E1000_READ_REG(hw, E1000_HICR);
+		if (!(hicr & E1000_HICR_C))
+			break;
+		msec_delay(1);
+	}
+
+	/* Check command successful completion. */
+	if (i == E1000_HI_COMMAND_TIMEOUT ||
+	    (!(E1000_READ_REG(hw, E1000_HICR) & E1000_HICR_SV))) {
+		DEBUGOUT("Command has failed with no status valid.\n");
+		return -E1000_ERR_HOST_INTERFACE_COMMAND;
+	}
+
+	for (i = 0; i < length; i++)
+		*((u32 *)buffer + i) = E1000_READ_REG_ARRAY_DWORD(hw,
+								  E1000_HOST_IF,
+								  i);
+
+	return E1000_SUCCESS;
+}
+/**
+ *  e1000_load_firmware - Writes proxy FW code buffer to host interface
+ *                        and execute.
+ *  @hw: pointer to the HW structure
+ *  @buffer: contains a firmware to write
+ *  @length: the byte length of the buffer, must be multiple of 4 bytes
+ *
+ *  Upon success returns E1000_SUCCESS, returns E1000_ERR_CONFIG if not enabled
+ *  in HW else returns E1000_ERR_HOST_INTERFACE_COMMAND.
+ **/
+s32 e1000_load_firmware(struct e1000_hw *hw, u8 *buffer, u32 length)
+{
+	u32 hicr, hibba, fwsm, icr, i;
+
+	DEBUGFUNC("e1000_load_firmware");
+
+	if (hw->mac.type < e1000_i210) {
+		DEBUGOUT("Hardware doesn't support loading FW by the driver\n");
+		return -E1000_ERR_CONFIG;
+	}
+
+	/* Check that the host interface is enabled. */
+	hicr = E1000_READ_REG(hw, E1000_HICR);
+	if (!(hicr & E1000_HICR_EN)) {
+		DEBUGOUT("E1000_HOST_EN bit disabled.\n");
+		return -E1000_ERR_CONFIG;
+	}
+	if (!(hicr & E1000_HICR_MEMORY_BASE_EN)) {
+		DEBUGOUT("E1000_HICR_MEMORY_BASE_EN bit disabled.\n");
+		return -E1000_ERR_CONFIG;
+	}
+
+	if (length == 0 || length & 0x3 || length > E1000_HI_FW_MAX_LENGTH) {
+		DEBUGOUT("Buffer length failure.\n");
+		return -E1000_ERR_INVALID_ARGUMENT;
+	}
+
+	/* Clear notification from ROM-FW by reading ICR register */
+	icr = E1000_READ_REG(hw, E1000_ICR_V2);
+
+	/* Reset ROM-FW */
+	hicr = E1000_READ_REG(hw, E1000_HICR);
+	hicr |= E1000_HICR_FW_RESET_ENABLE;
+	E1000_WRITE_REG(hw, E1000_HICR, hicr);
+	hicr |= E1000_HICR_FW_RESET;
+	E1000_WRITE_REG(hw, E1000_HICR, hicr);
+	E1000_WRITE_FLUSH(hw);
+
+	/* Wait till MAC notifies about its readiness after ROM-FW reset */
+	for (i = 0; i < (E1000_HI_COMMAND_TIMEOUT * 2); i++) {
+		icr = E1000_READ_REG(hw, E1000_ICR_V2);
+		if (icr & E1000_ICR_MNG)
+			break;
+		msec_delay(1);
+	}
+
+	/* Check for timeout */
+	if (i == E1000_HI_COMMAND_TIMEOUT) {
+		DEBUGOUT("FW reset failed.\n");
+		return -E1000_ERR_HOST_INTERFACE_COMMAND;
+	}
+
+	/* Wait till MAC is ready to accept new FW code */
+	for (i = 0; i < E1000_HI_COMMAND_TIMEOUT; i++) {
+		fwsm = E1000_READ_REG(hw, E1000_FWSM);
+		if ((fwsm & E1000_FWSM_FW_VALID) &&
+		    ((fwsm & E1000_FWSM_MODE_MASK) >> E1000_FWSM_MODE_SHIFT ==
+		    E1000_FWSM_HI_EN_ONLY_MODE))
+			break;
+		msec_delay(1);
+	}
+
+	/* Check for timeout */
+	if (i == E1000_HI_COMMAND_TIMEOUT) {
+		DEBUGOUT("FW reset failed.\n");
+		return -E1000_ERR_HOST_INTERFACE_COMMAND;
+	}
+
+	/* Calculate length in DWORDs */
+	length >>= 2;
+
+	/* The device driver writes the relevant FW code block
+	 * into the ram area in DWORDs via 1kB ram addressing window.
+	 */
+	for (i = 0; i < length; i++) {
+		if (!(i % E1000_HI_FW_BLOCK_DWORD_LENGTH)) {
+			/* Point to correct 1kB ram window */
+			hibba = E1000_HI_FW_BASE_ADDRESS +
+				((E1000_HI_FW_BLOCK_DWORD_LENGTH << 2) *
+				(i / E1000_HI_FW_BLOCK_DWORD_LENGTH));
+
+			E1000_WRITE_REG(hw, E1000_HIBBA, hibba);
+		}
+
+		E1000_WRITE_REG_ARRAY_DWORD(hw, E1000_HOST_IF,
+					    i % E1000_HI_FW_BLOCK_DWORD_LENGTH,
+					    *((u32 *)buffer + i));
+	}
+
+	/* Setting this bit tells the ARC that a new FW is ready to execute. */
+	hicr = E1000_READ_REG(hw, E1000_HICR);
+	E1000_WRITE_REG(hw, E1000_HICR, hicr | E1000_HICR_C);
+
+	for (i = 0; i < E1000_HI_COMMAND_TIMEOUT; i++) {
+		hicr = E1000_READ_REG(hw, E1000_HICR);
+		if (!(hicr & E1000_HICR_C))
+			break;
+		msec_delay(1);
+	}
+
+	/* Check for successful FW start. */
+	if (i == E1000_HI_COMMAND_TIMEOUT) {
+		DEBUGOUT("New FW did not start within timeout period.\n");
+		return -E1000_ERR_HOST_INTERFACE_COMMAND;
+	}
+
+	return E1000_SUCCESS;
+}
+
+
diff --git a/drivers/net/igb/e1000_manage.h b/drivers/net/igb/e1000_manage.h
new file mode 100644
index 000000000000..c94b218542e2
--- /dev/null
+++ b/drivers/net/igb/e1000_manage.h
@@ -0,0 +1,89 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+#ifndef _E1000_MANAGE_H_
+#define _E1000_MANAGE_H_
+
+bool e1000_check_mng_mode_generic(struct e1000_hw *hw);
+bool e1000_enable_tx_pkt_filtering_generic(struct e1000_hw *hw);
+s32  e1000_mng_enable_host_if_generic(struct e1000_hw *hw);
+s32  e1000_mng_host_if_write_generic(struct e1000_hw *hw, u8 *buffer,
+				     u16 length, u16 offset, u8 *sum);
+s32  e1000_mng_write_cmd_header_generic(struct e1000_hw *hw,
+				     struct e1000_host_mng_command_header *hdr);
+s32  e1000_mng_write_dhcp_info_generic(struct e1000_hw *hw,
+				       u8 *buffer, u16 length);
+bool e1000_enable_mng_pass_thru(struct e1000_hw *hw);
+u8 e1000_calculate_checksum(u8 *buffer, u32 length);
+s32 e1000_host_interface_command(struct e1000_hw *hw, u8 *buffer, u32 length);
+s32 e1000_load_firmware(struct e1000_hw *hw, u8 *buffer, u32 length);
+
+enum e1000_mng_mode {
+	e1000_mng_mode_none = 0,
+	e1000_mng_mode_asf,
+	e1000_mng_mode_pt,
+	e1000_mng_mode_ipmi,
+	e1000_mng_mode_host_if_only
+};
+
+#define E1000_FACTPS_MNGCG			0x20000000
+
+#define E1000_FWSM_MODE_MASK			0xE
+#define E1000_FWSM_MODE_SHIFT			1
+#define E1000_FWSM_FW_VALID			0x00008000
+#define E1000_FWSM_HI_EN_ONLY_MODE		0x4
+
+#define E1000_MNG_IAMT_MODE			0x3
+#define E1000_MNG_DHCP_COOKIE_LENGTH		0x10
+#define E1000_MNG_DHCP_COOKIE_OFFSET		0x6F0
+#define E1000_MNG_DHCP_COMMAND_TIMEOUT		10
+#define E1000_MNG_DHCP_TX_PAYLOAD_CMD		64
+#define E1000_MNG_DHCP_COOKIE_STATUS_PARSING	0x1
+#define E1000_MNG_DHCP_COOKIE_STATUS_VLAN	0x2
+
+#define E1000_VFTA_ENTRY_SHIFT			5
+#define E1000_VFTA_ENTRY_MASK			0x7F
+#define E1000_VFTA_ENTRY_BIT_SHIFT_MASK		0x1F
+
+#define E1000_HI_MAX_BLOCK_BYTE_LENGTH		1792 /* Num of bytes in range */
+#define E1000_HI_MAX_BLOCK_DWORD_LENGTH		448 /* Num of dwords in range */
+#define E1000_HI_COMMAND_TIMEOUT		500 /* Process HI cmd limit */
+#define E1000_HI_FW_BASE_ADDRESS		0x10000
+#define E1000_HI_FW_MAX_LENGTH			(64 * 1024) /* Num of bytes */
+#define E1000_HI_FW_BLOCK_DWORD_LENGTH		256 /* Num of DWORDs per page */
+#define E1000_HICR_MEMORY_BASE_EN		0x200 /* MB Enable bit - RO */
+#define E1000_HICR_EN			0x01  /* Enable bit - RO */
+/* Driver sets this bit when done to put command in RAM */
+#define E1000_HICR_C			0x02
+#define E1000_HICR_SV			0x04  /* Status Validity */
+#define E1000_HICR_FW_RESET_ENABLE	0x40
+#define E1000_HICR_FW_RESET		0x80
+
+/* Intel(R) Active Management Technology signature */
+#define E1000_IAMT_SIGNATURE		0x544D4149
+
+#endif
diff --git a/drivers/net/igb/e1000_mbx.c b/drivers/net/igb/e1000_mbx.c
index 74f2f11ac290..8750b4634603 100644
--- a/drivers/net/igb/e1000_mbx.c
+++ b/drivers/net/igb/e1000_mbx.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2007-2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -28,7 +28,33 @@
 #include "e1000_mbx.h"
 
 /**
- *  igb_read_mbx - Reads a message from the mailbox
+ *  e1000_null_mbx_check_for_flag - No-op function, return 0
+ *  @hw: pointer to the HW structure
+ **/
+static s32 e1000_null_mbx_check_for_flag(struct e1000_hw E1000_UNUSEDARG *hw,
+					 u16 E1000_UNUSEDARG mbx_id)
+{
+	DEBUGFUNC("e1000_null_mbx_check_flag");
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_null_mbx_transact - No-op function, return 0
+ *  @hw: pointer to the HW structure
+ **/
+static s32 e1000_null_mbx_transact(struct e1000_hw E1000_UNUSEDARG *hw,
+				   u32 E1000_UNUSEDARG *msg,
+				   u16 E1000_UNUSEDARG size,
+				   u16 E1000_UNUSEDARG mbx_id)
+{
+	DEBUGFUNC("e1000_null_mbx_rw_msg");
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_read_mbx - Reads a message from the mailbox
  *  @hw: pointer to the HW structure
  *  @msg: The message buffer
  *  @size: Length of buffer
@@ -36,11 +62,13 @@
  *
  *  returns SUCCESS if it successfuly read message from buffer
  **/
-s32 igb_read_mbx(struct e1000_hw *hw, u32 *msg, u16 size, u16 mbx_id)
+s32 e1000_read_mbx(struct e1000_hw *hw, u32 *msg, u16 size, u16 mbx_id)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
 	s32 ret_val = -E1000_ERR_MBX;
 
+	DEBUGFUNC("e1000_read_mbx");
+
 	/* limit read to size of mailbox */
 	if (size > mbx->size)
 		size = mbx->size;
@@ -52,7 +80,7 @@ s32 igb_read_mbx(struct e1000_hw *hw, u32 *msg, u16 size, u16 mbx_id)
 }
 
 /**
- *  igb_write_mbx - Write a message to the mailbox
+ *  e1000_write_mbx - Write a message to the mailbox
  *  @hw: pointer to the HW structure
  *  @msg: The message buffer
  *  @size: Length of buffer
@@ -60,10 +88,12 @@ s32 igb_read_mbx(struct e1000_hw *hw, u32 *msg, u16 size, u16 mbx_id)
  *
  *  returns SUCCESS if it successfully copied message into the buffer
  **/
-s32 igb_write_mbx(struct e1000_hw *hw, u32 *msg, u16 size, u16 mbx_id)
+s32 e1000_write_mbx(struct e1000_hw *hw, u32 *msg, u16 size, u16 mbx_id)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
-	s32 ret_val = 0;
+	s32 ret_val = E1000_SUCCESS;
+
+	DEBUGFUNC("e1000_write_mbx");
 
 	if (size > mbx->size)
 		ret_val = -E1000_ERR_MBX;
@@ -75,17 +105,19 @@ s32 igb_write_mbx(struct e1000_hw *hw, u32 *msg, u16 size, u16 mbx_id)
 }
 
 /**
- *  igb_check_for_msg - checks to see if someone sent us mail
+ *  e1000_check_for_msg - checks to see if someone sent us mail
  *  @hw: pointer to the HW structure
  *  @mbx_id: id of mailbox to check
  *
  *  returns SUCCESS if the Status bit was found or else ERR_MBX
  **/
-s32 igb_check_for_msg(struct e1000_hw *hw, u16 mbx_id)
+s32 e1000_check_for_msg(struct e1000_hw *hw, u16 mbx_id)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
 	s32 ret_val = -E1000_ERR_MBX;
 
+	DEBUGFUNC("e1000_check_for_msg");
+
 	if (mbx->ops.check_for_msg)
 		ret_val = mbx->ops.check_for_msg(hw, mbx_id);
 
@@ -93,17 +125,19 @@ s32 igb_check_for_msg(struct e1000_hw *hw, u16 mbx_id)
 }
 
 /**
- *  igb_check_for_ack - checks to see if someone sent us ACK
+ *  e1000_check_for_ack - checks to see if someone sent us ACK
  *  @hw: pointer to the HW structure
  *  @mbx_id: id of mailbox to check
  *
  *  returns SUCCESS if the Status bit was found or else ERR_MBX
  **/
-s32 igb_check_for_ack(struct e1000_hw *hw, u16 mbx_id)
+s32 e1000_check_for_ack(struct e1000_hw *hw, u16 mbx_id)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
 	s32 ret_val = -E1000_ERR_MBX;
 
+	DEBUGFUNC("e1000_check_for_ack");
+
 	if (mbx->ops.check_for_ack)
 		ret_val = mbx->ops.check_for_ack(hw, mbx_id);
 
@@ -111,17 +145,19 @@ s32 igb_check_for_ack(struct e1000_hw *hw, u16 mbx_id)
 }
 
 /**
- *  igb_check_for_rst - checks to see if other side has reset
+ *  e1000_check_for_rst - checks to see if other side has reset
  *  @hw: pointer to the HW structure
  *  @mbx_id: id of mailbox to check
  *
  *  returns SUCCESS if the Status bit was found or else ERR_MBX
  **/
-s32 igb_check_for_rst(struct e1000_hw *hw, u16 mbx_id)
+s32 e1000_check_for_rst(struct e1000_hw *hw, u16 mbx_id)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
 	s32 ret_val = -E1000_ERR_MBX;
 
+	DEBUGFUNC("e1000_check_for_rst");
+
 	if (mbx->ops.check_for_rst)
 		ret_val = mbx->ops.check_for_rst(hw, mbx_id);
 
@@ -129,17 +165,19 @@ s32 igb_check_for_rst(struct e1000_hw *hw, u16 mbx_id)
 }
 
 /**
- *  igb_poll_for_msg - Wait for message notification
+ *  e1000_poll_for_msg - Wait for message notification
  *  @hw: pointer to the HW structure
  *  @mbx_id: id of mailbox to write
  *
  *  returns SUCCESS if it successfully received a message notification
  **/
-static s32 igb_poll_for_msg(struct e1000_hw *hw, u16 mbx_id)
+static s32 e1000_poll_for_msg(struct e1000_hw *hw, u16 mbx_id)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
 	int countdown = mbx->timeout;
 
+	DEBUGFUNC("e1000_poll_for_msg");
+
 	if (!countdown || !mbx->ops.check_for_msg)
 		goto out;
 
@@ -147,28 +185,30 @@ static s32 igb_poll_for_msg(struct e1000_hw *hw, u16 mbx_id)
 		countdown--;
 		if (!countdown)
 			break;
-		udelay(mbx->usec_delay);
+		usec_delay(mbx->usec_delay);
 	}
 
 	/* if we failed, all future posted messages fail until reset */
 	if (!countdown)
 		mbx->timeout = 0;
 out:
-	return countdown ? 0 : -E1000_ERR_MBX;
+	return countdown ? E1000_SUCCESS : -E1000_ERR_MBX;
 }
 
 /**
- *  igb_poll_for_ack - Wait for message acknowledgement
+ *  e1000_poll_for_ack - Wait for message acknowledgement
  *  @hw: pointer to the HW structure
  *  @mbx_id: id of mailbox to write
  *
  *  returns SUCCESS if it successfully received a message acknowledgement
  **/
-static s32 igb_poll_for_ack(struct e1000_hw *hw, u16 mbx_id)
+static s32 e1000_poll_for_ack(struct e1000_hw *hw, u16 mbx_id)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
 	int countdown = mbx->timeout;
 
+	DEBUGFUNC("e1000_poll_for_ack");
+
 	if (!countdown || !mbx->ops.check_for_ack)
 		goto out;
 
@@ -176,18 +216,18 @@ static s32 igb_poll_for_ack(struct e1000_hw *hw, u16 mbx_id)
 		countdown--;
 		if (!countdown)
 			break;
-		udelay(mbx->usec_delay);
+		usec_delay(mbx->usec_delay);
 	}
 
 	/* if we failed, all future posted messages fail until reset */
 	if (!countdown)
 		mbx->timeout = 0;
 out:
-	return countdown ? 0 : -E1000_ERR_MBX;
+	return countdown ? E1000_SUCCESS : -E1000_ERR_MBX;
 }
 
 /**
- *  igb_read_posted_mbx - Wait for message notification and receive message
+ *  e1000_read_posted_mbx - Wait for message notification and receive message
  *  @hw: pointer to the HW structure
  *  @msg: The message buffer
  *  @size: Length of buffer
@@ -196,16 +236,19 @@ out:
  *  returns SUCCESS if it successfully received a message notification and
  *  copied it into the receive buffer.
  **/
-static s32 igb_read_posted_mbx(struct e1000_hw *hw, u32 *msg, u16 size, u16 mbx_id)
+s32 e1000_read_posted_mbx(struct e1000_hw *hw, u32 *msg, u16 size, u16 mbx_id)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
 	s32 ret_val = -E1000_ERR_MBX;
 
+	DEBUGFUNC("e1000_read_posted_mbx");
+
 	if (!mbx->ops.read)
 		goto out;
 
-	ret_val = igb_poll_for_msg(hw, mbx_id);
+	ret_val = e1000_poll_for_msg(hw, mbx_id);
 
+	/* if ack received read message, otherwise we timed out */
 	if (!ret_val)
 		ret_val = mbx->ops.read(hw, msg, size, mbx_id);
 out:
@@ -213,7 +256,7 @@ out:
 }
 
 /**
- *  igb_write_posted_mbx - Write a message to the mailbox, wait for ack
+ *  e1000_write_posted_mbx - Write a message to the mailbox, wait for ack
  *  @hw: pointer to the HW structure
  *  @msg: The message buffer
  *  @size: Length of buffer
@@ -222,11 +265,13 @@ out:
  *  returns SUCCESS if it successfully copied message into the buffer and
  *  received an ack to that message within delay * timeout period
  **/
-static s32 igb_write_posted_mbx(struct e1000_hw *hw, u32 *msg, u16 size, u16 mbx_id)
+s32 e1000_write_posted_mbx(struct e1000_hw *hw, u32 *msg, u16 size, u16 mbx_id)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
 	s32 ret_val = -E1000_ERR_MBX;
 
+	DEBUGFUNC("e1000_write_posted_mbx");
+
 	/* exit if either we can't write or there isn't a defined timeout */
 	if (!mbx->ops.write || !mbx->timeout)
 		goto out;
@@ -236,37 +281,58 @@ static s32 igb_write_posted_mbx(struct e1000_hw *hw, u32 *msg, u16 size, u16 mbx
 
 	/* if msg sent wait until we receive an ack */
 	if (!ret_val)
-		ret_val = igb_poll_for_ack(hw, mbx_id);
+		ret_val = e1000_poll_for_ack(hw, mbx_id);
 out:
 	return ret_val;
 }
 
-static s32 igb_check_for_bit_pf(struct e1000_hw *hw, u32 mask)
+/**
+ *  e1000_init_mbx_ops_generic - Initialize mbx function pointers
+ *  @hw: pointer to the HW structure
+ *
+ *  Sets the function pointers to no-op functions
+ **/
+void e1000_init_mbx_ops_generic(struct e1000_hw *hw)
 {
-	u32 mbvficr = rd32(E1000_MBVFICR);
+	struct e1000_mbx_info *mbx = &hw->mbx;
+	mbx->ops.init_params = e1000_null_ops_generic;
+	mbx->ops.read = e1000_null_mbx_transact;
+	mbx->ops.write = e1000_null_mbx_transact;
+	mbx->ops.check_for_msg = e1000_null_mbx_check_for_flag;
+	mbx->ops.check_for_ack = e1000_null_mbx_check_for_flag;
+	mbx->ops.check_for_rst = e1000_null_mbx_check_for_flag;
+	mbx->ops.read_posted = e1000_read_posted_mbx;
+	mbx->ops.write_posted = e1000_write_posted_mbx;
+}
+
+static s32 e1000_check_for_bit_pf(struct e1000_hw *hw, u32 mask)
+{
+	u32 mbvficr = E1000_READ_REG(hw, E1000_MBVFICR);
 	s32 ret_val = -E1000_ERR_MBX;
 
 	if (mbvficr & mask) {
-		ret_val = 0;
-		wr32(E1000_MBVFICR, mask);
+		ret_val = E1000_SUCCESS;
+		E1000_WRITE_REG(hw, E1000_MBVFICR, mask);
 	}
 
 	return ret_val;
 }
 
 /**
- *  igb_check_for_msg_pf - checks to see if the VF has sent mail
+ *  e1000_check_for_msg_pf - checks to see if the VF has sent mail
  *  @hw: pointer to the HW structure
  *  @vf_number: the VF index
  *
  *  returns SUCCESS if the VF has set the Status bit or else ERR_MBX
  **/
-static s32 igb_check_for_msg_pf(struct e1000_hw *hw, u16 vf_number)
+static s32 e1000_check_for_msg_pf(struct e1000_hw *hw, u16 vf_number)
 {
 	s32 ret_val = -E1000_ERR_MBX;
 
-	if (!igb_check_for_bit_pf(hw, E1000_MBVFICR_VFREQ_VF1 << vf_number)) {
-		ret_val = 0;
+	DEBUGFUNC("e1000_check_for_msg_pf");
+
+	if (!e1000_check_for_bit_pf(hw, E1000_MBVFICR_VFREQ_VF1 << vf_number)) {
+		ret_val = E1000_SUCCESS;
 		hw->mbx.stats.reqs++;
 	}
 
@@ -274,18 +340,20 @@ static s32 igb_check_for_msg_pf(struct e1000_hw *hw, u16 vf_number)
 }
 
 /**
- *  igb_check_for_ack_pf - checks to see if the VF has ACKed
+ *  e1000_check_for_ack_pf - checks to see if the VF has ACKed
  *  @hw: pointer to the HW structure
  *  @vf_number: the VF index
  *
  *  returns SUCCESS if the VF has set the Status bit or else ERR_MBX
  **/
-static s32 igb_check_for_ack_pf(struct e1000_hw *hw, u16 vf_number)
+static s32 e1000_check_for_ack_pf(struct e1000_hw *hw, u16 vf_number)
 {
 	s32 ret_val = -E1000_ERR_MBX;
 
-	if (!igb_check_for_bit_pf(hw, E1000_MBVFICR_VFACK_VF1 << vf_number)) {
-		ret_val = 0;
+	DEBUGFUNC("e1000_check_for_ack_pf");
+
+	if (!e1000_check_for_bit_pf(hw, E1000_MBVFICR_VFACK_VF1 << vf_number)) {
+		ret_val = E1000_SUCCESS;
 		hw->mbx.stats.acks++;
 	}
 
@@ -293,20 +361,22 @@ static s32 igb_check_for_ack_pf(struct e1000_hw *hw, u16 vf_number)
 }
 
 /**
- *  igb_check_for_rst_pf - checks to see if the VF has reset
+ *  e1000_check_for_rst_pf - checks to see if the VF has reset
  *  @hw: pointer to the HW structure
  *  @vf_number: the VF index
  *
  *  returns SUCCESS if the VF has set the Status bit or else ERR_MBX
  **/
-static s32 igb_check_for_rst_pf(struct e1000_hw *hw, u16 vf_number)
+static s32 e1000_check_for_rst_pf(struct e1000_hw *hw, u16 vf_number)
 {
-	u32 vflre = rd32(E1000_VFLRE);
+	u32 vflre = E1000_READ_REG(hw, E1000_VFLRE);
 	s32 ret_val = -E1000_ERR_MBX;
 
+	DEBUGFUNC("e1000_check_for_rst_pf");
+
 	if (vflre & (1 << vf_number)) {
-		ret_val = 0;
-		wr32(E1000_VFLRE, (1 << vf_number));
+		ret_val = E1000_SUCCESS;
+		E1000_WRITE_REG(hw, E1000_VFLRE, (1 << vf_number));
 		hw->mbx.stats.rsts++;
 	}
 
@@ -314,31 +384,32 @@ static s32 igb_check_for_rst_pf(struct e1000_hw *hw, u16 vf_number)
 }
 
 /**
- *  igb_obtain_mbx_lock_pf - obtain mailbox lock
+ *  e1000_obtain_mbx_lock_pf - obtain mailbox lock
  *  @hw: pointer to the HW structure
  *  @vf_number: the VF index
  *
  *  return SUCCESS if we obtained the mailbox lock
  **/
-static s32 igb_obtain_mbx_lock_pf(struct e1000_hw *hw, u16 vf_number)
+static s32 e1000_obtain_mbx_lock_pf(struct e1000_hw *hw, u16 vf_number)
 {
 	s32 ret_val = -E1000_ERR_MBX;
 	u32 p2v_mailbox;
 
+	DEBUGFUNC("e1000_obtain_mbx_lock_pf");
 
 	/* Take ownership of the buffer */
-	wr32(E1000_P2VMAILBOX(vf_number), E1000_P2VMAILBOX_PFU);
+	E1000_WRITE_REG(hw, E1000_P2VMAILBOX(vf_number), E1000_P2VMAILBOX_PFU);
 
 	/* reserve mailbox for vf use */
-	p2v_mailbox = rd32(E1000_P2VMAILBOX(vf_number));
+	p2v_mailbox = E1000_READ_REG(hw, E1000_P2VMAILBOX(vf_number));
 	if (p2v_mailbox & E1000_P2VMAILBOX_PFU)
-		ret_val = 0;
+		ret_val = E1000_SUCCESS;
 
 	return ret_val;
 }
 
 /**
- *  igb_write_mbx_pf - Places a message in the mailbox
+ *  e1000_write_mbx_pf - Places a message in the mailbox
  *  @hw: pointer to the HW structure
  *  @msg: The message buffer
  *  @size: Length of buffer
@@ -346,27 +417,29 @@ static s32 igb_obtain_mbx_lock_pf(struct e1000_hw *hw, u16 vf_number)
  *
  *  returns SUCCESS if it successfully copied message into the buffer
  **/
-static s32 igb_write_mbx_pf(struct e1000_hw *hw, u32 *msg, u16 size,
-                              u16 vf_number)
+static s32 e1000_write_mbx_pf(struct e1000_hw *hw, u32 *msg, u16 size,
+			      u16 vf_number)
 {
 	s32 ret_val;
 	u16 i;
 
+	DEBUGFUNC("e1000_write_mbx_pf");
+
 	/* lock the mailbox to prevent pf/vf race condition */
-	ret_val = igb_obtain_mbx_lock_pf(hw, vf_number);
+	ret_val = e1000_obtain_mbx_lock_pf(hw, vf_number);
 	if (ret_val)
 		goto out_no_write;
 
 	/* flush msg and acks as we are overwriting the message buffer */
-	igb_check_for_msg_pf(hw, vf_number);
-	igb_check_for_ack_pf(hw, vf_number);
+	e1000_check_for_msg_pf(hw, vf_number);
+	e1000_check_for_ack_pf(hw, vf_number);
 
 	/* copy the caller specified message to the mailbox memory buffer */
 	for (i = 0; i < size; i++)
-		array_wr32(E1000_VMBMEM(vf_number), i, msg[i]);
+		E1000_WRITE_REG_ARRAY(hw, E1000_VMBMEM(vf_number), i, msg[i]);
 
 	/* Interrupt VF to tell it a message has been sent and release buffer*/
-	wr32(E1000_P2VMAILBOX(vf_number), E1000_P2VMAILBOX_STS);
+	E1000_WRITE_REG(hw, E1000_P2VMAILBOX(vf_number), E1000_P2VMAILBOX_STS);
 
 	/* update stats */
 	hw->mbx.stats.msgs_tx++;
@@ -377,7 +450,7 @@ out_no_write:
 }
 
 /**
- *  igb_read_mbx_pf - Read a message from the mailbox
+ *  e1000_read_mbx_pf - Read a message from the mailbox
  *  @hw: pointer to the HW structure
  *  @msg: The message buffer
  *  @size: Length of buffer
@@ -387,23 +460,25 @@ out_no_write:
  *  memory buffer.  The presumption is that the caller knows that there was
  *  a message due to a VF request so no polling for message is needed.
  **/
-static s32 igb_read_mbx_pf(struct e1000_hw *hw, u32 *msg, u16 size,
-                             u16 vf_number)
+static s32 e1000_read_mbx_pf(struct e1000_hw *hw, u32 *msg, u16 size,
+			     u16 vf_number)
 {
 	s32 ret_val;
 	u16 i;
 
+	DEBUGFUNC("e1000_read_mbx_pf");
+
 	/* lock the mailbox to prevent pf/vf race condition */
-	ret_val = igb_obtain_mbx_lock_pf(hw, vf_number);
+	ret_val = e1000_obtain_mbx_lock_pf(hw, vf_number);
 	if (ret_val)
 		goto out_no_read;
 
 	/* copy the message to the mailbox memory buffer */
 	for (i = 0; i < size; i++)
-		msg[i] = array_rd32(E1000_VMBMEM(vf_number), i);
+		msg[i] = E1000_READ_REG_ARRAY(hw, E1000_VMBMEM(vf_number), i);
 
 	/* Acknowledge the message and release buffer */
-	wr32(E1000_P2VMAILBOX(vf_number), E1000_P2VMAILBOX_ACK);
+	E1000_WRITE_REG(hw, E1000_P2VMAILBOX(vf_number), E1000_P2VMAILBOX_ACK);
 
 	/* update stats */
 	hw->mbx.stats.msgs_rx++;
@@ -418,29 +493,34 @@ out_no_read:
  *
  *  Initializes the hw->mbx struct to correct values for pf mailbox
  */
-s32 igb_init_mbx_params_pf(struct e1000_hw *hw)
+s32 e1000_init_mbx_params_pf(struct e1000_hw *hw)
 {
 	struct e1000_mbx_info *mbx = &hw->mbx;
 
-	mbx->timeout = 0;
-	mbx->usec_delay = 0;
-
-	mbx->size = E1000_VFMAILBOX_SIZE;
-
-	mbx->ops.read = igb_read_mbx_pf;
-	mbx->ops.write = igb_write_mbx_pf;
-	mbx->ops.read_posted = igb_read_posted_mbx;
-	mbx->ops.write_posted = igb_write_posted_mbx;
-	mbx->ops.check_for_msg = igb_check_for_msg_pf;
-	mbx->ops.check_for_ack = igb_check_for_ack_pf;
-	mbx->ops.check_for_rst = igb_check_for_rst_pf;
-
-	mbx->stats.msgs_tx = 0;
-	mbx->stats.msgs_rx = 0;
-	mbx->stats.reqs = 0;
-	mbx->stats.acks = 0;
-	mbx->stats.rsts = 0;
-
-	return 0;
+	switch (hw->mac.type) {
+	case e1000_82576:
+	case e1000_i350:
+	case e1000_i354:
+		mbx->timeout = 0;
+		mbx->usec_delay = 0;
+
+		mbx->size = E1000_VFMAILBOX_SIZE;
+
+		mbx->ops.read = e1000_read_mbx_pf;
+		mbx->ops.write = e1000_write_mbx_pf;
+		mbx->ops.read_posted = e1000_read_posted_mbx;
+		mbx->ops.write_posted = e1000_write_posted_mbx;
+		mbx->ops.check_for_msg = e1000_check_for_msg_pf;
+		mbx->ops.check_for_ack = e1000_check_for_ack_pf;
+		mbx->ops.check_for_rst = e1000_check_for_rst_pf;
+
+		mbx->stats.msgs_tx = 0;
+		mbx->stats.msgs_rx = 0;
+		mbx->stats.reqs = 0;
+		mbx->stats.acks = 0;
+		mbx->stats.rsts = 0;
+	default:
+		return E1000_SUCCESS;
+	}
 }
 
diff --git a/drivers/net/igb/e1000_mbx.h b/drivers/net/igb/e1000_mbx.h
index eddb0f83dcea..bbf838c8cb4d 100644
--- a/drivers/net/igb/e1000_mbx.h
+++ b/drivers/net/igb/e1000_mbx.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2007-2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -28,50 +28,60 @@
 #ifndef _E1000_MBX_H_
 #define _E1000_MBX_H_
 
-#include "e1000_hw.h"
+#include "e1000_api.h"
 
-#define E1000_P2VMAILBOX_STS   0x00000001 /* Initiate message send to VF */
-#define E1000_P2VMAILBOX_ACK   0x00000002 /* Ack message recv'd from VF */
-#define E1000_P2VMAILBOX_VFU   0x00000004 /* VF owns the mailbox buffer */
-#define E1000_P2VMAILBOX_PFU   0x00000008 /* PF owns the mailbox buffer */
-#define E1000_P2VMAILBOX_RVFU  0x00000010 /* Reset VFU - used when VF stuck */
+#define E1000_P2VMAILBOX_STS	0x00000001 /* Initiate message send to VF */
+#define E1000_P2VMAILBOX_ACK	0x00000002 /* Ack message recv'd from VF */
+#define E1000_P2VMAILBOX_VFU	0x00000004 /* VF owns the mailbox buffer */
+#define E1000_P2VMAILBOX_PFU	0x00000008 /* PF owns the mailbox buffer */
+#define E1000_P2VMAILBOX_RVFU	0x00000010 /* Reset VFU - used when VF stuck */
 
 #define E1000_MBVFICR_VFREQ_MASK 0x000000FF /* bits for VF messages */
-#define E1000_MBVFICR_VFREQ_VF1  0x00000001 /* bit for VF 1 message */
+#define E1000_MBVFICR_VFREQ_VF1	0x00000001 /* bit for VF 1 message */
 #define E1000_MBVFICR_VFACK_MASK 0x00FF0000 /* bits for VF acks */
-#define E1000_MBVFICR_VFACK_VF1  0x00010000 /* bit for VF 1 ack */
+#define E1000_MBVFICR_VFACK_VF1	0x00010000 /* bit for VF 1 ack */
 
-#define E1000_VFMAILBOX_SIZE   16 /* 16 32 bit words - 64 bytes */
+#define E1000_VFMAILBOX_SIZE	16 /* 16 32 bit words - 64 bytes */
 
 /* If it's a E1000_VF_* msg then it originates in the VF and is sent to the
  * PF.  The reverse is true if it is E1000_PF_*.
  * Message ACK's are the value or'd with 0xF0000000
  */
-#define E1000_VT_MSGTYPE_ACK      0x80000000  /* Messages below or'd with
-                                               * this are the ACK */
-#define E1000_VT_MSGTYPE_NACK     0x40000000  /* Messages below or'd with
-                                               * this are the NACK */
-#define E1000_VT_MSGTYPE_CTS      0x20000000  /* Indicates that VF is still
-                                                 clear to send requests */
-#define E1000_VT_MSGINFO_SHIFT    16
-/* bits 23:16 are used for exra info for certain messages */
-#define E1000_VT_MSGINFO_MASK     (0xFF << E1000_VT_MSGINFO_SHIFT)
-
-#define E1000_VF_RESET            0x01 /* VF requests reset */
-#define E1000_VF_SET_MAC_ADDR     0x02 /* VF requests to set MAC addr */
-#define E1000_VF_SET_MULTICAST    0x03 /* VF requests to set MC addr */
-#define E1000_VF_SET_VLAN         0x04 /* VF requests to set VLAN */
-#define E1000_VF_SET_LPE          0x05 /* VF requests to set VMOLR.LPE */
-#define E1000_VF_SET_PROMISC      0x06 /*VF requests to clear VMOLR.ROPE/MPME*/
-#define E1000_VF_SET_PROMISC_MULTICAST    (0x02 << E1000_VT_MSGINFO_SHIFT)
-
-#define E1000_PF_CONTROL_MSG      0x0100 /* PF control message */
-
-s32 igb_read_mbx(struct e1000_hw *, u32 *, u16, u16);
-s32 igb_write_mbx(struct e1000_hw *, u32 *, u16, u16);
-s32 igb_check_for_msg(struct e1000_hw *, u16);
-s32 igb_check_for_ack(struct e1000_hw *, u16);
-s32 igb_check_for_rst(struct e1000_hw *, u16);
-s32 igb_init_mbx_params_pf(struct e1000_hw *);
+/* Msgs below or'd with this are the ACK */
+#define E1000_VT_MSGTYPE_ACK	0x80000000
+/* Msgs below or'd with this are the NACK */
+#define E1000_VT_MSGTYPE_NACK	0x40000000
+/* Indicates that VF is still clear to send requests */
+#define E1000_VT_MSGTYPE_CTS	0x20000000
+#define E1000_VT_MSGINFO_SHIFT	16
+/* bits 23:16 are used for extra info for certain messages */
+#define E1000_VT_MSGINFO_MASK	(0xFF << E1000_VT_MSGINFO_SHIFT)
+
+#define E1000_VF_RESET			0x01 /* VF requests reset */
+#define E1000_VF_SET_MAC_ADDR		0x02 /* VF requests to set MAC addr */
+#define E1000_VF_SET_MULTICAST		0x03 /* VF requests to set MC addr */
+#define E1000_VF_SET_MULTICAST_COUNT_MASK (0x1F << E1000_VT_MSGINFO_SHIFT)
+#define E1000_VF_SET_MULTICAST_OVERFLOW	(0x80 << E1000_VT_MSGINFO_SHIFT)
+#define E1000_VF_SET_VLAN		0x04 /* VF requests to set VLAN */
+#define E1000_VF_SET_VLAN_ADD		(0x01 << E1000_VT_MSGINFO_SHIFT)
+#define E1000_VF_SET_LPE		0x05 /* reqs to set VMOLR.LPE */
+#define E1000_VF_SET_PROMISC		0x06 /* reqs to clear VMOLR.ROPE/MPME*/
+#define E1000_VF_SET_PROMISC_UNICAST	(0x01 << E1000_VT_MSGINFO_SHIFT)
+#define E1000_VF_SET_PROMISC_MULTICAST	(0x02 << E1000_VT_MSGINFO_SHIFT)
+
+#define E1000_PF_CONTROL_MSG		0x0100 /* PF control message */
+
+#define E1000_VF_MBX_INIT_TIMEOUT	2000 /* number of retries on mailbox */
+#define E1000_VF_MBX_INIT_DELAY		500  /* microseconds between retries */
+
+s32 e1000_read_mbx(struct e1000_hw *, u32 *, u16, u16);
+s32 e1000_write_mbx(struct e1000_hw *, u32 *, u16, u16);
+s32 e1000_read_posted_mbx(struct e1000_hw *, u32 *, u16, u16);
+s32 e1000_write_posted_mbx(struct e1000_hw *, u32 *, u16, u16);
+s32 e1000_check_for_msg(struct e1000_hw *, u16);
+s32 e1000_check_for_ack(struct e1000_hw *, u16);
+s32 e1000_check_for_rst(struct e1000_hw *, u16);
+void e1000_init_mbx_ops_generic(struct e1000_hw *hw);
+s32 e1000_init_mbx_params_pf(struct e1000_hw *);
 
 #endif /* _E1000_MBX_H_ */
diff --git a/drivers/net/igb/e1000_nvm.c b/drivers/net/igb/e1000_nvm.c
index 40407124e722..b87f6d0ce711 100644
--- a/drivers/net/igb/e1000_nvm.c
+++ b/drivers/net/igb/e1000_nvm.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2007-2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -25,44 +25,110 @@
 
 *******************************************************************************/
 
-#include <linux/if_ether.h>
-#include <linux/delay.h>
+#include "e1000_api.h"
 
-#include "e1000_mac.h"
-#include "e1000_nvm.h"
+static void e1000_reload_nvm_generic(struct e1000_hw *hw);
 
 /**
- *  igb_raise_eec_clk - Raise EEPROM clock
+ *  e1000_init_nvm_ops_generic - Initialize NVM function pointers
+ *  @hw: pointer to the HW structure
+ *
+ *  Setups up the function pointers to no-op functions
+ **/
+void e1000_init_nvm_ops_generic(struct e1000_hw *hw)
+{
+	struct e1000_nvm_info *nvm = &hw->nvm;
+	DEBUGFUNC("e1000_init_nvm_ops_generic");
+
+	/* Initialize function pointers */
+	nvm->ops.init_params = e1000_null_ops_generic;
+	nvm->ops.acquire = e1000_null_ops_generic;
+	nvm->ops.read = e1000_null_read_nvm;
+	nvm->ops.release = e1000_null_nvm_generic;
+	nvm->ops.reload = e1000_reload_nvm_generic;
+	nvm->ops.update = e1000_null_ops_generic;
+	nvm->ops.valid_led_default = e1000_null_led_default;
+	nvm->ops.validate = e1000_null_ops_generic;
+	nvm->ops.write = e1000_null_write_nvm;
+}
+
+/**
+ *  e1000_null_nvm_read - No-op function, return 0
+ *  @hw: pointer to the HW structure
+ **/
+s32 e1000_null_read_nvm(struct e1000_hw E1000_UNUSEDARG *hw,
+			u16 E1000_UNUSEDARG a, u16 E1000_UNUSEDARG b,
+			u16 E1000_UNUSEDARG *c)
+{
+	DEBUGFUNC("e1000_null_read_nvm");
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_null_nvm_generic - No-op function, return void
+ *  @hw: pointer to the HW structure
+ **/
+void e1000_null_nvm_generic(struct e1000_hw E1000_UNUSEDARG *hw)
+{
+	DEBUGFUNC("e1000_null_nvm_generic");
+	return;
+}
+
+/**
+ *  e1000_null_led_default - No-op function, return 0
+ *  @hw: pointer to the HW structure
+ **/
+s32 e1000_null_led_default(struct e1000_hw E1000_UNUSEDARG *hw,
+			   u16 E1000_UNUSEDARG *data)
+{
+	DEBUGFUNC("e1000_null_led_default");
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_null_write_nvm - No-op function, return 0
+ *  @hw: pointer to the HW structure
+ **/
+s32 e1000_null_write_nvm(struct e1000_hw E1000_UNUSEDARG *hw,
+			 u16 E1000_UNUSEDARG a, u16 E1000_UNUSEDARG b,
+			 u16 E1000_UNUSEDARG *c)
+{
+	DEBUGFUNC("e1000_null_write_nvm");
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_raise_eec_clk - Raise EEPROM clock
  *  @hw: pointer to the HW structure
  *  @eecd: pointer to the EEPROM
  *
  *  Enable/Raise the EEPROM clock bit.
  **/
-static void igb_raise_eec_clk(struct e1000_hw *hw, u32 *eecd)
+static void e1000_raise_eec_clk(struct e1000_hw *hw, u32 *eecd)
 {
 	*eecd = *eecd | E1000_EECD_SK;
-	wr32(E1000_EECD, *eecd);
-	wrfl();
-	udelay(hw->nvm.delay_usec);
+	E1000_WRITE_REG(hw, E1000_EECD, *eecd);
+	E1000_WRITE_FLUSH(hw);
+	usec_delay(hw->nvm.delay_usec);
 }
 
 /**
- *  igb_lower_eec_clk - Lower EEPROM clock
+ *  e1000_lower_eec_clk - Lower EEPROM clock
  *  @hw: pointer to the HW structure
  *  @eecd: pointer to the EEPROM
  *
  *  Clear/Lower the EEPROM clock bit.
  **/
-static void igb_lower_eec_clk(struct e1000_hw *hw, u32 *eecd)
+static void e1000_lower_eec_clk(struct e1000_hw *hw, u32 *eecd)
 {
 	*eecd = *eecd & ~E1000_EECD_SK;
-	wr32(E1000_EECD, *eecd);
-	wrfl();
-	udelay(hw->nvm.delay_usec);
+	E1000_WRITE_REG(hw, E1000_EECD, *eecd);
+	E1000_WRITE_FLUSH(hw);
+	usec_delay(hw->nvm.delay_usec);
 }
 
 /**
- *  igb_shift_out_eec_bits - Shift data bits our to the EEPROM
+ *  e1000_shift_out_eec_bits - Shift data bits our to the EEPROM
  *  @hw: pointer to the HW structure
  *  @data: data to send to the EEPROM
  *  @count: number of bits to shift out
@@ -71,12 +137,14 @@ static void igb_lower_eec_clk(struct e1000_hw *hw, u32 *eecd)
  *  "data" parameter will be shifted out to the EEPROM one bit at a time.
  *  In order to do this, "data" must be broken down into bits.
  **/
-static void igb_shift_out_eec_bits(struct e1000_hw *hw, u16 data, u16 count)
+static void e1000_shift_out_eec_bits(struct e1000_hw *hw, u16 data, u16 count)
 {
 	struct e1000_nvm_info *nvm = &hw->nvm;
-	u32 eecd = rd32(E1000_EECD);
+	u32 eecd = E1000_READ_REG(hw, E1000_EECD);
 	u32 mask;
 
+	DEBUGFUNC("e1000_shift_out_eec_bits");
+
 	mask = 0x01 << (count - 1);
 	if (nvm->type == e1000_nvm_eeprom_spi)
 		eecd |= E1000_EECD_DO;
@@ -87,23 +155,23 @@ static void igb_shift_out_eec_bits(struct e1000_hw *hw, u16 data, u16 count)
 		if (data & mask)
 			eecd |= E1000_EECD_DI;
 
-		wr32(E1000_EECD, eecd);
-		wrfl();
+		E1000_WRITE_REG(hw, E1000_EECD, eecd);
+		E1000_WRITE_FLUSH(hw);
 
-		udelay(nvm->delay_usec);
+		usec_delay(nvm->delay_usec);
 
-		igb_raise_eec_clk(hw, &eecd);
-		igb_lower_eec_clk(hw, &eecd);
+		e1000_raise_eec_clk(hw, &eecd);
+		e1000_lower_eec_clk(hw, &eecd);
 
 		mask >>= 1;
 	} while (mask);
 
 	eecd &= ~E1000_EECD_DI;
-	wr32(E1000_EECD, eecd);
+	E1000_WRITE_REG(hw, E1000_EECD, eecd);
 }
 
 /**
- *  igb_shift_in_eec_bits - Shift data bits in from the EEPROM
+ *  e1000_shift_in_eec_bits - Shift data bits in from the EEPROM
  *  @hw: pointer to the HW structure
  *  @count: number of bits to shift in
  *
@@ -113,121 +181,124 @@ static void igb_shift_out_eec_bits(struct e1000_hw *hw, u16 data, u16 count)
  *  "DO" bit.  During this "shifting in" process the data in "DI" bit should
  *  always be clear.
  **/
-static u16 igb_shift_in_eec_bits(struct e1000_hw *hw, u16 count)
+static u16 e1000_shift_in_eec_bits(struct e1000_hw *hw, u16 count)
 {
 	u32 eecd;
 	u32 i;
 	u16 data;
 
-	eecd = rd32(E1000_EECD);
+	DEBUGFUNC("e1000_shift_in_eec_bits");
+
+	eecd = E1000_READ_REG(hw, E1000_EECD);
 
 	eecd &= ~(E1000_EECD_DO | E1000_EECD_DI);
 	data = 0;
 
 	for (i = 0; i < count; i++) {
 		data <<= 1;
-		igb_raise_eec_clk(hw, &eecd);
+		e1000_raise_eec_clk(hw, &eecd);
 
-		eecd = rd32(E1000_EECD);
+		eecd = E1000_READ_REG(hw, E1000_EECD);
 
 		eecd &= ~E1000_EECD_DI;
 		if (eecd & E1000_EECD_DO)
 			data |= 1;
 
-		igb_lower_eec_clk(hw, &eecd);
+		e1000_lower_eec_clk(hw, &eecd);
 	}
 
 	return data;
 }
 
 /**
- *  igb_poll_eerd_eewr_done - Poll for EEPROM read/write completion
+ *  e1000_poll_eerd_eewr_done - Poll for EEPROM read/write completion
  *  @hw: pointer to the HW structure
  *  @ee_reg: EEPROM flag for polling
  *
  *  Polls the EEPROM status bit for either read or write completion based
  *  upon the value of 'ee_reg'.
  **/
-static s32 igb_poll_eerd_eewr_done(struct e1000_hw *hw, int ee_reg)
+s32 e1000_poll_eerd_eewr_done(struct e1000_hw *hw, int ee_reg)
 {
 	u32 attempts = 100000;
 	u32 i, reg = 0;
-	s32 ret_val = -E1000_ERR_NVM;
+
+	DEBUGFUNC("e1000_poll_eerd_eewr_done");
 
 	for (i = 0; i < attempts; i++) {
 		if (ee_reg == E1000_NVM_POLL_READ)
-			reg = rd32(E1000_EERD);
+			reg = E1000_READ_REG(hw, E1000_EERD);
 		else
-			reg = rd32(E1000_EEWR);
+			reg = E1000_READ_REG(hw, E1000_EEWR);
 
-		if (reg & E1000_NVM_RW_REG_DONE) {
-			ret_val = 0;
-			break;
-		}
+		if (reg & E1000_NVM_RW_REG_DONE)
+			return E1000_SUCCESS;
 
-		udelay(5);
+		usec_delay(5);
 	}
 
-	return ret_val;
+	return -E1000_ERR_NVM;
 }
 
 /**
- *  igb_acquire_nvm - Generic request for access to EEPROM
+ *  e1000_acquire_nvm_generic - Generic request for access to EEPROM
  *  @hw: pointer to the HW structure
  *
  *  Set the EEPROM access request bit and wait for EEPROM access grant bit.
  *  Return successful if access grant bit set, else clear the request for
  *  EEPROM access and return -E1000_ERR_NVM (-1).
  **/
-s32 igb_acquire_nvm(struct e1000_hw *hw)
+s32 e1000_acquire_nvm_generic(struct e1000_hw *hw)
 {
-	u32 eecd = rd32(E1000_EECD);
+	u32 eecd = E1000_READ_REG(hw, E1000_EECD);
 	s32 timeout = E1000_NVM_GRANT_ATTEMPTS;
-	s32 ret_val = 0;
 
+	DEBUGFUNC("e1000_acquire_nvm_generic");
 
-	wr32(E1000_EECD, eecd | E1000_EECD_REQ);
-	eecd = rd32(E1000_EECD);
+	E1000_WRITE_REG(hw, E1000_EECD, eecd | E1000_EECD_REQ);
+	eecd = E1000_READ_REG(hw, E1000_EECD);
 
 	while (timeout) {
 		if (eecd & E1000_EECD_GNT)
 			break;
-		udelay(5);
-		eecd = rd32(E1000_EECD);
+		usec_delay(5);
+		eecd = E1000_READ_REG(hw, E1000_EECD);
 		timeout--;
 	}
 
 	if (!timeout) {
 		eecd &= ~E1000_EECD_REQ;
-		wr32(E1000_EECD, eecd);
-		hw_dbg("Could not acquire NVM grant\n");
-		ret_val = -E1000_ERR_NVM;
+		E1000_WRITE_REG(hw, E1000_EECD, eecd);
+		DEBUGOUT("Could not acquire NVM grant\n");
+		return -E1000_ERR_NVM;
 	}
 
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_standby_nvm - Return EEPROM to standby state
+ *  e1000_standby_nvm - Return EEPROM to standby state
  *  @hw: pointer to the HW structure
  *
  *  Return the EEPROM to a standby state.
  **/
-static void igb_standby_nvm(struct e1000_hw *hw)
+static void e1000_standby_nvm(struct e1000_hw *hw)
 {
 	struct e1000_nvm_info *nvm = &hw->nvm;
-	u32 eecd = rd32(E1000_EECD);
+	u32 eecd = E1000_READ_REG(hw, E1000_EECD);
+
+	DEBUGFUNC("e1000_standby_nvm");
 
 	if (nvm->type == e1000_nvm_eeprom_spi) {
 		/* Toggle CS to flush commands */
 		eecd |= E1000_EECD_CS;
-		wr32(E1000_EECD, eecd);
-		wrfl();
-		udelay(nvm->delay_usec);
+		E1000_WRITE_REG(hw, E1000_EECD, eecd);
+		E1000_WRITE_FLUSH(hw);
+		usec_delay(nvm->delay_usec);
 		eecd &= ~E1000_EECD_CS;
-		wr32(E1000_EECD, eecd);
-		wrfl();
-		udelay(nvm->delay_usec);
+		E1000_WRITE_REG(hw, E1000_EECD, eecd);
+		E1000_WRITE_FLUSH(hw);
+		usec_delay(nvm->delay_usec);
 	}
 }
 
@@ -241,85 +312,86 @@ static void e1000_stop_nvm(struct e1000_hw *hw)
 {
 	u32 eecd;
 
-	eecd = rd32(E1000_EECD);
+	DEBUGFUNC("e1000_stop_nvm");
+
+	eecd = E1000_READ_REG(hw, E1000_EECD);
 	if (hw->nvm.type == e1000_nvm_eeprom_spi) {
 		/* Pull CS high */
 		eecd |= E1000_EECD_CS;
-		igb_lower_eec_clk(hw, &eecd);
+		e1000_lower_eec_clk(hw, &eecd);
 	}
 }
 
 /**
- *  igb_release_nvm - Release exclusive access to EEPROM
+ *  e1000_release_nvm_generic - Release exclusive access to EEPROM
  *  @hw: pointer to the HW structure
  *
  *  Stop any current commands to the EEPROM and clear the EEPROM request bit.
  **/
-void igb_release_nvm(struct e1000_hw *hw)
+void e1000_release_nvm_generic(struct e1000_hw *hw)
 {
 	u32 eecd;
 
+	DEBUGFUNC("e1000_release_nvm_generic");
+
 	e1000_stop_nvm(hw);
 
-	eecd = rd32(E1000_EECD);
+	eecd = E1000_READ_REG(hw, E1000_EECD);
 	eecd &= ~E1000_EECD_REQ;
-	wr32(E1000_EECD, eecd);
+	E1000_WRITE_REG(hw, E1000_EECD, eecd);
 }
 
 /**
- *  igb_ready_nvm_eeprom - Prepares EEPROM for read/write
+ *  e1000_ready_nvm_eeprom - Prepares EEPROM for read/write
  *  @hw: pointer to the HW structure
  *
  *  Setups the EEPROM for reading and writing.
  **/
-static s32 igb_ready_nvm_eeprom(struct e1000_hw *hw)
+static s32 e1000_ready_nvm_eeprom(struct e1000_hw *hw)
 {
 	struct e1000_nvm_info *nvm = &hw->nvm;
-	u32 eecd = rd32(E1000_EECD);
-	s32 ret_val = 0;
-	u16 timeout = 0;
+	u32 eecd = E1000_READ_REG(hw, E1000_EECD);
 	u8 spi_stat_reg;
 
+	DEBUGFUNC("e1000_ready_nvm_eeprom");
 
 	if (nvm->type == e1000_nvm_eeprom_spi) {
+		u16 timeout = NVM_MAX_RETRY_SPI;
+
 		/* Clear SK and CS */
 		eecd &= ~(E1000_EECD_CS | E1000_EECD_SK);
-		wr32(E1000_EECD, eecd);
-		wrfl();
-		udelay(1);
-		timeout = NVM_MAX_RETRY_SPI;
+		E1000_WRITE_REG(hw, E1000_EECD, eecd);
+		E1000_WRITE_FLUSH(hw);
+		usec_delay(1);
 
-		/*
-		 * Read "Status Register" repeatedly until the LSB is cleared.
+		/* Read "Status Register" repeatedly until the LSB is cleared.
 		 * The EEPROM will signal that the command has been completed
 		 * by clearing bit 0 of the internal status register.  If it's
 		 * not cleared within 'timeout', then error out.
 		 */
 		while (timeout) {
-			igb_shift_out_eec_bits(hw, NVM_RDSR_OPCODE_SPI,
+			e1000_shift_out_eec_bits(hw, NVM_RDSR_OPCODE_SPI,
 						 hw->nvm.opcode_bits);
-			spi_stat_reg = (u8)igb_shift_in_eec_bits(hw, 8);
+			spi_stat_reg = (u8)e1000_shift_in_eec_bits(hw, 8);
 			if (!(spi_stat_reg & NVM_STATUS_RDY_SPI))
 				break;
 
-			udelay(5);
-			igb_standby_nvm(hw);
+			usec_delay(5);
+			e1000_standby_nvm(hw);
 			timeout--;
 		}
 
 		if (!timeout) {
-			hw_dbg("SPI NVM Status error\n");
-			ret_val = -E1000_ERR_NVM;
-			goto out;
+			DEBUGOUT("SPI NVM Status error\n");
+			return -E1000_ERR_NVM;
 		}
 	}
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_read_nvm_spi - Read EEPROM's using SPI
+ *  e1000_read_nvm_spi - Read EEPROM's using SPI
  *  @hw: pointer to the HW structure
  *  @offset: offset of word in the EEPROM to read
  *  @words: number of words to read
@@ -327,7 +399,7 @@ out:
  *
  *  Reads a 16 bit word from the EEPROM.
  **/
-s32 igb_read_nvm_spi(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
+s32 e1000_read_nvm_spi(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
 {
 	struct e1000_nvm_info *nvm = &hw->nvm;
 	u32 i = 0;
@@ -335,53 +407,51 @@ s32 igb_read_nvm_spi(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
 	u16 word_in;
 	u8 read_opcode = NVM_READ_OPCODE_SPI;
 
-	/*
-	 * A check for invalid values:  offset too large, too many words,
+	DEBUGFUNC("e1000_read_nvm_spi");
+
+	/* A check for invalid values:  offset too large, too many words,
 	 * and not enough words.
 	 */
 	if ((offset >= nvm->word_size) || (words > (nvm->word_size - offset)) ||
 	    (words == 0)) {
-		hw_dbg("nvm parameter(s) out of bounds\n");
-		ret_val = -E1000_ERR_NVM;
-		goto out;
+		DEBUGOUT("nvm parameter(s) out of bounds\n");
+		return -E1000_ERR_NVM;
 	}
 
 	ret_val = nvm->ops.acquire(hw);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	ret_val = igb_ready_nvm_eeprom(hw);
+	ret_val = e1000_ready_nvm_eeprom(hw);
 	if (ret_val)
 		goto release;
 
-	igb_standby_nvm(hw);
+	e1000_standby_nvm(hw);
 
 	if ((nvm->address_bits == 8) && (offset >= 128))
 		read_opcode |= NVM_A8_OPCODE_SPI;
 
 	/* Send the READ command (opcode + addr) */
-	igb_shift_out_eec_bits(hw, read_opcode, nvm->opcode_bits);
-	igb_shift_out_eec_bits(hw, (u16)(offset*2), nvm->address_bits);
+	e1000_shift_out_eec_bits(hw, read_opcode, nvm->opcode_bits);
+	e1000_shift_out_eec_bits(hw, (u16)(offset*2), nvm->address_bits);
 
-	/*
-	 * Read the data.  SPI NVMs increment the address with each byte
+	/* Read the data.  SPI NVMs increment the address with each byte
 	 * read and will roll over if reading beyond the end.  This allows
 	 * us to read the whole NVM from any offset
 	 */
 	for (i = 0; i < words; i++) {
-		word_in = igb_shift_in_eec_bits(hw, 16);
+		word_in = e1000_shift_in_eec_bits(hw, 16);
 		data[i] = (word_in >> 8) | (word_in << 8);
 	}
 
 release:
 	nvm->ops.release(hw);
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_read_nvm_eerd - Reads EEPROM using EERD register
+ *  e1000_read_nvm_eerd - Reads EEPROM using EERD register
  *  @hw: pointer to the HW structure
  *  @offset: offset of word in the EEPROM to read
  *  @words: number of words to read
@@ -389,42 +459,41 @@ out:
  *
  *  Reads a 16 bit word from the EEPROM using the EERD register.
  **/
-s32 igb_read_nvm_eerd(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
+s32 e1000_read_nvm_eerd(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
 {
 	struct e1000_nvm_info *nvm = &hw->nvm;
 	u32 i, eerd = 0;
-	s32 ret_val = 0;
+	s32 ret_val = E1000_SUCCESS;
 
-	/*
-	 * A check for invalid values:  offset too large, too many words,
-	 * and not enough words.
+	DEBUGFUNC("e1000_read_nvm_eerd");
+
+	/* A check for invalid values:  offset too large, too many words,
+	 * too many words for the offset, and not enough words.
 	 */
 	if ((offset >= nvm->word_size) || (words > (nvm->word_size - offset)) ||
 	    (words == 0)) {
-		hw_dbg("nvm parameter(s) out of bounds\n");
-		ret_val = -E1000_ERR_NVM;
-		goto out;
+		DEBUGOUT("nvm parameter(s) out of bounds\n");
+		return -E1000_ERR_NVM;
 	}
 
 	for (i = 0; i < words; i++) {
 		eerd = ((offset+i) << E1000_NVM_RW_ADDR_SHIFT) +
 		       E1000_NVM_RW_REG_START;
 
-		wr32(E1000_EERD, eerd);
-		ret_val = igb_poll_eerd_eewr_done(hw, E1000_NVM_POLL_READ);
+		E1000_WRITE_REG(hw, E1000_EERD, eerd);
+		ret_val = e1000_poll_eerd_eewr_done(hw, E1000_NVM_POLL_READ);
 		if (ret_val)
 			break;
 
-		data[i] = (rd32(E1000_EERD) >>
-			E1000_NVM_RW_REG_DATA);
+		data[i] = (E1000_READ_REG(hw, E1000_EERD) >>
+			   E1000_NVM_RW_REG_DATA);
 	}
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_write_nvm_spi - Write to EEPROM using SPI
+ *  e1000_write_nvm_spi - Write to EEPROM using SPI
  *  @hw: pointer to the HW structure
  *  @offset: offset within the EEPROM to be written to
  *  @words: number of words to write
@@ -433,208 +502,271 @@ out:
  *  Writes data to EEPROM at offset using SPI interface.
  *
  *  If e1000_update_nvm_checksum is not called after this function , the
- *  EEPROM will most likley contain an invalid checksum.
+ *  EEPROM will most likely contain an invalid checksum.
  **/
-s32 igb_write_nvm_spi(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
+s32 e1000_write_nvm_spi(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
 {
 	struct e1000_nvm_info *nvm = &hw->nvm;
-	s32 ret_val;
+	s32 ret_val = -E1000_ERR_NVM;
 	u16 widx = 0;
 
-	/*
-	 * A check for invalid values:  offset too large, too many words,
+	DEBUGFUNC("e1000_write_nvm_spi");
+
+	/* A check for invalid values:  offset too large, too many words,
 	 * and not enough words.
 	 */
 	if ((offset >= nvm->word_size) || (words > (nvm->word_size - offset)) ||
 	    (words == 0)) {
-		hw_dbg("nvm parameter(s) out of bounds\n");
-		ret_val = -E1000_ERR_NVM;
-		goto out;
+		DEBUGOUT("nvm parameter(s) out of bounds\n");
+		return -E1000_ERR_NVM;
 	}
 
-	ret_val = hw->nvm.ops.acquire(hw);
-	if (ret_val)
-		goto out;
-
-	msleep(10);
-
 	while (widx < words) {
 		u8 write_opcode = NVM_WRITE_OPCODE_SPI;
 
-		ret_val = igb_ready_nvm_eeprom(hw);
+		ret_val = nvm->ops.acquire(hw);
 		if (ret_val)
-			goto release;
+			return ret_val;
+
+		ret_val = e1000_ready_nvm_eeprom(hw);
+		if (ret_val) {
+			nvm->ops.release(hw);
+			return ret_val;
+		}
 
-		igb_standby_nvm(hw);
+		e1000_standby_nvm(hw);
 
 		/* Send the WRITE ENABLE command (8 bit opcode) */
-		igb_shift_out_eec_bits(hw, NVM_WREN_OPCODE_SPI,
+		e1000_shift_out_eec_bits(hw, NVM_WREN_OPCODE_SPI,
 					 nvm->opcode_bits);
 
-		igb_standby_nvm(hw);
+		e1000_standby_nvm(hw);
 
-		/*
-		 * Some SPI eeproms use the 8th address bit embedded in the
+		/* Some SPI eeproms use the 8th address bit embedded in the
 		 * opcode
 		 */
 		if ((nvm->address_bits == 8) && (offset >= 128))
 			write_opcode |= NVM_A8_OPCODE_SPI;
 
 		/* Send the Write command (8-bit opcode + addr) */
-		igb_shift_out_eec_bits(hw, write_opcode, nvm->opcode_bits);
-		igb_shift_out_eec_bits(hw, (u16)((offset + widx) * 2),
+		e1000_shift_out_eec_bits(hw, write_opcode, nvm->opcode_bits);
+		e1000_shift_out_eec_bits(hw, (u16)((offset + widx) * 2),
 					 nvm->address_bits);
 
 		/* Loop to allow for up to whole page write of eeprom */
 		while (widx < words) {
 			u16 word_out = data[widx];
 			word_out = (word_out >> 8) | (word_out << 8);
-			igb_shift_out_eec_bits(hw, word_out, 16);
+			e1000_shift_out_eec_bits(hw, word_out, 16);
 			widx++;
 
 			if ((((offset + widx) * 2) % nvm->page_size) == 0) {
-				igb_standby_nvm(hw);
+				e1000_standby_nvm(hw);
 				break;
 			}
 		}
+		msec_delay(10);
+		nvm->ops.release(hw);
 	}
 
-	msleep(10);
-release:
-	hw->nvm.ops.release(hw);
-
-out:
 	return ret_val;
 }
 
 /**
- *  igb_read_part_string - Read device part number
+ *  e1000_read_pba_string_generic - Read device part number
  *  @hw: pointer to the HW structure
- *  @part_num: pointer to device part number
- *  @part_num_size: size of part number buffer
+ *  @pba_num: pointer to device part number
+ *  @pba_num_size: size of part number buffer
  *
  *  Reads the product board assembly (PBA) number from the EEPROM and stores
- *  the value in part_num.
+ *  the value in pba_num.
  **/
-s32 igb_read_part_string(struct e1000_hw *hw, u8 *part_num, u32 part_num_size)
+s32 e1000_read_pba_string_generic(struct e1000_hw *hw, u8 *pba_num,
+				  u32 pba_num_size)
 {
 	s32 ret_val;
 	u16 nvm_data;
-	u16 pointer;
+	u16 pba_ptr;
 	u16 offset;
 	u16 length;
 
-	if (part_num == NULL) {
-		hw_dbg("PBA string buffer was null\n");
-		ret_val = E1000_ERR_INVALID_ARGUMENT;
-		goto out;
+	DEBUGFUNC("e1000_read_pba_string_generic");
+
+	if ((hw->mac.type >= e1000_i210) &&
+	    !e1000_get_flash_presence_i210(hw)) {
+		DEBUGOUT("Flashless no PBA string\n");
+		return -E1000_ERR_NVM_PBA_SECTION;
+	}
+
+	if (pba_num == NULL) {
+		DEBUGOUT("PBA string buffer was null\n");
+		return -E1000_ERR_INVALID_ARGUMENT;
 	}
 
 	ret_val = hw->nvm.ops.read(hw, NVM_PBA_OFFSET_0, 1, &nvm_data);
 	if (ret_val) {
-		hw_dbg("NVM Read Error\n");
-		goto out;
+		DEBUGOUT("NVM Read Error\n");
+		return ret_val;
 	}
 
-	ret_val = hw->nvm.ops.read(hw, NVM_PBA_OFFSET_1, 1, &pointer);
+	ret_val = hw->nvm.ops.read(hw, NVM_PBA_OFFSET_1, 1, &pba_ptr);
 	if (ret_val) {
-		hw_dbg("NVM Read Error\n");
-		goto out;
+		DEBUGOUT("NVM Read Error\n");
+		return ret_val;
 	}
 
-	/*
-	 * if nvm_data is not ptr guard the PBA must be in legacy format which
-	 * means pointer is actually our second data word for the PBA number
+	/* if nvm_data is not ptr guard the PBA must be in legacy format which
+	 * means pba_ptr is actually our second data word for the PBA number
 	 * and we can decode it into an ascii string
 	 */
 	if (nvm_data != NVM_PBA_PTR_GUARD) {
-		hw_dbg("NVM PBA number is not stored as string\n");
+		DEBUGOUT("NVM PBA number is not stored as string\n");
 
-		/* we will need 11 characters to store the PBA */
-		if (part_num_size < 11) {
-			hw_dbg("PBA string buffer too small\n");
+		/* make sure callers buffer is big enough to store the PBA */
+		if (pba_num_size < E1000_PBANUM_LENGTH) {
+			DEBUGOUT("PBA string buffer too small\n");
 			return E1000_ERR_NO_SPACE;
 		}
 
-		/* extract hex string from data and pointer */
-		part_num[0] = (nvm_data >> 12) & 0xF;
-		part_num[1] = (nvm_data >> 8) & 0xF;
-		part_num[2] = (nvm_data >> 4) & 0xF;
-		part_num[3] = nvm_data & 0xF;
-		part_num[4] = (pointer >> 12) & 0xF;
-		part_num[5] = (pointer >> 8) & 0xF;
-		part_num[6] = '-';
-		part_num[7] = 0;
-		part_num[8] = (pointer >> 4) & 0xF;
-		part_num[9] = pointer & 0xF;
+		/* extract hex string from data and pba_ptr */
+		pba_num[0] = (nvm_data >> 12) & 0xF;
+		pba_num[1] = (nvm_data >> 8) & 0xF;
+		pba_num[2] = (nvm_data >> 4) & 0xF;
+		pba_num[3] = nvm_data & 0xF;
+		pba_num[4] = (pba_ptr >> 12) & 0xF;
+		pba_num[5] = (pba_ptr >> 8) & 0xF;
+		pba_num[6] = '-';
+		pba_num[7] = 0;
+		pba_num[8] = (pba_ptr >> 4) & 0xF;
+		pba_num[9] = pba_ptr & 0xF;
 
 		/* put a null character on the end of our string */
-		part_num[10] = '\0';
+		pba_num[10] = '\0';
 
 		/* switch all the data but the '-' to hex char */
 		for (offset = 0; offset < 10; offset++) {
-			if (part_num[offset] < 0xA)
-				part_num[offset] += '0';
-			else if (part_num[offset] < 0x10)
-				part_num[offset] += 'A' - 0xA;
+			if (pba_num[offset] < 0xA)
+				pba_num[offset] += '0';
+			else if (pba_num[offset] < 0x10)
+				pba_num[offset] += 'A' - 0xA;
 		}
 
-		goto out;
+		return E1000_SUCCESS;
 	}
 
-	ret_val = hw->nvm.ops.read(hw, pointer, 1, &length);
+	ret_val = hw->nvm.ops.read(hw, pba_ptr, 1, &length);
 	if (ret_val) {
-		hw_dbg("NVM Read Error\n");
-		goto out;
+		DEBUGOUT("NVM Read Error\n");
+		return ret_val;
 	}
 
 	if (length == 0xFFFF || length == 0) {
-		hw_dbg("NVM PBA number section invalid length\n");
-		ret_val = E1000_ERR_NVM_PBA_SECTION;
-		goto out;
+		DEBUGOUT("NVM PBA number section invalid length\n");
+		return -E1000_ERR_NVM_PBA_SECTION;
 	}
-	/* check if part_num buffer is big enough */
-	if (part_num_size < (((u32)length * 2) - 1)) {
-		hw_dbg("PBA string buffer too small\n");
-		ret_val = E1000_ERR_NO_SPACE;
-		goto out;
+	/* check if pba_num buffer is big enough */
+	if (pba_num_size < (((u32)length * 2) - 1)) {
+		DEBUGOUT("PBA string buffer too small\n");
+		return -E1000_ERR_NO_SPACE;
 	}
 
 	/* trim pba length from start of string */
-	pointer++;
+	pba_ptr++;
 	length--;
 
 	for (offset = 0; offset < length; offset++) {
-		ret_val = hw->nvm.ops.read(hw, pointer + offset, 1, &nvm_data);
+		ret_val = hw->nvm.ops.read(hw, pba_ptr + offset, 1, &nvm_data);
 		if (ret_val) {
-			hw_dbg("NVM Read Error\n");
-			goto out;
+			DEBUGOUT("NVM Read Error\n");
+			return ret_val;
 		}
-		part_num[offset * 2] = (u8)(nvm_data >> 8);
-		part_num[(offset * 2) + 1] = (u8)(nvm_data & 0xFF);
+		pba_num[offset * 2] = (u8)(nvm_data >> 8);
+		pba_num[(offset * 2) + 1] = (u8)(nvm_data & 0xFF);
 	}
-	part_num[offset * 2] = '\0';
+	pba_num[offset * 2] = '\0';
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_read_pba_length_generic - Read device part number length
+ *  @hw: pointer to the HW structure
+ *  @pba_num_size: size of part number buffer
+ *
+ *  Reads the product board assembly (PBA) number length from the EEPROM and
+ *  stores the value in pba_num_size.
+ **/
+s32 e1000_read_pba_length_generic(struct e1000_hw *hw, u32 *pba_num_size)
+{
+	s32 ret_val;
+	u16 nvm_data;
+	u16 pba_ptr;
+	u16 length;
+
+	DEBUGFUNC("e1000_read_pba_length_generic");
+
+	if (pba_num_size == NULL) {
+		DEBUGOUT("PBA buffer size was null\n");
+		return -E1000_ERR_INVALID_ARGUMENT;
+	}
+
+	ret_val = hw->nvm.ops.read(hw, NVM_PBA_OFFSET_0, 1, &nvm_data);
+	if (ret_val) {
+		DEBUGOUT("NVM Read Error\n");
+		return ret_val;
+	}
+
+	ret_val = hw->nvm.ops.read(hw, NVM_PBA_OFFSET_1, 1, &pba_ptr);
+	if (ret_val) {
+		DEBUGOUT("NVM Read Error\n");
+		return ret_val;
+	}
+
+	 /* if data is not ptr guard the PBA must be in legacy format */
+	if (nvm_data != NVM_PBA_PTR_GUARD) {
+		*pba_num_size = E1000_PBANUM_LENGTH;
+		return E1000_SUCCESS;
+	}
+
+	ret_val = hw->nvm.ops.read(hw, pba_ptr, 1, &length);
+	if (ret_val) {
+		DEBUGOUT("NVM Read Error\n");
+		return ret_val;
+	}
+
+	if (length == 0xFFFF || length == 0) {
+		DEBUGOUT("NVM PBA number section invalid length\n");
+		return -E1000_ERR_NVM_PBA_SECTION;
+	}
+
+	/* Convert from length in u16 values to u8 chars, add 1 for NULL,
+	 * and subtract 2 because length field is included in length.
+	 */
+	*pba_num_size = ((u32)length * 2) - 1;
+
+	return E1000_SUCCESS;
 }
 
+
+
+
+
 /**
- *  igb_read_mac_addr - Read device MAC address
+ *  e1000_read_mac_addr_generic - Read device MAC address
  *  @hw: pointer to the HW structure
  *
  *  Reads the device MAC address from the EEPROM and stores the value.
  *  Since devices with two ports use the same EEPROM, we increment the
  *  last bit in the MAC address for the second port.
  **/
-s32 igb_read_mac_addr(struct e1000_hw *hw)
+s32 e1000_read_mac_addr_generic(struct e1000_hw *hw)
 {
 	u32 rar_high;
 	u32 rar_low;
 	u16 i;
 
-	rar_high = rd32(E1000_RAH(0));
-	rar_low = rd32(E1000_RAL(0));
+	rar_high = E1000_READ_REG(hw, E1000_RAH(0));
+	rar_low = E1000_READ_REG(hw, E1000_RAL(0));
 
 	for (i = 0; i < E1000_RAL_MAC_ADDR_LEN; i++)
 		hw->mac.perm_addr[i] = (u8)(rar_low >> (i*8));
@@ -642,72 +774,201 @@ s32 igb_read_mac_addr(struct e1000_hw *hw)
 	for (i = 0; i < E1000_RAH_MAC_ADDR_LEN; i++)
 		hw->mac.perm_addr[i+4] = (u8)(rar_high >> (i*8));
 
-	for (i = 0; i < ETH_ALEN; i++)
+	for (i = 0; i < ETH_ADDR_LEN; i++)
 		hw->mac.addr[i] = hw->mac.perm_addr[i];
 
-	return 0;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_validate_nvm_checksum - Validate EEPROM checksum
+ *  e1000_validate_nvm_checksum_generic - Validate EEPROM checksum
  *  @hw: pointer to the HW structure
  *
  *  Calculates the EEPROM checksum by reading/adding each word of the EEPROM
  *  and then verifies that the sum of the EEPROM is equal to 0xBABA.
  **/
-s32 igb_validate_nvm_checksum(struct e1000_hw *hw)
+s32 e1000_validate_nvm_checksum_generic(struct e1000_hw *hw)
 {
-	s32 ret_val = 0;
+	s32 ret_val;
 	u16 checksum = 0;
 	u16 i, nvm_data;
 
+	DEBUGFUNC("e1000_validate_nvm_checksum_generic");
+
 	for (i = 0; i < (NVM_CHECKSUM_REG + 1); i++) {
 		ret_val = hw->nvm.ops.read(hw, i, 1, &nvm_data);
 		if (ret_val) {
-			hw_dbg("NVM Read Error\n");
-			goto out;
+			DEBUGOUT("NVM Read Error\n");
+			return ret_val;
 		}
 		checksum += nvm_data;
 	}
 
 	if (checksum != (u16) NVM_SUM) {
-		hw_dbg("NVM Checksum Invalid\n");
-		ret_val = -E1000_ERR_NVM;
-		goto out;
+		DEBUGOUT("NVM Checksum Invalid\n");
+		return -E1000_ERR_NVM;
 	}
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_update_nvm_checksum - Update EEPROM checksum
+ *  e1000_update_nvm_checksum_generic - Update EEPROM checksum
  *  @hw: pointer to the HW structure
  *
  *  Updates the EEPROM checksum by reading/adding each word of the EEPROM
  *  up to the checksum.  Then calculates the EEPROM checksum and writes the
  *  value to the EEPROM.
  **/
-s32 igb_update_nvm_checksum(struct e1000_hw *hw)
+s32 e1000_update_nvm_checksum_generic(struct e1000_hw *hw)
 {
-	s32  ret_val;
+	s32 ret_val;
 	u16 checksum = 0;
 	u16 i, nvm_data;
 
+	DEBUGFUNC("e1000_update_nvm_checksum");
+
 	for (i = 0; i < NVM_CHECKSUM_REG; i++) {
 		ret_val = hw->nvm.ops.read(hw, i, 1, &nvm_data);
 		if (ret_val) {
-			hw_dbg("NVM Read Error while updating checksum.\n");
-			goto out;
+			DEBUGOUT("NVM Read Error while updating checksum.\n");
+			return ret_val;
 		}
 		checksum += nvm_data;
 	}
 	checksum = (u16) NVM_SUM - checksum;
 	ret_val = hw->nvm.ops.write(hw, NVM_CHECKSUM_REG, 1, &checksum);
 	if (ret_val)
-		hw_dbg("NVM Write Error while updating checksum.\n");
+		DEBUGOUT("NVM Write Error while updating checksum.\n");
 
-out:
 	return ret_val;
 }
 
+/**
+ *  e1000_reload_nvm_generic - Reloads EEPROM
+ *  @hw: pointer to the HW structure
+ *
+ *  Reloads the EEPROM by setting the "Reinitialize from EEPROM" bit in the
+ *  extended control register.
+ **/
+static void e1000_reload_nvm_generic(struct e1000_hw *hw)
+{
+	u32 ctrl_ext;
+
+	DEBUGFUNC("e1000_reload_nvm_generic");
+
+	usec_delay(10);
+	ctrl_ext = E1000_READ_REG(hw, E1000_CTRL_EXT);
+	ctrl_ext |= E1000_CTRL_EXT_EE_RST;
+	E1000_WRITE_REG(hw, E1000_CTRL_EXT, ctrl_ext);
+	E1000_WRITE_FLUSH(hw);
+}
+
+/**
+ *  e1000_get_fw_version - Get firmware version information
+ *  @hw: pointer to the HW structure
+ *  @fw_vers: pointer to output version structure
+ *
+ *  unsupported/not present features return 0 in version structure
+ **/
+void e1000_get_fw_version(struct e1000_hw *hw, struct e1000_fw_version *fw_vers)
+{
+	u16 eeprom_verh, eeprom_verl, etrack_test, fw_version;
+	u8 q, hval, rem, result;
+	u16 comb_verh, comb_verl, comb_offset;
+
+	memset(fw_vers, 0, sizeof(struct e1000_fw_version));
+
+	/* basic eeprom version numbers, bits used vary by part and by tool
+	 * used to create the nvm images */
+	/* Check which data format we have */
+	switch (hw->mac.type) {
+	case e1000_i211:
+		e1000_read_invm_version(hw, fw_vers);
+		return;
+	case e1000_82575:
+	case e1000_82576:
+	case e1000_82580:
+		hw->nvm.ops.read(hw, NVM_ETRACK_HIWORD, 1, &etrack_test);
+		/* Use this format, unless EETRACK ID exists,
+		 * then use alternate format
+		 */
+		if ((etrack_test &  NVM_MAJOR_MASK) != NVM_ETRACK_VALID) {
+			hw->nvm.ops.read(hw, NVM_VERSION, 1, &fw_version);
+			fw_vers->eep_major = (fw_version & NVM_MAJOR_MASK)
+					      >> NVM_MAJOR_SHIFT;
+			fw_vers->eep_minor = (fw_version & NVM_MINOR_MASK)
+					      >> NVM_MINOR_SHIFT;
+			fw_vers->eep_build = (fw_version & NVM_IMAGE_ID_MASK);
+			goto etrack_id;
+		}
+		break;
+	case e1000_i210:
+		if (!(e1000_get_flash_presence_i210(hw))) {
+			e1000_read_invm_version(hw, fw_vers);
+			return;
+		}
+		/* fall through */
+	case e1000_i350:
+		hw->nvm.ops.read(hw, NVM_ETRACK_HIWORD, 1, &etrack_test);
+		/* find combo image version */
+		hw->nvm.ops.read(hw, NVM_COMB_VER_PTR, 1, &comb_offset);
+		if ((comb_offset != 0x0) &&
+		    (comb_offset != NVM_VER_INVALID)) {
+
+			hw->nvm.ops.read(hw, (NVM_COMB_VER_OFF + comb_offset
+					 + 1), 1, &comb_verh);
+			hw->nvm.ops.read(hw, (NVM_COMB_VER_OFF + comb_offset),
+					 1, &comb_verl);
+
+			/* get Option Rom version if it exists and is valid */
+			if ((comb_verh && comb_verl) &&
+			    ((comb_verh != NVM_VER_INVALID) &&
+			     (comb_verl != NVM_VER_INVALID))) {
+
+				fw_vers->or_valid = true;
+				fw_vers->or_major =
+					comb_verl >> NVM_COMB_VER_SHFT;
+				fw_vers->or_build =
+					(comb_verl << NVM_COMB_VER_SHFT)
+					| (comb_verh >> NVM_COMB_VER_SHFT);
+				fw_vers->or_patch =
+					comb_verh & NVM_COMB_VER_MASK;
+			}
+		}
+		break;
+	default:
+		hw->nvm.ops.read(hw, NVM_ETRACK_HIWORD, 1, &etrack_test);
+		return;
+	}
+	hw->nvm.ops.read(hw, NVM_VERSION, 1, &fw_version);
+	fw_vers->eep_major = (fw_version & NVM_MAJOR_MASK)
+			      >> NVM_MAJOR_SHIFT;
+
+	/* check for old style version format in newer images*/
+	if ((fw_version & NVM_NEW_DEC_MASK) == 0x0) {
+		eeprom_verl = (fw_version & NVM_COMB_VER_MASK);
+	} else {
+		eeprom_verl = (fw_version & NVM_MINOR_MASK)
+				>> NVM_MINOR_SHIFT;
+	}
+	/* Convert minor value to hex before assigning to output struct
+	 * Val to be converted will not be higher than 99, per tool output
+	 */
+	q = eeprom_verl / NVM_HEX_CONV;
+	hval = q * NVM_HEX_TENS;
+	rem = eeprom_verl % NVM_HEX_CONV;
+	result = hval + rem;
+	fw_vers->eep_minor = result;
+
+etrack_id:
+	if ((etrack_test &  NVM_MAJOR_MASK) == NVM_ETRACK_VALID) {
+		hw->nvm.ops.read(hw, NVM_ETRACK_WORD, 1, &eeprom_verl);
+		hw->nvm.ops.read(hw, (NVM_ETRACK_WORD + 1), 1, &eeprom_verh);
+		fw_vers->etrack_id = (eeprom_verh << NVM_ETRACK_SHIFT)
+			| eeprom_verl;
+	}
+	return;
+}
+
+
diff --git a/drivers/net/igb/e1000_nvm.h b/drivers/net/igb/e1000_nvm.h
index a2a7ca9fa733..fe62785a04e6 100644
--- a/drivers/net/igb/e1000_nvm.h
+++ b/drivers/net/igb/e1000_nvm.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -28,16 +28,48 @@
 #ifndef _E1000_NVM_H_
 #define _E1000_NVM_H_
 
-s32  igb_acquire_nvm(struct e1000_hw *hw);
-void igb_release_nvm(struct e1000_hw *hw);
-s32  igb_read_mac_addr(struct e1000_hw *hw);
-s32  igb_read_part_num(struct e1000_hw *hw, u32 *part_num);
-s32  igb_read_part_string(struct e1000_hw *hw, u8 *part_num,
-                          u32 part_num_size);
-s32  igb_read_nvm_eerd(struct e1000_hw *hw, u16 offset, u16 words, u16 *data);
-s32  igb_read_nvm_spi(struct e1000_hw *hw, u16 offset, u16 words, u16 *data);
-s32  igb_write_nvm_spi(struct e1000_hw *hw, u16 offset, u16 words, u16 *data);
-s32  igb_validate_nvm_checksum(struct e1000_hw *hw);
-s32  igb_update_nvm_checksum(struct e1000_hw *hw);
+
+struct e1000_fw_version {
+	u32 etrack_id;
+	u16 eep_major;
+	u16 eep_minor;
+	u16 eep_build;
+
+	u8 invm_major;
+	u8 invm_minor;
+	u8 invm_img_type;
+
+	bool or_valid;
+	u16 or_major;
+	u16 or_build;
+	u16 or_patch;
+};
+
+
+void e1000_init_nvm_ops_generic(struct e1000_hw *hw);
+s32  e1000_null_read_nvm(struct e1000_hw *hw, u16 a, u16 b, u16 *c);
+void e1000_null_nvm_generic(struct e1000_hw *hw);
+s32  e1000_null_led_default(struct e1000_hw *hw, u16 *data);
+s32  e1000_null_write_nvm(struct e1000_hw *hw, u16 a, u16 b, u16 *c);
+s32  e1000_acquire_nvm_generic(struct e1000_hw *hw);
+
+s32  e1000_poll_eerd_eewr_done(struct e1000_hw *hw, int ee_reg);
+s32  e1000_read_mac_addr_generic(struct e1000_hw *hw);
+s32  e1000_read_pba_string_generic(struct e1000_hw *hw, u8 *pba_num,
+				   u32 pba_num_size);
+s32  e1000_read_pba_length_generic(struct e1000_hw *hw, u32 *pba_num_size);
+s32  e1000_read_nvm_spi(struct e1000_hw *hw, u16 offset, u16 words, u16 *data);
+s32  e1000_read_nvm_eerd(struct e1000_hw *hw, u16 offset, u16 words,
+			 u16 *data);
+s32  e1000_valid_led_default_generic(struct e1000_hw *hw, u16 *data);
+s32  e1000_validate_nvm_checksum_generic(struct e1000_hw *hw);
+s32  e1000_write_nvm_spi(struct e1000_hw *hw, u16 offset, u16 words,
+			 u16 *data);
+s32  e1000_update_nvm_checksum_generic(struct e1000_hw *hw);
+void e1000_release_nvm_generic(struct e1000_hw *hw);
+void e1000_get_fw_version(struct e1000_hw *hw,
+			  struct e1000_fw_version *fw_vers);
+
+#define E1000_STM_OPCODE	0xDB00
 
 #endif
diff --git a/drivers/net/igb/e1000_osdep.h b/drivers/net/igb/e1000_osdep.h
new file mode 100644
index 000000000000..70f5bd8c6d5b
--- /dev/null
+++ b/drivers/net/igb/e1000_osdep.h
@@ -0,0 +1,132 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+
+/* glue for the OS independent part of e1000
+ * includes register access macros
+ */
+
+#ifndef _E1000_OSDEP_H_
+#define _E1000_OSDEP_H_
+
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/if_ether.h>
+#include <linux/sched.h>
+#include "kcompat.h"
+
+#define usec_delay(x) udelay(x)
+#define usec_delay_irq(x) udelay(x)
+#ifndef msec_delay
+#define msec_delay(x) do { \
+	/* Don't mdelay in interrupt context! */ \
+	if (in_interrupt()) \
+		BUG(); \
+	else \
+		msleep(x); \
+} while (0)
+
+/* Some workarounds require millisecond delays and are run during interrupt
+ * context.  Most notably, when establishing link, the phy may need tweaking
+ * but cannot process phy register reads/writes faster than millisecond
+ * intervals...and we establish link due to a "link status change" interrupt.
+ */
+#define msec_delay_irq(x) mdelay(x)
+#endif
+
+#define PCI_COMMAND_REGISTER   PCI_COMMAND
+#define CMD_MEM_WRT_INVALIDATE PCI_COMMAND_INVALIDATE
+#define ETH_ADDR_LEN           ETH_ALEN
+
+#ifdef __BIG_ENDIAN
+#define E1000_BIG_ENDIAN __BIG_ENDIAN
+#endif
+
+
+#ifdef DEBUG
+#define DEBUGOUT(S) printk(KERN_DEBUG S)
+#define DEBUGOUT1(S, A...) printk(KERN_DEBUG S, ## A)
+#else
+#define DEBUGOUT(S)
+#define DEBUGOUT1(S, A...)
+#endif
+
+#ifdef DEBUG_FUNC
+#define DEBUGFUNC(F) DEBUGOUT(F "\n")
+#else
+#define DEBUGFUNC(F)
+#endif
+#define DEBUGOUT2 DEBUGOUT1
+#define DEBUGOUT3 DEBUGOUT2
+#define DEBUGOUT7 DEBUGOUT3
+
+#define E1000_REGISTER(a, reg) reg
+
+#define E1000_WRITE_REG(a, reg, value) ( \
+    writel((value), ((a)->hw_addr + E1000_REGISTER(a, reg))))
+
+#define E1000_READ_REG(a, reg) (readl((a)->hw_addr + E1000_REGISTER(a, reg)))
+
+#define E1000_WRITE_REG_ARRAY(a, reg, offset, value) ( \
+    writel((value), ((a)->hw_addr + E1000_REGISTER(a, reg) + ((offset) << 2))))
+
+#define E1000_READ_REG_ARRAY(a, reg, offset) ( \
+    readl((a)->hw_addr + E1000_REGISTER(a, reg) + ((offset) << 2)))
+
+#define E1000_READ_REG_ARRAY_DWORD E1000_READ_REG_ARRAY
+#define E1000_WRITE_REG_ARRAY_DWORD E1000_WRITE_REG_ARRAY
+
+#define E1000_WRITE_REG_ARRAY_WORD(a, reg, offset, value) ( \
+    writew((value), ((a)->hw_addr + E1000_REGISTER(a, reg) + ((offset) << 1))))
+
+#define E1000_READ_REG_ARRAY_WORD(a, reg, offset) ( \
+    readw((a)->hw_addr + E1000_REGISTER(a, reg) + ((offset) << 1)))
+
+#define E1000_WRITE_REG_ARRAY_BYTE(a, reg, offset, value) ( \
+    writeb((value), ((a)->hw_addr + E1000_REGISTER(a, reg) + (offset))))
+
+#define E1000_READ_REG_ARRAY_BYTE(a, reg, offset) ( \
+    readb((a)->hw_addr + E1000_REGISTER(a, reg) + (offset)))
+
+#define E1000_WRITE_REG_IO(a, reg, offset) do { \
+    outl(reg, ((a)->io_base));                  \
+    outl(offset, ((a)->io_base + 4));      } while (0)
+
+#define E1000_WRITE_FLUSH(a) E1000_READ_REG(a, E1000_STATUS)
+
+#define E1000_WRITE_FLASH_REG(a, reg, value) ( \
+    writel((value), ((a)->flash_address + reg)))
+
+#define E1000_WRITE_FLASH_REG16(a, reg, value) ( \
+    writew((value), ((a)->flash_address + reg)))
+
+#define E1000_READ_FLASH_REG(a, reg) (readl((a)->flash_address + reg))
+
+#define E1000_READ_FLASH_REG16(a, reg) (readw((a)->flash_address + reg))
+
+#endif /* _E1000_OSDEP_H_ */
diff --git a/drivers/net/igb/e1000_phy.c b/drivers/net/igb/e1000_phy.c
index e662554c62d6..fc5ef7d186ba 100644
--- a/drivers/net/igb/e1000_phy.c
+++ b/drivers/net/igb/e1000_phy.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2007-2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -25,131 +25,254 @@
 
 *******************************************************************************/
 
-#include <linux/if_ether.h>
-#include <linux/delay.h>
-
-#include "e1000_mac.h"
-#include "e1000_phy.h"
-
-static s32  igb_phy_setup_autoneg(struct e1000_hw *hw);
-static void igb_phy_force_speed_duplex_setup(struct e1000_hw *hw,
-					       u16 *phy_ctrl);
-static s32  igb_wait_autoneg(struct e1000_hw *hw);
+#include "e1000_api.h"
 
+static s32 e1000_wait_autoneg(struct e1000_hw *hw);
 /* Cable length tables */
-static const u16 e1000_m88_cable_length_table[] =
-	{ 0, 50, 80, 110, 140, 140, E1000_CABLE_LENGTH_UNDEFINED };
+static const u16 e1000_m88_cable_length_table[] = {
+	0, 50, 80, 110, 140, 140, E1000_CABLE_LENGTH_UNDEFINED };
 #define M88E1000_CABLE_LENGTH_TABLE_SIZE \
-                (sizeof(e1000_m88_cable_length_table) / \
-                 sizeof(e1000_m88_cable_length_table[0]))
-
-static const u16 e1000_igp_2_cable_length_table[] =
-    { 0, 0, 0, 0, 0, 0, 0, 0, 3, 5, 8, 11, 13, 16, 18, 21,
-      0, 0, 0, 3, 6, 10, 13, 16, 19, 23, 26, 29, 32, 35, 38, 41,
-      6, 10, 14, 18, 22, 26, 30, 33, 37, 41, 44, 48, 51, 54, 58, 61,
-      21, 26, 31, 35, 40, 44, 49, 53, 57, 61, 65, 68, 72, 75, 79, 82,
-      40, 45, 51, 56, 61, 66, 70, 75, 79, 83, 87, 91, 94, 98, 101, 104,
-      60, 66, 72, 77, 82, 87, 92, 96, 100, 104, 108, 111, 114, 117, 119, 121,
-      83, 89, 95, 100, 105, 109, 113, 116, 119, 122, 124,
-      104, 109, 114, 118, 121, 124};
+		(sizeof(e1000_m88_cable_length_table) / \
+		 sizeof(e1000_m88_cable_length_table[0]))
+
+static const u16 e1000_igp_2_cable_length_table[] = {
+	0, 0, 0, 0, 0, 0, 0, 0, 3, 5, 8, 11, 13, 16, 18, 21, 0, 0, 0, 3,
+	6, 10, 13, 16, 19, 23, 26, 29, 32, 35, 38, 41, 6, 10, 14, 18, 22,
+	26, 30, 33, 37, 41, 44, 48, 51, 54, 58, 61, 21, 26, 31, 35, 40,
+	44, 49, 53, 57, 61, 65, 68, 72, 75, 79, 82, 40, 45, 51, 56, 61,
+	66, 70, 75, 79, 83, 87, 91, 94, 98, 101, 104, 60, 66, 72, 77, 82,
+	87, 92, 96, 100, 104, 108, 111, 114, 117, 119, 121, 83, 89, 95,
+	100, 105, 109, 113, 116, 119, 122, 124, 104, 109, 114, 118, 121,
+	124};
 #define IGP02E1000_CABLE_LENGTH_TABLE_SIZE \
 		(sizeof(e1000_igp_2_cable_length_table) / \
 		 sizeof(e1000_igp_2_cable_length_table[0]))
 
 /**
- *  igb_check_reset_block - Check if PHY reset is blocked
+ *  e1000_init_phy_ops_generic - Initialize PHY function pointers
+ *  @hw: pointer to the HW structure
+ *
+ *  Setups up the function pointers to no-op functions
+ **/
+void e1000_init_phy_ops_generic(struct e1000_hw *hw)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	DEBUGFUNC("e1000_init_phy_ops_generic");
+
+	/* Initialize function pointers */
+	phy->ops.init_params = e1000_null_ops_generic;
+	phy->ops.acquire = e1000_null_ops_generic;
+	phy->ops.check_polarity = e1000_null_ops_generic;
+	phy->ops.check_reset_block = e1000_null_ops_generic;
+	phy->ops.commit = e1000_null_ops_generic;
+	phy->ops.force_speed_duplex = e1000_null_ops_generic;
+	phy->ops.get_cfg_done = e1000_null_ops_generic;
+	phy->ops.get_cable_length = e1000_null_ops_generic;
+	phy->ops.get_info = e1000_null_ops_generic;
+	phy->ops.set_page = e1000_null_set_page;
+	phy->ops.read_reg = e1000_null_read_reg;
+	phy->ops.read_reg_locked = e1000_null_read_reg;
+	phy->ops.read_reg_page = e1000_null_read_reg;
+	phy->ops.release = e1000_null_phy_generic;
+	phy->ops.reset = e1000_null_ops_generic;
+	phy->ops.set_d0_lplu_state = e1000_null_lplu_state;
+	phy->ops.set_d3_lplu_state = e1000_null_lplu_state;
+	phy->ops.write_reg = e1000_null_write_reg;
+	phy->ops.write_reg_locked = e1000_null_write_reg;
+	phy->ops.write_reg_page = e1000_null_write_reg;
+	phy->ops.power_up = e1000_null_phy_generic;
+	phy->ops.power_down = e1000_null_phy_generic;
+	phy->ops.read_i2c_byte = e1000_read_i2c_byte_null;
+	phy->ops.write_i2c_byte = e1000_write_i2c_byte_null;
+}
+
+/**
+ *  e1000_null_set_page - No-op function, return 0
+ *  @hw: pointer to the HW structure
+ **/
+s32 e1000_null_set_page(struct e1000_hw E1000_UNUSEDARG *hw,
+			u16 E1000_UNUSEDARG data)
+{
+	DEBUGFUNC("e1000_null_set_page");
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_null_read_reg - No-op function, return 0
+ *  @hw: pointer to the HW structure
+ **/
+s32 e1000_null_read_reg(struct e1000_hw E1000_UNUSEDARG *hw,
+			u32 E1000_UNUSEDARG offset, u16 E1000_UNUSEDARG *data)
+{
+	DEBUGFUNC("e1000_null_read_reg");
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_null_phy_generic - No-op function, return void
+ *  @hw: pointer to the HW structure
+ **/
+void e1000_null_phy_generic(struct e1000_hw E1000_UNUSEDARG *hw)
+{
+	DEBUGFUNC("e1000_null_phy_generic");
+	return;
+}
+
+/**
+ *  e1000_null_lplu_state - No-op function, return 0
+ *  @hw: pointer to the HW structure
+ **/
+s32 e1000_null_lplu_state(struct e1000_hw E1000_UNUSEDARG *hw,
+			  bool E1000_UNUSEDARG active)
+{
+	DEBUGFUNC("e1000_null_lplu_state");
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_null_write_reg - No-op function, return 0
+ *  @hw: pointer to the HW structure
+ **/
+s32 e1000_null_write_reg(struct e1000_hw E1000_UNUSEDARG *hw,
+			 u32 E1000_UNUSEDARG offset, u16 E1000_UNUSEDARG data)
+{
+	DEBUGFUNC("e1000_null_write_reg");
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_read_i2c_byte_null - No-op function, return 0
+ *  @hw: pointer to hardware structure
+ *  @byte_offset: byte offset to write
+ *  @dev_addr: device address
+ *  @data: data value read
+ *
+ **/
+s32 e1000_read_i2c_byte_null(struct e1000_hw E1000_UNUSEDARG *hw,
+			     u8 E1000_UNUSEDARG byte_offset,
+			     u8 E1000_UNUSEDARG dev_addr,
+			     u8 E1000_UNUSEDARG *data)
+{
+	DEBUGFUNC("e1000_read_i2c_byte_null");
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_write_i2c_byte_null - No-op function, return 0
+ *  @hw: pointer to hardware structure
+ *  @byte_offset: byte offset to write
+ *  @dev_addr: device address
+ *  @data: data value to write
+ *
+ **/
+s32 e1000_write_i2c_byte_null(struct e1000_hw E1000_UNUSEDARG *hw,
+			      u8 E1000_UNUSEDARG byte_offset,
+			      u8 E1000_UNUSEDARG dev_addr,
+			      u8 E1000_UNUSEDARG data)
+{
+	DEBUGFUNC("e1000_write_i2c_byte_null");
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_check_reset_block_generic - Check if PHY reset is blocked
  *  @hw: pointer to the HW structure
  *
  *  Read the PHY management control register and check whether a PHY reset
- *  is blocked.  If a reset is not blocked return 0, otherwise
+ *  is blocked.  If a reset is not blocked return E1000_SUCCESS, otherwise
  *  return E1000_BLK_PHY_RESET (12).
  **/
-s32 igb_check_reset_block(struct e1000_hw *hw)
+s32 e1000_check_reset_block_generic(struct e1000_hw *hw)
 {
 	u32 manc;
 
-	manc = rd32(E1000_MANC);
+	DEBUGFUNC("e1000_check_reset_block");
+
+	manc = E1000_READ_REG(hw, E1000_MANC);
 
 	return (manc & E1000_MANC_BLK_PHY_RST_ON_IDE) ?
-	       E1000_BLK_PHY_RESET : 0;
+	       E1000_BLK_PHY_RESET : E1000_SUCCESS;
 }
 
 /**
- *  igb_get_phy_id - Retrieve the PHY ID and revision
+ *  e1000_get_phy_id - Retrieve the PHY ID and revision
  *  @hw: pointer to the HW structure
  *
  *  Reads the PHY registers and stores the PHY ID and possibly the PHY
  *  revision in the hardware structure.
  **/
-s32 igb_get_phy_id(struct e1000_hw *hw)
+s32 e1000_get_phy_id(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
-	s32 ret_val = 0;
+	s32 ret_val = E1000_SUCCESS;
 	u16 phy_id;
 
+	DEBUGFUNC("e1000_get_phy_id");
+
+	if (!phy->ops.read_reg)
+		return E1000_SUCCESS;
+
 	ret_val = phy->ops.read_reg(hw, PHY_ID1, &phy_id);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	phy->id = (u32)(phy_id << 16);
-	udelay(20);
+	usec_delay(20);
 	ret_val = phy->ops.read_reg(hw, PHY_ID2, &phy_id);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	phy->id |= (u32)(phy_id & PHY_REVISION_MASK);
 	phy->revision = (u32)(phy_id & ~PHY_REVISION_MASK);
 
-out:
-	return ret_val;
+
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_phy_reset_dsp - Reset PHY DSP
+ *  e1000_phy_reset_dsp_generic - Reset PHY DSP
  *  @hw: pointer to the HW structure
  *
  *  Reset the digital signal processor.
  **/
-static s32 igb_phy_reset_dsp(struct e1000_hw *hw)
+s32 e1000_phy_reset_dsp_generic(struct e1000_hw *hw)
 {
-	s32 ret_val = 0;
+	s32 ret_val;
+
+	DEBUGFUNC("e1000_phy_reset_dsp_generic");
 
-	if (!(hw->phy.ops.write_reg))
-		goto out;
+	if (!hw->phy.ops.write_reg)
+		return E1000_SUCCESS;
 
 	ret_val = hw->phy.ops.write_reg(hw, M88E1000_PHY_GEN_CONTROL, 0xC1);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	ret_val = hw->phy.ops.write_reg(hw, M88E1000_PHY_GEN_CONTROL, 0);
-
-out:
-	return ret_val;
+	return hw->phy.ops.write_reg(hw, M88E1000_PHY_GEN_CONTROL, 0);
 }
 
 /**
- *  igb_read_phy_reg_mdic - Read MDI control register
+ *  e1000_read_phy_reg_mdic - Read MDI control register
  *  @hw: pointer to the HW structure
  *  @offset: register offset to be read
  *  @data: pointer to the read data
  *
- *  Reads the MDI control regsiter in the PHY at offset and stores the
+ *  Reads the MDI control register in the PHY at offset and stores the
  *  information read to data.
  **/
-s32 igb_read_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 *data)
+s32 e1000_read_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 *data)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	u32 i, mdic = 0;
-	s32 ret_val = 0;
+
+	DEBUGFUNC("e1000_read_phy_reg_mdic");
 
 	if (offset > MAX_PHY_REG_ADDRESS) {
-		hw_dbg("PHY Address %d is out of range\n", offset);
-		ret_val = -E1000_ERR_PARAM;
-		goto out;
+		DEBUGOUT1("PHY Address %d is out of range\n", offset);
+		return -E1000_ERR_PARAM;
 	}
 
-	/*
-	 * Set up Op-code, Phy Address, and register offset in the MDI
+	/* Set up Op-code, Phy Address, and register offset in the MDI
 	 * Control register.  The MAC will take care of interfacing with the
 	 * PHY to retrieve the desired data.
 	 */
@@ -157,57 +280,58 @@ s32 igb_read_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 *data)
 		(phy->addr << E1000_MDIC_PHY_SHIFT) |
 		(E1000_MDIC_OP_READ));
 
-	wr32(E1000_MDIC, mdic);
+	E1000_WRITE_REG(hw, E1000_MDIC, mdic);
 
-	/*
-	 * Poll the ready bit to see if the MDI read completed
+	/* Poll the ready bit to see if the MDI read completed
 	 * Increasing the time out as testing showed failures with
 	 * the lower time out
 	 */
 	for (i = 0; i < (E1000_GEN_POLL_TIMEOUT * 3); i++) {
-		udelay(50);
-		mdic = rd32(E1000_MDIC);
+		usec_delay_irq(50);
+		mdic = E1000_READ_REG(hw, E1000_MDIC);
 		if (mdic & E1000_MDIC_READY)
 			break;
 	}
 	if (!(mdic & E1000_MDIC_READY)) {
-		hw_dbg("MDI Read did not complete\n");
-		ret_val = -E1000_ERR_PHY;
-		goto out;
+		DEBUGOUT("MDI Read did not complete\n");
+		return -E1000_ERR_PHY;
 	}
 	if (mdic & E1000_MDIC_ERROR) {
-		hw_dbg("MDI Error\n");
-		ret_val = -E1000_ERR_PHY;
-		goto out;
+		DEBUGOUT("MDI Error\n");
+		return -E1000_ERR_PHY;
+	}
+	if (((mdic & E1000_MDIC_REG_MASK) >> E1000_MDIC_REG_SHIFT) != offset) {
+		DEBUGOUT2("MDI Read offset error - requested %d, returned %d\n",
+			  offset,
+			  (mdic & E1000_MDIC_REG_MASK) >> E1000_MDIC_REG_SHIFT);
+		return -E1000_ERR_PHY;
 	}
 	*data = (u16) mdic;
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_write_phy_reg_mdic - Write MDI control register
+ *  e1000_write_phy_reg_mdic - Write MDI control register
  *  @hw: pointer to the HW structure
  *  @offset: register offset to write to
  *  @data: data to write to register at offset
  *
  *  Writes data to MDI control register in the PHY at offset.
  **/
-s32 igb_write_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 data)
+s32 e1000_write_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 data)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	u32 i, mdic = 0;
-	s32 ret_val = 0;
+
+	DEBUGFUNC("e1000_write_phy_reg_mdic");
 
 	if (offset > MAX_PHY_REG_ADDRESS) {
-		hw_dbg("PHY Address %d is out of range\n", offset);
-		ret_val = -E1000_ERR_PARAM;
-		goto out;
+		DEBUGOUT1("PHY Address %d is out of range\n", offset);
+		return -E1000_ERR_PARAM;
 	}
 
-	/*
-	 * Set up Op-code, Phy Address, and register offset in the MDI
+	/* Set up Op-code, Phy Address, and register offset in the MDI
 	 * Control register.  The MAC will take care of interfacing with the
 	 * PHY to retrieve the desired data.
 	 */
@@ -216,36 +340,38 @@ s32 igb_write_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 data)
 		(phy->addr << E1000_MDIC_PHY_SHIFT) |
 		(E1000_MDIC_OP_WRITE));
 
-	wr32(E1000_MDIC, mdic);
+	E1000_WRITE_REG(hw, E1000_MDIC, mdic);
 
-	/*
-	 * Poll the ready bit to see if the MDI read completed
+	/* Poll the ready bit to see if the MDI read completed
 	 * Increasing the time out as testing showed failures with
 	 * the lower time out
 	 */
 	for (i = 0; i < (E1000_GEN_POLL_TIMEOUT * 3); i++) {
-		udelay(50);
-		mdic = rd32(E1000_MDIC);
+		usec_delay_irq(50);
+		mdic = E1000_READ_REG(hw, E1000_MDIC);
 		if (mdic & E1000_MDIC_READY)
 			break;
 	}
 	if (!(mdic & E1000_MDIC_READY)) {
-		hw_dbg("MDI Write did not complete\n");
-		ret_val = -E1000_ERR_PHY;
-		goto out;
+		DEBUGOUT("MDI Write did not complete\n");
+		return -E1000_ERR_PHY;
 	}
 	if (mdic & E1000_MDIC_ERROR) {
-		hw_dbg("MDI Error\n");
-		ret_val = -E1000_ERR_PHY;
-		goto out;
+		DEBUGOUT("MDI Error\n");
+		return -E1000_ERR_PHY;
+	}
+	if (((mdic & E1000_MDIC_REG_MASK) >> E1000_MDIC_REG_SHIFT) != offset) {
+		DEBUGOUT2("MDI Write offset error - requested %d, returned %d\n",
+			  offset,
+			  (mdic & E1000_MDIC_REG_MASK) >> E1000_MDIC_REG_SHIFT);
+		return -E1000_ERR_PHY;
 	}
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_read_phy_reg_i2c - Read PHY register using i2c
+ *  e1000_read_phy_reg_i2c - Read PHY register using i2c
  *  @hw: pointer to the HW structure
  *  @offset: register offset to be read
  *  @data: pointer to the read data
@@ -253,96 +379,231 @@ out:
  *  Reads the PHY register at offset using the i2c interface and stores the
  *  retrieved information in data.
  **/
-s32 igb_read_phy_reg_i2c(struct e1000_hw *hw, u32 offset, u16 *data)
+s32 e1000_read_phy_reg_i2c(struct e1000_hw *hw, u32 offset, u16 *data)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	u32 i, i2ccmd = 0;
 
+	DEBUGFUNC("e1000_read_phy_reg_i2c");
 
-	/*
-	 * Set up Op-code, Phy Address, and register address in the I2CCMD
+	/* Set up Op-code, Phy Address, and register address in the I2CCMD
 	 * register.  The MAC will take care of interfacing with the
 	 * PHY to retrieve the desired data.
 	 */
 	i2ccmd = ((offset << E1000_I2CCMD_REG_ADDR_SHIFT) |
-	          (phy->addr << E1000_I2CCMD_PHY_ADDR_SHIFT) |
-	          (E1000_I2CCMD_OPCODE_READ));
+		  (phy->addr << E1000_I2CCMD_PHY_ADDR_SHIFT) |
+		  (E1000_I2CCMD_OPCODE_READ));
 
-	wr32(E1000_I2CCMD, i2ccmd);
+	E1000_WRITE_REG(hw, E1000_I2CCMD, i2ccmd);
 
 	/* Poll the ready bit to see if the I2C read completed */
 	for (i = 0; i < E1000_I2CCMD_PHY_TIMEOUT; i++) {
-		udelay(50);
-		i2ccmd = rd32(E1000_I2CCMD);
+		usec_delay(50);
+		i2ccmd = E1000_READ_REG(hw, E1000_I2CCMD);
 		if (i2ccmd & E1000_I2CCMD_READY)
 			break;
 	}
 	if (!(i2ccmd & E1000_I2CCMD_READY)) {
-		hw_dbg("I2CCMD Read did not complete\n");
+		DEBUGOUT("I2CCMD Read did not complete\n");
 		return -E1000_ERR_PHY;
 	}
 	if (i2ccmd & E1000_I2CCMD_ERROR) {
-		hw_dbg("I2CCMD Error bit set\n");
+		DEBUGOUT("I2CCMD Error bit set\n");
 		return -E1000_ERR_PHY;
 	}
 
 	/* Need to byte-swap the 16-bit value. */
 	*data = ((i2ccmd >> 8) & 0x00FF) | ((i2ccmd << 8) & 0xFF00);
 
-	return 0;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_write_phy_reg_i2c - Write PHY register using i2c
+ *  e1000_write_phy_reg_i2c - Write PHY register using i2c
  *  @hw: pointer to the HW structure
  *  @offset: register offset to write to
  *  @data: data to write at register offset
  *
  *  Writes the data to PHY register at the offset using the i2c interface.
  **/
-s32 igb_write_phy_reg_i2c(struct e1000_hw *hw, u32 offset, u16 data)
+s32 e1000_write_phy_reg_i2c(struct e1000_hw *hw, u32 offset, u16 data)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	u32 i, i2ccmd = 0;
 	u16 phy_data_swapped;
 
+	DEBUGFUNC("e1000_write_phy_reg_i2c");
+
+	/* Prevent overwritting SFP I2C EEPROM which is at A0 address.*/
+	if ((hw->phy.addr == 0) || (hw->phy.addr > 7)) {
+		DEBUGOUT1("PHY I2C Address %d is out of range.\n",
+			  hw->phy.addr);
+		return -E1000_ERR_CONFIG;
+	}
 
 	/* Swap the data bytes for the I2C interface */
 	phy_data_swapped = ((data >> 8) & 0x00FF) | ((data << 8) & 0xFF00);
 
-	/*
-	 * Set up Op-code, Phy Address, and register address in the I2CCMD
+	/* Set up Op-code, Phy Address, and register address in the I2CCMD
 	 * register.  The MAC will take care of interfacing with the
 	 * PHY to retrieve the desired data.
 	 */
 	i2ccmd = ((offset << E1000_I2CCMD_REG_ADDR_SHIFT) |
-	          (phy->addr << E1000_I2CCMD_PHY_ADDR_SHIFT) |
-	          E1000_I2CCMD_OPCODE_WRITE |
-	          phy_data_swapped);
+		  (phy->addr << E1000_I2CCMD_PHY_ADDR_SHIFT) |
+		  E1000_I2CCMD_OPCODE_WRITE |
+		  phy_data_swapped);
 
-	wr32(E1000_I2CCMD, i2ccmd);
+	E1000_WRITE_REG(hw, E1000_I2CCMD, i2ccmd);
 
 	/* Poll the ready bit to see if the I2C read completed */
 	for (i = 0; i < E1000_I2CCMD_PHY_TIMEOUT; i++) {
-		udelay(50);
-		i2ccmd = rd32(E1000_I2CCMD);
+		usec_delay(50);
+		i2ccmd = E1000_READ_REG(hw, E1000_I2CCMD);
 		if (i2ccmd & E1000_I2CCMD_READY)
 			break;
 	}
 	if (!(i2ccmd & E1000_I2CCMD_READY)) {
-		hw_dbg("I2CCMD Write did not complete\n");
+		DEBUGOUT("I2CCMD Write did not complete\n");
 		return -E1000_ERR_PHY;
 	}
 	if (i2ccmd & E1000_I2CCMD_ERROR) {
-		hw_dbg("I2CCMD Error bit set\n");
+		DEBUGOUT("I2CCMD Error bit set\n");
 		return -E1000_ERR_PHY;
 	}
 
-	return 0;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_read_phy_reg_igp - Read igp PHY register
+ *  e1000_read_sfp_data_byte - Reads SFP module data.
+ *  @hw: pointer to the HW structure
+ *  @offset: byte location offset to be read
+ *  @data: read data buffer pointer
+ *
+ *  Reads one byte from SFP module data stored
+ *  in SFP resided EEPROM memory or SFP diagnostic area.
+ *  Function should be called with
+ *  E1000_I2CCMD_SFP_DATA_ADDR(<byte offset>) for SFP module database access
+ *  E1000_I2CCMD_SFP_DIAG_ADDR(<byte offset>) for SFP diagnostics parameters
+ *  access
+ **/
+s32 e1000_read_sfp_data_byte(struct e1000_hw *hw, u16 offset, u8 *data)
+{
+	u32 i = 0;
+	u32 i2ccmd = 0;
+	u32 data_local = 0;
+
+	DEBUGFUNC("e1000_read_sfp_data_byte");
+
+	if (offset > E1000_I2CCMD_SFP_DIAG_ADDR(255)) {
+		DEBUGOUT("I2CCMD command address exceeds upper limit\n");
+		return -E1000_ERR_PHY;
+	}
+
+	/* Set up Op-code, EEPROM Address,in the I2CCMD
+	 * register. The MAC will take care of interfacing with the
+	 * EEPROM to retrieve the desired data.
+	 */
+	i2ccmd = ((offset << E1000_I2CCMD_REG_ADDR_SHIFT) |
+		  E1000_I2CCMD_OPCODE_READ);
+
+	E1000_WRITE_REG(hw, E1000_I2CCMD, i2ccmd);
+
+	/* Poll the ready bit to see if the I2C read completed */
+	for (i = 0; i < E1000_I2CCMD_PHY_TIMEOUT; i++) {
+		usec_delay(50);
+		data_local = E1000_READ_REG(hw, E1000_I2CCMD);
+		if (data_local & E1000_I2CCMD_READY)
+			break;
+	}
+	if (!(data_local & E1000_I2CCMD_READY)) {
+		DEBUGOUT("I2CCMD Read did not complete\n");
+		return -E1000_ERR_PHY;
+	}
+	if (data_local & E1000_I2CCMD_ERROR) {
+		DEBUGOUT("I2CCMD Error bit set\n");
+		return -E1000_ERR_PHY;
+	}
+	*data = (u8) data_local & 0xFF;
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_write_sfp_data_byte - Writes SFP module data.
+ *  @hw: pointer to the HW structure
+ *  @offset: byte location offset to write to
+ *  @data: data to write
+ *
+ *  Writes one byte to SFP module data stored
+ *  in SFP resided EEPROM memory or SFP diagnostic area.
+ *  Function should be called with
+ *  E1000_I2CCMD_SFP_DATA_ADDR(<byte offset>) for SFP module database access
+ *  E1000_I2CCMD_SFP_DIAG_ADDR(<byte offset>) for SFP diagnostics parameters
+ *  access
+ **/
+s32 e1000_write_sfp_data_byte(struct e1000_hw *hw, u16 offset, u8 data)
+{
+	u32 i = 0;
+	u32 i2ccmd = 0;
+	u32 data_local = 0;
+
+	DEBUGFUNC("e1000_write_sfp_data_byte");
+
+	if (offset > E1000_I2CCMD_SFP_DIAG_ADDR(255)) {
+		DEBUGOUT("I2CCMD command address exceeds upper limit\n");
+		return -E1000_ERR_PHY;
+	}
+	/* The programming interface is 16 bits wide
+	 * so we need to read the whole word first
+	 * then update appropriate byte lane and write
+	 * the updated word back.
+	 */
+	/* Set up Op-code, EEPROM Address,in the I2CCMD
+	 * register. The MAC will take care of interfacing
+	 * with an EEPROM to write the data given.
+	 */
+	i2ccmd = ((offset << E1000_I2CCMD_REG_ADDR_SHIFT) |
+		  E1000_I2CCMD_OPCODE_READ);
+	/* Set a command to read single word */
+	E1000_WRITE_REG(hw, E1000_I2CCMD, i2ccmd);
+	for (i = 0; i < E1000_I2CCMD_PHY_TIMEOUT; i++) {
+		usec_delay(50);
+		/* Poll the ready bit to see if lastly
+		 * launched I2C operation completed
+		 */
+		i2ccmd = E1000_READ_REG(hw, E1000_I2CCMD);
+		if (i2ccmd & E1000_I2CCMD_READY) {
+			/* Check if this is READ or WRITE phase */
+			if ((i2ccmd & E1000_I2CCMD_OPCODE_READ) ==
+			    E1000_I2CCMD_OPCODE_READ) {
+				/* Write the selected byte
+				 * lane and update whole word
+				 */
+				data_local = i2ccmd & 0xFF00;
+				data_local |= data;
+				i2ccmd = ((offset <<
+					E1000_I2CCMD_REG_ADDR_SHIFT) |
+					E1000_I2CCMD_OPCODE_WRITE | data_local);
+				E1000_WRITE_REG(hw, E1000_I2CCMD, i2ccmd);
+			} else {
+				break;
+			}
+		}
+	}
+	if (!(i2ccmd & E1000_I2CCMD_READY)) {
+		DEBUGOUT("I2CCMD Write did not complete\n");
+		return -E1000_ERR_PHY;
+	}
+	if (i2ccmd & E1000_I2CCMD_ERROR) {
+		DEBUGOUT("I2CCMD Error bit set\n");
+		return -E1000_ERR_PHY;
+	}
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_read_phy_reg_m88 - Read m88 PHY register
  *  @hw: pointer to the HW structure
  *  @offset: register offset to be read
  *  @data: pointer to the read data
@@ -351,38 +612,29 @@ s32 igb_write_phy_reg_i2c(struct e1000_hw *hw, u32 offset, u16 data)
  *  and storing the retrieved information in data.  Release any acquired
  *  semaphores before exiting.
  **/
-s32 igb_read_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 *data)
+s32 e1000_read_phy_reg_m88(struct e1000_hw *hw, u32 offset, u16 *data)
 {
-	s32 ret_val = 0;
+	s32 ret_val;
+
+	DEBUGFUNC("e1000_read_phy_reg_m88");
 
-	if (!(hw->phy.ops.acquire))
-		goto out;
+	if (!hw->phy.ops.acquire)
+		return E1000_SUCCESS;
 
 	ret_val = hw->phy.ops.acquire(hw);
 	if (ret_val)
-		goto out;
-
-	if (offset > MAX_PHY_MULTI_PAGE_REG) {
-		ret_val = igb_write_phy_reg_mdic(hw,
-						   IGP01E1000_PHY_PAGE_SELECT,
-						   (u16)offset);
-		if (ret_val) {
-			hw->phy.ops.release(hw);
-			goto out;
-		}
-	}
+		return ret_val;
 
-	ret_val = igb_read_phy_reg_mdic(hw, MAX_PHY_REG_ADDRESS & offset,
-					data);
+	ret_val = e1000_read_phy_reg_mdic(hw, MAX_PHY_REG_ADDRESS & offset,
+					  data);
 
 	hw->phy.ops.release(hw);
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_write_phy_reg_igp - Write igp PHY register
+ *  e1000_write_phy_reg_m88 - Write m88 PHY register
  *  @hw: pointer to the HW structure
  *  @offset: register offset to write to
  *  @data: data to write at register offset
@@ -390,105 +642,462 @@ out:
  *  Acquires semaphore, if necessary, then writes the data to PHY register
  *  at the offset.  Release any acquired semaphores before exiting.
  **/
-s32 igb_write_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 data)
+s32 e1000_write_phy_reg_m88(struct e1000_hw *hw, u32 offset, u16 data)
 {
-	s32 ret_val = 0;
+	s32 ret_val;
+
+	DEBUGFUNC("e1000_write_phy_reg_m88");
 
-	if (!(hw->phy.ops.acquire))
-		goto out;
+	if (!hw->phy.ops.acquire)
+		return E1000_SUCCESS;
 
 	ret_val = hw->phy.ops.acquire(hw);
 	if (ret_val)
-		goto out;
+		return ret_val;
+
+	ret_val = e1000_write_phy_reg_mdic(hw, MAX_PHY_REG_ADDRESS & offset,
+					   data);
+
+	hw->phy.ops.release(hw);
 
-	if (offset > MAX_PHY_MULTI_PAGE_REG) {
-		ret_val = igb_write_phy_reg_mdic(hw,
+	return ret_val;
+}
+
+/**
+ *  e1000_set_page_igp - Set page as on IGP-like PHY(s)
+ *  @hw: pointer to the HW structure
+ *  @page: page to set (shifted left when necessary)
+ *
+ *  Sets PHY page required for PHY register access.  Assumes semaphore is
+ *  already acquired.  Note, this function sets phy.addr to 1 so the caller
+ *  must set it appropriately (if necessary) after this function returns.
+ **/
+s32 e1000_set_page_igp(struct e1000_hw *hw, u16 page)
+{
+	DEBUGFUNC("e1000_set_page_igp");
+
+	DEBUGOUT1("Setting page 0x%x\n", page);
+
+	hw->phy.addr = 1;
+
+	return e1000_write_phy_reg_mdic(hw, IGP01E1000_PHY_PAGE_SELECT, page);
+}
+
+/**
+ *  __e1000_read_phy_reg_igp - Read igp PHY register
+ *  @hw: pointer to the HW structure
+ *  @offset: register offset to be read
+ *  @data: pointer to the read data
+ *  @locked: semaphore has already been acquired or not
+ *
+ *  Acquires semaphore, if necessary, then reads the PHY register at offset
+ *  and stores the retrieved information in data.  Release any acquired
+ *  semaphores before exiting.
+ **/
+static s32 __e1000_read_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 *data,
+				    bool locked)
+{
+	s32 ret_val = E1000_SUCCESS;
+
+	DEBUGFUNC("__e1000_read_phy_reg_igp");
+
+	if (!locked) {
+		if (!hw->phy.ops.acquire)
+			return E1000_SUCCESS;
+
+		ret_val = hw->phy.ops.acquire(hw);
+		if (ret_val)
+			return ret_val;
+	}
+
+	if (offset > MAX_PHY_MULTI_PAGE_REG)
+		ret_val = e1000_write_phy_reg_mdic(hw,
 						   IGP01E1000_PHY_PAGE_SELECT,
 						   (u16)offset);
-		if (ret_val) {
-			hw->phy.ops.release(hw);
-			goto out;
-		}
-	}
+	if (!ret_val)
+		ret_val = e1000_read_phy_reg_mdic(hw,
+						  MAX_PHY_REG_ADDRESS & offset,
+						  data);
+	if (!locked)
+		hw->phy.ops.release(hw);
 
-	ret_val = igb_write_phy_reg_mdic(hw, MAX_PHY_REG_ADDRESS & offset,
-					   data);
+	return ret_val;
+}
 
-	hw->phy.ops.release(hw);
+/**
+ *  e1000_read_phy_reg_igp - Read igp PHY register
+ *  @hw: pointer to the HW structure
+ *  @offset: register offset to be read
+ *  @data: pointer to the read data
+ *
+ *  Acquires semaphore then reads the PHY register at offset and stores the
+ *  retrieved information in data.
+ *  Release the acquired semaphore before exiting.
+ **/
+s32 e1000_read_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 *data)
+{
+	return __e1000_read_phy_reg_igp(hw, offset, data, false);
+}
+
+/**
+ *  e1000_read_phy_reg_igp_locked - Read igp PHY register
+ *  @hw: pointer to the HW structure
+ *  @offset: register offset to be read
+ *  @data: pointer to the read data
+ *
+ *  Reads the PHY register at offset and stores the retrieved information
+ *  in data.  Assumes semaphore already acquired.
+ **/
+s32 e1000_read_phy_reg_igp_locked(struct e1000_hw *hw, u32 offset, u16 *data)
+{
+	return __e1000_read_phy_reg_igp(hw, offset, data, true);
+}
+
+/**
+ *  e1000_write_phy_reg_igp - Write igp PHY register
+ *  @hw: pointer to the HW structure
+ *  @offset: register offset to write to
+ *  @data: data to write at register offset
+ *  @locked: semaphore has already been acquired or not
+ *
+ *  Acquires semaphore, if necessary, then writes the data to PHY register
+ *  at the offset.  Release any acquired semaphores before exiting.
+ **/
+static s32 __e1000_write_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 data,
+				     bool locked)
+{
+	s32 ret_val = E1000_SUCCESS;
+
+	DEBUGFUNC("e1000_write_phy_reg_igp");
+
+	if (!locked) {
+		if (!hw->phy.ops.acquire)
+			return E1000_SUCCESS;
+
+		ret_val = hw->phy.ops.acquire(hw);
+		if (ret_val)
+			return ret_val;
+	}
+
+	if (offset > MAX_PHY_MULTI_PAGE_REG)
+		ret_val = e1000_write_phy_reg_mdic(hw,
+						   IGP01E1000_PHY_PAGE_SELECT,
+						   (u16)offset);
+	if (!ret_val)
+		ret_val = e1000_write_phy_reg_mdic(hw, MAX_PHY_REG_ADDRESS &
+						       offset,
+						   data);
+	if (!locked)
+		hw->phy.ops.release(hw);
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_copper_link_setup_82580 - Setup 82580 PHY for copper link
+ *  e1000_write_phy_reg_igp - Write igp PHY register
  *  @hw: pointer to the HW structure
+ *  @offset: register offset to write to
+ *  @data: data to write at register offset
  *
- *  Sets up Carrier-sense on Transmit and downshift values.
+ *  Acquires semaphore then writes the data to PHY register
+ *  at the offset.  Release any acquired semaphores before exiting.
  **/
-s32 igb_copper_link_setup_82580(struct e1000_hw *hw)
+s32 e1000_write_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 data)
+{
+	return __e1000_write_phy_reg_igp(hw, offset, data, false);
+}
+
+/**
+ *  e1000_write_phy_reg_igp_locked - Write igp PHY register
+ *  @hw: pointer to the HW structure
+ *  @offset: register offset to write to
+ *  @data: data to write at register offset
+ *
+ *  Writes the data to PHY register at the offset.
+ *  Assumes semaphore already acquired.
+ **/
+s32 e1000_write_phy_reg_igp_locked(struct e1000_hw *hw, u32 offset, u16 data)
+{
+	return __e1000_write_phy_reg_igp(hw, offset, data, true);
+}
+
+/**
+ *  __e1000_read_kmrn_reg - Read kumeran register
+ *  @hw: pointer to the HW structure
+ *  @offset: register offset to be read
+ *  @data: pointer to the read data
+ *  @locked: semaphore has already been acquired or not
+ *
+ *  Acquires semaphore, if necessary.  Then reads the PHY register at offset
+ *  using the kumeran interface.  The information retrieved is stored in data.
+ *  Release any acquired semaphores before exiting.
+ **/
+static s32 __e1000_read_kmrn_reg(struct e1000_hw *hw, u32 offset, u16 *data,
+				 bool locked)
+{
+	u32 kmrnctrlsta;
+
+	DEBUGFUNC("__e1000_read_kmrn_reg");
+
+	if (!locked) {
+		s32 ret_val = E1000_SUCCESS;
+
+		if (!hw->phy.ops.acquire)
+			return E1000_SUCCESS;
+
+		ret_val = hw->phy.ops.acquire(hw);
+		if (ret_val)
+			return ret_val;
+	}
+
+	kmrnctrlsta = ((offset << E1000_KMRNCTRLSTA_OFFSET_SHIFT) &
+		       E1000_KMRNCTRLSTA_OFFSET) | E1000_KMRNCTRLSTA_REN;
+	E1000_WRITE_REG(hw, E1000_KMRNCTRLSTA, kmrnctrlsta);
+	E1000_WRITE_FLUSH(hw);
+
+	usec_delay(2);
+
+	kmrnctrlsta = E1000_READ_REG(hw, E1000_KMRNCTRLSTA);
+	*data = (u16)kmrnctrlsta;
+
+	if (!locked)
+		hw->phy.ops.release(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_read_kmrn_reg_generic -  Read kumeran register
+ *  @hw: pointer to the HW structure
+ *  @offset: register offset to be read
+ *  @data: pointer to the read data
+ *
+ *  Acquires semaphore then reads the PHY register at offset using the
+ *  kumeran interface.  The information retrieved is stored in data.
+ *  Release the acquired semaphore before exiting.
+ **/
+s32 e1000_read_kmrn_reg_generic(struct e1000_hw *hw, u32 offset, u16 *data)
+{
+	return __e1000_read_kmrn_reg(hw, offset, data, false);
+}
+
+/**
+ *  e1000_read_kmrn_reg_locked -  Read kumeran register
+ *  @hw: pointer to the HW structure
+ *  @offset: register offset to be read
+ *  @data: pointer to the read data
+ *
+ *  Reads the PHY register at offset using the kumeran interface.  The
+ *  information retrieved is stored in data.
+ *  Assumes semaphore already acquired.
+ **/
+s32 e1000_read_kmrn_reg_locked(struct e1000_hw *hw, u32 offset, u16 *data)
+{
+	return __e1000_read_kmrn_reg(hw, offset, data, true);
+}
+
+/**
+ *  __e1000_write_kmrn_reg - Write kumeran register
+ *  @hw: pointer to the HW structure
+ *  @offset: register offset to write to
+ *  @data: data to write at register offset
+ *  @locked: semaphore has already been acquired or not
+ *
+ *  Acquires semaphore, if necessary.  Then write the data to PHY register
+ *  at the offset using the kumeran interface.  Release any acquired semaphores
+ *  before exiting.
+ **/
+static s32 __e1000_write_kmrn_reg(struct e1000_hw *hw, u32 offset, u16 data,
+				  bool locked)
+{
+	u32 kmrnctrlsta;
+
+	DEBUGFUNC("e1000_write_kmrn_reg_generic");
+
+	if (!locked) {
+		s32 ret_val = E1000_SUCCESS;
+
+		if (!hw->phy.ops.acquire)
+			return E1000_SUCCESS;
+
+		ret_val = hw->phy.ops.acquire(hw);
+		if (ret_val)
+			return ret_val;
+	}
+
+	kmrnctrlsta = ((offset << E1000_KMRNCTRLSTA_OFFSET_SHIFT) &
+		       E1000_KMRNCTRLSTA_OFFSET) | data;
+	E1000_WRITE_REG(hw, E1000_KMRNCTRLSTA, kmrnctrlsta);
+	E1000_WRITE_FLUSH(hw);
+
+	usec_delay(2);
+
+	if (!locked)
+		hw->phy.ops.release(hw);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_write_kmrn_reg_generic -  Write kumeran register
+ *  @hw: pointer to the HW structure
+ *  @offset: register offset to write to
+ *  @data: data to write at register offset
+ *
+ *  Acquires semaphore then writes the data to the PHY register at the offset
+ *  using the kumeran interface.  Release the acquired semaphore before exiting.
+ **/
+s32 e1000_write_kmrn_reg_generic(struct e1000_hw *hw, u32 offset, u16 data)
+{
+	return __e1000_write_kmrn_reg(hw, offset, data, false);
+}
+
+/**
+ *  e1000_write_kmrn_reg_locked -  Write kumeran register
+ *  @hw: pointer to the HW structure
+ *  @offset: register offset to write to
+ *  @data: data to write at register offset
+ *
+ *  Write the data to PHY register at the offset using the kumeran interface.
+ *  Assumes semaphore already acquired.
+ **/
+s32 e1000_write_kmrn_reg_locked(struct e1000_hw *hw, u32 offset, u16 data)
+{
+	return __e1000_write_kmrn_reg(hw, offset, data, true);
+}
+
+/**
+ *  e1000_set_master_slave_mode - Setup PHY for Master/slave mode
+ *  @hw: pointer to the HW structure
+ *
+ *  Sets up Master/slave mode
+ **/
+static s32 e1000_set_master_slave_mode(struct e1000_hw *hw)
 {
-	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 phy_data;
 
+	/* Resolve Master/Slave mode */
+	ret_val = hw->phy.ops.read_reg(hw, PHY_1000T_CTRL, &phy_data);
+	if (ret_val)
+		return ret_val;
+
+	/* load defaults for future use */
+	hw->phy.original_ms_type = (phy_data & CR_1000T_MS_ENABLE) ?
+				   ((phy_data & CR_1000T_MS_VALUE) ?
+				    e1000_ms_force_master :
+				    e1000_ms_force_slave) : e1000_ms_auto;
 
-	if (phy->reset_disable) {
-		ret_val = 0;
-		goto out;
+	switch (hw->phy.ms_type) {
+	case e1000_ms_force_master:
+		phy_data |= (CR_1000T_MS_ENABLE | CR_1000T_MS_VALUE);
+		break;
+	case e1000_ms_force_slave:
+		phy_data |= CR_1000T_MS_ENABLE;
+		phy_data &= ~(CR_1000T_MS_VALUE);
+		break;
+	case e1000_ms_auto:
+		phy_data &= ~CR_1000T_MS_ENABLE;
+		/* fall-through */
+	default:
+		break;
 	}
 
-	if (phy->type == e1000_phy_82580) {
+	return hw->phy.ops.write_reg(hw, PHY_1000T_CTRL, phy_data);
+}
+
+/**
+ *  e1000_copper_link_setup_82577 - Setup 82577 PHY for copper link
+ *  @hw: pointer to the HW structure
+ *
+ *  Sets up Carrier-sense on Transmit and downshift values.
+ **/
+s32 e1000_copper_link_setup_82577(struct e1000_hw *hw)
+{
+	s32 ret_val;
+	u16 phy_data;
+
+	DEBUGFUNC("e1000_copper_link_setup_82577");
+
+	if (hw->phy.reset_disable)
+		return E1000_SUCCESS;
+
+	if (hw->phy.type == e1000_phy_82580) {
 		ret_val = hw->phy.ops.reset(hw);
 		if (ret_val) {
-			hw_dbg("Error resetting the PHY.\n");
-			goto out;
+			DEBUGOUT("Error resetting the PHY.\n");
+			return ret_val;
 		}
 	}
 
-	/* Enable CRS on TX. This must be set for half-duplex operation. */
-	ret_val = phy->ops.read_reg(hw, I82580_CFG_REG, &phy_data);
+	/* Enable CRS on Tx. This must be set for half-duplex operation. */
+	ret_val = hw->phy.ops.read_reg(hw, I82577_CFG_REG, &phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	phy_data |= I82580_CFG_ASSERT_CRS_ON_TX;
+	phy_data |= I82577_CFG_ASSERT_CRS_ON_TX;
 
 	/* Enable downshift */
-	phy_data |= I82580_CFG_ENABLE_DOWNSHIFT;
+	phy_data |= I82577_CFG_ENABLE_DOWNSHIFT;
 
-	ret_val = phy->ops.write_reg(hw, I82580_CFG_REG, phy_data);
+	ret_val = hw->phy.ops.write_reg(hw, I82577_CFG_REG, phy_data);
+	if (ret_val)
+		return ret_val;
 
-out:
-	return ret_val;
+	/* Set MDI/MDIX mode */
+	ret_val = hw->phy.ops.read_reg(hw, I82577_PHY_CTRL_2, &phy_data);
+	if (ret_val)
+		return ret_val;
+	phy_data &= ~I82577_PHY_CTRL2_MDIX_CFG_MASK;
+	/* Options:
+	 *   0 - Auto (default)
+	 *   1 - MDI mode
+	 *   2 - MDI-X mode
+	 */
+	switch (hw->phy.mdix) {
+	case 1:
+		break;
+	case 2:
+		phy_data |= I82577_PHY_CTRL2_MANUAL_MDIX;
+		break;
+	case 0:
+	default:
+		phy_data |= I82577_PHY_CTRL2_AUTO_MDI_MDIX;
+		break;
+	}
+	ret_val = hw->phy.ops.write_reg(hw, I82577_PHY_CTRL_2, phy_data);
+	if (ret_val)
+		return ret_val;
+
+	return e1000_set_master_slave_mode(hw);
 }
 
 /**
- *  igb_copper_link_setup_m88 - Setup m88 PHY's for copper link
+ *  e1000_copper_link_setup_m88 - Setup m88 PHY's for copper link
  *  @hw: pointer to the HW structure
  *
  *  Sets up MDI/MDI-X and polarity for m88 PHY's.  If necessary, transmit clock
  *  and downshift values are set also.
  **/
-s32 igb_copper_link_setup_m88(struct e1000_hw *hw)
+s32 e1000_copper_link_setup_m88(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 phy_data;
 
-	if (phy->reset_disable) {
-		ret_val = 0;
-		goto out;
-	}
+	DEBUGFUNC("e1000_copper_link_setup_m88");
+
+	if (phy->reset_disable)
+		return E1000_SUCCESS;
 
-	/* Enable CRS on TX. This must be set for half-duplex operation. */
+	/* Enable CRS on Tx. This must be set for half-duplex operation. */
 	ret_val = phy->ops.read_reg(hw, M88E1000_PHY_SPEC_CTRL, &phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	phy_data |= M88E1000_PSCR_ASSERT_CRS_ON_TX;
 
-	/*
-	 * Options:
+	/* Options:
 	 *   MDI/MDI-X = 0 (default)
 	 *   0 - Auto for all speeds
 	 *   1 - MDI mode
@@ -513,30 +1122,28 @@ s32 igb_copper_link_setup_m88(struct e1000_hw *hw)
 		break;
 	}
 
-	/*
-	 * Options:
+	/* Options:
 	 *   disable_polarity_correction = 0 (default)
 	 *       Automatic Correction for Reversed Cable Polarity
 	 *   0 - Disabled
 	 *   1 - Enabled
 	 */
 	phy_data &= ~M88E1000_PSCR_POLARITY_REVERSAL;
-	if (phy->disable_polarity_correction == 1)
+	if (phy->disable_polarity_correction)
 		phy_data |= M88E1000_PSCR_POLARITY_REVERSAL;
 
 	ret_val = phy->ops.write_reg(hw, M88E1000_PHY_SPEC_CTRL, phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	if (phy->revision < E1000_REVISION_4) {
-		/*
-		 * Force TX_CLK in the Extended PHY Specific Control Register
+		/* Force TX_CLK in the Extended PHY Specific Control Register
 		 * to 25MHz clock.
 		 */
 		ret_val = phy->ops.read_reg(hw, M88E1000_EXT_PHY_SPEC_CTRL,
-					     &phy_data);
+					    &phy_data);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		phy_data |= M88E1000_EPSCR_TX_CLK_25;
 
@@ -548,52 +1155,50 @@ s32 igb_copper_link_setup_m88(struct e1000_hw *hw)
 		} else {
 			/* Configure Master and Slave downshift values */
 			phy_data &= ~(M88E1000_EPSCR_MASTER_DOWNSHIFT_MASK |
-				      M88E1000_EPSCR_SLAVE_DOWNSHIFT_MASK);
+				     M88E1000_EPSCR_SLAVE_DOWNSHIFT_MASK);
 			phy_data |= (M88E1000_EPSCR_MASTER_DOWNSHIFT_1X |
 				     M88E1000_EPSCR_SLAVE_DOWNSHIFT_1X);
 		}
 		ret_val = phy->ops.write_reg(hw, M88E1000_EXT_PHY_SPEC_CTRL,
 					     phy_data);
 		if (ret_val)
-			goto out;
+			return ret_val;
 	}
 
 	/* Commit the changes. */
-	ret_val = igb_phy_sw_reset(hw);
+	ret_val = phy->ops.commit(hw);
 	if (ret_val) {
-		hw_dbg("Error committing the PHY changes\n");
-		goto out;
+		DEBUGOUT("Error committing the PHY changes\n");
+		return ret_val;
 	}
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_copper_link_setup_m88_gen2 - Setup m88 PHY's for copper link
+ *  e1000_copper_link_setup_m88_gen2 - Setup m88 PHY's for copper link
  *  @hw: pointer to the HW structure
  *
  *  Sets up MDI/MDI-X and polarity for i347-AT4, m88e1322 and m88e1112 PHY's.
  *  Also enables and sets the downshift parameters.
  **/
-s32 igb_copper_link_setup_m88_gen2(struct e1000_hw *hw)
+s32 e1000_copper_link_setup_m88_gen2(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 phy_data;
 
-	if (phy->reset_disable) {
-		ret_val = 0;
-		goto out;
-	}
+	DEBUGFUNC("e1000_copper_link_setup_m88_gen2");
+
+	if (phy->reset_disable)
+		return E1000_SUCCESS;
 
 	/* Enable CRS on Tx. This must be set for half-duplex operation. */
 	ret_val = phy->ops.read_reg(hw, M88E1000_PHY_SPEC_CTRL, &phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	/*
-	 * Options:
+	/* Options:
 	 *   MDI/MDI-X = 0 (default)
 	 *   0 - Auto for all speeds
 	 *   1 - MDI mode
@@ -621,91 +1226,94 @@ s32 igb_copper_link_setup_m88_gen2(struct e1000_hw *hw)
 		break;
 	}
 
-	/*
-	 * Options:
+	/* Options:
 	 *   disable_polarity_correction = 0 (default)
 	 *       Automatic Correction for Reversed Cable Polarity
 	 *   0 - Disabled
 	 *   1 - Enabled
 	 */
 	phy_data &= ~M88E1000_PSCR_POLARITY_REVERSAL;
-	if (phy->disable_polarity_correction == 1)
+	if (phy->disable_polarity_correction)
 		phy_data |= M88E1000_PSCR_POLARITY_REVERSAL;
 
 	/* Enable downshift and setting it to X6 */
+	if (phy->id == M88E1543_E_PHY_ID) {
+		phy_data &= ~I347AT4_PSCR_DOWNSHIFT_ENABLE;
+		ret_val =
+		    phy->ops.write_reg(hw, M88E1000_PHY_SPEC_CTRL, phy_data);
+		if (ret_val)
+			return ret_val;
+
+		ret_val = phy->ops.commit(hw);
+		if (ret_val) {
+			DEBUGOUT("Error committing the PHY changes\n");
+			return ret_val;
+		}
+	}
+
 	phy_data &= ~I347AT4_PSCR_DOWNSHIFT_MASK;
 	phy_data |= I347AT4_PSCR_DOWNSHIFT_6X;
 	phy_data |= I347AT4_PSCR_DOWNSHIFT_ENABLE;
 
 	ret_val = phy->ops.write_reg(hw, M88E1000_PHY_SPEC_CTRL, phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	/* Commit the changes. */
-	ret_val = igb_phy_sw_reset(hw);
+	ret_val = phy->ops.commit(hw);
 	if (ret_val) {
-		hw_dbg("Error committing the PHY changes\n");
-		goto out;
+		DEBUGOUT("Error committing the PHY changes\n");
+		return ret_val;
 	}
 
-out:
-	return ret_val;
+	ret_val = e1000_set_master_slave_mode(hw);
+	if (ret_val)
+		return ret_val;
+
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_copper_link_setup_igp - Setup igp PHY's for copper link
+ *  e1000_copper_link_setup_igp - Setup igp PHY's for copper link
  *  @hw: pointer to the HW structure
  *
  *  Sets up LPLU, MDI/MDI-X, polarity, Smartspeed and Master/Slave config for
  *  igp PHY's.
  **/
-s32 igb_copper_link_setup_igp(struct e1000_hw *hw)
+s32 e1000_copper_link_setup_igp(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 data;
 
-	if (phy->reset_disable) {
-		ret_val = 0;
-		goto out;
-	}
+	DEBUGFUNC("e1000_copper_link_setup_igp");
 
-	ret_val = phy->ops.reset(hw);
+	if (phy->reset_disable)
+		return E1000_SUCCESS;
+
+	ret_val = hw->phy.ops.reset(hw);
 	if (ret_val) {
-		hw_dbg("Error resetting the PHY.\n");
-		goto out;
+		DEBUGOUT("Error resetting the PHY.\n");
+		return ret_val;
 	}
 
-	/*
-	 * Wait 100ms for MAC to configure PHY from NVM settings, to avoid
+	/* Wait 100ms for MAC to configure PHY from NVM settings, to avoid
 	 * timeout issues when LFS is enabled.
 	 */
-	msleep(100);
+	msec_delay(100);
 
-	/*
-	 * The NVM settings will configure LPLU in D3 for
-	 * non-IGP1 PHYs.
-	 */
-	if (phy->type == e1000_phy_igp) {
-		/* disable lplu d3 during driver init */
-		if (phy->ops.set_d3_lplu_state)
-			ret_val = phy->ops.set_d3_lplu_state(hw, false);
+	/* disable lplu d0 during driver init */
+	if (hw->phy.ops.set_d0_lplu_state) {
+		ret_val = hw->phy.ops.set_d0_lplu_state(hw, false);
 		if (ret_val) {
-			hw_dbg("Error Disabling LPLU D3\n");
-			goto out;
+			DEBUGOUT("Error Disabling LPLU D0\n");
+			return ret_val;
 		}
 	}
-
-	/* disable lplu d0 during driver init */
-	ret_val = phy->ops.set_d0_lplu_state(hw, false);
-	if (ret_val) {
-		hw_dbg("Error Disabling LPLU D0\n");
-		goto out;
-	}
 	/* Configure mdi-mdix settings */
 	ret_val = phy->ops.read_reg(hw, IGP01E1000_PHY_PORT_CTRL, &data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	data &= ~IGP01E1000_PSCR_AUTO_MDIX;
 
@@ -723,12 +1331,11 @@ s32 igb_copper_link_setup_igp(struct e1000_hw *hw)
 	}
 	ret_val = phy->ops.write_reg(hw, IGP01E1000_PHY_PORT_CTRL, data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	/* set auto-master slave resolution settings */
 	if (hw->mac.autoneg) {
-		/*
-		 * when autonegotiation advertisement is only 1000Mbps then we
+		/* when autonegotiation advertisement is only 1000Mbps then we
 		 * should disable SmartSpeed and enable Auto MasterSlave
 		 * resolution as hardware default.
 		 */
@@ -738,129 +1345,34 @@ s32 igb_copper_link_setup_igp(struct e1000_hw *hw)
 						    IGP01E1000_PHY_PORT_CONFIG,
 						    &data);
 			if (ret_val)
-				goto out;
+				return ret_val;
 
 			data &= ~IGP01E1000_PSCFR_SMART_SPEED;
 			ret_val = phy->ops.write_reg(hw,
 						     IGP01E1000_PHY_PORT_CONFIG,
 						     data);
 			if (ret_val)
-				goto out;
+				return ret_val;
 
 			/* Set auto Master/Slave resolution process */
 			ret_val = phy->ops.read_reg(hw, PHY_1000T_CTRL, &data);
 			if (ret_val)
-				goto out;
+				return ret_val;
 
 			data &= ~CR_1000T_MS_ENABLE;
 			ret_val = phy->ops.write_reg(hw, PHY_1000T_CTRL, data);
 			if (ret_val)
-				goto out;
-		}
-
-		ret_val = phy->ops.read_reg(hw, PHY_1000T_CTRL, &data);
-		if (ret_val)
-			goto out;
-
-		/* load defaults for future use */
-		phy->original_ms_type = (data & CR_1000T_MS_ENABLE) ?
-			((data & CR_1000T_MS_VALUE) ?
-			e1000_ms_force_master :
-			e1000_ms_force_slave) :
-			e1000_ms_auto;
-
-		switch (phy->ms_type) {
-		case e1000_ms_force_master:
-			data |= (CR_1000T_MS_ENABLE | CR_1000T_MS_VALUE);
-			break;
-		case e1000_ms_force_slave:
-			data |= CR_1000T_MS_ENABLE;
-			data &= ~(CR_1000T_MS_VALUE);
-			break;
-		case e1000_ms_auto:
-			data &= ~CR_1000T_MS_ENABLE;
-		default:
-			break;
+				return ret_val;
 		}
-		ret_val = phy->ops.write_reg(hw, PHY_1000T_CTRL, data);
-		if (ret_val)
-			goto out;
-	}
-
-out:
-	return ret_val;
-}
-
-/**
- *  igb_copper_link_autoneg - Setup/Enable autoneg for copper link
- *  @hw: pointer to the HW structure
- *
- *  Performs initial bounds checking on autoneg advertisement parameter, then
- *  configure to advertise the full capability.  Setup the PHY to autoneg
- *  and restart the negotiation process between the link partner.  If
- *  autoneg_wait_to_complete, then wait for autoneg to complete before exiting.
- **/
-static s32 igb_copper_link_autoneg(struct e1000_hw *hw)
-{
-	struct e1000_phy_info *phy = &hw->phy;
-	s32 ret_val;
-	u16 phy_ctrl;
-
-	/*
-	 * Perform some bounds checking on the autoneg advertisement
-	 * parameter.
-	 */
-	phy->autoneg_advertised &= phy->autoneg_mask;
-
-	/*
-	 * If autoneg_advertised is zero, we assume it was not defaulted
-	 * by the calling code so we set to advertise full capability.
-	 */
-	if (phy->autoneg_advertised == 0)
-		phy->autoneg_advertised = phy->autoneg_mask;
 
-	hw_dbg("Reconfiguring auto-neg advertisement params\n");
-	ret_val = igb_phy_setup_autoneg(hw);
-	if (ret_val) {
-		hw_dbg("Error Setting up Auto-Negotiation\n");
-		goto out;
+		ret_val = e1000_set_master_slave_mode(hw);
 	}
-	hw_dbg("Restarting Auto-Neg\n");
-
-	/*
-	 * Restart auto-negotiation by setting the Auto Neg Enable bit and
-	 * the Auto Neg Restart bit in the PHY control register.
-	 */
-	ret_val = phy->ops.read_reg(hw, PHY_CONTROL, &phy_ctrl);
-	if (ret_val)
-		goto out;
 
-	phy_ctrl |= (MII_CR_AUTO_NEG_EN | MII_CR_RESTART_AUTO_NEG);
-	ret_val = phy->ops.write_reg(hw, PHY_CONTROL, phy_ctrl);
-	if (ret_val)
-		goto out;
-
-	/*
-	 * Does the user want to wait for Auto-Neg to complete here, or
-	 * check at a later time (for example, callback routine).
-	 */
-	if (phy->autoneg_wait_to_complete) {
-		ret_val = igb_wait_autoneg(hw);
-		if (ret_val) {
-			hw_dbg("Error while waiting for "
-			       "autoneg to complete\n");
-			goto out;
-		}
-	}
-
-	hw->mac.get_link_status = true;
-
-out:
 	return ret_val;
 }
 
 /**
- *  igb_phy_setup_autoneg - Configure PHY for auto-negotiation
+ *  e1000_phy_setup_autoneg - Configure PHY for auto-negotiation
  *  @hw: pointer to the HW structure
  *
  *  Reads the MII auto-neg advertisement register and/or the 1000T control
@@ -868,38 +1380,38 @@ out:
  *  return successful.  Otherwise, setup advertisement and flow control to
  *  the appropriate values for the wanted auto-negotiation.
  **/
-static s32 igb_phy_setup_autoneg(struct e1000_hw *hw)
+static s32 e1000_phy_setup_autoneg(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 mii_autoneg_adv_reg;
 	u16 mii_1000t_ctrl_reg = 0;
 
+	DEBUGFUNC("e1000_phy_setup_autoneg");
+
 	phy->autoneg_advertised &= phy->autoneg_mask;
 
 	/* Read the MII Auto-Neg Advertisement Register (Address 4). */
 	ret_val = phy->ops.read_reg(hw, PHY_AUTONEG_ADV, &mii_autoneg_adv_reg);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	if (phy->autoneg_mask & ADVERTISE_1000_FULL) {
 		/* Read the MII 1000Base-T Control Register (Address 9). */
 		ret_val = phy->ops.read_reg(hw, PHY_1000T_CTRL,
 					    &mii_1000t_ctrl_reg);
 		if (ret_val)
-			goto out;
+			return ret_val;
 	}
 
-	/*
-	 * Need to parse both autoneg_advertised and fc and set up
+	/* Need to parse both autoneg_advertised and fc and set up
 	 * the appropriate PHY registers.  First we will parse for
 	 * autoneg_advertised software override.  Since we can advertise
 	 * a plethora of combinations, we need to check each bit
 	 * individually.
 	 */
 
-	/*
-	 * First we clear all the 10/100 mb speed bits in the Auto-Neg
+	/* First we clear all the 10/100 mb speed bits in the Auto-Neg
 	 * Advertisement Register (Address 4) and the 1000 mb speed bits in
 	 * the  1000Base-T Control Register (Address 9).
 	 */
@@ -909,44 +1421,43 @@ static s32 igb_phy_setup_autoneg(struct e1000_hw *hw)
 				 NWAY_AR_10T_HD_CAPS);
 	mii_1000t_ctrl_reg &= ~(CR_1000T_HD_CAPS | CR_1000T_FD_CAPS);
 
-	hw_dbg("autoneg_advertised %x\n", phy->autoneg_advertised);
+	DEBUGOUT1("autoneg_advertised %x\n", phy->autoneg_advertised);
 
 	/* Do we want to advertise 10 Mb Half Duplex? */
 	if (phy->autoneg_advertised & ADVERTISE_10_HALF) {
-		hw_dbg("Advertise 10mb Half duplex\n");
+		DEBUGOUT("Advertise 10mb Half duplex\n");
 		mii_autoneg_adv_reg |= NWAY_AR_10T_HD_CAPS;
 	}
 
 	/* Do we want to advertise 10 Mb Full Duplex? */
 	if (phy->autoneg_advertised & ADVERTISE_10_FULL) {
-		hw_dbg("Advertise 10mb Full duplex\n");
+		DEBUGOUT("Advertise 10mb Full duplex\n");
 		mii_autoneg_adv_reg |= NWAY_AR_10T_FD_CAPS;
 	}
 
 	/* Do we want to advertise 100 Mb Half Duplex? */
 	if (phy->autoneg_advertised & ADVERTISE_100_HALF) {
-		hw_dbg("Advertise 100mb Half duplex\n");
+		DEBUGOUT("Advertise 100mb Half duplex\n");
 		mii_autoneg_adv_reg |= NWAY_AR_100TX_HD_CAPS;
 	}
 
 	/* Do we want to advertise 100 Mb Full Duplex? */
 	if (phy->autoneg_advertised & ADVERTISE_100_FULL) {
-		hw_dbg("Advertise 100mb Full duplex\n");
+		DEBUGOUT("Advertise 100mb Full duplex\n");
 		mii_autoneg_adv_reg |= NWAY_AR_100TX_FD_CAPS;
 	}
 
 	/* We do not allow the Phy to advertise 1000 Mb Half Duplex */
 	if (phy->autoneg_advertised & ADVERTISE_1000_HALF)
-		hw_dbg("Advertise 1000mb Half duplex request denied!\n");
+		DEBUGOUT("Advertise 1000mb Half duplex request denied!\n");
 
 	/* Do we want to advertise 1000 Mb Full Duplex? */
 	if (phy->autoneg_advertised & ADVERTISE_1000_FULL) {
-		hw_dbg("Advertise 1000mb Full duplex\n");
+		DEBUGOUT("Advertise 1000mb Full duplex\n");
 		mii_1000t_ctrl_reg |= CR_1000T_FD_CAPS;
 	}
 
-	/*
-	 * Check for a software override of the flow control settings, and
+	/* Check for a software override of the flow control settings, and
 	 * setup the PHY advertisement registers accordingly.  If
 	 * auto-negotiation is enabled, then software will have to set the
 	 * "PAUSE" bits to the correct value in the Auto-Negotiation
@@ -959,72 +1470,126 @@ static s32 igb_phy_setup_autoneg(struct e1000_hw *hw)
 	 *          but not send pause frames).
 	 *      2:  Tx flow control is enabled (we can send pause frames
 	 *          but we do not support receiving pause frames).
-	 *      3:  Both Rx and TX flow control (symmetric) are enabled.
+	 *      3:  Both Rx and Tx flow control (symmetric) are enabled.
 	 *  other:  No software override.  The flow control configuration
 	 *          in the EEPROM is used.
 	 */
 	switch (hw->fc.current_mode) {
 	case e1000_fc_none:
-		/*
-		 * Flow control (RX & TX) is completely disabled by a
+		/* Flow control (Rx & Tx) is completely disabled by a
 		 * software over-ride.
 		 */
 		mii_autoneg_adv_reg &= ~(NWAY_AR_ASM_DIR | NWAY_AR_PAUSE);
 		break;
 	case e1000_fc_rx_pause:
-		/*
-		 * RX Flow control is enabled, and TX Flow control is
+		/* Rx Flow control is enabled, and Tx Flow control is
 		 * disabled, by a software over-ride.
 		 *
 		 * Since there really isn't a way to advertise that we are
-		 * capable of RX Pause ONLY, we will advertise that we
-		 * support both symmetric and asymmetric RX PAUSE.  Later
+		 * capable of Rx Pause ONLY, we will advertise that we
+		 * support both symmetric and asymmetric Rx PAUSE.  Later
 		 * (in e1000_config_fc_after_link_up) we will disable the
 		 * hw's ability to send PAUSE frames.
 		 */
 		mii_autoneg_adv_reg |= (NWAY_AR_ASM_DIR | NWAY_AR_PAUSE);
 		break;
 	case e1000_fc_tx_pause:
-		/*
-		 * TX Flow control is enabled, and RX Flow control is
+		/* Tx Flow control is enabled, and Rx Flow control is
 		 * disabled, by a software over-ride.
 		 */
 		mii_autoneg_adv_reg |= NWAY_AR_ASM_DIR;
 		mii_autoneg_adv_reg &= ~NWAY_AR_PAUSE;
 		break;
 	case e1000_fc_full:
-		/*
-		 * Flow control (both RX and TX) is enabled by a software
+		/* Flow control (both Rx and Tx) is enabled by a software
 		 * over-ride.
 		 */
 		mii_autoneg_adv_reg |= (NWAY_AR_ASM_DIR | NWAY_AR_PAUSE);
 		break;
 	default:
-		hw_dbg("Flow control param set incorrectly\n");
-		ret_val = -E1000_ERR_CONFIG;
-		goto out;
+		DEBUGOUT("Flow control param set incorrectly\n");
+		return -E1000_ERR_CONFIG;
 	}
 
 	ret_val = phy->ops.write_reg(hw, PHY_AUTONEG_ADV, mii_autoneg_adv_reg);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	hw_dbg("Auto-Neg Advertising %x\n", mii_autoneg_adv_reg);
+	DEBUGOUT1("Auto-Neg Advertising %x\n", mii_autoneg_adv_reg);
 
-	if (phy->autoneg_mask & ADVERTISE_1000_FULL) {
-		ret_val = phy->ops.write_reg(hw,
-					     PHY_1000T_CTRL,
+	if (phy->autoneg_mask & ADVERTISE_1000_FULL)
+		ret_val = phy->ops.write_reg(hw, PHY_1000T_CTRL,
 					     mii_1000t_ctrl_reg);
-		if (ret_val)
-			goto out;
+
+	return ret_val;
+}
+
+/**
+ *  e1000_copper_link_autoneg - Setup/Enable autoneg for copper link
+ *  @hw: pointer to the HW structure
+ *
+ *  Performs initial bounds checking on autoneg advertisement parameter, then
+ *  configure to advertise the full capability.  Setup the PHY to autoneg
+ *  and restart the negotiation process between the link partner.  If
+ *  autoneg_wait_to_complete, then wait for autoneg to complete before exiting.
+ **/
+static s32 e1000_copper_link_autoneg(struct e1000_hw *hw)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	s32 ret_val;
+	u16 phy_ctrl;
+
+	DEBUGFUNC("e1000_copper_link_autoneg");
+
+	/* Perform some bounds checking on the autoneg advertisement
+	 * parameter.
+	 */
+	phy->autoneg_advertised &= phy->autoneg_mask;
+
+	/* If autoneg_advertised is zero, we assume it was not defaulted
+	 * by the calling code so we set to advertise full capability.
+	 */
+	if (!phy->autoneg_advertised)
+		phy->autoneg_advertised = phy->autoneg_mask;
+
+	DEBUGOUT("Reconfiguring auto-neg advertisement params\n");
+	ret_val = e1000_phy_setup_autoneg(hw);
+	if (ret_val) {
+		DEBUGOUT("Error Setting up Auto-Negotiation\n");
+		return ret_val;
 	}
+	DEBUGOUT("Restarting Auto-Neg\n");
+
+	/* Restart auto-negotiation by setting the Auto Neg Enable bit and
+	 * the Auto Neg Restart bit in the PHY control register.
+	 */
+	ret_val = phy->ops.read_reg(hw, PHY_CONTROL, &phy_ctrl);
+	if (ret_val)
+		return ret_val;
+
+	phy_ctrl |= (MII_CR_AUTO_NEG_EN | MII_CR_RESTART_AUTO_NEG);
+	ret_val = phy->ops.write_reg(hw, PHY_CONTROL, phy_ctrl);
+	if (ret_val)
+		return ret_val;
+
+	/* Does the user want to wait for Auto-Neg to complete here, or
+	 * check at a later time (for example, callback routine).
+	 */
+	if (phy->autoneg_wait_to_complete) {
+		ret_val = e1000_wait_autoneg(hw);
+		if (ret_val) {
+			DEBUGOUT("Error while waiting for autoneg to complete\n");
+			return ret_val;
+		}
+	}
+
+	hw->mac.get_link_status = true;
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_setup_copper_link - Configure copper link settings
+ *  e1000_setup_copper_link_generic - Configure copper link settings
  *  @hw: pointer to the HW structure
  *
  *  Calls the appropriate function to configure the link for auto-neg or forced
@@ -1032,245 +1597,322 @@ out:
  *  to configure collision distance and flow control are called.  If link is
  *  not established, we return -E1000_ERR_PHY (-2).
  **/
-s32 igb_setup_copper_link(struct e1000_hw *hw)
+s32 e1000_setup_copper_link_generic(struct e1000_hw *hw)
 {
 	s32 ret_val;
 	bool link;
 
+	DEBUGFUNC("e1000_setup_copper_link_generic");
 
 	if (hw->mac.autoneg) {
-		/*
-		 * Setup autoneg and flow control advertisement and perform
+		/* Setup autoneg and flow control advertisement and perform
 		 * autonegotiation.
 		 */
-		ret_val = igb_copper_link_autoneg(hw);
+		ret_val = e1000_copper_link_autoneg(hw);
 		if (ret_val)
-			goto out;
+			return ret_val;
 	} else {
-		/*
-		 * PHY will be set to 10H, 10F, 100H or 100F
+		/* PHY will be set to 10H, 10F, 100H or 100F
 		 * depending on user settings.
 		 */
-		hw_dbg("Forcing Speed and Duplex\n");
+		DEBUGOUT("Forcing Speed and Duplex\n");
 		ret_val = hw->phy.ops.force_speed_duplex(hw);
 		if (ret_val) {
-			hw_dbg("Error Forcing Speed and Duplex\n");
-			goto out;
+			DEBUGOUT("Error Forcing Speed and Duplex\n");
+			return ret_val;
 		}
 	}
 
-	/*
-	 * Check link status. Wait up to 100 microseconds for link to become
+	/* Check link status. Wait up to 100 microseconds for link to become
 	 * valid.
 	 */
-	ret_val = igb_phy_has_link(hw,
-	                           COPPER_LINK_UP_LIMIT,
-	                           10,
-	                           &link);
+	ret_val = e1000_phy_has_link_generic(hw, COPPER_LINK_UP_LIMIT, 10,
+					     &link);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	if (link) {
-		hw_dbg("Valid link established!!!\n");
-		igb_config_collision_dist(hw);
-		ret_val = igb_config_fc_after_link_up(hw);
+		DEBUGOUT("Valid link established!!!\n");
+		hw->mac.ops.config_collision_dist(hw);
+		ret_val = e1000_config_fc_after_link_up_generic(hw);
 	} else {
-		hw_dbg("Unable to establish link!!!\n");
+		DEBUGOUT("Unable to establish link!!!\n");
 	}
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_phy_force_speed_duplex_igp - Force speed/duplex for igp PHY
+ *  e1000_phy_force_speed_duplex_igp - Force speed/duplex for igp PHY
  *  @hw: pointer to the HW structure
  *
  *  Calls the PHY setup function to force speed and duplex.  Clears the
  *  auto-crossover to force MDI manually.  Waits for link and returns
  *  successful if link up is successful, else -E1000_ERR_PHY (-2).
  **/
-s32 igb_phy_force_speed_duplex_igp(struct e1000_hw *hw)
+s32 e1000_phy_force_speed_duplex_igp(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 phy_data;
 	bool link;
 
+	DEBUGFUNC("e1000_phy_force_speed_duplex_igp");
+
 	ret_val = phy->ops.read_reg(hw, PHY_CONTROL, &phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	igb_phy_force_speed_duplex_setup(hw, &phy_data);
+	e1000_phy_force_speed_duplex_setup(hw, &phy_data);
 
 	ret_val = phy->ops.write_reg(hw, PHY_CONTROL, phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	/*
-	 * Clear Auto-Crossover to force MDI manually.  IGP requires MDI
+	/* Clear Auto-Crossover to force MDI manually.  IGP requires MDI
 	 * forced whenever speed and duplex are forced.
 	 */
 	ret_val = phy->ops.read_reg(hw, IGP01E1000_PHY_PORT_CTRL, &phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	phy_data &= ~IGP01E1000_PSCR_AUTO_MDIX;
 	phy_data &= ~IGP01E1000_PSCR_FORCE_MDI_MDIX;
 
 	ret_val = phy->ops.write_reg(hw, IGP01E1000_PHY_PORT_CTRL, phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	hw_dbg("IGP PSCR: %X\n", phy_data);
+	DEBUGOUT1("IGP PSCR: %X\n", phy_data);
 
-	udelay(1);
+	usec_delay(1);
 
 	if (phy->autoneg_wait_to_complete) {
-		hw_dbg("Waiting for forced speed/duplex link on IGP phy.\n");
+		DEBUGOUT("Waiting for forced speed/duplex link on IGP phy.\n");
 
-		ret_val = igb_phy_has_link(hw,
-						     PHY_FORCE_LIMIT,
-						     100000,
-						     &link);
+		ret_val = e1000_phy_has_link_generic(hw, PHY_FORCE_LIMIT,
+						     100000, &link);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		if (!link)
-			hw_dbg("Link taking longer than expected.\n");
+			DEBUGOUT("Link taking longer than expected.\n");
 
 		/* Try once more */
-		ret_val = igb_phy_has_link(hw,
-						     PHY_FORCE_LIMIT,
-						     100000,
-						     &link);
-		if (ret_val)
-			goto out;
+		ret_val = e1000_phy_has_link_generic(hw, PHY_FORCE_LIMIT,
+						     100000, &link);
 	}
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_phy_force_speed_duplex_m88 - Force speed/duplex for m88 PHY
+ *  e1000_phy_force_speed_duplex_m88 - Force speed/duplex for m88 PHY
  *  @hw: pointer to the HW structure
  *
  *  Calls the PHY setup function to force speed and duplex.  Clears the
  *  auto-crossover to force MDI manually.  Resets the PHY to commit the
  *  changes.  If time expires while waiting for link up, we reset the DSP.
- *  After reset, TX_CLK and CRS on TX must be set.  Return successful upon
+ *  After reset, TX_CLK and CRS on Tx must be set.  Return successful upon
  *  successful completion, else return corresponding error code.
  **/
-s32 igb_phy_force_speed_duplex_m88(struct e1000_hw *hw)
+s32 e1000_phy_force_speed_duplex_m88(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 phy_data;
 	bool link;
 
-	/*
-	 * Clear Auto-Crossover to force MDI manually.  M88E1000 requires MDI
-	 * forced whenever speed and duplex are forced.
-	 */
-	ret_val = phy->ops.read_reg(hw, M88E1000_PHY_SPEC_CTRL, &phy_data);
-	if (ret_val)
-		goto out;
+	DEBUGFUNC("e1000_phy_force_speed_duplex_m88");
 
-	phy_data &= ~M88E1000_PSCR_AUTO_X_MODE;
-	ret_val = phy->ops.write_reg(hw, M88E1000_PHY_SPEC_CTRL, phy_data);
-	if (ret_val)
-		goto out;
+	/* I210 and I211 devices support Auto-Crossover in forced operation. */
+	if (phy->type != e1000_phy_i210) {
+		/* Clear Auto-Crossover to force MDI manually.  M88E1000
+		 * requires MDI forced whenever speed and duplex are forced.
+		 */
+		ret_val = phy->ops.read_reg(hw, M88E1000_PHY_SPEC_CTRL,
+					    &phy_data);
+		if (ret_val)
+			return ret_val;
 
-	hw_dbg("M88E1000 PSCR: %X\n", phy_data);
+		phy_data &= ~M88E1000_PSCR_AUTO_X_MODE;
+		ret_val = phy->ops.write_reg(hw, M88E1000_PHY_SPEC_CTRL,
+					     phy_data);
+		if (ret_val)
+			return ret_val;
+	}
+
+	DEBUGOUT1("M88E1000 PSCR: %X\n", phy_data);
 
 	ret_val = phy->ops.read_reg(hw, PHY_CONTROL, &phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	igb_phy_force_speed_duplex_setup(hw, &phy_data);
+	e1000_phy_force_speed_duplex_setup(hw, &phy_data);
 
 	ret_val = phy->ops.write_reg(hw, PHY_CONTROL, phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	/* Reset the phy to commit changes. */
-	ret_val = igb_phy_sw_reset(hw);
+	ret_val = hw->phy.ops.commit(hw);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	if (phy->autoneg_wait_to_complete) {
-		hw_dbg("Waiting for forced speed/duplex link on M88 phy.\n");
+		DEBUGOUT("Waiting for forced speed/duplex link on M88 phy.\n");
 
-		ret_val = igb_phy_has_link(hw, PHY_FORCE_LIMIT, 100000, &link);
+		ret_val = e1000_phy_has_link_generic(hw, PHY_FORCE_LIMIT,
+						     100000, &link);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		if (!link) {
-			if (hw->phy.type != e1000_phy_m88 ||
-			    hw->phy.id == I347AT4_E_PHY_ID ||
-			    hw->phy.id == M88E1112_E_PHY_ID) {
-				hw_dbg("Link taking longer than expected.\n");
-			} else {
+			bool reset_dsp = true;
+
+			switch (hw->phy.id) {
+			case I347AT4_E_PHY_ID:
+			case M88E1340M_E_PHY_ID:
+			case M88E1112_E_PHY_ID:
+			case M88E1543_E_PHY_ID:
+			case M88E1512_E_PHY_ID:
+			case I210_I_PHY_ID:
+				reset_dsp = false;
+				break;
+			default:
+				if (hw->phy.type != e1000_phy_m88)
+					reset_dsp = false;
+				break;
+			}
 
-				/*
-				 * We didn't get link.
+			if (!reset_dsp) {
+				DEBUGOUT("Link taking longer than expected.\n");
+			} else {
+				/* We didn't get link.
 				 * Reset the DSP and cross our fingers.
 				 */
 				ret_val = phy->ops.write_reg(hw,
-							     M88E1000_PHY_PAGE_SELECT,
-							     0x001d);
+						M88E1000_PHY_PAGE_SELECT,
+						0x001d);
 				if (ret_val)
-					goto out;
-				ret_val = igb_phy_reset_dsp(hw);
+					return ret_val;
+				ret_val = e1000_phy_reset_dsp_generic(hw);
 				if (ret_val)
-					goto out;
+					return ret_val;
 			}
 		}
 
 		/* Try once more */
-		ret_val = igb_phy_has_link(hw, PHY_FORCE_LIMIT,
-					   100000, &link);
+		ret_val = e1000_phy_has_link_generic(hw, PHY_FORCE_LIMIT,
+						     100000, &link);
 		if (ret_val)
-			goto out;
+			return ret_val;
 	}
 
-	if (hw->phy.type != e1000_phy_m88 ||
-	    hw->phy.id == I347AT4_E_PHY_ID ||
-	    hw->phy.id == M88E1112_E_PHY_ID)
-		goto out;
+	if (hw->phy.type != e1000_phy_m88)
+		return E1000_SUCCESS;
 
+	if (hw->phy.id == I347AT4_E_PHY_ID ||
+		hw->phy.id == M88E1340M_E_PHY_ID ||
+		hw->phy.id == M88E1112_E_PHY_ID)
+		return E1000_SUCCESS;
+	if (hw->phy.id == I210_I_PHY_ID)
+		return E1000_SUCCESS;
+	if ((hw->phy.id == M88E1543_E_PHY_ID) ||
+	    (hw->phy.id == M88E1512_E_PHY_ID))
+		return E1000_SUCCESS;
 	ret_val = phy->ops.read_reg(hw, M88E1000_EXT_PHY_SPEC_CTRL, &phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	/*
-	 * Resetting the phy means we need to re-force TX_CLK in the
+	/* Resetting the phy means we need to re-force TX_CLK in the
 	 * Extended PHY Specific Control Register to 25MHz clock from
 	 * the reset value of 2.5MHz.
 	 */
 	phy_data |= M88E1000_EPSCR_TX_CLK_25;
 	ret_val = phy->ops.write_reg(hw, M88E1000_EXT_PHY_SPEC_CTRL, phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	/*
-	 * In addition, we must re-enable CRS on Tx for both half and full
+	/* In addition, we must re-enable CRS on Tx for both half and full
 	 * duplex.
 	 */
 	ret_val = phy->ops.read_reg(hw, M88E1000_PHY_SPEC_CTRL, &phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	phy_data |= M88E1000_PSCR_ASSERT_CRS_ON_TX;
 	ret_val = phy->ops.write_reg(hw, M88E1000_PHY_SPEC_CTRL, phy_data);
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_phy_force_speed_duplex_setup - Configure forced PHY speed/duplex
+ *  e1000_phy_force_speed_duplex_ife - Force PHY speed & duplex
+ *  @hw: pointer to the HW structure
+ *
+ *  Forces the speed and duplex settings of the PHY.
+ *  This is a function pointer entry point only called by
+ *  PHY setup routines.
+ **/
+s32 e1000_phy_force_speed_duplex_ife(struct e1000_hw *hw)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	s32 ret_val;
+	u16 data;
+	bool link;
+
+	DEBUGFUNC("e1000_phy_force_speed_duplex_ife");
+
+	ret_val = phy->ops.read_reg(hw, PHY_CONTROL, &data);
+	if (ret_val)
+		return ret_val;
+
+	e1000_phy_force_speed_duplex_setup(hw, &data);
+
+	ret_val = phy->ops.write_reg(hw, PHY_CONTROL, data);
+	if (ret_val)
+		return ret_val;
+
+	/* Disable MDI-X support for 10/100 */
+	ret_val = phy->ops.read_reg(hw, IFE_PHY_MDIX_CONTROL, &data);
+	if (ret_val)
+		return ret_val;
+
+	data &= ~IFE_PMC_AUTO_MDIX;
+	data &= ~IFE_PMC_FORCE_MDIX;
+
+	ret_val = phy->ops.write_reg(hw, IFE_PHY_MDIX_CONTROL, data);
+	if (ret_val)
+		return ret_val;
+
+	DEBUGOUT1("IFE PMC: %X\n", data);
+
+	usec_delay(1);
+
+	if (phy->autoneg_wait_to_complete) {
+		DEBUGOUT("Waiting for forced speed/duplex link on IFE phy.\n");
+
+		ret_val = e1000_phy_has_link_generic(hw, PHY_FORCE_LIMIT,
+						     100000, &link);
+		if (ret_val)
+			return ret_val;
+
+		if (!link)
+			DEBUGOUT("Link taking longer than expected.\n");
+
+		/* Try once more */
+		ret_val = e1000_phy_has_link_generic(hw, PHY_FORCE_LIMIT,
+						     100000, &link);
+		if (ret_val)
+			return ret_val;
+	}
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_phy_force_speed_duplex_setup - Configure forced PHY speed/duplex
  *  @hw: pointer to the HW structure
  *  @phy_ctrl: pointer to current value of PHY_CONTROL
  *
@@ -1281,17 +1923,18 @@ out:
  *  caller must write to the PHY_CONTROL register for these settings to
  *  take affect.
  **/
-static void igb_phy_force_speed_duplex_setup(struct e1000_hw *hw,
-					       u16 *phy_ctrl)
+void e1000_phy_force_speed_duplex_setup(struct e1000_hw *hw, u16 *phy_ctrl)
 {
 	struct e1000_mac_info *mac = &hw->mac;
 	u32 ctrl;
 
+	DEBUGFUNC("e1000_phy_force_speed_duplex_setup");
+
 	/* Turn off flow control when forcing speed/duplex */
 	hw->fc.current_mode = e1000_fc_none;
 
 	/* Force speed/duplex on the mac */
-	ctrl = rd32(E1000_CTRL);
+	ctrl = E1000_READ_REG(hw, E1000_CTRL);
 	ctrl |= (E1000_CTRL_FRCSPD | E1000_CTRL_FRCDPX);
 	ctrl &= ~E1000_CTRL_SPD_SEL;
 
@@ -1305,33 +1948,32 @@ static void igb_phy_force_speed_duplex_setup(struct e1000_hw *hw,
 	if (mac->forced_speed_duplex & E1000_ALL_HALF_DUPLEX) {
 		ctrl &= ~E1000_CTRL_FD;
 		*phy_ctrl &= ~MII_CR_FULL_DUPLEX;
-		hw_dbg("Half Duplex\n");
+		DEBUGOUT("Half Duplex\n");
 	} else {
 		ctrl |= E1000_CTRL_FD;
 		*phy_ctrl |= MII_CR_FULL_DUPLEX;
-		hw_dbg("Full Duplex\n");
+		DEBUGOUT("Full Duplex\n");
 	}
 
 	/* Forcing 10mb or 100mb? */
 	if (mac->forced_speed_duplex & E1000_ALL_100_SPEED) {
 		ctrl |= E1000_CTRL_SPD_100;
 		*phy_ctrl |= MII_CR_SPEED_100;
-		*phy_ctrl &= ~(MII_CR_SPEED_1000 | MII_CR_SPEED_10);
-		hw_dbg("Forcing 100mb\n");
+		*phy_ctrl &= ~MII_CR_SPEED_1000;
+		DEBUGOUT("Forcing 100mb\n");
 	} else {
 		ctrl &= ~(E1000_CTRL_SPD_1000 | E1000_CTRL_SPD_100);
-		*phy_ctrl |= MII_CR_SPEED_10;
 		*phy_ctrl &= ~(MII_CR_SPEED_1000 | MII_CR_SPEED_100);
-		hw_dbg("Forcing 10mb\n");
+		DEBUGOUT("Forcing 10mb\n");
 	}
 
-	igb_config_collision_dist(hw);
+	hw->mac.ops.config_collision_dist(hw);
 
-	wr32(E1000_CTRL, ctrl);
+	E1000_WRITE_REG(hw, E1000_CTRL, ctrl);
 }
 
 /**
- *  igb_set_d3_lplu_state - Sets low power link up state for D3
+ *  e1000_set_d3_lplu_state_generic - Sets low power link up state for D3
  *  @hw: pointer to the HW structure
  *  @active: boolean used to enable/disable lplu
  *
@@ -1344,27 +1986,28 @@ static void igb_phy_force_speed_duplex_setup(struct e1000_hw *hw,
  *  During driver activity, SmartSpeed should be enabled so performance is
  *  maintained.
  **/
-s32 igb_set_d3_lplu_state(struct e1000_hw *hw, bool active)
+s32 e1000_set_d3_lplu_state_generic(struct e1000_hw *hw, bool active)
 {
 	struct e1000_phy_info *phy = &hw->phy;
-	s32 ret_val = 0;
+	s32 ret_val;
 	u16 data;
 
-	if (!(hw->phy.ops.read_reg))
-		goto out;
+	DEBUGFUNC("e1000_set_d3_lplu_state_generic");
+
+	if (!hw->phy.ops.read_reg)
+		return E1000_SUCCESS;
 
 	ret_val = phy->ops.read_reg(hw, IGP02E1000_PHY_POWER_MGMT, &data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	if (!active) {
 		data &= ~IGP02E1000_PM_D3_LPLU;
 		ret_val = phy->ops.write_reg(hw, IGP02E1000_PHY_POWER_MGMT,
 					     data);
 		if (ret_val)
-			goto out;
-		/*
-		 * LPLU and SmartSpeed are mutually exclusive.  LPLU is used
+			return ret_val;
+		/* LPLU and SmartSpeed are mutually exclusive.  LPLU is used
 		 * during Dx states where the power conservation is most
 		 * important.  During driver activity we should enable
 		 * SmartSpeed, so performance is maintained.
@@ -1374,120 +2017,121 @@ s32 igb_set_d3_lplu_state(struct e1000_hw *hw, bool active)
 						    IGP01E1000_PHY_PORT_CONFIG,
 						    &data);
 			if (ret_val)
-				goto out;
+				return ret_val;
 
 			data |= IGP01E1000_PSCFR_SMART_SPEED;
 			ret_val = phy->ops.write_reg(hw,
 						     IGP01E1000_PHY_PORT_CONFIG,
 						     data);
 			if (ret_val)
-				goto out;
+				return ret_val;
 		} else if (phy->smart_speed == e1000_smart_speed_off) {
 			ret_val = phy->ops.read_reg(hw,
-						     IGP01E1000_PHY_PORT_CONFIG,
-						     &data);
+						    IGP01E1000_PHY_PORT_CONFIG,
+						    &data);
 			if (ret_val)
-				goto out;
+				return ret_val;
 
 			data &= ~IGP01E1000_PSCFR_SMART_SPEED;
 			ret_val = phy->ops.write_reg(hw,
 						     IGP01E1000_PHY_PORT_CONFIG,
 						     data);
 			if (ret_val)
-				goto out;
+				return ret_val;
 		}
 	} else if ((phy->autoneg_advertised == E1000_ALL_SPEED_DUPLEX) ||
 		   (phy->autoneg_advertised == E1000_ALL_NOT_GIG) ||
 		   (phy->autoneg_advertised == E1000_ALL_10_SPEED)) {
 		data |= IGP02E1000_PM_D3_LPLU;
 		ret_val = phy->ops.write_reg(hw, IGP02E1000_PHY_POWER_MGMT,
-					      data);
+					     data);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		/* When LPLU is enabled, we should disable SmartSpeed */
 		ret_val = phy->ops.read_reg(hw, IGP01E1000_PHY_PORT_CONFIG,
-					     &data);
+					    &data);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		data &= ~IGP01E1000_PSCFR_SMART_SPEED;
 		ret_val = phy->ops.write_reg(hw, IGP01E1000_PHY_PORT_CONFIG,
-					      data);
+					     data);
 	}
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_check_downshift - Checks whether a downshift in speed occurred
+ *  e1000_check_downshift_generic - Checks whether a downshift in speed occurred
  *  @hw: pointer to the HW structure
  *
  *  Success returns 0, Failure returns 1
  *
  *  A downshift is detected by querying the PHY link health.
  **/
-s32 igb_check_downshift(struct e1000_hw *hw)
+s32 e1000_check_downshift_generic(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 phy_data, offset, mask;
 
+	DEBUGFUNC("e1000_check_downshift_generic");
+
 	switch (phy->type) {
+	case e1000_phy_i210:
 	case e1000_phy_m88:
 	case e1000_phy_gg82563:
-		offset	= M88E1000_PHY_SPEC_STATUS;
-		mask	= M88E1000_PSSR_DOWNSHIFT;
+		offset = M88E1000_PHY_SPEC_STATUS;
+		mask = M88E1000_PSSR_DOWNSHIFT;
 		break;
 	case e1000_phy_igp_2:
-	case e1000_phy_igp:
 	case e1000_phy_igp_3:
-		offset	= IGP01E1000_PHY_LINK_HEALTH;
-		mask	= IGP01E1000_PLHR_SS_DOWNGRADE;
+		offset = IGP01E1000_PHY_LINK_HEALTH;
+		mask = IGP01E1000_PLHR_SS_DOWNGRADE;
 		break;
 	default:
 		/* speed downshift not supported */
 		phy->speed_downgraded = false;
-		ret_val = 0;
-		goto out;
+		return E1000_SUCCESS;
 	}
 
 	ret_val = phy->ops.read_reg(hw, offset, &phy_data);
 
 	if (!ret_val)
-		phy->speed_downgraded = (phy_data & mask) ? true : false;
+		phy->speed_downgraded = !!(phy_data & mask);
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_check_polarity_m88 - Checks the polarity.
+ *  e1000_check_polarity_m88 - Checks the polarity.
  *  @hw: pointer to the HW structure
  *
  *  Success returns 0, Failure returns -E1000_ERR_PHY (-2)
  *
  *  Polarity is determined based on the PHY specific status register.
  **/
-static s32 igb_check_polarity_m88(struct e1000_hw *hw)
+s32 e1000_check_polarity_m88(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 data;
 
+	DEBUGFUNC("e1000_check_polarity_m88");
+
 	ret_val = phy->ops.read_reg(hw, M88E1000_PHY_SPEC_STATUS, &data);
 
 	if (!ret_val)
-		phy->cable_polarity = (data & M88E1000_PSSR_REV_POLARITY)
-				      ? e1000_rev_polarity_reversed
-				      : e1000_rev_polarity_normal;
+		phy->cable_polarity = ((data & M88E1000_PSSR_REV_POLARITY)
+				       ? e1000_rev_polarity_reversed
+				       : e1000_rev_polarity_normal);
 
 	return ret_val;
 }
 
 /**
- *  igb_check_polarity_igp - Checks the polarity.
+ *  e1000_check_polarity_igp - Checks the polarity.
  *  @hw: pointer to the HW structure
  *
  *  Success returns 0, Failure returns -E1000_ERR_PHY (-2)
@@ -1495,56 +2139,94 @@ static s32 igb_check_polarity_m88(struct e1000_hw *hw)
  *  Polarity is determined based on the PHY port status register, and the
  *  current speed (since there is no polarity at 100Mbps).
  **/
-static s32 igb_check_polarity_igp(struct e1000_hw *hw)
+s32 e1000_check_polarity_igp(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 data, offset, mask;
 
-	/*
-	 * Polarity is determined based on the speed of
+	DEBUGFUNC("e1000_check_polarity_igp");
+
+	/* Polarity is determined based on the speed of
 	 * our connection.
 	 */
 	ret_val = phy->ops.read_reg(hw, IGP01E1000_PHY_PORT_STATUS, &data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	if ((data & IGP01E1000_PSSR_SPEED_MASK) ==
 	    IGP01E1000_PSSR_SPEED_1000MBPS) {
-		offset	= IGP01E1000_PHY_PCS_INIT_REG;
-		mask	= IGP01E1000_PHY_POLARITY_MASK;
+		offset = IGP01E1000_PHY_PCS_INIT_REG;
+		mask = IGP01E1000_PHY_POLARITY_MASK;
 	} else {
-		/*
-		 * This really only applies to 10Mbps since
+		/* This really only applies to 10Mbps since
 		 * there is no polarity for 100Mbps (always 0).
 		 */
-		offset	= IGP01E1000_PHY_PORT_STATUS;
-		mask	= IGP01E1000_PSSR_POLARITY_REVERSED;
+		offset = IGP01E1000_PHY_PORT_STATUS;
+		mask = IGP01E1000_PSSR_POLARITY_REVERSED;
 	}
 
 	ret_val = phy->ops.read_reg(hw, offset, &data);
 
 	if (!ret_val)
-		phy->cable_polarity = (data & mask)
-				      ? e1000_rev_polarity_reversed
-				      : e1000_rev_polarity_normal;
+		phy->cable_polarity = ((data & mask)
+				       ? e1000_rev_polarity_reversed
+				       : e1000_rev_polarity_normal);
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_wait_autoneg - Wait for auto-neg compeletion
+ *  e1000_check_polarity_ife - Check cable polarity for IFE PHY
+ *  @hw: pointer to the HW structure
+ *
+ *  Polarity is determined on the polarity reversal feature being enabled.
+ **/
+s32 e1000_check_polarity_ife(struct e1000_hw *hw)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	s32 ret_val;
+	u16 phy_data, offset, mask;
+
+	DEBUGFUNC("e1000_check_polarity_ife");
+
+	/* Polarity is determined based on the reversal feature being enabled.
+	 */
+	if (phy->polarity_correction) {
+		offset = IFE_PHY_EXTENDED_STATUS_CONTROL;
+		mask = IFE_PESC_POLARITY_REVERSED;
+	} else {
+		offset = IFE_PHY_SPECIAL_CONTROL;
+		mask = IFE_PSC_FORCE_POLARITY;
+	}
+
+	ret_val = phy->ops.read_reg(hw, offset, &phy_data);
+
+	if (!ret_val)
+		phy->cable_polarity = ((phy_data & mask)
+				       ? e1000_rev_polarity_reversed
+				       : e1000_rev_polarity_normal);
+
+	return ret_val;
+}
+
+/**
+ *  e1000_wait_autoneg - Wait for auto-neg completion
  *  @hw: pointer to the HW structure
  *
  *  Waits for auto-negotiation to complete or for the auto-negotiation time
  *  limit to expire, which ever happens first.
  **/
-static s32 igb_wait_autoneg(struct e1000_hw *hw)
+static s32 e1000_wait_autoneg(struct e1000_hw *hw)
 {
-	s32 ret_val = 0;
+	s32 ret_val = E1000_SUCCESS;
 	u16 i, phy_status;
 
+	DEBUGFUNC("e1000_wait_autoneg");
+
+	if (!hw->phy.ops.read_reg)
+		return E1000_SUCCESS;
+
 	/* Break after autoneg completes or PHY_AUTO_NEG_LIMIT expires. */
 	for (i = PHY_AUTO_NEG_LIMIT; i > 0; i--) {
 		ret_val = hw->phy.ops.read_reg(hw, PHY_STATUS, &phy_status);
@@ -1555,18 +2237,17 @@ static s32 igb_wait_autoneg(struct e1000_hw *hw)
 			break;
 		if (phy_status & MII_SR_AUTONEG_COMPLETE)
 			break;
-		msleep(100);
+		msec_delay(100);
 	}
 
-	/*
-	 * PHY_AUTO_NEG_TIME expiration doesn't guarantee auto-negotiation
+	/* PHY_AUTO_NEG_TIME expiration doesn't guarantee auto-negotiation
 	 * has completed.
 	 */
 	return ret_val;
 }
 
 /**
- *  igb_phy_has_link - Polls PHY for link
+ *  e1000_phy_has_link_generic - Polls PHY for link
  *  @hw: pointer to the HW structure
  *  @iterations: number of times to poll for link
  *  @usec_interval: delay between polling attempts
@@ -1574,26 +2255,32 @@ static s32 igb_wait_autoneg(struct e1000_hw *hw)
  *
  *  Polls the PHY status register for link, 'iterations' number of times.
  **/
-s32 igb_phy_has_link(struct e1000_hw *hw, u32 iterations,
+s32 e1000_phy_has_link_generic(struct e1000_hw *hw, u32 iterations,
 			       u32 usec_interval, bool *success)
 {
-	s32 ret_val = 0;
+	s32 ret_val = E1000_SUCCESS;
 	u16 i, phy_status;
 
+	DEBUGFUNC("e1000_phy_has_link_generic");
+
+	if (!hw->phy.ops.read_reg)
+		return E1000_SUCCESS;
+
 	for (i = 0; i < iterations; i++) {
-		/*
-		 * Some PHYs require the PHY_STATUS register to be read
+		/* Some PHYs require the PHY_STATUS register to be read
 		 * twice due to the link bit being sticky.  No harm doing
 		 * it across the board.
 		 */
 		ret_val = hw->phy.ops.read_reg(hw, PHY_STATUS, &phy_status);
 		if (ret_val) {
-			/*
-			 * If the first read fails, another entity may have
+			/* If the first read fails, another entity may have
 			 * ownership of the resources, wait and try again to
 			 * see if they have relinquished the resources yet.
 			 */
-			udelay(usec_interval);
+			if (usec_interval >= 1000)
+				msec_delay(usec_interval/1000);
+			else
+				usec_delay(usec_interval);
 		}
 		ret_val = hw->phy.ops.read_reg(hw, PHY_STATUS, &phy_status);
 		if (ret_val)
@@ -1601,18 +2288,18 @@ s32 igb_phy_has_link(struct e1000_hw *hw, u32 iterations,
 		if (phy_status & MII_SR_LINK_STATUS)
 			break;
 		if (usec_interval >= 1000)
-			mdelay(usec_interval/1000);
+			msec_delay(usec_interval/1000);
 		else
-			udelay(usec_interval);
+			usec_delay(usec_interval);
 	}
 
-	*success = (i < iterations) ? true : false;
+	*success = (i < iterations);
 
 	return ret_val;
 }
 
 /**
- *  igb_get_cable_length_m88 - Determine cable length for m88 PHY
+ *  e1000_get_cable_length_m88 - Determine cable length for m88 PHY
  *  @hw: pointer to the HW structure
  *
  *  Reads the PHY specific status register to retrieve the cable length
@@ -1626,96 +2313,123 @@ s32 igb_phy_has_link(struct e1000_hw *hw, u32 iterations,
  *	3			110 - 140 meters
  *	4			> 140 meters
  **/
-s32 igb_get_cable_length_m88(struct e1000_hw *hw)
+s32 e1000_get_cable_length_m88(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 phy_data, index;
 
+	DEBUGFUNC("e1000_get_cable_length_m88");
+
 	ret_val = phy->ops.read_reg(hw, M88E1000_PHY_SPEC_STATUS, &phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	index = (phy_data & M88E1000_PSSR_CABLE_LENGTH) >>
-		M88E1000_PSSR_CABLE_LENGTH_SHIFT;
-	if (index >= M88E1000_CABLE_LENGTH_TABLE_SIZE - 1) {
-		ret_val = -E1000_ERR_PHY;
-		goto out;
-	}
+	index = ((phy_data & M88E1000_PSSR_CABLE_LENGTH) >>
+		 M88E1000_PSSR_CABLE_LENGTH_SHIFT);
+
+	if (index >= M88E1000_CABLE_LENGTH_TABLE_SIZE - 1)
+		return -E1000_ERR_PHY;
 
 	phy->min_cable_length = e1000_m88_cable_length_table[index];
 	phy->max_cable_length = e1000_m88_cable_length_table[index + 1];
 
 	phy->cable_length = (phy->min_cable_length + phy->max_cable_length) / 2;
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
-s32 igb_get_cable_length_m88_gen2(struct e1000_hw *hw)
+s32 e1000_get_cable_length_m88_gen2(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
-	u16 phy_data, phy_data2, index, default_page, is_cm;
+	u16 phy_data, phy_data2, is_cm;
+	u16 index, default_page;
+
+	DEBUGFUNC("e1000_get_cable_length_m88_gen2");
 
 	switch (hw->phy.id) {
+	case I210_I_PHY_ID:
+		/* Get cable length from PHY Cable Diagnostics Control Reg */
+		ret_val = phy->ops.read_reg(hw, (0x7 << GS40G_PAGE_SHIFT) +
+					    (I347AT4_PCDL + phy->addr),
+					    &phy_data);
+		if (ret_val)
+			return ret_val;
+
+		/* Check if the unit of cable length is meters or cm */
+		ret_val = phy->ops.read_reg(hw, (0x7 << GS40G_PAGE_SHIFT) +
+					    I347AT4_PCDC, &phy_data2);
+		if (ret_val)
+			return ret_val;
+
+		is_cm = !(phy_data2 & I347AT4_PCDC_CABLE_LENGTH_UNIT);
+
+		/* Populate the phy structure with cable length in meters */
+		phy->min_cable_length = phy_data / (is_cm ? 100 : 1);
+		phy->max_cable_length = phy_data / (is_cm ? 100 : 1);
+		phy->cable_length = phy_data / (is_cm ? 100 : 1);
+		break;
+	case M88E1543_E_PHY_ID:
+	case M88E1512_E_PHY_ID:
+	case M88E1340M_E_PHY_ID:
 	case I347AT4_E_PHY_ID:
 		/* Remember the original page select and set it to 7 */
 		ret_val = phy->ops.read_reg(hw, I347AT4_PAGE_SELECT,
 					    &default_page);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		ret_val = phy->ops.write_reg(hw, I347AT4_PAGE_SELECT, 0x07);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		/* Get cable length from PHY Cable Diagnostics Control Reg */
 		ret_val = phy->ops.read_reg(hw, (I347AT4_PCDL + phy->addr),
 					    &phy_data);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		/* Check if the unit of cable length is meters or cm */
 		ret_val = phy->ops.read_reg(hw, I347AT4_PCDC, &phy_data2);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
-		is_cm = !(phy_data & I347AT4_PCDC_CABLE_LENGTH_UNIT);
+		is_cm = !(phy_data2 & I347AT4_PCDC_CABLE_LENGTH_UNIT);
 
 		/* Populate the phy structure with cable length in meters */
 		phy->min_cable_length = phy_data / (is_cm ? 100 : 1);
 		phy->max_cable_length = phy_data / (is_cm ? 100 : 1);
 		phy->cable_length = phy_data / (is_cm ? 100 : 1);
 
-		/* Reset the page selec to its original value */
+		/* Reset the page select to its original value */
 		ret_val = phy->ops.write_reg(hw, I347AT4_PAGE_SELECT,
 					     default_page);
 		if (ret_val)
-			goto out;
+			return ret_val;
 		break;
+
 	case M88E1112_E_PHY_ID:
 		/* Remember the original page select and set it to 5 */
 		ret_val = phy->ops.read_reg(hw, I347AT4_PAGE_SELECT,
 					    &default_page);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		ret_val = phy->ops.write_reg(hw, I347AT4_PAGE_SELECT, 0x05);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		ret_val = phy->ops.read_reg(hw, M88E1112_VCT_DSP_DISTANCE,
 					    &phy_data);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		index = (phy_data & M88E1000_PSSR_CABLE_LENGTH) >>
 			M88E1000_PSSR_CABLE_LENGTH_SHIFT;
-		if (index >= M88E1000_CABLE_LENGTH_TABLE_SIZE - 1) {
-			ret_val = -E1000_ERR_PHY;
-			goto out;
-		}
+
+		if (index >= M88E1000_CABLE_LENGTH_TABLE_SIZE - 1)
+			return -E1000_ERR_PHY;
 
 		phy->min_cable_length = e1000_m88_cable_length_table[index];
 		phy->max_cable_length = e1000_m88_cable_length_table[index + 1];
@@ -1727,20 +2441,18 @@ s32 igb_get_cable_length_m88_gen2(struct e1000_hw *hw)
 		ret_val = phy->ops.write_reg(hw, I347AT4_PAGE_SELECT,
 					     default_page);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		break;
 	default:
-		ret_val = -E1000_ERR_PHY;
-		goto out;
+		return -E1000_ERR_PHY;
 	}
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_get_cable_length_igp_2 - Determine cable length for igp2 PHY
+ *  e1000_get_cable_length_igp_2 - Determine cable length for igp2 PHY
  *  @hw: pointer to the HW structure
  *
  *  The automatic gain control (agc) normalizes the amplitude of the
@@ -1750,41 +2462,40 @@ out:
  *  into a lookup table to obtain the approximate cable length
  *  for each channel.
  **/
-s32 igb_get_cable_length_igp_2(struct e1000_hw *hw)
+s32 e1000_get_cable_length_igp_2(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
-	s32 ret_val = 0;
+	s32 ret_val;
 	u16 phy_data, i, agc_value = 0;
 	u16 cur_agc_index, max_agc_index = 0;
 	u16 min_agc_index = IGP02E1000_CABLE_LENGTH_TABLE_SIZE - 1;
 	static const u16 agc_reg_array[IGP02E1000_PHY_CHANNEL_NUM] = {
-	       IGP02E1000_PHY_AGC_A,
-	       IGP02E1000_PHY_AGC_B,
-	       IGP02E1000_PHY_AGC_C,
-	       IGP02E1000_PHY_AGC_D
+		IGP02E1000_PHY_AGC_A,
+		IGP02E1000_PHY_AGC_B,
+		IGP02E1000_PHY_AGC_C,
+		IGP02E1000_PHY_AGC_D
 	};
 
+	DEBUGFUNC("e1000_get_cable_length_igp_2");
+
 	/* Read the AGC registers for all channels */
 	for (i = 0; i < IGP02E1000_PHY_CHANNEL_NUM; i++) {
 		ret_val = phy->ops.read_reg(hw, agc_reg_array[i], &phy_data);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
-		/*
-		 * Getting bits 15:9, which represent the combination of
+		/* Getting bits 15:9, which represent the combination of
 		 * coarse and fine gain values.  The result is a number
 		 * that can be put into the lookup table to obtain the
 		 * approximate cable length.
 		 */
-		cur_agc_index = (phy_data >> IGP02E1000_AGC_LENGTH_SHIFT) &
-				IGP02E1000_AGC_LENGTH_MASK;
+		cur_agc_index = ((phy_data >> IGP02E1000_AGC_LENGTH_SHIFT) &
+				 IGP02E1000_AGC_LENGTH_MASK);
 
 		/* Array index bound check. */
 		if ((cur_agc_index >= IGP02E1000_CABLE_LENGTH_TABLE_SIZE) ||
-		    (cur_agc_index == 0)) {
-			ret_val = -E1000_ERR_PHY;
-			goto out;
-		}
+		    (cur_agc_index == 0))
+			return -E1000_ERR_PHY;
 
 		/* Remove min & max AGC values from calculation. */
 		if (e1000_igp_2_cable_length_table[min_agc_index] >
@@ -1802,18 +2513,17 @@ s32 igb_get_cable_length_igp_2(struct e1000_hw *hw)
 	agc_value /= (IGP02E1000_PHY_CHANNEL_NUM - 2);
 
 	/* Calculate cable length with the error range of +/- 10 meters. */
-	phy->min_cable_length = ((agc_value - IGP02E1000_AGC_RANGE) > 0) ?
-				 (agc_value - IGP02E1000_AGC_RANGE) : 0;
+	phy->min_cable_length = (((agc_value - IGP02E1000_AGC_RANGE) > 0) ?
+				 (agc_value - IGP02E1000_AGC_RANGE) : 0);
 	phy->max_cable_length = agc_value + IGP02E1000_AGC_RANGE;
 
 	phy->cable_length = (phy->min_cable_length + phy->max_cable_length) / 2;
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_get_phy_info_m88 - Retrieve PHY information
+ *  e1000_get_phy_info_m88 - Retrieve PHY information
  *  @hw: pointer to the HW structure
  *
  *  Valid for only copper links.  Read the PHY status register (sticky read)
@@ -1822,54 +2532,54 @@ out:
  *  special status register to determine MDI/MDIx and current speed.  If
  *  speed is 1000, then determine cable length, local and remote receiver.
  **/
-s32 igb_get_phy_info_m88(struct e1000_hw *hw)
+s32 e1000_get_phy_info_m88(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32  ret_val;
 	u16 phy_data;
 	bool link;
 
+	DEBUGFUNC("e1000_get_phy_info_m88");
+
 	if (phy->media_type != e1000_media_type_copper) {
-		hw_dbg("Phy info is only valid for copper media\n");
-		ret_val = -E1000_ERR_CONFIG;
-		goto out;
+		DEBUGOUT("Phy info is only valid for copper media\n");
+		return -E1000_ERR_CONFIG;
 	}
 
-	ret_val = igb_phy_has_link(hw, 1, 0, &link);
+	ret_val = e1000_phy_has_link_generic(hw, 1, 0, &link);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	if (!link) {
-		hw_dbg("Phy info is only valid if link is up\n");
-		ret_val = -E1000_ERR_CONFIG;
-		goto out;
+		DEBUGOUT("Phy info is only valid if link is up\n");
+		return -E1000_ERR_CONFIG;
 	}
 
 	ret_val = phy->ops.read_reg(hw, M88E1000_PHY_SPEC_CTRL, &phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	phy->polarity_correction = (phy_data & M88E1000_PSCR_POLARITY_REVERSAL)
-				   ? true : false;
+	phy->polarity_correction = !!(phy_data &
+				      M88E1000_PSCR_POLARITY_REVERSAL);
 
-	ret_val = igb_check_polarity_m88(hw);
+	ret_val = e1000_check_polarity_m88(hw);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	ret_val = phy->ops.read_reg(hw, M88E1000_PHY_SPEC_STATUS, &phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	phy->is_mdix = (phy_data & M88E1000_PSSR_MDIX) ? true : false;
+	phy->is_mdix = !!(phy_data & M88E1000_PSSR_MDIX);
 
 	if ((phy_data & M88E1000_PSSR_SPEED) == M88E1000_PSSR_1000MBS) {
-		ret_val = phy->ops.get_cable_length(hw);
+		ret_val = hw->phy.ops.get_cable_length(hw);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		ret_val = phy->ops.read_reg(hw, PHY_1000T_STATUS, &phy_data);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		phy->local_rx = (phy_data & SR_1000T_LOCAL_RX_STATUS)
 				? e1000_1000t_rx_status_ok
@@ -1885,12 +2595,11 @@ s32 igb_get_phy_info_m88(struct e1000_hw *hw)
 		phy->remote_rx = e1000_1000t_rx_status_undefined;
 	}
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_get_phy_info_igp - Retrieve igp PHY information
+ *  e1000_get_phy_info_igp - Retrieve igp PHY information
  *  @hw: pointer to the HW structure
  *
  *  Read PHY status to determine if link is up.  If link is up, then
@@ -1898,44 +2607,45 @@ out:
  *  PHY port status to determine MDI/MDIx and speed.  Based on the speed,
  *  determine on the cable length, local and remote receiver.
  **/
-s32 igb_get_phy_info_igp(struct e1000_hw *hw)
+s32 e1000_get_phy_info_igp(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 data;
 	bool link;
 
-	ret_val = igb_phy_has_link(hw, 1, 0, &link);
+	DEBUGFUNC("e1000_get_phy_info_igp");
+
+	ret_val = e1000_phy_has_link_generic(hw, 1, 0, &link);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	if (!link) {
-		hw_dbg("Phy info is only valid if link is up\n");
-		ret_val = -E1000_ERR_CONFIG;
-		goto out;
+		DEBUGOUT("Phy info is only valid if link is up\n");
+		return -E1000_ERR_CONFIG;
 	}
 
 	phy->polarity_correction = true;
 
-	ret_val = igb_check_polarity_igp(hw);
+	ret_val = e1000_check_polarity_igp(hw);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	ret_val = phy->ops.read_reg(hw, IGP01E1000_PHY_PORT_STATUS, &data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	phy->is_mdix = (data & IGP01E1000_PSSR_MDIX) ? true : false;
+	phy->is_mdix = !!(data & IGP01E1000_PSSR_MDIX);
 
 	if ((data & IGP01E1000_PSSR_SPEED_MASK) ==
 	    IGP01E1000_PSSR_SPEED_1000MBPS) {
 		ret_val = phy->ops.get_cable_length(hw);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		ret_val = phy->ops.read_reg(hw, PHY_1000T_STATUS, &data);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		phy->local_rx = (data & SR_1000T_LOCAL_RX_STATUS)
 				? e1000_1000t_rx_status_ok
@@ -1950,93 +2660,162 @@ s32 igb_get_phy_info_igp(struct e1000_hw *hw)
 		phy->remote_rx = e1000_1000t_rx_status_undefined;
 	}
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_phy_sw_reset - PHY software reset
+ *  e1000_get_phy_info_ife - Retrieves various IFE PHY states
+ *  @hw: pointer to the HW structure
+ *
+ *  Populates "phy" structure with various feature states.
+ **/
+s32 e1000_get_phy_info_ife(struct e1000_hw *hw)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	s32 ret_val;
+	u16 data;
+	bool link;
+
+	DEBUGFUNC("e1000_get_phy_info_ife");
+
+	ret_val = e1000_phy_has_link_generic(hw, 1, 0, &link);
+	if (ret_val)
+		return ret_val;
+
+	if (!link) {
+		DEBUGOUT("Phy info is only valid if link is up\n");
+		return -E1000_ERR_CONFIG;
+	}
+
+	ret_val = phy->ops.read_reg(hw, IFE_PHY_SPECIAL_CONTROL, &data);
+	if (ret_val)
+		return ret_val;
+	phy->polarity_correction = !(data & IFE_PSC_AUTO_POLARITY_DISABLE);
+
+	if (phy->polarity_correction) {
+		ret_val = e1000_check_polarity_ife(hw);
+		if (ret_val)
+			return ret_val;
+	} else {
+		/* Polarity is forced */
+		phy->cable_polarity = ((data & IFE_PSC_FORCE_POLARITY)
+				       ? e1000_rev_polarity_reversed
+				       : e1000_rev_polarity_normal);
+	}
+
+	ret_val = phy->ops.read_reg(hw, IFE_PHY_MDIX_CONTROL, &data);
+	if (ret_val)
+		return ret_val;
+
+	phy->is_mdix = !!(data & IFE_PMC_MDIX_STATUS);
+
+	/* The following parameters are undefined for 10/100 operation. */
+	phy->cable_length = E1000_CABLE_LENGTH_UNDEFINED;
+	phy->local_rx = e1000_1000t_rx_status_undefined;
+	phy->remote_rx = e1000_1000t_rx_status_undefined;
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_phy_sw_reset_generic - PHY software reset
  *  @hw: pointer to the HW structure
  *
  *  Does a software reset of the PHY by reading the PHY control register and
  *  setting/write the control register reset bit to the PHY.
  **/
-s32 igb_phy_sw_reset(struct e1000_hw *hw)
+s32 e1000_phy_sw_reset_generic(struct e1000_hw *hw)
 {
-	s32 ret_val = 0;
+	s32 ret_val;
 	u16 phy_ctrl;
 
-	if (!(hw->phy.ops.read_reg))
-		goto out;
+	DEBUGFUNC("e1000_phy_sw_reset_generic");
+
+	if (!hw->phy.ops.read_reg)
+		return E1000_SUCCESS;
 
 	ret_val = hw->phy.ops.read_reg(hw, PHY_CONTROL, &phy_ctrl);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	phy_ctrl |= MII_CR_RESET;
 	ret_val = hw->phy.ops.write_reg(hw, PHY_CONTROL, phy_ctrl);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	udelay(1);
+	usec_delay(1);
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_phy_hw_reset - PHY hardware reset
+ *  e1000_phy_hw_reset_generic - PHY hardware reset
  *  @hw: pointer to the HW structure
  *
  *  Verify the reset block is not blocking us from resetting.  Acquire
  *  semaphore (if necessary) and read/set/write the device control reset
  *  bit in the PHY.  Wait the appropriate delay time for the device to
- *  reset and relase the semaphore (if necessary).
+ *  reset and release the semaphore (if necessary).
  **/
-s32 igb_phy_hw_reset(struct e1000_hw *hw)
+s32 e1000_phy_hw_reset_generic(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
-	s32  ret_val;
+	s32 ret_val;
 	u32 ctrl;
 
-	ret_val = igb_check_reset_block(hw);
-	if (ret_val) {
-		ret_val = 0;
-		goto out;
+	DEBUGFUNC("e1000_phy_hw_reset_generic");
+
+	if (phy->ops.check_reset_block) {
+		ret_val = phy->ops.check_reset_block(hw);
+		if (ret_val)
+			return E1000_SUCCESS;
 	}
 
 	ret_val = phy->ops.acquire(hw);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	ctrl = rd32(E1000_CTRL);
-	wr32(E1000_CTRL, ctrl | E1000_CTRL_PHY_RST);
-	wrfl();
+	ctrl = E1000_READ_REG(hw, E1000_CTRL);
+	E1000_WRITE_REG(hw, E1000_CTRL, ctrl | E1000_CTRL_PHY_RST);
+	E1000_WRITE_FLUSH(hw);
 
-	udelay(phy->reset_delay_us);
+	usec_delay(phy->reset_delay_us);
 
-	wr32(E1000_CTRL, ctrl);
-	wrfl();
+	E1000_WRITE_REG(hw, E1000_CTRL, ctrl);
+	E1000_WRITE_FLUSH(hw);
 
-	udelay(150);
+	usec_delay(150);
 
 	phy->ops.release(hw);
 
-	ret_val = phy->ops.get_cfg_done(hw);
+	return phy->ops.get_cfg_done(hw);
+}
 
-out:
-	return ret_val;
+/**
+ *  e1000_get_cfg_done_generic - Generic configuration done
+ *  @hw: pointer to the HW structure
+ *
+ *  Generic function to wait 10 milli-seconds for configuration to complete
+ *  and return success.
+ **/
+s32 e1000_get_cfg_done_generic(struct e1000_hw E1000_UNUSEDARG *hw)
+{
+	DEBUGFUNC("e1000_get_cfg_done_generic");
+
+	msec_delay_irq(10);
+
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_phy_init_script_igp3 - Inits the IGP3 PHY
+ *  e1000_phy_init_script_igp3 - Inits the IGP3 PHY
  *  @hw: pointer to the HW structure
  *
  *  Initializes a Intel Gigabit PHY3 when an EEPROM is not present.
  **/
-s32 igb_phy_init_script_igp3(struct e1000_hw *hw)
+s32 e1000_phy_init_script_igp3(struct e1000_hw *hw)
 {
-	hw_dbg("Running IGP 3 PHY init script\n");
+	DEBUGOUT("Running IGP 3 PHY init script\n");
 
 	/* PHY init IGP 3 */
 	/* Enable rise/fall, 10-mode work in class-A */
@@ -2047,7 +2826,7 @@ s32 igb_phy_init_script_igp3(struct e1000_hw *hw)
 	hw->phy.ops.write_reg(hw, 0x2FB1, 0x8B24);
 	/* Increase Hybrid poly bias */
 	hw->phy.ops.write_reg(hw, 0x2FB2, 0xF8F0);
-	/* Add 4% to TX amplitude in Giga mode */
+	/* Add 4% to Tx amplitude in Gig mode */
 	hw->phy.ops.write_reg(hw, 0x2010, 0x10B0);
 	/* Disable trimming (TTT) */
 	hw->phy.ops.write_reg(hw, 0x2011, 0x0000);
@@ -2091,15 +2870,13 @@ s32 igb_phy_init_script_igp3(struct e1000_hw *hw)
 	hw->phy.ops.write_reg(hw, 0x1796, 0x0008);
 	/* Change cg_icount + enable integbp for channels BCD */
 	hw->phy.ops.write_reg(hw, 0x1798, 0xD008);
-	/*
-	 * Change cg_icount + enable integbp + change prop_factor_master
+	/* Change cg_icount + enable integbp + change prop_factor_master
 	 * to 8 for channel A
 	 */
 	hw->phy.ops.write_reg(hw, 0x1898, 0xD918);
 	/* Disable AHT in Slave mode on channel A */
 	hw->phy.ops.write_reg(hw, 0x187A, 0x0800);
-	/*
-	 * Enable LPLU and disable AN to 1000 in non-D0a states,
+	/* Enable LPLU and disable AN to 1000 in non-D0a states,
 	 * Enable SPD+B2B
 	 */
 	hw->phy.ops.write_reg(hw, 0x0019, 0x008D);
@@ -2110,142 +2887,220 @@ s32 igb_phy_init_script_igp3(struct e1000_hw *hw)
 	/* Restart AN, Speed selection is 1000 */
 	hw->phy.ops.write_reg(hw, 0x0000, 0x1340);
 
-	return 0;
+	return E1000_SUCCESS;
 }
 
 /**
- * igb_power_up_phy_copper - Restore copper link in case of PHY power down
+ *  e1000_get_phy_type_from_id - Get PHY type from id
+ *  @phy_id: phy_id read from the phy
+ *
+ *  Returns the phy type from the id.
+ **/
+enum e1000_phy_type e1000_get_phy_type_from_id(u32 phy_id)
+{
+	enum e1000_phy_type phy_type = e1000_phy_unknown;
+
+	switch (phy_id) {
+	case M88E1000_I_PHY_ID:
+	case M88E1000_E_PHY_ID:
+	case M88E1111_I_PHY_ID:
+	case M88E1011_I_PHY_ID:
+	case M88E1543_E_PHY_ID:
+	case M88E1512_E_PHY_ID:
+	case I347AT4_E_PHY_ID:
+	case M88E1112_E_PHY_ID:
+	case M88E1340M_E_PHY_ID:
+		phy_type = e1000_phy_m88;
+		break;
+	case IGP01E1000_I_PHY_ID: /* IGP 1 & 2 share this */
+		phy_type = e1000_phy_igp_2;
+		break;
+	case GG82563_E_PHY_ID:
+		phy_type = e1000_phy_gg82563;
+		break;
+	case IGP03E1000_E_PHY_ID:
+		phy_type = e1000_phy_igp_3;
+		break;
+	case IFE_E_PHY_ID:
+	case IFE_PLUS_E_PHY_ID:
+	case IFE_C_E_PHY_ID:
+		phy_type = e1000_phy_ife;
+		break;
+	case I82580_I_PHY_ID:
+		phy_type = e1000_phy_82580;
+		break;
+	case I210_I_PHY_ID:
+		phy_type = e1000_phy_i210;
+		break;
+	default:
+		phy_type = e1000_phy_unknown;
+		break;
+	}
+	return phy_type;
+}
+
+/**
+ *  e1000_determine_phy_address - Determines PHY address.
+ *  @hw: pointer to the HW structure
+ *
+ *  This uses a trial and error method to loop through possible PHY
+ *  addresses. It tests each by reading the PHY ID registers and
+ *  checking for a match.
+ **/
+s32 e1000_determine_phy_address(struct e1000_hw *hw)
+{
+	u32 phy_addr = 0;
+	u32 i;
+	enum e1000_phy_type phy_type = e1000_phy_unknown;
+
+	hw->phy.id = phy_type;
+
+	for (phy_addr = 0; phy_addr < E1000_MAX_PHY_ADDR; phy_addr++) {
+		hw->phy.addr = phy_addr;
+		i = 0;
+
+		do {
+			e1000_get_phy_id(hw);
+			phy_type = e1000_get_phy_type_from_id(hw->phy.id);
+
+			/* If phy_type is valid, break - we found our
+			 * PHY address
+			 */
+			if (phy_type != e1000_phy_unknown)
+				return E1000_SUCCESS;
+
+			msec_delay(1);
+			i++;
+		} while (i < 10);
+	}
+
+	return -E1000_ERR_PHY_TYPE;
+}
+
+/**
+ * e1000_power_up_phy_copper - Restore copper link in case of PHY power down
  * @hw: pointer to the HW structure
  *
  * In the case of a PHY power down to save power, or to turn off link during a
- * driver unload, restore the link to previous settings.
+ * driver unload, or wake on lan is not enabled, restore the link to previous
+ * settings.
  **/
-void igb_power_up_phy_copper(struct e1000_hw *hw)
+void e1000_power_up_phy_copper(struct e1000_hw *hw)
 {
 	u16 mii_reg = 0;
+	u16 power_reg = 0;
 
 	/* The PHY will retain its settings across a power down/up cycle */
 	hw->phy.ops.read_reg(hw, PHY_CONTROL, &mii_reg);
 	mii_reg &= ~MII_CR_POWER_DOWN;
+	if (hw->phy.type == e1000_phy_i210) {
+		hw->phy.ops.read_reg(hw, GS40G_COPPER_SPEC, &power_reg);
+		power_reg &= ~GS40G_CS_POWER_DOWN;
+		hw->phy.ops.write_reg(hw, GS40G_COPPER_SPEC, power_reg);
+	}
 	hw->phy.ops.write_reg(hw, PHY_CONTROL, mii_reg);
 }
 
 /**
- * igb_power_down_phy_copper - Power down copper PHY
+ * e1000_power_down_phy_copper - Restore copper link in case of PHY power down
  * @hw: pointer to the HW structure
  *
- * Power down PHY to save power when interface is down and wake on lan
- * is not enabled.
+ * In the case of a PHY power down to save power, or to turn off link during a
+ * driver unload, or wake on lan is not enabled, restore the link to previous
+ * settings.
  **/
-void igb_power_down_phy_copper(struct e1000_hw *hw)
+void e1000_power_down_phy_copper(struct e1000_hw *hw)
 {
 	u16 mii_reg = 0;
+	u16 power_reg = 0;
 
 	/* The PHY will retain its settings across a power down/up cycle */
 	hw->phy.ops.read_reg(hw, PHY_CONTROL, &mii_reg);
 	mii_reg |= MII_CR_POWER_DOWN;
+	/* i210 Phy requires an additional bit for power up/down */
+	if (hw->phy.type == e1000_phy_i210) {
+		hw->phy.ops.read_reg(hw, GS40G_COPPER_SPEC, &power_reg);
+		power_reg |= GS40G_CS_POWER_DOWN;
+		hw->phy.ops.write_reg(hw, GS40G_COPPER_SPEC, power_reg);
+	}
 	hw->phy.ops.write_reg(hw, PHY_CONTROL, mii_reg);
-	msleep(1);
+	msec_delay(1);
 }
 
 /**
- *  igb_check_polarity_82580 - Checks the polarity.
+ *  e1000_check_polarity_82577 - Checks the polarity.
  *  @hw: pointer to the HW structure
  *
  *  Success returns 0, Failure returns -E1000_ERR_PHY (-2)
  *
  *  Polarity is determined based on the PHY specific status register.
  **/
-static s32 igb_check_polarity_82580(struct e1000_hw *hw)
+s32 e1000_check_polarity_82577(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 data;
 
+	DEBUGFUNC("e1000_check_polarity_82577");
 
-	ret_val = phy->ops.read_reg(hw, I82580_PHY_STATUS_2, &data);
+	ret_val = phy->ops.read_reg(hw, I82577_PHY_STATUS_2, &data);
 
 	if (!ret_val)
-		phy->cable_polarity = (data & I82580_PHY_STATUS2_REV_POLARITY)
-		                      ? e1000_rev_polarity_reversed
-		                      : e1000_rev_polarity_normal;
+		phy->cable_polarity = ((data & I82577_PHY_STATUS2_REV_POLARITY)
+				       ? e1000_rev_polarity_reversed
+				       : e1000_rev_polarity_normal);
 
 	return ret_val;
 }
 
 /**
- *  igb_phy_force_speed_duplex_82580 - Force speed/duplex for I82580 PHY
+ *  e1000_phy_force_speed_duplex_82577 - Force speed/duplex for I82577 PHY
  *  @hw: pointer to the HW structure
  *
- *  Calls the PHY setup function to force speed and duplex.  Clears the
- *  auto-crossover to force MDI manually.  Waits for link and returns
- *  successful if link up is successful, else -E1000_ERR_PHY (-2).
+ *  Calls the PHY setup function to force speed and duplex.
  **/
-s32 igb_phy_force_speed_duplex_82580(struct e1000_hw *hw)
+s32 e1000_phy_force_speed_duplex_82577(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 phy_data;
 	bool link;
 
+	DEBUGFUNC("e1000_phy_force_speed_duplex_82577");
 
 	ret_val = phy->ops.read_reg(hw, PHY_CONTROL, &phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	igb_phy_force_speed_duplex_setup(hw, &phy_data);
+	e1000_phy_force_speed_duplex_setup(hw, &phy_data);
 
 	ret_val = phy->ops.write_reg(hw, PHY_CONTROL, phy_data);
 	if (ret_val)
-		goto out;
-
-	/*
-	 * Clear Auto-Crossover to force MDI manually.  82580 requires MDI
-	 * forced whenever speed and duplex are forced.
-	 */
-	ret_val = phy->ops.read_reg(hw, I82580_PHY_CTRL_2, &phy_data);
-	if (ret_val)
-		goto out;
-
-	phy_data &= ~I82580_PHY_CTRL2_AUTO_MDIX;
-	phy_data &= ~I82580_PHY_CTRL2_FORCE_MDI_MDIX;
-
-	ret_val = phy->ops.write_reg(hw, I82580_PHY_CTRL_2, phy_data);
-	if (ret_val)
-		goto out;
-
-	hw_dbg("I82580_PHY_CTRL_2: %X\n", phy_data);
+		return ret_val;
 
-	udelay(1);
+	usec_delay(1);
 
 	if (phy->autoneg_wait_to_complete) {
-		hw_dbg("Waiting for forced speed/duplex link on 82580 phy\n");
+		DEBUGOUT("Waiting for forced speed/duplex link on 82577 phy\n");
 
-		ret_val = igb_phy_has_link(hw,
-		                           PHY_FORCE_LIMIT,
-		                           100000,
-		                           &link);
+		ret_val = e1000_phy_has_link_generic(hw, PHY_FORCE_LIMIT,
+						     100000, &link);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		if (!link)
-			hw_dbg("Link taking longer than expected.\n");
+			DEBUGOUT("Link taking longer than expected.\n");
 
 		/* Try once more */
-		ret_val = igb_phy_has_link(hw,
-		                           PHY_FORCE_LIMIT,
-		                           100000,
-		                           &link);
-		if (ret_val)
-			goto out;
+		ret_val = e1000_phy_has_link_generic(hw, PHY_FORCE_LIMIT,
+						     100000, &link);
 	}
 
-out:
 	return ret_val;
 }
 
 /**
- *  igb_get_phy_info_82580 - Retrieve I82580 PHY information
+ *  e1000_get_phy_info_82577 - Retrieve I82577 PHY information
  *  @hw: pointer to the HW structure
  *
  *  Read PHY status to determine if link is up.  If link is up, then
@@ -2253,89 +3108,306 @@ out:
  *  PHY port status to determine MDI/MDIx and speed.  Based on the speed,
  *  determine on the cable length, local and remote receiver.
  **/
-s32 igb_get_phy_info_82580(struct e1000_hw *hw)
+s32 e1000_get_phy_info_82577(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 data;
 	bool link;
 
+	DEBUGFUNC("e1000_get_phy_info_82577");
 
-	ret_val = igb_phy_has_link(hw, 1, 0, &link);
+	ret_val = e1000_phy_has_link_generic(hw, 1, 0, &link);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
 	if (!link) {
-		hw_dbg("Phy info is only valid if link is up\n");
-		ret_val = -E1000_ERR_CONFIG;
-		goto out;
+		DEBUGOUT("Phy info is only valid if link is up\n");
+		return -E1000_ERR_CONFIG;
 	}
 
 	phy->polarity_correction = true;
 
-	ret_val = igb_check_polarity_82580(hw);
+	ret_val = e1000_check_polarity_82577(hw);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	ret_val = phy->ops.read_reg(hw, I82580_PHY_STATUS_2, &data);
+	ret_val = phy->ops.read_reg(hw, I82577_PHY_STATUS_2, &data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	phy->is_mdix = (data & I82580_PHY_STATUS2_MDIX) ? true : false;
+	phy->is_mdix = !!(data & I82577_PHY_STATUS2_MDIX);
 
-	if ((data & I82580_PHY_STATUS2_SPEED_MASK) ==
-	    I82580_PHY_STATUS2_SPEED_1000MBPS) {
+	if ((data & I82577_PHY_STATUS2_SPEED_MASK) ==
+	    I82577_PHY_STATUS2_SPEED_1000MBPS) {
 		ret_val = hw->phy.ops.get_cable_length(hw);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		ret_val = phy->ops.read_reg(hw, PHY_1000T_STATUS, &data);
 		if (ret_val)
-			goto out;
+			return ret_val;
 
 		phy->local_rx = (data & SR_1000T_LOCAL_RX_STATUS)
-		                ? e1000_1000t_rx_status_ok
-		                : e1000_1000t_rx_status_not_ok;
+				? e1000_1000t_rx_status_ok
+				: e1000_1000t_rx_status_not_ok;
 
 		phy->remote_rx = (data & SR_1000T_REMOTE_RX_STATUS)
-		                 ? e1000_1000t_rx_status_ok
-		                 : e1000_1000t_rx_status_not_ok;
+				 ? e1000_1000t_rx_status_ok
+				 : e1000_1000t_rx_status_not_ok;
 	} else {
 		phy->cable_length = E1000_CABLE_LENGTH_UNDEFINED;
 		phy->local_rx = e1000_1000t_rx_status_undefined;
 		phy->remote_rx = e1000_1000t_rx_status_undefined;
 	}
 
-out:
-	return ret_val;
+	return E1000_SUCCESS;
 }
 
 /**
- *  igb_get_cable_length_82580 - Determine cable length for 82580 PHY
+ *  e1000_get_cable_length_82577 - Determine cable length for 82577 PHY
  *  @hw: pointer to the HW structure
  *
  * Reads the diagnostic status register and verifies result is valid before
  * placing it in the phy_cable_length field.
  **/
-s32 igb_get_cable_length_82580(struct e1000_hw *hw)
+s32 e1000_get_cable_length_82577(struct e1000_hw *hw)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 phy_data, length;
 
+	DEBUGFUNC("e1000_get_cable_length_82577");
 
-	ret_val = phy->ops.read_reg(hw, I82580_PHY_DIAG_STATUS, &phy_data);
+	ret_val = phy->ops.read_reg(hw, I82577_PHY_DIAG_STATUS, &phy_data);
 	if (ret_val)
-		goto out;
+		return ret_val;
 
-	length = (phy_data & I82580_DSTATUS_CABLE_LENGTH) >>
-	         I82580_DSTATUS_CABLE_LENGTH_SHIFT;
+	length = ((phy_data & I82577_DSTATUS_CABLE_LENGTH) >>
+		  I82577_DSTATUS_CABLE_LENGTH_SHIFT);
 
 	if (length == E1000_CABLE_LENGTH_UNDEFINED)
-		ret_val = -E1000_ERR_PHY;
+		return -E1000_ERR_PHY;
 
 	phy->cable_length = length;
 
-out:
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_write_phy_reg_gs40g - Write GS40G  PHY register
+ *  @hw: pointer to the HW structure
+ *  @offset: register offset to write to
+ *  @data: data to write at register offset
+ *
+ *  Acquires semaphore, if necessary, then writes the data to PHY register
+ *  at the offset.  Release any acquired semaphores before exiting.
+ **/
+s32 e1000_write_phy_reg_gs40g(struct e1000_hw *hw, u32 offset, u16 data)
+{
+	s32 ret_val;
+	u16 page = offset >> GS40G_PAGE_SHIFT;
+
+	DEBUGFUNC("e1000_write_phy_reg_gs40g");
+
+	offset = offset & GS40G_OFFSET_MASK;
+	ret_val = hw->phy.ops.acquire(hw);
+	if (ret_val)
+		return ret_val;
+
+	ret_val = e1000_write_phy_reg_mdic(hw, GS40G_PAGE_SELECT, page);
+	if (ret_val)
+		goto release;
+	ret_val = e1000_write_phy_reg_mdic(hw, offset, data);
+
+release:
+	hw->phy.ops.release(hw);
+	return ret_val;
+}
+
+/**
+ *  e1000_read_phy_reg_gs40g - Read GS40G  PHY register
+ *  @hw: pointer to the HW structure
+ *  @offset: lower half is register offset to read to
+ *     upper half is page to use.
+ *  @data: data to read at register offset
+ *
+ *  Acquires semaphore, if necessary, then reads the data in the PHY register
+ *  at the offset.  Release any acquired semaphores before exiting.
+ **/
+s32 e1000_read_phy_reg_gs40g(struct e1000_hw *hw, u32 offset, u16 *data)
+{
+	s32 ret_val;
+	u16 page = offset >> GS40G_PAGE_SHIFT;
+
+	DEBUGFUNC("e1000_read_phy_reg_gs40g");
+
+	offset = offset & GS40G_OFFSET_MASK;
+	ret_val = hw->phy.ops.acquire(hw);
+	if (ret_val)
+		return ret_val;
+
+	ret_val = e1000_write_phy_reg_mdic(hw, GS40G_PAGE_SELECT, page);
+	if (ret_val)
+		goto release;
+	ret_val = e1000_read_phy_reg_mdic(hw, offset, data);
+
+release:
+	hw->phy.ops.release(hw);
 	return ret_val;
 }
+
+/**
+ *  e1000_read_phy_reg_mphy - Read mPHY control register
+ *  @hw: pointer to the HW structure
+ *  @address: address to be read
+ *  @data: pointer to the read data
+ *
+ *  Reads the mPHY control register in the PHY at offset and stores the
+ *  information read to data.
+ **/
+s32 e1000_read_phy_reg_mphy(struct e1000_hw *hw, u32 address, u32 *data)
+{
+	u32 mphy_ctrl = 0;
+	bool locked = false;
+	bool ready = false;
+
+	DEBUGFUNC("e1000_read_phy_reg_mphy");
+
+	/* Check if mPHY is ready to read/write operations */
+	ready = e1000_is_mphy_ready(hw);
+	if (!ready)
+		return -E1000_ERR_PHY;
+
+	/* Check if mPHY access is disabled and enable it if so */
+	mphy_ctrl = E1000_READ_REG(hw, E1000_MPHY_ADDR_CTRL);
+	if (mphy_ctrl & E1000_MPHY_DIS_ACCESS) {
+		locked = true;
+		ready = e1000_is_mphy_ready(hw);
+		if (!ready)
+			return -E1000_ERR_PHY;
+		mphy_ctrl |= E1000_MPHY_ENA_ACCESS;
+		E1000_WRITE_REG(hw, E1000_MPHY_ADDR_CTRL, mphy_ctrl);
+	}
+
+	/* Set the address that we want to read */
+	ready = e1000_is_mphy_ready(hw);
+	if (!ready)
+		return -E1000_ERR_PHY;
+
+	/* We mask address, because we want to use only current lane */
+	mphy_ctrl = (mphy_ctrl & ~E1000_MPHY_ADDRESS_MASK &
+		~E1000_MPHY_ADDRESS_FNC_OVERRIDE) |
+		(address & E1000_MPHY_ADDRESS_MASK);
+	E1000_WRITE_REG(hw, E1000_MPHY_ADDR_CTRL, mphy_ctrl);
+
+	/* Read data from the address */
+	ready = e1000_is_mphy_ready(hw);
+	if (!ready)
+		return -E1000_ERR_PHY;
+	*data = E1000_READ_REG(hw, E1000_MPHY_DATA);
+
+	/* Disable access to mPHY if it was originally disabled */
+	if (locked)
+		ready = e1000_is_mphy_ready(hw);
+		if (!ready)
+			return -E1000_ERR_PHY;
+		E1000_WRITE_REG(hw, E1000_MPHY_ADDR_CTRL,
+				E1000_MPHY_DIS_ACCESS);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_write_phy_reg_mphy - Write mPHY control register
+ *  @hw: pointer to the HW structure
+ *  @address: address to write to
+ *  @data: data to write to register at offset
+ *  @line_override: used when we want to use different line than default one
+ *
+ *  Writes data to mPHY control register.
+ **/
+s32 e1000_write_phy_reg_mphy(struct e1000_hw *hw, u32 address, u32 data,
+			     bool line_override)
+{
+	u32 mphy_ctrl = 0;
+	bool locked = false;
+	bool ready = false;
+
+	DEBUGFUNC("e1000_write_phy_reg_mphy");
+
+	/* Check if mPHY is ready to read/write operations */
+	ready = e1000_is_mphy_ready(hw);
+	if (!ready)
+		return -E1000_ERR_PHY;
+
+	/* Check if mPHY access is disabled and enable it if so */
+	mphy_ctrl = E1000_READ_REG(hw, E1000_MPHY_ADDR_CTRL);
+	if (mphy_ctrl & E1000_MPHY_DIS_ACCESS) {
+		locked = true;
+		ready = e1000_is_mphy_ready(hw);
+		if (!ready)
+			return -E1000_ERR_PHY;
+		mphy_ctrl |= E1000_MPHY_ENA_ACCESS;
+		E1000_WRITE_REG(hw, E1000_MPHY_ADDR_CTRL, mphy_ctrl);
+	}
+
+	/* Set the address that we want to read */
+	ready = e1000_is_mphy_ready(hw);
+	if (!ready)
+		return -E1000_ERR_PHY;
+
+	/* We mask address, because we want to use only current lane */
+	if (line_override)
+		mphy_ctrl |= E1000_MPHY_ADDRESS_FNC_OVERRIDE;
+	else
+		mphy_ctrl &= ~E1000_MPHY_ADDRESS_FNC_OVERRIDE;
+	mphy_ctrl = (mphy_ctrl & ~E1000_MPHY_ADDRESS_MASK) |
+		(address & E1000_MPHY_ADDRESS_MASK);
+	E1000_WRITE_REG(hw, E1000_MPHY_ADDR_CTRL, mphy_ctrl);
+
+	/* Read data from the address */
+	ready = e1000_is_mphy_ready(hw);
+	if (!ready)
+		return -E1000_ERR_PHY;
+	E1000_WRITE_REG(hw, E1000_MPHY_DATA, data);
+
+	/* Disable access to mPHY if it was originally disabled */
+	if (locked)
+		ready = e1000_is_mphy_ready(hw);
+		if (!ready)
+			return -E1000_ERR_PHY;
+		E1000_WRITE_REG(hw, E1000_MPHY_ADDR_CTRL,
+				E1000_MPHY_DIS_ACCESS);
+
+	return E1000_SUCCESS;
+}
+
+/**
+ *  e1000_is_mphy_ready - Check if mPHY control register is not busy
+ *  @hw: pointer to the HW structure
+ *
+ *  Returns mPHY control register status.
+ **/
+bool e1000_is_mphy_ready(struct e1000_hw *hw)
+{
+	u16 retry_count = 0;
+	u32 mphy_ctrl = 0;
+	bool ready = false;
+
+	while (retry_count < 2) {
+		mphy_ctrl = E1000_READ_REG(hw, E1000_MPHY_ADDR_CTRL);
+		if (mphy_ctrl & E1000_MPHY_BUSY) {
+			usec_delay(20);
+			retry_count++;
+			continue;
+		}
+		ready = true;
+		break;
+	}
+
+	if (!ready)
+		DEBUGOUT("ERROR READING mPHY control register, phy is busy.\n");
+
+	return ready;
+}
diff --git a/drivers/net/igb/e1000_phy.h b/drivers/net/igb/e1000_phy.h
index 8510797b9d81..5387c5e7ea14 100644
--- a/drivers/net/igb/e1000_phy.h
+++ b/drivers/net/igb/e1000_phy.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2007-2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -28,109 +28,229 @@
 #ifndef _E1000_PHY_H_
 #define _E1000_PHY_H_
 
-enum e1000_ms_type {
-	e1000_ms_hw_default = 0,
-	e1000_ms_force_master,
-	e1000_ms_force_slave,
-	e1000_ms_auto
-};
-
-enum e1000_smart_speed {
-	e1000_smart_speed_default = 0,
-	e1000_smart_speed_on,
-	e1000_smart_speed_off
-};
-
-s32  igb_check_downshift(struct e1000_hw *hw);
-s32  igb_check_reset_block(struct e1000_hw *hw);
-s32  igb_copper_link_setup_igp(struct e1000_hw *hw);
-s32  igb_copper_link_setup_m88(struct e1000_hw *hw);
-s32  igb_copper_link_setup_m88_gen2(struct e1000_hw *hw);
-s32  igb_phy_force_speed_duplex_igp(struct e1000_hw *hw);
-s32  igb_phy_force_speed_duplex_m88(struct e1000_hw *hw);
-s32  igb_get_cable_length_m88(struct e1000_hw *hw);
-s32  igb_get_cable_length_m88_gen2(struct e1000_hw *hw);
-s32  igb_get_cable_length_igp_2(struct e1000_hw *hw);
-s32  igb_get_phy_id(struct e1000_hw *hw);
-s32  igb_get_phy_info_igp(struct e1000_hw *hw);
-s32  igb_get_phy_info_m88(struct e1000_hw *hw);
-s32  igb_phy_sw_reset(struct e1000_hw *hw);
-s32  igb_phy_hw_reset(struct e1000_hw *hw);
-s32  igb_read_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 *data);
-s32  igb_set_d3_lplu_state(struct e1000_hw *hw, bool active);
-s32  igb_setup_copper_link(struct e1000_hw *hw);
-s32  igb_write_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 data);
-s32  igb_phy_has_link(struct e1000_hw *hw, u32 iterations,
+void e1000_init_phy_ops_generic(struct e1000_hw *hw);
+s32  e1000_null_read_reg(struct e1000_hw *hw, u32 offset, u16 *data);
+void e1000_null_phy_generic(struct e1000_hw *hw);
+s32  e1000_null_lplu_state(struct e1000_hw *hw, bool active);
+s32  e1000_null_write_reg(struct e1000_hw *hw, u32 offset, u16 data);
+s32  e1000_null_set_page(struct e1000_hw *hw, u16 data);
+s32 e1000_read_i2c_byte_null(struct e1000_hw *hw, u8 byte_offset,
+			     u8 dev_addr, u8 *data);
+s32 e1000_write_i2c_byte_null(struct e1000_hw *hw, u8 byte_offset,
+			      u8 dev_addr, u8 data);
+s32  e1000_check_downshift_generic(struct e1000_hw *hw);
+s32  e1000_check_polarity_m88(struct e1000_hw *hw);
+s32  e1000_check_polarity_igp(struct e1000_hw *hw);
+s32  e1000_check_polarity_ife(struct e1000_hw *hw);
+s32  e1000_check_reset_block_generic(struct e1000_hw *hw);
+s32  e1000_copper_link_setup_igp(struct e1000_hw *hw);
+s32  e1000_copper_link_setup_m88(struct e1000_hw *hw);
+s32  e1000_copper_link_setup_m88_gen2(struct e1000_hw *hw);
+s32  e1000_phy_force_speed_duplex_igp(struct e1000_hw *hw);
+s32  e1000_phy_force_speed_duplex_m88(struct e1000_hw *hw);
+s32  e1000_phy_force_speed_duplex_ife(struct e1000_hw *hw);
+s32  e1000_get_cable_length_m88(struct e1000_hw *hw);
+s32  e1000_get_cable_length_m88_gen2(struct e1000_hw *hw);
+s32  e1000_get_cable_length_igp_2(struct e1000_hw *hw);
+s32  e1000_get_cfg_done_generic(struct e1000_hw *hw);
+s32  e1000_get_phy_id(struct e1000_hw *hw);
+s32  e1000_get_phy_info_igp(struct e1000_hw *hw);
+s32  e1000_get_phy_info_m88(struct e1000_hw *hw);
+s32  e1000_get_phy_info_ife(struct e1000_hw *hw);
+s32  e1000_phy_sw_reset_generic(struct e1000_hw *hw);
+void e1000_phy_force_speed_duplex_setup(struct e1000_hw *hw, u16 *phy_ctrl);
+s32  e1000_phy_hw_reset_generic(struct e1000_hw *hw);
+s32  e1000_phy_reset_dsp_generic(struct e1000_hw *hw);
+s32  e1000_read_kmrn_reg_generic(struct e1000_hw *hw, u32 offset, u16 *data);
+s32  e1000_read_kmrn_reg_locked(struct e1000_hw *hw, u32 offset, u16 *data);
+s32  e1000_set_page_igp(struct e1000_hw *hw, u16 page);
+s32  e1000_read_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 *data);
+s32  e1000_read_phy_reg_igp_locked(struct e1000_hw *hw, u32 offset, u16 *data);
+s32  e1000_read_phy_reg_m88(struct e1000_hw *hw, u32 offset, u16 *data);
+s32  e1000_set_d3_lplu_state_generic(struct e1000_hw *hw, bool active);
+s32  e1000_setup_copper_link_generic(struct e1000_hw *hw);
+s32  e1000_write_kmrn_reg_generic(struct e1000_hw *hw, u32 offset, u16 data);
+s32  e1000_write_kmrn_reg_locked(struct e1000_hw *hw, u32 offset, u16 data);
+s32  e1000_write_phy_reg_igp(struct e1000_hw *hw, u32 offset, u16 data);
+s32  e1000_write_phy_reg_igp_locked(struct e1000_hw *hw, u32 offset, u16 data);
+s32  e1000_write_phy_reg_m88(struct e1000_hw *hw, u32 offset, u16 data);
+s32  e1000_phy_has_link_generic(struct e1000_hw *hw, u32 iterations,
 				u32 usec_interval, bool *success);
-void igb_power_up_phy_copper(struct e1000_hw *hw);
-void igb_power_down_phy_copper(struct e1000_hw *hw);
-s32  igb_phy_init_script_igp3(struct e1000_hw *hw);
-s32  igb_read_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 *data);
-s32  igb_write_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 data);
-s32  igb_read_phy_reg_i2c(struct e1000_hw *hw, u32 offset, u16 *data);
-s32  igb_write_phy_reg_i2c(struct e1000_hw *hw, u32 offset, u16 data);
-s32  igb_copper_link_setup_82580(struct e1000_hw *hw);
-s32  igb_get_phy_info_82580(struct e1000_hw *hw);
-s32  igb_phy_force_speed_duplex_82580(struct e1000_hw *hw);
-s32  igb_get_cable_length_82580(struct e1000_hw *hw);
+s32  e1000_phy_init_script_igp3(struct e1000_hw *hw);
+enum e1000_phy_type e1000_get_phy_type_from_id(u32 phy_id);
+s32  e1000_determine_phy_address(struct e1000_hw *hw);
+s32  e1000_enable_phy_wakeup_reg_access_bm(struct e1000_hw *hw, u16 *phy_reg);
+s32  e1000_disable_phy_wakeup_reg_access_bm(struct e1000_hw *hw, u16 *phy_reg);
+void e1000_power_up_phy_copper(struct e1000_hw *hw);
+void e1000_power_down_phy_copper(struct e1000_hw *hw);
+s32  e1000_read_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 *data);
+s32  e1000_write_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 data);
+s32  e1000_read_phy_reg_i2c(struct e1000_hw *hw, u32 offset, u16 *data);
+s32  e1000_write_phy_reg_i2c(struct e1000_hw *hw, u32 offset, u16 data);
+s32  e1000_read_sfp_data_byte(struct e1000_hw *hw, u16 offset, u8 *data);
+s32  e1000_write_sfp_data_byte(struct e1000_hw *hw, u16 offset, u8 data);
+s32  e1000_copper_link_setup_82577(struct e1000_hw *hw);
+s32  e1000_check_polarity_82577(struct e1000_hw *hw);
+s32  e1000_get_phy_info_82577(struct e1000_hw *hw);
+s32  e1000_phy_force_speed_duplex_82577(struct e1000_hw *hw);
+s32  e1000_get_cable_length_82577(struct e1000_hw *hw);
+s32  e1000_write_phy_reg_gs40g(struct e1000_hw *hw, u32 offset, u16 data);
+s32  e1000_read_phy_reg_gs40g(struct e1000_hw *hw, u32 offset, u16 *data);
+s32 e1000_read_phy_reg_mphy(struct e1000_hw *hw, u32 address, u32 *data);
+s32 e1000_write_phy_reg_mphy(struct e1000_hw *hw, u32 address, u32 data,
+			     bool line_override);
+bool e1000_is_mphy_ready(struct e1000_hw *hw);
+
+#define E1000_MAX_PHY_ADDR		8
 
 /* IGP01E1000 Specific Registers */
-#define IGP01E1000_PHY_PORT_CONFIG        0x10 /* Port Config */
-#define IGP01E1000_PHY_PORT_STATUS        0x11 /* Status */
-#define IGP01E1000_PHY_PORT_CTRL          0x12 /* Control */
-#define IGP01E1000_PHY_LINK_HEALTH        0x13 /* PHY Link Health */
-#define IGP02E1000_PHY_POWER_MGMT         0x19 /* Power Management */
-#define IGP01E1000_PHY_PAGE_SELECT        0x1F /* Page Select */
-#define IGP01E1000_PHY_PCS_INIT_REG       0x00B4
-#define IGP01E1000_PHY_POLARITY_MASK      0x0078
-#define IGP01E1000_PSCR_AUTO_MDIX         0x1000
-#define IGP01E1000_PSCR_FORCE_MDI_MDIX    0x2000 /* 0=MDI, 1=MDIX */
-#define IGP01E1000_PSCFR_SMART_SPEED      0x0080
-
-#define I82580_ADDR_REG                   16
-#define I82580_CFG_REG                    22
-#define I82580_CFG_ASSERT_CRS_ON_TX       (1 << 15)
-#define I82580_CFG_ENABLE_DOWNSHIFT       (3 << 10) /* auto downshift 100/10 */
-#define I82580_CTRL_REG                   23
-#define I82580_CTRL_DOWNSHIFT_MASK        (7 << 10)
-
-/* 82580 specific PHY registers */
-#define I82580_PHY_CTRL_2            18
-#define I82580_PHY_LBK_CTRL          19
-#define I82580_PHY_STATUS_2          26
-#define I82580_PHY_DIAG_STATUS       31
-
-/* I82580 PHY Status 2 */
-#define I82580_PHY_STATUS2_REV_POLARITY   0x0400
-#define I82580_PHY_STATUS2_MDIX           0x0800
-#define I82580_PHY_STATUS2_SPEED_MASK     0x0300
-#define I82580_PHY_STATUS2_SPEED_1000MBPS 0x0200
-#define I82580_PHY_STATUS2_SPEED_100MBPS  0x0100
-
-/* I82580 PHY Control 2 */
-#define I82580_PHY_CTRL2_AUTO_MDIX        0x0400
-#define I82580_PHY_CTRL2_FORCE_MDI_MDIX   0x0200
-
-/* I82580 PHY Diagnostics Status */
-#define I82580_DSTATUS_CABLE_LENGTH       0x03FC
-#define I82580_DSTATUS_CABLE_LENGTH_SHIFT 2
-/* Enable flexible speed on link-up */
-#define IGP02E1000_PM_D0_LPLU             0x0002 /* For D0a states */
-#define IGP02E1000_PM_D3_LPLU             0x0004 /* For all other states */
-#define IGP01E1000_PLHR_SS_DOWNGRADE      0x8000
-#define IGP01E1000_PSSR_POLARITY_REVERSED 0x0002
-#define IGP01E1000_PSSR_MDIX              0x0800
-#define IGP01E1000_PSSR_SPEED_MASK        0xC000
-#define IGP01E1000_PSSR_SPEED_1000MBPS    0xC000
-#define IGP02E1000_PHY_CHANNEL_NUM        4
-#define IGP02E1000_PHY_AGC_A              0x11B1
-#define IGP02E1000_PHY_AGC_B              0x12B1
-#define IGP02E1000_PHY_AGC_C              0x14B1
-#define IGP02E1000_PHY_AGC_D              0x18B1
-#define IGP02E1000_AGC_LENGTH_SHIFT       9   /* Course - 15:13, Fine - 12:9 */
-#define IGP02E1000_AGC_LENGTH_MASK        0x7F
-#define IGP02E1000_AGC_RANGE              15
-
-#define E1000_CABLE_LENGTH_UNDEFINED      0xFF
+#define IGP01E1000_PHY_PORT_CONFIG	0x10 /* Port Config */
+#define IGP01E1000_PHY_PORT_STATUS	0x11 /* Status */
+#define IGP01E1000_PHY_PORT_CTRL	0x12 /* Control */
+#define IGP01E1000_PHY_LINK_HEALTH	0x13 /* PHY Link Health */
+#define IGP02E1000_PHY_POWER_MGMT	0x19 /* Power Management */
+#define IGP01E1000_PHY_PAGE_SELECT	0x1F /* Page Select */
+#define BM_PHY_PAGE_SELECT		22   /* Page Select for BM */
+#define IGP_PAGE_SHIFT			5
+#define PHY_REG_MASK			0x1F
+
+/* GS40G - I210 PHY defines */
+#define GS40G_PAGE_SELECT		0x16
+#define GS40G_PAGE_SHIFT		16
+#define GS40G_OFFSET_MASK		0xFFFF
+#define GS40G_PAGE_2			0x20000
+#define GS40G_MAC_REG2			0x15
+#define GS40G_MAC_LB			0x4140
+#define GS40G_MAC_SPEED_1G		0X0006
+#define GS40G_COPPER_SPEC		0x0010
+#define GS40G_CS_POWER_DOWN		0x0002
+
+#define HV_INTC_FC_PAGE_START		768
+#define I82578_ADDR_REG			29
+#define I82577_ADDR_REG			16
+#define I82577_CFG_REG			22
+#define I82577_CFG_ASSERT_CRS_ON_TX	(1 << 15)
+#define I82577_CFG_ENABLE_DOWNSHIFT	(3 << 10) /* auto downshift */
+#define I82577_CTRL_REG			23
+
+/* 82577 specific PHY registers */
+#define I82577_PHY_CTRL_2		18
+#define I82577_PHY_LBK_CTRL		19
+#define I82577_PHY_STATUS_2		26
+#define I82577_PHY_DIAG_STATUS		31
+
+/* I82577 PHY Status 2 */
+#define I82577_PHY_STATUS2_REV_POLARITY		0x0400
+#define I82577_PHY_STATUS2_MDIX			0x0800
+#define I82577_PHY_STATUS2_SPEED_MASK		0x0300
+#define I82577_PHY_STATUS2_SPEED_1000MBPS	0x0200
+
+/* I82577 PHY Control 2 */
+#define I82577_PHY_CTRL2_MANUAL_MDIX		0x0200
+#define I82577_PHY_CTRL2_AUTO_MDI_MDIX		0x0400
+#define I82577_PHY_CTRL2_MDIX_CFG_MASK		0x0600
+
+/* I82577 PHY Diagnostics Status */
+#define I82577_DSTATUS_CABLE_LENGTH		0x03FC
+#define I82577_DSTATUS_CABLE_LENGTH_SHIFT	2
+
+/* 82580 PHY Power Management */
+#define E1000_82580_PHY_POWER_MGMT	0xE14
+#define E1000_82580_PM_SPD		0x0001 /* Smart Power Down */
+#define E1000_82580_PM_D0_LPLU		0x0002 /* For D0a states */
+#define E1000_82580_PM_D3_LPLU		0x0004 /* For all other states */
+#define E1000_82580_PM_GO_LINKD		0x0020 /* Go Link Disconnect */
+
+#define E1000_MPHY_DIS_ACCESS		0x80000000 /* disable_access bit */
+#define E1000_MPHY_ENA_ACCESS		0x40000000 /* enable_access bit */
+#define E1000_MPHY_BUSY			0x00010000 /* busy bit */
+#define E1000_MPHY_ADDRESS_FNC_OVERRIDE	0x20000000 /* fnc_override bit */
+#define E1000_MPHY_ADDRESS_MASK		0x0000FFFF /* address mask */
+
+#define IGP01E1000_PHY_PCS_INIT_REG	0x00B4
+#define IGP01E1000_PHY_POLARITY_MASK	0x0078
+
+#define IGP01E1000_PSCR_AUTO_MDIX	0x1000
+#define IGP01E1000_PSCR_FORCE_MDI_MDIX	0x2000 /* 0=MDI, 1=MDIX */
+
+#define IGP01E1000_PSCFR_SMART_SPEED	0x0080
+
+#define IGP02E1000_PM_SPD		0x0001 /* Smart Power Down */
+#define IGP02E1000_PM_D0_LPLU		0x0002 /* For D0a states */
+#define IGP02E1000_PM_D3_LPLU		0x0004 /* For all other states */
+
+#define IGP01E1000_PLHR_SS_DOWNGRADE	0x8000
+
+#define IGP01E1000_PSSR_POLARITY_REVERSED	0x0002
+#define IGP01E1000_PSSR_MDIX		0x0800
+#define IGP01E1000_PSSR_SPEED_MASK	0xC000
+#define IGP01E1000_PSSR_SPEED_1000MBPS	0xC000
+
+#define IGP02E1000_PHY_CHANNEL_NUM	4
+#define IGP02E1000_PHY_AGC_A		0x11B1
+#define IGP02E1000_PHY_AGC_B		0x12B1
+#define IGP02E1000_PHY_AGC_C		0x14B1
+#define IGP02E1000_PHY_AGC_D		0x18B1
+
+#define IGP02E1000_AGC_LENGTH_SHIFT	9   /* Course=15:13, Fine=12:9 */
+#define IGP02E1000_AGC_LENGTH_MASK	0x7F
+#define IGP02E1000_AGC_RANGE		15
+
+#define E1000_CABLE_LENGTH_UNDEFINED	0xFF
+
+#define E1000_KMRNCTRLSTA_OFFSET	0x001F0000
+#define E1000_KMRNCTRLSTA_OFFSET_SHIFT	16
+#define E1000_KMRNCTRLSTA_REN		0x00200000
+#define E1000_KMRNCTRLSTA_DIAG_OFFSET	0x3    /* Kumeran Diagnostic */
+#define E1000_KMRNCTRLSTA_TIMEOUTS	0x4    /* Kumeran Timeouts */
+#define E1000_KMRNCTRLSTA_INBAND_PARAM	0x9    /* Kumeran InBand Parameters */
+#define E1000_KMRNCTRLSTA_IBIST_DISABLE	0x0200 /* Kumeran IBIST Disable */
+#define E1000_KMRNCTRLSTA_DIAG_NELPBK	0x1000 /* Nearend Loopback mode */
+
+#define IFE_PHY_EXTENDED_STATUS_CONTROL	0x10
+#define IFE_PHY_SPECIAL_CONTROL		0x11 /* 100BaseTx PHY Special Ctrl */
+#define IFE_PHY_SPECIAL_CONTROL_LED	0x1B /* PHY Special and LED Ctrl */
+#define IFE_PHY_MDIX_CONTROL		0x1C /* MDI/MDI-X Control */
+
+/* IFE PHY Extended Status Control */
+#define IFE_PESC_POLARITY_REVERSED	0x0100
+
+/* IFE PHY Special Control */
+#define IFE_PSC_AUTO_POLARITY_DISABLE	0x0010
+#define IFE_PSC_FORCE_POLARITY		0x0020
+
+/* IFE PHY Special Control and LED Control */
+#define IFE_PSCL_PROBE_MODE		0x0020
+#define IFE_PSCL_PROBE_LEDS_OFF		0x0006 /* Force LEDs 0 and 2 off */
+#define IFE_PSCL_PROBE_LEDS_ON		0x0007 /* Force LEDs 0 and 2 on */
+
+/* IFE PHY MDIX Control */
+#define IFE_PMC_MDIX_STATUS		0x0020 /* 1=MDI-X, 0=MDI */
+#define IFE_PMC_FORCE_MDIX		0x0040 /* 1=force MDI-X, 0=force MDI */
+#define IFE_PMC_AUTO_MDIX		0x0080 /* 1=enable auto, 0=disable */
+
+/* SFP modules ID memory locations */
+#define E1000_SFF_IDENTIFIER_OFFSET	0x00
+#define E1000_SFF_IDENTIFIER_SFF	0x02
+#define E1000_SFF_IDENTIFIER_SFP	0x03
+
+#define E1000_SFF_ETH_FLAGS_OFFSET	0x06
+/* Flags for SFP modules compatible with ETH up to 1Gb */
+struct sfp_e1000_flags {
+	u8 e1000_base_sx:1;
+	u8 e1000_base_lx:1;
+	u8 e1000_base_cx:1;
+	u8 e1000_base_t:1;
+	u8 e100_base_lx:1;
+	u8 e100_base_fx:1;
+	u8 e10_base_bx10:1;
+	u8 e10_base_px:1;
+};
+
+/* Vendor OUIs: format of OUI is 0x[byte0][byte1][byte2][00] */
+#define E1000_SFF_VENDOR_OUI_TYCO	0x00407600
+#define E1000_SFF_VENDOR_OUI_FTL	0x00906500
+#define E1000_SFF_VENDOR_OUI_AVAGO	0x00176A00
+#define E1000_SFF_VENDOR_OUI_INTEL	0x001B2100
 
 #endif
diff --git a/drivers/net/igb/e1000_regs.h b/drivers/net/igb/e1000_regs.h
index 0990f6d860c7..5a2965d622a8 100644
--- a/drivers/net/igb/e1000_regs.h
+++ b/drivers/net/igb/e1000_regs.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2007-2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -28,327 +28,617 @@
 #ifndef _E1000_REGS_H_
 #define _E1000_REGS_H_
 
-#define E1000_CTRL     0x00000  /* Device Control - RW */
-#define E1000_STATUS   0x00008  /* Device Status - RO */
-#define E1000_EECD     0x00010  /* EEPROM/Flash Control - RW */
-#define E1000_EERD     0x00014  /* EEPROM Read - RW */
-#define E1000_CTRL_EXT 0x00018  /* Extended Device Control - RW */
-#define E1000_MDIC     0x00020  /* MDI Control - RW */
-#define E1000_MDICNFG  0x00E04  /* MDI Config - RW */
-#define E1000_SCTL     0x00024  /* SerDes Control - RW */
-#define E1000_FCAL     0x00028  /* Flow Control Address Low - RW */
-#define E1000_FCAH     0x0002C  /* Flow Control Address High -RW */
-#define E1000_FCT      0x00030  /* Flow Control Type - RW */
-#define E1000_CONNSW   0x00034  /* Copper/Fiber switch control - RW */
-#define E1000_VET      0x00038  /* VLAN Ether Type - RW */
-#define E1000_ICR      0x000C0  /* Interrupt Cause Read - R/clr */
-#define E1000_ITR      0x000C4  /* Interrupt Throttling Rate - RW */
-#define E1000_ICS      0x000C8  /* Interrupt Cause Set - WO */
-#define E1000_IMS      0x000D0  /* Interrupt Mask Set - RW */
-#define E1000_IMC      0x000D8  /* Interrupt Mask Clear - WO */
-#define E1000_IAM      0x000E0  /* Interrupt Acknowledge Auto Mask */
-#define E1000_RCTL     0x00100  /* RX Control - RW */
-#define E1000_FCTTV    0x00170  /* Flow Control Transmit Timer Value - RW */
-#define E1000_TXCW     0x00178  /* TX Configuration Word - RW */
-#define E1000_EICR     0x01580  /* Ext. Interrupt Cause Read - R/clr */
-#define E1000_EITR(_n) (0x01680 + (0x4 * (_n)))
-#define E1000_EICS     0x01520  /* Ext. Interrupt Cause Set - W0 */
-#define E1000_EIMS     0x01524  /* Ext. Interrupt Mask Set/Read - RW */
-#define E1000_EIMC     0x01528  /* Ext. Interrupt Mask Clear - WO */
-#define E1000_EIAC     0x0152C  /* Ext. Interrupt Auto Clear - RW */
-#define E1000_EIAM     0x01530  /* Ext. Interrupt Ack Auto Clear Mask - RW */
-#define E1000_GPIE     0x01514  /* General Purpose Interrupt Enable - RW */
-#define E1000_IVAR0    0x01700  /* Interrupt Vector Allocation (array) - RW */
-#define E1000_IVAR_MISC 0x01740 /* IVAR for "other" causes - RW */
-#define E1000_TCTL     0x00400  /* TX Control - RW */
-#define E1000_TCTL_EXT 0x00404  /* Extended TX Control - RW */
-#define E1000_TIPG     0x00410  /* TX Inter-packet gap -RW */
-#define E1000_AIT      0x00458  /* Adaptive Interframe Spacing Throttle - RW */
-#define E1000_LEDCTL   0x00E00  /* LED Control - RW */
-#define E1000_PBA      0x01000  /* Packet Buffer Allocation - RW */
-#define E1000_PBS      0x01008  /* Packet Buffer Size */
-#define E1000_EEMNGCTL 0x01010  /* MNG EEprom Control */
-#define E1000_EEWR     0x0102C  /* EEPROM Write Register - RW */
-#define E1000_I2CCMD   0x01028  /* SFPI2C Command Register - RW */
-#define E1000_FRTIMER  0x01048  /* Free Running Timer - RW */
-#define E1000_TCPTIMER 0x0104C  /* TCP Timer - RW */
-#define E1000_FCRTL    0x02160  /* Flow Control Receive Threshold Low - RW */
-#define E1000_FCRTH    0x02168  /* Flow Control Receive Threshold High - RW */
-#define E1000_FCRTV    0x02460  /* Flow Control Refresh Timer Value - RW */
-
-/* IEEE 1588 TIMESYNCH */
-#define E1000_TSYNCRXCTL 0x0B620 /* Rx Time Sync Control register - RW */
-#define E1000_TSYNCTXCTL 0x0B614 /* Tx Time Sync Control register - RW */
-#define E1000_TSYNCRXCFG 0x05F50 /* Time Sync Rx Configuration - RW */
-#define E1000_RXSTMPL    0x0B624 /* Rx timestamp Low - RO */
-#define E1000_RXSTMPH    0x0B628 /* Rx timestamp High - RO */
-#define E1000_RXSATRL    0x0B62C /* Rx timestamp attribute low - RO */
-#define E1000_RXSATRH    0x0B630 /* Rx timestamp attribute high - RO */
-#define E1000_TXSTMPL    0x0B618 /* Tx timestamp value Low - RO */
-#define E1000_TXSTMPH    0x0B61C /* Tx timestamp value High - RO */
-#define E1000_SYSTIML    0x0B600 /* System time register Low - RO */
-#define E1000_SYSTIMH    0x0B604 /* System time register High - RO */
-#define E1000_TIMINCA    0x0B608 /* Increment attributes register - RW */
-#define E1000_TSAUXC     0x0B640 /* Timesync Auxiliary Control register */
-#define E1000_SYSTIMR    0x0B6F8 /* System time register Residue */
+#define E1000_CTRL	0x00000  /* Device Control - RW */
+#define E1000_STATUS	0x00008  /* Device Status - RO */
+#define E1000_EECD	0x00010  /* EEPROM/Flash Control - RW */
+#define E1000_EERD	0x00014  /* EEPROM Read - RW */
+#define E1000_CTRL_EXT	0x00018  /* Extended Device Control - RW */
+#define E1000_FLA	0x0001C  /* Flash Access - RW */
+#define E1000_MDIC	0x00020  /* MDI Control - RW */
+#define E1000_MDICNFG	0x00E04  /* MDI Config - RW */
+#define E1000_REGISTER_SET_SIZE		0x20000 /* CSR Size */
+#define E1000_EEPROM_INIT_CTRL_WORD_2	0x0F /* EEPROM Init Ctrl Word 2 */
+#define E1000_EEPROM_PCIE_CTRL_WORD_2	0x28 /* EEPROM PCIe Ctrl Word 2 */
+#define E1000_BARCTRL			0x5BBC /* BAR ctrl reg */
+#define E1000_BARCTRL_FLSIZE		0x0700 /* BAR ctrl Flsize */
+#define E1000_BARCTRL_CSRSIZE		0x2000 /* BAR ctrl CSR size */
+#define E1000_MPHY_ADDR_CTRL	0x0024 /* GbE MPHY Address Control */
+#define E1000_MPHY_DATA		0x0E10 /* GBE MPHY Data */
+#define E1000_MPHY_STAT		0x0E0C /* GBE MPHY Statistics */
+#define E1000_PPHY_CTRL		0x5b48 /* PCIe PHY Control */
+#define E1000_I350_BARCTRL		0x5BFC /* BAR ctrl reg */
+#define E1000_I350_DTXMXPKTSZ		0x355C /* Maximum sent packet size reg*/
+#define E1000_SCTL	0x00024  /* SerDes Control - RW */
+#define E1000_FCAL	0x00028  /* Flow Control Address Low - RW */
+#define E1000_FCAH	0x0002C  /* Flow Control Address High -RW */
+#define E1000_FCT	0x00030  /* Flow Control Type - RW */
+#define E1000_CONNSW	0x00034  /* Copper/Fiber switch control - RW */
+#define E1000_VET	0x00038  /* VLAN Ether Type - RW */
+#define E1000_ICR	0x000C0  /* Interrupt Cause Read - R/clr */
+#define E1000_ITR	0x000C4  /* Interrupt Throttling Rate - RW */
+#define E1000_ICS	0x000C8  /* Interrupt Cause Set - WO */
+#define E1000_IMS	0x000D0  /* Interrupt Mask Set - RW */
+#define E1000_IMC	0x000D8  /* Interrupt Mask Clear - WO */
+#define E1000_IAM	0x000E0  /* Interrupt Acknowledge Auto Mask */
+#define E1000_RCTL	0x00100  /* Rx Control - RW */
+#define E1000_FCTTV	0x00170  /* Flow Control Transmit Timer Value - RW */
+#define E1000_TXCW	0x00178  /* Tx Configuration Word - RW */
+#define E1000_RXCW	0x00180  /* Rx Configuration Word - RO */
+#define E1000_EICR	0x01580  /* Ext. Interrupt Cause Read - R/clr */
+#define E1000_EITR(_n)	(0x01680 + (0x4 * (_n)))
+#define E1000_EICS	0x01520  /* Ext. Interrupt Cause Set - W0 */
+#define E1000_EIMS	0x01524  /* Ext. Interrupt Mask Set/Read - RW */
+#define E1000_EIMC	0x01528  /* Ext. Interrupt Mask Clear - WO */
+#define E1000_EIAC	0x0152C  /* Ext. Interrupt Auto Clear - RW */
+#define E1000_EIAM	0x01530  /* Ext. Interrupt Ack Auto Clear Mask - RW */
+#define E1000_GPIE	0x01514  /* General Purpose Interrupt Enable - RW */
+#define E1000_IVAR0	0x01700  /* Interrupt Vector Allocation (array) - RW */
+#define E1000_IVAR_MISC	0x01740 /* IVAR for "other" causes - RW */
+#define E1000_TCTL	0x00400  /* Tx Control - RW */
+#define E1000_TCTL_EXT	0x00404  /* Extended Tx Control - RW */
+#define E1000_TIPG	0x00410  /* Tx Inter-packet gap -RW */
+#define E1000_AIT	0x00458  /* Adaptive Interframe Spacing Throttle - RW */
+#define E1000_LEDCTL	0x00E00  /* LED Control - RW */
+#define E1000_LEDMUX	0x08130  /* LED MUX Control */
+#define E1000_EXTCNF_CTRL	0x00F00  /* Extended Configuration Control */
+#define E1000_EXTCNF_SIZE	0x00F08  /* Extended Configuration Size */
+#define E1000_PHY_CTRL	0x00F10  /* PHY Control Register in CSR */
+#define E1000_PBA	0x01000  /* Packet Buffer Allocation - RW */
+#define E1000_PBS	0x01008  /* Packet Buffer Size */
+#define E1000_EEMNGCTL	0x01010  /* MNG EEprom Control */
+#define E1000_EEARBC	0x01024  /* EEPROM Auto Read Bus Control */
+#define E1000_EEWR	0x0102C  /* EEPROM Write Register - RW */
+#define E1000_FLOP	0x0103C  /* FLASH Opcode Register */
+#define E1000_I2CCMD	0x01028  /* SFPI2C Command Register - RW */
+#define E1000_I2CPARAMS	0x0102C /* SFPI2C Parameters Register - RW */
+#define E1000_I2CBB_EN	0x00000100  /* I2C - Bit Bang Enable */
+#define E1000_I2C_CLK_OUT	0x00000200  /* I2C- Clock */
+#define E1000_I2C_DATA_OUT	0x00000400  /* I2C- Data Out */
+#define E1000_I2C_DATA_OE_N	0x00000800  /* I2C- Data Output Enable */
+#define E1000_I2C_DATA_IN	0x00001000  /* I2C- Data In */
+#define E1000_I2C_CLK_OE_N	0x00002000  /* I2C- Clock Output Enable */
+#define E1000_I2C_CLK_IN	0x00004000  /* I2C- Clock In */
+#define E1000_I2C_CLK_STRETCH_DIS	0x00008000 /* I2C- Dis Clk Stretching */
+#define E1000_WDSTP	0x01040  /* Watchdog Setup - RW */
+#define E1000_SWDSTS	0x01044  /* SW Device Status - RW */
+#define E1000_FRTIMER	0x01048  /* Free Running Timer - RW */
+#define E1000_TCPTIMER	0x0104C  /* TCP Timer - RW */
+#define E1000_VPDDIAG	0x01060  /* VPD Diagnostic - RO */
+#define E1000_ICR_V2	0x01500  /* Intr Cause - new location - RC */
+#define E1000_ICS_V2	0x01504  /* Intr Cause Set - new location - WO */
+#define E1000_IMS_V2	0x01508  /* Intr Mask Set/Read - new location - RW */
+#define E1000_IMC_V2	0x0150C  /* Intr Mask Clear - new location - WO */
+#define E1000_IAM_V2	0x01510  /* Intr Ack Auto Mask - new location - RW */
+#define E1000_ERT	0x02008  /* Early Rx Threshold - RW */
+#define E1000_FCRTL	0x02160  /* Flow Control Receive Threshold Low - RW */
+#define E1000_FCRTH	0x02168  /* Flow Control Receive Threshold High - RW */
+#define E1000_PSRCTL	0x02170  /* Packet Split Receive Control - RW */
+#define E1000_RDFH	0x02410  /* Rx Data FIFO Head - RW */
+#define E1000_RDFT	0x02418  /* Rx Data FIFO Tail - RW */
+#define E1000_RDFHS	0x02420  /* Rx Data FIFO Head Saved - RW */
+#define E1000_RDFTS	0x02428  /* Rx Data FIFO Tail Saved - RW */
+#define E1000_RDFPC	0x02430  /* Rx Data FIFO Packet Count - RW */
+#define E1000_PBRTH	0x02458  /* PB Rx Arbitration Threshold - RW */
+#define E1000_FCRTV	0x02460  /* Flow Control Refresh Timer Value - RW */
+/* Split and Replication Rx Control - RW */
+#define E1000_RDPUMB	0x025CC  /* DMA Rx Descriptor uC Mailbox - RW */
+#define E1000_RDPUAD	0x025D0  /* DMA Rx Descriptor uC Addr Command - RW */
+#define E1000_RDPUWD	0x025D4  /* DMA Rx Descriptor uC Data Write - RW */
+#define E1000_RDPURD	0x025D8  /* DMA Rx Descriptor uC Data Read - RW */
+#define E1000_RDPUCTL	0x025DC  /* DMA Rx Descriptor uC Control - RW */
+#define E1000_PBDIAG	0x02458  /* Packet Buffer Diagnostic - RW */
+#define E1000_RXPBS	0x02404  /* Rx Packet Buffer Size - RW */
+#define E1000_IRPBS	0x02404 /* Same as RXPBS, renamed for newer Si - RW */
+#define E1000_PBRWAC	0x024E8 /* Rx packet buffer wrap around counter - RO */
+#define E1000_RDTR	0x02820  /* Rx Delay Timer - RW */
+#define E1000_RADV	0x0282C  /* Rx Interrupt Absolute Delay Timer - RW */
+#define E1000_EMIADD	0x10     /* Extended Memory Indirect Address */
+#define E1000_EMIDATA	0x11     /* Extended Memory Indirect Data */
+#define E1000_SRWR		0x12018  /* Shadow Ram Write Register - RW */
+#define E1000_I210_FLMNGCTL	0x12038
+#define E1000_I210_FLMNGDATA	0x1203C
+#define E1000_I210_FLMNGCNT	0x12040
 
-/* Filtering Registers */
-#define E1000_SAQF(_n) (0x5980 + 4 * (_n))
-#define E1000_DAQF(_n) (0x59A0 + 4 * (_n))
-#define E1000_SPQF(_n) (0x59C0 + 4 * (_n))
-#define E1000_FTQF(_n) (0x59E0 + 4 * (_n))
-#define E1000_SAQF0 E1000_SAQF(0)
-#define E1000_DAQF0 E1000_DAQF(0)
-#define E1000_SPQF0 E1000_SPQF(0)
-#define E1000_FTQF0 E1000_FTQF(0)
-#define E1000_SYNQF(_n) (0x055FC + (4 * (_n))) /* SYN Packet Queue Fltr */
-#define E1000_ETQF(_n)  (0x05CB0 + (4 * (_n))) /* EType Queue Fltr */
-
-#define E1000_RQDPC(_n) (0x0C030 + ((_n) * 0x40))
+#define E1000_I210_FLSWCTL	0x12048
+#define E1000_I210_FLSWDATA	0x1204C
+#define E1000_I210_FLSWCNT	0x12050
 
-/* DMA Coalescing registers */
-#define E1000_DMACR             0x02508 /* Control Register */
-#define E1000_DMCTXTH           0x03550 /* Transmit Threshold */
-#define E1000_DMCTLX            0x02514 /* Time to Lx Request */
-#define E1000_DMCRTRH           0x05DD0 /* Receive Packet Rate Threshold */
-#define E1000_DMCCNT            0x05DD4 /* Current Rx Count */
-#define E1000_FCRTC             0x02170 /* Flow Control Rx high watermark */
-#define E1000_PCIEMISC          0x05BB8 /* PCIE misc config register */
-
-/* TX Rate Limit Registers */
-#define E1000_RTTDQSEL	0x3604	/* Tx Desc Plane Queue Select - WO */
-#define E1000_RTTBCNRC	0x36B0	/* Tx BCN Rate-Scheduler Config - WO */
-
-/* Split and Replication RX Control - RW */
-#define E1000_RXPBS    0x02404  /* Rx Packet Buffer Size - RW */
-/*
- * Convenience macros
+#define E1000_I210_FLA		0x1201C
+
+#define E1000_INVM_DATA_REG(_n)	(0x12120 + 4*(_n))
+#define E1000_INVM_SIZE		64 /* Number of INVM Data Registers */
+
+/* QAV Tx mode control register */
+#define E1000_I210_TQAVCTRL	0x3570
+
+/* QAV Tx mode control register bitfields masks */
+/* QAV enable */
+#define E1000_TQAVCTRL_MODE			(1 << 0)
+/* Fetching arbitration type */
+#define E1000_TQAVCTRL_FETCH_ARB		(1 << 4)
+/* Fetching timer enable */
+#define E1000_TQAVCTRL_FETCH_TIMER_ENABLE	(1 << 5)
+/* Launch arbitration type */
+#define E1000_TQAVCTRL_LAUNCH_ARB		(1 << 8)
+/* Launch timer enable */
+#define E1000_TQAVCTRL_LAUNCH_TIMER_ENABLE	(1 << 9)
+/* SP waits for SR enable */
+#define E1000_TQAVCTRL_SP_WAIT_SR		(1 << 10)
+/* Fetching timer correction */
+#define E1000_TQAVCTRL_FETCH_TIMER_DELTA_OFFSET	16
+#define E1000_TQAVCTRL_FETCH_TIMER_DELTA	\
+			(0xFFFF << E1000_TQAVCTRL_FETCH_TIMER_DELTA_OFFSET)
+
+/* High credit registers where _n can be 0 or 1. */
+#define E1000_I210_TQAVHC(_n)			(0x300C + 0x40 * (_n))
+
+/* Queues fetch arbitration priority control register */
+#define E1000_I210_TQAVARBCTRL			0x3574
+/* Queues priority masks where _n and _p can be 0-3. */
+#define E1000_TQAVARBCTRL_QUEUE_PRI(_n, _p)	((_p) << (2 * _n))
+/* QAV Tx mode control registers where _n can be 0 or 1. */
+#define E1000_I210_TQAVCC(_n)			(0x3004 + 0x40 * (_n))
+
+/* QAV Tx mode control register bitfields masks */
+#define E1000_TQAVCC_IDLE_SLOPE		0xFFFF /* Idle slope */
+#define E1000_TQAVCC_KEEP_CREDITS	(1 << 30) /* Keep credits opt enable */
+#define E1000_TQAVCC_QUEUE_MODE		(1 << 31) /* SP vs. SR Tx mode */
+
+/* Good transmitted packets counter registers */
+#define E1000_PQGPTC(_n)		(0x010014 + (0x100 * (_n)))
+
+/* Queues packet buffer size masks where _n can be 0-3 and _s 0-63 [kB] */
+#define E1000_I210_TXPBS_SIZE(_n, _s)	((_s) << (6 * _n))
+
+#define E1000_MMDAC			13 /* MMD Access Control */
+#define E1000_MMDAAD			14 /* MMD Access Address/Data */
+
+/* Convenience macros
  *
  * Note: "_n" is the queue number of the register to be written to.
  *
  * Example usage:
  * E1000_RDBAL_REG(current_rx_queue)
  */
-#define E1000_RDBAL(_n)   ((_n) < 4 ? (0x02800 + ((_n) * 0x100)) \
-				    : (0x0C000 + ((_n) * 0x40)))
-#define E1000_RDBAH(_n)   ((_n) < 4 ? (0x02804 + ((_n) * 0x100)) \
-				    : (0x0C004 + ((_n) * 0x40)))
-#define E1000_RDLEN(_n)   ((_n) < 4 ? (0x02808 + ((_n) * 0x100)) \
-				    : (0x0C008 + ((_n) * 0x40)))
-#define E1000_SRRCTL(_n)  ((_n) < 4 ? (0x0280C + ((_n) * 0x100)) \
-				    : (0x0C00C + ((_n) * 0x40)))
-#define E1000_RDH(_n)     ((_n) < 4 ? (0x02810 + ((_n) * 0x100)) \
-				    : (0x0C010 + ((_n) * 0x40)))
-#define E1000_RDT(_n)     ((_n) < 4 ? (0x02818 + ((_n) * 0x100)) \
-				    : (0x0C018 + ((_n) * 0x40)))
-#define E1000_RXDCTL(_n)  ((_n) < 4 ? (0x02828 + ((_n) * 0x100)) \
-				    : (0x0C028 + ((_n) * 0x40)))
-#define E1000_TDBAL(_n)   ((_n) < 4 ? (0x03800 + ((_n) * 0x100)) \
-				    : (0x0E000 + ((_n) * 0x40)))
-#define E1000_TDBAH(_n)   ((_n) < 4 ? (0x03804 + ((_n) * 0x100)) \
-				    : (0x0E004 + ((_n) * 0x40)))
-#define E1000_TDLEN(_n)   ((_n) < 4 ? (0x03808 + ((_n) * 0x100)) \
-				    : (0x0E008 + ((_n) * 0x40)))
-#define E1000_TDH(_n)     ((_n) < 4 ? (0x03810 + ((_n) * 0x100)) \
-				    : (0x0E010 + ((_n) * 0x40)))
-#define E1000_TDT(_n)     ((_n) < 4 ? (0x03818 + ((_n) * 0x100)) \
-				    : (0x0E018 + ((_n) * 0x40)))
-#define E1000_TXDCTL(_n)  ((_n) < 4 ? (0x03828 + ((_n) * 0x100)) \
-				    : (0x0E028 + ((_n) * 0x40)))
-#define E1000_DCA_TXCTRL(_n) (0x03814 + (_n << 8))
-#define E1000_DCA_RXCTRL(_n) (0x02814 + (_n << 8))
-#define E1000_TDWBAL(_n)  ((_n) < 4 ? (0x03838 + ((_n) * 0x100)) \
-				    : (0x0E038 + ((_n) * 0x40)))
-#define E1000_TDWBAH(_n)  ((_n) < 4 ? (0x0383C + ((_n) * 0x100)) \
-				    : (0x0E03C + ((_n) * 0x40)))
-#define E1000_TDFH     0x03410  /* TX Data FIFO Head - RW */
-#define E1000_TDFT     0x03418  /* TX Data FIFO Tail - RW */
-#define E1000_TDFHS    0x03420  /* TX Data FIFO Head Saved - RW */
-#define E1000_TDFPC    0x03430  /* TX Data FIFO Packet Count - RW */
-#define E1000_DTXCTL   0x03590  /* DMA TX Control - RW */
-#define E1000_CRCERRS  0x04000  /* CRC Error Count - R/clr */
-#define E1000_ALGNERRC 0x04004  /* Alignment Error Count - R/clr */
-#define E1000_SYMERRS  0x04008  /* Symbol Error Count - R/clr */
-#define E1000_RXERRC   0x0400C  /* Receive Error Count - R/clr */
-#define E1000_MPC      0x04010  /* Missed Packet Count - R/clr */
-#define E1000_SCC      0x04014  /* Single Collision Count - R/clr */
-#define E1000_ECOL     0x04018  /* Excessive Collision Count - R/clr */
-#define E1000_MCC      0x0401C  /* Multiple Collision Count - R/clr */
-#define E1000_LATECOL  0x04020  /* Late Collision Count - R/clr */
-#define E1000_COLC     0x04028  /* Collision Count - R/clr */
-#define E1000_DC       0x04030  /* Defer Count - R/clr */
-#define E1000_TNCRS    0x04034  /* TX-No CRS - R/clr */
-#define E1000_SEC      0x04038  /* Sequence Error Count - R/clr */
-#define E1000_CEXTERR  0x0403C  /* Carrier Extension Error Count - R/clr */
-#define E1000_RLEC     0x04040  /* Receive Length Error Count - R/clr */
-#define E1000_XONRXC   0x04048  /* XON RX Count - R/clr */
-#define E1000_XONTXC   0x0404C  /* XON TX Count - R/clr */
-#define E1000_XOFFRXC  0x04050  /* XOFF RX Count - R/clr */
-#define E1000_XOFFTXC  0x04054  /* XOFF TX Count - R/clr */
-#define E1000_FCRUC    0x04058  /* Flow Control RX Unsupported Count- R/clr */
-#define E1000_PRC64    0x0405C  /* Packets RX (64 bytes) - R/clr */
-#define E1000_PRC127   0x04060  /* Packets RX (65-127 bytes) - R/clr */
-#define E1000_PRC255   0x04064  /* Packets RX (128-255 bytes) - R/clr */
-#define E1000_PRC511   0x04068  /* Packets RX (255-511 bytes) - R/clr */
-#define E1000_PRC1023  0x0406C  /* Packets RX (512-1023 bytes) - R/clr */
-#define E1000_PRC1522  0x04070  /* Packets RX (1024-1522 bytes) - R/clr */
-#define E1000_GPRC     0x04074  /* Good Packets RX Count - R/clr */
-#define E1000_BPRC     0x04078  /* Broadcast Packets RX Count - R/clr */
-#define E1000_MPRC     0x0407C  /* Multicast Packets RX Count - R/clr */
-#define E1000_GPTC     0x04080  /* Good Packets TX Count - R/clr */
-#define E1000_GORCL    0x04088  /* Good Octets RX Count Low - R/clr */
-#define E1000_GORCH    0x0408C  /* Good Octets RX Count High - R/clr */
-#define E1000_GOTCL    0x04090  /* Good Octets TX Count Low - R/clr */
-#define E1000_GOTCH    0x04094  /* Good Octets TX Count High - R/clr */
-#define E1000_RNBC     0x040A0  /* RX No Buffers Count - R/clr */
-#define E1000_RUC      0x040A4  /* RX Undersize Count - R/clr */
-#define E1000_RFC      0x040A8  /* RX Fragment Count - R/clr */
-#define E1000_ROC      0x040AC  /* RX Oversize Count - R/clr */
-#define E1000_RJC      0x040B0  /* RX Jabber Count - R/clr */
-#define E1000_MGTPRC   0x040B4  /* Management Packets RX Count - R/clr */
-#define E1000_MGTPDC   0x040B8  /* Management Packets Dropped Count - R/clr */
-#define E1000_MGTPTC   0x040BC  /* Management Packets TX Count - R/clr */
-#define E1000_TORL     0x040C0  /* Total Octets RX Low - R/clr */
-#define E1000_TORH     0x040C4  /* Total Octets RX High - R/clr */
-#define E1000_TOTL     0x040C8  /* Total Octets TX Low - R/clr */
-#define E1000_TOTH     0x040CC  /* Total Octets TX High - R/clr */
-#define E1000_TPR      0x040D0  /* Total Packets RX - R/clr */
-#define E1000_TPT      0x040D4  /* Total Packets TX - R/clr */
-#define E1000_PTC64    0x040D8  /* Packets TX (64 bytes) - R/clr */
-#define E1000_PTC127   0x040DC  /* Packets TX (65-127 bytes) - R/clr */
-#define E1000_PTC255   0x040E0  /* Packets TX (128-255 bytes) - R/clr */
-#define E1000_PTC511   0x040E4  /* Packets TX (256-511 bytes) - R/clr */
-#define E1000_PTC1023  0x040E8  /* Packets TX (512-1023 bytes) - R/clr */
-#define E1000_PTC1522  0x040EC  /* Packets TX (1024-1522 Bytes) - R/clr */
-#define E1000_MPTC     0x040F0  /* Multicast Packets TX Count - R/clr */
-#define E1000_BPTC     0x040F4  /* Broadcast Packets TX Count - R/clr */
-#define E1000_TSCTC    0x040F8  /* TCP Segmentation Context TX - R/clr */
-#define E1000_TSCTFC   0x040FC  /* TCP Segmentation Context TX Fail - R/clr */
-#define E1000_IAC      0x04100  /* Interrupt Assertion Count */
-/* Interrupt Cause Rx Packet Timer Expire Count */
-#define E1000_ICRXPTC  0x04104
-/* Interrupt Cause Rx Absolute Timer Expire Count */
-#define E1000_ICRXATC  0x04108
-/* Interrupt Cause Tx Packet Timer Expire Count */
-#define E1000_ICTXPTC  0x0410C
-/* Interrupt Cause Tx Absolute Timer Expire Count */
-#define E1000_ICTXATC  0x04110
-/* Interrupt Cause Tx Queue Empty Count */
-#define E1000_ICTXQEC  0x04118
-/* Interrupt Cause Tx Queue Minimum Threshold Count */
-#define E1000_ICTXQMTC 0x0411C
-/* Interrupt Cause Rx Descriptor Minimum Threshold Count */
-#define E1000_ICRXDMTC 0x04120
-#define E1000_ICRXOC   0x04124  /* Interrupt Cause Receiver Overrun Count */
-#define E1000_PCS_CFG0    0x04200  /* PCS Configuration 0 - RW */
-#define E1000_PCS_LCTL    0x04208  /* PCS Link Control - RW */
-#define E1000_PCS_LSTAT   0x0420C  /* PCS Link Status - RO */
-#define E1000_CBTMPC      0x0402C  /* Circuit Breaker TX Packet Count */
-#define E1000_HTDPMC      0x0403C  /* Host Transmit Discarded Packets */
-#define E1000_CBRMPC      0x040FC  /* Circuit Breaker RX Packet Count */
-#define E1000_RPTHC       0x04104  /* Rx Packets To Host */
-#define E1000_HGPTC       0x04118  /* Host Good Packets TX Count */
-#define E1000_HTCBDPC     0x04124  /* Host TX Circuit Breaker Dropped Count */
-#define E1000_HGORCL      0x04128  /* Host Good Octets Received Count Low */
-#define E1000_HGORCH      0x0412C  /* Host Good Octets Received Count High */
-#define E1000_HGOTCL      0x04130  /* Host Good Octets Transmit Count Low */
-#define E1000_HGOTCH      0x04134  /* Host Good Octets Transmit Count High */
-#define E1000_LENERRS     0x04138  /* Length Errors Count */
-#define E1000_SCVPC       0x04228  /* SerDes/SGMII Code Violation Pkt Count */
-#define E1000_PCS_ANADV   0x04218  /* AN advertisement - RW */
-#define E1000_PCS_LPAB    0x0421C  /* Link Partner Ability - RW */
-#define E1000_PCS_NPTX    0x04220  /* AN Next Page Transmit - RW */
-#define E1000_PCS_LPABNP  0x04224  /* Link Partner Ability Next Page - RW */
-#define E1000_RXCSUM   0x05000  /* RX Checksum Control - RW */
-#define E1000_RLPML    0x05004  /* RX Long Packet Max Length */
-#define E1000_RFCTL    0x05008  /* Receive Filter Control*/
-#define E1000_MTA      0x05200  /* Multicast Table Array - RW Array */
-#define E1000_RA       0x05400  /* Receive Address - RW Array */
-#define E1000_RA2      0x054E0  /* 2nd half of receive address array - RW Array */
-#define E1000_PSRTYPE(_i)       (0x05480 + ((_i) * 4))
-#define E1000_RAL(_i)  (((_i) <= 15) ? (0x05400 + ((_i) * 8)) : \
-                                       (0x054E0 + ((_i - 16) * 8)))
-#define E1000_RAH(_i)  (((_i) <= 15) ? (0x05404 + ((_i) * 8)) : \
-                                       (0x054E4 + ((_i - 16) * 8)))
-#define E1000_IP4AT_REG(_i)     (0x05840 + ((_i) * 8))
-#define E1000_IP6AT_REG(_i)     (0x05880 + ((_i) * 4))
-#define E1000_WUPM_REG(_i)      (0x05A00 + ((_i) * 4))
-#define E1000_FFMT_REG(_i)      (0x09000 + ((_i) * 8))
-#define E1000_FFVT_REG(_i)      (0x09800 + ((_i) * 8))
-#define E1000_FFLT_REG(_i)      (0x05F00 + ((_i) * 8))
-#define E1000_VFTA     0x05600  /* VLAN Filter Table Array - RW Array */
-#define E1000_VT_CTL   0x0581C  /* VMDq Control - RW */
-#define E1000_WUC      0x05800  /* Wakeup Control - RW */
-#define E1000_WUFC     0x05808  /* Wakeup Filter Control - RW */
-#define E1000_WUS      0x05810  /* Wakeup Status - RO */
-#define E1000_MANC     0x05820  /* Management Control - RW */
-#define E1000_IPAV     0x05838  /* IP Address Valid - RW */
-#define E1000_WUPL     0x05900  /* Wakeup Packet Length - RW */
-
-#define E1000_SW_FW_SYNC  0x05B5C /* Software-Firmware Synchronization - RW */
-#define E1000_CCMCTL      0x05B48 /* CCM Control Register */
-#define E1000_GIOCTL      0x05B44 /* GIO Analog Control Register */
-#define E1000_SCCTL       0x05B4C /* PCIc PLL Configuration Register */
-#define E1000_GCR         0x05B00 /* PCI-Ex Control */
-#define E1000_FACTPS    0x05B30 /* Function Active and Power State to MNG */
-#define E1000_SWSM      0x05B50 /* SW Semaphore */
-#define E1000_FWSM      0x05B54 /* FW Semaphore */
-#define E1000_DCA_CTRL  0x05B74 /* DCA Control - RW */
+#define E1000_RDBAL(_n)	((_n) < 4 ? (0x02800 + ((_n) * 0x100)) : \
+			 (0x0C000 + ((_n) * 0x40)))
+#define E1000_RDBAH(_n)	((_n) < 4 ? (0x02804 + ((_n) * 0x100)) : \
+			 (0x0C004 + ((_n) * 0x40)))
+#define E1000_RDLEN(_n)	((_n) < 4 ? (0x02808 + ((_n) * 0x100)) : \
+			 (0x0C008 + ((_n) * 0x40)))
+#define E1000_SRRCTL(_n)	((_n) < 4 ? (0x0280C + ((_n) * 0x100)) : \
+				 (0x0C00C + ((_n) * 0x40)))
+#define E1000_RDH(_n)	((_n) < 4 ? (0x02810 + ((_n) * 0x100)) : \
+			 (0x0C010 + ((_n) * 0x40)))
+#define E1000_RXCTL(_n)	((_n) < 4 ? (0x02814 + ((_n) * 0x100)) : \
+			 (0x0C014 + ((_n) * 0x40)))
+#define E1000_DCA_RXCTRL(_n)	E1000_RXCTL(_n)
+#define E1000_RDT(_n)	((_n) < 4 ? (0x02818 + ((_n) * 0x100)) : \
+			 (0x0C018 + ((_n) * 0x40)))
+#define E1000_RXDCTL(_n)	((_n) < 4 ? (0x02828 + ((_n) * 0x100)) : \
+				 (0x0C028 + ((_n) * 0x40)))
+#define E1000_RQDPC(_n)	((_n) < 4 ? (0x02830 + ((_n) * 0x100)) : \
+			 (0x0C030 + ((_n) * 0x40)))
+#define E1000_TDBAL(_n)	((_n) < 4 ? (0x03800 + ((_n) * 0x100)) : \
+			 (0x0E000 + ((_n) * 0x40)))
+#define E1000_TDBAH(_n)	((_n) < 4 ? (0x03804 + ((_n) * 0x100)) : \
+			 (0x0E004 + ((_n) * 0x40)))
+#define E1000_TDLEN(_n)	((_n) < 4 ? (0x03808 + ((_n) * 0x100)) : \
+			 (0x0E008 + ((_n) * 0x40)))
+#define E1000_TDH(_n)	((_n) < 4 ? (0x03810 + ((_n) * 0x100)) : \
+			 (0x0E010 + ((_n) * 0x40)))
+#define E1000_TXCTL(_n)	((_n) < 4 ? (0x03814 + ((_n) * 0x100)) : \
+			 (0x0E014 + ((_n) * 0x40)))
+#define E1000_DCA_TXCTRL(_n) E1000_TXCTL(_n)
+#define E1000_TDT(_n)	((_n) < 4 ? (0x03818 + ((_n) * 0x100)) : \
+			 (0x0E018 + ((_n) * 0x40)))
+#define E1000_TXDCTL(_n)	((_n) < 4 ? (0x03828 + ((_n) * 0x100)) : \
+				 (0x0E028 + ((_n) * 0x40)))
+#define E1000_TDWBAL(_n)	((_n) < 4 ? (0x03838 + ((_n) * 0x100)) : \
+				 (0x0E038 + ((_n) * 0x40)))
+#define E1000_TDWBAH(_n)	((_n) < 4 ? (0x0383C + ((_n) * 0x100)) : \
+				 (0x0E03C + ((_n) * 0x40)))
+#define E1000_TARC(_n)		(0x03840 + ((_n) * 0x100))
+#define E1000_RSRPD		0x02C00  /* Rx Small Packet Detect - RW */
+#define E1000_RAID		0x02C08  /* Receive Ack Interrupt Delay - RW */
+#define E1000_KABGTXD		0x03004  /* AFE Band Gap Transmit Ref Data */
+#define E1000_PSRTYPE(_i)	(0x05480 + ((_i) * 4))
+#define E1000_RAL(_i)		(((_i) <= 15) ? (0x05400 + ((_i) * 8)) : \
+				 (0x054E0 + ((_i - 16) * 8)))
+#define E1000_RAH(_i)		(((_i) <= 15) ? (0x05404 + ((_i) * 8)) : \
+				 (0x054E4 + ((_i - 16) * 8)))
+#define E1000_SHRAL(_i)		(0x05438 + ((_i) * 8))
+#define E1000_SHRAH(_i)		(0x0543C + ((_i) * 8))
+#define E1000_IP4AT_REG(_i)	(0x05840 + ((_i) * 8))
+#define E1000_IP6AT_REG(_i)	(0x05880 + ((_i) * 4))
+#define E1000_WUPM_REG(_i)	(0x05A00 + ((_i) * 4))
+#define E1000_FFMT_REG(_i)	(0x09000 + ((_i) * 8))
+#define E1000_FFVT_REG(_i)	(0x09800 + ((_i) * 8))
+#define E1000_FFLT_REG(_i)	(0x05F00 + ((_i) * 8))
+#define E1000_PBSLAC		0x03100  /* Pkt Buffer Slave Access Control */
+#define E1000_PBSLAD(_n)	(0x03110 + (0x4 * (_n)))  /* Pkt Buffer DWORD */
+#define E1000_TXPBS		0x03404  /* Tx Packet Buffer Size - RW */
+/* Same as TXPBS, renamed for newer Si - RW */
+#define E1000_ITPBS		0x03404
+#define E1000_TDFH		0x03410  /* Tx Data FIFO Head - RW */
+#define E1000_TDFT		0x03418  /* Tx Data FIFO Tail - RW */
+#define E1000_TDFHS		0x03420  /* Tx Data FIFO Head Saved - RW */
+#define E1000_TDFTS		0x03428  /* Tx Data FIFO Tail Saved - RW */
+#define E1000_TDFPC		0x03430  /* Tx Data FIFO Packet Count - RW */
+#define E1000_TDPUMB		0x0357C  /* DMA Tx Desc uC Mail Box - RW */
+#define E1000_TDPUAD		0x03580  /* DMA Tx Desc uC Addr Command - RW */
+#define E1000_TDPUWD		0x03584  /* DMA Tx Desc uC Data Write - RW */
+#define E1000_TDPURD		0x03588  /* DMA Tx Desc uC Data  Read  - RW */
+#define E1000_TDPUCTL		0x0358C  /* DMA Tx Desc uC Control - RW */
+#define E1000_DTXCTL		0x03590  /* DMA Tx Control - RW */
+#define E1000_DTXTCPFLGL	0x0359C /* DMA Tx Control flag low - RW */
+#define E1000_DTXTCPFLGH	0x035A0 /* DMA Tx Control flag high - RW */
+/* DMA Tx Max Total Allow Size Reqs - RW */
+#define E1000_DTXMXSZRQ		0x03540
+#define E1000_TIDV	0x03820  /* Tx Interrupt Delay Value - RW */
+#define E1000_TADV	0x0382C  /* Tx Interrupt Absolute Delay Val - RW */
+#define E1000_CRCERRS	0x04000  /* CRC Error Count - R/clr */
+#define E1000_ALGNERRC	0x04004  /* Alignment Error Count - R/clr */
+#define E1000_SYMERRS	0x04008  /* Symbol Error Count - R/clr */
+#define E1000_RXERRC	0x0400C  /* Receive Error Count - R/clr */
+#define E1000_MPC	0x04010  /* Missed Packet Count - R/clr */
+#define E1000_SCC	0x04014  /* Single Collision Count - R/clr */
+#define E1000_ECOL	0x04018  /* Excessive Collision Count - R/clr */
+#define E1000_MCC	0x0401C  /* Multiple Collision Count - R/clr */
+#define E1000_LATECOL	0x04020  /* Late Collision Count - R/clr */
+#define E1000_COLC	0x04028  /* Collision Count - R/clr */
+#define E1000_DC	0x04030  /* Defer Count - R/clr */
+#define E1000_TNCRS	0x04034  /* Tx-No CRS - R/clr */
+#define E1000_SEC	0x04038  /* Sequence Error Count - R/clr */
+#define E1000_CEXTERR	0x0403C  /* Carrier Extension Error Count - R/clr */
+#define E1000_RLEC	0x04040  /* Receive Length Error Count - R/clr */
+#define E1000_XONRXC	0x04048  /* XON Rx Count - R/clr */
+#define E1000_XONTXC	0x0404C  /* XON Tx Count - R/clr */
+#define E1000_XOFFRXC	0x04050  /* XOFF Rx Count - R/clr */
+#define E1000_XOFFTXC	0x04054  /* XOFF Tx Count - R/clr */
+#define E1000_FCRUC	0x04058  /* Flow Control Rx Unsupported Count- R/clr */
+#define E1000_PRC64	0x0405C  /* Packets Rx (64 bytes) - R/clr */
+#define E1000_PRC127	0x04060  /* Packets Rx (65-127 bytes) - R/clr */
+#define E1000_PRC255	0x04064  /* Packets Rx (128-255 bytes) - R/clr */
+#define E1000_PRC511	0x04068  /* Packets Rx (255-511 bytes) - R/clr */
+#define E1000_PRC1023	0x0406C  /* Packets Rx (512-1023 bytes) - R/clr */
+#define E1000_PRC1522	0x04070  /* Packets Rx (1024-1522 bytes) - R/clr */
+#define E1000_GPRC	0x04074  /* Good Packets Rx Count - R/clr */
+#define E1000_BPRC	0x04078  /* Broadcast Packets Rx Count - R/clr */
+#define E1000_MPRC	0x0407C  /* Multicast Packets Rx Count - R/clr */
+#define E1000_GPTC	0x04080  /* Good Packets Tx Count - R/clr */
+#define E1000_GORCL	0x04088  /* Good Octets Rx Count Low - R/clr */
+#define E1000_GORCH	0x0408C  /* Good Octets Rx Count High - R/clr */
+#define E1000_GOTCL	0x04090  /* Good Octets Tx Count Low - R/clr */
+#define E1000_GOTCH	0x04094  /* Good Octets Tx Count High - R/clr */
+#define E1000_RNBC	0x040A0  /* Rx No Buffers Count - R/clr */
+#define E1000_RUC	0x040A4  /* Rx Undersize Count - R/clr */
+#define E1000_RFC	0x040A8  /* Rx Fragment Count - R/clr */
+#define E1000_ROC	0x040AC  /* Rx Oversize Count - R/clr */
+#define E1000_RJC	0x040B0  /* Rx Jabber Count - R/clr */
+#define E1000_MGTPRC	0x040B4  /* Management Packets Rx Count - R/clr */
+#define E1000_MGTPDC	0x040B8  /* Management Packets Dropped Count - R/clr */
+#define E1000_MGTPTC	0x040BC  /* Management Packets Tx Count - R/clr */
+#define E1000_TORL	0x040C0  /* Total Octets Rx Low - R/clr */
+#define E1000_TORH	0x040C4  /* Total Octets Rx High - R/clr */
+#define E1000_TOTL	0x040C8  /* Total Octets Tx Low - R/clr */
+#define E1000_TOTH	0x040CC  /* Total Octets Tx High - R/clr */
+#define E1000_TPR	0x040D0  /* Total Packets Rx - R/clr */
+#define E1000_TPT	0x040D4  /* Total Packets Tx - R/clr */
+#define E1000_PTC64	0x040D8  /* Packets Tx (64 bytes) - R/clr */
+#define E1000_PTC127	0x040DC  /* Packets Tx (65-127 bytes) - R/clr */
+#define E1000_PTC255	0x040E0  /* Packets Tx (128-255 bytes) - R/clr */
+#define E1000_PTC511	0x040E4  /* Packets Tx (256-511 bytes) - R/clr */
+#define E1000_PTC1023	0x040E8  /* Packets Tx (512-1023 bytes) - R/clr */
+#define E1000_PTC1522	0x040EC  /* Packets Tx (1024-1522 Bytes) - R/clr */
+#define E1000_MPTC	0x040F0  /* Multicast Packets Tx Count - R/clr */
+#define E1000_BPTC	0x040F4  /* Broadcast Packets Tx Count - R/clr */
+#define E1000_TSCTC	0x040F8  /* TCP Segmentation Context Tx - R/clr */
+#define E1000_TSCTFC	0x040FC  /* TCP Segmentation Context Tx Fail - R/clr */
+#define E1000_IAC	0x04100  /* Interrupt Assertion Count */
+#define E1000_ICRXPTC	0x04104  /* Interrupt Cause Rx Pkt Timer Expire Count */
+#define E1000_ICRXATC	0x04108  /* Interrupt Cause Rx Abs Timer Expire Count */
+#define E1000_ICTXPTC	0x0410C  /* Interrupt Cause Tx Pkt Timer Expire Count */
+#define E1000_ICTXATC	0x04110  /* Interrupt Cause Tx Abs Timer Expire Count */
+#define E1000_ICTXQEC	0x04118  /* Interrupt Cause Tx Queue Empty Count */
+#define E1000_ICTXQMTC	0x0411C  /* Interrupt Cause Tx Queue Min Thresh Count */
+#define E1000_ICRXDMTC	0x04120  /* Interrupt Cause Rx Desc Min Thresh Count */
+#define E1000_ICRXOC	0x04124  /* Interrupt Cause Receiver Overrun Count */
 
-/* RSS registers */
-#define E1000_MRQC      0x05818 /* Multiple Receive Control - RW */
-#define E1000_IMIR(_i)      (0x05A80 + ((_i) * 4))  /* Immediate Interrupt */
-#define E1000_IMIREXT(_i)   (0x05AA0 + ((_i) * 4))  /* Immediate Interrupt Ext*/
-#define E1000_IMIRVP    0x05AC0 /* Immediate Interrupt RX VLAN Priority - RW */
-/* MSI-X Allocation Register (_i) - RW */
-#define E1000_MSIXBM(_i)    (0x01600 + ((_i) * 4))
-/* Redirection Table - RW Array */
-#define E1000_RETA(_i)  (0x05C00 + ((_i) * 4))
-#define E1000_RSSRK(_i) (0x05C80 + ((_i) * 4)) /* RSS Random Key - RW Array */
+/* Virtualization statistical counters */
+#define E1000_PFVFGPRC(_n)	(0x010010 + (0x100 * (_n)))
+#define E1000_PFVFGPTC(_n)	(0x010014 + (0x100 * (_n)))
+#define E1000_PFVFGORC(_n)	(0x010018 + (0x100 * (_n)))
+#define E1000_PFVFGOTC(_n)	(0x010034 + (0x100 * (_n)))
+#define E1000_PFVFMPRC(_n)	(0x010038 + (0x100 * (_n)))
+#define E1000_PFVFGPRLBC(_n)	(0x010040 + (0x100 * (_n)))
+#define E1000_PFVFGPTLBC(_n)	(0x010044 + (0x100 * (_n)))
+#define E1000_PFVFGORLBC(_n)	(0x010048 + (0x100 * (_n)))
+#define E1000_PFVFGOTLBC(_n)	(0x010050 + (0x100 * (_n)))
 
+/* LinkSec */
+#define E1000_LSECTXUT		0x04300  /* Tx Untagged Pkt Cnt */
+#define E1000_LSECTXPKTE	0x04304  /* Encrypted Tx Pkts Cnt */
+#define E1000_LSECTXPKTP	0x04308  /* Protected Tx Pkt Cnt */
+#define E1000_LSECTXOCTE	0x0430C  /* Encrypted Tx Octets Cnt */
+#define E1000_LSECTXOCTP	0x04310  /* Protected Tx Octets Cnt */
+#define E1000_LSECRXUT		0x04314  /* Untagged non-Strict Rx Pkt Cnt */
+#define E1000_LSECRXOCTD	0x0431C  /* Rx Octets Decrypted Count */
+#define E1000_LSECRXOCTV	0x04320  /* Rx Octets Validated */
+#define E1000_LSECRXBAD		0x04324  /* Rx Bad Tag */
+#define E1000_LSECRXNOSCI	0x04328  /* Rx Packet No SCI Count */
+#define E1000_LSECRXUNSCI	0x0432C  /* Rx Packet Unknown SCI Count */
+#define E1000_LSECRXUNCH	0x04330  /* Rx Unchecked Packets Count */
+#define E1000_LSECRXDELAY	0x04340  /* Rx Delayed Packet Count */
+#define E1000_LSECRXLATE	0x04350  /* Rx Late Packets Count */
+#define E1000_LSECRXOK(_n)	(0x04360 + (0x04 * (_n))) /* Rx Pkt OK Cnt */
+#define E1000_LSECRXINV(_n)	(0x04380 + (0x04 * (_n))) /* Rx Invalid Cnt */
+#define E1000_LSECRXNV(_n)	(0x043A0 + (0x04 * (_n))) /* Rx Not Valid Cnt */
+#define E1000_LSECRXUNSA	0x043C0  /* Rx Unused SA Count */
+#define E1000_LSECRXNUSA	0x043D0  /* Rx Not Using SA Count */
+#define E1000_LSECTXCAP		0x0B000  /* Tx Capabilities Register - RO */
+#define E1000_LSECRXCAP		0x0B300  /* Rx Capabilities Register - RO */
+#define E1000_LSECTXCTRL	0x0B004  /* Tx Control - RW */
+#define E1000_LSECRXCTRL	0x0B304  /* Rx Control - RW */
+#define E1000_LSECTXSCL		0x0B008  /* Tx SCI Low - RW */
+#define E1000_LSECTXSCH		0x0B00C  /* Tx SCI High - RW */
+#define E1000_LSECTXSA		0x0B010  /* Tx SA0 - RW */
+#define E1000_LSECTXPN0		0x0B018  /* Tx SA PN 0 - RW */
+#define E1000_LSECTXPN1		0x0B01C  /* Tx SA PN 1 - RW */
+#define E1000_LSECRXSCL		0x0B3D0  /* Rx SCI Low - RW */
+#define E1000_LSECRXSCH		0x0B3E0  /* Rx SCI High - RW */
+/* LinkSec Tx 128-bit Key 0 - WO */
+#define E1000_LSECTXKEY0(_n)	(0x0B020 + (0x04 * (_n)))
+/* LinkSec Tx 128-bit Key 1 - WO */
+#define E1000_LSECTXKEY1(_n)	(0x0B030 + (0x04 * (_n)))
+#define E1000_LSECRXSA(_n)	(0x0B310 + (0x04 * (_n))) /* Rx SAs - RW */
+#define E1000_LSECRXPN(_n)	(0x0B330 + (0x04 * (_n))) /* Rx SAs - RW */
+/* LinkSec Rx Keys  - where _n is the SA no. and _m the 4 dwords of the 128 bit
+ * key - RW.
+ */
+#define E1000_LSECRXKEY(_n, _m)	(0x0B350 + (0x10 * (_n)) + (0x04 * (_m)))
+
+#define E1000_SSVPC		0x041A0 /* Switch Security Violation Pkt Cnt */
+#define E1000_IPSCTRL		0xB430  /* IpSec Control Register */
+#define E1000_IPSRXCMD		0x0B408 /* IPSec Rx Command Register - RW */
+#define E1000_IPSRXIDX		0x0B400 /* IPSec Rx Index - RW */
+/* IPSec Rx IPv4/v6 Address - RW */
+#define E1000_IPSRXIPADDR(_n)	(0x0B420 + (0x04 * (_n)))
+/* IPSec Rx 128-bit Key - RW */
+#define E1000_IPSRXKEY(_n)	(0x0B410 + (0x04 * (_n)))
+#define E1000_IPSRXSALT		0x0B404  /* IPSec Rx Salt - RW */
+#define E1000_IPSRXSPI		0x0B40C  /* IPSec Rx SPI - RW */
+/* IPSec Tx 128-bit Key - RW */
+#define E1000_IPSTXKEY(_n)	(0x0B460 + (0x04 * (_n)))
+#define E1000_IPSTXSALT		0x0B454  /* IPSec Tx Salt - RW */
+#define E1000_IPSTXIDX		0x0B450  /* IPSec Tx SA IDX - RW */
+#define E1000_PCS_CFG0	0x04200  /* PCS Configuration 0 - RW */
+#define E1000_PCS_LCTL	0x04208  /* PCS Link Control - RW */
+#define E1000_PCS_LSTAT	0x0420C  /* PCS Link Status - RO */
+#define E1000_CBTMPC	0x0402C  /* Circuit Breaker Tx Packet Count */
+#define E1000_HTDPMC	0x0403C  /* Host Transmit Discarded Packets */
+#define E1000_CBRDPC	0x04044  /* Circuit Breaker Rx Dropped Count */
+#define E1000_CBRMPC	0x040FC  /* Circuit Breaker Rx Packet Count */
+#define E1000_RPTHC	0x04104  /* Rx Packets To Host */
+#define E1000_HGPTC	0x04118  /* Host Good Packets Tx Count */
+#define E1000_HTCBDPC	0x04124  /* Host Tx Circuit Breaker Dropped Count */
+#define E1000_HGORCL	0x04128  /* Host Good Octets Received Count Low */
+#define E1000_HGORCH	0x0412C  /* Host Good Octets Received Count High */
+#define E1000_HGOTCL	0x04130  /* Host Good Octets Transmit Count Low */
+#define E1000_HGOTCH	0x04134  /* Host Good Octets Transmit Count High */
+#define E1000_LENERRS	0x04138  /* Length Errors Count */
+#define E1000_SCVPC	0x04228  /* SerDes/SGMII Code Violation Pkt Count */
+#define E1000_HRMPC	0x0A018  /* Header Redirection Missed Packet Count */
+#define E1000_PCS_ANADV	0x04218  /* AN advertisement - RW */
+#define E1000_PCS_LPAB	0x0421C  /* Link Partner Ability - RW */
+#define E1000_PCS_NPTX	0x04220  /* AN Next Page Transmit - RW */
+#define E1000_PCS_LPABNP	0x04224 /* Link Partner Ability Next Pg - RW */
+#define E1000_RXCSUM	0x05000  /* Rx Checksum Control - RW */
+#define E1000_RLPML	0x05004  /* Rx Long Packet Max Length */
+#define E1000_RFCTL	0x05008  /* Receive Filter Control*/
+#define E1000_MTA	0x05200  /* Multicast Table Array - RW Array */
+#define E1000_RA	0x05400  /* Receive Address - RW Array */
+#define E1000_RA2	0x054E0  /* 2nd half of Rx address array - RW Array */
+#define E1000_VFTA	0x05600  /* VLAN Filter Table Array - RW Array */
+#define E1000_VT_CTL	0x0581C  /* VMDq Control - RW */
+#define E1000_CIAA	0x05B88  /* Config Indirect Access Address - RW */
+#define E1000_CIAD	0x05B8C  /* Config Indirect Access Data - RW */
+#define E1000_VFQA0	0x0B000  /* VLAN Filter Queue Array 0 - RW Array */
+#define E1000_VFQA1	0x0B200  /* VLAN Filter Queue Array 1 - RW Array */
+#define E1000_WUC	0x05800  /* Wakeup Control - RW */
+#define E1000_WUFC	0x05808  /* Wakeup Filter Control - RW */
+#define E1000_WUS	0x05810  /* Wakeup Status - RO */
+#define E1000_MANC	0x05820  /* Management Control - RW */
+#define E1000_IPAV	0x05838  /* IP Address Valid - RW */
+#define E1000_IP4AT	0x05840  /* IPv4 Address Table - RW Array */
+#define E1000_IP6AT	0x05880  /* IPv6 Address Table - RW Array */
+#define E1000_WUPL	0x05900  /* Wakeup Packet Length - RW */
+#define E1000_WUPM	0x05A00  /* Wakeup Packet Memory - RO A */
+#define E1000_PBACL	0x05B68  /* MSIx PBA Clear - Read/Write 1's to clear */
+#define E1000_FFLT	0x05F00  /* Flexible Filter Length Table - RW Array */
+#define E1000_HOST_IF	0x08800  /* Host Interface */
+#define E1000_HIBBA	0x8F40   /* Host Interface Buffer Base Address */
+/* Flexible Host Filter Table */
+#define E1000_FHFT(_n)	(0x09000 + ((_n) * 0x100))
+/* Ext Flexible Host Filter Table */
+#define E1000_FHFT_EXT(_n)	(0x09A00 + ((_n) * 0x100))
+
+
+#define E1000_KMRNCTRLSTA	0x00034 /* MAC-PHY interface - RW */
+#define E1000_MANC2H		0x05860 /* Management Control To Host - RW */
+/* Management Decision Filters */
+#define E1000_MDEF(_n)		(0x05890 + (4 * (_n)))
+#define E1000_SW_FW_SYNC	0x05B5C /* SW-FW Synchronization - RW */
+#define E1000_CCMCTL	0x05B48 /* CCM Control Register */
+#define E1000_GIOCTL	0x05B44 /* GIO Analog Control Register */
+#define E1000_SCCTL	0x05B4C /* PCIc PLL Configuration Register */
+#define E1000_GCR	0x05B00 /* PCI-Ex Control */
+#define E1000_GCR2	0x05B64 /* PCI-Ex Control #2 */
+#define E1000_GSCL_1	0x05B10 /* PCI-Ex Statistic Control #1 */
+#define E1000_GSCL_2	0x05B14 /* PCI-Ex Statistic Control #2 */
+#define E1000_GSCL_3	0x05B18 /* PCI-Ex Statistic Control #3 */
+#define E1000_GSCL_4	0x05B1C /* PCI-Ex Statistic Control #4 */
+#define E1000_FACTPS	0x05B30 /* Function Active and Power State to MNG */
+#define E1000_SWSM	0x05B50 /* SW Semaphore */
+#define E1000_FWSM	0x05B54 /* FW Semaphore */
+/* Driver-only SW semaphore (not used by BOOT agents) */
+#define E1000_SWSM2	0x05B58
+#define E1000_DCA_ID	0x05B70 /* DCA Requester ID Information - RO */
+#define E1000_DCA_CTRL	0x05B74 /* DCA Control - RW */
+#define E1000_UFUSE	0x05B78 /* UFUSE - RO */
+#define E1000_FFLT_DBG	0x05F04 /* Debug Register */
+#define E1000_HICR	0x08F00 /* Host Interface Control */
+#define E1000_FWSTS	0x08F0C /* FW Status */
+
+/* RSS registers */
+#define E1000_CPUVEC	0x02C10 /* CPU Vector Register - RW */
+#define E1000_MRQC	0x05818 /* Multiple Receive Control - RW */
+#define E1000_IMIR(_i)	(0x05A80 + ((_i) * 4))  /* Immediate Interrupt */
+#define E1000_IMIREXT(_i)	(0x05AA0 + ((_i) * 4)) /* Immediate INTR Ext*/
+#define E1000_IMIRVP		0x05AC0 /* Immediate INT Rx VLAN Priority -RW */
+#define E1000_MSIXBM(_i)	(0x01600 + ((_i) * 4)) /* MSI-X Alloc Reg -RW */
+#define E1000_RETA(_i)	(0x05C00 + ((_i) * 4)) /* Redirection Table - RW */
+#define E1000_RSSRK(_i)	(0x05C80 + ((_i) * 4)) /* RSS Random Key - RW */
+#define E1000_RSSIM	0x05864 /* RSS Interrupt Mask */
+#define E1000_RSSIR	0x05868 /* RSS Interrupt Request */
 /* VT Registers */
-#define E1000_MBVFICR   0x00C80 /* Mailbox VF Cause - RWC */
-#define E1000_MBVFIMR   0x00C84 /* Mailbox VF int Mask - RW */
-#define E1000_VFLRE     0x00C88 /* VF Register Events - RWC */
-#define E1000_VFRE      0x00C8C /* VF Receive Enables */
-#define E1000_VFTE      0x00C90 /* VF Transmit Enables */
-#define E1000_QDE       0x02408 /* Queue Drop Enable - RW */
-#define E1000_DTXSWC    0x03500 /* DMA Tx Switch Control - RW */
-#define E1000_WVBR      0x03554 /* VM Wrong Behavior - RWS */
-#define E1000_RPLOLR    0x05AF0 /* Replication Offload - RW */
-#define E1000_UTA       0x0A000 /* Unicast Table Array - RW */
-#define E1000_IOVTCL    0x05BBC /* IOV Control Register */
+#define E1000_SWPBS	0x03004 /* Switch Packet Buffer Size - RW */
+#define E1000_MBVFICR	0x00C80 /* Mailbox VF Cause - RWC */
+#define E1000_MBVFIMR	0x00C84 /* Mailbox VF int Mask - RW */
+#define E1000_VFLRE	0x00C88 /* VF Register Events - RWC */
+#define E1000_VFRE	0x00C8C /* VF Receive Enables */
+#define E1000_VFTE	0x00C90 /* VF Transmit Enables */
+#define E1000_QDE	0x02408 /* Queue Drop Enable - RW */
+#define E1000_DTXSWC	0x03500 /* DMA Tx Switch Control - RW */
+#define E1000_WVBR	0x03554 /* VM Wrong Behavior - RWS */
+#define E1000_RPLOLR	0x05AF0 /* Replication Offload - RW */
+#define E1000_UTA	0x0A000 /* Unicast Table Array - RW */
+#define E1000_IOVTCL	0x05BBC /* IOV Control Register */
+#define E1000_VMRCTL	0X05D80 /* Virtual Mirror Rule Control */
+#define E1000_VMRVLAN	0x05D90 /* Virtual Mirror Rule VLAN */
+#define E1000_VMRVM	0x05DA0 /* Virtual Mirror Rule VM */
+#define E1000_MDFB	0x03558 /* Malicious Driver free block */
+#define E1000_LVMMC	0x03548 /* Last VM Misbehavior cause */
+#define E1000_TXSWC	0x05ACC /* Tx Switch Control */
+#define E1000_SCCRL	0x05DB0 /* Storm Control Control */
+#define E1000_BSCTRH	0x05DB8 /* Broadcast Storm Control Threshold */
+#define E1000_MSCTRH	0x05DBC /* Multicast Storm Control Threshold */
 /* These act per VF so an array friendly macro is used */
-#define E1000_P2VMAILBOX(_n)   (0x00C00 + (4 * (_n)))
-#define E1000_VMBMEM(_n)       (0x00800 + (64 * (_n)))
-#define E1000_VMOLR(_n)        (0x05AD0 + (4 * (_n)))
-#define E1000_VLVF(_n)         (0x05D00 + (4 * (_n))) /* VLAN Virtual Machine
-                                                       * Filter - RW */
-#define E1000_VMVIR(_n)        (0x03700 + (4 * (_n)))
-
-#define wr32(reg, value) (writel(value, hw->hw_addr + reg))
-#define rd32(reg) (readl(hw->hw_addr + reg))
-#define wrfl() ((void)rd32(E1000_STATUS))
-
-#define array_wr32(reg, offset, value) \
-	(writel(value, hw->hw_addr + reg + ((offset) << 2)))
-#define array_rd32(reg, offset) \
-	(readl(hw->hw_addr + reg + ((offset) << 2)))
+#define E1000_V2PMAILBOX(_n)	(0x00C40 + (4 * (_n)))
+#define E1000_P2VMAILBOX(_n)	(0x00C00 + (4 * (_n)))
+#define E1000_VMBMEM(_n)	(0x00800 + (64 * (_n)))
+#define E1000_VFVMBMEM(_n)	(0x00800 + (_n))
+#define E1000_VMOLR(_n)		(0x05AD0 + (4 * (_n)))
+/* VLAN Virtual Machine Filter - RW */
+#define E1000_VLVF(_n)		(0x05D00 + (4 * (_n)))
+#define E1000_VMVIR(_n)		(0x03700 + (4 * (_n)))
+#define E1000_DVMOLR(_n)	(0x0C038 + (0x40 * (_n))) /* DMA VM offload */
+#define E1000_VTCTRL(_n)	(0x10000 + (0x100 * (_n))) /* VT Control */
+#define E1000_TSYNCRXCTL	0x0B620 /* Rx Time Sync Control register - RW */
+#define E1000_TSYNCTXCTL	0x0B614 /* Tx Time Sync Control register - RW */
+#define E1000_TSYNCRXCFG	0x05F50 /* Time Sync Rx Configuration - RW */
+#define E1000_RXSTMPL	0x0B624 /* Rx timestamp Low - RO */
+#define E1000_RXSTMPH	0x0B628 /* Rx timestamp High - RO */
+#define E1000_RXSATRL	0x0B62C /* Rx timestamp attribute low - RO */
+#define E1000_RXSATRH	0x0B630 /* Rx timestamp attribute high - RO */
+#define E1000_TXSTMPL	0x0B618 /* Tx timestamp value Low - RO */
+#define E1000_TXSTMPH	0x0B61C /* Tx timestamp value High - RO */
+#define E1000_SYSTIML	0x0B600 /* System time register Low - RO */
+#define E1000_SYSTIMH	0x0B604 /* System time register High - RO */
+#define E1000_TIMINCA	0x0B608 /* Increment attributes register - RW */
+#define E1000_TIMADJL	0x0B60C /* Time sync time adjustment offset Low - RW */
+#define E1000_TIMADJH	0x0B610 /* Time sync time adjustment offset High - RW */
+#define E1000_TSAUXC	0x0B640 /* Timesync Auxiliary Control register */
+#define E1000_SYSTIMR	0x0B6F8 /* System time register Residue */
+#define E1000_TSICR	0x0B66C /* Interrupt Cause Register */
+#define E1000_TSIM	0x0B674 /* Interrupt Mask Register */
+
+/* Filtering Registers */
+#define E1000_SAQF(_n)	(0x05980 + (4 * (_n))) /* Source Address Queue Fltr */
+#define E1000_DAQF(_n)	(0x059A0 + (4 * (_n))) /* Dest Address Queue Fltr */
+#define E1000_SPQF(_n)	(0x059C0 + (4 * (_n))) /* Source Port Queue Fltr */
+#define E1000_FTQF(_n)	(0x059E0 + (4 * (_n))) /* 5-tuple Queue Fltr */
+#define E1000_TTQF(_n)	(0x059E0 + (4 * (_n))) /* 2-tuple Queue Fltr */
+#define E1000_SYNQF(_n)	(0x055FC + (4 * (_n))) /* SYN Packet Queue Fltr */
+#define E1000_ETQF(_n)	(0x05CB0 + (4 * (_n))) /* EType Queue Fltr */
+
+#define E1000_RTTDCS	0x3600 /* Reedtown Tx Desc plane control and status */
+#define E1000_RTTPCS	0x3474 /* Reedtown Tx Packet Plane control and status */
+#define E1000_RTRPCS	0x2474 /* Rx packet plane control and status */
+#define E1000_RTRUP2TC	0x05AC4 /* Rx User Priority to Traffic Class */
+#define E1000_RTTUP2TC	0x0418 /* Transmit User Priority to Traffic Class */
+/* Tx Desc plane TC Rate-scheduler config */
+#define E1000_RTTDTCRC(_n)	(0x3610 + ((_n) * 4))
+/* Tx Packet plane TC Rate-Scheduler Config */
+#define E1000_RTTPTCRC(_n)	(0x3480 + ((_n) * 4))
+/* Rx Packet plane TC Rate-Scheduler Config */
+#define E1000_RTRPTCRC(_n)	(0x2480 + ((_n) * 4))
+/* Tx Desc Plane TC Rate-Scheduler Status */
+#define E1000_RTTDTCRS(_n)	(0x3630 + ((_n) * 4))
+/* Tx Desc Plane TC Rate-Scheduler MMW */
+#define E1000_RTTDTCRM(_n)	(0x3650 + ((_n) * 4))
+/* Tx Packet plane TC Rate-Scheduler Status */
+#define E1000_RTTPTCRS(_n)	(0x34A0 + ((_n) * 4))
+/* Tx Packet plane TC Rate-scheduler MMW */
+#define E1000_RTTPTCRM(_n)	(0x34C0 + ((_n) * 4))
+/* Rx Packet plane TC Rate-Scheduler Status */
+#define E1000_RTRPTCRS(_n)	(0x24A0 + ((_n) * 4))
+/* Rx Packet plane TC Rate-Scheduler MMW */
+#define E1000_RTRPTCRM(_n)	(0x24C0 + ((_n) * 4))
+/* Tx Desc plane VM Rate-Scheduler MMW*/
+#define E1000_RTTDVMRM(_n)	(0x3670 + ((_n) * 4))
+/* Tx BCN Rate-Scheduler MMW */
+#define E1000_RTTBCNRM(_n)	(0x3690 + ((_n) * 4))
+#define E1000_RTTDQSEL	0x3604  /* Tx Desc Plane Queue Select */
+#define E1000_RTTDVMRC	0x3608  /* Tx Desc Plane VM Rate-Scheduler Config */
+#define E1000_RTTDVMRS	0x360C  /* Tx Desc Plane VM Rate-Scheduler Status */
+#define E1000_RTTBCNRC	0x36B0  /* Tx BCN Rate-Scheduler Config */
+#define E1000_RTTBCNRS	0x36B4  /* Tx BCN Rate-Scheduler Status */
+#define E1000_RTTBCNCR	0xB200  /* Tx BCN Control Register */
+#define E1000_RTTBCNTG	0x35A4  /* Tx BCN Tagging */
+#define E1000_RTTBCNCP	0xB208  /* Tx BCN Congestion point */
+#define E1000_RTRBCNCR	0xB20C  /* Rx BCN Control Register */
+#define E1000_RTTBCNRD	0x36B8  /* Tx BCN Rate Drift */
+#define E1000_PFCTOP	0x1080  /* Priority Flow Control Type and Opcode */
+#define E1000_RTTBCNIDX	0xB204  /* Tx BCN Congestion Point */
+#define E1000_RTTBCNACH	0x0B214 /* Tx BCN Control High */
+#define E1000_RTTBCNACL	0x0B210 /* Tx BCN Control Low */
 
 /* DMA Coalescing registers */
-#define E1000_PCIEMISC          0x05BB8 /* PCIE misc config register */
+#define E1000_DMACR	0x02508 /* Control Register */
+#define E1000_DMCTXTH	0x03550 /* Transmit Threshold */
+#define E1000_DMCTLX	0x02514 /* Time to Lx Request */
+#define E1000_DMCRTRH	0x05DD0 /* Receive Packet Rate Threshold */
+#define E1000_DMCCNT	0x05DD4 /* Current Rx Count */
+#define E1000_FCRTC	0x02170 /* Flow Control Rx high watermark */
+#define E1000_PCIEMISC	0x05BB8 /* PCIE misc config register */
 
-/* Energy Efficient Ethernet "EEE" register */
-#define E1000_IPCNFG  0x0E38  /* Internal PHY Configuration */
-#define E1000_EEER    0x0E30  /* Energy Efficient Ethernet */
+/* PCIe Parity Status Register */
+#define E1000_PCIEERRSTS	0x05BA8
 
-/* Thermal Sensor Register */
-#define E1000_THSTAT    0x08110 /* Thermal Sensor Status */
+#define E1000_PROXYS	0x5F64 /* Proxying Status */
+#define E1000_PROXYFC	0x5F60 /* Proxying Filter Control */
+/* Thermal sensor configuration and status registers */
+#define E1000_THMJT	0x08100 /* Junction Temperature */
+#define E1000_THLOWTC	0x08104 /* Low Threshold Control */
+#define E1000_THMIDTC	0x08108 /* Mid Threshold Control */
+#define E1000_THHIGHTC	0x0810C /* High Threshold Control */
+#define E1000_THSTAT	0x08110 /* Thermal Sensor Status */
+
+/* Energy Efficient Ethernet "EEE" registers */
+#define E1000_IPCNFG	0x0E38 /* Internal PHY Configuration */
+#define E1000_LTRC	0x01A0 /* Latency Tolerance Reporting Control */
+#define E1000_EEER	0x0E30 /* Energy Efficient Ethernet "EEE"*/
+#define E1000_EEE_SU	0x0E34 /* EEE Setup */
+#define E1000_TLPIC	0x4148 /* EEE Tx LPI Count - TLPIC */
+#define E1000_RLPIC	0x414C /* EEE Rx LPI Count - RLPIC */
 
 /* OS2BMC Registers */
-#define E1000_B2OSPC    0x08FE0 /* BMC2OS packets sent by BMC */
-#define E1000_B2OGPRC   0x04158 /* BMC2OS packets received by host */
-#define E1000_O2BGPTC   0x08FE4 /* OS2BMC packets received by BMC */
-#define E1000_O2BSPC    0x0415C /* OS2BMC packets transmitted by host */
+#define E1000_B2OSPC	0x08FE0 /* BMC2OS packets sent by BMC */
+#define E1000_B2OGPRC	0x04158 /* BMC2OS packets received by host */
+#define E1000_O2BGPTC	0x08FE4 /* OS2BMC packets received by BMC */
+#define E1000_O2BSPC	0x0415C /* OS2BMC packets transmitted by host */
+
+
 
 #endif
diff --git a/drivers/net/igb/igb.h b/drivers/net/igb/igb.h
index 265e151b66c4..ca7c53f11844 100644
--- a/drivers/net/igb/igb.h
+++ b/drivers/net/igb/igb.h
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2007-2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -25,28 +25,78 @@
 
 *******************************************************************************/
 
-
 /* Linux PRO/1000 Ethernet Driver main header file */
 
 #ifndef _IGB_H_
 #define _IGB_H_
 
-#include "e1000_mac.h"
+#include <linux/kobject.h>
+
+#ifndef IGB_NO_LRO
+#include <net/tcp.h>
+#endif
+
+#include <linux/pci.h>
+#include <linux/netdevice.h>
+#include <linux/vmalloc.h>
+
+#ifdef SIOCETHTOOL
+#include <linux/ethtool.h>
+#endif
+
+struct igb_adapter;
+
+#if defined(CONFIG_DCA) || defined(CONFIG_DCA_MODULE)
+#define IGB_DCA
+#endif
+#ifdef IGB_DCA
+#include <linux/dca.h>
+#endif
+
+#include "kcompat.h"
+
+#ifdef HAVE_SCTP
+#include <linux/sctp.h>
+#endif
+
+#include "e1000_api.h"
 #include "e1000_82575.h"
+#include "e1000_manage.h"
+#include "e1000_mbx.h"
 
+#define IGB_ERR(args...) printk(KERN_ERR "igb: " args)
+
+#define PFX "igb: "
+#define DPRINTK(nlevel, klevel, fmt, args...) \
+	(void)((NETIF_MSG_##nlevel & adapter->msg_enable) && \
+	printk(KERN_##klevel PFX "%s: %s: " fmt, adapter->netdev->name, \
+		__FUNCTION__ , ## args))
+
+#ifdef HAVE_PTP_1588_CLOCK
 #include <linux/clocksource.h>
-#include <linux/timecompare.h>
 #include <linux/net_tstamp.h>
-#include <linux/bitops.h>
-#include <linux/if_vlan.h>
+#include <linux/ptp_clock_kernel.h>
+#endif /* HAVE_PTP_1588_CLOCK */
 
-struct igb_adapter;
+#ifdef HAVE_I2C_SUPPORT
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+#endif /* HAVE_I2C_SUPPORT */
 
-/* ((1000000000ns / (6000ints/s * 1024ns)) << 2 = 648 */
-#define IGB_START_ITR 648
+/* Interrupt defines */
+#define IGB_START_ITR                    648 /* ~6000 ints/sec */
+#define IGB_4K_ITR                       980
+#define IGB_20K_ITR                      196
+#define IGB_70K_ITR                       56
+
+/* Interrupt modes, as used by the IntMode paramter */
+#define IGB_INT_MODE_LEGACY                0
+#define IGB_INT_MODE_MSI                   1
+#define IGB_INT_MODE_MSIX                  2
 
 /* TX/RX descriptor defines */
 #define IGB_DEFAULT_TXD                  256
+#define IGB_DEFAULT_TX_WORK		 128
 #define IGB_MIN_TXD                       80
 #define IGB_MAX_TXD                     4096
 
@@ -54,32 +104,46 @@ struct igb_adapter;
 #define IGB_MIN_RXD                       80
 #define IGB_MAX_RXD                     4096
 
-#define IGB_DEFAULT_ITR                    3 /* dynamic */
-#define IGB_MAX_ITR_USECS              10000
-#define IGB_MIN_ITR_USECS                 10
+#define IGB_MIN_ITR_USECS                 10 /* 100k irq/sec */
+#define IGB_MAX_ITR_USECS               8191 /* 120  irq/sec */
+
 #define NON_Q_VECTORS                      1
-#define MAX_Q_VECTORS                      8
+#define MAX_Q_VECTORS                     10
 
 /* Transmit and receive queues */
-#define IGB_MAX_RX_QUEUES                  (adapter->vfs_allocated_count ? 2 : \
-                                           (hw->mac.type > e1000_82575 ? 8 : 4))
-#define IGB_ABS_MAX_TX_QUEUES              8
-#define IGB_MAX_TX_QUEUES                  IGB_MAX_RX_QUEUES
+#define IGB_MAX_RX_QUEUES                 16
+#define IGB_MAX_RX_QUEUES_82575            4
+#define IGB_MAX_RX_QUEUES_I211             2
+#define IGB_MAX_TX_QUEUES                 16
 
-#define IGB_MAX_VF_MC_ENTRIES              30
+#define IGB_MAX_VF_MC_ENTRIES             30
 #define IGB_MAX_VF_FUNCTIONS               8
-#define IGB_MAX_VFTA_ENTRIES               128
+#define IGB_82576_VF_DEV_ID           0x10CA
+#define IGB_I350_VF_DEV_ID            0x1520
+#define IGB_MAX_UTA_ENTRIES              128
+#define MAX_EMULATION_MAC_ADDRS           16
+#define OUI_LEN                            3
+#define IGB_MAX_VMDQ_QUEUES                8
+
 
 struct vf_data_storage {
 	unsigned char vf_mac_addresses[ETH_ALEN];
 	u16 vf_mc_hashes[IGB_MAX_VF_MC_ENTRIES];
 	u16 num_vf_mc_hashes;
+	u16 default_vf_vlan_id;
 	u16 vlans_enabled;
+	unsigned char em_mac_addresses[MAX_EMULATION_MAC_ADDRS * ETH_ALEN];
+	u32 uta_table_copy[IGB_MAX_UTA_ENTRIES];
 	u32 flags;
 	unsigned long last_nack;
+#ifdef IFLA_VF_MAX
 	u16 pf_vlan; /* When set, guest VLAN config not allowed. */
 	u16 pf_qos;
 	u16 tx_rate;
+#ifdef HAVE_VF_SPOOFCHK_CONFIGURE
+	bool spoofchk_enabled;
+#endif
+#endif
 };
 
 #define IGB_VF_FLAG_CTS            0x00000001 /* VF is clear to send data */
@@ -98,33 +162,46 @@ struct vf_data_storage {
  *           descriptors until either it has this many to write back, or the
  *           ITR timer expires.
  */
-#define IGB_RX_PTHRESH                     8
-#define IGB_RX_HTHRESH                     8
-#define IGB_RX_WTHRESH                     1
-#define IGB_TX_PTHRESH                     8
-#define IGB_TX_HTHRESH                     1
-#define IGB_TX_WTHRESH                     ((hw->mac.type == e1000_82576 && \
-                                             adapter->msix_entries) ? 1 : 16)
+#define IGB_RX_PTHRESH	((hw->mac.type == e1000_i354) ? 12 : 8)
+#define IGB_RX_HTHRESH	8
+#define IGB_TX_PTHRESH	((hw->mac.type == e1000_i354) ? 20 : 8)
+#define IGB_TX_HTHRESH	1
+#define IGB_RX_WTHRESH	((hw->mac.type == e1000_82576 && \
+			  adapter->msix_entries) ? 1 : 4)
 
 /* this is the size past which hardware will drop packets when setting LPE=0 */
 #define MAXIMUM_ETHERNET_VLAN_SIZE 1522
 
+/* NOTE: netdev_alloc_skb reserves 16 bytes, NET_IP_ALIGN means we
+ * reserve 2 more, and skb_shared_info adds an additional 384 more,
+ * this adds roughly 448 bytes of extra data meaning the smallest
+ * allocation we could have is 1K.
+ * i.e. RXBUFFER_512 --> size-1024 slab
+ */
 /* Supported Rx Buffer Sizes */
-#define IGB_RXBUFFER_64    64     /* Used for packet split */
-#define IGB_RXBUFFER_128   128    /* Used for packet split */
-#define IGB_RXBUFFER_1024  1024
+#define IGB_RXBUFFER_256   256
 #define IGB_RXBUFFER_2048  2048
 #define IGB_RXBUFFER_16384 16384
+#define IGB_RX_HDR_LEN	   IGB_RXBUFFER_256
+#if MAX_SKB_FRAGS < 8
+#define IGB_RX_BUFSZ	   ALIGN(MAX_JUMBO_FRAME_SIZE / MAX_SKB_FRAGS, 1024)
+#else
+#define IGB_RX_BUFSZ	   IGB_RXBUFFER_2048
+#endif
+
 
-#define MAX_STD_JUMBO_FRAME_SIZE 9234
+/* Packet Buffer allocations */
+#define IGB_PBA_BYTES_SHIFT 0xA
+#define IGB_TX_HEAD_ADDR_SHIFT 7
+#define IGB_PBA_TX_MASK 0xFFFF0000
+
+#define IGB_FC_PAUSE_TIME 0x0680 /* 858 usec */
 
-/* How many Tx Descriptors do we need to call netif_wake_queue ? */
-#define IGB_TX_QUEUE_WAKE	16
 /* How many Rx Buffers do we bundle into one write to the hardware ? */
 #define IGB_RX_BUFFER_WRITE	16	/* Must be power of 2 */
 
-#define AUTO_ALL_MODES            0
 #define IGB_EEPROM_APME         0x0400
+#define AUTO_ALL_MODES          0
 
 #ifndef IGB_MASTER_SLAVE
 /* Switch to override PHY master/slave setting */
@@ -133,36 +210,115 @@ struct vf_data_storage {
 
 #define IGB_MNG_VLAN_NONE -1
 
+#ifndef IGB_NO_LRO
+#define IGB_LRO_MAX 32 /*Maximum number of LRO descriptors*/
+struct igb_lro_stats {
+	u32 flushed;
+	u32 coal;
+};
+
+/*
+ * igb_lro_header - header format to be aggregated by LRO
+ * @iph: IP header without options
+ * @tcp: TCP header
+ * @ts:  Optional TCP timestamp data in TCP options
+ *
+ * This structure relies on the check above that verifies that the header
+ * is IPv4 and does not contain any options.
+ */
+struct igb_lrohdr {
+	struct iphdr iph;
+	struct tcphdr th; 
+	__be32 ts[0];
+};
+
+struct igb_lro_list {
+	struct sk_buff_head active;
+	struct igb_lro_stats stats;
+};
+
+#endif /* IGB_NO_LRO */
+struct igb_cb {
+#ifndef IGB_NO_LRO
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+	union {				/* Union defining head/tail partner */
+		struct sk_buff *head;
+		struct sk_buff *tail;
+	};
+#endif
+	__be32	tsecr;			/* timestamp echo response */
+	u32	tsval;			/* timestamp value in host order */
+	u32	next_seq;		/* next expected sequence number */
+	u16	free;			/* 65521 minus total size */
+	u16	mss;			/* size of data portion of packet */
+	u16	append_cnt;		/* number of skb's appended */
+#endif /* IGB_NO_LRO */
+#ifdef HAVE_VLAN_RX_REGISTER
+	u16	vid;			/* VLAN tag */
+#endif
+};
+#define IGB_CB(skb) ((struct igb_cb *)(skb)->cb)
+
+enum igb_tx_flags {
+	/* cmd_type flags */
+	IGB_TX_FLAGS_VLAN	= 0x01,
+	IGB_TX_FLAGS_TSO	= 0x02,
+	IGB_TX_FLAGS_TSTAMP	= 0x04,
+
+	/* olinfo flags */
+	IGB_TX_FLAGS_IPV4	= 0x10,
+	IGB_TX_FLAGS_CSUM	= 0x20,
+};
+
+/* VLAN info */
+#define IGB_TX_FLAGS_VLAN_MASK		0xffff0000
+#define IGB_TX_FLAGS_VLAN_SHIFT		        16
+
+/*
+ * The largest size we can write to the descriptor is 65535.  In order to
+ * maintain a power of two alignment we have to limit ourselves to 32K.
+ */
+#define IGB_MAX_TXD_PWR		15
+#define IGB_MAX_DATA_PER_TXD	(1 << IGB_MAX_TXD_PWR)
+
+/* Tx Descriptors needed, worst case */
+#define TXD_USE_COUNT(S)	DIV_ROUND_UP((S), IGB_MAX_DATA_PER_TXD)
+#ifndef MAX_SKB_FRAGS
+#define DESC_NEEDED	4
+#elif (MAX_SKB_FRAGS < 16)
+#define DESC_NEEDED	((MAX_SKB_FRAGS * TXD_USE_COUNT(PAGE_SIZE)) + 4)
+#else
+#define DESC_NEEDED	(MAX_SKB_FRAGS + 4)
+#endif
+
 /* wrapper around a pointer to a socket buffer,
  * so a DMA handle can be stored along with the buffer */
-struct igb_buffer {
+struct igb_tx_buffer {
+	union e1000_adv_tx_desc *next_to_watch;
+	unsigned long time_stamp;
 	struct sk_buff *skb;
+	unsigned int bytecount;
+	u16 gso_segs;
+	__be16 protocol;
+	DEFINE_DMA_UNMAP_ADDR(dma);
+	DEFINE_DMA_UNMAP_LEN(len);
+	u32 tx_flags;
+};
+
+struct igb_rx_buffer {
 	dma_addr_t dma;
-	union {
-		/* TX */
-		struct {
-			unsigned long time_stamp;
-			u16 length;
-			u16 next_to_watch;
-			unsigned int bytecount;
-			u16 gso_segs;
-			u8 tx_flags;
-			u8 mapped_as_page;
-		};
-		/* RX */
-		struct {
-			struct page *page;
-			dma_addr_t page_dma;
-			u16 page_offset;
-		};
-	};
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+	struct sk_buff *skb;
+#else
+	struct page *page;
+	u32 page_offset;
+#endif
 };
 
 struct igb_tx_queue_stats {
 	u64 packets;
 	u64 bytes;
 	u64 restart_queue;
-	u64 restart_queue2;
 };
 
 struct igb_rx_queue_stats {
@@ -173,130 +329,257 @@ struct igb_rx_queue_stats {
 	u64 alloc_failed;
 };
 
-struct igb_q_vector {
-	struct igb_adapter *adapter; /* backlink */
-	struct igb_ring *rx_ring;
-	struct igb_ring *tx_ring;
-	struct napi_struct napi;
-
-	u32 eims_value;
-	u16 cpu;
-
-	u16 itr_val;
-	u8 set_itr;
-	void __iomem *itr_register;
+struct igb_rx_packet_stats {
+	u64 ipv4_packets;      /* IPv4 headers processed */
+	u64 ipv4e_packets;     /* IPv4E headers with extensions processed */
+	u64 ipv6_packets;      /* IPv6 headers processed */
+	u64 ipv6e_packets;     /* IPv6E headers with extensions processed */
+	u64 tcp_packets;       /* TCP headers processed */
+	u64 udp_packets;       /* UDP headers processed */
+	u64 sctp_packets;      /* SCTP headers processed */
+	u64 nfs_packets;       /* NFS headers processe */
+	u64 other_packets;
+};
 
-	char name[IFNAMSIZ + 9];
+struct igb_ring_container {
+	struct igb_ring *ring;		/* pointer to linked list of rings */
+	unsigned int total_bytes;	/* total bytes processed this int */
+	unsigned int total_packets;	/* total packets processed this int */
+	u16 work_limit;			/* total work allowed per interrupt */
+	u8 count;			/* total number of rings in vector */
+	u8 itr;				/* current ITR setting for ring */
 };
 
 struct igb_ring {
-	struct igb_q_vector *q_vector; /* backlink to q_vector */
-	struct net_device *netdev;     /* back pointer to net_device */
-	struct device *dev;            /* device pointer for dma mapping */
-	dma_addr_t dma;                /* phys address of the ring */
-	void *desc;                    /* descriptor ring memory */
-	unsigned int size;             /* length of desc. ring in bytes */
-	u16 count;                     /* number of desc. in the ring */
-	u16 next_to_use;
+	struct igb_q_vector *q_vector;  /* backlink to q_vector */
+	struct net_device *netdev;      /* back pointer to net_device */
+	struct device *dev;             /* device for dma mapping */
+	union {				/* array of buffer info structs */
+		struct igb_tx_buffer *tx_buffer_info;
+		struct igb_rx_buffer *rx_buffer_info;
+	};
+#ifdef HAVE_PTP_1588_CLOCK
+	unsigned long last_rx_timestamp;
+#endif /* HAVE_PTP_1588_CLOCK */
+	void *desc;                     /* descriptor ring memory */
+	unsigned long flags;            /* ring specific flags */
+	void __iomem *tail;             /* pointer to ring tail register */
+	dma_addr_t dma;			/* phys address of the ring */
+	unsigned int size;		/* length of desc. ring in bytes */
+
+	u16 count;                      /* number of desc. in the ring */
+	u8 queue_index;                 /* logical index of the ring*/
+	u8 reg_idx;                     /* physical index of the ring */
+
+	/* everything past this point are written often */
 	u16 next_to_clean;
-	u8 queue_index;
-	u8 reg_idx;
-	void __iomem *head;
-	void __iomem *tail;
-	struct igb_buffer *buffer_info; /* array of buffer info structs */
-
-	unsigned int total_bytes;
-	unsigned int total_packets;
-
-	u32 flags;
+	u16 next_to_use;
+	u16 next_to_alloc;
 
 	union {
 		/* TX */
 		struct {
 			struct igb_tx_queue_stats tx_stats;
-			struct u64_stats_sync tx_syncp;
-			struct u64_stats_sync tx_syncp2;
-			bool detect_tx_hung;
 		};
 		/* RX */
 		struct {
 			struct igb_rx_queue_stats rx_stats;
-			struct u64_stats_sync rx_syncp;
-			u32 rx_buffer_len;
+			struct igb_rx_packet_stats pkt_stats;
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+			u16 rx_buffer_len;
+#else
+			struct sk_buff *skb;
+#endif
 		};
 	};
-};
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+	struct net_device *vmdq_netdev;
+	int vqueue_index;		/* queue index for virtual netdev */
+#endif
+} ____cacheline_internodealigned_in_smp;
+
+struct igb_q_vector {
+	struct igb_adapter *adapter;	/* backlink */
+	int cpu;			/* CPU for DCA */
+	u32 eims_value;			/* EIMS mask value */
+
+	u16 itr_val;
+	u8 set_itr;
+	void __iomem *itr_register;
 
-#define IGB_RING_FLAG_RX_CSUM        0x00000001 /* RX CSUM enabled */
-#define IGB_RING_FLAG_RX_SCTP_CSUM   0x00000002 /* SCTP CSUM offload enabled */
+	struct igb_ring_container rx, tx;
 
-#define IGB_RING_FLAG_TX_CTX_IDX     0x00000001 /* HW requires context index */
+	struct napi_struct napi;
+#ifndef IGB_NO_LRO
+	struct igb_lro_list lrolist;   /* LRO list for queue vector*/
+#endif
+	struct rcu_head rcu;	/* to avoid race with update stats on free */
+	char name[IFNAMSIZ + 9];
+#ifndef HAVE_NETDEV_NAPI_LIST
+	struct net_device poll_dev;
+#endif
+
+	/* for dynamic allocation of rings associated with this q_vector */
+	struct igb_ring ring[0] ____cacheline_internodealigned_in_smp;
+};
 
-#define IGB_ADVTXD_DCMD (E1000_TXD_CMD_EOP | E1000_TXD_CMD_RS)
+enum e1000_ring_flags_t {
+#ifndef HAVE_NDO_SET_FEATURES
+	IGB_RING_FLAG_RX_CSUM,
+#endif
+	IGB_RING_FLAG_RX_SCTP_CSUM,
+	IGB_RING_FLAG_RX_LB_VLAN_BSWAP,
+	IGB_RING_FLAG_TX_CTX_IDX,
+	IGB_RING_FLAG_TX_DETECT_HANG,
+};
 
-#define E1000_RX_DESC_ADV(R, i)	    \
-	(&(((union e1000_adv_rx_desc *)((R).desc))[i]))
-#define E1000_TX_DESC_ADV(R, i)	    \
-	(&(((union e1000_adv_tx_desc *)((R).desc))[i]))
-#define E1000_TX_CTXTDESC_ADV(R, i)	    \
-	(&(((struct e1000_adv_tx_context_desc *)((R).desc))[i]))
+struct igb_mac_addr {
+	u8 addr[ETH_ALEN];
+	u16 queue;
+	u16 state; /* bitmask */
+};
+#define IGB_MAC_STATE_DEFAULT	0x1
+#define IGB_MAC_STATE_MODIFIED	0x2
+#define IGB_MAC_STATE_IN_USE	0x4
+
+#define IGB_TXD_DCMD (E1000_ADVTXD_DCMD_EOP | E1000_ADVTXD_DCMD_RS)
+
+#define IGB_RX_DESC(R, i)	    \
+	(&(((union e1000_adv_rx_desc *)((R)->desc))[i]))
+#define IGB_TX_DESC(R, i)	    \
+	(&(((union e1000_adv_tx_desc *)((R)->desc))[i]))
+#define IGB_TX_CTXTDESC(R, i)	    \
+	(&(((struct e1000_adv_tx_context_desc *)((R)->desc))[i]))
+
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+#define netdev_ring(ring) \
+	((ring->vmdq_netdev ? ring->vmdq_netdev : ring->netdev))
+#define ring_queue_index(ring) \
+	((ring->vmdq_netdev ? ring->vqueue_index : ring->queue_index))
+#else
+#define netdev_ring(ring) (ring->netdev)
+#define ring_queue_index(ring) (ring->queue_index)
+#endif /* CONFIG_IGB_VMDQ_NETDEV */
+
+/* igb_test_staterr - tests bits within Rx descriptor status and error fields */
+static inline __le32 igb_test_staterr(union e1000_adv_rx_desc *rx_desc,
+				      const u32 stat_err_bits)
+{
+	return rx_desc->wb.upper.status_error & cpu_to_le32(stat_err_bits);
+}
 
 /* igb_desc_unused - calculate if we have unused descriptors */
-static inline int igb_desc_unused(struct igb_ring *ring)
+static inline u16 igb_desc_unused(const struct igb_ring *ring)
 {
-	if (ring->next_to_clean > ring->next_to_use)
-		return ring->next_to_clean - ring->next_to_use - 1;
+	u16 ntc = ring->next_to_clean;
+	u16 ntu = ring->next_to_use;
+
+	return ((ntc > ntu) ? 0 : ring->count) + ntc - ntu - 1;
+}
 
-	return ring->count + ring->next_to_clean - ring->next_to_use - 1;
+#ifdef CONFIG_BQL
+static inline struct netdev_queue *txring_txq(const struct igb_ring *tx_ring)
+{
+	return netdev_get_tx_queue(tx_ring->netdev, tx_ring->queue_index);
 }
+#endif /* CONFIG_BQL */
+
+// #ifdef EXT_THERMAL_SENSOR_SUPPORT
+// #ifdef IGB_PROCFS
+struct igb_therm_proc_data
+{
+	struct e1000_hw *hw;
+	struct e1000_thermal_diode_data *sensor_data;
+};
+
+//  #endif /* IGB_PROCFS */
+// #endif /* EXT_THERMAL_SENSOR_SUPPORT */
+
+#ifdef IGB_HWMON
+#define IGB_HWMON_TYPE_LOC	0
+#define IGB_HWMON_TYPE_TEMP	1
+#define IGB_HWMON_TYPE_CAUTION	2
+#define IGB_HWMON_TYPE_MAX	3
+
+struct hwmon_attr {
+	struct device_attribute dev_attr;
+	struct e1000_hw *hw;
+	struct e1000_thermal_diode_data *sensor;
+	char name[12];
+	};
+
+struct hwmon_buff {
+	struct device *device;
+	struct hwmon_attr *hwmon_list;
+	unsigned int n_hwmon;
+	};
+#endif /* IGB_HWMON */
+#ifdef ETHTOOL_GRXFHINDIR
+#define IGB_RETA_SIZE	128
+#endif /* ETHTOOL_GRXFHINDIR */
 
 /* board specific private data structure */
 struct igb_adapter {
+#ifdef HAVE_VLAN_RX_REGISTER
+	/* vlgrp must be first member of structure */
+	struct vlan_group *vlgrp;
+#else
+	unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)];
+#endif
+	struct net_device *netdev;
+
+	unsigned long state;
+	unsigned int flags;
+
+	unsigned int num_q_vectors;
+	struct msix_entry *msix_entries;
+
+
+	/* TX */
+	u16 tx_work_limit;
+	u32 tx_timeout_count;
+	int num_tx_queues;
+	struct igb_ring *tx_ring[IGB_MAX_TX_QUEUES];
+
+	/* RX */
+	int num_rx_queues;
+	struct igb_ring *rx_ring[IGB_MAX_RX_QUEUES];
+
 	struct timer_list watchdog_timer;
+	struct timer_list dma_err_timer;
 	struct timer_list phy_info_timer;
-	unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)];
 	u16 mng_vlan_id;
 	u32 bd_number;
 	u32 wol;
 	u32 en_mng_pt;
 	u16 link_speed;
 	u16 link_duplex;
+	u8 port_num;
 
 	/* Interrupt Throttle Rate */
 	u32 rx_itr_setting;
 	u32 tx_itr_setting;
-	u16 tx_itr;
-	u16 rx_itr;
 
 	struct work_struct reset_task;
 	struct work_struct watchdog_task;
+	struct work_struct dma_err_task;
 	bool fc_autoneg;
 	u8  tx_timeout_factor;
-	struct timer_list blink_timer;
-	unsigned long led_status;
-
-	/* TX */
-	struct igb_ring *tx_ring[16];
-	u32 tx_timeout_count;
-
-	/* RX */
-	struct igb_ring *rx_ring[16];
-	int num_tx_queues;
-	int num_rx_queues;
 
+#ifdef DEBUG
+	bool tx_hang_detected;
+	bool disable_hw_reset;
+#endif
 	u32 max_frame_size;
-	u32 min_frame_size;
 
 	/* OS defined structs */
-	struct net_device *netdev;
 	struct pci_dev *pdev;
-	struct cyclecounter cycles;
-	struct timecounter clock;
-	struct timecompare compare;
-	struct hwtstamp_config hwtstamp_config;
-
-	spinlock_t stats64_lock;
-	struct rtnl_link_stats64 stats64;
+#ifndef HAVE_NETDEV_STATS_IN_NETDEV
+	struct net_device_stats net_stats;
+#endif
+#ifndef IGB_NO_LRO
+	struct igb_lro_stats lro_stats;
+#endif
 
 	/* structs defined in e1000_hw.h */
 	struct e1000_hw hw;
@@ -304,57 +587,196 @@ struct igb_adapter {
 	struct e1000_phy_info phy_info;
 	struct e1000_phy_stats phy_stats;
 
+#ifdef ETHTOOL_TEST
 	u32 test_icr;
 	struct igb_ring test_tx_ring;
 	struct igb_ring test_rx_ring;
+#endif
 
 	int msg_enable;
 
-	unsigned int num_q_vectors;
 	struct igb_q_vector *q_vector[MAX_Q_VECTORS];
-	struct msix_entry *msix_entries;
 	u32 eims_enable_mask;
 	u32 eims_other;
 
 	/* to not mess up cache alignment, always add to the bottom */
-	unsigned long state;
-	unsigned int flags;
-	u32 eeprom_wol;
-
-	struct igb_ring *multi_tx_table[IGB_ABS_MAX_TX_QUEUES];
+	u32 *config_space;
 	u16 tx_ring_count;
 	u16 rx_ring_count;
-	unsigned int vfs_allocated_count;
 	struct vf_data_storage *vf_data;
+#ifdef IFLA_VF_MAX
 	int vf_rate_link_speed;
+#endif
+	u32 lli_port;
+	u32 lli_size;
+	unsigned int vfs_allocated_count;
+	/* Malicious Driver Detection flag. Valid only when SR-IOV is enabled */
+	bool mdd;
+	int int_mode;
 	u32 rss_queues;
+	u32 tss_queues;
+	u32 vmdq_pools;
+	char fw_version[32];
 	u32 wvbr;
+	struct igb_mac_addr *mac_table;
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+	struct net_device *vmdq_netdev[IGB_MAX_VMDQ_QUEUES];
+#endif
+	int vferr_refcount;
+	int dmac;
+	u32 *shadow_vfta;
+
+	/* External Thermal Sensor support flag */
+	bool ets;
+#ifdef IGB_HWMON
+	struct hwmon_buff igb_hwmon_buff;
+#else /* IGB_HWMON */
+#ifdef IGB_PROCFS
+	struct proc_dir_entry *eth_dir;
+	struct proc_dir_entry *info_dir;
+	struct proc_dir_entry *therm_dir[E1000_MAX_SENSORS];
+	struct igb_therm_proc_data therm_data[E1000_MAX_SENSORS];
+	bool old_lsc;
+#endif /* IGB_PROCFS */
+#endif /* IGB_HWMON */
+	u32 etrack_id;
+
+#ifdef HAVE_PTP_1588_CLOCK
+	struct ptp_clock *ptp_clock;
+	struct ptp_clock_info ptp_caps;
+	struct delayed_work ptp_overflow_work;
+	struct work_struct ptp_tx_work;
+	struct sk_buff *ptp_tx_skb;
+	unsigned long ptp_tx_start;
+	unsigned long last_rx_ptp_check;
+	spinlock_t tmreg_lock;
+	struct cyclecounter cc;
+	struct timecounter tc;
+	u32 tx_hwtstamp_timeouts;
+	u32 rx_hwtstamp_cleared;
+#endif /* HAVE_PTP_1588_CLOCK */
+
+#ifdef HAVE_I2C_SUPPORT
+	struct i2c_algo_bit_data i2c_algo;
+	struct i2c_adapter i2c_adap;
+	struct i2c_client *i2c_client;
+#endif /* HAVE_I2C_SUPPORT */
+	unsigned long link_check_timeout;
+
+
+	int devrc;
+
+	int copper_tries;
+	u16 eee_advert;
+#ifdef ETHTOOL_GRXFHINDIR
+	u32 rss_indir_tbl_init;
+	u8 rss_indir_tbl[IGB_RETA_SIZE];
+#endif
 };
 
-#define IGB_FLAG_HAS_MSI           (1 << 0)
-#define IGB_FLAG_DCA_ENABLED       (1 << 1)
-#define IGB_FLAG_QUAD_PORT_A       (1 << 2)
-#define IGB_FLAG_QUEUE_PAIRS       (1 << 3)
-#define IGB_FLAG_DMAC              (1 << 4)
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+struct igb_vmdq_adapter {
+#ifdef HAVE_VLAN_RX_REGISTER
+	/* vlgrp must be first member of structure */
+	struct vlan_group *vlgrp;
+#else
+	unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)];
+#endif
+	struct igb_adapter *real_adapter;
+	struct net_device *vnetdev;
+	struct net_device_stats net_stats;
+	struct igb_ring *tx_ring;
+	struct igb_ring *rx_ring;
+};
+#endif
+
+#define IGB_FLAG_HAS_MSI		(1 << 0)
+#define IGB_FLAG_DCA_ENABLED		(1 << 1)
+#define IGB_FLAG_LLI_PUSH		(1 << 2)
+#define IGB_FLAG_QUAD_PORT_A		(1 << 3)
+#define IGB_FLAG_QUEUE_PAIRS		(1 << 4)
+#define IGB_FLAG_EEE			(1 << 5)
+#define IGB_FLAG_DMAC			(1 << 6)
+#define IGB_FLAG_DETECT_BAD_DMA		(1 << 7)
+#define IGB_FLAG_PTP			(1 << 8)
+#define IGB_FLAG_RSS_FIELD_IPV4_UDP	(1 << 9)
+#define IGB_FLAG_RSS_FIELD_IPV6_UDP	(1 << 10)
+#define IGB_FLAG_WOL_SUPPORTED		(1 << 11)
+#define IGB_FLAG_NEED_LINK_UPDATE	(1 << 12)
+#define IGB_FLAG_LOOPBACK_ENABLE	(1 << 13)
+#define IGB_FLAG_MEDIA_RESET		(1 << 14)
+#define IGB_FLAG_MAS_ENABLE		(1 << 15)
+
+/* Media Auto Sense */
+#define IGB_MAS_ENABLE_0		0X0001
+#define IGB_MAS_ENABLE_1		0X0002
+#define IGB_MAS_ENABLE_2		0X0004
+#define IGB_MAS_ENABLE_3		0X0008
 
-/* DMA Coalescing defines */
 #define IGB_MIN_TXPBSIZE           20408
 #define IGB_TX_BUF_4096            4096
+
 #define IGB_DMCTLX_DCFLUSH_DIS     0x80000000  /* Disable DMA Coal Flush */
 
+/* DMA Coalescing defines */
+#define IGB_DMAC_DISABLE          0
+#define IGB_DMAC_MIN            250
+#define IGB_DMAC_500            500
+#define IGB_DMAC_EN_DEFAULT    1000
+#define IGB_DMAC_2000          2000
+#define IGB_DMAC_3000          3000
+#define IGB_DMAC_4000          4000
+#define IGB_DMAC_5000          5000
+#define IGB_DMAC_6000          6000
+#define IGB_DMAC_7000          7000
+#define IGB_DMAC_8000          8000
+#define IGB_DMAC_9000          9000
+#define IGB_DMAC_MAX          10000
+
 #define IGB_82576_TSYNC_SHIFT 19
 #define IGB_82580_TSYNC_SHIFT 24
 #define IGB_TS_HDR_LEN        16
+
+/* CEM Support */
+#define FW_HDR_LEN           0x4
+#define FW_CMD_DRV_INFO      0xDD
+#define FW_CMD_DRV_INFO_LEN  0x5
+#define FW_CMD_RESERVED      0X0
+#define FW_RESP_SUCCESS      0x1
+#define FW_UNUSED_VER        0x0
+#define FW_MAX_RETRIES       3
+#define FW_STATUS_SUCCESS    0x1
+#define FW_FAMILY_DRV_VER    0Xffffffff
+
+#define IGB_MAX_LINK_TRIES   20
+
+struct e1000_fw_hdr {
+	u8 cmd;
+	u8 buf_len;
+	union
+	{
+		u8 cmd_resv;
+		u8 ret_status;
+	} cmd_or_resp;
+	u8 checksum;
+};
+
+#pragma pack(push,1)
+struct e1000_fw_drv_info {
+	struct e1000_fw_hdr hdr;
+	u8 port_num;
+	u32 drv_version;
+	u16 pad; /* end spacing to ensure length is mult. of dword */
+	u8  pad2; /* end spacing to ensure length is mult. of dword2 */
+};
+#pragma pack(pop)
+
 enum e1000_state_t {
 	__IGB_TESTING,
 	__IGB_RESETTING,
 	__IGB_DOWN
 };
 
-enum igb_boards {
-	board_82575,
-};
-
 extern char igb_driver_name[];
 extern char igb_driver_version[];
 
@@ -362,7 +784,10 @@ extern int igb_up(struct igb_adapter *);
 extern void igb_down(struct igb_adapter *);
 extern void igb_reinit_locked(struct igb_adapter *);
 extern void igb_reset(struct igb_adapter *);
-extern int igb_set_spd_dplx(struct igb_adapter *, u32, u8);
+#ifdef ETHTOOL_SRXFHINDIR
+extern void igb_write_rss_indir_tbl(struct igb_adapter *);
+#endif
+extern int igb_set_spd_dplx(struct igb_adapter *, u16);
 extern int igb_setup_tx_resources(struct igb_ring *);
 extern int igb_setup_rx_resources(struct igb_ring *);
 extern void igb_free_tx_resources(struct igb_ring *);
@@ -371,45 +796,81 @@ extern void igb_configure_tx_ring(struct igb_adapter *, struct igb_ring *);
 extern void igb_configure_rx_ring(struct igb_adapter *, struct igb_ring *);
 extern void igb_setup_tctl(struct igb_adapter *);
 extern void igb_setup_rctl(struct igb_adapter *);
-extern netdev_tx_t igb_xmit_frame_ring_adv(struct sk_buff *, struct igb_ring *);
+extern netdev_tx_t igb_xmit_frame_ring(struct sk_buff *, struct igb_ring *);
 extern void igb_unmap_and_free_tx_resource(struct igb_ring *,
-					   struct igb_buffer *);
-extern void igb_alloc_rx_buffers_adv(struct igb_ring *, int);
-extern void igb_update_stats(struct igb_adapter *, struct rtnl_link_stats64 *);
+                                           struct igb_tx_buffer *);
+extern void igb_alloc_rx_buffers(struct igb_ring *, u16);
+extern void igb_clean_rx_ring(struct igb_ring *);
+extern int igb_setup_queues(struct igb_adapter *adapter);
+extern void igb_update_stats(struct igb_adapter *);
 extern bool igb_has_link(struct igb_adapter *adapter);
 extern void igb_set_ethtool_ops(struct net_device *);
+extern void igb_check_options(struct igb_adapter *);
 extern void igb_power_up_link(struct igb_adapter *);
-
-static inline s32 igb_reset_phy(struct e1000_hw *hw)
+#ifdef HAVE_PTP_1588_CLOCK
+extern void igb_ptp_init(struct igb_adapter *adapter);
+extern void igb_ptp_stop(struct igb_adapter *adapter);
+extern void igb_ptp_reset(struct igb_adapter *adapter);
+extern void igb_ptp_tx_work(struct work_struct *work);
+extern void igb_ptp_rx_hang(struct igb_adapter *adapter);
+extern void igb_ptp_tx_hwtstamp(struct igb_adapter *adapter);
+extern void igb_ptp_rx_rgtstamp(struct igb_q_vector *q_vector,
+				struct sk_buff *skb);
+extern void igb_ptp_rx_pktstamp(struct igb_q_vector *q_vector,
+				unsigned char *va,
+				struct sk_buff *skb);
+static inline void igb_ptp_rx_hwtstamp(struct igb_ring *rx_ring,
+				       union e1000_adv_rx_desc *rx_desc,
+				       struct sk_buff *skb)
 {
-	if (hw->phy.ops.reset)
-		return hw->phy.ops.reset(hw);
-
-	return 0;
-}
+	if (igb_test_staterr(rx_desc, E1000_RXDADV_STAT_TSIP)) {
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+		igb_ptp_rx_pktstamp(rx_ring->q_vector, skb->data, skb);
+		skb_pull(skb, IGB_TS_HDR_LEN);
+#endif
+		return;
+	}
 
-static inline s32 igb_read_phy_reg(struct e1000_hw *hw, u32 offset, u16 *data)
-{
-	if (hw->phy.ops.read_reg)
-		return hw->phy.ops.read_reg(hw, offset, data);
+	if (igb_test_staterr(rx_desc, E1000_RXDADV_STAT_TS))
+		igb_ptp_rx_rgtstamp(rx_ring->q_vector, skb);
 
-	return 0;
+	/* Update the last_rx_timestamp timer in order to enable watchdog check
+	 * for error case of latched timestamp on a dropped packet.
+	 */
+	rx_ring->last_rx_timestamp = jiffies;
 }
 
-static inline s32 igb_write_phy_reg(struct e1000_hw *hw, u32 offset, u16 data)
-{
-	if (hw->phy.ops.write_reg)
-		return hw->phy.ops.write_reg(hw, offset, data);
+extern int igb_ptp_hwtstamp_ioctl(struct net_device *netdev,
+				  struct ifreq *ifr, int cmd);
+#endif /* HAVE_PTP_1588_CLOCK */
+#ifdef ETHTOOL_OPS_COMPAT
+extern int ethtool_ioctl(struct ifreq *);
+#endif
+extern int igb_write_mc_addr_list(struct net_device *netdev);
+extern int igb_add_mac_filter(struct igb_adapter *adapter, u8 *addr, u16 queue);
+extern int igb_del_mac_filter(struct igb_adapter *adapter, u8* addr, u16 queue);
+extern int igb_available_rars(struct igb_adapter *adapter);
+extern s32 igb_vlvf_set(struct igb_adapter *, u32, bool, u32);
+extern void igb_configure_vt_default_pool(struct igb_adapter *adapter);
+extern void igb_enable_vlan_tags(struct igb_adapter *adapter);
+#ifndef HAVE_VLAN_RX_REGISTER
+extern void igb_vlan_mode(struct net_device *, u32);
+#endif
 
-	return 0;
-}
+#define E1000_PCS_CFG_IGN_SD	1
+
+#ifdef IGB_HWMON
+void igb_sysfs_exit(struct igb_adapter *adapter);
+int igb_sysfs_init(struct igb_adapter *adapter);
+#else
+#ifdef IGB_PROCFS
+int igb_procfs_init(struct igb_adapter* adapter);
+void igb_procfs_exit(struct igb_adapter* adapter);
+int igb_procfs_topdir_init(void);
+void igb_procfs_topdir_exit(void);
+#endif /* IGB_PROCFS */
+#endif /* IGB_HWMON */
 
-static inline s32 igb_get_phy_info(struct e1000_hw *hw)
-{
-	if (hw->phy.ops.get_phy_info)
-		return hw->phy.ops.get_phy_info(hw);
 
-	return 0;
-}
 
 #endif /* _IGB_H_ */
diff --git a/drivers/net/igb/igb_debugfs.c b/drivers/net/igb/igb_debugfs.c
new file mode 100644
index 000000000000..d33c814a8795
--- /dev/null
+++ b/drivers/net/igb/igb_debugfs.c
@@ -0,0 +1,29 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+#include "igb.h"
+
diff --git a/drivers/net/igb/igb_ethtool.c b/drivers/net/igb/igb_ethtool.c
index 414b0225be89..dc3cacc1a9c9 100644
--- a/drivers/net/igb/igb_ethtool.c
+++ b/drivers/net/igb/igb_ethtool.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2007-2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -27,18 +27,27 @@
 
 /* ethtool support for igb */
 
-#include <linux/vmalloc.h>
 #include <linux/netdevice.h>
-#include <linux/pci.h>
-#include <linux/delay.h>
-#include <linux/interrupt.h>
-#include <linux/if_ether.h>
+#include <linux/vmalloc.h>
+
+#ifdef SIOCETHTOOL
 #include <linux/ethtool.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
+#ifdef CONFIG_PM_RUNTIME
+#include <linux/pm_runtime.h>
+#endif /* CONFIG_PM_RUNTIME */
+#include <linux/highmem.h>
 
 #include "igb.h"
-
+#include "igb_regtest.h"
+#include <linux/if_vlan.h>
+#ifdef ETHTOOL_GEEE
+#include <linux/mdio.h>
+#endif
+
+#ifdef ETHTOOL_OPS_COMPAT
+#include "kcompat_ethtool.c"
+#endif
+#ifdef ETHTOOL_GSTATS
 struct igb_stats {
 	char stat_string[ETH_GSTRING_LEN];
 	int sizeof_stat;
@@ -50,6 +59,7 @@ struct igb_stats {
 	.sizeof_stat = FIELD_SIZEOF(struct igb_adapter, _stat), \
 	.stat_offset = offsetof(struct igb_adapter, _stat) \
 }
+
 static const struct igb_stats igb_gstrings_stats[] = {
 	IGB_STAT("rx_packets", stats.gprc),
 	IGB_STAT("tx_packets", stats.gptc),
@@ -83,6 +93,10 @@ static const struct igb_stats igb_gstrings_stats[] = {
 	IGB_STAT("tx_flow_control_xoff", stats.xofftxc),
 	IGB_STAT("rx_long_byte_count", stats.gorc),
 	IGB_STAT("tx_dma_out_of_sync", stats.doosync),
+#ifndef IGB_NO_LRO
+	IGB_STAT("lro_aggregated", lro_stats.coal),
+	IGB_STAT("lro_flushed", lro_stats.flushed),
+#endif /* IGB_LRO */
 	IGB_STAT("tx_smbus", stats.mgptc),
 	IGB_STAT("rx_smbus", stats.mgprc),
 	IGB_STAT("dropped_smbus", stats.mgpdc),
@@ -90,13 +104,18 @@ static const struct igb_stats igb_gstrings_stats[] = {
 	IGB_STAT("os2bmc_tx_by_bmc", stats.b2ospc),
 	IGB_STAT("os2bmc_tx_by_host", stats.o2bspc),
 	IGB_STAT("os2bmc_rx_by_host", stats.b2ogprc),
+#ifdef HAVE_PTP_1588_CLOCK
+	IGB_STAT("tx_hwtstamp_timeouts", tx_hwtstamp_timeouts),
+	IGB_STAT("rx_hwtstamp_cleared", rx_hwtstamp_cleared),
+#endif /* HAVE_PTP_1588_CLOCK */
 };
 
 #define IGB_NETDEV_STAT(_net_stat) { \
-	.stat_string = __stringify(_net_stat), \
-	.sizeof_stat = FIELD_SIZEOF(struct rtnl_link_stats64, _net_stat), \
-	.stat_offset = offsetof(struct rtnl_link_stats64, _net_stat) \
+	.stat_string = #_net_stat, \
+	.sizeof_stat = FIELD_SIZEOF(struct net_device_stats, _net_stat), \
+	.stat_offset = offsetof(struct net_device_stats, _net_stat) \
 }
+
 static const struct igb_stats igb_gstrings_net_stats[] = {
 	IGB_NETDEV_STAT(rx_errors),
 	IGB_NETDEV_STAT(tx_errors),
@@ -109,15 +128,12 @@ static const struct igb_stats igb_gstrings_net_stats[] = {
 	IGB_NETDEV_STAT(tx_heartbeat_errors)
 };
 
-#define IGB_GLOBAL_STATS_LEN	\
-	(sizeof(igb_gstrings_stats) / sizeof(struct igb_stats))
-#define IGB_NETDEV_STATS_LEN	\
-	(sizeof(igb_gstrings_net_stats) / sizeof(struct igb_stats))
+#define IGB_GLOBAL_STATS_LEN ARRAY_SIZE(igb_gstrings_stats)
+#define IGB_NETDEV_STATS_LEN ARRAY_SIZE(igb_gstrings_net_stats)
 #define IGB_RX_QUEUE_STATS_LEN \
 	(sizeof(struct igb_rx_queue_stats) / sizeof(u64))
-
-#define IGB_TX_QUEUE_STATS_LEN 3 /* packets, bytes, restart_queue */
-
+#define IGB_TX_QUEUE_STATS_LEN \
+	(sizeof(struct igb_tx_queue_stats) / sizeof(u64))
 #define IGB_QUEUE_STATS_LEN \
 	((((struct igb_adapter *)netdev_priv(netdev))->num_rx_queues * \
 	  IGB_RX_QUEUE_STATS_LEN) + \
@@ -126,12 +142,16 @@ static const struct igb_stats igb_gstrings_net_stats[] = {
 #define IGB_STATS_LEN \
 	(IGB_GLOBAL_STATS_LEN + IGB_NETDEV_STATS_LEN + IGB_QUEUE_STATS_LEN)
 
+#endif /* ETHTOOL_GSTATS */
+#ifdef ETHTOOL_TEST
 static const char igb_gstrings_test[][ETH_GSTRING_LEN] = {
 	"Register test  (offline)", "Eeprom test    (offline)",
 	"Interrupt test (offline)", "Loopback test  (offline)",
 	"Link test   (on/offline)"
 };
+
 #define IGB_TEST_LEN (sizeof(igb_gstrings_test) / ETH_GSTRING_LEN)
+#endif /* ETHTOOL_TEST */
 
 static int igb_get_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
 {
@@ -147,7 +167,8 @@ static int igb_get_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
 				   SUPPORTED_100baseT_Full |
 				   SUPPORTED_1000baseT_Full|
 				   SUPPORTED_Autoneg |
-				   SUPPORTED_TP);
+				   SUPPORTED_TP |
+				   SUPPORTED_Pause);
 		ecmd->advertising = ADVERTISED_TP;
 
 		if (hw->mac.autoneg == 1) {
@@ -158,43 +179,102 @@ static int igb_get_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
 
 		ecmd->port = PORT_TP;
 		ecmd->phy_address = hw->phy.addr;
+		ecmd->transceiver = XCVR_INTERNAL;
+
 	} else {
-		ecmd->supported   = (SUPPORTED_1000baseT_Full |
-				     SUPPORTED_FIBRE |
-				     SUPPORTED_Autoneg);
+		ecmd->supported = (SUPPORTED_1000baseT_Full |
+				   SUPPORTED_100baseT_Full |
+				   SUPPORTED_FIBRE |
+				   SUPPORTED_Autoneg |
+				   SUPPORTED_Pause);
+		if (hw->mac.type == e1000_i354)
+			ecmd->supported |= (SUPPORTED_2500baseX_Full);
+
+		ecmd->advertising = ADVERTISED_FIBRE;
+
+		switch (adapter->link_speed) {
+		case SPEED_2500:
+			ecmd->advertising = ADVERTISED_2500baseX_Full;
+			break;
+		case SPEED_1000:
+			ecmd->advertising = ADVERTISED_1000baseT_Full;
+			break;
+		case SPEED_100:
+			ecmd->advertising = ADVERTISED_100baseT_Full;
+			break;
+		default:
+			break;
+		}
 
-		ecmd->advertising = (ADVERTISED_1000baseT_Full |
-				     ADVERTISED_FIBRE |
-				     ADVERTISED_Autoneg);
+		if (hw->mac.autoneg == 1)
+			ecmd->advertising |= ADVERTISED_Autoneg;
 
 		ecmd->port = PORT_FIBRE;
+		ecmd->transceiver = XCVR_EXTERNAL;
 	}
 
-	ecmd->transceiver = XCVR_INTERNAL;
+	if (hw->mac.autoneg != 1)
+		ecmd->advertising &= ~(ADVERTISED_Pause |
+				       ADVERTISED_Asym_Pause);
+
+	if (hw->fc.requested_mode == e1000_fc_full)
+		ecmd->advertising |= ADVERTISED_Pause;
+	else if (hw->fc.requested_mode == e1000_fc_rx_pause)
+		ecmd->advertising |= (ADVERTISED_Pause |
+				      ADVERTISED_Asym_Pause);
+	else if (hw->fc.requested_mode == e1000_fc_tx_pause)
+		ecmd->advertising |=  ADVERTISED_Asym_Pause;
+	else
+		ecmd->advertising &= ~(ADVERTISED_Pause |
+				       ADVERTISED_Asym_Pause);
 
-	status = rd32(E1000_STATUS);
+	status = E1000_READ_REG(hw, E1000_STATUS);
 
 	if (status & E1000_STATUS_LU) {
-
-		if ((status & E1000_STATUS_SPEED_1000) ||
-		    hw->phy.media_type != e1000_media_type_copper)
-			ethtool_cmd_speed_set(ecmd, SPEED_1000);
+		if ((hw->mac.type == e1000_i354) &&
+		    (status & E1000_STATUS_2P5_SKU) &&
+		    !(status & E1000_STATUS_2P5_SKU_OVER))
+			ecmd->speed = SPEED_2500;
+		else if (status & E1000_STATUS_SPEED_1000)
+			ecmd->speed = SPEED_1000;
 		else if (status & E1000_STATUS_SPEED_100)
-			ethtool_cmd_speed_set(ecmd, SPEED_100);
+			ecmd->speed = SPEED_100;
 		else
-			ethtool_cmd_speed_set(ecmd, SPEED_10);
+			ecmd->speed = SPEED_10;
 
 		if ((status & E1000_STATUS_FD) ||
 		    hw->phy.media_type != e1000_media_type_copper)
 			ecmd->duplex = DUPLEX_FULL;
 		else
 			ecmd->duplex = DUPLEX_HALF;
+
 	} else {
-		ethtool_cmd_speed_set(ecmd, -1);
+		ecmd->speed = -1;
 		ecmd->duplex = -1;
 	}
 
-	ecmd->autoneg = hw->mac.autoneg ? AUTONEG_ENABLE : AUTONEG_DISABLE;
+	if ((hw->phy.media_type == e1000_media_type_fiber) ||
+	    hw->mac.autoneg)
+		ecmd->autoneg = AUTONEG_ENABLE;
+	else
+		ecmd->autoneg = AUTONEG_DISABLE;
+#ifdef ETH_TP_MDI_X
+
+	/* MDI-X => 2; MDI =>1; Invalid =>0 */
+	if (hw->phy.media_type == e1000_media_type_copper)
+		ecmd->eth_tp_mdix = hw->phy.is_mdix ? ETH_TP_MDI_X :
+						      ETH_TP_MDI;
+	else
+		ecmd->eth_tp_mdix = ETH_TP_MDI_INVALID;
+
+#ifdef ETH_TP_MDI_AUTO
+	if (hw->phy.mdix == AUTO_ALL_MODES)
+		ecmd->eth_tp_mdix_ctrl = ETH_TP_MDI_AUTO;
+	else
+		ecmd->eth_tp_mdix_ctrl = hw->phy.mdix;
+
+#endif
+#endif /* ETH_TP_MDI_X */
 	return 0;
 }
 
@@ -203,33 +283,95 @@ static int igb_set_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
 	struct igb_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
 
+	if (ecmd->duplex  == DUPLEX_HALF) {
+		if (!hw->dev_spec._82575.eee_disable)
+			dev_info(pci_dev_to_dev(adapter->pdev), "EEE disabled: not supported with half duplex\n");
+		hw->dev_spec._82575.eee_disable = true;
+	} else {
+		if (hw->dev_spec._82575.eee_disable)
+			dev_info(pci_dev_to_dev(adapter->pdev), "EEE enabled\n");
+		hw->dev_spec._82575.eee_disable = false;
+	}
+
 	/* When SoL/IDER sessions are active, autoneg/speed/duplex
 	 * cannot be changed */
-	if (igb_check_reset_block(hw)) {
-		dev_err(&adapter->pdev->dev, "Cannot change link "
+	if (e1000_check_reset_block(hw)) {
+		dev_err(pci_dev_to_dev(adapter->pdev), "Cannot change link "
 			"characteristics when SoL/IDER is active.\n");
 		return -EINVAL;
 	}
 
+#ifdef ETH_TP_MDI_AUTO
+	/*
+	 * MDI setting is only allowed when autoneg enabled because
+	 * some hardware doesn't allow MDI setting when speed or
+	 * duplex is forced.
+	 */
+	if (ecmd->eth_tp_mdix_ctrl) {
+		if (hw->phy.media_type != e1000_media_type_copper)
+			return -EOPNOTSUPP;
+
+		if ((ecmd->eth_tp_mdix_ctrl != ETH_TP_MDI_AUTO) &&
+		    (ecmd->autoneg != AUTONEG_ENABLE)) {
+			dev_err(&adapter->pdev->dev, "forcing MDI/MDI-X state is not supported when link speed and/or duplex are forced\n");
+			return -EINVAL;
+		}
+	}
+
+#endif /* ETH_TP_MDI_AUTO */
 	while (test_and_set_bit(__IGB_RESETTING, &adapter->state))
-		msleep(1);
+		usleep_range(1000, 2000);
 
 	if (ecmd->autoneg == AUTONEG_ENABLE) {
 		hw->mac.autoneg = 1;
-		hw->phy.autoneg_advertised = ecmd->advertising |
-					     ADVERTISED_TP |
-					     ADVERTISED_Autoneg;
+		if (hw->phy.media_type == e1000_media_type_fiber) {
+			hw->phy.autoneg_advertised = ecmd->advertising |
+						     ADVERTISED_FIBRE |
+						     ADVERTISED_Autoneg;
+			switch (adapter->link_speed) {
+			case SPEED_2500:
+				hw->phy.autoneg_advertised =
+					ADVERTISED_2500baseX_Full;
+				break;
+			case SPEED_1000:
+				hw->phy.autoneg_advertised =
+					ADVERTISED_1000baseT_Full;
+				break;
+			case SPEED_100:
+				hw->phy.autoneg_advertised =
+					ADVERTISED_100baseT_Full;
+				break;
+			default:
+				break;
+			}
+		} else {
+			hw->phy.autoneg_advertised = ecmd->advertising |
+						     ADVERTISED_TP |
+						     ADVERTISED_Autoneg;
+		}
 		ecmd->advertising = hw->phy.autoneg_advertised;
 		if (adapter->fc_autoneg)
 			hw->fc.requested_mode = e1000_fc_default;
 	} else {
-		u32 speed = ethtool_cmd_speed(ecmd);
-		if (igb_set_spd_dplx(adapter, speed, ecmd->duplex)) {
+		if (igb_set_spd_dplx(adapter, ecmd->speed + ecmd->duplex)) {
 			clear_bit(__IGB_RESETTING, &adapter->state);
 			return -EINVAL;
 		}
 	}
 
+#ifdef ETH_TP_MDI_AUTO
+	/* MDI-X => 2; MDI => 1; Auto => 3 */
+	if (ecmd->eth_tp_mdix_ctrl) {
+		/* fix up the value for auto (3 => 0) as zero is mapped
+		 * internally to auto
+		 */
+		if (ecmd->eth_tp_mdix_ctrl == ETH_TP_MDI_AUTO)
+			hw->phy.mdix = AUTO_ALL_MODES;
+		else
+			hw->phy.mdix = ecmd->eth_tp_mdix_ctrl;
+	}
+
+#endif /* ETH_TP_MDI_AUTO */
 	/* reset the link */
 	if (netif_running(adapter->netdev)) {
 		igb_down(adapter);
@@ -288,7 +430,7 @@ static int igb_set_pauseparam(struct net_device *netdev,
 	adapter->fc_autoneg = pause->autoneg;
 
 	while (test_and_set_bit(__IGB_RESETTING, &adapter->state))
-		msleep(1);
+		usleep_range(1000, 2000);
 
 	if (adapter->fc_autoneg == AUTONEG_ENABLE) {
 		hw->fc.requested_mode = e1000_fc_default;
@@ -310,10 +452,18 @@ static int igb_set_pauseparam(struct net_device *netdev,
 
 		hw->fc.current_mode = hw->fc.requested_mode;
 
-		retval = ((hw->phy.media_type == e1000_media_type_copper) ?
-			  igb_force_mac_fc(hw) : igb_setup_link(hw));
+		if (hw->phy.media_type == e1000_media_type_fiber) {
+			retval = hw->mac.ops.setup_link(hw);
+			/* implicit goto out */
+		} else {
+			retval = e1000_force_mac_fc(hw);
+			if (retval)
+				goto out;
+			e1000_set_fc_watermarks_generic(hw);
+		}
 	}
 
+out:
 	clear_bit(__IGB_RESETTING, &adapter->state);
 	return retval;
 }
@@ -332,7 +482,7 @@ static void igb_set_msglevel(struct net_device *netdev, u32 data)
 
 static int igb_get_regs_len(struct net_device *netdev)
 {
-#define IGB_REGS_LEN 551
+#define IGB_REGS_LEN 555
 	return IGB_REGS_LEN * sizeof(u32);
 }
 
@@ -349,78 +499,78 @@ static void igb_get_regs(struct net_device *netdev,
 	regs->version = (1 << 24) | (hw->revision_id << 16) | hw->device_id;
 
 	/* General Registers */
-	regs_buff[0] = rd32(E1000_CTRL);
-	regs_buff[1] = rd32(E1000_STATUS);
-	regs_buff[2] = rd32(E1000_CTRL_EXT);
-	regs_buff[3] = rd32(E1000_MDIC);
-	regs_buff[4] = rd32(E1000_SCTL);
-	regs_buff[5] = rd32(E1000_CONNSW);
-	regs_buff[6] = rd32(E1000_VET);
-	regs_buff[7] = rd32(E1000_LEDCTL);
-	regs_buff[8] = rd32(E1000_PBA);
-	regs_buff[9] = rd32(E1000_PBS);
-	regs_buff[10] = rd32(E1000_FRTIMER);
-	regs_buff[11] = rd32(E1000_TCPTIMER);
+	regs_buff[0] = E1000_READ_REG(hw, E1000_CTRL);
+	regs_buff[1] = E1000_READ_REG(hw, E1000_STATUS);
+	regs_buff[2] = E1000_READ_REG(hw, E1000_CTRL_EXT);
+	regs_buff[3] = E1000_READ_REG(hw, E1000_MDIC);
+	regs_buff[4] = E1000_READ_REG(hw, E1000_SCTL);
+	regs_buff[5] = E1000_READ_REG(hw, E1000_CONNSW);
+	regs_buff[6] = E1000_READ_REG(hw, E1000_VET);
+	regs_buff[7] = E1000_READ_REG(hw, E1000_LEDCTL);
+	regs_buff[8] = E1000_READ_REG(hw, E1000_PBA);
+	regs_buff[9] = E1000_READ_REG(hw, E1000_PBS);
+	regs_buff[10] = E1000_READ_REG(hw, E1000_FRTIMER);
+	regs_buff[11] = E1000_READ_REG(hw, E1000_TCPTIMER);
 
 	/* NVM Register */
-	regs_buff[12] = rd32(E1000_EECD);
+	regs_buff[12] = E1000_READ_REG(hw, E1000_EECD);
 
 	/* Interrupt */
 	/* Reading EICS for EICR because they read the
 	 * same but EICS does not clear on read */
-	regs_buff[13] = rd32(E1000_EICS);
-	regs_buff[14] = rd32(E1000_EICS);
-	regs_buff[15] = rd32(E1000_EIMS);
-	regs_buff[16] = rd32(E1000_EIMC);
-	regs_buff[17] = rd32(E1000_EIAC);
-	regs_buff[18] = rd32(E1000_EIAM);
+	regs_buff[13] = E1000_READ_REG(hw, E1000_EICS);
+	regs_buff[14] = E1000_READ_REG(hw, E1000_EICS);
+	regs_buff[15] = E1000_READ_REG(hw, E1000_EIMS);
+	regs_buff[16] = E1000_READ_REG(hw, E1000_EIMC);
+	regs_buff[17] = E1000_READ_REG(hw, E1000_EIAC);
+	regs_buff[18] = E1000_READ_REG(hw, E1000_EIAM);
 	/* Reading ICS for ICR because they read the
 	 * same but ICS does not clear on read */
-	regs_buff[19] = rd32(E1000_ICS);
-	regs_buff[20] = rd32(E1000_ICS);
-	regs_buff[21] = rd32(E1000_IMS);
-	regs_buff[22] = rd32(E1000_IMC);
-	regs_buff[23] = rd32(E1000_IAC);
-	regs_buff[24] = rd32(E1000_IAM);
-	regs_buff[25] = rd32(E1000_IMIRVP);
+	regs_buff[19] = E1000_READ_REG(hw, E1000_ICS);
+	regs_buff[20] = E1000_READ_REG(hw, E1000_ICS);
+	regs_buff[21] = E1000_READ_REG(hw, E1000_IMS);
+	regs_buff[22] = E1000_READ_REG(hw, E1000_IMC);
+	regs_buff[23] = E1000_READ_REG(hw, E1000_IAC);
+	regs_buff[24] = E1000_READ_REG(hw, E1000_IAM);
+	regs_buff[25] = E1000_READ_REG(hw, E1000_IMIRVP);
 
 	/* Flow Control */
-	regs_buff[26] = rd32(E1000_FCAL);
-	regs_buff[27] = rd32(E1000_FCAH);
-	regs_buff[28] = rd32(E1000_FCTTV);
-	regs_buff[29] = rd32(E1000_FCRTL);
-	regs_buff[30] = rd32(E1000_FCRTH);
-	regs_buff[31] = rd32(E1000_FCRTV);
+	regs_buff[26] = E1000_READ_REG(hw, E1000_FCAL);
+	regs_buff[27] = E1000_READ_REG(hw, E1000_FCAH);
+	regs_buff[28] = E1000_READ_REG(hw, E1000_FCTTV);
+	regs_buff[29] = E1000_READ_REG(hw, E1000_FCRTL);
+	regs_buff[30] = E1000_READ_REG(hw, E1000_FCRTH);
+	regs_buff[31] = E1000_READ_REG(hw, E1000_FCRTV);
 
 	/* Receive */
-	regs_buff[32] = rd32(E1000_RCTL);
-	regs_buff[33] = rd32(E1000_RXCSUM);
-	regs_buff[34] = rd32(E1000_RLPML);
-	regs_buff[35] = rd32(E1000_RFCTL);
-	regs_buff[36] = rd32(E1000_MRQC);
-	regs_buff[37] = rd32(E1000_VT_CTL);
+	regs_buff[32] = E1000_READ_REG(hw, E1000_RCTL);
+	regs_buff[33] = E1000_READ_REG(hw, E1000_RXCSUM);
+	regs_buff[34] = E1000_READ_REG(hw, E1000_RLPML);
+	regs_buff[35] = E1000_READ_REG(hw, E1000_RFCTL);
+	regs_buff[36] = E1000_READ_REG(hw, E1000_MRQC);
+	regs_buff[37] = E1000_READ_REG(hw, E1000_VT_CTL);
 
 	/* Transmit */
-	regs_buff[38] = rd32(E1000_TCTL);
-	regs_buff[39] = rd32(E1000_TCTL_EXT);
-	regs_buff[40] = rd32(E1000_TIPG);
-	regs_buff[41] = rd32(E1000_DTXCTL);
+	regs_buff[38] = E1000_READ_REG(hw, E1000_TCTL);
+	regs_buff[39] = E1000_READ_REG(hw, E1000_TCTL_EXT);
+	regs_buff[40] = E1000_READ_REG(hw, E1000_TIPG);
+	regs_buff[41] = E1000_READ_REG(hw, E1000_DTXCTL);
 
 	/* Wake Up */
-	regs_buff[42] = rd32(E1000_WUC);
-	regs_buff[43] = rd32(E1000_WUFC);
-	regs_buff[44] = rd32(E1000_WUS);
-	regs_buff[45] = rd32(E1000_IPAV);
-	regs_buff[46] = rd32(E1000_WUPL);
+	regs_buff[42] = E1000_READ_REG(hw, E1000_WUC);
+	regs_buff[43] = E1000_READ_REG(hw, E1000_WUFC);
+	regs_buff[44] = E1000_READ_REG(hw, E1000_WUS);
+	regs_buff[45] = E1000_READ_REG(hw, E1000_IPAV);
+	regs_buff[46] = E1000_READ_REG(hw, E1000_WUPL);
 
 	/* MAC */
-	regs_buff[47] = rd32(E1000_PCS_CFG0);
-	regs_buff[48] = rd32(E1000_PCS_LCTL);
-	regs_buff[49] = rd32(E1000_PCS_LSTAT);
-	regs_buff[50] = rd32(E1000_PCS_ANADV);
-	regs_buff[51] = rd32(E1000_PCS_LPAB);
-	regs_buff[52] = rd32(E1000_PCS_NPTX);
-	regs_buff[53] = rd32(E1000_PCS_LPABNP);
+	regs_buff[47] = E1000_READ_REG(hw, E1000_PCS_CFG0);
+	regs_buff[48] = E1000_READ_REG(hw, E1000_PCS_LCTL);
+	regs_buff[49] = E1000_READ_REG(hw, E1000_PCS_LSTAT);
+	regs_buff[50] = E1000_READ_REG(hw, E1000_PCS_ANADV);
+	regs_buff[51] = E1000_READ_REG(hw, E1000_PCS_LPAB);
+	regs_buff[52] = E1000_READ_REG(hw, E1000_PCS_NPTX);
+	regs_buff[53] = E1000_READ_REG(hw, E1000_PCS_LPABNP);
 
 	/* Statistics */
 	regs_buff[54] = adapter->stats.crcerrs;
@@ -486,73 +636,75 @@ static void igb_get_regs(struct net_device *netdev,
 	regs_buff[120] = adapter->stats.hrmpc;
 
 	for (i = 0; i < 4; i++)
-		regs_buff[121 + i] = rd32(E1000_SRRCTL(i));
+		regs_buff[121 + i] = E1000_READ_REG(hw, E1000_SRRCTL(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[125 + i] = rd32(E1000_PSRTYPE(i));
+		regs_buff[125 + i] = E1000_READ_REG(hw, E1000_PSRTYPE(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[129 + i] = rd32(E1000_RDBAL(i));
+		regs_buff[129 + i] = E1000_READ_REG(hw, E1000_RDBAL(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[133 + i] = rd32(E1000_RDBAH(i));
+		regs_buff[133 + i] = E1000_READ_REG(hw, E1000_RDBAH(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[137 + i] = rd32(E1000_RDLEN(i));
+		regs_buff[137 + i] = E1000_READ_REG(hw, E1000_RDLEN(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[141 + i] = rd32(E1000_RDH(i));
+		regs_buff[141 + i] = E1000_READ_REG(hw, E1000_RDH(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[145 + i] = rd32(E1000_RDT(i));
+		regs_buff[145 + i] = E1000_READ_REG(hw, E1000_RDT(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[149 + i] = rd32(E1000_RXDCTL(i));
+		regs_buff[149 + i] = E1000_READ_REG(hw, E1000_RXDCTL(i));
 
 	for (i = 0; i < 10; i++)
-		regs_buff[153 + i] = rd32(E1000_EITR(i));
+		regs_buff[153 + i] = E1000_READ_REG(hw, E1000_EITR(i));
 	for (i = 0; i < 8; i++)
-		regs_buff[163 + i] = rd32(E1000_IMIR(i));
+		regs_buff[163 + i] = E1000_READ_REG(hw, E1000_IMIR(i));
 	for (i = 0; i < 8; i++)
-		regs_buff[171 + i] = rd32(E1000_IMIREXT(i));
+		regs_buff[171 + i] = E1000_READ_REG(hw, E1000_IMIREXT(i));
 	for (i = 0; i < 16; i++)
-		regs_buff[179 + i] = rd32(E1000_RAL(i));
+		regs_buff[179 + i] = E1000_READ_REG(hw, E1000_RAL(i));
 	for (i = 0; i < 16; i++)
-		regs_buff[195 + i] = rd32(E1000_RAH(i));
+		regs_buff[195 + i] = E1000_READ_REG(hw, E1000_RAH(i));
 
 	for (i = 0; i < 4; i++)
-		regs_buff[211 + i] = rd32(E1000_TDBAL(i));
+		regs_buff[211 + i] = E1000_READ_REG(hw, E1000_TDBAL(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[215 + i] = rd32(E1000_TDBAH(i));
+		regs_buff[215 + i] = E1000_READ_REG(hw, E1000_TDBAH(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[219 + i] = rd32(E1000_TDLEN(i));
+		regs_buff[219 + i] = E1000_READ_REG(hw, E1000_TDLEN(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[223 + i] = rd32(E1000_TDH(i));
+		regs_buff[223 + i] = E1000_READ_REG(hw, E1000_TDH(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[227 + i] = rd32(E1000_TDT(i));
+		regs_buff[227 + i] = E1000_READ_REG(hw, E1000_TDT(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[231 + i] = rd32(E1000_TXDCTL(i));
+		regs_buff[231 + i] = E1000_READ_REG(hw, E1000_TXDCTL(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[235 + i] = rd32(E1000_TDWBAL(i));
+		regs_buff[235 + i] = E1000_READ_REG(hw, E1000_TDWBAL(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[239 + i] = rd32(E1000_TDWBAH(i));
+		regs_buff[239 + i] = E1000_READ_REG(hw, E1000_TDWBAH(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[243 + i] = rd32(E1000_DCA_TXCTRL(i));
+		regs_buff[243 + i] = E1000_READ_REG(hw, E1000_DCA_TXCTRL(i));
 
 	for (i = 0; i < 4; i++)
-		regs_buff[247 + i] = rd32(E1000_IP4AT_REG(i));
+		regs_buff[247 + i] = E1000_READ_REG(hw, E1000_IP4AT_REG(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[251 + i] = rd32(E1000_IP6AT_REG(i));
+		regs_buff[251 + i] = E1000_READ_REG(hw, E1000_IP6AT_REG(i));
 	for (i = 0; i < 32; i++)
-		regs_buff[255 + i] = rd32(E1000_WUPM_REG(i));
+		regs_buff[255 + i] = E1000_READ_REG(hw, E1000_WUPM_REG(i));
 	for (i = 0; i < 128; i++)
-		regs_buff[287 + i] = rd32(E1000_FFMT_REG(i));
+		regs_buff[287 + i] = E1000_READ_REG(hw, E1000_FFMT_REG(i));
 	for (i = 0; i < 128; i++)
-		regs_buff[415 + i] = rd32(E1000_FFVT_REG(i));
+		regs_buff[415 + i] = E1000_READ_REG(hw, E1000_FFVT_REG(i));
 	for (i = 0; i < 4; i++)
-		regs_buff[543 + i] = rd32(E1000_FFLT_REG(i));
-
-	regs_buff[547] = rd32(E1000_TDFH);
-	regs_buff[548] = rd32(E1000_TDFT);
-	regs_buff[549] = rd32(E1000_TDFHS);
-	regs_buff[550] = rd32(E1000_TDFPC);
-	regs_buff[551] = adapter->stats.o2bgptc;
-	regs_buff[552] = adapter->stats.b2ospc;
-	regs_buff[553] = adapter->stats.o2bspc;
-	regs_buff[554] = adapter->stats.b2ogprc;
+		regs_buff[543 + i] = E1000_READ_REG(hw, E1000_FFLT_REG(i));
+
+	regs_buff[547] = E1000_READ_REG(hw, E1000_TDFH);
+	regs_buff[548] = E1000_READ_REG(hw, E1000_TDFT);
+	regs_buff[549] = E1000_READ_REG(hw, E1000_TDFHS);
+	regs_buff[550] = E1000_READ_REG(hw, E1000_TDFPC);
+	if (hw->mac.type > e1000_82580) {
+		regs_buff[551] = adapter->stats.o2bgptc;
+		regs_buff[552] = adapter->stats.b2ospc;
+		regs_buff[553] = adapter->stats.o2bspc;
+		regs_buff[554] = adapter->stats.b2ogprc;
+	}
 }
 
 static int igb_get_eeprom_len(struct net_device *netdev)
@@ -585,13 +737,13 @@ static int igb_get_eeprom(struct net_device *netdev,
 		return -ENOMEM;
 
 	if (hw->nvm.type == e1000_nvm_eeprom_spi)
-		ret_val = hw->nvm.ops.read(hw, first_word,
-					    last_word - first_word + 1,
-					    eeprom_buff);
+		ret_val = e1000_read_nvm(hw, first_word,
+					 last_word - first_word + 1,
+					 eeprom_buff);
 	else {
 		for (i = 0; i < last_word - first_word + 1; i++) {
-			ret_val = hw->nvm.ops.read(hw, first_word + i, 1,
-						    &eeprom_buff[i]);
+			ret_val = e1000_read_nvm(hw, first_word + i, 1,
+						 &eeprom_buff[i]);
 			if (ret_val)
 				break;
 		}
@@ -599,7 +751,7 @@ static int igb_get_eeprom(struct net_device *netdev,
 
 	/* Device's eeprom is always little-endian, word addressable */
 	for (i = 0; i < last_word - first_word + 1; i++)
-		le16_to_cpus(&eeprom_buff[i]);
+		eeprom_buff[i] = le16_to_cpu(eeprom_buff[i]);
 
 	memcpy(bytes, (u8 *)eeprom_buff + (eeprom->offset & 1),
 			eeprom->len);
@@ -637,15 +789,15 @@ static int igb_set_eeprom(struct net_device *netdev,
 	if (eeprom->offset & 1) {
 		/* need read/modify/write of first changed EEPROM word */
 		/* only the second byte of the word is being modified */
-		ret_val = hw->nvm.ops.read(hw, first_word, 1,
+		ret_val = e1000_read_nvm(hw, first_word, 1,
 					    &eeprom_buff[0]);
 		ptr++;
 	}
 	if (((eeprom->offset + eeprom->len) & 1) && (ret_val == 0)) {
 		/* need read/modify/write of last changed EEPROM word */
 		/* only the first byte of the word is being modified */
-		ret_val = hw->nvm.ops.read(hw, last_word, 1,
-				   &eeprom_buff[last_word - first_word]);
+		ret_val = e1000_read_nvm(hw, last_word, 1,
+			  &eeprom_buff[last_word - first_word]);
 	}
 
 	/* Device's eeprom is always little-endian, word addressable */
@@ -655,15 +807,15 @@ static int igb_set_eeprom(struct net_device *netdev,
 	memcpy(ptr, bytes, eeprom->len);
 
 	for (i = 0; i < last_word - first_word + 1; i++)
-		eeprom_buff[i] = cpu_to_le16(eeprom_buff[i]);
+		cpu_to_le16s(&eeprom_buff[i]);
 
-	ret_val = hw->nvm.ops.write(hw, first_word,
-				     last_word - first_word + 1, eeprom_buff);
+	ret_val = e1000_write_nvm(hw, first_word,
+				  last_word - first_word + 1, eeprom_buff);
 
-	/* Update the checksum over the first part of the EEPROM if needed
+	/* Update the checksum if write succeeded.
 	 * and flush shadow RAM for 82573 controllers */
-	if ((ret_val == 0) && ((first_word <= NVM_CHECKSUM_REG)))
-		hw->nvm.ops.update(hw);
+	if (ret_val == 0)
+		e1000_update_nvm_checksum(hw);
 
 	kfree(eeprom_buff);
 	return ret_val;
@@ -673,25 +825,13 @@ static void igb_get_drvinfo(struct net_device *netdev,
 			    struct ethtool_drvinfo *drvinfo)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
-	char firmware_version[32];
-	u16 eeprom_data;
 
 	strncpy(drvinfo->driver,  igb_driver_name, sizeof(drvinfo->driver) - 1);
-	strncpy(drvinfo->version, igb_driver_version,
-		sizeof(drvinfo->version) - 1);
-
-	/* EEPROM image version # is reported as firmware version # for
-	 * 82575 controllers */
-	adapter->hw.nvm.ops.read(&adapter->hw, 5, 1, &eeprom_data);
-	sprintf(firmware_version, "%d.%d-%d",
-		(eeprom_data & 0xF000) >> 12,
-		(eeprom_data & 0x0FF0) >> 4,
-		eeprom_data & 0x000F);
-
-	strncpy(drvinfo->fw_version, firmware_version,
+	strncpy(drvinfo->version, igb_driver_version, sizeof(drvinfo->version) - 1);
+
+	strncpy(drvinfo->fw_version, adapter->fw_version,
 		sizeof(drvinfo->fw_version) - 1);
-	strncpy(drvinfo->bus_info, pci_name(adapter->pdev),
-		sizeof(drvinfo->bus_info) - 1);
+	strncpy(drvinfo->bus_info, pci_name(adapter->pdev), sizeof(drvinfo->bus_info) -1);
 	drvinfo->n_stats = IGB_STATS_LEN;
 	drvinfo->testinfo_len = IGB_TEST_LEN;
 	drvinfo->regdump_len = igb_get_regs_len(netdev);
@@ -724,12 +864,12 @@ static int igb_set_ringparam(struct net_device *netdev,
 	if ((ring->rx_mini_pending) || (ring->rx_jumbo_pending))
 		return -EINVAL;
 
-	new_rx_count = min_t(u32, ring->rx_pending, IGB_MAX_RXD);
-	new_rx_count = max_t(u16, new_rx_count, IGB_MIN_RXD);
+	new_rx_count = min(ring->rx_pending, (u32)IGB_MAX_RXD);
+	new_rx_count = max(new_rx_count, (u16)IGB_MIN_RXD);
 	new_rx_count = ALIGN(new_rx_count, REQ_RX_DESCRIPTOR_MULTIPLE);
 
-	new_tx_count = min_t(u32, ring->tx_pending, IGB_MAX_TXD);
-	new_tx_count = max_t(u16, new_tx_count, IGB_MIN_TXD);
+	new_tx_count = min(ring->tx_pending, (u32)IGB_MAX_TXD);
+	new_tx_count = max(new_tx_count, (u16)IGB_MIN_TXD);
 	new_tx_count = ALIGN(new_tx_count, REQ_TX_DESCRIPTOR_MULTIPLE);
 
 	if ((new_tx_count == adapter->tx_ring_count) &&
@@ -739,7 +879,7 @@ static int igb_set_ringparam(struct net_device *netdev,
 	}
 
 	while (test_and_set_bit(__IGB_RESETTING, &adapter->state))
-		msleep(1);
+		usleep_range(1000, 2000);
 
 	if (!netif_running(adapter->netdev)) {
 		for (i = 0; i < adapter->num_tx_queues; i++)
@@ -828,191 +968,6 @@ clear_reset:
 	return err;
 }
 
-/* ethtool register test data */
-struct igb_reg_test {
-	u16 reg;
-	u16 reg_offset;
-	u16 array_len;
-	u16 test_type;
-	u32 mask;
-	u32 write;
-};
-
-/* In the hardware, registers are laid out either singly, in arrays
- * spaced 0x100 bytes apart, or in contiguous tables.  We assume
- * most tests take place on arrays or single registers (handled
- * as a single-element array) and special-case the tables.
- * Table tests are always pattern tests.
- *
- * We also make provision for some required setup steps by specifying
- * registers to be written without any read-back testing.
- */
-
-#define PATTERN_TEST	1
-#define SET_READ_TEST	2
-#define WRITE_NO_TEST	3
-#define TABLE32_TEST	4
-#define TABLE64_TEST_LO	5
-#define TABLE64_TEST_HI	6
-
-/* i350 reg test */
-static struct igb_reg_test reg_test_i350[] = {
-	{ E1000_FCAL,	   0x100, 1,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_FCAH,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
-	{ E1000_FCT,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
-	{ E1000_VET,	   0x100, 1,  PATTERN_TEST, 0xFFFF0000, 0xFFFF0000 },
-	{ E1000_RDBAL(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
-	{ E1000_RDBAH(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RDLEN(0),  0x100, 4,  PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
-	{ E1000_RDBAL(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
-	{ E1000_RDBAH(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RDLEN(4),  0x40,  4,  PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
-	/* RDH is read-only for i350, only test RDT. */
-	{ E1000_RDT(0),	   0x100, 4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
-	{ E1000_RDT(4),	   0x40,  4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
-	{ E1000_FCRTH,	   0x100, 1,  PATTERN_TEST, 0x0000FFF0, 0x0000FFF0 },
-	{ E1000_FCTTV,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
-	{ E1000_TIPG,	   0x100, 1,  PATTERN_TEST, 0x3FFFFFFF, 0x3FFFFFFF },
-	{ E1000_TDBAL(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
-	{ E1000_TDBAH(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_TDLEN(0),  0x100, 4,  PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
-	{ E1000_TDBAL(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
-	{ E1000_TDBAH(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_TDLEN(4),  0x40,  4,  PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
-	{ E1000_TDT(0),	   0x100, 4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
-	{ E1000_TDT(4),	   0x40,  4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
-	{ E1000_RCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
-	{ E1000_RCTL, 	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0x003FFFFB },
-	{ E1000_RCTL, 	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0xFFFFFFFF },
-	{ E1000_TCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
-	{ E1000_RA,	   0, 16, TABLE64_TEST_LO,
-						0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RA,	   0, 16, TABLE64_TEST_HI,
-						0xC3FFFFFF, 0xFFFFFFFF },
-	{ E1000_RA2,	   0, 16, TABLE64_TEST_LO,
-						0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RA2,	   0, 16, TABLE64_TEST_HI,
-						0xC3FFFFFF, 0xFFFFFFFF },
-	{ E1000_MTA,	   0, 128, TABLE32_TEST,
-						0xFFFFFFFF, 0xFFFFFFFF },
-	{ 0, 0, 0, 0 }
-};
-
-/* 82580 reg test */
-static struct igb_reg_test reg_test_82580[] = {
-	{ E1000_FCAL,	   0x100, 1,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_FCAH,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
-	{ E1000_FCT,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
-	{ E1000_VET,	   0x100, 1,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RDBAL(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
-	{ E1000_RDBAH(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RDLEN(0),  0x100, 4,  PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
-	{ E1000_RDBAL(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
-	{ E1000_RDBAH(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RDLEN(4),  0x40,  4,  PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
-	/* RDH is read-only for 82580, only test RDT. */
-	{ E1000_RDT(0),	   0x100, 4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
-	{ E1000_RDT(4),	   0x40,  4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
-	{ E1000_FCRTH,	   0x100, 1,  PATTERN_TEST, 0x0000FFF0, 0x0000FFF0 },
-	{ E1000_FCTTV,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
-	{ E1000_TIPG,	   0x100, 1,  PATTERN_TEST, 0x3FFFFFFF, 0x3FFFFFFF },
-	{ E1000_TDBAL(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
-	{ E1000_TDBAH(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_TDLEN(0),  0x100, 4,  PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
-	{ E1000_TDBAL(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
-	{ E1000_TDBAH(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_TDLEN(4),  0x40,  4,  PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
-	{ E1000_TDT(0),	   0x100, 4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
-	{ E1000_TDT(4),	   0x40,  4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
-	{ E1000_RCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
-	{ E1000_RCTL, 	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0x003FFFFB },
-	{ E1000_RCTL, 	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0xFFFFFFFF },
-	{ E1000_TCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
-	{ E1000_RA,	   0, 16, TABLE64_TEST_LO,
-						0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RA,	   0, 16, TABLE64_TEST_HI,
-						0x83FFFFFF, 0xFFFFFFFF },
-	{ E1000_RA2,	   0, 8, TABLE64_TEST_LO,
-						0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RA2,	   0, 8, TABLE64_TEST_HI,
-						0x83FFFFFF, 0xFFFFFFFF },
-	{ E1000_MTA,	   0, 128, TABLE32_TEST,
-						0xFFFFFFFF, 0xFFFFFFFF },
-	{ 0, 0, 0, 0 }
-};
-
-/* 82576 reg test */
-static struct igb_reg_test reg_test_82576[] = {
-	{ E1000_FCAL,	   0x100, 1,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_FCAH,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
-	{ E1000_FCT,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
-	{ E1000_VET,	   0x100, 1,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RDBAL(0),  0x100, 4, PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
-	{ E1000_RDBAH(0),  0x100, 4, PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RDLEN(0),  0x100, 4, PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
-	{ E1000_RDBAL(4),  0x40, 12, PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
-	{ E1000_RDBAH(4),  0x40, 12, PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RDLEN(4),  0x40, 12, PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
-	/* Enable all RX queues before testing. */
-	{ E1000_RXDCTL(0), 0x100, 4,  WRITE_NO_TEST, 0, E1000_RXDCTL_QUEUE_ENABLE },
-	{ E1000_RXDCTL(4), 0x40, 12,  WRITE_NO_TEST, 0, E1000_RXDCTL_QUEUE_ENABLE },
-	/* RDH is read-only for 82576, only test RDT. */
-	{ E1000_RDT(0),	   0x100, 4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
-	{ E1000_RDT(4),	   0x40, 12,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
-	{ E1000_RXDCTL(0), 0x100, 4,  WRITE_NO_TEST, 0, 0 },
-	{ E1000_RXDCTL(4), 0x40, 12,  WRITE_NO_TEST, 0, 0 },
-	{ E1000_FCRTH,	   0x100, 1,  PATTERN_TEST, 0x0000FFF0, 0x0000FFF0 },
-	{ E1000_FCTTV,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
-	{ E1000_TIPG,	   0x100, 1,  PATTERN_TEST, 0x3FFFFFFF, 0x3FFFFFFF },
-	{ E1000_TDBAL(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
-	{ E1000_TDBAH(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_TDLEN(0),  0x100, 4,  PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
-	{ E1000_TDBAL(4),  0x40, 12,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
-	{ E1000_TDBAH(4),  0x40, 12,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_TDLEN(4),  0x40, 12,  PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
-	{ E1000_RCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
-	{ E1000_RCTL, 	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0x003FFFFB },
-	{ E1000_RCTL, 	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0xFFFFFFFF },
-	{ E1000_TCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
-	{ E1000_RA,	   0, 16, TABLE64_TEST_LO, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RA,	   0, 16, TABLE64_TEST_HI, 0x83FFFFFF, 0xFFFFFFFF },
-	{ E1000_RA2,	   0, 8, TABLE64_TEST_LO, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RA2,	   0, 8, TABLE64_TEST_HI, 0x83FFFFFF, 0xFFFFFFFF },
-	{ E1000_MTA,	   0, 128,TABLE32_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ 0, 0, 0, 0 }
-};
-
-/* 82575 register test */
-static struct igb_reg_test reg_test_82575[] = {
-	{ E1000_FCAL,      0x100, 1, PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_FCAH,      0x100, 1, PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
-	{ E1000_FCT,       0x100, 1, PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
-	{ E1000_VET,       0x100, 1, PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RDBAL(0),  0x100, 4, PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
-	{ E1000_RDBAH(0),  0x100, 4, PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RDLEN(0),  0x100, 4, PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
-	/* Enable all four RX queues before testing. */
-	{ E1000_RXDCTL(0), 0x100, 4, WRITE_NO_TEST, 0, E1000_RXDCTL_QUEUE_ENABLE },
-	/* RDH is read-only for 82575, only test RDT. */
-	{ E1000_RDT(0),    0x100, 4, PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
-	{ E1000_RXDCTL(0), 0x100, 4, WRITE_NO_TEST, 0, 0 },
-	{ E1000_FCRTH,     0x100, 1, PATTERN_TEST, 0x0000FFF0, 0x0000FFF0 },
-	{ E1000_FCTTV,     0x100, 1, PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
-	{ E1000_TIPG,      0x100, 1, PATTERN_TEST, 0x3FFFFFFF, 0x3FFFFFFF },
-	{ E1000_TDBAL(0),  0x100, 4, PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
-	{ E1000_TDBAH(0),  0x100, 4, PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_TDLEN(0),  0x100, 4, PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
-	{ E1000_RCTL,      0x100, 1, SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
-	{ E1000_RCTL,      0x100, 1, SET_READ_TEST, 0x04CFB3FE, 0x003FFFFB },
-	{ E1000_RCTL,      0x100, 1, SET_READ_TEST, 0x04CFB3FE, 0xFFFFFFFF },
-	{ E1000_TCTL,      0x100, 1, SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
-	{ E1000_TXCW,      0x100, 1, PATTERN_TEST, 0xC000FFFF, 0x0000FFFF },
-	{ E1000_RA,        0, 16, TABLE64_TEST_LO, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ E1000_RA,        0, 16, TABLE64_TEST_HI, 0x800FFFFF, 0xFFFFFFFF },
-	{ E1000_MTA,       0, 128, TABLE32_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ 0, 0, 0, 0 }
-};
-
 static bool reg_pattern_test(struct igb_adapter *adapter, u64 *data,
 			     int reg, u32 mask, u32 write)
 {
@@ -1021,13 +976,13 @@ static bool reg_pattern_test(struct igb_adapter *adapter, u64 *data,
 	static const u32 _test[] =
 		{0x5A5A5A5A, 0xA5A5A5A5, 0x00000000, 0xFFFFFFFF};
 	for (pat = 0; pat < ARRAY_SIZE(_test); pat++) {
-		wr32(reg, (_test[pat] & write));
-		val = rd32(reg) & mask;
+		E1000_WRITE_REG(hw, reg, (_test[pat] & write));
+		val = E1000_READ_REG(hw, reg) & mask;
 		if (val != (_test[pat] & write & mask)) {
-			dev_err(&adapter->pdev->dev, "pattern test reg %04X "
+			dev_err(pci_dev_to_dev(adapter->pdev), "pattern test reg %04X "
 				"failed: got 0x%08X expected 0x%08X\n",
-				reg, val, (_test[pat] & write & mask));
-			*data = reg;
+			        E1000_REGISTER(hw, reg), val, (_test[pat] & write & mask));
+			*data = E1000_REGISTER(hw, reg);
 			return 1;
 		}
 	}
@@ -1040,13 +995,13 @@ static bool reg_set_and_check(struct igb_adapter *adapter, u64 *data,
 {
 	struct e1000_hw *hw = &adapter->hw;
 	u32 val;
-	wr32(reg, write & mask);
-	val = rd32(reg);
+	E1000_WRITE_REG(hw, reg, write & mask);
+	val = E1000_READ_REG(hw, reg);
 	if ((write & mask) != (val & mask)) {
-		dev_err(&adapter->pdev->dev, "set/check reg %04X test failed:"
+		dev_err(pci_dev_to_dev(adapter->pdev), "set/check reg %04X test failed:"
 			" got 0x%08X expected 0x%08X\n", reg,
 			(val & mask), (write & mask));
-		*data = reg;
+		*data = E1000_REGISTER(hw, reg);
 		return 1;
 	}
 
@@ -1074,9 +1029,15 @@ static int igb_reg_test(struct igb_adapter *adapter, u64 *data)
 
 	switch (adapter->hw.mac.type) {
 	case e1000_i350:
+	case e1000_i354:
 		test = reg_test_i350;
 		toggle = 0x7FEFF3FF;
 		break;
+	case e1000_i210:
+	case e1000_i211:
+		test = reg_test_i210;
+		toggle = 0x7FEFF3FF;
+		break;
 	case e1000_82580:
 		test = reg_test_82580;
 		toggle = 0x7FEFF3FF;
@@ -1096,18 +1057,18 @@ static int igb_reg_test(struct igb_adapter *adapter, u64 *data)
 	 * tests.  Some bits are read-only, some toggle, and some
 	 * are writable on newer MACs.
 	 */
-	before = rd32(E1000_STATUS);
-	value = (rd32(E1000_STATUS) & toggle);
-	wr32(E1000_STATUS, toggle);
-	after = rd32(E1000_STATUS) & toggle;
+	before = E1000_READ_REG(hw, E1000_STATUS);
+	value = (E1000_READ_REG(hw, E1000_STATUS) & toggle);
+	E1000_WRITE_REG(hw, E1000_STATUS, toggle);
+	after = E1000_READ_REG(hw, E1000_STATUS) & toggle;
 	if (value != after) {
-		dev_err(&adapter->pdev->dev, "failed STATUS register test "
+		dev_err(pci_dev_to_dev(adapter->pdev), "failed STATUS register test "
 			"got: 0x%08X expected: 0x%08X\n", after, value);
 		*data = 1;
 		return 1;
 	}
 	/* restore previous status */
-	wr32(E1000_STATUS, before);
+	E1000_WRITE_REG(hw, E1000_STATUS, before);
 
 	/* Perform the remainder of the register test, looping through
 	 * the test table until we either fail or reach the null entry.
@@ -1129,7 +1090,7 @@ static int igb_reg_test(struct igb_adapter *adapter, u64 *data)
 				break;
 			case WRITE_NO_TEST:
 				writel(test->write,
-				    (adapter->hw.hw_addr + test->reg)
+				       (adapter->hw.hw_addr + test->reg)
 					+ (i * test->reg_offset));
 				break;
 			case TABLE32_TEST:
@@ -1158,22 +1119,10 @@ static int igb_reg_test(struct igb_adapter *adapter, u64 *data)
 
 static int igb_eeprom_test(struct igb_adapter *adapter, u64 *data)
 {
-	u16 temp;
-	u16 checksum = 0;
-	u16 i;
-
 	*data = 0;
-	/* Read and add up the contents of the EEPROM */
-	for (i = 0; i < (NVM_CHECKSUM_REG + 1); i++) {
-		if ((adapter->hw.nvm.ops.read(&adapter->hw, i, 1, &temp)) < 0) {
-			*data = 1;
-			break;
-		}
-		checksum += temp;
-	}
 
-	/* If Checksum is not Correct return error else test passed */
-	if ((checksum != (u16) NVM_SUM) && !(*data))
+	/* Validate NVM checksum */
+	if (e1000_validate_nvm_checksum(&adapter->hw) < 0)
 		*data = 2;
 
 	return *data;
@@ -1184,7 +1133,7 @@ static irqreturn_t igb_test_intr(int irq, void *data)
 	struct igb_adapter *adapter = (struct igb_adapter *) data;
 	struct e1000_hw *hw = &adapter->hw;
 
-	adapter->test_icr |= rd32(E1000_ICR);
+	adapter->test_icr |= E1000_READ_REG(hw, E1000_ICR);
 
 	return IRQ_HANDLED;
 }
@@ -1193,7 +1142,7 @@ static int igb_intr_test(struct igb_adapter *adapter, u64 *data)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	struct net_device *netdev = adapter->netdev;
-	u32 mask, ics_mask, i = 0, shared_int = true;
+	u32 mask, ics_mask, i = 0, shared_int = TRUE;
 	u32 irq = adapter->pdev->irq;
 
 	*data = 0;
@@ -1201,12 +1150,12 @@ static int igb_intr_test(struct igb_adapter *adapter, u64 *data)
 	/* Hook up test interrupt handler just for this test */
 	if (adapter->msix_entries) {
 		if (request_irq(adapter->msix_entries[0].vector,
-		                igb_test_intr, 0, netdev->name, adapter)) {
+		                &igb_test_intr, 0, netdev->name, adapter)) {
 			*data = 1;
 			return -1;
 		}
 	} else if (adapter->flags & IGB_FLAG_HAS_MSI) {
-		shared_int = false;
+		shared_int = FALSE;
 		if (request_irq(irq,
 		                igb_test_intr, 0, netdev->name, adapter)) {
 			*data = 1;
@@ -1214,19 +1163,19 @@ static int igb_intr_test(struct igb_adapter *adapter, u64 *data)
 		}
 	} else if (!request_irq(irq, igb_test_intr, IRQF_PROBE_SHARED,
 				netdev->name, adapter)) {
-		shared_int = false;
-	} else if (request_irq(irq, igb_test_intr, IRQF_SHARED,
+		shared_int = FALSE;
+	} else if (request_irq(irq, &igb_test_intr, IRQF_SHARED,
 		 netdev->name, adapter)) {
 		*data = 1;
 		return -1;
 	}
-	dev_info(&adapter->pdev->dev, "testing %s interrupt\n",
-		(shared_int ? "shared" : "unshared"));
+	dev_info(pci_dev_to_dev(adapter->pdev), "testing %s interrupt\n",
+		 (shared_int ? "shared" : "unshared"));
 
 	/* Disable all the interrupts */
-	wr32(E1000_IMC, ~0);
-	wrfl();
-	msleep(10);
+	E1000_WRITE_REG(hw, E1000_IMC, ~0);
+	E1000_WRITE_FLUSH(hw);
+	usleep_range(10000, 20000);
 
 	/* Define all writable bits for ICS */
 	switch (hw->mac.type) {
@@ -1240,8 +1189,13 @@ static int igb_intr_test(struct igb_adapter *adapter, u64 *data)
 		ics_mask = 0x77DCFED5;
 		break;
 	case e1000_i350:
+	case e1000_i354:
 		ics_mask = 0x77DCFED5;
 		break;
+	case e1000_i210:
+	case e1000_i211:
+		ics_mask = 0x774CFED5;
+		break;
 	default:
 		ics_mask = 0x7FFFFFFF;
 		break;
@@ -1265,12 +1219,12 @@ static int igb_intr_test(struct igb_adapter *adapter, u64 *data)
 			adapter->test_icr = 0;
 
 			/* Flush any pending interrupts */
-			wr32(E1000_ICR, ~0);
+			E1000_WRITE_REG(hw, E1000_ICR, ~0);
 
-			wr32(E1000_IMC, mask);
-			wr32(E1000_ICS, mask);
-			wrfl();
-			msleep(10);
+			E1000_WRITE_REG(hw, E1000_IMC, mask);
+			E1000_WRITE_REG(hw, E1000_ICS, mask);
+			E1000_WRITE_FLUSH(hw);
+			usleep_range(10000, 20000);
 
 			if (adapter->test_icr & mask) {
 				*data = 3;
@@ -1287,12 +1241,12 @@ static int igb_intr_test(struct igb_adapter *adapter, u64 *data)
 		adapter->test_icr = 0;
 
 		/* Flush any pending interrupts */
-		wr32(E1000_ICR, ~0);
+		E1000_WRITE_REG(hw, E1000_ICR, ~0);
 
-		wr32(E1000_IMS, mask);
-		wr32(E1000_ICS, mask);
-		wrfl();
-		msleep(10);
+		E1000_WRITE_REG(hw, E1000_IMS, mask);
+		E1000_WRITE_REG(hw, E1000_ICS, mask);
+		E1000_WRITE_FLUSH(hw);
+		usleep_range(10000, 20000);
 
 		if (!(adapter->test_icr & mask)) {
 			*data = 4;
@@ -1309,12 +1263,12 @@ static int igb_intr_test(struct igb_adapter *adapter, u64 *data)
 			adapter->test_icr = 0;
 
 			/* Flush any pending interrupts */
-			wr32(E1000_ICR, ~0);
+			E1000_WRITE_REG(hw, E1000_ICR, ~0);
 
-			wr32(E1000_IMC, ~mask);
-			wr32(E1000_ICS, ~mask);
-			wrfl();
-			msleep(10);
+			E1000_WRITE_REG(hw, E1000_IMC, ~mask);
+			E1000_WRITE_REG(hw, E1000_ICS, ~mask);
+			E1000_WRITE_FLUSH(hw);
+			usleep_range(10000, 20000);
 
 			if (adapter->test_icr & mask) {
 				*data = 5;
@@ -1324,9 +1278,9 @@ static int igb_intr_test(struct igb_adapter *adapter, u64 *data)
 	}
 
 	/* Disable all the interrupts */
-	wr32(E1000_IMC, ~0);
-	wrfl();
-	msleep(10);
+	E1000_WRITE_REG(hw, E1000_IMC, ~0);
+	E1000_WRITE_FLUSH(hw);
+	usleep_range(10000, 20000);
 
 	/* Unhook test interrupt handler */
 	if (adapter->msix_entries)
@@ -1352,7 +1306,7 @@ static int igb_setup_desc_rings(struct igb_adapter *adapter)
 
 	/* Setup Tx descriptor ring and Tx buffers */
 	tx_ring->count = IGB_DEFAULT_TXD;
-	tx_ring->dev = &adapter->pdev->dev;
+	tx_ring->dev = pci_dev_to_dev(adapter->pdev);
 	tx_ring->netdev = adapter->netdev;
 	tx_ring->reg_idx = adapter->vfs_allocated_count;
 
@@ -1366,24 +1320,26 @@ static int igb_setup_desc_rings(struct igb_adapter *adapter)
 
 	/* Setup Rx descriptor ring and Rx buffers */
 	rx_ring->count = IGB_DEFAULT_RXD;
-	rx_ring->dev = &adapter->pdev->dev;
+	rx_ring->dev = pci_dev_to_dev(adapter->pdev);
 	rx_ring->netdev = adapter->netdev;
-	rx_ring->rx_buffer_len = IGB_RXBUFFER_2048;
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+	rx_ring->rx_buffer_len = IGB_RX_HDR_LEN;
+#endif
 	rx_ring->reg_idx = adapter->vfs_allocated_count;
 
 	if (igb_setup_rx_resources(rx_ring)) {
-		ret_val = 3;
+		ret_val = 2;
 		goto err_nomem;
 	}
 
 	/* set the default queue to queue 0 of PF */
-	wr32(E1000_MRQC, adapter->vfs_allocated_count << 3);
+	E1000_WRITE_REG(hw, E1000_MRQC, adapter->vfs_allocated_count << 3);
 
 	/* enable receive ring */
 	igb_setup_rctl(adapter);
 	igb_configure_rx_ring(adapter, rx_ring);
 
-	igb_alloc_rx_buffers_adv(rx_ring, igb_desc_unused(rx_ring));
+	igb_alloc_rx_buffers(rx_ring, igb_desc_unused(rx_ring));
 
 	return 0;
 
@@ -1397,38 +1353,44 @@ static void igb_phy_disable_receiver(struct igb_adapter *adapter)
 	struct e1000_hw *hw = &adapter->hw;
 
 	/* Write out to PHY registers 29 and 30 to disable the Receiver. */
-	igb_write_phy_reg(hw, 29, 0x001F);
-	igb_write_phy_reg(hw, 30, 0x8FFC);
-	igb_write_phy_reg(hw, 29, 0x001A);
-	igb_write_phy_reg(hw, 30, 0x8FF0);
+	e1000_write_phy_reg(hw, 29, 0x001F);
+	e1000_write_phy_reg(hw, 30, 0x8FFC);
+	e1000_write_phy_reg(hw, 29, 0x001A);
+	e1000_write_phy_reg(hw, 30, 0x8FF0);
 }
 
+
 static int igb_integrated_phy_loopback(struct igb_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	u32 ctrl_reg = 0;
 
-	hw->mac.autoneg = false;
+	hw->mac.autoneg = FALSE;
 
 	if (hw->phy.type == e1000_phy_m88) {
-		/* Auto-MDI/MDIX Off */
-		igb_write_phy_reg(hw, M88E1000_PHY_SPEC_CTRL, 0x0808);
-		/* reset to update Auto-MDI/MDIX */
-		igb_write_phy_reg(hw, PHY_CONTROL, 0x9140);
-		/* autoneg off */
-		igb_write_phy_reg(hw, PHY_CONTROL, 0x8140);
-	} else if (hw->phy.type == e1000_phy_82580) {
+		if (hw->phy.id != I210_I_PHY_ID) {
+			/* Auto-MDI/MDIX Off */
+			e1000_write_phy_reg(hw, M88E1000_PHY_SPEC_CTRL, 0x0808);
+			/* reset to update Auto-MDI/MDIX */
+			e1000_write_phy_reg(hw, PHY_CONTROL, 0x9140);
+			/* autoneg off */
+			e1000_write_phy_reg(hw, PHY_CONTROL, 0x8140);
+		} else {
+			/* force 1000, set loopback  */
+			e1000_write_phy_reg(hw, I347AT4_PAGE_SELECT, 0);
+			e1000_write_phy_reg(hw, PHY_CONTROL, 0x4140);
+		}
+	} else {
 		/* enable MII loopback */
-		igb_write_phy_reg(hw, I82580_PHY_LBK_CTRL, 0x8041);
+		if (hw->phy.type == e1000_phy_82580)
+			e1000_write_phy_reg(hw, I82577_PHY_LBK_CTRL, 0x8041);
 	}
 
-	ctrl_reg = rd32(E1000_CTRL);
-
-	/* force 1000, set loopback */
-	igb_write_phy_reg(hw, PHY_CONTROL, 0x4140);
+	/* force 1000, set loopback  */
+	e1000_write_phy_reg(hw, PHY_CONTROL, 0x4140);
 
 	/* Now set up the MAC to the same speed/duplex as the PHY. */
-	ctrl_reg = rd32(E1000_CTRL);
+	ctrl_reg = E1000_READ_REG(hw, E1000_CTRL);
 	ctrl_reg &= ~E1000_CTRL_SPD_SEL; /* Clear the speed sel bits */
 	ctrl_reg |= (E1000_CTRL_FRCSPD | /* Set the Force Speed Bit */
 		     E1000_CTRL_FRCDPX | /* Set the Force Duplex Bit */
@@ -1439,7 +1401,7 @@ static int igb_integrated_phy_loopback(struct igb_adapter *adapter)
 	if (hw->phy.type == e1000_phy_m88)
 		ctrl_reg |= E1000_CTRL_ILOS; /* Invert Loss of Signal */
 
-	wr32(E1000_CTRL, ctrl_reg);
+	E1000_WRITE_REG(hw, E1000_CTRL, ctrl_reg);
 
 	/* Disable the receiver on the PHY so when a cable is plugged in, the
 	 * PHY does not begin to autoneg when a cable is reconnected to the NIC.
@@ -1447,8 +1409,7 @@ static int igb_integrated_phy_loopback(struct igb_adapter *adapter)
 	if (hw->phy.type == e1000_phy_m88)
 		igb_phy_disable_receiver(adapter);
 
-	udelay(500);
-
+	mdelay(500);
 	return 0;
 }
 
@@ -1462,54 +1423,64 @@ static int igb_setup_loopback_test(struct igb_adapter *adapter)
 	struct e1000_hw *hw = &adapter->hw;
 	u32 reg;
 
-	reg = rd32(E1000_CTRL_EXT);
+	reg = E1000_READ_REG(hw, E1000_CTRL_EXT);
 
 	/* use CTRL_EXT to identify link type as SGMII can appear as copper */
 	if (reg & E1000_CTRL_EXT_LINK_MODE_MASK) {
 		if ((hw->device_id == E1000_DEV_ID_DH89XXCC_SGMII) ||
-		(hw->device_id == E1000_DEV_ID_DH89XXCC_SERDES) ||
-		(hw->device_id == E1000_DEV_ID_DH89XXCC_BACKPLANE) ||
-		(hw->device_id == E1000_DEV_ID_DH89XXCC_SFP)) {
+		    (hw->device_id == E1000_DEV_ID_DH89XXCC_SERDES) ||
+		    (hw->device_id == E1000_DEV_ID_DH89XXCC_BACKPLANE) ||
+		    (hw->device_id == E1000_DEV_ID_DH89XXCC_SFP) ||
+		    (hw->device_id == E1000_DEV_ID_I354_SGMII)) {
 
 			/* Enable DH89xxCC MPHY for near end loopback */
-			reg = rd32(E1000_MPHY_ADDR_CTL);
+			reg = E1000_READ_REG(hw, E1000_MPHY_ADDR_CTL);
 			reg = (reg & E1000_MPHY_ADDR_CTL_OFFSET_MASK) |
-			E1000_MPHY_PCS_CLK_REG_OFFSET;
-			wr32(E1000_MPHY_ADDR_CTL, reg);
+			       E1000_MPHY_PCS_CLK_REG_OFFSET;
+			E1000_WRITE_REG(hw, E1000_MPHY_ADDR_CTL, reg);
 
-			reg = rd32(E1000_MPHY_DATA);
+			reg = E1000_READ_REG(hw, E1000_MPHY_DATA);
 			reg |= E1000_MPHY_PCS_CLK_REG_DIGINELBEN;
-			wr32(E1000_MPHY_DATA, reg);
+			E1000_WRITE_REG(hw, E1000_MPHY_DATA, reg);
 		}
 
-		reg = rd32(E1000_RCTL);
+		reg = E1000_READ_REG(hw, E1000_RCTL);
 		reg |= E1000_RCTL_LBM_TCVR;
-		wr32(E1000_RCTL, reg);
+		E1000_WRITE_REG(hw, E1000_RCTL, reg);
 
-		wr32(E1000_SCTL, E1000_ENABLE_SERDES_LOOPBACK);
+		E1000_WRITE_REG(hw, E1000_SCTL, E1000_ENABLE_SERDES_LOOPBACK);
 
-		reg = rd32(E1000_CTRL);
+		reg = E1000_READ_REG(hw, E1000_CTRL);
 		reg &= ~(E1000_CTRL_RFCE |
 			 E1000_CTRL_TFCE |
 			 E1000_CTRL_LRST);
 		reg |= E1000_CTRL_SLU |
 		       E1000_CTRL_FD;
-		wr32(E1000_CTRL, reg);
+		E1000_WRITE_REG(hw, E1000_CTRL, reg);
 
 		/* Unset switch control to serdes energy detect */
-		reg = rd32(E1000_CONNSW);
+		reg = E1000_READ_REG(hw, E1000_CONNSW);
 		reg &= ~E1000_CONNSW_ENRGSRC;
-		wr32(E1000_CONNSW, reg);
+		E1000_WRITE_REG(hw, E1000_CONNSW, reg);
+
+		/* Unset sigdetect for SERDES loopback on
+		 * 82580 and newer devices
+		 */
+		if (hw->mac.type >= e1000_82580) {
+			reg = E1000_READ_REG(hw, E1000_PCS_CFG0);
+			reg |= E1000_PCS_CFG_IGN_SD;
+			E1000_WRITE_REG(hw, E1000_PCS_CFG0, reg);
+		}
 
 		/* Set PCS register for forced speed */
-		reg = rd32(E1000_PCS_LCTL);
+		reg = E1000_READ_REG(hw, E1000_PCS_LCTL);
 		reg &= ~E1000_PCS_LCTL_AN_ENABLE;     /* Disable Autoneg*/
 		reg |= E1000_PCS_LCTL_FLV_LINK_UP |   /* Force link up */
 		       E1000_PCS_LCTL_FSV_1000 |      /* Force 1000    */
 		       E1000_PCS_LCTL_FDV_FULL |      /* SerDes Full duplex */
 		       E1000_PCS_LCTL_FSD |           /* Force Speed */
 		       E1000_PCS_LCTL_FORCE_LINK;     /* Force Link */
-		wr32(E1000_PCS_LCTL, reg);
+		E1000_WRITE_REG(hw, E1000_PCS_LCTL, reg);
 
 		return 0;
 	}
@@ -1524,35 +1495,37 @@ static void igb_loopback_cleanup(struct igb_adapter *adapter)
 	u16 phy_reg;
 
 	if ((hw->device_id == E1000_DEV_ID_DH89XXCC_SGMII) ||
-	(hw->device_id == E1000_DEV_ID_DH89XXCC_SERDES) ||
-	(hw->device_id == E1000_DEV_ID_DH89XXCC_BACKPLANE) ||
-	(hw->device_id == E1000_DEV_ID_DH89XXCC_SFP)) {
+	    (hw->device_id == E1000_DEV_ID_DH89XXCC_SERDES) ||
+	    (hw->device_id == E1000_DEV_ID_DH89XXCC_BACKPLANE) ||
+	    (hw->device_id == E1000_DEV_ID_DH89XXCC_SFP) ||
+	    (hw->device_id == E1000_DEV_ID_I354_SGMII)) {
 		u32 reg;
 
 		/* Disable near end loopback on DH89xxCC */
-		reg = rd32(E1000_MPHY_ADDR_CTL);
-		reg = (reg & E1000_MPHY_ADDR_CTL_OFFSET_MASK) |
-		E1000_MPHY_PCS_CLK_REG_OFFSET;
-		wr32(E1000_MPHY_ADDR_CTL, reg);
+		reg = E1000_READ_REG(hw, E1000_MPHY_ADDR_CTL);
+		reg = (reg & E1000_MPHY_ADDR_CTL_OFFSET_MASK ) |
+		       E1000_MPHY_PCS_CLK_REG_OFFSET;
+		E1000_WRITE_REG(hw, E1000_MPHY_ADDR_CTL, reg);
 
-		reg = rd32(E1000_MPHY_DATA);
+		reg = E1000_READ_REG(hw, E1000_MPHY_DATA);
 		reg &= ~E1000_MPHY_PCS_CLK_REG_DIGINELBEN;
-		wr32(E1000_MPHY_DATA, reg);
+		E1000_WRITE_REG(hw, E1000_MPHY_DATA, reg);
 	}
 
-	rctl = rd32(E1000_RCTL);
+	rctl = E1000_READ_REG(hw, E1000_RCTL);
 	rctl &= ~(E1000_RCTL_LBM_TCVR | E1000_RCTL_LBM_MAC);
-	wr32(E1000_RCTL, rctl);
+	E1000_WRITE_REG(hw, E1000_RCTL, rctl);
 
-	hw->mac.autoneg = true;
-	igb_read_phy_reg(hw, PHY_CONTROL, &phy_reg);
+	hw->mac.autoneg = TRUE;
+	e1000_read_phy_reg(hw, PHY_CONTROL, &phy_reg);
 	if (phy_reg & MII_CR_LOOPBACK) {
 		phy_reg &= ~MII_CR_LOOPBACK;
-		igb_write_phy_reg(hw, PHY_CONTROL, phy_reg);
-		igb_phy_sw_reset(hw);
+		if (hw->phy.type == I210_I_PHY_ID)
+			e1000_write_phy_reg(hw, I347AT4_PAGE_SELECT, 0);
+		e1000_write_phy_reg(hw, PHY_CONTROL, phy_reg);
+		e1000_phy_commit(hw);
 	}
 }
-
 static void igb_create_lbtest_frame(struct sk_buff *skb,
 				    unsigned int frame_size)
 {
@@ -1563,51 +1536,77 @@ static void igb_create_lbtest_frame(struct sk_buff *skb,
 	memset(&skb->data[frame_size + 12], 0xAF, 1);
 }
 
-static int igb_check_lbtest_frame(struct sk_buff *skb, unsigned int frame_size)
+static int igb_check_lbtest_frame(struct igb_rx_buffer *rx_buffer,
+				  unsigned int frame_size)
 {
-	frame_size /= 2;
-	if (*(skb->data + 3) == 0xFF) {
-		if ((*(skb->data + frame_size + 10) == 0xBE) &&
-		   (*(skb->data + frame_size + 12) == 0xAF)) {
-			return 0;
-		}
-	}
-	return 13;
+	unsigned char *data;
+	bool match = true;
+
+	frame_size >>= 1;
+
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+	data = rx_buffer->skb->data;
+#else
+	data = kmap(rx_buffer->page);
+#endif
+
+	if (data[3] != 0xFF ||
+	    data[frame_size + 10] != 0xBE ||
+	    data[frame_size + 12] != 0xAF)
+		match = false;
+
+#ifndef CONFIG_IGB_DISABLE_PACKET_SPLIT
+	kunmap(rx_buffer->page);
+
+#endif
+	return match;
 }
 
-static int igb_clean_test_rings(struct igb_ring *rx_ring,
-                                struct igb_ring *tx_ring,
-                                unsigned int size)
+static u16 igb_clean_test_rings(struct igb_ring *rx_ring,
+				struct igb_ring *tx_ring,
+				unsigned int size)
 {
 	union e1000_adv_rx_desc *rx_desc;
-	struct igb_buffer *buffer_info;
-	int rx_ntc, tx_ntc, count = 0;
-	u32 staterr;
+	struct igb_rx_buffer *rx_buffer_info;
+	struct igb_tx_buffer *tx_buffer_info;
+	u16 rx_ntc, tx_ntc, count = 0;
 
 	/* initialize next to clean and descriptor values */
 	rx_ntc = rx_ring->next_to_clean;
 	tx_ntc = tx_ring->next_to_clean;
-	rx_desc = E1000_RX_DESC_ADV(*rx_ring, rx_ntc);
-	staterr = le32_to_cpu(rx_desc->wb.upper.status_error);
+	rx_desc = IGB_RX_DESC(rx_ring, rx_ntc);
 
-	while (staterr & E1000_RXD_STAT_DD) {
+	while (igb_test_staterr(rx_desc, E1000_RXD_STAT_DD)) {
 		/* check rx buffer */
-		buffer_info = &rx_ring->buffer_info[rx_ntc];
-
-		/* unmap rx buffer, will be remapped by alloc_rx_buffers */
-		dma_unmap_single(rx_ring->dev,
-		                 buffer_info->dma,
-				 rx_ring->rx_buffer_len,
-				 DMA_FROM_DEVICE);
-		buffer_info->dma = 0;
+		rx_buffer_info = &rx_ring->rx_buffer_info[rx_ntc];
+
+		/* sync Rx buffer for CPU read */
+		dma_sync_single_for_cpu(rx_ring->dev,
+					rx_buffer_info->dma,
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+					IGB_RX_HDR_LEN,
+#else
+					IGB_RX_BUFSZ,
+#endif
+					DMA_FROM_DEVICE);
 
 		/* verify contents of skb */
-		if (!igb_check_lbtest_frame(buffer_info->skb, size))
+		if (igb_check_lbtest_frame(rx_buffer_info, size))
 			count++;
 
+		/* sync Rx buffer for device write */
+		dma_sync_single_for_device(rx_ring->dev,
+					   rx_buffer_info->dma,
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+					   IGB_RX_HDR_LEN,
+#else
+					   IGB_RX_BUFSZ,
+#endif
+					   DMA_FROM_DEVICE);
+
 		/* unmap buffer on tx side */
-		buffer_info = &tx_ring->buffer_info[tx_ntc];
-		igb_unmap_and_free_tx_resource(tx_ring, buffer_info);
+		tx_buffer_info = &tx_ring->tx_buffer_info[tx_ntc];
+		igb_unmap_and_free_tx_resource(tx_ring, tx_buffer_info);
 
 		/* increment rx/tx next to clean counters */
 		rx_ntc++;
@@ -1618,12 +1617,11 @@ static int igb_clean_test_rings(struct igb_ring *rx_ring,
 			tx_ntc = 0;
 
 		/* fetch next descriptor */
-		rx_desc = E1000_RX_DESC_ADV(*rx_ring, rx_ntc);
-		staterr = le32_to_cpu(rx_desc->wb.upper.status_error);
+		rx_desc = IGB_RX_DESC(rx_ring, rx_ntc);
 	}
 
 	/* re-map buffers to ring, store next to clean values */
-	igb_alloc_rx_buffers_adv(rx_ring, count);
+	igb_alloc_rx_buffers(rx_ring, count);
 	rx_ring->next_to_clean = rx_ntc;
 	tx_ring->next_to_clean = tx_ntc;
 
@@ -1634,8 +1632,9 @@ static int igb_run_loopback_test(struct igb_adapter *adapter)
 {
 	struct igb_ring *tx_ring = &adapter->test_tx_ring;
 	struct igb_ring *rx_ring = &adapter->test_rx_ring;
-	int i, j, lc, good_cnt, ret_val = 0;
-	unsigned int size = 1024;
+	u16 i, j, lc, good_cnt;
+	int ret_val = 0;
+	unsigned int size = IGB_RX_HDR_LEN;
 	netdev_tx_t tx_ret_val;
 	struct sk_buff *skb;
 
@@ -1666,7 +1665,7 @@ static int igb_run_loopback_test(struct igb_adapter *adapter)
 		/* place 64 packets on the transmit queue*/
 		for (i = 0; i < 64; i++) {
 			skb_get(skb);
-			tx_ret_val = igb_xmit_frame_ring_adv(skb, tx_ring);
+			tx_ret_val = igb_xmit_frame_ring(skb, tx_ring);
 			if (tx_ret_val == NETDEV_TX_OK)
 				good_cnt++;
 		}
@@ -1696,13 +1695,14 @@ static int igb_loopback_test(struct igb_adapter *adapter, u64 *data)
 {
 	/* PHY loopback cannot be performed if SoL/IDER
 	 * sessions are active */
-	if (igb_check_reset_block(&adapter->hw)) {
-		dev_err(&adapter->pdev->dev,
+	if (e1000_check_reset_block(&adapter->hw)) {
+		dev_err(pci_dev_to_dev(adapter->pdev),
 			"Cannot do PHY loopback test "
 			"when SoL/IDER is active.\n");
 		*data = 0;
 		goto out;
 	}
+
 	*data = igb_setup_desc_rings(adapter);
 	if (*data)
 		goto out;
@@ -1710,6 +1710,7 @@ static int igb_loopback_test(struct igb_adapter *adapter, u64 *data)
 	if (*data)
 		goto err_loopback;
 	*data = igb_run_loopback_test(adapter);
+
 	igb_loopback_cleanup(adapter);
 
 err_loopback:
@@ -1720,31 +1721,40 @@ out:
 
 static int igb_link_test(struct igb_adapter *adapter, u64 *data)
 {
-	struct e1000_hw *hw = &adapter->hw;
+	u32 link;
+	int i, time;
+
 	*data = 0;
-	if (hw->phy.media_type == e1000_media_type_internal_serdes) {
+	time = 0;
+	if (adapter->hw.phy.media_type == e1000_media_type_internal_serdes) {
 		int i = 0;
-		hw->mac.serdes_has_link = false;
+		adapter->hw.mac.serdes_has_link = FALSE;
 
 		/* On some blade server designs, link establishment
 		 * could take as long as 2-3 minutes */
 		do {
-			hw->mac.ops.check_for_link(&adapter->hw);
-			if (hw->mac.serdes_has_link)
-				return *data;
+			e1000_check_for_link(&adapter->hw);
+			if (adapter->hw.mac.serdes_has_link)
+				goto out;
 			msleep(20);
 		} while (i++ < 3750);
 
 		*data = 1;
 	} else {
-		hw->mac.ops.check_for_link(&adapter->hw);
-		if (hw->mac.autoneg)
-			msleep(4000);
-
-		if (!(rd32(E1000_STATUS) & E1000_STATUS_LU))
+		for (i=0; i < IGB_MAX_LINK_TRIES; i++) {
+		link = igb_has_link(adapter);
+			if (link)
+				goto out;
+			else {
+				time++;
+				msleep(1000);
+			}
+		}
+		if (!link)
 			*data = 1;
 	}
-	return *data;
+	out:
+		return *data;
 }
 
 static void igb_diag_test(struct net_device *netdev,
@@ -1764,10 +1774,10 @@ static void igb_diag_test(struct net_device *netdev,
 		forced_speed_duplex = adapter->hw.mac.forced_speed_duplex;
 		autoneg = adapter->hw.mac.autoneg;
 
-		dev_info(&adapter->pdev->dev, "offline testing starting\n");
+		dev_info(pci_dev_to_dev(adapter->pdev), "offline testing starting\n");
 
 		/* power up link for link test */
-		igb_power_up_link(adapter);
+	 	igb_power_up_link(adapter);
 
 		/* Link test performed before hardware reset so autoneg doesn't
 		 * interfere with test result */
@@ -1792,8 +1802,10 @@ static void igb_diag_test(struct net_device *netdev,
 			eth_test->flags |= ETH_TEST_FL_FAILED;
 
 		igb_reset(adapter);
+
 		/* power up link for loopback test */
 		igb_power_up_link(adapter);
+
 		if (igb_loopback_test(adapter, &data[3]))
 			eth_test->flags |= ETH_TEST_FL_FAILED;
 
@@ -1803,15 +1815,15 @@ static void igb_diag_test(struct net_device *netdev,
 		adapter->hw.mac.autoneg = autoneg;
 
 		/* force this routine to wait until autoneg complete/timeout */
-		adapter->hw.phy.autoneg_wait_to_complete = true;
+		adapter->hw.phy.autoneg_wait_to_complete = TRUE;
 		igb_reset(adapter);
-		adapter->hw.phy.autoneg_wait_to_complete = false;
+		adapter->hw.phy.autoneg_wait_to_complete = FALSE;
 
 		clear_bit(__IGB_TESTING, &adapter->state);
 		if (if_running)
 			dev_open(netdev);
 	} else {
-		dev_info(&adapter->pdev->dev, "online testing starting\n");
+		dev_info(pci_dev_to_dev(adapter->pdev), "online testing starting\n");
 
 		/* PHY is powered down when interface is down */
 		if (if_running && igb_link_test(adapter, &data[4]))
@@ -1830,69 +1842,19 @@ static void igb_diag_test(struct net_device *netdev,
 	msleep_interruptible(4 * 1000);
 }
 
-static int igb_wol_exclusion(struct igb_adapter *adapter,
-			     struct ethtool_wolinfo *wol)
-{
-	struct e1000_hw *hw = &adapter->hw;
-	int retval = 1; /* fail by default */
-
-	switch (hw->device_id) {
-	case E1000_DEV_ID_82575GB_QUAD_COPPER:
-		/* WoL not supported */
-		wol->supported = 0;
-		break;
-	case E1000_DEV_ID_82575EB_FIBER_SERDES:
-	case E1000_DEV_ID_82576_FIBER:
-	case E1000_DEV_ID_82576_SERDES:
-		/* Wake events not supported on port B */
-		if (rd32(E1000_STATUS) & E1000_STATUS_FUNC_1) {
-			wol->supported = 0;
-			break;
-		}
-		/* return success for non excluded adapter ports */
-		retval = 0;
-		break;
-	case E1000_DEV_ID_82576_QUAD_COPPER:
-	case E1000_DEV_ID_82576_QUAD_COPPER_ET2:
-		/* quad port adapters only support WoL on port A */
-		if (!(adapter->flags & IGB_FLAG_QUAD_PORT_A)) {
-			wol->supported = 0;
-			break;
-		}
-		/* return success for non excluded adapter ports */
-		retval = 0;
-		break;
-	default:
-		/* dual port cards only support WoL on port A from now on
-		 * unless it was enabled in the eeprom for port B
-		 * so exclude FUNC_1 ports from having WoL enabled */
-		if ((rd32(E1000_STATUS) & E1000_STATUS_FUNC_MASK) &&
-		    !adapter->eeprom_wol) {
-			wol->supported = 0;
-			break;
-		}
-
-		retval = 0;
-	}
-
-	return retval;
-}
-
 static void igb_get_wol(struct net_device *netdev, struct ethtool_wolinfo *wol)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
 
-	wol->supported = WAKE_UCAST | WAKE_MCAST |
-	                 WAKE_BCAST | WAKE_MAGIC |
-	                 WAKE_PHY;
 	wol->wolopts = 0;
 
-	/* this function will set ->supported = 0 and return 1 if wol is not
-	 * supported by this hardware */
-	if (igb_wol_exclusion(adapter, wol) ||
-	    !device_can_wakeup(&adapter->pdev->dev))
+	if (!(adapter->flags & IGB_FLAG_WOL_SUPPORTED))
 		return;
 
+	wol->supported = WAKE_UCAST | WAKE_MCAST |
+	                 WAKE_BCAST | WAKE_MAGIC |
+	                 WAKE_PHY;
+	
 	/* apply any specific unsupported masks here */
 	switch (adapter->hw.device_id) {
 	default:
@@ -1918,8 +1880,7 @@ static int igb_set_wol(struct net_device *netdev, struct ethtool_wolinfo *wol)
 	if (wol->wolopts & (WAKE_ARP | WAKE_MAGICSECURE))
 		return -EOPNOTSUPP;
 
-	if (igb_wol_exclusion(adapter, wol) ||
-	    !device_can_wakeup(&adapter->pdev->dev))
+	if (!(adapter->flags & IGB_FLAG_WOL_SUPPORTED))
 		return wol->wolopts ? -EOPNOTSUPP : 0;
 
 	/* these settings will always override what we currently have */
@@ -1941,8 +1902,7 @@ static int igb_set_wol(struct net_device *netdev, struct ethtool_wolinfo *wol)
 }
 
 /* bit defines for adapter->led_status */
-#define IGB_LED_ON		0
-
+#ifdef HAVE_ETHTOOL_SET_PHYS_ID
 static int igb_set_phys_id(struct net_device *netdev,
 			   enum ethtool_phys_id_state state)
 {
@@ -1951,23 +1911,47 @@ static int igb_set_phys_id(struct net_device *netdev,
 
 	switch (state) {
 	case ETHTOOL_ID_ACTIVE:
-		igb_blink_led(hw);
+		e1000_blink_led(hw);
 		return 2;
 	case ETHTOOL_ID_ON:
-		igb_blink_led(hw);
+		e1000_led_on(hw);
 		break;
 	case ETHTOOL_ID_OFF:
-		igb_led_off(hw);
+		e1000_led_off(hw);
 		break;
 	case ETHTOOL_ID_INACTIVE:
-		igb_led_off(hw);
-		clear_bit(IGB_LED_ON, &adapter->led_status);
-		igb_cleanup_led(hw);
+		e1000_led_off(hw);
+		e1000_cleanup_led(hw);
 		break;
 	}
 
 	return 0;
 }
+#else
+static int igb_phys_id(struct net_device *netdev, u32 data)
+{
+	struct igb_adapter *adapter = netdev_priv(netdev);
+	struct e1000_hw *hw = &adapter->hw;
+	unsigned long timeout;
+
+	timeout = data * 1000;
+
+	/*
+	 *  msleep_interruptable only accepts unsigned int so we are limited
+	 * in how long a duration we can wait
+	 */
+	if (!timeout || timeout > UINT_MAX)
+		timeout = UINT_MAX;
+
+	e1000_blink_led(hw);
+	msleep_interruptible(timeout);
+
+	e1000_led_off(hw);
+	e1000_cleanup_led(hw);
+
+	return 0;
+}
+#endif /* HAVE_ETHTOOL_SET_PHYS_ID */
 
 static int igb_set_coalesce(struct net_device *netdev,
 			    struct ethtool_coalesce *ec)
@@ -1979,7 +1963,10 @@ static int igb_set_coalesce(struct net_device *netdev,
 	    ((ec->rx_coalesce_usecs > 3) &&
 	     (ec->rx_coalesce_usecs < IGB_MIN_ITR_USECS)) ||
 	    (ec->rx_coalesce_usecs == 2))
+	    {
+	    	printk("set_coalesce:invalid parameter..");
 		return -EINVAL;
+	}
 
 	if ((ec->tx_coalesce_usecs > IGB_MAX_ITR_USECS) ||
 	    ((ec->tx_coalesce_usecs > 3) &&
@@ -1990,10 +1977,12 @@ static int igb_set_coalesce(struct net_device *netdev,
 	if ((adapter->flags & IGB_FLAG_QUEUE_PAIRS) && ec->tx_coalesce_usecs)
 		return -EINVAL;
 
+	if (ec->tx_max_coalesced_frames_irq)
+		adapter->tx_work_limit = ec->tx_max_coalesced_frames_irq;
+
 	/* If ITR is disabled, disable DMAC */
 	if (ec->rx_coalesce_usecs == 0) {
-		if (adapter->flags & IGB_FLAG_DMAC)
-			adapter->flags &= ~IGB_FLAG_DMAC;
+		adapter->dmac = IGB_DMAC_DISABLE;
 	}
 
 	/* convert to rate of irq's per second */
@@ -2012,7 +2001,8 @@ static int igb_set_coalesce(struct net_device *netdev,
 
 	for (i = 0; i < adapter->num_q_vectors; i++) {
 		struct igb_q_vector *q_vector = adapter->q_vector[i];
-		if (q_vector->rx_ring)
+		q_vector->tx.work_limit = adapter->tx_work_limit;
+		if (q_vector->rx.ring)
 			q_vector->itr_val = adapter->rx_itr_setting;
 		else
 			q_vector->itr_val = adapter->tx_itr_setting;
@@ -2034,6 +2024,8 @@ static int igb_get_coalesce(struct net_device *netdev,
 	else
 		ec->rx_coalesce_usecs = adapter->rx_itr_setting >> 2;
 
+	ec->tx_max_coalesced_frames_irq = adapter->tx_work_limit;
+
 	if (!(adapter->flags & IGB_FLAG_QUEUE_PAIRS)) {
 		if (adapter->tx_itr_setting <= 3)
 			ec->tx_coalesce_usecs = adapter->tx_itr_setting;
@@ -2052,6 +2044,7 @@ static int igb_nway_reset(struct net_device *netdev)
 	return 0;
 }
 
+#ifdef HAVE_ETHTOOL_GET_SSET_COUNT
 static int igb_get_sset_count(struct net_device *netdev, int sset)
 {
 	switch (sset) {
@@ -2063,19 +2056,32 @@ static int igb_get_sset_count(struct net_device *netdev, int sset)
 		return -ENOTSUPP;
 	}
 }
+#else
+static int igb_get_stats_count(struct net_device *netdev)
+{
+	return IGB_STATS_LEN;
+}
+
+static int igb_diag_test_count(struct net_device *netdev)
+{
+	return IGB_TEST_LEN;
+}
+#endif
 
 static void igb_get_ethtool_stats(struct net_device *netdev,
 				  struct ethtool_stats *stats, u64 *data)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
-	struct rtnl_link_stats64 *net_stats = &adapter->stats64;
-	unsigned int start;
-	struct igb_ring *ring;
-	int i, j;
+#ifdef HAVE_NETDEV_STATS_IN_NETDEV
+	struct net_device_stats *net_stats = &netdev->stats;
+#else
+	struct net_device_stats *net_stats = &adapter->net_stats;
+#endif
+	u64 *queue_stat;
+	int i, j, k;
 	char *p;
 
-	spin_lock(&adapter->stats64_lock);
-	igb_update_stats(adapter, net_stats);
+	igb_update_stats(adapter);
 
 	for (i = 0; i < IGB_GLOBAL_STATS_LEN; i++) {
 		p = (char *)adapter + igb_gstrings_stats[i].stat_offset;
@@ -2088,36 +2094,15 @@ static void igb_get_ethtool_stats(struct net_device *netdev,
 			sizeof(u64)) ? *(u64 *)p : *(u32 *)p;
 	}
 	for (j = 0; j < adapter->num_tx_queues; j++) {
-		u64	restart2;
-
-		ring = adapter->tx_ring[j];
-		do {
-			start = u64_stats_fetch_begin_bh(&ring->tx_syncp);
-			data[i]   = ring->tx_stats.packets;
-			data[i+1] = ring->tx_stats.bytes;
-			data[i+2] = ring->tx_stats.restart_queue;
-		} while (u64_stats_fetch_retry_bh(&ring->tx_syncp, start));
-		do {
-			start = u64_stats_fetch_begin_bh(&ring->tx_syncp2);
-			restart2  = ring->tx_stats.restart_queue2;
-		} while (u64_stats_fetch_retry_bh(&ring->tx_syncp2, start));
-		data[i+2] += restart2;
-
-		i += IGB_TX_QUEUE_STATS_LEN;
+		queue_stat = (u64 *)&adapter->tx_ring[j]->tx_stats;
+		for (k = 0; k < IGB_TX_QUEUE_STATS_LEN; k++, i++)
+			data[i] = queue_stat[k];
 	}
 	for (j = 0; j < adapter->num_rx_queues; j++) {
-		ring = adapter->rx_ring[j];
-		do {
-			start = u64_stats_fetch_begin_bh(&ring->rx_syncp);
-			data[i]   = ring->rx_stats.packets;
-			data[i+1] = ring->rx_stats.bytes;
-			data[i+2] = ring->rx_stats.drops;
-			data[i+3] = ring->rx_stats.csum_err;
-			data[i+4] = ring->rx_stats.alloc_failed;
-		} while (u64_stats_fetch_retry_bh(&ring->rx_syncp, start));
-		i += IGB_RX_QUEUE_STATS_LEN;
+		queue_stat = (u64 *)&adapter->rx_ring[j]->rx_stats;
+		for (k = 0; k < IGB_RX_QUEUE_STATS_LEN; k++, i++)
+			data[i] = queue_stat[k];
 	}
-	spin_unlock(&adapter->stats64_lock);
 }
 
 static void igb_get_strings(struct net_device *netdev, u32 stringset, u8 *data)
@@ -2167,6 +2152,834 @@ static void igb_get_strings(struct net_device *netdev, u32 stringset, u8 *data)
 	}
 }
 
+#ifdef HAVE_ETHTOOL_GET_TS_INFO
+static int igb_get_ts_info(struct net_device *dev,
+			   struct ethtool_ts_info *info)
+{
+	struct igb_adapter *adapter = netdev_priv(dev);
+
+	switch (adapter->hw.mac.type) {
+#ifdef HAVE_PTP_1588_CLOCK
+	case e1000_82575:
+		info->so_timestamping =
+			SOF_TIMESTAMPING_TX_SOFTWARE |
+			SOF_TIMESTAMPING_RX_SOFTWARE |
+			SOF_TIMESTAMPING_SOFTWARE;
+		return 0;
+	case e1000_82576:
+	case e1000_82580:
+	case e1000_i350:
+	case e1000_i354:
+	case e1000_i210:
+	case e1000_i211:
+		info->so_timestamping =
+			SOF_TIMESTAMPING_TX_SOFTWARE |
+			SOF_TIMESTAMPING_RX_SOFTWARE |
+			SOF_TIMESTAMPING_SOFTWARE |
+			SOF_TIMESTAMPING_TX_HARDWARE |
+			SOF_TIMESTAMPING_RX_HARDWARE |
+			SOF_TIMESTAMPING_RAW_HARDWARE;
+
+		if (adapter->ptp_clock)
+			info->phc_index = ptp_clock_index(adapter->ptp_clock);
+		else
+			info->phc_index = -1;
+
+		info->tx_types =
+			(1 << HWTSTAMP_TX_OFF) |
+			(1 << HWTSTAMP_TX_ON);
+
+		info->rx_filters = 1 << HWTSTAMP_FILTER_NONE;
+
+		/* 82576 does not support timestamping all packets. */
+		if (adapter->hw.mac.type >= e1000_82580)
+			info->rx_filters |= 1 << HWTSTAMP_FILTER_ALL;
+		else
+			info->rx_filters |=
+				(1 << HWTSTAMP_FILTER_PTP_V1_L4_SYNC) |
+				(1 << HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ) |
+				(1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC) |
+				(1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC) |
+				(1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ) |
+				(1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ) |
+				(1 << HWTSTAMP_FILTER_PTP_V2_EVENT);
+
+		return 0;
+#endif /* HAVE_PTP_1588_CLOCK */
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+#endif /* HAVE_ETHTOOL_GET_TS_INFO */
+
+#ifdef CONFIG_PM_RUNTIME
+static int igb_ethtool_begin(struct net_device *netdev)
+{
+	struct igb_adapter *adapter = netdev_priv(netdev);
+
+	pm_runtime_get_sync(&adapter->pdev->dev);
+
+	return 0;
+}
+
+static void igb_ethtool_complete(struct net_device *netdev)
+{
+	struct igb_adapter *adapter = netdev_priv(netdev);
+
+	pm_runtime_put(&adapter->pdev->dev);
+}
+#endif /* CONFIG_PM_RUNTIME */
+
+#ifndef HAVE_NDO_SET_FEATURES
+static u32 igb_get_rx_csum(struct net_device *netdev)
+{
+	return !!(netdev->features & NETIF_F_RXCSUM);
+}
+
+static int igb_set_rx_csum(struct net_device *netdev, u32 data)
+{
+	const u32 feature_list = NETIF_F_RXCSUM;
+
+	if (data)
+		netdev->features |= feature_list;
+	else
+		netdev->features &= ~feature_list;
+
+	return 0;
+}
+
+static int igb_set_tx_csum(struct net_device *netdev, u32 data)
+{
+	struct igb_adapter *adapter = netdev_priv(netdev);
+#ifdef NETIF_F_IPV6_CSUM
+	u32 feature_list = NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM;
+#else
+	u32 feature_list = NETIF_F_IP_CSUM;
+#endif
+
+	if (adapter->hw.mac.type >= e1000_82576)
+		feature_list |= NETIF_F_SCTP_CSUM;
+
+	if (data)
+		netdev->features |= feature_list;
+	else
+		netdev->features &= ~feature_list;
+
+	return 0;
+}
+
+#ifdef NETIF_F_TSO
+static int igb_set_tso(struct net_device *netdev, u32 data)
+{
+#ifdef NETIF_F_TSO6
+	const u32 feature_list = NETIF_F_TSO | NETIF_F_TSO6;
+#else
+	const u32 feature_list = NETIF_F_TSO;
+#endif
+
+	if (data)
+		netdev->features |= feature_list;
+	else
+		netdev->features &= ~feature_list;
+
+#ifndef HAVE_NETDEV_VLAN_FEATURES
+	if (!data) {
+		struct igb_adapter *adapter = netdev_priv(netdev);
+		struct net_device *v_netdev;
+		int i;
+
+		/* disable TSO on all VLANs if they're present */
+		if (!adapter->vlgrp)
+			goto tso_out;
+
+		for (i = 0; i < VLAN_GROUP_ARRAY_LEN; i++) {
+			v_netdev = vlan_group_get_device(adapter->vlgrp, i);
+			if (!v_netdev)
+				continue;
+
+			v_netdev->features &= ~feature_list;
+			vlan_group_set_device(adapter->vlgrp, i, v_netdev);
+		}
+	}
+
+tso_out:
+
+#endif /* HAVE_NETDEV_VLAN_FEATURES */
+	return 0;
+}
+
+#endif /* NETIF_F_TSO */
+#ifdef ETHTOOL_GFLAGS
+static int igb_set_flags(struct net_device *netdev, u32 data)
+{
+	u32 supported_flags = ETH_FLAG_RXVLAN | ETH_FLAG_TXVLAN |
+			      ETH_FLAG_RXHASH;
+#ifndef HAVE_VLAN_RX_REGISTER
+	u32 changed = netdev->features ^ data;
+#endif
+	int rc;
+#ifndef IGB_NO_LRO
+
+	supported_flags |= ETH_FLAG_LRO;
+#endif
+	/*
+	 * Since there is no support for separate tx vlan accel
+	 * enabled make sure tx flag is cleared if rx is.
+	 */
+	if (!(data & ETH_FLAG_RXVLAN))
+		data &= ~ETH_FLAG_TXVLAN;
+
+	rc = ethtool_op_set_flags(netdev, data, supported_flags);
+	if (rc)
+		return rc;
+#ifndef HAVE_VLAN_RX_REGISTER
+
+	if (changed & ETH_FLAG_RXVLAN)
+		igb_vlan_mode(netdev, data);
+#endif
+
+	return 0;
+}
+
+#endif /* ETHTOOL_GFLAGS */
+#endif /* HAVE_NDO_SET_FEATURES */
+#ifdef ETHTOOL_SADV_COAL
+static int igb_set_adv_coal(struct net_device *netdev, struct ethtool_value *edata)
+{
+	struct igb_adapter *adapter = netdev_priv(netdev);
+
+	switch (edata->data) {
+	case IGB_DMAC_DISABLE:
+		adapter->dmac = edata->data;
+		break;
+	case IGB_DMAC_MIN:
+		adapter->dmac = edata->data;
+		break;
+	case IGB_DMAC_500:
+		adapter->dmac = edata->data;
+		break;
+	case IGB_DMAC_EN_DEFAULT:
+		adapter->dmac = edata->data;
+		break;
+	case IGB_DMAC_2000:
+		adapter->dmac = edata->data;
+		break;
+	case IGB_DMAC_3000:
+		adapter->dmac = edata->data;
+		break;
+	case IGB_DMAC_4000:
+		adapter->dmac = edata->data;
+		break;
+	case IGB_DMAC_5000:
+		adapter->dmac = edata->data;
+		break;
+	case IGB_DMAC_6000:
+		adapter->dmac = edata->data;
+		break;
+	case IGB_DMAC_7000:
+		adapter->dmac = edata->data;
+		break;
+	case IGB_DMAC_8000:
+		adapter->dmac = edata->data;
+		break;
+	case IGB_DMAC_9000:
+		adapter->dmac = edata->data;
+		break;
+	case IGB_DMAC_MAX:
+		adapter->dmac = edata->data;
+		break;
+	default:
+		adapter->dmac = IGB_DMAC_DISABLE;
+		printk("set_dmac: invalid setting, setting DMAC to %d\n",
+			adapter->dmac);
+	}
+	printk("%s: setting DMAC to %d\n", netdev->name, adapter->dmac);
+	return 0;
+}
+
+#endif /* ETHTOOL_SADV_COAL */
+#ifdef ETHTOOL_GADV_COAL
+static void igb_get_dmac(struct net_device *netdev,
+			    struct ethtool_value *edata)
+{
+	struct igb_adapter *adapter = netdev_priv(netdev);
+	edata->data = adapter->dmac;
+
+	return;
+}
+#endif
+
+#ifdef ETHTOOL_GEEE
+static int igb_get_eee(struct net_device *netdev, struct ethtool_eee *edata)
+{
+	struct igb_adapter *adapter = netdev_priv(netdev);
+	struct e1000_hw *hw = &adapter->hw;
+	u32 ret_val;
+	u16 phy_data;
+
+	if ((hw->mac.type < e1000_i350) ||
+	    (hw->phy.media_type != e1000_media_type_copper))
+		return -EOPNOTSUPP;
+
+	edata->supported = (SUPPORTED_1000baseT_Full |
+			    SUPPORTED_100baseT_Full);
+
+	if (!hw->dev_spec._82575.eee_disable)
+		edata->advertised =
+			mmd_eee_adv_to_ethtool_adv_t(adapter->eee_advert);
+
+	/* The IPCNFG and EEER registers are not supported on I354. */
+	if (hw->mac.type == e1000_i354) {
+		e1000_get_eee_status_i354(hw, (bool *)&edata->eee_active);
+	} else {
+		u32 eeer;
+
+		eeer = E1000_READ_REG(hw, E1000_EEER);
+
+		/* EEE status on negotiated link */
+		if (eeer & E1000_EEER_EEE_NEG)
+			edata->eee_active = true;
+
+		if (eeer & E1000_EEER_TX_LPI_EN)
+			edata->tx_lpi_enabled = true;
+	}
+
+	/* EEE Link Partner Advertised */
+	switch (hw->mac.type) {
+	case e1000_i350:
+		ret_val = e1000_read_emi_reg(hw, E1000_EEE_LP_ADV_ADDR_I350,
+					     &phy_data);
+		if (ret_val)
+			return -ENODATA;
+
+		edata->lp_advertised = mmd_eee_adv_to_ethtool_adv_t(phy_data);
+
+		break;
+	case e1000_i354:
+	case e1000_i210:
+	case e1000_i211:
+		ret_val = e1000_read_xmdio_reg(hw, E1000_EEE_LP_ADV_ADDR_I210,
+					       E1000_EEE_LP_ADV_DEV_I210,
+					       &phy_data);
+		if (ret_val)
+			return -ENODATA;
+
+		edata->lp_advertised = mmd_eee_adv_to_ethtool_adv_t(phy_data);
+
+		break;
+	default:
+		break;
+	}
+
+	edata->eee_enabled = !hw->dev_spec._82575.eee_disable;
+
+	if ((hw->mac.type == e1000_i354) &&
+	    (edata->eee_enabled))
+		edata->tx_lpi_enabled = true;
+
+	/*
+	 * report correct negotiated EEE status for devices that
+	 * wrongly report EEE at half-duplex
+	 */
+	if (adapter->link_duplex == HALF_DUPLEX) {
+		edata->eee_enabled = false;
+		edata->eee_active = false;
+		edata->tx_lpi_enabled = false;
+		edata->advertised &= ~edata->advertised;
+	}
+
+	return 0;
+}
+#endif
+
+#ifdef ETHTOOL_SEEE
+static int igb_set_eee(struct net_device *netdev,
+		       struct ethtool_eee *edata)
+{
+	struct igb_adapter *adapter = netdev_priv(netdev);
+	struct e1000_hw *hw = &adapter->hw;
+	struct ethtool_eee eee_curr;
+	s32 ret_val;
+
+	if ((hw->mac.type < e1000_i350) ||
+	    (hw->phy.media_type != e1000_media_type_copper))
+		return -EOPNOTSUPP;
+
+	ret_val = igb_get_eee(netdev, &eee_curr);
+	if (ret_val)
+		return ret_val;
+
+	if (eee_curr.eee_enabled) {
+		if (eee_curr.tx_lpi_enabled != edata->tx_lpi_enabled) {
+			dev_err(pci_dev_to_dev(adapter->pdev),
+				"Setting EEE tx-lpi is not supported\n");
+			return -EINVAL;
+		}
+
+		/* Tx LPI time is not implemented currently */
+		if (edata->tx_lpi_timer) {
+			dev_err(pci_dev_to_dev(adapter->pdev),
+				"Setting EEE Tx LPI timer is not supported\n");
+			return -EINVAL;
+		}
+
+		if (edata->advertised &
+		    ~(ADVERTISE_100_FULL | ADVERTISE_1000_FULL)) {
+			dev_err(pci_dev_to_dev(adapter->pdev),
+				"EEE Advertisement supports only 100Tx and or 100T full duplex\n");
+			return -EINVAL;
+		}
+
+	} else if (!edata->eee_enabled) {
+		dev_err(pci_dev_to_dev(adapter->pdev),
+			"Setting EEE options is not supported with EEE disabled\n");
+			return -EINVAL;
+		}
+
+	adapter->eee_advert = ethtool_adv_to_mmd_eee_adv_t(edata->advertised);
+
+	if (hw->dev_spec._82575.eee_disable != !edata->eee_enabled) {
+		hw->dev_spec._82575.eee_disable = !edata->eee_enabled;
+
+		/* reset link */
+		if (netif_running(netdev))
+			igb_reinit_locked(adapter);
+		else
+			igb_reset(adapter);
+	}
+
+	return 0;
+}
+#endif /* ETHTOOL_SEEE */
+#ifdef ETHTOOL_GRXFH
+#ifdef ETHTOOL_GRXFHINDIR
+
+static int igb_get_rss_hash_opts(struct igb_adapter *adapter,
+				 struct ethtool_rxnfc *cmd)
+{
+	cmd->data = 0;
+
+	/* Report default options for RSS on igb */
+	switch (cmd->flow_type) {
+	case TCP_V4_FLOW:
+		cmd->data |= RXH_L4_B_0_1 | RXH_L4_B_2_3;
+	case UDP_V4_FLOW:
+		if (adapter->flags & IGB_FLAG_RSS_FIELD_IPV4_UDP)
+			cmd->data |= RXH_L4_B_0_1 | RXH_L4_B_2_3;
+	case SCTP_V4_FLOW:
+	case AH_ESP_V4_FLOW:
+	case AH_V4_FLOW:
+	case ESP_V4_FLOW:
+	case IPV4_FLOW:
+		cmd->data |= RXH_IP_SRC | RXH_IP_DST;
+		break;
+	case TCP_V6_FLOW:
+		cmd->data |= RXH_L4_B_0_1 | RXH_L4_B_2_3;
+	case UDP_V6_FLOW:
+		if (adapter->flags & IGB_FLAG_RSS_FIELD_IPV6_UDP)
+			cmd->data |= RXH_L4_B_0_1 | RXH_L4_B_2_3;
+	case SCTP_V6_FLOW:
+	case AH_ESP_V6_FLOW:
+	case AH_V6_FLOW:
+	case ESP_V6_FLOW:
+	case IPV6_FLOW:
+		cmd->data |= RXH_IP_SRC | RXH_IP_DST;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+#endif /* ETHTOOL_GRXFHINDIR */
+static int igb_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd,
+#ifdef HAVE_ETHTOOL_GET_RXNFC_VOID_RULE_LOCS
+			   void *rule_locs)
+#else
+			   u32 *rule_locs)
+#endif
+{
+	struct igb_adapter *adapter = netdev_priv(dev);
+	int ret = -EOPNOTSUPP;
+
+	switch (cmd->cmd) {
+	case ETHTOOL_GRXRINGS:
+		cmd->data = adapter->num_rx_queues;
+		ret = 0;
+		break;
+#ifdef ETHTOOL_GRXFHINDIR
+	case ETHTOOL_GRXFHINDIR:
+		ret = igb_get_rss_hash_opts(adapter, cmd);
+		break;
+#endif /* ETHTOOL_GRXFHINDIR */
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+#define UDP_RSS_FLAGS (IGB_FLAG_RSS_FIELD_IPV4_UDP | \
+		       IGB_FLAG_RSS_FIELD_IPV6_UDP)
+static int igb_set_rss_hash_opt(struct igb_adapter *adapter,
+				struct ethtool_rxnfc *nfc)
+{
+	u32 flags = adapter->flags;
+
+	/*
+	 * RSS does not support anything other than hashing
+	 * to queues on src and dst IPs and ports
+	 */
+	if (nfc->data & ~(RXH_IP_SRC | RXH_IP_DST |
+			  RXH_L4_B_0_1 | RXH_L4_B_2_3))
+		return -EINVAL;
+
+	switch (nfc->flow_type) {
+	case TCP_V4_FLOW:
+	case TCP_V6_FLOW:
+		if (!(nfc->data & RXH_IP_SRC) ||
+		    !(nfc->data & RXH_IP_DST) ||
+		    !(nfc->data & RXH_L4_B_0_1) ||
+		    !(nfc->data & RXH_L4_B_2_3))
+			return -EINVAL;
+		break;
+	case UDP_V4_FLOW:
+		if (!(nfc->data & RXH_IP_SRC) ||
+		    !(nfc->data & RXH_IP_DST))
+			return -EINVAL;
+		switch (nfc->data & (RXH_L4_B_0_1 | RXH_L4_B_2_3)) {
+		case 0:
+			flags &= ~IGB_FLAG_RSS_FIELD_IPV4_UDP;
+			break;
+		case (RXH_L4_B_0_1 | RXH_L4_B_2_3):
+			flags |= IGB_FLAG_RSS_FIELD_IPV4_UDP;
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	case UDP_V6_FLOW:
+		if (!(nfc->data & RXH_IP_SRC) ||
+		    !(nfc->data & RXH_IP_DST))
+			return -EINVAL;
+		switch (nfc->data & (RXH_L4_B_0_1 | RXH_L4_B_2_3)) {
+		case 0:
+			flags &= ~IGB_FLAG_RSS_FIELD_IPV6_UDP;
+			break;
+		case (RXH_L4_B_0_1 | RXH_L4_B_2_3):
+			flags |= IGB_FLAG_RSS_FIELD_IPV6_UDP;
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	case AH_ESP_V4_FLOW:
+	case AH_V4_FLOW:
+	case ESP_V4_FLOW:
+	case SCTP_V4_FLOW:
+	case AH_ESP_V6_FLOW:
+	case AH_V6_FLOW:
+	case ESP_V6_FLOW:
+	case SCTP_V6_FLOW:
+		if (!(nfc->data & RXH_IP_SRC) ||
+		    !(nfc->data & RXH_IP_DST) ||
+		    (nfc->data & RXH_L4_B_0_1) ||
+		    (nfc->data & RXH_L4_B_2_3))
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* if we changed something we need to update flags */
+	if (flags != adapter->flags) {
+		struct e1000_hw *hw = &adapter->hw;
+		u32 mrqc = E1000_READ_REG(hw, E1000_MRQC);
+
+		if ((flags & UDP_RSS_FLAGS) &&
+		    !(adapter->flags & UDP_RSS_FLAGS))
+			DPRINTK(DRV, WARNING,
+				"enabling UDP RSS: fragmented packets may arrive out of order to the stack above\n");
+
+		adapter->flags = flags;
+
+		/* Perform hash on these packet types */
+		mrqc |= E1000_MRQC_RSS_FIELD_IPV4 |
+			E1000_MRQC_RSS_FIELD_IPV4_TCP |
+			E1000_MRQC_RSS_FIELD_IPV6 |
+			E1000_MRQC_RSS_FIELD_IPV6_TCP;
+
+		mrqc &= ~(E1000_MRQC_RSS_FIELD_IPV4_UDP |
+			  E1000_MRQC_RSS_FIELD_IPV6_UDP);
+
+		if (flags & IGB_FLAG_RSS_FIELD_IPV4_UDP)
+			mrqc |= E1000_MRQC_RSS_FIELD_IPV4_UDP;
+
+		if (flags & IGB_FLAG_RSS_FIELD_IPV6_UDP)
+			mrqc |= E1000_MRQC_RSS_FIELD_IPV6_UDP;
+
+		E1000_WRITE_REG(hw, E1000_MRQC, mrqc);
+	}
+
+	return 0;
+}
+
+static int igb_set_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd)
+{
+	struct igb_adapter *adapter = netdev_priv(dev);
+	int ret = -EOPNOTSUPP;
+
+	switch (cmd->cmd) {
+	case ETHTOOL_SRXFH:
+		ret = igb_set_rss_hash_opt(adapter, cmd);
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+#endif /* ETHTOOL_GRXFH */
+#ifdef ETHTOOL_GRXFHINDIR
+static u32 igb_get_rxfh_indir_size(struct net_device *netdev)
+{
+	return IGB_RETA_SIZE;
+}
+
+static int igb_get_rxfh_indir(struct net_device *netdev, u32 *indir)
+{
+	struct igb_adapter *adapter = netdev_priv(netdev);
+	int i;
+
+	for (i = 0; i < IGB_RETA_SIZE; i++)
+		indir[i] = adapter->rss_indir_tbl[i];
+
+	return 0;
+}
+
+#endif /* ETHTOOL_GRXFHINDIR */
+#ifdef ETHTOOL_SRXFHINDIR
+void igb_write_rss_indir_tbl(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u32 reg = E1000_RETA(0);
+	u32 shift = 0;
+	int i = 0;
+
+	switch (hw->mac.type) {
+	case e1000_82575:
+		shift = 6;
+		break;
+	case e1000_82576:
+		/* 82576 supports 2 RSS queues for SR-IOV */
+		if (adapter->vfs_allocated_count)
+			shift = 3;
+		break;
+	default:
+		break;
+	}
+
+	while (i < IGB_RETA_SIZE) {
+		u32 val = 0;
+		int j;
+
+		for (j = 3; j >= 0; j--) {
+			val <<= 8;
+			val |= adapter->rss_indir_tbl[i + j];
+		}
+
+		E1000_WRITE_REG(hw, reg, val << shift);
+		reg += 4;
+		i += 4;
+	}
+}
+
+static int igb_set_rxfh_indir(struct net_device *netdev, const u32 *indir)
+{
+	struct igb_adapter *adapter = netdev_priv(netdev);
+	struct e1000_hw *hw = &adapter->hw;
+	int i;
+	u32 num_queues;
+
+	num_queues = adapter->rss_queues;
+
+	switch (hw->mac.type) {
+	case e1000_82576:
+		/* 82576 supports 2 RSS queues for SR-IOV */
+		if (adapter->vfs_allocated_count)
+			num_queues = 2;
+		break;
+	default:
+		break;
+	}
+
+	/* Verify user input. */
+	for (i = 0; i < IGB_RETA_SIZE; i++)
+		if (indir[i] > num_queues)
+			return -EINVAL;
+
+
+	for (i = 0; i < IGB_RETA_SIZE; i++)
+		adapter->rss_indir_tbl[i] = indir[i];
+
+	igb_write_rss_indir_tbl(adapter);
+
+	return 0;
+}
+#endif /* ETHTOOL_SRXFHINDIR */
+#ifdef ETHTOOL_GCHANNELS
+
+static unsigned int igb_max_rss_queues(struct igb_adapter *adapter)
+{
+	unsigned int max_rss_queues;
+
+	/* Determine the maximum number of RSS queues supported. */
+	switch (adapter->hw.mac.type) {
+	case e1000_i211:
+		max_rss_queues = IGB_MAX_RX_QUEUES_I211;
+		break;
+	case e1000_82575:
+	case e1000_i210:
+		max_rss_queues = IGB_MAX_RX_QUEUES_82575;
+		break;
+	case e1000_i350:
+		/* I350 cannot do RSS and SR-IOV at the same time */
+		if (adapter->vfs_allocated_count) {
+			max_rss_queues = 1;
+			break;
+		}
+		/* fall through */
+	case e1000_82576:
+		if (adapter->vfs_allocated_count) {
+			max_rss_queues = 2;
+			break;
+		}
+		/* fall through */
+	case e1000_82580:
+	default:
+		max_rss_queues = IGB_MAX_RX_QUEUES;
+        break;
+	}
+
+	return max_rss_queues;
+}
+
+static void igb_get_channels(struct net_device *dev,
+			     struct ethtool_channels *ch)
+{
+	struct igb_adapter *adapter = netdev_priv(dev);
+
+	/* report maximum channels */
+	ch->max_combined = igb_max_rss_queues(adapter);
+	ch->max_rx = ch->max_combined;
+	if (adapter->vfs_allocated_count)
+		ch->max_tx = 1;
+	else
+		ch->max_tx = ch->max_combined;
+
+	/* report info for other vector */
+	if (adapter->msix_entries) {
+		ch->max_other = NON_Q_VECTORS;
+		ch->other_count = NON_Q_VECTORS;
+	}
+
+	/* record RSS/TSS queues */
+	if (adapter->flags & IGB_FLAG_QUEUE_PAIRS) {
+		if (adapter->num_rx_queues > adapter->num_tx_queues) {
+			ch->combined_count = adapter->num_tx_queues;
+			ch->rx_count = adapter->num_rx_queues -
+				       adapter->num_tx_queues;
+		} else if (adapter->num_rx_queues < adapter->num_tx_queues) {
+			ch->combined_count = adapter->num_rx_queues;
+			ch->tx_count = adapter->num_tx_queues -
+				       adapter->num_rx_queues;
+		} else {
+			ch->combined_count = adapter->num_rx_queues;
+		}
+	} else {
+		ch->rx_count = adapter->num_rx_queues;
+		ch->tx_count = adapter->num_tx_queues;
+	}
+}
+#endif /* ETHTOOL_GCHANNELS */
+#ifdef ETHTOOL_SCHANNELS
+
+static int igb_set_channels(struct net_device *dev,
+				struct ethtool_channels *ch)
+{
+	struct igb_adapter *adapter = netdev_priv(dev);
+	unsigned int max_rss_queues;
+
+	/* we cannot support combined, Rx, and Tx vectors simultaneously */
+	if (ch->combined_count && ch->rx_count && ch->tx_count)
+		return -EINVAL;
+
+	/* ignore other_count since it is not changeable */
+
+	/* verify we have at least one channel in each direction */
+	if (!ch->combined_count && (!ch->rx_count || !ch->tx_count))
+		return -EINVAL;
+
+	/* verify number of Tx queues does not exceed 1 if SR-IOV is enabled */
+	if (adapter->vfs_allocated_count &&
+	    ((ch->combined_count + ch->tx_count) > 1))
+		return -EINVAL;
+
+	/* verify the number of channels does not exceed hardware limits */
+	max_rss_queues = igb_max_rss_queues(adapter);
+	if (((ch->combined_count + ch->rx_count) > max_rss_queues) ||
+	    ((ch->combined_count + ch->tx_count) > max_rss_queues))
+		return -EINVAL;
+
+	/* Determine if we need to pair queues. */
+	switch (adapter->hw.mac.type) {
+	case e1000_82575:
+	case e1000_i211:
+		/* Device supports enough interrupts without queue pairing. */
+		break;
+	case e1000_i350:
+		/* The PF has 3 interrupts and 1 queue pair w/ SR-IOV */
+		if (adapter->vfs_allocated_count)
+			break;
+	case e1000_82576:
+		/*
+		 * The PF has access to 6 interrupt vectors if the number of
+		 * VFs is less than 7.  If that is the case we don't have
+		 * to pair up the queues.
+		 */
+		if ((adapter->vfs_allocated_count > 0) &&
+		    (adapter->vfs_allocated_count < 7))
+			break;
+		/* fall through */
+	case e1000_82580:
+	case e1000_i210:
+	default:
+		/* verify we can support as many queues as requested */
+		if ((ch->combined_count +
+		     ch->rx_count + ch->tx_count) > MAX_Q_VECTORS)
+			return -EINVAL;
+		break;
+	}
+
+	/* update configuration values */
+	adapter->rss_queues = ch->combined_count + ch->rx_count;
+	if (ch->rx_count == ch->tx_count || adapter->vfs_allocated_count)
+		adapter->tss_queues = 0;
+	else
+		adapter->tss_queues = ch->combined_count + ch->tx_count;
+
+	if (ch->combined_count)
+		adapter->flags |= IGB_FLAG_QUEUE_PAIRS;
+	else
+		adapter->flags &= ~IGB_FLAG_QUEUE_PAIRS;
+
+	/* update queue configuration for adapter */
+	return igb_setup_queues(adapter);
+}
+
+#endif /* ETHTOOL_SCHANNELS */
 static const struct ethtool_ops igb_ethtool_ops = {
 	.get_settings           = igb_get_settings,
 	.set_settings           = igb_set_settings,
@@ -2188,14 +3001,106 @@ static const struct ethtool_ops igb_ethtool_ops = {
 	.set_pauseparam         = igb_set_pauseparam,
 	.self_test              = igb_diag_test,
 	.get_strings            = igb_get_strings,
+#ifndef HAVE_RHEL6_ETHTOOL_OPS_EXT_STRUCT
+#ifdef HAVE_ETHTOOL_SET_PHYS_ID
 	.set_phys_id            = igb_set_phys_id,
+#else
+	.phys_id                = igb_phys_id,
+#endif /* HAVE_ETHTOOL_SET_PHYS_ID */
+#endif /* HAVE_RHEL6_ETHTOOL_OPS_EXT_STRUCT */
+#ifdef HAVE_ETHTOOL_GET_SSET_COUNT
 	.get_sset_count         = igb_get_sset_count,
+#else
+	.get_stats_count        = igb_get_stats_count,
+	.self_test_count        = igb_diag_test_count,
+#endif
 	.get_ethtool_stats      = igb_get_ethtool_stats,
+#ifdef HAVE_ETHTOOL_GET_PERM_ADDR
+	.get_perm_addr          = ethtool_op_get_perm_addr,
+#endif
 	.get_coalesce           = igb_get_coalesce,
 	.set_coalesce           = igb_set_coalesce,
+#ifndef HAVE_RHEL6_ETHTOOL_OPS_EXT_STRUCT
+#ifdef HAVE_ETHTOOL_GET_TS_INFO
+	.get_ts_info            = igb_get_ts_info,
+#endif /* HAVE_ETHTOOL_GET_TS_INFO */
+#endif /* HAVE_RHEL6_ETHTOOL_OPS_EXT_STRUCT */
+#ifdef CONFIG_PM_RUNTIME
+	.begin			= igb_ethtool_begin,
+	.complete		= igb_ethtool_complete,
+#endif /* CONFIG_PM_RUNTIME */
+#ifndef HAVE_NDO_SET_FEATURES
+	.get_rx_csum            = igb_get_rx_csum,
+	.set_rx_csum            = igb_set_rx_csum,
+	.get_tx_csum            = ethtool_op_get_tx_csum,
+	.set_tx_csum            = igb_set_tx_csum,
+	.get_sg                 = ethtool_op_get_sg,
+	.set_sg                 = ethtool_op_set_sg,
+#ifdef NETIF_F_TSO
+	.get_tso                = ethtool_op_get_tso,
+	.set_tso                = igb_set_tso,
+#endif
+#ifdef ETHTOOL_GFLAGS
+	.get_flags              = ethtool_op_get_flags,
+	.set_flags              = igb_set_flags,
+#endif /* ETHTOOL_GFLAGS */
+#endif /* HAVE_NDO_SET_FEATURES */
+#ifdef ETHTOOL_GADV_COAL
+	.get_advcoal		= igb_get_adv_coal,
+	.set_advcoal		= igb_set_dmac_coal,
+#endif /* ETHTOOL_GADV_COAL */
+#ifndef HAVE_RHEL6_ETHTOOL_OPS_EXT_STRUCT
+#ifdef ETHTOOL_GEEE
+	.get_eee		= igb_get_eee,
+#endif
+#ifdef ETHTOOL_SEEE
+	.set_eee		= igb_set_eee,
+#endif
+#ifdef ETHTOOL_GRXFHINDIR
+	.get_rxfh_indir_size	= igb_get_rxfh_indir_size,
+	.get_rxfh_indir		= igb_get_rxfh_indir,
+#endif /* ETHTOOL_GRXFHINDIR */
+#ifdef ETHTOOL_SRXFHINDIR
+	.set_rxfh_indir		= igb_set_rxfh_indir,
+#endif /* ETHTOOL_SRXFHINDIR */
+#ifdef ETHTOOL_GCHANNELS
+	.get_channels           = igb_get_channels,
+#endif /* ETHTOOL_GCHANNELS */
+#ifdef ETHTOOL_SCHANNELS
+	.set_channels           = igb_set_channels,
+#endif /* ETHTOOL_SCHANNELS */
+#endif /* HAVE_RHEL6_ETHTOOL_OPS_EXT_STRUCT */
+#ifdef ETHTOOL_GRXFH
+	.get_rxnfc		= igb_get_rxnfc,
+	.set_rxnfc		= igb_set_rxnfc,
+#endif
+};
+
+#ifdef HAVE_RHEL6_ETHTOOL_OPS_EXT_STRUCT
+static const struct ethtool_ops_ext igb_ethtool_ops_ext = {
+	.size			= sizeof(struct ethtool_ops_ext),
+	.get_ts_info		= igb_get_ts_info,
+	.set_phys_id		= igb_set_phys_id,
+	.get_eee		= igb_get_eee,
+	.set_eee		= igb_set_eee,
+	.get_rxfh_indir_size	= igb_get_rxfh_indir_size,
+	.get_rxfh_indir		= igb_get_rxfh_indir,
+	.set_rxfh_indir		= igb_set_rxfh_indir,
+	.get_channels           = igb_get_channels,
+	.set_channels           = igb_set_channels,
 };
 
 void igb_set_ethtool_ops(struct net_device *netdev)
 {
 	SET_ETHTOOL_OPS(netdev, &igb_ethtool_ops);
+	set_ethtool_ops_ext(netdev, &igb_ethtool_ops_ext);
 }
+#else
+void igb_set_ethtool_ops(struct net_device *netdev)
+{
+	/* have to "undeclare" const on this struct to remove warnings */
+	SET_ETHTOOL_OPS(netdev, (struct ethtool_ops *)&igb_ethtool_ops);
+}
+#endif /* HAVE_RHEL6_ETHTOOL_OPS_EXT_STRUCT */
+#endif	/* SIOCETHTOOL */
+
diff --git a/drivers/net/igb/igb_hwmon.c b/drivers/net/igb/igb_hwmon.c
new file mode 100644
index 000000000000..07a1ae072aa2
--- /dev/null
+++ b/drivers/net/igb/igb_hwmon.c
@@ -0,0 +1,260 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+#include "igb.h"
+#include "e1000_82575.h"
+#include "e1000_hw.h"
+#ifdef IGB_HWMON
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/sysfs.h>
+#include <linux/kobject.h>
+#include <linux/device.h>
+#include <linux/netdevice.h>
+#include <linux/hwmon.h>
+#include <linux/pci.h>
+
+#ifdef HAVE_I2C_SUPPORT
+static struct i2c_board_info i350_sensor_info = {
+	I2C_BOARD_INFO("i350bb", (0Xf8 >> 1)),
+};
+#endif /* HAVE_I2C_SUPPORT */
+
+/* hwmon callback functions */
+static ssize_t igb_hwmon_show_location(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct hwmon_attr *igb_attr = container_of(attr, struct hwmon_attr,
+						     dev_attr);
+	return sprintf(buf, "loc%u\n",
+		       igb_attr->sensor->location);
+}
+
+static ssize_t igb_hwmon_show_temp(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	struct hwmon_attr *igb_attr = container_of(attr, struct hwmon_attr,
+						     dev_attr);
+	unsigned int value;
+
+	/* reset the temp field */
+	igb_attr->hw->mac.ops.get_thermal_sensor_data(igb_attr->hw);
+
+	value = igb_attr->sensor->temp;
+
+	/* display millidegree */
+	value *= 1000;
+
+	return sprintf(buf, "%u\n", value);
+}
+
+static ssize_t igb_hwmon_show_cautionthresh(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	struct hwmon_attr *igb_attr = container_of(attr, struct hwmon_attr,
+						     dev_attr);
+	unsigned int value = igb_attr->sensor->caution_thresh;
+
+	/* display millidegree */
+	value *= 1000;
+
+	return sprintf(buf, "%u\n", value);
+}
+
+static ssize_t igb_hwmon_show_maxopthresh(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	struct hwmon_attr *igb_attr = container_of(attr, struct hwmon_attr,
+						     dev_attr);
+	unsigned int value = igb_attr->sensor->max_op_thresh;
+
+	/* display millidegree */
+	value *= 1000;
+
+	return sprintf(buf, "%u\n", value);
+}
+
+/* igb_add_hwmon_attr - Create hwmon attr table for a hwmon sysfs file.
+ * @ adapter: pointer to the adapter structure
+ * @ offset: offset in the eeprom sensor data table
+ * @ type: type of sensor data to display
+ *
+ * For each file we want in hwmon's sysfs interface we need a device_attribute
+ * This is included in our hwmon_attr struct that contains the references to
+ * the data structures we need to get the data to display.
+ */
+static int igb_add_hwmon_attr(struct igb_adapter *adapter,
+				unsigned int offset, int type) {
+	int rc;
+	unsigned int n_attr;
+	struct hwmon_attr *igb_attr;
+
+	n_attr = adapter->igb_hwmon_buff.n_hwmon;
+	igb_attr = &adapter->igb_hwmon_buff.hwmon_list[n_attr];
+
+	switch (type) {
+	case IGB_HWMON_TYPE_LOC:
+		igb_attr->dev_attr.show = igb_hwmon_show_location;
+		snprintf(igb_attr->name, sizeof(igb_attr->name),
+			 "temp%u_label", offset);
+		break;
+	case IGB_HWMON_TYPE_TEMP:
+		igb_attr->dev_attr.show = igb_hwmon_show_temp;
+		snprintf(igb_attr->name, sizeof(igb_attr->name),
+			 "temp%u_input", offset);
+		break;
+	case IGB_HWMON_TYPE_CAUTION:
+		igb_attr->dev_attr.show = igb_hwmon_show_cautionthresh;
+		snprintf(igb_attr->name, sizeof(igb_attr->name),
+			 "temp%u_max", offset);
+		break;
+	case IGB_HWMON_TYPE_MAX:
+		igb_attr->dev_attr.show = igb_hwmon_show_maxopthresh;
+		snprintf(igb_attr->name, sizeof(igb_attr->name),
+			 "temp%u_crit", offset);
+		break;
+	default:
+		rc = -EPERM;
+		return rc;
+	}
+
+	/* These always the same regardless of type */
+	igb_attr->sensor =
+		&adapter->hw.mac.thermal_sensor_data.sensor[offset];
+	igb_attr->hw = &adapter->hw;
+	igb_attr->dev_attr.store = NULL;
+	igb_attr->dev_attr.attr.mode = S_IRUGO;
+	igb_attr->dev_attr.attr.name = igb_attr->name;
+	sysfs_attr_init(&igb_attr->dev_attr.attr);
+	rc = device_create_file(&adapter->pdev->dev,
+				&igb_attr->dev_attr);
+	if (rc == 0)
+		++adapter->igb_hwmon_buff.n_hwmon;
+
+	return rc;
+}
+
+static void igb_sysfs_del_adapter(struct igb_adapter *adapter)
+{
+	int i;
+
+	if (adapter == NULL)
+		return;
+
+	for (i = 0; i < adapter->igb_hwmon_buff.n_hwmon; i++) {
+		device_remove_file(&adapter->pdev->dev,
+			   &adapter->igb_hwmon_buff.hwmon_list[i].dev_attr);
+	}
+
+	kfree(adapter->igb_hwmon_buff.hwmon_list);
+
+	if (adapter->igb_hwmon_buff.device)
+		hwmon_device_unregister(adapter->igb_hwmon_buff.device);
+}
+
+/* called from igb_main.c */
+void igb_sysfs_exit(struct igb_adapter *adapter)
+{
+	igb_sysfs_del_adapter(adapter);
+}
+
+/* called from igb_main.c */
+int igb_sysfs_init(struct igb_adapter *adapter)
+{
+	struct hwmon_buff *igb_hwmon = &adapter->igb_hwmon_buff;
+	unsigned int i;
+	int n_attrs;
+	int rc = 0;
+#ifdef HAVE_I2C_SUPPORT
+	struct i2c_client *client = NULL;
+#endif /* HAVE_I2C_SUPPORT */
+
+	/* If this method isn't defined we don't support thermals */
+	if (adapter->hw.mac.ops.init_thermal_sensor_thresh == NULL)
+		goto exit;
+
+	/* Don't create thermal hwmon interface if no sensors present */
+	rc = (adapter->hw.mac.ops.init_thermal_sensor_thresh(&adapter->hw));
+		if (rc)
+			goto exit;
+#ifdef HAVE_I2C_SUPPORT
+	/* init i2c_client */
+	client = i2c_new_device(&adapter->i2c_adap, &i350_sensor_info);
+	if (client == NULL) {
+		dev_info(&adapter->pdev->dev,
+			"Failed to create new i2c device..\n");
+		goto exit;
+	}
+	adapter->i2c_client = client;
+#endif /* HAVE_I2C_SUPPORT */
+
+	/* Allocation space for max attributes
+	 * max num sensors * values (loc, temp, max, caution)
+	 */
+	n_attrs = E1000_MAX_SENSORS * 4;
+	igb_hwmon->hwmon_list = kcalloc(n_attrs, sizeof(struct hwmon_attr),
+					  GFP_KERNEL);
+	if (!igb_hwmon->hwmon_list) {
+		rc = -ENOMEM;
+		goto err;
+	}
+
+	igb_hwmon->device = hwmon_device_register(&adapter->pdev->dev);
+	if (IS_ERR(igb_hwmon->device)) {
+		rc = PTR_ERR(igb_hwmon->device);
+		goto err;
+	}
+
+	for (i = 0; i < E1000_MAX_SENSORS; i++) {
+
+		/* Only create hwmon sysfs entries for sensors that have
+		 * meaningful data.
+		 */
+		if (adapter->hw.mac.thermal_sensor_data.sensor[i].location == 0)
+			continue;
+
+		/* Bail if any hwmon attr struct fails to initialize */
+		rc = igb_add_hwmon_attr(adapter, i, IGB_HWMON_TYPE_CAUTION);
+		rc |= igb_add_hwmon_attr(adapter, i, IGB_HWMON_TYPE_LOC);
+		rc |= igb_add_hwmon_attr(adapter, i, IGB_HWMON_TYPE_TEMP);
+		rc |= igb_add_hwmon_attr(adapter, i, IGB_HWMON_TYPE_MAX);
+		if (rc)
+			goto err;
+	}
+
+	goto exit;
+
+err:
+	igb_sysfs_del_adapter(adapter);
+exit:
+	return rc;
+}
+#endif /* IGB_HWMON */
diff --git a/drivers/net/igb/igb_main.c b/drivers/net/igb/igb_main.c
index 40d4c405fd7e..325d4f2abba0 100644
--- a/drivers/net/igb/igb_main.c
+++ b/drivers/net/igb/igb_main.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel(R) Gigabit Ethernet Linux driver
-  Copyright(c) 2007-2011 Intel Corporation.
+  Copyright(c) 2007-2013 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -28,71 +28,94 @@
 #include <linux/module.h>
 #include <linux/types.h>
 #include <linux/init.h>
-#include <linux/bitops.h>
 #include <linux/vmalloc.h>
 #include <linux/pagemap.h>
 #include <linux/netdevice.h>
-#include <linux/ipv6.h>
-#include <linux/slab.h>
+#include <linux/tcp.h>
+#ifdef NETIF_F_TSO
 #include <net/checksum.h>
+#ifdef NETIF_F_TSO6
+#include <linux/ipv6.h>
 #include <net/ip6_checksum.h>
-#include <linux/net_tstamp.h>
+#endif
+#endif
+#ifdef SIOCGMIIPHY
 #include <linux/mii.h>
+#endif
+#ifdef SIOCETHTOOL
 #include <linux/ethtool.h>
-#include <linux/if_vlan.h>
-#include <linux/pci.h>
-#include <linux/pci-aspm.h>
-#include <linux/delay.h>
-#include <linux/interrupt.h>
-#include <linux/if_ether.h>
-#include <linux/aer.h>
-#include <linux/prefetch.h>
-#ifdef CONFIG_IGB_DCA
-#include <linux/dca.h>
 #endif
+#include <linux/if_vlan.h>
+#ifdef CONFIG_PM_RUNTIME
+#include <linux/pm_runtime.h>
+#endif /* CONFIG_PM_RUNTIME */
+
+#include <linux/if_bridge.h>
+#include <linux/ctype.h>
 #include "igb.h"
+#include "igb_vmdq.h"
+
+#if defined(DEBUG) || defined (DEBUG_DUMP) || defined (DEBUG_ICR) || defined(DEBUG_ITR)
+#define DRV_DEBUG "_debug"
+#else
+#define DRV_DEBUG
+#endif
+#define DRV_HW_PERF
+#define VERSION_SUFFIX
+
+#define MAJ 5
+#define MIN 1
+#define BUILD 2
+#define DRV_VERSION __stringify(MAJ) "." __stringify(MIN) "." __stringify(BUILD) VERSION_SUFFIX DRV_DEBUG DRV_HW_PERF
 
-#define MAJ 3
-#define MIN 0
-#define BUILD 6
-#define DRV_VERSION __stringify(MAJ) "." __stringify(MIN) "." \
-__stringify(BUILD) "-k"
 char igb_driver_name[] = "igb";
 char igb_driver_version[] = DRV_VERSION;
 static const char igb_driver_string[] =
-				"Intel(R) Gigabit Ethernet Network Driver";
-static const char igb_copyright[] = "Copyright (c) 2007-2011 Intel Corporation.";
+                                "Intel(R) Gigabit Ethernet Network Driver";
+static const char igb_copyright[] =
+				"Copyright (c) 2007-2013 Intel Corporation.";
 
-static const struct e1000_info *igb_info_tbl[] = {
-	[board_82575] = &e1000_82575_info,
-};
+static char g_mac_addr[ETH_ALEN];
+static int g_usr_mac = 0;
 
 static DEFINE_PCI_DEVICE_TABLE(igb_pci_tbl) = {
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I350_COPPER), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I350_FIBER), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I350_SERDES), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I350_SGMII), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_COPPER), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_FIBER), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_QUAD_FIBER), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_SERDES), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_SGMII), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_COPPER_DUAL), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_DH89XXCC_SGMII), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_DH89XXCC_SERDES), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_DH89XXCC_BACKPLANE), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_DH89XXCC_SFP), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576_NS), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576_NS_SERDES), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576_FIBER), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576_SERDES), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576_SERDES_QUAD), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576_QUAD_COPPER_ET2), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576_QUAD_COPPER), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82575EB_COPPER), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82575EB_FIBER_SERDES), board_82575 },
-	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82575GB_QUAD_COPPER), board_82575 },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I354_BACKPLANE_1GBPS) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I354_SGMII) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I354_BACKPLANE_2_5GBPS) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I210_TOOLS_ONLY) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I210_COPPER) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I210_FIBER) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I210_SERDES) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I210_SGMII) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I210_COPPER_FLASHLESS) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I210_SERDES_FLASHLESS) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I211_TOOLS_ONLY) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I211_COPPER) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I350_COPPER) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I350_FIBER) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I350_SERDES) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_I350_SGMII) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_COPPER) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_FIBER) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_QUAD_FIBER) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_SERDES) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_SGMII) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82580_COPPER_DUAL) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_DH89XXCC_SGMII) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_DH89XXCC_SERDES) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_DH89XXCC_BACKPLANE) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_DH89XXCC_SFP) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576_NS) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576_NS_SERDES) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576_FIBER) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576_SERDES) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576_SERDES_QUAD) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576_QUAD_COPPER_ET2) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82576_QUAD_COPPER) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82575EB_COPPER) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82575EB_FIBER_SERDES) },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_82575GB_QUAD_COPPER) },
 	/* required last entry */
 	{0, }
 };
@@ -105,66 +128,116 @@ static int igb_setup_all_rx_resources(struct igb_adapter *);
 static void igb_free_all_tx_resources(struct igb_adapter *);
 static void igb_free_all_rx_resources(struct igb_adapter *);
 static void igb_setup_mrqc(struct igb_adapter *);
+void igb_update_stats(struct igb_adapter *);
 static int igb_probe(struct pci_dev *, const struct pci_device_id *);
 static void __devexit igb_remove(struct pci_dev *pdev);
-static void igb_init_hw_timer(struct igb_adapter *adapter);
 static int igb_sw_init(struct igb_adapter *);
 static int igb_open(struct net_device *);
 static int igb_close(struct net_device *);
+static void igb_configure(struct igb_adapter *);
 static void igb_configure_tx(struct igb_adapter *);
 static void igb_configure_rx(struct igb_adapter *);
 static void igb_clean_all_tx_rings(struct igb_adapter *);
 static void igb_clean_all_rx_rings(struct igb_adapter *);
 static void igb_clean_tx_ring(struct igb_ring *);
-static void igb_clean_rx_ring(struct igb_ring *);
 static void igb_set_rx_mode(struct net_device *);
 static void igb_update_phy_info(unsigned long);
 static void igb_watchdog(unsigned long);
 static void igb_watchdog_task(struct work_struct *);
-static netdev_tx_t igb_xmit_frame_adv(struct sk_buff *skb, struct net_device *);
-static struct rtnl_link_stats64 *igb_get_stats64(struct net_device *dev,
-						 struct rtnl_link_stats64 *stats);
+static void igb_dma_err_task(struct work_struct *);
+static void igb_dma_err_timer(unsigned long data);
+static netdev_tx_t igb_xmit_frame(struct sk_buff *skb, struct net_device *);
+static struct net_device_stats *igb_get_stats(struct net_device *);
 static int igb_change_mtu(struct net_device *, int);
+void igb_full_sync_mac_table(struct igb_adapter *adapter);
 static int igb_set_mac(struct net_device *, void *);
 static void igb_set_uta(struct igb_adapter *adapter);
 static irqreturn_t igb_intr(int irq, void *);
 static irqreturn_t igb_intr_msi(int irq, void *);
 static irqreturn_t igb_msix_other(int irq, void *);
+static void igb_rar_set_qsel(struct igb_adapter *, u8 *, u32, u8);
 static irqreturn_t igb_msix_ring(int irq, void *);
-#ifdef CONFIG_IGB_DCA
+#ifdef IGB_DCA
 static void igb_update_dca(struct igb_q_vector *);
 static void igb_setup_dca(struct igb_adapter *);
-#endif /* CONFIG_IGB_DCA */
-static bool igb_clean_tx_irq(struct igb_q_vector *);
+#endif /* IGB_DCA */
 static int igb_poll(struct napi_struct *, int);
-static bool igb_clean_rx_irq_adv(struct igb_q_vector *, int *, int);
+static bool igb_clean_tx_irq(struct igb_q_vector *);
+static bool igb_clean_rx_irq(struct igb_q_vector *, int);
 static int igb_ioctl(struct net_device *, struct ifreq *, int cmd);
 static void igb_tx_timeout(struct net_device *);
 static void igb_reset_task(struct work_struct *);
-static void igb_vlan_mode(struct net_device *netdev, u32 features);
+#ifdef HAVE_VLAN_RX_REGISTER
+static void igb_vlan_mode(struct net_device *, struct vlan_group *);
+#endif
+#ifdef HAVE_INT_NDO_VLAN_RX_ADD_VID
+#ifdef NETIF_F_HW_VLAN_CTAG_RX
+static int igb_vlan_rx_add_vid(struct net_device *,
+			       __always_unused __be16 proto, u16);
+static int igb_vlan_rx_kill_vid(struct net_device *,
+			        __always_unused __be16 proto, u16);
+#else
+static int igb_vlan_rx_add_vid(struct net_device *, u16);
+static int igb_vlan_rx_kill_vid(struct net_device *, u16);
+#endif
+#else
 static void igb_vlan_rx_add_vid(struct net_device *, u16);
 static void igb_vlan_rx_kill_vid(struct net_device *, u16);
+#endif
 static void igb_restore_vlan(struct igb_adapter *);
-static void igb_rar_set_qsel(struct igb_adapter *, u8 *, u32 , u8);
+void igb_rar_set(struct igb_adapter *adapter, u32 index);
 static void igb_ping_all_vfs(struct igb_adapter *);
 static void igb_msg_task(struct igb_adapter *);
 static void igb_vmm_control(struct igb_adapter *);
 static int igb_set_vf_mac(struct igb_adapter *, int, unsigned char *);
 static void igb_restore_vf_multicasts(struct igb_adapter *adapter);
-static int igb_ndo_set_vf_mac(struct net_device *netdev, int vf, u8 *mac);
+static void igb_process_mdd_event(struct igb_adapter *);
+#ifdef IFLA_VF_MAX
+static int igb_ndo_set_vf_mac( struct net_device *netdev, int vf, u8 *mac);
 static int igb_ndo_set_vf_vlan(struct net_device *netdev,
-			       int vf, u16 vlan, u8 qos);
+				int vf, u16 vlan, u8 qos);
+#ifdef HAVE_VF_SPOOFCHK_CONFIGURE
+static int igb_ndo_set_vf_spoofchk(struct net_device *netdev, int vf,
+				bool setting);
+#endif
 static int igb_ndo_set_vf_bw(struct net_device *netdev, int vf, int tx_rate);
 static int igb_ndo_get_vf_config(struct net_device *netdev, int vf,
 				 struct ifla_vf_info *ivi);
 static void igb_check_vf_rate_limit(struct igb_adapter *);
-
-#ifdef CONFIG_PM
-static int igb_suspend(struct pci_dev *, pm_message_t);
-static int igb_resume(struct pci_dev *);
 #endif
+static int igb_vf_configure(struct igb_adapter *adapter, int vf);
+#ifdef CONFIG_PM
+#ifdef HAVE_SYSTEM_SLEEP_PM_OPS
+static int igb_suspend(struct device *dev);
+static int igb_resume(struct device *dev);
+#ifdef CONFIG_PM_RUNTIME
+static int igb_runtime_suspend(struct device *dev);
+static int igb_runtime_resume(struct device *dev);
+static int igb_runtime_idle(struct device *dev);
+#endif /* CONFIG_PM_RUNTIME */
+static const struct dev_pm_ops igb_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(igb_suspend, igb_resume)
+#ifdef CONFIG_PM_RUNTIME
+	SET_RUNTIME_PM_OPS(igb_runtime_suspend, igb_runtime_resume,
+			igb_runtime_idle)
+#endif /* CONFIG_PM_RUNTIME */
+};
+#else
+static int igb_suspend(struct pci_dev *pdev, pm_message_t state);
+static int igb_resume(struct pci_dev *pdev);
+#endif /* HAVE_SYSTEM_SLEEP_PM_OPS */
+#endif /* CONFIG_PM */
+#ifndef USE_REBOOT_NOTIFIER
 static void igb_shutdown(struct pci_dev *);
-#ifdef CONFIG_IGB_DCA
+#else
+static int igb_notify_reboot(struct notifier_block *, unsigned long, void *);
+static struct notifier_block igb_notifier_reboot = {
+	.notifier_call	= igb_notify_reboot,
+	.next		= NULL,
+	.priority	= 0
+};
+#endif
+#ifdef IGB_DCA
 static int igb_notify_dca(struct notifier_block *, unsigned long, void *);
 static struct notifier_block dca_notifier = {
 	.notifier_call	= igb_notify_dca,
@@ -176,13 +249,8 @@ static struct notifier_block dca_notifier = {
 /* for netdump / net console */
 static void igb_netpoll(struct net_device *);
 #endif
-#ifdef CONFIG_PCI_IOV
-static unsigned int max_vfs = 0;
-module_param(max_vfs, uint, 0);
-MODULE_PARM_DESC(max_vfs, "Maximum number of virtual functions to allocate "
-                 "per physical function");
-#endif /* CONFIG_PCI_IOV */
 
+#ifdef HAVE_PCI_ERS
 static pci_ers_result_t igb_io_error_detected(struct pci_dev *,
 		     pci_channel_state_t);
 static pci_ers_result_t igb_io_slot_reset(struct pci_dev *);
@@ -193,7 +261,10 @@ static struct pci_error_handlers igb_err_handler = {
 	.slot_reset = igb_io_slot_reset,
 	.resume = igb_io_resume,
 };
+#endif
 
+static void igb_init_fw(struct igb_adapter *adapter);
+static void igb_init_dmac(struct igb_adapter *adapter, u32 pba);
 
 static struct pci_driver igb_driver = {
 	.name     = igb_driver_name,
@@ -201,12 +272,19 @@ static struct pci_driver igb_driver = {
 	.probe    = igb_probe,
 	.remove   = __devexit_p(igb_remove),
 #ifdef CONFIG_PM
-	/* Power Management Hooks */
+#ifdef HAVE_SYSTEM_SLEEP_PM_OPS
+	.driver.pm = &igb_pm_ops,
+#else
 	.suspend  = igb_suspend,
 	.resume   = igb_resume,
-#endif
+#endif /* HAVE_SYSTEM_SLEEP_PM_OPS */
+#endif /* CONFIG_PM */
+#ifndef USE_REBOOT_NOTIFIER
 	.shutdown = igb_shutdown,
+#endif
+#ifdef HAVE_PCI_ERS
 	.err_handler = &igb_err_handler
+#endif
 };
 
 MODULE_AUTHOR("Intel Corporation, <e1000-devel@lists.sourceforge.net>");
@@ -214,372 +292,71 @@ MODULE_DESCRIPTION("Intel(R) Gigabit Ethernet Network Driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_VERSION);
 
-struct igb_reg_info {
-	u32 ofs;
-	char *name;
-};
-
-static const struct igb_reg_info igb_reg_info_tbl[] = {
-
-	/* General Registers */
-	{E1000_CTRL, "CTRL"},
-	{E1000_STATUS, "STATUS"},
-	{E1000_CTRL_EXT, "CTRL_EXT"},
-
-	/* Interrupt Registers */
-	{E1000_ICR, "ICR"},
-
-	/* RX Registers */
-	{E1000_RCTL, "RCTL"},
-	{E1000_RDLEN(0), "RDLEN"},
-	{E1000_RDH(0), "RDH"},
-	{E1000_RDT(0), "RDT"},
-	{E1000_RXDCTL(0), "RXDCTL"},
-	{E1000_RDBAL(0), "RDBAL"},
-	{E1000_RDBAH(0), "RDBAH"},
-
-	/* TX Registers */
-	{E1000_TCTL, "TCTL"},
-	{E1000_TDBAL(0), "TDBAL"},
-	{E1000_TDBAH(0), "TDBAH"},
-	{E1000_TDLEN(0), "TDLEN"},
-	{E1000_TDH(0), "TDH"},
-	{E1000_TDT(0), "TDT"},
-	{E1000_TXDCTL(0), "TXDCTL"},
-	{E1000_TDFH, "TDFH"},
-	{E1000_TDFT, "TDFT"},
-	{E1000_TDFHS, "TDFHS"},
-	{E1000_TDFPC, "TDFPC"},
-
-	/* List Terminator */
-	{}
-};
-
-/*
- * igb_regdump - register printout routine
- */
-static void igb_regdump(struct e1000_hw *hw, struct igb_reg_info *reginfo)
+/* Retrieve user set MAC address */
+static int __init setup_igb_mac(char *macstr)
 {
-	int n = 0;
-	char rname[16];
-	u32 regs[8];
-
-	switch (reginfo->ofs) {
-	case E1000_RDLEN(0):
-		for (n = 0; n < 4; n++)
-			regs[n] = rd32(E1000_RDLEN(n));
-		break;
-	case E1000_RDH(0):
-		for (n = 0; n < 4; n++)
-			regs[n] = rd32(E1000_RDH(n));
-		break;
-	case E1000_RDT(0):
-		for (n = 0; n < 4; n++)
-			regs[n] = rd32(E1000_RDT(n));
-		break;
-	case E1000_RXDCTL(0):
-		for (n = 0; n < 4; n++)
-			regs[n] = rd32(E1000_RXDCTL(n));
-		break;
-	case E1000_RDBAL(0):
-		for (n = 0; n < 4; n++)
-			regs[n] = rd32(E1000_RDBAL(n));
-		break;
-	case E1000_RDBAH(0):
-		for (n = 0; n < 4; n++)
-			regs[n] = rd32(E1000_RDBAH(n));
-		break;
-	case E1000_TDBAL(0):
-		for (n = 0; n < 4; n++)
-			regs[n] = rd32(E1000_RDBAL(n));
-		break;
-	case E1000_TDBAH(0):
-		for (n = 0; n < 4; n++)
-			regs[n] = rd32(E1000_TDBAH(n));
-		break;
-	case E1000_TDLEN(0):
-		for (n = 0; n < 4; n++)
-			regs[n] = rd32(E1000_TDLEN(n));
-		break;
-	case E1000_TDH(0):
-		for (n = 0; n < 4; n++)
-			regs[n] = rd32(E1000_TDH(n));
-		break;
-	case E1000_TDT(0):
-		for (n = 0; n < 4; n++)
-			regs[n] = rd32(E1000_TDT(n));
-		break;
-	case E1000_TXDCTL(0):
-		for (n = 0; n < 4; n++)
-			regs[n] = rd32(E1000_TXDCTL(n));
-		break;
-	default:
-		printk(KERN_INFO "%-15s %08x\n",
-			reginfo->name, rd32(reginfo->ofs));
-		return;
-	}
-
-	snprintf(rname, 16, "%s%s", reginfo->name, "[0-3]");
-	printk(KERN_INFO "%-15s ", rname);
-	for (n = 0; n < 4; n++)
-		printk(KERN_CONT "%08x ", regs[n]);
-	printk(KERN_CONT "\n");
-}
-
-/*
- * igb_dump - Print registers, tx-rings and rx-rings
- */
-static void igb_dump(struct igb_adapter *adapter)
-{
-	struct net_device *netdev = adapter->netdev;
-	struct e1000_hw *hw = &adapter->hw;
-	struct igb_reg_info *reginfo;
-	int n = 0;
-	struct igb_ring *tx_ring;
-	union e1000_adv_tx_desc *tx_desc;
-	struct my_u0 { u64 a; u64 b; } *u0;
-	struct igb_buffer *buffer_info;
-	struct igb_ring *rx_ring;
-	union e1000_adv_rx_desc *rx_desc;
-	u32 staterr;
-	int i = 0;
-
-	if (!netif_msg_hw(adapter))
-		return;
-
-	/* Print netdevice Info */
-	if (netdev) {
-		dev_info(&adapter->pdev->dev, "Net device Info\n");
-		printk(KERN_INFO "Device Name     state            "
-			"trans_start      last_rx\n");
-		printk(KERN_INFO "%-15s %016lX %016lX %016lX\n",
-		netdev->name,
-		netdev->state,
-		netdev->trans_start,
-		netdev->last_rx);
-	}
-
-	/* Print Registers */
-	dev_info(&adapter->pdev->dev, "Register Dump\n");
-	printk(KERN_INFO " Register Name   Value\n");
-	for (reginfo = (struct igb_reg_info *)igb_reg_info_tbl;
-	     reginfo->name; reginfo++) {
-		igb_regdump(hw, reginfo);
-	}
-
-	/* Print TX Ring Summary */
-	if (!netdev || !netif_running(netdev))
-		goto exit;
-
-	dev_info(&adapter->pdev->dev, "TX Rings Summary\n");
-	printk(KERN_INFO "Queue [NTU] [NTC] [bi(ntc)->dma  ]"
-		" leng ntw timestamp\n");
-	for (n = 0; n < adapter->num_tx_queues; n++) {
-		tx_ring = adapter->tx_ring[n];
-		buffer_info = &tx_ring->buffer_info[tx_ring->next_to_clean];
-		printk(KERN_INFO " %5d %5X %5X %016llX %04X %3X %016llX\n",
-			   n, tx_ring->next_to_use, tx_ring->next_to_clean,
-			   (u64)buffer_info->dma,
-			   buffer_info->length,
-			   buffer_info->next_to_watch,
-			   (u64)buffer_info->time_stamp);
-	}
-
-	/* Print TX Rings */
-	if (!netif_msg_tx_done(adapter))
-		goto rx_ring_summary;
-
-	dev_info(&adapter->pdev->dev, "TX Rings Dump\n");
-
-	/* Transmit Descriptor Formats
-	 *
-	 * Advanced Transmit Descriptor
-	 *   +--------------------------------------------------------------+
-	 * 0 |         Buffer Address [63:0]                                |
-	 *   +--------------------------------------------------------------+
-	 * 8 | PAYLEN  | PORTS  |CC|IDX | STA | DCMD  |DTYP|MAC|RSV| DTALEN |
-	 *   +--------------------------------------------------------------+
-	 *   63      46 45    40 39 38 36 35 32 31   24             15       0
-	 */
-
-	for (n = 0; n < adapter->num_tx_queues; n++) {
-		tx_ring = adapter->tx_ring[n];
-		printk(KERN_INFO "------------------------------------\n");
-		printk(KERN_INFO "TX QUEUE INDEX = %d\n", tx_ring->queue_index);
-		printk(KERN_INFO "------------------------------------\n");
-		printk(KERN_INFO "T [desc]     [address 63:0  ] "
-			"[PlPOCIStDDM Ln] [bi->dma       ] "
-			"leng  ntw timestamp        bi->skb\n");
-
-		for (i = 0; tx_ring->desc && (i < tx_ring->count); i++) {
-			tx_desc = E1000_TX_DESC_ADV(*tx_ring, i);
-			buffer_info = &tx_ring->buffer_info[i];
-			u0 = (struct my_u0 *)tx_desc;
-			printk(KERN_INFO "T [0x%03X]    %016llX %016llX %016llX"
-				" %04X  %3X %016llX %p", i,
-				le64_to_cpu(u0->a),
-				le64_to_cpu(u0->b),
-				(u64)buffer_info->dma,
-				buffer_info->length,
-				buffer_info->next_to_watch,
-				(u64)buffer_info->time_stamp,
-				buffer_info->skb);
-			if (i == tx_ring->next_to_use &&
-				i == tx_ring->next_to_clean)
-				printk(KERN_CONT " NTC/U\n");
-			else if (i == tx_ring->next_to_use)
-				printk(KERN_CONT " NTU\n");
-			else if (i == tx_ring->next_to_clean)
-				printk(KERN_CONT " NTC\n");
-			else
-				printk(KERN_CONT "\n");
-
-			if (netif_msg_pktdata(adapter) && buffer_info->dma != 0)
-				print_hex_dump(KERN_INFO, "",
-					DUMP_PREFIX_ADDRESS,
-					16, 1, phys_to_virt(buffer_info->dma),
-					buffer_info->length, true);
+	int i, j;
+	unsigned char result, value;
+
+	for (i = 0; i < ETH_ALEN; i++) {
+		result = 0;
+
+		if (i != 5 && *(macstr + 2) != ':')
+			return -1;
+
+		for (j = 0; j < 2; j++) {
+			if (isxdigit(*macstr)
+			    && (value =
+				isdigit(*macstr) ? *macstr -
+				'0' : toupper(*macstr) - 'A' + 10) < 16) {
+				result = result * 16 + value;
+				macstr++;
+			} else
+				return -1;
 		}
-	}
 
-	/* Print RX Rings Summary */
-rx_ring_summary:
-	dev_info(&adapter->pdev->dev, "RX Rings Summary\n");
-	printk(KERN_INFO "Queue [NTU] [NTC]\n");
-	for (n = 0; n < adapter->num_rx_queues; n++) {
-		rx_ring = adapter->rx_ring[n];
-		printk(KERN_INFO " %5d %5X %5X\n", n,
-			   rx_ring->next_to_use, rx_ring->next_to_clean);
+		macstr++;
+		g_mac_addr[i] = result;
 	}
 
-	/* Print RX Rings */
-	if (!netif_msg_rx_status(adapter))
-		goto exit;
-
-	dev_info(&adapter->pdev->dev, "RX Rings Dump\n");
-
-	/* Advanced Receive Descriptor (Read) Format
-	 *    63                                           1        0
-	 *    +-----------------------------------------------------+
-	 *  0 |       Packet Buffer Address [63:1]           |A0/NSE|
-	 *    +----------------------------------------------+------+
-	 *  8 |       Header Buffer Address [63:1]           |  DD  |
-	 *    +-----------------------------------------------------+
-	 *
-	 *
-	 * Advanced Receive Descriptor (Write-Back) Format
-	 *
-	 *   63       48 47    32 31  30      21 20 17 16   4 3     0
-	 *   +------------------------------------------------------+
-	 * 0 | Packet     IP     |SPH| HDR_LEN   | RSV|Packet|  RSS |
-	 *   | Checksum   Ident  |   |           |    | Type | Type |
-	 *   +------------------------------------------------------+
-	 * 8 | VLAN Tag | Length | Extended Error | Extended Status |
-	 *   +------------------------------------------------------+
-	 *   63       48 47    32 31            20 19               0
-	 */
-
-	for (n = 0; n < adapter->num_rx_queues; n++) {
-		rx_ring = adapter->rx_ring[n];
-		printk(KERN_INFO "------------------------------------\n");
-		printk(KERN_INFO "RX QUEUE INDEX = %d\n", rx_ring->queue_index);
-		printk(KERN_INFO "------------------------------------\n");
-		printk(KERN_INFO "R  [desc]      [ PktBuf     A0] "
-			"[  HeadBuf   DD] [bi->dma       ] [bi->skb] "
-			"<-- Adv Rx Read format\n");
-		printk(KERN_INFO "RWB[desc]      [PcsmIpSHl PtRs] "
-			"[vl er S cks ln] ---------------- [bi->skb] "
-			"<-- Adv Rx Write-Back format\n");
-
-		for (i = 0; i < rx_ring->count; i++) {
-			buffer_info = &rx_ring->buffer_info[i];
-			rx_desc = E1000_RX_DESC_ADV(*rx_ring, i);
-			u0 = (struct my_u0 *)rx_desc;
-			staterr = le32_to_cpu(rx_desc->wb.upper.status_error);
-			if (staterr & E1000_RXD_STAT_DD) {
-				/* Descriptor Done */
-				printk(KERN_INFO "RWB[0x%03X]     %016llX "
-					"%016llX ---------------- %p", i,
-					le64_to_cpu(u0->a),
-					le64_to_cpu(u0->b),
-					buffer_info->skb);
-			} else {
-				printk(KERN_INFO "R  [0x%03X]     %016llX "
-					"%016llX %016llX %p", i,
-					le64_to_cpu(u0->a),
-					le64_to_cpu(u0->b),
-					(u64)buffer_info->dma,
-					buffer_info->skb);
-
-				if (netif_msg_pktdata(adapter)) {
-					print_hex_dump(KERN_INFO, "",
-						DUMP_PREFIX_ADDRESS,
-						16, 1,
-						phys_to_virt(buffer_info->dma),
-						rx_ring->rx_buffer_len, true);
-					if (rx_ring->rx_buffer_len
-						< IGB_RXBUFFER_1024)
-						print_hex_dump(KERN_INFO, "",
-						  DUMP_PREFIX_ADDRESS,
-						  16, 1,
-						  phys_to_virt(
-						    buffer_info->page_dma +
-						    buffer_info->page_offset),
-						  PAGE_SIZE/2, true);
-				}
-			}
-
-			if (i == rx_ring->next_to_use)
-				printk(KERN_CONT " NTU\n");
-			else if (i == rx_ring->next_to_clean)
-				printk(KERN_CONT " NTC\n");
-			else
-				printk(KERN_CONT "\n");
-
-		}
-	}
+	g_usr_mac = 1;
 
-exit:
-	return;
+	return 0;
 }
 
+__setup("igb_mac=", setup_igb_mac);
 
-/**
- * igb_read_clock - read raw cycle counter (to be used by time counter)
- */
-static cycle_t igb_read_clock(const struct cyclecounter *tc)
+static void igb_vfta_set(struct igb_adapter *adapter, u32 vid, bool add)
 {
-	struct igb_adapter *adapter =
-		container_of(tc, struct igb_adapter, cycles);
 	struct e1000_hw *hw = &adapter->hw;
-	u64 stamp = 0;
-	int shift = 0;
+	struct e1000_host_mng_dhcp_cookie *mng_cookie = &hw->mng_cookie;
+	u32 index = (vid >> E1000_VFTA_ENTRY_SHIFT) & E1000_VFTA_ENTRY_MASK;
+	u32 mask = 1 << (vid & E1000_VFTA_ENTRY_BIT_SHIFT_MASK);
+	u32 vfta;
 
 	/*
-	 * The timestamp latches on lowest register read. For the 82580
-	 * the lowest register is SYSTIMR instead of SYSTIML.  However we never
-	 * adjusted TIMINCA so SYSTIMR will just read as all 0s so ignore it.
+	 * if this is the management vlan the only option is to add it in so
+	 * that the management pass through will continue to work
 	 */
-	if (hw->mac.type == e1000_82580) {
-		stamp = rd32(E1000_SYSTIMR) >> 8;
-		shift = IGB_82580_TSYNC_SHIFT;
-	}
+	if ((mng_cookie->status & E1000_MNG_DHCP_COOKIE_STATUS_VLAN) &&
+	    (vid == mng_cookie->vlan_id))
+		add = TRUE;
 
-	stamp |= (u64)rd32(E1000_SYSTIML) << shift;
-	stamp |= (u64)rd32(E1000_SYSTIMH) << (shift + 32);
-	return stamp;
-}
+	vfta = adapter->shadow_vfta[index];
 
-/**
- * igb_get_hw_dev - return device
- * used by hardware layer to print debugging information
- **/
-struct net_device *igb_get_hw_dev(struct e1000_hw *hw)
-{
-	struct igb_adapter *adapter = hw->back;
-	return adapter->netdev;
+	if (add)
+		vfta |= mask;
+	else
+		vfta &= ~mask;
+
+	e1000_write_vfta(hw, index, vfta);
+	adapter->shadow_vfta[index] = vfta;
 }
 
+static int debug = NETIF_MSG_DRV | NETIF_MSG_PROBE;
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug level (0=none, ..., 16=all)");
+
 /**
  * igb_init_module - Driver Registration Routine
  *
@@ -589,15 +366,29 @@ struct net_device *igb_get_hw_dev(struct e1000_hw *hw)
 static int __init igb_init_module(void)
 {
 	int ret;
+
 	printk(KERN_INFO "%s - version %s\n",
 	       igb_driver_string, igb_driver_version);
 
 	printk(KERN_INFO "%s\n", igb_copyright);
+#ifdef IGB_HWMON
+/* only use IGB_PROCFS if IGB_HWMON is not defined */
+#else
+#ifdef IGB_PROCFS
+	if (igb_procfs_topdir_init())
+		printk(KERN_INFO "Procfs failed to initialize topdir\n");
+#endif /* IGB_PROCFS */
+#endif /* IGB_HWMON  */
 
-#ifdef CONFIG_IGB_DCA
+#ifdef IGB_DCA
 	dca_register_notify(&dca_notifier);
 #endif
 	ret = pci_register_driver(&igb_driver);
+#ifdef USE_REBOOT_NOTIFIER
+	if (ret >= 0) {
+		register_reboot_notifier(&igb_notifier_reboot);
+	}
+#endif
 	return ret;
 }
 
@@ -611,10 +402,21 @@ module_init(igb_init_module);
  **/
 static void __exit igb_exit_module(void)
 {
-#ifdef CONFIG_IGB_DCA
+#ifdef IGB_DCA
 	dca_unregister_notify(&dca_notifier);
 #endif
+#ifdef USE_REBOOT_NOTIFIER
+	unregister_reboot_notifier(&igb_notifier_reboot);
+#endif
 	pci_unregister_driver(&igb_driver);
+
+#ifdef IGB_HWMON
+/* only compile IGB_PROCFS if IGB_HWMON is not defined */
+#else
+#ifdef IGB_PROCFS
+	igb_procfs_topdir_exit();
+#endif /* IGB_PROCFS */
+#endif /* IGB_HWMON */
 }
 
 module_exit(igb_exit_module);
@@ -639,7 +441,7 @@ static void igb_cache_ring_register(struct igb_adapter *adapter)
 		 * In order to avoid collision we start at the first free queue
 		 * and continue consuming queues in the same sequence
 		 */
-		if (adapter->vfs_allocated_count) {
+		if ((adapter->rss_queues > 1) && adapter->vmdq_pools) {
 			for (; i < adapter->rss_queues; i++)
 				adapter->rx_ring[i]->reg_idx = rbase_offset +
 				                               Q_IDX_82576(i);
@@ -647,6 +449,9 @@ static void igb_cache_ring_register(struct igb_adapter *adapter)
 	case e1000_82575:
 	case e1000_82580:
 	case e1000_i350:
+	case e1000_i354:
+	case e1000_i210:
+	case e1000_i211:
 	default:
 		for (; i < adapter->num_rx_queues; i++)
 			adapter->rx_ring[i]->reg_idx = rbase_offset + i;
@@ -656,88 +461,81 @@ static void igb_cache_ring_register(struct igb_adapter *adapter)
 	}
 }
 
-static void igb_free_queues(struct igb_adapter *adapter)
+static void igb_configure_lli(struct igb_adapter *adapter)
 {
-	int i;
+	struct e1000_hw *hw = &adapter->hw;
+	u16 port;
 
-	for (i = 0; i < adapter->num_tx_queues; i++) {
-		kfree(adapter->tx_ring[i]);
-		adapter->tx_ring[i] = NULL;
+	/* LLI should only be enabled for MSI-X or MSI interrupts */
+	if (!adapter->msix_entries && !(adapter->flags & IGB_FLAG_HAS_MSI))
+		return;
+
+	if (adapter->lli_port) {
+		/* use filter 0 for port */
+		port = htons((u16)adapter->lli_port);
+		E1000_WRITE_REG(hw, E1000_IMIR(0),
+			(port | E1000_IMIR_PORT_IM_EN));
+		E1000_WRITE_REG(hw, E1000_IMIREXT(0),
+			(E1000_IMIREXT_SIZE_BP | E1000_IMIREXT_CTRL_BP));
 	}
-	for (i = 0; i < adapter->num_rx_queues; i++) {
-		kfree(adapter->rx_ring[i]);
-		adapter->rx_ring[i] = NULL;
+
+	if (adapter->flags & IGB_FLAG_LLI_PUSH) {
+		/* use filter 1 for push flag */
+		E1000_WRITE_REG(hw, E1000_IMIR(1),
+			(E1000_IMIR_PORT_BP | E1000_IMIR_PORT_IM_EN));
+		E1000_WRITE_REG(hw, E1000_IMIREXT(1),
+			(E1000_IMIREXT_SIZE_BP | E1000_IMIREXT_CTRL_PSH));
 	}
-	adapter->num_rx_queues = 0;
-	adapter->num_tx_queues = 0;
+
+	if (adapter->lli_size) {
+		/* use filter 2 for size */
+		E1000_WRITE_REG(hw, E1000_IMIR(2),
+			(E1000_IMIR_PORT_BP | E1000_IMIR_PORT_IM_EN));
+		E1000_WRITE_REG(hw, E1000_IMIREXT(2),
+			(adapter->lli_size | E1000_IMIREXT_CTRL_BP));
+	}
+
 }
 
 /**
- * igb_alloc_queues - Allocate memory for all rings
- * @adapter: board private structure to initialize
+ *  igb_write_ivar - configure ivar for given MSI-X vector
+ *  @hw: pointer to the HW structure
+ *  @msix_vector: vector number we are allocating to a given ring
+ *  @index: row index of IVAR register to write within IVAR table
+ *  @offset: column offset of in IVAR, should be multiple of 8
  *
- * We allocate one ring per queue at run-time since we don't know the
- * number of queues at compile-time.
+ *  This function is intended to handle the writing of the IVAR register
+ *  for adapters 82576 and newer.  The IVAR table consists of 2 columns,
+ *  each containing an cause allocation for an Rx and Tx ring, and a
+ *  variable number of rows depending on the number of queues supported.
  **/
-static int igb_alloc_queues(struct igb_adapter *adapter)
+static void igb_write_ivar(struct e1000_hw *hw, int msix_vector,
+			   int index, int offset)
 {
-	struct igb_ring *ring;
-	int i;
+	u32 ivar = E1000_READ_REG_ARRAY(hw, E1000_IVAR0, index);
 
-	for (i = 0; i < adapter->num_tx_queues; i++) {
-		ring = kzalloc(sizeof(struct igb_ring), GFP_KERNEL);
-		if (!ring)
-			goto err;
-		ring->count = adapter->tx_ring_count;
-		ring->queue_index = i;
-		ring->dev = &adapter->pdev->dev;
-		ring->netdev = adapter->netdev;
-		/* For 82575, context index must be unique per ring. */
-		if (adapter->hw.mac.type == e1000_82575)
-			ring->flags = IGB_RING_FLAG_TX_CTX_IDX;
-		adapter->tx_ring[i] = ring;
-	}
-
-	for (i = 0; i < adapter->num_rx_queues; i++) {
-		ring = kzalloc(sizeof(struct igb_ring), GFP_KERNEL);
-		if (!ring)
-			goto err;
-		ring->count = adapter->rx_ring_count;
-		ring->queue_index = i;
-		ring->dev = &adapter->pdev->dev;
-		ring->netdev = adapter->netdev;
-		ring->rx_buffer_len = MAXIMUM_ETHERNET_VLAN_SIZE;
-		ring->flags = IGB_RING_FLAG_RX_CSUM; /* enable rx checksum */
-		/* set flag indicating ring supports SCTP checksum offload */
-		if (adapter->hw.mac.type >= e1000_82576)
-			ring->flags |= IGB_RING_FLAG_RX_SCTP_CSUM;
-		adapter->rx_ring[i] = ring;
-	}
+	/* clear any bits that are currently set */
+	ivar &= ~((u32)0xFF << offset);
 
-	igb_cache_ring_register(adapter);
+	/* write vector and valid bit */
+	ivar |= (msix_vector | E1000_IVAR_VALID) << offset;
 
-	return 0;
-
-err:
-	igb_free_queues(adapter);
-
-	return -ENOMEM;
+	E1000_WRITE_REG_ARRAY(hw, E1000_IVAR0, index, ivar);
 }
 
 #define IGB_N0_QUEUE -1
 static void igb_assign_vector(struct igb_q_vector *q_vector, int msix_vector)
 {
-	u32 msixbm = 0;
 	struct igb_adapter *adapter = q_vector->adapter;
 	struct e1000_hw *hw = &adapter->hw;
-	u32 ivar, index;
 	int rx_queue = IGB_N0_QUEUE;
 	int tx_queue = IGB_N0_QUEUE;
+	u32 msixbm = 0;
 
-	if (q_vector->rx_ring)
-		rx_queue = q_vector->rx_ring->reg_idx;
-	if (q_vector->tx_ring)
-		tx_queue = q_vector->tx_ring->reg_idx;
+	if (q_vector->rx.ring)
+		rx_queue = q_vector->rx.ring->reg_idx;
+	if (q_vector->tx.ring)
+		tx_queue = q_vector->tx.ring->reg_idx;
 
 	switch (hw->mac.type) {
 	case e1000_82575:
@@ -751,76 +549,46 @@ static void igb_assign_vector(struct igb_q_vector *q_vector, int msix_vector)
 			msixbm |= E1000_EICR_TX_QUEUE0 << tx_queue;
 		if (!adapter->msix_entries && msix_vector == 0)
 			msixbm |= E1000_EIMS_OTHER;
-		array_wr32(E1000_MSIXBM(0), msix_vector, msixbm);
+		E1000_WRITE_REG_ARRAY(hw, E1000_MSIXBM(0), msix_vector, msixbm);
 		q_vector->eims_value = msixbm;
 		break;
 	case e1000_82576:
-		/* 82576 uses a table-based method for assigning vectors.
-		   Each queue has a single entry in the table to which we write
-		   a vector number along with a "valid" bit.  Sadly, the layout
-		   of the table is somewhat counterintuitive. */
-		if (rx_queue > IGB_N0_QUEUE) {
-			index = (rx_queue & 0x7);
-			ivar = array_rd32(E1000_IVAR0, index);
-			if (rx_queue < 8) {
-				/* vector goes into low byte of register */
-				ivar = ivar & 0xFFFFFF00;
-				ivar |= msix_vector | E1000_IVAR_VALID;
-			} else {
-				/* vector goes into third byte of register */
-				ivar = ivar & 0xFF00FFFF;
-				ivar |= (msix_vector | E1000_IVAR_VALID) << 16;
-			}
-			array_wr32(E1000_IVAR0, index, ivar);
-		}
-		if (tx_queue > IGB_N0_QUEUE) {
-			index = (tx_queue & 0x7);
-			ivar = array_rd32(E1000_IVAR0, index);
-			if (tx_queue < 8) {
-				/* vector goes into second byte of register */
-				ivar = ivar & 0xFFFF00FF;
-				ivar |= (msix_vector | E1000_IVAR_VALID) << 8;
-			} else {
-				/* vector goes into high byte of register */
-				ivar = ivar & 0x00FFFFFF;
-				ivar |= (msix_vector | E1000_IVAR_VALID) << 24;
-			}
-			array_wr32(E1000_IVAR0, index, ivar);
-		}
+		/*
+		 * 82576 uses a table that essentially consists of 2 columns
+		 * with 8 rows.  The ordering is column-major so we use the
+		 * lower 3 bits as the row index, and the 4th bit as the
+		 * column offset.
+		 */
+		if (rx_queue > IGB_N0_QUEUE)
+			igb_write_ivar(hw, msix_vector,
+				       rx_queue & 0x7,
+				       (rx_queue & 0x8) << 1);
+		if (tx_queue > IGB_N0_QUEUE)
+			igb_write_ivar(hw, msix_vector,
+				       tx_queue & 0x7,
+				       ((tx_queue & 0x8) << 1) + 8);
 		q_vector->eims_value = 1 << msix_vector;
 		break;
 	case e1000_82580:
 	case e1000_i350:
-		/* 82580 uses the same table-based approach as 82576 but has fewer
-		   entries as a result we carry over for queues greater than 4. */
-		if (rx_queue > IGB_N0_QUEUE) {
-			index = (rx_queue >> 1);
-			ivar = array_rd32(E1000_IVAR0, index);
-			if (rx_queue & 0x1) {
-				/* vector goes into third byte of register */
-				ivar = ivar & 0xFF00FFFF;
-				ivar |= (msix_vector | E1000_IVAR_VALID) << 16;
-			} else {
-				/* vector goes into low byte of register */
-				ivar = ivar & 0xFFFFFF00;
-				ivar |= msix_vector | E1000_IVAR_VALID;
-			}
-			array_wr32(E1000_IVAR0, index, ivar);
-		}
-		if (tx_queue > IGB_N0_QUEUE) {
-			index = (tx_queue >> 1);
-			ivar = array_rd32(E1000_IVAR0, index);
-			if (tx_queue & 0x1) {
-				/* vector goes into high byte of register */
-				ivar = ivar & 0x00FFFFFF;
-				ivar |= (msix_vector | E1000_IVAR_VALID) << 24;
-			} else {
-				/* vector goes into second byte of register */
-				ivar = ivar & 0xFFFF00FF;
-				ivar |= (msix_vector | E1000_IVAR_VALID) << 8;
-			}
-			array_wr32(E1000_IVAR0, index, ivar);
-		}
+	case e1000_i354:
+	case e1000_i210:
+	case e1000_i211:
+		/*
+		 * On 82580 and newer adapters the scheme is similar to 82576
+		 * however instead of ordering column-major we have things
+		 * ordered row-major.  So we traverse the table by using
+		 * bit 0 as the column offset, and the remaining bits as the
+		 * row index.
+		 */
+		if (rx_queue > IGB_N0_QUEUE)
+			igb_write_ivar(hw, msix_vector,
+				       rx_queue >> 1,
+				       (rx_queue & 0x1) << 4);
+		if (tx_queue > IGB_N0_QUEUE)
+			igb_write_ivar(hw, msix_vector,
+				       tx_queue >> 1,
+				       ((tx_queue & 0x1) << 4) + 8);
 		q_vector->eims_value = 1 << msix_vector;
 		break;
 	default:
@@ -852,7 +620,7 @@ static void igb_configure_msix(struct igb_adapter *adapter)
 	/* set vector for other causes, i.e. link changes */
 	switch (hw->mac.type) {
 	case e1000_82575:
-		tmp = rd32(E1000_CTRL_EXT);
+		tmp = E1000_READ_REG(hw, E1000_CTRL_EXT);
 		/* enable MSI-X PBA support*/
 		tmp |= E1000_CTRL_EXT_PBA_CLR;
 
@@ -860,10 +628,10 @@ static void igb_configure_msix(struct igb_adapter *adapter)
 		tmp |= E1000_CTRL_EXT_EIAME;
 		tmp |= E1000_CTRL_EXT_IRCA;
 
-		wr32(E1000_CTRL_EXT, tmp);
+		E1000_WRITE_REG(hw, E1000_CTRL_EXT, tmp);
 
 		/* enable msix_other interrupt */
-		array_wr32(E1000_MSIXBM(0), vector++,
+		E1000_WRITE_REG_ARRAY(hw, E1000_MSIXBM(0), vector++,
 		                      E1000_EIMS_OTHER);
 		adapter->eims_other = E1000_EIMS_OTHER;
 
@@ -872,9 +640,12 @@ static void igb_configure_msix(struct igb_adapter *adapter)
 	case e1000_82576:
 	case e1000_82580:
 	case e1000_i350:
+	case e1000_i354:
+	case e1000_i210:
+	case e1000_i211:
 		/* Turn on MSI-X capability first, or our settings
 		 * won't stick.  And it will take days to debug. */
-		wr32(E1000_GPIE, E1000_GPIE_MSIX_MODE |
+		E1000_WRITE_REG(hw, E1000_GPIE, E1000_GPIE_MSIX_MODE |
 		                E1000_GPIE_PBA | E1000_GPIE_EIAME |
 		                E1000_GPIE_NSICR);
 
@@ -882,7 +653,7 @@ static void igb_configure_msix(struct igb_adapter *adapter)
 		adapter->eims_other = 1 << vector;
 		tmp = (vector++ | E1000_IVAR_VALID) << 8;
 
-		wr32(E1000_IVAR_MISC, tmp);
+		E1000_WRITE_REG(hw, E1000_IVAR_MISC, tmp);
 		break;
 	default:
 		/* do nothing, since nothing else supports MSI-X */
@@ -894,7 +665,7 @@ static void igb_configure_msix(struct igb_adapter *adapter)
 	for (i = 0; i < adapter->num_q_vectors; i++)
 		igb_assign_vector(adapter->q_vector[i], vector++);
 
-	wrfl();
+	E1000_WRITE_FLUSH(hw);
 }
 
 /**
@@ -907,28 +678,29 @@ static int igb_request_msix(struct igb_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
 	struct e1000_hw *hw = &adapter->hw;
-	int i, err = 0, vector = 0;
+	int i, err = 0, vector = 0, free_vector = 0;
 
 	err = request_irq(adapter->msix_entries[vector].vector,
-	                  igb_msix_other, 0, netdev->name, adapter);
+	                  &igb_msix_other, 0, netdev->name, adapter);
 	if (err)
-		goto out;
-	vector++;
+		goto err_out;
 
 	for (i = 0; i < adapter->num_q_vectors; i++) {
 		struct igb_q_vector *q_vector = adapter->q_vector[i];
 
+		vector++;
+
 		q_vector->itr_register = hw->hw_addr + E1000_EITR(vector);
 
-		if (q_vector->rx_ring && q_vector->tx_ring)
+		if (q_vector->rx.ring && q_vector->tx.ring)
 			sprintf(q_vector->name, "%s-TxRx-%u", netdev->name,
-			        q_vector->rx_ring->queue_index);
-		else if (q_vector->tx_ring)
+			        q_vector->rx.ring->queue_index);
+		else if (q_vector->tx.ring)
 			sprintf(q_vector->name, "%s-tx-%u", netdev->name,
-			        q_vector->tx_ring->queue_index);
-		else if (q_vector->rx_ring)
+			        q_vector->tx.ring->queue_index);
+		else if (q_vector->rx.ring)
 			sprintf(q_vector->name, "%s-rx-%u", netdev->name,
-			        q_vector->rx_ring->queue_index);
+			        q_vector->rx.ring->queue_index);
 		else
 			sprintf(q_vector->name, "%s-unused", netdev->name);
 
@@ -936,13 +708,22 @@ static int igb_request_msix(struct igb_adapter *adapter)
 		                  igb_msix_ring, 0, q_vector->name,
 		                  q_vector);
 		if (err)
-			goto out;
-		vector++;
+			goto err_free;
 	}
 
 	igb_configure_msix(adapter);
 	return 0;
-out:
+
+err_free:
+	/* free already assigned IRQs */
+	free_irq(adapter->msix_entries[free_vector++].vector, adapter);
+
+	vector--;
+	for (i = 0; i < vector; i++) {
+		free_irq(adapter->msix_entries[free_vector++].vector,
+			 adapter->q_vector[i]);
+	}
+err_out:
 	return err;
 }
 
@@ -958,6 +739,34 @@ static void igb_reset_interrupt_capability(struct igb_adapter *adapter)
 }
 
 /**
+ * igb_free_q_vector - Free memory allocated for specific interrupt vector
+ * @adapter: board private structure to initialize
+ * @v_idx: Index of vector to be freed
+ *
+ * This function frees the memory allocated to the q_vector.  In addition if
+ * NAPI is enabled it will delete any references to the NAPI struct prior
+ * to freeing the q_vector.
+ **/
+static void igb_free_q_vector(struct igb_adapter *adapter, int v_idx)
+{
+	struct igb_q_vector *q_vector = adapter->q_vector[v_idx];
+
+	if (q_vector->tx.ring)
+		adapter->tx_ring[q_vector->tx.ring->queue_index] = NULL;
+
+	if (q_vector->rx.ring)
+		adapter->tx_ring[q_vector->rx.ring->queue_index] = NULL;
+
+	adapter->q_vector[v_idx] = NULL;
+	netif_napi_del(&q_vector->napi);
+#ifndef IGB_NO_LRO
+	__skb_queue_purge(&q_vector->lrolist.active);
+#endif
+
+	kfree_rcu(q_vector, rcu);
+}
+
+/**
  * igb_free_q_vectors - Free memory allocated for interrupt vectors
  * @adapter: board private structure to initialize
  *
@@ -967,17 +776,14 @@ static void igb_reset_interrupt_capability(struct igb_adapter *adapter)
  **/
 static void igb_free_q_vectors(struct igb_adapter *adapter)
 {
-	int v_idx;
+	int v_idx = adapter->num_q_vectors;
 
-	for (v_idx = 0; v_idx < adapter->num_q_vectors; v_idx++) {
-		struct igb_q_vector *q_vector = adapter->q_vector[v_idx];
-		adapter->q_vector[v_idx] = NULL;
-		if (!q_vector)
-			continue;
-		netif_napi_del(&q_vector->napi);
-		kfree(q_vector);
-	}
+	adapter->num_tx_queues = 0;
+	adapter->num_rx_queues = 0;
 	adapter->num_q_vectors = 0;
+
+	while (v_idx--)
+		igb_free_q_vector(adapter, v_idx);
 }
 
 /**
@@ -988,174 +794,477 @@ static void igb_free_q_vectors(struct igb_adapter *adapter)
  */
 static void igb_clear_interrupt_scheme(struct igb_adapter *adapter)
 {
-	igb_free_queues(adapter);
 	igb_free_q_vectors(adapter);
 	igb_reset_interrupt_capability(adapter);
 }
 
 /**
- * igb_set_interrupt_capability - set MSI or MSI-X if supported
+ * igb_process_mdd_event
+ * @adapter - board private structure
  *
- * Attempt to configure interrupts using the best available
- * capabilities of the hardware and kernel.
- **/
-static int igb_set_interrupt_capability(struct igb_adapter *adapter)
+ * Identify a malicious VF, disable the VF TX/RX queues and log a message.
+ */
+static void igb_process_mdd_event(struct igb_adapter *adapter)
 {
-	int err;
-	int numvecs, i;
+	struct e1000_hw *hw = &adapter->hw;
+	u32 lvmmc, vfte, vfre, mdfb;
+	u8 vf_queue;
 
-	/* Number of supported queues. */
-	adapter->num_rx_queues = adapter->rss_queues;
-	if (adapter->vfs_allocated_count)
-		adapter->num_tx_queues = 1;
-	else
-		adapter->num_tx_queues = adapter->rss_queues;
+	lvmmc = E1000_READ_REG(hw, E1000_LVMMC);
+	vf_queue = lvmmc >> 29;
 
-	/* start with one vector for every rx queue */
-	numvecs = adapter->num_rx_queues;
+	/* VF index cannot be bigger or equal to VFs allocated */
+	if (vf_queue >= adapter->vfs_allocated_count)
+		return;
 
-	/* if tx handler is separate add 1 for every tx queue */
-	if (!(adapter->flags & IGB_FLAG_QUEUE_PAIRS))
-		numvecs += adapter->num_tx_queues;
+	netdev_info(adapter->netdev,
+	            "VF %d misbehaved. VF queues are disabled. "
+	            "VM misbehavior code is 0x%x\n", vf_queue, lvmmc);
 
-	/* store the number of vectors reserved for queues */
-	adapter->num_q_vectors = numvecs;
+	/* Disable VFTE and VFRE related bits */
+	vfte = E1000_READ_REG(hw, E1000_VFTE);
+	vfte &= ~(1 << vf_queue);
+	E1000_WRITE_REG(hw, E1000_VFTE, vfte);
 
-	/* add 1 vector for link status interrupts */
-	numvecs++;
-	adapter->msix_entries = kcalloc(numvecs, sizeof(struct msix_entry),
-					GFP_KERNEL);
-	if (!adapter->msix_entries)
-		goto msi_only;
+	vfre = E1000_READ_REG(hw, E1000_VFRE);
+	vfre &= ~(1 << vf_queue);
+	E1000_WRITE_REG(hw, E1000_VFRE, vfre);
 
-	for (i = 0; i < numvecs; i++)
-		adapter->msix_entries[i].entry = i;
+	/* Disable MDFB related bit. Clear on write */
+	mdfb = E1000_READ_REG(hw, E1000_MDFB);
+	mdfb |= (1 << vf_queue);
+	E1000_WRITE_REG(hw, E1000_MDFB, mdfb);
 
-	err = pci_enable_msix(adapter->pdev,
-			      adapter->msix_entries,
-			      numvecs);
-	if (err == 0)
-		goto out;
+	/* Reset the specific VF */
+	E1000_WRITE_REG(hw, E1000_VTCTRL(vf_queue), E1000_VTCTRL_RST);
+}
 
-	igb_reset_interrupt_capability(adapter);
+/**
+ * igb_disable_mdd
+ * @adapter - board private structure
+ *
+ * Disable MDD behavior in the HW
+ **/
+static void igb_disable_mdd(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u32 reg;
 
-	/* If we can't do MSI-X, try MSI */
-msi_only:
-#ifdef CONFIG_PCI_IOV
-	/* disable SR-IOV for non MSI-X configurations */
+	if ((hw->mac.type != e1000_i350) ||
+	    (hw->mac.type != e1000_i354))
+		return;
+
+	reg = E1000_READ_REG(hw, E1000_DTXCTL);
+	reg &= (~E1000_DTXCTL_MDP_EN);
+	E1000_WRITE_REG(hw, E1000_DTXCTL, reg);
+}
+
+/**
+ * igb_enable_mdd
+ * @adapter - board private structure
+ *
+ * Enable the HW to detect malicious driver and sends an interrupt to
+ * the driver.
+ **/
+static void igb_enable_mdd(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u32 reg;
+
+	/* Only available on i350 device */
+	if (hw->mac.type != e1000_i350)
+		return;
+
+	reg = E1000_READ_REG(hw, E1000_DTXCTL);
+	reg |= E1000_DTXCTL_MDP_EN;
+	E1000_WRITE_REG(hw, E1000_DTXCTL, reg);
+}
+
+/**
+ * igb_reset_sriov_capability - disable SR-IOV if enabled
+ *
+ * Attempt to disable single root IO virtualization capabilites present in the
+ * kernel.
+ **/
+static void igb_reset_sriov_capability(struct igb_adapter *adapter)
+{
+	struct pci_dev *pdev = adapter->pdev;
+	struct e1000_hw *hw = &adapter->hw;
+
+	/* reclaim resources allocated to VFs */
 	if (adapter->vf_data) {
-		struct e1000_hw *hw = &adapter->hw;
-		/* disable iov and allow time for transactions to clear */
-		pci_disable_sriov(adapter->pdev);
-		msleep(500);
+		if (!pci_vfs_assigned(pdev)) {
+			/*
+			 * disable iov and allow time for transactions to
+			 * clear
+			 */
+			pci_disable_sriov(pdev);
+			msleep(500);
+
+			dev_info(pci_dev_to_dev(pdev), "IOV Disabled\n");
+		} else {
+			dev_info(pci_dev_to_dev(pdev), "IOV Not Disabled\n "
+					"VF(s) are assigned to guests!\n");
+		}
+		/* Disable Malicious Driver Detection */
+		igb_disable_mdd(adapter);
 
+		/* free vf data storage */
 		kfree(adapter->vf_data);
 		adapter->vf_data = NULL;
-		wr32(E1000_IOVCTL, E1000_IOVCTL_REUSE_VFQ);
-		wrfl();
+
+		/* switch rings back to PF ownership */
+		E1000_WRITE_REG(hw, E1000_IOVCTL,
+				E1000_IOVCTL_REUSE_VFQ);
+		E1000_WRITE_FLUSH(hw);
 		msleep(100);
-		dev_info(&adapter->pdev->dev, "IOV Disabled\n");
 	}
-#endif
+
 	adapter->vfs_allocated_count = 0;
-	adapter->rss_queues = 1;
-	adapter->flags |= IGB_FLAG_QUEUE_PAIRS;
-	adapter->num_rx_queues = 1;
-	adapter->num_tx_queues = 1;
-	adapter->num_q_vectors = 1;
-	if (!pci_enable_msi(adapter->pdev))
-		adapter->flags |= IGB_FLAG_HAS_MSI;
-out:
-	/* Notify the stack of the (possibly) reduced queue counts. */
-	netif_set_real_num_tx_queues(adapter->netdev, adapter->num_tx_queues);
-	return netif_set_real_num_rx_queues(adapter->netdev,
-					    adapter->num_rx_queues);
 }
 
 /**
- * igb_alloc_q_vectors - Allocate memory for interrupt vectors
- * @adapter: board private structure to initialize
+ * igb_set_sriov_capability - setup SR-IOV if supported
  *
- * We allocate one q_vector per queue interrupt.  If allocation fails we
- * return -ENOMEM.
+ * Attempt to enable single root IO virtualization capabilites present in the
+ * kernel.
  **/
-static int igb_alloc_q_vectors(struct igb_adapter *adapter)
+static void igb_set_sriov_capability(struct igb_adapter *adapter)
 {
-	struct igb_q_vector *q_vector;
-	struct e1000_hw *hw = &adapter->hw;
-	int v_idx;
+	struct pci_dev *pdev = adapter->pdev;
+	int old_vfs = 0;
+	int i;
+
+	old_vfs = pci_num_vf(pdev);
+	if (old_vfs) {
+		dev_info(pci_dev_to_dev(pdev),
+				"%d pre-allocated VFs found - override "
+				"max_vfs setting of %d\n", old_vfs,
+				adapter->vfs_allocated_count);
+		adapter->vfs_allocated_count = old_vfs;
+	}
+	/* no VFs requested, do nothing */
+	if (!adapter->vfs_allocated_count)
+		return;
+
+	/* allocate vf data storage */
+	adapter->vf_data = kcalloc(adapter->vfs_allocated_count,
+	                           sizeof(struct vf_data_storage),
+	                           GFP_KERNEL);
+
+	if (adapter->vf_data) {
+		if (!old_vfs) {
+			if (pci_enable_sriov(pdev,
+					adapter->vfs_allocated_count))
+				goto err_out;
+		}
+		for (i = 0; i < adapter->vfs_allocated_count; i++)
+			igb_vf_configure(adapter, i);
+
+		switch (adapter->hw.mac.type) {
+		case e1000_82576:
+		case e1000_i350:
+			/* Enable VM to VM loopback by default */
+			adapter->flags |= IGB_FLAG_LOOPBACK_ENABLE;
+			break;
+		default:
+			/* Currently no other hardware supports loopback */
+			break;
+		}
+
+		/* DMA Coalescing is not supported in IOV mode. */
+		if (adapter->hw.mac.type >= e1000_i350)
+		adapter->dmac = IGB_DMAC_DISABLE;
+		if (adapter->hw.mac.type < e1000_i350)
+		adapter->flags |= IGB_FLAG_DETECT_BAD_DMA;
+		return;
 
-	for (v_idx = 0; v_idx < adapter->num_q_vectors; v_idx++) {
-		q_vector = kzalloc(sizeof(struct igb_q_vector), GFP_KERNEL);
-		if (!q_vector)
-			goto err_out;
-		q_vector->adapter = adapter;
-		q_vector->itr_register = hw->hw_addr + E1000_EITR(0);
-		q_vector->itr_val = IGB_START_ITR;
-		netif_napi_add(adapter->netdev, &q_vector->napi, igb_poll, 64);
-		adapter->q_vector[v_idx] = q_vector;
 	}
-	return 0;
 
 err_out:
-	igb_free_q_vectors(adapter);
-	return -ENOMEM;
+	kfree(adapter->vf_data);
+	adapter->vf_data = NULL;
+	adapter->vfs_allocated_count = 0;
+	dev_warn(pci_dev_to_dev(pdev),
+			"Failed to initialize SR-IOV virtualization\n");
 }
 
-static void igb_map_rx_ring_to_vector(struct igb_adapter *adapter,
-                                      int ring_idx, int v_idx)
+/**
+ * igb_set_interrupt_capability - set MSI or MSI-X if supported
+ *
+ * Attempt to configure interrupts using the best available
+ * capabilities of the hardware and kernel.
+ **/
+static void igb_set_interrupt_capability(struct igb_adapter *adapter, bool msix)
 {
-	struct igb_q_vector *q_vector = adapter->q_vector[v_idx];
+	struct pci_dev *pdev = adapter->pdev;
+	int err;
+	int numvecs, i;
+
+	if (!msix)
+		adapter->int_mode = IGB_INT_MODE_MSI;
+
+	/* Number of supported queues. */
+	adapter->num_rx_queues = adapter->rss_queues;
+
+	if (adapter->vmdq_pools > 1)
+		adapter->num_rx_queues += adapter->vmdq_pools - 1;
 
-	q_vector->rx_ring = adapter->rx_ring[ring_idx];
-	q_vector->rx_ring->q_vector = q_vector;
-	q_vector->itr_val = adapter->rx_itr_setting;
-	if (q_vector->itr_val && q_vector->itr_val <= 3)
-		q_vector->itr_val = IGB_START_ITR;
+#ifdef HAVE_TX_MQ
+	if (adapter->vmdq_pools)
+		adapter->num_tx_queues = adapter->vmdq_pools;
+	else
+		adapter->num_tx_queues = adapter->num_rx_queues;
+#else
+	adapter->num_tx_queues = max_t(u32, 1, adapter->vmdq_pools);
+#endif
+
+	switch (adapter->int_mode) {
+	case IGB_INT_MODE_MSIX:
+		/* start with one vector for every Tx/Rx queue */
+		numvecs = max_t(int, adapter->num_tx_queues,
+				adapter->num_rx_queues);
+
+		/* if tx handler is seperate make it 1 for every queue */
+		if (!(adapter->flags & IGB_FLAG_QUEUE_PAIRS))
+			numvecs = adapter->num_tx_queues +
+				adapter->num_rx_queues;
+
+		/* store the number of vectors reserved for queues */
+		adapter->num_q_vectors = numvecs;
+
+		/* add 1 vector for link status interrupts */
+		numvecs++;
+		adapter->msix_entries = kcalloc(numvecs,
+		                                sizeof(struct msix_entry),
+		                                GFP_KERNEL);
+		if (adapter->msix_entries) {
+			for (i = 0; i < numvecs; i++)
+				adapter->msix_entries[i].entry = i;
+
+			err = pci_enable_msix(pdev,
+			                      adapter->msix_entries, numvecs);
+			if (err == 0)
+				break;
+		}
+		/* MSI-X failed, so fall through and try MSI */
+		dev_warn(pci_dev_to_dev(pdev), "Failed to initialize MSI-X interrupts. "
+		         "Falling back to MSI interrupts.\n");
+		igb_reset_interrupt_capability(adapter);
+	case IGB_INT_MODE_MSI:
+		if (!pci_enable_msi(pdev))
+			adapter->flags |= IGB_FLAG_HAS_MSI;
+		else
+			dev_warn(pci_dev_to_dev(pdev), "Failed to initialize MSI "
+			         "interrupts.  Falling back to legacy "
+			         "interrupts.\n");
+		/* Fall through */
+	case IGB_INT_MODE_LEGACY:
+		/* disable advanced features and set number of queues to 1 */
+		igb_reset_sriov_capability(adapter);
+		adapter->vmdq_pools = 0;
+		adapter->rss_queues = 1;
+		adapter->flags |= IGB_FLAG_QUEUE_PAIRS;
+		adapter->num_rx_queues = 1;
+		adapter->num_tx_queues = 1;
+		adapter->num_q_vectors = 1;
+		/* Don't do anything; this is system default */
+		break;
+	}
 }
 
-static void igb_map_tx_ring_to_vector(struct igb_adapter *adapter,
-                                      int ring_idx, int v_idx)
+static void igb_add_ring(struct igb_ring *ring,
+			 struct igb_ring_container *head)
 {
-	struct igb_q_vector *q_vector = adapter->q_vector[v_idx];
-
-	q_vector->tx_ring = adapter->tx_ring[ring_idx];
-	q_vector->tx_ring->q_vector = q_vector;
-	q_vector->itr_val = adapter->tx_itr_setting;
-	if (q_vector->itr_val && q_vector->itr_val <= 3)
-		q_vector->itr_val = IGB_START_ITR;
+	head->ring = ring;
+	head->count++;
 }
 
 /**
- * igb_map_ring_to_vector - maps allocated queues to vectors
+ * igb_alloc_q_vector - Allocate memory for a single interrupt vector
+ * @adapter: board private structure to initialize
+ * @v_count: q_vectors allocated on adapter, used for ring interleaving
+ * @v_idx: index of vector in adapter struct
+ * @txr_count: total number of Tx rings to allocate
+ * @txr_idx: index of first Tx ring to allocate
+ * @rxr_count: total number of Rx rings to allocate
+ * @rxr_idx: index of first Rx ring to allocate
  *
- * This function maps the recently allocated queues to vectors.
+ * We allocate one q_vector.  If allocation fails we return -ENOMEM.
  **/
-static int igb_map_ring_to_vector(struct igb_adapter *adapter)
+static int igb_alloc_q_vector(struct igb_adapter *adapter,
+			      unsigned int v_count, unsigned int v_idx,
+			      unsigned int txr_count, unsigned int txr_idx,
+			      unsigned int rxr_count, unsigned int rxr_idx)
 {
-	int i;
-	int v_idx = 0;
+	struct igb_q_vector *q_vector;
+	struct igb_ring *ring;
+	int ring_count, size;
+
+	/* igb only supports 1 Tx and/or 1 Rx queue per vector */
+	if (txr_count > 1 || rxr_count > 1)
+		return -ENOMEM;
+
+	ring_count = txr_count + rxr_count;
+	size = sizeof(struct igb_q_vector) +
+	       (sizeof(struct igb_ring) * ring_count);
 
-	if ((adapter->num_q_vectors < adapter->num_rx_queues) ||
-	    (adapter->num_q_vectors < adapter->num_tx_queues))
+	/* allocate q_vector and rings */
+	q_vector = kzalloc(size, GFP_KERNEL);
+	if (!q_vector)
 		return -ENOMEM;
 
-	if (adapter->num_q_vectors >=
-	    (adapter->num_rx_queues + adapter->num_tx_queues)) {
-		for (i = 0; i < adapter->num_rx_queues; i++)
-			igb_map_rx_ring_to_vector(adapter, i, v_idx++);
-		for (i = 0; i < adapter->num_tx_queues; i++)
-			igb_map_tx_ring_to_vector(adapter, i, v_idx++);
+#ifndef IGB_NO_LRO
+	/* initialize LRO */
+	__skb_queue_head_init(&q_vector->lrolist.active);
+
+#endif
+	/* initialize NAPI */
+	netif_napi_add(adapter->netdev, &q_vector->napi,
+		       igb_poll, 64);
+
+	/* tie q_vector and adapter together */
+	adapter->q_vector[v_idx] = q_vector;
+	q_vector->adapter = adapter;
+
+	/* initialize work limits */
+	q_vector->tx.work_limit = adapter->tx_work_limit;
+
+	/* initialize ITR configuration */
+	q_vector->itr_register = adapter->hw.hw_addr + E1000_EITR(0);
+	q_vector->itr_val = IGB_START_ITR;
+
+	/* initialize pointer to rings */
+	ring = q_vector->ring;
+
+	/* intialize ITR */
+	if (rxr_count) {
+		/* rx or rx/tx vector */
+		if (!adapter->rx_itr_setting || adapter->rx_itr_setting > 3)
+			q_vector->itr_val = adapter->rx_itr_setting;
 	} else {
-		for (i = 0; i < adapter->num_rx_queues; i++) {
-			if (i < adapter->num_tx_queues)
-				igb_map_tx_ring_to_vector(adapter, i, v_idx);
-			igb_map_rx_ring_to_vector(adapter, i, v_idx++);
+		/* tx only vector */
+		if (!adapter->tx_itr_setting || adapter->tx_itr_setting > 3)
+			q_vector->itr_val = adapter->tx_itr_setting;
+	}
+
+	if (txr_count) {
+		/* assign generic ring traits */
+		ring->dev = &adapter->pdev->dev;
+		ring->netdev = adapter->netdev;
+
+		/* configure backlink on ring */
+		ring->q_vector = q_vector;
+
+		/* update q_vector Tx values */
+		igb_add_ring(ring, &q_vector->tx);
+
+		/* For 82575, context index must be unique per ring. */
+		if (adapter->hw.mac.type == e1000_82575)
+			set_bit(IGB_RING_FLAG_TX_CTX_IDX, &ring->flags);
+
+		/* apply Tx specific ring traits */
+		ring->count = adapter->tx_ring_count;
+		ring->queue_index = txr_idx;
+
+		/* assign ring to adapter */
+		adapter->tx_ring[txr_idx] = ring;
+
+		/* push pointer to next ring */
+		ring++;
+	}
+
+	if (rxr_count) {
+		/* assign generic ring traits */
+		ring->dev = &adapter->pdev->dev;
+		ring->netdev = adapter->netdev;
+
+		/* configure backlink on ring */
+		ring->q_vector = q_vector;
+
+		/* update q_vector Rx values */
+		igb_add_ring(ring, &q_vector->rx);
+
+#ifndef HAVE_NDO_SET_FEATURES
+		/* enable rx checksum */
+		set_bit(IGB_RING_FLAG_RX_CSUM, &ring->flags);
+
+#endif
+		/* set flag indicating ring supports SCTP checksum offload */
+		if (adapter->hw.mac.type >= e1000_82576)
+			set_bit(IGB_RING_FLAG_RX_SCTP_CSUM, &ring->flags);
+
+		if ((adapter->hw.mac.type == e1000_i350) ||
+		    (adapter->hw.mac.type == e1000_i354))
+			set_bit(IGB_RING_FLAG_RX_LB_VLAN_BSWAP, &ring->flags);
+
+		/* apply Rx specific ring traits */
+		ring->count = adapter->rx_ring_count;
+		ring->queue_index = rxr_idx;
+
+		/* assign ring to adapter */
+		adapter->rx_ring[rxr_idx] = ring;
+	}
+
+	return 0;
+}
+
+/**
+ * igb_alloc_q_vectors - Allocate memory for interrupt vectors
+ * @adapter: board private structure to initialize
+ *
+ * We allocate one q_vector per queue interrupt.  If allocation fails we
+ * return -ENOMEM.
+ **/
+static int igb_alloc_q_vectors(struct igb_adapter *adapter)
+{
+	int q_vectors = adapter->num_q_vectors;
+	int rxr_remaining = adapter->num_rx_queues;
+	int txr_remaining = adapter->num_tx_queues;
+	int rxr_idx = 0, txr_idx = 0, v_idx = 0;
+	int err;
+
+	if (q_vectors >= (rxr_remaining + txr_remaining)) {
+		for (; rxr_remaining; v_idx++) {
+			err = igb_alloc_q_vector(adapter, q_vectors, v_idx,
+						 0, 0, 1, rxr_idx);
+
+			if (err)
+				goto err_out;
+
+			/* update counts and index */
+			rxr_remaining--;
+			rxr_idx++;
 		}
-		for (; i < adapter->num_tx_queues; i++)
-			igb_map_tx_ring_to_vector(adapter, i, v_idx++);
 	}
+
+	for (; v_idx < q_vectors; v_idx++) {
+		int rqpv = DIV_ROUND_UP(rxr_remaining, q_vectors - v_idx);
+		int tqpv = DIV_ROUND_UP(txr_remaining, q_vectors - v_idx);
+		err = igb_alloc_q_vector(adapter, q_vectors, v_idx,
+					 tqpv, txr_idx, rqpv, rxr_idx);
+
+		if (err)
+			goto err_out;
+
+		/* update counts and index */
+		rxr_remaining -= rqpv;
+		txr_remaining -= tqpv;
+		rxr_idx++;
+		txr_idx++;
+	}
+
 	return 0;
+
+err_out:
+	adapter->num_tx_queues = 0;
+	adapter->num_rx_queues = 0;
+	adapter->num_q_vectors = 0;
+
+	while (v_idx--)
+		igb_free_q_vector(adapter, v_idx);
+
+	return -ENOMEM;
 }
 
 /**
@@ -1163,39 +1272,23 @@ static int igb_map_ring_to_vector(struct igb_adapter *adapter)
  *
  * This function initializes the interrupts and allocates all of the queues.
  **/
-static int igb_init_interrupt_scheme(struct igb_adapter *adapter)
+static int igb_init_interrupt_scheme(struct igb_adapter *adapter, bool msix)
 {
 	struct pci_dev *pdev = adapter->pdev;
 	int err;
 
-	err = igb_set_interrupt_capability(adapter);
-	if (err)
-		return err;
+	igb_set_interrupt_capability(adapter, msix);
 
 	err = igb_alloc_q_vectors(adapter);
 	if (err) {
-		dev_err(&pdev->dev, "Unable to allocate memory for vectors\n");
+		dev_err(pci_dev_to_dev(pdev), "Unable to allocate memory for vectors\n");
 		goto err_alloc_q_vectors;
 	}
 
-	err = igb_alloc_queues(adapter);
-	if (err) {
-		dev_err(&pdev->dev, "Unable to allocate memory for queues\n");
-		goto err_alloc_queues;
-	}
-
-	err = igb_map_ring_to_vector(adapter);
-	if (err) {
-		dev_err(&pdev->dev, "Invalid q_vector to ring mapping\n");
-		goto err_map_queues;
-	}
-
+	igb_cache_ring_register(adapter);
 
 	return 0;
-err_map_queues:
-	igb_free_queues(adapter);
-err_alloc_queues:
-	igb_free_q_vectors(adapter);
+
 err_alloc_q_vectors:
 	igb_reset_interrupt_capability(adapter);
 	return err;
@@ -1218,35 +1311,23 @@ static int igb_request_irq(struct igb_adapter *adapter)
 		if (!err)
 			goto request_done;
 		/* fall back to MSI */
-		igb_clear_interrupt_scheme(adapter);
-		if (!pci_enable_msi(adapter->pdev))
-			adapter->flags |= IGB_FLAG_HAS_MSI;
 		igb_free_all_tx_resources(adapter);
 		igb_free_all_rx_resources(adapter);
-		adapter->num_tx_queues = 1;
-		adapter->num_rx_queues = 1;
-		adapter->num_q_vectors = 1;
-		err = igb_alloc_q_vectors(adapter);
-		if (err) {
-			dev_err(&pdev->dev,
-			        "Unable to allocate memory for vectors\n");
-			goto request_done;
-		}
-		err = igb_alloc_queues(adapter);
-		if (err) {
-			dev_err(&pdev->dev,
-			        "Unable to allocate memory for queues\n");
-			igb_free_q_vectors(adapter);
+
+		igb_clear_interrupt_scheme(adapter);
+		igb_reset_sriov_capability(adapter);
+		err = igb_init_interrupt_scheme(adapter, false);
+		if (err)
 			goto request_done;
-		}
 		igb_setup_all_tx_resources(adapter);
 		igb_setup_all_rx_resources(adapter);
-	} else {
-		igb_assign_vector(adapter->q_vector[0], 0);
+		igb_configure(adapter);
 	}
 
+	igb_assign_vector(adapter->q_vector[0], 0);
+
 	if (adapter->flags & IGB_FLAG_HAS_MSI) {
-		err = request_irq(adapter->pdev->irq, igb_intr_msi, 0,
+		err = request_irq(pdev->irq, &igb_intr_msi, 0,
 				  netdev->name, adapter);
 		if (!err)
 			goto request_done;
@@ -1256,11 +1337,11 @@ static int igb_request_irq(struct igb_adapter *adapter)
 		adapter->flags &= ~IGB_FLAG_HAS_MSI;
 	}
 
-	err = request_irq(adapter->pdev->irq, igb_intr, IRQF_SHARED,
+	err = request_irq(pdev->irq, &igb_intr, IRQF_SHARED,
 			  netdev->name, adapter);
 
 	if (err)
-		dev_err(&adapter->pdev->dev, "Error %d getting interrupt\n",
+		dev_err(pci_dev_to_dev(pdev), "Error %d getting interrupt\n",
 			err);
 
 request_done:
@@ -1274,11 +1355,9 @@ static void igb_free_irq(struct igb_adapter *adapter)
 
 		free_irq(adapter->msix_entries[vector++].vector, adapter);
 
-		for (i = 0; i < adapter->num_q_vectors; i++) {
-			struct igb_q_vector *q_vector = adapter->q_vector[i];
+		for (i = 0; i < adapter->num_q_vectors; i++)
 			free_irq(adapter->msix_entries[vector++].vector,
-			         q_vector);
-		}
+			         adapter->q_vector[i]);
 	} else {
 		free_irq(adapter->pdev->irq, adapter);
 	}
@@ -1298,20 +1377,24 @@ static void igb_irq_disable(struct igb_adapter *adapter)
 	 * issues on the VF drivers so we only need to clear what we set
 	 */
 	if (adapter->msix_entries) {
-		u32 regval = rd32(E1000_EIAM);
-		wr32(E1000_EIAM, regval & ~adapter->eims_enable_mask);
-		wr32(E1000_EIMC, adapter->eims_enable_mask);
-		regval = rd32(E1000_EIAC);
-		wr32(E1000_EIAC, regval & ~adapter->eims_enable_mask);
+		u32 regval = E1000_READ_REG(hw, E1000_EIAM);
+		E1000_WRITE_REG(hw, E1000_EIAM, regval & ~adapter->eims_enable_mask);
+		E1000_WRITE_REG(hw, E1000_EIMC, adapter->eims_enable_mask);
+		regval = E1000_READ_REG(hw, E1000_EIAC);
+		E1000_WRITE_REG(hw, E1000_EIAC, regval & ~adapter->eims_enable_mask);
 	}
 
-	wr32(E1000_IAM, 0);
-	wr32(E1000_IMC, ~0);
-	wrfl();
+	E1000_WRITE_REG(hw, E1000_IAM, 0);
+	E1000_WRITE_REG(hw, E1000_IMC, ~0);
+	E1000_WRITE_FLUSH(hw);
+
 	if (adapter->msix_entries) {
-		int i;
+		int vector = 0, i;
+
+		synchronize_irq(adapter->msix_entries[vector++].vector);
+
 		for (i = 0; i < adapter->num_q_vectors; i++)
-			synchronize_irq(adapter->msix_entries[i].vector);
+			synchronize_irq(adapter->msix_entries[vector++].vector);
 	} else {
 		synchronize_irq(adapter->pdev->irq);
 	}
@@ -1326,24 +1409,25 @@ static void igb_irq_enable(struct igb_adapter *adapter)
 	struct e1000_hw *hw = &adapter->hw;
 
 	if (adapter->msix_entries) {
-		u32 ims = E1000_IMS_LSC | E1000_IMS_DOUTSYNC;
-		u32 regval = rd32(E1000_EIAC);
-		wr32(E1000_EIAC, regval | adapter->eims_enable_mask);
-		regval = rd32(E1000_EIAM);
-		wr32(E1000_EIAM, regval | adapter->eims_enable_mask);
-		wr32(E1000_EIMS, adapter->eims_enable_mask);
+		u32 ims = E1000_IMS_LSC | E1000_IMS_DOUTSYNC | E1000_IMS_DRSTA;
+		u32 regval = E1000_READ_REG(hw, E1000_EIAC);
+		E1000_WRITE_REG(hw, E1000_EIAC, regval | adapter->eims_enable_mask);
+		regval = E1000_READ_REG(hw, E1000_EIAM);
+		E1000_WRITE_REG(hw, E1000_EIAM, regval | adapter->eims_enable_mask);
+		E1000_WRITE_REG(hw, E1000_EIMS, adapter->eims_enable_mask);
 		if (adapter->vfs_allocated_count) {
-			wr32(E1000_MBVFIMR, 0xFF);
+			E1000_WRITE_REG(hw, E1000_MBVFIMR, 0xFF);
 			ims |= E1000_IMS_VMMB;
+			if (adapter->mdd)
+				if ((adapter->hw.mac.type == e1000_i350) ||
+				    (adapter->hw.mac.type == e1000_i354))
+				ims |= E1000_IMS_MDDET;
 		}
-		if (adapter->hw.mac.type == e1000_82580)
-			ims |= E1000_IMS_DRSTA;
-
-		wr32(E1000_IMS, ims);
+		E1000_WRITE_REG(hw, E1000_IMS, ims);
 	} else {
-		wr32(E1000_IMS, IMS_ENABLE_MASK |
+		E1000_WRITE_REG(hw, E1000_IMS, IMS_ENABLE_MASK |
 				E1000_IMS_DRSTA);
-		wr32(E1000_IAM, IMS_ENABLE_MASK |
+		E1000_WRITE_REG(hw, E1000_IAM, IMS_ENABLE_MASK |
 				E1000_IMS_DRSTA);
 	}
 }
@@ -1356,7 +1440,7 @@ static void igb_update_mng_vlan(struct igb_adapter *adapter)
 
 	if (hw->mng_cookie.status & E1000_MNG_DHCP_COOKIE_STATUS_VLAN) {
 		/* add VID to filter table */
-		igb_vfta_set(hw, vid, true);
+		igb_vfta_set(adapter, vid, TRUE);
 		adapter->mng_vlan_id = vid;
 	} else {
 		adapter->mng_vlan_id = IGB_MNG_VLAN_NONE;
@@ -1364,9 +1448,13 @@ static void igb_update_mng_vlan(struct igb_adapter *adapter)
 
 	if ((old_vid != (u16)IGB_MNG_VLAN_NONE) &&
 	    (vid != old_vid) &&
+#ifdef HAVE_VLAN_RX_REGISTER
+	    !vlan_group_get_device(adapter->vlgrp, old_vid)) {
+#else
 	    !test_bit(old_vid, adapter->active_vlans)) {
+#endif
 		/* remove VID from filter table */
-		igb_vfta_set(hw, old_vid, false);
+		igb_vfta_set(adapter, old_vid, FALSE);
 	}
 }
 
@@ -1385,8 +1473,8 @@ static void igb_release_hw_control(struct igb_adapter *adapter)
 	u32 ctrl_ext;
 
 	/* Let firmware take over control of h/w */
-	ctrl_ext = rd32(E1000_CTRL_EXT);
-	wr32(E1000_CTRL_EXT,
+	ctrl_ext = E1000_READ_REG(hw, E1000_CTRL_EXT);
+	E1000_WRITE_REG(hw, E1000_CTRL_EXT,
 			ctrl_ext & ~E1000_CTRL_EXT_DRV_LOAD);
 }
 
@@ -1405,8 +1493,8 @@ static void igb_get_hw_control(struct igb_adapter *adapter)
 	u32 ctrl_ext;
 
 	/* Let firmware know the driver has taken over */
-	ctrl_ext = rd32(E1000_CTRL_EXT);
-	wr32(E1000_CTRL_EXT,
+	ctrl_ext = E1000_READ_REG(hw, E1000_CTRL_EXT);
+	E1000_WRITE_REG(hw, E1000_CTRL_EXT,
 			ctrl_ext | E1000_CTRL_EXT_DRV_LOAD);
 }
 
@@ -1431,14 +1519,20 @@ static void igb_configure(struct igb_adapter *adapter)
 	igb_configure_tx(adapter);
 	igb_configure_rx(adapter);
 
-	igb_rx_fifo_flush_82575(&adapter->hw);
+	e1000_rx_fifo_flush_82575(&adapter->hw);
+#ifdef CONFIG_NETDEVICES_MULTIQUEUE
+	if (adapter->num_tx_queues > 1)
+		netdev->features |= NETIF_F_MULTI_QUEUE;
+	else
+		netdev->features &= ~NETIF_F_MULTI_QUEUE;
+#endif
 
 	/* call igb_desc_unused which always leaves
 	 * at least 1 descriptor unused to make sure
 	 * next_to_use != next_to_clean */
 	for (i = 0; i < adapter->num_rx_queues; i++) {
 		struct igb_ring *ring = adapter->rx_ring[i];
-		igb_alloc_rx_buffers_adv(ring, igb_desc_unused(ring));
+		igb_alloc_rx_buffers(ring, igb_desc_unused(ring));
 	}
 }
 
@@ -1448,10 +1542,12 @@ static void igb_configure(struct igb_adapter *adapter)
  **/
 void igb_power_up_link(struct igb_adapter *adapter)
 {
+	e1000_phy_hw_reset(&adapter->hw);
+
 	if (adapter->hw.phy.media_type == e1000_media_type_copper)
-		igb_power_up_phy_copper(&adapter->hw);
+		e1000_power_up_phy(&adapter->hw);
 	else
-		igb_power_up_serdes_link_82575(&adapter->hw);
+		e1000_power_up_fiber_serdes_link(&adapter->hw);
 }
 
 /**
@@ -1461,11 +1557,194 @@ void igb_power_up_link(struct igb_adapter *adapter)
 static void igb_power_down_link(struct igb_adapter *adapter)
 {
 	if (adapter->hw.phy.media_type == e1000_media_type_copper)
-		igb_power_down_phy_copper_82575(&adapter->hw);
+		e1000_power_down_phy(&adapter->hw);
 	else
-		igb_shutdown_serdes_link_82575(&adapter->hw);
+		e1000_shutdown_fiber_serdes_link(&adapter->hw);
 }
 
+/* Detect and switch function for Media Auto Sense */
+static void igb_check_swap_media(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u32 ctrl_ext, connsw;
+	bool swap_now = false;
+	bool link;
+
+	ctrl_ext = E1000_READ_REG(hw, E1000_CTRL_EXT);
+	connsw = E1000_READ_REG(hw, E1000_CONNSW);
+	link = igb_has_link(adapter);
+
+	/* need to live swap if current media is copper and we have fiber/serdes
+	 * to go to.
+	 */
+
+	if ((hw->phy.media_type == e1000_media_type_copper) &&
+	    (!(connsw & E1000_CONNSW_AUTOSENSE_EN))) {
+		swap_now = true;
+	} else if (!(connsw & E1000_CONNSW_SERDESD)) {
+		/* copper signal takes time to appear */
+		if (adapter->copper_tries < 3) {
+			adapter->copper_tries++;
+			connsw |= E1000_CONNSW_AUTOSENSE_CONF;
+			E1000_WRITE_REG(hw, E1000_CONNSW, connsw);
+			return;
+		} else {
+			adapter->copper_tries = 0;
+			if ((connsw & E1000_CONNSW_PHYSD) &&
+			    (!(connsw & E1000_CONNSW_PHY_PDN))) {
+				swap_now = true;
+				connsw &= ~E1000_CONNSW_AUTOSENSE_CONF;
+				E1000_WRITE_REG(hw, E1000_CONNSW, connsw);
+			}
+		}
+	}
+
+	if (swap_now) {
+		switch (hw->phy.media_type) {
+		case e1000_media_type_copper:
+			dev_info(pci_dev_to_dev(adapter->pdev),
+				 "%s:MAS: changing media to fiber/serdes\n",
+			adapter->netdev->name);
+			ctrl_ext |=
+				E1000_CTRL_EXT_LINK_MODE_PCIE_SERDES;
+			adapter->flags |= IGB_FLAG_MEDIA_RESET;
+			adapter->copper_tries = 0;
+			break;
+		case e1000_media_type_internal_serdes:
+		case e1000_media_type_fiber:
+			dev_info(pci_dev_to_dev(adapter->pdev),
+				 "%s:MAS: changing media to copper\n",
+				 adapter->netdev->name);
+			ctrl_ext &=
+				~E1000_CTRL_EXT_LINK_MODE_PCIE_SERDES;
+			adapter->flags |= IGB_FLAG_MEDIA_RESET;
+			break;
+		default:
+			/* shouldn't get here during regular operation */
+			dev_err(pci_dev_to_dev(adapter->pdev),
+				"%s:AMS: Invalid media type found, returning\n",
+				adapter->netdev->name);
+			break;
+		}
+		E1000_WRITE_REG(hw, E1000_CTRL_EXT, ctrl_ext);
+	}
+}
+
+#ifdef HAVE_I2C_SUPPORT
+/*  igb_get_i2c_data - Reads the I2C SDA data bit
+ *  @hw: pointer to hardware structure
+ *  @i2cctl: Current value of I2CCTL register
+ *
+ *  Returns the I2C data bit value
+ */
+static int igb_get_i2c_data(void *data)
+{
+	struct igb_adapter *adapter = (struct igb_adapter *)data;
+	struct e1000_hw *hw = &adapter->hw;
+	s32 i2cctl = E1000_READ_REG(hw, E1000_I2CPARAMS);
+
+	return ((i2cctl & E1000_I2C_DATA_IN) != 0);
+}
+
+/* igb_set_i2c_data - Sets the I2C data bit
+ *  @data: pointer to hardware structure
+ *  @state: I2C data value (0 or 1) to set
+ *
+ *  Sets the I2C data bit
+ */
+static void igb_set_i2c_data(void *data, int state)
+{
+	struct igb_adapter *adapter = (struct igb_adapter *)data;
+	struct e1000_hw *hw = &adapter->hw;
+	s32 i2cctl = E1000_READ_REG(hw, E1000_I2CPARAMS);
+
+	if (state)
+		i2cctl |= E1000_I2C_DATA_OUT;
+	else
+		i2cctl &= ~E1000_I2C_DATA_OUT;
+
+	i2cctl &= ~E1000_I2C_DATA_OE_N;
+	i2cctl |= E1000_I2C_CLK_OE_N;
+
+	E1000_WRITE_REG(hw, E1000_I2CPARAMS, i2cctl);
+	E1000_WRITE_FLUSH(hw);
+
+}
+
+/* igb_set_i2c_clk - Sets the I2C SCL clock
+ *  @data: pointer to hardware structure
+ *  @state: state to set clock
+ *
+ *  Sets the I2C clock line to state
+ */
+static void igb_set_i2c_clk(void *data, int state)
+{
+	struct igb_adapter *adapter = (struct igb_adapter *)data;
+	struct e1000_hw *hw = &adapter->hw;
+	s32 i2cctl = E1000_READ_REG(hw, E1000_I2CPARAMS);
+
+	if (state) {
+		i2cctl |= E1000_I2C_CLK_OUT;
+		i2cctl &= ~E1000_I2C_CLK_OE_N;
+	} else {
+		i2cctl &= ~E1000_I2C_CLK_OUT;
+		i2cctl &= ~E1000_I2C_CLK_OE_N;
+	}
+	E1000_WRITE_REG(hw, E1000_I2CPARAMS, i2cctl);
+	E1000_WRITE_FLUSH(hw);
+}
+
+/* igb_get_i2c_clk - Gets the I2C SCL clock state
+ *  @data: pointer to hardware structure
+ *
+ *  Gets the I2C clock state
+ */
+static int igb_get_i2c_clk(void *data)
+{
+	struct igb_adapter *adapter = (struct igb_adapter *)data;
+	struct e1000_hw *hw = &adapter->hw;
+	s32 i2cctl = E1000_READ_REG(hw, E1000_I2CPARAMS);
+
+	return ((i2cctl & E1000_I2C_CLK_IN) != 0);
+}
+
+static const struct i2c_algo_bit_data igb_i2c_algo = {
+	.setsda		= igb_set_i2c_data,
+	.setscl		= igb_set_i2c_clk,
+	.getsda		= igb_get_i2c_data,
+	.getscl		= igb_get_i2c_clk,
+	.udelay		= 5,
+	.timeout	= 20,
+};
+
+/*  igb_init_i2c - Init I2C interface
+ *  @adapter: pointer to adapter structure
+ *
+ */
+static s32 igb_init_i2c(struct igb_adapter *adapter)
+{
+	s32 status = E1000_SUCCESS;
+
+	/* I2C interface supported on i350 devices */
+	if (adapter->hw.mac.type != e1000_i350)
+		return E1000_SUCCESS;
+
+	/* Initialize the i2c bus which is controlled by the registers.
+	 * This bus will use the i2c_algo_bit structue that implements
+	 * the protocol through toggling of the 4 bits in the register.
+	 */
+	adapter->i2c_adap.owner = THIS_MODULE;
+	adapter->i2c_algo = igb_i2c_algo;
+	adapter->i2c_algo.data = adapter;
+	adapter->i2c_adap.algo_data = &adapter->i2c_algo;
+	adapter->i2c_adap.dev.parent = &adapter->pdev->dev;
+	strlcpy(adapter->i2c_adap.name, "igb BB",
+		sizeof(adapter->i2c_adap.name));
+	status = i2c_bit_add_bus(&adapter->i2c_adap);
+	return status;
+}
+
+#endif /* HAVE_I2C_SUPPORT */
 /**
  * igb_up - Open the interface and prepare it to handle traffic
  * @adapter: board private structure
@@ -1480,32 +1759,39 @@ int igb_up(struct igb_adapter *adapter)
 
 	clear_bit(__IGB_DOWN, &adapter->state);
 
-	for (i = 0; i < adapter->num_q_vectors; i++) {
-		struct igb_q_vector *q_vector = adapter->q_vector[i];
-		napi_enable(&q_vector->napi);
-	}
+	for (i = 0; i < adapter->num_q_vectors; i++)
+		napi_enable(&(adapter->q_vector[i]->napi));
+
 	if (adapter->msix_entries)
 		igb_configure_msix(adapter);
 	else
 		igb_assign_vector(adapter->q_vector[0], 0);
 
+	igb_configure_lli(adapter);
+
 	/* Clear any pending interrupts. */
-	rd32(E1000_ICR);
+	E1000_READ_REG(hw, E1000_ICR);
 	igb_irq_enable(adapter);
 
 	/* notify VFs that reset has been completed */
 	if (adapter->vfs_allocated_count) {
-		u32 reg_data = rd32(E1000_CTRL_EXT);
+		u32 reg_data = E1000_READ_REG(hw, E1000_CTRL_EXT);
 		reg_data |= E1000_CTRL_EXT_PFRSTD;
-		wr32(E1000_CTRL_EXT, reg_data);
+		E1000_WRITE_REG(hw, E1000_CTRL_EXT, reg_data);
 	}
 
 	netif_tx_start_all_queues(adapter->netdev);
 
+	if (adapter->flags & IGB_FLAG_DETECT_BAD_DMA)
+		schedule_work(&adapter->dma_err_task);
 	/* start the watchdog. */
 	hw->mac.get_link_status = 1;
 	schedule_work(&adapter->watchdog_task);
 
+	if ((adapter->flags & IGB_FLAG_EEE) &&
+	    (!hw->dev_spec._82575.eee_disable))
+		adapter->eee_advert = MDIO_EEE_100TX | MDIO_EEE_1000T;
+
 	return 0;
 }
 
@@ -1521,46 +1807,49 @@ void igb_down(struct igb_adapter *adapter)
 	set_bit(__IGB_DOWN, &adapter->state);
 
 	/* disable receives in the hardware */
-	rctl = rd32(E1000_RCTL);
-	wr32(E1000_RCTL, rctl & ~E1000_RCTL_EN);
+	rctl = E1000_READ_REG(hw, E1000_RCTL);
+	E1000_WRITE_REG(hw, E1000_RCTL, rctl & ~E1000_RCTL_EN);
 	/* flush and sleep below */
 
 	netif_tx_stop_all_queues(netdev);
 
 	/* disable transmits in the hardware */
-	tctl = rd32(E1000_TCTL);
+	tctl = E1000_READ_REG(hw, E1000_TCTL);
 	tctl &= ~E1000_TCTL_EN;
-	wr32(E1000_TCTL, tctl);
+	E1000_WRITE_REG(hw, E1000_TCTL, tctl);
 	/* flush both disables and wait for them to finish */
-	wrfl();
-	msleep(10);
+	E1000_WRITE_FLUSH(hw);
+	usleep_range(10000, 20000);
 
-	for (i = 0; i < adapter->num_q_vectors; i++) {
-		struct igb_q_vector *q_vector = adapter->q_vector[i];
-		napi_disable(&q_vector->napi);
-	}
+	for (i = 0; i < adapter->num_q_vectors; i++)
+		napi_disable(&(adapter->q_vector[i]->napi));
 
 	igb_irq_disable(adapter);
 
+	adapter->flags &= ~IGB_FLAG_NEED_LINK_UPDATE;
+
 	del_timer_sync(&adapter->watchdog_timer);
+	if (adapter->flags & IGB_FLAG_DETECT_BAD_DMA)
+		del_timer_sync(&adapter->dma_err_timer);
 	del_timer_sync(&adapter->phy_info_timer);
 
 	netif_carrier_off(netdev);
 
 	/* record the stats before reset*/
-	spin_lock(&adapter->stats64_lock);
-	igb_update_stats(adapter, &adapter->stats64);
-	spin_unlock(&adapter->stats64_lock);
+	igb_update_stats(adapter);
 
 	adapter->link_speed = 0;
 	adapter->link_duplex = 0;
 
+#ifdef HAVE_PCI_ERS
 	if (!pci_channel_offline(adapter->pdev))
 		igb_reset(adapter);
+#else
+	igb_reset(adapter);
+#endif
 	igb_clean_all_tx_rings(adapter);
 	igb_clean_all_rx_rings(adapter);
-#ifdef CONFIG_IGB_DCA
-
+#ifdef IGB_DCA
 	/* since we reset the hardware DCA settings were cleared */
 	igb_setup_dca(adapter);
 #endif
@@ -1570,20 +1859,51 @@ void igb_reinit_locked(struct igb_adapter *adapter)
 {
 	WARN_ON(in_interrupt());
 	while (test_and_set_bit(__IGB_RESETTING, &adapter->state))
-		msleep(1);
+		usleep_range(1000, 2000);
 	igb_down(adapter);
 	igb_up(adapter);
 	clear_bit(__IGB_RESETTING, &adapter->state);
 }
 
+/**
+ * igb_enable_mas - Media Autosense re-enable after swap
+ *
+ * @adapter: adapter struct
+ **/
+static s32  igb_enable_mas(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u32 connsw;
+	s32 ret_val = E1000_SUCCESS;
+
+	connsw = E1000_READ_REG(hw, E1000_CONNSW);
+	if (hw->phy.media_type == e1000_media_type_copper) {
+		/* configure for SerDes media detect */
+		if (!(connsw & E1000_CONNSW_SERDESD)) {
+			connsw |= E1000_CONNSW_ENRGSRC;
+			connsw |= E1000_CONNSW_AUTOSENSE_EN;
+			E1000_WRITE_REG(hw, E1000_CONNSW, connsw);
+			E1000_WRITE_FLUSH(hw);
+		} else if (connsw & E1000_CONNSW_SERDESD) {
+			/* already SerDes, no need to enable anything */
+			return ret_val;
+		} else {
+			dev_info(pci_dev_to_dev(adapter->pdev),
+			"%s:MAS: Unable to configure feature, disabling..\n",
+			adapter->netdev->name);
+			adapter->flags &= ~IGB_FLAG_MAS_ENABLE;
+		}
+	}
+	return ret_val;
+}
+
 void igb_reset(struct igb_adapter *adapter)
 {
 	struct pci_dev *pdev = adapter->pdev;
 	struct e1000_hw *hw = &adapter->hw;
 	struct e1000_mac_info *mac = &hw->mac;
 	struct e1000_fc_info *fc = &hw->fc;
-	u32 pba = 0, tx_space, min_tx_space, min_rx_space;
-	u16 hwm;
+	u32 pba = 0, tx_space, min_tx_space, min_rx_space, hwm;
 
 	/* Repartition Pba for greater than 9k mtu
 	 * To take effect CTRL.RST is required.
@@ -1591,14 +1911,17 @@ void igb_reset(struct igb_adapter *adapter)
 	switch (mac->type) {
 	case e1000_i350:
 	case e1000_82580:
-		pba = rd32(E1000_RXPBS);
-		pba = igb_rxpbs_adjust_82580(pba);
+	case e1000_i354:
+		pba = E1000_READ_REG(hw, E1000_RXPBS);
+		pba = e1000_rxpbs_adjust_82580(pba);
 		break;
 	case e1000_82576:
-		pba = rd32(E1000_RXPBS);
+		pba = E1000_READ_REG(hw, E1000_RXPBS);
 		pba &= E1000_RXPBS_SIZE_MASK_82576;
 		break;
 	case e1000_82575:
+	case e1000_i210:
+	case e1000_i211:
 	default:
 		pba = E1000_PBA_34K;
 		break;
@@ -1607,7 +1930,7 @@ void igb_reset(struct igb_adapter *adapter)
 	if ((adapter->max_frame_size > ETH_FRAME_LEN + ETH_FCS_LEN) &&
 	    (mac->type < e1000_82576)) {
 		/* adjust PBA for jumbo frames */
-		wr32(E1000_PBA, pba);
+		E1000_WRITE_REG(hw, E1000_PBA, pba);
 
 		/* To maintain wire speed transmits, the Tx FIFO should be
 		 * large enough to accommodate two full transmit packets,
@@ -1615,7 +1938,7 @@ void igb_reset(struct igb_adapter *adapter)
 		 * the Rx FIFO should be large enough to accommodate at least
 		 * one full receive packet and is similarly rounded up and
 		 * expressed in KB. */
-		pba = rd32(E1000_PBA);
+		pba = E1000_READ_REG(hw, E1000_PBA);
 		/* upper 16 bits has Tx packet buffer allocation size in KB */
 		tx_space = pba >> 16;
 		/* lower 16 bits has Rx packet buffer allocation size in KB */
@@ -1644,7 +1967,7 @@ void igb_reset(struct igb_adapter *adapter)
 			if (pba < min_rx_space)
 				pba = min_rx_space;
 		}
-		wr32(E1000_PBA, pba);
+		E1000_WRITE_REG(hw, E1000_PBA, pba);
 	}
 
 	/* flow control settings */
@@ -1656,7 +1979,7 @@ void igb_reset(struct igb_adapter *adapter)
 	hwm = min(((pba << 10) * 9 / 10),
 			((pba << 10) - 2 * adapter->max_frame_size));
 
-	fc->high_water = hwm & 0xFFF0;	/* 16-byte granularity */
+	fc->high_water = hwm & 0xFFFFFFF0;	/* 16-byte granularity */
 	fc->low_water = fc->high_water - 16;
 	fc->pause_time = 0xFFFF;
 	fc->send_xon = 1;
@@ -1665,6 +1988,10 @@ void igb_reset(struct igb_adapter *adapter)
 	/* disable receive for all VFs and wait one second */
 	if (adapter->vfs_allocated_count) {
 		int i;
+		/*
+		 * Clear all flags except indication that the PF has set
+		 * the VF MAC addresses administratively
+		 */
 		for (i = 0 ; i < adapter->vfs_allocated_count; i++)
 			adapter->vf_data[i].flags &= IGB_VF_FLAG_PF_SET_MAC;
 
@@ -1672,124 +1999,294 @@ void igb_reset(struct igb_adapter *adapter)
 		igb_ping_all_vfs(adapter);
 
 		/* disable transmits and receives */
-		wr32(E1000_VFRE, 0);
-		wr32(E1000_VFTE, 0);
+		E1000_WRITE_REG(hw, E1000_VFRE, 0);
+		E1000_WRITE_REG(hw, E1000_VFTE, 0);
 	}
 
 	/* Allow time for pending master requests to run */
-	hw->mac.ops.reset_hw(hw);
-	wr32(E1000_WUC, 0);
-
-	if (hw->mac.ops.init_hw(hw))
-		dev_err(&pdev->dev, "Hardware Error\n");
-	if (hw->mac.type > e1000_82580) {
-		if (adapter->flags & IGB_FLAG_DMAC) {
-			u32 reg;
+	e1000_reset_hw(hw);
+	E1000_WRITE_REG(hw, E1000_WUC, 0);
 
-			/*
-			 * DMA Coalescing high water mark needs to be higher
-			 * than * the * Rx threshold.  The Rx threshold is
-			 * currently * pba - 6, so we * should use a high water
-			 * mark of pba * - 4. */
-			hwm = (pba - 4) << 10;
-
-			reg = (((pba-6) << E1000_DMACR_DMACTHR_SHIFT)
-			       & E1000_DMACR_DMACTHR_MASK);
-
-			/* transition to L0x or L1 if available..*/
-			reg |= (E1000_DMACR_DMAC_EN | E1000_DMACR_DMAC_LX_MASK);
-
-			/* watchdog timer= +-1000 usec in 32usec intervals */
-			reg |= (1000 >> 5);
-			wr32(E1000_DMACR, reg);
+	if (adapter->flags & IGB_FLAG_MEDIA_RESET) {
+		e1000_setup_init_funcs(hw, TRUE);
+		igb_check_options(adapter);
+		e1000_get_bus_info(hw);
+		adapter->flags &= ~IGB_FLAG_MEDIA_RESET;
+	}
+	if (adapter->flags & IGB_FLAG_MAS_ENABLE) {
+		if (igb_enable_mas(adapter))
+			dev_err(pci_dev_to_dev(pdev),
+				"Error enabling Media Auto Sense\n");
+	}
+	if (e1000_init_hw(hw))
+		dev_err(pci_dev_to_dev(pdev), "Hardware Error\n");
 
-			/* no lower threshold to disable coalescing(smart fifb)
-			 * -UTRESH=0*/
-			wr32(E1000_DMCRTRH, 0);
+	/*
+	 * Flow control settings reset on hardware reset, so guarantee flow
+	 * control is off when forcing speed.
+	 */
+	if (!hw->mac.autoneg)
+		e1000_force_mac_fc(hw);
 
-			/* set hwm to PBA -  2 * max frame size */
-			wr32(E1000_FCRTC, hwm);
+	igb_init_dmac(adapter, pba);
+	/* Re-initialize the thermal sensor on i350 devices. */
+	if (mac->type == e1000_i350 && hw->bus.func == 0) {
+		/*
+		 * If present, re-initialize the external thermal sensor
+		 * interface.
+		 */
+		if (adapter->ets)
+			e1000_set_i2c_bb(hw);
+		e1000_init_thermal_sensor_thresh(hw);
+	}
 
-			/*
-			 * This sets the time to wait before requesting tran-
-			 * sition to * low power state to number of usecs needed
-			 * to receive 1 512 * byte frame at gigabit line rate
-			 */
-			reg = rd32(E1000_DMCTLX);
-			reg |= IGB_DMCTLX_DCFLUSH_DIS;
-
-			/* Delay 255 usec before entering Lx state. */
-			reg |= 0xFF;
-			wr32(E1000_DMCTLX, reg);
-
-			/* free space in Tx packet buffer to wake from DMAC */
-			wr32(E1000_DMCTXTH,
-			     (IGB_MIN_TXPBSIZE -
-			     (IGB_TX_BUF_4096 + adapter->max_frame_size))
-			     >> 6);
-
-			/* make low power state decision controlled by DMAC */
-			reg = rd32(E1000_PCIEMISC);
-			reg |= E1000_PCIEMISC_LX_DECISION;
-			wr32(E1000_PCIEMISC, reg);
-		} /* end if IGB_FLAG_DMAC set */
-	}
-	if (hw->mac.type == e1000_82580) {
-		u32 reg = rd32(E1000_PCIEMISC);
-		wr32(E1000_PCIEMISC,
-		                reg & ~E1000_PCIEMISC_LX_DECISION);
+	/*Re-establish EEE setting */
+	if (hw->phy.media_type == e1000_media_type_copper) {
+		switch (mac->type) {
+		case e1000_i350:
+		case e1000_i210:
+		case e1000_i211:
+			e1000_set_eee_i350(hw);
+			break;
+		case e1000_i354:
+			e1000_set_eee_i354(hw);
+			break;
+		default:
+			break;
+		}
 	}
+
 	if (!netif_running(adapter->netdev))
 		igb_power_down_link(adapter);
 
 	igb_update_mng_vlan(adapter);
 
 	/* Enable h/w to recognize an 802.1Q VLAN Ethernet packet */
-	wr32(E1000_VET, ETHERNET_IEEE_VLAN_TYPE);
+	E1000_WRITE_REG(hw, E1000_VET, ETHERNET_IEEE_VLAN_TYPE);
+
 
-	igb_get_phy_info(hw);
+#ifdef HAVE_PTP_1588_CLOCK
+	/* Re-enable PTP, where applicable. */
+	igb_ptp_reset(adapter);
+#endif /* HAVE_PTP_1588_CLOCK */
+
+	e1000_get_phy_info(hw);
+
+	adapter->devrc++;
 }
 
-static u32 igb_fix_features(struct net_device *netdev, u32 features)
+#ifdef HAVE_NDO_SET_FEATURES
+static netdev_features_t igb_fix_features(struct net_device *netdev,
+					  netdev_features_t features)
 {
 	/*
-	 * Since there is no support for separate rx/tx vlan accel
-	 * enable/disable make sure tx flag is always in same state as rx.
+	 * Since there is no support for separate tx vlan accel
+	 * enabled make sure tx flag is cleared if rx is.
 	 */
-	if (features & NETIF_F_HW_VLAN_RX)
-		features |= NETIF_F_HW_VLAN_TX;
-	else
+#ifdef NETIF_F_HW_VLAN_CTAG_RX
+	if (!(features & NETIF_F_HW_VLAN_CTAG_RX))
+		features &= ~NETIF_F_HW_VLAN_CTAG_TX;
+#else
+	if (!(features & NETIF_F_HW_VLAN_RX))
 		features &= ~NETIF_F_HW_VLAN_TX;
+#endif
+
+	/* If Rx checksum is disabled, then LRO should also be disabled */
+	if (!(features & NETIF_F_RXCSUM))
+		features &= ~NETIF_F_LRO;
 
 	return features;
 }
 
-static int igb_set_features(struct net_device *netdev, u32 features)
+static int igb_set_features(struct net_device *netdev,
+			    netdev_features_t features)
 {
-	struct igb_adapter *adapter = netdev_priv(netdev);
-	int i;
 	u32 changed = netdev->features ^ features;
 
-	for (i = 0; i < adapter->num_rx_queues; i++) {
-		if (features & NETIF_F_RXCSUM)
-			adapter->rx_ring[i]->flags |= IGB_RING_FLAG_RX_CSUM;
+#ifdef NETIF_F_HW_VLAN_CTAG_RX
+	if (changed & NETIF_F_HW_VLAN_CTAG_RX)
+#else
+	if (changed & NETIF_F_HW_VLAN_RX)
+#endif
+		igb_vlan_mode(netdev, features);
+
+	return 0;
+}
+
+#ifdef HAVE_FDB_OPS
+#ifdef USE_CONST_DEV_UC_CHAR
+static int igb_ndo_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
+			   struct net_device *dev,
+			   const unsigned char *addr,
+			   u16 flags)
+#else
+static int igb_ndo_fdb_add(struct ndmsg *ndm,
+			   struct net_device *dev,
+			   unsigned char *addr,
+			   u16 flags)
+#endif
+{
+	struct igb_adapter *adapter = netdev_priv(dev);
+	struct e1000_hw *hw = &adapter->hw;
+	int err;
+
+	if (!(adapter->vfs_allocated_count))
+		return -EOPNOTSUPP;
+
+	/* Hardware does not support aging addresses so if a
+	 * ndm_state is given only allow permanent addresses
+	 */
+	if (ndm->ndm_state && !(ndm->ndm_state & NUD_PERMANENT)) {
+		pr_info("%s: FDB only supports static addresses\n",
+			igb_driver_name);
+		return -EINVAL;
+	}
+
+	if (is_unicast_ether_addr(addr) || is_link_local_ether_addr(addr)) {
+		u32 rar_uc_entries = hw->mac.rar_entry_count -
+					(adapter->vfs_allocated_count + 1);
+
+		if (netdev_uc_count(dev) < rar_uc_entries)
+			err = dev_uc_add_excl(dev, addr);
 		else
-			adapter->rx_ring[i]->flags &= ~IGB_RING_FLAG_RX_CSUM;
+			err = -ENOMEM;
+	} else if (is_multicast_ether_addr(addr)) {
+		err = dev_mc_add_excl(dev, addr);
+	} else {
+		err = -EINVAL;
 	}
 
-	if (changed & NETIF_F_HW_VLAN_RX)
-		igb_vlan_mode(netdev, features);
+	/* Only return duplicate errors if NLM_F_EXCL is set */
+	if (err == -EEXIST && !(flags & NLM_F_EXCL))
+		err = 0;
+
+	return err;
+}
+
+#ifndef USE_DEFAULT_FDB_DEL_DUMP
+#ifdef USE_CONST_DEV_UC_CHAR
+static int igb_ndo_fdb_del(struct ndmsg *ndm,
+			   struct net_device *dev,
+			   const unsigned char *addr)
+#else
+static int igb_ndo_fdb_del(struct ndmsg *ndm,
+			   struct net_device *dev,
+			   unsigned char *addr)
+#endif
+{
+	struct igb_adapter *adapter = netdev_priv(dev);
+	int err = -EOPNOTSUPP;
+
+	if (ndm->ndm_state & NUD_PERMANENT) {
+		pr_info("%s: FDB only supports static addresses\n",
+			igb_driver_name);
+		return -EINVAL;
+	}
+
+	if (adapter->vfs_allocated_count) {
+		if (is_unicast_ether_addr(addr))
+			err = dev_uc_del(dev, addr);
+		else if (is_multicast_ether_addr(addr))
+			err = dev_mc_del(dev, addr);
+		else
+			err = -EINVAL;
+	}
+
+	return err;
+}
+
+static int igb_ndo_fdb_dump(struct sk_buff *skb,
+			    struct netlink_callback *cb,
+			    struct net_device *dev,
+			    int idx)
+{
+	struct igb_adapter *adapter = netdev_priv(dev);
+
+	if (adapter->vfs_allocated_count)
+		idx = ndo_dflt_fdb_dump(skb, cb, dev, idx);
+
+	return idx;
+}
+#endif /* USE_DEFAULT_FDB_DEL_DUMP */
+
+#ifdef HAVE_BRIDGE_ATTRIBS
+static int igb_ndo_bridge_setlink(struct net_device *dev,
+				  struct nlmsghdr *nlh)
+{
+	struct igb_adapter *adapter = netdev_priv(dev);
+	struct e1000_hw *hw = &adapter->hw;
+	struct nlattr *attr, *br_spec;
+	int rem;
+
+	if (!(adapter->vfs_allocated_count))
+		return -EOPNOTSUPP;
+
+	switch (adapter->hw.mac.type) {
+	case e1000_82576:
+	case e1000_i350:
+	case e1000_i354:
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	br_spec = nlmsg_find_attr(nlh, sizeof(struct ifinfomsg), IFLA_AF_SPEC);
+
+	nla_for_each_nested(attr, br_spec, rem) {
+		__u16 mode;
+
+		if (nla_type(attr) != IFLA_BRIDGE_MODE)
+			continue;
+
+		mode = nla_get_u16(attr);
+		if (mode == BRIDGE_MODE_VEPA) {
+			e1000_vmdq_set_loopback_pf(hw, 0);
+			adapter->flags &= ~IGB_FLAG_LOOPBACK_ENABLE;
+		} else if (mode == BRIDGE_MODE_VEB) {
+			e1000_vmdq_set_loopback_pf(hw, 1);
+			adapter->flags |= IGB_FLAG_LOOPBACK_ENABLE;
+		} else
+			return -EINVAL;
+
+		netdev_info(adapter->netdev, "enabling bridge mode: %s\n",
+			    mode == BRIDGE_MODE_VEPA ? "VEPA" : "VEB");
+	}
 
 	return 0;
 }
 
+#ifdef HAVE_BRIDGE_FILTER
+static int igb_ndo_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
+				  struct net_device *dev, u32 filter_mask)
+#else
+static int igb_ndo_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
+				  struct net_device *dev)
+#endif
+{
+	struct igb_adapter *adapter = netdev_priv(dev);
+	u16 mode;
+
+	if (!(adapter->vfs_allocated_count))
+		return -EOPNOTSUPP;
+
+	if (adapter->flags & IGB_FLAG_LOOPBACK_ENABLE)
+		mode = BRIDGE_MODE_VEB;
+	else
+		mode = BRIDGE_MODE_VEPA;
+
+	return ndo_dflt_bridge_getlink(skb, pid, seq, dev, mode);
+}
+#endif /* HAVE_BRIDGE_ATTRIBS */
+#endif /* HAVE_FDB_OPS */
+
+#endif /* HAVE_NDO_SET_FEATURES */
+#ifdef HAVE_NET_DEVICE_OPS
 static const struct net_device_ops igb_netdev_ops = {
 	.ndo_open		= igb_open,
 	.ndo_stop		= igb_close,
-	.ndo_start_xmit		= igb_xmit_frame_adv,
-	.ndo_get_stats64	= igb_get_stats64,
+	.ndo_start_xmit		= igb_xmit_frame,
+	.ndo_get_stats		= igb_get_stats,
 	.ndo_set_rx_mode	= igb_set_rx_mode,
-	.ndo_set_multicast_list	= igb_set_rx_mode,
 	.ndo_set_mac_address	= igb_set_mac,
 	.ndo_change_mtu		= igb_change_mtu,
 	.ndo_do_ioctl		= igb_ioctl,
@@ -1797,17 +2294,221 @@ static const struct net_device_ops igb_netdev_ops = {
 	.ndo_validate_addr	= eth_validate_addr,
 	.ndo_vlan_rx_add_vid	= igb_vlan_rx_add_vid,
 	.ndo_vlan_rx_kill_vid	= igb_vlan_rx_kill_vid,
+#ifdef IFLA_VF_MAX
 	.ndo_set_vf_mac		= igb_ndo_set_vf_mac,
 	.ndo_set_vf_vlan	= igb_ndo_set_vf_vlan,
 	.ndo_set_vf_tx_rate	= igb_ndo_set_vf_bw,
 	.ndo_get_vf_config	= igb_ndo_get_vf_config,
+#ifdef HAVE_VF_SPOOFCHK_CONFIGURE
+	.ndo_set_vf_spoofchk	= igb_ndo_set_vf_spoofchk,
+#endif /* HAVE_VF_SPOOFCHK_CONFIGURE */
+#endif /* IFLA_VF_MAX */
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller	= igb_netpoll,
 #endif
+#ifdef HAVE_NDO_SET_FEATURES
 	.ndo_fix_features	= igb_fix_features,
 	.ndo_set_features	= igb_set_features,
+#endif
+#ifdef HAVE_VLAN_RX_REGISTER
+	.ndo_vlan_rx_register	= igb_vlan_mode,
+#endif
+#ifdef HAVE_FDB_OPS
+	.ndo_fdb_add		= igb_ndo_fdb_add,
+#ifndef USE_DEFAULT_FDB_DEL_DUMP
+	.ndo_fdb_del		= igb_ndo_fdb_del,
+	.ndo_fdb_dump		= igb_ndo_fdb_dump,
+#endif
+#ifdef HAVE_BRIDGE_ATTRIBS
+	.ndo_bridge_setlink	= igb_ndo_bridge_setlink,
+	.ndo_bridge_getlink	= igb_ndo_bridge_getlink,
+#endif /* HAVE_BRIDGE_ATTRIBS */
+#endif
 };
 
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+static const struct net_device_ops igb_vmdq_ops = {
+	.ndo_open		= &igb_vmdq_open,
+	.ndo_stop		= &igb_vmdq_close,
+	.ndo_start_xmit		= &igb_vmdq_xmit_frame,
+	.ndo_get_stats		= &igb_vmdq_get_stats,
+	.ndo_set_rx_mode	= &igb_vmdq_set_rx_mode,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_set_mac_address	= &igb_vmdq_set_mac,
+	.ndo_change_mtu		= &igb_vmdq_change_mtu,
+	.ndo_tx_timeout		= &igb_vmdq_tx_timeout,
+	.ndo_vlan_rx_register	= &igb_vmdq_vlan_rx_register,
+	.ndo_vlan_rx_add_vid	= &igb_vmdq_vlan_rx_add_vid,
+	.ndo_vlan_rx_kill_vid	= &igb_vmdq_vlan_rx_kill_vid,
+};
+
+#endif /* CONFIG_IGB_VMDQ_NETDEV */
+#endif /* HAVE_NET_DEVICE_OPS */
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+void igb_assign_vmdq_netdev_ops(struct net_device *vnetdev)
+{
+#ifdef HAVE_NET_DEVICE_OPS
+	vnetdev->netdev_ops = &igb_vmdq_ops;
+#else
+	dev->open = &igb_vmdq_open;
+	dev->stop = &igb_vmdq_close;
+	dev->hard_start_xmit = &igb_vmdq_xmit_frame;
+	dev->get_stats = &igb_vmdq_get_stats;
+#ifdef HAVE_SET_RX_MODE
+	dev->set_rx_mode = &igb_vmdq_set_rx_mode;
+#endif
+	dev->set_multicast_list = &igb_vmdq_set_rx_mode;
+	dev->set_mac_address = &igb_vmdq_set_mac;
+	dev->change_mtu = &igb_vmdq_change_mtu;
+#ifdef HAVE_TX_TIMEOUT
+	dev->tx_timeout = &igb_vmdq_tx_timeout;
+#endif
+#if defined(NETIF_F_HW_VLAN_TX) || defined(NETIF_F_HW_VLAN_CTAG_TX)
+	dev->vlan_rx_register = &igb_vmdq_vlan_rx_register;
+	dev->vlan_rx_add_vid = &igb_vmdq_vlan_rx_add_vid;
+	dev->vlan_rx_kill_vid = &igb_vmdq_vlan_rx_kill_vid;
+#endif
+#endif
+	igb_vmdq_set_ethtool_ops(vnetdev);
+	vnetdev->watchdog_timeo = 5 * HZ;
+
+}
+
+int igb_init_vmdq_netdevs(struct igb_adapter *adapter)
+{
+	int pool, err = 0, base_queue;
+	struct net_device *vnetdev;
+	struct igb_vmdq_adapter *vmdq_adapter;
+
+	for (pool = 1; pool < adapter->vmdq_pools; pool++) {
+		int qpp = (!adapter->rss_queues ? 1 : adapter->rss_queues);
+		base_queue = pool * qpp;
+		vnetdev = alloc_etherdev(sizeof(struct igb_vmdq_adapter));
+		if (!vnetdev) {
+			err = -ENOMEM;
+			break;
+		}
+		vmdq_adapter = netdev_priv(vnetdev);
+		vmdq_adapter->vnetdev = vnetdev;
+		vmdq_adapter->real_adapter = adapter;
+		vmdq_adapter->rx_ring = adapter->rx_ring[base_queue];
+		vmdq_adapter->tx_ring = adapter->tx_ring[base_queue];
+		igb_assign_vmdq_netdev_ops(vnetdev);
+		snprintf(vnetdev->name, IFNAMSIZ, "%sv%d",
+			 adapter->netdev->name, pool);
+		vnetdev->features = adapter->netdev->features;
+#ifdef HAVE_NETDEV_VLAN_FEATURES
+		vnetdev->vlan_features = adapter->netdev->vlan_features;
+#endif
+		adapter->vmdq_netdev[pool-1] = vnetdev;
+		err = register_netdev(vnetdev);
+		if (err)
+			break;
+	}
+	return err;
+}
+
+int igb_remove_vmdq_netdevs(struct igb_adapter *adapter)
+{
+	int pool, err = 0;
+
+	for (pool = 1; pool < adapter->vmdq_pools; pool++) {
+		unregister_netdev(adapter->vmdq_netdev[pool-1]);
+		free_netdev(adapter->vmdq_netdev[pool-1]);
+		adapter->vmdq_netdev[pool-1] = NULL;
+	}
+	return err;
+}
+#endif /* CONFIG_IGB_VMDQ_NETDEV */
+
+/**
+ * igb_set_fw_version - Configure version string for ethtool
+ * @adapter: adapter struct
+ *
+ **/
+static void igb_set_fw_version(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	struct e1000_fw_version fw;
+
+	e1000_get_fw_version(hw, &fw);
+
+	switch (hw->mac.type) {
+	case e1000_i210:
+	case e1000_i211:
+		if (!(e1000_get_flash_presence_i210(hw))) {
+			snprintf(adapter->fw_version,
+			    sizeof(adapter->fw_version),
+			    "%2d.%2d-%d",
+			    fw.invm_major, fw.invm_minor, fw.invm_img_type);
+			break;
+		}
+		/* fall through */
+	default:
+		/* if option rom is valid, display its version too*/
+		if (fw.or_valid) {
+			snprintf(adapter->fw_version,
+			    sizeof(adapter->fw_version),
+			    "%d.%d, 0x%08x, %d.%d.%d",
+			    fw.eep_major, fw.eep_minor, fw.etrack_id,
+			    fw.or_major, fw.or_build, fw.or_patch);
+		/* no option rom */
+		} else {
+			if (fw.etrack_id != 0X0000) {
+			snprintf(adapter->fw_version,
+			    sizeof(adapter->fw_version),
+			    "%d.%d, 0x%08x",
+			    fw.eep_major, fw.eep_minor, fw.etrack_id);
+			} else {
+			snprintf(adapter->fw_version,
+			    sizeof(adapter->fw_version),
+			    "%d.%d.%d",
+			    fw.eep_major, fw.eep_minor, fw.eep_build);
+			}
+		}
+		break;
+	}
+
+	return;
+}
+
+/**
+ * igb_init_mas - init Media Autosense feature if enabled in the NVM
+ *
+ * @adapter: adapter struct
+ **/
+static void igb_init_mas(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u16 eeprom_data;
+
+	e1000_read_nvm(hw, NVM_COMPAT, 1, &eeprom_data);
+	switch (hw->bus.func) {
+	case E1000_FUNC_0:
+		if (eeprom_data & IGB_MAS_ENABLE_0)
+			adapter->flags |= IGB_FLAG_MAS_ENABLE;
+		break;
+	case E1000_FUNC_1:
+		if (eeprom_data & IGB_MAS_ENABLE_1)
+			adapter->flags |= IGB_FLAG_MAS_ENABLE;
+		break;
+	case E1000_FUNC_2:
+		if (eeprom_data & IGB_MAS_ENABLE_2)
+			adapter->flags |= IGB_FLAG_MAS_ENABLE;
+		break;
+	case E1000_FUNC_3:
+		if (eeprom_data & IGB_MAS_ENABLE_3)
+			adapter->flags |= IGB_FLAG_MAS_ENABLE;
+		break;
+	default:
+		/* Shouldn't get here */
+		dev_err(pci_dev_to_dev(adapter->pdev),
+			"%s:AMS: Invalid port configuration, returning\n",
+			adapter->netdev->name);
+		break;
+	}
+}
+
 /**
  * igb_probe - Device Initialization Routine
  * @pdev: PCI device information struct
@@ -1826,47 +2527,49 @@ static int __devinit igb_probe(struct pci_dev *pdev,
 	struct igb_adapter *adapter;
 	struct e1000_hw *hw;
 	u16 eeprom_data = 0;
+	u8 pba_str[E1000_PBANUM_LENGTH];
 	s32 ret_val;
 	static int global_quad_port_a; /* global quad port a indication */
-	const struct e1000_info *ei = igb_info_tbl[ent->driver_data];
-	unsigned long mmio_start, mmio_len;
-	int err, pci_using_dac;
-	u16 eeprom_apme_mask = IGB_EEPROM_APME;
-	u8 part_str[E1000_PBANUM_LENGTH];
-
-	/* Catch broken hardware that put the wrong VF device ID in
-	 * the PCIe SR-IOV capability.
-	 */
-	if (pdev->is_virtfn) {
-		WARN(1, KERN_ERR "%s (%hx:%hx) should not be a VF!\n",
-		     pci_name(pdev), pdev->vendor, pdev->device);
-		return -EINVAL;
-	}
+	int i, err, pci_using_dac;
+	static int cards_found;
 
 	err = pci_enable_device_mem(pdev);
 	if (err)
 		return err;
 
 	pci_using_dac = 0;
-	err = dma_set_mask(&pdev->dev, DMA_BIT_MASK(64));
+	err = dma_set_mask(pci_dev_to_dev(pdev), DMA_BIT_MASK(64));
 	if (!err) {
-		err = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(64));
+		err = dma_set_coherent_mask(pci_dev_to_dev(pdev), DMA_BIT_MASK(64));
 		if (!err)
 			pci_using_dac = 1;
 	} else {
-		err = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
+		err = dma_set_mask(pci_dev_to_dev(pdev), DMA_BIT_MASK(32));
 		if (err) {
-			err = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
+			err = dma_set_coherent_mask(pci_dev_to_dev(pdev), DMA_BIT_MASK(32));
 			if (err) {
-				dev_err(&pdev->dev, "No usable DMA "
-					"configuration, aborting\n");
+				IGB_ERR("No usable DMA configuration, "
+				        "aborting\n");
 				goto err_dma;
 			}
 		}
 	}
 
-	err = pci_request_selected_regions(pdev, pci_select_bars(pdev,
-	                                   IORESOURCE_MEM),
+#ifndef HAVE_ASPM_QUIRKS
+	/* 82575 requires that the pci-e link partner disable the L0s state */
+	switch (pdev->device) {
+	case E1000_DEV_ID_82575EB_COPPER:
+	case E1000_DEV_ID_82575EB_FIBER_SERDES:
+	case E1000_DEV_ID_82575GB_QUAD_COPPER:
+		pci_disable_link_state(pdev, PCIE_LINK_STATE_L0S);
+	default:
+		break;
+	}
+
+#endif /* HAVE_ASPM_QUIRKS */
+	err = pci_request_selected_regions(pdev,
+	                                   pci_select_bars(pdev,
+                                                           IORESOURCE_MEM),
 	                                   igb_driver_name);
 	if (err)
 		goto err_pci_reg;
@@ -1874,14 +2577,18 @@ static int __devinit igb_probe(struct pci_dev *pdev,
 	pci_enable_pcie_error_reporting(pdev);
 
 	pci_set_master(pdev);
-	pci_save_state(pdev);
 
 	err = -ENOMEM;
+#ifdef HAVE_TX_MQ
 	netdev = alloc_etherdev_mq(sizeof(struct igb_adapter),
-	                           IGB_ABS_MAX_TX_QUEUES);
+	                           IGB_MAX_TX_QUEUES);
+#else
+	netdev = alloc_etherdev(sizeof(struct igb_adapter));
+#endif /* HAVE_TX_MQ */
 	if (!netdev)
 		goto err_alloc_etherdev;
 
+	SET_MODULE_OWNER(netdev);
 	SET_NETDEV_DEV(netdev, &pdev->dev);
 
 	pci_set_drvdata(pdev, netdev);
@@ -1890,122 +2597,228 @@ static int __devinit igb_probe(struct pci_dev *pdev,
 	adapter->pdev = pdev;
 	hw = &adapter->hw;
 	hw->back = adapter;
-	adapter->msg_enable = NETIF_MSG_DRV | NETIF_MSG_PROBE;
-
-	mmio_start = pci_resource_start(pdev, 0);
-	mmio_len = pci_resource_len(pdev, 0);
+	adapter->port_num = hw->bus.func;
+	adapter->msg_enable = (1 << debug) - 1;
 
+#ifdef HAVE_PCI_ERS
+	err = pci_save_state(pdev);
+	if (err)
+		goto err_ioremap;
+#endif
 	err = -EIO;
-	hw->hw_addr = ioremap(mmio_start, mmio_len);
+	hw->hw_addr = ioremap(pci_resource_start(pdev, 0),
+	                      pci_resource_len(pdev, 0));
 	if (!hw->hw_addr)
 		goto err_ioremap;
 
+#ifdef HAVE_NET_DEVICE_OPS
 	netdev->netdev_ops = &igb_netdev_ops;
+#else /* HAVE_NET_DEVICE_OPS */
+	netdev->open = &igb_open;
+	netdev->stop = &igb_close;
+	netdev->get_stats = &igb_get_stats;
+#ifdef HAVE_SET_RX_MODE
+	netdev->set_rx_mode = &igb_set_rx_mode;
+#endif
+	netdev->set_multicast_list = &igb_set_rx_mode;
+	netdev->set_mac_address = &igb_set_mac;
+	netdev->change_mtu = &igb_change_mtu;
+	netdev->do_ioctl = &igb_ioctl;
+#ifdef HAVE_TX_TIMEOUT
+	netdev->tx_timeout = &igb_tx_timeout;
+#endif
+	netdev->vlan_rx_register = igb_vlan_mode;
+	netdev->vlan_rx_add_vid = igb_vlan_rx_add_vid;
+	netdev->vlan_rx_kill_vid = igb_vlan_rx_kill_vid;
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	netdev->poll_controller = igb_netpoll;
+#endif
+	netdev->hard_start_xmit = &igb_xmit_frame;
+#endif /* HAVE_NET_DEVICE_OPS */
 	igb_set_ethtool_ops(netdev);
+#ifdef HAVE_TX_TIMEOUT
 	netdev->watchdog_timeo = 5 * HZ;
+#endif
 
 	strncpy(netdev->name, pci_name(pdev), sizeof(netdev->name) - 1);
 
-	netdev->mem_start = mmio_start;
-	netdev->mem_end = mmio_start + mmio_len;
-
-	/* PCI config space info */
-	hw->vendor_id = pdev->vendor;
-	hw->device_id = pdev->device;
-	hw->revision_id = pdev->revision;
-	hw->subsystem_vendor_id = pdev->subsystem_vendor;
-	hw->subsystem_device_id = pdev->subsystem_device;
-
-	/* Copy the default MAC, PHY and NVM function pointers */
-	memcpy(&hw->mac.ops, ei->mac_ops, sizeof(hw->mac.ops));
-	memcpy(&hw->phy.ops, ei->phy_ops, sizeof(hw->phy.ops));
-	memcpy(&hw->nvm.ops, ei->nvm_ops, sizeof(hw->nvm.ops));
-	/* Initialize skew-specific constants */
-	err = ei->get_invariants(hw);
-	if (err)
-		goto err_sw_init;
+	adapter->bd_number = cards_found;
 
 	/* setup the private structure */
 	err = igb_sw_init(adapter);
 	if (err)
 		goto err_sw_init;
 
-	igb_get_bus_info_pcie(hw);
+	e1000_get_bus_info(hw);
 
-	hw->phy.autoneg_wait_to_complete = false;
+	hw->phy.autoneg_wait_to_complete = FALSE;
+	hw->mac.adaptive_ifs = FALSE;
 
 	/* Copper options */
 	if (hw->phy.media_type == e1000_media_type_copper) {
 		hw->phy.mdix = AUTO_ALL_MODES;
-		hw->phy.disable_polarity_correction = false;
+		hw->phy.disable_polarity_correction = FALSE;
 		hw->phy.ms_type = e1000_ms_hw_default;
 	}
 
-	if (igb_check_reset_block(hw))
-		dev_info(&pdev->dev,
+	if (e1000_check_reset_block(hw))
+		dev_info(pci_dev_to_dev(pdev),
 			"PHY reset is blocked due to SOL/IDER session.\n");
 
-	netdev->hw_features = NETIF_F_SG |
-			   NETIF_F_IP_CSUM |
-			   NETIF_F_IPV6_CSUM |
-			   NETIF_F_TSO |
-			   NETIF_F_TSO6 |
-			   NETIF_F_RXCSUM |
-			   NETIF_F_HW_VLAN_RX;
-
-	netdev->features = netdev->hw_features |
-			   NETIF_F_HW_VLAN_TX |
-			   NETIF_F_HW_VLAN_FILTER;
-
-	netdev->vlan_features |= NETIF_F_TSO;
-	netdev->vlan_features |= NETIF_F_TSO6;
-	netdev->vlan_features |= NETIF_F_IP_CSUM;
-	netdev->vlan_features |= NETIF_F_IPV6_CSUM;
-	netdev->vlan_features |= NETIF_F_SG;
-
-	if (pci_using_dac) {
-		netdev->features |= NETIF_F_HIGHDMA;
-		netdev->vlan_features |= NETIF_F_HIGHDMA;
-	}
+	/*
+	 * features is initialized to 0 in allocation, it might have bits
+	 * set by igb_sw_init so we should use an or instead of an
+	 * assignment.
+	 */
+	netdev->features |= NETIF_F_SG |
+			    NETIF_F_IP_CSUM |
+#ifdef NETIF_F_IPV6_CSUM
+			    NETIF_F_IPV6_CSUM |
+#endif
+#ifdef NETIF_F_TSO
+			    NETIF_F_TSO |
+#ifdef NETIF_F_TSO6
+			    NETIF_F_TSO6 |
+#endif
+#endif /* NETIF_F_TSO */
+#ifdef NETIF_F_RXHASH
+			    NETIF_F_RXHASH |
+#endif
+			    NETIF_F_RXCSUM |
+#ifdef NETIF_F_HW_VLAN_CTAG_RX
+			    NETIF_F_HW_VLAN_CTAG_RX |
+			    NETIF_F_HW_VLAN_CTAG_TX;
+#else
+			    NETIF_F_HW_VLAN_RX |
+			    NETIF_F_HW_VLAN_TX;
+#endif
 
-	if (hw->mac.type >= e1000_82576) {
-		netdev->hw_features |= NETIF_F_SCTP_CSUM;
+	if (hw->mac.type >= e1000_82576)
 		netdev->features |= NETIF_F_SCTP_CSUM;
-	}
 
-	adapter->en_mng_pt = igb_enable_mng_pass_thru(hw);
+#ifdef HAVE_NDO_SET_FEATURES
+	/* copy netdev features into list of user selectable features */
+	netdev->hw_features |= netdev->features;
+#ifndef IGB_NO_LRO
+
+	/* give us the option of enabling LRO later */
+	netdev->hw_features |= NETIF_F_LRO;
+#endif
+#else
+#ifdef NETIF_F_GRO
+
+	/* this is only needed on kernels prior to 2.6.39 */
+	netdev->features |= NETIF_F_GRO;
+#endif
+#endif
+
+	/* set this bit last since it cannot be part of hw_features */
+#ifdef NETIF_F_HW_VLAN_CTAG_FILTER
+	netdev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
+#else
+	netdev->features |= NETIF_F_HW_VLAN_FILTER;
+#endif
+
+#ifdef HAVE_NETDEV_VLAN_FEATURES
+	netdev->vlan_features |= NETIF_F_TSO |
+				 NETIF_F_TSO6 |
+				 NETIF_F_IP_CSUM |
+				 NETIF_F_IPV6_CSUM |
+				 NETIF_F_SG;
+
+#endif
+	if (pci_using_dac)
+		netdev->features |= NETIF_F_HIGHDMA;
+
+	adapter->en_mng_pt = e1000_enable_mng_pass_thru(hw);
+#ifdef DEBUG
+	if (adapter->dmac != IGB_DMAC_DISABLE)
+		printk("%s: DMA Coalescing is enabled..\n", netdev->name);
+#endif
 
 	/* before reading the NVM, reset the controller to put the device in a
 	 * known good starting state */
-	hw->mac.ops.reset_hw(hw);
+	e1000_reset_hw(hw);
 
 	/* make sure the NVM is good */
-	if (hw->nvm.ops.validate(hw) < 0) {
-		dev_err(&pdev->dev, "The NVM Checksum Is Not Valid\n");
+	if (e1000_validate_nvm_checksum(hw) < 0) {
+#ifdef CONFIG_MACH_APALIS_T30
+		/* only warn on NVM validation failures */
+		dev_warn(pci_dev_to_dev(pdev), "The NVM Checksum Is Not"
+		        " Valid\n");
+#else /* CONFIG_MACH_APALIS_T30 */
+		dev_err(pci_dev_to_dev(pdev), "The NVM Checksum Is Not"
+		        " Valid\n");
 		err = -EIO;
 		goto err_eeprom;
+#endif /* CONFIG_MACH_APALIS_T30 */
 	}
 
 	/* copy the MAC address out of the NVM */
-	if (hw->mac.ops.read_mac_addr(hw))
-		dev_err(&pdev->dev, "NVM Read Error\n");
+	if (e1000_read_mac_addr(hw))
+		dev_err(pci_dev_to_dev(pdev), "NVM Read Error\n");
+
+#ifdef CONFIG_MACH_APALIS_T30
+	if (g_usr_mac && (g_usr_mac < 3)) {
+		/* Get user set MAC address */
+		if (g_usr_mac == 2) {
+			/* 0x100000 offset for 2nd Ethernet MAC */
+			g_mac_addr[3] += 0x10;
+			if (g_mac_addr[3] < 0x10)
+				dev_warn(&pdev->dev, "MAC address byte 3 (0x%02x) wrap around", g_mac_addr[3]);
+		}
+		memcpy(hw->mac.addr, g_mac_addr, ETH_ALEN);
+		g_usr_mac++;
+	}
+#endif /* CONFIG_MACH_APALIS_T30 */
 
 	memcpy(netdev->dev_addr, hw->mac.addr, netdev->addr_len);
+#ifdef ETHTOOL_GPERMADDR
 	memcpy(netdev->perm_addr, hw->mac.addr, netdev->addr_len);
 
 	if (!is_valid_ether_addr(netdev->perm_addr)) {
-		dev_err(&pdev->dev, "Invalid MAC Address\n");
+#else
+	if (!is_valid_ether_addr(netdev->dev_addr)) {
+#endif
+#ifdef CONFIG_MACH_APALIS_T30
+		/* Use Toradex OUI as default */
+		char default_mac_addr[ETH_ALEN] = {0x0, 0x14, 0x2d, 0x0, 0x0, 0x0};
+		dev_warn(&pdev->dev, "using Toradex OUI as default igb MAC");
+		memcpy(hw->mac.addr, default_mac_addr, ETH_ALEN);
+		memcpy(netdev->dev_addr, hw->mac.addr, netdev->addr_len);
+#ifdef ETHTOOL_GPERMADDR
+		memcpy(netdev->perm_addr, hw->mac.addr, netdev->addr_len);
+#endif
+#else /* CONFIG_MACH_APALIS_T30 */
+		dev_err(pci_dev_to_dev(pdev), "Invalid MAC Address\n");
 		err = -EIO;
 		goto err_eeprom;
+#endif /* CONFIG_MACH_APALIS_T30 */
 	}
 
-	setup_timer(&adapter->watchdog_timer, igb_watchdog,
+	memcpy(&adapter->mac_table[0].addr, hw->mac.addr, netdev->addr_len);
+	adapter->mac_table[0].queue = adapter->vfs_allocated_count;
+	adapter->mac_table[0].state = (IGB_MAC_STATE_DEFAULT | IGB_MAC_STATE_IN_USE);
+	igb_rar_set(adapter, 0);
+
+	/* get firmware version for ethtool -i */
+	igb_set_fw_version(adapter);
+
+	/* Check if Media Autosense is enabled */
+	if (hw->mac.type == e1000_82580)
+		igb_init_mas(adapter);
+	setup_timer(&adapter->watchdog_timer, &igb_watchdog,
 	            (unsigned long) adapter);
-	setup_timer(&adapter->phy_info_timer, igb_update_phy_info,
+	if (adapter->flags & IGB_FLAG_DETECT_BAD_DMA)
+		setup_timer(&adapter->dma_err_timer, &igb_dma_err_timer,
+			    (unsigned long) adapter);
+	setup_timer(&adapter->phy_info_timer, &igb_update_phy_info,
 	            (unsigned long) adapter);
 
 	INIT_WORK(&adapter->reset_task, igb_reset_task);
 	INIT_WORK(&adapter->watchdog_task, igb_watchdog_task);
+	if (adapter->flags & IGB_FLAG_DETECT_BAD_DMA)
+		INIT_WORK(&adapter->dma_err_task, igb_dma_err_task);
 
 	/* Initialize link properties that are user-changeable */
 	adapter->fc_autoneg = true;
@@ -2015,125 +2828,225 @@ static int __devinit igb_probe(struct pci_dev *pdev,
 	hw->fc.requested_mode = e1000_fc_default;
 	hw->fc.current_mode = e1000_fc_default;
 
-	igb_validate_mdi_setting(hw);
-
-	/* Initial Wake on LAN setting If APM wake is enabled in the EEPROM,
-	 * enable the ACPI Magic Packet filter
-	 */
+	e1000_validate_mdi_setting(hw);
 
+	/* By default, support wake on port A */
 	if (hw->bus.func == 0)
-		hw->nvm.ops.read(hw, NVM_INIT_CONTROL3_PORT_A, 1, &eeprom_data);
-	else if (hw->mac.type >= e1000_82580)
+		adapter->flags |= IGB_FLAG_WOL_SUPPORTED;
+
+	/* Check the NVM for wake support for non-port A ports */
+	if (hw->mac.type >= e1000_82580)
 		hw->nvm.ops.read(hw, NVM_INIT_CONTROL3_PORT_A +
 		                 NVM_82580_LAN_FUNC_OFFSET(hw->bus.func), 1,
 		                 &eeprom_data);
 	else if (hw->bus.func == 1)
-		hw->nvm.ops.read(hw, NVM_INIT_CONTROL3_PORT_B, 1, &eeprom_data);
+		e1000_read_nvm(hw, NVM_INIT_CONTROL3_PORT_B, 1, &eeprom_data);
 
-	if (eeprom_data & eeprom_apme_mask)
-		adapter->eeprom_wol |= E1000_WUFC_MAG;
+	if (eeprom_data & IGB_EEPROM_APME)
+		adapter->flags |= IGB_FLAG_WOL_SUPPORTED;
 
 	/* now that we have the eeprom settings, apply the special cases where
 	 * the eeprom may be wrong or the board simply won't support wake on
 	 * lan on a particular port */
 	switch (pdev->device) {
 	case E1000_DEV_ID_82575GB_QUAD_COPPER:
-		adapter->eeprom_wol = 0;
+		adapter->flags &= ~IGB_FLAG_WOL_SUPPORTED;
 		break;
 	case E1000_DEV_ID_82575EB_FIBER_SERDES:
 	case E1000_DEV_ID_82576_FIBER:
 	case E1000_DEV_ID_82576_SERDES:
 		/* Wake events only supported on port A for dual fiber
 		 * regardless of eeprom setting */
-		if (rd32(E1000_STATUS) & E1000_STATUS_FUNC_1)
-			adapter->eeprom_wol = 0;
+		if (E1000_READ_REG(hw, E1000_STATUS) & E1000_STATUS_FUNC_1)
+			adapter->flags &= ~IGB_FLAG_WOL_SUPPORTED;
 		break;
 	case E1000_DEV_ID_82576_QUAD_COPPER:
 	case E1000_DEV_ID_82576_QUAD_COPPER_ET2:
 		/* if quad port adapter, disable WoL on all but port A */
 		if (global_quad_port_a != 0)
-			adapter->eeprom_wol = 0;
+			adapter->flags &= ~IGB_FLAG_WOL_SUPPORTED;
 		else
 			adapter->flags |= IGB_FLAG_QUAD_PORT_A;
 		/* Reset for multiple quad port adapters */
 		if (++global_quad_port_a == 4)
 			global_quad_port_a = 0;
 		break;
+	default:
+		break;
 	}
 
 	/* initialize the wol settings based on the eeprom settings */
-	adapter->wol = adapter->eeprom_wol;
-	device_set_wakeup_enable(&adapter->pdev->dev, adapter->wol);
+	if (adapter->flags & IGB_FLAG_WOL_SUPPORTED)
+		adapter->wol |= E1000_WUFC_MAG;
+
+	/* Some vendors want WoL disabled by default, but still supported */
+	if ((hw->mac.type == e1000_i350) &&
+	    (pdev->subsystem_vendor == PCI_VENDOR_ID_HP)) {
+		adapter->flags |= IGB_FLAG_WOL_SUPPORTED;
+		adapter->wol = 0;
+	}
+
+	device_set_wakeup_enable(pci_dev_to_dev(adapter->pdev),
+				 adapter->flags & IGB_FLAG_WOL_SUPPORTED);
 
 	/* reset the hardware with the new settings */
 	igb_reset(adapter);
+	adapter->devrc = 0;
+
+#ifdef HAVE_I2C_SUPPORT
+	/* Init the I2C interface */
+	err = igb_init_i2c(adapter);
+	if (err) {
+		dev_err(&pdev->dev, "failed to init i2c interface\n");
+		goto err_eeprom;
+	}
+#endif /* HAVE_I2C_SUPPORT */
 
 	/* let the f/w know that the h/w is now under the control of the
 	 * driver. */
 	igb_get_hw_control(adapter);
 
-	strcpy(netdev->name, "eth%d");
+	strncpy(netdev->name, "eth%d", IFNAMSIZ);
 	err = register_netdev(netdev);
 	if (err)
 		goto err_register;
 
-	igb_vlan_mode(netdev, netdev->features);
-
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+	err = igb_init_vmdq_netdevs(adapter);
+	if (err)
+		goto err_register;
+#endif
 	/* carrier off reporting is important to ethtool even BEFORE open */
 	netif_carrier_off(netdev);
 
-#ifdef CONFIG_IGB_DCA
-	if (dca_add_requester(&pdev->dev) == 0) {
+#ifdef IGB_DCA
+	if (dca_add_requester(&pdev->dev) == E1000_SUCCESS) {
 		adapter->flags |= IGB_FLAG_DCA_ENABLED;
-		dev_info(&pdev->dev, "DCA enabled\n");
+		dev_info(pci_dev_to_dev(pdev), "DCA enabled\n");
 		igb_setup_dca(adapter);
 	}
 
 #endif
+#ifdef HAVE_PTP_1588_CLOCK
 	/* do hw tstamp init after resetting */
-	igb_init_hw_timer(adapter);
+	igb_ptp_init(adapter);
+#endif /* HAVE_PTP_1588_CLOCK */
 
-	dev_info(&pdev->dev, "Intel(R) Gigabit Ethernet Network Connection\n");
+	dev_info(pci_dev_to_dev(pdev), "Intel(R) Gigabit Ethernet Network Connection\n");
 	/* print bus type/speed/width info */
-	dev_info(&pdev->dev, "%s: (PCIe:%s:%s) %pM\n",
-		 netdev->name,
-		 ((hw->bus.speed == e1000_bus_speed_2500) ? "2.5Gb/s" :
-		  (hw->bus.speed == e1000_bus_speed_5000) ? "5.0Gb/s" :
-		                                            "unknown"),
-		 ((hw->bus.width == e1000_bus_width_pcie_x4) ? "Width x4" :
-		  (hw->bus.width == e1000_bus_width_pcie_x2) ? "Width x2" :
-		  (hw->bus.width == e1000_bus_width_pcie_x1) ? "Width x1" :
-		   "unknown"),
-		 netdev->dev_addr);
-
-	ret_val = igb_read_part_string(hw, part_str, E1000_PBANUM_LENGTH);
+	dev_info(pci_dev_to_dev(pdev), "%s: (PCIe:%s:%s) ",
+	         netdev->name,
+	         ((hw->bus.speed == e1000_bus_speed_2500) ? "2.5GT/s" :
+	          (hw->bus.speed == e1000_bus_speed_5000) ? "5.0GT/s" :
+		  (hw->mac.type == e1000_i354) ? "integrated" :
+	                                                    "unknown"),
+	         ((hw->bus.width == e1000_bus_width_pcie_x4) ? "Width x4" :
+	          (hw->bus.width == e1000_bus_width_pcie_x2) ? "Width x2" :
+	          (hw->bus.width == e1000_bus_width_pcie_x1) ? "Width x1" :
+		  (hw->mac.type == e1000_i354) ? "integrated" :
+	           "unknown"));
+	dev_info(pci_dev_to_dev(pdev), "%s: MAC: ", netdev->name);
+	for (i = 0; i < 6; i++)
+		printk("%2.2x%c", netdev->dev_addr[i], i == 5 ? '\n' : ':');
+
+	ret_val = e1000_read_pba_string(hw, pba_str, E1000_PBANUM_LENGTH);
 	if (ret_val)
-		strcpy(part_str, "Unknown");
-	dev_info(&pdev->dev, "%s: PBA No: %s\n", netdev->name, part_str);
-	dev_info(&pdev->dev,
-		"Using %s interrupts. %d rx queue(s), %d tx queue(s)\n",
-		adapter->msix_entries ? "MSI-X" :
-		(adapter->flags & IGB_FLAG_HAS_MSI) ? "MSI" : "legacy",
-		adapter->num_rx_queues, adapter->num_tx_queues);
-	switch (hw->mac.type) {
-	case e1000_i350:
-		igb_set_eee_i350(hw);
-		break;
-	default:
-		break;
+		strcpy(pba_str, "Unknown");
+	dev_info(pci_dev_to_dev(pdev), "%s: PBA No: %s\n", netdev->name,
+		 pba_str);
+
+
+	/* Initialize the thermal sensor on i350 devices. */
+	if (hw->mac.type == e1000_i350) {
+		if (hw->bus.func == 0) {
+			u16 ets_word;
+
+			/*
+			 * Read the NVM to determine if this i350 device
+			 * supports an external thermal sensor.
+			 */
+			e1000_read_nvm(hw, NVM_ETS_CFG, 1, &ets_word);
+			if (ets_word != 0x0000 && ets_word != 0xFFFF)
+				adapter->ets = true;
+			else
+				adapter->ets = false;
+		}
+#ifdef IGB_HWMON
+
+		igb_sysfs_init(adapter);
+#else
+#ifdef IGB_PROCFS
+
+		igb_procfs_init(adapter);
+#endif /* IGB_PROCFS */
+#endif /* IGB_HWMON */
+	} else {
+		adapter->ets = false;
+	}
+
+	if (hw->phy.media_type == e1000_media_type_copper) {
+		switch (hw->mac.type) {
+		case e1000_i350:
+		case e1000_i210:
+		case e1000_i211:
+			/* Enable EEE for internal copper PHY devices */
+			err = e1000_set_eee_i350(hw);
+			if ((!err) &&
+			    (adapter->flags & IGB_FLAG_EEE))
+				adapter->eee_advert =
+					MDIO_EEE_100TX | MDIO_EEE_1000T;
+			break;
+		case e1000_i354:
+			if ((E1000_READ_REG(hw, E1000_CTRL_EXT)) &
+			    (E1000_CTRL_EXT_LINK_MODE_SGMII)) {
+				err = e1000_set_eee_i354(hw);
+				if ((!err) &&
+				    (adapter->flags & IGB_FLAG_EEE))
+					adapter->eee_advert =
+					   MDIO_EEE_100TX | MDIO_EEE_1000T;
+			}
+			break;
+		default:
+			break;
+		}
 	}
+
+	/* send driver version info to firmware */
+	if ((hw->mac.type >= e1000_i350) &&
+	    (e1000_get_flash_presence_i210(hw)))
+		igb_init_fw(adapter);
+
+#ifndef IGB_NO_LRO
+	if (netdev->features & NETIF_F_LRO)
+		dev_info(pci_dev_to_dev(pdev), "Internal LRO is enabled \n");
+	else
+		dev_info(pci_dev_to_dev(pdev), "LRO is disabled \n");
+#endif
+	dev_info(pci_dev_to_dev(pdev),
+	         "Using %s interrupts. %d rx queue(s), %d tx queue(s)\n",
+	         adapter->msix_entries ? "MSI-X" :
+	         (adapter->flags & IGB_FLAG_HAS_MSI) ? "MSI" : "legacy",
+	         adapter->num_rx_queues, adapter->num_tx_queues);
+
+	cards_found++;
+
+	pm_runtime_put_noidle(&pdev->dev);
 	return 0;
 
 err_register:
 	igb_release_hw_control(adapter);
+#ifdef HAVE_I2C_SUPPORT
+	memset(&adapter->i2c_adap, 0, sizeof(adapter->i2c_adap));
+#endif /* HAVE_I2C_SUPPORT */
 err_eeprom:
-	if (!igb_check_reset_block(hw))
-		igb_reset_phy(hw);
+	if (!e1000_check_reset_block(hw))
+		e1000_phy_hw_reset(hw);
 
 	if (hw->flash_address)
 		iounmap(hw->flash_address);
 err_sw_init:
 	igb_clear_interrupt_scheme(adapter);
+	igb_reset_sriov_capability(adapter);
 	iounmap(hw->hw_addr);
 err_ioremap:
 	free_netdev(netdev);
@@ -2145,6 +3058,19 @@ err_dma:
 	pci_disable_device(pdev);
 	return err;
 }
+#ifdef HAVE_I2C_SUPPORT
+/*
+ *  igb_remove_i2c - Cleanup  I2C interface
+ *  @adapter: pointer to adapter structure
+ *
+ */
+static void igb_remove_i2c(struct igb_adapter *adapter)
+{
+
+	/* free the adapter bus structure */
+	i2c_del_adapter(&adapter->i2c_adap);
+}
+#endif /* HAVE_I2C_SUPPORT */
 
 /**
  * igb_remove - Device Removal Routine
@@ -2161,23 +3087,30 @@ static void __devexit igb_remove(struct pci_dev *pdev)
 	struct igb_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
 
-	/*
-	 * The watchdog timer may be rescheduled, so explicitly
-	 * disable watchdog from being rescheduled.
-	 */
+	pm_runtime_get_noresume(&pdev->dev);
+#ifdef HAVE_I2C_SUPPORT
+	igb_remove_i2c(adapter);
+#endif /* HAVE_I2C_SUPPORT */
+#ifdef HAVE_PTP_1588_CLOCK
+	igb_ptp_stop(adapter);
+#endif /* HAVE_PTP_1588_CLOCK */
+
+	/* flush_scheduled work may reschedule our watchdog task, so
+	 * explicitly disable watchdog tasks from being rescheduled  */
 	set_bit(__IGB_DOWN, &adapter->state);
 	del_timer_sync(&adapter->watchdog_timer);
+	if (adapter->flags & IGB_FLAG_DETECT_BAD_DMA)
+		del_timer_sync(&adapter->dma_err_timer);
 	del_timer_sync(&adapter->phy_info_timer);
 
-	cancel_work_sync(&adapter->reset_task);
-	cancel_work_sync(&adapter->watchdog_task);
+	flush_scheduled_work();
 
-#ifdef CONFIG_IGB_DCA
+#ifdef IGB_DCA
 	if (adapter->flags & IGB_FLAG_DCA_ENABLED) {
-		dev_info(&pdev->dev, "DCA disabled\n");
+		dev_info(pci_dev_to_dev(pdev), "DCA disabled\n");
 		dca_remove_requester(&pdev->dev);
 		adapter->flags &= ~IGB_FLAG_DCA_ENABLED;
-		wr32(E1000_DCA_CTRL, E1000_DCA_CTRL_DCA_MODE_DISABLE);
+		E1000_WRITE_REG(hw, E1000_DCA_CTRL, E1000_DCA_CTRL_DCA_DISABLE);
 	}
 #endif
 
@@ -2186,24 +3119,12 @@ static void __devexit igb_remove(struct pci_dev *pdev)
 	igb_release_hw_control(adapter);
 
 	unregister_netdev(netdev);
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+	igb_remove_vmdq_netdevs(adapter);
+#endif
 
 	igb_clear_interrupt_scheme(adapter);
-
-#ifdef CONFIG_PCI_IOV
-	/* reclaim resources allocated to VFs */
-	if (adapter->vf_data) {
-		/* disable iov and allow time for transactions to clear */
-		pci_disable_sriov(pdev);
-		msleep(500);
-
-		kfree(adapter->vf_data);
-		adapter->vf_data = NULL;
-		wr32(E1000_IOVCTL, E1000_IOVCTL_REUSE_VFQ);
-		wrfl();
-		msleep(100);
-		dev_info(&pdev->dev, "IOV Disabled\n");
-	}
-#endif
+	igb_reset_sriov_capability(adapter);
 
 	iounmap(hw->hw_addr);
 	if (hw->flash_address)
@@ -2211,6 +3132,15 @@ static void __devexit igb_remove(struct pci_dev *pdev)
 	pci_release_selected_regions(pdev,
 	                             pci_select_bars(pdev, IORESOURCE_MEM));
 
+#ifdef IGB_HWMON
+	igb_sysfs_exit(adapter);
+#else
+#ifdef IGB_PROCFS
+	igb_procfs_exit(adapter);
+#endif /* IGB_PROCFS */
+#endif /* IGB_HWMON */
+	kfree(adapter->mac_table);
+	kfree(adapter->shadow_vfta);
 	free_netdev(netdev);
 
 	pci_disable_pcie_error_reporting(pdev);
@@ -2219,161 +3149,6 @@ static void __devexit igb_remove(struct pci_dev *pdev)
 }
 
 /**
- * igb_probe_vfs - Initialize vf data storage and add VFs to pci config space
- * @adapter: board private structure to initialize
- *
- * This function initializes the vf specific data storage and then attempts to
- * allocate the VFs.  The reason for ordering it this way is because it is much
- * mor expensive time wise to disable SR-IOV than it is to allocate and free
- * the memory for the VFs.
- **/
-static void __devinit igb_probe_vfs(struct igb_adapter * adapter)
-{
-#ifdef CONFIG_PCI_IOV
-	struct pci_dev *pdev = adapter->pdev;
-
-	if (adapter->vfs_allocated_count) {
-		adapter->vf_data = kcalloc(adapter->vfs_allocated_count,
-		                           sizeof(struct vf_data_storage),
-		                           GFP_KERNEL);
-		/* if allocation failed then we do not support SR-IOV */
-		if (!adapter->vf_data) {
-			adapter->vfs_allocated_count = 0;
-			dev_err(&pdev->dev, "Unable to allocate memory for VF "
-			        "Data Storage\n");
-		}
-	}
-
-	if (pci_enable_sriov(pdev, adapter->vfs_allocated_count)) {
-		kfree(adapter->vf_data);
-		adapter->vf_data = NULL;
-#endif /* CONFIG_PCI_IOV */
-		adapter->vfs_allocated_count = 0;
-#ifdef CONFIG_PCI_IOV
-	} else {
-		unsigned char mac_addr[ETH_ALEN];
-		int i;
-		dev_info(&pdev->dev, "%d vfs allocated\n",
-		         adapter->vfs_allocated_count);
-		for (i = 0; i < adapter->vfs_allocated_count; i++) {
-			random_ether_addr(mac_addr);
-			igb_set_vf_mac(adapter, i, mac_addr);
-		}
-		/* DMA Coalescing is not supported in IOV mode. */
-		if (adapter->flags & IGB_FLAG_DMAC)
-			adapter->flags &= ~IGB_FLAG_DMAC;
-	}
-#endif /* CONFIG_PCI_IOV */
-}
-
-
-/**
- * igb_init_hw_timer - Initialize hardware timer used with IEEE 1588 timestamp
- * @adapter: board private structure to initialize
- *
- * igb_init_hw_timer initializes the function pointer and values for the hw
- * timer found in hardware.
- **/
-static void igb_init_hw_timer(struct igb_adapter *adapter)
-{
-	struct e1000_hw *hw = &adapter->hw;
-
-	switch (hw->mac.type) {
-	case e1000_i350:
-	case e1000_82580:
-		memset(&adapter->cycles, 0, sizeof(adapter->cycles));
-		adapter->cycles.read = igb_read_clock;
-		adapter->cycles.mask = CLOCKSOURCE_MASK(64);
-		adapter->cycles.mult = 1;
-		/*
-		 * The 82580 timesync updates the system timer every 8ns by 8ns
-		 * and the value cannot be shifted.  Instead we need to shift
-		 * the registers to generate a 64bit timer value.  As a result
-		 * SYSTIMR/L/H, TXSTMPL/H, RXSTMPL/H all have to be shifted by
-		 * 24 in order to generate a larger value for synchronization.
-		 */
-		adapter->cycles.shift = IGB_82580_TSYNC_SHIFT;
-		/* disable system timer temporarily by setting bit 31 */
-		wr32(E1000_TSAUXC, 0x80000000);
-		wrfl();
-
-		/* Set registers so that rollover occurs soon to test this. */
-		wr32(E1000_SYSTIMR, 0x00000000);
-		wr32(E1000_SYSTIML, 0x80000000);
-		wr32(E1000_SYSTIMH, 0x000000FF);
-		wrfl();
-
-		/* enable system timer by clearing bit 31 */
-		wr32(E1000_TSAUXC, 0x0);
-		wrfl();
-
-		timecounter_init(&adapter->clock,
-				 &adapter->cycles,
-				 ktime_to_ns(ktime_get_real()));
-		/*
-		 * Synchronize our NIC clock against system wall clock. NIC
-		 * time stamp reading requires ~3us per sample, each sample
-		 * was pretty stable even under load => only require 10
-		 * samples for each offset comparison.
-		 */
-		memset(&adapter->compare, 0, sizeof(adapter->compare));
-		adapter->compare.source = &adapter->clock;
-		adapter->compare.target = ktime_get_real;
-		adapter->compare.num_samples = 10;
-		timecompare_update(&adapter->compare, 0);
-		break;
-	case e1000_82576:
-		/*
-		 * Initialize hardware timer: we keep it running just in case
-		 * that some program needs it later on.
-		 */
-		memset(&adapter->cycles, 0, sizeof(adapter->cycles));
-		adapter->cycles.read = igb_read_clock;
-		adapter->cycles.mask = CLOCKSOURCE_MASK(64);
-		adapter->cycles.mult = 1;
-		/**
-		 * Scale the NIC clock cycle by a large factor so that
-		 * relatively small clock corrections can be added or
-		 * subtracted at each clock tick. The drawbacks of a large
-		 * factor are a) that the clock register overflows more quickly
-		 * (not such a big deal) and b) that the increment per tick has
-		 * to fit into 24 bits.  As a result we need to use a shift of
-		 * 19 so we can fit a value of 16 into the TIMINCA register.
-		 */
-		adapter->cycles.shift = IGB_82576_TSYNC_SHIFT;
-		wr32(E1000_TIMINCA,
-		                (1 << E1000_TIMINCA_16NS_SHIFT) |
-		                (16 << IGB_82576_TSYNC_SHIFT));
-
-		/* Set registers so that rollover occurs soon to test this. */
-		wr32(E1000_SYSTIML, 0x00000000);
-		wr32(E1000_SYSTIMH, 0xFF800000);
-		wrfl();
-
-		timecounter_init(&adapter->clock,
-				 &adapter->cycles,
-				 ktime_to_ns(ktime_get_real()));
-		/*
-		 * Synchronize our NIC clock against system wall clock. NIC
-		 * time stamp reading requires ~3us per sample, each sample
-		 * was pretty stable even under load => only require 10
-		 * samples for each offset comparison.
-		 */
-		memset(&adapter->compare, 0, sizeof(adapter->compare));
-		adapter->compare.source = &adapter->clock;
-		adapter->compare.target = ktime_get_real;
-		adapter->compare.num_samples = 10;
-		timecompare_update(&adapter->compare, 0);
-		break;
-	case e1000_82575:
-		/* 82575 does not support timesync */
-	default:
-		break;
-	}
-
-}
-
-/**
  * igb_sw_init - Initialize general software structures (struct igb_adapter)
  * @adapter: board private structure to initialize
  *
@@ -2387,60 +3162,56 @@ static int __devinit igb_sw_init(struct igb_adapter *adapter)
 	struct net_device *netdev = adapter->netdev;
 	struct pci_dev *pdev = adapter->pdev;
 
+	/* PCI config space info */
+
+	hw->vendor_id = pdev->vendor;
+	hw->device_id = pdev->device;
+	hw->subsystem_vendor_id = pdev->subsystem_vendor;
+	hw->subsystem_device_id = pdev->subsystem_device;
+
+	pci_read_config_byte(pdev, PCI_REVISION_ID, &hw->revision_id);
+
 	pci_read_config_word(pdev, PCI_COMMAND, &hw->bus.pci_cmd_word);
 
+	/* set default ring sizes */
 	adapter->tx_ring_count = IGB_DEFAULT_TXD;
 	adapter->rx_ring_count = IGB_DEFAULT_RXD;
-	adapter->rx_itr_setting = IGB_DEFAULT_ITR;
-	adapter->tx_itr_setting = IGB_DEFAULT_ITR;
 
-	adapter->max_frame_size = netdev->mtu + ETH_HLEN + ETH_FCS_LEN;
-	adapter->min_frame_size = ETH_ZLEN + ETH_FCS_LEN;
+	/* set default work limits */
+	adapter->tx_work_limit = IGB_DEFAULT_TX_WORK;
 
-	spin_lock_init(&adapter->stats64_lock);
-#ifdef CONFIG_PCI_IOV
-	switch (hw->mac.type) {
-	case e1000_82576:
-	case e1000_i350:
-		if (max_vfs > 7) {
-			dev_warn(&pdev->dev,
-				 "Maximum of 7 VFs per PF, using max\n");
-			adapter->vfs_allocated_count = 7;
-		} else
-			adapter->vfs_allocated_count = max_vfs;
-		break;
-	default:
-		break;
+	adapter->max_frame_size = netdev->mtu + ETH_HLEN + ETH_FCS_LEN +
+					      VLAN_HLEN;
+
+	/* Initialize the hardware-specific values */
+	if (e1000_setup_init_funcs(hw, TRUE)) {
+		dev_err(pci_dev_to_dev(pdev), "Hardware Initialization Failure\n");
+		return -EIO;
 	}
-#endif /* CONFIG_PCI_IOV */
-	adapter->rss_queues = min_t(u32, IGB_MAX_RX_QUEUES, num_online_cpus());
-	/* i350 cannot do RSS and SR-IOV at the same time */
-	if (hw->mac.type == e1000_i350 && adapter->vfs_allocated_count)
-		adapter->rss_queues = 1;
 
-	/*
-	 * if rss_queues > 4 or vfs are going to be allocated with rss_queues
-	 * then we should combine the queues into a queue pair in order to
-	 * conserve interrupts due to limited supply
-	 */
-	if ((adapter->rss_queues > 4) ||
-	    ((adapter->rss_queues > 1) && (adapter->vfs_allocated_count > 6)))
-		adapter->flags |= IGB_FLAG_QUEUE_PAIRS;
+	igb_check_options(adapter);
 
-	/* This call may decrease the number of queues */
-	if (igb_init_interrupt_scheme(adapter)) {
-		dev_err(&pdev->dev, "Unable to allocate memory for queues\n");
-		return -ENOMEM;
+	adapter->mac_table = kzalloc(sizeof(struct igb_mac_addr) *
+				     hw->mac.rar_entry_count,
+				     GFP_ATOMIC);
+
+	/* Setup and initialize a copy of the hw vlan table array */
+	adapter->shadow_vfta = (u32 *)kzalloc(sizeof(u32) * E1000_VFTA_ENTRIES,
+					GFP_ATOMIC);
+
+	/* These calls may decrease the number of queues */
+	if (hw->mac.type < e1000_i210) {
+		igb_set_sriov_capability(adapter);
 	}
 
-	igb_probe_vfs(adapter);
+	if (igb_init_interrupt_scheme(adapter, true)) {
+		dev_err(pci_dev_to_dev(pdev), "Unable to allocate memory for queues\n");
+		return -ENOMEM;
+	}
 
 	/* Explicitly disable IRQ since the NIC can be in any state. */
 	igb_irq_disable(adapter);
 
-	if (hw->mac.type == e1000_i350)
-		adapter->flags &= ~IGB_FLAG_DMAC;
-
 	set_bit(__IGB_DOWN, &adapter->state);
 	return 0;
 }
@@ -2457,16 +3228,26 @@ static int __devinit igb_sw_init(struct igb_adapter *adapter)
  * handler is registered with the OS, the watchdog timer is started,
  * and the stack is notified that the interface is ready.
  **/
-static int igb_open(struct net_device *netdev)
+static int __igb_open(struct net_device *netdev, bool resuming)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
+#ifdef CONFIG_PM_RUNTIME
+	struct pci_dev *pdev = adapter->pdev;
+#endif /* CONFIG_PM_RUNTIME */
 	int err;
 	int i;
 
 	/* disallow open during test */
-	if (test_bit(__IGB_TESTING, &adapter->state))
+	if (test_bit(__IGB_TESTING, &adapter->state)) {
+		WARN_ON(resuming);
 		return -EBUSY;
+	}
+
+#ifdef CONFIG_PM_RUNTIME
+	if (!resuming)
+		pm_runtime_get_sync(&pdev->dev);
+#endif /* CONFIG_PM_RUNTIME */
 
 	netif_carrier_off(netdev);
 
@@ -2492,34 +3273,49 @@ static int igb_open(struct net_device *netdev)
 	if (err)
 		goto err_req_irq;
 
+	/* Notify the stack of the actual queue counts. */
+	netif_set_real_num_tx_queues(netdev,
+				     adapter->vmdq_pools ? 1 :
+				     adapter->num_tx_queues);
+
+	err = netif_set_real_num_rx_queues(netdev,
+					   adapter->vmdq_pools ? 1 :
+					   adapter->num_rx_queues);
+	if (err)
+		goto err_set_queues;
+
 	/* From here on the code is the same as igb_up() */
 	clear_bit(__IGB_DOWN, &adapter->state);
 
-	for (i = 0; i < adapter->num_q_vectors; i++) {
-		struct igb_q_vector *q_vector = adapter->q_vector[i];
-		napi_enable(&q_vector->napi);
-	}
+	for (i = 0; i < adapter->num_q_vectors; i++)
+		napi_enable(&(adapter->q_vector[i]->napi));
+	igb_configure_lli(adapter);
 
 	/* Clear any pending interrupts. */
-	rd32(E1000_ICR);
+	E1000_READ_REG(hw, E1000_ICR);
 
 	igb_irq_enable(adapter);
 
 	/* notify VFs that reset has been completed */
 	if (adapter->vfs_allocated_count) {
-		u32 reg_data = rd32(E1000_CTRL_EXT);
+		u32 reg_data = E1000_READ_REG(hw, E1000_CTRL_EXT);
 		reg_data |= E1000_CTRL_EXT_PFRSTD;
-		wr32(E1000_CTRL_EXT, reg_data);
+		E1000_WRITE_REG(hw, E1000_CTRL_EXT, reg_data);
 	}
 
 	netif_tx_start_all_queues(netdev);
 
+	if (adapter->flags & IGB_FLAG_DETECT_BAD_DMA)
+		schedule_work(&adapter->dma_err_task);
+
 	/* start the watchdog. */
 	hw->mac.get_link_status = 1;
 	schedule_work(&adapter->watchdog_task);
 
-	return 0;
+	return E1000_SUCCESS;
 
+err_set_queues:
+	igb_free_irq(adapter);
 err_req_irq:
 	igb_release_hw_control(adapter);
 	igb_power_down_link(adapter);
@@ -2529,9 +3325,19 @@ err_setup_rx:
 err_setup_tx:
 	igb_reset(adapter);
 
+#ifdef CONFIG_PM_RUNTIME
+	if (!resuming)
+		pm_runtime_put(&pdev->dev);
+#endif /* CONFIG_PM_RUNTIME */
+
 	return err;
 }
 
+static int igb_open(struct net_device *netdev)
+{
+	return __igb_open(netdev, false);
+}
+
 /**
  * igb_close - Disables a network interface
  * @netdev: network interface device structure
@@ -2543,21 +3349,42 @@ err_setup_tx:
  * needs to be disabled.  A global MAC reset is issued to stop the
  * hardware, and all transmit and receive resources are freed.
  **/
-static int igb_close(struct net_device *netdev)
+static int __igb_close(struct net_device *netdev, bool suspending)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
+#ifdef CONFIG_PM_RUNTIME
+	struct pci_dev *pdev = adapter->pdev;
+#endif /* CONFIG_PM_RUNTIME */
 
 	WARN_ON(test_bit(__IGB_RESETTING, &adapter->state));
+
+#ifdef CONFIG_PM_RUNTIME
+	if (!suspending)
+		pm_runtime_get_sync(&pdev->dev);
+#endif /* CONFIG_PM_RUNTIME */
+
 	igb_down(adapter);
 
+	igb_release_hw_control(adapter);
+
 	igb_free_irq(adapter);
 
 	igb_free_all_tx_resources(adapter);
 	igb_free_all_rx_resources(adapter);
 
+#ifdef CONFIG_PM_RUNTIME
+	if (!suspending)
+		pm_runtime_put_sync(&pdev->dev);
+#endif /* CONFIG_PM_RUNTIME */
+
 	return 0;
 }
 
+static int igb_close(struct net_device *netdev)
+{
+	return __igb_close(netdev, false);
+}
+
 /**
  * igb_setup_tx_resources - allocate Tx resources (Descriptors)
  * @tx_ring: tx descriptor ring (for a specific queue) to setup
@@ -2569,29 +3396,28 @@ int igb_setup_tx_resources(struct igb_ring *tx_ring)
 	struct device *dev = tx_ring->dev;
 	int size;
 
-	size = sizeof(struct igb_buffer) * tx_ring->count;
-	tx_ring->buffer_info = vzalloc(size);
-	if (!tx_ring->buffer_info)
+	size = sizeof(struct igb_tx_buffer) * tx_ring->count;
+	tx_ring->tx_buffer_info = vzalloc(size);
+	if (!tx_ring->tx_buffer_info)
 		goto err;
 
 	/* round up to nearest 4K */
 	tx_ring->size = tx_ring->count * sizeof(union e1000_adv_tx_desc);
 	tx_ring->size = ALIGN(tx_ring->size, 4096);
 
-	tx_ring->desc = dma_alloc_coherent(dev,
-					   tx_ring->size,
-					   &tx_ring->dma,
-					   GFP_KERNEL);
+	tx_ring->desc = dma_alloc_coherent(dev, tx_ring->size,
+					   &tx_ring->dma, GFP_KERNEL);
 
 	if (!tx_ring->desc)
 		goto err;
 
 	tx_ring->next_to_use = 0;
 	tx_ring->next_to_clean = 0;
+
 	return 0;
 
 err:
-	vfree(tx_ring->buffer_info);
+	vfree(tx_ring->tx_buffer_info);
 	dev_err(dev,
 		"Unable to allocate memory for the transmit descriptor ring\n");
 	return -ENOMEM;
@@ -2612,7 +3438,7 @@ static int igb_setup_all_tx_resources(struct igb_adapter *adapter)
 	for (i = 0; i < adapter->num_tx_queues; i++) {
 		err = igb_setup_tx_resources(adapter->tx_ring[i]);
 		if (err) {
-			dev_err(&pdev->dev,
+			dev_err(pci_dev_to_dev(pdev),
 				"Allocation for Tx Queue %u failed\n", i);
 			for (i--; i >= 0; i--)
 				igb_free_tx_resources(adapter->tx_ring[i]);
@@ -2620,10 +3446,6 @@ static int igb_setup_all_tx_resources(struct igb_adapter *adapter)
 		}
 	}
 
-	for (i = 0; i < IGB_ABS_MAX_TX_QUEUES; i++) {
-		int r_idx = i % adapter->num_tx_queues;
-		adapter->multi_tx_table[i] = adapter->tx_ring[r_idx];
-	}
 	return err;
 }
 
@@ -2637,20 +3459,36 @@ void igb_setup_tctl(struct igb_adapter *adapter)
 	u32 tctl;
 
 	/* disable queue 0 which is enabled by default on 82575 and 82576 */
-	wr32(E1000_TXDCTL(0), 0);
+	E1000_WRITE_REG(hw, E1000_TXDCTL(0), 0);
 
 	/* Program the Transmit Control Register */
-	tctl = rd32(E1000_TCTL);
+	tctl = E1000_READ_REG(hw, E1000_TCTL);
 	tctl &= ~E1000_TCTL_CT;
 	tctl |= E1000_TCTL_PSP | E1000_TCTL_RTLC |
 		(E1000_COLLISION_THRESHOLD << E1000_CT_SHIFT);
 
-	igb_config_collision_dist(hw);
+	e1000_config_collision_dist(hw);
 
 	/* Enable transmits */
 	tctl |= E1000_TCTL_EN;
 
-	wr32(E1000_TCTL, tctl);
+	E1000_WRITE_REG(hw, E1000_TCTL, tctl);
+}
+
+static u32 igb_tx_wthresh(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	switch (hw->mac.type) {
+	case e1000_i354:
+		return 4;
+	case e1000_82576:
+		if (adapter->msix_entries)
+			return 1;
+	default:
+		break;
+	}
+
+	return 16;
 }
 
 /**
@@ -2664,34 +3502,31 @@ void igb_configure_tx_ring(struct igb_adapter *adapter,
                            struct igb_ring *ring)
 {
 	struct e1000_hw *hw = &adapter->hw;
-	u32 txdctl;
+	u32 txdctl = 0;
 	u64 tdba = ring->dma;
 	int reg_idx = ring->reg_idx;
 
 	/* disable the queue */
-	txdctl = rd32(E1000_TXDCTL(reg_idx));
-	wr32(E1000_TXDCTL(reg_idx),
-	                txdctl & ~E1000_TXDCTL_QUEUE_ENABLE);
-	wrfl();
+	E1000_WRITE_REG(hw, E1000_TXDCTL(reg_idx), 0);
+	E1000_WRITE_FLUSH(hw);
 	mdelay(10);
 
-	wr32(E1000_TDLEN(reg_idx),
+	E1000_WRITE_REG(hw, E1000_TDLEN(reg_idx),
 	                ring->count * sizeof(union e1000_adv_tx_desc));
-	wr32(E1000_TDBAL(reg_idx),
+	E1000_WRITE_REG(hw, E1000_TDBAL(reg_idx),
 	                tdba & 0x00000000ffffffffULL);
-	wr32(E1000_TDBAH(reg_idx), tdba >> 32);
+	E1000_WRITE_REG(hw, E1000_TDBAH(reg_idx), tdba >> 32);
 
-	ring->head = hw->hw_addr + E1000_TDH(reg_idx);
 	ring->tail = hw->hw_addr + E1000_TDT(reg_idx);
-	writel(0, ring->head);
+	E1000_WRITE_REG(hw, E1000_TDH(reg_idx), 0);
 	writel(0, ring->tail);
 
 	txdctl |= IGB_TX_PTHRESH;
 	txdctl |= IGB_TX_HTHRESH << 8;
-	txdctl |= IGB_TX_WTHRESH << 16;
+	txdctl |= igb_tx_wthresh(adapter) << 16;
 
 	txdctl |= E1000_TXDCTL_QUEUE_ENABLE;
-	wr32(E1000_TXDCTL(reg_idx), txdctl);
+	E1000_WRITE_REG(hw, E1000_TXDCTL(reg_idx), txdctl);
 }
 
 /**
@@ -2719,9 +3554,9 @@ int igb_setup_rx_resources(struct igb_ring *rx_ring)
 	struct device *dev = rx_ring->dev;
 	int size, desc_len;
 
-	size = sizeof(struct igb_buffer) * rx_ring->count;
-	rx_ring->buffer_info = vzalloc(size);
-	if (!rx_ring->buffer_info)
+	size = sizeof(struct igb_rx_buffer) * rx_ring->count;
+	rx_ring->rx_buffer_info = vzalloc(size);
+	if (!rx_ring->rx_buffer_info)
 		goto err;
 
 	desc_len = sizeof(union e1000_adv_rx_desc);
@@ -2730,22 +3565,21 @@ int igb_setup_rx_resources(struct igb_ring *rx_ring)
 	rx_ring->size = rx_ring->count * desc_len;
 	rx_ring->size = ALIGN(rx_ring->size, 4096);
 
-	rx_ring->desc = dma_alloc_coherent(dev,
-					   rx_ring->size,
-					   &rx_ring->dma,
-					   GFP_KERNEL);
+	rx_ring->desc = dma_alloc_coherent(dev, rx_ring->size,
+					   &rx_ring->dma, GFP_KERNEL);
 
 	if (!rx_ring->desc)
 		goto err;
 
+	rx_ring->next_to_alloc = 0;
 	rx_ring->next_to_clean = 0;
 	rx_ring->next_to_use = 0;
 
 	return 0;
 
 err:
-	vfree(rx_ring->buffer_info);
-	rx_ring->buffer_info = NULL;
+	vfree(rx_ring->rx_buffer_info);
+	rx_ring->rx_buffer_info = NULL;
 	dev_err(dev, "Unable to allocate memory for the receive descriptor"
 		" ring\n");
 	return -ENOMEM;
@@ -2766,7 +3600,7 @@ static int igb_setup_all_rx_resources(struct igb_adapter *adapter)
 	for (i = 0; i < adapter->num_rx_queues; i++) {
 		err = igb_setup_rx_resources(adapter->rx_ring[i]);
 		if (err) {
-			dev_err(&pdev->dev,
+			dev_err(pci_dev_to_dev(pdev),
 				"Allocation for Rx Queue %u failed\n", i);
 			for (i--; i >= 0; i--)
 				igb_free_rx_resources(adapter->rx_ring[i]);
@@ -2785,65 +3619,82 @@ static void igb_setup_mrqc(struct igb_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	u32 mrqc, rxcsum;
-	u32 j, num_rx_queues, shift = 0, shift2 = 0;
-	union e1000_reta {
-		u32 dword;
-		u8  bytes[4];
-	} reta;
-	static const u8 rsshash[40] = {
-		0x6d, 0x5a, 0x56, 0xda, 0x25, 0x5b, 0x0e, 0xc2, 0x41, 0x67,
-		0x25, 0x3d, 0x43, 0xa3, 0x8f, 0xb0, 0xd0, 0xca, 0x2b, 0xcb,
-		0xae, 0x7b, 0x30, 0xb4,	0x77, 0xcb, 0x2d, 0xa3, 0x80, 0x30,
-		0xf2, 0x0c, 0x6a, 0x42, 0xb7, 0x3b, 0xbe, 0xac, 0x01, 0xfa };
+	u32 j, num_rx_queues;
+#ifndef ETHTOOL_SRXFHINDIR
+	u32 shift = 0, shift2 = 0;
+#endif /* ETHTOOL_SRXFHINDIR */
+	static const u32 rsskey[10] = { 0xDA565A6D, 0xC20E5B25, 0x3D256741,
+					0xB08FA343, 0xCB2BCAD0, 0xB4307BAE,
+					0xA32DCB77, 0x0CF23080, 0x3BB7426A,
+					0xFA01ACBE };
 
 	/* Fill out hash function seeds */
-	for (j = 0; j < 10; j++) {
-		u32 rsskey = rsshash[(j * 4)];
-		rsskey |= rsshash[(j * 4) + 1] << 8;
-		rsskey |= rsshash[(j * 4) + 2] << 16;
-		rsskey |= rsshash[(j * 4) + 3] << 24;
-		array_wr32(E1000_RSSRK(0), j, rsskey);
-	}
+	for (j = 0; j < 10; j++)
+		E1000_WRITE_REG(hw, E1000_RSSRK(j), rsskey[j]);
 
 	num_rx_queues = adapter->rss_queues;
 
-	if (adapter->vfs_allocated_count) {
-		/* 82575 and 82576 supports 2 RSS queues for VMDq */
-		switch (hw->mac.type) {
-		case e1000_i350:
-		case e1000_82580:
-			num_rx_queues = 1;
-			shift = 0;
-			break;
-		case e1000_82576:
-			shift = 3;
+#ifdef ETHTOOL_SRXFHINDIR
+	if (hw->mac.type == e1000_82576) {
+		/* 82576 supports 2 RSS queues for SR-IOV */
+		if (adapter->vfs_allocated_count)
 			num_rx_queues = 2;
-			break;
-		case e1000_82575:
+	}
+	if (adapter->rss_indir_tbl_init != num_rx_queues) {
+		for (j = 0; j < IGB_RETA_SIZE; j++)
+			adapter->rss_indir_tbl[j] = (j * num_rx_queues) / IGB_RETA_SIZE;
+		adapter->rss_indir_tbl_init = num_rx_queues;
+	}
+	igb_write_rss_indir_tbl(adapter);
+#else
+	/* 82575 and 82576 supports 2 RSS queues for VMDq */
+	switch (hw->mac.type) {
+	case e1000_82575:
+		if (adapter->vmdq_pools) {
 			shift = 2;
 			shift2 = 6;
-		default:
-			break;
 		}
-	} else {
-		if (hw->mac.type == e1000_82575)
-			shift = 6;
+		shift = 6;
+		break;
+	case e1000_82576:
+		/* 82576 supports 2 RSS queues for SR-IOV */
+		if (adapter->vfs_allocated_count || adapter->vmdq_pools) {
+			shift = 3;
+			num_rx_queues = 2;
+		}
+		break;
+	default:
+		break;
 	}
 
-	for (j = 0; j < (32 * 4); j++) {
-		reta.bytes[j & 3] = (j % num_rx_queues) << shift;
+	/*
+	 * Populate the redirection table 4 entries at a time.  To do this
+	 * we are generating the results for n and n+2 and then interleaving
+	 * those with the results with n+1 and n+3.
+	 */
+	for (j = 0; j < 32; j++) {
+ 		/* first pass generates n and n+2 */
+ 		u32 base = ((j * 0x00040004) + 0x00020000) * num_rx_queues;
+ 		u32 reta = (base & 0x07800780) >> (7 - shift);
+
+		/* second pass generates n+1 and n+3 */
+		base += 0x00010001 * num_rx_queues;
+		reta |= (base & 0x07800780) << (1 + shift);
+
+		/* generate 2nd table for 82575 based parts */
 		if (shift2)
-			reta.bytes[j & 3] |= num_rx_queues << shift2;
-		if ((j & 3) == 3)
-			wr32(E1000_RETA(j >> 2), reta.dword);
+			reta |= (0x01010101 * num_rx_queues) << shift2;
+
+		E1000_WRITE_REG(hw, E1000_RETA(j), reta);
 	}
+#endif /* ETHTOOL_SRXFHINDIR */
 
 	/*
 	 * Disable raw packet checksumming so that RSS hash is placed in
 	 * descriptor on writeback.  No need to enable TCP/UDP/IP checksum
 	 * offloads as they are enabled by default
 	 */
-	rxcsum = rd32(E1000_RXCSUM);
+	rxcsum = E1000_READ_REG(hw, E1000_RXCSUM);
 	rxcsum |= E1000_RXCSUM_PCSD;
 
 	if (adapter->hw.mac.type >= e1000_82576)
@@ -2851,42 +3702,46 @@ static void igb_setup_mrqc(struct igb_adapter *adapter)
 		rxcsum |= E1000_RXCSUM_CRCOFL;
 
 	/* Don't need to set TUOFL or IPOFL, they default to 1 */
-	wr32(E1000_RXCSUM, rxcsum);
+	E1000_WRITE_REG(hw, E1000_RXCSUM, rxcsum);
+
+	/* Generate RSS hash based on packet types, TCP/UDP
+	 * port numbers and/or IPv4/v6 src and dst addresses
+	 */
+	mrqc = E1000_MRQC_RSS_FIELD_IPV4 |
+	       E1000_MRQC_RSS_FIELD_IPV4_TCP |
+	       E1000_MRQC_RSS_FIELD_IPV6 |
+	       E1000_MRQC_RSS_FIELD_IPV6_TCP |
+	       E1000_MRQC_RSS_FIELD_IPV6_TCP_EX;
+
+	if (adapter->flags & IGB_FLAG_RSS_FIELD_IPV4_UDP)
+		mrqc |= E1000_MRQC_RSS_FIELD_IPV4_UDP;
+	if (adapter->flags & IGB_FLAG_RSS_FIELD_IPV6_UDP)
+		mrqc |= E1000_MRQC_RSS_FIELD_IPV6_UDP;
 
 	/* If VMDq is enabled then we set the appropriate mode for that, else
 	 * we default to RSS so that an RSS hash is calculated per packet even
 	 * if we are only using one queue */
-	if (adapter->vfs_allocated_count) {
+	if (adapter->vfs_allocated_count || adapter->vmdq_pools) {
 		if (hw->mac.type > e1000_82575) {
 			/* Set the default pool for the PF's first queue */
-			u32 vtctl = rd32(E1000_VT_CTL);
+			u32 vtctl = E1000_READ_REG(hw, E1000_VT_CTL);
 			vtctl &= ~(E1000_VT_CTL_DEFAULT_POOL_MASK |
 				   E1000_VT_CTL_DISABLE_DEF_POOL);
 			vtctl |= adapter->vfs_allocated_count <<
 				E1000_VT_CTL_DEFAULT_POOL_SHIFT;
-			wr32(E1000_VT_CTL, vtctl);
+			E1000_WRITE_REG(hw, E1000_VT_CTL, vtctl);
 		}
 		if (adapter->rss_queues > 1)
-			mrqc = E1000_MRQC_ENABLE_VMDQ_RSS_2Q;
+			mrqc |= E1000_MRQC_ENABLE_VMDQ_RSS_2Q;
 		else
-			mrqc = E1000_MRQC_ENABLE_VMDQ;
+			mrqc |= E1000_MRQC_ENABLE_VMDQ;
 	} else {
-		mrqc = E1000_MRQC_ENABLE_RSS_4Q;
+		if (hw->mac.type != e1000_i211)
+			mrqc |= E1000_MRQC_ENABLE_RSS_4Q;
 	}
 	igb_vmm_control(adapter);
 
-	/*
-	 * Generate RSS hash based on TCP port numbers and/or
-	 * IPv4/v6 src and dst addresses since UDP cannot be
-	 * hashed reliably due to IP fragmentation
-	 */
-	mrqc |= E1000_MRQC_RSS_FIELD_IPV4 |
-		E1000_MRQC_RSS_FIELD_IPV4_TCP |
-		E1000_MRQC_RSS_FIELD_IPV6 |
-		E1000_MRQC_RSS_FIELD_IPV6_TCP |
-		E1000_MRQC_RSS_FIELD_IPV6_TCP_EX;
-
-	wr32(E1000_MRQC, mrqc);
+	E1000_WRITE_REG(hw, E1000_MRQC, mrqc);
 }
 
 /**
@@ -2898,7 +3753,7 @@ void igb_setup_rctl(struct igb_adapter *adapter)
 	struct e1000_hw *hw = &adapter->hw;
 	u32 rctl;
 
-	rctl = rd32(E1000_RCTL);
+	rctl = E1000_READ_REG(hw, E1000_RCTL);
 
 	rctl &= ~(3 << E1000_RCTL_MO_SHIFT);
 	rctl &= ~(E1000_RCTL_LBM_TCVR | E1000_RCTL_LBM_MAC);
@@ -2920,7 +3775,7 @@ void igb_setup_rctl(struct igb_adapter *adapter)
 	rctl |= E1000_RCTL_LPE;
 
 	/* disable queue 0 to prevent tail write w/o re-config */
-	wr32(E1000_RXDCTL(0), 0);
+	E1000_WRITE_REG(hw, E1000_RXDCTL(0), 0);
 
 	/* Attention!!!  For SR-IOV PF driver operations you must enable
 	 * queue drop for all VF and PF queues to prevent head of line blocking
@@ -2928,10 +3783,10 @@ void igb_setup_rctl(struct igb_adapter *adapter)
 	 */
 	if (adapter->vfs_allocated_count) {
 		/* set all queue drop enable bits */
-		wr32(E1000_QDE, ALL_QUEUES);
+		E1000_WRITE_REG(hw, E1000_QDE, ALL_QUEUES);
 	}
 
-	wr32(E1000_RCTL, rctl);
+	E1000_WRITE_REG(hw, E1000_RCTL, rctl);
 }
 
 static inline int igb_set_vf_rlpml(struct igb_adapter *adapter, int size,
@@ -2944,12 +3799,22 @@ static inline int igb_set_vf_rlpml(struct igb_adapter *adapter, int size,
 	 * increase the size to support vlan tags */
 	if (vfn < adapter->vfs_allocated_count &&
 	    adapter->vf_data[vfn].vlans_enabled)
-		size += VLAN_TAG_SIZE;
+		size += VLAN_HLEN;
+
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+	if (vfn >= adapter->vfs_allocated_count) {
+		int queue = vfn - adapter->vfs_allocated_count;
+		struct igb_vmdq_adapter *vadapter;
 
-	vmolr = rd32(E1000_VMOLR(vfn));
+		vadapter = netdev_priv(adapter->vmdq_netdev[queue-1]);
+		if (vadapter->vlgrp)
+			size += VLAN_HLEN;
+	}
+#endif
+	vmolr = E1000_READ_REG(hw, E1000_VMOLR(vfn));
 	vmolr &= ~E1000_VMOLR_RLPML_MASK;
 	vmolr |= size | E1000_VMOLR_LPE;
-	wr32(E1000_VMOLR(vfn), vmolr);
+	E1000_WRITE_REG(hw, E1000_VMOLR(vfn), vmolr);
 
 	return 0;
 }
@@ -2962,22 +3827,52 @@ static inline int igb_set_vf_rlpml(struct igb_adapter *adapter, int size,
  **/
 static void igb_rlpml_set(struct igb_adapter *adapter)
 {
-	u32 max_frame_size;
+	u32 max_frame_size = adapter->max_frame_size;
 	struct e1000_hw *hw = &adapter->hw;
 	u16 pf_id = adapter->vfs_allocated_count;
 
-	max_frame_size = adapter->max_frame_size + VLAN_TAG_SIZE;
-
-	/* if vfs are enabled we set RLPML to the largest possible request
-	 * size and set the VMOLR RLPML to the size we need */
-	if (pf_id) {
-		igb_set_vf_rlpml(adapter, max_frame_size, pf_id);
+	if (adapter->vmdq_pools && hw->mac.type != e1000_82575) {
+		int i;
+		for (i = 0; i < adapter->vmdq_pools; i++)
+			igb_set_vf_rlpml(adapter, max_frame_size, pf_id + i);
+		/*
+		 * If we're in VMDQ or SR-IOV mode, then set global RLPML
+		 * to our max jumbo frame size, in case we need to enable
+		 * jumbo frames on one of the rings later.
+		 * This will not pass over-length frames into the default
+		 * queue because it's gated by the VMOLR.RLPML.
+		 */
 		max_frame_size = MAX_JUMBO_FRAME_SIZE;
 	}
+	/* Set VF RLPML for the PF device. */
+	if (adapter->vfs_allocated_count)
+		igb_set_vf_rlpml(adapter, max_frame_size, pf_id);
 
-	wr32(E1000_RLPML, max_frame_size);
+	E1000_WRITE_REG(hw, E1000_RLPML, max_frame_size);
 }
 
+static inline void igb_set_vf_vlan_strip(struct igb_adapter *adapter,
+					int vfn, bool enable)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u32 val;
+	void __iomem *reg;
+
+	if (hw->mac.type < e1000_82576)
+		return;
+
+	if (hw->mac.type == e1000_i350)
+		reg = hw->hw_addr + E1000_DVMOLR(vfn);
+	else
+		reg = hw->hw_addr + E1000_VMOLR(vfn);
+
+	val = readl(reg);
+	if (enable)
+		val |= E1000_VMOLR_STRVLAN;
+	else
+		val &= ~(E1000_VMOLR_STRVLAN);
+	writel(val, reg);
+}
 static inline void igb_set_vmolr(struct igb_adapter *adapter,
 				 int vfn, bool aupe)
 {
@@ -2991,26 +3886,23 @@ static inline void igb_set_vmolr(struct igb_adapter *adapter,
 	if (hw->mac.type < e1000_82576)
 		return;
 
-	vmolr = rd32(E1000_VMOLR(vfn));
-	vmolr |= E1000_VMOLR_STRVLAN;      /* Strip vlan tags */
+	vmolr = E1000_READ_REG(hw, E1000_VMOLR(vfn));
+
 	if (aupe)
 		vmolr |= E1000_VMOLR_AUPE;        /* Accept untagged packets */
 	else
 		vmolr &= ~(E1000_VMOLR_AUPE); /* Tagged packets ONLY */
 
 	/* clear all bits that might not be set */
-	vmolr &= ~(E1000_VMOLR_BAM | E1000_VMOLR_RSSE);
+	vmolr &= ~E1000_VMOLR_RSSE;
 
 	if (adapter->rss_queues > 1 && vfn == adapter->vfs_allocated_count)
 		vmolr |= E1000_VMOLR_RSSE; /* enable RSS */
-	/*
-	 * for VMDq only allow the VFs and pool 0 to accept broadcast and
-	 * multicast packets
-	 */
-	if (vfn <= adapter->vfs_allocated_count)
-		vmolr |= E1000_VMOLR_BAM;	   /* Accept broadcast */
 
-	wr32(E1000_VMOLR(vfn), vmolr);
+	vmolr |= E1000_VMOLR_BAM;	   /* Accept broadcast */
+	vmolr |= E1000_VMOLR_LPE;	   /* Accept long packets */
+
+	E1000_WRITE_REG(hw, E1000_VMOLR(vfn), vmolr);
 }
 
 /**
@@ -3026,62 +3918,80 @@ void igb_configure_rx_ring(struct igb_adapter *adapter,
 	struct e1000_hw *hw = &adapter->hw;
 	u64 rdba = ring->dma;
 	int reg_idx = ring->reg_idx;
-	u32 srrctl, rxdctl;
+	u32 srrctl = 0, rxdctl = 0;
+
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+	/*
+	 * RLPML prevents us from receiving a frame larger than max_frame so
+	 * it is safe to just set the rx_buffer_len to max_frame without the
+	 * risk of an skb over panic.
+	 */
+	ring->rx_buffer_len = max_t(u32, adapter->max_frame_size,
+				    MAXIMUM_ETHERNET_VLAN_SIZE);
 
+#endif
 	/* disable the queue */
-	rxdctl = rd32(E1000_RXDCTL(reg_idx));
-	wr32(E1000_RXDCTL(reg_idx),
-	                rxdctl & ~E1000_RXDCTL_QUEUE_ENABLE);
+	E1000_WRITE_REG(hw, E1000_RXDCTL(reg_idx), 0);
 
 	/* Set DMA base address registers */
-	wr32(E1000_RDBAL(reg_idx),
-	     rdba & 0x00000000ffffffffULL);
-	wr32(E1000_RDBAH(reg_idx), rdba >> 32);
-	wr32(E1000_RDLEN(reg_idx),
+	E1000_WRITE_REG(hw, E1000_RDBAL(reg_idx),
+	                rdba & 0x00000000ffffffffULL);
+	E1000_WRITE_REG(hw, E1000_RDBAH(reg_idx), rdba >> 32);
+	E1000_WRITE_REG(hw, E1000_RDLEN(reg_idx),
 	               ring->count * sizeof(union e1000_adv_rx_desc));
 
 	/* initialize head and tail */
-	ring->head = hw->hw_addr + E1000_RDH(reg_idx);
 	ring->tail = hw->hw_addr + E1000_RDT(reg_idx);
-	writel(0, ring->head);
+	E1000_WRITE_REG(hw, E1000_RDH(reg_idx), 0);
 	writel(0, ring->tail);
 
-	/* set descriptor configuration */
-	if (ring->rx_buffer_len < IGB_RXBUFFER_1024) {
-		srrctl = ALIGN(ring->rx_buffer_len, 64) <<
-		         E1000_SRRCTL_BSIZEHDRSIZE_SHIFT;
-#if (PAGE_SIZE / 2) > IGB_RXBUFFER_16384
-		srrctl |= IGB_RXBUFFER_16384 >>
-		          E1000_SRRCTL_BSIZEPKT_SHIFT;
-#else
-		srrctl |= (PAGE_SIZE / 2) >>
-		          E1000_SRRCTL_BSIZEPKT_SHIFT;
+	/* reset next-to- use/clean to place SW in sync with hardwdare */
+	ring->next_to_clean = 0;
+	ring->next_to_use = 0;
+#ifndef CONFIG_IGB_DISABLE_PACKET_SPLIT
+	ring->next_to_alloc = 0;
+
 #endif
-		srrctl |= E1000_SRRCTL_DESCTYPE_HDR_SPLIT_ALWAYS;
-	} else {
-		srrctl = ALIGN(ring->rx_buffer_len, 1024) >>
-		         E1000_SRRCTL_BSIZEPKT_SHIFT;
-		srrctl |= E1000_SRRCTL_DESCTYPE_ADV_ONEBUF;
-	}
-	if (hw->mac.type == e1000_82580)
+	/* set descriptor configuration */
+#ifndef CONFIG_IGB_DISABLE_PACKET_SPLIT
+	srrctl = IGB_RX_HDR_LEN << E1000_SRRCTL_BSIZEHDRSIZE_SHIFT;
+	srrctl |= IGB_RX_BUFSZ >> E1000_SRRCTL_BSIZEPKT_SHIFT;
+#else /* CONFIG_IGB_DISABLE_PACKET_SPLIT */
+	srrctl = ALIGN(ring->rx_buffer_len, 1024) >>
+	         E1000_SRRCTL_BSIZEPKT_SHIFT;
+#endif /* CONFIG_IGB_DISABLE_PACKET_SPLIT */
+	srrctl |= E1000_SRRCTL_DESCTYPE_ADV_ONEBUF;
+#ifdef HAVE_PTP_1588_CLOCK
+	if (hw->mac.type >= e1000_82580)
 		srrctl |= E1000_SRRCTL_TIMESTAMP;
-	/* Only set Drop Enable if we are supporting multiple queues */
-	if (adapter->vfs_allocated_count || adapter->num_rx_queues > 1)
+#endif /* HAVE_PTP_1588_CLOCK */
+	/*
+	 * We should set the drop enable bit if:
+	 *  SR-IOV is enabled
+	 *   or
+	 *  Flow Control is disabled and number of RX queues > 1
+	 *
+	 *  This allows us to avoid head of line blocking for security
+	 *  and performance reasons.
+	 */
+	if (adapter->vfs_allocated_count ||
+	    (adapter->num_rx_queues > 1 &&
+	     (hw->fc.requested_mode == e1000_fc_none ||
+	      hw->fc.requested_mode == e1000_fc_rx_pause)))
 		srrctl |= E1000_SRRCTL_DROP_EN;
 
-	wr32(E1000_SRRCTL(reg_idx), srrctl);
+	E1000_WRITE_REG(hw, E1000_SRRCTL(reg_idx), srrctl);
 
 	/* set filtering for VMDQ pools */
 	igb_set_vmolr(adapter, reg_idx & 0x7, true);
 
-	/* enable receive descriptor fetching */
-	rxdctl = rd32(E1000_RXDCTL(reg_idx));
-	rxdctl |= E1000_RXDCTL_QUEUE_ENABLE;
-	rxdctl &= 0xFFF00000;
 	rxdctl |= IGB_RX_PTHRESH;
 	rxdctl |= IGB_RX_HTHRESH << 8;
 	rxdctl |= IGB_RX_WTHRESH << 16;
-	wr32(E1000_RXDCTL(reg_idx), rxdctl);
+
+	/* enable receive descriptor fetching */
+	rxdctl |= E1000_RXDCTL_QUEUE_ENABLE;
+	E1000_WRITE_REG(hw, E1000_RXDCTL(reg_idx), rxdctl);
 }
 
 /**
@@ -3099,7 +4009,7 @@ static void igb_configure_rx(struct igb_adapter *adapter)
 
 	/* set the correct pool for the PF default MAC address in entry 0 */
 	igb_rar_set_qsel(adapter, adapter->hw.mac.addr, 0,
-	                 adapter->vfs_allocated_count);
+			 adapter->vfs_allocated_count);
 
 	/* Setup the HW Rx Head and Tail Descriptor Pointers and
 	 * the Base and Length of the Rx Descriptor Ring */
@@ -3117,8 +4027,8 @@ void igb_free_tx_resources(struct igb_ring *tx_ring)
 {
 	igb_clean_tx_ring(tx_ring);
 
-	vfree(tx_ring->buffer_info);
-	tx_ring->buffer_info = NULL;
+	vfree(tx_ring->tx_buffer_info);
+	tx_ring->tx_buffer_info = NULL;
 
 	/* if not set, then don't free */
 	if (!tx_ring->desc)
@@ -3144,30 +4054,26 @@ static void igb_free_all_tx_resources(struct igb_adapter *adapter)
 		igb_free_tx_resources(adapter->tx_ring[i]);
 }
 
-void igb_unmap_and_free_tx_resource(struct igb_ring *tx_ring,
-				    struct igb_buffer *buffer_info)
+void igb_unmap_and_free_tx_resource(struct igb_ring *ring,
+				    struct igb_tx_buffer *tx_buffer)
 {
-	if (buffer_info->dma) {
-		if (buffer_info->mapped_as_page)
-			dma_unmap_page(tx_ring->dev,
-					buffer_info->dma,
-					buffer_info->length,
-					DMA_TO_DEVICE);
-		else
-			dma_unmap_single(tx_ring->dev,
-					buffer_info->dma,
-					buffer_info->length,
-					DMA_TO_DEVICE);
-		buffer_info->dma = 0;
-	}
-	if (buffer_info->skb) {
-		dev_kfree_skb_any(buffer_info->skb);
-		buffer_info->skb = NULL;
+	if (tx_buffer->skb) {
+		dev_kfree_skb_any(tx_buffer->skb);
+		if (dma_unmap_len(tx_buffer, len))
+			dma_unmap_single(ring->dev,
+			                 dma_unmap_addr(tx_buffer, dma),
+			                 dma_unmap_len(tx_buffer, len),
+			                 DMA_TO_DEVICE);
+	} else if (dma_unmap_len(tx_buffer, len)) {
+		dma_unmap_page(ring->dev,
+		               dma_unmap_addr(tx_buffer, dma),
+		               dma_unmap_len(tx_buffer, len),
+		               DMA_TO_DEVICE);
 	}
-	buffer_info->time_stamp = 0;
-	buffer_info->length = 0;
-	buffer_info->next_to_watch = 0;
-	buffer_info->mapped_as_page = false;
+	tx_buffer->next_to_watch = NULL;
+	tx_buffer->skb = NULL;
+	dma_unmap_len_set(tx_buffer, len, 0);
+	/* buffer_info must be completely set up in the transmit path */
 }
 
 /**
@@ -3176,21 +4082,23 @@ void igb_unmap_and_free_tx_resource(struct igb_ring *tx_ring,
  **/
 static void igb_clean_tx_ring(struct igb_ring *tx_ring)
 {
-	struct igb_buffer *buffer_info;
+	struct igb_tx_buffer *buffer_info;
 	unsigned long size;
-	unsigned int i;
+	u16 i;
 
-	if (!tx_ring->buffer_info)
+	if (!tx_ring->tx_buffer_info)
 		return;
 	/* Free all the Tx ring sk_buffs */
 
 	for (i = 0; i < tx_ring->count; i++) {
-		buffer_info = &tx_ring->buffer_info[i];
+		buffer_info = &tx_ring->tx_buffer_info[i];
 		igb_unmap_and_free_tx_resource(tx_ring, buffer_info);
 	}
 
-	size = sizeof(struct igb_buffer) * tx_ring->count;
-	memset(tx_ring->buffer_info, 0, size);
+	netdev_tx_reset_queue(txring_txq(tx_ring));
+
+	size = sizeof(struct igb_tx_buffer) * tx_ring->count;
+	memset(tx_ring->tx_buffer_info, 0, size);
 
 	/* Zero out the descriptor ring */
 	memset(tx_ring->desc, 0, tx_ring->size);
@@ -3221,8 +4129,8 @@ void igb_free_rx_resources(struct igb_ring *rx_ring)
 {
 	igb_clean_rx_ring(rx_ring);
 
-	vfree(rx_ring->buffer_info);
-	rx_ring->buffer_info = NULL;
+	vfree(rx_ring->rx_buffer_info);
+	rx_ring->rx_buffer_info = NULL;
 
 	/* if not set, then don't free */
 	if (!rx_ring->desc)
@@ -3252,18 +4160,24 @@ static void igb_free_all_rx_resources(struct igb_adapter *adapter)
  * igb_clean_rx_ring - Free Rx Buffers per Queue
  * @rx_ring: ring to free buffers from
  **/
-static void igb_clean_rx_ring(struct igb_ring *rx_ring)
+void igb_clean_rx_ring(struct igb_ring *rx_ring)
 {
-	struct igb_buffer *buffer_info;
 	unsigned long size;
-	unsigned int i;
+	u16 i;
 
-	if (!rx_ring->buffer_info)
+	if (!rx_ring->rx_buffer_info)
 		return;
 
+#ifndef CONFIG_IGB_DISABLE_PACKET_SPLIT
+	if (rx_ring->skb)
+		dev_kfree_skb(rx_ring->skb);
+	rx_ring->skb = NULL;
+
+#endif
 	/* Free all the Rx ring sk_buffs */
 	for (i = 0; i < rx_ring->count; i++) {
-		buffer_info = &rx_ring->buffer_info[i];
+		struct igb_rx_buffer *buffer_info = &rx_ring->rx_buffer_info[i];
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
 		if (buffer_info->dma) {
 			dma_unmap_single(rx_ring->dev,
 			                 buffer_info->dma,
@@ -3276,26 +4190,27 @@ static void igb_clean_rx_ring(struct igb_ring *rx_ring)
 			dev_kfree_skb(buffer_info->skb);
 			buffer_info->skb = NULL;
 		}
-		if (buffer_info->page_dma) {
-			dma_unmap_page(rx_ring->dev,
-			               buffer_info->page_dma,
-				       PAGE_SIZE / 2,
-				       DMA_FROM_DEVICE);
-			buffer_info->page_dma = 0;
-		}
-		if (buffer_info->page) {
-			put_page(buffer_info->page);
-			buffer_info->page = NULL;
-			buffer_info->page_offset = 0;
-		}
+#else
+		if (!buffer_info->page)
+			continue;
+
+		dma_unmap_page(rx_ring->dev,
+			       buffer_info->dma,
+			       PAGE_SIZE,
+			       DMA_FROM_DEVICE);
+		__free_page(buffer_info->page);
+
+		buffer_info->page = NULL;
+#endif
 	}
 
-	size = sizeof(struct igb_buffer) * rx_ring->count;
-	memset(rx_ring->buffer_info, 0, size);
+	size = sizeof(struct igb_rx_buffer) * rx_ring->count;
+	memset(rx_ring->rx_buffer_info, 0, size);
 
 	/* Zero out the descriptor ring */
 	memset(rx_ring->desc, 0, rx_ring->size);
 
+	rx_ring->next_to_alloc = 0;
 	rx_ring->next_to_clean = 0;
 	rx_ring->next_to_use = 0;
 }
@@ -3333,7 +4248,7 @@ static int igb_set_mac(struct net_device *netdev, void *p)
 
 	/* set the correct pool for the new PF MAC address in entry 0 */
 	igb_rar_set_qsel(adapter, hw->mac.addr, 0,
-	                 adapter->vfs_allocated_count);
+			 adapter->vfs_allocated_count);
 
 	return 0;
 }
@@ -3347,36 +4262,157 @@ static int igb_set_mac(struct net_device *netdev, void *p)
  *                0 on no addresses written
  *                X on writing X addresses to MTA
  **/
-static int igb_write_mc_addr_list(struct net_device *netdev)
+int igb_write_mc_addr_list(struct net_device *netdev)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
+#ifdef NETDEV_HW_ADDR_T_MULTICAST
 	struct netdev_hw_addr *ha;
+#else
+	struct dev_mc_list *ha;
+#endif
 	u8  *mta_list;
-	int i;
+	int i, count;
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+	int vm;
+#endif
+	count = netdev_mc_count(netdev);
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+	for (vm = 1; vm < adapter->vmdq_pools; vm++) {
+		if (!adapter->vmdq_netdev[vm])
+			break;
+		if (!netif_running(adapter->vmdq_netdev[vm]))
+			continue;
+		count += netdev_mc_count(adapter->vmdq_netdev[vm]);
+	}
+#endif
 
-	if (netdev_mc_empty(netdev)) {
-		/* nothing to program, so clear mc list */
-		igb_update_mc_addr_list(hw, NULL, 0);
-		igb_restore_vf_multicasts(adapter);
+	if (!count) {
+		e1000_update_mc_addr_list(hw, NULL, 0);
 		return 0;
 	}
-
-	mta_list = kzalloc(netdev_mc_count(netdev) * 6, GFP_ATOMIC);
+	mta_list = kzalloc(count * 6, GFP_ATOMIC);
 	if (!mta_list)
 		return -ENOMEM;
 
 	/* The shared function expects a packed array of only addresses. */
 	i = 0;
 	netdev_for_each_mc_addr(ha, netdev)
+#ifdef NETDEV_HW_ADDR_T_MULTICAST
 		memcpy(mta_list + (i++ * ETH_ALEN), ha->addr, ETH_ALEN);
-
-	igb_update_mc_addr_list(hw, mta_list, i);
+#else
+		memcpy(mta_list + (i++ * ETH_ALEN), ha->dmi_addr, ETH_ALEN);
+#endif
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+	for (vm = 1; vm < adapter->vmdq_pools; vm++) {
+		if (!adapter->vmdq_netdev[vm])
+			break;
+		if (!netif_running(adapter->vmdq_netdev[vm]) ||
+		    !netdev_mc_count(adapter->vmdq_netdev[vm]))
+			continue;
+		netdev_for_each_mc_addr(ha, adapter->vmdq_netdev[vm])
+#ifdef NETDEV_HW_ADDR_T_MULTICAST
+			memcpy(mta_list + (i++ * ETH_ALEN),
+			       ha->addr, ETH_ALEN);
+#else
+			memcpy(mta_list + (i++ * ETH_ALEN),
+			       ha->dmi_addr, ETH_ALEN);
+#endif
+	}
+#endif
+	e1000_update_mc_addr_list(hw, mta_list, i);
 	kfree(mta_list);
 
-	return netdev_mc_count(netdev);
+	return count;
+}
+
+void igb_rar_set(struct igb_adapter *adapter, u32 index)
+{
+	u32 rar_low, rar_high;
+	struct e1000_hw *hw = &adapter->hw;
+	u8 *addr = adapter->mac_table[index].addr;
+	/* HW expects these in little endian so we reverse the byte order
+	 * from network order (big endian) to little endian
+	 */
+	rar_low = ((u32) addr[0] | ((u32) addr[1] << 8) |
+	          ((u32) addr[2] << 16) | ((u32) addr[3] << 24));
+	rar_high = ((u32) addr[4] | ((u32) addr[5] << 8));
+
+	/* Indicate to hardware the Address is Valid. */
+	if (adapter->mac_table[index].state & IGB_MAC_STATE_IN_USE)
+		rar_high |= E1000_RAH_AV;
+
+	if (hw->mac.type == e1000_82575)
+		rar_high |= E1000_RAH_POOL_1 * adapter->mac_table[index].queue;
+	else
+		rar_high |= E1000_RAH_POOL_1 << adapter->mac_table[index].queue;
+
+	E1000_WRITE_REG(hw, E1000_RAL(index), rar_low);
+	E1000_WRITE_FLUSH(hw);
+	E1000_WRITE_REG(hw, E1000_RAH(index), rar_high);
+	E1000_WRITE_FLUSH(hw);
+}
+
+void igb_full_sync_mac_table(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	int i;
+	for (i = 0; i < hw->mac.rar_entry_count; i++) {
+			igb_rar_set(adapter, i);
+	}
+}
+
+void igb_sync_mac_table(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	int i;
+	for (i = 0; i < hw->mac.rar_entry_count; i++) {
+		if (adapter->mac_table[i].state & IGB_MAC_STATE_MODIFIED)
+			igb_rar_set(adapter, i);
+		adapter->mac_table[i].state &= ~(IGB_MAC_STATE_MODIFIED);
+	}
+}
+
+int igb_available_rars(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	int i, count = 0;
+
+	for (i = 0; i < hw->mac.rar_entry_count; i++) {
+		if (adapter->mac_table[i].state == 0)
+			count++;
+	}
+	return count;
 }
 
+static void igb_rar_set_qsel(struct igb_adapter *adapter, u8 *addr, u32 index,
+			     u8 qsel)
+{
+	u32 rar_low, rar_high;
+	struct e1000_hw *hw = &adapter->hw;
+
+	/* HW expects these in little endian so we reverse the byte order
+	 * from network order (big endian) to little endian
+	 */
+	rar_low = ((u32) addr[0] | ((u32) addr[1] << 8) |
+		   ((u32) addr[2] << 16) | ((u32) addr[3] << 24));
+	rar_high = ((u32) addr[4] | ((u32) addr[5] << 8));
+
+	/* Indicate to hardware the Address is Valid. */
+	rar_high |= E1000_RAH_AV;
+
+	if (hw->mac.type == e1000_82575)
+		rar_high |= E1000_RAH_POOL_1 * qsel;
+	else
+		rar_high |= E1000_RAH_POOL_1 << qsel;
+
+	E1000_WRITE_REG(hw, E1000_RAL(index), rar_low);
+	E1000_WRITE_FLUSH(hw);
+	E1000_WRITE_REG(hw, E1000_RAH(index), rar_high);
+	E1000_WRITE_FLUSH(hw);
+}
+
+#ifdef HAVE_SET_RX_MODE
 /**
  * igb_write_uc_addr_list - write unicast addresses to RAR table
  * @netdev: network interface device structure
@@ -3395,31 +4431,40 @@ static int igb_write_uc_addr_list(struct net_device *netdev)
 	int count = 0;
 
 	/* return ENOMEM indicating insufficient memory for addresses */
-	if (netdev_uc_count(netdev) > rar_entries)
+	if (netdev_uc_count(netdev) > igb_available_rars(adapter))
 		return -ENOMEM;
-
 	if (!netdev_uc_empty(netdev) && rar_entries) {
+#ifdef NETDEV_HW_ADDR_T_UNICAST
 		struct netdev_hw_addr *ha;
-
+#else
+		struct dev_mc_list *ha;
+#endif
 		netdev_for_each_uc_addr(ha, netdev) {
+#ifdef NETDEV_HW_ADDR_T_UNICAST
 			if (!rar_entries)
 				break;
 			igb_rar_set_qsel(adapter, ha->addr,
-			                 rar_entries--,
-			                 vfn);
+					 rar_entries--,
+					 vfn);
+#else
+			igb_rar_set_qsel(adapter, ha->da_addr,
+					 rar_entries--,
+					 vfn);
+#endif
 			count++;
 		}
 	}
+
 	/* write the addresses in reverse order to avoid write combining */
 	for (; rar_entries > 0 ; rar_entries--) {
-		wr32(E1000_RAH(rar_entries), 0);
-		wr32(E1000_RAL(rar_entries), 0);
+		E1000_WRITE_REG(hw, E1000_RAH(rar_entries), 0);
+		E1000_WRITE_REG(hw, E1000_RAL(rar_entries), 0);
 	}
-	wrfl();
-
+	E1000_WRITE_FLUSH(hw);
 	return count;
 }
 
+#endif /* HAVE_SET_RX_MODE */
 /**
  * igb_set_rx_mode - Secondary Unicast, Multicast and Promiscuous mode set
  * @netdev: network interface device structure
@@ -3438,7 +4483,7 @@ static void igb_set_rx_mode(struct net_device *netdev)
 	int count;
 
 	/* Check for Promiscuous and All Multicast modes */
-	rctl = rd32(E1000_RCTL);
+	rctl = E1000_READ_REG(hw, E1000_RCTL);
 
 	/* clear the effected bits */
 	rctl &= ~(E1000_RCTL_UPE | E1000_RCTL_MPE | E1000_RCTL_VFE);
@@ -3446,6 +4491,9 @@ static void igb_set_rx_mode(struct net_device *netdev)
 	if (netdev->flags & IFF_PROMISC) {
 		rctl |= (E1000_RCTL_UPE | E1000_RCTL_MPE);
 		vmolr |= (E1000_VMOLR_ROPE | E1000_VMOLR_MPME);
+		/* retain VLAN HW filtering if in VT mode */
+		if (adapter->vfs_allocated_count || adapter->vmdq_pools)
+			rctl |= E1000_RCTL_VFE;
 	} else {
 		if (netdev->flags & IFF_ALLMULTI) {
 			rctl |= E1000_RCTL_MPE;
@@ -3464,6 +4512,7 @@ static void igb_set_rx_mode(struct net_device *netdev)
 				vmolr |= E1000_VMOLR_ROMPE;
 			}
 		}
+#ifdef HAVE_SET_RX_MODE
 		/*
 		 * Write addresses to available RAR registers, if there is not
 		 * sufficient space to store all the addresses then enable
@@ -3474,9 +4523,10 @@ static void igb_set_rx_mode(struct net_device *netdev)
 			rctl |= E1000_RCTL_UPE;
 			vmolr |= E1000_VMOLR_ROPE;
 		}
+#endif /* HAVE_SET_RX_MODE */
 		rctl |= E1000_RCTL_VFE;
 	}
-	wr32(E1000_RCTL, rctl);
+	E1000_WRITE_REG(hw, E1000_RCTL, rctl);
 
 	/*
 	 * In order to support SR-IOV and eventually VMDq it is necessary to set
@@ -3487,9 +4537,9 @@ static void igb_set_rx_mode(struct net_device *netdev)
 	if (hw->mac.type < e1000_82576)
 		return;
 
-	vmolr |= rd32(E1000_VMOLR(vfn)) &
+	vmolr |= E1000_READ_REG(hw, E1000_VMOLR(vfn)) &
 	         ~(E1000_VMOLR_ROPE | E1000_VMOLR_MPME | E1000_VMOLR_ROMPE);
-	wr32(E1000_VMOLR(vfn), vmolr);
+	E1000_WRITE_REG(hw, E1000_VMOLR(vfn), vmolr);
 	igb_restore_vf_multicasts(adapter);
 }
 
@@ -3501,7 +4551,7 @@ static void igb_check_wvbr(struct igb_adapter *adapter)
 	switch (hw->mac.type) {
 	case e1000_82576:
 	case e1000_i350:
-		if (!(wvbr = rd32(E1000_WVBR)))
+		if (!(wvbr = E1000_READ_REG(hw, E1000_WVBR)))
 			return;
 		break;
 	default:
@@ -3520,15 +4570,30 @@ static void igb_spoof_check(struct igb_adapter *adapter)
 	if (!adapter->wvbr)
 		return;
 
-	for(j = 0; j < adapter->vfs_allocated_count; j++) {
-		if (adapter->wvbr & (1 << j) ||
-		    adapter->wvbr & (1 << (j + IGB_STAGGERED_QUEUE_OFFSET))) {
-			dev_warn(&adapter->pdev->dev,
-				"Spoof event(s) detected on VF %d\n", j);
-			adapter->wvbr &=
-				~((1 << j) |
-				  (1 << (j + IGB_STAGGERED_QUEUE_OFFSET)));
+	switch (adapter->hw.mac.type) {
+	case e1000_82576:
+		for (j = 0; j < adapter->vfs_allocated_count; j++) {
+			if (adapter->wvbr & (1 << j) ||
+			    adapter->wvbr & (1 << (j + IGB_STAGGERED_QUEUE_OFFSET))) {
+				DPRINTK(DRV, WARNING,
+					"Spoof event(s) detected on VF %d\n", j);
+				adapter->wvbr &=
+					~((1 << j) |
+					  (1 << (j + IGB_STAGGERED_QUEUE_OFFSET)));
+			}
+		}
+		break;
+	case e1000_i350:
+		for (j = 0; j < adapter->vfs_allocated_count; j++) {
+			if (adapter->wvbr & (1 << j)) {
+				DPRINTK(DRV, WARNING,
+					"Spoof event(s) detected on VF %d\n", j);
+				adapter->wvbr &= ~(1 << j);
+			}
 		}
+		break;
+	default:
+		break;
 	}
 }
 
@@ -3537,7 +4602,7 @@ static void igb_spoof_check(struct igb_adapter *adapter)
 static void igb_update_phy_info(unsigned long data)
 {
 	struct igb_adapter *adapter = (struct igb_adapter *) data;
-	igb_get_phy_info(&adapter->hw);
+	e1000_get_phy_info(&adapter->hw);
 }
 
 /**
@@ -3547,8 +4612,7 @@ static void igb_update_phy_info(unsigned long data)
 bool igb_has_link(struct igb_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
-	bool link_active = false;
-	s32 ret_val = 0;
+	bool link_active = FALSE;
 
 	/* get_link_status is set on LSC (link status) interrupt or
 	 * rx sequence error interrupt.  get_link_status will stay
@@ -3557,42 +4621,29 @@ bool igb_has_link(struct igb_adapter *adapter)
 	 */
 	switch (hw->phy.media_type) {
 	case e1000_media_type_copper:
-		if (hw->mac.get_link_status) {
-			ret_val = hw->mac.ops.check_for_link(hw);
-			link_active = !hw->mac.get_link_status;
-		} else {
-			link_active = true;
-		}
-		break;
+		if (!hw->mac.get_link_status)
+			return true;
 	case e1000_media_type_internal_serdes:
-		ret_val = hw->mac.ops.check_for_link(hw);
-		link_active = hw->mac.serdes_has_link;
+		e1000_check_for_link(hw);
+		link_active = !hw->mac.get_link_status;
 		break;
-	default:
 	case e1000_media_type_unknown:
+	default:
 		break;
 	}
 
-	return link_active;
-}
-
-static bool igb_thermal_sensor_event(struct e1000_hw *hw, u32 event)
-{
-	bool ret = false;
-	u32 ctrl_ext, thstat;
-
-	/* check for thermal sensor event on i350, copper only */
-	if (hw->mac.type == e1000_i350) {
-		thstat = rd32(E1000_THSTAT);
-		ctrl_ext = rd32(E1000_CTRL_EXT);
-
-		if ((hw->phy.media_type == e1000_media_type_copper) &&
-		    !(ctrl_ext & E1000_CTRL_EXT_LINK_MODE_SGMII)) {
-			ret = !!(thstat & event);
+	if (((hw->mac.type == e1000_i210) ||
+	     (hw->mac.type == e1000_i211)) &&
+	     (hw->phy.id == I210_I_PHY_ID)) {
+		if (!netif_carrier_ok(adapter->netdev)) {
+			adapter->flags &= ~IGB_FLAG_NEED_LINK_UPDATE;
+		} else if (!(adapter->flags & IGB_FLAG_NEED_LINK_UPDATE)) {
+			adapter->flags |= IGB_FLAG_NEED_LINK_UPDATE;
+			adapter->link_check_timeout = jiffies;
 		}
 	}
 
-	return ret;
+	return link_active;
 }
 
 /**
@@ -3613,18 +4664,45 @@ static void igb_watchdog_task(struct work_struct *work)
                                                    watchdog_task);
 	struct e1000_hw *hw = &adapter->hw;
 	struct net_device *netdev = adapter->netdev;
-	u32 link;
+	u32 thstat, ctrl_ext, link;
 	int i;
+	u32 connsw;
 
 	link = igb_has_link(adapter);
+
+	/* Force link down if we have fiber to swap to */
+	if (adapter->flags & IGB_FLAG_MAS_ENABLE) {
+		if (hw->phy.media_type == e1000_media_type_copper) {
+			connsw = E1000_READ_REG(hw, E1000_CONNSW);
+			if (!(connsw & E1000_CONNSW_AUTOSENSE_EN))
+				link = 0;
+		}
+	}
+	if (adapter->flags & IGB_FLAG_NEED_LINK_UPDATE) {
+		if (time_after(jiffies, (adapter->link_check_timeout + HZ)))
+			adapter->flags &= ~IGB_FLAG_NEED_LINK_UPDATE;
+		else
+			link = FALSE;
+	}
+
 	if (link) {
+		/* Perform a reset if the media type changed. */
+		if (hw->dev_spec._82575.media_changed) {
+			hw->dev_spec._82575.media_changed = false;
+			adapter->flags |= IGB_FLAG_MEDIA_RESET;
+			igb_reset(adapter);
+		}
+
+		/* Cancel scheduled suspend requests. */
+		pm_runtime_resume(netdev->dev.parent);
+
 		if (!netif_carrier_ok(netdev)) {
 			u32 ctrl;
-			hw->mac.ops.get_speed_and_duplex(hw,
-			                                 &adapter->link_speed,
-			                                 &adapter->link_duplex);
+			e1000_get_speed_and_duplex(hw,
+			                           &adapter->link_speed,
+			                           &adapter->link_duplex);
 
-			ctrl = rd32(E1000_CTRL);
+			ctrl = E1000_READ_REG(hw, E1000_CTRL);
 			/* Links status message must follow this format */
 			printk(KERN_INFO "igb: %s NIC Link is Up %d Mbps %s, "
 				 "Flow Control: %s\n",
@@ -3633,18 +4711,9 @@ static void igb_watchdog_task(struct work_struct *work)
 			       adapter->link_duplex == FULL_DUPLEX ?
 				 "Full Duplex" : "Half Duplex",
 			       ((ctrl & E1000_CTRL_TFCE) &&
-			        (ctrl & E1000_CTRL_RFCE)) ? "RX/TX" :
+			        (ctrl & E1000_CTRL_RFCE)) ? "RX/TX":
 			       ((ctrl & E1000_CTRL_RFCE) ?  "RX" :
 			       ((ctrl & E1000_CTRL_TFCE) ?  "TX" : "None")));
-
-			/* check for thermal sensor event */
-			if (igb_thermal_sensor_event(hw, E1000_THSTAT_LINK_THROTTLE)) {
-				printk(KERN_INFO "igb: %s The network adapter "
-						 "link speed was downshifted "
-						 "because it overheated.\n",
-						 netdev->name);
-			}
-
 			/* adjust timeout factor according to speed/duplex */
 			adapter->tx_timeout_factor = 1;
 			switch (adapter->link_speed) {
@@ -3654,12 +4723,17 @@ static void igb_watchdog_task(struct work_struct *work)
 			case SPEED_100:
 				/* maybe add some timeout factor ? */
 				break;
+			default:
+				break;
 			}
 
 			netif_carrier_on(netdev);
+			netif_tx_wake_all_queues(netdev);
 
 			igb_ping_all_vfs(adapter);
+#ifdef IFLA_VF_MAX
 			igb_check_vf_rate_limit(adapter);
+#endif /* IFLA_VF_MAX */
 
 			/* link state has changed, schedule phy info update */
 			if (!test_bit(__IGB_DOWN, &adapter->state))
@@ -3670,19 +4744,38 @@ static void igb_watchdog_task(struct work_struct *work)
 		if (netif_carrier_ok(netdev)) {
 			adapter->link_speed = 0;
 			adapter->link_duplex = 0;
-
-			/* check for thermal sensor event */
-			if (igb_thermal_sensor_event(hw, E1000_THSTAT_PWR_DOWN)) {
-				printk(KERN_ERR "igb: %s The network adapter "
-						"was stopped because it "
-						"overheated.\n",
+			/* check for thermal sensor event on i350 */
+			if (hw->mac.type == e1000_i350) {
+				thstat = E1000_READ_REG(hw, E1000_THSTAT);
+				ctrl_ext = E1000_READ_REG(hw, E1000_CTRL_EXT);
+				if ((hw->phy.media_type ==
+					e1000_media_type_copper) &&
+					!(ctrl_ext &
+					E1000_CTRL_EXT_LINK_MODE_SGMII)) {
+					if (thstat & E1000_THSTAT_PWR_DOWN) {
+						printk(KERN_ERR "igb: %s The "
+						"network adapter was stopped "
+						"because it overheated.\n",
 						netdev->name);
+					}
+					if (thstat & E1000_THSTAT_LINK_THROTTLE) {
+						printk(KERN_INFO
+							"igb: %s The network "
+							"adapter supported "
+							"link speed "
+							"was downshifted "
+							"because it "
+							"overheated.\n",
+							netdev->name);
+					}
+				}
 			}
 
 			/* Links status message must follow this format */
 			printk(KERN_INFO "igb: %s NIC Link is Down\n",
 			       netdev->name);
 			netif_carrier_off(netdev);
+			netif_tx_stop_all_queues(netdev);
 
 			igb_ping_all_vfs(adapter);
 
@@ -3690,12 +4783,32 @@ static void igb_watchdog_task(struct work_struct *work)
 			if (!test_bit(__IGB_DOWN, &adapter->state))
 				mod_timer(&adapter->phy_info_timer,
 					  round_jiffies(jiffies + 2 * HZ));
+			/* link is down, time to check for alternate media */
+			if (adapter->flags & IGB_FLAG_MAS_ENABLE) {
+				igb_check_swap_media(adapter);
+				if (adapter->flags & IGB_FLAG_MEDIA_RESET) {
+					schedule_work(&adapter->reset_task);
+					/* return immediately */
+					return;
+				}
+			}
+			pm_schedule_suspend(netdev->dev.parent,
+					    MSEC_PER_SEC * 5);
+
+		/* also check for alternate media here */
+		} else if (!netif_carrier_ok(netdev) &&
+			   (adapter->flags & IGB_FLAG_MAS_ENABLE)) {
+			hw->mac.ops.power_up_serdes(hw);
+			igb_check_swap_media(adapter);
+			if (adapter->flags & IGB_FLAG_MEDIA_RESET) {
+				schedule_work(&adapter->reset_task);
+				/* return immediately */
+				return;
+			}
 		}
 	}
 
-	spin_lock(&adapter->stats64_lock);
-	igb_update_stats(adapter, &adapter->stats64);
-	spin_unlock(&adapter->stats64_lock);
+	igb_update_stats(adapter);
 
 	for (i = 0; i < adapter->num_tx_queues; i++) {
 		struct igb_ring *tx_ring = adapter->tx_ring[i];
@@ -3713,27 +4826,94 @@ static void igb_watchdog_task(struct work_struct *work)
 		}
 
 		/* Force detection of hung controller every watchdog period */
-		tx_ring->detect_tx_hung = true;
+		set_bit(IGB_RING_FLAG_TX_DETECT_HANG, &tx_ring->flags);
 	}
 
 	/* Cause software interrupt to ensure rx ring is cleaned */
 	if (adapter->msix_entries) {
 		u32 eics = 0;
-		for (i = 0; i < adapter->num_q_vectors; i++) {
-			struct igb_q_vector *q_vector = adapter->q_vector[i];
-			eics |= q_vector->eims_value;
-		}
-		wr32(E1000_EICS, eics);
+		for (i = 0; i < adapter->num_q_vectors; i++)
+			eics |= adapter->q_vector[i]->eims_value;
+		E1000_WRITE_REG(hw, E1000_EICS, eics);
 	} else {
-		wr32(E1000_ICS, E1000_ICS_RXDMT0);
+		E1000_WRITE_REG(hw, E1000_ICS, E1000_ICS_RXDMT0);
 	}
 
 	igb_spoof_check(adapter);
 
 	/* Reset the timer */
+	if (!test_bit(__IGB_DOWN, &adapter->state)) {
+		if (adapter->flags & IGB_FLAG_NEED_LINK_UPDATE)
+			mod_timer(&adapter->watchdog_timer,
+				  round_jiffies(jiffies +  HZ));
+		else
+			mod_timer(&adapter->watchdog_timer,
+				  round_jiffies(jiffies + 2 * HZ));
+	}
+}
+
+static void igb_dma_err_task(struct work_struct *work)
+{
+	struct igb_adapter *adapter = container_of(work,
+	                                           struct igb_adapter,
+                                                   dma_err_task);
+	int vf;
+	struct e1000_hw *hw = &adapter->hw;
+	struct net_device *netdev = adapter->netdev;
+	u32 hgptc;
+	u32 ciaa, ciad;
+
+	hgptc = E1000_READ_REG(hw, E1000_HGPTC);
+	if (hgptc) /* If incrementing then no need for the check below */
+		goto dma_timer_reset;
+	/*
+	 * Check to see if a bad DMA write target from an errant or
+	 * malicious VF has caused a PCIe error.  If so then we can
+	 * issue a VFLR to the offending VF(s) and then resume without
+	 * requesting a full slot reset.
+	 */
+
+	for (vf = 0; vf < adapter->vfs_allocated_count; vf++) {
+		ciaa = (vf << 16) | 0x80000000;
+		/* 32 bit read so align, we really want status at offset 6 */
+		ciaa |= PCI_COMMAND;
+		E1000_WRITE_REG(hw, E1000_CIAA, ciaa);
+		ciad = E1000_READ_REG(hw, E1000_CIAD);
+		ciaa &= 0x7FFFFFFF;
+		/* disable debug mode asap after reading data */
+		E1000_WRITE_REG(hw, E1000_CIAA, ciaa);
+		/* Get the upper 16 bits which will be the PCI status reg */
+		ciad >>= 16;
+		if (ciad & (PCI_STATUS_REC_MASTER_ABORT |
+			    PCI_STATUS_REC_TARGET_ABORT |
+			    PCI_STATUS_SIG_SYSTEM_ERROR)) {
+			netdev_err(netdev, "VF %d suffered error\n", vf);
+			/* Issue VFLR */
+			ciaa = (vf << 16) | 0x80000000;
+			ciaa |= 0xA8;
+			E1000_WRITE_REG(hw, E1000_CIAA, ciaa);
+			ciad = 0x00008000;  /* VFLR */
+			E1000_WRITE_REG(hw, E1000_CIAD, ciad);
+			ciaa &= 0x7FFFFFFF;
+			E1000_WRITE_REG(hw, E1000_CIAA, ciaa);
+		}
+	}
+dma_timer_reset:
+	/* Reset the timer */
 	if (!test_bit(__IGB_DOWN, &adapter->state))
-		mod_timer(&adapter->watchdog_timer,
-			  round_jiffies(jiffies + 2 * HZ));
+		mod_timer(&adapter->dma_err_timer,
+			  round_jiffies(jiffies + HZ / 10));
+}
+
+/**
+ * igb_dma_err_timer - Timer Call-back
+ * @data: pointer to adapter cast into an unsigned long
+ **/
+static void igb_dma_err_timer(unsigned long data)
+{
+	struct igb_adapter *adapter = (struct igb_adapter *)data;
+	/* Do the rest outside of interrupt context */
+	schedule_work(&adapter->dma_err_task);
 }
 
 enum latency_range {
@@ -3764,33 +4944,28 @@ static void igb_update_ring_itr(struct igb_q_vector *q_vector)
 	int new_val = q_vector->itr_val;
 	int avg_wire_size = 0;
 	struct igb_adapter *adapter = q_vector->adapter;
-	struct igb_ring *ring;
 	unsigned int packets;
 
 	/* For non-gigabit speeds, just fix the interrupt rate at 4000
 	 * ints/sec - ITR timer value of 120 ticks.
 	 */
-	if (adapter->link_speed != SPEED_1000) {
-		new_val = 976;
+	switch (adapter->link_speed) {
+	case SPEED_10:
+	case SPEED_100:
+		new_val = IGB_4K_ITR;
 		goto set_itr_val;
+	default:
+		break;
 	}
 
-	ring = q_vector->rx_ring;
-	if (ring) {
-		packets = ACCESS_ONCE(ring->total_packets);
-
-		if (packets)
-			avg_wire_size = ring->total_bytes / packets;
-	}
-
-	ring = q_vector->tx_ring;
-	if (ring) {
-		packets = ACCESS_ONCE(ring->total_packets);
+	packets = q_vector->rx.total_packets;
+	if (packets)
+		avg_wire_size = q_vector->rx.total_bytes / packets;
 
-		if (packets)
-			avg_wire_size = max_t(u32, avg_wire_size,
-			                      ring->total_bytes / packets);
-	}
+	packets = q_vector->tx.total_packets;
+	if (packets)
+		avg_wire_size = max_t(u32, avg_wire_size,
+		                      q_vector->tx.total_bytes / packets);
 
 	/* if avg_wire_size isn't set no work was done */
 	if (!avg_wire_size)
@@ -3808,9 +4983,11 @@ static void igb_update_ring_itr(struct igb_q_vector *q_vector)
 	else
 		new_val = avg_wire_size / 2;
 
-	/* when in itr mode 3 do not exceed 20K ints/sec */
-	if (adapter->rx_itr_setting == 3 && new_val < 196)
-		new_val = 196;
+	/* conservative mode (itr 3) eliminates the lowest_latency setting */
+	if (new_val < IGB_20K_ITR &&
+	    ((q_vector->rx.ring && adapter->rx_itr_setting == 3) ||
+	     (!q_vector->rx.ring && adapter->tx_itr_setting == 3)))
+		new_val = IGB_20K_ITR;
 
 set_itr_val:
 	if (new_val != q_vector->itr_val) {
@@ -3818,14 +4995,10 @@ set_itr_val:
 		q_vector->set_itr = 1;
 	}
 clear_counts:
-	if (q_vector->rx_ring) {
-		q_vector->rx_ring->total_bytes = 0;
-		q_vector->rx_ring->total_packets = 0;
-	}
-	if (q_vector->tx_ring) {
-		q_vector->tx_ring->total_bytes = 0;
-		q_vector->tx_ring->total_packets = 0;
-	}
+	q_vector->rx.total_bytes = 0;
+	q_vector->rx.total_packets = 0;
+	q_vector->tx.total_bytes = 0;
+	q_vector->tx.total_packets = 0;
 }
 
 /**
@@ -3841,106 +5014,106 @@ clear_counts:
  *      parameter (see igb_param.c)
  *      NOTE:  These calculations are only valid when operating in a single-
  *             queue environment.
- * @adapter: pointer to adapter
- * @itr_setting: current q_vector->itr_val
- * @packets: the number of packets during this measurement interval
- * @bytes: the number of bytes during this measurement interval
+ * @q_vector: pointer to q_vector
+ * @ring_container: ring info to update the itr for
  **/
-static unsigned int igb_update_itr(struct igb_adapter *adapter, u16 itr_setting,
-				   int packets, int bytes)
+static void igb_update_itr(struct igb_q_vector *q_vector,
+			   struct igb_ring_container *ring_container)
 {
-	unsigned int retval = itr_setting;
+	unsigned int packets = ring_container->total_packets;
+	unsigned int bytes = ring_container->total_bytes;
+	u8 itrval = ring_container->itr;
 
+	/* no packets, exit with status unchanged */
 	if (packets == 0)
-		goto update_itr_done;
+		return;
 
-	switch (itr_setting) {
+	switch (itrval) {
 	case lowest_latency:
 		/* handle TSO and jumbo frames */
 		if (bytes/packets > 8000)
-			retval = bulk_latency;
+			itrval = bulk_latency;
 		else if ((packets < 5) && (bytes > 512))
-			retval = low_latency;
+			itrval = low_latency;
 		break;
 	case low_latency:  /* 50 usec aka 20000 ints/s */
 		if (bytes > 10000) {
 			/* this if handles the TSO accounting */
 			if (bytes/packets > 8000) {
-				retval = bulk_latency;
+				itrval = bulk_latency;
 			} else if ((packets < 10) || ((bytes/packets) > 1200)) {
-				retval = bulk_latency;
+				itrval = bulk_latency;
 			} else if ((packets > 35)) {
-				retval = lowest_latency;
+				itrval = lowest_latency;
 			}
 		} else if (bytes/packets > 2000) {
-			retval = bulk_latency;
+			itrval = bulk_latency;
 		} else if (packets <= 2 && bytes < 512) {
-			retval = lowest_latency;
+			itrval = lowest_latency;
 		}
 		break;
 	case bulk_latency: /* 250 usec aka 4000 ints/s */
 		if (bytes > 25000) {
 			if (packets > 35)
-				retval = low_latency;
+				itrval = low_latency;
 		} else if (bytes < 1500) {
-			retval = low_latency;
+			itrval = low_latency;
 		}
 		break;
 	}
 
-update_itr_done:
-	return retval;
+	/* clear work counters since we have the values we need */
+	ring_container->total_bytes = 0;
+	ring_container->total_packets = 0;
+
+	/* write updated itr to ring container */
+	ring_container->itr = itrval;
 }
 
-static void igb_set_itr(struct igb_adapter *adapter)
+static void igb_set_itr(struct igb_q_vector *q_vector)
 {
-	struct igb_q_vector *q_vector = adapter->q_vector[0];
-	u16 current_itr;
+	struct igb_adapter *adapter = q_vector->adapter;
 	u32 new_itr = q_vector->itr_val;
+	u8 current_itr = 0;
 
 	/* for non-gigabit speeds, just fix the interrupt rate at 4000 */
-	if (adapter->link_speed != SPEED_1000) {
+	switch (adapter->link_speed) {
+	case SPEED_10:
+	case SPEED_100:
 		current_itr = 0;
-		new_itr = 4000;
+		new_itr = IGB_4K_ITR;
 		goto set_itr_now;
+	default:
+		break;
 	}
 
-	adapter->rx_itr = igb_update_itr(adapter,
-				    adapter->rx_itr,
-				    q_vector->rx_ring->total_packets,
-				    q_vector->rx_ring->total_bytes);
+	igb_update_itr(q_vector, &q_vector->tx);
+	igb_update_itr(q_vector, &q_vector->rx);
 
-	adapter->tx_itr = igb_update_itr(adapter,
-				    adapter->tx_itr,
-				    q_vector->tx_ring->total_packets,
-				    q_vector->tx_ring->total_bytes);
-	current_itr = max(adapter->rx_itr, adapter->tx_itr);
+	current_itr = max(q_vector->rx.itr, q_vector->tx.itr);
 
 	/* conservative mode (itr 3) eliminates the lowest_latency setting */
-	if (adapter->rx_itr_setting == 3 && current_itr == lowest_latency)
+	if (current_itr == lowest_latency &&
+	    ((q_vector->rx.ring && adapter->rx_itr_setting == 3) ||
+	     (!q_vector->rx.ring && adapter->tx_itr_setting == 3)))
 		current_itr = low_latency;
 
 	switch (current_itr) {
 	/* counts and packets in update_itr are dependent on these numbers */
 	case lowest_latency:
-		new_itr = 56;  /* aka 70,000 ints/sec */
+		new_itr = IGB_70K_ITR; /* 70,000 ints/sec */
 		break;
 	case low_latency:
-		new_itr = 196; /* aka 20,000 ints/sec */
+		new_itr = IGB_20K_ITR; /* 20,000 ints/sec */
 		break;
 	case bulk_latency:
-		new_itr = 980; /* aka 4,000 ints/sec */
+		new_itr = IGB_4K_ITR;  /* 4,000 ints/sec */
 		break;
 	default:
 		break;
 	}
 
 set_itr_now:
-	q_vector->rx_ring->total_bytes = 0;
-	q_vector->rx_ring->total_packets = 0;
-	q_vector->tx_ring->total_bytes = 0;
-	q_vector->tx_ring->total_packets = 0;
-
 	if (new_itr != q_vector->itr_val) {
 		/* this attempts to bias the interrupt rate towards Bulk
 		 * by adding intermediate steps when interrupt rate is
@@ -3948,7 +5121,7 @@ set_itr_now:
 		new_itr = new_itr > q_vector->itr_val ?
 		             max((new_itr * q_vector->itr_val) /
 		                 (new_itr + (q_vector->itr_val >> 2)),
-		                 new_itr) :
+				 new_itr) :
 			     new_itr;
 		/* Don't write the value here; it resets the adapter's
 		 * internal timer, and causes us to delay far longer than
@@ -3961,35 +5134,57 @@ set_itr_now:
 	}
 }
 
-#define IGB_TX_FLAGS_CSUM		0x00000001
-#define IGB_TX_FLAGS_VLAN		0x00000002
-#define IGB_TX_FLAGS_TSO		0x00000004
-#define IGB_TX_FLAGS_IPV4		0x00000008
-#define IGB_TX_FLAGS_TSTAMP		0x00000010
-#define IGB_TX_FLAGS_VLAN_MASK		0xffff0000
-#define IGB_TX_FLAGS_VLAN_SHIFT		        16
-
-static inline int igb_tso_adv(struct igb_ring *tx_ring,
-			      struct sk_buff *skb, u32 tx_flags, u8 *hdr_len)
+void igb_tx_ctxtdesc(struct igb_ring *tx_ring, u32 vlan_macip_lens,
+		     u32 type_tucmd, u32 mss_l4len_idx)
 {
 	struct e1000_adv_tx_context_desc *context_desc;
-	unsigned int i;
-	int err;
-	struct igb_buffer *buffer_info;
-	u32 info = 0, tu_cmd = 0;
-	u32 mss_l4len_idx;
-	u8 l4len;
+	u16 i = tx_ring->next_to_use;
+
+	context_desc = IGB_TX_CTXTDESC(tx_ring, i);
+
+	i++;
+	tx_ring->next_to_use = (i < tx_ring->count) ? i : 0;
+
+	/* set bits to identify this as an advanced context descriptor */
+	type_tucmd |= E1000_TXD_CMD_DEXT | E1000_ADVTXD_DTYP_CTXT;
+
+	/* For 82575, context index must be unique per ring. */
+	if (test_bit(IGB_RING_FLAG_TX_CTX_IDX, &tx_ring->flags))
+		mss_l4len_idx |= tx_ring->reg_idx << 4;
+
+	context_desc->vlan_macip_lens	= cpu_to_le32(vlan_macip_lens);
+	context_desc->seqnum_seed	= 0;
+	context_desc->type_tucmd_mlhl	= cpu_to_le32(type_tucmd);
+	context_desc->mss_l4len_idx	= cpu_to_le32(mss_l4len_idx);
+}
+
+static int igb_tso(struct igb_ring *tx_ring,
+		   struct igb_tx_buffer *first,
+		   u8 *hdr_len)
+{
+#ifdef NETIF_F_TSO
+	struct sk_buff *skb = first->skb;
+	u32 vlan_macip_lens, type_tucmd;
+	u32 mss_l4len_idx, l4len;
+
+	if (skb->ip_summed != CHECKSUM_PARTIAL)
+		return 0;
+
+	if (!skb_is_gso(skb))
+#endif /* NETIF_F_TSO */
+		return 0;
+#ifdef NETIF_F_TSO
 
 	if (skb_header_cloned(skb)) {
-		err = pskb_expand_head(skb, 0, 0, GFP_ATOMIC);
+		int err = pskb_expand_head(skb, 0, 0, GFP_ATOMIC);
 		if (err)
 			return err;
 	}
 
-	l4len = tcp_hdrlen(skb);
-	*hdr_len += l4len;
+	/* ADV DTYP TUCMD MKRLOC/ISCSIHEDLEN */
+	type_tucmd = E1000_ADVTXD_TUCMD_L4T_TCP;
 
-	if (skb->protocol == htons(ETH_P_IP)) {
+	if (first->protocol == __constant_htons(ETH_P_IP)) {
 		struct iphdr *iph = ip_hdr(skb);
 		iph->tot_len = 0;
 		iph->check = 0;
@@ -3997,294 +5192,298 @@ static inline int igb_tso_adv(struct igb_ring *tx_ring,
 							 iph->daddr, 0,
 							 IPPROTO_TCP,
 							 0);
+		type_tucmd |= E1000_ADVTXD_TUCMD_IPV4;
+		first->tx_flags |= IGB_TX_FLAGS_TSO |
+				   IGB_TX_FLAGS_CSUM |
+				   IGB_TX_FLAGS_IPV4;
+#ifdef NETIF_F_TSO6
 	} else if (skb_is_gso_v6(skb)) {
 		ipv6_hdr(skb)->payload_len = 0;
 		tcp_hdr(skb)->check = ~csum_ipv6_magic(&ipv6_hdr(skb)->saddr,
 						       &ipv6_hdr(skb)->daddr,
 						       0, IPPROTO_TCP, 0);
+		first->tx_flags |= IGB_TX_FLAGS_TSO |
+				   IGB_TX_FLAGS_CSUM;
+#endif
 	}
 
-	i = tx_ring->next_to_use;
-
-	buffer_info = &tx_ring->buffer_info[i];
-	context_desc = E1000_TX_CTXTDESC_ADV(*tx_ring, i);
-	/* VLAN MACLEN IPLEN */
-	if (tx_flags & IGB_TX_FLAGS_VLAN)
-		info |= (tx_flags & IGB_TX_FLAGS_VLAN_MASK);
-	info |= (skb_network_offset(skb) << E1000_ADVTXD_MACLEN_SHIFT);
-	*hdr_len += skb_network_offset(skb);
-	info |= skb_network_header_len(skb);
-	*hdr_len += skb_network_header_len(skb);
-	context_desc->vlan_macip_lens = cpu_to_le32(info);
-
-	/* ADV DTYP TUCMD MKRLOC/ISCSIHEDLEN */
-	tu_cmd |= (E1000_TXD_CMD_DEXT | E1000_ADVTXD_DTYP_CTXT);
-
-	if (skb->protocol == htons(ETH_P_IP))
-		tu_cmd |= E1000_ADVTXD_TUCMD_IPV4;
-	tu_cmd |= E1000_ADVTXD_TUCMD_L4T_TCP;
+	/* compute header lengths */
+	l4len = tcp_hdrlen(skb);
+	*hdr_len = skb_transport_offset(skb) + l4len;
 
-	context_desc->type_tucmd_mlhl = cpu_to_le32(tu_cmd);
+	/* update gso size and bytecount with header size */
+	first->gso_segs = skb_shinfo(skb)->gso_segs;
+	first->bytecount += (first->gso_segs - 1) * *hdr_len;
 
 	/* MSS L4LEN IDX */
-	mss_l4len_idx = (skb_shinfo(skb)->gso_size << E1000_ADVTXD_MSS_SHIFT);
-	mss_l4len_idx |= (l4len << E1000_ADVTXD_L4LEN_SHIFT);
-
-	/* For 82575, context index must be unique per ring. */
-	if (tx_ring->flags & IGB_RING_FLAG_TX_CTX_IDX)
-		mss_l4len_idx |= tx_ring->reg_idx << 4;
-
-	context_desc->mss_l4len_idx = cpu_to_le32(mss_l4len_idx);
-	context_desc->seqnum_seed = 0;
+	mss_l4len_idx = l4len << E1000_ADVTXD_L4LEN_SHIFT;
+	mss_l4len_idx |= skb_shinfo(skb)->gso_size << E1000_ADVTXD_MSS_SHIFT;
 
-	buffer_info->time_stamp = jiffies;
-	buffer_info->next_to_watch = i;
-	buffer_info->dma = 0;
-	i++;
-	if (i == tx_ring->count)
-		i = 0;
+	/* VLAN MACLEN IPLEN */
+	vlan_macip_lens = skb_network_header_len(skb);
+	vlan_macip_lens |= skb_network_offset(skb) << E1000_ADVTXD_MACLEN_SHIFT;
+	vlan_macip_lens |= first->tx_flags & IGB_TX_FLAGS_VLAN_MASK;
 
-	tx_ring->next_to_use = i;
+	igb_tx_ctxtdesc(tx_ring, vlan_macip_lens, type_tucmd, mss_l4len_idx);
 
-	return true;
+	return 1;
+#endif  /* NETIF_F_TSO */
 }
 
-static inline bool igb_tx_csum_adv(struct igb_ring *tx_ring,
-				   struct sk_buff *skb, u32 tx_flags)
+static void igb_tx_csum(struct igb_ring *tx_ring, struct igb_tx_buffer *first)
 {
-	struct e1000_adv_tx_context_desc *context_desc;
-	struct device *dev = tx_ring->dev;
-	struct igb_buffer *buffer_info;
-	u32 info = 0, tu_cmd = 0;
-	unsigned int i;
-
-	if ((skb->ip_summed == CHECKSUM_PARTIAL) ||
-	    (tx_flags & IGB_TX_FLAGS_VLAN)) {
-		i = tx_ring->next_to_use;
-		buffer_info = &tx_ring->buffer_info[i];
-		context_desc = E1000_TX_CTXTDESC_ADV(*tx_ring, i);
-
-		if (tx_flags & IGB_TX_FLAGS_VLAN)
-			info |= (tx_flags & IGB_TX_FLAGS_VLAN_MASK);
-
-		info |= (skb_network_offset(skb) << E1000_ADVTXD_MACLEN_SHIFT);
-		if (skb->ip_summed == CHECKSUM_PARTIAL)
-			info |= skb_network_header_len(skb);
-
-		context_desc->vlan_macip_lens = cpu_to_le32(info);
-
-		tu_cmd |= (E1000_TXD_CMD_DEXT | E1000_ADVTXD_DTYP_CTXT);
-
-		if (skb->ip_summed == CHECKSUM_PARTIAL) {
-			__be16 protocol;
-
-			if (skb->protocol == cpu_to_be16(ETH_P_8021Q)) {
-				const struct vlan_ethhdr *vhdr =
-				          (const struct vlan_ethhdr*)skb->data;
+	struct sk_buff *skb = first->skb;
+	u32 vlan_macip_lens = 0;
+	u32 mss_l4len_idx = 0;
+	u32 type_tucmd = 0;
 
-				protocol = vhdr->h_vlan_encapsulated_proto;
-			} else {
-				protocol = skb->protocol;
+	if (skb->ip_summed != CHECKSUM_PARTIAL) {
+		if (!(first->tx_flags & IGB_TX_FLAGS_VLAN))
+			return;
+	} else {
+		u8 nexthdr = 0;
+		switch (first->protocol) {
+		case __constant_htons(ETH_P_IP):
+			vlan_macip_lens |= skb_network_header_len(skb);
+			type_tucmd |= E1000_ADVTXD_TUCMD_IPV4;
+			nexthdr = ip_hdr(skb)->protocol;
+			break;
+#ifdef NETIF_F_IPV6_CSUM
+		case __constant_htons(ETH_P_IPV6):
+			vlan_macip_lens |= skb_network_header_len(skb);
+			nexthdr = ipv6_hdr(skb)->nexthdr;
+			break;
+#endif
+		default:
+			if (unlikely(net_ratelimit())) {
+				dev_warn(tx_ring->dev,
+				 "partial checksum but proto=%x!\n",
+				 first->protocol);
 			}
+			break;
+		}
 
-			switch (protocol) {
-			case cpu_to_be16(ETH_P_IP):
-				tu_cmd |= E1000_ADVTXD_TUCMD_IPV4;
-				if (ip_hdr(skb)->protocol == IPPROTO_TCP)
-					tu_cmd |= E1000_ADVTXD_TUCMD_L4T_TCP;
-				else if (ip_hdr(skb)->protocol == IPPROTO_SCTP)
-					tu_cmd |= E1000_ADVTXD_TUCMD_L4T_SCTP;
-				break;
-			case cpu_to_be16(ETH_P_IPV6):
-				/* XXX what about other V6 headers?? */
-				if (ipv6_hdr(skb)->nexthdr == IPPROTO_TCP)
-					tu_cmd |= E1000_ADVTXD_TUCMD_L4T_TCP;
-				else if (ipv6_hdr(skb)->nexthdr == IPPROTO_SCTP)
-					tu_cmd |= E1000_ADVTXD_TUCMD_L4T_SCTP;
-				break;
-			default:
-				if (unlikely(net_ratelimit()))
-					dev_warn(dev,
-					    "partial checksum but proto=%x!\n",
-					    skb->protocol);
-				break;
+		switch (nexthdr) {
+		case IPPROTO_TCP:
+			type_tucmd |= E1000_ADVTXD_TUCMD_L4T_TCP;
+			mss_l4len_idx = tcp_hdrlen(skb) <<
+					E1000_ADVTXD_L4LEN_SHIFT;
+			break;
+#ifdef HAVE_SCTP
+		case IPPROTO_SCTP:
+			type_tucmd |= E1000_ADVTXD_TUCMD_L4T_SCTP;
+			mss_l4len_idx = sizeof(struct sctphdr) <<
+					E1000_ADVTXD_L4LEN_SHIFT;
+			break;
+#endif
+		case IPPROTO_UDP:
+			mss_l4len_idx = sizeof(struct udphdr) <<
+					E1000_ADVTXD_L4LEN_SHIFT;
+			break;
+		default:
+			if (unlikely(net_ratelimit())) {
+				dev_warn(tx_ring->dev,
+				 "partial checksum but l4 proto=%x!\n",
+				 nexthdr);
 			}
+			break;
 		}
 
-		context_desc->type_tucmd_mlhl = cpu_to_le32(tu_cmd);
-		context_desc->seqnum_seed = 0;
-		if (tx_ring->flags & IGB_RING_FLAG_TX_CTX_IDX)
-			context_desc->mss_l4len_idx =
-				cpu_to_le32(tx_ring->reg_idx << 4);
+		/* update TX checksum flag */
+		first->tx_flags |= IGB_TX_FLAGS_CSUM;
+	}
 
-		buffer_info->time_stamp = jiffies;
-		buffer_info->next_to_watch = i;
-		buffer_info->dma = 0;
+	vlan_macip_lens |= skb_network_offset(skb) << E1000_ADVTXD_MACLEN_SHIFT;
+	vlan_macip_lens |= first->tx_flags & IGB_TX_FLAGS_VLAN_MASK;
 
-		i++;
-		if (i == tx_ring->count)
-			i = 0;
-		tx_ring->next_to_use = i;
-
-		return true;
-	}
-	return false;
+	igb_tx_ctxtdesc(tx_ring, vlan_macip_lens, type_tucmd, mss_l4len_idx);
 }
 
-#define IGB_MAX_TXD_PWR	16
-#define IGB_MAX_DATA_PER_TXD	(1<<IGB_MAX_TXD_PWR)
+#define IGB_SET_FLAG(_input, _flag, _result) \
+	((_flag <= _result) ? \
+	 ((u32)(_input & _flag) * (_result / _flag)) : \
+	 ((u32)(_input & _flag) / (_flag / _result)))
 
-static inline int igb_tx_map_adv(struct igb_ring *tx_ring, struct sk_buff *skb,
-				 unsigned int first)
+static u32 igb_tx_cmd_type(struct sk_buff *skb, u32 tx_flags)
 {
-	struct igb_buffer *buffer_info;
-	struct device *dev = tx_ring->dev;
-	unsigned int hlen = skb_headlen(skb);
-	unsigned int count = 0, i;
-	unsigned int f;
-	u16 gso_segs = skb_shinfo(skb)->gso_segs ?: 1;
-
-	i = tx_ring->next_to_use;
-
-	buffer_info = &tx_ring->buffer_info[i];
-	BUG_ON(hlen >= IGB_MAX_DATA_PER_TXD);
-	buffer_info->length = hlen;
-	/* set time_stamp *before* dma to help avoid a possible race */
-	buffer_info->time_stamp = jiffies;
-	buffer_info->next_to_watch = i;
-	buffer_info->dma = dma_map_single(dev, skb->data, hlen,
-					  DMA_TO_DEVICE);
-	if (dma_mapping_error(dev, buffer_info->dma))
-		goto dma_error;
-
-	for (f = 0; f < skb_shinfo(skb)->nr_frags; f++) {
-		struct skb_frag_struct *frag = &skb_shinfo(skb)->frags[f];
-		unsigned int len = frag->size;
-
-		count++;
-		i++;
-		if (i == tx_ring->count)
-			i = 0;
+	/* set type for advanced descriptor with frame checksum insertion */
+	u32 cmd_type = E1000_ADVTXD_DTYP_DATA |
+		       E1000_ADVTXD_DCMD_DEXT |
+		       E1000_ADVTXD_DCMD_IFCS;
 
-		buffer_info = &tx_ring->buffer_info[i];
-		BUG_ON(len >= IGB_MAX_DATA_PER_TXD);
-		buffer_info->length = len;
-		buffer_info->time_stamp = jiffies;
-		buffer_info->next_to_watch = i;
-		buffer_info->mapped_as_page = true;
-		buffer_info->dma = dma_map_page(dev,
-						frag->page,
-						frag->page_offset,
-						len,
-						DMA_TO_DEVICE);
-		if (dma_mapping_error(dev, buffer_info->dma))
-			goto dma_error;
+	/* set HW vlan bit if vlan is present */
+	cmd_type |= IGB_SET_FLAG(tx_flags, IGB_TX_FLAGS_VLAN,
+				 (E1000_ADVTXD_DCMD_VLE));
 
-	}
+	/* set segmentation bits for TSO */
+	cmd_type |= IGB_SET_FLAG(tx_flags, IGB_TX_FLAGS_TSO,
+				 (E1000_ADVTXD_DCMD_TSE));
 
-	tx_ring->buffer_info[i].skb = skb;
-	tx_ring->buffer_info[i].tx_flags = skb_shinfo(skb)->tx_flags;
-	/* multiply data chunks by size of headers */
-	tx_ring->buffer_info[i].bytecount = ((gso_segs - 1) * hlen) + skb->len;
-	tx_ring->buffer_info[i].gso_segs = gso_segs;
-	tx_ring->buffer_info[first].next_to_watch = i;
+	/* set timestamp bit if present */
+	cmd_type |= IGB_SET_FLAG(tx_flags, IGB_TX_FLAGS_TSTAMP,
+				 (E1000_ADVTXD_MAC_TSTAMP));
 
-	return ++count;
+	return cmd_type;
+}
 
-dma_error:
-	dev_err(dev, "TX DMA map failed\n");
+static void igb_tx_olinfo_status(struct igb_ring *tx_ring,
+				 union e1000_adv_tx_desc *tx_desc,
+				 u32 tx_flags, unsigned int paylen)
+{
+	u32 olinfo_status = paylen << E1000_ADVTXD_PAYLEN_SHIFT;
 
-	/* clear timestamp and dma mappings for failed buffer_info mapping */
-	buffer_info->dma = 0;
-	buffer_info->time_stamp = 0;
-	buffer_info->length = 0;
-	buffer_info->next_to_watch = 0;
-	buffer_info->mapped_as_page = false;
+	/* 82575 requires a unique index per ring */
+	if (test_bit(IGB_RING_FLAG_TX_CTX_IDX, &tx_ring->flags))
+		olinfo_status |= tx_ring->reg_idx << 4;
 
-	/* clear timestamp and dma mappings for remaining portion of packet */
-	while (count--) {
-		if (i == 0)
-			i = tx_ring->count;
-		i--;
-		buffer_info = &tx_ring->buffer_info[i];
-		igb_unmap_and_free_tx_resource(tx_ring, buffer_info);
-	}
+	/* insert L4 checksum */
+	olinfo_status |= IGB_SET_FLAG(tx_flags,
+				      IGB_TX_FLAGS_CSUM,
+				      (E1000_TXD_POPTS_TXSM << 8));
 
-	return 0;
+	/* insert IPv4 checksum */
+	olinfo_status |= IGB_SET_FLAG(tx_flags,
+				      IGB_TX_FLAGS_IPV4,
+				      (E1000_TXD_POPTS_IXSM << 8));
+
+	tx_desc->read.olinfo_status = cpu_to_le32(olinfo_status);
 }
 
-static inline void igb_tx_queue_adv(struct igb_ring *tx_ring,
-				    u32 tx_flags, int count, u32 paylen,
-				    u8 hdr_len)
+static void igb_tx_map(struct igb_ring *tx_ring,
+		       struct igb_tx_buffer *first,
+		       const u8 hdr_len)
 {
+	struct sk_buff *skb = first->skb;
+	struct igb_tx_buffer *tx_buffer;
 	union e1000_adv_tx_desc *tx_desc;
-	struct igb_buffer *buffer_info;
-	u32 olinfo_status = 0, cmd_type_len;
-	unsigned int i = tx_ring->next_to_use;
+	struct skb_frag_struct *frag;
+	dma_addr_t dma;
+	unsigned int data_len, size;
+	u32 tx_flags = first->tx_flags;
+	u32 cmd_type = igb_tx_cmd_type(skb, tx_flags);
+	u16 i = tx_ring->next_to_use;
 
-	cmd_type_len = (E1000_ADVTXD_DTYP_DATA | E1000_ADVTXD_DCMD_IFCS |
-			E1000_ADVTXD_DCMD_DEXT);
+	tx_desc = IGB_TX_DESC(tx_ring, i);
 
-	if (tx_flags & IGB_TX_FLAGS_VLAN)
-		cmd_type_len |= E1000_ADVTXD_DCMD_VLE;
+	igb_tx_olinfo_status(tx_ring, tx_desc, tx_flags, skb->len - hdr_len);
 
-	if (tx_flags & IGB_TX_FLAGS_TSTAMP)
-		cmd_type_len |= E1000_ADVTXD_MAC_TSTAMP;
+	size = skb_headlen(skb);
+	data_len = skb->data_len;
 
-	if (tx_flags & IGB_TX_FLAGS_TSO) {
-		cmd_type_len |= E1000_ADVTXD_DCMD_TSE;
+	dma = dma_map_single(tx_ring->dev, skb->data, size, DMA_TO_DEVICE);
 
-		/* insert tcp checksum */
-		olinfo_status |= E1000_TXD_POPTS_TXSM << 8;
+	tx_buffer = first;
 
-		/* insert ip checksum */
-		if (tx_flags & IGB_TX_FLAGS_IPV4)
-			olinfo_status |= E1000_TXD_POPTS_IXSM << 8;
+	for (frag = &skb_shinfo(skb)->frags[0];; frag++) {
+		if (dma_mapping_error(tx_ring->dev, dma))
+			goto dma_error;
 
-	} else if (tx_flags & IGB_TX_FLAGS_CSUM) {
-		olinfo_status |= E1000_TXD_POPTS_TXSM << 8;
-	}
+		/* record length, and DMA address */
+		dma_unmap_len_set(tx_buffer, len, size);
+		dma_unmap_addr_set(tx_buffer, dma, dma);
 
-	if ((tx_ring->flags & IGB_RING_FLAG_TX_CTX_IDX) &&
-	    (tx_flags & (IGB_TX_FLAGS_CSUM |
-	                 IGB_TX_FLAGS_TSO |
-			 IGB_TX_FLAGS_VLAN)))
-		olinfo_status |= tx_ring->reg_idx << 4;
+		tx_desc->read.buffer_addr = cpu_to_le64(dma);
 
-	olinfo_status |= ((paylen - hdr_len) << E1000_ADVTXD_PAYLEN_SHIFT);
+		while (unlikely(size > IGB_MAX_DATA_PER_TXD)) {
+			tx_desc->read.cmd_type_len =
+				cpu_to_le32(cmd_type ^ IGB_MAX_DATA_PER_TXD);
+
+			i++;
+			tx_desc++;
+			if (i == tx_ring->count) {
+				tx_desc = IGB_TX_DESC(tx_ring, 0);
+				i = 0;
+			}
+			tx_desc->read.olinfo_status = 0;
+
+			dma += IGB_MAX_DATA_PER_TXD;
+			size -= IGB_MAX_DATA_PER_TXD;
+
+			tx_desc->read.buffer_addr = cpu_to_le64(dma);
+		}
+
+		if (likely(!data_len))
+			break;
+
+		tx_desc->read.cmd_type_len = cpu_to_le32(cmd_type ^ size);
 
-	do {
-		buffer_info = &tx_ring->buffer_info[i];
-		tx_desc = E1000_TX_DESC_ADV(*tx_ring, i);
-		tx_desc->read.buffer_addr = cpu_to_le64(buffer_info->dma);
-		tx_desc->read.cmd_type_len =
-			cpu_to_le32(cmd_type_len | buffer_info->length);
-		tx_desc->read.olinfo_status = cpu_to_le32(olinfo_status);
-		count--;
 		i++;
-		if (i == tx_ring->count)
+		tx_desc++;
+		if (i == tx_ring->count) {
+			tx_desc = IGB_TX_DESC(tx_ring, 0);
 			i = 0;
-	} while (count > 0);
+		}
+		tx_desc->read.olinfo_status = 0;
+
+		size = skb_frag_size(frag);
+		data_len -= size;
 
-	tx_desc->read.cmd_type_len |= cpu_to_le32(IGB_ADVTXD_DCMD);
-	/* Force memory writes to complete before letting h/w
-	 * know there are new descriptors to fetch.  (Only
-	 * applicable for weak-ordered memory model archs,
-	 * such as IA-64). */
+		dma = skb_frag_dma_map(tx_ring->dev, frag, 0,
+				       size, DMA_TO_DEVICE);
+
+		tx_buffer = &tx_ring->tx_buffer_info[i];
+	}
+
+	/* write last descriptor with RS and EOP bits */
+	cmd_type |= size | IGB_TXD_DCMD;
+	tx_desc->read.cmd_type_len = cpu_to_le32(cmd_type);
+
+	netdev_tx_sent_queue(txring_txq(tx_ring), first->bytecount);
+	/* set the timestamp */
+	first->time_stamp = jiffies;
+
+	/*
+	 * Force memory writes to complete before letting h/w know there
+	 * are new descriptors to fetch.  (Only applicable for weak-ordered
+	 * memory model archs, such as IA-64).
+	 *
+	 * We also need this memory barrier to make certain all of the
+	 * status bits have been updated before next_to_watch is written.
+	 */
 	wmb();
 
+	/* set next_to_watch value indicating a packet is present */
+	first->next_to_watch = tx_desc;
+
+	i++;
+	if (i == tx_ring->count)
+		i = 0;
+
 	tx_ring->next_to_use = i;
+
 	writel(i, tx_ring->tail);
+
 	/* we need this if more than one processor can write to our tail
 	 * at a time, it syncronizes IO on IA64/Altix systems */
 	mmiowb();
+
+	return;
+
+dma_error:
+	dev_err(tx_ring->dev, "TX DMA map failed\n");
+
+	/* clear dma mappings for failed tx_buffer_info map */
+	for (;;) {
+		tx_buffer = &tx_ring->tx_buffer_info[i];
+		igb_unmap_and_free_tx_resource(tx_ring, tx_buffer);
+		if (tx_buffer == first)
+			break;
+		if (i == 0)
+			i = tx_ring->count;
+		i--;
+	}
+
+	tx_ring->next_to_use = i;
 }
 
-static int __igb_maybe_stop_tx(struct igb_ring *tx_ring, int size)
+static int __igb_maybe_stop_tx(struct igb_ring *tx_ring, const u16 size)
 {
-	struct net_device *netdev = tx_ring->netdev;
+	struct net_device *netdev = netdev_ring(tx_ring);
 
-	netif_stop_subqueue(netdev, tx_ring->queue_index);
+	if (netif_is_multiqueue(netdev))
+		netif_stop_subqueue(netdev, ring_queue_index(tx_ring));
+	else
+		netif_stop_queue(netdev);
 
 	/* Herbert's original patch had:
 	 *  smp_mb__after_netif_stop_queue();
@@ -4297,95 +5496,128 @@ static int __igb_maybe_stop_tx(struct igb_ring *tx_ring, int size)
 		return -EBUSY;
 
 	/* A reprieve! */
-	netif_wake_subqueue(netdev, tx_ring->queue_index);
+	if (netif_is_multiqueue(netdev))
+		netif_wake_subqueue(netdev, ring_queue_index(tx_ring));
+	else
+		netif_wake_queue(netdev);
 
-	u64_stats_update_begin(&tx_ring->tx_syncp2);
-	tx_ring->tx_stats.restart_queue2++;
-	u64_stats_update_end(&tx_ring->tx_syncp2);
+	tx_ring->tx_stats.restart_queue++;
 
 	return 0;
 }
 
-static inline int igb_maybe_stop_tx(struct igb_ring *tx_ring, int size)
+static inline int igb_maybe_stop_tx(struct igb_ring *tx_ring, const u16 size)
 {
 	if (igb_desc_unused(tx_ring) >= size)
 		return 0;
 	return __igb_maybe_stop_tx(tx_ring, size);
 }
 
-netdev_tx_t igb_xmit_frame_ring_adv(struct sk_buff *skb,
-				    struct igb_ring *tx_ring)
+netdev_tx_t igb_xmit_frame_ring(struct sk_buff *skb,
+				struct igb_ring *tx_ring)
 {
-	int tso = 0, count;
+	struct igb_tx_buffer *first;
+	int tso;
 	u32 tx_flags = 0;
-	u16 first;
+#if PAGE_SIZE > IGB_MAX_DATA_PER_TXD
+	unsigned short f;
+#endif
+	u16 count = TXD_USE_COUNT(skb_headlen(skb));
+	__be16 protocol = vlan_get_protocol(skb);
 	u8 hdr_len = 0;
 
-	/* need: 1 descriptor per page,
+	/*
+	 * need: 1 descriptor per page * PAGE_SIZE/IGB_MAX_DATA_PER_TXD,
+	 *       + 1 desc for skb_headlen/IGB_MAX_DATA_PER_TXD,
 	 *       + 2 desc gap to keep tail from touching head,
-	 *       + 1 desc for skb->data,
 	 *       + 1 desc for context descriptor,
-	 * otherwise try next time */
-	if (igb_maybe_stop_tx(tx_ring, skb_shinfo(skb)->nr_frags + 4)) {
+	 * otherwise try next time
+	 */
+#if PAGE_SIZE > IGB_MAX_DATA_PER_TXD
+	for (f = 0; f < skb_shinfo(skb)->nr_frags; f++)
+		count += TXD_USE_COUNT(skb_shinfo(skb)->frags[f].size);
+#else
+	count += skb_shinfo(skb)->nr_frags;
+#endif
+	if (igb_maybe_stop_tx(tx_ring, count + 3)) {
 		/* this is a hard error */
 		return NETDEV_TX_BUSY;
 	}
 
+	/* record the location of the first descriptor for this packet */
+	first = &tx_ring->tx_buffer_info[tx_ring->next_to_use];
+	first->skb = skb;
+	first->bytecount = skb->len;
+	first->gso_segs = 1;
+
+	skb_tx_timestamp(skb);
+
+#ifdef HAVE_PTP_1588_CLOCK
 	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {
-		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
-		tx_flags |= IGB_TX_FLAGS_TSTAMP;
+		struct igb_adapter *adapter = netdev_priv(tx_ring->netdev);
+		if (!adapter->ptp_tx_skb) {
+			skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+			tx_flags |= IGB_TX_FLAGS_TSTAMP;
+
+			adapter->ptp_tx_skb = skb_get(skb);
+			adapter->ptp_tx_start = jiffies;
+			if (adapter->hw.mac.type == e1000_82576)
+				schedule_work(&adapter->ptp_tx_work);
+		}
 	}
+#endif /* HAVE_PTP_1588_CLOCK */
 
 	if (vlan_tx_tag_present(skb)) {
 		tx_flags |= IGB_TX_FLAGS_VLAN;
 		tx_flags |= (vlan_tx_tag_get(skb) << IGB_TX_FLAGS_VLAN_SHIFT);
 	}
 
-	if (skb->protocol == htons(ETH_P_IP))
-		tx_flags |= IGB_TX_FLAGS_IPV4;
+	/* record initial flags and protocol */
+	first->tx_flags = tx_flags;
+	first->protocol = protocol;
 
-	first = tx_ring->next_to_use;
-	if (skb_is_gso(skb)) {
-		tso = igb_tso_adv(tx_ring, skb, tx_flags, &hdr_len);
+	tso = igb_tso(tx_ring, first, &hdr_len);
+	if (tso < 0)
+		goto out_drop;
+	else if (!tso)
+		igb_tx_csum(tx_ring, first);
 
-		if (tso < 0) {
-			dev_kfree_skb_any(skb);
-			return NETDEV_TX_OK;
-		}
-	}
+	igb_tx_map(tx_ring, first, hdr_len);
 
-	if (tso)
-		tx_flags |= IGB_TX_FLAGS_TSO;
-	else if (igb_tx_csum_adv(tx_ring, skb, tx_flags) &&
-	         (skb->ip_summed == CHECKSUM_PARTIAL))
-		tx_flags |= IGB_TX_FLAGS_CSUM;
+#ifndef HAVE_TRANS_START_IN_QUEUE
+	netdev_ring(tx_ring)->trans_start = jiffies;
 
-	/*
-	 * count reflects descriptors mapped, if 0 or less then mapping error
-	 * has occurred and we need to rewind the descriptor queue
-	 */
-	count = igb_tx_map_adv(tx_ring, skb, first);
-	if (!count) {
-		dev_kfree_skb_any(skb);
-		tx_ring->buffer_info[first].time_stamp = 0;
-		tx_ring->next_to_use = first;
-		return NETDEV_TX_OK;
-	}
+#endif
+	/* Make sure there is space in the ring for the next send. */
+	igb_maybe_stop_tx(tx_ring, DESC_NEEDED);
 
-	igb_tx_queue_adv(tx_ring, tx_flags, count, skb->len, hdr_len);
+	return NETDEV_TX_OK;
 
-	/* Make sure there is space in the ring for the next send. */
-	igb_maybe_stop_tx(tx_ring, MAX_SKB_FRAGS + 4);
+out_drop:
+	igb_unmap_and_free_tx_resource(tx_ring, first);
 
 	return NETDEV_TX_OK;
 }
 
-static netdev_tx_t igb_xmit_frame_adv(struct sk_buff *skb,
-				      struct net_device *netdev)
+#ifdef HAVE_TX_MQ
+static inline struct igb_ring *igb_tx_queue_mapping(struct igb_adapter *adapter,
+                                                    struct sk_buff *skb)
+{
+	unsigned int r_idx = skb->queue_mapping;
+
+	if (r_idx >= adapter->num_tx_queues)
+		r_idx = r_idx % adapter->num_tx_queues;
+
+	return adapter->tx_ring[r_idx];
+}
+#else
+#define igb_tx_queue_mapping(_adapter, _skb) (_adapter)->tx_ring[0]
+#endif
+
+static netdev_tx_t igb_xmit_frame(struct sk_buff *skb,
+                                  struct net_device *netdev)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
-	struct igb_ring *tx_ring;
-	int r_idx = 0;
 
 	if (test_bit(__IGB_DOWN, &adapter->state)) {
 		dev_kfree_skb_any(skb);
@@ -4397,14 +5629,17 @@ static netdev_tx_t igb_xmit_frame_adv(struct sk_buff *skb,
 		return NETDEV_TX_OK;
 	}
 
-	r_idx = skb->queue_mapping & (IGB_ABS_MAX_TX_QUEUES - 1);
-	tx_ring = adapter->multi_tx_table[r_idx];
+	/*
+	 * The minimum packet size with TCTL.PSP set is 17 so pad the skb
+	 * in order to meet this minimum size requirement.
+	 */
+	if (skb->len < 17) {
+		if (skb_padto(skb, 17))
+			return NETDEV_TX_OK;
+		skb->len = 17;
+	}
 
-	/* This goes back to the question of how to logically map a tx queue
-	 * to a flow.  Right now, performance is impacted slightly negatively
-	 * if using multiple tx queues.  If the stack breaks away from a
-	 * single qdisc implementation, we can look at this again. */
-	return igb_xmit_frame_ring_adv(skb, tx_ring);
+	return igb_xmit_frame_ring(skb, igb_tx_queue_mapping(adapter, skb));
 }
 
 /**
@@ -4419,12 +5654,12 @@ static void igb_tx_timeout(struct net_device *netdev)
 	/* Do the reset outside of interrupt context */
 	adapter->tx_timeout_count++;
 
-	if (hw->mac.type == e1000_82580)
+	if (hw->mac.type >= e1000_82580)
 		hw->dev_spec._82575.global_device_reset = true;
 
 	schedule_work(&adapter->reset_task);
-	wr32(E1000_EICS,
-	     (adapter->eims_enable_mask & ~adapter->eims_other));
+	E1000_WRITE_REG(hw, E1000_EICS,
+			(adapter->eims_enable_mask & ~adapter->eims_other));
 }
 
 static void igb_reset_task(struct work_struct *work)
@@ -4432,28 +5667,30 @@ static void igb_reset_task(struct work_struct *work)
 	struct igb_adapter *adapter;
 	adapter = container_of(work, struct igb_adapter, reset_task);
 
-	igb_dump(adapter);
-	netdev_err(adapter->netdev, "Reset adapter\n");
 	igb_reinit_locked(adapter);
 }
 
 /**
- * igb_get_stats64 - Get System Network Statistics
+ * igb_get_stats - Get System Network Statistics
  * @netdev: network interface device structure
- * @stats: rtnl_link_stats64 pointer
  *
+ * Returns the address of the device statistics structure.
+ * The statistics are updated here and also from the timer callback.
  **/
-static struct rtnl_link_stats64 *igb_get_stats64(struct net_device *netdev,
-						 struct rtnl_link_stats64 *stats)
+static struct net_device_stats *igb_get_stats(struct net_device *netdev)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
 
-	spin_lock(&adapter->stats64_lock);
-	igb_update_stats(adapter, &adapter->stats64);
-	memcpy(stats, &adapter->stats64, sizeof(*stats));
-	spin_unlock(&adapter->stats64_lock);
+	if (!test_bit(__IGB_RESETTING, &adapter->state))
+		igb_update_stats(adapter);
 
-	return stats;
+#ifdef HAVE_NETDEV_STATS_IN_NETDEV
+	/* only return the current stats */
+	return &netdev->stats;
+#else
+	/* only return the current stats */
+	return &adapter->net_stats;
+#endif /* HAVE_NETDEV_STATS_IN_NETDEV */
 }
 
 /**
@@ -4466,59 +5703,38 @@ static struct rtnl_link_stats64 *igb_get_stats64(struct net_device *netdev,
 static int igb_change_mtu(struct net_device *netdev, int new_mtu)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
+	struct e1000_hw *hw = &adapter->hw;
 	struct pci_dev *pdev = adapter->pdev;
-	int max_frame = new_mtu + ETH_HLEN + ETH_FCS_LEN;
-	u32 rx_buffer_len, i;
+	int max_frame = new_mtu + ETH_HLEN + ETH_FCS_LEN + VLAN_HLEN;
 
 	if ((new_mtu < 68) || (max_frame > MAX_JUMBO_FRAME_SIZE)) {
-		dev_err(&pdev->dev, "Invalid MTU setting\n");
+		dev_err(pci_dev_to_dev(pdev), "Invalid MTU setting\n");
 		return -EINVAL;
 	}
 
+#define MAX_STD_JUMBO_FRAME_SIZE 9238
 	if (max_frame > MAX_STD_JUMBO_FRAME_SIZE) {
-		dev_err(&pdev->dev, "MTU > 9216 not supported.\n");
+		dev_err(pci_dev_to_dev(pdev), "MTU > 9216 not supported.\n");
 		return -EINVAL;
 	}
 
+	/* adjust max frame to be at least the size of a standard frame */
+	if (max_frame < (ETH_FRAME_LEN + ETH_FCS_LEN))
+		max_frame = ETH_FRAME_LEN + ETH_FCS_LEN;
+
 	while (test_and_set_bit(__IGB_RESETTING, &adapter->state))
-		msleep(1);
+		usleep_range(1000, 2000);
 
 	/* igb_down has a dependency on max_frame_size */
 	adapter->max_frame_size = max_frame;
 
-	/* NOTE: netdev_alloc_skb reserves 16 bytes, and typically NET_IP_ALIGN
-	 * means we reserve 2 more, this pushes us to allocate from the next
-	 * larger slab size.
-	 * i.e. RXBUFFER_2048 --> size-4096 slab
-	 */
-
-	if (adapter->hw.mac.type == e1000_82580)
-		max_frame += IGB_TS_HDR_LEN;
-
-	if (max_frame <= IGB_RXBUFFER_1024)
-		rx_buffer_len = IGB_RXBUFFER_1024;
-	else if (max_frame <= MAXIMUM_ETHERNET_VLAN_SIZE)
-		rx_buffer_len = MAXIMUM_ETHERNET_VLAN_SIZE;
-	else
-		rx_buffer_len = IGB_RXBUFFER_128;
-
-	if ((max_frame == ETH_FRAME_LEN + ETH_FCS_LEN + IGB_TS_HDR_LEN) ||
-	     (max_frame == MAXIMUM_ETHERNET_VLAN_SIZE + IGB_TS_HDR_LEN))
-		rx_buffer_len = MAXIMUM_ETHERNET_VLAN_SIZE + IGB_TS_HDR_LEN;
-
-	if ((adapter->hw.mac.type == e1000_82580) &&
-	    (rx_buffer_len == IGB_RXBUFFER_128))
-		rx_buffer_len += IGB_RXBUFFER_64;
-
 	if (netif_running(netdev))
 		igb_down(adapter);
 
-	dev_info(&pdev->dev, "changing MTU from %d to %d\n",
-		 netdev->mtu, new_mtu);
+	dev_info(pci_dev_to_dev(pdev), "changing MTU from %d to %d\n",
+	        netdev->mtu, new_mtu);
 	netdev->mtu = new_mtu;
-
-	for (i = 0; i < adapter->num_rx_queues; i++)
-		adapter->rx_ring[i]->rx_buffer_len = rx_buffer_len;
+	hw->dev_spec._82575.mtu = new_mtu;
 
 	if (netif_running(netdev))
 		igb_up(adapter);
@@ -4535,17 +5751,25 @@ static int igb_change_mtu(struct net_device *netdev, int new_mtu)
  * @adapter: board private structure
  **/
 
-void igb_update_stats(struct igb_adapter *adapter,
-		      struct rtnl_link_stats64 *net_stats)
+void igb_update_stats(struct igb_adapter *adapter)
 {
+#ifdef HAVE_NETDEV_STATS_IN_NETDEV
+	struct net_device_stats *net_stats = &adapter->netdev->stats;
+#else
+	struct net_device_stats *net_stats = &adapter->net_stats;
+#endif /* HAVE_NETDEV_STATS_IN_NETDEV */
 	struct e1000_hw *hw = &adapter->hw;
+#ifdef HAVE_PCI_ERS
 	struct pci_dev *pdev = adapter->pdev;
+#endif
 	u32 reg, mpc;
 	u16 phy_tmp;
 	int i;
 	u64 bytes, packets;
-	unsigned int start;
-	u64 _bytes, _packets;
+#ifndef IGB_NO_LRO
+	u32 flushed = 0, coal = 0;
+	struct igb_q_vector *q_vector;
+#endif
 
 #define PHY_IDLE_ERROR_COUNT_MASK 0x00FF
 
@@ -4555,25 +5779,39 @@ void igb_update_stats(struct igb_adapter *adapter,
 	 */
 	if (adapter->link_speed == 0)
 		return;
+#ifdef HAVE_PCI_ERS
 	if (pci_channel_offline(pdev))
 		return;
 
+#endif
+#ifndef IGB_NO_LRO
+	for (i = 0; i < adapter->num_q_vectors; i++) {
+		q_vector = adapter->q_vector[i];
+		if (!q_vector)
+			continue;
+		flushed += q_vector->lrolist.stats.flushed;
+		coal += q_vector->lrolist.stats.coal;
+	}
+	adapter->lro_stats.flushed = flushed;
+	adapter->lro_stats.coal = coal;
+
+#endif
 	bytes = 0;
 	packets = 0;
 	for (i = 0; i < adapter->num_rx_queues; i++) {
-		u32 rqdpc_tmp = rd32(E1000_RQDPC(i)) & 0x0FFF;
+		u32 rqdpc_tmp = E1000_READ_REG(hw, E1000_RQDPC(i)) & 0x0FFF;
 		struct igb_ring *ring = adapter->rx_ring[i];
-
 		ring->rx_stats.drops += rqdpc_tmp;
 		net_stats->rx_fifo_errors += rqdpc_tmp;
-
-		do {
-			start = u64_stats_fetch_begin_bh(&ring->rx_syncp);
-			_bytes = ring->rx_stats.bytes;
-			_packets = ring->rx_stats.packets;
-		} while (u64_stats_fetch_retry_bh(&ring->rx_syncp, start));
-		bytes += _bytes;
-		packets += _packets;
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+		if (!ring->vmdq_netdev) {
+			bytes += ring->rx_stats.bytes;
+			packets += ring->rx_stats.packets;
+		}
+#else
+		bytes += ring->rx_stats.bytes;
+		packets += ring->rx_stats.packets;
+#endif
 	}
 
 	net_stats->rx_bytes = bytes;
@@ -4583,93 +5821,98 @@ void igb_update_stats(struct igb_adapter *adapter,
 	packets = 0;
 	for (i = 0; i < adapter->num_tx_queues; i++) {
 		struct igb_ring *ring = adapter->tx_ring[i];
-		do {
-			start = u64_stats_fetch_begin_bh(&ring->tx_syncp);
-			_bytes = ring->tx_stats.bytes;
-			_packets = ring->tx_stats.packets;
-		} while (u64_stats_fetch_retry_bh(&ring->tx_syncp, start));
-		bytes += _bytes;
-		packets += _packets;
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+		if (!ring->vmdq_netdev) {
+			bytes += ring->tx_stats.bytes;
+			packets += ring->tx_stats.packets;
+		}
+#else
+		bytes += ring->tx_stats.bytes;
+		packets += ring->tx_stats.packets;
+#endif
 	}
 	net_stats->tx_bytes = bytes;
 	net_stats->tx_packets = packets;
 
 	/* read stats registers */
-	adapter->stats.crcerrs += rd32(E1000_CRCERRS);
-	adapter->stats.gprc += rd32(E1000_GPRC);
-	adapter->stats.gorc += rd32(E1000_GORCL);
-	rd32(E1000_GORCH); /* clear GORCL */
-	adapter->stats.bprc += rd32(E1000_BPRC);
-	adapter->stats.mprc += rd32(E1000_MPRC);
-	adapter->stats.roc += rd32(E1000_ROC);
-
-	adapter->stats.prc64 += rd32(E1000_PRC64);
-	adapter->stats.prc127 += rd32(E1000_PRC127);
-	adapter->stats.prc255 += rd32(E1000_PRC255);
-	adapter->stats.prc511 += rd32(E1000_PRC511);
-	adapter->stats.prc1023 += rd32(E1000_PRC1023);
-	adapter->stats.prc1522 += rd32(E1000_PRC1522);
-	adapter->stats.symerrs += rd32(E1000_SYMERRS);
-	adapter->stats.sec += rd32(E1000_SEC);
-
-	mpc = rd32(E1000_MPC);
+	adapter->stats.crcerrs += E1000_READ_REG(hw, E1000_CRCERRS);
+	adapter->stats.gprc += E1000_READ_REG(hw, E1000_GPRC);
+	adapter->stats.gorc += E1000_READ_REG(hw, E1000_GORCL);
+	E1000_READ_REG(hw, E1000_GORCH); /* clear GORCL */
+	adapter->stats.bprc += E1000_READ_REG(hw, E1000_BPRC);
+	adapter->stats.mprc += E1000_READ_REG(hw, E1000_MPRC);
+	adapter->stats.roc += E1000_READ_REG(hw, E1000_ROC);
+
+	adapter->stats.prc64 += E1000_READ_REG(hw, E1000_PRC64);
+	adapter->stats.prc127 += E1000_READ_REG(hw, E1000_PRC127);
+	adapter->stats.prc255 += E1000_READ_REG(hw, E1000_PRC255);
+	adapter->stats.prc511 += E1000_READ_REG(hw, E1000_PRC511);
+	adapter->stats.prc1023 += E1000_READ_REG(hw, E1000_PRC1023);
+	adapter->stats.prc1522 += E1000_READ_REG(hw, E1000_PRC1522);
+	adapter->stats.symerrs += E1000_READ_REG(hw, E1000_SYMERRS);
+	adapter->stats.sec += E1000_READ_REG(hw, E1000_SEC);
+
+	mpc = E1000_READ_REG(hw, E1000_MPC);
 	adapter->stats.mpc += mpc;
 	net_stats->rx_fifo_errors += mpc;
-	adapter->stats.scc += rd32(E1000_SCC);
-	adapter->stats.ecol += rd32(E1000_ECOL);
-	adapter->stats.mcc += rd32(E1000_MCC);
-	adapter->stats.latecol += rd32(E1000_LATECOL);
-	adapter->stats.dc += rd32(E1000_DC);
-	adapter->stats.rlec += rd32(E1000_RLEC);
-	adapter->stats.xonrxc += rd32(E1000_XONRXC);
-	adapter->stats.xontxc += rd32(E1000_XONTXC);
-	adapter->stats.xoffrxc += rd32(E1000_XOFFRXC);
-	adapter->stats.xofftxc += rd32(E1000_XOFFTXC);
-	adapter->stats.fcruc += rd32(E1000_FCRUC);
-	adapter->stats.gptc += rd32(E1000_GPTC);
-	adapter->stats.gotc += rd32(E1000_GOTCL);
-	rd32(E1000_GOTCH); /* clear GOTCL */
-	adapter->stats.rnbc += rd32(E1000_RNBC);
-	adapter->stats.ruc += rd32(E1000_RUC);
-	adapter->stats.rfc += rd32(E1000_RFC);
-	adapter->stats.rjc += rd32(E1000_RJC);
-	adapter->stats.tor += rd32(E1000_TORH);
-	adapter->stats.tot += rd32(E1000_TOTH);
-	adapter->stats.tpr += rd32(E1000_TPR);
-
-	adapter->stats.ptc64 += rd32(E1000_PTC64);
-	adapter->stats.ptc127 += rd32(E1000_PTC127);
-	adapter->stats.ptc255 += rd32(E1000_PTC255);
-	adapter->stats.ptc511 += rd32(E1000_PTC511);
-	adapter->stats.ptc1023 += rd32(E1000_PTC1023);
-	adapter->stats.ptc1522 += rd32(E1000_PTC1522);
-
-	adapter->stats.mptc += rd32(E1000_MPTC);
-	adapter->stats.bptc += rd32(E1000_BPTC);
-
-	adapter->stats.tpt += rd32(E1000_TPT);
-	adapter->stats.colc += rd32(E1000_COLC);
-
-	adapter->stats.algnerrc += rd32(E1000_ALGNERRC);
-	/* read internal phy specific stats */
-	reg = rd32(E1000_CTRL_EXT);
+	adapter->stats.scc += E1000_READ_REG(hw, E1000_SCC);
+	adapter->stats.ecol += E1000_READ_REG(hw, E1000_ECOL);
+	adapter->stats.mcc += E1000_READ_REG(hw, E1000_MCC);
+	adapter->stats.latecol += E1000_READ_REG(hw, E1000_LATECOL);
+	adapter->stats.dc += E1000_READ_REG(hw, E1000_DC);
+	adapter->stats.rlec += E1000_READ_REG(hw, E1000_RLEC);
+	adapter->stats.xonrxc += E1000_READ_REG(hw, E1000_XONRXC);
+	adapter->stats.xontxc += E1000_READ_REG(hw, E1000_XONTXC);
+	adapter->stats.xoffrxc += E1000_READ_REG(hw, E1000_XOFFRXC);
+	adapter->stats.xofftxc += E1000_READ_REG(hw, E1000_XOFFTXC);
+	adapter->stats.fcruc += E1000_READ_REG(hw, E1000_FCRUC);
+	adapter->stats.gptc += E1000_READ_REG(hw, E1000_GPTC);
+	adapter->stats.gotc += E1000_READ_REG(hw, E1000_GOTCL);
+	E1000_READ_REG(hw, E1000_GOTCH); /* clear GOTCL */
+	adapter->stats.rnbc += E1000_READ_REG(hw, E1000_RNBC);
+	adapter->stats.ruc += E1000_READ_REG(hw, E1000_RUC);
+	adapter->stats.rfc += E1000_READ_REG(hw, E1000_RFC);
+	adapter->stats.rjc += E1000_READ_REG(hw, E1000_RJC);
+	adapter->stats.tor += E1000_READ_REG(hw, E1000_TORH);
+	adapter->stats.tot += E1000_READ_REG(hw, E1000_TOTH);
+	adapter->stats.tpr += E1000_READ_REG(hw, E1000_TPR);
+
+	adapter->stats.ptc64 += E1000_READ_REG(hw, E1000_PTC64);
+	adapter->stats.ptc127 += E1000_READ_REG(hw, E1000_PTC127);
+	adapter->stats.ptc255 += E1000_READ_REG(hw, E1000_PTC255);
+	adapter->stats.ptc511 += E1000_READ_REG(hw, E1000_PTC511);
+	adapter->stats.ptc1023 += E1000_READ_REG(hw, E1000_PTC1023);
+	adapter->stats.ptc1522 += E1000_READ_REG(hw, E1000_PTC1522);
+
+	adapter->stats.mptc += E1000_READ_REG(hw, E1000_MPTC);
+	adapter->stats.bptc += E1000_READ_REG(hw, E1000_BPTC);
+
+	adapter->stats.tpt += E1000_READ_REG(hw, E1000_TPT);
+	adapter->stats.colc += E1000_READ_REG(hw, E1000_COLC);
+
+	adapter->stats.algnerrc += E1000_READ_REG(hw, E1000_ALGNERRC);
+	/* read internal phy sepecific stats */
+	reg = E1000_READ_REG(hw, E1000_CTRL_EXT);
 	if (!(reg & E1000_CTRL_EXT_LINK_MODE_MASK)) {
-		adapter->stats.rxerrc += rd32(E1000_RXERRC);
-		adapter->stats.tncrs += rd32(E1000_TNCRS);
-	}
-
-	adapter->stats.tsctc += rd32(E1000_TSCTC);
-	adapter->stats.tsctfc += rd32(E1000_TSCTFC);
-
-	adapter->stats.iac += rd32(E1000_IAC);
-	adapter->stats.icrxoc += rd32(E1000_ICRXOC);
-	adapter->stats.icrxptc += rd32(E1000_ICRXPTC);
-	adapter->stats.icrxatc += rd32(E1000_ICRXATC);
-	adapter->stats.ictxptc += rd32(E1000_ICTXPTC);
-	adapter->stats.ictxatc += rd32(E1000_ICTXATC);
-	adapter->stats.ictxqec += rd32(E1000_ICTXQEC);
-	adapter->stats.ictxqmtc += rd32(E1000_ICTXQMTC);
-	adapter->stats.icrxdmtc += rd32(E1000_ICRXDMTC);
+		adapter->stats.rxerrc += E1000_READ_REG(hw, E1000_RXERRC);
+
+		/* this stat has invalid values on i210/i211 */
+		if ((hw->mac.type != e1000_i210) &&
+		    (hw->mac.type != e1000_i211))
+			adapter->stats.tncrs += E1000_READ_REG(hw, E1000_TNCRS);
+	}
+	adapter->stats.tsctc += E1000_READ_REG(hw, E1000_TSCTC);
+	adapter->stats.tsctfc += E1000_READ_REG(hw, E1000_TSCTFC);
+
+	adapter->stats.iac += E1000_READ_REG(hw, E1000_IAC);
+	adapter->stats.icrxoc += E1000_READ_REG(hw, E1000_ICRXOC);
+	adapter->stats.icrxptc += E1000_READ_REG(hw, E1000_ICRXPTC);
+	adapter->stats.icrxatc += E1000_READ_REG(hw, E1000_ICRXATC);
+	adapter->stats.ictxptc += E1000_READ_REG(hw, E1000_ICTXPTC);
+	adapter->stats.ictxatc += E1000_READ_REG(hw, E1000_ICTXATC);
+	adapter->stats.ictxqec += E1000_READ_REG(hw, E1000_ICTXQEC);
+	adapter->stats.ictxqmtc += E1000_READ_REG(hw, E1000_ICTXQMTC);
+	adapter->stats.icrxdmtc += E1000_READ_REG(hw, E1000_ICRXDMTC);
 
 	/* Fill out the OS statistics structure */
 	net_stats->multicast = adapter->stats.mprc;
@@ -4701,24 +5944,20 @@ void igb_update_stats(struct igb_adapter *adapter,
 	/* Phy Stats */
 	if (hw->phy.media_type == e1000_media_type_copper) {
 		if ((adapter->link_speed == SPEED_1000) &&
-		   (!igb_read_phy_reg(hw, PHY_1000T_STATUS, &phy_tmp))) {
+		   (!e1000_read_phy_reg(hw, PHY_1000T_STATUS, &phy_tmp))) {
 			phy_tmp &= PHY_IDLE_ERROR_COUNT_MASK;
 			adapter->phy_stats.idle_errors += phy_tmp;
 		}
 	}
 
 	/* Management Stats */
-	adapter->stats.mgptc += rd32(E1000_MGTPTC);
-	adapter->stats.mgprc += rd32(E1000_MGTPRC);
-	adapter->stats.mgpdc += rd32(E1000_MGTPDC);
-
-	/* OS2BMC Stats */
-	reg = rd32(E1000_MANC);
-	if (reg & E1000_MANC_EN_BMC2OS) {
-		adapter->stats.o2bgptc += rd32(E1000_O2BGPTC);
-		adapter->stats.o2bspc += rd32(E1000_O2BSPC);
-		adapter->stats.b2ospc += rd32(E1000_B2OSPC);
-		adapter->stats.b2ogprc += rd32(E1000_B2OGPRC);
+	adapter->stats.mgptc += E1000_READ_REG(hw, E1000_MGTPTC);
+	adapter->stats.mgprc += E1000_READ_REG(hw, E1000_MGTPRC);
+	if (hw->mac.type > e1000_82580) {
+		adapter->stats.o2bgptc += E1000_READ_REG(hw, E1000_O2BGPTC);
+		adapter->stats.o2bspc += E1000_READ_REG(hw, E1000_O2BSPC);
+		adapter->stats.b2ospc += E1000_READ_REG(hw, E1000_B2OSPC);
+		adapter->stats.b2ogprc += E1000_READ_REG(hw, E1000_B2OGPRC);
 	}
 }
 
@@ -4726,7 +5965,7 @@ static irqreturn_t igb_msix_other(int irq, void *data)
 {
 	struct igb_adapter *adapter = data;
 	struct e1000_hw *hw = &adapter->hw;
-	u32 icr = rd32(E1000_ICR);
+	u32 icr = E1000_READ_REG(hw, E1000_ICR);
 	/* reading ICR causes bit 31 of EICR to be cleared */
 
 	if (icr & E1000_ICR_DRSTA)
@@ -4752,13 +5991,24 @@ static irqreturn_t igb_msix_other(int irq, void *data)
 			mod_timer(&adapter->watchdog_timer, jiffies + 1);
 	}
 
-	if (adapter->vfs_allocated_count)
-		wr32(E1000_IMS, E1000_IMS_LSC |
-				E1000_IMS_VMMB |
-				E1000_IMS_DOUTSYNC);
-	else
-		wr32(E1000_IMS, E1000_IMS_LSC | E1000_IMS_DOUTSYNC);
-	wr32(E1000_EIMS, adapter->eims_other);
+#ifdef HAVE_PTP_1588_CLOCK
+	if (icr & E1000_ICR_TS) {
+		u32 tsicr = E1000_READ_REG(hw, E1000_TSICR);
+
+		if (tsicr & E1000_TSICR_TXTS) {
+			/* acknowledge the interrupt */
+			E1000_WRITE_REG(hw, E1000_TSICR, E1000_TSICR_TXTS);
+			/* retrieve hardware timestamp */
+			schedule_work(&adapter->ptp_tx_work);
+		}
+	}
+#endif /* HAVE_PTP_1588_CLOCK */
+
+	/* Check for MDD event */
+	if (icr & E1000_ICR_MDDET)
+		igb_process_mdd_event(adapter);
+
+	E1000_WRITE_REG(hw, E1000_EIMS, adapter->eims_other);
 
 	return IRQ_HANDLED;
 }
@@ -4777,7 +6027,7 @@ static void igb_write_itr(struct igb_q_vector *q_vector)
 	if (adapter->hw.mac.type == e1000_82575)
 		itr_val |= itr_val << 16;
 	else
-		itr_val |= 0x8000000;
+		itr_val |= E1000_EITR_CNT_IGNR;
 
 	writel(itr_val, q_vector->itr_register);
 	q_vector->set_itr = 0;
@@ -4795,46 +6045,64 @@ static irqreturn_t igb_msix_ring(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-#ifdef CONFIG_IGB_DCA
+#ifdef IGB_DCA
+static void igb_update_tx_dca(struct igb_adapter *adapter,
+			      struct igb_ring *tx_ring,
+			      int cpu)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u32 txctrl = dca3_get_tag(tx_ring->dev, cpu);
+
+	if (hw->mac.type != e1000_82575)
+		txctrl <<= E1000_DCA_TXCTRL_CPUID_SHIFT_82576;
+
+	/*
+	 * We can enable relaxed ordering for reads, but not writes when
+	 * DCA is enabled.  This is due to a known issue in some chipsets
+	 * which will cause the DCA tag to be cleared.
+	 */
+	txctrl |= E1000_DCA_TXCTRL_DESC_RRO_EN |
+		  E1000_DCA_TXCTRL_DATA_RRO_EN |
+		  E1000_DCA_TXCTRL_DESC_DCA_EN;
+
+	E1000_WRITE_REG(hw, E1000_DCA_TXCTRL(tx_ring->reg_idx), txctrl);
+}
+
+static void igb_update_rx_dca(struct igb_adapter *adapter,
+			      struct igb_ring *rx_ring,
+			      int cpu)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u32 rxctrl = dca3_get_tag(&adapter->pdev->dev, cpu);
+
+	if (hw->mac.type != e1000_82575)
+		rxctrl <<= E1000_DCA_RXCTRL_CPUID_SHIFT_82576;
+
+	/*
+	 * We can enable relaxed ordering for reads, but not writes when
+	 * DCA is enabled.  This is due to a known issue in some chipsets
+	 * which will cause the DCA tag to be cleared.
+	 */
+	rxctrl |= E1000_DCA_RXCTRL_DESC_RRO_EN |
+		  E1000_DCA_RXCTRL_DESC_DCA_EN;
+
+	E1000_WRITE_REG(hw, E1000_DCA_RXCTRL(rx_ring->reg_idx), rxctrl);
+}
+
 static void igb_update_dca(struct igb_q_vector *q_vector)
 {
 	struct igb_adapter *adapter = q_vector->adapter;
-	struct e1000_hw *hw = &adapter->hw;
 	int cpu = get_cpu();
 
 	if (q_vector->cpu == cpu)
 		goto out_no_update;
 
-	if (q_vector->tx_ring) {
-		int q = q_vector->tx_ring->reg_idx;
-		u32 dca_txctrl = rd32(E1000_DCA_TXCTRL(q));
-		if (hw->mac.type == e1000_82575) {
-			dca_txctrl &= ~E1000_DCA_TXCTRL_CPUID_MASK;
-			dca_txctrl |= dca3_get_tag(&adapter->pdev->dev, cpu);
-		} else {
-			dca_txctrl &= ~E1000_DCA_TXCTRL_CPUID_MASK_82576;
-			dca_txctrl |= dca3_get_tag(&adapter->pdev->dev, cpu) <<
-			              E1000_DCA_TXCTRL_CPUID_SHIFT;
-		}
-		dca_txctrl |= E1000_DCA_TXCTRL_DESC_DCA_EN;
-		wr32(E1000_DCA_TXCTRL(q), dca_txctrl);
-	}
-	if (q_vector->rx_ring) {
-		int q = q_vector->rx_ring->reg_idx;
-		u32 dca_rxctrl = rd32(E1000_DCA_RXCTRL(q));
-		if (hw->mac.type == e1000_82575) {
-			dca_rxctrl &= ~E1000_DCA_RXCTRL_CPUID_MASK;
-			dca_rxctrl |= dca3_get_tag(&adapter->pdev->dev, cpu);
-		} else {
-			dca_rxctrl &= ~E1000_DCA_RXCTRL_CPUID_MASK_82576;
-			dca_rxctrl |= dca3_get_tag(&adapter->pdev->dev, cpu) <<
-			              E1000_DCA_RXCTRL_CPUID_SHIFT;
-		}
-		dca_rxctrl |= E1000_DCA_RXCTRL_DESC_DCA_EN;
-		dca_rxctrl |= E1000_DCA_RXCTRL_HEAD_DCA_EN;
-		dca_rxctrl |= E1000_DCA_RXCTRL_DATA_DCA_EN;
-		wr32(E1000_DCA_RXCTRL(q), dca_rxctrl);
-	}
+	if (q_vector->tx.ring)
+		igb_update_tx_dca(adapter, q_vector->tx.ring, cpu);
+
+	if (q_vector->rx.ring)
+		igb_update_rx_dca(adapter, q_vector->rx.ring, cpu);
+
 	q_vector->cpu = cpu;
 out_no_update:
 	put_cpu();
@@ -4849,7 +6117,7 @@ static void igb_setup_dca(struct igb_adapter *adapter)
 		return;
 
 	/* Always use CB2 mode, difference is masked in the CB driver. */
-	wr32(E1000_DCA_CTRL, E1000_DCA_CTRL_DCA_MODE_CB2);
+	E1000_WRITE_REG(hw, E1000_DCA_CTRL, E1000_DCA_CTRL_DCA_MODE_CB2);
 
 	for (i = 0; i < adapter->num_q_vectors; i++) {
 		adapter->q_vector[i]->cpu = -1;
@@ -4870,9 +6138,9 @@ static int __igb_notify_dca(struct device *dev, void *data)
 		/* if already enabled, don't do it again */
 		if (adapter->flags & IGB_FLAG_DCA_ENABLED)
 			break;
-		if (dca_add_requester(dev) == 0) {
+		if (dca_add_requester(dev) == E1000_SUCCESS) {
 			adapter->flags |= IGB_FLAG_DCA_ENABLED;
-			dev_info(&pdev->dev, "DCA enabled\n");
+			dev_info(pci_dev_to_dev(pdev), "DCA enabled\n");
 			igb_setup_dca(adapter);
 			break;
 		}
@@ -4882,14 +6150,14 @@ static int __igb_notify_dca(struct device *dev, void *data)
 			/* without this a class_device is left
 			 * hanging around in the sysfs model */
 			dca_remove_requester(dev);
-			dev_info(&pdev->dev, "DCA disabled\n");
+			dev_info(pci_dev_to_dev(pdev), "DCA disabled\n");
 			adapter->flags &= ~IGB_FLAG_DCA_ENABLED;
-			wr32(E1000_DCA_CTRL, E1000_DCA_CTRL_DCA_MODE_DISABLE);
+			E1000_WRITE_REG(hw, E1000_DCA_CTRL, E1000_DCA_CTRL_DCA_DISABLE);
 		}
 		break;
 	}
 
-	return 0;
+	return E1000_SUCCESS;
 }
 
 static int igb_notify_dca(struct notifier_block *nb, unsigned long event,
@@ -4902,7 +6170,24 @@ static int igb_notify_dca(struct notifier_block *nb, unsigned long event,
 
 	return ret_val ? NOTIFY_BAD : NOTIFY_DONE;
 }
-#endif /* CONFIG_IGB_DCA */
+#endif /* IGB_DCA */
+
+static int igb_vf_configure(struct igb_adapter *adapter, int vf)
+{
+	unsigned char mac_addr[ETH_ALEN];
+
+	random_ether_addr(mac_addr);
+	igb_set_vf_mac(adapter, vf, mac_addr);
+
+#ifdef IFLA_VF_MAX
+#ifdef HAVE_VF_SPOOFCHK_CONFIGURE
+	/* By default spoof check is enabled for all VFs */
+	adapter->vf_data[vf].spoofchk_enabled = true;
+#endif
+#endif
+
+	return true;
+}
 
 static void igb_ping_all_vfs(struct igb_adapter *adapter)
 {
@@ -4914,20 +6199,64 @@ static void igb_ping_all_vfs(struct igb_adapter *adapter)
 		ping = E1000_PF_CONTROL_MSG;
 		if (adapter->vf_data[i].flags & IGB_VF_FLAG_CTS)
 			ping |= E1000_VT_MSGTYPE_CTS;
-		igb_write_mbx(hw, &ping, 1, i);
+		e1000_write_mbx(hw, &ping, 1, i);
 	}
 }
 
+/**
+ *  igb_mta_set_ - Set multicast filter table address
+ *  @adapter: pointer to the adapter structure
+ *  @hash_value: determines the MTA register and bit to set
+ *
+ *  The multicast table address is a register array of 32-bit registers.
+ *  The hash_value is used to determine what register the bit is in, the
+ *  current value is read, the new bit is OR'd in and the new value is
+ *  written back into the register.
+ **/
+void igb_mta_set(struct igb_adapter *adapter, u32 hash_value)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u32 hash_bit, hash_reg, mta;
+
+	/*
+	 * The MTA is a register array of 32-bit registers. It is
+	 * treated like an array of (32*mta_reg_count) bits.  We want to
+	 * set bit BitArray[hash_value]. So we figure out what register
+	 * the bit is in, read it, OR in the new bit, then write
+	 * back the new value.  The (hw->mac.mta_reg_count - 1) serves as a
+	 * mask to bits 31:5 of the hash value which gives us the
+	 * register we're modifying.  The hash bit within that register
+	 * is determined by the lower 5 bits of the hash value.
+	 */
+	hash_reg = (hash_value >> 5) & (hw->mac.mta_reg_count - 1);
+	hash_bit = hash_value & 0x1F;
+
+	mta = E1000_READ_REG_ARRAY(hw, E1000_MTA, hash_reg);
+
+	mta |= (1 << hash_bit);
+
+	E1000_WRITE_REG_ARRAY(hw, E1000_MTA, hash_reg, mta);
+	E1000_WRITE_FLUSH(hw);
+}
+
 static int igb_set_vf_promisc(struct igb_adapter *adapter, u32 *msgbuf, u32 vf)
 {
+
 	struct e1000_hw *hw = &adapter->hw;
-	u32 vmolr = rd32(E1000_VMOLR(vf));
+	u32 vmolr = E1000_READ_REG(hw, E1000_VMOLR(vf));
 	struct vf_data_storage *vf_data = &adapter->vf_data[vf];
 
 	vf_data->flags &= ~(IGB_VF_FLAG_UNI_PROMISC |
 	                    IGB_VF_FLAG_MULTI_PROMISC);
 	vmolr &= ~(E1000_VMOLR_ROPE | E1000_VMOLR_ROMPE | E1000_VMOLR_MPME);
 
+#ifdef IGB_ENABLE_VF_PROMISC
+	if (*msgbuf & E1000_VF_SET_PROMISC_UNICAST) {
+		vmolr |= E1000_VMOLR_ROPE;
+		vf_data->flags |= IGB_VF_FLAG_UNI_PROMISC;
+		*msgbuf &= ~E1000_VF_SET_PROMISC_UNICAST;
+	}
+#endif
 	if (*msgbuf & E1000_VF_SET_PROMISC_MULTICAST) {
 		vmolr |= E1000_VMOLR_MPME;
 		vf_data->flags |= IGB_VF_FLAG_MULTI_PROMISC;
@@ -4944,11 +6273,11 @@ static int igb_set_vf_promisc(struct igb_adapter *adapter, u32 *msgbuf, u32 vf)
 			int j;
 			vmolr |= E1000_VMOLR_ROMPE;
 			for (j = 0; j < vf_data->num_vf_mc_hashes; j++)
-				igb_mta_set(hw, vf_data->vf_mc_hashes[j]);
+				igb_mta_set(adapter, vf_data->vf_mc_hashes[j]);
 		}
 	}
 
-	wr32(E1000_VMOLR(vf), vmolr);
+	E1000_WRITE_REG(hw, E1000_VMOLR(vf), vmolr);
 
 	/* there are flags left unprocessed, likely not supported */
 	if (*msgbuf & E1000_VT_MSGINFO_MASK)
@@ -4993,7 +6322,7 @@ static void igb_restore_vf_multicasts(struct igb_adapter *adapter)
 	int i, j;
 
 	for (i = 0; i < adapter->vfs_allocated_count; i++) {
-		u32 vmolr = rd32(E1000_VMOLR(i));
+		u32 vmolr = E1000_READ_REG(hw, E1000_VMOLR(i));
 		vmolr &= ~(E1000_VMOLR_ROMPE | E1000_VMOLR_MPME);
 
 		vf_data = &adapter->vf_data[i];
@@ -5004,9 +6333,9 @@ static void igb_restore_vf_multicasts(struct igb_adapter *adapter)
 		} else if (vf_data->num_vf_mc_hashes) {
 			vmolr |= E1000_VMOLR_ROMPE;
 			for (j = 0; j < vf_data->num_vf_mc_hashes; j++)
-				igb_mta_set(hw, vf_data->vf_mc_hashes[j]);
+				igb_mta_set(adapter, vf_data->vf_mc_hashes[j]);
 		}
-		wr32(E1000_VMOLR(i), vmolr);
+		E1000_WRITE_REG(hw, E1000_VMOLR(i), vmolr);
 	}
 }
 
@@ -5014,13 +6343,14 @@ static void igb_clear_vf_vfta(struct igb_adapter *adapter, u32 vf)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	u32 pool_mask, reg, vid;
+	u16 vlan_default;
 	int i;
 
 	pool_mask = 1 << (E1000_VLVF_POOLSEL_SHIFT + vf);
 
 	/* Find the vlan filter for this id */
 	for (i = 0; i < E1000_VLVF_ARRAY_SIZE; i++) {
-		reg = rd32(E1000_VLVF(i));
+		reg = E1000_READ_REG(hw, E1000_VLVF(i));
 
 		/* remove the vf from the pool */
 		reg &= ~pool_mask;
@@ -5030,16 +6360,20 @@ static void igb_clear_vf_vfta(struct igb_adapter *adapter, u32 vf)
 		    (reg & E1000_VLVF_VLANID_ENABLE)) {
 			reg = 0;
 			vid = reg & E1000_VLVF_VLANID_MASK;
-			igb_vfta_set(hw, vid, false);
+			igb_vfta_set(adapter, vid, FALSE);
 		}
 
-		wr32(E1000_VLVF(i), reg);
+		E1000_WRITE_REG(hw, E1000_VLVF(i), reg);
 	}
 
 	adapter->vf_data[vf].vlans_enabled = 0;
+
+	vlan_default = adapter->vf_data[vf].default_vf_vlan_id;
+	if (vlan_default)
+		igb_vlvf_set(adapter, vlan_default, true, vf);
 }
 
-static s32 igb_vlvf_set(struct igb_adapter *adapter, u32 vid, bool add, u32 vf)
+s32 igb_vlvf_set(struct igb_adapter *adapter, u32 vid, bool add, u32 vf)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	u32 reg, i;
@@ -5049,12 +6383,12 @@ static s32 igb_vlvf_set(struct igb_adapter *adapter, u32 vid, bool add, u32 vf)
 		return -1;
 
 	/* we only need to do this if VMDq is enabled */
-	if (!adapter->vfs_allocated_count)
+	if (!adapter->vmdq_pools)
 		return -1;
 
 	/* Find the vlan filter for this id */
 	for (i = 0; i < E1000_VLVF_ARRAY_SIZE; i++) {
-		reg = rd32(E1000_VLVF(i));
+		reg = E1000_READ_REG(hw, E1000_VLVF(i));
 		if ((reg & E1000_VLVF_VLANID_ENABLE) &&
 		    vid == (reg & E1000_VLVF_VLANID_MASK))
 			break;
@@ -5067,7 +6401,7 @@ static s32 igb_vlvf_set(struct igb_adapter *adapter, u32 vid, bool add, u32 vf)
 			 * one without the enable bit set
 			 */
 			for (i = 0; i < E1000_VLVF_ARRAY_SIZE; i++) {
-				reg = rd32(E1000_VLVF(i));
+				reg = E1000_READ_REG(hw, E1000_VLVF(i));
 				if (!(reg & E1000_VLVF_VLANID_ENABLE))
 					break;
 			}
@@ -5079,29 +6413,28 @@ static s32 igb_vlvf_set(struct igb_adapter *adapter, u32 vid, bool add, u32 vf)
 			/* if !enabled we need to set this up in vfta */
 			if (!(reg & E1000_VLVF_VLANID_ENABLE)) {
 				/* add VID to filter table */
-				igb_vfta_set(hw, vid, true);
+				igb_vfta_set(adapter, vid, TRUE);
 				reg |= E1000_VLVF_VLANID_ENABLE;
 			}
 			reg &= ~E1000_VLVF_VLANID_MASK;
 			reg |= vid;
-			wr32(E1000_VLVF(i), reg);
+			E1000_WRITE_REG(hw, E1000_VLVF(i), reg);
 
 			/* do not modify RLPML for PF devices */
 			if (vf >= adapter->vfs_allocated_count)
-				return 0;
+				return E1000_SUCCESS;
 
 			if (!adapter->vf_data[vf].vlans_enabled) {
 				u32 size;
-				reg = rd32(E1000_VMOLR(vf));
+				reg = E1000_READ_REG(hw, E1000_VMOLR(vf));
 				size = reg & E1000_VMOLR_RLPML_MASK;
 				size += 4;
 				reg &= ~E1000_VMOLR_RLPML_MASK;
 				reg |= size;
-				wr32(E1000_VMOLR(vf), reg);
+				E1000_WRITE_REG(hw, E1000_VMOLR(vf), reg);
 			}
 
 			adapter->vf_data[vf].vlans_enabled++;
-			return 0;
 		}
 	} else {
 		if (i < E1000_VLVF_ARRAY_SIZE) {
@@ -5110,37 +6443,38 @@ static s32 igb_vlvf_set(struct igb_adapter *adapter, u32 vid, bool add, u32 vf)
 			/* if pool is empty then remove entry from vfta */
 			if (!(reg & E1000_VLVF_POOLSEL_MASK)) {
 				reg = 0;
-				igb_vfta_set(hw, vid, false);
+				igb_vfta_set(adapter, vid, FALSE);
 			}
-			wr32(E1000_VLVF(i), reg);
+			E1000_WRITE_REG(hw, E1000_VLVF(i), reg);
 
 			/* do not modify RLPML for PF devices */
 			if (vf >= adapter->vfs_allocated_count)
-				return 0;
+				return E1000_SUCCESS;
 
 			adapter->vf_data[vf].vlans_enabled--;
 			if (!adapter->vf_data[vf].vlans_enabled) {
 				u32 size;
-				reg = rd32(E1000_VMOLR(vf));
+				reg = E1000_READ_REG(hw, E1000_VMOLR(vf));
 				size = reg & E1000_VMOLR_RLPML_MASK;
 				size -= 4;
 				reg &= ~E1000_VMOLR_RLPML_MASK;
 				reg |= size;
-				wr32(E1000_VMOLR(vf), reg);
+				E1000_WRITE_REG(hw, E1000_VMOLR(vf), reg);
 			}
 		}
 	}
-	return 0;
+	return E1000_SUCCESS;
 }
 
+#ifdef IFLA_VF_MAX
 static void igb_set_vmvir(struct igb_adapter *adapter, u32 vid, u32 vf)
 {
 	struct e1000_hw *hw = &adapter->hw;
 
 	if (vid)
-		wr32(E1000_VMVIR(vf), (vid | E1000_VMVIR_VLANA_DEFAULT));
+		E1000_WRITE_REG(hw, E1000_VMVIR(vf), (vid | E1000_VMVIR_VLANA_DEFAULT));
 	else
-		wr32(E1000_VMVIR(vf), 0);
+		E1000_WRITE_REG(hw, E1000_VMVIR(vf), 0);
 }
 
 static int igb_ndo_set_vf_vlan(struct net_device *netdev,
@@ -5149,7 +6483,8 @@ static int igb_ndo_set_vf_vlan(struct net_device *netdev,
 	int err = 0;
 	struct igb_adapter *adapter = netdev_priv(netdev);
 
-	if ((vf >= adapter->vfs_allocated_count) || (vlan > 4095) || (qos > 7))
+	/* VLAN IDs accepted range 0-4094 */
+	if ((vf >= adapter->vfs_allocated_count) || (vlan > VLAN_VID_MASK-1) || (qos > 7))
 		return -EINVAL;
 	if (vlan || qos) {
 		err = igb_vlvf_set(adapter, vlan, !!vlan, vf);
@@ -5159,6 +6494,7 @@ static int igb_ndo_set_vf_vlan(struct net_device *netdev,
 		igb_set_vmolr(adapter, vf, !vlan);
 		adapter->vf_data[vf].pf_vlan = vlan;
 		adapter->vf_data[vf].pf_qos = qos;
+		igb_set_vf_vlan_strip(adapter, vf, true);
 		dev_info(&adapter->pdev->dev,
 			 "Setting VLAN %d, QOS 0x%x on VF %d\n", vlan, qos, vf);
 		if (test_bit(__IGB_DOWN, &adapter->state)) {
@@ -5170,10 +6506,14 @@ static int igb_ndo_set_vf_vlan(struct net_device *netdev,
 				 " attempting to use the VF device.\n");
 		}
 	} else {
+		if (adapter->vf_data[vf].pf_vlan)
+			dev_info(&adapter->pdev->dev,
+				 "Clearing VLAN on VF %d\n", vf);
 		igb_vlvf_set(adapter, adapter->vf_data[vf].pf_vlan,
 				   false, vf);
 		igb_set_vmvir(adapter, vlan, vf);
 		igb_set_vmolr(adapter, vf, true);
+		igb_set_vf_vlan_strip(adapter, vf, false);
 		adapter->vf_data[vf].pf_vlan = 0;
 		adapter->vf_data[vf].pf_qos = 0;
        }
@@ -5181,17 +6521,119 @@ out:
        return err;
 }
 
+#ifdef HAVE_VF_SPOOFCHK_CONFIGURE
+static int igb_ndo_set_vf_spoofchk(struct net_device *netdev, int vf,
+				bool setting)
+{
+	struct igb_adapter *adapter = netdev_priv(netdev);
+	struct e1000_hw *hw = &adapter->hw;
+	u32 dtxswc, reg_offset;
+
+	if (!adapter->vfs_allocated_count)
+		return -EOPNOTSUPP;
+
+	if (vf >= adapter->vfs_allocated_count)
+		return -EINVAL;
+
+	reg_offset = (hw->mac.type == e1000_82576) ? E1000_DTXSWC : E1000_TXSWC;
+	dtxswc = E1000_READ_REG(hw, reg_offset);
+	if (setting)
+		dtxswc |= ((1 << vf) |
+			   (1 << (vf + E1000_DTXSWC_VLAN_SPOOF_SHIFT)));
+	else
+		dtxswc &= ~((1 << vf) |
+			    (1 << (vf + E1000_DTXSWC_VLAN_SPOOF_SHIFT)));
+	E1000_WRITE_REG(hw, reg_offset, dtxswc);
+
+	adapter->vf_data[vf].spoofchk_enabled = setting;
+	return E1000_SUCCESS;
+}
+#endif /* HAVE_VF_SPOOFCHK_CONFIGURE */
+#endif /* IFLA_VF_MAX */
+
+static int igb_find_vlvf_entry(struct igb_adapter *adapter, int vid)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	int i;
+	u32 reg;
+
+	/* Find the vlan filter for this id */
+	for (i = 0; i < E1000_VLVF_ARRAY_SIZE; i++) {
+		reg = E1000_READ_REG(hw, E1000_VLVF(i));
+		if ((reg & E1000_VLVF_VLANID_ENABLE) &&
+		    vid == (reg & E1000_VLVF_VLANID_MASK))
+			break;
+	}
+
+	if (i >= E1000_VLVF_ARRAY_SIZE)
+		i = -1;
+
+	return i;
+}
+
 static int igb_set_vf_vlan(struct igb_adapter *adapter, u32 *msgbuf, u32 vf)
 {
+	struct e1000_hw *hw = &adapter->hw;
 	int add = (msgbuf[0] & E1000_VT_MSGINFO_MASK) >> E1000_VT_MSGINFO_SHIFT;
 	int vid = (msgbuf[1] & E1000_VLVF_VLANID_MASK);
+	int err = 0;
 
-	return igb_vlvf_set(adapter, vid, add, vf);
+	if (vid)
+		igb_set_vf_vlan_strip(adapter, vf, true);
+	else
+		igb_set_vf_vlan_strip(adapter, vf, false);
+
+	/* If in promiscuous mode we need to make sure the PF also has
+	 * the VLAN filter set.
+	 */
+	if (add && (adapter->netdev->flags & IFF_PROMISC))
+		err = igb_vlvf_set(adapter, vid, add,
+				   adapter->vfs_allocated_count);
+	if (err)
+		goto out;
+
+	err = igb_vlvf_set(adapter, vid, add, vf);
+
+	if (err)
+		goto out;
+
+	/* Go through all the checks to see if the VLAN filter should
+	 * be wiped completely.
+	 */
+	if (!add && (adapter->netdev->flags & IFF_PROMISC)) {
+		u32 vlvf, bits;
+
+		int regndx = igb_find_vlvf_entry(adapter, vid);
+		if (regndx < 0)
+			goto out;
+		/* See if any other pools are set for this VLAN filter
+		 * entry other than the PF.
+		 */
+		vlvf = bits = E1000_READ_REG(hw, E1000_VLVF(regndx));
+		bits &= 1 << (E1000_VLVF_POOLSEL_SHIFT +
+			      adapter->vfs_allocated_count);
+		/* If the filter was removed then ensure PF pool bit
+		 * is cleared if the PF only added itself to the pool
+		 * because the PF is in promiscuous mode.
+		 */
+		if ((vlvf & VLAN_VID_MASK) == vid &&
+#ifndef HAVE_VLAN_RX_REGISTER
+		    !test_bit(vid, adapter->active_vlans) &&
+#endif
+		    !bits)
+			igb_vlvf_set(adapter, vid, add,
+				     adapter->vfs_allocated_count);
+	}
+
+out:
+	return err;
 }
 
 static inline void igb_vf_reset(struct igb_adapter *adapter, u32 vf)
 {
-	/* clear flags - except flag that indicates PF has set the MAC */
+	struct e1000_hw *hw = &adapter->hw;
+
+	/* clear flags except flag that the PF has set the MAC */
 	adapter->vf_data[vf].flags &= IGB_VF_FLAG_PF_SET_MAC;
 	adapter->vf_data[vf].last_nack = jiffies;
 
@@ -5200,18 +6642,31 @@ static inline void igb_vf_reset(struct igb_adapter *adapter, u32 vf)
 
 	/* reset vlans for device */
 	igb_clear_vf_vfta(adapter, vf);
+#ifdef IFLA_VF_MAX
 	if (adapter->vf_data[vf].pf_vlan)
 		igb_ndo_set_vf_vlan(adapter->netdev, vf,
 				    adapter->vf_data[vf].pf_vlan,
 				    adapter->vf_data[vf].pf_qos);
 	else
 		igb_clear_vf_vfta(adapter, vf);
+#endif
 
 	/* reset multicast table array for vf */
 	adapter->vf_data[vf].num_vf_mc_hashes = 0;
 
 	/* Flush and reset the mta with the new values */
 	igb_set_rx_mode(adapter->netdev);
+
+	/*
+	 * Reset the VFs TDWBAL and TDWBAH registers which are not
+	 * cleared by a VFLR
+	 */
+	E1000_WRITE_REG(hw, E1000_TDWBAH(vf), 0);
+	E1000_WRITE_REG(hw, E1000_TDWBAL(vf), 0);
+	if (hw->mac.type == e1000_82576) {
+		E1000_WRITE_REG(hw, E1000_TDWBAH(IGB_MAX_VF_FUNCTIONS + vf), 0);
+		E1000_WRITE_REG(hw, E1000_TDWBAL(IGB_MAX_VF_FUNCTIONS + vf), 0);
+	}
 }
 
 static void igb_vf_reset_event(struct igb_adapter *adapter, u32 vf)
@@ -5241,17 +6696,17 @@ static void igb_vf_reset_msg(struct igb_adapter *adapter, u32 vf)
 	igb_rar_set_qsel(adapter, vf_mac, rar_entry, vf);
 
 	/* enable transmit and receive for vf */
-	reg = rd32(E1000_VFTE);
-	wr32(E1000_VFTE, reg | (1 << vf));
-	reg = rd32(E1000_VFRE);
-	wr32(E1000_VFRE, reg | (1 << vf));
+	reg = E1000_READ_REG(hw, E1000_VFTE);
+	E1000_WRITE_REG(hw, E1000_VFTE, reg | (1 << vf));
+	reg = E1000_READ_REG(hw, E1000_VFRE);
+	E1000_WRITE_REG(hw, E1000_VFRE, reg | (1 << vf));
 
 	adapter->vf_data[vf].flags |= IGB_VF_FLAG_CTS;
 
 	/* reply to reset with ack and vf mac address */
 	msgbuf[0] = E1000_VF_RESET | E1000_VT_MSGTYPE_ACK;
 	memcpy(addr, vf_mac, 6);
-	igb_write_mbx(hw, msgbuf, 3, vf);
+	e1000_write_mbx(hw, msgbuf, 3, vf);
 }
 
 static int igb_set_vf_mac_addr(struct igb_adapter *adapter, u32 *msg, int vf)
@@ -5260,7 +6715,7 @@ static int igb_set_vf_mac_addr(struct igb_adapter *adapter, u32 *msg, int vf)
 	 * The VF MAC Address is stored in a packed array of bytes
 	 * starting at the second 32 bit word of the msg array
 	 */
-	unsigned char *addr = (char *)&msg[1];
+	unsigned char *addr = (unsigned char *)&msg[1];
 	int err = -1;
 
 	if (is_valid_ether_addr(addr))
@@ -5278,7 +6733,7 @@ static void igb_rcv_ack_from_vf(struct igb_adapter *adapter, u32 vf)
 	/* if device isn't clear to send it shouldn't be reading either */
 	if (!(vf_data->flags & IGB_VF_FLAG_CTS) &&
 	    time_after(jiffies, vf_data->last_nack + (2 * HZ))) {
-		igb_write_mbx(hw, &msg, 1, vf);
+		e1000_write_mbx(hw, &msg, 1, vf);
 		vf_data->last_nack = jiffies;
 	}
 }
@@ -5291,15 +6746,11 @@ static void igb_rcv_msg_from_vf(struct igb_adapter *adapter, u32 vf)
 	struct vf_data_storage *vf_data = &adapter->vf_data[vf];
 	s32 retval;
 
-	retval = igb_read_mbx(hw, msgbuf, E1000_VFMAILBOX_SIZE, vf);
+	retval = e1000_read_mbx(hw, msgbuf, E1000_VFMAILBOX_SIZE, vf);
 
 	if (retval) {
-		/* if receive failed revoke VF CTS stats and restart init */
-		dev_err(&pdev->dev, "Error receiving message from VF\n");
-		vf_data->flags &= ~IGB_VF_FLAG_CTS;
-		if (!time_after(jiffies, vf_data->last_nack + (2 * HZ)))
-			return;
-		goto out;
+		dev_err(pci_dev_to_dev(pdev), "Error receiving message from VF\n");
+		return;
 	}
 
 	/* this is a message we already processed, do nothing */
@@ -5317,22 +6768,26 @@ static void igb_rcv_msg_from_vf(struct igb_adapter *adapter, u32 vf)
 	}
 
 	if (!(vf_data->flags & IGB_VF_FLAG_CTS)) {
-		if (!time_after(jiffies, vf_data->last_nack + (2 * HZ)))
-			return;
-		retval = -1;
-		goto out;
+		msgbuf[0] = E1000_VT_MSGTYPE_NACK;
+		if (time_after(jiffies, vf_data->last_nack + (2 * HZ))) {
+			e1000_write_mbx(hw, msgbuf, 1, vf);
+			vf_data->last_nack = jiffies;
+		}
+		return;
 	}
 
 	switch ((msgbuf[0] & 0xFFFF)) {
 	case E1000_VF_SET_MAC_ADDR:
 		retval = -EINVAL;
+#ifndef IGB_DISABLE_VF_MAC_SET
 		if (!(vf_data->flags & IGB_VF_FLAG_PF_SET_MAC))
 			retval = igb_set_vf_mac_addr(adapter, msgbuf, vf);
 		else
-			dev_warn(&pdev->dev,
-				 "VF %d attempted to override administratively "
-				 "set MAC address\nReload the VF driver to "
-				 "resume operations\n", vf);
+			DPRINTK(DRV, INFO,
+				"VF %d attempted to override administratively "
+				"set MAC address\nReload the VF driver to "
+				"resume operations\n", vf);
+#endif
 		break;
 	case E1000_VF_SET_PROMISC:
 		retval = igb_set_vf_promisc(adapter, msgbuf, vf);
@@ -5345,29 +6800,31 @@ static void igb_rcv_msg_from_vf(struct igb_adapter *adapter, u32 vf)
 		break;
 	case E1000_VF_SET_VLAN:
 		retval = -1;
+#ifdef IFLA_VF_MAX
 		if (vf_data->pf_vlan)
-			dev_warn(&pdev->dev,
-				 "VF %d attempted to override administratively "
-				 "set VLAN tag\nReload the VF driver to "
-				 "resume operations\n", vf);
+			DPRINTK(DRV, INFO,
+				"VF %d attempted to override administratively "
+				"set VLAN tag\nReload the VF driver to "
+				"resume operations\n", vf);
 		else
+#endif
 			retval = igb_set_vf_vlan(adapter, msgbuf, vf);
 		break;
 	default:
-		dev_err(&pdev->dev, "Unhandled Msg %08x\n", msgbuf[0]);
-		retval = -1;
+		dev_err(pci_dev_to_dev(pdev), "Unhandled Msg %08x\n", msgbuf[0]);
+		retval = -E1000_ERR_MBX;
 		break;
 	}
 
-	msgbuf[0] |= E1000_VT_MSGTYPE_CTS;
-out:
 	/* notify the VF of the results of what it sent us */
 	if (retval)
 		msgbuf[0] |= E1000_VT_MSGTYPE_NACK;
 	else
 		msgbuf[0] |= E1000_VT_MSGTYPE_ACK;
 
-	igb_write_mbx(hw, msgbuf, 1, vf);
+	msgbuf[0] |= E1000_VT_MSGTYPE_CTS;
+
+	e1000_write_mbx(hw, msgbuf, 1, vf);
 }
 
 static void igb_msg_task(struct igb_adapter *adapter)
@@ -5377,15 +6834,15 @@ static void igb_msg_task(struct igb_adapter *adapter)
 
 	for (vf = 0; vf < adapter->vfs_allocated_count; vf++) {
 		/* process any reset requests */
-		if (!igb_check_for_rst(hw, vf))
+		if (!e1000_check_for_rst(hw, vf))
 			igb_vf_reset_event(adapter, vf);
 
 		/* process any messages pending */
-		if (!igb_check_for_msg(hw, vf))
+		if (!e1000_check_for_msg(hw, vf))
 			igb_rcv_msg_from_vf(adapter, vf);
 
 		/* process any acks */
-		if (!igb_check_for_ack(hw, vf))
+		if (!e1000_check_for_ack(hw, vf))
 			igb_rcv_ack_from_vf(adapter, vf);
 	}
 }
@@ -5410,11 +6867,11 @@ static void igb_set_uta(struct igb_adapter *adapter)
 		return;
 
 	/* we only need to do this if VMDq is enabled */
-	if (!adapter->vfs_allocated_count)
+	if (!adapter->vmdq_pools)
 		return;
 
 	for (i = 0; i < hw->mac.uta_reg_count; i++)
-		array_wr32(E1000_UTA, i, ~0);
+		E1000_WRITE_REG_ARRAY(hw, E1000_UTA, i, ~0);
 }
 
 /**
@@ -5428,7 +6885,7 @@ static irqreturn_t igb_intr_msi(int irq, void *data)
 	struct igb_q_vector *q_vector = adapter->q_vector[0];
 	struct e1000_hw *hw = &adapter->hw;
 	/* read ICR disables interrupts using IAM */
-	u32 icr = rd32(E1000_ICR);
+	u32 icr = E1000_READ_REG(hw, E1000_ICR);
 
 	igb_write_itr(q_vector);
 
@@ -5446,6 +6903,19 @@ static irqreturn_t igb_intr_msi(int irq, void *data)
 			mod_timer(&adapter->watchdog_timer, jiffies + 1);
 	}
 
+#ifdef HAVE_PTP_1588_CLOCK
+	if (icr & E1000_ICR_TS) {
+		u32 tsicr = E1000_READ_REG(hw, E1000_TSICR);
+
+		if (tsicr & E1000_TSICR_TXTS) {
+			/* acknowledge the interrupt */
+			E1000_WRITE_REG(hw, E1000_TSICR, E1000_TSICR_TXTS);
+			/* retrieve hardware timestamp */
+			schedule_work(&adapter->ptp_tx_work);
+		}
+	}
+#endif /* HAVE_PTP_1588_CLOCK */
+
 	napi_schedule(&q_vector->napi);
 
 	return IRQ_HANDLED;
@@ -5463,17 +6933,15 @@ static irqreturn_t igb_intr(int irq, void *data)
 	struct e1000_hw *hw = &adapter->hw;
 	/* Interrupt Auto-Mask...upon reading ICR, interrupts are masked.  No
 	 * need for the IMC write */
-	u32 icr = rd32(E1000_ICR);
-	if (!icr)
-		return IRQ_NONE;  /* Not our interrupt */
-
-	igb_write_itr(q_vector);
+	u32 icr = E1000_READ_REG(hw, E1000_ICR);
 
 	/* IMS will not auto-mask if INT_ASSERTED is not set, and if it is
 	 * not set, then the adapter didn't send an interrupt */
 	if (!(icr & E1000_ICR_INT_ASSERTED))
 		return IRQ_NONE;
 
+	igb_write_itr(q_vector);
+
 	if (icr & E1000_ICR_DRSTA)
 		schedule_work(&adapter->reset_task);
 
@@ -5489,27 +6957,40 @@ static irqreturn_t igb_intr(int irq, void *data)
 			mod_timer(&adapter->watchdog_timer, jiffies + 1);
 	}
 
+#ifdef HAVE_PTP_1588_CLOCK
+	if (icr & E1000_ICR_TS) {
+		u32 tsicr = E1000_READ_REG(hw, E1000_TSICR);
+
+		if (tsicr & E1000_TSICR_TXTS) {
+			/* acknowledge the interrupt */
+			E1000_WRITE_REG(hw, E1000_TSICR, E1000_TSICR_TXTS);
+			/* retrieve hardware timestamp */
+			schedule_work(&adapter->ptp_tx_work);
+		}
+	}
+#endif /* HAVE_PTP_1588_CLOCK */
+
 	napi_schedule(&q_vector->napi);
 
 	return IRQ_HANDLED;
 }
 
-static inline void igb_ring_irq_enable(struct igb_q_vector *q_vector)
+void igb_ring_irq_enable(struct igb_q_vector *q_vector)
 {
 	struct igb_adapter *adapter = q_vector->adapter;
 	struct e1000_hw *hw = &adapter->hw;
 
-	if ((q_vector->rx_ring && (adapter->rx_itr_setting & 3)) ||
-	    (!q_vector->rx_ring && (adapter->tx_itr_setting & 3))) {
-		if (!adapter->msix_entries)
-			igb_set_itr(adapter);
+	if ((q_vector->rx.ring && (adapter->rx_itr_setting & 3)) ||
+	    (!q_vector->rx.ring && (adapter->tx_itr_setting & 3))) {
+		if ((adapter->num_q_vectors == 1) && !adapter->vf_data)
+			igb_set_itr(q_vector);
 		else
 			igb_update_ring_itr(q_vector);
 	}
 
 	if (!test_bit(__IGB_DOWN, &adapter->state)) {
 		if (adapter->msix_entries)
-			wr32(E1000_EIMS, q_vector->eims_value);
+			E1000_WRITE_REG(hw, E1000_EIMS, q_vector->eims_value);
 		else
 			igb_irq_enable(adapter);
 	}
@@ -5522,166 +7003,170 @@ static inline void igb_ring_irq_enable(struct igb_q_vector *q_vector)
  **/
 static int igb_poll(struct napi_struct *napi, int budget)
 {
-	struct igb_q_vector *q_vector = container_of(napi,
-	                                             struct igb_q_vector,
-	                                             napi);
-	int tx_clean_complete = 1, work_done = 0;
+	struct igb_q_vector *q_vector = container_of(napi, struct igb_q_vector, napi);
+	bool clean_complete = true;
 
-#ifdef CONFIG_IGB_DCA
+#ifdef IGB_DCA
 	if (q_vector->adapter->flags & IGB_FLAG_DCA_ENABLED)
 		igb_update_dca(q_vector);
 #endif
-	if (q_vector->tx_ring)
-		tx_clean_complete = igb_clean_tx_irq(q_vector);
+	if (q_vector->tx.ring)
+		clean_complete = igb_clean_tx_irq(q_vector);
 
-	if (q_vector->rx_ring)
-		igb_clean_rx_irq_adv(q_vector, &work_done, budget);
+	if (q_vector->rx.ring)
+		clean_complete &= igb_clean_rx_irq(q_vector, budget);
 
-	if (!tx_clean_complete)
-		work_done = budget;
+#ifndef HAVE_NETDEV_NAPI_LIST
+	/* if netdev is disabled we need to stop polling */
+	if (!netif_running(q_vector->adapter->netdev))
+		clean_complete = true;
+
+#endif
+	/* If all work not completed, return budget and keep polling */
+	if (!clean_complete)
+		return budget;
 
 	/* If not enough Rx work done, exit the polling mode */
-	if (work_done < budget) {
-		napi_complete(napi);
-		igb_ring_irq_enable(q_vector);
-	}
+	napi_complete(napi);
+	igb_ring_irq_enable(q_vector);
 
-	return work_done;
+	return 0;
 }
 
 /**
- * igb_systim_to_hwtstamp - convert system time value to hw timestamp
- * @adapter: board private structure
- * @shhwtstamps: timestamp structure to update
- * @regval: unsigned 64bit system time value.
- *
- * We need to convert the system time value stored in the RX/TXSTMP registers
- * into a hwtstamp which can be used by the upper level timestamping functions
- */
-static void igb_systim_to_hwtstamp(struct igb_adapter *adapter,
-                                   struct skb_shared_hwtstamps *shhwtstamps,
-                                   u64 regval)
+ * igb_clean_tx_irq - Reclaim resources after transmit completes
+ * @q_vector: pointer to q_vector containing needed info
+ * returns TRUE if ring is completely cleaned
+ **/
+static bool igb_clean_tx_irq(struct igb_q_vector *q_vector)
 {
-	u64 ns;
+	struct igb_adapter *adapter = q_vector->adapter;
+	struct igb_ring *tx_ring = q_vector->tx.ring;
+	struct igb_tx_buffer *tx_buffer;
+	union e1000_adv_tx_desc *tx_desc;
+	unsigned int total_bytes = 0, total_packets = 0;
+	unsigned int budget = q_vector->tx.work_limit;
+	unsigned int i = tx_ring->next_to_clean;
 
-	/*
-	 * The 82580 starts with 1ns at bit 0 in RX/TXSTMPL, shift this up to
-	 * 24 to match clock shift we setup earlier.
-	 */
-	if (adapter->hw.mac.type == e1000_82580)
-		regval <<= IGB_82580_TSYNC_SHIFT;
+	if (test_bit(__IGB_DOWN, &adapter->state))
+		return true;
 
-	ns = timecounter_cyc2time(&adapter->clock, regval);
-	timecompare_update(&adapter->compare, ns);
-	memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
-	shhwtstamps->hwtstamp = ns_to_ktime(ns);
-	shhwtstamps->syststamp = timecompare_transform(&adapter->compare, ns);
-}
+	tx_buffer = &tx_ring->tx_buffer_info[i];
+	tx_desc = IGB_TX_DESC(tx_ring, i);
+	i -= tx_ring->count;
 
-/**
- * igb_tx_hwtstamp - utility function which checks for TX time stamp
- * @q_vector: pointer to q_vector containing needed info
- * @buffer: pointer to igb_buffer structure
- *
- * If we were asked to do hardware stamping and such a time stamp is
- * available, then it must have been for this skb here because we only
- * allow only one such packet into the queue.
- */
-static void igb_tx_hwtstamp(struct igb_q_vector *q_vector, struct igb_buffer *buffer_info)
-{
-	struct igb_adapter *adapter = q_vector->adapter;
-	struct e1000_hw *hw = &adapter->hw;
-	struct skb_shared_hwtstamps shhwtstamps;
-	u64 regval;
+	do {
+		union e1000_adv_tx_desc *eop_desc = tx_buffer->next_to_watch;
 
-	/* if skb does not support hw timestamp or TX stamp not valid exit */
-	if (likely(!(buffer_info->tx_flags & SKBTX_HW_TSTAMP)) ||
-	    !(rd32(E1000_TSYNCTXCTL) & E1000_TSYNCTXCTL_VALID))
-		return;
+		/* if next_to_watch is not set then there is no work pending */
+		if (!eop_desc)
+			break;
 
-	regval = rd32(E1000_TXSTMPL);
-	regval |= (u64)rd32(E1000_TXSTMPH) << 32;
+		/* prevent any other reads prior to eop_desc */
+		read_barrier_depends();
 
-	igb_systim_to_hwtstamp(adapter, &shhwtstamps, regval);
-	skb_tstamp_tx(buffer_info->skb, &shhwtstamps);
-}
+		/* if DD is not set pending work has not been completed */
+		if (!(eop_desc->wb.status & cpu_to_le32(E1000_TXD_STAT_DD)))
+			break;
 
-/**
- * igb_clean_tx_irq - Reclaim resources after transmit completes
- * @q_vector: pointer to q_vector containing needed info
- * returns true if ring is completely cleaned
- **/
-static bool igb_clean_tx_irq(struct igb_q_vector *q_vector)
-{
-	struct igb_adapter *adapter = q_vector->adapter;
-	struct igb_ring *tx_ring = q_vector->tx_ring;
-	struct net_device *netdev = tx_ring->netdev;
-	struct e1000_hw *hw = &adapter->hw;
-	struct igb_buffer *buffer_info;
-	union e1000_adv_tx_desc *tx_desc, *eop_desc;
-	unsigned int total_bytes = 0, total_packets = 0;
-	unsigned int i, eop, count = 0;
-	bool cleaned = false;
-
-	i = tx_ring->next_to_clean;
-	eop = tx_ring->buffer_info[i].next_to_watch;
-	eop_desc = E1000_TX_DESC_ADV(*tx_ring, eop);
-
-	while ((eop_desc->wb.status & cpu_to_le32(E1000_TXD_STAT_DD)) &&
-	       (count < tx_ring->count)) {
-		rmb();	/* read buffer_info after eop_desc status */
-		for (cleaned = false; !cleaned; count++) {
-			tx_desc = E1000_TX_DESC_ADV(*tx_ring, i);
-			buffer_info = &tx_ring->buffer_info[i];
-			cleaned = (i == eop);
-
-			if (buffer_info->skb) {
-				total_bytes += buffer_info->bytecount;
-				/* gso_segs is currently only valid for tcp */
-				total_packets += buffer_info->gso_segs;
-				igb_tx_hwtstamp(q_vector, buffer_info);
-			}
+		/* clear next_to_watch to prevent false hangs */
+		tx_buffer->next_to_watch = NULL;
+
+		/* update the statistics for this packet */
+		total_bytes += tx_buffer->bytecount;
+		total_packets += tx_buffer->gso_segs;
+
+		/* free the skb */
+		dev_kfree_skb_any(tx_buffer->skb);
 
-			igb_unmap_and_free_tx_resource(tx_ring, buffer_info);
-			tx_desc->wb.status = 0;
+		/* unmap skb header data */
+		dma_unmap_single(tx_ring->dev,
+		                 dma_unmap_addr(tx_buffer, dma),
+		                 dma_unmap_len(tx_buffer, len),
+		                 DMA_TO_DEVICE);
 
+		/* clear tx_buffer data */
+		tx_buffer->skb = NULL;
+		dma_unmap_len_set(tx_buffer, len, 0);
+
+		/* clear last DMA location and unmap remaining buffers */
+		while (tx_desc != eop_desc) {
+			tx_buffer++;
+			tx_desc++;
 			i++;
-			if (i == tx_ring->count)
-				i = 0;
+			if (unlikely(!i)) {
+				i -= tx_ring->count;
+				tx_buffer = tx_ring->tx_buffer_info;
+				tx_desc = IGB_TX_DESC(tx_ring, 0);
+			}
+
+			/* unmap any remaining paged data */
+			if (dma_unmap_len(tx_buffer, len)) {
+				dma_unmap_page(tx_ring->dev,
+				               dma_unmap_addr(tx_buffer, dma),
+				               dma_unmap_len(tx_buffer, len),
+				               DMA_TO_DEVICE);
+				dma_unmap_len_set(tx_buffer, len, 0);
+			}
 		}
-		eop = tx_ring->buffer_info[i].next_to_watch;
-		eop_desc = E1000_TX_DESC_ADV(*tx_ring, eop);
-	}
 
-	tx_ring->next_to_clean = i;
+		/* move us one more past the eop_desc for start of next pkt */
+		tx_buffer++;
+		tx_desc++;
+		i++;
+		if (unlikely(!i)) {
+			i -= tx_ring->count;
+			tx_buffer = tx_ring->tx_buffer_info;
+			tx_desc = IGB_TX_DESC(tx_ring, 0);
+		}
 
-	if (unlikely(count &&
-		     netif_carrier_ok(netdev) &&
-		     igb_desc_unused(tx_ring) >= IGB_TX_QUEUE_WAKE)) {
-		/* Make sure that anybody stopping the queue after this
-		 * sees the new next_to_clean.
-		 */
-		smp_mb();
-		if (__netif_subqueue_stopped(netdev, tx_ring->queue_index) &&
-		    !(test_bit(__IGB_DOWN, &adapter->state))) {
-			netif_wake_subqueue(netdev, tx_ring->queue_index);
+		/* issue prefetch for next Tx descriptor */
+		prefetch(tx_desc);
 
-			u64_stats_update_begin(&tx_ring->tx_syncp);
-			tx_ring->tx_stats.restart_queue++;
-			u64_stats_update_end(&tx_ring->tx_syncp);
-		}
-	}
+		/* update budget accounting */
+		budget--;
+	} while (likely(budget));
+
+	netdev_tx_completed_queue(txring_txq(tx_ring),
+				  total_packets, total_bytes);
+	
+	i += tx_ring->count;
+	tx_ring->next_to_clean = i;
+	tx_ring->tx_stats.bytes += total_bytes;
+	tx_ring->tx_stats.packets += total_packets;
+	q_vector->tx.total_bytes += total_bytes;
+	q_vector->tx.total_packets += total_packets;
+
+#ifdef DEBUG
+	if (test_bit(IGB_RING_FLAG_TX_DETECT_HANG, &tx_ring->flags) &&
+	    !(adapter->disable_hw_reset && adapter->tx_hang_detected)) {
+#else
+	if (test_bit(IGB_RING_FLAG_TX_DETECT_HANG, &tx_ring->flags)) {
+#endif
+		struct e1000_hw *hw = &adapter->hw;
 
-	if (tx_ring->detect_tx_hung) {
 		/* Detect a transmit hang in hardware, this serializes the
 		 * check with the clearing of time_stamp and movement of i */
-		tx_ring->detect_tx_hung = false;
-		if (tx_ring->buffer_info[i].time_stamp &&
-		    time_after(jiffies, tx_ring->buffer_info[i].time_stamp +
-			       (adapter->tx_timeout_factor * HZ)) &&
-		    !(rd32(E1000_STATUS) & E1000_STATUS_TXOFF)) {
+		clear_bit(IGB_RING_FLAG_TX_DETECT_HANG, &tx_ring->flags);
+		if (tx_buffer->next_to_watch &&
+		    time_after(jiffies, tx_buffer->time_stamp +
+		               (adapter->tx_timeout_factor * HZ))
+		    && !(E1000_READ_REG(hw, E1000_STATUS) &
+		         E1000_STATUS_TXOFF)) {
 
 			/* detected Tx unit hang */
+#ifdef DEBUG
+			adapter->tx_hang_detected = TRUE;
+			if (adapter->disable_hw_reset) {
+				DPRINTK(DRV, WARNING,
+					"Deactivating netdev watchdog timer\n");
+				if (del_timer(&netdev_ring(tx_ring)->watchdog_timer))
+					dev_put(netdev_ring(tx_ring));
+#ifndef HAVE_NET_DEVICE_OPS
+				netdev_ring(tx_ring)->tx_timeout = NULL;
+#endif
+			}
+#endif /* DEBUG */
 			dev_err(tx_ring->dev,
 				"Detected Tx Unit Hang\n"
 				"  Tx Queue             <%d>\n"
@@ -5691,357 +7176,1423 @@ static bool igb_clean_tx_irq(struct igb_q_vector *q_vector)
 				"  next_to_clean        <%x>\n"
 				"buffer_info[next_to_clean]\n"
 				"  time_stamp           <%lx>\n"
-				"  next_to_watch        <%x>\n"
+				"  next_to_watch        <%p>\n"
 				"  jiffies              <%lx>\n"
 				"  desc.status          <%x>\n",
 				tx_ring->queue_index,
-				readl(tx_ring->head),
+				E1000_READ_REG(hw, E1000_TDH(tx_ring->reg_idx)),
 				readl(tx_ring->tail),
 				tx_ring->next_to_use,
 				tx_ring->next_to_clean,
-				tx_ring->buffer_info[eop].time_stamp,
-				eop,
+				tx_buffer->time_stamp,
+				tx_buffer->next_to_watch,
 				jiffies,
-				eop_desc->wb.status);
-			netif_stop_subqueue(netdev, tx_ring->queue_index);
+				tx_buffer->next_to_watch->wb.status);
+			if (netif_is_multiqueue(netdev_ring(tx_ring)))
+				netif_stop_subqueue(netdev_ring(tx_ring),
+						    ring_queue_index(tx_ring));
+			else
+				netif_stop_queue(netdev_ring(tx_ring));
+
+			/* we are about to reset, no point in enabling stuff */
+			return true;
 		}
 	}
-	tx_ring->total_bytes += total_bytes;
-	tx_ring->total_packets += total_packets;
-	u64_stats_update_begin(&tx_ring->tx_syncp);
-	tx_ring->tx_stats.bytes += total_bytes;
-	tx_ring->tx_stats.packets += total_packets;
-	u64_stats_update_end(&tx_ring->tx_syncp);
-	return count < tx_ring->count;
+
+#define TX_WAKE_THRESHOLD (DESC_NEEDED * 2)
+	if (unlikely(total_packets &&
+		     netif_carrier_ok(netdev_ring(tx_ring)) &&
+		     igb_desc_unused(tx_ring) >= TX_WAKE_THRESHOLD)) {
+		/* Make sure that anybody stopping the queue after this
+		 * sees the new next_to_clean.
+		 */
+		smp_mb();
+		if (netif_is_multiqueue(netdev_ring(tx_ring))) {
+			if (__netif_subqueue_stopped(netdev_ring(tx_ring),
+						     ring_queue_index(tx_ring)) &&
+			    !(test_bit(__IGB_DOWN, &adapter->state))) {
+				netif_wake_subqueue(netdev_ring(tx_ring),
+						    ring_queue_index(tx_ring));
+				tx_ring->tx_stats.restart_queue++;
+			}
+		} else {
+			if (netif_queue_stopped(netdev_ring(tx_ring)) &&
+			    !(test_bit(__IGB_DOWN, &adapter->state))) {
+				netif_wake_queue(netdev_ring(tx_ring));
+				tx_ring->tx_stats.restart_queue++;
+			}
+		}
+	}
+
+	return !!budget;
 }
 
-static inline void igb_rx_checksum_adv(struct igb_ring *ring,
-				       u32 status_err, struct sk_buff *skb)
+#ifdef HAVE_VLAN_RX_REGISTER
+/**
+ * igb_receive_skb - helper function to handle rx indications
+ * @q_vector: structure containing interrupt and ring information
+ * @skb: packet to send up
+ **/
+static void igb_receive_skb(struct igb_q_vector *q_vector,
+                            struct sk_buff *skb)
+{
+	struct vlan_group **vlgrp = netdev_priv(skb->dev);
+
+	if (IGB_CB(skb)->vid) {
+		if (*vlgrp) {
+			vlan_gro_receive(&q_vector->napi, *vlgrp,
+					 IGB_CB(skb)->vid, skb);
+		} else {
+			dev_kfree_skb_any(skb);
+		}
+	} else {
+		napi_gro_receive(&q_vector->napi, skb);
+	}
+}
+
+#endif /* HAVE_VLAN_RX_REGISTER */
+#ifndef CONFIG_IGB_DISABLE_PACKET_SPLIT
+/**
+ * igb_reuse_rx_page - page flip buffer and store it back on the ring
+ * @rx_ring: rx descriptor ring to store buffers on
+ * @old_buff: donor buffer to have page reused
+ *
+ * Synchronizes page for reuse by the adapter
+ **/
+static void igb_reuse_rx_page(struct igb_ring *rx_ring,
+			      struct igb_rx_buffer *old_buff)
+{
+	struct igb_rx_buffer *new_buff;
+	u16 nta = rx_ring->next_to_alloc;
+
+	new_buff = &rx_ring->rx_buffer_info[nta];
+
+	/* update, and store next to alloc */
+	nta++;
+	rx_ring->next_to_alloc = (nta < rx_ring->count) ? nta : 0;
+
+	/* transfer page from old buffer to new buffer */
+	memcpy(new_buff, old_buff, sizeof(struct igb_rx_buffer));
+
+	/* sync the buffer for use by the device */
+	dma_sync_single_range_for_device(rx_ring->dev, old_buff->dma,
+					 old_buff->page_offset,
+					 IGB_RX_BUFSZ,
+					 DMA_FROM_DEVICE);
+}
+
+static bool igb_can_reuse_rx_page(struct igb_rx_buffer *rx_buffer,
+				  struct page *page,
+				  unsigned int truesize)
+{
+	/* avoid re-using remote pages */
+	if (unlikely(page_to_nid(page) != numa_node_id()))
+		return false;
+
+#if (PAGE_SIZE < 8192)
+	/* if we are only owner of page we can reuse it */
+	if (unlikely(page_count(page) != 1))
+		return false;
+
+	/* flip page offset to other buffer */
+	rx_buffer->page_offset ^= IGB_RX_BUFSZ;
+
+#else
+	/* move offset up to the next cache line */
+	rx_buffer->page_offset += truesize;
+
+	if (rx_buffer->page_offset > (PAGE_SIZE - IGB_RX_BUFSZ))
+		return false;
+#endif
+
+	/* bump ref count on page before it is given to the stack */
+	get_page(page);
+
+	return true;
+}
+
+/**
+ * igb_add_rx_frag - Add contents of Rx buffer to sk_buff
+ * @rx_ring: rx descriptor ring to transact packets on
+ * @rx_buffer: buffer containing page to add
+ * @rx_desc: descriptor containing length of buffer written by hardware
+ * @skb: sk_buff to place the data into
+ *
+ * This function will add the data contained in rx_buffer->page to the skb.
+ * This is done either through a direct copy if the data in the buffer is
+ * less than the skb header size, otherwise it will just attach the page as
+ * a frag to the skb.
+ *
+ * The function will then update the page offset if necessary and return
+ * true if the buffer can be reused by the adapter.
+ **/
+static bool igb_add_rx_frag(struct igb_ring *rx_ring,
+			    struct igb_rx_buffer *rx_buffer,
+			    union e1000_adv_rx_desc *rx_desc,
+			    struct sk_buff *skb)
+{
+	struct page *page = rx_buffer->page;
+	unsigned int size = le16_to_cpu(rx_desc->wb.upper.length);
+#if (PAGE_SIZE < 8192)
+	unsigned int truesize = IGB_RX_BUFSZ;
+#else
+	unsigned int truesize = ALIGN(size, L1_CACHE_BYTES);
+#endif
+
+	if ((size <= IGB_RX_HDR_LEN) && !skb_is_nonlinear(skb)) {
+		unsigned char *va = page_address(page) + rx_buffer->page_offset;
+
+#ifdef HAVE_PTP_1588_CLOCK
+		if (igb_test_staterr(rx_desc, E1000_RXDADV_STAT_TSIP)) {
+			igb_ptp_rx_pktstamp(rx_ring->q_vector, va, skb);
+			va += IGB_TS_HDR_LEN;
+			size -= IGB_TS_HDR_LEN;
+		}
+#endif /* HAVE_PTP_1588_CLOCK */
+
+		memcpy(__skb_put(skb, size), va, ALIGN(size, sizeof(long)));
+
+		/* we can reuse buffer as-is, just make sure it is local */
+		if (likely(page_to_nid(page) == numa_node_id()))
+			return true;
+
+		/* this page cannot be reused so discard it */
+		put_page(page);
+		return false;
+	}
+
+	skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags, page,
+			rx_buffer->page_offset, size, truesize);
+
+	return igb_can_reuse_rx_page(rx_buffer, page, truesize);
+}
+
+static struct sk_buff *igb_fetch_rx_buffer(struct igb_ring *rx_ring,
+					   union e1000_adv_rx_desc *rx_desc,
+					   struct sk_buff *skb)
+{
+	struct igb_rx_buffer *rx_buffer;
+	struct page *page;
+
+	rx_buffer = &rx_ring->rx_buffer_info[rx_ring->next_to_clean];
+
+	page = rx_buffer->page;
+	prefetchw(page);
+
+	if (likely(!skb)) {
+		void *page_addr = page_address(page) +
+				  rx_buffer->page_offset;
+
+		/* prefetch first cache line of first page */
+		prefetch(page_addr);
+#if L1_CACHE_BYTES < 128
+		prefetch(page_addr + L1_CACHE_BYTES);
+#endif
+
+		/* allocate a skb to store the frags */
+		skb = netdev_alloc_skb_ip_align(rx_ring->netdev,
+						IGB_RX_HDR_LEN);
+		if (unlikely(!skb)) {
+			rx_ring->rx_stats.alloc_failed++;
+			return NULL;
+		}
+
+		/*
+		 * we will be copying header into skb->data in
+		 * pskb_may_pull so it is in our interest to prefetch
+		 * it now to avoid a possible cache miss
+		 */
+		prefetchw(skb->data);
+	}
+
+	/* we are reusing so sync this buffer for CPU use */
+	dma_sync_single_range_for_cpu(rx_ring->dev,
+				      rx_buffer->dma,
+				      rx_buffer->page_offset,
+				      IGB_RX_BUFSZ,
+				      DMA_FROM_DEVICE);
+
+	/* pull page into skb */
+	if (igb_add_rx_frag(rx_ring, rx_buffer, rx_desc, skb)) {
+		/* hand second half of page back to the ring */
+		igb_reuse_rx_page(rx_ring, rx_buffer);
+	} else {
+		/* we are not reusing the buffer so unmap it */
+		dma_unmap_page(rx_ring->dev, rx_buffer->dma,
+			       PAGE_SIZE, DMA_FROM_DEVICE);
+	}
+
+	/* clear contents of rx_buffer */
+	rx_buffer->page = NULL;
+
+	return skb;
+}
+
+#endif
+static inline void igb_rx_checksum(struct igb_ring *ring,
+				   union e1000_adv_rx_desc *rx_desc,
+				   struct sk_buff *skb)
 {
 	skb_checksum_none_assert(skb);
 
-	/* Ignore Checksum bit is set or checksum is disabled through ethtool */
-	if (!(ring->flags & IGB_RING_FLAG_RX_CSUM) ||
-	     (status_err & E1000_RXD_STAT_IXSM))
+	/* Ignore Checksum bit is set */
+	if (igb_test_staterr(rx_desc, E1000_RXD_STAT_IXSM))
+		return;
+
+	/* Rx checksum disabled via ethtool */
+	if (!(netdev_ring(ring)->features & NETIF_F_RXCSUM))
 		return;
 
 	/* TCP/UDP checksum error bit is set */
-	if (status_err &
-	    (E1000_RXDEXT_STATERR_TCPE | E1000_RXDEXT_STATERR_IPE)) {
+	if (igb_test_staterr(rx_desc,
+			     E1000_RXDEXT_STATERR_TCPE |
+			     E1000_RXDEXT_STATERR_IPE)) {
 		/*
 		 * work around errata with sctp packets where the TCPE aka
 		 * L4E bit is set incorrectly on 64 byte (60 byte w/o crc)
 		 * packets, (aka let the stack check the crc32c)
 		 */
-		if ((skb->len == 60) &&
-		    (ring->flags & IGB_RING_FLAG_RX_SCTP_CSUM)) {
-			u64_stats_update_begin(&ring->rx_syncp);
+		if (!((skb->len == 60) &&
+		      test_bit(IGB_RING_FLAG_RX_SCTP_CSUM, &ring->flags)))
 			ring->rx_stats.csum_err++;
-			u64_stats_update_end(&ring->rx_syncp);
-		}
+
 		/* let the stack verify checksum errors */
 		return;
 	}
 	/* It must be a TCP or UDP packet with a valid checksum */
-	if (status_err & (E1000_RXD_STAT_TCPCS | E1000_RXD_STAT_UDPCS))
+	if (igb_test_staterr(rx_desc, E1000_RXD_STAT_TCPCS |
+				      E1000_RXD_STAT_UDPCS))
 		skb->ip_summed = CHECKSUM_UNNECESSARY;
+}
 
-	dev_dbg(ring->dev, "cksum success: bits %08X\n", status_err);
+#ifdef NETIF_F_RXHASH
+static inline void igb_rx_hash(struct igb_ring *ring,
+			       union e1000_adv_rx_desc *rx_desc,
+			       struct sk_buff *skb)
+{
+	if (netdev_ring(ring)->features & NETIF_F_RXHASH)
+		skb->rxhash = le32_to_cpu(rx_desc->wb.lower.hi_dword.rss);
 }
 
-static void igb_rx_hwtstamp(struct igb_q_vector *q_vector, u32 staterr,
-                                   struct sk_buff *skb)
+#endif
+#ifndef IGB_NO_LRO
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+/**
+ * igb_merge_active_tail - merge active tail into lro skb
+ * @tail: pointer to active tail in frag_list
+ *
+ * This function merges the length and data of an active tail into the
+ * skb containing the frag_list.  It resets the tail's pointer to the head,
+ * but it leaves the heads pointer to tail intact.
+ **/
+static inline struct sk_buff *igb_merge_active_tail(struct sk_buff *tail)
 {
-	struct igb_adapter *adapter = q_vector->adapter;
-	struct e1000_hw *hw = &adapter->hw;
-	u64 regval;
+	struct sk_buff *head = IGB_CB(tail)->head;
 
-	/*
-	 * If this bit is set, then the RX registers contain the time stamp. No
-	 * other packet will be time stamped until we read these registers, so
-	 * read the registers to make them available again. Because only one
-	 * packet can be time stamped at a time, we know that the register
-	 * values must belong to this one here and therefore we don't need to
-	 * compare any of the additional attributes stored for it.
-	 *
-	 * If nothing went wrong, then it should have a shared tx_flags that we
-	 * can turn into a skb_shared_hwtstamps.
-	 */
-	if (staterr & E1000_RXDADV_STAT_TSIP) {
-		u32 *stamp = (u32 *)skb->data;
-		regval = le32_to_cpu(*(stamp + 2));
-		regval |= (u64)le32_to_cpu(*(stamp + 3)) << 32;
-		skb_pull(skb, IGB_TS_HDR_LEN);
+	if (!head)
+		return tail;
+
+	head->len += tail->len;
+	head->data_len += tail->len;
+	head->truesize += tail->len;
+
+	IGB_CB(tail)->head = NULL;
+
+	return head;
+}
+
+/**
+ * igb_add_active_tail - adds an active tail into the skb frag_list
+ * @head: pointer to the start of the skb
+ * @tail: pointer to active tail to add to frag_list
+ *
+ * This function adds an active tail to the end of the frag list.  This tail
+ * will still be receiving data so we cannot yet ad it's stats to the main
+ * skb.  That is done via igb_merge_active_tail.
+ **/
+static inline void igb_add_active_tail(struct sk_buff *head, struct sk_buff *tail)
+{
+	struct sk_buff *old_tail = IGB_CB(head)->tail;
+
+	if (old_tail) {
+		igb_merge_active_tail(old_tail);
+		old_tail->next = tail;
 	} else {
-		if(!(rd32(E1000_TSYNCRXCTL) & E1000_TSYNCRXCTL_VALID))
-			return;
+		skb_shinfo(head)->frag_list = tail;
+	}
+
+	IGB_CB(tail)->head = head;
+	IGB_CB(head)->tail = tail;
+
+	IGB_CB(head)->append_cnt++;
+}
+
+/**
+ * igb_close_active_frag_list - cleanup pointers on a frag_list skb
+ * @head: pointer to head of an active frag list
+ *
+ * This function will clear the frag_tail_tracker pointer on an active
+ * frag_list and returns true if the pointer was actually set
+ **/
+static inline bool igb_close_active_frag_list(struct sk_buff *head)
+{
+	struct sk_buff *tail = IGB_CB(head)->tail;
+
+	if (!tail)
+		return false;
+
+	igb_merge_active_tail(tail);
+
+	IGB_CB(head)->tail = NULL;
 
-		regval = rd32(E1000_RXSTMPL);
-		regval |= (u64)rd32(E1000_RXSTMPH) << 32;
+	return true;
+}
+
+#endif /* CONFIG_IGB_DISABLE_PACKET_SPLIT */
+/**
+ * igb_can_lro - returns true if packet is TCP/IPV4 and LRO is enabled
+ * @adapter: board private structure
+ * @rx_desc: pointer to the rx descriptor
+ * @skb: pointer to the skb to be merged
+ *
+ **/
+static inline bool igb_can_lro(struct igb_ring *rx_ring,
+			       union e1000_adv_rx_desc *rx_desc,
+			       struct sk_buff *skb)
+{
+	struct iphdr *iph = (struct iphdr *)skb->data;
+	__le16 pkt_info = rx_desc->wb.lower.lo_dword.hs_rss.pkt_info;
+
+	/* verify hardware indicates this is IPv4/TCP */
+	if((!(pkt_info & cpu_to_le16(E1000_RXDADV_PKTTYPE_TCP)) ||
+	    !(pkt_info & cpu_to_le16(E1000_RXDADV_PKTTYPE_IPV4))))
+		return false;
+
+	/* .. and LRO is enabled */
+	if (!(netdev_ring(rx_ring)->features & NETIF_F_LRO))
+		return false;
+
+	/* .. and we are not in promiscuous mode */
+	if (netdev_ring(rx_ring)->flags & IFF_PROMISC)
+		return false;
+
+	/* .. and the header is large enough for us to read IP/TCP fields */
+	if (!pskb_may_pull(skb, sizeof(struct igb_lrohdr)))
+		return false;
+
+	/* .. and there are no VLANs on packet */
+	if (skb->protocol != __constant_htons(ETH_P_IP))
+		return false;
+
+	/* .. and we are version 4 with no options */
+	if (*(u8 *)iph != 0x45)
+		return false;
+
+	/* .. and the packet is not fragmented */
+	if (iph->frag_off & htons(IP_MF | IP_OFFSET))
+		return false;
+
+	/* .. and that next header is TCP */
+	if (iph->protocol != IPPROTO_TCP)
+		return false;
+
+	return true;
+}
+
+static inline struct igb_lrohdr *igb_lro_hdr(struct sk_buff *skb)
+{
+	return (struct igb_lrohdr *)skb->data;
+}
+
+/**
+ * igb_lro_flush - Indicate packets to upper layer.
+ *
+ * Update IP and TCP header part of head skb if more than one
+ * skb's chained and indicate packets to upper layer.
+ **/
+static void igb_lro_flush(struct igb_q_vector *q_vector,
+			  struct sk_buff *skb)
+{
+	struct igb_lro_list *lrolist = &q_vector->lrolist;
+
+	__skb_unlink(skb, &lrolist->active);
+
+	if (IGB_CB(skb)->append_cnt) {
+		struct igb_lrohdr *lroh = igb_lro_hdr(skb);
+
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+		/* close any active lro contexts */
+		igb_close_active_frag_list(skb);
+
+#endif
+		/* incorporate ip header and re-calculate checksum */
+		lroh->iph.tot_len = ntohs(skb->len);
+		lroh->iph.check = 0;
+
+		/* header length is 5 since we know no options exist */
+		lroh->iph.check = ip_fast_csum((u8 *)lroh, 5);
+
+		/* clear TCP checksum to indicate we are an LRO frame */
+		lroh->th.check = 0;
+
+		/* incorporate latest timestamp into the tcp header */
+		if (IGB_CB(skb)->tsecr) {
+			lroh->ts[2] = IGB_CB(skb)->tsecr;
+			lroh->ts[1] = htonl(IGB_CB(skb)->tsval);
+		}
+#ifdef NETIF_F_GSO
+
+		skb_shinfo(skb)->gso_size = IGB_CB(skb)->mss;
+		skb_shinfo(skb)->gso_type = SKB_GSO_TCPV4;
+#endif
 	}
 
-	igb_systim_to_hwtstamp(adapter, skb_hwtstamps(skb), regval);
+#ifdef HAVE_VLAN_RX_REGISTER
+	igb_receive_skb(q_vector, skb);
+#else
+	napi_gro_receive(&q_vector->napi, skb);
+#endif
+	lrolist->stats.flushed++;
+}
+
+static void igb_lro_flush_all(struct igb_q_vector *q_vector)
+{
+	struct igb_lro_list *lrolist = &q_vector->lrolist;
+	struct sk_buff *skb, *tmp;
+
+	skb_queue_reverse_walk_safe(&lrolist->active, skb, tmp)
+		igb_lro_flush(q_vector, skb);
 }
-static inline u16 igb_get_hlen(struct igb_ring *rx_ring,
-                               union e1000_adv_rx_desc *rx_desc)
+
+/*
+ * igb_lro_header_ok - Main LRO function.
+ **/
+static void igb_lro_header_ok(struct sk_buff *skb)
 {
-	/* HW will not DMA in data larger than the given buffer, even if it
-	 * parses the (NFS, of course) header to be larger.  In that case, it
-	 * fills the header buffer and spills the rest into the page.
+	struct igb_lrohdr *lroh = igb_lro_hdr(skb);
+	u16 opt_bytes, data_len;
+
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+	IGB_CB(skb)->tail = NULL;
+#endif
+	IGB_CB(skb)->tsecr = 0;
+	IGB_CB(skb)->append_cnt = 0;
+	IGB_CB(skb)->mss = 0;
+
+	/* ensure that the checksum is valid */
+	if (skb->ip_summed != CHECKSUM_UNNECESSARY)
+		return;
+
+	/* If we see CE codepoint in IP header, packet is not mergeable */
+	if (INET_ECN_is_ce(ipv4_get_dsfield(&lroh->iph)))
+		return;
+
+	/* ensure no bits set besides ack or psh */
+	if (lroh->th.fin || lroh->th.syn || lroh->th.rst ||
+	    lroh->th.urg || lroh->th.ece || lroh->th.cwr ||
+	    !lroh->th.ack)
+		return;
+
+	/* store the total packet length */
+	data_len = ntohs(lroh->iph.tot_len);
+
+	/* remove any padding from the end of the skb */
+	__pskb_trim(skb, data_len);
+
+	/* remove header length from data length */
+	data_len -= sizeof(struct igb_lrohdr);
+
+	/*
+	 * check for timestamps. Since the only option we handle are timestamps,
+	 * we only have to handle the simple case of aligned timestamps
 	 */
-	u16 hlen = (le16_to_cpu(rx_desc->wb.lower.lo_dword.hdr_info) &
-	           E1000_RXDADV_HDRBUFLEN_MASK) >> E1000_RXDADV_HDRBUFLEN_SHIFT;
-	if (hlen > rx_ring->rx_buffer_len)
-		hlen = rx_ring->rx_buffer_len;
-	return hlen;
+	opt_bytes = (lroh->th.doff << 2) - sizeof(struct tcphdr);
+	if (opt_bytes != 0) {
+		if ((opt_bytes != TCPOLEN_TSTAMP_ALIGNED) ||
+		    !pskb_may_pull(skb, sizeof(struct igb_lrohdr) +
+					TCPOLEN_TSTAMP_ALIGNED) ||
+		    (lroh->ts[0] != htonl((TCPOPT_NOP << 24) |
+					     (TCPOPT_NOP << 16) |
+					     (TCPOPT_TIMESTAMP << 8) |
+					      TCPOLEN_TIMESTAMP)) ||
+		    (lroh->ts[2] == 0)) {
+			return;
+		}
+
+		IGB_CB(skb)->tsval = ntohl(lroh->ts[1]);
+		IGB_CB(skb)->tsecr = lroh->ts[2];
+
+		data_len -= TCPOLEN_TSTAMP_ALIGNED;
+	}
+
+	/* record data_len as mss for the packet */
+	IGB_CB(skb)->mss = data_len;
+	IGB_CB(skb)->next_seq = ntohl(lroh->th.seq);
 }
 
-static bool igb_clean_rx_irq_adv(struct igb_q_vector *q_vector,
-                                 int *work_done, int budget)
+#ifndef CONFIG_IGB_DISABLE_PACKET_SPLIT
+static void igb_merge_frags(struct sk_buff *lro_skb, struct sk_buff *new_skb)
 {
-	struct igb_ring *rx_ring = q_vector->rx_ring;
-	struct net_device *netdev = rx_ring->netdev;
-	struct device *dev = rx_ring->dev;
-	union e1000_adv_rx_desc *rx_desc , *next_rxd;
-	struct igb_buffer *buffer_info , *next_buffer;
-	struct sk_buff *skb;
-	bool cleaned = false;
-	int cleaned_count = 0;
-	int current_node = numa_node_id();
-	unsigned int total_bytes = 0, total_packets = 0;
-	unsigned int i;
-	u32 staterr;
-	u16 length;
+	struct skb_shared_info *sh_info;
+	struct skb_shared_info *new_skb_info;
+	unsigned int data_len;
 
-	i = rx_ring->next_to_clean;
-	buffer_info = &rx_ring->buffer_info[i];
-	rx_desc = E1000_RX_DESC_ADV(*rx_ring, i);
-	staterr = le32_to_cpu(rx_desc->wb.upper.status_error);
+	sh_info = skb_shinfo(lro_skb);
+	new_skb_info = skb_shinfo(new_skb);
 
-	while (staterr & E1000_RXD_STAT_DD) {
-		if (*work_done >= budget)
-			break;
-		(*work_done)++;
-		rmb(); /* read descriptor and rx_buffer_info after status DD */
+	/* copy frags into the last skb */
+	memcpy(sh_info->frags + sh_info->nr_frags,
+	       new_skb_info->frags,
+	       new_skb_info->nr_frags * sizeof(skb_frag_t));
 
-		skb = buffer_info->skb;
-		prefetch(skb->data - NET_IP_ALIGN);
-		buffer_info->skb = NULL;
+	/* copy size data over */
+	sh_info->nr_frags += new_skb_info->nr_frags;
+	data_len = IGB_CB(new_skb)->mss;
+	lro_skb->len += data_len;
+	lro_skb->data_len += data_len;
+	lro_skb->truesize += data_len;
 
-		i++;
-		if (i == rx_ring->count)
-			i = 0;
+	/* wipe record of data from new_skb */
+	new_skb_info->nr_frags = 0;
+	new_skb->len = new_skb->data_len = 0;
+	dev_kfree_skb_any(new_skb);
+}
 
-		next_rxd = E1000_RX_DESC_ADV(*rx_ring, i);
-		prefetch(next_rxd);
-		next_buffer = &rx_ring->buffer_info[i];
+#endif /* CONFIG_IGB_DISABLE_PACKET_SPLIT */
+/**
+ * igb_lro_receive - if able, queue skb into lro chain
+ * @q_vector: structure containing interrupt and ring information
+ * @new_skb: pointer to current skb being checked
+ *
+ * Checks whether the skb given is eligible for LRO and if that's
+ * fine chains it to the existing lro_skb based on flowid. If an LRO for
+ * the flow doesn't exist create one.
+ **/
+static void igb_lro_receive(struct igb_q_vector *q_vector,
+			    struct sk_buff *new_skb)
+{
+	struct sk_buff *lro_skb;
+	struct igb_lro_list *lrolist = &q_vector->lrolist;
+	struct igb_lrohdr *lroh = igb_lro_hdr(new_skb);
+	__be32 saddr = lroh->iph.saddr;
+	__be32 daddr = lroh->iph.daddr;
+	__be32 tcp_ports = *(__be32 *)&lroh->th;
+	u16 data_len;
+#ifdef HAVE_VLAN_RX_REGISTER
+	u16 vid = IGB_CB(new_skb)->vid;
+#else
+	u16 vid = new_skb->vlan_tci;
+#endif
 
-		length = le16_to_cpu(rx_desc->wb.upper.length);
-		cleaned = true;
-		cleaned_count++;
+	igb_lro_header_ok(new_skb);
 
-		if (buffer_info->dma) {
-			dma_unmap_single(dev, buffer_info->dma,
-					 rx_ring->rx_buffer_len,
-					 DMA_FROM_DEVICE);
-			buffer_info->dma = 0;
-			if (rx_ring->rx_buffer_len >= IGB_RXBUFFER_1024) {
-				skb_put(skb, length);
-				goto send_up;
-			}
-			skb_put(skb, igb_get_hlen(rx_ring, rx_desc));
+	/*
+	 * we have a packet that might be eligible for LRO,
+	 * so see if it matches anything we might expect
+	 */
+	skb_queue_walk(&lrolist->active, lro_skb) {
+		if (*(__be32 *)&igb_lro_hdr(lro_skb)->th != tcp_ports ||
+		    igb_lro_hdr(lro_skb)->iph.saddr != saddr ||
+		    igb_lro_hdr(lro_skb)->iph.daddr != daddr)
+			continue;
+
+#ifdef HAVE_VLAN_RX_REGISTER
+		if (IGB_CB(lro_skb)->vid != vid)
+#else
+		if (lro_skb->vlan_tci != vid)
+#endif
+			continue;
+
+		/* out of order packet */
+		if (IGB_CB(lro_skb)->next_seq != IGB_CB(new_skb)->next_seq) {
+			igb_lro_flush(q_vector, lro_skb);
+			IGB_CB(new_skb)->mss = 0;
+			break;
 		}
 
-		if (length) {
-			dma_unmap_page(dev, buffer_info->page_dma,
-				       PAGE_SIZE / 2, DMA_FROM_DEVICE);
-			buffer_info->page_dma = 0;
+		/* TCP timestamp options have changed */
+		if (!IGB_CB(lro_skb)->tsecr != !IGB_CB(new_skb)->tsecr) {
+			igb_lro_flush(q_vector, lro_skb);
+			break;
+		}
 
-			skb_fill_page_desc(skb, skb_shinfo(skb)->nr_frags,
-						buffer_info->page,
-						buffer_info->page_offset,
-						length);
+		/* make sure timestamp values are increasing */
+		if (IGB_CB(lro_skb)->tsecr &&
+		    IGB_CB(lro_skb)->tsval > IGB_CB(new_skb)->tsval) {
+			igb_lro_flush(q_vector, lro_skb);
+			IGB_CB(new_skb)->mss = 0;
+			break;
+		}
 
-			if ((page_count(buffer_info->page) != 1) ||
-			    (page_to_nid(buffer_info->page) != current_node))
-				buffer_info->page = NULL;
-			else
-				get_page(buffer_info->page);
+		data_len = IGB_CB(new_skb)->mss;
+
+		/* Check for all of the above below
+		 *   malformed header
+		 *   no tcp data
+		 *   resultant packet would be too large
+		 *   new skb is larger than our current mss
+		 *   data would remain in header
+		 *   we would consume more frags then the sk_buff contains
+		 *   ack sequence numbers changed
+		 *   window size has changed
+		 */
+		if (data_len == 0 ||
+		    data_len > IGB_CB(lro_skb)->mss ||
+		    data_len > IGB_CB(lro_skb)->free ||
+#ifndef CONFIG_IGB_DISABLE_PACKET_SPLIT
+		    data_len != new_skb->data_len ||
+		    skb_shinfo(new_skb)->nr_frags >=
+		    (MAX_SKB_FRAGS - skb_shinfo(lro_skb)->nr_frags) ||
+#endif
+		    igb_lro_hdr(lro_skb)->th.ack_seq != lroh->th.ack_seq ||
+		    igb_lro_hdr(lro_skb)->th.window != lroh->th.window) {
+			igb_lro_flush(q_vector, lro_skb);
+			break;
+		}
 
-			skb->len += length;
-			skb->data_len += length;
-			skb->truesize += length;
+		/* Remove IP and TCP header*/
+		skb_pull(new_skb, new_skb->len - data_len);
+
+		/* update timestamp and timestamp echo response */
+		IGB_CB(lro_skb)->tsval = IGB_CB(new_skb)->tsval;
+		IGB_CB(lro_skb)->tsecr = IGB_CB(new_skb)->tsecr;
+
+		/* update sequence and free space */
+		IGB_CB(lro_skb)->next_seq += data_len;
+		IGB_CB(lro_skb)->free -= data_len;
+
+		/* update append_cnt */
+		IGB_CB(lro_skb)->append_cnt++;
+
+#ifndef CONFIG_IGB_DISABLE_PACKET_SPLIT
+		/* if header is empty pull pages into current skb */
+		igb_merge_frags(lro_skb, new_skb);
+#else
+		/* chain this new skb in frag_list */
+		igb_add_active_tail(lro_skb, new_skb);
+#endif
+
+		if ((data_len < IGB_CB(lro_skb)->mss) || lroh->th.psh ||
+		    skb_shinfo(lro_skb)->nr_frags == MAX_SKB_FRAGS) {
+			igb_lro_hdr(lro_skb)->th.psh |= lroh->th.psh;
+			igb_lro_flush(q_vector, lro_skb);
+		}
+
+		lrolist->stats.coal++;
+		return;
+	}
+
+	if (IGB_CB(new_skb)->mss && !lroh->th.psh) {
+		/* if we are at capacity flush the tail */
+		if (skb_queue_len(&lrolist->active) >= IGB_LRO_MAX) {
+			lro_skb = skb_peek_tail(&lrolist->active);
+			if (lro_skb)
+				igb_lro_flush(q_vector, lro_skb);
 		}
 
-		if (!(staterr & E1000_RXD_STAT_EOP)) {
-			buffer_info->skb = next_buffer->skb;
-			buffer_info->dma = next_buffer->dma;
-			next_buffer->skb = skb;
-			next_buffer->dma = 0;
-			goto next_desc;
+		/* update sequence and free space */
+		IGB_CB(new_skb)->next_seq += IGB_CB(new_skb)->mss;
+		IGB_CB(new_skb)->free = 65521 - new_skb->len;
+
+		/* .. and insert at the front of the active list */
+		__skb_queue_head(&lrolist->active, new_skb);
+
+		lrolist->stats.coal++;
+		return;
+	}
+
+	/* packet not handled by any of the above, pass it to the stack */
+#ifdef HAVE_VLAN_RX_REGISTER
+	igb_receive_skb(q_vector, new_skb);
+#else
+	napi_gro_receive(&q_vector->napi, new_skb);
+#endif
+}
+
+#endif /* IGB_NO_LRO */
+/**
+ * igb_process_skb_fields - Populate skb header fields from Rx descriptor
+ * @rx_ring: rx descriptor ring packet is being transacted on
+ * @rx_desc: pointer to the EOP Rx descriptor
+ * @skb: pointer to current skb being populated
+ *
+ * This function checks the ring, descriptor, and packet information in
+ * order to populate the hash, checksum, VLAN, timestamp, protocol, and
+ * other fields within the skb.
+ **/
+static void igb_process_skb_fields(struct igb_ring *rx_ring,
+				   union e1000_adv_rx_desc *rx_desc,
+				   struct sk_buff *skb)
+{
+	struct net_device *dev = rx_ring->netdev;
+	__le16 pkt_info = rx_desc->wb.lower.lo_dword.hs_rss.pkt_info;
+	bool notype;
+
+#ifdef NETIF_F_RXHASH
+	igb_rx_hash(rx_ring, rx_desc, skb);
+
+#endif
+	igb_rx_checksum(rx_ring, rx_desc, skb);
+
+	/* update packet type stats */
+	switch (pkt_info & E1000_RXDADV_PKTTYPE_ILMASK) {
+	case E1000_RXDADV_PKTTYPE_IPV4:
+		rx_ring->pkt_stats.ipv4_packets++;
+		break;
+	case E1000_RXDADV_PKTTYPE_IPV4_EX:
+		rx_ring->pkt_stats.ipv4e_packets++;
+		break;
+	case E1000_RXDADV_PKTTYPE_IPV6:
+		rx_ring->pkt_stats.ipv6_packets++;
+		break;
+	case E1000_RXDADV_PKTTYPE_IPV6_EX:
+		rx_ring->pkt_stats.ipv6e_packets++;
+		break;
+	default:
+		notype = true;
+		break;
+	}
+
+	switch (pkt_info & E1000_RXDADV_PKTTYPE_TLMASK) {
+	case E1000_RXDADV_PKTTYPE_TCP:
+		rx_ring->pkt_stats.tcp_packets++;
+		break;
+	case E1000_RXDADV_PKTTYPE_UDP:
+		rx_ring->pkt_stats.udp_packets++;
+		break;
+	case E1000_RXDADV_PKTTYPE_SCTP:
+		rx_ring->pkt_stats.sctp_packets++;
+		break;
+	case E1000_RXDADV_PKTTYPE_NFS:
+		rx_ring->pkt_stats.nfs_packets++;
+		break;
+	case E1000_RXDADV_PKTTYPE_NONE:
+		if (notype)
+			rx_ring->pkt_stats.other_packets++;
+		break;
+	default:
+		break;
+	}
+
+#ifdef HAVE_PTP_1588_CLOCK
+	igb_ptp_rx_hwtstamp(rx_ring, rx_desc, skb);
+#endif /* HAVE_PTP_1588_CLOCK */
+
+#ifdef NETIF_F_HW_VLAN_CTAG_RX
+	if ((dev->features & NETIF_F_HW_VLAN_CTAG_RX) &&
+#else
+	if ((dev->features & NETIF_F_HW_VLAN_RX) &&
+#endif
+	    igb_test_staterr(rx_desc, E1000_RXD_STAT_VP)) {
+		u16 vid = 0;
+		if (igb_test_staterr(rx_desc, E1000_RXDEXT_STATERR_LB) &&
+		    test_bit(IGB_RING_FLAG_RX_LB_VLAN_BSWAP, &rx_ring->flags))
+			vid = be16_to_cpu(rx_desc->wb.upper.vlan);
+		else
+			vid = le16_to_cpu(rx_desc->wb.upper.vlan);
+#ifdef HAVE_VLAN_RX_REGISTER
+		IGB_CB(skb)->vid = vid;
+	} else {
+		IGB_CB(skb)->vid = 0;
+#else
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
+#endif
+	}
+
+	skb_record_rx_queue(skb, rx_ring->queue_index);
+
+	skb->protocol = eth_type_trans(skb, dev);
+}
+
+/**
+ * igb_is_non_eop - process handling of non-EOP buffers
+ * @rx_ring: Rx ring being processed
+ * @rx_desc: Rx descriptor for current buffer
+ *
+ * This function updates next to clean.  If the buffer is an EOP buffer
+ * this function exits returning false, otherwise it will place the
+ * sk_buff in the next buffer to be chained and return true indicating
+ * that this is in fact a non-EOP buffer.
+ **/
+static bool igb_is_non_eop(struct igb_ring *rx_ring,
+			   union e1000_adv_rx_desc *rx_desc)
+{
+	u32 ntc = rx_ring->next_to_clean + 1;
+
+	/* fetch, update, and store next to clean */
+	ntc = (ntc < rx_ring->count) ? ntc : 0;
+	rx_ring->next_to_clean = ntc;
+
+	prefetch(IGB_RX_DESC(rx_ring, ntc));
+
+	if (likely(igb_test_staterr(rx_desc, E1000_RXD_STAT_EOP)))
+		return false;
+
+	return true;
+}
+
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+/* igb_clean_rx_irq -- * legacy */
+static bool igb_clean_rx_irq(struct igb_q_vector *q_vector, int budget)
+{
+	struct igb_ring *rx_ring = q_vector->rx.ring;
+	unsigned int total_bytes = 0, total_packets = 0;
+	u16 cleaned_count = igb_desc_unused(rx_ring);
+
+	do {
+		struct igb_rx_buffer *rx_buffer;
+		union e1000_adv_rx_desc *rx_desc;
+		struct sk_buff *skb;
+		u16 ntc;
+
+		/* return some buffers to hardware, one at a time is too slow */
+		if (cleaned_count >= IGB_RX_BUFFER_WRITE) {
+			igb_alloc_rx_buffers(rx_ring, cleaned_count);
+			cleaned_count = 0;
 		}
-send_up:
-		if (staterr & E1000_RXDEXT_ERR_FRAME_ERR_MASK) {
-			dev_kfree_skb_irq(skb);
-			goto next_desc;
+
+		ntc = rx_ring->next_to_clean;
+		rx_desc = IGB_RX_DESC(rx_ring, ntc);
+		rx_buffer = &rx_ring->rx_buffer_info[ntc];
+
+		if (!igb_test_staterr(rx_desc, E1000_RXD_STAT_DD))
+			break;
+
+		/*
+		 * This memory barrier is needed to keep us from reading
+		 * any other fields out of the rx_desc until we know the
+		 * RXD_STAT_DD bit is set
+		 */
+		rmb();
+
+		skb = rx_buffer->skb;
+
+		prefetch(skb->data);
+
+		/* pull the header of the skb in */
+		__skb_put(skb, le16_to_cpu(rx_desc->wb.upper.length));
+
+		/* clear skb reference in buffer info structure */
+		rx_buffer->skb = NULL;
+
+		cleaned_count++;
+
+		BUG_ON(igb_is_non_eop(rx_ring, rx_desc));
+
+		dma_unmap_single(rx_ring->dev, rx_buffer->dma,
+				 rx_ring->rx_buffer_len,
+				 DMA_FROM_DEVICE);
+		rx_buffer->dma = 0;
+
+		if (igb_test_staterr(rx_desc,
+				     E1000_RXDEXT_ERR_FRAME_ERR_MASK)) {
+			dev_kfree_skb_any(skb);
+			continue;
 		}
 
-		if (staterr & (E1000_RXDADV_STAT_TSIP | E1000_RXDADV_STAT_TS))
-			igb_rx_hwtstamp(q_vector, staterr, skb);
 		total_bytes += skb->len;
+
+		/* populate checksum, timestamp, VLAN, and protocol */
+		igb_process_skb_fields(rx_ring, rx_desc, skb);
+
+#ifndef IGB_NO_LRO
+		if (igb_can_lro(rx_ring, rx_desc, skb))
+			igb_lro_receive(q_vector, skb);
+		else
+#endif
+#ifdef HAVE_VLAN_RX_REGISTER
+			igb_receive_skb(q_vector, skb);
+#else
+			napi_gro_receive(&q_vector->napi, skb);
+#endif
+
+#ifndef NETIF_F_GRO
+		netdev_ring(rx_ring)->last_rx = jiffies;
+
+#endif
+		/* update budget accounting */
 		total_packets++;
+	} while (likely(total_packets < budget));
+
+	rx_ring->rx_stats.packets += total_packets;
+	rx_ring->rx_stats.bytes += total_bytes;
+	q_vector->rx.total_packets += total_packets;
+	q_vector->rx.total_bytes += total_bytes;
+
+	if (cleaned_count)
+		igb_alloc_rx_buffers(rx_ring, cleaned_count);
+
+#ifndef IGB_NO_LRO
+	igb_lro_flush_all(q_vector);
+
+#endif /* IGB_NO_LRO */
+	return (total_packets < budget);
+}
+#else /* CONFIG_IGB_DISABLE_PACKET_SPLIT */
+/**
+ * igb_get_headlen - determine size of header for LRO/GRO
+ * @data: pointer to the start of the headers
+ * @max_len: total length of section to find headers in
+ *
+ * This function is meant to determine the length of headers that will
+ * be recognized by hardware for LRO, and GRO offloads.  The main
+ * motivation of doing this is to only perform one pull for IPv4 TCP
+ * packets so that we can do basic things like calculating the gso_size
+ * based on the average data per packet.
+ **/
+static unsigned int igb_get_headlen(unsigned char *data,
+				    unsigned int max_len)
+{
+	union {
+		unsigned char *network;
+		/* l2 headers */
+		struct ethhdr *eth;
+		struct vlan_hdr *vlan;
+		/* l3 headers */
+		struct iphdr *ipv4;
+		struct ipv6hdr *ipv6;
+	} hdr;
+	__be16 protocol;
+	u8 nexthdr = 0;	/* default to not TCP */
+	u8 hlen;
+
+	/* this should never happen, but better safe than sorry */
+	if (max_len < ETH_HLEN)
+		return max_len;
+
+	/* initialize network frame pointer */
+	hdr.network = data;
+
+	/* set first protocol and move network header forward */
+	protocol = hdr.eth->h_proto;
+	hdr.network += ETH_HLEN;
+
+	/* handle any vlan tag if present */
+	if (protocol == __constant_htons(ETH_P_8021Q)) {
+		if ((hdr.network - data) > (max_len - VLAN_HLEN))
+			return max_len;
+
+		protocol = hdr.vlan->h_vlan_encapsulated_proto;
+		hdr.network += VLAN_HLEN;
+	}
+
+	/* handle L3 protocols */
+	if (protocol == __constant_htons(ETH_P_IP)) {
+		if ((hdr.network - data) > (max_len - sizeof(struct iphdr)))
+			return max_len;
+
+		/* access ihl as a u8 to avoid unaligned access on ia64 */
+		hlen = (hdr.network[0] & 0x0F) << 2;
+
+		/* verify hlen meets minimum size requirements */
+		if (hlen < sizeof(struct iphdr))
+			return hdr.network - data;
+
+		/* record next protocol if header is present */
+		if (!(hdr.ipv4->frag_off & htons(IP_OFFSET)))
+			nexthdr = hdr.ipv4->protocol;
+#ifdef NETIF_F_TSO6
+	} else if (protocol == __constant_htons(ETH_P_IPV6)) {
+		if ((hdr.network - data) > (max_len - sizeof(struct ipv6hdr)))
+			return max_len;
+
+		/* record next protocol */
+		nexthdr = hdr.ipv6->nexthdr;
+		hlen = sizeof(struct ipv6hdr);
+#endif /* NETIF_F_TSO6 */
+	} else {
+		return hdr.network - data;
+	}
+
+	/* relocate pointer to start of L4 header */
+	hdr.network += hlen;
 
-		igb_rx_checksum_adv(rx_ring, staterr, skb);
+	/* finally sort out TCP */
+	if (nexthdr == IPPROTO_TCP) {
+		if ((hdr.network - data) > (max_len - sizeof(struct tcphdr)))
+			return max_len;
 
-		skb->protocol = eth_type_trans(skb, netdev);
-		skb_record_rx_queue(skb, rx_ring->queue_index);
+		/* access doff as a u8 to avoid unaligned access on ia64 */
+		hlen = (hdr.network[12] & 0xF0) >> 2;
 
-		if (staterr & E1000_RXD_STAT_VP) {
-			u16 vid = le16_to_cpu(rx_desc->wb.upper.vlan);
+		/* verify hlen meets minimum size requirements */
+		if (hlen < sizeof(struct tcphdr))
+			return hdr.network - data;
 
-			__vlan_hwaccel_put_tag(skb, vid);
+		hdr.network += hlen;
+	} else if (nexthdr == IPPROTO_UDP) {
+		if ((hdr.network - data) > (max_len - sizeof(struct udphdr)))
+			return max_len;
+
+		hdr.network += sizeof(struct udphdr);
+	}
+
+	/*
+	 * If everything has gone correctly hdr.network should be the
+	 * data section of the packet and will be the end of the header.
+	 * If not then it probably represents the end of the last recognized
+	 * header.
+	 */
+	if ((hdr.network - data) < max_len)
+		return hdr.network - data;
+	else
+		return max_len;
+}
+
+/**
+ * igb_pull_tail - igb specific version of skb_pull_tail
+ * @rx_ring: rx descriptor ring packet is being transacted on
+ * @rx_desc: pointer to the EOP Rx descriptor
+ * @skb: pointer to current skb being adjusted
+ *
+ * This function is an igb specific version of __pskb_pull_tail.  The
+ * main difference between this version and the original function is that
+ * this function can make several assumptions about the state of things
+ * that allow for significant optimizations versus the standard function.
+ * As a result we can do things like drop a frag and maintain an accurate
+ * truesize for the skb.
+ */
+static void igb_pull_tail(struct igb_ring *rx_ring,
+			  union e1000_adv_rx_desc *rx_desc,
+			  struct sk_buff *skb)
+{
+	struct skb_frag_struct *frag = &skb_shinfo(skb)->frags[0];
+	unsigned char *va;
+	unsigned int pull_len;
+
+	/*
+	 * it is valid to use page_address instead of kmap since we are
+	 * working with pages allocated out of the lomem pool per
+	 * alloc_page(GFP_ATOMIC)
+	 */
+	va = skb_frag_address(frag);
+
+#ifdef HAVE_PTP_1588_CLOCK
+	if (igb_test_staterr(rx_desc, E1000_RXDADV_STAT_TSIP)) {
+		/* retrieve timestamp from buffer */
+		igb_ptp_rx_pktstamp(rx_ring->q_vector, va, skb);
+
+		/* update pointers to remove timestamp header */
+		skb_frag_size_sub(frag, IGB_TS_HDR_LEN);
+		frag->page_offset += IGB_TS_HDR_LEN;
+		skb->data_len -= IGB_TS_HDR_LEN;
+		skb->len -= IGB_TS_HDR_LEN;
+
+		/* move va to start of packet data */
+		va += IGB_TS_HDR_LEN;
+	}
+#endif /* HAVE_PTP_1588_CLOCK */
+
+	/*
+	 * we need the header to contain the greater of either ETH_HLEN or
+	 * 60 bytes if the skb->len is less than 60 for skb_pad.
+	 */
+	pull_len = igb_get_headlen(va, IGB_RX_HDR_LEN);
+
+	/* align pull length to size of long to optimize memcpy performance */
+	skb_copy_to_linear_data(skb, va, ALIGN(pull_len, sizeof(long)));
+
+	/* update all of the pointers */
+	skb_frag_size_sub(frag, pull_len);
+	frag->page_offset += pull_len;
+	skb->data_len -= pull_len;
+	skb->tail += pull_len;
+}
+
+/**
+ * igb_cleanup_headers - Correct corrupted or empty headers
+ * @rx_ring: rx descriptor ring packet is being transacted on
+ * @rx_desc: pointer to the EOP Rx descriptor
+ * @skb: pointer to current skb being fixed
+ *
+ * Address the case where we are pulling data in on pages only
+ * and as such no data is present in the skb header.
+ *
+ * In addition if skb is not at least 60 bytes we need to pad it so that
+ * it is large enough to qualify as a valid Ethernet frame.
+ *
+ * Returns true if an error was encountered and skb was freed.
+ **/
+static bool igb_cleanup_headers(struct igb_ring *rx_ring,
+				union e1000_adv_rx_desc *rx_desc,
+				struct sk_buff *skb)
+{
+
+	if (unlikely((igb_test_staterr(rx_desc,
+				       E1000_RXDEXT_ERR_FRAME_ERR_MASK)))) {
+		struct net_device *netdev = rx_ring->netdev;
+		if (!(netdev->features & NETIF_F_RXALL)) {
+			dev_kfree_skb_any(skb);
+			return true;
 		}
-		napi_gro_receive(&q_vector->napi, skb);
+	}
+
+	/* place header in linear portion of buffer */
+	if (skb_is_nonlinear(skb))
+		igb_pull_tail(rx_ring, rx_desc, skb);
+
+	/* if skb_pad returns an error the skb was freed */
+	if (unlikely(skb->len < 60)) {
+		int pad_len = 60 - skb->len;
+
+		if (skb_pad(skb, pad_len))
+			return true;
+		__skb_put(skb, pad_len);
+	}
+
+	return false;
+}
+
+/* igb_clean_rx_irq -- * packet split */
+static bool igb_clean_rx_irq(struct igb_q_vector *q_vector, int budget)
+{
+	struct igb_ring *rx_ring = q_vector->rx.ring;
+	struct sk_buff *skb = rx_ring->skb;
+	unsigned int total_bytes = 0, total_packets = 0;
+	u16 cleaned_count = igb_desc_unused(rx_ring);
 
-next_desc:
-		rx_desc->wb.upper.status_error = 0;
+	do {
+		union e1000_adv_rx_desc *rx_desc;
 
 		/* return some buffers to hardware, one at a time is too slow */
 		if (cleaned_count >= IGB_RX_BUFFER_WRITE) {
-			igb_alloc_rx_buffers_adv(rx_ring, cleaned_count);
+			igb_alloc_rx_buffers(rx_ring, cleaned_count);
 			cleaned_count = 0;
 		}
 
-		/* use prefetched values */
-		rx_desc = next_rxd;
-		buffer_info = next_buffer;
-		staterr = le32_to_cpu(rx_desc->wb.upper.status_error);
-	}
+		rx_desc = IGB_RX_DESC(rx_ring, rx_ring->next_to_clean);
+
+		if (!igb_test_staterr(rx_desc, E1000_RXD_STAT_DD))
+			break;
 
-	rx_ring->next_to_clean = i;
-	cleaned_count = igb_desc_unused(rx_ring);
+		/*
+		 * This memory barrier is needed to keep us from reading
+		 * any other fields out of the rx_desc until we know the
+		 * RXD_STAT_DD bit is set
+		 */
+		rmb();
 
-	if (cleaned_count)
-		igb_alloc_rx_buffers_adv(rx_ring, cleaned_count);
+		/* retrieve a buffer from the ring */
+		skb = igb_fetch_rx_buffer(rx_ring, rx_desc, skb);
+
+		/* exit if we failed to retrieve a buffer */
+		if (!skb)
+			break;
+
+		cleaned_count++;
+
+		/* fetch next buffer in frame if non-eop */
+		if (igb_is_non_eop(rx_ring, rx_desc))
+			continue;
+
+		/* verify the packet layout is correct */
+		if (igb_cleanup_headers(rx_ring, rx_desc, skb)) {
+			skb = NULL;
+			continue;
+		}
+
+		/* probably a little skewed due to removing CRC */
+		total_bytes += skb->len;
+
+		/* populate checksum, timestamp, VLAN, and protocol */
+		igb_process_skb_fields(rx_ring, rx_desc, skb);
+
+#ifndef IGB_NO_LRO
+		if (igb_can_lro(rx_ring, rx_desc, skb))
+			igb_lro_receive(q_vector, skb);
+		else
+#endif
+#ifdef HAVE_VLAN_RX_REGISTER
+			igb_receive_skb(q_vector, skb);
+#else
+			napi_gro_receive(&q_vector->napi, skb);
+#endif
+#ifndef NETIF_F_GRO
+
+		netdev_ring(rx_ring)->last_rx = jiffies;
+#endif
+
+		/* reset skb pointer */
+		skb = NULL;
+
+		/* update budget accounting */
+		total_packets++;
+	} while (likely(total_packets < budget));
+
+	/* place incomplete frames back on ring for completion */
+	rx_ring->skb = skb;
 
-	rx_ring->total_packets += total_packets;
-	rx_ring->total_bytes += total_bytes;
-	u64_stats_update_begin(&rx_ring->rx_syncp);
 	rx_ring->rx_stats.packets += total_packets;
 	rx_ring->rx_stats.bytes += total_bytes;
-	u64_stats_update_end(&rx_ring->rx_syncp);
-	return cleaned;
+	q_vector->rx.total_packets += total_packets;
+	q_vector->rx.total_bytes += total_bytes;
+
+	if (cleaned_count)
+		igb_alloc_rx_buffers(rx_ring, cleaned_count);
+
+#ifndef IGB_NO_LRO
+	igb_lro_flush_all(q_vector);
+
+#endif /* IGB_NO_LRO */
+	return (total_packets < budget);
+}
+#endif /* CONFIG_IGB_DISABLE_PACKET_SPLIT */
+
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+static bool igb_alloc_mapped_skb(struct igb_ring *rx_ring,
+				 struct igb_rx_buffer *bi)
+{
+	struct sk_buff *skb = bi->skb;
+	dma_addr_t dma = bi->dma;
+
+	if (dma)
+		return true;
+
+	if (likely(!skb)) {
+		skb = netdev_alloc_skb_ip_align(netdev_ring(rx_ring),
+						rx_ring->rx_buffer_len);
+		bi->skb = skb;
+		if (!skb) {
+			rx_ring->rx_stats.alloc_failed++;
+			return false;
+		}
+
+		/* initialize skb for ring */
+		skb_record_rx_queue(skb, ring_queue_index(rx_ring));
+	}
+
+	dma = dma_map_single(rx_ring->dev, skb->data,
+			     rx_ring->rx_buffer_len, DMA_FROM_DEVICE);
+
+	/* if mapping failed free memory back to system since
+	 * there isn't much point in holding memory we can't use
+	 */
+	if (dma_mapping_error(rx_ring->dev, dma)) {
+		dev_kfree_skb_any(skb);
+		bi->skb = NULL;
+
+		rx_ring->rx_stats.alloc_failed++;
+		return false;
+	}
+
+	bi->dma = dma;
+	return true;
 }
 
+#else /* CONFIG_IGB_DISABLE_PACKET_SPLIT */
+static bool igb_alloc_mapped_page(struct igb_ring *rx_ring,
+				  struct igb_rx_buffer *bi)
+{
+	struct page *page = bi->page;
+	dma_addr_t dma;
+
+	/* since we are recycling buffers we should seldom need to alloc */
+	if (likely(page))
+		return true;
+
+	/* alloc new page for storage */
+	page = alloc_page(GFP_ATOMIC | __GFP_COLD);
+	if (unlikely(!page)) {
+		rx_ring->rx_stats.alloc_failed++;
+		return false;
+	}
+
+	/* map page for use */
+	dma = dma_map_page(rx_ring->dev, page, 0, PAGE_SIZE, DMA_FROM_DEVICE);
+
+	/*
+	 * if mapping failed free memory back to system since
+	 * there isn't much point in holding memory we can't use
+	 */
+	if (dma_mapping_error(rx_ring->dev, dma)) {
+		__free_page(page);
+
+		rx_ring->rx_stats.alloc_failed++;
+		return false;
+	}
+
+	bi->dma = dma;
+	bi->page = page;
+	bi->page_offset = 0;
+
+	return true;
+}
+
+#endif /* CONFIG_IGB_DISABLE_PACKET_SPLIT */
 /**
- * igb_alloc_rx_buffers_adv - Replace used receive buffers; packet split
+ * igb_alloc_rx_buffers - Replace used receive buffers; packet split
  * @adapter: address of board private structure
  **/
-void igb_alloc_rx_buffers_adv(struct igb_ring *rx_ring, int cleaned_count)
+void igb_alloc_rx_buffers(struct igb_ring *rx_ring, u16 cleaned_count)
 {
-	struct net_device *netdev = rx_ring->netdev;
 	union e1000_adv_rx_desc *rx_desc;
-	struct igb_buffer *buffer_info;
-	struct sk_buff *skb;
-	unsigned int i;
-	int bufsz;
-
-	i = rx_ring->next_to_use;
-	buffer_info = &rx_ring->buffer_info[i];
-
-	bufsz = rx_ring->rx_buffer_len;
-
-	while (cleaned_count--) {
-		rx_desc = E1000_RX_DESC_ADV(*rx_ring, i);
-
-		if ((bufsz < IGB_RXBUFFER_1024) && !buffer_info->page_dma) {
-			if (!buffer_info->page) {
-				buffer_info->page = netdev_alloc_page(netdev);
-				if (unlikely(!buffer_info->page)) {
-					u64_stats_update_begin(&rx_ring->rx_syncp);
-					rx_ring->rx_stats.alloc_failed++;
-					u64_stats_update_end(&rx_ring->rx_syncp);
-					goto no_buffers;
-				}
-				buffer_info->page_offset = 0;
-			} else {
-				buffer_info->page_offset ^= PAGE_SIZE / 2;
-			}
-			buffer_info->page_dma =
-				dma_map_page(rx_ring->dev, buffer_info->page,
-					     buffer_info->page_offset,
-					     PAGE_SIZE / 2,
-					     DMA_FROM_DEVICE);
-			if (dma_mapping_error(rx_ring->dev,
-					      buffer_info->page_dma)) {
-				buffer_info->page_dma = 0;
-				u64_stats_update_begin(&rx_ring->rx_syncp);
-				rx_ring->rx_stats.alloc_failed++;
-				u64_stats_update_end(&rx_ring->rx_syncp);
-				goto no_buffers;
-			}
-		}
+	struct igb_rx_buffer *bi;
+	u16 i = rx_ring->next_to_use;
 
-		skb = buffer_info->skb;
-		if (!skb) {
-			skb = netdev_alloc_skb_ip_align(netdev, bufsz);
-			if (unlikely(!skb)) {
-				u64_stats_update_begin(&rx_ring->rx_syncp);
-				rx_ring->rx_stats.alloc_failed++;
-				u64_stats_update_end(&rx_ring->rx_syncp);
-				goto no_buffers;
-			}
+	/* nothing to do */
+	if (!cleaned_count)
+		return;
 
-			buffer_info->skb = skb;
-		}
-		if (!buffer_info->dma) {
-			buffer_info->dma = dma_map_single(rx_ring->dev,
-			                                  skb->data,
-							  bufsz,
-							  DMA_FROM_DEVICE);
-			if (dma_mapping_error(rx_ring->dev,
-					      buffer_info->dma)) {
-				buffer_info->dma = 0;
-				u64_stats_update_begin(&rx_ring->rx_syncp);
-				rx_ring->rx_stats.alloc_failed++;
-				u64_stats_update_end(&rx_ring->rx_syncp);
-				goto no_buffers;
-			}
-		}
-		/* Refresh the desc even if buffer_addrs didn't change because
-		 * each write-back erases this info. */
-		if (bufsz < IGB_RXBUFFER_1024) {
-			rx_desc->read.pkt_addr =
-			     cpu_to_le64(buffer_info->page_dma);
-			rx_desc->read.hdr_addr = cpu_to_le64(buffer_info->dma);
-		} else {
-			rx_desc->read.pkt_addr = cpu_to_le64(buffer_info->dma);
-			rx_desc->read.hdr_addr = 0;
-		}
+	rx_desc = IGB_RX_DESC(rx_ring, i);
+	bi = &rx_ring->rx_buffer_info[i];
+	i -= rx_ring->count;
+
+	do {
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+		if (!igb_alloc_mapped_skb(rx_ring, bi))
+#else
+		if (!igb_alloc_mapped_page(rx_ring, bi))
+#endif /* CONFIG_IGB_DISABLE_PACKET_SPLIT */
+			break;
+
+		/*
+		 * Refresh the desc even if buffer_addrs didn't change
+		 * because each write-back erases this info.
+		 */
+#ifdef CONFIG_IGB_DISABLE_PACKET_SPLIT
+		rx_desc->read.pkt_addr = cpu_to_le64(bi->dma);
+#else
+		rx_desc->read.pkt_addr = cpu_to_le64(bi->dma + bi->page_offset);
+#endif
 
+		rx_desc++;
+		bi++;
 		i++;
-		if (i == rx_ring->count)
-			i = 0;
-		buffer_info = &rx_ring->buffer_info[i];
-	}
+		if (unlikely(!i)) {
+			rx_desc = IGB_RX_DESC(rx_ring, 0);
+			bi = rx_ring->rx_buffer_info;
+			i -= rx_ring->count;
+		}
+
+		/* clear the hdr_addr for the next_to_use descriptor */
+		rx_desc->read.hdr_addr = 0;
+
+		cleaned_count--;
+	} while (cleaned_count);
+
+	i += rx_ring->count;
 
-no_buffers:
 	if (rx_ring->next_to_use != i) {
+		/* record the next descriptor to use */
 		rx_ring->next_to_use = i;
-		if (i == 0)
-			i = (rx_ring->count - 1);
-		else
-			i--;
 
-		/* Force memory writes to complete before letting h/w
+#ifndef CONFIG_IGB_DISABLE_PACKET_SPLIT
+		/* update next to alloc since we have filled the ring */
+		rx_ring->next_to_alloc = i;
+
+#endif
+		/*
+		 * Force memory writes to complete before letting h/w
 		 * know there are new descriptors to fetch.  (Only
 		 * applicable for weak-ordered memory model archs,
-		 * such as IA-64). */
+		 * such as IA-64).
+		 */
 		wmb();
 		writel(i, rx_ring->tail);
 	}
 }
 
+#ifdef SIOCGMIIPHY
 /**
  * igb_mii_ioctl -
  * @netdev:
@@ -6061,191 +8612,20 @@ static int igb_mii_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
 		data->phy_id = adapter->hw.phy.addr;
 		break;
 	case SIOCGMIIREG:
-		if (igb_read_phy_reg(&adapter->hw, data->reg_num & 0x1F,
-		                     &data->val_out))
+		if (!capable(CAP_NET_ADMIN))
+			return -EPERM;
+		if (e1000_read_phy_reg(&adapter->hw, data->reg_num & 0x1F,
+				   &data->val_out))
 			return -EIO;
 		break;
 	case SIOCSMIIREG:
 	default:
 		return -EOPNOTSUPP;
 	}
-	return 0;
-}
-
-/**
- * igb_hwtstamp_ioctl - control hardware time stamping
- * @netdev:
- * @ifreq:
- * @cmd:
- *
- * Outgoing time stamping can be enabled and disabled. Play nice and
- * disable it when requested, although it shouldn't case any overhead
- * when no packet needs it. At most one packet in the queue may be
- * marked for time stamping, otherwise it would be impossible to tell
- * for sure to which packet the hardware time stamp belongs.
- *
- * Incoming time stamping has to be configured via the hardware
- * filters. Not all combinations are supported, in particular event
- * type has to be specified. Matching the kind of event packet is
- * not supported, with the exception of "all V2 events regardless of
- * level 2 or 4".
- *
- **/
-static int igb_hwtstamp_ioctl(struct net_device *netdev,
-			      struct ifreq *ifr, int cmd)
-{
-	struct igb_adapter *adapter = netdev_priv(netdev);
-	struct e1000_hw *hw = &adapter->hw;
-	struct hwtstamp_config config;
-	u32 tsync_tx_ctl = E1000_TSYNCTXCTL_ENABLED;
-	u32 tsync_rx_ctl = E1000_TSYNCRXCTL_ENABLED;
-	u32 tsync_rx_cfg = 0;
-	bool is_l4 = false;
-	bool is_l2 = false;
-	u32 regval;
-
-	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
-		return -EFAULT;
-
-	/* reserved for future extensions */
-	if (config.flags)
-		return -EINVAL;
-
-	switch (config.tx_type) {
-	case HWTSTAMP_TX_OFF:
-		tsync_tx_ctl = 0;
-	case HWTSTAMP_TX_ON:
-		break;
-	default:
-		return -ERANGE;
-	}
-
-	switch (config.rx_filter) {
-	case HWTSTAMP_FILTER_NONE:
-		tsync_rx_ctl = 0;
-		break;
-	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
-	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
-	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
-	case HWTSTAMP_FILTER_ALL:
-		/*
-		 * register TSYNCRXCFG must be set, therefore it is not
-		 * possible to time stamp both Sync and Delay_Req messages
-		 * => fall back to time stamping all packets
-		 */
-		tsync_rx_ctl |= E1000_TSYNCRXCTL_TYPE_ALL;
-		config.rx_filter = HWTSTAMP_FILTER_ALL;
-		break;
-	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
-		tsync_rx_ctl |= E1000_TSYNCRXCTL_TYPE_L4_V1;
-		tsync_rx_cfg = E1000_TSYNCRXCFG_PTP_V1_SYNC_MESSAGE;
-		is_l4 = true;
-		break;
-	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
-		tsync_rx_ctl |= E1000_TSYNCRXCTL_TYPE_L4_V1;
-		tsync_rx_cfg = E1000_TSYNCRXCFG_PTP_V1_DELAY_REQ_MESSAGE;
-		is_l4 = true;
-		break;
-	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
-	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
-		tsync_rx_ctl |= E1000_TSYNCRXCTL_TYPE_L2_L4_V2;
-		tsync_rx_cfg = E1000_TSYNCRXCFG_PTP_V2_SYNC_MESSAGE;
-		is_l2 = true;
-		is_l4 = true;
-		config.rx_filter = HWTSTAMP_FILTER_SOME;
-		break;
-	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
-	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
-		tsync_rx_ctl |= E1000_TSYNCRXCTL_TYPE_L2_L4_V2;
-		tsync_rx_cfg = E1000_TSYNCRXCFG_PTP_V2_DELAY_REQ_MESSAGE;
-		is_l2 = true;
-		is_l4 = true;
-		config.rx_filter = HWTSTAMP_FILTER_SOME;
-		break;
-	case HWTSTAMP_FILTER_PTP_V2_EVENT:
-	case HWTSTAMP_FILTER_PTP_V2_SYNC:
-	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
-		tsync_rx_ctl |= E1000_TSYNCRXCTL_TYPE_EVENT_V2;
-		config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
-		is_l2 = true;
-		break;
-	default:
-		return -ERANGE;
-	}
-
-	if (hw->mac.type == e1000_82575) {
-		if (tsync_rx_ctl | tsync_tx_ctl)
-			return -EINVAL;
-		return 0;
-	}
-
-	/*
-	 * Per-packet timestamping only works if all packets are
-	 * timestamped, so enable timestamping in all packets as
-	 * long as one rx filter was configured.
-	 */
-	if ((hw->mac.type == e1000_82580) && tsync_rx_ctl) {
-		tsync_rx_ctl = E1000_TSYNCRXCTL_ENABLED;
-		tsync_rx_ctl |= E1000_TSYNCRXCTL_TYPE_ALL;
-	}
-
-	/* enable/disable TX */
-	regval = rd32(E1000_TSYNCTXCTL);
-	regval &= ~E1000_TSYNCTXCTL_ENABLED;
-	regval |= tsync_tx_ctl;
-	wr32(E1000_TSYNCTXCTL, regval);
-
-	/* enable/disable RX */
-	regval = rd32(E1000_TSYNCRXCTL);
-	regval &= ~(E1000_TSYNCRXCTL_ENABLED | E1000_TSYNCRXCTL_TYPE_MASK);
-	regval |= tsync_rx_ctl;
-	wr32(E1000_TSYNCRXCTL, regval);
-
-	/* define which PTP packets are time stamped */
-	wr32(E1000_TSYNCRXCFG, tsync_rx_cfg);
-
-	/* define ethertype filter for timestamped packets */
-	if (is_l2)
-		wr32(E1000_ETQF(3),
-		                (E1000_ETQF_FILTER_ENABLE | /* enable filter */
-		                 E1000_ETQF_1588 | /* enable timestamping */
-		                 ETH_P_1588));     /* 1588 eth protocol type */
-	else
-		wr32(E1000_ETQF(3), 0);
-
-#define PTP_PORT 319
-	/* L4 Queue Filter[3]: filter by destination port and protocol */
-	if (is_l4) {
-		u32 ftqf = (IPPROTO_UDP /* UDP */
-			| E1000_FTQF_VF_BP /* VF not compared */
-			| E1000_FTQF_1588_TIME_STAMP /* Enable Timestamping */
-			| E1000_FTQF_MASK); /* mask all inputs */
-		ftqf &= ~E1000_FTQF_MASK_PROTO_BP; /* enable protocol check */
-
-		wr32(E1000_IMIR(3), htons(PTP_PORT));
-		wr32(E1000_IMIREXT(3),
-		     (E1000_IMIREXT_SIZE_BP | E1000_IMIREXT_CTRL_BP));
-		if (hw->mac.type == e1000_82576) {
-			/* enable source port check */
-			wr32(E1000_SPQF(3), htons(PTP_PORT));
-			ftqf &= ~E1000_FTQF_MASK_SOURCE_PORT_BP;
-		}
-		wr32(E1000_FTQF(3), ftqf);
-	} else {
-		wr32(E1000_FTQF(3), E1000_FTQF_MASK);
-	}
-	wrfl();
-
-	adapter->hwtstamp_config = config;
-
-	/* clear TX/RX time stamp registers, just to be sure */
-	regval = rd32(E1000_TXSTMPH);
-	regval = rd32(E1000_RXSTMPH);
-
-	return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
-		-EFAULT : 0;
+	return E1000_SUCCESS;
 }
 
+#endif
 /**
  * igb_ioctl -
  * @netdev:
@@ -6255,140 +8635,272 @@ static int igb_hwtstamp_ioctl(struct net_device *netdev,
 static int igb_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
 {
 	switch (cmd) {
+#ifdef SIOCGMIIPHY
 	case SIOCGMIIPHY:
 	case SIOCGMIIREG:
 	case SIOCSMIIREG:
 		return igb_mii_ioctl(netdev, ifr, cmd);
+#endif
+#ifdef HAVE_PTP_1588_CLOCK
 	case SIOCSHWTSTAMP:
-		return igb_hwtstamp_ioctl(netdev, ifr, cmd);
+		return igb_ptp_hwtstamp_ioctl(netdev, ifr, cmd);
+#endif /* HAVE_PTP_1588_CLOCK */
+#ifdef ETHTOOL_OPS_COMPAT
+	case SIOCETHTOOL:
+		return ethtool_ioctl(ifr);
+#endif
 	default:
 		return -EOPNOTSUPP;
 	}
 }
 
-s32 igb_read_pcie_cap_reg(struct e1000_hw *hw, u32 reg, u16 *value)
+s32 e1000_read_pcie_cap_reg(struct e1000_hw *hw, u32 reg, u16 *value)
 {
 	struct igb_adapter *adapter = hw->back;
 	u16 cap_offset;
 
-	cap_offset = adapter->pdev->pcie_cap;
+	cap_offset = pci_find_capability(adapter->pdev, PCI_CAP_ID_EXP);
 	if (!cap_offset)
 		return -E1000_ERR_CONFIG;
 
 	pci_read_config_word(adapter->pdev, cap_offset + reg, value);
 
-	return 0;
+	return E1000_SUCCESS;
 }
 
-s32 igb_write_pcie_cap_reg(struct e1000_hw *hw, u32 reg, u16 *value)
+s32 e1000_write_pcie_cap_reg(struct e1000_hw *hw, u32 reg, u16 *value)
 {
 	struct igb_adapter *adapter = hw->back;
 	u16 cap_offset;
 
-	cap_offset = adapter->pdev->pcie_cap;
+	cap_offset = pci_find_capability(adapter->pdev, PCI_CAP_ID_EXP);
 	if (!cap_offset)
 		return -E1000_ERR_CONFIG;
 
 	pci_write_config_word(adapter->pdev, cap_offset + reg, *value);
 
-	return 0;
+	return E1000_SUCCESS;
 }
 
-static void igb_vlan_mode(struct net_device *netdev, u32 features)
+#ifdef HAVE_VLAN_RX_REGISTER
+static void igb_vlan_mode(struct net_device *netdev, struct vlan_group *vlgrp)
+#else
+void igb_vlan_mode(struct net_device *netdev, u32 features)
+#endif
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
 	u32 ctrl, rctl;
+	int i;
+#ifdef HAVE_VLAN_RX_REGISTER
+	bool enable = !!vlgrp;
 
 	igb_irq_disable(adapter);
 
-	if (features & NETIF_F_HW_VLAN_RX) {
+	adapter->vlgrp = vlgrp;
+
+	if (!test_bit(__IGB_DOWN, &adapter->state))
+		igb_irq_enable(adapter);
+#else
+#ifdef NETIF_F_HW_VLAN_CTAG_RX
+	bool enable = !!(features & NETIF_F_HW_VLAN_CTAG_RX);
+#else
+	bool enable = !!(features & NETIF_F_HW_VLAN_RX);
+#endif
+#endif
+
+	if (enable) {
 		/* enable VLAN tag insert/strip */
-		ctrl = rd32(E1000_CTRL);
+		ctrl = E1000_READ_REG(hw, E1000_CTRL);
 		ctrl |= E1000_CTRL_VME;
-		wr32(E1000_CTRL, ctrl);
+		E1000_WRITE_REG(hw, E1000_CTRL, ctrl);
 
 		/* Disable CFI check */
-		rctl = rd32(E1000_RCTL);
+		rctl = E1000_READ_REG(hw, E1000_RCTL);
 		rctl &= ~E1000_RCTL_CFIEN;
-		wr32(E1000_RCTL, rctl);
+		E1000_WRITE_REG(hw, E1000_RCTL, rctl);
 	} else {
 		/* disable VLAN tag insert/strip */
-		ctrl = rd32(E1000_CTRL);
+		ctrl = E1000_READ_REG(hw, E1000_CTRL);
 		ctrl &= ~E1000_CTRL_VME;
-		wr32(E1000_CTRL, ctrl);
+		E1000_WRITE_REG(hw, E1000_CTRL, ctrl);
 	}
 
-	igb_rlpml_set(adapter);
+#ifndef CONFIG_IGB_VMDQ_NETDEV
+	for (i = 0; i < adapter->vmdq_pools; i++) {
+		igb_set_vf_vlan_strip(adapter,
+				      adapter->vfs_allocated_count + i,
+				      enable);
+	}
 
-	if (!test_bit(__IGB_DOWN, &adapter->state))
-		igb_irq_enable(adapter);
+#else
+	igb_set_vf_vlan_strip(adapter,
+			      adapter->vfs_allocated_count,
+			      enable);
+
+	for (i = 1; i < adapter->vmdq_pools; i++) {
+#ifdef HAVE_VLAN_RX_REGISTER
+		struct igb_vmdq_adapter *vadapter;
+		vadapter = netdev_priv(adapter->vmdq_netdev[i-1]);
+		enable = !!vadapter->vlgrp;
+#else
+		struct net_device *vnetdev;
+		vnetdev = adapter->vmdq_netdev[i-1];
+#ifdef NETIF_F_HW_VLAN_CTAG_RX
+		enable = !!(vnetdev->features & NETIF_F_HW_VLAN_CTAG_RX);
+#else
+		enable = !!(vnetdev->features & NETIF_F_HW_VLAN_RX);
+#endif
+#endif
+		igb_set_vf_vlan_strip(adapter,
+				      adapter->vfs_allocated_count + i,
+				      enable);
+	}
+
+#endif
+	igb_rlpml_set(adapter);
 }
 
+#ifdef HAVE_INT_NDO_VLAN_RX_ADD_VID
+#ifdef NETIF_F_HW_VLAN_CTAG_RX
+static int igb_vlan_rx_add_vid(struct net_device *netdev,
+			       __always_unused __be16 proto, u16 vid)
+#else
+static int igb_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
+#endif
+#else
 static void igb_vlan_rx_add_vid(struct net_device *netdev, u16 vid)
+#endif
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
-	struct e1000_hw *hw = &adapter->hw;
 	int pf_id = adapter->vfs_allocated_count;
 
 	/* attempt to add filter to vlvf array */
-	igb_vlvf_set(adapter, vid, true, pf_id);
+	igb_vlvf_set(adapter, vid, TRUE, pf_id);
 
 	/* add the filter since PF can receive vlans w/o entry in vlvf */
-	igb_vfta_set(hw, vid, true);
+	igb_vfta_set(adapter, vid, TRUE);
+#ifndef HAVE_NETDEV_VLAN_FEATURES
+
+	/* Copy feature flags from netdev to the vlan netdev for this vid.
+	 * This allows things like TSO to bubble down to our vlan device.
+	 * There is no need to update netdev for vlan 0 (DCB), since it
+	 * wouldn't has v_netdev.
+	 */
+	if (adapter->vlgrp) {
+		struct vlan_group *vlgrp = adapter->vlgrp;
+		struct net_device *v_netdev = vlan_group_get_device(vlgrp, vid);
+		if (v_netdev) {
+			v_netdev->features |= netdev->features;
+			vlan_group_set_device(vlgrp, vid, v_netdev);
+		}
+	}
+#endif
+#ifndef HAVE_VLAN_RX_REGISTER
 
 	set_bit(vid, adapter->active_vlans);
+#endif
+#ifdef HAVE_INT_NDO_VLAN_RX_ADD_VID
+	return 0;
+#endif
 }
 
+#ifdef HAVE_INT_NDO_VLAN_RX_ADD_VID
+#ifdef NETIF_F_HW_VLAN_CTAG_RX
+static int igb_vlan_rx_kill_vid(struct net_device *netdev,
+				__always_unused __be16 proto, u16 vid)
+#else
+static int igb_vlan_rx_kill_vid(struct net_device *netdev, u16 vid)
+#endif
+#else
 static void igb_vlan_rx_kill_vid(struct net_device *netdev, u16 vid)
+#endif
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
-	struct e1000_hw *hw = &adapter->hw;
 	int pf_id = adapter->vfs_allocated_count;
 	s32 err;
 
+#ifdef HAVE_VLAN_RX_REGISTER
 	igb_irq_disable(adapter);
 
+	vlan_group_set_device(adapter->vlgrp, vid, NULL);
+
 	if (!test_bit(__IGB_DOWN, &adapter->state))
 		igb_irq_enable(adapter);
 
+#endif /* HAVE_VLAN_RX_REGISTER */
 	/* remove vlan from VLVF table array */
-	err = igb_vlvf_set(adapter, vid, false, pf_id);
+	err = igb_vlvf_set(adapter, vid, FALSE, pf_id);
 
 	/* if vid was not present in VLVF just remove it from table */
 	if (err)
-		igb_vfta_set(hw, vid, false);
+		igb_vfta_set(adapter, vid, FALSE);
+#ifndef HAVE_VLAN_RX_REGISTER
 
 	clear_bit(vid, adapter->active_vlans);
+#endif
+#ifdef HAVE_INT_NDO_VLAN_RX_ADD_VID
+	return 0;
+#endif
 }
 
 static void igb_restore_vlan(struct igb_adapter *adapter)
 {
+#ifdef HAVE_VLAN_RX_REGISTER
+	igb_vlan_mode(adapter->netdev, adapter->vlgrp);
+
+	if (adapter->vlgrp) {
+		u16 vid;
+		for (vid = 0; vid < VLAN_N_VID; vid++) {
+			if (!vlan_group_get_device(adapter->vlgrp, vid))
+				continue;
+#ifdef NETIF_F_HW_VLAN_CTAG_RX
+			igb_vlan_rx_add_vid(adapter->netdev,
+					    htons(ETH_P_8021Q), vid);
+#else
+			igb_vlan_rx_add_vid(adapter->netdev, vid);
+#endif
+		}
+	}
+#else
 	u16 vid;
 
+	igb_vlan_mode(adapter->netdev, adapter->netdev->features);
+
 	for_each_set_bit(vid, adapter->active_vlans, VLAN_N_VID)
+#ifdef NETIF_F_HW_VLAN_CTAG_RX
+		igb_vlan_rx_add_vid(adapter->netdev,
+				    htons(ETH_P_8021Q), vid);
+#else
 		igb_vlan_rx_add_vid(adapter->netdev, vid);
+#endif
+#endif
 }
 
-int igb_set_spd_dplx(struct igb_adapter *adapter, u32 spd, u8 dplx)
+int igb_set_spd_dplx(struct igb_adapter *adapter, u16 spddplx)
 {
 	struct pci_dev *pdev = adapter->pdev;
 	struct e1000_mac_info *mac = &adapter->hw.mac;
 
 	mac->autoneg = 0;
 
-	/* Make sure dplx is at most 1 bit and lsb of speed is not set
-	 * for the switch() below to work */
-	if ((spd & 1) || (dplx & ~1))
-		goto err_inval;
-
-	/* Fiber NIC's only allow 1000 Gbps Full duplex */
-	if ((adapter->hw.phy.media_type == e1000_media_type_internal_serdes) &&
-	    spd != SPEED_1000 &&
-	    dplx != DUPLEX_FULL)
-		goto err_inval;
+	/* SerDes device's does not support 10Mbps Full/duplex
+	 * and 100Mbps Half duplex
+	 */
+	if (adapter->hw.phy.media_type == e1000_media_type_internal_serdes) {
+		switch (spddplx) {
+		case SPEED_10 + DUPLEX_HALF:
+		case SPEED_10 + DUPLEX_FULL:
+		case SPEED_100 + DUPLEX_HALF:
+			dev_err(pci_dev_to_dev(pdev),
+				"Unsupported Speed/Duplex configuration\n");
+			return -EINVAL;
+		default:
+			break;
+		}
+	}
 
-	switch (spd + dplx) {
+	switch (spddplx) {
 	case SPEED_10 + DUPLEX_HALF:
 		mac->forced_speed_duplex = ADVERTISE_10_HALF;
 		break;
@@ -6407,30 +8919,74 @@ int igb_set_spd_dplx(struct igb_adapter *adapter, u32 spd, u8 dplx)
 		break;
 	case SPEED_1000 + DUPLEX_HALF: /* not supported */
 	default:
-		goto err_inval;
+		dev_err(pci_dev_to_dev(pdev), "Unsupported Speed/Duplex configuration\n");
+		return -EINVAL;
 	}
+
+	/* clear MDI, MDI(-X) override is only allowed when autoneg enabled */
+	adapter->hw.phy.mdix = AUTO_ALL_MODES;
+
 	return 0;
+}
+
+/* This function should only be called if RTNL lock is held */
+int igb_setup_queues(struct igb_adapter *adapter)
+{
+	struct net_device *dev = adapter->netdev;
+	int err;
+
+	if (adapter->rss_queues == adapter->num_rx_queues) {
+		if (adapter->tss_queues) {
+			if (adapter->tss_queues == adapter->num_tx_queues)
+				return 0;
+		} else if (adapter->vfs_allocated_count ||
+			   adapter->rss_queues == adapter->num_tx_queues) {
+			return 0;
+		}
+	}
 
-err_inval:
-	dev_err(&pdev->dev, "Unsupported Speed/Duplex configuration\n");
-	return -EINVAL;
+	/*
+	 * Hardware has to reinitialize queues and interrupts to
+	 * match the new configuration. Unfortunately, the hardware
+	 * is not flexible enough to do this dynamically.
+	 */
+	if (netif_running(dev))
+		igb_close(dev);
+
+	igb_clear_interrupt_scheme(adapter);
+
+	err = igb_init_interrupt_scheme(adapter, true);
+	if (err) {
+		dev_close(dev);
+		return err;
+	}
+
+	if (netif_running(dev))
+		err = igb_open(dev);
+
+	return err;
 }
 
-static int __igb_shutdown(struct pci_dev *pdev, bool *enable_wake)
+static int __igb_shutdown(struct pci_dev *pdev, bool *enable_wake,
+			  bool runtime)
 {
 	struct net_device *netdev = pci_get_drvdata(pdev);
 	struct igb_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
 	u32 ctrl, rctl, status;
-	u32 wufc = adapter->wol;
+	u32 wufc = runtime ? E1000_WUFC_LNKC : adapter->wol;
 #ifdef CONFIG_PM
 	int retval = 0;
 #endif
 
 	netif_device_detach(netdev);
 
+	status = E1000_READ_REG(hw, E1000_STATUS);
+	if (status & E1000_STATUS_LU)
+		wufc &= ~E1000_WUFC_LNKC;
+
 	if (netif_running(netdev))
-		igb_close(netdev);
+		__igb_close(netdev, true);
 
 	igb_clear_interrupt_scheme(adapter);
 
@@ -6440,37 +8996,31 @@ static int __igb_shutdown(struct pci_dev *pdev, bool *enable_wake)
 		return retval;
 #endif
 
-	status = rd32(E1000_STATUS);
-	if (status & E1000_STATUS_LU)
-		wufc &= ~E1000_WUFC_LNKC;
-
 	if (wufc) {
 		igb_setup_rctl(adapter);
 		igb_set_rx_mode(netdev);
 
 		/* turn on all-multi mode if wake on multicast is enabled */
 		if (wufc & E1000_WUFC_MC) {
-			rctl = rd32(E1000_RCTL);
+			rctl = E1000_READ_REG(hw, E1000_RCTL);
 			rctl |= E1000_RCTL_MPE;
-			wr32(E1000_RCTL, rctl);
+			E1000_WRITE_REG(hw, E1000_RCTL, rctl);
 		}
 
-		ctrl = rd32(E1000_CTRL);
-		/* advertise wake from D3Cold */
-		#define E1000_CTRL_ADVD3WUC 0x00100000
+		ctrl = E1000_READ_REG(hw, E1000_CTRL);
 		/* phy power management enable */
 		#define E1000_CTRL_EN_PHY_PWR_MGMT 0x00200000
 		ctrl |= E1000_CTRL_ADVD3WUC;
-		wr32(E1000_CTRL, ctrl);
+		E1000_WRITE_REG(hw, E1000_CTRL, ctrl);
 
 		/* Allow time for pending master requests to run */
-		igb_disable_pcie_master(hw);
+		e1000_disable_pcie_master(hw);
 
-		wr32(E1000_WUC, E1000_WUC_PME_EN);
-		wr32(E1000_WUFC, wufc);
+		E1000_WRITE_REG(hw, E1000_WUC, E1000_WUC_PME_EN);
+		E1000_WRITE_REG(hw, E1000_WUFC, wufc);
 	} else {
-		wr32(E1000_WUC, 0);
-		wr32(E1000_WUFC, 0);
+		E1000_WRITE_REG(hw, E1000_WUC, 0);
+		E1000_WRITE_REG(hw, E1000_WUFC, 0);
 	}
 
 	*enable_wake = wufc || adapter->en_mng_pt;
@@ -6489,12 +9039,19 @@ static int __igb_shutdown(struct pci_dev *pdev, bool *enable_wake)
 }
 
 #ifdef CONFIG_PM
+#ifdef HAVE_SYSTEM_SLEEP_PM_OPS
+static int igb_suspend(struct device *dev)
+#else
 static int igb_suspend(struct pci_dev *pdev, pm_message_t state)
+#endif /* HAVE_SYSTEM_SLEEP_PM_OPS */
 {
+#ifdef HAVE_SYSTEM_SLEEP_PM_OPS
+	struct pci_dev *pdev = to_pci_dev(dev);
+#endif /* HAVE_SYSTEM_SLEEP_PM_OPS */
 	int retval;
 	bool wake;
 
-	retval = __igb_shutdown(pdev, &wake);
+	retval = __igb_shutdown(pdev, &wake, 0);
 	if (retval)
 		return retval;
 
@@ -6508,8 +9065,15 @@ static int igb_suspend(struct pci_dev *pdev, pm_message_t state)
 	return 0;
 }
 
+#ifdef HAVE_SYSTEM_SLEEP_PM_OPS
+static int igb_resume(struct device *dev)
+#else
 static int igb_resume(struct pci_dev *pdev)
+#endif /* HAVE_SYSTEM_SLEEP_PM_OPS */
 {
+#ifdef HAVE_SYSTEM_SLEEP_PM_OPS
+	struct pci_dev *pdev = to_pci_dev(dev);
+#endif /* HAVE_SYSTEM_SLEEP_PM_OPS */
 	struct net_device *netdev = pci_get_drvdata(pdev);
 	struct igb_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
@@ -6521,7 +9085,7 @@ static int igb_resume(struct pci_dev *pdev)
 
 	err = pci_enable_device_mem(pdev);
 	if (err) {
-		dev_err(&pdev->dev,
+		dev_err(pci_dev_to_dev(pdev),
 			"igb: Cannot enable PCI device from suspend\n");
 		return err;
 	}
@@ -6530,8 +9094,8 @@ static int igb_resume(struct pci_dev *pdev)
 	pci_enable_wake(pdev, PCI_D3hot, 0);
 	pci_enable_wake(pdev, PCI_D3cold, 0);
 
-	if (igb_init_interrupt_scheme(adapter)) {
-		dev_err(&pdev->dev, "Unable to allocate memory for queues\n");
+	if (igb_init_interrupt_scheme(adapter, true)) {
+		dev_err(pci_dev_to_dev(pdev), "Unable to allocate memory for queues\n");
 		return -ENOMEM;
 	}
 
@@ -6541,10 +9105,12 @@ static int igb_resume(struct pci_dev *pdev)
 	 * driver. */
 	igb_get_hw_control(adapter);
 
-	wr32(E1000_WUS, ~0);
+	E1000_WRITE_REG(hw, E1000_WUS, ~0);
 
-	if (netif_running(netdev)) {
-		err = igb_open(netdev);
+	if (netdev->flags & IFF_UP) {
+		rtnl_lock();
+		err = __igb_open(netdev, true);
+		rtnl_unlock();
 		if (err)
 			return err;
 	}
@@ -6553,19 +9119,86 @@ static int igb_resume(struct pci_dev *pdev)
 
 	return 0;
 }
-#endif
 
-static void igb_shutdown(struct pci_dev *pdev)
+#ifdef CONFIG_PM_RUNTIME
+#ifdef HAVE_SYSTEM_SLEEP_PM_OPS
+static int igb_runtime_idle(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct net_device *netdev = pci_get_drvdata(pdev);
+	struct igb_adapter *adapter = netdev_priv(netdev);
+
+	if (!igb_has_link(adapter))
+		pm_schedule_suspend(dev, MSEC_PER_SEC * 5);
+
+	return -EBUSY;
+}
+
+static int igb_runtime_suspend(struct device *dev)
 {
+	struct pci_dev *pdev = to_pci_dev(dev);
+	int retval;
 	bool wake;
 
-	__igb_shutdown(pdev, &wake);
+	retval = __igb_shutdown(pdev, &wake, 1);
+	if (retval)
+		return retval;
+
+	if (wake) {
+		pci_prepare_to_sleep(pdev);
+	} else {
+		pci_wake_from_d3(pdev, false);
+		pci_set_power_state(pdev, PCI_D3hot);
+	}
+
+	return 0;
+}
+
+static int igb_runtime_resume(struct device *dev)
+{
+	return igb_resume(dev);
+}
+#endif /* HAVE_SYSTEM_SLEEP_PM_OPS */
+#endif /* CONFIG_PM_RUNTIME */
+#endif /* CONFIG_PM */
+
+#ifdef USE_REBOOT_NOTIFIER
+/* only want to do this for 2.4 kernels? */
+static int igb_notify_reboot(struct notifier_block *nb, unsigned long event,
+                             void *p)
+{
+	struct pci_dev *pdev = NULL;
+	bool wake;
+
+	switch (event) {
+	case SYS_DOWN:
+	case SYS_HALT:
+	case SYS_POWER_OFF:
+		while ((pdev = pci_find_device(PCI_ANY_ID, PCI_ANY_ID, pdev))) {
+			if (pci_dev_driver(pdev) == &igb_driver) {
+				__igb_shutdown(pdev, &wake, 0);
+				if (event == SYS_POWER_OFF) {
+					pci_wake_from_d3(pdev, wake);
+					pci_set_power_state(pdev, PCI_D3hot);
+				}
+			}
+		}
+	}
+	return NOTIFY_DONE;
+}
+#else
+static void igb_shutdown(struct pci_dev *pdev)
+{
+	bool wake = false;
+
+	__igb_shutdown(pdev, &wake, 0);
 
 	if (system_state == SYSTEM_POWER_OFF) {
 		pci_wake_from_d3(pdev, wake);
 		pci_set_power_state(pdev, PCI_D3hot);
 	}
 }
+#endif /* USE_REBOOT_NOTIFIER */
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
 /*
@@ -6577,23 +9210,22 @@ static void igb_netpoll(struct net_device *netdev)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
+	struct igb_q_vector *q_vector;
 	int i;
 
-	if (!adapter->msix_entries) {
-		struct igb_q_vector *q_vector = adapter->q_vector[0];
-		igb_irq_disable(adapter);
-		napi_schedule(&q_vector->napi);
-		return;
-	}
-
 	for (i = 0; i < adapter->num_q_vectors; i++) {
-		struct igb_q_vector *q_vector = adapter->q_vector[i];
-		wr32(E1000_EIMC, q_vector->eims_value);
+		q_vector = adapter->q_vector[i];
+		if (adapter->msix_entries)
+			E1000_WRITE_REG(hw, E1000_EIMC, q_vector->eims_value);
+		else
+			igb_irq_disable(adapter);
 		napi_schedule(&q_vector->napi);
 	}
 }
 #endif /* CONFIG_NET_POLL_CONTROLLER */
 
+#ifdef HAVE_PCI_ERS
+#define E1000_DEV_ID_82576_VF 0x10CA
 /**
  * igb_io_error_detected - called when PCI error is detected
  * @pdev: Pointer to PCI device
@@ -6608,6 +9240,83 @@ static pci_ers_result_t igb_io_error_detected(struct pci_dev *pdev,
 	struct net_device *netdev = pci_get_drvdata(pdev);
 	struct igb_adapter *adapter = netdev_priv(netdev);
 
+#ifdef CONFIG_PCI_IOV
+	struct pci_dev *bdev, *vfdev;
+	u32 dw0, dw1, dw2, dw3;
+	int vf, pos;
+	u16 req_id, pf_func;
+
+	if (!(adapter->flags & IGB_FLAG_DETECT_BAD_DMA))
+		goto skip_bad_vf_detection;
+
+	bdev = pdev->bus->self;
+	while (bdev && (pci_pcie_type(bdev) != PCI_EXP_TYPE_ROOT_PORT))
+		bdev = bdev->bus->self;
+
+	if (!bdev)
+		goto skip_bad_vf_detection;
+
+	pos = pci_find_ext_capability(bdev, PCI_EXT_CAP_ID_ERR);
+	if (!pos)
+		goto skip_bad_vf_detection;
+
+	pci_read_config_dword(bdev, pos + PCI_ERR_HEADER_LOG, &dw0);
+	pci_read_config_dword(bdev, pos + PCI_ERR_HEADER_LOG + 4, &dw1);
+	pci_read_config_dword(bdev, pos + PCI_ERR_HEADER_LOG + 8, &dw2);
+	pci_read_config_dword(bdev, pos + PCI_ERR_HEADER_LOG + 12, &dw3);
+
+	req_id = dw1 >> 16;
+	/* On the 82576 if bit 7 of the requestor ID is set then it's a VF */
+	if (!(req_id & 0x0080))
+		goto skip_bad_vf_detection;
+
+	pf_func = req_id & 0x01;
+	if ((pf_func & 1) == (pdev->devfn & 1)) {
+
+		vf = (req_id & 0x7F) >> 1;
+		dev_err(pci_dev_to_dev(pdev),
+			"VF %d has caused a PCIe error\n", vf);
+		dev_err(pci_dev_to_dev(pdev),
+			"TLP: dw0: %8.8x\tdw1: %8.8x\tdw2: "
+			"%8.8x\tdw3: %8.8x\n",
+			dw0, dw1, dw2, dw3);
+
+		/* Find the pci device of the offending VF */
+		vfdev = pci_get_device(PCI_VENDOR_ID_INTEL,
+				       E1000_DEV_ID_82576_VF, NULL);
+		while (vfdev) {
+			if (vfdev->devfn == (req_id & 0xFF))
+				break;
+			vfdev = pci_get_device(PCI_VENDOR_ID_INTEL,
+					       E1000_DEV_ID_82576_VF, vfdev);
+		}
+		/*
+		 * There's a slim chance the VF could have been hot plugged,
+		 * so if it is no longer present we don't need to issue the
+		 * VFLR.  Just clean up the AER in that case.
+		 */
+		if (vfdev) {
+			dev_err(pci_dev_to_dev(pdev),
+				"Issuing VFLR to VF %d\n", vf);
+			pci_write_config_dword(vfdev, 0xA8, 0x00008000);
+		}
+
+		pci_cleanup_aer_uncorrect_error_status(pdev);
+	}
+
+	/*
+	 * Even though the error may have occurred on the other port
+	 * we still need to increment the vf error reference count for
+	 * both ports because the I/O resume function will be called
+	 * for both of them.
+	 */
+	adapter->vferr_refcount++;
+
+	return PCI_ERS_RESULT_RECOVERED;
+
+skip_bad_vf_detection:
+#endif /* CONFIG_PCI_IOV */
+
 	netif_device_detach(netdev);
 
 	if (state == pci_channel_io_perm_failure)
@@ -6634,10 +9343,9 @@ static pci_ers_result_t igb_io_slot_reset(struct pci_dev *pdev)
 	struct igb_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
 	pci_ers_result_t result;
-	int err;
 
 	if (pci_enable_device_mem(pdev)) {
-		dev_err(&pdev->dev,
+		dev_err(pci_dev_to_dev(pdev),
 			"Cannot re-enable PCI device after reset.\n");
 		result = PCI_ERS_RESULT_DISCONNECT;
 	} else {
@@ -6648,17 +9356,12 @@ static pci_ers_result_t igb_io_slot_reset(struct pci_dev *pdev)
 		pci_enable_wake(pdev, PCI_D3hot, 0);
 		pci_enable_wake(pdev, PCI_D3cold, 0);
 
-		igb_reset(adapter);
-		wr32(E1000_WUS, ~0);
+		schedule_work(&adapter->reset_task);
+		E1000_WRITE_REG(hw, E1000_WUS, ~0);
 		result = PCI_ERS_RESULT_RECOVERED;
 	}
 
-	err = pci_cleanup_aer_uncorrect_error_status(pdev);
-	if (err) {
-		dev_err(&pdev->dev, "pci_cleanup_aer_uncorrect_error_status "
-		        "failed 0x%0x\n", err);
-		/* non-fatal, continue */
-	}
+	pci_cleanup_aer_uncorrect_error_status(pdev);
 
 	return result;
 }
@@ -6676,9 +9379,15 @@ static void igb_io_resume(struct pci_dev *pdev)
 	struct net_device *netdev = pci_get_drvdata(pdev);
 	struct igb_adapter *adapter = netdev_priv(netdev);
 
+	if (adapter->vferr_refcount) {
+		dev_info(pci_dev_to_dev(pdev), "Resuming after VF err\n");
+		adapter->vferr_refcount--;
+		return;
+	}
+
 	if (netif_running(netdev)) {
 		if (igb_up(adapter)) {
-			dev_err(&pdev->dev, "igb_up failed after reset\n");
+			dev_err(pci_dev_to_dev(pdev), "igb_up failed after reset\n");
 			return;
 		}
 	}
@@ -6690,39 +9399,55 @@ static void igb_io_resume(struct pci_dev *pdev)
 	igb_get_hw_control(adapter);
 }
 
-static void igb_rar_set_qsel(struct igb_adapter *adapter, u8 *addr, u32 index,
-                             u8 qsel)
+#endif /* HAVE_PCI_ERS */
+
+int igb_add_mac_filter(struct igb_adapter *adapter, u8 *addr, u16 queue)
 {
-	u32 rar_low, rar_high;
 	struct e1000_hw *hw = &adapter->hw;
+	int i;
 
-	/* HW expects these in little endian so we reverse the byte order
-	 * from network order (big endian) to little endian
-	 */
-	rar_low = ((u32) addr[0] | ((u32) addr[1] << 8) |
-	          ((u32) addr[2] << 16) | ((u32) addr[3] << 24));
-	rar_high = ((u32) addr[4] | ((u32) addr[5] << 8));
-
-	/* Indicate to hardware the Address is Valid. */
-	rar_high |= E1000_RAH_AV;
-
-	if (hw->mac.type == e1000_82575)
-		rar_high |= E1000_RAH_POOL_1 * qsel;
-	else
-		rar_high |= E1000_RAH_POOL_1 << qsel;
+	if (is_zero_ether_addr(addr))
+		return 0;
 
-	wr32(E1000_RAL(index), rar_low);
-	wrfl();
-	wr32(E1000_RAH(index), rar_high);
-	wrfl();
+	for (i = 0; i < hw->mac.rar_entry_count; i++) {
+		if (adapter->mac_table[i].state & IGB_MAC_STATE_IN_USE)
+			continue;
+		adapter->mac_table[i].state = (IGB_MAC_STATE_MODIFIED |
+						   IGB_MAC_STATE_IN_USE);
+		memcpy(adapter->mac_table[i].addr, addr, ETH_ALEN);
+		adapter->mac_table[i].queue = queue;
+		igb_sync_mac_table(adapter);
+		return 0;
+	}
+	return -ENOMEM;
 }
+int igb_del_mac_filter(struct igb_adapter *adapter, u8* addr, u16 queue)
+{
+	/* search table for addr, if found, set to 0 and sync */
+	int i;
+	struct e1000_hw *hw = &adapter->hw;
 
+	if (is_zero_ether_addr(addr))
+		return 0;
+	for (i = 0; i < hw->mac.rar_entry_count; i++) {
+		if (!ether_addr_equal(addr, adapter->mac_table[i].addr) &&
+		    adapter->mac_table[i].queue == queue) {
+			adapter->mac_table[i].state = IGB_MAC_STATE_MODIFIED;
+			memset(adapter->mac_table[i].addr, 0, ETH_ALEN);
+			adapter->mac_table[i].queue = 0;
+			igb_sync_mac_table(adapter);
+			return 0;
+		}
+	}
+	return -ENOMEM;
+}
 static int igb_set_vf_mac(struct igb_adapter *adapter,
                           int vf, unsigned char *mac_addr)
 {
 	struct e1000_hw *hw = &adapter->hw;
 	/* VF MAC addresses start at end of receive addresses and moves
-	 * torwards the first, as a result a collision should not be possible */
+	 * towards the first, as a result a collision should not be possible
+	 */
 	int rar_entry = hw->mac.rar_entry_count - (vf + 1);
 
 	memcpy(adapter->vf_data[vf].vf_mac_addresses, mac_addr, ETH_ALEN);
@@ -6732,6 +9457,7 @@ static int igb_set_vf_mac(struct igb_adapter *adapter,
 	return 0;
 }
 
+#ifdef IFLA_VF_MAX
 static int igb_ndo_set_vf_mac(struct net_device *netdev, int vf, u8 *mac)
 {
 	struct igb_adapter *adapter = netdev_priv(netdev);
@@ -6740,7 +9466,7 @@ static int igb_ndo_set_vf_mac(struct net_device *netdev, int vf, u8 *mac)
 	adapter->vf_data[vf].flags |= IGB_VF_FLAG_PF_SET_MAC;
 	dev_info(&adapter->pdev->dev, "setting MAC %pM on VF %d\n", mac, vf);
 	dev_info(&adapter->pdev->dev, "Reload the VF driver to make this"
-				      " change effective.");
+				      " change effective.\n");
 	if (test_bit(__IGB_DOWN, &adapter->state)) {
 		dev_warn(&adapter->pdev->dev, "The VF MAC address has been set,"
 			 " but the PF device is not up.\n");
@@ -6757,13 +9483,15 @@ static int igb_link_mbps(int internal_link_speed)
 		return 100;
 	case SPEED_1000:
 		return 1000;
+	case SPEED_2500:
+		return 2500;
 	default:
 		return 0;
 	}
 }
 
 static void igb_set_vf_rate_limit(struct e1000_hw *hw, int vf, int tx_rate,
-				  int link_speed)
+			int link_speed)
 {
 	int rf_dec, rf_int;
 	u32 bcnrc_val;
@@ -6776,14 +9504,19 @@ static void igb_set_vf_rate_limit(struct e1000_hw *hw, int vf, int tx_rate,
 
 		bcnrc_val = E1000_RTTBCNRC_RS_ENA;
 		bcnrc_val |= ((rf_int<<E1000_RTTBCNRC_RF_INT_SHIFT) &
-		               E1000_RTTBCNRC_RF_INT_MASK);
+				E1000_RTTBCNRC_RF_INT_MASK);
 		bcnrc_val |= (rf_dec & E1000_RTTBCNRC_RF_DEC_MASK);
 	} else {
 		bcnrc_val = 0;
 	}
 
-	wr32(E1000_RTTDQSEL, vf); /* vf X uses queue X */
-	wr32(E1000_RTTBCNRC, bcnrc_val);
+	E1000_WRITE_REG(hw, E1000_RTTDQSEL, vf); /* vf X uses queue X */
+	/*
+	 * Set global transmit compensation time to the MMW_SIZE in RTTBCNRM
+	 * register. MMW_SIZE=0x014 if 9728-byte jumbo is supported.
+	 */
+	E1000_WRITE_REG(hw, E1000_RTTBCNRM(0), 0x14);
+	E1000_WRITE_REG(hw, E1000_RTTBCNRC, bcnrc_val);
 }
 
 static void igb_check_vf_rate_limit(struct igb_adapter *adapter)
@@ -6791,9 +9524,9 @@ static void igb_check_vf_rate_limit(struct igb_adapter *adapter)
 	int actual_link_speed, i;
 	bool reset_rate = false;
 
-	/* VF TX rate limit was not set or not supported */
+	/* VF TX rate limit was not set */
 	if ((adapter->vf_rate_link_speed == 0) ||
-	    (adapter->hw.mac.type != e1000_82576))
+		(adapter->hw.mac.type != e1000_82576))
 		return;
 
 	actual_link_speed = igb_link_mbps(adapter->link_speed);
@@ -6801,8 +9534,7 @@ static void igb_check_vf_rate_limit(struct igb_adapter *adapter)
 		reset_rate = true;
 		adapter->vf_rate_link_speed = 0;
 		dev_info(&adapter->pdev->dev,
-		         "Link speed has been changed. VF Transmit "
-		         "rate is disabled\n");
+		"Link speed has been changed. VF Transmit rate is disabled\n");
 	}
 
 	for (i = 0; i < adapter->vfs_allocated_count; i++) {
@@ -6810,8 +9542,7 @@ static void igb_check_vf_rate_limit(struct igb_adapter *adapter)
 			adapter->vf_data[i].tx_rate = 0;
 
 		igb_set_vf_rate_limit(&adapter->hw, i,
-		                      adapter->vf_data[i].tx_rate,
-		                      actual_link_speed);
+			adapter->vf_data[i].tx_rate, actual_link_speed);
 	}
 }
 
@@ -6826,8 +9557,8 @@ static int igb_ndo_set_vf_bw(struct net_device *netdev, int vf, int tx_rate)
 
 	actual_link_speed = igb_link_mbps(adapter->link_speed);
 	if ((vf >= adapter->vfs_allocated_count) ||
-	    (!(rd32(E1000_STATUS) & E1000_STATUS_LU)) ||
-	    (tx_rate < 0) || (tx_rate > actual_link_speed))
+		(!(E1000_READ_REG(hw, E1000_STATUS) & E1000_STATUS_LU)) ||
+		(tx_rate < 0) || (tx_rate > actual_link_speed))
 		return -EINVAL;
 
 	adapter->vf_rate_link_speed = actual_link_speed;
@@ -6848,12 +9579,16 @@ static int igb_ndo_get_vf_config(struct net_device *netdev,
 	ivi->tx_rate = adapter->vf_data[vf].tx_rate;
 	ivi->vlan = adapter->vf_data[vf].pf_vlan;
 	ivi->qos = adapter->vf_data[vf].pf_qos;
+#ifdef HAVE_VF_SPOOFCHK_CONFIGURE
+	ivi->spoofchk = adapter->vf_data[vf].spoofchk_enabled;
+#endif
 	return 0;
 }
-
+#endif
 static void igb_vmm_control(struct igb_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
+	int count;
 	u32 reg;
 
 	switch (hw->mac.type) {
@@ -6863,28 +9598,261 @@ static void igb_vmm_control(struct igb_adapter *adapter)
 		return;
 	case e1000_82576:
 		/* notify HW that the MAC is adding vlan tags */
-		reg = rd32(E1000_DTXCTL);
-		reg |= E1000_DTXCTL_VLAN_ADDED;
-		wr32(E1000_DTXCTL, reg);
+		reg = E1000_READ_REG(hw, E1000_DTXCTL);
+		reg |= (E1000_DTXCTL_VLAN_ADDED |
+			E1000_DTXCTL_SPOOF_INT);
+		E1000_WRITE_REG(hw, E1000_DTXCTL, reg);
 	case e1000_82580:
 		/* enable replication vlan tag stripping */
-		reg = rd32(E1000_RPLOLR);
+		reg = E1000_READ_REG(hw, E1000_RPLOLR);
 		reg |= E1000_RPLOLR_STRVLAN;
-		wr32(E1000_RPLOLR, reg);
+		E1000_WRITE_REG(hw, E1000_RPLOLR, reg);
 	case e1000_i350:
+	case e1000_i354:
 		/* none of the above registers are supported by i350 */
 		break;
 	}
 
-	if (adapter->vfs_allocated_count) {
-		igb_vmdq_set_loopback_pf(hw, true);
-		igb_vmdq_set_replication_pf(hw, true);
-		igb_vmdq_set_anti_spoofing_pf(hw, true,
-						adapter->vfs_allocated_count);
-	} else {
-		igb_vmdq_set_loopback_pf(hw, false);
-		igb_vmdq_set_replication_pf(hw, false);
+	/* Enable Malicious Driver Detection */
+	if ((adapter->vfs_allocated_count) &&
+	    (adapter->mdd)) {
+		if (hw->mac.type == e1000_i350)
+			igb_enable_mdd(adapter);
 	}
+
+		/* enable replication and loopback support */
+ 		count = adapter->vfs_allocated_count || adapter->vmdq_pools;
+		if (adapter->flags & IGB_FLAG_LOOPBACK_ENABLE && count)
+			e1000_vmdq_set_loopback_pf(hw, 1);
+		e1000_vmdq_set_anti_spoofing_pf(hw,
+			adapter->vfs_allocated_count || adapter->vmdq_pools,
+			adapter->vfs_allocated_count);
+	e1000_vmdq_set_replication_pf(hw, adapter->vfs_allocated_count ||
+				      adapter->vmdq_pools);
 }
 
+static void igb_init_fw(struct igb_adapter *adapter)
+{
+	struct e1000_fw_drv_info fw_cmd;
+	struct e1000_hw *hw = &adapter->hw;
+	int i;
+	u16 mask;
+
+	if (hw->mac.type == e1000_i210)
+		mask = E1000_SWFW_EEP_SM;
+	else
+		mask = E1000_SWFW_PHY0_SM;
+	/* i211 parts do not support this feature */
+	if (hw->mac.type == e1000_i211)
+		hw->mac.arc_subsystem_valid = false;
+
+	if (!hw->mac.ops.acquire_swfw_sync(hw, mask)) {
+		for (i = 0; i <= FW_MAX_RETRIES; i++) {
+			E1000_WRITE_REG(hw, E1000_FWSTS, E1000_FWSTS_FWRI);
+			fw_cmd.hdr.cmd = FW_CMD_DRV_INFO;
+			fw_cmd.hdr.buf_len = FW_CMD_DRV_INFO_LEN;
+			fw_cmd.hdr.cmd_or_resp.cmd_resv = FW_CMD_RESERVED;
+			fw_cmd.port_num = hw->bus.func;
+			fw_cmd.drv_version = FW_FAMILY_DRV_VER;
+			fw_cmd.hdr.checksum = 0;
+			fw_cmd.hdr.checksum = e1000_calculate_checksum((u8 *)&fw_cmd,
+			                                           (FW_HDR_LEN +
+			                                            fw_cmd.hdr.buf_len));
+			 e1000_host_interface_command(hw, (u8*)&fw_cmd,
+			                             sizeof(fw_cmd));
+			if (fw_cmd.hdr.cmd_or_resp.ret_status == FW_STATUS_SUCCESS)
+				break;
+		}
+	} else
+		dev_warn(pci_dev_to_dev(adapter->pdev),
+			 "Unable to get semaphore, firmware init failed.\n");
+	hw->mac.ops.release_swfw_sync(hw, mask);
+}
+
+static void igb_init_dmac(struct igb_adapter *adapter, u32 pba)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u32 dmac_thr;
+	u16 hwm;
+	u32 status;
+
+	if (hw->mac.type == e1000_i211)
+		return;
+
+	if (hw->mac.type > e1000_82580) {
+		if (adapter->dmac != IGB_DMAC_DISABLE) {
+			u32 reg;
+
+			/* force threshold to 0.  */
+			E1000_WRITE_REG(hw, E1000_DMCTXTH, 0);
+
+			/*
+			 * DMA Coalescing high water mark needs to be greater
+			 * than the Rx threshold. Set hwm to PBA - max frame
+			 * size in 16B units, capping it at PBA - 6KB.
+			 */
+			hwm = 64 * pba - adapter->max_frame_size / 16;
+			if (hwm < 64 * (pba - 6))
+				hwm = 64 * (pba - 6);
+			reg = E1000_READ_REG(hw, E1000_FCRTC);
+			reg &= ~E1000_FCRTC_RTH_COAL_MASK;
+			reg |= ((hwm << E1000_FCRTC_RTH_COAL_SHIFT)
+				& E1000_FCRTC_RTH_COAL_MASK);
+			E1000_WRITE_REG(hw, E1000_FCRTC, reg);
+
+			/*
+			 * Set the DMA Coalescing Rx threshold to PBA - 2 * max
+			 * frame size, capping it at PBA - 10KB.
+			 */
+			dmac_thr = pba - adapter->max_frame_size / 512;
+			if (dmac_thr < pba - 10)
+				dmac_thr = pba - 10;
+			reg = E1000_READ_REG(hw, E1000_DMACR);
+			reg &= ~E1000_DMACR_DMACTHR_MASK;
+			reg |= ((dmac_thr << E1000_DMACR_DMACTHR_SHIFT)
+				& E1000_DMACR_DMACTHR_MASK);
+
+			/* transition to L0x or L1 if available..*/
+			reg |= (E1000_DMACR_DMAC_EN | E1000_DMACR_DMAC_LX_MASK);
+
+			/* Check if status is 2.5Gb backplane connection
+			 * before configuration of watchdog timer, which is
+			 * in msec values in 12.8usec intervals
+			 * watchdog timer= msec values in 32usec intervals
+			 * for non 2.5Gb connection
+			 */
+			if (hw->mac.type == e1000_i354) {
+				status = E1000_READ_REG(hw, E1000_STATUS);
+				if ((status & E1000_STATUS_2P5_SKU) &&
+				    (!(status & E1000_STATUS_2P5_SKU_OVER)))
+					reg |= ((adapter->dmac * 5) >> 6);
+				else
+					reg |= ((adapter->dmac) >> 5);
+			} else {
+				reg |= ((adapter->dmac) >> 5);
+			}
+
+			/*
+			 * Disable BMC-to-OS Watchdog enable
+			 * on devices that support OS-to-BMC
+			 */
+			if (hw->mac.type != e1000_i354)
+				reg &= ~E1000_DMACR_DC_BMC2OSW_EN;
+			E1000_WRITE_REG(hw, E1000_DMACR, reg);
+
+			/* no lower threshold to disable coalescing(smart fifb)-UTRESH=0*/
+			E1000_WRITE_REG(hw, E1000_DMCRTRH, 0);
+
+			/* This sets the time to wait before requesting
+			 * transition to low power state to number of usecs
+			 * needed to receive 1 512 byte frame at gigabit
+			 * line rate. On i350 device, time to make transition
+			 * to Lx state is delayed by 4 usec with flush disable
+			 * bit set to avoid losing mailbox interrupts
+			 */
+			reg = E1000_READ_REG(hw, E1000_DMCTLX);
+			if (hw->mac.type == e1000_i350)
+				reg |= IGB_DMCTLX_DCFLUSH_DIS;
+
+			/* in 2.5Gb connection, TTLX unit is 0.4 usec
+			 * which is 0x4*2 = 0xA. But delay is still 4 usec
+			 */
+			if (hw->mac.type == e1000_i354) {
+				status = E1000_READ_REG(hw, E1000_STATUS);
+				if ((status & E1000_STATUS_2P5_SKU) &&
+				    (!(status & E1000_STATUS_2P5_SKU_OVER)))
+					reg |= 0xA;
+				else
+					reg |= 0x4;
+			} else {
+				reg |= 0x4;
+			}
+			E1000_WRITE_REG(hw, E1000_DMCTLX, reg);
+
+			/* free space in tx packet buffer to wake from DMA coal */
+			E1000_WRITE_REG(hw, E1000_DMCTXTH, (IGB_MIN_TXPBSIZE -
+				(IGB_TX_BUF_4096 + adapter->max_frame_size)) >> 6);
+
+			/* make low power state decision controlled by DMA coal */
+			reg = E1000_READ_REG(hw, E1000_PCIEMISC);
+			reg &= ~E1000_PCIEMISC_LX_DECISION;
+			E1000_WRITE_REG(hw, E1000_PCIEMISC, reg);
+		} /* endif adapter->dmac is not disabled */
+	} else if (hw->mac.type == e1000_82580) {
+		u32 reg = E1000_READ_REG(hw, E1000_PCIEMISC);
+		E1000_WRITE_REG(hw, E1000_PCIEMISC,
+		                reg & ~E1000_PCIEMISC_LX_DECISION);
+		E1000_WRITE_REG(hw, E1000_DMACR, 0);
+	}
+}
+
+#ifdef HAVE_I2C_SUPPORT
+/*  igb_read_i2c_byte - Reads 8 bit word over I2C
+ *  @hw: pointer to hardware structure
+ *  @byte_offset: byte offset to read
+ *  @dev_addr: device address
+ *  @data: value read
+ *
+ *  Performs byte read operation over I2C interface at
+ *  a specified device address.
+ */
+s32 igb_read_i2c_byte(struct e1000_hw *hw, u8 byte_offset,
+				u8 dev_addr, u8 *data)
+{
+	struct igb_adapter *adapter = container_of(hw, struct igb_adapter, hw);
+	struct i2c_client *this_client = adapter->i2c_client;
+	s32 status;
+	u16 swfw_mask = 0;
+
+	if (!this_client)
+		return E1000_ERR_I2C;
+
+	swfw_mask = E1000_SWFW_PHY0_SM;
+
+	if (hw->mac.ops.acquire_swfw_sync(hw, swfw_mask)
+	    != E1000_SUCCESS)
+		return E1000_ERR_SWFW_SYNC;
+
+	status = i2c_smbus_read_byte_data(this_client, byte_offset);
+	hw->mac.ops.release_swfw_sync(hw, swfw_mask);
+
+	if (status < 0)
+		return E1000_ERR_I2C;
+	else {
+		*data = status;
+		return E1000_SUCCESS;
+	}
+}
+
+/*  igb_write_i2c_byte - Writes 8 bit word over I2C
+ *  @hw: pointer to hardware structure
+ *  @byte_offset: byte offset to write
+ *  @dev_addr: device address
+ *  @data: value to write
+ *
+ *  Performs byte write operation over I2C interface at
+ *  a specified device address.
+ */
+s32 igb_write_i2c_byte(struct e1000_hw *hw, u8 byte_offset,
+				 u8 dev_addr, u8 data)
+{
+	struct igb_adapter *adapter = container_of(hw, struct igb_adapter, hw);
+	struct i2c_client *this_client = adapter->i2c_client;
+	s32 status;
+	u16 swfw_mask = E1000_SWFW_PHY0_SM;
+
+	if (!this_client)
+		return E1000_ERR_I2C;
+
+	if (hw->mac.ops.acquire_swfw_sync(hw, swfw_mask) != E1000_SUCCESS)
+		return E1000_ERR_SWFW_SYNC;
+	status = i2c_smbus_write_byte_data(this_client, byte_offset, data);
+	hw->mac.ops.release_swfw_sync(hw, swfw_mask);
+
+	if (status)
+		return E1000_ERR_I2C;
+	else
+		return E1000_SUCCESS;
+}
+#endif /*  HAVE_I2C_SUPPORT */
 /* igb_main.c */
diff --git a/drivers/net/igb/igb_param.c b/drivers/net/igb/igb_param.c
new file mode 100644
index 000000000000..14439ad58468
--- /dev/null
+++ b/drivers/net/igb/igb_param.c
@@ -0,0 +1,848 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+
+#include <linux/netdevice.h>
+
+#include "igb.h"
+
+/* This is the only thing that needs to be changed to adjust the
+ * maximum number of ports that the driver can manage.
+ */
+
+#define IGB_MAX_NIC 32
+
+#define OPTION_UNSET   -1
+#define OPTION_DISABLED 0
+#define OPTION_ENABLED  1
+#define MAX_NUM_LIST_OPTS 15
+
+/* All parameters are treated the same, as an integer array of values.
+ * This macro just reduces the need to repeat the same declaration code
+ * over and over (plus this helps to avoid typo bugs).
+ */
+
+#define IGB_PARAM_INIT { [0 ... IGB_MAX_NIC] = OPTION_UNSET }
+#ifndef module_param_array
+/* Module Parameters are always initialized to -1, so that the driver
+ * can tell the difference between no user specified value or the
+ * user asking for the default value.
+ * The true default values are loaded in when igb_check_options is called.
+ *
+ * This is a GCC extension to ANSI C.
+ * See the item "Labeled Elements in Initializers" in the section
+ * "Extensions to the C Language Family" of the GCC documentation.
+ */
+
+#define IGB_PARAM(X, desc) \
+	static const int X[IGB_MAX_NIC+1] = IGB_PARAM_INIT; \
+	MODULE_PARM(X, "1-" __MODULE_STRING(IGB_MAX_NIC) "i"); \
+	MODULE_PARM_DESC(X, desc);
+#else
+#define IGB_PARAM(X, desc) \
+	static int X[IGB_MAX_NIC+1] = IGB_PARAM_INIT; \
+	static unsigned int num_##X; \
+	module_param_array_named(X, X, int, &num_##X, 0); \
+	MODULE_PARM_DESC(X, desc);
+#endif
+
+/* Interrupt Throttle Rate (interrupts/sec)
+ *
+ * Valid Range: 100-100000 (0=off, 1=dynamic, 3=dynamic conservative)
+ */
+IGB_PARAM(InterruptThrottleRate,
+	  "Maximum interrupts per second, per vector, (max 100000), default 3=adaptive");
+#define DEFAULT_ITR                    3
+#define MAX_ITR                   100000
+/* #define MIN_ITR                      120 */
+#define MIN_ITR                      0
+/* IntMode (Interrupt Mode)
+ *
+ * Valid Range: 0 - 2
+ *
+ * Default Value: 2 (MSI-X)
+ */
+IGB_PARAM(IntMode, "Change Interrupt Mode (0=Legacy, 1=MSI, 2=MSI-X), default 2");
+#define MAX_INTMODE                    IGB_INT_MODE_MSIX
+#define MIN_INTMODE                    IGB_INT_MODE_LEGACY
+
+IGB_PARAM(Node, "set the starting node to allocate memory on, default -1");
+
+/* LLIPort (Low Latency Interrupt TCP Port)
+ *
+ * Valid Range: 0 - 65535
+ *
+ * Default Value: 0 (disabled)
+ */
+IGB_PARAM(LLIPort, "Low Latency Interrupt TCP Port (0-65535), default 0=off");
+
+#define DEFAULT_LLIPORT                0
+#define MAX_LLIPORT               0xFFFF
+#define MIN_LLIPORT                    0
+
+/* LLIPush (Low Latency Interrupt on TCP Push flag)
+ *
+ * Valid Range: 0, 1
+ *
+ * Default Value: 0 (disabled)
+ */
+IGB_PARAM(LLIPush, "Low Latency Interrupt on TCP Push flag (0,1), default 0=off");
+
+#define DEFAULT_LLIPUSH                0
+#define MAX_LLIPUSH                    1
+#define MIN_LLIPUSH                    0
+
+/* LLISize (Low Latency Interrupt on Packet Size)
+ *
+ * Valid Range: 0 - 1500
+ *
+ * Default Value: 0 (disabled)
+ */
+IGB_PARAM(LLISize, "Low Latency Interrupt on Packet Size (0-1500), default 0=off");
+
+#define DEFAULT_LLISIZE                0
+#define MAX_LLISIZE                 1500
+#define MIN_LLISIZE                    0
+
+/* RSS (Enable RSS multiqueue receive)
+ *
+ * Valid Range: 0 - 8
+ *
+ * Default Value:  1
+ */
+IGB_PARAM(RSS, "Number of Receive-Side Scaling Descriptor Queues (0-8), default 1, 0=number of cpus");
+
+#define DEFAULT_RSS       1
+#define MAX_RSS           8
+#define MIN_RSS           0
+
+/* VMDQ (Enable VMDq multiqueue receive)
+ *
+ * Valid Range: 0 - 8
+ *
+ * Default Value:  0
+ */
+IGB_PARAM(VMDQ, "Number of Virtual Machine Device Queues: 0-1 = disable, 2-8 enable, default 0");
+
+#define DEFAULT_VMDQ      0
+#define MAX_VMDQ          MAX_RSS
+#define MIN_VMDQ          0
+
+/* max_vfs (Enable SR-IOV VF devices)
+ *
+ * Valid Range: 0 - 7
+ *
+ * Default Value:  0
+ */
+IGB_PARAM(max_vfs, "Number of Virtual Functions: 0 = disable, 1-7 enable, default 0");
+
+#define DEFAULT_SRIOV     0
+#define MAX_SRIOV         7
+#define MIN_SRIOV         0
+
+/* MDD (Enable Malicious Driver Detection)
+ *
+ * Only available when SR-IOV is enabled - max_vfs is greater than 0
+ * 
+ * Valid Range: 0, 1
+ *
+ * Default Value:  1
+ */
+IGB_PARAM(MDD, "Malicious Driver Detection (0/1), default 1 = enabled. "
+	  "Only available when max_vfs is greater than 0");
+
+#ifdef DEBUG
+
+/* Disable Hardware Reset on Tx Hang
+ *
+ * Valid Range: 0, 1
+ *
+ * Default Value: 0 (disabled, i.e. h/w will reset)
+ */
+IGB_PARAM(DisableHwReset, "Disable reset of hardware on Tx hang");
+
+/* Dump Transmit and Receive buffers
+ *
+ * Valid Range: 0, 1
+ *
+ * Default Value: 0
+ */
+IGB_PARAM(DumpBuffers, "Dump Tx/Rx buffers on Tx hang or by request");
+
+#endif /* DEBUG */
+
+/* QueuePairs (Enable TX/RX queue pairs for interrupt handling)
+ *
+ * Valid Range: 0 - 1
+ *
+ * Default Value:  1
+ */
+IGB_PARAM(QueuePairs, "Enable Tx/Rx queue pairs for interrupt handling (0,1), default 1=on");
+
+#define DEFAULT_QUEUE_PAIRS           1
+#define MAX_QUEUE_PAIRS               1
+#define MIN_QUEUE_PAIRS               0
+
+/* Enable/disable EEE (a.k.a. IEEE802.3az)
+ *
+ * Valid Range: 0, 1
+ *
+ * Default Value: 1
+ */
+ IGB_PARAM(EEE, "Enable/disable on parts that support the feature");
+
+/* Enable/disable DMA Coalescing
+ *
+ * Valid Values: 0(off), 1000, 2000, 3000, 4000, 5000, 6000, 7000, 8000,
+ * 9000, 10000(msec), 250(usec), 500(usec)
+ *
+ * Default Value: 0
+ */
+ IGB_PARAM(DMAC, "Disable or set latency for DMA Coalescing ((0=off, 1000-10000(msec), 250, 500 (usec))");
+
+#ifndef IGB_NO_LRO
+/* Enable/disable Large Receive Offload
+ *
+ * Valid Values: 0(off), 1(on)
+ *
+ * Default Value: 0
+ */
+ IGB_PARAM(LRO, "Large Receive Offload (0,1), default 0=off");
+
+#endif
+struct igb_opt_list {
+	int i;
+	char *str;
+};
+struct igb_option {
+	enum { enable_option, range_option, list_option } type;
+	const char *name;
+	const char *err;
+	int def;
+	union {
+		struct { /* range_option info */
+			int min;
+			int max;
+		} r;
+		struct { /* list_option info */
+			int nr;
+			struct igb_opt_list *p;
+		} l;
+	} arg;
+};
+
+static int igb_validate_option(unsigned int *value,
+			       struct igb_option *opt,
+			       struct igb_adapter *adapter)
+{
+	if (*value == OPTION_UNSET) {
+		*value = opt->def;
+		return 0;
+	}
+
+	switch (opt->type) {
+	case enable_option:
+		switch (*value) {
+		case OPTION_ENABLED:
+			DPRINTK(PROBE, INFO, "%s Enabled\n", opt->name);
+			return 0;
+		case OPTION_DISABLED:
+			DPRINTK(PROBE, INFO, "%s Disabled\n", opt->name);
+			return 0;
+		}
+		break;
+	case range_option:
+		if (*value >= opt->arg.r.min && *value <= opt->arg.r.max) {
+			DPRINTK(PROBE, INFO,
+					"%s set to %d\n", opt->name, *value);
+			return 0;
+		}
+		break;
+	case list_option: {
+		int i;
+		struct igb_opt_list *ent;
+
+		for (i = 0; i < opt->arg.l.nr; i++) {
+			ent = &opt->arg.l.p[i];
+			if (*value == ent->i) {
+				if (ent->str[0] != '\0')
+					DPRINTK(PROBE, INFO, "%s\n", ent->str);
+				return 0;
+			}
+		}
+	}
+		break;
+	default:
+		BUG();
+	}
+
+	DPRINTK(PROBE, INFO, "Invalid %s value specified (%d) %s\n",
+	       opt->name, *value, opt->err);
+	*value = opt->def;
+	return -1;
+}
+
+/**
+ * igb_check_options - Range Checking for Command Line Parameters
+ * @adapter: board private structure
+ *
+ * This routine checks all command line parameters for valid user
+ * input.  If an invalid value is given, or if no user specified
+ * value exists, a default value is used.  The final value is stored
+ * in a variable in the adapter structure.
+ **/
+
+void igb_check_options(struct igb_adapter *adapter)
+{
+	int bd = adapter->bd_number;
+	struct e1000_hw *hw = &adapter->hw;
+
+	if (bd >= IGB_MAX_NIC) {
+		DPRINTK(PROBE, NOTICE,
+		       "Warning: no configuration for board #%d\n", bd);
+		DPRINTK(PROBE, NOTICE, "Using defaults for all values\n");
+#ifndef module_param_array
+		bd = IGB_MAX_NIC;
+#endif
+	}
+
+	{ /* Interrupt Throttling Rate */
+		struct igb_option opt = {
+			.type = range_option,
+			.name = "Interrupt Throttling Rate (ints/sec)",
+			.err  = "using default of " __MODULE_STRING(DEFAULT_ITR),
+			.def  = DEFAULT_ITR,
+			.arg  = { .r = { .min = MIN_ITR,
+					 .max = MAX_ITR } }
+		};
+
+#ifdef module_param_array
+		if (num_InterruptThrottleRate > bd) {
+#endif
+			unsigned int itr = InterruptThrottleRate[bd];
+
+			switch (itr) {
+			case 0:
+				DPRINTK(PROBE, INFO, "%s turned off\n",
+				        opt.name);
+				if (hw->mac.type >= e1000_i350)
+					adapter->dmac = IGB_DMAC_DISABLE;
+				adapter->rx_itr_setting = itr;
+				break;
+			case 1:
+				DPRINTK(PROBE, INFO, "%s set to dynamic mode\n",
+					opt.name);
+				adapter->rx_itr_setting = itr;
+				break;
+			case 3:
+				DPRINTK(PROBE, INFO,
+				        "%s set to dynamic conservative mode\n",
+					opt.name);
+				adapter->rx_itr_setting = itr;
+				break;
+			default:
+				igb_validate_option(&itr, &opt, adapter);
+				/* Save the setting, because the dynamic bits
+				 * change itr.  In case of invalid user value,
+				 * default to conservative mode, else need to
+				 * clear the lower two bits because they are
+				 * used as control */
+				if (itr == 3) {
+					adapter->rx_itr_setting = itr;
+				} else {
+					adapter->rx_itr_setting = 1000000000 /
+					                          (itr * 256);
+					adapter->rx_itr_setting &= ~3;
+				}
+				break;
+			}
+#ifdef module_param_array
+		} else {
+			adapter->rx_itr_setting = opt.def;
+		}
+#endif
+		adapter->tx_itr_setting = adapter->rx_itr_setting;
+	}
+	{ /* Interrupt Mode */
+		struct igb_option opt = {
+			.type = range_option,
+			.name = "Interrupt Mode",
+			.err  = "defaulting to 2 (MSI-X)",
+			.def  = IGB_INT_MODE_MSIX,
+			.arg  = { .r = { .min = MIN_INTMODE,
+					 .max = MAX_INTMODE } }
+		};
+
+#ifdef module_param_array
+		if (num_IntMode > bd) {
+#endif
+			unsigned int int_mode = IntMode[bd];
+			igb_validate_option(&int_mode, &opt, adapter);
+			adapter->int_mode = int_mode;
+#ifdef module_param_array
+		} else {
+			adapter->int_mode = opt.def;
+		}
+#endif
+	}
+	{ /* Low Latency Interrupt TCP Port */
+		struct igb_option opt = {
+			.type = range_option,
+			.name = "Low Latency Interrupt TCP Port",
+			.err  = "using default of " __MODULE_STRING(DEFAULT_LLIPORT),
+			.def  = DEFAULT_LLIPORT,
+			.arg  = { .r = { .min = MIN_LLIPORT,
+					 .max = MAX_LLIPORT } }
+		};
+
+#ifdef module_param_array
+		if (num_LLIPort > bd) {
+#endif
+			adapter->lli_port = LLIPort[bd];
+			if (adapter->lli_port) {
+				igb_validate_option(&adapter->lli_port, &opt,
+				        adapter);
+			} else {
+				DPRINTK(PROBE, INFO, "%s turned off\n",
+					opt.name);
+			}
+#ifdef module_param_array
+		} else {
+			adapter->lli_port = opt.def;
+		}
+#endif
+	}
+	{ /* Low Latency Interrupt on Packet Size */
+		struct igb_option opt = {
+			.type = range_option,
+			.name = "Low Latency Interrupt on Packet Size",
+			.err  = "using default of " __MODULE_STRING(DEFAULT_LLISIZE),
+			.def  = DEFAULT_LLISIZE,
+			.arg  = { .r = { .min = MIN_LLISIZE,
+					 .max = MAX_LLISIZE } }
+		};
+
+#ifdef module_param_array
+		if (num_LLISize > bd) {
+#endif
+			adapter->lli_size = LLISize[bd];
+			if (adapter->lli_size) {
+				igb_validate_option(&adapter->lli_size, &opt,
+				        adapter);
+			} else {
+				DPRINTK(PROBE, INFO, "%s turned off\n",
+					opt.name);
+			}
+#ifdef module_param_array
+		} else {
+			adapter->lli_size = opt.def;
+		}
+#endif
+	}
+	{ /* Low Latency Interrupt on TCP Push flag */
+		struct igb_option opt = {
+			.type = enable_option,
+			.name = "Low Latency Interrupt on TCP Push flag",
+			.err  = "defaulting to Disabled",
+			.def  = OPTION_DISABLED
+		};
+
+#ifdef module_param_array
+		if (num_LLIPush > bd) {
+#endif
+			unsigned int lli_push = LLIPush[bd];
+			igb_validate_option(&lli_push, &opt, adapter);
+			adapter->flags |= lli_push ? IGB_FLAG_LLI_PUSH : 0;
+#ifdef module_param_array
+		} else {
+			adapter->flags |= opt.def ? IGB_FLAG_LLI_PUSH : 0;
+		}
+#endif
+	}
+	{ /* SRIOV - Enable SR-IOV VF devices */
+		struct igb_option opt = {
+			.type = range_option,
+			.name = "max_vfs - SR-IOV VF devices",
+			.err  = "using default of " __MODULE_STRING(DEFAULT_SRIOV),
+			.def  = DEFAULT_SRIOV,
+			.arg  = { .r = { .min = MIN_SRIOV,
+					 .max = MAX_SRIOV } }
+		};
+
+#ifdef module_param_array
+		if (num_max_vfs > bd) {
+#endif
+			adapter->vfs_allocated_count = max_vfs[bd];
+			igb_validate_option(&adapter->vfs_allocated_count, &opt, adapter);
+
+#ifdef module_param_array
+		} else {
+			adapter->vfs_allocated_count = opt.def;
+		}
+#endif
+		if (adapter->vfs_allocated_count) {
+			switch (hw->mac.type) {
+			case e1000_82575:
+			case e1000_82580:
+			case e1000_i210:
+			case e1000_i211:
+			case e1000_i354:
+				adapter->vfs_allocated_count = 0;
+				DPRINTK(PROBE, INFO, "SR-IOV option max_vfs not supported.\n");
+			default:
+				break;
+			}
+		}
+	}
+	{ /* VMDQ - Enable VMDq multiqueue receive */
+		struct igb_option opt = {
+			.type = range_option,
+			.name = "VMDQ - VMDq multiqueue queue count",
+			.err  = "using default of " __MODULE_STRING(DEFAULT_VMDQ),
+			.def  = DEFAULT_VMDQ,
+			.arg  = { .r = { .min = MIN_VMDQ,
+					 .max = (MAX_VMDQ - adapter->vfs_allocated_count) } }
+		};
+		if ((hw->mac.type != e1000_i210) ||
+		    (hw->mac.type != e1000_i211)) {
+#ifdef module_param_array
+		if (num_VMDQ > bd) {
+#endif
+			adapter->vmdq_pools = (VMDQ[bd] == 1 ? 0 : VMDQ[bd]);
+			if (adapter->vfs_allocated_count && !adapter->vmdq_pools) {
+				DPRINTK(PROBE, INFO, "Enabling SR-IOV requires VMDq be set to at least 1\n");
+				adapter->vmdq_pools = 1;
+			}
+			igb_validate_option(&adapter->vmdq_pools, &opt, adapter);
+
+#ifdef module_param_array
+		} else {
+			if (!adapter->vfs_allocated_count)
+				adapter->vmdq_pools = (opt.def == 1 ? 0 : opt.def);
+			else
+				adapter->vmdq_pools = 1;
+		}
+#endif
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+		if (hw->mac.type == e1000_82575 && adapter->vmdq_pools) {
+			DPRINTK(PROBE, INFO, "VMDq not supported on this part.\n");
+			adapter->vmdq_pools = 0;
+		}
+#endif
+
+	} else {
+		DPRINTK(PROBE, INFO, "VMDq option is not supported.\n");
+		adapter->vmdq_pools = opt.def;
+	}
+	}
+	{ /* RSS - Enable RSS multiqueue receives */
+		struct igb_option opt = {
+			.type = range_option,
+			.name = "RSS - RSS multiqueue receive count",
+			.err  = "using default of " __MODULE_STRING(DEFAULT_RSS),
+			.def  = DEFAULT_RSS,
+			.arg  = { .r = { .min = MIN_RSS,
+					 .max = MAX_RSS } }
+		};
+
+		switch (hw->mac.type) {
+		case e1000_82575:
+#ifndef CONFIG_IGB_VMDQ_NETDEV
+			if (!!adapter->vmdq_pools) {
+				if (adapter->vmdq_pools <= 2) {
+					if (adapter->vmdq_pools == 2)
+						opt.arg.r.max = 3;
+				} else {
+					opt.arg.r.max = 1;
+				}
+			} else {
+				opt.arg.r.max = 4;
+			}
+#else
+			opt.arg.r.max = !!adapter->vmdq_pools ? 1 : 4;
+#endif /* CONFIG_IGB_VMDQ_NETDEV */
+			break;
+		case e1000_i210:
+			opt.arg.r.max = 4;
+			break;
+		case e1000_i211:
+			opt.arg.r.max = 2;
+			break;
+		case e1000_82576:
+#ifndef CONFIG_IGB_VMDQ_NETDEV
+			if (!!adapter->vmdq_pools)
+				opt.arg.r.max = 2;
+			break;
+#endif /* CONFIG_IGB_VMDQ_NETDEV */
+		case e1000_82580:
+		case e1000_i350:
+		case e1000_i354:
+		default:
+			if (!!adapter->vmdq_pools)
+				opt.arg.r.max = 1;
+			break;
+		}
+
+		if (adapter->int_mode != IGB_INT_MODE_MSIX) {
+			DPRINTK(PROBE, INFO, "RSS is not supported when in MSI/Legacy Interrupt mode, %s\n",
+				opt.err);
+			opt.arg.r.max = 1;
+		}
+
+#ifdef module_param_array
+		if (num_RSS > bd) {
+#endif
+			adapter->rss_queues = RSS[bd];
+			switch (adapter->rss_queues) {
+			case 1:
+				break;
+			default:
+				igb_validate_option(&adapter->rss_queues, &opt, adapter);
+				if (adapter->rss_queues)
+					break;
+			case 0:
+				adapter->rss_queues = min_t(u32, opt.arg.r.max, num_online_cpus());
+				break;
+			}
+#ifdef module_param_array
+		} else {
+			adapter->rss_queues = opt.def;
+		}
+#endif
+	}
+	{ /* QueuePairs - Enable Tx/Rx queue pairs for interrupt handling */
+		struct igb_option opt = {
+			.type = enable_option,
+			.name = "QueuePairs - Tx/Rx queue pairs for interrupt handling",
+			.err  = "defaulting to Enabled",
+			.def  = OPTION_ENABLED
+		};
+#ifdef module_param_array
+		if (num_QueuePairs > bd) {
+#endif
+			unsigned int qp = QueuePairs[bd];
+			/*
+			 * We must enable queue pairs if the number of queues
+			 * exceeds the number of available interrupts. We are
+			 * limited to 10, or 3 per unallocated vf. On I210 and
+			 * I211 devices, we are limited to 5 interrupts.
+			 * However, since I211 only supports 2 queues, we do not
+			 * need to check and override the user option.
+			 */
+			if (qp == OPTION_DISABLED) {
+				if (adapter->rss_queues > 4)
+					qp = OPTION_ENABLED;
+
+				if (adapter->vmdq_pools > 4)
+					qp = OPTION_ENABLED;
+
+				if (adapter->rss_queues > 1 &&
+				    (adapter->vmdq_pools > 3 ||
+				     adapter->vfs_allocated_count > 6))
+					qp = OPTION_ENABLED;
+
+				if (hw->mac.type == e1000_i210 &&
+				    adapter->rss_queues > 2)
+					qp = OPTION_ENABLED;
+
+				if (qp == OPTION_ENABLED)
+					DPRINTK(PROBE, INFO, "Number of queues exceeds available interrupts, %s\n",
+						opt.err);
+			}
+			igb_validate_option(&qp, &opt, adapter);
+			adapter->flags |= qp ? IGB_FLAG_QUEUE_PAIRS : 0;
+#ifdef module_param_array
+		} else {
+			adapter->flags |= opt.def ? IGB_FLAG_QUEUE_PAIRS : 0;
+		}
+#endif
+	}
+	{ /* EEE -  Enable EEE for capable adapters */
+
+		if (hw->mac.type >= e1000_i350) {
+			struct igb_option opt = {
+				.type = enable_option,
+				.name = "EEE Support",
+				.err  = "defaulting to Enabled",
+				.def  = OPTION_ENABLED
+			};
+#ifdef module_param_array
+			if (num_EEE > bd) {
+#endif
+				unsigned int eee = EEE[bd];
+				igb_validate_option(&eee, &opt, adapter);
+				adapter->flags |= eee ? IGB_FLAG_EEE : 0;
+				if (eee)
+					hw->dev_spec._82575.eee_disable = false;
+				else
+					hw->dev_spec._82575.eee_disable = true;
+
+#ifdef module_param_array
+			} else {
+				adapter->flags |= opt.def ? IGB_FLAG_EEE : 0;
+				if (adapter->flags & IGB_FLAG_EEE)
+					hw->dev_spec._82575.eee_disable = false;
+				else
+					hw->dev_spec._82575.eee_disable = true;
+			}
+#endif
+		}
+	}
+	{ /* DMAC -  Enable DMA Coalescing for capable adapters */
+
+		if (hw->mac.type >= e1000_i350) {
+			struct igb_opt_list list [] = {
+				{ IGB_DMAC_DISABLE, "DMAC Disable"},
+				{ IGB_DMAC_MIN, "DMAC 250 usec"},
+				{ IGB_DMAC_500, "DMAC 500 usec"},
+				{ IGB_DMAC_EN_DEFAULT, "DMAC 1000 usec"},
+				{ IGB_DMAC_2000, "DMAC 2000 usec"},
+				{ IGB_DMAC_3000, "DMAC 3000 usec"},
+				{ IGB_DMAC_4000, "DMAC 4000 usec"},
+				{ IGB_DMAC_5000, "DMAC 5000 usec"},
+				{ IGB_DMAC_6000, "DMAC 6000 usec"},
+				{ IGB_DMAC_7000, "DMAC 7000 usec"},
+				{ IGB_DMAC_8000, "DMAC 8000 usec"},
+				{ IGB_DMAC_9000, "DMAC 9000 usec"},
+				{ IGB_DMAC_MAX, "DMAC 10000 usec"}
+			};
+			struct igb_option opt = {
+				.type = list_option,
+				.name = "DMA Coalescing",
+				.err  = "using default of "__MODULE_STRING(IGB_DMAC_DISABLE),
+				.def  = IGB_DMAC_DISABLE,
+				.arg = { .l = { .nr = 13,
+					 	.p = list
+					}
+				}
+			};
+#ifdef module_param_array
+			if (num_DMAC > bd) {
+#endif
+				unsigned int dmac = DMAC[bd];
+				if (adapter->rx_itr_setting == IGB_DMAC_DISABLE)
+					dmac = IGB_DMAC_DISABLE;
+				igb_validate_option(&dmac, &opt, adapter);
+				switch (dmac) {
+				case IGB_DMAC_DISABLE:
+					adapter->dmac = dmac;
+					break;
+				case IGB_DMAC_MIN:
+					adapter->dmac = dmac;
+					break;
+				case IGB_DMAC_500:
+					adapter->dmac = dmac;
+					break;
+				case IGB_DMAC_EN_DEFAULT:
+					adapter->dmac = dmac;
+					break;
+				case IGB_DMAC_2000:
+					adapter->dmac = dmac;
+					break;
+				case IGB_DMAC_3000:
+					adapter->dmac = dmac;
+					break;
+				case IGB_DMAC_4000:
+					adapter->dmac = dmac;
+					break;
+				case IGB_DMAC_5000:
+					adapter->dmac = dmac;
+					break;
+				case IGB_DMAC_6000:
+					adapter->dmac = dmac;
+					break;
+				case IGB_DMAC_7000:
+					adapter->dmac = dmac;
+					break;
+				case IGB_DMAC_8000:
+					adapter->dmac = dmac;
+					break;
+				case IGB_DMAC_9000:
+					adapter->dmac = dmac;
+					break;
+				case IGB_DMAC_MAX:
+					adapter->dmac = dmac;
+					break;
+				default:
+					adapter->dmac = opt.def;
+					DPRINTK(PROBE, INFO,
+					"Invalid DMAC setting, "
+					"resetting DMAC to %d\n", opt.def);
+				}
+#ifdef module_param_array
+			} else
+				adapter->dmac = opt.def;
+#endif
+		}
+	}
+#ifndef IGB_NO_LRO
+	{ /* LRO - Enable Large Receive Offload */
+		struct igb_option opt = {
+			.type = enable_option,
+			.name = "LRO - Large Receive Offload",
+			.err  = "defaulting to Disabled",
+			.def  = OPTION_DISABLED
+		};
+		struct net_device *netdev = adapter->netdev;
+#ifdef module_param_array
+		if (num_LRO > bd) {
+#endif
+			unsigned int lro = LRO[bd];
+			igb_validate_option(&lro, &opt, adapter);
+			netdev->features |= lro ? NETIF_F_LRO : 0;
+#ifdef module_param_array
+		} else if (opt.def == OPTION_ENABLED) {
+			netdev->features |= NETIF_F_LRO;
+		}
+#endif
+	}
+#endif /* IGB_NO_LRO */
+	{ /* MDD - Enable Malicious Driver Detection. Only available when
+	     SR-IOV is enabled. */
+		struct igb_option opt = {
+			.type = enable_option,
+			.name = "Malicious Driver Detection",
+			.err  = "defaulting to 1",
+			.def  = OPTION_ENABLED,
+			.arg  = { .r = { .min = OPTION_DISABLED,
+					 .max = OPTION_ENABLED } }
+		};
+
+#ifdef module_param_array
+		if (num_MDD > bd) {
+#endif
+			adapter->mdd = MDD[bd];
+			igb_validate_option((uint *)&adapter->mdd, &opt,
+					    adapter);
+#ifdef module_param_array
+		} else {
+			adapter->mdd = opt.def;
+		}
+#endif
+	}
+}
+
diff --git a/drivers/net/igb/igb_procfs.c b/drivers/net/igb/igb_procfs.c
new file mode 100644
index 000000000000..2e7850ca1d6e
--- /dev/null
+++ b/drivers/net/igb/igb_procfs.c
@@ -0,0 +1,363 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+#include "igb.h"
+#include "e1000_82575.h"
+#include "e1000_hw.h"
+
+#ifdef IGB_PROCFS
+#ifndef IGB_HWMON
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/proc_fs.h>
+#include <linux/device.h>
+#include <linux/netdevice.h>
+
+static struct proc_dir_entry *igb_top_dir = NULL;
+
+
+bool igb_thermal_present(struct igb_adapter *adapter)
+{
+	s32 status;
+	struct e1000_hw *hw;
+
+	if (adapter == NULL)
+		return false;
+	hw = &adapter->hw;
+
+	/*
+	 * Only set I2C bit-bang mode if an external thermal sensor is
+	 * supported on this device.
+	 */
+	if (adapter->ets) {
+		status = e1000_set_i2c_bb(hw);
+		if (status != E1000_SUCCESS)
+			return false;
+	}
+
+	status = hw->mac.ops.init_thermal_sensor_thresh(hw);
+	if (status != E1000_SUCCESS)
+		return false;
+	
+	return true;
+}
+
+
+static int igb_macburn(char *page, char **start, off_t off, int count, 
+			int *eof, void *data)
+{
+	struct e1000_hw *hw;
+	struct igb_adapter *adapter = (struct igb_adapter *)data;
+	if (adapter == NULL)
+		return snprintf(page, count, "error: no adapter\n");
+
+	hw = &adapter->hw;
+	if (hw == NULL)
+		return snprintf(page, count, "error: no hw data\n");
+
+	return snprintf(page, count, "0x%02X%02X%02X%02X%02X%02X\n",
+		       (unsigned int)hw->mac.perm_addr[0],
+		       (unsigned int)hw->mac.perm_addr[1],
+		       (unsigned int)hw->mac.perm_addr[2],
+		       (unsigned int)hw->mac.perm_addr[3],
+		       (unsigned int)hw->mac.perm_addr[4],
+		       (unsigned int)hw->mac.perm_addr[5]);
+}
+
+static int igb_macadmn(char *page, char **start, off_t off, 
+		       int count, int *eof, void *data)
+{
+	struct e1000_hw *hw;
+	struct igb_adapter *adapter = (struct igb_adapter *)data;
+	if (adapter == NULL)
+		return snprintf(page, count, "error: no adapter\n");
+
+	hw = &adapter->hw;
+	if (hw == NULL)
+		return snprintf(page, count, "error: no hw data\n");
+
+	return snprintf(page, count, "0x%02X%02X%02X%02X%02X%02X\n",
+		       (unsigned int)hw->mac.addr[0],
+		       (unsigned int)hw->mac.addr[1],
+		       (unsigned int)hw->mac.addr[2],
+		       (unsigned int)hw->mac.addr[3],
+		       (unsigned int)hw->mac.addr[4],
+		       (unsigned int)hw->mac.addr[5]);
+}
+
+static int igb_numeports(char *page, char **start, off_t off, int count,
+			 int *eof, void *data)
+{
+	struct e1000_hw *hw;
+	int ports;
+	struct igb_adapter *adapter = (struct igb_adapter *)data;
+	if (adapter == NULL)
+		return snprintf(page, count, "error: no adapter\n");
+
+	hw = &adapter->hw;
+	if (hw == NULL)
+		return snprintf(page, count, "error: no hw data\n");
+
+	ports = 4;
+
+	return snprintf(page, count, "%d\n", ports);
+}
+
+static int igb_porttype(char *page, char **start, off_t off, int count,
+			int *eof, void *data)
+{
+	struct igb_adapter *adapter = (struct igb_adapter *)data;
+	if (adapter == NULL)
+		return snprintf(page, count, "error: no adapter\n");
+
+	return snprintf(page, count, "%d\n",
+			test_bit(__IGB_DOWN, &adapter->state));
+}
+
+static int igb_therm_location(char *page, char **start, off_t off, 
+				     int count, int *eof, void *data)
+{
+	struct igb_therm_proc_data *therm_data =
+		(struct igb_therm_proc_data *)data;
+
+	if (therm_data == NULL)
+		return snprintf(page, count, "error: no therm_data\n");
+
+	return snprintf(page, count, "%d\n", therm_data->sensor_data->location);
+}
+
+static int igb_therm_maxopthresh(char *page, char **start, off_t off, 
+				    int count, int *eof, void *data)
+{
+	struct igb_therm_proc_data *therm_data =
+		(struct igb_therm_proc_data *)data;
+
+	if (therm_data == NULL)
+		return snprintf(page, count, "error: no therm_data\n");
+
+	return snprintf(page, count, "%d\n",
+			therm_data->sensor_data->max_op_thresh);
+}
+
+static int igb_therm_cautionthresh(char *page, char **start, off_t off, 
+				      int count, int *eof, void *data)
+{
+	struct igb_therm_proc_data *therm_data =
+		(struct igb_therm_proc_data *)data;
+
+	if (therm_data == NULL)
+		return snprintf(page, count, "error: no therm_data\n");
+
+	return snprintf(page, count, "%d\n",
+			therm_data->sensor_data->caution_thresh);
+}
+
+static int igb_therm_temp(char *page, char **start, off_t off, 
+			     int count, int *eof, void *data)
+{
+	s32 status;
+	struct igb_therm_proc_data *therm_data =
+		(struct igb_therm_proc_data *)data;
+
+	if (therm_data == NULL)
+		return snprintf(page, count, "error: no therm_data\n");
+
+	status = e1000_get_thermal_sensor_data(therm_data->hw);
+ 	if (status != E1000_SUCCESS)
+		snprintf(page, count, "error: status %d returned\n", status);
+
+	return snprintf(page, count, "%d\n", therm_data->sensor_data->temp);
+}
+
+struct igb_proc_type{
+	char name[32];
+	int (*read)(char*, char**, off_t, int, int*, void*);
+};
+
+struct igb_proc_type igb_proc_entries[] = {
+	{"numeports", &igb_numeports},
+	{"porttype", &igb_porttype},
+	{"macburn", &igb_macburn},
+	{"macadmn", &igb_macadmn},
+	{"", NULL}
+};
+
+struct igb_proc_type igb_internal_entries[] = {
+	{"location", &igb_therm_location},
+	{"temp", &igb_therm_temp},
+	{"cautionthresh", &igb_therm_cautionthresh},
+	{"maxopthresh", &igb_therm_maxopthresh},	
+	{"", NULL}
+};
+
+void igb_del_proc_entries(struct igb_adapter *adapter)
+{
+	int index, i;
+	char buf[16];	/* much larger than the sensor number will ever be */
+
+	if (igb_top_dir == NULL)
+		return;
+
+	for (i = 0; i < E1000_MAX_SENSORS; i++) {
+		if (adapter->therm_dir[i] == NULL)
+			continue;
+
+		for (index = 0; ; index++) {
+			if (igb_internal_entries[index].read == NULL)
+				break;
+
+			 remove_proc_entry(igb_internal_entries[index].name,
+					   adapter->therm_dir[i]);
+		}
+		snprintf(buf, sizeof(buf), "sensor_%d", i);
+		remove_proc_entry(buf, adapter->info_dir);
+	}
+
+	if (adapter->info_dir != NULL) {
+		for (index = 0; ; index++) {
+			if (igb_proc_entries[index].read == NULL)
+				break;
+		        remove_proc_entry(igb_proc_entries[index].name,
+					  adapter->info_dir); 
+		}
+		remove_proc_entry("info", adapter->eth_dir);
+	}
+
+	if (adapter->eth_dir != NULL)
+		remove_proc_entry(pci_name(adapter->pdev), igb_top_dir);
+}
+
+/* called from igb_main.c */
+void igb_procfs_exit(struct igb_adapter *adapter) 
+{
+	igb_del_proc_entries(adapter);
+}
+
+int igb_procfs_topdir_init(void) 
+{
+	igb_top_dir = proc_mkdir("driver/igb", NULL);
+	if (igb_top_dir == NULL)
+		return (-ENOMEM);
+
+	return 0;
+}
+
+void igb_procfs_topdir_exit(void) 
+{
+	remove_proc_entry("driver/igb", NULL);
+}
+
+/* called from igb_main.c */
+int igb_procfs_init(struct igb_adapter *adapter) 
+{
+	int rc = 0;
+	int i;
+	int index;
+	char buf[16];	/* much larger than the sensor number will ever be */
+
+	adapter->eth_dir = NULL;
+	adapter->info_dir = NULL;
+	for (i = 0; i < E1000_MAX_SENSORS; i++)
+		adapter->therm_dir[i] = NULL;
+
+	if ( igb_top_dir == NULL ) {
+		rc = -ENOMEM;
+		goto fail;
+	}
+
+	adapter->eth_dir = proc_mkdir(pci_name(adapter->pdev), igb_top_dir);
+	if (adapter->eth_dir == NULL) {
+		rc = -ENOMEM;
+		goto fail;
+	}
+
+	adapter->info_dir = proc_mkdir("info", adapter->eth_dir);
+	if (adapter->info_dir == NULL) {
+		rc = -ENOMEM;
+		goto fail;
+	}
+	for (index = 0; ; index++) {
+		if (igb_proc_entries[index].read == NULL) {
+			break;
+		}
+		if (!(create_proc_read_entry(igb_proc_entries[index].name, 
+					   0444, 
+					   adapter->info_dir, 
+					   igb_proc_entries[index].read, 
+					   adapter))) {
+
+			rc = -ENOMEM;
+			goto fail;
+		}
+	}
+	if (igb_thermal_present(adapter) == false)
+		goto exit;
+
+	for (i = 0; i < E1000_MAX_SENSORS; i++) {
+
+		 if (adapter->hw.mac.thermal_sensor_data.sensor[i].location== 0)
+			continue;
+
+		snprintf(buf, sizeof(buf), "sensor_%d", i);
+		adapter->therm_dir[i] = proc_mkdir(buf, adapter->info_dir);
+		if (adapter->therm_dir[i] == NULL) {
+			rc = -ENOMEM;
+			goto fail;
+		}
+		for (index = 0; ; index++) {
+			if (igb_internal_entries[index].read == NULL)
+				break;
+			/*
+			 * therm_data struct contains pointer the read func
+			 * will be needing
+			 */
+			adapter->therm_data[i].hw = &adapter->hw;
+			adapter->therm_data[i].sensor_data = 
+				&adapter->hw.mac.thermal_sensor_data.sensor[i];
+
+			if (!(create_proc_read_entry(
+					   igb_internal_entries[index].name, 
+					   0444, 
+					   adapter->therm_dir[i], 
+					   igb_internal_entries[index].read, 
+					   &adapter->therm_data[i]))) {
+				rc = -ENOMEM;
+				goto fail;
+			}
+		}
+	}
+	goto exit;
+
+fail:
+	igb_del_proc_entries(adapter);
+exit:
+	return rc;
+}
+
+#endif /* !IGB_HWMON */
+#endif /* IGB_PROCFS */
diff --git a/drivers/net/igb/igb_ptp.c b/drivers/net/igb/igb_ptp.c
new file mode 100644
index 000000000000..9fe0a03ca1ba
--- /dev/null
+++ b/drivers/net/igb/igb_ptp.c
@@ -0,0 +1,944 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+/******************************************************************************
+ Copyright(c) 2011 Richard Cochran <richardcochran@gmail.com> for some of the
+ 82576 and 82580 code
+******************************************************************************/
+
+#include "igb.h"
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/pci.h>
+#include <linux/ptp_classify.h>
+
+#define INCVALUE_MASK		0x7fffffff
+#define ISGN			0x80000000
+
+/*
+ * The 82580 timesync updates the system timer every 8ns by 8ns,
+ * and this update value cannot be reprogrammed.
+ *
+ * Neither the 82576 nor the 82580 offer registers wide enough to hold
+ * nanoseconds time values for very long. For the 82580, SYSTIM always
+ * counts nanoseconds, but the upper 24 bits are not availible. The
+ * frequency is adjusted by changing the 32 bit fractional nanoseconds
+ * register, TIMINCA.
+ *
+ * For the 82576, the SYSTIM register time unit is affect by the
+ * choice of the 24 bit TININCA:IV (incvalue) field. Five bits of this
+ * field are needed to provide the nominal 16 nanosecond period,
+ * leaving 19 bits for fractional nanoseconds.
+ *
+ * We scale the NIC clock cycle by a large factor so that relatively
+ * small clock corrections can be added or subtracted at each clock
+ * tick. The drawbacks of a large factor are a) that the clock
+ * register overflows more quickly (not such a big deal) and b) that
+ * the increment per tick has to fit into 24 bits.  As a result we
+ * need to use a shift of 19 so we can fit a value of 16 into the
+ * TIMINCA register.
+ *
+ *
+ *             SYSTIMH            SYSTIML
+ *        +--------------+   +---+---+------+
+ *  82576 |      32      |   | 8 | 5 |  19  |
+ *        +--------------+   +---+---+------+
+ *         \________ 45 bits _______/  fract
+ *
+ *        +----------+---+   +--------------+
+ *  82580 |    24    | 8 |   |      32      |
+ *        +----------+---+   +--------------+
+ *          reserved  \______ 40 bits _____/
+ *
+ *
+ * The 45 bit 82576 SYSTIM overflows every
+ *   2^45 * 10^-9 / 3600 = 9.77 hours.
+ *
+ * The 40 bit 82580 SYSTIM overflows every
+ *   2^40 * 10^-9 /  60  = 18.3 minutes.
+ */
+
+#define IGB_SYSTIM_OVERFLOW_PERIOD	(HZ * 60 * 9)
+#define IGB_PTP_TX_TIMEOUT		(HZ * 15)
+#define INCPERIOD_82576			(1 << E1000_TIMINCA_16NS_SHIFT)
+#define INCVALUE_82576_MASK		((1 << E1000_TIMINCA_16NS_SHIFT) - 1)
+#define INCVALUE_82576			(16 << IGB_82576_TSYNC_SHIFT)
+#define IGB_NBITS_82580			40
+
+/*
+ * SYSTIM read access for the 82576
+ */
+
+static cycle_t igb_ptp_read_82576(const struct cyclecounter *cc)
+{
+	struct igb_adapter *igb = container_of(cc, struct igb_adapter, cc);
+	struct e1000_hw *hw = &igb->hw;
+	u64 val;
+	u32 lo, hi;
+
+	lo = E1000_READ_REG(hw, E1000_SYSTIML);
+	hi = E1000_READ_REG(hw, E1000_SYSTIMH);
+
+	val = ((u64) hi) << 32;
+	val |= lo;
+
+	return val;
+}
+
+/*
+ * SYSTIM read access for the 82580
+ */
+
+static cycle_t igb_ptp_read_82580(const struct cyclecounter *cc)
+{
+	struct igb_adapter *igb = container_of(cc, struct igb_adapter, cc);
+	struct e1000_hw *hw = &igb->hw;
+	u64 val;
+	u32 lo, hi;
+
+	/* The timestamp latches on lowest register read. For the 82580
+	 * the lowest register is SYSTIMR instead of SYSTIML.  However we only
+	 * need to provide nanosecond resolution, so we just ignore it.
+	 */
+	E1000_READ_REG(hw, E1000_SYSTIMR);
+	lo = E1000_READ_REG(hw, E1000_SYSTIML);
+	hi = E1000_READ_REG(hw, E1000_SYSTIMH);
+
+	val = ((u64) hi) << 32;
+	val |= lo;
+
+	return val;
+}
+
+/*
+ * SYSTIM read access for I210/I211
+ */
+
+static void igb_ptp_read_i210(struct igb_adapter *adapter, struct timespec *ts)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u32 sec, nsec;
+
+	/* The timestamp latches on lowest register read. For I210/I211, the
+	 * lowest register is SYSTIMR. Since we only need to provide nanosecond
+	 * resolution, we can ignore it.
+	 */
+	E1000_READ_REG(hw, E1000_SYSTIMR);
+	nsec = E1000_READ_REG(hw, E1000_SYSTIML);
+	sec = E1000_READ_REG(hw, E1000_SYSTIMH);
+
+	ts->tv_sec = sec;
+	ts->tv_nsec = nsec;
+}
+
+static void igb_ptp_write_i210(struct igb_adapter *adapter,
+			       const struct timespec *ts)
+{
+	struct e1000_hw *hw = &adapter->hw;
+
+	/*
+	 * Writing the SYSTIMR register is not necessary as it only provides
+	 * sub-nanosecond resolution.
+	 */
+	E1000_WRITE_REG(hw, E1000_SYSTIML, ts->tv_nsec);
+	E1000_WRITE_REG(hw, E1000_SYSTIMH, ts->tv_sec);
+}
+
+/**
+ * igb_ptp_systim_to_hwtstamp - convert system time value to hw timestamp
+ * @adapter: board private structure
+ * @hwtstamps: timestamp structure to update
+ * @systim: unsigned 64bit system time value.
+ *
+ * We need to convert the system time value stored in the RX/TXSTMP registers
+ * into a hwtstamp which can be used by the upper level timestamping functions.
+ *
+ * The 'tmreg_lock' spinlock is used to protect the consistency of the
+ * system time value. This is needed because reading the 64 bit time
+ * value involves reading two (or three) 32 bit registers. The first
+ * read latches the value. Ditto for writing.
+ *
+ * In addition, here have extended the system time with an overflow
+ * counter in software.
+ **/
+static void igb_ptp_systim_to_hwtstamp(struct igb_adapter *adapter,
+				       struct skb_shared_hwtstamps *hwtstamps,
+				       u64 systim)
+{
+	unsigned long flags;
+	u64 ns;
+
+	switch (adapter->hw.mac.type) {
+	case e1000_82576:
+	case e1000_82580:
+	case e1000_i350:
+	case e1000_i354:
+		spin_lock_irqsave(&adapter->tmreg_lock, flags);
+
+		ns = timecounter_cyc2time(&adapter->tc, systim);
+
+		spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
+
+		memset(hwtstamps, 0, sizeof(*hwtstamps));
+		hwtstamps->hwtstamp = ns_to_ktime(ns);
+		break;
+	case e1000_i210:
+	case e1000_i211:
+		memset(hwtstamps, 0, sizeof(*hwtstamps));
+		/* Upper 32 bits contain s, lower 32 bits contain ns. */
+		hwtstamps->hwtstamp = ktime_set(systim >> 32,
+						systim & 0xFFFFFFFF);
+		break;
+	default:
+		break;
+	}
+}
+
+/*
+ * PTP clock operations
+ */
+
+static int igb_ptp_adjfreq_82576(struct ptp_clock_info *ptp, s32 ppb)
+{
+	struct igb_adapter *igb = container_of(ptp, struct igb_adapter,
+					       ptp_caps);
+	struct e1000_hw *hw = &igb->hw;
+	int neg_adj = 0;
+	u64 rate;
+	u32 incvalue;
+
+	if (ppb < 0) {
+		neg_adj = 1;
+		ppb = -ppb;
+	}
+	rate = ppb;
+	rate <<= 14;
+	rate = div_u64(rate, 1953125);
+
+	incvalue = 16 << IGB_82576_TSYNC_SHIFT;
+
+	if (neg_adj)
+		incvalue -= rate;
+	else
+		incvalue += rate;
+
+	E1000_WRITE_REG(hw, E1000_TIMINCA, INCPERIOD_82576 | (incvalue & INCVALUE_82576_MASK));
+
+	return 0;
+}
+
+static int igb_ptp_adjfreq_82580(struct ptp_clock_info *ptp, s32 ppb)
+{
+	struct igb_adapter *igb = container_of(ptp, struct igb_adapter,
+					       ptp_caps);
+	struct e1000_hw *hw = &igb->hw;
+	int neg_adj = 0;
+	u64 rate;
+	u32 inca;
+
+	if (ppb < 0) {
+		neg_adj = 1;
+		ppb = -ppb;
+	}
+	rate = ppb;
+	rate <<= 26;
+	rate = div_u64(rate, 1953125);
+
+	/* At 2.5G speeds, the TIMINCA register on I354 updates the clock 2.5x
+	 * as quickly. Account for this by dividing the adjustment by 2.5.
+	 */
+	if (hw->mac.type == e1000_i354) {
+		u32 status = E1000_READ_REG(hw, E1000_STATUS);
+
+		if ((status & E1000_STATUS_2P5_SKU) &&
+		    !(status & E1000_STATUS_2P5_SKU_OVER)) {
+			rate <<= 1;
+			rate = div_u64(rate, 5);
+		}
+	}
+
+	inca = rate & INCVALUE_MASK;
+	if (neg_adj)
+		inca |= ISGN;
+
+	E1000_WRITE_REG(hw, E1000_TIMINCA, inca);
+
+	return 0;
+}
+
+static int igb_ptp_adjtime_82576(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct igb_adapter *igb = container_of(ptp, struct igb_adapter,
+					       ptp_caps);
+	unsigned long flags;
+	s64 now;
+
+	spin_lock_irqsave(&igb->tmreg_lock, flags);
+
+	now = timecounter_read(&igb->tc);
+	now += delta;
+	timecounter_init(&igb->tc, &igb->cc, now);
+
+	spin_unlock_irqrestore(&igb->tmreg_lock, flags);
+
+	return 0;
+}
+
+static int igb_ptp_adjtime_i210(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct igb_adapter *igb = container_of(ptp, struct igb_adapter,
+					       ptp_caps);
+	unsigned long flags;
+	struct timespec now, then = ns_to_timespec(delta);
+
+	spin_lock_irqsave(&igb->tmreg_lock, flags);
+
+	igb_ptp_read_i210(igb, &now);
+	now = timespec_add(now, then);
+	igb_ptp_write_i210(igb, (const struct timespec *)&now);
+
+	spin_unlock_irqrestore(&igb->tmreg_lock, flags);
+
+	return 0;
+}
+
+static int igb_ptp_gettime_82576(struct ptp_clock_info *ptp,
+				 struct timespec *ts)
+{
+	struct igb_adapter *igb = container_of(ptp, struct igb_adapter,
+					       ptp_caps);
+	unsigned long flags;
+	u64 ns;
+	u32 remainder;
+
+	spin_lock_irqsave(&igb->tmreg_lock, flags);
+
+	ns = timecounter_read(&igb->tc);
+
+	spin_unlock_irqrestore(&igb->tmreg_lock, flags);
+
+	ts->tv_sec = div_u64_rem(ns, 1000000000, &remainder);
+	ts->tv_nsec = remainder;
+
+	return 0;
+}
+
+static int igb_ptp_gettime_i210(struct ptp_clock_info *ptp,
+				struct timespec *ts)
+{
+	struct igb_adapter *igb = container_of(ptp, struct igb_adapter,
+					       ptp_caps);
+	unsigned long flags;
+
+	spin_lock_irqsave(&igb->tmreg_lock, flags);
+
+	igb_ptp_read_i210(igb, ts);
+
+	spin_unlock_irqrestore(&igb->tmreg_lock, flags);
+
+	return 0;
+}
+
+static int igb_ptp_settime_82576(struct ptp_clock_info *ptp,
+				 const struct timespec *ts)
+{
+	struct igb_adapter *igb = container_of(ptp, struct igb_adapter,
+					       ptp_caps);
+	unsigned long flags;
+	u64 ns;
+
+	ns = ts->tv_sec * 1000000000ULL;
+	ns += ts->tv_nsec;
+
+	spin_lock_irqsave(&igb->tmreg_lock, flags);
+
+	timecounter_init(&igb->tc, &igb->cc, ns);
+
+	spin_unlock_irqrestore(&igb->tmreg_lock, flags);
+
+	return 0;
+}
+
+static int igb_ptp_settime_i210(struct ptp_clock_info *ptp,
+				const struct timespec *ts)
+{
+	struct igb_adapter *igb = container_of(ptp, struct igb_adapter,
+					       ptp_caps);
+	unsigned long flags;
+
+	spin_lock_irqsave(&igb->tmreg_lock, flags);
+
+	igb_ptp_write_i210(igb, ts);
+
+	spin_unlock_irqrestore(&igb->tmreg_lock, flags);
+
+	return 0;
+}
+
+static int igb_ptp_enable(struct ptp_clock_info *ptp,
+			  struct ptp_clock_request *rq, int on)
+{
+	return -EOPNOTSUPP;
+}
+
+/**
+ * igb_ptp_tx_work
+ * @work: pointer to work struct
+ *
+ * This work function polls the TSYNCTXCTL valid bit to determine when a
+ * timestamp has been taken for the current stored skb.
+ */
+void igb_ptp_tx_work(struct work_struct *work)
+{
+	struct igb_adapter *adapter = container_of(work, struct igb_adapter,
+						   ptp_tx_work);
+	struct e1000_hw *hw = &adapter->hw;
+	u32 tsynctxctl;
+
+	if (!adapter->ptp_tx_skb)
+		return;
+
+	if (time_is_before_jiffies(adapter->ptp_tx_start +
+				   IGB_PTP_TX_TIMEOUT)) {
+		dev_kfree_skb_any(adapter->ptp_tx_skb);
+		adapter->ptp_tx_skb = NULL;
+		adapter->tx_hwtstamp_timeouts++;
+		dev_warn(&adapter->pdev->dev, "clearing Tx timestamp hang");
+		return;
+	}
+
+	tsynctxctl = E1000_READ_REG(hw, E1000_TSYNCTXCTL);
+	if (tsynctxctl & E1000_TSYNCTXCTL_VALID)
+		igb_ptp_tx_hwtstamp(adapter);
+	else
+		/* reschedule to check later */
+		schedule_work(&adapter->ptp_tx_work);
+}
+
+static void igb_ptp_overflow_check(struct work_struct *work)
+{
+	struct igb_adapter *igb =
+		container_of(work, struct igb_adapter, ptp_overflow_work.work);
+	struct timespec ts;
+
+	igb->ptp_caps.gettime(&igb->ptp_caps, &ts);
+
+	pr_debug("igb overflow check at %ld.%09lu\n", ts.tv_sec, ts.tv_nsec);
+
+	schedule_delayed_work(&igb->ptp_overflow_work,
+			      IGB_SYSTIM_OVERFLOW_PERIOD);
+}
+
+/**
+ * igb_ptp_rx_hang - detect error case when Rx timestamp registers latched
+ * @adapter: private network adapter structure
+ *
+ * This watchdog task is scheduled to detect error case where hardware has
+ * dropped an Rx packet that was timestamped when the ring is full. The
+ * particular error is rare but leaves the device in a state unable to timestamp
+ * any future packets.
+ */
+void igb_ptp_rx_hang(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	struct igb_ring *rx_ring;
+	u32 tsyncrxctl = E1000_READ_REG(hw, E1000_TSYNCRXCTL);
+	unsigned long rx_event;
+	int n;
+
+	if (hw->mac.type != e1000_82576)
+		return;
+
+	/* If we don't have a valid timestamp in the registers, just update the
+	 * timeout counter and exit
+	 */
+	if (!(tsyncrxctl & E1000_TSYNCRXCTL_VALID)) {
+		adapter->last_rx_ptp_check = jiffies;
+		return;
+	}
+
+	/* Determine the most recent watchdog or rx_timestamp event */
+	rx_event = adapter->last_rx_ptp_check;
+	for (n = 0; n < adapter->num_rx_queues; n++) {
+		rx_ring = adapter->rx_ring[n];
+		if (time_after(rx_ring->last_rx_timestamp, rx_event))
+			rx_event = rx_ring->last_rx_timestamp;
+	}
+
+	/* Only need to read the high RXSTMP register to clear the lock */
+	if (time_is_before_jiffies(rx_event + 5 * HZ)) {
+		E1000_READ_REG(hw, E1000_RXSTMPH);
+		adapter->last_rx_ptp_check = jiffies;
+		adapter->rx_hwtstamp_cleared++;
+		dev_warn(&adapter->pdev->dev, "clearing Rx timestamp hang");
+	}
+}
+
+/**
+ * igb_ptp_tx_hwtstamp - utility function which checks for TX time stamp
+ * @adapter: Board private structure.
+ *
+ * If we were asked to do hardware stamping and such a time stamp is
+ * available, then it must have been for this skb here because we only
+ * allow only one such packet into the queue.
+ */
+void igb_ptp_tx_hwtstamp(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	struct skb_shared_hwtstamps shhwtstamps;
+	u64 regval;
+
+	regval = E1000_READ_REG(hw, E1000_TXSTMPL);
+	regval |= (u64)E1000_READ_REG(hw, E1000_TXSTMPH) << 32;
+
+	igb_ptp_systim_to_hwtstamp(adapter, &shhwtstamps, regval);
+	skb_tstamp_tx(adapter->ptp_tx_skb, &shhwtstamps);
+	dev_kfree_skb_any(adapter->ptp_tx_skb);
+	adapter->ptp_tx_skb = NULL;
+}
+
+/**
+ * igb_ptp_rx_pktstamp - retrieve Rx per packet timestamp
+ * @q_vector: Pointer to interrupt specific structure
+ * @va: Pointer to address containing Rx buffer
+ * @skb: Buffer containing timestamp and packet
+ *
+ * This function is meant to retrieve a timestamp from the first buffer of an
+ * incoming frame.  The value is stored in little endian format starting on
+ * byte 8.
+ */
+void igb_ptp_rx_pktstamp(struct igb_q_vector *q_vector,
+			 unsigned char *va,
+			 struct sk_buff *skb)
+{
+	__le64 *regval = (__le64 *)va;
+
+	/*
+	 * The timestamp is recorded in little endian format.
+	 * DWORD: 0        1        2        3
+	 * Field: Reserved Reserved SYSTIML  SYSTIMH
+	 */
+	igb_ptp_systim_to_hwtstamp(q_vector->adapter, skb_hwtstamps(skb),
+				   le64_to_cpu(regval[1]));
+}
+
+/**
+ * igb_ptp_rx_rgtstamp - retrieve Rx timestamp stored in register
+ * @q_vector: Pointer to interrupt specific structure
+ * @skb: Buffer containing timestamp and packet
+ *
+ * This function is meant to retrieve a timestamp from the internal registers
+ * of the adapter and store it in the skb.
+ */
+void igb_ptp_rx_rgtstamp(struct igb_q_vector *q_vector,
+			 struct sk_buff *skb)
+{
+	struct igb_adapter *adapter = q_vector->adapter;
+	struct e1000_hw *hw = &adapter->hw;
+	u64 regval;
+
+	/*
+	 * If this bit is set, then the RX registers contain the time stamp. No
+	 * other packet will be time stamped until we read these registers, so
+	 * read the registers to make them available again. Because only one
+	 * packet can be time stamped at a time, we know that the register
+	 * values must belong to this one here and therefore we don't need to
+	 * compare any of the additional attributes stored for it.
+	 *
+	 * If nothing went wrong, then it should have a shared tx_flags that we
+	 * can turn into a skb_shared_hwtstamps.
+	 */
+	if (!(E1000_READ_REG(hw, E1000_TSYNCRXCTL) & E1000_TSYNCRXCTL_VALID))
+		return;
+
+	regval = E1000_READ_REG(hw, E1000_RXSTMPL);
+	regval |= (u64)E1000_READ_REG(hw, E1000_RXSTMPH) << 32;
+
+	igb_ptp_systim_to_hwtstamp(adapter, skb_hwtstamps(skb), regval);
+}
+
+/**
+ * igb_ptp_hwtstamp_ioctl - control hardware time stamping
+ * @netdev:
+ * @ifreq:
+ * @cmd:
+ *
+ * Outgoing time stamping can be enabled and disabled. Play nice and
+ * disable it when requested, although it shouldn't case any overhead
+ * when no packet needs it. At most one packet in the queue may be
+ * marked for time stamping, otherwise it would be impossible to tell
+ * for sure to which packet the hardware time stamp belongs.
+ *
+ * Incoming time stamping has to be configured via the hardware
+ * filters. Not all combinations are supported, in particular event
+ * type has to be specified. Matching the kind of event packet is
+ * not supported, with the exception of "all V2 events regardless of
+ * level 2 or 4".
+ *
+ **/
+int igb_ptp_hwtstamp_ioctl(struct net_device *netdev,
+			   struct ifreq *ifr, int cmd)
+{
+	struct igb_adapter *adapter = netdev_priv(netdev);
+	struct e1000_hw *hw = &adapter->hw;
+	struct hwtstamp_config config;
+	u32 tsync_tx_ctl = E1000_TSYNCTXCTL_ENABLED;
+	u32 tsync_rx_ctl = E1000_TSYNCRXCTL_ENABLED;
+	u32 tsync_rx_cfg = 0;
+	bool is_l4 = false;
+	bool is_l2 = false;
+	u32 regval;
+
+	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
+		return -EFAULT;
+
+	/* reserved for future extensions */
+	if (config.flags)
+		return -EINVAL;
+
+	switch (config.tx_type) {
+	case HWTSTAMP_TX_OFF:
+		tsync_tx_ctl = 0;
+	case HWTSTAMP_TX_ON:
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	switch (config.rx_filter) {
+	case HWTSTAMP_FILTER_NONE:
+		tsync_rx_ctl = 0;
+		break;
+	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+		tsync_rx_ctl |= E1000_TSYNCRXCTL_TYPE_L4_V1;
+		tsync_rx_cfg = E1000_TSYNCRXCFG_PTP_V1_SYNC_MESSAGE;
+		is_l4 = true;
+		break;
+	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+		tsync_rx_ctl |= E1000_TSYNCRXCTL_TYPE_L4_V1;
+		tsync_rx_cfg = E1000_TSYNCRXCFG_PTP_V1_DELAY_REQ_MESSAGE;
+		is_l4 = true;
+		break;
+	case HWTSTAMP_FILTER_PTP_V2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+		tsync_rx_ctl |= E1000_TSYNCRXCTL_TYPE_EVENT_V2;
+		config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
+		is_l2 = true;
+		is_l4 = true;
+		break;
+	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+	case HWTSTAMP_FILTER_ALL:
+		/*
+		 * 82576 cannot timestamp all packets, which it needs to do to
+		 * support both V1 Sync and Delay_Req messages
+		 */
+		if (hw->mac.type != e1000_82576) {
+			tsync_rx_ctl |= E1000_TSYNCRXCTL_TYPE_ALL;
+			config.rx_filter = HWTSTAMP_FILTER_ALL;
+			break;
+		}
+		/* fall through */
+	default:
+		config.rx_filter = HWTSTAMP_FILTER_NONE;
+		return -ERANGE;
+	}
+
+	if (hw->mac.type == e1000_82575) {
+		if (tsync_rx_ctl | tsync_tx_ctl)
+			return -EINVAL;
+		return 0;
+	}
+
+	/*
+	 * Per-packet timestamping only works if all packets are
+	 * timestamped, so enable timestamping in all packets as
+	 * long as one rx filter was configured.
+	 */
+	if ((hw->mac.type >= e1000_82580) && tsync_rx_ctl) {
+		tsync_rx_ctl = E1000_TSYNCRXCTL_ENABLED;
+		tsync_rx_ctl |= E1000_TSYNCRXCTL_TYPE_ALL;
+		config.rx_filter = HWTSTAMP_FILTER_ALL;
+		is_l2 = true;
+		is_l4 = true;
+
+		if ((hw->mac.type == e1000_i210) ||
+		    (hw->mac.type == e1000_i211)) {
+			regval = E1000_READ_REG(hw, E1000_RXPBS);
+			regval |= E1000_RXPBS_CFG_TS_EN;
+			E1000_WRITE_REG(hw, E1000_RXPBS, regval);
+		}
+	}
+
+	/* enable/disable TX */
+	regval = E1000_READ_REG(hw, E1000_TSYNCTXCTL);
+	regval &= ~E1000_TSYNCTXCTL_ENABLED;
+	regval |= tsync_tx_ctl;
+	E1000_WRITE_REG(hw, E1000_TSYNCTXCTL, regval);
+
+	/* enable/disable RX */
+	regval = E1000_READ_REG(hw, E1000_TSYNCRXCTL);
+	regval &= ~(E1000_TSYNCRXCTL_ENABLED | E1000_TSYNCRXCTL_TYPE_MASK);
+	regval |= tsync_rx_ctl;
+	E1000_WRITE_REG(hw, E1000_TSYNCRXCTL, regval);
+
+	/* define which PTP packets are time stamped */
+	E1000_WRITE_REG(hw, E1000_TSYNCRXCFG, tsync_rx_cfg);
+
+	/* define ethertype filter for timestamped packets */
+	if (is_l2)
+		E1000_WRITE_REG(hw, E1000_ETQF(3),
+		     (E1000_ETQF_FILTER_ENABLE | /* enable filter */
+		      E1000_ETQF_1588 | /* enable timestamping */
+		      ETH_P_1588));     /* 1588 eth protocol type */
+	else
+		E1000_WRITE_REG(hw, E1000_ETQF(3), 0);
+
+	/* L4 Queue Filter[3]: filter by destination port and protocol */
+	if (is_l4) {
+		u32 ftqf = (IPPROTO_UDP /* UDP */
+			| E1000_FTQF_VF_BP /* VF not compared */
+			| E1000_FTQF_1588_TIME_STAMP /* Enable Timestamping */
+			| E1000_FTQF_MASK); /* mask all inputs */
+		ftqf &= ~E1000_FTQF_MASK_PROTO_BP; /* enable protocol check */
+
+		E1000_WRITE_REG(hw, E1000_IMIR(3), htons(PTP_EV_PORT));
+		E1000_WRITE_REG(hw, E1000_IMIREXT(3),
+		     (E1000_IMIREXT_SIZE_BP | E1000_IMIREXT_CTRL_BP));
+		if (hw->mac.type == e1000_82576) {
+			/* enable source port check */
+			E1000_WRITE_REG(hw, E1000_SPQF(3), htons(PTP_EV_PORT));
+			ftqf &= ~E1000_FTQF_MASK_SOURCE_PORT_BP;
+		}
+		E1000_WRITE_REG(hw, E1000_FTQF(3), ftqf);
+	} else {
+		E1000_WRITE_REG(hw, E1000_FTQF(3), E1000_FTQF_MASK);
+	}
+	E1000_WRITE_FLUSH(hw);
+
+	/* clear TX/RX time stamp registers, just to be sure */
+	regval = E1000_READ_REG(hw, E1000_TXSTMPL);
+	regval = E1000_READ_REG(hw, E1000_TXSTMPH);
+	regval = E1000_READ_REG(hw, E1000_RXSTMPL);
+	regval = E1000_READ_REG(hw, E1000_RXSTMPH);
+
+	return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
+		-EFAULT : 0;
+}
+
+void igb_ptp_init(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	struct net_device *netdev = adapter->netdev;
+
+	switch (hw->mac.type) {
+	case e1000_82576:
+		snprintf(adapter->ptp_caps.name, 16, "%pm", netdev->dev_addr);
+		adapter->ptp_caps.owner = THIS_MODULE;
+		adapter->ptp_caps.max_adj = 999999881;
+		adapter->ptp_caps.n_ext_ts = 0;
+		adapter->ptp_caps.pps = 0;
+		adapter->ptp_caps.adjfreq = igb_ptp_adjfreq_82576;
+		adapter->ptp_caps.adjtime = igb_ptp_adjtime_82576;
+		adapter->ptp_caps.gettime = igb_ptp_gettime_82576;
+		adapter->ptp_caps.settime = igb_ptp_settime_82576;
+		adapter->ptp_caps.enable = igb_ptp_enable;
+		adapter->cc.read = igb_ptp_read_82576;
+		adapter->cc.mask = CLOCKSOURCE_MASK(64);
+		adapter->cc.mult = 1;
+		adapter->cc.shift = IGB_82576_TSYNC_SHIFT;
+		/* Dial the nominal frequency. */
+		E1000_WRITE_REG(hw, E1000_TIMINCA, INCPERIOD_82576 |
+						   INCVALUE_82576);
+		break;
+	case e1000_82580:
+	case e1000_i350:
+	case e1000_i354:
+		snprintf(adapter->ptp_caps.name, 16, "%pm", netdev->dev_addr);
+		adapter->ptp_caps.owner = THIS_MODULE;
+		adapter->ptp_caps.max_adj = 62499999;
+		adapter->ptp_caps.n_ext_ts = 0;
+		adapter->ptp_caps.pps = 0;
+		adapter->ptp_caps.adjfreq = igb_ptp_adjfreq_82580;
+		adapter->ptp_caps.adjtime = igb_ptp_adjtime_82576;
+		adapter->ptp_caps.gettime = igb_ptp_gettime_82576;
+		adapter->ptp_caps.settime = igb_ptp_settime_82576;
+		adapter->ptp_caps.enable = igb_ptp_enable;
+		adapter->cc.read = igb_ptp_read_82580;
+		adapter->cc.mask = CLOCKSOURCE_MASK(IGB_NBITS_82580);
+		adapter->cc.mult = 1;
+		adapter->cc.shift = 0;
+		/* Enable the timer functions by clearing bit 31. */
+		E1000_WRITE_REG(hw, E1000_TSAUXC, 0x0);
+		break;
+	case e1000_i210:
+	case e1000_i211:
+		snprintf(adapter->ptp_caps.name, 16, "%pm", netdev->dev_addr);
+		adapter->ptp_caps.owner = THIS_MODULE;
+		adapter->ptp_caps.max_adj = 62499999;
+		adapter->ptp_caps.n_ext_ts = 0;
+		adapter->ptp_caps.pps = 0;
+		adapter->ptp_caps.adjfreq = igb_ptp_adjfreq_82580;
+		adapter->ptp_caps.adjtime = igb_ptp_adjtime_i210;
+		adapter->ptp_caps.gettime = igb_ptp_gettime_i210;
+		adapter->ptp_caps.settime = igb_ptp_settime_i210;
+		adapter->ptp_caps.enable = igb_ptp_enable;
+		/* Enable the timer functions by clearing bit 31. */
+		E1000_WRITE_REG(hw, E1000_TSAUXC, 0x0);
+		break;
+	default:
+		adapter->ptp_clock = NULL;
+		return;
+	}
+
+	E1000_WRITE_FLUSH(hw);
+
+	spin_lock_init(&adapter->tmreg_lock);
+	INIT_WORK(&adapter->ptp_tx_work, igb_ptp_tx_work);
+
+	/* Initialize the clock and overflow work for devices that need it. */
+	if ((hw->mac.type == e1000_i210) || (hw->mac.type == e1000_i211)) {
+		struct timespec ts = ktime_to_timespec(ktime_get_real());
+
+		igb_ptp_settime_i210(&adapter->ptp_caps, &ts);
+	} else {
+		timecounter_init(&adapter->tc, &adapter->cc,
+				 ktime_to_ns(ktime_get_real()));
+
+		INIT_DELAYED_WORK(&adapter->ptp_overflow_work,
+				  igb_ptp_overflow_check);
+
+		schedule_delayed_work(&adapter->ptp_overflow_work,
+				      IGB_SYSTIM_OVERFLOW_PERIOD);
+	}
+
+	/* Initialize the time sync interrupts for devices that support it. */
+	if (hw->mac.type >= e1000_82580) {
+		E1000_WRITE_REG(hw, E1000_TSIM, E1000_TSIM_TXTS);
+		E1000_WRITE_REG(hw, E1000_IMS, E1000_IMS_TS);
+	}
+
+	adapter->ptp_clock = ptp_clock_register(&adapter->ptp_caps,
+						&adapter->pdev->dev);
+	if (IS_ERR(adapter->ptp_clock)) {
+		adapter->ptp_clock = NULL;
+		dev_err(&adapter->pdev->dev, "ptp_clock_register failed\n");
+	} else {
+		dev_info(&adapter->pdev->dev, "added PHC on %s\n",
+			 adapter->netdev->name);
+		adapter->flags |= IGB_FLAG_PTP;
+	}
+}
+
+/**
+ * igb_ptp_stop - Disable PTP device and stop the overflow check.
+ * @adapter: Board private structure.
+ *
+ * This function stops the PTP support and cancels the delayed work.
+ **/
+void igb_ptp_stop(struct igb_adapter *adapter)
+{
+	switch (adapter->hw.mac.type) {
+	case e1000_82576:
+	case e1000_82580:
+	case e1000_i350:
+	case e1000_i354:
+		cancel_delayed_work_sync(&adapter->ptp_overflow_work);
+		break;
+	case e1000_i210:
+	case e1000_i211:
+		/* No delayed work to cancel. */
+		break;
+	default:
+		return;
+	}
+
+	cancel_work_sync(&adapter->ptp_tx_work);
+	if (adapter->ptp_tx_skb) {
+		dev_kfree_skb_any(adapter->ptp_tx_skb);
+		adapter->ptp_tx_skb = NULL;
+	}
+
+	if (adapter->ptp_clock) {
+		ptp_clock_unregister(adapter->ptp_clock);
+		dev_info(&adapter->pdev->dev, "removed PHC on %s\n",
+			 adapter->netdev->name);
+		adapter->flags &= ~IGB_FLAG_PTP;
+	}
+}
+
+/**
+ * igb_ptp_reset - Re-enable the adapter for PTP following a reset.
+ * @adapter: Board private structure.
+ *
+ * This function handles the reset work required to re-enable the PTP device.
+ **/
+void igb_ptp_reset(struct igb_adapter *adapter)
+{
+	struct e1000_hw *hw = &adapter->hw;
+
+	if (!(adapter->flags & IGB_FLAG_PTP))
+		return;
+
+	switch (adapter->hw.mac.type) {
+	case e1000_82576:
+		/* Dial the nominal frequency. */
+		E1000_WRITE_REG(hw, E1000_TIMINCA, INCPERIOD_82576 |
+						   INCVALUE_82576);
+		break;
+	case e1000_82580:
+	case e1000_i350:
+	case e1000_i354:
+	case e1000_i210:
+	case e1000_i211:
+		/* Enable the timer functions and interrupts. */
+		E1000_WRITE_REG(hw, E1000_TSAUXC, 0x0);
+		E1000_WRITE_REG(hw, E1000_TSIM, E1000_TSIM_TXTS);
+		E1000_WRITE_REG(hw, E1000_IMS, E1000_IMS_TS);
+		break;
+	default:
+		/* No work to do. */
+		return;
+	}
+
+	/* Re-initialize the timer. */
+	if ((hw->mac.type == e1000_i210) || (hw->mac.type == e1000_i211)) {
+		struct timespec ts = ktime_to_timespec(ktime_get_real());
+
+		igb_ptp_settime_i210(&adapter->ptp_caps, &ts);
+	} else {
+		timecounter_init(&adapter->tc, &adapter->cc,
+				 ktime_to_ns(ktime_get_real()));
+	}
+}
diff --git a/drivers/net/igb/igb_regtest.h b/drivers/net/igb/igb_regtest.h
new file mode 100644
index 000000000000..a6761db8acac
--- /dev/null
+++ b/drivers/net/igb/igb_regtest.h
@@ -0,0 +1,251 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+/* ethtool register test data */
+struct igb_reg_test {
+	u16 reg;
+	u16 reg_offset;
+	u16 array_len;
+	u16 test_type;
+	u32 mask;
+	u32 write;
+};
+
+/* In the hardware, registers are laid out either singly, in arrays
+ * spaced 0x100 bytes apart, or in contiguous tables.  We assume
+ * most tests take place on arrays or single registers (handled
+ * as a single-element array) and special-case the tables.
+ * Table tests are always pattern tests.
+ *
+ * We also make provision for some required setup steps by specifying
+ * registers to be written without any read-back testing.
+ */
+
+#define PATTERN_TEST	1
+#define SET_READ_TEST	2
+#define WRITE_NO_TEST	3
+#define TABLE32_TEST	4
+#define TABLE64_TEST_LO	5
+#define TABLE64_TEST_HI	6
+
+/* i210 reg test */
+static struct igb_reg_test reg_test_i210[] = {
+	{ E1000_FCAL,	   0x100, 1,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_FCAH,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
+	{ E1000_FCT,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
+	{ E1000_RDBAL(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_RDBAH(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RDLEN(0),  0x100, 4,  PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
+	/* RDH is read-only for i210, only test RDT. */
+	{ E1000_RDT(0),	   0x100, 4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_FCRTH,	   0x100, 1,  PATTERN_TEST, 0x0003FFF0, 0x0003FFF0 },
+	{ E1000_FCTTV,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_TIPG,	   0x100, 1,  PATTERN_TEST, 0x3FFFFFFF, 0x3FFFFFFF },
+	{ E1000_TDBAL(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_TDBAH(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_TDLEN(0),  0x100, 4,  PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
+	{ E1000_TDT(0),	   0x100, 4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_RCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
+	{ E1000_RCTL, 	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0x003FFFFB },
+	{ E1000_RCTL, 	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0xFFFFFFFF },
+	{ E1000_TCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
+	{ E1000_RA,	   0, 16, TABLE64_TEST_LO,
+						0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RA,	   0, 16, TABLE64_TEST_HI,
+						0x900FFFFF, 0xFFFFFFFF },
+	{ E1000_MTA,	   0, 128, TABLE32_TEST,
+						0xFFFFFFFF, 0xFFFFFFFF },
+	{ 0, 0, 0, 0 }
+};
+
+/* i350 reg test */
+static struct igb_reg_test reg_test_i350[] = {
+	{ E1000_FCAL,	   0x100, 1,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_FCAH,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
+	{ E1000_FCT,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
+	/* VET is readonly on i350 */
+	{ E1000_RDBAL(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_RDBAH(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RDLEN(0),  0x100, 4,  PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
+	{ E1000_RDBAL(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_RDBAH(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RDLEN(4),  0x40,  4,  PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
+	/* RDH is read-only for i350, only test RDT. */
+	{ E1000_RDT(0),	   0x100, 4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_RDT(4),	   0x40,  4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_FCRTH,	   0x100, 1,  PATTERN_TEST, 0x0000FFF0, 0x0000FFF0 },
+	{ E1000_FCTTV,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_TIPG,	   0x100, 1,  PATTERN_TEST, 0x3FFFFFFF, 0x3FFFFFFF },
+	{ E1000_TDBAL(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_TDBAH(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_TDLEN(0),  0x100, 4,  PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
+	{ E1000_TDBAL(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_TDBAH(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_TDLEN(4),  0x40,  4,  PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
+	{ E1000_TDT(0),	   0x100, 4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_TDT(4),	   0x40,  4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_RCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
+	{ E1000_RCTL, 	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0x003FFFFB },
+	{ E1000_RCTL, 	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0xFFFFFFFF },
+	{ E1000_TCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
+	{ E1000_RA,	   0, 16, TABLE64_TEST_LO,
+						0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RA,	   0, 16, TABLE64_TEST_HI,
+						0xC3FFFFFF, 0xFFFFFFFF },
+	{ E1000_RA2,	   0, 16, TABLE64_TEST_LO,
+						0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RA2,	   0, 16, TABLE64_TEST_HI,
+						0xC3FFFFFF, 0xFFFFFFFF },
+	{ E1000_MTA,	   0, 128, TABLE32_TEST,
+						0xFFFFFFFF, 0xFFFFFFFF },
+	{ 0, 0, 0, 0 }
+};
+
+/* 82580 reg test */
+static struct igb_reg_test reg_test_82580[] = {
+	{ E1000_FCAL,	   0x100, 1,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_FCAH,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
+	{ E1000_FCT,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
+	{ E1000_VET,	   0x100, 1,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RDBAL(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_RDBAH(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RDLEN(0),  0x100, 4,  PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
+	{ E1000_RDBAL(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_RDBAH(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RDLEN(4),  0x40,  4,  PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
+	/* RDH is read-only for 82580, only test RDT. */
+	{ E1000_RDT(0),	   0x100, 4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_RDT(4),	   0x40,  4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_FCRTH,	   0x100, 1,  PATTERN_TEST, 0x0000FFF0, 0x0000FFF0 },
+	{ E1000_FCTTV,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_TIPG,	   0x100, 1,  PATTERN_TEST, 0x3FFFFFFF, 0x3FFFFFFF },
+	{ E1000_TDBAL(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_TDBAH(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_TDLEN(0),  0x100, 4,  PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
+	{ E1000_TDBAL(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_TDBAH(4),  0x40,  4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_TDLEN(4),  0x40,  4,  PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
+	{ E1000_TDT(0),	   0x100, 4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_TDT(4),	   0x40,  4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_RCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
+	{ E1000_RCTL, 	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0x003FFFFB },
+	{ E1000_RCTL, 	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0xFFFFFFFF },
+	{ E1000_TCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
+	{ E1000_RA,	   0, 16, TABLE64_TEST_LO,
+						0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RA,	   0, 16, TABLE64_TEST_HI,
+						0x83FFFFFF, 0xFFFFFFFF },
+	{ E1000_RA2,	   0, 8, TABLE64_TEST_LO,
+						0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RA2,	   0, 8, TABLE64_TEST_HI,
+						0x83FFFFFF, 0xFFFFFFFF },
+	{ E1000_MTA,	   0, 128, TABLE32_TEST,
+						0xFFFFFFFF, 0xFFFFFFFF },
+	{ 0, 0, 0, 0 }
+};
+
+/* 82576 reg test */
+static struct igb_reg_test reg_test_82576[] = {
+	{ E1000_FCAL,	   0x100, 1,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_FCAH,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
+	{ E1000_FCT,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
+	{ E1000_VET,	   0x100, 1,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RDBAL(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_RDBAH(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RDLEN(0),  0x100, 4,  PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
+	{ E1000_RDBAL(4),  0x40,  12, PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_RDBAH(4),  0x40,  12, PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RDLEN(4),  0x40,  12, PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
+	/* Enable all queues before testing. */
+	{ E1000_RXDCTL(0), 0x100, 4,  WRITE_NO_TEST, 0, E1000_RXDCTL_QUEUE_ENABLE },
+	{ E1000_RXDCTL(4), 0x40,  12, WRITE_NO_TEST, 0, E1000_RXDCTL_QUEUE_ENABLE },
+	/* RDH is read-only for 82576, only test RDT. */
+	{ E1000_RDT(0),	   0x100, 4,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_RDT(4),	   0x40,  12, PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_RXDCTL(0), 0x100, 4,  WRITE_NO_TEST, 0, 0 },
+	{ E1000_RXDCTL(4), 0x40,  12, WRITE_NO_TEST, 0, 0 },
+	{ E1000_FCRTH,	   0x100, 1,  PATTERN_TEST, 0x0000FFF0, 0x0000FFF0 },
+	{ E1000_FCTTV,	   0x100, 1,  PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_TIPG,	   0x100, 1,  PATTERN_TEST, 0x3FFFFFFF, 0x3FFFFFFF },
+	{ E1000_TDBAL(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_TDBAH(0),  0x100, 4,  PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_TDLEN(0),  0x100, 4,  PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
+	{ E1000_TDBAL(4),  0x40,  12, PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_TDBAH(4),  0x40,  12, PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_TDLEN(4),  0x40,  12, PATTERN_TEST, 0x000FFFF0, 0x000FFFFF },
+	{ E1000_RCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
+	{ E1000_RCTL, 	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0x003FFFFB },
+	{ E1000_RCTL, 	   0x100, 1,  SET_READ_TEST, 0x04CFB0FE, 0xFFFFFFFF },
+	{ E1000_TCTL,	   0x100, 1,  SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
+	{ E1000_RA,	   0, 16, TABLE64_TEST_LO,
+						0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RA,	   0, 16, TABLE64_TEST_HI,
+						0x83FFFFFF, 0xFFFFFFFF },
+	{ E1000_RA2,	   0, 8, TABLE64_TEST_LO,
+						0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RA2,	   0, 8, TABLE64_TEST_HI,
+						0x83FFFFFF, 0xFFFFFFFF },
+	{ E1000_MTA,	   0, 128, TABLE32_TEST,
+						0xFFFFFFFF, 0xFFFFFFFF },
+	{ 0, 0, 0, 0 }
+};
+
+/* 82575 register test */
+static struct igb_reg_test reg_test_82575[] = {
+	{ E1000_FCAL,	0x100,	1, PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_FCAH,	0x100,	1, PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
+	{ E1000_FCT,	0x100,	1, PATTERN_TEST, 0x0000FFFF, 0xFFFFFFFF },
+	{ E1000_VET,	0x100,	1, PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RDBAL(0),	0x100,	4, PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_RDBAH(0),	0x100,	4, PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RDLEN(0),	0x100,	4, PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
+	/* Enable all four RX queues before testing. */
+	{ E1000_RXDCTL(0),	0x100,	4, WRITE_NO_TEST, 0, E1000_RXDCTL_QUEUE_ENABLE },
+	/* RDH is read-only for 82575, only test RDT. */
+	{ E1000_RDT(0),	0x100,	4, PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_RXDCTL(0),	0x100,	4, WRITE_NO_TEST, 0, 0 },
+	{ E1000_FCRTH,	0x100,	1, PATTERN_TEST, 0x0000FFF0, 0x0000FFF0 },
+	{ E1000_FCTTV,	0x100,	1, PATTERN_TEST, 0x0000FFFF, 0x0000FFFF },
+	{ E1000_TIPG,	0x100,	1, PATTERN_TEST, 0x3FFFFFFF, 0x3FFFFFFF },
+	{ E1000_TDBAL(0),	0x100,	4, PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
+	{ E1000_TDBAH(0),	0x100,	4, PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_TDLEN(0),	0x100,	4, PATTERN_TEST, 0x000FFF80, 0x000FFFFF },
+	{ E1000_RCTL,	0x100,	1, SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
+	{ E1000_RCTL, 	0x100,	1, SET_READ_TEST, 0x04CFB3FE, 0x003FFFFB },
+	{ E1000_RCTL, 	0x100,	1, SET_READ_TEST, 0x04CFB3FE, 0xFFFFFFFF },
+	{ E1000_TCTL,	0x100,	1, SET_READ_TEST, 0xFFFFFFFF, 0x00000000 },
+	{ E1000_TXCW,	0x100,	1, PATTERN_TEST, 0xC000FFFF, 0x0000FFFF },
+	{ E1000_RA,	0,	16, TABLE64_TEST_LO,
+						0xFFFFFFFF, 0xFFFFFFFF },
+	{ E1000_RA,	0,	16, TABLE64_TEST_HI,
+						0x800FFFFF, 0xFFFFFFFF },
+	{ E1000_MTA,	0,	128, TABLE32_TEST,
+						0xFFFFFFFF, 0xFFFFFFFF },
+	{ 0, 0, 0, 0 }
+};
+
+
diff --git a/drivers/net/igb/igb_vmdq.c b/drivers/net/igb/igb_vmdq.c
new file mode 100644
index 000000000000..9fc32a8847a7
--- /dev/null
+++ b/drivers/net/igb/igb_vmdq.c
@@ -0,0 +1,437 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+
+#include <linux/tcp.h>
+
+#include "igb.h"
+#include "igb_vmdq.h"
+#include <linux/if_vlan.h>
+
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+int igb_vmdq_open(struct net_device *dev)
+{
+	struct igb_vmdq_adapter *vadapter = netdev_priv(dev);
+	struct igb_adapter *adapter = vadapter->real_adapter;
+	struct net_device *main_netdev = adapter->netdev;
+	int hw_queue = vadapter->rx_ring->queue_index +
+		       adapter->vfs_allocated_count;
+
+	if (test_bit(__IGB_DOWN, &adapter->state)) {
+		DPRINTK(DRV, WARNING,
+			"Open %s before opening this device.\n",
+			main_netdev->name);
+		return -EAGAIN;
+	}
+	netif_carrier_off(dev);
+	vadapter->tx_ring->vmdq_netdev = dev;
+	vadapter->rx_ring->vmdq_netdev = dev;
+	if (is_valid_ether_addr(dev->dev_addr)) {
+		igb_del_mac_filter(adapter, dev->dev_addr, hw_queue);
+		igb_add_mac_filter(adapter, dev->dev_addr, hw_queue);
+	}
+	netif_carrier_on(dev);
+	return 0;
+}
+
+int igb_vmdq_close(struct net_device *dev)
+{
+	struct igb_vmdq_adapter *vadapter = netdev_priv(dev);
+	struct igb_adapter *adapter = vadapter->real_adapter;
+	int hw_queue = vadapter->rx_ring->queue_index +
+		       adapter->vfs_allocated_count;
+
+	netif_carrier_off(dev);
+	igb_del_mac_filter(adapter, dev->dev_addr, hw_queue);
+
+	vadapter->tx_ring->vmdq_netdev = NULL;
+	vadapter->rx_ring->vmdq_netdev = NULL;
+	return 0;
+}
+
+netdev_tx_t igb_vmdq_xmit_frame(struct sk_buff *skb, struct net_device *dev)
+{
+	struct igb_vmdq_adapter *vadapter = netdev_priv(dev);
+
+	return igb_xmit_frame_ring(skb, vadapter->tx_ring);
+}
+
+struct net_device_stats *igb_vmdq_get_stats(struct net_device *dev)
+{
+	struct igb_vmdq_adapter *vadapter = netdev_priv(dev);
+        struct igb_adapter *adapter = vadapter->real_adapter;
+        struct e1000_hw *hw = &adapter->hw;
+	int hw_queue = vadapter->rx_ring->queue_index +
+		       adapter->vfs_allocated_count;
+
+	vadapter->net_stats.rx_packets +=
+			E1000_READ_REG(hw, E1000_PFVFGPRC(hw_queue));
+	E1000_WRITE_REG(hw, E1000_PFVFGPRC(hw_queue), 0);
+        vadapter->net_stats.tx_packets +=
+			E1000_READ_REG(hw, E1000_PFVFGPTC(hw_queue));
+        E1000_WRITE_REG(hw, E1000_PFVFGPTC(hw_queue), 0);
+        vadapter->net_stats.rx_bytes +=
+			E1000_READ_REG(hw, E1000_PFVFGORC(hw_queue));
+        E1000_WRITE_REG(hw, E1000_PFVFGORC(hw_queue), 0);
+        vadapter->net_stats.tx_bytes +=
+			E1000_READ_REG(hw, E1000_PFVFGOTC(hw_queue));
+        E1000_WRITE_REG(hw, E1000_PFVFGOTC(hw_queue), 0);
+        vadapter->net_stats.multicast +=
+			E1000_READ_REG(hw, E1000_PFVFMPRC(hw_queue));
+        E1000_WRITE_REG(hw, E1000_PFVFMPRC(hw_queue), 0);
+	/* only return the current stats */
+	return &vadapter->net_stats;
+}
+
+/**
+ * igb_write_vm_addr_list - write unicast addresses to RAR table
+ * @netdev: network interface device structure
+ *
+ * Writes unicast address list to the RAR table.
+ * Returns: -ENOMEM on failure/insufficient address space
+ *                0 on no addresses written
+ *                X on writing X addresses to the RAR table
+ **/
+static int igb_write_vm_addr_list(struct net_device *netdev)
+{
+	struct igb_vmdq_adapter *vadapter = netdev_priv(netdev);
+        struct igb_adapter *adapter = vadapter->real_adapter;
+	int count = 0;
+	int hw_queue = vadapter->rx_ring->queue_index +
+		       adapter->vfs_allocated_count;
+
+	/* return ENOMEM indicating insufficient memory for addresses */
+	if (netdev_uc_count(netdev) > igb_available_rars(adapter))
+		return -ENOMEM;
+
+	if (!netdev_uc_empty(netdev)) {
+#ifdef NETDEV_HW_ADDR_T_UNICAST
+		struct netdev_hw_addr *ha;
+#else
+		struct dev_mc_list *ha;
+#endif
+		netdev_for_each_uc_addr(ha, netdev) {
+#ifdef NETDEV_HW_ADDR_T_UNICAST
+			igb_del_mac_filter(adapter, ha->addr, hw_queue);
+			igb_add_mac_filter(adapter, ha->addr, hw_queue);
+#else
+			igb_del_mac_filter(adapter, ha->da_addr, hw_queue);
+			igb_add_mac_filter(adapter, ha->da_addr, hw_queue);
+#endif
+			count++;
+		}
+	}
+	return count;
+}
+
+
+#define E1000_VMOLR_UPE		0x20000000 /* Unicast promiscuous mode */
+void igb_vmdq_set_rx_mode(struct net_device *dev)
+{
+	struct igb_vmdq_adapter *vadapter = netdev_priv(dev);
+        struct igb_adapter *adapter = vadapter->real_adapter;
+        struct e1000_hw *hw = &adapter->hw;
+	u32 vmolr, rctl;
+	int hw_queue = vadapter->rx_ring->queue_index +
+		       adapter->vfs_allocated_count;
+
+	/* Check for Promiscuous and All Multicast modes */
+	vmolr = E1000_READ_REG(hw, E1000_VMOLR(hw_queue));
+
+	/* clear the affected bits */
+	vmolr &= ~(E1000_VMOLR_UPE | E1000_VMOLR_MPME |
+		   E1000_VMOLR_ROPE | E1000_VMOLR_ROMPE);
+
+	if (dev->flags & IFF_PROMISC) {
+		vmolr |= E1000_VMOLR_UPE;
+		rctl = E1000_READ_REG(hw, E1000_RCTL);
+		rctl |= E1000_RCTL_UPE;
+		E1000_WRITE_REG(hw, E1000_RCTL, rctl);
+	} else {
+		rctl = E1000_READ_REG(hw, E1000_RCTL);
+		rctl &= ~E1000_RCTL_UPE;
+		E1000_WRITE_REG(hw, E1000_RCTL, rctl);
+		if (dev->flags & IFF_ALLMULTI) {
+			vmolr |= E1000_VMOLR_MPME;
+		} else {
+			/*
+			 * Write addresses to the MTA, if the attempt fails
+			 * then we should just turn on promiscous mode so
+			 * that we can at least receive multicast traffic
+			 */
+			if (igb_write_mc_addr_list(adapter->netdev) != 0)
+				vmolr |= E1000_VMOLR_ROMPE;
+		}
+#ifdef HAVE_SET_RX_MODE
+		/*
+		 * Write addresses to available RAR registers, if there is not
+		 * sufficient space to store all the addresses then enable
+		 * unicast promiscous mode
+		 */
+		if (igb_write_vm_addr_list(dev) < 0)
+			vmolr |= E1000_VMOLR_UPE;
+#endif
+	}
+	E1000_WRITE_REG(hw, E1000_VMOLR(hw_queue), vmolr);
+
+	return;
+}
+
+int igb_vmdq_set_mac(struct net_device *dev, void *p)
+{
+	struct sockaddr *addr = p;
+	struct igb_vmdq_adapter *vadapter = netdev_priv(dev);
+        struct igb_adapter *adapter = vadapter->real_adapter;
+	int hw_queue = vadapter->rx_ring->queue_index +
+		       adapter->vfs_allocated_count;
+
+	igb_del_mac_filter(adapter, dev->dev_addr, hw_queue);
+	memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);
+	return igb_add_mac_filter(adapter, dev->dev_addr, hw_queue);
+}
+
+int igb_vmdq_change_mtu(struct net_device *dev, int new_mtu)
+{
+	struct igb_vmdq_adapter *vadapter = netdev_priv(dev);
+	struct igb_adapter *adapter = vadapter->real_adapter;
+
+	if (adapter->netdev->mtu < new_mtu) {
+		DPRINTK(PROBE, INFO,
+			"Set MTU on %s to >= %d "
+			"before changing MTU on %s\n",
+			adapter->netdev->name, new_mtu, dev->name);
+		return -EINVAL;
+	}
+	dev->mtu = new_mtu;
+	return 0;
+}
+
+void igb_vmdq_tx_timeout(struct net_device *dev)
+{
+	return;
+}
+
+void igb_vmdq_vlan_rx_register(struct net_device *dev, struct vlan_group *grp)
+{
+	struct igb_vmdq_adapter *vadapter = netdev_priv(dev);
+	struct igb_adapter *adapter = vadapter->real_adapter;
+	struct e1000_hw *hw = &adapter->hw;
+	int hw_queue = vadapter->rx_ring->queue_index +
+		       adapter->vfs_allocated_count;
+
+	vadapter->vlgrp = grp;
+
+	igb_enable_vlan_tags(adapter);
+	E1000_WRITE_REG(hw, E1000_VMVIR(hw_queue), 0);
+
+	return;
+}
+void igb_vmdq_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
+{
+	struct igb_vmdq_adapter *vadapter = netdev_priv(dev);
+	struct igb_adapter *adapter = vadapter->real_adapter;
+#ifndef HAVE_NETDEV_VLAN_FEATURES
+	struct net_device *v_netdev;
+#endif
+	int hw_queue = vadapter->rx_ring->queue_index +
+		       adapter->vfs_allocated_count;
+
+	/* attempt to add filter to vlvf array */
+	igb_vlvf_set(adapter, vid, TRUE, hw_queue);
+
+#ifndef HAVE_NETDEV_VLAN_FEATURES
+
+	/* Copy feature flags from netdev to the vlan netdev for this vid.
+	 * This allows things like TSO to bubble down to our vlan device.
+	 */
+	v_netdev = vlan_group_get_device(vadapter->vlgrp, vid);
+	v_netdev->features |= adapter->netdev->features;
+	vlan_group_set_device(vadapter->vlgrp, vid, v_netdev);
+#endif
+
+	return;
+}
+void igb_vmdq_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
+{
+	struct igb_vmdq_adapter *vadapter = netdev_priv(dev);
+	struct igb_adapter *adapter = vadapter->real_adapter;
+	int hw_queue = vadapter->rx_ring->queue_index +
+		       adapter->vfs_allocated_count;
+
+	vlan_group_set_device(vadapter->vlgrp, vid, NULL);
+	/* remove vlan from VLVF table array */
+	igb_vlvf_set(adapter, vid, FALSE, hw_queue);
+
+
+	return;
+}
+
+static int igb_vmdq_get_settings(struct net_device *netdev,
+				   struct ethtool_cmd *ecmd)
+{
+	struct igb_vmdq_adapter *vadapter = netdev_priv(netdev);
+	struct igb_adapter *adapter = vadapter->real_adapter;
+	struct e1000_hw *hw = &adapter->hw;
+	u32 status;
+
+	if (hw->phy.media_type == e1000_media_type_copper) {
+
+		ecmd->supported = (SUPPORTED_10baseT_Half |
+				   SUPPORTED_10baseT_Full |
+				   SUPPORTED_100baseT_Half |
+				   SUPPORTED_100baseT_Full |
+				   SUPPORTED_1000baseT_Full|
+				   SUPPORTED_Autoneg |
+				   SUPPORTED_TP);
+		ecmd->advertising = ADVERTISED_TP;
+
+		if (hw->mac.autoneg == 1) {
+			ecmd->advertising |= ADVERTISED_Autoneg;
+			/* the e1000 autoneg seems to match ethtool nicely */
+			ecmd->advertising |= hw->phy.autoneg_advertised;
+		}
+
+		ecmd->port = PORT_TP;
+		ecmd->phy_address = hw->phy.addr;
+	} else {
+		ecmd->supported   = (SUPPORTED_1000baseT_Full |
+				     SUPPORTED_FIBRE |
+				     SUPPORTED_Autoneg);
+
+		ecmd->advertising = (ADVERTISED_1000baseT_Full |
+				     ADVERTISED_FIBRE |
+				     ADVERTISED_Autoneg);
+
+		ecmd->port = PORT_FIBRE;
+	}
+
+	ecmd->transceiver = XCVR_INTERNAL;
+
+	status = E1000_READ_REG(hw, E1000_STATUS);
+
+	if (status & E1000_STATUS_LU) {
+
+		if ((status & E1000_STATUS_SPEED_1000) ||
+		    hw->phy.media_type != e1000_media_type_copper)
+			ecmd->speed = SPEED_1000;
+		else if (status & E1000_STATUS_SPEED_100)
+			ecmd->speed = SPEED_100;
+		else
+			ecmd->speed = SPEED_10;
+
+		if ((status & E1000_STATUS_FD) ||
+		    hw->phy.media_type != e1000_media_type_copper)
+			ecmd->duplex = DUPLEX_FULL;
+		else
+			ecmd->duplex = DUPLEX_HALF;
+	} else {
+		ecmd->speed = -1;
+		ecmd->duplex = -1;
+	}
+
+	ecmd->autoneg = hw->mac.autoneg ? AUTONEG_ENABLE : AUTONEG_DISABLE;
+	return 0;
+}
+
+
+static u32 igb_vmdq_get_msglevel(struct net_device *netdev)
+{
+	struct igb_vmdq_adapter *vadapter = netdev_priv(netdev);
+	struct igb_adapter *adapter = vadapter->real_adapter;
+	return adapter->msg_enable;
+}
+
+static void igb_vmdq_get_drvinfo(struct net_device *netdev,
+				   struct ethtool_drvinfo *drvinfo)
+{
+	struct igb_vmdq_adapter *vadapter = netdev_priv(netdev);
+	struct igb_adapter *adapter = vadapter->real_adapter;
+	struct net_device *main_netdev = adapter->netdev;
+
+	strncpy(drvinfo->driver, igb_driver_name, 32);
+	strncpy(drvinfo->version, igb_driver_version, 32);
+
+	strncpy(drvinfo->fw_version, "N/A", 4);
+	snprintf(drvinfo->bus_info, 32, "%s VMDQ %d", main_netdev->name,
+		 vadapter->rx_ring->queue_index);
+	drvinfo->n_stats = 0;
+	drvinfo->testinfo_len = 0;
+	drvinfo->regdump_len = 0;
+}
+
+static void igb_vmdq_get_ringparam(struct net_device *netdev,
+				     struct ethtool_ringparam *ring)
+{
+	struct igb_vmdq_adapter *vadapter = netdev_priv(netdev);
+
+	struct igb_ring *tx_ring = vadapter->tx_ring;
+	struct igb_ring *rx_ring = vadapter->rx_ring;
+
+	ring->rx_max_pending = IGB_MAX_RXD;
+	ring->tx_max_pending = IGB_MAX_TXD;
+	ring->rx_mini_max_pending = 0;
+	ring->rx_jumbo_max_pending = 0;
+	ring->rx_pending = rx_ring->count;
+	ring->tx_pending = tx_ring->count;
+	ring->rx_mini_pending = 0;
+	ring->rx_jumbo_pending = 0;
+}
+static u32 igb_vmdq_get_rx_csum(struct net_device *netdev)
+{
+	struct igb_vmdq_adapter *vadapter = netdev_priv(netdev);
+	struct igb_adapter *adapter = vadapter->real_adapter;
+
+	return test_bit(IGB_RING_FLAG_RX_CSUM, &adapter->rx_ring[0]->flags);
+}
+
+
+static struct ethtool_ops igb_vmdq_ethtool_ops = {
+	.get_settings           = igb_vmdq_get_settings,
+	.get_drvinfo            = igb_vmdq_get_drvinfo,
+	.get_link               = ethtool_op_get_link,
+	.get_ringparam          = igb_vmdq_get_ringparam,
+	.get_rx_csum            = igb_vmdq_get_rx_csum,
+	.get_tx_csum            = ethtool_op_get_tx_csum,
+	.get_sg                 = ethtool_op_get_sg,
+	.set_sg                 = ethtool_op_set_sg,
+	.get_msglevel           = igb_vmdq_get_msglevel,
+#ifdef NETIF_F_TSO
+	.get_tso                = ethtool_op_get_tso,
+#endif
+#ifdef HAVE_ETHTOOL_GET_PERM_ADDR
+	.get_perm_addr          = ethtool_op_get_perm_addr,
+#endif
+};
+
+void igb_vmdq_set_ethtool_ops(struct net_device *netdev)
+{
+	SET_ETHTOOL_OPS(netdev, &igb_vmdq_ethtool_ops);
+}
+
+
+#endif /* CONFIG_IGB_VMDQ_NETDEV */
+
diff --git a/drivers/net/igb/igb_vmdq.h b/drivers/net/igb/igb_vmdq.h
new file mode 100644
index 000000000000..e51e7c4e13b7
--- /dev/null
+++ b/drivers/net/igb/igb_vmdq.h
@@ -0,0 +1,46 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+#ifndef _IGB_VMDQ_H_
+#define _IGB_VMDQ_H_
+
+#ifdef CONFIG_IGB_VMDQ_NETDEV
+int igb_vmdq_open(struct net_device *dev);
+int igb_vmdq_close(struct net_device *dev);
+netdev_tx_t igb_vmdq_xmit_frame(struct sk_buff *skb, struct net_device *dev);
+struct net_device_stats *igb_vmdq_get_stats(struct net_device *dev);
+void igb_vmdq_set_rx_mode(struct net_device *dev);
+int igb_vmdq_set_mac(struct net_device *dev, void *addr);
+int igb_vmdq_change_mtu(struct net_device *dev, int new_mtu);
+void igb_vmdq_tx_timeout(struct net_device *dev);
+void igb_vmdq_vlan_rx_register(struct net_device *dev,
+				 struct vlan_group *grp);
+void igb_vmdq_vlan_rx_add_vid(struct net_device *dev, unsigned short vid);
+void igb_vmdq_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid);
+void igb_vmdq_set_ethtool_ops(struct net_device *netdev);
+#endif /* CONFIG_IGB_VMDQ_NETDEV */
+#endif /* _IGB_VMDQ_H_ */
diff --git a/drivers/net/igb/kcompat.c b/drivers/net/igb/kcompat.c
new file mode 100644
index 000000000000..50d56a8687a9
--- /dev/null
+++ b/drivers/net/igb/kcompat.c
@@ -0,0 +1,1500 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+#include "igb.h"
+#include "kcompat.h"
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,8) )
+/* From lib/vsprintf.c */
+#include <asm/div64.h>
+
+static int skip_atoi(const char **s)
+{
+	int i=0;
+
+	while (isdigit(**s))
+		i = i*10 + *((*s)++) - '0';
+	return i;
+}
+
+#define _kc_ZEROPAD	1		/* pad with zero */
+#define _kc_SIGN	2		/* unsigned/signed long */
+#define _kc_PLUS	4		/* show plus */
+#define _kc_SPACE	8		/* space if plus */
+#define _kc_LEFT	16		/* left justified */
+#define _kc_SPECIAL	32		/* 0x */
+#define _kc_LARGE	64		/* use 'ABCDEF' instead of 'abcdef' */
+
+static char * number(char * buf, char * end, long long num, int base, int size, int precision, int type)
+{
+	char c,sign,tmp[66];
+	const char *digits;
+	const char small_digits[] = "0123456789abcdefghijklmnopqrstuvwxyz";
+	const char large_digits[] = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ";
+	int i;
+
+	digits = (type & _kc_LARGE) ? large_digits : small_digits;
+	if (type & _kc_LEFT)
+		type &= ~_kc_ZEROPAD;
+	if (base < 2 || base > 36)
+		return 0;
+	c = (type & _kc_ZEROPAD) ? '0' : ' ';
+	sign = 0;
+	if (type & _kc_SIGN) {
+		if (num < 0) {
+			sign = '-';
+			num = -num;
+			size--;
+		} else if (type & _kc_PLUS) {
+			sign = '+';
+			size--;
+		} else if (type & _kc_SPACE) {
+			sign = ' ';
+			size--;
+		}
+	}
+	if (type & _kc_SPECIAL) {
+		if (base == 16)
+			size -= 2;
+		else if (base == 8)
+			size--;
+	}
+	i = 0;
+	if (num == 0)
+		tmp[i++]='0';
+	else while (num != 0)
+		tmp[i++] = digits[do_div(num,base)];
+	if (i > precision)
+		precision = i;
+	size -= precision;
+	if (!(type&(_kc_ZEROPAD+_kc_LEFT))) {
+		while(size-->0) {
+			if (buf <= end)
+				*buf = ' ';
+			++buf;
+		}
+	}
+	if (sign) {
+		if (buf <= end)
+			*buf = sign;
+		++buf;
+	}
+	if (type & _kc_SPECIAL) {
+		if (base==8) {
+			if (buf <= end)
+				*buf = '0';
+			++buf;
+		} else if (base==16) {
+			if (buf <= end)
+				*buf = '0';
+			++buf;
+			if (buf <= end)
+				*buf = digits[33];
+			++buf;
+		}
+	}
+	if (!(type & _kc_LEFT)) {
+		while (size-- > 0) {
+			if (buf <= end)
+				*buf = c;
+			++buf;
+		}
+	}
+	while (i < precision--) {
+		if (buf <= end)
+			*buf = '0';
+		++buf;
+	}
+	while (i-- > 0) {
+		if (buf <= end)
+			*buf = tmp[i];
+		++buf;
+	}
+	while (size-- > 0) {
+		if (buf <= end)
+			*buf = ' ';
+		++buf;
+	}
+	return buf;
+}
+
+int _kc_vsnprintf(char *buf, size_t size, const char *fmt, va_list args)
+{
+	int len;
+	unsigned long long num;
+	int i, base;
+	char *str, *end, c;
+	const char *s;
+
+	int flags;		/* flags to number() */
+
+	int field_width;	/* width of output field */
+	int precision;		/* min. # of digits for integers; max
+				   number of chars for from string */
+	int qualifier;		/* 'h', 'l', or 'L' for integer fields */
+				/* 'z' support added 23/7/1999 S.H.    */
+				/* 'z' changed to 'Z' --davidm 1/25/99 */
+
+	str = buf;
+	end = buf + size - 1;
+
+	if (end < buf - 1) {
+		end = ((void *) -1);
+		size = end - buf + 1;
+	}
+
+	for (; *fmt ; ++fmt) {
+		if (*fmt != '%') {
+			if (str <= end)
+				*str = *fmt;
+			++str;
+			continue;
+		}
+
+		/* process flags */
+		flags = 0;
+		repeat:
+			++fmt;		/* this also skips first '%' */
+			switch (*fmt) {
+				case '-': flags |= _kc_LEFT; goto repeat;
+				case '+': flags |= _kc_PLUS; goto repeat;
+				case ' ': flags |= _kc_SPACE; goto repeat;
+				case '#': flags |= _kc_SPECIAL; goto repeat;
+				case '0': flags |= _kc_ZEROPAD; goto repeat;
+			}
+
+		/* get field width */
+		field_width = -1;
+		if (isdigit(*fmt))
+			field_width = skip_atoi(&fmt);
+		else if (*fmt == '*') {
+			++fmt;
+			/* it's the next argument */
+			field_width = va_arg(args, int);
+			if (field_width < 0) {
+				field_width = -field_width;
+				flags |= _kc_LEFT;
+			}
+		}
+
+		/* get the precision */
+		precision = -1;
+		if (*fmt == '.') {
+			++fmt;
+			if (isdigit(*fmt))
+				precision = skip_atoi(&fmt);
+			else if (*fmt == '*') {
+				++fmt;
+				/* it's the next argument */
+				precision = va_arg(args, int);
+			}
+			if (precision < 0)
+				precision = 0;
+		}
+
+		/* get the conversion qualifier */
+		qualifier = -1;
+		if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L' || *fmt =='Z') {
+			qualifier = *fmt;
+			++fmt;
+		}
+
+		/* default base */
+		base = 10;
+
+		switch (*fmt) {
+			case 'c':
+				if (!(flags & _kc_LEFT)) {
+					while (--field_width > 0) {
+						if (str <= end)
+							*str = ' ';
+						++str;
+					}
+				}
+				c = (unsigned char) va_arg(args, int);
+				if (str <= end)
+					*str = c;
+				++str;
+				while (--field_width > 0) {
+					if (str <= end)
+						*str = ' ';
+					++str;
+				}
+				continue;
+
+			case 's':
+				s = va_arg(args, char *);
+				if (!s)
+					s = "<NULL>";
+
+				len = strnlen(s, precision);
+
+				if (!(flags & _kc_LEFT)) {
+					while (len < field_width--) {
+						if (str <= end)
+							*str = ' ';
+						++str;
+					}
+				}
+				for (i = 0; i < len; ++i) {
+					if (str <= end)
+						*str = *s;
+					++str; ++s;
+				}
+				while (len < field_width--) {
+					if (str <= end)
+						*str = ' ';
+					++str;
+				}
+				continue;
+
+			case 'p':
+				if (field_width == -1) {
+					field_width = 2*sizeof(void *);
+					flags |= _kc_ZEROPAD;
+				}
+				str = number(str, end,
+						(unsigned long) va_arg(args, void *),
+						16, field_width, precision, flags);
+				continue;
+
+
+			case 'n':
+				/* FIXME:
+				* What does C99 say about the overflow case here? */
+				if (qualifier == 'l') {
+					long * ip = va_arg(args, long *);
+					*ip = (str - buf);
+				} else if (qualifier == 'Z') {
+					size_t * ip = va_arg(args, size_t *);
+					*ip = (str - buf);
+				} else {
+					int * ip = va_arg(args, int *);
+					*ip = (str - buf);
+				}
+				continue;
+
+			case '%':
+				if (str <= end)
+					*str = '%';
+				++str;
+				continue;
+
+				/* integer number formats - set up the flags and "break" */
+			case 'o':
+				base = 8;
+				break;
+
+			case 'X':
+				flags |= _kc_LARGE;
+			case 'x':
+				base = 16;
+				break;
+
+			case 'd':
+			case 'i':
+				flags |= _kc_SIGN;
+			case 'u':
+				break;
+
+			default:
+				if (str <= end)
+					*str = '%';
+				++str;
+				if (*fmt) {
+					if (str <= end)
+						*str = *fmt;
+					++str;
+				} else {
+					--fmt;
+				}
+				continue;
+		}
+		if (qualifier == 'L')
+			num = va_arg(args, long long);
+		else if (qualifier == 'l') {
+			num = va_arg(args, unsigned long);
+			if (flags & _kc_SIGN)
+				num = (signed long) num;
+		} else if (qualifier == 'Z') {
+			num = va_arg(args, size_t);
+		} else if (qualifier == 'h') {
+			num = (unsigned short) va_arg(args, int);
+			if (flags & _kc_SIGN)
+				num = (signed short) num;
+		} else {
+			num = va_arg(args, unsigned int);
+			if (flags & _kc_SIGN)
+				num = (signed int) num;
+		}
+		str = number(str, end, num, base,
+				field_width, precision, flags);
+	}
+	if (str <= end)
+		*str = '\0';
+	else if (size > 0)
+		/* don't write out a null byte if the buf size is zero */
+		*end = '\0';
+	/* the trailing null byte doesn't count towards the total
+	* ++str;
+	*/
+	return str-buf;
+}
+
+int _kc_snprintf(char * buf, size_t size, const char *fmt, ...)
+{
+	va_list args;
+	int i;
+
+	va_start(args, fmt);
+	i = _kc_vsnprintf(buf,size,fmt,args);
+	va_end(args);
+	return i;
+}
+#endif /* < 2.4.8 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,13) )
+
+/**************************************/
+/* PCI DMA MAPPING */
+
+#if defined(CONFIG_HIGHMEM)
+
+#ifndef PCI_DRAM_OFFSET
+#define PCI_DRAM_OFFSET 0
+#endif
+
+u64
+_kc_pci_map_page(struct pci_dev *dev, struct page *page, unsigned long offset,
+                 size_t size, int direction)
+{
+	return (((u64) (page - mem_map) << PAGE_SHIFT) + offset +
+		PCI_DRAM_OFFSET);
+}
+
+#else /* CONFIG_HIGHMEM */
+
+u64
+_kc_pci_map_page(struct pci_dev *dev, struct page *page, unsigned long offset,
+                 size_t size, int direction)
+{
+	return pci_map_single(dev, (void *)page_address(page) + offset, size,
+			      direction);
+}
+
+#endif /* CONFIG_HIGHMEM */
+
+void
+_kc_pci_unmap_page(struct pci_dev *dev, u64 dma_addr, size_t size,
+                   int direction)
+{
+	return pci_unmap_single(dev, dma_addr, size, direction);
+}
+
+#endif /* 2.4.13 => 2.4.3 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,3) )
+
+/**************************************/
+/* PCI DRIVER API */
+
+int
+_kc_pci_set_dma_mask(struct pci_dev *dev, dma_addr_t mask)
+{
+	if (!pci_dma_supported(dev, mask))
+		return -EIO;
+	dev->dma_mask = mask;
+	return 0;
+}
+
+int
+_kc_pci_request_regions(struct pci_dev *dev, char *res_name)
+{
+	int i;
+
+	for (i = 0; i < 6; i++) {
+		if (pci_resource_len(dev, i) == 0)
+			continue;
+
+		if (pci_resource_flags(dev, i) & IORESOURCE_IO) {
+			if (!request_region(pci_resource_start(dev, i), pci_resource_len(dev, i), res_name)) {
+				pci_release_regions(dev);
+				return -EBUSY;
+			}
+		} else if (pci_resource_flags(dev, i) & IORESOURCE_MEM) {
+			if (!request_mem_region(pci_resource_start(dev, i), pci_resource_len(dev, i), res_name)) {
+				pci_release_regions(dev);
+				return -EBUSY;
+			}
+		}
+	}
+	return 0;
+}
+
+void
+_kc_pci_release_regions(struct pci_dev *dev)
+{
+	int i;
+
+	for (i = 0; i < 6; i++) {
+		if (pci_resource_len(dev, i) == 0)
+			continue;
+
+		if (pci_resource_flags(dev, i) & IORESOURCE_IO)
+			release_region(pci_resource_start(dev, i), pci_resource_len(dev, i));
+
+		else if (pci_resource_flags(dev, i) & IORESOURCE_MEM)
+			release_mem_region(pci_resource_start(dev, i), pci_resource_len(dev, i));
+	}
+}
+
+/**************************************/
+/* NETWORK DRIVER API */
+
+struct net_device *
+_kc_alloc_etherdev(int sizeof_priv)
+{
+	struct net_device *dev;
+	int alloc_size;
+
+	alloc_size = sizeof(*dev) + sizeof_priv + IFNAMSIZ + 31;
+	dev = kzalloc(alloc_size, GFP_KERNEL);
+	if (!dev)
+		return NULL;
+
+	if (sizeof_priv)
+		dev->priv = (void *) (((unsigned long)(dev + 1) + 31) & ~31);
+	dev->name[0] = '\0';
+	ether_setup(dev);
+
+	return dev;
+}
+
+int
+_kc_is_valid_ether_addr(u8 *addr)
+{
+	const char zaddr[6] = { 0, };
+
+	return !(addr[0] & 1) && memcmp(addr, zaddr, 6);
+}
+
+#endif /* 2.4.3 => 2.4.0 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,6) )
+
+int
+_kc_pci_set_power_state(struct pci_dev *dev, int state)
+{
+	return 0;
+}
+
+int
+_kc_pci_enable_wake(struct pci_dev *pdev, u32 state, int enable)
+{
+	return 0;
+}
+
+#endif /* 2.4.6 => 2.4.3 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0) )
+void _kc_skb_fill_page_desc(struct sk_buff *skb, int i, struct page *page,
+                            int off, int size)
+{
+	skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
+	frag->page = page;
+	frag->page_offset = off;
+	frag->size = size;
+	skb_shinfo(skb)->nr_frags = i + 1;
+}
+
+/*
+ * Original Copyright:
+ * find_next_bit.c: fallback find next bit implementation
+ *
+ * Copyright (C) 2004 Red Hat, Inc. All Rights Reserved.
+ * Written by David Howells (dhowells@redhat.com)
+ */
+
+/**
+ * find_next_bit - find the next set bit in a memory region
+ * @addr: The address to base the search on
+ * @offset: The bitnumber to start searching at
+ * @size: The maximum size to search
+ */
+unsigned long find_next_bit(const unsigned long *addr, unsigned long size,
+                            unsigned long offset)
+{
+	const unsigned long *p = addr + BITOP_WORD(offset);
+	unsigned long result = offset & ~(BITS_PER_LONG-1);
+	unsigned long tmp;
+
+	if (offset >= size)
+		return size;
+	size -= result;
+	offset %= BITS_PER_LONG;
+	if (offset) {
+		tmp = *(p++);
+		tmp &= (~0UL << offset);
+		if (size < BITS_PER_LONG)
+			goto found_first;
+		if (tmp)
+			goto found_middle;
+		size -= BITS_PER_LONG;
+		result += BITS_PER_LONG;
+	}
+	while (size & ~(BITS_PER_LONG-1)) {
+		if ((tmp = *(p++)))
+			goto found_middle;
+		result += BITS_PER_LONG;
+		size -= BITS_PER_LONG;
+	}
+	if (!size)
+		return result;
+	tmp = *p;
+
+found_first:
+	tmp &= (~0UL >> (BITS_PER_LONG - size));
+	if (tmp == 0UL)		/* Are any bits set? */
+		return result + size;	/* Nope. */
+found_middle:
+	return result + ffs(tmp);
+}
+
+size_t _kc_strlcpy(char *dest, const char *src, size_t size)
+{
+	size_t ret = strlen(src);
+
+	if (size) {
+		size_t len = (ret >= size) ? size - 1 : ret;
+		memcpy(dest, src, len);
+		dest[len] = '\0';
+	}
+	return ret;
+}
+
+#ifndef do_div
+#if BITS_PER_LONG == 32
+uint32_t __attribute__((weak)) _kc__div64_32(uint64_t *n, uint32_t base)
+{
+	uint64_t rem = *n;
+	uint64_t b = base;
+	uint64_t res, d = 1;
+	uint32_t high = rem >> 32;
+
+	/* Reduce the thing a bit first */
+	res = 0;
+	if (high >= base) {
+		high /= base;
+		res = (uint64_t) high << 32;
+		rem -= (uint64_t) (high*base) << 32;
+	}
+
+	while ((int64_t)b > 0 && b < rem) {
+		b = b+b;
+		d = d+d;
+	}
+
+	do {
+		if (rem >= b) {
+			rem -= b;
+			res += d;
+		}
+		b >>= 1;
+		d >>= 1;
+	} while (d);
+
+	*n = res;
+	return rem;
+}
+#endif /* BITS_PER_LONG == 32 */
+#endif /* do_div */
+#endif /* 2.6.0 => 2.4.6 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,4) )
+int _kc_scnprintf(char * buf, size_t size, const char *fmt, ...)
+{
+	va_list args;
+	int i;
+
+	va_start(args, fmt);
+	i = vsnprintf(buf, size, fmt, args);
+	va_end(args);
+	return (i >= size) ? (size - 1) : i;
+}
+#endif /* < 2.6.4 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,10) )
+DECLARE_BITMAP(_kcompat_node_online_map, MAX_NUMNODES) = {1};
+#endif /* < 2.6.10 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,13) )
+char *_kc_kstrdup(const char *s, unsigned int gfp)
+{
+	size_t len;
+	char *buf;
+
+	if (!s)
+		return NULL;
+
+	len = strlen(s) + 1;
+	buf = kmalloc(len, gfp);
+	if (buf)
+		memcpy(buf, s, len);
+	return buf;
+}
+#endif /* < 2.6.13 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,14) )
+void *_kc_kzalloc(size_t size, int flags)
+{
+	void *ret = kmalloc(size, flags);
+	if (ret)
+		memset(ret, 0, size);
+	return ret;
+}
+#endif /* <= 2.6.13 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,19) )
+int _kc_skb_pad(struct sk_buff *skb, int pad)
+{
+	int ntail;
+
+        /* If the skbuff is non linear tailroom is always zero.. */
+        if(!skb_cloned(skb) && skb_tailroom(skb) >= pad) {
+		memset(skb->data+skb->len, 0, pad);
+		return 0;
+        }
+
+	ntail = skb->data_len + pad - (skb->end - skb->tail);
+	if (likely(skb_cloned(skb) || ntail > 0)) {
+		if (pskb_expand_head(skb, 0, ntail, GFP_ATOMIC));
+			goto free_skb;
+	}
+
+#ifdef MAX_SKB_FRAGS
+	if (skb_is_nonlinear(skb) &&
+	    !__pskb_pull_tail(skb, skb->data_len))
+		goto free_skb;
+
+#endif
+	memset(skb->data + skb->len, 0, pad);
+        return 0;
+
+free_skb:
+	kfree_skb(skb);
+	return -ENOMEM;
+}
+
+#if (!(RHEL_RELEASE_CODE && RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(5,4)))
+int _kc_pci_save_state(struct pci_dev *pdev)
+{
+	struct net_device *netdev = pci_get_drvdata(pdev);
+	struct adapter_struct *adapter = netdev_priv(netdev);
+	int size = PCI_CONFIG_SPACE_LEN, i;
+	u16 pcie_cap_offset, pcie_link_status;
+
+#if ( LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0) )
+	/* no ->dev for 2.4 kernels */
+	WARN_ON(pdev->dev.driver_data == NULL);
+#endif
+	pcie_cap_offset = pci_find_capability(pdev, PCI_CAP_ID_EXP);
+	if (pcie_cap_offset) {
+		if (!pci_read_config_word(pdev,
+		                          pcie_cap_offset + PCIE_LINK_STATUS,
+		                          &pcie_link_status))
+		size = PCIE_CONFIG_SPACE_LEN;
+	}
+	pci_config_space_ich8lan();
+#ifdef HAVE_PCI_ERS
+	if (adapter->config_space == NULL)
+#else
+	WARN_ON(adapter->config_space != NULL);
+#endif
+		adapter->config_space = kmalloc(size, GFP_KERNEL);
+	if (!adapter->config_space) {
+		printk(KERN_ERR "Out of memory in pci_save_state\n");
+		return -ENOMEM;
+	}
+	for (i = 0; i < (size / 4); i++)
+		pci_read_config_dword(pdev, i * 4, &adapter->config_space[i]);
+	return 0;
+}
+
+void _kc_pci_restore_state(struct pci_dev *pdev)
+{
+#if defined(DRIVER_IXGBE) || defined(DRIVER_I40E) || defined(DRIVER_IXGBEVF)
+	struct adapter_struct *adapter = pci_get_drvdata(pdev);
+#else
+	struct net_device *netdev = pci_get_drvdata(pdev);
+	struct adapter_struct *adapter = netdev_priv(netdev);
+#endif
+	int size = PCI_CONFIG_SPACE_LEN, i;
+	u16 pcie_cap_offset;
+	u16 pcie_link_status;
+
+	if (adapter->config_space != NULL) {
+		pcie_cap_offset = pci_find_capability(pdev, PCI_CAP_ID_EXP);
+		if (pcie_cap_offset &&
+		    !pci_read_config_word(pdev,
+		                          pcie_cap_offset + PCIE_LINK_STATUS,
+		                          &pcie_link_status))
+			size = PCIE_CONFIG_SPACE_LEN;
+
+		pci_config_space_ich8lan();
+		for (i = 0; i < (size / 4); i++)
+		pci_write_config_dword(pdev, i * 4, adapter->config_space[i]);
+#ifndef HAVE_PCI_ERS
+		kfree(adapter->config_space);
+		adapter->config_space = NULL;
+#endif
+	}
+}
+#endif /* !(RHEL_RELEASE_CODE >= RHEL 5.4) */
+
+#ifdef HAVE_PCI_ERS
+void _kc_free_netdev(struct net_device *netdev)
+{
+	struct adapter_struct *adapter = netdev_priv(netdev);
+
+	if (adapter->config_space != NULL)
+		kfree(adapter->config_space);
+#ifdef CONFIG_SYSFS
+	if (netdev->reg_state == NETREG_UNINITIALIZED) {
+		kfree((char *)netdev - netdev->padded);
+	} else {
+		BUG_ON(netdev->reg_state != NETREG_UNREGISTERED);
+		netdev->reg_state = NETREG_RELEASED;
+		class_device_put(&netdev->class_dev);
+	}
+#else
+	kfree((char *)netdev - netdev->padded);
+#endif
+}
+#endif
+
+void *_kc_kmemdup(const void *src, size_t len, unsigned gfp)
+{
+	void *p;
+
+	p = kzalloc(len, gfp);
+	if (p)
+		memcpy(p, src, len);
+	return p;
+}
+#endif /* <= 2.6.19 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21) )
+struct pci_dev *_kc_netdev_to_pdev(struct net_device *netdev)
+{
+	return ((struct adapter_struct *)netdev_priv(netdev))->pdev;
+}
+#endif /* < 2.6.21 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22) )
+/* hexdump code taken from lib/hexdump.c */
+static void _kc_hex_dump_to_buffer(const void *buf, size_t len, int rowsize,
+			int groupsize, unsigned char *linebuf,
+			size_t linebuflen, bool ascii)
+{
+	const u8 *ptr = buf;
+	u8 ch;
+	int j, lx = 0;
+	int ascii_column;
+
+	if (rowsize != 16 && rowsize != 32)
+		rowsize = 16;
+
+	if (!len)
+		goto nil;
+	if (len > rowsize)		/* limit to one line at a time */
+		len = rowsize;
+	if ((len % groupsize) != 0)	/* no mixed size output */
+		groupsize = 1;
+
+	switch (groupsize) {
+	case 8: {
+		const u64 *ptr8 = buf;
+		int ngroups = len / groupsize;
+
+		for (j = 0; j < ngroups; j++)
+			lx += scnprintf((char *)(linebuf + lx), linebuflen - lx,
+				"%s%16.16llx", j ? " " : "",
+				(unsigned long long)*(ptr8 + j));
+		ascii_column = 17 * ngroups + 2;
+		break;
+	}
+
+	case 4: {
+		const u32 *ptr4 = buf;
+		int ngroups = len / groupsize;
+
+		for (j = 0; j < ngroups; j++)
+			lx += scnprintf((char *)(linebuf + lx), linebuflen - lx,
+				"%s%8.8x", j ? " " : "", *(ptr4 + j));
+		ascii_column = 9 * ngroups + 2;
+		break;
+	}
+
+	case 2: {
+		const u16 *ptr2 = buf;
+		int ngroups = len / groupsize;
+
+		for (j = 0; j < ngroups; j++)
+			lx += scnprintf((char *)(linebuf + lx), linebuflen - lx,
+				"%s%4.4x", j ? " " : "", *(ptr2 + j));
+		ascii_column = 5 * ngroups + 2;
+		break;
+	}
+
+	default:
+		for (j = 0; (j < len) && (lx + 3) <= linebuflen; j++) {
+			ch = ptr[j];
+			linebuf[lx++] = hex_asc(ch >> 4);
+			linebuf[lx++] = hex_asc(ch & 0x0f);
+			linebuf[lx++] = ' ';
+		}
+		if (j)
+			lx--;
+
+		ascii_column = 3 * rowsize + 2;
+		break;
+	}
+	if (!ascii)
+		goto nil;
+
+	while (lx < (linebuflen - 1) && lx < (ascii_column - 1))
+		linebuf[lx++] = ' ';
+	for (j = 0; (j < len) && (lx + 2) < linebuflen; j++)
+		linebuf[lx++] = (isascii(ptr[j]) && isprint(ptr[j])) ? ptr[j]
+				: '.';
+nil:
+	linebuf[lx++] = '\0';
+}
+
+void _kc_print_hex_dump(const char *level,
+			const char *prefix_str, int prefix_type,
+			int rowsize, int groupsize,
+			const void *buf, size_t len, bool ascii)
+{
+	const u8 *ptr = buf;
+	int i, linelen, remaining = len;
+	unsigned char linebuf[200];
+
+	if (rowsize != 16 && rowsize != 32)
+		rowsize = 16;
+
+	for (i = 0; i < len; i += rowsize) {
+		linelen = min(remaining, rowsize);
+		remaining -= rowsize;
+		_kc_hex_dump_to_buffer(ptr + i, linelen, rowsize, groupsize,
+				linebuf, sizeof(linebuf), ascii);
+
+		switch (prefix_type) {
+		case DUMP_PREFIX_ADDRESS:
+			printk("%s%s%*p: %s\n", level, prefix_str,
+				(int)(2 * sizeof(void *)), ptr + i, linebuf);
+			break;
+		case DUMP_PREFIX_OFFSET:
+			printk("%s%s%.8x: %s\n", level, prefix_str, i, linebuf);
+			break;
+		default:
+			printk("%s%s%s\n", level, prefix_str, linebuf);
+			break;
+		}
+	}
+}
+
+#ifdef HAVE_I2C_SUPPORT
+struct i2c_client *
+_kc_i2c_new_device(struct i2c_adapter *adap, struct i2c_board_info const *info)
+{
+	struct i2c_client	*client;
+	int			status;
+
+	client = kzalloc(sizeof *client, GFP_KERNEL);
+	if (!client)
+		return NULL;
+
+	client->adapter = adap;
+
+	client->dev.platform_data = info->platform_data;
+
+	client->flags = info->flags;
+	client->addr = info->addr;
+
+	strlcpy(client->name, info->type, sizeof(client->name));
+
+	/* Check for address business */
+	status = i2c_check_addr(adap, client->addr);
+	if (status)
+		goto out_err;
+
+	client->dev.parent = &client->adapter->dev;
+	client->dev.bus = &i2c_bus_type;
+
+	status = i2c_attach_client(client);
+	if (status)
+		goto out_err;
+
+	dev_dbg(&adap->dev, "client [%s] registered with bus id %s\n",
+		client->name, dev_name(&client->dev));
+
+	return client;
+
+out_err:
+	dev_err(&adap->dev, "Failed to register i2c client %s at 0x%02x "
+		"(%d)\n", client->name, client->addr, status);
+	kfree(client);
+	return NULL;
+}
+#endif /* HAVE_I2C_SUPPORT */
+#endif /* < 2.6.22 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24) )
+#ifdef NAPI
+#if defined(DRIVER_IXGBE) || defined(DRIVER_IGB) || defined(DRIVER_I40E) || \
+	defined(DRIVER_IXGBEVF)
+struct net_device *napi_to_poll_dev(const struct napi_struct *napi)
+{
+	struct adapter_q_vector *q_vector = container_of(napi,
+	                                                struct adapter_q_vector,
+	                                                napi);
+	return &q_vector->poll_dev;
+}
+#endif
+
+int __kc_adapter_clean(struct net_device *netdev, int *budget)
+{
+	int work_done;
+	int work_to_do = min(*budget, netdev->quota);
+#if defined(DRIVER_IXGBE) || defined(DRIVER_IGB) || defined(DRIVER_I40E) || \
+	defined(E1000E_MQ) || defined(DRIVER_IXGBEVF)
+	/* kcompat.h netif_napi_add puts napi struct in "fake netdev->priv" */
+	struct napi_struct *napi = netdev->priv;
+#else
+	struct adapter_struct *adapter = netdev_priv(netdev);
+	struct napi_struct *napi = &adapter->rx_ring[0].napi;
+#endif
+	work_done = napi->poll(napi, work_to_do);
+	*budget -= work_done;
+	netdev->quota -= work_done;
+	return (work_done >= work_to_do) ? 1 : 0;
+}
+#endif /* NAPI */
+#endif /* <= 2.6.24 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,26) )
+void _kc_pci_disable_link_state(struct pci_dev *pdev, int state)
+{
+	struct pci_dev *parent = pdev->bus->self;
+	u16 link_state;
+	int pos;
+
+	if (!parent)
+		return;
+
+	pos = pci_find_capability(parent, PCI_CAP_ID_EXP);
+	if (pos) {
+		pci_read_config_word(parent, pos + PCI_EXP_LNKCTL, &link_state);
+		link_state &= ~state;
+		pci_write_config_word(parent, pos + PCI_EXP_LNKCTL, link_state);
+	}
+}
+#endif /* < 2.6.26 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,27) )
+#ifdef HAVE_TX_MQ
+void _kc_netif_tx_stop_all_queues(struct net_device *netdev)
+{
+	struct adapter_struct *adapter = netdev_priv(netdev);
+	int i;
+
+	netif_stop_queue(netdev);
+	if (netif_is_multiqueue(netdev))
+		for (i = 0; i < adapter->num_tx_queues; i++)
+			netif_stop_subqueue(netdev, i);
+}
+void _kc_netif_tx_wake_all_queues(struct net_device *netdev)
+{
+	struct adapter_struct *adapter = netdev_priv(netdev);
+	int i;
+
+	netif_wake_queue(netdev);
+	if (netif_is_multiqueue(netdev))
+		for (i = 0; i < adapter->num_tx_queues; i++)
+			netif_wake_subqueue(netdev, i);
+}
+void _kc_netif_tx_start_all_queues(struct net_device *netdev)
+{
+	struct adapter_struct *adapter = netdev_priv(netdev);
+	int i;
+
+	netif_start_queue(netdev);
+	if (netif_is_multiqueue(netdev))
+		for (i = 0; i < adapter->num_tx_queues; i++)
+			netif_start_subqueue(netdev, i);
+}
+#endif /* HAVE_TX_MQ */
+
+#ifndef __WARN_printf
+void __kc_warn_slowpath(const char *file, int line, const char *fmt, ...)
+{
+	va_list args;
+
+	printk(KERN_WARNING "------------[ cut here ]------------\n");
+	printk(KERN_WARNING "WARNING: at %s:%d %s()\n", file, line);
+	va_start(args, fmt);
+	vprintk(fmt, args);
+	va_end(args);
+
+	dump_stack();
+}
+#endif /* __WARN_printf */
+#endif /* < 2.6.27 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28) )
+
+int
+_kc_pci_prepare_to_sleep(struct pci_dev *dev)
+{
+	pci_power_t target_state;
+	int error;
+
+	target_state = pci_choose_state(dev, PMSG_SUSPEND);
+
+	pci_enable_wake(dev, target_state, true);
+
+	error = pci_set_power_state(dev, target_state);
+
+	if (error)
+		pci_enable_wake(dev, target_state, false);
+
+	return error;
+}
+
+int
+_kc_pci_wake_from_d3(struct pci_dev *dev, bool enable)
+{
+	int err;
+
+	err = pci_enable_wake(dev, PCI_D3cold, enable);
+	if (err)
+		goto out;
+
+	err = pci_enable_wake(dev, PCI_D3hot, enable);
+
+out:
+	return err;
+}
+#endif /* < 2.6.28 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,29) )
+static void __kc_pci_set_master(struct pci_dev *pdev, bool enable)
+{
+	u16 old_cmd, cmd;
+
+	pci_read_config_word(pdev, PCI_COMMAND, &old_cmd);
+	if (enable)
+		cmd = old_cmd | PCI_COMMAND_MASTER;
+	else
+		cmd = old_cmd & ~PCI_COMMAND_MASTER;
+	if (cmd != old_cmd) {
+		dev_dbg(pci_dev_to_dev(pdev), "%s bus mastering\n",
+			enable ? "enabling" : "disabling");
+		pci_write_config_word(pdev, PCI_COMMAND, cmd);
+	}
+#if ( LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,7) )
+	pdev->is_busmaster = enable;
+#endif
+}
+
+void _kc_pci_clear_master(struct pci_dev *dev)
+{
+	__kc_pci_set_master(dev, false);
+}
+#endif /* < 2.6.29 */
+
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,34) )
+#if (RHEL_RELEASE_CODE < RHEL_RELEASE_VERSION(6,0))
+int _kc_pci_num_vf(struct pci_dev *dev)
+{
+	int num_vf = 0;
+#ifdef CONFIG_PCI_IOV
+	struct pci_dev *vfdev;
+
+	/* loop through all ethernet devices starting at PF dev */
+	vfdev = pci_get_class(PCI_CLASS_NETWORK_ETHERNET << 8, NULL);
+	while (vfdev) {
+		if (vfdev->is_virtfn && vfdev->physfn == dev)
+			num_vf++;
+
+		vfdev = pci_get_class(PCI_CLASS_NETWORK_ETHERNET << 8, vfdev);
+	}
+
+#endif
+	return num_vf;
+}
+#endif /* RHEL_RELEASE_CODE */
+#endif /* < 2.6.34 */
+
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35) )
+#if defined(DRIVER_IXGBE) || defined(DRIVER_IGB) || defined(DRIVER_I40E) || \
+	defined(DRIVER_IXGBEVF)
+#ifdef HAVE_TX_MQ
+#if (!(RHEL_RELEASE_CODE && RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(6,0)))
+#ifndef CONFIG_NETDEVICES_MULTIQUEUE
+void _kc_netif_set_real_num_tx_queues(struct net_device *dev, unsigned int txq)
+{
+	unsigned int real_num = dev->real_num_tx_queues;
+	struct Qdisc *qdisc;
+	int i;
+
+	if (unlikely(txq > dev->num_tx_queues))
+		;
+	else if (txq > real_num)
+		dev->real_num_tx_queues = txq;
+	else if ( txq < real_num) {
+		dev->real_num_tx_queues = txq;
+		for (i = txq; i < dev->num_tx_queues; i++) {
+			qdisc = netdev_get_tx_queue(dev, i)->qdisc;
+			if (qdisc) {
+				spin_lock_bh(qdisc_lock(qdisc));
+				qdisc_reset(qdisc);
+				spin_unlock_bh(qdisc_lock(qdisc));
+			}
+		}
+	}
+}
+#endif /* CONFIG_NETDEVICES_MULTIQUEUE */
+#endif /* !(RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(6,0)) */
+#endif /* HAVE_TX_MQ */
+
+ssize_t _kc_simple_write_to_buffer(void *to, size_t available, loff_t *ppos,
+				   const void __user *from, size_t count)
+{
+        loff_t pos = *ppos;
+        size_t res;
+
+        if (pos < 0)
+                return -EINVAL;
+        if (pos >= available || !count)
+                return 0;
+        if (count > available - pos)
+                count = available - pos;
+        res = copy_from_user(to + pos, from, count);
+        if (res == count)
+                return -EFAULT;
+        count -= res;
+        *ppos = pos + count;
+        return count;
+}
+
+#endif /* defined(DRIVER_IXGBE) || defined(DRIVER_IGB) || defined(DRIVER_I40E) */
+#endif /* < 2.6.35 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,36) )
+static const u32 _kc_flags_dup_features =
+	(ETH_FLAG_LRO | ETH_FLAG_NTUPLE | ETH_FLAG_RXHASH);
+
+u32 _kc_ethtool_op_get_flags(struct net_device *dev)
+{
+	return dev->features & _kc_flags_dup_features;
+}
+
+int _kc_ethtool_op_set_flags(struct net_device *dev, u32 data, u32 supported)
+{
+	if (data & ~supported)
+		return -EINVAL;
+
+	dev->features = ((dev->features & ~_kc_flags_dup_features) |
+			 (data & _kc_flags_dup_features));
+	return 0;
+}
+#endif /* < 2.6.36 */
+
+/******************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,39) )
+#if (!(RHEL_RELEASE_CODE && RHEL_RELEASE_CODE > RHEL_RELEASE_VERSION(6,0)))
+
+
+
+#endif /* !(RHEL_RELEASE_CODE > RHEL_RELEASE_VERSION(6,0)) */
+#endif /* < 2.6.39 */
+
+/******************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,4,0) )
+void _kc_skb_add_rx_frag(struct sk_buff *skb, int i, struct page *page,
+			 int off, int size, unsigned int truesize)
+{
+	skb_fill_page_desc(skb, i, page, off, size);
+	skb->len += size;
+	skb->data_len += size;
+	skb->truesize += truesize;
+}
+
+int _kc_simple_open(struct inode *inode, struct file *file)
+{
+        if (inode->i_private)
+                file->private_data = inode->i_private;
+
+        return 0;
+}
+
+#endif /* < 3.4.0 */
+
+/******************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0) )
+static inline int __kc_pcie_cap_version(struct pci_dev *dev)
+{
+	int pos;
+	u16 reg16;
+
+	pos = pci_find_capability(dev, PCI_CAP_ID_EXP);
+	if (!pos)
+		return 0;
+	pci_read_config_word(dev, pos + PCI_EXP_FLAGS, &reg16);
+	return reg16 & PCI_EXP_FLAGS_VERS;
+}
+
+static inline bool __kc_pcie_cap_has_devctl(const struct pci_dev __always_unused *dev)
+{
+	return true;
+}
+
+static inline bool __kc_pcie_cap_has_lnkctl(struct pci_dev *dev)
+{
+	int type = pci_pcie_type(dev);
+
+	return __kc_pcie_cap_version(dev) > 1 ||
+	       type == PCI_EXP_TYPE_ROOT_PORT ||
+	       type == PCI_EXP_TYPE_ENDPOINT ||
+	       type == PCI_EXP_TYPE_LEG_END;
+}
+
+static inline bool __kc_pcie_cap_has_sltctl(struct pci_dev *dev)
+{
+	int type = pci_pcie_type(dev);
+	int pos;
+	u16 pcie_flags_reg;
+
+	pos = pci_find_capability(dev, PCI_CAP_ID_EXP);
+	if (!pos)
+		return 0;
+	pci_read_config_word(dev, pos + PCI_EXP_FLAGS, &pcie_flags_reg);
+
+	return __kc_pcie_cap_version(dev) > 1 ||
+	       type == PCI_EXP_TYPE_ROOT_PORT ||
+	       (type == PCI_EXP_TYPE_DOWNSTREAM &&
+		pcie_flags_reg & PCI_EXP_FLAGS_SLOT);
+}
+
+static inline bool __kc_pcie_cap_has_rtctl(struct pci_dev *dev)
+{
+	int type = pci_pcie_type(dev);
+
+	return __kc_pcie_cap_version(dev) > 1 ||
+	       type == PCI_EXP_TYPE_ROOT_PORT ||
+	       type == PCI_EXP_TYPE_RC_EC;
+}
+
+static bool __kc_pcie_capability_reg_implemented(struct pci_dev *dev, int pos)
+{
+	if (!pci_is_pcie(dev))
+		return false;
+
+	switch (pos) {
+	case PCI_EXP_FLAGS_TYPE:
+		return true;
+	case PCI_EXP_DEVCAP:
+	case PCI_EXP_DEVCTL:
+	case PCI_EXP_DEVSTA:
+		return __kc_pcie_cap_has_devctl(dev);
+	case PCI_EXP_LNKCAP:
+	case PCI_EXP_LNKCTL:
+	case PCI_EXP_LNKSTA:
+		return __kc_pcie_cap_has_lnkctl(dev);
+	case PCI_EXP_SLTCAP:
+	case PCI_EXP_SLTCTL:
+	case PCI_EXP_SLTSTA:
+		return __kc_pcie_cap_has_sltctl(dev);
+	case PCI_EXP_RTCTL:
+	case PCI_EXP_RTCAP:
+	case PCI_EXP_RTSTA:
+		return __kc_pcie_cap_has_rtctl(dev);
+	case PCI_EXP_DEVCAP2:
+	case PCI_EXP_DEVCTL2:
+	case PCI_EXP_LNKCAP2:
+	case PCI_EXP_LNKCTL2:
+	case PCI_EXP_LNKSTA2:
+		return __kc_pcie_cap_version(dev) > 1;
+	default:
+		return false;
+	}
+}
+
+/*
+ * Note that these accessor functions are only for the "PCI Express
+ * Capability" (see PCIe spec r3.0, sec 7.8).  They do not apply to the
+ * other "PCI Express Extended Capabilities" (AER, VC, ACS, MFVC, etc.)
+ */
+int __kc_pcie_capability_read_word(struct pci_dev *dev, int pos, u16 *val)
+{
+	int ret;
+
+	*val = 0;
+	if (pos & 1)
+		return -EINVAL;
+
+	if (__kc_pcie_capability_reg_implemented(dev, pos)) {
+		ret = pci_read_config_word(dev, pci_pcie_cap(dev) + pos, val);
+		/*
+		 * Reset *val to 0 if pci_read_config_word() fails, it may
+		 * have been written as 0xFFFF if hardware error happens
+		 * during pci_read_config_word().
+		 */
+		if (ret)
+			*val = 0;
+		return ret;
+	}
+
+	/*
+	 * For Functions that do not implement the Slot Capabilities,
+	 * Slot Status, and Slot Control registers, these spaces must
+	 * be hardwired to 0b, with the exception of the Presence Detect
+	 * State bit in the Slot Status register of Downstream Ports,
+	 * which must be hardwired to 1b.  (PCIe Base Spec 3.0, sec 7.8)
+	 */
+	if (pci_is_pcie(dev) && pos == PCI_EXP_SLTSTA &&
+	    pci_pcie_type(dev) == PCI_EXP_TYPE_DOWNSTREAM) {
+		*val = PCI_EXP_SLTSTA_PDS;
+	}
+
+	return 0;
+}
+
+int __kc_pcie_capability_write_word(struct pci_dev *dev, int pos, u16 val)
+{
+	if (pos & 1)
+		return -EINVAL;
+
+	if (!__kc_pcie_capability_reg_implemented(dev, pos))
+		return 0;
+
+	return pci_write_config_word(dev, pci_pcie_cap(dev) + pos, val);
+}
+
+int __kc_pcie_capability_clear_and_set_word(struct pci_dev *dev, int pos,
+					    u16 clear, u16 set)
+{
+	int ret;
+	u16 val;
+
+	ret = __kc_pcie_capability_read_word(dev, pos, &val);
+	if (!ret) {
+		val &= ~clear;
+		val |= set;
+		ret = __kc_pcie_capability_write_word(dev, pos, val);
+	}
+
+	return ret;
+}
+
+int __kc_pcie_capability_clear_word(struct pci_dev *dev, int pos,
+					     u16 clear)
+{
+	return __kc_pcie_capability_clear_and_set_word(dev, pos, clear, 0);
+}
+#endif /* < 3.7.0 */
+
+/******************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,9,0) )
+#endif /* 3.9.0 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,10,0) )
+#ifdef CONFIG_PCI_IOV
+int __kc_pci_vfs_assigned(struct pci_dev *dev)
+{
+	unsigned int vfs_assigned = 0;
+#ifdef HAVE_PCI_DEV_FLAGS_ASSIGNED
+	int pos;
+	struct pci_dev *vfdev;
+	unsigned short dev_id;
+
+	/* only search if we are a PF */
+	if (!dev->is_physfn)
+		return 0;
+
+	/* find SR-IOV capability */
+	pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_SRIOV);
+	if (!pos)
+		return 0;
+
+	/*
+	 * determine the device ID for the VFs, the vendor ID will be the
+	 * same as the PF so there is no need to check for that one
+	 */
+	pci_read_config_word(dev, pos + PCI_SRIOV_VF_DID, &dev_id);
+
+	/* loop through all the VFs to see if we own any that are assigned */
+	vfdev = pci_get_device(dev->vendor, dev_id, NULL);
+	while (vfdev) {
+		/*
+		 * It is considered assigned if it is a virtual function with
+		 * our dev as the physical function and the assigned bit is set
+		 */
+		if (vfdev->is_virtfn && (vfdev->physfn == dev) &&
+		    (vfdev->dev_flags & PCI_DEV_FLAGS_ASSIGNED))
+			vfs_assigned++;
+
+		vfdev = pci_get_device(dev->vendor, dev_id, vfdev);
+	}
+
+#endif /* HAVE_PCI_DEV_FLAGS_ASSIGNED */
+	return vfs_assigned;
+}
+
+#endif /* CONFIG_PCI_IOV */
+#endif /* 3.10.0 */
diff --git a/drivers/net/igb/kcompat.h b/drivers/net/igb/kcompat.h
new file mode 100644
index 000000000000..7d2098117b54
--- /dev/null
+++ b/drivers/net/igb/kcompat.h
@@ -0,0 +1,3961 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+#ifndef _KCOMPAT_H_
+#define _KCOMPAT_H_
+
+#ifndef LINUX_VERSION_CODE
+#include <linux/version.h>
+#else
+#define KERNEL_VERSION(a,b,c) (((a) << 16) + ((b) << 8) + (c))
+#endif
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/skbuff.h>
+#include <linux/ioport.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/udp.h>
+#include <linux/mii.h>
+#include <linux/vmalloc.h>
+#include <asm/io.h>
+#include <linux/ethtool.h>
+#include <linux/if_vlan.h>
+
+/* NAPI enable/disable flags here */
+#define NAPI
+
+#define adapter_struct igb_adapter
+#define adapter_q_vector igb_q_vector
+#define NAPI
+
+/* and finally set defines so that the code sees the changes */
+#ifdef NAPI
+#else
+#endif /* NAPI */
+
+/* Dynamic LTR and deeper C-State support disable/enable */
+
+/* packet split disable/enable */
+#ifdef DISABLE_PACKET_SPLIT
+#ifndef CONFIG_IGB_DISABLE_PACKET_SPLIT
+#define CONFIG_IGB_DISABLE_PACKET_SPLIT
+#endif
+#endif /* DISABLE_PACKET_SPLIT */
+
+/* MSI compatibility code for all kernels and drivers */
+#ifdef DISABLE_PCI_MSI
+#undef CONFIG_PCI_MSI
+#endif
+#ifndef CONFIG_PCI_MSI
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,8) )
+struct msix_entry {
+	u16 vector; /* kernel uses to write allocated vector */
+	u16 entry;  /* driver uses to specify entry, OS writes */
+};
+#endif
+#undef pci_enable_msi
+#define pci_enable_msi(a) -ENOTSUPP
+#undef pci_disable_msi
+#define pci_disable_msi(a) do {} while (0)
+#undef pci_enable_msix
+#define pci_enable_msix(a, b, c) -ENOTSUPP
+#undef pci_disable_msix
+#define pci_disable_msix(a) do {} while (0)
+#define msi_remove_pci_irq_vectors(a) do {} while (0)
+#endif /* CONFIG_PCI_MSI */
+#ifdef DISABLE_PM
+#undef CONFIG_PM
+#endif
+
+#ifdef DISABLE_NET_POLL_CONTROLLER
+#undef CONFIG_NET_POLL_CONTROLLER
+#endif
+
+#ifndef PMSG_SUSPEND
+#define PMSG_SUSPEND 3
+#endif
+
+/* generic boolean compatibility */
+#undef TRUE
+#undef FALSE
+#define TRUE true
+#define FALSE false
+#ifdef GCC_VERSION
+#if ( GCC_VERSION < 3000 )
+#define _Bool char
+#endif
+#else
+#define _Bool char
+#endif
+
+/* kernels less than 2.4.14 don't have this */
+#ifndef ETH_P_8021Q
+#define ETH_P_8021Q 0x8100
+#endif
+
+#ifndef module_param
+#define module_param(v,t,p) MODULE_PARM(v, "i");
+#endif
+
+#ifndef DMA_64BIT_MASK
+#define DMA_64BIT_MASK  0xffffffffffffffffULL
+#endif
+
+#ifndef DMA_32BIT_MASK
+#define DMA_32BIT_MASK  0x00000000ffffffffULL
+#endif
+
+#ifndef PCI_CAP_ID_EXP
+#define PCI_CAP_ID_EXP 0x10
+#endif
+
+#ifndef PCIE_LINK_STATE_L0S
+#define PCIE_LINK_STATE_L0S 1
+#endif
+#ifndef PCIE_LINK_STATE_L1
+#define PCIE_LINK_STATE_L1 2
+#endif
+
+#ifndef mmiowb
+#ifdef CONFIG_IA64
+#define mmiowb() asm volatile ("mf.a" ::: "memory")
+#else
+#define mmiowb()
+#endif
+#endif
+
+#ifndef SET_NETDEV_DEV
+#define SET_NETDEV_DEV(net, pdev)
+#endif
+
+#if !defined(HAVE_FREE_NETDEV) && ( LINUX_VERSION_CODE < KERNEL_VERSION(3,1,0) )
+#define free_netdev(x)	kfree(x)
+#endif
+
+#ifdef HAVE_POLL_CONTROLLER
+#define CONFIG_NET_POLL_CONTROLLER
+#endif
+
+#ifndef SKB_DATAREF_SHIFT
+/* if we do not have the infrastructure to detect if skb_header is cloned
+   just return false in all cases */
+#define skb_header_cloned(x) 0
+#endif
+
+#ifndef NETIF_F_GSO
+#define gso_size tso_size
+#define gso_segs tso_segs
+#endif
+
+#ifndef NETIF_F_GRO
+#define vlan_gro_receive(_napi, _vlgrp, _vlan, _skb) \
+		vlan_hwaccel_receive_skb(_skb, _vlgrp, _vlan)
+#define napi_gro_receive(_napi, _skb) netif_receive_skb(_skb)
+#endif
+
+#ifndef NETIF_F_SCTP_CSUM
+#define NETIF_F_SCTP_CSUM 0
+#endif
+
+#ifndef NETIF_F_LRO
+#define NETIF_F_LRO (1 << 15)
+#endif
+
+#ifndef NETIF_F_NTUPLE
+#define NETIF_F_NTUPLE (1 << 27)
+#endif
+
+#ifndef IPPROTO_SCTP
+#define IPPROTO_SCTP 132
+#endif
+
+#ifndef CHECKSUM_PARTIAL
+#define CHECKSUM_PARTIAL CHECKSUM_HW
+#define CHECKSUM_COMPLETE CHECKSUM_HW
+#endif
+
+#ifndef __read_mostly
+#define __read_mostly
+#endif
+
+#ifndef MII_RESV1
+#define MII_RESV1		0x17		/* Reserved...		*/
+#endif
+
+#ifndef unlikely
+#define unlikely(_x) _x
+#define likely(_x) _x
+#endif
+
+#ifndef WARN_ON
+#define WARN_ON(x)
+#endif
+
+#ifndef PCI_DEVICE
+#define PCI_DEVICE(vend,dev) \
+	.vendor = (vend), .device = (dev), \
+	.subvendor = PCI_ANY_ID, .subdevice = PCI_ANY_ID
+#endif
+
+#ifndef node_online
+#define node_online(node) ((node) == 0)
+#endif
+
+#ifndef num_online_cpus
+#define num_online_cpus() smp_num_cpus
+#endif
+
+#ifndef cpu_online
+#define cpu_online(cpuid) test_bit((cpuid), &cpu_online_map)
+#endif
+
+#ifndef _LINUX_RANDOM_H
+#include <linux/random.h>
+#endif
+
+#ifndef DECLARE_BITMAP
+#ifndef BITS_TO_LONGS
+#define BITS_TO_LONGS(bits) (((bits)+BITS_PER_LONG-1)/BITS_PER_LONG)
+#endif
+#define DECLARE_BITMAP(name,bits) long name[BITS_TO_LONGS(bits)]
+#endif
+
+#ifndef VLAN_HLEN
+#define VLAN_HLEN 4
+#endif
+
+#ifndef VLAN_ETH_HLEN
+#define VLAN_ETH_HLEN 18
+#endif
+
+#ifndef VLAN_ETH_FRAME_LEN
+#define VLAN_ETH_FRAME_LEN 1518
+#endif
+
+#if !defined(IXGBE_DCA) && !defined(IGB_DCA)
+#define dca_get_tag(b) 0
+#define dca_add_requester(a) -1
+#define dca_remove_requester(b) do { } while(0)
+#define DCA_PROVIDER_ADD     0x0001
+#define DCA_PROVIDER_REMOVE  0x0002
+#endif
+
+#ifndef DCA_GET_TAG_TWO_ARGS
+#define dca3_get_tag(a,b) dca_get_tag(b)
+#endif
+
+#ifndef CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS
+#if defined(__i386__) || defined(__x86_64__)
+#define CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS
+#endif
+#endif
+
+/* taken from 2.6.24 definition in linux/kernel.h */
+#ifndef IS_ALIGNED
+#define IS_ALIGNED(x,a)         (((x) % ((typeof(x))(a))) == 0)
+#endif
+
+#ifdef IS_ENABLED
+#undef IS_ENABLED
+#undef __ARG_PLACEHOLDER_1
+#undef config_enabled
+#undef _config_enabled
+#undef __config_enabled
+#undef ___config_enabled
+#endif
+
+#define __ARG_PLACEHOLDER_1 0,
+#define config_enabled(cfg) _config_enabled(cfg)
+#define _config_enabled(value) __config_enabled(__ARG_PLACEHOLDER_##value)
+#define __config_enabled(arg1_or_junk) ___config_enabled(arg1_or_junk 1, 0)
+#define ___config_enabled(__ignored, val, ...) val
+
+#define IS_ENABLED(option) \
+	(config_enabled(option) || config_enabled(option##_MODULE))
+
+#if !defined(NETIF_F_HW_VLAN_TX) && !defined(NETIF_F_HW_VLAN_CTAG_TX)
+struct _kc_vlan_ethhdr {
+	unsigned char	h_dest[ETH_ALEN];
+	unsigned char	h_source[ETH_ALEN];
+	__be16		h_vlan_proto;
+	__be16		h_vlan_TCI;
+	__be16		h_vlan_encapsulated_proto;
+};
+#define vlan_ethhdr _kc_vlan_ethhdr
+struct _kc_vlan_hdr {
+	__be16		h_vlan_TCI;
+	__be16		h_vlan_encapsulated_proto;
+};
+#define vlan_hdr _kc_vlan_hdr
+#define vlan_tx_tag_present(_skb) 0
+#define vlan_tx_tag_get(_skb) 0
+#endif /* NETIF_F_HW_VLAN_TX && NETIF_F_HW_VLAN_CTAG_TX */
+
+#ifndef VLAN_PRIO_SHIFT
+#define VLAN_PRIO_SHIFT 13
+#endif
+
+
+#ifndef __GFP_COLD
+#define __GFP_COLD 0
+#endif
+
+#ifndef __GFP_COMP
+#define __GFP_COMP 0
+#endif
+
+/*****************************************************************************/
+/* Installations with ethtool version without eeprom, adapter id, or statistics
+ * support */
+
+#ifndef ETH_GSTRING_LEN
+#define ETH_GSTRING_LEN 32
+#endif
+
+#ifndef ETHTOOL_GSTATS
+#define ETHTOOL_GSTATS 0x1d
+#undef ethtool_drvinfo
+#define ethtool_drvinfo k_ethtool_drvinfo
+struct k_ethtool_drvinfo {
+	u32 cmd;
+	char driver[32];
+	char version[32];
+	char fw_version[32];
+	char bus_info[32];
+	char reserved1[32];
+	char reserved2[16];
+	u32 n_stats;
+	u32 testinfo_len;
+	u32 eedump_len;
+	u32 regdump_len;
+};
+
+struct ethtool_stats {
+	u32 cmd;
+	u32 n_stats;
+	u64 data[0];
+};
+#endif /* ETHTOOL_GSTATS */
+
+#ifndef ETHTOOL_PHYS_ID
+#define ETHTOOL_PHYS_ID 0x1c
+#endif /* ETHTOOL_PHYS_ID */
+
+#ifndef ETHTOOL_GSTRINGS
+#define ETHTOOL_GSTRINGS 0x1b
+enum ethtool_stringset {
+	ETH_SS_TEST             = 0,
+	ETH_SS_STATS,
+};
+struct ethtool_gstrings {
+	u32 cmd;            /* ETHTOOL_GSTRINGS */
+	u32 string_set;     /* string set id e.c. ETH_SS_TEST, etc*/
+	u32 len;            /* number of strings in the string set */
+	u8 data[0];
+};
+#endif /* ETHTOOL_GSTRINGS */
+
+#ifndef ETHTOOL_TEST
+#define ETHTOOL_TEST 0x1a
+enum ethtool_test_flags {
+	ETH_TEST_FL_OFFLINE	= (1 << 0),
+	ETH_TEST_FL_FAILED	= (1 << 1),
+};
+struct ethtool_test {
+	u32 cmd;
+	u32 flags;
+	u32 reserved;
+	u32 len;
+	u64 data[0];
+};
+#endif /* ETHTOOL_TEST */
+
+#ifndef ETHTOOL_GEEPROM
+#define ETHTOOL_GEEPROM 0xb
+#undef ETHTOOL_GREGS
+struct ethtool_eeprom {
+	u32 cmd;
+	u32 magic;
+	u32 offset;
+	u32 len;
+	u8 data[0];
+};
+
+struct ethtool_value {
+	u32 cmd;
+	u32 data;
+};
+#endif /* ETHTOOL_GEEPROM */
+
+#ifndef ETHTOOL_GLINK
+#define ETHTOOL_GLINK 0xa
+#endif /* ETHTOOL_GLINK */
+
+#ifndef ETHTOOL_GWOL
+#define ETHTOOL_GWOL 0x5
+#define ETHTOOL_SWOL 0x6
+#define SOPASS_MAX      6
+struct ethtool_wolinfo {
+	u32 cmd;
+	u32 supported;
+	u32 wolopts;
+	u8 sopass[SOPASS_MAX]; /* SecureOn(tm) password */
+};
+#endif /* ETHTOOL_GWOL */
+
+#ifndef ETHTOOL_GREGS
+#define ETHTOOL_GREGS		0x00000004 /* Get NIC registers */
+#define ethtool_regs _kc_ethtool_regs
+/* for passing big chunks of data */
+struct _kc_ethtool_regs {
+	u32 cmd;
+	u32 version; /* driver-specific, indicates different chips/revs */
+	u32 len; /* bytes */
+	u8 data[0];
+};
+#endif /* ETHTOOL_GREGS */
+
+#ifndef ETHTOOL_GMSGLVL
+#define ETHTOOL_GMSGLVL		0x00000007 /* Get driver message level */
+#endif
+#ifndef ETHTOOL_SMSGLVL
+#define ETHTOOL_SMSGLVL		0x00000008 /* Set driver msg level, priv. */
+#endif
+#ifndef ETHTOOL_NWAY_RST
+#define ETHTOOL_NWAY_RST	0x00000009 /* Restart autonegotiation, priv */
+#endif
+#ifndef ETHTOOL_GLINK
+#define ETHTOOL_GLINK		0x0000000a /* Get link status */
+#endif
+#ifndef ETHTOOL_GEEPROM
+#define ETHTOOL_GEEPROM		0x0000000b /* Get EEPROM data */
+#endif
+#ifndef ETHTOOL_SEEPROM
+#define ETHTOOL_SEEPROM		0x0000000c /* Set EEPROM data */
+#endif
+#ifndef ETHTOOL_GCOALESCE
+#define ETHTOOL_GCOALESCE	0x0000000e /* Get coalesce config */
+/* for configuring coalescing parameters of chip */
+#define ethtool_coalesce _kc_ethtool_coalesce
+struct _kc_ethtool_coalesce {
+	u32	cmd;	/* ETHTOOL_{G,S}COALESCE */
+
+	/* How many usecs to delay an RX interrupt after
+	 * a packet arrives.  If 0, only rx_max_coalesced_frames
+	 * is used.
+	 */
+	u32	rx_coalesce_usecs;
+
+	/* How many packets to delay an RX interrupt after
+	 * a packet arrives.  If 0, only rx_coalesce_usecs is
+	 * used.  It is illegal to set both usecs and max frames
+	 * to zero as this would cause RX interrupts to never be
+	 * generated.
+	 */
+	u32	rx_max_coalesced_frames;
+
+	/* Same as above two parameters, except that these values
+	 * apply while an IRQ is being serviced by the host.  Not
+	 * all cards support this feature and the values are ignored
+	 * in that case.
+	 */
+	u32	rx_coalesce_usecs_irq;
+	u32	rx_max_coalesced_frames_irq;
+
+	/* How many usecs to delay a TX interrupt after
+	 * a packet is sent.  If 0, only tx_max_coalesced_frames
+	 * is used.
+	 */
+	u32	tx_coalesce_usecs;
+
+	/* How many packets to delay a TX interrupt after
+	 * a packet is sent.  If 0, only tx_coalesce_usecs is
+	 * used.  It is illegal to set both usecs and max frames
+	 * to zero as this would cause TX interrupts to never be
+	 * generated.
+	 */
+	u32	tx_max_coalesced_frames;
+
+	/* Same as above two parameters, except that these values
+	 * apply while an IRQ is being serviced by the host.  Not
+	 * all cards support this feature and the values are ignored
+	 * in that case.
+	 */
+	u32	tx_coalesce_usecs_irq;
+	u32	tx_max_coalesced_frames_irq;
+
+	/* How many usecs to delay in-memory statistics
+	 * block updates.  Some drivers do not have an in-memory
+	 * statistic block, and in such cases this value is ignored.
+	 * This value must not be zero.
+	 */
+	u32	stats_block_coalesce_usecs;
+
+	/* Adaptive RX/TX coalescing is an algorithm implemented by
+	 * some drivers to improve latency under low packet rates and
+	 * improve throughput under high packet rates.  Some drivers
+	 * only implement one of RX or TX adaptive coalescing.  Anything
+	 * not implemented by the driver causes these values to be
+	 * silently ignored.
+	 */
+	u32	use_adaptive_rx_coalesce;
+	u32	use_adaptive_tx_coalesce;
+
+	/* When the packet rate (measured in packets per second)
+	 * is below pkt_rate_low, the {rx,tx}_*_low parameters are
+	 * used.
+	 */
+	u32	pkt_rate_low;
+	u32	rx_coalesce_usecs_low;
+	u32	rx_max_coalesced_frames_low;
+	u32	tx_coalesce_usecs_low;
+	u32	tx_max_coalesced_frames_low;
+
+	/* When the packet rate is below pkt_rate_high but above
+	 * pkt_rate_low (both measured in packets per second) the
+	 * normal {rx,tx}_* coalescing parameters are used.
+	 */
+
+	/* When the packet rate is (measured in packets per second)
+	 * is above pkt_rate_high, the {rx,tx}_*_high parameters are
+	 * used.
+	 */
+	u32	pkt_rate_high;
+	u32	rx_coalesce_usecs_high;
+	u32	rx_max_coalesced_frames_high;
+	u32	tx_coalesce_usecs_high;
+	u32	tx_max_coalesced_frames_high;
+
+	/* How often to do adaptive coalescing packet rate sampling,
+	 * measured in seconds.  Must not be zero.
+	 */
+	u32	rate_sample_interval;
+};
+#endif /* ETHTOOL_GCOALESCE */
+
+#ifndef ETHTOOL_SCOALESCE
+#define ETHTOOL_SCOALESCE	0x0000000f /* Set coalesce config. */
+#endif
+#ifndef ETHTOOL_GRINGPARAM
+#define ETHTOOL_GRINGPARAM	0x00000010 /* Get ring parameters */
+/* for configuring RX/TX ring parameters */
+#define ethtool_ringparam _kc_ethtool_ringparam
+struct _kc_ethtool_ringparam {
+	u32	cmd;	/* ETHTOOL_{G,S}RINGPARAM */
+
+	/* Read only attributes.  These indicate the maximum number
+	 * of pending RX/TX ring entries the driver will allow the
+	 * user to set.
+	 */
+	u32	rx_max_pending;
+	u32	rx_mini_max_pending;
+	u32	rx_jumbo_max_pending;
+	u32	tx_max_pending;
+
+	/* Values changeable by the user.  The valid values are
+	 * in the range 1 to the "*_max_pending" counterpart above.
+	 */
+	u32	rx_pending;
+	u32	rx_mini_pending;
+	u32	rx_jumbo_pending;
+	u32	tx_pending;
+};
+#endif /* ETHTOOL_GRINGPARAM */
+
+#ifndef ETHTOOL_SRINGPARAM
+#define ETHTOOL_SRINGPARAM	0x00000011 /* Set ring parameters, priv. */
+#endif
+#ifndef ETHTOOL_GPAUSEPARAM
+#define ETHTOOL_GPAUSEPARAM	0x00000012 /* Get pause parameters */
+/* for configuring link flow control parameters */
+#define ethtool_pauseparam _kc_ethtool_pauseparam
+struct _kc_ethtool_pauseparam {
+	u32	cmd;	/* ETHTOOL_{G,S}PAUSEPARAM */
+
+	/* If the link is being auto-negotiated (via ethtool_cmd.autoneg
+	 * being true) the user may set 'autoneg' here non-zero to have the
+	 * pause parameters be auto-negotiated too.  In such a case, the
+	 * {rx,tx}_pause values below determine what capabilities are
+	 * advertised.
+	 *
+	 * If 'autoneg' is zero or the link is not being auto-negotiated,
+	 * then {rx,tx}_pause force the driver to use/not-use pause
+	 * flow control.
+	 */
+	u32	autoneg;
+	u32	rx_pause;
+	u32	tx_pause;
+};
+#endif /* ETHTOOL_GPAUSEPARAM */
+
+#ifndef ETHTOOL_SPAUSEPARAM
+#define ETHTOOL_SPAUSEPARAM	0x00000013 /* Set pause parameters. */
+#endif
+#ifndef ETHTOOL_GRXCSUM
+#define ETHTOOL_GRXCSUM		0x00000014 /* Get RX hw csum enable (ethtool_value) */
+#endif
+#ifndef ETHTOOL_SRXCSUM
+#define ETHTOOL_SRXCSUM		0x00000015 /* Set RX hw csum enable (ethtool_value) */
+#endif
+#ifndef ETHTOOL_GTXCSUM
+#define ETHTOOL_GTXCSUM		0x00000016 /* Get TX hw csum enable (ethtool_value) */
+#endif
+#ifndef ETHTOOL_STXCSUM
+#define ETHTOOL_STXCSUM		0x00000017 /* Set TX hw csum enable (ethtool_value) */
+#endif
+#ifndef ETHTOOL_GSG
+#define ETHTOOL_GSG		0x00000018 /* Get scatter-gather enable
+					    * (ethtool_value) */
+#endif
+#ifndef ETHTOOL_SSG
+#define ETHTOOL_SSG		0x00000019 /* Set scatter-gather enable
+					    * (ethtool_value). */
+#endif
+#ifndef ETHTOOL_TEST
+#define ETHTOOL_TEST		0x0000001a /* execute NIC self-test, priv. */
+#endif
+#ifndef ETHTOOL_GSTRINGS
+#define ETHTOOL_GSTRINGS	0x0000001b /* get specified string set */
+#endif
+#ifndef ETHTOOL_PHYS_ID
+#define ETHTOOL_PHYS_ID		0x0000001c /* identify the NIC */
+#endif
+#ifndef ETHTOOL_GSTATS
+#define ETHTOOL_GSTATS		0x0000001d /* get NIC-specific statistics */
+#endif
+#ifndef ETHTOOL_GTSO
+#define ETHTOOL_GTSO		0x0000001e /* Get TSO enable (ethtool_value) */
+#endif
+#ifndef ETHTOOL_STSO
+#define ETHTOOL_STSO		0x0000001f /* Set TSO enable (ethtool_value) */
+#endif
+
+#ifndef ETHTOOL_BUSINFO_LEN
+#define ETHTOOL_BUSINFO_LEN	32
+#endif
+
+#ifndef RHEL_RELEASE_VERSION
+#define RHEL_RELEASE_VERSION(a,b) (((a) << 8) + (b))
+#endif
+#ifndef AX_RELEASE_VERSION
+#define AX_RELEASE_VERSION(a,b) (((a) << 8) + (b))
+#endif
+
+#ifndef AX_RELEASE_CODE
+#define AX_RELEASE_CODE 0
+#endif
+
+#if (AX_RELEASE_CODE && AX_RELEASE_CODE == AX_RELEASE_VERSION(3,0))
+#define RHEL_RELEASE_CODE RHEL_RELEASE_VERSION(5,0)
+#elif (AX_RELEASE_CODE && AX_RELEASE_CODE == AX_RELEASE_VERSION(3,1))
+#define RHEL_RELEASE_CODE RHEL_RELEASE_VERSION(5,1)
+#elif (AX_RELEASE_CODE && AX_RELEASE_CODE == AX_RELEASE_VERSION(3,2))
+#define RHEL_RELEASE_CODE RHEL_RELEASE_VERSION(5,3)
+#endif
+
+#ifndef RHEL_RELEASE_CODE
+/* NOTE: RHEL_RELEASE_* introduced in RHEL4.5 */
+#define RHEL_RELEASE_CODE 0
+#endif
+
+/* SuSE version macro is the same as Linux kernel version */
+#ifndef SLE_VERSION
+#define SLE_VERSION(a,b,c) KERNEL_VERSION(a,b,c)
+#endif
+#ifdef CONFIG_SUSE_KERNEL
+#if ( LINUX_VERSION_CODE == KERNEL_VERSION(2,6,27) )
+/* SLES11 GA is 2.6.27 based */
+#define SLE_VERSION_CODE SLE_VERSION(11,0,0)
+#elif ( LINUX_VERSION_CODE == KERNEL_VERSION(2,6,32) )
+/* SLES11 SP1 is 2.6.32 based */
+#define SLE_VERSION_CODE SLE_VERSION(11,1,0)
+#elif ( LINUX_VERSION_CODE == KERNEL_VERSION(3,0,13) )
+/* SLES11 SP2 is 3.0.13 based */
+#define SLE_VERSION_CODE SLE_VERSION(11,2,0)
+#elif ((LINUX_VERSION_CODE == KERNEL_VERSION(3,0,76)))
+/* SLES11 SP3 is 3.0.76 based */
+#define SLE_VERSION_CODE SLE_VERSION(11,3,0)
+#endif /* LINUX_VERSION_CODE == KERNEL_VERSION(x,y,z) */
+#endif /* CONFIG_SUSE_KERNEL */
+#ifndef SLE_VERSION_CODE
+#define SLE_VERSION_CODE 0
+#endif /* SLE_VERSION_CODE */
+
+#ifdef __KLOCWORK__
+#ifdef ARRAY_SIZE
+#undef ARRAY_SIZE
+#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
+#endif
+#endif /* __KLOCWORK__ */
+
+/*****************************************************************************/
+/* 2.4.3 => 2.4.0 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,3) )
+
+/**************************************/
+/* PCI DRIVER API */
+
+#ifndef pci_set_dma_mask
+#define pci_set_dma_mask _kc_pci_set_dma_mask
+extern int _kc_pci_set_dma_mask(struct pci_dev *dev, dma_addr_t mask);
+#endif
+
+#ifndef pci_request_regions
+#define pci_request_regions _kc_pci_request_regions
+extern int _kc_pci_request_regions(struct pci_dev *pdev, char *res_name);
+#endif
+
+#ifndef pci_release_regions
+#define pci_release_regions _kc_pci_release_regions
+extern void _kc_pci_release_regions(struct pci_dev *pdev);
+#endif
+
+/**************************************/
+/* NETWORK DRIVER API */
+
+#ifndef alloc_etherdev
+#define alloc_etherdev _kc_alloc_etherdev
+extern struct net_device * _kc_alloc_etherdev(int sizeof_priv);
+#endif
+
+#ifndef is_valid_ether_addr
+#define is_valid_ether_addr _kc_is_valid_ether_addr
+extern int _kc_is_valid_ether_addr(u8 *addr);
+#endif
+
+/**************************************/
+/* MISCELLANEOUS */
+
+#ifndef INIT_TQUEUE
+#define INIT_TQUEUE(_tq, _routine, _data)		\
+	do {						\
+		INIT_LIST_HEAD(&(_tq)->list);		\
+		(_tq)->sync = 0;			\
+		(_tq)->routine = _routine;		\
+		(_tq)->data = _data;			\
+	} while (0)
+#endif
+
+#endif /* 2.4.3 => 2.4.0 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,5) )
+/* Generic MII registers. */
+#define MII_BMCR            0x00        /* Basic mode control register */
+#define MII_BMSR            0x01        /* Basic mode status register  */
+#define MII_PHYSID1         0x02        /* PHYS ID 1                   */
+#define MII_PHYSID2         0x03        /* PHYS ID 2                   */
+#define MII_ADVERTISE       0x04        /* Advertisement control reg   */
+#define MII_LPA             0x05        /* Link partner ability reg    */
+#define MII_EXPANSION       0x06        /* Expansion register          */
+/* Basic mode control register. */
+#define BMCR_FULLDPLX           0x0100  /* Full duplex                 */
+#define BMCR_ANENABLE           0x1000  /* Enable auto negotiation     */
+/* Basic mode status register. */
+#define BMSR_ERCAP              0x0001  /* Ext-reg capability          */
+#define BMSR_ANEGCAPABLE        0x0008  /* Able to do auto-negotiation */
+#define BMSR_10HALF             0x0800  /* Can do 10mbps, half-duplex  */
+#define BMSR_10FULL             0x1000  /* Can do 10mbps, full-duplex  */
+#define BMSR_100HALF            0x2000  /* Can do 100mbps, half-duplex */
+#define BMSR_100FULL            0x4000  /* Can do 100mbps, full-duplex */
+/* Advertisement control register. */
+#define ADVERTISE_CSMA          0x0001  /* Only selector supported     */
+#define ADVERTISE_10HALF        0x0020  /* Try for 10mbps half-duplex  */
+#define ADVERTISE_10FULL        0x0040  /* Try for 10mbps full-duplex  */
+#define ADVERTISE_100HALF       0x0080  /* Try for 100mbps half-duplex */
+#define ADVERTISE_100FULL       0x0100  /* Try for 100mbps full-duplex */
+#define ADVERTISE_ALL (ADVERTISE_10HALF | ADVERTISE_10FULL | \
+                       ADVERTISE_100HALF | ADVERTISE_100FULL)
+/* Expansion register for auto-negotiation. */
+#define EXPANSION_ENABLENPAGE   0x0004  /* This enables npage words    */
+#endif
+
+/*****************************************************************************/
+/* 2.4.6 => 2.4.3 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,6) )
+
+#ifndef pci_set_power_state
+#define pci_set_power_state _kc_pci_set_power_state
+extern int _kc_pci_set_power_state(struct pci_dev *dev, int state);
+#endif
+
+#ifndef pci_enable_wake
+#define pci_enable_wake _kc_pci_enable_wake
+extern int _kc_pci_enable_wake(struct pci_dev *pdev, u32 state, int enable);
+#endif
+
+#ifndef pci_disable_device
+#define pci_disable_device _kc_pci_disable_device
+extern void _kc_pci_disable_device(struct pci_dev *pdev);
+#endif
+
+/* PCI PM entry point syntax changed, so don't support suspend/resume */
+#undef CONFIG_PM
+
+#endif /* 2.4.6 => 2.4.3 */
+
+#ifndef HAVE_PCI_SET_MWI
+#define pci_set_mwi(X) pci_write_config_word(X, \
+			       PCI_COMMAND, adapter->hw.bus.pci_cmd_word | \
+			       PCI_COMMAND_INVALIDATE);
+#define pci_clear_mwi(X) pci_write_config_word(X, \
+			       PCI_COMMAND, adapter->hw.bus.pci_cmd_word & \
+			       ~PCI_COMMAND_INVALIDATE);
+#endif
+
+/*****************************************************************************/
+/* 2.4.10 => 2.4.9 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,10) )
+
+/**************************************/
+/* MODULE API */
+
+#ifndef MODULE_LICENSE
+	#define MODULE_LICENSE(X)
+#endif
+
+/**************************************/
+/* OTHER */
+
+#undef min
+#define min(x,y) ({ \
+	const typeof(x) _x = (x);	\
+	const typeof(y) _y = (y);	\
+	(void) (&_x == &_y);		\
+	_x < _y ? _x : _y; })
+
+#undef max
+#define max(x,y) ({ \
+	const typeof(x) _x = (x);	\
+	const typeof(y) _y = (y);	\
+	(void) (&_x == &_y);		\
+	_x > _y ? _x : _y; })
+
+#define min_t(type,x,y) ({ \
+	type _x = (x); \
+	type _y = (y); \
+	_x < _y ? _x : _y; })
+
+#define max_t(type,x,y) ({ \
+	type _x = (x); \
+	type _y = (y); \
+	_x > _y ? _x : _y; })
+
+#ifndef list_for_each_safe
+#define list_for_each_safe(pos, n, head) \
+	for (pos = (head)->next, n = pos->next; pos != (head); \
+		pos = n, n = pos->next)
+#endif
+
+#ifndef ____cacheline_aligned_in_smp
+#ifdef CONFIG_SMP
+#define ____cacheline_aligned_in_smp ____cacheline_aligned
+#else
+#define ____cacheline_aligned_in_smp
+#endif /* CONFIG_SMP */
+#endif
+
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,8) )
+extern int _kc_snprintf(char * buf, size_t size, const char *fmt, ...);
+#define snprintf(buf, size, fmt, args...) _kc_snprintf(buf, size, fmt, ##args)
+extern int _kc_vsnprintf(char *buf, size_t size, const char *fmt, va_list args);
+#define vsnprintf(buf, size, fmt, args) _kc_vsnprintf(buf, size, fmt, args)
+#else /* 2.4.8 => 2.4.9 */
+extern int snprintf(char * buf, size_t size, const char *fmt, ...);
+extern int vsnprintf(char *buf, size_t size, const char *fmt, va_list args);
+#endif
+#endif /* 2.4.10 -> 2.4.6 */
+
+
+/*****************************************************************************/
+/* 2.4.12 => 2.4.10 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,12) )
+#ifndef HAVE_NETIF_MSG
+#define HAVE_NETIF_MSG 1
+enum {
+	NETIF_MSG_DRV		= 0x0001,
+	NETIF_MSG_PROBE		= 0x0002,
+	NETIF_MSG_LINK		= 0x0004,
+	NETIF_MSG_TIMER		= 0x0008,
+	NETIF_MSG_IFDOWN	= 0x0010,
+	NETIF_MSG_IFUP		= 0x0020,
+	NETIF_MSG_RX_ERR	= 0x0040,
+	NETIF_MSG_TX_ERR	= 0x0080,
+	NETIF_MSG_TX_QUEUED	= 0x0100,
+	NETIF_MSG_INTR		= 0x0200,
+	NETIF_MSG_TX_DONE	= 0x0400,
+	NETIF_MSG_RX_STATUS	= 0x0800,
+	NETIF_MSG_PKTDATA	= 0x1000,
+	NETIF_MSG_HW		= 0x2000,
+	NETIF_MSG_WOL		= 0x4000,
+};
+
+#define netif_msg_drv(p)	((p)->msg_enable & NETIF_MSG_DRV)
+#define netif_msg_probe(p)	((p)->msg_enable & NETIF_MSG_PROBE)
+#define netif_msg_link(p)	((p)->msg_enable & NETIF_MSG_LINK)
+#define netif_msg_timer(p)	((p)->msg_enable & NETIF_MSG_TIMER)
+#define netif_msg_ifdown(p)	((p)->msg_enable & NETIF_MSG_IFDOWN)
+#define netif_msg_ifup(p)	((p)->msg_enable & NETIF_MSG_IFUP)
+#define netif_msg_rx_err(p)	((p)->msg_enable & NETIF_MSG_RX_ERR)
+#define netif_msg_tx_err(p)	((p)->msg_enable & NETIF_MSG_TX_ERR)
+#define netif_msg_tx_queued(p)	((p)->msg_enable & NETIF_MSG_TX_QUEUED)
+#define netif_msg_intr(p)	((p)->msg_enable & NETIF_MSG_INTR)
+#define netif_msg_tx_done(p)	((p)->msg_enable & NETIF_MSG_TX_DONE)
+#define netif_msg_rx_status(p)	((p)->msg_enable & NETIF_MSG_RX_STATUS)
+#define netif_msg_pktdata(p)	((p)->msg_enable & NETIF_MSG_PKTDATA)
+#endif /* !HAVE_NETIF_MSG */
+#endif /* 2.4.12 => 2.4.10 */
+
+/*****************************************************************************/
+/* 2.4.13 => 2.4.12 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,13) )
+
+/**************************************/
+/* PCI DMA MAPPING */
+
+#ifndef virt_to_page
+	#define virt_to_page(v) (mem_map + (virt_to_phys(v) >> PAGE_SHIFT))
+#endif
+
+#ifndef pci_map_page
+#define pci_map_page _kc_pci_map_page
+extern u64 _kc_pci_map_page(struct pci_dev *dev, struct page *page, unsigned long offset, size_t size, int direction);
+#endif
+
+#ifndef pci_unmap_page
+#define pci_unmap_page _kc_pci_unmap_page
+extern void _kc_pci_unmap_page(struct pci_dev *dev, u64 dma_addr, size_t size, int direction);
+#endif
+
+/* pci_set_dma_mask takes dma_addr_t, which is only 32-bits prior to 2.4.13 */
+
+#undef DMA_32BIT_MASK
+#define DMA_32BIT_MASK	0xffffffff
+#undef DMA_64BIT_MASK
+#define DMA_64BIT_MASK	0xffffffff
+
+/**************************************/
+/* OTHER */
+
+#ifndef cpu_relax
+#define cpu_relax()	rep_nop()
+#endif
+
+struct vlan_ethhdr {
+	unsigned char h_dest[ETH_ALEN];
+	unsigned char h_source[ETH_ALEN];
+	unsigned short h_vlan_proto;
+	unsigned short h_vlan_TCI;
+	unsigned short h_vlan_encapsulated_proto;
+};
+#endif /* 2.4.13 => 2.4.12 */
+
+/*****************************************************************************/
+/* 2.4.17 => 2.4.12 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,17) )
+
+#ifndef __devexit_p
+	#define __devexit_p(x) &(x)
+#endif
+
+#endif /* 2.4.17 => 2.4.13 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,18) )
+#define NETIF_MSG_HW	0x2000
+#define NETIF_MSG_WOL	0x4000
+
+#ifndef netif_msg_hw
+#define netif_msg_hw(p)		((p)->msg_enable & NETIF_MSG_HW)
+#endif
+#ifndef netif_msg_wol
+#define netif_msg_wol(p)	((p)->msg_enable & NETIF_MSG_WOL)
+#endif
+#endif /* 2.4.18 */
+
+/*****************************************************************************/
+
+/*****************************************************************************/
+/* 2.4.20 => 2.4.19 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,20) )
+
+/* we won't support NAPI on less than 2.4.20 */
+#ifdef NAPI
+#undef NAPI
+#endif
+
+#endif /* 2.4.20 => 2.4.19 */
+
+/*****************************************************************************/
+/* 2.4.22 => 2.4.17 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,22) )
+#define pci_name(x)	((x)->slot_name)
+
+#ifndef SUPPORTED_10000baseT_Full
+#define SUPPORTED_10000baseT_Full	(1 << 12)
+#endif
+#ifndef ADVERTISED_10000baseT_Full
+#define ADVERTISED_10000baseT_Full	(1 << 12)
+#endif
+#endif
+
+/*****************************************************************************/
+/* 2.4.22 => 2.4.17 */
+
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,22) )
+#ifndef IGB_NO_LRO
+#define IGB_NO_LRO
+#endif
+#endif
+
+/*****************************************************************************/
+/*****************************************************************************/
+/* 2.4.23 => 2.4.22 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,23) )
+/*****************************************************************************/
+#ifdef NAPI
+#ifndef netif_poll_disable
+#define netif_poll_disable(x) _kc_netif_poll_disable(x)
+static inline void _kc_netif_poll_disable(struct net_device *netdev)
+{
+	while (test_and_set_bit(__LINK_STATE_RX_SCHED, &netdev->state)) {
+		/* No hurry */
+		current->state = TASK_INTERRUPTIBLE;
+		schedule_timeout(1);
+	}
+}
+#endif
+#ifndef netif_poll_enable
+#define netif_poll_enable(x) _kc_netif_poll_enable(x)
+static inline void _kc_netif_poll_enable(struct net_device *netdev)
+{
+	clear_bit(__LINK_STATE_RX_SCHED, &netdev->state);
+}
+#endif
+#endif /* NAPI */
+#ifndef netif_tx_disable
+#define netif_tx_disable(x) _kc_netif_tx_disable(x)
+static inline void _kc_netif_tx_disable(struct net_device *dev)
+{
+	spin_lock_bh(&dev->xmit_lock);
+	netif_stop_queue(dev);
+	spin_unlock_bh(&dev->xmit_lock);
+}
+#endif
+#else /* 2.4.23 => 2.4.22 */
+#define HAVE_SCTP
+#endif /* 2.4.23 => 2.4.22 */
+
+/*****************************************************************************/
+/* 2.6.4 => 2.6.0 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,25) || \
+    ( LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0) && \
+      LINUX_VERSION_CODE < KERNEL_VERSION(2,6,4) ) )
+#define ETHTOOL_OPS_COMPAT
+#endif /* 2.6.4 => 2.6.0 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,27) )
+#define __user
+#endif /* < 2.4.27 */
+
+/*****************************************************************************/
+/* 2.5.71 => 2.4.x */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,5,71) )
+#define sk_protocol protocol
+#define pci_get_device pci_find_device
+#endif /* 2.5.70 => 2.4.x */
+
+/*****************************************************************************/
+/* < 2.4.27 or 2.6.0 <= 2.6.5 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,27) || \
+    ( LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0) && \
+      LINUX_VERSION_CODE < KERNEL_VERSION(2,6,5) ) )
+
+#ifndef netif_msg_init
+#define netif_msg_init _kc_netif_msg_init
+static inline u32 _kc_netif_msg_init(int debug_value, int default_msg_enable_bits)
+{
+	/* use default */
+	if (debug_value < 0 || debug_value >= (sizeof(u32) * 8))
+		return default_msg_enable_bits;
+	if (debug_value == 0) /* no output */
+		return 0;
+	/* set low N bits */
+	return (1 << debug_value) -1;
+}
+#endif
+
+#endif /* < 2.4.27 or 2.6.0 <= 2.6.5 */
+/*****************************************************************************/
+#if (( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,27) ) || \
+     (( LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0) ) && \
+      ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,3) )))
+#define netdev_priv(x) x->priv
+#endif
+
+/*****************************************************************************/
+/* <= 2.5.0 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0) )
+#include <linux/rtnetlink.h>
+#undef pci_register_driver
+#define pci_register_driver pci_module_init
+
+/*
+ * Most of the dma compat code is copied/modifed from the 2.4.37
+ * /include/linux/libata-compat.h header file
+ */
+/* These definitions mirror those in pci.h, so they can be used
+ * interchangeably with their PCI_ counterparts */
+enum dma_data_direction {
+	DMA_BIDIRECTIONAL = 0,
+	DMA_TO_DEVICE = 1,
+	DMA_FROM_DEVICE = 2,
+	DMA_NONE = 3,
+};
+
+struct device {
+	struct pci_dev pdev;
+};
+
+static inline struct pci_dev *to_pci_dev (struct device *dev)
+{
+	return (struct pci_dev *) dev;
+}
+static inline struct device *pci_dev_to_dev(struct pci_dev *pdev)
+{
+	return (struct device *) pdev;
+}
+
+#define pdev_printk(lvl, pdev, fmt, args...) 	\
+	printk("%s %s: " fmt, lvl, pci_name(pdev), ## args)
+#define dev_err(dev, fmt, args...)            \
+	pdev_printk(KERN_ERR, to_pci_dev(dev), fmt, ## args)
+#define dev_info(dev, fmt, args...)            \
+	pdev_printk(KERN_INFO, to_pci_dev(dev), fmt, ## args)
+#define dev_warn(dev, fmt, args...)            \
+	pdev_printk(KERN_WARNING, to_pci_dev(dev), fmt, ## args)
+#define dev_notice(dev, fmt, args...)            \
+	pdev_printk(KERN_NOTICE, to_pci_dev(dev), fmt, ## args)
+#define dev_dbg(dev, fmt, args...) \
+	pdev_printk(KERN_DEBUG, to_pci_dev(dev), fmt, ## args)
+
+/* NOTE: dangerous! we ignore the 'gfp' argument */
+#define dma_alloc_coherent(dev,sz,dma,gfp) \
+	pci_alloc_consistent(to_pci_dev(dev),(sz),(dma))
+#define dma_free_coherent(dev,sz,addr,dma_addr) \
+	pci_free_consistent(to_pci_dev(dev),(sz),(addr),(dma_addr))
+
+#define dma_map_page(dev,a,b,c,d) \
+	pci_map_page(to_pci_dev(dev),(a),(b),(c),(d))
+#define dma_unmap_page(dev,a,b,c) \
+	pci_unmap_page(to_pci_dev(dev),(a),(b),(c))
+
+#define dma_map_single(dev,a,b,c) \
+	pci_map_single(to_pci_dev(dev),(a),(b),(c))
+#define dma_unmap_single(dev,a,b,c) \
+	pci_unmap_single(to_pci_dev(dev),(a),(b),(c))
+
+#define dma_map_sg(dev, sg, nents, dir) \
+	pci_map_sg(to_pci_dev(dev), (sg), (nents), (dir)
+#define dma_unmap_sg(dev, sg, nents, dir) \
+	pci_unmap_sg(to_pci_dev(dev), (sg), (nents), (dir)
+
+#define dma_sync_single(dev,a,b,c) \
+	pci_dma_sync_single(to_pci_dev(dev),(a),(b),(c))
+
+/* for range just sync everything, that's all the pci API can do */
+#define dma_sync_single_range(dev,addr,off,sz,dir) \
+	pci_dma_sync_single(to_pci_dev(dev),(addr),(off)+(sz),(dir))
+
+#define dma_set_mask(dev,mask) \
+	pci_set_dma_mask(to_pci_dev(dev),(mask))
+
+/* hlist_* code - double linked lists */
+struct hlist_head {
+	struct hlist_node *first;
+};
+
+struct hlist_node {
+	struct hlist_node *next, **pprev;
+};
+
+static inline void __hlist_del(struct hlist_node *n)
+{
+	struct hlist_node *next = n->next;
+	struct hlist_node **pprev = n->pprev;
+	*pprev = next;
+	if (next)
+	next->pprev = pprev;
+}
+
+static inline void hlist_del(struct hlist_node *n)
+{
+	__hlist_del(n);
+	n->next = NULL;
+	n->pprev = NULL;
+}
+
+static inline void hlist_add_head(struct hlist_node *n, struct hlist_head *h)
+{
+	struct hlist_node *first = h->first;
+	n->next = first;
+	if (first)
+		first->pprev = &n->next;
+	h->first = n;
+	n->pprev = &h->first;
+}
+
+static inline int hlist_empty(const struct hlist_head *h)
+{
+	return !h->first;
+}
+#define HLIST_HEAD_INIT { .first = NULL }
+#define HLIST_HEAD(name) struct hlist_head name = {  .first = NULL }
+#define INIT_HLIST_HEAD(ptr) ((ptr)->first = NULL)
+static inline void INIT_HLIST_NODE(struct hlist_node *h)
+{
+	h->next = NULL;
+	h->pprev = NULL;
+}
+
+#ifndef might_sleep
+#define might_sleep()
+#endif
+#else
+static inline struct device *pci_dev_to_dev(struct pci_dev *pdev)
+{
+	return &pdev->dev;
+}
+#endif /* <= 2.5.0 */
+
+/*****************************************************************************/
+/* 2.5.28 => 2.4.23 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,5,28) )
+
+#include <linux/tqueue.h>
+#define work_struct tq_struct
+#undef INIT_WORK
+#define INIT_WORK(a,b) INIT_TQUEUE(a,(void (*)(void *))b,a)
+#undef container_of
+#define container_of list_entry
+#define schedule_work schedule_task
+#define flush_scheduled_work flush_scheduled_tasks
+#define cancel_work_sync(x) flush_scheduled_work()
+
+#endif /* 2.5.28 => 2.4.17 */
+
+/*****************************************************************************/
+/* 2.6.0 => 2.5.28 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0) )
+#ifndef read_barrier_depends
+#define read_barrier_depends() rmb()
+#endif
+
+#ifndef rcu_head
+struct __kc_callback_head {
+	struct __kc_callback_head *next;
+	void (*func)(struct callback_head *head);
+};
+#define rcu_head __kc_callback_head
+#endif
+
+#undef get_cpu
+#define get_cpu() smp_processor_id()
+#undef put_cpu
+#define put_cpu() do { } while(0)
+#define MODULE_INFO(version, _version)
+#ifndef CONFIG_E1000_DISABLE_PACKET_SPLIT
+#define CONFIG_E1000_DISABLE_PACKET_SPLIT 1
+#endif
+#ifndef CONFIG_IGB_DISABLE_PACKET_SPLIT
+#define CONFIG_IGB_DISABLE_PACKET_SPLIT 1
+#endif
+
+#define dma_set_coherent_mask(dev,mask) 1
+
+#undef dev_put
+#define dev_put(dev) __dev_put(dev)
+
+#ifndef skb_fill_page_desc
+#define skb_fill_page_desc _kc_skb_fill_page_desc
+extern void _kc_skb_fill_page_desc(struct sk_buff *skb, int i, struct page *page, int off, int size);
+#endif
+
+#undef ALIGN
+#define ALIGN(x,a) (((x)+(a)-1)&~((a)-1))
+
+#ifndef page_count
+#define page_count(p) atomic_read(&(p)->count)
+#endif
+
+#ifdef MAX_NUMNODES
+#undef MAX_NUMNODES
+#endif
+#define MAX_NUMNODES 1
+
+/* find_first_bit and find_next bit are not defined for most
+ * 2.4 kernels (except for the redhat 2.4.21 kernels
+ */
+#include <linux/bitops.h>
+#define BITOP_WORD(nr)          ((nr) / BITS_PER_LONG)
+#undef find_next_bit
+#define find_next_bit _kc_find_next_bit
+extern unsigned long _kc_find_next_bit(const unsigned long *addr,
+                                       unsigned long size,
+                                       unsigned long offset);
+#define find_first_bit(addr, size) find_next_bit((addr), (size), 0)
+
+
+#ifndef netdev_name
+static inline const char *_kc_netdev_name(const struct net_device *dev)
+{
+	if (strchr(dev->name, '%'))
+		return "(unregistered net_device)";
+	return dev->name;
+}
+#define netdev_name(netdev)	_kc_netdev_name(netdev)
+#endif /* netdev_name */
+
+#ifndef strlcpy
+#define strlcpy _kc_strlcpy
+extern size_t _kc_strlcpy(char *dest, const char *src, size_t size);
+#endif /* strlcpy */
+
+#ifndef do_div
+#if BITS_PER_LONG == 64
+# define do_div(n,base) ({					\
+	uint32_t __base = (base);				\
+	uint32_t __rem;						\
+	__rem = ((uint64_t)(n)) % __base;			\
+	(n) = ((uint64_t)(n)) / __base;				\
+	__rem;							\
+ })
+#elif BITS_PER_LONG == 32
+extern uint32_t _kc__div64_32(uint64_t *dividend, uint32_t divisor);
+# define do_div(n,base) ({				\
+	uint32_t __base = (base);			\
+	uint32_t __rem;					\
+	if (likely(((n) >> 32) == 0)) {			\
+		__rem = (uint32_t)(n) % __base;		\
+		(n) = (uint32_t)(n) / __base;		\
+	} else 						\
+		__rem = _kc__div64_32(&(n), __base);	\
+	__rem;						\
+ })
+#else /* BITS_PER_LONG == ?? */
+# error do_div() does not yet support the C64
+#endif /* BITS_PER_LONG */
+#endif /* do_div */
+
+#ifndef NSEC_PER_SEC
+#define NSEC_PER_SEC	1000000000L
+#endif
+
+#undef HAVE_I2C_SUPPORT
+#else /* 2.6.0 */
+#if IS_ENABLED(CONFIG_I2C_ALGOBIT) && \
+	(RHEL_RELEASE_CODE && (RHEL_RELEASE_CODE > RHEL_RELEASE_VERSION(4,9)))
+#define HAVE_I2C_SUPPORT
+#endif /* IS_ENABLED(CONFIG_I2C_ALGOBIT) */
+
+#endif /* 2.6.0 => 2.5.28 */
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,3) )
+#define dma_pool pci_pool
+#define dma_pool_destroy pci_pool_destroy
+#define dma_pool_alloc pci_pool_alloc
+#define dma_pool_free pci_pool_free
+
+#define dma_pool_create(name,dev,size,align,allocation) \
+       pci_pool_create((name),to_pci_dev(dev),(size),(align),(allocation))
+#endif /* < 2.6.3 */
+
+/*****************************************************************************/
+/* 2.6.4 => 2.6.0 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,4) )
+#define MODULE_VERSION(_version) MODULE_INFO(version, _version)
+#endif /* 2.6.4 => 2.6.0 */
+
+/*****************************************************************************/
+/* 2.6.5 => 2.6.0 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,5) )
+#define dma_sync_single_for_cpu		dma_sync_single
+#define dma_sync_single_for_device	dma_sync_single
+#define dma_sync_single_range_for_cpu		dma_sync_single_range
+#define dma_sync_single_range_for_device	dma_sync_single_range
+#ifndef pci_dma_mapping_error
+#define pci_dma_mapping_error _kc_pci_dma_mapping_error
+static inline int _kc_pci_dma_mapping_error(dma_addr_t dma_addr)
+{
+	return dma_addr == 0;
+}
+#endif
+#endif /* 2.6.5 => 2.6.0 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,4) )
+extern int _kc_scnprintf(char * buf, size_t size, const char *fmt, ...);
+#define scnprintf(buf, size, fmt, args...) _kc_scnprintf(buf, size, fmt, ##args)
+#endif /* < 2.6.4 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,6) )
+/* taken from 2.6 include/linux/bitmap.h */
+#undef bitmap_zero
+#define bitmap_zero _kc_bitmap_zero
+static inline void _kc_bitmap_zero(unsigned long *dst, int nbits)
+{
+        if (nbits <= BITS_PER_LONG)
+                *dst = 0UL;
+        else {
+                int len = BITS_TO_LONGS(nbits) * sizeof(unsigned long);
+                memset(dst, 0, len);
+        }
+}
+#define page_to_nid(x) 0
+
+#endif /* < 2.6.6 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,7) )
+#undef if_mii
+#define if_mii _kc_if_mii
+static inline struct mii_ioctl_data *_kc_if_mii(struct ifreq *rq)
+{
+	return (struct mii_ioctl_data *) &rq->ifr_ifru;
+}
+
+#ifndef __force
+#define __force
+#endif
+#endif /* < 2.6.7 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,8) )
+#ifndef PCI_EXP_DEVCTL
+#define PCI_EXP_DEVCTL 8
+#endif
+#ifndef PCI_EXP_DEVCTL_CERE
+#define PCI_EXP_DEVCTL_CERE 0x0001
+#endif
+#define PCI_EXP_FLAGS		2	/* Capabilities register */
+#define PCI_EXP_FLAGS_VERS	0x000f	/* Capability version */
+#define PCI_EXP_FLAGS_TYPE	0x00f0	/* Device/Port type */
+#define  PCI_EXP_TYPE_ENDPOINT	0x0	/* Express Endpoint */
+#define  PCI_EXP_TYPE_LEG_END	0x1	/* Legacy Endpoint */
+#define  PCI_EXP_TYPE_ROOT_PORT 0x4	/* Root Port */
+#define  PCI_EXP_TYPE_DOWNSTREAM 0x6	/* Downstream Port */
+#define PCI_EXP_FLAGS_SLOT	0x0100	/* Slot implemented */
+#define PCI_EXP_DEVCAP		4	/* Device capabilities */
+#define PCI_EXP_DEVSTA		10	/* Device Status */
+#define msleep(x)	do { set_current_state(TASK_UNINTERRUPTIBLE); \
+				schedule_timeout((x * HZ)/1000 + 2); \
+			} while (0)
+
+#endif /* < 2.6.8 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,9))
+#include <net/dsfield.h>
+#define __iomem
+
+#ifndef kcalloc
+#define kcalloc(n, size, flags) _kc_kzalloc(((n) * (size)), flags)
+extern void *_kc_kzalloc(size_t size, int flags);
+#endif
+#define MSEC_PER_SEC    1000L
+static inline unsigned int _kc_jiffies_to_msecs(const unsigned long j)
+{
+#if HZ <= MSEC_PER_SEC && !(MSEC_PER_SEC % HZ)
+	return (MSEC_PER_SEC / HZ) * j;
+#elif HZ > MSEC_PER_SEC && !(HZ % MSEC_PER_SEC)
+	return (j + (HZ / MSEC_PER_SEC) - 1)/(HZ / MSEC_PER_SEC);
+#else
+	return (j * MSEC_PER_SEC) / HZ;
+#endif
+}
+static inline unsigned long _kc_msecs_to_jiffies(const unsigned int m)
+{
+	if (m > _kc_jiffies_to_msecs(MAX_JIFFY_OFFSET))
+		return MAX_JIFFY_OFFSET;
+#if HZ <= MSEC_PER_SEC && !(MSEC_PER_SEC % HZ)
+	return (m + (MSEC_PER_SEC / HZ) - 1) / (MSEC_PER_SEC / HZ);
+#elif HZ > MSEC_PER_SEC && !(HZ % MSEC_PER_SEC)
+	return m * (HZ / MSEC_PER_SEC);
+#else
+	return (m * HZ + MSEC_PER_SEC - 1) / MSEC_PER_SEC;
+#endif
+}
+
+#define msleep_interruptible _kc_msleep_interruptible
+static inline unsigned long _kc_msleep_interruptible(unsigned int msecs)
+{
+	unsigned long timeout = _kc_msecs_to_jiffies(msecs) + 1;
+
+	while (timeout && !signal_pending(current)) {
+		__set_current_state(TASK_INTERRUPTIBLE);
+		timeout = schedule_timeout(timeout);
+	}
+	return _kc_jiffies_to_msecs(timeout);
+}
+
+/* Basic mode control register. */
+#define BMCR_SPEED1000		0x0040  /* MSB of Speed (1000)         */
+
+#ifndef __le16
+#define __le16 u16
+#endif
+#ifndef __le32
+#define __le32 u32
+#endif
+#ifndef __le64
+#define __le64 u64
+#endif
+#ifndef __be16
+#define __be16 u16
+#endif
+#ifndef __be32
+#define __be32 u32
+#endif
+#ifndef __be64
+#define __be64 u64
+#endif
+
+static inline struct vlan_ethhdr *vlan_eth_hdr(const struct sk_buff *skb)
+{
+	return (struct vlan_ethhdr *)skb->mac.raw;
+}
+
+/* Wake-On-Lan options. */
+#define WAKE_PHY		(1 << 0)
+#define WAKE_UCAST		(1 << 1)
+#define WAKE_MCAST		(1 << 2)
+#define WAKE_BCAST		(1 << 3)
+#define WAKE_ARP		(1 << 4)
+#define WAKE_MAGIC		(1 << 5)
+#define WAKE_MAGICSECURE	(1 << 6) /* only meaningful if WAKE_MAGIC */
+
+#define skb_header_pointer _kc_skb_header_pointer
+static inline void *_kc_skb_header_pointer(const struct sk_buff *skb,
+					    int offset, int len, void *buffer)
+{
+	int hlen = skb_headlen(skb);
+
+	if (hlen - offset >= len)
+		return skb->data + offset;
+
+#ifdef MAX_SKB_FRAGS
+	if (skb_copy_bits(skb, offset, buffer, len) < 0)
+		return NULL;
+
+	return buffer;
+#else
+	return NULL;
+#endif
+
+#ifndef NETDEV_TX_OK
+#define NETDEV_TX_OK 0
+#endif
+#ifndef NETDEV_TX_BUSY
+#define NETDEV_TX_BUSY 1
+#endif
+#ifndef NETDEV_TX_LOCKED
+#define NETDEV_TX_LOCKED -1
+#endif
+}
+
+#ifndef __bitwise
+#define __bitwise
+#endif
+#endif /* < 2.6.9 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,10) )
+#ifdef module_param_array_named
+#undef module_param_array_named
+#define module_param_array_named(name, array, type, nump, perm)          \
+	static struct kparam_array __param_arr_##name                    \
+	= { ARRAY_SIZE(array), nump, param_set_##type, param_get_##type, \
+	    sizeof(array[0]), array };                                   \
+	module_param_call(name, param_array_set, param_array_get,        \
+			  &__param_arr_##name, perm)
+#endif /* module_param_array_named */
+/*
+ * num_online is broken for all < 2.6.10 kernels.  This is needed to support
+ * Node module parameter of ixgbe.
+ */
+#undef num_online_nodes
+#define num_online_nodes(n) 1
+extern DECLARE_BITMAP(_kcompat_node_online_map, MAX_NUMNODES);
+#undef node_online_map
+#define node_online_map _kcompat_node_online_map
+#define pci_get_class pci_find_class
+#endif /* < 2.6.10 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11) )
+#define PCI_D0      0
+#define PCI_D1      1
+#define PCI_D2      2
+#define PCI_D3hot   3
+#define PCI_D3cold  4
+typedef int pci_power_t;
+#define pci_choose_state(pdev,state) state
+#define PMSG_SUSPEND 3
+#define PCI_EXP_LNKCTL	16
+
+#undef NETIF_F_LLTX
+
+#ifndef ARCH_HAS_PREFETCH
+#define prefetch(X)
+#endif
+
+#ifndef NET_IP_ALIGN
+#define NET_IP_ALIGN 2
+#endif
+
+#define KC_USEC_PER_SEC	1000000L
+#define usecs_to_jiffies _kc_usecs_to_jiffies
+static inline unsigned int _kc_jiffies_to_usecs(const unsigned long j)
+{
+#if HZ <= KC_USEC_PER_SEC && !(KC_USEC_PER_SEC % HZ)
+	return (KC_USEC_PER_SEC / HZ) * j;
+#elif HZ > KC_USEC_PER_SEC && !(HZ % KC_USEC_PER_SEC)
+	return (j + (HZ / KC_USEC_PER_SEC) - 1)/(HZ / KC_USEC_PER_SEC);
+#else
+	return (j * KC_USEC_PER_SEC) / HZ;
+#endif
+}
+static inline unsigned long _kc_usecs_to_jiffies(const unsigned int m)
+{
+	if (m > _kc_jiffies_to_usecs(MAX_JIFFY_OFFSET))
+		return MAX_JIFFY_OFFSET;
+#if HZ <= KC_USEC_PER_SEC && !(KC_USEC_PER_SEC % HZ)
+	return (m + (KC_USEC_PER_SEC / HZ) - 1) / (KC_USEC_PER_SEC / HZ);
+#elif HZ > KC_USEC_PER_SEC && !(HZ % KC_USEC_PER_SEC)
+	return m * (HZ / KC_USEC_PER_SEC);
+#else
+	return (m * HZ + KC_USEC_PER_SEC - 1) / KC_USEC_PER_SEC;
+#endif
+}
+
+#define PCI_EXP_LNKCAP		12	/* Link Capabilities */
+#define PCI_EXP_LNKSTA		18	/* Link Status */
+#define PCI_EXP_SLTCAP		20	/* Slot Capabilities */
+#define PCI_EXP_SLTCTL		24	/* Slot Control */
+#define PCI_EXP_SLTSTA		26	/* Slot Status */
+#define PCI_EXP_RTCTL		28	/* Root Control */
+#define PCI_EXP_RTCAP		30	/* Root Capabilities */
+#define PCI_EXP_RTSTA		32	/* Root Status */
+#endif /* < 2.6.11 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,12) )
+#include <linux/reboot.h>
+#define USE_REBOOT_NOTIFIER
+
+/* Generic MII registers. */
+#define MII_CTRL1000        0x09        /* 1000BASE-T control          */
+#define MII_STAT1000        0x0a        /* 1000BASE-T status           */
+/* Advertisement control register. */
+#define ADVERTISE_PAUSE_CAP     0x0400  /* Try for pause               */
+#define ADVERTISE_PAUSE_ASYM    0x0800  /* Try for asymmetric pause     */
+/* Link partner ability register. */
+#define LPA_PAUSE_CAP		0x0400	/* Can pause                   */
+#define LPA_PAUSE_ASYM		0x0800	/* Can pause asymetrically     */
+/* 1000BASE-T Control register */
+#define ADVERTISE_1000FULL      0x0200  /* Advertise 1000BASE-T full duplex */
+#define ADVERTISE_1000HALF	0x0100  /* Advertise 1000BASE-T half duplex */
+/* 1000BASE-T Status register */
+#define LPA_1000LOCALRXOK	0x2000	/* Link partner local receiver status */
+#define LPA_1000REMRXOK		0x1000	/* Link partner remote receiver status */
+
+#ifndef is_zero_ether_addr
+#define is_zero_ether_addr _kc_is_zero_ether_addr
+static inline int _kc_is_zero_ether_addr(const u8 *addr)
+{
+	return !(addr[0] | addr[1] | addr[2] | addr[3] | addr[4] | addr[5]);
+}
+#endif /* is_zero_ether_addr */
+#ifndef is_multicast_ether_addr
+#define is_multicast_ether_addr _kc_is_multicast_ether_addr
+static inline int _kc_is_multicast_ether_addr(const u8 *addr)
+{
+	return addr[0] & 0x01;
+}
+#endif /* is_multicast_ether_addr */
+#endif /* < 2.6.12 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,13) )
+#ifndef kstrdup
+#define kstrdup _kc_kstrdup
+extern char *_kc_kstrdup(const char *s, unsigned int gfp);
+#endif
+#endif /* < 2.6.13 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,14) )
+#define pm_message_t u32
+#ifndef kzalloc
+#define kzalloc _kc_kzalloc
+extern void *_kc_kzalloc(size_t size, int flags);
+#endif
+
+/* Generic MII registers. */
+#define MII_ESTATUS	    0x0f	/* Extended Status */
+/* Basic mode status register. */
+#define BMSR_ESTATEN		0x0100	/* Extended Status in R15 */
+/* Extended status register. */
+#define ESTATUS_1000_TFULL	0x2000	/* Can do 1000BT Full */
+#define ESTATUS_1000_THALF	0x1000	/* Can do 1000BT Half */
+
+#define SUPPORTED_Pause	        (1 << 13)
+#define SUPPORTED_Asym_Pause	(1 << 14)
+#define ADVERTISED_Pause	(1 << 13)
+#define ADVERTISED_Asym_Pause	(1 << 14)
+
+#if (!(RHEL_RELEASE_CODE && \
+       (RHEL_RELEASE_CODE > RHEL_RELEASE_VERSION(4,3)) && \
+       (RHEL_RELEASE_CODE < RHEL_RELEASE_VERSION(5,0))))
+#if ((LINUX_VERSION_CODE == KERNEL_VERSION(2,6,9)) && !defined(gfp_t))
+#define gfp_t unsigned
+#else
+typedef unsigned gfp_t;
+#endif
+#endif /* !RHEL4.3->RHEL5.0 */
+
+#if ( LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,9) )
+#ifdef CONFIG_X86_64
+#define dma_sync_single_range_for_cpu(dev, addr, off, sz, dir)       \
+	dma_sync_single_for_cpu((dev), (addr), (off) + (sz), (dir))
+#define dma_sync_single_range_for_device(dev, addr, off, sz, dir)    \
+	dma_sync_single_for_device((dev), (addr), (off) + (sz), (dir))
+#endif
+#endif
+#endif /* < 2.6.14 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,15) )
+#ifndef vmalloc_node
+#define vmalloc_node(a,b) vmalloc(a)
+#endif /* vmalloc_node*/
+
+#define setup_timer(_timer, _function, _data) \
+do { \
+	(_timer)->function = _function; \
+	(_timer)->data = _data; \
+	init_timer(_timer); \
+} while (0)
+#ifndef device_can_wakeup
+#define device_can_wakeup(dev)	(1)
+#endif
+#ifndef device_set_wakeup_enable
+#define device_set_wakeup_enable(dev, val)	do{}while(0)
+#endif
+#ifndef device_init_wakeup
+#define device_init_wakeup(dev,val) do {} while (0)
+#endif
+static inline unsigned _kc_compare_ether_addr(const u8 *addr1, const u8 *addr2)
+{
+	const u16 *a = (const u16 *) addr1;
+	const u16 *b = (const u16 *) addr2;
+
+	return ((a[0] ^ b[0]) | (a[1] ^ b[1]) | (a[2] ^ b[2])) != 0;
+}
+#undef compare_ether_addr
+#define compare_ether_addr(addr1, addr2) _kc_compare_ether_addr(addr1, addr2)
+#else /* 2.6.15 and above */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,37) )
+/* CMW fix device_can_wakeup here */
+#endif /* 2.6.15 and above, but < 2.6.37 */
+#endif /* < 2.6.15 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,16) )
+#undef DEFINE_MUTEX
+#define DEFINE_MUTEX(x)	DECLARE_MUTEX(x)
+#define mutex_lock(x)	down_interruptible(x)
+#define mutex_unlock(x)	up(x)
+
+#ifndef ____cacheline_internodealigned_in_smp
+#ifdef CONFIG_SMP
+#define ____cacheline_internodealigned_in_smp ____cacheline_aligned_in_smp
+#else
+#define ____cacheline_internodealigned_in_smp
+#endif /* CONFIG_SMP */
+#endif /* ____cacheline_internodealigned_in_smp */
+#undef HAVE_PCI_ERS
+#else /* 2.6.16 and above */
+#undef HAVE_PCI_ERS
+#define HAVE_PCI_ERS
+#endif /* < 2.6.16 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,17) )
+#ifndef dev_notice
+#define dev_notice(dev, fmt, args...)            \
+	dev_printk(KERN_NOTICE, dev, fmt, ## args)
+#endif
+
+#ifndef first_online_node
+#define first_online_node 0
+#endif
+#ifndef NET_SKB_PAD
+#define NET_SKB_PAD 16
+#endif
+#endif /* < 2.6.17 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,18) )
+
+#ifndef IRQ_HANDLED
+#define irqreturn_t void
+#define IRQ_HANDLED
+#define IRQ_NONE
+#endif
+
+#ifndef IRQF_PROBE_SHARED
+#ifdef SA_PROBEIRQ
+#define IRQF_PROBE_SHARED SA_PROBEIRQ
+#else
+#define IRQF_PROBE_SHARED 0
+#endif
+#endif
+
+#ifndef IRQF_SHARED
+#define IRQF_SHARED SA_SHIRQ
+#endif
+
+#ifndef ARRAY_SIZE
+#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
+#endif
+
+#ifndef FIELD_SIZEOF
+#define FIELD_SIZEOF(t, f) (sizeof(((t*)0)->f))
+#endif
+
+#ifndef skb_is_gso
+#ifdef NETIF_F_TSO
+#define skb_is_gso _kc_skb_is_gso
+static inline int _kc_skb_is_gso(const struct sk_buff *skb)
+{
+	return skb_shinfo(skb)->gso_size;
+}
+#else
+#define skb_is_gso(a) 0
+#endif
+#endif
+
+#ifndef resource_size_t
+#define resource_size_t unsigned long
+#endif
+
+#ifdef skb_pad
+#undef skb_pad
+#endif
+#define skb_pad(x,y) _kc_skb_pad(x, y)
+int _kc_skb_pad(struct sk_buff *skb, int pad);
+#ifdef skb_padto
+#undef skb_padto
+#endif
+#define skb_padto(x,y) _kc_skb_padto(x, y)
+static inline int _kc_skb_padto(struct sk_buff *skb, unsigned int len)
+{
+	unsigned int size = skb->len;
+	if(likely(size >= len))
+		return 0;
+	return _kc_skb_pad(skb, len - size);
+}
+
+#ifndef DECLARE_PCI_UNMAP_ADDR
+#define DECLARE_PCI_UNMAP_ADDR(ADDR_NAME) \
+	dma_addr_t ADDR_NAME
+#define DECLARE_PCI_UNMAP_LEN(LEN_NAME) \
+	u32 LEN_NAME
+#define pci_unmap_addr(PTR, ADDR_NAME) \
+	((PTR)->ADDR_NAME)
+#define pci_unmap_addr_set(PTR, ADDR_NAME, VAL) \
+	(((PTR)->ADDR_NAME) = (VAL))
+#define pci_unmap_len(PTR, LEN_NAME) \
+	((PTR)->LEN_NAME)
+#define pci_unmap_len_set(PTR, LEN_NAME, VAL) \
+	(((PTR)->LEN_NAME) = (VAL))
+#endif /* DECLARE_PCI_UNMAP_ADDR */
+#endif /* < 2.6.18 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,19) )
+
+#if (!(RHEL_RELEASE_CODE && RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(5,0)))
+#define i_private u.generic_ip
+#endif /* >= RHEL 5.0 */
+
+#ifndef DIV_ROUND_UP
+#define DIV_ROUND_UP(n,d) (((n) + (d) - 1) / (d))
+#endif
+#ifndef __ALIGN_MASK
+#define __ALIGN_MASK(x, mask) (((x) + (mask)) & ~(mask))
+#endif
+#if ( LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0) )
+#if (!((RHEL_RELEASE_CODE && \
+        ((RHEL_RELEASE_CODE > RHEL_RELEASE_VERSION(4,4) && \
+          RHEL_RELEASE_CODE < RHEL_RELEASE_VERSION(5,0)) || \
+         (RHEL_RELEASE_CODE > RHEL_RELEASE_VERSION(5,0))))))
+typedef irqreturn_t (*irq_handler_t)(int, void*, struct pt_regs *);
+#endif
+#if (RHEL_RELEASE_CODE && RHEL_RELEASE_CODE < RHEL_RELEASE_VERSION(6,0))
+#undef CONFIG_INET_LRO
+#undef CONFIG_INET_LRO_MODULE
+#ifdef IXGBE_FCOE
+#undef CONFIG_FCOE
+#undef CONFIG_FCOE_MODULE
+#endif /* IXGBE_FCOE */
+#endif
+typedef irqreturn_t (*new_handler_t)(int, void*);
+static inline irqreturn_t _kc_request_irq(unsigned int irq, new_handler_t handler, unsigned long flags, const char *devname, void *dev_id)
+#else /* 2.4.x */
+typedef void (*irq_handler_t)(int, void*, struct pt_regs *);
+typedef void (*new_handler_t)(int, void*);
+static inline int _kc_request_irq(unsigned int irq, new_handler_t handler, unsigned long flags, const char *devname, void *dev_id)
+#endif /* >= 2.5.x */
+{
+	irq_handler_t new_handler = (irq_handler_t) handler;
+	return request_irq(irq, new_handler, flags, devname, dev_id);
+}
+
+#undef request_irq
+#define request_irq(irq, handler, flags, devname, dev_id) _kc_request_irq((irq), (handler), (flags), (devname), (dev_id))
+
+#define irq_handler_t new_handler_t
+/* pci_restore_state and pci_save_state handles MSI/PCIE from 2.6.19 */
+#if (!(RHEL_RELEASE_CODE && RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(5,4)))
+#define PCIE_CONFIG_SPACE_LEN 256
+#define PCI_CONFIG_SPACE_LEN 64
+#define PCIE_LINK_STATUS 0x12
+#define pci_config_space_ich8lan() do {} while(0)
+#undef pci_save_state
+extern int _kc_pci_save_state(struct pci_dev *);
+#define pci_save_state(pdev) _kc_pci_save_state(pdev)
+#undef pci_restore_state
+extern void _kc_pci_restore_state(struct pci_dev *);
+#define pci_restore_state(pdev) _kc_pci_restore_state(pdev)
+#endif /* !(RHEL_RELEASE_CODE >= RHEL 5.4) */
+
+#ifdef HAVE_PCI_ERS
+#undef free_netdev
+extern void _kc_free_netdev(struct net_device *);
+#define free_netdev(netdev) _kc_free_netdev(netdev)
+#endif
+static inline int pci_enable_pcie_error_reporting(struct pci_dev *dev)
+{
+	return 0;
+}
+#define pci_disable_pcie_error_reporting(dev) do {} while (0)
+#define pci_cleanup_aer_uncorrect_error_status(dev) do {} while (0)
+
+extern void *_kc_kmemdup(const void *src, size_t len, unsigned gfp);
+#define kmemdup(src, len, gfp) _kc_kmemdup(src, len, gfp)
+#ifndef bool
+#define bool _Bool
+#define true 1
+#define false 0
+#endif
+#else /* 2.6.19 */
+#include <linux/aer.h>
+#include <linux/string.h>
+#endif /* < 2.6.19 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20) )
+#if ( LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,28) )
+#undef INIT_WORK
+#define INIT_WORK(_work, _func) \
+do { \
+	INIT_LIST_HEAD(&(_work)->entry); \
+	(_work)->pending = 0; \
+	(_work)->func = (void (*)(void *))_func; \
+	(_work)->data = _work; \
+	init_timer(&(_work)->timer); \
+} while (0)
+#endif
+
+#ifndef PCI_VDEVICE
+#define PCI_VDEVICE(ven, dev)        \
+	PCI_VENDOR_ID_##ven, (dev),  \
+	PCI_ANY_ID, PCI_ANY_ID, 0, 0
+#endif
+
+#ifndef PCI_VENDOR_ID_INTEL
+#define PCI_VENDOR_ID_INTEL 0x8086
+#endif
+
+#ifndef round_jiffies
+#define round_jiffies(x) x
+#endif
+
+#define csum_offset csum
+
+#define HAVE_EARLY_VMALLOC_NODE
+#define dev_to_node(dev) -1
+#undef set_dev_node
+/* remove compiler warning with b=b, for unused variable */
+#define set_dev_node(a, b) do { (b) = (b); } while(0)
+
+#if (!(RHEL_RELEASE_CODE && \
+       (((RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(4,7)) && \
+         (RHEL_RELEASE_CODE < RHEL_RELEASE_VERSION(5,0))) || \
+        (RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(5,6)))) && \
+     !(SLE_VERSION_CODE && SLE_VERSION_CODE >= SLE_VERSION(10,2,0)))
+typedef __u16 __bitwise __sum16;
+typedef __u32 __bitwise __wsum;
+#endif
+
+#if (!(RHEL_RELEASE_CODE && \
+       (((RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(4,7)) && \
+         (RHEL_RELEASE_CODE < RHEL_RELEASE_VERSION(5,0))) || \
+        (RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(5,4)))) && \
+     !(SLE_VERSION_CODE && SLE_VERSION_CODE >= SLE_VERSION(10,2,0)))
+static inline __wsum csum_unfold(__sum16 n)
+{
+	return (__force __wsum)n;
+}
+#endif
+
+#else /* < 2.6.20 */
+#define HAVE_DEVICE_NUMA_NODE
+#endif /* < 2.6.20 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21) )
+#define to_net_dev(class) container_of(class, struct net_device, class_dev)
+#define NETDEV_CLASS_DEV
+#if (!(RHEL_RELEASE_CODE && RHEL_RELEASE_CODE > RHEL_RELEASE_VERSION(5,5)))
+#define vlan_group_get_device(vg, id) (vg->vlan_devices[id])
+#define vlan_group_set_device(vg, id, dev)		\
+	do {						\
+		if (vg) vg->vlan_devices[id] = dev;	\
+	} while (0)
+#endif /* !(RHEL_RELEASE_CODE > RHEL_RELEASE_VERSION(5,5)) */
+#define pci_channel_offline(pdev) (pdev->error_state && \
+	pdev->error_state != pci_channel_io_normal)
+#define pci_request_selected_regions(pdev, bars, name) \
+        pci_request_regions(pdev, name)
+#define pci_release_selected_regions(pdev, bars) pci_release_regions(pdev);
+
+#ifndef __aligned
+#define __aligned(x)			__attribute__((aligned(x)))
+#endif
+
+extern struct pci_dev *_kc_netdev_to_pdev(struct net_device *netdev);
+#define netdev_to_dev(netdev)	\
+	pci_dev_to_dev(_kc_netdev_to_pdev(netdev))
+#else
+static inline struct device *netdev_to_dev(struct net_device *netdev)
+{
+	return &netdev->dev;
+}
+
+#endif /* < 2.6.21 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22) )
+#define tcp_hdr(skb) (skb->h.th)
+#define tcp_hdrlen(skb) (skb->h.th->doff << 2)
+#define skb_transport_offset(skb) (skb->h.raw - skb->data)
+#define skb_transport_header(skb) (skb->h.raw)
+#define ipv6_hdr(skb) (skb->nh.ipv6h)
+#define ip_hdr(skb) (skb->nh.iph)
+#define skb_network_offset(skb) (skb->nh.raw - skb->data)
+#define skb_network_header(skb) (skb->nh.raw)
+#define skb_tail_pointer(skb) skb->tail
+#define skb_reset_tail_pointer(skb) \
+	do { \
+		skb->tail = skb->data; \
+	} while (0)
+#define skb_set_tail_pointer(skb, offset) \
+	do { \
+		skb->tail = skb->data + offset; \
+	} while (0)
+#define skb_copy_to_linear_data(skb, from, len) \
+				memcpy(skb->data, from, len)
+#define skb_copy_to_linear_data_offset(skb, offset, from, len) \
+				memcpy(skb->data + offset, from, len)
+#define skb_network_header_len(skb) (skb->h.raw - skb->nh.raw)
+#define pci_register_driver pci_module_init
+#define skb_mac_header(skb) skb->mac.raw
+
+#ifdef NETIF_F_MULTI_QUEUE
+#ifndef alloc_etherdev_mq
+#define alloc_etherdev_mq(_a, _b) alloc_etherdev(_a)
+#endif
+#endif /* NETIF_F_MULTI_QUEUE */
+
+#ifndef ETH_FCS_LEN
+#define ETH_FCS_LEN 4
+#endif
+#define cancel_work_sync(x) flush_scheduled_work()
+#ifndef udp_hdr
+#define udp_hdr _udp_hdr
+static inline struct udphdr *_udp_hdr(const struct sk_buff *skb)
+{
+	return (struct udphdr *)skb_transport_header(skb);
+}
+#endif
+
+#ifdef cpu_to_be16
+#undef cpu_to_be16
+#endif
+#define cpu_to_be16(x) __constant_htons(x)
+
+#if (!(RHEL_RELEASE_CODE && RHEL_RELEASE_CODE > RHEL_RELEASE_VERSION(5,1)))
+enum {
+	DUMP_PREFIX_NONE,
+	DUMP_PREFIX_ADDRESS,
+	DUMP_PREFIX_OFFSET
+};
+#endif /* !(RHEL_RELEASE_CODE > RHEL_RELEASE_VERSION(5,1)) */
+#ifndef hex_asc
+#define hex_asc(x)	"0123456789abcdef"[x]
+#endif
+#include <linux/ctype.h>
+extern void _kc_print_hex_dump(const char *level, const char *prefix_str,
+			       int prefix_type, int rowsize, int groupsize,
+			       const void *buf, size_t len, bool ascii);
+#define print_hex_dump(lvl, s, t, r, g, b, l, a) \
+		_kc_print_hex_dump(lvl, s, t, r, g, b, l, a)
+#ifndef ADVERTISED_2500baseX_Full
+#define ADVERTISED_2500baseX_Full (1 << 15)
+#endif
+#ifndef SUPPORTED_2500baseX_Full
+#define SUPPORTED_2500baseX_Full (1 << 15)
+#endif
+
+#ifdef HAVE_I2C_SUPPORT
+#include <linux/i2c.h>
+#if (!(RHEL_RELEASE_CODE && RHEL_RELEASE_CODE > RHEL_RELEASE_VERSION(5,5)))
+struct i2c_board_info {
+	char	driver_name[KOBJ_NAME_LEN];
+	char	type[I2C_NAME_SIZE];
+	unsigned short	flags;
+	unsigned short	addr;
+	void		*platform_data;
+};
+#define I2C_BOARD_INFO(driver, dev_addr) .driver_name = (driver),\
+			.addr = (dev_addr)
+#endif /* !(RHEL_RELEASE_CODE > RHEL_RELEASE_VERSION(5,5)) */
+#define i2c_new_device(adap, info) _kc_i2c_new_device(adap, info)
+extern struct i2c_client *
+_kc_i2c_new_device(struct i2c_adapter *adap, struct i2c_board_info const *info);
+#endif /* HAVE_I2C_SUPPORT */
+
+#else /* 2.6.22 */
+#define ETH_TYPE_TRANS_SETS_DEV
+#define HAVE_NETDEV_STATS_IN_NETDEV
+#endif /* < 2.6.22 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE > KERNEL_VERSION(2,6,22) )
+#undef SET_MODULE_OWNER
+#define SET_MODULE_OWNER(dev) do { } while (0)
+#endif /* > 2.6.22 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,23) )
+#define netif_subqueue_stopped(_a, _b) 0
+#ifndef PTR_ALIGN
+#define PTR_ALIGN(p, a)         ((typeof(p))ALIGN((unsigned long)(p), (a)))
+#endif
+
+#ifndef CONFIG_PM_SLEEP
+#define CONFIG_PM_SLEEP	CONFIG_PM
+#endif
+
+#if ( LINUX_VERSION_CODE > KERNEL_VERSION(2,6,13) )
+#define HAVE_ETHTOOL_GET_PERM_ADDR
+#endif /* 2.6.14 through 2.6.22 */
+#endif /* < 2.6.23 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24) )
+#ifndef ETH_FLAG_LRO
+#define ETH_FLAG_LRO NETIF_F_LRO
+#endif
+
+/* if GRO is supported then the napi struct must already exist */
+#ifndef NETIF_F_GRO
+/* NAPI API changes in 2.6.24 break everything */
+struct napi_struct {
+	/* used to look up the real NAPI polling routine */
+	int (*poll)(struct napi_struct *, int);
+	struct net_device *dev;
+	int weight;
+};
+#endif
+
+#ifdef NAPI
+extern int __kc_adapter_clean(struct net_device *, int *);
+extern struct net_device *napi_to_poll_dev(const struct napi_struct *napi);
+#define netif_napi_add(_netdev, _napi, _poll, _weight) \
+	do { \
+		struct napi_struct *__napi = (_napi); \
+		struct net_device *poll_dev = napi_to_poll_dev(__napi); \
+		poll_dev->poll = &(__kc_adapter_clean); \
+		poll_dev->priv = (_napi); \
+		poll_dev->weight = (_weight); \
+		set_bit(__LINK_STATE_RX_SCHED, &poll_dev->state); \
+		set_bit(__LINK_STATE_START, &poll_dev->state);\
+		dev_hold(poll_dev); \
+		__napi->poll = &(_poll); \
+		__napi->weight = (_weight); \
+		__napi->dev = (_netdev); \
+	} while (0)
+#define netif_napi_del(_napi) \
+	do { \
+		struct net_device *poll_dev = napi_to_poll_dev(_napi); \
+		WARN_ON(!test_bit(__LINK_STATE_RX_SCHED, &poll_dev->state)); \
+		dev_put(poll_dev); \
+		memset(poll_dev, 0, sizeof(struct net_device));\
+	} while (0)
+#define napi_schedule_prep(_napi) \
+	(netif_running((_napi)->dev) && netif_rx_schedule_prep(napi_to_poll_dev(_napi)))
+#define napi_schedule(_napi) \
+	do { \
+		if (napi_schedule_prep(_napi)) \
+			__netif_rx_schedule(napi_to_poll_dev(_napi)); \
+	} while (0)
+#define napi_enable(_napi) netif_poll_enable(napi_to_poll_dev(_napi))
+#define napi_disable(_napi) netif_poll_disable(napi_to_poll_dev(_napi))
+#ifdef CONFIG_SMP
+static inline void napi_synchronize(const struct napi_struct *n)
+{
+	struct net_device *dev = napi_to_poll_dev(n);
+
+	while (test_bit(__LINK_STATE_RX_SCHED, &dev->state)) {
+		/* No hurry. */
+		msleep(1);
+	}
+}
+#else
+#define napi_synchronize(n)	barrier()
+#endif /* CONFIG_SMP */
+#define __napi_schedule(_napi) __netif_rx_schedule(napi_to_poll_dev(_napi))
+#ifndef NETIF_F_GRO
+#define napi_complete(_napi) netif_rx_complete(napi_to_poll_dev(_napi))
+#else
+#define napi_complete(_napi) \
+	do { \
+		napi_gro_flush(_napi); \
+		netif_rx_complete(napi_to_poll_dev(_napi)); \
+	} while (0)
+#endif /* NETIF_F_GRO */
+#else /* NAPI */
+#define netif_napi_add(_netdev, _napi, _poll, _weight) \
+	do { \
+		struct napi_struct *__napi = _napi; \
+		_netdev->poll = &(_poll); \
+		_netdev->weight = (_weight); \
+		__napi->poll = &(_poll); \
+		__napi->weight = (_weight); \
+		__napi->dev = (_netdev); \
+	} while (0)
+#define netif_napi_del(_a) do {} while (0)
+#endif /* NAPI */
+
+#undef dev_get_by_name
+#define dev_get_by_name(_a, _b) dev_get_by_name(_b)
+#define __netif_subqueue_stopped(_a, _b) netif_subqueue_stopped(_a, _b)
+#ifndef DMA_BIT_MASK
+#define DMA_BIT_MASK(n)	(((n) == 64) ? DMA_64BIT_MASK : ((1ULL<<(n))-1))
+#endif
+
+#ifdef NETIF_F_TSO6
+#define skb_is_gso_v6 _kc_skb_is_gso_v6
+static inline int _kc_skb_is_gso_v6(const struct sk_buff *skb)
+{
+	return skb_shinfo(skb)->gso_type & SKB_GSO_TCPV6;
+}
+#endif /* NETIF_F_TSO6 */
+
+#ifndef KERN_CONT
+#define KERN_CONT	""
+#endif
+#ifndef pr_err
+#define pr_err(fmt, arg...) \
+	printk(KERN_ERR fmt, ##arg)
+#endif
+
+#ifndef rounddown_pow_of_two
+#define rounddown_pow_of_two(n) \
+	__builtin_constant_p(n) ? ( \
+		(n == 1) ? 0 : \
+		(1UL << ilog2(n))) : \
+		(1UL << (fls_long(n) - 1))
+#endif
+
+#else /* < 2.6.24 */
+#define HAVE_ETHTOOL_GET_SSET_COUNT
+#define HAVE_NETDEV_NAPI_LIST
+#endif /* < 2.6.24 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE > KERNEL_VERSION(2,6,24) )
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,2,0) )
+#include <linux/pm_qos_params.h>
+#else /* >= 3.2.0 */
+#include <linux/pm_qos.h>
+#endif /* else >= 3.2.0 */
+#endif /* > 2.6.24 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,25) )
+#define PM_QOS_CPU_DMA_LATENCY	1
+
+#if ( LINUX_VERSION_CODE > KERNEL_VERSION(2,6,18) )
+#include <linux/latency.h>
+#define PM_QOS_DEFAULT_VALUE	INFINITE_LATENCY
+#define pm_qos_add_requirement(pm_qos_class, name, value) \
+		set_acceptable_latency(name, value)
+#define pm_qos_remove_requirement(pm_qos_class, name) \
+		remove_acceptable_latency(name)
+#define pm_qos_update_requirement(pm_qos_class, name, value) \
+		modify_acceptable_latency(name, value)
+#else
+#define PM_QOS_DEFAULT_VALUE	-1
+#define pm_qos_add_requirement(pm_qos_class, name, value)
+#define pm_qos_remove_requirement(pm_qos_class, name)
+#define pm_qos_update_requirement(pm_qos_class, name, value) { \
+	if (value != PM_QOS_DEFAULT_VALUE) { \
+		printk(KERN_WARNING "%s: unable to set PM QoS requirement\n", \
+			pci_name(adapter->pdev)); \
+	} \
+}
+
+#endif /* > 2.6.18 */
+
+#define pci_enable_device_mem(pdev) pci_enable_device(pdev)
+
+#ifndef DEFINE_PCI_DEVICE_TABLE
+#define DEFINE_PCI_DEVICE_TABLE(_table) struct pci_device_id _table[]
+#endif /* DEFINE_PCI_DEVICE_TABLE */
+
+#ifndef strict_strtol
+#define strict_strtol(s, b, r) _kc_strict_strtol(s, b, r)
+static inline int _kc_strict_strtol(const char *buf, unsigned int base, long *res)
+{
+	/* adapted from strict_strtoul() in 2.6.25 */
+	char *tail;
+	long val;
+	size_t len;
+
+	*res = 0;
+	len = strlen(buf);
+	if (!len)
+		return -EINVAL;
+	val = simple_strtol(buf, &tail, base);
+	if (tail == buf)
+		return -EINVAL;
+	if ((*tail == '\0') ||
+	    ((len == (size_t)(tail - buf) + 1) && (*tail == '\n'))) {
+		*res = val;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+#endif
+
+
+#if ( LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0) )
+#ifndef IGB_PROCFS
+#define IGB_PROCFS
+#endif /* IGB_PROCFS */
+#endif /* >= 2.6.0 */
+
+#else /* < 2.6.25 */
+
+
+#if IS_ENABLED(CONFIG_HWMON)
+#ifndef IGB_HWMON
+#define IGB_HWMON
+#endif /* IGB_HWMON */
+#endif /* CONFIG_HWMON */
+
+#endif /* < 2.6.25 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,26) )
+#ifndef clamp_t
+#define clamp_t(type, val, min, max) ({		\
+	type __val = (val);			\
+	type __min = (min);			\
+	type __max = (max);			\
+	__val = __val < __min ? __min : __val;	\
+	__val > __max ? __max : __val; })
+#endif /* clamp_t */
+#undef kzalloc_node
+#define kzalloc_node(_size, _flags, _node) kzalloc(_size, _flags)
+
+extern void _kc_pci_disable_link_state(struct pci_dev *dev, int state);
+#define pci_disable_link_state(p, s) _kc_pci_disable_link_state(p, s)
+#else /* < 2.6.26 */
+#include <linux/pci-aspm.h>
+#define HAVE_NETDEV_VLAN_FEATURES
+#ifndef PCI_EXP_LNKCAP_ASPMS
+#define PCI_EXP_LNKCAP_ASPMS 0x00000c00 /* ASPM Support */
+#endif /* PCI_EXP_LNKCAP_ASPMS */
+#endif /* < 2.6.26 */
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,27) )
+static inline void _kc_ethtool_cmd_speed_set(struct ethtool_cmd *ep,
+					     __u32 speed)
+{
+	ep->speed = (__u16)speed;
+	/* ep->speed_hi = (__u16)(speed >> 16); */
+}
+#define ethtool_cmd_speed_set _kc_ethtool_cmd_speed_set
+
+static inline __u32 _kc_ethtool_cmd_speed(struct ethtool_cmd *ep)
+{
+	/* no speed_hi before 2.6.27, and probably no need for it yet */
+	return (__u32)ep->speed;
+}
+#define ethtool_cmd_speed _kc_ethtool_cmd_speed
+
+#if ( LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,15) )
+#if ((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,23)) && defined(CONFIG_PM))
+#define ANCIENT_PM 1
+#elif ((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,23)) && \
+       (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,26)) && \
+       defined(CONFIG_PM_SLEEP))
+#define NEWER_PM 1
+#endif
+#if defined(ANCIENT_PM) || defined(NEWER_PM)
+#undef device_set_wakeup_enable
+#define device_set_wakeup_enable(dev, val) \
+	do { \
+		u16 pmc = 0; \
+		int pm = pci_find_capability(adapter->pdev, PCI_CAP_ID_PM); \
+		if (pm) { \
+			pci_read_config_word(adapter->pdev, pm + PCI_PM_PMC, \
+				&pmc); \
+		} \
+		(dev)->power.can_wakeup = !!(pmc >> 11); \
+		(dev)->power.should_wakeup = (val && (pmc >> 11)); \
+	} while (0)
+#endif /* 2.6.15-2.6.22 and CONFIG_PM or 2.6.23-2.6.25 and CONFIG_PM_SLEEP */
+#endif /* 2.6.15 through 2.6.27 */
+#ifndef netif_napi_del
+#define netif_napi_del(_a) do {} while (0)
+#ifdef NAPI
+#ifdef CONFIG_NETPOLL
+#undef netif_napi_del
+#define netif_napi_del(_a) list_del(&(_a)->dev_list);
+#endif
+#endif
+#endif /* netif_napi_del */
+#ifdef dma_mapping_error
+#undef dma_mapping_error
+#endif
+#define dma_mapping_error(dev, dma_addr) pci_dma_mapping_error(dma_addr)
+
+#ifdef CONFIG_NETDEVICES_MULTIQUEUE
+#define HAVE_TX_MQ
+#endif
+
+#ifdef HAVE_TX_MQ
+extern void _kc_netif_tx_stop_all_queues(struct net_device *);
+extern void _kc_netif_tx_wake_all_queues(struct net_device *);
+extern void _kc_netif_tx_start_all_queues(struct net_device *);
+#define netif_tx_stop_all_queues(a) _kc_netif_tx_stop_all_queues(a)
+#define netif_tx_wake_all_queues(a) _kc_netif_tx_wake_all_queues(a)
+#define netif_tx_start_all_queues(a) _kc_netif_tx_start_all_queues(a)
+#undef netif_stop_subqueue
+#define netif_stop_subqueue(_ndev,_qi) do { \
+	if (netif_is_multiqueue((_ndev))) \
+		netif_stop_subqueue((_ndev), (_qi)); \
+	else \
+		netif_stop_queue((_ndev)); \
+	} while (0)
+#undef netif_start_subqueue
+#define netif_start_subqueue(_ndev,_qi) do { \
+	if (netif_is_multiqueue((_ndev))) \
+		netif_start_subqueue((_ndev), (_qi)); \
+	else \
+		netif_start_queue((_ndev)); \
+	} while (0)
+#else /* HAVE_TX_MQ */
+#define netif_tx_stop_all_queues(a) netif_stop_queue(a)
+#define netif_tx_wake_all_queues(a) netif_wake_queue(a)
+#if ( LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,12) )
+#define netif_tx_start_all_queues(a) netif_start_queue(a)
+#else
+#define netif_tx_start_all_queues(a) do {} while (0)
+#endif
+#define netif_stop_subqueue(_ndev,_qi) netif_stop_queue((_ndev))
+#define netif_start_subqueue(_ndev,_qi) netif_start_queue((_ndev))
+#endif /* HAVE_TX_MQ */
+#ifndef NETIF_F_MULTI_QUEUE
+#define NETIF_F_MULTI_QUEUE 0
+#define netif_is_multiqueue(a) 0
+#define netif_wake_subqueue(a, b)
+#endif /* NETIF_F_MULTI_QUEUE */
+
+#ifndef __WARN_printf
+extern void __kc_warn_slowpath(const char *file, const int line,
+		const char *fmt, ...) __attribute__((format(printf, 3, 4)));
+#define __WARN_printf(arg...) __kc_warn_slowpath(__FILE__, __LINE__, arg)
+#endif /* __WARN_printf */
+
+#ifndef WARN
+#define WARN(condition, format...) ({						\
+	int __ret_warn_on = !!(condition);				\
+	if (unlikely(__ret_warn_on))					\
+		__WARN_printf(format);					\
+	unlikely(__ret_warn_on);					\
+})
+#endif /* WARN */
+#undef HAVE_IXGBE_DEBUG_FS
+#undef HAVE_IGB_DEBUG_FS
+#else /* < 2.6.27 */
+#define HAVE_TX_MQ
+#define HAVE_NETDEV_SELECT_QUEUE
+#ifdef CONFIG_DEBUG_FS
+#define HAVE_IXGBE_DEBUG_FS
+#define HAVE_IGB_DEBUG_FS
+#endif /* CONFIG_DEBUG_FS */
+#endif /* < 2.6.27 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,28) )
+#define pci_ioremap_bar(pdev, bar)	ioremap(pci_resource_start(pdev, bar), \
+					        pci_resource_len(pdev, bar))
+#define pci_wake_from_d3 _kc_pci_wake_from_d3
+#define pci_prepare_to_sleep _kc_pci_prepare_to_sleep
+extern int _kc_pci_wake_from_d3(struct pci_dev *dev, bool enable);
+extern int _kc_pci_prepare_to_sleep(struct pci_dev *dev);
+#define netdev_alloc_page(a) alloc_page(GFP_ATOMIC)
+#ifndef __skb_queue_head_init
+static inline void __kc_skb_queue_head_init(struct sk_buff_head *list)
+{
+	list->prev = list->next = (struct sk_buff *)list;
+	list->qlen = 0;
+}
+#define __skb_queue_head_init(_q) __kc_skb_queue_head_init(_q)
+#endif
+
+#define PCI_EXP_DEVCAP2		36	/* Device Capabilities 2 */
+#define PCI_EXP_DEVCTL2		40	/* Device Control 2 */
+
+#endif /* < 2.6.28 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,29) )
+#ifndef swap
+#define swap(a, b) \
+	do { typeof(a) __tmp = (a); (a) = (b); (b) = __tmp; } while (0)
+#endif
+#define pci_request_selected_regions_exclusive(pdev, bars, name) \
+		pci_request_selected_regions(pdev, bars, name)
+#ifndef CONFIG_NR_CPUS
+#define CONFIG_NR_CPUS 1
+#endif /* CONFIG_NR_CPUS */
+#ifndef pcie_aspm_enabled
+#define pcie_aspm_enabled()   (1)
+#endif /* pcie_aspm_enabled */
+
+#define  PCI_EXP_SLTSTA_PDS	0x0040	/* Presence Detect State */
+
+#ifndef pci_clear_master
+extern void _kc_pci_clear_master(struct pci_dev *dev);
+#define pci_clear_master(dev)	_kc_pci_clear_master(dev)
+#endif
+
+#ifndef PCI_EXP_LNKCTL_ASPMC
+#define  PCI_EXP_LNKCTL_ASPMC	0x0003	/* ASPM Control */
+#endif
+#else /* < 2.6.29 */
+#ifndef HAVE_NET_DEVICE_OPS
+#define HAVE_NET_DEVICE_OPS
+#endif
+#ifdef CONFIG_DCB
+#define HAVE_PFC_MODE_ENABLE
+#endif /* CONFIG_DCB */
+#endif /* < 2.6.29 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,30) )
+#define skb_rx_queue_recorded(a) false
+#define skb_get_rx_queue(a) 0
+#define skb_record_rx_queue(a, b) do {} while (0)
+#define skb_tx_hash(n, s) ___kc_skb_tx_hash((n), (s), (n)->real_num_tx_queues)
+#ifndef CONFIG_PCI_IOV
+#undef pci_enable_sriov
+#define pci_enable_sriov(a, b) -ENOTSUPP
+#undef pci_disable_sriov
+#define pci_disable_sriov(a) do {} while (0)
+#endif /* CONFIG_PCI_IOV */
+#ifndef pr_cont
+#define pr_cont(fmt, ...) \
+	printk(KERN_CONT fmt, ##__VA_ARGS__)
+#endif /* pr_cont */
+static inline void _kc_synchronize_irq(unsigned int a)
+{
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,5,28) )
+	synchronize_irq();
+#else /* < 2.5.28 */
+	synchronize_irq(a);
+#endif /* < 2.5.28 */
+}
+#undef synchronize_irq
+#define synchronize_irq(a) _kc_synchronize_irq(a)
+
+#define PCI_EXP_LNKCTL2		48	/* Link Control 2 */
+
+#ifdef nr_cpus_node
+#undef nr_cpus_node
+#define nr_cpus_node(node) cpumask_weight(cpumask_of_node(node))
+#endif
+
+#else /* < 2.6.30 */
+#define HAVE_ASPM_QUIRKS
+#endif /* < 2.6.30 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,31) )
+#define ETH_P_1588 0x88F7
+#define ETH_P_FIP  0x8914
+#ifndef netdev_uc_count
+#define netdev_uc_count(dev) ((dev)->uc_count)
+#endif
+#ifndef netdev_for_each_uc_addr
+#define netdev_for_each_uc_addr(uclist, dev) \
+	for (uclist = dev->uc_list; uclist; uclist = uclist->next)
+#endif
+#ifndef PORT_OTHER
+#define PORT_OTHER 0xff
+#endif
+#ifndef MDIO_PHY_ID_PRTAD
+#define MDIO_PHY_ID_PRTAD 0x03e0
+#endif
+#ifndef MDIO_PHY_ID_DEVAD
+#define MDIO_PHY_ID_DEVAD 0x001f
+#endif
+#ifndef skb_dst
+#define skb_dst(s) ((s)->dst)
+#endif
+
+#ifndef SUPPORTED_1000baseKX_Full
+#define SUPPORTED_1000baseKX_Full	(1 << 17)
+#endif
+#ifndef SUPPORTED_10000baseKX4_Full
+#define SUPPORTED_10000baseKX4_Full	(1 << 18)
+#endif
+#ifndef SUPPORTED_10000baseKR_Full
+#define SUPPORTED_10000baseKR_Full	(1 << 19)
+#endif
+
+#ifndef ADVERTISED_1000baseKX_Full
+#define ADVERTISED_1000baseKX_Full	(1 << 17)
+#endif
+#ifndef ADVERTISED_10000baseKX4_Full
+#define ADVERTISED_10000baseKX4_Full	(1 << 18)
+#endif
+#ifndef ADVERTISED_10000baseKR_Full
+#define ADVERTISED_10000baseKR_Full	(1 << 19)
+#endif
+
+#else /* < 2.6.31 */
+#ifndef HAVE_NETDEV_STORAGE_ADDRESS
+#define HAVE_NETDEV_STORAGE_ADDRESS
+#endif
+#ifndef HAVE_NETDEV_HW_ADDR
+#define HAVE_NETDEV_HW_ADDR
+#endif
+#ifndef HAVE_TRANS_START_IN_QUEUE
+#define HAVE_TRANS_START_IN_QUEUE
+#endif
+#ifndef HAVE_INCLUDE_LINUX_MDIO_H
+#define HAVE_INCLUDE_LINUX_MDIO_H
+#endif
+#include <linux/mdio.h>
+#endif /* < 2.6.31 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,32) )
+#undef netdev_tx_t
+#define netdev_tx_t int
+#if defined(CONFIG_FCOE) || defined(CONFIG_FCOE_MODULE)
+#ifndef NETIF_F_FCOE_MTU
+#define NETIF_F_FCOE_MTU       (1 << 26)
+#endif
+#endif /* CONFIG_FCOE || CONFIG_FCOE_MODULE */
+
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0) )
+static inline int _kc_pm_runtime_get_sync()
+{
+	return 1;
+}
+#define pm_runtime_get_sync(dev)	_kc_pm_runtime_get_sync()
+#else /* 2.6.0 => 2.6.32 */
+static inline int _kc_pm_runtime_get_sync(struct device *dev)
+{
+	return 1;
+}
+#ifndef pm_runtime_get_sync
+#define pm_runtime_get_sync(dev)	_kc_pm_runtime_get_sync(dev)
+#endif
+#endif /* 2.6.0 => 2.6.32 */
+#ifndef pm_runtime_put
+#define pm_runtime_put(dev)		do {} while (0)
+#endif
+#ifndef pm_runtime_put_sync
+#define pm_runtime_put_sync(dev)	do {} while (0)
+#endif
+#ifndef pm_runtime_resume
+#define pm_runtime_resume(dev)		do {} while (0)
+#endif
+#ifndef pm_schedule_suspend
+#define pm_schedule_suspend(dev, t)	do {} while (0)
+#endif
+#ifndef pm_runtime_set_suspended
+#define pm_runtime_set_suspended(dev)	do {} while (0)
+#endif
+#ifndef pm_runtime_disable
+#define pm_runtime_disable(dev)		do {} while (0)
+#endif
+#ifndef pm_runtime_put_noidle
+#define pm_runtime_put_noidle(dev)	do {} while (0)
+#endif
+#ifndef pm_runtime_set_active
+#define pm_runtime_set_active(dev)	do {} while (0)
+#endif
+#ifndef pm_runtime_enable
+#define pm_runtime_enable(dev)	do {} while (0)
+#endif
+#ifndef pm_runtime_get_noresume
+#define pm_runtime_get_noresume(dev)	do {} while (0)
+#endif
+#else /* < 2.6.32 */
+#if defined(CONFIG_FCOE) || defined(CONFIG_FCOE_MODULE)
+#ifndef HAVE_NETDEV_OPS_FCOE_ENABLE
+#define HAVE_NETDEV_OPS_FCOE_ENABLE
+#endif
+#endif /* CONFIG_FCOE || CONFIG_FCOE_MODULE */
+#ifdef CONFIG_DCB
+#ifndef HAVE_DCBNL_OPS_GETAPP
+#define HAVE_DCBNL_OPS_GETAPP
+#endif
+#endif /* CONFIG_DCB */
+#include <linux/pm_runtime.h>
+/* IOV bad DMA target work arounds require at least this kernel rev support */
+#define HAVE_PCIE_TYPE
+#endif /* < 2.6.32 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,33) )
+#ifndef pci_pcie_cap
+#define pci_pcie_cap(pdev) pci_find_capability(pdev, PCI_CAP_ID_EXP)
+#endif
+#ifndef IPV4_FLOW
+#define IPV4_FLOW 0x10
+#endif /* IPV4_FLOW */
+#ifndef IPV6_FLOW
+#define IPV6_FLOW 0x11
+#endif /* IPV6_FLOW */
+/* Features back-ported to RHEL6 or SLES11 SP1 after 2.6.32 */
+#if ( (RHEL_RELEASE_CODE && RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(6,0)) || \
+      (SLE_VERSION_CODE && SLE_VERSION_CODE >= SLE_VERSION(11,1,0)) )
+#if defined(CONFIG_FCOE) || defined(CONFIG_FCOE_MODULE)
+#ifndef HAVE_NETDEV_OPS_FCOE_GETWWN
+#define HAVE_NETDEV_OPS_FCOE_GETWWN
+#endif
+#endif /* CONFIG_FCOE || CONFIG_FCOE_MODULE */
+#endif /* RHEL6 or SLES11 SP1 */
+#ifndef __percpu
+#define __percpu
+#endif /* __percpu */
+#ifndef PORT_DA
+#define PORT_DA PORT_OTHER
+#endif
+#ifndef PORT_NONE
+#define PORT_NONE PORT_OTHER
+#endif
+
+#if ((RHEL_RELEASE_CODE && \
+     (RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(6,3)) && \
+     (RHEL_RELEASE_CODE < RHEL_RELEASE_VERSION(7,0))))
+#if !defined(CONFIG_X86_32) && !defined(CONFIG_NEED_DMA_MAP_STATE)
+#undef DEFINE_DMA_UNMAP_ADDR
+#define DEFINE_DMA_UNMAP_ADDR(ADDR_NAME)	dma_addr_t ADDR_NAME
+#undef DEFINE_DMA_UNMAP_LEN
+#define DEFINE_DMA_UNMAP_LEN(LEN_NAME)		__u32 LEN_NAME
+#undef dma_unmap_addr
+#define dma_unmap_addr(PTR, ADDR_NAME)		((PTR)->ADDR_NAME)
+#undef dma_unmap_addr_set
+#define dma_unmap_addr_set(PTR, ADDR_NAME, VAL)	(((PTR)->ADDR_NAME) = (VAL))
+#undef dma_unmap_len
+#define dma_unmap_len(PTR, LEN_NAME)		((PTR)->LEN_NAME)
+#undef dma_unmap_len_set
+#define dma_unmap_len_set(PTR, LEN_NAME, VAL)	(((PTR)->LEN_NAME) = (VAL))
+#endif /* CONFIG_X86_64 && !CONFIG_NEED_DMA_MAP_STATE */
+#endif /* RHEL_RELEASE_CODE */
+
+#if (!(RHEL_RELEASE_CODE && \
+       (((RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(5,8)) && \
+         (RHEL_RELEASE_CODE < RHEL_RELEASE_VERSION(6,0))) || \
+        ((RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(6,1)) && \
+         (RHEL_RELEASE_CODE < RHEL_RELEASE_VERSION(7,0))))))
+static inline bool pci_is_pcie(struct pci_dev *dev)
+{
+	return !!pci_pcie_cap(dev);
+}
+#endif /* RHEL_RELEASE_CODE */
+
+#ifndef __always_unused
+#define __always_unused __attribute__((__unused__))
+#endif
+#ifndef __maybe_unused
+#define __maybe_unused __attribute__((__unused__))
+#endif
+
+#if (!(RHEL_RELEASE_CODE && \
+      (RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(6,2))))
+#define sk_tx_queue_get(_sk) (-1)
+#define sk_tx_queue_set(_sk, _tx_queue) do {} while(0)
+#endif /* !(RHEL >= 6.2) */
+
+#if (RHEL_RELEASE_CODE && \
+     (RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(6,4)) && \
+     (RHEL_RELEASE_CODE < RHEL_RELEASE_VERSION(7,0)))
+#define HAVE_RHEL6_ETHTOOL_OPS_EXT_STRUCT
+#define HAVE_ETHTOOL_SET_PHYS_ID
+#define HAVE_ETHTOOL_GET_TS_INFO
+#endif /* RHEL >= 6.4 && RHEL < 7.0 */
+
+#else /* < 2.6.33 */
+#if defined(CONFIG_FCOE) || defined(CONFIG_FCOE_MODULE)
+#ifndef HAVE_NETDEV_OPS_FCOE_GETWWN
+#define HAVE_NETDEV_OPS_FCOE_GETWWN
+#endif
+#endif /* CONFIG_FCOE || CONFIG_FCOE_MODULE */
+#endif /* < 2.6.33 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,34) )
+#if (RHEL_RELEASE_CODE < RHEL_RELEASE_VERSION(6,0))
+#ifndef pci_num_vf
+#define pci_num_vf(pdev) _kc_pci_num_vf(pdev)
+extern int _kc_pci_num_vf(struct pci_dev *dev);
+#endif
+#endif /* RHEL_RELEASE_CODE */
+
+#ifndef ETH_FLAG_NTUPLE
+#define ETH_FLAG_NTUPLE NETIF_F_NTUPLE
+#endif
+
+#ifndef netdev_mc_count
+#define netdev_mc_count(dev) ((dev)->mc_count)
+#endif
+#ifndef netdev_mc_empty
+#define netdev_mc_empty(dev) (netdev_mc_count(dev) == 0)
+#endif
+#ifndef netdev_for_each_mc_addr
+#define netdev_for_each_mc_addr(mclist, dev) \
+	for (mclist = dev->mc_list; mclist; mclist = mclist->next)
+#endif
+#ifndef netdev_uc_count
+#define netdev_uc_count(dev) ((dev)->uc.count)
+#endif
+#ifndef netdev_uc_empty
+#define netdev_uc_empty(dev) (netdev_uc_count(dev) == 0)
+#endif
+#ifndef netdev_for_each_uc_addr
+#define netdev_for_each_uc_addr(ha, dev) \
+	list_for_each_entry(ha, &dev->uc.list, list)
+#endif
+#ifndef dma_set_coherent_mask
+#define dma_set_coherent_mask(dev,mask) \
+	pci_set_consistent_dma_mask(to_pci_dev(dev),(mask))
+#endif
+#ifndef pci_dev_run_wake
+#define pci_dev_run_wake(pdev)	(0)
+#endif
+
+/* netdev logging taken from include/linux/netdevice.h */
+#ifndef netdev_name
+static inline const char *_kc_netdev_name(const struct net_device *dev)
+{
+	if (dev->reg_state != NETREG_REGISTERED)
+		return "(unregistered net_device)";
+	return dev->name;
+}
+#define netdev_name(netdev)	_kc_netdev_name(netdev)
+#endif /* netdev_name */
+
+#undef netdev_printk
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0) )
+#define netdev_printk(level, netdev, format, args...)		\
+do {								\
+	struct pci_dev *pdev = _kc_netdev_to_pdev(netdev);	\
+	printk(level "%s: " format, pci_name(pdev), ##args);	\
+} while(0)
+#elif ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21) )
+#define netdev_printk(level, netdev, format, args...)		\
+do {								\
+	struct pci_dev *pdev = _kc_netdev_to_pdev(netdev);	\
+	struct device *dev = pci_dev_to_dev(pdev);		\
+	dev_printk(level, dev, "%s: " format,			\
+		   netdev_name(netdev), ##args);		\
+} while(0)
+#else /* 2.6.21 => 2.6.34 */
+#define netdev_printk(level, netdev, format, args...)		\
+	dev_printk(level, (netdev)->dev.parent,			\
+		   "%s: " format,				\
+		   netdev_name(netdev), ##args)
+#endif /* <2.6.0 <2.6.21 <2.6.34 */
+#undef netdev_emerg
+#define netdev_emerg(dev, format, args...)			\
+	netdev_printk(KERN_EMERG, dev, format, ##args)
+#undef netdev_alert
+#define netdev_alert(dev, format, args...)			\
+	netdev_printk(KERN_ALERT, dev, format, ##args)
+#undef netdev_crit
+#define netdev_crit(dev, format, args...)			\
+	netdev_printk(KERN_CRIT, dev, format, ##args)
+#undef netdev_err
+#define netdev_err(dev, format, args...)			\
+	netdev_printk(KERN_ERR, dev, format, ##args)
+#undef netdev_warn
+#define netdev_warn(dev, format, args...)			\
+	netdev_printk(KERN_WARNING, dev, format, ##args)
+#undef netdev_notice
+#define netdev_notice(dev, format, args...)			\
+	netdev_printk(KERN_NOTICE, dev, format, ##args)
+#undef netdev_info
+#define netdev_info(dev, format, args...)			\
+	netdev_printk(KERN_INFO, dev, format, ##args)
+#undef netdev_dbg
+#if defined(DEBUG)
+#define netdev_dbg(__dev, format, args...)			\
+	netdev_printk(KERN_DEBUG, __dev, format, ##args)
+#elif defined(CONFIG_DYNAMIC_DEBUG)
+#define netdev_dbg(__dev, format, args...)			\
+do {								\
+	dynamic_dev_dbg((__dev)->dev.parent, "%s: " format,	\
+			netdev_name(__dev), ##args);		\
+} while (0)
+#else /* DEBUG */
+#define netdev_dbg(__dev, format, args...)			\
+({								\
+	if (0)							\
+		netdev_printk(KERN_DEBUG, __dev, format, ##args); \
+	0;							\
+})
+#endif /* DEBUG */
+
+#undef netif_printk
+#define netif_printk(priv, type, level, dev, fmt, args...)	\
+do {								\
+	if (netif_msg_##type(priv))				\
+		netdev_printk(level, (dev), fmt, ##args);	\
+} while (0)
+
+#undef netif_emerg
+#define netif_emerg(priv, type, dev, fmt, args...)		\
+	netif_level(emerg, priv, type, dev, fmt, ##args)
+#undef netif_alert
+#define netif_alert(priv, type, dev, fmt, args...)		\
+	netif_level(alert, priv, type, dev, fmt, ##args)
+#undef netif_crit
+#define netif_crit(priv, type, dev, fmt, args...)		\
+	netif_level(crit, priv, type, dev, fmt, ##args)
+#undef netif_err
+#define netif_err(priv, type, dev, fmt, args...)		\
+	netif_level(err, priv, type, dev, fmt, ##args)
+#undef netif_warn
+#define netif_warn(priv, type, dev, fmt, args...)		\
+	netif_level(warn, priv, type, dev, fmt, ##args)
+#undef netif_notice
+#define netif_notice(priv, type, dev, fmt, args...)		\
+	netif_level(notice, priv, type, dev, fmt, ##args)
+#undef netif_info
+#define netif_info(priv, type, dev, fmt, args...)		\
+	netif_level(info, priv, type, dev, fmt, ##args)
+#undef netif_dbg
+#define netif_dbg(priv, type, dev, fmt, args...)		\
+	netif_level(dbg, priv, type, dev, fmt, ##args)
+
+#ifdef SET_SYSTEM_SLEEP_PM_OPS
+#define HAVE_SYSTEM_SLEEP_PM_OPS
+#endif
+
+#ifndef for_each_set_bit
+#define for_each_set_bit(bit, addr, size) \
+	for ((bit) = find_first_bit((addr), (size)); \
+		(bit) < (size); \
+		(bit) = find_next_bit((addr), (size), (bit) + 1))
+#endif /* for_each_set_bit */
+
+#ifndef DEFINE_DMA_UNMAP_ADDR
+#define DEFINE_DMA_UNMAP_ADDR DECLARE_PCI_UNMAP_ADDR
+#define DEFINE_DMA_UNMAP_LEN DECLARE_PCI_UNMAP_LEN
+#define dma_unmap_addr pci_unmap_addr
+#define dma_unmap_addr_set pci_unmap_addr_set
+#define dma_unmap_len pci_unmap_len
+#define dma_unmap_len_set pci_unmap_len_set
+#endif /* DEFINE_DMA_UNMAP_ADDR */
+
+#if (RHEL_RELEASE_CODE < RHEL_RELEASE_VERSION(6,3))
+#ifdef IGB_HWMON
+#ifdef CONFIG_DEBUG_LOCK_ALLOC
+#define sysfs_attr_init(attr)				\
+	do {						\
+		static struct lock_class_key __key;	\
+		(attr)->key = &__key;			\
+	} while (0)
+#else
+#define sysfs_attr_init(attr) do {} while (0)
+#endif /* CONFIG_DEBUG_LOCK_ALLOC */
+#endif /* IGB_HWMON */
+#endif /* RHEL_RELEASE_CODE */
+
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0) )
+static inline bool _kc_pm_runtime_suspended()
+{
+	return false;
+}
+#define pm_runtime_suspended(dev)	_kc_pm_runtime_suspended()
+#else /* 2.6.0 => 2.6.34 */
+static inline bool _kc_pm_runtime_suspended(struct device *dev)
+{
+	return false;
+}
+#ifndef pm_runtime_suspended
+#define pm_runtime_suspended(dev)	_kc_pm_runtime_suspended(dev)
+#endif
+#endif /* 2.6.0 => 2.6.34 */
+
+#else /* < 2.6.34 */
+#define HAVE_SYSTEM_SLEEP_PM_OPS
+#ifndef HAVE_SET_RX_MODE
+#define HAVE_SET_RX_MODE
+#endif
+
+#endif /* < 2.6.34 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35) )
+
+ssize_t _kc_simple_write_to_buffer(void *to, size_t available, loff_t *ppos,
+				   const void __user *from, size_t count);
+#define simple_write_to_buffer _kc_simple_write_to_buffer
+
+#ifndef numa_node_id
+#define numa_node_id() 0
+#endif
+#ifdef HAVE_TX_MQ
+#include <net/sch_generic.h>
+#ifndef CONFIG_NETDEVICES_MULTIQUEUE
+#if (!(RHEL_RELEASE_CODE && RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(6,0)))
+void _kc_netif_set_real_num_tx_queues(struct net_device *, unsigned int);
+#define netif_set_real_num_tx_queues  _kc_netif_set_real_num_tx_queues
+#endif /* !(RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(6,0)) */
+#else /* CONFIG_NETDEVICES_MULTI_QUEUE */
+#define netif_set_real_num_tx_queues(_netdev, _count) \
+	do { \
+		(_netdev)->egress_subqueue_count = _count; \
+	} while (0)
+#endif /* CONFIG_NETDEVICES_MULTI_QUEUE */
+#else /* HAVE_TX_MQ */
+#define netif_set_real_num_tx_queues(_netdev, _count) do {} while(0)
+#endif /* HAVE_TX_MQ */
+#ifndef ETH_FLAG_RXHASH
+#define ETH_FLAG_RXHASH (1<<28)
+#endif /* ETH_FLAG_RXHASH */
+#if (RHEL_RELEASE_CODE && RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(6,0))
+#define HAVE_IRQ_AFFINITY_HINT
+#endif
+#else /* < 2.6.35 */
+#define HAVE_PM_QOS_REQUEST_LIST
+#define HAVE_IRQ_AFFINITY_HINT
+#endif /* < 2.6.35 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,36) )
+extern int _kc_ethtool_op_set_flags(struct net_device *, u32, u32);
+#define ethtool_op_set_flags _kc_ethtool_op_set_flags
+extern u32 _kc_ethtool_op_get_flags(struct net_device *);
+#define ethtool_op_get_flags _kc_ethtool_op_get_flags
+
+#ifdef CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS
+#ifdef NET_IP_ALIGN
+#undef NET_IP_ALIGN
+#endif
+#define NET_IP_ALIGN 0
+#endif /* CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS */
+
+#ifdef NET_SKB_PAD
+#undef NET_SKB_PAD
+#endif
+
+#if (L1_CACHE_BYTES > 32)
+#define NET_SKB_PAD L1_CACHE_BYTES
+#else
+#define NET_SKB_PAD 32
+#endif
+
+static inline struct sk_buff *_kc_netdev_alloc_skb_ip_align(struct net_device *dev,
+							    unsigned int length)
+{
+	struct sk_buff *skb;
+
+	skb = alloc_skb(length + NET_SKB_PAD + NET_IP_ALIGN, GFP_ATOMIC);
+	if (skb) {
+#if (NET_IP_ALIGN + NET_SKB_PAD)
+		skb_reserve(skb, NET_IP_ALIGN + NET_SKB_PAD);
+#endif
+		skb->dev = dev;
+	}
+	return skb;
+}
+
+#ifdef netdev_alloc_skb_ip_align
+#undef netdev_alloc_skb_ip_align
+#endif
+#define netdev_alloc_skb_ip_align(n, l) _kc_netdev_alloc_skb_ip_align(n, l)
+
+#undef netif_level
+#define netif_level(level, priv, type, dev, fmt, args...)	\
+do {								\
+	if (netif_msg_##type(priv))				\
+		netdev_##level(dev, fmt, ##args);		\
+} while (0)
+
+#undef usleep_range
+#define usleep_range(min, max)	msleep(DIV_ROUND_UP(min, 1000))
+
+#define u64_stats_update_begin(a) do { } while(0)
+#define u64_stats_update_end(a) do { } while(0)
+#define u64_stats_fetch_begin(a) do { } while(0)
+#define u64_stats_fetch_retry_bh(a) (0)
+#define u64_stats_fetch_begin_bh(a) (0)
+
+#if (RHEL_RELEASE_CODE && RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(6,1))
+#define HAVE_8021P_SUPPORT
+#endif
+
+#else /* < 2.6.36 */
+
+
+#define HAVE_PM_QOS_REQUEST_ACTIVE
+#define HAVE_8021P_SUPPORT
+#define HAVE_NDO_GET_STATS64
+#endif /* < 2.6.36 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,37) )
+#ifndef netif_set_real_num_rx_queues
+static inline int __kc_netif_set_real_num_rx_queues(struct net_device *dev,
+						    unsigned int rxq)
+{
+	return 0;
+}
+#define netif_set_real_num_rx_queues(dev, rxq) \
+	__kc_netif_set_real_num_rx_queues((dev), (rxq))
+#endif
+#ifndef ETHTOOL_RXNTUPLE_ACTION_CLEAR
+#define ETHTOOL_RXNTUPLE_ACTION_CLEAR (-2)
+#endif
+#ifndef VLAN_N_VID
+#define VLAN_N_VID	VLAN_GROUP_ARRAY_LEN
+#endif /* VLAN_N_VID */
+#ifndef ETH_FLAG_TXVLAN
+#define ETH_FLAG_TXVLAN (1 << 7)
+#endif /* ETH_FLAG_TXVLAN */
+#ifndef ETH_FLAG_RXVLAN
+#define ETH_FLAG_RXVLAN (1 << 8)
+#endif /* ETH_FLAG_RXVLAN */
+
+static inline void _kc_skb_checksum_none_assert(struct sk_buff *skb)
+{
+	WARN_ON(skb->ip_summed != CHECKSUM_NONE);
+}
+#define skb_checksum_none_assert(skb) _kc_skb_checksum_none_assert(skb)
+
+static inline void *_kc_vzalloc_node(unsigned long size, int node)
+{
+	void *addr = vmalloc_node(size, node);
+	if (addr)
+		memset(addr, 0, size);
+	return addr;
+}
+#define vzalloc_node(_size, _node) _kc_vzalloc_node(_size, _node)
+
+static inline void *_kc_vzalloc(unsigned long size)
+{
+	void *addr = vmalloc(size);
+	if (addr)
+		memset(addr, 0, size);
+	return addr;
+}
+#define vzalloc(_size) _kc_vzalloc(_size)
+
+#ifndef vlan_get_protocol
+static inline __be16 __kc_vlan_get_protocol(const struct sk_buff *skb)
+{
+	if (vlan_tx_tag_present(skb) ||
+	    skb->protocol != cpu_to_be16(ETH_P_8021Q))
+		return skb->protocol;
+
+	if (skb_headlen(skb) < sizeof(struct vlan_ethhdr))
+		return 0;
+
+	return ((struct vlan_ethhdr*)skb->data)->h_vlan_encapsulated_proto;
+}
+#define vlan_get_protocol(_skb) __kc_vlan_get_protocol(_skb)
+#endif
+#ifdef HAVE_HW_TIME_STAMP
+#define SKBTX_HW_TSTAMP (1 << 0)
+#define SKBTX_IN_PROGRESS (1 << 2)
+#define SKB_SHARED_TX_IS_UNION
+#endif
+
+#ifndef device_wakeup_enable
+#define device_wakeup_enable(dev)	device_set_wakeup_enable(dev, true)
+#endif
+
+#if ( LINUX_VERSION_CODE > KERNEL_VERSION(2,4,18) )
+#ifndef HAVE_VLAN_RX_REGISTER
+#define HAVE_VLAN_RX_REGISTER
+#endif
+#endif /* > 2.4.18 */
+#endif /* < 2.6.37 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,38) )
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22) )
+#define skb_checksum_start_offset(skb) skb_transport_offset(skb)
+#else /* 2.6.22 -> 2.6.37 */
+static inline int _kc_skb_checksum_start_offset(const struct sk_buff *skb)
+{
+        return skb->csum_start - skb_headroom(skb);
+}
+#define skb_checksum_start_offset(skb) _kc_skb_checksum_start_offset(skb)
+#endif /* 2.6.22 -> 2.6.37 */
+#ifdef CONFIG_DCB
+#ifndef IEEE_8021QAZ_MAX_TCS
+#define IEEE_8021QAZ_MAX_TCS 8
+#endif
+#ifndef DCB_CAP_DCBX_HOST
+#define DCB_CAP_DCBX_HOST		0x01
+#endif
+#ifndef DCB_CAP_DCBX_LLD_MANAGED
+#define DCB_CAP_DCBX_LLD_MANAGED	0x02
+#endif
+#ifndef DCB_CAP_DCBX_VER_CEE
+#define DCB_CAP_DCBX_VER_CEE		0x04
+#endif
+#ifndef DCB_CAP_DCBX_VER_IEEE
+#define DCB_CAP_DCBX_VER_IEEE		0x08
+#endif
+#ifndef DCB_CAP_DCBX_STATIC
+#define DCB_CAP_DCBX_STATIC		0x10
+#endif
+#endif /* CONFIG_DCB */
+#if (RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(6,2))
+#define CONFIG_XPS
+#endif /* RHEL_RELEASE_VERSION(6,2) */
+#endif /* < 2.6.38 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,39) )
+#ifndef NETIF_F_RXCSUM
+#define NETIF_F_RXCSUM		(1 << 29)
+#endif
+#ifndef skb_queue_reverse_walk_safe
+#define skb_queue_reverse_walk_safe(queue, skb, tmp)				\
+		for (skb = (queue)->prev, tmp = skb->prev;			\
+		     skb != (struct sk_buff *)(queue);				\
+		     skb = tmp, tmp = skb->prev)
+#endif
+#else /* < 2.6.39 */
+#if defined(CONFIG_FCOE) || defined(CONFIG_FCOE_MODULE)
+#ifndef HAVE_NETDEV_OPS_FCOE_DDP_TARGET
+#define HAVE_NETDEV_OPS_FCOE_DDP_TARGET
+#endif
+#endif /* CONFIG_FCOE || CONFIG_FCOE_MODULE */
+#ifndef HAVE_MQPRIO
+#define HAVE_MQPRIO
+#endif
+#ifndef HAVE_SETUP_TC
+#define HAVE_SETUP_TC
+#endif
+#ifdef CONFIG_DCB
+#ifndef HAVE_DCBNL_IEEE
+#define HAVE_DCBNL_IEEE
+#endif
+#endif /* CONFIG_DCB */
+#ifndef HAVE_NDO_SET_FEATURES
+#define HAVE_NDO_SET_FEATURES
+#endif
+#endif /* < 2.6.39 */
+
+/*****************************************************************************/
+/* use < 2.6.40 because of a Fedora 15 kernel update where they
+ * updated the kernel version to 2.6.40.x and they back-ported 3.0 features
+ * like set_phys_id for ethtool.
+ */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,40) )
+#ifdef ETHTOOL_GRXRINGS
+#ifndef FLOW_EXT
+#define	FLOW_EXT	0x80000000
+union _kc_ethtool_flow_union {
+	struct ethtool_tcpip4_spec		tcp_ip4_spec;
+	struct ethtool_usrip4_spec		usr_ip4_spec;
+	__u8					hdata[60];
+};
+struct _kc_ethtool_flow_ext {
+	__be16	vlan_etype;
+	__be16	vlan_tci;
+	__be32	data[2];
+};
+struct _kc_ethtool_rx_flow_spec {
+	__u32		flow_type;
+	union _kc_ethtool_flow_union h_u;
+	struct _kc_ethtool_flow_ext h_ext;
+	union _kc_ethtool_flow_union m_u;
+	struct _kc_ethtool_flow_ext m_ext;
+	__u64		ring_cookie;
+	__u32		location;
+};
+#define ethtool_rx_flow_spec _kc_ethtool_rx_flow_spec
+#endif /* FLOW_EXT */
+#endif
+
+#define pci_disable_link_state_locked pci_disable_link_state
+
+#ifndef PCI_LTR_VALUE_MASK
+#define  PCI_LTR_VALUE_MASK	0x000003ff
+#endif
+#ifndef PCI_LTR_SCALE_MASK
+#define  PCI_LTR_SCALE_MASK	0x00001c00
+#endif
+#ifndef PCI_LTR_SCALE_SHIFT
+#define  PCI_LTR_SCALE_SHIFT	10
+#endif
+
+#else /* < 2.6.40 */
+#define HAVE_ETHTOOL_SET_PHYS_ID
+#endif /* < 2.6.40 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,0,0) )
+#define USE_LEGACY_PM_SUPPORT
+#ifndef kfree_rcu
+#define kfree_rcu(_ptr, _rcu_head) kfree(_ptr)
+#endif
+#ifndef kstrtol_from_user
+#define kstrtol_from_user(s, c, b, r) _kc_kstrtol_from_user(s, c, b, r)
+static inline int _kc_kstrtol_from_user(const char __user *s, size_t count, 
+					unsigned int base, long *res)
+{
+	/* sign, base 2 representation, newline, terminator */
+	char buf[1 + sizeof(long) * 8 + 1 + 1];
+
+	count = min(count, sizeof(buf) - 1);
+	if (copy_from_user(buf, s, count))
+		return -EFAULT;
+	buf[count] = '\0';
+	return strict_strtol(buf, base, res);
+}
+#endif
+#endif /* < 3.0.0 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,1,0) )
+#ifndef __netdev_alloc_skb_ip_align
+#define __netdev_alloc_skb_ip_align(d,l,_g) netdev_alloc_skb_ip_align(d,l)
+#endif /* __netdev_alloc_skb_ip_align */
+#define dcb_ieee_setapp(dev, app) dcb_setapp(dev, app)
+#define dcb_ieee_delapp(dev, app) 0
+#define dcb_ieee_getapp_mask(dev, app) (1 << app->priority)
+
+/* 1000BASE-T Control register */
+#define CTL1000_AS_MASTER	0x0800
+#define CTL1000_ENABLE_MASTER	0x1000
+
+#else /* < 3.1.0 */
+#ifndef HAVE_DCBNL_IEEE_DELAPP
+#define HAVE_DCBNL_IEEE_DELAPP
+#endif
+#ifdef CONFIG_MACH_APALIS_T30
+#define HAVE_ETHTOOL_GET_TS_INFO
+#endif
+#endif /* < 3.1.0 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,2,0) )
+#ifndef dma_zalloc_coherent
+#define dma_zalloc_coherent(d, s, h, f) _kc_dma_zalloc_coherent(d, s, h, f)
+static inline void *_kc_dma_zalloc_coherent(struct device *dev, size_t size,
+					    dma_addr_t *dma_handle, gfp_t flag)
+{
+	void *ret = dma_alloc_coherent(dev, size, dma_handle, flag);
+	if (ret)
+		memset(ret, 0, size);
+	return ret;
+}
+#endif
+#ifdef ETHTOOL_GRXRINGS
+#define HAVE_ETHTOOL_GET_RXNFC_VOID_RULE_LOCS
+#endif /* ETHTOOL_GRXRINGS */
+
+#ifndef skb_frag_size
+#define skb_frag_size(frag)	_kc_skb_frag_size(frag)
+static inline unsigned int _kc_skb_frag_size(const skb_frag_t *frag)
+{
+	return frag->size;
+}
+#endif /* skb_frag_size */
+
+#ifndef skb_frag_size_sub
+#define skb_frag_size_sub(frag, delta)	_kc_skb_frag_size_sub(frag, delta)
+static inline void _kc_skb_frag_size_sub(skb_frag_t *frag, int delta)
+{
+	frag->size -= delta;
+}
+#endif /* skb_frag_size_sub */
+
+#ifndef skb_frag_page
+#define skb_frag_page(frag)	_kc_skb_frag_page(frag)
+static inline struct page *_kc_skb_frag_page(const skb_frag_t *frag)
+{
+	return frag->page;
+}
+#endif /* skb_frag_page */
+
+#ifndef skb_frag_address
+#define skb_frag_address(frag)	_kc_skb_frag_address(frag)
+static inline void *_kc_skb_frag_address(const skb_frag_t *frag)
+{
+	return page_address(skb_frag_page(frag)) + frag->page_offset;
+}
+#endif /* skb_frag_address */
+
+#ifndef skb_frag_dma_map
+#if ( LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0) )
+#include <linux/dma-mapping.h>
+#endif
+#define skb_frag_dma_map(dev,frag,offset,size,dir) \
+		_kc_skb_frag_dma_map(dev,frag,offset,size,dir)
+static inline dma_addr_t _kc_skb_frag_dma_map(struct device *dev,
+					      const skb_frag_t *frag,
+					      size_t offset, size_t size,
+					      enum dma_data_direction dir)
+{
+	return dma_map_page(dev, skb_frag_page(frag),
+			    frag->page_offset + offset, size, dir);
+}
+#endif /* skb_frag_dma_map */
+
+#ifndef __skb_frag_unref
+#define __skb_frag_unref(frag) __kc_skb_frag_unref(frag)
+static inline void __kc_skb_frag_unref(skb_frag_t *frag)
+{
+	put_page(skb_frag_page(frag));
+}
+#endif /* __skb_frag_unref */
+
+#ifndef SPEED_UNKNOWN
+#define SPEED_UNKNOWN	-1
+#endif
+#ifndef DUPLEX_UNKNOWN
+#define DUPLEX_UNKNOWN	0xff
+#endif
+#if (RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(6,3))
+#ifndef HAVE_PCI_DEV_FLAGS_ASSIGNED
+#define HAVE_PCI_DEV_FLAGS_ASSIGNED
+#endif
+#endif
+#else /* < 3.2.0 */
+#ifndef HAVE_PCI_DEV_FLAGS_ASSIGNED
+#define HAVE_PCI_DEV_FLAGS_ASSIGNED
+#define HAVE_VF_SPOOFCHK_CONFIGURE
+#endif
+#endif /* < 3.2.0 */
+
+#if (RHEL_RELEASE_CODE && RHEL_RELEASE_CODE == RHEL_RELEASE_VERSION(6,2))
+#undef ixgbe_get_netdev_tc_txq
+#define ixgbe_get_netdev_tc_txq(dev, tc) (&netdev_extended(dev)->qos_data.tc_to_txq[tc])
+#endif
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,3,0) )
+#if !(RHEL_RELEASE_CODE >= RHEL_RELEASE_VERSION(6,5))
+typedef u32 netdev_features_t;
+#endif
+#undef PCI_EXP_TYPE_RC_EC
+#define  PCI_EXP_TYPE_RC_EC	0xa	/* Root Complex Event Collector */
+#ifndef CONFIG_BQL
+#define netdev_tx_completed_queue(_q, _p, _b) do {} while (0)
+#define netdev_completed_queue(_n, _p, _b) do {} while (0)
+#define netdev_tx_sent_queue(_q, _b) do {} while (0)
+#define netdev_sent_queue(_n, _b) do {} while (0)
+#define netdev_tx_reset_queue(_q) do {} while (0)
+#define netdev_reset_queue(_n) do {} while (0)
+#endif
+#else /* ! < 3.3.0 */
+#define HAVE_INT_NDO_VLAN_RX_ADD_VID
+#ifdef ETHTOOL_SRXNTUPLE
+#undef ETHTOOL_SRXNTUPLE
+#endif
+#endif /* < 3.3.0 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,4,0) )
+#ifndef NETIF_F_RXFCS
+#define NETIF_F_RXFCS	0
+#endif /* NETIF_F_RXFCS */
+#ifndef NETIF_F_RXALL
+#define NETIF_F_RXALL	0
+#endif /* NETIF_F_RXALL */
+
+#if !(SLE_VERSION_CODE && SLE_VERSION_CODE >= SLE_VERSION(11,3,0))
+#define NUMTCS_RETURNS_U8
+
+int _kc_simple_open(struct inode *inode, struct file *file);
+#define simple_open _kc_simple_open
+#endif /* !(SLE_VERSION_CODE && SLE_VERSION_CODE >= SLE_VERSION(11,3,0)) */
+
+
+#ifndef skb_add_rx_frag
+#define skb_add_rx_frag _kc_skb_add_rx_frag
+extern void _kc_skb_add_rx_frag(struct sk_buff *, int, struct page *,
+				int, int, unsigned int);
+#endif
+#ifdef NET_ADDR_RANDOM
+#define eth_hw_addr_random(N) do { \
+	eth_random_addr(N->dev_addr); \
+	N->addr_assign_type |= NET_ADDR_RANDOM; \
+	} while (0)
+#else /* NET_ADDR_RANDOM */
+#define eth_hw_addr_random(N) eth_random_addr(N->dev_addr)
+#endif /* NET_ADDR_RANDOM */
+#else /* < 3.4.0 */
+#include <linux/kconfig.h>
+#endif /* >= 3.4.0 */
+
+/*****************************************************************************/
+#if defined(E1000E_PTP) || defined(CONFIG_IGB_PTP) || defined(IXGBE_PTP) || defined(I40E_PTP)
+#if ( ( LINUX_VERSION_CODE >= KERNEL_VERSION(3,0,0) ) || \
+     ( RHEL_RELEASE_CODE && ( RHEL_RELEASE_CODE > RHEL_RELEASE_VERSION(6,4) ) ) ) && \
+    IS_ENABLED(CONFIG_PTP_1588_CLOCK)
+#define HAVE_PTP_1588_CLOCK
+#else
+#error Cannot enable PTP Hardware Clock support due to a pre-3.0 kernel version or CONFIG_PTP_1588_CLOCK not enabled in the kernel
+#endif /* > 3.0.0 && IS_ENABLED(CONFIG_PTP_1588_CLOCK) */
+#endif /* E1000E_PTP || CONFIG_IGB_PTP || IXGBE_PTP || I40E_PTP */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,5,0) )
+#define skb_tx_timestamp(skb) do {} while (0)
+
+#ifndef ether_addr_equal
+static inline bool __kc_ether_addr_equal(const u8 *addr1, const u8 *addr2)
+{
+	return !compare_ether_addr(addr1, addr2);
+}
+#define ether_addr_equal(_addr1, _addr2) __kc_ether_addr_equal((_addr1),(_addr2))
+#endif
+
+#else
+#define HAVE_FDB_OPS
+#define HAVE_ETHTOOL_GET_TS_INFO
+#endif /* < 3.5.0 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,6,0) )
+#define PCI_EXP_LNKCAP2		44	/* Link Capability 2 */
+
+#ifndef MDIO_EEE_100TX
+#define MDIO_EEE_100TX		0x0002	/* 100TX EEE cap */
+#endif
+#ifndef MDIO_EEE_1000T
+#define MDIO_EEE_1000T		0x0004	/* 1000T EEE cap */
+#endif
+#ifndef MDIO_EEE_10GT
+#define MDIO_EEE_10GT		0x0008	/* 10GT EEE cap */
+#endif
+#ifndef MDIO_EEE_1000KX
+#define MDIO_EEE_1000KX		0x0010	/* 1000KX EEE cap */
+#endif
+#ifndef MDIO_EEE_10GKX4
+#define MDIO_EEE_10GKX4		0x0020	/* 10G KX4 EEE cap */
+#endif
+#ifndef MDIO_EEE_10GKR
+#define MDIO_EEE_10GKR		0x0040	/* 10G KR EEE cap */
+#endif
+
+#ifndef eth_random_addr
+#define eth_random_addr _kc_eth_random_addr
+static inline void _kc_eth_random_addr(u8 *addr)
+{
+        get_random_bytes(addr, ETH_ALEN);
+        addr[0] &= 0xfe; /* clear multicast */
+        addr[0] |= 0x02; /* set local assignment */
+}
+#endif
+#endif /* < 3.6.0 */
+
+/******************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0) )
+#ifndef ADVERTISED_40000baseKR4_Full
+/* these defines were all added in one commit, so should be safe
+ * to trigger activiation on one define
+ */
+#define SUPPORTED_40000baseKR4_Full	(1 << 23)
+#define SUPPORTED_40000baseCR4_Full	(1 << 24)
+#define SUPPORTED_40000baseSR4_Full	(1 << 25)
+#define SUPPORTED_40000baseLR4_Full	(1 << 26)
+#define ADVERTISED_40000baseKR4_Full	(1 << 23)
+#define ADVERTISED_40000baseCR4_Full	(1 << 24)
+#define ADVERTISED_40000baseSR4_Full	(1 << 25)
+#define ADVERTISED_40000baseLR4_Full	(1 << 26)
+#endif
+
+#ifndef mmd_eee_cap_to_ethtool_sup_t
+/**
+ * mmd_eee_cap_to_ethtool_sup_t
+ * @eee_cap: value of the MMD EEE Capability register
+ *
+ * A small helper function that translates MMD EEE Capability (3.20) bits
+ * to ethtool supported settings.
+ */
+static inline u32 __kc_mmd_eee_cap_to_ethtool_sup_t(u16 eee_cap)
+{
+	u32 supported = 0;
+
+	if (eee_cap & MDIO_EEE_100TX)
+		supported |= SUPPORTED_100baseT_Full;
+	if (eee_cap & MDIO_EEE_1000T)
+		supported |= SUPPORTED_1000baseT_Full;
+	if (eee_cap & MDIO_EEE_10GT)
+		supported |= SUPPORTED_10000baseT_Full;
+	if (eee_cap & MDIO_EEE_1000KX)
+		supported |= SUPPORTED_1000baseKX_Full;
+	if (eee_cap & MDIO_EEE_10GKX4)
+		supported |= SUPPORTED_10000baseKX4_Full;
+	if (eee_cap & MDIO_EEE_10GKR)
+		supported |= SUPPORTED_10000baseKR_Full;
+
+	return supported;
+}
+#define mmd_eee_cap_to_ethtool_sup_t(eee_cap) \
+	__kc_mmd_eee_cap_to_ethtool_sup_t(eee_cap)
+#endif /* mmd_eee_cap_to_ethtool_sup_t */
+
+#ifndef mmd_eee_adv_to_ethtool_adv_t
+/**
+ * mmd_eee_adv_to_ethtool_adv_t
+ * @eee_adv: value of the MMD EEE Advertisement/Link Partner Ability registers
+ *
+ * A small helper function that translates the MMD EEE Advertisment (7.60)
+ * and MMD EEE Link Partner Ability (7.61) bits to ethtool advertisement
+ * settings.
+ */
+static inline u32 __kc_mmd_eee_adv_to_ethtool_adv_t(u16 eee_adv)
+{
+	u32 adv = 0;
+
+	if (eee_adv & MDIO_EEE_100TX)
+		adv |= ADVERTISED_100baseT_Full;
+	if (eee_adv & MDIO_EEE_1000T)
+		adv |= ADVERTISED_1000baseT_Full;
+	if (eee_adv & MDIO_EEE_10GT)
+		adv |= ADVERTISED_10000baseT_Full;
+	if (eee_adv & MDIO_EEE_1000KX)
+		adv |= ADVERTISED_1000baseKX_Full;
+	if (eee_adv & MDIO_EEE_10GKX4)
+		adv |= ADVERTISED_10000baseKX4_Full;
+	if (eee_adv & MDIO_EEE_10GKR)
+		adv |= ADVERTISED_10000baseKR_Full;
+
+	return adv;
+}
+
+#define mmd_eee_adv_to_ethtool_adv_t(eee_adv) \
+	__kc_mmd_eee_adv_to_ethtool_adv_t(eee_adv)
+#endif /* mmd_eee_adv_to_ethtool_adv_t */
+
+#ifndef ethtool_adv_to_mmd_eee_adv_t
+/**
+ * ethtool_adv_to_mmd_eee_adv_t
+ * @adv: the ethtool advertisement settings
+ *
+ * A small helper function that translates ethtool advertisement settings
+ * to EEE advertisements for the MMD EEE Advertisement (7.60) and
+ * MMD EEE Link Partner Ability (7.61) registers.
+ */
+static inline u16 __kc_ethtool_adv_to_mmd_eee_adv_t(u32 adv)
+{
+	u16 reg = 0;
+
+	if (adv & ADVERTISED_100baseT_Full)
+		reg |= MDIO_EEE_100TX;
+	if (adv & ADVERTISED_1000baseT_Full)
+		reg |= MDIO_EEE_1000T;
+	if (adv & ADVERTISED_10000baseT_Full)
+		reg |= MDIO_EEE_10GT;
+	if (adv & ADVERTISED_1000baseKX_Full)
+		reg |= MDIO_EEE_1000KX;
+	if (adv & ADVERTISED_10000baseKX4_Full)
+		reg |= MDIO_EEE_10GKX4;
+	if (adv & ADVERTISED_10000baseKR_Full)
+		reg |= MDIO_EEE_10GKR;
+
+	return reg;
+}
+#define ethtool_adv_to_mmd_eee_adv_t(adv) __kc_ethtool_adv_to_mmd_eee_adv_t(adv)
+#endif /* ethtool_adv_to_mmd_eee_adv_t */
+
+#ifndef pci_pcie_type
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24) )
+static inline u8 pci_pcie_type(struct pci_dev *pdev)
+{
+	int pos;
+	u16 reg16;
+
+	pos = pci_find_capability(pdev, PCI_CAP_ID_EXP);
+	if (!pos)
+		BUG();
+	pci_read_config_word(pdev, pos + PCI_EXP_FLAGS, &reg16);
+	return (reg16 & PCI_EXP_FLAGS_TYPE) >> 4;
+}
+#else /* < 2.6.24 */
+#define pci_pcie_type(x)	(x)->pcie_type
+#endif /* < 2.6.24 */
+#endif /* pci_pcie_type */
+
+#define ptp_clock_register(caps, args...) ptp_clock_register(caps)
+
+#ifndef pcie_capability_read_word
+int __kc_pcie_capability_read_word(struct pci_dev *dev, int pos, u16 *val);
+#define pcie_capability_read_word(d,p,v) __kc_pcie_capability_read_word(d,p,v)
+#endif /* pcie_capability_read_word */
+
+#ifndef pcie_capability_write_word
+int __kc_pcie_capability_write_word(struct pci_dev *dev, int pos, u16 val);
+#define pcie_capability_write_word(d,p,v) __kc_pcie_capability_write_word(d,p,v)
+#endif /* pcie_capability_write_word */
+
+#ifndef pcie_capability_clear_and_set_word
+int __kc_pcie_capability_clear_and_set_word(struct pci_dev *dev, int pos,
+					    u16 clear, u16 set);
+#define pcie_capability_clear_and_set_word(d,p,c,s) \
+	__kc_pcie_capability_clear_and_set_word(d,p,c,s)
+#endif /* pcie_capability_clear_and_set_word */
+
+#ifndef pcie_capability_clear_word
+int __kc_pcie_capability_clear_word(struct pci_dev *dev, int pos,
+					     u16 clear);
+#define pcie_capability_clear_word(d, p, c) \
+	__kc_pcie_capability_clear_word(d, p, c)
+#endif /* pcie_capability_clear_word */
+
+#ifndef PCI_EXP_LNKSTA2
+#define PCI_EXP_LNKSTA2		50	/* Link Status 2 */
+#endif
+
+#if (SLE_VERSION_CODE && SLE_VERSION_CODE >= SLE_VERSION(11,3,0))
+#define USE_CONST_DEV_UC_CHAR
+#endif
+
+#else /* >= 3.7.0 */
+#define HAVE_CONST_STRUCT_PCI_ERROR_HANDLERS
+#define USE_CONST_DEV_UC_CHAR
+#endif /* >= 3.7.0 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,8,0) )
+#ifndef PCI_EXP_LNKCTL_ASPM_L0S
+#define  PCI_EXP_LNKCTL_ASPM_L0S  0x01	/* L0s Enable */
+#endif
+#ifndef PCI_EXP_LNKCTL_ASPM_L1
+#define  PCI_EXP_LNKCTL_ASPM_L1   0x02	/* L1 Enable */
+#endif
+#define HAVE_CONFIG_HOTPLUG
+/* Reserved Ethernet Addresses per IEEE 802.1Q */
+static const u8 eth_reserved_addr_base[ETH_ALEN] __aligned(2) = {
+	0x01, 0x80, 0xc2, 0x00, 0x00, 0x00 };
+
+#ifndef is_link_local_ether_addr
+static inline bool __kc_is_link_local_ether_addr(const u8 *addr)
+{
+	__be16 *a = (__be16 *)addr;
+	static const __be16 *b = (const __be16 *)eth_reserved_addr_base;
+	static const __be16 m = cpu_to_be16(0xfff0);
+
+	return ((a[0] ^ b[0]) | (a[1] ^ b[1]) | ((a[2] ^ b[2]) & m)) == 0;
+}
+#define is_link_local_ether_addr(addr) __kc_is_link_local_ether_addr(addr)
+#endif /* is_link_local_ether_addr */
+#else /* >= 3.8.0 */
+#ifndef __devinit
+#define __devinit
+#define HAVE_ENCAP_CSUM_OFFLOAD
+#endif
+
+#ifndef __devinitdata
+#define __devinitdata
+#endif
+
+#ifndef __devexit
+#define __devexit
+#endif
+
+#ifndef __devexit_p
+#define __devexit_p
+#endif
+
+#ifndef HAVE_SRIOV_CONFIGURE
+#define HAVE_SRIOV_CONFIGURE
+#endif
+
+#define HAVE_BRIDGE_ATTRIBS
+#ifndef BRIDGE_MODE_VEB
+#define BRIDGE_MODE_VEB		0	/* Default loopback mode */
+#endif /* BRIDGE_MODE_VEB */
+#ifndef BRIDGE_MODE_VEPA
+#define BRIDGE_MODE_VEPA	1	/* 802.1Qbg defined VEPA mode */
+#endif /* BRIDGE_MODE_VEPA */
+#endif /* >= 3.8.0 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,9,0) )
+
+#undef hlist_entry
+#define hlist_entry(ptr, type, member) container_of(ptr,type,member)
+
+#undef hlist_entry_safe
+#define hlist_entry_safe(ptr, type, member) \
+	(ptr) ? hlist_entry(ptr, type, member) : NULL
+
+#undef hlist_for_each_entry
+#define hlist_for_each_entry(pos, head, member)                             \
+	for (pos = hlist_entry_safe((head)->first, typeof(*(pos)), member); \
+	     pos;                                                           \
+	     pos = hlist_entry_safe((pos)->member.next, typeof(*(pos)), member))
+
+#undef hlist_for_each_entry_safe
+#define hlist_for_each_entry_safe(pos, n, head, member) 		    \
+	for (pos = hlist_entry_safe((head)->first, typeof(*pos), member);   \
+	     pos && ({ n = pos->member.next; 1; });			    \
+	     pos = hlist_entry_safe(n, typeof(*pos), member))
+
+#ifdef CONFIG_XPS
+extern int __kc_netif_set_xps_queue(struct net_device *, struct cpumask *, u16);
+#define netif_set_xps_queue(_dev, _mask, _idx) __kc_netif_set_xps_queue((_dev), (_mask), (_idx))
+#else /* CONFIG_XPS */
+#define netif_set_xps_queue(_dev, _mask, _idx) do {} while (0)
+#endif /* CONFIG_XPS */
+
+#ifdef HAVE_NETDEV_SELECT_QUEUE
+#define _kc_hashrnd 0xd631614b /* not so random hash salt */
+extern u16 __kc_netdev_pick_tx(struct net_device *dev, struct sk_buff *skb);
+#define __netdev_pick_tx __kc_netdev_pick_tx
+#endif /* HAVE_NETDEV_SELECT_QUEUE */
+#else
+#define HAVE_BRIDGE_FILTER
+#define USE_DEFAULT_FDB_DEL_DUMP
+#endif /* < 3.9.0 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,10,0) )
+#ifdef CONFIG_PCI_IOV
+extern int __kc_pci_vfs_assigned(struct pci_dev *dev);
+#else
+static inline int __kc_pci_vfs_assigned(struct pci_dev *dev)
+{
+	return 0;
+}
+#endif
+#define pci_vfs_assigned(dev) __kc_pci_vfs_assigned(dev)
+
+#ifndef list_first_entry_or_null
+#define list_first_entry_or_null(ptr, type, member) \
+	(!list_empty(ptr) ? list_first_entry(ptr, type, member) : NULL)
+#endif
+
+#ifndef VLAN_TX_COOKIE_MAGIC
+static inline struct sk_buff *__kc__vlan_hwaccel_put_tag(struct sk_buff *skb,
+							 u16 vlan_tci)
+{
+#ifdef VLAN_TAG_PRESENT
+	vlan_tci |= VLAN_TAG_PRESENT;
+#endif
+	skb->vlan_tci = vlan_tci;
+        return skb;
+}
+#define __vlan_hwaccel_put_tag(skb, vlan_proto, vlan_tci) \
+	__kc__vlan_hwaccel_put_tag(skb, vlan_tci)
+#endif
+
+#else /* >= 3.10.0 */
+#define HAVE_ENCAP_TSO_OFFLOAD
+#endif /* >= 3.10.0 */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,12,0) )
+
+#else /* >= 3.12.0 */
+#define HAVE_VXLAN_RX_OFFLOAD
+#endif /* >= 3.12.0 */
+
+#if (SLE_VERSION_CODE && SLE_VERSION_CODE == SLE_VERSION(11,2,0))
+#undef ETHTOOL_GRXFHINDIR
+#undef ETHTOOL_SRXFHINDIR
+#endif /* SLES (11,2,0) */
+
+/*****************************************************************************/
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(3,13,0) )
+
+#else /* >= 3.13.0 */
+#define HAVE_VXLAN_CHECKS
+#endif
+
+#endif /* _KCOMPAT_H_ */
diff --git a/drivers/net/igb/kcompat_ethtool.c b/drivers/net/igb/kcompat_ethtool.c
new file mode 100644
index 000000000000..3adf8696d6b5
--- /dev/null
+++ b/drivers/net/igb/kcompat_ethtool.c
@@ -0,0 +1,1172 @@
+/*******************************************************************************
+
+  Intel(R) Gigabit Ethernet Linux driver
+  Copyright(c) 2007-2013 Intel Corporation.
+
+  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 it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc.,
+  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+
+  The full GNU General Public License is included in this distribution in
+  the file called "COPYING".
+
+  Contact Information:
+  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+/*
+ * net/core/ethtool.c - Ethtool ioctl handler
+ * Copyright (c) 2003 Matthew Wilcox <matthew@wil.cx>
+ *
+ * This file is where we call all the ethtool_ops commands to get
+ * the information ethtool needs.  We fall back to calling do_ioctl()
+ * for drivers which haven't been converted to ethtool_ops yet.
+ *
+ * It's GPL, stupid.
+ *
+ * Modification by sfeldma@pobox.com to work as backward compat
+ * solution for pre-ethtool_ops kernels.
+ * 	- copied struct ethtool_ops from ethtool.h
+ * 	- defined SET_ETHTOOL_OPS
+ * 	- put in some #ifndef NETIF_F_xxx wrappers
+ * 	- changes refs to dev->ethtool_ops to ethtool_ops
+ * 	- changed dev_ethtool to ethtool_ioctl
+ *      - remove EXPORT_SYMBOL()s
+ *      - added _kc_ prefix in built-in ethtool_op_xxx ops.
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/mii.h>
+#include <linux/ethtool.h>
+#include <linux/netdevice.h>
+#include <asm/uaccess.h>
+
+#include "kcompat.h"
+
+#undef SUPPORTED_10000baseT_Full
+#define SUPPORTED_10000baseT_Full	(1 << 12)
+#undef ADVERTISED_10000baseT_Full
+#define ADVERTISED_10000baseT_Full	(1 << 12)
+#undef SPEED_10000
+#define SPEED_10000		10000
+
+#undef ethtool_ops
+#define ethtool_ops _kc_ethtool_ops
+
+struct _kc_ethtool_ops {
+	int  (*get_settings)(struct net_device *, struct ethtool_cmd *);
+	int  (*set_settings)(struct net_device *, struct ethtool_cmd *);
+	void (*get_drvinfo)(struct net_device *, struct ethtool_drvinfo *);
+	int  (*get_regs_len)(struct net_device *);
+	void (*get_regs)(struct net_device *, struct ethtool_regs *, void *);
+	void (*get_wol)(struct net_device *, struct ethtool_wolinfo *);
+	int  (*set_wol)(struct net_device *, struct ethtool_wolinfo *);
+	u32  (*get_msglevel)(struct net_device *);
+	void (*set_msglevel)(struct net_device *, u32);
+	int  (*nway_reset)(struct net_device *);
+	u32  (*get_link)(struct net_device *);
+	int  (*get_eeprom_len)(struct net_device *);
+	int  (*get_eeprom)(struct net_device *, struct ethtool_eeprom *, u8 *);
+	int  (*set_eeprom)(struct net_device *, struct ethtool_eeprom *, u8 *);
+	int  (*get_coalesce)(struct net_device *, struct ethtool_coalesce *);
+	int  (*set_coalesce)(struct net_device *, struct ethtool_coalesce *);
+	void (*get_ringparam)(struct net_device *, struct ethtool_ringparam *);
+	int  (*set_ringparam)(struct net_device *, struct ethtool_ringparam *);
+	void (*get_pauseparam)(struct net_device *,
+	                       struct ethtool_pauseparam*);
+	int  (*set_pauseparam)(struct net_device *,
+	                       struct ethtool_pauseparam*);
+	u32  (*get_rx_csum)(struct net_device *);
+	int  (*set_rx_csum)(struct net_device *, u32);
+	u32  (*get_tx_csum)(struct net_device *);
+	int  (*set_tx_csum)(struct net_device *, u32);
+	u32  (*get_sg)(struct net_device *);
+	int  (*set_sg)(struct net_device *, u32);
+	u32  (*get_tso)(struct net_device *);
+	int  (*set_tso)(struct net_device *, u32);
+	int  (*self_test_count)(struct net_device *);
+	void (*self_test)(struct net_device *, struct ethtool_test *, u64 *);
+	void (*get_strings)(struct net_device *, u32 stringset, u8 *);
+	int  (*phys_id)(struct net_device *, u32);
+	int  (*get_stats_count)(struct net_device *);
+	void (*get_ethtool_stats)(struct net_device *, struct ethtool_stats *,
+	                          u64 *);
+} *ethtool_ops = NULL;
+
+#undef SET_ETHTOOL_OPS
+#define SET_ETHTOOL_OPS(netdev, ops) (ethtool_ops = (ops))
+
+/*
+ * Some useful ethtool_ops methods that are device independent. If we find that
+ * all drivers want to do the same thing here, we can turn these into dev_()
+ * function calls.
+ */
+
+#undef ethtool_op_get_link
+#define ethtool_op_get_link _kc_ethtool_op_get_link
+u32 _kc_ethtool_op_get_link(struct net_device *dev)
+{
+	return netif_carrier_ok(dev) ? 1 : 0;
+}
+
+#undef ethtool_op_get_tx_csum
+#define ethtool_op_get_tx_csum _kc_ethtool_op_get_tx_csum
+u32 _kc_ethtool_op_get_tx_csum(struct net_device *dev)
+{
+#ifdef NETIF_F_IP_CSUM
+	return (dev->features & NETIF_F_IP_CSUM) != 0;
+#else
+	return 0;
+#endif
+}
+
+#undef ethtool_op_set_tx_csum
+#define ethtool_op_set_tx_csum _kc_ethtool_op_set_tx_csum
+int _kc_ethtool_op_set_tx_csum(struct net_device *dev, u32 data)
+{
+#ifdef NETIF_F_IP_CSUM
+	if (data)
+#ifdef NETIF_F_IPV6_CSUM
+		dev->features |= (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM);
+	else
+		dev->features &= ~(NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM);
+#else
+		dev->features |= NETIF_F_IP_CSUM;
+	else
+		dev->features &= ~NETIF_F_IP_CSUM;
+#endif
+#endif
+
+	return 0;
+}
+
+#undef ethtool_op_get_sg
+#define ethtool_op_get_sg _kc_ethtool_op_get_sg
+u32 _kc_ethtool_op_get_sg(struct net_device *dev)
+{
+#ifdef NETIF_F_SG
+	return (dev->features & NETIF_F_SG) != 0;
+#else
+	return 0;
+#endif
+}
+
+#undef ethtool_op_set_sg
+#define ethtool_op_set_sg _kc_ethtool_op_set_sg
+int _kc_ethtool_op_set_sg(struct net_device *dev, u32 data)
+{
+#ifdef NETIF_F_SG
+	if (data)
+		dev->features |= NETIF_F_SG;
+	else
+		dev->features &= ~NETIF_F_SG;
+#endif
+
+	return 0;
+}
+
+#undef ethtool_op_get_tso
+#define ethtool_op_get_tso _kc_ethtool_op_get_tso
+u32 _kc_ethtool_op_get_tso(struct net_device *dev)
+{
+#ifdef NETIF_F_TSO
+	return (dev->features & NETIF_F_TSO) != 0;
+#else
+	return 0;
+#endif
+}
+
+#undef ethtool_op_set_tso
+#define ethtool_op_set_tso _kc_ethtool_op_set_tso
+int _kc_ethtool_op_set_tso(struct net_device *dev, u32 data)
+{
+#ifdef NETIF_F_TSO
+	if (data)
+		dev->features |= NETIF_F_TSO;
+	else
+		dev->features &= ~NETIF_F_TSO;
+#endif
+
+	return 0;
+}
+
+/* Handlers for each ethtool command */
+
+static int ethtool_get_settings(struct net_device *dev, void *useraddr)
+{
+	struct ethtool_cmd cmd = { ETHTOOL_GSET };
+	int err;
+
+	if (!ethtool_ops->get_settings)
+		return -EOPNOTSUPP;
+
+	err = ethtool_ops->get_settings(dev, &cmd);
+	if (err < 0)
+		return err;
+
+	if (copy_to_user(useraddr, &cmd, sizeof(cmd)))
+		return -EFAULT;
+	return 0;
+}
+
+static int ethtool_set_settings(struct net_device *dev, void *useraddr)
+{
+	struct ethtool_cmd cmd;
+
+	if (!ethtool_ops->set_settings)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&cmd, useraddr, sizeof(cmd)))
+		return -EFAULT;
+
+	return ethtool_ops->set_settings(dev, &cmd);
+}
+
+static int ethtool_get_drvinfo(struct net_device *dev, void *useraddr)
+{
+	struct ethtool_drvinfo info;
+	struct ethtool_ops *ops = ethtool_ops;
+
+	if (!ops->get_drvinfo)
+		return -EOPNOTSUPP;
+
+	memset(&info, 0, sizeof(info));
+	info.cmd = ETHTOOL_GDRVINFO;
+	ops->get_drvinfo(dev, &info);
+
+	if (ops->self_test_count)
+		info.testinfo_len = ops->self_test_count(dev);
+	if (ops->get_stats_count)
+		info.n_stats = ops->get_stats_count(dev);
+	if (ops->get_regs_len)
+		info.regdump_len = ops->get_regs_len(dev);
+	if (ops->get_eeprom_len)
+		info.eedump_len = ops->get_eeprom_len(dev);
+
+	if (copy_to_user(useraddr, &info, sizeof(info)))
+		return -EFAULT;
+	return 0;
+}
+
+static int ethtool_get_regs(struct net_device *dev, char *useraddr)
+{
+	struct ethtool_regs regs;
+	struct ethtool_ops *ops = ethtool_ops;
+	void *regbuf;
+	int reglen, ret;
+
+	if (!ops->get_regs || !ops->get_regs_len)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&regs, useraddr, sizeof(regs)))
+		return -EFAULT;
+
+	reglen = ops->get_regs_len(dev);
+	if (regs.len > reglen)
+		regs.len = reglen;
+
+	regbuf = kmalloc(reglen, GFP_USER);
+	if (!regbuf)
+		return -ENOMEM;
+
+	ops->get_regs(dev, &regs, regbuf);
+
+	ret = -EFAULT;
+	if (copy_to_user(useraddr, &regs, sizeof(regs)))
+		goto out;
+	useraddr += offsetof(struct ethtool_regs, data);
+	if (copy_to_user(useraddr, regbuf, reglen))
+		goto out;
+	ret = 0;
+
+out:
+	kfree(regbuf);
+	return ret;
+}
+
+static int ethtool_get_wol(struct net_device *dev, char *useraddr)
+{
+	struct ethtool_wolinfo wol = { ETHTOOL_GWOL };
+
+	if (!ethtool_ops->get_wol)
+		return -EOPNOTSUPP;
+
+	ethtool_ops->get_wol(dev, &wol);
+
+	if (copy_to_user(useraddr, &wol, sizeof(wol)))
+		return -EFAULT;
+	return 0;
+}
+
+static int ethtool_set_wol(struct net_device *dev, char *useraddr)
+{
+	struct ethtool_wolinfo wol;
+
+	if (!ethtool_ops->set_wol)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&wol, useraddr, sizeof(wol)))
+		return -EFAULT;
+
+	return ethtool_ops->set_wol(dev, &wol);
+}
+
+static int ethtool_get_msglevel(struct net_device *dev, char *useraddr)
+{
+	struct ethtool_value edata = { ETHTOOL_GMSGLVL };
+
+	if (!ethtool_ops->get_msglevel)
+		return -EOPNOTSUPP;
+
+	edata.data = ethtool_ops->get_msglevel(dev);
+
+	if (copy_to_user(useraddr, &edata, sizeof(edata)))
+		return -EFAULT;
+	return 0;
+}
+
+static int ethtool_set_msglevel(struct net_device *dev, char *useraddr)
+{
+	struct ethtool_value edata;
+
+	if (!ethtool_ops->set_msglevel)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&edata, useraddr, sizeof(edata)))
+		return -EFAULT;
+
+	ethtool_ops->set_msglevel(dev, edata.data);
+	return 0;
+}
+
+static int ethtool_nway_reset(struct net_device *dev)
+{
+	if (!ethtool_ops->nway_reset)
+		return -EOPNOTSUPP;
+
+	return ethtool_ops->nway_reset(dev);
+}
+
+static int ethtool_get_link(struct net_device *dev, void *useraddr)
+{
+	struct ethtool_value edata = { ETHTOOL_GLINK };
+
+	if (!ethtool_ops->get_link)
+		return -EOPNOTSUPP;
+
+	edata.data = ethtool_ops->get_link(dev);
+
+	if (copy_to_user(useraddr, &edata, sizeof(edata)))
+		return -EFAULT;
+	return 0;
+}
+
+static int ethtool_get_eeprom(struct net_device *dev, void *useraddr)
+{
+	struct ethtool_eeprom eeprom;
+	struct ethtool_ops *ops = ethtool_ops;
+	u8 *data;
+	int ret;
+
+	if (!ops->get_eeprom || !ops->get_eeprom_len)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&eeprom, useraddr, sizeof(eeprom)))
+		return -EFAULT;
+
+	/* Check for wrap and zero */
+	if (eeprom.offset + eeprom.len <= eeprom.offset)
+		return -EINVAL;
+
+	/* Check for exceeding total eeprom len */
+	if (eeprom.offset + eeprom.len > ops->get_eeprom_len(dev))
+		return -EINVAL;
+
+	data = kmalloc(eeprom.len, GFP_USER);
+	if (!data)
+		return -ENOMEM;
+
+	ret = -EFAULT;
+	if (copy_from_user(data, useraddr + sizeof(eeprom), eeprom.len))
+		goto out;
+
+	ret = ops->get_eeprom(dev, &eeprom, data);
+	if (ret)
+		goto out;
+
+	ret = -EFAULT;
+	if (copy_to_user(useraddr, &eeprom, sizeof(eeprom)))
+		goto out;
+	if (copy_to_user(useraddr + sizeof(eeprom), data, eeprom.len))
+		goto out;
+	ret = 0;
+
+out:
+	kfree(data);
+	return ret;
+}
+
+static int ethtool_set_eeprom(struct net_device *dev, void *useraddr)
+{
+	struct ethtool_eeprom eeprom;
+	struct ethtool_ops *ops = ethtool_ops;
+	u8 *data;
+	int ret;
+
+	if (!ops->set_eeprom || !ops->get_eeprom_len)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&eeprom, useraddr, sizeof(eeprom)))
+		return -EFAULT;
+
+	/* Check for wrap and zero */
+	if (eeprom.offset + eeprom.len <= eeprom.offset)
+		return -EINVAL;
+
+	/* Check for exceeding total eeprom len */
+	if (eeprom.offset + eeprom.len > ops->get_eeprom_len(dev))
+		return -EINVAL;
+
+	data = kmalloc(eeprom.len, GFP_USER);
+	if (!data)
+		return -ENOMEM;
+
+	ret = -EFAULT;
+	if (copy_from_user(data, useraddr + sizeof(eeprom), eeprom.len))
+		goto out;
+
+	ret = ops->set_eeprom(dev, &eeprom, data);
+	if (ret)
+		goto out;
+
+	if (copy_to_user(useraddr + sizeof(eeprom), data, eeprom.len))
+		ret = -EFAULT;
+
+out:
+	kfree(data);
+	return ret;
+}
+
+static int ethtool_get_coalesce(struct net_device *dev, void *useraddr)
+{
+	struct ethtool_coalesce coalesce = { ETHTOOL_GCOALESCE };
+
+	if (!ethtool_ops->get_coalesce)
+		return -EOPNOTSUPP;
+
+	ethtool_ops->get_coalesce(dev, &coalesce);
+
+	if (copy_to_user(useraddr, &coalesce, sizeof(coalesce)))
+		return -EFAULT;
+	return 0;
+}
+
+static int ethtool_set_coalesce(struct net_device *dev, void *useraddr)
+{
+	struct ethtool_coalesce coalesce;
+
+	if (!ethtool_ops->get_coalesce)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&coalesce, useraddr, sizeof(coalesce)))
+		return -EFAULT;
+
+	return ethtool_ops->set_coalesce(dev, &coalesce);
+}
+
+static int ethtool_get_ringparam(struct net_device *dev, void *useraddr)
+{
+	struct ethtool_ringparam ringparam = { ETHTOOL_GRINGPARAM };
+
+	if (!ethtool_ops->get_ringparam)
+		return -EOPNOTSUPP;
+
+	ethtool_ops->get_ringparam(dev, &ringparam);
+
+	if (copy_to_user(useraddr, &ringparam, sizeof(ringparam)))
+		return -EFAULT;
+	return 0;
+}
+
+static int ethtool_set_ringparam(struct net_device *dev, void *useraddr)
+{
+	struct ethtool_ringparam ringparam;
+
+	if (!ethtool_ops->get_ringparam)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&ringparam, useraddr, sizeof(ringparam)))
+		return -EFAULT;
+
+	return ethtool_ops->set_ringparam(dev, &ringparam);
+}
+
+static int ethtool_get_pauseparam(struct net_device *dev, void *useraddr)
+{
+	struct ethtool_pauseparam pauseparam = { ETHTOOL_GPAUSEPARAM };
+
+	if (!ethtool_ops->get_pauseparam)
+		return -EOPNOTSUPP;
+
+	ethtool_ops->get_pauseparam(dev, &pauseparam);
+
+	if (copy_to_user(useraddr, &pauseparam, sizeof(pauseparam)))
+		return -EFAULT;
+	return 0;
+}
+
+static int ethtool_set_pauseparam(struct net_device *dev, void *useraddr)
+{
+	struct ethtool_pauseparam pauseparam;
+
+	if (!ethtool_ops->get_pauseparam)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&pauseparam, useraddr, sizeof(pauseparam)))
+		return -EFAULT;
+
+	return ethtool_ops->set_pauseparam(dev, &pauseparam);
+}
+
+static int ethtool_get_rx_csum(struct net_device *dev, char *useraddr)
+{
+	struct ethtool_value edata = { ETHTOOL_GRXCSUM };
+
+	if (!ethtool_ops->get_rx_csum)
+		return -EOPNOTSUPP;
+
+	edata.data = ethtool_ops->get_rx_csum(dev);
+
+	if (copy_to_user(useraddr, &edata, sizeof(edata)))
+		return -EFAULT;
+	return 0;
+}
+
+static int ethtool_set_rx_csum(struct net_device *dev, char *useraddr)
+{
+	struct ethtool_value edata;
+
+	if (!ethtool_ops->set_rx_csum)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&edata, useraddr, sizeof(edata)))
+		return -EFAULT;
+
+	ethtool_ops->set_rx_csum(dev, edata.data);
+	return 0;
+}
+
+static int ethtool_get_tx_csum(struct net_device *dev, char *useraddr)
+{
+	struct ethtool_value edata = { ETHTOOL_GTXCSUM };
+
+	if (!ethtool_ops->get_tx_csum)
+		return -EOPNOTSUPP;
+
+	edata.data = ethtool_ops->get_tx_csum(dev);
+
+	if (copy_to_user(useraddr, &edata, sizeof(edata)))
+		return -EFAULT;
+	return 0;
+}
+
+static int ethtool_set_tx_csum(struct net_device *dev, char *useraddr)
+{
+	struct ethtool_value edata;
+
+	if (!ethtool_ops->set_tx_csum)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&edata, useraddr, sizeof(edata)))
+		return -EFAULT;
+
+	return ethtool_ops->set_tx_csum(dev, edata.data);
+}
+
+static int ethtool_get_sg(struct net_device *dev, char *useraddr)
+{
+	struct ethtool_value edata = { ETHTOOL_GSG };
+
+	if (!ethtool_ops->get_sg)
+		return -EOPNOTSUPP;
+
+	edata.data = ethtool_ops->get_sg(dev);
+
+	if (copy_to_user(useraddr, &edata, sizeof(edata)))
+		return -EFAULT;
+	return 0;
+}
+
+static int ethtool_set_sg(struct net_device *dev, char *useraddr)
+{
+	struct ethtool_value edata;
+
+	if (!ethtool_ops->set_sg)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&edata, useraddr, sizeof(edata)))
+		return -EFAULT;
+
+	return ethtool_ops->set_sg(dev, edata.data);
+}
+
+static int ethtool_get_tso(struct net_device *dev, char *useraddr)
+{
+	struct ethtool_value edata = { ETHTOOL_GTSO };
+
+	if (!ethtool_ops->get_tso)
+		return -EOPNOTSUPP;
+
+	edata.data = ethtool_ops->get_tso(dev);
+
+	if (copy_to_user(useraddr, &edata, sizeof(edata)))
+		return -EFAULT;
+	return 0;
+}
+
+static int ethtool_set_tso(struct net_device *dev, char *useraddr)
+{
+	struct ethtool_value edata;
+
+	if (!ethtool_ops->set_tso)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&edata, useraddr, sizeof(edata)))
+		return -EFAULT;
+
+	return ethtool_ops->set_tso(dev, edata.data);
+}
+
+static int ethtool_self_test(struct net_device *dev, char *useraddr)
+{
+	struct ethtool_test test;
+	struct ethtool_ops *ops = ethtool_ops;
+	u64 *data;
+	int ret;
+
+	if (!ops->self_test || !ops->self_test_count)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&test, useraddr, sizeof(test)))
+		return -EFAULT;
+
+	test.len = ops->self_test_count(dev);
+	data = kmalloc(test.len * sizeof(u64), GFP_USER);
+	if (!data)
+		return -ENOMEM;
+
+	ops->self_test(dev, &test, data);
+
+	ret = -EFAULT;
+	if (copy_to_user(useraddr, &test, sizeof(test)))
+		goto out;
+	useraddr += sizeof(test);
+	if (copy_to_user(useraddr, data, test.len * sizeof(u64)))
+		goto out;
+	ret = 0;
+
+out:
+	kfree(data);
+	return ret;
+}
+
+static int ethtool_get_strings(struct net_device *dev, void *useraddr)
+{
+	struct ethtool_gstrings gstrings;
+	struct ethtool_ops *ops = ethtool_ops;
+	u8 *data;
+	int ret;
+
+	if (!ops->get_strings)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&gstrings, useraddr, sizeof(gstrings)))
+		return -EFAULT;
+
+	switch (gstrings.string_set) {
+	case ETH_SS_TEST:
+		if (!ops->self_test_count)
+			return -EOPNOTSUPP;
+		gstrings.len = ops->self_test_count(dev);
+		break;
+	case ETH_SS_STATS:
+		if (!ops->get_stats_count)
+			return -EOPNOTSUPP;
+		gstrings.len = ops->get_stats_count(dev);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	data = kmalloc(gstrings.len * ETH_GSTRING_LEN, GFP_USER);
+	if (!data)
+		return -ENOMEM;
+
+	ops->get_strings(dev, gstrings.string_set, data);
+
+	ret = -EFAULT;
+	if (copy_to_user(useraddr, &gstrings, sizeof(gstrings)))
+		goto out;
+	useraddr += sizeof(gstrings);
+	if (copy_to_user(useraddr, data, gstrings.len * ETH_GSTRING_LEN))
+		goto out;
+	ret = 0;
+
+out:
+	kfree(data);
+	return ret;
+}
+
+static int ethtool_phys_id(struct net_device *dev, void *useraddr)
+{
+	struct ethtool_value id;
+
+	if (!ethtool_ops->phys_id)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&id, useraddr, sizeof(id)))
+		return -EFAULT;
+
+	return ethtool_ops->phys_id(dev, id.data);
+}
+
+static int ethtool_get_stats(struct net_device *dev, void *useraddr)
+{
+	struct ethtool_stats stats;
+	struct ethtool_ops *ops = ethtool_ops;
+	u64 *data;
+	int ret;
+
+	if (!ops->get_ethtool_stats || !ops->get_stats_count)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&stats, useraddr, sizeof(stats)))
+		return -EFAULT;
+
+	stats.n_stats = ops->get_stats_count(dev);
+	data = kmalloc(stats.n_stats * sizeof(u64), GFP_USER);
+	if (!data)
+		return -ENOMEM;
+
+	ops->get_ethtool_stats(dev, &stats, data);
+
+	ret = -EFAULT;
+	if (copy_to_user(useraddr, &stats, sizeof(stats)))
+		goto out;
+	useraddr += sizeof(stats);
+	if (copy_to_user(useraddr, data, stats.n_stats * sizeof(u64)))
+		goto out;
+	ret = 0;
+
+out:
+	kfree(data);
+	return ret;
+}
+
+/* The main entry point in this file.  Called from net/core/dev.c */
+
+#define ETHTOOL_OPS_COMPAT
+int ethtool_ioctl(struct ifreq *ifr)
+{
+	struct net_device *dev = __dev_get_by_name(ifr->ifr_name);
+	void *useraddr = (void *) ifr->ifr_data;
+	u32 ethcmd;
+
+	/*
+	 * XXX: This can be pushed down into the ethtool_* handlers that
+	 * need it.  Keep existing behavior for the moment.
+	 */
+	if (!capable(CAP_NET_ADMIN))
+		return -EPERM;
+
+	if (!dev || !netif_device_present(dev))
+		return -ENODEV;
+
+	if (copy_from_user(&ethcmd, useraddr, sizeof (ethcmd)))
+		return -EFAULT;
+
+	switch (ethcmd) {
+	case ETHTOOL_GSET:
+		return ethtool_get_settings(dev, useraddr);
+	case ETHTOOL_SSET:
+		return ethtool_set_settings(dev, useraddr);
+	case ETHTOOL_GDRVINFO:
+		return ethtool_get_drvinfo(dev, useraddr);
+	case ETHTOOL_GREGS:
+		return ethtool_get_regs(dev, useraddr);
+	case ETHTOOL_GWOL:
+		return ethtool_get_wol(dev, useraddr);
+	case ETHTOOL_SWOL:
+		return ethtool_set_wol(dev, useraddr);
+	case ETHTOOL_GMSGLVL:
+		return ethtool_get_msglevel(dev, useraddr);
+	case ETHTOOL_SMSGLVL:
+		return ethtool_set_msglevel(dev, useraddr);
+	case ETHTOOL_NWAY_RST:
+		return ethtool_nway_reset(dev);
+	case ETHTOOL_GLINK:
+		return ethtool_get_link(dev, useraddr);
+	case ETHTOOL_GEEPROM:
+		return ethtool_get_eeprom(dev, useraddr);
+	case ETHTOOL_SEEPROM:
+		return ethtool_set_eeprom(dev, useraddr);
+	case ETHTOOL_GCOALESCE:
+		return ethtool_get_coalesce(dev, useraddr);
+	case ETHTOOL_SCOALESCE:
+		return ethtool_set_coalesce(dev, useraddr);
+	case ETHTOOL_GRINGPARAM:
+		return ethtool_get_ringparam(dev, useraddr);
+	case ETHTOOL_SRINGPARAM:
+		return ethtool_set_ringparam(dev, useraddr);
+	case ETHTOOL_GPAUSEPARAM:
+		return ethtool_get_pauseparam(dev, useraddr);
+	case ETHTOOL_SPAUSEPARAM:
+		return ethtool_set_pauseparam(dev, useraddr);
+	case ETHTOOL_GRXCSUM:
+		return ethtool_get_rx_csum(dev, useraddr);
+	case ETHTOOL_SRXCSUM:
+		return ethtool_set_rx_csum(dev, useraddr);
+	case ETHTOOL_GTXCSUM:
+		return ethtool_get_tx_csum(dev, useraddr);
+	case ETHTOOL_STXCSUM:
+		return ethtool_set_tx_csum(dev, useraddr);
+	case ETHTOOL_GSG:
+		return ethtool_get_sg(dev, useraddr);
+	case ETHTOOL_SSG:
+		return ethtool_set_sg(dev, useraddr);
+	case ETHTOOL_GTSO:
+		return ethtool_get_tso(dev, useraddr);
+	case ETHTOOL_STSO:
+		return ethtool_set_tso(dev, useraddr);
+	case ETHTOOL_TEST:
+		return ethtool_self_test(dev, useraddr);
+	case ETHTOOL_GSTRINGS:
+		return ethtool_get_strings(dev, useraddr);
+	case ETHTOOL_PHYS_ID:
+		return ethtool_phys_id(dev, useraddr);
+	case ETHTOOL_GSTATS:
+		return ethtool_get_stats(dev, useraddr);
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	return -EOPNOTSUPP;
+}
+
+#define mii_if_info _kc_mii_if_info
+struct _kc_mii_if_info {
+	int phy_id;
+	int advertising;
+	int phy_id_mask;
+	int reg_num_mask;
+
+	unsigned int full_duplex : 1;	/* is full duplex? */
+	unsigned int force_media : 1;	/* is autoneg. disabled? */
+
+	struct net_device *dev;
+	int (*mdio_read) (struct net_device *dev, int phy_id, int location);
+	void (*mdio_write) (struct net_device *dev, int phy_id, int location, int val);
+};
+
+struct ethtool_cmd;
+struct mii_ioctl_data;
+
+#undef mii_link_ok
+#define mii_link_ok _kc_mii_link_ok
+#undef mii_nway_restart
+#define mii_nway_restart _kc_mii_nway_restart
+#undef mii_ethtool_gset
+#define mii_ethtool_gset _kc_mii_ethtool_gset
+#undef mii_ethtool_sset
+#define mii_ethtool_sset _kc_mii_ethtool_sset
+#undef mii_check_link
+#define mii_check_link _kc_mii_check_link
+extern int _kc_mii_link_ok (struct mii_if_info *mii);
+extern int _kc_mii_nway_restart (struct mii_if_info *mii);
+extern int _kc_mii_ethtool_gset(struct mii_if_info *mii,
+                                struct ethtool_cmd *ecmd);
+extern int _kc_mii_ethtool_sset(struct mii_if_info *mii,
+                                struct ethtool_cmd *ecmd);
+extern void _kc_mii_check_link (struct mii_if_info *mii);
+#if ( LINUX_VERSION_CODE > KERNEL_VERSION(2,4,6) )
+#undef generic_mii_ioctl
+#define generic_mii_ioctl _kc_generic_mii_ioctl
+extern int _kc_generic_mii_ioctl(struct mii_if_info *mii_if,
+                                 struct mii_ioctl_data *mii_data, int cmd,
+                                 unsigned int *duplex_changed);
+#endif /* > 2.4.6 */
+
+
+struct _kc_pci_dev_ext {
+	struct pci_dev *dev;
+	void *pci_drvdata;
+	struct pci_driver *driver;
+};
+
+struct _kc_net_dev_ext {
+	struct net_device *dev;
+	unsigned int carrier;
+};
+
+
+/**************************************/
+/* mii support */
+
+int _kc_mii_ethtool_gset(struct mii_if_info *mii, struct ethtool_cmd *ecmd)
+{
+	struct net_device *dev = mii->dev;
+	u32 advert, bmcr, lpa, nego;
+
+	ecmd->supported =
+	    (SUPPORTED_10baseT_Half | SUPPORTED_10baseT_Full |
+	     SUPPORTED_100baseT_Half | SUPPORTED_100baseT_Full |
+	     SUPPORTED_Autoneg | SUPPORTED_TP | SUPPORTED_MII);
+
+	/* only supports twisted-pair */
+	ecmd->port = PORT_MII;
+
+	/* only supports internal transceiver */
+	ecmd->transceiver = XCVR_INTERNAL;
+
+	/* this isn't fully supported at higher layers */
+	ecmd->phy_address = mii->phy_id;
+
+	ecmd->advertising = ADVERTISED_TP | ADVERTISED_MII;
+	advert = mii->mdio_read(dev, mii->phy_id, MII_ADVERTISE);
+	if (advert & ADVERTISE_10HALF)
+		ecmd->advertising |= ADVERTISED_10baseT_Half;
+	if (advert & ADVERTISE_10FULL)
+		ecmd->advertising |= ADVERTISED_10baseT_Full;
+	if (advert & ADVERTISE_100HALF)
+		ecmd->advertising |= ADVERTISED_100baseT_Half;
+	if (advert & ADVERTISE_100FULL)
+		ecmd->advertising |= ADVERTISED_100baseT_Full;
+
+	bmcr = mii->mdio_read(dev, mii->phy_id, MII_BMCR);
+	lpa = mii->mdio_read(dev, mii->phy_id, MII_LPA);
+	if (bmcr & BMCR_ANENABLE) {
+		ecmd->advertising |= ADVERTISED_Autoneg;
+		ecmd->autoneg = AUTONEG_ENABLE;
+
+		nego = mii_nway_result(advert & lpa);
+		if (nego == LPA_100FULL || nego == LPA_100HALF)
+			ecmd->speed = SPEED_100;
+		else
+			ecmd->speed = SPEED_10;
+		if (nego == LPA_100FULL || nego == LPA_10FULL) {
+			ecmd->duplex = DUPLEX_FULL;
+			mii->full_duplex = 1;
+		} else {
+			ecmd->duplex = DUPLEX_HALF;
+			mii->full_duplex = 0;
+		}
+	} else {
+		ecmd->autoneg = AUTONEG_DISABLE;
+
+		ecmd->speed = (bmcr & BMCR_SPEED100) ? SPEED_100 : SPEED_10;
+		ecmd->duplex = (bmcr & BMCR_FULLDPLX) ? DUPLEX_FULL : DUPLEX_HALF;
+	}
+
+	/* ignore maxtxpkt, maxrxpkt for now */
+
+	return 0;
+}
+
+int _kc_mii_ethtool_sset(struct mii_if_info *mii, struct ethtool_cmd *ecmd)
+{
+	struct net_device *dev = mii->dev;
+
+	if (ecmd->speed != SPEED_10 && ecmd->speed != SPEED_100)
+		return -EINVAL;
+	if (ecmd->duplex != DUPLEX_HALF && ecmd->duplex != DUPLEX_FULL)
+		return -EINVAL;
+	if (ecmd->port != PORT_MII)
+		return -EINVAL;
+	if (ecmd->transceiver != XCVR_INTERNAL)
+		return -EINVAL;
+	if (ecmd->phy_address != mii->phy_id)
+		return -EINVAL;
+	if (ecmd->autoneg != AUTONEG_DISABLE && ecmd->autoneg != AUTONEG_ENABLE)
+		return -EINVAL;
+
+	/* ignore supported, maxtxpkt, maxrxpkt */
+
+	if (ecmd->autoneg == AUTONEG_ENABLE) {
+		u32 bmcr, advert, tmp;
+
+		if ((ecmd->advertising & (ADVERTISED_10baseT_Half |
+					  ADVERTISED_10baseT_Full |
+					  ADVERTISED_100baseT_Half |
+					  ADVERTISED_100baseT_Full)) == 0)
+			return -EINVAL;
+
+		/* advertise only what has been requested */
+		advert = mii->mdio_read(dev, mii->phy_id, MII_ADVERTISE);
+		tmp = advert & ~(ADVERTISE_ALL | ADVERTISE_100BASE4);
+		if (ADVERTISED_10baseT_Half)
+			tmp |= ADVERTISE_10HALF;
+		if (ADVERTISED_10baseT_Full)
+			tmp |= ADVERTISE_10FULL;
+		if (ADVERTISED_100baseT_Half)
+			tmp |= ADVERTISE_100HALF;
+		if (ADVERTISED_100baseT_Full)
+			tmp |= ADVERTISE_100FULL;
+		if (advert != tmp) {
+			mii->mdio_write(dev, mii->phy_id, MII_ADVERTISE, tmp);
+			mii->advertising = tmp;
+		}
+
+		/* turn on autonegotiation, and force a renegotiate */
+		bmcr = mii->mdio_read(dev, mii->phy_id, MII_BMCR);
+		bmcr |= (BMCR_ANENABLE | BMCR_ANRESTART);
+		mii->mdio_write(dev, mii->phy_id, MII_BMCR, bmcr);
+
+		mii->force_media = 0;
+	} else {
+		u32 bmcr, tmp;
+
+		/* turn off auto negotiation, set speed and duplexity */
+		bmcr = mii->mdio_read(dev, mii->phy_id, MII_BMCR);
+		tmp = bmcr & ~(BMCR_ANENABLE | BMCR_SPEED100 | BMCR_FULLDPLX);
+		if (ecmd->speed == SPEED_100)
+			tmp |= BMCR_SPEED100;
+		if (ecmd->duplex == DUPLEX_FULL) {
+			tmp |= BMCR_FULLDPLX;
+			mii->full_duplex = 1;
+		} else
+			mii->full_duplex = 0;
+		if (bmcr != tmp)
+			mii->mdio_write(dev, mii->phy_id, MII_BMCR, tmp);
+
+		mii->force_media = 1;
+	}
+	return 0;
+}
+
+int _kc_mii_link_ok (struct mii_if_info *mii)
+{
+	/* first, a dummy read, needed to latch some MII phys */
+	mii->mdio_read(mii->dev, mii->phy_id, MII_BMSR);
+	if (mii->mdio_read(mii->dev, mii->phy_id, MII_BMSR) & BMSR_LSTATUS)
+		return 1;
+	return 0;
+}
+
+int _kc_mii_nway_restart (struct mii_if_info *mii)
+{
+	int bmcr;
+	int r = -EINVAL;
+
+	/* if autoneg is off, it's an error */
+	bmcr = mii->mdio_read(mii->dev, mii->phy_id, MII_BMCR);
+
+	if (bmcr & BMCR_ANENABLE) {
+		bmcr |= BMCR_ANRESTART;
+		mii->mdio_write(mii->dev, mii->phy_id, MII_BMCR, bmcr);
+		r = 0;
+	}
+
+	return r;
+}
+
+void _kc_mii_check_link (struct mii_if_info *mii)
+{
+	int cur_link = mii_link_ok(mii);
+	int prev_link = netif_carrier_ok(mii->dev);
+
+	if (cur_link && !prev_link)
+		netif_carrier_on(mii->dev);
+	else if (prev_link && !cur_link)
+		netif_carrier_off(mii->dev);
+}
+
+#if ( LINUX_VERSION_CODE > KERNEL_VERSION(2,4,6) )
+int _kc_generic_mii_ioctl(struct mii_if_info *mii_if,
+                          struct mii_ioctl_data *mii_data, int cmd,
+                          unsigned int *duplex_chg_out)
+{
+	int rc = 0;
+	unsigned int duplex_changed = 0;
+
+	if (duplex_chg_out)
+		*duplex_chg_out = 0;
+
+	mii_data->phy_id &= mii_if->phy_id_mask;
+	mii_data->reg_num &= mii_if->reg_num_mask;
+
+	switch(cmd) {
+	case SIOCDEVPRIVATE:	/* binary compat, remove in 2.5 */
+	case SIOCGMIIPHY:
+		mii_data->phy_id = mii_if->phy_id;
+		/* fall through */
+
+	case SIOCDEVPRIVATE + 1:/* binary compat, remove in 2.5 */
+	case SIOCGMIIREG:
+		mii_data->val_out =
+			mii_if->mdio_read(mii_if->dev, mii_data->phy_id,
+					  mii_data->reg_num);
+		break;
+
+	case SIOCDEVPRIVATE + 2:/* binary compat, remove in 2.5 */
+	case SIOCSMIIREG: {
+		u16 val = mii_data->val_in;
+
+		if (!capable(CAP_NET_ADMIN))
+			return -EPERM;
+
+		if (mii_data->phy_id == mii_if->phy_id) {
+			switch(mii_data->reg_num) {
+			case MII_BMCR: {
+				unsigned int new_duplex = 0;
+				if (val & (BMCR_RESET|BMCR_ANENABLE))
+					mii_if->force_media = 0;
+				else
+					mii_if->force_media = 1;
+				if (mii_if->force_media &&
+				    (val & BMCR_FULLDPLX))
+					new_duplex = 1;
+				if (mii_if->full_duplex != new_duplex) {
+					duplex_changed = 1;
+					mii_if->full_duplex = new_duplex;
+				}
+				break;
+			}
+			case MII_ADVERTISE:
+				mii_if->advertising = val;
+				break;
+			default:
+				/* do nothing */
+				break;
+			}
+		}
+
+		mii_if->mdio_write(mii_if->dev, mii_data->phy_id,
+				   mii_data->reg_num, val);
+		break;
+	}
+
+	default:
+		rc = -EOPNOTSUPP;
+		break;
+	}
+
+	if ((rc == 0) && (duplex_chg_out) && (duplex_changed))
+		*duplex_chg_out = 1;
+
+	return rc;
+}
+#endif /* > 2.4.6 */
+
diff --git a/drivers/net/phy/dp83640.c b/drivers/net/phy/dp83640.c
index edd7304773eb..dc44b73f907e 100644
--- a/drivers/net/phy/dp83640.c
+++ b/drivers/net/phy/dp83640.c
@@ -875,6 +875,7 @@ static void dp83640_remove(struct phy_device *phydev)
 	struct dp83640_clock *clock;
 	struct list_head *this, *next;
 	struct dp83640_private *tmp, *dp83640 = phydev->priv;
+	struct sk_buff *skb;
 
 	if (phydev->addr == BROADCAST_ADDR)
 		return;
@@ -882,6 +883,12 @@ static void dp83640_remove(struct phy_device *phydev)
 	enable_status_frames(phydev, false);
 	cancel_work_sync(&dp83640->ts_work);
 
+	while ((skb = skb_dequeue(&dp83640->rx_queue)) != NULL)
+		kfree_skb(skb);
+
+	while ((skb = skb_dequeue(&dp83640->tx_queue)) != NULL)
+		skb_complete_tx_timestamp(skb, NULL);
+
 	clock = dp83640_clock_get(dp83640->clock);
 
 	if (dp83640 == clock->chosen) {
@@ -1060,7 +1067,7 @@ static void dp83640_txtstamp(struct phy_device *phydev,
 	struct dp83640_private *dp83640 = phydev->priv;
 
 	if (!dp83640->hwts_tx_en) {
-		kfree_skb(skb);
+		skb_complete_tx_timestamp(skb, NULL);
 		return;
 	}
 	skb_queue_tail(&dp83640->tx_queue, skb);
diff --git a/drivers/net/pppolac.c b/drivers/net/pppolac.c
new file mode 100644
index 000000000000..c94b8507d92b
--- /dev/null
+++ b/drivers/net/pppolac.c
@@ -0,0 +1,449 @@
+/* drivers/net/pppolac.c
+ *
+ * Driver for PPP on L2TP Access Concentrator / PPPoLAC Socket (RFC 2661)
+ *
+ * Copyright (C) 2009 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ */
+
+/* This driver handles L2TP data packets between a UDP socket and a PPP channel.
+ * The socket must keep connected, and only one session per socket is permitted.
+ * Sequencing of outgoing packets is controlled by LNS. Incoming packets with
+ * sequences are reordered within a sliding window of one second. Currently
+ * reordering only happens when a packet is received. It is done for simplicity
+ * since no additional locks or threads are required. This driver only works on
+ * IPv4 due to the lack of UDP encapsulation support in IPv6. */
+
+#include <linux/module.h>
+#include <linux/jiffies.h>
+#include <linux/workqueue.h>
+#include <linux/skbuff.h>
+#include <linux/file.h>
+#include <linux/netdevice.h>
+#include <linux/net.h>
+#include <linux/udp.h>
+#include <linux/ppp_defs.h>
+#include <linux/if_ppp.h>
+#include <linux/if_pppox.h>
+#include <linux/ppp_channel.h>
+#include <net/tcp_states.h>
+#include <asm/uaccess.h>
+
+#define L2TP_CONTROL_BIT	0x80
+#define L2TP_LENGTH_BIT		0x40
+#define L2TP_SEQUENCE_BIT	0x08
+#define L2TP_OFFSET_BIT		0x02
+#define L2TP_VERSION		0x02
+#define L2TP_VERSION_MASK	0x0F
+
+#define PPP_ADDR	0xFF
+#define PPP_CTRL	0x03
+
+union unaligned {
+	__u32 u32;
+} __attribute__((packed));
+
+static inline union unaligned *unaligned(void *ptr)
+{
+	return (union unaligned *)ptr;
+}
+
+struct meta {
+	__u32 sequence;
+	__u32 timestamp;
+};
+
+static inline struct meta *skb_meta(struct sk_buff *skb)
+{
+	return (struct meta *)skb->cb;
+}
+
+/******************************************************************************/
+
+static int pppolac_recv_core(struct sock *sk_udp, struct sk_buff *skb)
+{
+	struct sock *sk = (struct sock *)sk_udp->sk_user_data;
+	struct pppolac_opt *opt = &pppox_sk(sk)->proto.lac;
+	struct meta *meta = skb_meta(skb);
+	__u32 now = jiffies;
+	__u8 bits;
+	__u8 *ptr;
+
+	/* Drop the packet if L2TP header is missing. */
+	if (skb->len < sizeof(struct udphdr) + 6)
+		goto drop;
+
+	/* Put it back if it is a control packet. */
+	if (skb->data[sizeof(struct udphdr)] & L2TP_CONTROL_BIT)
+		return opt->backlog_rcv(sk_udp, skb);
+
+	/* Skip UDP header. */
+	skb_pull(skb, sizeof(struct udphdr));
+
+	/* Check the version. */
+	if ((skb->data[1] & L2TP_VERSION_MASK) != L2TP_VERSION)
+		goto drop;
+	bits = skb->data[0];
+	ptr = &skb->data[2];
+
+	/* Check the length if it is present. */
+	if (bits & L2TP_LENGTH_BIT) {
+		if ((ptr[0] << 8 | ptr[1]) != skb->len)
+			goto drop;
+		ptr += 2;
+	}
+
+	/* Skip all fields including optional ones. */
+	if (!skb_pull(skb, 6 + (bits & L2TP_SEQUENCE_BIT ? 4 : 0) +
+			(bits & L2TP_LENGTH_BIT ? 2 : 0) +
+			(bits & L2TP_OFFSET_BIT ? 2 : 0)))
+		goto drop;
+
+	/* Skip the offset padding if it is present. */
+	if (bits & L2TP_OFFSET_BIT &&
+			!skb_pull(skb, skb->data[-2] << 8 | skb->data[-1]))
+		goto drop;
+
+	/* Check the tunnel and the session. */
+	if (unaligned(ptr)->u32 != opt->local)
+		goto drop;
+
+	/* Check the sequence if it is present. */
+	if (bits & L2TP_SEQUENCE_BIT) {
+		meta->sequence = ptr[4] << 8 | ptr[5];
+		if ((__s16)(meta->sequence - opt->recv_sequence) < 0)
+			goto drop;
+	}
+
+	/* Skip PPP address and control if they are present. */
+	if (skb->len >= 2 && skb->data[0] == PPP_ADDR &&
+			skb->data[1] == PPP_CTRL)
+		skb_pull(skb, 2);
+
+	/* Fix PPP protocol if it is compressed. */
+	if (skb->len >= 1 && skb->data[0] & 1)
+		skb_push(skb, 1)[0] = 0;
+
+	/* Drop the packet if PPP protocol is missing. */
+	if (skb->len < 2)
+		goto drop;
+
+	/* Perform reordering if sequencing is enabled. */
+	atomic_set(&opt->sequencing, bits & L2TP_SEQUENCE_BIT);
+	if (bits & L2TP_SEQUENCE_BIT) {
+		struct sk_buff *skb1;
+
+		/* Insert the packet into receive queue in order. */
+		skb_set_owner_r(skb, sk);
+		skb_queue_walk(&sk->sk_receive_queue, skb1) {
+			struct meta *meta1 = skb_meta(skb1);
+			__s16 order = meta->sequence - meta1->sequence;
+			if (order == 0)
+				goto drop;
+			if (order < 0) {
+				meta->timestamp = meta1->timestamp;
+				skb_insert(skb1, skb, &sk->sk_receive_queue);
+				skb = NULL;
+				break;
+			}
+		}
+		if (skb) {
+			meta->timestamp = now;
+			skb_queue_tail(&sk->sk_receive_queue, skb);
+		}
+
+		/* Remove packets from receive queue as long as
+		 * 1. the receive buffer is full,
+		 * 2. they are queued longer than one second, or
+		 * 3. there are no missing packets before them. */
+		skb_queue_walk_safe(&sk->sk_receive_queue, skb, skb1) {
+			meta = skb_meta(skb);
+			if (atomic_read(&sk->sk_rmem_alloc) < sk->sk_rcvbuf &&
+					now - meta->timestamp < HZ &&
+					meta->sequence != opt->recv_sequence)
+				break;
+			skb_unlink(skb, &sk->sk_receive_queue);
+			opt->recv_sequence = (__u16)(meta->sequence + 1);
+			skb_orphan(skb);
+			ppp_input(&pppox_sk(sk)->chan, skb);
+		}
+		return NET_RX_SUCCESS;
+	}
+
+	/* Flush receive queue if sequencing is disabled. */
+	skb_queue_purge(&sk->sk_receive_queue);
+	skb_orphan(skb);
+	ppp_input(&pppox_sk(sk)->chan, skb);
+	return NET_RX_SUCCESS;
+drop:
+	kfree_skb(skb);
+	return NET_RX_DROP;
+}
+
+static int pppolac_recv(struct sock *sk_udp, struct sk_buff *skb)
+{
+	sock_hold(sk_udp);
+	sk_receive_skb(sk_udp, skb, 0);
+	return 0;
+}
+
+static struct sk_buff_head delivery_queue;
+
+static void pppolac_xmit_core(struct work_struct *delivery_work)
+{
+	mm_segment_t old_fs = get_fs();
+	struct sk_buff *skb;
+
+	set_fs(KERNEL_DS);
+	while ((skb = skb_dequeue(&delivery_queue))) {
+		struct sock *sk_udp = skb->sk;
+		struct kvec iov = {.iov_base = skb->data, .iov_len = skb->len};
+		struct msghdr msg = {
+			.msg_iov = (struct iovec *)&iov,
+			.msg_iovlen = 1,
+			.msg_flags = MSG_NOSIGNAL | MSG_DONTWAIT,
+		};
+		sk_udp->sk_prot->sendmsg(NULL, sk_udp, &msg, skb->len);
+		kfree_skb(skb);
+	}
+	set_fs(old_fs);
+}
+
+static DECLARE_WORK(delivery_work, pppolac_xmit_core);
+
+static int pppolac_xmit(struct ppp_channel *chan, struct sk_buff *skb)
+{
+	struct sock *sk_udp = (struct sock *)chan->private;
+	struct pppolac_opt *opt = &pppox_sk(sk_udp->sk_user_data)->proto.lac;
+
+	/* Install PPP address and control. */
+	skb_push(skb, 2);
+	skb->data[0] = PPP_ADDR;
+	skb->data[1] = PPP_CTRL;
+
+	/* Install L2TP header. */
+	if (atomic_read(&opt->sequencing)) {
+		skb_push(skb, 10);
+		skb->data[0] = L2TP_SEQUENCE_BIT;
+		skb->data[6] = opt->xmit_sequence >> 8;
+		skb->data[7] = opt->xmit_sequence;
+		skb->data[8] = 0;
+		skb->data[9] = 0;
+		opt->xmit_sequence++;
+	} else {
+		skb_push(skb, 6);
+		skb->data[0] = 0;
+	}
+	skb->data[1] = L2TP_VERSION;
+	unaligned(&skb->data[2])->u32 = opt->remote;
+
+	/* Now send the packet via the delivery queue. */
+	skb_set_owner_w(skb, sk_udp);
+	skb_queue_tail(&delivery_queue, skb);
+	schedule_work(&delivery_work);
+	return 1;
+}
+
+/******************************************************************************/
+
+static struct ppp_channel_ops pppolac_channel_ops = {
+	.start_xmit = pppolac_xmit,
+};
+
+static int pppolac_connect(struct socket *sock, struct sockaddr *useraddr,
+	int addrlen, int flags)
+{
+	struct sock *sk = sock->sk;
+	struct pppox_sock *po = pppox_sk(sk);
+	struct sockaddr_pppolac *addr = (struct sockaddr_pppolac *)useraddr;
+	struct socket *sock_udp = NULL;
+	struct sock *sk_udp;
+	int error;
+
+	if (addrlen != sizeof(struct sockaddr_pppolac) ||
+			!addr->local.tunnel || !addr->local.session ||
+			!addr->remote.tunnel || !addr->remote.session) {
+		return -EINVAL;
+	}
+
+	lock_sock(sk);
+	error = -EALREADY;
+	if (sk->sk_state != PPPOX_NONE)
+		goto out;
+
+	sock_udp = sockfd_lookup(addr->udp_socket, &error);
+	if (!sock_udp)
+		goto out;
+	sk_udp = sock_udp->sk;
+	lock_sock(sk_udp);
+
+	/* Remove this check when IPv6 supports UDP encapsulation. */
+	error = -EAFNOSUPPORT;
+	if (sk_udp->sk_family != AF_INET)
+		goto out;
+	error = -EPROTONOSUPPORT;
+	if (sk_udp->sk_protocol != IPPROTO_UDP)
+		goto out;
+	error = -EDESTADDRREQ;
+	if (sk_udp->sk_state != TCP_ESTABLISHED)
+		goto out;
+	error = -EBUSY;
+	if (udp_sk(sk_udp)->encap_type || sk_udp->sk_user_data)
+		goto out;
+	if (!sk_udp->sk_bound_dev_if) {
+		struct dst_entry *dst = sk_dst_get(sk_udp);
+		error = -ENODEV;
+		if (!dst)
+			goto out;
+		sk_udp->sk_bound_dev_if = dst->dev->ifindex;
+		dst_release(dst);
+	}
+
+	po->chan.hdrlen = 12;
+	po->chan.private = sk_udp;
+	po->chan.ops = &pppolac_channel_ops;
+	po->chan.mtu = PPP_MTU - 80;
+	po->proto.lac.local = unaligned(&addr->local)->u32;
+	po->proto.lac.remote = unaligned(&addr->remote)->u32;
+	atomic_set(&po->proto.lac.sequencing, 1);
+	po->proto.lac.backlog_rcv = sk_udp->sk_backlog_rcv;
+
+	error = ppp_register_channel(&po->chan);
+	if (error)
+		goto out;
+
+	sk->sk_state = PPPOX_CONNECTED;
+	udp_sk(sk_udp)->encap_type = UDP_ENCAP_L2TPINUDP;
+	udp_sk(sk_udp)->encap_rcv = pppolac_recv;
+	sk_udp->sk_backlog_rcv = pppolac_recv_core;
+	sk_udp->sk_user_data = sk;
+out:
+	if (sock_udp) {
+		release_sock(sk_udp);
+		if (error)
+			sockfd_put(sock_udp);
+	}
+	release_sock(sk);
+	return error;
+}
+
+static int pppolac_release(struct socket *sock)
+{
+	struct sock *sk = sock->sk;
+
+	if (!sk)
+		return 0;
+
+	lock_sock(sk);
+	if (sock_flag(sk, SOCK_DEAD)) {
+		release_sock(sk);
+		return -EBADF;
+	}
+
+	if (sk->sk_state != PPPOX_NONE) {
+		struct sock *sk_udp = (struct sock *)pppox_sk(sk)->chan.private;
+		lock_sock(sk_udp);
+		skb_queue_purge(&sk->sk_receive_queue);
+		pppox_unbind_sock(sk);
+		udp_sk(sk_udp)->encap_type = 0;
+		udp_sk(sk_udp)->encap_rcv = NULL;
+		sk_udp->sk_backlog_rcv = pppox_sk(sk)->proto.lac.backlog_rcv;
+		sk_udp->sk_user_data = NULL;
+		release_sock(sk_udp);
+		sockfd_put(sk_udp->sk_socket);
+	}
+
+	sock_orphan(sk);
+	sock->sk = NULL;
+	release_sock(sk);
+	sock_put(sk);
+	return 0;
+}
+
+/******************************************************************************/
+
+static struct proto pppolac_proto = {
+	.name = "PPPOLAC",
+	.owner = THIS_MODULE,
+	.obj_size = sizeof(struct pppox_sock),
+};
+
+static struct proto_ops pppolac_proto_ops = {
+	.family = PF_PPPOX,
+	.owner = THIS_MODULE,
+	.release = pppolac_release,
+	.bind = sock_no_bind,
+	.connect = pppolac_connect,
+	.socketpair = sock_no_socketpair,
+	.accept = sock_no_accept,
+	.getname = sock_no_getname,
+	.poll = sock_no_poll,
+	.ioctl = pppox_ioctl,
+	.listen = sock_no_listen,
+	.shutdown = sock_no_shutdown,
+	.setsockopt = sock_no_setsockopt,
+	.getsockopt = sock_no_getsockopt,
+	.sendmsg = sock_no_sendmsg,
+	.recvmsg = sock_no_recvmsg,
+	.mmap = sock_no_mmap,
+};
+
+static int pppolac_create(struct net *net, struct socket *sock)
+{
+	struct sock *sk;
+
+	sk = sk_alloc(net, PF_PPPOX, GFP_KERNEL, &pppolac_proto);
+	if (!sk)
+		return -ENOMEM;
+
+	sock_init_data(sock, sk);
+	sock->state = SS_UNCONNECTED;
+	sock->ops = &pppolac_proto_ops;
+	sk->sk_protocol = PX_PROTO_OLAC;
+	sk->sk_state = PPPOX_NONE;
+	return 0;
+}
+
+/******************************************************************************/
+
+static struct pppox_proto pppolac_pppox_proto = {
+	.create = pppolac_create,
+	.owner = THIS_MODULE,
+};
+
+static int __init pppolac_init(void)
+{
+	int error;
+
+	error = proto_register(&pppolac_proto, 0);
+	if (error)
+		return error;
+
+	error = register_pppox_proto(PX_PROTO_OLAC, &pppolac_pppox_proto);
+	if (error)
+		proto_unregister(&pppolac_proto);
+	else
+		skb_queue_head_init(&delivery_queue);
+	return error;
+}
+
+static void __exit pppolac_exit(void)
+{
+	unregister_pppox_proto(PX_PROTO_OLAC);
+	proto_unregister(&pppolac_proto);
+}
+
+module_init(pppolac_init);
+module_exit(pppolac_exit);
+
+MODULE_DESCRIPTION("PPP on L2TP Access Concentrator (PPPoLAC)");
+MODULE_AUTHOR("Chia-chi Yeh <chiachi@android.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/pppopns.c b/drivers/net/pppopns.c
new file mode 100644
index 000000000000..fb8198447938
--- /dev/null
+++ b/drivers/net/pppopns.c
@@ -0,0 +1,428 @@
+/* drivers/net/pppopns.c
+ *
+ * Driver for PPP on PPTP Network Server / PPPoPNS Socket (RFC 2637)
+ *
+ * Copyright (C) 2009 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ */
+
+/* This driver handles PPTP data packets between a RAW socket and a PPP channel.
+ * The socket is created in the kernel space and connected to the same address
+ * of the control socket. Outgoing packets are always sent with sequences but
+ * without acknowledgements. Incoming packets with sequences are reordered
+ * within a sliding window of one second. Currently reordering only happens when
+ * a packet is received. It is done for simplicity since no additional locks or
+ * threads are required. This driver should work on both IPv4 and IPv6. */
+
+#include <linux/module.h>
+#include <linux/jiffies.h>
+#include <linux/workqueue.h>
+#include <linux/skbuff.h>
+#include <linux/file.h>
+#include <linux/netdevice.h>
+#include <linux/net.h>
+#include <linux/ppp_defs.h>
+#include <linux/if.h>
+#include <linux/if_ppp.h>
+#include <linux/if_pppox.h>
+#include <linux/ppp_channel.h>
+#include <asm/uaccess.h>
+
+#define GRE_HEADER_SIZE		8
+
+#define PPTP_GRE_BITS		htons(0x2001)
+#define PPTP_GRE_BITS_MASK	htons(0xEF7F)
+#define PPTP_GRE_SEQ_BIT	htons(0x1000)
+#define PPTP_GRE_ACK_BIT	htons(0x0080)
+#define PPTP_GRE_TYPE		htons(0x880B)
+
+#define PPP_ADDR	0xFF
+#define PPP_CTRL	0x03
+
+struct header {
+	__u16	bits;
+	__u16	type;
+	__u16	length;
+	__u16	call;
+	__u32	sequence;
+} __attribute__((packed));
+
+struct meta {
+	__u32 sequence;
+	__u32 timestamp;
+};
+
+static inline struct meta *skb_meta(struct sk_buff *skb)
+{
+	return (struct meta *)skb->cb;
+}
+
+/******************************************************************************/
+
+static int pppopns_recv_core(struct sock *sk_raw, struct sk_buff *skb)
+{
+	struct sock *sk = (struct sock *)sk_raw->sk_user_data;
+	struct pppopns_opt *opt = &pppox_sk(sk)->proto.pns;
+	struct meta *meta = skb_meta(skb);
+	__u32 now = jiffies;
+	struct header *hdr;
+
+	/* Skip transport header */
+	skb_pull(skb, skb_transport_header(skb) - skb->data);
+
+	/* Drop the packet if GRE header is missing. */
+	if (skb->len < GRE_HEADER_SIZE)
+		goto drop;
+	hdr = (struct header *)skb->data;
+
+	/* Check the header. */
+	if (hdr->type != PPTP_GRE_TYPE || hdr->call != opt->local ||
+			(hdr->bits & PPTP_GRE_BITS_MASK) != PPTP_GRE_BITS)
+		goto drop;
+
+	/* Skip all fields including optional ones. */
+	if (!skb_pull(skb, GRE_HEADER_SIZE +
+			(hdr->bits & PPTP_GRE_SEQ_BIT ? 4 : 0) +
+			(hdr->bits & PPTP_GRE_ACK_BIT ? 4 : 0)))
+		goto drop;
+
+	/* Check the length. */
+	if (skb->len != ntohs(hdr->length))
+		goto drop;
+
+	/* Check the sequence if it is present. */
+	if (hdr->bits & PPTP_GRE_SEQ_BIT) {
+		meta->sequence = ntohl(hdr->sequence);
+		if ((__s32)(meta->sequence - opt->recv_sequence) < 0)
+			goto drop;
+	}
+
+	/* Skip PPP address and control if they are present. */
+	if (skb->len >= 2 && skb->data[0] == PPP_ADDR &&
+			skb->data[1] == PPP_CTRL)
+		skb_pull(skb, 2);
+
+	/* Fix PPP protocol if it is compressed. */
+	if (skb->len >= 1 && skb->data[0] & 1)
+		skb_push(skb, 1)[0] = 0;
+
+	/* Drop the packet if PPP protocol is missing. */
+	if (skb->len < 2)
+		goto drop;
+
+	/* Perform reordering if sequencing is enabled. */
+	if (hdr->bits & PPTP_GRE_SEQ_BIT) {
+		struct sk_buff *skb1;
+
+		/* Insert the packet into receive queue in order. */
+		skb_set_owner_r(skb, sk);
+		skb_queue_walk(&sk->sk_receive_queue, skb1) {
+			struct meta *meta1 = skb_meta(skb1);
+			__s32 order = meta->sequence - meta1->sequence;
+			if (order == 0)
+				goto drop;
+			if (order < 0) {
+				meta->timestamp = meta1->timestamp;
+				skb_insert(skb1, skb, &sk->sk_receive_queue);
+				skb = NULL;
+				break;
+			}
+		}
+		if (skb) {
+			meta->timestamp = now;
+			skb_queue_tail(&sk->sk_receive_queue, skb);
+		}
+
+		/* Remove packets from receive queue as long as
+		 * 1. the receive buffer is full,
+		 * 2. they are queued longer than one second, or
+		 * 3. there are no missing packets before them. */
+		skb_queue_walk_safe(&sk->sk_receive_queue, skb, skb1) {
+			meta = skb_meta(skb);
+			if (atomic_read(&sk->sk_rmem_alloc) < sk->sk_rcvbuf &&
+					now - meta->timestamp < HZ &&
+					meta->sequence != opt->recv_sequence)
+				break;
+			skb_unlink(skb, &sk->sk_receive_queue);
+			opt->recv_sequence = meta->sequence + 1;
+			skb_orphan(skb);
+			ppp_input(&pppox_sk(sk)->chan, skb);
+		}
+		return NET_RX_SUCCESS;
+	}
+
+	/* Flush receive queue if sequencing is disabled. */
+	skb_queue_purge(&sk->sk_receive_queue);
+	skb_orphan(skb);
+	ppp_input(&pppox_sk(sk)->chan, skb);
+	return NET_RX_SUCCESS;
+drop:
+	kfree_skb(skb);
+	return NET_RX_DROP;
+}
+
+static void pppopns_recv(struct sock *sk_raw, int length)
+{
+	struct sk_buff *skb;
+	while ((skb = skb_dequeue(&sk_raw->sk_receive_queue))) {
+		sock_hold(sk_raw);
+		sk_receive_skb(sk_raw, skb, 0);
+	}
+}
+
+static struct sk_buff_head delivery_queue;
+
+static void pppopns_xmit_core(struct work_struct *delivery_work)
+{
+	mm_segment_t old_fs = get_fs();
+	struct sk_buff *skb;
+
+	set_fs(KERNEL_DS);
+	while ((skb = skb_dequeue(&delivery_queue))) {
+		struct sock *sk_raw = skb->sk;
+		struct kvec iov = {.iov_base = skb->data, .iov_len = skb->len};
+		struct msghdr msg = {
+			.msg_iov = (struct iovec *)&iov,
+			.msg_iovlen = 1,
+			.msg_flags = MSG_NOSIGNAL | MSG_DONTWAIT,
+		};
+		sk_raw->sk_prot->sendmsg(NULL, sk_raw, &msg, skb->len);
+		kfree_skb(skb);
+	}
+	set_fs(old_fs);
+}
+
+static DECLARE_WORK(delivery_work, pppopns_xmit_core);
+
+static int pppopns_xmit(struct ppp_channel *chan, struct sk_buff *skb)
+{
+	struct sock *sk_raw = (struct sock *)chan->private;
+	struct pppopns_opt *opt = &pppox_sk(sk_raw->sk_user_data)->proto.pns;
+	struct header *hdr;
+	__u16 length;
+
+	/* Install PPP address and control. */
+	skb_push(skb, 2);
+	skb->data[0] = PPP_ADDR;
+	skb->data[1] = PPP_CTRL;
+	length = skb->len;
+
+	/* Install PPTP GRE header. */
+	hdr = (struct header *)skb_push(skb, 12);
+	hdr->bits = PPTP_GRE_BITS | PPTP_GRE_SEQ_BIT;
+	hdr->type = PPTP_GRE_TYPE;
+	hdr->length = htons(length);
+	hdr->call = opt->remote;
+	hdr->sequence = htonl(opt->xmit_sequence);
+	opt->xmit_sequence++;
+
+	/* Now send the packet via the delivery queue. */
+	skb_set_owner_w(skb, sk_raw);
+	skb_queue_tail(&delivery_queue, skb);
+	schedule_work(&delivery_work);
+	return 1;
+}
+
+/******************************************************************************/
+
+static struct ppp_channel_ops pppopns_channel_ops = {
+	.start_xmit = pppopns_xmit,
+};
+
+static int pppopns_connect(struct socket *sock, struct sockaddr *useraddr,
+	int addrlen, int flags)
+{
+	struct sock *sk = sock->sk;
+	struct pppox_sock *po = pppox_sk(sk);
+	struct sockaddr_pppopns *addr = (struct sockaddr_pppopns *)useraddr;
+	struct sockaddr_storage ss;
+	struct socket *sock_tcp = NULL;
+	struct socket *sock_raw = NULL;
+	struct sock *sk_tcp;
+	struct sock *sk_raw;
+	int error;
+
+	if (addrlen != sizeof(struct sockaddr_pppopns))
+		return -EINVAL;
+
+	lock_sock(sk);
+	error = -EALREADY;
+	if (sk->sk_state != PPPOX_NONE)
+		goto out;
+
+	sock_tcp = sockfd_lookup(addr->tcp_socket, &error);
+	if (!sock_tcp)
+		goto out;
+	sk_tcp = sock_tcp->sk;
+	error = -EPROTONOSUPPORT;
+	if (sk_tcp->sk_protocol != IPPROTO_TCP)
+		goto out;
+	addrlen = sizeof(struct sockaddr_storage);
+	error = kernel_getpeername(sock_tcp, (struct sockaddr *)&ss, &addrlen);
+	if (error)
+		goto out;
+	if (!sk_tcp->sk_bound_dev_if) {
+		struct dst_entry *dst = sk_dst_get(sk_tcp);
+		error = -ENODEV;
+		if (!dst)
+			goto out;
+		sk_tcp->sk_bound_dev_if = dst->dev->ifindex;
+		dst_release(dst);
+	}
+
+	error = sock_create(ss.ss_family, SOCK_RAW, IPPROTO_GRE, &sock_raw);
+	if (error)
+		goto out;
+	sk_raw = sock_raw->sk;
+	sk_raw->sk_bound_dev_if = sk_tcp->sk_bound_dev_if;
+	error = kernel_connect(sock_raw, (struct sockaddr *)&ss, addrlen, 0);
+	if (error)
+		goto out;
+
+	po->chan.hdrlen = 14;
+	po->chan.private = sk_raw;
+	po->chan.ops = &pppopns_channel_ops;
+	po->chan.mtu = PPP_MTU - 80;
+	po->proto.pns.local = addr->local;
+	po->proto.pns.remote = addr->remote;
+	po->proto.pns.data_ready = sk_raw->sk_data_ready;
+	po->proto.pns.backlog_rcv = sk_raw->sk_backlog_rcv;
+
+	error = ppp_register_channel(&po->chan);
+	if (error)
+		goto out;
+
+	sk->sk_state = PPPOX_CONNECTED;
+	lock_sock(sk_raw);
+	sk_raw->sk_data_ready = pppopns_recv;
+	sk_raw->sk_backlog_rcv = pppopns_recv_core;
+	sk_raw->sk_user_data = sk;
+	release_sock(sk_raw);
+out:
+	if (sock_tcp)
+		sockfd_put(sock_tcp);
+	if (error && sock_raw)
+		sock_release(sock_raw);
+	release_sock(sk);
+	return error;
+}
+
+static int pppopns_release(struct socket *sock)
+{
+	struct sock *sk = sock->sk;
+
+	if (!sk)
+		return 0;
+
+	lock_sock(sk);
+	if (sock_flag(sk, SOCK_DEAD)) {
+		release_sock(sk);
+		return -EBADF;
+	}
+
+	if (sk->sk_state != PPPOX_NONE) {
+		struct sock *sk_raw = (struct sock *)pppox_sk(sk)->chan.private;
+		lock_sock(sk_raw);
+		skb_queue_purge(&sk->sk_receive_queue);
+		pppox_unbind_sock(sk);
+		sk_raw->sk_data_ready = pppox_sk(sk)->proto.pns.data_ready;
+		sk_raw->sk_backlog_rcv = pppox_sk(sk)->proto.pns.backlog_rcv;
+		sk_raw->sk_user_data = NULL;
+		release_sock(sk_raw);
+		sock_release(sk_raw->sk_socket);
+	}
+
+	sock_orphan(sk);
+	sock->sk = NULL;
+	release_sock(sk);
+	sock_put(sk);
+	return 0;
+}
+
+/******************************************************************************/
+
+static struct proto pppopns_proto = {
+	.name = "PPPOPNS",
+	.owner = THIS_MODULE,
+	.obj_size = sizeof(struct pppox_sock),
+};
+
+static struct proto_ops pppopns_proto_ops = {
+	.family = PF_PPPOX,
+	.owner = THIS_MODULE,
+	.release = pppopns_release,
+	.bind = sock_no_bind,
+	.connect = pppopns_connect,
+	.socketpair = sock_no_socketpair,
+	.accept = sock_no_accept,
+	.getname = sock_no_getname,
+	.poll = sock_no_poll,
+	.ioctl = pppox_ioctl,
+	.listen = sock_no_listen,
+	.shutdown = sock_no_shutdown,
+	.setsockopt = sock_no_setsockopt,
+	.getsockopt = sock_no_getsockopt,
+	.sendmsg = sock_no_sendmsg,
+	.recvmsg = sock_no_recvmsg,
+	.mmap = sock_no_mmap,
+};
+
+static int pppopns_create(struct net *net, struct socket *sock)
+{
+	struct sock *sk;
+
+	sk = sk_alloc(net, PF_PPPOX, GFP_KERNEL, &pppopns_proto);
+	if (!sk)
+		return -ENOMEM;
+
+	sock_init_data(sock, sk);
+	sock->state = SS_UNCONNECTED;
+	sock->ops = &pppopns_proto_ops;
+	sk->sk_protocol = PX_PROTO_OPNS;
+	sk->sk_state = PPPOX_NONE;
+	return 0;
+}
+
+/******************************************************************************/
+
+static struct pppox_proto pppopns_pppox_proto = {
+	.create = pppopns_create,
+	.owner = THIS_MODULE,
+};
+
+static int __init pppopns_init(void)
+{
+	int error;
+
+	error = proto_register(&pppopns_proto, 0);
+	if (error)
+		return error;
+
+	error = register_pppox_proto(PX_PROTO_OPNS, &pppopns_pppox_proto);
+	if (error)
+		proto_unregister(&pppopns_proto);
+	else
+		skb_queue_head_init(&delivery_queue);
+	return error;
+}
+
+static void __exit pppopns_exit(void)
+{
+	unregister_pppox_proto(PX_PROTO_OPNS);
+	proto_unregister(&pppopns_proto);
+}
+
+module_init(pppopns_init);
+module_exit(pppopns_exit);
+
+MODULE_DESCRIPTION("PPP on PPTP Network Server (PPPoPNS)");
+MODULE_AUTHOR("Chia-chi Yeh <chiachi@android.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/pptp.c b/drivers/net/pptp.c
index 89f829f5f725..f8a6853b692e 100644
--- a/drivers/net/pptp.c
+++ b/drivers/net/pptp.c
@@ -423,10 +423,8 @@ static int pptp_bind(struct socket *sock, struct sockaddr *uservaddr,
 	lock_sock(sk);
 
 	opt->src_addr = sp->sa_addr.pptp;
-	if (add_chan(po)) {
-		release_sock(sk);
+	if (add_chan(po))
 		error = -EBUSY;
-	}
 
 	release_sock(sk);
 	return error;
diff --git a/drivers/net/r8169.c b/drivers/net/r8169.c
index 6d657cabb951..a7ff1005ea64 100644
--- a/drivers/net/r8169.c
+++ b/drivers/net/r8169.c
@@ -1327,7 +1327,11 @@ static int rtl8169_set_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
 
 static const char *rtl_lookup_firmware_name(struct rtl8169_private *tp)
 {
+#ifdef CONFIG_R8169_FW_LOAD
 	return rtl_chip_infos[tp->mac_version].fw_name;
+#else
+	return NULL;
+#endif
 }
 
 static void rtl8169_get_drvinfo(struct net_device *dev,
diff --git a/drivers/net/rionet.c b/drivers/net/rionet.c
index 3bb131137033..7145714a5ec9 100644
--- a/drivers/net/rionet.c
+++ b/drivers/net/rionet.c
@@ -88,8 +88,8 @@ static struct rio_dev **rionet_active;
 #define dev_rionet_capable(dev) \
 	is_rionet_capable(dev->src_ops, dev->dst_ops)
 
-#define RIONET_MAC_MATCH(x)	(*(u32 *)x == 0x00010001)
-#define RIONET_GET_DESTID(x)	(*(u16 *)(x + 4))
+#define RIONET_MAC_MATCH(x)	(!memcmp((x), "\00\01\00\01", 4))
+#define RIONET_GET_DESTID(x)	((*((u8 *)x + 4) << 8) | *((u8 *)x + 5))
 
 static int rionet_rx_clean(struct net_device *ndev)
 {
diff --git a/drivers/net/sfc/ethtool.c b/drivers/net/sfc/ethtool.c
index bc4643af6dd1..cd011d086b76 100644
--- a/drivers/net/sfc/ethtool.c
+++ b/drivers/net/sfc/ethtool.c
@@ -943,40 +943,28 @@ static int efx_ethtool_set_rx_ntuple(struct net_device *net_dev,
 	return rc < 0 ? rc : 0;
 }
 
-static int efx_ethtool_get_rxfh_indir(struct net_device *net_dev,
-				      struct ethtool_rxfh_indir *indir)
+static u32 efx_ethtool_get_rxfh_indir_size(struct net_device *net_dev)
 {
 	struct efx_nic *efx = netdev_priv(net_dev);
-	size_t copy_size =
-		min_t(size_t, indir->size, ARRAY_SIZE(efx->rx_indir_table));
 
-	if (efx_nic_rev(efx) < EFX_REV_FALCON_B0)
-		return -EOPNOTSUPP;
+	return (efx_nic_rev(efx) < EFX_REV_FALCON_B0 ?
+		0 : ARRAY_SIZE(efx->rx_indir_table));
+}
+
+static int efx_ethtool_get_rxfh_indir(struct net_device *net_dev, u32 *indir)
+{
+	struct efx_nic *efx = netdev_priv(net_dev);
 
-	indir->size = ARRAY_SIZE(efx->rx_indir_table);
-	memcpy(indir->ring_index, efx->rx_indir_table,
-	       copy_size * sizeof(indir->ring_index[0]));
+	memcpy(indir, efx->rx_indir_table, sizeof(efx->rx_indir_table));
 	return 0;
 }
 
 static int efx_ethtool_set_rxfh_indir(struct net_device *net_dev,
-				      const struct ethtool_rxfh_indir *indir)
+				      const u32 *indir)
 {
 	struct efx_nic *efx = netdev_priv(net_dev);
-	size_t i;
-
-	if (efx_nic_rev(efx) < EFX_REV_FALCON_B0)
-		return -EOPNOTSUPP;
-
-	/* Validate size and indices */
-	if (indir->size != ARRAY_SIZE(efx->rx_indir_table))
-		return -EINVAL;
-	for (i = 0; i < ARRAY_SIZE(efx->rx_indir_table); i++)
-		if (indir->ring_index[i] >= efx->n_rx_channels)
-			return -EINVAL;
 
-	memcpy(efx->rx_indir_table, indir->ring_index,
-	       sizeof(efx->rx_indir_table));
+	memcpy(efx->rx_indir_table, indir, sizeof(efx->rx_indir_table));
 	efx_nic_push_rx_indir_table(efx);
 	return 0;
 }
@@ -1007,6 +995,7 @@ const struct ethtool_ops efx_ethtool_ops = {
 	.reset			= efx_ethtool_reset,
 	.get_rxnfc		= efx_ethtool_get_rxnfc,
 	.set_rx_ntuple		= efx_ethtool_set_rx_ntuple,
+	.get_rxfh_indir_size	= efx_ethtool_get_rxfh_indir_size,
 	.get_rxfh_indir		= efx_ethtool_get_rxfh_indir,
 	.set_rxfh_indir		= efx_ethtool_set_rxfh_indir,
 };
diff --git a/drivers/net/tg3.c b/drivers/net/tg3.c
index c11a2b8327f3..d469004704ad 100644
--- a/drivers/net/tg3.c
+++ b/drivers/net/tg3.c
@@ -6029,12 +6029,12 @@ static void tg3_tx_skb_unmap(struct tg3_napi *tnapi, u32 entry, int last)
 
 /* Workaround 4GB and 40-bit hardware DMA bugs. */
 static int tigon3_dma_hwbug_workaround(struct tg3_napi *tnapi,
-				       struct sk_buff *skb,
+				       struct sk_buff **pskb,
 				       u32 *entry, u32 *budget,
 				       u32 base_flags, u32 mss, u32 vlan)
 {
 	struct tg3 *tp = tnapi->tp;
-	struct sk_buff *new_skb;
+	struct sk_buff *new_skb, *skb = *pskb;
 	dma_addr_t new_addr = 0;
 	int ret = 0;
 
@@ -6076,7 +6076,7 @@ static int tigon3_dma_hwbug_workaround(struct tg3_napi *tnapi,
 	}
 
 	dev_kfree_skb(skb);
-
+	*pskb = new_skb;
 	return ret;
 }
 
@@ -6305,7 +6305,7 @@ static netdev_tx_t tg3_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		 */
 		entry = tnapi->tx_prod;
 		budget = tg3_tx_avail(tnapi);
-		if (tigon3_dma_hwbug_workaround(tnapi, skb, &entry, &budget,
+		if (tigon3_dma_hwbug_workaround(tnapi, &skb, &entry, &budget,
 						base_flags, mss, vlan))
 			goto out_unlock;
 	}
diff --git a/drivers/net/tun.c b/drivers/net/tun.c
index 71f3d1a35b74..a024708b5704 100644
--- a/drivers/net/tun.c
+++ b/drivers/net/tun.c
@@ -1243,6 +1243,12 @@ static long __tun_chr_ioctl(struct file *file, unsigned int cmd,
 	int vnet_hdr_sz;
 	int ret;
 
+#ifdef CONFIG_ANDROID_PARANOID_NETWORK
+	if (cmd != TUNGETIFF && !capable(CAP_NET_ADMIN)) {
+		return -EPERM;
+	}
+#endif
+
 	if (cmd == TUNSETIFF || _IOC_TYPE(cmd) == 0x89)
 		if (copy_from_user(&ifr, argp, ifreq_len))
 			return -EFAULT;
diff --git a/drivers/net/usb/Kconfig b/drivers/net/usb/Kconfig
index 84d4608153c9..7f0818038aa3 100644
--- a/drivers/net/usb/Kconfig
+++ b/drivers/net/usb/Kconfig
@@ -458,4 +458,12 @@ config USB_VL600
 
 	  http://ubuntuforums.org/showpost.php?p=10589647&postcount=17
 
+config USB_NET_RAW_IP
+	tristate "RAW-IP Driver for XMM6260 modems"
+	help
+	  Choose this option if you have a XMM6260 modem device with RAW-IP suport.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called rmnet.
+
 endmenu
diff --git a/drivers/net/usb/Makefile b/drivers/net/usb/Makefile
index c203fa21f6b1..ea9665cb8492 100644
--- a/drivers/net/usb/Makefile
+++ b/drivers/net/usb/Makefile
@@ -29,4 +29,5 @@ obj-$(CONFIG_USB_SIERRA_NET)	+= sierra_net.o
 obj-$(CONFIG_USB_NET_CX82310_ETH)	+= cx82310_eth.o
 obj-$(CONFIG_USB_NET_CDC_NCM)	+= cdc_ncm.o
 obj-$(CONFIG_USB_VL600)		+= lg-vl600.o
+obj-$(CONFIG_USB_NET_RAW_IP)	+= raw_ip_net.o
 
diff --git a/drivers/net/usb/asix.c b/drivers/net/usb/asix.c
index c5c4b4def7fb..f2d5822338d7 100644
--- a/drivers/net/usb/asix.c
+++ b/drivers/net/usb/asix.c
@@ -1,8 +1,7 @@
 /*
  * ASIX AX8817X based USB 2.0 Ethernet Devices
- * Copyright (C) 2003-2006 David Hollis <dhollis@davehollis.com>
+ * Copyright (C) 2003-2005 David Hollis <dhollis@davehollis.com>
  * Copyright (C) 2005 Phil Chang <pchang23@sbcglobal.net>
- * Copyright (C) 2006 James Painter <jamie.painter@iname.com>
  * Copyright (c) 2002-2003 TiVo Inc.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -20,11 +19,16 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
-// #define	DEBUG			// error path messages, extra info
-// #define	VERBOSE			// more; success messages
+//#define	DEBUG			// debug messages, extra info
 
+#include <linux/version.h>
+//#include <linux/config.h>
+#ifdef	CONFIG_USB_DEBUG
+#   define DEBUG
+#endif
 #include <linux/module.h>
 #include <linux/kmod.h>
+#include <linux/sched.h>
 #include <linux/init.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
@@ -33,690 +37,863 @@
 #include <linux/mii.h>
 #include <linux/usb.h>
 #include <linux/crc32.h>
-#include <linux/usb/usbnet.h>
-#include <linux/slab.h>
 
-#define DRIVER_VERSION "14-Jun-2006"
-static const char driver_name [] = "asix";
-
-/* ASIX AX8817X based USB 2.0 Ethernet Devices */
+#include "axusbnet.c"
+#include "asix.h"
+
+#define DRV_VERSION	"4.4.0"
+
+static char version[] =
+KERN_INFO "ASIX USB Ethernet Adapter:v" DRV_VERSION 
+	" " __TIME__ " " __DATE__ "\n"
+KERN_INFO "    http://www.asix.com.tw\n";
+
+static char g_mac_addr[ETH_ALEN];
+static int g_usr_mac = 0;
+
+/* configuration of maximum bulk in size */
+static int bsize = AX88772B_MAX_BULKIN_16K;
+module_param (bsize, int, 0);
+MODULE_PARM_DESC (bsize, "Maximum transfer size per bulk");
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void ax88772b_link_reset (void *data);
+static void ax88772a_link_reset (void *data);
+static void ax88772_link_reset (void *data);
+#else
+static void ax88772b_link_reset (struct work_struct *work);
+static void ax88772a_link_reset (struct work_struct *work);
+static void ax88772_link_reset (struct work_struct *work);
+#endif
+static int ax88772a_phy_powerup (struct usbnet *dev);
+
+/* Retrieve user set MAC address */
+static int __init setup_asix_mac(char *macstr)
+{
+	int i, j;
+	unsigned char result, value;
+
+	for (i = 0; i < ETH_ALEN; i++) {
+		result = 0;
+
+		if (i != 5 && *(macstr + 2) != ':')
+			return -1;
+
+		for (j = 0; j < 2; j++) {
+			if (isxdigit(*macstr)
+			    && (value =
+				isdigit(*macstr) ? *macstr -
+				'0' : toupper(*macstr) - 'A' + 10) < 16) {
+				result = result * 16 + value;
+				macstr++;
+			} else
+				return -1;
+		}
 
-#define AX_CMD_SET_SW_MII		0x06
-#define AX_CMD_READ_MII_REG		0x07
-#define AX_CMD_WRITE_MII_REG		0x08
-#define AX_CMD_SET_HW_MII		0x0a
-#define AX_CMD_READ_EEPROM		0x0b
-#define AX_CMD_WRITE_EEPROM		0x0c
-#define AX_CMD_WRITE_ENABLE		0x0d
-#define AX_CMD_WRITE_DISABLE		0x0e
-#define AX_CMD_READ_RX_CTL		0x0f
-#define AX_CMD_WRITE_RX_CTL		0x10
-#define AX_CMD_READ_IPG012		0x11
-#define AX_CMD_WRITE_IPG0		0x12
-#define AX_CMD_WRITE_IPG1		0x13
-#define AX_CMD_READ_NODE_ID		0x13
-#define AX_CMD_WRITE_NODE_ID		0x14
-#define AX_CMD_WRITE_IPG2		0x14
-#define AX_CMD_WRITE_MULTI_FILTER	0x16
-#define AX88172_CMD_READ_NODE_ID	0x17
-#define AX_CMD_READ_PHY_ID		0x19
-#define AX_CMD_READ_MEDIUM_STATUS	0x1a
-#define AX_CMD_WRITE_MEDIUM_MODE	0x1b
-#define AX_CMD_READ_MONITOR_MODE	0x1c
-#define AX_CMD_WRITE_MONITOR_MODE	0x1d
-#define AX_CMD_READ_GPIOS		0x1e
-#define AX_CMD_WRITE_GPIOS		0x1f
-#define AX_CMD_SW_RESET			0x20
-#define AX_CMD_SW_PHY_STATUS		0x21
-#define AX_CMD_SW_PHY_SELECT		0x22
-
-#define AX_MONITOR_MODE			0x01
-#define AX_MONITOR_LINK			0x02
-#define AX_MONITOR_MAGIC		0x04
-#define AX_MONITOR_HSFS			0x10
-
-/* AX88172 Medium Status Register values */
-#define AX88172_MEDIUM_FD		0x02
-#define AX88172_MEDIUM_TX		0x04
-#define AX88172_MEDIUM_FC		0x10
-#define AX88172_MEDIUM_DEFAULT \
-		( AX88172_MEDIUM_FD | AX88172_MEDIUM_TX | AX88172_MEDIUM_FC )
-
-#define AX_MCAST_FILTER_SIZE		8
-#define AX_MAX_MCAST			64
-
-#define AX_SWRESET_CLEAR		0x00
-#define AX_SWRESET_RR			0x01
-#define AX_SWRESET_RT			0x02
-#define AX_SWRESET_PRTE			0x04
-#define AX_SWRESET_PRL			0x08
-#define AX_SWRESET_BZ			0x10
-#define AX_SWRESET_IPRL			0x20
-#define AX_SWRESET_IPPD			0x40
-
-#define AX88772_IPG0_DEFAULT		0x15
-#define AX88772_IPG1_DEFAULT		0x0c
-#define AX88772_IPG2_DEFAULT		0x12
-
-/* AX88772 & AX88178 Medium Mode Register */
-#define AX_MEDIUM_PF		0x0080
-#define AX_MEDIUM_JFE		0x0040
-#define AX_MEDIUM_TFC		0x0020
-#define AX_MEDIUM_RFC		0x0010
-#define AX_MEDIUM_ENCK		0x0008
-#define AX_MEDIUM_AC		0x0004
-#define AX_MEDIUM_FD		0x0002
-#define AX_MEDIUM_GM		0x0001
-#define AX_MEDIUM_SM		0x1000
-#define AX_MEDIUM_SBP		0x0800
-#define AX_MEDIUM_PS		0x0200
-#define AX_MEDIUM_RE		0x0100
-
-#define AX88178_MEDIUM_DEFAULT	\
-	(AX_MEDIUM_PS | AX_MEDIUM_FD | AX_MEDIUM_AC | \
-	 AX_MEDIUM_RFC | AX_MEDIUM_TFC | AX_MEDIUM_JFE | \
-	 AX_MEDIUM_RE )
-
-#define AX88772_MEDIUM_DEFAULT	\
-	(AX_MEDIUM_FD | AX_MEDIUM_RFC | \
-	 AX_MEDIUM_TFC | AX_MEDIUM_PS | \
-	 AX_MEDIUM_AC | AX_MEDIUM_RE )
-
-/* AX88772 & AX88178 RX_CTL values */
-#define AX_RX_CTL_SO			0x0080
-#define AX_RX_CTL_AP			0x0020
-#define AX_RX_CTL_AM			0x0010
-#define AX_RX_CTL_AB			0x0008
-#define AX_RX_CTL_SEP			0x0004
-#define AX_RX_CTL_AMALL			0x0002
-#define AX_RX_CTL_PRO			0x0001
-#define AX_RX_CTL_MFB_2048		0x0000
-#define AX_RX_CTL_MFB_4096		0x0100
-#define AX_RX_CTL_MFB_8192		0x0200
-#define AX_RX_CTL_MFB_16384		0x0300
-
-#define AX_DEFAULT_RX_CTL	\
-	(AX_RX_CTL_SO | AX_RX_CTL_AB )
-
-/* GPIO 0 .. 2 toggles */
-#define AX_GPIO_GPO0EN		0x01	/* GPIO0 Output enable */
-#define AX_GPIO_GPO_0		0x02	/* GPIO0 Output value */
-#define AX_GPIO_GPO1EN		0x04	/* GPIO1 Output enable */
-#define AX_GPIO_GPO_1		0x08	/* GPIO1 Output value */
-#define AX_GPIO_GPO2EN		0x10	/* GPIO2 Output enable */
-#define AX_GPIO_GPO_2		0x20	/* GPIO2 Output value */
-#define AX_GPIO_RESERVED	0x40	/* Reserved */
-#define AX_GPIO_RSE		0x80	/* Reload serial EEPROM */
-
-#define AX_EEPROM_MAGIC		0xdeadbeef
-#define AX88172_EEPROM_LEN	0x40
-#define AX88772_EEPROM_LEN	0xff
-
-#define PHY_MODE_MARVELL	0x0000
-#define MII_MARVELL_LED_CTRL	0x0018
-#define MII_MARVELL_STATUS	0x001b
-#define MII_MARVELL_CTRL	0x0014
-
-#define MARVELL_LED_MANUAL	0x0019
-
-#define MARVELL_STATUS_HWCFG	0x0004
-
-#define MARVELL_CTRL_TXDELAY	0x0002
-#define MARVELL_CTRL_RXDELAY	0x0080
-
-/* This structure cannot exceed sizeof(unsigned long [5]) AKA 20 bytes */
-struct asix_data {
-	u8 multi_filter[AX_MCAST_FILTER_SIZE];
-	u8 mac_addr[ETH_ALEN];
-	u8 phymode;
-	u8 ledmode;
-	u8 eeprom_len;
-};
+		macstr++;
+		g_mac_addr[i] = result;
+	}
 
-struct ax88172_int_data {
-	__le16 res1;
-	u8 link;
-	__le16 res2;
-	u8 status;
-	__le16 res3;
-} __packed;
+	g_usr_mac = 1;
 
-static int asix_read_cmd(struct usbnet *dev, u8 cmd, u16 value, u16 index,
-			    u16 size, void *data)
-{
-	void *buf;
-	int err = -ENOMEM;
+	return 0;
+}
 
-	netdev_dbg(dev->net, "asix_read_cmd() cmd=0x%02x value=0x%04x index=0x%04x size=%d\n",
-		   cmd, value, index, size);
+__setup("asix_mac=", setup_asix_mac);
 
-	buf = kmalloc(size, GFP_KERNEL);
-	if (!buf)
-		goto out;
+/* ASIX AX8817X based USB 2.0 Ethernet Devices */
 
-	err = usb_control_msg(
+static int ax8817x_read_cmd(struct usbnet *dev, u8 cmd, u16 value, u16 index,
+			    u16 size, void *data)
+{
+	return usb_control_msg(
 		dev->udev,
 		usb_rcvctrlpipe(dev->udev, 0),
 		cmd,
 		USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
 		value,
 		index,
-		buf,
+		data,
 		size,
 		USB_CTRL_GET_TIMEOUT);
-	if (err == size)
-		memcpy(data, buf, size);
-	else if (err >= 0)
-		err = -EINVAL;
-	kfree(buf);
-
-out:
-	return err;
 }
 
-static int asix_write_cmd(struct usbnet *dev, u8 cmd, u16 value, u16 index,
+static int ax8817x_write_cmd(struct usbnet *dev, u8 cmd, u16 value, u16 index,
 			     u16 size, void *data)
 {
-	void *buf = NULL;
-	int err = -ENOMEM;
-
-	netdev_dbg(dev->net, "asix_write_cmd() cmd=0x%02x value=0x%04x index=0x%04x size=%d\n",
-		   cmd, value, index, size);
-
-	if (data) {
-		buf = kmemdup(data, size, GFP_KERNEL);
-		if (!buf)
-			goto out;
-	}
-
-	err = usb_control_msg(
+	return usb_control_msg(
 		dev->udev,
 		usb_sndctrlpipe(dev->udev, 0),
 		cmd,
 		USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
 		value,
 		index,
-		buf,
+		data,
 		size,
 		USB_CTRL_SET_TIMEOUT);
-	kfree(buf);
-
-out:
-	return err;
 }
 
-static void asix_async_cmd_callback(struct urb *urb)
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void ax8817x_async_cmd_callback(struct urb *urb, struct pt_regs *regs)
+#else
+static void ax8817x_async_cmd_callback(struct urb *urb)
+#endif
 {
 	struct usb_ctrlrequest *req = (struct usb_ctrlrequest *)urb->context;
-	int status = urb->status;
 
-	if (status < 0)
-		printk(KERN_DEBUG "asix_async_cmd_callback() failed with %d",
-			status);
+	if (urb->status < 0)
+		printk(KERN_DEBUG "ax8817x_async_cmd_callback() failed with %d",
+			urb->status);
 
 	kfree(req);
 	usb_free_urb(urb);
 }
 
-static void
-asix_write_cmd_async(struct usbnet *dev, u8 cmd, u16 value, u16 index,
-				    u16 size, void *data)
+static int ax8817x_set_mac_addr (struct net_device *net, void *p)
 {
-	struct usb_ctrlrequest *req;
-	int status;
-	struct urb *urb;
+	struct usbnet *dev = netdev_priv(net);
+	struct sockaddr *addr = p;
 
-	netdev_dbg(dev->net, "asix_write_cmd_async() cmd=0x%02x value=0x%04x index=0x%04x size=%d\n",
-		   cmd, value, index, size);
-	if ((urb = usb_alloc_urb(0, GFP_ATOMIC)) == NULL) {
-		netdev_err(dev->net, "Error allocating URB in write_cmd_async!\n");
+	memcpy (net->dev_addr, addr->sa_data, ETH_ALEN);
+
+	/* Set the MAC address */
+	return ax8817x_write_cmd (dev, AX88772_CMD_WRITE_NODE_ID,
+			   0, 0, ETH_ALEN, net->dev_addr);
+
+}
+
+static void ax88178_status(struct usbnet *dev, struct urb *urb)
+{
+	struct ax88172_int_data *event;
+	struct ax88178_data *ax178dataptr = (struct ax88178_data *)dev->priv;
+	int link;
+
+	if (urb->actual_length < 8)
 		return;
-	}
 
-	if ((req = kmalloc(sizeof(struct usb_ctrlrequest), GFP_ATOMIC)) == NULL) {
-		netdev_err(dev->net, "Failed to allocate memory for control request\n");
-		usb_free_urb(urb);
+	if (ax178dataptr->EepromData == PHY_MODE_MAC_TO_MAC_GMII)
 		return;
+
+	event = urb->transfer_buffer;
+	link = event->link & 0x01;
+	if (netif_carrier_ok(dev->net) != link) {
+		if (link) {
+			netif_carrier_on(dev->net);
+			axusbnet_defer_kevent (dev, EVENT_LINK_RESET);
+		} else
+			netif_carrier_off(dev->net);
+		devwarn(dev, "ax88178 - Link status is: %d", link);
 	}
+}
 
-	req->bRequestType = USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE;
-	req->bRequest = cmd;
-	req->wValue = cpu_to_le16(value);
-	req->wIndex = cpu_to_le16(index);
-	req->wLength = cpu_to_le16(size);
+static void ax8817x_status(struct usbnet *dev, struct urb *urb)
+{
+	struct ax88172_int_data *event;
+	int link;
 
-	usb_fill_control_urb(urb, dev->udev,
-			     usb_sndctrlpipe(dev->udev, 0),
-			     (void *)req, data, size,
-			     asix_async_cmd_callback, req);
+	if (urb->actual_length < 8)
+		return;
 
-	if((status = usb_submit_urb(urb, GFP_ATOMIC)) < 0) {
-		netdev_err(dev->net, "Error submitting the control message: status=%d\n",
-			   status);
-		kfree(req);
-		usb_free_urb(urb);
+	event = urb->transfer_buffer;
+	link = event->link & 0x01;
+	if (netif_carrier_ok(dev->net) != link) {
+		if (link) {
+			netif_carrier_on(dev->net);
+			axusbnet_defer_kevent (dev, EVENT_LINK_RESET );
+		} else
+			netif_carrier_off(dev->net);
+		devwarn(dev, "ax8817x - Link status is: %d", link);
 	}
 }
 
-static int asix_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
+static void ax88772_status(struct usbnet *dev, struct urb *urb)
 {
-	u8  *head;
-	u32  header;
-	char *packet;
-	struct sk_buff *ax_skb;
-	u16 size;
+	struct ax88172_int_data *event;
+	struct ax88772_data *ax772_data = (struct ax88772_data *)dev->priv;
+	int link;
+	
+	if (urb->actual_length < 8)
+		return;
 
-	head = (u8 *) skb->data;
-	memcpy(&header, head, sizeof(header));
-	le32_to_cpus(&header);
-	packet = head + sizeof(header);
+	event = urb->transfer_buffer;
+	link = event->link & 0x01;
+	
+	if (netif_carrier_ok(dev->net) != link) {
+		if (link) {
+			netif_carrier_on(dev->net);
+			ax772_data->Event = AX_SET_RX_CFG;
+		} else {
+			netif_carrier_off(dev->net);
+			if (ax772_data->Event == AX_NOP) {
+				ax772_data->Event = PHY_POWER_DOWN;
+				ax772_data->TickToExpire = 25;
+			}
+		}
 
-	skb_pull(skb, 4);
+		devwarn(dev, "ax88772 - Link status is: %d", link);
+	}
+	
+	if (ax772_data->Event)
+		queue_work (ax772_data->ax_work, &ax772_data->check_link);
+}
 
-	while (skb->len > 0) {
-		if ((header & 0x07ff) != ((~header >> 16) & 0x07ff))
-			netdev_err(dev->net, "asix_rx_fixup() Bad Header Length\n");
+static void ax88772a_status(struct usbnet *dev, struct urb *urb)
+{
+	struct ax88172_int_data *event;
+	struct ax88772a_data *ax772a_data = (struct ax88772a_data *)dev->priv;
+	int link;
+	int PowSave = (ax772a_data->EepromData >> 14);
 
-		/* get the packet length */
-		size = (u16) (header & 0x000007ff);
+	if (urb->actual_length < 8)
+		return;
 
-		if ((skb->len) - ((size + 1) & 0xfffe) == 0) {
-			u8 alignment = (unsigned long)skb->data & 0x3;
-			if (alignment != 0x2) {
-				/*
-				 * not 16bit aligned so use the room provided by
-				 * the 32 bit header to align the data
-				 *
-				 * note we want 16bit alignment as MAC header is
-				 * 14bytes thus ip header will be aligned on
-				 * 32bit boundary so accessing ipheader elements
-				 * using a cast to struct ip header wont cause
-				 * an unaligned accesses.
-				 */
-				u8 realignment = (alignment + 2) & 0x3;
-				memmove(skb->data - realignment,
-					skb->data,
-					size);
-				skb->data -= realignment;
-				skb_set_tail_pointer(skb, size);
-			}
-			return 2;
-		}
+	event = urb->transfer_buffer;
+	link = event->link & 0x01;
 
-		if (size > dev->net->mtu + ETH_HLEN) {
-			netdev_err(dev->net, "asix_rx_fixup() Bad RX Length %d\n",
-				   size);
-			return 0;
-		}
-		ax_skb = skb_clone(skb, GFP_ATOMIC);
-		if (ax_skb) {
-			u8 alignment = (unsigned long)packet & 0x3;
-			ax_skb->len = size;
+	if (netif_carrier_ok(dev->net) != link) {
 
-			if (alignment != 0x2) {
-				/*
-				 * not 16bit aligned use the room provided by
-				 * the 32 bit header to align the data
-				 */
-				u8 realignment = (alignment + 2) & 0x3;
-				memmove(packet - realignment, packet, size);
-				packet -= realignment;
+		if (link) {
+			netif_carrier_on(dev->net);
+			ax772a_data->Event = AX_SET_RX_CFG;
+		} else if ((PowSave == 0x3) || (PowSave == 0x1)) {
+			netif_carrier_off(dev->net);
+			if (ax772a_data->Event == AX_NOP) {
+				ax772a_data->Event = CHK_CABLE_EXIST;
+				ax772a_data->TickToExpire = 14;
 			}
-			ax_skb->data = packet;
-			skb_set_tail_pointer(ax_skb, size);
-			usbnet_skb_return(dev, ax_skb);
 		} else {
-			return 0;
+			netif_carrier_off(dev->net);
+			ax772a_data->Event = AX_NOP;
 		}
 
-		skb_pull(skb, (size + 1) & 0xfffe);
-
-		if (skb->len == 0)
-			break;
-
-		head = (u8 *) skb->data;
-		memcpy(&header, head, sizeof(header));
-		le32_to_cpus(&header);
-		packet = head + sizeof(header);
-		skb_pull(skb, 4);
-	}
-
-	if (skb->len < 0) {
-		netdev_err(dev->net, "asix_rx_fixup() Bad SKB Length %d\n",
-			   skb->len);
-		return 0;
+		devwarn(dev, "ax88772a - Link status is: %d", link);
 	}
-	return 1;
+	
+	if (ax772a_data->Event)
+		queue_work (ax772a_data->ax_work, &ax772a_data->check_link);
 }
 
-static struct sk_buff *asix_tx_fixup(struct usbnet *dev, struct sk_buff *skb,
-					gfp_t flags)
+static int ax88772b_stop(struct usbnet *dev)
 {
-	int padlen;
-	int headroom = skb_headroom(skb);
-	int tailroom = skb_tailroom(skb);
-	u32 packet_len;
-	u32 padbytes = 0xffff0000;
+	u16 *medium;
 
-	padlen = ((skb->len + 4) % 512) ? 0 : 4;
+	medium = kmalloc (2, GFP_ATOMIC);
+	if (medium) {
+		ax8817x_read_cmd (dev, AX_CMD_READ_MEDIUM_MODE, 0, 0, 2, medium);
+		ax8817x_write_cmd (dev, AX_CMD_WRITE_MEDIUM_MODE,
+				(*medium & ~AX88772_MEDIUM_RX_ENABLE), 0, 0, NULL);
 
-	if ((!skb_cloned(skb)) &&
-	    ((headroom + tailroom) >= (4 + padlen))) {
-		if ((headroom < 4) || (tailroom < padlen)) {
-			skb->data = memmove(skb->head + 4, skb->data, skb->len);
-			skb_set_tail_pointer(skb, skb->len);
-		}
-	} else {
-		struct sk_buff *skb2;
-		skb2 = skb_copy_expand(skb, 4, padlen, flags);
-		dev_kfree_skb_any(skb);
-		skb = skb2;
-		if (!skb)
-			return NULL;
+		kfree (medium);
+		return 0;
 	}
+	return -EINVAL;
+	
+}
 
-	skb_push(skb, 4);
-	packet_len = (((skb->len - 4) ^ 0x0000ffff) << 16) + (skb->len - 4);
-	cpu_to_le32s(&packet_len);
-	skb_copy_to_linear_data(skb, &packet_len, sizeof(packet_len));
+static int ax88772b_reset(struct usbnet *dev)
+{
+	int ret;
 
-	if ((skb->len % 512) == 0) {
-		cpu_to_le32s(&padbytes);
-		memcpy(skb_tail_pointer(skb), &padbytes, sizeof(padbytes));
-		skb_put(skb, sizeof(padbytes));
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_MEDIUM_MODE,
+				AX88772_MEDIUM_DEFAULT, 0, 0, NULL)) < 0) {
+		deverr(dev, "Write medium mode register: %d", ret);
 	}
-	return skb;
+	return ret;
+	
 }
 
-static void asix_status(struct usbnet *dev, struct urb *urb)
+
+static void ax88772b_status(struct usbnet *dev, struct urb *urb)
 {
+	struct ax88772b_data *ax772b_data = (struct ax88772b_data *)dev->priv;
 	struct ax88172_int_data *event;
 	int link;
 
 	if (urb->actual_length < 8)
 		return;
 
+	if (ax772b_data->OperationMode == OPERATION_PHY_MODE)
+		return;
+
 	event = urb->transfer_buffer;
-	link = event->link & 0x01;
+	if (ax772b_data->PhySelect == 0 && ax772b_data->OperationMode 
+							== OPERATION_MAC_MODE)
+		link = event->link & AX_INT_SPLS_LINK;
+	else
+		link = event->link & AX_INT_PPLS_LINK;
+
 	if (netif_carrier_ok(dev->net) != link) {
 		if (link) {
 			netif_carrier_on(dev->net);
-			usbnet_defer_kevent (dev, EVENT_LINK_RESET );
-		} else
+			ax772b_data->Event = AX_SET_RX_CFG;
+		} else {
 			netif_carrier_off(dev->net);
-		netdev_dbg(dev->net, "Link Status is: %d\n", link);
+			ax772b_data->time_to_chk = jiffies;
+		}
+		devwarn(dev, "ax88772b - Link status is: %d", link);
 	}
-}
-
-static inline int asix_set_sw_mii(struct usbnet *dev)
-{
-	int ret;
-	ret = asix_write_cmd(dev, AX_CMD_SET_SW_MII, 0x0000, 0, 0, NULL);
-	if (ret < 0)
-		netdev_err(dev->net, "Failed to enable software MII access\n");
-	return ret;
-}
 
-static inline int asix_set_hw_mii(struct usbnet *dev)
-{
-	int ret;
-	ret = asix_write_cmd(dev, AX_CMD_SET_HW_MII, 0x0000, 0, 0, NULL);
-	if (ret < 0)
-		netdev_err(dev->net, "Failed to enable hardware MII access\n");
-	return ret;
-}
-
-static inline int asix_get_phy_addr(struct usbnet *dev)
-{
-	u8 buf[2];
-	int ret = asix_read_cmd(dev, AX_CMD_READ_PHY_ID, 0, 0, 2, buf);
+	if (!link) {
 
-	netdev_dbg(dev->net, "asix_get_phy_addr()\n");
+		int no_cable = (event->link & AX_INT_CABOFF_UNPLUG) ? 1 : 0;
 
-	if (ret < 0) {
-		netdev_err(dev->net, "Error reading PHYID register: %02x\n", ret);
-		goto out;
+		if (no_cable) {
+			if ((ax772b_data->psc & 
+			    (AX_SWRESET_IPPSL_0 | AX_SWRESET_IPPSL_1)) &&
+			     !ax772b_data->pw_enabled) {
+				/* 
+				 * AX88772B already entered power saving state
+				 */
+				ax772b_data->pw_enabled = 1;
+			}
+/* Disable failing auto detach stuff for now */
+#if 0
+			ax772b_data->Event = AX_CHK_AUTODETACH;
+#endif
+		} else {
+			/* AX88772B resumed from power saving state */
+			if (ax772b_data->pw_enabled || 
+				(jiffies > (ax772b_data->time_to_chk + 
+				 AX88772B_WATCHDOG))) {
+				if (ax772b_data->pw_enabled)
+					ax772b_data->pw_enabled = 0;
+				ax772b_data->Event = PHY_POWER_UP;
+				ax772b_data->time_to_chk = jiffies;
+			}
+		}
 	}
-	netdev_dbg(dev->net, "asix_get_phy_addr() returning 0x%04x\n",
-		   *((__le16 *)buf));
-	ret = buf[1];
 
-out:
-	return ret;
+	if (ax772b_data->Event)
+		queue_work (ax772b_data->ax_work, &ax772b_data->check_link);
 }
 
-static int asix_sw_reset(struct usbnet *dev, u8 flags)
+void
+ax8817x_write_cmd_async(struct usbnet *dev, u8 cmd, u16 value, u16 index,
+				    u16 size, void *data)
 {
-	int ret;
+	struct usb_ctrlrequest *req;
+	int status;
+	struct urb *urb;
 
-        ret = asix_write_cmd(dev, AX_CMD_SW_RESET, flags, 0, 0, NULL);
-	if (ret < 0)
-		netdev_err(dev->net, "Failed to send software reset: %02x\n", ret);
+	if ((urb = usb_alloc_urb(0, GFP_ATOMIC)) == NULL) {
+		deverr(dev, "Error allocating URB in write_cmd_async!");
+		return;
+	}
 
-	return ret;
-}
+	if ((req = kmalloc (sizeof (struct usb_ctrlrequest),
+			    GFP_ATOMIC)) == NULL) {
+		deverr(dev, "Failed to allocate memory for control request");
+		usb_free_urb(urb);
+		return;
+	}
 
-static u16 asix_read_rx_ctl(struct usbnet *dev)
-{
-	__le16 v;
-	int ret = asix_read_cmd(dev, AX_CMD_READ_RX_CTL, 0, 0, 2, &v);
+	req->bRequestType = USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE;
+	req->bRequest = cmd;
+	req->wValue = cpu_to_le16(value);
+	req->wIndex = cpu_to_le16(index);
+	req->wLength = cpu_to_le16(size);
+
+	usb_fill_control_urb(urb, dev->udev,
+			     usb_sndctrlpipe(dev->udev, 0),
+			     (void *)req, data, size,
+			     ax8817x_async_cmd_callback, req);
 
-	if (ret < 0) {
-		netdev_err(dev->net, "Error reading RX_CTL register: %02x\n", ret);
-		goto out;
+	if((status = usb_submit_urb(urb, GFP_ATOMIC)) < 0) {
+		deverr(dev, "Error submitting the control message: status=%d",
+				status);
+		kfree(req);
+		usb_free_urb(urb);
 	}
-	ret = le16_to_cpu(v);
-out:
-	return ret;
 }
 
-static int asix_write_rx_ctl(struct usbnet *dev, u16 mode)
+static void ax8817x_set_multicast(struct net_device *net)
 {
-	int ret;
+	struct usbnet *dev = netdev_priv(net);
+	struct ax8817x_data *data = (struct ax8817x_data *)&dev->data;
+	u8 rx_ctl = AX_RX_CTL_START | AX_RX_CTL_AB;
+	int mc_count;
 
-	netdev_dbg(dev->net, "asix_write_rx_ctl() - mode = 0x%04x\n", mode);
-	ret = asix_write_cmd(dev, AX_CMD_WRITE_RX_CTL, mode, 0, 0, NULL);
-	if (ret < 0)
-		netdev_err(dev->net, "Failed to write RX_CTL mode to 0x%04x: %02x\n",
-			   mode, ret);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35)
+	mc_count = net->mc_count;
+#else
+	mc_count = netdev_mc_count (net);
+#endif
 
-	return ret;
-}
+	if (net->flags & IFF_PROMISC) {
+		rx_ctl |= AX_RX_CTL_PRO;
+	} else if (net->flags & IFF_ALLMULTI
+		   || mc_count > AX_MAX_MCAST) {
+		rx_ctl |= AX_RX_CTL_AMALL;
+	} else if (mc_count == 0) {
+		/* just broadcast and directed */
+	} else {
+		/* We use the 20 byte dev->data
+		 * for our 8 byte filter buffer
+		 * to avoid allocating memory that
+		 * is tricky to free later */
+		u32 crc_bits;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35)
+		struct dev_mc_list *mc_list = net->mc_list;
+		int i;
 
-static u16 asix_read_medium_status(struct usbnet *dev)
-{
-	__le16 v;
-	int ret = asix_read_cmd(dev, AX_CMD_READ_MEDIUM_STATUS, 0, 0, 2, &v);
+		memset(data->multi_filter, 0, AX_MCAST_FILTER_SIZE);
+
+		/* Build the multicast hash filter. */
+		for (i = 0; i < net->mc_count; i++) {
+			crc_bits =
+			    ether_crc(ETH_ALEN,
+				      mc_list->dmi_addr) >> 26;
+			data->multi_filter[crc_bits >> 3] |=
+			    1 << (crc_bits & 7);
+			mc_list = mc_list->next;
+		}
+#else
+		struct netdev_hw_addr *ha;
+		memset(data->multi_filter, 0, AX_MCAST_FILTER_SIZE);
+		netdev_for_each_mc_addr (ha, net) {
+			crc_bits = ether_crc(ETH_ALEN, ha->addr) >> 26;
+			data->multi_filter[crc_bits >> 3] |=
+				1 << (crc_bits & 7);
+		}
+#endif
+		ax8817x_write_cmd_async(dev, AX_CMD_WRITE_MULTI_FILTER, 0, 0,
+				   AX_MCAST_FILTER_SIZE, data->multi_filter);
 
-	if (ret < 0) {
-		netdev_err(dev->net, "Error reading Medium Status register: %02x\n",
-			   ret);
-		goto out;
+		rx_ctl |= AX_RX_CTL_AM;
 	}
-	ret = le16_to_cpu(v);
-out:
-	return ret;
+
+	ax8817x_write_cmd_async(dev, AX_CMD_WRITE_RX_CTL, rx_ctl, 0, 0, NULL);
 }
 
-static int asix_write_medium_mode(struct usbnet *dev, u16 mode)
+static void ax88178_set_multicast(struct net_device *net)
 {
-	int ret;
+	struct usbnet *dev = netdev_priv(net);
+	struct ax8817x_data *data = (struct ax8817x_data *)&dev->data;
+	u16 rx_ctl = (AX_RX_CTL_START | AX_RX_CTL_AB |  AX_RX_CTL_MFB);
+	int mc_count;
 
-	netdev_dbg(dev->net, "asix_write_medium_mode() - mode = 0x%04x\n", mode);
-	ret = asix_write_cmd(dev, AX_CMD_WRITE_MEDIUM_MODE, mode, 0, 0, NULL);
-	if (ret < 0)
-		netdev_err(dev->net, "Failed to write Medium Mode mode to 0x%04x: %02x\n",
-			   mode, ret);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35)
+	mc_count = net->mc_count;
+#else
+	mc_count = netdev_mc_count (net);
+#endif
 
-	return ret;
-}
+	if (net->flags & IFF_PROMISC) {
+		rx_ctl |= AX_RX_CTL_PRO;
+	} else if (net->flags & IFF_ALLMULTI
+		   || mc_count > AX_MAX_MCAST) {
+		rx_ctl |= AX_RX_CTL_AMALL;
+	} else if (mc_count == 0) {
+		/* just broadcast and directed */
+	} else {
+		/* We use the 20 byte dev->data
+		 * for our 8 byte filter buffer
+		 * to avoid allocating memory that
+		 * is tricky to free later */
+		u32 crc_bits;
 
-static int asix_write_gpio(struct usbnet *dev, u16 value, int sleep)
-{
-	int ret;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35)
+		struct dev_mc_list *mc_list = net->mc_list;
+		int i;
+
+		memset(data->multi_filter, 0, AX_MCAST_FILTER_SIZE);
 
-	netdev_dbg(dev->net, "asix_write_gpio() - value = 0x%04x\n", value);
-	ret = asix_write_cmd(dev, AX_CMD_WRITE_GPIOS, value, 0, 0, NULL);
-	if (ret < 0)
-		netdev_err(dev->net, "Failed to write GPIO value 0x%04x: %02x\n",
-			   value, ret);
+		/* Build the multicast hash filter. */
+		for (i = 0; i < net->mc_count; i++) {
+			crc_bits =
+			    ether_crc(ETH_ALEN,
+				      mc_list->dmi_addr) >> 26;
+			data->multi_filter[crc_bits >> 3] |=
+			    1 << (crc_bits & 7);
+			mc_list = mc_list->next;
+		}
+#else
+		struct netdev_hw_addr *ha;
+		memset(data->multi_filter, 0, AX_MCAST_FILTER_SIZE);
+		netdev_for_each_mc_addr (ha, net) {
+			crc_bits = ether_crc(ETH_ALEN, ha->addr) >> 26;
+			data->multi_filter[crc_bits >> 3] |=
+				1 << (crc_bits & 7);
+		}
+#endif
+		ax8817x_write_cmd_async(dev, AX_CMD_WRITE_MULTI_FILTER, 0, 0,
+				   AX_MCAST_FILTER_SIZE, data->multi_filter);
 
-	if (sleep)
-		msleep(sleep);
+		rx_ctl |= AX_RX_CTL_AM;
+	}
 
-	return ret;
+	ax8817x_write_cmd_async(dev, AX_CMD_WRITE_RX_CTL, rx_ctl, 0, 0, NULL);
 }
 
-/*
- * AX88772 & AX88178 have a 16-bit RX_CTL value
- */
-static void asix_set_multicast(struct net_device *net)
+static void ax88772b_set_multicast(struct net_device *net)
 {
 	struct usbnet *dev = netdev_priv(net);
-	struct asix_data *data = (struct asix_data *)&dev->data;
-	u16 rx_ctl = AX_DEFAULT_RX_CTL;
+	struct ax8817x_data *data = (struct ax8817x_data *)&dev->data;
+	u16 rx_ctl = (AX_RX_CTL_START | AX_RX_CTL_AB | AX_RX_HEADER_DEFAULT);
+	int mc_count;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35)
+	mc_count = net->mc_count;
+#else
+	mc_count = netdev_mc_count (net);
+#endif
 
 	if (net->flags & IFF_PROMISC) {
 		rx_ctl |= AX_RX_CTL_PRO;
-	} else if (net->flags & IFF_ALLMULTI ||
-		   netdev_mc_count(net) > AX_MAX_MCAST) {
+	} else if (net->flags & IFF_ALLMULTI
+		   || mc_count > AX_MAX_MCAST) {
 		rx_ctl |= AX_RX_CTL_AMALL;
-	} else if (netdev_mc_empty(net)) {
+	} else if (mc_count == 0) {
 		/* just broadcast and directed */
 	} else {
 		/* We use the 20 byte dev->data
 		 * for our 8 byte filter buffer
 		 * to avoid allocating memory that
 		 * is tricky to free later */
-		struct netdev_hw_addr *ha;
 		u32 crc_bits;
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35)
+		struct dev_mc_list *mc_list = net->mc_list;
+		int i;
+
 		memset(data->multi_filter, 0, AX_MCAST_FILTER_SIZE);
 
 		/* Build the multicast hash filter. */
-		netdev_for_each_mc_addr(ha, net) {
-			crc_bits = ether_crc(ETH_ALEN, ha->addr) >> 26;
+		for (i = 0; i < net->mc_count; i++) {
+			crc_bits =
+			    ether_crc(ETH_ALEN,
+				      mc_list->dmi_addr) >> 26;
 			data->multi_filter[crc_bits >> 3] |=
 			    1 << (crc_bits & 7);
+			mc_list = mc_list->next;
 		}
-
-		asix_write_cmd_async(dev, AX_CMD_WRITE_MULTI_FILTER, 0, 0,
+#else
+		struct netdev_hw_addr *ha;
+		memset(data->multi_filter, 0, AX_MCAST_FILTER_SIZE);
+		netdev_for_each_mc_addr (ha, net) {
+			crc_bits = ether_crc(ETH_ALEN, ha->addr) >> 26;
+			data->multi_filter[crc_bits >> 3] |=
+				1 << (crc_bits & 7);
+		}
+#endif
+		ax8817x_write_cmd_async(dev, AX_CMD_WRITE_MULTI_FILTER, 0, 0,
 				   AX_MCAST_FILTER_SIZE, data->multi_filter);
 
 		rx_ctl |= AX_RX_CTL_AM;
 	}
 
-	asix_write_cmd_async(dev, AX_CMD_WRITE_RX_CTL, rx_ctl, 0, 0, NULL);
+	ax8817x_write_cmd_async(dev, AX_CMD_WRITE_RX_CTL, rx_ctl, 0, 0, NULL);
 }
 
-static int asix_mdio_read(struct net_device *netdev, int phy_id, int loc)
+static int ax8817x_mdio_read(struct net_device *netdev, int phy_id, int loc)
 {
 	struct usbnet *dev = netdev_priv(netdev);
-	__le16 res;
+	u16 *res;
+	u16 ret;
+
+	res = kmalloc (2, GFP_ATOMIC);
+	if (!res)
+		return 0;
 
-	mutex_lock(&dev->phy_mutex);
-	asix_set_sw_mii(dev);
-	asix_read_cmd(dev, AX_CMD_READ_MII_REG, phy_id,
-				(__u16)loc, 2, &res);
-	asix_set_hw_mii(dev);
-	mutex_unlock(&dev->phy_mutex);
+	ax8817x_write_cmd(dev, AX_CMD_SET_SW_MII, 0, 0, 0, NULL);
+	ax8817x_read_cmd(dev, AX_CMD_READ_MII_REG, phy_id, (__u16)loc, 2, res);
+	ax8817x_write_cmd(dev, AX_CMD_SET_HW_MII, 0, 0, 0, NULL);
 
-	netdev_dbg(dev->net, "asix_mdio_read() phy_id=0x%02x, loc=0x%02x, returns=0x%04x\n",
-		   phy_id, loc, le16_to_cpu(res));
+	ret = *res & 0xffff;
+	kfree (res);
 
-	return le16_to_cpu(res);
+	return ret;
+}
+
+static int 
+ax8817x_swmii_mdio_read(struct net_device *netdev, int phy_id, int loc)
+{
+	struct usbnet *dev = netdev_priv(netdev);
+	u16 *res;
+	u16 ret;
+
+	res = kmalloc (2, GFP_ATOMIC);
+	if (!res)
+		return 0;
+
+	ax8817x_read_cmd(dev, AX_CMD_READ_MII_REG, phy_id,
+				(__u16)loc, 2, res);
+
+	ret = *res & 0xffff;
+	kfree (res);
+
+	return ret;
+}
+
+/* same as above, but converts resulting value to cpu byte order */
+static int ax8817x_mdio_read_le(struct net_device *netdev, int phy_id, int loc)
+{
+	return le16_to_cpu(ax8817x_mdio_read(netdev,phy_id, loc));
+}
+
+static int 
+ax8817x_swmii_mdio_read_le(struct net_device *netdev, int phy_id, int loc)
+{
+	return le16_to_cpu(ax8817x_swmii_mdio_read(netdev,phy_id, loc));
 }
 
 static void
-asix_mdio_write(struct net_device *netdev, int phy_id, int loc, int val)
+ax8817x_mdio_write(struct net_device *netdev, int phy_id, int loc, int val)
 {
 	struct usbnet *dev = netdev_priv(netdev);
-	__le16 res = cpu_to_le16(val);
+	u16 *res;
+
+	res = kmalloc (2, GFP_ATOMIC);
+	if (!res)
+		return;
+	*res = val;
+
+	ax8817x_write_cmd(dev, AX_CMD_SET_SW_MII, 0, 0, 0, NULL);
+	ax8817x_write_cmd(dev, AX_CMD_WRITE_MII_REG, phy_id,
+				(__u16)loc, 2, res);
+	ax8817x_write_cmd(dev, AX_CMD_SET_HW_MII, 0, 0, 0, NULL);
 
-	netdev_dbg(dev->net, "asix_mdio_write() phy_id=0x%02x, loc=0x%02x, val=0x%04x\n",
-		   phy_id, loc, val);
-	mutex_lock(&dev->phy_mutex);
-	asix_set_sw_mii(dev);
-	asix_write_cmd(dev, AX_CMD_WRITE_MII_REG, phy_id, (__u16)loc, 2, &res);
-	asix_set_hw_mii(dev);
-	mutex_unlock(&dev->phy_mutex);
+	kfree (res);
 }
 
-/* Get the PHY Identifier from the PHYSID1 & PHYSID2 MII registers */
-static u32 asix_get_phyid(struct usbnet *dev)
+static void ax8817x_swmii_mdio_write(struct net_device *netdev, 
+			int phy_id, int loc, int val)
 {
-	int phy_reg;
-	u32 phy_id;
+	struct usbnet *dev = netdev_priv(netdev);
+	u16 *res;
 
-	phy_reg = asix_mdio_read(dev->net, dev->mii.phy_id, MII_PHYSID1);
-	if (phy_reg < 0)
-		return 0;
+	res = kmalloc (2, GFP_ATOMIC);
+	if (!res)
+		return;
+	*res = val;
 
-	phy_id = (phy_reg & 0xffff) << 16;
+	ax8817x_write_cmd(dev, AX_CMD_WRITE_MII_REG, phy_id,
+				(__u16)loc, 2, res);
 
-	phy_reg = asix_mdio_read(dev->net, dev->mii.phy_id, MII_PHYSID2);
-	if (phy_reg < 0)
-		return 0;
+	kfree (res);
+}
 
-	phy_id |= (phy_reg & 0xffff);
+static void
+ax88772b_mdio_write(struct net_device *netdev, int phy_id, int loc, int val)
+{
+	struct usbnet *dev = netdev_priv(netdev);
+	u16 *res;
 
-	return phy_id;
+	res = kmalloc (2, GFP_ATOMIC);
+	if (!res)
+		return;
+	*res = val;
+
+	ax8817x_write_cmd(dev, AX_CMD_SET_SW_MII, 0, 0, 0, NULL);
+	ax8817x_write_cmd(dev, AX_CMD_WRITE_MII_REG, phy_id,
+				(__u16)loc, 2, res);
+
+	if (loc == MII_ADVERTISE) {
+		*res = cpu_to_le16(BMCR_ANENABLE | BMCR_ANRESTART);
+		ax8817x_write_cmd(dev, AX_CMD_WRITE_MII_REG, phy_id,
+				(__u16)MII_BMCR, 2, res);
+	}
+
+	ax8817x_write_cmd(dev, AX_CMD_SET_HW_MII, 0, 0, 0, NULL);
+
+	kfree (res);
+}
+
+/* same as above, but converts new value to le16 byte order before writing */
+static void
+ax8817x_mdio_write_le(struct net_device *netdev, int phy_id, int loc, int val)
+{
+	ax8817x_mdio_write( netdev, phy_id, loc, cpu_to_le16(val) );
+}
+
+static void ax8817x_swmii_mdio_write_le(struct net_device *netdev,
+			int phy_id, int loc, int val)
+{
+	ax8817x_swmii_mdio_write( netdev, phy_id, loc, cpu_to_le16(val) );
 }
 
 static void
-asix_get_wol(struct net_device *net, struct ethtool_wolinfo *wolinfo)
+ax88772b_mdio_write_le(struct net_device *netdev, int phy_id, int loc, int val)
+{
+	ax88772b_mdio_write( netdev, phy_id, loc, cpu_to_le16(val) );
+}
+
+static int ax88772_suspend (struct usb_interface *intf,
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,10)
+			pm_message_t message)
+#else
+			u32 message)
+#endif
+{
+	struct usbnet *dev = usb_get_intfdata(intf);
+	u16 *medium;
+
+	medium = kmalloc (2, GFP_ATOMIC);
+	if (!medium)
+		return axusbnet_suspend (intf, message);
+
+	ax8817x_read_cmd (dev, AX_CMD_READ_MEDIUM_MODE, 0, 0, 2, medium);
+	ax8817x_write_cmd (dev, AX_CMD_WRITE_MEDIUM_MODE,
+			(*medium & ~AX88772_MEDIUM_RX_ENABLE), 0, 0, NULL);
+
+	kfree (medium);
+	return axusbnet_suspend (intf, message);
+}
+
+static int ax88772b_suspend (struct usb_interface *intf,
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,10)
+			pm_message_t message)
+#else
+			u32 message)
+#endif
+{
+	struct usbnet *dev = usb_get_intfdata(intf);
+	struct ax88772b_data *ax772b_data = (struct ax88772b_data *)dev->priv;
+	u16 *tmp16;
+	u8 *opt;
+
+	tmp16 = kmalloc (2, GFP_ATOMIC);
+	if (!tmp16)
+		return axusbnet_suspend (intf, message);
+	opt = (u8 *)tmp16;
+
+	ax8817x_read_cmd (dev, AX_CMD_READ_MEDIUM_MODE, 0, 0, 2, tmp16);
+	ax8817x_write_cmd (dev, AX_CMD_WRITE_MEDIUM_MODE,
+			(*tmp16 & ~AX88772_MEDIUM_RX_ENABLE), 0, 0, NULL);
+
+	ax8817x_read_cmd(dev, AX_CMD_READ_MONITOR_MODE, 0, 0, 1, opt);
+	if (!(*opt & AX_MONITOR_LINK) && !(*opt & AX_MONITOR_MAGIC)) {
+		ax8817x_write_cmd (dev, AX_CMD_SW_RESET,
+			AX_SWRESET_IPRL | AX_SWRESET_IPPD, 0, 0, NULL);
+	} else {
+
+		if (ax772b_data->psc & AX_SWRESET_WOLLP) {
+			*tmp16 = ax8817x_mdio_read_le (dev->net,
+					dev->mii.phy_id, MII_BMCR);
+			ax8817x_mdio_write_le (dev->net, dev->mii.phy_id,
+					MII_BMCR, *tmp16 | BMCR_ANENABLE);
+
+			ax8817x_write_cmd (dev, AX_CMD_SW_RESET,
+				AX_SWRESET_IPRL | ax772b_data->psc, 0, 0, NULL);
+		}
+
+		if (ax772b_data->psc &
+		    (AX_SWRESET_IPPSL_0 | AX_SWRESET_IPPSL_1)) {
+			*opt |= AX_MONITOR_LINK;
+			ax8817x_write_cmd(dev, AX_CMD_WRITE_MONITOR_MODE,
+					*opt, 0, 0, NULL);
+		}
+	}
+
+	kfree (tmp16);
+	return axusbnet_suspend (intf, message);
+}
+
+static int ax88772_resume (struct usb_interface *intf)
+{
+	struct usbnet *dev = usb_get_intfdata(intf);
+
+	netif_carrier_off (dev->net);
+
+	return axusbnet_resume (intf);
+}
+
+static int ax88772b_resume (struct usb_interface *intf)
+{
+	struct usbnet *dev = usb_get_intfdata(intf);
+	struct ax88772b_data *ax772b_data = (struct ax88772b_data *)dev->priv;
+
+	if (ax772b_data->psc & AX_SWRESET_WOLLP) {
+		ax8817x_write_cmd (dev, AX_CMD_SW_RESET,
+				AX_SWRESET_IPRL | (ax772b_data->psc & 0x7FFF),
+				0, 0, NULL);
+	}
+
+	if (ax772b_data->psc & (AX_SWRESET_IPPSL_0 | AX_SWRESET_IPPSL_1)) {
+		ax88772a_phy_powerup (dev);
+	}
+
+	netif_carrier_off (dev->net);
+
+	return axusbnet_resume (intf);
+}
+
+static int ax88172_link_reset(struct usbnet *dev)
+{
+	u16 lpa;
+	u16 adv;
+	u16 res;
+	u8 mode;
+
+	mode = AX_MEDIUM_TX_ABORT_ALLOW | AX_MEDIUM_FLOW_CONTROL_EN;
+	lpa = ax8817x_mdio_read_le(dev->net, dev->mii.phy_id, MII_LPA);
+	adv = ax8817x_mdio_read_le(dev->net, dev->mii.phy_id, MII_ADVERTISE);
+	res = mii_nway_result(lpa|adv);
+	if (res & LPA_DUPLEX)
+		mode |= AX_MEDIUM_FULL_DUPLEX;
+	ax8817x_write_cmd(dev, AX_CMD_WRITE_MEDIUM_MODE, mode, 0, 0, NULL);
+
+	return 0;
+}
+
+static void
+ax8817x_get_wol(struct net_device *net, struct ethtool_wolinfo *wolinfo)
 {
 	struct usbnet *dev = netdev_priv(net);
-	u8 opt;
+	u8 *opt;
+
+	wolinfo->supported = 0;
+	wolinfo->wolopts = 0;
 
-	if (asix_read_cmd(dev, AX_CMD_READ_MONITOR_MODE, 0, 0, 1, &opt) < 0) {
-		wolinfo->supported = 0;
-		wolinfo->wolopts = 0;
+	opt = kmalloc (1, GFP_KERNEL);
+	if (!opt)
 		return;
-	}
+
+	if (ax8817x_read_cmd(dev, AX_CMD_READ_MONITOR_MODE, 0, 0, 1, opt) < 0)
+		return;
+
 	wolinfo->supported = WAKE_PHY | WAKE_MAGIC;
-	wolinfo->wolopts = 0;
-	if (opt & AX_MONITOR_MODE) {
-		if (opt & AX_MONITOR_LINK)
-			wolinfo->wolopts |= WAKE_PHY;
-		if (opt & AX_MONITOR_MAGIC)
-			wolinfo->wolopts |= WAKE_MAGIC;
-	}
+
+	if (*opt & AX_MONITOR_LINK)
+		wolinfo->wolopts |= WAKE_PHY;
+	if (*opt & AX_MONITOR_MAGIC)
+		wolinfo->wolopts |= WAKE_MAGIC;
+
+	kfree (opt);
 }
 
 static int
-asix_set_wol(struct net_device *net, struct ethtool_wolinfo *wolinfo)
+ax8817x_set_wol(struct net_device *net, struct ethtool_wolinfo *wolinfo)
 {
 	struct usbnet *dev = netdev_priv(net);
-	u8 opt = 0;
+	u8 *opt;
+
+	opt = kmalloc (1, GFP_KERNEL);
+	if (!opt)
+		return -ENOMEM;
 
+	*opt = 0;
 	if (wolinfo->wolopts & WAKE_PHY)
-		opt |= AX_MONITOR_LINK;
+		*opt |= AX_MONITOR_LINK;
 	if (wolinfo->wolopts & WAKE_MAGIC)
-		opt |= AX_MONITOR_MAGIC;
-	if (opt != 0)
-		opt |= AX_MONITOR_MODE;
+		*opt |= AX_MONITOR_MAGIC;
 
-	if (asix_write_cmd(dev, AX_CMD_WRITE_MONITOR_MODE,
-			      opt, 0, 0, NULL) < 0)
-		return -EINVAL;
+	ax8817x_write_cmd(dev, AX_CMD_WRITE_MONITOR_MODE, *opt, 0, 0, NULL);
 
+	kfree (opt);
 	return 0;
 }
 
-static int asix_get_eeprom_len(struct net_device *net)
+static int ax8817x_get_eeprom_len(struct net_device *net)
 {
-	struct usbnet *dev = netdev_priv(net);
-	struct asix_data *data = (struct asix_data *)&dev->data;
-
-	return data->eeprom_len;
+	return AX_EEPROM_LEN;
 }
 
-static int asix_get_eeprom(struct net_device *net,
+static int ax8817x_get_eeprom(struct net_device *net,
 			      struct ethtool_eeprom *eeprom, u8 *data)
 {
 	struct usbnet *dev = netdev_priv(net);
-	__le16 *ebuf = (__le16 *)data;
+	u16 *ebuf = (u16 *)data;
 	int i;
 
 	/* Crude hack to ensure that we don't overwrite memory
@@ -729,722 +906,2765 @@ static int asix_get_eeprom(struct net_device *net,
 
 	/* ax8817x returns 2 bytes from eeprom on read */
 	for (i=0; i < eeprom->len / 2; i++) {
-		if (asix_read_cmd(dev, AX_CMD_READ_EEPROM,
+		if (ax8817x_read_cmd(dev, AX_CMD_READ_EEPROM,
 			eeprom->offset + i, 0, 2, &ebuf[i]) < 0)
 			return -EINVAL;
 	}
 	return 0;
 }
 
-static void asix_get_drvinfo (struct net_device *net,
+static void ax8817x_get_drvinfo (struct net_device *net,
 				 struct ethtool_drvinfo *info)
 {
-	struct usbnet *dev = netdev_priv(net);
-	struct asix_data *data = (struct asix_data *)&dev->data;
-
 	/* Inherit standard device info */
-	usbnet_get_drvinfo(net, info);
-	strncpy (info->driver, driver_name, sizeof info->driver);
-	strncpy (info->version, DRIVER_VERSION, sizeof info->version);
-	info->eedump_len = data->eeprom_len;
+	axusbnet_get_drvinfo(net, info);
+	info->eedump_len = 0x3e;
 }
 
-static u32 asix_get_link(struct net_device *net)
+static int ax8817x_get_settings(struct net_device *net, struct ethtool_cmd *cmd)
 {
 	struct usbnet *dev = netdev_priv(net);
+	return mii_ethtool_gset(&dev->mii,cmd);
+}
 
-	return mii_link_ok(&dev->mii);
+static int ax8817x_set_settings(struct net_device *net, struct ethtool_cmd *cmd)
+{
+	struct usbnet *dev = netdev_priv(net);
+	return mii_ethtool_sset(&dev->mii,cmd);
 }
 
-static int asix_ioctl (struct net_device *net, struct ifreq *rq, int cmd)
+/* We need to override some ethtool_ops so we require our
+   own structure so we don't interfere with other usbnet
+   devices that may be connected at the same time. */
+static struct ethtool_ops ax8817x_ethtool_ops = {
+	.get_drvinfo		= ax8817x_get_drvinfo,
+	.get_link		= ethtool_op_get_link,
+	.get_msglevel		= axusbnet_get_msglevel,
+	.set_msglevel		= axusbnet_set_msglevel,
+	.get_wol		= ax8817x_get_wol,
+	.set_wol		= ax8817x_set_wol,
+	.get_eeprom_len	= ax8817x_get_eeprom_len,
+	.get_eeprom		= ax8817x_get_eeprom,
+	.get_settings		= ax8817x_get_settings,
+	.set_settings		= ax8817x_set_settings,
+};
+
+static int ax8817x_ioctl (struct net_device *net, struct ifreq *rq, int cmd)
 {
 	struct usbnet *dev = netdev_priv(net);
 
 	return generic_mii_ioctl(&dev->mii, if_mii(rq), cmd, NULL);
 }
 
-static int asix_set_mac_address(struct net_device *net, void *p)
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,29)
+static const struct net_device_ops ax88x72_netdev_ops = {
+	.ndo_open			= axusbnet_open,
+	.ndo_stop			= axusbnet_stop,
+	.ndo_start_xmit	= axusbnet_start_xmit,
+	.ndo_tx_timeout	= axusbnet_tx_timeout,
+	.ndo_change_mtu	= axusbnet_change_mtu,
+	.ndo_get_stats		= axusbnet_get_stats,
+	.ndo_do_ioctl		= ax8817x_ioctl,
+	.ndo_set_mac_address		= ax8817x_set_mac_addr,
+	.ndo_validate_addr		= eth_validate_addr,
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(3,2,0)
+	.ndo_set_multicast_list	= ax8817x_set_multicast,
+#else
+	.ndo_set_rx_mode	= ax8817x_set_multicast,
+#endif
+};
+#endif
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,29)
+static const struct net_device_ops ax88178_netdev_ops = {
+	.ndo_open			= axusbnet_open,
+	.ndo_stop			= axusbnet_stop,
+	.ndo_start_xmit	= axusbnet_start_xmit,
+	.ndo_tx_timeout	= axusbnet_tx_timeout,
+	.ndo_change_mtu	= axusbnet_change_mtu,
+	.ndo_get_stats		= axusbnet_get_stats,
+	.ndo_do_ioctl		= ax8817x_ioctl,
+	.ndo_set_mac_address		= ax8817x_set_mac_addr,
+	.ndo_validate_addr		= eth_validate_addr,
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(3,2,0)
+	.ndo_set_multicast_list	= ax88178_set_multicast,
+#else
+	.ndo_set_rx_mode	= ax88178_set_multicast,
+#endif
+};
+#endif
+
+static int ax8817x_bind(struct usbnet *dev, struct usb_interface *intf)
 {
-	struct usbnet *dev = netdev_priv(net);
-	struct asix_data *data = (struct asix_data *)&dev->data;
-	struct sockaddr *addr = p;
+	int ret = 0;
+	void *buf;
+	int i;
+	unsigned long gpio_bits = dev->driver_info->data;
+	struct ax8817x_data *data = (struct ax8817x_data *)&dev->data;
 
-	if (netif_running(net))
-		return -EBUSY;
-	if (!is_valid_ether_addr(addr->sa_data))
-		return -EADDRNOTAVAIL;
+	axusbnet_get_endpoints(dev,intf);
 
-	memcpy(net->dev_addr, addr->sa_data, ETH_ALEN);
+	buf = kmalloc(ETH_ALEN, GFP_KERNEL);
+	if(!buf) {
+		ret = -ENOMEM;
+		goto out1;
+	}
+
+	/* Toggle the GPIOs in a manufacturer/model specific way */
+	for (i = 2; i >= 0; i--) {
+		if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_GPIOS,
+					(gpio_bits >> (i * 8)) & 0xff, 0, 0,
+					NULL)) < 0)
+			goto out2;
+		msleep(5);
+	}
+
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_RX_CTL,
+				0x80, 0, 0, NULL)) < 0) {
+		deverr(dev, "send AX_CMD_WRITE_RX_CTL failed: %d", ret);
+		goto out2;
+	}
 
-	/* We use the 20 byte dev->data
-	 * for our 6 byte mac buffer
-	 * to avoid allocating memory that
-	 * is tricky to free later */
-	memcpy(data->mac_addr, addr->sa_data, ETH_ALEN);
-	asix_write_cmd_async(dev, AX_CMD_WRITE_NODE_ID, 0, 0, ETH_ALEN,
-							data->mac_addr);
+	/* Get the MAC address */
+	memset(buf, 0, ETH_ALEN);
+	if ((ret = ax8817x_read_cmd(dev, AX_CMD_READ_NODE_ID,
+				0, 0, 6, buf)) < 0) {
+		deverr(dev, "read AX_CMD_READ_NODE_ID failed: %d", ret);
+		goto out2;
+	}
+	memcpy(dev->net->dev_addr, buf, ETH_ALEN);
+
+	/* Get the PHY id */
+	if ((ret = ax8817x_read_cmd(dev, AX_CMD_READ_PHY_ID,
+				0, 0, 2, buf)) < 0) {
+		deverr(dev, "error on read AX_CMD_READ_PHY_ID: %02x", ret);
+		goto out2;
+	} else if (ret < 2) {
+		/* this should always return 2 bytes */
+		deverr(dev, "Read PHYID returned less than 2 bytes: ret=%02x",
+				ret);
+		ret = -EIO;
+		goto out2;
+	}
+
+	/* Initialize MII structure */
+	dev->mii.dev = dev->net;
+	dev->mii.mdio_read = ax8817x_mdio_read_le;
+	dev->mii.mdio_write = ax8817x_mdio_write_le;
+	dev->mii.phy_id_mask = 0x3f;
+	dev->mii.reg_num_mask = 0x1f;
+	dev->mii.phy_id = *((u8 *)buf + 1);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,30)
+	dev->net->do_ioctl = ax8817x_ioctl;
+	dev->net->set_multicast_list = ax8817x_set_multicast;
+	dev->net->set_mac_address = ax8817x_set_mac_addr;
+#else
+	dev->net->netdev_ops = &ax88x72_netdev_ops;
+#endif
+
+	dev->net->ethtool_ops = &ax8817x_ethtool_ops;
+
+	/* Register suspend and resume functions */
+	data->suspend = axusbnet_suspend;
+	data->resume = axusbnet_resume;
+
+	ax8817x_mdio_write_le(dev->net, dev->mii.phy_id, MII_BMCR, BMCR_RESET);
+	ax8817x_mdio_write_le(dev->net, dev->mii.phy_id, MII_ADVERTISE,
+		ADVERTISE_ALL | ADVERTISE_CSMA | ADVERTISE_PAUSE_CAP);
+	mii_nway_restart(&dev->mii);
+
+	printk (version);
 
 	return 0;
+out2:
+	kfree(buf);
+out1:
+	return ret;
 }
 
-/* We need to override some ethtool_ops so we require our
-   own structure so we don't interfere with other usbnet
-   devices that may be connected at the same time. */
-static const struct ethtool_ops ax88172_ethtool_ops = {
-	.get_drvinfo		= asix_get_drvinfo,
-	.get_link		= asix_get_link,
-	.get_msglevel		= usbnet_get_msglevel,
-	.set_msglevel		= usbnet_set_msglevel,
-	.get_wol		= asix_get_wol,
-	.set_wol		= asix_set_wol,
-	.get_eeprom_len		= asix_get_eeprom_len,
-	.get_eeprom		= asix_get_eeprom,
-	.get_settings		= usbnet_get_settings,
-	.set_settings		= usbnet_set_settings,
-	.nway_reset		= usbnet_nway_reset,
+static struct ethtool_ops ax88772_ethtool_ops = {
+	.get_drvinfo		= ax8817x_get_drvinfo,
+	.get_link		= ethtool_op_get_link,
+	.get_msglevel		= axusbnet_get_msglevel,
+	.set_msglevel		= axusbnet_set_msglevel,
+	.get_wol		= ax8817x_get_wol,
+	.set_wol		= ax8817x_set_wol,
+	.get_eeprom_len		= ax8817x_get_eeprom_len,
+	.get_eeprom		= ax8817x_get_eeprom,
+	.get_settings		= ax8817x_get_settings,
+	.set_settings		= ax8817x_set_settings,
 };
 
-static void ax88172_set_multicast(struct net_device *net)
+static int ax88772_bind(struct usbnet *dev, struct usb_interface *intf)
 {
-	struct usbnet *dev = netdev_priv(net);
-	struct asix_data *data = (struct asix_data *)&dev->data;
-	u8 rx_ctl = 0x8c;
+	int ret;
+	void *buf;
+	struct ax8817x_data *data = (struct ax8817x_data *)&dev->data;
+	struct ax88772_data *ax772_data = NULL;
 
-	if (net->flags & IFF_PROMISC) {
-		rx_ctl |= 0x01;
-	} else if (net->flags & IFF_ALLMULTI ||
-		   netdev_mc_count(net) > AX_MAX_MCAST) {
-		rx_ctl |= 0x02;
-	} else if (netdev_mc_empty(net)) {
-		/* just broadcast and directed */
-	} else {
-		/* We use the 20 byte dev->data
-		 * for our 8 byte filter buffer
-		 * to avoid allocating memory that
-		 * is tricky to free later */
-		struct netdev_hw_addr *ha;
-		u32 crc_bits;
+	axusbnet_get_endpoints(dev,intf);
 
-		memset(data->multi_filter, 0, AX_MCAST_FILTER_SIZE);
+	buf = kmalloc(6, GFP_KERNEL);
+	if(!buf) {
+		deverr(dev, "Cannot allocate memory for buffer");
+		ret = -ENOMEM;
+		goto out1;
+	}
 
-		/* Build the multicast hash filter. */
-		netdev_for_each_mc_addr(ha, net) {
-			crc_bits = ether_crc(ETH_ALEN, ha->addr) >> 26;
-			data->multi_filter[crc_bits >> 3] |=
-			    1 << (crc_bits & 7);
+        ax772_data = kmalloc (sizeof(*ax772_data), GFP_KERNEL);
+        if (!ax772_data) {
+                deverr(dev, "Cannot allocate memory for AX88772 data");
+                kfree (buf);
+                return -ENOMEM;
+        }
+        memset (ax772_data, 0, sizeof(*ax772_data));
+        dev->priv = ax772_data;
+
+        ax772_data->ax_work = create_singlethread_workqueue ("ax88772");
+        if (!ax772_data->ax_work) {
+                kfree (ax772_data);
+                kfree (buf);
+                return -ENOMEM;
+        }
+
+        ax772_data->dev = dev;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+        INIT_WORK (&ax772_data->check_link, ax88772_link_reset, dev);
+#else
+        INIT_WORK (&ax772_data->check_link, ax88772_link_reset);
+#endif
+
+	/* reload eeprom data */
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_GPIOS,
+				     0x00B0, 0, 0, NULL)) < 0)
+		goto out2;
+
+	msleep(5);
+
+	/* Initialize MII structure */
+	dev->mii.dev = dev->net;
+	dev->mii.mdio_read = ax8817x_mdio_read_le;
+	dev->mii.mdio_write = ax8817x_mdio_write_le;
+	dev->mii.phy_id_mask = 0xff;
+	dev->mii.reg_num_mask = 0xff;
+
+	/* Get the PHY id */
+	if ((ret = ax8817x_read_cmd(dev, AX_CMD_READ_PHY_ID,
+			0, 0, 2, buf)) < 0) {
+		deverr(dev, "Error reading PHY ID: %02x", ret);
+		goto out2;
+	} else if (ret < 2) {
+		/* this should always return 2 bytes */
+		deverr(dev, "Read PHYID returned less than 2 bytes: ret=%02x",
+		    ret);
+		ret = -EIO;
+		goto out2;
+	}
+	dev->mii.phy_id = *((u8 *)buf + 1);
+
+	if (dev->mii.phy_id == 0x10)
+	{
+		if ((ret = ax8817x_write_cmd(dev, AX_CMD_SW_PHY_SELECT,
+					0x0001, 0, 0, NULL)) < 0) {
+			deverr(dev, "Select PHY #1 failed: %d", ret);
+			goto out2;
 		}
 
-		asix_write_cmd_async(dev, AX_CMD_WRITE_MULTI_FILTER, 0, 0,
-				   AX_MCAST_FILTER_SIZE, data->multi_filter);
+		if ((ret = ax8817x_write_cmd (dev, AX_CMD_SW_RESET,
+					AX_SWRESET_IPPD,
+					0, 0, NULL)) < 0) {
+			deverr(dev, "Failed to power down PHY: %d", ret);
+			goto out2;
+		}
+
+		msleep(150);
+		if ((ret = ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+					AX_SWRESET_CLEAR,
+					0, 0, NULL)) < 0) {
+			deverr(dev, 
+			      "Failed to perform software reset: %d", ret);
+			goto out2;
+		}
+
+		msleep(150);
+		if ((ret = ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+		     			AX_SWRESET_IPRL | AX_SWRESET_PRL,
+					0, 0, NULL)) < 0) {
+			deverr(dev,
+			      "Failed to set PHY reset control: %d", ret);
+			goto out2;
+		}
+	}
+	else
+	{
+		if ((ret = ax8817x_write_cmd(dev, AX_CMD_SW_PHY_SELECT,
+					0x0000, 0, 0, NULL)) < 0) {
+			deverr(dev, "Select PHY #1 failed: %d", ret);
+			goto out2;
+		}
+
+		if ((ret = ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+					AX_SWRESET_IPPD | AX_SWRESET_PRL,
+					0, 0, NULL)) < 0) {
+			deverr(dev,
+			      "Failed to power down internal PHY: %d", ret);
+			goto out2;
+		}
+	}
+
+	msleep(150);
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_RX_CTL,
+				0x0000, 0, 0, NULL)) < 0) {
+		deverr(dev, "Failed to reset RX_CTL: %d", ret);
+		goto out2;
+	}
+
+	/* Get the MAC address */
+	memset(buf, 0, ETH_ALEN);
+	if ((ret = ax8817x_read_cmd(dev, AX88772_CMD_READ_NODE_ID,
+				0, 0, ETH_ALEN, buf)) < 0) {
+		deverr(dev, "Failed to read MAC address: %d", ret);
+		goto out2;
+	}
+	memcpy(dev->net->dev_addr, buf, ETH_ALEN);
+
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_SET_SW_MII,
+				0, 0, 0, NULL)) < 0) {
+		deverr(dev, "Enabling software MII failed: %d", ret);
+		goto out2;
+	}
+
+	if (dev->mii.phy_id == 0x10)
+	{
+		if ((ret = ax8817x_mdio_read_le(dev->net,
+				dev->mii.phy_id, 2)) != 0x003b) {
+			deverr(dev, "Read PHY register 2 must be 0x3b00: %d",
+				ret);
+			goto out2;
+		}
+		
+		if ((ret = ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+					AX_SWRESET_PRL,
+					0, 0, NULL)) < 0) {
+			deverr(dev, "Set external PHY reset pin level: %d", ret);
+			goto out2;
+		}
+		msleep(150);
+		if ((ret = ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+			 		AX_SWRESET_IPRL | AX_SWRESET_PRL,
+					0, 0, NULL)) < 0) {
+			deverr(dev, 
+				"Set Internal/External PHY reset control: %d",
+				ret);
+			goto out2;
+		}
+		msleep(150);
+	}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,30)
+	dev->net->do_ioctl = ax8817x_ioctl;
+	dev->net->set_multicast_list = ax8817x_set_multicast;
+	dev->net->set_mac_address = ax8817x_set_mac_addr;
+#else
+	dev->net->netdev_ops = &ax88x72_netdev_ops;
+#endif
+
+	dev->net->ethtool_ops = &ax88772_ethtool_ops;
+
+	/* Register suspend and resume functions */
+	data->suspend = ax88772_suspend;
+	data->resume = ax88772_resume;
+
+	ax8817x_mdio_write_le(dev->net, dev->mii.phy_id, MII_BMCR, BMCR_RESET);
+	ax8817x_mdio_write_le(dev->net, dev->mii.phy_id, MII_ADVERTISE,
+			ADVERTISE_ALL | ADVERTISE_CSMA);
 
-		rx_ctl |= 0x10;
+	mii_nway_restart(&dev->mii);
+        ax772_data->autoneg_start = jiffies;
+        ax772_data->Event = WAIT_AUTONEG_COMPLETE;
+
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_MEDIUM_MODE,
+				0, 0, 0, NULL)) < 0) {
+		deverr(dev, "Write medium mode register: %d", ret);
+		goto out2;
+	}
+
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_IPG0,
+			AX88772_IPG0_DEFAULT | AX88772_IPG1_DEFAULT << 8,
+			AX88772_IPG2_DEFAULT, 0, NULL)) < 0) {
+		deverr(dev, "Write IPG,IPG1,IPG2 failed: %d", ret);
+		goto out2;
+	}
+	if ((ret =
+	     ax8817x_write_cmd(dev, AX_CMD_SET_HW_MII, 0, 0, 0, NULL)) < 0) {
+		deverr(dev, "Failed to set hardware MII: %02x", ret);
+		goto out2;
+	}
+
+	/* Set RX_CTL to default values with 2k buffer, and enable cactus */
+	if ((ret =
+	     ax8817x_write_cmd(dev, AX_CMD_WRITE_RX_CTL, 0x0088, 0, 0,
+			       NULL)) < 0) {
+		deverr(dev, "Reset RX_CTL failed: %d", ret);
+		goto out2;
+	}
+
+	/* Asix framing packs multiple eth frames into a 2K usb bulk transfer */
+	if (dev->driver_info->flags & FLAG_FRAMING_AX) {
+		/* hard_mtu  is still the default - the device does not support
+		   jumbo eth frames */
+		dev->rx_urb_size = 2048;
 	}
 
-	asix_write_cmd_async(dev, AX_CMD_WRITE_RX_CTL, rx_ctl, 0, 0, NULL);
+	kfree (buf);
+	printk (version);
+	return 0;
+
+out2:
+	destroy_workqueue (ax772_data->ax_work);
+        kfree (ax772_data);
+	kfree(buf);
+out1:
+	return ret;
 }
 
-static int ax88172_link_reset(struct usbnet *dev)
+static void ax88772_unbind(struct usbnet *dev, struct usb_interface *intf)
 {
-	u8 mode;
-	struct ethtool_cmd ecmd = { .cmd = ETHTOOL_GSET };
+	struct ax88772_data *ax772_data = (struct ax88772_data *)dev->priv;
 
-	mii_check_media(&dev->mii, 1, 1);
-	mii_ethtool_gset(&dev->mii, &ecmd);
-	mode = AX88172_MEDIUM_DEFAULT;
+	if (ax772_data) {
 
-	if (ecmd.duplex != DUPLEX_FULL)
-		mode |= ~AX88172_MEDIUM_FD;
+		flush_workqueue (ax772_data->ax_work);
+		destroy_workqueue (ax772_data->ax_work);
 
-	netdev_dbg(dev->net, "ax88172_link_reset() speed: %u duplex: %d setting mode to 0x%04x\n",
-		   ethtool_cmd_speed(&ecmd), ecmd.duplex, mode);
+		/* stop MAC operation */
+		ax8817x_write_cmd(dev, AX_CMD_WRITE_RX_CTL,
+					AX_RX_CTL_STOP, 0, 0, NULL);
 
-	asix_write_medium_mode(dev, mode);
+		/* Power down PHY */
+		ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+					AX_SWRESET_IPPD, 0, 0, NULL);
 
-	return 0;
+		kfree (ax772_data);
+	}
 }
 
-static const struct net_device_ops ax88172_netdev_ops = {
-	.ndo_open		= usbnet_open,
-	.ndo_stop		= usbnet_stop,
-	.ndo_start_xmit		= usbnet_start_xmit,
-	.ndo_tx_timeout		= usbnet_tx_timeout,
-	.ndo_change_mtu		= usbnet_change_mtu,
-	.ndo_set_mac_address 	= eth_mac_addr,
-	.ndo_validate_addr	= eth_validate_addr,
-	.ndo_do_ioctl		= asix_ioctl,
-	.ndo_set_multicast_list = ax88172_set_multicast,
-};
+static int ax88772a_phy_powerup (struct usbnet *dev)
+{
+	int ret;
+	/* set the embedded Ethernet PHY in power-down state */
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+			AX_SWRESET_IPPD | AX_SWRESET_IPRL, 0, 0, NULL)) < 0) {
+		deverr(dev, "Failed to power down PHY: %d", ret);
+		return ret;
+	}
+
+	msleep(10);
+
+	
+	/* set the embedded Ethernet PHY in power-up state */
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+			AX_SWRESET_IPRL, 0, 0, NULL)) < 0) {
+		deverr(dev, "Failed to reset PHY: %d", ret);
+		return ret;
+	}
+
+	msleep(600);
+
+	/* set the embedded Ethernet PHY in reset state */
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+			AX_SWRESET_CLEAR, 0, 0, NULL)) < 0) {
+		deverr(dev, "Failed to power up PHY: %d", ret);
+		return ret;
+	}
+
+	/* set the embedded Ethernet PHY in power-up state */
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+			AX_SWRESET_IPRL, 0, 0, NULL)) < 0) {
+		deverr(dev, "Failed to reset PHY: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
 
-static int ax88172_bind(struct usbnet *dev, struct usb_interface *intf)
+static int ax88772a_bind(struct usbnet *dev, struct usb_interface *intf)
 {
-	int ret = 0;
-	u8 buf[ETH_ALEN];
-	int i;
-	unsigned long gpio_bits = dev->driver_info->data;
-	struct asix_data *data = (struct asix_data *)&dev->data;
+	int ret = -EIO;
+	void *buf;
+	struct ax8817x_data *data = (struct ax8817x_data *)&dev->data;
+	struct ax88772a_data *ax772a_data = NULL;
 
-	data->eeprom_len = AX88172_EEPROM_LEN;
+	axusbnet_get_endpoints(dev,intf);
 
-	usbnet_get_endpoints(dev,intf);
+	buf = kmalloc(6, GFP_KERNEL);
+	if(!buf) {
+		deverr(dev, "Cannot allocate memory for buffer");
+		ret = -ENOMEM;
+		goto out1;
+	}
 
-	/* Toggle the GPIOs in a manufacturer/model specific way */
-	for (i = 2; i >= 0; i--) {
-		if ((ret = asix_write_cmd(dev, AX_CMD_WRITE_GPIOS,
-					(gpio_bits >> (i * 8)) & 0xff, 0, 0,
-					NULL)) < 0)
-			goto out;
-		msleep(5);
+	ax772a_data = kmalloc (sizeof(*ax772a_data), GFP_KERNEL);
+	if (!ax772a_data) {
+		deverr(dev, "Cannot allocate memory for AX88772A data");
+		kfree (buf);
+		return -ENOMEM;
+	}
+	memset (ax772a_data, 0, sizeof(*ax772a_data));
+	dev->priv = ax772a_data;
+
+	ax772a_data->ax_work = create_singlethread_workqueue ("ax88772a");
+	if (!ax772a_data->ax_work) {
+		kfree (ax772a_data);
+		kfree (buf);
+		return -ENOMEM;
+	}
+
+	ax772a_data->dev = dev;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	INIT_WORK (&ax772a_data->check_link, ax88772a_link_reset, dev);
+#else
+	INIT_WORK (&ax772a_data->check_link, ax88772a_link_reset);
+#endif
+
+	/* Get the EEPROM data*/
+	if ((ret = ax8817x_read_cmd(dev, AX_CMD_READ_EEPROM, 
+			0x0017, 0, 2, (void *)&ax772a_data->EepromData)) < 0) {
+		deverr(dev, "read SROM address 17h failed: %d", ret);
+		goto out2;
+	}
+	le16_to_cpus (&ax772a_data->EepromData);
+	/* End of get EEPROM data */
+
+	/* reload eeprom data */
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_GPIOS,
+			AXGPIOS_RSE, 0, 0, NULL)) < 0)
+		goto out2;
+
+	msleep(5);
+
+	/* Initialize MII structure */
+	dev->mii.dev = dev->net;
+	dev->mii.mdio_read = ax8817x_mdio_read_le;
+	dev->mii.mdio_write = ax8817x_mdio_write_le;
+	dev->mii.phy_id_mask = 0xff;
+	dev->mii.reg_num_mask = 0xff;
+
+	/* Get the PHY id */
+	if ((ret = ax8817x_read_cmd(dev, AX_CMD_READ_PHY_ID,
+			0, 0, 2, buf)) < 0) {
+		deverr(dev, "Error reading PHY ID: %02x", ret);
+		goto out2;
+	} else if (ret < 2) {
+		/* this should always return 2 bytes */
+		deverr(dev, "Read PHYID returned less than 2 bytes: ret=%02x",
+			ret);
+		goto out2;
+	}
+	dev->mii.phy_id = *((u8 *)buf + 1);
+
+	if(dev->mii.phy_id != 0x10) {
+		deverr(dev, "Got wrong PHY ID: %02x", dev->mii.phy_id);
+		goto out2;
+	}
+
+	/* select the embedded 10/100 Ethernet PHY */
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_SW_PHY_SELECT,
+			AX_PHYSEL_SSEN | AX_PHYSEL_PSEL | AX_PHYSEL_SSMII,
+			0, 0, NULL)) < 0) {
+		deverr(dev, "Select PHY #1 failed: %d", ret);
+		goto out2;
 	}
 
-	if ((ret = asix_write_rx_ctl(dev, 0x80)) < 0)
-		goto out;
+	if ((ret = ax88772a_phy_powerup (dev)) < 0)
+		goto out2;
+
+	/* stop MAC operation */
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_RX_CTL,
+			AX_RX_CTL_STOP, 0, 0, NULL)) < 0) {
+		deverr(dev, "Reset RX_CTL failed: %d", ret);
+		goto out2;
+	}
 
 	/* Get the MAC address */
-	if ((ret = asix_read_cmd(dev, AX88172_CMD_READ_NODE_ID,
+	memset(buf, 0, ETH_ALEN);
+	if ((ret = ax8817x_read_cmd(dev, AX88772_CMD_READ_NODE_ID,
 				0, 0, ETH_ALEN, buf)) < 0) {
-		dbg("read AX_CMD_READ_NODE_ID failed: %d", ret);
-		goto out;
+		deverr(dev, "Failed to read MAC address: %d", ret);
+		goto out2;
 	}
 	memcpy(dev->net->dev_addr, buf, ETH_ALEN);
 
-	/* Initialize MII structure */
-	dev->mii.dev = dev->net;
-	dev->mii.mdio_read = asix_mdio_read;
-	dev->mii.mdio_write = asix_mdio_write;
-	dev->mii.phy_id_mask = 0x3f;
-	dev->mii.reg_num_mask = 0x1f;
-	dev->mii.phy_id = asix_get_phy_addr(dev);
+	/* make sure the driver can enable sw mii operation */
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_SET_SW_MII,
+			0, 0, 0, NULL)) < 0) {
+		deverr(dev, "Enabling software MII failed: %d", ret);
+		goto out2;
+	}
 
-	dev->net->netdev_ops = &ax88172_netdev_ops;
-	dev->net->ethtool_ops = &ax88172_ethtool_ops;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,30)
+	dev->net->do_ioctl = ax8817x_ioctl;
+	dev->net->set_multicast_list = ax8817x_set_multicast;
+	dev->net->set_mac_address = ax8817x_set_mac_addr;
+#else
+	dev->net->netdev_ops = &ax88x72_netdev_ops;
+#endif
+
+	dev->net->ethtool_ops = &ax88772_ethtool_ops;
+
+	/* Register suspend and resume functions */
+	data->suspend = ax88772_suspend;
+	data->resume = ax88772_resume;
+
+	ax8817x_mdio_write_le(dev->net, dev->mii.phy_id, MII_BMCR, BMCR_RESET);
+	ax8817x_mdio_write_le(dev->net, dev->mii.phy_id, MII_ADVERTISE,
+			ADVERTISE_ALL | ADVERTISE_CSMA | ADVERTISE_PAUSE_CAP);
 
-	asix_mdio_write(dev->net, dev->mii.phy_id, MII_BMCR, BMCR_RESET);
-	asix_mdio_write(dev->net, dev->mii.phy_id, MII_ADVERTISE,
-		ADVERTISE_ALL | ADVERTISE_CSMA | ADVERTISE_PAUSE_CAP);
 	mii_nway_restart(&dev->mii);
+	ax772a_data->autoneg_start = jiffies;
+	ax772a_data->Event = WAIT_AUTONEG_COMPLETE;
 
-	return 0;
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_MEDIUM_MODE,
+				0, 0, 0, NULL)) < 0) {
+		deverr(dev, "Write medium mode register: %d", ret);
+		goto out2;
+	}
+
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_IPG0,
+			AX88772A_IPG0_DEFAULT | AX88772A_IPG1_DEFAULT << 8,
+			AX88772A_IPG2_DEFAULT, 0, NULL)) < 0) {
+		deverr(dev, "Write IPG,IPG1,IPG2 failed: %d", ret);
+		goto out2;
+	}
+
+	/* Set RX_CTL to default values with 2k buffer, and enable cactus */
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_RX_CTL,
+			(AX_RX_CTL_START | AX_RX_CTL_AB),
+			0, 0, NULL)) < 0) {
+		deverr(dev, "Reset RX_CTL failed: %d", ret);
+		goto out2;
+	}
+
+	/* Asix framing packs multiple eth frames into a 2K usb bulk transfer */
+	if (dev->driver_info->flags & FLAG_FRAMING_AX) {
+		/* hard_mtu  is still the default - the device does not support
+		   jumbo eth frames */
+		dev->rx_urb_size = 2048;
+	}
+
+	kfree (buf);
 
-out:
+	printk (version);
+
+	return ret;
+out2:
+	destroy_workqueue (ax772a_data->ax_work);
+	kfree (ax772a_data);
+	kfree (buf);
+out1:
 	return ret;
 }
 
-static const struct ethtool_ops ax88772_ethtool_ops = {
-	.get_drvinfo		= asix_get_drvinfo,
-	.get_link		= asix_get_link,
-	.get_msglevel		= usbnet_get_msglevel,
-	.set_msglevel		= usbnet_set_msglevel,
-	.get_wol		= asix_get_wol,
-	.set_wol		= asix_set_wol,
-	.get_eeprom_len		= asix_get_eeprom_len,
-	.get_eeprom		= asix_get_eeprom,
-	.get_settings		= usbnet_get_settings,
-	.set_settings		= usbnet_set_settings,
-	.nway_reset		= usbnet_nway_reset,
-};
-
-static int ax88772_link_reset(struct usbnet *dev)
+static void ax88772a_unbind(struct usbnet *dev, struct usb_interface *intf)
 {
-	u16 mode;
-	struct ethtool_cmd ecmd = { .cmd = ETHTOOL_GSET };
+	struct ax88772a_data *ax772a_data = (struct ax88772a_data *)dev->priv;
+
+	if (ax772a_data) {
+
+		flush_workqueue (ax772a_data->ax_work);
+		destroy_workqueue (ax772a_data->ax_work);
+
+		/* stop MAC operation */
+		ax8817x_write_cmd(dev, AX_CMD_WRITE_RX_CTL,
+					AX_RX_CTL_STOP, 0, 0, NULL);
+
+		/* Power down PHY */
+		ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+					AX_SWRESET_IPPD, 0, 0, NULL);
+
+		kfree (ax772a_data);
+	}
+}
 
-	mii_check_media(&dev->mii, 1, 1);
-	mii_ethtool_gset(&dev->mii, &ecmd);
-	mode = AX88772_MEDIUM_DEFAULT;
+static int ax88772b_set_csums(struct usbnet *dev)
+{
+	struct ax88772b_data *ax772b_data = (struct ax88772b_data *)dev->priv;
+	u16 checksum;
 
-	if (ethtool_cmd_speed(&ecmd) != SPEED_100)
-		mode &= ~AX_MEDIUM_PS;
+	if (ax772b_data->checksum & AX_RX_CHECKSUM)
+		checksum = AX_RXCOE_DEF_CSUM;
+	else
+		checksum = 0;
 
-	if (ecmd.duplex != DUPLEX_FULL)
-		mode &= ~AX_MEDIUM_FD;
+	ax8817x_write_cmd (dev, AX_CMD_WRITE_RXCOE_CTL,
+				 checksum, 0, 0, NULL);
 
-	netdev_dbg(dev->net, "ax88772_link_reset() speed: %u duplex: %d setting mode to 0x%04x\n",
-		   ethtool_cmd_speed(&ecmd), ecmd.duplex, mode);
+	if (ax772b_data->checksum & AX_TX_CHECKSUM)
+		checksum = AX_TXCOE_DEF_CSUM;
+	else
+		checksum = 0;
 
-	asix_write_medium_mode(dev, mode);
+	ax8817x_write_cmd (dev, AX_CMD_WRITE_TXCOE_CTL,
+				 checksum, 0, 0, NULL);
 
 	return 0;
 }
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3,3,0)
+static u32 ax88772b_get_tx_csum(struct net_device *netdev)
+{
+	struct usbnet *dev = netdev_priv(netdev);
+	struct ax88772b_data *ax772b_data = (struct ax88772b_data *)dev->priv;
 
-static const struct net_device_ops ax88772_netdev_ops = {
-	.ndo_open		= usbnet_open,
-	.ndo_stop		= usbnet_stop,
-	.ndo_start_xmit		= usbnet_start_xmit,
-	.ndo_tx_timeout		= usbnet_tx_timeout,
-	.ndo_change_mtu		= usbnet_change_mtu,
-	.ndo_set_mac_address 	= asix_set_mac_address,
-	.ndo_validate_addr	= eth_validate_addr,
-	.ndo_do_ioctl		= asix_ioctl,
-	.ndo_set_multicast_list = asix_set_multicast,
-};
+	return (ax772b_data->checksum & AX_TX_CHECKSUM);
+}
 
-static int ax88772_bind(struct usbnet *dev, struct usb_interface *intf)
+static u32 ax88772b_get_rx_csum(struct net_device *netdev)
+{
+	struct usbnet *dev = netdev_priv(netdev);
+	struct ax88772b_data *ax772b_data = (struct ax88772b_data *)dev->priv;
+
+	return (ax772b_data->checksum & AX_RX_CHECKSUM);
+}
+
+static int ax88772b_set_rx_csum(struct net_device *netdev, u32 val)
+{
+	struct usbnet *dev = netdev_priv(netdev);
+	struct ax88772b_data *ax772b_data = (struct ax88772b_data *)dev->priv;
+
+	if (val)
+		ax772b_data->checksum |= AX_RX_CHECKSUM;
+	else
+		ax772b_data->checksum &= ~AX_RX_CHECKSUM;
+
+	return ax88772b_set_csums(dev);
+}
+
+static int ax88772b_set_tx_csum(struct net_device *netdev, u32 val)
 {
-	int ret, embd_phy;
-	u16 rx_ctl;
-	struct asix_data *data = (struct asix_data *)&dev->data;
-	u8 buf[ETH_ALEN];
-	u32 phyid;
+	struct usbnet *dev = netdev_priv(netdev);
+	struct ax88772b_data *ax772b_data = (struct ax88772b_data *)dev->priv;
 
-	data->eeprom_len = AX88772_EEPROM_LEN;
+	if (val)
+		ax772b_data->checksum |= AX_TX_CHECKSUM;
+	else
+		ax772b_data->checksum &= ~AX_TX_CHECKSUM;
+
+	ethtool_op_set_tx_csum(netdev, val);
+
+	return ax88772b_set_csums(dev);
+}
+#endif
+static struct ethtool_ops ax88772b_ethtool_ops = {
+	.get_drvinfo		= ax8817x_get_drvinfo,
+	.get_link		= ethtool_op_get_link,
+	.get_msglevel		= axusbnet_get_msglevel,
+	.set_msglevel		= axusbnet_set_msglevel,
+	.get_wol		= ax8817x_get_wol,
+	.set_wol		= ax8817x_set_wol,
+	.get_eeprom_len		= ax8817x_get_eeprom_len,
+	.get_eeprom		= ax8817x_get_eeprom,
+	.get_settings		= ax8817x_get_settings,
+	.set_settings		= ax8817x_set_settings,
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3,3,0)
+	.set_tx_csum		= ax88772b_set_tx_csum,
+	.get_tx_csum		= ax88772b_get_tx_csum,
+	.get_rx_csum		= ax88772b_get_rx_csum,
+	.set_rx_csum		= ax88772b_set_rx_csum,
+#endif
+};
 
-	usbnet_get_endpoints(dev,intf);
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,29)
+static const struct net_device_ops ax88772b_netdev_ops = {
+	.ndo_open			= axusbnet_open,
+	.ndo_stop			= axusbnet_stop,
+	.ndo_start_xmit	= axusbnet_start_xmit,
+	.ndo_tx_timeout	= axusbnet_tx_timeout,
+	.ndo_change_mtu	= axusbnet_change_mtu,
+	.ndo_do_ioctl		= ax8817x_ioctl,
+	.ndo_get_stats		= axusbnet_get_stats,
+	.ndo_set_mac_address 	= ax8817x_set_mac_addr,
+	.ndo_validate_addr		= eth_validate_addr,
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(3,2,0)
+	.ndo_set_multicast_list	= ax88772b_set_multicast,
+#else
+	.ndo_set_rx_mode	= ax88772b_set_multicast,
+#endif
+};
+#endif
 
-	if ((ret = asix_write_gpio(dev,
-			AX_GPIO_RSE | AX_GPIO_GPO_2 | AX_GPIO_GPO2EN, 5)) < 0)
-		goto out;
+static int ax88772b_bind(struct usbnet *dev, struct usb_interface *intf)
+{
+	int ret;
+	void *buf;
+	struct ax8817x_data *data = (struct ax8817x_data *)&dev->data;
+	struct ax88772b_data *ax772b_data;
+	u16 *tmp16;
+	u8 i;
+	u8 TempPhySelect;
+	bool InternalPhy;
+	u8 default_asix_mac[ETH_ALEN] = { 0x00, 0x0e, 0xc6, 0x87, 0x72, 0x01 };
+
+	axusbnet_get_endpoints(dev,intf);
+
+	buf = kmalloc (6, GFP_KERNEL);
+	if (!buf) {
+		deverr(dev, "Cannot allocate memory for buffer");
+		return -ENOMEM;
+	}
+	tmp16 = (u16 *)buf;
 
-	/* 0x10 is the phy id of the embedded 10/100 ethernet phy */
-	embd_phy = ((asix_get_phy_addr(dev) & 0x1f) == 0x10 ? 1 : 0);
-	if ((ret = asix_write_cmd(dev, AX_CMD_SW_PHY_SELECT,
-				embd_phy, 0, 0, NULL)) < 0) {
-		dbg("Select PHY #1 failed: %d", ret);
-		goto out;
+	ax772b_data = kmalloc (sizeof(*ax772b_data), GFP_KERNEL);
+	if (!ax772b_data) {
+		deverr(dev, "Cannot allocate memory for AX88772B data");
+		kfree (buf);
+		return -ENOMEM;
+	}
+	memset (ax772b_data, 0, sizeof(*ax772b_data));
+	dev->priv = ax772b_data;
+
+	ax772b_data->ax_work = create_singlethread_workqueue ("ax88772b");
+	if (!ax772b_data->ax_work) {
+		kfree (buf);
+		kfree (ax772b_data);
+		return -ENOMEM;
 	}
 
-	if ((ret = asix_sw_reset(dev, AX_SWRESET_IPPD | AX_SWRESET_PRL)) < 0)
-		goto out;
+	ax772b_data->dev = dev;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	INIT_WORK (&ax772b_data->check_link, ax88772b_link_reset, dev);
+#else
+	INIT_WORK (&ax772b_data->check_link, ax88772b_link_reset);
+#endif
+
+	if ((ret = ax8817x_read_cmd (dev, AX_CMD_SW_PHY_STATUS, 0, 0, 1,
+        					&TempPhySelect)) < 0) {
+		deverr(dev, "read SW interface selection status register"
+							"failed: %d\n", ret);
+		goto err_out;
+	}
 
-	msleep(150);
-	if ((ret = asix_sw_reset(dev, AX_SWRESET_CLEAR)) < 0)
-		goto out;
+	TempPhySelect &= 0x0C;
 
-	msleep(150);
-	if (embd_phy) {
-		if ((ret = asix_sw_reset(dev, AX_SWRESET_IPRL)) < 0)
-			goto out;
+	if (TempPhySelect == AX_PHYSEL_SSRMII) {
+		InternalPhy = false;
+		ax772b_data->OperationMode = OPERATION_MAC_MODE;
+		ax772b_data->PhySelect = 0x00;
+	}
+	else if (TempPhySelect == AX_PHYSEL_SSRRMII) {
+		InternalPhy = true;
+		ax772b_data->OperationMode = OPERATION_PHY_MODE;
+		ax772b_data->PhySelect = 0x00;
+	}
+	else if (TempPhySelect == AX_PHYSEL_SSMII) {
+		InternalPhy = true;
+		ax772b_data->OperationMode = OPERATION_MAC_MODE;
+		ax772b_data->PhySelect = 0x01;
 	}
 	else {
-		if ((ret = asix_sw_reset(dev, AX_SWRESET_PRTE)) < 0)
-			goto out;
+		deverr(dev, "Unknown MII type\n");
+		goto err_out;
 	}
 
-	msleep(150);
-	rx_ctl = asix_read_rx_ctl(dev);
-	dbg("RX_CTL is 0x%04x after software reset", rx_ctl);
-	if ((ret = asix_write_rx_ctl(dev, 0x0000)) < 0)
-		goto out;
+	/* reload eeprom data */
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_GPIOS,
+			AXGPIOS_RSE, 0, 0, NULL)) < 0) {
+		deverr(dev, "Failed to enable GPIO finction: %d", ret);
+		goto err_out;
+	}
+	msleep(5);
 
-	rx_ctl = asix_read_rx_ctl(dev);
-	dbg("RX_CTL is 0x%04x setting to 0x0000", rx_ctl);
+	/* Get the EEPROM data*/
+	if ((ret = ax8817x_read_cmd (dev, AX_CMD_READ_EEPROM,
+				     0x18, 0, 2, (void *)tmp16)) < 0) {
+		deverr(dev, "read SROM address 18h failed: %d", ret);
+		goto err_out;
+	}
+	le16_to_cpus(tmp16);
+	ax772b_data->psc = *tmp16 & 0xFF00;
+	/* End of get EEPROM data */
+
+	/* Get the MAC address from EEPROM */
+	memset(buf, 0, ETH_ALEN);
+	for (i = 0; i < (ETH_ALEN >> 1); i++) {
+		if ((ret = ax8817x_read_cmd (dev, AX_CMD_READ_EEPROM,
+					0x04 + i, 0, 2, (buf + i * 2))) < 0) {
+			deverr(dev, "read SROM address 04h failed: %d", ret);
+			goto err_out;
+		}
+	}
 
-	/* Get the MAC address */
-	if ((ret = asix_read_cmd(dev, AX_CMD_READ_NODE_ID,
-				0, 0, ETH_ALEN, buf)) < 0) {
-		dbg("Failed to read MAC address: %d", ret);
-		goto out;
+	/* Check for default ASIX MAC (e.g. 00:0e:c6:87:72:01) in case of no EEPROM being present */
+	if (!memcmp(buf, default_asix_mac, ETH_ALEN)) {
+		if (g_usr_mac && (g_usr_mac < 3)) {
+			/* Get user set MAC address */
+			if (g_usr_mac == 2) {
+				/* 0x100000 offset for 2nd Ethernet MAC */
+				g_mac_addr[3] += 0x10;
+				if (g_mac_addr[3] < 0x10)
+					devwarn(dev, "MAC address byte 3 (0x%02x) wrap around", g_mac_addr[3]);
+			}
+			memcpy(buf, g_mac_addr, ETH_ALEN);
+			g_usr_mac++;
+		} else devwarn(dev, "using default ASIX MAC");
 	}
 	memcpy(dev->net->dev_addr, buf, ETH_ALEN);
 
+	/* Set the MAC address */
+	if ((ret = ax8817x_write_cmd (dev, AX88772_CMD_WRITE_NODE_ID,
+			0, 0, ETH_ALEN, buf)) < 0) {
+		deverr(dev, "set MAC address failed: %d", ret);
+		goto err_out;
+	}
+
 	/* Initialize MII structure */
 	dev->mii.dev = dev->net;
-	dev->mii.mdio_read = asix_mdio_read;
-	dev->mii.mdio_write = asix_mdio_write;
-	dev->mii.phy_id_mask = 0x1f;
-	dev->mii.reg_num_mask = 0x1f;
-	dev->mii.phy_id = asix_get_phy_addr(dev);
+	dev->mii.mdio_read = ax8817x_mdio_read_le;
+	dev->mii.mdio_write = ax88772b_mdio_write_le;
+	dev->mii.phy_id_mask = 0xff;
+	dev->mii.reg_num_mask = 0xff;
 
-	phyid = asix_get_phyid(dev);
-	dbg("PHYID=0x%08x", phyid);
+	/* Get the PHY id */
+	if ((ret = ax8817x_read_cmd(dev, AX_CMD_READ_PHY_ID,
+			0, 0, 2, buf)) < 0) {
+		deverr(dev, "Error reading PHY ID: %02x", ret);
+		goto err_out;
+	} else if (ret < 2) {
+		/* this should always return 2 bytes */
+		deverr(dev, "Read PHYID returned less than 2 bytes: ret=%02x",
+		    ret);
+		ret = -EIO;
+		goto err_out;
+	}
 
-	if ((ret = asix_sw_reset(dev, AX_SWRESET_PRL)) < 0)
-		goto out;
+	if (InternalPhy)
+		dev->mii.phy_id = *((u8 *)buf + 1);
+	else
+		dev->mii.phy_id = *((u8 *)buf);
 
-	msleep(150);
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_SW_PHY_SELECT, 
+			ax772b_data->PhySelect, 0, 0, NULL)) < 0) {
+		deverr(dev, "Select PHY #1 failed: %d", ret);
+		goto err_out;
+	}
 
-	if ((ret = asix_sw_reset(dev, AX_SWRESET_IPRL | AX_SWRESET_PRL)) < 0)
-		goto out;
+#if 0
+	/* select the embedded 10/100 Ethernet PHY */
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_SW_PHY_SELECT,
+			AX_PHYSEL_SSEN | AX_PHYSEL_PSEL | AX_PHYSEL_SSMII
+						, 0, 0, NULL)) < 0) {
+		deverr(dev, "Select PHY #1 failed: %d", ret);
+		goto err_out;
+	}
 
-	msleep(150);
+	if(dev->mii.phy_id != 0x10) {
+		deverr(dev, "Got wrong PHY ID: %02x", dev->mii.phy_id);
+		ret = -EIO;
+		goto err_out;
+	}
+#endif
+	if ((ret = ax88772a_phy_powerup (dev)) < 0)
+		goto err_out;
+
+	/* stop MAC operation */
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_RX_CTL,
+			AX_RX_CTL_STOP, 0, 0, NULL)) < 0) {
+		deverr(dev, "Reset RX_CTL failed: %d", ret);
+		goto err_out;
+	}
 
-	dev->net->netdev_ops = &ax88772_netdev_ops;
-	dev->net->ethtool_ops = &ax88772_ethtool_ops;
+	/* make sure the driver can enable sw mii operation */
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_SET_SW_MII,
+			0, 0, 0, NULL)) < 0) {
+		deverr(dev, "Enabling software MII failed: %d", ret);
+		goto err_out;
+	}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,30)
+	dev->net->do_ioctl = ax8817x_ioctl;
+	dev->net->set_multicast_list = ax88772b_set_multicast;
+	dev->net->set_mac_address = ax8817x_set_mac_addr;
+#else
+	dev->net->netdev_ops = &ax88772b_netdev_ops;
+#endif
+
+	dev->net->ethtool_ops = &ax88772b_ethtool_ops;
+
+	/* Register suspend and resume functions */
+	data->suspend = ax88772b_suspend;
+	data->resume = ax88772b_resume;
+
+	if (ax772b_data->OperationMode == OPERATION_PHY_MODE)
+		ax8817x_mdio_write_le(dev->net, dev->mii.phy_id
+						, MII_BMCR, 0x3900);
+
+	if (dev->mii.phy_id != 0x10)
+		ax8817x_mdio_write_le(dev->net, 0x10, MII_BMCR, 0x3900);
+		
+
+	if (dev->mii.phy_id == 0x10 && ax772b_data->OperationMode 
+						!= OPERATION_PHY_MODE) {
+
+		*tmp16 = ax8817x_mdio_read_le(dev->net, dev->mii.phy_id, 0x12);
+		ax8817x_mdio_write_le(dev->net, dev->mii.phy_id, 0x12,
+					((*tmp16 & 0xFF9F) | 0x0040));
+	}
+	
+	ax8817x_mdio_write_le(dev->net, dev->mii.phy_id, MII_ADVERTISE,
+			ADVERTISE_ALL | ADVERTISE_CSMA | ADVERTISE_PAUSE_CAP);
 
-	asix_mdio_write(dev->net, dev->mii.phy_id, MII_BMCR, BMCR_RESET);
-	asix_mdio_write(dev->net, dev->mii.phy_id, MII_ADVERTISE,
-			ADVERTISE_ALL | ADVERTISE_CSMA);
 	mii_nway_restart(&dev->mii);
 
-	if ((ret = asix_write_medium_mode(dev, AX88772_MEDIUM_DEFAULT)) < 0)
-		goto out;
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_MEDIUM_MODE,
+				0, 0, 0, NULL)) < 0) {
+		deverr(dev, "Failed to write medium mode: %d", ret);
+		goto err_out;
+	}
 
-	if ((ret = asix_write_cmd(dev, AX_CMD_WRITE_IPG0,
-				AX88772_IPG0_DEFAULT | AX88772_IPG1_DEFAULT,
-				AX88772_IPG2_DEFAULT, 0, NULL)) < 0) {
-		dbg("Write IPG,IPG1,IPG2 failed: %d", ret);
-		goto out;
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_IPG0,
+			AX88772A_IPG0_DEFAULT | AX88772A_IPG1_DEFAULT << 8,
+			AX88772A_IPG2_DEFAULT, 0, NULL)) < 0) {
+		deverr(dev, "Failed to write interframe gap: %d", ret);
+		goto err_out;
 	}
 
-	/* Set RX_CTL to default values with 2k buffer, and enable cactus */
-	if ((ret = asix_write_rx_ctl(dev, AX_DEFAULT_RX_CTL)) < 0)
-		goto out;
+	dev->net->features |= NETIF_F_IP_CSUM;
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,22)
+	dev->net->features |= NETIF_F_IPV6_CSUM;
+#endif
 
-	rx_ctl = asix_read_rx_ctl(dev);
-	dbg("RX_CTL is 0x%04x after all initializations", rx_ctl);
+	ax772b_data->checksum = AX_RX_CHECKSUM | AX_TX_CHECKSUM;
+	if ((ret = ax88772b_set_csums(dev)) < 0) {
+		deverr(dev, "Write RX_COE/TX_COE failed: %d", ret);
+		goto err_out;
+	}
 
-	rx_ctl = asix_read_medium_status(dev);
-	dbg("Medium Status is 0x%04x after all initializations", rx_ctl);
+	dev->rx_size = bsize & 0x07;
+	if (dev->udev->speed == USB_SPEED_HIGH) {
 
-	/* Asix framing packs multiple eth frames into a 2K usb bulk transfer */
-	if (dev->driver_info->flags & FLAG_FRAMING_AX) {
-		/* hard_mtu  is still the default - the device does not support
-		   jumbo eth frames */
+		if ((ret = ax8817x_write_cmd (dev, 0x2A,
+				AX88772B_BULKIN_SIZE[dev->rx_size].byte_cnt,
+				AX88772B_BULKIN_SIZE[dev->rx_size].threshold,
+				0, NULL)) < 0) {
+			deverr(dev, "Reset RX_CTL failed: %d", ret);
+			goto err_out;
+		}
+
+		dev->rx_urb_size = AX88772B_BULKIN_SIZE[dev->rx_size].size;
+	} else {
+		if ((ret = ax8817x_write_cmd (dev, 0x2A,
+				0x8000, 0x8001, 0, NULL)) < 0) {
+			deverr(dev, "Reset RX_CTL failed: %d", ret);
+			goto err_out;
+		}
 		dev->rx_urb_size = 2048;
 	}
-	return 0;
 
-out:
+	/* Configure RX header type */
+	if ((ret = ax8817x_write_cmd (dev, AX_CMD_WRITE_RX_CTL,
+		      (AX_RX_CTL_START | AX_RX_CTL_AB | AX_RX_HEADER_DEFAULT),
+		      0, 0, NULL)) < 0) {
+		deverr(dev, "Reset RX_CTL failed: %d", ret);
+		goto err_out;
+	}
+
+	/* Overwrite power saving configuration from eeprom */
+	if ((ret = ax8817x_write_cmd (dev, AX_CMD_SW_RESET,
+	    AX_SWRESET_IPRL | (ax772b_data->psc & 0x7FFF), 0, 0, NULL)) < 0) {
+		deverr(dev, "Failed to configure PHY power saving: %d", ret);
+		goto err_out;
+	}
+
+	if (ax772b_data->OperationMode == OPERATION_PHY_MODE)
+		netif_carrier_on(dev->net);
+
+	kfree (buf);
+	printk (version);
+
+	return ret;
+err_out:
+	destroy_workqueue (ax772b_data->ax_work);
+	kfree (buf);
+	kfree (ax772b_data);
 	return ret;
 }
 
-static struct ethtool_ops ax88178_ethtool_ops = {
-	.get_drvinfo		= asix_get_drvinfo,
-	.get_link		= asix_get_link,
-	.get_msglevel		= usbnet_get_msglevel,
-	.set_msglevel		= usbnet_set_msglevel,
-	.get_wol		= asix_get_wol,
-	.set_wol		= asix_set_wol,
-	.get_eeprom_len		= asix_get_eeprom_len,
-	.get_eeprom		= asix_get_eeprom,
-	.get_settings		= usbnet_get_settings,
-	.set_settings		= usbnet_set_settings,
-	.nway_reset		= usbnet_nway_reset,
-};
+static void ax88772b_unbind(struct usbnet *dev, struct usb_interface *intf)
+{
+	struct ax88772b_data *ax772b_data = (struct ax88772b_data *)dev->priv;
+
+	if (ax772b_data) {
+		/* Check for user set MAC address */
+		if (!memcmp(dev->net->dev_addr, g_mac_addr, ETH_ALEN)) {
+			/* Release user set MAC address */
+			g_usr_mac--;
+
+			if (g_usr_mac == 2) {
+				/* 0x100000 offset for 2nd Ethernet MAC */
+				g_mac_addr[3] -= 0x10;
+				if (g_mac_addr[3] > 0xf0)
+					devwarn(dev, "MAC address byte 3 (0x%02x) wrap around", g_mac_addr[3]);
+			}
+		}
+
+		flush_workqueue (ax772b_data->ax_work);
+		destroy_workqueue (ax772b_data->ax_work);
+
+		/* stop MAC operation */
+		ax8817x_write_cmd(dev, AX_CMD_WRITE_RX_CTL,
+					AX_RX_CTL_STOP, 0, 0, NULL);
+
+		/* Power down PHY */
+		ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+					AX_SWRESET_IPPD, 0, 0, NULL);
+
+		kfree (ax772b_data);
+	}
+}
 
-static int marvell_phy_init(struct usbnet *dev)
+static int 
+ax88178_media_check (struct usbnet *dev, struct ax88178_data *ax178dataptr)
 {
-	struct asix_data *data = (struct asix_data *)&dev->data;
-	u16 reg;
+	int fullduplex;
+	u16 tempshort = 0;
+	u16 media;
+	u16 advertise, lpa, result, stat1000;
+
+	advertise = ax8817x_mdio_read_le (dev->net,
+				dev->mii.phy_id, MII_ADVERTISE);
+	lpa = ax8817x_mdio_read_le (dev->net, dev->mii.phy_id, MII_LPA);
+	result = advertise & lpa;
+
+	stat1000 = ax8817x_mdio_read_le (dev->net,
+				dev->mii.phy_id, MII_STAT1000);
+
+	if ((ax178dataptr->PhyMode == PHY_MODE_MARVELL) &&
+	    (ax178dataptr->LedMode == 1)) { 
+		tempshort = ax8817x_mdio_read_le (dev->net,
+				dev->mii.phy_id, MARVELL_MANUAL_LED) & 0xfc0f;
+	}
 
-	netdev_dbg(dev->net, "marvell_phy_init()\n");
+	fullduplex=1;
+	if (stat1000 & LPA_1000FULL) {
+		media = MEDIUM_GIGA_MODE | MEDIUM_FULL_DUPLEX_MODE |
+			MEDIUM_ENABLE_125MHZ | MEDIUM_ENABLE_RECEIVE;
+		if ((ax178dataptr->PhyMode == PHY_MODE_MARVELL) &&
+		    (ax178dataptr->LedMode == 1))
+			tempshort|= 0x3e0;
+	} else if (result & LPA_100FULL) {
+		media = MEDIUM_FULL_DUPLEX_MODE | MEDIUM_ENABLE_RECEIVE |
+			MEDIUM_MII_100M_MODE;
+		if ((ax178dataptr->PhyMode == PHY_MODE_MARVELL) &&
+		    (ax178dataptr->LedMode == 1))
+			tempshort|= 0x3b0;
+	} else if (result & LPA_100HALF) {
+		fullduplex = 0;
+		media = MEDIUM_ENABLE_RECEIVE | MEDIUM_MII_100M_MODE;
+		if ((ax178dataptr->PhyMode == PHY_MODE_MARVELL) &&
+		    (ax178dataptr->LedMode == 1))
+			tempshort |= 0x3b0;
+	} else if (result & LPA_10FULL) {
+		media = MEDIUM_FULL_DUPLEX_MODE | MEDIUM_ENABLE_RECEIVE;
+		if ((ax178dataptr->PhyMode == PHY_MODE_MARVELL) &&
+		    (ax178dataptr->LedMode == 1))
+			tempshort |= 0x2f0;
+	} else {
+		media = MEDIUM_ENABLE_RECEIVE;
+		fullduplex=0;
+		if ((ax178dataptr->PhyMode == PHY_MODE_MARVELL) &&
+		    (ax178dataptr->LedMode == 1))
+				tempshort |= 0x02f0;
+	}
 
-	reg = asix_mdio_read(dev->net, dev->mii.phy_id, MII_MARVELL_STATUS);
-	netdev_dbg(dev->net, "MII_MARVELL_STATUS = 0x%04x\n", reg);
+	if ((ax178dataptr->PhyMode == PHY_MODE_MARVELL) &&
+	    (ax178dataptr->LedMode == 1)) {
+		ax8817x_mdio_write_le (dev->net, 
+			dev->mii.phy_id, MARVELL_MANUAL_LED, tempshort);
+	}
 
-	asix_mdio_write(dev->net, dev->mii.phy_id, MII_MARVELL_CTRL,
-			MARVELL_CTRL_RXDELAY | MARVELL_CTRL_TXDELAY);
+	media |= 0x0004;
+	if(ax178dataptr->UseRgmii)
+		media |= 0x0008;
+	if(fullduplex) {
+		media |= 0x0020;  //ebable tx flow control as default;
+		media |= 0x0010;  //ebable rx flow control as default;
+	}
 
-	if (data->ledmode) {
-		reg = asix_mdio_read(dev->net, dev->mii.phy_id,
-			MII_MARVELL_LED_CTRL);
-		netdev_dbg(dev->net, "MII_MARVELL_LED_CTRL (1) = 0x%04x\n", reg);
+	return media;
+}
 
-		reg &= 0xf8ff;
-		reg |= (1 + 0x0100);
-		asix_mdio_write(dev->net, dev->mii.phy_id,
-			MII_MARVELL_LED_CTRL, reg);
+static void Vitess_8601_Init (struct usbnet *dev, int State)
+{
+	u16 reg;
 
-		reg = asix_mdio_read(dev->net, dev->mii.phy_id,
-			MII_MARVELL_LED_CTRL);
-		netdev_dbg(dev->net, "MII_MARVELL_LED_CTRL (2) = 0x%04x\n", reg);
-		reg &= 0xfc0f;
+	switch (State) {
+	case 0:	// tx, rx clock skew
+		ax8817x_swmii_mdio_write_le (dev->net, dev->mii.phy_id, 31, 1);
+		ax8817x_swmii_mdio_write_le (dev->net, dev->mii.phy_id, 28, 0);
+		ax8817x_swmii_mdio_write_le (dev->net, dev->mii.phy_id, 31, 0);
+		break;
+
+	case 1:
+		ax8817x_swmii_mdio_write_le (dev->net,
+				dev->mii.phy_id, 31, 0x52B5);
+		ax8817x_swmii_mdio_write_le (dev->net,
+				dev->mii.phy_id, 18, 0x009E);
+		ax8817x_swmii_mdio_write_le (dev->net,
+				dev->mii.phy_id, 17, 0xDD39);
+		ax8817x_swmii_mdio_write_le (dev->net,
+				dev->mii.phy_id, 16, 0x87AA);
+
+		ax8817x_swmii_mdio_write_le (dev->net,
+				dev->mii.phy_id, 16, 0xA7B4);
+
+		ax8817x_swmii_mdio_write_le (dev->net,
+				dev->mii.phy_id, 18,
+				ax8817x_swmii_mdio_read_le (dev->net,
+						dev->mii.phy_id, 18));
+
+		reg = (ax8817x_swmii_mdio_read_le (dev->net,
+				dev->mii.phy_id, 17) & ~0x003f) | 0x003c;
+		ax8817x_swmii_mdio_write_le (dev->net,
+				dev->mii.phy_id, 17, reg);
+		ax8817x_swmii_mdio_write_le (dev->net,
+				dev->mii.phy_id, 16, 0x87B4);
+
+		ax8817x_swmii_mdio_write_le (dev->net,
+				dev->mii.phy_id, 16, 0xa794);
+
+		ax8817x_swmii_mdio_write_le (dev->net,
+				dev->mii.phy_id, 18,
+				ax8817x_swmii_mdio_read_le (dev->net,
+					dev->mii.phy_id, 18));
+
+		reg = (ax8817x_swmii_mdio_read_le (dev->net,
+				dev->mii.phy_id, 17) & ~0x003f) | 0x003e;
+		ax8817x_swmii_mdio_write_le (dev->net,
+				dev->mii.phy_id, 17, reg);
+		ax8817x_swmii_mdio_write_le (dev->net,
+				dev->mii.phy_id, 16, 0x8794);
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 18, 0x00f7);
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 17, 0xbe36);
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 16, 0x879e);
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 16, 0xa7a0);
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 18, 
+				ax8817x_swmii_mdio_read_le (dev->net, 
+						dev->mii.phy_id, 18));
+
+		reg = (ax8817x_swmii_mdio_read_le (dev->net, 
+				dev->mii.phy_id, 17) & ~0x003f) | 0x0034;
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 17, reg);
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 16, 0x87a0);
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 18, 0x003c);
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 17, 0xf3cf);
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 16, 0x87a2);
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 18, 0x003c);
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 17, 0xf3cf);
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 16, 0x87a4);
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 18, 0x003c);
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 17, 0xd287);
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 16, 0x87a6);
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 16, 0xa7a8);
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 18, 
+				ax8817x_swmii_mdio_read_le (dev->net, 
+						dev->mii.phy_id, 18));
+
+		reg = (ax8817x_swmii_mdio_read_le (dev->net, 
+				dev->mii.phy_id, 17) & ~0x0fff) | 0x0125;
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 17, reg);
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 16, 0x87a8);
+
+		// Enable Smart Pre-emphasis
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 16, 0xa7fa);
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 18, 
+				ax8817x_swmii_mdio_read_le (dev->net, 
+						dev->mii.phy_id, 18));
+
+		reg = (ax8817x_swmii_mdio_read_le (dev->net, 
+				dev->mii.phy_id, 17) & ~0x0008) | 0x0008;
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 17, reg);
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 16, 0x87fa);
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 31, 0);
+
+		break;
 	}
+}
 
-	return 0;
+static void
+marvell_88E1510_magic_init(struct usbnet *dev)
+{
+	ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 22, 0xff);
+	ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 17, 0x214b);
+	ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 16, 0x2144);
+	ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 17, 0x0c28);
+	ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 16, 0x2146);
+	ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 17, 0xb233);
+	ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 16, 0x214d);
+	ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 17, 0xcc0c);
+	ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 16, 0x2159);
+	ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 22, 0x00fb);
+	ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 7, 0xc00d);
+	ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 22, 0);
 }
 
-static int marvell_led_status(struct usbnet *dev, u16 speed)
+static int 
+ax88178_phy_init (struct usbnet *dev, struct ax88178_data *ax178dataptr)
 {
-	u16 reg = asix_mdio_read(dev->net, dev->mii.phy_id, MARVELL_LED_MANUAL);
+	int i;
+	u16 PhyAnar, PhyAuxCtrl, PhyCtrl, TempShort, PhyID1;
+	u16 PhyReg = 0;
 
-	netdev_dbg(dev->net, "marvell_led_status() read 0x%04x\n", reg);
+	//Disable MII operation of AX88178 Hardware
+	ax8817x_write_cmd (dev, AX_CMD_SET_SW_MII, 0x0000, 0, 0, NULL);
 
-	/* Clear out the center LED bits - 0x03F0 */
-	reg &= 0xfc0f;
 
-	switch (speed) {
-		case SPEED_1000:
-			reg |= 0x03e0;
-			break;
-		case SPEED_100:
-			reg |= 0x03b0;
-			break;
-		default:
-			reg |= 0x02f0;
+	//Read SROM - MiiPhy Address (ID)
+	ax8817x_read_cmd (dev, AX_CMD_READ_PHY_ID, 0, 0, 2, &dev->mii.phy_id);
+	le32_to_cpus (&dev->mii.phy_id);
+
+	/* Initialize MII structure */
+	dev->mii.phy_id >>= 8;
+	dev->mii.phy_id &= PHY_ID_MASK;
+	dev->mii.dev = dev->net;
+	dev->mii.mdio_read = ax8817x_mdio_read_le;
+	dev->mii.mdio_write = ax8817x_mdio_write_le;
+	dev->mii.phy_id_mask = 0x3f;
+	dev->mii.reg_num_mask = 0x1f;
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,11)
+	dev->mii.supports_gmii = 1;
+#endif
+
+	if (ax178dataptr->PhyMode == PHY_MODE_MAC_TO_MAC_GMII)
+	{
+		ax178dataptr->UseRgmii = 0;
+		ax178dataptr->MediaLink = MEDIUM_GIGA_MODE |
+					  MEDIUM_FULL_DUPLEX_MODE |
+					  MEDIUM_ENABLE_125MHZ |
+					  MEDIUM_ENABLE_RECEIVE |
+					  MEDIUM_ENABLE_RX_FLOWCTRL |
+					  MEDIUM_ENABLE_TX_FLOWCTRL;
+
+		goto SkipPhySetting;
 	}
 
-	netdev_dbg(dev->net, "marvell_led_status() writing 0x%04x\n", reg);
-	asix_mdio_write(dev->net, dev->mii.phy_id, MARVELL_LED_MANUAL, reg);
+	// test read phy register 2
+	if (!ax178dataptr->UseGpio0) {
+		i = 1000;
+		while (i--) {
+			PhyID1 = ax8817x_swmii_mdio_read_le (dev->net, 
+						dev->mii.phy_id, GMII_PHY_OUI);
+			if ((PhyID1 == 0x000f) || (PhyID1 == 0x0141) ||
+			    (PhyID1 == 0x0282) || (PhyID1 == 0x004d) ||
+			    (PhyID1 == 0x0243) || (PhyID1 == 0x001C) ||
+			    (PhyID1 == 0x0007))
+				break;
+			msleep(5);
+		}
+		if (i < 0)
+			return -EIO;
+	}
 
-	return 0;
-}
+	ax178dataptr->UseRgmii = 0;
+	if (ax178dataptr->PhyMode == PHY_MODE_MARVELL) {
+		PhyReg = ax8817x_swmii_mdio_read_le(dev->net, 
+					dev->mii.phy_id, 27);
+		if (!(PhyReg & 4) && !(ax178dataptr->LedMode & 0x10)) {
+			ax178dataptr->UseRgmii = 1;
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 20, 0x82);
+			ax178dataptr->MediaLink |= MEDIUM_ENABLE_125MHZ;
+		} else if (ax178dataptr->LedMode & 0x10) {
+
+			ax178dataptr->UseRgmii = 1;
+			ax178dataptr->MediaLink |= MEDIUM_ENABLE_125MHZ;
+			marvell_88E1510_magic_init(dev);
+
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 22, 2);
+
+			PhyReg = ax8817x_swmii_mdio_read_le(dev->net, 
+					dev->mii.phy_id, 21);
+
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 21, PhyReg | 0x30);
+
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 22, 0);
+			
+		} 
+	} else if ((ax178dataptr->PhyMode == PHY_MODE_AGERE_V0) ||
+		 (ax178dataptr->PhyMode == PHY_MODE_AGERE_V0_GMII)) {
+		if (ax178dataptr->PhyMode == PHY_MODE_AGERE_V0) {
+			ax178dataptr->UseRgmii = 1;
+			ax178dataptr->MediaLink |= MEDIUM_ENABLE_125MHZ;
+		}
+	} else if (ax178dataptr->PhyMode == PHY_MODE_CICADA_V1) {
+		// not Cameo
+		if (!ax178dataptr->UseGpio0 || ax178dataptr->LedMode) {
+			ax178dataptr->UseRgmii = 1;
+			ax178dataptr->MediaLink |= MEDIUM_ENABLE_125MHZ;
+		}
 
-static int ax88178_link_reset(struct usbnet *dev)
-{
-	u16 mode;
-	struct ethtool_cmd ecmd = { .cmd = ETHTOOL_GSET };
-	struct asix_data *data = (struct asix_data *)&dev->data;
-	u32 speed;
+		for (i = 0; i < (sizeof(CICADA_FAMILY_HWINIT) / 
+			 	 sizeof(CICADA_FAMILY_HWINIT[0])); i++) {
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id,
+					CICADA_FAMILY_HWINIT[i].offset,
+					CICADA_FAMILY_HWINIT[i].value);
+		}
 
-	netdev_dbg(dev->net, "ax88178_link_reset()\n");
+	} else if (ax178dataptr->PhyMode == PHY_MODE_CICADA_V2) {
+		// not Cameo
+		if (!ax178dataptr->UseGpio0 || ax178dataptr->LedMode)
+		{
+			ax178dataptr->UseRgmii = 1;
+			ax178dataptr->MediaLink |= MEDIUM_ENABLE_125MHZ;
+		}
 
-	mii_check_media(&dev->mii, 1, 1);
-	mii_ethtool_gset(&dev->mii, &ecmd);
-	mode = AX88178_MEDIUM_DEFAULT;
-	speed = ethtool_cmd_speed(&ecmd);
+		for (i = 0; i < (sizeof(CICADA_V2_HWINIT) / 
+				 sizeof(CICADA_V2_HWINIT[0])); i++) {
+			ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, CICADA_V2_HWINIT[i].offset,
+				CICADA_V2_HWINIT[i].value);
+		}
+	} else if (ax178dataptr->PhyMode == PHY_MODE_CICADA_V2_ASIX) {
+		// not Cameo
+		if (!ax178dataptr->UseGpio0 || ax178dataptr->LedMode)
+		{
+			ax178dataptr->UseRgmii = 1;
+			ax178dataptr->MediaLink |= MEDIUM_ENABLE_125MHZ;
+		}
 
-	if (speed == SPEED_1000)
-		mode |= AX_MEDIUM_GM;
-	else if (speed == SPEED_100)
-		mode |= AX_MEDIUM_PS;
-	else
-		mode &= ~(AX_MEDIUM_PS | AX_MEDIUM_GM);
+		for (i = 0; i < (sizeof(CICADA_V2_HWINIT) / 
+				 sizeof(CICADA_V2_HWINIT[0])); i++) {
+			ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, CICADA_V2_HWINIT[i].offset, 
+				CICADA_V2_HWINIT[i].value);
+		}
+	} else if (ax178dataptr->PhyMode == PHY_MODE_RTL8211CL) {
+		ax178dataptr->UseRgmii = 1;
+		ax178dataptr->MediaLink |= MEDIUM_ENABLE_125MHZ;
+	} else if (ax178dataptr->PhyMode == PHY_MODE_RTL8211BN) {
+		ax178dataptr->UseRgmii = 1;
+		ax178dataptr->MediaLink |= MEDIUM_ENABLE_125MHZ;
+	} else if (ax178dataptr->PhyMode == PHY_MODE_RTL8251CL) {
+		ax178dataptr->UseRgmii = 1;
+		ax178dataptr->MediaLink |= MEDIUM_ENABLE_125MHZ;
+	} else if (ax178dataptr->PhyMode == PHY_MODE_VSC8601) {
+		ax178dataptr->UseRgmii = 1;
+		ax178dataptr->MediaLink |= MEDIUM_ENABLE_125MHZ;
+//		Vitess_8601_Init (dev, 0);
+	}
 
-	mode |= AX_MEDIUM_ENCK;
+	if (ax178dataptr->PhyMode != PHY_MODE_ATTANSIC_V0) {
+		// software reset
+		ax8817x_swmii_mdio_write_le (
+			dev->net, dev->mii.phy_id, GMII_PHY_CONTROL,
+			ax8817x_swmii_mdio_read_le (
+				dev->net, dev->mii.phy_id, GMII_PHY_CONTROL)
+				| GMII_CONTROL_RESET);
+		msleep (1);
+	}
 
-	if (ecmd.duplex == DUPLEX_FULL)
-		mode |= AX_MEDIUM_FD;
-	else
-		mode &= ~AX_MEDIUM_FD;
+	if ((ax178dataptr->PhyMode == PHY_MODE_AGERE_V0) ||
+	    (ax178dataptr->PhyMode == PHY_MODE_AGERE_V0_GMII)) {
+		if (ax178dataptr->PhyMode == PHY_MODE_AGERE_V0)
+		{
+			i = 1000;
+			while (i--)
+			{
+				ax8817x_swmii_mdio_write_le (dev->net, 
+						dev->mii.phy_id, 21, 0x1001);
+
+				PhyReg = ax8817x_swmii_mdio_read_le (dev->net, 
+						dev->mii.phy_id, 21);
+				if ((PhyReg & 0xf00f) == 0x1001)
+					break;
+			}
+			if (i < 0)
+				return -EIO;
+		}
+
+		if (ax178dataptr->LedMode == 4) {
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 28, 0x7417);
+		} else if (ax178dataptr->LedMode == 9) {
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 28, 0x7a10);
+		} else if (ax178dataptr->LedMode == 10) {
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 28, 0x7a13);
+		}
+
+		for (i = 0; i < (sizeof(AGERE_FAMILY_HWINIT) /
+				 sizeof(AGERE_FAMILY_HWINIT[0])); i++) {
+			ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, AGERE_FAMILY_HWINIT[i].offset,
+				AGERE_FAMILY_HWINIT[i].value);
+		}
+	} else if (ax178dataptr->PhyMode == PHY_MODE_RTL8211CL) {
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 0x1f, 0x0005);
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 0x0c, 0);
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 0x01, 
+				(ax8817x_swmii_mdio_read_le (dev->net, 
+					dev->mii.phy_id, 0x01) | 0x0080));
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 0x1f, 0);
+
+		if (ax178dataptr->LedMode == 12) {
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 0x1f, 0x0002);
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 0x1a, 0x00cb);
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 0x1f, 0);
+		}
+	} else if (ax178dataptr->PhyMode == PHY_MODE_VSC8601) {
+		Vitess_8601_Init (dev, 1);
+	}
+
+	// read phy register 0
+	PhyCtrl = ax8817x_swmii_mdio_read_le (dev->net, 
+				dev->mii.phy_id, GMII_PHY_CONTROL);
+	TempShort = PhyCtrl;
+	PhyCtrl &= ~(GMII_CONTROL_POWER_DOWN | GMII_CONTROL_ISOLATE);
+	if (PhyCtrl != TempShort) {
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, GMII_PHY_CONTROL, PhyCtrl);
+	}
+
+	// led
+	if (ax178dataptr->PhyMode == PHY_MODE_MARVELL) {
+		if (ax178dataptr->LedMode == 1)	{
+
+			PhyReg = (ax8817x_swmii_mdio_read_le (dev->net, 
+				dev->mii.phy_id, 24) & 0xf8ff) | (1 + 0x100);
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 24, PhyReg);
+			PhyReg = ax8817x_swmii_mdio_read_le (dev->net, 
+					dev->mii.phy_id, 25) & 0xfc0f;
+
+		} else if (ax178dataptr->LedMode == 2) {
+
+			PhyReg = (ax8817x_swmii_mdio_read_le (dev->net, 
+					dev->mii.phy_id, 24) & 0xf886) | 
+					(1 + 0x10 + 0x300);
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 24, PhyReg);
+
+		} else if (ax178dataptr->LedMode == 5) {
+
+			PhyReg = (ax8817x_swmii_mdio_read_le (dev->net, 
+					dev->mii.phy_id, 24) & 0xf8be) | 
+					(1 + 0x40 + 0x300);
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 24, PhyReg);
+
+		} else if (ax178dataptr->LedMode == 7) {
+
+			PhyReg = (ax8817x_swmii_mdio_read_le (dev->net, 
+						dev->mii.phy_id, 24) & 0xf8ff) |
+						(1 + 0x100);
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 24, PhyReg);
+
+		} else if (ax178dataptr->LedMode == 8) {
+
+			PhyReg = (ax8817x_swmii_mdio_read_le (dev->net, 
+					dev->mii.phy_id, 24) & 0xf8be) | 
+					(1 + 0x40 + 0x100);
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 24, PhyReg);
+
+		} else if (ax178dataptr->LedMode == 11) {
+
+			PhyReg = ax8817x_swmii_mdio_read_le (dev->net, 
+					dev->mii.phy_id, 24) & 0x4106;
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 24, PhyReg);
+
+		} else if (ax178dataptr->LedMode == 0x10) {
+			//MARVEL 88e1510 use default led setting
+		}
+
+	} else if ((ax178dataptr->PhyMode == PHY_MODE_CICADA_V1) ||
+		   (ax178dataptr->PhyMode == PHY_MODE_CICADA_V2) ||
+		   (ax178dataptr->PhyMode == PHY_MODE_CICADA_V2_ASIX)) {
+
+		if (ax178dataptr->LedMode == 3) {
+
+			PhyReg = (ax8817x_swmii_mdio_read_le (dev->net, 
+					dev->mii.phy_id, 27) & 0xFCFF) | 0x0100;
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 27, PhyReg);
+		}
+
+	}
+
+	if (ax178dataptr->PhyMode == PHY_MODE_MARVELL)
+	{
+		if (ax178dataptr->LedMode == 1)
+			PhyReg |= 0x3f0;
+	}
+
+	PhyAnar = 1 | (GMII_ANAR_PAUSE | GMII_ANAR_100TXFD | GMII_ANAR_100TX |
+		       GMII_ANAR_10TFD | GMII_ANAR_10T | GMII_ANAR_ASYM_PAUSE);
+
+	PhyAuxCtrl = GMII_1000_AUX_CTRL_FD_CAPABLE;
+
+	ax8817x_swmii_mdio_write_le (dev->net, 
+			dev->mii.phy_id, GMII_PHY_ANAR, PhyAnar);
+	ax8817x_swmii_mdio_write_le (dev->net, 
+			dev->mii.phy_id, GMII_PHY_1000BT_CONTROL, PhyAuxCtrl);
+
+	if (ax178dataptr->PhyMode == PHY_MODE_VSC8601)
+	{
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 31, 0x52B5);
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 16, 0xA7F8);
+
+		TempShort = ax8817x_swmii_mdio_read_le (dev->net, 
+					dev->mii.phy_id, 17) & (~0x0018);
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 17, TempShort);
+
+		TempShort = ax8817x_swmii_mdio_read_le (dev->net, 
+					dev->mii.phy_id, 18);
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 18, TempShort);
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 16, 0x87F8);
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, 31, 0);
+	}
+
+	if (ax178dataptr->PhyMode == PHY_MODE_ATTANSIC_V0) {
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, GMII_PHY_CONTROL, 0x9000);
+
+	} else {
+		PhyCtrl &= ~GMII_CONTROL_LOOPBACK;
+		PhyCtrl |= (GMII_CONTROL_ENABLE_AUTO | GMII_CONTROL_START_AUTO);
+
+		ax8817x_swmii_mdio_write_le (dev->net, 
+				dev->mii.phy_id, GMII_PHY_CONTROL, PhyCtrl);
+	}
+
+	if (ax178dataptr->PhyMode == PHY_MODE_MARVELL) {
+		if (ax178dataptr->LedMode == 1)
+			ax8817x_swmii_mdio_write_le (dev->net, 
+					dev->mii.phy_id, 25, PhyReg);
+	}
 
-	netdev_dbg(dev->net, "ax88178_link_reset() speed: %u duplex: %d setting mode to 0x%04x\n",
-		   speed, ecmd.duplex, mode);
+SkipPhySetting:
 
-	asix_write_medium_mode(dev, mode);
+	ax8817x_write_cmd (dev, AX_CMD_WRITE_MEDIUM_MODE, 
+			ax178dataptr->MediaLink, 0, 0, NULL);
 
-	if (data->phymode == PHY_MODE_MARVELL && data->ledmode)
-		marvell_led_status(dev, speed);
+	ax8817x_write_cmd (dev, AX_CMD_WRITE_IPG0, 
+			AX88772_IPG0_DEFAULT | (AX88772_IPG1_DEFAULT << 8), 
+			AX88772_IPG2_DEFAULT, 0, NULL);
+
+	msleep (1);
+
+	ax8817x_write_cmd (dev, AX_CMD_SET_HW_MII, 0, 0, 0, NULL);
 
 	return 0;
 }
 
-static void ax88178_set_mfb(struct usbnet *dev)
+static int ax88178_bind(struct usbnet *dev, struct usb_interface *intf)
 {
-	u16 mfb = AX_RX_CTL_MFB_16384;
-	u16 rxctl;
-	u16 medium;
-	int old_rx_urb_size = dev->rx_urb_size;
+	int ret;
+	void *buf;
+	struct ax8817x_data *data = (struct ax8817x_data *)&dev->data;
+	struct ax88178_data *ax178dataptr = NULL;
 
-	if (dev->hard_mtu < 2048) {
-		dev->rx_urb_size = 2048;
-		mfb = AX_RX_CTL_MFB_2048;
-	} else if (dev->hard_mtu < 4096) {
-		dev->rx_urb_size = 4096;
-		mfb = AX_RX_CTL_MFB_4096;
-	} else if (dev->hard_mtu < 8192) {
-		dev->rx_urb_size = 8192;
-		mfb = AX_RX_CTL_MFB_8192;
-	} else if (dev->hard_mtu < 16384) {
+	axusbnet_get_endpoints(dev,intf);
+
+	buf = kmalloc(6, GFP_KERNEL);
+	if(!buf) {
+		deverr(dev, "Cannot allocate memory for buffer");
+		return -ENOMEM;
+	}
+
+	/* allocate 178 data */
+	ax178dataptr = kmalloc (sizeof (*ax178dataptr), GFP_KERNEL);
+	if (!ax178dataptr) {
+		deverr(dev, "Cannot allocate memory for AX88178 data");
+		ret = -ENOMEM;
+		goto error_out;
+	}
+	memset (ax178dataptr, 0, sizeof (struct ax88178_data));
+	dev->priv = ax178dataptr;
+	/* end of allocate 178 data */
+
+	/* Get the EEPROM data*/
+	if ((ret = ax8817x_read_cmd (dev, AX_CMD_READ_EEPROM, 0x0017, 0, 2,
+				(void *)(&ax178dataptr->EepromData))) < 0) {
+		deverr(dev, "read SROM address 17h failed: %d", ret);
+		goto error_out;
+	}
+	le16_to_cpus (&ax178dataptr->EepromData);
+	/* End of get EEPROM data */
+
+	if (ax178dataptr->EepromData == 0xffff) {
+		ax178dataptr->PhyMode  = PHY_MODE_MARVELL;
+		ax178dataptr->LedMode  = 0;
+		ax178dataptr->UseGpio0 = 1; //True
+	} else {
+		ax178dataptr->PhyMode = (u8)(ax178dataptr->EepromData & 
+						EEPROMMASK);
+		ax178dataptr->LedMode = (u8)(ax178dataptr->EepromData >> 8);
+		if (ax178dataptr->LedMode == 6)	// for buffalo new (use gpio2)
+			ax178dataptr->LedMode = 1;
+		else if (ax178dataptr->LedMode == 1)
+			ax178dataptr->BuffaloOld = 1;
+
+
+		if(ax178dataptr->EepromData & 0x80) {
+			ax178dataptr->UseGpio0=0; //MARVEL se and other
+		} else {
+			ax178dataptr->UseGpio0=1; //cameo
+		}
+	}
+
+	if (ax178dataptr->UseGpio0) {
+
+		if (ax178dataptr->PhyMode == PHY_MODE_MARVELL) {
+
+			if ((ret = ax8817x_write_cmd (dev, AX_CMD_WRITE_GPIOS,
+					AXGPIOS_GPO0EN | AXGPIOS_RSE,
+					0, 0, NULL)) < 0) {
+				deverr(dev, "write GPIO failed: %d", ret);
+				goto error_out;
+			}
+
+			msleep (25);
+
+			if ((ret = ax8817x_write_cmd (dev, AX_CMD_WRITE_GPIOS,
+					(AXGPIOS_GPO2 | AXGPIOS_GPO2EN |
+					 AXGPIOS_GPO0EN), 0, 0, NULL)) < 0) {
+				deverr(dev, "write GPIO failed: %d", ret);
+				goto error_out;
+			}
+
+			msleep (15);
+
+			if ((ret = ax8817x_write_cmd (dev, AX_CMD_WRITE_GPIOS,
+					AXGPIOS_GPO2EN | AXGPIOS_GPO0EN,
+					0, 0, NULL)) < 0) {
+				deverr(dev, "write GPIO failed: %d", ret);
+				goto error_out;
+			}
+
+			msleep (245);
+
+			if ((ret = ax8817x_write_cmd (dev, AX_CMD_WRITE_GPIOS,
+					(AXGPIOS_GPO2 | AXGPIOS_GPO2EN |
+					 AXGPIOS_GPO0EN), 0, 0, NULL)) < 0) {
+				deverr(dev, "write GPIO failed: %d", ret);
+				goto error_out;
+			}
+
+		} else { // vitesse
+
+			if ((ret = ax8817x_write_cmd (dev, AX_CMD_WRITE_GPIOS,
+					(AXGPIOS_RSE | AXGPIOS_GPO0EN |
+					 AXGPIOS_GPO0), 0, 0, NULL)) < 0) {
+				deverr(dev, "write GPIO failed: %d", ret);
+				goto error_out;
+			}
+
+			msleep (25);
+
+			if ((ret = ax8817x_write_cmd (dev, AX_CMD_WRITE_GPIOS,
+					(AXGPIOS_GPO0EN | AXGPIOS_GPO0 |
+					 AXGPIOS_GPO2EN | AXGPIOS_GPO2),
+					0, 0, NULL)) < 0) {
+				deverr(dev, "write GPIO failed: %d", ret);
+				goto error_out;
+			}
+
+			msleep (25);
+
+			if ((ret = ax8817x_write_cmd (dev, AX_CMD_WRITE_GPIOS,
+					(AXGPIOS_GPO0EN | AXGPIOS_GPO0 |
+					 AXGPIOS_GPO2EN), 0, 0, NULL)) < 0) {
+				deverr(dev, "write GPIO failed: %d", ret);
+				goto error_out;
+			}
+
+			msleep (245);
+
+			if ((ret = ax8817x_write_cmd (dev, AX_CMD_WRITE_GPIOS,
+					(AXGPIOS_GPO0EN | AXGPIOS_GPO0 |
+					 AXGPIOS_GPO2EN | AXGPIOS_GPO2),
+					0, 0, NULL)) < 0) {
+				deverr(dev, "write GPIO failed: %d", ret);
+				goto error_out;
+			}
+		}
+	} else {	// use gpio1
+
+		if ((ret = ax8817x_write_cmd (dev, AX_CMD_WRITE_GPIOS,
+				(AXGPIOS_GPO1 |AXGPIOS_GPO1EN | AXGPIOS_RSE),
+				0, 0, NULL)) < 0) {
+			deverr(dev, "write GPIO failed: %d", ret);
+			goto error_out;
+		}
+
+		if (ax178dataptr->BuffaloOld) {
+
+			msleep (350);
+
+			if ((ret = ax8817x_write_cmd (dev, AX_CMD_WRITE_GPIOS,
+					AXGPIOS_GPO1EN, 0, 0, NULL)) < 0) {
+				deverr(dev, "write GPIO failed: %d", ret);
+				goto error_out;
+			}
+
+			msleep (350);
+
+			if ((ret = ax8817x_write_cmd (dev, AX_CMD_WRITE_GPIOS,
+					AXGPIOS_GPO1EN | AXGPIOS_GPO1,
+					0, 0, NULL)) < 0) {
+				deverr(dev, "write GPIO failed: %d", ret);
+				goto error_out;
+			}
+		}
+		else
+		{
+			msleep (25);
+
+			if ((ret = ax8817x_write_cmd (dev, AX_CMD_WRITE_GPIOS,
+					(AXGPIOS_GPO1EN | AXGPIOS_GPO1 |
+					 AXGPIOS_GPO2EN | AXGPIOS_GPO2),
+					0, 0, NULL)) < 0) {
+				deverr(dev, "write GPIO failed: %d", ret);
+				goto error_out;
+			}
+
+			msleep (25);
+
+			if ((ret = ax8817x_write_cmd (dev, AX_CMD_WRITE_GPIOS,
+					(AXGPIOS_GPO1EN | AXGPIOS_GPO1 |
+					 AXGPIOS_GPO2EN), 0, 0, NULL)) < 0) {
+				deverr(dev, "write GPIO failed: %d", ret);
+				goto error_out;
+			}
+
+			msleep (245);
+
+			if ((ret = ax8817x_write_cmd (dev, AX_CMD_WRITE_GPIOS,
+					(AXGPIOS_GPO1EN | AXGPIOS_GPO1 |
+					 AXGPIOS_GPO2EN | AXGPIOS_GPO2),
+					0, 0, NULL)) < 0) {
+				deverr(dev, "write GPIO failed: %d", ret);
+				goto error_out;
+			}
+		}
+	}
+
+	if ((ret = ax8817x_write_cmd(dev,
+			AX_CMD_SW_PHY_SELECT, 0, 0, 0, NULL)) < 0) {
+		deverr(dev, "Select PHY failed: %d", ret);
+		goto error_out;
+	}
+
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+			AX_SWRESET_IPPD | AX_SWRESET_PRL, 0, 0, NULL)) < 0) {
+		deverr(dev, "Issue sw reset failed: %d", ret);
+		goto error_out;
+	}
+
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_RX_CTL,
+			0, 0, 0, NULL)) < 0) {
+		deverr(dev, "Issue rx ctrl failed: %d", ret);
+		goto error_out;
+	}
+
+	/* Get the MAC address */
+	memset(buf, 0, ETH_ALEN);
+	if ((ret = ax8817x_read_cmd (dev, AX88772_CMD_READ_NODE_ID,
+				     0, 0, ETH_ALEN, buf)) < 0) {
+		deverr(dev, "read AX_CMD_READ_NODE_ID failed: %d", ret);
+		goto error_out;
+	}
+	memcpy(dev->net->dev_addr, buf, ETH_ALEN);
+	/* End of get MAC address */
+
+	if ((ret = ax88178_phy_init (dev, ax178dataptr)) < 0)
+		goto error_out;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,30)
+	dev->net->do_ioctl = ax8817x_ioctl;
+	dev->net->set_multicast_list = ax88178_set_multicast;
+	dev->net->set_mac_address = ax88178_set_mac_addr;
+#else
+	dev->net->netdev_ops = &ax88178_netdev_ops;
+#endif
+	dev->net->ethtool_ops = &ax8817x_ethtool_ops;
+
+	/* Register suspend and resume functions */
+	data->suspend = ax88772_suspend;
+	data->resume = ax88772_resume;
+
+	if (dev->driver_info->flags & FLAG_FRAMING_AX) {
 		dev->rx_urb_size = 16384;
-		mfb = AX_RX_CTL_MFB_16384;
 	}
 
-	rxctl = asix_read_rx_ctl(dev);
-	asix_write_rx_ctl(dev, (rxctl & ~AX_RX_CTL_MFB_16384) | mfb);
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_RX_CTL,
+			(AX_RX_CTL_MFB | AX_RX_CTL_START | AX_RX_CTL_AB),
+			0, 0, NULL)) < 0) {
+		deverr(dev, "write RX ctrl reg failed: %d", ret);
+		goto error_out;
+	}
 
-	medium = asix_read_medium_status(dev);
-	if (dev->net->mtu > 1500)
-		medium |= AX_MEDIUM_JFE;
-	else
-		medium &= ~AX_MEDIUM_JFE;
-	asix_write_medium_mode(dev, medium);
+	kfree (buf);
+	printk (version);
+	return ret;
 
-	if (dev->rx_urb_size > old_rx_urb_size)
-		usbnet_unlink_rx_urbs(dev);
+error_out:
+	if (ax178dataptr)
+		kfree (ax178dataptr);
+	kfree (buf);
+	return ret;
 }
 
-static int ax88178_change_mtu(struct net_device *net, int new_mtu)
+static void ax88178_unbind(struct usbnet *dev, struct usb_interface *intf)
 {
-	struct usbnet *dev = netdev_priv(net);
-	int ll_mtu = new_mtu + net->hard_header_len + 4;
+	struct ax88178_data *ax178dataptr = (struct ax88178_data *)dev->priv;
 
-	netdev_dbg(dev->net, "ax88178_change_mtu() new_mtu=%d\n", new_mtu);
+	if (ax178dataptr) {
 
-	if (new_mtu <= 0 || ll_mtu > 16384)
-		return -EINVAL;
+		/* stop MAC operation */
+		ax8817x_write_cmd(dev, AX_CMD_WRITE_RX_CTL,
+					AX_RX_CTL_STOP, 0, 0, NULL);
+
+		kfree (ax178dataptr);
+	}
+}
+
+static int ax88772_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
+{
+	u8  *head;
+	u32  header;
+	char *packet;
+	struct sk_buff *ax_skb;
+	u16 size;
+
+	head = (u8 *) skb->data;
+	memcpy(&header, head, sizeof(header));
+	le32_to_cpus(&header);
+	packet = head + sizeof(header);
 
-	if ((ll_mtu % dev->maxpacket) == 0)
-		return -EDOM;
+	skb_pull(skb, 4);
 
-	net->mtu = new_mtu;
-	dev->hard_mtu = net->mtu + net->hard_header_len;
-	ax88178_set_mfb(dev);
+	while (skb->len > 0) {
+		if ((short)(header & 0x0000ffff) !=
+		    ~((short)((header & 0xffff0000) >> 16))) {
+			deverr(dev, "header length data is error 0x%08x, %d\n",
+				header, skb->len);
+		}
+		/* get the packet length */
+		size = (u16) (header & 0x0000ffff);
 
-	return 0;
+		if ((skb->len) - ((size + 1) & 0xfffe) == 0) {
+
+			/* Make sure ip header is aligned on 32-bit boundary */
+			if (!((unsigned long)skb->data & 0x02)) {
+				memmove (skb->data - 2, skb->data, size);
+				skb->data -= 2;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22)
+				skb->tail = skb->data + size;
+#else
+				skb_set_tail_pointer (skb, size);
+#endif
+			}
+			skb->truesize = size + sizeof(struct sk_buff);
+			return 2;
+		}
+
+		if (size > ETH_FRAME_LEN) {
+			deverr(dev, "invalid rx length %d", size);
+			return 0;
+		}
+		ax_skb = skb_clone(skb, GFP_ATOMIC);
+		if (ax_skb) {
+
+			/* Make sure ip header is aligned on 32-bit boundary */
+			if (!((unsigned long)packet & 0x02)) {
+				memmove (packet - 2, packet, size);
+				packet -= 2;
+			}
+			ax_skb->data = packet;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22)
+			ax_skb->tail = packet + size;
+#else
+			skb_set_tail_pointer (ax_skb, size);
+#endif
+			ax_skb->truesize = size + sizeof(struct sk_buff);
+			axusbnet_skb_return(dev, ax_skb);
+
+		} else {
+			return 0;
+		}
+
+		skb_pull(skb, (size + 1) & 0xfffe);
+
+		if (skb->len == 0)
+			break;
+
+		head = (u8 *) skb->data;
+		memcpy(&header, head, sizeof(header));
+		le32_to_cpus(&header);
+		packet = head + sizeof(header);
+		skb_pull(skb, 4);
+	}
+
+	if (skb->len < 0) {
+		deverr(dev, "invalid rx length %d", skb->len);
+		return 0;
+	}
+	return 1;
 }
 
-static const struct net_device_ops ax88178_netdev_ops = {
-	.ndo_open		= usbnet_open,
-	.ndo_stop		= usbnet_stop,
-	.ndo_start_xmit		= usbnet_start_xmit,
-	.ndo_tx_timeout		= usbnet_tx_timeout,
-	.ndo_set_mac_address 	= asix_set_mac_address,
-	.ndo_validate_addr	= eth_validate_addr,
-	.ndo_set_multicast_list = asix_set_multicast,
-	.ndo_do_ioctl 		= asix_ioctl,
-	.ndo_change_mtu 	= ax88178_change_mtu,
-};
+static struct sk_buff *ax88772_tx_fixup(struct usbnet *dev, struct sk_buff *skb, gfp_t flags)
+{
+	int padlen = ((skb->len + 4) % 512) ? 0 : 4;
+	u32 packet_len;
+	u32 padbytes = 0xffff0000;
 
-static int ax88178_bind(struct usbnet *dev, struct usb_interface *intf)
+#if (!AX_FORCE_BUFF_ALIGN)
+	int headroom = skb_headroom(skb);
+	int tailroom = skb_tailroom(skb);
+
+	if ((!skb_cloned(skb))
+	    && ((headroom + tailroom) >= (4 + padlen))) {
+		if ((headroom < 4) || (tailroom < padlen)) {
+			skb->data = memmove(skb->head + 4, skb->data, skb->len);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22)
+		skb->tail = skb->data + skb->len;
+#else
+		skb_set_tail_pointer (skb, skb->len);
+#endif
+		}
+	} else
+#endif
+	{
+		struct sk_buff *skb2;
+		skb2 = skb_copy_expand(skb, 4, padlen, flags);
+		dev_kfree_skb_any(skb);
+		skb = skb2;
+		if (!skb)
+			return NULL;
+	}
+
+	skb_push(skb, 4);
+	packet_len = (((skb->len - 4) ^ 0x0000ffff) << 16) + (skb->len - 4);
+	cpu_to_le32s(&packet_len);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22)
+	memcpy(skb->data, &packet_len, sizeof(packet_len));
+#else
+	skb_copy_to_linear_data(skb, &packet_len, sizeof(packet_len));
+#endif
+
+	if ((skb->len % 512) == 0) {
+		cpu_to_le32s(&padbytes);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22)
+		memcpy(skb->tail, &padbytes, sizeof(padbytes));
+#else
+		memcpy(skb_tail_pointer(skb), &padbytes, sizeof(padbytes));
+#endif
+		skb_put(skb, sizeof(padbytes));
+	}
+	return skb;
+}
+
+static void
+ax88772b_rx_checksum (struct sk_buff *skb, struct ax88772b_rx_header *rx_hdr)
 {
-	struct asix_data *data = (struct asix_data *)&dev->data;
-	int ret;
-	u8 buf[ETH_ALEN];
-	__le16 eeprom;
-	u8 status;
-	int gpio0 = 0;
-	u32 phyid;
+	skb->ip_summed = CHECKSUM_NONE;
+
+	/* checksum error bit is set */
+	if (rx_hdr->l3_csum_err || rx_hdr->l4_csum_err) {
+		return;
+	}
+
+	/* It must be a TCP or UDP packet with a valid checksum */
+	if ((rx_hdr->l4_type == AX_RXHDR_L4_TYPE_TCP) ||
+	    (rx_hdr->l4_type == AX_RXHDR_L4_TYPE_UDP)) {
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+	}
+}
 
-	usbnet_get_endpoints(dev,intf);
+static int ax88772b_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
+{
+	struct ax88772b_rx_header rx_hdr;
+	struct sk_buff *ax_skb;
+	struct ax88772b_data *ax772b_data = (struct ax88772b_data *)dev->priv;
 
-	asix_read_cmd(dev, AX_CMD_READ_GPIOS, 0, 0, 1, &status);
-	dbg("GPIO Status: 0x%04x", status);
+	while (skb->len > 0) {
 
-	asix_write_cmd(dev, AX_CMD_WRITE_ENABLE, 0, 0, 0, NULL);
-	asix_read_cmd(dev, AX_CMD_READ_EEPROM, 0x0017, 0, 2, &eeprom);
-	asix_write_cmd(dev, AX_CMD_WRITE_DISABLE, 0, 0, 0, NULL);
+		le16_to_cpus((u16 *)skb->data);
+		le16_to_cpus(((u16 *)skb->data) + 1);
 
-	dbg("EEPROM index 0x17 is 0x%04x", eeprom);
+		memcpy (&rx_hdr, skb->data, sizeof (struct ax88772b_rx_header));
 
-	if (eeprom == cpu_to_le16(0xffff)) {
-		data->phymode = PHY_MODE_MARVELL;
-		data->ledmode = 0;
-		gpio0 = 1;
-	} else {
-		data->phymode = le16_to_cpu(eeprom) & 7;
-		data->ledmode = le16_to_cpu(eeprom) >> 8;
-		gpio0 = (le16_to_cpu(eeprom) & 0x80) ? 0 : 1;
+		if ((short)rx_hdr.len != (~((short)rx_hdr.len_bar) & 0x7FF)) {
+			return 0;
+		}
+
+		if (rx_hdr.len > (ETH_FRAME_LEN + 4)) {
+			deverr(dev, "invalid rx length %d", rx_hdr.len);
+			return 0;
+		}
+
+		if (skb->len - ((rx_hdr.len + 
+				 sizeof (struct ax88772b_rx_header) + 3) & 
+				 0xfffc) == 0) {
+			skb_pull(skb, sizeof (struct ax88772b_rx_header));
+			skb->len = rx_hdr.len;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22)
+			skb->tail = skb->data + rx_hdr.len;
+#else
+			skb_set_tail_pointer(skb, rx_hdr.len);
+#endif
+			skb->truesize = rx_hdr.len + sizeof(struct sk_buff);
+
+			if (ax772b_data->checksum & AX_RX_CHECKSUM)
+				ax88772b_rx_checksum (skb, &rx_hdr);
+
+			return 2;
+		}
+
+		ax_skb = skb_clone(skb, GFP_ATOMIC);
+		if (ax_skb) {
+			ax_skb->len = rx_hdr.len;
+			ax_skb->data = skb->data + 
+				       sizeof (struct ax88772b_rx_header);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22)
+			ax_skb->tail = ax_skb->data + rx_hdr.len;
+#else
+			skb_set_tail_pointer(ax_skb, rx_hdr.len);
+#endif
+			ax_skb->truesize = rx_hdr.len + sizeof(struct sk_buff);
+
+			if (ax772b_data->checksum & AX_RX_CHECKSUM) {
+				ax88772b_rx_checksum (ax_skb, &rx_hdr);
+			}
+
+			axusbnet_skb_return(dev, ax_skb);
+
+		} else {
+			return 0;
+		}
+
+		skb_pull(skb, ((rx_hdr.len + 
+				sizeof (struct ax88772b_rx_header) + 3)
+				& 0xfffc));
 	}
-	dbg("GPIO0: %d, PhyMode: %d", gpio0, data->phymode);
 
-	asix_write_gpio(dev, AX_GPIO_RSE | AX_GPIO_GPO_1 | AX_GPIO_GPO1EN, 40);
-	if ((le16_to_cpu(eeprom) >> 8) != 1) {
-		asix_write_gpio(dev, 0x003c, 30);
-		asix_write_gpio(dev, 0x001c, 300);
-		asix_write_gpio(dev, 0x003c, 30);
-	} else {
-		dbg("gpio phymode == 1 path");
-		asix_write_gpio(dev, AX_GPIO_GPO1EN, 30);
-		asix_write_gpio(dev, AX_GPIO_GPO1EN | AX_GPIO_GPO_1, 30);
+	if (skb->len < 0) {
+		deverr(dev, "invalid rx length %d", skb->len);
+		return 0;
 	}
+	return 1;
+}
 
-	asix_sw_reset(dev, 0);
-	msleep(150);
+static struct sk_buff *
+ax88772b_tx_fixup(struct usbnet *dev, struct sk_buff *skb, gfp_t flags)
+{
+	int padlen = ((skb->len + 4) % 512) ? 0 : 4;
+	u32 packet_len;
+	u32 padbytes = 0xffff0000;
 
-	asix_sw_reset(dev, AX_SWRESET_PRL | AX_SWRESET_IPPD);
-	msleep(150);
+#if (!AX_FORCE_BUFF_ALIGN)
+	int headroom = skb_headroom(skb);
+	int tailroom = skb_tailroom(skb);
+
+	if ((!skb_cloned(skb))
+	    && ((headroom + tailroom) >= (4 + padlen))) {
+		if ((headroom < 4) || (tailroom < padlen)) {
+			skb->data = memmove(skb->head + 4, skb->data, skb->len);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22)
+			skb->tail = skb->data + skb->len;
+#else
+			skb_set_tail_pointer(skb, skb->len);
+#endif
+		}
+	} else
+#endif
+	{
+		struct sk_buff *skb2;
+		skb2 = skb_copy_expand(skb, 4, padlen, flags);
+		dev_kfree_skb_any(skb);
+		skb = skb2;
+		if (!skb)
+			return NULL;
+	}
 
-	asix_write_rx_ctl(dev, 0);
+	skb_push(skb, 4);
+	packet_len = (((skb->len - 4) ^ 0x0000ffff) << 16) + (skb->len - 4);
 
-	/* Get the MAC address */
-	if ((ret = asix_read_cmd(dev, AX_CMD_READ_NODE_ID,
-				0, 0, ETH_ALEN, buf)) < 0) {
-		dbg("Failed to read MAC address: %d", ret);
-		goto out;
+	cpu_to_le32s (&packet_len);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22)
+	memcpy(skb->data, &packet_len, sizeof(packet_len));
+#else
+	skb_copy_to_linear_data(skb, &packet_len, sizeof(packet_len));
+#endif
+
+	if ((skb->len % 512) == 0) {
+		cpu_to_le32s (&padbytes);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22)
+		memcpy(skb->tail, &padbytes, sizeof(padbytes));
+#else
+		memcpy(skb_tail_pointer(skb), &padbytes, sizeof(padbytes));
+#endif
+		skb_put(skb, sizeof(padbytes));
 	}
-	memcpy(dev->net->dev_addr, buf, ETH_ALEN);
 
-	/* Initialize MII structure */
-	dev->mii.dev = dev->net;
-	dev->mii.mdio_read = asix_mdio_read;
-	dev->mii.mdio_write = asix_mdio_write;
-	dev->mii.phy_id_mask = 0x1f;
-	dev->mii.reg_num_mask = 0xff;
-	dev->mii.supports_gmii = 1;
-	dev->mii.phy_id = asix_get_phy_addr(dev);
+	return skb;
+}
 
-	dev->net->netdev_ops = &ax88178_netdev_ops;
-	dev->net->ethtool_ops = &ax88178_ethtool_ops;
+static const u8 ChkCntSel [6][3] = 
+{
+	{12, 23, 31},
+	{12, 31, 23},
+	{23, 31, 12},
+	{23, 12, 31},
+	{31, 12, 23},
+	{31, 23, 12}
+};
 
-	phyid = asix_get_phyid(dev);
-	dbg("PHYID=0x%08x", phyid);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void ax88772_link_reset (void *data)
+{
+	struct usbnet *dev = (struct usbnet *)data;
+	struct ax88772_data *ax772_data = (struct ax88772_data *)dev->priv;
+#else
+static void ax88772_link_reset (struct work_struct *work)
+{
+	struct ax88772_data *ax772_data = container_of (work, 
+					struct ax88772_data, check_link);
+	struct usbnet *dev = ax772_data->dev;
+#endif
+	
+	if (ax772_data->Event == AX_SET_RX_CFG) {
+		u16 bmcr;
+		u16 mode;
+		
+		ax772_data->Event = AX_NOP;
+	
+		mode = AX88772_MEDIUM_DEFAULT;
+
+		bmcr = ax8817x_mdio_read_le(dev->net, 
+				dev->mii.phy_id, MII_BMCR);
+		if (!(bmcr & BMCR_FULLDPLX))
+			mode &= ~AX88772_MEDIUM_FULL_DUPLEX;
+		if (!(bmcr & BMCR_SPEED100))
+			mode &= ~AX88772_MEDIUM_100MB;
+
+		ax8817x_write_cmd(dev, AX_CMD_WRITE_MEDIUM_MODE, 
+			mode, 0, 0, NULL);
+		return;
+	}
+	
+	switch (ax772_data->Event) {
+	  case WAIT_AUTONEG_COMPLETE:
+		if (jiffies > (ax772_data->autoneg_start + 5 * HZ)) {
+			ax772_data->Event = PHY_POWER_DOWN;
+			ax772_data->TickToExpire = 23;
+		}
+		break;
+	  case PHY_POWER_DOWN:
+		if (ax772_data->TickToExpire == 23) {
+			/* Set Phy Power Down */
+			ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+					  AX_SWRESET_IPPD,
+					  0, 0, NULL);
+			--ax772_data->TickToExpire;
+		} else if (--ax772_data->TickToExpire == 0) {
+			/* Set Phy Power Up */
+			ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+				AX_SWRESET_IPRL, 0, 0, NULL);
+			ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+				AX_SWRESET_IPPD | AX_SWRESET_IPRL, 0, 0, NULL);
+			msleep(10);
+			ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+				AX_SWRESET_IPRL, 0, 0, NULL);
+			msleep(60);
+			ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+				AX_SWRESET_CLEAR, 0, 0, NULL);
+			ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+				AX_SWRESET_IPRL, 0, 0, NULL);
+
+			ax8817x_mdio_write_le(dev->net, dev->mii.phy_id, 
+				MII_ADVERTISE,
+				ADVERTISE_ALL | ADVERTISE_CSMA | 
+				ADVERTISE_PAUSE_CAP);
+			mii_nway_restart(&dev->mii);
+			
+			ax772_data->Event = PHY_POWER_UP;
+			ax772_data->TickToExpire = 47;
+		}
+		break;
+	  case PHY_POWER_UP:
+		if (--ax772_data->TickToExpire == 0) {
+			ax772_data->Event = PHY_POWER_DOWN;
+			ax772_data->TickToExpire = 23;
+		}
+		break;
+	  default:
+		break;
+	}
+	return;
+}
 
-	if (data->phymode == PHY_MODE_MARVELL) {
-		marvell_phy_init(dev);
-		msleep(60);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void ax88772a_link_reset (void *data)
+{
+	struct usbnet *dev = (struct usbnet *)data;
+	struct ax88772a_data *ax772a_data = (struct ax88772a_data *)dev->priv;
+#else
+static void ax88772a_link_reset (struct work_struct *work)
+{
+	struct ax88772a_data *ax772a_data = container_of (work, 
+					struct ax88772a_data, check_link);
+	struct usbnet *dev = ax772a_data->dev;
+#endif
+	int PowSave = (ax772a_data->EepromData >> 14);
+	u16 phy_reg;
+	
+	if (ax772a_data->Event == AX_SET_RX_CFG) {
+		u16 bmcr;
+		u16 mode;
+
+		ax772a_data->Event = AX_NOP;
+	
+		mode = AX88772_MEDIUM_DEFAULT;
+
+		bmcr = ax8817x_mdio_read_le(dev->net, 
+				dev->mii.phy_id, MII_BMCR);
+		if (!(bmcr & BMCR_FULLDPLX))
+			mode &= ~AX88772_MEDIUM_FULL_DUPLEX;
+		if (!(bmcr & BMCR_SPEED100))
+			mode &= ~AX88772_MEDIUM_100MB;
+
+		ax8817x_write_cmd(dev, AX_CMD_WRITE_MEDIUM_MODE, 
+			mode, 0, 0, NULL);
+		return;
 	}
 
-	asix_mdio_write(dev->net, dev->mii.phy_id, MII_BMCR,
-			BMCR_RESET | BMCR_ANENABLE);
-	asix_mdio_write(dev->net, dev->mii.phy_id, MII_ADVERTISE,
+	switch (ax772a_data->Event) {
+	case WAIT_AUTONEG_COMPLETE:
+		if (jiffies > (ax772a_data->autoneg_start + 5 * HZ)) {
+			ax772a_data->Event = CHK_CABLE_EXIST;
+			ax772a_data->TickToExpire = 14;
+		}
+		break;
+	case CHK_CABLE_EXIST:
+		phy_reg = ax8817x_mdio_read_le(dev->net, dev->mii.phy_id, 0x12);
+		if ((phy_reg != 0x8012) && (phy_reg != 0x8013)) {
+			ax8817x_mdio_write_le(dev->net,
+				dev->mii.phy_id, 0x16, 0x4040);
+			mii_nway_restart(&dev->mii);
+			ax772a_data->Event = CHK_CABLE_STATUS;
+			ax772a_data->TickToExpire = 31;
+		} else if (--ax772a_data->TickToExpire == 0) {
+			mii_nway_restart(&dev->mii);
+			ax772a_data->Event = CHK_CABLE_EXIST_AGAIN;
+			if (PowSave == 0x03){
+			  ax772a_data->TickToExpire = 47;
+			} else if (PowSave == 0x01) {
+			  ax772a_data->DlyIndex = (u8)(jiffies % 6);
+			  ax772a_data->DlySel = 0;
+			  ax772a_data->TickToExpire = 
+			  ChkCntSel[ax772a_data->DlyIndex][ax772a_data->DlySel];
+			}
+		}
+		break;
+	case CHK_CABLE_EXIST_AGAIN:
+		/* if cable disconnected */
+		phy_reg = ax8817x_mdio_read_le(dev->net, dev->mii.phy_id, 0x12);
+		if ((phy_reg != 0x8012) && (phy_reg != 0x8013)) {
+			mii_nway_restart(&dev->mii);
+			ax772a_data->Event = CHK_CABLE_STATUS;
+			ax772a_data->TickToExpire = 31;
+		} else if (--ax772a_data->TickToExpire == 0) {
+			/* Power down PHY */
+			ax8817x_write_cmd(dev, AX_CMD_SW_RESET,
+					  AX_SWRESET_IPPD,
+					  0, 0, NULL);
+			ax772a_data->Event = PHY_POWER_DOWN;
+			if (PowSave == 0x03){
+			  ax772a_data->TickToExpire = 23;
+			} else if (PowSave == 0x01) {
+			  ax772a_data->TickToExpire = 31;
+			}
+		}
+		break;
+	case PHY_POWER_DOWN:
+		if (--ax772a_data->TickToExpire == 0) {
+			ax772a_data->Event = PHY_POWER_UP;
+		}
+		break;
+	case CHK_CABLE_STATUS:
+		if (--ax772a_data->TickToExpire == 0) {
+			ax8817x_mdio_write_le(dev->net,
+					dev->mii.phy_id, 0x16, 0x4040);
+			mii_nway_restart(&dev->mii);
+			ax772a_data->Event = CHK_CABLE_EXIST_AGAIN;
+			if (PowSave == 0x03){
+			  ax772a_data->TickToExpire = 47;
+			} else if (PowSave == 0x01) {
+			  ax772a_data->DlyIndex = (u8)(jiffies % 6);
+			  ax772a_data->DlySel = 0;
+			  ax772a_data->TickToExpire = 
+			  ChkCntSel[ax772a_data->DlyIndex][ax772a_data->DlySel];
+			}
+		}
+		break;
+	case PHY_POWER_UP:
+
+		ax88772a_phy_powerup (dev);
+
+		ax8817x_mdio_write_le(dev->net, dev->mii.phy_id, MII_ADVERTISE,
 			ADVERTISE_ALL | ADVERTISE_CSMA | ADVERTISE_PAUSE_CAP);
-	asix_mdio_write(dev->net, dev->mii.phy_id, MII_CTRL1000,
-			ADVERTISE_1000FULL);
 
-	mii_nway_restart(&dev->mii);
+		mii_nway_restart(&dev->mii);
+
+		ax772a_data->Event = CHK_CABLE_EXIST_AGAIN;
+
+		if (PowSave == 0x03){
+			  ax772a_data->TickToExpire = 47;
+			  
+		} else if (PowSave == 0x01) {
+		  
+		  if (++ax772a_data->DlySel >= 3) {
+		    ax772a_data->DlyIndex = (u8)(jiffies % 6);
+		    ax772a_data->DlySel = 0;
+		  }  
+		  ax772a_data->TickToExpire = 
+			ChkCntSel[ax772a_data->DlyIndex][ax772a_data->DlySel];
+		}
+		break;
+	default:
+		break;
+	}
 
-	if ((ret = asix_write_medium_mode(dev, AX88178_MEDIUM_DEFAULT)) < 0)
-		goto out;
+	return;
+}
 
-	if ((ret = asix_write_rx_ctl(dev, AX_DEFAULT_RX_CTL)) < 0)
-		goto out;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void ax88772b_link_reset (void *data)
+{
+	struct usbnet *dev = (struct usbnet *)data;
+	struct ax88772b_data *ax772b_data = (struct ax88772b_data *)dev->priv;
+#else
+static void ax88772b_link_reset (struct work_struct *work)
+{
+	struct ax88772b_data *ax772b_data = container_of (work, 
+					struct ax88772b_data, check_link);
+	struct usbnet *dev = ax772b_data->dev;
+#endif
+
+	switch (ax772b_data->Event) {
+
+	case AX_SET_RX_CFG:
+	{
+		u16 bmcr = ax8817x_mdio_read_le(dev->net,
+					dev->mii.phy_id, MII_BMCR);
+		u16 mode = AX88772_MEDIUM_DEFAULT;
+
+		if (!(bmcr & BMCR_FULLDPLX))
+			mode &= ~AX88772_MEDIUM_FULL_DUPLEX;
+		if (!(bmcr & BMCR_SPEED100))
+			mode &= ~AX88772_MEDIUM_100MB;
+
+		ax8817x_write_cmd(dev, AX_CMD_WRITE_MEDIUM_MODE, 
+					mode, 0, 0, NULL);
+		break;
+	}
+	case PHY_POWER_UP:
+	{
+		u16 tmp16;
 
-	/* Asix framing packs multiple eth frames into a 2K usb bulk transfer */
-	if (dev->driver_info->flags & FLAG_FRAMING_AX) {
-		/* hard_mtu  is still the default - the device does not support
-		   jumbo eth frames */
-		dev->rx_urb_size = 2048;
+		ax88772a_phy_powerup (dev);
+		tmp16 = ax8817x_mdio_read_le(dev->net, dev->mii.phy_id, 0x12);
+		ax8817x_mdio_write_le(dev->net, dev->mii.phy_id, 0x12,
+				((tmp16 & 0xFF9F) | 0x0040));
+
+		ax8817x_mdio_write_le(dev->net, dev->mii.phy_id, MII_ADVERTISE,
+			ADVERTISE_ALL | ADVERTISE_CSMA | ADVERTISE_PAUSE_CAP);
+		break;
+	}
+	case AX_CHK_AUTODETACH:
+	{
+		u16 tmp16;
+		int ret;
+		void *buf;
+		buf = kmalloc(6, GFP_KERNEL);
+		devwarn(dev, "EVENT: AX_CHK_AUTODETACH\n");
+		/* Get the EEPROM data*/
+		if ((ret = ax8817x_read_cmd (dev, AX_CMD_READ_EEPROM,
+				     0x18, 0, 2, (void *)(&tmp16))) < 0) {
+			deverr(dev, "read SROM address 18h failed: %d", ret);
+		}
+		else {
+			ax772b_data->psc = le16_to_cpu(tmp16) & 0xFF00;
+			devwarn(dev, "EEPROM (0x18) = : %04X", ax772b_data->psc);
+			if ((ret = ax8817x_write_cmd (dev, AX_CMD_SW_RESET,
+	    				AX_SWRESET_IPRL | (ax772b_data->psc & 0x7FFF), 0, 0, buf)) < 0) {
+				deverr(dev, "Failed to configure PHY power saving: %d", ret);
+			}
+		}
+		break;
 	}
+	default:
+		break;
+	}
+
+	ax772b_data->Event = AX_NOP;
+
+	return;
+}
+
+static int ax88178_set_media(struct usbnet *dev)
+{
+	int	ret;
+	struct ax88178_data *ax178dataptr = (struct ax88178_data *)dev->priv;
+	int media;
+
+	media = ax88178_media_check (dev, ax178dataptr);
+	if (media < 0)
+		return media;
+
+	if ((ret = ax8817x_write_cmd(dev, AX_CMD_WRITE_MEDIUM_MODE, 
+			media, 0, 0, NULL)) < 0) {
+		deverr(dev, "write mode medium reg failed: %d", ret);
+		return ret;
+	}
+
 	return 0;
+}
 
-out:
-	return ret;
+static int ax88178_link_reset(struct usbnet *dev)
+{
+	return ax88178_set_media (dev);
+}
+
+static int ax_suspend (struct usb_interface *intf,
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,10)
+			pm_message_t message)
+#else
+			u32 message)
+#endif
+{
+	struct usbnet *dev = usb_get_intfdata(intf);
+	struct ax8817x_data *data = (struct ax8817x_data *)&dev->data;
+
+	return data->suspend (intf, message);
+}
+
+static int ax_resume (struct usb_interface *intf)
+{
+	struct usbnet *dev = usb_get_intfdata(intf);
+	struct ax8817x_data *data = (struct ax8817x_data *)&dev->data;
+
+	return data->resume (intf);
 }
 
+static const struct driver_info ax88178_info = {
+	.description = "ASIX AX88178 USB 2.0 Ethernet",
+	.bind = ax88178_bind,
+	.unbind = ax88178_unbind,
+	.status = ax88178_status,
+	.link_reset = ax88178_link_reset,
+	.reset = ax88178_link_reset,
+	.flags =  FLAG_ETHER | FLAG_FRAMING_AX,
+	.rx_fixup = ax88772_rx_fixup,
+	.tx_fixup = ax88772_tx_fixup,
+};
+
+static const struct driver_info belkin178_info = {
+	.description = "Belkin Gigabit USB 2.0 Network Adapter",
+	.bind = ax88178_bind,
+	.unbind = ax88178_unbind,
+	.status = ax8817x_status,
+	.link_reset = ax88178_link_reset,
+	.reset = ax88178_link_reset,
+	.flags =  FLAG_ETHER | FLAG_FRAMING_AX,
+	.rx_fixup = ax88772_rx_fixup,
+	.tx_fixup = ax88772_tx_fixup,
+};
+
 static const struct driver_info ax8817x_info = {
 	.description = "ASIX AX8817x USB 2.0 Ethernet",
-	.bind = ax88172_bind,
-	.status = asix_status,
+	.bind = ax8817x_bind,
+	.status = ax8817x_status,
 	.link_reset = ax88172_link_reset,
 	.reset = ax88172_link_reset,
-	.flags =  FLAG_ETHER | FLAG_LINK_INTR,
-	.data = 0x00130103,
+	.flags =  FLAG_ETHER,
 };
 
 static const struct driver_info dlink_dub_e100_info = {
 	.description = "DLink DUB-E100 USB Ethernet",
-	.bind = ax88172_bind,
-	.status = asix_status,
+	.bind = ax8817x_bind,
+	.status = ax8817x_status,
 	.link_reset = ax88172_link_reset,
 	.reset = ax88172_link_reset,
-	.flags =  FLAG_ETHER | FLAG_LINK_INTR,
-	.data = 0x009f9d9f,
+	.flags =  FLAG_ETHER,
 };
 
 static const struct driver_info netgear_fa120_info = {
 	.description = "Netgear FA-120 USB Ethernet",
-	.bind = ax88172_bind,
-	.status = asix_status,
+	.bind = ax8817x_bind,
+	.status = ax8817x_status,
 	.link_reset = ax88172_link_reset,
 	.reset = ax88172_link_reset,
-	.flags =  FLAG_ETHER | FLAG_LINK_INTR,
-	.data = 0x00130103,
+	.flags =  FLAG_ETHER,
 };
 
 static const struct driver_info hawking_uf200_info = {
 	.description = "Hawking UF200 USB Ethernet",
-	.bind = ax88172_bind,
-	.status = asix_status,
+	.bind = ax8817x_bind,
+	.status = ax8817x_status,
 	.link_reset = ax88172_link_reset,
 	.reset = ax88172_link_reset,
-	.flags =  FLAG_ETHER | FLAG_LINK_INTR,
-	.data = 0x001f1d1f,
+	.flags =  FLAG_ETHER,
 };
 
 static const struct driver_info ax88772_info = {
 	.description = "ASIX AX88772 USB 2.0 Ethernet",
 	.bind = ax88772_bind,
-	.status = asix_status,
-	.link_reset = ax88772_link_reset,
-	.reset = ax88772_link_reset,
-	.flags = FLAG_ETHER | FLAG_FRAMING_AX | FLAG_LINK_INTR,
-	.rx_fixup = asix_rx_fixup,
-	.tx_fixup = asix_tx_fixup,
+	.unbind = ax88772_unbind,
+	.status = ax88772_status,
+	.flags = FLAG_ETHER | FLAG_FRAMING_AX,
+	.rx_fixup = ax88772_rx_fixup,
+	.tx_fixup = ax88772_tx_fixup,
+	.stop = ax88772b_stop,
+	.reset =  ax88772b_reset,
 };
 
-static const struct driver_info ax88178_info = {
-	.description = "ASIX AX88178 USB 2.0 Ethernet",
-	.bind = ax88178_bind,
-	.status = asix_status,
-	.link_reset = ax88178_link_reset,
-	.reset = ax88178_link_reset,
-	.flags = FLAG_ETHER | FLAG_FRAMING_AX | FLAG_LINK_INTR,
-	.rx_fixup = asix_rx_fixup,
-	.tx_fixup = asix_tx_fixup,
+static const struct driver_info dlink_dub_e100b_info = {
+	.description = "D-Link DUB-E100 USB 2.0 Fast Ethernet Adapter",
+	.bind = ax88772_bind,
+	.unbind = ax88772_unbind,
+	.status = ax88772_status,
+	.flags = FLAG_ETHER | FLAG_FRAMING_AX,
+	.rx_fixup = ax88772_rx_fixup,
+	.tx_fixup = ax88772_tx_fixup,
+	.stop = ax88772b_stop,
+	.reset =  ax88772b_reset,
+};
+
+static const struct driver_info ax88772a_info = {
+	.description = "ASIX AX88772A USB 2.0 Ethernet",
+	.bind = ax88772a_bind,
+	.unbind = ax88772a_unbind,
+	.status = ax88772a_status,
+	.flags = FLAG_ETHER | FLAG_FRAMING_AX,
+	.rx_fixup = ax88772_rx_fixup,
+	.tx_fixup = ax88772_tx_fixup,
+	.stop = ax88772b_stop,
+	.reset =  ax88772b_reset,
+};
+
+static const struct driver_info ax88772b_info = {
+	.description = "ASIX AX88772B USB 2.0 Ethernet",
+	.bind = ax88772b_bind,
+	.unbind = ax88772b_unbind,
+	.status = ax88772b_status,
+	.flags = FLAG_ETHER | FLAG_FRAMING_AX | FLAG_HW_IP_ALIGNMENT,
+	.rx_fixup = ax88772b_rx_fixup,
+	.tx_fixup = ax88772b_tx_fixup,
+	.stop = ax88772b_stop,
+	.reset =  ax88772b_reset,
 };
 
 static const struct usb_device_id	products [] = {
 {
+	// 88178
+	USB_DEVICE (0x0b95, 0x1780),
+	.driver_info =	(unsigned long) &ax88178_info,
+}, {
+	// 88178 for billianton linksys
+	USB_DEVICE (0x077b, 0x2226),
+	.driver_info =	(unsigned long) &ax88178_info,
+}, {
+	// ABOCOM for linksys
+	USB_DEVICE (0x1737, 0x0039),
+	.driver_info =	(unsigned long) &ax88178_info,
+}, {
+	// ABOCOM  for pci
+	USB_DEVICE (0x14ea, 0xab11),
+	.driver_info =	(unsigned long) &ax88178_info,
+}, {
+	// Belkin
+	USB_DEVICE (0x050d, 0x5055),
+	.driver_info =	(unsigned long) &belkin178_info,
+}, {
 	// Linksys USB200M
 	USB_DEVICE (0x077b, 0x2226),
 	.driver_info =	(unsigned long) &ax8817x_info,
@@ -1457,6 +3677,14 @@ static const struct usb_device_id	products [] = {
 	USB_DEVICE (0x2001, 0x1a00),
 	.driver_info =  (unsigned long) &dlink_dub_e100_info,
 }, {
+	// DLink DUB-E100B
+	USB_DEVICE (0x2001, 0x3c05),
+	.driver_info =  (unsigned long) &dlink_dub_e100b_info,
+}, {
+	// DLink DUB-E100B
+	USB_DEVICE (0x07d1, 0x3c05),
+	.driver_info =  (unsigned long) &dlink_dub_e100b_info,
+}, {
 	// Intellinet, ST Lab USB Ethernet
 	USB_DEVICE (0x0b95, 0x1720),
 	.driver_info =  (unsigned long) &ax8817x_info,
@@ -1465,9 +3693,9 @@ static const struct usb_device_id	products [] = {
 	USB_DEVICE (0x07b8, 0x420a),
 	.driver_info =  (unsigned long) &hawking_uf200_info,
 }, {
-	// Billionton Systems, USB2AR
-	USB_DEVICE (0x08dd, 0x90ff),
-	.driver_info =  (unsigned long) &ax8817x_info,
+        // Billionton Systems, USB2AR
+        USB_DEVICE (0x08dd, 0x90ff),
+        .driver_info =  (unsigned long) &ax8817x_info,
 }, {
 	// ATEN UC210T
 	USB_DEVICE (0x0557, 0x2009),
@@ -1477,10 +3705,6 @@ static const struct usb_device_id	products [] = {
 	USB_DEVICE (0x0411, 0x003d),
 	.driver_info =  (unsigned long) &ax8817x_info,
 }, {
-	// Buffalo LUA-U2-GT 10/100/1000
-	USB_DEVICE (0x0411, 0x006e),
-	.driver_info =  (unsigned long) &ax88178_info,
-}, {
 	// Sitecom LN-029 "USB 2.0 10/100 Ethernet adapter"
 	USB_DEVICE (0x6189, 0x182d),
 	.driver_info =  (unsigned long) &ax8817x_info,
@@ -1497,82 +3721,53 @@ static const struct usb_device_id	products [] = {
 	USB_DEVICE (0x1631, 0x6200),
 	.driver_info = (unsigned long) &ax8817x_info,
 }, {
-	// JVC MP-PRX1 Port Replicator
-	USB_DEVICE (0x04f1, 0x3008),
-	.driver_info = (unsigned long) &ax8817x_info,
-}, {
-	// ASIX AX88772B 10/100
-	USB_DEVICE (0x0b95, 0x772b),
-	.driver_info = (unsigned long) &ax88772_info,
-}, {
 	// ASIX AX88772 10/100
-	USB_DEVICE (0x0b95, 0x7720),
-	.driver_info = (unsigned long) &ax88772_info,
-}, {
-	// ASIX AX88178 10/100/1000
-	USB_DEVICE (0x0b95, 0x1780),
-	.driver_info = (unsigned long) &ax88178_info,
-}, {
-	// Logitec LAN-GTJ/U2A
-	USB_DEVICE (0x0789, 0x0160),
-	.driver_info = (unsigned long) &ax88178_info,
+        USB_DEVICE (0x0b95, 0x7720),
+        .driver_info = (unsigned long) &ax88772_info,
 }, {
-	// Linksys USB200M Rev 2
-	USB_DEVICE (0x13b1, 0x0018),
-	.driver_info = (unsigned long) &ax88772_info,
-}, {
-	// 0Q0 cable ethernet
-	USB_DEVICE (0x1557, 0x7720),
-	.driver_info = (unsigned long) &ax88772_info,
-}, {
-	// DLink DUB-E100 H/W Ver B1
-	USB_DEVICE (0x07d1, 0x3c05),
-	.driver_info = (unsigned long) &ax88772_info,
-}, {
-	// DLink DUB-E100 H/W Ver B1 Alternate
-	USB_DEVICE (0x2001, 0x3c05),
-	.driver_info = (unsigned long) &ax88772_info,
+	// ASIX AX88772 10/100
+        USB_DEVICE (0x125E, 0x180D),
+        .driver_info = (unsigned long) &ax88772_info,
 }, {
-	// Linksys USB1000
-	USB_DEVICE (0x1737, 0x0039),
-	.driver_info = (unsigned long) &ax88178_info,
+	// ASIX AX88772A 10/100
+        USB_DEVICE (0x0b95, 0x772A),
+        .driver_info = (unsigned long) &ax88772a_info,
 }, {
-	// IO-DATA ETG-US2
-	USB_DEVICE (0x04bb, 0x0930),
-	.driver_info = (unsigned long) &ax88178_info,
+	// ASIX AX88772A 10/100
+        USB_DEVICE (0x0db0, 0xA877),
+        .driver_info = (unsigned long) &ax88772a_info,
 }, {
-	// Belkin F5D5055
-	USB_DEVICE(0x050d, 0x5055),
-	.driver_info = (unsigned long) &ax88178_info,
+	// ASIX AX88772A 10/100
+        USB_DEVICE (0x0421, 0x772A),
+        .driver_info = (unsigned long) &ax88772a_info,
 }, {
-	// Apple USB Ethernet Adapter
-	USB_DEVICE(0x05ac, 0x1402),
-	.driver_info = (unsigned long) &ax88772_info,
+	// Linksys 200M
+        USB_DEVICE (0x13B1, 0x0018),
+        .driver_info = (unsigned long) &ax88772a_info,
 }, {
-	// Cables-to-Go USB Ethernet Adapter
-	USB_DEVICE(0x0b95, 0x772a),
-	.driver_info = (unsigned long) &ax88772_info,
+        USB_DEVICE (0x05ac, 0x1402),
+        .driver_info = (unsigned long) &ax88772a_info,
 }, {
-	// ABOCOM for pci
-	USB_DEVICE(0x14ea, 0xab11),
-	.driver_info = (unsigned long) &ax88178_info,
+	// ASIX AX88772B 10/100
+        USB_DEVICE (0x0b95, 0x772B),
+        .driver_info = (unsigned long) &ax88772b_info,
 }, {
-	// ASIX 88772a
-	USB_DEVICE(0x0db0, 0xa877),
-	.driver_info = (unsigned long) &ax88772_info,
+	// ASIX AX88772B 10/100
+        USB_DEVICE (0x0b95, 0x7E2B),
+        .driver_info = (unsigned long) &ax88772b_info,
 },
 	{ },		// END
 };
 MODULE_DEVICE_TABLE(usb, products);
 
 static struct usb_driver asix_driver = {
+//	.owner =	THIS_MODULE,
 	.name =		"asix",
 	.id_table =	products,
-	.probe =	usbnet_probe,
-	.suspend =	usbnet_suspend,
-	.resume =	usbnet_resume,
-	.disconnect =	usbnet_disconnect,
-	.supports_autosuspend = 1,
+	.probe =	axusbnet_probe,
+	.suspend =	ax_suspend,
+	.resume =	ax_resume,
+	.disconnect =	axusbnet_disconnect,
 };
 
 static int __init asix_init(void)
diff --git a/drivers/net/usb/asix.h b/drivers/net/usb/asix.h
new file mode 100644
index 000000000000..ea51eb7001e2
--- /dev/null
+++ b/drivers/net/usb/asix.h
@@ -0,0 +1,558 @@
+#ifndef	__LINUX_USBNET_ASIX_H
+#define	__LINUX_USBNET_ASIX_H
+
+/*
+ * Turn on this flag if the implementation of your USB host controller
+ * cannot handle non-double word aligned buffer.
+ * When turn on this flag, driver will fixup egress packet aligned on double
+ * word boundary before deliver to USB host controller. And will Disable the
+ * function "skb_reserve (skb, NET_IP_ALIGN)" to retain the buffer aligned on
+ * double word alignment for ingress packets.
+ */
+#define AX_FORCE_BUFF_ALIGN		0
+
+#define AX_MONITOR_MODE			0x01
+#define AX_MONITOR_LINK			0x02
+#define AX_MONITOR_MAGIC		0x04
+#define AX_MONITOR_HSFS			0x10
+
+/* AX88172 Medium Status Register values */
+#define AX_MEDIUM_FULL_DUPLEX		0x02
+#define AX_MEDIUM_TX_ABORT_ALLOW	0x04
+#define AX_MEDIUM_FLOW_CONTROL_EN	0x10
+#define AX_MCAST_FILTER_SIZE		8
+#define AX_MAX_MCAST			64
+
+#define AX_EEPROM_LEN			0x40
+
+#define AX_SWRESET_CLEAR		0x00
+#define AX_SWRESET_RR			0x01
+#define AX_SWRESET_RT			0x02
+#define AX_SWRESET_PRTE			0x04
+#define AX_SWRESET_PRL			0x08
+#define AX_SWRESET_BZ			0x10
+#define AX_SWRESET_IPRL			0x20
+#define AX_SWRESET_IPPD			0x40
+#define AX_SWRESET_IPOSC		0x0080
+#define AX_SWRESET_IPPSL_0		0x0100
+#define AX_SWRESET_IPPSL_1		0x0200
+#define AX_SWRESET_IPCOPS		0x0400
+#define AX_SWRESET_IPCOPSC		0x0800
+#define AX_SWRESET_AUTODETACH		0x1000
+#define AX_SWRESET_WOLLP		0x8000
+
+#define AX88772_IPG0_DEFAULT		0x15
+#define AX88772_IPG1_DEFAULT		0x0c
+#define AX88772_IPG2_DEFAULT		0x0E
+
+#define AX88772A_IPG0_DEFAULT		0x15
+#define AX88772A_IPG1_DEFAULT		0x16
+#define AX88772A_IPG2_DEFAULT		0x1A
+
+#define AX88772_MEDIUM_FULL_DUPLEX	0x0002
+#define AX88772_MEDIUM_RESERVED		0x0004
+#define AX88772_MEDIUM_RX_FC_ENABLE	0x0010
+#define AX88772_MEDIUM_TX_FC_ENABLE	0x0020
+#define AX88772_MEDIUM_PAUSE_FORMAT	0x0080
+#define AX88772_MEDIUM_RX_ENABLE	0x0100
+#define AX88772_MEDIUM_100MB		0x0200
+#define AX88772_MEDIUM_DEFAULT	\
+	(AX88772_MEDIUM_FULL_DUPLEX | AX88772_MEDIUM_RX_FC_ENABLE | \
+	 AX88772_MEDIUM_TX_FC_ENABLE | AX88772_MEDIUM_100MB | \
+	 AX88772_MEDIUM_RESERVED | AX88772_MEDIUM_RX_ENABLE )
+
+#define AX_CMD_SET_SW_MII		0x06
+#define AX_CMD_READ_MII_REG		0x07
+#define AX_CMD_WRITE_MII_REG		0x08
+#define AX_CMD_SET_HW_MII		0x0a
+#define AX_CMD_READ_EEPROM		0x0b
+#define AX_CMD_WRITE_EEPROM		0x0c
+#define AX_CMD_WRITE_EEPROM_EN		0x0d
+#define AX_CMD_WRITE_EEPROM_DIS		0x0e
+#define AX_CMD_WRITE_RX_CTL		0x10
+#define AX_CMD_READ_IPG012		0x11
+#define AX_CMD_WRITE_IPG0		0x12
+#define AX_CMD_WRITE_IPG1		0x13
+#define AX_CMD_WRITE_IPG2		0x14
+#define AX_CMD_WRITE_MULTI_FILTER	0x16
+#define AX_CMD_READ_NODE_ID		0x17
+#define AX_CMD_READ_PHY_ID		0x19
+#define AX_CMD_READ_MEDIUM_MODE		0x1a
+#define AX_CMD_WRITE_MEDIUM_MODE	0x1b
+#define AX_CMD_READ_MONITOR_MODE	0x1c
+#define AX_CMD_WRITE_MONITOR_MODE	0x1d
+#define AX_CMD_WRITE_GPIOS		0x1f
+#define AX_CMD_SW_RESET 		0x20
+#define AX_CMD_SW_PHY_STATUS		0x21
+#define AX_CMD_SW_PHY_SELECT		0x22
+	#define AX_PHYSEL_PSEL		(1 << 0)
+	#define AX_PHYSEL_ASEL		(1 << 1)
+	#define AX_PHYSEL_SSMII		(0 << 2)
+	#define AX_PHYSEL_SSRMII	(1 << 2)
+	#define AX_PHYSEL_SSRRMII	(3 << 2)
+	#define AX_PHYSEL_SSEN		(1 << 4)
+#define AX88772_CMD_READ_NODE_ID	0x13
+#define AX88772_CMD_WRITE_NODE_ID	0x14
+#define AX_CMD_READ_RXCOE_CTL		0x2b
+#define AX_CMD_WRITE_RXCOE_CTL		0x2c
+#define AX_CMD_READ_TXCOE_CTL		0x2d
+#define AX_CMD_WRITE_TXCOE_CTL		0x2e
+
+#define REG_LENGTH			2
+#define PHY_ID_MASK			0x1f
+
+#define AX_RXCOE_IPCE			0x0001
+#define AX_RXCOE_IPVE			0x0002
+#define AX_RXCOE_V6VE			0x0004
+#define AX_RXCOE_TCPE			0x0008
+#define AX_RXCOE_UDPE			0x0010
+#define AX_RXCOE_ICMP			0x0020
+#define AX_RXCOE_IGMP			0x0040
+#define AX_RXCOE_ICV6			0x0080
+#define AX_RXCOE_TCPV6			0x0100
+#define AX_RXCOE_UDPV6			0x0200
+#define AX_RXCOE_ICMV6			0x0400
+#define AX_RXCOE_IGMV6			0x0800
+#define AX_RXCOE_ICV6V6			0x1000
+#define AX_RXCOE_FOPC			0x8000
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,22)
+#define AX_RXCOE_DEF_CSUM		(AX_RXCOE_IPCE | AX_RXCOE_IPVE | \
+					 AX_RXCOE_V6VE | AX_RXCOE_TCPE | \
+					 AX_RXCOE_UDPE |  AX_RXCOE_ICV6 | \
+					 AX_RXCOE_TCPV6 | AX_RXCOE_UDPV6)
+#else
+#define AX_RXCOE_DEF_CSUM		(AX_RXCOE_IPCE | AX_RXCOE_IPVE | \
+					 AX_RXCOE_TCPE | AX_RXCOE_UDPE)
+#endif
+
+#define AX_RXCOE_64TE			0x0100
+#define AX_RXCOE_PPPOE			0x0200
+#define AX_RXCOE_RPCE			0x8000
+
+#define AX_TXCOE_IP			0x0001
+#define AX_TXCOE_TCP			0x0002
+#define AX_TXCOE_UDP			0x0004
+#define AX_TXCOE_ICMP			0x0008
+#define AX_TXCOE_IGMP			0x0010
+#define AX_TXCOE_ICV6			0x0020
+
+#define AX_TXCOE_TCPV6			0x0100
+#define AX_TXCOE_UDPV6			0x0200
+#define AX_TXCOE_ICMV6			0x0400
+#define AX_TXCOE_IGMV6			0x0800
+#define AX_TXCOE_ICV6V6			0x1000
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,22)
+#define AX_TXCOE_DEF_CSUM		(AX_TXCOE_TCP | AX_TXCOE_UDP | \
+					 AX_TXCOE_TCPV6 | AX_TXCOE_UDPV6)
+#else
+#define AX_TXCOE_DEF_CSUM		(AX_TXCOE_TCP | AX_TXCOE_UDP)
+#endif
+
+#define AX_TXCOE_64TE			0x0001
+#define AX_TXCOE_PPPE			0x0002
+
+#define AX88772B_MAX_BULKIN_2K		0
+#define AX88772B_MAX_BULKIN_4K		1
+#define AX88772B_MAX_BULKIN_6K		2
+#define AX88772B_MAX_BULKIN_8K		3
+#define AX88772B_MAX_BULKIN_16K		4
+#define AX88772B_MAX_BULKIN_20K		5
+#define AX88772B_MAX_BULKIN_24K		6
+#define AX88772B_MAX_BULKIN_32K		7
+struct {unsigned short size, byte_cnt,threshold;} AX88772B_BULKIN_SIZE[] =
+{
+	/* 2k */
+	{2048, 0x8000, 0x8001},
+	/* 4k */
+	{4096, 0x8100, 0x8147},
+	/* 6k */
+	{6144, 0x8200, 0x81EB},
+	/* 8k */
+	{8192, 0x8300, 0x83D7},
+	/* 16 */
+	{16384, 0x8400, 0x851E},
+	/* 20k */
+	{20480, 0x8500, 0x8666},
+	/* 24k */
+	{24576, 0x8600, 0x87AE},
+	/* 32k */
+	{32768, 0x8700, 0x8A3D},
+};
+
+
+#define AX_RX_CTL_RH1M			0x0100		/* Enable RX-Header mode 0 */
+#define AX_RX_CTL_RH2M			0x0200		/* Enable IP header in receive buffer aligned on 32-bit aligment */
+#define AX_RX_CTL_RH3M			0x0400		/* checksum value in rx header 3 */
+#define AX_RX_HEADER_DEFAULT		(AX_RX_CTL_RH1M | AX_RX_CTL_RH2M)
+
+#define AX_RX_CTL_MFB			0x0300		/* Maximum Frame size 16384bytes */
+#define AX_RX_CTL_START			0x0080		/* Ethernet MAC start */
+#define AX_RX_CTL_AP			0x0020		/* Accept physcial address from Multicast array */
+#define AX_RX_CTL_AM			0x0010	
+#define AX_RX_CTL_AB			0x0008		/* Accetp Brocadcast frames*/
+#define AX_RX_CTL_SEP			0x0004		/* Save error packets */	
+#define AX_RX_CTL_AMALL			0x0002		/* Accetp all multicast frames */
+#define AX_RX_CTL_PRO			0x0001		/* Promiscuous Mode */
+#define AX_RX_CTL_STOP			0x0000		/* Stop MAC */
+
+#define AX_MONITOR_MODE 		0x01
+#define AX_MONITOR_LINK 		0x02
+#define AX_MONITOR_MAGIC		0x04
+#define AX_MONITOR_HSFS 		0x10
+
+#define AX_MCAST_FILTER_SIZE		8
+#define AX_MAX_MCAST			64
+#define AX_INTERRUPT_BUFSIZE		8
+
+#define AX_EEPROM_LEN			0x40
+#define AX_EEPROM_MAGIC 		0xdeadbeef
+#define EEPROMMASK			0x7f
+
+/* GPIO REGISTER */
+#define AXGPIOS_GPO0EN			0X01 // 1 << 0
+#define AXGPIOS_GPO0			0X02 // 1 << 1
+#define AXGPIOS_GPO1EN			0X04 // 1 << 2
+#define AXGPIOS_GPO1			0X08 // 1 << 3
+#define AXGPIOS_GPO2EN			0X10 // 1 << 4
+#define AXGPIOS_GPO2			0X20 // 1 << 5
+#define AXGPIOS_RSE			0X80 // 1 << 7
+
+/* TX-header format */
+#define AX_TX_HDR_CPHI			0x4000
+#define AX_TX_HDR_DICF			0x8000
+
+// GMII register definitions
+#define GMII_PHY_CONTROL		0x00	// control reg
+#define GMII_PHY_STATUS			0x01	// status reg
+#define GMII_PHY_OUI			0x02	// most of the OUI bits
+#define GMII_PHY_MODEL			0x03	// model/rev bits, and rest of OUI
+#define GMII_PHY_ANAR			0x04	// AN advertisement reg
+#define GMII_PHY_ANLPAR			0x05	// AN Link Partner
+#define GMII_PHY_ANER			0x06	// AN expansion reg
+#define GMII_PHY_1000BT_CONTROL		0x09	// control reg for 1000BT
+#define GMII_PHY_1000BT_STATUS		0x0A	// status reg for 1000BT
+
+// Bit definitions: GMII Control
+#define GMII_CONTROL_RESET		0x8000	// reset bit in control reg
+#define GMII_CONTROL_LOOPBACK		0x4000	// loopback bit in control reg
+#define GMII_CONTROL_10MB		0x0000	// 10 Mbit
+#define GMII_CONTROL_100MB		0x2000	// 100Mbit
+#define GMII_CONTROL_1000MB		0x0040	// 1000Mbit
+#define GMII_CONTROL_SPEED_BITS		0x2040	// speed bit mask
+#define GMII_CONTROL_ENABLE_AUTO	0x1000	// autonegotiate enable
+#define GMII_CONTROL_POWER_DOWN		0x0800
+#define GMII_CONTROL_ISOLATE		0x0400	// islolate bit
+#define GMII_CONTROL_START_AUTO		0x0200	// restart autonegotiate
+#define GMII_CONTROL_FULL_DUPLEX	0x0100
+
+// Bit definitions: GMII Status
+#define GMII_STATUS_100MB_MASK		0xE000	// any of these indicate 100 Mbit
+#define GMII_STATUS_10MB_MASK		0x1800	// either of these indicate 10 Mbit
+#define GMII_STATUS_AUTO_DONE		0x0020	// auto negotiation complete
+#define GMII_STATUS_AUTO		0x0008	// auto negotiation is available
+#define GMII_STATUS_LINK_UP		0x0004	// link status bit
+#define GMII_STATUS_EXTENDED		0x0001	// extended regs exist
+#define GMII_STATUS_100T4		0x8000	// capable of 100BT4
+#define GMII_STATUS_100TXFD		0x4000	// capable of 100BTX full duplex
+#define GMII_STATUS_100TX		0x2000	// capable of 100BTX
+#define GMII_STATUS_10TFD		0x1000	// capable of 10BT full duplex
+#define GMII_STATUS_10T			0x0800	// capable of 10BT
+
+// Bit definitions: Auto-Negotiation Advertisement
+#define GMII_ANAR_ASYM_PAUSE		0x0800	// support asymetric pause
+#define GMII_ANAR_PAUSE			0x0400	// support pause packets
+#define GMII_ANAR_100T4			0x0200	// support 100BT4
+#define GMII_ANAR_100TXFD		0x0100	// support 100BTX full duplex
+#define GMII_ANAR_100TX			0x0080	// support 100BTX half duplex
+#define GMII_ANAR_10TFD			0x0040	// support 10BT full duplex
+#define GMII_ANAR_10T			0x0020	// support 10BT half duplex
+#define GMII_SELECTOR_FIELD		0x001F	// selector field.
+
+// Bit definitions: Auto-Negotiation Link Partner Ability
+#define GMII_ANLPAR_100T4		0x0200	// support 100BT4
+#define GMII_ANLPAR_100TXFD		0x0100	// support 100BTX full duplex
+#define GMII_ANLPAR_100TX		0x0080	// support 100BTX half duplex
+#define GMII_ANLPAR_10TFD		0x0040	// support 10BT full duplex
+#define GMII_ANLPAR_10T			0x0020	// support 10BT half duplex
+#define GMII_ANLPAR_PAUSE		0x0400	// support pause packets
+#define GMII_ANLPAR_ASYM_PAUSE		0x0800	// support asymetric pause
+#define GMII_ANLPAR_ACK			0x4000	// means LCB was successfully rx'd
+#define GMII_SELECTOR_8023		0x0001;
+
+// Bit definitions: 1000BaseT AUX Control
+#define GMII_1000_AUX_CTRL_MASTER_SLAVE		0x1000
+#define GMII_1000_AUX_CTRL_FD_CAPABLE		0x0200	// full duplex capable
+#define GMII_1000_AUX_CTRL_HD_CAPABLE		0x0100	// half duplex capable
+
+// Bit definitions: 1000BaseT AUX Status
+#define GMII_1000_AUX_STATUS_FD_CAPABLE		0x0800	// full duplex capable
+#define GMII_1000_AUX_STATUS_HD_CAPABLE 	0x0400	// half duplex capable
+
+// Cicada MII Registers
+#define GMII_AUX_CTRL_STATUS			0x1C
+#define GMII_AUX_ANEG_CPLT			0x8000
+#define GMII_AUX_FDX				0x0020
+#define GMII_AUX_SPEED_1000			0x0010
+#define GMII_AUX_SPEED_100			0x0008
+
+#ifndef ADVERTISE_PAUSE_CAP
+#define ADVERTISE_PAUSE_CAP			0x0400
+#endif
+
+#ifndef MII_STAT1000
+#define MII_STAT1000				0x000A
+#endif
+
+#ifndef LPA_1000FULL
+#define LPA_1000FULL				0x0800
+#endif
+
+// medium mode register
+#define MEDIUM_GIGA_MODE			0x0001
+#define MEDIUM_FULL_DUPLEX_MODE			0x0002
+#define MEDIUM_TX_ABORT_MODE			0x0004
+#define MEDIUM_ENABLE_125MHZ			0x0008
+#define MEDIUM_ENABLE_RX_FLOWCTRL		0x0010
+#define MEDIUM_ENABLE_TX_FLOWCTRL		0x0020
+#define MEDIUM_ENABLE_JUMBO_FRAME		0x0040
+#define MEDIUM_CHECK_PAUSE_FRAME_MODE		0x0080
+#define MEDIUM_ENABLE_RECEIVE			0x0100
+#define MEDIUM_MII_100M_MODE			0x0200
+#define MEDIUM_ENABLE_JAM_PATTERN		0x0400
+#define MEDIUM_ENABLE_STOP_BACKPRESSURE		0x0800
+#define MEDIUM_ENABLE_SUPPER_MAC_SUPPORT	0x1000
+
+/* PHY mode */
+#define PHY_MODE_MARVELL		0
+#define PHY_MODE_CICADA_FAMILY		1
+#define PHY_MODE_CICADA_V1		1
+#define PHY_MODE_AGERE_FAMILY		2
+#define PHY_MODE_AGERE_V0		2
+#define PHY_MODE_CICADA_V2		5
+#define PHY_MODE_AGERE_V0_GMII		6
+#define PHY_MODE_CICADA_V2_ASIX		9
+#define PHY_MODE_VSC8601		10
+#define PHY_MODE_RTL8211CL		12
+#define PHY_MODE_RTL8211BN		13
+#define PHY_MODE_RTL8251CL		14
+#define PHY_MODE_ATTANSIC_V0		0x40
+#define PHY_MODE_ATTANSIC_FAMILY	0x40
+#define PHY_MODE_MAC_TO_MAC_GMII	0x7C
+
+/*  */
+#define LED_MODE_MARVELL		0
+#define LED_MODE_CAMEO			1
+
+#define MARVELL_LED_CTRL		0x18
+#define MARVELL_MANUAL_LED		0x19
+
+#define PHY_IDENTIFIER			0x0002
+#define PHY_AGERE_IDENTIFIER		0x0282
+#define PHY_CICADA_IDENTIFIER		0x000f
+#define PHY_MARVELL_IDENTIFIER		0x0141
+
+#define PHY_MARVELL_STATUS		0x001b
+#define MARVELL_STATUS_HWCFG		0x0004		/* SGMII without clock */
+
+#define PHY_MARVELL_CTRL		0x0014
+#define MARVELL_CTRL_RXDELAY		0x0080
+#define MARVELL_CTRL_TXDELAY		0x0002
+
+#define PHY_CICADA_EXTPAGE		0x001f
+#define CICADA_EXTPAGE_EN		0x0001
+#define CICADA_EXTPAGE_DIS		0x0000
+
+
+struct {unsigned short value, offset; } CICADA_FAMILY_HWINIT[] =
+{
+	{0x0001, 0x001f}, {0x1c25, 0x0017}, {0x2a30, 0x001f}, {0x234c, 0x0010},
+	{0x2a30, 0x001f}, {0x0212, 0x0008}, {0x52b5, 0x001f}, {0xa7fa, 0x0000},
+	{0x0012, 0x0002}, {0x3002, 0x0001}, {0x87fa, 0x0000}, {0x52b5, 0x001f},
+	{0xafac, 0x0000}, {0x000d, 0x0002}, {0x001c, 0x0001}, {0x8fac, 0x0000},
+	{0x2a30, 0x001f}, {0x0012, 0x0008}, {0x2a30, 0x001f}, {0x0400, 0x0014},
+	{0x2a30, 0x001f}, {0x0212, 0x0008}, {0x52b5, 0x001f}, {0xa760, 0x0000},
+	{0x0000, 0x0002}, {0xfaff, 0x0001}, {0x8760, 0x0000}, {0x52b5, 0x001f},
+	{0xa760, 0x0000}, {0x0000, 0x0002}, {0xfaff, 0x0001}, {0x8760, 0x0000},
+	{0x52b5, 0x001f}, {0xafae, 0x0000}, {0x0004, 0x0002}, {0x0671, 0x0001},
+	{0x8fae, 0x0000}, {0x2a30, 0x001f}, {0x0012, 0x0008}, {0x0000, 0x001f},
+};
+
+struct {unsigned short value, offset; } CICADA_V2_HWINIT[] =
+{
+	{0x2a30, 0x001f}, {0x0212, 0x0008}, {0x52b5, 0x001f}, {0x000f, 0x0002},
+	{0x472a, 0x0001}, {0x8fa4, 0x0000}, {0x2a30, 0x001f}, {0x0212, 0x0008},
+	{0x0000, 0x001f},
+};
+
+struct {unsigned short value, offset; } CICADA_V2_ASIX_HWINIT[] =
+{
+	{0x2a30, 0x001f}, {0x0212, 0x0008}, {0x52b5, 0x001f}, {0x0012, 0x0002},
+	{0x3002, 0x0001}, {0x87fa, 0x0000}, {0x52b5, 0x001f}, {0x000f, 0x0002},
+	{0x472a, 0x0001}, {0x8fa4, 0x0000}, {0x2a30, 0x001f}, {0x0212, 0x0008},
+	{0x0000, 0x001f},
+};
+
+struct {unsigned short value, offset; } AGERE_FAMILY_HWINIT[] =
+{
+	{0x0800, 0x0000}, {0x0007, 0x0012}, {0x8805, 0x0010}, {0xb03e, 0x0011},
+	{0x8808, 0x0010}, {0xe110, 0x0011}, {0x8806, 0x0010}, {0xb03e, 0x0011},
+	{0x8807, 0x0010}, {0xff00, 0x0011}, {0x880e, 0x0010}, {0xb4d3, 0x0011},
+	{0x880f, 0x0010}, {0xb4d3, 0x0011}, {0x8810, 0x0010}, {0xb4d3, 0x0011},
+	{0x8817, 0x0010}, {0x1c00, 0x0011}, {0x300d, 0x0010}, {0x0001, 0x0011},
+	{0x0002, 0x0012},
+};
+
+struct ax88178_data {
+	u16	EepromData;
+	u16	MediaLink;
+	int	UseGpio0;
+	int	UseRgmii;
+	u8	PhyMode;
+	u8	LedMode;
+	u8	BuffaloOld;
+};
+
+enum watchdog_state {
+	AX_NOP = 0,
+	CHK_LINK,			/* Routine A */
+	CHK_CABLE_EXIST,		/* Called by A */
+	CHK_CABLE_EXIST_AGAIN,		/* Routine B */
+	PHY_POWER_UP,			/* Called by B */
+	PHY_POWER_UP_BH,
+	PHY_POWER_DOWN,
+	CHK_CABLE_STATUS,		/* Routine C */
+	WAIT_AUTONEG_COMPLETE,
+	AX_SET_RX_CFG,
+	AX_CHK_AUTODETACH,
+};
+
+struct ax88772b_data {
+	struct usbnet *dev;
+	struct workqueue_struct *ax_work;
+	struct work_struct check_link;
+	unsigned long time_to_chk;
+	u16 psc;
+	u8 pw_enabled;
+	u8 Event;
+	u8 checksum;
+	u8 PhySelect:1;
+	u8 OperationMode:1;
+	
+};
+
+// define for MAC or PHY mode
+#define OPERATION_MAC_MODE			0
+#define OPERATION_PHY_MODE			1
+
+struct ax88772a_data {
+	struct usbnet *dev;
+	struct workqueue_struct *ax_work;
+	struct work_struct check_link;
+	unsigned long autoneg_start;
+#define AX88772B_WATCHDOG	(6 * HZ)
+	u8 Event;
+	u8 TickToExpire;
+	u8 DlyIndex;
+	u8 DlySel;
+	u16 EepromData;
+};
+
+struct ax88772_data {
+	struct usbnet *dev;
+	struct workqueue_struct *ax_work;
+	struct work_struct check_link;
+	unsigned long autoneg_start;
+	u8 Event;
+	u8 TickToExpire;
+};
+
+#define AX_RX_CHECKSUM		1
+#define AX_TX_CHECKSUM		2
+
+/* This structure cannot exceed sizeof(unsigned long [5]) AKA 20 bytes */
+struct ax8817x_data {
+	u8 multi_filter[AX_MCAST_FILTER_SIZE];
+	int (*resume) (struct usb_interface *intf);
+	int (*suspend) (struct usb_interface *intf,
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,10)
+					pm_message_t message);
+#else
+					u32 message);
+#endif
+};
+
+struct ax88172_int_data {
+	u16 res1;
+#define AX_INT_PPLS_LINK		(1 << 0)
+#define AX_INT_SPLS_LINK		(1 << 1)
+#define AX_INT_CABOFF_UNPLUG		(1 << 7)
+	u8 link;
+	u16 res2;
+	u8 status;
+	u16 res3;
+} __attribute__ ((packed));
+
+#define AX_RXHDR_L4_ERR		(1 << 8)
+#define AX_RXHDR_L3_ERR		(1 << 9)
+
+#define AX_RXHDR_L4_TYPE_UDP		1
+#define AX_RXHDR_L4_TYPE_ICMP		2
+#define AX_RXHDR_L4_TYPE_IGMP		3
+#define AX_RXHDR_L4_TYPE_TCP		4
+#define AX_RXHDR_L4_TYPE_TCMPV6	5
+#define AX_RXHDR_L4_TYPE_MASK		7
+
+#define AX_RXHDR_L3_TYPE_IP		1
+#define AX_RXHDR_L3_TYPE_IPV6		2
+
+struct ax88772b_rx_header {
+#if defined(__LITTLE_ENDIAN_BITFIELD)
+	u16	len:11,
+		res1:1,
+		crc:1,
+		mii:1,
+		runt:1,
+		mc_bc:1;
+
+	u16	len_bar:11,
+		res2:5;
+
+	u8	vlan_ind:3,
+		vlan_tag_striped:1,
+		pri:3,
+		res3:1;
+
+	u8	l4_csum_err:1,
+		l3_csum_err:1,
+		l4_type:3,
+		l3_type:2,
+		ce:1;
+#elif defined (__BIG_ENDIAN_BITFIELD)
+	u16	mc_bc:1,
+		runt:1,
+		mii:1,
+		crc:1,
+		res1:1,
+		len:11;
+
+	u16	res2:5,
+		len_bar:11;
+
+	u8	res3:1,
+		pri:3,
+		vlan_tag_striped:1,
+		vlan_ind:3;
+
+	u8	ce:1,
+		l3_type:2,
+		l4_type:3,
+		l3_csum_err:1,
+		l4_csum_err:1;
+#else
+#error	"Please fix <asm/byteorder.h>"
+#endif
+
+} __attribute__ ((packed));
+
+#endif /* __LINUX_USBNET_ASIX_H */
+
diff --git a/drivers/net/usb/axusbnet.c b/drivers/net/usb/axusbnet.c
new file mode 100644
index 000000000000..00393d38a379
--- /dev/null
+++ b/drivers/net/usb/axusbnet.c
@@ -0,0 +1,1374 @@
+/*
+ * USB Network driver infrastructure
+ * Copyright (C) 2000-2005 by David Brownell
+ * Copyright (C) 2003-2005 David Hollis <dhollis@davehollis.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * 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
+ */
+
+/*
+ * This is a generic "USB networking" framework that works with several
+ * kinds of full and high speed networking devices:  host-to-host cables,
+ * smart usb peripherals, and actual Ethernet adapters.
+ *
+ * These devices usually differ in terms of control protocols (if they
+ * even have one!) and sometimes they define new framing to wrap or batch
+ * Ethernet packets.  Otherwise, they talk to USB pretty much the same,
+ * so interface (un)binding, endpoint I/O queues, fault handling, and other
+ * issues can usefully be addressed by this framework.
+ */
+
+#define	DEBUG			// error path messages, extra info
+// #define	VERBOSE			// more; success messages
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/ctype.h>
+#include <linux/ethtool.h>
+#include <linux/workqueue.h>
+#include <linux/mii.h>
+#include <linux/usb.h>
+/*#include <linux/usb/usbnet.h>*/
+
+#include "asix.h"
+#include "axusbnet.h"
+
+#define DRIVER_VERSION		"22-Aug-2005"
+
+static void axusbnet_unlink_rx_urbs(struct usbnet *);
+
+extern void
+ax8817x_write_cmd_async(struct usbnet *dev, u8 cmd, u16 value, u16 index,
+				    u16 size, void *data);
+
+/*-------------------------------------------------------------------------*/
+
+/*
+ * Nineteen USB 1.1 max size bulk transactions per frame (ms), max.
+ * Several dozen bytes of IPv4 data can fit in two such transactions.
+ * One maximum size Ethernet packet takes twenty four of them.
+ * For high speed, each frame comfortably fits almost 36 max size
+ * Ethernet packets (so queues should be bigger).
+ *
+ * REVISIT qlens should be members of 'struct usbnet'; the goal is to
+ * let the USB host controller be busy for 5msec or more before an irq
+ * is required, under load.  Jumbograms change the equation.
+ */
+#define RX_MAX_QUEUE_MEMORY (60 * 1518)
+#define	RX_QLEN(dev) (((dev)->udev->speed == USB_SPEED_HIGH) ? \
+			(RX_MAX_QUEUE_MEMORY/(dev)->rx_urb_size) : 4)
+#define	TX_QLEN(dev) (((dev)->udev->speed == USB_SPEED_HIGH) ? \
+			(RX_MAX_QUEUE_MEMORY/(dev)->hard_mtu) : 4)
+
+// reawaken network queue this soon after stopping; else watchdog barks
+//#define TX_TIMEOUT_JIFFIES	(5*HZ)
+#define TX_TIMEOUT_JIFFIES	(30*HZ)
+
+// throttle rx/tx briefly after some faults, so khubd might disconnect()
+// us (it polls at HZ/4 usually) before we report too many false errors.
+#define THROTTLE_JIFFIES	(HZ/8)
+
+// between wakeups
+#define UNLINK_TIMEOUT_MS	3
+
+/*-------------------------------------------------------------------------*/
+
+static const char driver_name [] = "axusbnet";
+
+/* use ethtool to change the level for any given device */
+static int msg_level = -1;
+module_param (msg_level, int, 0);
+MODULE_PARM_DESC (msg_level, "Override default message level");
+
+/*-------------------------------------------------------------------------*/
+
+/* handles CDC Ethernet and many other network "bulk data" interfaces */
+static
+int axusbnet_get_endpoints(struct usbnet *dev, struct usb_interface *intf)
+{
+	int				tmp;
+	struct usb_host_interface	*alt = NULL;
+	struct usb_host_endpoint	*in = NULL, *out = NULL;
+	struct usb_host_endpoint	*status = NULL;
+
+	for (tmp = 0; tmp < intf->num_altsetting; tmp++) {
+		unsigned	ep;
+
+		in = out = status = NULL;
+		alt = intf->altsetting + tmp;
+
+		/* take the first altsetting with in-bulk + out-bulk;
+		 * remember any status endpoint, just in case;
+		 * ignore other endpoints and altsetttings.
+		 */
+		for (ep = 0; ep < alt->desc.bNumEndpoints; ep++) {
+			struct usb_host_endpoint	*e;
+			int				intr = 0;
+
+			e = alt->endpoint + ep;
+			switch (e->desc.bmAttributes) {
+			case USB_ENDPOINT_XFER_INT:
+				if (!(e->desc.bEndpointAddress & USB_DIR_IN))
+					continue;
+				intr = 1;
+				/* FALLTHROUGH */
+			case USB_ENDPOINT_XFER_BULK:
+				break;
+			default:
+				continue;
+			}
+			if (e->desc.bEndpointAddress & USB_DIR_IN) {
+				if (!intr && !in)
+					in = e;
+				else if (intr && !status)
+					status = e;
+			} else {
+				if (!out)
+					out = e;
+			}
+		}
+		if (in && out)
+			break;
+	}
+	if (!alt || !in || !out)
+		return -EINVAL;
+
+	if (alt->desc.bAlternateSetting != 0
+			|| !(dev->driver_info->flags & FLAG_NO_SETINT)) {
+		tmp = usb_set_interface (dev->udev, alt->desc.bInterfaceNumber,
+				alt->desc.bAlternateSetting);
+		if (tmp < 0)
+			return tmp;
+	}
+
+	dev->in = usb_rcvbulkpipe (dev->udev,
+			in->desc.bEndpointAddress & USB_ENDPOINT_NUMBER_MASK);
+	dev->out = usb_sndbulkpipe (dev->udev,
+			out->desc.bEndpointAddress & USB_ENDPOINT_NUMBER_MASK);
+	dev->status = status;
+	return 0;
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,19)
+static void intr_complete (struct urb *urb, struct pt_regs *regs);
+#else
+static void intr_complete (struct urb *urb);
+#endif
+
+static int init_status (struct usbnet *dev, struct usb_interface *intf)
+{
+	char		*buf = NULL;
+	unsigned	pipe = 0;
+	unsigned	maxp;
+	unsigned	period;
+
+	if (!dev->driver_info->status)
+		return 0;
+
+	pipe = usb_rcvintpipe (dev->udev,
+			dev->status->desc.bEndpointAddress
+				& USB_ENDPOINT_NUMBER_MASK);
+	maxp = usb_maxpacket (dev->udev, pipe, 0);
+
+	/* avoid 1 msec chatter:  min 8 msec poll rate */
+	period = max ((int) dev->status->desc.bInterval,
+		(dev->udev->speed == USB_SPEED_HIGH) ? 7 : 3);
+
+	buf = kmalloc (maxp, GFP_KERNEL);
+	if (buf) {
+		dev->interrupt = usb_alloc_urb (0, GFP_KERNEL);
+		if (!dev->interrupt) {
+			kfree (buf);
+			return -ENOMEM;
+		} else {
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,14)
+			dev->interrupt->transfer_flags |= URB_ASYNC_UNLINK;
+#endif
+			usb_fill_int_urb(dev->interrupt, dev->udev, pipe,
+				buf, maxp, intr_complete, dev, period);
+			devdbg(dev,
+				"status ep%din, %d bytes period %d",
+				usb_pipeendpoint(pipe), maxp, period);
+		}
+	}
+	return 0;
+}
+
+/* Passes this packet up the stack, updating its accounting.
+ * Some link protocols batch packets, so their rx_fixup paths
+ * can return clones as well as just modify the original skb.
+ */
+static
+void axusbnet_skb_return (struct usbnet *dev, struct sk_buff *skb)
+{
+	int	status;
+
+	skb->dev = dev->net;
+	skb->protocol = eth_type_trans (skb, dev->net);
+	dev->stats.rx_packets++;
+	dev->stats.rx_bytes += skb->len;
+
+	if (netif_msg_rx_status (dev))
+		devdbg (dev, "< rx, len %zu, type 0x%x",
+			skb->len + sizeof (struct ethhdr), skb->protocol);
+	memset (skb->cb, 0, sizeof (struct skb_data));
+	status = netif_rx (skb);
+	if (status != NET_RX_SUCCESS && netif_msg_rx_err (dev))
+		devdbg (dev, "netif_rx status %d", status);
+}
+
+/*-------------------------------------------------------------------------
+ *
+ * Network Device Driver (peer link to "Host Device", from USB host)
+ *
+ *-------------------------------------------------------------------------*/
+
+static
+int axusbnet_change_mtu (struct net_device *net, int new_mtu)
+{
+	struct usbnet	*dev = netdev_priv(net);
+	int		ll_mtu = new_mtu + net->hard_header_len;
+	int		old_hard_mtu = dev->hard_mtu;
+	int		old_rx_urb_size = dev->rx_urb_size;
+
+	if (new_mtu <= 0)
+		return -EINVAL;
+	// no second zero-length packet read wanted after mtu-sized packets
+	if ((ll_mtu % dev->maxpacket) == 0)
+		return -EDOM;
+	net->mtu = new_mtu;
+
+	dev->hard_mtu = net->mtu + net->hard_header_len;
+	if (dev->rx_urb_size == old_hard_mtu) {
+		dev->rx_urb_size = dev->hard_mtu;
+		if (dev->rx_urb_size > old_rx_urb_size)
+			axusbnet_unlink_rx_urbs(dev);
+	}
+
+	return 0;
+}
+
+static struct net_device_stats *axusbnet_get_stats (struct net_device *net)
+{
+	struct usbnet *dev = netdev_priv (net);
+	return &dev->stats;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* some LK 2.4 HCDs oopsed if we freed or resubmitted urbs from
+ * completion callbacks.  2.5 should have fixed those bugs...
+ */
+
+static void
+defer_bh(struct usbnet *dev, struct sk_buff *skb, struct sk_buff_head *list)
+{
+	unsigned long		flags;
+
+	spin_lock_irqsave(&list->lock, flags);
+	__skb_unlink(skb, list);
+	spin_unlock(&list->lock);
+	spin_lock(&dev->done.lock);
+	__skb_queue_tail(&dev->done, skb);
+	if (dev->done.qlen == 1)
+		tasklet_schedule(&dev->bh);
+	spin_unlock_irqrestore(&dev->done.lock, flags);
+}
+
+/* some work can't be done in tasklets, so we use keventd
+ *
+ * NOTE:  annoying asymmetry:  if it's active, schedule_work() fails,
+ * but tasklet_schedule() doesn't.  hope the failure is rare.
+ */
+static
+void axusbnet_defer_kevent (struct usbnet *dev, int work)
+{
+	set_bit (work, &dev->flags);
+	if (!schedule_work (&dev->kevent))
+		deverr (dev, "kevent %d may have been dropped", work);
+	else
+		devdbg (dev, "kevent %d scheduled", work);
+}
+
+/*-------------------------------------------------------------------------*/
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,19)
+static void rx_complete (struct urb *urb, struct pt_regs *regs);
+#else
+static void rx_complete (struct urb *urb);
+#endif
+
+static void rx_submit (struct usbnet *dev, struct urb *urb, gfp_t flags)
+{
+	struct sk_buff		*skb;
+	struct skb_data		*entry;
+	int			retval = 0;
+	unsigned long		lockflags;
+	size_t			size = dev->rx_urb_size;
+	struct driver_info	*info = dev->driver_info;
+	u8			align;
+
+#if (AX_FORCE_BUFF_ALIGN)
+	align = 0;
+#else
+	if (!(info->flags & FLAG_HW_IP_ALIGNMENT))
+		align = NET_IP_ALIGN;
+	else
+		align = 0;
+#endif
+
+	if ((skb = alloc_skb (size + align, flags)) == NULL) {
+
+		if (netif_msg_rx_err (dev))
+			devdbg (dev, "no rx skb");
+
+		if((dev->rx_urb_size > 2048) && dev->rx_size) {
+			dev->rx_size--;
+			dev->rx_urb_size = AX88772B_BULKIN_SIZE[dev->rx_size].size;
+
+			ax8817x_write_cmd_async (dev, 0x2A,
+				AX88772B_BULKIN_SIZE[dev->rx_size].byte_cnt,
+				AX88772B_BULKIN_SIZE[dev->rx_size].threshold,
+				0, NULL);
+		}
+
+		if (!(dev->flags & EVENT_RX_MEMORY))
+			axusbnet_defer_kevent (dev, EVENT_RX_MEMORY);
+		usb_free_urb (urb);
+		return;
+	}
+
+	if (align)
+		skb_reserve (skb, NET_IP_ALIGN);
+
+	entry = (struct skb_data *) skb->cb;
+	entry->urb = urb;
+	entry->dev = dev;
+	entry->state = rx_start;
+	entry->length = 0;
+
+	usb_fill_bulk_urb (urb, dev->udev, dev->in,
+		skb->data, size, rx_complete, skb);
+
+	spin_lock_irqsave (&dev->rxq.lock, lockflags);
+
+	if (netif_running (dev->net)
+			&& netif_device_present (dev->net)
+			&& !test_bit (EVENT_RX_HALT, &dev->flags)) {
+		switch (retval = usb_submit_urb (urb, GFP_ATOMIC)) {
+		case -EPIPE:
+			axusbnet_defer_kevent (dev, EVENT_RX_HALT);
+			break;
+		case -ENOMEM:
+			axusbnet_defer_kevent (dev, EVENT_RX_MEMORY);
+			break;
+		case -ENODEV:
+			if (netif_msg_ifdown (dev))
+				devdbg (dev, "device gone");
+			netif_device_detach (dev->net);
+			break;
+		default:
+			if (netif_msg_rx_err (dev))
+				devdbg (dev, "rx submit, %d", retval);
+			tasklet_schedule (&dev->bh);
+			break;
+		case 0:
+			__skb_queue_tail (&dev->rxq, skb);
+		}
+	} else {
+		if (netif_msg_ifdown (dev))
+			devdbg (dev, "rx: stopped");
+		retval = -ENOLINK;
+	}
+	spin_unlock_irqrestore (&dev->rxq.lock, lockflags);
+	if (retval) {
+		dev_kfree_skb_any (skb);
+		usb_free_urb (urb);
+	}
+}
+
+
+/*-------------------------------------------------------------------------*/
+
+static inline void rx_process (struct usbnet *dev, struct sk_buff *skb)
+{
+	if (dev->driver_info->rx_fixup
+			&& !dev->driver_info->rx_fixup (dev, skb))
+		goto error;
+	// else network stack removes extra byte if we forced a short packet
+
+	if (skb->len)
+		axusbnet_skb_return (dev, skb);
+	else {
+		if (netif_msg_rx_err (dev))
+			devdbg (dev, "drop");
+error:
+		dev->stats.rx_errors++;
+		skb_queue_tail (&dev->done, skb);
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,19)
+static void rx_complete (struct urb *urb, struct pt_regs *regs)
+#else
+static void rx_complete (struct urb *urb)
+#endif
+{
+	struct sk_buff		*skb = (struct sk_buff *) urb->context;
+	struct skb_data		*entry = (struct skb_data *) skb->cb;
+	struct usbnet		*dev = entry->dev;
+	int			urb_status = urb->status;
+
+	skb_put (skb, urb->actual_length);
+	entry->state = rx_done;
+	entry->urb = NULL;
+
+	switch (urb_status) {
+	/* success */
+	case 0:
+		if (skb->len < dev->net->hard_header_len) {
+			entry->state = rx_cleanup;
+			dev->stats.rx_errors++;
+			dev->stats.rx_length_errors++;
+			if (netif_msg_rx_err (dev))
+				devdbg (dev, "rx length %d", skb->len);
+		}
+		break;
+
+	/* stalls need manual reset. this is rare ... except that
+	 * when going through USB 2.0 TTs, unplug appears this way.
+	 * we avoid the highspeed version of the ETIMEDOUT/EILSEQ
+	 * storm, recovering as needed.
+	 */
+	case -EPIPE:
+		dev->stats.rx_errors++;
+		axusbnet_defer_kevent (dev, EVENT_RX_HALT);
+		// FALLTHROUGH
+
+	/* software-driven interface shutdown */
+	case -ECONNRESET:		/* async unlink */
+	case -ESHUTDOWN:		/* hardware gone */
+		if (netif_msg_ifdown (dev))
+			devdbg (dev, "rx shutdown, code %d", urb_status);
+		goto block;
+
+	/* we get controller i/o faults during khubd disconnect() delays.
+	 * throttle down resubmits, to avoid log floods; just temporarily,
+	 * so we still recover when the fault isn't a khubd delay.
+	 */
+	case -EPROTO:
+	case -ETIME:
+	case -EILSEQ:
+		dev->stats.rx_errors++;
+		if (!timer_pending (&dev->delay)) {
+			mod_timer (&dev->delay, jiffies + THROTTLE_JIFFIES);
+			if (netif_msg_link (dev))
+				devdbg (dev, "rx throttle %d", urb_status);
+		}
+block:
+		entry->state = rx_cleanup;
+		entry->urb = urb;
+		urb = NULL;
+		break;
+
+	/* data overrun ... flush fifo? */
+	case -EOVERFLOW:
+		dev->stats.rx_over_errors++;
+		// FALLTHROUGH
+
+	default:
+		entry->state = rx_cleanup;
+		dev->stats.rx_errors++;
+		if (netif_msg_rx_err (dev))
+			devdbg (dev, "rx status %d", urb_status);
+		break;
+	}
+
+	defer_bh(dev, skb, &dev->rxq);
+
+	if (urb) {
+		if (netif_running (dev->net)
+				&& !test_bit (EVENT_RX_HALT, &dev->flags)) {
+			rx_submit (dev, urb, GFP_ATOMIC);
+			return;
+		}
+		usb_free_urb (urb);
+	}
+	if (netif_msg_rx_err (dev))
+		devdbg (dev, "no read resubmitted");
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,19)
+static void intr_complete (struct urb *urb, struct pt_regs *regs)
+#else
+static void intr_complete (struct urb *urb)
+#endif
+{
+	struct usbnet	*dev = urb->context;
+	int		status = urb->status;
+
+	switch (status) {
+	/* success */
+	case 0:
+		dev->driver_info->status(dev, urb);
+		break;
+
+	/* software-driven interface shutdown */
+	case -ENOENT:		/* urb killed */
+	case -ESHUTDOWN:	/* hardware gone */
+		if (netif_msg_ifdown (dev))
+			devdbg (dev, "intr shutdown, code %d", status);
+		return;
+
+	/* NOTE:  not throttling like RX/TX, since this endpoint
+	 * already polls infrequently
+	 */
+	default:
+		devdbg (dev, "intr status %d", status);
+		break;
+	}
+
+	if (!netif_running (dev->net))
+		return;
+
+	memset(urb->transfer_buffer, 0, urb->transfer_buffer_length);
+	status = usb_submit_urb (urb, GFP_ATOMIC);
+	if (status != 0 && netif_msg_timer (dev))
+		deverr(dev, "intr resubmit --> %d", status);
+}
+
+/*-------------------------------------------------------------------------*/
+
+// unlink pending rx/tx; completion handlers do all other cleanup
+
+static int unlink_urbs (struct usbnet *dev, struct sk_buff_head *q)
+{
+	unsigned long		flags;
+	struct sk_buff		*skb, *skbnext;
+	int			count = 0;
+
+	spin_lock_irqsave (&q->lock, flags);
+	skb_queue_walk_safe(q, skb, skbnext) {
+		struct skb_data		*entry;
+		struct urb		*urb;
+		int			retval;
+
+		entry = (struct skb_data *) skb->cb;
+		urb = entry->urb;
+
+		// during some PM-driven resume scenarios,
+		// these (async) unlinks complete immediately
+		retval = usb_unlink_urb (urb);
+		if (retval != -EINPROGRESS && retval != 0)
+			devdbg (dev, "unlink urb err, %d", retval);
+		else
+			count++;
+	}
+	spin_unlock_irqrestore (&q->lock, flags);
+	return count;
+}
+
+// Flush all pending rx urbs
+// minidrivers may need to do this when the MTU changes
+
+static
+void axusbnet_unlink_rx_urbs(struct usbnet *dev)
+{
+	if (netif_running(dev->net)) {
+		(void) unlink_urbs (dev, &dev->rxq);
+		tasklet_schedule(&dev->bh);
+	}
+}
+
+/*-------------------------------------------------------------------------*/
+
+// precondition: never called in_interrupt
+
+static
+int axusbnet_stop (struct net_device *net)
+{
+	struct usbnet		*dev = netdev_priv(net);
+	struct driver_info	*info = dev->driver_info;
+	int			temp;
+	int			retval;
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,18)
+	DECLARE_WAIT_QUEUE_HEAD_ONSTACK (unlink_wakeup);
+#else
+	DECLARE_WAIT_QUEUE_HEAD (unlink_wakeup);
+#endif
+	DECLARE_WAITQUEUE (wait, current);
+
+	netif_stop_queue (net);
+
+	if (netif_msg_ifdown (dev))
+		devinfo (dev, "stop stats: rx/tx %ld/%ld, errs %ld/%ld",
+			dev->stats.rx_packets, dev->stats.tx_packets,
+			dev->stats.rx_errors, dev->stats.tx_errors
+			);
+
+	/* allow minidriver to stop correctly (wireless devices to turn off
+	 * radio etc) */
+	if (info->stop) {
+		retval = info->stop(dev);
+		if (retval < 0 && netif_msg_ifdown(dev))
+			devinfo(dev,
+				"stop fail (%d) usbnet usb-%s-%s, %s",
+				retval,
+				dev->udev->bus->bus_name, dev->udev->devpath,
+				info->description);
+	}
+
+	if (!(info->flags & FLAG_AVOID_UNLINK_URBS)) {
+		/* ensure there are no more active urbs */
+		add_wait_queue(&unlink_wakeup, &wait);
+		dev->wait = &unlink_wakeup;
+		temp = unlink_urbs(dev, &dev->txq) +
+			unlink_urbs(dev, &dev->rxq);
+
+		/* maybe wait for deletions to finish. */
+		while (!skb_queue_empty(&dev->rxq)
+				&& !skb_queue_empty(&dev->txq)
+				&& !skb_queue_empty(&dev->done)) {
+			msleep(UNLINK_TIMEOUT_MS);
+			if (netif_msg_ifdown(dev))
+				devdbg(dev, "waited for %d urb completions",
+					temp);
+		}
+		dev->wait = NULL;
+		remove_wait_queue(&unlink_wakeup, &wait);
+	}
+
+	usb_kill_urb(dev->interrupt);
+
+	/* deferred work (task, timer, softirq) must also stop.
+	 * can't flush_scheduled_work() until we drop rtnl (later),
+	 * else workers could deadlock; so make workers a NOP.
+	 */
+	dev->flags = 0;
+	del_timer_sync (&dev->delay);
+	tasklet_kill (&dev->bh);
+
+	return 0;
+}
+
+/*-------------------------------------------------------------------------*/
+
+// posts reads, and enables write queuing
+
+// precondition: never called in_interrupt
+
+static
+int axusbnet_open (struct net_device *net)
+{
+	struct usbnet		*dev = netdev_priv(net);
+	int			retval = 0;
+	struct driver_info	*info = dev->driver_info;
+
+	// put into "known safe" state
+	if (info->reset && (retval = info->reset (dev)) < 0) {
+		if (netif_msg_ifup (dev))
+			devinfo (dev,
+				"open reset fail (%d) usbnet usb-%s-%s, %s",
+				retval,
+				dev->udev->bus->bus_name, dev->udev->devpath,
+			info->description);
+		goto done;
+	}
+
+	// insist peer be connected
+	if (info->check_connect && (retval = info->check_connect (dev)) < 0) {
+		if (netif_msg_ifup (dev))
+			devdbg (dev, "can't open; %d", retval);
+		goto done;
+	}
+
+	/* start any status interrupt transfer */
+	if (dev->interrupt) {
+		retval = usb_submit_urb (dev->interrupt, GFP_KERNEL);
+		if (retval < 0) {
+			if (netif_msg_ifup (dev))
+				deverr (dev, "intr submit %d", retval);
+			goto done;
+		}
+	}
+
+	netif_start_queue (net);
+	if (netif_msg_ifup (dev)) {
+		char	*framing;
+
+		if (dev->driver_info->flags & FLAG_FRAMING_NC)
+			framing = "NetChip";
+		else if (dev->driver_info->flags & FLAG_FRAMING_GL)
+			framing = "GeneSys";
+		else if (dev->driver_info->flags & FLAG_FRAMING_Z)
+			framing = "Zaurus";
+		else if (dev->driver_info->flags & FLAG_FRAMING_RN)
+			framing = "RNDIS";
+		else if (dev->driver_info->flags & FLAG_FRAMING_AX)
+			framing = "ASIX";
+		else
+			framing = "simple";
+
+		devinfo (dev, "open: enable queueing "
+				"(rx %d, tx %d) mtu %d %s framing",
+			(int)RX_QLEN (dev), (int)TX_QLEN (dev), dev->net->mtu,
+			framing);
+	}
+
+	// delay posting reads until we're fully open
+	tasklet_schedule (&dev->bh);
+	return retval;
+done:
+	return retval;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* ethtool methods; minidrivers may need to add some more, but
+ * they'll probably want to use this base set.
+ */
+
+static
+int axusbnet_get_settings (struct net_device *net, struct ethtool_cmd *cmd)
+{
+	struct usbnet *dev = netdev_priv(net);
+
+	if (!dev->mii.mdio_read)
+		return -EOPNOTSUPP;
+
+	return mii_ethtool_gset(&dev->mii, cmd);
+}
+
+static
+int axusbnet_set_settings (struct net_device *net, struct ethtool_cmd *cmd)
+{
+	struct usbnet *dev = netdev_priv(net);
+	int retval;
+
+	if (!dev->mii.mdio_write)
+		return -EOPNOTSUPP;
+
+	retval = mii_ethtool_sset(&dev->mii, cmd);
+
+	/* link speed/duplex might have changed */
+	if (dev->driver_info->link_reset)
+		dev->driver_info->link_reset(dev);
+
+	return retval;
+
+}
+
+static
+u32 axusbnet_get_link (struct net_device *net)
+{
+	struct usbnet *dev = netdev_priv(net);
+
+	/* If a check_connect is defined, return its result */
+	if (dev->driver_info->check_connect)
+		return dev->driver_info->check_connect (dev) == 0;
+
+	/* if the device has mii operations, use those */
+	if (dev->mii.mdio_read)
+		return mii_link_ok(&dev->mii);
+
+	/* Otherwise, dtrt for drivers calling netif_carrier_{on,off} */
+	return ethtool_op_get_link(net);
+}
+
+static
+int axusbnet_nway_reset(struct net_device *net)
+{
+	struct usbnet *dev = netdev_priv(net);
+
+	if (!dev->mii.mdio_write)
+		return -EOPNOTSUPP;
+
+	return mii_nway_restart(&dev->mii);
+}
+
+static
+void axusbnet_get_drvinfo (struct net_device *net, struct ethtool_drvinfo *info)
+{
+	struct usbnet *dev = netdev_priv(net);
+
+	strncpy (info->driver, dev->driver_name, sizeof info->driver);
+	strncpy (info->version, DRIVER_VERSION, sizeof info->version);
+	strncpy (info->fw_version, dev->driver_info->description,
+		sizeof info->fw_version);
+	usb_make_path (dev->udev, info->bus_info, sizeof info->bus_info);
+}
+
+static
+u32 axusbnet_get_msglevel (struct net_device *net)
+{
+	struct usbnet *dev = netdev_priv(net);
+
+	return dev->msg_enable;
+}
+
+static
+void axusbnet_set_msglevel (struct net_device *net, u32 level)
+{
+	struct usbnet *dev = netdev_priv(net);
+
+	dev->msg_enable = level;
+}
+
+/* drivers may override default ethtool_ops in their bind() routine */
+static struct ethtool_ops axusbnet_ethtool_ops = {
+	.get_settings		= axusbnet_get_settings,
+	.set_settings		= axusbnet_set_settings,
+	.get_link		= axusbnet_get_link,
+	.nway_reset		= axusbnet_nway_reset,
+	.get_drvinfo		= axusbnet_get_drvinfo,
+	.get_msglevel		= axusbnet_get_msglevel,
+	.set_msglevel		= axusbnet_set_msglevel,
+};
+
+/*-------------------------------------------------------------------------*/
+
+/* work that cannot be done in interrupt context uses keventd.
+ *
+ * NOTE:  with 2.5 we could do more of this using completion callbacks,
+ * especially now that control transfers can be queued.
+ */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void kevent (void *data)
+{
+	struct usbnet *dev = (struct usbnet *)data;
+#else
+static void kevent (struct work_struct *work)
+{
+	struct usbnet		*dev =
+		container_of(work, struct usbnet, kevent);
+#endif
+	int			status;
+
+	/* usb_clear_halt() needs a thread context */
+	if (test_bit (EVENT_TX_HALT, &dev->flags)) {
+
+		unlink_urbs (dev, &dev->txq);
+		status = usb_clear_halt (dev->udev, dev->out);
+		if (status < 0
+				&& status != -EPIPE
+				&& status != -ESHUTDOWN) {
+			if (netif_msg_tx_err (dev))
+				deverr (dev, "can't clear tx halt, status %d",
+					status);
+		} else {
+			clear_bit (EVENT_TX_HALT, &dev->flags);
+			if (status != -ESHUTDOWN)
+				netif_wake_queue (dev->net);
+		}
+	}
+	if (test_bit (EVENT_RX_HALT, &dev->flags)) {
+
+		unlink_urbs (dev, &dev->rxq);
+		status = usb_clear_halt (dev->udev, dev->in);
+		if (status < 0
+				&& status != -EPIPE
+				&& status != -ESHUTDOWN) {
+			if (netif_msg_rx_err (dev))
+				deverr (dev, "can't clear rx halt, status %d",
+					status);
+		} else {
+			clear_bit (EVENT_RX_HALT, &dev->flags);
+			tasklet_schedule (&dev->bh);
+		}
+	}
+
+	/* tasklet could resubmit itself forever if memory is tight */
+	if (test_bit (EVENT_RX_MEMORY, &dev->flags)) {
+		struct urb	*urb = NULL;
+
+		if (netif_running (dev->net))
+			urb = usb_alloc_urb (0, GFP_KERNEL);
+		else
+			clear_bit (EVENT_RX_MEMORY, &dev->flags);
+		if (urb != NULL) {
+			clear_bit (EVENT_RX_MEMORY, &dev->flags);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,14)
+			urb->transfer_flags |= URB_ASYNC_UNLINK;
+#endif
+			rx_submit (dev, urb, GFP_KERNEL);
+			tasklet_schedule (&dev->bh);
+		}
+	}
+
+	if (test_bit (EVENT_LINK_RESET, &dev->flags)) {
+		struct driver_info	*info = dev->driver_info;
+		int			retval = 0;
+
+		clear_bit (EVENT_LINK_RESET, &dev->flags);
+		if(info->link_reset && (retval = info->link_reset(dev)) < 0) {
+			devinfo(dev, "link reset failed (%d) usbnet usb-%s-%s, %s",
+				retval,
+				dev->udev->bus->bus_name, dev->udev->devpath,
+				info->description);
+		}
+	}
+
+	if (dev->flags)
+		devdbg (dev, "kevent done, flags = 0x%lx",
+			dev->flags);
+}
+
+/*-------------------------------------------------------------------------*/
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,19)
+static void tx_complete (struct urb *urb, struct pt_regs *regs)
+#else
+static void tx_complete (struct urb *urb)
+#endif
+{
+	struct sk_buff		*skb = (struct sk_buff *) urb->context;
+	struct skb_data		*entry = (struct skb_data *) skb->cb;
+	struct usbnet		*dev = entry->dev;
+
+	if (urb->status == 0) {
+		dev->stats.tx_packets++;
+		dev->stats.tx_bytes += entry->length;
+	} else {
+		dev->stats.tx_errors++;
+
+		switch (urb->status) {
+		case -EPIPE:
+			axusbnet_defer_kevent (dev, EVENT_TX_HALT);
+			break;
+
+		/* software-driven interface shutdown */
+		case -ECONNRESET:		// async unlink
+		case -ESHUTDOWN:		// hardware gone
+			break;
+
+		// like rx, tx gets controller i/o faults during khubd delays
+		// and so it uses the same throttling mechanism.
+		case -EPROTO:
+		case -ETIME:
+		case -EILSEQ:
+			if (!timer_pending (&dev->delay)) {
+				mod_timer (&dev->delay,
+					jiffies + THROTTLE_JIFFIES);
+				if (netif_msg_link (dev))
+					devdbg (dev, "tx throttle %d",
+							urb->status);
+			}
+			netif_stop_queue (dev->net);
+			break;
+		default:
+			if (netif_msg_tx_err (dev))
+				devdbg (dev, "tx err %d", entry->urb->status);
+			break;
+		}
+	}
+
+	urb->dev = NULL;
+	entry->state = tx_done;
+	defer_bh(dev, skb, &dev->txq);
+}
+
+/*-------------------------------------------------------------------------*/
+
+static
+void axusbnet_tx_timeout (struct net_device *net)
+{
+	struct usbnet		*dev = netdev_priv(net);
+
+	unlink_urbs (dev, &dev->txq);
+	tasklet_schedule (&dev->bh);
+
+	// FIXME: device recovery -- reset?
+}
+
+/*-------------------------------------------------------------------------*/
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,32)
+static int
+#else
+static netdev_tx_t
+#endif
+axusbnet_start_xmit (struct sk_buff *skb,
+				     struct net_device *net)
+{
+	struct usbnet		*dev = netdev_priv(net);
+	int			length;
+	struct urb		*urb = NULL;
+	struct skb_data		*entry;
+	struct driver_info	*info = dev->driver_info;
+	unsigned long		flags;
+	int retval;
+
+	// some devices want funky USB-level framing, for
+	// win32 driver (usually) and/or hardware quirks
+	if (info->tx_fixup) {
+		skb = info->tx_fixup (dev, skb, GFP_ATOMIC);
+		if (!skb) {
+			if (netif_msg_tx_err (dev))
+				devdbg (dev, "can't tx_fixup skb");
+			goto drop;
+		}
+	}
+	length = skb->len;
+
+	if (!(urb = usb_alloc_urb (0, GFP_ATOMIC))) {
+		if (netif_msg_tx_err (dev))
+			devdbg (dev, "no urb");
+		goto drop;
+	}
+
+	entry = (struct skb_data *) skb->cb;
+	entry->urb = urb;
+	entry->dev = dev;
+	entry->state = tx_start;
+	entry->length = length;
+
+	usb_fill_bulk_urb (urb, dev->udev, dev->out,
+			skb->data, skb->len, tx_complete, skb);
+
+	/* don't assume the hardware handles USB_ZERO_PACKET
+	 * NOTE:  strictly conforming cdc-ether devices should expect
+	 * the ZLP here, but ignore the one-byte packet.
+	 */
+	if (!(info->flags & FLAG_SEND_ZLP) && (length % dev->maxpacket) == 0) {
+		urb->transfer_buffer_length++;
+		if (skb_tailroom(skb)) {
+			skb->data[skb->len] = 0;
+			__skb_put(skb, 1);
+		}
+	}
+
+	spin_lock_irqsave (&dev->txq.lock, flags);
+
+	switch ((retval = usb_submit_urb (urb, GFP_ATOMIC))) {
+	case -EPIPE:
+		netif_stop_queue (net);
+		axusbnet_defer_kevent (dev, EVENT_TX_HALT);
+		break;
+	default:
+		if (netif_msg_tx_err (dev))
+			devdbg (dev, "tx: submit urb err %d", retval);
+		break;
+	case 0:
+		net->trans_start = jiffies;
+		__skb_queue_tail (&dev->txq, skb);
+		if (dev->txq.qlen >= TX_QLEN (dev))
+			netif_stop_queue (net);
+	}
+	spin_unlock_irqrestore (&dev->txq.lock, flags);
+
+	if (retval) {
+		if (netif_msg_tx_err (dev))
+			devdbg (dev, "drop, code %d", retval);
+drop:
+		dev->stats.tx_dropped++;
+		if (skb)
+			dev_kfree_skb_any (skb);
+		usb_free_urb (urb);
+	} else if (netif_msg_tx_queued (dev)) {
+		devdbg (dev, "> tx, len %d, type 0x%x",
+			length, skb->protocol);
+	}
+	return NETDEV_TX_OK;
+}
+
+/*-------------------------------------------------------------------------*/
+
+// tasklet (work deferred from completions, in_irq) or timer
+
+static void axusbnet_bh (unsigned long param)
+{
+	struct usbnet		*dev = (struct usbnet *) param;
+	struct sk_buff		*skb;
+	struct skb_data		*entry;
+
+	while ((skb = skb_dequeue (&dev->done))) {
+		entry = (struct skb_data *) skb->cb;
+		switch (entry->state) {
+		case rx_done:
+			entry->state = rx_cleanup;
+			rx_process (dev, skb);
+			continue;
+		case tx_done:
+		case rx_cleanup:
+			usb_free_urb (entry->urb);
+			dev_kfree_skb (skb);
+			continue;
+		default:
+			devdbg (dev, "bogus skb state %d", entry->state);
+		}
+	}
+
+	// waiting for all pending urbs to complete?
+	if (dev->wait) {
+		if ((dev->txq.qlen + dev->rxq.qlen + dev->done.qlen) == 0) {
+			wake_up (dev->wait);
+		}
+
+	// or are we maybe short a few urbs?
+	} else if (netif_running (dev->net)
+			&& netif_device_present (dev->net)
+			&& !timer_pending (&dev->delay)
+			&& !test_bit (EVENT_RX_HALT, &dev->flags)) {
+		int	temp = dev->rxq.qlen;
+		int	qlen = RX_QLEN (dev);
+
+		if (temp < qlen) {
+			struct urb	*urb;
+			int		i;
+
+			// don't refill the queue all at once
+			for (i = 0; i < 10 && dev->rxq.qlen < qlen; i++) {
+				urb = usb_alloc_urb (0, GFP_ATOMIC);
+				if (urb != NULL) {
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,14)
+					urb->transfer_flags |= URB_ASYNC_UNLINK;
+#endif
+					rx_submit (dev, urb, GFP_ATOMIC);
+				}
+			}
+			if (temp != dev->rxq.qlen && netif_msg_link (dev))
+				devdbg (dev, "rxqlen %d --> %d",
+						temp, dev->rxq.qlen);
+			if (dev->rxq.qlen < qlen)
+				tasklet_schedule (&dev->bh);
+		}
+		if (dev->txq.qlen < TX_QLEN (dev))
+			netif_wake_queue (dev->net);
+	}
+}
+
+
+/*-------------------------------------------------------------------------
+ *
+ * USB Device Driver support
+ *
+ *-------------------------------------------------------------------------*/
+
+// precondition: never called in_interrupt
+
+static
+void axusbnet_disconnect (struct usb_interface *intf)
+{
+	struct usbnet		*dev;
+	struct usb_device	*xdev;
+	struct net_device	*net;
+
+	dev = usb_get_intfdata(intf);
+	usb_set_intfdata(intf, NULL);
+	if (!dev)
+		return;
+
+	xdev = interface_to_usbdev (intf);
+
+	if (netif_msg_probe (dev))
+		devinfo (dev, "unregister '%s' usb-%s-%s, %s",
+			intf->dev.driver->name,
+			xdev->bus->bus_name, xdev->devpath,
+			dev->driver_info->description);
+
+	net = dev->net;
+	unregister_netdev (net);
+
+	/* we don't hold rtnl here ... */
+	flush_scheduled_work ();
+
+	if (dev->driver_info->unbind)
+		dev->driver_info->unbind (dev, intf);
+
+	free_netdev(net);
+	usb_put_dev (xdev);
+}
+
+/*-------------------------------------------------------------------------*/
+
+// precondition: never called in_interrupt
+
+static int
+axusbnet_probe (struct usb_interface *udev, const struct usb_device_id *prod)
+{
+	struct usbnet			*dev;
+	struct net_device		*net;
+	struct usb_host_interface	*interface;
+	struct driver_info		*info;
+	struct usb_device		*xdev;
+	int				status;
+	const char			*name;
+
+	name = udev->dev.driver->name;
+	info = (struct driver_info *) prod->driver_info;
+	if (!info) {
+		printk (KERN_ERR "blacklisted by %s\n", name);
+		return -ENODEV;
+	}
+	xdev = interface_to_usbdev (udev);
+	interface = udev->cur_altsetting;
+
+	usb_get_dev (xdev);
+
+	status = -ENOMEM;
+
+	// set up our own records
+	net = alloc_etherdev(sizeof(*dev));
+	if (!net) {
+		dbg ("can't kmalloc dev");
+		goto out;
+	}
+
+	dev = netdev_priv(net);
+	dev->udev = xdev;
+	dev->intf = udev;
+	dev->driver_info = info;
+	dev->driver_name = name;
+	dev->msg_enable = netif_msg_init (msg_level, NETIF_MSG_DRV
+				| NETIF_MSG_PROBE | NETIF_MSG_LINK);
+	skb_queue_head_init (&dev->rxq);
+	skb_queue_head_init (&dev->txq);
+	skb_queue_head_init (&dev->done);
+	dev->bh.func = axusbnet_bh;
+	dev->bh.data = (unsigned long) dev;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	INIT_WORK (&dev->kevent, kevent, dev);
+#else
+	INIT_WORK (&dev->kevent, kevent);
+#endif
+
+	dev->delay.function = axusbnet_bh;
+	dev->delay.data = (unsigned long) dev;
+	init_timer (&dev->delay);
+//	mutex_init (&dev->phy_mutex);
+
+	dev->net = net;
+
+	/* rx and tx sides can use different message sizes;
+	 * bind() should set rx_urb_size in that case.
+	 */
+	dev->hard_mtu = net->mtu + net->hard_header_len;
+
+#if 0
+// dma_supported() is deeply broken on almost all architectures
+	// possible with some EHCI controllers
+	if (dma_supported (&udev->dev, DMA_BIT_MASK(64)))
+		net->features |= NETIF_F_HIGHDMA;
+#endif
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,30)
+	net->open		= axusbnet_open,
+	net->stop		= axusbnet_stop,
+	net->hard_start_xmit	= axusbnet_start_xmit,
+	net->tx_timeout	= axusbnet_tx_timeout,
+	net->get_stats = axusbnet_get_stats;
+#endif
+
+	net->watchdog_timeo = TX_TIMEOUT_JIFFIES;
+	net->ethtool_ops = &axusbnet_ethtool_ops;
+
+	// allow device-specific bind/init procedures
+	// NOTE net->name still not usable ...
+	status = info->bind (dev, udev);
+	if (status < 0) {
+		deverr(dev, "Binding device failed: %d", status);
+		goto out1;
+	}
+
+	/* maybe the remote can't receive an Ethernet MTU */
+	if (net->mtu > (dev->hard_mtu - net->hard_header_len))
+		net->mtu = dev->hard_mtu - net->hard_header_len;
+
+	status = init_status (dev, udev);
+	if (status < 0)
+		goto out3;
+
+	if (!dev->rx_urb_size)
+		dev->rx_urb_size = dev->hard_mtu;
+	dev->maxpacket = usb_maxpacket (dev->udev, dev->out, 1);
+
+	SET_NETDEV_DEV(net, &udev->dev);
+	status = register_netdev (net);
+	if (status) {
+		deverr(dev, "net device registration failed: %d", status);
+		goto out3;
+	}
+
+	if (netif_msg_probe (dev))
+		devinfo (dev, "register '%s' at usb-%s-%s, %s, %pM",
+			udev->dev.driver->name,
+			xdev->bus->bus_name, xdev->devpath,
+			dev->driver_info->description,
+			net->dev_addr);
+
+	// ok, it's ready to go.
+	usb_set_intfdata (udev, dev);
+
+	// start as if the link is up
+	netif_device_attach (net);
+
+	return 0;
+
+out3:
+	if (info->unbind)
+		info->unbind (dev, udev);
+out1:
+	free_netdev(net);
+out:
+	usb_put_dev(xdev);
+	return status;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/*
+ * suspend the whole driver as soon as the first interface is suspended
+ * resume only when the last interface is resumed
+ */
+
+static int axusbnet_suspend (struct usb_interface *intf,
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,10)
+pm_message_t message)
+#else
+u32 message)
+#endif
+{
+	struct usbnet		*dev = usb_get_intfdata(intf);
+
+	if (!dev->suspend_count++) {
+		/*
+		 * accelerate emptying of the rx and queues, to avoid
+		 * having everything error out.
+		 */
+		netif_device_detach (dev->net);
+		(void) unlink_urbs (dev, &dev->rxq);
+		(void) unlink_urbs (dev, &dev->txq);
+		/*
+		 * reattach so runtime management can use and
+		 * wake the device
+		 */
+		netif_device_attach (dev->net);
+	}
+	return 0;
+}
+
+static int
+axusbnet_resume (struct usb_interface *intf)
+{
+	struct usbnet		*dev = usb_get_intfdata(intf);
+
+	if (!--dev->suspend_count)
+		tasklet_schedule (&dev->bh);
+
+	return 0;
+}
+
diff --git a/drivers/net/usb/axusbnet.h b/drivers/net/usb/axusbnet.h
new file mode 100644
index 000000000000..d492de3b80e1
--- /dev/null
+++ b/drivers/net/usb/axusbnet.h
@@ -0,0 +1,208 @@
+/*
+ * USB Networking Link Interface
+ *
+ * Copyright (C) 2000-2005 by David Brownell <dbrownell@users.sourceforge.net>
+ * Copyright (C) 2003-2005 David Hollis <dhollis@davehollis.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * 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	__LINUX_USB_USBNET_H
+#define	__LINUX_USB_USBNET_H
+
+#ifndef gfp_t
+#define gfp_t int
+#endif
+
+/* interface from usbnet core to each USB networking link we handle */
+struct usbnet {
+	/* housekeeping */
+	struct usb_device	*udev;
+	struct usb_interface	*intf;
+	struct driver_info	*driver_info;
+	const char		*driver_name;
+	void			*driver_priv;
+	wait_queue_head_t	*wait;
+//	struct mutex		phy_mutex;
+	unsigned char		suspend_count;
+
+	/* i/o info: pipes etc */
+	unsigned		in, out;
+	struct usb_host_endpoint *status;
+	unsigned		maxpacket;
+	struct timer_list	delay;
+
+	/* protocol/interface state */
+	struct net_device	*net;
+	struct net_device_stats stats;
+	int			msg_enable;
+	unsigned long		data [5];
+	u32			xid;
+	u32			hard_mtu;	/* count any extra framing */
+	size_t			rx_urb_size;	/* size for rx urbs */
+	struct mii_if_info	mii;
+
+	/* various kinds of pending driver work */
+	struct sk_buff_head	rxq;
+	struct sk_buff_head	txq;
+	struct sk_buff_head	done;
+	struct sk_buff_head	rxq_pause;
+	struct urb		*interrupt;
+	struct tasklet_struct	bh;
+
+	struct work_struct	kevent;
+	unsigned long		flags;
+#		define EVENT_TX_HALT	0
+#		define EVENT_RX_HALT	1
+#		define EVENT_RX_MEMORY	2
+#		define EVENT_STS_SPLIT	3
+#		define EVENT_LINK_RESET	4
+#		define EVENT_RX_PAUSED	5
+
+	void			*priv;	/* point to minidriver private data */
+	unsigned char		rx_size;
+};
+
+static inline struct usb_driver *driver_of(struct usb_interface *intf)
+{
+	return to_usb_driver(intf->dev.driver);
+}
+
+/* interface from the device/framing level "minidriver" to core */
+struct driver_info {
+	char		*description;
+
+	int		flags;
+/* framing is CDC Ethernet, not writing ZLPs (hw issues), or optionally: */
+#define FLAG_FRAMING_NC	0x0001		/* guard against device dropouts */
+#define FLAG_FRAMING_GL	0x0002		/* genelink batches packets */
+#define FLAG_FRAMING_Z	0x0004		/* zaurus adds a trailer */
+#define FLAG_FRAMING_RN	0x0008		/* RNDIS batches, plus huge header */
+
+#define FLAG_NO_SETINT	0x0010		/* device can't set_interface() */
+#define FLAG_ETHER	0x0020		/* maybe use "eth%d" names */
+
+#define FLAG_FRAMING_AX 0x0040		/* AX88772/178 packets */
+#define FLAG_WLAN	0x0080		/* use "wlan%d" names */
+#define FLAG_AVOID_UNLINK_URBS	0x0100	/* don't unlink urbs at usbnet_stop() */
+#define FLAG_SEND_ZLP	0x0200		/* hw requires ZLPs are sent */
+#define FLAG_HW_IP_ALIGNMENT	0x0400	/* AX88772B support hardware IP alignment */
+
+
+	/* init device ... can sleep, or cause probe() failure */
+	int	(*bind)(struct usbnet *, struct usb_interface *);
+
+	/* cleanup device ... can sleep, but can't fail */
+	void	(*unbind)(struct usbnet *, struct usb_interface *);
+
+	/* reset device ... can sleep */
+	int	(*reset)(struct usbnet *);
+
+	/* stop device ... can sleep */
+	int	(*stop)(struct usbnet *);
+
+	/* see if peer is connected ... can sleep */
+	int	(*check_connect)(struct usbnet *);
+
+	/* for status polling */
+	void	(*status)(struct usbnet *, struct urb *);
+
+	/* link reset handling, called from defer_kevent */
+	int	(*link_reset)(struct usbnet *);
+
+	/* fixup rx packet (strip framing) */
+	int	(*rx_fixup)(struct usbnet *dev, struct sk_buff *skb);
+
+	/* fixup tx packet (add framing) */
+	struct sk_buff	*(*tx_fixup)(struct usbnet *dev,
+				struct sk_buff *skb, gfp_t flags);
+
+	/* early initialization code, can sleep. This is for minidrivers
+	 * having 'subminidrivers' that need to do extra initialization
+	 * right after minidriver have initialized hardware. */
+	int	(*early_init)(struct usbnet *dev);
+
+	/* called by minidriver when receiving indication */
+	void	(*indication)(struct usbnet *dev, void *ind, int indlen);
+
+	/* for new devices, use the descriptor-reading code instead */
+	int		in;		/* rx endpoint */
+	int		out;		/* tx endpoint */
+
+	unsigned long	data;		/* Misc driver specific data */
+};
+
+/* Drivers that reuse some of the standard USB CDC infrastructure
+ * (notably, using multiple interfaces according to the CDC
+ * union descriptor) get some helper code.
+ */
+struct cdc_state {
+	struct usb_cdc_header_desc	*header;
+	struct usb_cdc_union_desc	*u;
+	struct usb_cdc_ether_desc	*ether;
+	struct usb_interface		*control;
+	struct usb_interface		*data;
+};
+
+/* CDC and RNDIS support the same host-chosen packet filters for IN transfers */
+#define	DEFAULT_FILTER	(USB_CDC_PACKET_TYPE_BROADCAST \
+			|USB_CDC_PACKET_TYPE_ALL_MULTICAST \
+			|USB_CDC_PACKET_TYPE_PROMISCUOUS \
+			|USB_CDC_PACKET_TYPE_DIRECTED)
+
+
+/* we record the state for each of our queued skbs */
+enum skb_state {
+	illegal = 0,
+	tx_start, tx_done,
+	rx_start, rx_done, rx_cleanup
+};
+
+struct skb_data {	/* skb->cb is one of these */
+	struct urb		*urb;
+	struct usbnet		*dev;
+	enum skb_state		state;
+	size_t			length;
+};
+
+#ifndef skb_queue_walk_safe
+#define skb_queue_walk_safe(queue, skb, tmp)				\
+			for (skb = (queue)->next, tmp = skb->next;	\
+			skb != (struct sk_buff *)(queue);		\
+			skb = tmp, tmp = skb->next)
+#endif
+
+/* messaging support includes the interface name, so it must not be
+ * used before it has one ... notably, in minidriver bind() calls.
+ */
+#ifdef DEBUG
+#define devdbg(usbnet, fmt, arg...) \
+	printk("%s: " fmt "\n" , (usbnet)->net->name , ## arg)
+#else
+#define devdbg(usbnet, fmt, arg...) \
+	({ if (0) printk("%s: " fmt "\n" , (usbnet)->net->name , \
+		## arg); 0; })
+#endif
+
+#define deverr(usbnet, fmt, arg...) \
+	printk(KERN_ERR "%s: " fmt "\n" , (usbnet)->net->name , ## arg)
+#define devwarn(usbnet, fmt, arg...) \
+	printk(KERN_WARNING "%s: " fmt "\n" , (usbnet)->net->name , ## arg)
+
+#define devinfo(usbnet, fmt, arg...) \
+	printk(KERN_INFO "%s: " fmt "\n" , (usbnet)->net->name , ## arg); \
+
+
+#endif /* __LINUX_USB_USBNET_H */
diff --git a/drivers/net/usb/cdc_ether.c b/drivers/net/usb/cdc_ether.c
index c924ea2bce07..14bda2ecabde 100644
--- a/drivers/net/usb/cdc_ether.c
+++ b/drivers/net/usb/cdc_ether.c
@@ -469,6 +469,15 @@ static const struct driver_info wwan_info = {
 	.manage_power =	cdc_manage_power,
 };
 
+static const struct driver_info rmnet_info = {
+	.description =	"Mobile Broadband Network Device",
+	.flags =	FLAG_RMNET,
+	.bind =		usbnet_cdc_bind,
+	.unbind =	usbnet_cdc_unbind,
+	.status =	usbnet_cdc_status,
+	.manage_power =	cdc_manage_power,
+};
+
 /*-------------------------------------------------------------------------*/
 
 #define HUAWEI_VENDOR_ID	0x12D1
@@ -570,6 +579,39 @@ static const struct usb_device_id	products [] = {
 	.driver_info = (unsigned long)&wwan_info,
 },
 
+/* PH450 */
+{
+	.match_flags = USB_DEVICE_ID_MATCH_INT_INFO
+		| USB_DEVICE_ID_MATCH_DEVICE,
+	USB_DEVICE(0x1983, 0x0310),
+	.driver_info = (unsigned long)&rmnet_info,
+}, {
+	.match_flags = USB_DEVICE_ID_MATCH_INT_INFO
+		| USB_DEVICE_ID_MATCH_DEVICE,
+	USB_DEVICE(0x1983, 0x0321),
+	.driver_info = (unsigned long)&rmnet_info,
+}, {
+	.match_flags = USB_DEVICE_ID_MATCH_INT_INFO
+		| USB_DEVICE_ID_MATCH_DEVICE,
+	USB_DEVICE(0x1983, 0x0327),	/* 5AE */
+	.driver_info = (unsigned long)&rmnet_info,
+},
+
+/* Tango module */
+{
+	.match_flags = USB_DEVICE_ID_MATCH_INT_INFO
+		 | USB_DEVICE_ID_MATCH_DEVICE,
+	USB_DEVICE(0x0489,0xE03A),
+	.driver_info = (unsigned long)&rmnet_info,
+},
+
+/* ZM5250 */
+{
+	.match_flags = USB_DEVICE_ID_MATCH_INT_INFO
+		 | USB_DEVICE_ID_MATCH_DEVICE,
+	USB_DEVICE(0x19D2,0x1554),
+	.driver_info = (unsigned long)&rmnet_info,
+},
 /*
  * WHITELIST!!!
  *
diff --git a/drivers/net/usb/cdc_ncm.c b/drivers/net/usb/cdc_ncm.c
index f06fb78383a1..df82afdf62ce 100644
--- a/drivers/net/usb/cdc_ncm.c
+++ b/drivers/net/usb/cdc_ncm.c
@@ -137,11 +137,21 @@ struct cdc_ncm_ctx {
 
 static void cdc_ncm_tx_timeout(unsigned long arg);
 static const struct driver_info cdc_ncm_info;
+static const struct driver_info cdc_mbm_info;
 static struct usb_driver cdc_ncm_driver;
 static struct ethtool_ops cdc_ncm_ethtool_ops;
 
 static const struct usb_device_id cdc_devs[] = {
-	{ USB_INTERFACE_INFO(USB_CLASS_COMM,
+	{
+		/* Ericsson f5521gw */
+		.match_flags = USB_DEVICE_ID_MATCH_INT_INFO
+			| USB_DEVICE_ID_MATCH_DEVICE,
+		USB_DEVICE(0x0BDB,0x190D),
+		.driver_info = (unsigned long)&cdc_mbm_info,
+	},
+	{
+		/* Standard NCM class device */
+		USB_INTERFACE_INFO(USB_CLASS_COMM,
 		USB_CDC_SUBCLASS_NCM, USB_CDC_PROTO_NONE),
 		.driver_info = (unsigned long)&cdc_ncm_info,
 	},
@@ -1211,6 +1221,18 @@ static const struct driver_info cdc_ncm_info = {
 	.tx_fixup = cdc_ncm_tx_fixup,
 };
 
+static const struct driver_info cdc_mbm_info = {
+	.description = "CDC NCM",
+	.flags = FLAG_RMNET | FLAG_NO_SETINT | FLAG_MULTI_PACKET,
+	.bind = cdc_ncm_bind,
+	.unbind = cdc_ncm_unbind,
+	.check_connect = cdc_ncm_check_connect,
+	.manage_power = cdc_ncm_manage_power,
+	.status = cdc_ncm_status,
+	.rx_fixup = cdc_ncm_rx_fixup,
+	.tx_fixup = cdc_ncm_tx_fixup,
+};
+
 static struct usb_driver cdc_ncm_driver = {
 	.name = "cdc_ncm",
 	.id_table = cdc_devs,
diff --git a/drivers/net/usb/cdc_subset.c b/drivers/net/usb/cdc_subset.c
index fc5f13d47ad9..47eac7b92372 100644
--- a/drivers/net/usb/cdc_subset.c
+++ b/drivers/net/usb/cdc_subset.c
@@ -222,13 +222,8 @@ static const struct driver_info	blob_info = {
 
 #endif	/* CONFIG_USB_ARMLINUX */
 
-
 /*-------------------------------------------------------------------------*/
 
-#ifndef	HAVE_HARDWARE
-#warning You need to configure some hardware for this driver
-#endif
-
 /*
  * chip vendor names won't normally be on the cables, and
  * may not be on the device.
diff --git a/drivers/net/usb/raw_ip_net.c b/drivers/net/usb/raw_ip_net.c
new file mode 100644
index 000000000000..dc9b5e61fd9e
--- /dev/null
+++ b/drivers/net/usb/raw_ip_net.c
@@ -0,0 +1,1190 @@
+/*
+ * raw_ip_net.c
+ *
+ * USB network driver for RAW-IP modems.
+ *
+ * Copyright (c) 2011-2012, NVIDIA 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/errno.h>
+#include <linux/types.h>
+#include <linux/etherdevice.h>
+#include <linux/usb.h>
+
+#define BASEBAND_USB_NET_DEV_NAME		"rmnet%d"
+
+/* ethernet packet ethertype for IP packets */
+#define NET_IP_ETHERTYPE		0x08, 0x00
+
+#define	TX_TIMEOUT		10
+
+#ifndef USB_NET_BUFSIZ
+#define USB_NET_BUFSIZ				8192
+#endif  /* USB_NET_BUFSIZ */
+
+/* maximum interface number supported */
+#define MAX_INTFS	5
+
+MODULE_LICENSE("GPL");
+
+static int g_i;
+
+/* To support more rmnet interfaces, increase the default max_intfs or
+ * pass kernel module parameter.
+ * e.g. insmod raw_ip_net.ko max_intfs=5
+ */
+static int max_intfs = 2;	/* default number of interfaces */
+
+static unsigned long usb_net_raw_ip_intf[MAX_INTFS] = { 3, 5, 9, 11, 13};
+unsigned long usb_net_raw_ip_rx_debug;
+unsigned long usb_net_raw_ip_tx_debug;
+
+module_param(max_intfs, int, 0644);
+MODULE_PARM_DESC(max_intfs, "usb net (raw-ip) - max. interfaces supported");
+module_param(usb_net_raw_ip_rx_debug, ulong, 0644);
+MODULE_PARM_DESC(usb_net_raw_ip_rx_debug, "usb net (raw-ip) - rx debug");
+module_param(usb_net_raw_ip_tx_debug, ulong, 0644);
+MODULE_PARM_DESC(usb_net_raw_ip_tx_debug, "usb net (raw-ip) - tx debug");
+
+struct baseband_usb {
+	/* semaphore between disconnect/suspend/resume */
+	struct semaphore sem;
+	/* instance */
+	int baseband_index;
+	/* network statistics */
+	struct net_device_stats stats;
+	/* usb context */
+	struct {
+		struct usb_driver *driver;
+		struct usb_device *device;
+		struct usb_interface *interface;
+		struct {
+			struct {
+				unsigned int in;
+				unsigned int out;
+			} isoch, bulk, interrupt;
+		} pipe;
+		/* currently active rx urb */
+		struct urb *rx_urb;
+		/* currently active tx urb */
+		struct urb *tx_urb;
+		struct usb_anchor tx_urb_deferred;
+		struct workqueue_struct *tx_workqueue;
+		struct work_struct tx_work;
+	} usb;
+	/* re-usable rx urb */
+	struct urb *urb_r;
+	void *buff;
+	/* suspend count */
+	int susp_count;
+};
+
+static struct baseband_usb *baseband_usb_net[MAX_INTFS] = { 0, 0, 0, 0, 0};
+
+static struct net_device *usb_net_raw_ip_dev[MAX_INTFS] = { 0, 0, 0, 0, 0};
+
+static struct usb_interface *g_usb_interface[MAX_INTFS];
+
+static int usb_net_raw_ip_rx_urb_submit(struct baseband_usb *usb);
+static void usb_net_raw_ip_rx_urb_comp(struct urb *urb);
+
+static int usb_net_raw_ip_tx_urb_submit(struct baseband_usb *usb,
+	struct sk_buff *skb);
+static void usb_net_raw_ip_tx_urb_work(struct work_struct *work);
+static void usb_net_raw_ip_tx_urb_comp(struct urb *urb);
+
+static int baseband_usb_driver_probe(struct usb_interface *intf,
+	const struct usb_device_id *id)
+{
+	int i = g_i, j;
+
+	pr_debug("%s(%d) { intf %p id %p\n", __func__, __LINE__, intf, id);
+
+	pr_debug("i %d\n", i);
+
+	pr_debug("intf->cur_altsetting->desc.bInterfaceNumber %02x\n",
+		intf->cur_altsetting->desc.bInterfaceNumber);
+	pr_debug("intf->cur_altsetting->desc.bAlternateSetting %02x\n",
+		intf->cur_altsetting->desc.bAlternateSetting);
+	pr_debug("intf->cur_altsetting->desc.bNumEndpoints %02x\n",
+		intf->cur_altsetting->desc.bNumEndpoints);
+	pr_debug("intf->cur_altsetting->desc.bInterfaceClass %02x\n",
+		intf->cur_altsetting->desc.bInterfaceClass);
+	pr_debug("intf->cur_altsetting->desc.bInterfaceSubClass %02x\n",
+		intf->cur_altsetting->desc.bInterfaceSubClass);
+	pr_debug("intf->cur_altsetting->desc.bInterfaceProtocol %02x\n",
+		intf->cur_altsetting->desc.bInterfaceProtocol);
+	pr_debug("intf->cur_altsetting->desc.iInterface %02x\n",
+		intf->cur_altsetting->desc.iInterface);
+
+	/* register interfaces that are assigned to raw-ip */
+	for (j = 0; j < max_intfs; j++) {
+		if (usb_net_raw_ip_intf[j] ==
+				intf->cur_altsetting->desc.bInterfaceNumber) {
+			pr_info("%s: raw_ip using interface %d\n", __func__,
+				intf->cur_altsetting->desc.bInterfaceNumber);
+			g_usb_interface[j] = intf;
+			return 0;
+		}
+	}
+	pr_debug("%s(%d) }\n", __func__, __LINE__);
+	return -ENODEV;
+}
+
+static void baseband_usb_driver_disconnect(struct usb_interface *intf)
+{
+	int i;
+	struct urb *urb;
+
+	pr_debug("%s intf %p\n", __func__, intf);
+
+	for (i = 0; i < max_intfs; i++) {
+		pr_debug("[%d]\n", i);
+		if (!baseband_usb_net[i])
+			continue;
+		if (baseband_usb_net[i]->usb.interface != intf) {
+			pr_debug("%p != %p\n",
+				baseband_usb_net[i]->usb.interface, intf);
+			continue;
+		}
+		/* acquire semaphore */
+		if (down_interruptible(&baseband_usb_net[i]->sem)) {
+			pr_err("%s: cannot acquire semaphore\n", __func__);
+			continue;
+		}
+		/* kill usb tx */
+		while ((urb = usb_get_from_anchor(&baseband_usb_net[i]->
+			usb.tx_urb_deferred)) != (struct urb *) 0) {
+			pr_info("%s: kill deferred tx urb %p\n",
+				__func__, urb);
+			/* decrement count from usb_get_from_anchor() */
+			usb_free_urb(urb);
+			/* kill tx urb */
+			usb_kill_urb(urb);
+			/* free tx urb + tx urb transfer buffer */
+			if (urb->transfer_buffer) {
+				kfree(urb->transfer_buffer);
+				urb->transfer_buffer = (void *) 0;
+			}
+			usb_free_urb(urb);
+		}
+		if (baseband_usb_net[i]->usb.tx_workqueue) {
+			flush_workqueue(baseband_usb_net[i]
+				->usb.tx_workqueue);
+		}
+		if (baseband_usb_net[i]->usb.tx_urb) {
+			usb_kill_urb(baseband_usb_net[i]->usb.tx_urb);
+			baseband_usb_net[i]->usb.tx_urb
+				= (struct urb *) 0;
+		}
+		/* kill usb rx */
+		if (baseband_usb_net[i]->usb.rx_urb) {
+			usb_kill_urb(baseband_usb_net[i]->usb.rx_urb);
+			baseband_usb_net[i]->usb.rx_urb
+				= (struct urb *) 0;
+		}
+		/* mark interface as disconnected */
+		baseband_usb_net[i]->usb.interface
+			= (struct usb_interface *) 0;
+		/* release semaphore */
+		up(&baseband_usb_net[i]->sem);
+	}
+
+}
+
+#ifdef CONFIG_PM
+static int baseband_usb_driver_suspend(struct usb_interface *intf,
+	pm_message_t message)
+{
+	int i, susp_count;
+
+	pr_debug("%s intf %p\n", __func__, intf);
+
+	pr_debug("%s: cnt %d intf=%p &intf->dev=%p kobj=%s\n",
+			__func__, atomic_read(&intf->dev.power.usage_count),
+			intf, &intf->dev, kobject_name(&intf->dev.kobj));
+
+	for (i = 0; i < max_intfs; i++) {
+		pr_debug("[%d]\n", i);
+		if (!baseband_usb_net[i])
+			continue;
+		if (baseband_usb_net[i]->usb.interface != intf) {
+			pr_debug("%p != %p\n",
+				baseband_usb_net[i]->usb.interface, intf);
+			continue;
+		}
+		/* increment suspend count */
+		susp_count = (baseband_usb_net[i]->susp_count)++;
+		if (susp_count > 0) {
+			pr_debug("%s: susp_count %d > 0 (already suspended)\n",
+				__func__, susp_count);
+			continue;
+		}
+		if (susp_count < 0) {
+			pr_debug("%s: susp_count %d < 0 (ILLEGAL VALUE)\n",
+				__func__, susp_count);
+			baseband_usb_net[i]->susp_count = 0;
+			continue;
+		}
+		pr_debug("%s: susp_count = %d (suspending...)\n",
+			__func__, susp_count);
+		/* acquire semaphore */
+		if (down_interruptible(&baseband_usb_net[i]->sem)) {
+			pr_err("%s: cannot acquire semaphore\n", __func__);
+			continue;
+		}
+		/* kill usb rx */
+		if (!baseband_usb_net[i]->usb.rx_urb) {
+			pr_debug("rx_usb already killed\n");
+			up(&baseband_usb_net[i]->sem);
+			continue;
+		}
+		pr_debug("%s: kill rx_urb {\n",__func__);
+		usb_kill_urb(baseband_usb_net[i]->usb.rx_urb);
+		pr_debug("%s: kill rx_urb }\n",__func__);
+		baseband_usb_net[i]->usb.rx_urb = (struct urb *) 0;
+		/* cancel tx urb work (will restart after resume) */
+		if (!baseband_usb_net[i]->usb.tx_workqueue) {
+			pr_err("%s: !tx_workqueue\n", __func__);
+			up(&baseband_usb_net[i]->sem);
+			continue;
+		}
+		pr_debug("%s: cancel_work_sync {\n",__func__);
+		cancel_work_sync(&baseband_usb_net[i]->usb.tx_work);
+		pr_debug("%s: cancel_work_sync }\n",__func__);
+		/* release semaphore */
+		up(&baseband_usb_net[i]->sem);
+	}
+
+	return 0;
+}
+
+static int baseband_usb_driver_resume(struct usb_interface *intf)
+{
+	int i, err, susp_count;
+
+	pr_debug("%s intf %p\n", __func__, intf);
+
+	pr_debug("%s: cnt %d intf=%p &intf->dev=%p kobj=%s\n",
+			__func__, atomic_read(&intf->dev.power.usage_count),
+			intf, &intf->dev, kobject_name(&intf->dev.kobj));
+
+	for (i = 0; i < max_intfs; i++) {
+		pr_debug("[%d]\n", i);
+		if (!baseband_usb_net[i])
+			continue;
+		if (baseband_usb_net[i]->usb.interface != intf) {
+			pr_debug("%p != %p\n",
+				baseband_usb_net[i]->usb.interface, intf);
+			continue;
+		}
+		/* decrement suspend count */
+		susp_count = --(baseband_usb_net[i]->susp_count);
+		if (susp_count > 0) {
+			pr_debug("%s: susp_count %d > 0 (not resuming yet)\n",
+				__func__, susp_count);
+			continue;
+		}
+		if (susp_count < 0) {
+			pr_debug("%s: susp_count %d < 0 (ILLEGAL VALUE)\n",
+				__func__, susp_count);
+			baseband_usb_net[i]->susp_count = 0;
+			continue;
+		}
+		pr_debug("%s: susp_count = %d (resuming...)\n",
+			__func__, susp_count);
+		/* acquire semaphore */
+		if (down_interruptible(&baseband_usb_net[i]->sem)) {
+			pr_err("%s: cannot acquire semaphore\n", __func__);
+			continue;
+		}
+		/* start usb rx */
+		if (baseband_usb_net[i]->usb.rx_urb) {
+			pr_debug("rx_usb already exists\n");
+			up(&baseband_usb_net[i]->sem);
+			continue;
+		}
+		err = usb_net_raw_ip_rx_urb_submit(baseband_usb_net[i]);
+		if (err < 0) {
+			pr_err("submit rx failed - err %d\n", err);
+			up(&baseband_usb_net[i]->sem);
+			continue;
+		}
+		/* restart tx urb work (cancelled in suspend) */
+		if (!baseband_usb_net[i]->usb.tx_workqueue) {
+			pr_err("%s: !tx_workqueue\n", __func__);
+			up(&baseband_usb_net[i]->sem);
+			continue;
+		}
+		queue_work(baseband_usb_net[i]->usb.tx_workqueue,
+			&baseband_usb_net[i]->usb.tx_work);
+		/* release semaphore */
+		up(&baseband_usb_net[i]->sem);
+	}
+
+	return 0;
+}
+static int baseband_usb_driver_reset_resume(struct usb_interface *intf)
+{
+	pr_debug("%s intf %p\n", __func__, intf);
+	return baseband_usb_driver_resume(intf);
+}
+#endif /* CONFIG_PM */
+
+static struct usb_device_id baseband_usb_driver_id_table[] = {
+	/* xmm modem vid, pid */
+	{ USB_DEVICE(0x1519, 0x0020), },
+	{ },
+};
+
+static struct usb_driver baseband_usb_driver = {
+		.name = "bb_raw_ip_net",
+		.probe = baseband_usb_driver_probe,
+		.disconnect = baseband_usb_driver_disconnect,
+		.id_table = baseband_usb_driver_id_table,
+#ifdef CONFIG_PM
+		.suspend = baseband_usb_driver_suspend,
+		.resume = baseband_usb_driver_resume,
+		.reset_resume = baseband_usb_driver_reset_resume,
+		.supports_autosuspend = 1,
+#endif
+};
+
+MODULE_DEVICE_TABLE(usb, baseband_usb_driver_id_table);
+
+static void find_usb_pipe(struct baseband_usb *usb)
+{
+	struct usb_device *usbdev = usb->usb.device;
+	struct usb_interface *intf = usb->usb.interface;
+	unsigned char numendpoint = intf->cur_altsetting->desc.bNumEndpoints;
+	struct usb_host_endpoint *endpoint = intf->cur_altsetting->endpoint;
+	unsigned char n;
+
+	for (n = 0; n < numendpoint; n++) {
+		if (usb_endpoint_is_isoc_in(&endpoint[n].desc)) {
+			pr_debug("endpoint[%d] isochronous in\n", n);
+			usb->usb.pipe.isoch.in = usb_rcvisocpipe(usbdev,
+				endpoint[n].desc.bEndpointAddress);
+		} else if (usb_endpoint_is_isoc_out(&endpoint[n].desc)) {
+			pr_debug("endpoint[%d] isochronous out\n", n);
+			usb->usb.pipe.isoch.out = usb_sndisocpipe(usbdev,
+				endpoint[n].desc.bEndpointAddress);
+		} else if (usb_endpoint_is_bulk_in(&endpoint[n].desc)) {
+			pr_debug("endpoint[%d] bulk in\n", n);
+			usb->usb.pipe.bulk.in = usb_rcvbulkpipe(usbdev,
+				endpoint[n].desc.bEndpointAddress);
+		} else if (usb_endpoint_is_bulk_out(&endpoint[n].desc)) {
+			pr_debug("endpoint[%d] bulk out\n", n);
+			usb->usb.pipe.bulk.out = usb_sndbulkpipe(usbdev,
+				endpoint[n].desc.bEndpointAddress);
+		} else if (usb_endpoint_is_int_in(&endpoint[n].desc)) {
+			pr_debug("endpoint[%d] interrupt in\n", n);
+			usb->usb.pipe.interrupt.in = usb_rcvintpipe(usbdev,
+				endpoint[n].desc.bEndpointAddress);
+		} else if (usb_endpoint_is_int_out(&endpoint[n].desc)) {
+			pr_debug("endpoint[%d] interrupt out\n", n);
+			usb->usb.pipe.interrupt.out = usb_sndintpipe(usbdev,
+				endpoint[n].desc.bEndpointAddress);
+		} else {
+			pr_debug("endpoint[%d] skipped\n", n);
+		}
+	}
+}
+
+void baseband_usb_close(struct baseband_usb *usb);
+
+struct baseband_usb *baseband_usb_open(int index, unsigned int intf)
+{
+	struct baseband_usb *usb;
+	int i;
+
+	pr_debug("baseband_usb_open {\n");
+
+	/* allocate baseband usb structure */
+	usb = kzalloc(sizeof(struct baseband_usb),
+		GFP_KERNEL);
+	if (!usb)
+		return (struct baseband_usb *) 0;
+
+	/* create semaphores */
+	sema_init(&usb->sem, 1);
+
+	/* open usb interface */
+	usb->baseband_index = index;
+	usb->usb.driver = &baseband_usb_driver;
+	if (!g_usb_interface[index]) {
+		/* wait for usb probe */
+		for (i = 0; i < 50; i++)
+			if (!g_usb_interface[index])
+				msleep(20);
+		if (!g_usb_interface[index]) {
+			pr_err("can't open usb: !g_usb_interface[%d]\n", index);
+			kfree(usb);
+			return NULL;
+		}
+	}
+	usb->usb.device = interface_to_usbdev(g_usb_interface[index]);
+	usb->usb.interface = g_usb_interface[index];
+	find_usb_pipe(usb);
+	usb->usb.rx_urb = (struct urb *) 0;
+	usb->usb.tx_urb = (struct urb *) 0;
+	g_usb_interface[index] = (struct usb_interface *) 0;
+	pr_debug("usb->usb.driver->name %s\n", usb->usb.driver->name);
+	pr_debug("usb->usb.device %p\n", usb->usb.device);
+	pr_debug("usb->usb.interface %p\n", usb->usb.interface);
+	pr_debug("usb->usb.pipe.isoch.in %x\n", usb->usb.pipe.isoch.in);
+	pr_debug("usb->usb.pipe.isoch.out %x\n", usb->usb.pipe.isoch.out);
+	pr_debug("usb->usb.pipe.bulk.in %x\n", usb->usb.pipe.bulk.in);
+	pr_debug("usb->usb.pipe.bulk.out %x\n", usb->usb.pipe.bulk.out);
+	pr_debug("usb->usb.pipe.interrupt.in %x\n", usb->usb.pipe.interrupt.in);
+	pr_debug("usb->usb.pipe.interrupt.out %x\n",
+		usb->usb.pipe.interrupt.out);
+
+	pr_debug("baseband_usb_open }\n");
+	return usb;
+}
+
+void baseband_usb_close(struct baseband_usb *usb)
+{
+	pr_debug("baseband_usb_close {\n");
+
+	/* check input */
+	if (!usb)
+		return;
+
+	/* close usb driver */
+	usb->usb.driver = (struct usb_driver *) 0;
+
+	/* destroy semaphores */
+	memset(&usb->sem, 0, sizeof(usb->sem));
+
+	/* free baseband usb structure */
+	kfree(usb);
+
+	pr_debug("baseband_usb_close }\n");
+}
+
+static int baseband_usb_netdev_init(struct net_device *dev)
+{
+	pr_debug("baseband_usb_netdev_init\n");
+	return 0;
+}
+
+static void baseband_usb_netdev_uninit(struct net_device *dev)
+{
+	pr_debug("baseband_usb_netdev_uninit\n");
+}
+
+static int baseband_usb_netdev_open(struct net_device *dev)
+{
+	pr_debug("baseband_usb_netdev_open\n");
+	netif_start_queue(dev);
+	return 0;
+}
+
+static int baseband_usb_netdev_stop(struct net_device *dev)
+{
+	pr_debug("baseband_usb_netdev_stop\n");
+	netif_stop_queue(dev);
+	return 0;
+}
+
+static netdev_tx_t baseband_usb_netdev_start_xmit(
+	struct sk_buff *skb, struct net_device *dev)
+{
+	int i;
+	struct baseband_usb *usb;
+	int err;
+
+	pr_debug("baseband_usb_netdev_start_xmit\n");
+
+	/* check input */
+	if (!skb) {
+		pr_err("no skb\n");
+		return NETDEV_TX_BUSY;
+	}
+	if (!dev) {
+		pr_err("no net dev\n");
+		return NETDEV_TX_BUSY;
+	}
+
+	/* find index of network device which is transmitting */
+	for (i = 0; i < max_intfs; i++) {
+		if (usb_net_raw_ip_dev[i] == dev)
+			break;
+	}
+	if (i >= max_intfs) {
+		pr_err("unknown net dev %p\n", dev);
+		return NETDEV_TX_BUSY;
+	}
+	usb = baseband_usb_net[i];
+
+	/* autoresume if suspended */
+	if (usb->usb.interface) {
+		usb_autopm_get_interface_async(usb->usb.interface);
+	} else {
+		pr_err("%s: tx get interface error\n", __func__);
+		netif_stop_queue(dev);
+		usb->stats.tx_errors++;
+		return NETDEV_TX_BUSY;
+	}
+
+	/* submit tx urb */
+	err = usb_net_raw_ip_tx_urb_submit(usb, skb);
+	if (err < 0) {
+		pr_err("%s: tx urb submit error\n", __func__);
+		netif_stop_queue(dev);
+		usb->stats.tx_errors++;
+		return NETDEV_TX_BUSY;
+	}
+
+	return NETDEV_TX_OK;
+}
+
+static struct net_device_stats *baseband_usb_netdev_get_stats(
+				struct net_device *dev)
+{
+	int i;
+	for (i = 0; i < max_intfs; i++) {
+		if (dev == usb_net_raw_ip_dev[i]) {
+			pr_debug("%s idx(%d)\n", __func__, i);
+			return &baseband_usb_net[i]->stats;
+		}
+	}
+	pr_debug("%s mismatch dev, default idx(0)\n", __func__);
+	return &baseband_usb_net[0]->stats;
+}
+
+static struct net_device_ops usb_net_raw_ip_ops = {
+	.ndo_init =		baseband_usb_netdev_init,
+	.ndo_uninit =		baseband_usb_netdev_uninit,
+	.ndo_open =		baseband_usb_netdev_open,
+	.ndo_stop =		baseband_usb_netdev_stop,
+	.ndo_start_xmit =	baseband_usb_netdev_start_xmit,
+	.ndo_get_stats = baseband_usb_netdev_get_stats,
+};
+
+static int usb_net_raw_ip_rx_urb_submit(struct baseband_usb *usb)
+{
+	struct urb *urb;
+	void *buf;
+	int err;
+
+	pr_debug("usb_net_raw_ip_rx_urb_submit { usb %p\n", usb);
+
+	/* check input */
+	if (!usb) {
+		pr_err("%s: !usb\n", __func__);
+		return -EINVAL;
+	}
+	if (!usb->usb.interface) {
+		pr_err("usb interface disconnected - not submitting rx urb\n");
+		return -EINVAL;
+	}
+	if (usb->usb.rx_urb) {
+		pr_err("previous urb still active\n");
+		return -EBUSY;
+	}
+	if (!usb->urb_r || !usb->buff) {
+		pr_err("no reusable rx urb found\n");
+		return -ENOMEM;
+	}
+
+	/* reuse rx urb */
+	urb = usb->urb_r;
+	buf = usb->buff;
+	usb_fill_bulk_urb(urb, usb->usb.device, usb->usb.pipe.bulk.in,
+		buf, USB_NET_BUFSIZ,
+		usb_net_raw_ip_rx_urb_comp,
+		usb);
+	urb->transfer_flags = 0;
+
+	/* submit rx urb */
+	usb_mark_last_busy(usb->usb.device);
+	usb->usb.rx_urb = urb;
+	err = usb_submit_urb(urb, GFP_ATOMIC);
+	if (err < 0) {
+		pr_err("usb_submit_urb() failed - err %d\n", err);
+		usb->usb.rx_urb = (struct urb *) 0;
+		return err;
+	}
+
+	pr_debug("usb_net_raw_ip_rx_urb_submit }\n");
+	return err;
+}
+
+static void usb_net_raw_ip_rx_urb_comp(struct urb *urb)
+{
+	struct baseband_usb *usb;
+	int i;
+	struct sk_buff *skb;
+	unsigned char *dst;
+	unsigned char ethernet_header[14] = {
+		/* Destination MAC */
+		0x00, 0x00,
+		0x00, 0x00,
+		0x00, 0x00,
+		/* Source MAC */
+		0x00, 0x00,
+		0x00, 0x00,
+		0x00, 0x00,
+		/* EtherType */
+		NET_IP_ETHERTYPE,
+	};
+
+	pr_debug("usb_net_raw_ip_rx_urb_comp { urb %p\n", urb);
+
+	/* check input */
+	if (!urb) {
+		pr_err("no urb\n");
+		return;
+	}
+	usb = (struct baseband_usb *)urb->context;
+	i = usb->baseband_index;
+	switch (urb->status) {
+	case 0:
+		break;
+	case -ESHUTDOWN:
+		/* fall through */
+		pr_info("%s: rx urb %p - link shutdown %d\n",
+			__func__, urb, urb->status);
+		goto err_exit;
+	case -EPROTO:
+		pr_info("%s: rx urb %p - link shutdown %d EPROTO\n",
+			__func__, urb, urb->status);
+		goto err_exit;
+	default:
+		pr_info("%s: rx urb %p - status %d\n",
+			__func__, urb, urb->status);
+		break;
+	}
+
+	/* put rx urb data in rx buffer */
+	if (urb->actual_length > 0) {
+		pr_debug("usb_net_raw_ip_rx_urb_comp - "
+			"urb->actual_length %d\n", urb->actual_length);
+		/* allocate skb with space for
+		 * - dummy ethernet header
+		 * - rx IP packet from modem
+		 */
+		skb = netdev_alloc_skb(usb_net_raw_ip_dev[i],
+			NET_IP_ALIGN + 14 + urb->actual_length);
+		if (skb) {
+			/* generate a dummy ethernet header
+			 * since modem sends IP packets without
+			 * any ethernet headers
+			 */
+			memcpy(ethernet_header + 0,
+				usb_net_raw_ip_dev[i]->dev_addr, 6);
+			memcpy(ethernet_header + 6,
+				"0x01\0x02\0x03\0x04\0x05\0x06", 6);
+			/* fill skb with
+			 * - dummy ethernet header
+			 * - rx IP packet from modem
+			 */
+			skb_reserve(skb, NET_IP_ALIGN);
+			dst = skb_put(skb, 14);
+			memcpy(dst, ethernet_header, 14);
+			if ((((unsigned char *) urb->transfer_buffer)[0]
+				& 0xf0) == 0x60) {
+				/* ipv6 ether type */
+				dst[12] = 0x86;
+				dst[13] = 0xdd;
+			}
+			dst = skb_put(skb, urb->actual_length);
+			memcpy(dst, urb->transfer_buffer, urb->actual_length);
+			skb->protocol = eth_type_trans(skb,
+				usb_net_raw_ip_dev[i]);
+			pr_debug("%s: ntohs(skb->protocol) %04x (%s)\n",
+				__func__, ntohs(skb->protocol),
+				(ntohs(skb->protocol) == 0x0800)
+					? "IPv4"
+					: (ntohs(skb->protocol) == 0x86dd)
+					? "IPv6"
+					: "unknown");
+			pr_debug("%s: %02x %02x %02x %02x\n", __func__,
+				((unsigned char *)urb->transfer_buffer)[0],
+				((unsigned char *)urb->transfer_buffer)[1],
+				((unsigned char *)urb->transfer_buffer)[2],
+				((unsigned char *)urb->transfer_buffer)[3]);
+			/* pass skb to network stack */
+			if (netif_rx(skb) < 0) {
+				pr_err("usb_net_raw_ip_rx_urb_comp_work - "
+					"netif_rx(%p) failed\n", skb);
+				kfree_skb(skb);
+				usb->stats.rx_errors++;
+			} else {
+				usb->stats.rx_packets++;
+				usb->stats.rx_bytes +=
+				    (14 + urb->actual_length);
+			}
+		} else {
+			pr_err("usb_net_raw_ip_rx_urb_comp_work - "
+				"netdev_alloc_skb() failed\n");
+		}
+	}
+
+	/* mark rx urb complete */
+	usb->usb.rx_urb = (struct urb *) 0;
+
+	/* do not submit urb if interface is suspending */
+	if (urb->status == -ENOENT)
+		return;
+
+	/* submit next rx urb */
+	usb_net_raw_ip_rx_urb_submit(usb);
+	return;
+
+err_exit:
+	/* mark rx urb complete */
+	usb->usb.rx_urb = (struct urb *) 0;
+
+	pr_debug("usb_net_raw_ip_rx_urb_comp }\n");
+	return;
+}
+
+static int usb_net_raw_ip_setup_rx_urb( struct baseband_usb *usb)
+{
+	pr_debug("usb_net_raw_ip_setup_rx_urb {\n");
+
+	/* check input */
+	if (!usb) {
+		pr_err("%s: !usb\n", __func__);
+		return -EINVAL;
+	}
+	if (usb->urb_r) {
+		pr_err("%s: reusable rx urb already allocated\n", __func__);
+		return -EINVAL;
+	}
+
+	/* allocate reusable rx urb */
+	usb->urb_r = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!usb->urb_r) {
+		pr_err("usb_alloc_urb() failed\n");
+		return -ENOMEM;
+	}
+	usb->buff = kzalloc(USB_NET_BUFSIZ, GFP_ATOMIC);
+	if (!usb->buff) {
+		pr_err("usb buffer kzalloc() failed\n");
+		usb_free_urb(usb->urb_r);
+		usb->urb_r = (struct urb *) 0;
+		return -ENOMEM;
+	}
+
+	pr_debug("usb_net_raw_setup_ip_rx_urb }\n");
+	return 0;
+}
+
+static void usb_net_raw_ip_free_rx_urb(struct baseband_usb *usb)
+{
+	pr_debug("usb_net_raw_ip_free_rx_urb {\n");
+
+	/* check input */
+	if (!usb) {
+		pr_err("%s: !usb\n", __func__);
+		return;
+	}
+
+	/* free reusable rx urb */
+	if (usb->urb_r) {
+		usb_free_urb(usb->urb_r);
+		usb->urb_r = (struct urb *) 0;
+	}
+	if (usb->buff) {
+		kfree(usb->buff);
+		usb->buff = (void *) 0;
+	}
+
+	pr_debug("usb_net_raw_ip_free_rx_urb }\n");
+}
+
+static int usb_net_raw_ip_tx_urb_submit(struct baseband_usb *usb,
+	struct sk_buff *skb)
+{
+	struct urb *urb;
+	unsigned char *buf;
+	int err;
+
+	pr_debug("usb_net_raw_ip_tx_urb_submit {\n");
+
+	/* check input */
+	if (!usb) {
+		pr_err("%s: !usb\n", __func__);
+		return -EINVAL;
+	}
+	if (!usb->usb.interface) {
+		pr_err("usb interface disconnected - not submitting tx urb\n");
+		return -EINVAL;
+	}
+	if (!skb) {
+		pr_err("%s: !skb\n", __func__);
+		usb_autopm_put_interface_async(usb->usb.interface);
+		return -EINVAL;
+	}
+
+	/* allocate urb */
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!urb) {
+		pr_err("usb_alloc_urb() failed\n");
+		usb_autopm_put_interface_async(usb->usb.interface);
+		return -ENOMEM;
+	}
+	buf = kzalloc(skb->len - 14, GFP_ATOMIC);
+	if (!buf) {
+		pr_err("usb buffer kzalloc() failed\n");
+		usb_free_urb(urb);
+		usb_autopm_put_interface_async(usb->usb.interface);
+		return -ENOMEM;
+	}
+	err = skb_copy_bits(skb, 14, buf, skb->len - 14);
+	if (err < 0) {
+		pr_err("skb_copy_bits() failed - %d\n", err);
+		kfree(buf);
+		usb_free_urb(urb);
+		usb_autopm_put_interface_async(usb->usb.interface);
+		return err;
+	}
+	usb_fill_bulk_urb(urb, usb->usb.device, usb->usb.pipe.bulk.out,
+		buf, skb->len - 14,
+		usb_net_raw_ip_tx_urb_comp,
+		usb);
+	urb->transfer_flags = URB_ZERO_PACKET;
+	pr_debug("%s: ntohs(skb->protocol) %04x (%s)\n",
+		__func__, ntohs(skb->protocol),
+		(ntohs(skb->protocol) == 0x0800)
+			? "IPv4"
+			: (ntohs(skb->protocol) == 0x86dd)
+			? "IPv6"
+			: "unknown");
+	pr_debug("%s: %02x %02x %02x %02x\n", __func__,
+		((unsigned char *)urb->transfer_buffer)[0],
+		((unsigned char *)urb->transfer_buffer)[1],
+		((unsigned char *)urb->transfer_buffer)[2],
+		((unsigned char *)urb->transfer_buffer)[3]);
+
+	/* queue tx urb work */
+	usb_anchor_urb(urb, &usb->usb.tx_urb_deferred);
+	queue_work(usb->usb.tx_workqueue, &usb->usb.tx_work);
+
+	/* free skb */
+	consume_skb(skb);
+
+	pr_debug("usb_net_raw_ip_tx_urb_submit }\n");
+	return 0;
+}
+
+static void usb_net_raw_ip_tx_urb_work(struct work_struct *work)
+{
+	struct baseband_usb *usb
+		= container_of(work, struct baseband_usb, usb.tx_work);
+	struct urb *urb;
+	int err;
+
+	pr_debug("usb_net_raw_ip_tx_urb_work {\n");
+
+	/* check if tx urb(s) queued */
+	if (usb == NULL ||
+		(!usb->usb.tx_urb &&
+		usb_anchor_empty(&usb->usb.tx_urb_deferred))) {
+		pr_debug("%s: nothing to do!\n", __func__);
+		return;
+	}
+
+	/* check if usb interface disconnected */
+	if (!usb->usb.interface) {
+		pr_err("%s: not submitting tx urb -interface disconnected\n",
+			__func__);
+		return;
+	}
+
+	/* check if suspended */
+	if (usb->susp_count > 0) {
+		pr_info("%s: usb->susp_count %d > 0 (suspended)\n",
+			__func__, usb->susp_count);
+		return;
+	}
+
+	/* submit queued tx urb(s) */
+	while ((urb = usb_get_from_anchor(&usb->usb.tx_urb_deferred))
+		!= (struct urb *) 0) {
+		/* decrement count from usb_get_from_anchor() */
+		usb_free_urb(urb);
+		/* check if usb interface disconnected */
+		if (!usb->usb.interface) {
+			pr_err("%s: not submitting tx urb %p"
+				" - interface disconnected\n",
+				__func__, urb);
+			if (urb->transfer_buffer) {
+				kfree(urb->transfer_buffer);
+				urb->transfer_buffer = (void *) 0;
+			}
+			usb_free_urb(urb);
+			usb->stats.tx_errors++;
+			continue;
+		}
+		/* autoresume before tx */
+		usb_mark_last_busy(usb->usb.device);
+		/* submit tx urb */
+		err = usb_submit_urb(urb, GFP_ATOMIC);
+		if (err < 0) {
+			pr_err("%s: usb_submit_urb(%p) failed - err %d\n",
+				__func__, urb, err);
+			usb_autopm_put_interface_async(usb->usb.interface);
+			if (urb->transfer_buffer) {
+				kfree(urb->transfer_buffer);
+				urb->transfer_buffer = (void *) 0;
+			}
+			usb_free_urb(urb);
+			usb->stats.tx_errors++;
+			continue;
+		}
+		/* free tx urb
+		 * - actual urb free occurs when refcnt which was incremented
+		 *   in usb_submit_urb is decremented to 0 (usually after urb
+		 *   completion function returns)
+		 * - tx urb transfer buffer will be freed in urb completion
+		 *   function
+		 */
+		usb_free_urb(urb);
+	}
+
+	pr_debug("usb_net_raw_ip_tx_urb_work }\n");
+}
+
+static void usb_net_raw_ip_tx_urb_comp(struct urb *urb)
+{
+	struct baseband_usb *usb;
+
+	pr_debug("usb_net_raw_ip_tx_urb_comp {\n");
+
+	/* check input */
+	if (!urb) {
+		pr_err("no urb\n");
+		return;
+	}
+	usb = (struct baseband_usb *)urb->context;
+	switch (urb->status) {
+	case 0:
+		break;
+	case -ENOENT:
+		/* fall through */
+	case -ESHUTDOWN:
+		/* fall through */
+	case -EPROTO:
+		pr_info("%s: tx urb %p - link shutdown %d\n",
+			__func__, urb, urb->status);
+		usb_autopm_put_interface_async(usb->usb.interface);
+		goto err_exit;
+	default:
+		pr_info("%s: tx urb %p - status %d\n",
+			__func__, urb, urb->status);
+		break;
+	}
+	if (urb->status)
+		usb->stats.tx_errors++;
+	else {
+		usb->stats.tx_packets++;
+		usb->stats.tx_bytes += urb->transfer_buffer_length;
+	}
+
+	/* autosuspend after tx completed */
+	if (!usb->usb.interface) {
+		pr_err("%s: usb interface disconnected"
+			" before tx urb completed!\n",
+			__func__);
+		goto err_exit;
+	}
+	usb_autopm_put_interface_async(usb->usb.interface);
+
+err_exit:
+	/* free tx urb transfer buffer */
+	if (urb->transfer_buffer) {
+		kfree(urb->transfer_buffer);
+		urb->transfer_buffer = (void *) 0;
+	}
+	pr_debug("usb_net_raw_ip_tx_urb_comp }\n");
+}
+
+static int usb_net_raw_ip_init(void)
+{
+	int i;
+	int err;
+	char name[32];
+
+	pr_debug("usb_net_raw_ip_init {\n");
+
+	err = usb_register(&baseband_usb_driver);
+	if (err < 0) {
+		pr_err("cannot open usb driver - err %d\n", err);
+		return err;
+	}
+
+	/* create multiple raw-ip network devices */
+	for (i = 0; i < max_intfs; i++) {
+		/* open baseband usb */
+		g_i = i;
+		baseband_usb_net[i] = baseband_usb_open(i,
+						usb_net_raw_ip_intf[i]);
+		if (!baseband_usb_net[i]) {
+			pr_err("cannot open baseband usb net\n");
+			err = -1;
+			goto error_exit;
+		}
+		init_usb_anchor(&baseband_usb_net[i]->usb.tx_urb_deferred);
+		/* register network device */
+		usb_net_raw_ip_dev[i] = alloc_netdev(0,
+			BASEBAND_USB_NET_DEV_NAME,
+			ether_setup);
+		if (!usb_net_raw_ip_dev[i]) {
+			pr_err("alloc_netdev() failed\n");
+			err = -ENOMEM;
+			goto error_exit;
+		}
+		usb_net_raw_ip_dev[i]->netdev_ops = &usb_net_raw_ip_ops;
+		usb_net_raw_ip_dev[i]->watchdog_timeo = TX_TIMEOUT;
+		random_ether_addr(usb_net_raw_ip_dev[i]->dev_addr);
+		err = register_netdev(usb_net_raw_ip_dev[i]);
+		if (err < 0) {
+			pr_err("cannot register network device - %d\n", err);
+			goto error_exit;
+		}
+		pr_debug("registered baseband usb network device"
+				" - dev %p name %s\n", usb_net_raw_ip_dev[i],
+				 BASEBAND_USB_NET_DEV_NAME);
+		/* start usb rx */
+		err = usb_net_raw_ip_setup_rx_urb(baseband_usb_net[i]);
+		if (err < 0) {
+			pr_err("setup reusable rx urb failed - err %d\n", err);
+			goto error_exit;
+		}
+		err = usb_net_raw_ip_rx_urb_submit(baseband_usb_net[i]);
+		if (err < 0) {
+			pr_err("submit rx failed - err %d\n", err);
+			goto error_exit;
+		}
+		/* start usb tx */
+		sprintf(name, "raw_ip_tx_wq-%d",
+			baseband_usb_net[i]->baseband_index);
+		baseband_usb_net[i]->usb.tx_workqueue
+			= create_singlethread_workqueue(name);
+		if (!baseband_usb_net[i]->usb.tx_workqueue) {
+			pr_err("cannot create workqueue\n");
+			goto error_exit;
+		}
+		INIT_WORK(&baseband_usb_net[i]->usb.tx_work,
+			usb_net_raw_ip_tx_urb_work);
+	}
+
+	pr_debug("usb_net_raw_ip_init }\n");
+	return 0;
+
+error_exit:
+	/* destroy multiple raw-ip network devices */
+	for (i = 0; i < max_intfs; i++) {
+		/* unregister network device */
+		if (usb_net_raw_ip_dev[i]) {
+			unregister_netdev(usb_net_raw_ip_dev[i]);
+			free_netdev(usb_net_raw_ip_dev[i]);
+			usb_net_raw_ip_dev[i] = (struct net_device *) 0;
+		}
+		/* close baseband usb */
+		if (baseband_usb_net[i]) {
+			/* stop usb tx */
+			if (baseband_usb_net[i]->usb.tx_workqueue) {
+				destroy_workqueue(baseband_usb_net[i]
+					->usb.tx_workqueue);
+				baseband_usb_net[i]->usb.tx_workqueue
+					= (struct workqueue_struct *) 0;
+			}
+			if (baseband_usb_net[i]->usb.tx_urb) {
+				usb_kill_urb(baseband_usb_net[i]->usb.tx_urb);
+				baseband_usb_net[i]->usb.tx_urb
+					= (struct urb *) 0;
+			}
+			/* stop usb rx */
+			if (baseband_usb_net[i]->usb.rx_urb) {
+				usb_kill_urb(baseband_usb_net[i]->usb.rx_urb);
+				baseband_usb_net[i]->usb.rx_urb
+					= (struct urb *) 0;
+			}
+			usb_net_raw_ip_free_rx_urb(baseband_usb_net[i]);
+			/* close usb */
+			baseband_usb_close(baseband_usb_net[i]);
+			baseband_usb_net[i] = (struct baseband_usb *) 0;
+		}
+	}
+	usb_deregister(&baseband_usb_driver);
+
+	return err;
+}
+
+static void usb_net_raw_ip_exit(void)
+{
+	int i;
+
+	pr_debug("usb_net_raw_ip_exit {\n");
+
+	/* destroy multiple raw-ip network devices */
+	for (i = 0; i < max_intfs; i++) {
+		/* unregister network device */
+		if (usb_net_raw_ip_dev[i]) {
+			unregister_netdev(usb_net_raw_ip_dev[i]);
+			free_netdev(usb_net_raw_ip_dev[i]);
+			usb_net_raw_ip_dev[i] = (struct net_device *) 0;
+		}
+		/* close baseband usb */
+		if (baseband_usb_net[i]) {
+			/* stop usb tx */
+			if (baseband_usb_net[i]->usb.tx_workqueue) {
+				destroy_workqueue(baseband_usb_net[i]
+					->usb.tx_workqueue);
+				baseband_usb_net[i]->usb.tx_workqueue
+					= (struct workqueue_struct *) 0;
+			}
+			if (baseband_usb_net[i]->usb.tx_urb) {
+				usb_kill_urb(baseband_usb_net[i]->usb.tx_urb);
+				baseband_usb_net[i]->usb.tx_urb
+					= (struct urb *) 0;
+			}
+			/* stop usb rx */
+			if (baseband_usb_net[i]->usb.rx_urb) {
+				usb_kill_urb(baseband_usb_net[i]->usb.rx_urb);
+				baseband_usb_net[i]->usb.rx_urb
+					= (struct urb *) 0;
+			}
+			usb_net_raw_ip_free_rx_urb(baseband_usb_net[i]);
+			/* close usb */
+			baseband_usb_close(baseband_usb_net[i]);
+			baseband_usb_net[i] = (struct baseband_usb *) 0;
+		}
+	}
+
+	pr_debug("close usb driver {\n");
+	usb_deregister(&baseband_usb_driver);
+	pr_debug("close usb driver }\n");
+
+	pr_debug("usb_net_raw_ip_exit }\n");
+}
+
+module_init(usb_net_raw_ip_init)
+module_exit(usb_net_raw_ip_exit)
+
diff --git a/drivers/net/usb/smsc95xx.c b/drivers/net/usb/smsc95xx.c
index f74f3ce71526..370846911800 100644
--- a/drivers/net/usb/smsc95xx.c
+++ b/drivers/net/usb/smsc95xx.c
@@ -1,7 +1,7 @@
  /***************************************************************************
  *
  * Copyright (C) 2007-2008 SMSC
- *
+ * Copyright (C) 2012 NVIDIA Corporation.
  * 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
@@ -63,6 +63,11 @@ static int turbo_mode = true;
 module_param(turbo_mode, bool, 0644);
 MODULE_PARM_DESC(turbo_mode, "Enable multiple frames per Rx transaction");
 
+static u8 mac_addr[6] = {0};
+static bool smsc_mac_addr_set;
+module_param_array_named(mac_addr, mac_addr, byte, NULL, 0);
+MODULE_PARM_DESC(mac_addr, "SMSC command line MAC address");
+
 static int smsc95xx_read_reg(struct usbnet *dev, u32 index, u32 *data)
 {
 	u32 *buf = kmalloc(4, GFP_KERNEL);
@@ -612,6 +617,15 @@ static void smsc95xx_init_mac_address(struct usbnet *dev)
 		}
 	}
 
+	/* try reading mac address from command line */
+	if (is_valid_ether_addr(mac_addr) && !smsc_mac_addr_set) {
+		memcpy(dev->net->dev_addr, mac_addr, sizeof(mac_addr));
+		smsc_mac_addr_set = true;
+		netif_dbg(dev, ifup, dev->net,
+				"MAC address read from command line");
+		return;
+	}
+
 	/* no eeprom, or eeprom values are invalid. generate random MAC */
 	random_ether_addr(dev->net->dev_addr);
 	netif_dbg(dev, ifup, dev->net, "MAC address set to random_ether_addr\n");
@@ -1284,6 +1298,11 @@ static const struct usb_device_id products[] = {
 		USB_DEVICE(0x0424, 0x9E08),
 		.driver_info = (unsigned long) &smsc95xx_info,
 	},
+	{
+		/* SMSC89530 USB Ethernet Device on Automotive VCM */
+		USB_DEVICE(0x0424, 0x9E08),
+		.driver_info = (unsigned long) &smsc95xx_info,
+	},
 	{ },		/* END */
 };
 MODULE_DEVICE_TABLE(usb, products);
diff --git a/drivers/net/usb/usbnet.c b/drivers/net/usb/usbnet.c
index ce395fe5de26..930a4c1959d2 100644
--- a/drivers/net/usb/usbnet.c
+++ b/drivers/net/usb/usbnet.c
@@ -210,6 +210,7 @@ static int init_status (struct usbnet *dev, struct usb_interface *intf)
 		} else {
 			usb_fill_int_urb(dev->interrupt, dev->udev, pipe,
 				buf, maxp, intr_complete, dev, period);
+			dev->interrupt->transfer_flags |= URB_FREE_BUFFER;
 			dev_dbg(&intf->dev,
 				"status ep%din, %d bytes period %d\n",
 				usb_pipeendpoint(pipe), maxp, period);
@@ -277,17 +278,32 @@ int usbnet_change_mtu (struct net_device *net, int new_mtu)
 }
 EXPORT_SYMBOL_GPL(usbnet_change_mtu);
 
+/* The caller must hold list->lock */
+static void __usbnet_queue_skb(struct sk_buff_head *list,
+			struct sk_buff *newsk, enum skb_state state)
+{
+	struct skb_data *entry = (struct skb_data *) newsk->cb;
+
+	__skb_queue_tail(list, newsk);
+	entry->state = state;
+}
+
 /*-------------------------------------------------------------------------*/
 
 /* some LK 2.4 HCDs oopsed if we freed or resubmitted urbs from
  * completion callbacks.  2.5 should have fixed those bugs...
  */
 
-static void defer_bh(struct usbnet *dev, struct sk_buff *skb, struct sk_buff_head *list)
+static enum skb_state defer_bh(struct usbnet *dev, struct sk_buff *skb,
+		struct sk_buff_head *list, enum skb_state state)
 {
 	unsigned long		flags;
+	enum skb_state 		old_state;
+	struct skb_data *entry = (struct skb_data *) skb->cb;
 
 	spin_lock_irqsave(&list->lock, flags);
+	old_state = entry->state;
+	entry->state = state;
 	__skb_unlink(skb, list);
 	spin_unlock(&list->lock);
 	spin_lock(&dev->done.lock);
@@ -295,6 +311,7 @@ static void defer_bh(struct usbnet *dev, struct sk_buff *skb, struct sk_buff_hea
 	if (dev->done.qlen == 1)
 		tasklet_schedule(&dev->bh);
 	spin_unlock_irqrestore(&dev->done.lock, flags);
+	return old_state;
 }
 
 /* some work can't be done in tasklets, so we use keventd
@@ -324,18 +341,17 @@ static int rx_submit (struct usbnet *dev, struct urb *urb, gfp_t flags)
 	unsigned long		lockflags;
 	size_t			size = dev->rx_urb_size;
 
-	if ((skb = alloc_skb (size + NET_IP_ALIGN, flags)) == NULL) {
+	skb = __netdev_alloc_skb_ip_align(dev->net, size, flags);
+	if (!skb) {
 		netif_dbg(dev, rx_err, dev->net, "no rx skb\n");
 		usbnet_defer_kevent (dev, EVENT_RX_MEMORY);
 		usb_free_urb (urb);
 		return -ENOMEM;
 	}
-	skb_reserve (skb, NET_IP_ALIGN);
 
 	entry = (struct skb_data *) skb->cb;
 	entry->urb = urb;
 	entry->dev = dev;
-	entry->state = rx_start;
 	entry->length = 0;
 
 	usb_fill_bulk_urb (urb, dev->udev, dev->in,
@@ -367,7 +383,7 @@ static int rx_submit (struct usbnet *dev, struct urb *urb, gfp_t flags)
 			tasklet_schedule (&dev->bh);
 			break;
 		case 0:
-			__skb_queue_tail (&dev->rxq, skb);
+			__usbnet_queue_skb(&dev->rxq, skb, rx_start);
 		}
 	} else {
 		netif_dbg(dev, ifdown, dev->net, "rx: stopped\n");
@@ -418,16 +434,17 @@ static void rx_complete (struct urb *urb)
 	struct skb_data		*entry = (struct skb_data *) skb->cb;
 	struct usbnet		*dev = entry->dev;
 	int			urb_status = urb->status;
+	enum skb_state		state;
 
 	skb_put (skb, urb->actual_length);
-	entry->state = rx_done;
+	state = rx_done;
 	entry->urb = NULL;
 
 	switch (urb_status) {
 	/* success */
 	case 0:
 		if (skb->len < dev->net->hard_header_len) {
-			entry->state = rx_cleanup;
+			state = rx_cleanup;
 			dev->net->stats.rx_errors++;
 			dev->net->stats.rx_length_errors++;
 			netif_dbg(dev, rx_err, dev->net,
@@ -466,7 +483,7 @@ static void rx_complete (struct urb *urb)
 				  "rx throttle %d\n", urb_status);
 		}
 block:
-		entry->state = rx_cleanup;
+		state = rx_cleanup;
 		entry->urb = urb;
 		urb = NULL;
 		break;
@@ -477,18 +494,20 @@ block:
 		// FALLTHROUGH
 
 	default:
-		entry->state = rx_cleanup;
+		state = rx_cleanup;
 		dev->net->stats.rx_errors++;
 		netif_dbg(dev, rx_err, dev->net, "rx status %d\n", urb_status);
 		break;
 	}
 
-	defer_bh(dev, skb, &dev->rxq);
+	state = defer_bh(dev, skb, &dev->rxq, state);
 
 	if (urb) {
 		if (netif_running (dev->net) &&
-		    !test_bit (EVENT_RX_HALT, &dev->flags)) {
+		    !test_bit (EVENT_RX_HALT, &dev->flags) &&
+		    state != unlink_start) {
 			rx_submit (dev, urb, GFP_ATOMIC);
+			usb_mark_last_busy(dev->udev);
 			return;
 		}
 		usb_free_urb (urb);
@@ -573,18 +592,34 @@ EXPORT_SYMBOL_GPL(usbnet_purge_paused_rxq);
 static int unlink_urbs (struct usbnet *dev, struct sk_buff_head *q)
 {
 	unsigned long		flags;
-	struct sk_buff		*skb, *skbnext;
+	struct sk_buff		*skb;
 	int			count = 0;
 
 	spin_lock_irqsave (&q->lock, flags);
-	skb_queue_walk_safe(q, skb, skbnext) {
+	while (!skb_queue_empty(q)) {
 		struct skb_data		*entry;
 		struct urb		*urb;
 		int			retval;
 
-		entry = (struct skb_data *) skb->cb;
+		skb_queue_walk(q, skb) {
+			entry = (struct skb_data *) skb->cb;
+			if (entry->state != unlink_start)
+				goto found;
+		}
+		break;
+found:
+		entry->state = unlink_start;
 		urb = entry->urb;
 
+		/*
+		 * Get reference count of the URB to avoid it to be
+		 * freed during usb_unlink_urb, which may trigger
+		 * use-after-free problem inside usb_unlink_urb since
+		 * usb_unlink_urb is always racing with .complete
+		 * handler(include defer_bh).
+		 */
+		usb_get_urb(urb);
+		spin_unlock_irqrestore(&q->lock, flags);
 		// during some PM-driven resume scenarios,
 		// these (async) unlinks complete immediately
 		retval = usb_unlink_urb (urb);
@@ -592,6 +627,8 @@ static int unlink_urbs (struct usbnet *dev, struct sk_buff_head *q)
 			netdev_dbg(dev->net, "unlink urb err, %d\n", retval);
 		else
 			count++;
+		usb_put_urb(urb);
+		spin_lock_irqsave(&q->lock, flags);
 	}
 	spin_unlock_irqrestore (&q->lock, flags);
 	return count;
@@ -1022,9 +1059,7 @@ static void tx_complete (struct urb *urb)
 	}
 
 	usb_autopm_put_interface_async(dev->intf);
-	urb->dev = NULL;
-	entry->state = tx_done;
-	defer_bh(dev, skb, &dev->txq);
+	(void) defer_bh(dev, skb, &dev->txq, tx_done);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -1077,7 +1112,6 @@ netdev_tx_t usbnet_start_xmit (struct sk_buff *skb,
 	entry = (struct skb_data *) skb->cb;
 	entry->urb = urb;
 	entry->dev = dev;
-	entry->state = tx_start;
 	entry->length = length;
 
 	usb_fill_bulk_urb (urb, dev->udev, dev->out,
@@ -1136,7 +1170,7 @@ netdev_tx_t usbnet_start_xmit (struct sk_buff *skb,
 		break;
 	case 0:
 		net->trans_start = jiffies;
-		__skb_queue_tail (&dev->txq, skb);
+		__usbnet_queue_skb(&dev->txq, skb, tx_start);
 		if (dev->txq.qlen >= TX_QLEN (dev))
 			netif_stop_queue (net);
 	}
@@ -1394,6 +1428,9 @@ usbnet_probe (struct usb_interface *udev, const struct usb_device_id *prod)
 		/* WWAN devices should always be named "wwan%d" */
 		if ((dev->driver_info->flags & FLAG_WWAN) != 0)
 			strcpy(net->name, "wwan%d");
+		/* RMNET devices should always be named "rmnet%d" */
+		if ((dev->driver_info->flags & FLAG_RMNET) != 0)
+			strcpy(net->name, "rmnet%d");
 
 		/* maybe the remote can't receive an Ethernet MTU */
 		if (net->mtu > (dev->hard_mtu - net->hard_header_len))
@@ -1528,7 +1565,7 @@ int usbnet_resume (struct usb_interface *intf)
 
 		if (test_bit(EVENT_DEV_OPEN, &dev->flags)) {
 			if (!(dev->txq.qlen >= TX_QLEN(dev)))
-				netif_start_queue(dev->net);
+				netif_tx_wake_all_queues(dev->net);
 			tasklet_schedule (&dev->bh);
 		}
 	}
diff --git a/drivers/net/vmxnet3/vmxnet3_ethtool.c b/drivers/net/vmxnet3/vmxnet3_ethtool.c
index 27400edeef55..f51172f4e6b9 100644
--- a/drivers/net/vmxnet3/vmxnet3_ethtool.c
+++ b/drivers/net/vmxnet3/vmxnet3_ethtool.c
@@ -570,44 +570,38 @@ vmxnet3_get_rxnfc(struct net_device *netdev, struct ethtool_rxnfc *info,
 }
 
 #ifdef VMXNET3_RSS
+static u32
+vmxnet3_get_rss_indir_size(struct net_device *netdev)
+{
+	struct vmxnet3_adapter *adapter = netdev_priv(netdev);
+	struct UPT1_RSSConf *rssConf = adapter->rss_conf;
+
+	return rssConf->indTableSize;
+}
+
 static int
-vmxnet3_get_rss_indir(struct net_device *netdev,
-		      struct ethtool_rxfh_indir *p)
+vmxnet3_get_rss_indir(struct net_device *netdev, u32 *p)
 {
 	struct vmxnet3_adapter *adapter = netdev_priv(netdev);
 	struct UPT1_RSSConf *rssConf = adapter->rss_conf;
-	unsigned int n = min_t(unsigned int, p->size, rssConf->indTableSize);
+	unsigned int n = rssConf->indTableSize;
 
-	p->size = rssConf->indTableSize;
 	while (n--)
-		p->ring_index[n] = rssConf->indTable[n];
+		p[n] = rssConf->indTable[n];
 	return 0;
 
 }
 
 static int
-vmxnet3_set_rss_indir(struct net_device *netdev,
-		      const struct ethtool_rxfh_indir *p)
+vmxnet3_set_rss_indir(struct net_device *netdev, const u32 *p)
 {
 	unsigned int i;
 	unsigned long flags;
 	struct vmxnet3_adapter *adapter = netdev_priv(netdev);
 	struct UPT1_RSSConf *rssConf = adapter->rss_conf;
 
-	if (p->size != rssConf->indTableSize)
-		return -EINVAL;
-	for (i = 0; i < rssConf->indTableSize; i++) {
-		/*
-		 * Return with error code if any of the queue indices
-		 * is out of range
-		 */
-		if (p->ring_index[i] < 0 ||
-		    p->ring_index[i] >= adapter->num_rx_queues)
-			return -EINVAL;
-	}
-
 	for (i = 0; i < rssConf->indTableSize; i++)
-		rssConf->indTable[i] = p->ring_index[i];
+		rssConf->indTable[i] = p[i];
 
 	spin_lock_irqsave(&adapter->cmd_lock, flags);
 	VMXNET3_WRITE_BAR1_REG(adapter, VMXNET3_REG_CMD,
@@ -634,6 +628,7 @@ static struct ethtool_ops vmxnet3_ethtool_ops = {
 	.set_ringparam     = vmxnet3_set_ringparam,
 	.get_rxnfc         = vmxnet3_get_rxnfc,
 #ifdef VMXNET3_RSS
+	.get_rxfh_indir_size = vmxnet3_get_rss_indir_size,
 	.get_rxfh_indir    = vmxnet3_get_rss_indir,
 	.set_rxfh_indir    = vmxnet3_set_rss_indir,
 #endif
diff --git a/drivers/net/wireless/Kconfig b/drivers/net/wireless/Kconfig
index f354bd4e121e..802bbd6c2b8e 100644
--- a/drivers/net/wireless/Kconfig
+++ b/drivers/net/wireless/Kconfig
@@ -268,9 +268,17 @@ config MWL8K
 	  To compile this driver as a module, choose M here: the module
 	  will be called mwl8k.  If unsure, say N.
 
+config WIFI_CONTROL_FUNC
+	bool "Enable WiFi control function abstraction"
+	help
+	  Enables Power/Reset/Carddetect function abstraction
+
 source "drivers/net/wireless/ath/Kconfig"
 source "drivers/net/wireless/b43/Kconfig"
 source "drivers/net/wireless/b43legacy/Kconfig"
+source "drivers/net/wireless/bcm4329/Kconfig"
+source "drivers/net/wireless/bcmdhd/Kconfig"
+source "drivers/net/wireless/sd8797/Kconfig"
 source "drivers/net/wireless/hostap/Kconfig"
 source "drivers/net/wireless/ipw2x00/Kconfig"
 source "drivers/net/wireless/iwlwifi/Kconfig"
diff --git a/drivers/net/wireless/Makefile b/drivers/net/wireless/Makefile
index 7bba6a82b875..3731f0ee3084 100644
--- a/drivers/net/wireless/Makefile
+++ b/drivers/net/wireless/Makefile
@@ -11,7 +11,7 @@ obj-$(CONFIG_AIRO)		+= airo.o
 obj-$(CONFIG_AIRO_CS)		+= airo_cs.o airo.o
 
 obj-$(CONFIG_ATMEL)             += atmel.o
-obj-$(CONFIG_PCI_ATMEL)         += atmel_pci.o 
+obj-$(CONFIG_PCI_ATMEL)         += atmel_pci.o
 obj-$(CONFIG_PCMCIA_ATMEL)      += atmel_cs.o
 
 obj-$(CONFIG_AT76C50X_USB)      += at76c50x-usb.o
@@ -58,3 +58,7 @@ obj-$(CONFIG_WL12XX_PLATFORM_DATA)	+= wl12xx/
 obj-$(CONFIG_IWM)	+= iwmc3200wifi/
 
 obj-$(CONFIG_MWIFIEX)	+= mwifiex/
+
+obj-$(CONFIG_BCM4329)	+= bcm4329/
+obj-$(CONFIG_BCMDHD)	+= bcmdhd/
+obj-$(CONFIG_SD8797)	+= sd8797/
diff --git a/drivers/net/wireless/ath/ath9k/ani.c b/drivers/net/wireless/ath/ath9k/ani.c
index bfb6481f01f9..4e4e7c3dcdde 100644
--- a/drivers/net/wireless/ath/ath9k/ani.c
+++ b/drivers/net/wireless/ath/ath9k/ani.c
@@ -502,9 +502,6 @@ static void ath9k_ani_reset_old(struct ath_hw *ah, bool is_scanning)
 		ath9k_hw_ani_control(ah, ATH9K_ANI_CCK_WEAK_SIGNAL_THR,
 				     ATH9K_ANI_CCK_WEAK_SIG_THR);
 
-		ath9k_hw_setrxfilter(ah, ath9k_hw_getrxfilter(ah) |
-				     ATH9K_RX_FILTER_PHYERR);
-
 		ath9k_ani_restart(ah);
 		return;
 	}
@@ -525,8 +522,6 @@ static void ath9k_ani_reset_old(struct ath_hw *ah, bool is_scanning)
 		ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL,
 				     aniState->firstepLevel);
 
-	ath9k_hw_setrxfilter(ah, ath9k_hw_getrxfilter(ah) &
-			     ~ATH9K_RX_FILTER_PHYERR);
 	ath9k_ani_restart(ah);
 
 	ENABLE_REGWRITE_BUFFER(ah);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_calib.c b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
index f48051c50092..7c2aaad24ed0 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_calib.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
@@ -643,8 +643,9 @@ static void ar9003_hw_detect_outlier(int *mp_coeff, int nmeasurement,
 			outlier_idx = max_idx;
 		else
 			outlier_idx = min_idx;
+
+		mp_coeff[outlier_idx] = mp_avg;
 	}
-	mp_coeff[outlier_idx] = mp_avg;
 }
 
 static void ar9003_hw_tx_iqcal_load_avg_2_passes(struct ath_hw *ah,
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mac.c b/drivers/net/wireless/ath/ath9k/ar9003_mac.c
index 8ff0b88a29b9..048f6afea199 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mac.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mac.c
@@ -253,8 +253,6 @@ static int ar9003_hw_proc_txdesc(struct ath_hw *ah, void *ds,
 		return -EIO;
 	}
 
-	if (status & AR_TxOpExceeded)
-		ts->ts_status |= ATH9K_TXERR_XTXOP;
 	ts->ts_rateindex = MS(status, AR_FinalTxIdx);
 	ts->ts_seqnum = MS(status, AR_SeqNum);
 	ts->tid = MS(status, AR_TxTid);
@@ -264,6 +262,8 @@ static int ar9003_hw_proc_txdesc(struct ath_hw *ah, void *ds,
 	ts->ts_status = 0;
 	ts->ts_flags  = 0;
 
+	if (status & AR_TxOpExceeded)
+		ts->ts_status |= ATH9K_TXERR_XTXOP;
 	status = ACCESS_ONCE(ads->status2);
 	ts->ts_rssi_ctl0 = MS(status, AR_TxRSSIAnt00);
 	ts->ts_rssi_ctl1 = MS(status, AR_TxRSSIAnt01);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_phy.h b/drivers/net/wireless/ath/ath9k/ar9003_phy.h
index 5c590429f120..32ac05f18776 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.h
@@ -572,12 +572,12 @@
 
 #define AR_PHY_TXGAIN_TABLE      (AR_SM_BASE + 0x300)
 
-#define AR_PHY_TX_IQCAL_CONTROL_1   (AR_SM_BASE + AR_SREV_9485(ah) ? \
-						 0x3c8 : 0x448)
-#define AR_PHY_TX_IQCAL_START       (AR_SM_BASE + AR_SREV_9485(ah) ? \
-						 0x3c4 : 0x440)
-#define AR_PHY_TX_IQCAL_STATUS_B0   (AR_SM_BASE + AR_SREV_9485(ah) ? \
-						 0x3f0 : 0x48c)
+#define AR_PHY_TX_IQCAL_CONTROL_1   (AR_SM_BASE + (AR_SREV_9485(ah) ? \
+						 0x3c8 : 0x448))
+#define AR_PHY_TX_IQCAL_START       (AR_SM_BASE + (AR_SREV_9485(ah) ? \
+						 0x3c4 : 0x440))
+#define AR_PHY_TX_IQCAL_STATUS_B0   (AR_SM_BASE + (AR_SREV_9485(ah) ? \
+						 0x3f0 : 0x48c))
 #define AR_PHY_TX_IQCAL_CORR_COEFF_B0(_i)    (AR_SM_BASE + \
 					     (AR_SREV_9485(ah) ? \
 					      0x3d0 : 0x450) + ((_i) << 2))
diff --git a/drivers/net/wireless/ath/ath9k/ar9485_initvals.h b/drivers/net/wireless/ath/ath9k/ar9485_initvals.h
index 611ea6ce8508..d16d029f81a9 100644
--- a/drivers/net/wireless/ath/ath9k/ar9485_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9485_initvals.h
@@ -521,7 +521,7 @@ static const u32 ar9485_1_1_radio_postamble[][2] = {
 	{0x000160ac, 0x24611800},
 	{0x000160b0, 0x03284f3e},
 	{0x0001610c, 0x00170000},
-	{0x00016140, 0x10804008},
+	{0x00016140, 0x50804008},
 };
 
 static const u32 ar9485_1_1_mac_postamble[][5] = {
@@ -603,7 +603,7 @@ static const u32 ar9485_1_1_radio_core[][2] = {
 
 static const u32 ar9485_1_1_pcie_phy_pll_on_clkreq_enable_L1[][2] = {
 	/* Addr        allmodes */
-	{0x00018c00, 0x10052e5e},
+	{0x00018c00, 0x18052e5e},
 	{0x00018c04, 0x000801d8},
 	{0x00018c08, 0x0000080c},
 };
@@ -776,7 +776,7 @@ static const u32 ar9485_modes_green_ob_db_tx_gain_1_1[][5] = {
 
 static const u32 ar9485_1_1_pcie_phy_clkreq_disable_L1[][2] = {
 	/* Addr        allmodes */
-	{0x00018c00, 0x10013e5e},
+	{0x00018c00, 0x18013e5e},
 	{0x00018c04, 0x000801d8},
 	{0x00018c08, 0x0000080c},
 };
@@ -882,7 +882,7 @@ static const u32 ar9485_fast_clock_1_1_baseband_postamble[][3] = {
 
 static const u32 ar9485_1_1_pcie_phy_pll_on_clkreq_disable_L1[][2] = {
 	/* Addr        allmodes  */
-	{0x00018c00, 0x10012e5e},
+	{0x00018c00, 0x18012e5e},
 	{0x00018c04, 0x000801d8},
 	{0x00018c08, 0x0000080c},
 };
@@ -1021,7 +1021,7 @@ static const u32 ar9485_common_rx_gain_1_1[][2] = {
 
 static const u32 ar9485_1_1_pcie_phy_clkreq_enable_L1[][2] = {
 	/* Addr        allmodes */
-	{0x00018c00, 0x10053e5e},
+	{0x00018c00, 0x18053e5e},
 	{0x00018c04, 0x000801d8},
 	{0x00018c08, 0x0000080c},
 };
diff --git a/drivers/net/wireless/ath/ath9k/hif_usb.c b/drivers/net/wireless/ath/ath9k/hif_usb.c
index d3f4a59cd456..77c8ded8de57 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(0x057c, 0x8403) }, /* AVM FRITZ!WLAN 11N v2 USB */
 
 	{ USB_DEVICE(0x0cf3, 0x7015),
 	  .driver_info = AR9287_USB },  /* Atheros */
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index 8dcefe74f4c3..0be84b1714c5 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -2101,6 +2101,10 @@ int ath9k_hw_fill_cap_info(struct ath_hw *ah)
 		pCap->num_gpio_pins = AR9271_NUM_GPIO;
 	else if (AR_DEVID_7010(ah))
 		pCap->num_gpio_pins = AR7010_NUM_GPIO;
+	else if (AR_SREV_9300_20_OR_LATER(ah))
+		pCap->num_gpio_pins = AR9300_NUM_GPIO;
+	else if (AR_SREV_9287_11_OR_LATER(ah))
+		pCap->num_gpio_pins = AR9287_NUM_GPIO;
 	else if (AR_SREV_9285_12_OR_LATER(ah))
 		pCap->num_gpio_pins = AR9285_NUM_GPIO;
 	else if (AR_SREV_9280_20_OR_LATER(ah))
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index 722967b86cf1..69736d84ee0a 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -1841,6 +1841,9 @@ static void ath9k_sta_notify(struct ieee80211_hw *hw,
 	struct ath_softc *sc = hw->priv;
 	struct ath_node *an = (struct ath_node *) sta->drv_priv;
 
+	if (!(sc->sc_flags & SC_OP_TXAGGR))
+		return;
+
 	switch (cmd) {
 	case STA_NOTIFY_SLEEP:
 		an->sleeping = true;
diff --git a/drivers/net/wireless/ath/ath9k/rc.c b/drivers/net/wireless/ath/ath9k/rc.c
index c04a6c3cac7f..297d762dc253 100644
--- a/drivers/net/wireless/ath/ath9k/rc.c
+++ b/drivers/net/wireless/ath/ath9k/rc.c
@@ -1250,7 +1250,9 @@ static void ath_rc_init(struct ath_softc *sc,
 
 	ath_rc_priv->max_valid_rate = k;
 	ath_rc_sort_validrates(rate_table, ath_rc_priv);
-	ath_rc_priv->rate_max_phy = ath_rc_priv->valid_rate_index[k-4];
+	ath_rc_priv->rate_max_phy = (k > 4) ?
+					ath_rc_priv->valid_rate_index[k-4] :
+					ath_rc_priv->valid_rate_index[k-1];
 	ath_rc_priv->rate_table = rate_table;
 
 	ath_dbg(common, ATH_DBG_CONFIG,
diff --git a/drivers/net/wireless/ath/ath9k/recv.c b/drivers/net/wireless/ath/ath9k/recv.c
index 4c21f8cbdeb5..60a3bb2a8af0 100644
--- a/drivers/net/wireless/ath/ath9k/recv.c
+++ b/drivers/net/wireless/ath/ath9k/recv.c
@@ -433,12 +433,9 @@ void ath_rx_cleanup(struct ath_softc *sc)
 
 u32 ath_calcrxfilter(struct ath_softc *sc)
 {
-#define	RX_FILTER_PRESERVE (ATH9K_RX_FILTER_PHYERR | ATH9K_RX_FILTER_PHYRADAR)
-
 	u32 rfilt;
 
-	rfilt = (ath9k_hw_getrxfilter(sc->sc_ah) & RX_FILTER_PRESERVE)
-		| ATH9K_RX_FILTER_UCAST | ATH9K_RX_FILTER_BCAST
+	rfilt = ATH9K_RX_FILTER_UCAST | ATH9K_RX_FILTER_BCAST
 		| ATH9K_RX_FILTER_MCAST;
 
 	if (sc->rx.rxfilter & FIF_PROBE_REQ)
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index e293a7921bf0..fdb4df2dd2ad 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -2508,6 +2508,13 @@ static int b43_upload_microcode(struct b43_wldev *dev)
 		b43_print_fw_helptext(dev->wl, 1);
 		err = -EOPNOTSUPP;
 		goto error;
+	} else if (fwrev >= 598) {
+		b43err(dev->wl, "YOUR FIRMWARE IS TOO NEW. Support for "
+		       "firmware 598 and up requires kernel 3.2 or newer. You "
+		       "have to install older firmware or upgrade kernel.\n");
+		b43_print_fw_helptext(dev->wl, 1);
+		err = -EOPNOTSUPP;
+		goto error;
 	}
 	dev->fw.rev = fwrev;
 	dev->fw.patch = fwpatch;
diff --git a/drivers/net/wireless/bcm4329/Kconfig b/drivers/net/wireless/bcm4329/Kconfig
new file mode 100644
index 000000000000..a04ba4c4d4c3
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/Kconfig
@@ -0,0 +1,82 @@
+config BCM4329
+	tristate "Broadcom 4329 wireless cards support"
+	depends on MMC
+	select WIRELESS_EXT
+	select WEXT_PRIV
+	---help---
+	  This module adds support for wireless adapters based on
+	  Broadcom 4329 chipset.
+
+	  This driver uses the kernel's wireless extensions subsystem.
+
+	  If you choose to build a module, it'll be called dhd. Say M if
+	  unsure.
+
+config BCM4329_FIRST_SCAN
+	depends on BCM4329
+	bool "first scan support"
+	default n
+	---help---
+	Initiate broadcast scan (active scan) just after
+	initializing with network interface.
+
+config BCM4329_FW_PATH
+	depends on BCM4329
+	string "Firmware path"
+	default "/system/etc/firmware/fw_bcm4329.bin"
+	---help---
+	  Path to the firmware file.
+
+config BCM4329_NVRAM_PATH
+	depends on BCM4329
+	string "NVRAM path"
+	default "/proc/calibration"
+	---help---
+	  Path to the calibration file.
+
+config BCM4329_WIFI_CONTROL_FUNC
+	bool "Use bcm4329_wlan device"
+	depends on BCM4329
+	default n
+	---help---
+	   Use this option to get various parameters from architecture specific
+	   bcm4329_wlan platform device. Say n if unsure.
+
+if BCM4329_WIFI_CONTROL_FUNC
+
+config BCM4329_DHD_USE_STATIC_BUF
+	bool "Use static buffer"
+	depends on BCM4329
+	default n
+	---help---
+	   Use static buffer from kernel heap allocated during bcm4329_wlan
+	   platform device creation.
+
+config BCM4329_HW_OOB
+	bool "Use out of band interrupt"
+	depends on BCM4329
+	default n
+	---help---
+	   Use out of band interrupt for wake on wireless.
+
+config BCM4329_OOB_INTR_ONLY
+	bool "Use out of band interrupt only"
+	depends on BCM4329
+	default n
+	---help---
+	   Use out of band interrupt for all interrupts(including SDIO interrupts).
+
+config BCM4329_GET_CUSTOM_MAC_ENABLE
+	bool "Use custom mac address"
+	depends on BCM4329
+	default n
+	---help---
+	   Use mac address provided by bcm4329_wlan platform device.
+
+config BCM4329_CSCAN_ENABLE
+	bool "Enable Combo Scan"
+	depends on BCM4329
+	default n
+	---help---
+	   Enable Combo Scan
+endif
diff --git a/drivers/net/wireless/bcm4329/Makefile b/drivers/net/wireless/bcm4329/Makefile
new file mode 100644
index 000000000000..60297ece4907
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/Makefile
@@ -0,0 +1,56 @@
+# bcm4329
+DHDCFLAGS = -DLINUX -DBCMDRIVER -DBCMDONGLEHOST -DDHDTHREAD -DBCMWPA2         \
+	-DUNRELEASEDCHIP -Dlinux -DDHD_SDALIGN=64 -DMAX_HDR_READ=64           \
+	-DDHD_FIRSTREAD=64 -DDHD_GPL -DDHD_SCHED -DBDC -DTOE -DDHD_BCMEVENTS  \
+	-DSHOW_EVENTS -DBCMSDIO -DDHD_GPL -DBCMLXSDMMC -DBCMPLATFORM_BUS      \
+	-Wall -Wstrict-prototypes -Werror -DCUSTOMER_HW2 -DMMC_SDIO_ABORT     \
+	-DDHD_DEBUG_TRAP -DSOFTAP -DEMBEDDED_PLATFORM -DARP_OFFLOAD_SUPPORT   \
+	-DPKT_FILTER_SUPPORT -DSET_RANDOM_MAC_SOFTAP                          \
+	-DKEEP_ALIVE                                                          \
+	-Idrivers/net/wireless/bcm4329 -Idrivers/net/wireless/bcm4329/include
+
+ifeq ($(CONFIG_BCM4329_WIFI_CONTROL_FUNC),y)
+DHDCFLAGS += -DCONFIG_WIFI_CONTROL_FUNC
+endif
+
+ifeq ($(CONFIG_BCM4329_FIRST_SCAN),y)
+DHDCFLAGS += -DCONFIG_FIRST_SCAN
+endif
+
+ifeq ($(CONFIG_BCM4329_DHD_USE_STATIC_BUF),y)
+DHDCFLAGS += -DDHD_USE_STATIC_BUF
+endif
+ifeq ($(CONFIG_BCM4329_OOB_INTR_ONLY),y)
+DHDCFLAGS += -DOOB_INTR_ONLY
+endif
+ifeq ($(CONFIG_BCM4329_GET_CUSTOM_MAC_ENABLE),y)
+DHDCFLAGS += -DGET_CUSTOM_MAC_ENABLE
+endif
+ifeq ($(CONFIG_BCM4329_HW_OOB),y)
+DHDCFLAGS += -DHW_OOB
+else
+DHDCFLAGS += -DSDIO_ISR_THREAD
+endif
+ifeq ($(CONFIG_BCM4329_CSCAN_ENABLE),y)
+# implementation of PNO_SUPPORT currently requires CSCAN
+DHDCFLAGS += -DCSCAN -DPNO_SUPPORT
+endif
+
+ifeq ($(TARGET_USE_NEW_TOOLCHAIN),1)
+  # gcc-4.6.1 warns a lot more than previous compilers.
+  # The following is the minimal set of warnings that need to not error out
+  # the build for it to succeed. -Wno-error would also work, but this
+  # explicit list allows them to be fixed in smaller chunks.
+  DHDCFLAGS += -Wno-error=unused-but-set-variable
+  DHDCFLAGS += -Wno-error=array-bounds
+endif
+
+DHDOFILES = dhd_linux.o linux_osl.o bcmutils.o dhd_common.o dhd_custom_gpio.o \
+	wl_iw.o siutils.o sbutils.o aiutils.o hndpmu.o bcmwifi.o dhd_sdio.o   \
+	dhd_linux_sched.o dhd_cdc.o bcmsdh_sdmmc.o bcmsdh.o bcmsdh_linux.o    \
+	bcmsdh_sdmmc_linux.o
+
+obj-$(CONFIG_BCM4329) += bcm4329.o
+bcm4329-objs += $(DHDOFILES)
+EXTRA_CFLAGS = $(DHDCFLAGS)
+EXTRA_LDFLAGS += --strip-debug
diff --git a/drivers/net/wireless/bcm4329/aiutils.c b/drivers/net/wireless/bcm4329/aiutils.c
new file mode 100644
index 000000000000..df48ac0d83d4
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/aiutils.c
@@ -0,0 +1,686 @@
+/*
+ * Misc utility routines for accessing chip-specific features
+ * of the SiliconBackplane-based Broadcom chips.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: aiutils.c,v 1.6.4.7.4.6 2010/04/21 20:43:47 Exp $
+ */
+
+#include <typedefs.h>
+#include <bcmdefs.h>
+#include <osl.h>
+#include <bcmutils.h>
+#include <siutils.h>
+#include <hndsoc.h>
+#include <sbchipc.h>
+#include <pcicfg.h>
+
+#include "siutils_priv.h"
+
+STATIC uint32
+get_asd(si_t *sih, uint32 *eromptr, uint sp, uint ad, uint st,
+	uint32 *addrl, uint32 *addrh, uint32 *sizel, uint32 *sizeh);
+
+
+/* EROM parsing */
+
+static uint32
+get_erom_ent(si_t *sih, uint32 *eromptr, uint32 mask, uint32 match)
+{
+	uint32 ent;
+	uint inv = 0, nom = 0;
+
+	while (TRUE) {
+		ent = R_REG(si_osh(sih), (uint32 *)(uintptr)(*eromptr));
+		*eromptr += sizeof(uint32);
+
+		if (mask == 0)
+			break;
+
+		if ((ent & ER_VALID) == 0) {
+			inv++;
+			continue;
+		}
+
+		if (ent == (ER_END | ER_VALID))
+			break;
+
+		if ((ent & mask) == match)
+			break;
+
+		nom++;
+	}
+
+	SI_MSG(("%s: Returning ent 0x%08x\n", __FUNCTION__, ent));
+	if (inv + nom)
+		SI_MSG(("  after %d invalid and %d non-matching entries\n", inv, nom));
+	return ent;
+}
+
+STATIC uint32
+get_asd(si_t *sih, uint32 *eromptr, uint sp, uint ad, uint st,
+	uint32 *addrl, uint32 *addrh, uint32 *sizel, uint32 *sizeh)
+{
+	uint32 asd, sz, szd;
+
+	asd = get_erom_ent(sih, eromptr, ER_VALID, ER_VALID);
+	if (((asd & ER_TAG1) != ER_ADD) ||
+	    (((asd & AD_SP_MASK) >> AD_SP_SHIFT) != sp) ||
+	    ((asd & AD_ST_MASK) != st)) {
+		/* This is not what we want, "push" it back */
+		*eromptr -= sizeof(uint32);
+		return 0;
+	}
+	*addrl = asd & AD_ADDR_MASK;
+	if (asd & AD_AG32)
+		*addrh = get_erom_ent(sih, eromptr, 0, 0);
+	else
+		*addrh = 0;
+	*sizeh = 0;
+	sz = asd & AD_SZ_MASK;
+	if (sz == AD_SZ_SZD) {
+		szd = get_erom_ent(sih, eromptr, 0, 0);
+		*sizel = szd & SD_SZ_MASK;
+		if (szd & SD_SG32)
+			*sizeh = get_erom_ent(sih, eromptr, 0, 0);
+	} else
+		*sizel = AD_SZ_BASE << (sz >> AD_SZ_SHIFT);
+
+	SI_MSG(("  SP %d, ad %d: st = %d, 0x%08x_0x%08x @ 0x%08x_0x%08x\n",
+	        sp, ad, st, *sizeh, *sizel, *addrh, *addrl));
+
+	return asd;
+}
+
+/* parse the enumeration rom to identify all cores */
+void
+ai_scan(si_t *sih, void *regs, uint devid)
+{
+	si_info_t *sii = SI_INFO(sih);
+	chipcregs_t *cc = (chipcregs_t *)regs;
+	uint32 erombase, eromptr, eromlim;
+
+	erombase = R_REG(sii->osh, &cc->eromptr);
+
+	switch (BUSTYPE(sih->bustype)) {
+	case SI_BUS:
+		eromptr = (uintptr)REG_MAP(erombase, SI_CORE_SIZE);
+		break;
+
+	case PCI_BUS:
+		/* Set wrappers address */
+		sii->curwrap = (void *)((uintptr)regs + SI_CORE_SIZE);
+
+		/* Now point the window at the erom */
+		OSL_PCI_WRITE_CONFIG(sii->osh, PCI_BAR0_WIN, 4, erombase);
+		eromptr = (uint32)(uintptr)regs;
+		break;
+
+	case SPI_BUS:
+	case SDIO_BUS:
+		eromptr = erombase;
+		break;
+
+	case PCMCIA_BUS:
+	default:
+		SI_ERROR(("Don't know how to do AXI enumertion on bus %d\n", sih->bustype));
+		ASSERT(0);
+		return;
+	}
+	eromlim = eromptr + ER_REMAPCONTROL;
+
+	SI_MSG(("ai_scan: regs = 0x%p, erombase = 0x%08x, eromptr = 0x%08x, eromlim = 0x%08x\n",
+	        regs, erombase, eromptr, eromlim));
+	while (eromptr < eromlim) {
+		uint32 cia, cib, base, cid, mfg, crev, nmw, nsw, nmp, nsp;
+		uint32 mpd, asd, addrl, addrh, sizel, sizeh;
+		uint i, j, idx;
+		bool br;
+
+		br = FALSE;
+
+		/* Grok a component */
+		cia = get_erom_ent(sih, &eromptr, ER_TAG, ER_CI);
+		if (cia == (ER_END | ER_VALID)) {
+			SI_MSG(("Found END of erom after %d cores\n", sii->numcores));
+			return;
+		}
+		base = eromptr - sizeof(uint32);
+		cib = get_erom_ent(sih, &eromptr, 0, 0);
+
+		if ((cib & ER_TAG) != ER_CI) {
+			SI_ERROR(("CIA not followed by CIB\n"));
+			goto error;
+		}
+
+		cid = (cia & CIA_CID_MASK) >> CIA_CID_SHIFT;
+		mfg = (cia & CIA_MFG_MASK) >> CIA_MFG_SHIFT;
+		crev = (cib & CIB_REV_MASK) >> CIB_REV_SHIFT;
+		nmw = (cib & CIB_NMW_MASK) >> CIB_NMW_SHIFT;
+		nsw = (cib & CIB_NSW_MASK) >> CIB_NSW_SHIFT;
+		nmp = (cib & CIB_NMP_MASK) >> CIB_NMP_SHIFT;
+		nsp = (cib & CIB_NSP_MASK) >> CIB_NSP_SHIFT;
+
+		SI_MSG(("Found component 0x%04x/0x%4x rev %d at erom addr 0x%08x, with nmw = %d, "
+		        "nsw = %d, nmp = %d & nsp = %d\n",
+		        mfg, cid, crev, base, nmw, nsw, nmp, nsp));
+
+		if (((mfg == MFGID_ARM) && (cid == DEF_AI_COMP)) || (nsp == 0))
+			continue;
+		if ((nmw + nsw == 0)) {
+			/* A component which is not a core */
+			if (cid == OOB_ROUTER_CORE_ID) {
+				asd = get_asd(sih, &eromptr, 0, 0, AD_ST_SLAVE,
+					&addrl, &addrh, &sizel, &sizeh);
+				if (asd != 0) {
+					sii->common_info->oob_router = addrl;
+				}
+			}
+			continue;
+		}
+
+		idx = sii->numcores;
+/*		sii->eromptr[idx] = base; */
+		sii->common_info->cia[idx] = cia;
+		sii->common_info->cib[idx] = cib;
+		sii->common_info->coreid[idx] = cid;
+
+		for (i = 0; i < nmp; i++) {
+			mpd = get_erom_ent(sih, &eromptr, ER_VALID, ER_VALID);
+			if ((mpd & ER_TAG) != ER_MP) {
+				SI_ERROR(("Not enough MP entries for component 0x%x\n", cid));
+				goto error;
+			}
+			SI_MSG(("  Master port %d, mp: %d id: %d\n", i,
+			        (mpd & MPD_MP_MASK) >> MPD_MP_SHIFT,
+			        (mpd & MPD_MUI_MASK) >> MPD_MUI_SHIFT));
+		}
+
+		/* First Slave Address Descriptor should be port 0:
+		 * the main register space for the core
+		 */
+		asd = get_asd(sih, &eromptr, 0, 0, AD_ST_SLAVE, &addrl, &addrh, &sizel, &sizeh);
+		if (asd == 0) {
+			/* Try again to see if it is a bridge */
+			asd = get_asd(sih, &eromptr, 0, 0, AD_ST_BRIDGE, &addrl, &addrh,
+			              &sizel, &sizeh);
+			if (asd != 0)
+				br = TRUE;
+			else
+				if ((addrh != 0) || (sizeh != 0) || (sizel != SI_CORE_SIZE)) {
+					SI_ERROR(("First Slave ASD for core 0x%04x malformed "
+					          "(0x%08x)\n", cid, asd));
+					goto error;
+				}
+		}
+		sii->common_info->coresba[idx] = addrl;
+		sii->common_info->coresba_size[idx] = sizel;
+		/* Get any more ASDs in port 0 */
+		j = 1;
+		do {
+			asd = get_asd(sih, &eromptr, 0, j, AD_ST_SLAVE, &addrl, &addrh,
+			              &sizel, &sizeh);
+			if ((asd != 0) && (j == 1) && (sizel == SI_CORE_SIZE))
+				sii->common_info->coresba2[idx] = addrl;
+				sii->common_info->coresba2_size[idx] = sizel;
+			j++;
+		} while (asd != 0);
+
+		/* Go through the ASDs for other slave ports */
+		for (i = 1; i < nsp; i++) {
+			j = 0;
+			do {
+				asd = get_asd(sih, &eromptr, i, j++, AD_ST_SLAVE, &addrl, &addrh,
+				              &sizel, &sizeh);
+			} while (asd != 0);
+			if (j == 0) {
+				SI_ERROR((" SP %d has no address descriptors\n", i));
+				goto error;
+			}
+		}
+
+		/* Now get master wrappers */
+		for (i = 0; i < nmw; i++) {
+			asd = get_asd(sih, &eromptr, i, 0, AD_ST_MWRAP, &addrl, &addrh,
+			              &sizel, &sizeh);
+			if (asd == 0) {
+				SI_ERROR(("Missing descriptor for MW %d\n", i));
+				goto error;
+			}
+			if ((sizeh != 0) || (sizel != SI_CORE_SIZE)) {
+				SI_ERROR(("Master wrapper %d is not 4KB\n", i));
+				goto error;
+			}
+			if (i == 0)
+				sii->common_info->wrapba[idx] = addrl;
+		}
+
+		/* And finally slave wrappers */
+		for (i = 0; i < nsw; i++) {
+			uint fwp = (nsp == 1) ? 0 : 1;
+			asd = get_asd(sih, &eromptr, fwp + i, 0, AD_ST_SWRAP, &addrl, &addrh,
+			              &sizel, &sizeh);
+			if (asd == 0) {
+				SI_ERROR(("Missing descriptor for SW %d\n", i));
+				goto error;
+			}
+			if ((sizeh != 0) || (sizel != SI_CORE_SIZE)) {
+				SI_ERROR(("Slave wrapper %d is not 4KB\n", i));
+				goto error;
+			}
+			if ((nmw == 0) && (i == 0))
+				sii->common_info->wrapba[idx] = addrl;
+		}
+
+		/* Don't record bridges */
+		if (br)
+			continue;
+
+		/* Done with core */
+		sii->numcores++;
+	}
+
+	SI_ERROR(("Reached end of erom without finding END"));
+
+error:
+	sii->numcores = 0;
+	return;
+}
+
+/* This function changes the logical "focus" to the indicated core.
+ * Return the current core's virtual address.
+ */
+void *
+ai_setcoreidx(si_t *sih, uint coreidx)
+{
+	si_info_t *sii = SI_INFO(sih);
+	uint32 addr = sii->common_info->coresba[coreidx];
+	uint32 wrap = sii->common_info->wrapba[coreidx];
+	void *regs;
+
+	if (coreidx >= sii->numcores)
+		return (NULL);
+
+	/*
+	 * If the user has provided an interrupt mask enabled function,
+	 * then assert interrupts are disabled before switching the core.
+	 */
+	ASSERT((sii->intrsenabled_fn == NULL) || !(*(sii)->intrsenabled_fn)((sii)->intr_arg));
+
+	switch (BUSTYPE(sih->bustype)) {
+	case SI_BUS:
+		/* map new one */
+		if (!sii->common_info->regs[coreidx]) {
+			sii->common_info->regs[coreidx] = REG_MAP(addr, SI_CORE_SIZE);
+			ASSERT(GOODREGS(sii->common_info->regs[coreidx]));
+		}
+		sii->curmap = regs = sii->common_info->regs[coreidx];
+		if (!sii->common_info->wrappers[coreidx]) {
+			sii->common_info->wrappers[coreidx] = REG_MAP(wrap, SI_CORE_SIZE);
+			ASSERT(GOODREGS(sii->common_info->wrappers[coreidx]));
+		}
+		sii->curwrap = sii->common_info->wrappers[coreidx];
+		break;
+
+
+	case SPI_BUS:
+	case SDIO_BUS:
+		sii->curmap = regs = (void *)((uintptr)addr);
+		sii->curwrap = (void *)((uintptr)wrap);
+		break;
+
+	case PCMCIA_BUS:
+	default:
+		ASSERT(0);
+		regs = NULL;
+		break;
+	}
+
+	sii->curmap = regs;
+	sii->curidx = coreidx;
+
+	return regs;
+}
+
+/* Return the number of address spaces in current core */
+int
+ai_numaddrspaces(si_t *sih)
+{
+	return 2;
+}
+
+/* Return the address of the nth address space in the current core */
+uint32
+ai_addrspace(si_t *sih, uint asidx)
+{
+	si_info_t *sii;
+	uint cidx;
+
+	sii = SI_INFO(sih);
+	cidx = sii->curidx;
+
+	if (asidx == 0)
+		return sii->common_info->coresba[cidx];
+	else if (asidx == 1)
+		return sii->common_info->coresba2[cidx];
+	else {
+		SI_ERROR(("%s: Need to parse the erom again to find addr space %d\n",
+		          __FUNCTION__, asidx));
+		return 0;
+	}
+}
+
+/* Return the size of the nth address space in the current core */
+uint32
+ai_addrspacesize(si_t *sih, uint asidx)
+{
+	si_info_t *sii;
+	uint cidx;
+
+	sii = SI_INFO(sih);
+	cidx = sii->curidx;
+
+	if (asidx == 0)
+		return sii->common_info->coresba_size[cidx];
+	else if (asidx == 1)
+		return sii->common_info->coresba2_size[cidx];
+	else {
+		SI_ERROR(("%s: Need to parse the erom again to find addr space %d\n",
+		          __FUNCTION__, asidx));
+		return 0;
+	}
+}
+
+uint
+ai_flag(si_t *sih)
+{
+	si_info_t *sii;
+	aidmp_t *ai;
+
+	sii = SI_INFO(sih);
+	ai = sii->curwrap;
+
+	return (R_REG(sii->osh, &ai->oobselouta30) & 0x1f);
+}
+
+void
+ai_setint(si_t *sih, int siflag)
+{
+}
+
+void
+ai_write_wrap_reg(si_t *sih, uint32 offset, uint32 val)
+{
+	si_info_t *sii = SI_INFO(sih);
+	aidmp_t *ai = sii->curwrap;
+	W_REG(sii->osh, (uint32 *)((uint8 *)ai+offset), val);
+	return;
+}
+
+uint
+ai_corevendor(si_t *sih)
+{
+	si_info_t *sii;
+	uint32 cia;
+
+	sii = SI_INFO(sih);
+	cia = sii->common_info->cia[sii->curidx];
+	return ((cia & CIA_MFG_MASK) >> CIA_MFG_SHIFT);
+}
+
+uint
+ai_corerev(si_t *sih)
+{
+	si_info_t *sii;
+	uint32 cib;
+
+	sii = SI_INFO(sih);
+	cib = sii->common_info->cib[sii->curidx];
+	return ((cib & CIB_REV_MASK) >> CIB_REV_SHIFT);
+}
+
+bool
+ai_iscoreup(si_t *sih)
+{
+	si_info_t *sii;
+	aidmp_t *ai;
+
+	sii = SI_INFO(sih);
+	ai = sii->curwrap;
+
+	return (((R_REG(sii->osh, &ai->ioctrl) & (SICF_FGC | SICF_CLOCK_EN)) == SICF_CLOCK_EN) &&
+	        ((R_REG(sii->osh, &ai->resetctrl) & AIRC_RESET) == 0));
+}
+
+/*
+ * Switch to 'coreidx', issue a single arbitrary 32bit register mask&set operation,
+ * switch back to the original core, and return the new value.
+ *
+ * When using the silicon backplane, no fidleing with interrupts or core switches are needed.
+ *
+ * Also, when using pci/pcie, we can optimize away the core switching for pci registers
+ * and (on newer pci cores) chipcommon registers.
+ */
+uint
+ai_corereg(si_t *sih, uint coreidx, uint regoff, uint mask, uint val)
+{
+	uint origidx = 0;
+	uint32 *r = NULL;
+	uint w;
+	uint intr_val = 0;
+	bool fast = FALSE;
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	ASSERT(GOODIDX(coreidx));
+	ASSERT(regoff < SI_CORE_SIZE);
+	ASSERT((val & ~mask) == 0);
+
+	if (coreidx >= SI_MAXCORES)
+		return 0;
+
+	if (BUSTYPE(sih->bustype) == SI_BUS) {
+		/* If internal bus, we can always get at everything */
+		fast = TRUE;
+		/* map if does not exist */
+		if (!sii->common_info->wrappers[coreidx]) {
+			sii->common_info->regs[coreidx] =
+			    REG_MAP(sii->common_info->coresba[coreidx], SI_CORE_SIZE);
+			ASSERT(GOODREGS(sii->common_info->regs[coreidx]));
+		}
+		r = (uint32 *)((uchar *)sii->common_info->regs[coreidx] + regoff);
+	} else if (BUSTYPE(sih->bustype) == PCI_BUS) {
+		/* If pci/pcie, we can get at pci/pcie regs and on newer cores to chipc */
+
+		if ((sii->common_info->coreid[coreidx] == CC_CORE_ID) && SI_FAST(sii)) {
+			/* Chipc registers are mapped at 12KB */
+
+			fast = TRUE;
+			r = (uint32 *)((char *)sii->curmap + PCI_16KB0_CCREGS_OFFSET + regoff);
+		} else if (sii->pub.buscoreidx == coreidx) {
+			/* pci registers are at either in the last 2KB of an 8KB window
+			 * or, in pcie and pci rev 13 at 8KB
+			 */
+			fast = TRUE;
+			if (SI_FAST(sii))
+				r = (uint32 *)((char *)sii->curmap +
+				               PCI_16KB0_PCIREGS_OFFSET + regoff);
+			else
+				r = (uint32 *)((char *)sii->curmap +
+				               ((regoff >= SBCONFIGOFF) ?
+				                PCI_BAR0_PCISBR_OFFSET : PCI_BAR0_PCIREGS_OFFSET) +
+				               regoff);
+		}
+	}
+
+	if (!fast) {
+		INTR_OFF(sii, intr_val);
+
+		/* save current core index */
+		origidx = si_coreidx(&sii->pub);
+
+		/* switch core */
+		r = (uint32*) ((uchar*) ai_setcoreidx(&sii->pub, coreidx) + regoff);
+	}
+	ASSERT(r != NULL);
+
+	/* mask and set */
+	if (mask || val) {
+		w = (R_REG(sii->osh, r) & ~mask) | val;
+		W_REG(sii->osh, r, w);
+	}
+
+	/* readback */
+	w = R_REG(sii->osh, r);
+
+	if (!fast) {
+		/* restore core index */
+		if (origidx != coreidx)
+			ai_setcoreidx(&sii->pub, origidx);
+
+		INTR_RESTORE(sii, intr_val);
+	}
+
+	return (w);
+}
+
+void
+ai_core_disable(si_t *sih, uint32 bits)
+{
+	si_info_t *sii;
+	volatile uint32 dummy;
+	aidmp_t *ai;
+
+	sii = SI_INFO(sih);
+
+	ASSERT(GOODREGS(sii->curwrap));
+	ai = sii->curwrap;
+
+	/* if core is already in reset, just return */
+	if (R_REG(sii->osh, &ai->resetctrl) & AIRC_RESET)
+		return;
+
+	W_REG(sii->osh, &ai->ioctrl, bits);
+	dummy = R_REG(sii->osh, &ai->ioctrl);
+	OSL_DELAY(10);
+
+	W_REG(sii->osh, &ai->resetctrl, AIRC_RESET);
+	OSL_DELAY(1);
+}
+
+/* reset and re-enable a core
+ * inputs:
+ * bits - core specific bits that are set during and after reset sequence
+ * resetbits - core specific bits that are set only during reset sequence
+ */
+void
+ai_core_reset(si_t *sih, uint32 bits, uint32 resetbits)
+{
+	si_info_t *sii;
+	aidmp_t *ai;
+	volatile uint32 dummy;
+
+	sii = SI_INFO(sih);
+	ASSERT(GOODREGS(sii->curwrap));
+	ai = sii->curwrap;
+
+	/*
+	 * Must do the disable sequence first to work for arbitrary current core state.
+	 */
+	ai_core_disable(sih, (bits | resetbits));
+
+	/*
+	 * Now do the initialization sequence.
+	 */
+	W_REG(sii->osh, &ai->ioctrl, (bits | SICF_FGC | SICF_CLOCK_EN));
+	dummy = R_REG(sii->osh, &ai->ioctrl);
+	W_REG(sii->osh, &ai->resetctrl, 0);
+	OSL_DELAY(1);
+
+	W_REG(sii->osh, &ai->ioctrl, (bits | SICF_CLOCK_EN));
+	dummy = R_REG(sii->osh, &ai->ioctrl);
+	OSL_DELAY(1);
+}
+
+
+void
+ai_core_cflags_wo(si_t *sih, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	aidmp_t *ai;
+	uint32 w;
+
+	sii = SI_INFO(sih);
+	ASSERT(GOODREGS(sii->curwrap));
+	ai = sii->curwrap;
+
+	ASSERT((val & ~mask) == 0);
+
+	if (mask || val) {
+		w = ((R_REG(sii->osh, &ai->ioctrl) & ~mask) | val);
+		W_REG(sii->osh, &ai->ioctrl, w);
+	}
+}
+
+uint32
+ai_core_cflags(si_t *sih, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	aidmp_t *ai;
+	uint32 w;
+
+	sii = SI_INFO(sih);
+	ASSERT(GOODREGS(sii->curwrap));
+	ai = sii->curwrap;
+
+	ASSERT((val & ~mask) == 0);
+
+	if (mask || val) {
+		w = ((R_REG(sii->osh, &ai->ioctrl) & ~mask) | val);
+		W_REG(sii->osh, &ai->ioctrl, w);
+	}
+
+	return R_REG(sii->osh, &ai->ioctrl);
+}
+
+uint32
+ai_core_sflags(si_t *sih, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	aidmp_t *ai;
+	uint32 w;
+
+	sii = SI_INFO(sih);
+	ASSERT(GOODREGS(sii->curwrap));
+	ai = sii->curwrap;
+
+	ASSERT((val & ~mask) == 0);
+	ASSERT((mask & ~SISF_CORE_BITS) == 0);
+
+	if (mask || val) {
+		w = ((R_REG(sii->osh, &ai->iostatus) & ~mask) | val);
+		W_REG(sii->osh, &ai->iostatus, w);
+	}
+
+	return R_REG(sii->osh, &ai->iostatus);
+}
diff --git a/drivers/net/wireless/bcm4329/bcmpcispi.c b/drivers/net/wireless/bcm4329/bcmpcispi.c
new file mode 100644
index 000000000000..1a8b6717f924
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/bcmpcispi.c
@@ -0,0 +1,630 @@
+/*
+ * Broadcom SPI over PCI-SPI Host Controller, low-level hardware driver
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmpcispi.c,v 1.22.2.4.4.5.6.1 2010/08/13 00:26:05 Exp $
+ */
+
+#include <typedefs.h>
+#include <bcmutils.h>
+
+#include <sdio.h>		/* SDIO Specs */
+#include <bcmsdbus.h>		/* bcmsdh to/from specific controller APIs */
+#include <sdiovar.h>		/* to get msglevel bit values */
+
+#include <pcicfg.h>
+#include <bcmsdspi.h>
+#include <bcmspi.h>
+#include <bcmpcispi.h>		/* BRCM PCI-SPI Host Controller Register definitions */
+
+
+/* ndis_osl.h needs to do a runtime check of the osh to map
+ * R_REG/W_REG to bus specific access similar to linux_osl.h.
+ * Until then...
+ */
+/* linux */
+
+#define SPIPCI_RREG R_REG
+#define SPIPCI_WREG W_REG
+
+
+#define	SPIPCI_ANDREG(osh, r, v) SPIPCI_WREG(osh, (r), (SPIPCI_RREG(osh, r) & (v)))
+#define	SPIPCI_ORREG(osh, r, v)	SPIPCI_WREG(osh, (r), (SPIPCI_RREG(osh, r) | (v)))
+
+
+int bcmpcispi_dump = 0;		/* Set to dump complete trace of all SPI bus transactions */
+
+typedef struct spih_info_ {
+	uint		bar0;		/* BAR0 of PCI Card */
+	uint		bar1;		/* BAR1 of PCI Card */
+	osl_t 		*osh;		/* osh handle */
+	spih_pciregs_t	*pciregs;	/* PCI Core Registers */
+	spih_regs_t	*regs;		/* SPI Controller Registers */
+	uint8		rev;		/* PCI Card Revision ID */
+} spih_info_t;
+
+
+/* Attach to PCI-SPI Host Controller Hardware */
+bool
+spi_hw_attach(sdioh_info_t *sd)
+{
+	osl_t *osh;
+	spih_info_t *si;
+
+	sd_trace(("%s: enter\n", __FUNCTION__));
+
+	osh = sd->osh;
+
+	if ((si = (spih_info_t *)MALLOC(osh, sizeof(spih_info_t))) == NULL) {
+		sd_err(("%s: out of memory, malloced %d bytes\n", __FUNCTION__, MALLOCED(osh)));
+		return FALSE;
+	}
+
+	bzero(si, sizeof(spih_info_t));
+
+	sd->controller = si;
+
+	si->osh = sd->osh;
+	si->rev = OSL_PCI_READ_CONFIG(sd->osh, PCI_CFG_REV, 4) & 0xFF;
+
+	if (si->rev < 3) {
+		sd_err(("Host controller %d not supported, please upgrade to rev >= 3\n", si->rev));
+		MFREE(osh, si, sizeof(spih_info_t));
+		return (FALSE);
+	}
+
+	sd_err(("Attaching to Generic PCI SPI Host Controller Rev %d\n", si->rev));
+
+	/* FPGA Revision < 3 not supported by driver anymore. */
+	ASSERT(si->rev >= 3);
+
+	si->bar0 = sd->bar0;
+
+	/* Rev < 10 PciSpiHost has 2 BARs:
+	 *    BAR0 = PCI Core Registers
+	 *    BAR1 = PciSpiHost Registers (all other cores on backplane)
+	 *
+	 * Rev 10 and up use a different PCI core which only has a single
+	 * BAR0 which contains the PciSpiHost Registers.
+	 */
+	if (si->rev < 10) {
+		si->pciregs = (spih_pciregs_t *)spi_reg_map(osh,
+		                                              (uintptr)si->bar0,
+		                                              sizeof(spih_pciregs_t));
+		sd_err(("Mapped PCI Core regs to BAR0 at %p\n", si->pciregs));
+
+		si->bar1 = OSL_PCI_READ_CONFIG(sd->osh, PCI_CFG_BAR1, 4);
+		si->regs = (spih_regs_t *)spi_reg_map(osh,
+		                                        (uintptr)si->bar1,
+		                                        sizeof(spih_regs_t));
+		sd_err(("Mapped SPI Controller regs to BAR1 at %p\n", si->regs));
+	} else {
+		si->regs = (spih_regs_t *)spi_reg_map(osh,
+		                                              (uintptr)si->bar0,
+		                                              sizeof(spih_regs_t));
+		sd_err(("Mapped SPI Controller regs to BAR0 at %p\n", si->regs));
+		si->pciregs = NULL;
+	}
+	/* Enable SPI Controller, 16.67MHz SPI Clock */
+	SPIPCI_WREG(osh, &si->regs->spih_ctrl, 0x000000d1);
+
+	/* Set extended feature register to defaults */
+	SPIPCI_WREG(osh, &si->regs->spih_ext, 0x00000000);
+
+	/* Set GPIO CS# High (de-asserted) */
+	SPIPCI_WREG(osh, &si->regs->spih_gpio_data, SPIH_CS);
+
+	/* set GPIO[0] to output for CS# */
+	/* set GPIO[1] to output for power control */
+	/* set GPIO[2] to input for card detect */
+	SPIPCI_WREG(osh, &si->regs->spih_gpio_ctrl, (SPIH_CS | SPIH_SLOT_POWER));
+
+	/* Clear out the Read FIFO in case there is any stuff left in there from a previous run. */
+	while ((SPIPCI_RREG(osh, &si->regs->spih_stat) & SPIH_RFEMPTY) == 0) {
+		SPIPCI_RREG(osh, &si->regs->spih_data);
+	}
+
+	/* Wait for power to stabilize to the SDIO Card (100msec was insufficient) */
+	OSL_DELAY(250000);
+
+	/* Check card detect on FPGA Revision >= 4 */
+	if (si->rev >= 4) {
+		if (SPIPCI_RREG(osh, &si->regs->spih_gpio_data) & SPIH_CARD_DETECT) {
+			sd_err(("%s: no card detected in SD slot\n", __FUNCTION__));
+			spi_reg_unmap(osh, (uintptr)si->regs, sizeof(spih_regs_t));
+			if (si->pciregs) {
+				spi_reg_unmap(osh, (uintptr)si->pciregs, sizeof(spih_pciregs_t));
+			}
+			MFREE(osh, si, sizeof(spih_info_t));
+			return FALSE;
+		}
+	}
+
+	/* Interrupts are level sensitive */
+	SPIPCI_WREG(osh, &si->regs->spih_int_edge, 0x80000000);
+
+	/* Interrupts are active low. */
+	SPIPCI_WREG(osh, &si->regs->spih_int_pol, 0x40000004);
+
+	/* Enable interrupts through PCI Core. */
+	if (si->pciregs) {
+		SPIPCI_WREG(osh, &si->pciregs->ICR, PCI_INT_PROP_EN);
+	}
+
+	sd_trace(("%s: exit\n", __FUNCTION__));
+	return TRUE;
+}
+
+/* Detach and return PCI-SPI Hardware to unconfigured state */
+bool
+spi_hw_detach(sdioh_info_t *sd)
+{
+	spih_info_t *si = (spih_info_t *)sd->controller;
+	osl_t *osh = si->osh;
+	spih_regs_t *regs = si->regs;
+	spih_pciregs_t *pciregs = si->pciregs;
+
+	sd_trace(("%s: enter\n", __FUNCTION__));
+
+	SPIPCI_WREG(osh, &regs->spih_ctrl, 0x00000010);
+	SPIPCI_WREG(osh, &regs->spih_gpio_ctrl, 0x00000000);	/* Disable GPIO for CS# */
+	SPIPCI_WREG(osh, &regs->spih_int_mask, 0x00000000);	/* Clear Intmask */
+	SPIPCI_WREG(osh, &regs->spih_hex_disp, 0x0000DEAF);
+	SPIPCI_WREG(osh, &regs->spih_int_edge, 0x00000000);
+	SPIPCI_WREG(osh, &regs->spih_int_pol, 0x00000000);
+	SPIPCI_WREG(osh, &regs->spih_hex_disp, 0x0000DEAD);
+
+	/* Disable interrupts through PCI Core. */
+	if (si->pciregs) {
+		SPIPCI_WREG(osh, &pciregs->ICR, 0x00000000);
+		spi_reg_unmap(osh, (uintptr)pciregs, sizeof(spih_pciregs_t));
+	}
+	spi_reg_unmap(osh, (uintptr)regs, sizeof(spih_regs_t));
+
+	MFREE(osh, si, sizeof(spih_info_t));
+
+	sd->controller = NULL;
+
+	sd_trace(("%s: exit\n", __FUNCTION__));
+	return TRUE;
+}
+
+/* Switch between internal (PCI) and external clock oscillator */
+static bool
+sdspi_switch_clock(sdioh_info_t *sd, bool ext_clk)
+{
+	spih_info_t *si = (spih_info_t *)sd->controller;
+	osl_t *osh = si->osh;
+	spih_regs_t *regs = si->regs;
+
+	/* Switch to desired clock, and reset the PLL. */
+	SPIPCI_WREG(osh, &regs->spih_pll_ctrl, ext_clk ? SPIH_EXT_CLK : 0);
+
+	SPINWAIT(((SPIPCI_RREG(osh, &regs->spih_pll_status) & SPIH_PLL_LOCKED)
+	          != SPIH_PLL_LOCKED), 1000);
+	if ((SPIPCI_RREG(osh, &regs->spih_pll_status) & SPIH_PLL_LOCKED) != SPIH_PLL_LOCKED) {
+		sd_err(("%s: timeout waiting for PLL to lock\n", __FUNCTION__));
+		return (FALSE);
+	}
+	return (TRUE);
+
+}
+
+/* Configure PCI-SPI Host Controller's SPI Clock rate as a divisor into the
+ * base clock rate.  The base clock is either the PCI Clock (33MHz) or the
+ * external clock oscillator at U17 on the PciSpiHost.
+ */
+bool
+spi_start_clock(sdioh_info_t *sd, uint16 div)
+{
+	spih_info_t *si = (spih_info_t *)sd->controller;
+	osl_t *osh = si->osh;
+	spih_regs_t *regs = si->regs;
+	uint32 t, espr, disp;
+	uint32 disp_xtal_freq;
+	bool	ext_clock = FALSE;
+	char disp_string[5];
+
+	if (div > 2048) {
+		sd_err(("%s: divisor %d too large; using max of 2048\n", __FUNCTION__, div));
+		div = 2048;
+	} else if (div & (div - 1)) {	/* Not a power of 2? */
+		/* Round up to a power of 2 */
+		while ((div + 1) & div)
+			div |= div >> 1;
+		div++;
+	}
+
+	/* For FPGA Rev >= 5, the use of an external clock oscillator is supported.
+	 * If the oscillator is populated, use it to provide the SPI base clock,
+	 * otherwise, default to the PCI clock as the SPI base clock.
+	 */
+	if (si->rev >= 5) {
+		uint32 clk_tick;
+		/* Enable the External Clock Oscillator as PLL clock source. */
+		if (!sdspi_switch_clock(sd, TRUE)) {
+			sd_err(("%s: error switching to external clock\n", __FUNCTION__));
+		}
+
+		/* Check to make sure the external clock is running.  If not, then it
+		 * is not populated on the card, so we will default to the PCI clock.
+		 */
+		clk_tick = SPIPCI_RREG(osh, &regs->spih_clk_count);
+		if (clk_tick == SPIPCI_RREG(osh, &regs->spih_clk_count)) {
+
+			/* Switch back to the PCI clock as the clock source. */
+			if (!sdspi_switch_clock(sd, FALSE)) {
+				sd_err(("%s: error switching to external clock\n", __FUNCTION__));
+			}
+		} else {
+			ext_clock = TRUE;
+		}
+	}
+
+	/* Hack to allow hot-swapping oscillators:
+	 * 1. Force PCI clock as clock source, using sd_divisor of 0.
+	 * 2. Swap oscillator
+	 * 3. Set desired sd_divisor (will switch to external oscillator as clock source.
+	 */
+	if (div == 0) {
+		ext_clock = FALSE;
+		div = 2;
+
+		/* Select PCI clock as the clock source. */
+		if (!sdspi_switch_clock(sd, FALSE)) {
+			sd_err(("%s: error switching to external clock\n", __FUNCTION__));
+		}
+
+		sd_err(("%s: Ok to hot-swap oscillators.\n", __FUNCTION__));
+	}
+
+	/* If using the external oscillator, read the clock frequency from the controller
+	 * The value read is in units of 10000Hz, and it's not a nice round number because
+	 * it is calculated by the FPGA.  So to make up for that, we round it off.
+	 */
+	if (ext_clock == TRUE) {
+		uint32 xtal_freq;
+
+		OSL_DELAY(1000);
+		xtal_freq = SPIPCI_RREG(osh, &regs->spih_xtal_freq) * 10000;
+
+		sd_info(("%s: Oscillator is %dHz\n", __FUNCTION__, xtal_freq));
+
+
+		disp_xtal_freq = xtal_freq / 10000;
+
+		/* Round it off to a nice number. */
+		if ((disp_xtal_freq % 100) > 50) {
+			disp_xtal_freq += 100;
+		}
+
+		disp_xtal_freq = (disp_xtal_freq / 100) * 100;
+	} else {
+		sd_err(("%s: no external oscillator installed, using PCI clock.\n", __FUNCTION__));
+		disp_xtal_freq = 3333;
+	}
+
+	/* Convert the SPI Clock frequency to BCD format. */
+	sprintf(disp_string, "%04d", disp_xtal_freq / div);
+
+	disp  = (disp_string[0] - '0') << 12;
+	disp |= (disp_string[1] - '0') << 8;
+	disp |= (disp_string[2] - '0') << 4;
+	disp |= (disp_string[3] - '0');
+
+	/* Select the correct ESPR register value based on the divisor. */
+	switch (div) {
+		case 1:		espr = 0x0; break;
+		case 2:		espr = 0x1; break;
+		case 4:		espr = 0x2; break;
+		case 8:		espr = 0x5; break;
+		case 16:	espr = 0x3; break;
+		case 32:	espr = 0x4; break;
+		case 64:	espr = 0x6; break;
+		case 128:	espr = 0x7; break;
+		case 256:	espr = 0x8; break;
+		case 512:	espr = 0x9; break;
+		case 1024:	espr = 0xa; break;
+		case 2048:	espr = 0xb; break;
+		default:	espr = 0x0; ASSERT(0); break;
+	}
+
+	t = SPIPCI_RREG(osh, &regs->spih_ctrl);
+	t &= ~3;
+	t |= espr & 3;
+	SPIPCI_WREG(osh, &regs->spih_ctrl, t);
+
+	t = SPIPCI_RREG(osh, &regs->spih_ext);
+	t &= ~3;
+	t |= (espr >> 2) & 3;
+	SPIPCI_WREG(osh, &regs->spih_ext, t);
+
+	SPIPCI_WREG(osh, &regs->spih_hex_disp, disp);
+
+	/* For Rev 8, writing to the PLL_CTRL register resets
+	 * the PLL, and it can re-acquire in 200uS.  For
+	 * Rev 7 and older, we use a software delay to allow
+	 * the PLL to re-acquire, which takes more than 2mS.
+	 */
+	if (si->rev < 8) {
+		/* Wait for clock to settle. */
+		OSL_DELAY(5000);
+	}
+
+	sd_info(("%s: SPI_CTRL=0x%08x SPI_EXT=0x%08x\n",
+	         __FUNCTION__,
+	         SPIPCI_RREG(osh, &regs->spih_ctrl),
+	         SPIPCI_RREG(osh, &regs->spih_ext)));
+
+	return TRUE;
+}
+
+/* Configure PCI-SPI Host Controller High-Speed Clocking mode setting */
+bool
+spi_controller_highspeed_mode(sdioh_info_t *sd, bool hsmode)
+{
+	spih_info_t *si = (spih_info_t *)sd->controller;
+	osl_t *osh = si->osh;
+	spih_regs_t *regs = si->regs;
+
+	if (si->rev >= 10) {
+		if (hsmode) {
+			SPIPCI_ORREG(osh, &regs->spih_ext, 0x10);
+		} else {
+			SPIPCI_ANDREG(osh, &regs->spih_ext, ~0x10);
+		}
+	}
+
+	return TRUE;
+}
+
+/* Disable device interrupt */
+void
+spi_devintr_off(sdioh_info_t *sd)
+{
+	spih_info_t *si = (spih_info_t *)sd->controller;
+	osl_t *osh = si->osh;
+	spih_regs_t *regs = si->regs;
+
+	sd_trace(("%s: %d\n", __FUNCTION__, sd->use_client_ints));
+	if (sd->use_client_ints) {
+		sd->intmask &= ~SPIH_DEV_INTR;
+		SPIPCI_WREG(osh, &regs->spih_int_mask, sd->intmask);	/* Clear Intmask */
+	}
+}
+
+/* Enable device interrupt */
+void
+spi_devintr_on(sdioh_info_t *sd)
+{
+	spih_info_t *si = (spih_info_t *)sd->controller;
+	osl_t *osh = si->osh;
+	spih_regs_t *regs = si->regs;
+
+	ASSERT(sd->lockcount == 0);
+	sd_trace(("%s: %d\n", __FUNCTION__, sd->use_client_ints));
+	if (sd->use_client_ints) {
+		if (SPIPCI_RREG(osh, &regs->spih_ctrl) & 0x02) {
+			/* Ack in case one was pending but is no longer... */
+			SPIPCI_WREG(osh, &regs->spih_int_status, SPIH_DEV_INTR);
+		}
+		sd->intmask |= SPIH_DEV_INTR;
+		/* Set device intr in Intmask */
+		SPIPCI_WREG(osh, &regs->spih_int_mask, sd->intmask);
+	}
+}
+
+/* Check to see if an interrupt belongs to the PCI-SPI Host or a SPI Device */
+bool
+spi_check_client_intr(sdioh_info_t *sd, int *is_dev_intr)
+{
+	spih_info_t *si = (spih_info_t *)sd->controller;
+	osl_t *osh = si->osh;
+	spih_regs_t *regs = si->regs;
+	bool ours = FALSE;
+
+	uint32 raw_int, cur_int;
+	ASSERT(sd);
+
+	if (is_dev_intr)
+		*is_dev_intr = FALSE;
+	raw_int = SPIPCI_RREG(osh, &regs->spih_int_status);
+	cur_int = raw_int & sd->intmask;
+	if (cur_int & SPIH_DEV_INTR) {
+		if (sd->client_intr_enabled && sd->use_client_ints) {
+			sd->intrcount++;
+			ASSERT(sd->intr_handler);
+			ASSERT(sd->intr_handler_arg);
+			(sd->intr_handler)(sd->intr_handler_arg);
+			if (is_dev_intr)
+				*is_dev_intr = TRUE;
+		} else {
+			sd_trace(("%s: Not ready for intr: enabled %d, handler 0x%p\n",
+			        __FUNCTION__, sd->client_intr_enabled, sd->intr_handler));
+		}
+		SPIPCI_WREG(osh, &regs->spih_int_status, SPIH_DEV_INTR);
+		SPIPCI_RREG(osh, &regs->spih_int_status);
+		ours = TRUE;
+	} else if (cur_int & SPIH_CTLR_INTR) {
+		/* Interrupt is from SPI FIFO... just clear and ack it... */
+		sd_trace(("%s: SPI CTLR interrupt: raw_int 0x%08x cur_int 0x%08x\n",
+		          __FUNCTION__, raw_int, cur_int));
+
+		/* Clear the interrupt in the SPI_STAT register */
+		SPIPCI_WREG(osh, &regs->spih_stat, 0x00000080);
+
+		/* Ack the interrupt in the interrupt controller */
+		SPIPCI_WREG(osh, &regs->spih_int_status, SPIH_CTLR_INTR);
+		SPIPCI_RREG(osh, &regs->spih_int_status);
+
+		ours = TRUE;
+	} else if (cur_int & SPIH_WFIFO_INTR) {
+		sd_trace(("%s: SPI WR FIFO Empty interrupt: raw_int 0x%08x cur_int 0x%08x\n",
+		          __FUNCTION__, raw_int, cur_int));
+
+		/* Disable the FIFO Empty Interrupt */
+		sd->intmask &= ~SPIH_WFIFO_INTR;
+		SPIPCI_WREG(osh, &regs->spih_int_mask, sd->intmask);
+
+		sd->local_intrcount++;
+		sd->got_hcint = TRUE;
+		ours = TRUE;
+	} else {
+		/* Not an error: can share interrupts... */
+		sd_trace(("%s: Not my interrupt: raw_int 0x%08x cur_int 0x%08x\n",
+		          __FUNCTION__, raw_int, cur_int));
+		ours = FALSE;
+	}
+
+	return ours;
+}
+
+static void
+hexdump(char *pfx, unsigned char *msg, int msglen)
+{
+	int i, col;
+	char buf[80];
+
+	ASSERT(strlen(pfx) + 49 <= sizeof(buf));
+
+	col = 0;
+
+	for (i = 0; i < msglen; i++, col++) {
+		if (col % 16 == 0)
+			strcpy(buf, pfx);
+		sprintf(buf + strlen(buf), "%02x", msg[i]);
+		if ((col + 1) % 16 == 0)
+			printf("%s\n", buf);
+		else
+			sprintf(buf + strlen(buf), " ");
+	}
+
+	if (col % 16 != 0)
+		printf("%s\n", buf);
+}
+
+/* Send/Receive an SPI Packet */
+void
+spi_sendrecv(sdioh_info_t *sd, uint8 *msg_out, uint8 *msg_in, int msglen)
+{
+	spih_info_t *si = (spih_info_t *)sd->controller;
+	osl_t *osh = si->osh;
+	spih_regs_t *regs = si->regs;
+	uint32 count;
+	uint32 spi_data_out;
+	uint32 spi_data_in;
+	bool yield;
+
+	sd_trace(("%s: enter\n", __FUNCTION__));
+
+	if (bcmpcispi_dump) {
+		printf("SENDRECV(len=%d)\n", msglen);
+		hexdump(" OUT: ", msg_out, msglen);
+	}
+
+#ifdef BCMSDYIELD
+	/* Only yield the CPU and wait for interrupt on Rev 8 and newer FPGA images. */
+	yield = ((msglen > 500) && (si->rev >= 8));
+#else
+	yield = FALSE;
+#endif /* BCMSDYIELD */
+
+	ASSERT(msglen % 4 == 0);
+
+
+	SPIPCI_ANDREG(osh, &regs->spih_gpio_data, ~SPIH_CS);	/* Set GPIO CS# Low (asserted) */
+
+	for (count = 0; count < (uint32)msglen/4; count++) {
+		spi_data_out = ((uint32)((uint32 *)msg_out)[count]);
+		SPIPCI_WREG(osh, &regs->spih_data, spi_data_out);
+	}
+
+#ifdef BCMSDYIELD
+	if (yield) {
+		/* Ack the interrupt in the interrupt controller */
+		SPIPCI_WREG(osh, &regs->spih_int_status, SPIH_WFIFO_INTR);
+		SPIPCI_RREG(osh, &regs->spih_int_status);
+
+		/* Enable the FIFO Empty Interrupt */
+		sd->intmask |= SPIH_WFIFO_INTR;
+		sd->got_hcint = FALSE;
+		SPIPCI_WREG(osh, &regs->spih_int_mask, sd->intmask);
+
+	}
+#endif /* BCMSDYIELD */
+
+	/* Wait for write fifo to empty... */
+	SPIPCI_ANDREG(osh, &regs->spih_gpio_data, ~0x00000020);	/* Set GPIO 5 Low */
+
+	if (yield) {
+		ASSERT((SPIPCI_RREG(sd->osh, &regs->spih_stat) & SPIH_WFEMPTY) == 0);
+	}
+
+	spi_waitbits(sd, yield);
+	SPIPCI_ORREG(osh, &regs->spih_gpio_data, 0x00000020);	/* Set GPIO 5 High (de-asserted) */
+
+	for (count = 0; count < (uint32)msglen/4; count++) {
+		spi_data_in = SPIPCI_RREG(osh, &regs->spih_data);
+		((uint32 *)msg_in)[count] = spi_data_in;
+	}
+
+	/* Set GPIO CS# High (de-asserted) */
+	SPIPCI_ORREG(osh, &regs->spih_gpio_data, SPIH_CS);
+
+	if (bcmpcispi_dump) {
+		hexdump(" IN : ", msg_in, msglen);
+	}
+}
+
+void
+spi_spinbits(sdioh_info_t *sd)
+{
+	spih_info_t *si = (spih_info_t *)sd->controller;
+	osl_t *osh = si->osh;
+	spih_regs_t *regs = si->regs;
+	uint spin_count; /* Spin loop bound check */
+
+	spin_count = 0;
+	while ((SPIPCI_RREG(sd->osh, &regs->spih_stat) & SPIH_WFEMPTY) == 0) {
+		if (spin_count > SPI_SPIN_BOUND) {
+			sd_err(("%s: SPIH_WFEMPTY spin bits out of bound %u times \n",
+				__FUNCTION__, spin_count));
+			ASSERT(FALSE);
+		}
+		spin_count++;
+	}
+
+	/* Wait for SPI Transfer state machine to return to IDLE state.
+	 * The state bits are only implemented in Rev >= 5 FPGA.  These
+	 * bits are hardwired to 00 for Rev < 5, so this check doesn't cause
+	 * any problems.
+	 */
+	spin_count = 0;
+	while ((SPIPCI_RREG(osh, &regs->spih_stat) & SPIH_STATE_MASK) != 0) {
+		if (spin_count > SPI_SPIN_BOUND) {
+			sd_err(("%s: SPIH_STATE_MASK spin bits out of bound %u times \n",
+				__FUNCTION__, spin_count));
+			ASSERT(FALSE);
+		}
+		spin_count++;
+	}
+}
diff --git a/drivers/net/wireless/bcm4329/bcmsdh.c b/drivers/net/wireless/bcm4329/bcmsdh.c
new file mode 100644
index 000000000000..4bf5889e5a68
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/bcmsdh.c
@@ -0,0 +1,652 @@
+/*
+ *  BCMSDH interface glue
+ *  implement bcmsdh API for SDIOH driver
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdh.c,v 1.35.2.1.4.8.6.13 2010/04/06 03:26:57 Exp $
+ */
+/* ****************** BCMSDH Interface Functions *************************** */
+
+#include <typedefs.h>
+#include <bcmdevs.h>
+#include <bcmendian.h>
+#include <bcmutils.h>
+#include <hndsoc.h>
+#include <siutils.h>
+#include <osl.h>
+
+#include <bcmsdh.h>	/* BRCM API for SDIO clients (such as wl, dhd) */
+#include <bcmsdbus.h>	/* common SDIO/controller interface */
+#include <sbsdio.h>	/* BRCM sdio device core */
+
+#include <sdio.h>	/* sdio spec */
+
+#define SDIOH_API_ACCESS_RETRY_LIMIT	2
+const uint bcmsdh_msglevel = BCMSDH_ERROR_VAL;
+
+
+struct bcmsdh_info
+{
+	bool	init_success;	/* underlying driver successfully attached */
+	void	*sdioh;		/* handler for sdioh */
+	uint32  vendevid;	/* Target Vendor and Device ID on SD bus */
+	osl_t   *osh;
+	bool	regfail;	/* Save status of last reg_read/reg_write call */
+	uint32	sbwad;		/* Save backplane window address */
+};
+/* local copy of bcm sd handler */
+bcmsdh_info_t * l_bcmsdh = NULL;
+
+#if defined(OOB_INTR_ONLY) && defined(HW_OOB)
+extern int
+sdioh_enable_hw_oob_intr(void *sdioh, bool enable);
+
+void
+bcmsdh_enable_hw_oob_intr(bcmsdh_info_t *sdh, bool enable)
+{
+	sdioh_enable_hw_oob_intr(sdh->sdioh, enable);
+}
+#endif
+
+bcmsdh_info_t *
+bcmsdh_attach(osl_t *osh, void *cfghdl, void **regsva, uint irq)
+{
+	bcmsdh_info_t *bcmsdh;
+
+	if ((bcmsdh = (bcmsdh_info_t *)MALLOC(osh, sizeof(bcmsdh_info_t))) == NULL) {
+		BCMSDH_ERROR(("bcmsdh_attach: out of memory, malloced %d bytes\n", MALLOCED(osh)));
+		return NULL;
+	}
+	bzero((char *)bcmsdh, sizeof(bcmsdh_info_t));
+
+	/* save the handler locally */
+	l_bcmsdh = bcmsdh;
+
+	if (!(bcmsdh->sdioh = sdioh_attach(osh, cfghdl, irq))) {
+		bcmsdh_detach(osh, bcmsdh);
+		return NULL;
+	}
+
+	bcmsdh->osh = osh;
+	bcmsdh->init_success = TRUE;
+
+	*regsva = (uint32 *)SI_ENUM_BASE;
+
+	/* Report the BAR, to fix if needed */
+	bcmsdh->sbwad = SI_ENUM_BASE;
+	return bcmsdh;
+}
+
+int
+bcmsdh_detach(osl_t *osh, void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	if (bcmsdh != NULL) {
+		if (bcmsdh->sdioh) {
+			sdioh_detach(osh, bcmsdh->sdioh);
+			bcmsdh->sdioh = NULL;
+		}
+		MFREE(osh, bcmsdh, sizeof(bcmsdh_info_t));
+	}
+
+	l_bcmsdh = NULL;
+	return 0;
+}
+
+int
+bcmsdh_iovar_op(void *sdh, const char *name,
+                void *params, int plen, void *arg, int len, bool set)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	return sdioh_iovar_op(bcmsdh->sdioh, name, params, plen, arg, len, set);
+}
+
+bool
+bcmsdh_intr_query(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	bool on;
+
+	ASSERT(bcmsdh);
+	status = sdioh_interrupt_query(bcmsdh->sdioh, &on);
+	if (SDIOH_API_SUCCESS(status))
+		return FALSE;
+	else
+		return on;
+}
+
+int
+bcmsdh_intr_enable(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	ASSERT(bcmsdh);
+
+	status = sdioh_interrupt_set(bcmsdh->sdioh, TRUE);
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_ERROR);
+}
+
+int
+bcmsdh_intr_disable(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	ASSERT(bcmsdh);
+
+	status = sdioh_interrupt_set(bcmsdh->sdioh, FALSE);
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_ERROR);
+}
+
+int
+bcmsdh_intr_reg(void *sdh, bcmsdh_cb_fn_t fn, void *argh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	ASSERT(bcmsdh);
+
+	status = sdioh_interrupt_register(bcmsdh->sdioh, fn, argh);
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_ERROR);
+}
+
+int
+bcmsdh_intr_dereg(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	ASSERT(bcmsdh);
+
+	status = sdioh_interrupt_deregister(bcmsdh->sdioh);
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_ERROR);
+}
+
+#if defined(DHD_DEBUG)
+bool
+bcmsdh_intr_pending(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	ASSERT(sdh);
+	return sdioh_interrupt_pending(bcmsdh->sdioh);
+}
+#endif
+
+
+int
+bcmsdh_devremove_reg(void *sdh, bcmsdh_cb_fn_t fn, void *argh)
+{
+	ASSERT(sdh);
+
+	/* don't support yet */
+	return BCME_UNSUPPORTED;
+}
+
+uint8
+bcmsdh_cfg_read(void *sdh, uint fnc_num, uint32 addr, int *err)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+#ifdef SDIOH_API_ACCESS_RETRY_LIMIT
+	int32 retry = 0;
+#endif
+	uint8 data = 0;
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	ASSERT(bcmsdh->init_success);
+
+#ifdef SDIOH_API_ACCESS_RETRY_LIMIT
+	do {
+		if (retry)	/* wait for 1 ms till bus get settled down */
+			OSL_DELAY(1000);
+#endif
+	status = sdioh_cfg_read(bcmsdh->sdioh, fnc_num, addr, (uint8 *)&data);
+#ifdef SDIOH_API_ACCESS_RETRY_LIMIT
+	} while (!SDIOH_API_SUCCESS(status) && (retry++ < SDIOH_API_ACCESS_RETRY_LIMIT));
+#endif
+	if (err)
+		*err = (SDIOH_API_SUCCESS(status) ? 0 : BCME_SDIO_ERROR);
+
+	BCMSDH_INFO(("%s:fun = %d, addr = 0x%x, uint8data = 0x%x\n", __FUNCTION__,
+	            fnc_num, addr, data));
+
+	return data;
+}
+
+void
+bcmsdh_cfg_write(void *sdh, uint fnc_num, uint32 addr, uint8 data, int *err)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+#ifdef SDIOH_API_ACCESS_RETRY_LIMIT
+	int32 retry = 0;
+#endif
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	ASSERT(bcmsdh->init_success);
+
+#ifdef SDIOH_API_ACCESS_RETRY_LIMIT
+	do {
+		if (retry)	/* wait for 1 ms till bus get settled down */
+			OSL_DELAY(1000);
+#endif
+	status = sdioh_cfg_write(bcmsdh->sdioh, fnc_num, addr, (uint8 *)&data);
+#ifdef SDIOH_API_ACCESS_RETRY_LIMIT
+	} while (!SDIOH_API_SUCCESS(status) && (retry++ < SDIOH_API_ACCESS_RETRY_LIMIT));
+#endif
+	if (err)
+		*err = SDIOH_API_SUCCESS(status) ? 0 : BCME_SDIO_ERROR;
+
+	BCMSDH_INFO(("%s:fun = %d, addr = 0x%x, uint8data = 0x%x\n", __FUNCTION__,
+	            fnc_num, addr, data));
+}
+
+uint32
+bcmsdh_cfg_read_word(void *sdh, uint fnc_num, uint32 addr, int *err)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	uint32 data = 0;
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	ASSERT(bcmsdh->init_success);
+
+	status = sdioh_request_word(bcmsdh->sdioh, SDIOH_CMD_TYPE_NORMAL, SDIOH_READ, fnc_num,
+	                            addr, &data, 4);
+
+	if (err)
+		*err = (SDIOH_API_SUCCESS(status) ? 0 : BCME_SDIO_ERROR);
+
+	BCMSDH_INFO(("%s:fun = %d, addr = 0x%x, uint32data = 0x%x\n", __FUNCTION__,
+	            fnc_num, addr, data));
+
+	return data;
+}
+
+void
+bcmsdh_cfg_write_word(void *sdh, uint fnc_num, uint32 addr, uint32 data, int *err)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	ASSERT(bcmsdh->init_success);
+
+	status = sdioh_request_word(bcmsdh->sdioh, SDIOH_CMD_TYPE_NORMAL, SDIOH_WRITE, fnc_num,
+	                            addr, &data, 4);
+
+	if (err)
+		*err = (SDIOH_API_SUCCESS(status) ? 0 : BCME_SDIO_ERROR);
+
+	BCMSDH_INFO(("%s:fun = %d, addr = 0x%x, uint32data = 0x%x\n", __FUNCTION__, fnc_num,
+	             addr, data));
+}
+
+
+int
+bcmsdh_cis_read(void *sdh, uint func, uint8 *cis, uint length)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+
+	uint8 *tmp_buf, *tmp_ptr;
+	uint8 *ptr;
+	bool ascii = func & ~0xf;
+	func &= 0x7;
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	ASSERT(bcmsdh->init_success);
+	ASSERT(cis);
+	ASSERT(length <= SBSDIO_CIS_SIZE_LIMIT);
+
+	status = sdioh_cis_read(bcmsdh->sdioh, func, cis, length);
+
+	if (ascii) {
+		/* Move binary bits to tmp and format them into the provided buffer. */
+		if ((tmp_buf = (uint8 *)MALLOC(bcmsdh->osh, length)) == NULL) {
+			BCMSDH_ERROR(("%s: out of memory\n", __FUNCTION__));
+			return BCME_NOMEM;
+		}
+		bcopy(cis, tmp_buf, length);
+		for (tmp_ptr = tmp_buf, ptr = cis; ptr < (cis + length - 4); tmp_ptr++) {
+			ptr += sprintf((char*)ptr, "%.2x ", *tmp_ptr & 0xff);
+			if ((((tmp_ptr - tmp_buf) + 1) & 0xf) == 0)
+				ptr += sprintf((char *)ptr, "\n");
+		}
+		MFREE(bcmsdh->osh, tmp_buf, length);
+	}
+
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_ERROR);
+}
+
+
+static int
+bcmsdhsdio_set_sbaddr_window(void *sdh, uint32 address)
+{
+	int err = 0;
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	bcmsdh_cfg_write(bcmsdh, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRLOW,
+	                 (address >> 8) & SBSDIO_SBADDRLOW_MASK, &err);
+	if (!err)
+		bcmsdh_cfg_write(bcmsdh, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRMID,
+		                 (address >> 16) & SBSDIO_SBADDRMID_MASK, &err);
+	if (!err)
+		bcmsdh_cfg_write(bcmsdh, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRHIGH,
+		                 (address >> 24) & SBSDIO_SBADDRHIGH_MASK, &err);
+
+
+	return err;
+}
+
+uint32
+bcmsdh_reg_read(void *sdh, uint32 addr, uint size)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	uint32 word = 0;
+	uint bar0 = addr & ~SBSDIO_SB_OFT_ADDR_MASK;
+
+	BCMSDH_INFO(("%s:fun = 1, addr = 0x%x, ", __FUNCTION__, addr));
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	ASSERT(bcmsdh->init_success);
+
+	if (bar0 != bcmsdh->sbwad) {
+		if (bcmsdhsdio_set_sbaddr_window(bcmsdh, bar0))
+			return 0xFFFFFFFF;
+
+		bcmsdh->sbwad = bar0;
+	}
+
+	addr &= SBSDIO_SB_OFT_ADDR_MASK;
+	if (size == 4)
+		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
+
+	status = sdioh_request_word(bcmsdh->sdioh, SDIOH_CMD_TYPE_NORMAL,
+		SDIOH_READ, SDIO_FUNC_1, addr, &word, size);
+
+	bcmsdh->regfail = !(SDIOH_API_SUCCESS(status));
+
+	BCMSDH_INFO(("uint32data = 0x%x\n", word));
+
+	/* if ok, return appropriately masked word */
+	if (SDIOH_API_SUCCESS(status)) {
+		switch (size) {
+			case sizeof(uint8):
+				return (word & 0xff);
+			case sizeof(uint16):
+				return (word & 0xffff);
+			case sizeof(uint32):
+				return word;
+			default:
+				bcmsdh->regfail = TRUE;
+
+		}
+	}
+
+	/* otherwise, bad sdio access or invalid size */
+	BCMSDH_ERROR(("%s: error reading addr 0x%04x size %d\n", __FUNCTION__, addr, size));
+	return 0xFFFFFFFF;
+}
+
+uint32
+bcmsdh_reg_write(void *sdh, uint32 addr, uint size, uint32 data)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	uint bar0 = addr & ~SBSDIO_SB_OFT_ADDR_MASK;
+	int err = 0;
+
+	BCMSDH_INFO(("%s:fun = 1, addr = 0x%x, uint%ddata = 0x%x\n",
+	             __FUNCTION__, addr, size*8, data));
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	ASSERT(bcmsdh->init_success);
+
+	if (bar0 != bcmsdh->sbwad) {
+		if ((err = bcmsdhsdio_set_sbaddr_window(bcmsdh, bar0)))
+			return err;
+
+		bcmsdh->sbwad = bar0;
+	}
+
+	addr &= SBSDIO_SB_OFT_ADDR_MASK;
+	if (size == 4)
+		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
+	status = sdioh_request_word(bcmsdh->sdioh, SDIOH_CMD_TYPE_NORMAL, SDIOH_WRITE, SDIO_FUNC_1,
+	                            addr, &data, size);
+	bcmsdh->regfail = !(SDIOH_API_SUCCESS(status));
+
+	if (SDIOH_API_SUCCESS(status))
+		return 0;
+
+	BCMSDH_ERROR(("%s: error writing 0x%08x to addr 0x%04x size %d\n",
+	              __FUNCTION__, data, addr, size));
+	return 0xFFFFFFFF;
+}
+
+bool
+bcmsdh_regfail(void *sdh)
+{
+	return ((bcmsdh_info_t *)sdh)->regfail;
+}
+
+int
+bcmsdh_recv_buf(void *sdh, uint32 addr, uint fn, uint flags,
+                uint8 *buf, uint nbytes, void *pkt,
+                bcmsdh_cmplt_fn_t complete, void *handle)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	uint incr_fix;
+	uint width;
+	uint bar0 = addr & ~SBSDIO_SB_OFT_ADDR_MASK;
+	int err = 0;
+
+	ASSERT(bcmsdh);
+	ASSERT(bcmsdh->init_success);
+
+	BCMSDH_INFO(("%s:fun = %d, addr = 0x%x, size = %d\n",
+	             __FUNCTION__, fn, addr, nbytes));
+
+	/* Async not implemented yet */
+	ASSERT(!(flags & SDIO_REQ_ASYNC));
+	if (flags & SDIO_REQ_ASYNC)
+		return BCME_UNSUPPORTED;
+
+	if (bar0 != bcmsdh->sbwad) {
+		if ((err = bcmsdhsdio_set_sbaddr_window(bcmsdh, bar0)))
+			return err;
+
+		bcmsdh->sbwad = bar0;
+	}
+
+	addr &= SBSDIO_SB_OFT_ADDR_MASK;
+
+	incr_fix = (flags & SDIO_REQ_FIXED) ? SDIOH_DATA_FIX : SDIOH_DATA_INC;
+	width = (flags & SDIO_REQ_4BYTE) ? 4 : 2;
+	if (width == 4)
+		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
+
+	status = sdioh_request_buffer(bcmsdh->sdioh, SDIOH_DATA_PIO, incr_fix,
+	                              SDIOH_READ, fn, addr, width, nbytes, buf, pkt);
+
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_SDIO_ERROR);
+}
+
+int
+bcmsdh_send_buf(void *sdh, uint32 addr, uint fn, uint flags,
+                uint8 *buf, uint nbytes, void *pkt,
+                bcmsdh_cmplt_fn_t complete, void *handle)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	uint incr_fix;
+	uint width;
+	uint bar0 = addr & ~SBSDIO_SB_OFT_ADDR_MASK;
+	int err = 0;
+
+	ASSERT(bcmsdh);
+	ASSERT(bcmsdh->init_success);
+
+	BCMSDH_INFO(("%s:fun = %d, addr = 0x%x, size = %d\n",
+	            __FUNCTION__, fn, addr, nbytes));
+
+	/* Async not implemented yet */
+	ASSERT(!(flags & SDIO_REQ_ASYNC));
+	if (flags & SDIO_REQ_ASYNC)
+		return BCME_UNSUPPORTED;
+
+	if (bar0 != bcmsdh->sbwad) {
+		if ((err = bcmsdhsdio_set_sbaddr_window(bcmsdh, bar0)))
+			return err;
+
+		bcmsdh->sbwad = bar0;
+	}
+
+	addr &= SBSDIO_SB_OFT_ADDR_MASK;
+
+	incr_fix = (flags & SDIO_REQ_FIXED) ? SDIOH_DATA_FIX : SDIOH_DATA_INC;
+	width = (flags & SDIO_REQ_4BYTE) ? 4 : 2;
+	if (width == 4)
+		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
+
+	status = sdioh_request_buffer(bcmsdh->sdioh, SDIOH_DATA_PIO, incr_fix,
+	                              SDIOH_WRITE, fn, addr, width, nbytes, buf, pkt);
+
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_ERROR);
+}
+
+int
+bcmsdh_rwdata(void *sdh, uint rw, uint32 addr, uint8 *buf, uint nbytes)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+
+	ASSERT(bcmsdh);
+	ASSERT(bcmsdh->init_success);
+	ASSERT((addr & SBSDIO_SBWINDOW_MASK) == 0);
+
+	addr &= SBSDIO_SB_OFT_ADDR_MASK;
+	addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
+
+	status = sdioh_request_buffer(bcmsdh->sdioh, SDIOH_DATA_PIO, SDIOH_DATA_INC,
+	                              (rw ? SDIOH_WRITE : SDIOH_READ), SDIO_FUNC_1,
+	                              addr, 4, nbytes, buf, NULL);
+
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_ERROR);
+}
+
+int
+bcmsdh_abort(void *sdh, uint fn)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	return sdioh_abort(bcmsdh->sdioh, fn);
+}
+
+int
+bcmsdh_start(void *sdh, int stage)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	return sdioh_start(bcmsdh->sdioh, stage);
+}
+
+int
+bcmsdh_stop(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	return sdioh_stop(bcmsdh->sdioh);
+}
+
+
+int
+bcmsdh_query_device(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	bcmsdh->vendevid = (VENDOR_BROADCOM << 16) | 0;
+	return (bcmsdh->vendevid);
+}
+
+uint
+bcmsdh_query_iofnum(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	return (sdioh_query_iofnum(bcmsdh->sdioh));
+}
+
+int
+bcmsdh_reset(bcmsdh_info_t *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	return sdioh_sdio_reset(bcmsdh->sdioh);
+}
+
+void *bcmsdh_get_sdioh(bcmsdh_info_t *sdh)
+{
+	ASSERT(sdh);
+	return sdh->sdioh;
+}
+
+/* Function to pass device-status bits to DHD. */
+uint32
+bcmsdh_get_dstatus(void *sdh)
+{
+	return 0;
+}
+uint32
+bcmsdh_cur_sbwad(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	return (bcmsdh->sbwad);
+}
+
+void
+bcmsdh_chipinfo(void *sdh, uint32 chip, uint32 chiprev)
+{
+	return;
+}
diff --git a/drivers/net/wireless/bcm4329/bcmsdh_linux.c b/drivers/net/wireless/bcm4329/bcmsdh_linux.c
new file mode 100644
index 000000000000..22e1e9c7997c
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/bcmsdh_linux.c
@@ -0,0 +1,735 @@
+/*
+ * SDIO access interface for drivers - linux specific (pci only)
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdh_linux.c,v 1.42.10.10.2.14.4.2 2010/09/15 00:30:11 Exp $
+ */
+
+/**
+ * @file bcmsdh_linux.c
+ */
+
+#define __UNDEF_NO_VERSION__
+
+#include <typedefs.h>
+#include <linuxver.h>
+
+#include <linux/pci.h>
+#include <linux/completion.h>
+
+#include <osl.h>
+#include <pcicfg.h>
+#include <bcmdefs.h>
+#include <bcmdevs.h>
+
+#if defined(OOB_INTR_ONLY)
+#include <linux/irq.h>
+extern void dhdsdio_isr(void * args);
+#include <bcmutils.h>
+#include <dngl_stats.h>
+#include <dhd.h>
+#endif /* defined(OOB_INTR_ONLY) */
+#if defined(CONFIG_MACH_SANDGATE2G) || defined(CONFIG_MACH_LOGICPD_PXA270)
+#if !defined(BCMPLATFORM_BUS)
+#define BCMPLATFORM_BUS
+#endif /* !defined(BCMPLATFORM_BUS) */
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19))
+#include <linux/platform_device.h>
+#endif /* KERNEL_VERSION(2, 6, 19) */
+#endif /* CONFIG_MACH_SANDGATE2G || CONFIG_MACH_LOGICPD_PXA270 */
+
+/**
+ * SDIO Host Controller info
+ */
+typedef struct bcmsdh_hc bcmsdh_hc_t;
+
+struct bcmsdh_hc {
+	bcmsdh_hc_t *next;
+#ifdef BCMPLATFORM_BUS
+	struct device *dev;			/* platform device handle */
+#else
+	struct pci_dev *dev;		/* pci device handle */
+#endif /* BCMPLATFORM_BUS */
+	osl_t *osh;
+	void *regs;			/* SDIO Host Controller address */
+	bcmsdh_info_t *sdh;		/* SDIO Host Controller handle */
+	void *ch;
+	unsigned int oob_irq;
+	unsigned long oob_flags; /* OOB Host specifiction as edge and etc */
+	bool oob_irq_registered;
+#if defined(OOB_INTR_ONLY)
+	spinlock_t irq_lock;
+#endif
+};
+static bcmsdh_hc_t *sdhcinfo = NULL;
+
+/* driver info, initialized when bcmsdh_register is called */
+static bcmsdh_driver_t drvinfo = {NULL, NULL};
+
+/* debugging macros */
+#define SDLX_MSG(x)
+
+/**
+ * Checks to see if vendor and device IDs match a supported SDIO Host Controller.
+ */
+bool
+bcmsdh_chipmatch(uint16 vendor, uint16 device)
+{
+	/* Add other vendors and devices as required */
+
+#ifdef BCMSDIOH_STD
+	/* Check for Arasan host controller */
+	if (vendor == VENDOR_SI_IMAGE) {
+		return (TRUE);
+	}
+	/* Check for BRCM 27XX Standard host controller */
+	if (device == BCM27XX_SDIOH_ID && vendor == VENDOR_BROADCOM) {
+		return (TRUE);
+	}
+	/* Check for BRCM Standard host controller */
+	if (device == SDIOH_FPGA_ID && vendor == VENDOR_BROADCOM) {
+		return (TRUE);
+	}
+	/* Check for TI PCIxx21 Standard host controller */
+	if (device == PCIXX21_SDIOH_ID && vendor == VENDOR_TI) {
+		return (TRUE);
+	}
+	if (device == PCIXX21_SDIOH0_ID && vendor == VENDOR_TI) {
+		return (TRUE);
+	}
+	/* Ricoh R5C822 Standard SDIO Host */
+	if (device == R5C822_SDIOH_ID && vendor == VENDOR_RICOH) {
+		return (TRUE);
+	}
+	/* JMicron Standard SDIO Host */
+	if (device == JMICRON_SDIOH_ID && vendor == VENDOR_JMICRON) {
+		return (TRUE);
+	}
+
+#endif /* BCMSDIOH_STD */
+#ifdef BCMSDIOH_SPI
+	/* This is the PciSpiHost. */
+	if (device == SPIH_FPGA_ID && vendor == VENDOR_BROADCOM) {
+		printf("Found PCI SPI Host Controller\n");
+		return (TRUE);
+	}
+
+#endif /* BCMSDIOH_SPI */
+
+	return (FALSE);
+}
+
+#if defined(BCMPLATFORM_BUS)
+#if defined(BCMLXSDMMC)
+/* forward declarations */
+int bcmsdh_probe(struct device *dev);
+int bcmsdh_remove(struct device *dev);
+
+EXPORT_SYMBOL(bcmsdh_probe);
+EXPORT_SYMBOL(bcmsdh_remove);
+
+#else
+/* forward declarations */
+static int __devinit bcmsdh_probe(struct device *dev);
+static int __devexit bcmsdh_remove(struct device *dev);
+#endif /* BCMLXSDMMC */
+
+#ifndef BCMLXSDMMC
+static struct device_driver bcmsdh_driver = {
+	.name		= "pxa2xx-mci",
+	.bus		= &platform_bus_type,
+	.probe		= bcmsdh_probe,
+	.remove		= bcmsdh_remove,
+	.suspend	= NULL,
+	.resume		= NULL,
+	};
+#endif /* BCMLXSDMMC */
+
+#ifndef BCMLXSDMMC
+static
+#endif /* BCMLXSDMMC */
+int bcmsdh_probe(struct device *dev)
+{
+	osl_t *osh = NULL;
+	bcmsdh_hc_t *sdhc = NULL;
+	ulong regs = 0;
+	bcmsdh_info_t *sdh = NULL;
+#if !defined(BCMLXSDMMC) && defined(BCMPLATFORM_BUS)
+	struct platform_device *pdev;
+	struct resource *r;
+#endif /* BCMLXSDMMC */
+	int irq = 0;
+	uint32 vendevid;
+	unsigned long irq_flags = 0;
+
+#if !defined(BCMLXSDMMC) && defined(BCMPLATFORM_BUS)
+	pdev = to_platform_device(dev);
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq = platform_get_irq(pdev, 0);
+	if (!r || irq == NO_IRQ)
+		return -ENXIO;
+#endif /* BCMLXSDMMC */
+
+#if defined(OOB_INTR_ONLY)
+#ifdef HW_OOB
+	irq_flags = \
+		IORESOURCE_IRQ | IORESOURCE_IRQ_HIGHLEVEL | IORESOURCE_IRQ_SHAREABLE;
+#else
+	 irq_flags = IRQF_TRIGGER_FALLING;
+#endif /* HW_OOB */
+	irq = dhd_customer_oob_irq_map(&irq_flags);
+	if  (irq < 0) {
+		SDLX_MSG(("%s: Host irq is not defined\n", __FUNCTION__));
+		return 1;
+	}
+#endif /* defined(OOB_INTR_ONLY) */
+	/* allocate SDIO Host Controller state info */
+	if (!(osh = osl_attach(dev, PCI_BUS, FALSE))) {
+		SDLX_MSG(("%s: osl_attach failed\n", __FUNCTION__));
+		goto err;
+	}
+	if (!(sdhc = MALLOC(osh, sizeof(bcmsdh_hc_t)))) {
+		SDLX_MSG(("%s: out of memory, allocated %d bytes\n",
+			__FUNCTION__,
+			MALLOCED(osh)));
+		goto err;
+	}
+	bzero(sdhc, sizeof(bcmsdh_hc_t));
+	sdhc->osh = osh;
+
+	sdhc->dev = (void *)dev;
+
+#ifdef BCMLXSDMMC
+	if (!(sdh = bcmsdh_attach(osh, (void *)0,
+	                          (void **)&regs, irq))) {
+		SDLX_MSG(("%s: bcmsdh_attach failed\n", __FUNCTION__));
+		goto err;
+	}
+#else
+	if (!(sdh = bcmsdh_attach(osh, (void *)r->start,
+	                          (void **)&regs, irq))) {
+		SDLX_MSG(("%s: bcmsdh_attach failed\n", __FUNCTION__));
+		goto err;
+	}
+#endif /* BCMLXSDMMC */
+	sdhc->sdh = sdh;
+	sdhc->oob_irq = irq;
+	sdhc->oob_flags = irq_flags;
+	sdhc->oob_irq_registered = FALSE;	/* to make sure.. */
+#if defined(OOB_INTR_ONLY)
+	spin_lock_init(&sdhc->irq_lock);
+#endif
+
+	/* chain SDIO Host Controller info together */
+	sdhc->next = sdhcinfo;
+	sdhcinfo = sdhc;
+	/* Read the vendor/device ID from the CIS */
+	vendevid = bcmsdh_query_device(sdh);
+
+	/* try to attach to the target device */
+	if (!(sdhc->ch = drvinfo.attach((vendevid >> 16),
+	                                 (vendevid & 0xFFFF), 0, 0, 0, 0,
+	                                (void *)regs, NULL, sdh, dev))) {
+		SDLX_MSG(("%s: device attach failed\n", __FUNCTION__));
+		goto err;
+	}
+
+	return 0;
+
+	/* error handling */
+err:
+	if (sdhc) {
+		if (sdhc->sdh)
+			bcmsdh_detach(sdhc->osh, sdhc->sdh);
+		MFREE(osh, sdhc, sizeof(bcmsdh_hc_t));
+	}
+	if (osh)
+		osl_detach(osh);
+	return -ENODEV;
+}
+
+#ifndef BCMLXSDMMC
+static
+#endif /* BCMLXSDMMC */
+int bcmsdh_remove(struct device *dev)
+{
+	bcmsdh_hc_t *sdhc, *prev;
+	osl_t *osh;
+
+	sdhc = sdhcinfo;
+	drvinfo.detach(sdhc->ch);
+	bcmsdh_detach(sdhc->osh, sdhc->sdh);
+	/* find the SDIO Host Controller state for this pdev and take it out from the list */
+	for (sdhc = sdhcinfo, prev = NULL; sdhc; sdhc = sdhc->next) {
+		if (sdhc->dev == (void *)dev) {
+			if (prev)
+				prev->next = sdhc->next;
+			else
+				sdhcinfo = NULL;
+			break;
+		}
+		prev = sdhc;
+	}
+	if (!sdhc) {
+		SDLX_MSG(("%s: failed\n", __FUNCTION__));
+		return 0;
+	}
+
+
+	/* release SDIO Host Controller info */
+	osh = sdhc->osh;
+	MFREE(osh, sdhc, sizeof(bcmsdh_hc_t));
+	osl_detach(osh);
+
+#if !defined(BCMLXSDMMC) || defined(OOB_INTR_ONLY)
+	dev_set_drvdata(dev, NULL);
+#endif /* !defined(BCMLXSDMMC) */
+
+	return 0;
+}
+
+#else /* BCMPLATFORM_BUS */
+
+#if !defined(BCMLXSDMMC)
+/* forward declarations for PCI probe and remove functions. */
+static int __devinit bcmsdh_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent);
+static void __devexit bcmsdh_pci_remove(struct pci_dev *pdev);
+
+/**
+ * pci id table
+ */
+static struct pci_device_id bcmsdh_pci_devid[] __devinitdata = {
+	{ vendor: PCI_ANY_ID,
+	device: PCI_ANY_ID,
+	subvendor: PCI_ANY_ID,
+	subdevice: PCI_ANY_ID,
+	class: 0,
+	class_mask: 0,
+	driver_data: 0,
+	},
+	{ 0, }
+};
+MODULE_DEVICE_TABLE(pci, bcmsdh_pci_devid);
+
+/**
+ * SDIO Host Controller pci driver info
+ */
+static struct pci_driver bcmsdh_pci_driver = {
+	node:		{},
+	name:		"bcmsdh",
+	id_table:	bcmsdh_pci_devid,
+	probe:		bcmsdh_pci_probe,
+	remove:		bcmsdh_pci_remove,
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
+	save_state:	NULL,
+#endif
+	suspend:	NULL,
+	resume:		NULL,
+};
+
+
+extern uint sd_pci_slot;	/* Force detection to a particular PCI */
+				/* slot only . Allows for having multiple */
+				/* WL devices at once in a PC */
+				/* Only one instance of dhd will be */
+				/* usable at a time */
+				/* Upper word is bus number, */
+				/* lower word is slot number */
+				/* Default value of 0xFFFFffff turns this */
+				/* off */
+module_param(sd_pci_slot, uint, 0);
+
+
+/**
+ * Detect supported SDIO Host Controller and attach if found.
+ *
+ * Determine if the device described by pdev is a supported SDIO Host
+ * Controller.  If so, attach to it and attach to the target device.
+ */
+static int __devinit
+bcmsdh_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	osl_t *osh = NULL;
+	bcmsdh_hc_t *sdhc = NULL;
+	ulong regs;
+	bcmsdh_info_t *sdh = NULL;
+	int rc;
+
+	if (sd_pci_slot != 0xFFFFffff) {
+		if (pdev->bus->number != (sd_pci_slot>>16) ||
+			PCI_SLOT(pdev->devfn) != (sd_pci_slot&0xffff)) {
+			SDLX_MSG(("%s: %s: bus %X, slot %X, vend %X, dev %X\n",
+			          __FUNCTION__,
+			          bcmsdh_chipmatch(pdev->vendor, pdev->device) ?
+			          "Found compatible SDIOHC" :
+			          "Probing unknown device",
+			          pdev->bus->number, PCI_SLOT(pdev->devfn),
+			          pdev->vendor, pdev->device));
+			return -ENODEV;
+		}
+		SDLX_MSG(("%s: %s: bus %X, slot %X, vendor %X, device %X (good PCI location)\n",
+		          __FUNCTION__,
+		          bcmsdh_chipmatch(pdev->vendor, pdev->device) ?
+		          "Using compatible SDIOHC" :
+		          "WARNING, forced use of unkown device",
+		          pdev->bus->number, PCI_SLOT(pdev->devfn),
+		          pdev->vendor, pdev->device));
+	}
+
+	if ((pdev->vendor == VENDOR_TI) && ((pdev->device == PCIXX21_FLASHMEDIA_ID) ||
+	    (pdev->device == PCIXX21_FLASHMEDIA0_ID))) {
+		uint32 config_reg;
+
+		SDLX_MSG(("%s: Disabling TI FlashMedia Controller.\n", __FUNCTION__));
+		if (!(osh = osl_attach(pdev, PCI_BUS, FALSE))) {
+			SDLX_MSG(("%s: osl_attach failed\n", __FUNCTION__));
+			goto err;
+		}
+
+		config_reg = OSL_PCI_READ_CONFIG(osh, 0x4c, 4);
+
+		/*
+		 * Set MMC_SD_DIS bit in FlashMedia Controller.
+		 * Disbling the SD/MMC Controller in the FlashMedia Controller
+		 * allows the Standard SD Host Controller to take over control
+		 * of the SD Slot.
+		 */
+		config_reg |= 0x02;
+		OSL_PCI_WRITE_CONFIG(osh, 0x4c, 4, config_reg);
+		osl_detach(osh);
+	}
+	/* match this pci device with what we support */
+	/* we can't solely rely on this to believe it is our SDIO Host Controller! */
+	if (!bcmsdh_chipmatch(pdev->vendor, pdev->device)) {
+		return -ENODEV;
+	}
+
+	/* this is a pci device we might support */
+	SDLX_MSG(("%s: Found possible SDIO Host Controller: bus %d slot %d func %d irq %d\n",
+		__FUNCTION__,
+		pdev->bus->number, PCI_SLOT(pdev->devfn),
+		PCI_FUNC(pdev->devfn), pdev->irq));
+
+	/* use bcmsdh_query_device() to get the vendor ID of the target device so
+	 * it will eventually appear in the Broadcom string on the console
+	 */
+
+	/* allocate SDIO Host Controller state info */
+	if (!(osh = osl_attach(pdev, PCI_BUS, FALSE))) {
+		SDLX_MSG(("%s: osl_attach failed\n", __FUNCTION__));
+		goto err;
+	}
+	if (!(sdhc = MALLOC(osh, sizeof(bcmsdh_hc_t)))) {
+		SDLX_MSG(("%s: out of memory, allocated %d bytes\n",
+			__FUNCTION__,
+			MALLOCED(osh)));
+		goto err;
+	}
+	bzero(sdhc, sizeof(bcmsdh_hc_t));
+	sdhc->osh = osh;
+
+	sdhc->dev = pdev;
+
+	/* map to address where host can access */
+	pci_set_master(pdev);
+	rc = pci_enable_device(pdev);
+	if (rc) {
+		SDLX_MSG(("%s: Cannot enable PCI device\n", __FUNCTION__));
+		goto err;
+	}
+	if (!(sdh = bcmsdh_attach(osh, (void *)(uintptr)pci_resource_start(pdev, 0),
+	                          (void **)&regs, pdev->irq))) {
+		SDLX_MSG(("%s: bcmsdh_attach failed\n", __FUNCTION__));
+		goto err;
+	}
+
+	sdhc->sdh = sdh;
+
+	/* try to attach to the target device */
+	if (!(sdhc->ch = drvinfo.attach(VENDOR_BROADCOM, /* pdev->vendor, */
+	                                bcmsdh_query_device(sdh) & 0xFFFF, 0, 0, 0, 0,
+	                                (void *)regs, NULL, sdh, pdev->dev))) {
+		SDLX_MSG(("%s: device attach failed\n", __FUNCTION__));
+		goto err;
+	}
+
+	/* chain SDIO Host Controller info together */
+	sdhc->next = sdhcinfo;
+	sdhcinfo = sdhc;
+
+	return 0;
+
+	/* error handling */
+err:
+	if (sdhc->sdh)
+		bcmsdh_detach(sdhc->osh, sdhc->sdh);
+	if (sdhc)
+		MFREE(osh, sdhc, sizeof(bcmsdh_hc_t));
+	if (osh)
+		osl_detach(osh);
+	return -ENODEV;
+}
+
+
+/**
+ * Detach from target devices and SDIO Host Controller
+ */
+static void __devexit
+bcmsdh_pci_remove(struct pci_dev *pdev)
+{
+	bcmsdh_hc_t *sdhc, *prev;
+	osl_t *osh;
+
+	/* find the SDIO Host Controller state for this pdev and take it out from the list */
+	for (sdhc = sdhcinfo, prev = NULL; sdhc; sdhc = sdhc->next) {
+		if (sdhc->dev == pdev) {
+			if (prev)
+				prev->next = sdhc->next;
+			else
+				sdhcinfo = NULL;
+			break;
+		}
+		prev = sdhc;
+	}
+	if (!sdhc)
+		return;
+
+	drvinfo.detach(sdhc->ch);
+
+	bcmsdh_detach(sdhc->osh, sdhc->sdh);
+
+	/* release SDIO Host Controller info */
+	osh = sdhc->osh;
+	MFREE(osh, sdhc, sizeof(bcmsdh_hc_t));
+	osl_detach(osh);
+}
+#endif /* BCMLXSDMMC */
+#endif /* BCMPLATFORM_BUS */
+
+extern int sdio_function_init(void);
+
+int
+bcmsdh_register(bcmsdh_driver_t *driver)
+{
+	int error = 0;
+
+	drvinfo = *driver;
+
+#if defined(BCMPLATFORM_BUS)
+#if defined(BCMLXSDMMC)
+	SDLX_MSG(("Linux Kernel SDIO/MMC Driver\n"));
+	error = sdio_function_init();
+#else
+	SDLX_MSG(("Intel PXA270 SDIO Driver\n"));
+	error = driver_register(&bcmsdh_driver);
+#endif /* defined(BCMLXSDMMC) */
+	return error;
+#endif /* defined(BCMPLATFORM_BUS) */
+
+#if !defined(BCMPLATFORM_BUS) && !defined(BCMLXSDMMC)
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
+	if (!(error = pci_module_init(&bcmsdh_pci_driver)))
+		return 0;
+#else
+	if (!(error = pci_register_driver(&bcmsdh_pci_driver)))
+		return 0;
+#endif
+
+	SDLX_MSG(("%s: pci_module_init failed 0x%x\n", __FUNCTION__, error));
+#endif /* BCMPLATFORM_BUS */
+
+	return error;
+}
+
+extern void sdio_function_cleanup(void);
+
+void
+bcmsdh_unregister(void)
+{
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
+	if (bcmsdh_pci_driver.node.next)
+#endif
+
+#if defined(BCMPLATFORM_BUS) && !defined(BCMLXSDMMC)
+		driver_unregister(&bcmsdh_driver);
+#endif
+#if defined(BCMLXSDMMC)
+	sdio_function_cleanup();
+#endif /* BCMLXSDMMC */
+#if !defined(BCMPLATFORM_BUS) && !defined(BCMLXSDMMC)
+		pci_unregister_driver(&bcmsdh_pci_driver);
+#endif /* BCMPLATFORM_BUS */
+}
+
+#if defined(OOB_INTR_ONLY)
+void bcmsdh_oob_intr_set(bool enable)
+{
+	static bool curstate = 1;
+	unsigned long flags;
+
+	spin_lock_irqsave(&sdhcinfo->irq_lock, flags);
+	if (curstate != enable) {
+		if (enable)
+			enable_irq(sdhcinfo->oob_irq);
+		else
+			disable_irq_nosync(sdhcinfo->oob_irq);
+		curstate = enable;
+	}
+	spin_unlock_irqrestore(&sdhcinfo->irq_lock, flags);
+}
+
+static irqreturn_t wlan_oob_irq(int irq, void *dev_id)
+{
+	dhd_pub_t *dhdp;
+
+	dhdp = (dhd_pub_t *)dev_get_drvdata(sdhcinfo->dev);
+
+	bcmsdh_oob_intr_set(0);
+
+	if (dhdp == NULL) {
+		SDLX_MSG(("Out of band GPIO interrupt fired way too early\n"));
+		return IRQ_HANDLED;
+	}
+
+	dhdsdio_isr((void *)dhdp->bus);
+
+	return IRQ_HANDLED;
+}
+
+int bcmsdh_register_oob_intr(void * dhdp)
+{
+	int error = 0;
+
+	SDLX_MSG(("%s Enter\n", __FUNCTION__));
+
+/* Example of  HW_OOB for HW2: please refer to your host  specifiction */
+/* sdhcinfo->oob_flags = IORESOURCE_IRQ | IORESOURCE_IRQ_HIGHLEVEL | IORESOURCE_IRQ_SHAREABLE; */
+
+	dev_set_drvdata(sdhcinfo->dev, dhdp);
+
+	if (!sdhcinfo->oob_irq_registered) {
+		SDLX_MSG(("%s IRQ=%d Type=%X \n", __FUNCTION__, \
+				(int)sdhcinfo->oob_irq, (int)sdhcinfo->oob_flags));
+		/* Refer to customer Host IRQ docs about proper irqflags definition */
+		error = request_irq(sdhcinfo->oob_irq, wlan_oob_irq, sdhcinfo->oob_flags,
+			"bcmsdh_sdmmc", NULL);
+		if (error)
+			return -ENODEV;
+
+		enable_irq_wake(sdhcinfo->oob_irq);
+		sdhcinfo->oob_irq_registered = TRUE;
+	}
+
+	return 0;
+}
+
+void bcmsdh_set_irq(int flag)
+{
+	if (sdhcinfo->oob_irq_registered) {
+		SDLX_MSG(("%s Flag = %d", __FUNCTION__, flag));
+		if (flag) {
+			enable_irq(sdhcinfo->oob_irq);
+			enable_irq_wake(sdhcinfo->oob_irq);
+		} else {
+			disable_irq_wake(sdhcinfo->oob_irq);
+			disable_irq(sdhcinfo->oob_irq);
+		}
+	}
+}
+
+void bcmsdh_unregister_oob_intr(void)
+{
+	SDLX_MSG(("%s: Enter\n", __FUNCTION__));
+
+	if (sdhcinfo->oob_irq_registered) {
+		disable_irq_wake(sdhcinfo->oob_irq);
+		disable_irq(sdhcinfo->oob_irq);	/* just in case.. */
+		free_irq(sdhcinfo->oob_irq, NULL);
+		sdhcinfo->oob_irq_registered = FALSE;
+	}
+}
+#endif /* defined(OOB_INTR_ONLY) */
+/* Module parameters specific to each host-controller driver */
+
+extern uint sd_msglevel;	/* Debug message level */
+module_param(sd_msglevel, uint, 0);
+
+extern uint sd_power;	/* 0 = SD Power OFF, 1 = SD Power ON. */
+module_param(sd_power, uint, 0);
+
+extern uint sd_clock;	/* SD Clock Control, 0 = SD Clock OFF, 1 = SD Clock ON */
+module_param(sd_clock, uint, 0);
+
+extern uint sd_divisor;	/* Divisor (-1 means external clock) */
+module_param(sd_divisor, uint, 0);
+
+extern uint sd_sdmode;	/* Default is SD4, 0=SPI, 1=SD1, 2=SD4 */
+module_param(sd_sdmode, uint, 0);
+
+extern uint sd_hiok;	/* Ok to use hi-speed mode */
+module_param(sd_hiok, uint, 0);
+
+extern uint sd_f2_blocksize;
+module_param(sd_f2_blocksize, int, 0);
+
+
+#ifdef BCMSDH_MODULE
+EXPORT_SYMBOL(bcmsdh_attach);
+EXPORT_SYMBOL(bcmsdh_detach);
+EXPORT_SYMBOL(bcmsdh_intr_query);
+EXPORT_SYMBOL(bcmsdh_intr_enable);
+EXPORT_SYMBOL(bcmsdh_intr_disable);
+EXPORT_SYMBOL(bcmsdh_intr_reg);
+EXPORT_SYMBOL(bcmsdh_intr_dereg);
+
+#if defined(DHD_DEBUG)
+EXPORT_SYMBOL(bcmsdh_intr_pending);
+#endif
+
+EXPORT_SYMBOL(bcmsdh_devremove_reg);
+EXPORT_SYMBOL(bcmsdh_cfg_read);
+EXPORT_SYMBOL(bcmsdh_cfg_write);
+EXPORT_SYMBOL(bcmsdh_cis_read);
+EXPORT_SYMBOL(bcmsdh_reg_read);
+EXPORT_SYMBOL(bcmsdh_reg_write);
+EXPORT_SYMBOL(bcmsdh_regfail);
+EXPORT_SYMBOL(bcmsdh_send_buf);
+EXPORT_SYMBOL(bcmsdh_recv_buf);
+
+EXPORT_SYMBOL(bcmsdh_rwdata);
+EXPORT_SYMBOL(bcmsdh_abort);
+EXPORT_SYMBOL(bcmsdh_query_device);
+EXPORT_SYMBOL(bcmsdh_query_iofnum);
+EXPORT_SYMBOL(bcmsdh_iovar_op);
+EXPORT_SYMBOL(bcmsdh_register);
+EXPORT_SYMBOL(bcmsdh_unregister);
+EXPORT_SYMBOL(bcmsdh_chipmatch);
+EXPORT_SYMBOL(bcmsdh_reset);
+
+EXPORT_SYMBOL(bcmsdh_get_dstatus);
+EXPORT_SYMBOL(bcmsdh_cfg_read_word);
+EXPORT_SYMBOL(bcmsdh_cfg_write_word);
+EXPORT_SYMBOL(bcmsdh_cur_sbwad);
+EXPORT_SYMBOL(bcmsdh_chipinfo);
+
+#endif /* BCMSDH_MODULE */
diff --git a/drivers/net/wireless/bcm4329/bcmsdh_sdmmc.c b/drivers/net/wireless/bcm4329/bcmsdh_sdmmc.c
new file mode 100644
index 000000000000..031367b8f18f
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/bcmsdh_sdmmc.c
@@ -0,0 +1,1304 @@
+/*
+ * BCMSDH Function Driver for the native SDIO/MMC driver in the Linux Kernel
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdh_sdmmc.c,v 1.1.2.5.6.30.4.1 2010/09/02 23:12:21 Exp $
+ */
+#include <typedefs.h>
+
+#include <bcmdevs.h>
+#include <bcmendian.h>
+#include <bcmutils.h>
+#include <osl.h>
+#include <sdio.h>	/* SDIO Device and Protocol Specs */
+#include <sdioh.h>	/* SDIO Host Controller Specification */
+#include <bcmsdbus.h>	/* bcmsdh to/from specific controller APIs */
+#include <sdiovar.h>	/* ioctl/iovars */
+
+#include <linux/mmc/core.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/sdio_ids.h>
+
+#include <dngl_stats.h>
+#include <dhd.h>
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP)
+#include <linux/suspend.h>
+extern volatile bool dhd_mmc_suspend;
+#endif
+#include "bcmsdh_sdmmc.h"
+
+#ifndef BCMSDH_MODULE
+extern int sdio_function_init(void);
+extern void sdio_function_cleanup(void);
+#endif /* BCMSDH_MODULE */
+
+#if !defined(OOB_INTR_ONLY)
+static void IRQHandler(struct sdio_func *func);
+static void IRQHandlerF2(struct sdio_func *func);
+#endif /* !defined(OOB_INTR_ONLY) */
+static int sdioh_sdmmc_get_cisaddr(sdioh_info_t *sd, uint32 regaddr);
+extern int sdio_reset_comm(struct mmc_card *card);
+
+extern PBCMSDH_SDMMC_INSTANCE gInstance;
+
+uint sd_sdmode = SDIOH_MODE_SD4;	/* Use SD4 mode by default */
+uint sd_f2_blocksize = 512;		/* Default blocksize */
+
+uint sd_divisor = 2;			/* Default 48MHz/2 = 24MHz */
+
+uint sd_power = 1;		/* Default to SD Slot powered ON */
+uint sd_clock = 1;		/* Default to SD Clock turned ON */
+uint sd_hiok = FALSE;	/* Don't use hi-speed mode by default */
+uint sd_msglevel = 0x01;
+uint sd_use_dma = TRUE;
+DHD_PM_RESUME_WAIT_INIT(sdioh_request_byte_wait);
+DHD_PM_RESUME_WAIT_INIT(sdioh_request_word_wait);
+DHD_PM_RESUME_WAIT_INIT(sdioh_request_packet_wait);
+DHD_PM_RESUME_WAIT_INIT(sdioh_request_buffer_wait);
+
+#define DMA_ALIGN_MASK	0x03
+
+int sdioh_sdmmc_card_regread(sdioh_info_t *sd, int func, uint32 regaddr, int regsize, uint32 *data);
+
+static int
+sdioh_sdmmc_card_enablefuncs(sdioh_info_t *sd)
+{
+	int err_ret;
+	uint32 fbraddr;
+	uint8 func;
+
+	sd_trace(("%s\n", __FUNCTION__));
+
+	/* Get the Card's common CIS address */
+	sd->com_cis_ptr = sdioh_sdmmc_get_cisaddr(sd, SDIOD_CCCR_CISPTR_0);
+	sd->func_cis_ptr[0] = sd->com_cis_ptr;
+	sd_info(("%s: Card's Common CIS Ptr = 0x%x\n", __FUNCTION__, sd->com_cis_ptr));
+
+	/* Get the Card's function CIS (for each function) */
+	for (fbraddr = SDIOD_FBR_STARTADDR, func = 1;
+	     func <= sd->num_funcs; func++, fbraddr += SDIOD_FBR_SIZE) {
+		sd->func_cis_ptr[func] = sdioh_sdmmc_get_cisaddr(sd, SDIOD_FBR_CISPTR_0 + fbraddr);
+		sd_info(("%s: Function %d CIS Ptr = 0x%x\n",
+		         __FUNCTION__, func, sd->func_cis_ptr[func]));
+	}
+
+	sd->func_cis_ptr[0] = sd->com_cis_ptr;
+	sd_info(("%s: Card's Common CIS Ptr = 0x%x\n", __FUNCTION__, sd->com_cis_ptr));
+
+	/* Enable Function 1 */
+	sdio_claim_host(gInstance->func[1]);
+	err_ret = sdio_enable_func(gInstance->func[1]);
+	sdio_release_host(gInstance->func[1]);
+	if (err_ret) {
+		sd_err(("bcmsdh_sdmmc: Failed to enable F1 Err: 0x%08x", err_ret));
+	}
+
+	return FALSE;
+}
+
+/*
+ *	Public entry points & extern's
+ */
+extern sdioh_info_t *
+sdioh_attach(osl_t *osh, void *bar0, uint irq)
+{
+	sdioh_info_t *sd;
+	int err_ret;
+
+	sd_trace(("%s\n", __FUNCTION__));
+
+	if (gInstance == NULL) {
+		sd_err(("%s: SDIO Device not present\n", __FUNCTION__));
+		return NULL;
+	}
+
+	if ((sd = (sdioh_info_t *)MALLOC(osh, sizeof(sdioh_info_t))) == NULL) {
+		sd_err(("sdioh_attach: out of memory, malloced %d bytes\n", MALLOCED(osh)));
+		return NULL;
+	}
+	bzero((char *)sd, sizeof(sdioh_info_t));
+	sd->osh = osh;
+	if (sdioh_sdmmc_osinit(sd) != 0) {
+		sd_err(("%s:sdioh_sdmmc_osinit() failed\n", __FUNCTION__));
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+		return NULL;
+	}
+
+	sd->num_funcs = 2;
+	sd->sd_blockmode = TRUE;
+	sd->use_client_ints = TRUE;
+	sd->client_block_size[0] = 64;
+
+	gInstance->sd = sd;
+
+	/* Claim host controller */
+	sdio_claim_host(gInstance->func[1]);
+
+	sd->client_block_size[1] = 64;
+	err_ret = sdio_set_block_size(gInstance->func[1], 64);
+	if (err_ret) {
+		sd_err(("bcmsdh_sdmmc: Failed to set F1 blocksize\n"));
+	}
+
+	/* Release host controller F1 */
+	sdio_release_host(gInstance->func[1]);
+
+	if (gInstance->func[2]) {
+		/* Claim host controller F2 */
+		sdio_claim_host(gInstance->func[2]);
+
+		sd->client_block_size[2] = sd_f2_blocksize;
+		err_ret = sdio_set_block_size(gInstance->func[2], sd_f2_blocksize);
+		if (err_ret) {
+			sd_err(("bcmsdh_sdmmc: Failed to set F2 blocksize to %d\n",
+				sd_f2_blocksize));
+		}
+
+		/* Release host controller F2 */
+		sdio_release_host(gInstance->func[2]);
+	}
+
+	sdioh_sdmmc_card_enablefuncs(sd);
+
+	sd_trace(("%s: Done\n", __FUNCTION__));
+	return sd;
+}
+
+
+extern SDIOH_API_RC
+sdioh_detach(osl_t *osh, sdioh_info_t *sd)
+{
+	sd_trace(("%s\n", __FUNCTION__));
+
+	if (sd) {
+
+		/* Disable Function 2 */
+		sdio_claim_host(gInstance->func[2]);
+		sdio_disable_func(gInstance->func[2]);
+		sdio_release_host(gInstance->func[2]);
+
+		/* Disable Function 1 */
+		sdio_claim_host(gInstance->func[1]);
+		sdio_disable_func(gInstance->func[1]);
+		sdio_release_host(gInstance->func[1]);
+
+		/* deregister irq */
+		sdioh_sdmmc_osfree(sd);
+
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+	}
+	return SDIOH_API_RC_SUCCESS;
+}
+
+#if defined(OOB_INTR_ONLY) && defined(HW_OOB)
+
+extern SDIOH_API_RC
+sdioh_enable_func_intr(void)
+{
+	uint8 reg;
+	int err;
+
+	if (gInstance->func[0]) {
+		sdio_claim_host(gInstance->func[0]);
+
+		reg = sdio_readb(gInstance->func[0], SDIOD_CCCR_INTEN, &err);
+		if (err) {
+			sd_err(("%s: error for read SDIO_CCCR_IENx : 0x%x\n", __FUNCTION__, err));
+			sdio_release_host(gInstance->func[0]);
+			return SDIOH_API_RC_FAIL;
+		}
+
+		/* Enable F1 and F2 interrupts, set master enable */
+		reg |= (INTR_CTL_FUNC1_EN | INTR_CTL_FUNC2_EN | INTR_CTL_MASTER_EN);
+
+		sdio_writeb(gInstance->func[0], reg, SDIOD_CCCR_INTEN, &err);
+		sdio_release_host(gInstance->func[0]);
+
+		if (err) {
+			sd_err(("%s: error for write SDIO_CCCR_IENx : 0x%x\n", __FUNCTION__, err));
+			return SDIOH_API_RC_FAIL;
+		}
+	}
+
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_disable_func_intr(void)
+{
+	uint8 reg;
+	int err;
+
+	if (gInstance->func[0]) {
+		sdio_claim_host(gInstance->func[0]);
+		reg = sdio_readb(gInstance->func[0], SDIOD_CCCR_INTEN, &err);
+		if (err) {
+			sd_err(("%s: error for read SDIO_CCCR_IENx : 0x%x\n", __FUNCTION__, err));
+			sdio_release_host(gInstance->func[0]);
+			return SDIOH_API_RC_FAIL;
+		}
+
+		reg &= ~(INTR_CTL_FUNC1_EN | INTR_CTL_FUNC2_EN);
+		/* Disable master interrupt with the last function interrupt */
+		if (!(reg & 0xFE))
+			reg = 0;
+		sdio_writeb(gInstance->func[0], reg, SDIOD_CCCR_INTEN, &err);
+
+		sdio_release_host(gInstance->func[0]);
+		if (err) {
+			sd_err(("%s: error for write SDIO_CCCR_IENx : 0x%x\n", __FUNCTION__, err));
+			return SDIOH_API_RC_FAIL;
+		}
+	}
+	return SDIOH_API_RC_SUCCESS;
+}
+#endif /* defined(OOB_INTR_ONLY) && defined(HW_OOB) */
+
+/* Configure callback to client when we recieve client interrupt */
+extern SDIOH_API_RC
+sdioh_interrupt_register(sdioh_info_t *sd, sdioh_cb_fn_t fn, void *argh)
+{
+	sd_trace(("%s: Entering\n", __FUNCTION__));
+	if (fn == NULL) {
+		sd_err(("%s: interrupt handler is NULL, not registering\n", __FUNCTION__));
+		return SDIOH_API_RC_FAIL;
+	}
+#if !defined(OOB_INTR_ONLY)
+	sd->intr_handler = fn;
+	sd->intr_handler_arg = argh;
+	sd->intr_handler_valid = TRUE;
+
+	/* register and unmask irq */
+	if (gInstance->func[2]) {
+		sdio_claim_host(gInstance->func[2]);
+		sdio_claim_irq(gInstance->func[2], IRQHandlerF2);
+		sdio_release_host(gInstance->func[2]);
+	}
+
+	if (gInstance->func[1]) {
+		sdio_claim_host(gInstance->func[1]);
+		sdio_claim_irq(gInstance->func[1], IRQHandler);
+		sdio_release_host(gInstance->func[1]);
+	}
+#elif defined(HW_OOB)
+	sdioh_enable_func_intr();
+#endif /* defined(OOB_INTR_ONLY) */
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_interrupt_deregister(sdioh_info_t *sd)
+{
+	sd_trace(("%s: Entering\n", __FUNCTION__));
+
+#if !defined(OOB_INTR_ONLY)
+	if (gInstance->func[1]) {
+		/* register and unmask irq */
+		sdio_claim_host(gInstance->func[1]);
+		sdio_release_irq(gInstance->func[1]);
+		sdio_release_host(gInstance->func[1]);
+	}
+
+	if (gInstance->func[2]) {
+		/* Claim host controller F2 */
+		sdio_claim_host(gInstance->func[2]);
+		sdio_release_irq(gInstance->func[2]);
+		/* Release host controller F2 */
+		sdio_release_host(gInstance->func[2]);
+	}
+
+	sd->intr_handler_valid = FALSE;
+	sd->intr_handler = NULL;
+	sd->intr_handler_arg = NULL;
+#elif defined(HW_OOB)
+	sdioh_disable_func_intr();
+#endif /*  !defined(OOB_INTR_ONLY) */
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_interrupt_query(sdioh_info_t *sd, bool *onoff)
+{
+	sd_trace(("%s: Entering\n", __FUNCTION__));
+	*onoff = sd->client_intr_enabled;
+	return SDIOH_API_RC_SUCCESS;
+}
+
+#if defined(DHD_DEBUG)
+extern bool
+sdioh_interrupt_pending(sdioh_info_t *sd)
+{
+	return (0);
+}
+#endif
+
+uint
+sdioh_query_iofnum(sdioh_info_t *sd)
+{
+	return sd->num_funcs;
+}
+
+/* IOVar table */
+enum {
+	IOV_MSGLEVEL = 1,
+	IOV_BLOCKMODE,
+	IOV_BLOCKSIZE,
+	IOV_DMA,
+	IOV_USEINTS,
+	IOV_NUMINTS,
+	IOV_NUMLOCALINTS,
+	IOV_HOSTREG,
+	IOV_DEVREG,
+	IOV_DIVISOR,
+	IOV_SDMODE,
+	IOV_HISPEED,
+	IOV_HCIREGS,
+	IOV_POWER,
+	IOV_CLOCK,
+	IOV_RXCHAIN
+};
+
+const bcm_iovar_t sdioh_iovars[] = {
+	{"sd_msglevel", IOV_MSGLEVEL,	0,	IOVT_UINT32,	0 },
+	{"sd_blockmode", IOV_BLOCKMODE, 0,	IOVT_BOOL,	0 },
+	{"sd_blocksize", IOV_BLOCKSIZE, 0,	IOVT_UINT32,	0 }, /* ((fn << 16) | size) */
+	{"sd_dma",	IOV_DMA,	0,	IOVT_BOOL,	0 },
+	{"sd_ints", 	IOV_USEINTS,	0,	IOVT_BOOL,	0 },
+	{"sd_numints",	IOV_NUMINTS,	0,	IOVT_UINT32,	0 },
+	{"sd_numlocalints", IOV_NUMLOCALINTS, 0, IOVT_UINT32,	0 },
+	{"sd_hostreg",	IOV_HOSTREG,	0,	IOVT_BUFFER,	sizeof(sdreg_t) },
+	{"sd_devreg",	IOV_DEVREG, 	0,	IOVT_BUFFER,	sizeof(sdreg_t) },
+	{"sd_divisor",	IOV_DIVISOR,	0,	IOVT_UINT32,	0 },
+	{"sd_power",	IOV_POWER,	0,	IOVT_UINT32,	0 },
+	{"sd_clock",	IOV_CLOCK,	0,	IOVT_UINT32,	0 },
+	{"sd_mode", 	IOV_SDMODE, 	0,	IOVT_UINT32,	100},
+	{"sd_highspeed", IOV_HISPEED,	0,	IOVT_UINT32,	0 },
+	{"sd_rxchain",  IOV_RXCHAIN,    0, 	IOVT_BOOL,	0 },
+	{NULL, 0, 0, 0, 0 }
+};
+
+int
+sdioh_iovar_op(sdioh_info_t *si, const char *name,
+                           void *params, int plen, void *arg, int len, bool set)
+{
+	const bcm_iovar_t *vi = NULL;
+	int bcmerror = 0;
+	int val_size;
+	int32 int_val = 0;
+	bool bool_val;
+	uint32 actionid;
+
+	ASSERT(name);
+	ASSERT(len >= 0);
+
+	/* Get must have return space; Set does not take qualifiers */
+	ASSERT(set || (arg && len));
+	ASSERT(!set || (!params && !plen));
+
+	sd_trace(("%s: Enter (%s %s)\n", __FUNCTION__, (set ? "set" : "get"), name));
+
+	if ((vi = bcm_iovar_lookup(sdioh_iovars, name)) == NULL) {
+		bcmerror = BCME_UNSUPPORTED;
+		goto exit;
+	}
+
+	if ((bcmerror = bcm_iovar_lencheck(vi, arg, len, set)) != 0)
+		goto exit;
+
+	/* Set up params so get and set can share the convenience variables */
+	if (params == NULL) {
+		params = arg;
+		plen = len;
+	}
+
+	if (vi->type == IOVT_VOID)
+		val_size = 0;
+	else if (vi->type == IOVT_BUFFER)
+		val_size = len;
+	else
+		val_size = sizeof(int);
+
+	if (plen >= (int)sizeof(int_val))
+		bcopy(params, &int_val, sizeof(int_val));
+
+	bool_val = (int_val != 0) ? TRUE : FALSE;
+
+	actionid = set ? IOV_SVAL(vi->varid) : IOV_GVAL(vi->varid);
+	switch (actionid) {
+	case IOV_GVAL(IOV_MSGLEVEL):
+		int_val = (int32)sd_msglevel;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_MSGLEVEL):
+		sd_msglevel = int_val;
+		break;
+
+	case IOV_GVAL(IOV_BLOCKMODE):
+		int_val = (int32)si->sd_blockmode;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_BLOCKMODE):
+		si->sd_blockmode = (bool)int_val;
+		/* Haven't figured out how to make non-block mode with DMA */
+		break;
+
+	case IOV_GVAL(IOV_BLOCKSIZE):
+		if ((uint32)int_val > si->num_funcs) {
+			bcmerror = BCME_BADARG;
+			break;
+		}
+		int_val = (int32)si->client_block_size[int_val];
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_BLOCKSIZE):
+	{
+		uint func = ((uint32)int_val >> 16);
+		uint blksize = (uint16)int_val;
+		uint maxsize;
+
+		if (func > si->num_funcs) {
+			bcmerror = BCME_BADARG;
+			break;
+		}
+
+		switch (func) {
+		case 0: maxsize = 32; break;
+		case 1: maxsize = BLOCK_SIZE_4318; break;
+		case 2: maxsize = BLOCK_SIZE_4328; break;
+		default: maxsize = 0;
+		}
+		if (blksize > maxsize) {
+			bcmerror = BCME_BADARG;
+			break;
+		}
+		if (!blksize) {
+			blksize = maxsize;
+		}
+
+		/* Now set it */
+		si->client_block_size[func] = blksize;
+
+		break;
+	}
+
+	case IOV_GVAL(IOV_RXCHAIN):
+		int_val = FALSE;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_DMA):
+		int_val = (int32)si->sd_use_dma;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_DMA):
+		si->sd_use_dma = (bool)int_val;
+		break;
+
+	case IOV_GVAL(IOV_USEINTS):
+		int_val = (int32)si->use_client_ints;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_USEINTS):
+		si->use_client_ints = (bool)int_val;
+		if (si->use_client_ints)
+			si->intmask |= CLIENT_INTR;
+		else
+			si->intmask &= ~CLIENT_INTR;
+
+		break;
+
+	case IOV_GVAL(IOV_DIVISOR):
+		int_val = (uint32)sd_divisor;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_DIVISOR):
+		sd_divisor = int_val;
+		break;
+
+	case IOV_GVAL(IOV_POWER):
+		int_val = (uint32)sd_power;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_POWER):
+		sd_power = int_val;
+		break;
+
+	case IOV_GVAL(IOV_CLOCK):
+		int_val = (uint32)sd_clock;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_CLOCK):
+		sd_clock = int_val;
+		break;
+
+	case IOV_GVAL(IOV_SDMODE):
+		int_val = (uint32)sd_sdmode;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_SDMODE):
+		sd_sdmode = int_val;
+		break;
+
+	case IOV_GVAL(IOV_HISPEED):
+		int_val = (uint32)sd_hiok;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_HISPEED):
+		sd_hiok = int_val;
+		break;
+
+	case IOV_GVAL(IOV_NUMINTS):
+		int_val = (int32)si->intrcount;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_NUMLOCALINTS):
+		int_val = (int32)0;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_HOSTREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+
+		if (sd_ptr->offset < SD_SysAddr || sd_ptr->offset > SD_MaxCurCap) {
+			sd_err(("%s: bad offset 0x%x\n", __FUNCTION__, sd_ptr->offset));
+			bcmerror = BCME_BADARG;
+			break;
+		}
+
+		sd_trace(("%s: rreg%d at offset %d\n", __FUNCTION__,
+		                  (sd_ptr->offset & 1) ? 8 : ((sd_ptr->offset & 2) ? 16 : 32),
+		                  sd_ptr->offset));
+		if (sd_ptr->offset & 1)
+			int_val = 8; /* sdioh_sdmmc_rreg8(si, sd_ptr->offset); */
+		else if (sd_ptr->offset & 2)
+			int_val = 16; /* sdioh_sdmmc_rreg16(si, sd_ptr->offset); */
+		else
+			int_val = 32; /* sdioh_sdmmc_rreg(si, sd_ptr->offset); */
+
+		bcopy(&int_val, arg, sizeof(int_val));
+		break;
+	}
+
+	case IOV_SVAL(IOV_HOSTREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+
+		if (sd_ptr->offset < SD_SysAddr || sd_ptr->offset > SD_MaxCurCap) {
+			sd_err(("%s: bad offset 0x%x\n", __FUNCTION__, sd_ptr->offset));
+			bcmerror = BCME_BADARG;
+			break;
+		}
+
+		sd_trace(("%s: wreg%d value 0x%08x at offset %d\n", __FUNCTION__, sd_ptr->value,
+		                  (sd_ptr->offset & 1) ? 8 : ((sd_ptr->offset & 2) ? 16 : 32),
+		                  sd_ptr->offset));
+		break;
+	}
+
+	case IOV_GVAL(IOV_DEVREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+		uint8 data = 0;
+
+		if (sdioh_cfg_read(si, sd_ptr->func, sd_ptr->offset, &data)) {
+			bcmerror = BCME_SDIO_ERROR;
+			break;
+		}
+
+		int_val = (int)data;
+		bcopy(&int_val, arg, sizeof(int_val));
+		break;
+	}
+
+	case IOV_SVAL(IOV_DEVREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+		uint8 data = (uint8)sd_ptr->value;
+
+		if (sdioh_cfg_write(si, sd_ptr->func, sd_ptr->offset, &data)) {
+			bcmerror = BCME_SDIO_ERROR;
+			break;
+		}
+		break;
+	}
+
+	default:
+		bcmerror = BCME_UNSUPPORTED;
+		break;
+	}
+exit:
+
+	return bcmerror;
+}
+
+#if defined(OOB_INTR_ONLY) && defined(HW_OOB)
+
+SDIOH_API_RC
+sdioh_enable_hw_oob_intr(sdioh_info_t *sd, bool enable)
+{
+	SDIOH_API_RC status;
+	uint8 data;
+
+	if (enable)
+		data = 3;	/* enable hw oob interrupt */
+	else
+		data = 4;	/* disable hw oob interrupt */
+	data |= 4;		/* Active HIGH */
+
+	status = sdioh_request_byte(sd, SDIOH_WRITE, 0, 0xf2, &data);
+	return status;
+}
+#endif /* defined(OOB_INTR_ONLY) && defined(HW_OOB) */
+
+extern SDIOH_API_RC
+sdioh_cfg_read(sdioh_info_t *sd, uint fnc_num, uint32 addr, uint8 *data)
+{
+	SDIOH_API_RC status;
+	/* No lock needed since sdioh_request_byte does locking */
+	status = sdioh_request_byte(sd, SDIOH_READ, fnc_num, addr, data);
+	return status;
+}
+
+extern SDIOH_API_RC
+sdioh_cfg_write(sdioh_info_t *sd, uint fnc_num, uint32 addr, uint8 *data)
+{
+	/* No lock needed since sdioh_request_byte does locking */
+	SDIOH_API_RC status;
+	status = sdioh_request_byte(sd, SDIOH_WRITE, fnc_num, addr, data);
+	return status;
+}
+
+static int
+sdioh_sdmmc_get_cisaddr(sdioh_info_t *sd, uint32 regaddr)
+{
+	/* read 24 bits and return valid 17 bit addr */
+	int i;
+	uint32 scratch, regdata;
+	uint8 *ptr = (uint8 *)&scratch;
+	for (i = 0; i < 3; i++) {
+		if ((sdioh_sdmmc_card_regread (sd, 0, regaddr, 1, &regdata)) != SUCCESS)
+			sd_err(("%s: Can't read!\n", __FUNCTION__));
+
+		*ptr++ = (uint8) regdata;
+		regaddr++;
+	}
+
+	/* Only the lower 17-bits are valid */
+	scratch = ltoh32(scratch);
+	scratch &= 0x0001FFFF;
+	return (scratch);
+}
+
+extern SDIOH_API_RC
+sdioh_cis_read(sdioh_info_t *sd, uint func, uint8 *cisd, uint32 length)
+{
+	uint32 count;
+	int offset;
+	uint32 foo;
+	uint8 *cis = cisd;
+
+	sd_trace(("%s: Func = %d\n", __FUNCTION__, func));
+
+	if (!sd->func_cis_ptr[func]) {
+		bzero(cis, length);
+		sd_err(("%s: no func_cis_ptr[%d]\n", __FUNCTION__, func));
+		return SDIOH_API_RC_FAIL;
+	}
+
+	sd_err(("%s: func_cis_ptr[%d]=0x%04x\n", __FUNCTION__, func, sd->func_cis_ptr[func]));
+
+	for (count = 0; count < length; count++) {
+		offset =  sd->func_cis_ptr[func] + count;
+		if (sdioh_sdmmc_card_regread (sd, 0, offset, 1, &foo) < 0) {
+			sd_err(("%s: regread failed: Can't read CIS\n", __FUNCTION__));
+			return SDIOH_API_RC_FAIL;
+		}
+
+		*cis = (uint8)(foo & 0xff);
+		cis++;
+	}
+
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_request_byte(sdioh_info_t *sd, uint rw, uint func, uint regaddr, uint8 *byte)
+{
+	int err_ret;
+
+	sd_info(("%s: rw=%d, func=%d, addr=0x%05x\n", __FUNCTION__, rw, func, regaddr));
+
+	DHD_PM_RESUME_WAIT(sdioh_request_byte_wait);
+	DHD_PM_RESUME_RETURN_ERROR(SDIOH_API_RC_FAIL);
+	if(rw) { /* CMD52 Write */
+		if (func == 0) {
+			/* Can only directly write to some F0 registers.  Handle F2 enable
+			 * as a special case.
+			 */
+			if (regaddr == SDIOD_CCCR_IOEN) {
+				if (gInstance->func[2]) {
+					sdio_claim_host(gInstance->func[2]);
+					if (*byte & SDIO_FUNC_ENABLE_2) {
+						/* Enable Function 2 */
+						err_ret = sdio_enable_func(gInstance->func[2]);
+						if (err_ret) {
+							sd_err(("bcmsdh_sdmmc: enable F2 failed:%d",
+								err_ret));
+						}
+					} else {
+						/* Disable Function 2 */
+						err_ret = sdio_disable_func(gInstance->func[2]);
+						if (err_ret) {
+							sd_err(("bcmsdh_sdmmc: Disab F2 failed:%d",
+								err_ret));
+						}
+					}
+					sdio_release_host(gInstance->func[2]);
+				}
+			}
+#if defined(MMC_SDIO_ABORT)
+			/* to allow abort command through F1 */
+			else if (regaddr == SDIOD_CCCR_IOABORT) {
+				sdio_claim_host(gInstance->func[func]);
+				/*
+				* this sdio_f0_writeb() can be replaced with another api
+				* depending upon MMC driver change.
+				* As of this time, this is temporaray one
+				*/
+				sdio_writeb(gInstance->func[func], *byte, regaddr, &err_ret);
+				sdio_release_host(gInstance->func[func]);
+			}
+#endif /* MMC_SDIO_ABORT */
+			else if (regaddr < 0xF0) {
+				sd_err(("bcmsdh_sdmmc: F0 Wr:0x%02x: write disallowed\n", regaddr));
+			} else {
+				/* Claim host controller, perform F0 write, and release */
+				sdio_claim_host(gInstance->func[func]);
+				sdio_f0_writeb(gInstance->func[func], *byte, regaddr, &err_ret);
+				sdio_release_host(gInstance->func[func]);
+			}
+		} else {
+			/* Claim host controller, perform Fn write, and release */
+			sdio_claim_host(gInstance->func[func]);
+			sdio_writeb(gInstance->func[func], *byte, regaddr, &err_ret);
+			sdio_release_host(gInstance->func[func]);
+		}
+	} else { /* CMD52 Read */
+		/* Claim host controller, perform Fn read, and release */
+		sdio_claim_host(gInstance->func[func]);
+
+		if (func == 0) {
+			*byte = sdio_f0_readb(gInstance->func[func], regaddr, &err_ret);
+		} else {
+			*byte = sdio_readb(gInstance->func[func], regaddr, &err_ret);
+		}
+
+		sdio_release_host(gInstance->func[func]);
+	}
+
+	if (err_ret) {
+		sd_err(("bcmsdh_sdmmc: Failed to %s byte F%d:@0x%05x=%02x, Err: %d\n",
+		                        rw ? "Write" : "Read", func, regaddr, *byte, err_ret));
+	}
+
+	return ((err_ret == 0) ? SDIOH_API_RC_SUCCESS : SDIOH_API_RC_FAIL);
+}
+
+extern SDIOH_API_RC
+sdioh_request_word(sdioh_info_t *sd, uint cmd_type, uint rw, uint func, uint addr,
+                                   uint32 *word, uint nbytes)
+{
+	int err_ret = SDIOH_API_RC_FAIL;
+
+	if (func == 0) {
+		sd_err(("%s: Only CMD52 allowed to F0.\n", __FUNCTION__));
+		return SDIOH_API_RC_FAIL;
+	}
+
+	sd_info(("%s: cmd_type=%d, rw=%d, func=%d, addr=0x%05x, nbytes=%d\n",
+	         __FUNCTION__, cmd_type, rw, func, addr, nbytes));
+
+	DHD_PM_RESUME_WAIT(sdioh_request_word_wait);
+	DHD_PM_RESUME_RETURN_ERROR(SDIOH_API_RC_FAIL);
+	/* Claim host controller */
+	sdio_claim_host(gInstance->func[func]);
+
+	if(rw) { /* CMD52 Write */
+		if (nbytes == 4) {
+			sdio_writel(gInstance->func[func], *word, addr, &err_ret);
+		} else if (nbytes == 2) {
+			sdio_writew(gInstance->func[func], (*word & 0xFFFF), addr, &err_ret);
+		} else {
+			sd_err(("%s: Invalid nbytes: %d\n", __FUNCTION__, nbytes));
+		}
+	} else { /* CMD52 Read */
+		if (nbytes == 4) {
+			*word = sdio_readl(gInstance->func[func], addr, &err_ret);
+		} else if (nbytes == 2) {
+			*word = sdio_readw(gInstance->func[func], addr, &err_ret) & 0xFFFF;
+		} else {
+			sd_err(("%s: Invalid nbytes: %d\n", __FUNCTION__, nbytes));
+		}
+	}
+
+	/* Release host controller */
+	sdio_release_host(gInstance->func[func]);
+
+	if (err_ret) {
+		sd_err(("bcmsdh_sdmmc: Failed to %s word, Err: 0x%08x",
+		                        rw ? "Write" : "Read", err_ret));
+	}
+
+	return ((err_ret == 0) ? SDIOH_API_RC_SUCCESS : SDIOH_API_RC_FAIL);
+}
+
+static SDIOH_API_RC
+sdioh_request_packet(sdioh_info_t *sd, uint fix_inc, uint write, uint func,
+                     uint addr, void *pkt)
+{
+	bool fifo = (fix_inc == SDIOH_DATA_FIX);
+	uint32	SGCount = 0;
+	int err_ret = 0;
+
+	void *pnext;
+
+	sd_trace(("%s: Enter\n", __FUNCTION__));
+
+	ASSERT(pkt);
+	DHD_PM_RESUME_WAIT(sdioh_request_packet_wait);
+	DHD_PM_RESUME_RETURN_ERROR(SDIOH_API_RC_FAIL);
+
+	/* Claim host controller */
+	sdio_claim_host(gInstance->func[func]);
+	for (pnext = pkt; pnext; pnext = PKTNEXT(sd->osh, pnext)) {
+		uint pkt_len = PKTLEN(sd->osh, pnext);
+		pkt_len += 3;
+		pkt_len &= 0xFFFFFFFC;
+
+#ifdef CONFIG_MMC_MSM7X00A
+		if ((pkt_len % 64) == 32) {
+			sd_trace(("%s: Rounding up TX packet +=32\n", __FUNCTION__));
+			pkt_len += 32;
+		}
+#endif /* CONFIG_MMC_MSM7X00A */
+		/* Make sure the packet is aligned properly. If it isn't, then this
+		 * is the fault of sdioh_request_buffer() which is supposed to give
+		 * us something we can work with.
+		 */
+		ASSERT(((uint32)(PKTDATA(sd->osh, pkt)) & DMA_ALIGN_MASK) == 0);
+
+		if ((write) && (!fifo)) {
+			err_ret = sdio_memcpy_toio(gInstance->func[func], addr,
+				((uint8*)PKTDATA(sd->osh, pnext)),
+				pkt_len);
+		} else if (write) {
+			err_ret = sdio_memcpy_toio(gInstance->func[func], addr,
+				((uint8*)PKTDATA(sd->osh, pnext)),
+				pkt_len);
+		} else if (fifo) {
+			err_ret = sdio_readsb(gInstance->func[func],
+				((uint8*)PKTDATA(sd->osh, pnext)),
+				addr,
+				pkt_len);
+		} else {
+			err_ret = sdio_memcpy_fromio(gInstance->func[func],
+				((uint8*)PKTDATA(sd->osh, pnext)),
+				addr,
+				pkt_len);
+		}
+
+		if (err_ret) {
+			sd_err(("%s: %s FAILED %p[%d], addr=0x%05x, pkt_len=%d, ERR=0x%08x\n",
+				__FUNCTION__,
+				(write) ? "TX" : "RX",
+				pnext, SGCount, addr, pkt_len, err_ret));
+		} else {
+			sd_trace(("%s: %s xfr'd %p[%d], addr=0x%05x, len=%d\n",
+				__FUNCTION__,
+				(write) ? "TX" : "RX",
+				pnext, SGCount, addr, pkt_len));
+		}
+
+		if (!fifo) {
+			addr += pkt_len;
+		}
+		SGCount ++;
+
+	}
+
+	/* Release host controller */
+	sdio_release_host(gInstance->func[func]);
+
+	sd_trace(("%s: Exit\n", __FUNCTION__));
+	return ((err_ret == 0) ? SDIOH_API_RC_SUCCESS : SDIOH_API_RC_FAIL);
+}
+
+
+/*
+ * This function takes a buffer or packet, and fixes everything up so that in the
+ * end, a DMA-able packet is created.
+ *
+ * A buffer does not have an associated packet pointer, and may or may not be aligned.
+ * A packet may consist of a single packet, or a packet chain.  If it is a packet chain,
+ * then all the packets in the chain must be properly aligned.  If the packet data is not
+ * aligned, then there may only be one packet, and in this case, it is copied to a new
+ * aligned packet.
+ *
+ */
+extern SDIOH_API_RC
+sdioh_request_buffer(sdioh_info_t *sd, uint pio_dma, uint fix_inc, uint write, uint func,
+                     uint addr, uint reg_width, uint buflen_u, uint8 *buffer, void *pkt)
+{
+	SDIOH_API_RC Status;
+	void *mypkt = NULL;
+
+	sd_trace(("%s: Enter\n", __FUNCTION__));
+
+	DHD_PM_RESUME_WAIT(sdioh_request_buffer_wait);
+	DHD_PM_RESUME_RETURN_ERROR(SDIOH_API_RC_FAIL);
+	/* Case 1: we don't have a packet. */
+	if (pkt == NULL) {
+		sd_data(("%s: Creating new %s Packet, len=%d\n",
+		         __FUNCTION__, write ? "TX" : "RX", buflen_u));
+#ifdef DHD_USE_STATIC_BUF
+		if (!(mypkt = PKTGET_STATIC(sd->osh, buflen_u, write ? TRUE : FALSE))) {
+#else
+		if (!(mypkt = PKTGET(sd->osh, buflen_u, write ? TRUE : FALSE))) {
+#endif /* DHD_USE_STATIC_BUF */
+			sd_err(("%s: PKTGET failed: len %d\n",
+			           __FUNCTION__, buflen_u));
+			return SDIOH_API_RC_FAIL;
+		}
+
+		/* For a write, copy the buffer data into the packet. */
+		if (write) {
+			bcopy(buffer, PKTDATA(sd->osh, mypkt), buflen_u);
+		}
+
+		Status = sdioh_request_packet(sd, fix_inc, write, func, addr, mypkt);
+
+		/* For a read, copy the packet data back to the buffer. */
+		if (!write) {
+			bcopy(PKTDATA(sd->osh, mypkt), buffer, buflen_u);
+		}
+#ifdef DHD_USE_STATIC_BUF
+		PKTFREE_STATIC(sd->osh, mypkt, write ? TRUE : FALSE);
+#else
+		PKTFREE(sd->osh, mypkt, write ? TRUE : FALSE);
+#endif /* DHD_USE_STATIC_BUF */
+	} else if (((uint32)(PKTDATA(sd->osh, pkt)) & DMA_ALIGN_MASK) != 0) {
+		/* Case 2: We have a packet, but it is unaligned. */
+
+		/* In this case, we cannot have a chain. */
+		ASSERT(PKTNEXT(sd->osh, pkt) == NULL);
+
+		sd_data(("%s: Creating aligned %s Packet, len=%d\n",
+		         __FUNCTION__, write ? "TX" : "RX", PKTLEN(sd->osh, pkt)));
+#ifdef DHD_USE_STATIC_BUF
+		if (!(mypkt = PKTGET_STATIC(sd->osh, PKTLEN(sd->osh, pkt), write ? TRUE : FALSE))) {
+#else
+		if (!(mypkt = PKTGET(sd->osh, PKTLEN(sd->osh, pkt), write ? TRUE : FALSE))) {
+#endif /* DHD_USE_STATIC_BUF */
+			sd_err(("%s: PKTGET failed: len %d\n",
+			           __FUNCTION__, PKTLEN(sd->osh, pkt)));
+			return SDIOH_API_RC_FAIL;
+		}
+
+		/* For a write, copy the buffer data into the packet. */
+		if (write) {
+			bcopy(PKTDATA(sd->osh, pkt),
+			      PKTDATA(sd->osh, mypkt),
+			      PKTLEN(sd->osh, pkt));
+		}
+
+		Status = sdioh_request_packet(sd, fix_inc, write, func, addr, mypkt);
+
+		/* For a read, copy the packet data back to the buffer. */
+		if (!write) {
+			bcopy(PKTDATA(sd->osh, mypkt),
+			      PKTDATA(sd->osh, pkt),
+			      PKTLEN(sd->osh, mypkt));
+		}
+#ifdef DHD_USE_STATIC_BUF
+		PKTFREE_STATIC(sd->osh, mypkt, write ? TRUE : FALSE);
+#else
+		PKTFREE(sd->osh, mypkt, write ? TRUE : FALSE);
+#endif /* DHD_USE_STATIC_BUF */
+	} else { /* case 3: We have a packet and it is aligned. */
+		sd_data(("%s: Aligned %s Packet, direct DMA\n",
+		         __FUNCTION__, write ? "Tx" : "Rx"));
+		Status = sdioh_request_packet(sd, fix_inc, write, func, addr, pkt);
+	}
+
+	return (Status);
+}
+
+/* this function performs "abort" for both of host & device */
+extern int
+sdioh_abort(sdioh_info_t *sd, uint func)
+{
+#if defined(MMC_SDIO_ABORT)
+	char t_func = (char) func;
+#endif /* defined(MMC_SDIO_ABORT) */
+	sd_trace(("%s: Enter\n", __FUNCTION__));
+
+#if defined(MMC_SDIO_ABORT)
+	/* issue abort cmd52 command through F1 */
+	sdioh_request_byte(sd, SD_IO_OP_WRITE, SDIO_FUNC_0, SDIOD_CCCR_IOABORT, &t_func);
+#endif /* defined(MMC_SDIO_ABORT) */
+
+	sd_trace(("%s: Exit\n", __FUNCTION__));
+	return SDIOH_API_RC_SUCCESS;
+}
+
+/* Reset and re-initialize the device */
+int sdioh_sdio_reset(sdioh_info_t *si)
+{
+	sd_trace(("%s: Enter\n", __FUNCTION__));
+	sd_trace(("%s: Exit\n", __FUNCTION__));
+	return SDIOH_API_RC_SUCCESS;
+}
+
+/* Disable device interrupt */
+void
+sdioh_sdmmc_devintr_off(sdioh_info_t *sd)
+{
+	sd_trace(("%s: %d\n", __FUNCTION__, sd->use_client_ints));
+	sd->intmask &= ~CLIENT_INTR;
+}
+
+/* Enable device interrupt */
+void
+sdioh_sdmmc_devintr_on(sdioh_info_t *sd)
+{
+	sd_trace(("%s: %d\n", __FUNCTION__, sd->use_client_ints));
+	sd->intmask |= CLIENT_INTR;
+}
+
+/* Read client card reg */
+int
+sdioh_sdmmc_card_regread(sdioh_info_t *sd, int func, uint32 regaddr, int regsize, uint32 *data)
+{
+
+	if ((func == 0) || (regsize == 1)) {
+		uint8 temp = 0;
+
+		sdioh_request_byte(sd, SDIOH_READ, func, regaddr, &temp);
+		*data = temp;
+		*data &= 0xff;
+		sd_data(("%s: byte read data=0x%02x\n",
+		         __FUNCTION__, *data));
+	} else {
+		sdioh_request_word(sd, 0, SDIOH_READ, func, regaddr, data, regsize);
+		if (regsize == 2)
+			*data &= 0xffff;
+
+		sd_data(("%s: word read data=0x%08x\n",
+		         __FUNCTION__, *data));
+	}
+
+	return SUCCESS;
+}
+
+#if !defined(OOB_INTR_ONLY)
+/* bcmsdh_sdmmc interrupt handler */
+static void IRQHandler(struct sdio_func *func)
+{
+	sdioh_info_t *sd;
+
+	sd_trace(("bcmsdh_sdmmc: ***IRQHandler\n"));
+	sd = gInstance->sd;
+
+	ASSERT(sd != NULL);
+	sdio_release_host(gInstance->func[0]);
+
+	if (sd->use_client_ints) {
+		sd->intrcount++;
+		ASSERT(sd->intr_handler);
+		ASSERT(sd->intr_handler_arg);
+		(sd->intr_handler)(sd->intr_handler_arg);
+	} else {
+		sd_err(("bcmsdh_sdmmc: ***IRQHandler\n"));
+
+		sd_err(("%s: Not ready for intr: enabled %d, handler %p\n",
+		        __FUNCTION__, sd->client_intr_enabled, sd->intr_handler));
+	}
+
+	sdio_claim_host(gInstance->func[0]);
+}
+
+/* bcmsdh_sdmmc interrupt handler for F2 (dummy handler) */
+static void IRQHandlerF2(struct sdio_func *func)
+{
+	sdioh_info_t *sd;
+
+	sd_trace(("bcmsdh_sdmmc: ***IRQHandlerF2\n"));
+
+	sd = gInstance->sd;
+
+	ASSERT(sd != NULL);
+}
+#endif /* !defined(OOB_INTR_ONLY) */
+
+#ifdef NOTUSED
+/* Write client card reg */
+static int
+sdioh_sdmmc_card_regwrite(sdioh_info_t *sd, int func, uint32 regaddr, int regsize, uint32 data)
+{
+
+	if ((func == 0) || (regsize == 1)) {
+		uint8 temp;
+
+		temp = data & 0xff;
+		sdioh_request_byte(sd, SDIOH_READ, func, regaddr, &temp);
+		sd_data(("%s: byte write data=0x%02x\n",
+		         __FUNCTION__, data));
+	} else {
+		if (regsize == 2)
+			data &= 0xffff;
+
+		sdioh_request_word(sd, 0, SDIOH_READ, func, regaddr, &data, regsize);
+
+		sd_data(("%s: word write data=0x%08x\n",
+		         __FUNCTION__, data));
+	}
+
+	return SUCCESS;
+}
+#endif /* NOTUSED */
+
+int
+sdioh_start(sdioh_info_t *si, int stage)
+{
+	int ret;
+	sdioh_info_t *sd = gInstance->sd;
+
+	/* Need to do this stages as we can't enable the interrupt till
+		downloading of the firmware is complete, other wise polling
+		sdio access will come in way
+	*/
+	if (gInstance->func[0]) {
+			if (stage == 0) {
+		/* Since the power to the chip is killed, we will have
+			re enumerate the device again. Set the block size
+			and enable the fucntion 1 for in preparation for
+			downloading the code
+		*/
+		/* sdio_reset_comm() - has been fixed in latest kernel/msm.git for Linux
+		   2.6.27. The implementation prior to that is buggy, and needs broadcom's
+		   patch for it
+		*/
+		if ((ret = sdio_reset_comm(gInstance->func[0]->card)))
+			sd_err(("%s Failed, error = %d\n", __FUNCTION__, ret));
+		else {
+			sd->num_funcs = 2;
+			sd->sd_blockmode = TRUE;
+			sd->use_client_ints = TRUE;
+			sd->client_block_size[0] = 64;
+
+			/* Claim host controller */
+			sdio_claim_host(gInstance->func[1]);
+
+			sd->client_block_size[1] = 64;
+			if (sdio_set_block_size(gInstance->func[1], 64)) {
+				sd_err(("bcmsdh_sdmmc: Failed to set F1 blocksize\n"));
+			}
+
+			/* Release host controller F1 */
+			sdio_release_host(gInstance->func[1]);
+
+			if (gInstance->func[2]) {
+				/* Claim host controller F2 */
+				sdio_claim_host(gInstance->func[2]);
+
+				sd->client_block_size[2] = sd_f2_blocksize;
+				if (sdio_set_block_size(gInstance->func[2],
+					sd_f2_blocksize)) {
+					sd_err(("bcmsdh_sdmmc: Failed to set F2 "
+						"blocksize to %d\n", sd_f2_blocksize));
+				}
+
+				/* Release host controller F2 */
+				sdio_release_host(gInstance->func[2]);
+			}
+
+			sdioh_sdmmc_card_enablefuncs(sd);
+			}
+		} else {
+#if !defined(OOB_INTR_ONLY)
+			sdio_claim_host(gInstance->func[0]);
+			sdio_claim_irq(gInstance->func[2], IRQHandlerF2);
+			sdio_claim_irq(gInstance->func[1], IRQHandler);
+			sdio_release_host(gInstance->func[0]);
+#else /* defined(OOB_INTR_ONLY) */
+#if defined(HW_OOB)
+			sdioh_enable_func_intr();
+#endif
+			bcmsdh_oob_intr_set(TRUE);
+#endif /* !defined(OOB_INTR_ONLY) */
+		}
+	}
+	else
+		sd_err(("%s Failed\n", __FUNCTION__));
+
+	return (0);
+}
+
+int
+sdioh_stop(sdioh_info_t *si)
+{
+	/* MSM7201A Android sdio stack has bug with interrupt
+		So internaly within SDIO stack they are polling
+		which cause issue when device is turned off. So
+		unregister interrupt with SDIO stack to stop the
+		polling
+	*/
+	if (gInstance->func[0]) {
+#if !defined(OOB_INTR_ONLY)
+		sdio_claim_host(gInstance->func[0]);
+		sdio_release_irq(gInstance->func[1]);
+		sdio_release_irq(gInstance->func[2]);
+		sdio_release_host(gInstance->func[0]);
+#else /* defined(OOB_INTR_ONLY) */
+#if defined(HW_OOB)
+		sdioh_disable_func_intr();
+#endif
+		bcmsdh_oob_intr_set(FALSE);
+#endif /* !defined(OOB_INTR_ONLY) */
+	}
+	else
+		sd_err(("%s Failed\n", __FUNCTION__));
+	return (0);
+}
diff --git a/drivers/net/wireless/bcm4329/bcmsdh_sdmmc_linux.c b/drivers/net/wireless/bcm4329/bcmsdh_sdmmc_linux.c
new file mode 100644
index 000000000000..b1fc35b77836
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/bcmsdh_sdmmc_linux.c
@@ -0,0 +1,316 @@
+/*
+ * BCMSDH Function Driver for the native SDIO/MMC driver in the Linux Kernel
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdh_sdmmc_linux.c,v 1.1.2.5.6.17 2010/08/13 00:36:19 Exp $
+ */
+
+#include <typedefs.h>
+#include <bcmutils.h>
+#include <sdio.h>	/* SDIO Specs */
+#include <bcmsdbus.h>	/* bcmsdh to/from specific controller APIs */
+#include <sdiovar.h>	/* to get msglevel bit values */
+
+#include <linux/sched.h>	/* request_irq() */
+
+#include <linux/mmc/core.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/sdio_ids.h>
+
+#if !defined(SDIO_VENDOR_ID_BROADCOM)
+#define SDIO_VENDOR_ID_BROADCOM		0x02d0
+#endif /* !defined(SDIO_VENDOR_ID_BROADCOM) */
+
+#define SDIO_DEVICE_ID_BROADCOM_DEFAULT	0x0000
+
+#if !defined(SDIO_DEVICE_ID_BROADCOM_4325_SDGWB)
+#define SDIO_DEVICE_ID_BROADCOM_4325_SDGWB	0x0492	/* BCM94325SDGWB */
+#endif /* !defined(SDIO_DEVICE_ID_BROADCOM_4325_SDGWB) */
+#if !defined(SDIO_DEVICE_ID_BROADCOM_4325)
+#define SDIO_DEVICE_ID_BROADCOM_4325	0x0493
+#endif /* !defined(SDIO_DEVICE_ID_BROADCOM_4325) */
+#if !defined(SDIO_DEVICE_ID_BROADCOM_4329)
+#define SDIO_DEVICE_ID_BROADCOM_4329	0x4329
+#endif /* !defined(SDIO_DEVICE_ID_BROADCOM_4329) */
+#if !defined(SDIO_DEVICE_ID_BROADCOM_4319)
+#define SDIO_DEVICE_ID_BROADCOM_4319	0x4319
+#endif /* !defined(SDIO_DEVICE_ID_BROADCOM_4329) */
+
+#include <bcmsdh_sdmmc.h>
+
+#include <dhd_dbg.h>
+
+extern void sdioh_sdmmc_devintr_off(sdioh_info_t *sd);
+extern void sdioh_sdmmc_devintr_on(sdioh_info_t *sd);
+
+int sdio_function_init(void);
+void sdio_function_cleanup(void);
+
+#define DESCRIPTION "bcmsdh_sdmmc Driver"
+#define AUTHOR "Broadcom Corporation"
+
+/* module param defaults */
+static int clockoverride = 0;
+
+module_param(clockoverride, int, 0644);
+MODULE_PARM_DESC(clockoverride, "SDIO card clock override");
+
+PBCMSDH_SDMMC_INSTANCE gInstance;
+
+/* Maximum number of bcmsdh_sdmmc devices supported by driver */
+#define BCMSDH_SDMMC_MAX_DEVICES 1
+
+extern int bcmsdh_probe(struct device *dev);
+extern int bcmsdh_remove(struct device *dev);
+
+static int bcmsdh_sdmmc_probe(struct sdio_func *func,
+                              const struct sdio_device_id *id)
+{
+	int ret = 0;
+	static struct sdio_func sdio_func_0;
+	sd_trace(("bcmsdh_sdmmc: %s Enter\n", __FUNCTION__));
+	sd_trace(("sdio_bcmsdh: func->class=%x\n", func->class));
+	sd_trace(("sdio_vendor: 0x%04x\n", func->vendor));
+	sd_trace(("sdio_device: 0x%04x\n", func->device));
+	sd_trace(("Function#: 0x%04x\n", func->num));
+
+	if (func->num == 1) {
+		sdio_func_0.num = 0;
+		sdio_func_0.card = func->card;
+		gInstance->func[0] = &sdio_func_0;
+		if(func->device == 0x4) { /* 4318 */
+			gInstance->func[2] = NULL;
+			sd_trace(("NIC found, calling bcmsdh_probe...\n"));
+			ret = bcmsdh_probe(&func->dev);
+		}
+	}
+
+	gInstance->func[func->num] = func;
+
+	if (func->num == 2) {
+		sd_trace(("F2 found, calling bcmsdh_probe...\n"));
+		ret = bcmsdh_probe(&func->dev);
+	}
+
+	return ret;
+}
+
+static void bcmsdh_sdmmc_remove(struct sdio_func *func)
+{
+	sd_trace(("bcmsdh_sdmmc: %s Enter\n", __FUNCTION__));
+	sd_info(("sdio_bcmsdh: func->class=%x\n", func->class));
+	sd_info(("sdio_vendor: 0x%04x\n", func->vendor));
+	sd_info(("sdio_device: 0x%04x\n", func->device));
+	sd_info(("Function#: 0x%04x\n", func->num));
+
+	if (func->num == 2) {
+		sd_trace(("F2 found, calling bcmsdh_remove...\n"));
+		bcmsdh_remove(&func->dev);
+	}
+}
+
+/* devices we support, null terminated */
+static const struct sdio_device_id bcmsdh_sdmmc_ids[] = {
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_DEFAULT) },
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4325_SDGWB) },
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4325) },
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4329) },
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4319) },
+	{ /* end: all zeroes */				},
+};
+
+MODULE_DEVICE_TABLE(sdio, bcmsdh_sdmmc_ids);
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 39)) && defined(CONFIG_PM)
+static int bcmsdh_sdmmc_suspend(struct device *pdev)
+{
+	struct sdio_func *func = dev_to_sdio_func(pdev);
+	mmc_pm_flag_t sdio_flags;
+	int ret = 0;
+
+	if (func->num != 2)
+		return 0;
+
+	sdio_flags = sdio_get_host_pm_caps(func);
+
+	if (!(sdio_flags & MMC_PM_KEEP_POWER)) {
+		sd_err(("can't keep power while host "
+				"is suspended\n", __FUNCTION__));
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* keep power while host suspended */
+	ret = sdio_set_host_pm_flags(func, MMC_PM_KEEP_POWER);
+	if (ret) {
+		sd_err(("error while trying to keep power\n", __FUNCTION__));
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
+static int bcmsdh_sdmmc_resume(struct device *pdev)
+{
+	/* nothing to do since MMC_PM_KEEP_POWER is cleared by MMC stack */
+	return 0;
+}
+
+static const struct dev_pm_ops bcmsdh_sdmmc_pm_ops = {
+	.suspend	= bcmsdh_sdmmc_suspend,
+	.resume		= bcmsdh_sdmmc_resume,
+};
+#endif
+
+static struct sdio_driver bcmsdh_sdmmc_driver = {
+	.probe		= bcmsdh_sdmmc_probe,
+	.remove		= bcmsdh_sdmmc_remove,
+	.name		= "bcmsdh_sdmmc",
+	.id_table	= bcmsdh_sdmmc_ids,
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 39)) && defined(CONFIG_PM)
+	.drv = {
+		.pm     = &bcmsdh_sdmmc_pm_ops,
+	},
+#endif
+	};
+
+struct sdos_info {
+	sdioh_info_t *sd;
+	spinlock_t lock;
+};
+
+
+int
+sdioh_sdmmc_osinit(sdioh_info_t *sd)
+{
+	struct sdos_info *sdos;
+
+	sdos = (struct sdos_info*)MALLOC(sd->osh, sizeof(struct sdos_info));
+	sd->sdos_info = (void*)sdos;
+	if (sdos == NULL)
+		return BCME_NOMEM;
+
+	sdos->sd = sd;
+	spin_lock_init(&sdos->lock);
+	return BCME_OK;
+}
+
+void
+sdioh_sdmmc_osfree(sdioh_info_t *sd)
+{
+	struct sdos_info *sdos;
+	ASSERT(sd && sd->sdos_info);
+
+	sdos = (struct sdos_info *)sd->sdos_info;
+	MFREE(sd->osh, sdos, sizeof(struct sdos_info));
+}
+
+/* Interrupt enable/disable */
+SDIOH_API_RC
+sdioh_interrupt_set(sdioh_info_t *sd, bool enable)
+{
+	ulong flags;
+	struct sdos_info *sdos;
+
+	sd_trace(("%s: %s\n", __FUNCTION__, enable ? "Enabling" : "Disabling"));
+
+	sdos = (struct sdos_info *)sd->sdos_info;
+	ASSERT(sdos);
+
+#if !defined(OOB_INTR_ONLY)
+	if (enable && !(sd->intr_handler && sd->intr_handler_arg)) {
+		sd_err(("%s: no handler registered, will not enable\n", __FUNCTION__));
+		return SDIOH_API_RC_FAIL;
+	}
+#endif /* !defined(OOB_INTR_ONLY) */
+
+	/* Ensure atomicity for enable/disable calls */
+	spin_lock_irqsave(&sdos->lock, flags);
+
+	sd->client_intr_enabled = enable;
+	if (enable) {
+		sdioh_sdmmc_devintr_on(sd);
+	} else {
+		sdioh_sdmmc_devintr_off(sd);
+	}
+
+	spin_unlock_irqrestore(&sdos->lock, flags);
+
+	return SDIOH_API_RC_SUCCESS;
+}
+
+
+#ifdef BCMSDH_MODULE
+static int __init
+bcmsdh_module_init(void)
+{
+	int error = 0;
+	sdio_function_init();
+	return error;
+}
+
+static void __exit
+bcmsdh_module_cleanup(void)
+{
+	sdio_function_cleanup();
+}
+
+module_init(bcmsdh_module_init);
+module_exit(bcmsdh_module_cleanup);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION(DESCRIPTION);
+MODULE_AUTHOR(AUTHOR);
+
+#endif /* BCMSDH_MODULE */
+/*
+ * module init
+*/
+int sdio_function_init(void)
+{
+	int error = 0;
+	sd_trace(("bcmsdh_sdmmc: %s Enter\n", __FUNCTION__));
+
+	gInstance = kzalloc(sizeof(BCMSDH_SDMMC_INSTANCE), GFP_KERNEL);
+	if (!gInstance)
+		return -ENOMEM;
+
+	error = sdio_register_driver(&bcmsdh_sdmmc_driver);
+
+	return error;
+}
+
+/*
+ * module cleanup
+*/
+extern int bcmsdh_remove(struct device *dev);
+void sdio_function_cleanup(void)
+{
+	sd_trace(("%s Enter\n", __FUNCTION__));
+
+	sdio_unregister_driver(&bcmsdh_sdmmc_driver);
+
+	if (gInstance)
+		kfree(gInstance);
+}
diff --git a/drivers/net/wireless/bcm4329/bcmsdspi.c b/drivers/net/wireless/bcm4329/bcmsdspi.c
new file mode 100644
index 000000000000..636539be5ea5
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/bcmsdspi.c
@@ -0,0 +1,1596 @@
+/*
+ * Broadcom BCMSDH to SPI Protocol Conversion Layer
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdspi.c,v 1.14.4.2.4.4.6.5 2010/03/10 03:09:48 Exp $
+ */
+
+#include <typedefs.h>
+
+#include <bcmdevs.h>
+#include <bcmendian.h>
+#include <bcmutils.h>
+#include <osl.h>
+#include <siutils.h>
+#include <sdio.h>		/* SDIO Device and Protocol Specs */
+#include <sdioh.h>		/* SDIO Host Controller Specification */
+#include <bcmsdbus.h>		/* bcmsdh to/from specific controller APIs */
+#include <sdiovar.h>		/* ioctl/iovars */
+
+#include <pcicfg.h>
+
+
+#include <bcmsdspi.h>
+#include <bcmspi.h>
+
+#include <proto/sdspi.h>
+
+#define SD_PAGE 4096
+
+/* Globals */
+
+uint sd_msglevel = SDH_ERROR_VAL;
+uint sd_hiok = FALSE;		/* Use hi-speed mode if available? */
+uint sd_sdmode = SDIOH_MODE_SPI;		/* Use SD4 mode by default */
+uint sd_f2_blocksize = 512;	/* Default blocksize */
+
+uint sd_divisor = 2;		/* Default 33MHz/2 = 16MHz for dongle */
+uint sd_power = 1;		/* Default to SD Slot powered ON */
+uint sd_clock = 1;		/* Default to SD Clock turned ON */
+uint sd_crc = 0;		/* Default to SPI CRC Check turned OFF */
+uint sd_pci_slot = 0xFFFFffff; /* Used to force selection of a particular PCI slot */
+
+uint sd_toctl = 7;
+
+/* Prototypes */
+static bool sdspi_start_power(sdioh_info_t *sd);
+static int sdspi_set_highspeed_mode(sdioh_info_t *sd, bool HSMode);
+static int sdspi_card_enablefuncs(sdioh_info_t *sd);
+static void sdspi_cmd_getrsp(sdioh_info_t *sd, uint32 *rsp_buffer, int count);
+static int sdspi_cmd_issue(sdioh_info_t *sd, bool use_dma, uint32 cmd, uint32 arg,
+                           uint32 *data, uint32 datalen);
+static int sdspi_card_regread(sdioh_info_t *sd, int func, uint32 regaddr,
+                              int regsize, uint32 *data);
+static int sdspi_card_regwrite(sdioh_info_t *sd, int func, uint32 regaddr,
+                               int regsize, uint32 data);
+static int sdspi_driver_init(sdioh_info_t *sd);
+static bool sdspi_reset(sdioh_info_t *sd, bool host_reset, bool client_reset);
+static int sdspi_card_buf(sdioh_info_t *sd, int rw, int func, bool fifo,
+                          uint32 addr, int nbytes, uint32 *data);
+static int sdspi_abort(sdioh_info_t *sd, uint func);
+
+static int set_client_block_size(sdioh_info_t *sd, int func, int blocksize);
+
+static uint8 sdspi_crc7(unsigned char* p, uint32 len);
+static uint16 sdspi_crc16(unsigned char* p, uint32 len);
+static int sdspi_crc_onoff(sdioh_info_t *sd, bool use_crc);
+
+/*
+ *  Public entry points & extern's
+ */
+extern sdioh_info_t *
+sdioh_attach(osl_t *osh, void *bar0, uint irq)
+{
+	sdioh_info_t *sd;
+
+	sd_trace(("%s\n", __FUNCTION__));
+	if ((sd = (sdioh_info_t *)MALLOC(osh, sizeof(sdioh_info_t))) == NULL) {
+		sd_err(("sdioh_attach: out of memory, malloced %d bytes\n", MALLOCED(osh)));
+		return NULL;
+	}
+	bzero((char *)sd, sizeof(sdioh_info_t));
+	sd->osh = osh;
+
+	if (spi_osinit(sd) != 0) {
+		sd_err(("%s: spi_osinit() failed\n", __FUNCTION__));
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+		return NULL;
+	}
+
+	sd->bar0 = (uintptr)bar0;
+	sd->irq = irq;
+	sd->intr_handler = NULL;
+	sd->intr_handler_arg = NULL;
+	sd->intr_handler_valid = FALSE;
+
+	/* Set defaults */
+	sd->sd_blockmode = FALSE;
+	sd->use_client_ints = TRUE;
+	sd->sd_use_dma = FALSE;	/* DMA Not supported */
+
+	/* Haven't figured out how to make bytemode work with dma */
+	if (!sd->sd_blockmode)
+		sd->sd_use_dma = 0;
+
+	if (!spi_hw_attach(sd)) {
+		sd_err(("%s: spi_hw_attach() failed\n", __FUNCTION__));
+		spi_osfree(sd);
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+		return NULL;
+	}
+
+	if (sdspi_driver_init(sd) != SUCCESS) {
+		if (sdspi_driver_init(sd) != SUCCESS) {
+			sd_err(("%s:sdspi_driver_init() failed()\n", __FUNCTION__));
+			spi_hw_detach(sd);
+			spi_osfree(sd);
+			MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+			return (NULL);
+		}
+	}
+
+	if (spi_register_irq(sd, irq) != SUCCESS) {
+		sd_err(("%s: spi_register_irq() failed for irq = %d\n", __FUNCTION__, irq));
+		spi_hw_detach(sd);
+		spi_osfree(sd);
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+		return (NULL);
+	}
+
+	sd_trace(("%s: Done\n", __FUNCTION__));
+	return sd;
+}
+
+extern SDIOH_API_RC
+sdioh_detach(osl_t *osh, sdioh_info_t *sd)
+{
+	sd_trace(("%s\n", __FUNCTION__));
+
+	if (sd) {
+		if (sd->card_init_done)
+			sdspi_reset(sd, 1, 1);
+
+		sd_info(("%s: detaching from hardware\n", __FUNCTION__));
+		spi_free_irq(sd->irq, sd);
+		spi_hw_detach(sd);
+		spi_osfree(sd);
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+	}
+
+	return SDIOH_API_RC_SUCCESS;
+}
+
+/* Configure callback to client when we recieve client interrupt */
+extern SDIOH_API_RC
+sdioh_interrupt_register(sdioh_info_t *sd, sdioh_cb_fn_t fn, void *argh)
+{
+	sd_trace(("%s: Entering\n", __FUNCTION__));
+
+	sd->intr_handler = fn;
+	sd->intr_handler_arg = argh;
+	sd->intr_handler_valid = TRUE;
+
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_interrupt_deregister(sdioh_info_t *sd)
+{
+	sd_trace(("%s: Entering\n", __FUNCTION__));
+
+	sd->intr_handler_valid = FALSE;
+	sd->intr_handler = NULL;
+	sd->intr_handler_arg = NULL;
+
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_interrupt_query(sdioh_info_t *sd, bool *onoff)
+{
+	sd_trace(("%s: Entering\n", __FUNCTION__));
+
+	*onoff = sd->client_intr_enabled;
+
+	return SDIOH_API_RC_SUCCESS;
+}
+
+#if defined(DHD_DEBUG)
+extern bool
+sdioh_interrupt_pending(sdioh_info_t *sd)
+{
+	return 0;
+}
+#endif
+
+uint
+sdioh_query_iofnum(sdioh_info_t *sd)
+{
+	return sd->num_funcs;
+}
+
+/* IOVar table */
+enum {
+	IOV_MSGLEVEL = 1,
+	IOV_BLOCKMODE,
+	IOV_BLOCKSIZE,
+	IOV_DMA,
+	IOV_USEINTS,
+	IOV_NUMINTS,
+	IOV_NUMLOCALINTS,
+	IOV_HOSTREG,
+	IOV_DEVREG,
+	IOV_DIVISOR,
+	IOV_SDMODE,
+	IOV_HISPEED,
+	IOV_HCIREGS,
+	IOV_POWER,
+	IOV_CLOCK,
+	IOV_CRC
+};
+
+const bcm_iovar_t sdioh_iovars[] = {
+	{"sd_msglevel",	IOV_MSGLEVEL, 	0,	IOVT_UINT32,	0 },
+	{"sd_blockmode", IOV_BLOCKMODE,	0,	IOVT_BOOL,	0 },
+	{"sd_blocksize", IOV_BLOCKSIZE, 0,	IOVT_UINT32,	0 }, /* ((fn << 16) | size) */
+	{"sd_dma",	IOV_DMA,	0,	IOVT_BOOL,	0 },
+	{"sd_ints",	IOV_USEINTS,	0,	IOVT_BOOL,	0 },
+	{"sd_numints",	IOV_NUMINTS,	0,	IOVT_UINT32,	0 },
+	{"sd_numlocalints", IOV_NUMLOCALINTS, 0, IOVT_UINT32,	0 },
+	{"sd_hostreg",	IOV_HOSTREG,	0,	IOVT_BUFFER,	sizeof(sdreg_t) },
+	{"sd_devreg",	IOV_DEVREG,	0,	IOVT_BUFFER,	sizeof(sdreg_t)	},
+	{"sd_divisor",	IOV_DIVISOR,	0,	IOVT_UINT32,	0 },
+	{"sd_power",	IOV_POWER,	0,	IOVT_UINT32,	0 },
+	{"sd_clock",	IOV_CLOCK,	0,	IOVT_UINT32,	0 },
+	{"sd_crc",	IOV_CRC,	0,	IOVT_UINT32,	0 },
+	{"sd_mode",	IOV_SDMODE,	0,	IOVT_UINT32,	100},
+	{"sd_highspeed",	IOV_HISPEED,	0,	IOVT_UINT32,	0},
+	{NULL, 0, 0, 0, 0 }
+};
+
+int
+sdioh_iovar_op(sdioh_info_t *si, const char *name,
+               void *params, int plen, void *arg, int len, bool set)
+{
+	const bcm_iovar_t *vi = NULL;
+	int bcmerror = 0;
+	int val_size;
+	int32 int_val = 0;
+	bool bool_val;
+	uint32 actionid;
+
+	ASSERT(name);
+	ASSERT(len >= 0);
+
+	/* Get must have return space; Set does not take qualifiers */
+	ASSERT(set || (arg && len));
+	ASSERT(!set || (!params && !plen));
+
+	sd_trace(("%s: Enter (%s %s)\n", __FUNCTION__, (set ? "set" : "get"), name));
+
+	if ((vi = bcm_iovar_lookup(sdioh_iovars, name)) == NULL) {
+		bcmerror = BCME_UNSUPPORTED;
+		goto exit;
+	}
+
+	if ((bcmerror = bcm_iovar_lencheck(vi, arg, len, set)) != 0)
+		goto exit;
+
+	/* Set up params so get and set can share the convenience variables */
+	if (params == NULL) {
+		params = arg;
+		plen = len;
+	}
+
+	if (vi->type == IOVT_VOID)
+		val_size = 0;
+	else if (vi->type == IOVT_BUFFER)
+		val_size = len;
+	else
+		val_size = sizeof(int);
+
+	if (plen >= (int)sizeof(int_val))
+		bcopy(params, &int_val, sizeof(int_val));
+
+	bool_val = (int_val != 0) ? TRUE : FALSE;
+
+	actionid = set ? IOV_SVAL(vi->varid) : IOV_GVAL(vi->varid);
+	switch (actionid) {
+	case IOV_GVAL(IOV_MSGLEVEL):
+		int_val = (int32)sd_msglevel;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_MSGLEVEL):
+		sd_msglevel = int_val;
+		break;
+
+	case IOV_GVAL(IOV_BLOCKMODE):
+		int_val = (int32)si->sd_blockmode;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_BLOCKMODE):
+		si->sd_blockmode = (bool)int_val;
+		/* Haven't figured out how to make non-block mode with DMA */
+		if (!si->sd_blockmode)
+			si->sd_use_dma = 0;
+		break;
+
+	case IOV_GVAL(IOV_BLOCKSIZE):
+		if ((uint32)int_val > si->num_funcs) {
+			bcmerror = BCME_BADARG;
+			break;
+		}
+		int_val = (int32)si->client_block_size[int_val];
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_BLOCKSIZE):
+	{
+		uint func = ((uint32)int_val >> 16);
+		uint blksize = (uint16)int_val;
+		uint maxsize;
+
+		if (func > si->num_funcs) {
+			bcmerror = BCME_BADARG;
+			break;
+		}
+
+		switch (func) {
+		case 0: maxsize = 32; break;
+		case 1: maxsize = BLOCK_SIZE_4318; break;
+		case 2: maxsize = BLOCK_SIZE_4328; break;
+		default: maxsize = 0;
+		}
+		if (blksize > maxsize) {
+			bcmerror = BCME_BADARG;
+			break;
+		}
+		if (!blksize) {
+			blksize = maxsize;
+		}
+
+		/* Now set it */
+		spi_lock(si);
+		bcmerror = set_client_block_size(si, func, blksize);
+		spi_unlock(si);
+		break;
+	}
+
+	case IOV_GVAL(IOV_DMA):
+		int_val = (int32)si->sd_use_dma;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_DMA):
+		si->sd_use_dma = (bool)int_val;
+		break;
+
+	case IOV_GVAL(IOV_USEINTS):
+		int_val = (int32)si->use_client_ints;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_USEINTS):
+		break;
+
+	case IOV_GVAL(IOV_DIVISOR):
+		int_val = (uint32)sd_divisor;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_DIVISOR):
+		sd_divisor = int_val;
+		if (!spi_start_clock(si, (uint16)sd_divisor)) {
+			sd_err(("set clock failed!\n"));
+			bcmerror = BCME_ERROR;
+		}
+		break;
+
+	case IOV_GVAL(IOV_POWER):
+		int_val = (uint32)sd_power;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_POWER):
+		sd_power = int_val;
+		break;
+
+	case IOV_GVAL(IOV_CLOCK):
+		int_val = (uint32)sd_clock;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_CLOCK):
+		sd_clock = int_val;
+		break;
+
+	case IOV_GVAL(IOV_CRC):
+		int_val = (uint32)sd_crc;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_CRC):
+		/* Apply new setting, but don't change sd_crc until
+		 * after the CRC-mode is selected in the device.  This
+		 * is required because the software must generate a
+		 * correct CRC for the CMD59 in order to be able to
+		 * turn OFF the CRC.
+		 */
+		sdspi_crc_onoff(si, int_val ? 1 : 0);
+		sd_crc = int_val;
+		break;
+
+	case IOV_GVAL(IOV_SDMODE):
+		int_val = (uint32)sd_sdmode;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_SDMODE):
+		sd_sdmode = int_val;
+		break;
+
+	case IOV_GVAL(IOV_HISPEED):
+		int_val = (uint32)sd_hiok;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_HISPEED):
+		sd_hiok = int_val;
+
+		if (!sdspi_set_highspeed_mode(si, (bool)sd_hiok)) {
+			sd_err(("Failed changing highspeed mode to %d.\n", sd_hiok));
+			bcmerror = BCME_ERROR;
+			return ERROR;
+		}
+		break;
+
+	case IOV_GVAL(IOV_NUMINTS):
+		int_val = (int32)si->intrcount;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_NUMLOCALINTS):
+		int_val = (int32)si->local_intrcount;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_HOSTREG):
+	{
+		break;
+	}
+
+	case IOV_SVAL(IOV_HOSTREG):
+	{
+		sd_err(("IOV_HOSTREG unsupported\n"));
+		break;
+	}
+
+	case IOV_GVAL(IOV_DEVREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+		uint8 data;
+
+		if (sdioh_cfg_read(si, sd_ptr->func, sd_ptr->offset, &data)) {
+			bcmerror = BCME_SDIO_ERROR;
+			break;
+		}
+
+		int_val = (int)data;
+		bcopy(&int_val, arg, sizeof(int_val));
+		break;
+	}
+
+	case IOV_SVAL(IOV_DEVREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+		uint8 data = (uint8)sd_ptr->value;
+
+		if (sdioh_cfg_write(si, sd_ptr->func, sd_ptr->offset, &data)) {
+			bcmerror = BCME_SDIO_ERROR;
+			break;
+		}
+		break;
+	}
+
+
+	default:
+		bcmerror = BCME_UNSUPPORTED;
+		break;
+	}
+exit:
+
+	return bcmerror;
+}
+
+extern SDIOH_API_RC
+sdioh_cfg_read(sdioh_info_t *sd, uint fnc_num, uint32 addr, uint8 *data)
+{
+	SDIOH_API_RC status;
+	/* No lock needed since sdioh_request_byte does locking */
+	status = sdioh_request_byte(sd, SDIOH_READ, fnc_num, addr, data);
+	return status;
+}
+
+extern SDIOH_API_RC
+sdioh_cfg_write(sdioh_info_t *sd, uint fnc_num, uint32 addr, uint8 *data)
+{
+	/* No lock needed since sdioh_request_byte does locking */
+	SDIOH_API_RC status;
+	status = sdioh_request_byte(sd, SDIOH_WRITE, fnc_num, addr, data);
+	return status;
+}
+
+extern SDIOH_API_RC
+sdioh_cis_read(sdioh_info_t *sd, uint func, uint8 *cisd, uint32 length)
+{
+	uint32 count;
+	int offset;
+	uint32 foo;
+	uint8 *cis = cisd;
+
+	sd_trace(("%s: Func = %d\n", __FUNCTION__, func));
+
+	if (!sd->func_cis_ptr[func]) {
+		bzero(cis, length);
+		return SDIOH_API_RC_FAIL;
+	}
+
+	spi_lock(sd);
+	*cis = 0;
+	for (count = 0; count < length; count++) {
+		offset =  sd->func_cis_ptr[func] + count;
+		if (sdspi_card_regread (sd, 0, offset, 1, &foo) < 0) {
+			sd_err(("%s: regread failed: Can't read CIS\n", __FUNCTION__));
+			spi_unlock(sd);
+			return SDIOH_API_RC_FAIL;
+		}
+		*cis = (uint8)(foo & 0xff);
+		cis++;
+	}
+	spi_unlock(sd);
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_request_byte(sdioh_info_t *sd, uint rw, uint func, uint regaddr, uint8 *byte)
+{
+	int status;
+	uint32 cmd_arg;
+	uint32 rsp5;
+
+	spi_lock(sd);
+
+	cmd_arg = 0;
+	cmd_arg = SFIELD(cmd_arg, CMD52_FUNCTION, func);
+	cmd_arg = SFIELD(cmd_arg, CMD52_REG_ADDR, regaddr);
+	cmd_arg = SFIELD(cmd_arg, CMD52_RW_FLAG, rw == SDIOH_READ ? 0 : 1);
+	cmd_arg = SFIELD(cmd_arg, CMD52_RAW, 0);
+	cmd_arg = SFIELD(cmd_arg, CMD52_DATA, rw == SDIOH_READ ? 0 : *byte);
+
+	sd_trace(("%s: rw=%d, func=%d, regaddr=0x%08x\n", __FUNCTION__, rw, func, regaddr));
+
+	if ((status = sdspi_cmd_issue(sd, sd->sd_use_dma,
+	                              SDIOH_CMD_52, cmd_arg, NULL, 0)) != SUCCESS) {
+		spi_unlock(sd);
+		return status;
+	}
+
+	sdspi_cmd_getrsp(sd, &rsp5, 1);
+	if (rsp5 != 0x00) {
+		sd_err(("%s: rsp5 flags is 0x%x func=%d\n",
+		        __FUNCTION__, rsp5, func));
+		/* ASSERT(0); */
+		spi_unlock(sd);
+		return SDIOH_API_RC_FAIL;
+	}
+
+	if (rw == SDIOH_READ)
+		*byte = sd->card_rsp_data >> 24;
+
+	spi_unlock(sd);
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_request_word(sdioh_info_t *sd, uint cmd_type, uint rw, uint func, uint addr,
+                   uint32 *word, uint nbytes)
+{
+	int status;
+
+	spi_lock(sd);
+
+	if (rw == SDIOH_READ)
+		status = sdspi_card_regread(sd, func, addr, nbytes, word);
+	else
+		status = sdspi_card_regwrite(sd, func, addr, nbytes, *word);
+
+	spi_unlock(sd);
+	return (status == SUCCESS ?  SDIOH_API_RC_SUCCESS : SDIOH_API_RC_FAIL);
+}
+
+extern SDIOH_API_RC
+sdioh_request_buffer(sdioh_info_t *sd, uint pio_dma, uint fix_inc, uint rw, uint func,
+                     uint addr, uint reg_width, uint buflen_u, uint8 *buffer, void *pkt)
+{
+	int len;
+	int buflen = (int)buflen_u;
+	bool fifo = (fix_inc == SDIOH_DATA_FIX);
+
+	spi_lock(sd);
+
+	ASSERT(reg_width == 4);
+	ASSERT(buflen_u < (1 << 30));
+	ASSERT(sd->client_block_size[func]);
+
+	sd_data(("%s: %c len %d r_cnt %d t_cnt %d, pkt @0x%p\n",
+	         __FUNCTION__, rw == SDIOH_READ ? 'R' : 'W',
+	         buflen_u, sd->r_cnt, sd->t_cnt, pkt));
+
+	/* Break buffer down into blocksize chunks:
+	 * Bytemode: 1 block at a time.
+	 */
+	while (buflen > 0) {
+		if (sd->sd_blockmode) {
+			/* Max xfer is Page size */
+			len = MIN(SD_PAGE, buflen);
+
+			/* Round down to a block boundry */
+			if (buflen > sd->client_block_size[func])
+				len = (len/sd->client_block_size[func]) *
+				        sd->client_block_size[func];
+		} else {
+			/* Byte mode: One block at a time */
+			len = MIN(sd->client_block_size[func], buflen);
+		}
+
+		if (sdspi_card_buf(sd, rw, func, fifo, addr, len, (uint32 *)buffer) != SUCCESS) {
+			spi_unlock(sd);
+			return SDIOH_API_RC_FAIL;
+		}
+		buffer += len;
+		buflen -= len;
+		if (!fifo)
+			addr += len;
+	}
+	spi_unlock(sd);
+	return SDIOH_API_RC_SUCCESS;
+}
+
+static int
+sdspi_abort(sdioh_info_t *sd, uint func)
+{
+	uint8 spi_databuf[] = { 0x74, 0x80, 0x00, 0x0C, 0xFF, 0x95, 0xFF, 0xFF,
+	                        0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+	uint8 spi_rspbuf[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+	                       0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+	int err = 0;
+
+	sd_err(("Sending SPI Abort to F%d\n", func));
+	spi_databuf[4] = func & 0x7;
+	/* write to function 0, addr 6 (IOABORT) func # in 3 LSBs. */
+	spi_sendrecv(sd, spi_databuf, spi_rspbuf, sizeof(spi_databuf));
+
+	return err;
+}
+
+extern int
+sdioh_abort(sdioh_info_t *sd, uint fnum)
+{
+	int ret;
+
+	spi_lock(sd);
+	ret = sdspi_abort(sd, fnum);
+	spi_unlock(sd);
+
+	return ret;
+}
+
+int
+sdioh_start(sdioh_info_t *sd, int stage)
+{
+	return SUCCESS;
+}
+
+int
+sdioh_stop(sdioh_info_t *sd)
+{
+	return SUCCESS;
+}
+
+
+/*
+ * Private/Static work routines
+ */
+static bool
+sdspi_reset(sdioh_info_t *sd, bool host_reset, bool client_reset)
+{
+	if (!sd)
+		return TRUE;
+
+	spi_lock(sd);
+	/* Reset client card */
+	if (client_reset && (sd->adapter_slot != -1)) {
+		if (sdspi_card_regwrite(sd, 0, SDIOD_CCCR_IOABORT, 1, 0x8) != SUCCESS)
+			sd_err(("%s: Cannot write to card reg 0x%x\n",
+			        __FUNCTION__, SDIOD_CCCR_IOABORT));
+		else
+			sd->card_rca = 0;
+	}
+
+	/* The host reset is a NOP in the sd-spi case. */
+	if (host_reset) {
+		sd->sd_mode = SDIOH_MODE_SPI;
+	}
+	spi_unlock(sd);
+	return TRUE;
+}
+
+static int
+sdspi_host_init(sdioh_info_t *sd)
+{
+	sdspi_reset(sd, 1, 0);
+
+	/* Default power on mode is SD1 */
+	sd->sd_mode = SDIOH_MODE_SPI;
+	sd->polled_mode = TRUE;
+	sd->host_init_done = TRUE;
+	sd->card_init_done = FALSE;
+	sd->adapter_slot = 1;
+
+	return (SUCCESS);
+}
+
+#define CMD0_RETRIES 3
+#define CMD5_RETRIES 10
+
+static int
+get_ocr(sdioh_info_t *sd, uint32 *cmd_arg, uint32 *cmd_rsp)
+{
+	uint32 rsp5;
+	int retries, status;
+
+	/* First issue a CMD0 to get the card into SPI mode. */
+	for (retries = 0; retries <= CMD0_RETRIES; retries++) {
+		if ((status = sdspi_cmd_issue(sd, sd->sd_use_dma,
+		                              SDIOH_CMD_0, *cmd_arg, NULL, 0)) != SUCCESS) {
+			sd_err(("%s: No response to CMD0\n", __FUNCTION__));
+			continue;
+		}
+
+		sdspi_cmd_getrsp(sd, &rsp5, 1);
+
+		if (GFIELD(rsp5, SPI_RSP_ILL_CMD)) {
+			printf("%s: Card already initialized (continuing)\n", __FUNCTION__);
+			break;
+		}
+
+		if (GFIELD(rsp5, SPI_RSP_IDLE)) {
+			printf("%s: Card in SPI mode\n", __FUNCTION__);
+			break;
+		}
+	}
+
+	if (retries > CMD0_RETRIES) {
+		sd_err(("%s: Too many retries for CMD0\n", __FUNCTION__));
+		return ERROR;
+	}
+
+	/* Get the Card's Operation Condition. */
+	/* Occasionally the board takes a while to become ready. */
+	for (retries = 0; retries <= CMD5_RETRIES; retries++) {
+		if ((status = sdspi_cmd_issue(sd, sd->sd_use_dma,
+		                              SDIOH_CMD_5, *cmd_arg, NULL, 0)) != SUCCESS) {
+			sd_err(("%s: No response to CMD5\n", __FUNCTION__));
+			continue;
+		}
+
+		printf("CMD5 response data was: 0x%08x\n", sd->card_rsp_data);
+
+		if (GFIELD(sd->card_rsp_data, RSP4_CARD_READY)) {
+			printf("%s: Card ready\n", __FUNCTION__);
+			break;
+		}
+	}
+
+	if (retries > CMD5_RETRIES) {
+		sd_err(("%s: Too many retries for CMD5\n", __FUNCTION__));
+		return ERROR;
+	}
+
+	*cmd_rsp = sd->card_rsp_data;
+
+	sdspi_crc_onoff(sd, sd_crc ? 1 : 0);
+
+	return (SUCCESS);
+}
+
+static int
+sdspi_crc_onoff(sdioh_info_t *sd, bool use_crc)
+{
+	uint32 args;
+	int status;
+
+	args = use_crc ? 1 : 0;
+	if ((status = sdspi_cmd_issue(sd, sd->sd_use_dma,
+	                              SDIOH_CMD_59, args, NULL, 0)) != SUCCESS) {
+		sd_err(("%s: No response to CMD59\n", __FUNCTION__));
+	}
+
+	sd_info(("CMD59 response data was: 0x%08x\n", sd->card_rsp_data));
+
+	sd_err(("SD-SPI CRC turned %s\n", use_crc ? "ON" : "OFF"));
+	return (SUCCESS);
+}
+
+static int
+sdspi_client_init(sdioh_info_t *sd)
+{
+	uint8 fn_ints;
+
+	sd_trace(("%s: Powering up slot %d\n", __FUNCTION__, sd->adapter_slot));
+
+	/* Start at ~400KHz clock rate for initialization */
+	if (!spi_start_clock(sd, 128)) {
+		sd_err(("spi_start_clock failed\n"));
+		return ERROR;
+	}
+
+	if (!sdspi_start_power(sd)) {
+		sd_err(("sdspi_start_power failed\n"));
+		return ERROR;
+	}
+
+	if (sd->num_funcs == 0) {
+		sd_err(("%s: No IO funcs!\n", __FUNCTION__));
+		return ERROR;
+	}
+
+	sdspi_card_enablefuncs(sd);
+
+	set_client_block_size(sd, 1, BLOCK_SIZE_4318);
+	fn_ints = INTR_CTL_FUNC1_EN;
+
+	if (sd->num_funcs >= 2) {
+		set_client_block_size(sd, 2, sd_f2_blocksize /* BLOCK_SIZE_4328 */);
+		fn_ints |= INTR_CTL_FUNC2_EN;
+	}
+
+	/* Enable/Disable Client interrupts */
+	/* Turn on here but disable at host controller */
+	if (sdspi_card_regwrite(sd, 0, SDIOD_CCCR_INTEN, 1,
+	                        (fn_ints | INTR_CTL_MASTER_EN)) != SUCCESS) {
+		sd_err(("%s: Could not enable ints in CCCR\n", __FUNCTION__));
+		return ERROR;
+	}
+
+	/* Switch to High-speed clocking mode if both host and device support it */
+	sdspi_set_highspeed_mode(sd, (bool)sd_hiok);
+
+	/* After configuring for High-Speed mode, set the desired clock rate. */
+	if (!spi_start_clock(sd, (uint16)sd_divisor)) {
+		sd_err(("spi_start_clock failed\n"));
+		return ERROR;
+	}
+
+	sd->card_init_done = TRUE;
+
+	return SUCCESS;
+}
+
+static int
+sdspi_set_highspeed_mode(sdioh_info_t *sd, bool HSMode)
+{
+	uint32 regdata;
+	int status;
+	bool hsmode;
+
+	if (HSMode == TRUE) {
+
+		sd_err(("Attempting to enable High-Speed mode.\n"));
+
+		if ((status = sdspi_card_regread(sd, 0, SDIOD_CCCR_SPEED_CONTROL,
+		                                 1, &regdata)) != SUCCESS) {
+			return status;
+		}
+		if (regdata & SDIO_SPEED_SHS) {
+			sd_err(("Device supports High-Speed mode.\n"));
+
+			regdata |= SDIO_SPEED_EHS;
+
+			sd_err(("Writing %08x to Card at %08x\n",
+			         regdata, SDIOD_CCCR_SPEED_CONTROL));
+			if ((status = sdspi_card_regwrite(sd, 0, SDIOD_CCCR_SPEED_CONTROL,
+			                                  1, regdata)) != BCME_OK) {
+				return status;
+			}
+
+			hsmode = 1;
+
+			sd_err(("High-speed clocking mode enabled.\n"));
+		}
+		else {
+			sd_err(("Device does not support High-Speed Mode.\n"));
+			hsmode = 0;
+		}
+	} else {
+		if ((status = sdspi_card_regread(sd, 0, SDIOD_CCCR_SPEED_CONTROL,
+		                                 1, &regdata)) != SUCCESS) {
+			return status;
+		}
+
+		regdata = ~SDIO_SPEED_EHS;
+
+		sd_err(("Writing %08x to Card at %08x\n",
+		         regdata, SDIOD_CCCR_SPEED_CONTROL));
+		if ((status = sdspi_card_regwrite(sd, 0, SDIOD_CCCR_SPEED_CONTROL,
+		                                  1, regdata)) != BCME_OK) {
+			return status;
+		}
+
+		sd_err(("Low-speed clocking mode enabled.\n"));
+		hsmode = 0;
+	}
+
+	spi_controller_highspeed_mode(sd, hsmode);
+
+	return TRUE;
+}
+
+bool
+sdspi_start_power(sdioh_info_t *sd)
+{
+	uint32 cmd_arg;
+	uint32 cmd_rsp;
+
+	sd_trace(("%s\n", __FUNCTION__));
+
+	/* Get the Card's Operation Condition.  Occasionally the board
+	 * takes a while to become ready
+	 */
+
+	cmd_arg = 0;
+	if (get_ocr(sd, &cmd_arg, &cmd_rsp) != SUCCESS) {
+		sd_err(("%s: Failed to get OCR; bailing\n", __FUNCTION__));
+		return FALSE;
+	}
+
+	sd_err(("mem_present = %d\n", GFIELD(cmd_rsp, RSP4_MEM_PRESENT)));
+	sd_err(("num_funcs = %d\n", GFIELD(cmd_rsp, RSP4_NUM_FUNCS)));
+	sd_err(("card_ready = %d\n", GFIELD(cmd_rsp, RSP4_CARD_READY)));
+	sd_err(("OCR = 0x%x\n", GFIELD(cmd_rsp, RSP4_IO_OCR)));
+
+	/* Verify that the card supports I/O mode */
+	if (GFIELD(cmd_rsp, RSP4_NUM_FUNCS) == 0) {
+		sd_err(("%s: Card does not support I/O\n", __FUNCTION__));
+		return ERROR;
+	}
+
+	sd->num_funcs = GFIELD(cmd_rsp, RSP4_NUM_FUNCS);
+
+	/* Examine voltage: Arasan only supports 3.3 volts,
+	 * so look for 3.2-3.3 Volts and also 3.3-3.4 volts.
+	 */
+
+	if ((GFIELD(cmd_rsp, RSP4_IO_OCR) & (0x3 << 20)) == 0) {
+		sd_err(("This client does not support 3.3 volts!\n"));
+		return ERROR;
+	}
+
+
+	return TRUE;
+}
+
+static int
+sdspi_driver_init(sdioh_info_t *sd)
+{
+	sd_trace(("%s\n", __FUNCTION__));
+
+	if ((sdspi_host_init(sd)) != SUCCESS) {
+		return ERROR;
+	}
+
+	if (sdspi_client_init(sd) != SUCCESS) {
+		return ERROR;
+	}
+
+	return SUCCESS;
+}
+
+static int
+sdspi_card_enablefuncs(sdioh_info_t *sd)
+{
+	int status;
+	uint32 regdata;
+	uint32 regaddr, fbraddr;
+	uint8 func;
+	uint8 *ptr;
+
+	sd_trace(("%s\n", __FUNCTION__));
+	/* Get the Card's common CIS address */
+	ptr = (uint8 *) &sd->com_cis_ptr;
+	for (regaddr = SDIOD_CCCR_CISPTR_0; regaddr <= SDIOD_CCCR_CISPTR_2; regaddr++) {
+		if ((status = sdspi_card_regread (sd, 0, regaddr, 1, &regdata)) != SUCCESS)
+			return status;
+
+		*ptr++ = (uint8) regdata;
+	}
+
+	/* Only the lower 17-bits are valid */
+	sd->com_cis_ptr &= 0x0001FFFF;
+	sd->func_cis_ptr[0] = sd->com_cis_ptr;
+	sd_info(("%s: Card's Common CIS Ptr = 0x%x\n", __FUNCTION__, sd->com_cis_ptr));
+
+	/* Get the Card's function CIS (for each function) */
+	for (fbraddr = SDIOD_FBR_STARTADDR, func = 1;
+	     func <= sd->num_funcs; func++, fbraddr += SDIOD_FBR_SIZE) {
+		ptr = (uint8 *) &sd->func_cis_ptr[func];
+		for (regaddr = SDIOD_FBR_CISPTR_0; regaddr <= SDIOD_FBR_CISPTR_2; regaddr++) {
+			if ((status = sdspi_card_regread (sd, 0, regaddr + fbraddr, 1, &regdata))
+			    != SUCCESS)
+				return status;
+
+			*ptr++ = (uint8) regdata;
+		}
+
+		/* Only the lower 17-bits are valid */
+		sd->func_cis_ptr[func] &= 0x0001FFFF;
+		sd_info(("%s: Function %d CIS Ptr = 0x%x\n",
+		         __FUNCTION__, func, sd->func_cis_ptr[func]));
+	}
+
+	sd_info(("%s: write ESCI bit\n", __FUNCTION__));
+	/* Enable continuous SPI interrupt (ESCI bit) */
+	sdspi_card_regwrite(sd, 0, SDIOD_CCCR_BICTRL, 1, 0x60);
+
+	sd_info(("%s: enable f1\n", __FUNCTION__));
+	/* Enable function 1 on the card */
+	regdata = SDIO_FUNC_ENABLE_1;
+	if ((status = sdspi_card_regwrite(sd, 0, SDIOD_CCCR_IOEN, 1, regdata)) != SUCCESS)
+		return status;
+
+	sd_info(("%s: done\n", __FUNCTION__));
+	return SUCCESS;
+}
+
+/* Read client card reg */
+static int
+sdspi_card_regread(sdioh_info_t *sd, int func, uint32 regaddr, int regsize, uint32 *data)
+{
+	int status;
+	uint32 cmd_arg;
+	uint32 rsp5;
+
+	cmd_arg = 0;
+
+	if ((func == 0) || (regsize == 1)) {
+		cmd_arg = SFIELD(cmd_arg, CMD52_FUNCTION, func);
+		cmd_arg = SFIELD(cmd_arg, CMD52_REG_ADDR, regaddr);
+		cmd_arg = SFIELD(cmd_arg, CMD52_RW_FLAG, SDIOH_XFER_TYPE_READ);
+		cmd_arg = SFIELD(cmd_arg, CMD52_RAW, 0);
+		cmd_arg = SFIELD(cmd_arg, CMD52_DATA, 0);
+
+		if ((status = sdspi_cmd_issue(sd, sd->sd_use_dma, SDIOH_CMD_52, cmd_arg, NULL, 0))
+		    != SUCCESS)
+			return status;
+
+		sdspi_cmd_getrsp(sd, &rsp5, 1);
+
+		if (rsp5 != 0x00)
+			sd_err(("%s: rsp5 flags is 0x%x\t %d\n",
+			        __FUNCTION__, rsp5, func));
+
+		*data = sd->card_rsp_data >> 24;
+	} else {
+		cmd_arg = SFIELD(cmd_arg, CMD53_BYTE_BLK_CNT, regsize);
+		cmd_arg = SFIELD(cmd_arg, CMD53_OP_CODE, 1);
+		cmd_arg = SFIELD(cmd_arg, CMD53_BLK_MODE, 0);
+		cmd_arg = SFIELD(cmd_arg, CMD53_FUNCTION, func);
+		cmd_arg = SFIELD(cmd_arg, CMD53_REG_ADDR, regaddr);
+		cmd_arg = SFIELD(cmd_arg, CMD53_RW_FLAG, SDIOH_XFER_TYPE_READ);
+
+		sd->data_xfer_count = regsize;
+
+		/* sdspi_cmd_issue() returns with the command complete bit
+		 * in the ISR already cleared
+		 */
+		if ((status = sdspi_cmd_issue(sd, sd->sd_use_dma, SDIOH_CMD_53, cmd_arg, NULL, 0))
+		    != SUCCESS)
+			return status;
+
+		sdspi_cmd_getrsp(sd, &rsp5, 1);
+
+		if (rsp5 != 0x00)
+			sd_err(("%s: rsp5 flags is 0x%x\t %d\n",
+			        __FUNCTION__, rsp5, func));
+
+		*data = sd->card_rsp_data;
+		if (regsize == 2) {
+			*data &= 0xffff;
+		}
+
+		sd_info(("%s: CMD53 func %d, addr 0x%x, size %d, data 0x%08x\n",
+		         __FUNCTION__, func, regaddr, regsize, *data));
+
+
+	}
+
+	return SUCCESS;
+}
+
+/* write a client register */
+static int
+sdspi_card_regwrite(sdioh_info_t *sd, int func, uint32 regaddr, int regsize, uint32 data)
+{
+	int status;
+	uint32 cmd_arg, rsp5, flags;
+
+	cmd_arg = 0;
+
+	if ((func == 0) || (regsize == 1)) {
+		cmd_arg = SFIELD(cmd_arg, CMD52_FUNCTION, func);
+		cmd_arg = SFIELD(cmd_arg, CMD52_REG_ADDR, regaddr);
+		cmd_arg = SFIELD(cmd_arg, CMD52_RW_FLAG, SDIOH_XFER_TYPE_WRITE);
+		cmd_arg = SFIELD(cmd_arg, CMD52_RAW, 0);
+		cmd_arg = SFIELD(cmd_arg, CMD52_DATA, data & 0xff);
+		if ((status = sdspi_cmd_issue(sd, sd->sd_use_dma, SDIOH_CMD_52, cmd_arg, NULL, 0))
+		    != SUCCESS)
+			return status;
+
+		sdspi_cmd_getrsp(sd, &rsp5, 1);
+		flags = GFIELD(rsp5, RSP5_FLAGS);
+		if (flags && (flags != 0x10))
+			sd_err(("%s: rsp5.rsp5.flags = 0x%x, expecting 0x10\n",
+			        __FUNCTION__,  flags));
+	}
+	else {
+		cmd_arg = SFIELD(cmd_arg, CMD53_BYTE_BLK_CNT, regsize);
+		cmd_arg = SFIELD(cmd_arg, CMD53_OP_CODE, 1);
+		cmd_arg = SFIELD(cmd_arg, CMD53_BLK_MODE, 0);
+		cmd_arg = SFIELD(cmd_arg, CMD53_FUNCTION, func);
+		cmd_arg = SFIELD(cmd_arg, CMD53_REG_ADDR, regaddr);
+		cmd_arg = SFIELD(cmd_arg, CMD53_RW_FLAG, SDIOH_XFER_TYPE_WRITE);
+
+		sd->data_xfer_count = regsize;
+		sd->cmd53_wr_data = data;
+
+		sd_info(("%s: CMD53 func %d, addr 0x%x, size %d, data 0x%08x\n",
+		         __FUNCTION__, func, regaddr, regsize, data));
+
+		/* sdspi_cmd_issue() returns with the command complete bit
+		 * in the ISR already cleared
+		 */
+		if ((status = sdspi_cmd_issue(sd, sd->sd_use_dma, SDIOH_CMD_53, cmd_arg, NULL, 0))
+		    != SUCCESS)
+			return status;
+
+		sdspi_cmd_getrsp(sd, &rsp5, 1);
+
+		if (rsp5 != 0x00)
+			sd_err(("%s: rsp5 flags = 0x%x, expecting 0x00\n",
+			        __FUNCTION__,  rsp5));
+
+	}
+	return SUCCESS;
+}
+
+void
+sdspi_cmd_getrsp(sdioh_info_t *sd, uint32 *rsp_buffer, int count /* num 32 bit words */)
+{
+	*rsp_buffer = sd->card_response;
+}
+
+int max_errors = 0;
+
+#define SPI_MAX_PKT_LEN		768
+uint8	spi_databuf[SPI_MAX_PKT_LEN];
+uint8	spi_rspbuf[SPI_MAX_PKT_LEN];
+
+/* datalen is used for CMD53 length only (0 for sd->data_xfer_count) */
+static int
+sdspi_cmd_issue(sdioh_info_t *sd, bool use_dma, uint32 cmd, uint32 arg,
+                uint32 *data, uint32 datalen)
+{
+	uint32 cmd_reg;
+	uint32 cmd_arg = arg;
+	uint8 cmd_crc = 0x95;		/* correct CRC for CMD0 and don't care for others. */
+	uint16 dat_crc;
+	uint8 cmd52data = 0;
+	uint32 i, j;
+	uint32 spi_datalen = 0;
+	uint32 spi_pre_cmd_pad	= 0;
+	uint32 spi_max_response_pad = 128;
+
+	cmd_reg = 0;
+	cmd_reg = SFIELD(cmd_reg, SPI_DIR, 1);
+	cmd_reg = SFIELD(cmd_reg, SPI_CMD_INDEX, cmd);
+
+	if (GFIELD(cmd_arg, CMD52_RW_FLAG) == 1) {	/* Same for CMD52 and CMD53 */
+		cmd_reg = SFIELD(cmd_reg, SPI_RW, 1);
+	}
+
+	switch (cmd) {
+	case SDIOH_CMD_59:	/* CRC_ON_OFF (SPI Mode Only) - Response R1 */
+		cmd52data = arg & 0x1;
+	case SDIOH_CMD_0:	/* Set Card to Idle State - No Response */
+	case SDIOH_CMD_5:	/* Send Operation condition - Response R4 */
+		sd_trace(("%s: CMD%d\n", __FUNCTION__, cmd));
+		spi_datalen = 44;
+		spi_pre_cmd_pad = 12;
+		spi_max_response_pad = 28;
+		break;
+
+	case SDIOH_CMD_3:	/* Ask card to send RCA - Response R6 */
+	case SDIOH_CMD_7:	/* Select card - Response R1 */
+	case SDIOH_CMD_15:	/* Set card to inactive state - Response None */
+		sd_err(("%s: CMD%d is invalid for SPI Mode.\n", __FUNCTION__, cmd));
+		return ERROR;
+		break;
+
+	case SDIOH_CMD_52:	/* IO R/W Direct (single byte) - Response R5 */
+		cmd52data = GFIELD(cmd_arg, CMD52_DATA);
+		cmd_arg = arg;
+		cmd_reg = SFIELD(cmd_reg, SPI_FUNC, GFIELD(cmd_arg, CMD52_FUNCTION));
+		cmd_reg = SFIELD(cmd_reg, SPI_ADDR, GFIELD(cmd_arg, CMD52_REG_ADDR));
+		/* Display trace for byte write */
+		if (GFIELD(cmd_arg, CMD52_RW_FLAG) == 1) {
+			sd_trace(("%s: CMD52: Wr F:%d @0x%04x=%02x\n",
+			          __FUNCTION__,
+			          GFIELD(cmd_arg, CMD52_FUNCTION),
+			          GFIELD(cmd_arg, CMD52_REG_ADDR),
+			          cmd52data));
+		}
+
+		spi_datalen = 32;
+		spi_max_response_pad = 28;
+
+		break;
+	case SDIOH_CMD_53:	/* IO R/W Extended (multiple bytes/blocks) */
+		cmd_arg = arg;
+		cmd_reg = SFIELD(cmd_reg, SPI_FUNC, GFIELD(cmd_arg, CMD53_FUNCTION));
+		cmd_reg = SFIELD(cmd_reg, SPI_ADDR, GFIELD(cmd_arg, CMD53_REG_ADDR));
+		cmd_reg = SFIELD(cmd_reg, SPI_BLKMODE, 0);
+		cmd_reg = SFIELD(cmd_reg, SPI_OPCODE, GFIELD(cmd_arg, CMD53_OP_CODE));
+		cmd_reg = SFIELD(cmd_reg, SPI_STUFF0, (sd->data_xfer_count>>8));
+		cmd52data = (uint8)sd->data_xfer_count;
+
+		/* Set upper bit in byte count if necessary, but don't set it for 512 bytes. */
+		if ((sd->data_xfer_count > 255) && (sd->data_xfer_count < 512)) {
+			cmd_reg |= 1;
+		}
+
+		if (GFIELD(cmd_reg, SPI_RW) == 1) { /* Write */
+			spi_max_response_pad = 32;
+			spi_datalen = (sd->data_xfer_count + spi_max_response_pad) & 0xFFFC;
+		} else { /* Read */
+
+			spi_max_response_pad = 32;
+			spi_datalen = (sd->data_xfer_count + spi_max_response_pad) & 0xFFFC;
+		}
+		sd_trace(("%s: CMD53: %s F:%d @0x%04x len=0x%02x\n",
+		          __FUNCTION__,
+		          (GFIELD(cmd_reg, SPI_RW) == 1 ? "Wr" : "Rd"),
+		          GFIELD(cmd_arg, CMD53_FUNCTION),
+		          GFIELD(cmd_arg, CMD53_REG_ADDR),
+		          cmd52data));
+		break;
+
+	default:
+		sd_err(("%s: Unknown command %d\n", __FUNCTION__, cmd));
+		return ERROR;
+	}
+
+	/* Set up and issue the SDIO command */
+	memset(spi_databuf, SDSPI_IDLE_PAD, spi_datalen);
+	spi_databuf[spi_pre_cmd_pad + 0] = (cmd_reg & 0xFF000000) >> 24;
+	spi_databuf[spi_pre_cmd_pad + 1] = (cmd_reg & 0x00FF0000) >> 16;
+	spi_databuf[spi_pre_cmd_pad + 2] = (cmd_reg & 0x0000FF00) >> 8;
+	spi_databuf[spi_pre_cmd_pad + 3] = (cmd_reg & 0x000000FF);
+	spi_databuf[spi_pre_cmd_pad + 4] = cmd52data;
+
+	/* Generate CRC7 for command, if CRC is enabled, otherwise, a
+	 * default CRC7 of 0x95, which is correct for CMD0, is used.
+	 */
+	if (sd_crc) {
+		cmd_crc = sdspi_crc7(&spi_databuf[spi_pre_cmd_pad], 5);
+	}
+	spi_databuf[spi_pre_cmd_pad + 5] = cmd_crc;
+#define SPI_STOP_TRAN		0xFD
+
+	/* for CMD53 Write, put the data into the output buffer  */
+	if ((cmd == SDIOH_CMD_53) && (GFIELD(cmd_arg, CMD53_RW_FLAG) == 1)) {
+		if (datalen != 0) {
+			spi_databuf[spi_pre_cmd_pad + 9] = SDSPI_IDLE_PAD;
+			spi_databuf[spi_pre_cmd_pad + 10] = SDSPI_START_BLOCK;
+
+			for (i = 0; i < sd->data_xfer_count; i++) {
+				spi_databuf[i + 11 + spi_pre_cmd_pad] = ((uint8 *)data)[i];
+			}
+			if (sd_crc) {
+				dat_crc = sdspi_crc16(&spi_databuf[spi_pre_cmd_pad+11], i);
+			} else {
+				dat_crc = 0xAAAA;
+			}
+			spi_databuf[i + 11 + spi_pre_cmd_pad] = (dat_crc >> 8) & 0xFF;
+			spi_databuf[i + 12 + spi_pre_cmd_pad] = dat_crc & 0xFF;
+		} else if (sd->data_xfer_count == 2) {
+			spi_databuf[spi_pre_cmd_pad + 9] = SDSPI_IDLE_PAD;
+			spi_databuf[spi_pre_cmd_pad + 10] = SDSPI_START_BLOCK;
+			spi_databuf[spi_pre_cmd_pad + 11]  = sd->cmd53_wr_data & 0xFF;
+			spi_databuf[spi_pre_cmd_pad + 12] = (sd->cmd53_wr_data & 0x0000FF00) >> 8;
+			if (sd_crc) {
+				dat_crc = sdspi_crc16(&spi_databuf[spi_pre_cmd_pad+11], 2);
+			} else {
+				dat_crc = 0x22AA;
+			}
+			spi_databuf[spi_pre_cmd_pad + 13] = (dat_crc >> 8) & 0xFF;
+			spi_databuf[spi_pre_cmd_pad + 14] = (dat_crc & 0xFF);
+		} else if (sd->data_xfer_count == 4) {
+			spi_databuf[spi_pre_cmd_pad + 9] = SDSPI_IDLE_PAD;
+			spi_databuf[spi_pre_cmd_pad + 10] = SDSPI_START_BLOCK;
+			spi_databuf[spi_pre_cmd_pad + 11]  = sd->cmd53_wr_data & 0xFF;
+			spi_databuf[spi_pre_cmd_pad + 12] = (sd->cmd53_wr_data & 0x0000FF00) >> 8;
+			spi_databuf[spi_pre_cmd_pad + 13] = (sd->cmd53_wr_data & 0x00FF0000) >> 16;
+			spi_databuf[spi_pre_cmd_pad + 14] = (sd->cmd53_wr_data & 0xFF000000) >> 24;
+			if (sd_crc) {
+				dat_crc = sdspi_crc16(&spi_databuf[spi_pre_cmd_pad+11], 4);
+			} else {
+				dat_crc = 0x44AA;
+			}
+			spi_databuf[spi_pre_cmd_pad + 15] = (dat_crc >> 8) & 0xFF;
+			spi_databuf[spi_pre_cmd_pad + 16] = (dat_crc & 0xFF);
+		} else {
+			printf("CMD53 Write: size %d unsupported\n", sd->data_xfer_count);
+		}
+	}
+
+	spi_sendrecv(sd, spi_databuf, spi_rspbuf, spi_datalen);
+
+	for (i = spi_pre_cmd_pad + SDSPI_COMMAND_LEN; i < spi_max_response_pad; i++) {
+		if ((spi_rspbuf[i] & SDSPI_START_BIT_MASK) == 0) {
+			break;
+		}
+	}
+
+	if (i == spi_max_response_pad) {
+		sd_err(("%s: Did not get a response for CMD%d\n", __FUNCTION__, cmd));
+		return ERROR;
+	}
+
+	/* Extract the response. */
+	sd->card_response = spi_rspbuf[i];
+
+	/* for CMD53 Read, find the start of the response data... */
+	if ((cmd == SDIOH_CMD_53) && (GFIELD(cmd_arg, CMD52_RW_FLAG) == 0)) {
+		for (; i < spi_max_response_pad; i++) {
+			if (spi_rspbuf[i] == SDSPI_START_BLOCK) {
+				break;
+			}
+		}
+
+		if (i == spi_max_response_pad) {
+			printf("Did not get a start of data phase for CMD%d\n", cmd);
+			max_errors++;
+			sdspi_abort(sd, GFIELD(cmd_arg, CMD53_FUNCTION));
+		}
+		sd->card_rsp_data = spi_rspbuf[i+1];
+		sd->card_rsp_data |= spi_rspbuf[i+2] << 8;
+		sd->card_rsp_data |= spi_rspbuf[i+3] << 16;
+		sd->card_rsp_data |= spi_rspbuf[i+4] << 24;
+
+		if (datalen != 0) {
+			i++;
+			for (j = 0; j < sd->data_xfer_count; j++) {
+				((uint8 *)data)[j] = spi_rspbuf[i+j];
+			}
+			if (sd_crc) {
+				uint16 recv_crc;
+
+				recv_crc = spi_rspbuf[i+j] << 8 | spi_rspbuf[i+j+1];
+				dat_crc = sdspi_crc16((uint8 *)data, datalen);
+				if (dat_crc != recv_crc) {
+					sd_err(("%s: Incorrect data CRC: expected 0x%04x, "
+					        "received 0x%04x\n",
+					        __FUNCTION__, dat_crc, recv_crc));
+				}
+			}
+		}
+		return SUCCESS;
+	}
+
+	sd->card_rsp_data = spi_rspbuf[i+4];
+	sd->card_rsp_data |= spi_rspbuf[i+3] << 8;
+	sd->card_rsp_data |= spi_rspbuf[i+2] << 16;
+	sd->card_rsp_data |= spi_rspbuf[i+1] << 24;
+
+	/* Display trace for byte read */
+	if ((cmd == SDIOH_CMD_52) && (GFIELD(cmd_arg, CMD52_RW_FLAG) == 0)) {
+		sd_trace(("%s: CMD52: Rd F:%d @0x%04x=%02x\n",
+		          __FUNCTION__,
+		          GFIELD(cmd_arg, CMD53_FUNCTION),
+		          GFIELD(cmd_arg, CMD53_REG_ADDR),
+		          sd->card_rsp_data >> 24));
+	}
+
+	return SUCCESS;
+}
+
+/*
+ * On entry: if single-block or non-block, buffer size <= block size.
+ * If multi-block, buffer size is unlimited.
+ * Question is how to handle the left-overs in either single- or multi-block.
+ * I think the caller should break the buffer up so this routine will always
+ * use block size == buffer size to handle the end piece of the buffer
+ */
+
+static int
+sdspi_card_buf(sdioh_info_t *sd, int rw, int func, bool fifo, uint32 addr, int nbytes, uint32 *data)
+{
+	int status;
+	uint32 cmd_arg;
+	uint32 rsp5;
+	int num_blocks, blocksize;
+	bool local_blockmode, local_dma;
+	bool read = rw == SDIOH_READ ? 1 : 0;
+
+	ASSERT(nbytes);
+
+	cmd_arg = 0;
+	sd_data(("%s: %s 53 func %d, %s, addr 0x%x, len %d bytes, r_cnt %d t_cnt %d\n",
+	         __FUNCTION__, read ? "Rd" : "Wr", func, fifo ? "FIXED" : "INCR",
+	         addr, nbytes, sd->r_cnt, sd->t_cnt));
+
+	if (read) sd->r_cnt++; else sd->t_cnt++;
+
+	local_blockmode = sd->sd_blockmode;
+	local_dma = sd->sd_use_dma;
+
+	/* Don't bother with block mode on small xfers */
+	if (nbytes < sd->client_block_size[func]) {
+		sd_info(("setting local blockmode to false: nbytes (%d) != block_size (%d)\n",
+		         nbytes, sd->client_block_size[func]));
+		local_blockmode = FALSE;
+		local_dma = FALSE;
+	}
+
+	if (local_blockmode) {
+		blocksize = MIN(sd->client_block_size[func], nbytes);
+		num_blocks = nbytes/blocksize;
+		cmd_arg = SFIELD(cmd_arg, CMD53_BYTE_BLK_CNT, num_blocks);
+		cmd_arg = SFIELD(cmd_arg, CMD53_BLK_MODE, 1);
+	} else {
+		num_blocks =  1;
+		blocksize = nbytes;
+		cmd_arg = SFIELD(cmd_arg, CMD53_BYTE_BLK_CNT, nbytes);
+		cmd_arg = SFIELD(cmd_arg, CMD53_BLK_MODE, 0);
+	}
+
+	if (fifo)
+		cmd_arg = SFIELD(cmd_arg, CMD53_OP_CODE, 0);
+	else
+		cmd_arg = SFIELD(cmd_arg, CMD53_OP_CODE, 1);
+
+	cmd_arg = SFIELD(cmd_arg, CMD53_FUNCTION, func);
+	cmd_arg = SFIELD(cmd_arg, CMD53_REG_ADDR, addr);
+	if (read)
+		cmd_arg = SFIELD(cmd_arg, CMD53_RW_FLAG, SDIOH_XFER_TYPE_READ);
+	else
+		cmd_arg = SFIELD(cmd_arg, CMD53_RW_FLAG, SDIOH_XFER_TYPE_WRITE);
+
+	sd->data_xfer_count = nbytes;
+	if ((func == 2) && (fifo == 1)) {
+		sd_data(("%s: %s 53 func %d, %s, addr 0x%x, len %d bytes, r_cnt %d t_cnt %d\n",
+		         __FUNCTION__, read ? "Rd" : "Wr", func, fifo ? "FIXED" : "INCR",
+		         addr, nbytes, sd->r_cnt, sd->t_cnt));
+	}
+
+	/* sdspi_cmd_issue() returns with the command complete bit
+	 * in the ISR already cleared
+	 */
+	if ((status = sdspi_cmd_issue(sd, local_dma,
+	                              SDIOH_CMD_53, cmd_arg,
+	                              data, nbytes)) != SUCCESS) {
+		sd_err(("%s: cmd_issue failed for %s\n", __FUNCTION__, (read ? "read" : "write")));
+		return status;
+	}
+
+	sdspi_cmd_getrsp(sd, &rsp5, 1);
+
+	if (rsp5 != 0x00) {
+		sd_err(("%s: rsp5 flags = 0x%x, expecting 0x00\n",
+		        __FUNCTION__,  rsp5));
+		return ERROR;
+	}
+
+	return SUCCESS;
+}
+
+static int
+set_client_block_size(sdioh_info_t *sd, int func, int block_size)
+{
+	int base;
+	int err = 0;
+
+	sd_err(("%s: Setting block size %d, func %d\n", __FUNCTION__, block_size, func));
+	sd->client_block_size[func] = block_size;
+
+	/* Set the block size in the SDIO Card register */
+	base = func * SDIOD_FBR_SIZE;
+	err = sdspi_card_regwrite(sd, 0, base + SDIOD_CCCR_BLKSIZE_0, 1, block_size & 0xff);
+	if (!err) {
+		err = sdspi_card_regwrite(sd, 0, base + SDIOD_CCCR_BLKSIZE_1, 1,
+		                          (block_size >> 8) & 0xff);
+	}
+
+	/*
+	 * Do not set the block size in the SDIO Host register; that
+	 * is func dependent and will get done on an individual
+	 * transaction basis.
+	 */
+
+	return (err ? BCME_SDIO_ERROR : 0);
+}
+
+/* Reset and re-initialize the device */
+int
+sdioh_sdio_reset(sdioh_info_t *si)
+{
+	si->card_init_done = FALSE;
+	return sdspi_client_init(si);
+}
+
+#define CRC7_POLYNOM	0x09
+#define CRC7_CRCHIGHBIT	0x40
+
+static uint8 sdspi_crc7(unsigned char* p, uint32 len)
+{
+	uint8 c, j, bit, crc = 0;
+	uint32 i;
+
+	for (i = 0; i < len; i++) {
+		c = *p++;
+		for (j = 0x80; j; j >>= 1) {
+			bit = crc & CRC7_CRCHIGHBIT;
+			crc <<= 1;
+			if (c & j) bit ^= CRC7_CRCHIGHBIT;
+			if (bit) crc ^= CRC7_POLYNOM;
+		}
+	}
+
+	/* Convert the CRC7 to an 8-bit SD CRC */
+	crc = (crc << 1) | 1;
+
+	return (crc);
+}
+
+#define CRC16_POLYNOM	0x1021
+#define CRC16_CRCHIGHBIT	0x8000
+
+static uint16 sdspi_crc16(unsigned char* p, uint32 len)
+{
+	uint32 i;
+	uint16 j, c, bit;
+	uint16 crc = 0;
+
+	for (i = 0; i < len; i++) {
+		c = *p++;
+		for (j = 0x80; j; j >>= 1) {
+			bit = crc & CRC16_CRCHIGHBIT;
+			crc <<= 1;
+			if (c & j) bit ^= CRC16_CRCHIGHBIT;
+			if (bit) crc ^= CRC16_POLYNOM;
+		}
+	}
+
+	return (crc);
+}
diff --git a/drivers/net/wireless/bcm4329/bcmsdspi_linux.c b/drivers/net/wireless/bcm4329/bcmsdspi_linux.c
new file mode 100644
index 000000000000..e2e0ca6abe46
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/bcmsdspi_linux.c
@@ -0,0 +1,252 @@
+/*
+ * Broadcom SPI Host Controller Driver - Linux Per-port
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdspi_linux.c,v 1.7.2.1.4.3 2008/06/30 21:09:36 Exp $
+ */
+
+#include <typedefs.h>
+#include <pcicfg.h>
+#include <bcmutils.h>
+
+#include <sdio.h>		/* SDIO Specs */
+#include <bcmsdbus.h>		/* bcmsdh to/from specific controller APIs */
+#include <sdiovar.h>		/* to get msglevel bit values */
+
+#include <linux/sched.h>	/* request_irq(), free_irq() */
+
+#include <bcmsdspi.h>
+#include <bcmspi.h>
+
+extern uint sd_crc;
+module_param(sd_crc, uint, 0);
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+#define KERNEL26
+#endif
+
+struct sdos_info {
+	sdioh_info_t *sd;
+	spinlock_t lock;
+	wait_queue_head_t intr_wait_queue;
+};
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+#define BLOCKABLE()	(!in_atomic())
+#else
+#define BLOCKABLE()	(!in_interrupt())
+#endif
+
+/* Interrupt handler */
+static irqreturn_t
+sdspi_isr(int irq, void *dev_id
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 20)
+, struct pt_regs *ptregs
+#endif
+)
+{
+	sdioh_info_t *sd;
+	struct sdos_info *sdos;
+	bool ours;
+
+	sd = (sdioh_info_t *)dev_id;
+	sd->local_intrcount++;
+
+	if (!sd->card_init_done) {
+		sd_err(("%s: Hey Bogus intr...not even initted: irq %d\n", __FUNCTION__, irq));
+		return IRQ_RETVAL(FALSE);
+	} else {
+		ours = spi_check_client_intr(sd, NULL);
+
+		/* For local interrupts, wake the waiting process */
+		if (ours && sd->got_hcint) {
+			sdos = (struct sdos_info *)sd->sdos_info;
+			wake_up_interruptible(&sdos->intr_wait_queue);
+		}
+
+		return IRQ_RETVAL(ours);
+	}
+}
+
+/* Register with Linux for interrupts */
+int
+spi_register_irq(sdioh_info_t *sd, uint irq)
+{
+	sd_trace(("Entering %s: irq == %d\n", __FUNCTION__, irq));
+	if (request_irq(irq, sdspi_isr, IRQF_SHARED, "bcmsdspi", sd) < 0) {
+		sd_err(("%s: request_irq() failed\n", __FUNCTION__));
+		return ERROR;
+	}
+	return SUCCESS;
+}
+
+/* Free Linux irq */
+void
+spi_free_irq(uint irq, sdioh_info_t *sd)
+{
+	free_irq(irq, sd);
+}
+
+/* Map Host controller registers */
+
+uint32 *
+spi_reg_map(osl_t *osh, uintptr addr, int size)
+{
+	return (uint32 *)REG_MAP(addr, size);
+}
+
+void
+spi_reg_unmap(osl_t *osh, uintptr addr, int size)
+{
+	REG_UNMAP((void*)(uintptr)addr);
+}
+
+int
+spi_osinit(sdioh_info_t *sd)
+{
+	struct sdos_info *sdos;
+
+	sdos = (struct sdos_info*)MALLOC(sd->osh, sizeof(struct sdos_info));
+	sd->sdos_info = (void*)sdos;
+	if (sdos == NULL)
+		return BCME_NOMEM;
+
+	sdos->sd = sd;
+	spin_lock_init(&sdos->lock);
+	init_waitqueue_head(&sdos->intr_wait_queue);
+	return BCME_OK;
+}
+
+void
+spi_osfree(sdioh_info_t *sd)
+{
+	struct sdos_info *sdos;
+	ASSERT(sd && sd->sdos_info);
+
+	sdos = (struct sdos_info *)sd->sdos_info;
+	MFREE(sd->osh, sdos, sizeof(struct sdos_info));
+}
+
+/* Interrupt enable/disable */
+SDIOH_API_RC
+sdioh_interrupt_set(sdioh_info_t *sd, bool enable)
+{
+	ulong flags;
+	struct sdos_info *sdos;
+
+	sd_trace(("%s: %s\n", __FUNCTION__, enable ? "Enabling" : "Disabling"));
+
+	sdos = (struct sdos_info *)sd->sdos_info;
+	ASSERT(sdos);
+
+	if (!(sd->host_init_done && sd->card_init_done)) {
+		sd_err(("%s: Card & Host are not initted - bailing\n", __FUNCTION__));
+		return SDIOH_API_RC_FAIL;
+	}
+
+	if (enable && !(sd->intr_handler && sd->intr_handler_arg)) {
+		sd_err(("%s: no handler registered, will not enable\n", __FUNCTION__));
+		return SDIOH_API_RC_FAIL;
+	}
+
+	/* Ensure atomicity for enable/disable calls */
+	spin_lock_irqsave(&sdos->lock, flags);
+
+	sd->client_intr_enabled = enable;
+	if (enable && !sd->lockcount)
+		spi_devintr_on(sd);
+	else
+		spi_devintr_off(sd);
+
+	spin_unlock_irqrestore(&sdos->lock, flags);
+
+	return SDIOH_API_RC_SUCCESS;
+}
+
+/* Protect against reentrancy (disable device interrupts while executing) */
+void
+spi_lock(sdioh_info_t *sd)
+{
+	ulong flags;
+	struct sdos_info *sdos;
+
+	sdos = (struct sdos_info *)sd->sdos_info;
+	ASSERT(sdos);
+
+	sd_trace(("%s: %d\n", __FUNCTION__, sd->lockcount));
+
+	spin_lock_irqsave(&sdos->lock, flags);
+	if (sd->lockcount) {
+		sd_err(("%s: Already locked!\n", __FUNCTION__));
+		ASSERT(sd->lockcount == 0);
+	}
+	spi_devintr_off(sd);
+	sd->lockcount++;
+	spin_unlock_irqrestore(&sdos->lock, flags);
+}
+
+/* Enable client interrupt */
+void
+spi_unlock(sdioh_info_t *sd)
+{
+	ulong flags;
+	struct sdos_info *sdos;
+
+	sd_trace(("%s: %d, %d\n", __FUNCTION__, sd->lockcount, sd->client_intr_enabled));
+	ASSERT(sd->lockcount > 0);
+
+	sdos = (struct sdos_info *)sd->sdos_info;
+	ASSERT(sdos);
+
+	spin_lock_irqsave(&sdos->lock, flags);
+	if (--sd->lockcount == 0 && sd->client_intr_enabled) {
+		spi_devintr_on(sd);
+	}
+	spin_unlock_irqrestore(&sdos->lock, flags);
+}
+
+void spi_waitbits(sdioh_info_t *sd, bool yield)
+{
+	struct sdos_info *sdos;
+
+	sdos = (struct sdos_info *)sd->sdos_info;
+
+#ifndef BCMSDYIELD
+	ASSERT(!yield);
+#endif
+	sd_trace(("%s: yield %d canblock %d\n",
+	          __FUNCTION__, yield, BLOCKABLE()));
+
+	/* Clear the "interrupt happened" flag and last intrstatus */
+	sd->got_hcint = FALSE;
+
+#ifdef BCMSDYIELD
+	if (yield && BLOCKABLE()) {
+		/* Wait for the indication, the interrupt will be masked when the ISR fires. */
+		wait_event_interruptible(sdos->intr_wait_queue, (sd->got_hcint));
+	} else
+#endif /* BCMSDYIELD */
+	{
+		spi_spinbits(sd);
+	}
+
+}
diff --git a/drivers/net/wireless/bcm4329/bcmsdstd.c b/drivers/net/wireless/bcm4329/bcmsdstd.c
new file mode 100644
index 000000000000..0ca1f8ff8a24
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/bcmsdstd.c
@@ -0,0 +1,3127 @@
+/*
+ *  'Standard' SDIO HOST CONTROLLER driver
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdstd.c,v 1.64.4.1.4.4.2.18 2010/08/17 17:00:48 Exp $
+ */
+
+#include <typedefs.h>
+
+#include <bcmdevs.h>
+#include <bcmendian.h>
+#include <bcmutils.h>
+#include <osl.h>
+#include <siutils.h>
+#include <sdio.h>	/* SDIO Device and Protocol Specs */
+#include <sdioh.h>	/* SDIO Host Controller Specification */
+#include <bcmsdbus.h>	/* bcmsdh to/from specific controller APIs */
+#include <sdiovar.h>	/* ioctl/iovars */
+#include <pcicfg.h>
+
+
+#define SD_PAGE_BITS	12
+#define SD_PAGE 	(1 << SD_PAGE_BITS)
+
+#include <bcmsdstd.h>
+
+/* Globals */
+uint sd_msglevel = SDH_ERROR_VAL;
+uint sd_hiok = TRUE;			/* Use hi-speed mode if available? */
+uint sd_sdmode = SDIOH_MODE_SD4;	/* Use SD4 mode by default */
+uint sd_f2_blocksize = 64;		/* Default blocksize */
+
+#ifdef BCMSDYIELD
+bool sd_yieldcpu = TRUE;		/* Allow CPU yielding for buffer requests */
+uint sd_minyield = 0;			/* Minimum xfer size to allow CPU yield */
+bool sd_forcerb = FALSE;		/* Force sync readback in intrs_on/off */
+#endif
+
+uint sd_divisor = 2;			/* Default 48MHz/2 = 24MHz */
+
+uint sd_power = 1;		/* Default to SD Slot powered ON */
+uint sd_clock = 1;		/* Default to SD Clock turned ON */
+uint sd_pci_slot = 0xFFFFffff; /* Used to force selection of a particular PCI slot */
+uint8 sd_dma_mode = DMA_MODE_SDMA; /* Default to SDMA for now */
+
+uint sd_toctl = 7;
+
+static bool trap_errs = FALSE;
+
+static const char *dma_mode_description[] = { "PIO", "SDMA", "ADMA1", "32b ADMA2", "64b ADMA2" };
+
+/* Prototypes */
+static bool sdstd_start_clock(sdioh_info_t *sd, uint16 divisor);
+static bool sdstd_start_power(sdioh_info_t *sd);
+static bool sdstd_bus_width(sdioh_info_t *sd, int width);
+static int sdstd_set_highspeed_mode(sdioh_info_t *sd, bool HSMode);
+static int sdstd_set_dma_mode(sdioh_info_t *sd, int8 dma_mode);
+static int sdstd_card_enablefuncs(sdioh_info_t *sd);
+static void sdstd_cmd_getrsp(sdioh_info_t *sd, uint32 *rsp_buffer, int count);
+static int sdstd_cmd_issue(sdioh_info_t *sd, bool use_dma, uint32 cmd, uint32 arg);
+static int sdstd_card_regread(sdioh_info_t *sd, int func, uint32 regaddr,
+                              int regsize, uint32 *data);
+static int sdstd_card_regwrite(sdioh_info_t *sd, int func, uint32 regaddr,
+                               int regsize, uint32 data);
+static int sdstd_driver_init(sdioh_info_t *sd);
+static bool sdstd_reset(sdioh_info_t *sd, bool host_reset, bool client_reset);
+static int sdstd_card_buf(sdioh_info_t *sd, int rw, int func, bool fifo,
+                          uint32 addr, int nbytes, uint32 *data);
+static int sdstd_abort(sdioh_info_t *sd, uint func);
+static int sdstd_check_errs(sdioh_info_t *sdioh_info, uint32 cmd, uint32 arg);
+static int set_client_block_size(sdioh_info_t *sd, int func, int blocksize);
+static void sd_map_dma(sdioh_info_t * sd);
+static void sd_unmap_dma(sdioh_info_t * sd);
+static void sd_clear_adma_dscr_buf(sdioh_info_t *sd);
+static void sd_fill_dma_data_buf(sdioh_info_t *sd, uint8 data);
+static void sd_create_adma_descriptor(sdioh_info_t *sd,
+                                      uint32 index, uint32 addr_phys,
+                                      uint16 length, uint16 flags);
+static void sd_dump_adma_dscr(sdioh_info_t *sd);
+static void sdstd_dumpregs(sdioh_info_t *sd);
+
+
+/*
+ * Private register access routines.
+ */
+
+/* 16 bit PCI regs */
+
+extern uint16 sdstd_rreg16(sdioh_info_t *sd, uint reg);
+uint16
+sdstd_rreg16(sdioh_info_t *sd, uint reg)
+{
+
+	volatile uint16 data = *(volatile uint16 *)(sd->mem_space + reg);
+	sd_ctrl(("16: R Reg 0x%02x, Data 0x%x\n", reg, data));
+	return data;
+}
+
+extern void sdstd_wreg16(sdioh_info_t *sd, uint reg, uint16 data);
+void
+sdstd_wreg16(sdioh_info_t *sd, uint reg, uint16 data)
+{
+	*(volatile uint16 *)(sd->mem_space + reg) = (uint16)data;
+	sd_ctrl(("16: W Reg 0x%02x, Data 0x%x\n", reg, data));
+}
+
+static void
+sdstd_or_reg16(sdioh_info_t *sd, uint reg, uint16 val)
+{
+	volatile uint16 data = *(volatile uint16 *)(sd->mem_space + reg);
+	sd_ctrl(("16: OR Reg 0x%02x, Val 0x%x\n", reg, val));
+	data |= val;
+	*(volatile uint16 *)(sd->mem_space + reg) = (uint16)data;
+
+}
+static void
+sdstd_mod_reg16(sdioh_info_t *sd, uint reg, int16 mask, uint16 val)
+{
+
+	volatile uint16 data = *(volatile uint16 *)(sd->mem_space + reg);
+	sd_ctrl(("16: MOD Reg 0x%02x, Mask 0x%x, Val 0x%x\n", reg, mask, val));
+	data &= ~mask;
+	data |= (val & mask);
+	*(volatile uint16 *)(sd->mem_space + reg) = (uint16)data;
+}
+
+
+/* 32 bit PCI regs */
+static uint32
+sdstd_rreg(sdioh_info_t *sd, uint reg)
+{
+	volatile uint32 data = *(volatile uint32 *)(sd->mem_space + reg);
+	sd_ctrl(("32: R Reg 0x%02x, Data 0x%x\n", reg, data));
+	return data;
+}
+static inline void
+sdstd_wreg(sdioh_info_t *sd, uint reg, uint32 data)
+{
+	*(volatile uint32 *)(sd->mem_space + reg) = (uint32)data;
+	sd_ctrl(("32: W Reg 0x%02x, Data 0x%x\n", reg, data));
+
+}
+
+/* 8 bit PCI regs */
+static inline void
+sdstd_wreg8(sdioh_info_t *sd, uint reg, uint8 data)
+{
+	*(volatile uint8 *)(sd->mem_space + reg) = (uint8)data;
+	sd_ctrl(("08: W Reg 0x%02x, Data 0x%x\n", reg, data));
+}
+static uint8
+sdstd_rreg8(sdioh_info_t *sd, uint reg)
+{
+	volatile uint8 data = *(volatile uint8 *)(sd->mem_space + reg);
+	sd_ctrl(("08: R Reg 0x%02x, Data 0x%x\n", reg, data));
+	return data;
+}
+
+/*
+ * Private work routines
+ */
+
+sdioh_info_t *glob_sd;
+
+/*
+ *  Public entry points & extern's
+ */
+extern sdioh_info_t *
+sdioh_attach(osl_t *osh, void *bar0, uint irq)
+{
+	sdioh_info_t *sd;
+
+	sd_trace(("%s\n", __FUNCTION__));
+	if ((sd = (sdioh_info_t *)MALLOC(osh, sizeof(sdioh_info_t))) == NULL) {
+		sd_err(("sdioh_attach: out of memory, malloced %d bytes\n", MALLOCED(osh)));
+		return NULL;
+	}
+	bzero((char *)sd, sizeof(sdioh_info_t));
+	glob_sd = sd;
+	sd->osh = osh;
+	if (sdstd_osinit(sd) != 0) {
+		sd_err(("%s:sdstd_osinit() failed\n", __FUNCTION__));
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+		return NULL;
+	}
+	sd->mem_space = (volatile char *)sdstd_reg_map(osh, (uintptr)bar0, SDIOH_REG_WINSZ);
+	sd_init_dma(sd);
+	sd->irq = irq;
+	if (sd->mem_space == NULL) {
+		sd_err(("%s:ioremap() failed\n", __FUNCTION__));
+		sdstd_osfree(sd);
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+		return NULL;
+	}
+	sd_info(("%s:sd->mem_space = %p\n", __FUNCTION__, sd->mem_space));
+	sd->intr_handler = NULL;
+	sd->intr_handler_arg = NULL;
+	sd->intr_handler_valid = FALSE;
+
+	/* Set defaults */
+	sd->sd_blockmode = TRUE;
+	sd->use_client_ints = TRUE;
+	sd->sd_dma_mode = sd_dma_mode;
+
+	if (!sd->sd_blockmode)
+		sd->sd_dma_mode = DMA_MODE_NONE;
+
+	if (sdstd_driver_init(sd) != SUCCESS) {
+		/* If host CPU was reset without resetting SD bus or
+		   SD device, the device will still have its RCA but
+		   driver no longer knows what it is (since driver has been restarted).
+		   go through once to clear the RCA and a gain reassign it.
+		 */
+		sd_info(("driver_init failed - Reset RCA and try again\n"));
+		if (sdstd_driver_init(sd) != SUCCESS) {
+			sd_err(("%s:driver_init() failed()\n", __FUNCTION__));
+			if (sd->mem_space) {
+				sdstd_reg_unmap(osh, (uintptr)sd->mem_space, SDIOH_REG_WINSZ);
+				sd->mem_space = NULL;
+			}
+			sdstd_osfree(sd);
+			MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+			return (NULL);
+		}
+	}
+
+	OSL_DMADDRWIDTH(osh, 32);
+
+	/* Always map DMA buffers, so we can switch between DMA modes. */
+	sd_map_dma(sd);
+
+	if (sdstd_register_irq(sd, irq) != SUCCESS) {
+		sd_err(("%s: sdstd_register_irq() failed for irq = %d\n", __FUNCTION__, irq));
+		sdstd_free_irq(sd->irq, sd);
+		if (sd->mem_space) {
+			sdstd_reg_unmap(osh, (uintptr)sd->mem_space, SDIOH_REG_WINSZ);
+			sd->mem_space = NULL;
+		}
+
+		sdstd_osfree(sd);
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+		return (NULL);
+	}
+
+	sd_trace(("%s: Done\n", __FUNCTION__));
+	return sd;
+}
+
+extern SDIOH_API_RC
+sdioh_detach(osl_t *osh, sdioh_info_t *sd)
+{
+	sd_trace(("%s\n", __FUNCTION__));
+	if (sd) {
+		sd_unmap_dma(sd);
+		sdstd_wreg16(sd, SD_IntrSignalEnable, 0);
+		sd_trace(("%s: freeing irq %d\n", __FUNCTION__, sd->irq));
+		sdstd_free_irq(sd->irq, sd);
+		if (sd->card_init_done)
+			sdstd_reset(sd, 1, 1);
+		if (sd->mem_space) {
+			sdstd_reg_unmap(osh, (uintptr)sd->mem_space, SDIOH_REG_WINSZ);
+			sd->mem_space = NULL;
+		}
+
+		sdstd_osfree(sd);
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+	}
+	return SDIOH_API_RC_SUCCESS;
+}
+
+/* Configure callback to client when we receive client interrupt */
+extern SDIOH_API_RC
+sdioh_interrupt_register(sdioh_info_t *sd, sdioh_cb_fn_t fn, void *argh)
+{
+	sd_trace(("%s: Entering\n", __FUNCTION__));
+	sd->intr_handler = fn;
+	sd->intr_handler_arg = argh;
+	sd->intr_handler_valid = TRUE;
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_interrupt_deregister(sdioh_info_t *sd)
+{
+	sd_trace(("%s: Entering\n", __FUNCTION__));
+	sd->intr_handler_valid = FALSE;
+	sd->intr_handler = NULL;
+	sd->intr_handler_arg = NULL;
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_interrupt_query(sdioh_info_t *sd, bool *onoff)
+{
+	sd_trace(("%s: Entering\n", __FUNCTION__));
+	*onoff = sd->client_intr_enabled;
+	return SDIOH_API_RC_SUCCESS;
+}
+
+#if defined(DHD_DEBUG)
+extern bool
+sdioh_interrupt_pending(sdioh_info_t *sd)
+{
+	uint16 intrstatus;
+	intrstatus = sdstd_rreg16(sd, SD_IntrStatus);
+	return !!(intrstatus & CLIENT_INTR);
+}
+#endif
+
+uint
+sdioh_query_iofnum(sdioh_info_t *sd)
+{
+	return sd->num_funcs;
+}
+
+/* IOVar table */
+enum {
+	IOV_MSGLEVEL = 1,
+	IOV_BLOCKMODE,
+	IOV_BLOCKSIZE,
+	IOV_DMA,
+	IOV_USEINTS,
+	IOV_NUMINTS,
+	IOV_NUMLOCALINTS,
+	IOV_HOSTREG,
+	IOV_DEVREG,
+	IOV_DIVISOR,
+	IOV_SDMODE,
+	IOV_HISPEED,
+	IOV_HCIREGS,
+	IOV_POWER,
+	IOV_YIELDCPU,
+	IOV_MINYIELD,
+	IOV_FORCERB,
+	IOV_CLOCK
+};
+
+const bcm_iovar_t sdioh_iovars[] = {
+	{"sd_msglevel",	IOV_MSGLEVEL, 	0,	IOVT_UINT32,	0 },
+	{"sd_blockmode", IOV_BLOCKMODE,	0,	IOVT_BOOL,	0 },
+	{"sd_blocksize", IOV_BLOCKSIZE, 0,	IOVT_UINT32,	0 }, /* ((fn << 16) | size) */
+	{"sd_dma",	IOV_DMA,	0,	IOVT_UINT32,	0 },
+#ifdef BCMSDYIELD
+	{"sd_yieldcpu",	IOV_YIELDCPU,	0,	IOVT_BOOL,	0 },
+	{"sd_minyield",	IOV_MINYIELD,	0,	IOVT_UINT32,	0 },
+	{"sd_forcerb",	IOV_FORCERB,	0,	IOVT_BOOL,	0 },
+#endif
+	{"sd_ints",	IOV_USEINTS,	0,	IOVT_BOOL,	0 },
+	{"sd_numints",	IOV_NUMINTS,	0,	IOVT_UINT32,	0 },
+	{"sd_numlocalints", IOV_NUMLOCALINTS, 0, IOVT_UINT32,	0 },
+	{"sd_hostreg",	IOV_HOSTREG,	0,	IOVT_BUFFER,	sizeof(sdreg_t) },
+	{"sd_devreg",	IOV_DEVREG,	0,	IOVT_BUFFER,	sizeof(sdreg_t)	},
+	{"sd_divisor",	IOV_DIVISOR,	0,	IOVT_UINT32,	0 },
+	{"sd_power",	IOV_POWER,	0,	IOVT_UINT32,	0 },
+	{"sd_clock",	IOV_CLOCK,	0,	IOVT_UINT32,	0 },
+	{"sd_mode",	IOV_SDMODE,	0,	IOVT_UINT32,	100},
+	{"sd_highspeed",	IOV_HISPEED,	0,	IOVT_UINT32,	0},
+	{NULL, 0, 0, 0, 0 }
+};
+
+int
+sdioh_iovar_op(sdioh_info_t *si, const char *name,
+               void *params, int plen, void *arg, int len, bool set)
+{
+	const bcm_iovar_t *vi = NULL;
+	int bcmerror = 0;
+	int val_size;
+	int32 int_val = 0;
+	bool bool_val;
+	uint32 actionid;
+
+	ASSERT(name);
+	ASSERT(len >= 0);
+
+	/* Get must have return space; Set does not take qualifiers */
+	ASSERT(set || (arg && len));
+	ASSERT(!set || (!params && !plen));
+
+	sd_trace(("%s: Enter (%s %s)\n", __FUNCTION__, (set ? "set" : "get"), name));
+
+	if ((vi = bcm_iovar_lookup(sdioh_iovars, name)) == NULL) {
+		bcmerror = BCME_UNSUPPORTED;
+		goto exit;
+	}
+
+	if ((bcmerror = bcm_iovar_lencheck(vi, arg, len, set)) != 0)
+		goto exit;
+
+	/* Set up params so get and set can share the convenience variables */
+	if (params == NULL) {
+		params = arg;
+		plen = len;
+	}
+
+	if (vi->type == IOVT_VOID)
+		val_size = 0;
+	else if (vi->type == IOVT_BUFFER)
+		val_size = len;
+	else
+		val_size = sizeof(int);
+
+	if (plen >= (int)sizeof(int_val))
+		bcopy(params, &int_val, sizeof(int_val));
+
+	bool_val = (int_val != 0) ? TRUE : FALSE;
+
+	actionid = set ? IOV_SVAL(vi->varid) : IOV_GVAL(vi->varid);
+	switch (actionid) {
+	case IOV_GVAL(IOV_MSGLEVEL):
+		int_val = (int32)sd_msglevel;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_MSGLEVEL):
+		sd_msglevel = int_val;
+		break;
+
+	case IOV_GVAL(IOV_BLOCKMODE):
+		int_val = (int32)si->sd_blockmode;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_BLOCKMODE):
+		si->sd_blockmode = (bool)int_val;
+		/* Haven't figured out how to make non-block mode with DMA */
+		if (!si->sd_blockmode)
+			si->sd_dma_mode = DMA_MODE_NONE;
+		break;
+
+#ifdef BCMSDYIELD
+	case IOV_GVAL(IOV_YIELDCPU):
+		int_val = sd_yieldcpu;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_YIELDCPU):
+		sd_yieldcpu = (bool)int_val;
+		break;
+
+	case IOV_GVAL(IOV_MINYIELD):
+		int_val = sd_minyield;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_MINYIELD):
+		sd_minyield = (bool)int_val;
+		break;
+
+	case IOV_GVAL(IOV_FORCERB):
+		int_val = sd_forcerb;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_FORCERB):
+		sd_forcerb = (bool)int_val;
+		break;
+#endif /* BCMSDYIELD */
+
+	case IOV_GVAL(IOV_BLOCKSIZE):
+		if ((uint32)int_val > si->num_funcs) {
+			bcmerror = BCME_BADARG;
+			break;
+		}
+		int_val = (int32)si->client_block_size[int_val];
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_BLOCKSIZE):
+	{
+		uint func = ((uint32)int_val >> 16);
+		uint blksize = (uint16)int_val;
+		uint maxsize;
+
+		if (func > si->num_funcs) {
+			bcmerror = BCME_BADARG;
+			break;
+		}
+
+		switch (func) {
+		case 0: maxsize = 32; break;
+		case 1: maxsize = BLOCK_SIZE_4318; break;
+		case 2: maxsize = BLOCK_SIZE_4328; break;
+		default: maxsize = 0;
+		}
+		if (blksize > maxsize) {
+			bcmerror = BCME_BADARG;
+			break;
+		}
+		if (!blksize) {
+			blksize = maxsize;
+		}
+
+		/* Now set it */
+		sdstd_lock(si);
+		bcmerror = set_client_block_size(si, func, blksize);
+		sdstd_unlock(si);
+		break;
+	}
+
+	case IOV_GVAL(IOV_DMA):
+		int_val = (int32)si->sd_dma_mode;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_DMA):
+		si->sd_dma_mode = (char)int_val;
+		sdstd_set_dma_mode(si, si->sd_dma_mode);
+		break;
+
+	case IOV_GVAL(IOV_USEINTS):
+		int_val = (int32)si->use_client_ints;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_USEINTS):
+		si->use_client_ints = (bool)int_val;
+		if (si->use_client_ints)
+			si->intmask |= CLIENT_INTR;
+		else
+			si->intmask &= ~CLIENT_INTR;
+		break;
+
+	case IOV_GVAL(IOV_DIVISOR):
+		int_val = (uint32)sd_divisor;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_DIVISOR):
+		sd_divisor = int_val;
+		if (!sdstd_start_clock(si, (uint16)sd_divisor)) {
+			sd_err(("set clock failed!\n"));
+			bcmerror = BCME_ERROR;
+		}
+		break;
+
+	case IOV_GVAL(IOV_POWER):
+		int_val = (uint32)sd_power;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_POWER):
+		sd_power = int_val;
+		if (sd_power == 1) {
+			if (sdstd_driver_init(si) != SUCCESS) {
+				sd_err(("set SD Slot power failed!\n"));
+				bcmerror = BCME_ERROR;
+			} else {
+				sd_err(("SD Slot Powered ON.\n"));
+			}
+		} else {
+			uint8 pwr = 0;
+
+			pwr = SFIELD(pwr, PWR_BUS_EN, 0);
+			sdstd_wreg8(si, SD_PwrCntrl, pwr); /* Set Voltage level */
+			sd_err(("SD Slot Powered OFF.\n"));
+		}
+		break;
+
+	case IOV_GVAL(IOV_CLOCK):
+		int_val = (uint32)sd_clock;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_CLOCK):
+		sd_clock = int_val;
+		if (sd_clock == 1) {
+			sd_info(("SD Clock turned ON.\n"));
+			if (!sdstd_start_clock(si, (uint16)sd_divisor)) {
+				sd_err(("sdstd_start_clock failed\n"));
+				bcmerror = BCME_ERROR;
+			}
+		} else {
+			/* turn off HC clock */
+			sdstd_wreg16(si, SD_ClockCntrl,
+			             sdstd_rreg16(si, SD_ClockCntrl) & ~((uint16)0x4));
+
+			sd_info(("SD Clock turned OFF.\n"));
+		}
+		break;
+
+	case IOV_GVAL(IOV_SDMODE):
+		int_val = (uint32)sd_sdmode;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_SDMODE):
+		sd_sdmode = int_val;
+
+		if (!sdstd_bus_width(si, sd_sdmode)) {
+			sd_err(("sdstd_bus_width failed\n"));
+			bcmerror = BCME_ERROR;
+		}
+		break;
+
+	case IOV_GVAL(IOV_HISPEED):
+		int_val = (uint32)sd_hiok;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_HISPEED):
+		sd_hiok = int_val;
+		bcmerror = sdstd_set_highspeed_mode(si, (bool)sd_hiok);
+		break;
+
+	case IOV_GVAL(IOV_NUMINTS):
+		int_val = (int32)si->intrcount;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_NUMLOCALINTS):
+		int_val = (int32)si->local_intrcount;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_HOSTREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+
+		if (sd_ptr->offset < SD_SysAddr || sd_ptr->offset > SD_MaxCurCap) {
+			sd_err(("%s: bad offset 0x%x\n", __FUNCTION__, sd_ptr->offset));
+			bcmerror = BCME_BADARG;
+			break;
+		}
+
+		sd_trace(("%s: rreg%d at offset %d\n", __FUNCTION__,
+		          (sd_ptr->offset & 1) ? 8 : ((sd_ptr->offset & 2) ? 16 : 32),
+		          sd_ptr->offset));
+		if (sd_ptr->offset & 1)
+			int_val = sdstd_rreg8(si, sd_ptr->offset);
+		else if (sd_ptr->offset & 2)
+			int_val = sdstd_rreg16(si, sd_ptr->offset);
+		else
+			int_val = sdstd_rreg(si, sd_ptr->offset);
+
+		bcopy(&int_val, arg, sizeof(int_val));
+		break;
+	}
+
+	case IOV_SVAL(IOV_HOSTREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+
+		if (sd_ptr->offset < SD_SysAddr || sd_ptr->offset > SD_MaxCurCap) {
+			sd_err(("%s: bad offset 0x%x\n", __FUNCTION__, sd_ptr->offset));
+			bcmerror = BCME_BADARG;
+			break;
+		}
+
+		sd_trace(("%s: wreg%d value 0x%08x at offset %d\n", __FUNCTION__, sd_ptr->value,
+		          (sd_ptr->offset & 1) ? 8 : ((sd_ptr->offset & 2) ? 16 : 32),
+		          sd_ptr->offset));
+		if (sd_ptr->offset & 1)
+			sdstd_wreg8(si, sd_ptr->offset, (uint8)sd_ptr->value);
+		else if (sd_ptr->offset & 2)
+			sdstd_wreg16(si, sd_ptr->offset, (uint16)sd_ptr->value);
+		else
+			sdstd_wreg(si, sd_ptr->offset, (uint32)sd_ptr->value);
+
+		break;
+	}
+
+	case IOV_GVAL(IOV_DEVREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+		uint8 data;
+
+		if (sdioh_cfg_read(si, sd_ptr->func, sd_ptr->offset, &data)) {
+			bcmerror = BCME_SDIO_ERROR;
+			break;
+		}
+
+		int_val = (int)data;
+		bcopy(&int_val, arg, sizeof(int_val));
+		break;
+	}
+
+	case IOV_SVAL(IOV_DEVREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+		uint8 data = (uint8)sd_ptr->value;
+
+		if (sdioh_cfg_write(si, sd_ptr->func, sd_ptr->offset, &data)) {
+			bcmerror = BCME_SDIO_ERROR;
+			break;
+		}
+		break;
+	}
+
+
+	default:
+		bcmerror = BCME_UNSUPPORTED;
+		break;
+	}
+exit:
+
+	return bcmerror;
+}
+
+extern SDIOH_API_RC
+sdioh_cfg_read(sdioh_info_t *sd, uint fnc_num, uint32 addr, uint8 *data)
+{
+	SDIOH_API_RC status;
+	/* No lock needed since sdioh_request_byte does locking */
+	status = sdioh_request_byte(sd, SDIOH_READ, fnc_num, addr, data);
+	return status;
+}
+
+extern SDIOH_API_RC
+sdioh_cfg_write(sdioh_info_t *sd, uint fnc_num, uint32 addr, uint8 *data)
+{
+	/* No lock needed since sdioh_request_byte does locking */
+	SDIOH_API_RC status;
+	status = sdioh_request_byte(sd, SDIOH_WRITE, fnc_num, addr, data);
+	return status;
+}
+
+extern SDIOH_API_RC
+sdioh_cis_read(sdioh_info_t *sd, uint func, uint8 *cisd, uint32 length)
+{
+	uint32 count;
+	int offset;
+	uint32 foo;
+	uint8 *cis = cisd;
+
+	sd_trace(("%s: Func = %d\n", __FUNCTION__, func));
+
+	if (!sd->func_cis_ptr[func]) {
+		bzero(cis, length);
+		return SDIOH_API_RC_FAIL;
+	}
+
+	sdstd_lock(sd);
+	*cis = 0;
+	for (count = 0; count < length; count++) {
+		offset =  sd->func_cis_ptr[func] + count;
+		if (sdstd_card_regread(sd, 0, offset, 1, &foo)) {
+			sd_err(("%s: regread failed: Can't read CIS\n", __FUNCTION__));
+			sdstd_unlock(sd);
+			return SDIOH_API_RC_FAIL;
+		}
+		*cis = (uint8)(foo & 0xff);
+		cis++;
+	}
+	sdstd_unlock(sd);
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_request_byte(sdioh_info_t *sd, uint rw, uint func, uint regaddr, uint8 *byte)
+{
+	int status;
+	uint32 cmd_arg;
+	uint32 rsp5;
+
+	sdstd_lock(sd);
+	cmd_arg = 0;
+	cmd_arg = SFIELD(cmd_arg, CMD52_FUNCTION, func);
+	cmd_arg = SFIELD(cmd_arg, CMD52_REG_ADDR, regaddr);
+	cmd_arg = SFIELD(cmd_arg, CMD52_RW_FLAG, rw == SDIOH_READ ? 0 : 1);
+	cmd_arg = SFIELD(cmd_arg, CMD52_RAW, 0);
+	cmd_arg = SFIELD(cmd_arg, CMD52_DATA, rw == SDIOH_READ ? 0 : *byte);
+
+	if ((status = sdstd_cmd_issue(sd, USE_DMA(sd), SDIOH_CMD_52, cmd_arg)) != SUCCESS) {
+		sdstd_unlock(sd);
+		return status;
+	}
+
+	sdstd_cmd_getrsp(sd, &rsp5, 1);
+	if (sdstd_rreg16 (sd, SD_ErrorIntrStatus) != 0) {
+		sd_err(("%s: 1: ErrorintrStatus 0x%x\n",
+		        __FUNCTION__, sdstd_rreg16(sd, SD_ErrorIntrStatus)));
+	}
+	if (GFIELD(rsp5, RSP5_FLAGS) != 0x10)
+		sd_err(("%s: rsp5 flags is 0x%x\t %d\n",
+		        __FUNCTION__, GFIELD(rsp5, RSP5_FLAGS), func));
+
+	if (GFIELD(rsp5, RSP5_STUFF))
+		sd_err(("%s: rsp5 stuff is 0x%x: should be 0\n",
+		        __FUNCTION__, GFIELD(rsp5, RSP5_STUFF)));
+
+	if (rw == SDIOH_READ)
+		*byte = GFIELD(rsp5, RSP5_DATA);
+
+	sdstd_unlock(sd);
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_request_word(sdioh_info_t *sd, uint cmd_type, uint rw, uint func, uint addr,
+                   uint32 *word, uint nbytes)
+{
+	int status;
+	bool swap = FALSE;
+
+	sdstd_lock(sd);
+
+	if (rw == SDIOH_READ) {
+		status = sdstd_card_regread(sd, func, addr, nbytes, word);
+		if (swap)
+			*word = BCMSWAP32(*word);
+	} else {
+		if (swap)
+			*word = BCMSWAP32(*word);
+		status = sdstd_card_regwrite(sd, func, addr, nbytes, *word);
+	}
+
+	sdstd_unlock(sd);
+	return (status == SUCCESS ?  SDIOH_API_RC_SUCCESS : SDIOH_API_RC_FAIL);
+}
+
+extern SDIOH_API_RC
+sdioh_request_buffer(sdioh_info_t *sd, uint pio_dma, uint fix_inc, uint rw, uint func,
+                     uint addr, uint reg_width, uint buflen_u, uint8 *buffer, void *pkt)
+{
+	int len;
+	int buflen = (int)buflen_u;
+	bool fifo = (fix_inc == SDIOH_DATA_FIX);
+	uint8 *localbuf = NULL, *tmpbuf = NULL;
+	uint tmplen = 0;
+	bool local_blockmode = sd->sd_blockmode;
+
+	sdstd_lock(sd);
+
+	ASSERT(reg_width == 4);
+	ASSERT(buflen_u < (1 << 30));
+	ASSERT(sd->client_block_size[func]);
+
+	sd_data(("%s: %c len %d r_cnt %d t_cnt %d, pkt @0x%p\n",
+	         __FUNCTION__, rw == SDIOH_READ ? 'R' : 'W',
+	         buflen_u, sd->r_cnt, sd->t_cnt, pkt));
+
+	/* Break buffer down into blocksize chunks:
+	 * Bytemode: 1 block at a time.
+	 * Blockmode: Multiples of blocksizes at a time w/ max of SD_PAGE.
+	 * Both: leftovers are handled last (will be sent via bytemode).
+	 */
+	while (buflen > 0) {
+		if (local_blockmode) {
+			/* Max xfer is Page size */
+			len = MIN(SD_PAGE, buflen);
+
+			/* Round down to a block boundry */
+			if (buflen > sd->client_block_size[func])
+				len = (len/sd->client_block_size[func]) *
+				        sd->client_block_size[func];
+			if ((func == SDIO_FUNC_1) && ((len % 4) == 3) && (rw == SDIOH_WRITE)) {
+				tmplen = len;
+				sd_err(("%s: Rounding up buffer to mod4 length.\n", __FUNCTION__));
+				len++;
+				tmpbuf = buffer;
+				if ((localbuf = (uint8 *)MALLOC(sd->osh, len)) == NULL) {
+					sd_err(("out of memory, malloced %d bytes\n",
+					        MALLOCED(sd->osh)));
+					sdstd_unlock(sd);
+					return SDIOH_API_RC_FAIL;
+				}
+				bcopy(buffer, localbuf, len);
+				buffer = localbuf;
+			}
+		} else {
+			/* Byte mode: One block at a time */
+			len = MIN(sd->client_block_size[func], buflen);
+		}
+
+		if (sdstd_card_buf(sd, rw, func, fifo, addr, len, (uint32 *)buffer) != SUCCESS) {
+			sdstd_unlock(sd);
+			return SDIOH_API_RC_FAIL;
+		}
+
+		if (local_blockmode) {
+			if ((func == SDIO_FUNC_1) && ((tmplen % 4) == 3) && (rw == SDIOH_WRITE)) {
+				if (localbuf)
+					MFREE(sd->osh, localbuf, len);
+				len--;
+				buffer = tmpbuf;
+				sd_err(("%s: Restoring back buffer ptr and len.\n", __FUNCTION__));
+			}
+		}
+
+		buffer += len;
+		buflen -= len;
+		if (!fifo)
+			addr += len;
+	}
+	sdstd_unlock(sd);
+	return SDIOH_API_RC_SUCCESS;
+}
+
+static
+int sdstd_abort(sdioh_info_t *sd, uint func)
+{
+	int err = 0;
+	int retries;
+
+	uint16 cmd_reg;
+	uint32 cmd_arg;
+	uint32 rsp5;
+	uint8 rflags;
+
+	uint16 int_reg = 0;
+	uint16 plain_intstatus;
+
+	/* Argument is write to F0 (CCCR) IOAbort with function number */
+	cmd_arg = 0;
+	cmd_arg = SFIELD(cmd_arg, CMD52_FUNCTION, SDIO_FUNC_0);
+	cmd_arg = SFIELD(cmd_arg, CMD52_REG_ADDR, SDIOD_CCCR_IOABORT);
+	cmd_arg = SFIELD(cmd_arg, CMD52_RW_FLAG, SD_IO_OP_WRITE);
+	cmd_arg = SFIELD(cmd_arg, CMD52_RAW, 0);
+	cmd_arg = SFIELD(cmd_arg, CMD52_DATA, func);
+
+	/* Command is CMD52 write */
+	cmd_reg = 0;
+	cmd_reg = SFIELD(cmd_reg, CMD_RESP_TYPE, RESP_TYPE_48_BUSY);
+	cmd_reg = SFIELD(cmd_reg, CMD_CRC_EN, 1);
+	cmd_reg = SFIELD(cmd_reg, CMD_INDEX_EN, 1);
+	cmd_reg = SFIELD(cmd_reg, CMD_DATA_EN, 0);
+	cmd_reg = SFIELD(cmd_reg, CMD_TYPE, CMD_TYPE_ABORT);
+	cmd_reg = SFIELD(cmd_reg, CMD_INDEX, SDIOH_CMD_52);
+
+	if (sd->sd_mode == SDIOH_MODE_SPI) {
+		cmd_reg = SFIELD(cmd_reg, CMD_CRC_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX_EN, 0);
+	}
+
+	/* Wait for CMD_INHIBIT to go away as per spec section 3.6.1.1 */
+	retries = RETRIES_SMALL;
+	while (GFIELD(sdstd_rreg(sd, SD_PresentState), PRES_CMD_INHIBIT)) {
+		if (retries == RETRIES_SMALL)
+			sd_err(("%s: Waiting for Command Inhibit, state 0x%08x\n",
+			        __FUNCTION__, sdstd_rreg(sd, SD_PresentState)));
+		if (!--retries) {
+			sd_err(("%s: Command Inhibit timeout, state 0x%08x\n",
+			        __FUNCTION__, sdstd_rreg(sd, SD_PresentState)));
+			if (trap_errs)
+				ASSERT(0);
+			err = BCME_SDIO_ERROR;
+			goto done;
+		}
+	}
+
+	/* Clear errors from any previous commands */
+	if ((plain_intstatus = sdstd_rreg16(sd, SD_ErrorIntrStatus)) != 0) {
+		sd_err(("abort: clearing errstat 0x%04x\n", plain_intstatus));
+		sdstd_wreg16(sd, SD_ErrorIntrStatus, plain_intstatus);
+	}
+	plain_intstatus = sdstd_rreg16(sd, SD_IntrStatus);
+	if (plain_intstatus & ~(SFIELD(0, INTSTAT_CARD_INT, 1))) {
+		sd_err(("abort: intstatus 0x%04x\n", plain_intstatus));
+		if (GFIELD(plain_intstatus, INTSTAT_CMD_COMPLETE)) {
+			sd_err(("SDSTD_ABORT: CMD COMPLETE SET BEFORE COMMAND GIVEN!!!\n"));
+		}
+		if (GFIELD(plain_intstatus, INTSTAT_CARD_REMOVAL)) {
+			sd_err(("SDSTD_ABORT: INTSTAT_CARD_REMOVAL\n"));
+			err = BCME_NODEVICE;
+			goto done;
+		}
+	}
+
+	/* Issue the command */
+	sdstd_wreg(sd, SD_Arg0, cmd_arg);
+	sdstd_wreg16(sd, SD_Command, cmd_reg);
+
+	/* In interrupt mode return, expect later CMD_COMPLETE interrupt */
+	if (!sd->polled_mode)
+		return err;
+
+	/* Otherwise, wait for the command to complete */
+	retries = RETRIES_LARGE;
+	do {
+		int_reg = sdstd_rreg16(sd, SD_IntrStatus);
+	} while (--retries &&
+	         (GFIELD(int_reg, INTSTAT_ERROR_INT) == 0) &&
+	         (GFIELD(int_reg, INTSTAT_CMD_COMPLETE) == 0));
+
+	/* If command completion fails, do a cmd reset and note the error */
+	if (!retries) {
+		sd_err(("%s: CMD_COMPLETE timeout: intr 0x%04x err 0x%04x state 0x%08x\n",
+		        __FUNCTION__, int_reg,
+		        sdstd_rreg16(sd, SD_ErrorIntrStatus),
+		        sdstd_rreg(sd, SD_PresentState)));
+
+		sdstd_wreg8(sd, SD_SoftwareReset, SFIELD(0, SW_RESET_CMD, 1));
+		retries = RETRIES_LARGE;
+		do {
+			sd_trace(("%s: waiting for CMD line reset\n", __FUNCTION__));
+		} while ((GFIELD(sdstd_rreg8(sd, SD_SoftwareReset),
+		                 SW_RESET_CMD)) && retries--);
+
+		if (!retries) {
+			sd_err(("%s: Timeout waiting for CMD line reset\n", __FUNCTION__));
+		}
+
+		if (trap_errs)
+			ASSERT(0);
+
+		err = BCME_SDIO_ERROR;
+	}
+
+	/* Clear Command Complete interrupt */
+	int_reg = SFIELD(0, INTSTAT_CMD_COMPLETE, 1);
+	sdstd_wreg16(sd, SD_IntrStatus, int_reg);
+
+	/* Check for Errors */
+	if ((plain_intstatus = sdstd_rreg16 (sd, SD_ErrorIntrStatus)) != 0) {
+		sd_err(("%s: ErrorintrStatus: 0x%x, "
+		        "(intrstatus = 0x%x, present state 0x%x) clearing\n",
+		        __FUNCTION__, plain_intstatus,
+		        sdstd_rreg16(sd, SD_IntrStatus),
+		        sdstd_rreg(sd, SD_PresentState)));
+
+		sdstd_wreg16(sd, SD_ErrorIntrStatus, plain_intstatus);
+
+		sdstd_wreg8(sd, SD_SoftwareReset, SFIELD(0, SW_RESET_DAT, 1));
+		retries = RETRIES_LARGE;
+		do {
+			sd_trace(("%s: waiting for DAT line reset\n", __FUNCTION__));
+		} while ((GFIELD(sdstd_rreg8(sd, SD_SoftwareReset),
+		                 SW_RESET_DAT)) && retries--);
+
+		if (!retries) {
+			sd_err(("%s: Timeout waiting for DAT line reset\n", __FUNCTION__));
+		}
+
+		if (trap_errs)
+			ASSERT(0);
+
+		/* ABORT is dataless, only cmd errs count */
+		if (plain_intstatus & ERRINT_CMD_ERRS)
+			err = BCME_SDIO_ERROR;
+	}
+
+	/* If command failed don't bother looking at response */
+	if (err)
+		goto done;
+
+	/* Otherwise, check the response */
+	sdstd_cmd_getrsp(sd, &rsp5, 1);
+	rflags = GFIELD(rsp5, RSP5_FLAGS);
+
+	if (rflags & SD_RSP_R5_ERRBITS) {
+		sd_err(("%s: R5 flags include errbits: 0x%02x\n", __FUNCTION__, rflags));
+
+		/* The CRC error flag applies to the previous command */
+		if (rflags & (SD_RSP_R5_ERRBITS & ~SD_RSP_R5_COM_CRC_ERROR)) {
+			err = BCME_SDIO_ERROR;
+			goto done;
+		}
+	}
+
+	if (((rflags & (SD_RSP_R5_IO_CURRENTSTATE0 | SD_RSP_R5_IO_CURRENTSTATE1)) != 0x10) &&
+	    ((rflags & (SD_RSP_R5_IO_CURRENTSTATE0 | SD_RSP_R5_IO_CURRENTSTATE1)) != 0x20)) {
+		sd_err(("%s: R5 flags has bad state: 0x%02x\n", __FUNCTION__, rflags));
+		err = BCME_SDIO_ERROR;
+		goto done;
+	}
+
+	if (GFIELD(rsp5, RSP5_STUFF)) {
+		sd_err(("%s: rsp5 stuff is 0x%x: should be 0\n",
+		        __FUNCTION__, GFIELD(rsp5, RSP5_STUFF)));
+		err = BCME_SDIO_ERROR;
+		goto done;
+	}
+
+done:
+	if (err == BCME_NODEVICE)
+		return err;
+
+	sdstd_wreg8(sd, SD_SoftwareReset,
+	            SFIELD(SFIELD(0, SW_RESET_DAT, 1), SW_RESET_CMD, 1));
+
+	retries = RETRIES_LARGE;
+	do {
+		rflags = sdstd_rreg8(sd, SD_SoftwareReset);
+		if (!GFIELD(rflags, SW_RESET_DAT) && !GFIELD(rflags, SW_RESET_CMD))
+			break;
+	} while (--retries);
+
+	if (!retries) {
+		sd_err(("%s: Timeout waiting for DAT/CMD reset: 0x%02x\n",
+		        __FUNCTION__, rflags));
+		err = BCME_SDIO_ERROR;
+	}
+
+	return err;
+}
+
+extern int
+sdioh_abort(sdioh_info_t *sd, uint fnum)
+{
+	int ret;
+
+	sdstd_lock(sd);
+	ret = sdstd_abort(sd, fnum);
+	sdstd_unlock(sd);
+
+	return ret;
+}
+
+int
+sdioh_start(sdioh_info_t *sd, int stage)
+{
+	return SUCCESS;
+}
+
+int
+sdioh_stop(sdioh_info_t *sd)
+{
+	return SUCCESS;
+}
+
+static int
+sdstd_check_errs(sdioh_info_t *sdioh_info, uint32 cmd, uint32 arg)
+{
+	uint16 regval;
+	uint retries;
+	uint function = 0;
+
+	/* If no errors, we're done */
+	if ((regval = sdstd_rreg16(sdioh_info, SD_ErrorIntrStatus)) == 0)
+		return SUCCESS;
+
+	sd_info(("%s: ErrorIntrStatus 0x%04x (clearing), IntrStatus 0x%04x PresentState 0x%08x\n",
+	        __FUNCTION__, regval, sdstd_rreg16(sdioh_info, SD_IntrStatus),
+	        sdstd_rreg(sdioh_info, SD_PresentState)));
+	sdstd_wreg16(sdioh_info, SD_ErrorIntrStatus, regval);
+
+	/* On command error, issue CMD reset */
+	if (regval & ERRINT_CMD_ERRS) {
+		sd_trace(("%s: issuing CMD reset\n", __FUNCTION__));
+		sdstd_wreg8(sdioh_info, SD_SoftwareReset, SFIELD(0, SW_RESET_CMD, 1));
+		for (retries = RETRIES_LARGE; retries; retries--)
+			if (!(GFIELD(sdstd_rreg8(sdioh_info, SD_SoftwareReset), SW_RESET_CMD)))
+				break;
+		if (!retries) {
+			sd_err(("%s: Timeout waiting for CMD line reset\n", __FUNCTION__));
+		}
+	}
+
+	/* On data error, issue DAT reset */
+	if (regval & ERRINT_DATA_ERRS) {
+		sd_trace(("%s: issuing DAT reset\n", __FUNCTION__));
+		sdstd_wreg8(sdioh_info, SD_SoftwareReset, SFIELD(0, SW_RESET_DAT, 1));
+		for (retries = RETRIES_LARGE; retries; retries--)
+			if (!(GFIELD(sdstd_rreg8(sdioh_info, SD_SoftwareReset), SW_RESET_DAT)))
+				break;
+		if (!retries) {
+			sd_err(("%s: Timeout waiting for DAT line reset\n", __FUNCTION__));
+		}
+	}
+
+	/* For an IO command (CMD52 or CMD53) issue an abort to the appropriate function */
+	if (cmd == SDIOH_CMD_53)
+		function = GFIELD(arg, CMD53_FUNCTION);
+	else if (cmd == SDIOH_CMD_52)
+		function = GFIELD(arg, CMD52_FUNCTION);
+	if (function) {
+		sd_trace(("%s: requesting abort for function %d after cmd %d\n",
+		          __FUNCTION__, function, cmd));
+		sdstd_abort(sdioh_info, function);
+	}
+
+	if (trap_errs)
+		ASSERT(0);
+
+	return ERROR;
+}
+
+
+
+/*
+ * Private/Static work routines
+ */
+static bool
+sdstd_reset(sdioh_info_t *sd, bool host_reset, bool client_reset)
+{
+	int retries = RETRIES_LARGE;
+	uchar regval;
+
+	if (!sd)
+		return TRUE;
+
+	sdstd_lock(sd);
+	/* Reset client card */
+	if (client_reset && (sd->adapter_slot != -1)) {
+		if (sdstd_card_regwrite(sd, 0, SDIOD_CCCR_IOABORT, 1, 0x8) != SUCCESS)
+			sd_err(("%s: Cannot write to card reg 0x%x\n",
+			        __FUNCTION__, SDIOD_CCCR_IOABORT));
+		else
+			sd->card_rca = 0;
+	}
+
+	/* Reset host controller */
+	if (host_reset) {
+		regval = SFIELD(0, SW_RESET_ALL, 1);
+		sdstd_wreg8(sd, SD_SoftwareReset, regval);
+		do {
+			sd_trace(("%s: waiting for reset\n", __FUNCTION__));
+		} while ((sdstd_rreg8(sd, SD_SoftwareReset) & regval) && retries--);
+
+		if (!retries) {
+			sd_err(("%s: Timeout waiting for host reset\n", __FUNCTION__));
+			sdstd_unlock(sd);
+			return (FALSE);
+		}
+
+		/* A reset should reset bus back to 1 bit mode */
+		sd->sd_mode = SDIOH_MODE_SD1;
+		sdstd_set_dma_mode(sd, sd->sd_dma_mode);
+	}
+	sdstd_unlock(sd);
+	return TRUE;
+}
+
+/* Disable device interrupt */
+void
+sdstd_devintr_off(sdioh_info_t *sd)
+{
+	sd_trace(("%s: %d\n", __FUNCTION__, sd->use_client_ints));
+	if (sd->use_client_ints) {
+		sd->intmask &= ~CLIENT_INTR;
+		sdstd_wreg16(sd, SD_IntrSignalEnable, sd->intmask);
+		sdstd_rreg16(sd, SD_IntrSignalEnable); /* Sync readback */
+	}
+}
+
+/* Enable device interrupt */
+void
+sdstd_devintr_on(sdioh_info_t *sd)
+{
+	ASSERT(sd->lockcount == 0);
+	sd_trace(("%s: %d\n", __FUNCTION__, sd->use_client_ints));
+	if (sd->use_client_ints) {
+		uint16 status = sdstd_rreg16(sd, SD_IntrStatusEnable);
+		sdstd_wreg16(sd, SD_IntrStatusEnable, SFIELD(status, INTSTAT_CARD_INT, 0));
+		sdstd_wreg16(sd, SD_IntrStatusEnable, status);
+
+		sd->intmask |= CLIENT_INTR;
+		sdstd_wreg16(sd, SD_IntrSignalEnable, sd->intmask);
+		sdstd_rreg16(sd, SD_IntrSignalEnable); /* Sync readback */
+	}
+}
+
+#ifdef BCMSDYIELD
+/* Enable/disable other interrupts */
+void
+sdstd_intrs_on(sdioh_info_t *sd, uint16 norm, uint16 err)
+{
+	if (err) {
+		norm = SFIELD(norm, INTSTAT_ERROR_INT, 1);
+		sdstd_wreg16(sd, SD_ErrorIntrSignalEnable, err);
+	}
+
+	sd->intmask |= norm;
+	sdstd_wreg16(sd, SD_IntrSignalEnable, sd->intmask);
+	if (sd_forcerb)
+		sdstd_rreg16(sd, SD_IntrSignalEnable); /* Sync readback */
+}
+
+void
+sdstd_intrs_off(sdioh_info_t *sd, uint16 norm, uint16 err)
+{
+	if (err) {
+		norm = SFIELD(norm, INTSTAT_ERROR_INT, 1);
+		sdstd_wreg16(sd, SD_ErrorIntrSignalEnable, 0);
+	}
+
+	sd->intmask &= ~norm;
+	sdstd_wreg16(sd, SD_IntrSignalEnable, sd->intmask);
+	if (sd_forcerb)
+		sdstd_rreg16(sd, SD_IntrSignalEnable); /* Sync readback */
+}
+#endif /* BCMSDYIELD */
+
+static int
+sdstd_host_init(sdioh_info_t *sd)
+{
+	int 		num_slots, full_slot;
+	uint8		reg8;
+
+	uint32		card_ins;
+	int			slot, first_bar = 0;
+	bool		detect_slots = FALSE;
+	uint		bar;
+
+	/* Check for Arasan ID */
+	if ((OSL_PCI_READ_CONFIG(sd->osh, PCI_CFG_VID, 4) & 0xFFFF) == VENDOR_SI_IMAGE) {
+		sd_info(("%s: Found Arasan Standard SDIO Host Controller\n", __FUNCTION__));
+		sd->controller_type = SDIOH_TYPE_ARASAN_HDK;
+		detect_slots = TRUE;
+	} else if ((OSL_PCI_READ_CONFIG(sd->osh, PCI_CFG_VID, 4) & 0xFFFF) == VENDOR_BROADCOM) {
+		sd_info(("%s: Found Broadcom 27xx Standard SDIO Host Controller\n", __FUNCTION__));
+		sd->controller_type = SDIOH_TYPE_BCM27XX;
+		detect_slots = FALSE;
+	} else if ((OSL_PCI_READ_CONFIG(sd->osh, PCI_CFG_VID, 4) & 0xFFFF) == VENDOR_TI) {
+		sd_info(("%s: Found TI PCIxx21 Standard SDIO Host Controller\n", __FUNCTION__));
+		sd->controller_type = SDIOH_TYPE_TI_PCIXX21;
+		detect_slots = TRUE;
+	} else if ((OSL_PCI_READ_CONFIG(sd->osh, PCI_CFG_VID, 4) & 0xFFFF) == VENDOR_RICOH) {
+		sd_info(("%s: Ricoh Co Ltd R5C822 SD/SDIO/MMC/MS/MSPro Host Adapter\n",
+			__FUNCTION__));
+		sd->controller_type = SDIOH_TYPE_RICOH_R5C822;
+		detect_slots = TRUE;
+	} else if ((OSL_PCI_READ_CONFIG(sd->osh, PCI_CFG_VID, 4) & 0xFFFF) == VENDOR_JMICRON) {
+		sd_info(("%s: JMicron Standard SDIO Host Controller\n",
+			__FUNCTION__));
+		sd->controller_type = SDIOH_TYPE_JMICRON;
+		detect_slots = TRUE;
+	} else {
+		return ERROR;
+	}
+
+	/*
+	 * Determine num of slots
+	 * Search each slot
+	 */
+
+	first_bar = OSL_PCI_READ_CONFIG(sd->osh, SD_SlotInfo, 4) & 0x7;
+	num_slots = (OSL_PCI_READ_CONFIG(sd->osh, SD_SlotInfo, 4) & 0xff) >> 4;
+	num_slots &= 7;
+	num_slots++;   	/* map bits to num slots according to spec */
+
+	if (OSL_PCI_READ_CONFIG(sd->osh, PCI_CFG_VID, 4) ==
+	    ((SDIOH_FPGA_ID << 16) | VENDOR_BROADCOM)) {
+		sd_err(("%s: Found Broadcom Standard SDIO Host Controller FPGA\n", __FUNCTION__));
+		/* Set BAR0 Window to SDIOSTH core */
+		OSL_PCI_WRITE_CONFIG(sd->osh, PCI_BAR0_WIN, 4, 0x18001000);
+
+		/* Set defaults particular to this controller. */
+		detect_slots = TRUE;
+		num_slots = 1;
+		first_bar = 0;
+
+		/* Controller supports ADMA2, so turn it on here. */
+		sd->sd_dma_mode = DMA_MODE_ADMA2;
+	}
+
+	/* Map in each slot on the board and query it to see if a
+	 * card is inserted.  Use the first populated slot found.
+	 */
+	if (sd->mem_space) {
+		sdstd_reg_unmap(sd->osh, (uintptr)sd->mem_space, SDIOH_REG_WINSZ);
+		sd->mem_space = NULL;
+	}
+
+	full_slot = -1;
+
+	for (slot = 0; slot < num_slots; slot++) {
+		bar = OSL_PCI_READ_CONFIG(sd->osh, PCI_CFG_BAR0 + (4*(slot + first_bar)), 4);
+		sd->mem_space = (volatile char *)sdstd_reg_map(sd->osh,
+		                                               (uintptr)bar, SDIOH_REG_WINSZ);
+
+		sd->adapter_slot = -1;
+
+		if (detect_slots) {
+			card_ins = GFIELD(sdstd_rreg(sd, SD_PresentState), PRES_CARD_PRESENT);
+		} else {
+			card_ins = TRUE;
+		}
+
+		if (card_ins) {
+			sd_info(("%s: SDIO slot %d: Full\n", __FUNCTION__, slot));
+			if (full_slot < 0)
+				full_slot = slot;
+		} else {
+			sd_info(("%s: SDIO slot %d: Empty\n", __FUNCTION__, slot));
+		}
+
+		if (sd->mem_space) {
+			sdstd_reg_unmap(sd->osh, (uintptr)sd->mem_space, SDIOH_REG_WINSZ);
+			sd->mem_space = NULL;
+		}
+	}
+
+	if (full_slot < 0) {
+		sd_err(("No slots on SDIO controller are populated\n"));
+		return -1;
+	}
+
+	bar = OSL_PCI_READ_CONFIG(sd->osh, PCI_CFG_BAR0 + (4*(full_slot + first_bar)), 4);
+	sd->mem_space = (volatile char *)sdstd_reg_map(sd->osh, (uintptr)bar, SDIOH_REG_WINSZ);
+
+	sd_err(("Using slot %d at BAR%d [0x%08x] mem_space 0x%p\n",
+		full_slot,
+		(full_slot + first_bar),
+		OSL_PCI_READ_CONFIG(sd->osh, PCI_CFG_BAR0 + (4*(full_slot + first_bar)), 4),
+		sd->mem_space));
+
+
+	sd->adapter_slot = full_slot;
+
+	sd->version = sdstd_rreg16(sd, SD_HostControllerVersion) & 0xFF;
+	switch (sd->version) {
+		case 0:
+			sd_err(("Host Controller version 1.0, Vendor Revision: 0x%02x\n",
+				sdstd_rreg16(sd, SD_HostControllerVersion) >> 8));
+			break;
+		case 1:
+		case 2:
+			sd_err(("Host Controller version 2.0, Vendor Revision: 0x%02x\n",
+				sdstd_rreg16(sd, SD_HostControllerVersion) >> 8));
+			break;
+		default:
+			sd_err(("%s: Host Controller version 0x%02x not supported.\n",
+			    __FUNCTION__, sd->version));
+			break;
+	}
+
+	sd->caps = sdstd_rreg(sd, SD_Capabilities);	/* Cache this for later use */
+	sd->curr_caps = sdstd_rreg(sd, SD_MaxCurCap);
+
+	sdstd_set_dma_mode(sd, sd->sd_dma_mode);
+
+
+	sdstd_reset(sd, 1, 0);
+
+	/* Read SD4/SD1 mode */
+	if ((reg8 = sdstd_rreg8(sd, SD_HostCntrl))) {
+		if (reg8 & SD4_MODE) {
+			sd_err(("%s: Host cntrlr already in 4 bit mode: 0x%x\n",
+			        __FUNCTION__,  reg8));
+		}
+	}
+
+	/* Default power on mode is SD1 */
+	sd->sd_mode = SDIOH_MODE_SD1;
+	sd->polled_mode = TRUE;
+	sd->host_init_done = TRUE;
+	sd->card_init_done = FALSE;
+	sd->adapter_slot = full_slot;
+
+	return (SUCCESS);
+}
+#define CMD5_RETRIES 200
+static int
+get_ocr(sdioh_info_t *sd, uint32 *cmd_arg, uint32 *cmd_rsp)
+{
+	int retries, status;
+
+	/* Get the Card's Operation Condition.  Occasionally the board
+	 * takes a while to become ready
+	 */
+	retries = CMD5_RETRIES;
+	do {
+		*cmd_rsp = 0;
+		if ((status = sdstd_cmd_issue(sd, USE_DMA(sd), SDIOH_CMD_5, *cmd_arg))
+		    != SUCCESS) {
+			sd_err(("%s: CMD5 failed\n", __FUNCTION__));
+			return status;
+		}
+		sdstd_cmd_getrsp(sd, cmd_rsp, 1);
+		if (!GFIELD(*cmd_rsp, RSP4_CARD_READY))
+			sd_trace(("%s: Waiting for card to become ready\n", __FUNCTION__));
+	} while ((!GFIELD(*cmd_rsp, RSP4_CARD_READY)) && --retries);
+	if (!retries)
+		return ERROR;
+
+	return (SUCCESS);
+}
+
+static int
+sdstd_client_init(sdioh_info_t *sd)
+{
+	uint32 cmd_arg, cmd_rsp;
+	int status;
+	uint8 fn_ints;
+
+
+	sd_trace(("%s: Powering up slot %d\n", __FUNCTION__, sd->adapter_slot));
+
+	/* Clear any pending ints */
+	sdstd_wreg16(sd, SD_IntrStatus, 0x1ff);
+	sdstd_wreg16(sd, SD_ErrorIntrStatus, 0x0fff);
+
+	/* Enable both Normal and Error Status.  This does not enable
+	 * interrupts, it only enables the status bits to
+	 * become 'live'
+	 */
+	sdstd_wreg16(sd, SD_IntrStatusEnable, 0x1ff);
+	sdstd_wreg16(sd, SD_ErrorIntrStatusEnable, 0xffff);
+
+	sdstd_wreg16(sd, SD_IntrSignalEnable, 0);	  /* Disable ints for now. */
+
+	/* Start at ~400KHz clock rate for initialization */
+	if (!sdstd_start_clock(sd, 128)) {
+		sd_err(("sdstd_start_clock failed\n"));
+		return ERROR;
+	}
+	if (!sdstd_start_power(sd)) {
+		sd_err(("sdstd_start_power failed\n"));
+		return ERROR;
+	}
+
+	if (sd->num_funcs == 0) {
+		sd_err(("%s: No IO funcs!\n", __FUNCTION__));
+		return ERROR;
+	}
+
+	/* In SPI mode, issue CMD0 first */
+	if (sd->sd_mode == SDIOH_MODE_SPI) {
+		cmd_arg = 0;
+		if ((status = sdstd_cmd_issue(sd, USE_DMA(sd), SDIOH_CMD_0, cmd_arg))
+		    != SUCCESS) {
+			sd_err(("BCMSDIOH: cardinit: CMD0 failed!\n"));
+			return status;
+		}
+	}
+
+	if (sd->sd_mode != SDIOH_MODE_SPI) {
+		uint16 rsp6_status;
+
+		/* Card is operational. Ask it to send an RCA */
+		cmd_arg = 0;
+		if ((status = sdstd_cmd_issue(sd, USE_DMA(sd), SDIOH_CMD_3, cmd_arg))
+		    != SUCCESS) {
+			sd_err(("%s: CMD3 failed!\n", __FUNCTION__));
+			return status;
+		}
+
+		/* Verify the card status returned with the cmd response */
+		sdstd_cmd_getrsp(sd, &cmd_rsp, 1);
+		rsp6_status = GFIELD(cmd_rsp, RSP6_STATUS);
+		if (GFIELD(rsp6_status, RSP6STAT_COM_CRC_ERROR) ||
+		    GFIELD(rsp6_status, RSP6STAT_ILLEGAL_CMD) ||
+		    GFIELD(rsp6_status, RSP6STAT_ERROR)) {
+			sd_err(("%s: CMD3 response error. Response = 0x%x!\n",
+			        __FUNCTION__, rsp6_status));
+			return ERROR;
+		}
+
+		/* Save the Card's RCA */
+		sd->card_rca = GFIELD(cmd_rsp, RSP6_IO_RCA);
+		sd_info(("RCA is 0x%x\n", sd->card_rca));
+
+		if (rsp6_status)
+			sd_err(("raw status is 0x%x\n", rsp6_status));
+
+		/* Select the card */
+		cmd_arg = SFIELD(0, CMD7_RCA, sd->card_rca);
+		if ((status = sdstd_cmd_issue(sd, USE_DMA(sd), SDIOH_CMD_7, cmd_arg))
+		    != SUCCESS) {
+			sd_err(("%s: CMD7 failed!\n", __FUNCTION__));
+			return status;
+		}
+		sdstd_cmd_getrsp(sd, &cmd_rsp, 1);
+		if (cmd_rsp != SDIOH_CMD7_EXP_STATUS) {
+			sd_err(("%s: CMD7 response error. Response = 0x%x!\n",
+			        __FUNCTION__, cmd_rsp));
+			return ERROR;
+		}
+	}
+
+	sdstd_card_enablefuncs(sd);
+
+	if (!sdstd_bus_width(sd, sd_sdmode)) {
+		sd_err(("sdstd_bus_width failed\n"));
+		return ERROR;
+	}
+
+	set_client_block_size(sd, 1, BLOCK_SIZE_4318);
+	fn_ints = INTR_CTL_FUNC1_EN;
+
+	if (sd->num_funcs >= 2) {
+		set_client_block_size(sd, 2, sd_f2_blocksize /* BLOCK_SIZE_4328 */);
+		fn_ints |= INTR_CTL_FUNC2_EN;
+	}
+
+	/* Enable/Disable Client interrupts */
+	/* Turn on here but disable at host controller? */
+	if (sdstd_card_regwrite(sd, 0, SDIOD_CCCR_INTEN, 1,
+	                        (fn_ints | INTR_CTL_MASTER_EN)) != SUCCESS) {
+		sd_err(("%s: Could not enable ints in CCCR\n", __FUNCTION__));
+		return ERROR;
+	}
+
+	/* Switch to High-speed clocking mode if both host and device support it */
+	sdstd_set_highspeed_mode(sd, (bool)sd_hiok);
+
+	/* After configuring for High-Speed mode, set the desired clock rate. */
+	if (!sdstd_start_clock(sd, (uint16)sd_divisor)) {
+		sd_err(("sdstd_start_clock failed\n"));
+		return ERROR;
+	}
+
+	sd->card_init_done = TRUE;
+
+	return SUCCESS;
+}
+
+static int
+sdstd_set_highspeed_mode(sdioh_info_t *sd, bool HSMode)
+{
+	uint32 regdata;
+	int status;
+	uint8 reg8;
+
+	reg8 = sdstd_rreg8(sd, SD_HostCntrl);
+
+
+	if (HSMode == TRUE) {
+		if (sd_hiok && (GFIELD(sd->caps, CAP_HIGHSPEED)) == 0) {
+			sd_err(("Host Controller does not support hi-speed mode.\n"));
+			return BCME_ERROR;
+		}
+
+		sd_info(("Attempting to enable High-Speed mode.\n"));
+
+		if ((status = sdstd_card_regread(sd, 0, SDIOD_CCCR_SPEED_CONTROL,
+		                                 1, &regdata)) != SUCCESS) {
+			return BCME_SDIO_ERROR;
+		}
+		if (regdata & SDIO_SPEED_SHS) {
+			sd_info(("Device supports High-Speed mode.\n"));
+
+			regdata |= SDIO_SPEED_EHS;
+
+			sd_info(("Writing %08x to Card at %08x\n",
+			         regdata, SDIOD_CCCR_SPEED_CONTROL));
+			if ((status = sdstd_card_regwrite(sd, 0, SDIOD_CCCR_SPEED_CONTROL,
+			                                  1, regdata)) != BCME_OK) {
+				return BCME_SDIO_ERROR;
+			}
+
+			if ((status = sdstd_card_regread(sd, 0, SDIOD_CCCR_SPEED_CONTROL,
+			                                 1, &regdata)) != BCME_OK) {
+				return BCME_SDIO_ERROR;
+			}
+
+			sd_info(("Read %08x to Card at %08x\n", regdata, SDIOD_CCCR_SPEED_CONTROL));
+
+			reg8 = SFIELD(reg8, HOST_HI_SPEED_EN, 1);
+
+			sd_err(("High-speed clocking mode enabled.\n"));
+		}
+		else {
+			sd_err(("Device does not support High-Speed Mode.\n"));
+			reg8 = SFIELD(reg8, HOST_HI_SPEED_EN, 0);
+		}
+	} else {
+		/* Force off device bit */
+		if ((status = sdstd_card_regread(sd, 0, SDIOD_CCCR_SPEED_CONTROL,
+		                                 1, &regdata)) != BCME_OK) {
+			return status;
+		}
+		if (regdata & SDIO_SPEED_EHS) {
+			regdata &= ~SDIO_SPEED_EHS;
+			if ((status = sdstd_card_regwrite(sd, 0, SDIOD_CCCR_SPEED_CONTROL,
+			                                  1, regdata)) != BCME_OK) {
+				return status;
+			}
+		}
+
+		sd_err(("High-speed clocking mode disabled.\n"));
+		reg8 = SFIELD(reg8, HOST_HI_SPEED_EN, 0);
+	}
+
+	sdstd_wreg8(sd, SD_HostCntrl, reg8);
+
+	return BCME_OK;
+}
+
+/* Select DMA Mode:
+ * If dma_mode == DMA_MODE_AUTO, pick the "best" mode.
+ * Otherwise, pick the selected mode if supported.
+ * If not supported, use PIO mode.
+ */
+static int
+sdstd_set_dma_mode(sdioh_info_t *sd, int8 dma_mode)
+{
+	uint8 reg8, dma_sel_bits = SDIOH_SDMA_MODE;
+	int8 prev_dma_mode = sd->sd_dma_mode;
+
+	switch (prev_dma_mode) {
+		case DMA_MODE_AUTO:
+			sd_dma(("%s: Selecting best DMA mode supported by controller.\n",
+			          __FUNCTION__));
+			if (GFIELD(sd->caps, CAP_ADMA2)) {
+				sd->sd_dma_mode = DMA_MODE_ADMA2;
+				dma_sel_bits = SDIOH_ADMA2_MODE;
+			} else if (GFIELD(sd->caps, CAP_ADMA1)) {
+				sd->sd_dma_mode = DMA_MODE_ADMA1;
+				dma_sel_bits = SDIOH_ADMA1_MODE;
+			} else if (GFIELD(sd->caps, CAP_DMA)) {
+				sd->sd_dma_mode = DMA_MODE_SDMA;
+			} else {
+				sd->sd_dma_mode = DMA_MODE_NONE;
+			}
+			break;
+		case DMA_MODE_NONE:
+			sd->sd_dma_mode = DMA_MODE_NONE;
+			break;
+		case DMA_MODE_SDMA:
+			if (GFIELD(sd->caps, CAP_DMA)) {
+				sd->sd_dma_mode = DMA_MODE_SDMA;
+			} else {
+				sd_err(("%s: SDMA not supported by controller.\n", __FUNCTION__));
+				sd->sd_dma_mode = DMA_MODE_NONE;
+			}
+			break;
+		case DMA_MODE_ADMA1:
+			if (GFIELD(sd->caps, CAP_ADMA1)) {
+				sd->sd_dma_mode = DMA_MODE_ADMA1;
+				dma_sel_bits = SDIOH_ADMA1_MODE;
+			} else {
+				sd_err(("%s: ADMA1 not supported by controller.\n", __FUNCTION__));
+				sd->sd_dma_mode = DMA_MODE_NONE;
+			}
+			break;
+		case DMA_MODE_ADMA2:
+			if (GFIELD(sd->caps, CAP_ADMA2)) {
+				sd->sd_dma_mode = DMA_MODE_ADMA2;
+				dma_sel_bits = SDIOH_ADMA2_MODE;
+			} else {
+				sd_err(("%s: ADMA2 not supported by controller.\n", __FUNCTION__));
+				sd->sd_dma_mode = DMA_MODE_NONE;
+			}
+			break;
+		case DMA_MODE_ADMA2_64:
+			sd_err(("%s: 64b ADMA2 not supported by driver.\n", __FUNCTION__));
+			sd->sd_dma_mode = DMA_MODE_NONE;
+			break;
+		default:
+			sd_err(("%s: Unsupported DMA Mode %d requested.\n", __FUNCTION__,
+			        prev_dma_mode));
+			sd->sd_dma_mode = DMA_MODE_NONE;
+			break;
+	}
+
+	/* clear SysAddr, only used for SDMA */
+	sdstd_wreg(sd, SD_SysAddr, 0);
+
+	sd_err(("%s: %s mode selected.\n", __FUNCTION__, dma_mode_description[sd->sd_dma_mode]));
+
+	reg8 = sdstd_rreg8(sd, SD_HostCntrl);
+	reg8 = SFIELD(reg8, HOST_DMA_SEL, dma_sel_bits);
+	sdstd_wreg8(sd, SD_HostCntrl, reg8);
+	sd_dma(("%s: SD_HostCntrl=0x%02x\n", __FUNCTION__, reg8));
+
+	return BCME_OK;
+}
+
+
+bool
+sdstd_start_clock(sdioh_info_t *sd, uint16 new_sd_divisor)
+{
+	uint rc, count;
+	uint16 divisor;
+
+	/* turn off HC clock */
+	sdstd_wreg16(sd, SD_ClockCntrl,
+	             sdstd_rreg16(sd, SD_ClockCntrl) & ~((uint16)0x4)); /*  Disable the HC clock */
+
+	/* Set divisor */
+
+	divisor = (new_sd_divisor >> 1) << 8;
+
+	sd_info(("Clock control is 0x%x\n", sdstd_rreg16(sd, SD_ClockCntrl)));
+	sdstd_mod_reg16(sd, SD_ClockCntrl, 0xff00, divisor);
+	sd_info(("%s: Using clock divisor of %d (regval 0x%04x)\n", __FUNCTION__,
+	         new_sd_divisor, divisor));
+
+	sd_info(("Primary Clock Freq = %d MHz\n", GFIELD(sd->caps, CAP_TO_CLKFREQ)));
+
+	if (GFIELD(sd->caps, CAP_TO_CLKFREQ) == 50) {
+		sd_info(("%s: Resulting SDIO clock is %d %s\n", __FUNCTION__,
+		        ((50 % new_sd_divisor) ? (50000 / new_sd_divisor) : (50 / new_sd_divisor)),
+		        ((50 % new_sd_divisor) ? "KHz" : "MHz")));
+	} else if (GFIELD(sd->caps, CAP_TO_CLKFREQ) == 48) {
+		sd_info(("%s: Resulting SDIO clock is %d %s\n", __FUNCTION__,
+		        ((48 % new_sd_divisor) ? (48000 / new_sd_divisor) : (48 / new_sd_divisor)),
+		        ((48 % new_sd_divisor) ? "KHz" : "MHz")));
+	} else if (GFIELD(sd->caps, CAP_TO_CLKFREQ) == 33) {
+		sd_info(("%s: Resulting SDIO clock is %d %s\n", __FUNCTION__,
+		        ((33 % new_sd_divisor) ? (33000 / new_sd_divisor) : (33 / new_sd_divisor)),
+		        ((33 % new_sd_divisor) ? "KHz" : "MHz")));
+
+	} else if (sd->controller_type == SDIOH_TYPE_BCM27XX) {
+	} else {
+		sd_err(("Need to determine divisor for %d MHz clocks\n",
+		        GFIELD(sd->caps, CAP_TO_CLKFREQ)));
+		sd_err(("Consult SD Host Controller Spec: Clock Control Register\n"));
+		return (FALSE);
+	}
+
+	sdstd_or_reg16(sd, SD_ClockCntrl, 0x1); /*  Enable the clock */
+
+	/* Wait for clock to stabilize */
+	rc = (sdstd_rreg16(sd, SD_ClockCntrl) & 2);
+	count = 0;
+	while (!rc) {
+		OSL_DELAY(1);
+		sd_info(("Waiting for clock to become stable 0x%x\n", rc));
+		rc = (sdstd_rreg16(sd, SD_ClockCntrl) & 2);
+		count++;
+		if (count > 10000) {
+			sd_err(("%s:Clocks failed to stabilize after %u attempts",
+			        __FUNCTION__, count));
+			return (FALSE);
+		}
+	}
+	/* Turn on clock */
+	sdstd_or_reg16(sd, SD_ClockCntrl, 0x4);
+
+	/* Set timeout control (adjust default value based on divisor).
+	 * Disabling timeout interrupts during setting is advised by host spec.
+	 */
+	{
+		uint16 regdata;
+		uint toval;
+
+		toval = sd_toctl;
+		divisor = new_sd_divisor;
+
+		while (toval && !(divisor & 1)) {
+			toval -= 1;
+			divisor >>= 1;
+		}
+
+		regdata = sdstd_rreg16(sd, SD_ErrorIntrStatusEnable);
+		sdstd_wreg16(sd, SD_ErrorIntrStatusEnable, (regdata & ~ERRINT_DATA_TIMEOUT_BIT));
+		sdstd_wreg8(sd, SD_TimeoutCntrl, (uint8)toval);
+		sdstd_wreg16(sd, SD_ErrorIntrStatusEnable, regdata);
+	}
+
+	OSL_DELAY(2);
+
+	sd_info(("Final Clock control is 0x%x\n", sdstd_rreg16(sd, SD_ClockCntrl)));
+
+	return TRUE;
+}
+
+bool
+sdstd_start_power(sdioh_info_t *sd)
+{
+	char *s;
+	uint32 cmd_arg;
+	uint32 cmd_rsp;
+	uint8 pwr = 0;
+	int volts;
+
+	volts = 0;
+	s = NULL;
+	if (GFIELD(sd->caps, CAP_VOLT_1_8)) {
+		volts = 5;
+		s = "1.8";
+	}
+	if (GFIELD(sd->caps, CAP_VOLT_3_0)) {
+		volts = 6;
+		s = "3.0";
+	}
+	if (GFIELD(sd->caps, CAP_VOLT_3_3)) {
+		volts = 7;
+		s = "3.3";
+	}
+
+	pwr = SFIELD(pwr, PWR_VOLTS, volts);
+	pwr = SFIELD(pwr, PWR_BUS_EN, 1);
+	sdstd_wreg8(sd, SD_PwrCntrl, pwr); /* Set Voltage level */
+	sd_info(("Setting Bus Power to %s Volts\n", s));
+
+	/* Wait for power to stabilize, Dongle takes longer than NIC. */
+	OSL_DELAY(250000);
+
+	/* Get the Card's Operation Condition.  Occasionally the board
+	 * takes a while to become ready
+	 */
+	cmd_arg = 0;
+	cmd_rsp = 0;
+	if (get_ocr(sd, &cmd_arg, &cmd_rsp) != SUCCESS) {
+		sd_err(("%s: Failed to get OCR bailing\n", __FUNCTION__));
+		sdstd_reset(sd, 0, 1);
+		return FALSE;
+	}
+
+	sd_info(("mem_present = %d\n", GFIELD(cmd_rsp, RSP4_MEM_PRESENT)));
+	sd_info(("num_funcs = %d\n", GFIELD(cmd_rsp, RSP4_NUM_FUNCS)));
+	sd_info(("card_ready = %d\n", GFIELD(cmd_rsp, RSP4_CARD_READY)));
+	sd_info(("OCR = 0x%x\n", GFIELD(cmd_rsp, RSP4_IO_OCR)));
+
+	/* Verify that the card supports I/O mode */
+	if (GFIELD(cmd_rsp, RSP4_NUM_FUNCS) == 0) {
+		sd_err(("%s: Card does not support I/O\n", __FUNCTION__));
+		return ERROR;
+	}
+	sd->num_funcs = GFIELD(cmd_rsp, RSP4_NUM_FUNCS);
+
+	/* Examine voltage: Arasan only supports 3.3 volts,
+	 * so look for 3.2-3.3 Volts and also 3.3-3.4 volts.
+	 */
+
+	if ((GFIELD(cmd_rsp, RSP4_IO_OCR) & (0x3 << 20)) == 0) {
+		sd_err(("This client does not support 3.3 volts!\n"));
+		return ERROR;
+	}
+	sd_info(("Leaving bus power at 3.3 Volts\n"));
+
+	cmd_arg = SFIELD(0, CMD5_OCR, 0xfff000);
+	cmd_rsp = 0;
+	get_ocr(sd, &cmd_arg, &cmd_rsp);
+	sd_info(("OCR = 0x%x\n", GFIELD(cmd_rsp, RSP4_IO_OCR)));
+	return TRUE;
+}
+
+bool
+sdstd_bus_width(sdioh_info_t *sd, int new_mode)
+{
+	uint32 regdata;
+	int status;
+	uint8 reg8;
+
+	sd_trace(("%s\n", __FUNCTION__));
+	if (sd->sd_mode == new_mode) {
+		sd_info(("%s: Already at width %d\n", __FUNCTION__, new_mode));
+		/* Could exit, but continue just in case... */
+	}
+
+	/* Set client side via reg 0x7 in CCCR */
+	if ((status = sdstd_card_regread (sd, 0, SDIOD_CCCR_BICTRL, 1, &regdata)) != SUCCESS)
+		return (bool)status;
+	regdata &= ~BUS_SD_DATA_WIDTH_MASK;
+	if (new_mode == SDIOH_MODE_SD4) {
+		sd_info(("Changing to SD4 Mode\n"));
+		regdata |= SD4_MODE;
+	} else if (new_mode == SDIOH_MODE_SD1) {
+		sd_info(("Changing to SD1 Mode\n"));
+	} else {
+		sd_err(("SPI Mode not supported by Standard Host Controller\n"));
+	}
+
+	if ((status = sdstd_card_regwrite (sd, 0, SDIOD_CCCR_BICTRL, 1, regdata)) != SUCCESS)
+		return (bool)status;
+
+	/* Set host side via Host reg */
+	reg8 = sdstd_rreg8(sd, SD_HostCntrl) & ~SD4_MODE;
+	if (new_mode == SDIOH_MODE_SD4)
+		reg8 |= SD4_MODE;
+	sdstd_wreg8(sd, SD_HostCntrl, reg8);
+
+	sd->sd_mode = new_mode;
+
+	return TRUE;
+}
+
+static int
+sdstd_driver_init(sdioh_info_t *sd)
+{
+	sd_trace(("%s\n", __FUNCTION__));
+	if ((sdstd_host_init(sd)) != SUCCESS) {
+		return ERROR;
+	}
+
+	if (sdstd_client_init(sd) != SUCCESS) {
+		return ERROR;
+	}
+
+	return SUCCESS;
+}
+
+static int
+sdstd_get_cisaddr(sdioh_info_t *sd, uint32 regaddr)
+{
+	/* read 24 bits and return valid 17 bit addr */
+	int i;
+	uint32 scratch, regdata;
+	uint8 *ptr = (uint8 *)&scratch;
+	for (i = 0; i < 3; i++) {
+		if ((sdstd_card_regread (sd, 0, regaddr, 1, &regdata)) != SUCCESS)
+			sd_err(("%s: Can't read!\n", __FUNCTION__));
+
+		*ptr++ = (uint8) regdata;
+		regaddr++;
+	}
+	/* Only the lower 17-bits are valid */
+	scratch = ltoh32(scratch);
+	scratch &= 0x0001FFFF;
+	return (scratch);
+}
+
+static int
+sdstd_card_enablefuncs(sdioh_info_t *sd)
+{
+	int status;
+	uint32 regdata;
+	uint32 fbraddr;
+	uint8 func;
+
+	sd_trace(("%s\n", __FUNCTION__));
+
+	/* Get the Card's common CIS address */
+	sd->com_cis_ptr = sdstd_get_cisaddr(sd, SDIOD_CCCR_CISPTR_0);
+	sd->func_cis_ptr[0] = sd->com_cis_ptr;
+	sd_info(("%s: Card's Common CIS Ptr = 0x%x\n", __FUNCTION__, sd->com_cis_ptr));
+
+	/* Get the Card's function CIS (for each function) */
+	for (fbraddr = SDIOD_FBR_STARTADDR, func = 1;
+	     func <= sd->num_funcs; func++, fbraddr += SDIOD_FBR_SIZE) {
+		sd->func_cis_ptr[func] = sdstd_get_cisaddr(sd, SDIOD_FBR_CISPTR_0 + fbraddr);
+		sd_info(("%s: Function %d CIS Ptr = 0x%x\n",
+		         __FUNCTION__, func, sd->func_cis_ptr[func]));
+	}
+
+	/* Enable function 1 on the card */
+	regdata = SDIO_FUNC_ENABLE_1;
+	if ((status = sdstd_card_regwrite(sd, 0, SDIOD_CCCR_IOEN, 1, regdata)) != SUCCESS)
+		return status;
+
+	return SUCCESS;
+}
+
+/* Read client card reg */
+static int
+sdstd_card_regread(sdioh_info_t *sd, int func, uint32 regaddr, int regsize, uint32 *data)
+{
+	int status;
+	uint32 cmd_arg;
+	uint32 rsp5;
+
+
+	cmd_arg = 0;
+
+	if ((func == 0) || (regsize == 1)) {
+		cmd_arg = SFIELD(cmd_arg, CMD52_FUNCTION, func);
+		cmd_arg = SFIELD(cmd_arg, CMD52_REG_ADDR, regaddr);
+		cmd_arg = SFIELD(cmd_arg, CMD52_RW_FLAG, SDIOH_XFER_TYPE_READ);
+		cmd_arg = SFIELD(cmd_arg, CMD52_RAW, 0);
+		cmd_arg = SFIELD(cmd_arg, CMD52_DATA, 0);
+
+		if ((status = sdstd_cmd_issue(sd, USE_DMA(sd), SDIOH_CMD_52, cmd_arg))
+		    != SUCCESS)
+			return status;
+
+		sdstd_cmd_getrsp(sd, &rsp5, 1);
+		if (sdstd_rreg16(sd, SD_ErrorIntrStatus) != 0) {
+			sd_err(("%s: 1: ErrorintrStatus 0x%x\n",
+			        __FUNCTION__, sdstd_rreg16(sd, SD_ErrorIntrStatus)));
+		}
+
+		if (GFIELD(rsp5, RSP5_FLAGS) != 0x10)
+			sd_err(("%s: rsp5 flags is 0x%x\t %d\n",
+			        __FUNCTION__, GFIELD(rsp5, RSP5_FLAGS), func));
+
+		if (GFIELD(rsp5, RSP5_STUFF))
+			sd_err(("%s: rsp5 stuff is 0x%x: should be 0\n",
+			        __FUNCTION__, GFIELD(rsp5, RSP5_STUFF)));
+		*data = GFIELD(rsp5, RSP5_DATA);
+	} else {
+		cmd_arg = SFIELD(cmd_arg, CMD53_BYTE_BLK_CNT, regsize);
+		cmd_arg = SFIELD(cmd_arg, CMD53_OP_CODE, 1);
+		cmd_arg = SFIELD(cmd_arg, CMD53_BLK_MODE, 0);
+		cmd_arg = SFIELD(cmd_arg, CMD53_FUNCTION, func);
+		cmd_arg = SFIELD(cmd_arg, CMD53_REG_ADDR, regaddr);
+		cmd_arg = SFIELD(cmd_arg, CMD53_RW_FLAG, SDIOH_XFER_TYPE_READ);
+
+		sd->data_xfer_count = regsize;
+
+		/* sdstd_cmd_issue() returns with the command complete bit
+		 * in the ISR already cleared
+		 */
+		if ((status = sdstd_cmd_issue(sd, USE_DMA(sd), SDIOH_CMD_53, cmd_arg))
+		    != SUCCESS)
+			return status;
+
+		sdstd_cmd_getrsp(sd, &rsp5, 1);
+
+		if (GFIELD(rsp5, RSP5_FLAGS) != 0x10)
+			sd_err(("%s: rsp5 flags is 0x%x\t %d\n",
+			        __FUNCTION__, GFIELD(rsp5, RSP5_FLAGS), func));
+
+		if (GFIELD(rsp5, RSP5_STUFF))
+			sd_err(("%s: rsp5 stuff is 0x%x: should be 0\n",
+			        __FUNCTION__, GFIELD(rsp5, RSP5_STUFF)));
+
+		if (sd->polled_mode) {
+			volatile uint16 int_reg;
+			int retries = RETRIES_LARGE;
+
+			/* Wait for Read Buffer to become ready */
+			do {
+				int_reg = sdstd_rreg16(sd, SD_IntrStatus);
+			} while (--retries && (GFIELD(int_reg, INTSTAT_BUF_READ_READY) == 0));
+
+			if (!retries) {
+				sd_err(("%s: Timeout on Buf_Read_Ready: "
+				        "intStat: 0x%x errint: 0x%x PresentState 0x%x\n",
+				        __FUNCTION__, int_reg,
+				        sdstd_rreg16(sd, SD_ErrorIntrStatus),
+				        sdstd_rreg(sd, SD_PresentState)));
+				sdstd_check_errs(sd, SDIOH_CMD_53, cmd_arg);
+				return (ERROR);
+			}
+
+			/* Have Buffer Ready, so clear it and read the data */
+			sdstd_wreg16(sd, SD_IntrStatus, SFIELD(0, INTSTAT_BUF_READ_READY, 1));
+			if (regsize == 2)
+				*data = sdstd_rreg16(sd, SD_BufferDataPort0);
+			else
+				*data = sdstd_rreg(sd, SD_BufferDataPort0);
+
+			/* Check Status.
+			 * After the data is read, the Transfer Complete bit should be on
+			 */
+			retries = RETRIES_LARGE;
+			do {
+				int_reg = sdstd_rreg16(sd, SD_IntrStatus);
+			} while (--retries && (GFIELD(int_reg, INTSTAT_XFER_COMPLETE) == 0));
+
+			/* Check for any errors from the data phase */
+			if (sdstd_check_errs(sd, SDIOH_CMD_53, cmd_arg))
+				return ERROR;
+
+			if (!retries) {
+				sd_err(("%s: Timeout on xfer complete: "
+				        "intr 0x%04x err 0x%04x state 0x%08x\n",
+				        __FUNCTION__, int_reg,
+				        sdstd_rreg16(sd, SD_ErrorIntrStatus),
+				        sdstd_rreg(sd, SD_PresentState)));
+				return (ERROR);
+			}
+
+			sdstd_wreg16(sd, SD_IntrStatus, SFIELD(0, INTSTAT_XFER_COMPLETE, 1));
+		}
+	}
+	if (sd->polled_mode) {
+		if (regsize == 2)
+			*data &= 0xffff;
+	}
+	return SUCCESS;
+}
+
+bool
+check_client_intr(sdioh_info_t *sd)
+{
+	uint16 raw_int, cur_int, old_int;
+
+	raw_int = sdstd_rreg16(sd, SD_IntrStatus);
+	cur_int = raw_int & sd->intmask;
+
+	if (!cur_int) {
+		/* Not an error -- might share interrupts... */
+		return FALSE;
+	}
+
+	if (GFIELD(cur_int, INTSTAT_CARD_INT)) {
+		old_int = sdstd_rreg16(sd, SD_IntrStatusEnable);
+		sdstd_wreg16(sd, SD_IntrStatusEnable, SFIELD(old_int, INTSTAT_CARD_INT, 0));
+
+		if (sd->client_intr_enabled && sd->use_client_ints) {
+			sd->intrcount++;
+			ASSERT(sd->intr_handler);
+			ASSERT(sd->intr_handler_arg);
+			(sd->intr_handler)(sd->intr_handler_arg);
+		} else {
+			sd_err(("%s: Not ready for intr: enabled %d, handler %p\n",
+			        __FUNCTION__, sd->client_intr_enabled, sd->intr_handler));
+		}
+		sdstd_wreg16(sd, SD_IntrStatusEnable, old_int);
+	} else {
+		/* Local interrupt: disable, set flag, and save intrstatus */
+		sdstd_wreg16(sd, SD_IntrSignalEnable, 0);
+		sdstd_wreg16(sd, SD_ErrorIntrSignalEnable, 0);
+		sd->local_intrcount++;
+		sd->got_hcint = TRUE;
+		sd->last_intrstatus = cur_int;
+	}
+
+	return TRUE;
+}
+
+void
+sdstd_spinbits(sdioh_info_t *sd, uint16 norm, uint16 err)
+{
+	uint16 int_reg, err_reg;
+	int retries = RETRIES_LARGE;
+
+	do {
+		int_reg = sdstd_rreg16(sd, SD_IntrStatus);
+		err_reg = sdstd_rreg16(sd, SD_ErrorIntrStatus);
+	} while (--retries && !(int_reg & norm) && !(err_reg & err));
+
+	norm |= sd->intmask;
+	if (err_reg & err)
+		norm = SFIELD(norm, INTSTAT_ERROR_INT, 1);
+	sd->last_intrstatus = int_reg & norm;
+}
+
+/* write a client register */
+static int
+sdstd_card_regwrite(sdioh_info_t *sd, int func, uint32 regaddr, int regsize, uint32 data)
+{
+	int status;
+	uint32 cmd_arg, rsp5, flags;
+
+	cmd_arg = 0;
+
+	if ((func == 0) || (regsize == 1)) {
+		cmd_arg = SFIELD(cmd_arg, CMD52_FUNCTION, func);
+		cmd_arg = SFIELD(cmd_arg, CMD52_REG_ADDR, regaddr);
+		cmd_arg = SFIELD(cmd_arg, CMD52_RW_FLAG, SDIOH_XFER_TYPE_WRITE);
+		cmd_arg = SFIELD(cmd_arg, CMD52_RAW, 0);
+		cmd_arg = SFIELD(cmd_arg, CMD52_DATA, data & 0xff);
+		if ((status = sdstd_cmd_issue(sd, USE_DMA(sd), SDIOH_CMD_52, cmd_arg))
+		    != SUCCESS)
+			return status;
+
+		sdstd_cmd_getrsp(sd, &rsp5, 1);
+		flags = GFIELD(rsp5, RSP5_FLAGS);
+		if (flags && (flags != 0x10))
+			sd_err(("%s: rsp5.rsp5.flags = 0x%x, expecting 0x10\n",
+			        __FUNCTION__,  flags));
+	}
+	else {
+		cmd_arg = SFIELD(cmd_arg, CMD53_BYTE_BLK_CNT, regsize);
+		cmd_arg = SFIELD(cmd_arg, CMD53_OP_CODE, 1);
+		cmd_arg = SFIELD(cmd_arg, CMD53_BLK_MODE, 0);
+		cmd_arg = SFIELD(cmd_arg, CMD53_FUNCTION, func);
+		cmd_arg = SFIELD(cmd_arg, CMD53_REG_ADDR, regaddr);
+		cmd_arg = SFIELD(cmd_arg, CMD53_RW_FLAG, SDIOH_XFER_TYPE_WRITE);
+
+		sd->data_xfer_count = regsize;
+
+		/* sdstd_cmd_issue() returns with the command complete bit
+		 * in the ISR already cleared
+		 */
+		if ((status = sdstd_cmd_issue(sd, USE_DMA(sd), SDIOH_CMD_53, cmd_arg))
+		    != SUCCESS)
+			return status;
+
+		sdstd_cmd_getrsp(sd, &rsp5, 1);
+
+		if (GFIELD(rsp5, RSP5_FLAGS) != 0x10)
+			sd_err(("%s: rsp5 flags = 0x%x, expecting 0x10\n",
+			        __FUNCTION__,  GFIELD(rsp5, RSP5_FLAGS)));
+		if (GFIELD(rsp5, RSP5_STUFF))
+			sd_err(("%s: rsp5 stuff is 0x%x: expecting 0\n",
+			        __FUNCTION__, GFIELD(rsp5, RSP5_STUFF)));
+
+		if (sd->polled_mode) {
+			uint16 int_reg;
+			int retries = RETRIES_LARGE;
+
+			/* Wait for Write Buffer to become ready */
+			do {
+				int_reg = sdstd_rreg16(sd, SD_IntrStatus);
+			} while (--retries && (GFIELD(int_reg, INTSTAT_BUF_WRITE_READY) == 0));
+
+			if (!retries) {
+				sd_err(("%s: Timeout on Buf_Write_Ready: intStat: 0x%x "
+				        "errint: 0x%x PresentState 0x%x\n",
+				        __FUNCTION__, int_reg,
+				        sdstd_rreg16(sd, SD_ErrorIntrStatus),
+				        sdstd_rreg(sd, SD_PresentState)));
+				sdstd_check_errs(sd, SDIOH_CMD_53, cmd_arg);
+				return (ERROR);
+			}
+			/* Clear Write Buf Ready bit */
+			int_reg = 0;
+			int_reg = SFIELD(int_reg, INTSTAT_BUF_WRITE_READY, 1);
+			sdstd_wreg16(sd, SD_IntrStatus, int_reg);
+
+			/* At this point we have Buffer Ready, so write the data */
+			if (regsize == 2)
+				sdstd_wreg16(sd, SD_BufferDataPort0, (uint16) data);
+			else
+				sdstd_wreg(sd, SD_BufferDataPort0, data);
+
+			/* Wait for Transfer Complete */
+			retries = RETRIES_LARGE;
+			do {
+				int_reg = sdstd_rreg16(sd, SD_IntrStatus);
+			} while (--retries && (GFIELD(int_reg, INTSTAT_XFER_COMPLETE) == 0));
+
+			/* Check for any errors from the data phase */
+			if (sdstd_check_errs(sd, SDIOH_CMD_53, cmd_arg))
+				return ERROR;
+
+			if (retries == 0) {
+				sd_err(("%s: Timeout for xfer complete; State = 0x%x, "
+				        "intr state=0x%x, Errintstatus 0x%x rcnt %d, tcnt %d\n",
+				        __FUNCTION__, sdstd_rreg(sd, SD_PresentState),
+				        int_reg, sdstd_rreg16(sd, SD_ErrorIntrStatus),
+				        sd->r_cnt, sd->t_cnt));
+			}
+			/* Clear the status bits */
+			sdstd_wreg16(sd, SD_IntrStatus, SFIELD(int_reg, INTSTAT_CARD_INT, 0));
+		}
+	}
+	return SUCCESS;
+}
+
+void
+sdstd_cmd_getrsp(sdioh_info_t *sd, uint32 *rsp_buffer, int count /* num 32 bit words */)
+{
+	int rsp_count;
+	int respaddr = SD_Response0;
+
+	if (count > 4)
+		count = 4;
+
+	for (rsp_count = 0; rsp_count < count; rsp_count++) {
+		*rsp_buffer++ = sdstd_rreg(sd, respaddr);
+		respaddr += 4;
+	}
+}
+
+static int
+sdstd_cmd_issue(sdioh_info_t *sdioh_info, bool use_dma, uint32 cmd, uint32 arg)
+{
+	uint16 cmd_reg;
+	int retries;
+	uint32 cmd_arg;
+	uint16 xfer_reg = 0;
+
+
+	if ((sdioh_info->sd_mode == SDIOH_MODE_SPI) &&
+	    ((cmd == SDIOH_CMD_3) || (cmd == SDIOH_CMD_7) || (cmd == SDIOH_CMD_15))) {
+		sd_err(("%s: Cmd %d is not for SPI\n", __FUNCTION__, cmd));
+		return ERROR;
+	}
+
+	retries = RETRIES_SMALL;
+	while ((GFIELD(sdstd_rreg(sdioh_info, SD_PresentState), PRES_CMD_INHIBIT)) && --retries) {
+		if (retries == RETRIES_SMALL)
+			sd_err(("%s: Waiting for Command Inhibit cmd = %d 0x%x\n",
+			        __FUNCTION__, cmd, sdstd_rreg(sdioh_info, SD_PresentState)));
+	}
+	if (!retries) {
+		sd_err(("%s: Command Inhibit timeout\n", __FUNCTION__));
+		if (trap_errs)
+			ASSERT(0);
+		return ERROR;
+	}
+
+
+	cmd_reg = 0;
+	switch (cmd) {
+	case SDIOH_CMD_0:       /* Set Card to Idle State - No Response */
+		sd_data(("%s: CMD%d\n", __FUNCTION__, cmd));
+		cmd_reg = SFIELD(cmd_reg, CMD_RESP_TYPE, RESP_TYPE_NONE);
+		cmd_reg = SFIELD(cmd_reg, CMD_CRC_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_DATA_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_TYPE, CMD_TYPE_NORMAL);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX, cmd);
+		break;
+
+	case SDIOH_CMD_3:	/* Ask card to send RCA - Response R6 */
+		sd_data(("%s: CMD%d\n", __FUNCTION__, cmd));
+		cmd_reg = SFIELD(cmd_reg, CMD_RESP_TYPE, RESP_TYPE_48);
+		cmd_reg = SFIELD(cmd_reg, CMD_CRC_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_DATA_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_TYPE, CMD_TYPE_NORMAL);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX, cmd);
+		break;
+
+	case SDIOH_CMD_5:	/* Send Operation condition - Response R4 */
+		sd_data(("%s: CMD%d\n", __FUNCTION__, cmd));
+		cmd_reg = SFIELD(cmd_reg, CMD_RESP_TYPE, RESP_TYPE_48);
+		cmd_reg = SFIELD(cmd_reg, CMD_CRC_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_DATA_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_TYPE, CMD_TYPE_NORMAL);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX, cmd);
+		break;
+
+	case SDIOH_CMD_7:	/* Select card - Response R1 */
+		sd_data(("%s: CMD%d\n", __FUNCTION__, cmd));
+		cmd_reg = SFIELD(cmd_reg, CMD_RESP_TYPE, RESP_TYPE_48);
+		cmd_reg = SFIELD(cmd_reg, CMD_CRC_EN, 1);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX_EN, 1);
+		cmd_reg = SFIELD(cmd_reg, CMD_DATA_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_TYPE, CMD_TYPE_NORMAL);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX, cmd);
+		break;
+
+	case SDIOH_CMD_15:	/* Set card to inactive state - Response None */
+		sd_data(("%s: CMD%d\n", __FUNCTION__, cmd));
+		cmd_reg = SFIELD(cmd_reg, CMD_RESP_TYPE, RESP_TYPE_NONE);
+		cmd_reg = SFIELD(cmd_reg, CMD_CRC_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_DATA_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_TYPE, CMD_TYPE_NORMAL);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX, cmd);
+		break;
+
+	case SDIOH_CMD_52:	/* IO R/W Direct (single byte) - Response R5 */
+
+		sd_data(("%s: CMD52 func(%d) addr(0x%x) %s data(0x%x)\n",
+			__FUNCTION__,
+			GFIELD(arg, CMD52_FUNCTION),
+			GFIELD(arg, CMD52_REG_ADDR),
+			GFIELD(arg, CMD52_RW_FLAG) ? "W" : "R",
+			GFIELD(arg, CMD52_DATA)));
+
+		cmd_reg = SFIELD(cmd_reg, CMD_RESP_TYPE, RESP_TYPE_48);
+		cmd_reg = SFIELD(cmd_reg, CMD_CRC_EN, 1);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX_EN, 1);
+		cmd_reg = SFIELD(cmd_reg, CMD_DATA_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_TYPE, CMD_TYPE_NORMAL);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX, cmd);
+		break;
+
+	case SDIOH_CMD_53:	/* IO R/W Extended (multiple bytes/blocks) */
+
+		sd_data(("%s: CMD53 func(%d) addr(0x%x) %s mode(%s) cnt(%d), %s\n",
+			__FUNCTION__,
+			GFIELD(arg, CMD53_FUNCTION),
+			GFIELD(arg, CMD53_REG_ADDR),
+			GFIELD(arg, CMD53_RW_FLAG) ? "W" : "R",
+			GFIELD(arg, CMD53_BLK_MODE) ? "Block" : "Byte",
+			GFIELD(arg, CMD53_BYTE_BLK_CNT),
+			GFIELD(arg, CMD53_OP_CODE) ? "Incrementing addr" : "Single addr"));
+
+		cmd_arg = arg;
+		xfer_reg = 0;
+
+		cmd_reg = SFIELD(cmd_reg, CMD_RESP_TYPE, RESP_TYPE_48);
+		cmd_reg = SFIELD(cmd_reg, CMD_CRC_EN, 1);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX_EN, 1);
+		cmd_reg = SFIELD(cmd_reg, CMD_DATA_EN, 1);
+		cmd_reg = SFIELD(cmd_reg, CMD_TYPE, CMD_TYPE_NORMAL);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX, cmd);
+
+		use_dma = USE_DMA(sdioh_info) && GFIELD(cmd_arg, CMD53_BLK_MODE);
+
+		if (GFIELD(cmd_arg, CMD53_BLK_MODE)) {
+			uint16 blocksize;
+			uint16 blockcount;
+			int func;
+
+			ASSERT(sdioh_info->sd_blockmode);
+
+			func = GFIELD(cmd_arg, CMD53_FUNCTION);
+			blocksize = MIN((int)sdioh_info->data_xfer_count,
+			                sdioh_info->client_block_size[func]);
+			blockcount = GFIELD(cmd_arg, CMD53_BYTE_BLK_CNT);
+
+			/* data_xfer_cnt is already setup so that for multiblock mode,
+			 * it is the entire buffer length.  For non-block or single block,
+			 * it is < 64 bytes
+			 */
+			if (use_dma) {
+				switch (sdioh_info->sd_dma_mode) {
+				case DMA_MODE_SDMA:
+					sd_dma(("%s: SDMA: SysAddr reg was 0x%x now 0x%x\n",
+					      __FUNCTION__, sdstd_rreg(sdioh_info, SD_SysAddr),
+					     (uint32)sdioh_info->dma_phys));
+				sdstd_wreg(sdioh_info, SD_SysAddr, sdioh_info->dma_phys);
+					break;
+				case DMA_MODE_ADMA1:
+				case DMA_MODE_ADMA2:
+					sd_dma(("%s: ADMA: Using ADMA\n", __FUNCTION__));
+						sd_create_adma_descriptor(sdioh_info, 0,
+						sdioh_info->dma_phys, blockcount*blocksize,
+						ADMA2_ATTRIBUTE_VALID | ADMA2_ATTRIBUTE_END |
+						ADMA2_ATTRIBUTE_INT | ADMA2_ATTRIBUTE_ACT_TRAN);
+					/* Dump descriptor if DMA debugging is enabled. */
+					if (sd_msglevel & SDH_DMA_VAL) {
+						sd_dump_adma_dscr(sdioh_info);
+					}
+
+					sdstd_wreg(sdioh_info, SD_ADMA_SysAddr,
+					           sdioh_info->adma2_dscr_phys);
+					break;
+				default:
+					sd_err(("%s: unsupported DMA mode %d.\n",
+						__FUNCTION__, sdioh_info->sd_dma_mode));
+					break;
+				}
+			}
+
+			sd_trace(("%s: Setting block count %d, block size %d bytes\n",
+			          __FUNCTION__, blockcount, blocksize));
+			sdstd_wreg16(sdioh_info, SD_BlockSize, blocksize);
+			sdstd_wreg16(sdioh_info, SD_BlockCount, blockcount);
+
+			xfer_reg = SFIELD(xfer_reg, XFER_DMA_ENABLE, use_dma);
+
+			if (sdioh_info->client_block_size[func] != blocksize)
+				set_client_block_size(sdioh_info, 1, blocksize);
+
+			if (blockcount > 1) {
+				xfer_reg = SFIELD(xfer_reg, XFER_MULTI_BLOCK, 1);
+				xfer_reg = SFIELD(xfer_reg, XFER_BLK_COUNT_EN, 1);
+				xfer_reg = SFIELD(xfer_reg, XFER_CMD_12_EN, 0);
+			} else {
+				xfer_reg = SFIELD(xfer_reg, XFER_MULTI_BLOCK, 0);
+				xfer_reg = SFIELD(xfer_reg, XFER_BLK_COUNT_EN, 0);
+				xfer_reg = SFIELD(xfer_reg, XFER_CMD_12_EN, 0);
+			}
+
+			if (GFIELD(cmd_arg, CMD53_RW_FLAG) == SDIOH_XFER_TYPE_READ)
+				xfer_reg = SFIELD(xfer_reg, XFER_DATA_DIRECTION, 1);
+			else
+				xfer_reg = SFIELD(xfer_reg, XFER_DATA_DIRECTION, 0);
+
+			retries = RETRIES_SMALL;
+			while (GFIELD(sdstd_rreg(sdioh_info, SD_PresentState),
+			              PRES_DAT_INHIBIT) && --retries)
+				sd_err(("%s: Waiting for Data Inhibit cmd = %d\n",
+				        __FUNCTION__, cmd));
+			if (!retries) {
+				sd_err(("%s: Data Inhibit timeout\n", __FUNCTION__));
+				if (trap_errs)
+					ASSERT(0);
+				return ERROR;
+			}
+			sdstd_wreg16(sdioh_info, SD_TransferMode, xfer_reg);
+
+		} else {	/* Non block mode */
+			uint16 bytes = GFIELD(cmd_arg, CMD53_BYTE_BLK_CNT);
+			/* The byte/block count field only has 9 bits,
+			 * so, to do a 512-byte bytemode transfer, this
+			 * field will contain 0, but we need to tell the
+			 * controller we're transferring 512 bytes.
+			 */
+			if (bytes == 0) bytes = 512;
+
+			if (use_dma)
+				sdstd_wreg(sdioh_info, SD_SysAddr, sdioh_info->dma_phys);
+
+			/* PCI: Transfer Mode register 0x0c */
+			xfer_reg = SFIELD(xfer_reg, XFER_DMA_ENABLE, bytes <= 4 ? 0 : use_dma);
+			xfer_reg = SFIELD(xfer_reg, XFER_CMD_12_EN, 0);
+			if (GFIELD(cmd_arg, CMD53_RW_FLAG) == SDIOH_XFER_TYPE_READ)
+				xfer_reg = SFIELD(xfer_reg, XFER_DATA_DIRECTION, 1);
+			else
+				xfer_reg = SFIELD(xfer_reg, XFER_DATA_DIRECTION, 0);
+			/* See table 2-8 Host Controller spec ver 1.00 */
+			xfer_reg = SFIELD(xfer_reg, XFER_BLK_COUNT_EN, 0); /* Dont care */
+			xfer_reg = SFIELD(xfer_reg, XFER_MULTI_BLOCK, 0);
+
+			sdstd_wreg16(sdioh_info, SD_BlockSize,  bytes);
+
+			sdstd_wreg16(sdioh_info, SD_BlockCount, 1);
+
+			retries = RETRIES_SMALL;
+			while (GFIELD(sdstd_rreg(sdioh_info, SD_PresentState),
+			              PRES_DAT_INHIBIT) && --retries)
+				sd_err(("%s: Waiting for Data Inhibit cmd = %d\n",
+				        __FUNCTION__, cmd));
+			if (!retries) {
+				sd_err(("%s: Data Inhibit timeout\n", __FUNCTION__));
+				if (trap_errs)
+					ASSERT(0);
+				return ERROR;
+			}
+			sdstd_wreg16(sdioh_info, SD_TransferMode, xfer_reg);
+		}
+		break;
+
+	default:
+		sd_err(("%s: Unknown command\n", __FUNCTION__));
+		return ERROR;
+	}
+
+	if (sdioh_info->sd_mode == SDIOH_MODE_SPI) {
+		cmd_reg = SFIELD(cmd_reg, CMD_CRC_EN, 0);
+		cmd_reg = SFIELD(cmd_reg, CMD_INDEX_EN, 0);
+	}
+
+	/* Setup and issue the SDIO command */
+	sdstd_wreg(sdioh_info, SD_Arg0, arg);
+	sdstd_wreg16(sdioh_info, SD_Command, cmd_reg);
+
+	/* If we are in polled mode, wait for the command to complete.
+	 * In interrupt mode, return immediately. The calling function will
+	 * know that the command has completed when the CMDATDONE interrupt
+	 * is asserted
+	 */
+	if (sdioh_info->polled_mode) {
+		uint16 int_reg = 0;
+		int retries = RETRIES_LARGE;
+
+		do {
+			int_reg = sdstd_rreg16(sdioh_info, SD_IntrStatus);
+		} while (--retries &&
+		         (GFIELD(int_reg, INTSTAT_ERROR_INT) == 0) &&
+		         (GFIELD(int_reg, INTSTAT_CMD_COMPLETE) == 0));
+
+		if (!retries) {
+			sd_err(("%s: CMD_COMPLETE timeout: intrStatus: 0x%x "
+			        "error stat 0x%x state 0x%x\n",
+			        __FUNCTION__, int_reg,
+			        sdstd_rreg16(sdioh_info, SD_ErrorIntrStatus),
+			        sdstd_rreg(sdioh_info, SD_PresentState)));
+
+			/* Attempt to reset CMD line when we get a CMD timeout */
+			sdstd_wreg8(sdioh_info, SD_SoftwareReset, SFIELD(0, SW_RESET_CMD, 1));
+			retries = RETRIES_LARGE;
+			do {
+				sd_trace(("%s: waiting for CMD line reset\n", __FUNCTION__));
+			} while ((GFIELD(sdstd_rreg8(sdioh_info, SD_SoftwareReset),
+			                 SW_RESET_CMD)) && retries--);
+
+			if (!retries) {
+				sd_err(("%s: Timeout waiting for CMD line reset\n", __FUNCTION__));
+			}
+
+			if (trap_errs)
+				ASSERT(0);
+			return (ERROR);
+		}
+
+		/* Clear Command Complete interrupt */
+		int_reg = SFIELD(0, INTSTAT_CMD_COMPLETE, 1);
+		sdstd_wreg16(sdioh_info, SD_IntrStatus, int_reg);
+
+		/* Check for Errors */
+		if (sdstd_check_errs(sdioh_info, cmd, arg)) {
+			if (trap_errs)
+				ASSERT(0);
+			return ERROR;
+		}
+	}
+	return SUCCESS;
+}
+
+
+static int
+sdstd_card_buf(sdioh_info_t *sd, int rw, int func, bool fifo, uint32 addr, int nbytes, uint32 *data)
+{
+	int status;
+	uint32 cmd_arg;
+	uint32 rsp5;
+	uint16 int_reg, int_bit;
+	uint flags;
+	int num_blocks, blocksize;
+	bool local_blockmode, local_dma;
+	bool read = rw == SDIOH_READ ? 1 : 0;
+	bool yield = FALSE;
+
+	ASSERT(nbytes);
+
+	cmd_arg = 0;
+
+	sd_data(("%s: %s 53 addr 0x%x, len %d bytes, r_cnt %d t_cnt %d\n",
+	         __FUNCTION__, read ? "Rd" : "Wr", addr, nbytes, sd->r_cnt, sd->t_cnt));
+
+	if (read) sd->r_cnt++; else sd->t_cnt++;
+
+	local_blockmode = sd->sd_blockmode;
+	local_dma = USE_DMA(sd);
+
+	/* Don't bother with block mode on small xfers */
+	if (nbytes < sd->client_block_size[func]) {
+		sd_data(("setting local blockmode to false: nbytes (%d) != block_size (%d)\n",
+		         nbytes, sd->client_block_size[func]));
+		local_blockmode = FALSE;
+		local_dma = FALSE;
+	}
+
+	if (local_blockmode) {
+		blocksize = MIN(sd->client_block_size[func], nbytes);
+		num_blocks = nbytes/blocksize;
+		cmd_arg = SFIELD(cmd_arg, CMD53_BYTE_BLK_CNT, num_blocks);
+		cmd_arg = SFIELD(cmd_arg, CMD53_BLK_MODE, 1);
+	} else {
+		num_blocks =  1;
+		blocksize = nbytes;
+		cmd_arg = SFIELD(cmd_arg, CMD53_BYTE_BLK_CNT, nbytes);
+		cmd_arg = SFIELD(cmd_arg, CMD53_BLK_MODE, 0);
+	}
+
+	if (local_dma && !read) {
+		bcopy(data, sd->dma_buf, nbytes);
+		sd_sync_dma(sd, read, nbytes);
+	}
+
+	if (fifo)
+		cmd_arg = SFIELD(cmd_arg, CMD53_OP_CODE, 0);
+	else
+		cmd_arg = SFIELD(cmd_arg, CMD53_OP_CODE, 1);
+
+	cmd_arg = SFIELD(cmd_arg, CMD53_FUNCTION, func);
+	cmd_arg = SFIELD(cmd_arg, CMD53_REG_ADDR, addr);
+	if (read)
+		cmd_arg = SFIELD(cmd_arg, CMD53_RW_FLAG, SDIOH_XFER_TYPE_READ);
+	else
+		cmd_arg = SFIELD(cmd_arg, CMD53_RW_FLAG, SDIOH_XFER_TYPE_WRITE);
+
+	sd->data_xfer_count = nbytes;
+
+	/* sdstd_cmd_issue() returns with the command complete bit
+	 * in the ISR already cleared
+	 */
+	if ((status = sdstd_cmd_issue(sd, local_dma, SDIOH_CMD_53, cmd_arg)) != SUCCESS) {
+		sd_err(("%s: cmd_issue failed for %s\n", __FUNCTION__, (read ? "read" : "write")));
+		return status;
+	}
+
+	sdstd_cmd_getrsp(sd, &rsp5, 1);
+
+	if ((flags = GFIELD(rsp5, RSP5_FLAGS)) != 0x10) {
+		sd_err(("%s: Rsp5: nbytes %d, dma %d blockmode %d, read %d "
+		        "numblocks %d, blocksize %d\n",
+		        __FUNCTION__, nbytes, local_dma, local_dma, read, num_blocks, blocksize));
+
+		if (flags & 1)
+			sd_err(("%s: rsp5: Command not accepted: arg out of range 0x%x, "
+			        "bytes %d dma %d\n",
+			        __FUNCTION__, flags, GFIELD(cmd_arg, CMD53_BYTE_BLK_CNT),
+			        GFIELD(cmd_arg, CMD53_BLK_MODE)));
+		if (flags & 0x8)
+			sd_err(("%s: Rsp5: General Error\n", __FUNCTION__));
+
+		sd_err(("%s: rsp5 flags = 0x%x, expecting 0x10 returning error\n",
+		        __FUNCTION__,  flags));
+		if (trap_errs)
+			ASSERT(0);
+		return ERROR;
+	}
+
+	if (GFIELD(rsp5, RSP5_STUFF))
+		sd_err(("%s: rsp5 stuff is 0x%x: expecting 0\n",
+		        __FUNCTION__, GFIELD(rsp5, RSP5_STUFF)));
+
+#ifdef BCMSDYIELD
+	yield = sd_yieldcpu && ((uint)nbytes >= sd_minyield);
+#endif
+
+	if (!local_dma) {
+		int bytes, i;
+		uint32 tmp;
+		for (i = 0; i < num_blocks; i++) {
+			int words;
+
+			/* Decide which status bit we're waiting for */
+			if (read)
+				int_bit = SFIELD(0, INTSTAT_BUF_READ_READY, 1);
+			else
+				int_bit = SFIELD(0, INTSTAT_BUF_WRITE_READY, 1);
+
+			/* If not on, wait for it (or for xfer error) */
+			int_reg = sdstd_rreg16(sd, SD_IntrStatus);
+			if (!(int_reg & int_bit))
+				int_reg = sdstd_waitbits(sd, int_bit, ERRINT_TRANSFER_ERRS, yield);
+
+			/* Confirm we got the bit w/o error */
+			if (!(int_reg & int_bit) || GFIELD(int_reg, INTSTAT_ERROR_INT)) {
+				sd_err(("%s: Error or timeout for Buf_%s_Ready: intStat: 0x%x "
+				        "errint: 0x%x PresentState 0x%x\n",
+				        __FUNCTION__, read ? "Read" : "Write", int_reg,
+				        sdstd_rreg16(sd, SD_ErrorIntrStatus),
+				        sdstd_rreg(sd, SD_PresentState)));
+				sdstd_dumpregs(sd);
+				sdstd_check_errs(sd, SDIOH_CMD_53, cmd_arg);
+				return (ERROR);
+			}
+
+			/* Clear Buf Ready bit */
+			sdstd_wreg16(sd, SD_IntrStatus, int_bit);
+
+			/* At this point we have Buffer Ready, write the data 4 bytes at a time */
+			for (words = blocksize/4; words; words--) {
+				if (read)
+					*data = sdstd_rreg(sd, SD_BufferDataPort0);
+				else
+					sdstd_wreg(sd, SD_BufferDataPort0, *data);
+				data++;
+			}
+
+			bytes = blocksize % 4;
+
+			/* If no leftover bytes, go to next block */
+			if (!bytes)
+				continue;
+
+			switch (bytes) {
+			case 1:
+				/* R/W 8 bits */
+				if (read)
+					*(data++) = (uint32)(sdstd_rreg8(sd, SD_BufferDataPort0));
+				else
+					sdstd_wreg8(sd, SD_BufferDataPort0,
+					            (uint8)(*(data++) & 0xff));
+				break;
+			case 2:
+				/* R/W 16 bits */
+				if (read)
+					*(data++) = (uint32)sdstd_rreg16(sd, SD_BufferDataPort0);
+				else
+					sdstd_wreg16(sd, SD_BufferDataPort0, (uint16)(*(data++)));
+				break;
+			case 3:
+				/* R/W 24 bits:
+				 * SD_BufferDataPort0[0-15] | SD_BufferDataPort1[16-23]
+				 */
+				if (read) {
+					tmp = (uint32)sdstd_rreg16(sd, SD_BufferDataPort0);
+					tmp |= ((uint32)(sdstd_rreg8(sd,
+					                             SD_BufferDataPort1)) << 16);
+					*(data++) = tmp;
+				} else {
+					tmp = *(data++);
+					sdstd_wreg16(sd, SD_BufferDataPort0, (uint16)tmp & 0xffff);
+					sdstd_wreg8(sd, SD_BufferDataPort1,
+					            (uint8)((tmp >> 16) & 0xff));
+				}
+				break;
+			default:
+				sd_err(("%s: Unexpected bytes leftover %d\n",
+				        __FUNCTION__, bytes));
+				ASSERT(0);
+				break;
+			}
+		}
+	}	/* End PIO processing */
+
+	/* Wait for Transfer Complete or Transfer Error */
+	int_bit = SFIELD(0, INTSTAT_XFER_COMPLETE, 1);
+
+	/* If not on, wait for it (or for xfer error) */
+	int_reg = sdstd_rreg16(sd, SD_IntrStatus);
+	if (!(int_reg & int_bit))
+		int_reg = sdstd_waitbits(sd, int_bit, ERRINT_TRANSFER_ERRS, yield);
+
+	/* Check for any errors from the data phase */
+	if (sdstd_check_errs(sd, SDIOH_CMD_53, cmd_arg))
+		return ERROR;
+
+	/* May have gotten a software timeout if not blocking? */
+	int_reg = sdstd_rreg16(sd, SD_IntrStatus);
+	if (!(int_reg & int_bit)) {
+		sd_err(("%s: Error or Timeout for xfer complete; %s, dma %d, State 0x%08x, "
+		        "intr 0x%04x, Err 0x%04x, len = %d, rcnt %d, tcnt %d\n",
+		        __FUNCTION__, read ? "R" : "W", local_dma,
+		        sdstd_rreg(sd, SD_PresentState), int_reg,
+		        sdstd_rreg16(sd, SD_ErrorIntrStatus), nbytes,
+		        sd->r_cnt, sd->t_cnt));
+		sdstd_dumpregs(sd);
+		return ERROR;
+	}
+
+	/* Clear the status bits */
+	int_reg = int_bit;
+	if (local_dma) {
+		/* DMA Complete */
+		/* Reads in particular don't have DMA_COMPLETE set */
+		int_reg = SFIELD(int_reg, INTSTAT_DMA_INT, 1);
+	}
+	sdstd_wreg16(sd, SD_IntrStatus, int_reg);
+
+	/* Fetch data */
+	if (local_dma && read) {
+		sd_sync_dma(sd, read, nbytes);
+		bcopy(sd->dma_buf, data, nbytes);
+	}
+	return SUCCESS;
+}
+
+static int
+set_client_block_size(sdioh_info_t *sd, int func, int block_size)
+{
+	int base;
+	int err = 0;
+
+
+	sd_err(("%s: Setting block size %d, func %d\n", __FUNCTION__, block_size, func));
+	sd->client_block_size[func] = block_size;
+
+	/* Set the block size in the SDIO Card register */
+	base = func * SDIOD_FBR_SIZE;
+	err = sdstd_card_regwrite(sd, 0, base+SDIOD_CCCR_BLKSIZE_0, 1, block_size & 0xff);
+	if (!err) {
+		err = sdstd_card_regwrite(sd, 0, base+SDIOD_CCCR_BLKSIZE_1, 1,
+		                          (block_size >> 8) & 0xff);
+	}
+
+	/* Do not set the block size in the SDIO Host register, that
+	 * is func dependent and will get done on an individual
+	 * transaction basis
+	 */
+
+	return (err ? BCME_SDIO_ERROR : 0);
+}
+
+/* Reset and re-initialize the device */
+int
+sdioh_sdio_reset(sdioh_info_t *si)
+{
+	uint8 hreg;
+
+	/* Reset the attached device (use slower clock for safety) */
+	sdstd_start_clock(si, 128);
+	sdstd_reset(si, 0, 1);
+
+	/* Reset portions of the host state accordingly */
+	hreg = sdstd_rreg8(si, SD_HostCntrl);
+	hreg = SFIELD(hreg, HOST_HI_SPEED_EN, 0);
+	hreg = SFIELD(hreg, HOST_DATA_WIDTH, 0);
+	si->sd_mode = SDIOH_MODE_SD1;
+
+	/* Reinitialize the card */
+	si->card_init_done = FALSE;
+	return sdstd_client_init(si);
+}
+
+
+static void
+sd_map_dma(sdioh_info_t * sd)
+{
+
+	void *va;
+
+	if ((va = DMA_ALLOC_CONSISTENT(sd->osh, SD_PAGE,
+		&sd->dma_start_phys, 0x12, 12)) == NULL) {
+		sd->sd_dma_mode = DMA_MODE_NONE;
+		sd->dma_start_buf = 0;
+		sd->dma_buf = (void *)0;
+		sd->dma_phys = 0;
+		sd->alloced_dma_size = SD_PAGE;
+		sd_err(("%s: DMA_ALLOC failed. Disabling DMA support.\n", __FUNCTION__));
+	} else {
+		sd->dma_start_buf = va;
+		sd->dma_buf = (void *)ROUNDUP((uintptr)va, SD_PAGE);
+		sd->dma_phys = ROUNDUP((sd->dma_start_phys), SD_PAGE);
+		sd->alloced_dma_size = SD_PAGE;
+		sd_err(("%s: Mapped DMA Buffer %dbytes @virt/phys: %p/0x%lx\n",
+		        __FUNCTION__, sd->alloced_dma_size, sd->dma_buf, sd->dma_phys));
+		sd_fill_dma_data_buf(sd, 0xA5);
+	}
+
+	if ((va = DMA_ALLOC_CONSISTENT(sd->osh, SD_PAGE,
+		&sd->adma2_dscr_start_phys, 0x12, 12)) == NULL) {
+		sd->sd_dma_mode = DMA_MODE_NONE;
+		sd->adma2_dscr_start_buf = 0;
+		sd->adma2_dscr_buf = (void *)0;
+		sd->adma2_dscr_phys = 0;
+		sd->alloced_adma2_dscr_size = 0;
+		sd_err(("%s: DMA_ALLOC failed for descriptor buffer. "
+		        "Disabling DMA support.\n", __FUNCTION__));
+	} else {
+		sd->adma2_dscr_start_buf = va;
+		sd->adma2_dscr_buf = (void *)ROUNDUP((uintptr)va, SD_PAGE);
+		sd->adma2_dscr_phys = ROUNDUP((sd->adma2_dscr_start_phys), SD_PAGE);
+		sd->alloced_adma2_dscr_size = SD_PAGE;
+	}
+
+	sd_err(("%s: Mapped ADMA2 Descriptor Buffer %dbytes @virt/phys: %p/0x%lx\n",
+	        __FUNCTION__, sd->alloced_adma2_dscr_size, sd->adma2_dscr_buf,
+	        sd->adma2_dscr_phys));
+	sd_clear_adma_dscr_buf(sd);
+}
+
+static void
+sd_unmap_dma(sdioh_info_t * sd)
+{
+	if (sd->dma_start_buf) {
+		DMA_FREE_CONSISTENT(sd->osh, sd->dma_start_buf, sd->alloced_dma_size,
+			sd->dma_start_phys, 0x12);
+	}
+
+	if (sd->adma2_dscr_start_buf) {
+		DMA_FREE_CONSISTENT(sd->osh, sd->adma2_dscr_start_buf, sd->alloced_adma2_dscr_size,
+		                    sd->adma2_dscr_start_phys, 0x12);
+	}
+}
+
+static void sd_clear_adma_dscr_buf(sdioh_info_t *sd)
+{
+	bzero((char *)sd->adma2_dscr_buf, SD_PAGE);
+	sd_dump_adma_dscr(sd);
+}
+
+static void sd_fill_dma_data_buf(sdioh_info_t *sd, uint8 data)
+{
+	memset((char *)sd->dma_buf, data, SD_PAGE);
+}
+
+
+static void sd_create_adma_descriptor(sdioh_info_t *sd, uint32 index,
+                                      uint32 addr_phys, uint16 length, uint16 flags)
+{
+	adma2_dscr_32b_t *adma2_dscr_table;
+	adma1_dscr_t *adma1_dscr_table;
+
+	adma2_dscr_table = sd->adma2_dscr_buf;
+	adma1_dscr_table = sd->adma2_dscr_buf;
+
+	switch (sd->sd_dma_mode) {
+		case DMA_MODE_ADMA2:
+			sd_dma(("%s: creating ADMA2 descriptor for index %d\n",
+				__FUNCTION__, index));
+
+			adma2_dscr_table[index].phys_addr = addr_phys;
+			adma2_dscr_table[index].len_attr = length << 16;
+			adma2_dscr_table[index].len_attr |= flags;
+			break;
+		case DMA_MODE_ADMA1:
+			/* ADMA1 requires two descriptors, one for len
+			 * and the other for data transfer
+			 */
+			index <<= 1;
+
+			sd_dma(("%s: creating ADMA1 descriptor for index %d\n",
+				__FUNCTION__, index));
+
+			adma1_dscr_table[index].phys_addr_attr = length << 12;
+			adma1_dscr_table[index].phys_addr_attr |= (ADMA1_ATTRIBUTE_ACT_SET |
+			                                           ADMA2_ATTRIBUTE_VALID);
+			adma1_dscr_table[index+1].phys_addr_attr = addr_phys & 0xFFFFF000;
+			adma1_dscr_table[index+1].phys_addr_attr |= (flags & 0x3f);
+			break;
+		default:
+			sd_err(("%s: cannot create ADMA descriptor for DMA mode %d\n",
+				__FUNCTION__, sd->sd_dma_mode));
+			break;
+	}
+}
+
+
+static void sd_dump_adma_dscr(sdioh_info_t *sd)
+{
+	adma2_dscr_32b_t *adma2_dscr_table;
+	adma1_dscr_t *adma1_dscr_table;
+	uint32 i = 0;
+	uint16 flags;
+	char flags_str[32];
+
+	ASSERT(sd->adma2_dscr_buf != NULL);
+
+	adma2_dscr_table = sd->adma2_dscr_buf;
+	adma1_dscr_table = sd->adma2_dscr_buf;
+
+	switch (sd->sd_dma_mode) {
+		case DMA_MODE_ADMA2:
+			sd_err(("ADMA2 Descriptor Table (%dbytes) @virt/phys: %p/0x%lx\n",
+				SD_PAGE, sd->adma2_dscr_buf, sd->adma2_dscr_phys));
+			sd_err((" #[Descr VA  ]  Buffer PA  | Len    | Flags  (5:4  2   1   0)"
+			        "     |\n"));
+			while (adma2_dscr_table->len_attr & ADMA2_ATTRIBUTE_VALID) {
+				flags = adma2_dscr_table->len_attr & 0xFFFF;
+				sprintf(flags_str, "%s%s%s%s",
+					((flags & ADMA2_ATTRIBUTE_ACT_LINK) ==
+					ADMA2_ATTRIBUTE_ACT_LINK) ? "LINK " :
+					((flags & ADMA2_ATTRIBUTE_ACT_LINK) ==
+					ADMA2_ATTRIBUTE_ACT_TRAN) ? "TRAN " :
+					((flags & ADMA2_ATTRIBUTE_ACT_LINK) ==
+					ADMA2_ATTRIBUTE_ACT_NOP) ? "NOP  " : "RSV  ",
+					(flags & ADMA2_ATTRIBUTE_INT ? "INT " : "    "),
+					(flags & ADMA2_ATTRIBUTE_END ? "END " : "    "),
+					(flags & ADMA2_ATTRIBUTE_VALID ? "VALID" : ""));
+				sd_err(("%2d[0x%p]: 0x%08x | 0x%04x | 0x%04x (%s) |\n",
+				        i, adma2_dscr_table, adma2_dscr_table->phys_addr,
+				        adma2_dscr_table->len_attr >> 16, flags, flags_str));
+				i++;
+
+				/* Follow LINK descriptors or skip to next. */
+				if ((flags & ADMA2_ATTRIBUTE_ACT_LINK) ==
+				     ADMA2_ATTRIBUTE_ACT_LINK) {
+					adma2_dscr_table = phys_to_virt(
+					    adma2_dscr_table->phys_addr);
+				} else {
+					adma2_dscr_table++;
+				}
+
+			}
+			break;
+		case DMA_MODE_ADMA1:
+			sd_err(("ADMA1 Descriptor Table (%dbytes) @virt/phys: %p/0x%lx\n",
+			         SD_PAGE, sd->adma2_dscr_buf, sd->adma2_dscr_phys));
+			sd_err((" #[Descr VA  ]  Buffer PA  | Flags  (5:4  2   1   0)     |\n"));
+
+			for (i = 0; adma1_dscr_table->phys_addr_attr & ADMA2_ATTRIBUTE_VALID; i++) {
+				flags = adma1_dscr_table->phys_addr_attr & 0x3F;
+				sprintf(flags_str, "%s%s%s%s",
+					((flags & ADMA2_ATTRIBUTE_ACT_LINK) ==
+					ADMA2_ATTRIBUTE_ACT_LINK) ? "LINK " :
+					((flags & ADMA2_ATTRIBUTE_ACT_LINK) ==
+					ADMA2_ATTRIBUTE_ACT_TRAN) ? "TRAN " :
+					((flags & ADMA2_ATTRIBUTE_ACT_LINK) ==
+					ADMA2_ATTRIBUTE_ACT_NOP) ? "NOP  " : "SET  ",
+					(flags & ADMA2_ATTRIBUTE_INT ? "INT " : "    "),
+					(flags & ADMA2_ATTRIBUTE_END ? "END " : "    "),
+					(flags & ADMA2_ATTRIBUTE_VALID ? "VALID" : ""));
+				sd_err(("%2d[0x%p]: 0x%08x | 0x%04x | (%s) |\n",
+				        i, adma1_dscr_table,
+				        adma1_dscr_table->phys_addr_attr & 0xFFFFF000,
+				        flags, flags_str));
+
+				/* Follow LINK descriptors or skip to next. */
+				if ((flags & ADMA2_ATTRIBUTE_ACT_LINK) ==
+				     ADMA2_ATTRIBUTE_ACT_LINK) {
+					adma1_dscr_table = phys_to_virt(
+						adma1_dscr_table->phys_addr_attr & 0xFFFFF000);
+				} else {
+					adma1_dscr_table++;
+				}
+			}
+			break;
+		default:
+			sd_err(("Unknown DMA Descriptor Table Format.\n"));
+			break;
+	}
+}
+
+static void sdstd_dumpregs(sdioh_info_t *sd)
+{
+	sd_err(("IntrStatus:       0x%04x ErrorIntrStatus       0x%04x\n",
+	            sdstd_rreg16(sd, SD_IntrStatus),
+	            sdstd_rreg16(sd, SD_ErrorIntrStatus)));
+	sd_err(("IntrStatusEnable: 0x%04x ErrorIntrStatusEnable 0x%04x\n",
+	            sdstd_rreg16(sd, SD_IntrStatusEnable),
+	            sdstd_rreg16(sd, SD_ErrorIntrStatusEnable)));
+	sd_err(("IntrSignalEnable: 0x%04x ErrorIntrSignalEnable 0x%04x\n",
+	            sdstd_rreg16(sd, SD_IntrSignalEnable),
+	            sdstd_rreg16(sd, SD_ErrorIntrSignalEnable)));
+}
diff --git a/drivers/net/wireless/bcm4329/bcmsdstd_linux.c b/drivers/net/wireless/bcm4329/bcmsdstd_linux.c
new file mode 100644
index 000000000000..a8b98e2054a0
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/bcmsdstd_linux.c
@@ -0,0 +1,251 @@
+/*
+ *  'Standard' SDIO HOST CONTROLLER driver - linux portion
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdstd_linux.c,v 1.11.18.2.16.1 2010/08/17 17:03:13 Exp $
+ */
+
+#include <typedefs.h>
+#include <pcicfg.h>
+#include <bcmutils.h>
+#include <sdio.h>	/* SDIO Specs */
+#include <bcmsdbus.h>	/* bcmsdh to/from specific controller APIs */
+#include <sdiovar.h>	/* to get msglevel bit values */
+
+#include <linux/sched.h>	/* request_irq() */
+
+#include <bcmsdstd.h>
+
+struct sdos_info {
+	sdioh_info_t *sd;
+	spinlock_t lock;
+	wait_queue_head_t intr_wait_queue;
+};
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+#define BLOCKABLE()	(!in_atomic())
+#else
+#define BLOCKABLE()	(!in_interrupt())
+#endif
+
+/* Interrupt handler */
+static irqreturn_t
+sdstd_isr(int irq, void *dev_id
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 20)
+, struct pt_regs *ptregs
+#endif
+)
+{
+	sdioh_info_t *sd;
+	struct sdos_info *sdos;
+	bool ours;
+
+	sd = (sdioh_info_t *)dev_id;
+
+	if (!sd->card_init_done) {
+		sd_err(("%s: Hey Bogus intr...not even initted: irq %d\n", __FUNCTION__, irq));
+		return IRQ_RETVAL(FALSE);
+	} else {
+		ours = check_client_intr(sd);
+
+		/* For local interrupts, wake the waiting process */
+		if (ours && sd->got_hcint) {
+			sd_trace(("INTR->WAKE\n"));
+			sdos = (struct sdos_info *)sd->sdos_info;
+			wake_up_interruptible(&sdos->intr_wait_queue);
+		}
+		return IRQ_RETVAL(ours);
+	}
+}
+
+/* Register with Linux for interrupts */
+int
+sdstd_register_irq(sdioh_info_t *sd, uint irq)
+{
+	sd_trace(("Entering %s: irq == %d\n", __FUNCTION__, irq));
+	if (request_irq(irq, sdstd_isr, IRQF_SHARED, "bcmsdstd", sd) < 0) {
+		sd_err(("%s: request_irq() failed\n", __FUNCTION__));
+		return ERROR;
+	}
+	return SUCCESS;
+}
+
+/* Free Linux irq */
+void
+sdstd_free_irq(uint irq, sdioh_info_t *sd)
+{
+	free_irq(irq, sd);
+}
+
+/* Map Host controller registers */
+
+uint32 *
+sdstd_reg_map(osl_t *osh, int32 addr, int size)
+{
+	return (uint32 *)REG_MAP(addr, size);
+}
+
+void
+sdstd_reg_unmap(osl_t *osh, int32 addr, int size)
+{
+	REG_UNMAP((void*)(uintptr)addr);
+}
+
+int
+sdstd_osinit(sdioh_info_t *sd)
+{
+	struct sdos_info *sdos;
+
+	sdos = (struct sdos_info*)MALLOC(sd->osh, sizeof(struct sdos_info));
+	sd->sdos_info = (void*)sdos;
+	if (sdos == NULL)
+		return BCME_NOMEM;
+
+	sdos->sd = sd;
+	spin_lock_init(&sdos->lock);
+	init_waitqueue_head(&sdos->intr_wait_queue);
+	return BCME_OK;
+}
+
+void
+sdstd_osfree(sdioh_info_t *sd)
+{
+	struct sdos_info *sdos;
+	ASSERT(sd && sd->sdos_info);
+
+	sdos = (struct sdos_info *)sd->sdos_info;
+	MFREE(sd->osh, sdos, sizeof(struct sdos_info));
+}
+
+/* Interrupt enable/disable */
+SDIOH_API_RC
+sdioh_interrupt_set(sdioh_info_t *sd, bool enable)
+{
+	ulong flags;
+	struct sdos_info *sdos;
+
+	sd_trace(("%s: %s\n", __FUNCTION__, enable ? "Enabling" : "Disabling"));
+
+	sdos = (struct sdos_info *)sd->sdos_info;
+	ASSERT(sdos);
+
+	if (!(sd->host_init_done && sd->card_init_done)) {
+		sd_err(("%s: Card & Host are not initted - bailing\n", __FUNCTION__));
+		return SDIOH_API_RC_FAIL;
+	}
+
+	if (enable && !(sd->intr_handler && sd->intr_handler_arg)) {
+		sd_err(("%s: no handler registered, will not enable\n", __FUNCTION__));
+		return SDIOH_API_RC_FAIL;
+	}
+
+	/* Ensure atomicity for enable/disable calls */
+	spin_lock_irqsave(&sdos->lock, flags);
+
+	sd->client_intr_enabled = enable;
+	if (enable && !sd->lockcount)
+		sdstd_devintr_on(sd);
+	else
+		sdstd_devintr_off(sd);
+
+	spin_unlock_irqrestore(&sdos->lock, flags);
+
+	return SDIOH_API_RC_SUCCESS;
+}
+
+/* Protect against reentrancy (disable device interrupts while executing) */
+void
+sdstd_lock(sdioh_info_t *sd)
+{
+	ulong flags;
+	struct sdos_info *sdos;
+
+	sdos = (struct sdos_info *)sd->sdos_info;
+	ASSERT(sdos);
+
+	sd_trace(("%s: %d\n", __FUNCTION__, sd->lockcount));
+
+	spin_lock_irqsave(&sdos->lock, flags);
+	if (sd->lockcount) {
+		sd_err(("%s: Already locked! called from %p\n",
+		       __FUNCTION__,
+		       __builtin_return_address(0)));
+		ASSERT(sd->lockcount == 0);
+	}
+	sdstd_devintr_off(sd);
+	sd->lockcount++;
+	spin_unlock_irqrestore(&sdos->lock, flags);
+}
+
+/* Enable client interrupt */
+void
+sdstd_unlock(sdioh_info_t *sd)
+{
+	ulong flags;
+	struct sdos_info *sdos;
+
+	sd_trace(("%s: %d, %d\n", __FUNCTION__, sd->lockcount, sd->client_intr_enabled));
+	ASSERT(sd->lockcount > 0);
+
+	sdos = (struct sdos_info *)sd->sdos_info;
+	ASSERT(sdos);
+
+	spin_lock_irqsave(&sdos->lock, flags);
+	if (--sd->lockcount == 0 && sd->client_intr_enabled) {
+		sdstd_devintr_on(sd);
+	}
+	spin_unlock_irqrestore(&sdos->lock, flags);
+}
+
+uint16
+sdstd_waitbits(sdioh_info_t *sd, uint16 norm, uint16 err, bool yield)
+{
+	struct sdos_info *sdos;
+
+	sdos = (struct sdos_info *)sd->sdos_info;
+
+#ifndef BCMSDYIELD
+	ASSERT(!yield);
+#endif
+	sd_trace(("%s: int 0x%02x err 0x%02x yield %d canblock %d\n",
+	          __FUNCTION__, norm, err, yield, BLOCKABLE()));
+
+	/* Clear the "interrupt happened" flag and last intrstatus */
+	sd->got_hcint = FALSE;
+	sd->last_intrstatus = 0;
+
+#ifdef BCMSDYIELD
+	if (yield && BLOCKABLE()) {
+		/* Enable interrupts, wait for the indication, then disable */
+		sdstd_intrs_on(sd, norm, err);
+		wait_event_interruptible(sdos->intr_wait_queue, (sd->got_hcint));
+		sdstd_intrs_off(sd, norm, err);
+	} else
+#endif /* BCMSDYIELD */
+	{
+		sdstd_spinbits(sd, norm, err);
+	}
+
+	sd_trace(("%s: last_intrstatus 0x%04x\n", __FUNCTION__, sd->last_intrstatus));
+
+	return sd->last_intrstatus;
+}
diff --git a/drivers/net/wireless/bcm4329/bcmutils.c b/drivers/net/wireless/bcm4329/bcmutils.c
new file mode 100644
index 000000000000..43c04ee92f38
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/bcmutils.c
@@ -0,0 +1,1838 @@
+/*
+ * Driver O/S-independent utility routines
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: bcmutils.c,v 1.210.4.5.2.4.6.19 2010/04/26 06:05:25 Exp $
+ */
+
+#include <typedefs.h>
+#include <bcmdefs.h>
+#include <stdarg.h>
+#include <bcmutils.h>
+#ifdef BCMDRIVER
+#include <osl.h>
+#include <siutils.h>
+#else
+#include <stdio.h>
+#include <string.h>
+/* This case for external supplicant use */
+#if defined(BCMEXTSUP)
+#include <bcm_osl.h>
+#endif
+
+#endif /* BCMDRIVER */
+#include <bcmendian.h>
+#include <bcmdevs.h>
+#include <proto/ethernet.h>
+#include <proto/vlan.h>
+#include <proto/bcmip.h>
+#include <proto/802.1d.h>
+#include <proto/802.11.h>
+
+
+#ifdef BCMDRIVER
+
+
+/* copy a pkt buffer chain into a buffer */
+uint
+pktcopy(osl_t *osh, void *p, uint offset, int len, uchar *buf)
+{
+	uint n, ret = 0;
+
+	if (len < 0)
+		len = 4096;	/* "infinite" */
+
+	/* skip 'offset' bytes */
+	for (; p && offset; p = PKTNEXT(osh, p)) {
+		if (offset < (uint)PKTLEN(osh, p))
+			break;
+		offset -= PKTLEN(osh, p);
+	}
+
+	if (!p)
+		return 0;
+
+	/* copy the data */
+	for (; p && len; p = PKTNEXT(osh, p)) {
+		n = MIN((uint)PKTLEN(osh, p) - offset, (uint)len);
+		bcopy(PKTDATA(osh, p) + offset, buf, n);
+		buf += n;
+		len -= n;
+		ret += n;
+		offset = 0;
+	}
+
+	return ret;
+}
+
+/* copy a buffer into a pkt buffer chain */
+uint
+pktfrombuf(osl_t *osh, void *p, uint offset, int len, uchar *buf)
+{
+	uint n, ret = 0;
+
+	/* skip 'offset' bytes */
+	for (; p && offset; p = PKTNEXT(osh, p)) {
+		if (offset < (uint)PKTLEN(osh, p))
+			break;
+		offset -= PKTLEN(osh, p);
+	}
+
+	if (!p)
+		return 0;
+
+	/* copy the data */
+	for (; p && len; p = PKTNEXT(osh, p)) {
+		n = MIN((uint)PKTLEN(osh, p) - offset, (uint)len);
+		bcopy(buf, PKTDATA(osh, p) + offset, n);
+		buf += n;
+		len -= n;
+		ret += n;
+		offset = 0;
+	}
+
+	return ret;
+}
+
+
+
+/* return total length of buffer chain */
+uint
+pkttotlen(osl_t *osh, void *p)
+{
+	uint total;
+
+	total = 0;
+	for (; p; p = PKTNEXT(osh, p))
+		total += PKTLEN(osh, p);
+	return (total);
+}
+
+/* return the last buffer of chained pkt */
+void *
+pktlast(osl_t *osh, void *p)
+{
+	for (; PKTNEXT(osh, p); p = PKTNEXT(osh, p))
+		;
+
+	return (p);
+}
+
+/* count segments of a chained packet */
+uint
+pktsegcnt(osl_t *osh, void *p)
+{
+	uint cnt;
+
+	for (cnt = 0; p; p = PKTNEXT(osh, p))
+		cnt++;
+
+	return cnt;
+}
+
+
+/*
+ * osl multiple-precedence packet queue
+ * hi_prec is always >= the number of the highest non-empty precedence
+ */
+void *
+pktq_penq(struct pktq *pq, int prec, void *p)
+{
+	struct pktq_prec *q;
+
+	ASSERT(prec >= 0 && prec < pq->num_prec);
+	ASSERT(PKTLINK(p) == NULL);         /* queueing chains not allowed */
+
+	ASSERT(!pktq_full(pq));
+	ASSERT(!pktq_pfull(pq, prec));
+
+	q = &pq->q[prec];
+
+	if (q->head)
+		PKTSETLINK(q->tail, p);
+	else
+		q->head = p;
+
+	q->tail = p;
+	q->len++;
+
+	pq->len++;
+
+	if (pq->hi_prec < prec)
+		pq->hi_prec = (uint8)prec;
+
+	return p;
+}
+
+void *
+pktq_penq_head(struct pktq *pq, int prec, void *p)
+{
+	struct pktq_prec *q;
+
+	ASSERT(prec >= 0 && prec < pq->num_prec);
+	ASSERT(PKTLINK(p) == NULL);         /* queueing chains not allowed */
+
+	ASSERT(!pktq_full(pq));
+	ASSERT(!pktq_pfull(pq, prec));
+
+	q = &pq->q[prec];
+
+	if (q->head == NULL)
+		q->tail = p;
+
+	PKTSETLINK(p, q->head);
+	q->head = p;
+	q->len++;
+
+	pq->len++;
+
+	if (pq->hi_prec < prec)
+		pq->hi_prec = (uint8)prec;
+
+	return p;
+}
+
+void *
+pktq_pdeq(struct pktq *pq, int prec)
+{
+	struct pktq_prec *q;
+	void *p;
+
+	ASSERT(prec >= 0 && prec < pq->num_prec);
+
+	q = &pq->q[prec];
+
+	if ((p = q->head) == NULL)
+		return NULL;
+
+	if ((q->head = PKTLINK(p)) == NULL)
+		q->tail = NULL;
+
+	q->len--;
+
+	pq->len--;
+
+	PKTSETLINK(p, NULL);
+
+	return p;
+}
+
+void *
+pktq_pdeq_tail(struct pktq *pq, int prec)
+{
+	struct pktq_prec *q;
+	void *p, *prev;
+
+	ASSERT(prec >= 0 && prec < pq->num_prec);
+
+	q = &pq->q[prec];
+
+	if ((p = q->head) == NULL)
+		return NULL;
+
+	for (prev = NULL; p != q->tail; p = PKTLINK(p))
+		prev = p;
+
+	if (prev)
+		PKTSETLINK(prev, NULL);
+	else
+		q->head = NULL;
+
+	q->tail = prev;
+	q->len--;
+
+	pq->len--;
+
+	return p;
+}
+
+void
+pktq_pflush(osl_t *osh, struct pktq *pq, int prec, bool dir)
+{
+	struct pktq_prec *q;
+	void *p;
+
+	q = &pq->q[prec];
+	p = q->head;
+	while (p) {
+		q->head = PKTLINK(p);
+		PKTSETLINK(p, NULL);
+		PKTFREE(osh, p, dir);
+		q->len--;
+		pq->len--;
+		p = q->head;
+	}
+	ASSERT(q->len == 0);
+	q->tail = NULL;
+}
+
+bool
+pktq_pdel(struct pktq *pq, void *pktbuf, int prec)
+{
+	struct pktq_prec *q;
+	void *p;
+
+	ASSERT(prec >= 0 && prec < pq->num_prec);
+
+	if (!pktbuf)
+		return FALSE;
+
+	q = &pq->q[prec];
+
+	if (q->head == pktbuf) {
+		if ((q->head = PKTLINK(pktbuf)) == NULL)
+			q->tail = NULL;
+	} else {
+		for (p = q->head; p && PKTLINK(p) != pktbuf; p = PKTLINK(p))
+			;
+		if (p == NULL)
+			return FALSE;
+
+		PKTSETLINK(p, PKTLINK(pktbuf));
+		if (q->tail == pktbuf)
+			q->tail = p;
+	}
+
+	q->len--;
+	pq->len--;
+	PKTSETLINK(pktbuf, NULL);
+	return TRUE;
+}
+
+void
+pktq_init(struct pktq *pq, int num_prec, int max_len)
+{
+	int prec;
+
+	ASSERT(num_prec > 0 && num_prec <= PKTQ_MAX_PREC);
+
+	/* pq is variable size; only zero out what's requested */
+	bzero(pq, OFFSETOF(struct pktq, q) + (sizeof(struct pktq_prec) * num_prec));
+
+	pq->num_prec = (uint16)num_prec;
+
+	pq->max = (uint16)max_len;
+
+	for (prec = 0; prec < num_prec; prec++)
+		pq->q[prec].max = pq->max;
+}
+
+void *
+pktq_deq(struct pktq *pq, int *prec_out)
+{
+	struct pktq_prec *q;
+	void *p;
+	int prec;
+
+	if (pq->len == 0)
+		return NULL;
+
+	while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
+		pq->hi_prec--;
+
+	q = &pq->q[prec];
+
+	if ((p = q->head) == NULL)
+		return NULL;
+
+	if ((q->head = PKTLINK(p)) == NULL)
+		q->tail = NULL;
+
+	q->len--;
+
+	pq->len--;
+
+	if (prec_out)
+		*prec_out = prec;
+
+	PKTSETLINK(p, NULL);
+
+	return p;
+}
+
+void *
+pktq_deq_tail(struct pktq *pq, int *prec_out)
+{
+	struct pktq_prec *q;
+	void *p, *prev;
+	int prec;
+
+	if (pq->len == 0)
+		return NULL;
+
+	for (prec = 0; prec < pq->hi_prec; prec++)
+		if (pq->q[prec].head)
+			break;
+
+	q = &pq->q[prec];
+
+	if ((p = q->head) == NULL)
+		return NULL;
+
+	for (prev = NULL; p != q->tail; p = PKTLINK(p))
+		prev = p;
+
+	if (prev)
+		PKTSETLINK(prev, NULL);
+	else
+		q->head = NULL;
+
+	q->tail = prev;
+	q->len--;
+
+	pq->len--;
+
+	if (prec_out)
+		*prec_out = prec;
+
+	PKTSETLINK(p, NULL);
+
+	return p;
+}
+
+void *
+pktq_peek(struct pktq *pq, int *prec_out)
+{
+	int prec;
+
+	if (pq->len == 0)
+		return NULL;
+
+	while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
+		pq->hi_prec--;
+
+	if (prec_out)
+		*prec_out = prec;
+
+	return (pq->q[prec].head);
+}
+
+void *
+pktq_peek_tail(struct pktq *pq, int *prec_out)
+{
+	int prec;
+
+	if (pq->len == 0)
+		return NULL;
+
+	for (prec = 0; prec < pq->hi_prec; prec++)
+		if (pq->q[prec].head)
+			break;
+
+	if (prec_out)
+		*prec_out = prec;
+
+	return (pq->q[prec].tail);
+}
+
+void
+pktq_flush(osl_t *osh, struct pktq *pq, bool dir)
+{
+	int prec;
+	for (prec = 0; prec < pq->num_prec; prec++)
+		pktq_pflush(osh, pq, prec, dir);
+	ASSERT(pq->len == 0);
+}
+
+/* Return sum of lengths of a specific set of precedences */
+int
+pktq_mlen(struct pktq *pq, uint prec_bmp)
+{
+	int prec, len;
+
+	len = 0;
+
+	for (prec = 0; prec <= pq->hi_prec; prec++)
+		if (prec_bmp & (1 << prec))
+			len += pq->q[prec].len;
+
+	return len;
+}
+
+/* Priority dequeue from a specific set of precedences */
+void *
+pktq_mdeq(struct pktq *pq, uint prec_bmp, int *prec_out)
+{
+	struct pktq_prec *q;
+	void *p;
+	int prec;
+
+	if (pq->len == 0)
+		return NULL;
+
+	while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
+		pq->hi_prec--;
+
+	while ((prec_bmp & (1 << prec)) == 0 || pq->q[prec].head == NULL)
+		if (prec-- == 0)
+			return NULL;
+
+	q = &pq->q[prec];
+
+	if ((p = q->head) == NULL)
+		return NULL;
+
+	if ((q->head = PKTLINK(p)) == NULL)
+		q->tail = NULL;
+
+	q->len--;
+
+	if (prec_out)
+		*prec_out = prec;
+
+	pq->len--;
+
+	PKTSETLINK(p, NULL);
+
+	return p;
+}
+#endif /* BCMDRIVER */
+
+
+
+const unsigned char bcm_ctype[] = {
+	_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,			/* 0-7 */
+	_BCM_C, _BCM_C|_BCM_S, _BCM_C|_BCM_S, _BCM_C|_BCM_S, _BCM_C|_BCM_S, _BCM_C|_BCM_S, _BCM_C,
+	_BCM_C,	/* 8-15 */
+	_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,			/* 16-23 */
+	_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,			/* 24-31 */
+	_BCM_S|_BCM_SP,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,		/* 32-39 */
+	_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,			/* 40-47 */
+	_BCM_D,_BCM_D,_BCM_D,_BCM_D,_BCM_D,_BCM_D,_BCM_D,_BCM_D,			/* 48-55 */
+	_BCM_D,_BCM_D,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,			/* 56-63 */
+	_BCM_P, _BCM_U|_BCM_X, _BCM_U|_BCM_X, _BCM_U|_BCM_X, _BCM_U|_BCM_X, _BCM_U|_BCM_X,
+	_BCM_U|_BCM_X, _BCM_U, /* 64-71 */
+	_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,			/* 72-79 */
+	_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,			/* 80-87 */
+	_BCM_U,_BCM_U,_BCM_U,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,			/* 88-95 */
+	_BCM_P, _BCM_L|_BCM_X, _BCM_L|_BCM_X, _BCM_L|_BCM_X, _BCM_L|_BCM_X, _BCM_L|_BCM_X,
+	_BCM_L|_BCM_X, _BCM_L, /* 96-103 */
+	_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L, /* 104-111 */
+	_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L, /* 112-119 */
+	_BCM_L,_BCM_L,_BCM_L,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_C, /* 120-127 */
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,		/* 128-143 */
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,		/* 144-159 */
+	_BCM_S|_BCM_SP, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P,
+	_BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P,	/* 160-175 */
+	_BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P,
+	_BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P,	/* 176-191 */
+	_BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U,
+	_BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U,	/* 192-207 */
+	_BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_P, _BCM_U, _BCM_U, _BCM_U,
+	_BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_L,	/* 208-223 */
+	_BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L,
+	_BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L,	/* 224-239 */
+	_BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_P, _BCM_L, _BCM_L, _BCM_L,
+	_BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L /* 240-255 */
+};
+
+ulong
+bcm_strtoul(char *cp, char **endp, uint base)
+{
+	ulong result, last_result = 0, value;
+	bool minus;
+
+	minus = FALSE;
+
+	while (bcm_isspace(*cp))
+		cp++;
+
+	if (cp[0] == '+')
+		cp++;
+	else if (cp[0] == '-') {
+		minus = TRUE;
+		cp++;
+	}
+
+	if (base == 0) {
+		if (cp[0] == '0') {
+			if ((cp[1] == 'x') || (cp[1] == 'X')) {
+				base = 16;
+				cp = &cp[2];
+			} else {
+				base = 8;
+				cp = &cp[1];
+			}
+		} else
+			base = 10;
+	} else if (base == 16 && (cp[0] == '0') && ((cp[1] == 'x') || (cp[1] == 'X'))) {
+		cp = &cp[2];
+	}
+
+	result = 0;
+
+	while (bcm_isxdigit(*cp) &&
+	       (value = bcm_isdigit(*cp) ? *cp-'0' : bcm_toupper(*cp)-'A'+10) < base) {
+		result = result*base + value;
+		/* Detected overflow */
+		if (result < last_result && !minus)
+			return (ulong)-1;
+		last_result = result;
+		cp++;
+	}
+
+	if (minus)
+		result = (ulong)(-(long)result);
+
+	if (endp)
+		*endp = (char *)cp;
+
+	return (result);
+}
+
+int
+bcm_atoi(char *s)
+{
+	return (int)bcm_strtoul(s, NULL, 10);
+}
+
+/* return pointer to location of substring 'needle' in 'haystack' */
+char*
+bcmstrstr(char *haystack, char *needle)
+{
+	int len, nlen;
+	int i;
+
+	if ((haystack == NULL) || (needle == NULL))
+		return (haystack);
+
+	nlen = strlen(needle);
+	len = strlen(haystack) - nlen + 1;
+
+	for (i = 0; i < len; i++)
+		if (memcmp(needle, &haystack[i], nlen) == 0)
+			return (&haystack[i]);
+	return (NULL);
+}
+
+char*
+bcmstrcat(char *dest, const char *src)
+{
+	char *p;
+
+	p = dest + strlen(dest);
+
+	while ((*p++ = *src++) != '\0')
+		;
+
+	return (dest);
+}
+
+char*
+bcmstrncat(char *dest, const char *src, uint size)
+{
+	char *endp;
+	char *p;
+
+	p = dest + strlen(dest);
+	endp = p + size;
+
+	while (p != endp && (*p++ = *src++) != '\0')
+		;
+
+	return (dest);
+}
+
+
+/****************************************************************************
+* Function:   bcmstrtok
+*
+* Purpose:
+*  Tokenizes a string. This function is conceptually similiar to ANSI C strtok(),
+*  but allows strToken() to be used by different strings or callers at the same
+*  time. Each call modifies '*string' by substituting a NULL character for the
+*  first delimiter that is encountered, and updates 'string' to point to the char
+*  after the delimiter. Leading delimiters are skipped.
+*
+* Parameters:
+*  string      (mod) Ptr to string ptr, updated by token.
+*  delimiters  (in)  Set of delimiter characters.
+*  tokdelim    (out) Character that delimits the returned token. (May
+*                    be set to NULL if token delimiter is not required).
+*
+* Returns:  Pointer to the next token found. NULL when no more tokens are found.
+*****************************************************************************
+*/
+char *
+bcmstrtok(char **string, const char *delimiters, char *tokdelim)
+{
+	unsigned char *str;
+	unsigned long map[8];
+	int count;
+	char *nextoken;
+
+	if (tokdelim != NULL) {
+		/* Prime the token delimiter */
+		*tokdelim = '\0';
+	}
+
+	/* Clear control map */
+	for (count = 0; count < 8; count++) {
+		map[count] = 0;
+	}
+
+	/* Set bits in delimiter table */
+	do {
+		map[*delimiters >> 5] |= (1 << (*delimiters & 31));
+	}
+	while (*delimiters++);
+
+	str = (unsigned char*)*string;
+
+	/* Find beginning of token (skip over leading delimiters). Note that
+	 * there is no token iff this loop sets str to point to the terminal
+	 * null (*str == '\0')
+	 */
+	while (((map[*str >> 5] & (1 << (*str & 31))) && *str) || (*str == ' ')) {
+		str++;
+	}
+
+	nextoken = (char*)str;
+
+	/* Find the end of the token. If it is not the end of the string,
+	 * put a null there.
+	 */
+	for (; *str; str++) {
+		if (map[*str >> 5] & (1 << (*str & 31))) {
+			if (tokdelim != NULL) {
+				*tokdelim = *str;
+			}
+
+			*str++ = '\0';
+			break;
+		}
+	}
+
+	*string = (char*)str;
+
+	/* Determine if a token has been found. */
+	if (nextoken == (char *) str) {
+		return NULL;
+	}
+	else {
+		return nextoken;
+	}
+}
+
+
+#define xToLower(C) \
+	((C >= 'A' && C <= 'Z') ? (char)((int)C - (int)'A' + (int)'a') : C)
+
+
+/****************************************************************************
+* Function:   bcmstricmp
+*
+* Purpose:    Compare to strings case insensitively.
+*
+* Parameters: s1 (in) First string to compare.
+*             s2 (in) Second string to compare.
+*
+* Returns:    Return 0 if the two strings are equal, -1 if t1 < t2 and 1 if
+*             t1 > t2, when ignoring case sensitivity.
+*****************************************************************************
+*/
+int
+bcmstricmp(const char *s1, const char *s2)
+{
+	char dc, sc;
+
+	while (*s2 && *s1) {
+		dc = xToLower(*s1);
+		sc = xToLower(*s2);
+		if (dc < sc) return -1;
+		if (dc > sc) return 1;
+		s1++;
+		s2++;
+	}
+
+	if (*s1 && !*s2) return 1;
+	if (!*s1 && *s2) return -1;
+	return 0;
+}
+
+
+/****************************************************************************
+* Function:   bcmstrnicmp
+*
+* Purpose:    Compare to strings case insensitively, upto a max of 'cnt'
+*             characters.
+*
+* Parameters: s1  (in) First string to compare.
+*             s2  (in) Second string to compare.
+*             cnt (in) Max characters to compare.
+*
+* Returns:    Return 0 if the two strings are equal, -1 if t1 < t2 and 1 if
+*             t1 > t2, when ignoring case sensitivity.
+*****************************************************************************
+*/
+int
+bcmstrnicmp(const char* s1, const char* s2, int cnt)
+{
+	char dc, sc;
+
+	while (*s2 && *s1 && cnt) {
+		dc = xToLower(*s1);
+		sc = xToLower(*s2);
+		if (dc < sc) return -1;
+		if (dc > sc) return 1;
+		s1++;
+		s2++;
+		cnt--;
+	}
+
+	if (!cnt) return 0;
+	if (*s1 && !*s2) return 1;
+	if (!*s1 && *s2) return -1;
+	return 0;
+}
+
+/* parse a xx:xx:xx:xx:xx:xx format ethernet address */
+int
+bcm_ether_atoe(char *p, struct ether_addr *ea)
+{
+	int i = 0;
+
+	for (;;) {
+		ea->octet[i++] = (char) bcm_strtoul(p, &p, 16);
+		if (!*p++ || i == 6)
+			break;
+	}
+
+	return (i == 6);
+}
+
+
+#if defined(CONFIG_USBRNDIS_RETAIL) || defined(NDIS_MINIPORT_DRIVER)
+/* registry routine buffer preparation utility functions:
+ * parameter order is like strncpy, but returns count
+ * of bytes copied. Minimum bytes copied is null char(1)/wchar(2)
+ */
+ulong
+wchar2ascii(char *abuf, ushort *wbuf, ushort wbuflen, ulong abuflen)
+{
+	ulong copyct = 1;
+	ushort i;
+
+	if (abuflen == 0)
+		return 0;
+
+	/* wbuflen is in bytes */
+	wbuflen /= sizeof(ushort);
+
+	for (i = 0; i < wbuflen; ++i) {
+		if (--abuflen == 0)
+			break;
+		*abuf++ = (char) *wbuf++;
+		++copyct;
+	}
+	*abuf = '\0';
+
+	return copyct;
+}
+#endif /* CONFIG_USBRNDIS_RETAIL || NDIS_MINIPORT_DRIVER */
+
+char *
+bcm_ether_ntoa(const struct ether_addr *ea, char *buf)
+{
+	static const char template[] = "%02x:%02x:%02x:%02x:%02x:%02x";
+	snprintf(buf, 18, template,
+		ea->octet[0]&0xff, ea->octet[1]&0xff, ea->octet[2]&0xff,
+		ea->octet[3]&0xff, ea->octet[4]&0xff, ea->octet[5]&0xff);
+	return (buf);
+}
+
+char *
+bcm_ip_ntoa(struct ipv4_addr *ia, char *buf)
+{
+	snprintf(buf, 16, "%d.%d.%d.%d",
+	         ia->addr[0], ia->addr[1], ia->addr[2], ia->addr[3]);
+	return (buf);
+}
+
+#ifdef BCMDRIVER
+
+void
+bcm_mdelay(uint ms)
+{
+	uint i;
+
+	for (i = 0; i < ms; i++) {
+		OSL_DELAY(1000);
+	}
+}
+
+
+
+
+
+
+#if defined(DHD_DEBUG)
+/* pretty hex print a pkt buffer chain */
+void
+prpkt(const char *msg, osl_t *osh, void *p0)
+{
+	void *p;
+
+	if (msg && (msg[0] != '\0'))
+		printf("%s:\n", msg);
+
+	for (p = p0; p; p = PKTNEXT(osh, p))
+		prhex(NULL, PKTDATA(osh, p), PKTLEN(osh, p));
+}
+#endif	
+
+/* Takes an Ethernet frame and sets out-of-bound PKTPRIO.
+ * Also updates the inplace vlan tag if requested.
+ * For debugging, it returns an indication of what it did.
+ */
+uint
+pktsetprio(void *pkt, bool update_vtag)
+{
+	struct ether_header *eh;
+	struct ethervlan_header *evh;
+	uint8 *pktdata;
+	int priority = 0;
+	int rc = 0;
+
+	pktdata = (uint8 *) PKTDATA(NULL, pkt);
+	ASSERT(ISALIGNED((uintptr)pktdata, sizeof(uint16)));
+
+	eh = (struct ether_header *) pktdata;
+
+	if (ntoh16(eh->ether_type) == ETHER_TYPE_8021Q) {
+		uint16 vlan_tag;
+		int vlan_prio, dscp_prio = 0;
+
+		evh = (struct ethervlan_header *)eh;
+
+		vlan_tag = ntoh16(evh->vlan_tag);
+		vlan_prio = (int) (vlan_tag >> VLAN_PRI_SHIFT) & VLAN_PRI_MASK;
+
+		if (ntoh16(evh->ether_type) == ETHER_TYPE_IP) {
+			uint8 *ip_body = pktdata + sizeof(struct ethervlan_header);
+			uint8 tos_tc = IP_TOS(ip_body);
+			dscp_prio = (int)(tos_tc >> IPV4_TOS_PREC_SHIFT);
+		}
+
+		/* DSCP priority gets precedence over 802.1P (vlan tag) */
+		if (dscp_prio != 0) {
+			priority = dscp_prio;
+			rc |= PKTPRIO_VDSCP;
+		} else {
+			priority = vlan_prio;
+			rc |= PKTPRIO_VLAN;
+		}
+		/*
+		 * If the DSCP priority is not the same as the VLAN priority,
+		 * then overwrite the priority field in the vlan tag, with the
+		 * DSCP priority value. This is required for Linux APs because
+		 * the VLAN driver on Linux, overwrites the skb->priority field
+		 * with the priority value in the vlan tag
+		 */
+		if (update_vtag && (priority != vlan_prio)) {
+			vlan_tag &= ~(VLAN_PRI_MASK << VLAN_PRI_SHIFT);
+			vlan_tag |= (uint16)priority << VLAN_PRI_SHIFT;
+			evh->vlan_tag = hton16(vlan_tag);
+			rc |= PKTPRIO_UPD;
+		}
+	} else if (ntoh16(eh->ether_type) == ETHER_TYPE_IP) {
+		uint8 *ip_body = pktdata + sizeof(struct ether_header);
+		uint8 tos_tc = IP_TOS(ip_body);
+		priority = (int)(tos_tc >> IPV4_TOS_PREC_SHIFT);
+		rc |= PKTPRIO_DSCP;
+	}
+
+	ASSERT(priority >= 0 && priority <= MAXPRIO);
+	PKTSETPRIO(pkt, priority);
+	return (rc | priority);
+}
+
+static char bcm_undeferrstr[BCME_STRLEN];
+
+static const char *bcmerrorstrtable[] = BCMERRSTRINGTABLE;
+
+/* Convert the error codes into related error strings  */
+const char *
+bcmerrorstr(int bcmerror)
+{
+	/* check if someone added a bcmerror code but forgot to add errorstring */
+	ASSERT(ABS(BCME_LAST) == (ARRAYSIZE(bcmerrorstrtable) - 1));
+
+	if (bcmerror > 0 || bcmerror < BCME_LAST) {
+		snprintf(bcm_undeferrstr, BCME_STRLEN, "Undefined error %d", bcmerror);
+		return bcm_undeferrstr;
+	}
+
+	ASSERT(strlen(bcmerrorstrtable[-bcmerror]) < BCME_STRLEN);
+
+	return bcmerrorstrtable[-bcmerror];
+}
+
+
+
+/* iovar table lookup */
+const bcm_iovar_t*
+bcm_iovar_lookup(const bcm_iovar_t *table, const char *name)
+{
+	const bcm_iovar_t *vi;
+	const char *lookup_name;
+
+	/* skip any ':' delimited option prefixes */
+	lookup_name = strrchr(name, ':');
+	if (lookup_name != NULL)
+		lookup_name++;
+	else
+		lookup_name = name;
+
+	ASSERT(table != NULL);
+
+	for (vi = table; vi->name; vi++) {
+		if (!strcmp(vi->name, lookup_name))
+			return vi;
+	}
+	/* ran to end of table */
+
+	return NULL; /* var name not found */
+}
+
+int
+bcm_iovar_lencheck(const bcm_iovar_t *vi, void *arg, int len, bool set)
+{
+	int bcmerror = 0;
+
+	/* length check on io buf */
+	switch (vi->type) {
+	case IOVT_BOOL:
+	case IOVT_INT8:
+	case IOVT_INT16:
+	case IOVT_INT32:
+	case IOVT_UINT8:
+	case IOVT_UINT16:
+	case IOVT_UINT32:
+		/* all integers are int32 sized args at the ioctl interface */
+		if (len < (int)sizeof(int)) {
+			bcmerror = BCME_BUFTOOSHORT;
+		}
+		break;
+
+	case IOVT_BUFFER:
+		/* buffer must meet minimum length requirement */
+		if (len < vi->minlen) {
+			bcmerror = BCME_BUFTOOSHORT;
+		}
+		break;
+
+	case IOVT_VOID:
+		if (!set) {
+			/* Cannot return nil... */
+			bcmerror = BCME_UNSUPPORTED;
+		} else if (len) {
+			/* Set is an action w/o parameters */
+			bcmerror = BCME_BUFTOOLONG;
+		}
+		break;
+
+	default:
+		/* unknown type for length check in iovar info */
+		ASSERT(0);
+		bcmerror = BCME_UNSUPPORTED;
+	}
+
+	return bcmerror;
+}
+
+#endif	/* BCMDRIVER */
+
+/*******************************************************************************
+ * crc8
+ *
+ * Computes a crc8 over the input data using the polynomial:
+ *
+ *       x^8 + x^7 +x^6 + x^4 + x^2 + 1
+ *
+ * The caller provides the initial value (either CRC8_INIT_VALUE
+ * or the previous returned value) to allow for processing of
+ * discontiguous blocks of data.  When generating the CRC the
+ * caller is responsible for complementing the final return value
+ * and inserting it into the byte stream.  When checking, a final
+ * return value of CRC8_GOOD_VALUE indicates a valid CRC.
+ *
+ * Reference: Dallas Semiconductor Application Note 27
+ *   Williams, Ross N., "A Painless Guide to CRC Error Detection Algorithms",
+ *     ver 3, Aug 1993, ross@guest.adelaide.edu.au, Rocksoft Pty Ltd.,
+ *     ftp://ftp.rocksoft.com/clients/rocksoft/papers/crc_v3.txt
+ *
+ * ****************************************************************************
+ */
+
+STATIC const uint8 crc8_table[256] = {
+    0x00, 0xF7, 0xB9, 0x4E, 0x25, 0xD2, 0x9C, 0x6B,
+    0x4A, 0xBD, 0xF3, 0x04, 0x6F, 0x98, 0xD6, 0x21,
+    0x94, 0x63, 0x2D, 0xDA, 0xB1, 0x46, 0x08, 0xFF,
+    0xDE, 0x29, 0x67, 0x90, 0xFB, 0x0C, 0x42, 0xB5,
+    0x7F, 0x88, 0xC6, 0x31, 0x5A, 0xAD, 0xE3, 0x14,
+    0x35, 0xC2, 0x8C, 0x7B, 0x10, 0xE7, 0xA9, 0x5E,
+    0xEB, 0x1C, 0x52, 0xA5, 0xCE, 0x39, 0x77, 0x80,
+    0xA1, 0x56, 0x18, 0xEF, 0x84, 0x73, 0x3D, 0xCA,
+    0xFE, 0x09, 0x47, 0xB0, 0xDB, 0x2C, 0x62, 0x95,
+    0xB4, 0x43, 0x0D, 0xFA, 0x91, 0x66, 0x28, 0xDF,
+    0x6A, 0x9D, 0xD3, 0x24, 0x4F, 0xB8, 0xF6, 0x01,
+    0x20, 0xD7, 0x99, 0x6E, 0x05, 0xF2, 0xBC, 0x4B,
+    0x81, 0x76, 0x38, 0xCF, 0xA4, 0x53, 0x1D, 0xEA,
+    0xCB, 0x3C, 0x72, 0x85, 0xEE, 0x19, 0x57, 0xA0,
+    0x15, 0xE2, 0xAC, 0x5B, 0x30, 0xC7, 0x89, 0x7E,
+    0x5F, 0xA8, 0xE6, 0x11, 0x7A, 0x8D, 0xC3, 0x34,
+    0xAB, 0x5C, 0x12, 0xE5, 0x8E, 0x79, 0x37, 0xC0,
+    0xE1, 0x16, 0x58, 0xAF, 0xC4, 0x33, 0x7D, 0x8A,
+    0x3F, 0xC8, 0x86, 0x71, 0x1A, 0xED, 0xA3, 0x54,
+    0x75, 0x82, 0xCC, 0x3B, 0x50, 0xA7, 0xE9, 0x1E,
+    0xD4, 0x23, 0x6D, 0x9A, 0xF1, 0x06, 0x48, 0xBF,
+    0x9E, 0x69, 0x27, 0xD0, 0xBB, 0x4C, 0x02, 0xF5,
+    0x40, 0xB7, 0xF9, 0x0E, 0x65, 0x92, 0xDC, 0x2B,
+    0x0A, 0xFD, 0xB3, 0x44, 0x2F, 0xD8, 0x96, 0x61,
+    0x55, 0xA2, 0xEC, 0x1B, 0x70, 0x87, 0xC9, 0x3E,
+    0x1F, 0xE8, 0xA6, 0x51, 0x3A, 0xCD, 0x83, 0x74,
+    0xC1, 0x36, 0x78, 0x8F, 0xE4, 0x13, 0x5D, 0xAA,
+    0x8B, 0x7C, 0x32, 0xC5, 0xAE, 0x59, 0x17, 0xE0,
+    0x2A, 0xDD, 0x93, 0x64, 0x0F, 0xF8, 0xB6, 0x41,
+    0x60, 0x97, 0xD9, 0x2E, 0x45, 0xB2, 0xFC, 0x0B,
+    0xBE, 0x49, 0x07, 0xF0, 0x9B, 0x6C, 0x22, 0xD5,
+    0xF4, 0x03, 0x4D, 0xBA, 0xD1, 0x26, 0x68, 0x9F
+};
+
+#define CRC_INNER_LOOP(n, c, x) \
+	(c) = ((c) >> 8) ^ crc##n##_table[((c) ^ (x)) & 0xff]
+
+uint8
+hndcrc8(
+	uint8 *pdata,	/* pointer to array of data to process */
+	uint  nbytes,	/* number of input data bytes to process */
+	uint8 crc	/* either CRC8_INIT_VALUE or previous return value */
+)
+{
+	/* hard code the crc loop instead of using CRC_INNER_LOOP macro
+	 * to avoid the undefined and unnecessary (uint8 >> 8) operation.
+	 */
+	while (nbytes-- > 0)
+		crc = crc8_table[(crc ^ *pdata++) & 0xff];
+
+	return crc;
+}
+
+/*******************************************************************************
+ * crc16
+ *
+ * Computes a crc16 over the input data using the polynomial:
+ *
+ *       x^16 + x^12 +x^5 + 1
+ *
+ * The caller provides the initial value (either CRC16_INIT_VALUE
+ * or the previous returned value) to allow for processing of
+ * discontiguous blocks of data.  When generating the CRC the
+ * caller is responsible for complementing the final return value
+ * and inserting it into the byte stream.  When checking, a final
+ * return value of CRC16_GOOD_VALUE indicates a valid CRC.
+ *
+ * Reference: Dallas Semiconductor Application Note 27
+ *   Williams, Ross N., "A Painless Guide to CRC Error Detection Algorithms",
+ *     ver 3, Aug 1993, ross@guest.adelaide.edu.au, Rocksoft Pty Ltd.,
+ *     ftp://ftp.rocksoft.com/clients/rocksoft/papers/crc_v3.txt
+ *
+ * ****************************************************************************
+ */
+
+static const uint16 crc16_table[256] = {
+    0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
+    0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7,
+    0x1081, 0x0108, 0x3393, 0x221A, 0x56A5, 0x472C, 0x75B7, 0x643E,
+    0x9CC9, 0x8D40, 0xBFDB, 0xAE52, 0xDAED, 0xCB64, 0xF9FF, 0xE876,
+    0x2102, 0x308B, 0x0210, 0x1399, 0x6726, 0x76AF, 0x4434, 0x55BD,
+    0xAD4A, 0xBCC3, 0x8E58, 0x9FD1, 0xEB6E, 0xFAE7, 0xC87C, 0xD9F5,
+    0x3183, 0x200A, 0x1291, 0x0318, 0x77A7, 0x662E, 0x54B5, 0x453C,
+    0xBDCB, 0xAC42, 0x9ED9, 0x8F50, 0xFBEF, 0xEA66, 0xD8FD, 0xC974,
+    0x4204, 0x538D, 0x6116, 0x709F, 0x0420, 0x15A9, 0x2732, 0x36BB,
+    0xCE4C, 0xDFC5, 0xED5E, 0xFCD7, 0x8868, 0x99E1, 0xAB7A, 0xBAF3,
+    0x5285, 0x430C, 0x7197, 0x601E, 0x14A1, 0x0528, 0x37B3, 0x263A,
+    0xDECD, 0xCF44, 0xFDDF, 0xEC56, 0x98E9, 0x8960, 0xBBFB, 0xAA72,
+    0x6306, 0x728F, 0x4014, 0x519D, 0x2522, 0x34AB, 0x0630, 0x17B9,
+    0xEF4E, 0xFEC7, 0xCC5C, 0xDDD5, 0xA96A, 0xB8E3, 0x8A78, 0x9BF1,
+    0x7387, 0x620E, 0x5095, 0x411C, 0x35A3, 0x242A, 0x16B1, 0x0738,
+    0xFFCF, 0xEE46, 0xDCDD, 0xCD54, 0xB9EB, 0xA862, 0x9AF9, 0x8B70,
+    0x8408, 0x9581, 0xA71A, 0xB693, 0xC22C, 0xD3A5, 0xE13E, 0xF0B7,
+    0x0840, 0x19C9, 0x2B52, 0x3ADB, 0x4E64, 0x5FED, 0x6D76, 0x7CFF,
+    0x9489, 0x8500, 0xB79B, 0xA612, 0xD2AD, 0xC324, 0xF1BF, 0xE036,
+    0x18C1, 0x0948, 0x3BD3, 0x2A5A, 0x5EE5, 0x4F6C, 0x7DF7, 0x6C7E,
+    0xA50A, 0xB483, 0x8618, 0x9791, 0xE32E, 0xF2A7, 0xC03C, 0xD1B5,
+    0x2942, 0x38CB, 0x0A50, 0x1BD9, 0x6F66, 0x7EEF, 0x4C74, 0x5DFD,
+    0xB58B, 0xA402, 0x9699, 0x8710, 0xF3AF, 0xE226, 0xD0BD, 0xC134,
+    0x39C3, 0x284A, 0x1AD1, 0x0B58, 0x7FE7, 0x6E6E, 0x5CF5, 0x4D7C,
+    0xC60C, 0xD785, 0xE51E, 0xF497, 0x8028, 0x91A1, 0xA33A, 0xB2B3,
+    0x4A44, 0x5BCD, 0x6956, 0x78DF, 0x0C60, 0x1DE9, 0x2F72, 0x3EFB,
+    0xD68D, 0xC704, 0xF59F, 0xE416, 0x90A9, 0x8120, 0xB3BB, 0xA232,
+    0x5AC5, 0x4B4C, 0x79D7, 0x685E, 0x1CE1, 0x0D68, 0x3FF3, 0x2E7A,
+    0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1,
+    0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9,
+    0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330,
+    0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78
+};
+
+uint16
+hndcrc16(
+    uint8 *pdata,  /* pointer to array of data to process */
+    uint nbytes, /* number of input data bytes to process */
+    uint16 crc     /* either CRC16_INIT_VALUE or previous return value */
+)
+{
+	while (nbytes-- > 0)
+		CRC_INNER_LOOP(16, crc, *pdata++);
+	return crc;
+}
+
+STATIC const uint32 crc32_table[256] = {
+    0x00000000, 0x77073096, 0xEE0E612C, 0x990951BA,
+    0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3,
+    0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988,
+    0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, 0x90BF1D91,
+    0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE,
+    0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7,
+    0x136C9856, 0x646BA8C0, 0xFD62F97A, 0x8A65C9EC,
+    0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5,
+    0x3B6E20C8, 0x4C69105E, 0xD56041E4, 0xA2677172,
+    0x3C03E4D1, 0x4B04D447, 0xD20D85FD, 0xA50AB56B,
+    0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940,
+    0x32D86CE3, 0x45DF5C75, 0xDCD60DCF, 0xABD13D59,
+    0x26D930AC, 0x51DE003A, 0xC8D75180, 0xBFD06116,
+    0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F,
+    0x2802B89E, 0x5F058808, 0xC60CD9B2, 0xB10BE924,
+    0x2F6F7C87, 0x58684C11, 0xC1611DAB, 0xB6662D3D,
+    0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A,
+    0x71B18589, 0x06B6B51F, 0x9FBFE4A5, 0xE8B8D433,
+    0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818,
+    0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01,
+    0x6B6B51F4, 0x1C6C6162, 0x856530D8, 0xF262004E,
+    0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457,
+    0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C,
+    0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3, 0xFBD44C65,
+    0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2,
+    0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB,
+    0x4369E96A, 0x346ED9FC, 0xAD678846, 0xDA60B8D0,
+    0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9,
+    0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086,
+    0x5768B525, 0x206F85B3, 0xB966D409, 0xCE61E49F,
+    0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4,
+    0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD,
+    0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A,
+    0xEAD54739, 0x9DD277AF, 0x04DB2615, 0x73DC1683,
+    0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8,
+    0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1,
+    0xF00F9344, 0x8708A3D2, 0x1E01F268, 0x6906C2FE,
+    0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7,
+    0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC,
+    0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43, 0x60B08ED5,
+    0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252,
+    0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B,
+    0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6, 0x41047A60,
+    0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79,
+    0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236,
+    0xCC0C7795, 0xBB0B4703, 0x220216B9, 0x5505262F,
+    0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04,
+    0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D,
+    0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A,
+    0x9C0906A9, 0xEB0E363F, 0x72076785, 0x05005713,
+    0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38,
+    0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21,
+    0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8, 0x1FDA836E,
+    0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777,
+    0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C,
+    0x8F659EFF, 0xF862AE69, 0x616BFFD3, 0x166CCF45,
+    0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2,
+    0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB,
+    0xAED16A4A, 0xD9D65ADC, 0x40DF0B66, 0x37D83BF0,
+    0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9,
+    0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6,
+    0xBAD03605, 0xCDD70693, 0x54DE5729, 0x23D967BF,
+    0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94,
+    0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D
+};
+
+uint32
+hndcrc32(
+    uint8 *pdata,  /* pointer to array of data to process */
+    uint   nbytes, /* number of input data bytes to process */
+    uint32 crc     /* either CRC32_INIT_VALUE or previous return value */
+)
+{
+	uint8 *pend;
+#ifdef __mips__
+	uint8 tmp[4];
+	ulong *tptr = (ulong *)tmp;
+
+	/* in case the beginning of the buffer isn't aligned */
+	pend = (uint8 *)((uint)(pdata + 3) & 0xfffffffc);
+	nbytes -= (pend - pdata);
+	while (pdata < pend)
+		CRC_INNER_LOOP(32, crc, *pdata++);
+
+	/* handle bulk of data as 32-bit words */
+	pend = pdata + (nbytes & 0xfffffffc);
+	while (pdata < pend) {
+		*tptr = *(ulong *)pdata;
+		pdata += sizeof(ulong *);
+		CRC_INNER_LOOP(32, crc, tmp[0]);
+		CRC_INNER_LOOP(32, crc, tmp[1]);
+		CRC_INNER_LOOP(32, crc, tmp[2]);
+		CRC_INNER_LOOP(32, crc, tmp[3]);
+	}
+
+	/* 1-3 bytes at end of buffer */
+	pend = pdata + (nbytes & 0x03);
+	while (pdata < pend)
+		CRC_INNER_LOOP(32, crc, *pdata++);
+#else
+	pend = pdata + nbytes;
+	while (pdata < pend)
+		CRC_INNER_LOOP(32, crc, *pdata++);
+#endif /* __mips__ */
+
+	return crc;
+}
+
+#ifdef notdef
+#define CLEN 	1499 	/*  CRC Length */
+#define CBUFSIZ 	(CLEN+4)
+#define CNBUFS		5 /* # of bufs */
+
+void testcrc32(void)
+{
+	uint j, k, l;
+	uint8 *buf;
+	uint len[CNBUFS];
+	uint32 crcr;
+	uint32 crc32tv[CNBUFS] =
+		{0xd2cb1faa, 0xd385c8fa, 0xf5b4f3f3, 0x55789e20, 0x00343110};
+
+	ASSERT((buf = MALLOC(CBUFSIZ*CNBUFS)) != NULL);
+
+	/* step through all possible alignments */
+	for (l = 0; l <= 4; l++) {
+		for (j = 0; j < CNBUFS; j++) {
+			len[j] = CLEN;
+			for (k = 0; k < len[j]; k++)
+				*(buf + j*CBUFSIZ + (k+l)) = (j+k) & 0xff;
+		}
+
+		for (j = 0; j < CNBUFS; j++) {
+			crcr = crc32(buf + j*CBUFSIZ + l, len[j], CRC32_INIT_VALUE);
+			ASSERT(crcr == crc32tv[j]);
+		}
+	}
+
+	MFREE(buf, CBUFSIZ*CNBUFS);
+	return;
+}
+#endif /* notdef */
+
+/*
+ * Advance from the current 1-byte tag/1-byte length/variable-length value
+ * triple, to the next, returning a pointer to the next.
+ * If the current or next TLV is invalid (does not fit in given buffer length),
+ * NULL is returned.
+ * *buflen is not modified if the TLV elt parameter is invalid, or is decremented
+ * by the TLV parameter's length if it is valid.
+ */
+bcm_tlv_t *
+bcm_next_tlv(bcm_tlv_t *elt, int *buflen)
+{
+	int len;
+
+	/* validate current elt */
+	if (!bcm_valid_tlv(elt, *buflen))
+		return NULL;
+
+	/* advance to next elt */
+	len = elt->len;
+	elt = (bcm_tlv_t*)(elt->data + len);
+	*buflen -= (2 + len);
+
+	/* validate next elt */
+	if (!bcm_valid_tlv(elt, *buflen))
+		return NULL;
+
+	return elt;
+}
+
+/*
+ * Traverse a string of 1-byte tag/1-byte length/variable-length value
+ * triples, returning a pointer to the substring whose first element
+ * matches tag
+ */
+bcm_tlv_t *
+bcm_parse_tlvs(void *buf, int buflen, uint key)
+{
+	bcm_tlv_t *elt;
+	int totlen;
+
+	elt = (bcm_tlv_t*)buf;
+	totlen = buflen;
+
+	/* find tagged parameter */
+	while (totlen >= 2) {
+		int len = elt->len;
+
+		/* validate remaining totlen */
+		if ((elt->id == key) && (totlen >= (len + 2)))
+			return (elt);
+
+		elt = (bcm_tlv_t*)((uint8*)elt + (len + 2));
+		totlen -= (len + 2);
+	}
+
+	return NULL;
+}
+
+/*
+ * Traverse a string of 1-byte tag/1-byte length/variable-length value
+ * triples, returning a pointer to the substring whose first element
+ * matches tag.  Stop parsing when we see an element whose ID is greater
+ * than the target key.
+ */
+bcm_tlv_t *
+bcm_parse_ordered_tlvs(void *buf, int buflen, uint key)
+{
+	bcm_tlv_t *elt;
+	int totlen;
+
+	elt = (bcm_tlv_t*)buf;
+	totlen = buflen;
+
+	/* find tagged parameter */
+	while (totlen >= 2) {
+		uint id = elt->id;
+		int len = elt->len;
+
+		/* Punt if we start seeing IDs > than target key */
+		if (id > key)
+			return (NULL);
+
+		/* validate remaining totlen */
+		if ((id == key) && (totlen >= (len + 2)))
+			return (elt);
+
+		elt = (bcm_tlv_t*)((uint8*)elt + (len + 2));
+		totlen -= (len + 2);
+	}
+	return NULL;
+}
+
+#if defined(WLMSG_PRHDRS) || defined(WLMSG_PRPKT) || defined(WLMSG_ASSOC) || \
+	defined(DHD_DEBUG)
+int
+bcm_format_flags(const bcm_bit_desc_t *bd, uint32 flags, char* buf, int len)
+{
+	int i;
+	char* p = buf;
+	char hexstr[16];
+	int slen = 0;
+	uint32 bit;
+	const char* name;
+
+	if (len < 2 || !buf)
+		return 0;
+
+	buf[0] = '\0';
+	len -= 1;
+
+	for (i = 0; flags != 0; i++) {
+		bit = bd[i].bit;
+		name = bd[i].name;
+		if (bit == 0 && flags) {
+			/* print any unnamed bits */
+			sprintf(hexstr, "0x%X", flags);
+			name = hexstr;
+			flags = 0;	/* exit loop */
+		} else if ((flags & bit) == 0)
+			continue;
+		slen += strlen(name);
+		if (len < slen)
+			break;
+		if (p != buf) p += sprintf(p, " "); /* btwn flag space */
+		strcat(p, name);
+		p += strlen(name);
+		flags &= ~bit;
+		len -= slen;
+		slen = 1;	/* account for btwn flag space */
+	}
+
+	/* indicate the str was too short */
+	if (flags != 0) {
+		if (len == 0)
+			p--;	/* overwrite last char */
+		p += sprintf(p, ">");
+	}
+
+	return (int)(p - buf);
+}
+
+/* print bytes formatted as hex to a string. return the resulting string length */
+int
+bcm_format_hex(char *str, const void *bytes, int len)
+{
+	int i;
+	char *p = str;
+	const uint8 *src = (const uint8*)bytes;
+
+	for (i = 0; i < len; i++) {
+		p += sprintf(p, "%02X", *src);
+		src++;
+	}
+	return (int)(p - str);
+}
+
+/* pretty hex print a contiguous buffer */
+void
+prhex(const char *msg, uchar *buf, uint nbytes)
+{
+	char line[128], *p;
+	uint i;
+
+	if (msg && (msg[0] != '\0'))
+		printf("%s:\n", msg);
+
+	p = line;
+	for (i = 0; i < nbytes; i++) {
+		if (i % 16 == 0) {
+			p += sprintf(p, "  %04d: ", i);	/* line prefix */
+		}
+		p += sprintf(p, "%02x ", buf[i]);
+		if (i % 16 == 15) {
+			printf("%s\n", line);		/* flush line */
+			p = line;
+		}
+	}
+
+	/* flush last partial line */
+	if (p != line)
+		printf("%s\n", line);
+}
+#endif 
+
+
+/* Produce a human-readable string for boardrev */
+char *
+bcm_brev_str(uint32 brev, char *buf)
+{
+	if (brev < 0x100)
+		snprintf(buf, 8, "%d.%d", (brev & 0xf0) >> 4, brev & 0xf);
+	else
+		snprintf(buf, 8, "%c%03x", ((brev & 0xf000) == 0x1000) ? 'P' : 'A', brev & 0xfff);
+
+	return (buf);
+}
+
+#define BUFSIZE_TODUMP_ATONCE 512 /* Buffer size */
+
+/* dump large strings to console */
+void
+printbig(char *buf)
+{
+	uint len, max_len;
+	char c;
+
+	len = strlen(buf);
+
+	max_len = BUFSIZE_TODUMP_ATONCE;
+
+	while (len > max_len) {
+		c = buf[max_len];
+		buf[max_len] = '\0';
+		printf("%s", buf);
+		buf[max_len] = c;
+
+		buf += max_len;
+		len -= max_len;
+	}
+	/* print the remaining string */
+	printf("%s\n", buf);
+	return;
+}
+
+/* routine to dump fields in a fileddesc structure */
+uint
+bcmdumpfields(bcmutl_rdreg_rtn read_rtn, void *arg0, uint arg1, struct fielddesc *fielddesc_array,
+	char *buf, uint32 bufsize)
+{
+	uint  filled_len;
+	int len;
+	struct fielddesc *cur_ptr;
+
+	filled_len = 0;
+	cur_ptr = fielddesc_array;
+
+	while (bufsize > 1) {
+		if (cur_ptr->nameandfmt == NULL)
+			break;
+		len = snprintf(buf, bufsize, cur_ptr->nameandfmt,
+		               read_rtn(arg0, arg1, cur_ptr->offset));
+		/* check for snprintf overflow or error */
+		if (len < 0 || (uint32)len >= bufsize)
+			len = bufsize - 1;
+		buf += len;
+		bufsize -= len;
+		filled_len += len;
+		cur_ptr++;
+	}
+	return filled_len;
+}
+
+uint
+bcm_mkiovar(char *name, char *data, uint datalen, char *buf, uint buflen)
+{
+	uint len;
+
+	len = strlen(name) + 1;
+
+	if ((len + datalen) > buflen)
+		return 0;
+
+	strncpy(buf, name, buflen);
+
+	/* append data onto the end of the name string */
+	memcpy(&buf[len], data, datalen);
+	len += datalen;
+
+	return len;
+}
+
+/* Quarter dBm units to mW
+ * Table starts at QDBM_OFFSET, so the first entry is mW for qdBm=153
+ * Table is offset so the last entry is largest mW value that fits in
+ * a uint16.
+ */
+
+#define QDBM_OFFSET 153		/* Offset for first entry */
+#define QDBM_TABLE_LEN 40	/* Table size */
+
+/* Smallest mW value that will round up to the first table entry, QDBM_OFFSET.
+ * Value is ( mW(QDBM_OFFSET - 1) + mW(QDBM_OFFSET) ) / 2
+ */
+#define QDBM_TABLE_LOW_BOUND 6493 /* Low bound */
+
+/* Largest mW value that will round down to the last table entry,
+ * QDBM_OFFSET + QDBM_TABLE_LEN-1.
+ * Value is ( mW(QDBM_OFFSET + QDBM_TABLE_LEN - 1) + mW(QDBM_OFFSET + QDBM_TABLE_LEN) ) / 2.
+ */
+#define QDBM_TABLE_HIGH_BOUND 64938 /* High bound */
+
+static const uint16 nqdBm_to_mW_map[QDBM_TABLE_LEN] = {
+/* qdBm: 	+0 	+1 	+2 	+3 	+4 	+5 	+6 	+7 */
+/* 153: */      6683,	7079,	7499,	7943,	8414,	8913,	9441,	10000,
+/* 161: */      10593,	11220,	11885,	12589,	13335,	14125,	14962,	15849,
+/* 169: */      16788,	17783,	18836,	19953,	21135,	22387,	23714,	25119,
+/* 177: */      26607,	28184,	29854,	31623,	33497,	35481,	37584,	39811,
+/* 185: */      42170,	44668,	47315,	50119,	53088,	56234,	59566,	63096
+};
+
+uint16
+bcm_qdbm_to_mw(uint8 qdbm)
+{
+	uint factor = 1;
+	int idx = qdbm - QDBM_OFFSET;
+
+	if (idx >= QDBM_TABLE_LEN) {
+		/* clamp to max uint16 mW value */
+		return 0xFFFF;
+	}
+
+	/* scale the qdBm index up to the range of the table 0-40
+	 * where an offset of 40 qdBm equals a factor of 10 mW.
+	 */
+	while (idx < 0) {
+		idx += 40;
+		factor *= 10;
+	}
+
+	/* return the mW value scaled down to the correct factor of 10,
+	 * adding in factor/2 to get proper rounding.
+	 */
+	return ((nqdBm_to_mW_map[idx] + factor/2) / factor);
+}
+
+uint8
+bcm_mw_to_qdbm(uint16 mw)
+{
+	uint8 qdbm;
+	int offset;
+	uint mw_uint = mw;
+	uint boundary;
+
+	/* handle boundary case */
+	if (mw_uint <= 1)
+		return 0;
+
+	offset = QDBM_OFFSET;
+
+	/* move mw into the range of the table */
+	while (mw_uint < QDBM_TABLE_LOW_BOUND) {
+		mw_uint *= 10;
+		offset -= 40;
+	}
+
+	for (qdbm = 0; qdbm < QDBM_TABLE_LEN-1; qdbm++) {
+		boundary = nqdBm_to_mW_map[qdbm] + (nqdBm_to_mW_map[qdbm+1] -
+		                                    nqdBm_to_mW_map[qdbm])/2;
+		if (mw_uint < boundary) break;
+	}
+
+	qdbm += (uint8)offset;
+
+	return (qdbm);
+}
+
+
+uint
+bcm_bitcount(uint8 *bitmap, uint length)
+{
+	uint bitcount = 0, i;
+	uint8 tmp;
+	for (i = 0; i < length; i++) {
+		tmp = bitmap[i];
+		while (tmp) {
+			bitcount++;
+			tmp &= (tmp - 1);
+		}
+	}
+	return bitcount;
+}
+
+#ifdef BCMDRIVER
+
+/* Initialization of bcmstrbuf structure */
+void
+bcm_binit(struct bcmstrbuf *b, char *buf, uint size)
+{
+	b->origsize = b->size = size;
+	b->origbuf = b->buf = buf;
+}
+
+/* Buffer sprintf wrapper to guard against buffer overflow */
+int
+bcm_bprintf(struct bcmstrbuf *b, const char *fmt, ...)
+{
+	va_list ap;
+	int r;
+
+	va_start(ap, fmt);
+	r = vsnprintf(b->buf, b->size, fmt, ap);
+
+	/* Non Ansi C99 compliant returns -1,
+	 * Ansi compliant return r >= b->size,
+	 * bcmstdlib returns 0, handle all
+	 */
+	if ((r == -1) || (r >= (int)b->size) || (r == 0)) {
+		b->size = 0;
+	} else {
+		b->size -= r;
+		b->buf += r;
+	}
+
+	va_end(ap);
+
+	return r;
+}
+
+void
+bcm_inc_bytes(uchar *num, int num_bytes, uint8 amount)
+{
+	int i;
+
+	for (i = 0; i < num_bytes; i++) {
+		num[i] += amount;
+		if (num[i] >= amount)
+			break;
+		amount = 1;
+	}
+}
+
+int
+bcm_cmp_bytes(uchar *arg1, uchar *arg2, uint8 nbytes)
+{
+	int i;
+
+	for (i = nbytes - 1; i >= 0; i--) {
+		if (arg1[i] != arg2[i])
+			return (arg1[i] - arg2[i]);
+	}
+	return 0;
+}
+
+void
+bcm_print_bytes(char *name, const uchar *data, int len)
+{
+	int i;
+	int per_line = 0;
+
+	printf("%s: %d \n", name ? name : "", len);
+	for (i = 0; i < len; i++) {
+		printf("%02x ", *data++);
+		per_line++;
+		if (per_line == 16) {
+			per_line = 0;
+			printf("\n");
+		}
+	}
+	printf("\n");
+}
+
+/*
+ * buffer length needed for wlc_format_ssid
+ * 32 SSID chars, max of 4 chars for each SSID char "\xFF", plus NULL.
+ */
+
+#if defined(WLTINYDUMP) || defined(WLMSG_INFORM) || defined(WLMSG_ASSOC) || \
+	defined(WLMSG_PRPKT) || defined(WLMSG_WSEC)
+int
+bcm_format_ssid(char* buf, const uchar ssid[], uint ssid_len)
+{
+	uint i, c;
+	char *p = buf;
+	char *endp = buf + SSID_FMT_BUF_LEN;
+
+	if (ssid_len > DOT11_MAX_SSID_LEN) ssid_len = DOT11_MAX_SSID_LEN;
+
+	for (i = 0; i < ssid_len; i++) {
+		c = (uint)ssid[i];
+		if (c == '\\') {
+			*p++ = '\\';
+			*p++ = '\\';
+		} else if (bcm_isprint((uchar)c)) {
+			*p++ = (char)c;
+		} else {
+			p += snprintf(p, (endp - p), "\\x%02X", c);
+		}
+	}
+	*p = '\0';
+	ASSERT(p < endp);
+
+	return (int)(p - buf);
+}
+#endif 
+
+#endif /* BCMDRIVER */
diff --git a/drivers/net/wireless/bcm4329/bcmwifi.c b/drivers/net/wireless/bcm4329/bcmwifi.c
new file mode 100644
index 000000000000..803acf842a29
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/bcmwifi.c
@@ -0,0 +1,199 @@
+/*
+ * Misc utility routines used by kernel or app-level.
+ * Contents are wifi-specific, used by any kernel or app-level
+ * software that might want wifi things as it grows.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: bcmwifi.c,v 1.18.24.2.4.1 2009/09/25 00:32:01 Exp $
+ */
+
+
+#include <typedefs.h>
+
+#ifdef BCMDRIVER
+#include <osl.h>
+#include <bcmutils.h>
+#define strtoul(nptr, endptr, base) bcm_strtoul((nptr), (endptr), (base))
+#define tolower(c) (bcm_isupper((c)) ? ((c) + 'a' - 'A') : (c))
+#else
+#include <stdio.h>
+#include <stdlib.h>
+#include <ctype.h>
+#endif 
+#include <bcmwifi.h>
+
+#if defined(WIN32) && (defined(BCMDLL) || defined(WLMDLL))
+#include <bcmstdlib.h> 	
+#endif
+
+
+
+
+
+char *
+wf_chspec_ntoa(chanspec_t chspec, char *buf)
+{
+	const char *band, *bw, *sb;
+	uint channel;
+
+	band = "";
+	bw = "";
+	sb = "";
+	channel = CHSPEC_CHANNEL(chspec);
+	
+	if ((CHSPEC_IS2G(chspec) && channel > CH_MAX_2G_CHANNEL) ||
+	    (CHSPEC_IS5G(chspec) && channel <= CH_MAX_2G_CHANNEL))
+		band = (CHSPEC_IS2G(chspec)) ? "b" : "a";
+	if (CHSPEC_IS40(chspec)) {
+		if (CHSPEC_SB_UPPER(chspec)) {
+			sb = "u";
+			channel += CH_10MHZ_APART;
+		} else {
+			sb = "l";
+			channel -= CH_10MHZ_APART;
+		}
+	} else if (CHSPEC_IS10(chspec)) {
+		bw = "n";
+	}
+
+	
+	snprintf(buf, 6, "%d%s%s%s", channel, band, bw, sb);
+	return (buf);
+}
+
+
+chanspec_t
+wf_chspec_aton(char *a)
+{
+	char *endp = NULL;
+	uint channel, band, bw, ctl_sb;
+	char c;
+
+	channel = strtoul(a, &endp, 10);
+
+	
+	if (endp == a)
+		return 0;
+
+	if (channel > MAXCHANNEL)
+		return 0;
+
+	band = ((channel <= CH_MAX_2G_CHANNEL) ? WL_CHANSPEC_BAND_2G : WL_CHANSPEC_BAND_5G);
+	bw = WL_CHANSPEC_BW_20;
+	ctl_sb = WL_CHANSPEC_CTL_SB_NONE;
+
+	a = endp;
+
+	c = tolower(a[0]);
+	if (c == '\0')
+		goto done;
+
+	
+	if (c == 'a' || c == 'b') {
+		band = (c == 'a') ? WL_CHANSPEC_BAND_5G : WL_CHANSPEC_BAND_2G;
+		a++;
+		c = tolower(a[0]);
+		if (c == '\0')
+			goto done;
+	}
+
+	
+	if (c == 'n') {
+		bw = WL_CHANSPEC_BW_10;
+	} else if (c == 'l') {
+		bw = WL_CHANSPEC_BW_40;
+		ctl_sb = WL_CHANSPEC_CTL_SB_LOWER;
+		
+		if (channel <= (MAXCHANNEL - CH_20MHZ_APART))
+			channel += CH_10MHZ_APART;
+		else
+			return 0;
+	} else if (c == 'u') {
+		bw = WL_CHANSPEC_BW_40;
+		ctl_sb = WL_CHANSPEC_CTL_SB_UPPER;
+		
+		if (channel > CH_20MHZ_APART)
+			channel -= CH_10MHZ_APART;
+		else
+			return 0;
+	} else {
+		return 0;
+	}
+
+done:
+	return (channel | band | bw | ctl_sb);
+}
+
+
+int
+wf_mhz2channel(uint freq, uint start_factor)
+{
+	int ch = -1;
+	uint base;
+	int offset;
+
+	
+	if (start_factor == 0) {
+		if (freq >= 2400 && freq <= 2500)
+			start_factor = WF_CHAN_FACTOR_2_4_G;
+		else if (freq >= 5000 && freq <= 6000)
+			start_factor = WF_CHAN_FACTOR_5_G;
+	}
+
+	if (freq == 2484 && start_factor == WF_CHAN_FACTOR_2_4_G)
+		return 14;
+
+	base = start_factor / 2;
+
+	
+	if ((freq < base) || (freq > base + 1000))
+		return -1;
+
+	offset = freq - base;
+	ch = offset / 5;
+
+	
+	if (offset != (ch * 5))
+		return -1;
+
+	
+	if (start_factor == WF_CHAN_FACTOR_2_4_G && (ch < 1 || ch > 13))
+		return -1;
+
+	return ch;
+}
+
+
+int
+wf_channel2mhz(uint ch, uint start_factor)
+{
+	int freq;
+
+	if ((start_factor == WF_CHAN_FACTOR_2_4_G && (ch < 1 || ch > 14)) ||
+	    (ch <= 200))
+		freq = -1;
+	if ((start_factor == WF_CHAN_FACTOR_2_4_G) && (ch == 14))
+		freq = 2484;
+	else
+		freq = ch * 5 + start_factor / 2;
+
+	return freq;
+}
diff --git a/drivers/net/wireless/bcm4329/dhd.h b/drivers/net/wireless/bcm4329/dhd.h
new file mode 100644
index 000000000000..8f0910cfc46f
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/dhd.h
@@ -0,0 +1,472 @@
+/*
+ * Header file describing the internal (inter-module) DHD interfaces.
+ *
+ * Provides type definitions and function prototypes used to link the
+ * DHD OS, bus, and protocol modules.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd.h,v 1.32.4.7.2.4.14.49.4.9 2011/01/14 22:40:45 Exp $
+ */
+
+/****************
+ * Common types *
+ */
+
+#ifndef _dhd_h_
+#define _dhd_h_
+
+#if defined(LINUX)
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/random.h>
+#include <linux/spinlock.h>
+#include <linux/ethtool.h>
+#include <linux/sched.h>
+#include <asm/uaccess.h>
+#include <asm/unaligned.h>
+
+#ifdef CONFIG_HAS_WAKELOCK
+#include <linux/wakelock.h>
+#endif
+
+/* The kernel threading is sdio-specific */
+#else /* LINUX */
+#define ENOMEM		1
+#define EFAULT      2
+#define EINVAL		3
+#define EIO			4
+#define ETIMEDOUT	5
+#define ERESTARTSYS 6
+#endif /* LINUX */
+
+#include <wlioctl.h>
+
+#ifdef DHD_DEBUG
+#ifndef DHD_DEBUG_TRAP
+#define DHD_DEBUG_TRAP
+#endif
+#endif
+
+/* Forward decls */
+struct dhd_bus;
+struct dhd_prot;
+struct dhd_info;
+
+/* The level of bus communication with the dongle */
+enum dhd_bus_state {
+	DHD_BUS_DOWN,		/* Not ready for frame transfers */
+	DHD_BUS_LOAD,		/* Download access only (CPU reset) */
+	DHD_BUS_DATA		/* Ready for frame transfers */
+};
+
+enum dhd_bus_wake_state {
+	WAKE_LOCK_OFF,
+	WAKE_LOCK_PRIV,
+	WAKE_LOCK_DPC,
+	WAKE_LOCK_IOCTL,
+	WAKE_LOCK_DOWNLOAD,
+	WAKE_LOCK_TMOUT,
+	WAKE_LOCK_WATCHDOG,
+	WAKE_LOCK_LINK_DOWN_TMOUT,
+	WAKE_LOCK_PNO_FIND_TMOUT,
+	WAKE_LOCK_SOFTAP_SET,
+	WAKE_LOCK_SOFTAP_STOP,
+	WAKE_LOCK_SOFTAP_START,
+	WAKE_LOCK_SOFTAP_THREAD,
+	WAKE_LOCK_MAX
+};
+enum dhd_prealloc_index {
+	DHD_PREALLOC_PROT = 0,
+	DHD_PREALLOC_RXBUF,
+	DHD_PREALLOC_DATABUF,
+	DHD_PREALLOC_OSL_BUF
+};
+#ifdef DHD_USE_STATIC_BUF
+extern void * dhd_os_prealloc(int section, unsigned long size);
+#endif
+/* Common structure for module and instance linkage */
+typedef struct dhd_pub {
+	/* Linkage ponters */
+	osl_t *osh;		/* OSL handle */
+	struct dhd_bus *bus;	/* Bus module handle */
+	struct dhd_prot *prot;	/* Protocol module handle */
+	struct dhd_info  *info; /* Info module handle */
+
+	/* Internal dhd items */
+	bool up;		/* Driver up/down (to OS) */
+	bool txoff;		/* Transmit flow-controlled */
+	bool dongle_reset;  /* TRUE = DEVRESET put dongle into reset */
+	enum dhd_bus_state busstate;
+	uint hdrlen;		/* Total DHD header length (proto + bus) */
+	uint maxctl;		/* Max size rxctl request from proto to bus */
+	uint rxsz;		/* Rx buffer size bus module should use */
+	uint8 wme_dp;	/* wme discard priority */
+
+	/* Dongle media info */
+	bool iswl;		/* Dongle-resident driver is wl */
+	ulong drv_version;	/* Version of dongle-resident driver */
+	struct ether_addr mac;	/* MAC address obtained from dongle */
+	dngl_stats_t dstats;	/* Stats for dongle-based data */
+
+	/* Additional stats for the bus level */
+	ulong tx_packets;	/* Data packets sent to dongle */
+	ulong tx_multicast;	/* Multicast data packets sent to dongle */
+	ulong tx_errors;	/* Errors in sending data to dongle */
+	ulong tx_ctlpkts;	/* Control packets sent to dongle */
+	ulong tx_ctlerrs;	/* Errors sending control frames to dongle */
+	ulong rx_packets;	/* Packets sent up the network interface */
+	ulong rx_multicast;	/* Multicast packets sent up the network interface */
+	ulong rx_errors;	/* Errors processing rx data packets */
+	ulong rx_ctlpkts;	/* Control frames processed from dongle */
+	ulong rx_ctlerrs;	/* Errors in processing rx control frames */
+	ulong rx_dropped;	/* Packets dropped locally (no memory) */
+	ulong rx_flushed;  /* Packets flushed due to unscheduled sendup thread */
+	ulong wd_dpc_sched;   /* Number of times dhd dpc scheduled by watchdog timer */
+
+	ulong rx_readahead_cnt;	/* Number of packets where header read-ahead was used. */
+	ulong tx_realloc;	/* Number of tx packets we had to realloc for headroom */
+	ulong fc_packets;	/* Number of flow control pkts recvd */
+
+	/* Last error return */
+	int bcmerror;
+	uint tickcnt;
+
+	/* Last error from dongle */
+	int dongle_error;
+
+	/* Suspend disable flag and "in suspend" flag */
+	int suspend_disable_flag; /* "1" to disable all extra powersaving during suspend */
+	int in_suspend;			/* flag set to 1 when early suspend called */
+	int hang_was_sent;	/* flag that message was send at least once */
+#ifdef PNO_SUPPORT
+	int pno_enable;                 /* pno status : "1" is pno enable */
+#endif /* PNO_SUPPORT */
+	int dtim_skip;         /* dtim skip , default 0 means wake each dtim */
+
+	/* Pkt filter defination */
+	char * pktfilter[100];
+	int pktfilter_count;
+
+	wl_country_t dhd_cspec;		/* Current Locale info */
+	char eventmask[WL_EVENTING_MASK_LEN];
+
+#ifdef CONFIG_HAS_WAKELOCK
+	struct wake_lock 	wow_wakelock;
+#endif
+} dhd_pub_t;
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP)
+
+	#define DHD_PM_RESUME_WAIT_INIT(a) DECLARE_WAIT_QUEUE_HEAD(a);
+	#define _DHD_PM_RESUME_WAIT(a, b) do { \
+			int retry = 0; \
+			smp_mb(); \
+			while (dhd_mmc_suspend && retry++ != b) { \
+				wait_event_interruptible_timeout(a, FALSE, 3 * HZ); \
+			} \
+		} 	while (0)
+	#define DHD_PM_RESUME_WAIT(a) 		_DHD_PM_RESUME_WAIT(a, 200)
+	#define DHD_PM_RESUME_WAIT_FOREVER(a) 	_DHD_PM_RESUME_WAIT(a, ~0)
+	#define DHD_PM_RESUME_RETURN_ERROR(a)	do { if (dhd_mmc_suspend) return a; } while (0)
+	#define DHD_PM_RESUME_RETURN		do { if (dhd_mmc_suspend) return; } while (0)
+
+	#define DHD_SPINWAIT_SLEEP_INIT(a) DECLARE_WAIT_QUEUE_HEAD(a);
+	#define SPINWAIT_SLEEP(a, exp, us) do { \
+		uint countdown = (us) + 9999; \
+		while ((exp) && (countdown >= 10000)) { \
+			wait_event_interruptible_timeout(a, FALSE, HZ/100); \
+			countdown -= 10000; \
+		} \
+	} while (0)
+
+#else
+
+	#define DHD_PM_RESUME_WAIT_INIT(a)
+	#define DHD_PM_RESUME_WAIT(a)
+	#define DHD_PM_RESUME_WAIT_FOREVER(a)
+	#define DHD_PM_RESUME_RETURN_ERROR(a)
+	#define DHD_PM_RESUME_RETURN
+
+	#define DHD_SPINWAIT_SLEEP_INIT(a)
+	#define SPINWAIT_SLEEP(a, exp, us)  do { \
+		uint countdown = (us) + 9; \
+		while ((exp) && (countdown >= 10)) { \
+			OSL_DELAY(10);  \
+			countdown -= 10;  \
+		} \
+	} while (0)
+
+#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) */
+
+#define DHD_IF_VIF	0x01	/* Virtual IF (Hidden from user) */
+
+inline static void NETIF_ADDR_LOCK(struct net_device *dev)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29))
+	netif_addr_lock_bh(dev);
+#endif
+}
+
+inline static void NETIF_ADDR_UNLOCK(struct net_device *dev)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29))
+	netif_addr_unlock_bh(dev);
+#endif
+}
+
+/* Wakelock Functions */
+extern int dhd_os_wake_lock(dhd_pub_t *pub);
+extern int dhd_os_wake_unlock(dhd_pub_t *pub);
+extern int dhd_os_wake_lock_timeout(dhd_pub_t *pub);
+extern int dhd_os_wake_lock_timeout_enable(dhd_pub_t *pub);
+
+extern void dhd_os_start_lock(dhd_pub_t *pub);
+extern void dhd_os_start_unlock(dhd_pub_t *pub);
+extern unsigned long dhd_os_spin_lock(dhd_pub_t *pub);
+extern void dhd_os_spin_unlock(dhd_pub_t *pub, unsigned long flags);
+
+typedef struct dhd_if_event {
+	uint8 ifidx;
+	uint8 action;
+	uint8 flags;
+	uint8 bssidx;
+} dhd_if_event_t;
+
+/*
+ * Exported from dhd OS modules (dhd_linux/dhd_ndis)
+ */
+
+/* To allow osl_attach/detach calls from os-independent modules */
+osl_t *dhd_osl_attach(void *pdev, uint bustype);
+void dhd_osl_detach(osl_t *osh);
+
+/* Indication from bus module regarding presence/insertion of dongle.
+ * Return dhd_pub_t pointer, used as handle to OS module in later calls.
+ * Returned structure should have bus and prot pointers filled in.
+ * bus_hdrlen specifies required headroom for bus module header.
+ */
+extern dhd_pub_t *dhd_attach(osl_t *osh, struct dhd_bus *bus, uint bus_hdrlen, void *dev);
+extern int dhd_net_attach(dhd_pub_t *dhdp, int idx);
+
+/* Indication from bus module regarding removal/absence of dongle */
+extern void dhd_detach(dhd_pub_t *dhdp);
+
+/* Indication from bus module to change flow-control state */
+extern void dhd_txflowcontrol(dhd_pub_t *dhdp, int ifidx, bool on);
+
+extern bool dhd_prec_enq(dhd_pub_t *dhdp, struct pktq *q, void *pkt, int prec);
+
+/* Receive frame for delivery to OS.  Callee disposes of rxp. */
+extern void dhd_rx_frame(dhd_pub_t *dhdp, int ifidx, void *rxp, int numpkt);
+
+/* Return pointer to interface name */
+extern char *dhd_ifname(dhd_pub_t *dhdp, int idx);
+
+/* Request scheduling of the bus dpc */
+extern void dhd_sched_dpc(dhd_pub_t *dhdp);
+
+/* Notify tx completion */
+extern void dhd_txcomplete(dhd_pub_t *dhdp, void *txp, bool success);
+
+/* Query ioctl */
+extern int  dhdcdc_query_ioctl(dhd_pub_t *dhd, int ifidx, uint cmd, void *buf, uint len);
+
+/* OS independent layer functions */
+extern int dhd_os_proto_block(dhd_pub_t * pub);
+extern int dhd_os_proto_unblock(dhd_pub_t * pub);
+extern int dhd_os_ioctl_resp_wait(dhd_pub_t * pub, uint * condition, bool * pending);
+extern int dhd_os_ioctl_resp_wake(dhd_pub_t * pub);
+extern unsigned int dhd_os_get_ioctl_resp_timeout(void);
+extern void dhd_os_set_ioctl_resp_timeout(unsigned int timeout_msec);
+extern void * dhd_os_open_image(char * filename);
+extern int dhd_os_get_image_block(char * buf, int len, void * image);
+extern void dhd_os_close_image(void * image);
+extern void dhd_os_wd_timer(void *bus, uint wdtick);
+extern void dhd_os_sdlock(dhd_pub_t * pub);
+extern void dhd_os_sdunlock(dhd_pub_t * pub);
+extern void dhd_os_sdlock_txq(dhd_pub_t * pub);
+extern void dhd_os_sdunlock_txq(dhd_pub_t * pub);
+extern void dhd_os_sdlock_rxq(dhd_pub_t * pub);
+extern void dhd_os_sdunlock_rxq(dhd_pub_t * pub);
+extern void dhd_os_sdlock_sndup_rxq(dhd_pub_t * pub);
+extern void dhd_customer_gpio_wlan_ctrl(int onoff);
+extern int dhd_custom_get_mac_address(unsigned char *buf);
+extern void dhd_os_sdunlock_sndup_rxq(dhd_pub_t * pub);
+extern void dhd_os_sdlock_eventq(dhd_pub_t * pub);
+extern void dhd_os_sdunlock_eventq(dhd_pub_t * pub);
+#ifdef DHD_DEBUG
+extern int write_to_file(dhd_pub_t *dhd, uint8 *buf, int size);
+#endif /* DHD_DEBUG */
+#if defined(OOB_INTR_ONLY)
+extern int dhd_customer_oob_irq_map(unsigned long *irq_flags_ptr);
+#endif /* defined(OOB_INTR_ONLY) */
+extern void dhd_os_sdtxlock(dhd_pub_t * pub);
+extern void dhd_os_sdtxunlock(dhd_pub_t * pub);
+
+int setScheduler(struct task_struct *p, int policy, struct sched_param *param);
+
+typedef struct {
+	uint32 limit;		/* Expiration time (usec) */
+	uint32 increment;	/* Current expiration increment (usec) */
+	uint32 elapsed;		/* Current elapsed time (usec) */
+	uint32 tick;		/* O/S tick time (usec) */
+} dhd_timeout_t;
+
+extern void dhd_timeout_start(dhd_timeout_t *tmo, uint usec);
+extern int dhd_timeout_expired(dhd_timeout_t *tmo);
+
+extern int dhd_ifname2idx(struct dhd_info *dhd, char *name);
+extern uint8 *dhd_bssidx2bssid(dhd_pub_t *dhd, int idx);
+extern int wl_host_event(struct dhd_info *dhd, int *idx, void *pktdata,
+                         wl_event_msg_t *, void **data_ptr);
+extern void wl_event_to_host_order(wl_event_msg_t * evt);
+
+extern void dhd_common_init(void);
+
+extern int dhd_add_if(struct dhd_info *dhd, int ifidx, void *handle,
+	char *name, uint8 *mac_addr, uint32 flags, uint8 bssidx);
+extern void dhd_del_if(struct dhd_info *dhd, int ifidx);
+
+extern void dhd_vif_add(struct dhd_info *dhd, int ifidx, char * name);
+extern void dhd_vif_del(struct dhd_info *dhd, int ifidx);
+
+extern void dhd_event(struct dhd_info *dhd, char *evpkt, int evlen, int ifidx);
+extern void dhd_vif_sendup(struct dhd_info *dhd, int ifidx, uchar *cp, int len);
+
+
+/* Send packet to dongle via data channel */
+extern int dhd_sendpkt(dhd_pub_t *dhdp, int ifidx, void *pkt);
+
+/* Send event to host */
+extern void dhd_sendup_event(dhd_pub_t *dhdp, wl_event_msg_t *event, void *data);
+extern int dhd_bus_devreset(dhd_pub_t *dhdp, uint8 flag);
+extern uint dhd_bus_status(dhd_pub_t *dhdp);
+extern int  dhd_bus_start(dhd_pub_t *dhdp);
+
+extern void print_buf(void *pbuf, int len, int bytes_per_line);
+
+
+typedef enum cust_gpio_modes {
+	WLAN_RESET_ON,
+	WLAN_RESET_OFF,
+	WLAN_POWER_ON,
+	WLAN_POWER_OFF
+} cust_gpio_modes_t;
+
+extern int wl_iw_iscan_set_scan_broadcast_prep(struct net_device *dev, uint flag);
+extern int wl_iw_send_priv_event(struct net_device *dev, char *flag);
+extern int net_os_send_hang_message(struct net_device *dev);
+
+/*
+ * Insmod parameters for debug/test
+ */
+
+/* Watchdog timer interval */
+extern uint dhd_watchdog_ms;
+
+#if defined(DHD_DEBUG)
+/* Console output poll interval */
+extern uint dhd_console_ms;
+#endif /* defined(DHD_DEBUG) */
+
+/* Use interrupts */
+extern uint dhd_intr;
+
+/* Use polling */
+extern uint dhd_poll;
+
+/* ARP offload agent mode */
+extern uint dhd_arp_mode;
+
+/* ARP offload enable */
+extern uint dhd_arp_enable;
+
+/* Pkt filte enable control */
+extern uint dhd_pkt_filter_enable;
+
+/*  Pkt filter init setup */
+extern uint dhd_pkt_filter_init;
+
+/* Pkt filter mode control */
+extern uint dhd_master_mode;
+
+/* Roaming mode control */
+extern uint dhd_roam;
+
+/* Roaming mode control */
+extern uint dhd_radio_up;
+
+/* Initial idletime ticks (may be -1 for immediate idle, 0 for no idle) */
+extern int dhd_idletime;
+#define DHD_IDLETIME_TICKS 1
+
+/* SDIO Drive Strength */
+extern uint dhd_sdiod_drive_strength;
+
+/* Override to force tx queueing all the time */
+extern uint dhd_force_tx_queueing;
+
+/* Default KEEP_ALIVE Period is 55 sec to prevent AP from sending Keep Alive probe frame */
+#define KEEP_ALIVE_PERIOD 55000
+#define NULL_PKT_STR	"null_pkt"
+
+#ifdef SDTEST
+/* Echo packet generator (SDIO), pkts/s */
+extern uint dhd_pktgen;
+
+/* Echo packet len (0 => sawtooth, max 1800) */
+extern uint dhd_pktgen_len;
+#define MAX_PKTGEN_LEN 1800
+#endif
+
+
+/* optionally set by a module_param_string() */
+#define MOD_PARAM_PATHLEN	2048
+extern char fw_path[MOD_PARAM_PATHLEN];
+extern char nv_path[MOD_PARAM_PATHLEN];
+
+/* For supporting multiple interfaces */
+#define DHD_MAX_IFS	16
+#define DHD_DEL_IF	-0xe
+#define DHD_BAD_IF	-0xf
+
+
+extern void dhd_wait_for_event(dhd_pub_t *dhd, bool *lockvar);
+extern void dhd_wait_event_wakeup(dhd_pub_t*dhd);
+
+/* dhd_commn arp offload wrapers */
+extern void dhd_arp_cleanup(dhd_pub_t *dhd);
+int dhd_arp_get_arp_hostip_table(dhd_pub_t *dhd, void *buf, int buflen);
+void dhd_arp_offload_add_ip(dhd_pub_t *dhd, u32 ipaddr);
+
+#define DHD_UNICAST_FILTER_NUM         0
+#define DHD_BROADCAST_FILTER_NUM       1
+#define DHD_MULTICAST4_FILTER_NUM      2
+#define DHD_MULTICAST6_FILTER_NUM      3
+extern int net_os_set_packet_filter(struct net_device *dev, int val);
+extern int net_os_rxfilter_add_remove(struct net_device *dev, int val, int num);
+
+#endif /* _dhd_h_ */
diff --git a/drivers/net/wireless/bcm4329/dhd_bus.h b/drivers/net/wireless/bcm4329/dhd_bus.h
new file mode 100644
index 000000000000..97af41b313d0
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/dhd_bus.h
@@ -0,0 +1,93 @@
+/*
+ * Header file describing the internal (inter-module) DHD interfaces.
+ *
+ * Provides type definitions and function prototypes used to link the
+ * DHD OS, bus, and protocol modules.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_bus.h,v 1.4.6.3.2.3.6.7 2010/08/13 01:35:24 Exp $
+ */
+
+#ifndef _dhd_bus_h_
+#define _dhd_bus_h_
+
+/*
+ * Exported from dhd bus module (dhd_usb, dhd_sdio)
+ */
+
+/* Indicate (dis)interest in finding dongles. */
+extern int dhd_bus_register(void);
+extern void dhd_bus_unregister(void);
+
+/* Download firmware image and nvram image */
+extern bool dhd_bus_download_firmware(struct dhd_bus *bus, osl_t *osh,
+	char *fw_path, char *nv_path);
+
+/* Stop bus module: clear pending frames, disable data flow */
+extern void dhd_bus_stop(struct dhd_bus *bus, bool enforce_mutex);
+
+/* Initialize bus module: prepare for communication w/dongle */
+extern int dhd_bus_init(dhd_pub_t *dhdp, bool enforce_mutex);
+
+/* Send a data frame to the dongle.  Callee disposes of txp. */
+extern int dhd_bus_txdata(struct dhd_bus *bus, void *txp);
+
+/* Send/receive a control message to/from the dongle.
+ * Expects caller to enforce a single outstanding transaction.
+ */
+extern int dhd_bus_txctl(struct dhd_bus *bus, uchar *msg, uint msglen);
+extern int dhd_bus_rxctl(struct dhd_bus *bus, uchar *msg, uint msglen);
+
+/* Watchdog timer function */
+extern bool dhd_bus_watchdog(dhd_pub_t *dhd);
+
+#ifdef DHD_DEBUG
+/* Device console input function */
+extern int dhd_bus_console_in(dhd_pub_t *dhd, uchar *msg, uint msglen);
+#endif /* DHD_DEBUG */
+
+/* Deferred processing for the bus, return TRUE requests reschedule */
+extern bool dhd_bus_dpc(struct dhd_bus *bus);
+extern void dhd_bus_isr(bool * InterruptRecognized, bool * QueueMiniportHandleInterrupt, void *arg);
+
+
+/* Check for and handle local prot-specific iovar commands */
+extern int dhd_bus_iovar_op(dhd_pub_t *dhdp, const char *name,
+                            void *params, int plen, void *arg, int len, bool set);
+
+/* Add bus dump output to a buffer */
+extern void dhd_bus_dump(dhd_pub_t *dhdp, struct bcmstrbuf *strbuf);
+
+/* Clear any bus counters */
+extern void dhd_bus_clearcounts(dhd_pub_t *dhdp);
+
+/* return the dongle chipid */
+extern uint dhd_bus_chip(struct dhd_bus *bus);
+
+/* Set user-specified nvram parameters. */
+extern void dhd_bus_set_nvram_params(struct dhd_bus * bus, const char *nvram_params);
+
+extern void *dhd_bus_pub(struct dhd_bus *bus);
+extern void *dhd_bus_txq(struct dhd_bus *bus);
+extern uint dhd_bus_hdrlen(struct dhd_bus *bus);
+
+#endif /* _dhd_bus_h_ */
diff --git a/drivers/net/wireless/bcm4329/dhd_cdc.c b/drivers/net/wireless/bcm4329/dhd_cdc.c
new file mode 100644
index 000000000000..4bec0b606dc9
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/dhd_cdc.c
@@ -0,0 +1,535 @@
+/*
+ * DHD Protocol Module for CDC and BDC.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_cdc.c,v 1.22.4.2.4.7.2.41 2010/06/23 19:58:18 Exp $
+ *
+ * BDC is like CDC, except it includes a header for data packets to convey
+ * packet priority over the bus, and flags (e.g. to indicate checksum status
+ * for dongle offload).
+ */
+
+#include <typedefs.h>
+#include <osl.h>
+
+#include <bcmutils.h>
+#include <bcmcdc.h>
+#include <bcmendian.h>
+
+#include <dngl_stats.h>
+#include <dhd.h>
+#include <dhd_proto.h>
+#include <dhd_bus.h>
+#include <dhd_dbg.h>
+
+extern int dhd_preinit_ioctls(dhd_pub_t *dhd);
+
+/* Packet alignment for most efficient SDIO (can change based on platform) */
+#ifndef DHD_SDALIGN
+#define DHD_SDALIGN	32
+#endif
+#if !ISPOWEROF2(DHD_SDALIGN)
+#error DHD_SDALIGN is not a power of 2!
+#endif
+
+#define RETRIES 2		/* # of retries to retrieve matching ioctl response */
+#define BUS_HEADER_LEN	(16+DHD_SDALIGN)	/* Must be atleast SDPCM_RESERVE
+				 * defined in dhd_sdio.c (amount of header tha might be added)
+				 * plus any space that might be needed for alignment padding.
+				 */
+#define ROUND_UP_MARGIN	2048 	/* Biggest SDIO block size possible for
+				 * round off at the end of buffer
+				 */
+
+typedef struct dhd_prot {
+	uint16 reqid;
+	uint8 pending;
+	uint32 lastcmd;
+	uint8 bus_header[BUS_HEADER_LEN];
+	cdc_ioctl_t msg;
+	unsigned char buf[WLC_IOCTL_MAXLEN + ROUND_UP_MARGIN];
+} dhd_prot_t;
+
+static int
+dhdcdc_msg(dhd_pub_t *dhd)
+{
+	dhd_prot_t *prot = dhd->prot;
+	int len = ltoh32(prot->msg.len) + sizeof(cdc_ioctl_t);
+	int ret;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	dhd_os_wake_lock(dhd);
+
+	/* NOTE : cdc->msg.len holds the desired length of the buffer to be
+	 *        returned. Only up to CDC_MAX_MSG_SIZE of this buffer area
+	 *	  is actually sent to the dongle
+	 */
+	if (len > CDC_MAX_MSG_SIZE)
+		len = CDC_MAX_MSG_SIZE;
+
+	/* Send request */
+	ret = dhd_bus_txctl(dhd->bus, (uchar*)&prot->msg, len);
+	dhd_os_wake_unlock(dhd);
+	return ret;
+}
+
+static int
+dhdcdc_cmplt(dhd_pub_t *dhd, uint32 id, uint32 len)
+{
+	int ret;
+	dhd_prot_t *prot = dhd->prot;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	do {
+		ret = dhd_bus_rxctl(dhd->bus, (uchar*)&prot->msg, len+sizeof(cdc_ioctl_t));
+		if (ret < 0)
+			break;
+	} while (CDC_IOC_ID(ltoh32(prot->msg.flags)) != id);
+
+	return ret;
+}
+
+int
+dhdcdc_query_ioctl(dhd_pub_t *dhd, int ifidx, uint cmd, void *buf, uint len)
+{
+	dhd_prot_t *prot = dhd->prot;
+	cdc_ioctl_t *msg = &prot->msg;
+	void *info;
+	int ret = 0, retries = 0;
+	uint32 id, flags = 0;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+	DHD_CTL(("%s: cmd %d len %d\n", __FUNCTION__, cmd, len));
+
+
+	/* Respond "bcmerror" and "bcmerrorstr" with local cache */
+	if (cmd == WLC_GET_VAR && buf)
+	{
+		if (!strcmp((char *)buf, "bcmerrorstr"))
+		{
+			strncpy((char *)buf, bcmerrorstr(dhd->dongle_error), BCME_STRLEN);
+			goto done;
+		}
+		else if (!strcmp((char *)buf, "bcmerror"))
+		{
+			*(int *)buf = dhd->dongle_error;
+			goto done;
+		}
+	}
+
+	memset(msg, 0, sizeof(cdc_ioctl_t));
+
+	msg->cmd = htol32(cmd);
+	msg->len = htol32(len);
+	msg->flags = (++prot->reqid << CDCF_IOC_ID_SHIFT);
+	CDC_SET_IF_IDX(msg, ifidx);
+	msg->flags = htol32(msg->flags);
+
+	if (buf)
+		memcpy(prot->buf, buf, len);
+
+	if ((ret = dhdcdc_msg(dhd)) < 0) {
+		if (!dhd->hang_was_sent)
+			DHD_ERROR(("dhdcdc_query_ioctl: dhdcdc_msg failed w/status %d\n", ret));
+		goto done;
+	}
+
+retry:
+	/* wait for interrupt and get first fragment */
+	if ((ret = dhdcdc_cmplt(dhd, prot->reqid, len)) < 0)
+		goto done;
+
+	flags = ltoh32(msg->flags);
+	id = (flags & CDCF_IOC_ID_MASK) >> CDCF_IOC_ID_SHIFT;
+
+	if ((id < prot->reqid) && (++retries < RETRIES))
+		goto retry;
+	if (id != prot->reqid) {
+		DHD_ERROR(("%s: %s: unexpected request id %d (expected %d)\n",
+		           dhd_ifname(dhd, ifidx), __FUNCTION__, id, prot->reqid));
+		ret = -EINVAL;
+		goto done;
+	}
+
+	/* Check info buffer */
+	info = (void*)&msg[1];
+
+	/* Copy info buffer */
+	if (buf)
+	{
+		if (ret < (int)len)
+			len = ret;
+		memcpy(buf, info, len);
+	}
+
+	/* Check the ERROR flag */
+	if (flags & CDCF_IOC_ERROR)
+	{
+		ret = ltoh32(msg->status);
+		/* Cache error from dongle */
+		dhd->dongle_error = ret;
+	}
+
+done:
+	return ret;
+}
+
+int
+dhdcdc_set_ioctl(dhd_pub_t *dhd, int ifidx, uint cmd, void *buf, uint len)
+{
+	dhd_prot_t *prot = dhd->prot;
+	cdc_ioctl_t *msg = &prot->msg;
+	int ret = 0;
+	uint32 flags, id;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+	DHD_CTL(("%s: cmd %d len %d\n", __FUNCTION__, cmd, len));
+
+	if (dhd->busstate == DHD_BUS_DOWN) {
+		DHD_ERROR(("%s : bus is down. we have nothing to do\n", __FUNCTION__));
+		return -EIO;
+	}
+
+	/* don't talk to the dongle if fw is about to be reloaded */
+	if (dhd->hang_was_sent) {
+		DHD_ERROR(("%s: HANG was sent up earlier. Not talking to the chip\n",
+			__FUNCTION__));
+		return -EIO;
+	}
+
+	memset(msg, 0, sizeof(cdc_ioctl_t));
+
+	msg->cmd = htol32(cmd);
+	msg->len = htol32(len);
+	msg->flags = (++prot->reqid << CDCF_IOC_ID_SHIFT) | CDCF_IOC_SET;
+	CDC_SET_IF_IDX(msg, ifidx);
+	msg->flags = htol32(msg->flags);
+
+	if (buf)
+		memcpy(prot->buf, buf, len);
+
+	if ((ret = dhdcdc_msg(dhd)) < 0)
+		goto done;
+
+	if ((ret = dhdcdc_cmplt(dhd, prot->reqid, len)) < 0)
+		goto done;
+
+	flags = ltoh32(msg->flags);
+	id = (flags & CDCF_IOC_ID_MASK) >> CDCF_IOC_ID_SHIFT;
+
+	if (id != prot->reqid) {
+		DHD_ERROR(("%s: %s: unexpected request id %d (expected %d)\n",
+		           dhd_ifname(dhd, ifidx), __FUNCTION__, id, prot->reqid));
+		ret = -EINVAL;
+		goto done;
+	}
+
+	/* Check the ERROR flag */
+	if (flags & CDCF_IOC_ERROR)
+	{
+		ret = ltoh32(msg->status);
+		/* Cache error from dongle */
+		dhd->dongle_error = ret;
+	}
+
+done:
+	return ret;
+}
+
+extern int dhd_bus_interface(struct dhd_bus *bus, uint arg, void* arg2);
+int
+dhd_prot_ioctl(dhd_pub_t *dhd, int ifidx, wl_ioctl_t * ioc, void * buf, int len)
+{
+	dhd_prot_t *prot = dhd->prot;
+	int ret = -1;
+
+	if ((dhd->busstate == DHD_BUS_DOWN) || dhd->hang_was_sent) {
+		DHD_ERROR(("%s : bus is down. we have nothing to do\n", __FUNCTION__));
+		return ret;
+	}
+	dhd_os_proto_block(dhd);
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	ASSERT(len <= WLC_IOCTL_MAXLEN);
+
+	if (len > WLC_IOCTL_MAXLEN)
+		goto done;
+
+	if (prot->pending == TRUE) {
+		DHD_TRACE(("CDC packet is pending!!!! cmd=0x%x (%lu) lastcmd=0x%x (%lu)\n",
+			ioc->cmd, (unsigned long)ioc->cmd, prot->lastcmd,
+			(unsigned long)prot->lastcmd));
+		if ((ioc->cmd == WLC_SET_VAR) || (ioc->cmd == WLC_GET_VAR)) {
+			DHD_TRACE(("iovar cmd=%s\n", (char*)buf));
+		}
+		goto done;
+	}
+
+	prot->pending = TRUE;
+	prot->lastcmd = ioc->cmd;
+	if (ioc->set)
+		ret = dhdcdc_set_ioctl(dhd, ifidx, ioc->cmd, buf, len);
+	else {
+		ret = dhdcdc_query_ioctl(dhd, ifidx, ioc->cmd, buf, len);
+		if (ret > 0)
+			ioc->used = ret - sizeof(cdc_ioctl_t);
+	}
+
+	/* Too many programs assume ioctl() returns 0 on success */
+	if (ret >= 0)
+		ret = 0;
+	else {
+		cdc_ioctl_t *msg = &prot->msg;
+		ioc->needed = ltoh32(msg->len); /* len == needed when set/query fails from dongle */
+	}
+
+	/* Intercept the wme_dp ioctl here */
+	if ((!ret) && (ioc->cmd == WLC_SET_VAR) && (!strcmp(buf, "wme_dp"))) {
+		int slen, val = 0;
+
+		slen = strlen("wme_dp") + 1;
+		if (len >= (int)(slen + sizeof(int)))
+			bcopy(((char *)buf + slen), &val, sizeof(int));
+		dhd->wme_dp = (uint8) ltoh32(val);
+	}
+
+	prot->pending = FALSE;
+
+done:
+	dhd_os_proto_unblock(dhd);
+
+	return ret;
+}
+
+int
+dhd_prot_iovar_op(dhd_pub_t *dhdp, const char *name,
+                  void *params, int plen, void *arg, int len, bool set)
+{
+	return BCME_UNSUPPORTED;
+}
+
+void
+dhd_prot_dump(dhd_pub_t *dhdp, struct bcmstrbuf *strbuf)
+{
+	bcm_bprintf(strbuf, "Protocol CDC: reqid %d\n", dhdp->prot->reqid);
+}
+
+
+void
+dhd_prot_hdrpush(dhd_pub_t *dhd, int ifidx, void *pktbuf)
+{
+#ifdef BDC
+	struct bdc_header *h;
+#endif /* BDC */
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+#ifdef BDC
+	/* Push BDC header used to convey priority for buses that don't */
+
+
+	PKTPUSH(dhd->osh, pktbuf, BDC_HEADER_LEN);
+
+	h = (struct bdc_header *)PKTDATA(dhd->osh, pktbuf);
+
+	h->flags = (BDC_PROTO_VER << BDC_FLAG_VER_SHIFT);
+	if (PKTSUMNEEDED(pktbuf))
+		h->flags |= BDC_FLAG_SUM_NEEDED;
+
+
+	h->priority = (PKTPRIO(pktbuf) & BDC_PRIORITY_MASK);
+	h->flags2 = 0;
+	h->rssi = 0;
+#endif /* BDC */
+	BDC_SET_IF_IDX(h, ifidx);
+}
+
+
+bool
+dhd_proto_fcinfo(dhd_pub_t *dhd, void *pktbuf, uint8 *fcbits)
+{
+#ifdef BDC
+	struct bdc_header *h;
+
+	if (PKTLEN(dhd->osh, pktbuf) < BDC_HEADER_LEN) {
+		DHD_ERROR(("%s: rx data too short (%d < %d)\n",
+			__FUNCTION__, PKTLEN(dhd->osh, pktbuf), BDC_HEADER_LEN));
+		return BCME_ERROR;
+	}
+
+	h = (struct bdc_header *)PKTDATA(dhd->osh, pktbuf);
+
+	*fcbits = h->priority >> BDC_PRIORITY_FC_SHIFT;
+	if ((h->flags2 & BDC_FLAG2_FC_FLAG) == BDC_FLAG2_FC_FLAG)
+		return TRUE;
+#endif
+	return FALSE;
+}
+
+
+int
+dhd_prot_hdrpull(dhd_pub_t *dhd, int *ifidx, void *pktbuf)
+{
+#ifdef BDC
+	struct bdc_header *h;
+#endif
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+#ifdef BDC
+	/* Pop BDC header used to convey priority for buses that don't */
+
+	if (PKTLEN(dhd->osh, pktbuf) < BDC_HEADER_LEN) {
+		DHD_ERROR(("%s: rx data too short (%d < %d)\n", __FUNCTION__,
+		           PKTLEN(dhd->osh, pktbuf), BDC_HEADER_LEN));
+		return BCME_ERROR;
+	}
+
+	h = (struct bdc_header *)PKTDATA(dhd->osh, pktbuf);
+
+	if ((*ifidx = BDC_GET_IF_IDX(h)) >= DHD_MAX_IFS) {
+		DHD_ERROR(("%s: rx data ifnum out of range (%d)\n",
+		           __FUNCTION__, *ifidx));
+		return BCME_ERROR;
+	}
+
+	if (((h->flags & BDC_FLAG_VER_MASK) >> BDC_FLAG_VER_SHIFT) != BDC_PROTO_VER) {
+		DHD_ERROR(("%s: non-BDC packet received, flags 0x%x\n",
+		           dhd_ifname(dhd, *ifidx), h->flags));
+		return BCME_ERROR;
+	}
+
+	if (h->flags & BDC_FLAG_SUM_GOOD) {
+		DHD_INFO(("%s: BDC packet received with good rx-csum, flags 0x%x\n",
+		          dhd_ifname(dhd, *ifidx), h->flags));
+		PKTSETSUMGOOD(pktbuf, TRUE);
+	}
+
+	PKTSETPRIO(pktbuf, (h->priority & BDC_PRIORITY_MASK));
+
+	PKTPULL(dhd->osh, pktbuf, BDC_HEADER_LEN);
+#endif /* BDC */
+
+	return 0;
+}
+
+int
+dhd_prot_attach(dhd_pub_t *dhd)
+{
+	dhd_prot_t *cdc;
+
+#ifndef DHD_USE_STATIC_BUF
+	if (!(cdc = (dhd_prot_t *)MALLOC(dhd->osh, sizeof(dhd_prot_t)))) {
+		DHD_ERROR(("%s: kmalloc failed\n", __FUNCTION__));
+		goto fail;
+	}
+#else
+	if (!(cdc = (dhd_prot_t *)dhd_os_prealloc(DHD_PREALLOC_PROT, sizeof(dhd_prot_t)))) {
+		DHD_ERROR(("%s: kmalloc failed\n", __FUNCTION__));
+		goto fail;
+	}
+#endif /* DHD_USE_STATIC_BUF */
+	memset(cdc, 0, sizeof(dhd_prot_t));
+
+	/* ensure that the msg buf directly follows the cdc msg struct */
+	if ((uintptr)(&cdc->msg + 1) != (uintptr)cdc->buf) {
+		DHD_ERROR(("dhd_prot_t is not correctly defined\n"));
+		goto fail;
+	}
+
+	dhd->prot = cdc;
+#ifdef BDC
+	dhd->hdrlen += BDC_HEADER_LEN;
+#endif
+	dhd->maxctl = WLC_IOCTL_MAXLEN + sizeof(cdc_ioctl_t) + ROUND_UP_MARGIN;
+	return 0;
+
+fail:
+#ifndef DHD_USE_STATIC_BUF
+	if (cdc != NULL)
+		MFREE(dhd->osh, cdc, sizeof(dhd_prot_t));
+#endif
+	return BCME_NOMEM;
+}
+
+/* ~NOTE~ What if another thread is waiting on the semaphore?  Holding it? */
+void
+dhd_prot_detach(dhd_pub_t *dhd)
+{
+#ifndef DHD_USE_STATIC_BUF
+	MFREE(dhd->osh, dhd->prot, sizeof(dhd_prot_t));
+#endif
+	dhd->prot = NULL;
+}
+
+void
+dhd_prot_dstats(dhd_pub_t *dhd)
+{
+	/* No stats from dongle added yet, copy bus stats */
+	dhd->dstats.tx_packets = dhd->tx_packets;
+	dhd->dstats.tx_errors = dhd->tx_errors;
+	dhd->dstats.rx_packets = dhd->rx_packets;
+	dhd->dstats.rx_errors = dhd->rx_errors;
+	dhd->dstats.rx_dropped = dhd->rx_dropped;
+	dhd->dstats.multicast = dhd->rx_multicast;
+	return;
+}
+
+int
+dhd_prot_init(dhd_pub_t *dhd)
+{
+	int ret = 0;
+	char buf[128];
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	dhd_os_proto_block(dhd);
+
+	/* Get the device MAC address */
+	strcpy(buf, "cur_etheraddr");
+	ret = dhdcdc_query_ioctl(dhd, 0, WLC_GET_VAR, buf, sizeof(buf));
+	if (ret < 0) {
+		dhd_os_proto_unblock(dhd);
+		return ret;
+	}
+	memcpy(dhd->mac.octet, buf, ETHER_ADDR_LEN);
+
+	dhd_os_proto_unblock(dhd);
+
+#ifdef EMBEDDED_PLATFORM
+	ret = dhd_preinit_ioctls(dhd);
+#endif /* EMBEDDED_PLATFORM */
+
+	/* Always assumes wl for now */
+	dhd->iswl = TRUE;
+
+	return ret;
+}
+
+void
+dhd_prot_stop(dhd_pub_t *dhd)
+{
+	/* Nothing to do for CDC */
+}
diff --git a/drivers/net/wireless/bcm4329/dhd_common.c b/drivers/net/wireless/bcm4329/dhd_common.c
new file mode 100644
index 000000000000..f7cd372d68c8
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/dhd_common.c
@@ -0,0 +1,2432 @@
+/*
+ * Broadcom Dongle Host Driver (DHD), common DHD core.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_common.c,v 1.5.6.8.2.6.6.69.4.25 2011-02-11 21:16:02 Exp $
+ */
+#include <typedefs.h>
+#include <osl.h>
+
+#include <epivers.h>
+#include <bcmutils.h>
+
+#include <bcmendian.h>
+#include <dngl_stats.h>
+#include <dhd.h>
+#include <dhd_bus.h>
+#include <dhd_proto.h>
+#include <dhd_dbg.h>
+#include <msgtrace.h>
+
+#include <wlioctl.h>
+
+#ifdef SET_RANDOM_MAC_SOFTAP
+#include <linux/random.h>
+#include <linux/jiffies.h>
+#endif
+
+#ifdef GET_CUSTOM_MAC_ENABLE
+int wifi_get_mac_addr(unsigned char *buf);
+#endif /* GET_CUSTOM_MAC_ENABLE */
+
+int dhd_msg_level;
+
+#include <wl_iw.h>
+
+char fw_path[MOD_PARAM_PATHLEN];
+char nv_path[MOD_PARAM_PATHLEN];
+
+/* Last connection success/failure status */
+uint32 dhd_conn_event;
+uint32 dhd_conn_status;
+uint32 dhd_conn_reason;
+
+#define htod32(i) i
+#define htod16(i) i
+#define dtoh32(i) i
+#define dtoh16(i) i
+
+extern int dhdcdc_set_ioctl(dhd_pub_t *dhd, int ifidx, uint cmd, void *buf, uint len);
+extern void dhd_ind_scan_confirm(void *h, bool status);
+extern int dhd_wl_ioctl(dhd_pub_t *dhd, uint cmd, char *buf, uint buflen);
+void dhd_iscan_lock(void);
+void dhd_iscan_unlock(void);
+
+#if defined(SOFTAP)
+extern bool ap_fw_loaded;
+#endif
+#if defined(KEEP_ALIVE)
+int dhd_keep_alive_onoff(dhd_pub_t *dhd, int ka_on);
+#endif /* KEEP_ALIVE */
+
+/* Packet alignment for most efficient SDIO (can change based on platform) */
+#ifndef DHD_SDALIGN
+#define DHD_SDALIGN	32
+#endif
+#if !ISPOWEROF2(DHD_SDALIGN)
+#error DHD_SDALIGN is not a power of 2!
+#endif
+
+#ifdef DHD_DEBUG
+const char dhd_version[] = "Dongle Host Driver, version " EPI_VERSION_STR "\nCompiled on "
+	__DATE__ " at " __TIME__;
+#else
+const char dhd_version[] = "Dongle Host Driver, version " EPI_VERSION_STR;
+#endif
+
+void dhd_set_timer(void *bus, uint wdtick);
+
+/* IOVar table */
+enum {
+	IOV_VERSION = 1,
+	IOV_MSGLEVEL,
+	IOV_BCMERRORSTR,
+	IOV_BCMERROR,
+	IOV_WDTICK,
+	IOV_DUMP,
+#ifdef DHD_DEBUG
+	IOV_CONS,
+	IOV_DCONSOLE_POLL,
+#endif
+	IOV_CLEARCOUNTS,
+	IOV_LOGDUMP,
+	IOV_LOGCAL,
+	IOV_LOGSTAMP,
+	IOV_GPIOOB,
+	IOV_IOCTLTIMEOUT,
+	IOV_LAST
+};
+
+const bcm_iovar_t dhd_iovars[] = {
+	{"version", 	IOV_VERSION,	0,	IOVT_BUFFER,	sizeof(dhd_version) },
+#ifdef DHD_DEBUG
+	{"msglevel",	IOV_MSGLEVEL,	0,	IOVT_UINT32,	0 },
+#endif /* DHD_DEBUG */
+	{"bcmerrorstr", IOV_BCMERRORSTR, 0, IOVT_BUFFER,	BCME_STRLEN },
+	{"bcmerror",	IOV_BCMERROR,	0,	IOVT_INT8,	0 },
+	{"wdtick",	IOV_WDTICK, 0,	IOVT_UINT32,	0 },
+	{"dump",	IOV_DUMP,	0,	IOVT_BUFFER,	DHD_IOCTL_MAXLEN },
+#ifdef DHD_DEBUG
+	{"dconpoll",	IOV_DCONSOLE_POLL, 0,	IOVT_UINT32,	0 },
+	{"cons",	IOV_CONS,	0,	IOVT_BUFFER,	0 },
+#endif
+	{"clearcounts", IOV_CLEARCOUNTS, 0, IOVT_VOID,	0 },
+	{"gpioob",	IOV_GPIOOB,	0,	IOVT_UINT32,	0 },
+	{"ioctl_timeout",	IOV_IOCTLTIMEOUT,	0,	IOVT_UINT32,	0 },
+	{NULL, 0, 0, 0, 0 }
+};
+
+void
+dhd_common_init(void)
+{
+	/* Init global variables at run-time, not as part of the declaration.
+	 * This is required to support init/de-init of the driver. Initialization
+	 * of globals as part of the declaration results in non-deterministic
+	 * behaviour since the value of the globals may be different on the
+	 * first time that the driver is initialized vs subsequent initializations.
+	 */
+	dhd_msg_level = DHD_ERROR_VAL;
+#ifdef CONFIG_BCM4329_FW_PATH
+	strncpy(fw_path, CONFIG_BCM4329_FW_PATH, MOD_PARAM_PATHLEN-1);
+#else
+	fw_path[0] = '\0';
+#endif
+#ifdef CONFIG_BCM4329_NVRAM_PATH
+	strncpy(nv_path, CONFIG_BCM4329_NVRAM_PATH, MOD_PARAM_PATHLEN-1);
+#else
+	nv_path[0] = '\0';
+#endif
+}
+
+static int
+dhd_dump(dhd_pub_t *dhdp, char *buf, int buflen)
+{
+	char eabuf[ETHER_ADDR_STR_LEN];
+
+	struct bcmstrbuf b;
+	struct bcmstrbuf *strbuf = &b;
+
+	bcm_binit(strbuf, buf, buflen);
+
+	/* Base DHD info */
+	bcm_bprintf(strbuf, "%s\n", dhd_version);
+	bcm_bprintf(strbuf, "\n");
+	bcm_bprintf(strbuf, "pub.up %d pub.txoff %d pub.busstate %d\n",
+	            dhdp->up, dhdp->txoff, dhdp->busstate);
+	bcm_bprintf(strbuf, "pub.hdrlen %d pub.maxctl %d pub.rxsz %d\n",
+	            dhdp->hdrlen, dhdp->maxctl, dhdp->rxsz);
+	bcm_bprintf(strbuf, "pub.iswl %d pub.drv_version %ld pub.mac %s\n",
+	            dhdp->iswl, dhdp->drv_version, bcm_ether_ntoa(&dhdp->mac, eabuf));
+	bcm_bprintf(strbuf, "pub.bcmerror %d tickcnt %d\n", dhdp->bcmerror, dhdp->tickcnt);
+
+	bcm_bprintf(strbuf, "dongle stats:\n");
+	bcm_bprintf(strbuf, "tx_packets %ld tx_bytes %ld tx_errors %ld tx_dropped %ld\n",
+	            dhdp->dstats.tx_packets, dhdp->dstats.tx_bytes,
+	            dhdp->dstats.tx_errors, dhdp->dstats.tx_dropped);
+	bcm_bprintf(strbuf, "rx_packets %ld rx_bytes %ld rx_errors %ld rx_dropped %ld\n",
+	            dhdp->dstats.rx_packets, dhdp->dstats.rx_bytes,
+	            dhdp->dstats.rx_errors, dhdp->dstats.rx_dropped);
+	bcm_bprintf(strbuf, "multicast %ld\n", dhdp->dstats.multicast);
+
+	bcm_bprintf(strbuf, "bus stats:\n");
+	bcm_bprintf(strbuf, "tx_packets %ld tx_multicast %ld tx_errors %ld\n",
+	            dhdp->tx_packets, dhdp->tx_multicast, dhdp->tx_errors);
+	bcm_bprintf(strbuf, "tx_ctlpkts %ld tx_ctlerrs %ld\n",
+	            dhdp->tx_ctlpkts, dhdp->tx_ctlerrs);
+	bcm_bprintf(strbuf, "rx_packets %ld rx_multicast %ld rx_errors %ld \n",
+	            dhdp->rx_packets, dhdp->rx_multicast, dhdp->rx_errors);
+	bcm_bprintf(strbuf, "rx_ctlpkts %ld rx_ctlerrs %ld rx_dropped %ld rx_flushed %ld\n",
+	            dhdp->rx_ctlpkts, dhdp->rx_ctlerrs, dhdp->rx_dropped, dhdp->rx_flushed);
+	bcm_bprintf(strbuf, "rx_readahead_cnt %ld tx_realloc %ld fc_packets %ld\n",
+	            dhdp->rx_readahead_cnt, dhdp->tx_realloc, dhdp->fc_packets);
+	bcm_bprintf(strbuf, "wd_dpc_sched %ld\n", dhdp->wd_dpc_sched);
+	bcm_bprintf(strbuf, "\n");
+
+	/* Add any prot info */
+	dhd_prot_dump(dhdp, strbuf);
+	bcm_bprintf(strbuf, "\n");
+
+	/* Add any bus info */
+	dhd_bus_dump(dhdp, strbuf);
+
+	return (!strbuf->size ? BCME_BUFTOOSHORT : 0);
+}
+
+static int
+dhd_doiovar(dhd_pub_t *dhd_pub, const bcm_iovar_t *vi, uint32 actionid, const char *name,
+            void *params, int plen, void *arg, int len, int val_size)
+{
+	int bcmerror = 0;
+	int32 int_val = 0;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if ((bcmerror = bcm_iovar_lencheck(vi, arg, len, IOV_ISSET(actionid))) != 0)
+		goto exit;
+
+	if (plen >= (int)sizeof(int_val))
+		bcopy(params, &int_val, sizeof(int_val));
+
+	switch (actionid) {
+	case IOV_GVAL(IOV_VERSION):
+		/* Need to have checked buffer length */
+		strncpy((char*)arg, dhd_version, len);
+		break;
+
+	case IOV_GVAL(IOV_MSGLEVEL):
+		int_val = (int32)dhd_msg_level;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_MSGLEVEL):
+		dhd_msg_level = int_val;
+		break;
+
+	case IOV_GVAL(IOV_BCMERRORSTR):
+		strncpy((char *)arg, bcmerrorstr(dhd_pub->bcmerror), BCME_STRLEN);
+		((char *)arg)[BCME_STRLEN - 1] = 0x00;
+		break;
+
+	case IOV_GVAL(IOV_BCMERROR):
+		int_val = (int32)dhd_pub->bcmerror;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_WDTICK):
+		int_val = (int32)dhd_watchdog_ms;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_WDTICK):
+		if (!dhd_pub->up) {
+			bcmerror = BCME_NOTUP;
+			break;
+		}
+		dhd_os_wd_timer(dhd_pub, (uint)int_val);
+		break;
+
+	case IOV_GVAL(IOV_DUMP):
+		bcmerror = dhd_dump(dhd_pub, arg, len);
+		break;
+
+#ifdef DHD_DEBUG
+	case IOV_GVAL(IOV_DCONSOLE_POLL):
+		int_val = (int32)dhd_console_ms;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_DCONSOLE_POLL):
+		dhd_console_ms = (uint)int_val;
+		break;
+
+	case IOV_SVAL(IOV_CONS):
+		if (len > 0)
+			bcmerror = dhd_bus_console_in(dhd_pub, arg, len - 1);
+		break;
+#endif
+
+	case IOV_SVAL(IOV_CLEARCOUNTS):
+		dhd_pub->tx_packets = dhd_pub->rx_packets = 0;
+		dhd_pub->tx_errors = dhd_pub->rx_errors = 0;
+		dhd_pub->tx_ctlpkts = dhd_pub->rx_ctlpkts = 0;
+		dhd_pub->tx_ctlerrs = dhd_pub->rx_ctlerrs = 0;
+		dhd_pub->rx_dropped = 0;
+		dhd_pub->rx_readahead_cnt = 0;
+		dhd_pub->tx_realloc = 0;
+		dhd_pub->wd_dpc_sched = 0;
+		memset(&dhd_pub->dstats, 0, sizeof(dhd_pub->dstats));
+		dhd_bus_clearcounts(dhd_pub);
+		break;
+
+
+	case IOV_GVAL(IOV_IOCTLTIMEOUT): {
+		int_val = (int32)dhd_os_get_ioctl_resp_timeout();
+		bcopy(&int_val, arg, sizeof(int_val));
+		break;
+	}
+
+	case IOV_SVAL(IOV_IOCTLTIMEOUT): {
+		if (int_val <= 0)
+			bcmerror = BCME_BADARG;
+		else
+			dhd_os_set_ioctl_resp_timeout((unsigned int)int_val);
+		break;
+	}
+
+
+	default:
+		bcmerror = BCME_UNSUPPORTED;
+		break;
+	}
+
+exit:
+	return bcmerror;
+}
+
+/* Store the status of a connection attempt for later retrieval by an iovar */
+void
+dhd_store_conn_status(uint32 event, uint32 status, uint32 reason)
+{
+	/* Do not overwrite a WLC_E_PRUNE with a WLC_E_SET_SSID
+	 * because an encryption/rsn mismatch results in both events, and
+	 * the important information is in the WLC_E_PRUNE.
+	 */
+	if (!(event == WLC_E_SET_SSID && status == WLC_E_STATUS_FAIL &&
+	      dhd_conn_event == WLC_E_PRUNE)) {
+		dhd_conn_event = event;
+		dhd_conn_status = status;
+		dhd_conn_reason = reason;
+	}
+}
+
+bool
+dhd_prec_enq(dhd_pub_t *dhdp, struct pktq *q, void *pkt, int prec)
+{
+	void *p;
+	int eprec = -1;		/* precedence to evict from */
+	bool discard_oldest;
+
+	/* Fast case, precedence queue is not full and we are also not
+	 * exceeding total queue length
+	 */
+	if (!pktq_pfull(q, prec) && !pktq_full(q)) {
+		pktq_penq(q, prec, pkt);
+		return TRUE;
+	}
+
+	/* Determine precedence from which to evict packet, if any */
+	if (pktq_pfull(q, prec))
+		eprec = prec;
+	else if (pktq_full(q)) {
+		p = pktq_peek_tail(q, &eprec);
+		ASSERT(p);
+		if (eprec > prec)
+			return FALSE;
+	}
+
+	/* Evict if needed */
+	if (eprec >= 0) {
+		/* Detect queueing to unconfigured precedence */
+		ASSERT(!pktq_pempty(q, eprec));
+		discard_oldest = AC_BITMAP_TST(dhdp->wme_dp, eprec);
+		if (eprec == prec && !discard_oldest)
+			return FALSE;		/* refuse newer (incoming) packet */
+		/* Evict packet according to discard policy */
+		p = discard_oldest ? pktq_pdeq(q, eprec) : pktq_pdeq_tail(q, eprec);
+		if (p == NULL) {
+			DHD_ERROR(("%s: pktq_penq() failed, oldest %d.",
+				__FUNCTION__, discard_oldest));
+			ASSERT(p);
+		}
+
+		PKTFREE(dhdp->osh, p, TRUE);
+	}
+
+	/* Enqueue */
+	p = pktq_penq(q, prec, pkt);
+	if (p == NULL) {
+		DHD_ERROR(("%s: pktq_penq() failed.", __FUNCTION__));
+		ASSERT(p);
+	}
+
+	return TRUE;
+}
+
+static int
+dhd_iovar_op(dhd_pub_t *dhd_pub, const char *name,
+             void *params, int plen, void *arg, int len, bool set)
+{
+	int bcmerror = 0;
+	int val_size;
+	const bcm_iovar_t *vi = NULL;
+	uint32 actionid;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	ASSERT(name);
+	ASSERT(len >= 0);
+
+	/* Get MUST have return space */
+	ASSERT(set || (arg && len));
+
+	/* Set does NOT take qualifiers */
+	ASSERT(!set || (!params && !plen));
+
+	if ((vi = bcm_iovar_lookup(dhd_iovars, name)) == NULL) {
+		bcmerror = BCME_UNSUPPORTED;
+		goto exit;
+	}
+
+	DHD_CTL(("%s: %s %s, len %d plen %d\n", __FUNCTION__,
+	         name, (set ? "set" : "get"), len, plen));
+
+	/* set up 'params' pointer in case this is a set command so that
+	 * the convenience int and bool code can be common to set and get
+	 */
+	if (params == NULL) {
+		params = arg;
+		plen = len;
+	}
+
+	if (vi->type == IOVT_VOID)
+		val_size = 0;
+	else if (vi->type == IOVT_BUFFER)
+		val_size = len;
+	else
+		/* all other types are integer sized */
+		val_size = sizeof(int);
+
+	actionid = set ? IOV_SVAL(vi->varid) : IOV_GVAL(vi->varid);
+	bcmerror = dhd_doiovar(dhd_pub, vi, actionid, name, params, plen, arg, len, val_size);
+
+exit:
+	return bcmerror;
+}
+
+int
+dhd_ioctl(dhd_pub_t *dhd_pub, dhd_ioctl_t *ioc, void *buf, uint buflen)
+{
+	int bcmerror = 0;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (!buf) return BCME_BADARG;
+
+	switch (ioc->cmd) {
+	case DHD_GET_MAGIC:
+		if (buflen < sizeof(int))
+			bcmerror = BCME_BUFTOOSHORT;
+		else
+			*(int*)buf = DHD_IOCTL_MAGIC;
+		break;
+
+	case DHD_GET_VERSION:
+		if (buflen < sizeof(int))
+			bcmerror = -BCME_BUFTOOSHORT;
+		else
+			*(int*)buf = DHD_IOCTL_VERSION;
+		break;
+
+	case DHD_GET_VAR:
+	case DHD_SET_VAR: {
+		char *arg;
+		uint arglen;
+
+		/* scan past the name to any arguments */
+		for (arg = buf, arglen = buflen; *arg && arglen; arg++, arglen--);
+
+		if (*arg) {
+			bcmerror = BCME_BUFTOOSHORT;
+			break;
+		}
+
+		/* account for the NUL terminator */
+		arg++, arglen--;
+
+		/* call with the appropriate arguments */
+		if (ioc->cmd == DHD_GET_VAR)
+			bcmerror = dhd_iovar_op(dhd_pub, buf, arg, arglen,
+			buf, buflen, IOV_GET);
+		else
+			bcmerror = dhd_iovar_op(dhd_pub, buf, NULL, 0, arg, arglen, IOV_SET);
+		if (bcmerror != BCME_UNSUPPORTED)
+			break;
+
+		/* not in generic table, try protocol module */
+		if (ioc->cmd == DHD_GET_VAR)
+			bcmerror = dhd_prot_iovar_op(dhd_pub, buf, arg,
+			                             arglen, buf, buflen, IOV_GET);
+		else
+			bcmerror = dhd_prot_iovar_op(dhd_pub, buf,
+			                             NULL, 0, arg, arglen, IOV_SET);
+		if (bcmerror != BCME_UNSUPPORTED)
+			break;
+
+		/* if still not found, try bus module */
+		if (ioc->cmd == DHD_GET_VAR)
+			bcmerror = dhd_bus_iovar_op(dhd_pub, buf,
+			                            arg, arglen, buf, buflen, IOV_GET);
+		else
+			bcmerror = dhd_bus_iovar_op(dhd_pub, buf,
+			                            NULL, 0, arg, arglen, IOV_SET);
+
+		break;
+	}
+
+	default:
+		bcmerror = BCME_UNSUPPORTED;
+	}
+
+	return bcmerror;
+}
+
+
+#ifdef SHOW_EVENTS
+static void
+wl_show_host_event(wl_event_msg_t *event, void *event_data)
+{
+	uint i, status, reason;
+	bool group = FALSE, flush_txq = FALSE, link = FALSE;
+	char *auth_str, *event_name;
+	uchar *buf;
+	char err_msg[256], eabuf[ETHER_ADDR_STR_LEN];
+	static struct {uint event; char *event_name;} event_names[] = {
+		{WLC_E_SET_SSID, "SET_SSID"},
+		{WLC_E_JOIN, "JOIN"},
+		{WLC_E_START, "START"},
+		{WLC_E_AUTH, "AUTH"},
+		{WLC_E_AUTH_IND, "AUTH_IND"},
+		{WLC_E_DEAUTH, "DEAUTH"},
+		{WLC_E_DEAUTH_IND, "DEAUTH_IND"},
+		{WLC_E_ASSOC, "ASSOC"},
+		{WLC_E_ASSOC_IND, "ASSOC_IND"},
+		{WLC_E_REASSOC, "REASSOC"},
+		{WLC_E_REASSOC_IND, "REASSOC_IND"},
+		{WLC_E_DISASSOC, "DISASSOC"},
+		{WLC_E_DISASSOC_IND, "DISASSOC_IND"},
+		{WLC_E_QUIET_START, "START_QUIET"},
+		{WLC_E_QUIET_END, "END_QUIET"},
+		{WLC_E_BEACON_RX, "BEACON_RX"},
+		{WLC_E_LINK, "LINK"},
+		{WLC_E_MIC_ERROR, "MIC_ERROR"},
+		{WLC_E_NDIS_LINK, "NDIS_LINK"},
+		{WLC_E_ROAM, "ROAM"},
+		{WLC_E_TXFAIL, "TXFAIL"},
+		{WLC_E_PMKID_CACHE, "PMKID_CACHE"},
+		{WLC_E_RETROGRADE_TSF, "RETROGRADE_TSF"},
+		{WLC_E_PRUNE, "PRUNE"},
+		{WLC_E_AUTOAUTH, "AUTOAUTH"},
+		{WLC_E_EAPOL_MSG, "EAPOL_MSG"},
+		{WLC_E_SCAN_COMPLETE, "SCAN_COMPLETE"},
+		{WLC_E_ADDTS_IND, "ADDTS_IND"},
+		{WLC_E_DELTS_IND, "DELTS_IND"},
+		{WLC_E_BCNSENT_IND, "BCNSENT_IND"},
+		{WLC_E_BCNRX_MSG, "BCNRX_MSG"},
+		{WLC_E_BCNLOST_MSG, "BCNLOST_MSG"},
+		{WLC_E_ROAM_PREP, "ROAM_PREP"},
+		{WLC_E_PFN_NET_FOUND, "PNO_NET_FOUND"},
+		{WLC_E_PFN_NET_LOST, "PNO_NET_LOST"},
+		{WLC_E_RESET_COMPLETE, "RESET_COMPLETE"},
+		{WLC_E_JOIN_START, "JOIN_START"},
+		{WLC_E_ROAM_START, "ROAM_START"},
+		{WLC_E_ASSOC_START, "ASSOC_START"},
+		{WLC_E_IBSS_ASSOC, "IBSS_ASSOC"},
+		{WLC_E_RADIO, "RADIO"},
+		{WLC_E_PSM_WATCHDOG, "PSM_WATCHDOG"},
+		{WLC_E_PROBREQ_MSG, "PROBREQ_MSG"},
+		{WLC_E_SCAN_CONFIRM_IND, "SCAN_CONFIRM_IND"},
+		{WLC_E_PSK_SUP, "PSK_SUP"},
+		{WLC_E_COUNTRY_CODE_CHANGED, "COUNTRY_CODE_CHANGED"},
+		{WLC_E_EXCEEDED_MEDIUM_TIME, "EXCEEDED_MEDIUM_TIME"},
+		{WLC_E_ICV_ERROR, "ICV_ERROR"},
+		{WLC_E_UNICAST_DECODE_ERROR, "UNICAST_DECODE_ERROR"},
+		{WLC_E_MULTICAST_DECODE_ERROR, "MULTICAST_DECODE_ERROR"},
+		{WLC_E_TRACE, "TRACE"},
+		{WLC_E_ACTION_FRAME, "ACTION FRAME"},
+		{WLC_E_ACTION_FRAME_COMPLETE, "ACTION FRAME TX COMPLETE"},
+		{WLC_E_IF, "IF"},
+		{WLC_E_RSSI, "RSSI"},
+		{WLC_E_PFN_SCAN_COMPLETE, "SCAN_COMPLETE"}
+	};
+	uint event_type, flags, auth_type, datalen;
+	event_type = ntoh32(event->event_type);
+	flags = ntoh16(event->flags);
+	status = ntoh32(event->status);
+	reason = ntoh32(event->reason);
+	auth_type = ntoh32(event->auth_type);
+	datalen = ntoh32(event->datalen);
+	/* debug dump of event messages */
+	sprintf(eabuf, "%02x:%02x:%02x:%02x:%02x:%02x",
+	        (uchar)event->addr.octet[0]&0xff,
+	        (uchar)event->addr.octet[1]&0xff,
+	        (uchar)event->addr.octet[2]&0xff,
+	        (uchar)event->addr.octet[3]&0xff,
+	        (uchar)event->addr.octet[4]&0xff,
+	        (uchar)event->addr.octet[5]&0xff);
+
+	event_name = "UNKNOWN";
+	for (i = 0; i < ARRAYSIZE(event_names); i++) {
+		if (event_names[i].event == event_type)
+			event_name = event_names[i].event_name;
+	}
+
+	DHD_EVENT(("EVENT: %s, event ID = %d\n", event_name, event_type));
+
+	if (flags & WLC_EVENT_MSG_LINK)
+		link = TRUE;
+	if (flags & WLC_EVENT_MSG_GROUP)
+		group = TRUE;
+	if (flags & WLC_EVENT_MSG_FLUSHTXQ)
+		flush_txq = TRUE;
+
+	switch (event_type) {
+	case WLC_E_START:
+	case WLC_E_DEAUTH:
+	case WLC_E_DISASSOC:
+		DHD_EVENT(("MACEVENT: %s, MAC %s\n", event_name, eabuf));
+		break;
+
+	case WLC_E_ASSOC_IND:
+	case WLC_E_REASSOC_IND:
+		DHD_EVENT(("MACEVENT: %s, MAC %s\n", event_name, eabuf));
+		break;
+
+	case WLC_E_ASSOC:
+	case WLC_E_REASSOC:
+		if (status == WLC_E_STATUS_SUCCESS) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, SUCCESS\n", event_name, eabuf));
+		} else if (status == WLC_E_STATUS_TIMEOUT) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, TIMEOUT\n", event_name, eabuf));
+		} else if (status == WLC_E_STATUS_FAIL) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, FAILURE, reason %d\n",
+			       event_name, eabuf, (int)reason));
+		} else {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, unexpected status %d\n",
+			       event_name, eabuf, (int)status));
+		}
+		break;
+
+	case WLC_E_DEAUTH_IND:
+	case WLC_E_DISASSOC_IND:
+		DHD_EVENT(("MACEVENT: %s, MAC %s, reason %d\n", event_name, eabuf, (int)reason));
+		break;
+
+	case WLC_E_AUTH:
+	case WLC_E_AUTH_IND:
+		if (auth_type == DOT11_OPEN_SYSTEM)
+			auth_str = "Open System";
+		else if (auth_type == DOT11_SHARED_KEY)
+			auth_str = "Shared Key";
+		else {
+			sprintf(err_msg, "AUTH unknown: %d", (int)auth_type);
+			auth_str = err_msg;
+		}
+		if (event_type == WLC_E_AUTH_IND) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, %s\n", event_name, eabuf, auth_str));
+		} else if (status == WLC_E_STATUS_SUCCESS) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, %s, SUCCESS\n",
+				event_name, eabuf, auth_str));
+		} else if (status == WLC_E_STATUS_TIMEOUT) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, %s, TIMEOUT\n",
+				event_name, eabuf, auth_str));
+		} else if (status == WLC_E_STATUS_FAIL) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, %s, FAILURE, reason %d\n",
+			       event_name, eabuf, auth_str, (int)reason));
+		}
+
+		break;
+
+	case WLC_E_JOIN:
+	case WLC_E_ROAM:
+	case WLC_E_SET_SSID:
+		if (status == WLC_E_STATUS_SUCCESS) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s\n", event_name, eabuf));
+		} else if (status == WLC_E_STATUS_FAIL) {
+			DHD_EVENT(("MACEVENT: %s, failed\n", event_name));
+		} else if (status == WLC_E_STATUS_NO_NETWORKS) {
+			DHD_EVENT(("MACEVENT: %s, no networks found\n", event_name));
+		} else {
+			DHD_EVENT(("MACEVENT: %s, unexpected status %d\n",
+				event_name, (int)status));
+		}
+		break;
+
+	case WLC_E_BEACON_RX:
+		if (status == WLC_E_STATUS_SUCCESS) {
+			DHD_EVENT(("MACEVENT: %s, SUCCESS\n", event_name));
+		} else if (status == WLC_E_STATUS_FAIL) {
+			DHD_EVENT(("MACEVENT: %s, FAIL\n", event_name));
+		} else {
+			DHD_EVENT(("MACEVENT: %s, status %d\n", event_name, status));
+		}
+		break;
+
+	case WLC_E_LINK:
+		DHD_EVENT(("MACEVENT: %s %s\n", event_name, link?"UP":"DOWN"));
+		break;
+
+	case WLC_E_MIC_ERROR:
+		DHD_EVENT(("MACEVENT: %s, MAC %s, Group %d, Flush %d\n",
+		       event_name, eabuf, group, flush_txq));
+		break;
+
+	case WLC_E_ICV_ERROR:
+	case WLC_E_UNICAST_DECODE_ERROR:
+	case WLC_E_MULTICAST_DECODE_ERROR:
+		DHD_EVENT(("MACEVENT: %s, MAC %s\n",
+		       event_name, eabuf));
+		break;
+
+	case WLC_E_TXFAIL:
+		DHD_EVENT(("MACEVENT: %s, RA %s\n", event_name, eabuf));
+		break;
+
+	case WLC_E_SCAN_COMPLETE:
+	case WLC_E_PMKID_CACHE:
+		DHD_EVENT(("MACEVENT: %s\n", event_name));
+		break;
+
+	case WLC_E_PFN_NET_FOUND:
+	case WLC_E_PFN_NET_LOST:
+	case WLC_E_PFN_SCAN_COMPLETE:
+		DHD_EVENT(("PNOEVENT: %s\n", event_name));
+		break;
+
+	case WLC_E_PSK_SUP:
+	case WLC_E_PRUNE:
+		DHD_EVENT(("MACEVENT: %s, status %d, reason %d\n",
+		           event_name, (int)status, (int)reason));
+		break;
+
+	case WLC_E_TRACE:
+		{
+			static uint32 seqnum_prev = 0;
+			msgtrace_hdr_t hdr;
+			uint32 nblost;
+			char *s, *p;
+
+			buf = (uchar *) event_data;
+			memcpy(&hdr, buf, MSGTRACE_HDRLEN);
+
+			if (hdr.version != MSGTRACE_VERSION) {
+				printf("\nMACEVENT: %s [unsupported version --> "
+				       "dhd version:%d dongle version:%d]\n",
+				       event_name, MSGTRACE_VERSION, hdr.version);
+				/* Reset datalen to avoid display below */
+				datalen = 0;
+				break;
+			}
+
+			/* There are 2 bytes available at the end of data */
+			buf[MSGTRACE_HDRLEN + ntoh16(hdr.len)] = '\0';
+
+			if (ntoh32(hdr.discarded_bytes) || ntoh32(hdr.discarded_printf)) {
+				printf("\nWLC_E_TRACE: [Discarded traces in dongle -->"
+				       "discarded_bytes %d discarded_printf %d]\n",
+				       ntoh32(hdr.discarded_bytes), ntoh32(hdr.discarded_printf));
+			}
+
+			nblost = ntoh32(hdr.seqnum) - seqnum_prev - 1;
+			if (nblost > 0) {
+				printf("\nWLC_E_TRACE: [Event lost --> seqnum %d nblost %d\n",
+				        ntoh32(hdr.seqnum), nblost);
+			}
+			seqnum_prev = ntoh32(hdr.seqnum);
+
+			/* Display the trace buffer. Advance from \n to \n to avoid display big
+			 * printf (issue with Linux printk )
+			 */
+			p = (char *)&buf[MSGTRACE_HDRLEN];
+			while ((s = strstr(p, "\n")) != NULL) {
+				*s = '\0';
+				printf("%s\n", p);
+				p = s + 1;
+			}
+			printf("%s\n", p);
+
+			/* Reset datalen to avoid display below */
+			datalen = 0;
+		}
+		break;
+
+
+	case WLC_E_RSSI:
+		DHD_EVENT(("MACEVENT: %s %d\n", event_name, ntoh32(*((int *)event_data))));
+		break;
+
+	default:
+		DHD_EVENT(("MACEVENT: %s %d, MAC %s, status %d, reason %d, auth %d\n",
+		       event_name, event_type, eabuf, (int)status, (int)reason,
+		       (int)auth_type));
+		break;
+	}
+
+	/* show any appended data */
+	if (datalen) {
+		buf = (uchar *) event_data;
+		DHD_EVENT((" data (%d) : ", datalen));
+		for (i = 0; i < datalen; i++)
+			DHD_EVENT((" 0x%02x ", *buf++));
+		DHD_EVENT(("\n"));
+	}
+}
+#endif /* SHOW_EVENTS */
+
+int
+wl_host_event(struct dhd_info *dhd, int *ifidx, void *pktdata,
+              wl_event_msg_t *event, void **data_ptr)
+{
+	/* check whether packet is a BRCM event pkt */
+	bcm_event_t *pvt_data = (bcm_event_t *)pktdata;
+	char *event_data;
+	uint32 type, status;
+	uint16 flags;
+	int evlen;
+
+	if (bcmp(BRCM_OUI, &pvt_data->bcm_hdr.oui[0], DOT11_OUI_LEN)) {
+		DHD_ERROR(("%s: mismatched OUI, bailing\n", __FUNCTION__));
+		return (BCME_ERROR);
+	}
+
+	/* BRCM event pkt may be unaligned - use xxx_ua to load user_subtype. */
+	if (ntoh16_ua((void *)&pvt_data->bcm_hdr.usr_subtype) != BCMILCP_BCM_SUBTYPE_EVENT) {
+		DHD_ERROR(("%s: mismatched subtype, bailing\n", __FUNCTION__));
+		return (BCME_ERROR);
+	}
+
+	*data_ptr = &pvt_data[1];
+	event_data = *data_ptr;
+
+	/* memcpy since BRCM event pkt may be unaligned. */
+	memcpy(event, &pvt_data->event, sizeof(wl_event_msg_t));
+
+	type = ntoh32_ua((void *)&event->event_type);
+	flags = ntoh16_ua((void *)&event->flags);
+	status = ntoh32_ua((void *)&event->status);
+	evlen = ntoh32_ua((void *)&event->datalen) + sizeof(bcm_event_t);
+
+	switch (type) {
+		case WLC_E_IF:
+			{
+				dhd_if_event_t *ifevent = (dhd_if_event_t *)event_data;
+				DHD_TRACE(("%s: if event\n", __FUNCTION__));
+
+				if (ifevent->ifidx > 0 && ifevent->ifidx < DHD_MAX_IFS)
+				{
+					if (ifevent->action == WLC_E_IF_ADD)
+						dhd_add_if(dhd, ifevent->ifidx,
+							NULL, event->ifname,
+							pvt_data->eth.ether_dhost,
+							ifevent->flags, ifevent->bssidx);
+					else
+						dhd_del_if(dhd, ifevent->ifidx);
+				} else {
+					DHD_ERROR(("%s: Invalid ifidx %d for %s\n",
+						__FUNCTION__, ifevent->ifidx, event->ifname));
+				}
+			}
+			/* send up the if event: btamp user needs it */
+			*ifidx = dhd_ifname2idx(dhd, event->ifname);
+			/* push up to external supp/auth */
+			dhd_event(dhd, (char *)pvt_data, evlen, *ifidx);
+			break;
+
+
+#ifdef P2P
+		case WLC_E_NDIS_LINK:
+			break;
+#endif
+		/* fall through */
+		/* These are what external supplicant/authenticator wants */
+		case WLC_E_LINK:
+		case WLC_E_ASSOC_IND:
+		case WLC_E_REASSOC_IND:
+		case WLC_E_DISASSOC_IND:
+		case WLC_E_MIC_ERROR:
+		default:
+		/* Fall through: this should get _everything_  */
+
+			*ifidx = dhd_ifname2idx(dhd, event->ifname);
+			/* push up to external supp/auth */
+			dhd_event(dhd, (char *)pvt_data, evlen, *ifidx);
+			DHD_TRACE(("%s: MAC event %d, flags %x, status %x\n",
+			           __FUNCTION__, type, flags, status));
+
+			/* put it back to WLC_E_NDIS_LINK */
+			if (type == WLC_E_NDIS_LINK) {
+				uint32 temp;
+
+				temp = ntoh32_ua((void *)&event->event_type);
+				DHD_TRACE(("Converted to WLC_E_LINK type %d\n", temp));
+
+				temp = ntoh32(WLC_E_NDIS_LINK);
+				memcpy((void *)(&pvt_data->event.event_type), &temp,
+					sizeof(pvt_data->event.event_type));
+			}
+			break;
+	}
+
+#ifdef SHOW_EVENTS
+	wl_show_host_event(event, event_data);
+#endif /* SHOW_EVENTS */
+
+	return (BCME_OK);
+}
+
+
+void
+wl_event_to_host_order(wl_event_msg_t *evt)
+{
+	/* Event struct members passed from dongle to host are stored in network
+	 * byte order. Convert all members to host-order.
+	 */
+	evt->event_type = ntoh32(evt->event_type);
+	evt->flags = ntoh16(evt->flags);
+	evt->status = ntoh32(evt->status);
+	evt->reason = ntoh32(evt->reason);
+	evt->auth_type = ntoh32(evt->auth_type);
+	evt->datalen = ntoh32(evt->datalen);
+	evt->version = ntoh16(evt->version);
+}
+
+void print_buf(void *pbuf, int len, int bytes_per_line)
+{
+	int i, j = 0;
+	unsigned char *buf = pbuf;
+
+	if (bytes_per_line == 0) {
+		bytes_per_line = len;
+	}
+
+	for (i = 0; i < len; i++) {
+		printf("%2.2x", *buf++);
+		j++;
+		if (j == bytes_per_line) {
+			printf("\n");
+			j = 0;
+		} else {
+			printf(":");
+		}
+	}
+	printf("\n");
+}
+
+#define strtoul(nptr, endptr, base) bcm_strtoul((nptr), (endptr), (base))
+
+#ifdef PKT_FILTER_SUPPORT
+/* Convert user's input in hex pattern to byte-size mask */
+static int
+wl_pattern_atoh(char *src, char *dst)
+{
+	int i;
+	if (strncmp(src, "0x", 2) != 0 &&
+	    strncmp(src, "0X", 2) != 0) {
+		DHD_ERROR(("Mask invalid format. Needs to start with 0x\n"));
+		return -1;
+	}
+	src = src + 2; /* Skip past 0x */
+	if (strlen(src) % 2 != 0) {
+		DHD_ERROR(("Mask invalid format. Needs to be of even length\n"));
+		return -1;
+	}
+	for (i = 0; *src != '\0'; i++) {
+		char num[3];
+		strncpy(num, src, 2);
+		num[2] = '\0';
+		dst[i] = (uint8)strtoul(num, NULL, 16);
+		src += 2;
+	}
+	return i;
+}
+
+void
+dhd_pktfilter_offload_enable(dhd_pub_t * dhd, char *arg, int enable, int master_mode)
+{
+	char				*argv[8];
+	int					i = 0;
+	const char 			*str;
+	int					buf_len;
+	int					str_len;
+	char				*arg_save = 0, *arg_org = 0;
+	int					rc;
+	char				buf[128];
+	wl_pkt_filter_enable_t	enable_parm;
+	wl_pkt_filter_enable_t	* pkt_filterp;
+
+	if (!arg)
+		return;
+
+	if (!(arg_save = MALLOC(dhd->osh, strlen(arg) + 1))) {
+		DHD_ERROR(("%s: kmalloc failed\n", __FUNCTION__));
+		goto fail;
+	}
+	arg_org = arg_save;
+	memcpy(arg_save, arg, strlen(arg) + 1);
+
+	argv[i] = bcmstrtok(&arg_save, " ", 0);
+
+	i = 0;
+	if (NULL == argv[i]) {
+		DHD_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 = (wl_pkt_filter_enable_t *)(buf + str_len + 1);
+
+	/* Parse packet filter id. */
+	enable_parm.id = htod32(strtoul(argv[i], NULL, 0));
+
+	/* Parse enable/disable value. */
+	enable_parm.enable = htod32(enable);
+
+	buf_len += sizeof(enable_parm);
+	memcpy((char *)pkt_filterp,
+	       &enable_parm,
+	       sizeof(enable_parm));
+
+	/* Enable/disable the specified filter. */
+	rc = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, buf, buf_len);
+	rc = rc >= 0 ? 0 : rc;
+	if (rc)
+		DHD_TRACE(("%s: failed to add pktfilter %s, retcode = %d\n",
+		__FUNCTION__, arg, rc));
+	else
+		DHD_TRACE(("%s: successfully added pktfilter %s\n",
+		__FUNCTION__, arg));
+
+	/* Contorl the master mode */
+	bcm_mkiovar("pkt_filter_mode", (char *)&master_mode, 4, buf, sizeof(buf));
+	rc = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, buf, sizeof(buf));
+	rc = rc >= 0 ? 0 : rc;
+	if (rc)
+		DHD_TRACE(("%s: failed to add pktfilter %s, retcode = %d\n",
+		__FUNCTION__, arg, rc));
+
+fail:
+	if (arg_org)
+		MFREE(dhd->osh, arg_org, strlen(arg) + 1);
+}
+
+void
+dhd_pktfilter_offload_set(dhd_pub_t * dhd, char *arg)
+{
+	const char 			*str;
+	wl_pkt_filter_t		pkt_filter;
+	wl_pkt_filter_t		*pkt_filterp;
+	int					buf_len;
+	int					str_len;
+	int 				rc;
+	uint32				mask_size;
+	uint32				pattern_size;
+	char				*argv[8], * buf = 0;
+	int					i = 0;
+	char				*arg_save = 0, *arg_org = 0;
+#define BUF_SIZE		2048
+
+	if (!arg)
+		return;
+
+	if (!(arg_save = MALLOC(dhd->osh, strlen(arg) + 1))) {
+		DHD_ERROR(("%s: kmalloc failed\n", __FUNCTION__));
+		goto fail;
+	}
+
+	arg_org = arg_save;
+
+	if (!(buf = MALLOC(dhd->osh, BUF_SIZE))) {
+		DHD_ERROR(("%s: kmalloc failed\n", __FUNCTION__));
+		goto fail;
+	}
+
+	memcpy(arg_save, arg, strlen(arg) + 1);
+
+	if (strlen(arg) > BUF_SIZE) {
+		DHD_ERROR(("Not enough buffer %d < %d\n", (int)strlen(arg), (int)sizeof(buf)));
+		goto fail;
+	}
+
+	argv[i] = bcmstrtok(&arg_save, " ", 0);
+	while (argv[i++])
+		argv[i] = bcmstrtok(&arg_save, " ", 0);
+
+	i = 0;
+	if (NULL == argv[i]) {
+		DHD_ERROR(("No args provided\n"));
+		goto fail;
+	}
+
+	str = "pkt_filter_add";
+	str_len = strlen(str);
+	strncpy(buf, str, str_len);
+	buf[ str_len ] = '\0';
+	buf_len = str_len + 1;
+
+	pkt_filterp = (wl_pkt_filter_t *) (buf + str_len + 1);
+
+	/* Parse packet filter id. */
+	pkt_filter.id = htod32(strtoul(argv[i], NULL, 0));
+
+	if (NULL == argv[++i]) {
+		DHD_ERROR(("Polarity not provided\n"));
+		goto fail;
+	}
+
+	/* Parse filter polarity. */
+	pkt_filter.negate_match = htod32(strtoul(argv[i], NULL, 0));
+
+	if (NULL == argv[++i]) {
+		DHD_ERROR(("Filter type not provided\n"));
+		goto fail;
+	}
+
+	/* Parse filter type. */
+	pkt_filter.type = htod32(strtoul(argv[i], NULL, 0));
+
+	if (NULL == argv[++i]) {
+		DHD_ERROR(("Offset not provided\n"));
+		goto fail;
+	}
+
+	/* Parse pattern filter offset. */
+	pkt_filter.u.pattern.offset = htod32(strtoul(argv[i], NULL, 0));
+
+	if (NULL == argv[++i]) {
+		DHD_ERROR(("Bitmask not provided\n"));
+		goto fail;
+	}
+
+	/* Parse pattern filter mask. */
+	mask_size =
+		htod32(wl_pattern_atoh(argv[i], (char *) pkt_filterp->u.pattern.mask_and_pattern));
+
+	if (NULL == argv[++i]) {
+		DHD_ERROR(("Pattern not provided\n"));
+		goto fail;
+	}
+
+	/* Parse pattern filter pattern. */
+	pattern_size =
+		htod32(wl_pattern_atoh(argv[i],
+	         (char *) &pkt_filterp->u.pattern.mask_and_pattern[mask_size]));
+
+	if (mask_size != pattern_size) {
+		DHD_ERROR(("Mask and pattern not the same size\n"));
+		goto fail;
+	}
+
+	pkt_filter.u.pattern.size_bytes = mask_size;
+	buf_len += WL_PKT_FILTER_FIXED_LEN;
+	buf_len += (WL_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,
+	       WL_PKT_FILTER_FIXED_LEN + WL_PKT_FILTER_PATTERN_FIXED_LEN);
+
+	rc = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, buf, buf_len);
+	rc = rc >= 0 ? 0 : rc;
+
+	if (rc)
+		DHD_TRACE(("%s: failed to add pktfilter %s, retcode = %d\n",
+		__FUNCTION__, arg, rc));
+	else
+		DHD_TRACE(("%s: successfully added pktfilter %s\n",
+		__FUNCTION__, arg));
+
+fail:
+	if (arg_org)
+		MFREE(dhd->osh, arg_org, strlen(arg) + 1);
+
+	if (buf)
+		MFREE(dhd->osh, buf, BUF_SIZE);
+}
+#endif
+
+#ifdef ARP_OFFLOAD_SUPPORT
+void
+dhd_arp_offload_set(dhd_pub_t * dhd, int arp_mode)
+{
+	char iovbuf[32];
+	int retcode;
+
+	bcm_mkiovar("arp_ol", (char *)&arp_mode, 4, iovbuf, sizeof(iovbuf));
+	retcode = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf));
+	retcode = retcode >= 0 ? 0 : retcode;
+	if (retcode)
+		DHD_TRACE(("%s: failed to set ARP offload mode to 0x%x, retcode = %d\n",
+		__FUNCTION__, arp_mode, retcode));
+	else
+		DHD_TRACE(("%s: successfully set ARP offload mode to 0x%x\n",
+		__FUNCTION__, arp_mode));
+}
+
+void
+dhd_arp_offload_enable(dhd_pub_t * dhd, int arp_enable)
+{
+	char iovbuf[32];
+	int retcode;
+
+	bcm_mkiovar("arpoe", (char *)&arp_enable, 4, iovbuf, sizeof(iovbuf));
+	retcode = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf));
+	retcode = retcode >= 0 ? 0 : retcode;
+	if (retcode)
+		DHD_TRACE(("%s: failed to enabe ARP offload to %d, retcode = %d\n",
+		__FUNCTION__, arp_enable, retcode));
+	else
+		DHD_TRACE(("%s: successfully enabed ARP offload to %d\n",
+		__FUNCTION__, arp_enable));
+}
+#endif
+
+
+void dhd_arp_cleanup(dhd_pub_t *dhd)
+{
+#ifdef ARP_OFFLOAD_SUPPORT
+	int ret = 0;
+	int iov_len = 0;
+	char iovbuf[128];
+
+	if (dhd == NULL) return;
+
+	dhd_os_proto_block(dhd);
+
+	iov_len = bcm_mkiovar("arp_hostip_clear", 0, 0, iovbuf, sizeof(iovbuf));
+	if ((ret  = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, iov_len)) < 0)
+		DHD_ERROR(("%s failed code %d\n", __FUNCTION__, ret));
+
+	iov_len = bcm_mkiovar("arp_table_clear", 0, 0, iovbuf, sizeof(iovbuf));
+	if ((ret  = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, iov_len)) < 0)
+		DHD_ERROR(("%s failed code %d\n", __FUNCTION__, ret));
+
+	dhd_os_proto_unblock(dhd);
+
+#endif /* ARP_OFFLOAD_SUPPORT */
+}
+
+void dhd_arp_offload_add_ip(dhd_pub_t *dhd, u32 ipaddr)
+{
+#ifdef ARP_OFFLOAD_SUPPORT
+	int iov_len = 0;
+	char iovbuf[32];
+	int retcode;
+
+	dhd_os_proto_block(dhd);
+
+	iov_len = bcm_mkiovar("arp_hostip", (char *)&ipaddr, 4, iovbuf, sizeof(iovbuf));
+	retcode = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, iov_len);
+
+	dhd_os_proto_unblock(dhd);
+
+	if (retcode)
+		DHD_TRACE(("%s: ARP ip addr add failed, retcode = %d\n",
+		__FUNCTION__, retcode));
+	else
+		DHD_TRACE(("%s: ARP ipaddr entry added\n",
+		__FUNCTION__));
+#endif /* ARP_OFFLOAD_SUPPORT */
+}
+
+
+int dhd_arp_get_arp_hostip_table(dhd_pub_t *dhd, void *buf, int buflen)
+{
+#ifdef ARP_OFFLOAD_SUPPORT
+	int retcode;
+	int iov_len = 0;
+
+	if (!buf)
+		return -1;
+
+	dhd_os_proto_block(dhd);
+
+	iov_len = bcm_mkiovar("arp_hostip", 0, 0, buf, buflen);
+	retcode = dhdcdc_query_ioctl(dhd, 0, WLC_GET_VAR, buf, buflen);
+
+	dhd_os_proto_unblock(dhd);
+
+	if (retcode) {
+		DHD_TRACE(("%s: ioctl WLC_GET_VAR error %d\n",
+		__FUNCTION__, retcode));
+
+		return -1;
+	}
+#endif /* ARP_OFFLOAD_SUPPORT */
+	return 0;
+}
+
+
+int
+dhd_preinit_ioctls(dhd_pub_t *dhd)
+{
+	char iovbuf[WL_EVENTING_MASK_LEN + 12];	/*  Room for "event_msgs" + '\0' + bitvec  */
+	uint up = 0;
+	char buf[128], *ptr;
+	uint power_mode = PM_FAST;
+	uint32 dongle_align = DHD_SDALIGN;
+	uint32 glom = 0;
+	uint bcn_timeout = 4;
+	int scan_assoc_time = 40;
+	int scan_unassoc_time = 40;
+	uint32 listen_interval = LISTEN_INTERVAL; /* Default Listen Interval in Beacons */
+#if defined(SOFTAP)
+	uint dtim = 1;
+#endif
+	int ret = 0;
+#ifdef GET_CUSTOM_MAC_ENABLE
+	struct ether_addr ea_addr;
+#endif /* GET_CUSTOM_MAC_ENABLE */
+
+	dhd_os_proto_block(dhd);
+
+#ifdef GET_CUSTOM_MAC_ENABLE
+	/*
+	** Read MAC address from external customer place
+	** NOTE that default mac address has to be present in otp or nvram file
+	** to bring up firmware but unique per board mac address maybe provided
+	** by customer code
+	*/
+	ret = dhd_custom_get_mac_address(ea_addr.octet);
+	if (!ret) {
+		bcm_mkiovar("cur_etheraddr", (void *)&ea_addr, ETHER_ADDR_LEN, buf, sizeof(buf));
+		ret = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, buf, sizeof(buf));
+		if (ret < 0) {
+			DHD_ERROR(("%s: can't set MAC address , error=%d\n", __FUNCTION__, ret));
+		} else
+			memcpy(dhd->mac.octet, (void *)&ea_addr, ETHER_ADDR_LEN);
+	}
+#endif /* GET_CUSTOM_MAC_ENABLE */
+
+#ifdef SET_RANDOM_MAC_SOFTAP
+	if (strstr(fw_path, "apsta") != NULL) {
+		uint rand_mac;
+
+		srandom32((uint)jiffies);
+		rand_mac = random32();
+		iovbuf[0] = 0x02;              /* locally administered bit */
+		iovbuf[1] = 0x1A;
+		iovbuf[2] = 0x11;
+		iovbuf[3] = (unsigned char)(rand_mac & 0x0F) | 0xF0;
+		iovbuf[4] = (unsigned char)(rand_mac >> 8);
+		iovbuf[5] = (unsigned char)(rand_mac >> 16);
+
+		printk("Broadcom Dongle Host Driver mac=%02x:%02x:%02x:%02x:%02x:%02x\n",
+			iovbuf[0], iovbuf[1], iovbuf[2], iovbuf[3], iovbuf[4], iovbuf[5]);
+
+		bcm_mkiovar("cur_etheraddr", (void *)iovbuf, ETHER_ADDR_LEN, buf, sizeof(buf));
+		ret = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, buf, sizeof(buf));
+		if (ret < 0) {
+			DHD_ERROR(("%s: can't set MAC address , error=%d\n", __FUNCTION__, ret));
+		} else
+			memcpy(dhd->mac.octet, iovbuf, ETHER_ADDR_LEN);
+	}
+#endif /* SET_RANDOM_MAC_SOFTAP */
+
+	/* Set Country code */
+	if (dhd->dhd_cspec.ccode[0] != 0) {
+		bcm_mkiovar("country", (char *)&dhd->dhd_cspec, \
+			sizeof(wl_country_t), iovbuf, sizeof(iovbuf));
+		if ((ret = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf))) < 0) {
+			DHD_ERROR(("%s: country code setting failed\n", __FUNCTION__));
+		}
+	}
+
+	/* Set Listen Interval */
+	bcm_mkiovar("assoc_listen", (char *)&listen_interval, 4, iovbuf, sizeof(iovbuf));
+	if ((ret = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf))) < 0)
+		DHD_ERROR(("%s assoc_listen failed %d\n", __FUNCTION__, ret));
+
+	/* query for 'ver' to get version info from firmware */
+	memset(buf, 0, sizeof(buf));
+	ptr = buf;
+	bcm_mkiovar("ver", 0, 0, buf, sizeof(buf));
+	dhdcdc_query_ioctl(dhd, 0, WLC_GET_VAR, buf, sizeof(buf));
+	bcmstrtok(&ptr, "\n", 0);
+	/* Print fw version info */
+	DHD_ERROR(("Firmware version = %s\n", buf));
+
+	/* Set PowerSave mode */
+	dhdcdc_set_ioctl(dhd, 0, WLC_SET_PM, (char *)&power_mode, sizeof(power_mode));
+
+	/* Match Host and Dongle rx alignment */
+	bcm_mkiovar("bus:txglomalign", (char *)&dongle_align, 4, iovbuf, sizeof(iovbuf));
+	dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf));
+
+	/* disable glom option per default */
+	bcm_mkiovar("bus:txglom", (char *)&glom, 4, iovbuf, sizeof(iovbuf));
+	dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf));
+
+	/* Setup timeout if Beacons are lost and roam is off to report link down */
+	bcm_mkiovar("bcn_timeout", (char *)&bcn_timeout, 4, iovbuf, sizeof(iovbuf));
+	dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf));
+
+	/* Enable/Disable build-in roaming to allowed ext supplicant to take of romaing */
+	bcm_mkiovar("roam_off", (char *)&dhd_roam, 4, iovbuf, sizeof(iovbuf));
+	dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf));
+
+#if defined(SOFTAP)
+	if (ap_fw_loaded == TRUE) {
+		dhdcdc_set_ioctl(dhd, 0, WLC_SET_DTIMPRD, (char *)&dtim, sizeof(dtim));
+	}
+#endif
+
+	if (dhd_roam == 0)
+	{
+		/* set internal roaming roaming parameters */
+		int roam_scan_period = 30; /* in sec */
+		int roam_fullscan_period = 120; /* in sec */
+		int roam_trigger = -85;
+		int roam_delta = 15;
+		int band;
+		int band_temp_set = WLC_BAND_2G;
+
+		if (dhdcdc_set_ioctl(dhd, 0, WLC_SET_ROAM_SCAN_PERIOD, \
+			(char *)&roam_scan_period, sizeof(roam_scan_period)) < 0)
+			DHD_ERROR(("%s: roam scan setup failed\n", __FUNCTION__));
+
+		bcm_mkiovar("fullroamperiod", (char *)&roam_fullscan_period, \
+					 4, iovbuf, sizeof(iovbuf));
+		if (dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, \
+			iovbuf, sizeof(iovbuf)) < 0)
+			DHD_ERROR(("%s: roam fullscan setup failed\n", __FUNCTION__));
+
+		if (dhdcdc_query_ioctl(dhd, 0, WLC_GET_BAND, \
+				(char *)&band, sizeof(band)) < 0)
+			DHD_ERROR(("%s: roam delta setting failed\n", __FUNCTION__));
+		else {
+			if ((band == WLC_BAND_AUTO) || (band == WLC_BAND_ALL))
+			{
+				/* temp set band to insert new roams values */
+				if (dhdcdc_set_ioctl(dhd, 0, WLC_SET_BAND, \
+					(char *)&band_temp_set, sizeof(band_temp_set)) < 0)
+					DHD_ERROR(("%s: local band seting failed\n", __FUNCTION__));
+			}
+			if (dhdcdc_set_ioctl(dhd, 0, WLC_SET_ROAM_DELTA, \
+				(char *)&roam_delta, sizeof(roam_delta)) < 0)
+				DHD_ERROR(("%s: roam delta setting failed\n", __FUNCTION__));
+
+			if (dhdcdc_set_ioctl(dhd, 0, WLC_SET_ROAM_TRIGGER, \
+				(char *)&roam_trigger, sizeof(roam_trigger)) < 0)
+				DHD_ERROR(("%s: roam trigger setting failed\n", __FUNCTION__));
+
+			/* Restore original band settinngs */
+			if (dhdcdc_set_ioctl(dhd, 0, WLC_SET_BAND, \
+				(char *)&band, sizeof(band)) < 0)
+				DHD_ERROR(("%s: Original band restore failed\n", __FUNCTION__));
+		}
+	}
+
+	/* Force STA UP */
+	if (dhd_radio_up)
+		dhdcdc_set_ioctl(dhd, 0, WLC_UP, (char *)&up, sizeof(up));
+
+	/* Setup event_msgs */
+	bcm_mkiovar("event_msgs", dhd->eventmask, WL_EVENTING_MASK_LEN, iovbuf, sizeof(iovbuf));
+	dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf));
+
+	dhdcdc_set_ioctl(dhd, 0, WLC_SET_SCAN_CHANNEL_TIME, (char *)&scan_assoc_time,
+		sizeof(scan_assoc_time));
+	dhdcdc_set_ioctl(dhd, 0, WLC_SET_SCAN_UNASSOC_TIME, (char *)&scan_unassoc_time,
+		sizeof(scan_unassoc_time));
+
+#ifdef ARP_OFFLOAD_SUPPORT
+	/* Set and enable ARP offload feature */
+	if (dhd_arp_enable)
+		dhd_arp_offload_set(dhd, dhd_arp_mode);
+	dhd_arp_offload_enable(dhd, dhd_arp_enable);
+#endif /* ARP_OFFLOAD_SUPPORT */
+
+#ifdef PKT_FILTER_SUPPORT
+	{
+		int i;
+		/* Set up pkt filter */
+		if (dhd_pkt_filter_enable) {
+			for (i = 0; i < dhd->pktfilter_count; i++) {
+				dhd_pktfilter_offload_set(dhd, dhd->pktfilter[i]);
+				dhd_pktfilter_offload_enable(dhd, dhd->pktfilter[i],
+					dhd_pkt_filter_init, dhd_master_mode);
+			}
+		}
+	}
+#endif /* PKT_FILTER_SUPPORT */
+
+#if defined(KEEP_ALIVE)
+	{
+	/* Set Keep Alive : be sure to use FW with -keepalive */
+	int res;
+
+	if (ap_fw_loaded == FALSE) {
+		if ((res = dhd_keep_alive_onoff(dhd, 1)) < 0)
+			DHD_ERROR(("%s set keeplive failed %d\n", \
+		__FUNCTION__, res));
+		}
+	}
+#endif
+
+	dhd_os_proto_unblock(dhd);
+
+	return 0;
+}
+
+#ifdef SIMPLE_ISCAN
+
+uint iscan_thread_id;
+iscan_buf_t * iscan_chain = 0;
+
+iscan_buf_t *
+dhd_iscan_allocate_buf(dhd_pub_t *dhd, iscan_buf_t **iscanbuf)
+{
+	iscan_buf_t *iscanbuf_alloc = 0;
+	iscan_buf_t *iscanbuf_head;
+
+	dhd_iscan_lock();
+
+	iscanbuf_alloc = (iscan_buf_t*)MALLOC(dhd->osh, sizeof(iscan_buf_t));
+	if (iscanbuf_alloc == NULL)
+		goto fail;
+
+	iscanbuf_alloc->next = NULL;
+	iscanbuf_head = *iscanbuf;
+
+	DHD_ISCAN(("%s: addr of allocated node = 0x%X"
+		   "addr of iscanbuf_head = 0x%X dhd = 0x%X\n",
+		   __FUNCTION__, iscanbuf_alloc, iscanbuf_head, dhd));
+
+	if (iscanbuf_head == NULL) {
+		*iscanbuf = iscanbuf_alloc;
+		DHD_ISCAN(("%s: Head is allocated\n", __FUNCTION__));
+		goto fail;
+	}
+
+	while (iscanbuf_head->next)
+		iscanbuf_head = iscanbuf_head->next;
+
+	iscanbuf_head->next = iscanbuf_alloc;
+
+fail:
+	dhd_iscan_unlock();
+	return iscanbuf_alloc;
+}
+
+void
+dhd_iscan_free_buf(void *dhdp, iscan_buf_t *iscan_delete)
+{
+	iscan_buf_t *iscanbuf_free = 0;
+	iscan_buf_t *iscanbuf_prv = 0;
+	iscan_buf_t *iscanbuf_cur = iscan_chain;
+	dhd_pub_t *dhd = dhd_bus_pub(dhdp);
+
+	dhd_iscan_lock();
+	/* If iscan_delete is null then delete the entire
+	 * chain or else delete specific one provided
+	 */
+	if (!iscan_delete) {
+		while (iscanbuf_cur) {
+			iscanbuf_free = iscanbuf_cur;
+			iscanbuf_cur = iscanbuf_cur->next;
+			iscanbuf_free->next = 0;
+			MFREE(dhd->osh, iscanbuf_free, sizeof(iscan_buf_t));
+		}
+		iscan_chain = 0;
+	} else {
+		while (iscanbuf_cur) {
+			if (iscanbuf_cur == iscan_delete)
+				break;
+			iscanbuf_prv = iscanbuf_cur;
+			iscanbuf_cur = iscanbuf_cur->next;
+		}
+		if (iscanbuf_prv)
+			iscanbuf_prv->next = iscan_delete->next;
+
+		iscan_delete->next = 0;
+		MFREE(dhd->osh, iscan_delete, sizeof(iscan_buf_t));
+
+		if (!iscanbuf_prv)
+			iscan_chain = 0;
+	}
+	dhd_iscan_unlock();
+}
+
+iscan_buf_t *
+dhd_iscan_result_buf(void)
+{
+	return iscan_chain;
+}
+
+
+
+/*
+* print scan cache
+* print partial iscan_skip list differently
+*/
+int
+dhd_iscan_print_cache(iscan_buf_t *iscan_skip)
+{
+	int i = 0, l = 0;
+	iscan_buf_t *iscan_cur;
+	wl_iscan_results_t *list;
+	wl_scan_results_t *results;
+	wl_bss_info_t UNALIGNED *bi;
+
+	dhd_iscan_lock();
+
+	iscan_cur = dhd_iscan_result_buf();
+
+	while (iscan_cur) {
+		list = (wl_iscan_results_t *)iscan_cur->iscan_buf;
+		if (!list)
+			break;
+
+		results = (wl_scan_results_t *)&list->results;
+		if (!results)
+			break;
+
+		if (results->version != WL_BSS_INFO_VERSION) {
+			DHD_ISCAN(("%s: results->version %d != WL_BSS_INFO_VERSION\n",
+				__FUNCTION__, results->version));
+			goto done;
+		}
+
+		bi = results->bss_info;
+		for (i = 0; i < results->count; i++) {
+			if (!bi)
+				break;
+
+			DHD_ISCAN(("%s[%2.2d:%2.2d] %X:%X:%X:%X:%X:%X\n",
+				iscan_cur != iscan_skip?"BSS":"bss", l, i,
+				bi->BSSID.octet[0], bi->BSSID.octet[1], bi->BSSID.octet[2],
+				bi->BSSID.octet[3], bi->BSSID.octet[4], bi->BSSID.octet[5]));
+
+			bi = (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length));
+		}
+		iscan_cur = iscan_cur->next;
+		l++;
+	}
+
+done:
+	dhd_iscan_unlock();
+	return 0;
+}
+
+/*
+* delete disappeared AP from specific scan cache but skip partial list in iscan_skip
+*/
+int
+dhd_iscan_delete_bss(void *dhdp, void *addr, iscan_buf_t *iscan_skip)
+{
+	int i = 0, j = 0, l = 0;
+	iscan_buf_t *iscan_cur;
+	wl_iscan_results_t *list;
+	wl_scan_results_t *results;
+	wl_bss_info_t UNALIGNED *bi, *bi_new, *bi_next;
+
+	uchar *s_addr = addr;
+
+	dhd_iscan_lock();
+	DHD_ISCAN(("%s: BSS to remove %X:%X:%X:%X:%X:%X\n",
+		__FUNCTION__, s_addr[0], s_addr[1], s_addr[2],
+		s_addr[3], s_addr[4], s_addr[5]));
+
+	iscan_cur = dhd_iscan_result_buf();
+
+	while (iscan_cur) {
+		if (iscan_cur != iscan_skip) {
+			list = (wl_iscan_results_t *)iscan_cur->iscan_buf;
+			if (!list)
+				break;
+
+			results = (wl_scan_results_t *)&list->results;
+			if (!results)
+				break;
+
+			if (results->version != WL_BSS_INFO_VERSION) {
+				DHD_ERROR(("%s: results->version %d != WL_BSS_INFO_VERSION\n",
+				__FUNCTION__, results->version));
+				goto done;
+			}
+
+			bi = results->bss_info;
+			for (i = 0; i < results->count; i++) {
+				if (!bi)
+					break;
+
+				if (!memcmp(bi->BSSID.octet, addr, ETHER_ADDR_LEN)) {
+					DHD_ISCAN(("%s: Del BSS[%2.2d:%2.2d] %X:%X:%X:%X:%X:%X\n",
+					__FUNCTION__, l, i, bi->BSSID.octet[0],
+					bi->BSSID.octet[1], bi->BSSID.octet[2],
+					bi->BSSID.octet[3], bi->BSSID.octet[4],
+					bi->BSSID.octet[5]));
+
+					bi_new = bi;
+					bi = (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length));
+/*
+					if(bi && bi_new) {
+						bcopy(bi, bi_new, results->buflen -
+						dtoh32(bi_new->length));
+						results->buflen -= dtoh32(bi_new->length);
+					}
+*/
+					results->buflen -= dtoh32(bi_new->length);
+					results->count--;
+
+					for (j = i; j < results->count; j++) {
+						if (bi && bi_new) {
+							DHD_ISCAN(("%s: Moved up BSS[%2.2d:%2.2d]"
+							"%X:%X:%X:%X:%X:%X\n",
+							__FUNCTION__, l, j, bi->BSSID.octet[0],
+							bi->BSSID.octet[1], bi->BSSID.octet[2],
+							bi->BSSID.octet[3], bi->BSSID.octet[4],
+							bi->BSSID.octet[5]));
+
+							bi_next = (wl_bss_info_t *)((uintptr)bi +
+								dtoh32(bi->length));
+							bcopy(bi, bi_new, dtoh32(bi->length));
+							bi_new = (wl_bss_info_t *)((uintptr)bi_new +
+								dtoh32(bi_new->length));
+							bi = bi_next;
+						}
+					}
+
+					if (results->count == 0) {
+						/* Prune now empty partial scan list */
+						dhd_iscan_free_buf(dhdp, iscan_cur);
+						goto done;
+					}
+					break;
+				}
+				bi = (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length));
+			}
+		}
+		iscan_cur = iscan_cur->next;
+		l++;
+	}
+
+done:
+	dhd_iscan_unlock();
+	return 0;
+}
+
+int
+dhd_iscan_remove_duplicates(void * dhdp, iscan_buf_t *iscan_cur)
+{
+	int i = 0;
+	wl_iscan_results_t *list;
+	wl_scan_results_t *results;
+	wl_bss_info_t UNALIGNED *bi, *bi_new, *bi_next;
+
+	dhd_iscan_lock();
+
+	DHD_ISCAN(("%s: Scan cache before delete\n",
+		__FUNCTION__));
+	dhd_iscan_print_cache(iscan_cur);
+
+	if (!iscan_cur)
+		goto done;
+
+	list = (wl_iscan_results_t *)iscan_cur->iscan_buf;
+	if (!list)
+		goto done;
+
+	results = (wl_scan_results_t *)&list->results;
+	if (!results)
+		goto done;
+
+	if (results->version != WL_BSS_INFO_VERSION) {
+		DHD_ERROR(("%s: results->version %d != WL_BSS_INFO_VERSION\n",
+			__FUNCTION__, results->version));
+		goto done;
+	}
+
+	bi = results->bss_info;
+	for (i = 0; i < results->count; i++) {
+		if (!bi)
+			break;
+
+		DHD_ISCAN(("%s: Find dups for BSS[%2.2d] %X:%X:%X:%X:%X:%X\n",
+			__FUNCTION__, i, bi->BSSID.octet[0], bi->BSSID.octet[1], bi->BSSID.octet[2],
+			bi->BSSID.octet[3], bi->BSSID.octet[4], bi->BSSID.octet[5]));
+
+		dhd_iscan_delete_bss(dhdp, bi->BSSID.octet, iscan_cur);
+
+		bi = (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length));
+	}
+
+done:
+	DHD_ISCAN(("%s: Scan cache after delete\n", __FUNCTION__));
+	dhd_iscan_print_cache(iscan_cur);
+	dhd_iscan_unlock();
+	return 0;
+}
+
+void
+dhd_iscan_ind_scan_confirm(void *dhdp, bool status)
+{
+
+	dhd_ind_scan_confirm(dhdp, status);
+}
+
+int
+dhd_iscan_request(void * dhdp, uint16 action)
+{
+	int rc;
+	wl_iscan_params_t params;
+	dhd_pub_t *dhd = dhd_bus_pub(dhdp);
+	char buf[WLC_IOCTL_SMLEN];
+
+
+	memset(&params, 0, sizeof(wl_iscan_params_t));
+	memcpy(&params.params.bssid, &ether_bcast, ETHER_ADDR_LEN);
+
+	params.params.bss_type = DOT11_BSSTYPE_ANY;
+	params.params.scan_type = DOT11_SCANTYPE_ACTIVE;
+
+	params.params.nprobes = htod32(-1);
+	params.params.active_time = htod32(-1);
+	params.params.passive_time = htod32(-1);
+	params.params.home_time = htod32(-1);
+	params.params.channel_num = htod32(0);
+
+	params.version = htod32(ISCAN_REQ_VERSION);
+	params.action = htod16(action);
+	params.scan_duration = htod16(0);
+
+	bcm_mkiovar("iscan", (char *)&params, sizeof(wl_iscan_params_t), buf, WLC_IOCTL_SMLEN);
+	rc = dhd_wl_ioctl(dhdp, WLC_SET_VAR, buf, WLC_IOCTL_SMLEN);
+
+	return rc;
+}
+
+static int
+dhd_iscan_get_partial_result(void *dhdp, uint *scan_count)
+{
+	wl_iscan_results_t *list_buf;
+	wl_iscan_results_t list;
+	wl_scan_results_t *results;
+	iscan_buf_t *iscan_cur;
+	int status = -1;
+	dhd_pub_t *dhd = dhd_bus_pub(dhdp);
+	int rc;
+
+
+	iscan_cur = dhd_iscan_allocate_buf(dhd, &iscan_chain);
+	if (!iscan_cur) {
+		DHD_ERROR(("%s: Failed to allocate node\n", __FUNCTION__));
+		dhd_iscan_free_buf(dhdp, 0);
+		dhd_iscan_request(dhdp, WL_SCAN_ACTION_ABORT);
+		goto fail;
+	}
+
+	dhd_iscan_lock();
+
+	memset(iscan_cur->iscan_buf, 0, WLC_IW_ISCAN_MAXLEN);
+	list_buf = (wl_iscan_results_t*)iscan_cur->iscan_buf;
+	results = &list_buf->results;
+	results->buflen = WL_ISCAN_RESULTS_FIXED_SIZE;
+	results->version = 0;
+	results->count = 0;
+
+	memset(&list, 0, sizeof(list));
+	list.results.buflen = htod32(WLC_IW_ISCAN_MAXLEN);
+	bcm_mkiovar("iscanresults", (char *)&list, WL_ISCAN_RESULTS_FIXED_SIZE,
+		iscan_cur->iscan_buf, WLC_IW_ISCAN_MAXLEN);
+	rc = dhd_wl_ioctl(dhdp, WLC_GET_VAR, iscan_cur->iscan_buf, WLC_IW_ISCAN_MAXLEN);
+
+	results->buflen = dtoh32(results->buflen);
+	results->version = dtoh32(results->version);
+	*scan_count = results->count = dtoh32(results->count);
+	status = dtoh32(list_buf->status);
+
+	dhd_iscan_unlock();
+
+	if (!(*scan_count))
+		dhd_iscan_free_buf(dhdp, iscan_cur);
+	else
+		dhd_iscan_remove_duplicates(dhdp, iscan_cur);
+
+
+fail:
+	return status;
+}
+
+#endif
+
+/* Function to estimate possible DTIM_SKIP value */
+int dhd_get_dtim_skip(dhd_pub_t *dhd)
+{
+	int bcn_li_dtim;
+	char buf[128];
+	int ret;
+	int dtim_assoc = 0;
+
+	if ((dhd->dtim_skip == 0) || (dhd->dtim_skip == 1))
+		bcn_li_dtim = 3;
+	else
+		bcn_li_dtim = dhd->dtim_skip;
+
+	/* Read DTIM value if associated */
+	memset(buf, 0, sizeof(buf));
+	bcm_mkiovar("dtim_assoc", 0, 0, buf, sizeof(buf));
+	if ((ret = dhdcdc_query_ioctl(dhd, 0, WLC_GET_VAR, buf, sizeof(buf))) < 0) {
+		DHD_ERROR(("%s failed code %d\n", __FUNCTION__, ret));
+		bcn_li_dtim = 1;
+		goto exit;
+	}
+	else
+		dtim_assoc = dtoh32(*(int *)buf);
+
+	DHD_ERROR(("%s bcn_li_dtim=%d DTIM=%d Listen=%d\n", \
+			 __FUNCTION__, bcn_li_dtim, dtim_assoc, LISTEN_INTERVAL));
+
+	/* if not assocated just eixt */
+	if (dtim_assoc == 0) {
+		goto exit;
+	}
+
+	/* check if sta listen interval fits into AP dtim */
+	if (dtim_assoc > LISTEN_INTERVAL) {
+		/* AP DTIM to big for our Listen Interval : no dtim skiping */
+		bcn_li_dtim = 1;
+		DHD_ERROR(("%s DTIM=%d > Listen=%d : too big ...\n", \
+				 __FUNCTION__, dtim_assoc, LISTEN_INTERVAL));
+		goto exit;
+	}
+
+	if ((bcn_li_dtim * dtim_assoc) > LISTEN_INTERVAL) {
+		/* Round up dtim_skip to fit into STAs Listen Interval */
+		bcn_li_dtim = (int)(LISTEN_INTERVAL / dtim_assoc);
+		DHD_TRACE(("%s agjust dtim_skip as %d\n", __FUNCTION__, bcn_li_dtim));
+	}
+
+exit:
+	return bcn_li_dtim;
+}
+
+#ifdef PNO_SUPPORT
+int dhd_pno_clean(dhd_pub_t *dhd)
+{
+	char iovbuf[128];
+	int pfn_enabled = 0;
+	int iov_len = 0;
+	int ret;
+
+	/* Disable pfn */
+	iov_len = bcm_mkiovar("pfn", (char *)&pfn_enabled, 4, iovbuf, sizeof(iovbuf));
+	if ((ret = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf))) >= 0) {
+		/* clear pfn */
+		iov_len = bcm_mkiovar("pfnclear", 0, 0, iovbuf, sizeof(iovbuf));
+		if (iov_len) {
+			if ((ret = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, iov_len)) < 0) {
+				DHD_ERROR(("%s failed code %d\n", __FUNCTION__, ret));
+			}
+		}
+		else {
+			ret = -1;
+			DHD_ERROR(("%s failed code %d\n", __FUNCTION__, iov_len));
+		}
+	}
+	else
+		DHD_ERROR(("%s failed code %d\n", __FUNCTION__, ret));
+
+	return ret;
+}
+
+int dhd_pno_enable(dhd_pub_t *dhd, int pfn_enabled)
+{
+	char iovbuf[128];
+	uint8 bssid[6];
+	int ret = -1;
+
+	if ((!dhd) && ((pfn_enabled != 0) || (pfn_enabled != 1))) {
+		DHD_ERROR(("%s error exit\n", __FUNCTION__));
+		return ret;
+	}
+
+	memset(iovbuf, 0, sizeof(iovbuf));
+
+	/* Check if disassoc to enable pno */
+	if ((pfn_enabled) && \
+		((ret = dhdcdc_set_ioctl(dhd, 0, WLC_GET_BSSID, \
+				 (char *)&bssid, ETHER_ADDR_LEN)) == BCME_NOTASSOCIATED)) {
+		DHD_TRACE(("%s pno enable called in disassoc mode\n", __FUNCTION__));
+	}
+	else if (pfn_enabled) {
+		DHD_ERROR(("%s pno enable called in assoc mode ret=%d\n", \
+			__FUNCTION__, ret));
+		return ret;
+	}
+
+	/* Enable/disable PNO */
+	if ((ret = bcm_mkiovar("pfn", (char *)&pfn_enabled, 4, iovbuf, sizeof(iovbuf))) > 0) {
+		if ((ret = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf))) < 0) {
+			DHD_ERROR(("%s failed for error=%d\n", __FUNCTION__, ret));
+			return ret;
+		}
+		else {
+			dhd->pno_enable = pfn_enabled;
+			DHD_TRACE(("%s set pno as %d\n", __FUNCTION__, dhd->pno_enable));
+		}
+	}
+	else DHD_ERROR(("%s failed err=%d\n", __FUNCTION__, ret));
+
+	return ret;
+}
+
+/* Function to execute combined scan */
+int
+dhd_pno_set(dhd_pub_t *dhd, wlc_ssid_t* ssids_local, int nssid, ushort scan_fr, \
+			int pno_repeat, int pno_freq_expo_max)
+{
+	int err = -1;
+	char iovbuf[128];
+	int k, i;
+	wl_pfn_param_t pfn_param;
+	wl_pfn_t	pfn_element;
+
+	DHD_TRACE(("%s nssid=%d nchan=%d\n", __FUNCTION__, nssid, scan_fr));
+
+	if ((!dhd) && (!ssids_local)) {
+		DHD_ERROR(("%s error exit\n", __FUNCTION__));
+		err = -1;
+	}
+
+	/* Check for broadcast ssid */
+	for (k = 0; k < nssid; k++) {
+		if (!ssids_local[k].SSID_len) {
+			DHD_ERROR(("%d: Broadcast SSID is ilegal for PNO setting\n", k));
+			return err;
+		}
+	}
+/* #define  PNO_DUMP 1 */
+#ifdef PNO_DUMP
+	{
+		int j;
+		for (j = 0; j < nssid; j++) {
+			DHD_ERROR(("%d: scan  for  %s size =%d\n", j,
+				ssids_local[j].SSID, ssids_local[j].SSID_len));
+		}
+	}
+#endif /* PNO_DUMP */
+
+	/* clean up everything */
+	if  ((err = dhd_pno_clean(dhd)) < 0) {
+		DHD_ERROR(("%s failed error=%d\n", __FUNCTION__, err));
+		return err;
+	}
+	memset(&pfn_param, 0, sizeof(pfn_param));
+	memset(&pfn_element, 0, sizeof(pfn_element));
+
+	/* set pfn parameters */
+	pfn_param.version = htod32(PFN_VERSION);
+	pfn_param.flags = htod16((PFN_LIST_ORDER << SORT_CRITERIA_BIT));
+
+	/* check and set extra pno params */
+	if ((pno_repeat != 0) || (pno_freq_expo_max != 0)) {
+		pfn_param.flags |= htod16(ENABLE << ENABLE_ADAPTSCAN_BIT);
+		pfn_param.repeat_scan = htod32(pno_repeat);
+		pfn_param.max_freq_adjust = htod32(pno_freq_expo_max);
+	}
+
+	/* set up pno scan fr */
+	if (scan_fr  != 0)
+		pfn_param.scan_freq = htod32(scan_fr);
+
+	if (pfn_param.scan_freq > PNO_SCAN_MAX_FW_SEC) {
+		DHD_ERROR(("%s pno freq above %d sec\n", __FUNCTION__, PNO_SCAN_MAX_FW_SEC));
+		return err;
+	}
+	if (pfn_param.scan_freq < PNO_SCAN_MIN_FW_SEC) {
+		DHD_ERROR(("%s pno freq less %d sec\n", __FUNCTION__, PNO_SCAN_MIN_FW_SEC));
+		return err;
+	}
+
+	bcm_mkiovar("pfn_set", (char *)&pfn_param, sizeof(pfn_param), iovbuf, sizeof(iovbuf));
+	dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf));
+
+	/* set all pfn ssid */
+	for (i = 0; i < nssid; i++) {
+
+		pfn_element.bss_type = htod32(DOT11_BSSTYPE_INFRASTRUCTURE);
+		pfn_element.auth = (DOT11_OPEN_SYSTEM);
+		pfn_element.infra = htod32(1);
+
+		memcpy((char *)pfn_element.ssid.SSID, ssids_local[i].SSID, ssids_local[i].SSID_len);
+		pfn_element.ssid.SSID_len = ssids_local[i].SSID_len;
+
+		if ((err =
+		bcm_mkiovar("pfn_add", (char *)&pfn_element,
+			sizeof(pfn_element), iovbuf, sizeof(iovbuf))) > 0) {
+			if ((err =
+			dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf))) < 0) {
+				DHD_ERROR(("%s failed for i=%d error=%d\n",
+					__FUNCTION__, i, err));
+				return err;
+			}
+			else
+				DHD_ERROR(("%s set OK with PNO time=%d repeat=%d max_adjust=%d\n", \
+					__FUNCTION__, pfn_param.scan_freq, \
+					pfn_param.repeat_scan, pfn_param.max_freq_adjust));
+		}
+		else DHD_ERROR(("%s failed err=%d\n", __FUNCTION__, err));
+	}
+
+	/* Enable PNO */
+	/* dhd_pno_enable(dhd, 1); */
+	return err;
+}
+
+int dhd_pno_get_status(dhd_pub_t *dhd)
+{
+	int ret = -1;
+
+	if (!dhd)
+		return ret;
+	else
+		return (dhd->pno_enable);
+}
+
+#endif /* PNO_SUPPORT */
+
+#if defined(KEEP_ALIVE)
+int dhd_keep_alive_onoff(dhd_pub_t *dhd, int ka_on)
+{
+	char buf[256];
+	char *buf_ptr = buf;
+	wl_keep_alive_pkt_t keep_alive_pkt;
+	char * str;
+	int str_len, buf_len;
+	int res = 0;
+	int keep_alive_period = KEEP_ALIVE_PERIOD; /* in ms */
+
+	DHD_TRACE(("%s: ka:%d\n", __FUNCTION__, ka_on));
+
+	if (ka_on) { /* on suspend */
+		keep_alive_pkt.period_msec = keep_alive_period;
+
+	} else {
+		/* on resume, turn off keep_alive packets  */
+		keep_alive_pkt.period_msec = 0;
+	}
+
+	/* IOC var name  */
+	str = "keep_alive";
+	str_len = strlen(str);
+	strncpy(buf, str, str_len);
+	buf[str_len] = '\0';
+	buf_len = str_len + 1;
+
+	/* set ptr to IOCTL payload after the var name */
+	buf_ptr += buf_len; /* include term Z */
+
+	/* copy Keep-alive attributes from local var keep_alive_pkt */
+	str = NULL_PKT_STR;
+	keep_alive_pkt.len_bytes = strlen(str);
+
+	memcpy(buf_ptr, &keep_alive_pkt, WL_KEEP_ALIVE_FIXED_LEN);
+	buf_ptr += WL_KEEP_ALIVE_FIXED_LEN;
+
+	/* copy packet data */
+	memcpy(buf_ptr, str, keep_alive_pkt.len_bytes);
+	buf_len += (WL_KEEP_ALIVE_FIXED_LEN + keep_alive_pkt.len_bytes);
+
+	res = dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, buf, buf_len);
+	return res;
+}
+#endif /* defined(KEEP_ALIVE) */
+
+#if defined(CSCAN)
+
+/* Androd ComboSCAN support */
+/*
+ *  data parsing from ComboScan tlv list
+*/
+int
+wl_iw_parse_data_tlv(char** list_str, void *dst, int dst_size, const char token,
+                     int input_size, int *bytes_left)
+{
+	char* str = *list_str;
+	uint16 short_temp;
+	uint32 int_temp;
+
+	if ((list_str == NULL) || (*list_str == NULL) ||(bytes_left == NULL) || (*bytes_left < 0)) {
+		DHD_ERROR(("%s error paramters\n", __FUNCTION__));
+		return -1;
+	}
+
+	/* Clean all dest bytes */
+	memset(dst, 0, dst_size);
+	while (*bytes_left > 0) {
+
+		if (str[0] != token) {
+			DHD_TRACE(("%s NOT Type=%d get=%d left_parse=%d \n",
+				__FUNCTION__, token, str[0], *bytes_left));
+			return -1;
+		}
+
+		*bytes_left -= 1;
+		str += 1;
+
+		if (input_size == 1) {
+			memcpy(dst, str, input_size);
+		}
+		else if (input_size == 2) {
+			memcpy(dst, (char *)htod16(memcpy(&short_temp, str, input_size)),
+				input_size);
+		}
+		else if (input_size == 4) {
+			memcpy(dst, (char *)htod32(memcpy(&int_temp, str, input_size)),
+				input_size);
+		}
+
+		*bytes_left -= input_size;
+		str += input_size;
+		*list_str = str;
+		return 1;
+	}
+	return 1;
+}
+
+/*
+ *  channel list parsing from cscan tlv list
+*/
+int
+wl_iw_parse_channel_list_tlv(char** list_str, uint16* channel_list,
+                             int channel_num, int *bytes_left)
+{
+	char* str = *list_str;
+	int idx = 0;
+
+	if ((list_str == NULL) || (*list_str == NULL) ||(bytes_left == NULL) || (*bytes_left < 0)) {
+		DHD_ERROR(("%s error paramters\n", __FUNCTION__));
+		return -1;
+	}
+
+	while (*bytes_left > 0) {
+
+		if (str[0] != CSCAN_TLV_TYPE_CHANNEL_IE) {
+			*list_str = str;
+			DHD_TRACE(("End channel=%d left_parse=%d %d\n", idx, *bytes_left, str[0]));
+			return idx;
+		}
+		/* Get proper CSCAN_TLV_TYPE_CHANNEL_IE */
+		*bytes_left -= 1;
+		str += 1;
+
+		if (str[0] == 0) {
+			/* All channels */
+			channel_list[idx] = 0x0;
+		}
+		else {
+			channel_list[idx] = (uint16)str[0];
+			DHD_TRACE(("%s channel=%d \n", __FUNCTION__,  channel_list[idx]));
+		}
+		*bytes_left -= 1;
+		str += 1;
+
+		if (idx++ > 255) {
+			DHD_ERROR(("%s Too many channels \n", __FUNCTION__));
+			return -1;
+		}
+	}
+
+	*list_str = str;
+	return idx;
+}
+
+/*
+ *  SSIDs list parsing from cscan tlv list
+ */
+int
+wl_iw_parse_ssid_list_tlv(char** list_str, wlc_ssid_t* ssid, int max, int *bytes_left)
+{
+	char* str =  *list_str;
+	int idx = 0;
+
+	if ((list_str == NULL) || (*list_str == NULL) || (*bytes_left < 0)) {
+		DHD_ERROR(("%s error paramters\n", __FUNCTION__));
+		return -1;
+	}
+
+	while (*bytes_left > 0) {
+
+		if (str[0] != CSCAN_TLV_TYPE_SSID_IE) {
+			*list_str = str;
+			DHD_TRACE(("nssid=%d left_parse=%d %d\n", idx, *bytes_left, str[0]));
+			return idx;
+		}
+
+		/* Get proper CSCAN_TLV_TYPE_SSID_IE */
+		*bytes_left -= 1;
+		str += 1;
+
+		if (str[0] == 0) {
+			/* Broadcast SSID */
+			ssid[idx].SSID_len = 0;
+			memset((char*)ssid[idx].SSID, 0x0, DOT11_MAX_SSID_LEN);
+			*bytes_left -= 1;
+			str += 1;
+
+			DHD_TRACE(("BROADCAST SCAN  left=%d\n", *bytes_left));
+		}
+		else if (str[0] <= DOT11_MAX_SSID_LEN) {
+			/* Get proper SSID size */
+			ssid[idx].SSID_len = str[0];
+			*bytes_left -= 1;
+			str += 1;
+
+			/* Get SSID */
+			if (ssid[idx].SSID_len > *bytes_left) {
+				DHD_ERROR(("%s out of memory range len=%d but left=%d\n",
+				__FUNCTION__, ssid[idx].SSID_len, *bytes_left));
+				return -1;
+			}
+
+			memcpy((char*)ssid[idx].SSID, str, ssid[idx].SSID_len);
+
+			*bytes_left -= ssid[idx].SSID_len;
+			str += ssid[idx].SSID_len;
+
+			DHD_TRACE(("%s :size=%d left=%d\n",
+				(char*)ssid[idx].SSID, ssid[idx].SSID_len, *bytes_left));
+		}
+		else {
+			DHD_ERROR(("### SSID size more that %d\n", str[0]));
+			return -1;
+		}
+
+		if (idx++ >  max) {
+			DHD_ERROR(("%s number of SSIDs more that %d\n", __FUNCTION__, idx));
+			return -1;
+		}
+	}
+
+	*list_str = str;
+	return idx;
+}
+
+/* Parse a comma-separated list from list_str into ssid array, starting
+ * at index idx.  Max specifies size of the ssid array.  Parses ssids
+ * and returns updated idx; if idx >= max not all fit, the excess have
+ * not been copied.  Returns -1 on empty string, or on ssid too long.
+ */
+int
+wl_iw_parse_ssid_list(char** list_str, wlc_ssid_t* ssid, int idx, int max)
+{
+	char* str, *ptr;
+
+	if ((list_str == NULL) || (*list_str == NULL))
+		return -1;
+
+	for (str = *list_str; str != NULL; str = ptr) {
+
+		/* check for next TAG */
+		if (!strncmp(str, GET_CHANNEL, strlen(GET_CHANNEL))) {
+			*list_str	 = str + strlen(GET_CHANNEL);
+			return idx;
+		}
+
+		if ((ptr = strchr(str, ',')) != NULL) {
+			*ptr++ = '\0';
+		}
+
+		if (strlen(str) > DOT11_MAX_SSID_LEN) {
+			DHD_ERROR(("ssid <%s> exceeds %d\n", str, DOT11_MAX_SSID_LEN));
+			return -1;
+		}
+
+		if (strlen(str) == 0)
+			ssid[idx].SSID_len = 0;
+
+		if (idx < max) {
+			strcpy((char*)ssid[idx].SSID, str);
+			ssid[idx].SSID_len = strlen(str);
+		}
+		idx++;
+	}
+	return idx;
+}
+
+/*
+ * Parse channel list from iwpriv CSCAN
+ */
+int
+wl_iw_parse_channel_list(char** list_str, uint16* channel_list, int channel_num)
+{
+	int num;
+	int val;
+	char* str;
+	char* endptr = NULL;
+
+	if ((list_str == NULL)||(*list_str == NULL))
+		return -1;
+
+	str = *list_str;
+	num = 0;
+	while (strncmp(str, GET_NPROBE, strlen(GET_NPROBE))) {
+		val = (int)strtoul(str, &endptr, 0);
+		if (endptr == str) {
+			printf("could not parse channel number starting at"
+				" substring \"%s\" in list:\n%s\n",
+				str, *list_str);
+			return -1;
+		}
+		str = endptr + strspn(endptr, " ,");
+
+		if (num == channel_num) {
+			DHD_ERROR(("too many channels (more than %d) in channel list:\n%s\n",
+				channel_num, *list_str));
+			return -1;
+		}
+
+		channel_list[num++] = (uint16)val;
+	}
+	*list_str = str;
+	return num;
+}
+
+#endif
diff --git a/drivers/net/wireless/bcm4329/dhd_custom_gpio.c b/drivers/net/wireless/bcm4329/dhd_custom_gpio.c
new file mode 100644
index 000000000000..4d32863e2982
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/dhd_custom_gpio.c
@@ -0,0 +1,272 @@
+/*
+* Customer code to add GPIO control during WLAN start/stop
+* Copyright (C) 1999-2010, Broadcom Corporation
+* 
+*      Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2 (the "GPL"),
+* available at http://www.broadcom.com/licenses/GPLv2.php, with the
+* following added to such license:
+* 
+*      As a special exception, the copyright holders of this software give you
+* permission to link this software with independent modules, and to copy and
+* distribute the resulting executable under terms of your choice, provided that
+* you also meet, for each linked independent module, the terms and conditions of
+* the license of that module.  An independent module is a module which is not
+* derived from this software.  The special exception does not apply to any
+* modifications of the software.
+* 
+*      Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a license
+* other than the GPL, without Broadcom's express prior written consent.
+*
+* $Id: dhd_custom_gpio.c,v 1.1.4.8.4.4 2011/01/20 20:23:09 Exp $
+*/
+
+
+#include <typedefs.h>
+#include <linuxver.h>
+#include <osl.h>
+#include <bcmutils.h>
+
+#include <dngl_stats.h>
+#include <dhd.h>
+
+#include <wlioctl.h>
+#include <wl_iw.h>
+
+#define WL_ERROR(x) printf x
+#define WL_TRACE(x)
+
+#ifdef CUSTOMER_HW
+extern  void bcm_wlan_power_off(int);
+extern  void bcm_wlan_power_on(int);
+#endif /* CUSTOMER_HW */
+#ifdef CUSTOMER_HW2
+int wifi_set_carddetect(int on);
+int wifi_set_power(int on, unsigned long msec);
+int wifi_get_irq_number(unsigned long *irq_flags_ptr);
+int wifi_get_mac_addr(unsigned char *buf);
+void *wifi_get_country_code(char *ccode);
+#endif
+
+#if defined(OOB_INTR_ONLY)
+
+#if defined(BCMLXSDMMC)
+extern int sdioh_mmc_irq(int irq);
+#endif /* (BCMLXSDMMC)  */
+
+#ifdef CUSTOMER_HW3
+#include <mach/gpio.h>
+#endif
+
+/* Customer specific Host GPIO defintion  */
+static int dhd_oob_gpio_num = -1; /* GG 19 */
+
+module_param(dhd_oob_gpio_num, int, 0644);
+MODULE_PARM_DESC(dhd_oob_gpio_num, "DHD oob gpio number");
+
+int dhd_customer_oob_irq_map(unsigned long *irq_flags_ptr)
+{
+	int  host_oob_irq = 0;
+
+#ifdef CUSTOMER_HW2
+	host_oob_irq = wifi_get_irq_number(irq_flags_ptr);
+
+#else /* for NOT  CUSTOMER_HW2 */
+#if defined(CUSTOM_OOB_GPIO_NUM)
+	if (dhd_oob_gpio_num < 0) {
+		dhd_oob_gpio_num = CUSTOM_OOB_GPIO_NUM;
+	}
+#endif
+
+	if (dhd_oob_gpio_num < 0) {
+		WL_ERROR(("%s: ERROR customer specific Host GPIO is NOT defined \n",
+			__FUNCTION__));
+		return (dhd_oob_gpio_num);
+	}
+
+	WL_ERROR(("%s: customer specific Host GPIO number is (%d)\n",
+	         __FUNCTION__, dhd_oob_gpio_num));
+
+#if defined CUSTOMER_HW
+	host_oob_irq = MSM_GPIO_TO_INT(dhd_oob_gpio_num);
+#elif defined CUSTOMER_HW3
+	gpio_request(dhd_oob_gpio_num, "oob irq");
+	host_oob_irq = gpio_to_irq(dhd_oob_gpio_num);
+	gpio_direction_input(dhd_oob_gpio_num);
+#endif /* CUSTOMER_HW */
+#endif /* CUSTOMER_HW2 */
+
+	return (host_oob_irq);
+}
+#endif /* defined(OOB_INTR_ONLY) */
+
+/* Customer function to control hw specific wlan gpios */
+void
+dhd_customer_gpio_wlan_ctrl(int onoff)
+{
+	switch (onoff) {
+		case WLAN_RESET_OFF:
+			WL_TRACE(("%s: call customer specific GPIO to insert WLAN RESET\n",
+				__FUNCTION__));
+#ifdef CUSTOMER_HW
+			bcm_wlan_power_off(2);
+#endif /* CUSTOMER_HW */
+#ifdef CUSTOMER_HW2
+			wifi_set_power(0, 0);
+#endif
+			WL_ERROR(("=========== WLAN placed in RESET ========\n"));
+		break;
+
+		case WLAN_RESET_ON:
+			WL_TRACE(("%s: callc customer specific GPIO to remove WLAN RESET\n",
+				__FUNCTION__));
+#ifdef CUSTOMER_HW
+			bcm_wlan_power_on(2);
+#endif /* CUSTOMER_HW */
+#ifdef CUSTOMER_HW2
+			wifi_set_power(1, 0);
+#endif
+			WL_ERROR(("=========== WLAN going back to live  ========\n"));
+		break;
+
+		case WLAN_POWER_OFF:
+			WL_TRACE(("%s: call customer specific GPIO to turn off WL_REG_ON\n",
+				__FUNCTION__));
+#ifdef CUSTOMER_HW
+			bcm_wlan_power_off(1);
+#endif /* CUSTOMER_HW */
+		break;
+
+		case WLAN_POWER_ON:
+			WL_TRACE(("%s: call customer specific GPIO to turn on WL_REG_ON\n",
+				__FUNCTION__));
+#ifdef CUSTOMER_HW
+			bcm_wlan_power_on(1);
+			/* Lets customer power to get stable */
+			OSL_DELAY(50);
+#endif /* CUSTOMER_HW */
+		break;
+	}
+}
+
+#ifdef GET_CUSTOM_MAC_ENABLE
+/* Function to get custom MAC address */
+int
+dhd_custom_get_mac_address(unsigned char *buf)
+{
+	int ret = 0;
+
+	WL_TRACE(("%s Enter\n", __FUNCTION__));
+	if (!buf)
+		return -EINVAL;
+
+	/* Customer access to MAC address stored outside of DHD driver */
+#ifdef CUSTOMER_HW2
+	ret = wifi_get_mac_addr(buf);
+#endif
+
+#ifdef EXAMPLE_GET_MAC
+	/* EXAMPLE code */
+	{
+		struct ether_addr ea_example = {{0x00, 0x11, 0x22, 0x33, 0x44, 0xFF}};
+		bcopy((char *)&ea_example, buf, sizeof(struct ether_addr));
+	}
+#endif /* EXAMPLE_GET_MAC */
+
+	return ret;
+}
+#endif /* GET_CUSTOM_MAC_ENABLE */
+
+/* Customized Locale table : OPTIONAL feature */
+const struct cntry_locales_custom translate_custom_table[] = {
+/* Table should be filled out based on custom platform regulatory requirement */
+#ifdef EXAMPLE_TABLE
+	{"",   "XY", 4},  /* universal */
+	{"US", "US", 69}, /* input ISO "US" to : US regrev 69 */
+	{"CA", "US", 69}, /* input ISO "CA" to : US regrev 69 */
+	{"EU", "EU", 5},  /* European union countries */
+	{"AT", "EU", 5},
+	{"BE", "EU", 5},
+	{"BG", "EU", 5},
+	{"CY", "EU", 5},
+	{"CZ", "EU", 5},
+	{"DK", "EU", 5},
+	{"EE", "EU", 5},
+	{"FI", "EU", 5},
+	{"FR", "EU", 5},
+	{"DE", "EU", 5},
+	{"GR", "EU", 5},
+	{"HU", "EU", 5},
+	{"IE", "EU", 5},
+	{"IT", "EU", 5},
+	{"LV", "EU", 5},
+	{"LI", "EU", 5},
+	{"LT", "EU", 5},
+	{"LU", "EU", 5},
+	{"MT", "EU", 5},
+	{"NL", "EU", 5},
+	{"PL", "EU", 5},
+	{"PT", "EU", 5},
+	{"RO", "EU", 5},
+	{"SK", "EU", 5},
+	{"SI", "EU", 5},
+	{"ES", "EU", 5},
+	{"SE", "EU", 5},
+	{"GB", "EU", 5},  /* input ISO "GB" to : EU regrev 05 */
+	{"IL", "IL", 0},
+	{"CH", "CH", 0},
+	{"TR", "TR", 0},
+	{"NO", "NO", 0},
+	{"KR", "XY", 3},
+	{"AU", "XY", 3},
+	{"CN", "XY", 3},  /* input ISO "CN" to : XY regrev 03 */
+	{"TW", "XY", 3},
+	{"AR", "XY", 3},
+	{"MX", "XY", 3}
+#endif /* EXAMPLE_TABLE */
+};
+
+
+/* Customized Locale convertor
+*  input : ISO 3166-1 country abbreviation
+*  output: customized cspec
+*/
+void get_customized_country_code(char *country_iso_code, wl_country_t *cspec)
+{
+#ifdef CUSTOMER_HW2
+	struct cntry_locales_custom *cloc_ptr;
+
+	if (!cspec)
+		return;
+
+	cloc_ptr = wifi_get_country_code(country_iso_code);
+	if (cloc_ptr) {
+		strlcpy(cspec->ccode, cloc_ptr->custom_locale, WLC_CNTRY_BUF_SZ);
+		cspec->rev = cloc_ptr->custom_locale_rev;
+	}
+	return;
+#else
+	int size, i;
+
+	size = ARRAYSIZE(translate_custom_table);
+
+	if (cspec == 0)
+		return;
+
+	if (size == 0)
+		return;
+
+	for (i = 0; i < size; i++) {
+		if (strcmp(country_iso_code, translate_custom_table[i].iso_abbrev) == 0) {
+			memcpy(cspec->ccode, translate_custom_table[i].custom_locale, WLC_CNTRY_BUF_SZ);
+			cspec->rev = translate_custom_table[i].custom_locale_rev;
+			return;
+		}
+	}
+	memcpy(cspec->ccode, translate_custom_table[0].custom_locale, WLC_CNTRY_BUF_SZ);
+	cspec->rev = translate_custom_table[0].custom_locale_rev;
+	return;
+#endif
+}
diff --git a/drivers/net/wireless/bcm4329/dhd_dbg.h b/drivers/net/wireless/bcm4329/dhd_dbg.h
new file mode 100644
index 000000000000..b48c1d70f144
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/dhd_dbg.h
@@ -0,0 +1,100 @@
+/*
+ * Debug/trace/assert driver definitions for Dongle Host Driver.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_dbg.h,v 1.5.6.2.4.2.14.10 2010/05/21 21:49:38 Exp $
+ */
+
+#ifndef _dhd_dbg_
+#define _dhd_dbg_
+
+#ifdef DHD_DEBUG
+
+#define DHD_ERROR(args)	       do {if ((dhd_msg_level & DHD_ERROR_VAL) && (net_ratelimit())) \
+								printf args;} while (0)
+#define DHD_TRACE(args)		do {if (dhd_msg_level & DHD_TRACE_VAL) printf args;} while (0)
+#define DHD_INFO(args)		do {if (dhd_msg_level & DHD_INFO_VAL) printf args;} while (0)
+#define DHD_DATA(args)		do {if (dhd_msg_level & DHD_DATA_VAL) printf args;} while (0)
+#define DHD_CTL(args)		do {if (dhd_msg_level & DHD_CTL_VAL) printf args;} while (0)
+#define DHD_TIMER(args)		do {if (dhd_msg_level & DHD_TIMER_VAL) printf args;} while (0)
+#define DHD_HDRS(args)		do {if (dhd_msg_level & DHD_HDRS_VAL) printf args;} while (0)
+#define DHD_BYTES(args)		do {if (dhd_msg_level & DHD_BYTES_VAL) printf args;} while (0)
+#define DHD_INTR(args)		do {if (dhd_msg_level & DHD_INTR_VAL) printf args;} while (0)
+#define DHD_GLOM(args)		do {if (dhd_msg_level & DHD_GLOM_VAL) printf args;} while (0)
+#define DHD_EVENT(args)		do {if (dhd_msg_level & DHD_EVENT_VAL) printf args;} while (0)
+#define DHD_BTA(args)		do {if (dhd_msg_level & DHD_BTA_VAL) printf args;} while (0)
+#define DHD_ISCAN(args)		do {if (dhd_msg_level & DHD_ISCAN_VAL) printf args;} while (0)
+
+#define DHD_ERROR_ON()		(dhd_msg_level & DHD_ERROR_VAL)
+#define DHD_TRACE_ON()		(dhd_msg_level & DHD_TRACE_VAL)
+#define DHD_INFO_ON()		(dhd_msg_level & DHD_INFO_VAL)
+#define DHD_DATA_ON()		(dhd_msg_level & DHD_DATA_VAL)
+#define DHD_CTL_ON()		(dhd_msg_level & DHD_CTL_VAL)
+#define DHD_TIMER_ON()		(dhd_msg_level & DHD_TIMER_VAL)
+#define DHD_HDRS_ON()		(dhd_msg_level & DHD_HDRS_VAL)
+#define DHD_BYTES_ON()		(dhd_msg_level & DHD_BYTES_VAL)
+#define DHD_INTR_ON()		(dhd_msg_level & DHD_INTR_VAL)
+#define DHD_GLOM_ON()		(dhd_msg_level & DHD_GLOM_VAL)
+#define DHD_EVENT_ON()		(dhd_msg_level & DHD_EVENT_VAL)
+#define DHD_BTA_ON()		(dhd_msg_level & DHD_BTA_VAL)
+#define DHD_ISCAN_ON()		(dhd_msg_level & DHD_ISCAN_VAL)
+
+#else /* DHD_DEBUG */
+
+#define DHD_ERROR(args)    	do {if (net_ratelimit()) printf args;} while (0)
+#define DHD_TRACE(args)
+#define DHD_INFO(args)
+#define DHD_DATA(args)
+#define DHD_CTL(args)
+#define DHD_TIMER(args)
+#define DHD_HDRS(args)
+#define DHD_BYTES(args)
+#define DHD_INTR(args)
+#define DHD_GLOM(args)
+#define DHD_EVENT(args)
+#define DHD_BTA(args)
+#define DHD_ISCAN(args)
+
+#define DHD_ERROR_ON()		0
+#define DHD_TRACE_ON()		0
+#define DHD_INFO_ON()		0
+#define DHD_DATA_ON()		0
+#define DHD_CTL_ON()		0
+#define DHD_TIMER_ON()		0
+#define DHD_HDRS_ON()		0
+#define DHD_BYTES_ON()		0
+#define DHD_INTR_ON()		0
+#define DHD_GLOM_ON()		0
+#define DHD_EVENT_ON()		0
+#define DHD_BTA_ON()		0
+#define DHD_ISCAN_ON()		0
+#endif /* DHD_DEBUG */
+
+#define DHD_LOG(args)
+
+#define DHD_NONE(args)
+extern int dhd_msg_level;
+
+/* Defines msg bits */
+#include <dhdioctl.h>
+
+#endif /* _dhd_dbg_ */
diff --git a/drivers/net/wireless/bcm4329/dhd_linux.c b/drivers/net/wireless/bcm4329/dhd_linux.c
new file mode 100644
index 000000000000..2327ad5fa406
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/dhd_linux.c
@@ -0,0 +1,3451 @@
+/*
+ * Broadcom Dongle Host Driver (DHD), Linux-specific network interface
+ * Basically selected code segments from usb-cdc.c and usb-rndis.c
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_linux.c,v 1.65.4.9.2.12.2.104.4.40 2011/02/03 19:55:18 Exp $
+ */
+
+#include <typedefs.h>
+#include <linuxver.h>
+#include <osl.h>
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/random.h>
+#include <linux/spinlock.h>
+#include <linux/ethtool.h>
+#include <linux/fcntl.h>
+#include <linux/fs.h>
+#include <linux/inetdevice.h>
+#include <linux/mutex.h>
+#include <linux/device.h>
+
+#include <asm/uaccess.h>
+#include <asm/unaligned.h>
+
+#include <epivers.h>
+#include <bcmutils.h>
+#include <bcmendian.h>
+
+#include <proto/ethernet.h>
+#include <dngl_stats.h>
+#include <dhd.h>
+#include <dhd_bus.h>
+#include <dhd_proto.h>
+#include <dhd_dbg.h>
+#include <wl_iw.h>
+#ifdef CONFIG_HAS_WAKELOCK
+#include <linux/wakelock.h>
+#endif
+#ifdef CUSTOMER_HW2
+#include <linux/platform_device.h>
+#ifdef CONFIG_WIFI_CONTROL_FUNC
+#include <linux/wlan_plat.h>
+static struct wifi_platform_data *wifi_control_data = NULL;
+#endif
+struct semaphore wifi_control_sem;
+
+static struct resource *wifi_irqres = NULL;
+
+int wifi_get_irq_number(unsigned long *irq_flags_ptr)
+{
+	if (wifi_irqres) {
+		*irq_flags_ptr = wifi_irqres->flags & IRQF_TRIGGER_MASK;
+		return (int)wifi_irqres->start;
+	}
+#ifdef CUSTOM_OOB_GPIO_NUM
+	return CUSTOM_OOB_GPIO_NUM;
+#else
+	return -1;
+#endif
+}
+
+int wifi_set_carddetect(int on)
+{
+	printk("%s = %d\n", __FUNCTION__, on);
+#ifdef CONFIG_WIFI_CONTROL_FUNC
+	if (wifi_control_data && wifi_control_data->set_carddetect) {
+		wifi_control_data->set_carddetect(on);
+	}
+#endif
+	return 0;
+}
+
+int wifi_set_power(int on, unsigned long msec)
+{
+	printk("%s = %d\n", __FUNCTION__, on);
+#ifdef CONFIG_WIFI_CONTROL_FUNC
+	if (wifi_control_data && wifi_control_data->set_power) {
+		wifi_control_data->set_power(on);
+	}
+#endif
+	if (msec)
+		mdelay(msec);
+	return 0;
+}
+
+int wifi_set_reset(int on, unsigned long msec)
+{
+	DHD_TRACE(("%s = %d\n", __FUNCTION__, on));
+#ifdef CONFIG_WIFI_CONTROL_FUNC
+	if (wifi_control_data && wifi_control_data->set_reset) {
+		wifi_control_data->set_reset(on);
+	}
+#endif
+	if (msec)
+		mdelay(msec);
+	return 0;
+}
+
+int wifi_get_mac_addr(unsigned char *buf)
+{
+	DHD_TRACE(("%s\n", __FUNCTION__));
+	if (!buf)
+		return -EINVAL;
+#ifdef CONFIG_WIFI_CONTROL_FUNC
+	if (wifi_control_data && wifi_control_data->get_mac_addr) {
+		return wifi_control_data->get_mac_addr(buf);
+	}
+#endif
+	return -EOPNOTSUPP;
+}
+
+void *wifi_get_country_code(char *ccode)
+{
+	DHD_TRACE(("%s\n", __FUNCTION__));
+#ifdef CONFIG_WIFI_CONTROL_FUNC
+	if (!ccode)
+		return NULL;
+	if (wifi_control_data && wifi_control_data->get_country_code) {
+		return wifi_control_data->get_country_code(ccode);
+	}
+#endif
+	return NULL;
+}
+
+static int wifi_probe(struct platform_device *pdev)
+{
+#ifdef CONFIG_WIFI_CONTROL_FUNC
+	struct wifi_platform_data *wifi_ctrl =
+		(struct wifi_platform_data *)(pdev->dev.platform_data);
+
+	wifi_control_data = wifi_ctrl;
+#endif
+
+	DHD_TRACE(("## %s\n", __FUNCTION__));
+	wifi_irqres = platform_get_resource_byname(pdev, IORESOURCE_IRQ, "bcm4329_wlan_irq");
+
+	wifi_set_power(1, 0);	/* Power On */
+	wifi_set_carddetect(1);	/* CardDetect (0->1) */
+
+	up(&wifi_control_sem);
+	return 0;
+}
+
+static int wifi_remove(struct platform_device *pdev)
+{
+#ifdef CONFIG_WIFI_CONTROL_FUNC
+	struct wifi_platform_data *wifi_ctrl =
+		(struct wifi_platform_data *)(pdev->dev.platform_data);
+
+	wifi_control_data = wifi_ctrl;
+#endif
+	DHD_TRACE(("## %s\n", __FUNCTION__));
+	wifi_set_power(0, 0);	/* Power Off */
+	wifi_set_carddetect(0);	/* CardDetect (1->0) */
+
+	up(&wifi_control_sem);
+	return 0;
+}
+
+static int wifi_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	DHD_TRACE(("##> %s\n", __FUNCTION__));
+#if defined(OOB_INTR_ONLY)
+	bcmsdh_oob_intr_set(0);
+#endif /* (OOB_INTR_ONLY) */
+	return 0;
+}
+static int wifi_resume(struct platform_device *pdev)
+{
+	DHD_TRACE(("##> %s\n", __FUNCTION__));
+#if defined(OOB_INTR_ONLY)
+	bcmsdh_oob_intr_set(1);
+#endif /* (OOB_INTR_ONLY) */
+	return 0;
+}
+
+static struct platform_driver wifi_device = {
+	.probe          = wifi_probe,
+	.remove         = wifi_remove,
+	.suspend        = wifi_suspend,
+	.resume         = wifi_resume,
+	.driver         = {
+	.name   = "bcm4329_wlan",
+	}
+};
+
+int wifi_add_dev(void)
+{
+	DHD_TRACE(("## Calling platform_driver_register\n"));
+	return platform_driver_register(&wifi_device);
+}
+
+void wifi_del_dev(void)
+{
+	DHD_TRACE(("## Unregister platform_driver_register\n"));
+	platform_driver_unregister(&wifi_device);
+}
+#endif /* defined(CUSTOMER_HW2) */
+
+static int dhd_device_event(struct notifier_block *this, unsigned long event,
+				void *ptr);
+
+static struct notifier_block dhd_notifier = {
+	.notifier_call = dhd_device_event
+};
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP)
+#include <linux/suspend.h>
+volatile bool dhd_mmc_suspend = FALSE;
+DECLARE_WAIT_QUEUE_HEAD(dhd_dpc_wait);
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) */
+
+#if defined(OOB_INTR_ONLY)
+extern void dhd_enable_oob_intr(struct dhd_bus *bus, bool enable);
+#endif /* defined(OOB_INTR_ONLY) */
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+MODULE_LICENSE("GPL v2");
+#endif /* LinuxVer */
+
+#if LINUX_VERSION_CODE == KERNEL_VERSION(2, 6, 15)
+const char *
+print_tainted()
+{
+	return "";
+}
+#endif	/* LINUX_VERSION_CODE == KERNEL_VERSION(2, 6, 15) */
+
+/* Linux wireless extension support */
+#if defined(CONFIG_WIRELESS_EXT)
+#include <wl_iw.h>
+#endif /* defined(CONFIG_WIRELESS_EXT) */
+
+extern int dhdcdc_set_ioctl(dhd_pub_t *dhd, int ifidx, uint cmd, void *buf, uint len);
+
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+#include <linux/earlysuspend.h>
+#endif /* defined(CONFIG_HAS_EARLYSUSPEND) */
+
+#ifdef PKT_FILTER_SUPPORT
+extern void dhd_pktfilter_offload_set(dhd_pub_t * dhd, char *arg);
+extern void dhd_pktfilter_offload_enable(dhd_pub_t * dhd, char *arg, int enable, int master_mode);
+#endif
+
+/* Interface control information */
+typedef struct dhd_if {
+	struct dhd_info *info;			/* back pointer to dhd_info */
+	/* OS/stack specifics */
+	struct net_device *net;
+	struct net_device_stats stats;
+	int 			idx;			/* iface idx in dongle */
+	int 			state;			/* interface state */
+	uint 			subunit;		/* subunit */
+	uint8			mac_addr[ETHER_ADDR_LEN];	/* assigned MAC address */
+	bool			attached;		/* Delayed attachment when unset */
+	bool			txflowcontrol;	/* Per interface flow control indicator */
+	char			name[IFNAMSIZ+1]; /* linux interface name */
+} dhd_if_t;
+
+/* Local private structure (extension of pub) */
+typedef struct dhd_info {
+#if defined(CONFIG_WIRELESS_EXT)
+	wl_iw_t		iw;		/* wireless extensions state (must be first) */
+#endif /* defined(CONFIG_WIRELESS_EXT) */
+
+	dhd_pub_t pub;
+
+	/* OS/stack specifics */
+	dhd_if_t *iflist[DHD_MAX_IFS];
+
+	struct mutex proto_sem;
+	wait_queue_head_t ioctl_resp_wait;
+	struct timer_list timer;
+	bool wd_timer_valid;
+	struct tasklet_struct tasklet;
+	spinlock_t	sdlock;
+	spinlock_t	txqlock;
+	spinlock_t	dhd_lock;
+
+	/* Thread based operation */
+	bool threads_only;
+	struct mutex sdsem;
+	long watchdog_pid;
+	struct semaphore watchdog_sem;
+	struct completion watchdog_exited;
+	long dpc_pid;
+	struct semaphore dpc_sem;
+	struct completion dpc_exited;
+
+	/* Wakelocks */
+#ifdef CONFIG_HAS_WAKELOCK
+	struct wake_lock wl_wifi;   /* Wifi wakelock */
+	struct wake_lock wl_rxwake; /* Wifi rx wakelock */
+#endif
+	spinlock_t wl_lock;
+	int wl_count;
+	int wl_packet;
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
+	struct mutex wl_start_lock; /* mutex when START called to prevent any other Linux calls */
+#endif
+	/* Thread to issue ioctl for multicast */
+	long sysioc_pid;
+	struct semaphore sysioc_sem;
+	struct completion sysioc_exited;
+	bool set_multicast;
+	bool set_macaddress;
+	struct ether_addr macvalue;
+	wait_queue_head_t ctrl_wait;
+	atomic_t pend_8021x_cnt;
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	struct early_suspend early_suspend;
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+} dhd_info_t;
+
+/* Definitions to provide path to the firmware and nvram
+ * example nvram_path[MOD_PARAM_PATHLEN]="/projects/wlan/nvram.txt"
+ */
+char firmware_path[MOD_PARAM_PATHLEN];
+char nvram_path[MOD_PARAM_PATHLEN];
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+struct semaphore dhd_registration_sem;
+#define DHD_REGISTRATION_TIMEOUT  24000  /* msec : allowed time to finished dhd registration */
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
+/* load firmware and/or nvram values from the filesystem */
+module_param_string(firmware_path, firmware_path, MOD_PARAM_PATHLEN, 0);
+module_param_string(nvram_path, nvram_path, MOD_PARAM_PATHLEN, 0);
+
+/* Error bits */
+module_param(dhd_msg_level, int, 0);
+
+/* Spawn a thread for system ioctls (set mac, set mcast) */
+uint dhd_sysioc = TRUE;
+module_param(dhd_sysioc, uint, 0);
+
+/* Watchdog interval */
+uint dhd_watchdog_ms = 10;
+module_param(dhd_watchdog_ms, uint, 0);
+
+#ifdef DHD_DEBUG
+/* Console poll interval */
+uint dhd_console_ms = 0;
+module_param(dhd_console_ms, uint, 0);
+#endif /* DHD_DEBUG */
+
+/* ARP offload agent mode : Enable ARP Host Auto-Reply and ARP Peer Auto-Reply */
+uint dhd_arp_mode = 0xb;
+module_param(dhd_arp_mode, uint, 0);
+
+/* ARP offload enable */
+uint dhd_arp_enable = TRUE;
+module_param(dhd_arp_enable, uint, 0);
+
+/* Global Pkt filter enable control */
+uint dhd_pkt_filter_enable = TRUE;
+module_param(dhd_pkt_filter_enable, uint, 0);
+
+/*  Pkt filter init setup */
+uint dhd_pkt_filter_init = 0;
+module_param(dhd_pkt_filter_init, uint, 0);
+
+/* Pkt filter mode control */
+uint dhd_master_mode = TRUE;
+module_param(dhd_master_mode, uint, 1);
+
+/* Watchdog thread priority, -1 to use kernel timer */
+int dhd_watchdog_prio = 97;
+module_param(dhd_watchdog_prio, int, 0);
+
+/* DPC thread priority, -1 to use tasklet */
+int dhd_dpc_prio = 98;
+module_param(dhd_dpc_prio, int, 0);
+
+/* DPC thread priority, -1 to use tasklet */
+extern int dhd_dongle_memsize;
+module_param(dhd_dongle_memsize, int, 0);
+
+/* Control fw roaming */
+#ifdef CUSTOMER_HW2
+uint dhd_roam = 0;
+#else
+uint dhd_roam = 1;
+#endif
+
+/* Control radio state */
+uint dhd_radio_up = 1;
+
+/* Network inteface name */
+char iface_name[IFNAMSIZ];
+module_param_string(iface_name, iface_name, IFNAMSIZ, 0);
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+#define DAEMONIZE(a) daemonize(a); \
+	allow_signal(SIGKILL); \
+	allow_signal(SIGTERM);
+#else /* Linux 2.4 (w/o preemption patch) */
+#define RAISE_RX_SOFTIRQ() \
+	cpu_raise_softirq(smp_processor_id(), NET_RX_SOFTIRQ)
+#define DAEMONIZE(a) daemonize(); \
+	do { if (a) \
+		strncpy(current->comm, a, MIN(sizeof(current->comm), (strlen(a) + 1))); \
+	} while (0);
+#endif /* LINUX_VERSION_CODE  */
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+#define BLOCKABLE()	(!in_atomic())
+#else
+#define BLOCKABLE()	(!in_interrupt())
+#endif
+
+/* The following are specific to the SDIO dongle */
+
+/* IOCTL response timeout */
+int dhd_ioctl_timeout_msec = IOCTL_RESP_TIMEOUT;
+
+/* Idle timeout for backplane clock */
+int dhd_idletime = DHD_IDLETIME_TICKS;
+module_param(dhd_idletime, int, 0);
+
+/* Use polling */
+uint dhd_poll = FALSE;
+module_param(dhd_poll, uint, 0);
+
+/* Use interrupts */
+uint dhd_intr = TRUE;
+module_param(dhd_intr, uint, 0);
+
+/* SDIO Drive Strength (in milliamps) */
+uint dhd_sdiod_drive_strength = 6;
+module_param(dhd_sdiod_drive_strength, uint, 0);
+
+/* Tx/Rx bounds */
+extern uint dhd_txbound;
+extern uint dhd_rxbound;
+module_param(dhd_txbound, uint, 0);
+module_param(dhd_rxbound, uint, 0);
+
+/* Deferred transmits */
+extern uint dhd_deferred_tx;
+module_param(dhd_deferred_tx, uint, 0);
+
+
+
+#ifdef SDTEST
+/* Echo packet generator (pkts/s) */
+uint dhd_pktgen = 0;
+module_param(dhd_pktgen, uint, 0);
+
+/* Echo packet len (0 => sawtooth, max 2040) */
+uint dhd_pktgen_len = 0;
+module_param(dhd_pktgen_len, uint, 0);
+#endif
+
+/* Version string to report */
+#ifdef DHD_DEBUG
+#ifndef SRCBASE
+#define SRCBASE        "drivers/net/wireless/bcm4329"
+#endif
+#define DHD_COMPILED "\nCompiled in " SRCBASE
+#else
+#define DHD_COMPILED
+#endif
+
+static char dhd_version[] = "Dongle Host Driver, version " EPI_VERSION_STR
+#ifdef DHD_DEBUG
+"\nCompiled in " SRCBASE " on " __DATE__ " at " __TIME__
+#endif
+;
+
+
+#if defined(CONFIG_WIRELESS_EXT)
+struct iw_statistics *dhd_get_wireless_stats(struct net_device *dev);
+#endif /* defined(CONFIG_WIRELESS_EXT) */
+
+static void dhd_dpc(ulong data);
+/* forward decl */
+extern int dhd_wait_pend8021x(struct net_device *dev);
+
+#ifdef TOE
+#ifndef BDC
+#error TOE requires BDC
+#endif /* !BDC */
+static int dhd_toe_get(dhd_info_t *dhd, int idx, uint32 *toe_ol);
+static int dhd_toe_set(dhd_info_t *dhd, int idx, uint32 toe_ol);
+#endif /* TOE */
+
+static int dhd_wl_host_event(dhd_info_t *dhd, int *ifidx, void *pktdata,
+                             wl_event_msg_t *event_ptr, void **data_ptr);
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP)
+static int dhd_sleep_pm_callback(struct notifier_block *nfb, unsigned long action, void *ignored)
+{
+	int ret = NOTIFY_DONE;
+
+	switch (action) {
+	case PM_HIBERNATION_PREPARE:
+	case PM_SUSPEND_PREPARE:
+		dhd_mmc_suspend = TRUE;
+		ret = NOTIFY_OK;
+		break;
+	case PM_POST_HIBERNATION:
+	case PM_POST_SUSPEND:
+		dhd_mmc_suspend = FALSE;
+		ret = NOTIFY_OK;
+		break;
+	}
+	smp_mb();
+	return ret;
+}
+
+static struct notifier_block dhd_sleep_pm_notifier = {
+	.notifier_call = dhd_sleep_pm_callback,
+	.priority = 0
+};
+extern int register_pm_notifier(struct notifier_block *nb);
+extern int unregister_pm_notifier(struct notifier_block *nb);
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) */
+
+static void dhd_set_packet_filter(int value, dhd_pub_t *dhd)
+{
+#ifdef PKT_FILTER_SUPPORT
+	DHD_TRACE(("%s: %d\n", __FUNCTION__, value));
+	/* 1 - Enable packet filter, only allow unicast packet to send up */
+	/* 0 - Disable packet filter */
+	if (dhd_pkt_filter_enable) {
+		int i;
+
+		for (i = 0; i < dhd->pktfilter_count; i++) {
+			dhd_pktfilter_offload_set(dhd, dhd->pktfilter[i]);
+			dhd_pktfilter_offload_enable(dhd, dhd->pktfilter[i],
+					value, dhd_master_mode);
+		}
+	}
+#endif
+}
+
+
+
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+static int dhd_set_suspend(int value, dhd_pub_t *dhd)
+{
+	int power_mode = PM_MAX;
+	/* wl_pkt_filter_enable_t	enable_parm; */
+	char iovbuf[32];
+	int bcn_li_dtim = 3;
+#ifdef CUSTOMER_HW2
+	uint roamvar = 1;
+#endif /* CUSTOMER_HW2 */
+
+	DHD_TRACE(("%s: enter, value = %d in_suspend = %d\n",
+			__FUNCTION__, value, dhd->in_suspend));
+
+	if (dhd && dhd->up) {
+		if (value && dhd->in_suspend) {
+
+			/* Kernel suspended */
+			DHD_TRACE(("%s: force extra Suspend setting \n", __FUNCTION__));
+
+			dhdcdc_set_ioctl(dhd, 0, WLC_SET_PM,
+				(char *)&power_mode, sizeof(power_mode));
+
+			/* Enable packet filter, only allow unicast packet to send up */
+			dhd_set_packet_filter(1, dhd);
+
+			/* if dtim skip setup as default force it to wake each thrid dtim
+			 *  for better power saving.
+			 *  Note that side effect is chance to miss BC/MC packet
+			*/
+			bcn_li_dtim = dhd_get_dtim_skip(dhd);
+			bcm_mkiovar("bcn_li_dtim", (char *)&bcn_li_dtim,
+				4, iovbuf, sizeof(iovbuf));
+			dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf));
+#ifdef CUSTOMER_HW2
+			/* Disable build-in roaming during suspend */
+			bcm_mkiovar("roam_off", (char *)&roamvar, 4, iovbuf, sizeof(iovbuf));
+			dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf));
+#endif /* CUSTOMER_HW2 */
+
+		} else {
+
+			/* Kernel resumed  */
+			DHD_TRACE(("%s: Remove extra suspend setting \n", __FUNCTION__));
+
+			power_mode = PM_FAST;
+			dhdcdc_set_ioctl(dhd, 0, WLC_SET_PM, (char *)&power_mode,
+				sizeof(power_mode));
+
+			/* disable pkt filter */
+			dhd_set_packet_filter(0, dhd);
+
+			/* restore pre-suspend setting for dtim_skip */
+			bcm_mkiovar("bcn_li_dtim", (char *)&dhd->dtim_skip,
+				4, iovbuf, sizeof(iovbuf));
+
+			dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf));
+#ifdef CUSTOMER_HW2
+			roamvar = dhd_roam;
+			bcm_mkiovar("roam_off", (char *)&roamvar, 4, iovbuf, sizeof(iovbuf));
+			dhdcdc_set_ioctl(dhd, 0, WLC_SET_VAR, iovbuf, sizeof(iovbuf));
+#endif /* CUSTOMER_HW2 */
+		}
+	}
+
+	return 0;
+}
+
+static void dhd_suspend_resume_helper(struct dhd_info *dhd, int val)
+{
+	dhd_pub_t *dhdp = &dhd->pub;
+
+	dhd_os_wake_lock(dhdp);
+	dhd_os_proto_block(dhdp);
+	/* Set flag when early suspend was called */
+	dhdp->in_suspend = val;
+	if (!dhdp->suspend_disable_flag)
+		dhd_set_suspend(val, dhdp);
+	dhd_os_proto_unblock(dhdp);
+	dhd_os_wake_unlock(dhdp);
+}
+
+static void dhd_early_suspend(struct early_suspend *h)
+{
+	struct dhd_info *dhd = container_of(h, struct dhd_info, early_suspend);
+
+	DHD_TRACE(("%s: enter\n", __FUNCTION__));
+
+	if (dhd)
+		dhd_suspend_resume_helper(dhd, 1);
+}
+
+static void dhd_late_resume(struct early_suspend *h)
+{
+	struct dhd_info *dhd = container_of(h, struct dhd_info, early_suspend);
+
+	DHD_TRACE(("%s: enter\n", __FUNCTION__));
+
+	if (dhd)
+		dhd_suspend_resume_helper(dhd, 0);
+}
+#endif /* defined(CONFIG_HAS_EARLYSUSPEND) */
+
+/*
+ * Generalized timeout mechanism.  Uses spin sleep with exponential back-off until
+ * the sleep time reaches one jiffy, then switches over to task delay.  Usage:
+ *
+ *      dhd_timeout_start(&tmo, usec);
+ *      while (!dhd_timeout_expired(&tmo))
+ *              if (poll_something())
+ *                      break;
+ *      if (dhd_timeout_expired(&tmo))
+ *              fatal();
+ */
+
+void
+dhd_timeout_start(dhd_timeout_t *tmo, uint usec)
+{
+	tmo->limit = usec;
+	tmo->increment = 0;
+	tmo->elapsed = 0;
+	tmo->tick = 1000000 / HZ;
+}
+
+int
+dhd_timeout_expired(dhd_timeout_t *tmo)
+{
+	/* Does nothing the first call */
+	if (tmo->increment == 0) {
+		tmo->increment = 1;
+		return 0;
+	}
+
+	if (tmo->elapsed >= tmo->limit)
+		return 1;
+
+	/* Add the delay that's about to take place */
+	tmo->elapsed += tmo->increment;
+
+	if (tmo->increment < tmo->tick) {
+		OSL_DELAY(tmo->increment);
+		tmo->increment *= 2;
+		if (tmo->increment > tmo->tick)
+			tmo->increment = tmo->tick;
+	} else {
+		wait_queue_head_t delay_wait;
+		DECLARE_WAITQUEUE(wait, current);
+		int pending;
+		init_waitqueue_head(&delay_wait);
+		add_wait_queue(&delay_wait, &wait);
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(1);
+		pending = signal_pending(current);
+		remove_wait_queue(&delay_wait, &wait);
+		set_current_state(TASK_RUNNING);
+		if (pending)
+			return 1;	/* Interrupted */
+	}
+
+	return 0;
+}
+
+static int
+dhd_net2idx(dhd_info_t *dhd, struct net_device *net)
+{
+	int i = 0;
+
+	ASSERT(dhd);
+	while (i < DHD_MAX_IFS) {
+		if (dhd->iflist[i] && (dhd->iflist[i]->net == net))
+			return i;
+		i++;
+	}
+
+	return DHD_BAD_IF;
+}
+
+int
+dhd_ifname2idx(dhd_info_t *dhd, char *name)
+{
+	int i = DHD_MAX_IFS;
+
+	ASSERT(dhd);
+
+	if (name == NULL || *name == '\0')
+		return 0;
+
+	while (--i > 0)
+		if (dhd->iflist[i] && !strncmp(dhd->iflist[i]->name, name, IFNAMSIZ))
+				break;
+
+	DHD_TRACE(("%s: return idx %d for \"%s\"\n", __FUNCTION__, i, name));
+
+	return i;	/* default - the primary interface */
+}
+
+char *
+dhd_ifname(dhd_pub_t *dhdp, int ifidx)
+{
+	dhd_info_t *dhd = (dhd_info_t *)dhdp->info;
+
+	ASSERT(dhd);
+
+	if (ifidx < 0 || ifidx >= DHD_MAX_IFS) {
+		DHD_ERROR(("%s: ifidx %d out of range\n", __FUNCTION__, ifidx));
+		return "<if_bad>";
+	}
+
+	if (dhd->iflist[ifidx] == NULL) {
+		DHD_ERROR(("%s: null i/f %d\n", __FUNCTION__, ifidx));
+		return "<if_null>";
+	}
+
+	if (dhd->iflist[ifidx]->net)
+		return dhd->iflist[ifidx]->net->name;
+
+	return "<if_none>";
+}
+
+static void
+_dhd_set_multicast_list(dhd_info_t *dhd, int ifidx)
+{
+	struct net_device *dev;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35)
+	struct netdev_hw_addr *ha;
+#else
+	struct dev_mc_list *mclist;
+#endif
+	uint32 allmulti, cnt;
+
+	wl_ioctl_t ioc;
+	char *buf, *bufp;
+	uint buflen;
+	int ret;
+
+	ASSERT(dhd && dhd->iflist[ifidx]);
+	dev = dhd->iflist[ifidx]->net;
+
+	NETIF_ADDR_LOCK(dev);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35)
+	cnt = netdev_mc_count(dev);
+#else
+	cnt = dev->mc_count;
+#endif
+	NETIF_ADDR_UNLOCK(dev);
+
+	/* Determine initial value of allmulti flag */
+	allmulti = (dev->flags & IFF_ALLMULTI) ? TRUE : FALSE;
+
+	/* Send down the multicast list first. */
+	buflen = sizeof("mcast_list") + sizeof(cnt) + (cnt * ETHER_ADDR_LEN);
+	if (!(bufp = buf = MALLOC(dhd->pub.osh, buflen))) {
+		DHD_ERROR(("%s: out of memory for mcast_list, cnt %d\n",
+		           dhd_ifname(&dhd->pub, ifidx), cnt));
+		return;
+	}
+
+	strcpy(bufp, "mcast_list");
+	bufp += strlen("mcast_list") + 1;
+
+	cnt = htol32(cnt);
+	memcpy(bufp, &cnt, sizeof(cnt));
+	bufp += sizeof(cnt);
+
+	NETIF_ADDR_LOCK(dev);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35)
+	netdev_for_each_mc_addr(ha, dev) {
+		if (!cnt)
+			break;
+		memcpy(bufp, ha->addr, ETHER_ADDR_LEN);
+		bufp += ETHER_ADDR_LEN;
+		cnt--;
+	}
+#else
+	for (mclist = dev->mc_list; (mclist && (cnt > 0)); cnt--, mclist = mclist->next) {
+		memcpy(bufp, (void *)mclist->dmi_addr, ETHER_ADDR_LEN);
+		bufp += ETHER_ADDR_LEN;
+	}
+#endif
+	NETIF_ADDR_UNLOCK(dev);
+
+	memset(&ioc, 0, sizeof(ioc));
+	ioc.cmd = WLC_SET_VAR;
+	ioc.buf = buf;
+	ioc.len = buflen;
+	ioc.set = TRUE;
+
+	ret = dhd_prot_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len);
+	if (ret < 0) {
+		DHD_ERROR(("%s: set mcast_list failed, cnt %d\n",
+			dhd_ifname(&dhd->pub, ifidx), cnt));
+		allmulti = cnt ? TRUE : allmulti;
+	}
+
+	MFREE(dhd->pub.osh, buf, buflen);
+
+	/* Now send the allmulti setting.  This is based on the setting in the
+	 * net_device flags, but might be modified above to be turned on if we
+	 * were trying to set some addresses and dongle rejected it...
+	 */
+
+	buflen = sizeof("allmulti") + sizeof(allmulti);
+	if (!(buf = MALLOC(dhd->pub.osh, buflen))) {
+		DHD_ERROR(("%s: out of memory for allmulti\n", dhd_ifname(&dhd->pub, ifidx)));
+		return;
+	}
+	allmulti = htol32(allmulti);
+
+	if (!bcm_mkiovar("allmulti", (void*)&allmulti, sizeof(allmulti), buf, buflen)) {
+		DHD_ERROR(("%s: mkiovar failed for allmulti, datalen %d buflen %u\n",
+		           dhd_ifname(&dhd->pub, ifidx), (int)sizeof(allmulti), buflen));
+		MFREE(dhd->pub.osh, buf, buflen);
+		return;
+	}
+
+
+	memset(&ioc, 0, sizeof(ioc));
+	ioc.cmd = WLC_SET_VAR;
+	ioc.buf = buf;
+	ioc.len = buflen;
+	ioc.set = TRUE;
+
+	ret = dhd_prot_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len);
+	if (ret < 0) {
+		DHD_ERROR(("%s: set allmulti %d failed\n",
+		           dhd_ifname(&dhd->pub, ifidx), ltoh32(allmulti)));
+	}
+
+	MFREE(dhd->pub.osh, buf, buflen);
+
+	/* Finally, pick up the PROMISC flag as well, like the NIC driver does */
+
+	allmulti = (dev->flags & IFF_PROMISC) ? TRUE : FALSE;
+	allmulti = htol32(allmulti);
+
+	memset(&ioc, 0, sizeof(ioc));
+	ioc.cmd = WLC_SET_PROMISC;
+	ioc.buf = &allmulti;
+	ioc.len = sizeof(allmulti);
+	ioc.set = TRUE;
+
+	ret = dhd_prot_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len);
+	if (ret < 0) {
+		DHD_ERROR(("%s: set promisc %d failed\n",
+		           dhd_ifname(&dhd->pub, ifidx), ltoh32(allmulti)));
+	}
+}
+
+static int
+_dhd_set_mac_address(dhd_info_t *dhd, int ifidx, struct ether_addr *addr)
+{
+	char buf[32];
+	wl_ioctl_t ioc;
+	int ret;
+
+	DHD_TRACE(("%s enter\n", __FUNCTION__));
+	if (!bcm_mkiovar("cur_etheraddr", (char*)addr, ETHER_ADDR_LEN, buf, 32)) {
+		DHD_ERROR(("%s: mkiovar failed for cur_etheraddr\n", dhd_ifname(&dhd->pub, ifidx)));
+		return -1;
+	}
+	memset(&ioc, 0, sizeof(ioc));
+	ioc.cmd = WLC_SET_VAR;
+	ioc.buf = buf;
+	ioc.len = 32;
+	ioc.set = TRUE;
+
+	ret = dhd_prot_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len);
+	if (ret < 0) {
+		DHD_ERROR(("%s: set cur_etheraddr failed\n", dhd_ifname(&dhd->pub, ifidx)));
+	} else {
+		memcpy(dhd->iflist[ifidx]->net->dev_addr, addr, ETHER_ADDR_LEN);
+	}
+
+	return ret;
+}
+
+#ifdef SOFTAP
+extern struct net_device *ap_net_dev;
+/* semaphore that the soft AP CODE waits on */
+extern struct semaphore ap_eth_sema;
+#endif
+
+static void
+dhd_op_if(dhd_if_t *ifp)
+{
+	dhd_info_t *dhd;
+	int ret = 0, err = 0;
+#ifdef SOFTAP
+	unsigned long flags;
+#endif
+
+	ASSERT(ifp && ifp->info && ifp->idx);	/* Virtual interfaces only */
+
+	dhd = ifp->info;
+
+	DHD_TRACE(("%s: idx %d, state %d\n", __FUNCTION__, ifp->idx, ifp->state));
+
+	switch (ifp->state) {
+	case WLC_E_IF_ADD:
+		/*
+		 * Delete the existing interface before overwriting it
+		 * in case we missed the WLC_E_IF_DEL event.
+		 */
+		if (ifp->net != NULL) {
+			DHD_ERROR(("%s: ERROR: netdev:%s already exists, try free & unregister \n",
+			 __FUNCTION__, ifp->net->name));
+			netif_stop_queue(ifp->net);
+			unregister_netdev(ifp->net);
+			free_netdev(ifp->net);
+		}
+		/* Allocate etherdev, including space for private structure */
+		if (!(ifp->net = alloc_etherdev(sizeof(dhd)))) {
+			DHD_ERROR(("%s: OOM - alloc_etherdev\n", __FUNCTION__));
+			ret = -ENOMEM;
+		}
+		if (ret == 0) {
+			strcpy(ifp->net->name, ifp->name);
+			memcpy(netdev_priv(ifp->net), &dhd, sizeof(dhd));
+			if ((err = dhd_net_attach(&dhd->pub, ifp->idx)) != 0) {
+				DHD_ERROR(("%s: dhd_net_attach failed, err %d\n",
+					__FUNCTION__, err));
+				ret = -EOPNOTSUPP;
+			} else {
+#ifdef SOFTAP
+				flags = dhd_os_spin_lock(&dhd->pub);
+				/* save ptr to wl0.1 netdev for use in wl_iw.c  */
+				ap_net_dev = ifp->net;
+				 /* signal to the SOFTAP 'sleeper' thread, wl0.1 is ready */
+				up(&ap_eth_sema);
+				dhd_os_spin_unlock(&dhd->pub, flags);
+#endif
+				DHD_TRACE(("\n ==== pid:%x, net_device for if:%s created ===\n\n",
+					current->pid, ifp->net->name));
+				ifp->state = 0;
+			}
+		}
+		break;
+	case WLC_E_IF_DEL:
+		if (ifp->net != NULL) {
+			DHD_TRACE(("\n%s: got 'WLC_E_IF_DEL' state\n", __FUNCTION__));
+			netif_stop_queue(ifp->net);
+			unregister_netdev(ifp->net);
+			ret = DHD_DEL_IF;	/* Make sure the free_netdev() is called */
+		}
+		break;
+	default:
+		DHD_ERROR(("%s: bad op %d\n", __FUNCTION__, ifp->state));
+		ASSERT(!ifp->state);
+		break;
+	}
+
+	if (ret < 0) {
+		if (ifp->net) {
+			free_netdev(ifp->net);
+		}
+		dhd->iflist[ifp->idx] = NULL;
+		MFREE(dhd->pub.osh, ifp, sizeof(*ifp));
+#ifdef SOFTAP
+		flags = dhd_os_spin_lock(&dhd->pub);
+		if (ifp->net == ap_net_dev)
+			ap_net_dev = NULL;     /* NULL SOFTAP global as well */
+		dhd_os_spin_unlock(&dhd->pub, flags);
+#endif /*  SOFTAP */
+	}
+}
+
+static int
+_dhd_sysioc_thread(void *data)
+{
+	dhd_info_t *dhd = (dhd_info_t *)data;
+	int i;
+#ifdef SOFTAP
+	bool in_ap = FALSE;
+	unsigned long flags;
+#endif
+
+	DAEMONIZE("dhd_sysioc");
+
+	while (down_interruptible(&dhd->sysioc_sem) == 0) {
+		dhd_os_start_lock(&dhd->pub);
+		dhd_os_wake_lock(&dhd->pub);
+		for (i = 0; i < DHD_MAX_IFS; i++) {
+			if (dhd->iflist[i]) {
+				DHD_TRACE(("%s: interface %d\n",__FUNCTION__, i));
+#ifdef SOFTAP
+				flags = dhd_os_spin_lock(&dhd->pub);
+				in_ap = (ap_net_dev != NULL);
+				dhd_os_spin_unlock(&dhd->pub, flags);
+#endif /* SOFTAP */
+				if (dhd->iflist[i]->state)
+					dhd_op_if(dhd->iflist[i]);
+#ifdef SOFTAP
+				if (dhd->iflist[i] == NULL) {
+					DHD_TRACE(("%s: interface %d just been removed!\n\n", __FUNCTION__, i));
+					continue;
+				}
+
+				if (in_ap && dhd->set_macaddress) {
+					DHD_TRACE(("attempt to set MAC for %s in AP Mode blocked.\n", dhd->iflist[i]->net->name));
+					dhd->set_macaddress = FALSE;
+					continue;
+				}
+
+				if (in_ap && dhd->set_multicast)  {
+					DHD_TRACE(("attempt to set MULTICAST list for %s in AP Mode blocked.\n", dhd->iflist[i]->net->name));
+					dhd->set_multicast = FALSE;
+					continue;
+				}
+#endif /* SOFTAP */
+				if (dhd->set_multicast) {
+					dhd->set_multicast = FALSE;
+					_dhd_set_multicast_list(dhd, i);
+				}
+				if (dhd->set_macaddress) {
+					dhd->set_macaddress = FALSE;
+					_dhd_set_mac_address(dhd, i, &dhd->macvalue);
+				}
+			}
+		}
+		dhd_os_wake_unlock(&dhd->pub);
+		dhd_os_start_unlock(&dhd->pub);
+	}
+	DHD_TRACE(("%s: stopped\n",__FUNCTION__));
+	complete_and_exit(&dhd->sysioc_exited, 0);
+}
+
+static int
+dhd_set_mac_address(struct net_device *dev, void *addr)
+{
+	int ret = 0;
+
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	struct sockaddr *sa = (struct sockaddr *)addr;
+	int ifidx;
+
+	DHD_TRACE(("%s: Enter\n",__FUNCTION__));
+	ifidx = dhd_net2idx(dhd, dev);
+	if (ifidx == DHD_BAD_IF)
+		return -1;
+
+	ASSERT(dhd->sysioc_pid >= 0);
+	memcpy(&dhd->macvalue, sa->sa_data, ETHER_ADDR_LEN);
+	dhd->set_macaddress = TRUE;
+	up(&dhd->sysioc_sem);
+
+	return ret;
+}
+
+static void
+dhd_set_multicast_list(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ifidx;
+
+	DHD_TRACE(("%s: Enter\n",__FUNCTION__));
+	ifidx = dhd_net2idx(dhd, dev);
+	if (ifidx == DHD_BAD_IF)
+		return;
+
+	ASSERT(dhd->sysioc_pid >= 0);
+	dhd->set_multicast = TRUE;
+	up(&dhd->sysioc_sem);
+}
+
+int
+dhd_sendpkt(dhd_pub_t *dhdp, int ifidx, void *pktbuf)
+{
+	int ret;
+	dhd_info_t *dhd = (dhd_info_t *)(dhdp->info);
+
+	/* Reject if down */
+	if (!dhdp->up || (dhdp->busstate == DHD_BUS_DOWN)) {
+		return -ENODEV;
+	}
+
+	/* Update multicast statistic */
+	if (PKTLEN(dhdp->osh, pktbuf) >= ETHER_ADDR_LEN) {
+		uint8 *pktdata = (uint8 *)PKTDATA(dhdp->osh, pktbuf);
+		struct ether_header *eh = (struct ether_header *)pktdata;
+
+		if (ETHER_ISMULTI(eh->ether_dhost))
+			dhdp->tx_multicast++;
+		if (ntoh16(eh->ether_type) == ETHER_TYPE_802_1X)
+			atomic_inc(&dhd->pend_8021x_cnt);
+	}
+
+	/* Look into the packet and update the packet priority */
+	if ((PKTPRIO(pktbuf) == 0))
+		pktsetprio(pktbuf, FALSE);
+
+	/* If the protocol uses a data header, apply it */
+	dhd_prot_hdrpush(dhdp, ifidx, pktbuf);
+
+	/* Use bus module to send data frame */
+#ifdef BCMDBUS
+	ret = dbus_send_pkt(dhdp->dbus, pktbuf, NULL /* pktinfo */);
+#else
+	ret = dhd_bus_txdata(dhdp->bus, pktbuf);
+#endif /* BCMDBUS */
+
+	return ret;
+}
+
+static int
+dhd_start_xmit(struct sk_buff *skb, struct net_device *net)
+{
+	int ret;
+	void *pktbuf;
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
+	int ifidx;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	dhd_os_wake_lock(&dhd->pub);
+
+	/* Reject if down */
+	if (!dhd->pub.up || (dhd->pub.busstate == DHD_BUS_DOWN)) {
+		DHD_ERROR(("%s: xmit rejected pub.up=%d busstate=%d\n",
+			 __FUNCTION__, dhd->pub.up, dhd->pub.busstate));
+		netif_stop_queue(net);
+		/* Send Event when bus down detected during data session */
+		if (dhd->pub.busstate == DHD_BUS_DOWN)  {
+			DHD_ERROR(("%s: Event HANG send up\n", __FUNCTION__));
+			net_os_send_hang_message(net);
+		}
+		dhd_os_wake_unlock(&dhd->pub);
+		return -ENODEV;
+	}
+
+	ifidx = dhd_net2idx(dhd, net);
+	if (ifidx == DHD_BAD_IF) {
+		DHD_ERROR(("%s: bad ifidx %d\n", __FUNCTION__, ifidx));
+		netif_stop_queue(net);
+		dhd_os_wake_unlock(&dhd->pub);
+		return -ENODEV;
+	}
+
+	/* Make sure there's enough room for any header */
+	if (skb_headroom(skb) < dhd->pub.hdrlen) {
+		struct sk_buff *skb2;
+
+		DHD_INFO(("%s: insufficient headroom\n",
+		          dhd_ifname(&dhd->pub, ifidx)));
+		dhd->pub.tx_realloc++;
+		skb2 = skb_realloc_headroom(skb, dhd->pub.hdrlen);
+		dev_kfree_skb(skb);
+		if ((skb = skb2) == NULL) {
+			DHD_ERROR(("%s: skb_realloc_headroom failed\n",
+			           dhd_ifname(&dhd->pub, ifidx)));
+			ret = -ENOMEM;
+			goto done;
+		}
+	}
+
+	/* Convert to packet */
+	if (!(pktbuf = PKTFRMNATIVE(dhd->pub.osh, skb))) {
+		DHD_ERROR(("%s: PKTFRMNATIVE failed\n",
+		           dhd_ifname(&dhd->pub, ifidx)));
+		dev_kfree_skb_any(skb);
+		ret = -ENOMEM;
+		goto done;
+	}
+
+	ret = dhd_sendpkt(&dhd->pub, ifidx, pktbuf);
+
+done:
+	if (ret)
+		dhd->pub.dstats.tx_dropped++;
+	else
+		dhd->pub.tx_packets++;
+
+	dhd_os_wake_unlock(&dhd->pub);
+
+	/* Return ok: we always eat the packet */
+	return 0;
+}
+
+void
+dhd_txflowcontrol(dhd_pub_t *dhdp, int ifidx, bool state)
+{
+	struct net_device *net;
+	dhd_info_t *dhd = dhdp->info;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	dhdp->txoff = state;
+	ASSERT(dhd && dhd->iflist[ifidx]);
+	net = dhd->iflist[ifidx]->net;
+	if (state == ON)
+		netif_stop_queue(net);
+	else
+		netif_wake_queue(net);
+}
+
+void
+dhd_rx_frame(dhd_pub_t *dhdp, int ifidx, void *pktbuf, int numpkt)
+{
+	dhd_info_t *dhd = (dhd_info_t *)dhdp->info;
+	struct sk_buff *skb;
+	uchar *eth;
+	uint len;
+	void * data, *pnext, *save_pktbuf;
+	int i;
+	dhd_if_t *ifp;
+	wl_event_msg_t event;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	save_pktbuf = pktbuf;
+
+	for (i = 0; pktbuf && i < numpkt; i++, pktbuf = pnext) {
+
+		pnext = PKTNEXT(dhdp->osh, pktbuf);
+		PKTSETNEXT(wl->sh.osh, pktbuf, NULL);
+
+
+		skb = PKTTONATIVE(dhdp->osh, pktbuf);
+
+		/* Get the protocol, maintain skb around eth_type_trans()
+		 * The main reason for this hack is for the limitation of
+		 * Linux 2.4 where 'eth_type_trans' uses the 'net->hard_header_len'
+		 * to perform skb_pull inside vs ETH_HLEN. Since to avoid
+		 * coping of the packet coming from the network stack to add
+		 * BDC, Hardware header etc, during network interface registration
+		 * we set the 'net->hard_header_len' to ETH_HLEN + extra space required
+		 * for BDC, Hardware header etc. and not just the ETH_HLEN
+		 */
+		eth = skb->data;
+		len = skb->len;
+
+		ifp = dhd->iflist[ifidx];
+		if (ifp == NULL)
+			ifp = dhd->iflist[0];
+
+		ASSERT(ifp);
+		skb->dev = ifp->net;
+		skb->protocol = eth_type_trans(skb, skb->dev);
+
+		if (skb->pkt_type == PACKET_MULTICAST) {
+			dhd->pub.rx_multicast++;
+		}
+
+		skb->data = eth;
+		skb->len = len;
+
+		/* Strip header, count, deliver upward */
+		skb_pull(skb, ETH_HLEN);
+
+		/* Process special event packets and then discard them */
+		if (ntoh16(skb->protocol) == ETHER_TYPE_BRCM)
+			dhd_wl_host_event(dhd, &ifidx,
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24)
+			skb->mac_header,
+#else
+			skb->mac.raw,
+#endif
+			&event,
+			&data);
+
+		ASSERT(ifidx < DHD_MAX_IFS && dhd->iflist[ifidx]);
+		if (dhd->iflist[ifidx] && !dhd->iflist[ifidx]->state)
+			ifp = dhd->iflist[ifidx];
+
+		if (ifp->net)
+			ifp->net->last_rx = jiffies;
+
+		dhdp->dstats.rx_bytes += skb->len;
+		dhdp->rx_packets++; /* Local count */
+
+		if (in_interrupt()) {
+			netif_rx(skb);
+		} else {
+			/* If the receive is not processed inside an ISR,
+			 * the softirqd must be woken explicitly to service
+			 * the NET_RX_SOFTIRQ.  In 2.6 kernels, this is handled
+			 * by netif_rx_ni(), but in earlier kernels, we need
+			 * to do it manually.
+			 */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0)
+			netif_rx_ni(skb);
+#else
+			ulong flags;
+			netif_rx(skb);
+			local_irq_save(flags);
+			RAISE_RX_SOFTIRQ();
+			local_irq_restore(flags);
+#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0) */
+		}
+	}
+	dhd_os_wake_lock_timeout_enable(dhdp);
+}
+
+void
+dhd_event(struct dhd_info *dhd, char *evpkt, int evlen, int ifidx)
+{
+	/* Linux version has nothing to do */
+	return;
+}
+
+void
+dhd_txcomplete(dhd_pub_t *dhdp, void *txp, bool success)
+{
+	uint ifidx;
+	dhd_info_t *dhd = (dhd_info_t *)(dhdp->info);
+	struct ether_header *eh;
+	uint16 type;
+
+	dhd_prot_hdrpull(dhdp, &ifidx, txp);
+
+	eh = (struct ether_header *)PKTDATA(dhdp->osh, txp);
+	type  = ntoh16(eh->ether_type);
+
+	if (type == ETHER_TYPE_802_1X)
+		atomic_dec(&dhd->pend_8021x_cnt);
+
+}
+
+static struct net_device_stats *
+dhd_get_stats(struct net_device *net)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
+	dhd_if_t *ifp;
+	int ifidx;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	ifidx = dhd_net2idx(dhd, net);
+	if (ifidx == DHD_BAD_IF)
+		return NULL;
+
+	ifp = dhd->iflist[ifidx];
+	ASSERT(dhd && ifp);
+
+	if (dhd->pub.up) {
+		/* Use the protocol to get dongle stats */
+		dhd_prot_dstats(&dhd->pub);
+	}
+
+	/* Copy dongle stats to net device stats */
+	ifp->stats.rx_packets = dhd->pub.dstats.rx_packets;
+	ifp->stats.tx_packets = dhd->pub.dstats.tx_packets;
+	ifp->stats.rx_bytes = dhd->pub.dstats.rx_bytes;
+	ifp->stats.tx_bytes = dhd->pub.dstats.tx_bytes;
+	ifp->stats.rx_errors = dhd->pub.dstats.rx_errors;
+	ifp->stats.tx_errors = dhd->pub.dstats.tx_errors;
+	ifp->stats.rx_dropped = dhd->pub.dstats.rx_dropped;
+	ifp->stats.tx_dropped = dhd->pub.dstats.tx_dropped;
+	ifp->stats.multicast = dhd->pub.dstats.multicast;
+
+	return &ifp->stats;
+}
+
+static int
+dhd_watchdog_thread(void *data)
+{
+	dhd_info_t *dhd = (dhd_info_t *)data;
+
+	/* This thread doesn't need any user-level access,
+	 * so get rid of all our resources
+	 */
+#ifdef DHD_SCHED
+	if (dhd_watchdog_prio > 0) {
+		struct sched_param param;
+		param.sched_priority = (dhd_watchdog_prio < MAX_RT_PRIO)?
+			dhd_watchdog_prio:(MAX_RT_PRIO-1);
+		setScheduler(current, SCHED_FIFO, &param);
+	}
+#endif /* DHD_SCHED */
+
+	DAEMONIZE("dhd_watchdog");
+
+	/* Run until signal received */
+	while (1) {
+		if (down_interruptible (&dhd->watchdog_sem) == 0) {
+			dhd_os_sdlock(&dhd->pub);
+			if (dhd->pub.dongle_reset == FALSE) {
+				DHD_TIMER(("%s:\n", __FUNCTION__));
+				/* Call the bus module watchdog */
+				dhd_bus_watchdog(&dhd->pub);
+
+				/* Count the tick for reference */
+				dhd->pub.tickcnt++;
+
+				/* Reschedule the watchdog */
+				if (dhd->wd_timer_valid)
+					mod_timer(&dhd->timer, jiffies + dhd_watchdog_ms * HZ / 1000);
+			}
+			dhd_os_sdunlock(&dhd->pub);
+			dhd_os_wake_unlock(&dhd->pub);
+		} else {
+			break;
+		}
+	}
+
+	complete_and_exit(&dhd->watchdog_exited, 0);
+}
+
+static void
+dhd_watchdog(ulong data)
+{
+	dhd_info_t *dhd = (dhd_info_t *)data;
+
+	dhd_os_wake_lock(&dhd->pub);
+	if (dhd->pub.dongle_reset) {
+		dhd_os_wake_unlock(&dhd->pub);
+		return;
+	}
+
+	if (dhd->watchdog_pid >= 0) {
+		up(&dhd->watchdog_sem);
+		return;
+	}
+
+	dhd_os_sdlock(&dhd->pub);
+	/* Call the bus module watchdog */
+	dhd_bus_watchdog(&dhd->pub);
+
+	/* Count the tick for reference */
+	dhd->pub.tickcnt++;
+
+	/* Reschedule the watchdog */
+	if (dhd->wd_timer_valid)
+		mod_timer(&dhd->timer, jiffies + dhd_watchdog_ms * HZ / 1000);
+	dhd_os_sdunlock(&dhd->pub);
+	dhd_os_wake_unlock(&dhd->pub);
+}
+
+static int
+dhd_dpc_thread(void *data)
+{
+	dhd_info_t *dhd = (dhd_info_t *)data;
+
+	/* This thread doesn't need any user-level access,
+	 * so get rid of all our resources
+	 */
+#ifdef DHD_SCHED
+	if (dhd_dpc_prio > 0)
+	{
+		struct sched_param param;
+		param.sched_priority = (dhd_dpc_prio < MAX_RT_PRIO)?dhd_dpc_prio:(MAX_RT_PRIO-1);
+		setScheduler(current, SCHED_FIFO, &param);
+	}
+#endif /* DHD_SCHED */
+
+	DAEMONIZE("dhd_dpc");
+
+	/* Run until signal received */
+	while (1) {
+		if (down_interruptible(&dhd->dpc_sem) == 0) {
+			/* Call bus dpc unless it indicated down (then clean stop) */
+			if (dhd->pub.busstate != DHD_BUS_DOWN) {
+				if (dhd_bus_dpc(dhd->pub.bus)) {
+					up(&dhd->dpc_sem);
+				}
+				else {
+					dhd_os_wake_unlock(&dhd->pub);
+				}
+			} else {
+				if (dhd->pub.up)
+					dhd_bus_stop(dhd->pub.bus, TRUE);
+				dhd_os_wake_unlock(&dhd->pub);
+			}
+		}
+		else
+			break;
+	}
+
+	complete_and_exit(&dhd->dpc_exited, 0);
+}
+
+static void
+dhd_dpc(ulong data)
+{
+	dhd_info_t *dhd;
+
+	dhd = (dhd_info_t *)data;
+
+	/* Call bus dpc unless it indicated down (then clean stop) */
+	if (dhd->pub.busstate != DHD_BUS_DOWN) {
+		if (dhd_bus_dpc(dhd->pub.bus))
+			tasklet_schedule(&dhd->tasklet);
+	} else {
+		dhd_bus_stop(dhd->pub.bus, TRUE);
+	}
+}
+
+void
+dhd_sched_dpc(dhd_pub_t *dhdp)
+{
+	dhd_info_t *dhd = (dhd_info_t *)dhdp->info;
+
+	dhd_os_wake_lock(dhdp);
+	if (dhd->dpc_pid >= 0) {
+		up(&dhd->dpc_sem);
+		return;
+	}
+
+	tasklet_schedule(&dhd->tasklet);
+}
+
+#ifdef TOE
+/* Retrieve current toe component enables, which are kept as a bitmap in toe_ol iovar */
+static int
+dhd_toe_get(dhd_info_t *dhd, int ifidx, uint32 *toe_ol)
+{
+	wl_ioctl_t ioc;
+	char buf[32];
+	int ret;
+
+	memset(&ioc, 0, sizeof(ioc));
+
+	ioc.cmd = WLC_GET_VAR;
+	ioc.buf = buf;
+	ioc.len = (uint)sizeof(buf);
+	ioc.set = FALSE;
+
+	strcpy(buf, "toe_ol");
+	if ((ret = dhd_prot_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len)) < 0) {
+		/* Check for older dongle image that doesn't support toe_ol */
+		if (ret == -EIO) {
+			DHD_ERROR(("%s: toe not supported by device\n",
+				dhd_ifname(&dhd->pub, ifidx)));
+			return -EOPNOTSUPP;
+		}
+
+		DHD_INFO(("%s: could not get toe_ol: ret=%d\n", dhd_ifname(&dhd->pub, ifidx), ret));
+		return ret;
+	}
+
+	memcpy(toe_ol, buf, sizeof(uint32));
+	return 0;
+}
+
+/* Set current toe component enables in toe_ol iovar, and set toe global enable iovar */
+static int
+dhd_toe_set(dhd_info_t *dhd, int ifidx, uint32 toe_ol)
+{
+	wl_ioctl_t ioc;
+	char buf[32];
+	int toe, ret;
+
+	memset(&ioc, 0, sizeof(ioc));
+
+	ioc.cmd = WLC_SET_VAR;
+	ioc.buf = buf;
+	ioc.len = (uint)sizeof(buf);
+	ioc.set = TRUE;
+
+	/* Set toe_ol as requested */
+
+	strcpy(buf, "toe_ol");
+	memcpy(&buf[sizeof("toe_ol")], &toe_ol, sizeof(uint32));
+
+	if ((ret = dhd_prot_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len)) < 0) {
+		DHD_ERROR(("%s: could not set toe_ol: ret=%d\n",
+			dhd_ifname(&dhd->pub, ifidx), ret));
+		return ret;
+	}
+
+	/* Enable toe globally only if any components are enabled. */
+
+	toe = (toe_ol != 0);
+
+	strcpy(buf, "toe");
+	memcpy(&buf[sizeof("toe")], &toe, sizeof(uint32));
+
+	if ((ret = dhd_prot_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len)) < 0) {
+		DHD_ERROR(("%s: could not set toe: ret=%d\n", dhd_ifname(&dhd->pub, ifidx), ret));
+		return ret;
+	}
+
+	return 0;
+}
+#endif /* TOE */
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24)
+static void dhd_ethtool_get_drvinfo(struct net_device *net,
+                                    struct ethtool_drvinfo *info)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
+
+	sprintf(info->driver, "wl");
+	sprintf(info->version, "%lu", dhd->pub.drv_version);
+}
+
+struct ethtool_ops dhd_ethtool_ops = {
+	.get_drvinfo = dhd_ethtool_get_drvinfo
+};
+#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24) */
+
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 4, 2)
+static int
+dhd_ethtool(dhd_info_t *dhd, void *uaddr)
+{
+	struct ethtool_drvinfo info;
+	char drvname[sizeof(info.driver)];
+	uint32 cmd;
+#ifdef TOE
+	struct ethtool_value edata;
+	uint32 toe_cmpnt, csum_dir;
+	int ret;
+#endif
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	/* all ethtool calls start with a cmd word */
+	if (copy_from_user(&cmd, uaddr, sizeof (uint32)))
+		return -EFAULT;
+
+	switch (cmd) {
+	case ETHTOOL_GDRVINFO:
+		/* Copy out any request driver name */
+		if (copy_from_user(&info, uaddr, sizeof(info)))
+			return -EFAULT;
+		strncpy(drvname, info.driver, sizeof(info.driver));
+		drvname[sizeof(info.driver)-1] = '\0';
+
+		/* clear struct for return */
+		memset(&info, 0, sizeof(info));
+		info.cmd = cmd;
+
+		/* if dhd requested, identify ourselves */
+		if (strcmp(drvname, "?dhd") == 0) {
+			sprintf(info.driver, "dhd");
+			strcpy(info.version, EPI_VERSION_STR);
+		}
+
+		/* otherwise, require dongle to be up */
+		else if (!dhd->pub.up) {
+			DHD_ERROR(("%s: dongle is not up\n", __FUNCTION__));
+			return -ENODEV;
+		}
+
+		/* finally, report dongle driver type */
+		else if (dhd->pub.iswl)
+			sprintf(info.driver, "wl");
+		else
+			sprintf(info.driver, "xx");
+
+		sprintf(info.version, "%lu", dhd->pub.drv_version);
+		if (copy_to_user(uaddr, &info, sizeof(info)))
+			return -EFAULT;
+		DHD_CTL(("%s: given %*s, returning %s\n", __FUNCTION__,
+		         (int)sizeof(drvname), drvname, info.driver));
+		break;
+
+#ifdef TOE
+	/* Get toe offload components from dongle */
+	case ETHTOOL_GRXCSUM:
+	case ETHTOOL_GTXCSUM:
+		if ((ret = dhd_toe_get(dhd, 0, &toe_cmpnt)) < 0)
+			return ret;
+
+		csum_dir = (cmd == ETHTOOL_GTXCSUM) ? TOE_TX_CSUM_OL : TOE_RX_CSUM_OL;
+
+		edata.cmd = cmd;
+		edata.data = (toe_cmpnt & csum_dir) ? 1 : 0;
+
+		if (copy_to_user(uaddr, &edata, sizeof(edata)))
+			return -EFAULT;
+		break;
+
+	/* Set toe offload components in dongle */
+	case ETHTOOL_SRXCSUM:
+	case ETHTOOL_STXCSUM:
+		if (copy_from_user(&edata, uaddr, sizeof(edata)))
+			return -EFAULT;
+
+		/* Read the current settings, update and write back */
+		if ((ret = dhd_toe_get(dhd, 0, &toe_cmpnt)) < 0)
+			return ret;
+
+		csum_dir = (cmd == ETHTOOL_STXCSUM) ? TOE_TX_CSUM_OL : TOE_RX_CSUM_OL;
+
+		if (edata.data != 0)
+			toe_cmpnt |= csum_dir;
+		else
+			toe_cmpnt &= ~csum_dir;
+
+		if ((ret = dhd_toe_set(dhd, 0, toe_cmpnt)) < 0)
+			return ret;
+
+		/* If setting TX checksum mode, tell Linux the new mode */
+		if (cmd == ETHTOOL_STXCSUM) {
+			if (edata.data)
+				dhd->iflist[0]->net->features |= NETIF_F_IP_CSUM;
+			else
+				dhd->iflist[0]->net->features &= ~NETIF_F_IP_CSUM;
+		}
+
+		break;
+#endif /* TOE */
+
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+#endif /* LINUX_VERSION_CODE > KERNEL_VERSION(2, 4, 2) */
+
+static int
+dhd_ioctl_entry(struct net_device *net, struct ifreq *ifr, int cmd)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
+	dhd_ioctl_t ioc;
+	int bcmerror = 0;
+	int buflen = 0;
+	void *buf = NULL;
+	uint driver = 0;
+	int ifidx;
+	bool is_set_key_cmd;
+	int ret;
+
+	dhd_os_wake_lock(&dhd->pub);
+
+	/* send to dongle only if we are not waiting for reload already */
+	if (dhd->pub.hang_was_sent) {
+		DHD_ERROR(("%s: HANG was sent up earlier\n", __FUNCTION__));
+		dhd_os_wake_lock_timeout_enable(&dhd->pub);
+		dhd_os_wake_unlock(&dhd->pub);
+		return OSL_ERROR(BCME_DONGLE_DOWN);
+	}
+
+	ifidx = dhd_net2idx(dhd, net);
+	DHD_TRACE(("%s: ifidx %d, cmd 0x%04x\n", __FUNCTION__, ifidx, cmd));
+
+	if (ifidx == DHD_BAD_IF) {
+		dhd_os_wake_unlock(&dhd->pub);
+		return -1;
+	}
+
+#if defined(CONFIG_WIRELESS_EXT)
+	/* linux wireless extensions */
+	if ((cmd >= SIOCIWFIRST) && (cmd <= SIOCIWLAST)) {
+		/* may recurse, do NOT lock */
+		ret = wl_iw_ioctl(net, ifr, cmd);
+		dhd_os_wake_unlock(&dhd->pub);
+		return ret;
+	}
+#endif /* defined(CONFIG_WIRELESS_EXT) */
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 4, 2)
+	if (cmd == SIOCETHTOOL) {
+		ret = dhd_ethtool(dhd, (void*)ifr->ifr_data);
+		dhd_os_wake_unlock(&dhd->pub);
+		return ret;
+	}
+#endif /* LINUX_VERSION_CODE > KERNEL_VERSION(2, 4, 2) */
+
+	if (cmd != SIOCDEVPRIVATE) {
+		dhd_os_wake_unlock(&dhd->pub);
+		return -EOPNOTSUPP;
+	}
+
+	memset(&ioc, 0, sizeof(ioc));
+
+	/* Copy the ioc control structure part of ioctl request */
+	if (copy_from_user(&ioc, ifr->ifr_data, sizeof(wl_ioctl_t))) {
+		bcmerror = -BCME_BADADDR;
+		goto done;
+	}
+
+	/* Copy out any buffer passed */
+	if (ioc.buf) {
+		buflen = MIN(ioc.len, DHD_IOCTL_MAXLEN);
+		/* optimization for direct ioctl calls from kernel */
+		/*
+		if (segment_eq(get_fs(), KERNEL_DS)) {
+			buf = ioc.buf;
+		} else {
+		*/
+		{
+			if (!(buf = (char*)MALLOC(dhd->pub.osh, buflen))) {
+				bcmerror = -BCME_NOMEM;
+				goto done;
+			}
+			if (copy_from_user(buf, ioc.buf, buflen)) {
+				bcmerror = -BCME_BADADDR;
+				goto done;
+			}
+		}
+	}
+
+	/* To differentiate between wl and dhd read 4 more byes */
+	if ((copy_from_user(&driver, (char *)ifr->ifr_data + sizeof(wl_ioctl_t),
+		sizeof(uint)) != 0)) {
+		bcmerror = -BCME_BADADDR;
+		goto done;
+	}
+
+	if (!capable(CAP_NET_ADMIN)) {
+		bcmerror = -BCME_EPERM;
+		goto done;
+	}
+
+	/* check for local dhd ioctl and handle it */
+	if (driver == DHD_IOCTL_MAGIC) {
+		bcmerror = dhd_ioctl((void *)&dhd->pub, &ioc, buf, buflen);
+		if (bcmerror)
+			dhd->pub.bcmerror = bcmerror;
+		goto done;
+	}
+
+	/* send to dongle (must be up, and wl) */
+	if (dhd->pub.busstate != DHD_BUS_DATA) {
+		DHD_ERROR(("%s DONGLE_DOWN\n", __FUNCTION__));
+		bcmerror = BCME_DONGLE_DOWN;
+		goto done;
+	}
+
+	if (!dhd->pub.iswl) {
+		bcmerror = BCME_DONGLE_DOWN;
+		goto done;
+	}
+
+	/* Intercept WLC_SET_KEY IOCTL - serialize M4 send and set key IOCTL to
+	 * prevent M4 encryption.
+	 */
+	is_set_key_cmd = ((ioc.cmd == WLC_SET_KEY) ||
+	                 ((ioc.cmd == WLC_SET_VAR) &&
+	                        !(strncmp("wsec_key", ioc.buf, 9))) ||
+	                 ((ioc.cmd == WLC_SET_VAR) &&
+	                        !(strncmp("bsscfg:wsec_key", ioc.buf, 15))));
+	if (is_set_key_cmd) {
+		dhd_wait_pend8021x(net);
+	}
+
+	bcmerror = dhd_prot_ioctl(&dhd->pub, ifidx, (wl_ioctl_t *)&ioc, buf, buflen);
+
+done:
+	if ((bcmerror == -ETIMEDOUT) || ((dhd->pub.busstate == DHD_BUS_DOWN) &&
+			(!dhd->pub.dongle_reset))) {
+		DHD_ERROR(("%s: Event HANG send up\n", __FUNCTION__));
+		net_os_send_hang_message(net);
+	}
+
+	if (!bcmerror && buf && ioc.buf) {
+		if (copy_to_user(ioc.buf, buf, buflen))
+			bcmerror = -EFAULT;
+	}
+
+	if (buf)
+		MFREE(dhd->pub.osh, buf, buflen);
+
+	dhd_os_wake_unlock(&dhd->pub);
+
+	return OSL_ERROR(bcmerror);
+}
+
+static int
+dhd_stop(struct net_device *net)
+{
+#if !defined(IGNORE_ETH0_DOWN)
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
+
+	DHD_TRACE(("%s: Enter %s\n", __FUNCTION__, net->name));
+	if (dhd->pub.up == 0) {
+		return 0;
+	}
+
+	/* Set state and stop OS transmissions */
+	dhd->pub.up = 0;
+	netif_stop_queue(net);
+#else
+	DHD_ERROR(("BYPASS %s:due to BRCM compilation : under investigation ...\n", __FUNCTION__));
+#endif /* !defined(IGNORE_ETH0_DOWN) */
+	dhd->pub.hang_was_sent = 0;
+	OLD_MOD_DEC_USE_COUNT;
+	return 0;
+}
+
+static int
+dhd_open(struct net_device *net)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
+#ifdef TOE
+	uint32 toe_ol;
+#endif
+	int ifidx;
+
+	/*  Force start if ifconfig_up gets called before START command */
+	wl_control_wl_start(net);
+
+	ifidx = dhd_net2idx(dhd, net);
+	DHD_TRACE(("%s: ifidx %d\n", __FUNCTION__, ifidx));
+
+	if (ifidx == DHD_BAD_IF)
+		return -1;
+
+	if ((dhd->iflist[ifidx]) && (dhd->iflist[ifidx]->state == WLC_E_IF_DEL)) {
+		DHD_ERROR(("%s: Error: called when IF already deleted\n", __FUNCTION__));
+		return -1;
+	}
+
+	if (ifidx == 0) { /* do it only for primary eth0 */
+
+		atomic_set(&dhd->pend_8021x_cnt, 0);
+
+	memcpy(net->dev_addr, dhd->pub.mac.octet, ETHER_ADDR_LEN);
+
+#ifdef TOE
+	/* Get current TOE mode from dongle */
+	if (dhd_toe_get(dhd, ifidx, &toe_ol) >= 0 && (toe_ol & TOE_TX_CSUM_OL) != 0)
+		dhd->iflist[ifidx]->net->features |= NETIF_F_IP_CSUM;
+	else
+		dhd->iflist[ifidx]->net->features &= ~NETIF_F_IP_CSUM;
+#endif
+	}
+	/* Allow transmit calls */
+	netif_start_queue(net);
+	dhd->pub.up = 1;
+
+	OLD_MOD_INC_USE_COUNT;
+	return 0;
+}
+
+osl_t *
+dhd_osl_attach(void *pdev, uint bustype)
+{
+	return osl_attach(pdev, bustype, TRUE);
+}
+
+void
+dhd_osl_detach(osl_t *osh)
+{
+	if (MALLOCED(osh)) {
+		DHD_ERROR(("%s: MEMORY LEAK %d bytes\n", __FUNCTION__, MALLOCED(osh)));
+	}
+	osl_detach(osh);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && 1
+	up(&dhd_registration_sem);
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
+}
+
+int
+dhd_add_if(dhd_info_t *dhd, int ifidx, void *handle, char *name,
+	uint8 *mac_addr, uint32 flags, uint8 bssidx)
+{
+	dhd_if_t *ifp;
+
+	DHD_TRACE(("%s: idx %d, handle->%p\n", __FUNCTION__, ifidx, handle));
+
+	ASSERT(dhd && (ifidx < DHD_MAX_IFS));
+
+	ifp = dhd->iflist[ifidx];
+	if (!ifp && !(ifp = MALLOC(dhd->pub.osh, sizeof(dhd_if_t)))) {
+		DHD_ERROR(("%s: OOM - dhd_if_t\n", __FUNCTION__));
+		return -ENOMEM;
+	}
+
+	memset(ifp, 0, sizeof(dhd_if_t));
+	ifp->info = dhd;
+	dhd->iflist[ifidx] = ifp;
+	strncpy(ifp->name, name, IFNAMSIZ);
+	ifp->name[IFNAMSIZ] = '\0';
+	if (mac_addr != NULL)
+		memcpy(&ifp->mac_addr, mac_addr, ETHER_ADDR_LEN);
+
+	if (handle == NULL) {
+		ifp->state = WLC_E_IF_ADD;
+		ifp->idx = ifidx;
+		ASSERT(dhd->sysioc_pid >= 0);
+		up(&dhd->sysioc_sem);
+	} else
+		ifp->net = (struct net_device *)handle;
+
+	return 0;
+}
+
+void
+dhd_del_if(dhd_info_t *dhd, int ifidx)
+{
+	dhd_if_t *ifp;
+
+	DHD_TRACE(("%s: idx %d\n", __FUNCTION__, ifidx));
+
+	ASSERT(dhd && ifidx && (ifidx < DHD_MAX_IFS));
+	ifp = dhd->iflist[ifidx];
+	if (!ifp) {
+		DHD_ERROR(("%s: Null interface\n", __FUNCTION__));
+		return;
+	}
+
+	ifp->state = WLC_E_IF_DEL;
+	ifp->idx = ifidx;
+	ASSERT(dhd->sysioc_pid >= 0);
+	up(&dhd->sysioc_sem);
+}
+
+
+dhd_pub_t *
+dhd_attach(osl_t *osh, struct dhd_bus *bus, uint bus_hdrlen, void *dev)
+{
+	dhd_info_t *dhd = NULL;
+	struct net_device *net;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+	/* updates firmware nvram path if it was provided as module paramters */
+	if ((firmware_path != NULL) && (firmware_path[0] != '\0'))
+		strcpy(fw_path, firmware_path);
+	if ((nvram_path != NULL) && (nvram_path[0] != '\0'))
+		strcpy(nv_path, nvram_path);
+
+	/* Allocate etherdev, including space for private structure */
+	if (!(net = alloc_etherdev(sizeof(dhd)))) {
+		DHD_ERROR(("%s: OOM - alloc_etherdev\n", __FUNCTION__));
+		goto fail;
+	}
+	SET_NETDEV_DEV(net, (struct device *)dev);
+	/* Allocate primary dhd_info */
+	if (!(dhd = MALLOC(osh, sizeof(dhd_info_t)))) {
+		DHD_ERROR(("%s: OOM - alloc dhd_info\n", __FUNCTION__));
+		goto fail;
+	}
+
+	memset(dhd, 0, sizeof(dhd_info_t));
+
+	/*
+	 * Save the dhd_info into the priv
+	 */
+	memcpy(netdev_priv(net), &dhd, sizeof(dhd));
+	dhd->pub.osh = osh;
+
+	/* Set network interface name if it was provided as module parameter */
+	if (iface_name[0]) {
+		int len;
+		char ch;
+		strncpy(net->name, iface_name, IFNAMSIZ);
+		net->name[IFNAMSIZ - 1] = 0;
+		len = strlen(net->name);
+		ch = net->name[len - 1];
+		if ((ch > '9' || ch < '0') && (len < IFNAMSIZ - 2))
+			strcat(net->name, "%d");
+	}
+
+	if (dhd_add_if(dhd, 0, (void *)net, net->name, NULL, 0, 0) == DHD_BAD_IF)
+		goto fail;
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 31))
+	net->open = NULL;
+#else
+	net->netdev_ops = NULL;
+#endif
+
+	mutex_init(&dhd->proto_sem);
+	/* Initialize other structure content */
+	init_waitqueue_head(&dhd->ioctl_resp_wait);
+	init_waitqueue_head(&dhd->ctrl_wait);
+
+	/* Initialize the spinlocks */
+	spin_lock_init(&dhd->sdlock);
+	spin_lock_init(&dhd->txqlock);
+	spin_lock_init(&dhd->dhd_lock);
+
+	/* Initialize Wakelock stuff */
+	spin_lock_init(&dhd->wl_lock);
+	dhd->wl_count = 0;
+	dhd->wl_packet = 0;
+#ifdef CONFIG_HAS_WAKELOCK
+	wake_lock_init(&dhd->wl_wifi, WAKE_LOCK_SUSPEND, "wlan_wake");
+	wake_lock_init(&dhd->wl_rxwake, WAKE_LOCK_SUSPEND, "wlan_rx_wake");
+#endif
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
+	mutex_init(&dhd->wl_start_lock);
+#endif
+	/* Link to info module */
+	dhd->pub.info = dhd;
+
+	/* Link to bus module */
+	dhd->pub.bus = bus;
+	dhd->pub.hdrlen = bus_hdrlen;
+
+	/* Attach and link in the protocol */
+	if (dhd_prot_attach(&dhd->pub) != 0) {
+		DHD_ERROR(("dhd_prot_attach failed\n"));
+		goto fail;
+	}
+#if defined(CONFIG_WIRELESS_EXT)
+	/* Attach and link in the iw */
+	if (wl_iw_attach(net, (void *)&dhd->pub) != 0) {
+		DHD_ERROR(("wl_iw_attach failed\n"));
+		goto fail;
+	}
+#endif /* defined(CONFIG_WIRELESS_EXT) */
+
+	/* Set up the watchdog timer */
+	init_timer(&dhd->timer);
+	dhd->timer.data = (ulong)dhd;
+	dhd->timer.function = dhd_watchdog;
+
+	/* Initialize thread based operation and lock */
+	mutex_init(&dhd->sdsem);
+	if ((dhd_watchdog_prio >= 0) && (dhd_dpc_prio >= 0)) {
+		dhd->threads_only = TRUE;
+	}
+	else {
+		dhd->threads_only = FALSE;
+	}
+
+	if (dhd_dpc_prio >= 0) {
+		/* Initialize watchdog thread */
+		sema_init(&dhd->watchdog_sem, 0);
+		init_completion(&dhd->watchdog_exited);
+		dhd->watchdog_pid = kernel_thread(dhd_watchdog_thread, dhd, 0);
+	} else {
+		dhd->watchdog_pid = -1;
+	}
+
+	/* Set up the bottom half handler */
+	if (dhd_dpc_prio >= 0) {
+		/* Initialize DPC thread */
+		sema_init(&dhd->dpc_sem, 0);
+		init_completion(&dhd->dpc_exited);
+		dhd->dpc_pid = kernel_thread(dhd_dpc_thread, dhd, 0);
+	} else {
+		tasklet_init(&dhd->tasklet, dhd_dpc, (ulong)dhd);
+		dhd->dpc_pid = -1;
+	}
+
+	if (dhd_sysioc) {
+		sema_init(&dhd->sysioc_sem, 0);
+		init_completion(&dhd->sysioc_exited);
+		dhd->sysioc_pid = kernel_thread(_dhd_sysioc_thread, dhd, 0);
+	} else {
+		dhd->sysioc_pid = -1;
+	}
+
+	/*
+	 * Save the dhd_info into the priv
+	 */
+	memcpy(netdev_priv(net), &dhd, sizeof(dhd));
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP)
+	register_pm_notifier(&dhd_sleep_pm_notifier);
+#endif /*  (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) */
+
+#ifdef CONFIG_HAS_WAKELOCK
+	wake_lock_init(&dhd->pub.wow_wakelock, WAKE_LOCK_SUSPEND, "wow_wake_lock");
+#endif
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	dhd->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 20;
+	dhd->early_suspend.suspend = dhd_early_suspend;
+	dhd->early_suspend.resume = dhd_late_resume;
+	register_early_suspend(&dhd->early_suspend);
+#endif
+
+	register_inetaddr_notifier(&dhd_notifier);
+
+	return &dhd->pub;
+
+fail:
+	if (net)
+		free_netdev(net);
+	if (dhd)
+		dhd_detach(&dhd->pub);
+
+	return NULL;
+}
+
+
+int
+dhd_bus_start(dhd_pub_t *dhdp)
+{
+	int ret = -1;
+	dhd_info_t *dhd = (dhd_info_t*)dhdp->info;
+#ifdef EMBEDDED_PLATFORM
+	char iovbuf[WL_EVENTING_MASK_LEN + 12];	/*  Room for "event_msgs" + '\0' + bitvec  */
+#endif /* EMBEDDED_PLATFORM */
+
+	ASSERT(dhd);
+
+	DHD_TRACE(("%s: \n", __FUNCTION__));
+
+	dhd_os_sdlock(dhdp);
+
+	/* try to download image and nvram to the dongle */
+	if  (dhd->pub.busstate == DHD_BUS_DOWN) {
+		if (!(dhd_bus_download_firmware(dhd->pub.bus, dhd->pub.osh,
+		                                fw_path, nv_path))) {
+			DHD_ERROR(("%s: dhdsdio_probe_download failed. firmware = %s nvram = %s\n",
+			           __FUNCTION__, fw_path, nv_path));
+			dhd_os_sdunlock(dhdp);
+			return -1;
+		}
+	}
+
+	/* Start the watchdog timer */
+	dhd->pub.tickcnt = 0;
+	dhd_os_wd_timer(&dhd->pub, dhd_watchdog_ms);
+
+	/* Bring up the bus */
+	if ((ret = dhd_bus_init(&dhd->pub, FALSE)) != 0) {
+		DHD_ERROR(("%s, dhd_bus_init failed %d\n", __FUNCTION__, ret));
+		dhd_os_sdunlock(dhdp);
+		return ret;
+	}
+#if defined(OOB_INTR_ONLY)
+	/* Host registration for OOB interrupt */
+	if (bcmsdh_register_oob_intr(dhdp)) {
+		dhd->wd_timer_valid = FALSE;
+		del_timer_sync(&dhd->timer);
+		DHD_ERROR(("%s Host failed to resgister for OOB\n", __FUNCTION__));
+		dhd_os_sdunlock(dhdp);
+		return -ENODEV;
+	}
+
+	/* Enable oob at firmware */
+	dhd_enable_oob_intr(dhd->pub.bus, TRUE);
+#endif /* defined(OOB_INTR_ONLY) */
+
+	/* If bus is not ready, can't come up */
+	if (dhd->pub.busstate != DHD_BUS_DATA) {
+		dhd->wd_timer_valid = FALSE;
+		del_timer_sync(&dhd->timer);
+		DHD_ERROR(("%s failed bus is not ready\n", __FUNCTION__));
+		dhd_os_sdunlock(dhdp);
+		return -ENODEV;
+	}
+
+	dhd_os_sdunlock(dhdp);
+
+#ifdef EMBEDDED_PLATFORM
+	bcm_mkiovar("event_msgs", dhdp->eventmask, WL_EVENTING_MASK_LEN, iovbuf, sizeof(iovbuf));
+	dhdcdc_query_ioctl(dhdp, 0, WLC_GET_VAR, iovbuf, sizeof(iovbuf));
+	bcopy(iovbuf, dhdp->eventmask, WL_EVENTING_MASK_LEN);
+
+	setbit(dhdp->eventmask, WLC_E_SET_SSID);
+	setbit(dhdp->eventmask, WLC_E_PRUNE);
+	setbit(dhdp->eventmask, WLC_E_AUTH);
+	setbit(dhdp->eventmask, WLC_E_REASSOC);
+	setbit(dhdp->eventmask, WLC_E_REASSOC_IND);
+	setbit(dhdp->eventmask, WLC_E_DEAUTH_IND);
+	setbit(dhdp->eventmask, WLC_E_DISASSOC_IND);
+	setbit(dhdp->eventmask, WLC_E_DISASSOC);
+	setbit(dhdp->eventmask, WLC_E_JOIN);
+	setbit(dhdp->eventmask, WLC_E_ASSOC_IND);
+	setbit(dhdp->eventmask, WLC_E_PSK_SUP);
+	setbit(dhdp->eventmask, WLC_E_LINK);
+	setbit(dhdp->eventmask, WLC_E_NDIS_LINK);
+	setbit(dhdp->eventmask, WLC_E_MIC_ERROR);
+	setbit(dhdp->eventmask, WLC_E_PMKID_CACHE);
+	setbit(dhdp->eventmask, WLC_E_TXFAIL);
+	setbit(dhdp->eventmask, WLC_E_JOIN_START);
+	setbit(dhdp->eventmask, WLC_E_SCAN_COMPLETE);
+	setbit(dhdp->eventmask, WLC_E_RELOAD);
+#ifdef PNO_SUPPORT
+	setbit(dhdp->eventmask, WLC_E_PFN_NET_FOUND);
+#endif /* PNO_SUPPORT */
+
+/* enable dongle roaming event */
+	setbit(dhdp->eventmask, WLC_E_ROAM);
+
+	dhdp->pktfilter_count = 4;
+	/* Setup filter to allow only unicast */
+	dhdp->pktfilter[0] = "100 0 0 0 0x01 0x00";
+	dhdp->pktfilter[1] = NULL;
+	dhdp->pktfilter[2] = NULL;
+	dhdp->pktfilter[3] = NULL;
+#endif /* EMBEDDED_PLATFORM */
+
+	/* Bus is ready, do any protocol initialization */
+	if ((ret = dhd_prot_init(&dhd->pub)) < 0)
+		return ret;
+
+	return 0;
+}
+
+int
+dhd_iovar(dhd_pub_t *pub, int ifidx, char *name, char *cmd_buf, uint cmd_len, int set)
+{
+	char buf[strlen(name) + 1 + cmd_len];
+	int len = sizeof(buf);
+	wl_ioctl_t ioc;
+	int ret;
+
+	len = bcm_mkiovar(name, cmd_buf, cmd_len, buf, len);
+
+	memset(&ioc, 0, sizeof(ioc));
+
+	ioc.cmd = set? WLC_SET_VAR : WLC_GET_VAR;
+	ioc.buf = buf;
+	ioc.len = len;
+	ioc.set = set;
+
+	ret = dhd_prot_ioctl(pub, ifidx, &ioc, ioc.buf, ioc.len);
+	if (!set && ret >= 0)
+		memcpy(cmd_buf, buf, cmd_len);
+
+	return ret;
+}
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 31))
+static struct net_device_ops dhd_ops_pri = {
+	.ndo_open = dhd_open,
+	.ndo_stop = dhd_stop,
+	.ndo_get_stats = dhd_get_stats,
+	.ndo_do_ioctl = dhd_ioctl_entry,
+	.ndo_start_xmit = dhd_start_xmit,
+	.ndo_set_mac_address = dhd_set_mac_address,
+	.ndo_set_multicast_list = dhd_set_multicast_list,
+};
+
+static struct net_device_ops dhd_ops_virt = {
+	.ndo_get_stats = dhd_get_stats,
+	.ndo_do_ioctl = dhd_ioctl_entry,
+	.ndo_start_xmit = dhd_start_xmit,
+	.ndo_set_mac_address = dhd_set_mac_address,
+	.ndo_set_multicast_list = dhd_set_multicast_list,
+};
+#endif
+
+static int dhd_device_event(struct notifier_block *this, unsigned long event,
+				void *ptr)
+{
+	struct in_ifaddr *ifa = (struct in_ifaddr *)ptr;
+	dhd_info_t *dhd;
+	dhd_pub_t *dhd_pub;
+
+	if (!ifa)
+		return NOTIFY_DONE;
+
+	dhd = *(dhd_info_t **)netdev_priv(ifa->ifa_dev->dev);
+	dhd_pub = &dhd->pub;
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 31))
+	if (ifa->ifa_dev->dev->netdev_ops == &dhd_ops_pri) {
+#else
+	if (ifa->ifa_dev->dev->open == &dhd_open) {
+#endif
+		switch (event) {
+		case NETDEV_UP:
+			DHD_TRACE(("%s: [%s] Up IP: 0x%x\n",
+			    __FUNCTION__, ifa->ifa_label, ifa->ifa_address));
+
+			dhd_arp_cleanup(dhd_pub);
+			break;
+
+		case NETDEV_DOWN:
+			DHD_TRACE(("%s: [%s] Down IP: 0x%x\n",
+			    __FUNCTION__, ifa->ifa_label, ifa->ifa_address));
+
+			dhd_arp_cleanup(dhd_pub);
+			break;
+
+		default:
+			DHD_TRACE(("%s: [%s] Event: %lu\n",
+			    __FUNCTION__, ifa->ifa_label, event));
+			break;
+		}
+	}
+	return NOTIFY_DONE;
+}
+
+int
+dhd_net_attach(dhd_pub_t *dhdp, int ifidx)
+{
+	dhd_info_t *dhd = (dhd_info_t *)dhdp->info;
+	struct net_device *net;
+	uint8 temp_addr[ETHER_ADDR_LEN] = { 0x00, 0x90, 0x4c, 0x11, 0x22, 0x33 };
+
+	DHD_TRACE(("%s: ifidx %d\n", __FUNCTION__, ifidx));
+
+	ASSERT(dhd && dhd->iflist[ifidx]);
+	net = dhd->iflist[ifidx]->net;
+
+	ASSERT(net);
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 31))
+	ASSERT(!net->open);
+	net->get_stats = dhd_get_stats;
+	net->do_ioctl = dhd_ioctl_entry;
+	net->hard_start_xmit = dhd_start_xmit;
+	net->set_mac_address = dhd_set_mac_address;
+	net->set_multicast_list = dhd_set_multicast_list;
+	net->open = net->stop = NULL;
+#else
+	ASSERT(!net->netdev_ops);
+	net->netdev_ops = &dhd_ops_virt;
+#endif
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 31))
+		net->open = dhd_open;
+		net->stop = dhd_stop;
+#else
+		net->netdev_ops = &dhd_ops_pri;
+#endif
+
+	/*
+	 * We have to use the primary MAC for virtual interfaces
+	 */
+	if (ifidx != 0) {
+		/* for virtual interfaces use the primary MAC  */
+		memcpy(temp_addr, dhd->pub.mac.octet, ETHER_ADDR_LEN);
+	}
+
+	if (ifidx == 1) {
+		DHD_TRACE(("%s ACCESS POINT MAC: \n", __FUNCTION__));
+		/*  ACCESSPOINT INTERFACE CASE */
+		temp_addr[0] |= 0x02;  /* set bit 2 , - Locally Administered address  */
+	}
+	net->hard_header_len = ETH_HLEN + dhd->pub.hdrlen;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24)
+	net->ethtool_ops = &dhd_ethtool_ops;
+#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24) */
+
+#if defined(CONFIG_WIRELESS_EXT)
+#if WIRELESS_EXT < 19
+	net->get_wireless_stats = dhd_get_wireless_stats;
+#endif /* WIRELESS_EXT < 19 */
+#if WIRELESS_EXT > 12
+	net->wireless_handlers = (struct iw_handler_def *)&wl_iw_handler_def;
+#endif /* WIRELESS_EXT > 12 */
+#endif /* defined(CONFIG_WIRELESS_EXT) */
+
+	dhd->pub.rxsz = net->mtu + net->hard_header_len + dhd->pub.hdrlen;
+
+	memcpy(net->dev_addr, temp_addr, ETHER_ADDR_LEN);
+
+	if (register_netdev(net) != 0) {
+		DHD_ERROR(("%s: couldn't register the net device\n", __FUNCTION__));
+		goto fail;
+	}
+
+	printf("%s: Broadcom Dongle Host Driver mac=%.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n", net->name,
+	       dhd->pub.mac.octet[0], dhd->pub.mac.octet[1], dhd->pub.mac.octet[2],
+	       dhd->pub.mac.octet[3], dhd->pub.mac.octet[4], dhd->pub.mac.octet[5]);
+
+
+#if defined(CONFIG_WIRELESS_EXT)
+#if defined(CONFIG_FIRST_SCAN)
+#ifdef SOFTAP
+	if (ifidx == 0)
+		/* Don't call for SOFTAP Interface in SOFTAP MODE */
+		wl_iw_iscan_set_scan_broadcast_prep(net, 1);
+#else
+		wl_iw_iscan_set_scan_broadcast_prep(net, 1);
+#endif /* SOFTAP */
+#endif /* CONFIG_FIRST_SCAN */
+#endif /* CONFIG_WIRELESS_EXT */
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	up(&dhd_registration_sem);
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
+	return 0;
+
+fail:
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 31))
+	net->open = NULL;
+#else
+	net->netdev_ops = NULL;
+#endif
+	return BCME_ERROR;
+}
+
+void
+dhd_bus_detach(dhd_pub_t *dhdp)
+{
+	dhd_info_t *dhd;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (dhdp) {
+		dhd = (dhd_info_t *)dhdp->info;
+		if (dhd) {
+			/* Stop the protocol module */
+			dhd_prot_stop(&dhd->pub);
+
+			/* Stop the bus module */
+			dhd_bus_stop(dhd->pub.bus, TRUE);
+#if defined(OOB_INTR_ONLY)
+			bcmsdh_unregister_oob_intr();
+#endif /* defined(OOB_INTR_ONLY) */
+
+			/* Clear the watchdog timer */
+			dhd->wd_timer_valid = FALSE;
+			del_timer_sync(&dhd->timer);
+		}
+	}
+}
+
+void
+dhd_detach(dhd_pub_t *dhdp)
+{
+	dhd_info_t *dhd;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (dhdp) {
+		dhd = (dhd_info_t *)dhdp->info;
+		if (dhd) {
+			dhd_if_t *ifp;
+			int i;
+
+			unregister_inetaddr_notifier(&dhd_notifier);
+
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+			if (dhd->early_suspend.suspend)
+				unregister_early_suspend(&dhd->early_suspend);
+#endif	/* defined(CONFIG_HAS_EARLYSUSPEND) */
+#if defined(CONFIG_WIRELESS_EXT)
+			/* Attach and link in the iw */
+			wl_iw_detach();
+#endif
+			if (dhd->sysioc_pid >= 0) {
+				KILL_PROC(dhd->sysioc_pid, SIGTERM);
+				wait_for_completion(&dhd->sysioc_exited);
+			}
+
+			for (i = 1; i < DHD_MAX_IFS; i++)
+				if (dhd->iflist[i]) {
+					dhd->iflist[i]->state = WLC_E_IF_DEL;
+					dhd->iflist[i]->idx = i;
+					dhd_op_if(dhd->iflist[i]);
+				}
+
+			ifp = dhd->iflist[0];
+			ASSERT(ifp);
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 31))
+			if (ifp->net->open) {
+#else
+			if (ifp->net->netdev_ops == &dhd_ops_pri) {
+#endif
+				dhd_stop(ifp->net);
+				unregister_netdev(ifp->net);
+			}
+
+			if (dhd->watchdog_pid >= 0)
+			{
+				KILL_PROC(dhd->watchdog_pid, SIGTERM);
+				wait_for_completion(&dhd->watchdog_exited);
+			}
+
+			if (dhd->dpc_pid >= 0)
+			{
+				KILL_PROC(dhd->dpc_pid, SIGTERM);
+				wait_for_completion(&dhd->dpc_exited);
+			}
+			else
+				tasklet_kill(&dhd->tasklet);
+
+			dhd_bus_detach(dhdp);
+
+			if (dhdp->prot)
+				dhd_prot_detach(dhdp);
+
+#ifdef CONFIG_HAS_WAKELOCK
+			wake_lock_destroy(&dhdp->wow_wakelock);
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP)
+			unregister_pm_notifier(&dhd_sleep_pm_notifier);
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) */
+			free_netdev(ifp->net);
+#ifdef CONFIG_HAS_WAKELOCK
+			wake_lock_destroy(&dhd->wl_wifi);
+			wake_lock_destroy(&dhd->wl_rxwake);
+#endif
+			MFREE(dhd->pub.osh, ifp, sizeof(*ifp));
+			MFREE(dhd->pub.osh, dhd, sizeof(*dhd));
+		}
+	}
+}
+
+static void __exit
+dhd_module_cleanup(void)
+{
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	dhd_bus_unregister();
+#if defined(CUSTOMER_HW2) && defined(CONFIG_WIFI_CONTROL_FUNC)
+	wifi_del_dev();
+#endif
+	/* Call customer gpio to turn off power with WL_REG_ON signal */
+	dhd_customer_gpio_wlan_ctrl(WLAN_POWER_OFF);
+}
+
+static int __init
+dhd_module_init(void)
+{
+	int error;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	/* Sanity check on the module parameters */
+	do {
+		/* Both watchdog and DPC as tasklets are ok */
+		if ((dhd_watchdog_prio < 0) && (dhd_dpc_prio < 0))
+			break;
+
+		/* If both watchdog and DPC are threads, TX must be deferred */
+		if ((dhd_watchdog_prio >= 0) && (dhd_dpc_prio >= 0) && dhd_deferred_tx)
+			break;
+
+		DHD_ERROR(("Invalid module parameters.\n"));
+		return -EINVAL;
+	} while (0);
+
+	/* Call customer gpio to turn on power with WL_REG_ON signal */
+	dhd_customer_gpio_wlan_ctrl(WLAN_POWER_ON);
+
+#if defined(CUSTOMER_HW2) && defined(CONFIG_WIFI_CONTROL_FUNC)
+	sema_init(&wifi_control_sem, 0);
+
+	error = wifi_add_dev();
+	if (error) {
+		DHD_ERROR(("%s: platform_driver_register failed\n", __FUNCTION__));
+		goto fail_0;
+	}
+
+	/* Waiting callback after platform_driver_register is done or exit with error */
+	if (down_timeout(&wifi_control_sem,  msecs_to_jiffies(5000)) != 0) {
+		error = -EINVAL;
+		DHD_ERROR(("%s: platform_driver_register timeout\n", __FUNCTION__));
+		goto fail_1;
+	}
+#endif /* #if defined(CUSTOMER_HW2) && defined(CONFIG_WIFI_CONTROL_FUNC) */
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	sema_init(&dhd_registration_sem, 0);
+#endif 
+
+	error = dhd_bus_register();
+
+	if (!error)
+		printf("\n%s\n", dhd_version);
+	else {
+		DHD_ERROR(("%s: sdio_register_driver failed\n", __FUNCTION__));
+		goto fail_1;
+	}
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	/*
+	 * Wait till MMC sdio_register_driver callback called and made driver attach.
+	 * It's needed to make sync up exit from dhd insmod  and
+	 * Kernel MMC sdio device callback registration
+	 */
+	if (down_timeout(&dhd_registration_sem,  msecs_to_jiffies(DHD_REGISTRATION_TIMEOUT)) != 0) {
+		error = -EINVAL;
+		DHD_ERROR(("%s: sdio_register_driver timeout\n", __FUNCTION__));
+		goto fail_2;
+	}
+#endif
+	return error;
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+fail_2:
+	dhd_bus_unregister();
+#endif
+fail_1:
+#if defined(CUSTOMER_HW2) && defined(CONFIG_WIFI_CONTROL_FUNC)
+	wifi_del_dev();
+fail_0:
+#endif /* defined(CUSTOMER_HW2) && defined(CONFIG_WIFI_CONTROL_FUNC) */
+
+	/* Call customer gpio to turn off power with WL_REG_ON signal */
+	dhd_customer_gpio_wlan_ctrl(WLAN_POWER_OFF);
+
+	return error;
+}
+
+module_init(dhd_module_init);
+module_exit(dhd_module_cleanup);
+
+/*
+ * OS specific functions required to implement DHD driver in OS independent way
+ */
+int
+dhd_os_proto_block(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+
+	if (dhd) {
+		mutex_lock(&dhd->proto_sem);
+		return 1;
+	}
+
+	return 0;
+}
+
+int
+dhd_os_proto_unblock(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+
+	if (dhd) {
+		mutex_unlock(&dhd->proto_sem);
+		return 1;
+	}
+
+	return 0;
+}
+
+unsigned int
+dhd_os_get_ioctl_resp_timeout(void)
+{
+	return ((unsigned int)dhd_ioctl_timeout_msec);
+}
+
+void
+dhd_os_set_ioctl_resp_timeout(unsigned int timeout_msec)
+{
+	dhd_ioctl_timeout_msec = (int)timeout_msec;
+}
+
+int
+dhd_os_ioctl_resp_wait(dhd_pub_t *pub, uint *condition, bool *pending)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+	DECLARE_WAITQUEUE(wait, current);
+	int timeout = dhd_ioctl_timeout_msec;
+
+	/* Convert timeout in millsecond to jiffies */
+	/* timeout = timeout * HZ / 1000; */
+	timeout = msecs_to_jiffies(timeout);
+
+	/* Wait until control frame is available */
+	add_wait_queue(&dhd->ioctl_resp_wait, &wait);
+	set_current_state(TASK_INTERRUPTIBLE);
+	smp_mb();
+	while (!(*condition) && (!signal_pending(current) && timeout)) {
+		timeout = schedule_timeout(timeout);
+		smp_mb();
+	}
+
+	if (signal_pending(current))
+		*pending = TRUE;
+
+	set_current_state(TASK_RUNNING);
+	remove_wait_queue(&dhd->ioctl_resp_wait, &wait);
+
+	return timeout;
+}
+
+int
+dhd_os_ioctl_resp_wake(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+
+	if (waitqueue_active(&dhd->ioctl_resp_wait)) {
+		wake_up_interruptible(&dhd->ioctl_resp_wait);
+	}
+
+	return 0;
+}
+
+void
+dhd_os_wd_timer(void *bus, uint wdtick)
+{
+	dhd_pub_t *pub = bus;
+	dhd_info_t *dhd = (dhd_info_t *)pub->info;
+	unsigned long flags;
+	int del_timer_flag = FALSE;
+
+	flags = dhd_os_spin_lock(pub);
+
+	/* don't start the wd until fw is loaded */
+	if (pub->busstate != DHD_BUS_DOWN) {
+		if (wdtick) {
+			dhd_watchdog_ms = (uint)wdtick;
+			dhd->wd_timer_valid = TRUE;
+			/* Re arm the timer, at last watchdog period */
+			mod_timer(&dhd->timer, jiffies + dhd_watchdog_ms * HZ / 1000);
+		} else if (dhd->wd_timer_valid == TRUE) {
+			/* Totally stop the timer */
+			dhd->wd_timer_valid = FALSE;
+			del_timer_flag = TRUE;
+		}
+	}
+	dhd_os_spin_unlock(pub, flags);
+	if (del_timer_flag) {
+		del_timer_sync(&dhd->timer);
+	}
+}
+
+void *
+dhd_os_open_image(char *filename)
+{
+	struct file *fp;
+
+	fp = filp_open(filename, O_RDONLY, 0);
+	/*
+	 * 2.6.11 (FC4) supports filp_open() but later revs don't?
+	 * Alternative:
+	 * fp = open_namei(AT_FDCWD, filename, O_RD, 0);
+	 * ???
+	 */
+	 if (IS_ERR(fp))
+		 fp = NULL;
+
+	 return fp;
+}
+
+int
+dhd_os_get_image_block(char *buf, int len, void *image)
+{
+	struct file *fp = (struct file *)image;
+	int rdlen;
+
+	if (!image)
+		return 0;
+
+	rdlen = kernel_read(fp, fp->f_pos, buf, len);
+	if (rdlen > 0)
+		fp->f_pos += rdlen;
+
+	return rdlen;
+}
+
+void
+dhd_os_close_image(void *image)
+{
+	if (image)
+		filp_close((struct file *)image, NULL);
+}
+
+
+void
+dhd_os_sdlock(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd;
+
+	dhd = (dhd_info_t *)(pub->info);
+
+	if (dhd->threads_only)
+		mutex_lock(&dhd->sdsem);
+	else
+		spin_lock_bh(&dhd->sdlock);
+}
+
+void
+dhd_os_sdunlock(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd;
+
+	dhd = (dhd_info_t *)(pub->info);
+
+	if (dhd->threads_only)
+		mutex_unlock(&dhd->sdsem);
+	else
+		spin_unlock_bh(&dhd->sdlock);
+}
+
+void
+dhd_os_sdlock_txq(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd;
+
+	dhd = (dhd_info_t *)(pub->info);
+	spin_lock_bh(&dhd->txqlock);
+}
+
+void
+dhd_os_sdunlock_txq(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd;
+
+	dhd = (dhd_info_t *)(pub->info);
+	spin_unlock_bh(&dhd->txqlock);
+}
+void
+dhd_os_sdlock_rxq(dhd_pub_t *pub)
+{
+}
+void
+dhd_os_sdunlock_rxq(dhd_pub_t *pub)
+{
+}
+
+void
+dhd_os_sdtxlock(dhd_pub_t *pub)
+{
+	dhd_os_sdlock(pub);
+}
+
+void
+dhd_os_sdtxunlock(dhd_pub_t *pub)
+{
+	dhd_os_sdunlock(pub);
+}
+
+#ifdef DHD_USE_STATIC_BUF
+void * dhd_os_prealloc(int section, unsigned long size)
+{
+#if defined(CUSTOMER_HW2) && defined(CONFIG_WIFI_CONTROL_FUNC)
+	void *alloc_ptr = NULL;
+	if (wifi_control_data && wifi_control_data->mem_prealloc)
+	{
+		alloc_ptr = wifi_control_data->mem_prealloc(section, size);
+		if (alloc_ptr)
+		{
+			DHD_INFO(("success alloc section %d\n", section));
+			bzero(alloc_ptr, size);
+			return alloc_ptr;
+		}
+	}
+
+	DHD_ERROR(("can't alloc section %d\n", section));
+	return 0;
+#else
+return MALLOC(0, size);
+#endif /* #if defined(CUSTOMER_HW2) && defined(CONFIG_WIFI_CONTROL_FUNC) */
+}
+#endif /* DHD_USE_STATIC_BUF */
+#if defined(CONFIG_WIRELESS_EXT)
+struct iw_statistics *
+dhd_get_wireless_stats(struct net_device *dev)
+{
+	int res = 0;
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	res = wl_iw_get_wireless_stats(dev, &dhd->iw.wstats);
+
+	if (res == 0)
+		return &dhd->iw.wstats;
+	else
+		return NULL;
+}
+#endif /* defined(CONFIG_WIRELESS_EXT) */
+
+static int
+dhd_wl_host_event(dhd_info_t *dhd, int *ifidx, void *pktdata,
+	wl_event_msg_t *event, void **data)
+{
+	int bcmerror = 0;
+
+	ASSERT(dhd != NULL);
+
+	bcmerror = wl_host_event(dhd, ifidx, pktdata, event, data);
+	if (bcmerror != BCME_OK)
+		return (bcmerror);
+
+#if defined(CONFIG_WIRELESS_EXT)
+	ASSERT(dhd->iflist[*ifidx] != NULL);
+
+	if (ntoh32(event->event_type) == WLC_E_IF) {
+		DHD_INFO(("<0> interface:%d OP:%d don't pass to wext,"
+			"net_device might not be created yet\n",
+				*ifidx, ntoh32(event->event_type)));
+		return bcmerror;
+	}
+
+	ASSERT(dhd->iflist[*ifidx]->net != NULL);
+
+	if (dhd->iflist[*ifidx]->net)
+		wl_iw_event(dhd->iflist[*ifidx]->net, event, *data);
+#endif /* defined(CONFIG_WIRELESS_EXT) */
+
+	return (bcmerror);
+}
+
+/* send up locally generated event */
+void
+dhd_sendup_event(dhd_pub_t *dhdp, wl_event_msg_t *event, void *data)
+{
+	switch (ntoh32(event->event_type)) {
+	default:
+		break;
+	}
+}
+
+void dhd_wait_for_event(dhd_pub_t *dhd, bool *lockvar)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+	struct dhd_info *dhdinfo =  dhd->info;
+	dhd_os_sdunlock(dhd);
+	wait_event_interruptible_timeout(dhdinfo->ctrl_wait, (*lockvar == FALSE), HZ * 2);
+	dhd_os_sdlock(dhd);
+#endif
+	return;
+}
+
+void dhd_wait_event_wakeup(dhd_pub_t *dhd)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+	struct dhd_info *dhdinfo =  dhd->info;
+	if (waitqueue_active(&dhdinfo->ctrl_wait))
+		wake_up_interruptible(&dhdinfo->ctrl_wait);
+#endif
+	return;
+}
+
+int
+dhd_dev_reset(struct net_device *dev, uint8 flag)
+{
+	int ret;
+
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	ret = dhd_bus_devreset(&dhd->pub, flag);
+	if (ret) {
+		DHD_ERROR(("%s: dhd_bus_devreset: %d\n", __FUNCTION__, ret));
+		return ret;
+	}
+	DHD_ERROR(("%s: WLAN %s DONE\n", __FUNCTION__, flag ? "OFF" : "ON"));
+
+	return ret;
+}
+
+int net_os_set_suspend_disable(struct net_device *dev, int val)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ret = 0;
+
+	if (dhd) {
+		ret = dhd->pub.suspend_disable_flag;
+		dhd->pub.suspend_disable_flag = val;
+	}
+	return ret;
+}
+
+int net_os_set_suspend(struct net_device *dev, int val)
+{
+	int ret = 0;
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	if (dhd) {
+		dhd_os_proto_block(&dhd->pub);
+		ret = dhd_set_suspend(val, &dhd->pub);
+		dhd_os_proto_unblock(&dhd->pub);
+	}
+#endif /* defined(CONFIG_HAS_EARLYSUSPEND) */
+	return ret;
+}
+
+int net_os_set_dtim_skip(struct net_device *dev, int val)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	if (dhd)
+		dhd->pub.dtim_skip = val;
+
+	return 0;
+}
+
+int net_os_rxfilter_add_remove(struct net_device *dev, int add_remove, int num)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	char *filterp = NULL;
+	int ret = 0;
+
+	if (!dhd || (num == DHD_UNICAST_FILTER_NUM))
+		return ret;
+	if (num >= dhd->pub.pktfilter_count)
+		return -EINVAL;
+	if (add_remove) {
+		switch (num) {
+		case DHD_BROADCAST_FILTER_NUM:
+			filterp = "101 0 0 0 0xFFFFFFFFFFFF 0xFFFFFFFFFFFF";
+			break;
+		case DHD_MULTICAST4_FILTER_NUM:
+			filterp = "102 0 0 0 0xFFFFFF 0x01005E";
+			break;
+		case DHD_MULTICAST6_FILTER_NUM:
+			filterp = "103 0 0 0 0xFFFF 0x3333";
+			break;
+		default:
+			return -EINVAL;
+		}
+	}
+	dhd->pub.pktfilter[num] = filterp;
+	return ret;
+}
+
+int net_os_set_packet_filter(struct net_device *dev, int val)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ret = 0;
+
+	/* Packet filtering is set only if we still in early-suspend and
+	 * we need either to turn it ON or turn it OFF
+	 * We can always turn it OFF in case of early-suspend, but we turn it
+	 * back ON only if suspend_disable_flag was not set
+	*/
+	if (dhd && dhd->pub.up) {
+		dhd_os_proto_block(&dhd->pub);
+		if (dhd->pub.in_suspend) {
+			if (!val || (val && !dhd->pub.suspend_disable_flag))
+				dhd_set_packet_filter(val, &dhd->pub);
+		}
+		dhd_os_proto_unblock(&dhd->pub);
+	}
+	return ret;
+}
+
+
+void
+dhd_dev_init_ioctl(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	dhd_preinit_ioctls(&dhd->pub);
+}
+
+#ifdef PNO_SUPPORT
+/* Linux wrapper to call common dhd_pno_clean */
+int
+dhd_dev_pno_reset(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	return (dhd_pno_clean(&dhd->pub));
+}
+
+
+/* Linux wrapper to call common dhd_pno_enable */
+int
+dhd_dev_pno_enable(struct net_device *dev,  int pfn_enabled)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	return (dhd_pno_enable(&dhd->pub, pfn_enabled));
+}
+
+
+/* Linux wrapper to call common dhd_pno_set */
+int
+dhd_dev_pno_set(struct net_device *dev, wlc_ssid_t* ssids_local, int nssid,
+		ushort scan_fr, int pno_repeat, int pno_freq_expo_max)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	return (dhd_pno_set(&dhd->pub, ssids_local, nssid, scan_fr, pno_repeat, pno_freq_expo_max));
+}
+
+/* Linux wrapper to get  pno status */
+int
+dhd_dev_get_pno_status(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	return (dhd_pno_get_status(&dhd->pub));
+}
+
+#endif /* PNO_SUPPORT */
+
+int net_os_send_hang_message(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ret = 0;
+
+	if (dhd) {
+		if (!dhd->pub.hang_was_sent) {
+			dhd->pub.hang_was_sent = 1;
+			ret = wl_iw_send_priv_event(dev, "HANG");
+		}
+	}
+	return ret;
+}
+
+void dhd_bus_country_set(struct net_device *dev, wl_country_t *cspec)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	if (dhd && dhd->pub.up)
+		memcpy(&dhd->pub.dhd_cspec, cspec, sizeof(wl_country_t));
+}
+
+char *dhd_bus_country_get(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	if (dhd && (dhd->pub.dhd_cspec.ccode[0] != 0))
+		return dhd->pub.dhd_cspec.ccode;
+	return NULL;
+}
+
+void dhd_os_start_lock(dhd_pub_t *pub)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+
+	if (dhd)
+		mutex_lock(&dhd->wl_start_lock);
+#endif
+}
+
+void dhd_os_start_unlock(dhd_pub_t *pub)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+
+	if (dhd)
+		mutex_unlock(&dhd->wl_start_lock);
+#endif
+}
+
+static int
+dhd_get_pend_8021x_cnt(dhd_info_t *dhd)
+{
+	return (atomic_read(&dhd->pend_8021x_cnt));
+}
+
+#define MAX_WAIT_FOR_8021X_TX	10
+
+int
+dhd_wait_pend8021x(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int timeout = 10 * HZ / 1000;
+	int ntimes = MAX_WAIT_FOR_8021X_TX;
+	int pend = dhd_get_pend_8021x_cnt(dhd);
+
+	while (ntimes && pend) {
+		if (pend) {
+			set_current_state(TASK_INTERRUPTIBLE);
+			schedule_timeout(timeout);
+			set_current_state(TASK_RUNNING);
+			ntimes--;
+		}
+		pend = dhd_get_pend_8021x_cnt(dhd);
+	}
+	return pend;
+}
+
+#ifdef DHD_DEBUG
+int
+write_to_file(dhd_pub_t *dhd, uint8 *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) {
+		printf("%s: open file error\n", __FUNCTION__);
+		ret = -1;
+		goto exit;
+	}
+
+	/* Write buf to file */
+	fp->f_op->write(fp, buf, size, &pos);
+
+exit:
+	/* free buf before return */
+	MFREE(dhd->osh, buf, size);
+	/* close file before return */
+	if (fp)
+		filp_close(fp, current->files);
+	/* restore previous address limit */
+	set_fs(old_fs);
+
+	return ret;
+}
+#endif /* DHD_DEBUG */
+
+int dhd_os_wake_lock_timeout(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+	unsigned long flags;
+	int ret = 0;
+
+	if (dhd) {
+		spin_lock_irqsave(&dhd->wl_lock, flags);
+		ret = dhd->wl_packet;
+#ifdef CONFIG_HAS_WAKELOCK
+		if (dhd->wl_packet)
+			wake_lock_timeout(&dhd->wl_rxwake, HZ);
+#endif
+		dhd->wl_packet = 0;
+		spin_unlock_irqrestore(&dhd->wl_lock, flags);
+	}
+	/* printk("%s: %d\n", __FUNCTION__, ret); */
+	return ret;
+}
+
+int net_os_wake_lock_timeout(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ret = 0;
+
+	if (dhd)
+		ret = dhd_os_wake_lock_timeout(&dhd->pub);
+	return ret;
+}
+
+int dhd_os_wake_lock_timeout_enable(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+	unsigned long flags;
+
+	if (dhd) {
+		spin_lock_irqsave(&dhd->wl_lock, flags);
+		dhd->wl_packet = 1;
+		spin_unlock_irqrestore(&dhd->wl_lock, flags);
+	}
+	/* printk("%s\n",__func__); */
+	return 0;
+}
+
+int net_os_wake_lock_timeout_enable(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ret = 0;
+
+	if (dhd)
+		ret = dhd_os_wake_lock_timeout_enable(&dhd->pub);
+	return ret;
+}
+
+int dhd_os_wake_lock(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+	unsigned long flags;
+	int ret = 0;
+
+	if (dhd) {
+		spin_lock_irqsave(&dhd->wl_lock, flags);
+#ifdef CONFIG_HAS_WAKELOCK
+		if (!dhd->wl_count)
+			wake_lock(&dhd->wl_wifi);
+#endif
+		dhd->wl_count++;
+		ret = dhd->wl_count;
+		spin_unlock_irqrestore(&dhd->wl_lock, flags);
+	}
+	/* printk("%s: %d\n", __FUNCTION__, ret); */
+	return ret;
+}
+
+int net_os_wake_lock(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ret = 0;
+
+	if (dhd)
+		ret = dhd_os_wake_lock(&dhd->pub);
+	return ret;
+}
+
+int dhd_os_wake_unlock(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+	unsigned long flags;
+	int ret = 0;
+
+	dhd_os_wake_lock_timeout(pub);
+	if (dhd) {
+		spin_lock_irqsave(&dhd->wl_lock, flags);
+		if (dhd->wl_count) {
+			dhd->wl_count--;
+#ifdef CONFIG_HAS_WAKELOCK
+			if (!dhd->wl_count)
+				wake_unlock(&dhd->wl_wifi);
+#endif
+			ret = dhd->wl_count;
+		}
+		spin_unlock_irqrestore(&dhd->wl_lock, flags);
+	}
+	/* printk("%s: %d\n", __FUNCTION__, ret); */
+	return ret;
+}
+
+int net_os_wake_unlock(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ret = 0;
+
+	if (dhd)
+		ret = dhd_os_wake_unlock(&dhd->pub);
+	return ret;
+}
+
+unsigned long dhd_os_spin_lock(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+	unsigned long flags = 0;
+
+	if (dhd)
+		spin_lock_irqsave(&dhd->dhd_lock, flags);
+
+	return flags;
+}
+
+void dhd_os_spin_unlock(dhd_pub_t *pub, unsigned long flags)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+
+	if (dhd)
+		spin_unlock_irqrestore(&dhd->dhd_lock, flags);
+}
diff --git a/drivers/net/wireless/bcm4329/dhd_linux_sched.c b/drivers/net/wireless/bcm4329/dhd_linux_sched.c
new file mode 100644
index 000000000000..480b416657ee
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/dhd_linux_sched.c
@@ -0,0 +1,38 @@
+/*
+ * Expose some of the kernel scheduler routines
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_linux_sched.c,v 1.1.34.1.6.1 2009/01/16 01:17:40 Exp $
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linuxver.h>
+
+int setScheduler(struct task_struct *p, int policy, struct sched_param *param)
+{
+	int rc = 0;
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+	rc = sched_setscheduler(p, policy, param);
+#endif /* LinuxVer */
+	return rc;
+}
diff --git a/drivers/net/wireless/bcm4329/dhd_proto.h b/drivers/net/wireless/bcm4329/dhd_proto.h
new file mode 100644
index 000000000000..7ef6929a5bf7
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/dhd_proto.h
@@ -0,0 +1,102 @@
+/*
+ * Header file describing the internal (inter-module) DHD interfaces.
+ *
+ * Provides type definitions and function prototypes used to link the
+ * DHD OS, bus, and protocol modules.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_proto.h,v 1.2.82.1.4.1.16.7 2010/05/10 12:54:59 Exp $
+ */
+
+#ifndef _dhd_proto_h_
+#define _dhd_proto_h_
+
+#include <dhdioctl.h>
+#include <wlioctl.h>
+
+#ifndef IOCTL_RESP_TIMEOUT
+#define IOCTL_RESP_TIMEOUT  3000 /* In milli second */
+#endif
+
+#ifndef IOCTL_CHIP_ACTIVE_TIMEOUT
+#define IOCTL_CHIP_ACTIVE_TIMEOUT  10 /* In milli second */
+#endif
+
+/*
+ * Exported from the dhd protocol module (dhd_cdc, dhd_rndis)
+ */
+
+/* Linkage, sets prot link and updates hdrlen in pub */
+extern int dhd_prot_attach(dhd_pub_t *dhdp);
+
+/* Unlink, frees allocated protocol memory (including dhd_prot) */
+extern void dhd_prot_detach(dhd_pub_t *dhdp);
+
+/* Initialize protocol: sync w/dongle state.
+ * Sets dongle media info (iswl, drv_version, mac address).
+ */
+extern int dhd_prot_init(dhd_pub_t *dhdp);
+
+/* Stop protocol: sync w/dongle state. */
+extern void dhd_prot_stop(dhd_pub_t *dhdp);
+
+extern bool dhd_proto_fcinfo(dhd_pub_t *dhd, void *pktbuf, uint8 *fcbits);
+
+/* Add any protocol-specific data header.
+ * Caller must reserve prot_hdrlen prepend space.
+ */
+extern void dhd_prot_hdrpush(dhd_pub_t *, int ifidx, void *txp);
+
+/* Remove any protocol-specific data header. */
+extern int dhd_prot_hdrpull(dhd_pub_t *, int *ifidx, void *rxp);
+
+/* Use protocol to issue ioctl to dongle */
+extern int dhd_prot_ioctl(dhd_pub_t *dhd, int ifidx, wl_ioctl_t * ioc, void * buf, int len);
+
+/* Check for and handle local prot-specific iovar commands */
+extern int dhd_prot_iovar_op(dhd_pub_t *dhdp, const char *name,
+                             void *params, int plen, void *arg, int len, bool set);
+
+/* Add prot dump output to a buffer */
+extern void dhd_prot_dump(dhd_pub_t *dhdp, struct bcmstrbuf *strbuf);
+
+/* Update local copy of dongle statistics */
+extern void dhd_prot_dstats(dhd_pub_t *dhdp);
+
+extern int dhd_ioctl(dhd_pub_t * dhd_pub, dhd_ioctl_t *ioc, void * buf, uint buflen);
+
+extern int dhd_preinit_ioctls(dhd_pub_t *dhd);
+
+/********************************
+ * For version-string expansion *
+ */
+#if defined(BDC)
+#define DHD_PROTOCOL "bdc"
+#elif defined(CDC)
+#define DHD_PROTOCOL "cdc"
+#elif defined(RNDIS)
+#define DHD_PROTOCOL "rndis"
+#else
+#define DHD_PROTOCOL "unknown"
+#endif /* proto */
+
+#endif /* _dhd_proto_h_ */
diff --git a/drivers/net/wireless/bcm4329/dhd_sdio.c b/drivers/net/wireless/bcm4329/dhd_sdio.c
new file mode 100644
index 000000000000..446eb4a458d2
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/dhd_sdio.c
@@ -0,0 +1,5841 @@
+/*
+ * DHD Bus Module for SDIO
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_sdio.c,v 1.157.2.27.2.33.2.129.4.1 2010/09/02 23:13:16 Exp $
+ */
+
+#include <typedefs.h>
+#include <osl.h>
+#include <bcmsdh.h>
+
+#ifdef BCMEMBEDIMAGE
+#include BCMEMBEDIMAGE
+#endif /* BCMEMBEDIMAGE */
+
+#include <bcmdefs.h>
+#include <bcmutils.h>
+#include <bcmendian.h>
+#include <bcmdevs.h>
+#include <siutils.h>
+#include <hndpmu.h>
+#include <hndsoc.h>
+#include <sbchipc.h>
+#include <sbhnddma.h>
+#include <sdio.h>
+#include <sbsdio.h>
+#include <sbsdpcmdev.h>
+#include <bcmsdpcm.h>
+
+#include <proto/ethernet.h>
+#include <proto/802.1d.h>
+#include <proto/802.11.h>
+
+#include <dngl_stats.h>
+#include <dhd.h>
+#include <dhd_bus.h>
+#include <dhd_proto.h>
+#include <dhd_dbg.h>
+#include <dhdioctl.h>
+#include <sdiovar.h>
+
+#ifdef CONFIG_HAS_WAKELOCK
+#include <linux/wakelock.h>
+#endif
+
+#ifdef DHD_DEBUG
+#include <hndrte_cons.h>
+#endif /* DHD_DEBUG */
+#ifdef DHD_DEBUG_TRAP
+#include <hndrte_armtrap.h>
+#endif /* DHD_DEBUG_TRAP */
+
+#define QLEN		2048	/* bulk rx and tx queue lengths */
+#define FCHI		(QLEN - 256)
+#define FCLOW		(FCHI -256)
+#define PRIOMASK	7
+
+#define TXRETRIES	2	/* # of retries for tx frames */
+
+#if defined(CONFIG_MACH_SANDGATE2G)
+#define DHD_RXBOUND	250	/* Default for max rx frames in one scheduling */
+#else
+#define DHD_RXBOUND	50	/* Default for max rx frames in one scheduling */
+#endif /* defined(CONFIG_MACH_SANDGATE2G) */
+
+#define DHD_TXBOUND	20	/* Default for max tx frames in one scheduling */
+
+#define DHD_TXMINMAX	1	/* Max tx frames if rx still pending */
+
+#define MEMBLOCK	2048		/* Block size used for downloading of dongle image */
+#define MAX_DATA_BUF	(32 * 1024)	/* Must be large enough to hold biggest possible glom */
+
+/* Packet alignment for most efficient SDIO (can change based on platform) */
+#ifndef DHD_SDALIGN
+#define DHD_SDALIGN	32
+#endif
+#if !ISPOWEROF2(DHD_SDALIGN)
+#error DHD_SDALIGN is not a power of 2!
+#endif
+
+#ifndef DHD_FIRSTREAD
+#define DHD_FIRSTREAD	32
+#endif
+#if !ISPOWEROF2(DHD_FIRSTREAD)
+#error DHD_FIRSTREAD is not a power of 2!
+#endif
+
+/* Total length of frame header for dongle protocol */
+#define SDPCM_HDRLEN	(SDPCM_FRAMETAG_LEN + SDPCM_SWHEADER_LEN)
+#ifdef SDTEST
+#define SDPCM_RESERVE	(SDPCM_HDRLEN + SDPCM_TEST_HDRLEN + DHD_SDALIGN)
+#else
+#define SDPCM_RESERVE	(SDPCM_HDRLEN + DHD_SDALIGN)
+#endif
+
+/* Space for header read, limit for data packets */
+#ifndef MAX_HDR_READ
+#define MAX_HDR_READ	32
+#endif
+#if !ISPOWEROF2(MAX_HDR_READ)
+#error MAX_HDR_READ is not a power of 2!
+#endif
+
+#define MAX_RX_DATASZ	2048
+
+/* Maximum milliseconds to wait for F2 to come up */
+#define DHD_WAIT_F2RDY	3000
+
+/* Bump up limit on waiting for HT to account for first startup;
+ * if the image is doing a CRC calculation before programming the PMU
+ * for HT availability, it could take a couple hundred ms more, so
+ * max out at a 1 second (1000000us).
+ */
+#if (PMU_MAX_TRANSITION_DLY < 1000000)
+#undef PMU_MAX_TRANSITION_DLY
+#define PMU_MAX_TRANSITION_DLY 1000000
+#endif
+
+/* Value for ChipClockCSR during initial setup */
+#define DHD_INIT_CLKCTL1	(SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_ALP_AVAIL_REQ)
+#define DHD_INIT_CLKCTL2	(SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_FORCE_ALP)
+
+/* Flags for SDH calls */
+#define F2SYNC	(SDIO_REQ_4BYTE | SDIO_REQ_FIXED)
+
+/* Packet free applicable unconditionally for sdio and sdspi.  Conditional if
+ * bufpool was present for gspi bus.
+ */
+#define PKTFREE2()		if ((bus->bus != SPI_BUS) || bus->usebufpool) \
+					PKTFREE(bus->dhd->osh, pkt, FALSE);
+DHD_SPINWAIT_SLEEP_INIT(sdioh_spinwait_sleep);
+extern int dhdcdc_set_ioctl(dhd_pub_t *dhd, int ifidx, uint cmd, void *buf, uint len);
+
+extern void bcmsdh_set_irq(int flag);
+
+#ifdef DHD_DEBUG
+/* Device console log buffer state */
+typedef struct dhd_console {
+	uint		count;			/* Poll interval msec counter */
+	uint		log_addr;		/* Log struct address (fixed) */
+	hndrte_log_t	log;			/* Log struct (host copy) */
+	uint		bufsize;		/* Size of log buffer */
+	uint8		*buf;			/* Log buffer (host copy) */
+	uint		last;			/* Last buffer read index */
+} dhd_console_t;
+#endif /* DHD_DEBUG */
+
+/* Private data for SDIO bus interaction */
+typedef struct dhd_bus {
+	dhd_pub_t	*dhd;
+
+	bcmsdh_info_t	*sdh;			/* Handle for BCMSDH calls */
+	si_t		*sih;			/* Handle for SI calls */
+	char		*vars;			/* Variables (from CIS and/or other) */
+	uint		varsz;			/* Size of variables buffer */
+	uint32		sbaddr;			/* Current SB window pointer (-1, invalid) */
+
+	sdpcmd_regs_t	*regs;			/* Registers for SDIO core */
+	uint		sdpcmrev;		/* SDIO core revision */
+	uint		armrev;			/* CPU core revision */
+	uint		ramrev;			/* SOCRAM core revision */
+	uint32		ramsize;		/* Size of RAM in SOCRAM (bytes) */
+	uint32		orig_ramsize;		/* Size of RAM in SOCRAM (bytes) */
+
+	uint32		bus;			/* gSPI or SDIO bus */
+	uint32		hostintmask;		/* Copy of Host Interrupt Mask */
+	uint32		intstatus;		/* Intstatus bits (events) pending */
+	bool		dpc_sched;		/* Indicates DPC schedule (intrpt rcvd) */
+	bool		fcstate;		/* State of dongle flow-control */
+
+	uint16		cl_devid;		/* cached devid for dhdsdio_probe_attach() */
+	char		*fw_path; /* module_param: path to firmware image */
+	char		*nv_path; /* module_param: path to nvram vars file */
+	const char      *nvram_params;		/* user specified nvram params. */
+
+	uint		blocksize;		/* Block size of SDIO transfers */
+	uint		roundup;		/* Max roundup limit */
+
+	struct pktq	txq;			/* Queue length used for flow-control */
+	uint8		flowcontrol;		/* per prio flow control bitmask */
+	uint8		tx_seq;			/* Transmit sequence number (next) */
+	uint8		tx_max;			/* Maximum transmit sequence allowed */
+
+	uint8		hdrbuf[MAX_HDR_READ + DHD_SDALIGN];
+	uint8		*rxhdr;			/* Header of current rx frame (in hdrbuf) */
+	uint16		nextlen;		/* Next Read Len from last header */
+	uint8		rx_seq;			/* Receive sequence number (expected) */
+	bool		rxskip;			/* Skip receive (awaiting NAK ACK) */
+
+	void		*glomd;			/* Packet containing glomming descriptor */
+	void		*glom;			/* Packet chain for glommed superframe */
+	uint		glomerr;		/* Glom packet read errors */
+
+	uint8		*rxbuf;			/* Buffer for receiving control packets */
+	uint		rxblen;			/* Allocated length of rxbuf */
+	uint8		*rxctl;			/* Aligned pointer into rxbuf */
+	uint8		*databuf;		/* Buffer for receiving big glom packet */
+	uint8		*dataptr;		/* Aligned pointer into databuf */
+	uint		rxlen;			/* Length of valid data in buffer */
+
+	uint8		sdpcm_ver;		/* Bus protocol reported by dongle */
+
+	bool		intr;			/* Use interrupts */
+	bool		poll;			/* Use polling */
+	bool		ipend;			/* Device interrupt is pending */
+	bool		intdis;			/* Interrupts disabled by isr */
+	uint 		intrcount;		/* Count of device interrupt callbacks */
+	uint		lastintrs;		/* Count as of last watchdog timer */
+	uint		spurious;		/* Count of spurious interrupts */
+	uint		pollrate;		/* Ticks between device polls */
+	uint		polltick;		/* Tick counter */
+	uint		pollcnt;		/* Count of active polls */
+
+#ifdef DHD_DEBUG
+	dhd_console_t	console;		/* Console output polling support */
+	uint		console_addr;		/* Console address from shared struct */
+#endif /* DHD_DEBUG */
+
+	uint		regfails;		/* Count of R_REG/W_REG failures */
+
+	uint		clkstate;		/* State of sd and backplane clock(s) */
+	bool		activity;		/* Activity flag for clock down */
+	int32		idletime;		/* Control for activity timeout */
+	int32		idlecount;		/* Activity timeout counter */
+	int32		idleclock;		/* How to set bus driver when idle */
+	int32		sd_divisor;		/* Speed control to bus driver */
+	int32		sd_mode;		/* Mode control to bus driver */
+	int32		sd_rxchain;		/* If bcmsdh api accepts PKT chains */
+	bool		use_rxchain;		/* If dhd should use PKT chains */
+	bool		sleeping;		/* Is SDIO bus sleeping? */
+	bool		rxflow_mode;	/* Rx flow control mode */
+	bool		rxflow;			/* Is rx flow control on */
+	uint		prev_rxlim_hit;		/* Is prev rx limit exceeded (per dpc schedule) */
+	bool		alp_only;		/* Don't use HT clock (ALP only) */
+	/* Field to decide if rx of control frames happen in rxbuf or lb-pool */
+	bool		usebufpool;
+
+#ifdef SDTEST
+	/* external loopback */
+	bool		ext_loop;
+	uint8		loopid;
+
+	/* pktgen configuration */
+	uint		pktgen_freq;		/* Ticks between bursts */
+	uint		pktgen_count;		/* Packets to send each burst */
+	uint		pktgen_print;		/* Bursts between count displays */
+	uint		pktgen_total;		/* Stop after this many */
+	uint		pktgen_minlen;		/* Minimum packet data len */
+	uint		pktgen_maxlen;		/* Maximum packet data len */
+	uint		pktgen_mode;		/* Configured mode: tx, rx, or echo */
+	uint		pktgen_stop;		/* Number of tx failures causing stop */
+
+	/* active pktgen fields */
+	uint		pktgen_tick;		/* Tick counter for bursts */
+	uint		pktgen_ptick;		/* Burst counter for printing */
+	uint		pktgen_sent;		/* Number of test packets generated */
+	uint		pktgen_rcvd;		/* Number of test packets received */
+	uint		pktgen_fail;		/* Number of failed send attempts */
+	uint16		pktgen_len;		/* Length of next packet to send */
+#endif /* SDTEST */
+
+	/* Some additional counters */
+	uint		tx_sderrs;		/* Count of tx attempts with sd errors */
+	uint		fcqueued;		/* Tx packets that got queued */
+	uint		rxrtx;			/* Count of rtx requests (NAK to dongle) */
+	uint		rx_toolong;		/* Receive frames too long to receive */
+	uint		rxc_errors;		/* SDIO errors when reading control frames */
+	uint		rx_hdrfail;		/* SDIO errors on header reads */
+	uint		rx_badhdr;		/* Bad received headers (roosync?) */
+	uint		rx_badseq;		/* Mismatched rx sequence number */
+	uint		fc_rcvd;		/* Number of flow-control events received */
+	uint		fc_xoff;		/* Number which turned on flow-control */
+	uint		fc_xon;			/* Number which turned off flow-control */
+	uint		rxglomfail;		/* Failed deglom attempts */
+	uint		rxglomframes;		/* Number of glom frames (superframes) */
+	uint		rxglompkts;		/* Number of packets from glom frames */
+	uint		f2rxhdrs;		/* Number of header reads */
+	uint		f2rxdata;		/* Number of frame data reads */
+	uint		f2txdata;		/* Number of f2 frame writes */
+	uint		f1regdata;		/* Number of f1 register accesses */
+
+	uint8		*ctrl_frame_buf;
+	uint32		ctrl_frame_len;
+	bool		ctrl_frame_stat;
+} dhd_bus_t;
+
+/* clkstate */
+#define CLK_NONE	0
+#define CLK_SDONLY	1
+#define CLK_PENDING	2	/* Not used yet */
+#define CLK_AVAIL	3
+
+#define DHD_NOPMU(dhd)	(FALSE)
+
+#ifdef DHD_DEBUG
+static int qcount[NUMPRIO];
+static int tx_packets[NUMPRIO];
+#endif /* DHD_DEBUG */
+
+/* Deferred transmit */
+const uint dhd_deferred_tx = 1;
+
+extern uint dhd_watchdog_ms;
+extern void dhd_os_wd_timer(void *bus, uint wdtick);
+
+/* Tx/Rx bounds */
+uint dhd_txbound;
+uint dhd_rxbound;
+uint dhd_txminmax;
+
+/* override the RAM size if possible */
+#define DONGLE_MIN_MEMSIZE (128 *1024)
+int dhd_dongle_memsize;
+
+static bool dhd_doflow;
+static bool dhd_alignctl;
+
+static bool sd1idle;
+
+static bool retrydata;
+#define RETRYCHAN(chan) (((chan) == SDPCM_EVENT_CHANNEL) || retrydata)
+
+static const uint watermark = 8;
+static const uint firstread = DHD_FIRSTREAD;
+
+#define HDATLEN (firstread - (SDPCM_HDRLEN))
+
+/* Retry count for register access failures */
+static const uint retry_limit = 2;
+
+/* Force even SD lengths (some host controllers mess up on odd bytes) */
+static bool forcealign;
+
+#define ALIGNMENT  4
+
+#if defined(OOB_INTR_ONLY) && defined(HW_OOB)
+extern void bcmsdh_enable_hw_oob_intr(void *sdh, bool enable);
+#endif
+
+#if defined(OOB_INTR_ONLY) && defined(SDIO_ISR_THREAD)
+#error OOB_INTR_ONLY is NOT working with SDIO_ISR_THREAD
+#endif /* defined(OOB_INTR_ONLY) && defined(SDIO_ISR_THREAD) */
+#define PKTALIGN(osh, p, len, align)					\
+	do {								\
+		uint datalign;						\
+		datalign = (uintptr)PKTDATA((osh), (p));		\
+		datalign = ROUNDUP(datalign, (align)) - datalign;	\
+		ASSERT(datalign < (align));				\
+		ASSERT(PKTLEN((osh), (p)) >= ((len) + datalign));	\
+		if (datalign)						\
+			PKTPULL((osh), (p), datalign);			\
+		PKTSETLEN((osh), (p), (len));				\
+	} while (0)
+
+/* Limit on rounding up frames */
+static const uint max_roundup = 512;
+
+/* Try doing readahead */
+static bool dhd_readahead;
+
+
+/* To check if there's window offered */
+#define DATAOK(bus) \
+	(((uint8)(bus->tx_max - bus->tx_seq) != 0) && \
+	(((uint8)(bus->tx_max - bus->tx_seq) & 0x80) == 0))
+
+/* Macros to get register read/write status */
+/* NOTE: these assume a local dhdsdio_bus_t *bus! */
+#define R_SDREG(regvar, regaddr, retryvar) \
+do { \
+	retryvar = 0; \
+	do { \
+		regvar = R_REG(bus->dhd->osh, regaddr); \
+	} while (bcmsdh_regfail(bus->sdh) && (++retryvar <= retry_limit)); \
+	if (retryvar) { \
+		bus->regfails += (retryvar-1); \
+		if (retryvar > retry_limit) { \
+			DHD_ERROR(("%s: FAILED" #regvar "READ, LINE %d\n", \
+			           __FUNCTION__, __LINE__)); \
+			regvar = 0; \
+		} \
+	} \
+} while (0)
+
+#define W_SDREG(regval, regaddr, retryvar) \
+do { \
+	retryvar = 0; \
+	do { \
+		W_REG(bus->dhd->osh, regaddr, regval); \
+	} while (bcmsdh_regfail(bus->sdh) && (++retryvar <= retry_limit)); \
+	if (retryvar) { \
+		bus->regfails += (retryvar-1); \
+		if (retryvar > retry_limit) \
+			DHD_ERROR(("%s: FAILED REGISTER WRITE, LINE %d\n", \
+			           __FUNCTION__, __LINE__)); \
+	} \
+} while (0)
+
+
+#define DHD_BUS			SDIO_BUS
+
+#define PKT_AVAILABLE()		(intstatus & I_HMB_FRAME_IND)
+
+#define HOSTINTMASK		(I_HMB_SW_MASK | I_CHIPACTIVE)
+
+#define GSPI_PR55150_BAILOUT
+
+
+#ifdef SDTEST
+static void dhdsdio_testrcv(dhd_bus_t *bus, void *pkt, uint seq);
+static void dhdsdio_sdtest_set(dhd_bus_t *bus, bool start);
+#endif
+
+#ifdef DHD_DEBUG_TRAP
+static int dhdsdio_checkdied(dhd_bus_t *bus, uint8 *data, uint size);
+#endif /* DHD_DEBUG_TRAP */
+static int dhdsdio_download_state(dhd_bus_t *bus, bool enter);
+
+static void dhdsdio_release(dhd_bus_t *bus, osl_t *osh);
+static void dhdsdio_release_malloc(dhd_bus_t *bus, osl_t *osh);
+static void dhdsdio_disconnect(void *ptr);
+static bool dhdsdio_chipmatch(uint16 chipid);
+static bool dhdsdio_probe_attach(dhd_bus_t *bus, osl_t *osh, void *sdh,
+                                 void * regsva, uint16  devid);
+static bool dhdsdio_probe_malloc(dhd_bus_t *bus, osl_t *osh, void *sdh);
+static bool dhdsdio_probe_init(dhd_bus_t *bus, osl_t *osh, void *sdh);
+static void dhdsdio_release_dongle(dhd_bus_t *bus, osl_t *osh, int reset_flag);
+
+static uint process_nvram_vars(char *varbuf, uint len);
+
+static void dhd_dongle_setmemsize(struct dhd_bus *bus, int mem_size);
+static int dhd_bcmsdh_recv_buf(dhd_bus_t *bus, uint32 addr, uint fn, uint flags,
+	uint8 *buf, uint nbytes,
+	void *pkt, bcmsdh_cmplt_fn_t complete, void *handle);
+static int dhd_bcmsdh_send_buf(dhd_bus_t *bus, uint32 addr, uint fn, uint flags,
+	uint8 *buf, uint nbytes,
+	void *pkt, bcmsdh_cmplt_fn_t complete, void *handle);
+
+static bool dhdsdio_download_firmware(struct dhd_bus *bus, osl_t *osh, void *sdh);
+static int _dhdsdio_download_firmware(struct dhd_bus *bus);
+
+static int dhdsdio_download_code_file(struct dhd_bus *bus, char *image_path);
+static int dhdsdio_download_nvram(struct dhd_bus *bus);
+#ifdef BCMEMBEDIMAGE
+static int dhdsdio_download_code_array(struct dhd_bus *bus);
+#endif
+
+
+static void
+dhd_dongle_setmemsize(struct dhd_bus *bus, int mem_size)
+{
+	int32 min_size =  DONGLE_MIN_MEMSIZE;
+	/* Restrict the memsize to user specified limit */
+	DHD_ERROR(("user: Restrict the dongle ram size to %d, min accepted %d\n",
+		dhd_dongle_memsize, min_size));
+	if ((dhd_dongle_memsize > min_size) &&
+		(dhd_dongle_memsize < (int32)bus->orig_ramsize))
+		bus->ramsize = dhd_dongle_memsize;
+}
+
+static int
+dhdsdio_set_siaddr_window(dhd_bus_t *bus, uint32 address)
+{
+	int err = 0;
+	bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRLOW,
+	                 (address >> 8) & SBSDIO_SBADDRLOW_MASK, &err);
+	if (!err)
+		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRMID,
+		                 (address >> 16) & SBSDIO_SBADDRMID_MASK, &err);
+	if (!err)
+		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRHIGH,
+		                 (address >> 24) & SBSDIO_SBADDRHIGH_MASK, &err);
+	return err;
+}
+
+
+/* Turn backplane clock on or off */
+static int
+dhdsdio_htclk(dhd_bus_t *bus, bool on, bool pendok)
+{
+	int err;
+	uint8 clkctl, clkreq, devctl;
+	bcmsdh_info_t *sdh;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+#if defined(OOB_INTR_ONLY)
+	pendok = FALSE;
+#endif
+	clkctl = 0;
+	sdh = bus->sdh;
+
+
+	if (on) {
+		/* Request HT Avail */
+		clkreq = bus->alp_only ? SBSDIO_ALP_AVAIL_REQ : SBSDIO_HT_AVAIL_REQ;
+
+		if ((bus->sih->chip == BCM4329_CHIP_ID) && (bus->sih->chiprev == 0))
+			clkreq |= SBSDIO_FORCE_ALP;
+
+
+
+
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, clkreq, &err);
+		if (err) {
+			DHD_ERROR(("%s: HT Avail request error: %d\n", __FUNCTION__, err));
+			return BCME_ERROR;
+		}
+
+		if (pendok &&
+		    ((bus->sih->buscoretype == PCMCIA_CORE_ID) && (bus->sih->buscorerev == 9))) {
+			uint32 dummy, retries;
+			R_SDREG(dummy, &bus->regs->clockctlstatus, retries);
+		}
+
+		/* Check current status */
+		clkctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, &err);
+		if (err) {
+			DHD_ERROR(("%s: HT Avail read error: %d\n", __FUNCTION__, err));
+			return BCME_ERROR;
+		}
+
+		/* Go to pending and await interrupt if appropriate */
+		if (!SBSDIO_CLKAV(clkctl, bus->alp_only) && pendok) {
+			/* Allow only clock-available interrupt */
+			devctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, &err);
+			if (err) {
+				DHD_ERROR(("%s: Devctl access error setting CA: %d\n",
+				           __FUNCTION__, err));
+				return BCME_ERROR;
+			}
+
+			devctl |= SBSDIO_DEVCTL_CA_INT_ONLY;
+			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, devctl, &err);
+			DHD_INFO(("CLKCTL: set PENDING\n"));
+			bus->clkstate = CLK_PENDING;
+			return BCME_OK;
+		} else if (bus->clkstate == CLK_PENDING) {
+			/* Cancel CA-only interrupt filter */
+			devctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, &err);
+			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
+			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, devctl, &err);
+		}
+
+		/* Otherwise, wait here (polling) for HT Avail */
+		if (!SBSDIO_CLKAV(clkctl, bus->alp_only)) {
+			SPINWAIT_SLEEP(sdioh_spinwait_sleep,
+				((clkctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+			                                    SBSDIO_FUNC1_CHIPCLKCSR, &err)),
+			          !SBSDIO_CLKAV(clkctl, bus->alp_only)), PMU_MAX_TRANSITION_DLY);
+		}
+		if (err) {
+			DHD_ERROR(("%s: HT Avail request error: %d\n", __FUNCTION__, err));
+			return BCME_ERROR;
+		}
+		if (!SBSDIO_CLKAV(clkctl, bus->alp_only)) {
+			DHD_ERROR(("%s: HT Avail timeout (%d): clkctl 0x%02x\n",
+			           __FUNCTION__, PMU_MAX_TRANSITION_DLY, clkctl));
+			return BCME_ERROR;
+		}
+
+
+		/* Mark clock available */
+		bus->clkstate = CLK_AVAIL;
+		DHD_INFO(("CLKCTL: turned ON\n"));
+
+#if defined(DHD_DEBUG)
+		if (bus->alp_only == TRUE) {
+#if !defined(BCMLXSDMMC)
+			if (!SBSDIO_ALPONLY(clkctl)) {
+				DHD_ERROR(("%s: HT Clock, when ALP Only\n", __FUNCTION__));
+			}
+#endif /* !defined(BCMLXSDMMC) */
+		} else {
+			if (SBSDIO_ALPONLY(clkctl)) {
+				DHD_ERROR(("%s: HT Clock should be on.\n", __FUNCTION__));
+			}
+		}
+#endif /* defined (DHD_DEBUG) */
+
+		bus->activity = TRUE;
+	} else {
+		clkreq = 0;
+
+		if (bus->clkstate == CLK_PENDING) {
+			/* Cancel CA-only interrupt filter */
+			devctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, &err);
+			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
+			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, devctl, &err);
+		}
+
+		bus->clkstate = CLK_SDONLY;
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, clkreq, &err);
+		DHD_INFO(("CLKCTL: turned OFF\n"));
+		if (err) {
+			DHD_ERROR(("%s: Failed access turning clock off: %d\n",
+			           __FUNCTION__, err));
+			return BCME_ERROR;
+		}
+	}
+	return BCME_OK;
+}
+
+/* Change idle/active SD state */
+static int
+dhdsdio_sdclk(dhd_bus_t *bus, bool on)
+{
+	int err;
+	int32 iovalue;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (on) {
+		if (bus->idleclock == DHD_IDLE_STOP) {
+			/* Turn on clock and restore mode */
+			iovalue = 1;
+			err = bcmsdh_iovar_op(bus->sdh, "sd_clock", NULL, 0,
+			                      &iovalue, sizeof(iovalue), TRUE);
+			if (err) {
+				DHD_ERROR(("%s: error enabling sd_clock: %d\n",
+				           __FUNCTION__, err));
+				return BCME_ERROR;
+			}
+
+			iovalue = bus->sd_mode;
+			err = bcmsdh_iovar_op(bus->sdh, "sd_mode", NULL, 0,
+			                      &iovalue, sizeof(iovalue), TRUE);
+			if (err) {
+				DHD_ERROR(("%s: error changing sd_mode: %d\n",
+				           __FUNCTION__, err));
+				return BCME_ERROR;
+			}
+		} else if (bus->idleclock != DHD_IDLE_ACTIVE) {
+			/* Restore clock speed */
+			iovalue = bus->sd_divisor;
+			err = bcmsdh_iovar_op(bus->sdh, "sd_divisor", NULL, 0,
+			                      &iovalue, sizeof(iovalue), TRUE);
+			if (err) {
+				DHD_ERROR(("%s: error restoring sd_divisor: %d\n",
+				           __FUNCTION__, err));
+				return BCME_ERROR;
+			}
+		}
+		bus->clkstate = CLK_SDONLY;
+	} else {
+		/* Stop or slow the SD clock itself */
+		if ((bus->sd_divisor == -1) || (bus->sd_mode == -1)) {
+			DHD_TRACE(("%s: can't idle clock, divisor %d mode %d\n",
+			           __FUNCTION__, bus->sd_divisor, bus->sd_mode));
+			return BCME_ERROR;
+		}
+		if (bus->idleclock == DHD_IDLE_STOP) {
+			if (sd1idle) {
+				/* Change to SD1 mode and turn off clock */
+				iovalue = 1;
+				err = bcmsdh_iovar_op(bus->sdh, "sd_mode", NULL, 0,
+				                      &iovalue, sizeof(iovalue), TRUE);
+				if (err) {
+					DHD_ERROR(("%s: error changing sd_clock: %d\n",
+					           __FUNCTION__, err));
+					return BCME_ERROR;
+				}
+			}
+
+			iovalue = 0;
+			err = bcmsdh_iovar_op(bus->sdh, "sd_clock", NULL, 0,
+			                      &iovalue, sizeof(iovalue), TRUE);
+			if (err) {
+				DHD_ERROR(("%s: error disabling sd_clock: %d\n",
+				           __FUNCTION__, err));
+				return BCME_ERROR;
+			}
+		} else if (bus->idleclock != DHD_IDLE_ACTIVE) {
+			/* Set divisor to idle value */
+			iovalue = bus->idleclock;
+			err = bcmsdh_iovar_op(bus->sdh, "sd_divisor", NULL, 0,
+			                      &iovalue, sizeof(iovalue), TRUE);
+			if (err) {
+				DHD_ERROR(("%s: error changing sd_divisor: %d\n",
+				           __FUNCTION__, err));
+				return BCME_ERROR;
+			}
+		}
+		bus->clkstate = CLK_NONE;
+	}
+
+	return BCME_OK;
+}
+
+/* Transition SD and backplane clock readiness */
+static int
+dhdsdio_clkctl(dhd_bus_t *bus, uint target, bool pendok)
+{
+	int ret = BCME_OK;
+#ifdef DHD_DEBUG
+	uint oldstate = bus->clkstate;
+#endif /* DHD_DEBUG */
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	/* Early exit if we're already there */
+	if (bus->clkstate == target) {
+		if (target == CLK_AVAIL) {
+			dhd_os_wd_timer(bus->dhd, dhd_watchdog_ms);
+			bus->activity = TRUE;
+		}
+		return ret;
+	}
+
+	switch (target) {
+	case CLK_AVAIL:
+		/* Make sure SD clock is available */
+		if (bus->clkstate == CLK_NONE)
+			dhdsdio_sdclk(bus, TRUE);
+		/* Now request HT Avail on the backplane */
+		ret = dhdsdio_htclk(bus, TRUE, pendok);
+		if (ret == BCME_OK) {
+			dhd_os_wd_timer(bus->dhd, dhd_watchdog_ms);
+			bus->activity = TRUE;
+		}
+		break;
+
+	case CLK_SDONLY:
+		/* Remove HT request, or bring up SD clock */
+		if (bus->clkstate == CLK_NONE)
+			ret = dhdsdio_sdclk(bus, TRUE);
+		else if (bus->clkstate == CLK_AVAIL)
+			ret = dhdsdio_htclk(bus, FALSE, FALSE);
+		else
+			DHD_ERROR(("dhdsdio_clkctl: request for %d -> %d\n",
+			           bus->clkstate, target));
+		if (ret == BCME_OK)
+			dhd_os_wd_timer(bus->dhd, dhd_watchdog_ms);
+		break;
+
+	case CLK_NONE:
+		/* Make sure to remove HT request */
+		if (bus->clkstate == CLK_AVAIL)
+			ret = dhdsdio_htclk(bus, FALSE, FALSE);
+		/* Now remove the SD clock */
+		ret = dhdsdio_sdclk(bus, FALSE);
+		dhd_os_wd_timer(bus->dhd, 0);
+		break;
+	}
+#ifdef DHD_DEBUG
+	DHD_INFO(("dhdsdio_clkctl: %d -> %d\n", oldstate, bus->clkstate));
+#endif /* DHD_DEBUG */
+
+	return ret;
+}
+
+int
+dhdsdio_bussleep(dhd_bus_t *bus, bool sleep)
+{
+	bcmsdh_info_t *sdh = bus->sdh;
+	sdpcmd_regs_t *regs = bus->regs;
+	uint retries = 0;
+
+	DHD_INFO(("dhdsdio_bussleep: request %s (currently %s)\n",
+	          (sleep ? "SLEEP" : "WAKE"),
+	          (bus->sleeping ? "SLEEP" : "WAKE")));
+
+	/* Done if we're already in the requested state */
+	if (sleep == bus->sleeping)
+		return BCME_OK;
+
+	/* Going to sleep: set the alarm and turn off the lights... */
+	if (sleep) {
+		/* Don't sleep if something is pending */
+		if (bus->dpc_sched || bus->rxskip || pktq_len(&bus->txq))
+			return BCME_BUSY;
+
+
+		/* Disable SDIO interrupts (no longer interested) */
+		bcmsdh_intr_disable(bus->sdh);
+
+		/* Make sure the controller has the bus up */
+		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+
+		/* Tell device to start using OOB wakeup */
+		W_SDREG(SMB_USE_OOB, &regs->tosbmailbox, retries);
+		if (retries > retry_limit)
+			DHD_ERROR(("CANNOT SIGNAL CHIP, WILL NOT WAKE UP!!\n"));
+
+		/* Turn off our contribution to the HT clock request */
+		dhdsdio_clkctl(bus, CLK_SDONLY, FALSE);
+
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
+		                 SBSDIO_FORCE_HW_CLKREQ_OFF, NULL);
+
+		/* Isolate the bus */
+		if (bus->sih->chip != BCM4329_CHIP_ID && bus->sih->chip != BCM4319_CHIP_ID) {
+				bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL,
+					SBSDIO_DEVCTL_PADS_ISO, NULL);
+		}
+
+		/* Change state */
+		bus->sleeping = TRUE;
+
+	} else {
+		/* Waking up: bus power up is ok, set local state */
+
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
+		                 0, NULL);
+
+		/* Force pad isolation off if possible (in case power never toggled) */
+		if ((bus->sih->buscoretype == PCMCIA_CORE_ID) && (bus->sih->buscorerev >= 10))
+			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, 0, NULL);
+
+
+		/* Make sure the controller has the bus up */
+		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+
+		/* Send misc interrupt to indicate OOB not needed */
+		W_SDREG(0, &regs->tosbmailboxdata, retries);
+		if (retries <= retry_limit)
+			W_SDREG(SMB_DEV_INT, &regs->tosbmailbox, retries);
+
+		if (retries > retry_limit)
+			DHD_ERROR(("CANNOT SIGNAL CHIP TO CLEAR OOB!!\n"));
+
+		/* Make sure we have SD bus access */
+		dhdsdio_clkctl(bus, CLK_SDONLY, FALSE);
+
+		/* Change state */
+		bus->sleeping = FALSE;
+
+		/* Enable interrupts again */
+		if (bus->intr && (bus->dhd->busstate == DHD_BUS_DATA)) {
+			bus->intdis = FALSE;
+			bcmsdh_intr_enable(bus->sdh);
+		}
+	}
+
+	return BCME_OK;
+}
+#if defined(OOB_INTR_ONLY)
+void
+dhd_enable_oob_intr(struct dhd_bus *bus, bool enable)
+{
+#if defined(HW_OOB)
+	bcmsdh_enable_hw_oob_intr(bus->sdh, enable);
+#else
+	sdpcmd_regs_t *regs = bus->regs;
+	uint retries = 0;
+
+	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+	if (enable == TRUE) {
+
+		/* Tell device to start using OOB wakeup */
+		W_SDREG(SMB_USE_OOB, &regs->tosbmailbox, retries);
+		if (retries > retry_limit)
+			DHD_ERROR(("CANNOT SIGNAL CHIP, WILL NOT WAKE UP!!\n"));
+
+	} else {
+		/* Send misc interrupt to indicate OOB not needed */
+		W_SDREG(0, &regs->tosbmailboxdata, retries);
+		if (retries <= retry_limit)
+			W_SDREG(SMB_DEV_INT, &regs->tosbmailbox, retries);
+	}
+
+	/* Turn off our contribution to the HT clock request */
+	dhdsdio_clkctl(bus, CLK_SDONLY, FALSE);
+#endif /* !defined(HW_OOB) */
+}
+#endif /* defined(OOB_INTR_ONLY) */
+
+#define BUS_WAKE(bus) \
+	do { \
+		if ((bus)->sleeping) \
+			dhdsdio_bussleep((bus), FALSE); \
+	} while (0);
+
+
+/* Writes a HW/SW header into the packet and sends it. */
+/* Assumes: (a) header space already there, (b) caller holds lock */
+static int
+dhdsdio_txpkt(dhd_bus_t *bus, void *pkt, uint chan, bool free_pkt)
+{
+	int ret;
+	osl_t *osh;
+	uint8 *frame;
+	uint16 len, pad = 0;
+	uint32 swheader;
+	uint retries = 0;
+	bcmsdh_info_t *sdh;
+	void *new;
+	int i;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	sdh = bus->sdh;
+	osh = bus->dhd->osh;
+
+	if (bus->dhd->dongle_reset) {
+		ret = BCME_NOTREADY;
+		goto done;
+	}
+
+	frame = (uint8*)PKTDATA(osh, pkt);
+
+	/* Add alignment padding, allocate new packet if needed */
+	if ((pad = ((uintptr)frame % DHD_SDALIGN))) {
+		if (PKTHEADROOM(osh, pkt) < pad) {
+			DHD_INFO(("%s: insufficient headroom %d for %d pad\n",
+			          __FUNCTION__, (int)PKTHEADROOM(osh, pkt), pad));
+			bus->dhd->tx_realloc++;
+			new = PKTGET(osh, (PKTLEN(osh, pkt) + DHD_SDALIGN), TRUE);
+			if (!new) {
+				DHD_ERROR(("%s: couldn't allocate new %d-byte packet\n",
+				           __FUNCTION__, PKTLEN(osh, pkt) + DHD_SDALIGN));
+				ret = BCME_NOMEM;
+				goto done;
+			}
+
+			PKTALIGN(osh, new, PKTLEN(osh, pkt), DHD_SDALIGN);
+			bcopy(PKTDATA(osh, pkt), PKTDATA(osh, new), PKTLEN(osh, pkt));
+			if (free_pkt)
+				PKTFREE(osh, pkt, TRUE);
+			/* free the pkt if canned one is not used */
+			free_pkt = TRUE;
+			pkt = new;
+			frame = (uint8*)PKTDATA(osh, pkt);
+			ASSERT(((uintptr)frame % DHD_SDALIGN) == 0);
+			pad = 0;
+		} else {
+			PKTPUSH(osh, pkt, pad);
+			frame = (uint8*)PKTDATA(osh, pkt);
+
+			ASSERT((pad + SDPCM_HDRLEN) <= (int) PKTLEN(osh, pkt));
+			bzero(frame, pad + SDPCM_HDRLEN);
+		}
+	}
+	ASSERT(pad < DHD_SDALIGN);
+
+	/* Hardware tag: 2 byte len followed by 2 byte ~len check (all LE) */
+	len = (uint16)PKTLEN(osh, pkt);
+	*(uint16*)frame = htol16(len);
+	*(((uint16*)frame) + 1) = htol16(~len);
+
+	/* Software tag: channel, sequence number, data offset */
+	swheader = ((chan << SDPCM_CHANNEL_SHIFT) & SDPCM_CHANNEL_MASK) | bus->tx_seq |
+	        (((pad + SDPCM_HDRLEN) << SDPCM_DOFFSET_SHIFT) & SDPCM_DOFFSET_MASK);
+	htol32_ua_store(swheader, frame + SDPCM_FRAMETAG_LEN);
+	htol32_ua_store(0, frame + SDPCM_FRAMETAG_LEN + sizeof(swheader));
+
+#ifdef DHD_DEBUG
+	tx_packets[PKTPRIO(pkt)]++;
+	if (DHD_BYTES_ON() &&
+	    (((DHD_CTL_ON() && (chan == SDPCM_CONTROL_CHANNEL)) ||
+	      (DHD_DATA_ON() && (chan != SDPCM_CONTROL_CHANNEL))))) {
+		prhex("Tx Frame", frame, len);
+	} else if (DHD_HDRS_ON()) {
+		prhex("TxHdr", frame, MIN(len, 16));
+	}
+#endif
+
+	/* Raise len to next SDIO block to eliminate tail command */
+	if (bus->roundup && bus->blocksize && (len > bus->blocksize)) {
+		uint16 pad = bus->blocksize - (len % bus->blocksize);
+		if ((pad <= bus->roundup) && (pad < bus->blocksize))
+#ifdef NOTUSED
+			if (pad <= PKTTAILROOM(osh, pkt))
+#endif /* NOTUSED */
+				len += pad;
+	} else if (len % DHD_SDALIGN) {
+		len += DHD_SDALIGN - (len % DHD_SDALIGN);
+	}
+
+	/* Some controllers have trouble with odd bytes -- round to even */
+	if (forcealign && (len & (ALIGNMENT - 1))) {
+#ifdef NOTUSED
+		if (PKTTAILROOM(osh, pkt))
+#endif
+			len = ROUNDUP(len, ALIGNMENT);
+#ifdef NOTUSED
+		else
+			DHD_ERROR(("%s: sending unrounded %d-byte packet\n", __FUNCTION__, len));
+#endif
+	}
+
+	do {
+		ret = dhd_bcmsdh_send_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
+		                      frame, len, pkt, NULL, NULL);
+		bus->f2txdata++;
+		ASSERT(ret != BCME_PENDING);
+
+		if (ret < 0) {
+			/* On failure, abort the command and terminate the frame */
+			DHD_INFO(("%s: sdio error %d, abort command and terminate frame.\n",
+			          __FUNCTION__, ret));
+			bus->tx_sderrs++;
+
+			bcmsdh_abort(sdh, SDIO_FUNC_2);
+			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_FRAMECTRL,
+			                 SFC_WF_TERM, NULL);
+			bus->f1regdata++;
+
+			for (i = 0; i < 3; i++) {
+				uint8 hi, lo;
+				hi = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+				                     SBSDIO_FUNC1_WFRAMEBCHI, NULL);
+				lo = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+				                     SBSDIO_FUNC1_WFRAMEBCLO, NULL);
+				bus->f1regdata += 2;
+				if ((hi == 0) && (lo == 0))
+					break;
+			}
+
+		}
+		if (ret == 0) {
+			bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQUENCE_WRAP;
+		}
+	} while ((ret < 0) && retrydata && retries++ < TXRETRIES);
+
+done:
+	/* restore pkt buffer pointer before calling tx complete routine */
+	PKTPULL(osh, pkt, SDPCM_HDRLEN + pad);
+	dhd_os_sdunlock(bus->dhd);
+	dhd_txcomplete(bus->dhd, pkt, ret != 0);
+	dhd_os_sdlock(bus->dhd);
+
+	if (free_pkt)
+		PKTFREE(osh, pkt, TRUE);
+
+	return ret;
+}
+
+int
+dhd_bus_txdata(struct dhd_bus *bus, void *pkt)
+{
+	int ret = BCME_ERROR;
+	osl_t *osh;
+	uint datalen, prec;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	osh = bus->dhd->osh;
+	datalen = PKTLEN(osh, pkt);
+
+#ifdef SDTEST
+	/* Push the test header if doing loopback */
+	if (bus->ext_loop) {
+		uint8* data;
+		PKTPUSH(osh, pkt, SDPCM_TEST_HDRLEN);
+		data = PKTDATA(osh, pkt);
+		*data++ = SDPCM_TEST_ECHOREQ;
+		*data++ = (uint8)bus->loopid++;
+		*data++ = (datalen >> 0);
+		*data++ = (datalen >> 8);
+		datalen += SDPCM_TEST_HDRLEN;
+	}
+#endif /* SDTEST */
+
+	/* Add space for the header */
+	PKTPUSH(osh, pkt, SDPCM_HDRLEN);
+	ASSERT(ISALIGNED((uintptr)PKTDATA(osh, pkt), 2));
+
+	prec = PRIO2PREC((PKTPRIO(pkt) & PRIOMASK));
+
+
+	/* Check for existing queue, current flow-control, pending event, or pending clock */
+	if (dhd_deferred_tx || bus->fcstate || pktq_len(&bus->txq) || bus->dpc_sched ||
+	    (!DATAOK(bus)) || (bus->flowcontrol & NBITVAL(prec)) ||
+	    (bus->clkstate != CLK_AVAIL)) {
+		DHD_TRACE(("%s: deferring pktq len %d\n", __FUNCTION__,
+			pktq_len(&bus->txq)));
+		bus->fcqueued++;
+
+		/* Priority based enq */
+		dhd_os_sdlock_txq(bus->dhd);
+		if (dhd_prec_enq(bus->dhd, &bus->txq, pkt, prec) == FALSE) {
+			PKTPULL(osh, pkt, SDPCM_HDRLEN);
+			dhd_txcomplete(bus->dhd, pkt, FALSE);
+			PKTFREE(osh, pkt, TRUE);
+			DHD_ERROR(("%s: out of bus->txq !!!\n", __FUNCTION__));
+			ret = BCME_NORESOURCE;
+		} else {
+			ret = BCME_OK;
+		}
+		dhd_os_sdunlock_txq(bus->dhd);
+
+		if ((pktq_len(&bus->txq) >= FCHI) && dhd_doflow)
+			dhd_txflowcontrol(bus->dhd, 0, ON);
+
+#ifdef DHD_DEBUG
+		if (pktq_plen(&bus->txq, prec) > qcount[prec])
+			qcount[prec] = pktq_plen(&bus->txq, prec);
+#endif
+		/* Schedule DPC if needed to send queued packet(s) */
+		if (dhd_deferred_tx && !bus->dpc_sched) {
+			bus->dpc_sched = TRUE;
+			dhd_sched_dpc(bus->dhd);
+		}
+	} else {
+		/* Lock: we're about to use shared data/code (and SDIO) */
+		dhd_os_sdlock(bus->dhd);
+
+		/* Otherwise, send it now */
+		BUS_WAKE(bus);
+		/* Make sure back plane ht clk is on, no pending allowed */
+		dhdsdio_clkctl(bus, CLK_AVAIL, TRUE);
+
+#ifndef SDTEST
+		DHD_TRACE(("%s: calling txpkt\n", __FUNCTION__));
+		ret = dhdsdio_txpkt(bus, pkt, SDPCM_DATA_CHANNEL, TRUE);
+#else
+		ret = dhdsdio_txpkt(bus, pkt,
+		        (bus->ext_loop ? SDPCM_TEST_CHANNEL : SDPCM_DATA_CHANNEL), TRUE);
+#endif
+		if (ret)
+			bus->dhd->tx_errors++;
+		else
+			bus->dhd->dstats.tx_bytes += datalen;
+
+		if ((bus->idletime == DHD_IDLE_IMMEDIATE) && !bus->dpc_sched) {
+			bus->activity = FALSE;
+			dhdsdio_clkctl(bus, CLK_NONE, TRUE);
+		}
+
+		dhd_os_sdunlock(bus->dhd);
+	}
+
+
+	return ret;
+}
+
+static uint
+dhdsdio_sendfromq(dhd_bus_t *bus, uint maxframes)
+{
+	void *pkt;
+	uint32 intstatus = 0;
+	uint retries = 0;
+	int ret = 0, prec_out;
+	uint cnt = 0;
+	uint datalen;
+	uint8 tx_prec_map;
+
+	dhd_pub_t *dhd = bus->dhd;
+	sdpcmd_regs_t *regs = bus->regs;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	tx_prec_map = ~bus->flowcontrol;
+
+	/* Send frames until the limit or some other event */
+	for (cnt = 0; (cnt < maxframes) && DATAOK(bus); cnt++) {
+		dhd_os_sdlock_txq(bus->dhd);
+		if ((pkt = pktq_mdeq(&bus->txq, tx_prec_map, &prec_out)) == NULL) {
+			dhd_os_sdunlock_txq(bus->dhd);
+			break;
+		}
+		dhd_os_sdunlock_txq(bus->dhd);
+		datalen = PKTLEN(bus->dhd->osh, pkt) - SDPCM_HDRLEN;
+
+#ifndef SDTEST
+		ret = dhdsdio_txpkt(bus, pkt, SDPCM_DATA_CHANNEL, TRUE);
+#else
+		ret = dhdsdio_txpkt(bus, pkt,
+		        (bus->ext_loop ? SDPCM_TEST_CHANNEL : SDPCM_DATA_CHANNEL), TRUE);
+#endif
+		if (ret)
+			bus->dhd->tx_errors++;
+		else
+			bus->dhd->dstats.tx_bytes += datalen;
+
+		/* In poll mode, need to check for other events */
+		if (!bus->intr && cnt)
+		{
+			/* Check device status, signal pending interrupt */
+			R_SDREG(intstatus, &regs->intstatus, retries);
+			bus->f2txdata++;
+			if (bcmsdh_regfail(bus->sdh))
+				break;
+			if (intstatus & bus->hostintmask)
+				bus->ipend = TRUE;
+		}
+	}
+
+	/* Deflow-control stack if needed */
+	if (dhd_doflow && dhd->up && (dhd->busstate == DHD_BUS_DATA) &&
+	    dhd->txoff && (pktq_len(&bus->txq) < FCLOW))
+		dhd_txflowcontrol(dhd, 0, OFF);
+
+	return cnt;
+}
+
+int
+dhd_bus_txctl(struct dhd_bus *bus, uchar *msg, uint msglen)
+{
+	uint8 *frame;
+	uint16 len;
+	uint32 swheader;
+	uint retries = 0;
+	bcmsdh_info_t *sdh = bus->sdh;
+	uint8 doff = 0;
+	int ret = -1;
+	int i;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (bus->dhd->dongle_reset)
+		return -EIO;
+
+	/* Back the pointer to make a room for bus header */
+	frame = msg - SDPCM_HDRLEN;
+	len = (msglen += SDPCM_HDRLEN);
+
+	/* Add alignment padding (optional for ctl frames) */
+	if (dhd_alignctl) {
+		if ((doff = ((uintptr)frame % DHD_SDALIGN))) {
+			frame -= doff;
+			len += doff;
+			msglen += doff;
+			bzero(frame, doff + SDPCM_HDRLEN);
+		}
+		ASSERT(doff < DHD_SDALIGN);
+	}
+	doff += SDPCM_HDRLEN;
+
+	/* Round send length to next SDIO block */
+	if (bus->roundup && bus->blocksize && (len > bus->blocksize)) {
+		uint16 pad = bus->blocksize - (len % bus->blocksize);
+		if ((pad <= bus->roundup) && (pad < bus->blocksize))
+			len += pad;
+	} else if (len % DHD_SDALIGN) {
+		len += DHD_SDALIGN - (len % DHD_SDALIGN);
+	}
+
+	/* Satisfy length-alignment requirements */
+	if (forcealign && (len & (ALIGNMENT - 1)))
+		len = ROUNDUP(len, ALIGNMENT);
+
+	ASSERT(ISALIGNED((uintptr)frame, 2));
+
+
+	/* Need to lock here to protect txseq and SDIO tx calls */
+	dhd_os_sdlock(bus->dhd);
+
+	BUS_WAKE(bus);
+
+	/* Make sure backplane clock is on */
+	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+
+	/* Hardware tag: 2 byte len followed by 2 byte ~len check (all LE) */
+	*(uint16*)frame = htol16((uint16)msglen);
+	*(((uint16*)frame) + 1) = htol16(~msglen);
+
+	/* Software tag: channel, sequence number, data offset */
+	swheader = ((SDPCM_CONTROL_CHANNEL << SDPCM_CHANNEL_SHIFT) & SDPCM_CHANNEL_MASK)
+	        | bus->tx_seq | ((doff << SDPCM_DOFFSET_SHIFT) & SDPCM_DOFFSET_MASK);
+	htol32_ua_store(swheader, frame + SDPCM_FRAMETAG_LEN);
+	htol32_ua_store(0, frame + SDPCM_FRAMETAG_LEN + sizeof(swheader));
+
+	if (!DATAOK(bus)) {
+		DHD_INFO(("%s: No bus credit bus->tx_max %d, bus->tx_seq %d\n",
+			__FUNCTION__, bus->tx_max, bus->tx_seq));
+		bus->ctrl_frame_stat = TRUE;
+		/* Send from dpc */
+		bus->ctrl_frame_buf = frame;
+		bus->ctrl_frame_len = len;
+
+		dhd_wait_for_event(bus->dhd, &bus->ctrl_frame_stat);
+
+		if (bus->ctrl_frame_stat == FALSE) {
+			DHD_INFO(("%s: ctrl_frame_stat == FALSE\n", __FUNCTION__));
+			ret = 0;
+		} else {
+			if (!bus->dhd->hang_was_sent)
+				DHD_ERROR(("%s: ctrl_frame_stat == TRUE\n", __FUNCTION__));
+			ret = -1;
+		}
+	}
+
+	if (ret == -1) {
+#ifdef DHD_DEBUG
+		if (DHD_BYTES_ON() && DHD_CTL_ON()) {
+			prhex("Tx Frame", frame, len);
+		} else if (DHD_HDRS_ON()) {
+			prhex("TxHdr", frame, MIN(len, 16));
+		}
+#endif
+
+		do {
+			bus->ctrl_frame_stat = FALSE;
+			ret = dhd_bcmsdh_send_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
+			                          frame, len, NULL, NULL, NULL);
+			ASSERT(ret != BCME_PENDING);
+
+			if (ret < 0) {
+				/* On failure, abort the command and terminate the frame */
+				DHD_INFO(("%s: sdio error %d, abort command and terminate frame.\n",
+				          __FUNCTION__, ret));
+				bus->tx_sderrs++;
+
+				bcmsdh_abort(sdh, SDIO_FUNC_2);
+
+				bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_FRAMECTRL,
+				                 SFC_WF_TERM, NULL);
+				bus->f1regdata++;
+
+				for (i = 0; i < 3; i++) {
+					uint8 hi, lo;
+					hi = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+					                     SBSDIO_FUNC1_WFRAMEBCHI, NULL);
+					lo = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+					                     SBSDIO_FUNC1_WFRAMEBCLO, NULL);
+					bus->f1regdata += 2;
+					if ((hi == 0) && (lo == 0))
+						break;
+				}
+
+			}
+			if (ret == 0) {
+				bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQUENCE_WRAP;
+			}
+		} while ((ret < 0) && retries++ < TXRETRIES);
+	}
+
+	if ((bus->idletime == DHD_IDLE_IMMEDIATE) && !bus->dpc_sched) {
+		bus->activity = FALSE;
+		dhdsdio_clkctl(bus, CLK_NONE, TRUE);
+	}
+
+	dhd_os_sdunlock(bus->dhd);
+
+	if (ret)
+		bus->dhd->tx_ctlerrs++;
+	else
+		bus->dhd->tx_ctlpkts++;
+
+	return ret ? -EIO : 0;
+}
+
+int
+dhd_bus_rxctl(struct dhd_bus *bus, uchar *msg, uint msglen)
+{
+	int timeleft;
+	uint rxlen = 0;
+	bool pending;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (bus->dhd->dongle_reset)
+		return -EIO;
+
+	/* Wait until control frame is available */
+	timeleft = dhd_os_ioctl_resp_wait(bus->dhd, &bus->rxlen, &pending);
+
+	dhd_os_sdlock(bus->dhd);
+	rxlen = bus->rxlen;
+	bcopy(bus->rxctl, msg, MIN(msglen, rxlen));
+	bus->rxlen = 0;
+	dhd_os_sdunlock(bus->dhd);
+
+	if (rxlen) {
+		DHD_CTL(("%s: resumed on rxctl frame, got %d expected %d\n",
+		         __FUNCTION__, rxlen, msglen));
+	} else if (timeleft == 0) {
+		DHD_ERROR(("%s: resumed on timeout\n", __FUNCTION__));
+#ifdef DHD_DEBUG_TRAP
+		dhd_os_sdlock(bus->dhd);
+		dhdsdio_checkdied(bus, NULL, 0);
+		dhd_os_sdunlock(bus->dhd);
+#endif /* DHD_DEBUG_TRAP */
+	} else if (pending == TRUE) {
+		DHD_CTL(("%s: cancelled\n", __FUNCTION__));
+		return -ERESTARTSYS;
+	} else {
+		DHD_CTL(("%s: resumed for unknown reason?\n", __FUNCTION__));
+#ifdef DHD_DEBUG_TRAP
+		dhd_os_sdlock(bus->dhd);
+		dhdsdio_checkdied(bus, NULL, 0);
+		dhd_os_sdunlock(bus->dhd);
+#endif /* DHD_DEBUG_TRAP */
+	}
+
+	if (rxlen)
+		bus->dhd->rx_ctlpkts++;
+	else
+		bus->dhd->rx_ctlerrs++;
+
+	return rxlen ? (int)rxlen : -ETIMEDOUT;
+}
+
+/* IOVar table */
+enum {
+	IOV_INTR = 1,
+	IOV_POLLRATE,
+	IOV_SDREG,
+	IOV_SBREG,
+	IOV_SDCIS,
+	IOV_MEMBYTES,
+	IOV_MEMSIZE,
+#ifdef DHD_DEBUG_TRAP
+	IOV_CHECKDIED,
+#endif
+	IOV_DOWNLOAD,
+	IOV_FORCEEVEN,
+	IOV_SDIOD_DRIVE,
+	IOV_READAHEAD,
+	IOV_SDRXCHAIN,
+	IOV_ALIGNCTL,
+	IOV_SDALIGN,
+	IOV_DEVRESET,
+	IOV_CPU,
+#ifdef SDTEST
+	IOV_PKTGEN,
+	IOV_EXTLOOP,
+#endif /* SDTEST */
+	IOV_SPROM,
+	IOV_TXBOUND,
+	IOV_RXBOUND,
+	IOV_TXMINMAX,
+	IOV_IDLETIME,
+	IOV_IDLECLOCK,
+	IOV_SD1IDLE,
+	IOV_SLEEP,
+	IOV_VARS
+};
+
+const bcm_iovar_t dhdsdio_iovars[] = {
+	{"intr",	IOV_INTR,	0,	IOVT_BOOL,	0 },
+	{"sleep",	IOV_SLEEP,	0,	IOVT_BOOL,	0 },
+	{"pollrate",	IOV_POLLRATE,	0,	IOVT_UINT32,	0 },
+	{"idletime",	IOV_IDLETIME,	0,	IOVT_INT32,	0 },
+	{"idleclock",	IOV_IDLECLOCK,	0,	IOVT_INT32,	0 },
+	{"sd1idle",	IOV_SD1IDLE,	0,	IOVT_BOOL,	0 },
+	{"membytes",	IOV_MEMBYTES,	0,	IOVT_BUFFER,	2 * sizeof(int) },
+	{"memsize",	IOV_MEMSIZE,	0,	IOVT_UINT32,	0 },
+	{"download",	IOV_DOWNLOAD,	0,	IOVT_BOOL,	0 },
+	{"vars",	IOV_VARS,	0,	IOVT_BUFFER,	0 },
+	{"sdiod_drive",	IOV_SDIOD_DRIVE, 0,	IOVT_UINT32,	0 },
+	{"readahead",	IOV_READAHEAD,	0,	IOVT_BOOL,	0 },
+	{"sdrxchain",	IOV_SDRXCHAIN,	0,	IOVT_BOOL,	0 },
+	{"alignctl",	IOV_ALIGNCTL,	0,	IOVT_BOOL,	0 },
+	{"sdalign",	IOV_SDALIGN,	0,	IOVT_BOOL,	0 },
+	{"devreset",	IOV_DEVRESET,	0,	IOVT_BOOL,	0 },
+#ifdef DHD_DEBUG
+	{"sdreg",	IOV_SDREG,	0,	IOVT_BUFFER,	sizeof(sdreg_t) },
+	{"sbreg",	IOV_SBREG,	0,	IOVT_BUFFER,	sizeof(sdreg_t) },
+	{"sd_cis",	IOV_SDCIS,	0,	IOVT_BUFFER,	DHD_IOCTL_MAXLEN },
+	{"forcealign",	IOV_FORCEEVEN,	0,	IOVT_BOOL,	0 },
+	{"txbound",	IOV_TXBOUND,	0,	IOVT_UINT32,	0 },
+	{"rxbound",	IOV_RXBOUND,	0,	IOVT_UINT32,	0 },
+	{"txminmax", IOV_TXMINMAX,	0,	IOVT_UINT32,	0 },
+	{"cpu",		IOV_CPU,	0,	IOVT_BOOL,	0 },
+#endif /* DHD_DEBUG */
+#ifdef DHD_DEBUG_TRAP
+	{"checkdied",	IOV_CHECKDIED,	0,	IOVT_BUFFER,	0 },
+#endif /* DHD_DEBUG_TRAP  */
+#ifdef SDTEST
+	{"extloop",	IOV_EXTLOOP,	0,	IOVT_BOOL,	0 },
+	{"pktgen",	IOV_PKTGEN,	0,	IOVT_BUFFER,	sizeof(dhd_pktgen_t) },
+#endif /* SDTEST */
+
+	{NULL, 0, 0, 0, 0 }
+};
+
+static void
+dhd_dump_pct(struct bcmstrbuf *strbuf, char *desc, uint num, uint div)
+{
+	uint q1, q2;
+
+	if (!div) {
+		bcm_bprintf(strbuf, "%s N/A", desc);
+	} else {
+		q1 = num / div;
+		q2 = (100 * (num - (q1 * div))) / div;
+		bcm_bprintf(strbuf, "%s %d.%02d", desc, q1, q2);
+	}
+}
+
+void
+dhd_bus_dump(dhd_pub_t *dhdp, struct bcmstrbuf *strbuf)
+{
+	dhd_bus_t *bus = dhdp->bus;
+
+	bcm_bprintf(strbuf, "Bus SDIO structure:\n");
+	bcm_bprintf(strbuf, "hostintmask 0x%08x intstatus 0x%08x sdpcm_ver %d\n",
+	            bus->hostintmask, bus->intstatus, bus->sdpcm_ver);
+	bcm_bprintf(strbuf, "fcstate %d qlen %d tx_seq %d, max %d, rxskip %d rxlen %d rx_seq %d\n",
+	            bus->fcstate, pktq_len(&bus->txq), bus->tx_seq, bus->tx_max, bus->rxskip,
+	            bus->rxlen, bus->rx_seq);
+	bcm_bprintf(strbuf, "intr %d intrcount %d lastintrs %d spurious %d\n",
+	            bus->intr, bus->intrcount, bus->lastintrs, bus->spurious);
+	bcm_bprintf(strbuf, "pollrate %d pollcnt %d regfails %d\n",
+	            bus->pollrate, bus->pollcnt, bus->regfails);
+
+	bcm_bprintf(strbuf, "\nAdditional counters:\n");
+	bcm_bprintf(strbuf, "tx_sderrs %d fcqueued %d rxrtx %d rx_toolong %d rxc_errors %d\n",
+	            bus->tx_sderrs, bus->fcqueued, bus->rxrtx, bus->rx_toolong,
+	            bus->rxc_errors);
+	bcm_bprintf(strbuf, "rx_hdrfail %d badhdr %d badseq %d\n",
+	            bus->rx_hdrfail, bus->rx_badhdr, bus->rx_badseq);
+	bcm_bprintf(strbuf, "fc_rcvd %d, fc_xoff %d, fc_xon %d\n",
+	            bus->fc_rcvd, bus->fc_xoff, bus->fc_xon);
+	bcm_bprintf(strbuf, "rxglomfail %d, rxglomframes %d, rxglompkts %d\n",
+	            bus->rxglomfail, bus->rxglomframes, bus->rxglompkts);
+	bcm_bprintf(strbuf, "f2rx (hdrs/data) %d (%d/%d), f2tx %d f1regs %d\n",
+	            (bus->f2rxhdrs + bus->f2rxdata), bus->f2rxhdrs, bus->f2rxdata,
+	            bus->f2txdata, bus->f1regdata);
+	{
+		dhd_dump_pct(strbuf, "\nRx: pkts/f2rd", bus->dhd->rx_packets,
+		             (bus->f2rxhdrs + bus->f2rxdata));
+		dhd_dump_pct(strbuf, ", pkts/f1sd", bus->dhd->rx_packets, bus->f1regdata);
+		dhd_dump_pct(strbuf, ", pkts/sd", bus->dhd->rx_packets,
+		             (bus->f2rxhdrs + bus->f2rxdata + bus->f1regdata));
+		dhd_dump_pct(strbuf, ", pkts/int", bus->dhd->rx_packets, bus->intrcount);
+		bcm_bprintf(strbuf, "\n");
+
+		dhd_dump_pct(strbuf, "Rx: glom pct", (100 * bus->rxglompkts),
+		             bus->dhd->rx_packets);
+		dhd_dump_pct(strbuf, ", pkts/glom", bus->rxglompkts, bus->rxglomframes);
+		bcm_bprintf(strbuf, "\n");
+
+		dhd_dump_pct(strbuf, "Tx: pkts/f2wr", bus->dhd->tx_packets, bus->f2txdata);
+		dhd_dump_pct(strbuf, ", pkts/f1sd", bus->dhd->tx_packets, bus->f1regdata);
+		dhd_dump_pct(strbuf, ", pkts/sd", bus->dhd->tx_packets,
+		             (bus->f2txdata + bus->f1regdata));
+		dhd_dump_pct(strbuf, ", pkts/int", bus->dhd->tx_packets, bus->intrcount);
+		bcm_bprintf(strbuf, "\n");
+
+		dhd_dump_pct(strbuf, "Total: pkts/f2rw",
+		             (bus->dhd->tx_packets + bus->dhd->rx_packets),
+		             (bus->f2txdata + bus->f2rxhdrs + bus->f2rxdata));
+		dhd_dump_pct(strbuf, ", pkts/f1sd",
+		             (bus->dhd->tx_packets + bus->dhd->rx_packets), bus->f1regdata);
+		dhd_dump_pct(strbuf, ", pkts/sd",
+		             (bus->dhd->tx_packets + bus->dhd->rx_packets),
+		             (bus->f2txdata + bus->f2rxhdrs + bus->f2rxdata + bus->f1regdata));
+		dhd_dump_pct(strbuf, ", pkts/int",
+		             (bus->dhd->tx_packets + bus->dhd->rx_packets), bus->intrcount);
+		bcm_bprintf(strbuf, "\n\n");
+	}
+
+#ifdef SDTEST
+	if (bus->pktgen_count) {
+		bcm_bprintf(strbuf, "pktgen config and count:\n");
+		bcm_bprintf(strbuf, "freq %d count %d print %d total %d min %d len %d\n",
+		            bus->pktgen_freq, bus->pktgen_count, bus->pktgen_print,
+		            bus->pktgen_total, bus->pktgen_minlen, bus->pktgen_maxlen);
+		bcm_bprintf(strbuf, "send attempts %d rcvd %d fail %d\n",
+		            bus->pktgen_sent, bus->pktgen_rcvd, bus->pktgen_fail);
+	}
+#endif /* SDTEST */
+#ifdef DHD_DEBUG
+	bcm_bprintf(strbuf, "dpc_sched %d host interrupt%spending\n",
+	            bus->dpc_sched, (bcmsdh_intr_pending(bus->sdh) ? " " : " not "));
+	bcm_bprintf(strbuf, "blocksize %d roundup %d\n", bus->blocksize, bus->roundup);
+#endif /* DHD_DEBUG */
+	bcm_bprintf(strbuf, "clkstate %d activity %d idletime %d idlecount %d sleeping %d\n",
+	            bus->clkstate, bus->activity, bus->idletime, bus->idlecount, bus->sleeping);
+}
+
+void
+dhd_bus_clearcounts(dhd_pub_t *dhdp)
+{
+	dhd_bus_t *bus = (dhd_bus_t *)dhdp->bus;
+
+	bus->intrcount = bus->lastintrs = bus->spurious = bus->regfails = 0;
+	bus->rxrtx = bus->rx_toolong = bus->rxc_errors = 0;
+	bus->rx_hdrfail = bus->rx_badhdr = bus->rx_badseq = 0;
+	bus->tx_sderrs = bus->fc_rcvd = bus->fc_xoff = bus->fc_xon = 0;
+	bus->rxglomfail = bus->rxglomframes = bus->rxglompkts = 0;
+	bus->f2rxhdrs = bus->f2rxdata = bus->f2txdata = bus->f1regdata = 0;
+}
+
+#ifdef SDTEST
+static int
+dhdsdio_pktgen_get(dhd_bus_t *bus, uint8 *arg)
+{
+	dhd_pktgen_t pktgen;
+
+	pktgen.version = DHD_PKTGEN_VERSION;
+	pktgen.freq = bus->pktgen_freq;
+	pktgen.count = bus->pktgen_count;
+	pktgen.print = bus->pktgen_print;
+	pktgen.total = bus->pktgen_total;
+	pktgen.minlen = bus->pktgen_minlen;
+	pktgen.maxlen = bus->pktgen_maxlen;
+	pktgen.numsent = bus->pktgen_sent;
+	pktgen.numrcvd = bus->pktgen_rcvd;
+	pktgen.numfail = bus->pktgen_fail;
+	pktgen.mode = bus->pktgen_mode;
+	pktgen.stop = bus->pktgen_stop;
+
+	bcopy(&pktgen, arg, sizeof(pktgen));
+
+	return 0;
+}
+
+static int
+dhdsdio_pktgen_set(dhd_bus_t *bus, uint8 *arg)
+{
+	dhd_pktgen_t pktgen;
+	uint oldcnt, oldmode;
+
+	bcopy(arg, &pktgen, sizeof(pktgen));
+	if (pktgen.version != DHD_PKTGEN_VERSION)
+		return BCME_BADARG;
+
+	oldcnt = bus->pktgen_count;
+	oldmode = bus->pktgen_mode;
+
+	bus->pktgen_freq = pktgen.freq;
+	bus->pktgen_count = pktgen.count;
+	bus->pktgen_print = pktgen.print;
+	bus->pktgen_total = pktgen.total;
+	bus->pktgen_minlen = pktgen.minlen;
+	bus->pktgen_maxlen = pktgen.maxlen;
+	bus->pktgen_mode = pktgen.mode;
+	bus->pktgen_stop = pktgen.stop;
+
+	bus->pktgen_tick = bus->pktgen_ptick = 0;
+	bus->pktgen_len = MAX(bus->pktgen_len, bus->pktgen_minlen);
+	bus->pktgen_len = MIN(bus->pktgen_len, bus->pktgen_maxlen);
+
+	/* Clear counts for a new pktgen (mode change, or was stopped) */
+	if (bus->pktgen_count && (!oldcnt || oldmode != bus->pktgen_mode))
+		bus->pktgen_sent = bus->pktgen_rcvd = bus->pktgen_fail = 0;
+
+	return 0;
+}
+#endif /* SDTEST */
+
+static int
+dhdsdio_membytes(dhd_bus_t *bus, bool write, uint32 address, uint8 *data, uint size)
+{
+	int bcmerror = 0;
+	uint32 sdaddr;
+	uint dsize;
+
+	/* Determine initial transfer parameters */
+	sdaddr = address & SBSDIO_SB_OFT_ADDR_MASK;
+	if ((sdaddr + size) & SBSDIO_SBWINDOW_MASK)
+		dsize = (SBSDIO_SB_OFT_ADDR_LIMIT - sdaddr);
+	else
+		dsize = size;
+
+	/* Set the backplane window to include the start address */
+	if ((bcmerror = dhdsdio_set_siaddr_window(bus, address))) {
+		DHD_ERROR(("%s: window change failed\n", __FUNCTION__));
+		goto xfer_done;
+	}
+
+	/* Do the transfer(s) */
+	while (size) {
+		DHD_INFO(("%s: %s %d bytes at offset 0x%08x in window 0x%08x\n",
+		          __FUNCTION__, (write ? "write" : "read"), dsize, sdaddr,
+		          (address & SBSDIO_SBWINDOW_MASK)));
+		if ((bcmerror = bcmsdh_rwdata(bus->sdh, write, sdaddr, data, dsize))) {
+			DHD_ERROR(("%s: membytes transfer failed\n", __FUNCTION__));
+			break;
+		}
+
+		/* Adjust for next transfer (if any) */
+		if ((size -= dsize)) {
+			data += dsize;
+			address += dsize;
+			if ((bcmerror = dhdsdio_set_siaddr_window(bus, address))) {
+				DHD_ERROR(("%s: window change failed\n", __FUNCTION__));
+				break;
+			}
+			sdaddr = 0;
+			dsize = MIN(SBSDIO_SB_OFT_ADDR_LIMIT, size);
+		}
+	}
+
+xfer_done:
+	/* Return the window to backplane enumeration space for core access */
+	if (dhdsdio_set_siaddr_window(bus, bcmsdh_cur_sbwad(bus->sdh))) {
+		DHD_ERROR(("%s: FAILED to set window back to 0x%x\n", __FUNCTION__,
+			bcmsdh_cur_sbwad(bus->sdh)));
+	}
+
+	return bcmerror;
+}
+
+#ifdef DHD_DEBUG_TRAP
+static int
+dhdsdio_readshared(dhd_bus_t *bus, sdpcm_shared_t *sh)
+{
+	uint32 addr;
+	int rv;
+
+	/* Read last word in memory to determine address of sdpcm_shared structure */
+	if ((rv = dhdsdio_membytes(bus, FALSE, bus->ramsize - 4, (uint8 *)&addr, 4)) < 0)
+		return rv;
+
+	addr = ltoh32(addr);
+
+	DHD_INFO(("sdpcm_shared address 0x%08X\n", addr));
+
+	/*
+	 * Check if addr is valid.
+	 * NVRAM length at the end of memory should have been overwritten.
+	 */
+	if (addr == 0 || ((~addr >> 16) & 0xffff) == (addr & 0xffff)) {
+		DHD_ERROR(("%s: address (0x%08x) of sdpcm_shared invalid\n", __FUNCTION__, addr));
+		return BCME_ERROR;
+	}
+
+	/* Read hndrte_shared structure */
+	if ((rv = dhdsdio_membytes(bus, FALSE, addr, (uint8 *)sh, sizeof(sdpcm_shared_t))) < 0)
+		return rv;
+
+	/* Endianness */
+	sh->flags = ltoh32(sh->flags);
+	sh->trap_addr = ltoh32(sh->trap_addr);
+	sh->assert_exp_addr = ltoh32(sh->assert_exp_addr);
+	sh->assert_file_addr = ltoh32(sh->assert_file_addr);
+	sh->assert_line = ltoh32(sh->assert_line);
+	sh->console_addr = ltoh32(sh->console_addr);
+	sh->msgtrace_addr = ltoh32(sh->msgtrace_addr);
+
+	if ((sh->flags & SDPCM_SHARED_VERSION_MASK) != SDPCM_SHARED_VERSION) {
+		DHD_ERROR(("%s: sdpcm_shared version %d in dhd "
+		           "is different than sdpcm_shared version %d in dongle\n",
+		           __FUNCTION__, SDPCM_SHARED_VERSION,
+		           sh->flags & SDPCM_SHARED_VERSION_MASK));
+		return BCME_ERROR;
+	}
+
+	return BCME_OK;
+}
+
+static int
+dhdsdio_checkdied(dhd_bus_t *bus, uint8 *data, uint size)
+{
+	int bcmerror = 0;
+	uint msize = 512;
+	char *mbuffer = NULL;
+	uint maxstrlen = 256;
+	char *str = NULL;
+	trap_t tr;
+	sdpcm_shared_t sdpcm_shared;
+	struct bcmstrbuf strbuf;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (data == NULL) {
+		/*
+		 * Called after a rx ctrl timeout. "data" is NULL.
+		 * allocate memory to trace the trap or assert.
+		 */
+		size = msize;
+		mbuffer = data = MALLOC(bus->dhd->osh, msize);
+		if (mbuffer == NULL) {
+			DHD_ERROR(("%s: MALLOC(%d) failed \n", __FUNCTION__, msize));
+			bcmerror = BCME_NOMEM;
+			goto done;
+		}
+	}
+
+	if ((str = MALLOC(bus->dhd->osh, maxstrlen)) == NULL) {
+		DHD_ERROR(("%s: MALLOC(%d) failed \n", __FUNCTION__, maxstrlen));
+		bcmerror = BCME_NOMEM;
+		goto done;
+	}
+
+	if ((bcmerror = dhdsdio_readshared(bus, &sdpcm_shared)) < 0)
+		goto done;
+
+	bcm_binit(&strbuf, data, size);
+
+	bcm_bprintf(&strbuf, "msgtrace address : 0x%08X\nconsole address  : 0x%08X\n",
+	            sdpcm_shared.msgtrace_addr, sdpcm_shared.console_addr);
+
+	if ((sdpcm_shared.flags & SDPCM_SHARED_ASSERT_BUILT) == 0) {
+		/* NOTE: Misspelled assert is intentional - DO NOT FIX.
+		 * (Avoids conflict with real asserts for programmatic parsing of output.)
+		 */
+		bcm_bprintf(&strbuf, "Assrt not built in dongle\n");
+	}
+
+	if ((sdpcm_shared.flags & (SDPCM_SHARED_ASSERT|SDPCM_SHARED_TRAP)) == 0) {
+		/* NOTE: Misspelled assert is intentional - DO NOT FIX.
+		 * (Avoids conflict with real asserts for programmatic parsing of output.)
+		 */
+		bcm_bprintf(&strbuf, "No trap%s in dongle",
+		          (sdpcm_shared.flags & SDPCM_SHARED_ASSERT_BUILT)
+		          ?"/assrt" :"");
+	} else {
+		if (sdpcm_shared.flags & SDPCM_SHARED_ASSERT) {
+			/* Download assert */
+			bcm_bprintf(&strbuf, "Dongle assert");
+			if (sdpcm_shared.assert_exp_addr != 0) {
+				str[0] = '\0';
+				if ((bcmerror = dhdsdio_membytes(bus, FALSE,
+				                                 sdpcm_shared.assert_exp_addr,
+				                                 (uint8 *)str, maxstrlen)) < 0)
+					goto done;
+
+				str[maxstrlen - 1] = '\0';
+				bcm_bprintf(&strbuf, " expr \"%s\"", str);
+			}
+
+			if (sdpcm_shared.assert_file_addr != 0) {
+				str[0] = '\0';
+				if ((bcmerror = dhdsdio_membytes(bus, FALSE,
+				                                 sdpcm_shared.assert_file_addr,
+				                                 (uint8 *)str, maxstrlen)) < 0)
+					goto done;
+
+				str[maxstrlen - 1] = '\0';
+				bcm_bprintf(&strbuf, " file \"%s\"", str);
+			}
+
+			bcm_bprintf(&strbuf, " line %d ", sdpcm_shared.assert_line);
+		}
+
+		if (sdpcm_shared.flags & SDPCM_SHARED_TRAP) {
+			if ((bcmerror = dhdsdio_membytes(bus, FALSE,
+			                                 sdpcm_shared.trap_addr,
+			                                 (uint8*)&tr, sizeof(trap_t))) < 0)
+				goto done;
+
+			bcm_bprintf(&strbuf,
+			"Dongle trap type 0x%x @ epc 0x%x, cpsr 0x%x, spsr 0x%x, sp 0x%x,"
+			"lp 0x%x, rpc 0x%x Trap offset 0x%x, "
+			"r0 0x%x, r1 0x%x, r2 0x%x, r3 0x%x, r4 0x%x, r5 0x%x, r6 0x%x, r7 0x%x\n",
+			tr.type, tr.epc, tr.cpsr, tr.spsr, tr.r13, tr.r14, tr.pc,
+			sdpcm_shared.trap_addr,
+			tr.r0, tr.r1, tr.r2, tr.r3, tr.r4, tr.r5, tr.r6, tr.r7);
+		}
+	}
+
+	if (sdpcm_shared.flags & (SDPCM_SHARED_ASSERT | SDPCM_SHARED_TRAP)) {
+		DHD_ERROR(("%s: %s\n", __FUNCTION__, strbuf.origbuf));
+	}
+
+done:
+	if (mbuffer)
+		MFREE(bus->dhd->osh, mbuffer, msize);
+	if (str)
+		MFREE(bus->dhd->osh, str, maxstrlen);
+
+	return bcmerror;
+}
+#endif /* DHD_DEBUG_TRAP */
+
+#ifdef DHD_DEBUG
+#define CONSOLE_LINE_MAX	192
+
+static int
+dhdsdio_readconsole(dhd_bus_t *bus)
+{
+	dhd_console_t *c = &bus->console;
+	uint8 line[CONSOLE_LINE_MAX], ch;
+	uint32 n, idx, addr;
+	int rv;
+
+	/* Don't do anything until FWREADY updates console address */
+	if (bus->console_addr == 0)
+		return 0;
+
+	/* Read console log struct */
+	addr = bus->console_addr + OFFSETOF(hndrte_cons_t, log);
+	if ((rv = dhdsdio_membytes(bus, FALSE, addr, (uint8 *)&c->log, sizeof(c->log))) < 0)
+		return rv;
+
+	/* Allocate console buffer (one time only) */
+	if (c->buf == NULL) {
+		c->bufsize = ltoh32(c->log.buf_size);
+		if ((c->buf = MALLOC(bus->dhd->osh, c->bufsize)) == NULL)
+			return BCME_NOMEM;
+	}
+
+	idx = ltoh32(c->log.idx);
+
+	/* Protect against corrupt value */
+	if (idx > c->bufsize)
+		return BCME_ERROR;
+
+	/* Skip reading the console buffer if the index pointer has not moved */
+	if (idx == c->last)
+		return BCME_OK;
+
+	/* Read the console buffer */
+	addr = ltoh32(c->log.buf);
+	if ((rv = dhdsdio_membytes(bus, FALSE, addr, c->buf, c->bufsize)) < 0)
+		return rv;
+
+	while (c->last != idx) {
+		for (n = 0; n < CONSOLE_LINE_MAX - 2; n++) {
+			if (c->last == idx) {
+				/* This would output a partial line.  Instead, back up
+				 * the buffer pointer and output this line next time around.
+				 */
+				if (c->last >= n)
+					c->last -= n;
+				else
+					c->last = c->bufsize - n;
+				goto break2;
+			}
+			ch = c->buf[c->last];
+			c->last = (c->last + 1) % c->bufsize;
+			if (ch == '\n')
+				break;
+			line[n] = ch;
+		}
+
+		if (n > 0) {
+			if (line[n - 1] == '\r')
+				n--;
+			line[n] = 0;
+			printf("CONSOLE: %s\n", line);
+		}
+	}
+break2:
+
+	return BCME_OK;
+}
+#endif /* DHD_DEBUG */
+
+int
+dhdsdio_downloadvars(dhd_bus_t *bus, void *arg, int len)
+{
+	int bcmerror = BCME_OK;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	/* Basic sanity checks */
+	if (bus->dhd->up) {
+		bcmerror = BCME_NOTDOWN;
+		goto err;
+	}
+	if (!len) {
+		bcmerror = BCME_BUFTOOSHORT;
+		goto err;
+	}
+
+	/* Free the old ones and replace with passed variables */
+	if (bus->vars)
+		MFREE(bus->dhd->osh, bus->vars, bus->varsz);
+
+	bus->vars = MALLOC(bus->dhd->osh, len);
+	bus->varsz = bus->vars ? len : 0;
+	if (bus->vars == NULL) {
+		bcmerror = BCME_NOMEM;
+		goto err;
+	}
+
+	/* Copy the passed variables, which should include the terminating double-null */
+	bcopy(arg, bus->vars, bus->varsz);
+err:
+	return bcmerror;
+}
+
+static int
+dhdsdio_doiovar(dhd_bus_t *bus, const bcm_iovar_t *vi, uint32 actionid, const char *name,
+                void *params, int plen, void *arg, int len, int val_size)
+{
+	int bcmerror = 0;
+	int32 int_val = 0;
+	bool bool_val = 0;
+
+	DHD_TRACE(("%s: Enter, action %d name %s params %p plen %d arg %p len %d val_size %d\n",
+	           __FUNCTION__, actionid, name, params, plen, arg, len, val_size));
+
+	if ((bcmerror = bcm_iovar_lencheck(vi, arg, len, IOV_ISSET(actionid))) != 0)
+		goto exit;
+
+	if (plen >= (int)sizeof(int_val))
+		bcopy(params, &int_val, sizeof(int_val));
+
+	bool_val = (int_val != 0) ? TRUE : FALSE;
+
+
+	/* Some ioctls use the bus */
+	dhd_os_sdlock(bus->dhd);
+
+	/* Check if dongle is in reset. If so, only allow DEVRESET iovars */
+	if (bus->dhd->dongle_reset && !(actionid == IOV_SVAL(IOV_DEVRESET) ||
+	                                actionid == IOV_GVAL(IOV_DEVRESET))) {
+		bcmerror = BCME_NOTREADY;
+		goto exit;
+	}
+
+	/* Handle sleep stuff before any clock mucking */
+	if (vi->varid == IOV_SLEEP) {
+		if (IOV_ISSET(actionid)) {
+			bcmerror = dhdsdio_bussleep(bus, bool_val);
+		} else {
+			int_val = (int32)bus->sleeping;
+			bcopy(&int_val, arg, val_size);
+		}
+		goto exit;
+	}
+
+	/* Request clock to allow SDIO accesses */
+	if (!bus->dhd->dongle_reset) {
+		BUS_WAKE(bus);
+		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+	}
+
+	switch (actionid) {
+	case IOV_GVAL(IOV_INTR):
+		int_val = (int32)bus->intr;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_INTR):
+		bus->intr = bool_val;
+		bus->intdis = FALSE;
+		if (bus->dhd->up) {
+			if (bus->intr) {
+				DHD_INTR(("%s: enable SDIO device interrupts\n", __FUNCTION__));
+				bcmsdh_intr_enable(bus->sdh);
+			} else {
+				DHD_INTR(("%s: disable SDIO interrupts\n", __FUNCTION__));
+				bcmsdh_intr_disable(bus->sdh);
+			}
+		}
+		break;
+
+	case IOV_GVAL(IOV_POLLRATE):
+		int_val = (int32)bus->pollrate;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_POLLRATE):
+		bus->pollrate = (uint)int_val;
+		bus->poll = (bus->pollrate != 0);
+		break;
+
+	case IOV_GVAL(IOV_IDLETIME):
+		int_val = bus->idletime;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_IDLETIME):
+		if ((int_val < 0) && (int_val != DHD_IDLE_IMMEDIATE)) {
+			bcmerror = BCME_BADARG;
+		} else {
+			bus->idletime = int_val;
+		}
+		break;
+
+	case IOV_GVAL(IOV_IDLECLOCK):
+		int_val = (int32)bus->idleclock;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_IDLECLOCK):
+		bus->idleclock = int_val;
+		break;
+
+	case IOV_GVAL(IOV_SD1IDLE):
+		int_val = (int32)sd1idle;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_SD1IDLE):
+		sd1idle = bool_val;
+		break;
+
+
+	case IOV_SVAL(IOV_MEMBYTES):
+	case IOV_GVAL(IOV_MEMBYTES):
+	{
+		uint32 address;
+		uint size, dsize;
+		uint8 *data;
+
+		bool set = (actionid == IOV_SVAL(IOV_MEMBYTES));
+
+		ASSERT(plen >= 2*sizeof(int));
+
+		address = (uint32)int_val;
+		bcopy((char *)params + sizeof(int_val), &int_val, sizeof(int_val));
+		size = (uint)int_val;
+
+		/* Do some validation */
+		dsize = set ? plen - (2 * sizeof(int)) : len;
+		if (dsize < size) {
+			DHD_ERROR(("%s: error on %s membytes, addr 0x%08x size %d dsize %d\n",
+			           __FUNCTION__, (set ? "set" : "get"), address, size, dsize));
+			bcmerror = BCME_BADARG;
+			break;
+		}
+
+		DHD_INFO(("%s: Request to %s %d bytes at address 0x%08x\n", __FUNCTION__,
+		          (set ? "write" : "read"), size, address));
+
+		/* If we know about SOCRAM, check for a fit */
+		if ((bus->orig_ramsize) &&
+		    ((address > bus->orig_ramsize) || (address + size > bus->orig_ramsize))) {
+			DHD_ERROR(("%s: ramsize 0x%08x doesn't have %d bytes at 0x%08x\n",
+			           __FUNCTION__, bus->orig_ramsize, size, address));
+			bcmerror = BCME_BADARG;
+			break;
+		}
+
+		/* Generate the actual data pointer */
+		data = set ? (uint8*)params + 2 * sizeof(int): (uint8*)arg;
+
+		/* Call to do the transfer */
+		bcmerror = dhdsdio_membytes(bus, set, address, data, size);
+
+		break;
+	}
+
+	case IOV_GVAL(IOV_MEMSIZE):
+		int_val = (int32)bus->ramsize;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_SDIOD_DRIVE):
+		int_val = (int32)dhd_sdiod_drive_strength;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_SDIOD_DRIVE):
+		dhd_sdiod_drive_strength = int_val;
+		si_sdiod_drive_strength_init(bus->sih, bus->dhd->osh, dhd_sdiod_drive_strength);
+		break;
+
+	case IOV_SVAL(IOV_DOWNLOAD):
+		bcmerror = dhdsdio_download_state(bus, bool_val);
+		break;
+
+	case IOV_SVAL(IOV_VARS):
+		bcmerror = dhdsdio_downloadvars(bus, arg, len);
+		break;
+
+	case IOV_GVAL(IOV_READAHEAD):
+		int_val = (int32)dhd_readahead;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_READAHEAD):
+		if (bool_val && !dhd_readahead)
+			bus->nextlen = 0;
+		dhd_readahead = bool_val;
+		break;
+
+	case IOV_GVAL(IOV_SDRXCHAIN):
+		int_val = (int32)bus->use_rxchain;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_SDRXCHAIN):
+		if (bool_val && !bus->sd_rxchain)
+			bcmerror = BCME_UNSUPPORTED;
+		else
+			bus->use_rxchain = bool_val;
+		break;
+	case IOV_GVAL(IOV_ALIGNCTL):
+		int_val = (int32)dhd_alignctl;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_ALIGNCTL):
+		dhd_alignctl = bool_val;
+		break;
+
+	case IOV_GVAL(IOV_SDALIGN):
+		int_val = DHD_SDALIGN;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+#ifdef DHD_DEBUG
+	case IOV_GVAL(IOV_VARS):
+		if (bus->varsz < (uint)len)
+			bcopy(bus->vars, arg, bus->varsz);
+		else
+			bcmerror = BCME_BUFTOOSHORT;
+		break;
+#endif /* DHD_DEBUG */
+
+#ifdef DHD_DEBUG
+	case IOV_GVAL(IOV_SDREG):
+	{
+		sdreg_t *sd_ptr;
+		uint32 addr, size;
+
+		sd_ptr = (sdreg_t *)params;
+
+		addr = (uintptr)bus->regs + sd_ptr->offset;
+		size = sd_ptr->func;
+		int_val = (int32)bcmsdh_reg_read(bus->sdh, addr, size);
+		if (bcmsdh_regfail(bus->sdh))
+			bcmerror = BCME_SDIO_ERROR;
+		bcopy(&int_val, arg, sizeof(int32));
+		break;
+	}
+
+	case IOV_SVAL(IOV_SDREG):
+	{
+		sdreg_t *sd_ptr;
+		uint32 addr, size;
+
+		sd_ptr = (sdreg_t *)params;
+
+		addr = (uintptr)bus->regs + sd_ptr->offset;
+		size = sd_ptr->func;
+		bcmsdh_reg_write(bus->sdh, addr, size, sd_ptr->value);
+		if (bcmsdh_regfail(bus->sdh))
+			bcmerror = BCME_SDIO_ERROR;
+		break;
+	}
+
+	/* Same as above, but offset is not backplane (not SDIO core) */
+	case IOV_GVAL(IOV_SBREG):
+	{
+		sdreg_t sdreg;
+		uint32 addr, size;
+
+		bcopy(params, &sdreg, sizeof(sdreg));
+
+		addr = SI_ENUM_BASE + sdreg.offset;
+		size = sdreg.func;
+		int_val = (int32)bcmsdh_reg_read(bus->sdh, addr, size);
+		if (bcmsdh_regfail(bus->sdh))
+			bcmerror = BCME_SDIO_ERROR;
+		bcopy(&int_val, arg, sizeof(int32));
+		break;
+	}
+
+	case IOV_SVAL(IOV_SBREG):
+	{
+		sdreg_t sdreg;
+		uint32 addr, size;
+
+		bcopy(params, &sdreg, sizeof(sdreg));
+
+		addr = SI_ENUM_BASE + sdreg.offset;
+		size = sdreg.func;
+		bcmsdh_reg_write(bus->sdh, addr, size, sdreg.value);
+		if (bcmsdh_regfail(bus->sdh))
+			bcmerror = BCME_SDIO_ERROR;
+		break;
+	}
+
+	case IOV_GVAL(IOV_SDCIS):
+	{
+		*(char *)arg = 0;
+
+		bcmstrcat(arg, "\nFunc 0\n");
+		bcmsdh_cis_read(bus->sdh, 0x10, (uint8 *)arg + strlen(arg), SBSDIO_CIS_SIZE_LIMIT);
+		bcmstrcat(arg, "\nFunc 1\n");
+		bcmsdh_cis_read(bus->sdh, 0x11, (uint8 *)arg + strlen(arg), SBSDIO_CIS_SIZE_LIMIT);
+		bcmstrcat(arg, "\nFunc 2\n");
+		bcmsdh_cis_read(bus->sdh, 0x12, (uint8 *)arg + strlen(arg), SBSDIO_CIS_SIZE_LIMIT);
+		break;
+	}
+
+	case IOV_GVAL(IOV_FORCEEVEN):
+		int_val = (int32)forcealign;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_FORCEEVEN):
+		forcealign = bool_val;
+		break;
+
+	case IOV_GVAL(IOV_TXBOUND):
+		int_val = (int32)dhd_txbound;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_TXBOUND):
+		dhd_txbound = (uint)int_val;
+		break;
+
+	case IOV_GVAL(IOV_RXBOUND):
+		int_val = (int32)dhd_rxbound;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_RXBOUND):
+		dhd_rxbound = (uint)int_val;
+		break;
+
+	case IOV_GVAL(IOV_TXMINMAX):
+		int_val = (int32)dhd_txminmax;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_TXMINMAX):
+		dhd_txminmax = (uint)int_val;
+		break;
+
+
+
+#endif /* DHD_DEBUG */
+
+
+#ifdef SDTEST
+	case IOV_GVAL(IOV_EXTLOOP):
+		int_val = (int32)bus->ext_loop;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_EXTLOOP):
+		bus->ext_loop = bool_val;
+		break;
+
+	case IOV_GVAL(IOV_PKTGEN):
+		bcmerror = dhdsdio_pktgen_get(bus, arg);
+		break;
+
+	case IOV_SVAL(IOV_PKTGEN):
+		bcmerror = dhdsdio_pktgen_set(bus, arg);
+		break;
+#endif /* SDTEST */
+
+
+	case IOV_SVAL(IOV_DEVRESET):
+		DHD_TRACE(("%s: Called set IOV_DEVRESET=%d dongle_reset=%d busstate=%d\n",
+		           __FUNCTION__, bool_val, bus->dhd->dongle_reset,
+		           bus->dhd->busstate));
+
+		ASSERT(bus->dhd->osh);
+		/* ASSERT(bus->cl_devid); */
+
+		dhd_bus_devreset(bus->dhd, (uint8)bool_val);
+
+		break;
+
+	case IOV_GVAL(IOV_DEVRESET):
+		DHD_TRACE(("%s: Called get IOV_DEVRESET\n", __FUNCTION__));
+
+		/* Get its status */
+		int_val = (bool) bus->dhd->dongle_reset;
+		bcopy(&int_val, arg, val_size);
+
+		break;
+
+	default:
+		bcmerror = BCME_UNSUPPORTED;
+		break;
+	}
+
+exit:
+	if ((bus->idletime == DHD_IDLE_IMMEDIATE) && !bus->dpc_sched) {
+		bus->activity = FALSE;
+		dhdsdio_clkctl(bus, CLK_NONE, TRUE);
+	}
+
+	dhd_os_sdunlock(bus->dhd);
+
+	if (actionid == IOV_SVAL(IOV_DEVRESET) && bool_val == FALSE)
+		dhd_preinit_ioctls((dhd_pub_t *) bus->dhd);
+
+	return bcmerror;
+}
+
+static int
+dhdsdio_write_vars(dhd_bus_t *bus)
+{
+	int bcmerror = 0;
+	uint32 varsize;
+	uint32 varaddr;
+	uint8 *vbuffer;
+	uint32 varsizew;
+#ifdef DHD_DEBUG
+	char *nvram_ularray;
+#endif /* DHD_DEBUG */
+
+	/* Even if there are no vars are to be written, we still need to set the ramsize. */
+	varsize = bus->varsz ? ROUNDUP(bus->varsz, 4) : 0;
+	varaddr = (bus->ramsize - 4) - varsize;
+
+	if (bus->vars) {
+		vbuffer = (uint8 *)MALLOC(bus->dhd->osh, varsize);
+		if (!vbuffer)
+			return BCME_NOMEM;
+
+		bzero(vbuffer, varsize);
+		bcopy(bus->vars, vbuffer, bus->varsz);
+
+		/* Write the vars list */
+		bcmerror = dhdsdio_membytes(bus, TRUE, varaddr, vbuffer, varsize);
+#ifdef DHD_DEBUG
+		/* Verify NVRAM bytes */
+		DHD_INFO(("Compare NVRAM dl & ul; varsize=%d\n", varsize));
+		nvram_ularray = (char*)MALLOC(bus->dhd->osh, varsize);
+		if (!nvram_ularray)
+			return BCME_NOMEM;
+
+		/* Upload image to verify downloaded contents. */
+		memset(nvram_ularray, 0xaa, varsize);
+
+		/* Read the vars list to temp buffer for comparison */
+		bcmerror = dhdsdio_membytes(bus, FALSE, varaddr, nvram_ularray, varsize);
+		if (bcmerror) {
+				DHD_ERROR(("%s: error %d on reading %d nvram bytes at 0x%08x\n",
+					__FUNCTION__, bcmerror, varsize, varaddr));
+		}
+		/* Compare the org NVRAM with the one read from RAM */
+		if (memcmp(vbuffer, nvram_ularray, varsize)) {
+			DHD_ERROR(("%s: Downloaded NVRAM image is corrupted.\n", __FUNCTION__));
+		} else
+			DHD_ERROR(("%s: Download, Upload and compare of NVRAM succeeded.\n",
+			__FUNCTION__));
+
+		MFREE(bus->dhd->osh, nvram_ularray, varsize);
+#endif /* DHD_DEBUG */
+
+		MFREE(bus->dhd->osh, vbuffer, varsize);
+	}
+
+	/* adjust to the user specified RAM */
+	DHD_INFO(("Physical memory size: %d, usable memory size: %d\n",
+		bus->orig_ramsize, bus->ramsize));
+	DHD_INFO(("Vars are at %d, orig varsize is %d\n",
+		varaddr, varsize));
+	varsize = ((bus->orig_ramsize - 4) - varaddr);
+
+	/*
+	 * Determine the length token:
+	 * Varsize, converted to words, in lower 16-bits, checksum in upper 16-bits.
+	 */
+	if (bcmerror) {
+		varsizew = 0;
+	} else {
+		varsizew = varsize / 4;
+		varsizew = (~varsizew << 16) | (varsizew & 0x0000FFFF);
+		varsizew = htol32(varsizew);
+	}
+
+	DHD_INFO(("New varsize is %d, length token=0x%08x\n", varsize, varsizew));
+
+	/* Write the length token to the last word */
+	bcmerror = dhdsdio_membytes(bus, TRUE, (bus->orig_ramsize - 4),
+		(uint8*)&varsizew, 4);
+
+	return bcmerror;
+}
+
+static int
+dhdsdio_download_state(dhd_bus_t *bus, bool enter)
+{
+	uint retries;
+	int bcmerror = 0;
+
+	/* To enter download state, disable ARM and reset SOCRAM.
+	 * To exit download state, simply reset ARM (default is RAM boot).
+	 */
+	if (enter) {
+
+		bus->alp_only = TRUE;
+
+		if (!(si_setcore(bus->sih, ARM7S_CORE_ID, 0)) &&
+		    !(si_setcore(bus->sih, ARMCM3_CORE_ID, 0))) {
+			DHD_ERROR(("%s: Failed to find ARM core!\n", __FUNCTION__));
+			bcmerror = BCME_ERROR;
+			goto fail;
+		}
+
+		si_core_disable(bus->sih, 0);
+		if (bcmsdh_regfail(bus->sdh)) {
+			bcmerror = BCME_SDIO_ERROR;
+			goto fail;
+		}
+
+		if (!(si_setcore(bus->sih, SOCRAM_CORE_ID, 0))) {
+			DHD_ERROR(("%s: Failed to find SOCRAM core!\n", __FUNCTION__));
+			bcmerror = BCME_ERROR;
+			goto fail;
+		}
+
+		si_core_reset(bus->sih, 0, 0);
+		if (bcmsdh_regfail(bus->sdh)) {
+			DHD_ERROR(("%s: Failure trying reset SOCRAM core?\n", __FUNCTION__));
+			bcmerror = BCME_SDIO_ERROR;
+			goto fail;
+		}
+
+		/* Clear the top bit of memory */
+		if (bus->ramsize) {
+			uint32 zeros = 0;
+			dhdsdio_membytes(bus, TRUE, bus->ramsize - 4, (uint8*)&zeros, 4);
+		}
+	} else {
+		if (!(si_setcore(bus->sih, SOCRAM_CORE_ID, 0))) {
+			DHD_ERROR(("%s: Failed to find SOCRAM core!\n", __FUNCTION__));
+			bcmerror = BCME_ERROR;
+			goto fail;
+		}
+
+		if (!si_iscoreup(bus->sih)) {
+			DHD_ERROR(("%s: SOCRAM core is down after reset?\n", __FUNCTION__));
+			bcmerror = BCME_ERROR;
+			goto fail;
+		}
+
+		if ((bcmerror = dhdsdio_write_vars(bus))) {
+			DHD_ERROR(("%s: no vars written to RAM\n", __FUNCTION__));
+			bcmerror = 0;
+		}
+
+		if (!si_setcore(bus->sih, PCMCIA_CORE_ID, 0) &&
+		    !si_setcore(bus->sih, SDIOD_CORE_ID, 0)) {
+			DHD_ERROR(("%s: Can't change back to SDIO core?\n", __FUNCTION__));
+			bcmerror = BCME_ERROR;
+			goto fail;
+		}
+		W_SDREG(0xFFFFFFFF, &bus->regs->intstatus, retries);
+
+
+		if (!(si_setcore(bus->sih, ARM7S_CORE_ID, 0)) &&
+		    !(si_setcore(bus->sih, ARMCM3_CORE_ID, 0))) {
+			DHD_ERROR(("%s: Failed to find ARM core!\n", __FUNCTION__));
+			bcmerror = BCME_ERROR;
+			goto fail;
+		}
+
+		si_core_reset(bus->sih, 0, 0);
+		if (bcmsdh_regfail(bus->sdh)) {
+			DHD_ERROR(("%s: Failure trying to reset ARM core?\n", __FUNCTION__));
+			bcmerror = BCME_SDIO_ERROR;
+			goto fail;
+		}
+
+		/* Allow HT Clock now that the ARM is running. */
+		bus->alp_only = FALSE;
+
+		bus->dhd->busstate = DHD_BUS_LOAD;
+	}
+
+fail:
+	/* Always return to SDIOD core */
+	if (!si_setcore(bus->sih, PCMCIA_CORE_ID, 0))
+		si_setcore(bus->sih, SDIOD_CORE_ID, 0);
+
+	return bcmerror;
+}
+
+int
+dhd_bus_iovar_op(dhd_pub_t *dhdp, const char *name,
+                 void *params, int plen, void *arg, int len, bool set)
+{
+	dhd_bus_t *bus = dhdp->bus;
+	const bcm_iovar_t *vi = NULL;
+	int bcmerror = 0;
+	int val_size;
+	uint32 actionid;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	ASSERT(name);
+	ASSERT(len >= 0);
+
+	/* Get MUST have return space */
+	ASSERT(set || (arg && len));
+
+	/* Set does NOT take qualifiers */
+	ASSERT(!set || (!params && !plen));
+
+	/* Look up var locally; if not found pass to host driver */
+	if ((vi = bcm_iovar_lookup(dhdsdio_iovars, name)) == NULL) {
+		dhd_os_sdlock(bus->dhd);
+
+		BUS_WAKE(bus);
+
+		/* Turn on clock in case SD command needs backplane */
+		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+
+		bcmerror = bcmsdh_iovar_op(bus->sdh, name, params, plen, arg, len, set);
+
+		/* Check for bus configuration changes of interest */
+
+		/* If it was divisor change, read the new one */
+		if (set && strcmp(name, "sd_divisor") == 0) {
+			if (bcmsdh_iovar_op(bus->sdh, "sd_divisor", NULL, 0,
+			                    &bus->sd_divisor, sizeof(int32), FALSE) != BCME_OK) {
+				bus->sd_divisor = -1;
+				DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, name));
+			} else {
+				DHD_INFO(("%s: noted %s update, value now %d\n",
+				          __FUNCTION__, name, bus->sd_divisor));
+			}
+		}
+		/* If it was a mode change, read the new one */
+		if (set && strcmp(name, "sd_mode") == 0) {
+			if (bcmsdh_iovar_op(bus->sdh, "sd_mode", NULL, 0,
+			                    &bus->sd_mode, sizeof(int32), FALSE) != BCME_OK) {
+				bus->sd_mode = -1;
+				DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, name));
+			} else {
+				DHD_INFO(("%s: noted %s update, value now %d\n",
+				          __FUNCTION__, name, bus->sd_mode));
+			}
+		}
+		/* Similar check for blocksize change */
+		if (set && strcmp(name, "sd_blocksize") == 0) {
+			int32 fnum = 2;
+			if (bcmsdh_iovar_op(bus->sdh, "sd_blocksize", &fnum, sizeof(int32),
+			                    &bus->blocksize, sizeof(int32), FALSE) != BCME_OK) {
+				bus->blocksize = 0;
+				DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, "sd_blocksize"));
+			} else {
+				DHD_INFO(("%s: noted %s update, value now %d\n",
+				          __FUNCTION__, "sd_blocksize", bus->blocksize));
+			}
+		}
+		bus->roundup = MIN(max_roundup, bus->blocksize);
+
+		if ((bus->idletime == DHD_IDLE_IMMEDIATE) && !bus->dpc_sched) {
+			bus->activity = FALSE;
+			dhdsdio_clkctl(bus, CLK_NONE, TRUE);
+		}
+
+		dhd_os_sdunlock(bus->dhd);
+		goto exit;
+	}
+
+	DHD_CTL(("%s: %s %s, len %d plen %d\n", __FUNCTION__,
+	         name, (set ? "set" : "get"), len, plen));
+
+	/* set up 'params' pointer in case this is a set command so that
+	 * the convenience int and bool code can be common to set and get
+	 */
+	if (params == NULL) {
+		params = arg;
+		plen = len;
+	}
+
+	if (vi->type == IOVT_VOID)
+		val_size = 0;
+	else if (vi->type == IOVT_BUFFER)
+		val_size = len;
+	else
+		/* all other types are integer sized */
+		val_size = sizeof(int);
+
+	actionid = set ? IOV_SVAL(vi->varid) : IOV_GVAL(vi->varid);
+	bcmerror = dhdsdio_doiovar(bus, vi, actionid, name, params, plen, arg, len, val_size);
+
+exit:
+	return bcmerror;
+}
+
+void
+dhd_bus_stop(struct dhd_bus *bus, bool enforce_mutex)
+{
+	osl_t *osh = bus->dhd->osh;
+	uint32 local_hostintmask;
+	uint8 saveclk;
+	uint retries;
+	int err;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (enforce_mutex)
+		dhd_os_sdlock(bus->dhd);
+
+	BUS_WAKE(bus);
+
+	/* Change our idea of bus state */
+	bus->dhd->busstate = DHD_BUS_DOWN;
+
+	/* Enable clock for device interrupts */
+	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+
+	/* Disable and clear interrupts at the chip level also */
+	W_SDREG(0, &bus->regs->hostintmask, retries);
+	local_hostintmask = bus->hostintmask;
+	bus->hostintmask = 0;
+
+	/* Force clocks on backplane to be sure F2 interrupt propagates */
+	saveclk = bcmsdh_cfg_read(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, &err);
+	if (!err) {
+		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
+		                 (saveclk | SBSDIO_FORCE_HT), &err);
+	}
+	if (err) {
+		DHD_ERROR(("%s: Failed to force clock for F2: err %d\n", __FUNCTION__, err));
+	}
+
+	/* Turn off the bus (F2), free any pending packets */
+	DHD_INTR(("%s: disable SDIO interrupts\n", __FUNCTION__));
+	bcmsdh_intr_disable(bus->sdh);
+	bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_0, SDIOD_CCCR_IOEN, SDIO_FUNC_ENABLE_1, NULL);
+
+	/* Clear any pending interrupts now that F2 is disabled */
+	W_SDREG(local_hostintmask, &bus->regs->intstatus, retries);
+
+	/* Turn off the backplane clock (only) */
+	dhdsdio_clkctl(bus, CLK_SDONLY, FALSE);
+
+	/* Clear the data packet queues */
+	pktq_flush(osh, &bus->txq, TRUE);
+
+	/* Clear any held glomming stuff */
+	if (bus->glomd)
+		PKTFREE(osh, bus->glomd, FALSE);
+
+	if (bus->glom)
+		PKTFREE(osh, bus->glom, FALSE);
+
+	bus->glom = bus->glomd = NULL;
+
+	/* Clear rx control and wake any waiters */
+	bus->rxlen = 0;
+	dhd_os_ioctl_resp_wake(bus->dhd);
+
+	/* Reset some F2 state stuff */
+	bus->rxskip = FALSE;
+	bus->tx_seq = bus->rx_seq = 0;
+
+	if (enforce_mutex)
+		dhd_os_sdunlock(bus->dhd);
+}
+
+int
+dhd_bus_init(dhd_pub_t *dhdp, bool enforce_mutex)
+{
+	dhd_bus_t *bus = dhdp->bus;
+	dhd_timeout_t tmo;
+	uint retries = 0;
+	uint8 ready, enable;
+	int err, ret = BCME_ERROR;
+	uint8 saveclk;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	ASSERT(bus->dhd);
+	if (!bus->dhd)
+		return BCME_OK;
+
+	if (enforce_mutex)
+		dhd_os_sdlock(bus->dhd);
+
+	/* Make sure backplane clock is on, needed to generate F2 interrupt */
+	err = dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+	if ((err != BCME_OK) || (bus->clkstate != CLK_AVAIL)) {
+		DHD_ERROR(("%s: Failed to set backplane clock: err %d\n", __FUNCTION__, err));
+		goto exit;
+	}
+
+	/* Force clocks on backplane to be sure F2 interrupt propagates */
+	saveclk = bcmsdh_cfg_read(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, &err);
+	if (!err) {
+		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
+		                 (saveclk | SBSDIO_FORCE_HT), &err);
+	}
+	if (err) {
+		DHD_ERROR(("%s: Failed to force clock for F2: err %d\n", __FUNCTION__, err));
+		goto exit;
+	}
+
+	/* Enable function 2 (frame transfers) */
+	W_SDREG((SDPCM_PROT_VERSION << SMB_DATA_VERSION_SHIFT),
+	        &bus->regs->tosbmailboxdata, retries);
+	enable = (SDIO_FUNC_ENABLE_1 | SDIO_FUNC_ENABLE_2);
+
+	bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_0, SDIOD_CCCR_IOEN, enable, NULL);
+
+	/* Give the dongle some time to do its thing and set IOR2 */
+	dhd_timeout_start(&tmo, DHD_WAIT_F2RDY * 1000);
+
+	ready = 0;
+	while (ready != enable && !dhd_timeout_expired(&tmo))
+	        ready = bcmsdh_cfg_read(bus->sdh, SDIO_FUNC_0, SDIOD_CCCR_IORDY, NULL);
+
+
+	DHD_INFO(("%s: enable 0x%02x, ready 0x%02x (waited %uus)\n",
+	          __FUNCTION__, enable, ready, tmo.elapsed));
+
+
+	/* If F2 successfully enabled, set core and enable interrupts */
+	if (ready == enable) {
+		/* Make sure we're talking to the core. */
+		if (!(bus->regs = si_setcore(bus->sih, PCMCIA_CORE_ID, 0)))
+			bus->regs = si_setcore(bus->sih, SDIOD_CORE_ID, 0);
+
+		/* Set up the interrupt mask and enable interrupts */
+		bus->hostintmask = HOSTINTMASK;
+		W_SDREG(bus->hostintmask, &bus->regs->hostintmask, retries);
+
+		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_WATERMARK, (uint8)watermark, &err);
+
+		/* Set bus state according to enable result */
+		dhdp->busstate = DHD_BUS_DATA;
+
+		/* bcmsdh_intr_unmask(bus->sdh); */
+
+		bus->intdis = FALSE;
+		if (bus->intr) {
+			DHD_INTR(("%s: enable SDIO device interrupts\n", __FUNCTION__));
+			bcmsdh_intr_enable(bus->sdh);
+		} else {
+			DHD_INTR(("%s: disable SDIO interrupts\n", __FUNCTION__));
+			bcmsdh_intr_disable(bus->sdh);
+		}
+
+	}
+
+
+	else {
+		/* Disable F2 again */
+		enable = SDIO_FUNC_ENABLE_1;
+		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_0, SDIOD_CCCR_IOEN, enable, NULL);
+	}
+
+	/* Restore previous clock setting */
+	bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, saveclk, &err);
+
+
+	/* If we didn't come up, turn off backplane clock */
+	if (dhdp->busstate != DHD_BUS_DATA)
+		dhdsdio_clkctl(bus, CLK_NONE, FALSE);
+
+	ret = BCME_OK;
+exit:
+	if (enforce_mutex)
+		dhd_os_sdunlock(bus->dhd);
+
+	return ret;
+}
+
+static void
+dhdsdio_rxfail(dhd_bus_t *bus, bool abort, bool rtx)
+{
+	bcmsdh_info_t *sdh = bus->sdh;
+	sdpcmd_regs_t *regs = bus->regs;
+	uint retries = 0;
+	uint16 lastrbc;
+	uint8 hi, lo;
+	int err;
+
+	DHD_ERROR(("%s: %sterminate frame%s\n", __FUNCTION__,
+	           (abort ? "abort command, " : ""), (rtx ? ", send NAK" : "")));
+
+	if (abort) {
+		bcmsdh_abort(sdh, SDIO_FUNC_2);
+	}
+
+	bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_FRAMECTRL, SFC_RF_TERM, &err);
+	bus->f1regdata++;
+
+	/* Wait until the packet has been flushed (device/FIFO stable) */
+	for (lastrbc = retries = 0xffff; retries > 0; retries--) {
+		hi = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_RFRAMEBCHI, NULL);
+		lo = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_RFRAMEBCLO, NULL);
+		bus->f1regdata += 2;
+
+		if ((hi == 0) && (lo == 0))
+			break;
+
+		if ((hi > (lastrbc >> 8)) && (lo > (lastrbc & 0x00ff))) {
+			DHD_ERROR(("%s: count growing: last 0x%04x now 0x%04x\n",
+			           __FUNCTION__, lastrbc, ((hi << 8) + lo)));
+		}
+		lastrbc = (hi << 8) + lo;
+	}
+
+	if (!retries) {
+		DHD_ERROR(("%s: count never zeroed: last 0x%04x\n", __FUNCTION__, lastrbc));
+	} else {
+		DHD_INFO(("%s: flush took %d iterations\n", __FUNCTION__, (0xffff - retries)));
+	}
+
+	if (rtx) {
+		bus->rxrtx++;
+		W_SDREG(SMB_NAK, &regs->tosbmailbox, retries);
+		bus->f1regdata++;
+		if (retries <= retry_limit) {
+			bus->rxskip = TRUE;
+		}
+	}
+
+	/* Clear partial in any case */
+	bus->nextlen = 0;
+
+	/* If we can't reach the device, signal failure */
+	if (err || bcmsdh_regfail(sdh))
+		bus->dhd->busstate = DHD_BUS_DOWN;
+}
+
+static void
+dhdsdio_read_control(dhd_bus_t *bus, uint8 *hdr, uint len, uint doff)
+{
+	bcmsdh_info_t *sdh = bus->sdh;
+	uint rdlen, pad;
+
+	int sdret;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	/* Control data already received in aligned rxctl */
+	if ((bus->bus == SPI_BUS) && (!bus->usebufpool))
+		goto gotpkt;
+
+	ASSERT(bus->rxbuf);
+	/* Set rxctl for frame (w/optional alignment) */
+	bus->rxctl = bus->rxbuf;
+	if (dhd_alignctl) {
+		bus->rxctl += firstread;
+		if ((pad = ((uintptr)bus->rxctl % DHD_SDALIGN)))
+			bus->rxctl += (DHD_SDALIGN - pad);
+		bus->rxctl -= firstread;
+	}
+	ASSERT(bus->rxctl >= bus->rxbuf);
+
+	/* Copy the already-read portion over */
+	bcopy(hdr, bus->rxctl, firstread);
+	if (len <= firstread)
+		goto gotpkt;
+
+	/* Copy the full data pkt in gSPI case and process ioctl. */
+	if (bus->bus == SPI_BUS) {
+		bcopy(hdr, bus->rxctl, len);
+		goto gotpkt;
+	}
+
+	/* Raise rdlen to next SDIO block to avoid tail command */
+	rdlen = len - firstread;
+	if (bus->roundup && bus->blocksize && (rdlen > bus->blocksize)) {
+		pad = bus->blocksize - (rdlen % bus->blocksize);
+		if ((pad <= bus->roundup) && (pad < bus->blocksize) &&
+		    ((len + pad) < bus->dhd->maxctl))
+			rdlen += pad;
+	} else if (rdlen % DHD_SDALIGN) {
+		rdlen += DHD_SDALIGN - (rdlen % DHD_SDALIGN);
+	}
+
+	/* Satisfy length-alignment requirements */
+	if (forcealign && (rdlen & (ALIGNMENT - 1)))
+		rdlen = ROUNDUP(rdlen, ALIGNMENT);
+
+	/* Drop if the read is too big or it exceeds our maximum */
+	if ((rdlen + firstread) > bus->dhd->maxctl) {
+		DHD_ERROR(("%s: %d-byte control read exceeds %d-byte buffer\n",
+		           __FUNCTION__, rdlen, bus->dhd->maxctl));
+		bus->dhd->rx_errors++;
+		dhdsdio_rxfail(bus, FALSE, FALSE);
+		goto done;
+	}
+
+	if ((len - doff) > bus->dhd->maxctl) {
+		DHD_ERROR(("%s: %d-byte ctl frame (%d-byte ctl data) exceeds %d-byte limit\n",
+		           __FUNCTION__, len, (len - doff), bus->dhd->maxctl));
+		bus->dhd->rx_errors++; bus->rx_toolong++;
+		dhdsdio_rxfail(bus, FALSE, FALSE);
+		goto done;
+	}
+
+
+	/* Read remainder of frame body into the rxctl buffer */
+	sdret = dhd_bcmsdh_recv_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
+	                            (bus->rxctl + firstread), rdlen, NULL, NULL, NULL);
+	bus->f2rxdata++;
+	ASSERT(sdret != BCME_PENDING);
+
+	/* Control frame failures need retransmission */
+	if (sdret < 0) {
+		DHD_ERROR(("%s: read %d control bytes failed: %d\n", __FUNCTION__, rdlen, sdret));
+		bus->rxc_errors++; /* dhd.rx_ctlerrs is higher level */
+		dhdsdio_rxfail(bus, TRUE, TRUE);
+		goto done;
+	}
+
+gotpkt:
+
+#ifdef DHD_DEBUG
+	if (DHD_BYTES_ON() && DHD_CTL_ON()) {
+		prhex("RxCtrl", bus->rxctl, len);
+	}
+#endif
+
+	/* Point to valid data and indicate its length */
+	bus->rxctl += doff;
+	bus->rxlen = len - doff;
+
+done:
+	/* Awake any waiters */
+	dhd_os_ioctl_resp_wake(bus->dhd);
+}
+
+static uint8
+dhdsdio_rxglom(dhd_bus_t *bus, uint8 rxseq)
+{
+	uint16 dlen, totlen;
+	uint8 *dptr, num = 0;
+
+	uint16 sublen, check;
+	void *pfirst, *plast, *pnext, *save_pfirst;
+	osl_t *osh = bus->dhd->osh;
+
+	int errcode;
+	uint8 chan, seq, doff, sfdoff;
+	uint8 txmax;
+
+	int ifidx = 0;
+	bool usechain = bus->use_rxchain;
+
+	/* If packets, issue read(s) and send up packet chain */
+	/* Return sequence numbers consumed? */
+
+	DHD_TRACE(("dhdsdio_rxglom: start: glomd %p glom %p\n", bus->glomd, bus->glom));
+
+	/* If there's a descriptor, generate the packet chain */
+	if (bus->glomd) {
+		dhd_os_sdlock_rxq(bus->dhd);
+
+		pfirst = plast = pnext = NULL;
+		dlen = (uint16)PKTLEN(osh, bus->glomd);
+		dptr = PKTDATA(osh, bus->glomd);
+		if (!dlen || (dlen & 1)) {
+			DHD_ERROR(("%s: bad glomd len (%d), ignore descriptor\n",
+			           __FUNCTION__, dlen));
+			dlen = 0;
+		}
+
+		for (totlen = num = 0; dlen; num++) {
+			/* Get (and move past) next length */
+			sublen = ltoh16_ua(dptr);
+			dlen -= sizeof(uint16);
+			dptr += sizeof(uint16);
+			if ((sublen < SDPCM_HDRLEN) ||
+			    ((num == 0) && (sublen < (2 * SDPCM_HDRLEN)))) {
+				DHD_ERROR(("%s: descriptor len %d bad: %d\n",
+				           __FUNCTION__, num, sublen));
+				pnext = NULL;
+				break;
+			}
+			if (sublen % DHD_SDALIGN) {
+				DHD_ERROR(("%s: sublen %d not a multiple of %d\n",
+				           __FUNCTION__, sublen, DHD_SDALIGN));
+				usechain = FALSE;
+			}
+			totlen += sublen;
+
+			/* For last frame, adjust read len so total is a block multiple */
+			if (!dlen) {
+				sublen += (ROUNDUP(totlen, bus->blocksize) - totlen);
+				totlen = ROUNDUP(totlen, bus->blocksize);
+			}
+
+			/* Allocate/chain packet for next subframe */
+			if ((pnext = PKTGET(osh, sublen + DHD_SDALIGN, FALSE)) == NULL) {
+				DHD_ERROR(("%s: PKTGET failed, num %d len %d\n",
+				           __FUNCTION__, num, sublen));
+				break;
+			}
+			ASSERT(!PKTLINK(pnext));
+			if (!pfirst) {
+				ASSERT(!plast);
+				pfirst = plast = pnext;
+			} else {
+				ASSERT(plast);
+				PKTSETNEXT(osh, plast, pnext);
+				plast = pnext;
+			}
+
+			/* Adhere to start alignment requirements */
+			PKTALIGN(osh, pnext, sublen, DHD_SDALIGN);
+		}
+
+		/* If all allocations succeeded, save packet chain in bus structure */
+		if (pnext) {
+			DHD_GLOM(("%s: allocated %d-byte packet chain for %d subframes\n",
+			          __FUNCTION__, totlen, num));
+			if (DHD_GLOM_ON() && bus->nextlen) {
+				if (totlen != bus->nextlen) {
+					DHD_GLOM(("%s: glomdesc mismatch: nextlen %d glomdesc %d "
+					          "rxseq %d\n", __FUNCTION__, bus->nextlen,
+					          totlen, rxseq));
+				}
+			}
+			bus->glom = pfirst;
+			pfirst = pnext = NULL;
+		} else {
+			if (pfirst)
+				PKTFREE(osh, pfirst, FALSE);
+			bus->glom = NULL;
+			num = 0;
+		}
+
+		/* Done with descriptor packet */
+		PKTFREE(osh, bus->glomd, FALSE);
+		bus->glomd = NULL;
+		bus->nextlen = 0;
+
+		dhd_os_sdunlock_rxq(bus->dhd);
+	}
+
+	/* Ok -- either we just generated a packet chain, or had one from before */
+	if (bus->glom) {
+		if (DHD_GLOM_ON()) {
+			DHD_GLOM(("%s: attempt superframe read, packet chain:\n", __FUNCTION__));
+			for (pnext = bus->glom; pnext; pnext = PKTNEXT(osh, pnext)) {
+				DHD_GLOM(("    %p: %p len 0x%04x (%d)\n",
+				          pnext, (uint8*)PKTDATA(osh, pnext),
+				          PKTLEN(osh, pnext), PKTLEN(osh, pnext)));
+			}
+		}
+
+		pfirst = bus->glom;
+		dlen = (uint16)pkttotlen(osh, pfirst);
+
+		/* Do an SDIO read for the superframe.  Configurable iovar to
+		 * read directly into the chained packet, or allocate a large
+		 * packet and and copy into the chain.
+		 */
+		if (usechain) {
+			errcode = dhd_bcmsdh_recv_buf(bus,
+			                              bcmsdh_cur_sbwad(bus->sdh), SDIO_FUNC_2,
+			                              F2SYNC, (uint8*)PKTDATA(osh, pfirst),
+			                              dlen, pfirst, NULL, NULL);
+		} else if (bus->dataptr) {
+			errcode = dhd_bcmsdh_recv_buf(bus,
+			                              bcmsdh_cur_sbwad(bus->sdh), SDIO_FUNC_2,
+			                              F2SYNC, bus->dataptr,
+			                              dlen, NULL, NULL, NULL);
+			sublen = (uint16)pktfrombuf(osh, pfirst, 0, dlen, bus->dataptr);
+			if (sublen != dlen) {
+				DHD_ERROR(("%s: FAILED TO COPY, dlen %d sublen %d\n",
+				           __FUNCTION__, dlen, sublen));
+				errcode = -1;
+			}
+			pnext = NULL;
+		} else {
+			DHD_ERROR(("COULDN'T ALLOC %d-BYTE GLOM, FORCE FAILURE\n", dlen));
+			errcode = -1;
+		}
+		bus->f2rxdata++;
+		ASSERT(errcode != BCME_PENDING);
+
+		/* On failure, kill the superframe, allow a couple retries */
+		if (errcode < 0) {
+			DHD_ERROR(("%s: glom read of %d bytes failed: %d\n",
+			           __FUNCTION__, dlen, errcode));
+			bus->dhd->rx_errors++;
+
+			if (bus->glomerr++ < 3) {
+				dhdsdio_rxfail(bus, TRUE, TRUE);
+			} else {
+				bus->glomerr = 0;
+				dhdsdio_rxfail(bus, TRUE, FALSE);
+				dhd_os_sdlock_rxq(bus->dhd);
+				PKTFREE(osh, bus->glom, FALSE);
+				dhd_os_sdunlock_rxq(bus->dhd);
+				bus->rxglomfail++;
+				bus->glom = NULL;
+			}
+			return 0;
+		}
+
+#ifdef DHD_DEBUG
+		if (DHD_GLOM_ON()) {
+			prhex("SUPERFRAME", PKTDATA(osh, pfirst),
+			      MIN(PKTLEN(osh, pfirst), 48));
+		}
+#endif
+
+
+		/* Validate the superframe header */
+		dptr = (uint8 *)PKTDATA(osh, pfirst);
+		sublen = ltoh16_ua(dptr);
+		check = ltoh16_ua(dptr + sizeof(uint16));
+
+		chan = SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN]);
+		seq = SDPCM_PACKET_SEQUENCE(&dptr[SDPCM_FRAMETAG_LEN]);
+		bus->nextlen = dptr[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
+		if ((bus->nextlen << 4) > MAX_RX_DATASZ) {
+			DHD_INFO(("%s: got frame w/nextlen too large (%d) seq %d\n",
+			          __FUNCTION__, bus->nextlen, seq));
+			bus->nextlen = 0;
+		}
+		doff = SDPCM_DOFFSET_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
+		txmax = SDPCM_WINDOW_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
+
+		errcode = 0;
+		if ((uint16)~(sublen^check)) {
+			DHD_ERROR(("%s (superframe): HW hdr error: len/check 0x%04x/0x%04x\n",
+			           __FUNCTION__, sublen, check));
+			errcode = -1;
+		} else if (ROUNDUP(sublen, bus->blocksize) != dlen) {
+			DHD_ERROR(("%s (superframe): len 0x%04x, rounded 0x%04x, expect 0x%04x\n",
+			           __FUNCTION__, sublen, ROUNDUP(sublen, bus->blocksize), dlen));
+			errcode = -1;
+		} else if (SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN]) != SDPCM_GLOM_CHANNEL) {
+			DHD_ERROR(("%s (superframe): bad channel %d\n", __FUNCTION__,
+			           SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN])));
+			errcode = -1;
+		} else if (SDPCM_GLOMDESC(&dptr[SDPCM_FRAMETAG_LEN])) {
+			DHD_ERROR(("%s (superframe): got second descriptor?\n", __FUNCTION__));
+			errcode = -1;
+		} else if ((doff < SDPCM_HDRLEN) ||
+		           (doff > (PKTLEN(osh, pfirst) - SDPCM_HDRLEN))) {
+			DHD_ERROR(("%s (superframe): Bad data offset %d: HW %d pkt %d min %d\n",
+			           __FUNCTION__, doff, sublen, PKTLEN(osh, pfirst), SDPCM_HDRLEN));
+			errcode = -1;
+		}
+
+		/* Check sequence number of superframe SW header */
+		if (rxseq != seq) {
+			DHD_INFO(("%s: (superframe) rx_seq %d, expected %d\n",
+			          __FUNCTION__, seq, rxseq));
+			bus->rx_badseq++;
+			rxseq = seq;
+		}
+
+		/* Check window for sanity */
+		if ((uint8)(txmax - bus->tx_seq) > 0x40) {
+			DHD_ERROR(("%s: got unlikely tx max %d with tx_seq %d\n",
+			           __FUNCTION__, txmax, bus->tx_seq));
+			txmax = bus->tx_seq + 2;
+		}
+		bus->tx_max = txmax;
+
+		/* Remove superframe header, remember offset */
+		PKTPULL(osh, pfirst, doff);
+		sfdoff = doff;
+
+		/* Validate all the subframe headers */
+		for (num = 0, pnext = pfirst; pnext && !errcode;
+		     num++, pnext = PKTNEXT(osh, pnext)) {
+			dptr = (uint8 *)PKTDATA(osh, pnext);
+			dlen = (uint16)PKTLEN(osh, pnext);
+			sublen = ltoh16_ua(dptr);
+			check = ltoh16_ua(dptr + sizeof(uint16));
+			chan = SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN]);
+			doff = SDPCM_DOFFSET_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
+#ifdef DHD_DEBUG
+			if (DHD_GLOM_ON()) {
+				prhex("subframe", dptr, 32);
+			}
+#endif
+
+			if ((uint16)~(sublen^check)) {
+				DHD_ERROR(("%s (subframe %d): HW hdr error: "
+				           "len/check 0x%04x/0x%04x\n",
+				           __FUNCTION__, num, sublen, check));
+				errcode = -1;
+			} else if ((sublen > dlen) || (sublen < SDPCM_HDRLEN)) {
+				DHD_ERROR(("%s (subframe %d): length mismatch: "
+				           "len 0x%04x, expect 0x%04x\n",
+				           __FUNCTION__, num, sublen, dlen));
+				errcode = -1;
+			} else if ((chan != SDPCM_DATA_CHANNEL) &&
+			           (chan != SDPCM_EVENT_CHANNEL)) {
+				DHD_ERROR(("%s (subframe %d): bad channel %d\n",
+				           __FUNCTION__, num, chan));
+				errcode = -1;
+			} else if ((doff < SDPCM_HDRLEN) || (doff > sublen)) {
+				DHD_ERROR(("%s (subframe %d): Bad data offset %d: HW %d min %d\n",
+				           __FUNCTION__, num, doff, sublen, SDPCM_HDRLEN));
+				errcode = -1;
+			}
+		}
+
+		if (errcode) {
+			/* Terminate frame on error, request a couple retries */
+			if (bus->glomerr++ < 3) {
+				/* Restore superframe header space */
+				PKTPUSH(osh, pfirst, sfdoff);
+				dhdsdio_rxfail(bus, TRUE, TRUE);
+			} else {
+				bus->glomerr = 0;
+				dhdsdio_rxfail(bus, TRUE, FALSE);
+				dhd_os_sdlock_rxq(bus->dhd);
+				PKTFREE(osh, bus->glom, FALSE);
+				dhd_os_sdunlock_rxq(bus->dhd);
+				bus->rxglomfail++;
+				bus->glom = NULL;
+			}
+			bus->nextlen = 0;
+			return 0;
+		}
+
+		/* Basic SD framing looks ok - process each packet (header) */
+		save_pfirst = pfirst;
+		bus->glom = NULL;
+		plast = NULL;
+
+		dhd_os_sdlock_rxq(bus->dhd);
+		for (num = 0; pfirst; rxseq++, pfirst = pnext) {
+			pnext = PKTNEXT(osh, pfirst);
+			PKTSETNEXT(osh, pfirst, NULL);
+
+			dptr = (uint8 *)PKTDATA(osh, pfirst);
+			sublen = ltoh16_ua(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]);
+
+			DHD_GLOM(("%s: Get subframe %d, %p(%p/%d), sublen %d chan %d seq %d\n",
+			          __FUNCTION__, num, pfirst, PKTDATA(osh, pfirst),
+			          PKTLEN(osh, pfirst), sublen, chan, seq));
+
+			ASSERT((chan == SDPCM_DATA_CHANNEL) || (chan == SDPCM_EVENT_CHANNEL));
+
+			if (rxseq != seq) {
+				DHD_GLOM(("%s: rx_seq %d, expected %d\n",
+				          __FUNCTION__, seq, rxseq));
+				bus->rx_badseq++;
+				rxseq = seq;
+			}
+
+#ifdef DHD_DEBUG
+			if (DHD_BYTES_ON() && DHD_DATA_ON()) {
+				prhex("Rx Subframe Data", dptr, dlen);
+			}
+#endif
+
+			PKTSETLEN(osh, pfirst, sublen);
+			PKTPULL(osh, pfirst, doff);
+
+			if (PKTLEN(osh, pfirst) == 0) {
+				PKTFREE(bus->dhd->osh, pfirst, FALSE);
+				if (plast) {
+					PKTSETNEXT(osh, plast, pnext);
+				} else {
+					ASSERT(save_pfirst == pfirst);
+					save_pfirst = pnext;
+				}
+				continue;
+			} else if (dhd_prot_hdrpull(bus->dhd, &ifidx, pfirst) != 0) {
+				DHD_ERROR(("%s: rx protocol error\n", __FUNCTION__));
+				bus->dhd->rx_errors++;
+				PKTFREE(osh, pfirst, FALSE);
+				if (plast) {
+					PKTSETNEXT(osh, plast, pnext);
+				} else {
+					ASSERT(save_pfirst == pfirst);
+					save_pfirst = pnext;
+				}
+				continue;
+			}
+
+			/* this packet will go up, link back into chain and count it */
+			PKTSETNEXT(osh, pfirst, pnext);
+			plast = pfirst;
+			num++;
+
+#ifdef DHD_DEBUG
+			if (DHD_GLOM_ON()) {
+				DHD_GLOM(("%s subframe %d to stack, %p(%p/%d) nxt/lnk %p/%p\n",
+				          __FUNCTION__, num, pfirst,
+				          PKTDATA(osh, pfirst), PKTLEN(osh, pfirst),
+				          PKTNEXT(osh, pfirst), PKTLINK(pfirst)));
+				prhex("", (uint8 *)PKTDATA(osh, pfirst),
+				      MIN(PKTLEN(osh, pfirst), 32));
+			}
+#endif /* DHD_DEBUG */
+		}
+		dhd_os_sdunlock_rxq(bus->dhd);
+		if (num) {
+			dhd_os_sdunlock(bus->dhd);
+			dhd_rx_frame(bus->dhd, ifidx, save_pfirst, num);
+			dhd_os_sdlock(bus->dhd);
+		}
+
+		bus->rxglomframes++;
+		bus->rxglompkts += num;
+	}
+	return num;
+}
+
+/* Return TRUE if there may be more frames to read */
+static uint
+dhdsdio_readframes(dhd_bus_t *bus, uint maxframes, bool *finished)
+{
+	osl_t *osh = bus->dhd->osh;
+	bcmsdh_info_t *sdh = bus->sdh;
+
+	uint16 len, check;	/* Extracted hardware header fields */
+	uint8 chan, seq, doff;	/* Extracted software header fields */
+	uint8 fcbits;		/* Extracted fcbits from software header */
+	uint8 delta;
+
+	void *pkt;	/* Packet for event or data frames */
+	uint16 pad;	/* Number of pad bytes to read */
+	uint16 rdlen;	/* Total number of bytes to read */
+	uint8 rxseq;	/* Next sequence number to expect */
+	uint rxleft = 0;	/* Remaining number of frames allowed */
+	int sdret;	/* Return code from bcmsdh calls */
+	uint8 txmax;	/* Maximum tx sequence offered */
+	bool len_consistent; /* Result of comparing readahead len and len from hw-hdr */
+	uint8 *rxbuf;
+	int ifidx = 0;
+	uint rxcount = 0; /* Total frames read */
+
+#if defined(DHD_DEBUG) || defined(SDTEST)
+	bool sdtest = FALSE;	/* To limit message spew from test mode */
+#endif
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	ASSERT(maxframes);
+
+#ifdef SDTEST
+	/* Allow pktgen to override maxframes */
+	if (bus->pktgen_count && (bus->pktgen_mode == DHD_PKTGEN_RECV)) {
+		maxframes = bus->pktgen_count;
+		sdtest = TRUE;
+	}
+#endif
+
+	/* Not finished unless we encounter no more frames indication */
+	*finished = FALSE;
+
+
+	for (rxseq = bus->rx_seq, rxleft = maxframes;
+	     !bus->rxskip && rxleft && bus->dhd->busstate != DHD_BUS_DOWN;
+	     rxseq++, rxleft--) {
+
+		/* Handle glomming separately */
+		if (bus->glom || bus->glomd) {
+			uint8 cnt;
+			DHD_GLOM(("%s: calling rxglom: glomd %p, glom %p\n",
+			          __FUNCTION__, bus->glomd, bus->glom));
+			cnt = dhdsdio_rxglom(bus, rxseq);
+			DHD_GLOM(("%s: rxglom returned %d\n", __FUNCTION__, cnt));
+			rxseq += cnt - 1;
+			rxleft = (rxleft > cnt) ? (rxleft - cnt) : 1;
+			continue;
+		}
+
+		/* Try doing single read if we can */
+		if (dhd_readahead && bus->nextlen) {
+			uint16 nextlen = bus->nextlen;
+			bus->nextlen = 0;
+
+			if (bus->bus == SPI_BUS) {
+				rdlen = len = nextlen;
+			}
+			else {
+				rdlen = len = nextlen << 4;
+
+				/* Pad read to blocksize for efficiency */
+				if (bus->roundup && bus->blocksize && (rdlen > bus->blocksize)) {
+					pad = bus->blocksize - (rdlen % bus->blocksize);
+					if ((pad <= bus->roundup) && (pad < bus->blocksize) &&
+						((rdlen + pad + firstread) < MAX_RX_DATASZ))
+						rdlen += pad;
+				} else if (rdlen % DHD_SDALIGN) {
+					rdlen += DHD_SDALIGN - (rdlen % DHD_SDALIGN);
+				}
+			}
+
+			/* We use bus->rxctl buffer in WinXP for initial control pkt receives.
+			 * Later we use buffer-poll for data as well as control packets.
+			 * This is required becuase dhd receives full frame in gSPI unlike SDIO.
+			 * After the frame is received we have to distinguish whether it is data
+			 * or non-data frame.
+			 */
+			/* Allocate a packet buffer */
+			dhd_os_sdlock_rxq(bus->dhd);
+			if (!(pkt = PKTGET(osh, rdlen + DHD_SDALIGN, FALSE))) {
+				if (bus->bus == SPI_BUS) {
+					bus->usebufpool = FALSE;
+					bus->rxctl = bus->rxbuf;
+					if (dhd_alignctl) {
+						bus->rxctl += firstread;
+						if ((pad = ((uintptr)bus->rxctl % DHD_SDALIGN)))
+							bus->rxctl += (DHD_SDALIGN - pad);
+						bus->rxctl -= firstread;
+					}
+					ASSERT(bus->rxctl >= bus->rxbuf);
+					rxbuf = bus->rxctl;
+					/* Read the entire frame */
+					sdret = dhd_bcmsdh_recv_buf(bus,
+					                            bcmsdh_cur_sbwad(sdh),
+					                            SDIO_FUNC_2,
+					                            F2SYNC, rxbuf, rdlen,
+					                            NULL, NULL, NULL);
+					bus->f2rxdata++;
+					ASSERT(sdret != BCME_PENDING);
+
+
+					/* Control frame failures need retransmission */
+					if (sdret < 0) {
+						DHD_ERROR(("%s: read %d control bytes failed: %d\n",
+						   __FUNCTION__, rdlen, sdret));
+						/* dhd.rx_ctlerrs is higher level */
+						bus->rxc_errors++;
+						dhd_os_sdunlock_rxq(bus->dhd);
+						dhdsdio_rxfail(bus, TRUE,
+						    (bus->bus == SPI_BUS) ? FALSE : TRUE);
+						continue;
+					}
+				} else {
+					/* Give up on data, request rtx of events */
+					DHD_ERROR(("%s (nextlen): PKTGET failed: len %d rdlen %d "
+					           "expected rxseq %d\n",
+					           __FUNCTION__, len, rdlen, rxseq));
+					/* Just go try again w/normal header read */
+					dhd_os_sdunlock_rxq(bus->dhd);
+					continue;
+				}
+			} else {
+				if (bus->bus == SPI_BUS)
+					bus->usebufpool = TRUE;
+
+				ASSERT(!PKTLINK(pkt));
+				PKTALIGN(osh, pkt, rdlen, DHD_SDALIGN);
+				rxbuf = (uint8 *)PKTDATA(osh, pkt);
+				/* Read the entire frame */
+				sdret = dhd_bcmsdh_recv_buf(bus, bcmsdh_cur_sbwad(sdh),
+				                            SDIO_FUNC_2,
+				                            F2SYNC, rxbuf, rdlen,
+				                            pkt, NULL, NULL);
+				bus->f2rxdata++;
+				ASSERT(sdret != BCME_PENDING);
+
+				if (sdret < 0) {
+					DHD_ERROR(("%s (nextlen): read %d bytes failed: %d\n",
+					   __FUNCTION__, rdlen, sdret));
+					PKTFREE(bus->dhd->osh, pkt, FALSE);
+					bus->dhd->rx_errors++;
+					dhd_os_sdunlock_rxq(bus->dhd);
+					/* Force retry w/normal header read.  Don't attemp NAK for
+					 * gSPI
+					 */
+					dhdsdio_rxfail(bus, TRUE,
+					      (bus->bus == SPI_BUS) ? FALSE : TRUE);
+					continue;
+				}
+			}
+			dhd_os_sdunlock_rxq(bus->dhd);
+
+			/* Now check the header */
+			bcopy(rxbuf, bus->rxhdr, SDPCM_HDRLEN);
+
+			/* Extract hardware header fields */
+			len = ltoh16_ua(bus->rxhdr);
+			check = ltoh16_ua(bus->rxhdr + sizeof(uint16));
+
+			/* All zeros means readahead info was bad */
+			if (!(len|check)) {
+				DHD_INFO(("%s (nextlen): read zeros in HW header???\n",
+				           __FUNCTION__));
+				dhd_os_sdlock_rxq(bus->dhd);
+				PKTFREE2();
+				dhd_os_sdunlock_rxq(bus->dhd);
+				GSPI_PR55150_BAILOUT;
+				continue;
+			}
+
+			/* Validate check bytes */
+			if ((uint16)~(len^check)) {
+				DHD_ERROR(("%s (nextlen): HW hdr error: nextlen/len/check"
+				           " 0x%04x/0x%04x/0x%04x\n", __FUNCTION__, nextlen,
+				           len, check));
+				dhd_os_sdlock_rxq(bus->dhd);
+				PKTFREE2();
+				dhd_os_sdunlock_rxq(bus->dhd);
+				bus->rx_badhdr++;
+				dhdsdio_rxfail(bus, FALSE, FALSE);
+				GSPI_PR55150_BAILOUT;
+				continue;
+			}
+
+			/* Validate frame length */
+			if (len < SDPCM_HDRLEN) {
+				DHD_ERROR(("%s (nextlen): HW hdr length invalid: %d\n",
+				           __FUNCTION__, len));
+				dhd_os_sdlock_rxq(bus->dhd);
+				PKTFREE2();
+				dhd_os_sdunlock_rxq(bus->dhd);
+				GSPI_PR55150_BAILOUT;
+				continue;
+			}
+
+			/* Check for consistency with readahead info */
+				len_consistent = (nextlen != (ROUNDUP(len, 16) >> 4));
+			if (len_consistent) {
+				/* Mismatch, force retry w/normal header (may be >4K) */
+				DHD_ERROR(("%s (nextlen): mismatch, nextlen %d len %d rnd %d; "
+				           "expected rxseq %d\n",
+				           __FUNCTION__, nextlen, len, ROUNDUP(len, 16), rxseq));
+				dhd_os_sdlock_rxq(bus->dhd);
+				PKTFREE2();
+				dhd_os_sdunlock_rxq(bus->dhd);
+				dhdsdio_rxfail(bus, TRUE, (bus->bus == SPI_BUS) ? FALSE : TRUE);
+				GSPI_PR55150_BAILOUT;
+				continue;
+			}
+
+
+			/* Extract software header fields */
+			chan = SDPCM_PACKET_CHANNEL(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+			seq = SDPCM_PACKET_SEQUENCE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+			doff = SDPCM_DOFFSET_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+			txmax = SDPCM_WINDOW_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+
+				bus->nextlen =
+				         bus->rxhdr[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
+				if ((bus->nextlen << 4) > MAX_RX_DATASZ) {
+					DHD_INFO(("%s (nextlen): got frame w/nextlen too large"
+					          " (%d), seq %d\n", __FUNCTION__, bus->nextlen,
+					          seq));
+					bus->nextlen = 0;
+				}
+
+				bus->dhd->rx_readahead_cnt ++;
+			/* Handle Flow Control */
+			fcbits = SDPCM_FCMASK_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+
+			delta = 0;
+			if (~bus->flowcontrol & fcbits) {
+				bus->fc_xoff++;
+				delta = 1;
+			}
+			if (bus->flowcontrol & ~fcbits) {
+				bus->fc_xon++;
+				delta = 1;
+			}
+
+			if (delta) {
+				bus->fc_rcvd++;
+				bus->flowcontrol = fcbits;
+			}
+
+			/* Check and update sequence number */
+			if (rxseq != seq) {
+				DHD_INFO(("%s (nextlen): rx_seq %d, expected %d\n",
+				          __FUNCTION__, seq, rxseq));
+				bus->rx_badseq++;
+				rxseq = seq;
+			}
+
+			/* Check window for sanity */
+			if ((uint8)(txmax - bus->tx_seq) > 0x40) {
+					DHD_ERROR(("%s: got unlikely tx max %d with tx_seq %d\n",
+						__FUNCTION__, txmax, bus->tx_seq));
+					txmax = bus->tx_seq + 2;
+			}
+			bus->tx_max = txmax;
+
+#ifdef DHD_DEBUG
+			if (DHD_BYTES_ON() && DHD_DATA_ON()) {
+				prhex("Rx Data", rxbuf, len);
+			} else if (DHD_HDRS_ON()) {
+				prhex("RxHdr", bus->rxhdr, SDPCM_HDRLEN);
+			}
+#endif
+
+			if (chan == SDPCM_CONTROL_CHANNEL) {
+				if (bus->bus == SPI_BUS) {
+					dhdsdio_read_control(bus, rxbuf, len, doff);
+					if (bus->usebufpool) {
+						dhd_os_sdlock_rxq(bus->dhd);
+						PKTFREE(bus->dhd->osh, pkt, FALSE);
+						dhd_os_sdunlock_rxq(bus->dhd);
+					}
+					continue;
+				} else {
+					DHD_ERROR(("%s (nextlen): readahead on control"
+					           " packet %d?\n", __FUNCTION__, seq));
+					/* Force retry w/normal header read */
+					bus->nextlen = 0;
+					dhdsdio_rxfail(bus, FALSE, TRUE);
+					dhd_os_sdlock_rxq(bus->dhd);
+					PKTFREE2();
+					dhd_os_sdunlock_rxq(bus->dhd);
+					continue;
+				}
+			}
+
+			if ((bus->bus == SPI_BUS) && !bus->usebufpool) {
+				DHD_ERROR(("Received %d bytes on %d channel. Running out of "
+				           "rx pktbuf's or not yet malloced.\n", len, chan));
+				continue;
+			}
+
+			/* Validate data offset */
+			if ((doff < SDPCM_HDRLEN) || (doff > len)) {
+				DHD_ERROR(("%s (nextlen): bad data offset %d: HW len %d min %d\n",
+				           __FUNCTION__, doff, len, SDPCM_HDRLEN));
+				dhd_os_sdlock_rxq(bus->dhd);
+				PKTFREE2();
+				dhd_os_sdunlock_rxq(bus->dhd);
+				ASSERT(0);
+				dhdsdio_rxfail(bus, FALSE, FALSE);
+				continue;
+			}
+
+			/* All done with this one -- now deliver the packet */
+			goto deliver;
+		}
+		/* gSPI frames should not be handled in fractions */
+		if (bus->bus == SPI_BUS) {
+			break;
+		}
+
+		/* Read frame header (hardware and software) */
+		sdret = dhd_bcmsdh_recv_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
+		                            bus->rxhdr, firstread, NULL, NULL, NULL);
+		bus->f2rxhdrs++;
+		ASSERT(sdret != BCME_PENDING);
+
+		if (sdret < 0) {
+			DHD_ERROR(("%s: RXHEADER FAILED: %d\n", __FUNCTION__, sdret));
+			bus->rx_hdrfail++;
+			dhdsdio_rxfail(bus, TRUE, TRUE);
+			continue;
+		}
+
+#ifdef DHD_DEBUG
+		if (DHD_BYTES_ON() || DHD_HDRS_ON()) {
+			prhex("RxHdr", bus->rxhdr, SDPCM_HDRLEN);
+		}
+#endif
+
+		/* Extract hardware header fields */
+		len = ltoh16_ua(bus->rxhdr);
+		check = ltoh16_ua(bus->rxhdr + sizeof(uint16));
+
+		/* All zeros means no more frames */
+		if (!(len|check)) {
+			*finished = TRUE;
+			break;
+		}
+
+		/* Validate check bytes */
+		if ((uint16)~(len^check)) {
+			DHD_ERROR(("%s: HW hdr error: len/check 0x%04x/0x%04x\n",
+			           __FUNCTION__, len, check));
+			bus->rx_badhdr++;
+			dhdsdio_rxfail(bus, FALSE, FALSE);
+			continue;
+		}
+
+		/* Validate frame length */
+		if (len < SDPCM_HDRLEN) {
+			DHD_ERROR(("%s: HW hdr length invalid: %d\n", __FUNCTION__, len));
+			continue;
+		}
+
+		/* Extract software header fields */
+		chan = SDPCM_PACKET_CHANNEL(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+		seq = SDPCM_PACKET_SEQUENCE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+		doff = SDPCM_DOFFSET_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+		txmax = SDPCM_WINDOW_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+
+		/* Validate data offset */
+		if ((doff < SDPCM_HDRLEN) || (doff > len)) {
+			DHD_ERROR(("%s: Bad data offset %d: HW len %d, min %d seq %d\n",
+			           __FUNCTION__, doff, len, SDPCM_HDRLEN, seq));
+			bus->rx_badhdr++;
+			ASSERT(0);
+			dhdsdio_rxfail(bus, FALSE, FALSE);
+			continue;
+		}
+
+		/* Save the readahead length if there is one */
+		bus->nextlen = bus->rxhdr[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
+		if ((bus->nextlen << 4) > MAX_RX_DATASZ) {
+			DHD_INFO(("%s (nextlen): got frame w/nextlen too large (%d), seq %d\n",
+			          __FUNCTION__, bus->nextlen, seq));
+			bus->nextlen = 0;
+		}
+
+		/* Handle Flow Control */
+		fcbits = SDPCM_FCMASK_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+
+		delta = 0;
+		if (~bus->flowcontrol & fcbits) {
+			bus->fc_xoff++;
+			delta = 1;
+		}
+		if (bus->flowcontrol & ~fcbits) {
+			bus->fc_xon++;
+			delta = 1;
+		}
+
+		if (delta) {
+			bus->fc_rcvd++;
+			bus->flowcontrol = fcbits;
+		}
+
+		/* Check and update sequence number */
+		if (rxseq != seq) {
+			DHD_INFO(("%s: rx_seq %d, expected %d\n", __FUNCTION__, seq, rxseq));
+			bus->rx_badseq++;
+			rxseq = seq;
+		}
+
+		/* Check window for sanity */
+		if ((uint8)(txmax - bus->tx_seq) > 0x40) {
+			DHD_ERROR(("%s: got unlikely tx max %d with tx_seq %d\n",
+			           __FUNCTION__, txmax, bus->tx_seq));
+			txmax = bus->tx_seq + 2;
+		}
+		bus->tx_max = txmax;
+
+		/* Call a separate function for control frames */
+		if (chan == SDPCM_CONTROL_CHANNEL) {
+			dhdsdio_read_control(bus, bus->rxhdr, len, doff);
+			continue;
+		}
+
+		ASSERT((chan == SDPCM_DATA_CHANNEL) || (chan == SDPCM_EVENT_CHANNEL) ||
+		       (chan == SDPCM_TEST_CHANNEL) || (chan == SDPCM_GLOM_CHANNEL));
+
+		/* Length to read */
+		rdlen = (len > firstread) ? (len - firstread) : 0;
+
+		/* May pad read to blocksize for efficiency */
+		if (bus->roundup && bus->blocksize && (rdlen > bus->blocksize)) {
+			pad = bus->blocksize - (rdlen % bus->blocksize);
+			if ((pad <= bus->roundup) && (pad < bus->blocksize) &&
+			    ((rdlen + pad + firstread) < MAX_RX_DATASZ))
+				rdlen += pad;
+		} else if (rdlen % DHD_SDALIGN) {
+			rdlen += DHD_SDALIGN - (rdlen % DHD_SDALIGN);
+		}
+
+		/* Satisfy length-alignment requirements */
+		if (forcealign && (rdlen & (ALIGNMENT - 1)))
+			rdlen = ROUNDUP(rdlen, ALIGNMENT);
+
+		if ((rdlen + firstread) > MAX_RX_DATASZ) {
+			/* Too long -- skip this frame */
+			DHD_ERROR(("%s: too long: len %d rdlen %d\n", __FUNCTION__, len, rdlen));
+			bus->dhd->rx_errors++; bus->rx_toolong++;
+			dhdsdio_rxfail(bus, FALSE, FALSE);
+			continue;
+		}
+
+		dhd_os_sdlock_rxq(bus->dhd);
+		if (!(pkt = PKTGET(osh, (rdlen + firstread + DHD_SDALIGN), FALSE))) {
+			/* Give up on data, request rtx of events */
+			DHD_ERROR(("%s: PKTGET failed: rdlen %d chan %d\n",
+			           __FUNCTION__, rdlen, chan));
+			bus->dhd->rx_dropped++;
+			dhd_os_sdunlock_rxq(bus->dhd);
+			dhdsdio_rxfail(bus, FALSE, RETRYCHAN(chan));
+			continue;
+		}
+		dhd_os_sdunlock_rxq(bus->dhd);
+
+		ASSERT(!PKTLINK(pkt));
+
+		/* Leave room for what we already read, and align remainder */
+		ASSERT(firstread < (PKTLEN(osh, pkt)));
+		PKTPULL(osh, pkt, firstread);
+		PKTALIGN(osh, pkt, rdlen, DHD_SDALIGN);
+
+		/* Read the remaining frame data */
+		sdret = dhd_bcmsdh_recv_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
+		                            ((uint8 *)PKTDATA(osh, pkt)), rdlen, pkt, NULL, NULL);
+		bus->f2rxdata++;
+		ASSERT(sdret != BCME_PENDING);
+
+		if (sdret < 0) {
+			DHD_ERROR(("%s: read %d %s bytes failed: %d\n", __FUNCTION__, rdlen,
+			           ((chan == SDPCM_EVENT_CHANNEL) ? "event" :
+			            ((chan == SDPCM_DATA_CHANNEL) ? "data" : "test")), sdret));
+			dhd_os_sdlock_rxq(bus->dhd);
+			PKTFREE(bus->dhd->osh, pkt, FALSE);
+			dhd_os_sdunlock_rxq(bus->dhd);
+			bus->dhd->rx_errors++;
+			dhdsdio_rxfail(bus, TRUE, RETRYCHAN(chan));
+			continue;
+		}
+
+		/* Copy the already-read portion */
+		PKTPUSH(osh, pkt, firstread);
+		bcopy(bus->rxhdr, PKTDATA(osh, pkt), firstread);
+
+#ifdef DHD_DEBUG
+		if (DHD_BYTES_ON() && DHD_DATA_ON()) {
+			prhex("Rx Data", PKTDATA(osh, pkt), len);
+		}
+#endif
+
+deliver:
+		/* Save superframe descriptor and allocate packet frame */
+		if (chan == SDPCM_GLOM_CHANNEL) {
+			if (SDPCM_GLOMDESC(&bus->rxhdr[SDPCM_FRAMETAG_LEN])) {
+				DHD_GLOM(("%s: got glom descriptor, %d bytes:\n",
+				          __FUNCTION__, len));
+#ifdef DHD_DEBUG
+				if (DHD_GLOM_ON()) {
+					prhex("Glom Data", PKTDATA(osh, pkt), len);
+				}
+#endif
+				PKTSETLEN(osh, pkt, len);
+				ASSERT(doff == SDPCM_HDRLEN);
+				PKTPULL(osh, pkt, SDPCM_HDRLEN);
+				bus->glomd = pkt;
+			} else {
+				DHD_ERROR(("%s: glom superframe w/o descriptor!\n", __FUNCTION__));
+				dhdsdio_rxfail(bus, FALSE, FALSE);
+			}
+			continue;
+		}
+
+		/* Fill in packet len and prio, deliver upward */
+		PKTSETLEN(osh, pkt, len);
+		PKTPULL(osh, pkt, doff);
+
+#ifdef SDTEST
+		/* Test channel packets are processed separately */
+		if (chan == SDPCM_TEST_CHANNEL) {
+			dhdsdio_testrcv(bus, pkt, seq);
+			continue;
+		}
+#endif /* SDTEST */
+
+		if (PKTLEN(osh, pkt) == 0) {
+			dhd_os_sdlock_rxq(bus->dhd);
+			PKTFREE(bus->dhd->osh, pkt, FALSE);
+			dhd_os_sdunlock_rxq(bus->dhd);
+			continue;
+		} else if (dhd_prot_hdrpull(bus->dhd, &ifidx, pkt) != 0) {
+			DHD_ERROR(("%s: rx protocol error\n", __FUNCTION__));
+			dhd_os_sdlock_rxq(bus->dhd);
+			PKTFREE(bus->dhd->osh, pkt, FALSE);
+			dhd_os_sdunlock_rxq(bus->dhd);
+			bus->dhd->rx_errors++;
+			continue;
+		}
+
+
+		/* Unlock during rx call */
+		dhd_os_sdunlock(bus->dhd);
+		dhd_rx_frame(bus->dhd, ifidx, pkt, 1);
+		dhd_os_sdlock(bus->dhd);
+	}
+	rxcount = maxframes - rxleft;
+#ifdef DHD_DEBUG
+	/* Message if we hit the limit */
+	if (!rxleft && !sdtest)
+		DHD_DATA(("%s: hit rx limit of %d frames\n", __FUNCTION__, maxframes));
+	else
+#endif /* DHD_DEBUG */
+	DHD_DATA(("%s: processed %d frames\n", __FUNCTION__, rxcount));
+	/* Back off rxseq if awaiting rtx, update rx_seq */
+	if (bus->rxskip)
+		rxseq--;
+	bus->rx_seq = rxseq;
+
+	return rxcount;
+}
+
+static uint32
+dhdsdio_hostmail(dhd_bus_t *bus)
+{
+	sdpcmd_regs_t *regs = bus->regs;
+	uint32 intstatus = 0;
+	uint32 hmb_data;
+	uint8 fcbits;
+	uint retries = 0;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	/* Read mailbox data and ack that we did so */
+	R_SDREG(hmb_data, &regs->tohostmailboxdata, retries);
+	if (retries <= retry_limit)
+		W_SDREG(SMB_INT_ACK, &regs->tosbmailbox, retries);
+	bus->f1regdata += 2;
+
+	/* Dongle recomposed rx frames, accept them again */
+	if (hmb_data & HMB_DATA_NAKHANDLED) {
+		DHD_INFO(("Dongle reports NAK handled, expect rtx of %d\n", bus->rx_seq));
+		if (!bus->rxskip) {
+			DHD_ERROR(("%s: unexpected NAKHANDLED!\n", __FUNCTION__));
+		}
+		bus->rxskip = FALSE;
+		intstatus |= I_HMB_FRAME_IND;
+	}
+
+	/*
+	 * DEVREADY does not occur with gSPI.
+	 */
+	if (hmb_data & (HMB_DATA_DEVREADY | HMB_DATA_FWREADY)) {
+		bus->sdpcm_ver = (hmb_data & HMB_DATA_VERSION_MASK) >> HMB_DATA_VERSION_SHIFT;
+		if (bus->sdpcm_ver != SDPCM_PROT_VERSION)
+			DHD_ERROR(("Version mismatch, dongle reports %d, expecting %d\n",
+			           bus->sdpcm_ver, SDPCM_PROT_VERSION));
+		else
+			DHD_INFO(("Dongle ready, protocol version %d\n", bus->sdpcm_ver));
+	}
+
+	/*
+	 * Flow Control has been moved into the RX headers and this out of band
+	 * method isn't used any more.  Leae this here for possibly remaining backward
+	 * compatible with older dongles
+	 */
+	if (hmb_data & HMB_DATA_FC) {
+		fcbits = (hmb_data & HMB_DATA_FCDATA_MASK) >> HMB_DATA_FCDATA_SHIFT;
+
+		if (fcbits & ~bus->flowcontrol)
+			bus->fc_xoff++;
+		if (bus->flowcontrol & ~fcbits)
+			bus->fc_xon++;
+
+		bus->fc_rcvd++;
+		bus->flowcontrol = fcbits;
+	}
+
+	/* Shouldn't be any others */
+	if (hmb_data & ~(HMB_DATA_DEVREADY |
+	                 HMB_DATA_NAKHANDLED |
+	                 HMB_DATA_FC |
+	                 HMB_DATA_FWREADY |
+	                 HMB_DATA_FCDATA_MASK |
+	                 HMB_DATA_VERSION_MASK)) {
+		DHD_ERROR(("Unknown mailbox data content: 0x%02x\n", hmb_data));
+	}
+
+	return intstatus;
+}
+
+bool
+dhdsdio_dpc(dhd_bus_t *bus)
+{
+	bcmsdh_info_t *sdh = bus->sdh;
+	sdpcmd_regs_t *regs = bus->regs;
+	uint32 intstatus, newstatus = 0;
+	uint retries = 0;
+	uint rxlimit = dhd_rxbound; /* Rx frames to read before resched */
+	uint txlimit = dhd_txbound; /* Tx frames to send before resched */
+	uint framecnt = 0;		  /* Temporary counter of tx/rx frames */
+	bool rxdone = TRUE;		  /* Flag for no more read data */
+	bool resched = FALSE;	  /* Flag indicating resched wanted */
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	/* Start with leftover status bits */
+	intstatus = bus->intstatus;
+
+	dhd_os_sdlock(bus->dhd);
+
+	/* If waiting for HTAVAIL, check status */
+	if (bus->clkstate == CLK_PENDING) {
+		int err;
+		uint8 clkctl, devctl = 0;
+
+#ifdef DHD_DEBUG
+		/* Check for inconsistent device control */
+		devctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, &err);
+		if (err) {
+			DHD_ERROR(("%s: error reading DEVCTL: %d\n", __FUNCTION__, err));
+			bus->dhd->busstate = DHD_BUS_DOWN;
+		} else {
+			ASSERT(devctl & SBSDIO_DEVCTL_CA_INT_ONLY);
+		}
+#endif /* DHD_DEBUG */
+
+		/* Read CSR, if clock on switch to AVAIL, else ignore */
+		clkctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, &err);
+		if (err) {
+			DHD_ERROR(("%s: error reading CSR: %d\n", __FUNCTION__, err));
+			bus->dhd->busstate = DHD_BUS_DOWN;
+		}
+
+		DHD_INFO(("DPC: PENDING, devctl 0x%02x clkctl 0x%02x\n", devctl, clkctl));
+
+		if (SBSDIO_HTAV(clkctl)) {
+			devctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, &err);
+			if (err) {
+				DHD_ERROR(("%s: error reading DEVCTL: %d\n",
+				           __FUNCTION__, err));
+				bus->dhd->busstate = DHD_BUS_DOWN;
+			}
+			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
+			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, devctl, &err);
+			if (err) {
+				DHD_ERROR(("%s: error writing DEVCTL: %d\n",
+				           __FUNCTION__, err));
+				bus->dhd->busstate = DHD_BUS_DOWN;
+			}
+			bus->clkstate = CLK_AVAIL;
+		} else {
+			goto clkwait;
+		}
+	}
+
+	BUS_WAKE(bus);
+
+	/* Make sure backplane clock is on */
+	dhdsdio_clkctl(bus, CLK_AVAIL, TRUE);
+	if (bus->clkstate == CLK_PENDING)
+		goto clkwait;
+
+	/* Pending interrupt indicates new device status */
+	if (bus->ipend) {
+		bus->ipend = FALSE;
+		R_SDREG(newstatus, &regs->intstatus, retries);
+		bus->f1regdata++;
+		if (bcmsdh_regfail(bus->sdh))
+			newstatus = 0;
+		newstatus &= bus->hostintmask;
+		bus->fcstate = !!(newstatus & I_HMB_FC_STATE);
+		if (newstatus) {
+			W_SDREG(newstatus, &regs->intstatus, retries);
+			bus->f1regdata++;
+		}
+	}
+
+	/* Merge new bits with previous */
+	intstatus |= newstatus;
+	bus->intstatus = 0;
+
+	/* Handle flow-control change: read new state in case our ack
+	 * crossed another change interrupt.  If change still set, assume
+	 * FC ON for safety, let next loop through do the debounce.
+	 */
+	if (intstatus & I_HMB_FC_CHANGE) {
+		intstatus &= ~I_HMB_FC_CHANGE;
+		W_SDREG(I_HMB_FC_CHANGE, &regs->intstatus, retries);
+		R_SDREG(newstatus, &regs->intstatus, retries);
+		bus->f1regdata += 2;
+		bus->fcstate = !!(newstatus & (I_HMB_FC_STATE | I_HMB_FC_CHANGE));
+		intstatus |= (newstatus & bus->hostintmask);
+	}
+
+	/* Handle host mailbox indication */
+	if (intstatus & I_HMB_HOST_INT) {
+#ifdef CONFIG_HAS_WAKELOCK
+		wake_lock_timeout(&bus->dhd->wow_wakelock, 3*HZ);
+#endif
+		intstatus &= ~I_HMB_HOST_INT;
+		intstatus |= dhdsdio_hostmail(bus);
+	}
+
+	/* Generally don't ask for these, can get CRC errors... */
+	if (intstatus & I_WR_OOSYNC) {
+		DHD_ERROR(("Dongle reports WR_OOSYNC\n"));
+		intstatus &= ~I_WR_OOSYNC;
+	}
+
+	if (intstatus & I_RD_OOSYNC) {
+		DHD_ERROR(("Dongle reports RD_OOSYNC\n"));
+		intstatus &= ~I_RD_OOSYNC;
+	}
+
+	if (intstatus & I_SBINT) {
+		DHD_ERROR(("Dongle reports SBINT\n"));
+		intstatus &= ~I_SBINT;
+	}
+
+	/* Would be active due to wake-wlan in gSPI */
+	if (intstatus & I_CHIPACTIVE) {
+		DHD_INFO(("Dongle reports CHIPACTIVE\n"));
+		intstatus &= ~I_CHIPACTIVE;
+	}
+
+	/* Ignore frame indications if rxskip is set */
+	if (bus->rxskip)
+		intstatus &= ~I_HMB_FRAME_IND;
+
+	/* On frame indication, read available frames */
+	if (PKT_AVAILABLE()) {
+#ifdef CONFIG_HAS_WAKELOCK
+		wake_lock_timeout(&bus->dhd->wow_wakelock, 3*HZ);
+#endif
+		framecnt = dhdsdio_readframes(bus, rxlimit, &rxdone);
+		if (rxdone || bus->rxskip)
+			intstatus &= ~I_HMB_FRAME_IND;
+		rxlimit -= MIN(framecnt, rxlimit);
+	}
+
+	/* Keep still-pending events for next scheduling */
+	bus->intstatus = intstatus;
+
+clkwait:
+	/* Re-enable interrupts to detect new device events (mailbox, rx frame)
+	 * or clock availability.  (Allows tx loop to check ipend if desired.)
+	 * (Unless register access seems hosed, as we may not be able to ACK...)
+	 */
+	if (bus->intr && bus->intdis && !bcmsdh_regfail(sdh)) {
+		DHD_INTR(("%s: enable SDIO interrupts, rxdone %d framecnt %d\n",
+		          __FUNCTION__, rxdone, framecnt));
+		bus->intdis = FALSE;
+#if defined(OOB_INTR_ONLY)
+		bcmsdh_oob_intr_set(1);
+#endif /* (OOB_INTR_ONLY) */
+		bcmsdh_intr_enable(sdh);
+	}
+
+	if (DATAOK(bus) && bus->ctrl_frame_stat && (bus->clkstate == CLK_AVAIL)) {
+		int ret, i;
+
+		ret = dhd_bcmsdh_send_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
+		                      (uint8 *)bus->ctrl_frame_buf, (uint32)bus->ctrl_frame_len,
+			NULL, NULL, NULL);
+		ASSERT(ret != BCME_PENDING);
+
+		if (ret < 0) {
+			/* On failure, abort the command and terminate the frame */
+			DHD_INFO(("%s: sdio error %d, abort command and terminate frame.\n",
+			          __FUNCTION__, ret));
+			bus->tx_sderrs++;
+
+			bcmsdh_abort(sdh, SDIO_FUNC_2);
+
+			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_FRAMECTRL,
+			                 SFC_WF_TERM, NULL);
+			bus->f1regdata++;
+
+			for (i = 0; i < 3; i++) {
+				uint8 hi, lo;
+				hi = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+				                     SBSDIO_FUNC1_WFRAMEBCHI, NULL);
+				lo = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+				                     SBSDIO_FUNC1_WFRAMEBCLO, NULL);
+				bus->f1regdata += 2;
+				if ((hi == 0) && (lo == 0))
+					break;
+			}
+
+		}
+		if (ret == 0) {
+				bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQUENCE_WRAP;
+		}
+
+		printf("Return_dpc value is : %d\n", ret);
+		bus->ctrl_frame_stat = FALSE;
+		dhd_wait_event_wakeup(bus->dhd);
+	}
+	/* Send queued frames (limit 1 if rx may still be pending) */
+	else if ((bus->clkstate == CLK_AVAIL) && !bus->fcstate &&
+	    pktq_mlen(&bus->txq, ~bus->flowcontrol) && txlimit && DATAOK(bus)) {
+		framecnt = rxdone ? txlimit : MIN(txlimit, dhd_txminmax);
+		framecnt = dhdsdio_sendfromq(bus, framecnt);
+		txlimit -= framecnt;
+	}
+
+	/* Resched if events or tx frames are pending, else await next interrupt */
+	/* On failed register access, all bets are off: no resched or interrupts */
+	if ((bus->dhd->busstate == DHD_BUS_DOWN) || bcmsdh_regfail(sdh)) {
+		DHD_ERROR(("%s: failed backplane access over SDIO, halting operation %d \n",
+		           __FUNCTION__, bcmsdh_regfail(sdh)));
+		bus->dhd->busstate = DHD_BUS_DOWN;
+		bus->intstatus = 0;
+	} else if (bus->clkstate == CLK_PENDING) {
+		DHD_INFO(("%s: rescheduled due to CLK_PENDING awaiting \
+			I_CHIPACTIVE interrupt", __FUNCTION__));
+			resched = TRUE;
+	} else if (bus->intstatus || bus->ipend ||
+	           (!bus->fcstate && pktq_mlen(&bus->txq, ~bus->flowcontrol) && DATAOK(bus)) ||
+			PKT_AVAILABLE()) {  /* Read multiple frames */
+		resched = TRUE;
+	}
+
+
+	bus->dpc_sched = resched;
+
+	/* If we're done for now, turn off clock request. */
+	if ((bus->clkstate != CLK_PENDING) && bus->idletime == DHD_IDLE_IMMEDIATE) {
+		bus->activity = FALSE;
+		dhdsdio_clkctl(bus, CLK_NONE, FALSE);
+	}
+
+	dhd_os_sdunlock(bus->dhd);
+
+	return resched;
+}
+
+bool
+dhd_bus_dpc(struct dhd_bus *bus)
+{
+	bool resched;
+
+	/* Call the DPC directly. */
+	DHD_TRACE(("Calling dhdsdio_dpc() from %s\n", __FUNCTION__));
+	resched = dhdsdio_dpc(bus);
+
+	return resched;
+}
+
+void
+dhdsdio_isr(void *arg)
+{
+	dhd_bus_t *bus = (dhd_bus_t*)arg;
+	bcmsdh_info_t *sdh;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (!bus) {
+		DHD_ERROR(("%s : bus is null pointer , exit \n", __FUNCTION__));
+		return;
+	}
+	sdh = bus->sdh;
+
+	if (bus->dhd->busstate == DHD_BUS_DOWN) {
+		DHD_ERROR(("%s : bus is down. we have nothing to do\n", __FUNCTION__));
+		return;
+	}
+	/* Count the interrupt call */
+	bus->intrcount++;
+	bus->ipend = TRUE;
+
+	/* Shouldn't get this interrupt if we're sleeping? */
+	if (bus->sleeping) {
+		DHD_ERROR(("INTERRUPT WHILE SLEEPING??\n"));
+		return;
+	}
+
+	/* Disable additional interrupts (is this needed now)? */
+	if (bus->intr) {
+		DHD_INTR(("%s: disable SDIO interrupts\n", __FUNCTION__));
+	} else {
+		DHD_ERROR(("dhdsdio_isr() w/o interrupt configured!\n"));
+	}
+
+	bcmsdh_intr_disable(sdh);
+	bus->intdis = TRUE;
+
+#if defined(SDIO_ISR_THREAD)
+	DHD_TRACE(("Calling dhdsdio_dpc() from %s\n", __FUNCTION__));
+	dhd_os_wake_lock(bus->dhd);
+	while (dhdsdio_dpc(bus));
+	dhd_os_wake_unlock(bus->dhd);
+#else
+	bus->dpc_sched = TRUE;
+	dhd_sched_dpc(bus->dhd);
+#endif 
+
+}
+
+#ifdef SDTEST
+static void
+dhdsdio_pktgen_init(dhd_bus_t *bus)
+{
+	/* Default to specified length, or full range */
+	if (dhd_pktgen_len) {
+		bus->pktgen_maxlen = MIN(dhd_pktgen_len, MAX_PKTGEN_LEN);
+		bus->pktgen_minlen = bus->pktgen_maxlen;
+	} else {
+		bus->pktgen_maxlen = MAX_PKTGEN_LEN;
+		bus->pktgen_minlen = 0;
+	}
+	bus->pktgen_len = (uint16)bus->pktgen_minlen;
+
+	/* Default to per-watchdog burst with 10s print time */
+	bus->pktgen_freq = 1;
+	bus->pktgen_print = 10000 / dhd_watchdog_ms;
+	bus->pktgen_count = (dhd_pktgen * dhd_watchdog_ms + 999) / 1000;
+
+	/* Default to echo mode */
+	bus->pktgen_mode = DHD_PKTGEN_ECHO;
+	bus->pktgen_stop = 1;
+}
+
+static void
+dhdsdio_pktgen(dhd_bus_t *bus)
+{
+	void *pkt;
+	uint8 *data;
+	uint pktcount;
+	uint fillbyte;
+	osl_t *osh = bus->dhd->osh;
+	uint16 len;
+
+	/* Display current count if appropriate */
+	if (bus->pktgen_print && (++bus->pktgen_ptick >= bus->pktgen_print)) {
+		bus->pktgen_ptick = 0;
+		printf("%s: send attempts %d rcvd %d\n",
+		       __FUNCTION__, bus->pktgen_sent, bus->pktgen_rcvd);
+	}
+
+	/* For recv mode, just make sure dongle has started sending */
+	if (bus->pktgen_mode == DHD_PKTGEN_RECV) {
+		if (!bus->pktgen_rcvd)
+			dhdsdio_sdtest_set(bus, TRUE);
+		return;
+	}
+
+	/* Otherwise, generate or request the specified number of packets */
+	for (pktcount = 0; pktcount < bus->pktgen_count; pktcount++) {
+		/* Stop if total has been reached */
+		if (bus->pktgen_total && (bus->pktgen_sent >= bus->pktgen_total)) {
+			bus->pktgen_count = 0;
+			break;
+		}
+
+		/* Allocate an appropriate-sized packet */
+		len = bus->pktgen_len;
+		if (!(pkt = PKTGET(osh, (len + SDPCM_HDRLEN + SDPCM_TEST_HDRLEN + DHD_SDALIGN),
+		                   TRUE))) {;
+			DHD_ERROR(("%s: PKTGET failed!\n", __FUNCTION__));
+			break;
+		}
+		PKTALIGN(osh, pkt, (len + SDPCM_HDRLEN + SDPCM_TEST_HDRLEN), DHD_SDALIGN);
+		data = (uint8*)PKTDATA(osh, pkt) + SDPCM_HDRLEN;
+
+		/* Write test header cmd and extra based on mode */
+		switch (bus->pktgen_mode) {
+		case DHD_PKTGEN_ECHO:
+			*data++ = SDPCM_TEST_ECHOREQ;
+			*data++ = (uint8)bus->pktgen_sent;
+			break;
+
+		case DHD_PKTGEN_SEND:
+			*data++ = SDPCM_TEST_DISCARD;
+			*data++ = (uint8)bus->pktgen_sent;
+			break;
+
+		case DHD_PKTGEN_RXBURST:
+			*data++ = SDPCM_TEST_BURST;
+			*data++ = (uint8)bus->pktgen_count;
+			break;
+
+		default:
+			DHD_ERROR(("Unrecognized pktgen mode %d\n", bus->pktgen_mode));
+			PKTFREE(osh, pkt, TRUE);
+			bus->pktgen_count = 0;
+			return;
+		}
+
+		/* Write test header length field */
+		*data++ = (len >> 0);
+		*data++ = (len >> 8);
+
+		/* Then fill in the remainder -- N/A for burst, but who cares... */
+		for (fillbyte = 0; fillbyte < len; fillbyte++)
+			*data++ = SDPCM_TEST_FILL(fillbyte, (uint8)bus->pktgen_sent);
+
+#ifdef DHD_DEBUG
+		if (DHD_BYTES_ON() && DHD_DATA_ON()) {
+			data = (uint8*)PKTDATA(osh, pkt) + SDPCM_HDRLEN;
+			prhex("dhdsdio_pktgen: Tx Data", data, PKTLEN(osh, pkt) - SDPCM_HDRLEN);
+		}
+#endif
+
+		/* Send it */
+		if (dhdsdio_txpkt(bus, pkt, SDPCM_TEST_CHANNEL, TRUE)) {
+			bus->pktgen_fail++;
+			if (bus->pktgen_stop && bus->pktgen_stop == bus->pktgen_fail)
+				bus->pktgen_count = 0;
+		}
+		bus->pktgen_sent++;
+
+		/* Bump length if not fixed, wrap at max */
+		if (++bus->pktgen_len > bus->pktgen_maxlen)
+			bus->pktgen_len = (uint16)bus->pktgen_minlen;
+
+		/* Special case for burst mode: just send one request! */
+		if (bus->pktgen_mode == DHD_PKTGEN_RXBURST)
+			break;
+	}
+}
+
+static void
+dhdsdio_sdtest_set(dhd_bus_t *bus, bool start)
+{
+	void *pkt;
+	uint8 *data;
+	osl_t *osh = bus->dhd->osh;
+
+	/* Allocate the packet */
+	if (!(pkt = PKTGET(osh, SDPCM_HDRLEN + SDPCM_TEST_HDRLEN + DHD_SDALIGN, TRUE))) {
+		DHD_ERROR(("%s: PKTGET failed!\n", __FUNCTION__));
+		return;
+	}
+	PKTALIGN(osh, pkt, (SDPCM_HDRLEN + SDPCM_TEST_HDRLEN), DHD_SDALIGN);
+	data = (uint8*)PKTDATA(osh, pkt) + SDPCM_HDRLEN;
+
+	/* Fill in the test header */
+	*data++ = SDPCM_TEST_SEND;
+	*data++ = start;
+	*data++ = (bus->pktgen_maxlen >> 0);
+	*data++ = (bus->pktgen_maxlen >> 8);
+
+	/* Send it */
+	if (dhdsdio_txpkt(bus, pkt, SDPCM_TEST_CHANNEL, TRUE))
+		bus->pktgen_fail++;
+}
+
+
+static void
+dhdsdio_testrcv(dhd_bus_t *bus, void *pkt, uint seq)
+{
+	osl_t *osh = bus->dhd->osh;
+	uint8 *data;
+	uint pktlen;
+
+	uint8 cmd;
+	uint8 extra;
+	uint16 len;
+	uint16 offset;
+
+	/* Check for min length */
+	if ((pktlen = PKTLEN(osh, pkt)) < SDPCM_TEST_HDRLEN) {
+		DHD_ERROR(("dhdsdio_restrcv: toss runt frame, pktlen %d\n", pktlen));
+		PKTFREE(osh, pkt, FALSE);
+		return;
+	}
+
+	/* Extract header fields */
+	data = PKTDATA(osh, pkt);
+	cmd = *data++;
+	extra = *data++;
+	len = *data++; len += *data++ << 8;
+
+	/* Check length for relevant commands */
+	if (cmd == SDPCM_TEST_DISCARD || cmd == SDPCM_TEST_ECHOREQ || cmd == SDPCM_TEST_ECHORSP) {
+		if (pktlen != len + SDPCM_TEST_HDRLEN) {
+			DHD_ERROR(("dhdsdio_testrcv: frame length mismatch, pktlen %d seq %d"
+			           " cmd %d extra %d len %d\n", pktlen, seq, cmd, extra, len));
+			PKTFREE(osh, pkt, FALSE);
+			return;
+		}
+	}
+
+	/* Process as per command */
+	switch (cmd) {
+	case SDPCM_TEST_ECHOREQ:
+		/* Rx->Tx turnaround ok (even on NDIS w/current implementation) */
+		*(uint8 *)(PKTDATA(osh, pkt)) = SDPCM_TEST_ECHORSP;
+		if (dhdsdio_txpkt(bus, pkt, SDPCM_TEST_CHANNEL, TRUE) == 0) {
+			bus->pktgen_sent++;
+		} else {
+			bus->pktgen_fail++;
+			PKTFREE(osh, pkt, FALSE);
+		}
+		bus->pktgen_rcvd++;
+		break;
+
+	case SDPCM_TEST_ECHORSP:
+		if (bus->ext_loop) {
+			PKTFREE(osh, pkt, FALSE);
+			bus->pktgen_rcvd++;
+			break;
+		}
+
+		for (offset = 0; offset < len; offset++, data++) {
+			if (*data != SDPCM_TEST_FILL(offset, extra)) {
+				DHD_ERROR(("dhdsdio_testrcv: echo data mismatch: "
+				           "offset %d (len %d) expect 0x%02x rcvd 0x%02x\n",
+				           offset, len, SDPCM_TEST_FILL(offset, extra), *data));
+				break;
+			}
+		}
+		PKTFREE(osh, pkt, FALSE);
+		bus->pktgen_rcvd++;
+		break;
+
+	case SDPCM_TEST_DISCARD:
+		PKTFREE(osh, pkt, FALSE);
+		bus->pktgen_rcvd++;
+		break;
+
+	case SDPCM_TEST_BURST:
+	case SDPCM_TEST_SEND:
+	default:
+		DHD_INFO(("dhdsdio_testrcv: unsupported or unknown command, pktlen %d seq %d"
+		          " cmd %d extra %d len %d\n", pktlen, seq, cmd, extra, len));
+		PKTFREE(osh, pkt, FALSE);
+		break;
+	}
+
+	/* For recv mode, stop at limie (and tell dongle to stop sending) */
+	if (bus->pktgen_mode == DHD_PKTGEN_RECV) {
+		if (bus->pktgen_total && (bus->pktgen_rcvd >= bus->pktgen_total)) {
+			bus->pktgen_count = 0;
+			dhdsdio_sdtest_set(bus, FALSE);
+		}
+	}
+}
+#endif /* SDTEST */
+
+extern bool
+dhd_bus_watchdog(dhd_pub_t *dhdp)
+{
+	dhd_bus_t *bus;
+
+	DHD_TIMER(("%s: Enter\n", __FUNCTION__));
+
+	bus = dhdp->bus;
+
+	if (bus->dhd->dongle_reset)
+		return FALSE;
+
+	/* Ignore the timer if simulating bus down */
+	if (bus->sleeping)
+		return FALSE;
+
+	/* Poll period: check device if appropriate. */
+	if (bus->poll && (++bus->polltick >= bus->pollrate)) {
+		uint32 intstatus = 0;
+
+		/* Reset poll tick */
+		bus->polltick = 0;
+
+		/* Check device if no interrupts */
+		if (!bus->intr || (bus->intrcount == bus->lastintrs)) {
+
+			if (!bus->dpc_sched) {
+				uint8 devpend;
+				devpend = bcmsdh_cfg_read(bus->sdh, SDIO_FUNC_0,
+				                          SDIOD_CCCR_INTPEND, NULL);
+				intstatus = devpend & (INTR_STATUS_FUNC1 | INTR_STATUS_FUNC2);
+			}
+
+			/* If there is something, make like the ISR and schedule the DPC */
+			if (intstatus) {
+				bus->pollcnt++;
+				bus->ipend = TRUE;
+				if (bus->intr) {
+					bcmsdh_intr_disable(bus->sdh);
+				}
+				bus->dpc_sched = TRUE;
+				dhd_sched_dpc(bus->dhd);
+
+			}
+		}
+
+		/* Update interrupt tracking */
+		bus->lastintrs = bus->intrcount;
+	}
+
+#ifdef DHD_DEBUG
+	/* Poll for console output periodically */
+	if (dhdp->busstate == DHD_BUS_DATA && dhd_console_ms != 0) {
+		bus->console.count += dhd_watchdog_ms;
+		if (bus->console.count >= dhd_console_ms) {
+			bus->console.count -= dhd_console_ms;
+			/* Make sure backplane clock is on */
+			dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+			if (dhdsdio_readconsole(bus) < 0)
+				dhd_console_ms = 0;	/* On error, stop trying */
+		}
+	}
+#endif /* DHD_DEBUG */
+
+#ifdef SDTEST
+	/* Generate packets if configured */
+	if (bus->pktgen_count && (++bus->pktgen_tick >= bus->pktgen_freq)) {
+		/* Make sure backplane clock is on */
+		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+		bus->pktgen_tick = 0;
+		dhdsdio_pktgen(bus);
+	}
+#endif
+
+	/* On idle timeout clear activity flag and/or turn off clock */
+	if ((bus->idletime > 0) && (bus->clkstate == CLK_AVAIL)) {
+		if (++bus->idlecount >= bus->idletime) {
+			bus->idlecount = 0;
+			if (bus->activity) {
+				bus->activity = FALSE;
+				dhdsdio_clkctl(bus, CLK_NONE, FALSE);
+			}
+		}
+	}
+
+	return bus->ipend;
+}
+
+#ifdef DHD_DEBUG
+extern int
+dhd_bus_console_in(dhd_pub_t *dhdp, uchar *msg, uint msglen)
+{
+	dhd_bus_t *bus = dhdp->bus;
+	uint32 addr, val;
+	int rv;
+	void *pkt;
+
+	/* Address could be zero if CONSOLE := 0 in dongle Makefile */
+	if (bus->console_addr == 0)
+		return BCME_UNSUPPORTED;
+
+	/* Exclusive bus access */
+	dhd_os_sdlock(bus->dhd);
+
+	/* Don't allow input if dongle is in reset */
+	if (bus->dhd->dongle_reset) {
+		dhd_os_sdunlock(bus->dhd);
+		return BCME_NOTREADY;
+	}
+
+	/* Request clock to allow SDIO accesses */
+	BUS_WAKE(bus);
+	/* No pend allowed since txpkt is called later, ht clk has to be on */
+	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+
+	/* Zero cbuf_index */
+	addr = bus->console_addr + OFFSETOF(hndrte_cons_t, cbuf_idx);
+	val = htol32(0);
+	if ((rv = dhdsdio_membytes(bus, TRUE, addr, (uint8 *)&val, sizeof(val))) < 0)
+		goto done;
+
+	/* Write message into cbuf */
+	addr = bus->console_addr + OFFSETOF(hndrte_cons_t, cbuf);
+	if ((rv = dhdsdio_membytes(bus, TRUE, addr, (uint8 *)msg, msglen)) < 0)
+		goto done;
+
+	/* Write length into vcons_in */
+	addr = bus->console_addr + OFFSETOF(hndrte_cons_t, vcons_in);
+	val = htol32(msglen);
+	if ((rv = dhdsdio_membytes(bus, TRUE, addr, (uint8 *)&val, sizeof(val))) < 0)
+		goto done;
+
+	/* Bump dongle by sending an empty event pkt.
+	 * sdpcm_sendup (RX) checks for virtual console input.
+	 */
+	if (((pkt = PKTGET(bus->dhd->osh, 4 + SDPCM_RESERVE, TRUE)) != NULL) &&
+		bus->clkstate == CLK_AVAIL)
+		dhdsdio_txpkt(bus, pkt, SDPCM_EVENT_CHANNEL, TRUE);
+
+done:
+	if ((bus->idletime == DHD_IDLE_IMMEDIATE) && !bus->dpc_sched) {
+		bus->activity = FALSE;
+		dhdsdio_clkctl(bus, CLK_NONE, TRUE);
+	}
+
+	dhd_os_sdunlock(bus->dhd);
+
+	return rv;
+}
+#endif /* DHD_DEBUG */
+
+#ifdef DHD_DEBUG
+static void
+dhd_dump_cis(uint fn, uint8 *cis)
+{
+	uint byte, tag, tdata;
+	DHD_INFO(("Function %d CIS:\n", fn));
+
+	for (tdata = byte = 0; byte < SBSDIO_CIS_SIZE_LIMIT; byte++) {
+		if ((byte % 16) == 0)
+			DHD_INFO(("    "));
+		DHD_INFO(("%02x ", cis[byte]));
+		if ((byte % 16) == 15)
+			DHD_INFO(("\n"));
+		if (!tdata--) {
+			tag = cis[byte];
+			if (tag == 0xff)
+				break;
+			else if (!tag)
+				tdata = 0;
+			else if ((byte + 1) < SBSDIO_CIS_SIZE_LIMIT)
+				tdata = cis[byte + 1] + 1;
+			else
+				DHD_INFO(("]"));
+		}
+	}
+	if ((byte % 16) != 15)
+		DHD_INFO(("\n"));
+}
+#endif /* DHD_DEBUG */
+
+static bool
+dhdsdio_chipmatch(uint16 chipid)
+{
+	if (chipid == BCM4325_CHIP_ID)
+		return TRUE;
+	if (chipid == BCM4329_CHIP_ID)
+		return TRUE;
+	if (chipid == BCM4315_CHIP_ID)
+		return TRUE;
+	if (chipid == BCM4319_CHIP_ID)
+		return TRUE;
+	return FALSE;
+}
+
+static void *
+dhdsdio_probe(uint16 venid, uint16 devid, uint16 bus_no, uint16 slot,
+	uint16 func, uint bustype, void *regsva, osl_t * osh, void *sdh,
+	void *dev)
+{
+	int ret;
+	dhd_bus_t *bus;
+
+	/* Init global variables at run-time, not as part of the declaration.
+	 * This is required to support init/de-init of the driver. Initialization
+	 * of globals as part of the declaration results in non-deterministic
+	 * behavior since the value of the globals may be different on the
+	 * first time that the driver is initialized vs subsequent initializations.
+	 */
+	dhd_txbound = DHD_TXBOUND;
+	dhd_rxbound = DHD_RXBOUND;
+	dhd_alignctl = TRUE;
+	sd1idle = TRUE;
+	dhd_readahead = TRUE;
+	retrydata = FALSE;
+	dhd_doflow = TRUE;
+	dhd_dongle_memsize = 0;
+	dhd_txminmax = DHD_TXMINMAX;
+
+	forcealign = TRUE;
+
+
+	dhd_common_init();
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+	DHD_INFO(("%s: venid 0x%04x devid 0x%04x\n", __FUNCTION__, venid, devid));
+
+	/* We make assumptions about address window mappings */
+	ASSERT((uintptr)regsva == SI_ENUM_BASE);
+
+	/* BCMSDH passes venid and devid based on CIS parsing -- but low-power start
+	 * means early parse could fail, so here we should get either an ID
+	 * we recognize OR (-1) indicating we must request power first.
+	 */
+	/* Check the Vendor ID */
+	switch (venid) {
+		case 0x0000:
+		case VENDOR_BROADCOM:
+			break;
+		default:
+			DHD_ERROR(("%s: unknown vendor: 0x%04x\n",
+			           __FUNCTION__, venid));
+			return NULL;
+	}
+
+	/* Check the Device ID and make sure it's one that we support */
+	switch (devid) {
+		case BCM4325_D11DUAL_ID:		/* 4325 802.11a/g id */
+		case BCM4325_D11G_ID:			/* 4325 802.11g 2.4Ghz band id */
+		case BCM4325_D11A_ID:			/* 4325 802.11a 5Ghz band id */
+			DHD_INFO(("%s: found 4325 Dongle\n", __FUNCTION__));
+			break;
+		case BCM4329_D11NDUAL_ID:		/* 4329 802.11n dualband device */
+		case BCM4329_D11N2G_ID:		/* 4329 802.11n 2.4G device */
+		case BCM4329_D11N5G_ID:		/* 4329 802.11n 5G device */
+		case 0x4329:
+			DHD_INFO(("%s: found 4329 Dongle\n", __FUNCTION__));
+			break;
+		case BCM4315_D11DUAL_ID:		/* 4315 802.11a/g id */
+		case BCM4315_D11G_ID:			/* 4315 802.11g id */
+		case BCM4315_D11A_ID:			/* 4315 802.11a id */
+			DHD_INFO(("%s: found 4315 Dongle\n", __FUNCTION__));
+			break;
+		case BCM4319_D11N_ID:			/* 4319 802.11n id */
+		case BCM4319_D11N2G_ID:			/* 4319 802.11n2g id */
+		case BCM4319_D11N5G_ID:			/* 4319 802.11n5g id */
+			DHD_INFO(("%s: found 4319 Dongle\n", __FUNCTION__));
+			break;
+		case 0:
+			DHD_INFO(("%s: allow device id 0, will check chip internals\n",
+			          __FUNCTION__));
+			break;
+
+		default:
+			DHD_ERROR(("%s: skipping 0x%04x/0x%04x, not a dongle\n",
+			           __FUNCTION__, venid, devid));
+			return NULL;
+	}
+
+	if (osh == NULL) {
+		/* Ask the OS interface part for an OSL handle */
+		if (!(osh = dhd_osl_attach(sdh, DHD_BUS))) {
+			DHD_ERROR(("%s: osl_attach failed!\n", __FUNCTION__));
+			return NULL;
+		}
+	}
+
+	/* Allocate private bus interface state */
+	if (!(bus = MALLOC(osh, sizeof(dhd_bus_t)))) {
+		DHD_ERROR(("%s: MALLOC of dhd_bus_t failed\n", __FUNCTION__));
+		goto fail;
+	}
+	bzero(bus, sizeof(dhd_bus_t));
+	bus->sdh = sdh;
+	bus->cl_devid = (uint16)devid;
+	bus->bus = DHD_BUS;
+	bus->tx_seq = SDPCM_SEQUENCE_WRAP - 1;
+	bus->usebufpool = FALSE; /* Use bufpool if allocated, else use locally malloced rxbuf */
+
+	/* attempt to attach to the dongle */
+	if (!(dhdsdio_probe_attach(bus, osh, sdh, regsva, devid))) {
+		DHD_ERROR(("%s: dhdsdio_probe_attach failed\n", __FUNCTION__));
+		goto fail;
+	}
+
+	/* Attach to the dhd/OS/network interface */
+	if (!(bus->dhd = dhd_attach(osh, bus, SDPCM_RESERVE, dev))) {
+		DHD_ERROR(("%s: dhd_attach failed\n", __FUNCTION__));
+		goto fail;
+	}
+
+	/* Allocate buffers */
+	if (!(dhdsdio_probe_malloc(bus, osh, sdh))) {
+		DHD_ERROR(("%s: dhdsdio_probe_malloc failed\n", __FUNCTION__));
+		goto fail;
+	}
+
+	if (!(dhdsdio_probe_init(bus, osh, sdh))) {
+		DHD_ERROR(("%s: dhdsdio_probe_init failed\n", __FUNCTION__));
+		goto fail;
+	}
+
+	/* Register interrupt callback, but mask it (not operational yet). */
+	DHD_INTR(("%s: disable SDIO interrupts (not interested yet)\n", __FUNCTION__));
+	bcmsdh_intr_disable(sdh);
+	if ((ret = bcmsdh_intr_reg(sdh, dhdsdio_isr, bus)) != 0) {
+		DHD_ERROR(("%s: FAILED: bcmsdh_intr_reg returned %d\n",
+		           __FUNCTION__, ret));
+		goto fail;
+	}
+	DHD_INTR(("%s: registered SDIO interrupt function ok\n", __FUNCTION__));
+
+	DHD_INFO(("%s: completed!!\n", __FUNCTION__));
+
+
+	/* if firmware path present try to download and bring up bus */
+	if ((ret = dhd_bus_start(bus->dhd)) != 0) {
+#if 1
+		DHD_ERROR(("%s: failed\n", __FUNCTION__));
+		goto fail;
+#else
+		if (ret == BCME_NOTUP)  {
+			DHD_ERROR(("%s: dongle is not responding\n", __FUNCTION__));
+			goto fail;
+		}
+#endif
+	}
+	/* Ok, have the per-port tell the stack we're open for business */
+	if (dhd_net_attach(bus->dhd, 0) != 0) {
+		DHD_ERROR(("%s: Net attach failed!!\n", __FUNCTION__));
+		goto fail;
+	}
+
+	return bus;
+
+fail:
+	dhdsdio_release(bus, osh);
+	return NULL;
+}
+
+
+static bool
+dhdsdio_probe_attach(struct dhd_bus *bus, osl_t *osh, void *sdh, void *regsva,
+                    uint16 devid)
+{
+	uint8 clkctl = 0;
+	int err = 0;
+
+	bus->alp_only = TRUE;
+
+	/* Return the window to backplane enumeration space for core access */
+	if (dhdsdio_set_siaddr_window(bus, SI_ENUM_BASE)) {
+		DHD_ERROR(("%s: FAILED to return to SI_ENUM_BASE\n", __FUNCTION__));
+	}
+
+#ifdef DHD_DEBUG
+	printf("F1 signature read @0x18000000=0x%4x\n",
+	       bcmsdh_reg_read(bus->sdh, SI_ENUM_BASE, 4));
+
+
+#endif /* DHD_DEBUG */
+
+
+	/* Force PLL off until si_attach() programs PLL control regs */
+
+
+
+	bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, DHD_INIT_CLKCTL1, &err);
+	if (!err)
+		clkctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, &err);
+
+	if (err || ((clkctl & ~SBSDIO_AVBITS) != DHD_INIT_CLKCTL1)) {
+		DHD_ERROR(("dhdsdio_probe: ChipClkCSR access: err %d wrote 0x%02x read 0x%02x\n",
+		           err, DHD_INIT_CLKCTL1, clkctl));
+		goto fail;
+	}
+
+
+#ifdef DHD_DEBUG
+	if (DHD_INFO_ON()) {
+		uint fn, numfn;
+		uint8 *cis[SDIOD_MAX_IOFUNCS];
+		int err = 0;
+
+		numfn = bcmsdh_query_iofnum(sdh);
+		ASSERT(numfn <= SDIOD_MAX_IOFUNCS);
+
+		/* Make sure ALP is available before trying to read CIS */
+		SPINWAIT(((clkctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+		                                    SBSDIO_FUNC1_CHIPCLKCSR, NULL)),
+		          !SBSDIO_ALPAV(clkctl)), PMU_MAX_TRANSITION_DLY);
+
+		/* Now request ALP be put on the bus */
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
+		                 DHD_INIT_CLKCTL2, &err);
+		OSL_DELAY(65);
+
+		for (fn = 0; fn <= numfn; fn++) {
+			if (!(cis[fn] = MALLOC(osh, SBSDIO_CIS_SIZE_LIMIT))) {
+				DHD_INFO(("dhdsdio_probe: fn %d cis malloc failed\n", fn));
+				break;
+			}
+			bzero(cis[fn], SBSDIO_CIS_SIZE_LIMIT);
+
+			if ((err = bcmsdh_cis_read(sdh, fn, cis[fn], SBSDIO_CIS_SIZE_LIMIT))) {
+				DHD_INFO(("dhdsdio_probe: fn %d cis read err %d\n", fn, err));
+				MFREE(osh, cis[fn], SBSDIO_CIS_SIZE_LIMIT);
+				break;
+			}
+			dhd_dump_cis(fn, cis[fn]);
+		}
+
+		while (fn-- > 0) {
+			ASSERT(cis[fn]);
+			MFREE(osh, cis[fn], SBSDIO_CIS_SIZE_LIMIT);
+		}
+
+		if (err) {
+			DHD_ERROR(("dhdsdio_probe: failure reading or parsing CIS\n"));
+			goto fail;
+		}
+	}
+#endif /* DHD_DEBUG */
+
+	/* si_attach() will provide an SI handle and scan the backplane */
+	if (!(bus->sih = si_attach((uint)devid, osh, regsva, DHD_BUS, sdh,
+	                           &bus->vars, &bus->varsz))) {
+		DHD_ERROR(("%s: si_attach failed!\n", __FUNCTION__));
+		goto fail;
+	}
+
+	bcmsdh_chipinfo(sdh, bus->sih->chip, bus->sih->chiprev);
+
+	if (!dhdsdio_chipmatch((uint16)bus->sih->chip)) {
+		DHD_ERROR(("%s: unsupported chip: 0x%04x\n",
+		           __FUNCTION__, bus->sih->chip));
+		goto fail;
+	}
+
+	si_sdiod_drive_strength_init(bus->sih, osh, dhd_sdiod_drive_strength);
+
+
+	/* Get info on the ARM and SOCRAM cores... */
+	if (!DHD_NOPMU(bus)) {
+		if ((si_setcore(bus->sih, ARM7S_CORE_ID, 0)) ||
+		    (si_setcore(bus->sih, ARMCM3_CORE_ID, 0))) {
+			bus->armrev = si_corerev(bus->sih);
+		} else {
+			DHD_ERROR(("%s: failed to find ARM core!\n", __FUNCTION__));
+			goto fail;
+		}
+		if (!(bus->orig_ramsize = si_socram_size(bus->sih))) {
+			DHD_ERROR(("%s: failed to find SOCRAM memory!\n", __FUNCTION__));
+			goto fail;
+		}
+		bus->ramsize = bus->orig_ramsize;
+		if (dhd_dongle_memsize)
+			dhd_dongle_setmemsize(bus, dhd_dongle_memsize);
+
+		DHD_ERROR(("DHD: dongle ram size is set to %d(orig %d)\n",
+			bus->ramsize, bus->orig_ramsize));
+	}
+
+	/* ...but normally deal with the SDPCMDEV core */
+	if (!(bus->regs = si_setcore(bus->sih, PCMCIA_CORE_ID, 0)) &&
+	    !(bus->regs = si_setcore(bus->sih, SDIOD_CORE_ID, 0))) {
+		DHD_ERROR(("%s: failed to find SDIODEV core!\n", __FUNCTION__));
+		goto fail;
+	}
+	bus->sdpcmrev = si_corerev(bus->sih);
+
+	/* Set core control so an SDIO reset does a backplane reset */
+	OR_REG(osh, &bus->regs->corecontrol, CC_BPRESEN);
+
+	pktq_init(&bus->txq, (PRIOMASK + 1), QLEN);
+
+	/* Locate an appropriately-aligned portion of hdrbuf */
+	bus->rxhdr = (uint8 *)ROUNDUP((uintptr)&bus->hdrbuf[0], DHD_SDALIGN);
+
+	/* Set the poll and/or interrupt flags */
+	bus->intr = (bool)dhd_intr;
+	if ((bus->poll = (bool)dhd_poll))
+		bus->pollrate = 1;
+
+	return TRUE;
+
+fail:
+	return FALSE;
+}
+
+static bool
+dhdsdio_probe_malloc(dhd_bus_t *bus, osl_t *osh, void *sdh)
+{
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+#ifndef DHD_USE_STATIC_BUF
+	if (bus->dhd->maxctl) {
+		bus->rxblen = ROUNDUP((bus->dhd->maxctl + SDPCM_HDRLEN), ALIGNMENT) + DHD_SDALIGN;
+		if (!(bus->rxbuf = MALLOC(osh, bus->rxblen))) {
+			DHD_ERROR(("%s: MALLOC of %d-byte rxbuf failed\n",
+			           __FUNCTION__, bus->rxblen));
+			goto fail;
+		}
+	}
+
+	/* Allocate buffer to receive glomed packet */
+	if (!(bus->databuf = MALLOC(osh, MAX_DATA_BUF))) {
+		DHD_ERROR(("%s: MALLOC of %d-byte databuf failed\n",
+			__FUNCTION__, MAX_DATA_BUF));
+		/* release rxbuf which was already located as above */
+		if (!bus->rxblen) MFREE(osh, bus->rxbuf, bus->rxblen);
+		goto fail;
+	}
+#else
+	if (bus->dhd->maxctl) {
+		bus->rxblen = ROUNDUP((bus->dhd->maxctl + SDPCM_HDRLEN), ALIGNMENT) + DHD_SDALIGN;
+		if (!(bus->rxbuf = dhd_os_prealloc(DHD_PREALLOC_RXBUF, bus->rxblen))) {
+			DHD_ERROR(("%s: MALLOC of %d-byte rxbuf failed\n",
+			           __FUNCTION__, bus->rxblen));
+			goto fail;
+		}
+	}
+	/* Allocate buffer to receive glomed packet */
+	if (!(bus->databuf = dhd_os_prealloc(DHD_PREALLOC_DATABUF, MAX_DATA_BUF))) {
+		DHD_ERROR(("%s: MALLOC of %d-byte databuf failed\n",
+			__FUNCTION__, MAX_DATA_BUF));
+		goto fail;
+	}
+#endif /* DHD_USE_STATIC_BUF */
+
+	/* Align the buffer */
+	if ((uintptr)bus->databuf % DHD_SDALIGN)
+		bus->dataptr = bus->databuf + (DHD_SDALIGN - ((uintptr)bus->databuf % DHD_SDALIGN));
+	else
+		bus->dataptr = bus->databuf;
+
+	return TRUE;
+
+fail:
+	return FALSE;
+}
+
+
+static bool
+dhdsdio_probe_init(dhd_bus_t *bus, osl_t *osh, void *sdh)
+{
+	int32 fnum;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+#ifdef SDTEST
+	dhdsdio_pktgen_init(bus);
+#endif /* SDTEST */
+
+	/* Disable F2 to clear any intermediate frame state on the dongle */
+	bcmsdh_cfg_write(sdh, SDIO_FUNC_0, SDIOD_CCCR_IOEN, SDIO_FUNC_ENABLE_1, NULL);
+
+	bus->dhd->busstate = DHD_BUS_DOWN;
+	bus->sleeping = FALSE;
+	bus->rxflow = FALSE;
+	bus->prev_rxlim_hit = 0;
+
+
+	/* Done with backplane-dependent accesses, can drop clock... */
+	bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, 0, NULL);
+
+	/* ...and initialize clock/power states */
+	bus->clkstate = CLK_SDONLY;
+	bus->idletime = (int32)dhd_idletime;
+	bus->idleclock = DHD_IDLE_ACTIVE;
+
+	/* Query the SD clock speed */
+	if (bcmsdh_iovar_op(sdh, "sd_divisor", NULL, 0,
+	                    &bus->sd_divisor, sizeof(int32), FALSE) != BCME_OK) {
+		DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, "sd_divisor"));
+		bus->sd_divisor = -1;
+	} else {
+		DHD_INFO(("%s: Initial value for %s is %d\n",
+		          __FUNCTION__, "sd_divisor", bus->sd_divisor));
+	}
+
+	/* Query the SD bus mode */
+	if (bcmsdh_iovar_op(sdh, "sd_mode", NULL, 0,
+	                    &bus->sd_mode, sizeof(int32), FALSE) != BCME_OK) {
+		DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, "sd_mode"));
+		bus->sd_mode = -1;
+	} else {
+		DHD_INFO(("%s: Initial value for %s is %d\n",
+		          __FUNCTION__, "sd_mode", bus->sd_mode));
+	}
+
+	/* Query the F2 block size, set roundup accordingly */
+	fnum = 2;
+	if (bcmsdh_iovar_op(sdh, "sd_blocksize", &fnum, sizeof(int32),
+	                    &bus->blocksize, sizeof(int32), FALSE) != BCME_OK) {
+		bus->blocksize = 0;
+		DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, "sd_blocksize"));
+	} else {
+		DHD_INFO(("%s: Initial value for %s is %d\n",
+		          __FUNCTION__, "sd_blocksize", bus->blocksize));
+	}
+	bus->roundup = MIN(max_roundup, bus->blocksize);
+
+	/* Query if bus module supports packet chaining, default to use if supported */
+	if (bcmsdh_iovar_op(sdh, "sd_rxchain", NULL, 0,
+	                    &bus->sd_rxchain, sizeof(int32), FALSE) != BCME_OK) {
+		bus->sd_rxchain = FALSE;
+	} else {
+		DHD_INFO(("%s: bus module (through bcmsdh API) %s chaining\n",
+		          __FUNCTION__, (bus->sd_rxchain ? "supports" : "does not support")));
+	}
+	bus->use_rxchain = (bool)bus->sd_rxchain;
+
+	return TRUE;
+}
+
+bool
+dhd_bus_download_firmware(struct dhd_bus *bus, osl_t *osh,
+                          char *fw_path, char *nv_path)
+{
+	bool ret;
+	bus->fw_path = fw_path;
+	bus->nv_path = nv_path;
+
+	ret = dhdsdio_download_firmware(bus, osh, bus->sdh);
+
+	return ret;
+}
+
+static bool
+dhdsdio_download_firmware(struct dhd_bus *bus, osl_t *osh, void *sdh)
+{
+	bool ret;
+
+	/* Download the firmware */
+	dhd_os_wake_lock(bus->dhd);
+	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+
+	ret = _dhdsdio_download_firmware(bus) == 0;
+
+	dhdsdio_clkctl(bus, CLK_SDONLY, FALSE);
+	dhd_os_wake_unlock(bus->dhd);
+	return ret;
+}
+
+/* Detach and free everything */
+static void
+dhdsdio_release(dhd_bus_t *bus, osl_t *osh)
+{
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (bus) {
+		ASSERT(osh);
+
+
+		/* De-register interrupt handler */
+		bcmsdh_intr_disable(bus->sdh);
+		bcmsdh_intr_dereg(bus->sdh);
+
+		if (bus->dhd) {
+
+			dhdsdio_release_dongle(bus, osh, TRUE);
+
+			dhd_detach(bus->dhd);
+			bus->dhd = NULL;
+		}
+
+		dhdsdio_release_malloc(bus, osh);
+
+
+		MFREE(osh, bus, sizeof(dhd_bus_t));
+	}
+
+	if (osh)
+		dhd_osl_detach(osh);
+
+	DHD_TRACE(("%s: Disconnected\n", __FUNCTION__));
+}
+
+static void
+dhdsdio_release_malloc(dhd_bus_t *bus, osl_t *osh)
+{
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (bus->dhd && bus->dhd->dongle_reset)
+		return;
+
+	if (bus->rxbuf) {
+#ifndef DHD_USE_STATIC_BUF
+		MFREE(osh, bus->rxbuf, bus->rxblen);
+#endif
+		bus->rxctl = bus->rxbuf = NULL;
+		bus->rxlen = 0;
+	}
+
+	if (bus->databuf) {
+#ifndef DHD_USE_STATIC_BUF
+		MFREE(osh, bus->databuf, MAX_DATA_BUF);
+#endif
+		bus->databuf = NULL;
+	}
+}
+
+
+static void
+dhdsdio_release_dongle(dhd_bus_t *bus, osl_t *osh, int reset_flag)
+{
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if ((bus->dhd && bus->dhd->dongle_reset) && reset_flag)
+		return;
+
+	if (bus->sih) {
+		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+#if !defined(BCMLXSDMMC)
+		si_watchdog(bus->sih, 4);
+#endif /* !defined(BCMLXSDMMC) */
+		dhdsdio_clkctl(bus, CLK_NONE, FALSE);
+		si_detach(bus->sih);
+		if (bus->vars && bus->varsz)
+			MFREE(osh, bus->vars, bus->varsz);
+		bus->vars = NULL;
+	}
+
+	DHD_TRACE(("%s: Disconnected\n", __FUNCTION__));
+}
+
+static void
+dhdsdio_disconnect(void *ptr)
+{
+	dhd_bus_t *bus = (dhd_bus_t *)ptr;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (bus) {
+		ASSERT(bus->dhd);
+		dhdsdio_release(bus, bus->dhd->osh);
+	}
+
+	DHD_TRACE(("%s: Disconnected\n", __FUNCTION__));
+}
+
+
+/* Register/Unregister functions are called by the main DHD entry
+ * point (e.g. module insertion) to link with the bus driver, in
+ * order to look for or await the device.
+ */
+
+static bcmsdh_driver_t dhd_sdio = {
+	dhdsdio_probe,
+	dhdsdio_disconnect
+};
+
+int
+dhd_bus_register(void)
+{
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	return bcmsdh_register(&dhd_sdio);
+}
+
+void
+dhd_bus_unregister(void)
+{
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	bcmsdh_unregister();
+}
+
+#ifdef BCMEMBEDIMAGE
+static int
+dhdsdio_download_code_array(struct dhd_bus *bus)
+{
+	int bcmerror = -1;
+	int offset = 0;
+
+	DHD_INFO(("%s: download embedded firmware...\n", __FUNCTION__));
+
+	/* Download image */
+	while ((offset + MEMBLOCK) < sizeof(dlarray)) {
+		bcmerror = dhdsdio_membytes(bus, TRUE, offset, dlarray + offset, MEMBLOCK);
+		if (bcmerror) {
+			DHD_ERROR(("%s: error %d on writing %d membytes at 0x%08x\n",
+			        __FUNCTION__, bcmerror, MEMBLOCK, offset));
+			goto err;
+		}
+
+		offset += MEMBLOCK;
+	}
+
+	if (offset < sizeof(dlarray)) {
+		bcmerror = dhdsdio_membytes(bus, TRUE, offset,
+			dlarray + offset, sizeof(dlarray) - offset);
+		if (bcmerror) {
+			DHD_ERROR(("%s: error %d on writing %d membytes at 0x%08x\n",
+			        __FUNCTION__, bcmerror, sizeof(dlarray) - offset, offset));
+			goto err;
+		}
+	}
+
+#ifdef DHD_DEBUG
+	/* Upload and compare the downloaded code */
+	{
+		unsigned char *ularray;
+
+		ularray = MALLOC(bus->dhd->osh, bus->ramsize);
+		/* Upload image to verify downloaded contents. */
+		offset = 0;
+		memset(ularray, 0xaa, bus->ramsize);
+		while ((offset + MEMBLOCK) < sizeof(dlarray)) {
+			bcmerror = dhdsdio_membytes(bus, FALSE, offset, ularray + offset, MEMBLOCK);
+			if (bcmerror) {
+				DHD_ERROR(("%s: error %d on reading %d membytes at 0x%08x\n",
+					__FUNCTION__, bcmerror, MEMBLOCK, offset));
+				goto err;
+			}
+
+			offset += MEMBLOCK;
+		}
+
+		if (offset < sizeof(dlarray)) {
+			bcmerror = dhdsdio_membytes(bus, FALSE, offset,
+				ularray + offset, sizeof(dlarray) - offset);
+			if (bcmerror) {
+				DHD_ERROR(("%s: error %d on reading %d membytes at 0x%08x\n",
+					__FUNCTION__, bcmerror, sizeof(dlarray) - offset, offset));
+				goto err;
+			}
+		}
+
+		if (memcmp(dlarray, ularray, sizeof(dlarray))) {
+			DHD_ERROR(("%s: Downloaded image is corrupted.\n", __FUNCTION__));
+			ASSERT(0);
+			goto err;
+		} else
+			DHD_ERROR(("%s: Download, Upload and compare succeeded.\n", __FUNCTION__));
+
+		MFREE(bus->dhd->osh, ularray, bus->ramsize);
+	}
+#endif /* DHD_DEBUG */
+
+err:
+	return bcmerror;
+}
+#endif /* BCMEMBEDIMAGE */
+
+static int
+dhdsdio_download_code_file(struct dhd_bus *bus, char *fw_path)
+{
+	int bcmerror = -1;
+	int offset = 0;
+	uint len;
+	void *image = NULL;
+	uint8 *memblock = NULL, *memptr;
+
+	DHD_INFO(("%s: download firmware %s\n", __FUNCTION__, fw_path));
+
+	image = dhd_os_open_image(fw_path);
+	if (image == NULL)
+		goto err;
+
+	memptr = memblock = MALLOC(bus->dhd->osh, MEMBLOCK + DHD_SDALIGN);
+	if (memblock == NULL) {
+		DHD_ERROR(("%s: Failed to allocate memory %d bytes\n", __FUNCTION__, MEMBLOCK));
+		goto err;
+	}
+	if ((uint32)(uintptr)memblock % DHD_SDALIGN)
+		memptr += (DHD_SDALIGN - ((uint32)(uintptr)memblock % DHD_SDALIGN));
+
+	/* Download image */
+	while ((len = dhd_os_get_image_block((char*)memptr, MEMBLOCK, image))) {
+		bcmerror = dhdsdio_membytes(bus, TRUE, offset, memptr, len);
+		if (bcmerror) {
+			DHD_ERROR(("%s: error %d on writing %d membytes at 0x%08x\n",
+			        __FUNCTION__, bcmerror, MEMBLOCK, offset));
+			goto err;
+		}
+
+		offset += MEMBLOCK;
+	}
+
+err:
+	if (memblock)
+		MFREE(bus->dhd->osh, memblock, MEMBLOCK + DHD_SDALIGN);
+
+	if (image)
+		dhd_os_close_image(image);
+
+	return bcmerror;
+}
+
+/*
+ * ProcessVars:Takes a buffer of "<var>=<value>\n" lines read from a file and ending in a NUL.
+ * Removes carriage returns, empty lines, comment lines, and converts newlines to NULs.
+ * Shortens buffer as needed and pads with NULs.  End of buffer is marked by two NULs.
+*/
+
+static uint
+process_nvram_vars(char *varbuf, uint len)
+{
+	char *dp;
+	bool findNewline;
+	int column;
+	uint buf_len, n;
+
+	dp = varbuf;
+
+	findNewline = FALSE;
+	column = 0;
+
+	for (n = 0; n < len; n++) {
+		if (varbuf[n] == 0)
+			break;
+		if (varbuf[n] == '\r')
+			continue;
+		if (findNewline && varbuf[n] != '\n')
+			continue;
+		findNewline = FALSE;
+		if (varbuf[n] == '#') {
+			findNewline = TRUE;
+			continue;
+		}
+		if (varbuf[n] == '\n') {
+			if (column == 0)
+				continue;
+			*dp++ = 0;
+			column = 0;
+			continue;
+		}
+		*dp++ = varbuf[n];
+		column++;
+	}
+	buf_len = dp - varbuf;
+
+	while (dp < varbuf + n)
+		*dp++ = 0;
+
+	return buf_len;
+}
+
+/*
+	EXAMPLE: nvram_array
+	nvram_arry format:
+	name=value
+	Use carriage return at the end of each assignment, and an empty string with
+	carriage return at the end of array.
+
+	For example:
+	unsigned char  nvram_array[] = {"name1=value1\n", "name2=value2\n", "\n"};
+	Hex values start with 0x, and mac addr format: xx:xx:xx:xx:xx:xx.
+
+	Search "EXAMPLE: nvram_array" to see how the array is activated.
+*/
+
+void
+dhd_bus_set_nvram_params(struct dhd_bus * bus, const char *nvram_params)
+{
+	bus->nvram_params = nvram_params;
+}
+
+static int
+dhdsdio_download_nvram(struct dhd_bus *bus)
+{
+	int bcmerror = -1;
+	uint len;
+	void * image = NULL;
+	char * memblock = NULL;
+	char *bufp;
+	char *nv_path;
+	bool nvram_file_exists;
+
+	nv_path = bus->nv_path;
+
+	nvram_file_exists = ((nv_path != NULL) && (nv_path[0] != '\0'));
+	if (!nvram_file_exists && (bus->nvram_params == NULL))
+		return (0);
+
+	if (nvram_file_exists) {
+		image = dhd_os_open_image(nv_path);
+		if (image == NULL)
+			goto err;
+	}
+
+	memblock = MALLOC(bus->dhd->osh, MEMBLOCK);
+	if (memblock == NULL) {
+		DHD_ERROR(("%s: Failed to allocate memory %d bytes\n",
+		           __FUNCTION__, MEMBLOCK));
+		goto err;
+	}
+
+	/* Download variables */
+	if (nvram_file_exists) {
+		len = dhd_os_get_image_block(memblock, MEMBLOCK, image);
+	}
+	else {
+		len = strlen(bus->nvram_params);
+		ASSERT(len <= MEMBLOCK);
+		if (len > MEMBLOCK)
+			len = MEMBLOCK;
+		memcpy(memblock, bus->nvram_params, len);
+	}
+
+	if (len > 0 && len < MEMBLOCK) {
+		bufp = (char *)memblock;
+		bufp[len] = 0;
+		len = process_nvram_vars(bufp, len);
+		bufp += len;
+		*bufp++ = 0;
+		if (len)
+			bcmerror = dhdsdio_downloadvars(bus, memblock, len + 1);
+		if (bcmerror) {
+			DHD_ERROR(("%s: error downloading vars: %d\n",
+			           __FUNCTION__, bcmerror));
+		}
+	}
+	else {
+		DHD_ERROR(("%s: error reading nvram file: %d\n",
+		           __FUNCTION__, len));
+		bcmerror = BCME_SDIO_ERROR;
+	}
+
+err:
+	if (memblock)
+		MFREE(bus->dhd->osh, memblock, MEMBLOCK);
+
+	if (image)
+		dhd_os_close_image(image);
+
+	return bcmerror;
+}
+
+static int
+_dhdsdio_download_firmware(struct dhd_bus *bus)
+{
+	int bcmerror = -1;
+
+	bool embed = FALSE;	/* download embedded firmware */
+	bool dlok = FALSE;	/* download firmware succeeded */
+
+	/* Out immediately if no image to download */
+	if ((bus->fw_path == NULL) || (bus->fw_path[0] == '\0')) {
+#ifdef BCMEMBEDIMAGE
+		embed = TRUE;
+#else
+		return bcmerror;
+#endif
+	}
+
+	/* Keep arm in reset */
+	if (dhdsdio_download_state(bus, TRUE)) {
+		DHD_ERROR(("%s: error placing ARM core in reset\n", __FUNCTION__));
+		goto err;
+	}
+
+	/* External image takes precedence if specified */
+	if ((bus->fw_path != NULL) && (bus->fw_path[0] != '\0')) {
+		if (dhdsdio_download_code_file(bus, bus->fw_path)) {
+			DHD_ERROR(("%s: dongle image file download failed\n", __FUNCTION__));
+#ifdef BCMEMBEDIMAGE
+			embed = TRUE;
+#else
+			goto err;
+#endif
+		}
+		else {
+			embed = FALSE;
+			dlok = TRUE;
+		}
+	}
+#ifdef BCMEMBEDIMAGE
+	if (embed) {
+		if (dhdsdio_download_code_array(bus)) {
+			DHD_ERROR(("%s: dongle image array download failed\n", __FUNCTION__));
+			goto err;
+		}
+		else {
+			dlok = TRUE;
+		}
+	}
+#endif
+	if (!dlok) {
+		DHD_ERROR(("%s: dongle image download failed\n", __FUNCTION__));
+		goto err;
+	}
+
+	/* EXAMPLE: nvram_array */
+	/* If a valid nvram_arry is specified as above, it can be passed down to dongle */
+	/* dhd_bus_set_nvram_params(bus, (char *)&nvram_array); */
+
+	/* External nvram takes precedence if specified */
+	if (dhdsdio_download_nvram(bus)) {
+		DHD_ERROR(("%s: dongle nvram file download failed\n", __FUNCTION__));
+	}
+
+	/* Take arm out of reset */
+	if (dhdsdio_download_state(bus, FALSE)) {
+		DHD_ERROR(("%s: error getting out of ARM core reset\n", __FUNCTION__));
+		goto err;
+	}
+
+	bcmerror = 0;
+
+err:
+	return bcmerror;
+}
+
+static int
+dhd_bcmsdh_recv_buf(dhd_bus_t *bus, uint32 addr, uint fn, uint flags, uint8 *buf, uint nbytes,
+	void *pkt, bcmsdh_cmplt_fn_t complete, void *handle)
+{
+	int status;
+
+	/* 4329: GSPI check */
+	status = bcmsdh_recv_buf(bus->sdh, addr, fn, flags, buf, nbytes, pkt, complete, handle);
+	return status;
+}
+
+static int
+dhd_bcmsdh_send_buf(dhd_bus_t *bus, uint32 addr, uint fn, uint flags, uint8 *buf, uint nbytes,
+	void *pkt, bcmsdh_cmplt_fn_t complete, void *handle)
+{
+	return (bcmsdh_send_buf(bus->sdh, addr, fn, flags, buf, nbytes, pkt, complete, handle));
+}
+
+uint
+dhd_bus_chip(struct dhd_bus *bus)
+{
+	ASSERT(bus->sih != NULL);
+	return bus->sih->chip;
+}
+
+void *
+dhd_bus_pub(struct dhd_bus *bus)
+{
+	return bus->dhd;
+}
+
+void *
+dhd_bus_txq(struct dhd_bus *bus)
+{
+	return &bus->txq;
+}
+
+uint
+dhd_bus_hdrlen(struct dhd_bus *bus)
+{
+	return SDPCM_HDRLEN;
+}
+
+int
+dhd_bus_devreset(dhd_pub_t *dhdp, uint8 flag)
+{
+	int bcmerror = 0;
+	dhd_bus_t *bus;
+
+	bus = dhdp->bus;
+
+	if (flag == TRUE) {
+		if (!bus->dhd->dongle_reset) {
+			dhd_os_sdlock(dhdp);
+			/* Turning off watchdog */
+			dhd_os_wd_timer(dhdp, 0);
+#if !defined(IGNORE_ETH0_DOWN)
+			/* Force flow control as protection when stop come before ifconfig_down */
+			dhd_txflowcontrol(bus->dhd, 0, ON);
+#endif /* !defined(IGNORE_ETH0_DOWN) */
+
+#if !defined(OOB_INTR_ONLY)
+			/* to avoid supurious client interrupt during stop process */
+			bcmsdh_stop(bus->sdh);
+#endif /* !defined(OOB_INTR_ONLY) */
+
+			/* Expect app to have torn down any connection before calling */
+			/* Stop the bus, disable F2 */
+			dhd_bus_stop(bus, FALSE);
+#if defined(OOB_INTR_ONLY)
+			bcmsdh_set_irq(FALSE);
+#endif /* defined(OOB_INTR_ONLY) */
+			/* Clean tx/rx buffer pointers, detach from the dongle */
+			dhdsdio_release_dongle(bus, bus->dhd->osh, TRUE);
+
+			bus->dhd->dongle_reset = TRUE;
+			bus->dhd->up = FALSE;
+			dhd_os_sdunlock(dhdp);
+
+			DHD_TRACE(("%s:  WLAN OFF DONE\n", __FUNCTION__));
+			/* App can now remove power from device */
+		} else
+			bcmerror = BCME_SDIO_ERROR;
+	} else {
+		/* App must have restored power to device before calling */
+
+		DHD_TRACE(("\n\n%s: == WLAN ON ==\n", __FUNCTION__));
+
+		if (bus->dhd->dongle_reset) {
+			/* Turn on WLAN */
+			dhd_os_sdlock(dhdp);
+
+			/* Reset SD client */
+			bcmsdh_reset(bus->sdh);
+
+			/* Attempt to re-attach & download */
+			if (dhdsdio_probe_attach(bus, bus->dhd->osh, bus->sdh,
+			                        (uint32 *)SI_ENUM_BASE,
+			                        bus->cl_devid)) {
+				/* Attempt to download binary to the dongle */
+				if (dhdsdio_probe_init(bus, bus->dhd->osh, bus->sdh) &&
+					dhdsdio_download_firmware(bus, bus->dhd->osh, bus->sdh)) {
+
+					/* Re-init bus, enable F2 transfer */
+					bcmerror = dhd_bus_init((dhd_pub_t *) bus->dhd, FALSE);
+					if (bcmerror == BCME_OK) {
+#if defined(OOB_INTR_ONLY)
+						bcmsdh_set_irq(TRUE);
+						dhd_enable_oob_intr(bus, TRUE);
+#endif /* defined(OOB_INTR_ONLY) */
+						bus->dhd->dongle_reset = FALSE;
+						bus->dhd->up = TRUE;
+#if !defined(IGNORE_ETH0_DOWN)
+						/* Restore flow control  */
+						dhd_txflowcontrol(bus->dhd, 0, OFF);
+#endif
+						/* Turning on watchdog back */
+						dhd_os_wd_timer(dhdp, dhd_watchdog_ms);
+
+						DHD_TRACE(("%s: WLAN ON DONE\n", __FUNCTION__));
+					} else {
+						dhd_bus_stop(bus, FALSE);
+						dhdsdio_release_dongle(bus, bus->dhd->osh, FALSE);
+					}
+				} else
+					bcmerror = BCME_SDIO_ERROR;
+			} else
+				bcmerror = BCME_SDIO_ERROR;
+
+			dhd_os_sdunlock(dhdp);
+		} else {
+			bcmerror = BCME_NOTDOWN;
+			DHD_ERROR(("%s: Set DEVRESET=FALSE invoked when device is on\n",
+				__FUNCTION__));
+			bcmerror = BCME_SDIO_ERROR;
+		}
+	}
+	return bcmerror;
+}
diff --git a/drivers/net/wireless/bcm4329/dngl_stats.h b/drivers/net/wireless/bcm4329/dngl_stats.h
new file mode 100644
index 000000000000..e5db54e7edfe
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/dngl_stats.h
@@ -0,0 +1,43 @@
+/*
+ * Common stats definitions for clients of dongle
+ * ports
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dngl_stats.h,v 1.2.140.3 2008/05/26 16:52:08 Exp $
+ */
+
+#ifndef _dngl_stats_h_
+#define _dngl_stats_h_
+
+typedef struct {
+	unsigned long	rx_packets;		/* total packets received */
+	unsigned long	tx_packets;		/* total packets transmitted */
+	unsigned long	rx_bytes;		/* total bytes received */
+	unsigned long	tx_bytes;		/* total bytes transmitted */
+	unsigned long	rx_errors;		/* bad packets received */
+	unsigned long	tx_errors;		/* packet transmit problems */
+	unsigned long	rx_dropped;		/* packets dropped by dongle */
+	unsigned long	tx_dropped;		/* packets dropped by dongle */
+	unsigned long   multicast;      /* multicast packets received */
+} dngl_stats_t;
+
+#endif /* _dngl_stats_h_ */
diff --git a/drivers/net/wireless/bcm4329/hndpmu.c b/drivers/net/wireless/bcm4329/hndpmu.c
new file mode 100644
index 000000000000..307347a43bde
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/hndpmu.c
@@ -0,0 +1,131 @@
+/*
+ * Misc utility routines for accessing PMU corerev specific features
+ * of the SiliconBackplane-based Broadcom chips.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: hndpmu.c,v 1.95.2.17.4.11.2.63 2010/07/21 13:55:09 Exp $
+ */
+
+#include <typedefs.h>
+#include <bcmdefs.h>
+#include <osl.h>
+#include <bcmutils.h>
+#include <siutils.h>
+#include <bcmdevs.h>
+#include <hndsoc.h>
+#include <sbchipc.h>
+#include <hndpmu.h>
+
+/* debug/trace */
+#define	PMU_ERROR(args)
+
+#define	PMU_MSG(args)
+
+
+/* SDIO Pad drive strength to select value mappings */
+typedef struct {
+	uint8 strength;			/* Pad Drive Strength in mA */
+	uint8 sel;			/* Chip-specific select value */
+} sdiod_drive_str_t;
+
+/* SDIO Drive Strength to sel value table for PMU Rev 1 */
+static const sdiod_drive_str_t sdiod_drive_strength_tab1[] = {
+	{4, 0x2},
+	{2, 0x3},
+	{1, 0x0},
+	{0, 0x0} };
+
+/* SDIO Drive Strength to sel value table for PMU Rev 2, 3 */
+static const sdiod_drive_str_t sdiod_drive_strength_tab2[] = {
+	{12, 0x7},
+	{10, 0x6},
+	{8, 0x5},
+	{6, 0x4},
+	{4, 0x2},
+	{2, 0x1},
+	{0, 0x0} };
+
+#define SDIOD_DRVSTR_KEY(chip, pmu)	(((chip) << 16) | (pmu))
+
+void
+si_sdiod_drive_strength_init(si_t *sih, osl_t *osh, uint32 drivestrength)
+{
+	chipcregs_t *cc;
+	uint origidx, intr_val = 0;
+	sdiod_drive_str_t *str_tab = NULL;
+	uint32 str_mask = 0;
+	uint32 str_shift = 0;
+
+	if (!(sih->cccaps & CC_CAP_PMU)) {
+		return;
+	}
+
+	/* Remember original core before switch to chipc */
+	cc = (chipcregs_t *) si_switch_core(sih, CC_CORE_ID, &origidx, &intr_val);
+
+	switch (SDIOD_DRVSTR_KEY(sih->chip, sih->pmurev)) {
+	case SDIOD_DRVSTR_KEY(BCM4325_CHIP_ID, 1):
+		str_tab = (sdiod_drive_str_t *)&sdiod_drive_strength_tab1;
+		str_mask = 0x30000000;
+		str_shift = 28;
+		break;
+	case SDIOD_DRVSTR_KEY(BCM4325_CHIP_ID, 2):
+	case SDIOD_DRVSTR_KEY(BCM4325_CHIP_ID, 3):
+	case SDIOD_DRVSTR_KEY(BCM4315_CHIP_ID, 4):
+		str_tab = (sdiod_drive_str_t *)&sdiod_drive_strength_tab2;
+		str_mask = 0x00003800;
+		str_shift = 11;
+		break;
+
+	default:
+		PMU_MSG(("No SDIO Drive strength init done for chip %x rev %d pmurev %d\n",
+		         sih->chip, sih->chiprev, sih->pmurev));
+
+		break;
+	}
+
+	if (str_tab != NULL) {
+		uint32 drivestrength_sel = 0;
+		uint32 cc_data_temp;
+		int i;
+
+		for (i = 0; str_tab[i].strength != 0; i ++) {
+			if (drivestrength >= str_tab[i].strength) {
+				drivestrength_sel = str_tab[i].sel;
+				break;
+			}
+		}
+
+		W_REG(osh, &cc->chipcontrol_addr, 1);
+		cc_data_temp = R_REG(osh, &cc->chipcontrol_data);
+		cc_data_temp &= ~str_mask;
+		drivestrength_sel <<= str_shift;
+		cc_data_temp |= drivestrength_sel;
+		W_REG(osh, &cc->chipcontrol_data, cc_data_temp);
+
+		PMU_MSG(("SDIO: %dmA drive strength selected, set to 0x%08x\n",
+		         drivestrength, cc_data_temp));
+	}
+
+	/* Return to original core */
+	si_restore_core(sih, origidx, intr_val);
+}
diff --git a/drivers/net/wireless/bcm4329/include/Makefile b/drivers/net/wireless/bcm4329/include/Makefile
new file mode 100644
index 000000000000..439ead14a0e6
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/Makefile
@@ -0,0 +1,21 @@
+#
+# include/Makefile
+#
+# Copyright 2005, Broadcom, Inc.
+#
+# $Id: Makefile,v 13.5 2005/02/17 19:11:31 Exp $
+#
+
+SRCBASE	= ..
+
+TARGETS	= epivers.h
+
+
+all release:
+	bash epivers.sh
+
+clean:
+	rm -rf ${TARGETS} *.prev
+
+
+.PHONY: all release clean
diff --git a/drivers/net/wireless/bcm4329/include/aidmp.h b/drivers/net/wireless/bcm4329/include/aidmp.h
new file mode 100644
index 000000000000..a927e5dae586
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/aidmp.h
@@ -0,0 +1,368 @@
+/*
+ * Broadcom AMBA Interconnect definitions.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: aidmp.h,v 13.2.10.1 2008/05/07 20:32:12 Exp $
+ */
+
+
+#ifndef	_AIDMP_H
+#define	_AIDMP_H
+
+
+#define	MFGID_ARM		0x43b
+#define	MFGID_BRCM		0x4bf
+#define	MFGID_MIPS		0x4a7
+
+
+#define	CC_SIM			0
+#define	CC_EROM			1
+#define	CC_CORESIGHT		9
+#define	CC_VERIF		0xb
+#define	CC_OPTIMO		0xd
+#define	CC_GEN			0xe
+#define	CC_PRIMECELL		0xf
+
+
+#define	ER_EROMENTRY		0x000
+#define	ER_REMAPCONTROL		0xe00
+#define	ER_REMAPSELECT		0xe04
+#define	ER_MASTERSELECT		0xe10
+#define	ER_ITCR			0xf00
+#define	ER_ITIP			0xf04
+
+
+#define	ER_TAG			0xe
+#define	ER_TAG1			0x6
+#define	ER_VALID		1
+#define	ER_CI			0
+#define	ER_MP			2
+#define	ER_ADD			4
+#define	ER_END			0xe
+#define	ER_BAD			0xffffffff
+
+
+#define	CIA_MFG_MASK		0xfff00000
+#define	CIA_MFG_SHIFT		20
+#define	CIA_CID_MASK		0x000fff00
+#define	CIA_CID_SHIFT		8
+#define	CIA_CCL_MASK		0x000000f0
+#define	CIA_CCL_SHIFT		4
+
+
+#define	CIB_REV_MASK		0xff000000
+#define	CIB_REV_SHIFT		24
+#define	CIB_NSW_MASK		0x00f80000
+#define	CIB_NSW_SHIFT		19
+#define	CIB_NMW_MASK		0x0007c000
+#define	CIB_NMW_SHIFT		14
+#define	CIB_NSP_MASK		0x00003e00
+#define	CIB_NSP_SHIFT		9
+#define	CIB_NMP_MASK		0x000001f0
+#define	CIB_NMP_SHIFT		4
+
+
+#define	MPD_MUI_MASK		0x0000ff00
+#define	MPD_MUI_SHIFT		8
+#define	MPD_MP_MASK		0x000000f0
+#define	MPD_MP_SHIFT		4
+
+
+#define	AD_ADDR_MASK		0xfffff000
+#define	AD_SP_MASK		0x00000f00
+#define	AD_SP_SHIFT		8
+#define	AD_ST_MASK		0x000000c0
+#define	AD_ST_SHIFT		6
+#define	AD_ST_SLAVE		0x00000000
+#define	AD_ST_BRIDGE		0x00000040
+#define	AD_ST_SWRAP		0x00000080
+#define	AD_ST_MWRAP		0x000000c0
+#define	AD_SZ_MASK		0x00000030
+#define	AD_SZ_SHIFT		4
+#define	AD_SZ_4K		0x00000000
+#define	AD_SZ_8K		0x00000010
+#define	AD_SZ_16K		0x00000020
+#define	AD_SZ_SZD		0x00000030
+#define	AD_AG32			0x00000008
+#define	AD_ADDR_ALIGN		0x00000fff
+#define	AD_SZ_BASE		0x00001000	
+
+
+#define	SD_SZ_MASK		0xfffff000
+#define	SD_SG32			0x00000008
+#define	SD_SZ_ALIGN		0x00000fff
+
+
+#ifndef _LANGUAGE_ASSEMBLY
+
+typedef volatile struct _aidmp {
+	uint32	oobselina30;	
+	uint32	oobselina74;	
+	uint32	PAD[6];
+	uint32	oobselinb30;	
+	uint32	oobselinb74;	
+	uint32	PAD[6];
+	uint32	oobselinc30;	
+	uint32	oobselinc74;	
+	uint32	PAD[6];
+	uint32	oobselind30;	
+	uint32	oobselind74;	
+	uint32	PAD[38];
+	uint32	oobselouta30;	
+	uint32	oobselouta74;	
+	uint32	PAD[6];
+	uint32	oobseloutb30;	
+	uint32	oobseloutb74;	
+	uint32	PAD[6];
+	uint32	oobseloutc30;	
+	uint32	oobseloutc74;	
+	uint32	PAD[6];
+	uint32	oobseloutd30;	
+	uint32	oobseloutd74;	
+	uint32	PAD[38];
+	uint32	oobsynca;	
+	uint32	oobseloutaen;	
+	uint32	PAD[6];
+	uint32	oobsyncb;	
+	uint32	oobseloutben;	
+	uint32	PAD[6];
+	uint32	oobsyncc;	
+	uint32	oobseloutcen;	
+	uint32	PAD[6];
+	uint32	oobsyncd;	
+	uint32	oobseloutden;	
+	uint32	PAD[38];
+	uint32	oobaextwidth;	
+	uint32	oobainwidth;	
+	uint32	oobaoutwidth;	
+	uint32	PAD[5];
+	uint32	oobbextwidth;	
+	uint32	oobbinwidth;	
+	uint32	oobboutwidth;	
+	uint32	PAD[5];
+	uint32	oobcextwidth;	
+	uint32	oobcinwidth;	
+	uint32	oobcoutwidth;	
+	uint32	PAD[5];
+	uint32	oobdextwidth;	
+	uint32	oobdinwidth;	
+	uint32	oobdoutwidth;	
+	uint32	PAD[37];
+	uint32	ioctrlset;	
+	uint32	ioctrlclear;	
+	uint32	ioctrl;		
+	uint32	PAD[61];
+	uint32	iostatus;	
+	uint32	PAD[127];
+	uint32	ioctrlwidth;	
+	uint32	iostatuswidth;	
+	uint32	PAD[62];
+	uint32	resetctrl;	
+	uint32	resetstatus;	
+	uint32	resetreadid;	
+	uint32	resetwriteid;	
+	uint32	PAD[60];
+	uint32	errlogctrl;	
+	uint32	errlogdone;	
+	uint32	errlogstatus;	
+	uint32	errlogaddrlo;	
+	uint32	errlogaddrhi;	
+	uint32	errlogid;	
+	uint32	errloguser;	
+	uint32	errlogflags;	
+	uint32	PAD[56];
+	uint32	intstatus;	
+	uint32	PAD[127];
+	uint32	config;		
+	uint32	PAD[63];
+	uint32	itcr;		
+	uint32	PAD[3];
+	uint32	itipooba;	
+	uint32	itipoobb;	
+	uint32	itipoobc;	
+	uint32	itipoobd;	
+	uint32	PAD[4];
+	uint32	itipoobaout;	
+	uint32	itipoobbout;	
+	uint32	itipoobcout;	
+	uint32	itipoobdout;	
+	uint32	PAD[4];
+	uint32	itopooba;	
+	uint32	itopoobb;	
+	uint32	itopoobc;	
+	uint32	itopoobd;	
+	uint32	PAD[4];
+	uint32	itopoobain;	
+	uint32	itopoobbin;	
+	uint32	itopoobcin;	
+	uint32	itopoobdin;	
+	uint32	PAD[4];
+	uint32	itopreset;	
+	uint32	PAD[15];
+	uint32	peripherialid4;	
+	uint32	peripherialid5;	
+	uint32	peripherialid6;	
+	uint32	peripherialid7;	
+	uint32	peripherialid0;	
+	uint32	peripherialid1;	
+	uint32	peripherialid2;	
+	uint32	peripherialid3;	
+	uint32	componentid0;	
+	uint32	componentid1;	
+	uint32	componentid2;	
+	uint32	componentid3;	
+} aidmp_t;
+
+#endif 
+
+
+#define	OOB_BUSCONFIG		0x020
+#define	OOB_STATUSA		0x100
+#define	OOB_STATUSB		0x104
+#define	OOB_STATUSC		0x108
+#define	OOB_STATUSD		0x10c
+#define	OOB_ENABLEA0		0x200
+#define	OOB_ENABLEA1		0x204
+#define	OOB_ENABLEA2		0x208
+#define	OOB_ENABLEA3		0x20c
+#define	OOB_ENABLEB0		0x280
+#define	OOB_ENABLEB1		0x284
+#define	OOB_ENABLEB2		0x288
+#define	OOB_ENABLEB3		0x28c
+#define	OOB_ENABLEC0		0x300
+#define	OOB_ENABLEC1		0x304
+#define	OOB_ENABLEC2		0x308
+#define	OOB_ENABLEC3		0x30c
+#define	OOB_ENABLED0		0x380
+#define	OOB_ENABLED1		0x384
+#define	OOB_ENABLED2		0x388
+#define	OOB_ENABLED3		0x38c
+#define	OOB_ITCR		0xf00
+#define	OOB_ITIPOOBA		0xf10
+#define	OOB_ITIPOOBB		0xf14
+#define	OOB_ITIPOOBC		0xf18
+#define	OOB_ITIPOOBD		0xf1c
+#define	OOB_ITOPOOBA		0xf30
+#define	OOB_ITOPOOBB		0xf34
+#define	OOB_ITOPOOBC		0xf38
+#define	OOB_ITOPOOBD		0xf3c
+
+
+#define	AI_OOBSELINA30		0x000
+#define	AI_OOBSELINA74		0x004
+#define	AI_OOBSELINB30		0x020
+#define	AI_OOBSELINB74		0x024
+#define	AI_OOBSELINC30		0x040
+#define	AI_OOBSELINC74		0x044
+#define	AI_OOBSELIND30		0x060
+#define	AI_OOBSELIND74		0x064
+#define	AI_OOBSELOUTA30		0x100
+#define	AI_OOBSELOUTA74		0x104
+#define	AI_OOBSELOUTB30		0x120
+#define	AI_OOBSELOUTB74		0x124
+#define	AI_OOBSELOUTC30		0x140
+#define	AI_OOBSELOUTC74		0x144
+#define	AI_OOBSELOUTD30		0x160
+#define	AI_OOBSELOUTD74		0x164
+#define	AI_OOBSYNCA		0x200
+#define	AI_OOBSELOUTAEN		0x204
+#define	AI_OOBSYNCB		0x220
+#define	AI_OOBSELOUTBEN		0x224
+#define	AI_OOBSYNCC		0x240
+#define	AI_OOBSELOUTCEN		0x244
+#define	AI_OOBSYNCD		0x260
+#define	AI_OOBSELOUTDEN		0x264
+#define	AI_OOBAEXTWIDTH		0x300
+#define	AI_OOBAINWIDTH		0x304
+#define	AI_OOBAOUTWIDTH		0x308
+#define	AI_OOBBEXTWIDTH		0x320
+#define	AI_OOBBINWIDTH		0x324
+#define	AI_OOBBOUTWIDTH		0x328
+#define	AI_OOBCEXTWIDTH		0x340
+#define	AI_OOBCINWIDTH		0x344
+#define	AI_OOBCOUTWIDTH		0x348
+#define	AI_OOBDEXTWIDTH		0x360
+#define	AI_OOBDINWIDTH		0x364
+#define	AI_OOBDOUTWIDTH		0x368
+#define	AI_IOCTRLSET		0x400
+#define	AI_IOCTRLCLEAR		0x404
+#define	AI_IOCTRL		0x408
+#define	AI_IOSTATUS		0x500
+#define	AI_IOCTRLWIDTH		0x700
+#define	AI_IOSTATUSWIDTH	0x704
+#define	AI_RESETCTRL		0x800
+#define	AI_RESETSTATUS		0x804
+#define	AI_RESETREADID		0x808
+#define	AI_RESETWRITEID		0x80c
+#define	AI_ERRLOGCTRL		0xa00
+#define	AI_ERRLOGDONE		0xa04
+#define	AI_ERRLOGSTATUS		0xa08
+#define	AI_ERRLOGADDRLO		0xa0c
+#define	AI_ERRLOGADDRHI		0xa10
+#define	AI_ERRLOGID		0xa14
+#define	AI_ERRLOGUSER		0xa18
+#define	AI_ERRLOGFLAGS		0xa1c
+#define	AI_INTSTATUS		0xa00
+#define	AI_CONFIG		0xe00
+#define	AI_ITCR			0xf00
+#define	AI_ITIPOOBA		0xf10
+#define	AI_ITIPOOBB		0xf14
+#define	AI_ITIPOOBC		0xf18
+#define	AI_ITIPOOBD		0xf1c
+#define	AI_ITIPOOBAOUT		0xf30
+#define	AI_ITIPOOBBOUT		0xf34
+#define	AI_ITIPOOBCOUT		0xf38
+#define	AI_ITIPOOBDOUT		0xf3c
+#define	AI_ITOPOOBA		0xf50
+#define	AI_ITOPOOBB		0xf54
+#define	AI_ITOPOOBC		0xf58
+#define	AI_ITOPOOBD		0xf5c
+#define	AI_ITOPOOBAIN		0xf70
+#define	AI_ITOPOOBBIN		0xf74
+#define	AI_ITOPOOBCIN		0xf78
+#define	AI_ITOPOOBDIN		0xf7c
+#define	AI_ITOPRESET		0xf90
+#define	AI_PERIPHERIALID4	0xfd0
+#define	AI_PERIPHERIALID5	0xfd4
+#define	AI_PERIPHERIALID6	0xfd8
+#define	AI_PERIPHERIALID7	0xfdc
+#define	AI_PERIPHERIALID0	0xfe0
+#define	AI_PERIPHERIALID1	0xfe4
+#define	AI_PERIPHERIALID2	0xfe8
+#define	AI_PERIPHERIALID3	0xfec
+#define	AI_COMPONENTID0		0xff0
+#define	AI_COMPONENTID1		0xff4
+#define	AI_COMPONENTID2		0xff8
+#define	AI_COMPONENTID3		0xffc
+
+
+#define	AIRC_RESET		1
+
+
+#define	AICFG_OOB		0x00000020
+#define	AICFG_IOS		0x00000010
+#define	AICFG_IOC		0x00000008
+#define	AICFG_TO		0x00000004
+#define	AICFG_ERRL		0x00000002
+#define	AICFG_RST		0x00000001
+
+#endif	
diff --git a/drivers/net/wireless/bcm4329/include/bcmcdc.h b/drivers/net/wireless/bcm4329/include/bcmcdc.h
new file mode 100644
index 000000000000..c2a860beab24
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmcdc.h
@@ -0,0 +1,100 @@
+/*
+ * CDC network driver ioctl/indication encoding
+ * Broadcom 802.11abg Networking Device Driver
+ *
+ * Definitions subject to change without notice.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmcdc.h,v 13.14.16.3.16.4 2009/04/12 16:58:45 Exp $
+ */
+#include <proto/ethernet.h>
+
+typedef struct cdc_ioctl {
+	uint32 cmd;      /* ioctl command value */
+	uint32 len;      /* lower 16: output buflen; upper 16: input buflen (excludes header) */
+	uint32 flags;    /* flag defns given below */
+	uint32 status;   /* status code returned from the device */
+} cdc_ioctl_t;
+
+/* Max valid buffer size that can be sent to the dongle */
+#define CDC_MAX_MSG_SIZE   ETHER_MAX_LEN
+
+/* len field is divided into input and output buffer lengths */
+#define CDCL_IOC_OUTLEN_MASK   0x0000FFFF  /* maximum or expected response length, */
+					   /* excluding IOCTL header */
+#define CDCL_IOC_OUTLEN_SHIFT  0
+#define CDCL_IOC_INLEN_MASK    0xFFFF0000   /* input buffer length, excluding IOCTL header */
+#define CDCL_IOC_INLEN_SHIFT   16
+
+/* CDC flag definitions */
+#define CDCF_IOC_ERROR		0x01	/* 0=success, 1=ioctl cmd failed */
+#define CDCF_IOC_SET		0x02	/* 0=get, 1=set cmd */
+#define CDCF_IOC_IF_MASK	0xF000	/* I/F index */
+#define CDCF_IOC_IF_SHIFT	12
+#define CDCF_IOC_ID_MASK	0xFFFF0000	/* used to uniquely id an ioctl req/resp pairing */
+#define CDCF_IOC_ID_SHIFT	16		/* # of bits of shift for ID Mask */
+
+#define CDC_IOC_IF_IDX(flags)	(((flags) & CDCF_IOC_IF_MASK) >> CDCF_IOC_IF_SHIFT)
+#define CDC_IOC_ID(flags)	(((flags) & CDCF_IOC_ID_MASK) >> CDCF_IOC_ID_SHIFT)
+
+#define CDC_GET_IF_IDX(hdr) \
+	((int)((((hdr)->flags) & CDCF_IOC_IF_MASK) >> CDCF_IOC_IF_SHIFT))
+#define CDC_SET_IF_IDX(hdr, idx) \
+	((hdr)->flags = (((hdr)->flags & ~CDCF_IOC_IF_MASK) | ((idx) << CDCF_IOC_IF_SHIFT)))
+
+/*
+ * BDC header
+ *
+ *   The BDC header is used on data packets to convey priority across USB.
+ */
+
+#define	BDC_HEADER_LEN		4
+
+#define BDC_PROTO_VER		1	/* Protocol version */
+
+#define BDC_FLAG_VER_MASK	0xf0	/* Protocol version mask */
+#define BDC_FLAG_VER_SHIFT	4	/* Protocol version shift */
+
+#define BDC_FLAG__UNUSED	0x03	/* Unassigned */
+#define BDC_FLAG_SUM_GOOD	0x04	/* Dongle has verified good RX checksums */
+#define BDC_FLAG_SUM_NEEDED	0x08	/* Dongle needs to do TX checksums */
+
+#define BDC_PRIORITY_MASK	0x7
+
+#define BDC_FLAG2_FC_FLAG	0x10	/* flag to indicate if pkt contains */
+									/* FLOW CONTROL info only */
+#define BDC_PRIORITY_FC_SHIFT	4		/* flow control info shift */
+
+#define BDC_FLAG2_IF_MASK	0x0f	/* APSTA: interface on which the packet was received */
+#define BDC_FLAG2_IF_SHIFT	0
+
+#define BDC_GET_IF_IDX(hdr) \
+	((int)((((hdr)->flags2) & BDC_FLAG2_IF_MASK) >> BDC_FLAG2_IF_SHIFT))
+#define BDC_SET_IF_IDX(hdr, idx) \
+	((hdr)->flags2 = (((hdr)->flags2 & ~BDC_FLAG2_IF_MASK) | ((idx) << BDC_FLAG2_IF_SHIFT)))
+
+struct bdc_header {
+	uint8	flags;			/* Flags */
+	uint8	priority;	/* 802.1d Priority 0:2 bits, 4:7 flow control info for usb */
+	uint8	flags2;
+	uint8	rssi;
+};
diff --git a/drivers/net/wireless/bcm4329/include/bcmdefs.h b/drivers/net/wireless/bcm4329/include/bcmdefs.h
new file mode 100644
index 000000000000..f4e99461971b
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmdefs.h
@@ -0,0 +1,114 @@
+/*
+ * Misc system wide definitions
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: bcmdefs.h,v 13.38.4.10.2.7.6.11 2010/02/01 05:51:55 Exp $
+ */
+
+
+#ifndef	_bcmdefs_h_
+#define	_bcmdefs_h_
+
+#define STATIC	static
+
+#define	SI_BUS			0	
+#define	PCI_BUS			1	
+#define	PCMCIA_BUS		2	
+#define SDIO_BUS		3	
+#define JTAG_BUS		4	
+#define USB_BUS			5	
+#define SPI_BUS			6	
+
+
+#ifdef BCMBUSTYPE
+#define BUSTYPE(bus) 	(BCMBUSTYPE)
+#else
+#define BUSTYPE(bus) 	(bus)
+#endif
+
+
+#ifdef BCMCHIPTYPE
+#define CHIPTYPE(bus) 	(BCMCHIPTYPE)
+#else
+#define CHIPTYPE(bus) 	(bus)
+#endif
+
+
+
+#if defined(BCMSPROMBUS)
+#define SPROMBUS	(BCMSPROMBUS)
+#elif defined(SI_PCMCIA_SROM)
+#define SPROMBUS	(PCMCIA_BUS)
+#else
+#define SPROMBUS	(PCI_BUS)
+#endif
+
+
+#ifdef BCMCHIPID
+#define CHIPID(chip)	(BCMCHIPID)
+#else
+#define CHIPID(chip)	(chip)
+#endif
+
+
+#define DMADDR_MASK_32 0x0		
+#define DMADDR_MASK_30 0xc0000000	
+#define DMADDR_MASK_0  0xffffffff	
+
+#define	DMADDRWIDTH_30  30 
+#define	DMADDRWIDTH_32  32 
+#define	DMADDRWIDTH_63  63 
+#define	DMADDRWIDTH_64  64 
+
+
+#define BCMEXTRAHDROOM 164
+
+
+#define BCMDONGLEHDRSZ 12
+#define BCMDONGLEPADSZ 16
+
+#define BCMDONGLEOVERHEAD	(BCMDONGLEHDRSZ + BCMDONGLEPADSZ)
+
+
+
+#define BITFIELD_MASK(width) \
+		(((unsigned)1 << (width)) - 1)
+#define GFIELD(val, field) \
+		(((val) >> field ## _S) & field ## _M)
+#define SFIELD(val, field, bits) \
+		(((val) & (~(field ## _M << field ## _S))) | \
+		 ((unsigned)(bits) << field ## _S))
+
+
+#ifdef BCMSMALL
+#undef	BCMSPACE
+#define bcmspace	FALSE	
+#else
+#define	BCMSPACE
+#define bcmspace	TRUE	
+#endif
+
+
+#define	MAXSZ_NVRAM_VARS	4096
+
+#define LOCATOR_EXTERN static
+
+#endif 
diff --git a/drivers/net/wireless/bcm4329/include/bcmdevs.h b/drivers/net/wireless/bcm4329/include/bcmdevs.h
new file mode 100644
index 000000000000..14853f17795c
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmdevs.h
@@ -0,0 +1,124 @@
+/*
+ * Broadcom device-specific manifest constants.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmdevs.h,v 13.172.4.5.4.10.2.36 2010/05/25 08:33:44 Exp $
+ */
+
+
+#ifndef	_BCMDEVS_H
+#define	_BCMDEVS_H
+
+
+#define	VENDOR_EPIGRAM		0xfeda
+#define	VENDOR_BROADCOM		0x14e4
+#define VENDOR_SI_IMAGE		0x1095		
+#define VENDOR_TI		0x104c		
+#define VENDOR_RICOH		0x1180		
+#define VENDOR_JMICRON		0x197b
+
+
+#define	VENDOR_BROADCOM_PCMCIA	0x02d0
+
+
+#define	VENDOR_BROADCOM_SDIO	0x00BF
+
+
+#define BCM_DNGL_VID            0xa5c
+#define BCM_DNGL_BL_PID_4320    0xbd11
+#define BCM_DNGL_BL_PID_4328    0xbd12
+#define BCM_DNGL_BL_PID_4322    0xbd13
+#define BCM_DNGL_BL_PID_4325    0xbd14
+#define BCM_DNGL_BL_PID_4315    0xbd15
+#define BCM_DNGL_BL_PID_4319    0xbd16
+#define BCM_DNGL_BDC_PID        0xbdc
+
+#define	BCM4325_D11DUAL_ID	0x431b		
+#define	BCM4325_D11G_ID		0x431c		
+#define	BCM4325_D11A_ID		0x431d		
+#define BCM4329_D11NDUAL_ID	0x432e		
+#define BCM4329_D11N2G_ID	0x432f		
+#define BCM4329_D11N5G_ID	0x4330		
+#define BCM4336_D11N_ID		0x4343		
+#define	BCM4315_D11DUAL_ID	0x4334		
+#define	BCM4315_D11G_ID		0x4335		
+#define	BCM4315_D11A_ID		0x4336		
+#define BCM4319_D11N_ID		0x4337		
+#define BCM4319_D11N2G_ID	0x4338		
+#define BCM4319_D11N5G_ID	0x4339		
+
+
+#define SDIOH_FPGA_ID		0x43f2		
+#define SPIH_FPGA_ID		0x43f5		
+#define	BCM4710_DEVICE_ID	0x4710		
+#define BCM27XX_SDIOH_ID	0x2702		
+#define PCIXX21_FLASHMEDIA0_ID	0x8033		
+#define PCIXX21_SDIOH0_ID	0x8034		
+#define PCIXX21_FLASHMEDIA_ID	0x803b		
+#define PCIXX21_SDIOH_ID	0x803c		
+#define R5C822_SDIOH_ID		0x0822		
+#define JMICRON_SDIOH_ID	0x2381		
+
+
+#define	BCM4306_CHIP_ID		0x4306		
+#define	BCM4311_CHIP_ID		0x4311		
+#define	BCM4312_CHIP_ID		0x4312		
+#define	BCM4315_CHIP_ID		0x4315		
+#define	BCM4318_CHIP_ID		0x4318		
+#define	BCM4319_CHIP_ID		0x4319		
+#define	BCM4320_CHIP_ID		0x4320		
+#define	BCM4321_CHIP_ID		0x4321		
+#define	BCM4322_CHIP_ID		0x4322		
+#define	BCM4325_CHIP_ID		0x4325		
+#define	BCM4328_CHIP_ID		0x4328		
+#define	BCM4329_CHIP_ID		0x4329		
+#define BCM4336_CHIP_ID		0x4336		
+#define	BCM4402_CHIP_ID		0x4402		
+#define	BCM4704_CHIP_ID		0x4704		
+#define	BCM4710_CHIP_ID		0x4710		
+#define	BCM4712_CHIP_ID		0x4712		
+#define BCM4785_CHIP_ID		0x4785		
+#define	BCM5350_CHIP_ID		0x5350		
+#define	BCM5352_CHIP_ID		0x5352		
+#define	BCM5354_CHIP_ID		0x5354		
+#define BCM5365_CHIP_ID		0x5365          
+
+
+
+#define	BCM4303_PKG_ID		2		
+#define	BCM4309_PKG_ID		1		
+#define	BCM4712LARGE_PKG_ID	0		
+#define	BCM4712SMALL_PKG_ID	1		
+#define	BCM4712MID_PKG_ID	2		
+#define BCM4328USBD11G_PKG_ID	2		
+#define BCM4328USBDUAL_PKG_ID	3		
+#define BCM4328SDIOD11G_PKG_ID	4		
+#define BCM4328SDIODUAL_PKG_ID	5		
+#define BCM4329_289PIN_PKG_ID	0		
+#define BCM4329_182PIN_PKG_ID	1		
+#define BCM5354E_PKG_ID		1		
+#define HDLSIM5350_PKG_ID	1		
+#define HDLSIM_PKG_ID		14		
+#define HWSIM_PKG_ID		15		
+
+
+#endif 
diff --git a/drivers/net/wireless/bcm4329/include/bcmendian.h b/drivers/net/wireless/bcm4329/include/bcmendian.h
new file mode 100644
index 000000000000..ae468383aa74
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmendian.h
@@ -0,0 +1,205 @@
+/*
+ * Byte order utilities
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ *  $Id: bcmendian.h,v 1.31.302.1.16.1 2009/02/03 18:34:31 Exp $
+ *
+ * This file by default provides proper behavior on little-endian architectures.
+ * On big-endian architectures, IL_BIGENDIAN should be defined.
+ */
+
+
+#ifndef _BCMENDIAN_H_
+#define _BCMENDIAN_H_
+
+#include <typedefs.h>
+
+
+#define BCMSWAP16(val) \
+	((uint16)((((uint16)(val) & (uint16)0x00ffU) << 8) | \
+		  (((uint16)(val) & (uint16)0xff00U) >> 8)))
+
+
+#define BCMSWAP32(val) \
+	((uint32)((((uint32)(val) & (uint32)0x000000ffU) << 24) | \
+		  (((uint32)(val) & (uint32)0x0000ff00U) <<  8) | \
+		  (((uint32)(val) & (uint32)0x00ff0000U) >>  8) | \
+		  (((uint32)(val) & (uint32)0xff000000U) >> 24)))
+
+
+#define BCMSWAP32BY16(val) \
+	((uint32)((((uint32)(val) & (uint32)0x0000ffffU) << 16) | \
+		  (((uint32)(val) & (uint32)0xffff0000U) >> 16)))
+
+
+static INLINE uint16
+bcmswap16(uint16 val)
+{
+	return BCMSWAP16(val);
+}
+
+static INLINE uint32
+bcmswap32(uint32 val)
+{
+	return BCMSWAP32(val);
+}
+
+static INLINE uint32
+bcmswap32by16(uint32 val)
+{
+	return BCMSWAP32BY16(val);
+}
+
+
+
+
+static INLINE void
+bcmswap16_buf(uint16 *buf, uint len)
+{
+	len = len / 2;
+
+	while (len--) {
+		*buf = bcmswap16(*buf);
+		buf++;
+	}
+}
+
+#ifndef hton16
+#ifndef IL_BIGENDIAN
+#define HTON16(i) BCMSWAP16(i)
+#define HTON32(i) BCMSWAP32(i)
+#define	hton16(i) bcmswap16(i)
+#define	hton32(i) bcmswap32(i)
+#define	ntoh16(i) bcmswap16(i)
+#define	ntoh32(i) bcmswap32(i)
+#define HTOL16(i) (i)
+#define HTOL32(i) (i)
+#define ltoh16(i) (i)
+#define ltoh32(i) (i)
+#define htol16(i) (i)
+#define htol32(i) (i)
+#else
+#define HTON16(i) (i)
+#define HTON32(i) (i)
+#define	hton16(i) (i)
+#define	hton32(i) (i)
+#define	ntoh16(i) (i)
+#define	ntoh32(i) (i)
+#define HTOL16(i) BCMSWAP16(i)
+#define HTOL32(i) BCMSWAP32(i)
+#define	ltoh16(i) bcmswap16(i)
+#define	ltoh32(i) bcmswap32(i)
+#define htol16(i) bcmswap16(i)
+#define htol32(i) bcmswap32(i)
+#endif 
+#endif 
+
+#ifndef IL_BIGENDIAN
+#define ltoh16_buf(buf, i)
+#define htol16_buf(buf, i)
+#else
+#define ltoh16_buf(buf, i) bcmswap16_buf((uint16 *)buf, i)
+#define htol16_buf(buf, i) bcmswap16_buf((uint16 *)buf, i)
+#endif 
+
+
+static INLINE void
+htol16_ua_store(uint16 val, uint8 *bytes)
+{
+	bytes[0] = val & 0xff;
+	bytes[1] = val >> 8;
+}
+
+
+static INLINE void
+htol32_ua_store(uint32 val, uint8 *bytes)
+{
+	bytes[0] = val & 0xff;
+	bytes[1] = (val >> 8) & 0xff;
+	bytes[2] = (val >> 16) & 0xff;
+	bytes[3] = val >> 24;
+}
+
+
+static INLINE void
+hton16_ua_store(uint16 val, uint8 *bytes)
+{
+	bytes[0] = val >> 8;
+	bytes[1] = val & 0xff;
+}
+
+
+static INLINE void
+hton32_ua_store(uint32 val, uint8 *bytes)
+{
+	bytes[0] = val >> 24;
+	bytes[1] = (val >> 16) & 0xff;
+	bytes[2] = (val >> 8) & 0xff;
+	bytes[3] = val & 0xff;
+}
+
+#define _LTOH16_UA(cp)	((cp)[0] | ((cp)[1] << 8))
+#define _LTOH32_UA(cp)	((cp)[0] | ((cp)[1] << 8) | ((cp)[2] << 16) | ((cp)[3] << 24))
+#define _NTOH16_UA(cp)	(((cp)[0] << 8) | (cp)[1])
+#define _NTOH32_UA(cp)	(((cp)[0] << 24) | ((cp)[1] << 16) | ((cp)[2] << 8) | (cp)[3])
+
+
+static INLINE uint16
+ltoh16_ua(const void *bytes)
+{
+	return _LTOH16_UA((const uint8 *)bytes);
+}
+
+
+static INLINE uint32
+ltoh32_ua(const void *bytes)
+{
+	return _LTOH32_UA((const uint8 *)bytes);
+}
+
+
+static INLINE uint16
+ntoh16_ua(const void *bytes)
+{
+	return _NTOH16_UA((const uint8 *)bytes);
+}
+
+
+static INLINE uint32
+ntoh32_ua(const void *bytes)
+{
+	return _NTOH32_UA((const uint8 *)bytes);
+}
+
+#define ltoh_ua(ptr) \
+	(sizeof(*(ptr)) == sizeof(uint8) ? *(const uint8 *)ptr : \
+	 sizeof(*(ptr)) == sizeof(uint16) ? _LTOH16_UA((const uint8 *)ptr) : \
+	 sizeof(*(ptr)) == sizeof(uint32) ? _LTOH32_UA((const uint8 *)ptr) : \
+	 0xfeedf00d)
+
+#define ntoh_ua(ptr) \
+	(sizeof(*(ptr)) == sizeof(uint8) ? *(const uint8 *)ptr : \
+	 sizeof(*(ptr)) == sizeof(uint16) ? _NTOH16_UA((const uint8 *)ptr) : \
+	 sizeof(*(ptr)) == sizeof(uint32) ? _NTOH32_UA((const uint8 *)ptr) : \
+	 0xfeedf00d)
+
+#endif 
diff --git a/drivers/net/wireless/bcm4329/include/bcmpcispi.h b/drivers/net/wireless/bcm4329/include/bcmpcispi.h
new file mode 100644
index 000000000000..7d98fb7cbdc8
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmpcispi.h
@@ -0,0 +1,205 @@
+/*
+ * Broadcom PCI-SPI Host Controller Register Definitions
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmpcispi.h,v 13.11.8.3 2008/07/09 21:23:29 Exp $
+ */
+
+/* cpp contortions to concatenate w/arg prescan */
+#ifndef PAD
+#define	_PADLINE(line)	pad ## line
+#define	_XSTR(line)	_PADLINE(line)
+#define	PAD		_XSTR(__LINE__)
+#endif	/* PAD */
+
+/*
++---------------------------------------------------------------------------+
+|                                                                           |
+|                     7     6     5     4     3     2     1       0         |
+| 0x0000  SPI_CTRL    SPIE  SPE   0     MSTR  CPOL  CPHA  SPR1    SPR0      |
+| 0x0004  SPI_STAT    SPIF  WCOL  ST1   ST0   WFFUL WFEMP RFFUL   RFEMP     |
+| 0x0008  SPI_DATA    Bits 31:0, data to send out on MOSI                   |
+| 0x000C  SPI_EXT     ICNT1 ICNT0 BSWAP *HSMODE           ESPR1   ESPR0     |
+| 0x0020  GPIO_OE     0=input, 1=output                   PWR_OE  CS_OE     |
+| 0x0024  GPIO_DATA   CARD:1=missing, 0=present     CARD  PWR_DAT CS_DAT    |
+| 0x0040  INT_EDGE    0=level, 1=edge                     DEV_E   SPI_E     |
+| 0x0044  INT_POL     1=active high, 0=active low         DEV_P   SPI_P     |
+| 0x0048  INTMASK                                         DEV     SPI       |
+| 0x004C  INTSTATUS                                       DEV     SPI       |
+| 0x0060  HEXDISP     Reset value: 0x14e443f5.  In hexdisp mode, value      |
+|                     shows on the Raggedstone1 4-digit 7-segment display.  |
+| 0x0064  CURRENT_MA  Low 16 bits indicate card current consumption in mA   |
+| 0x006C  DISP_SEL    Display mode (0=hexdisp, 1=current)         DSP       |
+| 0x00C0  PLL_CTL  bit31=ext_clk, remainder unused.                         |
+| 0x00C4  PLL_STAT                            LOCK                          |
+| 0x00C8  CLK_FREQ                                                          |
+| 0x00CC  CLK_CNT                                                           |
+|                                                                           |
+| *Notes: HSMODE is not implemented, never set this bit!                    |
+| BSWAP is available in rev >= 8                                            |
+|                                                                           |
++---------------------------------------------------------------------------+
+*/
+
+typedef volatile struct {
+	uint32 spih_ctrl;		/* 0x00 SPI Control Register */
+	uint32 spih_stat;		/* 0x04 SPI Status Register */
+	uint32 spih_data;		/* 0x08 SPI Data Register, 32-bits wide */
+	uint32 spih_ext;		/* 0x0C SPI Extension Register */
+	uint32 PAD[4];			/* 0x10-0x1F PADDING */
+
+	uint32 spih_gpio_ctrl;		/* 0x20 SPI GPIO Control Register */
+	uint32 spih_gpio_data;		/* 0x24 SPI GPIO Data Register */
+	uint32 PAD[6];			/* 0x28-0x3F PADDING */
+
+	uint32 spih_int_edge;		/* 0x40 SPI Interrupt Edge Register (0=Level, 1=Edge) */
+	uint32 spih_int_pol;		/* 0x44 SPI Interrupt Polarity Register (0=Active Low, */
+							/* 1=Active High) */
+	uint32 spih_int_mask;		/* 0x48 SPI Interrupt Mask */
+	uint32 spih_int_status;		/* 0x4C SPI Interrupt Status */
+	uint32 PAD[4];			/* 0x50-0x5F PADDING */
+
+	uint32 spih_hex_disp;		/* 0x60 SPI 4-digit hex display value */
+	uint32 spih_current_ma;		/* 0x64 SPI SD card current consumption in mA */
+	uint32 PAD[1];			/* 0x68 PADDING */
+	uint32 spih_disp_sel;		/* 0x6c SPI 4-digit hex display mode select (1=current) */
+	uint32 PAD[4];			/* 0x70-0x7F PADDING */
+	uint32 PAD[8];			/* 0x80-0x9F PADDING */
+	uint32 PAD[8];			/* 0xA0-0xBF PADDING */
+	uint32 spih_pll_ctrl;	/* 0xC0 PLL Control Register */
+	uint32 spih_pll_status;	/* 0xC4 PLL Status Register */
+	uint32 spih_xtal_freq;	/* 0xC8 External Clock Frequency in units of 10000Hz */
+	uint32 spih_clk_count;	/* 0xCC External Clock Count Register */
+
+} spih_regs_t;
+
+typedef volatile struct {
+	uint32 cfg_space[0x40];		/* 0x000-0x0FF PCI Configuration Space (Read Only) */
+	uint32 P_IMG_CTRL0;		/* 0x100 PCI Image0 Control Register */
+
+	uint32 P_BA0;			/* 0x104 32 R/W PCI Image0 Base Address register */
+	uint32 P_AM0;			/* 0x108 32 R/W PCI Image0 Address Mask register */
+	uint32 P_TA0;			/* 0x10C 32 R/W PCI Image0 Translation Address register */
+	uint32 P_IMG_CTRL1;		/* 0x110 32 R/W PCI Image1 Control register */
+	uint32 P_BA1;			/* 0x114 32 R/W PCI Image1 Base Address register */
+	uint32 P_AM1;			/* 0x118 32 R/W PCI Image1 Address Mask register */
+	uint32 P_TA1;			/* 0x11C 32 R/W PCI Image1 Translation Address register */
+	uint32 P_IMG_CTRL2;		/* 0x120 32 R/W PCI Image2 Control register */
+	uint32 P_BA2;			/* 0x124 32 R/W PCI Image2 Base Address register */
+	uint32 P_AM2;			/* 0x128 32 R/W PCI Image2 Address Mask register */
+	uint32 P_TA2;			/* 0x12C 32 R/W PCI Image2 Translation Address register */
+	uint32 P_IMG_CTRL3;		/* 0x130 32 R/W PCI Image3 Control register */
+	uint32 P_BA3;			/* 0x134 32 R/W PCI Image3 Base Address register */
+	uint32 P_AM3;			/* 0x138 32 R/W PCI Image3 Address Mask register */
+	uint32 P_TA3;			/* 0x13C 32 R/W PCI Image3 Translation Address register */
+	uint32 P_IMG_CTRL4;		/* 0x140 32 R/W PCI Image4 Control register */
+	uint32 P_BA4;			/* 0x144 32 R/W PCI Image4 Base Address register */
+	uint32 P_AM4;			/* 0x148 32 R/W PCI Image4 Address Mask register */
+	uint32 P_TA4;			/* 0x14C 32 R/W PCI Image4 Translation Address register */
+	uint32 P_IMG_CTRL5;		/* 0x150 32 R/W PCI Image5 Control register */
+	uint32 P_BA5;			/* 0x154 32 R/W PCI Image5 Base Address register */
+	uint32 P_AM5;			/* 0x158 32 R/W PCI Image5 Address Mask register */
+	uint32 P_TA5;			/* 0x15C 32 R/W PCI Image5 Translation Address register */
+	uint32 P_ERR_CS;		/* 0x160 32 R/W PCI Error Control and Status register */
+	uint32 P_ERR_ADDR;		/* 0x164 32 R PCI Erroneous Address register */
+	uint32 P_ERR_DATA;		/* 0x168 32 R PCI Erroneous Data register */
+
+	uint32 PAD[5];			/* 0x16C-0x17F PADDING */
+
+	uint32 WB_CONF_SPC_BAR;		/* 0x180 32 R WISHBONE Configuration Space Base Address */
+	uint32 W_IMG_CTRL1;		/* 0x184 32 R/W WISHBONE Image1 Control register */
+	uint32 W_BA1;			/* 0x188 32 R/W WISHBONE Image1 Base Address register */
+	uint32 W_AM1;			/* 0x18C 32 R/W WISHBONE Image1 Address Mask register */
+	uint32 W_TA1;			/* 0x190 32 R/W WISHBONE Image1 Translation Address reg */
+	uint32 W_IMG_CTRL2;		/* 0x194 32 R/W WISHBONE Image2 Control register */
+	uint32 W_BA2;			/* 0x198 32 R/W WISHBONE Image2 Base Address register */
+	uint32 W_AM2;			/* 0x19C 32 R/W WISHBONE Image2 Address Mask register */
+	uint32 W_TA2;			/* 0x1A0 32 R/W WISHBONE Image2 Translation Address reg */
+	uint32 W_IMG_CTRL3;		/* 0x1A4 32 R/W WISHBONE Image3 Control register */
+	uint32 W_BA3;			/* 0x1A8 32 R/W WISHBONE Image3 Base Address register */
+	uint32 W_AM3;			/* 0x1AC 32 R/W WISHBONE Image3 Address Mask register */
+	uint32 W_TA3;			/* 0x1B0 32 R/W WISHBONE Image3 Translation Address reg */
+	uint32 W_IMG_CTRL4;		/* 0x1B4 32 R/W WISHBONE Image4 Control register */
+	uint32 W_BA4;			/* 0x1B8 32 R/W WISHBONE Image4 Base Address register */
+	uint32 W_AM4;			/* 0x1BC 32 R/W WISHBONE Image4 Address Mask register */
+	uint32 W_TA4;			/* 0x1C0 32 R/W WISHBONE Image4 Translation Address reg */
+	uint32 W_IMG_CTRL5;		/* 0x1C4 32 R/W WISHBONE Image5 Control register */
+	uint32 W_BA5;			/* 0x1C8 32 R/W WISHBONE Image5 Base Address register */
+	uint32 W_AM5;			/* 0x1CC 32 R/W WISHBONE Image5 Address Mask register */
+	uint32 W_TA5;			/* 0x1D0 32 R/W WISHBONE Image5 Translation Address reg */
+	uint32 W_ERR_CS;		/* 0x1D4 32 R/W WISHBONE Error Control and Status reg */
+	uint32 W_ERR_ADDR;		/* 0x1D8 32 R WISHBONE Erroneous Address register */
+	uint32 W_ERR_DATA;		/* 0x1DC 32 R WISHBONE Erroneous Data register */
+	uint32 CNF_ADDR;		/* 0x1E0 32 R/W Configuration Cycle register */
+	uint32 CNF_DATA;		/* 0x1E4 32 R/W Configuration Cycle Generation Data reg */
+
+	uint32 INT_ACK;			/* 0x1E8 32 R Interrupt Acknowledge register */
+	uint32 ICR;			/* 0x1EC 32 R/W Interrupt Control register */
+	uint32 ISR;			/* 0x1F0 32 R/W Interrupt Status register */
+} spih_pciregs_t;
+
+/*
+ * PCI Core interrupt enable and status bit definitions.
+ */
+
+/* PCI Core ICR Register bit definitions */
+#define PCI_INT_PROP_EN		(1 << 0)	/* Interrupt Propagation Enable */
+#define PCI_WB_ERR_INT_EN	(1 << 1)	/* Wishbone Error Interrupt Enable */
+#define PCI_PCI_ERR_INT_EN	(1 << 2)	/* PCI Error Interrupt Enable */
+#define PCI_PAR_ERR_INT_EN	(1 << 3)	/* Parity Error Interrupt Enable */
+#define PCI_SYS_ERR_INT_EN	(1 << 4)	/* System Error Interrupt Enable */
+#define PCI_SOFTWARE_RESET	(1U << 31)	/* Software reset of the PCI Core. */
+
+
+/* PCI Core ISR Register bit definitions */
+#define PCI_INT_PROP_ST		(1 << 0)	/* Interrupt Propagation Status */
+#define PCI_WB_ERR_INT_ST	(1 << 1)	/* Wishbone Error Interrupt Status */
+#define PCI_PCI_ERR_INT_ST	(1 << 2)	/* PCI Error Interrupt Status */
+#define PCI_PAR_ERR_INT_ST	(1 << 3)	/* Parity Error Interrupt Status */
+#define PCI_SYS_ERR_INT_ST	(1 << 4)	/* System Error Interrupt Status */
+
+
+/* Registers on the Wishbone bus */
+#define SPIH_CTLR_INTR		(1 << 0)	/* SPI Host Controller Core Interrupt */
+#define SPIH_DEV_INTR		(1 << 1)	/* SPI Device Interrupt */
+#define SPIH_WFIFO_INTR		(1 << 2)	/* SPI Tx FIFO Empty Intr (FPGA Rev >= 8) */
+
+/* GPIO Bit definitions */
+#define SPIH_CS				(1 << 0)	/* SPI Chip Select (active low) */
+#define SPIH_SLOT_POWER		(1 << 1)	/* SD Card Slot Power Enable */
+#define SPIH_CARD_DETECT	(1 << 2)	/* SD Card Detect */
+
+/* SPI Status Register Bit definitions */
+#define SPIH_STATE_MASK		0x30		/* SPI Transfer State Machine state mask */
+#define SPIH_STATE_SHIFT	4		/* SPI Transfer State Machine state shift */
+#define SPIH_WFFULL			(1 << 3)	/* SPI Write FIFO Full */
+#define SPIH_WFEMPTY		(1 << 2)	/* SPI Write FIFO Empty */
+#define SPIH_RFFULL			(1 << 1)	/* SPI Read FIFO Full */
+#define SPIH_RFEMPTY		(1 << 0)	/* SPI Read FIFO Empty */
+
+#define SPIH_EXT_CLK		(1U << 31)	/* Use External Clock as PLL Clock source. */
+
+#define SPIH_PLL_NO_CLK		(1 << 1)	/* Set to 1 if the PLL's input clock is lost. */
+#define SPIH_PLL_LOCKED		(1 << 3)	/* Set to 1 when the PLL is locked. */
+
+/* Spin bit loop bound check */
+#define SPI_SPIN_BOUND		0xf4240		/* 1 million */
diff --git a/drivers/net/wireless/bcm4329/include/bcmperf.h b/drivers/net/wireless/bcm4329/include/bcmperf.h
new file mode 100644
index 000000000000..2a78784e85d3
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmperf.h
@@ -0,0 +1,36 @@
+/*
+ * Performance counters software interface.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmperf.h,v 13.5 2007/09/14 22:00:59 Exp $
+ */
+/* essai */
+#ifndef _BCMPERF_H_
+#define _BCMPERF_H_
+/* get cache hits and misses */
+#define BCMPERF_ENABLE_INSTRCOUNT()
+#define BCMPERF_ENABLE_ICACHE_MISS()
+#define BCMPERF_ENABLE_ICACHE_HIT()
+#define	BCMPERF_GETICACHE_MISS(x)	((x) = 0)
+#define	BCMPERF_GETICACHE_HIT(x)	((x) = 0)
+#define	BCMPERF_GETINSTRCOUNT(x)	((x) = 0)
+#endif /* _BCMPERF_H_ */
diff --git a/drivers/net/wireless/bcm4329/include/bcmsdbus.h b/drivers/net/wireless/bcm4329/include/bcmsdbus.h
new file mode 100644
index 000000000000..b7b67bc66248
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmsdbus.h
@@ -0,0 +1,117 @@
+/*
+ * Definitions for API from sdio common code (bcmsdh) to individual
+ * host controller drivers.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdbus.h,v 13.11.14.2.6.6 2009/10/27 17:20:28 Exp $
+ */
+
+#ifndef	_sdio_api_h_
+#define	_sdio_api_h_
+
+
+#define SDIOH_API_RC_SUCCESS                          (0x00)
+#define SDIOH_API_RC_FAIL	                      (0x01)
+#define SDIOH_API_SUCCESS(status) (status == 0)
+
+#define SDIOH_READ              0	/* Read request */
+#define SDIOH_WRITE             1	/* Write request */
+
+#define SDIOH_DATA_FIX          0	/* Fixed addressing */
+#define SDIOH_DATA_INC          1	/* Incremental addressing */
+
+#define SDIOH_CMD_TYPE_NORMAL   0       /* Normal command */
+#define SDIOH_CMD_TYPE_APPEND   1       /* Append command */
+#define SDIOH_CMD_TYPE_CUTTHRU  2       /* Cut-through command */
+
+#define SDIOH_DATA_PIO          0       /* PIO mode */
+#define SDIOH_DATA_DMA          1       /* DMA mode */
+
+
+typedef int SDIOH_API_RC;
+
+/* SDio Host structure */
+typedef struct sdioh_info sdioh_info_t;
+
+/* callback function, taking one arg */
+typedef void (*sdioh_cb_fn_t)(void *);
+
+/* attach, return handler on success, NULL if failed.
+ *  The handler shall be provided by all subsequent calls. No local cache
+ *  cfghdl points to the starting address of pci device mapped memory
+ */
+extern sdioh_info_t * sdioh_attach(osl_t *osh, void *cfghdl, uint irq);
+extern SDIOH_API_RC sdioh_detach(osl_t *osh, sdioh_info_t *si);
+extern SDIOH_API_RC sdioh_interrupt_register(sdioh_info_t *si, sdioh_cb_fn_t fn, void *argh);
+extern SDIOH_API_RC sdioh_interrupt_deregister(sdioh_info_t *si);
+
+/* query whether SD interrupt is enabled or not */
+extern SDIOH_API_RC sdioh_interrupt_query(sdioh_info_t *si, bool *onoff);
+
+/* enable or disable SD interrupt */
+extern SDIOH_API_RC sdioh_interrupt_set(sdioh_info_t *si, bool enable_disable);
+
+#if defined(DHD_DEBUG)
+extern bool sdioh_interrupt_pending(sdioh_info_t *si);
+#endif
+
+/* read or write one byte using cmd52 */
+extern SDIOH_API_RC sdioh_request_byte(sdioh_info_t *si, uint rw, uint fnc, uint addr, uint8 *byte);
+
+/* read or write 2/4 bytes using cmd53 */
+extern SDIOH_API_RC sdioh_request_word(sdioh_info_t *si, uint cmd_type, uint rw, uint fnc,
+	uint addr, uint32 *word, uint nbyte);
+
+/* read or write any buffer using cmd53 */
+extern SDIOH_API_RC sdioh_request_buffer(sdioh_info_t *si, uint pio_dma, uint fix_inc,
+	uint rw, uint fnc_num, uint32 addr, uint regwidth, uint32 buflen, uint8 *buffer,
+	void *pkt);
+
+/* get cis data */
+extern SDIOH_API_RC sdioh_cis_read(sdioh_info_t *si, uint fuc, uint8 *cis, uint32 length);
+
+extern SDIOH_API_RC sdioh_cfg_read(sdioh_info_t *si, uint fuc, uint32 addr, uint8 *data);
+extern SDIOH_API_RC sdioh_cfg_write(sdioh_info_t *si, uint fuc, uint32 addr, uint8 *data);
+
+/* query number of io functions */
+extern uint sdioh_query_iofnum(sdioh_info_t *si);
+
+/* handle iovars */
+extern int sdioh_iovar_op(sdioh_info_t *si, const char *name,
+                          void *params, int plen, void *arg, int len, bool set);
+
+/* Issue abort to the specified function and clear controller as needed */
+extern int sdioh_abort(sdioh_info_t *si, uint fnc);
+
+/* Start and Stop SDIO without re-enumerating the SD card. */
+extern int sdioh_start(sdioh_info_t *si, int stage);
+extern int sdioh_stop(sdioh_info_t *si);
+
+/* Reset and re-initialize the device */
+extern int sdioh_sdio_reset(sdioh_info_t *si);
+
+/* Helper function */
+void *bcmsdh_get_sdioh(bcmsdh_info_t *sdh);
+
+
+
+#endif /* _sdio_api_h_ */
diff --git a/drivers/net/wireless/bcm4329/include/bcmsdh.h b/drivers/net/wireless/bcm4329/include/bcmsdh.h
new file mode 100644
index 000000000000..caf0b9988aa8
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmsdh.h
@@ -0,0 +1,208 @@
+/*
+ * SDIO host client driver interface of Broadcom HNBU
+ *     export functions to client drivers
+ *     abstract OS and BUS specific details of SDIO
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdh.h,v 13.35.14.7.6.8 2009/10/14 04:22:25 Exp $
+ */
+
+#ifndef	_bcmsdh_h_
+#define	_bcmsdh_h_
+
+#define BCMSDH_ERROR_VAL	0x0001 /* Error */
+#define BCMSDH_INFO_VAL		0x0002 /* Info */
+extern const uint bcmsdh_msglevel;
+
+#define BCMSDH_ERROR(x)
+#define BCMSDH_INFO(x)
+
+/* forward declarations */
+typedef struct bcmsdh_info bcmsdh_info_t;
+typedef void (*bcmsdh_cb_fn_t)(void *);
+
+/* Attach and build an interface to the underlying SD host driver.
+ *  - Allocates resources (structs, arrays, mem, OS handles, etc) needed by bcmsdh.
+ *  - Returns the bcmsdh handle and virtual address base for register access.
+ *    The returned handle should be used in all subsequent calls, but the bcmsh
+ *    implementation may maintain a single "default" handle (e.g. the first or
+ *    most recent one) to enable single-instance implementations to pass NULL.
+ */
+extern bcmsdh_info_t *bcmsdh_attach(osl_t *osh, void *cfghdl, void **regsva, uint irq);
+
+/* Detach - freeup resources allocated in attach */
+extern int bcmsdh_detach(osl_t *osh, void *sdh);
+
+/* Query if SD device interrupts are enabled */
+extern bool bcmsdh_intr_query(void *sdh);
+
+/* Enable/disable SD interrupt */
+extern int bcmsdh_intr_enable(void *sdh);
+extern int bcmsdh_intr_disable(void *sdh);
+
+/* Register/deregister device interrupt handler. */
+extern int bcmsdh_intr_reg(void *sdh, bcmsdh_cb_fn_t fn, void *argh);
+extern int bcmsdh_intr_dereg(void *sdh);
+
+#if defined(DHD_DEBUG)
+/* Query pending interrupt status from the host controller */
+extern bool bcmsdh_intr_pending(void *sdh);
+#endif
+
+#ifdef BCMLXSDMMC
+extern int bcmsdh_claim_host_and_lock(void *sdh);
+extern int bcmsdh_release_host_and_unlock(void *sdh);
+#endif /* BCMLXSDMMC */
+
+/* Register a callback to be called if and when bcmsdh detects
+ * device removal. No-op in the case of non-removable/hardwired devices.
+ */
+extern int bcmsdh_devremove_reg(void *sdh, bcmsdh_cb_fn_t fn, void *argh);
+
+/* Access SDIO address space (e.g. CCCR) using CMD52 (single-byte interface).
+ *   fn:   function number
+ *   addr: unmodified SDIO-space address
+ *   data: data byte to write
+ *   err:  pointer to error code (or NULL)
+ */
+extern uint8 bcmsdh_cfg_read(void *sdh, uint func, uint32 addr, int *err);
+extern void bcmsdh_cfg_write(void *sdh, uint func, uint32 addr, uint8 data, int *err);
+
+/* Read/Write 4bytes from/to cfg space */
+extern uint32 bcmsdh_cfg_read_word(void *sdh, uint fnc_num, uint32 addr, int *err);
+extern void bcmsdh_cfg_write_word(void *sdh, uint fnc_num, uint32 addr, uint32 data, int *err);
+
+/* Read CIS content for specified function.
+ *   fn:     function whose CIS is being requested (0 is common CIS)
+ *   cis:    pointer to memory location to place results
+ *   length: number of bytes to read
+ * Internally, this routine uses the values from the cis base regs (0x9-0xB)
+ * to form an SDIO-space address to read the data from.
+ */
+extern int bcmsdh_cis_read(void *sdh, uint func, uint8 *cis, uint length);
+
+/* Synchronous access to device (client) core registers via CMD53 to F1.
+ *   addr: backplane address (i.e. >= regsva from attach)
+ *   size: register width in bytes (2 or 4)
+ *   data: data for register write
+ */
+extern uint32 bcmsdh_reg_read(void *sdh, uint32 addr, uint size);
+extern uint32 bcmsdh_reg_write(void *sdh, uint32 addr, uint size, uint32 data);
+
+/* Indicate if last reg read/write failed */
+extern bool bcmsdh_regfail(void *sdh);
+
+/* Buffer transfer to/from device (client) core via cmd53.
+ *   fn:       function number
+ *   addr:     backplane address (i.e. >= regsva from attach)
+ *   flags:    backplane width, address increment, sync/async
+ *   buf:      pointer to memory data buffer
+ *   nbytes:   number of bytes to transfer to/from buf
+ *   pkt:      pointer to packet associated with buf (if any)
+ *   complete: callback function for command completion (async only)
+ *   handle:   handle for completion callback (first arg in callback)
+ * Returns 0 or error code.
+ * NOTE: Async operation is not currently supported.
+ */
+typedef void (*bcmsdh_cmplt_fn_t)(void *handle, int status, bool sync_waiting);
+extern int bcmsdh_send_buf(void *sdh, uint32 addr, uint fn, uint flags,
+                           uint8 *buf, uint nbytes, void *pkt,
+                           bcmsdh_cmplt_fn_t complete, void *handle);
+extern int bcmsdh_recv_buf(void *sdh, uint32 addr, uint fn, uint flags,
+                           uint8 *buf, uint nbytes, void *pkt,
+                           bcmsdh_cmplt_fn_t complete, void *handle);
+
+/* Flags bits */
+#define SDIO_REQ_4BYTE	0x1	/* Four-byte target (backplane) width (vs. two-byte) */
+#define SDIO_REQ_FIXED	0x2	/* Fixed address (FIFO) (vs. incrementing address) */
+#define SDIO_REQ_ASYNC	0x4	/* Async request (vs. sync request) */
+
+/* Pending (non-error) return code */
+#define BCME_PENDING	1
+
+/* Read/write to memory block (F1, no FIFO) via CMD53 (sync only).
+ *   rw:       read or write (0/1)
+ *   addr:     direct SDIO address
+ *   buf:      pointer to memory data buffer
+ *   nbytes:   number of bytes to transfer to/from buf
+ * Returns 0 or error code.
+ */
+extern int bcmsdh_rwdata(void *sdh, uint rw, uint32 addr, uint8 *buf, uint nbytes);
+
+/* Issue an abort to the specified function */
+extern int bcmsdh_abort(void *sdh, uint fn);
+
+/* Start SDIO Host Controller communication */
+extern int bcmsdh_start(void *sdh, int stage);
+
+/* Stop SDIO Host Controller communication */
+extern int bcmsdh_stop(void *sdh);
+
+/* Returns the "Device ID" of target device on the SDIO bus. */
+extern int bcmsdh_query_device(void *sdh);
+
+/* Returns the number of IO functions reported by the device */
+extern uint bcmsdh_query_iofnum(void *sdh);
+
+/* Miscellaneous knob tweaker. */
+extern int bcmsdh_iovar_op(void *sdh, const char *name,
+                           void *params, int plen, void *arg, int len, bool set);
+
+/* Reset and reinitialize the device */
+extern int bcmsdh_reset(bcmsdh_info_t *sdh);
+
+/* helper functions */
+
+extern void *bcmsdh_get_sdioh(bcmsdh_info_t *sdh);
+
+/* callback functions */
+typedef struct {
+	/* attach to device */
+	void *(*attach)(uint16 vend_id, uint16 dev_id, uint16 bus, uint16 slot,
+	                uint16 func, uint bustype, void * regsva, osl_t * osh,
+	                void * param, void *dev);
+	/* detach from device */
+	void (*detach)(void *ch);
+} bcmsdh_driver_t;
+
+/* platform specific/high level functions */
+extern int bcmsdh_register(bcmsdh_driver_t *driver);
+extern void bcmsdh_unregister(void);
+extern bool bcmsdh_chipmatch(uint16 vendor, uint16 device);
+extern void bcmsdh_device_remove(void * sdh);
+
+#if defined(OOB_INTR_ONLY)
+extern int bcmsdh_register_oob_intr(void * dhdp);
+extern void bcmsdh_unregister_oob_intr(void);
+extern void bcmsdh_oob_intr_set(bool enable);
+#endif /* defined(OOB_INTR_ONLY) */
+/* Function to pass device-status bits to DHD. */
+extern uint32 bcmsdh_get_dstatus(void *sdh);
+
+/* Function to return current window addr */
+extern uint32 bcmsdh_cur_sbwad(void *sdh);
+
+/* Function to pass chipid and rev to lower layers for controlling pr's */
+extern void bcmsdh_chipinfo(void *sdh, uint32 chip, uint32 chiprev);
+
+
+#endif	/* _bcmsdh_h_ */
diff --git a/drivers/net/wireless/bcm4329/include/bcmsdh_sdmmc.h b/drivers/net/wireless/bcm4329/include/bcmsdh_sdmmc.h
new file mode 100644
index 000000000000..4e6d1b5bd94f
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmsdh_sdmmc.h
@@ -0,0 +1,122 @@
+/*
+ * BCMSDH Function Driver for the native SDIO/MMC driver in the Linux Kernel
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdh_sdmmc.h,v 13.1.2.1.8.7 2009/10/27 18:22:52 Exp $
+ */
+
+#ifndef __BCMSDH_SDMMC_H__
+#define __BCMSDH_SDMMC_H__
+
+#define sd_err(x)
+#define sd_trace(x)
+#define sd_info(x)
+#define sd_debug(x)
+#define sd_data(x)
+#define sd_ctrl(x)
+
+#define sd_sync_dma(sd, read, nbytes)
+#define sd_init_dma(sd)
+#define sd_ack_intr(sd)
+#define sd_wakeup(sd);
+
+/* Allocate/init/free per-OS private data */
+extern int sdioh_sdmmc_osinit(sdioh_info_t *sd);
+extern void sdioh_sdmmc_osfree(sdioh_info_t *sd);
+
+#define sd_log(x)
+
+#define SDIOH_ASSERT(exp) \
+	do { if (!(exp)) \
+		printf("!!!ASSERT fail: file %s lines %d", __FILE__, __LINE__); \
+	} while (0)
+
+#define BLOCK_SIZE_4318 64
+#define BLOCK_SIZE_4328 512
+
+/* internal return code */
+#define SUCCESS	0
+#define ERROR	1
+
+/* private bus modes */
+#define SDIOH_MODE_SD4		2
+#define CLIENT_INTR 		0x100	/* Get rid of this! */
+
+struct sdioh_info {
+	osl_t 		*osh;			/* osh handler */
+	bool		client_intr_enabled;	/* interrupt connnected flag */
+	bool		intr_handler_valid;	/* client driver interrupt handler valid */
+	sdioh_cb_fn_t	intr_handler;		/* registered interrupt handler */
+	void		*intr_handler_arg;	/* argument to call interrupt handler */
+	uint16		intmask;		/* Current active interrupts */
+	void		*sdos_info;		/* Pointer to per-OS private data */
+
+	uint 		irq;			/* Client irq */
+	int 		intrcount;		/* Client interrupts */
+
+	bool		sd_use_dma;		/* DMA on CMD53 */
+	bool 		sd_blockmode;		/* sd_blockmode == FALSE => 64 Byte Cmd 53s. */
+						/*  Must be on for sd_multiblock to be effective */
+	bool 		use_client_ints;	/* If this is false, make sure to restore */
+	int 		sd_mode;		/* SD1/SD4/SPI */
+	int 		client_block_size[SDIOD_MAX_IOFUNCS];		/* Blocksize */
+	uint8 		num_funcs;		/* Supported funcs on client */
+	uint32 		com_cis_ptr;
+	uint32 		func_cis_ptr[SDIOD_MAX_IOFUNCS];
+	uint		max_dma_len;
+	uint		max_dma_descriptors;	/* DMA Descriptors supported by this controller. */
+//	SDDMA_DESCRIPTOR	SGList[32];	/* Scatter/Gather DMA List */
+};
+
+/************************************************************
+ * Internal interfaces: per-port references into bcmsdh_sdmmc.c
+ */
+
+/* Global message bits */
+extern uint sd_msglevel;
+
+/* OS-independent interrupt handler */
+extern bool check_client_intr(sdioh_info_t *sd);
+
+/* Core interrupt enable/disable of device interrupts */
+extern void sdioh_sdmmc_devintr_on(sdioh_info_t *sd);
+extern void sdioh_sdmmc_devintr_off(sdioh_info_t *sd);
+
+
+/**************************************************************
+ * Internal interfaces: bcmsdh_sdmmc.c references to per-port code
+ */
+
+/* Register mapping routines */
+extern uint32 *sdioh_sdmmc_reg_map(osl_t *osh, int32 addr, int size);
+extern void sdioh_sdmmc_reg_unmap(osl_t *osh, int32 addr, int size);
+
+/* Interrupt (de)registration routines */
+extern int sdioh_sdmmc_register_irq(sdioh_info_t *sd, uint irq);
+extern void sdioh_sdmmc_free_irq(uint irq, sdioh_info_t *sd);
+
+typedef struct _BCMSDH_SDMMC_INSTANCE {
+	sdioh_info_t	*sd;
+	struct sdio_func *func[SDIOD_MAX_IOFUNCS];
+} BCMSDH_SDMMC_INSTANCE, *PBCMSDH_SDMMC_INSTANCE;
+
+#endif /* __BCMSDH_SDMMC_H__ */
diff --git a/drivers/net/wireless/bcm4329/include/bcmsdpcm.h b/drivers/net/wireless/bcm4329/include/bcmsdpcm.h
new file mode 100644
index 000000000000..77aca4500ad8
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmsdpcm.h
@@ -0,0 +1,263 @@
+/*
+ * Broadcom SDIO/PCMCIA
+ * Software-specific definitions shared between device and host side
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdpcm.h,v 1.1.2.4 2010/07/02 01:15:46 Exp $
+ */
+
+#ifndef	_bcmsdpcm_h_
+#define	_bcmsdpcm_h_
+
+/*
+ * Software allocation of To SB Mailbox resources
+ */
+
+/* intstatus bits */
+#define I_SMB_NAK	I_SMB_SW0	/* To SB Mailbox Frame NAK */
+#define I_SMB_INT_ACK	I_SMB_SW1	/* To SB Mailbox Host Interrupt ACK */
+#define I_SMB_USE_OOB	I_SMB_SW2	/* To SB Mailbox Use OOB Wakeup */
+#define I_SMB_DEV_INT	I_SMB_SW3	/* To SB Mailbox Miscellaneous Interrupt */
+
+/* tosbmailbox bits corresponding to intstatus bits */
+#define SMB_NAK		(1 << 0)	/* To SB Mailbox Frame NAK */
+#define SMB_INT_ACK	(1 << 1)	/* To SB Mailbox Host Interrupt ACK */
+#define SMB_USE_OOB	(1 << 2)	/* To SB Mailbox Use OOB Wakeup */
+#define SMB_DEV_INT	(1 << 3)	/* To SB Mailbox Miscellaneous Interrupt */
+#define SMB_MASK	0x0000000f	/* To SB Mailbox Mask */
+
+/* tosbmailboxdata */
+#define SMB_DATA_VERSION_MASK	0x00ff0000	/* host protocol version (sent with F2 enable) */
+#define SMB_DATA_VERSION_SHIFT	16		/* host protocol version (sent with F2 enable) */
+
+/*
+ * Software allocation of To Host Mailbox resources
+ */
+
+/* intstatus bits */
+#define I_HMB_FC_STATE	I_HMB_SW0	/* To Host Mailbox Flow Control State */
+#define I_HMB_FC_CHANGE	I_HMB_SW1	/* To Host Mailbox Flow Control State Changed */
+#define I_HMB_FRAME_IND	I_HMB_SW2	/* To Host Mailbox Frame Indication */
+#define I_HMB_HOST_INT	I_HMB_SW3	/* To Host Mailbox Miscellaneous Interrupt */
+
+/* tohostmailbox bits corresponding to intstatus bits */
+#define HMB_FC_ON	(1 << 0)	/* To Host Mailbox Flow Control State */
+#define HMB_FC_CHANGE	(1 << 1)	/* To Host Mailbox Flow Control State Changed */
+#define HMB_FRAME_IND	(1 << 2)	/* To Host Mailbox Frame Indication */
+#define HMB_HOST_INT	(1 << 3)	/* To Host Mailbox Miscellaneous Interrupt */
+#define HMB_MASK	0x0000000f	/* To Host Mailbox Mask */
+
+/* tohostmailboxdata */
+#define HMB_DATA_NAKHANDLED	1	/* we're ready to retransmit NAK'd frame to host */
+#define HMB_DATA_DEVREADY	2	/* we're ready to to talk to host after enable */
+#define HMB_DATA_FC		4	/* per prio flowcontrol update flag to host */
+#define HMB_DATA_FWREADY	8	/* firmware is ready for protocol activity */
+
+#define HMB_DATA_FCDATA_MASK	0xff000000	/* per prio flowcontrol data */
+#define HMB_DATA_FCDATA_SHIFT	24		/* per prio flowcontrol data */
+
+#define HMB_DATA_VERSION_MASK	0x00ff0000	/* device protocol version (with devready) */
+#define HMB_DATA_VERSION_SHIFT	16		/* device protocol version (with devready) */
+
+/*
+ * Software-defined protocol header
+ */
+
+/* Current protocol version */
+#define SDPCM_PROT_VERSION	4
+
+/* SW frame header */
+#define SDPCM_SEQUENCE_MASK		0x000000ff	/* Sequence Number Mask */
+#define SDPCM_PACKET_SEQUENCE(p) (((uint8 *)p)[0] & 0xff) /* p starts w/SW Header */
+
+#define SDPCM_CHANNEL_MASK		0x00000f00	/* Channel Number Mask */
+#define SDPCM_CHANNEL_SHIFT		8		/* Channel Number Shift */
+#define SDPCM_PACKET_CHANNEL(p) (((uint8 *)p)[1] & 0x0f) /* p starts w/SW Header */
+
+#define SDPCM_FLAGS_MASK		0x0000f000	/* Mask of flag bits */
+#define SDPCM_FLAGS_SHIFT		12		/* Flag bits shift */
+#define SDPCM_PACKET_FLAGS(p) ((((uint8 *)p)[1] & 0xf0) >> 4) /* p starts w/SW Header */
+
+/* Next Read Len: lookahead length of next frame, in 16-byte units (rounded up) */
+#define SDPCM_NEXTLEN_MASK		0x00ff0000	/* Next Read Len Mask */
+#define SDPCM_NEXTLEN_SHIFT		16		/* Next Read Len Shift */
+#define SDPCM_NEXTLEN_VALUE(p) ((((uint8 *)p)[2] & 0xff) << 4) /* p starts w/SW Header */
+#define SDPCM_NEXTLEN_OFFSET		2
+
+/* Data Offset from SOF (HW Tag, SW Tag, Pad) */
+#define SDPCM_DOFFSET_OFFSET		3		/* Data Offset */
+#define SDPCM_DOFFSET_VALUE(p) 		(((uint8 *)p)[SDPCM_DOFFSET_OFFSET] & 0xff)
+#define SDPCM_DOFFSET_MASK		0xff000000
+#define SDPCM_DOFFSET_SHIFT		24
+
+#define SDPCM_FCMASK_OFFSET		4		/* Flow control */
+#define SDPCM_FCMASK_VALUE(p)		(((uint8 *)p)[SDPCM_FCMASK_OFFSET ] & 0xff)
+#define SDPCM_WINDOW_OFFSET		5		/* Credit based fc */
+#define SDPCM_WINDOW_VALUE(p)		(((uint8 *)p)[SDPCM_WINDOW_OFFSET] & 0xff)
+#define SDPCM_VERSION_OFFSET		6		/* Version # */
+#define SDPCM_VERSION_VALUE(p)		(((uint8 *)p)[SDPCM_VERSION_OFFSET] & 0xff)
+#define SDPCM_UNUSED_OFFSET		7		/* Spare */
+#define SDPCM_UNUSED_VALUE(p)		(((uint8 *)p)[SDPCM_UNUSED_OFFSET] & 0xff)
+
+#define SDPCM_SWHEADER_LEN	8	/* SW header is 64 bits */
+
+/* logical channel numbers */
+#define SDPCM_CONTROL_CHANNEL	0	/* Control Request/Response Channel Id */
+#define SDPCM_EVENT_CHANNEL	1	/* Asyc Event Indication Channel Id */
+#define SDPCM_DATA_CHANNEL	2	/* Data Xmit/Recv Channel Id */
+#define SDPCM_GLOM_CHANNEL	3	/* For coalesced packets (superframes) */
+#define SDPCM_TEST_CHANNEL	15	/* Reserved for test/debug packets */
+#define SDPCM_MAX_CHANNEL	15
+
+#define SDPCM_SEQUENCE_WRAP	256	/* wrap-around val for eight-bit frame seq number */
+
+#define SDPCM_FLAG_RESVD0	0x01
+#define SDPCM_FLAG_RESVD1	0x02
+#define SDPCM_FLAG_GSPI_TXENAB	0x04
+#define SDPCM_FLAG_GLOMDESC	0x08	/* Superframe descriptor mask */
+
+/* For GLOM_CHANNEL frames, use a flag to indicate descriptor frame */
+#define SDPCM_GLOMDESC_FLAG	(SDPCM_FLAG_GLOMDESC << SDPCM_FLAGS_SHIFT)
+
+#define SDPCM_GLOMDESC(p)	(((uint8 *)p)[1] & 0x80)
+
+/* For TEST_CHANNEL packets, define another 4-byte header */
+#define SDPCM_TEST_HDRLEN	4	/* Generally: Cmd(1), Ext(1), Len(2);
+					 * Semantics of Ext byte depend on command.
+					 * Len is current or requested frame length, not
+					 * including test header; sent little-endian.
+					 */
+#define SDPCM_TEST_DISCARD	0x01	/* Receiver discards. Ext is a pattern id. */
+#define SDPCM_TEST_ECHOREQ	0x02	/* Echo request. Ext is a pattern id. */
+#define SDPCM_TEST_ECHORSP	0x03	/* Echo response. Ext is a pattern id. */
+#define SDPCM_TEST_BURST	0x04	/* Receiver to send a burst. Ext is a frame count */
+#define SDPCM_TEST_SEND		0x05	/* Receiver sets send mode. Ext is boolean on/off */
+
+/* Handy macro for filling in datagen packets with a pattern */
+#define SDPCM_TEST_FILL(byteno, id)	((uint8)(id + byteno))
+
+/*
+ * Software counters (first part matches hardware counters)
+ */
+
+typedef volatile struct {
+	uint32 cmd52rd;		/* Cmd52RdCount, SDIO: cmd52 reads */
+	uint32 cmd52wr;		/* Cmd52WrCount, SDIO: cmd52 writes */
+	uint32 cmd53rd;		/* Cmd53RdCount, SDIO: cmd53 reads */
+	uint32 cmd53wr;		/* Cmd53WrCount, SDIO: cmd53 writes */
+	uint32 abort;		/* AbortCount, SDIO: aborts */
+	uint32 datacrcerror;	/* DataCrcErrorCount, SDIO: frames w/CRC error */
+	uint32 rdoutofsync;	/* RdOutOfSyncCount, SDIO/PCMCIA: Rd Frm out of sync */
+	uint32 wroutofsync;	/* RdOutOfSyncCount, SDIO/PCMCIA: Wr Frm out of sync */
+	uint32 writebusy;	/* WriteBusyCount, SDIO: device asserted "busy" */
+	uint32 readwait;	/* ReadWaitCount, SDIO: no data ready for a read cmd */
+	uint32 readterm;	/* ReadTermCount, SDIO: read frame termination cmds */
+	uint32 writeterm;	/* WriteTermCount, SDIO: write frames termination cmds */
+	uint32 rxdescuflo;	/* receive descriptor underflows */
+	uint32 rxfifooflo;	/* receive fifo overflows */
+	uint32 txfifouflo;	/* transmit fifo underflows */
+	uint32 runt;		/* runt (too short) frames recv'd from bus */
+	uint32 badlen;		/* frame's rxh len does not match its hw tag len */
+	uint32 badcksum;	/* frame's hw tag chksum doesn't agree with len value */
+	uint32 seqbreak;	/* break in sequence # space from one rx frame to the next */
+	uint32 rxfcrc;		/* frame rx header indicates crc error */
+	uint32 rxfwoos;		/* frame rx header indicates write out of sync */
+	uint32 rxfwft;		/* frame rx header indicates write frame termination */
+	uint32 rxfabort;	/* frame rx header indicates frame aborted */
+	uint32 woosint;		/* write out of sync interrupt */
+	uint32 roosint;		/* read out of sync interrupt */
+	uint32 rftermint;	/* read frame terminate interrupt */
+	uint32 wftermint;	/* write frame terminate interrupt */
+} sdpcmd_cnt_t;
+
+/*
+ * Register Access Macros
+ */
+
+#define SDIODREV_IS(var, val)	((var) == (val))
+#define SDIODREV_GE(var, val)	((var) >= (val))
+#define SDIODREV_GT(var, val)	((var) > (val))
+#define SDIODREV_LT(var, val)	((var) < (val))
+#define SDIODREV_LE(var, val)	((var) <= (val))
+
+#define SDIODDMAREG32(h, dir, chnl) \
+	((dir) == DMA_TX ? \
+	 (void *)(uintptr)&((h)->regs->dma.sdiod32.dma32regs[chnl].xmt) : \
+	 (void *)(uintptr)&((h)->regs->dma.sdiod32.dma32regs[chnl].rcv))
+
+#define SDIODDMAREG64(h, dir, chnl) \
+	((dir) == DMA_TX ? \
+	 (void *)(uintptr)&((h)->regs->dma.sdiod64.dma64regs[chnl].xmt) : \
+	 (void *)(uintptr)&((h)->regs->dma.sdiod64.dma64regs[chnl].rcv))
+
+#define SDIODDMAREG(h, dir, chnl) \
+	(SDIODREV_LT((h)->corerev, 1) ? \
+	 SDIODDMAREG32((h), (dir), (chnl)) : \
+	 SDIODDMAREG64((h), (dir), (chnl)))
+
+#define PCMDDMAREG(h, dir, chnl) \
+	((dir) == DMA_TX ? \
+	 (void *)(uintptr)&((h)->regs->dma.pcm32.dmaregs.xmt) : \
+	 (void *)(uintptr)&((h)->regs->dma.pcm32.dmaregs.rcv))
+
+#define SDPCMDMAREG(h, dir, chnl, coreid) \
+	((coreid) == SDIOD_CORE_ID ? \
+	 SDIODDMAREG(h, dir, chnl) : \
+	 PCMDDMAREG(h, dir, chnl))
+
+#define SDIODFIFOREG(h, corerev) \
+	(SDIODREV_LT((corerev), 1) ? \
+	 ((dma32diag_t *)(uintptr)&((h)->regs->dma.sdiod32.dmafifo)) : \
+	 ((dma32diag_t *)(uintptr)&((h)->regs->dma.sdiod64.dmafifo)))
+
+#define PCMDFIFOREG(h) \
+	((dma32diag_t *)(uintptr)&((h)->regs->dma.pcm32.dmafifo))
+
+#define SDPCMFIFOREG(h, coreid, corerev) \
+	((coreid) == SDIOD_CORE_ID ? \
+	 SDIODFIFOREG(h, corerev) : \
+	 PCMDFIFOREG(h))
+
+/*
+ * Shared structure between dongle and the host
+ * The structure contains pointers to trap or assert information shared with the host
+ */
+#define SDPCM_SHARED_VERSION       0x0002
+#define SDPCM_SHARED_VERSION_MASK  0x00FF
+#define SDPCM_SHARED_ASSERT_BUILT  0x0100
+#define SDPCM_SHARED_ASSERT        0x0200
+#define SDPCM_SHARED_TRAP          0x0400
+
+typedef struct {
+	uint32	flags;
+	uint32  trap_addr;
+	uint32  assert_exp_addr;
+	uint32  assert_file_addr;
+	uint32  assert_line;
+	uint32	console_addr;		/* Address of hndrte_cons_t */
+	uint32  msgtrace_addr;
+	uint8   tag[32];
+} sdpcm_shared_t;
+
+extern sdpcm_shared_t sdpcm_shared;
+
+#endif	/* _bcmsdpcm_h_ */
diff --git a/drivers/net/wireless/bcm4329/include/bcmsdspi.h b/drivers/net/wireless/bcm4329/include/bcmsdspi.h
new file mode 100644
index 000000000000..eaae10d8bf19
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmsdspi.h
@@ -0,0 +1,131 @@
+/*
+ * SD-SPI Protocol Conversion - BCMSDH->SPI Translation Layer
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdspi.h,v 13.8.10.2 2008/06/30 21:09:40 Exp $
+ */
+
+/* global msglevel for debug messages - bitvals come from sdiovar.h */
+
+#define sd_err(x)
+#define sd_trace(x)
+#define sd_info(x)
+#define sd_debug(x)
+#define sd_data(x)
+#define sd_ctrl(x)
+
+#define sd_log(x)
+
+#define SDIOH_ASSERT(exp) \
+	do { if (!(exp)) \
+		printf("!!!ASSERT fail: file %s lines %d", __FILE__, __LINE__); \
+	} while (0)
+
+#define BLOCK_SIZE_4318 64
+#define BLOCK_SIZE_4328 512
+
+/* internal return code */
+#define SUCCESS	0
+#undef ERROR
+#define ERROR	1
+
+/* private bus modes */
+#define SDIOH_MODE_SPI		0
+
+#define USE_BLOCKMODE		0x2	/* Block mode can be single block or multi */
+#define USE_MULTIBLOCK		0x4
+
+struct sdioh_info {
+	uint cfg_bar;                   	/* pci cfg address for bar */
+	uint32 caps;                    	/* cached value of capabilities reg */
+	uint		bar0;			/* BAR0 for PCI Device */
+	osl_t 		*osh;			/* osh handler */
+	void		*controller;	/* Pointer to SPI Controller's private data struct */
+
+	uint		lockcount; 		/* nest count of sdspi_lock() calls */
+	bool		client_intr_enabled;	/* interrupt connnected flag */
+	bool		intr_handler_valid;	/* client driver interrupt handler valid */
+	sdioh_cb_fn_t	intr_handler;		/* registered interrupt handler */
+	void		*intr_handler_arg;	/* argument to call interrupt handler */
+	bool		initialized;		/* card initialized */
+	uint32		target_dev;		/* Target device ID */
+	uint32		intmask;		/* Current active interrupts */
+	void		*sdos_info;		/* Pointer to per-OS private data */
+
+	uint32		controller_type;	/* Host controller type */
+	uint8		version;		/* Host Controller Spec Compliance Version */
+	uint 		irq;			/* Client irq */
+	uint32 		intrcount;		/* Client interrupts */
+	uint32 		local_intrcount;	/* Controller interrupts */
+	bool 		host_init_done;		/* Controller initted */
+	bool 		card_init_done;		/* Client SDIO interface initted */
+	bool 		polled_mode;		/* polling for command completion */
+
+	bool		sd_use_dma;		/* DMA on CMD53 */
+	bool 		sd_blockmode;		/* sd_blockmode == FALSE => 64 Byte Cmd 53s. */
+						/*  Must be on for sd_multiblock to be effective */
+	bool 		use_client_ints;	/* If this is false, make sure to restore */
+	bool		got_hcint;		/* Host Controller interrupt. */
+						/*  polling hack in wl_linux.c:wl_timer() */
+	int 		adapter_slot;		/* Maybe dealing with multiple slots/controllers */
+	int 		sd_mode;		/* SD1/SD4/SPI */
+	int 		client_block_size[SDIOD_MAX_IOFUNCS];		/* Blocksize */
+	uint32 		data_xfer_count;	/* Current register transfer size */
+	uint32		cmd53_wr_data;		/* Used to pass CMD53 write data */
+	uint32		card_response;		/* Used to pass back response status byte */
+	uint32		card_rsp_data;		/* Used to pass back response data word */
+	uint16 		card_rca;		/* Current Address */
+	uint8 		num_funcs;		/* Supported funcs on client */
+	uint32 		com_cis_ptr;
+	uint32 		func_cis_ptr[SDIOD_MAX_IOFUNCS];
+	void		*dma_buf;
+	ulong		dma_phys;
+	int 		r_cnt;			/* rx count */
+	int 		t_cnt;			/* tx_count */
+};
+
+/************************************************************
+ * Internal interfaces: per-port references into bcmsdspi.c
+ */
+
+/* Global message bits */
+extern uint sd_msglevel;
+
+/**************************************************************
+ * Internal interfaces: bcmsdspi.c references to per-port code
+ */
+
+/* Register mapping routines */
+extern uint32 *spi_reg_map(osl_t *osh, uintptr addr, int size);
+extern void spi_reg_unmap(osl_t *osh, uintptr addr, int size);
+
+/* Interrupt (de)registration routines */
+extern int spi_register_irq(sdioh_info_t *sd, uint irq);
+extern void spi_free_irq(uint irq, sdioh_info_t *sd);
+
+/* OS-specific interrupt wrappers (atomic interrupt enable/disable) */
+extern void spi_lock(sdioh_info_t *sd);
+extern void spi_unlock(sdioh_info_t *sd);
+
+/* Allocate/init/free per-OS private data */
+extern int spi_osinit(sdioh_info_t *sd);
+extern void spi_osfree(sdioh_info_t *sd);
diff --git a/drivers/net/wireless/bcm4329/include/bcmsdstd.h b/drivers/net/wireless/bcm4329/include/bcmsdstd.h
new file mode 100644
index 000000000000..974b3d41698d
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmsdstd.h
@@ -0,0 +1,223 @@
+/*
+ *  'Standard' SDIO HOST CONTROLLER driver
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdstd.h,v 13.16.18.1.16.3 2009/12/10 01:09:23 Exp $
+ */
+
+/* global msglevel for debug messages - bitvals come from sdiovar.h */
+
+#define sd_err(x)	do { if (sd_msglevel & SDH_ERROR_VAL) printf x; } while (0)
+#define sd_trace(x)
+#define sd_info(x)
+#define sd_debug(x)
+#define sd_data(x)
+#define sd_ctrl(x)
+#define sd_dma(x)
+
+#define sd_sync_dma(sd, read, nbytes)
+#define sd_init_dma(sd)
+#define sd_ack_intr(sd)
+#define sd_wakeup(sd);
+/* Allocate/init/free per-OS private data */
+extern int sdstd_osinit(sdioh_info_t *sd);
+extern void sdstd_osfree(sdioh_info_t *sd);
+
+#define sd_log(x)
+
+#define SDIOH_ASSERT(exp) \
+	do { if (!(exp)) \
+		printf("!!!ASSERT fail: file %s lines %d", __FILE__, __LINE__); \
+	} while (0)
+
+#define BLOCK_SIZE_4318 64
+#define BLOCK_SIZE_4328 512
+
+/* internal return code */
+#define SUCCESS	0
+#define ERROR	1
+
+/* private bus modes */
+#define SDIOH_MODE_SPI		0
+#define SDIOH_MODE_SD1		1
+#define SDIOH_MODE_SD4		2
+
+#define MAX_SLOTS 6 	/* For PCI: Only 6 BAR entries => 6 slots */
+#define SDIOH_REG_WINSZ	0x100 /* Number of registers in Standard Host Controller */
+
+#define SDIOH_TYPE_ARASAN_HDK	1
+#define SDIOH_TYPE_BCM27XX	2
+#define SDIOH_TYPE_TI_PCIXX21	4	/* TI PCIxx21 Standard Host Controller */
+#define SDIOH_TYPE_RICOH_R5C822	5	/* Ricoh Co Ltd R5C822 SD/SDIO/MMC/MS/MSPro Host Adapter */
+#define SDIOH_TYPE_JMICRON	6	/* JMicron Standard SDIO Host Controller */
+
+/* For linux, allow yielding for dongle */
+#define BCMSDYIELD
+
+/* Expected card status value for CMD7 */
+#define SDIOH_CMD7_EXP_STATUS   0x00001E00
+
+#define RETRIES_LARGE 100000
+#define RETRIES_SMALL 100
+
+
+#define USE_BLOCKMODE		0x2	/* Block mode can be single block or multi */
+#define USE_MULTIBLOCK		0x4
+
+#define USE_FIFO		0x8	/* Fifo vs non-fifo */
+
+#define CLIENT_INTR 		0x100	/* Get rid of this! */
+
+
+struct sdioh_info {
+	uint cfg_bar;                   	/* pci cfg address for bar */
+	uint32 caps;                    	/* cached value of capabilities reg */
+	uint32 curr_caps;                    	/* max current capabilities reg */
+
+	osl_t 		*osh;			/* osh handler */
+	volatile char 	*mem_space;		/* pci device memory va */
+	uint		lockcount; 		/* nest count of sdstd_lock() calls */
+	bool		client_intr_enabled;	/* interrupt connnected flag */
+	bool		intr_handler_valid;	/* client driver interrupt handler valid */
+	sdioh_cb_fn_t	intr_handler;		/* registered interrupt handler */
+	void		*intr_handler_arg;	/* argument to call interrupt handler */
+	bool		initialized;		/* card initialized */
+	uint		target_dev;		/* Target device ID */
+	uint16		intmask;		/* Current active interrupts */
+	void		*sdos_info;		/* Pointer to per-OS private data */
+
+	uint32		controller_type;	/* Host controller type */
+	uint8		version;		/* Host Controller Spec Compliance Version */
+	uint 		irq;			/* Client irq */
+	int 		intrcount;		/* Client interrupts */
+	int 		local_intrcount;	/* Controller interrupts */
+	bool 		host_init_done;		/* Controller initted */
+	bool 		card_init_done;		/* Client SDIO interface initted */
+	bool 		polled_mode;		/* polling for command completion */
+
+	bool 		sd_blockmode;		/* sd_blockmode == FALSE => 64 Byte Cmd 53s. */
+						/*  Must be on for sd_multiblock to be effective */
+	bool 		use_client_ints;	/* If this is false, make sure to restore */
+						/*  polling hack in wl_linux.c:wl_timer() */
+	int 		adapter_slot;		/* Maybe dealing with multiple slots/controllers */
+	int 		sd_mode;		/* SD1/SD4/SPI */
+	int 		client_block_size[SDIOD_MAX_IOFUNCS];		/* Blocksize */
+	uint32 		data_xfer_count;	/* Current transfer */
+	uint16 		card_rca;		/* Current Address */
+	int8		sd_dma_mode;		/* DMA Mode (PIO, SDMA, ... ADMA2) on CMD53 */
+	uint8 		num_funcs;		/* Supported funcs on client */
+	uint32 		com_cis_ptr;
+	uint32 		func_cis_ptr[SDIOD_MAX_IOFUNCS];
+	void		*dma_buf;		/* DMA Buffer virtual address */
+	ulong		dma_phys;		/* DMA Buffer physical address */
+	void		*adma2_dscr_buf;	/* ADMA2 Descriptor Buffer virtual address */
+	ulong		adma2_dscr_phys;	/* ADMA2 Descriptor Buffer physical address */
+
+	/* adjustments needed to make the dma align properly */
+	void		*dma_start_buf;
+	ulong		dma_start_phys;
+	uint		alloced_dma_size;
+	void		*adma2_dscr_start_buf;
+	ulong		adma2_dscr_start_phys;
+	uint		alloced_adma2_dscr_size;
+
+	int 		r_cnt;			/* rx count */
+	int 		t_cnt;			/* tx_count */
+	bool		got_hcint;		/* local interrupt flag */
+	uint16		last_intrstatus;	/* to cache intrstatus */
+};
+
+#define DMA_MODE_NONE	0
+#define DMA_MODE_SDMA	1
+#define DMA_MODE_ADMA1	2
+#define DMA_MODE_ADMA2	3
+#define DMA_MODE_ADMA2_64 4
+#define DMA_MODE_AUTO	-1
+
+#define USE_DMA(sd)		((bool)((sd->sd_dma_mode > 0) ? TRUE : FALSE))
+
+/* SDIO Host Control Register DMA Mode Definitions */
+#define SDIOH_SDMA_MODE			0
+#define SDIOH_ADMA1_MODE		1
+#define SDIOH_ADMA2_MODE		2
+#define SDIOH_ADMA2_64_MODE		3
+
+#define ADMA2_ATTRIBUTE_VALID		(1 << 0)	/* ADMA Descriptor line valid */
+#define ADMA2_ATTRIBUTE_END			(1 << 1)	/* End of Descriptor */
+#define ADMA2_ATTRIBUTE_INT			(1 << 2)	/* Interrupt when line is done */
+#define ADMA2_ATTRIBUTE_ACT_NOP		(0 << 4)	/* Skip current line, go to next. */
+#define ADMA2_ATTRIBUTE_ACT_RSV		(1 << 4)	/* Same as NOP */
+#define ADMA1_ATTRIBUTE_ACT_SET		(1 << 4)	/* ADMA1 Only - set transfer length */
+#define ADMA2_ATTRIBUTE_ACT_TRAN	(2 << 4)	/* Transfer Data of one descriptor line. */
+#define ADMA2_ATTRIBUTE_ACT_LINK	(3 << 4)	/* Link Descriptor */
+
+/* ADMA2 Descriptor Table Entry for 32-bit Address */
+typedef struct adma2_dscr_32b {
+	uint32 len_attr;
+	uint32 phys_addr;
+} adma2_dscr_32b_t;
+
+/* ADMA1 Descriptor Table Entry */
+typedef struct adma1_dscr {
+	uint32 phys_addr_attr;
+} adma1_dscr_t;
+
+/************************************************************
+ * Internal interfaces: per-port references into bcmsdstd.c
+ */
+
+/* Global message bits */
+extern uint sd_msglevel;
+
+/* OS-independent interrupt handler */
+extern bool check_client_intr(sdioh_info_t *sd);
+
+/* Core interrupt enable/disable of device interrupts */
+extern void sdstd_devintr_on(sdioh_info_t *sd);
+extern void sdstd_devintr_off(sdioh_info_t *sd);
+
+/* Enable/disable interrupts for local controller events */
+extern void sdstd_intrs_on(sdioh_info_t *sd, uint16 norm, uint16 err);
+extern void sdstd_intrs_off(sdioh_info_t *sd, uint16 norm, uint16 err);
+
+/* Wait for specified interrupt and error bits to be set */
+extern void sdstd_spinbits(sdioh_info_t *sd, uint16 norm, uint16 err);
+
+
+/**************************************************************
+ * Internal interfaces: bcmsdstd.c references to per-port code
+ */
+
+/* Register mapping routines */
+extern uint32 *sdstd_reg_map(osl_t *osh, int32 addr, int size);
+extern void sdstd_reg_unmap(osl_t *osh, int32 addr, int size);
+
+/* Interrupt (de)registration routines */
+extern int sdstd_register_irq(sdioh_info_t *sd, uint irq);
+extern void sdstd_free_irq(uint irq, sdioh_info_t *sd);
+
+/* OS-specific interrupt wrappers (atomic interrupt enable/disable) */
+extern void sdstd_lock(sdioh_info_t *sd);
+extern void sdstd_unlock(sdioh_info_t *sd);
+
+/* OS-specific wait-for-interrupt-or-status */
+extern uint16 sdstd_waitbits(sdioh_info_t *sd, uint16 norm, uint16 err, bool yield);
diff --git a/drivers/net/wireless/bcm4329/include/bcmspi.h b/drivers/net/wireless/bcm4329/include/bcmspi.h
new file mode 100644
index 000000000000..2e2bc935716f
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmspi.h
@@ -0,0 +1,36 @@
+/*
+ * Broadcom SPI Low-Level Hardware Driver API
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmspi.h,v 13.3.10.2 2008/06/30 21:09:40 Exp $
+ */
+
+extern void spi_devintr_off(sdioh_info_t *sd);
+extern void spi_devintr_on(sdioh_info_t *sd);
+extern bool spi_start_clock(sdioh_info_t *sd, uint16 new_sd_divisor);
+extern bool spi_controller_highspeed_mode(sdioh_info_t *sd, bool hsmode);
+extern bool spi_check_client_intr(sdioh_info_t *sd, int *is_dev_intr);
+extern bool spi_hw_attach(sdioh_info_t *sd);
+extern bool spi_hw_detach(sdioh_info_t *sd);
+extern void spi_sendrecv(sdioh_info_t *sd, uint8 *msg_out, uint8 *msg_in, int msglen);
+extern void spi_spinbits(sdioh_info_t *sd);
+extern void spi_waitbits(sdioh_info_t *sd, bool yield);
diff --git a/drivers/net/wireless/bcm4329/include/bcmspibrcm.h b/drivers/net/wireless/bcm4329/include/bcmspibrcm.h
new file mode 100644
index 000000000000..9dce878d11e2
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmspibrcm.h
@@ -0,0 +1,134 @@
+/*
+ * SD-SPI Protocol Conversion - BCMSDH->gSPI Translation Layer
+ *
+ * Copyright (C) 2010, Broadcom Corporation
+ * All Rights Reserved.
+ * 
+ * This is UNPUBLISHED PROPRIETARY SOURCE CODE of Broadcom Corporation;
+ * the contents of this file may not be disclosed to third parties, copied
+ * or duplicated in any form, in whole or in part, without the prior
+ * written permission of Broadcom Corporation.
+ *
+ * $Id: bcmspibrcm.h,v 1.4.4.1.4.3.6.1 2008/09/27 17:03:25 Exp $
+ */
+
+/* global msglevel for debug messages - bitvals come from sdiovar.h */
+
+#define sd_err(x)
+#define sd_trace(x)
+#define sd_info(x)
+#define sd_debug(x)
+#define sd_data(x)
+#define sd_ctrl(x)
+
+#define sd_log(x)
+
+#define SDIOH_ASSERT(exp) \
+	do { if (!(exp)) \
+		printf("!!!ASSERT fail: file %s lines %d", __FILE__, __LINE__); \
+	} while (0)
+
+#define BLOCK_SIZE_F1		64
+#define BLOCK_SIZE_F2 		2048
+#define BLOCK_SIZE_F3 		2048
+
+/* internal return code */
+#define SUCCESS	0
+#undef ERROR
+#define ERROR	1
+#define ERROR_UF	2
+#define ERROR_OF	3
+
+/* private bus modes */
+#define SDIOH_MODE_SPI		0
+
+#define USE_BLOCKMODE		0x2	/* Block mode can be single block or multi */
+#define USE_MULTIBLOCK		0x4
+
+struct sdioh_info {
+	uint 		cfg_bar;		/* pci cfg address for bar */
+	uint32		caps;			/* cached value of capabilities reg */
+	void		*bar0;			/* BAR0 for PCI Device */
+	osl_t 		*osh;			/* osh handler */
+	void		*controller;	/* Pointer to SPI Controller's private data struct */
+
+	uint		lockcount; 		/* nest count of spi_lock() calls */
+	bool		client_intr_enabled;	/* interrupt connnected flag */
+	bool		intr_handler_valid;	/* client driver interrupt handler valid */
+	sdioh_cb_fn_t	intr_handler;		/* registered interrupt handler */
+	void		*intr_handler_arg;	/* argument to call interrupt handler */
+	bool		initialized;		/* card initialized */
+	uint32		target_dev;		/* Target device ID */
+	uint32		intmask;		/* Current active interrupts */
+	void		*sdos_info;		/* Pointer to per-OS private data */
+
+	uint32		controller_type;	/* Host controller type */
+	uint8		version;		/* Host Controller Spec Compliance Version */
+	uint 		irq;			/* Client irq */
+	uint32 		intrcount;		/* Client interrupts */
+	uint32 		local_intrcount;	/* Controller interrupts */
+	bool 		host_init_done;		/* Controller initted */
+	bool 		card_init_done;		/* Client SDIO interface initted */
+	bool 		polled_mode;		/* polling for command completion */
+
+	bool		sd_use_dma;		/* DMA on CMD53 */
+	bool 		sd_blockmode;		/* sd_blockmode == FALSE => 64 Byte Cmd 53s. */
+						/*  Must be on for sd_multiblock to be effective */
+	bool 		use_client_ints;	/* If this is false, make sure to restore */
+						/*  polling hack in wl_linux.c:wl_timer() */
+	int 		adapter_slot;		/* Maybe dealing with multiple slots/controllers */
+	int 		sd_mode;		/* SD1/SD4/SPI */
+	int 		client_block_size[SPI_MAX_IOFUNCS];		/* Blocksize */
+	uint32 		data_xfer_count;	/* Current transfer */
+	uint16 		card_rca;		/* Current Address */
+	uint8 		num_funcs;		/* Supported funcs on client */
+	uint32 		card_dstatus;		/* 32bit device status */
+	uint32 		com_cis_ptr;
+	uint32 		func_cis_ptr[SPI_MAX_IOFUNCS];
+	void		*dma_buf;
+	ulong		dma_phys;
+	int 		r_cnt;			/* rx count */
+	int 		t_cnt;			/* tx_count */
+	uint32		wordlen;			/* host processor 16/32bits */
+	uint32		prev_fun;
+	uint32		chip;
+	uint32		chiprev;
+	bool		resp_delay_all;
+	bool		dwordmode;
+
+	struct spierrstats_t spierrstats;
+};
+
+/************************************************************
+ * Internal interfaces: per-port references into bcmspibrcm.c
+ */
+
+/* Global message bits */
+extern uint sd_msglevel;
+
+/**************************************************************
+ * Internal interfaces: bcmspibrcm.c references to per-port code
+ */
+
+/* Interrupt (de)registration routines */
+extern int spi_register_irq(sdioh_info_t *sd, uint irq);
+extern void spi_free_irq(uint irq, sdioh_info_t *sd);
+
+/* OS-specific interrupt wrappers (atomic interrupt enable/disable) */
+extern void spi_lock(sdioh_info_t *sd);
+extern void spi_unlock(sdioh_info_t *sd);
+
+/* Allocate/init/free per-OS private data */
+extern int spi_osinit(sdioh_info_t *sd);
+extern void spi_osfree(sdioh_info_t *sd);
+
+#define SPI_RW_FLAG_M			BITFIELD_MASK(1)	/* Bit [31] - R/W Command Bit */
+#define SPI_RW_FLAG_S			31
+#define SPI_ACCESS_M			BITFIELD_MASK(1)	/* Bit [30] - Fixed/Incr Access */
+#define SPI_ACCESS_S			30
+#define SPI_FUNCTION_M			BITFIELD_MASK(2)	/* Bit [29:28] - Function Number */
+#define SPI_FUNCTION_S			28
+#define SPI_REG_ADDR_M			BITFIELD_MASK(17)	/* Bit [27:11] - Address */
+#define SPI_REG_ADDR_S			11
+#define SPI_LEN_M			BITFIELD_MASK(11)	/* Bit [10:0] - Packet length */
+#define SPI_LEN_S			0
diff --git a/drivers/net/wireless/bcm4329/include/bcmutils.h b/drivers/net/wireless/bcm4329/include/bcmutils.h
new file mode 100644
index 000000000000..f85ed351d663
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmutils.h
@@ -0,0 +1,637 @@
+/*
+ * Misc useful os-independent macros and functions.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: bcmutils.h,v 13.184.4.6.2.1.18.25 2010/04/26 06:05:24 Exp $
+ */
+
+
+#ifndef	_bcmutils_h_
+#define	_bcmutils_h_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+#define _BCM_U	0x01	
+#define _BCM_L	0x02	
+#define _BCM_D	0x04	
+#define _BCM_C	0x08	
+#define _BCM_P	0x10	
+#define _BCM_S	0x20	
+#define _BCM_X	0x40	
+#define _BCM_SP	0x80	
+
+extern const unsigned char bcm_ctype[];
+#define bcm_ismask(x)	(bcm_ctype[(int)(unsigned char)(x)])
+
+#define bcm_isalnum(c)	((bcm_ismask(c)&(_BCM_U|_BCM_L|_BCM_D)) != 0)
+#define bcm_isalpha(c)	((bcm_ismask(c)&(_BCM_U|_BCM_L)) != 0)
+#define bcm_iscntrl(c)	((bcm_ismask(c)&(_BCM_C)) != 0)
+#define bcm_isdigit(c)	((bcm_ismask(c)&(_BCM_D)) != 0)
+#define bcm_isgraph(c)	((bcm_ismask(c)&(_BCM_P|_BCM_U|_BCM_L|_BCM_D)) != 0)
+#define bcm_islower(c)	((bcm_ismask(c)&(_BCM_L)) != 0)
+#define bcm_isprint(c)	((bcm_ismask(c)&(_BCM_P|_BCM_U|_BCM_L|_BCM_D|_BCM_SP)) != 0)
+#define bcm_ispunct(c)	((bcm_ismask(c)&(_BCM_P)) != 0)
+#define bcm_isspace(c)	((bcm_ismask(c)&(_BCM_S)) != 0)
+#define bcm_isupper(c)	((bcm_ismask(c)&(_BCM_U)) != 0)
+#define bcm_isxdigit(c)	((bcm_ismask(c)&(_BCM_D|_BCM_X)) != 0)
+#define bcm_tolower(c)	(bcm_isupper((c)) ? ((c) + 'a' - 'A') : (c))
+#define bcm_toupper(c)	(bcm_islower((c)) ? ((c) + 'A' - 'a') : (c))
+
+
+
+struct bcmstrbuf {
+	char *buf;	
+	unsigned int size;	
+	char *origbuf;	
+	unsigned int origsize;	
+};
+
+
+#ifdef BCMDRIVER
+#include <osl.h>
+
+#define GPIO_PIN_NOTDEFINED 	0x20	
+
+
+#define SPINWAIT(exp, us) { \
+	uint countdown = (us) + 9; \
+	while ((exp) && (countdown >= 10)) {\
+		OSL_DELAY(10); \
+		countdown -= 10; \
+	} \
+}
+
+
+
+#ifndef PKTQ_LEN_DEFAULT
+#define PKTQ_LEN_DEFAULT        128	
+#endif
+#ifndef PKTQ_MAX_PREC
+#define PKTQ_MAX_PREC           16	
+#endif
+
+typedef struct pktq_prec {
+	void *head;     
+	void *tail;     
+	uint16 len;     
+	uint16 max;     
+} pktq_prec_t;
+
+
+
+struct pktq {
+	uint16 num_prec;        
+	uint16 hi_prec;         
+	uint16 max;             
+	uint16 len;             
+	
+	struct pktq_prec q[PKTQ_MAX_PREC];
+};
+
+
+struct spktq {
+	uint16 num_prec;        
+	uint16 hi_prec;         
+	uint16 max;             
+	uint16 len;             
+	
+	struct pktq_prec q[1];
+};
+
+#define PKTQ_PREC_ITER(pq, prec)        for (prec = (pq)->num_prec - 1; prec >= 0; prec--)
+
+
+
+
+struct ether_addr;
+
+extern int ether_isbcast(const void *ea);
+extern int ether_isnulladdr(const void *ea);
+
+
+
+#define pktq_psetmax(pq, prec, _max)    ((pq)->q[prec].max = (_max))
+#define pktq_plen(pq, prec)             ((pq)->q[prec].len)
+#define pktq_pavail(pq, prec)           ((pq)->q[prec].max - (pq)->q[prec].len)
+#define pktq_pfull(pq, prec)            ((pq)->q[prec].len >= (pq)->q[prec].max)
+#define pktq_pempty(pq, prec)           ((pq)->q[prec].len == 0)
+
+#define pktq_ppeek(pq, prec)            ((pq)->q[prec].head)
+#define pktq_ppeek_tail(pq, prec)       ((pq)->q[prec].tail)
+
+extern void *pktq_penq(struct pktq *pq, int prec, void *p);
+extern void *pktq_penq_head(struct pktq *pq, int prec, void *p);
+extern void *pktq_pdeq(struct pktq *pq, int prec);
+extern void *pktq_pdeq_tail(struct pktq *pq, int prec);
+
+extern bool pktq_pdel(struct pktq *pq, void *p, int prec);
+
+
+extern void pktq_pflush(osl_t *osh, struct pktq *pq, int prec, bool dir);
+
+extern void pktq_flush(osl_t *osh, struct pktq *pq, bool dir);
+
+
+
+extern int pktq_mlen(struct pktq *pq, uint prec_bmp);
+extern void *pktq_mdeq(struct pktq *pq, uint prec_bmp, int *prec_out);
+
+
+
+#define pktq_len(pq)                    ((int)(pq)->len)
+#define pktq_max(pq)                    ((int)(pq)->max)
+#define pktq_avail(pq)                  ((int)((pq)->max - (pq)->len))
+#define pktq_full(pq)                   ((pq)->len >= (pq)->max)
+#define pktq_empty(pq)                  ((pq)->len == 0)
+
+
+#define pktenq(pq, p)		pktq_penq(((struct pktq *)pq), 0, (p))
+#define pktenq_head(pq, p)	pktq_penq_head(((struct pktq *)pq), 0, (p))
+#define pktdeq(pq)		pktq_pdeq(((struct pktq *)pq), 0)
+#define pktdeq_tail(pq)		pktq_pdeq_tail(((struct pktq *)pq), 0)
+#define pktqinit(pq, len) pktq_init(((struct pktq *)pq), 1, len)
+
+extern void pktq_init(struct pktq *pq, int num_prec, int max_len);
+
+extern void *pktq_deq(struct pktq *pq, int *prec_out);
+extern void *pktq_deq_tail(struct pktq *pq, int *prec_out);
+extern void *pktq_peek(struct pktq *pq, int *prec_out);
+extern void *pktq_peek_tail(struct pktq *pq, int *prec_out);
+
+
+
+extern uint pktcopy(osl_t *osh, void *p, uint offset, int len, uchar *buf);
+extern uint pktfrombuf(osl_t *osh, void *p, uint offset, int len, uchar *buf);
+extern uint pkttotlen(osl_t *osh, void *p);
+extern void *pktlast(osl_t *osh, void *p);
+extern uint pktsegcnt(osl_t *osh, void *p);
+
+
+extern uint pktsetprio(void *pkt, bool update_vtag);
+#define	PKTPRIO_VDSCP	0x100		
+#define	PKTPRIO_VLAN	0x200		
+#define	PKTPRIO_UPD	0x400		
+#define	PKTPRIO_DSCP	0x800		
+
+
+extern int bcm_atoi(char *s);
+extern ulong bcm_strtoul(char *cp, char **endp, uint base);
+extern char *bcmstrstr(char *haystack, char *needle);
+extern char *bcmstrcat(char *dest, const char *src);
+extern char *bcmstrncat(char *dest, const char *src, uint size);
+extern ulong wchar2ascii(char *abuf, ushort *wbuf, ushort wbuflen, ulong abuflen);
+char* bcmstrtok(char **string, const char *delimiters, char *tokdelim);
+int bcmstricmp(const char *s1, const char *s2);
+int bcmstrnicmp(const char* s1, const char* s2, int cnt);
+
+
+
+extern char *bcm_ether_ntoa(const struct ether_addr *ea, char *buf);
+extern int bcm_ether_atoe(char *p, struct ether_addr *ea);
+
+
+struct ipv4_addr;
+extern char *bcm_ip_ntoa(struct ipv4_addr *ia, char *buf);
+
+
+extern void bcm_mdelay(uint ms);
+
+extern char *getvar(char *vars, const char *name);
+extern int getintvar(char *vars, const char *name);
+extern uint getgpiopin(char *vars, char *pin_name, uint def_pin);
+#define bcm_perf_enable()
+#define bcmstats(fmt)
+#define	bcmlog(fmt, a1, a2)
+#define	bcmdumplog(buf, size)	*buf = '\0'
+#define	bcmdumplogent(buf, idx)	-1
+
+#define bcmtslog(tstamp, fmt, a1, a2)
+#define bcmprinttslogs()
+#define bcmprinttstamp(us)
+
+
+
+
+typedef struct bcm_iovar {
+	const char *name;	
+	uint16 varid;		
+	uint16 flags;		
+	uint16 type;		
+	uint16 minlen;		
+} bcm_iovar_t;
+
+
+
+
+#define IOV_GET 0 
+#define IOV_SET 1 
+
+
+#define IOV_GVAL(id)		((id)*2)
+#define IOV_SVAL(id)		(((id)*2)+IOV_SET)
+#define IOV_ISSET(actionid)	((actionid & IOV_SET) == IOV_SET)
+
+
+
+extern const bcm_iovar_t *bcm_iovar_lookup(const bcm_iovar_t *table, const char *name);
+extern int bcm_iovar_lencheck(const bcm_iovar_t *table, void *arg, int len, bool set);
+
+#endif	
+
+
+#define IOVT_VOID	0	
+#define IOVT_BOOL	1	
+#define IOVT_INT8	2	
+#define IOVT_UINT8	3	
+#define IOVT_INT16	4	
+#define IOVT_UINT16	5	
+#define IOVT_INT32	6	
+#define IOVT_UINT32	7	
+#define IOVT_BUFFER	8	
+#define BCM_IOVT_VALID(type) (((unsigned int)(type)) <= IOVT_BUFFER)
+
+
+#define BCM_IOV_TYPE_INIT { \
+	"void", \
+	"bool", \
+	"int8", \
+	"uint8", \
+	"int16", \
+	"uint16", \
+	"int32", \
+	"uint32", \
+	"buffer", \
+	"" }
+
+#define BCM_IOVT_IS_INT(type) (\
+	(type == IOVT_BOOL) || \
+	(type == IOVT_INT8) || \
+	(type == IOVT_UINT8) || \
+	(type == IOVT_INT16) || \
+	(type == IOVT_UINT16) || \
+	(type == IOVT_INT32) || \
+	(type == IOVT_UINT32))
+
+
+
+#define BCME_STRLEN 		64	
+#define VALID_BCMERROR(e)  ((e <= 0) && (e >= BCME_LAST))
+
+
+
+
+#define BCME_OK				0	
+#define BCME_ERROR			-1	
+#define BCME_BADARG			-2	
+#define BCME_BADOPTION			-3	
+#define BCME_NOTUP			-4	
+#define BCME_NOTDOWN			-5	
+#define BCME_NOTAP			-6	
+#define BCME_NOTSTA			-7	
+#define BCME_BADKEYIDX			-8	
+#define BCME_RADIOOFF 			-9	
+#define BCME_NOTBANDLOCKED		-10	
+#define BCME_NOCLK			-11	
+#define BCME_BADRATESET			-12	
+#define BCME_BADBAND			-13	
+#define BCME_BUFTOOSHORT		-14	
+#define BCME_BUFTOOLONG			-15	
+#define BCME_BUSY			-16	
+#define BCME_NOTASSOCIATED		-17	
+#define BCME_BADSSIDLEN			-18	
+#define BCME_OUTOFRANGECHAN		-19	
+#define BCME_BADCHAN			-20	
+#define BCME_BADADDR			-21	
+#define BCME_NORESOURCE			-22	
+#define BCME_UNSUPPORTED		-23	
+#define BCME_BADLEN			-24	
+#define BCME_NOTREADY			-25	
+#define BCME_EPERM			-26	
+#define BCME_NOMEM			-27	
+#define BCME_ASSOCIATED			-28	
+#define BCME_RANGE			-29	
+#define BCME_NOTFOUND			-30	
+#define BCME_WME_NOT_ENABLED		-31	
+#define BCME_TSPEC_NOTFOUND		-32	
+#define BCME_ACM_NOTSUPPORTED		-33	
+#define BCME_NOT_WME_ASSOCIATION	-34	
+#define BCME_SDIO_ERROR			-35	
+#define BCME_DONGLE_DOWN		-36	
+#define BCME_VERSION			-37	
+#define BCME_TXFAIL			-38	
+#define BCME_RXFAIL			-39	
+#define BCME_NODEVICE			-40	
+#define BCME_UNFINISHED			-41	
+#define BCME_LAST			BCME_UNFINISHED
+
+
+#define BCMERRSTRINGTABLE {		\
+	"OK",				\
+	"Undefined error",		\
+	"Bad Argument",			\
+	"Bad Option",			\
+	"Not up",			\
+	"Not down",			\
+	"Not AP",			\
+	"Not STA",			\
+	"Bad Key Index",		\
+	"Radio Off",			\
+	"Not band locked",		\
+	"No clock",			\
+	"Bad Rate valueset",		\
+	"Bad Band",			\
+	"Buffer too short",		\
+	"Buffer too long",		\
+	"Busy",				\
+	"Not Associated",		\
+	"Bad SSID len",			\
+	"Out of Range Channel",		\
+	"Bad Channel",			\
+	"Bad Address",			\
+	"Not Enough Resources",		\
+	"Unsupported",			\
+	"Bad length",			\
+	"Not Ready",			\
+	"Not Permitted",		\
+	"No Memory",			\
+	"Associated",			\
+	"Not In Range",			\
+	"Not Found",			\
+	"WME Not Enabled",		\
+	"TSPEC Not Found",		\
+	"ACM Not Supported",		\
+	"Not WME Association",		\
+	"SDIO Bus Error",		\
+	"Dongle Not Accessible",	\
+	"Incorrect version",		\
+	"TX Failure",			\
+	"RX Failure",			\
+	"Device Not Present",		\
+	"Command not finished",		\
+}
+
+#ifndef ABS
+#define	ABS(a)			(((a) < 0)?-(a):(a))
+#endif 
+
+#ifndef MIN
+#define	MIN(a, b)		(((a) < (b))?(a):(b))
+#endif 
+
+#ifndef MAX
+#define	MAX(a, b)		(((a) > (b))?(a):(b))
+#endif 
+
+#define CEIL(x, y)		(((x) + ((y)-1)) / (y))
+#define	ROUNDUP(x, y)		((((x)+((y)-1))/(y))*(y))
+#define	ISALIGNED(a, x)		(((a) & ((x)-1)) == 0)
+#define ALIGN_ADDR(addr, boundary) (void *)(((uintptr)(addr) + (boundary) - 1) \
+	                                         & ~((boundary) - 1))
+#define	ISPOWEROF2(x)		((((x)-1)&(x)) == 0)
+#define VALID_MASK(mask)	!((mask) & ((mask) + 1))
+#ifndef OFFSETOF
+#define	OFFSETOF(type, member)	((uint)(uintptr)&((type *)0)->member)
+#endif 
+#ifndef ARRAYSIZE
+#define ARRAYSIZE(a)		(sizeof(a)/sizeof(a[0]))
+#endif
+
+
+#ifndef setbit
+#ifndef NBBY		      
+#define	NBBY	8	
+#endif 
+#define	setbit(a, i)	(((uint8 *)a)[(i)/NBBY] |= 1<<((i)%NBBY))
+#define	clrbit(a, i)	(((uint8 *)a)[(i)/NBBY] &= ~(1<<((i)%NBBY)))
+#define	isset(a, i)	(((const uint8 *)a)[(i)/NBBY] & (1<<((i)%NBBY)))
+#define	isclr(a, i)	((((const uint8 *)a)[(i)/NBBY] & (1<<((i)%NBBY))) == 0)
+#endif 
+
+#define	NBITS(type)	(sizeof(type) * 8)
+#define NBITVAL(nbits)	(1 << (nbits))
+#define MAXBITVAL(nbits)	((1 << (nbits)) - 1)
+#define	NBITMASK(nbits)	MAXBITVAL(nbits)
+#define MAXNBVAL(nbyte)	MAXBITVAL((nbyte) * 8)
+
+
+#define MUX(pred, true, false) ((pred) ? (true) : (false))
+
+
+#define MODDEC(x, bound) MUX((x) == 0, (bound) - 1, (x) - 1)
+#define MODINC(x, bound) MUX((x) == (bound) - 1, 0, (x) + 1)
+
+
+#define MODDEC_POW2(x, bound) (((x) - 1) & ((bound) - 1))
+#define MODINC_POW2(x, bound) (((x) + 1) & ((bound) - 1))
+
+
+#define MODADD(x, y, bound) \
+    MUX((x) + (y) >= (bound), (x) + (y) - (bound), (x) + (y))
+#define MODSUB(x, y, bound) \
+    MUX(((int)(x)) - ((int)(y)) < 0, (x) - (y) + (bound), (x) - (y))
+
+
+#define MODADD_POW2(x, y, bound) (((x) + (y)) & ((bound) - 1))
+#define MODSUB_POW2(x, y, bound) (((x) - (y)) & ((bound) - 1))
+
+
+#define CRC8_INIT_VALUE  0xff		
+#define CRC8_GOOD_VALUE  0x9f		
+#define CRC16_INIT_VALUE 0xffff		
+#define CRC16_GOOD_VALUE 0xf0b8		
+#define CRC32_INIT_VALUE 0xffffffff	
+#define CRC32_GOOD_VALUE 0xdebb20e3	
+
+
+typedef struct bcm_bit_desc {
+	uint32	bit;
+	const char* name;
+} bcm_bit_desc_t;
+
+
+typedef struct bcm_tlv {
+	uint8	id;
+	uint8	len;
+	uint8	data[1];
+} bcm_tlv_t;
+
+
+#define bcm_valid_tlv(elt, buflen) ((buflen) >= 2 && (int)(buflen) >= (int)(2 + (elt)->len))
+
+
+#define ETHER_ADDR_STR_LEN	18	
+
+
+#ifdef IL_BIGENDIAN
+static INLINE uint32
+load32_ua(uint8 *a)
+{
+	return ((a[0] << 24) | (a[1] << 16) | (a[2] << 8) | a[3]);
+}
+
+static INLINE void
+store32_ua(uint8 *a, uint32 v)
+{
+	a[0] = (v >> 24) & 0xff;
+	a[1] = (v >> 16) & 0xff;
+	a[2] = (v >> 8) & 0xff;
+	a[3] = v & 0xff;
+}
+
+static INLINE uint16
+load16_ua(uint8 *a)
+{
+	return ((a[0] << 8) | a[1]);
+}
+
+static INLINE void
+store16_ua(uint8 *a, uint16 v)
+{
+	a[0] = (v >> 8) & 0xff;
+	a[1] = v & 0xff;
+}
+
+#else 
+
+static INLINE uint32
+load32_ua(uint8 *a)
+{
+	return ((a[3] << 24) | (a[2] << 16) | (a[1] << 8) | a[0]);
+}
+
+static INLINE void
+store32_ua(uint8 *a, uint32 v)
+{
+	a[3] = (v >> 24) & 0xff;
+	a[2] = (v >> 16) & 0xff;
+	a[1] = (v >> 8) & 0xff;
+	a[0] = v & 0xff;
+}
+
+static INLINE uint16
+load16_ua(uint8 *a)
+{
+	return ((a[1] << 8) | a[0]);
+}
+
+static INLINE void
+store16_ua(uint8 *a, uint16 v)
+{
+	a[1] = (v >> 8) & 0xff;
+	a[0] = v & 0xff;
+}
+
+#endif 
+
+
+
+static INLINE void
+xor_128bit_block(const uint8 *src1, const uint8 *src2, uint8 *dst)
+{
+	if (
+#ifdef __i386__
+	    1 ||
+#endif
+	    (((uintptr)src1 | (uintptr)src2 | (uintptr)dst) & 3) == 0) {
+		
+		
+		((uint32 *)dst)[0] = ((uint32 *)src1)[0] ^ ((uint32 *)src2)[0];
+		((uint32 *)dst)[1] = ((uint32 *)src1)[1] ^ ((uint32 *)src2)[1];
+		((uint32 *)dst)[2] = ((uint32 *)src1)[2] ^ ((uint32 *)src2)[2];
+		((uint32 *)dst)[3] = ((uint32 *)src1)[3] ^ ((uint32 *)src2)[3];
+	} else {
+		
+		int k;
+		for (k = 0; k < 16; k++)
+			dst[k] = src1[k] ^ src2[k];
+	}
+}
+
+
+
+extern uint8 hndcrc8(uint8 *p, uint nbytes, uint8 crc);
+extern uint16 hndcrc16(uint8 *p, uint nbytes, uint16 crc);
+extern uint32 hndcrc32(uint8 *p, uint nbytes, uint32 crc);
+
+#if defined(DHD_DEBUG) || defined(WLMSG_PRHDRS) || defined(WLMSG_PRPKT) || \
+	defined(WLMSG_ASSOC)
+extern int bcm_format_flags(const bcm_bit_desc_t *bd, uint32 flags, char* buf, int len);
+extern int bcm_format_hex(char *str, const void *bytes, int len);
+extern void prhex(const char *msg, uchar *buf, uint len);
+#endif 
+extern char *bcm_brev_str(uint32 brev, char *buf);
+extern void printbig(char *buf);
+
+
+extern bcm_tlv_t *bcm_next_tlv(bcm_tlv_t *elt, int *buflen);
+extern bcm_tlv_t *bcm_parse_tlvs(void *buf, int buflen, uint key);
+extern bcm_tlv_t *bcm_parse_ordered_tlvs(void *buf, int buflen, uint key);
+
+
+extern const char *bcmerrorstr(int bcmerror);
+
+
+typedef uint32 mbool;
+#define mboolset(mb, bit)		((mb) |= (bit))		
+#define mboolclr(mb, bit)		((mb) &= ~(bit))	
+#define mboolisset(mb, bit)		(((mb) & (bit)) != 0)	
+#define	mboolmaskset(mb, mask, val)	((mb) = (((mb) & ~(mask)) | (val)))
+
+
+extern uint16 bcm_qdbm_to_mw(uint8 qdbm);
+extern uint8 bcm_mw_to_qdbm(uint16 mw);
+
+
+struct fielddesc {
+	const char *nameandfmt;
+	uint32 	offset;
+	uint32 	len;
+};
+
+extern void bcm_binit(struct bcmstrbuf *b, char *buf, uint size);
+extern int bcm_bprintf(struct bcmstrbuf *b, const char *fmt, ...);
+extern void bcm_inc_bytes(uchar *num, int num_bytes, uint8 amount);
+extern int bcm_cmp_bytes(uchar *arg1, uchar *arg2, uint8 nbytes);
+extern void bcm_print_bytes(char *name, const uchar *cdata, int len);
+
+typedef  uint32 (*bcmutl_rdreg_rtn)(void *arg0, uint arg1, uint32 offset);
+extern uint bcmdumpfields(bcmutl_rdreg_rtn func_ptr, void *arg0, uint arg1, struct fielddesc *str,
+                          char *buf, uint32 bufsize);
+
+extern uint bcm_mkiovar(char *name, char *data, uint datalen, char *buf, uint len);
+extern uint bcm_bitcount(uint8 *bitmap, uint bytelength);
+
+#if defined(WLTINYDUMP) || defined(WLMSG_INFORM) || defined(WLMSG_ASSOC) || \
+	defined(WLMSG_PRPKT) || defined(WLMSG_WSEC)
+extern int bcm_format_ssid(char* buf, const uchar ssid[], uint ssid_len);
+#endif 
+
+
+#define SSID_FMT_BUF_LEN	((4 * DOT11_MAX_SSID_LEN) + 1)
+
+#ifdef __cplusplus
+	}
+#endif
+
+#endif	
diff --git a/drivers/net/wireless/bcm4329/include/bcmwifi.h b/drivers/net/wireless/bcm4329/include/bcmwifi.h
new file mode 100644
index 000000000000..038aedcdb3c8
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/bcmwifi.h
@@ -0,0 +1,154 @@
+/*
+ * Misc utility routines for WL and Apps
+ * This header file housing the define and function prototype use by
+ * both the wl driver, tools & Apps.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: bcmwifi.h,v 1.15.30.4 2010/03/10 20:10:52 Exp $
+ */
+
+
+#ifndef	_bcmwifi_h_
+#define	_bcmwifi_h_
+
+
+
+typedef uint16 chanspec_t;
+
+
+#define CH_UPPER_SB			0x01
+#define CH_LOWER_SB			0x02
+#define CH_EWA_VALID			0x04
+#define CH_20MHZ_APART			4
+#define CH_10MHZ_APART			2
+#define CH_5MHZ_APART			1
+#define CH_MAX_2G_CHANNEL		14	
+#define WLC_MAX_2G_CHANNEL		CH_MAX_2G_CHANNEL 
+#define	MAXCHANNEL		224	
+
+#define WL_CHANSPEC_CHAN_MASK		0x00ff
+#define WL_CHANSPEC_CHAN_SHIFT		0
+
+#define WL_CHANSPEC_CTL_SB_MASK		0x0300
+#define WL_CHANSPEC_CTL_SB_SHIFT	     8
+#define WL_CHANSPEC_CTL_SB_LOWER	0x0100
+#define WL_CHANSPEC_CTL_SB_UPPER	0x0200
+#define WL_CHANSPEC_CTL_SB_NONE		0x0300
+
+#define WL_CHANSPEC_BW_MASK		0x0C00
+#define WL_CHANSPEC_BW_SHIFT		    10
+#define WL_CHANSPEC_BW_10		0x0400
+#define WL_CHANSPEC_BW_20		0x0800
+#define WL_CHANSPEC_BW_40		0x0C00
+
+#define WL_CHANSPEC_BAND_MASK		0xf000
+#define WL_CHANSPEC_BAND_SHIFT		12
+#define WL_CHANSPEC_BAND_5G		0x1000
+#define WL_CHANSPEC_BAND_2G		0x2000
+#define INVCHANSPEC			255
+
+
+#define WF_CHAN_FACTOR_2_4_G		4814	
+#define WF_CHAN_FACTOR_5_G		10000	
+#define WF_CHAN_FACTOR_4_G		8000	
+
+
+#define LOWER_20_SB(channel)	((channel > CH_10MHZ_APART) ? (channel - CH_10MHZ_APART) : 0)
+#define UPPER_20_SB(channel)	((channel < (MAXCHANNEL - CH_10MHZ_APART)) ? \
+				(channel + CH_10MHZ_APART) : 0)
+#define CHSPEC_WLCBANDUNIT(chspec)	(CHSPEC_IS5G(chspec) ? BAND_5G_INDEX : BAND_2G_INDEX)
+#define CH20MHZ_CHSPEC(channel)	(chanspec_t)((chanspec_t)(channel) | WL_CHANSPEC_BW_20 | \
+				WL_CHANSPEC_CTL_SB_NONE | (((channel) <= CH_MAX_2G_CHANNEL) ? \
+				WL_CHANSPEC_BAND_2G : WL_CHANSPEC_BAND_5G))
+#define NEXT_20MHZ_CHAN(channel)	((channel < (MAXCHANNEL - CH_20MHZ_APART)) ? \
+					(channel + CH_20MHZ_APART) : 0)
+#define CH40MHZ_CHSPEC(channel, ctlsb)	(chanspec_t) \
+					((channel) | (ctlsb) | WL_CHANSPEC_BW_40 | \
+					((channel) <= CH_MAX_2G_CHANNEL ? WL_CHANSPEC_BAND_2G : \
+					WL_CHANSPEC_BAND_5G))
+#define CHSPEC_CHANNEL(chspec)	((uint8)(chspec & WL_CHANSPEC_CHAN_MASK))
+#define CHSPEC_BAND(chspec)	(chspec & WL_CHANSPEC_BAND_MASK)
+
+#ifdef WL20MHZ_ONLY
+
+#define CHSPEC_CTL_SB(chspec)	WL_CHANSPEC_CTL_SB_NONE
+#define CHSPEC_BW(chspec)	WL_CHANSPEC_BW_20
+#define CHSPEC_IS10(chspec)	0
+#define CHSPEC_IS20(chspec)	1
+#ifndef CHSPEC_IS40
+#define CHSPEC_IS40(chspec)	0
+#endif
+
+#else 
+
+#define CHSPEC_CTL_SB(chspec)	(chspec & WL_CHANSPEC_CTL_SB_MASK)
+#define CHSPEC_BW(chspec)	(chspec & WL_CHANSPEC_BW_MASK)
+#define CHSPEC_IS10(chspec)	((chspec & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_10)
+#define CHSPEC_IS20(chspec)	((chspec & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_20)
+#ifndef CHSPEC_IS40
+#define CHSPEC_IS40(chspec)	(((chspec) & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_40)
+#endif
+
+#endif 
+
+#define CHSPEC_IS5G(chspec)	((chspec & WL_CHANSPEC_BAND_MASK) == WL_CHANSPEC_BAND_5G)
+#define CHSPEC_IS2G(chspec)	((chspec & WL_CHANSPEC_BAND_MASK) == WL_CHANSPEC_BAND_2G)
+#define CHSPEC_SB_NONE(chspec)	((chspec & WL_CHANSPEC_CTL_SB_MASK) == WL_CHANSPEC_CTL_SB_NONE)
+#define CHSPEC_SB_UPPER(chspec)	((chspec & WL_CHANSPEC_CTL_SB_MASK) == WL_CHANSPEC_CTL_SB_UPPER)
+#define CHSPEC_SB_LOWER(chspec)	((chspec & WL_CHANSPEC_CTL_SB_MASK) == WL_CHANSPEC_CTL_SB_LOWER)
+#define CHSPEC_CTL_CHAN(chspec)  ((CHSPEC_SB_LOWER(chspec)) ? \
+				  (LOWER_20_SB(((chspec) & WL_CHANSPEC_CHAN_MASK))) : \
+				  (UPPER_20_SB(((chspec) & WL_CHANSPEC_CHAN_MASK))))
+
+#define CHSPEC2WLC_BAND(chspec) (CHSPEC_IS5G((chspec))? WLC_BAND_5G: WLC_BAND_2G)
+
+#define CHANSPEC_STR_LEN    8
+
+
+#define WLC_MAXRATE	108	
+#define WLC_RATE_1M	2	
+#define WLC_RATE_2M	4	
+#define WLC_RATE_5M5	11	
+#define WLC_RATE_11M	22	
+#define WLC_RATE_6M	12	
+#define WLC_RATE_9M	18	
+#define WLC_RATE_12M	24	
+#define WLC_RATE_18M	36	
+#define WLC_RATE_24M	48	
+#define WLC_RATE_36M	72	
+#define WLC_RATE_48M	96	
+#define WLC_RATE_54M	108	
+
+#define WLC_2G_25MHZ_OFFSET		5	
+
+
+extern char * wf_chspec_ntoa(chanspec_t chspec, char *buf);
+
+
+extern chanspec_t wf_chspec_aton(char *a);
+
+
+extern int wf_mhz2channel(uint freq, uint start_factor);
+
+
+extern int wf_channel2mhz(uint channel, uint start_factor);
+
+#endif	
diff --git a/drivers/net/wireless/bcm4329/include/dhdioctl.h b/drivers/net/wireless/bcm4329/include/dhdioctl.h
new file mode 100644
index 000000000000..980a14301003
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/dhdioctl.h
@@ -0,0 +1,123 @@
+/*
+ * Definitions for ioctls to access DHD iovars.
+ * Based on wlioctl.h (for Broadcom 802.11abg driver).
+ * (Moves towards generic ioctls for BCM drivers/iovars.)
+ *
+ * Definitions subject to change without notice.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhdioctl.h,v 13.7.8.1.4.1.16.5 2010/05/21 21:49:38 Exp $
+ */
+
+#ifndef _dhdioctl_h_
+#define	_dhdioctl_h_
+
+#include <typedefs.h>
+
+
+/* require default structure packing */
+#define BWL_DEFAULT_PACKING
+#include <packed_section_start.h>
+
+
+/* Linux network driver ioctl encoding */
+typedef struct dhd_ioctl {
+	uint cmd;	/* common ioctl definition */
+	void *buf;	/* pointer to user buffer */
+	uint len;	/* length of user buffer */
+	bool set;	/* get or set request (optional) */
+	uint used;	/* bytes read or written (optional) */
+	uint needed;	/* bytes needed (optional) */
+	uint driver;	/* to identify target driver */
+} dhd_ioctl_t;
+
+/* per-driver magic numbers */
+#define DHD_IOCTL_MAGIC		0x00444944
+
+/* bump this number if you change the ioctl interface */
+#define DHD_IOCTL_VERSION	1
+
+#define	DHD_IOCTL_MAXLEN	8192		/* max length ioctl buffer required */
+#define	DHD_IOCTL_SMLEN		256		/* "small" length ioctl buffer required */
+
+/* common ioctl definitions */
+#define DHD_GET_MAGIC				0
+#define DHD_GET_VERSION				1
+#define DHD_GET_VAR				2
+#define DHD_SET_VAR				3
+
+/* message levels */
+#define DHD_ERROR_VAL	0x0001
+#define DHD_TRACE_VAL	0x0002
+#define DHD_INFO_VAL	0x0004
+#define DHD_DATA_VAL	0x0008
+#define DHD_CTL_VAL	0x0010
+#define DHD_TIMER_VAL	0x0020
+#define DHD_HDRS_VAL	0x0040
+#define DHD_BYTES_VAL	0x0080
+#define DHD_INTR_VAL	0x0100
+#define DHD_LOG_VAL	0x0200
+#define DHD_GLOM_VAL	0x0400
+#define DHD_EVENT_VAL	0x0800
+#define DHD_BTA_VAL	0x1000
+#define DHD_ISCAN_VAL 0x2000
+
+#ifdef SDTEST
+/* For pktgen iovar */
+typedef struct dhd_pktgen {
+	uint version;		/* To allow structure change tracking */
+	uint freq;		/* Max ticks between tx/rx attempts */
+	uint count;		/* Test packets to send/rcv each attempt */
+	uint print;		/* Print counts every <print> attempts */
+	uint total;		/* Total packets (or bursts) */
+	uint minlen;		/* Minimum length of packets to send */
+	uint maxlen;		/* Maximum length of packets to send */
+	uint numsent;		/* Count of test packets sent */
+	uint numrcvd;		/* Count of test packets received */
+	uint numfail;		/* Count of test send failures */
+	uint mode;		/* Test mode (type of test packets) */
+	uint stop;		/* Stop after this many tx failures */
+} dhd_pktgen_t;
+
+/* Version in case structure changes */
+#define DHD_PKTGEN_VERSION 2
+
+/* Type of test packets to use */
+#define DHD_PKTGEN_ECHO		1 /* Send echo requests */
+#define DHD_PKTGEN_SEND 	2 /* Send discard packets */
+#define DHD_PKTGEN_RXBURST	3 /* Request dongle send N packets */
+#define DHD_PKTGEN_RECV		4 /* Continuous rx from continuous tx dongle */
+#endif /* SDTEST */
+
+/* Enter idle immediately (no timeout) */
+#define DHD_IDLE_IMMEDIATE	(-1)
+
+/* Values for idleclock iovar: other values are the sd_divisor to use when idle */
+#define DHD_IDLE_ACTIVE	0	/* Do not request any SD clock change when idle */
+#define DHD_IDLE_STOP   (-1)	/* Request SD clock be stopped (and use SD1 mode) */
+
+
+/* require default structure packing */
+#include <packed_section_end.h>
+
+
+#endif /* _dhdioctl_h_ */
diff --git a/drivers/net/wireless/bcm4329/include/epivers.h b/drivers/net/wireless/bcm4329/include/epivers.h
new file mode 100644
index 000000000000..cd66a9501cb6
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/epivers.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: epivers.h.in,v 13.25 2005/10/28 18:35:33 Exp $
+ *
+*/
+
+
+#ifndef _epivers_h_
+#define _epivers_h_
+
+#define	EPI_MAJOR_VERSION	4
+
+#define	EPI_MINOR_VERSION	218
+
+#define	EPI_RC_NUMBER		248
+
+#define	EPI_INCREMENTAL_NUMBER	23
+
+#define	EPI_BUILD_NUMBER	0
+
+#define	EPI_VERSION		4, 218, 248, 23
+
+#define	EPI_VERSION_NUM		0x04daf817
+
+
+#define	EPI_VERSION_STR		"4.218.248.23"
+#define	EPI_ROUTER_VERSION_STR	"4.219.248.23"
+
+#endif 
diff --git a/drivers/net/wireless/bcm4329/include/hndpmu.h b/drivers/net/wireless/bcm4329/include/hndpmu.h
new file mode 100644
index 000000000000..e829b3df2d0b
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/hndpmu.h
@@ -0,0 +1,34 @@
+/*
+ * HND SiliconBackplane PMU support.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: hndpmu.h,v 13.14.4.3.4.3.8.7 2010/04/09 13:20:51 Exp $
+ */
+
+#ifndef _hndpmu_h_
+#define _hndpmu_h_
+
+
+extern void si_pmu_otp_power(si_t *sih, osl_t *osh, bool on);
+extern void si_sdiod_drive_strength_init(si_t *sih, osl_t *osh, uint32 drivestrength);
+
+#endif /* _hndpmu_h_ */
diff --git a/drivers/net/wireless/bcm4329/include/hndrte_armtrap.h b/drivers/net/wireless/bcm4329/include/hndrte_armtrap.h
new file mode 100644
index 000000000000..ca3281b6d901
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/hndrte_armtrap.h
@@ -0,0 +1,88 @@
+/*
+ * HNDRTE arm trap handling.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: hndrte_armtrap.h,v 13.3.196.2 2010/07/15 19:06:11 Exp $
+ */
+
+#ifndef	_hndrte_armtrap_h
+#define	_hndrte_armtrap_h
+
+
+/* ARM trap handling */
+
+/* Trap types defined by ARM (see arminc.h) */
+
+/* Trap locations in lo memory */
+#define	TRAP_STRIDE	4
+#define FIRST_TRAP	TR_RST
+#define LAST_TRAP	(TR_FIQ * TRAP_STRIDE)
+
+#if defined(__ARM_ARCH_4T__)
+#define	MAX_TRAP_TYPE	(TR_FIQ + 1)
+#elif defined(__ARM_ARCH_7M__)
+#define	MAX_TRAP_TYPE	(TR_ISR + ARMCM3_NUMINTS)
+#endif	/* __ARM_ARCH_7M__ */
+
+/* The trap structure is defined here as offsets for assembly */
+#define	TR_TYPE		0x00
+#define	TR_EPC		0x04
+#define	TR_CPSR		0x08
+#define	TR_SPSR		0x0c
+#define	TR_REGS		0x10
+#define	TR_REG(n)	(TR_REGS + (n) * 4)
+#define	TR_SP		TR_REG(13)
+#define	TR_LR		TR_REG(14)
+#define	TR_PC		TR_REG(15)
+
+#define	TRAP_T_SIZE	80
+
+#ifndef	_LANGUAGE_ASSEMBLY
+
+#include <typedefs.h>
+
+typedef struct _trap_struct {
+	uint32		type;
+	uint32		epc;
+	uint32		cpsr;
+	uint32		spsr;
+	uint32		r0;
+	uint32		r1;
+	uint32		r2;
+	uint32		r3;
+	uint32		r4;
+	uint32		r5;
+	uint32		r6;
+	uint32		r7;
+	uint32		r8;
+	uint32		r9;
+	uint32		r10;
+	uint32		r11;
+	uint32		r12;
+	uint32		r13;
+	uint32		r14;
+	uint32		pc;
+} trap_t;
+
+#endif	/* !_LANGUAGE_ASSEMBLY */
+
+#endif	/* _hndrte_armtrap_h */
diff --git a/drivers/net/wireless/bcm4329/include/hndrte_cons.h b/drivers/net/wireless/bcm4329/include/hndrte_cons.h
new file mode 100644
index 000000000000..a42417478a16
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/hndrte_cons.h
@@ -0,0 +1,63 @@
+/*
+ * Console support for hndrte.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: hndrte_cons.h,v 13.1.2.4 2010/07/15 19:06:11 Exp $
+ */
+
+#include <typedefs.h>
+
+#define CBUF_LEN	(128)
+
+#define LOG_BUF_LEN	1024
+
+typedef struct {
+	uint32		buf;		/* Can't be pointer on (64-bit) hosts */
+	uint		buf_size;
+	uint		idx;
+	char		*_buf_compat;	/* Redundant pointer for backward compat. */
+} hndrte_log_t;
+
+typedef struct {
+	/* Virtual UART
+	 *   When there is no UART (e.g. Quickturn), the host should write a complete
+	 *   input line directly into cbuf and then write the length into vcons_in.
+	 *   This may also be used when there is a real UART (at risk of conflicting with
+	 *   the real UART).  vcons_out is currently unused.
+	 */
+	volatile uint	vcons_in;
+	volatile uint	vcons_out;
+
+	/* Output (logging) buffer
+	 *   Console output is written to a ring buffer log_buf at index log_idx.
+	 *   The host may read the output when it sees log_idx advance.
+	 *   Output will be lost if the output wraps around faster than the host polls.
+	 */
+	hndrte_log_t	log;
+
+	/* Console input line buffer
+	 *   Characters are read one at a time into cbuf until <CR> is received, then
+	 *   the buffer is processed as a command line.  Also used for virtual UART.
+	 */
+	uint		cbuf_idx;
+	char		cbuf[CBUF_LEN];
+} hndrte_cons_t;
diff --git a/drivers/net/wireless/bcm4329/include/hndsoc.h b/drivers/net/wireless/bcm4329/include/hndsoc.h
new file mode 100644
index 000000000000..35424175f55e
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/hndsoc.h
@@ -0,0 +1,195 @@
+/*
+ * Broadcom HND chip & on-chip-interconnect-related definitions.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: hndsoc.h,v 13.3.10.3 2008/08/06 03:43:25 Exp $
+ */
+
+#ifndef	_HNDSOC_H
+#define	_HNDSOC_H
+
+/* Include the soci specific files */
+#include <sbconfig.h>
+#include <aidmp.h>
+
+/*
+ * SOC Interconnect Address Map.
+ * All regions may not exist on all chips.
+ */
+#define SI_SDRAM_BASE		0x00000000	/* Physical SDRAM */
+#define SI_PCI_MEM		0x08000000	/* Host Mode sb2pcitranslation0 (64 MB) */
+#define SI_PCI_MEM_SZ		(64 * 1024 * 1024)
+#define SI_PCI_CFG		0x0c000000	/* Host Mode sb2pcitranslation1 (64 MB) */
+#define	SI_SDRAM_SWAPPED	0x10000000	/* Byteswapped Physical SDRAM */
+
+#define SI_ENUM_BASE    	0x18000000	/* Enumeration space base */
+#define SI_CORE_SIZE    	0x1000		/* each core gets 4Kbytes for registers */
+#ifndef SI_MAXCORES
+#define	SI_MAXCORES		16		/* Max cores (this is arbitrary, for software
+						 * convenience and could be changed if we
+						 * make any larger chips
+						 */
+#endif
+
+#define	SI_FASTRAM		0x19000000	/* On-chip RAM on chips that also have DDR */
+
+#define	SI_FLASH2		0x1c000000	/* Flash Region 2 (region 1 shadowed here) */
+#define	SI_FLASH2_SZ		0x02000000	/* Size of Flash Region 2 */
+#define	SI_ARMCM3_ROM		0x1e000000	/* ARM Cortex-M3 ROM */
+#define	SI_FLASH1		0x1fc00000	/* MIPS Flash Region 1 */
+#define	SI_FLASH1_SZ		0x00400000	/* MIPS Size of Flash Region 1 */
+#define	SI_ARM7S_ROM		0x20000000	/* ARM7TDMI-S ROM */
+#define	SI_ARMCM3_SRAM2		0x60000000	/* ARM Cortex-M3 SRAM Region 2 */
+#define	SI_ARM7S_SRAM2		0x80000000	/* ARM7TDMI-S SRAM Region 2 */
+#define	SI_ARM_FLASH1		0xffff0000	/* ARM Flash Region 1 */
+#define	SI_ARM_FLASH1_SZ	0x00010000	/* ARM Size of Flash Region 1 */
+
+#define SI_PCI_DMA		0x40000000	/* Client Mode sb2pcitranslation2 (1 GB) */
+#define SI_PCI_DMA2		0x80000000	/* Client Mode sb2pcitranslation2 (1 GB) */
+#define SI_PCI_DMA_SZ		0x40000000	/* Client Mode sb2pcitranslation2 size in bytes */
+#define SI_PCIE_DMA_L32		0x00000000	/* PCIE Client Mode sb2pcitranslation2
+						 * (2 ZettaBytes), low 32 bits
+						 */
+#define SI_PCIE_DMA_H32		0x80000000	/* PCIE Client Mode sb2pcitranslation2
+						 * (2 ZettaBytes), high 32 bits
+						 */
+
+/* core codes */
+#define	NODEV_CORE_ID		0x700		/* Invalid coreid */
+#define	CC_CORE_ID		0x800		/* chipcommon core */
+#define	ILINE20_CORE_ID		0x801		/* iline20 core */
+#define	SRAM_CORE_ID		0x802		/* sram core */
+#define	SDRAM_CORE_ID		0x803		/* sdram core */
+#define	PCI_CORE_ID		0x804		/* pci core */
+#define	MIPS_CORE_ID		0x805		/* mips core */
+#define	ENET_CORE_ID		0x806		/* enet mac core */
+#define	CODEC_CORE_ID		0x807		/* v90 codec core */
+#define	USB_CORE_ID		0x808		/* usb 1.1 host/device core */
+#define	ADSL_CORE_ID		0x809		/* ADSL core */
+#define	ILINE100_CORE_ID	0x80a		/* iline100 core */
+#define	IPSEC_CORE_ID		0x80b		/* ipsec core */
+#define	UTOPIA_CORE_ID		0x80c		/* utopia core */
+#define	PCMCIA_CORE_ID		0x80d		/* pcmcia core */
+#define	SOCRAM_CORE_ID		0x80e		/* internal memory core */
+#define	MEMC_CORE_ID		0x80f		/* memc sdram core */
+#define	OFDM_CORE_ID		0x810		/* OFDM phy core */
+#define	EXTIF_CORE_ID		0x811		/* external interface core */
+#define	D11_CORE_ID		0x812		/* 802.11 MAC core */
+#define	APHY_CORE_ID		0x813		/* 802.11a phy core */
+#define	BPHY_CORE_ID		0x814		/* 802.11b phy core */
+#define	GPHY_CORE_ID		0x815		/* 802.11g phy core */
+#define	MIPS33_CORE_ID		0x816		/* mips3302 core */
+#define	USB11H_CORE_ID		0x817		/* usb 1.1 host core */
+#define	USB11D_CORE_ID		0x818		/* usb 1.1 device core */
+#define	USB20H_CORE_ID		0x819		/* usb 2.0 host core */
+#define	USB20D_CORE_ID		0x81a		/* usb 2.0 device core */
+#define	SDIOH_CORE_ID		0x81b		/* sdio host core */
+#define	ROBO_CORE_ID		0x81c		/* roboswitch core */
+#define	ATA100_CORE_ID		0x81d		/* parallel ATA core */
+#define	SATAXOR_CORE_ID		0x81e		/* serial ATA & XOR DMA core */
+#define	GIGETH_CORE_ID		0x81f		/* gigabit ethernet core */
+#define	PCIE_CORE_ID		0x820		/* pci express core */
+#define	NPHY_CORE_ID		0x821		/* 802.11n 2x2 phy core */
+#define	SRAMC_CORE_ID		0x822		/* SRAM controller core */
+#define	MINIMAC_CORE_ID		0x823		/* MINI MAC/phy core */
+#define	ARM11_CORE_ID		0x824		/* ARM 1176 core */
+#define	ARM7S_CORE_ID		0x825		/* ARM7tdmi-s core */
+#define	LPPHY_CORE_ID		0x826		/* 802.11a/b/g phy core */
+#define	PMU_CORE_ID		0x827		/* PMU core */
+#define	SSNPHY_CORE_ID		0x828		/* 802.11n single-stream phy core */
+#define	SDIOD_CORE_ID		0x829		/* SDIO device core */
+#define	ARMCM3_CORE_ID		0x82a		/* ARM Cortex M3 core */
+#define	QNPHY_CORE_ID		0x82b		/* 802.11n 4x4 phy core */
+#define	MIPS74K_CORE_ID		0x82c		/* mips 74k core */
+#define	GMAC_CORE_ID		0x82d		/* Gigabit MAC core */
+#define	DMEMC_CORE_ID		0x82e		/* DDR1/2 memory controller core */
+#define	PCIERC_CORE_ID		0x82f		/* PCIE Root Complex core */
+#define	OCP_CORE_ID		0x830		/* OCP2OCP bridge core */
+#define	SC_CORE_ID		0x831		/* shared common core */
+#define	AHB_CORE_ID		0x832		/* OCP2AHB bridge core */
+#define	SPIH_CORE_ID		0x833		/* SPI host core */
+#define	I2S_CORE_ID		0x834		/* I2S core */
+#define OOB_ROUTER_CORE_ID	0x367		/* OOB router core ID */
+#define	DEF_AI_COMP		0xfff		/* Default component, in ai chips it maps all
+						 * unused address ranges
+						 */
+
+/* There are TWO constants on all HND chips: SI_ENUM_BASE above,
+ * and chipcommon being the first core:
+ */
+#define	SI_CC_IDX		0
+
+/* SOC Interconnect types (aka chip types) */
+#define	SOCI_SB			0
+#define	SOCI_AI			1
+
+/* Common core control flags */
+#define	SICF_BIST_EN		0x8000
+#define	SICF_PME_EN		0x4000
+#define	SICF_CORE_BITS		0x3ffc
+#define	SICF_FGC		0x0002
+#define	SICF_CLOCK_EN		0x0001
+
+/* Common core status flags */
+#define	SISF_BIST_DONE		0x8000
+#define	SISF_BIST_ERROR		0x4000
+#define	SISF_GATED_CLK		0x2000
+#define	SISF_DMA64		0x1000
+#define	SISF_CORE_BITS		0x0fff
+
+/* A register that is common to all cores to
+ * communicate w/PMU regarding clock control.
+ */
+#define SI_CLK_CTL_ST		0x1e0		/* clock control and status */
+
+/* clk_ctl_st register */
+#define	CCS_FORCEALP		0x00000001	/* force ALP request */
+#define	CCS_FORCEHT		0x00000002	/* force HT request */
+#define	CCS_FORCEILP		0x00000004	/* force ILP request */
+#define	CCS_ALPAREQ		0x00000008	/* ALP Avail Request */
+#define	CCS_HTAREQ		0x00000010	/* HT Avail Request */
+#define	CCS_FORCEHWREQOFF	0x00000020	/* Force HW Clock Request Off */
+#define	CCS_ALPAVAIL		0x00010000	/* ALP is available */
+#define	CCS_HTAVAIL		0x00020000	/* HT is available */
+#define	CCS0_HTAVAIL		0x00010000	/* HT avail in chipc and pcmcia on 4328a0 */
+#define	CCS0_ALPAVAIL		0x00020000	/* ALP avail in chipc and pcmcia on 4328a0 */
+
+/* Not really related to SOC Interconnect, but a couple of software
+ * conventions for the use the flash space:
+ */
+
+/* Minumum amount of flash we support */
+#define FLASH_MIN		0x00020000	/* Minimum flash size */
+
+/* A boot/binary may have an embedded block that describes its size  */
+#define	BISZ_OFFSET		0x3e0		/* At this offset into the binary */
+#define	BISZ_MAGIC		0x4249535a	/* Marked with this value: 'BISZ' */
+#define	BISZ_MAGIC_IDX		0		/* Word 0: magic */
+#define	BISZ_TXTST_IDX		1		/*	1: text start */
+#define	BISZ_TXTEND_IDX		2		/*	2: text end */
+#define	BISZ_DATAST_IDX		3		/*	3: data start */
+#define	BISZ_DATAEND_IDX	4		/*	4: data end */
+#define	BISZ_BSSST_IDX		5		/*	5: bss start */
+#define	BISZ_BSSEND_IDX		6		/*	6: bss end */
+#define BISZ_SIZE		7		/* descriptor size in 32-bit intergers */
+
+#endif /* _HNDSOC_H */
diff --git a/drivers/net/wireless/bcm4329/include/linux_osl.h b/drivers/net/wireless/bcm4329/include/linux_osl.h
new file mode 100644
index 000000000000..b059c2adb17d
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/linux_osl.h
@@ -0,0 +1,322 @@
+/*
+ * Linux OS Independent Layer
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: linux_osl.h,v 13.131.30.8 2010/04/26 05:42:18 Exp $
+ */
+
+
+#ifndef _linux_osl_h_
+#define _linux_osl_h_
+
+#include <typedefs.h>
+
+
+#include <linuxver.h>
+
+
+#ifdef __GNUC__
+#define GCC_VERSION (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__)
+#if GCC_VERSION > 30100
+#define	ASSERT(exp)		do {} while (0)
+#else
+
+#define	ASSERT(exp)
+#endif 
+#endif 
+
+
+#define	OSL_DELAY(usec)		osl_delay(usec)
+extern void osl_delay(uint usec);
+
+
+
+#define	OSL_PCMCIA_READ_ATTR(osh, offset, buf, size) \
+	osl_pcmcia_read_attr((osh), (offset), (buf), (size))
+#define	OSL_PCMCIA_WRITE_ATTR(osh, offset, buf, size) \
+	osl_pcmcia_write_attr((osh), (offset), (buf), (size))
+extern void osl_pcmcia_read_attr(osl_t *osh, uint offset, void *buf, int size);
+extern void osl_pcmcia_write_attr(osl_t *osh, uint offset, void *buf, int size);
+
+
+#define	OSL_PCI_READ_CONFIG(osh, offset, size) \
+	osl_pci_read_config((osh), (offset), (size))
+#define	OSL_PCI_WRITE_CONFIG(osh, offset, size, val) \
+	osl_pci_write_config((osh), (offset), (size), (val))
+extern uint32 osl_pci_read_config(osl_t *osh, uint offset, uint size);
+extern void osl_pci_write_config(osl_t *osh, uint offset, uint size, uint val);
+
+
+#define OSL_PCI_BUS(osh)	osl_pci_bus(osh)
+#define OSL_PCI_SLOT(osh)	osl_pci_slot(osh)
+extern uint osl_pci_bus(osl_t *osh);
+extern uint osl_pci_slot(osl_t *osh);
+
+
+typedef struct {
+	bool pkttag;
+	uint pktalloced; 	
+	bool mmbus;		
+	pktfree_cb_fn_t tx_fn;  
+	void *tx_ctx;		
+} osl_pubinfo_t;
+
+
+extern osl_t *osl_attach(void *pdev, uint bustype, bool pkttag);
+extern void osl_detach(osl_t *osh);
+
+#define PKTFREESETCB(osh, _tx_fn, _tx_ctx) \
+	do { \
+	   ((osl_pubinfo_t*)osh)->tx_fn = _tx_fn; \
+	   ((osl_pubinfo_t*)osh)->tx_ctx = _tx_ctx; \
+	} while (0)
+
+
+#define BUS_SWAP32(v)		(v)
+
+
+#define	MALLOC(osh, size)	osl_malloc((osh), (size))
+#define	MFREE(osh, addr, size)	osl_mfree((osh), (addr), (size))
+#define MALLOCED(osh)		osl_malloced((osh))
+
+
+#define	MALLOC_FAILED(osh)	osl_malloc_failed((osh))
+
+extern void *osl_malloc(osl_t *osh, uint size);
+extern void osl_mfree(osl_t *osh, void *addr, uint size);
+extern uint osl_malloced(osl_t *osh);
+extern uint osl_malloc_failed(osl_t *osh);
+
+
+#define	DMA_CONSISTENT_ALIGN	PAGE_SIZE
+#define	DMA_ALLOC_CONSISTENT(osh, size, pap, dmah, alignbits) \
+	osl_dma_alloc_consistent((osh), (size), (pap))
+#define	DMA_FREE_CONSISTENT(osh, va, size, pa, dmah) \
+	osl_dma_free_consistent((osh), (void*)(va), (size), (pa))
+extern void *osl_dma_alloc_consistent(osl_t *osh, uint size, ulong *pap);
+extern void osl_dma_free_consistent(osl_t *osh, void *va, uint size, ulong pa);
+
+
+#define	DMA_TX	1	
+#define	DMA_RX	2	
+
+
+#define	DMA_MAP(osh, va, size, direction, p, dmah) \
+	osl_dma_map((osh), (va), (size), (direction))
+#define	DMA_UNMAP(osh, pa, size, direction, p, dmah) \
+	osl_dma_unmap((osh), (pa), (size), (direction))
+extern uint osl_dma_map(osl_t *osh, void *va, uint size, int direction);
+extern void osl_dma_unmap(osl_t *osh, uint pa, uint size, int direction);
+
+
+#define OSL_DMADDRWIDTH(osh, addrwidth) do {} while (0)
+
+
+#include <bcmsdh.h>
+#define OSL_WRITE_REG(osh, r, v) (bcmsdh_reg_write(NULL, (uintptr)(r), sizeof(*(r)), (v)))
+#define OSL_READ_REG(osh, r) (bcmsdh_reg_read(NULL, (uintptr)(r), sizeof(*(r))))
+
+#define SELECT_BUS_WRITE(osh, mmap_op, bus_op) if (((osl_pubinfo_t*)(osh))->mmbus) \
+	mmap_op else bus_op
+#define SELECT_BUS_READ(osh, mmap_op, bus_op) (((osl_pubinfo_t*)(osh))->mmbus) ? \
+	mmap_op : bus_op
+
+
+
+
+#ifndef printf
+#define	printf(fmt, args...)	printk(fmt, ## args)
+#endif 
+#include <linux/kernel.h>
+#include <linux/string.h>
+
+
+#ifndef IL_BIGENDIAN
+#define R_REG(osh, r) (\
+	SELECT_BUS_READ(osh, sizeof(*(r)) == sizeof(uint8) ? readb((volatile uint8*)(r)) : \
+	sizeof(*(r)) == sizeof(uint16) ? readw((volatile uint16*)(r)) : \
+	readl((volatile uint32*)(r)), OSL_READ_REG(osh, r)) \
+)
+#define W_REG(osh, r, v) do { \
+	SELECT_BUS_WRITE(osh,  \
+		switch (sizeof(*(r))) { \
+			case sizeof(uint8):	writeb((uint8)(v), (volatile uint8*)(r)); break; \
+			case sizeof(uint16):	writew((uint16)(v), (volatile uint16*)(r)); break; \
+			case sizeof(uint32):	writel((uint32)(v), (volatile uint32*)(r)); break; \
+		}, \
+		(OSL_WRITE_REG(osh, r, v))); \
+	} while (0)
+#else	
+#define R_REG(osh, r) (\
+	SELECT_BUS_READ(osh, \
+		({ \
+			__typeof(*(r)) __osl_v; \
+			switch (sizeof(*(r))) { \
+				case sizeof(uint8):	__osl_v = \
+					readb((volatile uint8*)((uintptr)(r)^3)); break; \
+				case sizeof(uint16):	__osl_v = \
+					readw((volatile uint16*)((uintptr)(r)^2)); break; \
+				case sizeof(uint32):	__osl_v = \
+					readl((volatile uint32*)(r)); break; \
+			} \
+			__osl_v; \
+		}), \
+		OSL_READ_REG(osh, r)) \
+)
+#define W_REG(osh, r, v) do { \
+	SELECT_BUS_WRITE(osh,  \
+		switch (sizeof(*(r))) { \
+			case sizeof(uint8):	writeb((uint8)(v), \
+					(volatile uint8*)((uintptr)(r)^3)); break; \
+			case sizeof(uint16):	writew((uint16)(v), \
+					(volatile uint16*)((uintptr)(r)^2)); break; \
+			case sizeof(uint32):	writel((uint32)(v), \
+					(volatile uint32*)(r)); break; \
+		}, \
+		(OSL_WRITE_REG(osh, r, v))); \
+	} while (0)
+#endif 
+
+#define	AND_REG(osh, r, v)		W_REG(osh, (r), R_REG(osh, r) & (v))
+#define	OR_REG(osh, r, v)		W_REG(osh, (r), R_REG(osh, r) | (v))
+
+
+#define	bcopy(src, dst, len)	memcpy((dst), (src), (len))
+#define	bcmp(b1, b2, len)	memcmp((b1), (b2), (len))
+#define	bzero(b, len)		memset((b), '\0', (len))
+
+
+#define OSL_UNCACHED(va)	((void*)va)
+
+
+#if defined(__i386__)
+#define	OSL_GETCYCLES(x)	rdtscl((x))
+#else
+#define OSL_GETCYCLES(x)	((x) = 0)
+#endif 
+
+
+#define	BUSPROBE(val, addr)	({ (val) = R_REG(NULL, (addr)); 0; })
+
+
+#if !defined(CONFIG_MMC_MSM7X00A)
+#define	REG_MAP(pa, size)	ioremap_nocache((unsigned long)(pa), (unsigned long)(size))
+#else
+#define REG_MAP(pa, size)       (void *)(0)
+#endif 
+#define	REG_UNMAP(va)		iounmap((va))
+
+
+#define	R_SM(r)			*(r)
+#define	W_SM(r, v)		(*(r) = (v))
+#define	BZERO_SM(r, len)	memset((r), '\0', (len))
+
+
+#define	PKTGET(osh, len, send)		osl_pktget((osh), (len))
+#define	PKTFREE(osh, skb, send)		osl_pktfree((osh), (skb), (send))
+#ifdef DHD_USE_STATIC_BUF
+#define	PKTGET_STATIC(osh, len, send)		osl_pktget_static((osh), (len))
+#define	PKTFREE_STATIC(osh, skb, send)		osl_pktfree_static((osh), (skb), (send))
+#endif 
+#define	PKTDATA(osh, skb)		(((struct sk_buff*)(skb))->data)
+#define	PKTLEN(osh, skb)		(((struct sk_buff*)(skb))->len)
+#define PKTHEADROOM(osh, skb)		(PKTDATA(osh, skb)-(((struct sk_buff*)(skb))->head))
+#define PKTTAILROOM(osh, skb) ((((struct sk_buff*)(skb))->end)-(((struct sk_buff*)(skb))->tail))
+#define	PKTNEXT(osh, skb)		(((struct sk_buff*)(skb))->next)
+#define	PKTSETNEXT(osh, skb, x)		(((struct sk_buff*)(skb))->next = (struct sk_buff*)(x))
+#define	PKTSETLEN(osh, skb, len)	__skb_trim((struct sk_buff*)(skb), (len))
+#define	PKTPUSH(osh, skb, bytes)	skb_push((struct sk_buff*)(skb), (bytes))
+#define	PKTPULL(osh, skb, bytes)	skb_pull((struct sk_buff*)(skb), (bytes))
+#define	PKTDUP(osh, skb)		osl_pktdup((osh), (skb))
+#define	PKTTAG(skb)			((void*)(((struct sk_buff*)(skb))->cb))
+#define PKTALLOCED(osh)			((osl_pubinfo_t *)(osh))->pktalloced
+#define PKTSETPOOL(osh, skb, x, y)	do {} while (0)
+#define PKTPOOL(osh, skb)		FALSE
+#define PKTPOOLLEN(osh, pktp)		(0)
+#define PKTPOOLAVAIL(osh, pktp)		(0)
+#define PKTPOOLADD(osh, pktp, p)	BCME_ERROR
+#define PKTPOOLGET(osh, pktp)		NULL
+#define PKTLIST_DUMP(osh, buf)
+
+extern void *osl_pktget(osl_t *osh, uint len);
+extern void osl_pktfree(osl_t *osh, void *skb, bool send);
+extern void *osl_pktget_static(osl_t *osh, uint len);
+extern void osl_pktfree_static(osl_t *osh, void *skb, bool send);
+extern void *osl_pktdup(osl_t *osh, void *skb);
+
+
+
+static INLINE void *
+osl_pkt_frmnative(osl_pubinfo_t *osh, struct sk_buff *skb)
+{
+	struct sk_buff *nskb;
+
+	if (osh->pkttag)
+		bzero((void*)skb->cb, OSL_PKTTAG_SZ);
+
+	
+	for (nskb = skb; nskb; nskb = nskb->next) {
+		osh->pktalloced++;
+	}
+
+	return (void *)skb;
+}
+#define PKTFRMNATIVE(osh, skb)	osl_pkt_frmnative(((osl_pubinfo_t *)osh), (struct sk_buff*)(skb))
+
+
+static INLINE struct sk_buff *
+osl_pkt_tonative(osl_pubinfo_t *osh, void *pkt)
+{
+	struct sk_buff *nskb;
+
+	if (osh->pkttag)
+		bzero(((struct sk_buff*)pkt)->cb, OSL_PKTTAG_SZ);
+
+	
+	for (nskb = (struct sk_buff *)pkt; nskb; nskb = nskb->next) {
+		osh->pktalloced--;
+	}
+
+	return (struct sk_buff *)pkt;
+}
+#define PKTTONATIVE(osh, pkt)		osl_pkt_tonative((osl_pubinfo_t *)(osh), (pkt))
+
+#define	PKTLINK(skb)			(((struct sk_buff*)(skb))->prev)
+#define	PKTSETLINK(skb, x)		(((struct sk_buff*)(skb))->prev = (struct sk_buff*)(x))
+#define	PKTPRIO(skb)			(((struct sk_buff*)(skb))->priority)
+#define	PKTSETPRIO(skb, x)		(((struct sk_buff*)(skb))->priority = (x))
+#define PKTSUMNEEDED(skb)		(((struct sk_buff*)(skb))->ip_summed == CHECKSUM_HW)
+#define PKTSETSUMGOOD(skb, x)		(((struct sk_buff*)(skb))->ip_summed = \
+						((x) ? CHECKSUM_UNNECESSARY : CHECKSUM_NONE))
+
+#define PKTSHARED(skb)                  (((struct sk_buff*)(skb))->cloned)
+
+
+#define OSL_ERROR(bcmerror)	osl_error(bcmerror)
+extern int osl_error(int bcmerror);
+
+
+#define	PKTBUFSZ	2048   
+
+
+#define OSL_SYSUPTIME()		((uint32)jiffies * (1000 / HZ))
+#endif	
diff --git a/drivers/net/wireless/bcm4329/include/linuxver.h b/drivers/net/wireless/bcm4329/include/linuxver.h
new file mode 100644
index 000000000000..6ed22658a72b
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/linuxver.h
@@ -0,0 +1,447 @@
+/*
+ * Linux-specific abstractions to gain some independence from linux kernel versions.
+ * Pave over some 2.2 versus 2.4 versus 2.6 kernel differences.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: linuxver.h,v 13.38.8.1.8.6 2010/04/29 05:00:46 Exp $
+ */
+
+
+#ifndef _linuxver_h_
+#define _linuxver_h_
+
+#include <linux/version.h>
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
+#include <linux/config.h>
+#elif (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 33))
+#include <linux/autoconf.h>
+#endif
+#include <linux/module.h>
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 0))
+
+#ifdef __UNDEF_NO_VERSION__
+#undef __NO_VERSION__
+#else
+#define __NO_VERSION__
+#endif
+#endif	
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 5, 0)
+#define module_param(_name_, _type_, _perm_)	MODULE_PARM(_name_, "i")
+#define module_param_string(_name_, _string_, _size_, _perm_) \
+		MODULE_PARM(_string_, "c" __MODULE_STRING(_size_))
+#endif
+
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 4, 9))
+#include <linux/malloc.h>
+#else
+#include <linux/slab.h>
+#endif
+
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/pci.h>
+#include <linux/interrupt.h>
+#include <linux/netdevice.h>
+#include <linux/semaphore.h>
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 28))
+#undef IP_TOS
+#endif 
+#include <asm/io.h>
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 5, 41))
+#include <linux/workqueue.h>
+#else
+#include <linux/tqueue.h>
+#ifndef work_struct
+#define work_struct tq_struct
+#endif
+#ifndef INIT_WORK
+#define INIT_WORK(_work, _func, _data) INIT_TQUEUE((_work), (_func), (_data))
+#endif
+#ifndef schedule_work
+#define schedule_work(_work) schedule_task((_work))
+#endif
+#ifndef flush_scheduled_work
+#define flush_scheduled_work() flush_scheduled_tasks()
+#endif
+#endif	
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+#define	MY_INIT_WORK(_work, _func, _data)	INIT_WORK(_work, _func)
+#else
+#define	MY_INIT_WORK(_work, _func, _data)	INIT_WORK(_work, _func, _data)
+typedef void (*work_func_t)(void *work);
+#endif	
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
+
+#ifndef IRQ_NONE
+typedef void irqreturn_t;
+#define IRQ_NONE
+#define IRQ_HANDLED
+#define IRQ_RETVAL(x)
+#endif
+#else
+typedef irqreturn_t(*FN_ISR) (int irq, void *dev_id, struct pt_regs *ptregs);
+#endif	
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18)
+#define IRQF_SHARED	SA_SHIRQ
+#endif 
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 17)
+#ifdef	CONFIG_NET_RADIO
+#define	CONFIG_WIRELESS_EXT
+#endif
+#endif	
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 5, 67)
+#ifndef SANDGATE2G
+#define MOD_INC_USE_COUNT
+#endif 
+#endif 
+
+
+#ifndef __exit
+#define __exit
+#endif
+#ifndef __devexit
+#define __devexit
+#endif
+#ifndef __devinit
+#define __devinit	__init
+#endif
+#ifndef __devinitdata
+#define __devinitdata
+#endif
+#ifndef __devexit_p
+#define __devexit_p(x)	x
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 4, 0))
+
+#define pci_get_drvdata(dev)		(dev)->sysdata
+#define pci_set_drvdata(dev, value)	(dev)->sysdata = (value)
+
+
+
+struct pci_device_id {
+	unsigned int vendor, device;		
+	unsigned int subvendor, subdevice;	
+	unsigned int class, class_mask;		
+	unsigned long driver_data;		
+};
+
+struct pci_driver {
+	struct list_head node;
+	char *name;
+	const struct pci_device_id *id_table;	
+	int (*probe)(struct pci_dev *dev,
+	             const struct pci_device_id *id); 
+	void (*remove)(struct pci_dev *dev);	
+	void (*suspend)(struct pci_dev *dev);	
+	void (*resume)(struct pci_dev *dev);	
+};
+
+#define MODULE_DEVICE_TABLE(type, name)
+#define PCI_ANY_ID (~0)
+
+
+#define pci_module_init pci_register_driver
+extern int pci_register_driver(struct pci_driver *drv);
+extern void pci_unregister_driver(struct pci_driver *drv);
+
+#endif 
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 18))
+#define pci_module_init pci_register_driver
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 2, 18))
+#ifdef MODULE
+#define module_init(x) int init_module(void) { return x(); }
+#define module_exit(x) void cleanup_module(void) { x(); }
+#else
+#define module_init(x)	__initcall(x);
+#define module_exit(x)	__exitcall(x);
+#endif
+#endif	
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 48))
+#define list_for_each(pos, head) \
+	for (pos = (head)->next; pos != (head); pos = pos->next)
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 13))
+#define pci_resource_start(dev, bar)	((dev)->base_address[(bar)])
+#elif (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 44))
+#define pci_resource_start(dev, bar)	((dev)->resource[(bar)].start)
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 23))
+#define pci_enable_device(dev) do { } while (0)
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 14))
+#define net_device device
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 42))
+
+
+
+#ifndef PCI_DMA_TODEVICE
+#define	PCI_DMA_TODEVICE	1
+#define	PCI_DMA_FROMDEVICE	2
+#endif
+
+typedef u32 dma_addr_t;
+
+
+static inline int get_order(unsigned long size)
+{
+	int order;
+
+	size = (size-1) >> (PAGE_SHIFT-1);
+	order = -1;
+	do {
+		size >>= 1;
+		order++;
+	} while (size);
+	return order;
+}
+
+static inline void *pci_alloc_consistent(struct pci_dev *hwdev, size_t size,
+                                         dma_addr_t *dma_handle)
+{
+	void *ret;
+	int gfp = GFP_ATOMIC | GFP_DMA;
+
+	ret = (void *)__get_free_pages(gfp, get_order(size));
+
+	if (ret != NULL) {
+		memset(ret, 0, size);
+		*dma_handle = virt_to_bus(ret);
+	}
+	return ret;
+}
+static inline void pci_free_consistent(struct pci_dev *hwdev, size_t size,
+                                       void *vaddr, dma_addr_t dma_handle)
+{
+	free_pages((unsigned long)vaddr, get_order(size));
+}
+#define pci_map_single(cookie, address, size, dir)	virt_to_bus(address)
+#define pci_unmap_single(cookie, address, size, dir)
+
+#endif 
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 43))
+
+#define dev_kfree_skb_any(a)		dev_kfree_skb(a)
+#define netif_down(dev)			do { (dev)->start = 0; } while (0)
+
+
+#ifndef _COMPAT_NETDEVICE_H
+
+
+
+#define dev_kfree_skb_irq(a)	dev_kfree_skb(a)
+#define netif_wake_queue(dev) \
+		do { clear_bit(0, &(dev)->tbusy); mark_bh(NET_BH); } while (0)
+#define netif_stop_queue(dev)	set_bit(0, &(dev)->tbusy)
+
+static inline void netif_start_queue(struct net_device *dev)
+{
+	dev->tbusy = 0;
+	dev->interrupt = 0;
+	dev->start = 1;
+}
+
+#define netif_queue_stopped(dev)	(dev)->tbusy
+#define netif_running(dev)		(dev)->start
+
+#endif 
+
+#define netif_device_attach(dev)	netif_start_queue(dev)
+#define netif_device_detach(dev)	netif_stop_queue(dev)
+
+
+#define tasklet_struct				tq_struct
+static inline void tasklet_schedule(struct tasklet_struct *tasklet)
+{
+	queue_task(tasklet, &tq_immediate);
+	mark_bh(IMMEDIATE_BH);
+}
+
+static inline void tasklet_init(struct tasklet_struct *tasklet,
+                                void (*func)(unsigned long),
+                                unsigned long data)
+{
+	tasklet->next = NULL;
+	tasklet->sync = 0;
+	tasklet->routine = (void (*)(void *))func;
+	tasklet->data = (void *)data;
+}
+#define tasklet_kill(tasklet)	{ do {} while (0); }
+
+
+#define del_timer_sync(timer) del_timer(timer)
+
+#else
+
+#define netif_down(dev)
+
+#endif 
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 4, 3))
+
+
+#define PREPARE_TQUEUE(_tq, _routine, _data)			\
+	do {							\
+		(_tq)->routine = _routine;			\
+		(_tq)->data = _data;				\
+	} while (0)
+
+
+#define INIT_TQUEUE(_tq, _routine, _data)			\
+	do {							\
+		INIT_LIST_HEAD(&(_tq)->list);			\
+		(_tq)->sync = 0;				\
+		PREPARE_TQUEUE((_tq), (_routine), (_data));	\
+	} while (0)
+
+#endif	
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 4, 6))
+
+
+
+static inline int
+pci_save_state(struct pci_dev *dev, u32 *buffer)
+{
+	int i;
+	if (buffer) {
+		for (i = 0; i < 16; i++)
+			pci_read_config_dword(dev, i * 4, &buffer[i]);
+	}
+	return 0;
+}
+
+static inline int
+pci_restore_state(struct pci_dev *dev, u32 *buffer)
+{
+	int i;
+
+	if (buffer) {
+		for (i = 0; i < 16; i++)
+			pci_write_config_dword(dev, i * 4, buffer[i]);
+	}
+	
+	else {
+		for (i = 0; i < 6; i ++)
+			pci_write_config_dword(dev,
+			                       PCI_BASE_ADDRESS_0 + (i * 4),
+			                       pci_resource_start(dev, i));
+		pci_write_config_byte(dev, PCI_INTERRUPT_LINE, dev->irq);
+	}
+	return 0;
+}
+
+#endif 
+
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 4, 19))
+#define read_c0_count() read_32bit_cp0_register(CP0_COUNT)
+#endif
+
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 24))
+#ifndef SET_MODULE_OWNER
+#define SET_MODULE_OWNER(dev)		do {} while (0)
+#define OLD_MOD_INC_USE_COUNT		MOD_INC_USE_COUNT
+#define OLD_MOD_DEC_USE_COUNT		MOD_DEC_USE_COUNT
+#else
+#define OLD_MOD_INC_USE_COUNT		do {} while (0)
+#define OLD_MOD_DEC_USE_COUNT		do {} while (0)
+#endif
+#else 
+#ifndef SET_MODULE_OWNER
+#define SET_MODULE_OWNER(dev)		do {} while (0)
+#endif
+#ifndef MOD_INC_USE_COUNT
+#define MOD_INC_USE_COUNT			do {} while (0)
+#endif
+#ifndef MOD_DEC_USE_COUNT
+#define MOD_DEC_USE_COUNT			do {} while (0)
+#endif
+#define OLD_MOD_INC_USE_COUNT		MOD_INC_USE_COUNT
+#define OLD_MOD_DEC_USE_COUNT		MOD_DEC_USE_COUNT
+#endif 
+
+#ifndef SET_NETDEV_DEV
+#define SET_NETDEV_DEV(net, pdev)	do {} while (0)
+#endif
+
+#ifndef HAVE_FREE_NETDEV
+#define free_netdev(dev)		kfree(dev)
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
+
+#define af_packet_priv			data
+#endif
+
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 11)
+#define DRV_SUSPEND_STATE_TYPE pm_message_t
+#else
+#define DRV_SUSPEND_STATE_TYPE uint32
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+#define CHECKSUM_HW	CHECKSUM_PARTIAL
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+#define KILL_PROC(pid, sig) \
+{ \
+	struct task_struct *tsk; \
+	tsk = pid_task(find_vpid(pid), PIDTYPE_PID); \
+	if (tsk) send_sig(sig, tsk, 1); \
+}
+#else
+#define KILL_PROC(pid, sig) \
+{ \
+	kill_proc(pid, sig, 1); \
+}
+#endif 
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
+#define netdev_priv(dev) dev->priv
+#endif
+
+#endif 
diff --git a/drivers/net/wireless/bcm4329/include/miniopt.h b/drivers/net/wireless/bcm4329/include/miniopt.h
new file mode 100644
index 000000000000..3667fb1e215b
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/miniopt.h
@@ -0,0 +1,77 @@
+/*
+ * Command line options parser.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: miniopt.h,v 1.1.6.2 2009/01/14 23:52:48 Exp $
+ */
+
+
+#ifndef MINI_OPT_H
+#define MINI_OPT_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* ---- Include Files ---------------------------------------------------- */
+/* ---- Constants and Types ---------------------------------------------- */
+
+#define MINIOPT_MAXKEY	128	/* Max options */
+typedef struct miniopt {
+
+	/* These are persistent after miniopt_init() */
+	const char* name;		/* name for prompt in error strings */
+	const char* flags;		/* option chars that take no args */
+	bool longflags;		/* long options may be flags */
+	bool opt_end;		/* at end of options (passed a "--") */
+
+	/* These are per-call to miniopt() */
+
+	int consumed;		/* number of argv entries cosumed in
+				 * the most recent call to miniopt()
+				 */
+	bool positional;
+	bool good_int;		/* 'val' member is the result of a sucessful
+				 * strtol conversion of the option value
+				 */
+	char opt;
+	char key[MINIOPT_MAXKEY];
+	char* valstr;		/* positional param, or value for the option,
+				 * or null if the option had
+				 * no accompanying value
+				 */
+	uint uval;		/* strtol translation of valstr */
+	int  val;		/* strtol translation of valstr */
+} miniopt_t;
+
+void miniopt_init(miniopt_t *t, const char* name, const char* flags, bool longflags);
+int miniopt(miniopt_t *t, char **argv);
+
+
+/* ---- Variable Externs ------------------------------------------------- */
+/* ---- Function Prototypes ---------------------------------------------- */
+
+
+#ifdef __cplusplus
+	}
+#endif
+
+#endif  /* MINI_OPT_H  */
diff --git a/drivers/net/wireless/bcm4329/include/msgtrace.h b/drivers/net/wireless/bcm4329/include/msgtrace.h
new file mode 100644
index 000000000000..1479086dba3e
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/msgtrace.h
@@ -0,0 +1,72 @@
+/*
+ * Trace messages sent over HBUS
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: msgtrace.h,v 1.1.2.4 2009/01/27 04:09:40 Exp $
+ */
+
+#ifndef	_MSGTRACE_H
+#define	_MSGTRACE_H
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+
+/* This marks the start of a packed structure section. */
+#include <packed_section_start.h>
+
+#define MSGTRACE_VERSION 1
+
+/* Message trace header */
+typedef BWL_PRE_PACKED_STRUCT struct msgtrace_hdr {
+	uint8	version;
+	uint8   spare;
+	uint16	len;	/* Len of the trace */
+	uint32	seqnum;	/* Sequence number of message. Useful if the messsage has been lost
+			 * because of DMA error or a bus reset (ex: SDIO Func2)
+			 */
+	uint32  discarded_bytes;  /* Number of discarded bytes because of trace overflow  */
+	uint32  discarded_printf; /* Number of discarded printf because of trace overflow */
+} BWL_POST_PACKED_STRUCT msgtrace_hdr_t;
+
+#define MSGTRACE_HDRLEN 	sizeof(msgtrace_hdr_t)
+
+/* The hbus driver generates traces when sending a trace message. This causes endless traces.
+ * This flag must be set to TRUE in any hbus traces. The flag is reset in the function msgtrace_put.
+ * This prevents endless traces but generates hasardous lost of traces only in bus device code.
+ * It is recommendat to set this flag in macro SD_TRACE but not in SD_ERROR for avoiding missing
+ * hbus error traces. hbus error trace should not generates endless traces.
+ */
+extern bool msgtrace_hbus_trace;
+
+typedef void (*msgtrace_func_send_t)(void *hdl1, void *hdl2, uint8 *hdr,
+                                     uint16 hdrlen, uint8 *buf, uint16 buflen);
+
+extern void msgtrace_sent(void);
+extern void msgtrace_put(char *buf, int count);
+extern void msgtrace_init(void *hdl1, void *hdl2, msgtrace_func_send_t func_send);
+
+/* This marks the end of a packed structure section. */
+#include <packed_section_end.h>
+
+#endif	/* _MSGTRACE_H */
diff --git a/drivers/net/wireless/bcm4329/include/osl.h b/drivers/net/wireless/bcm4329/include/osl.h
new file mode 100644
index 000000000000..5599e536eeea
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/osl.h
@@ -0,0 +1,55 @@
+/*
+ * OS Abstraction Layer
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: osl.h,v 13.37.32.1 2008/11/20 00:51:15 Exp $
+ */
+
+
+#ifndef _osl_h_
+#define _osl_h_
+
+
+typedef struct osl_info osl_t;
+typedef struct osl_dmainfo osldma_t;
+
+#define OSL_PKTTAG_SZ	32 
+
+
+typedef void (*pktfree_cb_fn_t)(void *ctx, void *pkt, unsigned int status);
+
+#include <linux_osl.h>
+
+
+
+
+#define	SET_REG(osh, r, mask, val)	W_REG((osh), (r), ((R_REG((osh), r) & ~(mask)) | (val)))
+
+#ifndef AND_REG
+#define AND_REG(osh, r, v)		W_REG(osh, (r), R_REG(osh, r) & (v))
+#endif   
+
+#ifndef OR_REG
+#define OR_REG(osh, r, v)		W_REG(osh, (r), R_REG(osh, r) | (v))
+#endif   
+
+
+#endif	
diff --git a/drivers/net/wireless/bcm4329/include/packed_section_end.h b/drivers/net/wireless/bcm4329/include/packed_section_end.h
new file mode 100644
index 000000000000..5b61c18fcd08
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/packed_section_end.h
@@ -0,0 +1,54 @@
+/*
+ * Declare directives for structure packing. No padding will be provided
+ * between the members of packed structures, and therefore, there is no
+ * guarantee that structure members will be aligned.
+ *
+ * Declaring packed structures is compiler specific. In order to handle all
+ * cases, packed structures should be delared as:
+ *
+ * #include <packed_section_start.h>
+ *
+ * typedef BWL_PRE_PACKED_STRUCT struct foobar_t {
+ *    some_struct_members;
+ * } BWL_POST_PACKED_STRUCT foobar_t;
+ *
+ * #include <packed_section_end.h>
+ *
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: packed_section_end.h,v 1.1.6.3 2008/12/10 00:27:54 Exp $
+ */
+
+
+
+
+#ifdef BWL_PACKED_SECTION
+	#undef BWL_PACKED_SECTION
+#else
+	#error "BWL_PACKED_SECTION is NOT defined!"
+#endif
+
+
+
+
+
+#undef	BWL_PRE_PACKED_STRUCT
+#undef	BWL_POST_PACKED_STRUCT
diff --git a/drivers/net/wireless/bcm4329/include/packed_section_start.h b/drivers/net/wireless/bcm4329/include/packed_section_start.h
new file mode 100644
index 000000000000..cb93aa64079a
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/packed_section_start.h
@@ -0,0 +1,61 @@
+/*
+ * Declare directives for structure packing. No padding will be provided
+ * between the members of packed structures, and therefore, there is no
+ * guarantee that structure members will be aligned.
+ *
+ * Declaring packed structures is compiler specific. In order to handle all
+ * cases, packed structures should be delared as:
+ *
+ * #include <packed_section_start.h>
+ *
+ * typedef BWL_PRE_PACKED_STRUCT struct foobar_t {
+ *    some_struct_members;
+ * } BWL_POST_PACKED_STRUCT foobar_t;
+ *
+ * #include <packed_section_end.h>
+ *
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: packed_section_start.h,v 1.1.6.3 2008/12/10 00:27:54 Exp $
+ */
+
+
+
+
+#ifdef BWL_PACKED_SECTION
+	#error "BWL_PACKED_SECTION is already defined!"
+#else
+	#define BWL_PACKED_SECTION
+#endif
+
+
+
+
+
+#if defined(__GNUC__)
+	#define	BWL_PRE_PACKED_STRUCT
+	#define	BWL_POST_PACKED_STRUCT	__attribute__((packed))
+#elif defined(__CC_ARM)
+	#define	BWL_PRE_PACKED_STRUCT	__packed
+	#define	BWL_POST_PACKED_STRUCT
+#else
+	#error "Unknown compiler!"
+#endif
diff --git a/drivers/net/wireless/bcm4329/include/pcicfg.h b/drivers/net/wireless/bcm4329/include/pcicfg.h
new file mode 100644
index 000000000000..898962c942a8
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/pcicfg.h
@@ -0,0 +1,52 @@
+/*
+ * pcicfg.h: PCI configuration constants and structures.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: pcicfg.h,v 1.41.12.3 2008/06/26 22:49:41 Exp $
+ */
+
+
+#ifndef	_h_pcicfg_
+#define	_h_pcicfg_
+
+
+#define	PCI_CFG_VID		0
+#define	PCI_CFG_CMD		4
+#define	PCI_CFG_REV		8
+#define	PCI_CFG_BAR0		0x10
+#define	PCI_CFG_BAR1		0x14
+#define	PCI_BAR0_WIN		0x80	
+#define	PCI_INT_STATUS		0x90	
+#define	PCI_INT_MASK		0x94	
+
+#define PCIE_EXTCFG_OFFSET	0x100
+#define	PCI_BAR0_PCIREGS_OFFSET	(6 * 1024)	
+#define	PCI_BAR0_PCISBR_OFFSET	(4 * 1024)	
+
+#define PCI_BAR0_WINSZ		(16 * 1024)	
+
+
+#define	PCI_16KB0_PCIREGS_OFFSET (8 * 1024)	
+#define	PCI_16KB0_CCREGS_OFFSET	(12 * 1024)	
+#define PCI_16KBB0_WINSZ	(16 * 1024)	
+
+#endif	
diff --git a/drivers/net/wireless/bcm4329/include/proto/802.11.h b/drivers/net/wireless/bcm4329/include/proto/802.11.h
new file mode 100644
index 000000000000..fd26317361da
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/proto/802.11.h
@@ -0,0 +1,1433 @@
+/*
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * Fundamental types and constants relating to 802.11
+ *
+ * $Id: 802.11.h,v 9.219.4.1.4.5.6.11 2010/02/09 13:23:26 Exp $
+ */
+
+
+#ifndef _802_11_H_
+#define _802_11_H_
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+#ifndef _NET_ETHERNET_H_
+#include <proto/ethernet.h>
+#endif
+
+#include <proto/wpa.h>
+
+
+#include <packed_section_start.h>
+
+
+#define DOT11_TU_TO_US			1024	
+
+
+#define DOT11_A3_HDR_LEN		24	
+#define DOT11_A4_HDR_LEN		30	
+#define DOT11_MAC_HDR_LEN		DOT11_A3_HDR_LEN	
+#define DOT11_FCS_LEN			4	
+#define DOT11_ICV_LEN			4	
+#define DOT11_ICV_AES_LEN		8	
+#define DOT11_QOS_LEN			2	
+#define DOT11_HTC_LEN			4	
+
+#define DOT11_KEY_INDEX_SHIFT		6	
+#define DOT11_IV_LEN			4	
+#define DOT11_IV_TKIP_LEN		8	
+#define DOT11_IV_AES_OCB_LEN		4	
+#define DOT11_IV_AES_CCM_LEN		8	
+#define DOT11_IV_MAX_LEN		8	
+
+
+#define DOT11_MAX_MPDU_BODY_LEN		2304	
+
+#define DOT11_MAX_MPDU_LEN		(DOT11_A4_HDR_LEN + \
+					 DOT11_QOS_LEN + \
+					 DOT11_IV_AES_CCM_LEN + \
+					 DOT11_MAX_MPDU_BODY_LEN + \
+					 DOT11_ICV_LEN + \
+					 DOT11_FCS_LEN)	
+
+#define DOT11_MAX_SSID_LEN		32	
+
+
+#define DOT11_DEFAULT_RTS_LEN		2347	
+#define DOT11_MAX_RTS_LEN		2347	
+
+
+#define DOT11_MIN_FRAG_LEN		256	
+#define DOT11_MAX_FRAG_LEN		2346	
+#define DOT11_DEFAULT_FRAG_LEN		2346	
+
+
+#define DOT11_MIN_BEACON_PERIOD		1	
+#define DOT11_MAX_BEACON_PERIOD		0xFFFF	
+
+
+#define DOT11_MIN_DTIM_PERIOD		1	
+#define DOT11_MAX_DTIM_PERIOD		0xFF	
+
+
+#define DOT11_LLC_SNAP_HDR_LEN		8	
+#define DOT11_OUI_LEN			3	
+BWL_PRE_PACKED_STRUCT struct dot11_llc_snap_header {
+	uint8	dsap;				
+	uint8	ssap;				
+	uint8	ctl;				
+	uint8	oui[DOT11_OUI_LEN];		
+	uint16	type;				
+} BWL_POST_PACKED_STRUCT;
+
+
+#define RFC1042_HDR_LEN	(ETHER_HDR_LEN + DOT11_LLC_SNAP_HDR_LEN)	
+
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_header {
+	uint16			fc;		
+	uint16			durid;		
+	struct ether_addr	a1;		
+	struct ether_addr	a2;		
+	struct ether_addr	a3;		
+	uint16			seq;		
+	struct ether_addr	a4;		
+} BWL_POST_PACKED_STRUCT;
+
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_rts_frame {
+	uint16			fc;		
+	uint16			durid;		
+	struct ether_addr	ra;		
+	struct ether_addr	ta;		
+} BWL_POST_PACKED_STRUCT;
+#define	DOT11_RTS_LEN		16		
+
+BWL_PRE_PACKED_STRUCT struct dot11_cts_frame {
+	uint16			fc;		
+	uint16			durid;		
+	struct ether_addr	ra;		
+} BWL_POST_PACKED_STRUCT;
+#define	DOT11_CTS_LEN		10		
+
+BWL_PRE_PACKED_STRUCT struct dot11_ack_frame {
+	uint16			fc;		
+	uint16			durid;		
+	struct ether_addr	ra;		
+} BWL_POST_PACKED_STRUCT;
+#define	DOT11_ACK_LEN		10		
+
+BWL_PRE_PACKED_STRUCT struct dot11_ps_poll_frame {
+	uint16			fc;		
+	uint16			durid;		
+	struct ether_addr	bssid;		
+	struct ether_addr	ta;		
+} BWL_POST_PACKED_STRUCT;
+#define	DOT11_PS_POLL_LEN	16		
+
+BWL_PRE_PACKED_STRUCT struct dot11_cf_end_frame {
+	uint16			fc;		
+	uint16			durid;		
+	struct ether_addr	ra;		
+	struct ether_addr	bssid;		
+} BWL_POST_PACKED_STRUCT;
+#define	DOT11_CS_END_LEN	16		
+
+BWL_PRE_PACKED_STRUCT struct dot11_action_wifi_vendor_specific {
+	uint8	category;
+	uint8	OUI[3];
+	uint8	type;
+	uint8	subtype;
+	uint8	data[1040];
+	struct dot11_action_wifi_vendor_specific* next_node;
+} BWL_POST_PACKED_STRUCT;
+
+typedef struct dot11_action_wifi_vendor_specific dot11_action_wifi_vendor_specific_t;
+
+#define DOT11_BA_CTL_POLICY_NORMAL	0x0000	
+#define DOT11_BA_CTL_POLICY_NOACK	0x0001	
+#define DOT11_BA_CTL_POLICY_MASK	0x0001	
+
+#define DOT11_BA_CTL_MTID		0x0002	
+#define DOT11_BA_CTL_COMPRESSED		0x0004	
+
+#define DOT11_BA_CTL_NUMMSDU_MASK	0x0FC0	
+#define DOT11_BA_CTL_NUMMSDU_SHIFT	6	
+
+#define DOT11_BA_CTL_TID_MASK		0xF000	
+#define DOT11_BA_CTL_TID_SHIFT		12	
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_ctl_header {
+	uint16			fc;		
+	uint16			durid;		
+	struct ether_addr	ra;		
+	struct ether_addr	ta;		
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_CTL_HDR_LEN	16		
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_bar {
+	uint16			bar_control;	
+	uint16			seqnum;		
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_BAR_LEN		4		
+
+#define DOT11_BA_BITMAP_LEN	128		
+#define DOT11_BA_CMP_BITMAP_LEN	8		
+
+BWL_PRE_PACKED_STRUCT struct dot11_ba {
+	uint16			ba_control;	
+	uint16			seqnum;		
+	uint8			bitmap[DOT11_BA_BITMAP_LEN];	
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_BA_LEN		4		
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_management_header {
+	uint16			fc;		
+	uint16			durid;		
+	struct ether_addr	da;		
+	struct ether_addr	sa;		
+	struct ether_addr	bssid;		
+	uint16			seq;		
+} BWL_POST_PACKED_STRUCT;
+#define	DOT11_MGMT_HDR_LEN	24		
+
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_bcn_prb {
+	uint32			timestamp[2];
+	uint16			beacon_interval;
+	uint16			capability;
+} BWL_POST_PACKED_STRUCT;
+#define	DOT11_BCN_PRB_LEN	12		
+
+BWL_PRE_PACKED_STRUCT struct dot11_auth {
+	uint16			alg;		
+	uint16			seq;		
+	uint16			status;		
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_AUTH_FIXED_LEN	6		
+
+BWL_PRE_PACKED_STRUCT struct dot11_assoc_req {
+	uint16			capability;	
+	uint16			listen;		
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_ASSOC_REQ_FIXED_LEN	4	
+
+BWL_PRE_PACKED_STRUCT struct dot11_reassoc_req {
+	uint16			capability;	
+	uint16			listen;		
+	struct ether_addr	ap;		
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_REASSOC_REQ_FIXED_LEN	10	
+
+BWL_PRE_PACKED_STRUCT struct dot11_assoc_resp {
+	uint16			capability;	
+	uint16			status;		
+	uint16			aid;		
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_ASSOC_RESP_FIXED_LEN	6	
+
+BWL_PRE_PACKED_STRUCT struct dot11_action_measure {
+	uint8	category;
+	uint8	action;
+	uint8	token;
+	uint8	data[1];
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_ACTION_MEASURE_LEN	3	
+
+BWL_PRE_PACKED_STRUCT struct dot11_action_ht_ch_width {
+	uint8	category;
+	uint8	action;
+	uint8	ch_width;
+} BWL_POST_PACKED_STRUCT;
+
+BWL_PRE_PACKED_STRUCT struct dot11_action_ht_mimops {
+	uint8	category;
+	uint8	action;
+	uint8	control;
+} BWL_POST_PACKED_STRUCT;
+
+#define SM_PWRSAVE_ENABLE	1
+#define SM_PWRSAVE_MODE		2
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_power_cnst {
+	uint8 id;
+	uint8 len;
+	uint8 power;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_power_cnst dot11_power_cnst_t;
+
+BWL_PRE_PACKED_STRUCT struct dot11_power_cap {
+	uint8 min;
+	uint8 max;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_power_cap dot11_power_cap_t;
+
+BWL_PRE_PACKED_STRUCT struct dot11_tpc_rep {
+	uint8 id;
+	uint8 len;
+	uint8 tx_pwr;
+	uint8 margin;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_tpc_rep dot11_tpc_rep_t;
+#define DOT11_MNG_IE_TPC_REPORT_LEN	2 	
+
+BWL_PRE_PACKED_STRUCT struct dot11_supp_channels {
+	uint8 id;
+	uint8 len;
+	uint8 first_channel;
+	uint8 num_channels;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_supp_channels dot11_supp_channels_t;
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_extch {
+	uint8	id;		
+	uint8	len;		
+	uint8	extch;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_extch dot11_extch_ie_t;
+
+BWL_PRE_PACKED_STRUCT struct dot11_brcm_extch {
+	uint8	id;		
+	uint8	len;		
+	uint8	oui[3];		
+	uint8	type;           
+	uint8	extch;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_brcm_extch dot11_brcm_extch_ie_t;
+
+#define BRCM_EXTCH_IE_LEN	5
+#define BRCM_EXTCH_IE_TYPE	53	
+#define DOT11_EXTCH_IE_LEN	1
+#define DOT11_EXT_CH_MASK	0x03	
+#define DOT11_EXT_CH_UPPER	0x01	
+#define DOT11_EXT_CH_LOWER	0x03	
+#define DOT11_EXT_CH_NONE	0x00	
+
+BWL_PRE_PACKED_STRUCT struct dot11_action_frmhdr {
+	uint8	category;
+	uint8	action;
+	uint8	data[1];
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_ACTION_FRMHDR_LEN	2
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_channel_switch {
+	uint8 id;	
+	uint8 len;	
+	uint8 mode;	
+	uint8 channel;	
+	uint8 count;	
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_channel_switch dot11_chan_switch_ie_t;
+
+#define DOT11_SWITCH_IE_LEN	3	
+
+#define DOT11_CSA_MODE_ADVISORY		0	
+#define DOT11_CSA_MODE_NO_TX		1	
+
+BWL_PRE_PACKED_STRUCT struct dot11_action_switch_channel {
+	uint8	category;
+	uint8	action;
+	dot11_chan_switch_ie_t chan_switch_ie;	
+	dot11_brcm_extch_ie_t extch_ie;		
+} BWL_POST_PACKED_STRUCT;
+
+BWL_PRE_PACKED_STRUCT struct dot11_csa_body {
+	uint8 mode;	
+	uint8 reg;	
+	uint8 channel;	
+	uint8 count;	
+} BWL_POST_PACKED_STRUCT;
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_ext_csa {
+	uint8 id;	
+	uint8 len;	
+	struct dot11_csa_body b; 
+} BWL_POST_PACKED_STRUCT;
+
+BWL_PRE_PACKED_STRUCT struct dot11y_action_ext_csa {
+	uint8	category;
+	uint8	action;
+	struct dot11_csa_body b;	
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_ext_csa dot11_ext_csa_ie_t;
+#define DOT11_EXT_CSA_IE_LEN	4	
+
+BWL_PRE_PACKED_STRUCT struct dot11_action_ext_csa {
+	uint8	category;
+	uint8	action;
+	dot11_ext_csa_ie_t chan_switch_ie;	
+} BWL_POST_PACKED_STRUCT;
+
+BWL_PRE_PACKED_STRUCT struct dot11_obss_coex {
+	uint8	id;
+	uint8	len;
+	uint8	info;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_obss_coex dot11_obss_coex_t;
+#define DOT11_OBSS_COEXINFO_LEN	1	
+
+#define	DOT11_OBSS_COEX_INFO_REQ		0x01
+#define	DOT11_OBSS_COEX_40MHZ_INTOLERANT	0x02
+#define	DOT11_OBSS_COEX_20MHZ_WIDTH_REQ	0x04
+
+BWL_PRE_PACKED_STRUCT struct dot11_obss_chanlist {
+	uint8	id;
+	uint8	len;
+	uint8	regclass;
+	uint8	chanlist[1];
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_obss_chanlist dot11_obss_chanlist_t;
+#define DOT11_OBSS_CHANLIST_FIXED_LEN	1	
+
+BWL_PRE_PACKED_STRUCT struct dot11_extcap_ie {
+	uint8 id;
+	uint8 len;
+	uint8 cap;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_extcap_ie dot11_extcap_ie_t;
+#define DOT11_EXTCAP_LEN	1
+
+
+
+#define DOT11_MEASURE_TYPE_BASIC 	0	
+#define DOT11_MEASURE_TYPE_CCA 		1	
+#define DOT11_MEASURE_TYPE_RPI		2	
+
+
+#define DOT11_MEASURE_MODE_ENABLE 	(1<<1)	
+#define DOT11_MEASURE_MODE_REQUEST	(1<<2)	
+#define DOT11_MEASURE_MODE_REPORT 	(1<<3)	
+
+#define DOT11_MEASURE_MODE_LATE 	(1<<0)	
+#define DOT11_MEASURE_MODE_INCAPABLE	(1<<1)	
+#define DOT11_MEASURE_MODE_REFUSED	(1<<2)	
+
+#define DOT11_MEASURE_BASIC_MAP_BSS	((uint8)(1<<0))	
+#define DOT11_MEASURE_BASIC_MAP_OFDM	((uint8)(1<<1))	
+#define DOT11_MEASURE_BASIC_MAP_UKNOWN	((uint8)(1<<2))	
+#define DOT11_MEASURE_BASIC_MAP_RADAR	((uint8)(1<<3))	
+#define DOT11_MEASURE_BASIC_MAP_UNMEAS	((uint8)(1<<4))	
+
+BWL_PRE_PACKED_STRUCT struct dot11_meas_req {
+	uint8 id;
+	uint8 len;
+	uint8 token;
+	uint8 mode;
+	uint8 type;
+	uint8 channel;
+	uint8 start_time[8];
+	uint16 duration;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_meas_req dot11_meas_req_t;
+#define DOT11_MNG_IE_MREQ_LEN 14	
+
+#define DOT11_MNG_IE_MREQ_FIXED_LEN 3	
+
+BWL_PRE_PACKED_STRUCT struct dot11_meas_rep {
+	uint8 id;
+	uint8 len;
+	uint8 token;
+	uint8 mode;
+	uint8 type;
+	BWL_PRE_PACKED_STRUCT union
+	{
+		BWL_PRE_PACKED_STRUCT struct {
+			uint8 channel;
+			uint8 start_time[8];
+			uint16 duration;
+			uint8 map;
+		} BWL_POST_PACKED_STRUCT basic;
+		uint8 data[1];
+	} BWL_POST_PACKED_STRUCT rep;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_meas_rep dot11_meas_rep_t;
+
+
+#define DOT11_MNG_IE_MREP_FIXED_LEN	3	
+
+BWL_PRE_PACKED_STRUCT struct dot11_meas_rep_basic {
+	uint8 channel;
+	uint8 start_time[8];
+	uint16 duration;
+	uint8 map;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_meas_rep_basic dot11_meas_rep_basic_t;
+#define DOT11_MEASURE_BASIC_REP_LEN	12	
+
+BWL_PRE_PACKED_STRUCT struct dot11_quiet {
+	uint8 id;
+	uint8 len;
+	uint8 count;	
+	uint8 period;	
+	uint16 duration;	
+	uint16 offset;	
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_quiet dot11_quiet_t;
+
+BWL_PRE_PACKED_STRUCT struct chan_map_tuple {
+	uint8 channel;
+	uint8 map;
+} BWL_POST_PACKED_STRUCT;
+typedef struct chan_map_tuple chan_map_tuple_t;
+
+BWL_PRE_PACKED_STRUCT struct dot11_ibss_dfs {
+	uint8 id;
+	uint8 len;
+	uint8 eaddr[ETHER_ADDR_LEN];
+	uint8 interval;
+	chan_map_tuple_t map[1];
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_ibss_dfs dot11_ibss_dfs_t;
+
+
+#define WME_OUI			"\x00\x50\xf2"	
+#define WME_VER			1	
+#define WME_TYPE		2	
+#define WME_SUBTYPE_IE		0	
+#define WME_SUBTYPE_PARAM_IE	1	
+#define WME_SUBTYPE_TSPEC	2	
+
+
+#define AC_BE			0	
+#define AC_BK			1	
+#define AC_VI			2	
+#define AC_VO			3	
+#define AC_COUNT		4	
+
+typedef uint8 ac_bitmap_t;	
+
+#define AC_BITMAP_NONE		0x0	
+#define AC_BITMAP_ALL		0xf	
+#define AC_BITMAP_TST(ab, ac)	(((ab) & (1 << (ac))) != 0)
+#define AC_BITMAP_SET(ab, ac)	(((ab) |= (1 << (ac))))
+#define AC_BITMAP_RESET(ab, ac) (((ab) &= ~(1 << (ac))))
+
+
+BWL_PRE_PACKED_STRUCT struct wme_ie {
+	uint8 oui[3];
+	uint8 type;
+	uint8 subtype;
+	uint8 version;
+	uint8 qosinfo;
+} BWL_POST_PACKED_STRUCT;
+typedef struct wme_ie wme_ie_t;
+#define WME_IE_LEN 7	
+
+BWL_PRE_PACKED_STRUCT struct edcf_acparam {
+	uint8	ACI;
+	uint8	ECW;
+	uint16  TXOP;		
+} BWL_POST_PACKED_STRUCT;
+typedef struct edcf_acparam edcf_acparam_t;
+
+
+BWL_PRE_PACKED_STRUCT struct wme_param_ie {
+	uint8 oui[3];
+	uint8 type;
+	uint8 subtype;
+	uint8 version;
+	uint8 qosinfo;
+	uint8 rsvd;
+	edcf_acparam_t acparam[AC_COUNT];
+} BWL_POST_PACKED_STRUCT;
+typedef struct wme_param_ie wme_param_ie_t;
+#define WME_PARAM_IE_LEN            24          
+
+
+#define WME_QI_AP_APSD_MASK         0x80        
+#define WME_QI_AP_APSD_SHIFT        7           
+#define WME_QI_AP_COUNT_MASK        0x0f        
+#define WME_QI_AP_COUNT_SHIFT       0           
+
+
+#define WME_QI_STA_MAXSPLEN_MASK    0x60        
+#define WME_QI_STA_MAXSPLEN_SHIFT   5           
+#define WME_QI_STA_APSD_ALL_MASK    0xf         
+#define WME_QI_STA_APSD_ALL_SHIFT   0           
+#define WME_QI_STA_APSD_BE_MASK     0x8         
+#define WME_QI_STA_APSD_BE_SHIFT    3           
+#define WME_QI_STA_APSD_BK_MASK     0x4         
+#define WME_QI_STA_APSD_BK_SHIFT    2           
+#define WME_QI_STA_APSD_VI_MASK     0x2         
+#define WME_QI_STA_APSD_VI_SHIFT    1           
+#define WME_QI_STA_APSD_VO_MASK     0x1         
+#define WME_QI_STA_APSD_VO_SHIFT    0           
+
+
+#define EDCF_AIFSN_MIN               1           
+#define EDCF_AIFSN_MAX               15          
+#define EDCF_AIFSN_MASK              0x0f        
+#define EDCF_ACM_MASK                0x10        
+#define EDCF_ACI_MASK                0x60        
+#define EDCF_ACI_SHIFT               5           
+#define EDCF_AIFSN_SHIFT             12          
+
+
+#define EDCF_ECW_MIN                 0           
+#define EDCF_ECW_MAX                 15          
+#define EDCF_ECW2CW(exp)             ((1 << (exp)) - 1)
+#define EDCF_ECWMIN_MASK             0x0f        
+#define EDCF_ECWMAX_MASK             0xf0        
+#define EDCF_ECWMAX_SHIFT            4           
+
+
+#define EDCF_TXOP_MIN                0           
+#define EDCF_TXOP_MAX                65535       
+#define EDCF_TXOP2USEC(txop)         ((txop) << 5)
+
+
+#define NON_EDCF_AC_BE_ACI_STA          0x02
+
+
+#define EDCF_AC_BE_ACI_STA           0x03	
+#define EDCF_AC_BE_ECW_STA           0xA4	
+#define EDCF_AC_BE_TXOP_STA          0x0000	
+#define EDCF_AC_BK_ACI_STA           0x27	
+#define EDCF_AC_BK_ECW_STA           0xA4	
+#define EDCF_AC_BK_TXOP_STA          0x0000	
+#define EDCF_AC_VI_ACI_STA           0x42	
+#define EDCF_AC_VI_ECW_STA           0x43	
+#define EDCF_AC_VI_TXOP_STA          0x005e	
+#define EDCF_AC_VO_ACI_STA           0x62	
+#define EDCF_AC_VO_ECW_STA           0x32	
+#define EDCF_AC_VO_TXOP_STA          0x002f	
+
+
+#define EDCF_AC_BE_ACI_AP            0x03	
+#define EDCF_AC_BE_ECW_AP            0x64	
+#define EDCF_AC_BE_TXOP_AP           0x0000	
+#define EDCF_AC_BK_ACI_AP            0x27	
+#define EDCF_AC_BK_ECW_AP            0xA4	
+#define EDCF_AC_BK_TXOP_AP           0x0000	
+#define EDCF_AC_VI_ACI_AP            0x41	
+#define EDCF_AC_VI_ECW_AP            0x43	
+#define EDCF_AC_VI_TXOP_AP           0x005e	
+#define EDCF_AC_VO_ACI_AP            0x61	
+#define EDCF_AC_VO_ECW_AP            0x32	
+#define EDCF_AC_VO_TXOP_AP           0x002f	
+
+
+BWL_PRE_PACKED_STRUCT struct edca_param_ie {
+	uint8 qosinfo;
+	uint8 rsvd;
+	edcf_acparam_t acparam[AC_COUNT];
+} BWL_POST_PACKED_STRUCT;
+typedef struct edca_param_ie edca_param_ie_t;
+#define EDCA_PARAM_IE_LEN            18          
+
+
+BWL_PRE_PACKED_STRUCT struct qos_cap_ie {
+	uint8 qosinfo;
+} BWL_POST_PACKED_STRUCT;
+typedef struct qos_cap_ie qos_cap_ie_t;
+
+BWL_PRE_PACKED_STRUCT struct dot11_qbss_load_ie {
+	uint8 id; 			
+	uint8 length;
+	uint16 station_count; 		
+	uint8 channel_utilization;	
+	uint16 aac; 			
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_qbss_load_ie dot11_qbss_load_ie_t;
+
+
+#define FIXED_MSDU_SIZE 0x8000		
+#define MSDU_SIZE_MASK	0x7fff		
+
+
+
+#define	INTEGER_SHIFT	13	
+#define FRACTION_MASK	0x1FFF	
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_management_notification {
+	uint8 category;			
+	uint8 action;
+	uint8 token;
+	uint8 status;
+	uint8 data[1];			
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_MGMT_NOTIFICATION_LEN 4	
+
+
+#define WME_ADDTS_REQUEST	0	
+#define WME_ADDTS_RESPONSE	1	
+#define WME_DELTS_REQUEST	2	
+
+
+#define WME_ADMISSION_ACCEPTED		0	
+#define WME_INVALID_PARAMETERS		1	
+#define WME_ADMISSION_REFUSED		3	
+
+
+#define BCN_PRB_SSID(body) ((char*)(body) + DOT11_BCN_PRB_LEN)
+
+
+#define DOT11_OPEN_SYSTEM	0	
+#define DOT11_SHARED_KEY	1	
+
+#define DOT11_OPEN_SHARED	2	
+#define DOT11_CHALLENGE_LEN	128	
+
+
+#define FC_PVER_MASK		0x3	
+#define FC_PVER_SHIFT		0	
+#define FC_TYPE_MASK		0xC	
+#define FC_TYPE_SHIFT		2	
+#define FC_SUBTYPE_MASK		0xF0	
+#define FC_SUBTYPE_SHIFT	4	
+#define FC_TODS			0x100	
+#define FC_TODS_SHIFT		8	
+#define FC_FROMDS		0x200	
+#define FC_FROMDS_SHIFT		9	
+#define FC_MOREFRAG		0x400	
+#define FC_MOREFRAG_SHIFT	10	
+#define FC_RETRY		0x800	
+#define FC_RETRY_SHIFT		11	
+#define FC_PM			0x1000	
+#define FC_PM_SHIFT		12	
+#define FC_MOREDATA		0x2000	
+#define FC_MOREDATA_SHIFT	13	
+#define FC_WEP			0x4000	
+#define FC_WEP_SHIFT		14	
+#define FC_ORDER		0x8000	
+#define FC_ORDER_SHIFT		15	
+
+
+#define SEQNUM_SHIFT		4	
+#define SEQNUM_MAX		0x1000	
+#define FRAGNUM_MASK		0xF	
+
+
+
+
+#define FC_TYPE_MNG		0	
+#define FC_TYPE_CTL		1	
+#define FC_TYPE_DATA		2	
+
+
+#define FC_SUBTYPE_ASSOC_REQ		0	
+#define FC_SUBTYPE_ASSOC_RESP		1	
+#define FC_SUBTYPE_REASSOC_REQ		2	
+#define FC_SUBTYPE_REASSOC_RESP		3	
+#define FC_SUBTYPE_PROBE_REQ		4	
+#define FC_SUBTYPE_PROBE_RESP		5	
+#define FC_SUBTYPE_BEACON		8	
+#define FC_SUBTYPE_ATIM			9	
+#define FC_SUBTYPE_DISASSOC		10	
+#define FC_SUBTYPE_AUTH			11	
+#define FC_SUBTYPE_DEAUTH		12	
+#define FC_SUBTYPE_ACTION		13	
+#define FC_SUBTYPE_ACTION_NOACK		14	
+
+
+#define FC_SUBTYPE_CTL_WRAPPER		7	
+#define FC_SUBTYPE_BLOCKACK_REQ		8	
+#define FC_SUBTYPE_BLOCKACK		9	
+#define FC_SUBTYPE_PS_POLL		10	
+#define FC_SUBTYPE_RTS			11	
+#define FC_SUBTYPE_CTS			12	
+#define FC_SUBTYPE_ACK			13	
+#define FC_SUBTYPE_CF_END		14	
+#define FC_SUBTYPE_CF_END_ACK		15	
+
+
+#define FC_SUBTYPE_DATA			0	
+#define FC_SUBTYPE_DATA_CF_ACK		1	
+#define FC_SUBTYPE_DATA_CF_POLL		2	
+#define FC_SUBTYPE_DATA_CF_ACK_POLL	3	
+#define FC_SUBTYPE_NULL			4	
+#define FC_SUBTYPE_CF_ACK		5	
+#define FC_SUBTYPE_CF_POLL		6	
+#define FC_SUBTYPE_CF_ACK_POLL		7	
+#define FC_SUBTYPE_QOS_DATA		8	
+#define FC_SUBTYPE_QOS_DATA_CF_ACK	9	
+#define FC_SUBTYPE_QOS_DATA_CF_POLL	10	
+#define FC_SUBTYPE_QOS_DATA_CF_ACK_POLL	11	
+#define FC_SUBTYPE_QOS_NULL		12	
+#define FC_SUBTYPE_QOS_CF_POLL		14	
+#define FC_SUBTYPE_QOS_CF_ACK_POLL	15	
+
+
+#define FC_SUBTYPE_ANY_QOS(s)		(((s) & 8) != 0)
+#define FC_SUBTYPE_ANY_NULL(s)		(((s) & 4) != 0)
+#define FC_SUBTYPE_ANY_CF_POLL(s)	(((s) & 2) != 0)
+#define FC_SUBTYPE_ANY_CF_ACK(s)	(((s) & 1) != 0)
+
+
+#define FC_KIND_MASK		(FC_TYPE_MASK | FC_SUBTYPE_MASK)	
+
+#define FC_KIND(t, s)	(((t) << FC_TYPE_SHIFT) | ((s) << FC_SUBTYPE_SHIFT))	
+
+#define FC_SUBTYPE(fc)	(((fc) & FC_SUBTYPE_MASK) >> FC_SUBTYPE_SHIFT)	
+#define FC_TYPE(fc)	(((fc) & FC_TYPE_MASK) >> FC_TYPE_SHIFT)	
+
+#define FC_ASSOC_REQ	FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_ASSOC_REQ)	
+#define FC_ASSOC_RESP	FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_ASSOC_RESP)	
+#define FC_REASSOC_REQ	FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_REASSOC_REQ)	
+#define FC_REASSOC_RESP	FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_REASSOC_RESP)	
+#define FC_PROBE_REQ	FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_PROBE_REQ)	
+#define FC_PROBE_RESP	FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_PROBE_RESP)	
+#define FC_BEACON	FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_BEACON)		
+#define FC_DISASSOC	FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_DISASSOC)	
+#define FC_AUTH		FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_AUTH)		
+#define FC_DEAUTH	FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_DEAUTH)		
+#define FC_ACTION	FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_ACTION)		
+#define FC_ACTION_NOACK	FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_ACTION_NOACK)	
+
+#define FC_CTL_WRAPPER	FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_CTL_WRAPPER)	
+#define FC_BLOCKACK_REQ	FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_BLOCKACK_REQ)	
+#define FC_BLOCKACK	FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_BLOCKACK)	
+#define FC_PS_POLL	FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_PS_POLL)	
+#define FC_RTS		FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_RTS)		
+#define FC_CTS		FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_CTS)		
+#define FC_ACK		FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_ACK)		
+#define FC_CF_END	FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_CF_END)		
+#define FC_CF_END_ACK	FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_CF_END_ACK)	
+
+#define FC_DATA		FC_KIND(FC_TYPE_DATA, FC_SUBTYPE_DATA)		
+#define FC_NULL_DATA	FC_KIND(FC_TYPE_DATA, FC_SUBTYPE_NULL)		
+#define FC_DATA_CF_ACK	FC_KIND(FC_TYPE_DATA, FC_SUBTYPE_DATA_CF_ACK)	
+#define FC_QOS_DATA	FC_KIND(FC_TYPE_DATA, FC_SUBTYPE_QOS_DATA)	
+#define FC_QOS_NULL	FC_KIND(FC_TYPE_DATA, FC_SUBTYPE_QOS_NULL)	
+
+
+
+
+#define QOS_PRIO_SHIFT		0	
+#define QOS_PRIO_MASK		0x0007	
+#define QOS_PRIO(qos)		(((qos) & QOS_PRIO_MASK) >> QOS_PRIO_SHIFT)	
+
+
+#define QOS_TID_SHIFT		0	
+#define QOS_TID_MASK		0x000f	
+#define QOS_TID(qos)		(((qos) & QOS_TID_MASK) >> QOS_TID_SHIFT)	
+
+
+#define QOS_EOSP_SHIFT		4	
+#define QOS_EOSP_MASK		0x0010	
+#define QOS_EOSP(qos)		(((qos) & QOS_EOSP_MASK) >> QOS_EOSP_SHIFT)	
+
+
+#define QOS_ACK_NORMAL_ACK	0	
+#define QOS_ACK_NO_ACK		1	
+#define QOS_ACK_NO_EXP_ACK	2	
+#define QOS_ACK_BLOCK_ACK	3	
+#define QOS_ACK_SHIFT		5	
+#define QOS_ACK_MASK		0x0060	
+#define QOS_ACK(qos)		(((qos) & QOS_ACK_MASK) >> QOS_ACK_SHIFT)	
+
+
+#define QOS_AMSDU_SHIFT		7	
+#define QOS_AMSDU_MASK		0x0080	
+
+
+
+
+
+
+#define DOT11_MNG_AUTH_ALGO_LEN		2	
+#define DOT11_MNG_AUTH_SEQ_LEN		2	
+#define DOT11_MNG_BEACON_INT_LEN	2	
+#define DOT11_MNG_CAP_LEN		2	
+#define DOT11_MNG_AP_ADDR_LEN		6	
+#define DOT11_MNG_LISTEN_INT_LEN	2	
+#define DOT11_MNG_REASON_LEN		2	
+#define DOT11_MNG_AID_LEN		2	
+#define DOT11_MNG_STATUS_LEN		2	
+#define DOT11_MNG_TIMESTAMP_LEN		8	
+
+
+#define DOT11_AID_MASK			0x3fff	
+
+
+#define DOT11_RC_RESERVED		0	
+#define DOT11_RC_UNSPECIFIED		1	
+#define DOT11_RC_AUTH_INVAL		2	
+#define DOT11_RC_DEAUTH_LEAVING		3	
+#define DOT11_RC_INACTIVITY		4	
+#define DOT11_RC_BUSY			5	
+#define DOT11_RC_INVAL_CLASS_2		6	
+#define DOT11_RC_INVAL_CLASS_3		7	
+#define DOT11_RC_DISASSOC_LEAVING	8	
+#define DOT11_RC_NOT_AUTH		9	
+#define DOT11_RC_BAD_PC			10	
+#define DOT11_RC_BAD_CHANNELS		11	
+
+
+
+#define DOT11_RC_UNSPECIFIED_QOS	32	
+#define DOT11_RC_INSUFFCIENT_BW		33	
+#define DOT11_RC_EXCESSIVE_FRAMES	34	
+#define DOT11_RC_TX_OUTSIDE_TXOP	35	
+#define DOT11_RC_LEAVING_QBSS		36	
+#define DOT11_RC_BAD_MECHANISM		37	
+#define DOT11_RC_SETUP_NEEDED		38	
+#define DOT11_RC_TIMEOUT		39	
+
+#define DOT11_RC_MAX			23	
+
+
+#define DOT11_SC_SUCCESS		0	
+#define DOT11_SC_FAILURE		1	
+#define DOT11_SC_CAP_MISMATCH		10	
+#define DOT11_SC_REASSOC_FAIL		11	
+#define DOT11_SC_ASSOC_FAIL		12	
+#define DOT11_SC_AUTH_MISMATCH		13	
+#define DOT11_SC_AUTH_SEQ		14	
+#define DOT11_SC_AUTH_CHALLENGE_FAIL	15	
+#define DOT11_SC_AUTH_TIMEOUT		16	
+#define DOT11_SC_ASSOC_BUSY_FAIL	17	
+#define DOT11_SC_ASSOC_RATE_MISMATCH	18	
+#define DOT11_SC_ASSOC_SHORT_REQUIRED	19	
+#define DOT11_SC_ASSOC_PBCC_REQUIRED	20	
+#define DOT11_SC_ASSOC_AGILITY_REQUIRED	21	
+#define DOT11_SC_ASSOC_SPECTRUM_REQUIRED	22	
+#define DOT11_SC_ASSOC_BAD_POWER_CAP	23	
+#define DOT11_SC_ASSOC_BAD_SUP_CHANNELS	24	
+#define DOT11_SC_ASSOC_SHORTSLOT_REQUIRED	25	
+#define DOT11_SC_ASSOC_ERPBCC_REQUIRED	26	
+#define DOT11_SC_ASSOC_DSSOFDM_REQUIRED	27	
+
+#define	DOT11_SC_DECLINED		37	
+#define	DOT11_SC_INVALID_PARAMS		38	
+
+
+#define DOT11_MNG_DS_PARAM_LEN			1	
+#define DOT11_MNG_IBSS_PARAM_LEN		2	
+
+
+#define DOT11_MNG_TIM_FIXED_LEN			3	
+#define DOT11_MNG_TIM_DTIM_COUNT		0	
+#define DOT11_MNG_TIM_DTIM_PERIOD		1	
+#define DOT11_MNG_TIM_BITMAP_CTL		2	
+#define DOT11_MNG_TIM_PVB			3	
+
+
+#define TLV_TAG_OFF		0	
+#define TLV_LEN_OFF		1	
+#define TLV_HDR_LEN		2	
+#define TLV_BODY_OFF		2	
+
+
+#define DOT11_MNG_SSID_ID			0	
+#define DOT11_MNG_RATES_ID			1	
+#define DOT11_MNG_FH_PARMS_ID			2	
+#define DOT11_MNG_DS_PARMS_ID			3	
+#define DOT11_MNG_CF_PARMS_ID			4	
+#define DOT11_MNG_TIM_ID			5	
+#define DOT11_MNG_IBSS_PARMS_ID			6	
+#define DOT11_MNG_COUNTRY_ID			7	
+#define DOT11_MNG_HOPPING_PARMS_ID		8	
+#define DOT11_MNG_HOPPING_TABLE_ID		9	
+#define DOT11_MNG_REQUEST_ID			10	
+#define DOT11_MNG_QBSS_LOAD_ID 			11	
+#define DOT11_MNG_EDCA_PARAM_ID			12	
+#define DOT11_MNG_CHALLENGE_ID			16	
+#define DOT11_MNG_PWR_CONSTRAINT_ID		32	
+#define DOT11_MNG_PWR_CAP_ID			33	
+#define DOT11_MNG_TPC_REQUEST_ID 		34	
+#define DOT11_MNG_TPC_REPORT_ID			35	
+#define DOT11_MNG_SUPP_CHANNELS_ID		36	
+#define DOT11_MNG_CHANNEL_SWITCH_ID		37	
+#define DOT11_MNG_MEASURE_REQUEST_ID		38	
+#define DOT11_MNG_MEASURE_REPORT_ID		39	
+#define DOT11_MNG_QUIET_ID			40	
+#define DOT11_MNG_IBSS_DFS_ID			41	
+#define DOT11_MNG_ERP_ID			42	
+#define DOT11_MNG_TS_DELAY_ID			43	
+#define	DOT11_MNG_HT_CAP			45	
+#define DOT11_MNG_QOS_CAP_ID			46	
+#define DOT11_MNG_NONERP_ID			47	
+#define DOT11_MNG_RSN_ID			48	
+#define DOT11_MNG_EXT_RATES_ID			50	
+#define	DOT11_MNG_REGCLASS_ID			59	
+#define DOT11_MNG_EXT_CSA_ID			60	
+#define	DOT11_MNG_HT_ADD			61	
+#define	DOT11_MNG_EXT_CHANNEL_OFFSET		62	
+#define DOT11_MNG_WAPI_ID				68	
+#define	DOT11_MNG_HT_BSS_COEXINFO_ID		72	
+#define	DOT11_MNG_HT_BSS_CHANNEL_REPORT_ID	73	
+#define	DOT11_MNG_HT_OBSS_ID			74	
+#define	DOT11_MNG_EXT_CAP			127	
+#define DOT11_MNG_WPA_ID			221	
+#define DOT11_MNG_PROPR_ID			221	
+
+
+#define DOT11_RATE_BASIC			0x80	
+#define DOT11_RATE_MASK				0x7F	
+
+
+#define DOT11_MNG_ERP_LEN			1	
+#define DOT11_MNG_NONERP_PRESENT		0x01	
+#define DOT11_MNG_USE_PROTECTION		0x02	
+#define DOT11_MNG_BARKER_PREAMBLE		0x04	
+
+#define DOT11_MGN_TS_DELAY_LEN		4	
+#define TS_DELAY_FIELD_SIZE			4	
+
+
+#define DOT11_CAP_ESS				0x0001	
+#define DOT11_CAP_IBSS				0x0002	
+#define DOT11_CAP_POLLABLE			0x0004	
+#define DOT11_CAP_POLL_RQ			0x0008	
+#define DOT11_CAP_PRIVACY			0x0010	
+#define DOT11_CAP_SHORT				0x0020	
+#define DOT11_CAP_PBCC				0x0040	
+#define DOT11_CAP_AGILITY			0x0080	
+#define DOT11_CAP_SPECTRUM			0x0100	
+#define DOT11_CAP_SHORTSLOT			0x0400	
+#define DOT11_CAP_CCK_OFDM			0x2000	
+
+
+#define DOT11_OBSS_COEX_MNG_SUPPORT	0x01	
+
+
+#define DOT11_ACTION_HDR_LEN		2	
+#define DOT11_ACTION_CAT_ERR_MASK	0x80	
+#define DOT11_ACTION_CAT_MASK		0x7F	
+#define DOT11_ACTION_CAT_SPECT_MNG	0	
+#define DOT11_ACTION_CAT_BLOCKACK	3	
+#define DOT11_ACTION_CAT_PUBLIC		4	
+#define DOT11_ACTION_CAT_HT		7	
+#define DOT11_ACTION_CAT_VS			127	
+#define DOT11_ACTION_NOTIFICATION	0x11	
+
+#define DOT11_ACTION_ID_M_REQ		0	
+#define DOT11_ACTION_ID_M_REP		1	
+#define DOT11_ACTION_ID_TPC_REQ		2	
+#define DOT11_ACTION_ID_TPC_REP		3	
+#define DOT11_ACTION_ID_CHANNEL_SWITCH	4	
+#define DOT11_ACTION_ID_EXT_CSA		5	
+
+
+#define DOT11_ACTION_ID_HT_CH_WIDTH	0	
+#define DOT11_ACTION_ID_HT_MIMO_PS	1	
+
+
+#define DOT11_PUB_ACTION_BSS_COEX_MNG	0	
+#define DOT11_PUB_ACTION_CHANNEL_SWITCH	4	
+
+
+#define DOT11_BA_ACTION_ADDBA_REQ	0	
+#define DOT11_BA_ACTION_ADDBA_RESP	1	
+#define DOT11_BA_ACTION_DELBA		2	
+
+
+#define DOT11_ADDBA_PARAM_AMSDU_SUP	0x0001	
+#define DOT11_ADDBA_PARAM_POLICY_MASK	0x0002	
+#define DOT11_ADDBA_PARAM_POLICY_SHIFT	1	
+#define DOT11_ADDBA_PARAM_TID_MASK	0x003c	
+#define DOT11_ADDBA_PARAM_TID_SHIFT	2	
+#define DOT11_ADDBA_PARAM_BSIZE_MASK	0xffc0	
+#define DOT11_ADDBA_PARAM_BSIZE_SHIFT	6	
+
+#define DOT11_ADDBA_POLICY_DELAYED	0	
+#define DOT11_ADDBA_POLICY_IMMEDIATE	1	
+
+BWL_PRE_PACKED_STRUCT struct dot11_addba_req {
+	uint8 category;				
+	uint8 action;				
+	uint8 token;				
+	uint16 addba_param_set;			
+	uint16 timeout;				
+	uint16 start_seqnum;			
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_addba_req dot11_addba_req_t;
+#define DOT11_ADDBA_REQ_LEN		9	
+
+BWL_PRE_PACKED_STRUCT struct dot11_addba_resp {
+	uint8 category;				
+	uint8 action;				
+	uint8 token;				
+	uint16 status;				
+	uint16 addba_param_set;			
+	uint16 timeout;				
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_addba_resp dot11_addba_resp_t;
+#define DOT11_ADDBA_RESP_LEN		9	
+
+
+#define DOT11_DELBA_PARAM_INIT_MASK	0x0800	
+#define DOT11_DELBA_PARAM_INIT_SHIFT	11	
+#define DOT11_DELBA_PARAM_TID_MASK	0xf000	
+#define DOT11_DELBA_PARAM_TID_SHIFT	12	
+
+BWL_PRE_PACKED_STRUCT struct dot11_delba {
+	uint8 category;				
+	uint8 action;				
+	uint16 delba_param_set;			
+	uint16 reason;				
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_delba dot11_delba_t;
+#define DOT11_DELBA_LEN			6	
+
+
+#define DOT11_BSSTYPE_INFRASTRUCTURE		0	
+#define DOT11_BSSTYPE_INDEPENDENT		1	
+#define DOT11_BSSTYPE_ANY			2	
+#define DOT11_SCANTYPE_ACTIVE			0	
+#define DOT11_SCANTYPE_PASSIVE			1	
+
+
+#define PREN_PREAMBLE		24	
+#define PREN_MM_EXT		8	
+#define PREN_PREAMBLE_EXT	4	
+
+
+#define NPHY_RIFS_TIME		2	
+
+
+#define APHY_SLOT_TIME		9	
+#define APHY_SIFS_TIME		16	
+#define APHY_DIFS_TIME		(APHY_SIFS_TIME + (2 * APHY_SLOT_TIME))	
+#define APHY_PREAMBLE_TIME	16	
+#define APHY_SIGNAL_TIME	4	
+#define APHY_SYMBOL_TIME	4	
+#define APHY_SERVICE_NBITS	16	
+#define APHY_TAIL_NBITS		6	
+#define	APHY_CWMIN		15	
+
+
+#define BPHY_SLOT_TIME		20	
+#define BPHY_SIFS_TIME		10	
+#define BPHY_DIFS_TIME		50	
+#define BPHY_PLCP_TIME		192	
+#define BPHY_PLCP_SHORT_TIME	96	
+#define	BPHY_CWMIN		31	
+
+
+#define DOT11_OFDM_SIGNAL_EXTENSION	6	
+
+#define PHY_CWMAX		1023	
+
+#define	DOT11_MAXNUMFRAGS	16	
+
+
+typedef struct d11cnt {
+	uint32		txfrag;		
+	uint32		txmulti;	
+	uint32		txfail;		
+	uint32		txretry;	
+	uint32		txretrie;	
+	uint32		rxdup;		
+	uint32		txrts;		
+	uint32		txnocts;	
+	uint32		txnoack;	
+	uint32		rxfrag;		
+	uint32		rxmulti;	
+	uint32		rxcrc;		
+	uint32		txfrmsnt;	
+	uint32		rxundec;	
+} d11cnt_t;
+
+
+#define BRCM_PROP_OUI		"\x00\x90\x4C"	
+
+
+
+
+BWL_PRE_PACKED_STRUCT struct brcm_prop_ie_s {
+	uint8 id;		
+	uint8 len;		
+	uint8 oui[3];		
+	uint8 type;		
+	uint16 cap;		
+} BWL_POST_PACKED_STRUCT;
+typedef struct brcm_prop_ie_s brcm_prop_ie_t;
+
+#define BRCM_PROP_IE_LEN	6	
+
+#define DPT_IE_TYPE		2
+
+
+#define BRCM_OUI		"\x00\x10\x18"	
+
+
+BWL_PRE_PACKED_STRUCT struct brcm_ie {
+	uint8	id;		
+	uint8	len;		
+	uint8	oui[3];		
+	uint8	ver;		
+	uint8	assoc;		
+	uint8	flags;		
+	uint8	flags1;		
+	uint16	amsdu_mtu_pref;	
+} BWL_POST_PACKED_STRUCT;
+typedef	struct brcm_ie brcm_ie_t;
+#define BRCM_IE_LEN		11	
+#define BRCM_IE_VER		2	
+#define BRCM_IE_LEGACY_AES_VER	1	
+
+
+#ifdef WLAFTERBURNER
+#define	BRF_ABCAP		0x1	
+#define	BRF_ABRQRD		0x2	
+#define BRF_ABCOUNTER_MASK	0xf0	
+#define BRF_ABCOUNTER_SHIFT	4	
+#endif 
+#define	BRF_LZWDS		0x4	
+#define	BRF_BLOCKACK		0x8	
+
+
+#define	BRF1_AMSDU		0x1	
+#define BRF1_WMEPS		0x4	
+#define BRF1_PSOFIX		0x8	
+
+#ifdef WLAFTERBURNER
+#define AB_WDS_TIMEOUT_MAX	15	
+#define AB_WDS_TIMEOUT_MIN	1	
+#endif 
+
+#define AB_GUARDCOUNT	10		
+
+#define MCSSET_LEN	16	
+#define MAX_MCS_NUM	(128)	
+
+BWL_PRE_PACKED_STRUCT struct ht_cap_ie {
+	uint16	cap;
+	uint8	params;
+	uint8	supp_mcs[MCSSET_LEN];
+	uint16	ext_htcap;
+	uint32	txbf_cap;
+	uint8	as_cap;
+} BWL_POST_PACKED_STRUCT;
+typedef struct ht_cap_ie ht_cap_ie_t;
+
+
+
+BWL_PRE_PACKED_STRUCT struct ht_prop_cap_ie {
+	uint8	id;		
+	uint8	len;		
+	uint8	oui[3];		
+	uint8	type;           
+	ht_cap_ie_t cap_ie;
+} BWL_POST_PACKED_STRUCT;
+typedef struct ht_prop_cap_ie ht_prop_cap_ie_t;
+#define HT_PROP_IE_OVERHEAD	4	
+#define HT_CAP_IE_LEN	26
+#define HT_CAP_IE_TYPE	51
+
+#define HT_CAP_LDPC_CODING	0x0001	
+#define HT_CAP_40MHZ		0x0002  
+#define HT_CAP_MIMO_PS_MASK	0x000C  
+#define HT_CAP_MIMO_PS_SHIFT	0x0002	
+#define HT_CAP_MIMO_PS_OFF	0x0003	
+#define HT_CAP_MIMO_PS_RTS	0x0001	
+#define HT_CAP_MIMO_PS_ON	0x0000	
+#define HT_CAP_GF		0x0010	
+#define HT_CAP_SHORT_GI_20	0x0020	
+#define HT_CAP_SHORT_GI_40	0x0040	
+#define HT_CAP_TX_STBC		0x0080	
+#define HT_CAP_RX_STBC_MASK	0x0300	
+#define HT_CAP_RX_STBC_SHIFT	8	
+#define HT_CAP_DELAYED_BA	0x0400	
+#define HT_CAP_MAX_AMSDU	0x0800	
+#define HT_CAP_DSSS_CCK	0x1000	
+#define HT_CAP_PSMP		0x2000	
+#define HT_CAP_40MHZ_INTOLERANT 0x4000	
+#define HT_CAP_LSIG_TXOP	0x8000	
+
+#define HT_CAP_RX_STBC_NO		0x0	
+#define HT_CAP_RX_STBC_ONE_STREAM	0x1	
+#define HT_CAP_RX_STBC_TWO_STREAM	0x2	
+#define HT_CAP_RX_STBC_THREE_STREAM	0x3	
+
+#define HT_MAX_AMSDU		7935	
+#define HT_MIN_AMSDU		3835	
+
+#define HT_PARAMS_RX_FACTOR_MASK	0x03	
+#define HT_PARAMS_DENSITY_MASK		0x1C	
+#define HT_PARAMS_DENSITY_SHIFT	2	
+
+
+#define AMPDU_MAX_MPDU_DENSITY	7	
+#define AMPDU_RX_FACTOR_64K	3	
+#define AMPDU_RX_FACTOR_BASE	8*1024	
+#define AMPDU_DELIMITER_LEN	4	
+
+#define HT_CAP_EXT_PCO			0x0001
+#define HT_CAP_EXT_PCO_TTIME_MASK	0x0006
+#define HT_CAP_EXT_PCO_TTIME_SHIFT	1
+#define HT_CAP_EXT_MCS_FEEDBACK_MASK	0x0300
+#define HT_CAP_EXT_MCS_FEEDBACK_SHIFT	8
+#define HT_CAP_EXT_HTC			0x0400
+#define HT_CAP_EXT_RD_RESP		0x0800
+
+BWL_PRE_PACKED_STRUCT struct ht_add_ie {
+	uint8	ctl_ch;			
+	uint8	byte1;			
+	uint16	opmode;			
+	uint16	misc_bits;		
+	uint8	basic_mcs[MCSSET_LEN];  
+} BWL_POST_PACKED_STRUCT;
+typedef struct ht_add_ie ht_add_ie_t;
+
+
+
+BWL_PRE_PACKED_STRUCT struct ht_prop_add_ie {
+	uint8	id;		
+	uint8	len;		
+	uint8	oui[3];		
+	uint8	type;		
+	ht_add_ie_t add_ie;
+} BWL_POST_PACKED_STRUCT;
+typedef struct ht_prop_add_ie ht_prop_add_ie_t;
+
+#define HT_ADD_IE_LEN	22
+#define HT_ADD_IE_TYPE	52
+
+
+#define HT_BW_ANY		0x04	
+#define HT_RIFS_PERMITTED     	0x08	
+
+
+#define HT_OPMODE_MASK	        0x0003	
+#define HT_OPMODE_SHIFT	0	
+#define HT_OPMODE_PURE		0x0000	
+#define HT_OPMODE_OPTIONAL	0x0001	
+#define HT_OPMODE_HT20IN40	0x0002	
+#define HT_OPMODE_MIXED	0x0003	
+#define HT_OPMODE_NONGF	0x0004	
+#define DOT11N_TXBURST		0x0008	
+#define DOT11N_OBSS_NONHT	0x0010	
+
+
+#define HT_BASIC_STBC_MCS	0x007f	
+#define HT_DUAL_STBC_PROT	0x0080	
+#define HT_SECOND_BCN		0x0100	
+#define HT_LSIG_TXOP		0x0200	
+#define HT_PCO_ACTIVE		0x0400	
+#define HT_PCO_PHASE		0x0800	
+#define HT_DUALCTS_PROTECTION	0x0080	
+
+
+#define DOT11N_2G_TXBURST_LIMIT	6160	
+#define DOT11N_5G_TXBURST_LIMIT	3080	
+
+
+#define GET_HT_OPMODE(add_ie)		((ltoh16_ua(&add_ie->opmode) & HT_OPMODE_MASK) \
+					>> HT_OPMODE_SHIFT)
+#define HT_MIXEDMODE_PRESENT(add_ie)	((ltoh16_ua(&add_ie->opmode) & HT_OPMODE_MASK) \
+					== HT_OPMODE_MIXED)	
+#define HT_HT20_PRESENT(add_ie)	((ltoh16_ua(&add_ie->opmode) & HT_OPMODE_MASK) \
+					== HT_OPMODE_HT20IN40)	
+#define HT_OPTIONAL_PRESENT(add_ie)	((ltoh16_ua(&add_ie->opmode) & HT_OPMODE_MASK) \
+					== HT_OPMODE_OPTIONAL)	
+#define HT_USE_PROTECTION(add_ie)	(HT_HT20_PRESENT((add_ie)) || \
+					HT_MIXEDMODE_PRESENT((add_ie))) 
+#define HT_NONGF_PRESENT(add_ie)	((ltoh16_ua(&add_ie->opmode) & HT_OPMODE_NONGF) \
+					== HT_OPMODE_NONGF)	
+#define DOT11N_TXBURST_PRESENT(add_ie)	((ltoh16_ua(&add_ie->opmode) & DOT11N_TXBURST) \
+					== DOT11N_TXBURST)	
+#define DOT11N_OBSS_NONHT_PRESENT(add_ie)	((ltoh16_ua(&add_ie->opmode) & DOT11N_OBSS_NONHT) \
+					== DOT11N_OBSS_NONHT)	
+
+BWL_PRE_PACKED_STRUCT struct obss_params {
+	uint16	passive_dwell;
+	uint16	active_dwell;
+	uint16	bss_widthscan_interval;
+	uint16	passive_total;
+	uint16	active_total;
+	uint16	chanwidth_transition_dly;
+	uint16	activity_threshold;
+} BWL_POST_PACKED_STRUCT;
+typedef struct obss_params obss_params_t;
+
+BWL_PRE_PACKED_STRUCT struct dot11_obss_ie {
+	uint8	id;
+	uint8	len;
+	obss_params_t obss_params;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_obss_ie dot11_obss_ie_t;
+#define DOT11_OBSS_SCAN_IE_LEN	sizeof(obss_params_t)	
+
+
+BWL_PRE_PACKED_STRUCT struct vndr_ie {
+	uchar id;
+	uchar len;
+	uchar oui [3];
+	uchar data [1]; 	
+} BWL_POST_PACKED_STRUCT;
+typedef struct vndr_ie vndr_ie_t;
+
+#define VNDR_IE_HDR_LEN		2	
+#define VNDR_IE_MIN_LEN		3	
+#define VNDR_IE_MAX_LEN		256	
+
+
+#define WPA_VERSION		1	
+#define WPA_OUI			"\x00\x50\xF2"	
+
+#define WPA2_VERSION		1	
+#define WPA2_VERSION_LEN	2	
+#define WPA2_OUI		"\x00\x0F\xAC"	
+
+#define WPA_OUI_LEN	3	
+
+
+#define RSN_AKM_NONE		0	
+#define RSN_AKM_UNSPECIFIED	1	
+#define RSN_AKM_PSK		2	
+
+
+#define DOT11_MAX_DEFAULT_KEYS	4	
+#define DOT11_MAX_KEY_SIZE	32	
+#define DOT11_MAX_IV_SIZE	16	
+#define DOT11_EXT_IV_FLAG	(1<<5)	
+#define DOT11_WPA_KEY_RSC_LEN   8       
+
+#define WEP1_KEY_SIZE		5	
+#define WEP1_KEY_HEX_SIZE	10	
+#define WEP128_KEY_SIZE		13	
+#define WEP128_KEY_HEX_SIZE	26	
+#define TKIP_MIC_SIZE		8	
+#define TKIP_EOM_SIZE		7	
+#define TKIP_EOM_FLAG		0x5a	
+#define TKIP_KEY_SIZE		32	
+#define TKIP_MIC_AUTH_TX	16	
+#define TKIP_MIC_AUTH_RX	24	
+#define TKIP_MIC_SUP_RX		TKIP_MIC_AUTH_TX	
+#define TKIP_MIC_SUP_TX		TKIP_MIC_AUTH_RX	
+#define AES_KEY_SIZE		16	
+#define AES_MIC_SIZE		8	
+
+#define SMS4_KEY_LEN		16
+#define SMS4_WPI_CBC_MAC_LEN	16
+
+
+#include <packed_section_end.h>
+
+
+#endif 
diff --git a/drivers/net/wireless/bcm4329/include/proto/802.11e.h b/drivers/net/wireless/bcm4329/include/proto/802.11e.h
new file mode 100644
index 000000000000..1dd6f45b1ed8
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/proto/802.11e.h
@@ -0,0 +1,131 @@
+/*
+ * 802.11e protocol header file
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: 802.11e.h,v 1.5.56.1 2008/11/20 00:51:18 Exp $
+ */
+
+#ifndef _802_11e_H_
+#define _802_11e_H_
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+/* This marks the start of a packed structure section. */
+#include <packed_section_start.h>
+
+
+/* WME Traffic Specification (TSPEC) element */
+#define WME_TSPEC_HDR_LEN           2           /* WME TSPEC header length */
+#define WME_TSPEC_BODY_OFF          2           /* WME TSPEC body offset */
+
+#define WME_CATEGORY_CODE_OFFSET	0		/* WME Category code offset */
+#define WME_ACTION_CODE_OFFSET		1		/* WME Action code offset */
+#define WME_TOKEN_CODE_OFFSET		2		/* WME Token code offset */
+#define WME_STATUS_CODE_OFFSET		3		/* WME Status code offset */
+
+BWL_PRE_PACKED_STRUCT struct tsinfo {
+	uint8 octets[3];
+} BWL_POST_PACKED_STRUCT;
+
+typedef struct tsinfo tsinfo_t;
+
+/* 802.11e TSPEC IE */
+typedef BWL_PRE_PACKED_STRUCT struct tspec {
+	uint8 oui[DOT11_OUI_LEN];	/* WME_OUI */
+	uint8 type;					/* WME_TYPE */
+	uint8 subtype;				/* WME_SUBTYPE_TSPEC */
+	uint8 version;				/* WME_VERSION */
+	tsinfo_t tsinfo;			/* TS Info bit field */
+	uint16 nom_msdu_size;		/* (Nominal or fixed) MSDU Size (bytes) */
+	uint16 max_msdu_size;		/* Maximum MSDU Size (bytes) */
+	uint32 min_srv_interval;	/* Minimum Service Interval (us) */
+	uint32 max_srv_interval;	/* Maximum Service Interval (us) */
+	uint32 inactivity_interval;	/* Inactivity Interval (us) */
+	uint32 suspension_interval; /* Suspension Interval (us) */
+	uint32 srv_start_time;		/* Service Start Time (us) */
+	uint32 min_data_rate;		/* Minimum Data Rate (bps) */
+	uint32 mean_data_rate;		/* Mean Data Rate (bps) */
+	uint32 peak_data_rate;		/* Peak Data Rate (bps) */
+	uint32 max_burst_size;		/* Maximum Burst Size (bytes) */
+	uint32 delay_bound;			/* Delay Bound (us) */
+	uint32 min_phy_rate;		/* Minimum PHY Rate (bps) */
+	uint16 surplus_bw;			/* Surplus Bandwidth Allowance (range 1.0-8.0) */
+	uint16 medium_time;			/* Medium Time (32 us/s periods) */
+} BWL_POST_PACKED_STRUCT tspec_t;
+
+#define WME_TSPEC_LEN	(sizeof(tspec_t))		/* not including 2-bytes of header */
+
+/* ts_info */
+/* 802.1D priority is duplicated - bits 13-11 AND bits 3-1 */
+#define TS_INFO_TID_SHIFT		1	/* TS info. TID shift */
+#define TS_INFO_TID_MASK		(0xf << TS_INFO_TID_SHIFT)	/* TS info. TID mask */
+#define TS_INFO_CONTENTION_SHIFT	7	/* TS info. contention shift */
+#define TS_INFO_CONTENTION_MASK	(0x1 << TS_INFO_CONTENTION_SHIFT) /* TS info. contention mask */
+#define TS_INFO_DIRECTION_SHIFT	5	/* TS info. direction shift */
+#define TS_INFO_DIRECTION_MASK	(0x3 << TS_INFO_DIRECTION_SHIFT) /* TS info. direction mask */
+#define TS_INFO_PSB_SHIFT		2		/* TS info. PSB bit Shift */
+#define TS_INFO_PSB_MASK		(1 << TS_INFO_PSB_SHIFT)	/* TS info. PSB mask */
+#define TS_INFO_UPLINK			(0 << TS_INFO_DIRECTION_SHIFT)	/* TS info. uplink */
+#define TS_INFO_DOWNLINK		(1 << TS_INFO_DIRECTION_SHIFT)	/* TS info. downlink */
+#define TS_INFO_BIDIRECTIONAL	(3 << TS_INFO_DIRECTION_SHIFT)	/* TS info. bidirectional */
+#define TS_INFO_USER_PRIO_SHIFT	3	/* TS info. user priority shift */
+/* TS info. user priority mask */
+#define TS_INFO_USER_PRIO_MASK	(0x7 << TS_INFO_USER_PRIO_SHIFT)
+
+/* Macro to get/set bit(s) field in TSINFO */
+#define WLC_CAC_GET_TID(pt)	((((pt).octets[0]) & TS_INFO_TID_MASK) >> TS_INFO_TID_SHIFT)
+#define WLC_CAC_GET_DIR(pt)	((((pt).octets[0]) & \
+	TS_INFO_DIRECTION_MASK) >> TS_INFO_DIRECTION_SHIFT)
+#define WLC_CAC_GET_PSB(pt)	((((pt).octets[1]) & TS_INFO_PSB_MASK) >> TS_INFO_PSB_SHIFT)
+#define WLC_CAC_GET_USER_PRIO(pt)	((((pt).octets[1]) & \
+	TS_INFO_USER_PRIO_MASK) >> TS_INFO_USER_PRIO_SHIFT)
+
+#define WLC_CAC_SET_TID(pt, id)	((((pt).octets[0]) & (~TS_INFO_TID_MASK)) | \
+	((id) << TS_INFO_TID_SHIFT))
+#define WLC_CAC_SET_USER_PRIO(pt, prio)	((((pt).octets[0]) & (~TS_INFO_USER_PRIO_MASK)) | \
+	((prio) << TS_INFO_USER_PRIO_SHIFT))
+
+/* 802.11e QBSS Load IE */
+#define QBSS_LOAD_IE_LEN		5	/* QBSS Load IE length */
+#define QBSS_LOAD_AAC_OFF		3	/* AAC offset in IE */
+
+#define CAC_ADDTS_RESP_TIMEOUT		300	/* default ADDTS response timeout in ms */
+
+/* 802.11e ADDTS status code */
+#define DOT11E_STATUS_ADMISSION_ACCEPTED	0	/* TSPEC Admission accepted status */
+#define DOT11E_STATUS_ADDTS_INVALID_PARAM	1	/* TSPEC invalid parameter status */
+#define DOT11E_STATUS_ADDTS_REFUSED_NSBW	3	/* ADDTS refused (non-sufficient BW) */
+#define DOT11E_STATUS_ADDTS_REFUSED_AWHILE	47	/* ADDTS refused but could retry later */
+
+/* 802.11e DELTS status code */
+#define DOT11E_STATUS_QSTA_LEAVE_QBSS		36	/* STA leave QBSS */
+#define DOT11E_STATUS_END_TS				37	/* END TS */
+#define DOT11E_STATUS_UNKNOWN_TS			38	/* UNKNOWN TS */
+#define DOT11E_STATUS_QSTA_REQ_TIMEOUT		39	/* STA ADDTS request timeout */
+
+
+/* This marks the end of a packed structure section. */
+#include <packed_section_end.h>
+
+#endif /* _802_11e_CAC_H_ */
diff --git a/drivers/net/wireless/bcm4329/include/proto/802.1d.h b/drivers/net/wireless/bcm4329/include/proto/802.1d.h
new file mode 100644
index 000000000000..45c728bc2976
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/proto/802.1d.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * Fundamental types and constants relating to 802.1D
+ *
+ * $Id: 802.1d.h,v 9.3 2007/04/10 21:33:06 Exp $
+ */
+
+
+#ifndef _802_1_D_
+#define _802_1_D_
+
+
+#define	PRIO_8021D_NONE		2	
+#define	PRIO_8021D_BK		1	
+#define	PRIO_8021D_BE		0	
+#define	PRIO_8021D_EE		3	
+#define	PRIO_8021D_CL		4	
+#define	PRIO_8021D_VI		5	
+#define	PRIO_8021D_VO		6	
+#define	PRIO_8021D_NC		7	
+#define	MAXPRIO			7	
+#define NUMPRIO			(MAXPRIO + 1)
+
+#define ALLPRIO		-1	
+
+
+#define PRIO2PREC(prio) \
+	(((prio) == PRIO_8021D_NONE || (prio) == PRIO_8021D_BE) ? ((prio^2)) : (prio))
+
+#endif 
diff --git a/drivers/net/wireless/bcm4329/include/proto/bcmeth.h b/drivers/net/wireless/bcm4329/include/proto/bcmeth.h
new file mode 100644
index 000000000000..fdb5a2a5648f
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/proto/bcmeth.h
@@ -0,0 +1,83 @@
+/*
+ * Broadcom Ethernettype  protocol definitions
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmeth.h,v 9.9.46.1 2008/11/20 00:51:20 Exp $
+ */
+
+
+
+
+#ifndef _BCMETH_H_
+#define _BCMETH_H_
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+
+#include <packed_section_start.h>
+
+
+
+
+
+
+
+#define	BCMILCP_SUBTYPE_RATE		1
+#define	BCMILCP_SUBTYPE_LINK		2
+#define	BCMILCP_SUBTYPE_CSA		3
+#define	BCMILCP_SUBTYPE_LARQ		4
+#define BCMILCP_SUBTYPE_VENDOR		5
+#define	BCMILCP_SUBTYPE_FLH		17
+
+#define BCMILCP_SUBTYPE_VENDOR_LONG	32769
+#define BCMILCP_SUBTYPE_CERT		32770
+#define BCMILCP_SUBTYPE_SES		32771
+
+
+#define BCMILCP_BCM_SUBTYPE_RESERVED		0
+#define BCMILCP_BCM_SUBTYPE_EVENT		1
+#define BCMILCP_BCM_SUBTYPE_SES			2
+
+
+#define BCMILCP_BCM_SUBTYPE_DPT			4
+
+#define BCMILCP_BCM_SUBTYPEHDR_MINLENGTH	8
+#define BCMILCP_BCM_SUBTYPEHDR_VERSION		0
+
+
+typedef BWL_PRE_PACKED_STRUCT struct bcmeth_hdr
+{
+	uint16	subtype;	
+	uint16	length;
+	uint8	version;	
+	uint8	oui[3];		
+	
+	uint16	usr_subtype;
+} BWL_POST_PACKED_STRUCT bcmeth_hdr_t;
+
+
+
+#include <packed_section_end.h>
+
+#endif	
diff --git a/drivers/net/wireless/bcm4329/include/proto/bcmevent.h b/drivers/net/wireless/bcm4329/include/proto/bcmevent.h
new file mode 100644
index 000000000000..1f8ecb14d97a
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/proto/bcmevent.h
@@ -0,0 +1,212 @@
+/*
+ * Broadcom Event  protocol definitions
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ *
+ * Dependencies: proto/bcmeth.h
+ *
+ * $Id: bcmevent.h,v 9.34.4.1.20.16.64.1 2010/11/08 21:57:03 Exp $
+ *
+ */
+
+
+
+
+#ifndef _BCMEVENT_H_
+#define _BCMEVENT_H_
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+
+#include <packed_section_start.h>
+
+#define BCM_EVENT_MSG_VERSION		1	
+#define BCM_MSG_IFNAME_MAX		16	
+
+
+#define WLC_EVENT_MSG_LINK		0x01	
+#define WLC_EVENT_MSG_FLUSHTXQ		0x02	
+#define WLC_EVENT_MSG_GROUP		0x04	
+
+
+typedef BWL_PRE_PACKED_STRUCT struct
+{
+	uint16	version;
+	uint16	flags;			
+	uint32	event_type;		
+	uint32	status;			
+	uint32	reason;			
+	uint32	auth_type;		
+	uint32	datalen;		
+	struct ether_addr	addr;	
+	char	ifname[BCM_MSG_IFNAME_MAX]; 
+} BWL_POST_PACKED_STRUCT wl_event_msg_t;
+
+
+typedef BWL_PRE_PACKED_STRUCT struct bcm_event {
+	struct ether_header eth;
+	bcmeth_hdr_t		bcm_hdr;
+	wl_event_msg_t		event;
+	
+} BWL_POST_PACKED_STRUCT bcm_event_t;
+
+#define BCM_MSG_LEN	(sizeof(bcm_event_t) - sizeof(bcmeth_hdr_t) - sizeof(struct ether_header))
+
+
+#define WLC_E_SET_SSID		0	
+#define WLC_E_JOIN		1	
+#define WLC_E_START		2	
+#define WLC_E_AUTH		3	
+#define WLC_E_AUTH_IND		4	
+#define WLC_E_DEAUTH		5	
+#define WLC_E_DEAUTH_IND	6	
+#define WLC_E_ASSOC		7	
+#define WLC_E_ASSOC_IND		8	
+#define WLC_E_REASSOC		9	
+#define WLC_E_REASSOC_IND	10	
+#define WLC_E_DISASSOC		11	
+#define WLC_E_DISASSOC_IND	12	
+#define WLC_E_QUIET_START	13	
+#define WLC_E_QUIET_END		14	
+#define WLC_E_BEACON_RX		15	
+#define WLC_E_LINK		16	
+#define WLC_E_MIC_ERROR		17	
+#define WLC_E_NDIS_LINK		18	
+#define WLC_E_ROAM		19	
+#define WLC_E_TXFAIL		20	
+#define WLC_E_PMKID_CACHE	21	
+#define WLC_E_RETROGRADE_TSF	22	
+#define WLC_E_PRUNE		23	
+#define WLC_E_AUTOAUTH		24	
+#define WLC_E_EAPOL_MSG		25	
+#define WLC_E_SCAN_COMPLETE	26	
+#define WLC_E_ADDTS_IND		27	
+#define WLC_E_DELTS_IND		28	
+#define WLC_E_BCNSENT_IND	29	
+#define WLC_E_BCNRX_MSG		30	
+#define WLC_E_BCNLOST_MSG	31	
+#define WLC_E_ROAM_PREP		32	
+#define WLC_E_PFN_NET_FOUND	33	
+#define WLC_E_PFN_NET_LOST	34	
+#define WLC_E_RESET_COMPLETE	35
+#define WLC_E_JOIN_START	36
+#define WLC_E_ROAM_START	37
+#define WLC_E_ASSOC_START	38
+#define WLC_E_IBSS_ASSOC	39
+#define WLC_E_RADIO		40
+#define WLC_E_PSM_WATCHDOG	41	
+#define WLC_E_PROBREQ_MSG       44      
+#define WLC_E_SCAN_CONFIRM_IND  45
+#define WLC_E_PSK_SUP	46	
+#define WLC_E_COUNTRY_CODE_CHANGED 47
+#define	WLC_E_EXCEEDED_MEDIUM_TIME 48	
+#define WLC_E_ICV_ERROR		49	
+#define WLC_E_UNICAST_DECODE_ERROR 50 
+#define WLC_E_MULTICAST_DECODE_ERROR 51 
+#define WLC_E_TRACE 52
+#define WLC_E_IF		54	
+#define WLC_E_RSSI		56	
+#define WLC_E_PFN_SCAN_COMPLETE	57	
+#define WLC_E_ACTION_FRAME      58      
+#define WLC_E_ACTION_FRAME_COMPLETE 59  
+
+#define WLC_E_ESCAN_RESULT	69
+#define WLC_E_WAKE_EVENT	70
+#define WLC_E_RELOAD		71
+#define WLC_E_LAST		72
+
+
+
+#define WLC_E_STATUS_SUCCESS		0	
+#define WLC_E_STATUS_FAIL		1	
+#define WLC_E_STATUS_TIMEOUT		2	
+#define WLC_E_STATUS_NO_NETWORKS	3	
+#define WLC_E_STATUS_ABORT		4	
+#define WLC_E_STATUS_NO_ACK		5	
+#define WLC_E_STATUS_UNSOLICITED	6	
+#define WLC_E_STATUS_ATTEMPT		7	
+#define WLC_E_STATUS_PARTIAL		8	
+#define WLC_E_STATUS_NEWSCAN	9	
+#define WLC_E_STATUS_NEWASSOC	10	
+#define WLC_E_STATUS_11HQUIET	11	
+#define WLC_E_STATUS_SUPPRESS	12	
+#define WLC_E_STATUS_NOCHANS	13	
+#define WLC_E_STATUS_CCXFASTRM	14	
+#define WLC_E_STATUS_CS_ABORT	15	
+
+
+#define WLC_E_REASON_INITIAL_ASSOC	0	
+#define WLC_E_REASON_LOW_RSSI		1	
+#define WLC_E_REASON_DEAUTH		2	
+#define WLC_E_REASON_DISASSOC		3	
+#define WLC_E_REASON_BCNS_LOST		4	
+#define WLC_E_REASON_FAST_ROAM_FAILED	5	
+#define WLC_E_REASON_DIRECTED_ROAM	6	
+#define WLC_E_REASON_TSPEC_REJECTED	7	
+#define WLC_E_REASON_BETTER_AP		8	
+
+
+#define WLC_E_PRUNE_ENCR_MISMATCH	1	
+#define WLC_E_PRUNE_BCAST_BSSID		2	
+#define WLC_E_PRUNE_MAC_DENY		3	
+#define WLC_E_PRUNE_MAC_NA		4	
+#define WLC_E_PRUNE_REG_PASSV		5	
+#define WLC_E_PRUNE_SPCT_MGMT		6	
+#define WLC_E_PRUNE_RADAR		7	
+#define WLC_E_RSN_MISMATCH		8	
+#define WLC_E_PRUNE_NO_COMMON_RATES	9	
+#define WLC_E_PRUNE_BASIC_RATES		10	
+#define WLC_E_PRUNE_CIPHER_NA		12	
+#define WLC_E_PRUNE_KNOWN_STA		13	
+#define WLC_E_PRUNE_WDS_PEER		15	
+#define WLC_E_PRUNE_QBSS_LOAD		16	
+#define WLC_E_PRUNE_HOME_AP		17	
+
+
+#define WLC_E_SUP_OTHER				0	
+#define WLC_E_SUP_DECRYPT_KEY_DATA	1	
+#define WLC_E_SUP_BAD_UCAST_WEP128	2	
+#define WLC_E_SUP_BAD_UCAST_WEP40	3	
+#define WLC_E_SUP_UNSUP_KEY_LEN		4	
+#define WLC_E_SUP_PW_KEY_CIPHER		5	
+#define WLC_E_SUP_MSG3_TOO_MANY_IE	6	
+#define WLC_E_SUP_MSG3_IE_MISMATCH	7	
+#define WLC_E_SUP_NO_INSTALL_FLAG	8	
+#define WLC_E_SUP_MSG3_NO_GTK		9	
+#define WLC_E_SUP_GRP_KEY_CIPHER	10	
+#define WLC_E_SUP_GRP_MSG1_NO_GTK	11	
+#define WLC_E_SUP_GTK_DECRYPT_FAIL	12	
+#define WLC_E_SUP_SEND_FAIL			13	
+#define WLC_E_SUP_DEAUTH			14	
+#define WLC_E_SUP_WPA_PSK_TMO       15  
+
+
+#define WLC_E_IF_ADD		1	
+#define WLC_E_IF_DEL		2	
+
+#define WLC_E_RELOAD_STATUS1	1
+
+#include <packed_section_end.h>
+
+#endif 
diff --git a/drivers/net/wireless/bcm4329/include/proto/bcmip.h b/drivers/net/wireless/bcm4329/include/proto/bcmip.h
new file mode 100644
index 000000000000..9d2fd6fba484
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/proto/bcmip.h
@@ -0,0 +1,157 @@
+/*
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * Fundamental constants relating to IP Protocol
+ *
+ * $Id: bcmip.h,v 9.16.186.4 2009/01/27 04:25:25 Exp $
+ */
+
+
+#ifndef _bcmip_h_
+#define _bcmip_h_
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+
+#include <packed_section_start.h>
+
+
+
+#define IP_VER_OFFSET		0x0	
+#define IP_VER_MASK		0xf0	
+#define IP_VER_SHIFT		4	
+#define IP_VER_4		4	
+#define IP_VER_6		6	
+
+#define IP_VER(ip_body) \
+	((((uint8 *)(ip_body))[IP_VER_OFFSET] & IP_VER_MASK) >> IP_VER_SHIFT)
+
+#define IP_PROT_ICMP		0x1	
+#define IP_PROT_TCP		0x6	
+#define IP_PROT_UDP		0x11	
+
+
+#define IPV4_VER_HL_OFFSET	0	
+#define IPV4_TOS_OFFSET		1	
+#define IPV4_PKTLEN_OFFSET	2	
+#define IPV4_PKTFLAG_OFFSET	6	
+#define IPV4_PROT_OFFSET	9	
+#define IPV4_CHKSUM_OFFSET	10	
+#define IPV4_SRC_IP_OFFSET	12	
+#define IPV4_DEST_IP_OFFSET	16	
+#define IPV4_OPTIONS_OFFSET	20	
+
+
+#define IPV4_VER_MASK		0xf0	
+#define IPV4_VER_SHIFT		4	
+
+#define IPV4_HLEN_MASK		0x0f	
+#define IPV4_HLEN(ipv4_body)	(4 * (((uint8 *)(ipv4_body))[IPV4_VER_HL_OFFSET] & IPV4_HLEN_MASK))
+
+#define IPV4_ADDR_LEN		4	
+
+#define IPV4_ADDR_NULL(a)	((((uint8 *)(a))[0] | ((uint8 *)(a))[1] | \
+				  ((uint8 *)(a))[2] | ((uint8 *)(a))[3]) == 0)
+
+#define IPV4_ADDR_BCAST(a)	((((uint8 *)(a))[0] & ((uint8 *)(a))[1] & \
+				  ((uint8 *)(a))[2] & ((uint8 *)(a))[3]) == 0xff)
+
+#define	IPV4_TOS_DSCP_MASK	0xfc	
+#define	IPV4_TOS_DSCP_SHIFT	2	
+
+#define	IPV4_TOS(ipv4_body)	(((uint8 *)(ipv4_body))[IPV4_TOS_OFFSET])
+
+#define	IPV4_TOS_PREC_MASK	0xe0	
+#define	IPV4_TOS_PREC_SHIFT	5	
+
+#define IPV4_TOS_LOWDELAY	0x10	
+#define IPV4_TOS_THROUGHPUT	0x8	
+#define IPV4_TOS_RELIABILITY	0x4	
+
+#define IPV4_PROT(ipv4_body)	(((uint8 *)(ipv4_body))[IPV4_PROT_OFFSET])
+
+#define IPV4_FRAG_RESV		0x8000	
+#define IPV4_FRAG_DONT		0x4000	
+#define IPV4_FRAG_MORE		0x2000	
+#define IPV4_FRAG_OFFSET_MASK	0x1fff	
+
+#define IPV4_ADDR_STR_LEN	16	
+
+
+BWL_PRE_PACKED_STRUCT struct ipv4_addr {
+	uint8	addr[IPV4_ADDR_LEN];
+} BWL_POST_PACKED_STRUCT;
+
+BWL_PRE_PACKED_STRUCT struct ipv4_hdr {
+	uint8	version_ihl;		
+	uint8	tos;			
+	uint16	tot_len;		
+	uint16	id;
+	uint16	frag;			
+	uint8	ttl;			
+	uint8	prot;			
+	uint16	hdr_chksum;		
+	uint8	src_ip[IPV4_ADDR_LEN];	
+	uint8	dst_ip[IPV4_ADDR_LEN];	
+} BWL_POST_PACKED_STRUCT;
+
+
+#define IPV6_PAYLOAD_LEN_OFFSET	4	
+#define IPV6_NEXT_HDR_OFFSET	6	
+#define IPV6_HOP_LIMIT_OFFSET	7	
+#define IPV6_SRC_IP_OFFSET	8	
+#define IPV6_DEST_IP_OFFSET	24	
+
+
+#define IPV6_TRAFFIC_CLASS(ipv6_body) \
+	(((((uint8 *)(ipv6_body))[0] & 0x0f) << 4) | \
+	 ((((uint8 *)(ipv6_body))[1] & 0xf0) >> 4))
+
+#define IPV6_FLOW_LABEL(ipv6_body) \
+	(((((uint8 *)(ipv6_body))[1] & 0x0f) << 16) | \
+	 (((uint8 *)(ipv6_body))[2] << 8) | \
+	 (((uint8 *)(ipv6_body))[3]))
+
+#define IPV6_PAYLOAD_LEN(ipv6_body) \
+	((((uint8 *)(ipv6_body))[IPV6_PAYLOAD_LEN_OFFSET + 0] << 8) | \
+	 ((uint8 *)(ipv6_body))[IPV6_PAYLOAD_LEN_OFFSET + 1])
+
+#define IPV6_NEXT_HDR(ipv6_body) \
+	(((uint8 *)(ipv6_body))[IPV6_NEXT_HDR_OFFSET])
+
+#define IPV6_PROT(ipv6_body)	IPV6_NEXT_HDR(ipv6_body)
+
+#define IPV6_ADDR_LEN		16	
+
+
+#ifndef IP_TOS
+#define IP_TOS(ip_body) \
+	(IP_VER(ip_body) == IP_VER_4 ? IPV4_TOS(ip_body) : \
+	 IP_VER(ip_body) == IP_VER_6 ? IPV6_TRAFFIC_CLASS(ip_body) : 0)
+#endif
+
+
+
+#include <packed_section_end.h>
+
+#endif	
diff --git a/drivers/net/wireless/bcm4329/include/proto/eapol.h b/drivers/net/wireless/bcm4329/include/proto/eapol.h
new file mode 100644
index 000000000000..95e76ff18c6b
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/proto/eapol.h
@@ -0,0 +1,172 @@
+/*
+ * 802.1x EAPOL definitions
+ *
+ * See
+ * IEEE Std 802.1X-2001
+ * IEEE 802.1X RADIUS Usage Guidelines
+ *
+ * Copyright (C) 2002 Broadcom Corporation
+ *
+ * $Id: eapol.h,v 9.18.260.1.2.1.6.6 2009/04/08 05:00:08 Exp $
+ */
+
+#ifndef _eapol_h_
+#define _eapol_h_
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+/* This marks the start of a packed structure section. */
+#include <packed_section_start.h>
+
+#define AKW_BLOCK_LEN	8	/* The only def we need here */
+
+/* EAPOL for 802.3/Ethernet */
+typedef struct {
+	struct ether_header eth;	/* 802.3/Ethernet header */
+	unsigned char version;		/* EAPOL protocol version */
+	unsigned char type;		/* EAPOL type */
+	unsigned short length;		/* Length of body */
+	unsigned char body[1];		/* Body (optional) */
+} eapol_header_t;
+
+#define EAPOL_HEADER_LEN 18
+
+/* EAPOL version */
+#define WPA2_EAPOL_VERSION	2
+#define WPA_EAPOL_VERSION	1
+#define LEAP_EAPOL_VERSION	1
+#define SES_EAPOL_VERSION	1
+
+/* EAPOL types */
+#define EAP_PACKET		0
+#define EAPOL_START		1
+#define EAPOL_LOGOFF		2
+#define EAPOL_KEY		3
+#define EAPOL_ASF		4
+
+/* EAPOL-Key types */
+#define EAPOL_RC4_KEY		1
+#define EAPOL_WPA2_KEY		2	/* 802.11i/WPA2 */
+#define EAPOL_WPA_KEY		254	/* WPA */
+
+/* RC4 EAPOL-Key header field sizes */
+#define EAPOL_KEY_REPLAY_LEN	8
+#define EAPOL_KEY_IV_LEN	16
+#define EAPOL_KEY_SIG_LEN	16
+
+/* RC4 EAPOL-Key */
+typedef BWL_PRE_PACKED_STRUCT struct {
+	unsigned char type;			/* Key Descriptor Type */
+	unsigned short length;			/* Key Length (unaligned) */
+	unsigned char replay[EAPOL_KEY_REPLAY_LEN];	/* Replay Counter */
+	unsigned char iv[EAPOL_KEY_IV_LEN];		/* Key IV */
+	unsigned char index;				/* Key Flags & Index */
+	unsigned char signature[EAPOL_KEY_SIG_LEN];	/* Key Signature */
+	unsigned char key[1];				/* Key (optional) */
+} BWL_POST_PACKED_STRUCT eapol_key_header_t;
+
+#define EAPOL_KEY_HEADER_LEN 	44
+
+/* RC4 EAPOL-Key flags */
+#define EAPOL_KEY_FLAGS_MASK	0x80
+#define EAPOL_KEY_BROADCAST	0
+#define EAPOL_KEY_UNICAST	0x80
+
+/* RC4 EAPOL-Key index */
+#define EAPOL_KEY_INDEX_MASK	0x7f
+
+/* WPA/802.11i/WPA2 EAPOL-Key header field sizes */
+#define EAPOL_WPA_KEY_REPLAY_LEN	8
+#define EAPOL_WPA_KEY_NONCE_LEN		32
+#define EAPOL_WPA_KEY_IV_LEN		16
+#define EAPOL_WPA_KEY_ID_LEN		8
+#define EAPOL_WPA_KEY_RSC_LEN		8
+#define EAPOL_WPA_KEY_MIC_LEN		16
+#define EAPOL_WPA_KEY_DATA_LEN		(EAPOL_WPA_MAX_KEY_SIZE + AKW_BLOCK_LEN)
+#define EAPOL_WPA_MAX_KEY_SIZE		32
+
+/* WPA EAPOL-Key */
+typedef BWL_PRE_PACKED_STRUCT struct {
+	unsigned char type;		/* Key Descriptor Type */
+	unsigned short key_info;	/* Key Information (unaligned) */
+	unsigned short key_len;		/* Key Length (unaligned) */
+	unsigned char replay[EAPOL_WPA_KEY_REPLAY_LEN];	/* Replay Counter */
+	unsigned char nonce[EAPOL_WPA_KEY_NONCE_LEN];	/* Nonce */
+	unsigned char iv[EAPOL_WPA_KEY_IV_LEN];		/* Key IV */
+	unsigned char rsc[EAPOL_WPA_KEY_RSC_LEN];	/* Key RSC */
+	unsigned char id[EAPOL_WPA_KEY_ID_LEN];		/* WPA:Key ID, 802.11i/WPA2: Reserved */
+	unsigned char mic[EAPOL_WPA_KEY_MIC_LEN];	/* Key MIC */
+	unsigned short data_len;			/* Key Data Length */
+	unsigned char data[EAPOL_WPA_KEY_DATA_LEN];	/* Key data */
+} BWL_POST_PACKED_STRUCT eapol_wpa_key_header_t;
+
+#define EAPOL_WPA_KEY_LEN 		95
+
+/* WPA/802.11i/WPA2 KEY KEY_INFO bits */
+#define WPA_KEY_DESC_V1		0x01
+#define WPA_KEY_DESC_V2		0x02
+#define WPA_KEY_PAIRWISE	0x08
+#define WPA_KEY_INSTALL		0x40
+#define WPA_KEY_ACK		0x80
+#define WPA_KEY_MIC		0x100
+#define WPA_KEY_SECURE		0x200
+#define WPA_KEY_ERROR		0x400
+#define WPA_KEY_REQ		0x800
+
+/* WPA-only KEY KEY_INFO bits */
+#define WPA_KEY_INDEX_0		0x00
+#define WPA_KEY_INDEX_1		0x10
+#define WPA_KEY_INDEX_2		0x20
+#define WPA_KEY_INDEX_3		0x30
+#define WPA_KEY_INDEX_MASK	0x30
+#define WPA_KEY_INDEX_SHIFT	0x04
+
+/* 802.11i/WPA2-only KEY KEY_INFO bits */
+#define WPA_KEY_ENCRYPTED_DATA	0x1000
+
+/* Key Data encapsulation */
+typedef BWL_PRE_PACKED_STRUCT struct {
+	uint8 type;
+	uint8 length;
+	uint8 oui[3];
+	uint8 subtype;
+	uint8 data[1];
+} BWL_POST_PACKED_STRUCT eapol_wpa2_encap_data_t;
+
+#define EAPOL_WPA2_ENCAP_DATA_HDR_LEN 	6
+
+#define WPA2_KEY_DATA_SUBTYPE_GTK	1
+#define WPA2_KEY_DATA_SUBTYPE_STAKEY	2
+#define WPA2_KEY_DATA_SUBTYPE_MAC	3
+#define WPA2_KEY_DATA_SUBTYPE_PMKID	4
+
+/* GTK encapsulation */
+typedef BWL_PRE_PACKED_STRUCT struct {
+	uint8	flags;
+	uint8	reserved;
+	uint8	gtk[EAPOL_WPA_MAX_KEY_SIZE];
+} BWL_POST_PACKED_STRUCT eapol_wpa2_key_gtk_encap_t;
+
+#define EAPOL_WPA2_KEY_GTK_ENCAP_HDR_LEN 	2
+
+#define WPA2_GTK_INDEX_MASK	0x03
+#define WPA2_GTK_INDEX_SHIFT	0x00
+
+#define WPA2_GTK_TRANSMIT	0x04
+
+/* STAKey encapsulation */
+typedef BWL_PRE_PACKED_STRUCT struct {
+	uint8	reserved[2];
+	uint8	mac[ETHER_ADDR_LEN];
+	uint8	stakey[EAPOL_WPA_MAX_KEY_SIZE];
+} BWL_POST_PACKED_STRUCT eapol_wpa2_key_stakey_encap_t;
+
+#define WPA2_KEY_DATA_PAD	0xdd
+
+
+/* This marks the end of a packed structure section. */
+#include <packed_section_end.h>
+
+#endif /* _eapol_h_ */
diff --git a/drivers/net/wireless/bcm4329/include/proto/ethernet.h b/drivers/net/wireless/bcm4329/include/proto/ethernet.h
new file mode 100644
index 000000000000..9ad2ea0c70fd
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/proto/ethernet.h
@@ -0,0 +1,148 @@
+/*
+ * From FreeBSD 2.2.7: Fundamental constants relating to ethernet.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: ethernet.h,v 9.45.56.5 2010/02/22 22:04:36 Exp $
+ */
+
+
+#ifndef _NET_ETHERNET_H_	      
+#define _NET_ETHERNET_H_
+
+#ifndef _TYPEDEFS_H_
+#include "typedefs.h"
+#endif
+
+
+#include <packed_section_start.h>
+
+
+
+#define	ETHER_ADDR_LEN		6
+
+
+#define	ETHER_TYPE_LEN		2
+
+
+#define	ETHER_CRC_LEN		4
+
+
+#define	ETHER_HDR_LEN		(ETHER_ADDR_LEN * 2 + ETHER_TYPE_LEN)
+
+
+#define	ETHER_MIN_LEN		64
+
+
+#define	ETHER_MIN_DATA		46
+
+
+#define	ETHER_MAX_LEN		1518
+
+
+#define	ETHER_MAX_DATA		1500
+
+
+#define ETHER_TYPE_MIN		0x0600		
+#define	ETHER_TYPE_IP		0x0800		
+#define ETHER_TYPE_ARP		0x0806		
+#define ETHER_TYPE_8021Q	0x8100		
+#define	ETHER_TYPE_BRCM		0x886c		
+#define	ETHER_TYPE_802_1X	0x888e		
+#define ETHER_TYPE_WAI		0x88b4		
+#ifdef BCMWPA2
+#define	ETHER_TYPE_802_1X_PREAUTH 0x88c7	
+#endif
+
+
+#define	ETHER_BRCM_SUBTYPE_LEN	4	
+#define	ETHER_BRCM_CRAM		1	
+
+
+#define ETHER_DEST_OFFSET	(0 * ETHER_ADDR_LEN)	
+#define ETHER_SRC_OFFSET	(1 * ETHER_ADDR_LEN)	
+#define ETHER_TYPE_OFFSET	(2 * ETHER_ADDR_LEN)	
+
+
+#define	ETHER_IS_VALID_LEN(foo)	\
+	((foo) >= ETHER_MIN_LEN && (foo) <= ETHER_MAX_LEN)
+
+
+#ifndef __INCif_etherh       
+
+BWL_PRE_PACKED_STRUCT struct ether_header {
+	uint8	ether_dhost[ETHER_ADDR_LEN];
+	uint8	ether_shost[ETHER_ADDR_LEN];
+	uint16	ether_type;
+} BWL_POST_PACKED_STRUCT;
+
+
+BWL_PRE_PACKED_STRUCT struct	ether_addr {
+	uint8 octet[ETHER_ADDR_LEN];
+} BWL_POST_PACKED_STRUCT;
+#endif	
+
+
+#define ETHER_SET_LOCALADDR(ea)	(((uint8 *)(ea))[0] = (((uint8 *)(ea))[0] | 2))
+#define ETHER_IS_LOCALADDR(ea) 	(((uint8 *)(ea))[0] & 2)
+#define ETHER_CLR_LOCALADDR(ea)	(((uint8 *)(ea))[0] = (((uint8 *)(ea))[0] & 0xd))
+#define ETHER_TOGGLE_LOCALADDR(ea)	(((uint8 *)(ea))[0] = (((uint8 *)(ea))[0] ^ 2))
+
+
+#define ETHER_SET_UNICAST(ea)	(((uint8 *)(ea))[0] = (((uint8 *)(ea))[0] & ~1))
+
+
+#define ETHER_ISMULTI(ea) (((const uint8 *)(ea))[0] & 1)
+
+
+
+#define	ether_cmp(a, b)	(!(((short*)a)[0] == ((short*)b)[0]) | \
+			 !(((short*)a)[1] == ((short*)b)[1]) | \
+			 !(((short*)a)[2] == ((short*)b)[2]))
+
+
+#define	ether_copy(s, d) { \
+		((short*)d)[0] = ((short*)s)[0]; \
+		((short*)d)[1] = ((short*)s)[1]; \
+		((short*)d)[2] = ((short*)s)[2]; }
+
+
+static const struct ether_addr ether_bcast = {{255, 255, 255, 255, 255, 255}};
+static const struct ether_addr ether_null = {{0, 0, 0, 0, 0, 0}};
+
+#define ETHER_ISBCAST(ea)	((((uint8 *)(ea))[0] &		\
+	                          ((uint8 *)(ea))[1] &		\
+				  ((uint8 *)(ea))[2] &		\
+				  ((uint8 *)(ea))[3] &		\
+				  ((uint8 *)(ea))[4] &		\
+				  ((uint8 *)(ea))[5]) == 0xff)
+#define ETHER_ISNULLADDR(ea)	((((uint8 *)(ea))[0] |		\
+				  ((uint8 *)(ea))[1] |		\
+				  ((uint8 *)(ea))[2] |		\
+				  ((uint8 *)(ea))[3] |		\
+				  ((uint8 *)(ea))[4] |		\
+				  ((uint8 *)(ea))[5]) == 0)
+
+
+
+#include <packed_section_end.h>
+
+#endif 
diff --git a/drivers/net/wireless/bcm4329/include/proto/sdspi.h b/drivers/net/wireless/bcm4329/include/proto/sdspi.h
new file mode 100644
index 000000000000..7739e68a2440
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/proto/sdspi.h
@@ -0,0 +1,71 @@
+/*
+ * SD-SPI Protocol Standard
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sdspi.h,v 9.1.20.1 2008/05/06 22:59:19 Exp $
+ */
+
+#define SPI_START_M		BITFIELD_MASK(1)	/* Bit [31] 	- Start Bit */
+#define SPI_START_S		31
+#define SPI_DIR_M		BITFIELD_MASK(1)	/* Bit [30] 	- Direction */
+#define SPI_DIR_S		30
+#define SPI_CMD_INDEX_M		BITFIELD_MASK(6)	/* Bits [29:24] - Command number */
+#define SPI_CMD_INDEX_S		24
+#define SPI_RW_M		BITFIELD_MASK(1)	/* Bit [23] 	- Read=0, Write=1 */
+#define SPI_RW_S		23
+#define SPI_FUNC_M		BITFIELD_MASK(3)	/* Bits [22:20]	- Function Number */
+#define SPI_FUNC_S		20
+#define SPI_RAW_M		BITFIELD_MASK(1)	/* Bit [19] 	- Read After Wr */
+#define SPI_RAW_S		19
+#define SPI_STUFF_M		BITFIELD_MASK(1)	/* Bit [18] 	- Stuff bit */
+#define SPI_STUFF_S		18
+#define SPI_BLKMODE_M		BITFIELD_MASK(1)	/* Bit [19] 	- Blockmode 1=blk */
+#define SPI_BLKMODE_S		19
+#define SPI_OPCODE_M		BITFIELD_MASK(1)	/* Bit [18] 	- OP Code */
+#define SPI_OPCODE_S		18
+#define SPI_ADDR_M		BITFIELD_MASK(17)	/* Bits [17:1] 	- Address */
+#define SPI_ADDR_S		1
+#define SPI_STUFF0_M		BITFIELD_MASK(1)	/* Bit [0] 	- Stuff bit */
+#define SPI_STUFF0_S		0
+
+#define SPI_RSP_START_M		BITFIELD_MASK(1)	/* Bit [7] 	- Start Bit (always 0) */
+#define SPI_RSP_START_S		7
+#define SPI_RSP_PARAM_ERR_M	BITFIELD_MASK(1)	/* Bit [6] 	- Parameter Error */
+#define SPI_RSP_PARAM_ERR_S	6
+#define SPI_RSP_RFU5_M		BITFIELD_MASK(1)	/* Bit [5] 	- RFU (Always 0) */
+#define SPI_RSP_RFU5_S		5
+#define SPI_RSP_FUNC_ERR_M	BITFIELD_MASK(1)	/* Bit [4] 	- Function number error */
+#define SPI_RSP_FUNC_ERR_S	4
+#define SPI_RSP_CRC_ERR_M	BITFIELD_MASK(1)	/* Bit [3] 	- COM CRC Error */
+#define SPI_RSP_CRC_ERR_S	3
+#define SPI_RSP_ILL_CMD_M	BITFIELD_MASK(1)	/* Bit [2] 	- Illegal Command error */
+#define SPI_RSP_ILL_CMD_S	2
+#define SPI_RSP_RFU1_M		BITFIELD_MASK(1)	/* Bit [1] 	- RFU (Always 0) */
+#define SPI_RSP_RFU1_S		1
+#define SPI_RSP_IDLE_M		BITFIELD_MASK(1)	/* Bit [0] 	- In idle state */
+#define SPI_RSP_IDLE_S		0
+
+/* SD-SPI Protocol Definitions */
+#define SDSPI_COMMAND_LEN	6	/* Number of bytes in an SD command */
+#define SDSPI_START_BLOCK	0xFE	/* SD Start Block Token */
+#define SDSPI_IDLE_PAD		0xFF	/* SD-SPI idle value for MOSI */
+#define SDSPI_START_BIT_MASK	0x80
diff --git a/drivers/net/wireless/bcm4329/include/proto/vlan.h b/drivers/net/wireless/bcm4329/include/proto/vlan.h
new file mode 100644
index 000000000000..670bc44c6bd6
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/proto/vlan.h
@@ -0,0 +1,63 @@
+/*
+ * 802.1Q VLAN protocol definitions
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: vlan.h,v 9.4.196.2 2008/12/07 21:19:20 Exp $
+ */
+
+
+#ifndef _vlan_h_
+#define _vlan_h_
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+
+#include <packed_section_start.h>
+
+#define VLAN_VID_MASK		0xfff	
+#define	VLAN_CFI_SHIFT		12	
+#define VLAN_PRI_SHIFT		13	
+
+#define VLAN_PRI_MASK		7	
+
+#define	VLAN_TAG_LEN		4
+#define	VLAN_TAG_OFFSET		(2 * ETHER_ADDR_LEN)	
+
+#define VLAN_TPID		0x8100	
+
+struct ethervlan_header {
+	uint8	ether_dhost[ETHER_ADDR_LEN];
+	uint8	ether_shost[ETHER_ADDR_LEN];
+	uint16	vlan_type;		
+	uint16	vlan_tag;		
+	uint16	ether_type;
+};
+
+#define	ETHERVLAN_HDR_LEN	(ETHER_HDR_LEN + VLAN_TAG_LEN)
+
+
+
+#include <packed_section_end.h>
+
+#endif 
diff --git a/drivers/net/wireless/bcm4329/include/proto/wpa.h b/drivers/net/wireless/bcm4329/include/proto/wpa.h
new file mode 100644
index 000000000000..f5d0cd539777
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/proto/wpa.h
@@ -0,0 +1,159 @@
+/*
+ * Fundamental types and constants relating to WPA
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wpa.h,v 1.16.166.1.20.1 2008/11/20 00:51:31 Exp $
+ */
+
+
+#ifndef _proto_wpa_h_
+#define _proto_wpa_h_
+
+#include <typedefs.h>
+#include <proto/ethernet.h>
+
+
+
+#include <packed_section_start.h>
+
+
+
+
+#define DOT11_RC_INVALID_WPA_IE		13	
+#define DOT11_RC_MIC_FAILURE		14	
+#define DOT11_RC_4WH_TIMEOUT		15	
+#define DOT11_RC_GTK_UPDATE_TIMEOUT	16	
+#define DOT11_RC_WPA_IE_MISMATCH	17	
+#define DOT11_RC_INVALID_MC_CIPHER	18	
+#define DOT11_RC_INVALID_UC_CIPHER	19	
+#define DOT11_RC_INVALID_AKMP		20	
+#define DOT11_RC_BAD_WPA_VERSION	21	
+#define DOT11_RC_INVALID_WPA_CAP	22	
+#define DOT11_RC_8021X_AUTH_FAIL	23	
+
+#define WPA2_PMKID_LEN	16
+
+
+typedef BWL_PRE_PACKED_STRUCT struct
+{
+	uint8 tag;	
+	uint8 length;	
+	uint8 oui[3];	
+	uint8 oui_type;	
+	BWL_PRE_PACKED_STRUCT struct {
+		uint8 low;
+		uint8 high;
+	} BWL_POST_PACKED_STRUCT version;	
+} BWL_POST_PACKED_STRUCT wpa_ie_fixed_t;
+#define WPA_IE_OUITYPE_LEN	4
+#define WPA_IE_FIXED_LEN	8
+#define WPA_IE_TAG_FIXED_LEN	6
+
+typedef BWL_PRE_PACKED_STRUCT struct {
+	uint8 tag;	
+	uint8 length;	
+	BWL_PRE_PACKED_STRUCT struct {
+		uint8 low;
+		uint8 high;
+	} BWL_POST_PACKED_STRUCT version;	
+} BWL_POST_PACKED_STRUCT wpa_rsn_ie_fixed_t;
+#define WPA_RSN_IE_FIXED_LEN	4
+#define WPA_RSN_IE_TAG_FIXED_LEN	2
+typedef uint8 wpa_pmkid_t[WPA2_PMKID_LEN];
+
+
+typedef BWL_PRE_PACKED_STRUCT struct
+{
+	uint8 oui[3];
+	uint8 type;
+} BWL_POST_PACKED_STRUCT wpa_suite_t, wpa_suite_mcast_t;
+#define WPA_SUITE_LEN	4
+
+
+typedef BWL_PRE_PACKED_STRUCT struct
+{
+	BWL_PRE_PACKED_STRUCT struct {
+		uint8 low;
+		uint8 high;
+	} BWL_POST_PACKED_STRUCT count;
+	wpa_suite_t list[1];
+} BWL_POST_PACKED_STRUCT wpa_suite_ucast_t, wpa_suite_auth_key_mgmt_t;
+#define WPA_IE_SUITE_COUNT_LEN	2
+typedef BWL_PRE_PACKED_STRUCT struct
+{
+	BWL_PRE_PACKED_STRUCT struct {
+		uint8 low;
+		uint8 high;
+	} BWL_POST_PACKED_STRUCT count;
+	wpa_pmkid_t list[1];
+} BWL_POST_PACKED_STRUCT wpa_pmkid_list_t;
+
+
+#define WPA_CIPHER_NONE		0	
+#define WPA_CIPHER_WEP_40	1	
+#define WPA_CIPHER_TKIP		2	
+#define WPA_CIPHER_AES_OCB	3	
+#define WPA_CIPHER_AES_CCM	4	
+#define WPA_CIPHER_WEP_104	5	
+
+#define IS_WPA_CIPHER(cipher)	((cipher) == WPA_CIPHER_NONE || \
+				 (cipher) == WPA_CIPHER_WEP_40 || \
+				 (cipher) == WPA_CIPHER_WEP_104 || \
+				 (cipher) == WPA_CIPHER_TKIP || \
+				 (cipher) == WPA_CIPHER_AES_OCB || \
+				 (cipher) == WPA_CIPHER_AES_CCM)
+
+
+#define WPA_TKIP_CM_DETECT	60	
+#define WPA_TKIP_CM_BLOCK	60	
+
+
+#define RSN_CAP_LEN		2	
+
+
+#define RSN_CAP_PREAUTH			0x0001
+#define RSN_CAP_NOPAIRWISE		0x0002
+#define RSN_CAP_PTK_REPLAY_CNTR_MASK	0x000C
+#define RSN_CAP_PTK_REPLAY_CNTR_SHIFT	2
+#define RSN_CAP_GTK_REPLAY_CNTR_MASK	0x0030
+#define RSN_CAP_GTK_REPLAY_CNTR_SHIFT	4
+#define RSN_CAP_1_REPLAY_CNTR		0
+#define RSN_CAP_2_REPLAY_CNTRS		1
+#define RSN_CAP_4_REPLAY_CNTRS		2
+#define RSN_CAP_16_REPLAY_CNTRS		3
+
+
+#define WPA_CAP_4_REPLAY_CNTRS		RSN_CAP_4_REPLAY_CNTRS
+#define WPA_CAP_16_REPLAY_CNTRS		RSN_CAP_16_REPLAY_CNTRS
+#define WPA_CAP_REPLAY_CNTR_SHIFT	RSN_CAP_PTK_REPLAY_CNTR_SHIFT
+#define WPA_CAP_REPLAY_CNTR_MASK	RSN_CAP_PTK_REPLAY_CNTR_MASK
+
+
+#define WPA_CAP_LEN	RSN_CAP_LEN	
+
+#define	WPA_CAP_WPA2_PREAUTH		RSN_CAP_PREAUTH
+
+
+
+#include <packed_section_end.h>
+
+#endif 
diff --git a/drivers/net/wireless/bcm4329/include/sbchipc.h b/drivers/net/wireless/bcm4329/include/sbchipc.h
new file mode 100644
index 000000000000..39e5c8d6aed0
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/sbchipc.h
@@ -0,0 +1,1026 @@
+/*
+ * SiliconBackplane Chipcommon core hardware definitions.
+ *
+ * The chipcommon core provides chip identification, SB control,
+ * jtag, 0/1/2 uarts, clock frequency control, a watchdog interrupt timer,
+ * gpio interface, extbus, and support for serial and parallel flashes.
+ *
+ * $Id: sbchipc.h,v 13.103.2.5.4.5.2.9 2009/07/03 14:23:21 Exp $
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ */
+
+
+#ifndef	_SBCHIPC_H
+#define	_SBCHIPC_H
+
+#ifndef _LANGUAGE_ASSEMBLY
+
+
+#ifndef PAD
+#define	_PADLINE(line)	pad ## line
+#define	_XSTR(line)	_PADLINE(line)
+#define	PAD		_XSTR(__LINE__)
+#endif	
+
+typedef volatile struct {
+	uint32	chipid;			
+	uint32	capabilities;
+	uint32	corecontrol;		
+	uint32	bist;
+
+	
+	uint32	otpstatus;		
+	uint32	otpcontrol;
+	uint32	otpprog;
+	uint32	PAD;
+
+	
+	uint32	intstatus;		
+	uint32	intmask;
+	uint32	chipcontrol;		
+	uint32	chipstatus;		
+
+	
+	uint32	jtagcmd;		
+	uint32	jtagir;
+	uint32	jtagdr;
+	uint32	jtagctrl;
+
+	
+	uint32	flashcontrol;		
+	uint32	flashaddress;
+	uint32	flashdata;
+	uint32	PAD[1];
+
+	
+	uint32	broadcastaddress;	
+	uint32	broadcastdata;
+
+	
+	uint32	gpiopullup;		
+	uint32	gpiopulldown;		
+	uint32	gpioin;			
+	uint32	gpioout;
+	uint32	gpioouten;
+	uint32	gpiocontrol;
+	uint32	gpiointpolarity;
+	uint32	gpiointmask;
+
+	
+	uint32	gpioevent;
+	uint32	gpioeventintmask;
+
+	
+	uint32	watchdog;		
+
+	
+	uint32	gpioeventintpolarity;
+
+	
+	uint32  gpiotimerval;		
+	uint32  gpiotimeroutmask;
+
+	
+	uint32	clockcontrol_n;		
+	uint32	clockcontrol_sb;	
+	uint32	clockcontrol_pci;	
+	uint32	clockcontrol_m2;	
+	uint32	clockcontrol_m3;	
+	uint32	clkdiv;			
+	uint32	PAD[2];
+
+	
+	uint32	pll_on_delay;		
+	uint32	fref_sel_delay;
+	uint32	slow_clk_ctl;		
+	uint32	PAD[1];
+
+	
+	uint32	system_clk_ctl;		
+	uint32	clkstatestretch;
+	uint32	PAD[13];
+
+	
+	uint32	eromptr;
+
+	
+	uint32	pcmcia_config;		
+	uint32	pcmcia_memwait;
+	uint32	pcmcia_attrwait;
+	uint32	pcmcia_iowait;
+	uint32	ide_config;
+	uint32	ide_memwait;
+	uint32	ide_attrwait;
+	uint32	ide_iowait;
+	uint32	prog_config;
+	uint32	prog_waitcount;
+	uint32	flash_config;
+	uint32	flash_waitcount;
+	uint32	PAD[4];
+	uint32	PAD[40];
+
+
+	
+	uint32	clk_ctl_st;		
+	uint32	hw_war;
+	uint32	PAD[70];
+
+	
+	uint8	uart0data;		
+	uint8	uart0imr;
+	uint8	uart0fcr;
+	uint8	uart0lcr;
+	uint8	uart0mcr;
+	uint8	uart0lsr;
+	uint8	uart0msr;
+	uint8	uart0scratch;
+	uint8	PAD[248];		
+
+	uint8	uart1data;		
+	uint8	uart1imr;
+	uint8	uart1fcr;
+	uint8	uart1lcr;
+	uint8	uart1mcr;
+	uint8	uart1lsr;
+	uint8	uart1msr;
+	uint8	uart1scratch;
+	uint32	PAD[126];
+
+	
+	uint32	pmucontrol;		
+	uint32	pmucapabilities;
+	uint32	pmustatus;
+	uint32	res_state;
+	uint32	res_pending;
+	uint32	pmutimer;
+	uint32	min_res_mask;
+	uint32	max_res_mask;
+	uint32	res_table_sel;
+	uint32	res_dep_mask;
+	uint32	res_updn_timer;
+	uint32	res_timer;
+	uint32	clkstretch;
+	uint32	pmuwatchdog;
+	uint32	gpiosel;		
+	uint32	gpioenable;		
+	uint32	res_req_timer_sel;
+	uint32	res_req_timer;
+	uint32	res_req_mask;
+	uint32	PAD;
+	uint32	chipcontrol_addr;	
+	uint32	chipcontrol_data;	
+	uint32	regcontrol_addr;
+	uint32	regcontrol_data;
+	uint32	pllcontrol_addr;
+	uint32	pllcontrol_data;
+	uint32	PAD[102];
+	uint16	otp[768];
+} chipcregs_t;
+
+#endif 
+
+#define	CC_CHIPID		0
+#define	CC_CAPABILITIES		4
+#define CC_OTPST		0x10
+#define CC_CHIPST		0x2c
+#define	CC_JTAGCMD		0x30
+#define	CC_JTAGIR		0x34
+#define	CC_JTAGDR		0x38
+#define	CC_JTAGCTRL		0x3c
+#define	CC_WATCHDOG		0x80
+#define	CC_CLKC_N		0x90
+#define	CC_CLKC_M0		0x94
+#define	CC_CLKC_M1		0x98
+#define	CC_CLKC_M2		0x9c
+#define	CC_CLKC_M3		0xa0
+#define	CC_CLKDIV		0xa4
+#define	CC_SYS_CLK_CTL		0xc0
+#define	CC_CLK_CTL_ST		SI_CLK_CTL_ST
+#define	CC_EROMPTR		0xfc
+#define	PMU_CTL			0x600
+#define	PMU_CAP			0x604
+#define	PMU_ST			0x608
+#define PMU_RES_STATE		0x60c
+#define PMU_TIMER		0x614
+#define	PMU_MIN_RES_MASK	0x618
+#define	PMU_MAX_RES_MASK	0x61c
+#define PMU_REG_CONTROL_ADDR	0x658
+#define PMU_REG_CONTROL_DATA	0x65C
+#define PMU_PLL_CONTROL_ADDR 	0x660
+#define PMU_PLL_CONTROL_DATA 	0x664
+#define	CC_OTP			0x800		
+
+
+#define	CID_ID_MASK		0x0000ffff	
+#define	CID_REV_MASK		0x000f0000	
+#define	CID_REV_SHIFT		16		
+#define	CID_PKG_MASK		0x00f00000	
+#define	CID_PKG_SHIFT		20		
+#define	CID_CC_MASK		0x0f000000	
+#define CID_CC_SHIFT		24
+#define	CID_TYPE_MASK		0xf0000000	
+#define CID_TYPE_SHIFT		28
+
+
+#define	CC_CAP_UARTS_MASK	0x00000003	
+#define CC_CAP_MIPSEB		0x00000004	
+#define CC_CAP_UCLKSEL		0x00000018	
+#define CC_CAP_UINTCLK		0x00000008	
+#define CC_CAP_UARTGPIO		0x00000020	
+#define CC_CAP_EXTBUS_MASK	0x000000c0	
+#define CC_CAP_EXTBUS_NONE	0x00000000	
+#define CC_CAP_EXTBUS_FULL	0x00000040	
+#define CC_CAP_EXTBUS_PROG	0x00000080	
+#define	CC_CAP_FLASH_MASK	0x00000700	
+#define	CC_CAP_PLL_MASK		0x00038000	
+#define CC_CAP_PWR_CTL		0x00040000	
+#define CC_CAP_OTPSIZE		0x00380000	
+#define CC_CAP_OTPSIZE_SHIFT	19		
+#define CC_CAP_OTPSIZE_BASE	5		
+#define CC_CAP_JTAGP		0x00400000	
+#define CC_CAP_ROM		0x00800000	
+#define CC_CAP_BKPLN64		0x08000000	
+#define	CC_CAP_PMU		0x10000000	
+#define	CC_CAP_ECI		0x20000000	
+
+
+#define PLL_NONE		0x00000000
+#define PLL_TYPE1		0x00010000	
+#define PLL_TYPE2		0x00020000	
+#define PLL_TYPE3		0x00030000	
+#define PLL_TYPE4		0x00008000	
+#define PLL_TYPE5		0x00018000	
+#define PLL_TYPE6		0x00028000	
+#define PLL_TYPE7		0x00038000	
+
+
+#define	ILP_CLOCK		32000
+
+
+#define	ALP_CLOCK		20000000
+
+
+#define	HT_CLOCK		80000000
+
+
+#define CC_UARTCLKO		0x00000001	
+#define	CC_SE			0x00000002	
+#define CC_UARTCLKEN		0x00000008	
+
+
+#define CHIPCTRL_4321A0_DEFAULT	0x3a4
+#define CHIPCTRL_4321A1_DEFAULT	0x0a4
+#define CHIPCTRL_4321_PLL_DOWN	0x800000	
+
+
+#define OTPS_OL_MASK		0x000000ff
+#define OTPS_OL_MFG		0x00000001	
+#define OTPS_OL_OR1		0x00000002	
+#define OTPS_OL_OR2		0x00000004	
+#define OTPS_OL_GU		0x00000008	
+#define OTPS_GUP_MASK		0x00000f00
+#define OTPS_GUP_SHIFT		8
+#define OTPS_GUP_HW		0x00000100	
+#define OTPS_GUP_SW		0x00000200	
+#define OTPS_GUP_CI		0x00000400	
+#define OTPS_GUP_FUSE		0x00000800	
+#define OTPS_READY		0x00001000
+#define OTPS_RV(x)		(1 << (16 + (x)))	
+#define OTPS_RV_MASK		0x0fff0000
+
+
+#define OTPC_PROGSEL		0x00000001
+#define OTPC_PCOUNT_MASK	0x0000000e
+#define OTPC_PCOUNT_SHIFT	1
+#define OTPC_VSEL_MASK		0x000000f0
+#define OTPC_VSEL_SHIFT		4
+#define OTPC_TMM_MASK		0x00000700
+#define OTPC_TMM_SHIFT		8
+#define OTPC_ODM		0x00000800
+#define OTPC_PROGEN		0x80000000
+
+
+#define OTPP_COL_MASK		0x000000ff
+#define OTPP_COL_SHIFT		0
+#define OTPP_ROW_MASK		0x0000ff00
+#define OTPP_ROW_SHIFT		8
+#define OTPP_OC_MASK		0x0f000000
+#define OTPP_OC_SHIFT		24
+#define OTPP_READERR		0x10000000
+#define OTPP_VALUE_MASK		0x20000000
+#define OTPP_VALUE_SHIFT	29
+#define OTPP_START_BUSY		0x80000000
+
+
+#define OTPPOC_READ		0
+#define OTPPOC_BIT_PROG		1
+#define OTPPOC_VERIFY		3
+#define OTPPOC_INIT		4
+#define OTPPOC_SET		5
+#define OTPPOC_RESET		6
+#define OTPPOC_OCST		7
+#define OTPPOC_ROW_LOCK		8
+#define OTPPOC_PRESCN_TEST	9
+
+
+#define JCMD_START		0x80000000
+#define JCMD_BUSY		0x80000000
+#define JCMD_STATE_MASK		0x60000000
+#define JCMD_STATE_TLR		0x00000000	
+#define JCMD_STATE_PIR		0x20000000	
+#define JCMD_STATE_PDR		0x40000000	
+#define JCMD_STATE_RTI		0x60000000	
+#define JCMD0_ACC_MASK		0x0000f000
+#define JCMD0_ACC_IRDR		0x00000000
+#define JCMD0_ACC_DR		0x00001000
+#define JCMD0_ACC_IR		0x00002000
+#define JCMD0_ACC_RESET		0x00003000
+#define JCMD0_ACC_IRPDR		0x00004000
+#define JCMD0_ACC_PDR		0x00005000
+#define JCMD0_IRW_MASK		0x00000f00
+#define JCMD_ACC_MASK		0x000f0000	
+#define JCMD_ACC_IRDR		0x00000000
+#define JCMD_ACC_DR		0x00010000
+#define JCMD_ACC_IR		0x00020000
+#define JCMD_ACC_RESET		0x00030000
+#define JCMD_ACC_IRPDR		0x00040000
+#define JCMD_ACC_PDR		0x00050000
+#define JCMD_ACC_PIR		0x00060000
+#define JCMD_ACC_IRDR_I		0x00070000	
+#define JCMD_ACC_DR_I		0x00080000	
+#define JCMD_IRW_MASK		0x00001f00
+#define JCMD_IRW_SHIFT		8
+#define JCMD_DRW_MASK		0x0000003f
+
+
+#define JCTRL_FORCE_CLK		4		
+#define JCTRL_EXT_EN		2		
+#define JCTRL_EN		1		
+
+
+#define	CLKD_SFLASH		0x0f000000
+#define	CLKD_SFLASH_SHIFT	24
+#define	CLKD_OTP		0x000f0000
+#define	CLKD_OTP_SHIFT		16
+#define	CLKD_JTAG		0x00000f00
+#define	CLKD_JTAG_SHIFT		8
+#define	CLKD_UART		0x000000ff
+
+
+#define	CI_GPIO			0x00000001	
+#define	CI_EI			0x00000002	
+#define	CI_TEMP			0x00000004	
+#define	CI_SIRQ			0x00000008	
+#define	CI_ECI			0x00000010	
+#define	CI_PMU			0x00000020	
+#define	CI_UART			0x00000040	
+#define	CI_WDRESET		0x80000000	
+
+
+#define SCC_SS_MASK		0x00000007	
+#define	SCC_SS_LPO		0x00000000	
+#define	SCC_SS_XTAL		0x00000001	
+#define	SCC_SS_PCI		0x00000002	
+#define SCC_LF			0x00000200	
+#define SCC_LP			0x00000400	
+#define SCC_FS			0x00000800	
+#define SCC_IP			0x00001000	
+#define SCC_XC			0x00002000	
+#define SCC_XP			0x00004000	
+#define SCC_CD_MASK		0xffff0000	
+#define SCC_CD_SHIFT		16
+
+
+#define	SYCC_IE			0x00000001	
+#define	SYCC_AE			0x00000002	
+#define	SYCC_FP			0x00000004	
+#define	SYCC_AR			0x00000008	
+#define	SYCC_HR			0x00000010	
+#define SYCC_CD_MASK		0xffff0000	
+#define SYCC_CD_SHIFT		16
+
+
+#define	CF_EN			0x00000001	
+#define	CF_EM_MASK		0x0000000e	
+#define	CF_EM_SHIFT		1
+#define	CF_EM_FLASH		0		
+#define	CF_EM_SYNC		2		
+#define	CF_EM_PCMCIA		4		
+#define	CF_DS			0x00000010	
+#define	CF_BS			0x00000020	
+#define	CF_CD_MASK		0x000000c0	
+#define	CF_CD_SHIFT		6
+#define	CF_CD_DIV2		0x00000000	
+#define	CF_CD_DIV3		0x00000040	
+#define	CF_CD_DIV4		0x00000080	
+#define	CF_CE			0x00000100	
+#define	CF_SB			0x00000200	
+
+
+#define	PM_W0_MASK		0x0000003f	
+#define	PM_W1_MASK		0x00001f00	
+#define	PM_W1_SHIFT		8
+#define	PM_W2_MASK		0x001f0000	
+#define	PM_W2_SHIFT		16
+#define	PM_W3_MASK		0x1f000000	
+#define	PM_W3_SHIFT		24
+
+
+#define	PA_W0_MASK		0x0000003f	
+#define	PA_W1_MASK		0x00001f00	
+#define	PA_W1_SHIFT		8
+#define	PA_W2_MASK		0x001f0000	
+#define	PA_W2_SHIFT		16
+#define	PA_W3_MASK		0x1f000000	
+#define	PA_W3_SHIFT		24
+
+
+#define	PI_W0_MASK		0x0000003f	
+#define	PI_W1_MASK		0x00001f00	
+#define	PI_W1_SHIFT		8
+#define	PI_W2_MASK		0x001f0000	
+#define	PI_W2_SHIFT		16
+#define	PI_W3_MASK		0x1f000000	
+#define	PI_W3_SHIFT		24
+
+
+#define	PW_W0_MASK		0x0000001f	
+#define	PW_W1_MASK		0x00001f00	
+#define	PW_W1_SHIFT		8
+#define	PW_W2_MASK		0x001f0000	
+#define	PW_W2_SHIFT		16
+#define	PW_W3_MASK		0x1f000000	
+#define	PW_W3_SHIFT		24
+
+#define PW_W0       		0x0000000c
+#define PW_W1       		0x00000a00
+#define PW_W2       		0x00020000
+#define PW_W3       		0x01000000
+
+
+#define	FW_W0_MASK		0x0000003f	
+#define	FW_W1_MASK		0x00001f00	
+#define	FW_W1_SHIFT		8
+#define	FW_W2_MASK		0x001f0000	
+#define	FW_W2_SHIFT		16
+#define	FW_W3_MASK		0x1f000000	
+#define	FW_W3_SHIFT		24
+
+
+#define WATCHDOG_CLOCK		48000000	
+#define WATCHDOG_CLOCK_5354 	32000		
+
+
+#define	PCTL_ILP_DIV_MASK	0xffff0000
+#define	PCTL_ILP_DIV_SHIFT	16
+#define PCTL_PLL_PLLCTL_UPD	0x00000400	
+#define PCTL_NOILP_ON_WAIT	0x00000200	
+#define	PCTL_HT_REQ_EN		0x00000100
+#define	PCTL_ALP_REQ_EN		0x00000080
+#define	PCTL_XTALFREQ_MASK	0x0000007c
+#define	PCTL_XTALFREQ_SHIFT	2
+#define	PCTL_ILP_DIV_EN		0x00000002
+#define	PCTL_LPO_SEL		0x00000001
+
+
+#define CSTRETCH_HT		0xffff0000
+#define CSTRETCH_ALP		0x0000ffff
+
+
+#define GPIO_ONTIME_SHIFT	16
+
+
+#define	CN_N1_MASK		0x3f		
+#define	CN_N2_MASK		0x3f00		
+#define	CN_N2_SHIFT		8
+#define	CN_PLLC_MASK		0xf0000		
+#define	CN_PLLC_SHIFT		16
+
+
+#define	CC_M1_MASK		0x3f		
+#define	CC_M2_MASK		0x3f00		
+#define	CC_M2_SHIFT		8
+#define	CC_M3_MASK		0x3f0000	
+#define	CC_M3_SHIFT		16
+#define	CC_MC_MASK		0x1f000000	
+#define	CC_MC_SHIFT		24
+
+
+#define	CC_F6_2			0x02		
+#define	CC_F6_3			0x03		
+#define	CC_F6_4			0x05		
+#define	CC_F6_5			0x09
+#define	CC_F6_6			0x11
+#define	CC_F6_7			0x21
+
+#define	CC_F5_BIAS		5		
+
+#define	CC_MC_BYPASS		0x08
+#define	CC_MC_M1		0x04
+#define	CC_MC_M1M2		0x02
+#define	CC_MC_M1M2M3		0x01
+#define	CC_MC_M1M3		0x11
+
+
+#define	CC_T2_BIAS		2		
+#define	CC_T2M2_BIAS		3		
+
+#define	CC_T2MC_M1BYP		1
+#define	CC_T2MC_M2BYP		2
+#define	CC_T2MC_M3BYP		4
+
+
+#define	CC_T6_MMASK		1		
+#define	CC_T6_M0		120000000	
+#define	CC_T6_M1		100000000	
+#define	SB2MIPS_T6(sb)		(2 * (sb))
+
+
+#define	CC_CLOCK_BASE1		24000000	
+#define CC_CLOCK_BASE2		12500000	
+
+
+#define	CLKC_5350_N		0x0311
+#define	CLKC_5350_M		0x04020009
+
+
+#define FLASH_NONE		0x000		
+#define SFLASH_ST		0x100		
+#define SFLASH_AT		0x200		
+#define	PFLASH			0x700		
+
+
+#define	CC_CFG_EN		0x0001		
+#define	CC_CFG_EM_MASK		0x000e		
+#define	CC_CFG_EM_ASYNC		0x0000		
+#define	CC_CFG_EM_SYNC		0x0002		
+#define	CC_CFG_EM_PCMCIA	0x0004		
+#define	CC_CFG_EM_IDE		0x0006		
+#define	CC_CFG_DS		0x0010		
+#define	CC_CFG_CD_MASK		0x00e0		
+#define	CC_CFG_CE		0x0100		
+#define	CC_CFG_SB		0x0200		
+#define	CC_CFG_IS		0x0400		
+
+
+#define	CC_EB_BASE		0x1a000000	
+#define	CC_EB_PCMCIA_MEM	0x1a000000	
+#define	CC_EB_PCMCIA_IO		0x1a200000	
+#define	CC_EB_PCMCIA_CFG	0x1a400000	
+#define	CC_EB_IDE		0x1a800000	
+#define	CC_EB_PCMCIA1_MEM	0x1a800000	
+#define	CC_EB_PCMCIA1_IO	0x1aa00000	
+#define	CC_EB_PCMCIA1_CFG	0x1ac00000	
+#define	CC_EB_PROGIF		0x1b000000	
+
+
+
+#define SFLASH_OPCODE		0x000000ff
+#define SFLASH_ACTION		0x00000700
+#define	SFLASH_CS_ACTIVE	0x00001000	
+#define SFLASH_START		0x80000000
+#define SFLASH_BUSY		SFLASH_START
+
+
+#define	SFLASH_ACT_OPONLY	0x0000		
+#define	SFLASH_ACT_OP1D		0x0100		
+#define	SFLASH_ACT_OP3A		0x0200		
+#define	SFLASH_ACT_OP3A1D	0x0300		
+#define	SFLASH_ACT_OP3A4D	0x0400		
+#define	SFLASH_ACT_OP3A4X4D	0x0500		
+#define	SFLASH_ACT_OP3A1X4D	0x0700		
+
+
+#define SFLASH_ST_WREN		0x0006		
+#define SFLASH_ST_WRDIS		0x0004		
+#define SFLASH_ST_RDSR		0x0105		
+#define SFLASH_ST_WRSR		0x0101		
+#define SFLASH_ST_READ		0x0303		
+#define SFLASH_ST_PP		0x0302		
+#define SFLASH_ST_SE		0x02d8		
+#define SFLASH_ST_BE		0x00c7		
+#define SFLASH_ST_DP		0x00b9		
+#define SFLASH_ST_RES		0x03ab		
+#define SFLASH_ST_CSA		0x1000		
+
+
+#define SFLASH_ST_WIP		0x01		
+#define SFLASH_ST_WEL		0x02		
+#define SFLASH_ST_BP_MASK	0x1c		
+#define SFLASH_ST_BP_SHIFT	2
+#define SFLASH_ST_SRWD		0x80		
+
+
+#define SFLASH_AT_READ				0x07e8
+#define SFLASH_AT_PAGE_READ			0x07d2
+#define SFLASH_AT_BUF1_READ
+#define SFLASH_AT_BUF2_READ
+#define SFLASH_AT_STATUS			0x01d7
+#define SFLASH_AT_BUF1_WRITE			0x0384
+#define SFLASH_AT_BUF2_WRITE			0x0387
+#define SFLASH_AT_BUF1_ERASE_PROGRAM		0x0283
+#define SFLASH_AT_BUF2_ERASE_PROGRAM		0x0286
+#define SFLASH_AT_BUF1_PROGRAM			0x0288
+#define SFLASH_AT_BUF2_PROGRAM			0x0289
+#define SFLASH_AT_PAGE_ERASE			0x0281
+#define SFLASH_AT_BLOCK_ERASE			0x0250
+#define SFLASH_AT_BUF1_WRITE_ERASE_PROGRAM	0x0382
+#define SFLASH_AT_BUF2_WRITE_ERASE_PROGRAM	0x0385
+#define SFLASH_AT_BUF1_LOAD			0x0253
+#define SFLASH_AT_BUF2_LOAD			0x0255
+#define SFLASH_AT_BUF1_COMPARE			0x0260
+#define SFLASH_AT_BUF2_COMPARE			0x0261
+#define SFLASH_AT_BUF1_REPROGRAM		0x0258
+#define SFLASH_AT_BUF2_REPROGRAM		0x0259
+
+
+#define SFLASH_AT_READY				0x80
+#define SFLASH_AT_MISMATCH			0x40
+#define SFLASH_AT_ID_MASK			0x38
+#define SFLASH_AT_ID_SHIFT			3
+
+
+
+#define UART_RX		0	
+#define UART_TX		0	
+#define UART_DLL	0	
+#define UART_IER	1	
+#define UART_DLM	1	
+#define UART_IIR	2	
+#define UART_FCR	2	
+#define UART_LCR	3	
+#define UART_MCR	4	
+#define UART_LSR	5	
+#define UART_MSR	6	
+#define UART_SCR	7	
+#define UART_LCR_DLAB	0x80	
+#define UART_LCR_WLEN8	0x03	
+#define UART_MCR_OUT2	0x08	
+#define UART_MCR_LOOP	0x10	
+#define UART_LSR_RX_FIFO 	0x80	
+#define UART_LSR_TDHR		0x40	
+#define UART_LSR_THRE		0x20	
+#define UART_LSR_BREAK		0x10	
+#define UART_LSR_FRAMING	0x08	
+#define UART_LSR_PARITY		0x04	
+#define UART_LSR_OVERRUN	0x02	
+#define UART_LSR_RXRDY		0x01	
+#define UART_FCR_FIFO_ENABLE 1	
+
+
+#define UART_IIR_FIFO_MASK	0xc0	
+#define UART_IIR_INT_MASK	0xf	
+#define UART_IIR_MDM_CHG	0x0	
+#define UART_IIR_NOINT		0x1	
+#define UART_IIR_THRE		0x2	
+#define UART_IIR_RCVD_DATA	0x4	
+#define UART_IIR_RCVR_STATUS 	0x6	
+#define UART_IIR_CHAR_TIME 	0xc	
+
+
+#define UART_IER_EDSSI	8	
+#define UART_IER_ELSI	4	
+#define UART_IER_ETBEI  2	
+#define UART_IER_ERBFI	1	
+
+
+#define	PST_INTPEND	0x0040
+#define	PST_SBCLKST	0x0030
+#define	PST_SBCLKST_ILP	0x0010
+#define	PST_SBCLKST_ALP	0x0020
+#define	PST_SBCLKST_HT	0x0030
+#define	PST_ALPAVAIL	0x0008
+#define	PST_HTAVAIL	0x0004
+#define	PST_RESINIT	0x0003
+
+
+#define PCAP_REV_MASK	0x000000ff
+#define PCAP_RC_MASK	0x00001f00
+#define PCAP_RC_SHIFT	8
+#define PCAP_TC_MASK	0x0001e000
+#define PCAP_TC_SHIFT	13
+#define PCAP_PC_MASK	0x001e0000
+#define PCAP_PC_SHIFT	17
+#define PCAP_VC_MASK	0x01e00000
+#define PCAP_VC_SHIFT	21
+#define PCAP_CC_MASK	0x1e000000
+#define PCAP_CC_SHIFT	25
+#define PCAP5_PC_MASK	0x003e0000	
+#define PCAP5_PC_SHIFT	17
+#define PCAP5_VC_MASK	0x07c00000
+#define PCAP5_VC_SHIFT	22
+#define PCAP5_CC_MASK	0xf8000000
+#define PCAP5_CC_SHIFT	27
+
+
+
+#define	PRRT_TIME_MASK	0x03ff
+#define	PRRT_INTEN	0x0400
+#define	PRRT_REQ_ACTIVE	0x0800
+#define	PRRT_ALP_REQ	0x1000
+#define	PRRT_HT_REQ	0x2000
+
+
+#define PMURES_BIT(bit)	(1 << (bit))
+
+
+#define PMURES_MAX_RESNUM	30
+
+
+
+
+#define	PMU0_PLL0_PLLCTL0		0
+#define	PMU0_PLL0_PC0_PDIV_MASK		1
+#define	PMU0_PLL0_PC0_PDIV_FREQ		25000
+#define PMU0_PLL0_PC0_DIV_ARM_MASK	0x00000038
+#define PMU0_PLL0_PC0_DIV_ARM_SHIFT	3
+#define PMU0_PLL0_PC0_DIV_ARM_BASE	8
+
+
+#define PMU0_PLL0_PC0_DIV_ARM_110MHZ	0
+#define PMU0_PLL0_PC0_DIV_ARM_97_7MHZ	1
+#define PMU0_PLL0_PC0_DIV_ARM_88MHZ	2
+#define PMU0_PLL0_PC0_DIV_ARM_80MHZ	3 
+#define PMU0_PLL0_PC0_DIV_ARM_73_3MHZ	4
+#define PMU0_PLL0_PC0_DIV_ARM_67_7MHZ	5
+#define PMU0_PLL0_PC0_DIV_ARM_62_9MHZ	6
+#define PMU0_PLL0_PC0_DIV_ARM_58_6MHZ	7
+
+
+#define	PMU0_PLL0_PLLCTL1		1
+#define	PMU0_PLL0_PC1_WILD_INT_MASK	0xf0000000
+#define	PMU0_PLL0_PC1_WILD_INT_SHIFT	28
+#define	PMU0_PLL0_PC1_WILD_FRAC_MASK	0x0fffff00
+#define	PMU0_PLL0_PC1_WILD_FRAC_SHIFT	8
+#define	PMU0_PLL0_PC1_STOP_MOD		0x00000040
+
+
+#define	PMU0_PLL0_PLLCTL2		2
+#define	PMU0_PLL0_PC2_WILD_INT_MASK	0xf
+#define	PMU0_PLL0_PC2_WILD_INT_SHIFT	4
+
+
+#define RES4328_EXT_SWITCHER_PWM	0	
+#define RES4328_BB_SWITCHER_PWM		1	
+#define RES4328_BB_SWITCHER_BURST	2	
+#define RES4328_BB_EXT_SWITCHER_BURST	3	
+#define RES4328_ILP_REQUEST		4	
+#define RES4328_RADIO_SWITCHER_PWM	5	
+#define RES4328_RADIO_SWITCHER_BURST	6	
+#define RES4328_ROM_SWITCH		7	
+#define RES4328_PA_REF_LDO		8	
+#define RES4328_RADIO_LDO		9	
+#define RES4328_AFE_LDO			10	
+#define RES4328_PLL_LDO			11	
+#define RES4328_BG_FILTBYP		12	
+#define RES4328_TX_FILTBYP		13	
+#define RES4328_RX_FILTBYP		14	
+#define RES4328_XTAL_PU			15	
+#define RES4328_XTAL_EN			16	
+#define RES4328_BB_PLL_FILTBYP		17	
+#define RES4328_RF_PLL_FILTBYP		18	
+#define RES4328_BB_PLL_PU		19	
+
+#define RES5354_EXT_SWITCHER_PWM	0	
+#define RES5354_BB_SWITCHER_PWM		1	
+#define RES5354_BB_SWITCHER_BURST	2	
+#define RES5354_BB_EXT_SWITCHER_BURST	3	
+#define RES5354_ILP_REQUEST		4	
+#define RES5354_RADIO_SWITCHER_PWM	5	
+#define RES5354_RADIO_SWITCHER_BURST	6	
+#define RES5354_ROM_SWITCH		7	
+#define RES5354_PA_REF_LDO		8	
+#define RES5354_RADIO_LDO		9	
+#define RES5354_AFE_LDO			10	
+#define RES5354_PLL_LDO			11	
+#define RES5354_BG_FILTBYP		12	
+#define RES5354_TX_FILTBYP		13	
+#define RES5354_RX_FILTBYP		14	
+#define RES5354_XTAL_PU			15	
+#define RES5354_XTAL_EN			16	
+#define RES5354_BB_PLL_FILTBYP		17	
+#define RES5354_RF_PLL_FILTBYP		18	
+#define RES5354_BB_PLL_PU		19	
+
+
+
+#define DOT11MAC_880MHZ_CLK_DIVISOR_SHIFT 8
+#define DOT11MAC_880MHZ_CLK_DIVISOR_MASK (0xFF << DOT11MAC_880MHZ_CLK_DIVISOR_SHIFT)
+#define DOT11MAC_880MHZ_CLK_DIVISOR_VAL  (0xE << DOT11MAC_880MHZ_CLK_DIVISOR_SHIFT)
+
+
+#define PMU2_PHY_PLL_PLLCTL		4
+#define PMU2_SI_PLL_PLLCTL		10
+
+
+#define RES4325_BUCK_BOOST_BURST	0	
+#define RES4325_CBUCK_BURST		1	
+#define RES4325_CBUCK_PWM		2	
+#define RES4325_CLDO_CBUCK_BURST	3	
+#define RES4325_CLDO_CBUCK_PWM		4	
+#define RES4325_BUCK_BOOST_PWM		5	
+#define RES4325_ILP_REQUEST		6	
+#define RES4325_ABUCK_BURST		7	
+#define RES4325_ABUCK_PWM		8	
+#define RES4325_LNLDO1_PU		9	
+#define RES4325_OTP_PU			10	
+#define RES4325_LNLDO3_PU		11	
+#define RES4325_LNLDO4_PU		12	
+#define RES4325_XTAL_PU			13	
+#define RES4325_ALP_AVAIL		14	
+#define RES4325_RX_PWRSW_PU		15	
+#define RES4325_TX_PWRSW_PU		16	
+#define RES4325_RFPLL_PWRSW_PU		17	
+#define RES4325_LOGEN_PWRSW_PU		18	
+#define RES4325_AFE_PWRSW_PU		19	
+#define RES4325_BBPLL_PWRSW_PU		20	
+#define RES4325_HT_AVAIL		21	
+
+
+#define RES4325B0_CBUCK_LPOM		1	
+#define RES4325B0_CBUCK_BURST		2	
+#define RES4325B0_CBUCK_PWM		3	
+#define RES4325B0_CLDO_PU		4	
+
+
+#define RES4325C1_OTP_PWRSW_PU		10	
+#define RES4325C1_LNLDO2_PU		12	
+
+
+#define CST4325_SPROM_OTP_SEL_MASK	0x00000003
+#define CST4325_DEFCIS_SEL		0	
+#define CST4325_SPROM_SEL		1	
+#define CST4325_OTP_SEL			2	
+#define CST4325_OTP_PWRDN		3	
+#define CST4325_SDIO_USB_MODE_MASK	0x00000004
+#define CST4325_SDIO_USB_MODE_SHIFT	2
+#define CST4325_RCAL_VALID_MASK		0x00000008
+#define CST4325_RCAL_VALID_SHIFT	3
+#define CST4325_RCAL_VALUE_MASK		0x000001f0
+#define CST4325_RCAL_VALUE_SHIFT	4
+#define CST4325_PMUTOP_2B_MASK 		0x00000200	
+#define CST4325_PMUTOP_2B_SHIFT   	9
+
+#define RES4329_RESERVED0		0	
+#define RES4329_CBUCK_LPOM		1	
+#define RES4329_CBUCK_BURST		2	
+#define RES4329_CBUCK_PWM		3	
+#define RES4329_CLDO_PU			4	
+#define RES4329_PALDO_PU		5	
+#define RES4329_ILP_REQUEST		6	
+#define RES4329_RESERVED7		7	
+#define RES4329_RESERVED8		8	
+#define RES4329_LNLDO1_PU		9	
+#define RES4329_OTP_PU			10	
+#define RES4329_RESERVED11		11	
+#define RES4329_LNLDO2_PU		12	
+#define RES4329_XTAL_PU			13	
+#define RES4329_ALP_AVAIL		14	
+#define RES4329_RX_PWRSW_PU		15	
+#define RES4329_TX_PWRSW_PU		16	
+#define RES4329_RFPLL_PWRSW_PU		17	
+#define RES4329_LOGEN_PWRSW_PU		18	
+#define RES4329_AFE_PWRSW_PU		19	
+#define RES4329_BBPLL_PWRSW_PU		20	
+#define RES4329_HT_AVAIL		21	
+
+#define CST4329_SPROM_OTP_SEL_MASK	0x00000003
+#define CST4329_DEFCIS_SEL		0	
+#define CST4329_SPROM_SEL		1	
+#define CST4329_OTP_SEL			2	
+#define CST4329_OTP_PWRDN		3	
+#define CST4329_SPI_SDIO_MODE_MASK	0x00000004
+#define CST4329_SPI_SDIO_MODE_SHIFT	2
+
+
+#define RES4312_SWITCHER_BURST		0	
+#define RES4312_SWITCHER_PWM    	1	
+#define RES4312_PA_REF_LDO		2	
+#define RES4312_CORE_LDO_BURST		3	
+#define RES4312_CORE_LDO_PWM		4	
+#define RES4312_RADIO_LDO		5	
+#define RES4312_ILP_REQUEST		6	
+#define RES4312_BG_FILTBYP		7	
+#define RES4312_TX_FILTBYP		8	
+#define RES4312_RX_FILTBYP		9	
+#define RES4312_XTAL_PU			10	
+#define RES4312_ALP_AVAIL		11	
+#define RES4312_BB_PLL_FILTBYP		12	
+#define RES4312_RF_PLL_FILTBYP		13	
+#define RES4312_HT_AVAIL		14	
+
+#define RES4322_RF_LDO			0
+#define RES4322_ILP_REQUEST		1
+#define RES4322_XTAL_PU			2
+#define RES4322_ALP_AVAIL		3
+#define RES4322_SI_PLL_ON		4
+#define RES4322_HT_SI_AVAIL		5
+#define RES4322_PHY_PLL_ON		6
+#define RES4322_HT_PHY_AVAIL		7
+#define RES4322_OTP_PU			8
+
+
+#define CST4322_XTAL_FREQ_20_40MHZ	0x00000020
+#define CST4322_SPROM_OTP_SEL_MASK	0x000000c0
+#define CST4322_SPROM_OTP_SEL_SHIFT	6
+#define CST4322_NO_SPROM_OTP		0	
+#define CST4322_SPROM_PRESENT		1	
+#define CST4322_OTP_PRESENT		2	
+#define CST4322_PCI_OR_USB		0x00000100
+#define CST4322_BOOT_MASK		0x00000600
+#define CST4322_BOOT_SHIFT		9
+#define CST4322_BOOT_FROM_SRAM		0	
+#define CST4322_BOOT_FROM_ROM		1	
+#define CST4322_BOOT_FROM_FLASH		2	
+#define CST4322_BOOT_FROM_INVALID	3
+#define CST4322_ILP_DIV_EN		0x00000800
+#define CST4322_FLASH_TYPE_MASK		0x00001000
+#define CST4322_FLASH_TYPE_SHIFT	12
+#define CST4322_FLASH_TYPE_SHIFT_ST	0	
+#define CST4322_FLASH_TYPE_SHIFT_ATMEL	1	
+#define CST4322_ARM_TAP_SEL		0x00002000
+#define CST4322_RES_INIT_MODE_MASK	0x0000c000
+#define CST4322_RES_INIT_MODE_SHIFT	14
+#define CST4322_RES_INIT_MODE_ILPAVAIL	0	
+#define CST4322_RES_INIT_MODE_ILPREQ	1	
+#define CST4322_RES_INIT_MODE_ALPAVAIL	2	
+#define CST4322_RES_INIT_MODE_HTAVAIL	3	
+#define CST4322_PCIPLLCLK_GATING	0x00010000
+#define CST4322_CLK_SWITCH_PCI_TO_ALP	0x00020000
+#define CST4322_PCI_CARDBUS_MODE	0x00040000
+
+#define RES4315_CBUCK_LPOM		1	
+#define RES4315_CBUCK_BURST		2	
+#define RES4315_CBUCK_PWM		3	
+#define RES4315_CLDO_PU			4	
+#define RES4315_PALDO_PU		5	
+#define RES4315_ILP_REQUEST		6	
+#define RES4315_LNLDO1_PU		9	
+#define RES4315_OTP_PU			10	
+#define RES4315_LNLDO2_PU		12	
+#define RES4315_XTAL_PU			13	
+#define RES4315_ALP_AVAIL		14	
+#define RES4315_RX_PWRSW_PU		15	
+#define RES4315_TX_PWRSW_PU		16	
+#define RES4315_RFPLL_PWRSW_PU		17	
+#define RES4315_LOGEN_PWRSW_PU		18	
+#define RES4315_AFE_PWRSW_PU		19	
+#define RES4315_BBPLL_PWRSW_PU		20	
+#define RES4315_HT_AVAIL		21	
+
+#define CST4315_SPROM_OTP_SEL_MASK	0x00000003	
+#define CST4315_DEFCIS_SEL		0x00000000	
+#define CST4315_SPROM_SEL		0x00000001	
+#define CST4315_OTP_SEL			0x00000002	
+#define CST4315_OTP_PWRDN		0x00000003	
+#define CST4315_SDIO_MODE		0x00000004	
+#define CST4315_RCAL_VALID		0x00000008
+#define CST4315_RCAL_VALUE_MASK		0x000001f0
+#define CST4315_RCAL_VALUE_SHIFT	4
+#define CST4315_PALDO_EXTPNP		0x00000200	
+#define CST4315_CBUCK_MODE_MASK		0x00000c00
+#define CST4315_CBUCK_MODE_BURST	0x00000400
+#define CST4315_CBUCK_MODE_LPBURST	0x00000c00
+
+#define PMU_MAX_TRANSITION_DLY	15000
+
+
+#define PMURES_UP_TRANSITION	2
+
+
+
+
+
+#define ECI_BW_20   0x0
+#define ECI_BW_25   0x1
+#define ECI_BW_30   0x2
+#define ECI_BW_35   0x3
+#define ECI_BW_40   0x4
+#define ECI_BW_45   0x5
+#define ECI_BW_50   0x6
+#define ECI_BW_ALL  0x7
+
+
+#define WLAN_NUM_ANT1 TXANT_0
+#define WLAN_NUM_ANT2 TXANT_1
+
+#endif	
diff --git a/drivers/net/wireless/bcm4329/include/sbconfig.h b/drivers/net/wireless/bcm4329/include/sbconfig.h
new file mode 100644
index 000000000000..da18ccbe9ab8
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/sbconfig.h
@@ -0,0 +1,276 @@
+/*
+ * Broadcom SiliconBackplane hardware register definitions.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sbconfig.h,v 13.67.30.1 2008/05/07 20:17:27 Exp $
+ */
+
+
+#ifndef	_SBCONFIG_H
+#define	_SBCONFIG_H
+
+
+#ifndef PAD
+#define	_PADLINE(line)	pad ## line
+#define	_XSTR(line)	_PADLINE(line)
+#define	PAD		_XSTR(__LINE__)
+#endif
+
+
+#define SB_BUS_SIZE		0x10000		
+#define SB_BUS_BASE(b)		(SI_ENUM_BASE + (b) * SB_BUS_SIZE)
+#define	SB_BUS_MAXCORES		(SB_BUS_SIZE / SI_CORE_SIZE)	
+
+
+#define	SBCONFIGOFF		0xf00		
+#define	SBCONFIGSIZE		256		
+
+#define SBIPSFLAG		0x08
+#define SBTPSFLAG		0x18
+#define	SBTMERRLOGA		0x48		
+#define	SBTMERRLOG		0x50		
+#define SBADMATCH3		0x60
+#define SBADMATCH2		0x68
+#define SBADMATCH1		0x70
+#define SBIMSTATE		0x90
+#define SBINTVEC		0x94
+#define SBTMSTATELOW		0x98
+#define SBTMSTATEHIGH		0x9c
+#define SBBWA0			0xa0
+#define SBIMCONFIGLOW		0xa8
+#define SBIMCONFIGHIGH		0xac
+#define SBADMATCH0		0xb0
+#define SBTMCONFIGLOW		0xb8
+#define SBTMCONFIGHIGH		0xbc
+#define SBBCONFIG		0xc0
+#define SBBSTATE		0xc8
+#define SBACTCNFG		0xd8
+#define	SBFLAGST		0xe8
+#define SBIDLOW			0xf8
+#define SBIDHIGH		0xfc
+
+
+
+#define SBIMERRLOGA		0xea8
+#define SBIMERRLOG		0xeb0
+#define SBTMPORTCONNID0		0xed8
+#define SBTMPORTLOCK0		0xef8
+
+#ifndef _LANGUAGE_ASSEMBLY
+
+typedef volatile struct _sbconfig {
+	uint32	PAD[2];
+	uint32	sbipsflag;		
+	uint32	PAD[3];
+	uint32	sbtpsflag;		
+	uint32	PAD[11];
+	uint32	sbtmerrloga;		
+	uint32	PAD;
+	uint32	sbtmerrlog;		
+	uint32	PAD[3];
+	uint32	sbadmatch3;		
+	uint32	PAD;
+	uint32	sbadmatch2;		
+	uint32	PAD;
+	uint32	sbadmatch1;		
+	uint32	PAD[7];
+	uint32	sbimstate;		
+	uint32	sbintvec;		
+	uint32	sbtmstatelow;		
+	uint32	sbtmstatehigh;		
+	uint32	sbbwa0;			
+	uint32	PAD;
+	uint32	sbimconfiglow;		
+	uint32	sbimconfighigh;		
+	uint32	sbadmatch0;		
+	uint32	PAD;
+	uint32	sbtmconfiglow;		
+	uint32	sbtmconfighigh;		
+	uint32	sbbconfig;		
+	uint32	PAD;
+	uint32	sbbstate;		
+	uint32	PAD[3];
+	uint32	sbactcnfg;		
+	uint32	PAD[3];
+	uint32	sbflagst;		
+	uint32	PAD[3];
+	uint32	sbidlow;		
+	uint32	sbidhigh;		
+} sbconfig_t;
+
+#endif 
+
+
+#define	SBIPS_INT1_MASK		0x3f		
+#define	SBIPS_INT1_SHIFT	0
+#define	SBIPS_INT2_MASK		0x3f00		
+#define	SBIPS_INT2_SHIFT	8
+#define	SBIPS_INT3_MASK		0x3f0000	
+#define	SBIPS_INT3_SHIFT	16
+#define	SBIPS_INT4_MASK		0x3f000000	
+#define	SBIPS_INT4_SHIFT	24
+
+
+#define	SBTPS_NUM0_MASK		0x3f		
+#define	SBTPS_F0EN0		0x40		
+
+
+#define	SBTMEL_CM		0x00000007	
+#define	SBTMEL_CI		0x0000ff00	
+#define	SBTMEL_EC		0x0f000000	
+#define	SBTMEL_ME		0x80000000	
+
+
+#define	SBIM_PC			0xf		
+#define	SBIM_AP_MASK		0x30		
+#define	SBIM_AP_BOTH		0x00		
+#define	SBIM_AP_TS		0x10		
+#define	SBIM_AP_TK		0x20		
+#define	SBIM_AP_RSV		0x30		
+#define	SBIM_IBE		0x20000		
+#define	SBIM_TO			0x40000		
+#define	SBIM_BY			0x01800000	
+#define	SBIM_RJ			0x02000000	
+
+
+#define	SBTML_RESET		0x0001		
+#define	SBTML_REJ_MASK		0x0006		
+#define	SBTML_REJ		0x0002		
+#define	SBTML_TMPREJ		0x0004		
+
+#define	SBTML_SICF_SHIFT	16		
+
+
+#define	SBTMH_SERR		0x0001		
+#define	SBTMH_INT		0x0002		
+#define	SBTMH_BUSY		0x0004		
+#define	SBTMH_TO		0x0020		
+
+#define	SBTMH_SISF_SHIFT	16		
+
+
+#define	SBBWA_TAB0_MASK		0xffff		
+#define	SBBWA_TAB1_MASK		0xffff		
+#define	SBBWA_TAB1_SHIFT	16
+
+
+#define	SBIMCL_STO_MASK		0x7		
+#define	SBIMCL_RTO_MASK		0x70		
+#define	SBIMCL_RTO_SHIFT	4
+#define	SBIMCL_CID_MASK		0xff0000	
+#define	SBIMCL_CID_SHIFT	16
+
+
+#define	SBIMCH_IEM_MASK		0xc		
+#define	SBIMCH_TEM_MASK		0x30		
+#define	SBIMCH_TEM_SHIFT	4
+#define	SBIMCH_BEM_MASK		0xc0		
+#define	SBIMCH_BEM_SHIFT	6
+
+
+#define	SBAM_TYPE_MASK		0x3		
+#define	SBAM_AD64		0x4		
+#define	SBAM_ADINT0_MASK	0xf8		
+#define	SBAM_ADINT0_SHIFT	3
+#define	SBAM_ADINT1_MASK	0x1f8		
+#define	SBAM_ADINT1_SHIFT	3
+#define	SBAM_ADINT2_MASK	0x1f8		
+#define	SBAM_ADINT2_SHIFT	3
+#define	SBAM_ADEN		0x400		
+#define	SBAM_ADNEG		0x800		
+#define	SBAM_BASE0_MASK		0xffffff00	
+#define	SBAM_BASE0_SHIFT	8
+#define	SBAM_BASE1_MASK		0xfffff000	
+#define	SBAM_BASE1_SHIFT	12
+#define	SBAM_BASE2_MASK		0xffff0000	
+#define	SBAM_BASE2_SHIFT	16
+
+
+#define	SBTMCL_CD_MASK		0xff		
+#define	SBTMCL_CO_MASK		0xf800		
+#define	SBTMCL_CO_SHIFT		11
+#define	SBTMCL_IF_MASK		0xfc0000	
+#define	SBTMCL_IF_SHIFT		18
+#define	SBTMCL_IM_MASK		0x3000000	
+#define	SBTMCL_IM_SHIFT		24
+
+
+#define	SBTMCH_BM_MASK		0x3		
+#define	SBTMCH_RM_MASK		0x3		
+#define	SBTMCH_RM_SHIFT		2
+#define	SBTMCH_SM_MASK		0x30		
+#define	SBTMCH_SM_SHIFT		4
+#define	SBTMCH_EM_MASK		0x300		
+#define	SBTMCH_EM_SHIFT		8
+#define	SBTMCH_IM_MASK		0xc00		
+#define	SBTMCH_IM_SHIFT		10
+
+
+#define	SBBC_LAT_MASK		0x3		
+#define	SBBC_MAX0_MASK		0xf0000		
+#define	SBBC_MAX0_SHIFT		16
+#define	SBBC_MAX1_MASK		0xf00000	
+#define	SBBC_MAX1_SHIFT		20
+
+
+#define	SBBS_SRD		0x1		
+#define	SBBS_HRD		0x2		
+
+
+#define	SBIDL_CS_MASK		0x3		
+#define	SBIDL_AR_MASK		0x38		
+#define	SBIDL_AR_SHIFT		3
+#define	SBIDL_SYNCH		0x40		
+#define	SBIDL_INIT		0x80		
+#define	SBIDL_MINLAT_MASK	0xf00		
+#define	SBIDL_MINLAT_SHIFT	8
+#define	SBIDL_MAXLAT		0xf000		
+#define	SBIDL_MAXLAT_SHIFT	12
+#define	SBIDL_FIRST		0x10000		
+#define	SBIDL_CW_MASK		0xc0000		
+#define	SBIDL_CW_SHIFT		18
+#define	SBIDL_TP_MASK		0xf00000	
+#define	SBIDL_TP_SHIFT		20
+#define	SBIDL_IP_MASK		0xf000000	
+#define	SBIDL_IP_SHIFT		24
+#define	SBIDL_RV_MASK		0xf0000000	
+#define	SBIDL_RV_SHIFT		28
+#define	SBIDL_RV_2_2		0x00000000	
+#define	SBIDL_RV_2_3		0x10000000	
+
+
+#define	SBIDH_RC_MASK		0x000f		
+#define	SBIDH_RCE_MASK		0x7000		
+#define	SBIDH_RCE_SHIFT		8
+#define	SBCOREREV(sbidh) \
+	((((sbidh) & SBIDH_RCE_MASK) >> SBIDH_RCE_SHIFT) | ((sbidh) & SBIDH_RC_MASK))
+#define	SBIDH_CC_MASK		0x8ff0		
+#define	SBIDH_CC_SHIFT		4
+#define	SBIDH_VC_MASK		0xffff0000	
+#define	SBIDH_VC_SHIFT		16
+
+#define	SB_COMMIT		0xfd8		
+
+
+#define	SB_VEND_BCM		0x4243		
+
+#endif	
diff --git a/drivers/net/wireless/bcm4329/include/sbhnddma.h b/drivers/net/wireless/bcm4329/include/sbhnddma.h
new file mode 100644
index 000000000000..7681395f5b3b
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/sbhnddma.h
@@ -0,0 +1,294 @@
+/*
+ * Generic Broadcom Home Networking Division (HND) DMA engine HW interface
+ * This supports the following chips: BCM42xx, 44xx, 47xx .
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sbhnddma.h,v 13.11.250.5.16.1 2009/07/21 14:04:51 Exp $
+ */
+
+
+#ifndef	_sbhnddma_h_
+#define	_sbhnddma_h_
+
+
+
+
+
+
+
+typedef volatile struct {
+	uint32	control;		
+	uint32	addr;			
+	uint32	ptr;			
+	uint32	status;			
+} dma32regs_t;
+
+typedef volatile struct {
+	dma32regs_t	xmt;		
+	dma32regs_t	rcv;		
+} dma32regp_t;
+
+typedef volatile struct {	
+	uint32	fifoaddr;		
+	uint32	fifodatalow;		
+	uint32	fifodatahigh;		
+	uint32	pad;			
+} dma32diag_t;
+
+
+typedef volatile struct {
+	uint32	ctrl;		
+	uint32	addr;		
+} dma32dd_t;
+
+
+#define	D32RINGALIGN_BITS	12
+#define	D32MAXRINGSZ	(1 << D32RINGALIGN_BITS)
+#define	D32RINGALIGN	(1 << D32RINGALIGN_BITS)
+#define	D32MAXDD	(D32MAXRINGSZ / sizeof (dma32dd_t))
+
+
+#define	XC_XE		((uint32)1 << 0)	
+#define	XC_SE		((uint32)1 << 1)	
+#define	XC_LE		((uint32)1 << 2)	
+#define	XC_FL		((uint32)1 << 4)	
+#define	XC_PD		((uint32)1 << 11)	
+#define	XC_AE		((uint32)3 << 16)	
+#define	XC_AE_SHIFT	16
+
+
+#define	XP_LD_MASK	0xfff			
+
+
+#define	XS_CD_MASK	0x0fff			
+#define	XS_XS_MASK	0xf000			
+#define	XS_XS_SHIFT	12
+#define	XS_XS_DISABLED	0x0000			
+#define	XS_XS_ACTIVE	0x1000			
+#define	XS_XS_IDLE	0x2000			
+#define	XS_XS_STOPPED	0x3000			
+#define	XS_XS_SUSP	0x4000			
+#define	XS_XE_MASK	0xf0000			
+#define	XS_XE_SHIFT	16
+#define	XS_XE_NOERR	0x00000			
+#define	XS_XE_DPE	0x10000			
+#define	XS_XE_DFU	0x20000			
+#define	XS_XE_BEBR	0x30000			
+#define	XS_XE_BEDA	0x40000			
+#define	XS_AD_MASK	0xfff00000		
+#define	XS_AD_SHIFT	20
+
+
+#define	RC_RE		((uint32)1 << 0)	
+#define	RC_RO_MASK	0xfe			
+#define	RC_RO_SHIFT	1
+#define	RC_FM		((uint32)1 << 8)	
+#define	RC_SH		((uint32)1 << 9)	
+#define	RC_OC		((uint32)1 << 10)	
+#define	RC_PD		((uint32)1 << 11)	
+#define	RC_AE		((uint32)3 << 16)	
+#define	RC_AE_SHIFT	16
+
+
+#define	RP_LD_MASK	0xfff			
+
+
+#define	RS_CD_MASK	0x0fff			
+#define	RS_RS_MASK	0xf000			
+#define	RS_RS_SHIFT	12
+#define	RS_RS_DISABLED	0x0000			
+#define	RS_RS_ACTIVE	0x1000			
+#define	RS_RS_IDLE	0x2000			
+#define	RS_RS_STOPPED	0x3000			
+#define	RS_RE_MASK	0xf0000			
+#define	RS_RE_SHIFT	16
+#define	RS_RE_NOERR	0x00000			
+#define	RS_RE_DPE	0x10000			
+#define	RS_RE_DFO	0x20000			
+#define	RS_RE_BEBW	0x30000			
+#define	RS_RE_BEDA	0x40000			
+#define	RS_AD_MASK	0xfff00000		
+#define	RS_AD_SHIFT	20
+
+
+#define	FA_OFF_MASK	0xffff			
+#define	FA_SEL_MASK	0xf0000			
+#define	FA_SEL_SHIFT	16
+#define	FA_SEL_XDD	0x00000			
+#define	FA_SEL_XDP	0x10000			
+#define	FA_SEL_RDD	0x40000			
+#define	FA_SEL_RDP	0x50000			
+#define	FA_SEL_XFD	0x80000			
+#define	FA_SEL_XFP	0x90000			
+#define	FA_SEL_RFD	0xc0000			
+#define	FA_SEL_RFP	0xd0000			
+#define	FA_SEL_RSD	0xe0000			
+#define	FA_SEL_RSP	0xf0000			
+
+
+#define	CTRL_BC_MASK	0x1fff			
+#define	CTRL_AE		((uint32)3 << 16)	
+#define	CTRL_AE_SHIFT	16
+#define	CTRL_EOT	((uint32)1 << 28)	
+#define	CTRL_IOC	((uint32)1 << 29)	
+#define	CTRL_EOF	((uint32)1 << 30)	
+#define	CTRL_SOF	((uint32)1 << 31)	
+
+
+#define	CTRL_CORE_MASK	0x0ff00000
+
+
+
+
+typedef volatile struct {
+	uint32	control;		
+	uint32	ptr;			
+	uint32	addrlow;		
+	uint32	addrhigh;		
+	uint32	status0;		
+	uint32	status1;		
+} dma64regs_t;
+
+typedef volatile struct {
+	dma64regs_t	tx;		
+	dma64regs_t	rx;		
+} dma64regp_t;
+
+typedef volatile struct {		
+	uint32	fifoaddr;		
+	uint32	fifodatalow;		
+	uint32	fifodatahigh;		
+	uint32	pad;			
+} dma64diag_t;
+
+
+typedef volatile struct {
+	uint32	ctrl1;		
+	uint32	ctrl2;		
+	uint32	addrlow;	
+	uint32	addrhigh;	
+} dma64dd_t;
+
+
+#define D64RINGALIGN_BITS 13	
+#define	D64MAXRINGSZ	(1 << D64RINGALIGN_BITS)
+#define	D64RINGALIGN	(1 << D64RINGALIGN_BITS)
+#define	D64MAXDD	(D64MAXRINGSZ / sizeof (dma64dd_t))
+
+
+#define	D64_XC_XE		0x00000001	
+#define	D64_XC_SE		0x00000002	
+#define	D64_XC_LE		0x00000004	
+#define	D64_XC_FL		0x00000010	
+#define	D64_XC_PD		0x00000800	
+#define	D64_XC_AE		0x00030000	
+#define	D64_XC_AE_SHIFT		16
+
+
+#define	D64_XP_LD_MASK		0x00000fff	
+
+
+#define	D64_XS0_CD_MASK		0x00001fff	
+#define	D64_XS0_XS_MASK		0xf0000000     	
+#define	D64_XS0_XS_SHIFT		28
+#define	D64_XS0_XS_DISABLED	0x00000000	
+#define	D64_XS0_XS_ACTIVE	0x10000000	
+#define	D64_XS0_XS_IDLE		0x20000000	
+#define	D64_XS0_XS_STOPPED	0x30000000	
+#define	D64_XS0_XS_SUSP		0x40000000	
+
+#define	D64_XS1_AD_MASK		0x0001ffff	
+#define	D64_XS1_XE_MASK		0xf0000000     	
+#define	D64_XS1_XE_SHIFT		28
+#define	D64_XS1_XE_NOERR	0x00000000	
+#define	D64_XS1_XE_DPE		0x10000000	
+#define	D64_XS1_XE_DFU		0x20000000	
+#define	D64_XS1_XE_DTE		0x30000000	
+#define	D64_XS1_XE_DESRE	0x40000000	
+#define	D64_XS1_XE_COREE	0x50000000	
+
+
+#define	D64_RC_RE		0x00000001	
+#define	D64_RC_RO_MASK		0x000000fe	
+#define	D64_RC_RO_SHIFT		1
+#define	D64_RC_FM		0x00000100	
+#define	D64_RC_SH		0x00000200	
+#define	D64_RC_OC		0x00000400	
+#define	D64_RC_PD		0x00000800	
+#define	D64_RC_AE		0x00030000	
+#define	D64_RC_AE_SHIFT		16
+
+
+#define	D64_RP_LD_MASK		0x00000fff	
+
+
+#define	D64_RS0_CD_MASK		0x00001fff	
+#define	D64_RS0_RS_MASK		0xf0000000     	
+#define	D64_RS0_RS_SHIFT		28
+#define	D64_RS0_RS_DISABLED	0x00000000	
+#define	D64_RS0_RS_ACTIVE	0x10000000	
+#define	D64_RS0_RS_IDLE		0x20000000	
+#define	D64_RS0_RS_STOPPED	0x30000000	
+#define	D64_RS0_RS_SUSP		0x40000000	
+
+#define	D64_RS1_AD_MASK		0x0001ffff	
+#define	D64_RS1_RE_MASK		0xf0000000     	
+#define	D64_RS1_RE_SHIFT		28
+#define	D64_RS1_RE_NOERR	0x00000000	
+#define	D64_RS1_RE_DPO		0x10000000	
+#define	D64_RS1_RE_DFU		0x20000000	
+#define	D64_RS1_RE_DTE		0x30000000	
+#define	D64_RS1_RE_DESRE	0x40000000	
+#define	D64_RS1_RE_COREE	0x50000000	
+
+
+#define	D64_FA_OFF_MASK		0xffff		
+#define	D64_FA_SEL_MASK		0xf0000		
+#define	D64_FA_SEL_SHIFT	16
+#define	D64_FA_SEL_XDD		0x00000		
+#define	D64_FA_SEL_XDP		0x10000		
+#define	D64_FA_SEL_RDD		0x40000		
+#define	D64_FA_SEL_RDP		0x50000		
+#define	D64_FA_SEL_XFD		0x80000		
+#define	D64_FA_SEL_XFP		0x90000		
+#define	D64_FA_SEL_RFD		0xc0000		
+#define	D64_FA_SEL_RFP		0xd0000		
+#define	D64_FA_SEL_RSD		0xe0000		
+#define	D64_FA_SEL_RSP		0xf0000		
+
+
+#define	D64_CTRL1_EOT		((uint32)1 << 28)	
+#define	D64_CTRL1_IOC		((uint32)1 << 29)	
+#define	D64_CTRL1_EOF		((uint32)1 << 30)	
+#define	D64_CTRL1_SOF		((uint32)1 << 31)	
+
+
+#define	D64_CTRL2_BC_MASK	0x00007fff	
+#define	D64_CTRL2_AE		0x00030000	
+#define	D64_CTRL2_AE_SHIFT	16
+#define D64_CTRL2_PARITY	0x00040000      
+
+
+#define	D64_CTRL_CORE_MASK	0x0ff00000
+
+
+#endif	
diff --git a/drivers/net/wireless/bcm4329/include/sbpcmcia.h b/drivers/net/wireless/bcm4329/include/sbpcmcia.h
new file mode 100644
index 000000000000..d6d80334258a
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/sbpcmcia.h
@@ -0,0 +1,109 @@
+/*
+ * BCM43XX Sonics SiliconBackplane PCMCIA core hardware definitions.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sbpcmcia.h,v 13.31.4.1.2.3.8.7 2009/06/22 05:14:24 Exp $
+ */
+
+
+#ifndef	_SBPCMCIA_H
+#define	_SBPCMCIA_H
+
+
+
+
+#define	PCMCIA_FCR		(0x700 / 2)
+
+#define	FCR0_OFF		0
+#define	FCR1_OFF		(0x40 / 2)
+#define	FCR2_OFF		(0x80 / 2)
+#define	FCR3_OFF		(0xc0 / 2)
+
+#define	PCMCIA_FCR0		(0x700 / 2)
+#define	PCMCIA_FCR1		(0x740 / 2)
+#define	PCMCIA_FCR2		(0x780 / 2)
+#define	PCMCIA_FCR3		(0x7c0 / 2)
+
+
+
+#define	PCMCIA_COR		0
+
+#define	COR_RST			0x80
+#define	COR_LEV			0x40
+#define	COR_IRQEN		0x04
+#define	COR_BLREN		0x01
+#define	COR_FUNEN		0x01
+
+
+#define	PCICIA_FCSR		(2 / 2)
+#define	PCICIA_PRR		(4 / 2)
+#define	PCICIA_SCR		(6 / 2)
+#define	PCICIA_ESR		(8 / 2)
+
+
+#define PCM_MEMOFF		0x0000
+#define F0_MEMOFF		0x1000
+#define F1_MEMOFF		0x2000
+#define F2_MEMOFF		0x3000
+#define F3_MEMOFF		0x4000
+
+
+#define MEM_ADDR0		(0x728 / 2)
+#define MEM_ADDR1		(0x72a / 2)
+#define MEM_ADDR2		(0x72c / 2)
+
+
+#define PCMCIA_ADDR0		(0x072e / 2)
+#define PCMCIA_ADDR1		(0x0730 / 2)
+#define PCMCIA_ADDR2		(0x0732 / 2)
+
+#define MEM_SEG			(0x0734 / 2)
+#define SROM_CS			(0x0736 / 2)
+#define SROM_DATAL		(0x0738 / 2)
+#define SROM_DATAH		(0x073a / 2)
+#define SROM_ADDRL		(0x073c / 2)
+#define SROM_ADDRH		(0x073e / 2)
+#define	SROM_INFO2		(0x0772 / 2)	
+#define	SROM_INFO		(0x07be / 2)	
+
+
+#define SROM_IDLE		0
+#define SROM_WRITE		1
+#define SROM_READ		2
+#define SROM_WEN		4
+#define SROM_WDS		7
+#define SROM_DONE		8
+
+
+#define	SRI_SZ_MASK		0x03
+#define	SRI_BLANK		0x04
+#define	SRI_OTP			0x80
+
+
+
+#define SBTML_INT_ACK		0x40000		
+#define SBTML_INT_EN		0x20000		
+
+
+#define SBTMH_INT_STATUS	0x40000		
+
+#endif	
diff --git a/drivers/net/wireless/bcm4329/include/sbsdio.h b/drivers/net/wireless/bcm4329/include/sbsdio.h
new file mode 100644
index 000000000000..75aaf4d88f7d
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/sbsdio.h
@@ -0,0 +1,166 @@
+/*
+ * SDIO device core hardware definitions.
+ * sdio is a portion of the pcmcia core in core rev 3 - rev 8
+ *
+ * SDIO core support 1bit, 4 bit SDIO mode as well as SPI mode.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sbsdio.h,v 13.29.4.1.22.3 2009/03/11 20:26:57 Exp $
+ */
+
+#ifndef	_SBSDIO_H
+#define	_SBSDIO_H
+
+#define SBSDIO_NUM_FUNCTION		3	/* as of sdiod rev 0, supports 3 functions */
+
+/* function 1 miscellaneous registers */
+#define SBSDIO_SPROM_CS			0x10000		/* sprom command and status */
+#define SBSDIO_SPROM_INFO		0x10001		/* sprom info register */
+#define SBSDIO_SPROM_DATA_LOW		0x10002		/* sprom indirect access data byte 0 */
+#define SBSDIO_SPROM_DATA_HIGH		0x10003 	/* sprom indirect access data byte 1 */
+#define SBSDIO_SPROM_ADDR_LOW		0x10004		/* sprom indirect access addr byte 0 */
+#define SBSDIO_SPROM_ADDR_HIGH		0x10005		/* sprom indirect access addr byte 0 */
+#define SBSDIO_CHIP_CTRL_DATA		0x10006		/* xtal_pu (gpio) output */
+#define SBSDIO_CHIP_CTRL_EN		0x10007		/* xtal_pu (gpio) enable */
+#define SBSDIO_WATERMARK		0x10008		/* rev < 7, watermark for sdio device */
+#define SBSDIO_DEVICE_CTL		0x10009		/* control busy signal generation */
+
+/* registers introduced in rev 8, some content (mask/bits) defs in sbsdpcmdev.h */
+#define SBSDIO_FUNC1_SBADDRLOW		0x1000A		/* SB Address Window Low (b15) */
+#define SBSDIO_FUNC1_SBADDRMID		0x1000B		/* SB Address Window Mid (b23:b16) */
+#define SBSDIO_FUNC1_SBADDRHIGH		0x1000C		/* SB Address Window High (b31:b24)    */
+#define SBSDIO_FUNC1_FRAMECTRL		0x1000D		/* Frame Control (frame term/abort) */
+#define SBSDIO_FUNC1_CHIPCLKCSR		0x1000E		/* ChipClockCSR (ALP/HT ctl/status) */
+#define SBSDIO_FUNC1_SDIOPULLUP 	0x1000F		/* SdioPullUp (on cmd, d0-d2) */
+#define SBSDIO_FUNC1_WFRAMEBCLO		0x10019		/* Write Frame Byte Count Low */
+#define SBSDIO_FUNC1_WFRAMEBCHI		0x1001A		/* Write Frame Byte Count High */
+#define SBSDIO_FUNC1_RFRAMEBCLO		0x1001B		/* Read Frame Byte Count Low */
+#define SBSDIO_FUNC1_RFRAMEBCHI		0x1001C		/* Read Frame Byte Count High */
+
+#define SBSDIO_FUNC1_MISC_REG_START	0x10000 	/* f1 misc register start */
+#define SBSDIO_FUNC1_MISC_REG_LIMIT	0x1001C 	/* f1 misc register end */
+
+/* SBSDIO_SPROM_CS */
+#define SBSDIO_SPROM_IDLE		0
+#define SBSDIO_SPROM_WRITE		1
+#define SBSDIO_SPROM_READ		2
+#define SBSDIO_SPROM_WEN		4
+#define SBSDIO_SPROM_WDS		7
+#define SBSDIO_SPROM_DONE		8
+
+/* SBSDIO_SPROM_INFO */
+#define SROM_SZ_MASK			0x03		/* SROM size, 1: 4k, 2: 16k */
+#define SROM_BLANK			0x04		/* depreciated in corerev 6 */
+#define	SROM_OTP			0x80		/* OTP present */
+
+/* SBSDIO_CHIP_CTRL */
+#define SBSDIO_CHIP_CTRL_XTAL		0x01		/* or'd with onchip xtal_pu,
+							 * 1: power on oscillator
+							 * (for 4318 only)
+							 */
+/* SBSDIO_WATERMARK */
+#define SBSDIO_WATERMARK_MASK		0x7f		/* number of words - 1 for sd device
+							 * to wait before sending data to host
+							 */
+
+/* SBSDIO_DEVICE_CTL */
+#define SBSDIO_DEVCTL_SETBUSY		0x01		/* 1: device will assert busy signal when
+							 * receiving CMD53
+							 */
+#define SBSDIO_DEVCTL_SPI_INTR_SYNC	0x02		/* 1: assertion of sdio interrupt is
+							 * synchronous to the sdio clock
+							 */
+#define SBSDIO_DEVCTL_CA_INT_ONLY	0x04		/* 1: mask all interrupts to host
+							 * except the chipActive (rev 8)
+							 */
+#define SBSDIO_DEVCTL_PADS_ISO		0x08		/* 1: isolate internal sdio signals, put
+							 * external pads in tri-state; requires
+							 * sdio bus power cycle to clear (rev 9)
+							 */
+#define SBSDIO_DEVCTL_SB_RST_CTL	0x30		/* Force SD->SB reset mapping (rev 11) */
+#define SBSDIO_DEVCTL_RST_CORECTL	0x00		/*   Determined by CoreControl bit */
+#define SBSDIO_DEVCTL_RST_BPRESET	0x10		/*   Force backplane reset */
+#define SBSDIO_DEVCTL_RST_NOBPRESET	0x20		/*   Force no backplane reset */
+
+
+/* SBSDIO_FUNC1_CHIPCLKCSR */
+#define SBSDIO_FORCE_ALP		0x01		/* Force ALP request to backplane */
+#define SBSDIO_FORCE_HT			0x02		/* Force HT request to backplane */
+#define SBSDIO_FORCE_ILP		0x04		/* Force ILP request to backplane */
+#define SBSDIO_ALP_AVAIL_REQ		0x08		/* Make ALP ready (power up xtal) */
+#define SBSDIO_HT_AVAIL_REQ		0x10		/* Make HT ready (power up PLL) */
+#define SBSDIO_FORCE_HW_CLKREQ_OFF	0x20		/* Squelch clock requests from HW */
+#define SBSDIO_ALP_AVAIL		0x40		/* Status: ALP is ready */
+#define SBSDIO_HT_AVAIL			0x80		/* Status: HT is ready */
+/* In rev8, actual avail bits followed original docs */
+#define SBSDIO_Rev8_HT_AVAIL		0x40
+#define SBSDIO_Rev8_ALP_AVAIL		0x80
+
+#define SBSDIO_AVBITS			(SBSDIO_HT_AVAIL | SBSDIO_ALP_AVAIL)
+#define SBSDIO_ALPAV(regval)		((regval) & SBSDIO_AVBITS)
+#define SBSDIO_HTAV(regval)		(((regval) & SBSDIO_AVBITS) == SBSDIO_AVBITS)
+#define SBSDIO_ALPONLY(regval)		(SBSDIO_ALPAV(regval) && !SBSDIO_HTAV(regval))
+#define SBSDIO_CLKAV(regval, alponly)	(SBSDIO_ALPAV(regval) && \
+					(alponly ? 1 : SBSDIO_HTAV(regval)))
+
+/* SBSDIO_FUNC1_SDIOPULLUP */
+#define SBSDIO_PULLUP_D0		0x01		/* Enable D0/MISO pullup */
+#define SBSDIO_PULLUP_D1		0x02		/* Enable D1/INT# pullup */
+#define SBSDIO_PULLUP_D2		0x04		/* Enable D2 pullup */
+#define SBSDIO_PULLUP_CMD		0x08		/* Enable CMD/MOSI pullup */
+#define SBSDIO_PULLUP_ALL		0x0f		/* All valid bits */
+
+/* function 1 OCP space */
+#define SBSDIO_SB_OFT_ADDR_MASK		0x07FFF		/* sb offset addr is <= 15 bits, 32k */
+#define SBSDIO_SB_OFT_ADDR_LIMIT	0x08000
+#define SBSDIO_SB_ACCESS_2_4B_FLAG	0x08000		/* with b15, maps to 32-bit SB access */
+
+/* some duplication with sbsdpcmdev.h here */
+/* valid bits in SBSDIO_FUNC1_SBADDRxxx regs */
+#define SBSDIO_SBADDRLOW_MASK		0x80		/* Valid bits in SBADDRLOW */
+#define SBSDIO_SBADDRMID_MASK		0xff		/* Valid bits in SBADDRMID */
+#define SBSDIO_SBADDRHIGH_MASK		0xffU		/* Valid bits in SBADDRHIGH */
+#define SBSDIO_SBWINDOW_MASK		0xffff8000	/* Address bits from SBADDR regs */
+
+/* direct(mapped) cis space */
+#define SBSDIO_CIS_BASE_COMMON		0x1000		/* MAPPED common CIS address */
+#define SBSDIO_CIS_SIZE_LIMIT		0x200		/* maximum bytes in one CIS */
+#define SBSDIO_OTP_CIS_SIZE_LIMIT       0x078           /* maximum bytes OTP CIS */
+
+#define SBSDIO_CIS_OFT_ADDR_MASK	0x1FFFF		/* cis offset addr is < 17 bits */
+
+#define SBSDIO_CIS_MANFID_TUPLE_LEN	6		/* manfid tuple length, include tuple,
+							 * link bytes
+							 */
+
+/* indirect cis access (in sprom) */
+#define SBSDIO_SPROM_CIS_OFFSET		0x8		/* 8 control bytes first, CIS starts from
+							 * 8th byte
+							 */
+
+#define SBSDIO_BYTEMODE_DATALEN_MAX	64		/* sdio byte mode: maximum length of one
+							 * data comamnd
+							 */
+
+#define SBSDIO_CORE_ADDR_MASK		0x1FFFF		/* sdio core function one address mask */
+
+#endif	/* _SBSDIO_H */
diff --git a/drivers/net/wireless/bcm4329/include/sbsdpcmdev.h b/drivers/net/wireless/bcm4329/include/sbsdpcmdev.h
new file mode 100644
index 000000000000..7c7c7e4de0f6
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/sbsdpcmdev.h
@@ -0,0 +1,288 @@
+/*
+ * Broadcom SiliconBackplane SDIO/PCMCIA hardware-specific device core support
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sbsdpcmdev.h,v 13.29.4.1.4.6.6.2 2008/12/31 21:16:51 Exp $
+ */
+
+#ifndef	_sbsdpcmdev_h_
+#define	_sbsdpcmdev_h_
+
+/* cpp contortions to concatenate w/arg prescan */
+#ifndef PAD
+#define	_PADLINE(line)	pad ## line
+#define	_XSTR(line)	_PADLINE(line)
+#define	PAD		_XSTR(__LINE__)
+#endif	/* PAD */
+
+
+typedef volatile struct {
+	dma64regs_t	xmt;		/* dma tx */
+	uint32 PAD[2];
+	dma64regs_t	rcv;		/* dma rx */
+	uint32 PAD[2];
+} dma64p_t;
+
+/* dma64 sdiod corerev >= 1 */
+typedef volatile struct {
+	dma64p_t dma64regs[2];
+	dma64diag_t dmafifo;		/* DMA Diagnostic Regs, 0x280-0x28c */
+	uint32 PAD[92];
+} sdiodma64_t;
+
+/* dma32 sdiod corerev == 0 */
+typedef volatile struct {
+	dma32regp_t dma32regs[2];	/* dma tx & rx, 0x200-0x23c */
+	dma32diag_t dmafifo;		/* DMA Diagnostic Regs, 0x240-0x24c */
+	uint32 PAD[108];
+} sdiodma32_t;
+
+/* dma32 regs for pcmcia core */
+typedef volatile struct {
+	dma32regp_t dmaregs;		/* DMA Regs, 0x200-0x21c, rev8 */
+	dma32diag_t dmafifo;		/* DMA Diagnostic Regs, 0x220-0x22c */
+	uint32 PAD[116];
+} pcmdma32_t;
+
+/* core registers */
+typedef volatile struct {
+	uint32 corecontrol;		/* CoreControl, 0x000, rev8 */
+	uint32 corestatus;		/* CoreStatus, 0x004, rev8  */
+	uint32 PAD[1];
+	uint32 biststatus;		/* BistStatus, 0x00c, rev8  */
+
+	/* PCMCIA access */
+	uint16 pcmciamesportaladdr;	/* PcmciaMesPortalAddr, 0x010, rev8   */
+	uint16 PAD[1];
+	uint16 pcmciamesportalmask;	/* PcmciaMesPortalMask, 0x014, rev8   */
+	uint16 PAD[1];
+	uint16 pcmciawrframebc;		/* PcmciaWrFrameBC, 0x018, rev8   */
+	uint16 PAD[1];
+	uint16 pcmciaunderflowtimer;	/* PcmciaUnderflowTimer, 0x01c, rev8   */
+	uint16 PAD[1];
+
+	/* interrupt */
+	uint32 intstatus;		/* IntStatus, 0x020, rev8   */
+	uint32 hostintmask;		/* IntHostMask, 0x024, rev8   */
+	uint32 intmask;			/* IntSbMask, 0x028, rev8   */
+	uint32 sbintstatus;		/* SBIntStatus, 0x02c, rev8   */
+	uint32 sbintmask;		/* SBIntMask, 0x030, rev8   */
+	uint32 PAD[3];
+	uint32 tosbmailbox;		/* ToSBMailbox, 0x040, rev8   */
+	uint32 tohostmailbox;		/* ToHostMailbox, 0x044, rev8   */
+	uint32 tosbmailboxdata;		/* ToSbMailboxData, 0x048, rev8   */
+	uint32 tohostmailboxdata;	/* ToHostMailboxData, 0x04c, rev8   */
+
+	/* synchronized access to registers in SDIO clock domain */
+	uint32 sdioaccess;		/* SdioAccess, 0x050, rev8   */
+	uint32 PAD[3];
+
+	/* PCMCIA frame control */
+	uint8  pcmciaframectrl;		/* pcmciaFrameCtrl, 0x060, rev8   */
+	uint8  PAD[3];
+	uint8  pcmciawatermark;		/* pcmciaWaterMark, 0x064, rev8   */
+	uint8  PAD[155];
+
+	/* interrupt batching control */
+	uint32 intrcvlazy;		/* IntRcvLazy, 0x100, rev8 */
+	uint32 PAD[3];
+
+	/* counters */
+	uint32 cmd52rd;			/* Cmd52RdCount, 0x110, rev8, SDIO: cmd52 reads */
+	uint32 cmd52wr;			/* Cmd52WrCount, 0x114, rev8, SDIO: cmd52 writes */
+	uint32 cmd53rd;			/* Cmd53RdCount, 0x118, rev8, SDIO: cmd53 reads */
+	uint32 cmd53wr;			/* Cmd53WrCount, 0x11c, rev8, SDIO: cmd53 writes */
+	uint32 abort;			/* AbortCount, 0x120, rev8, SDIO: aborts */
+	uint32 datacrcerror;		/* DataCrcErrorCount, 0x124, rev8, SDIO: frames w/bad CRC */
+	uint32 rdoutofsync;		/* RdOutOfSyncCount, 0x128, rev8, SDIO/PCMCIA: Rd Frm OOS */
+	uint32 wroutofsync;		/* RdOutOfSyncCount, 0x12c, rev8, SDIO/PCMCIA: Wr Frm OOS */
+	uint32 writebusy;		/* WriteBusyCount, 0x130, rev8, SDIO: dev asserted "busy" */
+	uint32 readwait;		/* ReadWaitCount, 0x134, rev8, SDIO: read: no data avail */
+	uint32 readterm;		/* ReadTermCount, 0x138, rev8, SDIO: rd frm terminates */
+	uint32 writeterm;		/* WriteTermCount, 0x13c, rev8, SDIO: wr frm terminates */
+	uint32 PAD[40];
+	uint32 clockctlstatus;		/* ClockCtlStatus, 0x1e0, rev8 */
+	uint32 PAD[7];
+
+	/* DMA engines */
+	volatile union {
+		pcmdma32_t pcm32;
+		sdiodma32_t sdiod32;
+		sdiodma64_t sdiod64;
+	} dma;
+
+	/* SDIO/PCMCIA CIS region */
+	char cis[512];			/* 512 byte CIS, 0x400-0x5ff, rev6 */
+
+	/* PCMCIA function control registers */
+	char pcmciafcr[256];		/* PCMCIA FCR, 0x600-6ff, rev6 */
+	uint16 PAD[55];
+
+	/* PCMCIA backplane access */
+	uint16 backplanecsr;		/* BackplaneCSR, 0x76E, rev6 */
+	uint16 backplaneaddr0;		/* BackplaneAddr0, 0x770, rev6 */
+	uint16 backplaneaddr1;		/* BackplaneAddr1, 0x772, rev6 */
+	uint16 backplaneaddr2;		/* BackplaneAddr2, 0x774, rev6 */
+	uint16 backplaneaddr3;		/* BackplaneAddr3, 0x776, rev6 */
+	uint16 backplanedata0;		/* BackplaneData0, 0x778, rev6 */
+	uint16 backplanedata1;		/* BackplaneData1, 0x77a, rev6 */
+	uint16 backplanedata2;		/* BackplaneData2, 0x77c, rev6 */
+	uint16 backplanedata3;		/* BackplaneData3, 0x77e, rev6 */
+	uint16 PAD[31];
+
+	/* sprom "size" & "blank" info */
+	uint16 spromstatus;		/* SPROMStatus, 0x7BE, rev2 */
+	uint32 PAD[464];
+
+	/* Sonics SiliconBackplane registers */
+	sbconfig_t sbconfig;		/* SbConfig Regs, 0xf00-0xfff, rev8 */
+} sdpcmd_regs_t;
+
+/* corecontrol */
+#define CC_CISRDY	(1 << 0)	/* CIS Ready */
+#define CC_BPRESEN	(1 << 1)	/* CCCR RES signal causes backplane reset */
+#define CC_F2RDY	(1 << 2)	/* set CCCR IOR2 bit */
+#define CC_CLRPADSISO	(1 << 3)	/* clear SDIO pads isolation bit (rev 11) */
+
+/* corestatus */
+#define CS_PCMCIAMODE	(1 << 0)	/* Device Mode; 0=SDIO, 1=PCMCIA */
+#define CS_SMARTDEV	(1 << 1)	/* 1=smartDev enabled */
+#define CS_F2ENABLED	(1 << 2)	/* 1=host has enabled the device */
+
+#define PCMCIA_MES_PA_MASK	0x7fff	/* PCMCIA Message Portal Address Mask */
+#define PCMCIA_MES_PM_MASK	0x7fff	/* PCMCIA Message Portal Mask Mask */
+#define PCMCIA_WFBC_MASK	0xffff	/* PCMCIA Write Frame Byte Count Mask */
+#define PCMCIA_UT_MASK		0x07ff	/* PCMCIA Underflow Timer Mask */
+
+/* intstatus */
+#define I_SMB_SW0	(1 << 0)	/* To SB Mail S/W interrupt 0 */
+#define I_SMB_SW1	(1 << 1)	/* To SB Mail S/W interrupt 1 */
+#define I_SMB_SW2	(1 << 2)	/* To SB Mail S/W interrupt 2 */
+#define I_SMB_SW3	(1 << 3)	/* To SB Mail S/W interrupt 3 */
+#define I_SMB_SW_MASK	0x0000000f	/* To SB Mail S/W interrupts mask */
+#define I_SMB_SW_SHIFT	0		/* To SB Mail S/W interrupts shift */
+#define I_HMB_SW0	(1 << 4)	/* To Host Mail S/W interrupt 0 */
+#define I_HMB_SW1	(1 << 5)	/* To Host Mail S/W interrupt 1 */
+#define I_HMB_SW2	(1 << 6)	/* To Host Mail S/W interrupt 2 */
+#define I_HMB_SW3	(1 << 7)	/* To Host Mail S/W interrupt 3 */
+#define I_HMB_SW_MASK	0x000000f0	/* To Host Mail S/W interrupts mask */
+#define I_HMB_SW_SHIFT	4		/* To Host Mail S/W interrupts shift */
+#define I_WR_OOSYNC	(1 << 8)	/* Write Frame Out Of Sync */
+#define I_RD_OOSYNC	(1 << 9)	/* Read Frame Out Of Sync */
+#define	I_PC		(1 << 10)	/* descriptor error */
+#define	I_PD		(1 << 11)	/* data error */
+#define	I_DE		(1 << 12)	/* Descriptor protocol Error */
+#define	I_RU		(1 << 13)	/* Receive descriptor Underflow */
+#define	I_RO		(1 << 14)	/* Receive fifo Overflow */
+#define	I_XU		(1 << 15)	/* Transmit fifo Underflow */
+#define	I_RI		(1 << 16)	/* Receive Interrupt */
+#define I_BUSPWR	(1 << 17)	/* SDIO Bus Power Change (rev 9) */
+#define	I_XI		(1 << 24)	/* Transmit Interrupt */
+#define I_RF_TERM	(1 << 25)	/* Read Frame Terminate */
+#define I_WF_TERM	(1 << 26)	/* Write Frame Terminate */
+#define I_PCMCIA_XU	(1 << 27)	/* PCMCIA Transmit FIFO Underflow */
+#define I_SBINT		(1 << 28)	/* sbintstatus Interrupt */
+#define I_CHIPACTIVE	(1 << 29)	/* chip transitioned from doze to active state */
+#define I_SRESET	(1 << 30)	/* CCCR RES interrupt */
+#define I_IOE2		(1U << 31)	/* CCCR IOE2 Bit Changed */
+#define	I_ERRORS	(I_PC | I_PD | I_DE | I_RU | I_RO | I_XU)	/* DMA Errors */
+#define I_DMA		(I_RI | I_XI | I_ERRORS)
+
+/* sbintstatus */
+#define I_SB_SERR	(1 << 8)	/* Backplane SError (write) */
+#define I_SB_RESPERR	(1 << 9)	/* Backplane Response Error (read) */
+#define I_SB_SPROMERR	(1 << 10)	/* Error accessing the sprom */
+
+/* sdioaccess */
+#define SDA_DATA_MASK	0x000000ff	/* Read/Write Data Mask */
+#define SDA_ADDR_MASK	0x000fff00	/* Read/Write Address Mask */
+#define SDA_ADDR_SHIFT	8		/* Read/Write Address Shift */
+#define SDA_WRITE	0x01000000	/* Write bit  */
+#define SDA_READ	0x00000000	/* Write bit cleared for Read */
+#define SDA_BUSY	0x80000000	/* Busy bit */
+
+/* sdioaccess-accessible register address spaces */
+#define SDA_CCCR_SPACE		0x000	/* sdioAccess CCCR register space */
+#define SDA_F1_FBR_SPACE	0x100	/* sdioAccess F1 FBR register space */
+#define SDA_F2_FBR_SPACE	0x200	/* sdioAccess F2 FBR register space */
+#define SDA_F1_REG_SPACE	0x300	/* sdioAccess F1 core-specific register space */
+
+/* SDA_F1_REG_SPACE sdioaccess-accessible F1 reg space register offsets */
+#define SDA_CHIPCONTROLDATA	0x006	/* ChipControlData */
+#define SDA_CHIPCONTROLENAB	0x007	/* ChipControlEnable */
+#define SDA_F2WATERMARK		0x008	/* Function 2 Watermark */
+#define SDA_DEVICECONTROL	0x009	/* DeviceControl */
+#define SDA_SBADDRLOW		0x00a	/* SbAddrLow */
+#define SDA_SBADDRMID		0x00b	/* SbAddrMid */
+#define SDA_SBADDRHIGH		0x00c	/* SbAddrHigh */
+#define SDA_FRAMECTRL		0x00d	/* FrameCtrl */
+#define SDA_CHIPCLOCKCSR	0x00e	/* ChipClockCSR */
+#define SDA_SDIOPULLUP		0x00f	/* SdioPullUp */
+#define SDA_SDIOWRFRAMEBCLOW	0x019	/* SdioWrFrameBCLow */
+#define SDA_SDIOWRFRAMEBCHIGH	0x01a	/* SdioWrFrameBCHigh */
+#define SDA_SDIORDFRAMEBCLOW	0x01b	/* SdioRdFrameBCLow */
+#define SDA_SDIORDFRAMEBCHIGH	0x01c	/* SdioRdFrameBCHigh */
+
+/* SDA_F2WATERMARK */
+#define SDA_F2WATERMARK_MASK	0x7f	/* F2Watermark Mask */
+
+/* SDA_SBADDRLOW */
+#define SDA_SBADDRLOW_MASK	0x80	/* SbAddrLow Mask */
+
+/* SDA_SBADDRMID */
+#define SDA_SBADDRMID_MASK	0xff	/* SbAddrMid Mask */
+
+/* SDA_SBADDRHIGH */
+#define SDA_SBADDRHIGH_MASK	0xff	/* SbAddrHigh Mask */
+
+/* SDA_FRAMECTRL */
+#define SFC_RF_TERM	(1 << 0)	/* Read Frame Terminate */
+#define SFC_WF_TERM	(1 << 1)	/* Write Frame Terminate */
+#define SFC_CRC4WOOS	(1 << 2)	/* HW reports CRC error for write out of sync */
+#define SFC_ABORTALL	(1 << 3)	/* Abort cancels all in-progress frames */
+
+/* pcmciaframectrl */
+#define PFC_RF_TERM	(1 << 0)	/* Read Frame Terminate */
+#define PFC_WF_TERM	(1 << 1)	/* Write Frame Terminate */
+
+/* intrcvlazy */
+#define	IRL_TO_MASK	0x00ffffff	/* timeout */
+#define	IRL_FC_MASK	0xff000000	/* frame count */
+#define	IRL_FC_SHIFT	24		/* frame count */
+
+/* rx header */
+typedef volatile struct {
+	uint16 len;
+	uint16 flags;
+} sdpcmd_rxh_t;
+
+/* rx header flags */
+#define RXF_CRC		0x0001		/* CRC error detected */
+#define RXF_WOOS	0x0002		/* write frame out of sync */
+#define RXF_WF_TERM	0x0004		/* write frame terminated */
+#define RXF_ABORT	0x0008		/* write frame aborted */
+#define RXF_DISCARD	(RXF_CRC | RXF_WOOS | RXF_WF_TERM | RXF_ABORT)	/* bad frame */
+
+/* HW frame tag */
+#define SDPCM_FRAMETAG_LEN	4	/* HW frametag: 2 bytes len, 2 bytes check val */
+
+#endif	/* _sbsdpcmdev_h_ */
diff --git a/drivers/net/wireless/bcm4329/include/sbsocram.h b/drivers/net/wireless/bcm4329/include/sbsocram.h
new file mode 100644
index 000000000000..5ede0b66d97f
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/sbsocram.h
@@ -0,0 +1,150 @@
+/*
+ * BCM47XX Sonics SiliconBackplane embedded ram core
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sbsocram.h,v 13.9.162.2 2008/12/12 14:13:27 Exp $
+ */
+
+
+#ifndef	_SBSOCRAM_H
+#define	_SBSOCRAM_H
+
+#ifndef _LANGUAGE_ASSEMBLY
+
+
+#ifndef PAD
+#define	_PADLINE(line)	pad ## line
+#define	_XSTR(line)	_PADLINE(line)
+#define	PAD		_XSTR(__LINE__)
+#endif	
+
+
+typedef volatile struct sbsocramregs {
+	uint32	coreinfo;
+	uint32	bwalloc;
+	uint32	extracoreinfo;
+	uint32	biststat;
+	uint32	bankidx;
+	uint32	standbyctrl;
+
+	uint32	errlogstatus;	
+	uint32	errlogaddr;	
+	
+	uint32	cambankidx;
+	uint32	cambankstandbyctrl;
+	uint32	cambankpatchctrl;
+	uint32	cambankpatchtblbaseaddr;
+	uint32	cambankcmdreg;
+	uint32	cambankdatareg;
+	uint32	cambankmaskreg;
+	uint32	PAD[17];
+	uint32	extmemconfig;
+	uint32	extmemparitycsr;
+	uint32	extmemparityerrdata;
+	uint32	extmemparityerrcnt;
+	uint32	extmemwrctrlandsize;
+	uint32	PAD[84];
+	uint32	workaround;
+	uint32	pwrctl;		
+} sbsocramregs_t;
+
+#endif	
+
+
+#define	SR_COREINFO		0x00
+#define	SR_BWALLOC		0x04
+#define	SR_BISTSTAT		0x0c
+#define	SR_BANKINDEX		0x10
+#define	SR_BANKSTBYCTL		0x14
+#define SR_PWRCTL		0x1e8
+
+
+#define	SRCI_PT_MASK		0x00070000	
+#define	SRCI_PT_SHIFT		16
+
+#define SRCI_PT_OCP_OCP		0
+#define SRCI_PT_AXI_OCP		1
+#define SRCI_PT_ARM7AHB_OCP	2
+#define SRCI_PT_CM3AHB_OCP	3
+#define SRCI_PT_AXI_AXI		4
+#define SRCI_PT_AHB_AXI		5
+
+#define SRCI_LSS_MASK		0x00f00000
+#define SRCI_LSS_SHIFT		20
+#define SRCI_LRS_MASK		0x0f000000
+#define SRCI_LRS_SHIFT		24
+
+
+#define	SRCI_MS0_MASK		0xf
+#define SR_MS0_BASE		16
+
+
+#define	SRCI_ROMNB_MASK		0xf000
+#define	SRCI_ROMNB_SHIFT	12
+#define	SRCI_ROMBSZ_MASK	0xf00
+#define	SRCI_ROMBSZ_SHIFT	8
+#define	SRCI_SRNB_MASK		0xf0
+#define	SRCI_SRNB_SHIFT		4
+#define	SRCI_SRBSZ_MASK		0xf
+#define	SRCI_SRBSZ_SHIFT	0
+
+#define SR_BSZ_BASE		14
+
+
+#define	SRSC_SBYOVR_MASK	0x80000000
+#define	SRSC_SBYOVR_SHIFT	31
+#define	SRSC_SBYOVRVAL_MASK	0x60000000
+#define	SRSC_SBYOVRVAL_SHIFT	29
+#define	SRSC_SBYEN_MASK		0x01000000	
+#define	SRSC_SBYEN_SHIFT	24
+
+
+#define SRPC_PMU_STBYDIS_MASK	0x00000010	
+#define SRPC_PMU_STBYDIS_SHIFT	4
+#define SRPC_STBYOVRVAL_MASK	0x00000008
+#define SRPC_STBYOVRVAL_SHIFT	3
+#define SRPC_STBYOVR_MASK	0x00000007
+#define SRPC_STBYOVR_SHIFT	0
+
+
+#define SRECC_NUM_BANKS_MASK   0x000000F0
+#define SRECC_NUM_BANKS_SHIFT  4
+#define SRECC_BANKSIZE_MASK    0x0000000F
+#define SRECC_BANKSIZE_SHIFT   0
+
+#define SRECC_BANKSIZE(value)	 (1 << (value))
+
+
+#define SRCBPC_PATCHENABLE 0x80000000
+
+#define SRP_ADDRESS   0x0001FFFC
+#define SRP_VALID     0x8000
+
+
+#define SRCMD_WRITE  0x00020000
+#define SRCMD_READ   0x00010000
+#define SRCMD_DONE   0x80000000
+
+#define SRCMD_DONE_DLY	1000
+
+
+#endif	
diff --git a/drivers/net/wireless/bcm4329/include/sdio.h b/drivers/net/wireless/bcm4329/include/sdio.h
new file mode 100644
index 000000000000..280cb845fb04
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/sdio.h
@@ -0,0 +1,566 @@
+/*
+ * SDIO spec header file
+ * Protocol and standard (common) device definitions
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sdio.h,v 13.24.4.1.4.1.16.1 2009/08/12 01:08:02 Exp $
+ */
+
+#ifndef	_SDIO_H
+#define	_SDIO_H
+
+
+/* CCCR structure for function 0 */
+typedef volatile struct {
+	uint8	cccr_sdio_rev;		/* RO, cccr and sdio revision */
+	uint8	sd_rev;			/* RO, sd spec revision */
+	uint8	io_en;			/* I/O enable */
+	uint8	io_rdy;			/* I/O ready reg */
+	uint8	intr_ctl;		/* Master and per function interrupt enable control */
+	uint8	intr_status;		/* RO, interrupt pending status */
+	uint8	io_abort;		/* read/write abort or reset all functions */
+	uint8	bus_inter;		/* bus interface control */
+	uint8	capability;		/* RO, card capability */
+
+	uint8	cis_base_low;		/* 0x9 RO, common CIS base address, LSB */
+	uint8	cis_base_mid;
+	uint8	cis_base_high;		/* 0xB RO, common CIS base address, MSB */
+
+	/* suspend/resume registers */
+	uint8	bus_suspend;		/* 0xC */
+	uint8	func_select;		/* 0xD */
+	uint8	exec_flag;		/* 0xE */
+	uint8	ready_flag;		/* 0xF */
+
+	uint8	fn0_blk_size[2];	/* 0x10(LSB), 0x11(MSB) */
+
+	uint8	power_control;		/* 0x12 (SDIO version 1.10) */
+
+	uint8	speed_control;		/* 0x13 */
+} sdio_regs_t;
+
+/* SDIO Device CCCR offsets */
+#define SDIOD_CCCR_REV			0x00
+#define SDIOD_CCCR_SDREV		0x01
+#define SDIOD_CCCR_IOEN			0x02
+#define SDIOD_CCCR_IORDY		0x03
+#define SDIOD_CCCR_INTEN		0x04
+#define SDIOD_CCCR_INTPEND		0x05
+#define SDIOD_CCCR_IOABORT		0x06
+#define SDIOD_CCCR_BICTRL		0x07
+#define SDIOD_CCCR_CAPABLITIES		0x08
+#define SDIOD_CCCR_CISPTR_0		0x09
+#define SDIOD_CCCR_CISPTR_1		0x0A
+#define SDIOD_CCCR_CISPTR_2		0x0B
+#define SDIOD_CCCR_BUSSUSP		0x0C
+#define SDIOD_CCCR_FUNCSEL		0x0D
+#define SDIOD_CCCR_EXECFLAGS		0x0E
+#define SDIOD_CCCR_RDYFLAGS		0x0F
+#define SDIOD_CCCR_BLKSIZE_0		0x10
+#define SDIOD_CCCR_BLKSIZE_1		0x11
+#define SDIOD_CCCR_POWER_CONTROL	0x12
+#define SDIOD_CCCR_SPEED_CONTROL	0x13
+
+/* Broadcom extensions (corerev >= 1) */
+#define SDIOD_CCCR_BRCM_SEPINT		0xf2
+
+/* cccr_sdio_rev */
+#define SDIO_REV_SDIOID_MASK	0xf0	/* SDIO spec revision number */
+#define SDIO_REV_CCCRID_MASK	0x0f	/* CCCR format version number */
+
+/* sd_rev */
+#define SD_REV_PHY_MASK		0x0f	/* SD format version number */
+
+/* io_en */
+#define SDIO_FUNC_ENABLE_1	0x02	/* function 1 I/O enable */
+#define SDIO_FUNC_ENABLE_2	0x04	/* function 2 I/O enable */
+
+/* io_rdys */
+#define SDIO_FUNC_READY_1	0x02	/* function 1 I/O ready */
+#define SDIO_FUNC_READY_2	0x04	/* function 2 I/O ready */
+
+/* intr_ctl */
+#define INTR_CTL_MASTER_EN	0x1	/* interrupt enable master */
+#define INTR_CTL_FUNC1_EN	0x2	/* interrupt enable for function 1 */
+#define INTR_CTL_FUNC2_EN	0x4	/* interrupt enable for function 2 */
+
+/* intr_status */
+#define INTR_STATUS_FUNC1	0x2	/* interrupt pending for function 1 */
+#define INTR_STATUS_FUNC2	0x4	/* interrupt pending for function 2 */
+
+/* io_abort */
+#define IO_ABORT_RESET_ALL	0x08	/* I/O card reset */
+#define IO_ABORT_FUNC_MASK	0x07	/* abort selction: function x */
+
+/* bus_inter */
+#define BUS_CARD_DETECT_DIS	0x80	/* Card Detect disable */
+#define BUS_SPI_CONT_INTR_CAP	0x40	/* support continuous SPI interrupt */
+#define BUS_SPI_CONT_INTR_EN	0x20	/* continuous SPI interrupt enable */
+#define BUS_SD_DATA_WIDTH_MASK	0x03	/* bus width mask */
+#define BUS_SD_DATA_WIDTH_4BIT	0x02	/* bus width 4-bit mode */
+#define BUS_SD_DATA_WIDTH_1BIT	0x00	/* bus width 1-bit mode */
+
+/* capability */
+#define SDIO_CAP_4BLS		0x80	/* 4-bit support for low speed card */
+#define SDIO_CAP_LSC		0x40	/* low speed card */
+#define SDIO_CAP_E4MI		0x20	/* enable interrupt between block of data in 4-bit mode */
+#define SDIO_CAP_S4MI		0x10	/* support interrupt between block of data in 4-bit mode */
+#define SDIO_CAP_SBS		0x08	/* support suspend/resume */
+#define SDIO_CAP_SRW		0x04	/* support read wait */
+#define SDIO_CAP_SMB		0x02	/* support multi-block transfer */
+#define SDIO_CAP_SDC		0x01	/* Support Direct commands during multi-byte transfer */
+
+/* power_control */
+#define SDIO_POWER_SMPC		0x01	/* supports master power control (RO) */
+#define SDIO_POWER_EMPC		0x02	/* enable master power control (allow > 200mA) (RW) */
+
+/* speed_control (control device entry into high-speed clocking mode) */
+#define SDIO_SPEED_SHS		0x01	/* supports high-speed [clocking] mode (RO) */
+#define SDIO_SPEED_EHS		0x02	/* enable high-speed [clocking] mode (RW) */
+
+/* brcm sepint */
+#define SDIO_SEPINT_MASK	0x01	/* route sdpcmdev intr onto separate pad (chip-specific) */
+#define SDIO_SEPINT_OE		0x02	/* 1 asserts output enable for above pad */
+#define SDIO_SEPINT_ACT_HI	0x04	/* use active high interrupt level instead of active low */
+
+/* FBR structure for function 1-7, FBR addresses and register offsets */
+typedef volatile struct {
+	uint8	devctr;			/* device interface, CSA control */
+	uint8	ext_dev;		/* extended standard I/O device type code */
+	uint8	pwr_sel;		/* power selection support */
+	uint8	PAD[6];			/* reserved */
+
+	uint8	cis_low;		/* CIS LSB */
+	uint8	cis_mid;
+	uint8	cis_high;		/* CIS MSB */
+	uint8	csa_low;		/* code storage area, LSB */
+	uint8	csa_mid;
+	uint8	csa_high;		/* code storage area, MSB */
+	uint8	csa_dat_win;		/* data access window to function */
+
+	uint8	fnx_blk_size[2];	/* block size, little endian */
+} sdio_fbr_t;
+
+/* Maximum number of I/O funcs */
+#define SDIOD_MAX_IOFUNCS		7
+
+/* SDIO Device FBR Start Address  */
+#define SDIOD_FBR_STARTADDR		0x100
+
+/* SDIO Device FBR Size */
+#define SDIOD_FBR_SIZE			0x100
+
+/* Macro to calculate FBR register base */
+#define SDIOD_FBR_BASE(n)		((n) * 0x100)
+
+/* Function register offsets */
+#define SDIOD_FBR_DEVCTR		0x00	/* basic info for function */
+#define SDIOD_FBR_EXT_DEV		0x01	/* extended I/O device code */
+#define SDIOD_FBR_PWR_SEL		0x02	/* power selection bits */
+
+/* SDIO Function CIS ptr offset */
+#define SDIOD_FBR_CISPTR_0		0x09
+#define SDIOD_FBR_CISPTR_1		0x0A
+#define SDIOD_FBR_CISPTR_2		0x0B
+
+/* Code Storage Area pointer */
+#define SDIOD_FBR_CSA_ADDR_0		0x0C
+#define SDIOD_FBR_CSA_ADDR_1		0x0D
+#define SDIOD_FBR_CSA_ADDR_2		0x0E
+#define SDIOD_FBR_CSA_DATA		0x0F
+
+/* SDIO Function I/O Block Size */
+#define SDIOD_FBR_BLKSIZE_0		0x10
+#define SDIOD_FBR_BLKSIZE_1		0x11
+
+/* devctr */
+#define SDIOD_FBR_DEVCTR_DIC	0x0f	/* device interface code */
+#define SDIOD_FBR_DECVTR_CSA	0x40	/* CSA support flag */
+#define SDIOD_FBR_DEVCTR_CSA_EN	0x80	/* CSA enabled */
+/* interface codes */
+#define SDIOD_DIC_NONE		0	/* SDIO standard interface is not supported */
+#define SDIOD_DIC_UART		1
+#define SDIOD_DIC_BLUETOOTH_A	2
+#define SDIOD_DIC_BLUETOOTH_B	3
+#define SDIOD_DIC_GPS		4
+#define SDIOD_DIC_CAMERA	5
+#define SDIOD_DIC_PHS		6
+#define SDIOD_DIC_WLAN		7
+#define SDIOD_DIC_EXT		0xf	/* extended device interface, read ext_dev register */
+
+/* pwr_sel */
+#define SDIOD_PWR_SEL_SPS	0x01	/* supports power selection */
+#define SDIOD_PWR_SEL_EPS	0x02	/* enable power selection (low-current mode) */
+
+/* misc defines */
+#define SDIO_FUNC_0		0
+#define SDIO_FUNC_1		1
+#define SDIO_FUNC_2		2
+#define SDIO_FUNC_3		3
+#define SDIO_FUNC_4		4
+#define SDIO_FUNC_5		5
+#define SDIO_FUNC_6		6
+#define SDIO_FUNC_7		7
+
+#define SD_CARD_TYPE_UNKNOWN	0	/* bad type or unrecognized */
+#define SD_CARD_TYPE_IO		1	/* IO only card */
+#define SD_CARD_TYPE_MEMORY	2	/* memory only card */
+#define SD_CARD_TYPE_COMBO	3	/* IO and memory combo card */
+
+#define SDIO_MAX_BLOCK_SIZE	2048	/* maximum block size for block mode operation */
+#define SDIO_MIN_BLOCK_SIZE	1	/* minimum block size for block mode operation */
+
+/* Card registers: status bit position */
+#define CARDREG_STATUS_BIT_OUTOFRANGE		31
+#define CARDREG_STATUS_BIT_COMCRCERROR		23
+#define CARDREG_STATUS_BIT_ILLEGALCOMMAND	22
+#define CARDREG_STATUS_BIT_ERROR		19
+#define CARDREG_STATUS_BIT_IOCURRENTSTATE3	12
+#define CARDREG_STATUS_BIT_IOCURRENTSTATE2	11
+#define CARDREG_STATUS_BIT_IOCURRENTSTATE1	10
+#define CARDREG_STATUS_BIT_IOCURRENTSTATE0	9
+#define CARDREG_STATUS_BIT_FUN_NUM_ERROR	4
+
+
+
+#define SD_CMD_GO_IDLE_STATE		0	/* mandatory for SDIO */
+#define SD_CMD_SEND_OPCOND		1
+#define SD_CMD_MMC_SET_RCA		3
+#define SD_CMD_IO_SEND_OP_COND		5	/* mandatory for SDIO */
+#define SD_CMD_SELECT_DESELECT_CARD	7
+#define SD_CMD_SEND_CSD			9
+#define SD_CMD_SEND_CID			10
+#define SD_CMD_STOP_TRANSMISSION	12
+#define SD_CMD_SEND_STATUS		13
+#define SD_CMD_GO_INACTIVE_STATE	15
+#define SD_CMD_SET_BLOCKLEN		16
+#define SD_CMD_READ_SINGLE_BLOCK	17
+#define SD_CMD_READ_MULTIPLE_BLOCK	18
+#define SD_CMD_WRITE_BLOCK		24
+#define SD_CMD_WRITE_MULTIPLE_BLOCK	25
+#define SD_CMD_PROGRAM_CSD		27
+#define SD_CMD_SET_WRITE_PROT		28
+#define SD_CMD_CLR_WRITE_PROT		29
+#define SD_CMD_SEND_WRITE_PROT		30
+#define SD_CMD_ERASE_WR_BLK_START	32
+#define SD_CMD_ERASE_WR_BLK_END		33
+#define SD_CMD_ERASE			38
+#define SD_CMD_LOCK_UNLOCK		42
+#define SD_CMD_IO_RW_DIRECT		52	/* mandatory for SDIO */
+#define SD_CMD_IO_RW_EXTENDED		53	/* mandatory for SDIO */
+#define SD_CMD_APP_CMD			55
+#define SD_CMD_GEN_CMD			56
+#define SD_CMD_READ_OCR			58
+#define SD_CMD_CRC_ON_OFF		59	/* mandatory for SDIO */
+#define SD_ACMD_SD_STATUS		13
+#define SD_ACMD_SEND_NUM_WR_BLOCKS	22
+#define SD_ACMD_SET_WR_BLOCK_ERASE_CNT	23
+#define SD_ACMD_SD_SEND_OP_COND		41
+#define SD_ACMD_SET_CLR_CARD_DETECT	42
+#define SD_ACMD_SEND_SCR		51
+
+/* argument for SD_CMD_IO_RW_DIRECT and SD_CMD_IO_RW_EXTENDED */
+#define SD_IO_OP_READ		0   /* Read_Write: Read */
+#define SD_IO_OP_WRITE		1   /* Read_Write: Write */
+#define SD_IO_RW_NORMAL		0   /* no RAW */
+#define SD_IO_RW_RAW		1   /* RAW */
+#define SD_IO_BYTE_MODE		0   /* Byte Mode */
+#define SD_IO_BLOCK_MODE	1   /* BlockMode */
+#define SD_IO_FIXED_ADDRESS	0   /* fix Address */
+#define SD_IO_INCREMENT_ADDRESS	1   /* IncrementAddress */
+
+/* build SD_CMD_IO_RW_DIRECT Argument */
+#define SDIO_IO_RW_DIRECT_ARG(rw, raw, func, addr, data) \
+	((((rw) & 1) << 31) | (((func) & 0x7) << 28) | (((raw) & 1) << 27) | \
+	 (((addr) & 0x1FFFF) << 9) | ((data) & 0xFF))
+
+/* build SD_CMD_IO_RW_EXTENDED Argument */
+#define SDIO_IO_RW_EXTENDED_ARG(rw, blk, func, addr, inc_addr, count) \
+	((((rw) & 1) << 31) | (((func) & 0x7) << 28) | (((blk) & 1) << 27) | \
+	 (((inc_addr) & 1) << 26) | (((addr) & 0x1FFFF) << 9) | ((count) & 0x1FF))
+
+/* SDIO response parameters */
+#define SD_RSP_NO_NONE			0
+#define SD_RSP_NO_1			1
+#define SD_RSP_NO_2			2
+#define SD_RSP_NO_3			3
+#define SD_RSP_NO_4			4
+#define SD_RSP_NO_5			5
+#define SD_RSP_NO_6			6
+
+	/* Modified R6 response (to CMD3) */
+#define SD_RSP_MR6_COM_CRC_ERROR	0x8000
+#define SD_RSP_MR6_ILLEGAL_COMMAND	0x4000
+#define SD_RSP_MR6_ERROR		0x2000
+
+	/* Modified R1 in R4 Response (to CMD5) */
+#define SD_RSP_MR1_SBIT			0x80
+#define SD_RSP_MR1_PARAMETER_ERROR	0x40
+#define SD_RSP_MR1_RFU5			0x20
+#define SD_RSP_MR1_FUNC_NUM_ERROR	0x10
+#define SD_RSP_MR1_COM_CRC_ERROR	0x08
+#define SD_RSP_MR1_ILLEGAL_COMMAND	0x04
+#define SD_RSP_MR1_RFU1			0x02
+#define SD_RSP_MR1_IDLE_STATE		0x01
+
+	/* R5 response (to CMD52 and CMD53) */
+#define SD_RSP_R5_COM_CRC_ERROR		0x80
+#define SD_RSP_R5_ILLEGAL_COMMAND	0x40
+#define SD_RSP_R5_IO_CURRENTSTATE1	0x20
+#define SD_RSP_R5_IO_CURRENTSTATE0	0x10
+#define SD_RSP_R5_ERROR			0x08
+#define SD_RSP_R5_RFU			0x04
+#define SD_RSP_R5_FUNC_NUM_ERROR	0x02
+#define SD_RSP_R5_OUT_OF_RANGE		0x01
+
+#define SD_RSP_R5_ERRBITS		0xCB
+
+
+/* ------------------------------------------------
+ *  SDIO Commands and responses
+ *
+ *  I/O only commands are:
+ *      CMD0, CMD3, CMD5, CMD7, CMD15, CMD52, CMD53
+ * ------------------------------------------------
+ */
+
+/* SDIO Commands */
+#define SDIOH_CMD_0		0
+#define SDIOH_CMD_3		3
+#define SDIOH_CMD_5		5
+#define SDIOH_CMD_7		7
+#define SDIOH_CMD_15		15
+#define SDIOH_CMD_52		52
+#define SDIOH_CMD_53		53
+#define SDIOH_CMD_59		59
+
+/* SDIO Command Responses */
+#define SDIOH_RSP_NONE		0
+#define SDIOH_RSP_R1		1
+#define SDIOH_RSP_R2		2
+#define SDIOH_RSP_R3		3
+#define SDIOH_RSP_R4		4
+#define SDIOH_RSP_R5		5
+#define SDIOH_RSP_R6		6
+
+/*
+ *  SDIO Response Error flags
+ */
+#define SDIOH_RSP5_ERROR_FLAGS	0xCB
+
+/* ------------------------------------------------
+ * SDIO Command structures. I/O only commands are:
+ *
+ * 	CMD0, CMD3, CMD5, CMD7, CMD15, CMD52, CMD53
+ * ------------------------------------------------
+ */
+
+#define CMD5_OCR_M		BITFIELD_MASK(24)
+#define CMD5_OCR_S		0
+
+#define CMD7_RCA_M		BITFIELD_MASK(16)
+#define CMD7_RCA_S		16
+
+#define CMD_15_RCA_M		BITFIELD_MASK(16)
+#define CMD_15_RCA_S		16
+
+#define CMD52_DATA_M		BITFIELD_MASK(8)  /* Bits [7:0]    - Write Data/Stuff bits of CMD52
+						   */
+#define CMD52_DATA_S		0
+#define CMD52_REG_ADDR_M	BITFIELD_MASK(17) /* Bits [25:9]   - register address */
+#define CMD52_REG_ADDR_S	9
+#define CMD52_RAW_M		BITFIELD_MASK(1)  /* Bit  27       - Read after Write flag */
+#define CMD52_RAW_S		27
+#define CMD52_FUNCTION_M	BITFIELD_MASK(3)  /* Bits [30:28]  - Function number */
+#define CMD52_FUNCTION_S	28
+#define CMD52_RW_FLAG_M		BITFIELD_MASK(1)  /* Bit  31       - R/W flag */
+#define CMD52_RW_FLAG_S		31
+
+
+#define CMD53_BYTE_BLK_CNT_M	BITFIELD_MASK(9) /* Bits [8:0]     - Byte/Block Count of CMD53 */
+#define CMD53_BYTE_BLK_CNT_S	0
+#define CMD53_REG_ADDR_M	BITFIELD_MASK(17) /* Bits [25:9]   - register address */
+#define CMD53_REG_ADDR_S	9
+#define CMD53_OP_CODE_M		BITFIELD_MASK(1)  /* Bit  26       - R/W Operation Code */
+#define CMD53_OP_CODE_S		26
+#define CMD53_BLK_MODE_M	BITFIELD_MASK(1)  /* Bit  27       - Block Mode */
+#define CMD53_BLK_MODE_S	27
+#define CMD53_FUNCTION_M	BITFIELD_MASK(3)  /* Bits [30:28]  - Function number */
+#define CMD53_FUNCTION_S	28
+#define CMD53_RW_FLAG_M		BITFIELD_MASK(1)  /* Bit  31       - R/W flag */
+#define CMD53_RW_FLAG_S		31
+
+/* ------------------------------------------------------
+ * SDIO Command Response structures for SD1 and SD4 modes
+ *  -----------------------------------------------------
+ */
+#define RSP4_IO_OCR_M		BITFIELD_MASK(24) /* Bits [23:0]  - Card's OCR Bits [23:0] */
+#define RSP4_IO_OCR_S		0
+#define RSP4_STUFF_M		BITFIELD_MASK(3)  /* Bits [26:24] - Stuff bits */
+#define RSP4_STUFF_S		24
+#define RSP4_MEM_PRESENT_M	BITFIELD_MASK(1)  /* Bit  27      - Memory present */
+#define RSP4_MEM_PRESENT_S	27
+#define RSP4_NUM_FUNCS_M	BITFIELD_MASK(3)  /* Bits [30:28] - Number of I/O funcs */
+#define RSP4_NUM_FUNCS_S	28
+#define RSP4_CARD_READY_M	BITFIELD_MASK(1)  /* Bit  31      - SDIO card ready */
+#define RSP4_CARD_READY_S	31
+
+#define RSP6_STATUS_M		BITFIELD_MASK(16) /* Bits [15:0]  - Card status bits [19,22,23,12:0]
+						   */
+#define RSP6_STATUS_S		0
+#define RSP6_IO_RCA_M		BITFIELD_MASK(16) /* Bits [31:16] - RCA bits[31-16] */
+#define RSP6_IO_RCA_S		16
+
+#define RSP1_AKE_SEQ_ERROR_M	BITFIELD_MASK(1)  /* Bit 3       - Authentication seq error */
+#define RSP1_AKE_SEQ_ERROR_S	3
+#define RSP1_APP_CMD_M		BITFIELD_MASK(1)  /* Bit 5       - Card expects ACMD */
+#define RSP1_APP_CMD_S		5
+#define RSP1_READY_FOR_DATA_M	BITFIELD_MASK(1)  /* Bit 8       - Ready for data (buff empty) */
+#define RSP1_READY_FOR_DATA_S	8
+#define RSP1_CURR_STATE_M	BITFIELD_MASK(4)  /* Bits [12:9] - State of card
+						   * when Cmd was received
+						   */
+#define RSP1_CURR_STATE_S	9
+#define RSP1_EARSE_RESET_M	BITFIELD_MASK(1)  /* Bit 13   - Erase seq cleared */
+#define RSP1_EARSE_RESET_S	13
+#define RSP1_CARD_ECC_DISABLE_M	BITFIELD_MASK(1)  /* Bit 14   - Card ECC disabled */
+#define RSP1_CARD_ECC_DISABLE_S	14
+#define RSP1_WP_ERASE_SKIP_M	BITFIELD_MASK(1)  /* Bit 15   - Partial blocks erased due to W/P */
+#define RSP1_WP_ERASE_SKIP_S	15
+#define RSP1_CID_CSD_OVERW_M	BITFIELD_MASK(1)  /* Bit 16   - Illegal write to CID or R/O bits
+						   * of CSD
+						   */
+#define RSP1_CID_CSD_OVERW_S	16
+#define RSP1_ERROR_M		BITFIELD_MASK(1)  /* Bit 19   - General/Unknown error */
+#define RSP1_ERROR_S		19
+#define RSP1_CC_ERROR_M		BITFIELD_MASK(1)  /* Bit 20   - Internal Card Control error */
+#define RSP1_CC_ERROR_S		20
+#define RSP1_CARD_ECC_FAILED_M	BITFIELD_MASK(1)  /* Bit 21   - Card internal ECC failed
+						   * to correct data
+						   */
+#define RSP1_CARD_ECC_FAILED_S	21
+#define RSP1_ILLEGAL_CMD_M	BITFIELD_MASK(1)  /* Bit 22   - Cmd not legal for the card state */
+#define RSP1_ILLEGAL_CMD_S	22
+#define RSP1_COM_CRC_ERROR_M	BITFIELD_MASK(1)  /* Bit 23   - CRC check of previous command failed
+						   */
+#define RSP1_COM_CRC_ERROR_S	23
+#define RSP1_LOCK_UNLOCK_FAIL_M	BITFIELD_MASK(1)  /* Bit 24   - Card lock-unlock Cmd Seq error */
+#define RSP1_LOCK_UNLOCK_FAIL_S	24
+#define RSP1_CARD_LOCKED_M	BITFIELD_MASK(1)  /* Bit 25   - Card locked by the host */
+#define RSP1_CARD_LOCKED_S	25
+#define RSP1_WP_VIOLATION_M	BITFIELD_MASK(1)  /* Bit 26   - Attempt to program
+						   * write-protected blocks
+						   */
+#define RSP1_WP_VIOLATION_S	26
+#define RSP1_ERASE_PARAM_M	BITFIELD_MASK(1)  /* Bit 27   - Invalid erase blocks */
+#define RSP1_ERASE_PARAM_S	27
+#define RSP1_ERASE_SEQ_ERR_M	BITFIELD_MASK(1)  /* Bit 28   - Erase Cmd seq error */
+#define RSP1_ERASE_SEQ_ERR_S	28
+#define RSP1_BLK_LEN_ERR_M	BITFIELD_MASK(1)  /* Bit 29   - Block length error */
+#define RSP1_BLK_LEN_ERR_S	29
+#define RSP1_ADDR_ERR_M		BITFIELD_MASK(1)  /* Bit 30   - Misaligned address */
+#define RSP1_ADDR_ERR_S		30
+#define RSP1_OUT_OF_RANGE_M	BITFIELD_MASK(1)  /* Bit 31   - Cmd arg was out of range */
+#define RSP1_OUT_OF_RANGE_S	31
+
+
+#define RSP5_DATA_M		BITFIELD_MASK(8)  /* Bits [0:7]   - data */
+#define RSP5_DATA_S		0
+#define RSP5_FLAGS_M		BITFIELD_MASK(8)  /* Bit  [15:8]  - Rsp flags */
+#define RSP5_FLAGS_S		8
+#define RSP5_STUFF_M		BITFIELD_MASK(16) /* Bits [31:16] - Stuff bits */
+#define RSP5_STUFF_S		16
+
+/* ----------------------------------------------
+ * SDIO Command Response structures for SPI mode
+ * ----------------------------------------------
+ */
+#define SPIRSP4_IO_OCR_M	BITFIELD_MASK(16) /* Bits [15:0]    - Card's OCR Bits [23:8] */
+#define SPIRSP4_IO_OCR_S	0
+#define SPIRSP4_STUFF_M		BITFIELD_MASK(3)  /* Bits [18:16]   - Stuff bits */
+#define SPIRSP4_STUFF_S		16
+#define SPIRSP4_MEM_PRESENT_M	BITFIELD_MASK(1)  /* Bit  19        - Memory present */
+#define SPIRSP4_MEM_PRESENT_S	19
+#define SPIRSP4_NUM_FUNCS_M	BITFIELD_MASK(3)  /* Bits [22:20]   - Number of I/O funcs */
+#define SPIRSP4_NUM_FUNCS_S	20
+#define SPIRSP4_CARD_READY_M	BITFIELD_MASK(1)  /* Bit  23        - SDIO card ready */
+#define SPIRSP4_CARD_READY_S	23
+#define SPIRSP4_IDLE_STATE_M	BITFIELD_MASK(1)  /* Bit  24        - idle state */
+#define SPIRSP4_IDLE_STATE_S	24
+#define SPIRSP4_ILLEGAL_CMD_M	BITFIELD_MASK(1)  /* Bit  26        - Illegal Cmd error */
+#define SPIRSP4_ILLEGAL_CMD_S	26
+#define SPIRSP4_COM_CRC_ERROR_M	BITFIELD_MASK(1)  /* Bit  27        - COM CRC error */
+#define SPIRSP4_COM_CRC_ERROR_S	27
+#define SPIRSP4_FUNC_NUM_ERROR_M	BITFIELD_MASK(1)  /* Bit  28        - Function number error
+							   */
+#define SPIRSP4_FUNC_NUM_ERROR_S	28
+#define SPIRSP4_PARAM_ERROR_M	BITFIELD_MASK(1)  /* Bit  30        - Parameter Error Bit */
+#define SPIRSP4_PARAM_ERROR_S	30
+#define SPIRSP4_START_BIT_M	BITFIELD_MASK(1)  /* Bit  31        - Start Bit */
+#define SPIRSP4_START_BIT_S	31
+
+#define SPIRSP5_DATA_M			BITFIELD_MASK(8)  /* Bits [23:16]   - R/W Data */
+#define SPIRSP5_DATA_S			16
+#define SPIRSP5_IDLE_STATE_M		BITFIELD_MASK(1)  /* Bit  24        - Idle state */
+#define SPIRSP5_IDLE_STATE_S		24
+#define SPIRSP5_ILLEGAL_CMD_M		BITFIELD_MASK(1)  /* Bit  26        - Illegal Cmd error */
+#define SPIRSP5_ILLEGAL_CMD_S		26
+#define SPIRSP5_COM_CRC_ERROR_M		BITFIELD_MASK(1)  /* Bit  27        - COM CRC error */
+#define SPIRSP5_COM_CRC_ERROR_S		27
+#define SPIRSP5_FUNC_NUM_ERROR_M	BITFIELD_MASK(1)  /* Bit  28        - Function number error
+							   */
+#define SPIRSP5_FUNC_NUM_ERROR_S	28
+#define SPIRSP5_PARAM_ERROR_M		BITFIELD_MASK(1)  /* Bit  30        - Parameter Error Bit */
+#define SPIRSP5_PARAM_ERROR_S		30
+#define SPIRSP5_START_BIT_M		BITFIELD_MASK(1)  /* Bit  31        - Start Bit */
+#define SPIRSP5_START_BIT_S		31
+
+/* RSP6 card status format; Pg 68 Physical Layer spec v 1.10 */
+#define RSP6STAT_AKE_SEQ_ERROR_M	BITFIELD_MASK(1)  /* Bit 3	- Authentication seq error
+							   */
+#define RSP6STAT_AKE_SEQ_ERROR_S	3
+#define RSP6STAT_APP_CMD_M		BITFIELD_MASK(1)  /* Bit 5	- Card expects ACMD */
+#define RSP6STAT_APP_CMD_S		5
+#define RSP6STAT_READY_FOR_DATA_M	BITFIELD_MASK(1)  /* Bit 8	- Ready for data
+							   * (buff empty)
+							   */
+#define RSP6STAT_READY_FOR_DATA_S	8
+#define RSP6STAT_CURR_STATE_M		BITFIELD_MASK(4)  /* Bits [12:9] - Card state at
+							   * Cmd reception
+							   */
+#define RSP6STAT_CURR_STATE_S		9
+#define RSP6STAT_ERROR_M		BITFIELD_MASK(1)  /* Bit 13  - General/Unknown error Bit 19
+							   */
+#define RSP6STAT_ERROR_S		13
+#define RSP6STAT_ILLEGAL_CMD_M		BITFIELD_MASK(1)  /* Bit 14  - Illegal cmd for
+							   * card state Bit 22
+							   */
+#define RSP6STAT_ILLEGAL_CMD_S		14
+#define RSP6STAT_COM_CRC_ERROR_M	BITFIELD_MASK(1)  /* Bit 15  - CRC previous command
+							   * failed Bit 23
+							   */
+#define RSP6STAT_COM_CRC_ERROR_S	15
+
+#define SDIOH_XFER_TYPE_READ    SD_IO_OP_READ
+#define SDIOH_XFER_TYPE_WRITE   SD_IO_OP_WRITE
+
+#endif /* _SDIO_H */
diff --git a/drivers/net/wireless/bcm4329/include/sdioh.h b/drivers/net/wireless/bcm4329/include/sdioh.h
new file mode 100644
index 000000000000..8123452eac2b
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/sdioh.h
@@ -0,0 +1,299 @@
+/*
+ * SDIO Host Controller Spec header file
+ * Register map and definitions for the Standard Host Controller
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sdioh.h,v 13.13.18.1.16.3 2009/12/08 22:34:21 Exp $
+ */
+
+#ifndef	_SDIOH_H
+#define	_SDIOH_H
+
+#define SD_SysAddr			0x000
+#define SD_BlockSize			0x004
+#define SD_BlockCount 			0x006
+#define SD_Arg0				0x008
+#define SD_Arg1 			0x00A
+#define SD_TransferMode			0x00C
+#define SD_Command 			0x00E
+#define SD_Response0			0x010
+#define SD_Response1 			0x012
+#define SD_Response2			0x014
+#define SD_Response3 			0x016
+#define SD_Response4			0x018
+#define SD_Response5 			0x01A
+#define SD_Response6			0x01C
+#define SD_Response7 			0x01E
+#define SD_BufferDataPort0		0x020
+#define SD_BufferDataPort1 		0x022
+#define SD_PresentState			0x024
+#define SD_HostCntrl			0x028
+#define SD_PwrCntrl			0x029
+#define SD_BlockGapCntrl 		0x02A
+#define SD_WakeupCntrl 			0x02B
+#define SD_ClockCntrl			0x02C
+#define SD_TimeoutCntrl 		0x02E
+#define SD_SoftwareReset		0x02F
+#define SD_IntrStatus			0x030
+#define SD_ErrorIntrStatus 		0x032
+#define SD_IntrStatusEnable		0x034
+#define SD_ErrorIntrStatusEnable 	0x036
+#define SD_IntrSignalEnable		0x038
+#define SD_ErrorIntrSignalEnable 	0x03A
+#define SD_CMD12ErrorStatus		0x03C
+#define SD_Capabilities			0x040
+#define SD_Capabilities_Reserved	0x044
+#define SD_MaxCurCap			0x048
+#define SD_MaxCurCap_Reserved		0x04C
+#define SD_ADMA_SysAddr			0x58
+#define SD_SlotInterruptStatus		0x0FC
+#define SD_HostControllerVersion 	0x0FE
+
+/* SD specific registers in PCI config space */
+#define SD_SlotInfo	0x40
+
+/* SD_Capabilities reg (0x040) */
+#define CAP_TO_CLKFREQ_M 	BITFIELD_MASK(6)
+#define CAP_TO_CLKFREQ_S 	0
+#define CAP_TO_CLKUNIT_M  	BITFIELD_MASK(1)
+#define CAP_TO_CLKUNIT_S 	7
+#define CAP_BASECLK_M 		BITFIELD_MASK(6)
+#define CAP_BASECLK_S 		8
+#define CAP_MAXBLOCK_M 		BITFIELD_MASK(2)
+#define CAP_MAXBLOCK_S		16
+#define CAP_ADMA2_M		BITFIELD_MASK(1)
+#define CAP_ADMA2_S		19
+#define CAP_ADMA1_M		BITFIELD_MASK(1)
+#define CAP_ADMA1_S		20
+#define CAP_HIGHSPEED_M		BITFIELD_MASK(1)
+#define CAP_HIGHSPEED_S		21
+#define CAP_DMA_M		BITFIELD_MASK(1)
+#define CAP_DMA_S		22
+#define CAP_SUSPEND_M		BITFIELD_MASK(1)
+#define CAP_SUSPEND_S		23
+#define CAP_VOLT_3_3_M		BITFIELD_MASK(1)
+#define CAP_VOLT_3_3_S		24
+#define CAP_VOLT_3_0_M		BITFIELD_MASK(1)
+#define CAP_VOLT_3_0_S		25
+#define CAP_VOLT_1_8_M		BITFIELD_MASK(1)
+#define CAP_VOLT_1_8_S		26
+#define CAP_64BIT_HOST_M	BITFIELD_MASK(1)
+#define CAP_64BIT_HOST_S	28
+
+/* SD_MaxCurCap reg (0x048) */
+#define CAP_CURR_3_3_M		BITFIELD_MASK(8)
+#define CAP_CURR_3_3_S		0
+#define CAP_CURR_3_0_M		BITFIELD_MASK(8)
+#define CAP_CURR_3_0_S		8
+#define CAP_CURR_1_8_M		BITFIELD_MASK(8)
+#define CAP_CURR_1_8_S		16
+
+/* SD_SysAddr: Offset 0x0000, Size 4 bytes */
+
+/* SD_BlockSize: Offset 0x004, Size 2 bytes */
+#define BLKSZ_BLKSZ_M		BITFIELD_MASK(12)
+#define BLKSZ_BLKSZ_S		0
+#define BLKSZ_BNDRY_M		BITFIELD_MASK(3)
+#define BLKSZ_BNDRY_S		12
+
+/* SD_BlockCount: Offset 0x006, size 2 bytes */
+
+/* SD_Arg0: Offset 0x008, size = 4 bytes  */
+/* SD_TransferMode Offset 0x00C, size = 2 bytes */
+#define XFER_DMA_ENABLE_M   	BITFIELD_MASK(1)
+#define XFER_DMA_ENABLE_S	0
+#define XFER_BLK_COUNT_EN_M 	BITFIELD_MASK(1)
+#define XFER_BLK_COUNT_EN_S	1
+#define XFER_CMD_12_EN_M    	BITFIELD_MASK(1)
+#define XFER_CMD_12_EN_S 	2
+#define XFER_DATA_DIRECTION_M	BITFIELD_MASK(1)
+#define XFER_DATA_DIRECTION_S	4
+#define XFER_MULTI_BLOCK_M	BITFIELD_MASK(1)
+#define XFER_MULTI_BLOCK_S	5
+
+/* SD_Command: Offset 0x00E, size = 2 bytes */
+/* resp_type field */
+#define RESP_TYPE_NONE 		0
+#define RESP_TYPE_136  		1
+#define RESP_TYPE_48   		2
+#define RESP_TYPE_48_BUSY	3
+/* type field */
+#define CMD_TYPE_NORMAL		0
+#define CMD_TYPE_SUSPEND	1
+#define CMD_TYPE_RESUME		2
+#define CMD_TYPE_ABORT		3
+
+#define CMD_RESP_TYPE_M		BITFIELD_MASK(2)	/* Bits [0-1] 	- Response type */
+#define CMD_RESP_TYPE_S		0
+#define CMD_CRC_EN_M		BITFIELD_MASK(1)	/* Bit 3 	- CRC enable */
+#define CMD_CRC_EN_S		3
+#define CMD_INDEX_EN_M		BITFIELD_MASK(1)	/* Bit 4 	- Enable index checking */
+#define CMD_INDEX_EN_S		4
+#define CMD_DATA_EN_M		BITFIELD_MASK(1)	/* Bit 5 	- Using DAT line */
+#define CMD_DATA_EN_S		5
+#define CMD_TYPE_M		BITFIELD_MASK(2)	/* Bit [6-7] 	- Normal, abort, resume, etc
+							 */
+#define CMD_TYPE_S		6
+#define CMD_INDEX_M		BITFIELD_MASK(6)	/* Bits [8-13] 	- Command number */
+#define CMD_INDEX_S		8
+
+/* SD_BufferDataPort0	: Offset 0x020, size = 2 or 4 bytes */
+/* SD_BufferDataPort1 	: Offset 0x022, size = 2 bytes */
+/* SD_PresentState	: Offset 0x024, size = 4 bytes */
+#define PRES_CMD_INHIBIT_M	BITFIELD_MASK(1)	/* Bit 0	May use CMD */
+#define PRES_CMD_INHIBIT_S	0
+#define PRES_DAT_INHIBIT_M	BITFIELD_MASK(1)	/* Bit 1	May use DAT */
+#define PRES_DAT_INHIBIT_S	1
+#define PRES_DAT_BUSY_M		BITFIELD_MASK(1)	/* Bit 2	DAT is busy */
+#define PRES_DAT_BUSY_S		2
+#define PRES_PRESENT_RSVD_M	BITFIELD_MASK(5)	/* Bit [3-7]	rsvd */
+#define PRES_PRESENT_RSVD_S	3
+#define PRES_WRITE_ACTIVE_M	BITFIELD_MASK(1)	/* Bit 8	Write is active */
+#define PRES_WRITE_ACTIVE_S	8
+#define PRES_READ_ACTIVE_M	BITFIELD_MASK(1)	/* Bit 9	Read is active */
+#define PRES_READ_ACTIVE_S	9
+#define PRES_WRITE_DATA_RDY_M	BITFIELD_MASK(1)	/* Bit 10	Write buf is avail */
+#define PRES_WRITE_DATA_RDY_S	10
+#define PRES_READ_DATA_RDY_M	BITFIELD_MASK(1)	/* Bit 11	Read buf data avail */
+#define PRES_READ_DATA_RDY_S	11
+#define PRES_CARD_PRESENT_M	BITFIELD_MASK(1)	/* Bit 16	Card present - debounced */
+#define PRES_CARD_PRESENT_S	16
+#define PRES_CARD_STABLE_M	BITFIELD_MASK(1)	/* Bit 17	Debugging */
+#define PRES_CARD_STABLE_S	17
+#define PRES_CARD_PRESENT_RAW_M	BITFIELD_MASK(1)	/* Bit 18	Not debounced */
+#define PRES_CARD_PRESENT_RAW_S	18
+#define PRES_WRITE_ENABLED_M	BITFIELD_MASK(1)	/* Bit 19	Write protected? */
+#define PRES_WRITE_ENABLED_S	19
+#define PRES_DAT_SIGNAL_M	BITFIELD_MASK(4)	/* Bit [20-23]	Debugging */
+#define PRES_DAT_SIGNAL_S	20
+#define PRES_CMD_SIGNAL_M	BITFIELD_MASK(1)	/* Bit 24	Debugging */
+#define PRES_CMD_SIGNAL_S	24
+
+/* SD_HostCntrl: Offset 0x028, size = 1 bytes */
+#define HOST_LED_M		BITFIELD_MASK(1)	/* Bit 0	LED On/Off */
+#define HOST_LED_S		0
+#define HOST_DATA_WIDTH_M	BITFIELD_MASK(1)	/* Bit 1	4 bit enable */
+#define HOST_DATA_WIDTH_S	1
+#define HOST_HI_SPEED_EN_M	BITFIELD_MASK(1)	/* Bit 2	High speed vs low speed */
+#define HOST_DMA_SEL_S		3
+#define HOST_DMA_SEL_M		BITFIELD_MASK(2)	/* Bit 4:3	DMA Select */
+#define HOST_HI_SPEED_EN_S	2
+
+/* misc defines */
+#define SD1_MODE 		0x1	/* SD Host Cntrlr Spec */
+#define SD4_MODE 		0x2	/* SD Host Cntrlr Spec */
+
+/* SD_PwrCntrl: Offset 0x029, size = 1 bytes */
+#define PWR_BUS_EN_M		BITFIELD_MASK(1)	/* Bit 0	Power the bus */
+#define PWR_BUS_EN_S		0
+#define PWR_VOLTS_M		BITFIELD_MASK(3)	/* Bit [1-3]	Voltage Select */
+#define PWR_VOLTS_S		1
+
+/* SD_SoftwareReset: Offset 0x02F, size = 1 byte */
+#define SW_RESET_ALL_M		BITFIELD_MASK(1)	/* Bit 0	Reset All */
+#define SW_RESET_ALL_S		0
+#define SW_RESET_CMD_M		BITFIELD_MASK(1)	/* Bit 1	CMD Line Reset */
+#define SW_RESET_CMD_S		1
+#define SW_RESET_DAT_M		BITFIELD_MASK(1)	/* Bit 2	DAT Line Reset */
+#define SW_RESET_DAT_S		2
+
+/* SD_IntrStatus: Offset 0x030, size = 2 bytes */
+/* Defs also serve SD_IntrStatusEnable and SD_IntrSignalEnable */
+#define INTSTAT_CMD_COMPLETE_M		BITFIELD_MASK(1)	/* Bit 0 */
+#define INTSTAT_CMD_COMPLETE_S		0
+#define INTSTAT_XFER_COMPLETE_M		BITFIELD_MASK(1)
+#define INTSTAT_XFER_COMPLETE_S		1
+#define INTSTAT_BLOCK_GAP_EVENT_M	BITFIELD_MASK(1)
+#define INTSTAT_BLOCK_GAP_EVENT_S	2
+#define INTSTAT_DMA_INT_M		BITFIELD_MASK(1)
+#define INTSTAT_DMA_INT_S		3
+#define INTSTAT_BUF_WRITE_READY_M	BITFIELD_MASK(1)
+#define INTSTAT_BUF_WRITE_READY_S	4
+#define INTSTAT_BUF_READ_READY_M	BITFIELD_MASK(1)
+#define INTSTAT_BUF_READ_READY_S	5
+#define INTSTAT_CARD_INSERTION_M	BITFIELD_MASK(1)
+#define INTSTAT_CARD_INSERTION_S	6
+#define INTSTAT_CARD_REMOVAL_M		BITFIELD_MASK(1)
+#define INTSTAT_CARD_REMOVAL_S		7
+#define INTSTAT_CARD_INT_M		BITFIELD_MASK(1)
+#define INTSTAT_CARD_INT_S		8
+#define INTSTAT_ERROR_INT_M		BITFIELD_MASK(1)	/* Bit 15 */
+#define INTSTAT_ERROR_INT_S		15
+
+/* SD_ErrorIntrStatus: Offset 0x032, size = 2 bytes */
+/* Defs also serve SD_ErrorIntrStatusEnable and SD_ErrorIntrSignalEnable */
+#define ERRINT_CMD_TIMEOUT_M		BITFIELD_MASK(1)
+#define ERRINT_CMD_TIMEOUT_S		0
+#define ERRINT_CMD_CRC_M		BITFIELD_MASK(1)
+#define ERRINT_CMD_CRC_S		1
+#define ERRINT_CMD_ENDBIT_M		BITFIELD_MASK(1)
+#define ERRINT_CMD_ENDBIT_S		2
+#define ERRINT_CMD_INDEX_M		BITFIELD_MASK(1)
+#define ERRINT_CMD_INDEX_S		3
+#define ERRINT_DATA_TIMEOUT_M		BITFIELD_MASK(1)
+#define ERRINT_DATA_TIMEOUT_S		4
+#define ERRINT_DATA_CRC_M		BITFIELD_MASK(1)
+#define ERRINT_DATA_CRC_S		5
+#define ERRINT_DATA_ENDBIT_M		BITFIELD_MASK(1)
+#define ERRINT_DATA_ENDBIT_S		6
+#define ERRINT_CURRENT_LIMIT_M		BITFIELD_MASK(1)
+#define ERRINT_CURRENT_LIMIT_S		7
+#define ERRINT_AUTO_CMD12_M		BITFIELD_MASK(1)
+#define ERRINT_AUTO_CMD12_S		8
+#define ERRINT_VENDOR_M			BITFIELD_MASK(4)
+#define ERRINT_VENDOR_S			12
+
+/* Also provide definitions in "normal" form to allow combined masks */
+#define ERRINT_CMD_TIMEOUT_BIT		0x0001
+#define ERRINT_CMD_CRC_BIT		0x0002
+#define ERRINT_CMD_ENDBIT_BIT		0x0004
+#define ERRINT_CMD_INDEX_BIT		0x0008
+#define ERRINT_DATA_TIMEOUT_BIT		0x0010
+#define ERRINT_DATA_CRC_BIT		0x0020
+#define ERRINT_DATA_ENDBIT_BIT		0x0040
+#define ERRINT_CURRENT_LIMIT_BIT	0x0080
+#define ERRINT_AUTO_CMD12_BIT		0x0100
+
+/* Masks to select CMD vs. DATA errors */
+#define ERRINT_CMD_ERRS		(ERRINT_CMD_TIMEOUT_BIT | ERRINT_CMD_CRC_BIT |\
+				 ERRINT_CMD_ENDBIT_BIT | ERRINT_CMD_INDEX_BIT)
+#define ERRINT_DATA_ERRS	(ERRINT_DATA_TIMEOUT_BIT | ERRINT_DATA_CRC_BIT |\
+				 ERRINT_DATA_ENDBIT_BIT)
+#define ERRINT_TRANSFER_ERRS	(ERRINT_CMD_ERRS | ERRINT_DATA_ERRS)
+
+/* SD_WakeupCntr_BlockGapCntrl : Offset 0x02A , size = bytes */
+/* SD_ClockCntrl	: Offset 0x02C , size = bytes */
+/* SD_SoftwareReset_TimeoutCntrl 	: Offset 0x02E , size = bytes */
+/* SD_IntrStatus	: Offset 0x030 , size = bytes */
+/* SD_ErrorIntrStatus 	: Offset 0x032 , size = bytes */
+/* SD_IntrStatusEnable	: Offset 0x034 , size = bytes */
+/* SD_ErrorIntrStatusEnable : Offset 0x036 , size = bytes */
+/* SD_IntrSignalEnable	: Offset 0x038 , size = bytes */
+/* SD_ErrorIntrSignalEnable : Offset 0x03A , size = bytes */
+/* SD_CMD12ErrorStatus	: Offset 0x03C , size = bytes */
+/* SD_Capabilities	: Offset 0x040 , size = bytes */
+/* SD_MaxCurCap		: Offset 0x048 , size = bytes */
+/* SD_MaxCurCap_Reserved: Offset 0x04C , size = bytes */
+/* SD_SlotInterruptStatus: Offset 0x0FC , size = bytes */
+/* SD_HostControllerVersion : Offset 0x0FE , size = bytes */
+
+#endif /* _SDIOH_H */
diff --git a/drivers/net/wireless/bcm4329/include/sdiovar.h b/drivers/net/wireless/bcm4329/include/sdiovar.h
new file mode 100644
index 000000000000..0179d4cb96db
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/sdiovar.h
@@ -0,0 +1,58 @@
+/*
+ * Structure used by apps whose drivers access SDIO drivers.
+ * Pulled out separately so dhdu and wlu can both use it.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sdiovar.h,v 13.5.14.2.16.2 2009/12/08 22:34:21 Exp $
+ */
+
+#ifndef _sdiovar_h_
+#define _sdiovar_h_
+
+#include <typedefs.h>
+
+/* require default structure packing */
+#define BWL_DEFAULT_PACKING
+#include <packed_section_start.h>
+
+typedef struct sdreg {
+	int func;
+	int offset;
+	int value;
+} sdreg_t;
+
+/* Common msglevel constants */
+#define SDH_ERROR_VAL		0x0001	/* Error */
+#define SDH_TRACE_VAL		0x0002	/* Trace */
+#define SDH_INFO_VAL		0x0004	/* Info */
+#define SDH_DEBUG_VAL		0x0008	/* Debug */
+#define SDH_DATA_VAL		0x0010	/* Data */
+#define SDH_CTRL_VAL		0x0020	/* Control Regs */
+#define SDH_LOG_VAL		0x0040	/* Enable bcmlog */
+#define SDH_DMA_VAL		0x0080	/* DMA */
+
+#define NUM_PREV_TRANSACTIONS	16
+
+
+#include <packed_section_end.h>
+
+#endif /* _sdiovar_h_ */
diff --git a/drivers/net/wireless/bcm4329/include/siutils.h b/drivers/net/wireless/bcm4329/include/siutils.h
new file mode 100644
index 000000000000..cb9f1407b73b
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/siutils.h
@@ -0,0 +1,235 @@
+/*
+ * Misc utility routines for accessing the SOC Interconnects
+ * of Broadcom HNBU chips.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: siutils.h,v 13.197.4.2.4.3.8.16 2010/06/23 21:36:05 Exp $
+ */
+
+
+#ifndef	_siutils_h_
+#define	_siutils_h_
+
+
+struct si_pub {
+	uint	socitype;		
+
+	uint	bustype;		
+	uint	buscoretype;		
+	uint	buscorerev;		
+	uint	buscoreidx;		
+	int	ccrev;			
+	uint32	cccaps;			
+	int	pmurev;			
+	uint32	pmucaps;		
+	uint	boardtype;		
+	uint	boardvendor;		
+	uint	boardflags;		
+	uint	chip;			
+	uint	chiprev;		
+	uint	chippkg;		
+	uint32	chipst;			
+	bool	issim;			
+	uint    socirev;		
+	bool	pci_pr32414;
+};
+
+#if defined(WLC_HIGH) && !defined(WLC_LOW)
+typedef struct si_pub si_t;
+#else
+typedef const struct si_pub si_t;
+#endif
+
+
+#define	SI_OSH		NULL	
+
+
+#define	XTAL			0x1	
+#define	PLL			0x2	
+
+
+#define	CLK_FAST		0	
+#define	CLK_DYNAMIC		2	
+
+
+#define GPIO_DRV_PRIORITY	0	
+#define GPIO_APP_PRIORITY	1	
+#define GPIO_HI_PRIORITY	2	
+
+
+#define GPIO_PULLUP		0
+#define GPIO_PULLDN		1
+
+
+#define GPIO_REGEVT		0	
+#define GPIO_REGEVT_INTMSK	1	
+#define GPIO_REGEVT_INTPOL	2	
+
+
+#define SI_DEVPATH_BUFSZ	16	
+
+
+#define	SI_DOATTACH	1
+#define SI_PCIDOWN	2
+#define SI_PCIUP	3
+
+#define	ISSIM_ENAB(sih)	0
+
+
+#if defined(BCMPMUCTL)
+#define PMUCTL_ENAB(sih)	(BCMPMUCTL)
+#else
+#define PMUCTL_ENAB(sih)	((sih)->cccaps & CC_CAP_PMU)
+#endif
+
+
+#if defined(BCMPMUCTL) && BCMPMUCTL
+#define CCCTL_ENAB(sih)		(0)
+#define CCPLL_ENAB(sih)		(0)
+#else
+#define CCCTL_ENAB(sih)		((sih)->cccaps & CC_CAP_PWR_CTL)
+#define CCPLL_ENAB(sih)		((sih)->cccaps & CC_CAP_PLL_MASK)
+#endif
+
+typedef void (*gpio_handler_t)(uint32 stat, void *arg);
+
+
+
+extern si_t *si_attach(uint pcidev, osl_t *osh, void *regs, uint bustype,
+                       void *sdh, char **vars, uint *varsz);
+extern si_t *si_kattach(osl_t *osh);
+extern void si_detach(si_t *sih);
+extern bool si_pci_war16165(si_t *sih);
+
+extern uint si_corelist(si_t *sih, uint coreid[]);
+extern uint si_coreid(si_t *sih);
+extern uint si_flag(si_t *sih);
+extern uint si_intflag(si_t *sih);
+extern uint si_coreidx(si_t *sih);
+extern uint si_coreunit(si_t *sih);
+extern uint si_corevendor(si_t *sih);
+extern uint si_corerev(si_t *sih);
+extern void *si_osh(si_t *sih);
+extern void si_setosh(si_t *sih, osl_t *osh);
+extern uint si_corereg(si_t *sih, uint coreidx, uint regoff, uint mask, uint val);
+extern void *si_coreregs(si_t *sih);
+extern void si_write_wrapperreg(si_t *sih, uint32 offset, uint32 val);
+extern uint32 si_core_cflags(si_t *sih, uint32 mask, uint32 val);
+extern void si_core_cflags_wo(si_t *sih, uint32 mask, uint32 val);
+extern uint32 si_core_sflags(si_t *sih, uint32 mask, uint32 val);
+extern bool si_iscoreup(si_t *sih);
+extern uint si_findcoreidx(si_t *sih, uint coreid, uint coreunit);
+extern void *si_setcoreidx(si_t *sih, uint coreidx);
+extern void *si_setcore(si_t *sih, uint coreid, uint coreunit);
+extern void *si_switch_core(si_t *sih, uint coreid, uint *origidx, uint *intr_val);
+extern void si_restore_core(si_t *sih, uint coreid, uint intr_val);
+extern int si_numaddrspaces(si_t *sih);
+extern uint32 si_addrspace(si_t *sih, uint asidx);
+extern uint32 si_addrspacesize(si_t *sih, uint asidx);
+extern int si_corebist(si_t *sih);
+extern void si_core_reset(si_t *sih, uint32 bits, uint32 resetbits);
+extern void si_core_tofixup(si_t *sih);
+extern void si_core_disable(si_t *sih, uint32 bits);
+extern uint32 si_clock_rate(uint32 pll_type, uint32 n, uint32 m);
+extern uint32 si_clock(si_t *sih);
+extern void si_clock_pmu_spuravoid(si_t *sih, bool spuravoid);
+extern uint32 si_alp_clock(si_t *sih);
+extern uint32 si_ilp_clock(si_t *sih);
+extern void si_pci_setup(si_t *sih, uint coremask);
+extern void si_pcmcia_init(si_t *sih);
+extern void si_setint(si_t *sih, int siflag);
+extern bool si_backplane64(si_t *sih);
+extern void si_register_intr_callback(si_t *sih, void *intrsoff_fn, void *intrsrestore_fn,
+	void *intrsenabled_fn, void *intr_arg);
+extern void si_deregister_intr_callback(si_t *sih);
+extern void si_clkctl_init(si_t *sih);
+extern uint16 si_clkctl_fast_pwrup_delay(si_t *sih);
+extern bool si_clkctl_cc(si_t *sih, uint mode);
+extern int si_clkctl_xtal(si_t *sih, uint what, bool on);
+extern uint32 si_gpiotimerval(si_t *sih, uint32 mask, uint32 val);
+extern bool si_backplane64(si_t *sih);
+extern void si_btcgpiowar(si_t *sih);
+extern bool si_deviceremoved(si_t *sih);
+extern uint32 si_socram_size(si_t *sih);
+
+extern void si_watchdog(si_t *sih, uint ticks);
+extern void si_watchdog_ms(si_t *sih, uint32 ms);
+extern void *si_gpiosetcore(si_t *sih);
+extern uint32 si_gpiocontrol(si_t *sih, uint32 mask, uint32 val, uint8 priority);
+extern uint32 si_gpioouten(si_t *sih, uint32 mask, uint32 val, uint8 priority);
+extern uint32 si_gpioout(si_t *sih, uint32 mask, uint32 val, uint8 priority);
+extern uint32 si_gpioin(si_t *sih);
+extern uint32 si_gpiointpolarity(si_t *sih, uint32 mask, uint32 val, uint8 priority);
+extern uint32 si_gpiointmask(si_t *sih, uint32 mask, uint32 val, uint8 priority);
+extern uint32 si_gpioled(si_t *sih, uint32 mask, uint32 val);
+extern uint32 si_gpioreserve(si_t *sih, uint32 gpio_num, uint8 priority);
+extern uint32 si_gpiorelease(si_t *sih, uint32 gpio_num, uint8 priority);
+extern uint32 si_gpiopull(si_t *sih, bool updown, uint32 mask, uint32 val);
+extern uint32 si_gpioevent(si_t *sih, uint regtype, uint32 mask, uint32 val);
+extern uint32 si_gpio_int_enable(si_t *sih, bool enable);
+
+
+extern void *si_gpio_handler_register(si_t *sih, uint32 e, bool lev, gpio_handler_t cb, void *arg);
+extern void si_gpio_handler_unregister(si_t *sih, void* gpioh);
+extern void si_gpio_handler_process(si_t *sih);
+
+
+extern bool si_pci_pmecap(si_t *sih);
+struct osl_info;
+extern bool si_pci_fastpmecap(struct osl_info *osh);
+extern bool si_pci_pmeclr(si_t *sih);
+extern void si_pci_pmeen(si_t *sih);
+extern uint si_pcie_readreg(void *sih, uint addrtype, uint offset);
+
+extern void si_sdio_init(si_t *sih);
+
+extern uint16 si_d11_devid(si_t *sih);
+extern int si_corepciid(si_t *sih, uint func, uint16 *pcivendor, uint16 *pcidevice,
+	uint8 *pciclass, uint8 *pcisubclass, uint8 *pciprogif, uint8 *pciheader);
+
+#define si_eci_init(sih) (0)
+#define si_eci_notify_bt(sih, type, val, interrupt)  (0)
+
+
+
+extern int si_devpath(si_t *sih, char *path, int size);
+
+extern char *si_getdevpathvar(si_t *sih, const char *name);
+extern int si_getdevpathintvar(si_t *sih, const char *name);
+
+
+extern uint8 si_pcieclkreq(si_t *sih, uint32 mask, uint32 val);
+extern void si_war42780_clkreq(si_t *sih, bool clkreq);
+extern void si_pci_sleep(si_t *sih);
+extern void si_pci_down(si_t *sih);
+extern void si_pci_up(si_t *sih);
+extern void si_pcie_war_ovr_disable(si_t *sih);
+extern void si_pcie_extendL1timer(si_t *sih, bool extend);
+extern int si_pci_fixcfg(si_t *sih);
+
+
+
+
+
+
+
+#endif	
diff --git a/drivers/net/wireless/bcm4329/include/spid.h b/drivers/net/wireless/bcm4329/include/spid.h
new file mode 100644
index 000000000000..c740296de9af
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/spid.h
@@ -0,0 +1,153 @@
+/*
+ * SPI device spec header file
+ *
+ * Copyright (C) 2010, Broadcom Corporation
+ * All Rights Reserved.
+ * 
+ * This is UNPUBLISHED PROPRIETARY SOURCE CODE of Broadcom Corporation;
+ * the contents of this file may not be disclosed to third parties, copied
+ * or duplicated in any form, in whole or in part, without the prior
+ * written permission of Broadcom Corporation.
+ *
+ * $Id: spid.h,v 1.7.10.1.16.3 2009/04/09 19:23:14 Exp $
+ */
+
+#ifndef	_SPI_H
+#define	_SPI_H
+
+/*
+ * Brcm SPI Device Register Map.
+ *
+ */
+
+typedef volatile struct {
+	uint8	config;			/* 0x00, len, endian, clock, speed, polarity, wakeup */
+	uint8	response_delay;		/* 0x01, read response delay in bytes (corerev < 3) */
+	uint8	status_enable;		/* 0x02, status-enable, intr with status, response_delay
+					 * function selection, command/data error check
+					 */
+	uint8	reset_bp;		/* 0x03, reset on wlan/bt backplane reset (corerev >= 1) */
+	uint16	intr_reg;		/* 0x04, Intr status register */
+	uint16	intr_en_reg;		/* 0x06, Intr mask register */
+	uint32	status_reg;		/* 0x08, RO, Status bits of last spi transfer */
+	uint16	f1_info_reg;		/* 0x0c, RO, enabled, ready for data transfer, blocksize */
+	uint16	f2_info_reg;		/* 0x0e, RO, enabled, ready for data transfer, blocksize */
+	uint16	f3_info_reg;		/* 0x10, RO, enabled, ready for data transfer, blocksize */
+	uint32	test_read;		/* 0x14, RO 0xfeedbead signature */
+	uint32	test_rw;		/* 0x18, RW */
+	uint8	resp_delay_f0;		/* 0x1c, read resp delay bytes for F0 (corerev >= 3) */
+	uint8	resp_delay_f1;		/* 0x1d, read resp delay bytes for F1 (corerev >= 3) */
+	uint8	resp_delay_f2;		/* 0x1e, read resp delay bytes for F2 (corerev >= 3) */
+	uint8	resp_delay_f3;		/* 0x1f, read resp delay bytes for F3 (corerev >= 3) */
+} spi_regs_t;
+
+/* SPI device register offsets */
+#define SPID_CONFIG			0x00
+#define SPID_RESPONSE_DELAY		0x01
+#define SPID_STATUS_ENABLE		0x02
+#define SPID_RESET_BP			0x03	/* (corerev >= 1) */
+#define SPID_INTR_REG			0x04	/* 16 bits - Interrupt status */
+#define SPID_INTR_EN_REG		0x06	/* 16 bits - Interrupt mask */
+#define SPID_STATUS_REG			0x08	/* 32 bits */
+#define SPID_F1_INFO_REG		0x0C	/* 16 bits */
+#define SPID_F2_INFO_REG		0x0E	/* 16 bits */
+#define SPID_F3_INFO_REG		0x10	/* 16 bits */
+#define SPID_TEST_READ			0x14	/* 32 bits */
+#define SPID_TEST_RW			0x18	/* 32 bits */
+#define SPID_RESP_DELAY_F0		0x1c	/* 8 bits (corerev >= 3) */
+#define SPID_RESP_DELAY_F1		0x1d	/* 8 bits (corerev >= 3) */
+#define SPID_RESP_DELAY_F2		0x1e	/* 8 bits (corerev >= 3) */
+#define SPID_RESP_DELAY_F3		0x1f	/* 8 bits (corerev >= 3) */
+
+/* Bit masks for SPID_CONFIG device register */
+#define WORD_LENGTH_32	0x1	/* 0/1 16/32 bit word length */
+#define ENDIAN_BIG	0x2	/* 0/1 Little/Big Endian */
+#define CLOCK_PHASE	0x4	/* 0/1 clock phase delay */
+#define CLOCK_POLARITY	0x8	/* 0/1 Idle state clock polarity is low/high */
+#define HIGH_SPEED_MODE	0x10	/* 1/0 High Speed mode / Normal mode */
+#define INTR_POLARITY	0x20	/* 1/0 Interrupt active polarity is high/low */
+#define WAKE_UP		0x80	/* 0/1 Wake-up command from Host to WLAN */
+
+/* Bit mask for SPID_RESPONSE_DELAY device register */
+#define RESPONSE_DELAY_MASK	0xFF	/* Configurable rd response delay in multiples of 8 bits */
+
+/* Bit mask for SPID_STATUS_ENABLE device register */
+#define STATUS_ENABLE		0x1	/* 1/0 Status sent/not sent to host after read/write */
+#define INTR_WITH_STATUS	0x2	/* 0/1 Do-not / do-interrupt if status is sent */
+#define RESP_DELAY_ALL		0x4	/* Applicability of resp delay to F1 or all func's read */
+#define DWORD_PKT_LEN_EN	0x8	/* Packet len denoted in dwords instead of bytes */
+#define CMD_ERR_CHK_EN		0x20	/* Command error check enable */
+#define DATA_ERR_CHK_EN		0x40	/* Data error check enable */
+
+/* Bit mask for SPID_RESET_BP device register */
+#define RESET_ON_WLAN_BP_RESET	0x4	/* enable reset for WLAN backplane */
+#define RESET_ON_BT_BP_RESET	0x8	/* enable reset for BT backplane */
+#define RESET_SPI		0x80	/* reset the above enabled logic */
+
+/* Bit mask for SPID_INTR_REG device register */
+#define DATA_UNAVAILABLE	0x0001	/* Requested data not available; Clear by writing a "1" */
+#define F2_F3_FIFO_RD_UNDERFLOW	0x0002
+#define F2_F3_FIFO_WR_OVERFLOW	0x0004
+#define COMMAND_ERROR		0x0008	/* Cleared by writing 1 */
+#define DATA_ERROR		0x0010	/* Cleared by writing 1 */
+#define F2_PACKET_AVAILABLE	0x0020
+#define F3_PACKET_AVAILABLE	0x0040
+#define F1_OVERFLOW		0x0080	/* Due to last write. Bkplane has pending write requests */
+#define MISC_INTR0		0x0100
+#define MISC_INTR1		0x0200
+#define MISC_INTR2		0x0400
+#define MISC_INTR3		0x0800
+#define MISC_INTR4		0x1000
+#define F1_INTR			0x2000
+#define F2_INTR			0x4000
+#define F3_INTR			0x8000
+
+/* Bit mask for 32bit SPID_STATUS_REG device register */
+#define STATUS_DATA_NOT_AVAILABLE	0x00000001
+#define STATUS_UNDERFLOW		0x00000002
+#define STATUS_OVERFLOW			0x00000004
+#define STATUS_F2_INTR			0x00000008
+#define STATUS_F3_INTR			0x00000010
+#define STATUS_F2_RX_READY		0x00000020
+#define STATUS_F3_RX_READY		0x00000040
+#define STATUS_HOST_CMD_DATA_ERR	0x00000080
+#define STATUS_F2_PKT_AVAILABLE		0x00000100
+#define STATUS_F2_PKT_LEN_MASK		0x000FFE00
+#define STATUS_F2_PKT_LEN_SHIFT		9
+#define STATUS_F3_PKT_AVAILABLE		0x00100000
+#define STATUS_F3_PKT_LEN_MASK		0xFFE00000
+#define STATUS_F3_PKT_LEN_SHIFT		21
+
+/* Bit mask for 16 bits SPID_F1_INFO_REG device register */
+#define F1_ENABLED 			0x0001
+#define F1_RDY_FOR_DATA_TRANSFER	0x0002
+#define F1_MAX_PKT_SIZE			0x01FC
+
+/* Bit mask for 16 bits SPID_F2_INFO_REG device register */
+#define F2_ENABLED 			0x0001
+#define F2_RDY_FOR_DATA_TRANSFER	0x0002
+#define F2_MAX_PKT_SIZE			0x3FFC
+
+/* Bit mask for 16 bits SPID_F3_INFO_REG device register */
+#define F3_ENABLED 			0x0001
+#define F3_RDY_FOR_DATA_TRANSFER	0x0002
+#define F3_MAX_PKT_SIZE			0x3FFC
+
+/* Bit mask for 32 bits SPID_TEST_READ device register read in 16bit LE mode */
+#define TEST_RO_DATA_32BIT_LE		0xFEEDBEAD
+
+/* Maximum number of I/O funcs */
+#define SPI_MAX_IOFUNCS		4
+
+#define SPI_MAX_PKT_LEN		(2048*4)
+
+/* Misc defines */
+#define SPI_FUNC_0		0
+#define SPI_FUNC_1		1
+#define SPI_FUNC_2		2
+#define SPI_FUNC_3		3
+
+#define WAIT_F2RXFIFORDY	100
+#define WAIT_F2RXFIFORDY_DELAY	20
+
+#endif /* _SPI_H */
diff --git a/drivers/net/wireless/bcm4329/include/trxhdr.h b/drivers/net/wireless/bcm4329/include/trxhdr.h
new file mode 100644
index 000000000000..8f5eed9410eb
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/trxhdr.h
@@ -0,0 +1,46 @@
+/*
+ * TRX image file header format.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: trxhdr.h,v 13.11.310.1 2008/08/17 12:58:58 Exp $
+ */
+
+#include <typedefs.h>
+
+#define TRX_MAGIC	0x30524448	/* "HDR0" */
+#define TRX_VERSION	1		/* Version 1 */
+#define TRX_MAX_LEN	0x3A0000	/* Max length */
+#define TRX_NO_HEADER	1		/* Do not write TRX header */
+#define TRX_GZ_FILES	0x2     /* Contains up to TRX_MAX_OFFSET individual gzip files */
+#define TRX_MAX_OFFSET	3		/* Max number of individual files */
+#define TRX_UNCOMP_IMAGE	0x20	/* Trx contains uncompressed rtecdc.bin image */
+
+struct trx_header {
+	uint32 magic;		/* "HDR0" */
+	uint32 len;		/* Length of file including header */
+	uint32 crc32;		/* 32-bit CRC from flag_version to end of file */
+	uint32 flag_version;	/* 0:15 flags, 16:31 version */
+	uint32 offsets[TRX_MAX_OFFSET];	/* Offsets of partitions from start of header */
+};
+
+/* Compatibility */
+typedef struct trx_header TRXHDR, *PTRXHDR;
diff --git a/drivers/net/wireless/bcm4329/include/typedefs.h b/drivers/net/wireless/bcm4329/include/typedefs.h
new file mode 100644
index 000000000000..4d9dd761ed64
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/typedefs.h
@@ -0,0 +1,303 @@
+/*
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: typedefs.h,v 1.85.34.1.2.5 2009/01/27 04:09:40 Exp $
+ */
+
+
+#ifndef _TYPEDEFS_H_
+#define _TYPEDEFS_H_
+
+#ifdef SITE_TYPEDEFS
+
+
+
+#include "site_typedefs.h"
+
+#else
+
+
+
+#ifdef __cplusplus
+
+#define TYPEDEF_BOOL
+#ifndef FALSE
+#define FALSE	false
+#endif
+#ifndef TRUE
+#define TRUE	true
+#endif
+
+#else	
+
+
+#endif	
+
+#if defined(__x86_64__)
+#define TYPEDEF_UINTPTR
+typedef unsigned long long int uintptr;
+#endif
+
+
+
+
+#if defined(TARGETOS_nucleus)
+
+#include <stddef.h>
+
+
+#define TYPEDEF_FLOAT_T
+#endif   
+
+#if defined(_NEED_SIZE_T_)
+typedef long unsigned int size_t;
+#endif
+
+#ifdef __DJGPP__
+typedef long unsigned int size_t;
+#endif 
+
+
+
+
+
+#define TYPEDEF_UINT
+#ifndef TARGETENV_android
+#define TYPEDEF_USHORT
+#define TYPEDEF_ULONG
+#endif 
+#ifdef __KERNEL__
+#include <linux/version.h>
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19))
+#define TYPEDEF_BOOL
+#endif	
+#endif	
+
+
+
+
+
+#if defined(__GNUC__) && defined(__STRICT_ANSI__)
+#define TYPEDEF_INT64
+#define TYPEDEF_UINT64
+#endif
+
+
+#if defined(__ICL)
+
+#define TYPEDEF_INT64
+
+#if defined(__STDC__)
+#define TYPEDEF_UINT64
+#endif
+
+#endif 
+
+#if !defined(__DJGPP__) && !defined(TARGETOS_nucleus)
+
+
+#if defined(__KERNEL__)
+
+#include <linux/types.h>	
+
+#else
+
+
+#include <sys/types.h>
+
+#endif 
+
+#endif 
+
+
+
+
+#define USE_TYPEDEF_DEFAULTS
+
+#endif 
+
+
+
+
+#ifdef USE_TYPEDEF_DEFAULTS
+#undef USE_TYPEDEF_DEFAULTS
+
+#ifndef TYPEDEF_BOOL
+typedef	 unsigned char	bool;
+#endif
+
+
+
+#ifndef TYPEDEF_UCHAR
+typedef unsigned char	uchar;
+#endif
+
+#ifndef TYPEDEF_USHORT
+typedef unsigned short	ushort;
+#endif
+
+#ifndef TYPEDEF_UINT
+typedef unsigned int	uint;
+#endif
+
+#ifndef TYPEDEF_ULONG
+typedef unsigned long	ulong;
+#endif
+
+
+
+#ifndef TYPEDEF_UINT8
+typedef unsigned char	uint8;
+#endif
+
+#ifndef TYPEDEF_UINT16
+typedef unsigned short	uint16;
+#endif
+
+#ifndef TYPEDEF_UINT32
+typedef unsigned int	uint32;
+#endif
+
+#ifndef TYPEDEF_UINT64
+typedef unsigned long long uint64;
+#endif
+
+#ifndef TYPEDEF_UINTPTR
+typedef unsigned int	uintptr;
+#endif
+
+#ifndef TYPEDEF_INT8
+typedef signed char	int8;
+#endif
+
+#ifndef TYPEDEF_INT16
+typedef signed short	int16;
+#endif
+
+#ifndef TYPEDEF_INT32
+typedef signed int	int32;
+#endif
+
+#ifndef TYPEDEF_INT64
+typedef signed long long int64;
+#endif
+
+
+
+#ifndef TYPEDEF_FLOAT32
+typedef float		float32;
+#endif
+
+#ifndef TYPEDEF_FLOAT64
+typedef double		float64;
+#endif
+
+
+
+#ifndef TYPEDEF_FLOAT_T
+
+#if defined(FLOAT32)
+typedef float32 float_t;
+#else 
+typedef float64 float_t;
+#endif
+
+#endif 
+
+
+
+#ifndef FALSE
+#define FALSE	0
+#endif
+
+#ifndef TRUE
+#define TRUE	1  
+#endif
+
+#ifndef NULL
+#define	NULL	0
+#endif
+
+#ifndef OFF
+#define	OFF	0
+#endif
+
+#ifndef ON
+#define	ON	1  
+#endif
+
+#define	AUTO	(-1) 
+
+
+
+#ifndef PTRSZ
+#define	PTRSZ	sizeof(char*)
+#endif
+
+
+
+#if defined(__GNUC__)
+	#define BWL_COMPILER_GNU
+#elif defined(__CC_ARM)
+	#define BWL_COMPILER_ARMCC
+#else
+	#error "Unknown compiler!"
+#endif 
+
+
+#ifndef INLINE
+	#if defined(BWL_COMPILER_MICROSOFT)
+		#define INLINE __inline
+	#elif defined(BWL_COMPILER_GNU)
+		#define INLINE __inline__
+	#elif defined(BWL_COMPILER_ARMCC)
+		#define INLINE	__inline
+	#else
+		#define INLINE
+	#endif 
+#endif 
+
+#undef TYPEDEF_BOOL
+#undef TYPEDEF_UCHAR
+#undef TYPEDEF_USHORT
+#undef TYPEDEF_UINT
+#undef TYPEDEF_ULONG
+#undef TYPEDEF_UINT8
+#undef TYPEDEF_UINT16
+#undef TYPEDEF_UINT32
+#undef TYPEDEF_UINT64
+#undef TYPEDEF_UINTPTR
+#undef TYPEDEF_INT8
+#undef TYPEDEF_INT16
+#undef TYPEDEF_INT32
+#undef TYPEDEF_INT64
+#undef TYPEDEF_FLOAT32
+#undef TYPEDEF_FLOAT64
+#undef TYPEDEF_FLOAT_T
+
+#endif 
+
+
+#define UNUSED_PARAMETER(x) (void)(x)
+
+
+#include <bcmdefs.h>
+
+#endif 
diff --git a/drivers/net/wireless/bcm4329/include/wlioctl.h b/drivers/net/wireless/bcm4329/include/wlioctl.h
new file mode 100644
index 000000000000..00c61f10782f
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/include/wlioctl.h
@@ -0,0 +1,1673 @@
+/*
+ * Custom OID/ioctl definitions for
+ * Broadcom 802.11abg Networking Device Driver
+ *
+ * Definitions subject to change without notice.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wlioctl.h,v 1.601.4.15.2.14.2.62.4.3 2011/02/09 23:31:02 Exp $
+ */
+
+
+#ifndef _wlioctl_h_
+#define	_wlioctl_h_
+
+#include <typedefs.h>
+#include <proto/ethernet.h>
+#include <proto/bcmeth.h>
+#include <proto/bcmevent.h>
+#include <proto/802.11.h>
+#include <bcmwifi.h>
+
+
+
+#define ACTION_FRAME_SIZE 1040
+
+typedef struct wl_action_frame {
+	struct ether_addr	da;
+	uint16				len;
+	uint32				packetId;
+	uint8				data[ACTION_FRAME_SIZE];
+} wl_action_frame_t;
+
+#define WL_WIFI_ACTION_FRAME_SIZE sizeof(struct wl_action_frame)
+
+
+#define BWL_DEFAULT_PACKING
+#include <packed_section_start.h>
+
+#define RWL_ACTION_WIFI_CATEGORY	127  
+#define RWL_WIFI_OUI_BYTE1		0x90 
+#define RWL_WIFI_OUI_BYTE2		0x4C
+#define RWL_WIFI_OUI_BYTE3		0x0F
+#define RWL_WIFI_ACTION_FRAME_SIZE	sizeof(struct dot11_action_wifi_vendor_specific)
+#define RWL_WIFI_DEFAULT                0x00
+#define RWL_WIFI_FIND_MY_PEER		0x09 
+#define RWL_WIFI_FOUND_PEER		0x0A 
+#define RWL_ACTION_WIFI_FRAG_TYPE	0x55 
+
+typedef struct ssid_info
+{
+	uint8		ssid_len;		
+	uint8		ssid[32];		
+} ssid_info_t;
+
+typedef struct cnt_rx
+{
+	uint32 cnt_rxundec;
+	uint32 cnt_rxframe;
+} cnt_rx_t;
+
+
+
+#define RWL_REF_MAC_ADDRESS_OFFSET	17
+#define RWL_DUT_MAC_ADDRESS_OFFSET	23
+#define RWL_WIFI_CLIENT_CHANNEL_OFFSET	50
+#define RWL_WIFI_SERVER_CHANNEL_OFFSET	51
+
+
+
+
+
+#define	WL_BSS_INFO_VERSION	108		
+
+
+typedef struct wl_bss_info {
+	uint32		version;		
+	uint32		length;			
+	struct ether_addr BSSID;
+	uint16		beacon_period;		
+	uint16		capability;		
+	uint8		SSID_len;
+	uint8		SSID[32];
+	struct {
+		uint	count;			
+		uint8	rates[16];		
+	} rateset;				
+	chanspec_t	chanspec;		
+	uint16		atim_window;		
+	uint8		dtim_period;		
+	int16		RSSI;			
+	int8		phy_noise;		
+
+	uint8		n_cap;			
+	uint32		nbss_cap;		
+	uint8		ctl_ch;			
+	uint32		reserved32[1];		
+	uint8		flags;			
+	uint8		reserved[3];		
+	uint8		basic_mcs[MCSSET_LEN];	
+
+	uint16		ie_offset;		
+	uint32		ie_length;		
+	
+	
+} wl_bss_info_t;
+
+typedef struct wlc_ssid {
+	uint32		SSID_len;
+	uchar		SSID[32];
+} wlc_ssid_t;
+
+
+#define WL_BSSTYPE_INFRA 1
+#define WL_BSSTYPE_INDEP 0
+#define WL_BSSTYPE_ANY   2
+
+
+#define WL_SCANFLAGS_PASSIVE 0x01
+#define WL_SCANFLAGS_PROHIBITED	0x04
+
+typedef struct wl_scan_params {
+	wlc_ssid_t ssid;		
+	struct ether_addr bssid;	
+	int8 bss_type;			
+	int8 scan_type;		
+	int32 nprobes;			
+	int32 active_time;		
+	int32 passive_time;		
+	int32 home_time;		
+	int32 channel_num;		
+	uint16 channel_list[1];		
+} wl_scan_params_t;
+
+#define WL_SCAN_PARAMS_FIXED_SIZE 64
+
+
+#define WL_SCAN_PARAMS_COUNT_MASK 0x0000ffff
+#define WL_SCAN_PARAMS_NSSID_SHIFT 16
+
+#define WL_SCAN_ACTION_START      1
+#define WL_SCAN_ACTION_CONTINUE   2
+#define WL_SCAN_ACTION_ABORT      3
+
+#define ISCAN_REQ_VERSION 1
+
+
+typedef struct wl_iscan_params {
+	uint32 version;
+	uint16 action;
+	uint16 scan_duration;
+	wl_scan_params_t params;
+} wl_iscan_params_t;
+
+#define WL_ISCAN_PARAMS_FIXED_SIZE (OFFSETOF(wl_iscan_params_t, params) + sizeof(wlc_ssid_t))
+
+typedef struct wl_scan_results {
+	uint32 buflen;
+	uint32 version;
+	uint32 count;
+	wl_bss_info_t bss_info[1];
+} wl_scan_results_t;
+
+#define WL_SCAN_RESULTS_FIXED_SIZE 12
+
+
+#define WL_SCAN_RESULTS_SUCCESS	0
+#define WL_SCAN_RESULTS_PARTIAL	1
+#define WL_SCAN_RESULTS_PENDING	2
+#define WL_SCAN_RESULTS_ABORTED	3
+#define WL_SCAN_RESULTS_NO_MEM	4
+
+#define ESCAN_REQ_VERSION 1
+
+typedef struct wl_escan_params {
+	uint32 version;
+	uint16 action;
+	uint16 sync_id;
+	wl_scan_params_t params;
+} wl_escan_params_t;
+
+#define WL_ESCAN_PARAMS_FIXED_SIZE (OFFSETOF(wl_escan_params_t, params) + sizeof(wlc_ssid_t))
+
+typedef struct wl_escan_result {
+	uint32 buflen;
+	uint32 version;
+	uint16 sync_id;
+	uint16 bss_count;
+	wl_bss_info_t bss_info[1];
+} wl_escan_result_t;
+
+#define WL_ESCAN_RESULTS_FIXED_SIZE (sizeof(wl_escan_result_t) - sizeof(wl_bss_info_t))
+
+
+typedef struct wl_iscan_results {
+	uint32 status;
+	wl_scan_results_t results;
+} wl_iscan_results_t;
+
+#define WL_ISCAN_RESULTS_FIXED_SIZE \
+	(WL_SCAN_RESULTS_FIXED_SIZE + OFFSETOF(wl_iscan_results_t, results))
+
+#define WL_NUMRATES		16	
+typedef struct wl_rateset {
+	uint32	count;			
+	uint8	rates[WL_NUMRATES];	
+} wl_rateset_t;
+
+
+typedef struct wl_uint32_list {
+	
+	uint32 count;
+	
+	uint32 element[1];
+} wl_uint32_list_t;
+
+
+typedef struct wl_assoc_params {
+	struct ether_addr bssid;	
+	uint16 bssid_cnt;		
+	int32 chanspec_num;		
+	chanspec_t chanspec_list[1];	
+} wl_assoc_params_t;
+#define WL_ASSOC_PARAMS_FIXED_SIZE 	(sizeof(wl_assoc_params_t) - sizeof(chanspec_t))
+
+
+typedef wl_assoc_params_t wl_reassoc_params_t;
+#define WL_REASSOC_PARAMS_FIXED_SIZE	WL_ASSOC_PARAMS_FIXED_SIZE
+
+
+typedef struct wl_join_params {
+	wlc_ssid_t ssid;
+	wl_assoc_params_t params;	
+} wl_join_params_t;
+#define WL_JOIN_PARAMS_FIXED_SIZE 	(sizeof(wl_join_params_t) - sizeof(chanspec_t))
+
+#define WLC_CNTRY_BUF_SZ	4		
+
+typedef struct wl_country {
+	char country_abbrev[WLC_CNTRY_BUF_SZ];
+	int32 rev;
+	char ccode[WLC_CNTRY_BUF_SZ];
+} wl_country_t;
+
+typedef enum sup_auth_status {
+	
+	WLC_SUP_DISCONNECTED = 0,
+	WLC_SUP_CONNECTING,
+	WLC_SUP_IDREQUIRED,
+	WLC_SUP_AUTHENTICATING,
+	WLC_SUP_AUTHENTICATED,
+	WLC_SUP_KEYXCHANGE,
+	WLC_SUP_KEYED,
+	WLC_SUP_TIMEOUT,
+	WLC_SUP_LAST_BASIC_STATE,
+
+	
+	WLC_SUP_KEYXCHANGE_WAIT_M1 = WLC_SUP_AUTHENTICATED,
+	                                
+	WLC_SUP_KEYXCHANGE_PREP_M2 = WLC_SUP_KEYXCHANGE,
+	                                
+	WLC_SUP_KEYXCHANGE_WAIT_M3 = WLC_SUP_LAST_BASIC_STATE,
+	                                
+	WLC_SUP_KEYXCHANGE_PREP_M4,		
+	WLC_SUP_KEYXCHANGE_WAIT_G1,		
+	WLC_SUP_KEYXCHANGE_PREP_G2		
+} sup_auth_status_t;
+
+
+#define	CRYPTO_ALGO_OFF			0
+#define	CRYPTO_ALGO_WEP1		1
+#define	CRYPTO_ALGO_TKIP		2
+#define	CRYPTO_ALGO_WEP128		3
+#define CRYPTO_ALGO_AES_CCM		4
+#define CRYPTO_ALGO_AES_OCB_MSDU	5
+#define CRYPTO_ALGO_AES_OCB_MPDU	6
+#define CRYPTO_ALGO_NALG		7
+#define CRYPTO_ALGO_SMS4		11
+
+#define WSEC_GEN_MIC_ERROR	0x0001
+#define WSEC_GEN_REPLAY		0x0002
+#define WSEC_GEN_ICV_ERROR	0x0004
+
+#define WL_SOFT_KEY	(1 << 0)	
+#define WL_PRIMARY_KEY	(1 << 1)	
+#define WL_KF_RES_4	(1 << 4)	
+#define WL_KF_RES_5	(1 << 5)	
+#define WL_IBSS_PEER_GROUP_KEY	(1 << 6)	
+
+typedef struct wl_wsec_key {
+	uint32		index;		
+	uint32		len;		
+	uint8		data[DOT11_MAX_KEY_SIZE];	
+	uint32		pad_1[18];
+	uint32		algo;		
+	uint32		flags;		
+	uint32		pad_2[2];
+	int		pad_3;
+	int		iv_initialized;	
+	int		pad_4;
+	
+	struct {
+		uint32	hi;		
+		uint16	lo;		
+	} rxiv;
+	uint32		pad_5[2];
+	struct ether_addr ea;		
+} wl_wsec_key_t;
+
+#define WSEC_MIN_PSK_LEN	8
+#define WSEC_MAX_PSK_LEN	64
+
+
+#define WSEC_PASSPHRASE		(1<<0)
+
+
+typedef struct {
+	ushort	key_len;		
+	ushort	flags;			
+	uint8	key[WSEC_MAX_PSK_LEN];	
+} wsec_pmk_t;
+
+
+#define WEP_ENABLED		0x0001
+#define TKIP_ENABLED		0x0002
+#define AES_ENABLED		0x0004
+#define WSEC_SWFLAG		0x0008
+#define SES_OW_ENABLED		0x0040	
+#define SMS4_ENABLED		0x0100
+
+
+#define WPA_AUTH_DISABLED	0x0000	
+#define WPA_AUTH_NONE		0x0001	
+#define WPA_AUTH_UNSPECIFIED	0x0002	
+#define WPA_AUTH_PSK		0x0004	
+	
+#define WPA2_AUTH_UNSPECIFIED	0x0040	
+#define WPA2_AUTH_PSK		0x0080	
+#define BRCM_AUTH_PSK           0x0100  
+#define BRCM_AUTH_DPT		0x0200	
+#define WPA_AUTH_WAPI		0x0400	
+
+#define WPA_AUTH_PFN_ANY	0xffffffff	
+
+
+#define	MAXPMKID		16
+
+typedef struct _pmkid {
+	struct ether_addr	BSSID;
+	uint8			PMKID[WPA2_PMKID_LEN];
+} pmkid_t;
+
+typedef struct _pmkid_list {
+	uint32	npmkid;
+	pmkid_t	pmkid[1];
+} pmkid_list_t;
+
+typedef struct _pmkid_cand {
+	struct ether_addr	BSSID;
+	uint8			preauth;
+} pmkid_cand_t;
+
+typedef struct _pmkid_cand_list {
+	uint32	npmkid_cand;
+	pmkid_cand_t	pmkid_cand[1];
+} pmkid_cand_list_t;
+
+
+
+
+typedef struct {
+	uint32	val;
+	struct ether_addr ea;
+} scb_val_t;
+
+
+
+typedef struct channel_info {
+	int hw_channel;
+	int target_channel;
+	int scan_channel;
+} channel_info_t;
+
+
+struct maclist {
+	uint count;			
+	struct ether_addr ea[1];	
+};
+
+
+typedef struct get_pktcnt {
+	uint rx_good_pkt;
+	uint rx_bad_pkt;
+	uint tx_good_pkt;
+	uint tx_bad_pkt;
+	uint rx_ocast_good_pkt; 
+} get_pktcnt_t;
+
+
+typedef struct wl_ioctl {
+	uint cmd;	
+	void *buf;	
+	uint len;	
+	uint8 set;	
+	uint used;	
+	uint needed;	
+} wl_ioctl_t;
+
+
+
+#define WLC_IOCTL_MAGIC		0x14e46c77
+
+
+#define WLC_IOCTL_VERSION	1
+
+#define	WLC_IOCTL_MAXLEN	8192	
+#define	WLC_IOCTL_SMLEN		256		
+#define	WLC_IOCTL_MEDLEN	1536	
+
+
+
+#define WLC_GET_MAGIC				0
+#define WLC_GET_VERSION				1
+#define WLC_UP					2
+#define WLC_DOWN				3
+#define WLC_GET_LOOP				4
+#define WLC_SET_LOOP				5
+#define WLC_DUMP				6
+#define WLC_GET_MSGLEVEL			7
+#define WLC_SET_MSGLEVEL			8
+#define WLC_GET_PROMISC				9
+#define WLC_SET_PROMISC				10
+ 
+#define WLC_GET_RATE				12
+ 
+#define WLC_GET_INSTANCE			14
+ 
+ 
+ 
+ 
+#define WLC_GET_INFRA				19
+#define WLC_SET_INFRA				20
+#define WLC_GET_AUTH				21
+#define WLC_SET_AUTH				22
+#define WLC_GET_BSSID				23
+#define WLC_SET_BSSID				24
+#define WLC_GET_SSID				25
+#define WLC_SET_SSID				26
+#define WLC_RESTART				27
+ 
+#define WLC_GET_CHANNEL				29
+#define WLC_SET_CHANNEL				30
+#define WLC_GET_SRL				31
+#define WLC_SET_SRL				32
+#define WLC_GET_LRL				33
+#define WLC_SET_LRL				34
+#define WLC_GET_PLCPHDR				35
+#define WLC_SET_PLCPHDR				36
+#define WLC_GET_RADIO				37
+#define WLC_SET_RADIO				38
+#define WLC_GET_PHYTYPE				39
+#define WLC_DUMP_RATE				40
+#define WLC_SET_RATE_PARAMS			41
+ 
+ 
+#define WLC_GET_KEY				44
+#define WLC_SET_KEY				45
+#define WLC_GET_REGULATORY			46
+#define WLC_SET_REGULATORY			47
+#define WLC_GET_PASSIVE_SCAN			48
+#define WLC_SET_PASSIVE_SCAN			49
+#define WLC_SCAN				50
+#define WLC_SCAN_RESULTS			51
+#define WLC_DISASSOC				52
+#define WLC_REASSOC				53
+#define WLC_GET_ROAM_TRIGGER			54
+#define WLC_SET_ROAM_TRIGGER			55
+#define WLC_GET_ROAM_DELTA			56
+#define WLC_SET_ROAM_DELTA			57
+#define WLC_GET_ROAM_SCAN_PERIOD		58
+#define WLC_SET_ROAM_SCAN_PERIOD		59
+#define WLC_EVM					60	
+#define WLC_GET_TXANT				61
+#define WLC_SET_TXANT				62
+#define WLC_GET_ANTDIV				63
+#define WLC_SET_ANTDIV				64
+ 
+ 
+#define WLC_GET_CLOSED				67
+#define WLC_SET_CLOSED				68
+#define WLC_GET_MACLIST				69
+#define WLC_SET_MACLIST				70
+#define WLC_GET_RATESET				71
+#define WLC_SET_RATESET				72
+ 
+#define WLC_LONGTRAIN				74
+#define WLC_GET_BCNPRD				75
+#define WLC_SET_BCNPRD				76
+#define WLC_GET_DTIMPRD				77
+#define WLC_SET_DTIMPRD				78
+#define WLC_GET_SROM				79
+#define WLC_SET_SROM				80
+#define WLC_GET_WEP_RESTRICT			81
+#define WLC_SET_WEP_RESTRICT			82
+#define WLC_GET_COUNTRY				83
+#define WLC_SET_COUNTRY				84
+#define WLC_GET_PM				85
+#define WLC_SET_PM				86
+#define WLC_GET_WAKE				87
+#define WLC_SET_WAKE				88
+ 
+#define WLC_GET_FORCELINK			90	
+#define WLC_SET_FORCELINK			91	
+#define WLC_FREQ_ACCURACY			92	
+#define WLC_CARRIER_SUPPRESS			93	
+#define WLC_GET_PHYREG				94
+#define WLC_SET_PHYREG				95
+#define WLC_GET_RADIOREG			96
+#define WLC_SET_RADIOREG			97
+#define WLC_GET_REVINFO				98
+#define WLC_GET_UCANTDIV			99
+#define WLC_SET_UCANTDIV			100
+#define WLC_R_REG				101
+#define WLC_W_REG				102
+
+ 
+#define WLC_GET_MACMODE				105
+#define WLC_SET_MACMODE				106
+#define WLC_GET_MONITOR				107
+#define WLC_SET_MONITOR				108
+#define WLC_GET_GMODE				109
+#define WLC_SET_GMODE				110
+#define WLC_GET_LEGACY_ERP			111
+#define WLC_SET_LEGACY_ERP			112
+#define WLC_GET_RX_ANT				113
+#define WLC_GET_CURR_RATESET			114	
+#define WLC_GET_SCANSUPPRESS			115
+#define WLC_SET_SCANSUPPRESS			116
+#define WLC_GET_AP				117
+#define WLC_SET_AP				118
+#define WLC_GET_EAP_RESTRICT			119
+#define WLC_SET_EAP_RESTRICT			120
+#define WLC_SCB_AUTHORIZE			121
+#define WLC_SCB_DEAUTHORIZE			122
+#define WLC_GET_WDSLIST				123
+#define WLC_SET_WDSLIST				124
+#define WLC_GET_ATIM				125
+#define WLC_SET_ATIM				126
+#define WLC_GET_RSSI				127
+#define WLC_GET_PHYANTDIV			128
+#define WLC_SET_PHYANTDIV			129
+#define WLC_AP_RX_ONLY				130
+#define WLC_GET_TX_PATH_PWR			131
+#define WLC_SET_TX_PATH_PWR			132
+#define WLC_GET_WSEC				133
+#define WLC_SET_WSEC				134
+#define WLC_GET_PHY_NOISE			135
+#define WLC_GET_BSS_INFO			136
+#define WLC_GET_PKTCNTS				137
+#define WLC_GET_LAZYWDS				138
+#define WLC_SET_LAZYWDS				139
+#define WLC_GET_BANDLIST			140
+#define WLC_GET_BAND				141
+#define WLC_SET_BAND				142
+#define WLC_SCB_DEAUTHENTICATE			143
+#define WLC_GET_SHORTSLOT			144
+#define WLC_GET_SHORTSLOT_OVERRIDE		145
+#define WLC_SET_SHORTSLOT_OVERRIDE		146
+#define WLC_GET_SHORTSLOT_RESTRICT		147
+#define WLC_SET_SHORTSLOT_RESTRICT		148
+#define WLC_GET_GMODE_PROTECTION		149
+#define WLC_GET_GMODE_PROTECTION_OVERRIDE	150
+#define WLC_SET_GMODE_PROTECTION_OVERRIDE	151
+#define WLC_UPGRADE				152
+ 
+ 
+#define WLC_GET_IGNORE_BCNS			155
+#define WLC_SET_IGNORE_BCNS			156
+#define WLC_GET_SCB_TIMEOUT			157
+#define WLC_SET_SCB_TIMEOUT			158
+#define WLC_GET_ASSOCLIST			159
+#define WLC_GET_CLK				160
+#define WLC_SET_CLK				161
+#define WLC_GET_UP				162
+#define WLC_OUT					163
+#define WLC_GET_WPA_AUTH			164
+#define WLC_SET_WPA_AUTH			165
+#define WLC_GET_UCFLAGS				166
+#define WLC_SET_UCFLAGS				167
+#define WLC_GET_PWRIDX				168
+#define WLC_SET_PWRIDX				169
+#define WLC_GET_TSSI				170
+#define WLC_GET_SUP_RATESET_OVERRIDE		171
+#define WLC_SET_SUP_RATESET_OVERRIDE		172
+ 
+ 
+ 
+ 
+ 
+#define WLC_GET_PROTECTION_CONTROL		178
+#define WLC_SET_PROTECTION_CONTROL		179
+#define WLC_GET_PHYLIST				180
+#define WLC_ENCRYPT_STRENGTH			181	
+#define WLC_DECRYPT_STATUS			182	
+#define WLC_GET_KEY_SEQ				183
+#define WLC_GET_SCAN_CHANNEL_TIME		184
+#define WLC_SET_SCAN_CHANNEL_TIME		185
+#define WLC_GET_SCAN_UNASSOC_TIME		186
+#define WLC_SET_SCAN_UNASSOC_TIME		187
+#define WLC_GET_SCAN_HOME_TIME			188
+#define WLC_SET_SCAN_HOME_TIME			189
+#define WLC_GET_SCAN_NPROBES			190
+#define WLC_SET_SCAN_NPROBES			191
+#define WLC_GET_PRB_RESP_TIMEOUT		192
+#define WLC_SET_PRB_RESP_TIMEOUT		193
+#define WLC_GET_ATTEN				194
+#define WLC_SET_ATTEN				195
+#define WLC_GET_SHMEM				196	
+#define WLC_SET_SHMEM				197	
+ 
+ 
+#define WLC_SET_WSEC_TEST			200
+#define WLC_SCB_DEAUTHENTICATE_FOR_REASON	201
+#define WLC_TKIP_COUNTERMEASURES		202
+#define WLC_GET_PIOMODE				203
+#define WLC_SET_PIOMODE				204
+#define WLC_SET_ASSOC_PREFER			205
+#define WLC_GET_ASSOC_PREFER			206
+#define WLC_SET_ROAM_PREFER			207
+#define WLC_GET_ROAM_PREFER			208
+#define WLC_SET_LED				209
+#define WLC_GET_LED				210
+#define WLC_GET_INTERFERENCE_MODE		211
+#define WLC_SET_INTERFERENCE_MODE		212
+#define WLC_GET_CHANNEL_QA			213
+#define WLC_START_CHANNEL_QA			214
+#define WLC_GET_CHANNEL_SEL			215
+#define WLC_START_CHANNEL_SEL			216
+#define WLC_GET_VALID_CHANNELS			217
+#define WLC_GET_FAKEFRAG			218
+#define WLC_SET_FAKEFRAG			219
+#define WLC_GET_PWROUT_PERCENTAGE		220
+#define WLC_SET_PWROUT_PERCENTAGE		221
+#define WLC_SET_BAD_FRAME_PREEMPT		222
+#define WLC_GET_BAD_FRAME_PREEMPT		223
+#define WLC_SET_LEAP_LIST			224
+#define WLC_GET_LEAP_LIST			225
+#define WLC_GET_CWMIN				226
+#define WLC_SET_CWMIN				227
+#define WLC_GET_CWMAX				228
+#define WLC_SET_CWMAX				229
+#define WLC_GET_WET				230
+#define WLC_SET_WET				231
+#define WLC_GET_PUB				232
+ 
+ 
+#define WLC_GET_KEY_PRIMARY			235
+#define WLC_SET_KEY_PRIMARY			236
+ 
+#define WLC_GET_ACI_ARGS			238
+#define WLC_SET_ACI_ARGS			239
+#define WLC_UNSET_CALLBACK			240
+#define WLC_SET_CALLBACK			241
+#define WLC_GET_RADAR				242
+#define WLC_SET_RADAR				243
+#define WLC_SET_SPECT_MANAGMENT			244
+#define WLC_GET_SPECT_MANAGMENT			245
+#define WLC_WDS_GET_REMOTE_HWADDR		246	
+#define WLC_WDS_GET_WPA_SUP			247
+#define WLC_SET_CS_SCAN_TIMER			248
+#define WLC_GET_CS_SCAN_TIMER			249
+#define WLC_MEASURE_REQUEST			250
+#define WLC_INIT				251
+#define WLC_SEND_QUIET				252
+#define WLC_KEEPALIVE			253
+#define WLC_SEND_PWR_CONSTRAINT			254
+#define WLC_UPGRADE_STATUS			255
+#define WLC_CURRENT_PWR				256
+#define WLC_GET_SCAN_PASSIVE_TIME		257
+#define WLC_SET_SCAN_PASSIVE_TIME		258
+#define WLC_LEGACY_LINK_BEHAVIOR		259
+#define WLC_GET_CHANNELS_IN_COUNTRY		260
+#define WLC_GET_COUNTRY_LIST			261
+#define WLC_GET_VAR				262	
+#define WLC_SET_VAR				263	
+#define WLC_NVRAM_GET				264	
+#define WLC_NVRAM_SET				265
+#define WLC_NVRAM_DUMP				266
+#define WLC_REBOOT				267
+#define WLC_SET_WSEC_PMK			268
+#define WLC_GET_AUTH_MODE			269
+#define WLC_SET_AUTH_MODE			270
+#define WLC_GET_WAKEENTRY			271
+#define WLC_SET_WAKEENTRY			272
+#define WLC_NDCONFIG_ITEM			273	
+#define WLC_NVOTPW				274
+#define WLC_OTPW				275
+#define WLC_IOV_BLOCK_GET			276
+#define WLC_IOV_MODULES_GET			277
+#define WLC_SOFT_RESET				278
+#define WLC_GET_ALLOW_MODE			279
+#define WLC_SET_ALLOW_MODE			280
+#define WLC_GET_DESIRED_BSSID			281
+#define WLC_SET_DESIRED_BSSID			282
+#define	WLC_DISASSOC_MYAP			283
+#define WLC_GET_NBANDS				284	
+#define WLC_GET_BANDSTATES			285	
+#define WLC_GET_WLC_BSS_INFO			286	
+#define WLC_GET_ASSOC_INFO			287	
+#define WLC_GET_OID_PHY				288	
+#define WLC_SET_OID_PHY				289	
+#define WLC_SET_ASSOC_TIME			290	
+#define WLC_GET_DESIRED_SSID			291	
+#define WLC_GET_CHANSPEC			292	
+#define WLC_GET_ASSOC_STATE			293	
+#define WLC_SET_PHY_STATE			294	
+#define WLC_GET_SCAN_PENDING			295	
+#define WLC_GET_SCANREQ_PENDING			296	
+#define WLC_GET_PREV_ROAM_REASON		297	
+#define WLC_SET_PREV_ROAM_REASON		298	
+#define WLC_GET_BANDSTATES_PI			299	
+#define WLC_GET_PHY_STATE			300	
+#define WLC_GET_BSS_WPA_RSN			301	
+#define WLC_GET_BSS_WPA2_RSN			302	
+#define WLC_GET_BSS_BCN_TS			303	
+#define WLC_GET_INT_DISASSOC			304	
+#define WLC_SET_NUM_PEERS			305     
+#define WLC_GET_NUM_BSS				306	
+#define WLC_LAST				307	
+
+
+
+#define WL_RADIO_SW_DISABLE		(1<<0)
+#define WL_RADIO_HW_DISABLE		(1<<1)
+#define WL_RADIO_MPC_DISABLE		(1<<2)
+#define WL_RADIO_COUNTRY_DISABLE	(1<<3)	
+
+
+#define WL_TXPWR_OVERRIDE	(1U<<31)
+
+#define WL_PHY_PAVARS_LEN	6	
+
+
+#define WL_DIAG_INTERRUPT			1	
+#define WL_DIAG_LOOPBACK			2	
+#define WL_DIAG_MEMORY				3	
+#define WL_DIAG_LED				4	
+#define WL_DIAG_REG				5	
+#define WL_DIAG_SROM				6	
+#define WL_DIAG_DMA				7	
+
+#define WL_DIAGERR_SUCCESS			0
+#define WL_DIAGERR_FAIL_TO_RUN			1	
+#define WL_DIAGERR_NOT_SUPPORTED		2	
+#define WL_DIAGERR_INTERRUPT_FAIL		3	
+#define WL_DIAGERR_LOOPBACK_FAIL		4	
+#define WL_DIAGERR_SROM_FAIL			5	
+#define WL_DIAGERR_SROM_BADCRC			6	
+#define WL_DIAGERR_REG_FAIL			7	
+#define WL_DIAGERR_MEMORY_FAIL			8	
+#define WL_DIAGERR_NOMEM			9	
+#define WL_DIAGERR_DMA_FAIL			10	
+
+#define WL_DIAGERR_MEMORY_TIMEOUT		11	
+#define WL_DIAGERR_MEMORY_BADPATTERN		12	
+
+
+#define	WLC_BAND_AUTO		0	
+#define	WLC_BAND_5G		1	
+#define	WLC_BAND_2G		2	
+#define	WLC_BAND_ALL		3	
+
+
+#define WL_CHAN_FREQ_RANGE_2G      0
+#define WL_CHAN_FREQ_RANGE_5GL     1
+#define WL_CHAN_FREQ_RANGE_5GM     2
+#define WL_CHAN_FREQ_RANGE_5GH     3
+
+
+#define	WLC_PHY_TYPE_A		0
+#define	WLC_PHY_TYPE_B		1
+#define	WLC_PHY_TYPE_G		2
+#define	WLC_PHY_TYPE_N		4
+#define	WLC_PHY_TYPE_LP		5
+#define	WLC_PHY_TYPE_SSN	6
+#define	WLC_PHY_TYPE_NULL	0xf
+
+
+#define WLC_MACMODE_DISABLED	0	
+#define WLC_MACMODE_DENY	1	
+#define WLC_MACMODE_ALLOW	2	
+
+
+#define GMODE_LEGACY_B		0
+#define GMODE_AUTO		1
+#define GMODE_ONLY		2
+#define GMODE_B_DEFERRED	3
+#define GMODE_PERFORMANCE	4
+#define GMODE_LRS		5
+#define GMODE_MAX		6
+
+
+#define WLC_PLCP_AUTO	-1
+#define WLC_PLCP_SHORT	0
+#define WLC_PLCP_LONG	1
+
+
+#define WLC_PROTECTION_AUTO		-1
+#define WLC_PROTECTION_OFF		0
+#define WLC_PROTECTION_ON		1
+#define WLC_PROTECTION_MMHDR_ONLY	2
+#define WLC_PROTECTION_CTS_ONLY		3
+
+
+#define WLC_PROTECTION_CTL_OFF		0
+#define WLC_PROTECTION_CTL_LOCAL	1
+#define WLC_PROTECTION_CTL_OVERLAP	2
+
+
+#define WLC_N_PROTECTION_OFF		0
+#define WLC_N_PROTECTION_OPTIONAL	1
+#define WLC_N_PROTECTION_20IN40		2
+#define WLC_N_PROTECTION_MIXEDMODE	3
+
+
+#define WLC_N_PREAMBLE_MIXEDMODE	0
+#define WLC_N_PREAMBLE_GF		1
+
+
+#define WLC_N_BW_20ALL			0
+#define WLC_N_BW_40ALL			1
+#define WLC_N_BW_20IN2G_40IN5G		2
+
+
+#define WLC_N_TXRX_CHAIN0		0
+#define WLC_N_TXRX_CHAIN1		1
+
+
+#define WLC_N_SGI_20			0x01
+#define WLC_N_SGI_40			0x02
+
+
+#define PM_OFF	0
+#define PM_MAX	1
+#define PM_FAST 2
+
+#define LISTEN_INTERVAL			10
+
+#define	INTERFERE_NONE	0	
+#define	NON_WLAN	1	
+#define	WLAN_MANUAL	2	
+#define	WLAN_AUTO	3	
+#define AUTO_ACTIVE	(1 << 7) 
+
+typedef struct wl_aci_args {
+	int enter_aci_thresh; 
+	int exit_aci_thresh; 
+	int usec_spin; 
+	int glitch_delay; 
+	uint16 nphy_adcpwr_enter_thresh;	
+	uint16 nphy_adcpwr_exit_thresh;	
+	uint16 nphy_repeat_ctr;		
+	uint16 nphy_num_samples;	
+	uint16 nphy_undetect_window_sz;	
+	uint16 nphy_b_energy_lo_aci;	
+	uint16 nphy_b_energy_md_aci;	
+	uint16 nphy_b_energy_hi_aci;	
+} wl_aci_args_t;
+
+#define WL_ACI_ARGS_LEGACY_LENGTH	16	
+
+
+
+#define WL_ERROR_VAL		0x00000001
+#define WL_TRACE_VAL		0x00000002
+#define WL_PRHDRS_VAL		0x00000004
+#define WL_PRPKT_VAL		0x00000008
+#define WL_INFORM_VAL		0x00000010
+#define WL_TMP_VAL		0x00000020
+#define WL_OID_VAL		0x00000040
+#define WL_RATE_VAL		0x00000080
+#define WL_ASSOC_VAL		0x00000100
+#define WL_PRUSR_VAL		0x00000200
+#define WL_PS_VAL		0x00000400
+#define WL_TXPWR_VAL		0x00000800
+#define WL_PORT_VAL		0x00001000
+#define WL_DUAL_VAL		0x00002000
+#define WL_WSEC_VAL		0x00004000
+#define WL_WSEC_DUMP_VAL	0x00008000
+#define WL_LOG_VAL		0x00010000
+#define WL_NRSSI_VAL		0x00020000
+#define WL_LOFT_VAL		0x00040000
+#define WL_REGULATORY_VAL	0x00080000
+#define WL_PHYCAL_VAL		0x00100000
+#define WL_RADAR_VAL		0x00200000
+#define WL_MPC_VAL		0x00400000
+#define WL_APSTA_VAL		0x00800000
+#define WL_DFS_VAL		0x01000000
+#define WL_BA_VAL		0x02000000
+#define WL_MBSS_VAL		0x04000000
+#define WL_CAC_VAL		0x08000000
+#define WL_AMSDU_VAL		0x10000000
+#define WL_AMPDU_VAL		0x20000000
+#define WL_FFPLD_VAL		0x40000000
+
+
+#define WL_DPT_VAL 		0x00000001
+#define WL_SCAN_VAL		0x00000002
+#define WL_WOWL_VAL		0x00000004
+#define WL_COEX_VAL		0x00000008
+#define WL_RTDC_VAL		0x00000010
+#define WL_BTA_VAL		0x00000040
+
+
+#define	WL_LED_NUMGPIO		16	
+
+
+#define	WL_LED_OFF		0		
+#define	WL_LED_ON		1		
+#define	WL_LED_ACTIVITY		2		
+#define	WL_LED_RADIO		3		
+#define	WL_LED_ARADIO		4		
+#define	WL_LED_BRADIO		5		
+#define	WL_LED_BGMODE		6		
+#define	WL_LED_WI1		7
+#define	WL_LED_WI2		8
+#define	WL_LED_WI3		9
+#define	WL_LED_ASSOC		10		
+#define	WL_LED_INACTIVE		11		
+#define	WL_LED_ASSOCACT		12		
+#define	WL_LED_NUMBEHAVIOR	13
+
+
+#define	WL_LED_BEH_MASK		0x7f		
+#define	WL_LED_AL_MASK		0x80		
+
+
+#define WL_NUMCHANNELS		64
+#define WL_NUMCHANSPECS		100
+
+
+#define WL_WDS_WPA_ROLE_AUTH	0	
+#define WL_WDS_WPA_ROLE_SUP	1	
+#define WL_WDS_WPA_ROLE_AUTO	255	
+
+
+#define WL_EVENTING_MASK_LEN	16
+
+
+#define VNDR_IE_CMD_LEN		4	
+
+
+#define VNDR_IE_BEACON_FLAG	0x1
+#define VNDR_IE_PRBRSP_FLAG	0x2
+#define VNDR_IE_ASSOCRSP_FLAG	0x4
+#define VNDR_IE_AUTHRSP_FLAG	0x8
+#define VNDR_IE_PRBREQ_FLAG	0x10
+#define VNDR_IE_ASSOCREQ_FLAG	0x20
+#define VNDR_IE_CUSTOM_FLAG		0x100 
+
+#define VNDR_IE_INFO_HDR_LEN	(sizeof(uint32))
+
+typedef struct {
+	uint32 pktflag;			
+	vndr_ie_t vndr_ie_data;		
+} vndr_ie_info_t;
+
+typedef struct {
+	int iecount;			
+	vndr_ie_info_t vndr_ie_list[1];	
+} vndr_ie_buf_t;
+
+typedef struct {
+	char cmd[VNDR_IE_CMD_LEN];	
+	vndr_ie_buf_t vndr_ie_buffer;	
+} vndr_ie_setbuf_t;
+
+
+
+
+#define WL_JOIN_PREF_RSSI	1	
+#define WL_JOIN_PREF_WPA	2	
+#define WL_JOIN_PREF_BAND	3	
+
+
+#define WLJP_BAND_ASSOC_PREF	255	
+
+
+#define WL_WPA_ACP_MCS_ANY	"\x00\x00\x00\x00"
+
+struct tsinfo_arg {
+	uint8 octets[3];
+};
+
+
+#define	NFIFO			6	
+
+#define	WL_CNT_T_VERSION	5	
+#define	WL_CNT_EXT_T_VERSION	1
+
+typedef struct {
+	uint16	version;	
+	uint16	length;		
+
+	
+	uint32	txframe;	
+	uint32	txbyte;		
+	uint32	txretrans;	
+	uint32	txerror;	
+	uint32	txctl;		
+	uint32	txprshort;	
+	uint32	txserr;		
+	uint32	txnobuf;	
+	uint32	txnoassoc;	
+	uint32	txrunt;		
+	uint32	txchit;		
+	uint32	txcmiss;	
+
+	
+	uint32	txuflo;		
+	uint32	txphyerr;	
+	uint32	txphycrs;
+
+	
+	uint32	rxframe;	
+	uint32	rxbyte;		
+	uint32	rxerror;	
+	uint32	rxctl;		
+	uint32	rxnobuf;	
+	uint32	rxnondata;	
+	uint32	rxbadds;	
+	uint32	rxbadcm;	
+	uint32	rxfragerr;	
+	uint32	rxrunt;		
+	uint32	rxgiant;	
+	uint32	rxnoscb;	
+	uint32	rxbadproto;	
+	uint32	rxbadsrcmac;	
+	uint32	rxbadda;	
+	uint32	rxfilter;	
+
+	
+	uint32	rxoflo;		
+	uint32	rxuflo[NFIFO];	
+
+	uint32	d11cnt_txrts_off;	
+	uint32	d11cnt_rxcrc_off;	
+	uint32	d11cnt_txnocts_off;	
+
+	
+	uint32	dmade;		
+	uint32	dmada;		
+	uint32	dmape;		
+	uint32	reset;		
+	uint32	tbtt;		
+	uint32	txdmawar;
+	uint32	pkt_callback_reg_fail;	
+
+	
+	uint32	txallfrm;	
+	uint32	txrtsfrm;	
+	uint32	txctsfrm;	
+	uint32	txackfrm;	
+	uint32	txdnlfrm;	
+	uint32	txbcnfrm;	
+	uint32	txfunfl[8];	
+	uint32	txtplunfl;	
+	uint32	txphyerror;	
+	uint32	rxfrmtoolong;	
+	uint32	rxfrmtooshrt;	
+	uint32	rxinvmachdr;	
+	uint32	rxbadfcs;	
+	uint32	rxbadplcp;	
+	uint32	rxcrsglitch;	
+	uint32	rxstrt;		
+	uint32	rxdfrmucastmbss; 
+	uint32	rxmfrmucastmbss; 
+	uint32	rxcfrmucast;	
+	uint32	rxrtsucast;	
+	uint32	rxctsucast;	
+	uint32	rxackucast;	
+	uint32	rxdfrmocast;	
+	uint32	rxmfrmocast;	
+	uint32	rxcfrmocast;	
+	uint32	rxrtsocast;	
+	uint32	rxctsocast;	
+	uint32	rxdfrmmcast;	
+	uint32	rxmfrmmcast;	
+	uint32	rxcfrmmcast;	
+	uint32	rxbeaconmbss;	
+	uint32	rxdfrmucastobss; 
+	uint32	rxbeaconobss;	
+	uint32	rxrsptmout;	
+	uint32	bcntxcancl;	
+	uint32	rxf0ovfl;	
+	uint32	rxf1ovfl;	
+	uint32	rxf2ovfl;	
+	uint32	txsfovfl;	
+	uint32	pmqovfl;	
+	uint32	rxcgprqfrm;	
+	uint32	rxcgprsqovfl;	
+	uint32	txcgprsfail;	
+	uint32	txcgprssuc;	
+	uint32	prs_timeout;	
+	uint32	rxnack;
+	uint32	frmscons;
+	uint32	txnack;
+	uint32	txglitch_nack;	
+	uint32	txburst;	
+
+	
+	uint32	txfrag;		
+	uint32	txmulti;	
+	uint32	txfail;		
+	uint32	txretry;	
+	uint32	txretrie;	
+	uint32	rxdup;		
+	uint32	txrts;		
+	uint32	txnocts;	
+	uint32	txnoack;	
+	uint32	rxfrag;		
+	uint32	rxmulti;	
+	uint32	rxcrc;		
+	uint32	txfrmsnt;	
+	uint32	rxundec;	
+
+	
+	uint32	tkipmicfaill;	
+	uint32	tkipcntrmsr;	
+	uint32	tkipreplay;	
+	uint32	ccmpfmterr;	
+	uint32	ccmpreplay;	
+	uint32	ccmpundec;	
+	uint32	fourwayfail;	
+	uint32	wepundec;	
+	uint32	wepicverr;	
+	uint32	decsuccess;	
+	uint32	tkipicverr;	
+	uint32	wepexcluded;	
+
+	uint32	txchanrej;	
+	uint32	psmwds;		
+	uint32	phywatchdog;	
+
+	
+	uint32	prq_entries_handled;	
+	uint32	prq_undirected_entries;	
+	uint32	prq_bad_entries;	
+	uint32	atim_suppress_count;	
+	uint32	bcn_template_not_ready;	
+	uint32	bcn_template_not_ready_done; 
+	uint32	late_tbtt_dpc;	
+
+	
+	uint32  rx1mbps;	
+	uint32  rx2mbps;	
+	uint32  rx5mbps5;	
+	uint32  rx6mbps;	
+	uint32  rx9mbps;	
+	uint32  rx11mbps;	
+	uint32  rx12mbps;	
+	uint32  rx18mbps;	
+	uint32  rx24mbps;	
+	uint32  rx36mbps;	
+	uint32  rx48mbps;	
+	uint32  rx54mbps;	
+	uint32  rx108mbps; 	
+	uint32  rx162mbps;	
+	uint32  rx216mbps;	
+	uint32  rx270mbps;	
+	uint32  rx324mbps;	
+	uint32  rx378mbps;	
+	uint32  rx432mbps;	
+	uint32  rx486mbps;	
+	uint32  rx540mbps;	
+	
+	uint32	pktengrxducast; 
+	uint32	pktengrxdmcast; 
+} wl_cnt_t;
+
+typedef struct {
+	uint16	version;	
+	uint16	length;		
+
+	uint32 rxampdu_sgi;	
+	uint32 rxampdu_stbc; 
+	uint32 rxmpdu_sgi;	
+	uint32 rxmpdu_stbc;  
+	uint32	rxmcs0_40M;  
+	uint32	rxmcs1_40M;  
+	uint32	rxmcs2_40M;  
+	uint32	rxmcs3_40M;  
+	uint32	rxmcs4_40M;  
+	uint32	rxmcs5_40M;  
+	uint32	rxmcs6_40M;  
+	uint32	rxmcs7_40M;  
+	uint32	rxmcs32_40M;  
+
+	uint32	txfrmsnt_20Mlo;  
+	uint32	txfrmsnt_20Mup;  
+	uint32	txfrmsnt_40M;   
+
+	uint32 rx_20ul;
+} wl_cnt_ext_t;
+
+#define	WL_RXDIV_STATS_T_VERSION	1	
+typedef struct {
+	uint16	version;	
+	uint16	length;		
+
+	uint32 rxant[4];	
+} wl_rxdiv_stats_t;
+
+#define	WL_DELTA_STATS_T_VERSION	1	
+
+typedef struct {
+	uint16 version;     
+	uint16 length;      
+
+	
+	uint32 txframe;     
+	uint32 txbyte;      
+	uint32 txretrans;   
+	uint32 txfail;      
+
+	
+	uint32 rxframe;     
+	uint32 rxbyte;      
+
+	
+	uint32  rx1mbps;	
+	uint32  rx2mbps;	
+	uint32  rx5mbps5;	
+	uint32  rx6mbps;	
+	uint32  rx9mbps;	
+	uint32  rx11mbps;	
+	uint32  rx12mbps;	
+	uint32  rx18mbps;	
+	uint32  rx24mbps;	
+	uint32  rx36mbps;	
+	uint32  rx48mbps;	
+	uint32  rx54mbps;	
+	uint32  rx108mbps; 	
+	uint32  rx162mbps;	
+	uint32  rx216mbps;	
+	uint32  rx270mbps;	
+	uint32  rx324mbps;	
+	uint32  rx378mbps;	
+	uint32  rx432mbps;	
+	uint32  rx486mbps;	
+	uint32  rx540mbps;	
+} wl_delta_stats_t;
+
+#define WL_WME_CNT_VERSION	1	
+
+typedef struct {
+	uint32 packets;
+	uint32 bytes;
+} wl_traffic_stats_t;
+
+typedef struct {
+	uint16	version;	
+	uint16	length;		
+
+	wl_traffic_stats_t tx[AC_COUNT];	
+	wl_traffic_stats_t tx_failed[AC_COUNT];	
+	wl_traffic_stats_t rx[AC_COUNT];	
+	wl_traffic_stats_t rx_failed[AC_COUNT];	
+
+	wl_traffic_stats_t forward[AC_COUNT];	
+
+	wl_traffic_stats_t tx_expired[AC_COUNT];	
+
+} wl_wme_cnt_t;
+
+
+
+#define WLC_ROAM_TRIGGER_DEFAULT	0 
+#define WLC_ROAM_TRIGGER_BANDWIDTH	1 
+#define WLC_ROAM_TRIGGER_DISTANCE	2 
+#define WLC_ROAM_TRIGGER_MAX_VALUE	2 
+
+
+enum {
+	PFN_LIST_ORDER,
+	PFN_RSSI
+};
+
+enum {
+	DISABLE,
+	ENABLE
+};
+
+#define SORT_CRITERIA_BIT		0
+#define AUTO_NET_SWITCH_BIT		1
+#define ENABLE_BKGRD_SCAN_BIT	2
+#define IMMEDIATE_SCAN_BIT		3
+#define	AUTO_CONNECT_BIT		4
+#define	ENABLE_BD_SCAN_BIT		5
+#define ENABLE_ADAPTSCAN_BIT	6
+
+#define SORT_CRITERIA_MASK		0x01
+#define AUTO_NET_SWITCH_MASK	0x02
+#define ENABLE_BKGRD_SCAN_MASK	0x04
+#define IMMEDIATE_SCAN_MASK		0x08
+#define	AUTO_CONNECT_MASK		0x10
+#define ENABLE_BD_SCAN_MASK		0x20
+#define ENABLE_ADAPTSCAN_MASK	0x40
+
+#define PFN_VERSION			1
+
+#define MAX_PFN_LIST_COUNT	16
+
+
+typedef struct wl_pfn_param {
+	int32 version;			
+	int32 scan_freq;		
+	int32 lost_network_timeout;	
+	int16 flags;			
+	int16 rssi_margin;		
+	int32  repeat_scan;
+	int32  max_freq_adjust;
+} wl_pfn_param_t;
+
+typedef struct wl_pfn {
+	wlc_ssid_t		ssid;			
+	int32			bss_type;		
+	int32			infra;			
+	int32			auth;			
+	uint32			wpa_auth;		
+	int32			wsec;			
+} wl_pfn_t;
+
+#define PNO_SCAN_MAX_FW		508*1000
+#define PNO_SCAN_MAX_FW_SEC	PNO_SCAN_MAX_FW/1000
+#define PNO_SCAN_MIN_FW_SEC	10
+
+
+#define TOE_TX_CSUM_OL		0x00000001
+#define TOE_RX_CSUM_OL		0x00000002
+
+
+#define TOE_ERRTEST_TX_CSUM	0x00000001
+#define TOE_ERRTEST_RX_CSUM	0x00000002
+#define TOE_ERRTEST_RX_CSUM2	0x00000004
+
+struct toe_ol_stats_t {
+	
+	uint32 tx_summed;
+
+	
+	uint32 tx_iph_fill;
+	uint32 tx_tcp_fill;
+	uint32 tx_udp_fill;
+	uint32 tx_icmp_fill;
+
+	
+	uint32 rx_iph_good;
+	uint32 rx_iph_bad;
+	uint32 rx_tcp_good;
+	uint32 rx_tcp_bad;
+	uint32 rx_udp_good;
+	uint32 rx_udp_bad;
+	uint32 rx_icmp_good;
+	uint32 rx_icmp_bad;
+
+	
+	uint32 tx_tcp_errinj;
+	uint32 tx_udp_errinj;
+	uint32 tx_icmp_errinj;
+
+	
+	uint32 rx_tcp_errinj;
+	uint32 rx_udp_errinj;
+	uint32 rx_icmp_errinj;
+};
+
+
+#define ARP_OL_AGENT		0x00000001
+#define ARP_OL_SNOOP		0x00000002
+#define ARP_OL_HOST_AUTO_REPLY	0x00000004
+#define ARP_OL_PEER_AUTO_REPLY	0x00000008
+
+
+#define ARP_ERRTEST_REPLY_PEER	0x1
+#define ARP_ERRTEST_REPLY_HOST	0x2
+
+#define ARP_MULTIHOMING_MAX	8	
+
+
+struct arp_ol_stats_t {
+	uint32  host_ip_entries;	
+	uint32  host_ip_overflow;	
+
+	uint32  arp_table_entries;	
+	uint32  arp_table_overflow;	
+
+	uint32  host_request;		
+	uint32  host_reply;		
+	uint32  host_service;		
+
+	uint32  peer_request;		
+	uint32  peer_request_drop;	
+	uint32  peer_reply;		
+	uint32  peer_reply_drop;	
+	uint32  peer_service;		
+};
+
+
+
+
+
+typedef struct wl_keep_alive_pkt {
+	uint32	period_msec;	
+	uint16	len_bytes;	
+	uint8	data[1];	
+} wl_keep_alive_pkt_t;
+
+#define WL_KEEP_ALIVE_FIXED_LEN		OFFSETOF(wl_keep_alive_pkt_t, data)
+
+
+
+
+
+typedef enum wl_pkt_filter_type {
+	WL_PKT_FILTER_TYPE_PATTERN_MATCH	
+} wl_pkt_filter_type_t;
+
+#define WL_PKT_FILTER_TYPE wl_pkt_filter_type_t
+
+
+typedef struct wl_pkt_filter_pattern {
+	uint32	offset;		
+	uint32	size_bytes;	
+	uint8   mask_and_pattern[1]; 
+} wl_pkt_filter_pattern_t;
+
+
+typedef struct wl_pkt_filter {
+	uint32	id;		
+	uint32	type;		
+	uint32	negate_match;	
+	union {			
+		wl_pkt_filter_pattern_t pattern;	
+	} u;
+} wl_pkt_filter_t;
+
+#define WL_PKT_FILTER_FIXED_LEN		  OFFSETOF(wl_pkt_filter_t, u)
+#define WL_PKT_FILTER_PATTERN_FIXED_LEN	  OFFSETOF(wl_pkt_filter_pattern_t, mask_and_pattern)
+
+
+typedef struct wl_pkt_filter_enable {
+	uint32	id;		
+	uint32	enable;		
+} wl_pkt_filter_enable_t;
+
+
+typedef struct wl_pkt_filter_list {
+	uint32	num;		
+	wl_pkt_filter_t	filter[1];	
+} wl_pkt_filter_list_t;
+
+#define WL_PKT_FILTER_LIST_FIXED_LEN	  OFFSETOF(wl_pkt_filter_list_t, filter)
+
+
+typedef struct wl_pkt_filter_stats {
+	uint32	num_pkts_matched;	
+	uint32	num_pkts_forwarded;	
+	uint32	num_pkts_discarded;	
+} wl_pkt_filter_stats_t;
+
+
+typedef struct wl_seq_cmd_ioctl {
+	uint32 cmd;		
+	uint32 len;		
+} wl_seq_cmd_ioctl_t;
+
+#define WL_SEQ_CMD_ALIGN_BYTES	4
+
+
+#define WL_SEQ_CMDS_GET_IOCTL_FILTER(cmd) \
+	(((cmd) == WLC_GET_MAGIC)		|| \
+	 ((cmd) == WLC_GET_VERSION)		|| \
+	 ((cmd) == WLC_GET_AP)			|| \
+	 ((cmd) == WLC_GET_INSTANCE))
+
+
+
+#define WL_PKTENG_PER_TX_START			0x01
+#define WL_PKTENG_PER_TX_STOP			0x02
+#define WL_PKTENG_PER_RX_START			0x04
+#define WL_PKTENG_PER_RX_WITH_ACK_START 	0x05
+#define WL_PKTENG_PER_TX_WITH_ACK_START 	0x06
+#define WL_PKTENG_PER_RX_STOP			0x08
+#define WL_PKTENG_PER_MASK			0xff
+
+#define WL_PKTENG_SYNCHRONOUS			0x100	
+
+typedef struct wl_pkteng {
+	uint32 flags;
+	uint32 delay;			
+	uint32 nframes;			
+	uint32 length;			
+	uint8  seqno;			
+	struct ether_addr dest;		
+	struct ether_addr src;		
+} wl_pkteng_t;
+
+#define NUM_80211b_RATES	4
+#define NUM_80211ag_RATES	8
+#define NUM_80211n_RATES	32
+#define NUM_80211_RATES		(NUM_80211b_RATES+NUM_80211ag_RATES+NUM_80211n_RATES)
+typedef struct wl_pkteng_stats {
+	uint32 lostfrmcnt;		
+	int32 rssi;			
+	int32 snr;			
+	uint16 rxpktcnt[NUM_80211_RATES+1];
+} wl_pkteng_stats_t;
+
+#define WL_WOWL_MAGIC	(1 << 0)	
+#define WL_WOWL_NET	(1 << 1)	
+#define WL_WOWL_DIS	(1 << 2)	
+#define WL_WOWL_RETR	(1 << 3)	
+#define WL_WOWL_BCN	(1 << 4)	
+#define WL_WOWL_TST	(1 << 5)	
+#define WL_WOWL_BCAST	(1 << 15)	
+
+#define MAGIC_PKT_MINLEN 102	
+
+typedef struct {
+	uint masksize;		
+	uint offset;		
+	uint patternoffset;	
+	uint patternsize;	
+	
+	
+} wl_wowl_pattern_t;
+
+typedef struct {
+	uint			count;
+	wl_wowl_pattern_t	pattern[1];
+} wl_wowl_pattern_list_t;
+
+typedef struct {
+	uint8	pci_wakeind;	
+	uint16	ucode_wakeind;	
+} wl_wowl_wakeind_t;
+
+
+typedef struct wl_txrate_class {
+	uint8		init_rate;
+	uint8		min_rate;
+	uint8		max_rate;
+} wl_txrate_class_t;
+
+
+
+
+#define WLC_OBSS_SCAN_PASSIVE_DWELL_DEFAULT		100	
+#define WLC_OBSS_SCAN_PASSIVE_DWELL_MIN			5	
+#define WLC_OBSS_SCAN_PASSIVE_DWELL_MAX			1000	
+#define WLC_OBSS_SCAN_ACTIVE_DWELL_DEFAULT		20	
+#define WLC_OBSS_SCAN_ACTIVE_DWELL_MIN			10	
+#define WLC_OBSS_SCAN_ACTIVE_DWELL_MAX			1000	
+#define WLC_OBSS_SCAN_WIDTHSCAN_INTERVAL_DEFAULT	300	
+#define WLC_OBSS_SCAN_WIDTHSCAN_INTERVAL_MIN		10	
+#define WLC_OBSS_SCAN_WIDTHSCAN_INTERVAL_MAX		900	
+#define WLC_OBSS_SCAN_CHANWIDTH_TRANSITION_DLY_DEFAULT	5
+#define WLC_OBSS_SCAN_CHANWIDTH_TRANSITION_DLY_MIN	5
+#define WLC_OBSS_SCAN_CHANWIDTH_TRANSITION_DLY_MAX	100
+#define WLC_OBSS_SCAN_PASSIVE_TOTAL_PER_CHANNEL_DEFAULT	200	
+#define WLC_OBSS_SCAN_PASSIVE_TOTAL_PER_CHANNEL_MIN	200	
+#define WLC_OBSS_SCAN_PASSIVE_TOTAL_PER_CHANNEL_MAX	10000	
+#define WLC_OBSS_SCAN_ACTIVE_TOTAL_PER_CHANNEL_DEFAULT	20	
+#define WLC_OBSS_SCAN_ACTIVE_TOTAL_PER_CHANNEL_MIN	20	
+#define WLC_OBSS_SCAN_ACTIVE_TOTAL_PER_CHANNEL_MAX	10000	
+#define WLC_OBSS_SCAN_ACTIVITY_THRESHOLD_DEFAULT	25	
+#define WLC_OBSS_SCAN_ACTIVITY_THRESHOLD_MIN		0	
+#define WLC_OBSS_SCAN_ACTIVITY_THRESHOLD_MAX		100	
+
+
+typedef struct wl_obss_scan_arg {
+	int16	passive_dwell;
+	int16	active_dwell;
+	int16	bss_widthscan_interval;
+	int16	passive_total;
+	int16	active_total;
+	int16	chanwidth_transition_delay;
+	int16	activity_threshold;
+} wl_obss_scan_arg_t;
+#define WL_OBSS_SCAN_PARAM_LEN	sizeof(wl_obss_scan_arg_t)
+#define WL_MIN_NUM_OBSS_SCAN_ARG 7	
+
+#define WL_COEX_INFO_MASK		0x07
+#define WL_COEX_INFO_REQ		0x01
+#define	WL_COEX_40MHZ_INTOLERANT	0x02
+#define	WL_COEX_WIDTH20			0x04
+
+typedef struct wl_action_obss_coex_req {
+	uint8 info;
+	uint8 num;
+	uint8 ch_list[1];
+} wl_action_obss_coex_req_t;
+
+
+#define MAX_RSSI_LEVELS 8
+
+
+typedef struct wl_rssi_event {
+	
+	uint32 rate_limit_msec;
+	
+	uint8 num_rssi_levels;
+	
+	int8 rssi_levels[MAX_RSSI_LEVELS];
+} wl_rssi_event_t;
+
+
+
+#define WLFEATURE_DISABLE_11N		0x00000001
+#define WLFEATURE_DISABLE_11N_STBC_TX	0x00000002
+#define WLFEATURE_DISABLE_11N_STBC_RX	0x00000004
+#define WLFEATURE_DISABLE_11N_SGI_TX	0x00000008
+#define WLFEATURE_DISABLE_11N_SGI_RX	0x00000010
+#define WLFEATURE_DISABLE_11N_AMPDU_TX	0x00000020
+#define WLFEATURE_DISABLE_11N_AMPDU_RX	0x00000040
+#define WLFEATURE_DISABLE_11N_GF	0x00000080
+
+
+
+#include <packed_section_end.h>
+
+
+#include <packed_section_start.h>
+
+
+typedef BWL_PRE_PACKED_STRUCT struct sta_prbreq_wps_ie_hdr {
+	struct ether_addr staAddr;
+	uint16 ieLen;
+} BWL_POST_PACKED_STRUCT sta_prbreq_wps_ie_hdr_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct sta_prbreq_wps_ie_data {
+	sta_prbreq_wps_ie_hdr_t hdr;
+	uint8 ieData[1];
+} BWL_POST_PACKED_STRUCT sta_prbreq_wps_ie_data_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct sta_prbreq_wps_ie_list {
+	uint32 totLen;
+	uint8 ieDataList[1];
+} BWL_POST_PACKED_STRUCT sta_prbreq_wps_ie_list_t;
+
+
+#include <packed_section_end.h>
+
+#endif 
diff --git a/drivers/net/wireless/bcm4329/linux_osl.c b/drivers/net/wireless/bcm4329/linux_osl.c
new file mode 100644
index 000000000000..cf72a077bd90
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/linux_osl.c
@@ -0,0 +1,625 @@
+/*
+ * Linux OS Independent Layer
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: linux_osl.c,v 1.125.12.3.8.7 2010/05/04 21:10:04 Exp $
+ */
+
+
+#define LINUX_OSL
+
+#include <typedefs.h>
+#include <bcmendian.h>
+#include <linuxver.h>
+#include <bcmdefs.h>
+#include <osl.h>
+#include <bcmutils.h>
+#include <linux/delay.h>
+#include <pcicfg.h>
+#include <linux/mutex.h>
+
+#define PCI_CFG_RETRY 		10
+
+#define OS_HANDLE_MAGIC		0x1234abcd	
+#define BCM_MEM_FILENAME_LEN 	24		
+
+#ifdef DHD_USE_STATIC_BUF
+#define MAX_STATIC_BUF_NUM 16
+#define STATIC_BUF_SIZE	(PAGE_SIZE*2)
+#define STATIC_BUF_TOTAL_LEN (MAX_STATIC_BUF_NUM*STATIC_BUF_SIZE)
+typedef struct bcm_static_buf {
+	struct mutex static_sem;
+	unsigned char *buf_ptr;
+	unsigned char buf_use[MAX_STATIC_BUF_NUM];
+} bcm_static_buf_t;
+
+static bcm_static_buf_t *bcm_static_buf = 0;
+
+#define MAX_STATIC_PKT_NUM 8
+typedef struct bcm_static_pkt {
+	struct sk_buff *skb_4k[MAX_STATIC_PKT_NUM];
+	struct sk_buff *skb_8k[MAX_STATIC_PKT_NUM];
+	struct mutex osl_pkt_sem;
+	unsigned char pkt_use[MAX_STATIC_PKT_NUM*2];
+} bcm_static_pkt_t;
+static bcm_static_pkt_t *bcm_static_skb = 0;
+
+#endif 
+typedef struct bcm_mem_link {
+	struct bcm_mem_link *prev;
+	struct bcm_mem_link *next;
+	uint	size;
+	int	line;
+	char	file[BCM_MEM_FILENAME_LEN];
+} bcm_mem_link_t;
+
+struct osl_info {
+	osl_pubinfo_t pub;
+	uint magic;
+	void *pdev;
+	uint malloced;
+	uint failed;
+	uint bustype;
+	bcm_mem_link_t *dbgmem_list;
+};
+
+static int16 linuxbcmerrormap[] =
+{	0, 			
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL, 		
+	-EINVAL, 		
+	-EINVAL, 		
+	-E2BIG,			
+	-E2BIG,			
+	-EBUSY, 		
+	-EINVAL, 		
+	-EINVAL, 		
+	-EINVAL, 		
+	-EINVAL, 		
+	-EFAULT, 		
+	-ENOMEM, 		
+	-EOPNOTSUPP,		
+	-EMSGSIZE,		
+	-EINVAL,		
+	-EPERM,			
+	-ENOMEM, 		
+	-EINVAL, 		
+	-ERANGE, 		
+	-EINVAL, 		
+	-EINVAL, 		
+	-EINVAL, 		
+	-EINVAL, 		
+	-EINVAL,		
+	-EIO,			
+	-ENODEV,		
+	-EINVAL,		
+	-EIO,			
+	-EIO,			
+	-EINVAL,		
+	-EINVAL,		
+
+
+
+#if BCME_LAST != -41
+#error "You need to add a OS error translation in the linuxbcmerrormap \
+	for new error code defined in bcmutils.h"
+#endif 
+};
+
+
+int
+osl_error(int bcmerror)
+{
+	if (bcmerror > 0)
+		bcmerror = 0;
+	else if (bcmerror < BCME_LAST)
+		bcmerror = BCME_ERROR;
+
+	
+	return linuxbcmerrormap[-bcmerror];
+}
+
+void * dhd_os_prealloc(int section, unsigned long size);
+osl_t *
+osl_attach(void *pdev, uint bustype, bool pkttag)
+{
+	osl_t *osh;
+	gfp_t flags;
+
+	flags = (in_atomic()) ? GFP_ATOMIC : GFP_KERNEL;
+	osh = kmalloc(sizeof(osl_t), flags);
+	ASSERT(osh);
+
+	bzero(osh, sizeof(osl_t));
+
+	
+	ASSERT(ABS(BCME_LAST) == (ARRAYSIZE(linuxbcmerrormap) - 1));
+
+	osh->magic = OS_HANDLE_MAGIC;
+	osh->malloced = 0;
+	osh->failed = 0;
+	osh->dbgmem_list = NULL;
+	osh->pdev = pdev;
+	osh->pub.pkttag = pkttag;
+	osh->bustype = bustype;
+
+	switch (bustype) {
+		case PCI_BUS:
+		case SI_BUS:
+		case PCMCIA_BUS:
+			osh->pub.mmbus = TRUE;
+			break;
+		case JTAG_BUS:
+		case SDIO_BUS:
+		case USB_BUS:
+		case SPI_BUS:
+			osh->pub.mmbus = FALSE;
+			break;
+		default:
+			ASSERT(FALSE);
+			break;
+	}
+
+#ifdef DHD_USE_STATIC_BUF
+
+
+	if (!bcm_static_buf) {
+		if (!(bcm_static_buf = (bcm_static_buf_t *)dhd_os_prealloc(3, STATIC_BUF_SIZE+
+			STATIC_BUF_TOTAL_LEN))) {
+			printk("can not alloc static buf!\n");
+		}
+		else {
+			/* printk("alloc static buf at %x!\n", (unsigned int)bcm_static_buf); */
+		}
+		
+		mutex_init(&bcm_static_buf->static_sem);
+
+		
+		bcm_static_buf->buf_ptr = (unsigned char *)bcm_static_buf + STATIC_BUF_SIZE;
+
+	}
+	
+	if (!bcm_static_skb)
+	{
+		int i;
+		void *skb_buff_ptr = 0;
+		bcm_static_skb = (bcm_static_pkt_t *)((char *)bcm_static_buf + 2048);
+		skb_buff_ptr = dhd_os_prealloc(4, 0);
+
+		bcopy(skb_buff_ptr, bcm_static_skb, sizeof(struct sk_buff *)*16);
+		for (i = 0; i < MAX_STATIC_PKT_NUM*2; i++)
+			bcm_static_skb->pkt_use[i] = 0;
+
+		mutex_init(&bcm_static_skb->osl_pkt_sem);
+	}
+#endif 
+	return osh;
+}
+
+void
+osl_detach(osl_t *osh)
+{
+	if (osh == NULL)
+		return;
+
+#ifdef DHD_USE_STATIC_BUF
+	if (bcm_static_buf) {
+		bcm_static_buf = 0;
+	}
+	if (bcm_static_skb) {
+		bcm_static_skb = 0;
+	}
+#endif 
+	ASSERT(osh->magic == OS_HANDLE_MAGIC);
+	kfree(osh);
+}
+
+
+void*
+osl_pktget(osl_t *osh, uint len)
+{
+	struct sk_buff *skb;
+	gfp_t flags;
+
+	flags = (in_atomic()) ? GFP_ATOMIC : GFP_KERNEL;
+	if ((skb = __dev_alloc_skb(len, flags))) {
+		skb_put(skb, len);
+		skb->priority = 0;
+
+
+		osh->pub.pktalloced++;
+	}
+
+	return ((void*) skb);
+}
+
+
+void
+osl_pktfree(osl_t *osh, void *p, bool send)
+{
+	struct sk_buff *skb, *nskb;
+
+	skb = (struct sk_buff*) p;
+
+	if (send && osh->pub.tx_fn)
+		osh->pub.tx_fn(osh->pub.tx_ctx, p, 0);
+
+	
+	while (skb) {
+		nskb = skb->next;
+		skb->next = NULL;
+
+
+		if (skb->destructor) {
+			
+			dev_kfree_skb_any(skb);
+		} else {
+			
+			dev_kfree_skb(skb);
+		}
+
+		osh->pub.pktalloced--;
+
+		skb = nskb;
+	}
+}
+
+#ifdef DHD_USE_STATIC_BUF
+void*
+osl_pktget_static(osl_t *osh, uint len)
+{
+	int i = 0;
+	struct sk_buff *skb;
+
+	
+	if (len > (PAGE_SIZE*2))
+	{
+		printk("Do we really need this big skb??\n");
+		return osl_pktget(osh, len);
+	}
+
+	
+	mutex_lock(&bcm_static_skb->osl_pkt_sem);
+	if (len <= PAGE_SIZE)
+	{
+		
+		for (i = 0; i < MAX_STATIC_PKT_NUM; i++)
+		{
+			if (bcm_static_skb->pkt_use[i] == 0)
+				break;
+		}
+
+		if (i != MAX_STATIC_PKT_NUM)
+		{
+			bcm_static_skb->pkt_use[i] = 1;
+			mutex_unlock(&bcm_static_skb->osl_pkt_sem);
+
+			skb = bcm_static_skb->skb_4k[i];
+			skb->tail = skb->data + len;
+			skb->len = len;
+			
+			return skb;
+		}
+	}
+
+	
+	for (i = 0; i < MAX_STATIC_PKT_NUM; i++)
+	{
+		if (bcm_static_skb->pkt_use[i+MAX_STATIC_PKT_NUM] == 0)
+			break;
+	}
+
+	if (i != MAX_STATIC_PKT_NUM)
+	{
+		bcm_static_skb->pkt_use[i+MAX_STATIC_PKT_NUM] = 1;
+		mutex_unlock(&bcm_static_skb->osl_pkt_sem);
+		skb = bcm_static_skb->skb_8k[i];
+		skb->tail = skb->data + len;
+		skb->len = len;
+		
+		return skb;
+	}
+
+
+	
+	mutex_unlock(&bcm_static_skb->osl_pkt_sem);
+	printk("all static pkt in use!\n");
+	return osl_pktget(osh, len);
+}
+
+
+void
+osl_pktfree_static(osl_t *osh, void *p, bool send)
+{
+	int i;
+	
+	for (i = 0; i < MAX_STATIC_PKT_NUM*2; i++)
+	{
+		if (p == bcm_static_skb->skb_4k[i])
+		{
+			mutex_lock(&bcm_static_skb->osl_pkt_sem);
+			bcm_static_skb->pkt_use[i] = 0;
+			mutex_unlock(&bcm_static_skb->osl_pkt_sem);
+
+			
+			return;
+		}
+	}
+	return osl_pktfree(osh, p, send);
+}
+#endif 
+uint32
+osl_pci_read_config(osl_t *osh, uint offset, uint size)
+{
+	uint val = 0;
+	uint retry = PCI_CFG_RETRY;
+
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+
+	
+	ASSERT(size == 4);
+
+	do {
+		pci_read_config_dword(osh->pdev, offset, &val);
+		if (val != 0xffffffff)
+			break;
+	} while (retry--);
+
+
+	return (val);
+}
+
+void
+osl_pci_write_config(osl_t *osh, uint offset, uint size, uint val)
+{
+	uint retry = PCI_CFG_RETRY;
+
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+
+	
+	ASSERT(size == 4);
+
+	do {
+		pci_write_config_dword(osh->pdev, offset, val);
+		if (offset != PCI_BAR0_WIN)
+			break;
+		if (osl_pci_read_config(osh, offset, size) == val)
+			break;
+	} while (retry--);
+
+}
+
+
+uint
+osl_pci_bus(osl_t *osh)
+{
+	ASSERT(osh && (osh->magic == OS_HANDLE_MAGIC) && osh->pdev);
+
+	return ((struct pci_dev *)osh->pdev)->bus->number;
+}
+
+
+uint
+osl_pci_slot(osl_t *osh)
+{
+	ASSERT(osh && (osh->magic == OS_HANDLE_MAGIC) && osh->pdev);
+
+	return PCI_SLOT(((struct pci_dev *)osh->pdev)->devfn);
+}
+
+static void
+osl_pcmcia_attr(osl_t *osh, uint offset, char *buf, int size, bool write)
+{
+}
+
+void
+osl_pcmcia_read_attr(osl_t *osh, uint offset, void *buf, int size)
+{
+	osl_pcmcia_attr(osh, offset, (char *) buf, size, FALSE);
+}
+
+void
+osl_pcmcia_write_attr(osl_t *osh, uint offset, void *buf, int size)
+{
+	osl_pcmcia_attr(osh, offset, (char *) buf, size, TRUE);
+}
+
+
+
+void*
+osl_malloc(osl_t *osh, uint size)
+{
+	void *addr;
+	gfp_t flags;
+
+	if (osh)
+		ASSERT(osh->magic == OS_HANDLE_MAGIC);
+
+#ifdef DHD_USE_STATIC_BUF
+	if (bcm_static_buf)
+	{
+		int i = 0;
+		if ((size >= PAGE_SIZE)&&(size <= STATIC_BUF_SIZE))
+		{
+			mutex_lock(&bcm_static_buf->static_sem);
+			
+			for (i = 0; i < MAX_STATIC_BUF_NUM; i++)
+			{
+				if (bcm_static_buf->buf_use[i] == 0)
+					break;
+			}
+			
+			if (i == MAX_STATIC_BUF_NUM)
+			{
+				mutex_unlock(&bcm_static_buf->static_sem);
+				printk("all static buff in use!\n");
+				goto original;
+			}
+			
+			bcm_static_buf->buf_use[i] = 1;
+			mutex_unlock(&bcm_static_buf->static_sem);
+
+			bzero(bcm_static_buf->buf_ptr+STATIC_BUF_SIZE*i, size);
+			if (osh)
+				osh->malloced += size;
+
+			return ((void *)(bcm_static_buf->buf_ptr+STATIC_BUF_SIZE*i));
+		}
+	}
+original:
+#endif 
+	flags = (in_atomic()) ? GFP_ATOMIC : GFP_KERNEL;
+	if ((addr = kmalloc(size, flags)) == NULL) {
+		if (osh)
+			osh->failed++;
+		return (NULL);
+	}
+	if (osh)
+		osh->malloced += size;
+
+	return (addr);
+}
+
+void
+osl_mfree(osl_t *osh, void *addr, uint size)
+{
+#ifdef DHD_USE_STATIC_BUF
+	if (bcm_static_buf)
+	{
+		if ((addr > (void *)bcm_static_buf) && ((unsigned char *)addr
+			<= ((unsigned char *)bcm_static_buf + STATIC_BUF_TOTAL_LEN)))
+		{
+			int buf_idx = 0;
+			
+			buf_idx = ((unsigned char *)addr - bcm_static_buf->buf_ptr)/STATIC_BUF_SIZE;
+			
+			mutex_lock(&bcm_static_buf->static_sem);
+			bcm_static_buf->buf_use[buf_idx] = 0;
+			mutex_unlock(&bcm_static_buf->static_sem);
+
+			if (osh) {
+				ASSERT(osh->magic == OS_HANDLE_MAGIC);
+				osh->malloced -= size;
+			}
+			return;
+		}
+	}
+#endif 
+	if (osh) {
+		ASSERT(osh->magic == OS_HANDLE_MAGIC);
+		osh->malloced -= size;
+	}
+	kfree(addr);
+}
+
+uint
+osl_malloced(osl_t *osh)
+{
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+	return (osh->malloced);
+}
+
+uint
+osl_malloc_failed(osl_t *osh)
+{
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+	return (osh->failed);
+}
+
+void*
+osl_dma_alloc_consistent(osl_t *osh, uint size, ulong *pap)
+{
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+
+	return (pci_alloc_consistent(osh->pdev, size, (dma_addr_t*)pap));
+}
+
+void
+osl_dma_free_consistent(osl_t *osh, void *va, uint size, ulong pa)
+{
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+
+	pci_free_consistent(osh->pdev, size, va, (dma_addr_t)pa);
+}
+
+uint
+osl_dma_map(osl_t *osh, void *va, uint size, int direction)
+{
+	int dir;
+
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+	dir = (direction == DMA_TX)? PCI_DMA_TODEVICE: PCI_DMA_FROMDEVICE;
+	return (pci_map_single(osh->pdev, va, size, dir));
+}
+
+void
+osl_dma_unmap(osl_t *osh, uint pa, uint size, int direction)
+{
+	int dir;
+
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+	dir = (direction == DMA_TX)? PCI_DMA_TODEVICE: PCI_DMA_FROMDEVICE;
+	pci_unmap_single(osh->pdev, (uint32)pa, size, dir);
+}
+
+
+void
+osl_delay(uint usec)
+{
+	uint d;
+
+	while (usec > 0) {
+		d = MIN(usec, 1000);
+		udelay(d);
+		usec -= d;
+	}
+}
+
+
+
+void *
+osl_pktdup(osl_t *osh, void *skb)
+{
+	void * p;
+	gfp_t flags;
+
+	flags = (in_atomic()) ? GFP_ATOMIC : GFP_KERNEL;
+	if ((p = skb_clone((struct sk_buff*)skb, flags)) == NULL)
+		return NULL;
+
+	
+	if (osh->pub.pkttag)
+		bzero((void*)((struct sk_buff *)p)->cb, OSL_PKTTAG_SZ);
+
+	
+	osh->pub.pktalloced++;
+	return (p);
+}
diff --git a/drivers/net/wireless/bcm4329/miniopt.c b/drivers/net/wireless/bcm4329/miniopt.c
new file mode 100644
index 000000000000..6a184a75f06b
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/miniopt.c
@@ -0,0 +1,163 @@
+/*
+ * Description.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: miniopt.c,v 1.1.6.4 2009/09/25 00:32:01 Exp $
+ */
+
+/* ---- Include Files ---------------------------------------------------- */
+
+#include <typedefs.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "miniopt.h"
+
+
+/* ---- Public Variables ------------------------------------------------- */
+/* ---- Private Constants and Types -------------------------------------- */
+
+
+
+/* ---- Private Variables ------------------------------------------------ */
+/* ---- Private Function Prototypes -------------------------------------- */
+/* ---- Functions -------------------------------------------------------- */
+
+/* ----------------------------------------------------------------------- */
+void
+miniopt_init(miniopt_t *t, const char* name, const char* flags, bool longflags)
+{
+	static const char *null_flags = "";
+
+	memset(t, 0, sizeof(miniopt_t));
+	t->name = name;
+	if (flags == NULL)
+		t->flags = null_flags;
+	else
+		t->flags = flags;
+	t->longflags = longflags;
+}
+
+
+/* ----------------------------------------------------------------------- */
+int
+miniopt(miniopt_t *t, char **argv)
+{
+	int keylen;
+	char *p, *eq, *valstr, *endptr = NULL;
+	int err = 0;
+
+	t->consumed = 0;
+	t->positional = FALSE;
+	memset(t->key, 0, MINIOPT_MAXKEY);
+	t->opt = '\0';
+	t->valstr = NULL;
+	t->good_int = FALSE;
+	valstr = NULL;
+
+	if (*argv == NULL) {
+		err = -1;
+		goto exit;
+	}
+
+	p = *argv++;
+	t->consumed++;
+
+	if (!t->opt_end && !strcmp(p, "--")) {
+		t->opt_end = TRUE;
+		if (*argv == NULL) {
+			err = -1;
+			goto exit;
+		}
+		p = *argv++;
+		t->consumed++;
+	}
+
+	if (t->opt_end) {
+		t->positional = TRUE;
+		valstr = p;
+	}
+	else if (!strncmp(p, "--", 2)) {
+		eq = strchr(p, '=');
+		if (eq == NULL && !t->longflags) {
+			fprintf(stderr,
+				"%s: missing \" = \" in long param \"%s\"\n", t->name, p);
+			err = 1;
+			goto exit;
+		}
+		keylen = eq ? (eq - (p + 2)) : (int)strlen(p) - 2;
+		if (keylen > 63) keylen = 63;
+		memcpy(t->key, p + 2, keylen);
+
+		if (eq) {
+			valstr = eq + 1;
+			if (*valstr == '\0') {
+				fprintf(stderr,
+				        "%s: missing value after \" = \" in long param \"%s\"\n",
+				        t->name, p);
+				err = 1;
+				goto exit;
+			}
+		}
+	}
+	else if (!strncmp(p, "-", 1)) {
+		t->opt = p[1];
+		if (strlen(p) > 2) {
+			fprintf(stderr,
+				"%s: only single char options, error on param \"%s\"\n",
+				t->name, p);
+			err = 1;
+			goto exit;
+		}
+		if (strchr(t->flags, t->opt)) {
+			/* this is a flag option, no value expected */
+			valstr = NULL;
+		} else {
+			if (*argv == NULL) {
+				fprintf(stderr,
+				"%s: missing value parameter after \"%s\"\n", t->name, p);
+				err = 1;
+				goto exit;
+			}
+			valstr = *argv;
+			argv++;
+			t->consumed++;
+		}
+	} else {
+		t->positional = TRUE;
+		valstr = p;
+	}
+
+	/* parse valstr as int just in case */
+	if (valstr) {
+		t->uval = (uint)strtoul(valstr, &endptr, 0);
+		t->val = (int)t->uval;
+		t->good_int = (*endptr == '\0');
+	}
+
+	t->valstr = valstr;
+
+exit:
+	if (err == 1)
+		t->opt = '?';
+
+	return err;
+}
diff --git a/drivers/net/wireless/bcm4329/sbutils.c b/drivers/net/wireless/bcm4329/sbutils.c
new file mode 100644
index 000000000000..46cd51010b78
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/sbutils.c
@@ -0,0 +1,1004 @@
+/*
+ * Misc utility routines for accessing chip-specific features
+ * of the SiliconBackplane-based Broadcom chips.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sbutils.c,v 1.662.4.10.2.7.4.2 2010/04/19 05:48:48 Exp $
+ */
+
+#include <typedefs.h>
+#include <bcmdefs.h>
+#include <osl.h>
+#include <bcmutils.h>
+#include <siutils.h>
+#include <bcmdevs.h>
+#include <hndsoc.h>
+#include <sbchipc.h>
+#include <pcicfg.h>
+#include <sbpcmcia.h>
+
+#include "siutils_priv.h"
+
+/* local prototypes */
+static uint _sb_coreidx(si_info_t *sii, uint32 sba);
+static uint _sb_scan(si_info_t *sii, uint32 sba, void *regs, uint bus, uint32 sbba,
+                     uint ncores);
+static uint32 _sb_coresba(si_info_t *sii);
+static void *_sb_setcoreidx(si_info_t *sii, uint coreidx);
+
+#define	SET_SBREG(sii, r, mask, val)	\
+		W_SBREG((sii), (r), ((R_SBREG((sii), (r)) & ~(mask)) | (val)))
+#define	REGS2SB(va)	(sbconfig_t*) ((int8*)(va) + SBCONFIGOFF)
+
+/* sonicsrev */
+#define	SONICS_2_2	(SBIDL_RV_2_2 >> SBIDL_RV_SHIFT)
+#define	SONICS_2_3	(SBIDL_RV_2_3 >> SBIDL_RV_SHIFT)
+
+#define	R_SBREG(sii, sbr)	sb_read_sbreg((sii), (sbr))
+#define	W_SBREG(sii, sbr, v)	sb_write_sbreg((sii), (sbr), (v))
+#define	AND_SBREG(sii, sbr, v)	W_SBREG((sii), (sbr), (R_SBREG((sii), (sbr)) & (v)))
+#define	OR_SBREG(sii, sbr, v)	W_SBREG((sii), (sbr), (R_SBREG((sii), (sbr)) | (v)))
+
+static uint32
+sb_read_sbreg(si_info_t *sii, volatile uint32 *sbr)
+{
+	uint8 tmp;
+	uint32 val, intr_val = 0;
+
+
+	/*
+	 * compact flash only has 11 bits address, while we needs 12 bits address.
+	 * MEM_SEG will be OR'd with other 11 bits address in hardware,
+	 * so we program MEM_SEG with 12th bit when necessary(access sb regsiters).
+	 * For normal PCMCIA bus(CFTable_regwinsz > 2k), do nothing special
+	 */
+	if (PCMCIA(sii)) {
+		INTR_OFF(sii, intr_val);
+		tmp = 1;
+		OSL_PCMCIA_WRITE_ATTR(sii->osh, MEM_SEG, &tmp, 1);
+		sbr = (volatile uint32 *)((uintptr)sbr & ~(1 << 11)); /* mask out bit 11 */
+	}
+
+	val = R_REG(sii->osh, sbr);
+
+	if (PCMCIA(sii)) {
+		tmp = 0;
+		OSL_PCMCIA_WRITE_ATTR(sii->osh, MEM_SEG, &tmp, 1);
+		INTR_RESTORE(sii, intr_val);
+	}
+
+	return (val);
+}
+
+static void
+sb_write_sbreg(si_info_t *sii, volatile uint32 *sbr, uint32 v)
+{
+	uint8 tmp;
+	volatile uint32 dummy;
+	uint32 intr_val = 0;
+
+
+	/*
+	 * compact flash only has 11 bits address, while we needs 12 bits address.
+	 * MEM_SEG will be OR'd with other 11 bits address in hardware,
+	 * so we program MEM_SEG with 12th bit when necessary(access sb regsiters).
+	 * For normal PCMCIA bus(CFTable_regwinsz > 2k), do nothing special
+	 */
+	if (PCMCIA(sii)) {
+		INTR_OFF(sii, intr_val);
+		tmp = 1;
+		OSL_PCMCIA_WRITE_ATTR(sii->osh, MEM_SEG, &tmp, 1);
+		sbr = (volatile uint32 *)((uintptr)sbr & ~(1 << 11)); /* mask out bit 11 */
+	}
+
+	if (BUSTYPE(sii->pub.bustype) == PCMCIA_BUS) {
+#ifdef IL_BIGENDIAN
+		dummy = R_REG(sii->osh, sbr);
+		W_REG(sii->osh, ((volatile uint16 *)sbr + 1), (uint16)((v >> 16) & 0xffff));
+		dummy = R_REG(sii->osh, sbr);
+		W_REG(sii->osh, (volatile uint16 *)sbr, (uint16)(v & 0xffff));
+#else
+		dummy = R_REG(sii->osh, sbr);
+		W_REG(sii->osh, (volatile uint16 *)sbr, (uint16)(v & 0xffff));
+		dummy = R_REG(sii->osh, sbr);
+		W_REG(sii->osh, ((volatile uint16 *)sbr + 1), (uint16)((v >> 16) & 0xffff));
+#endif	/* IL_BIGENDIAN */
+	} else
+		W_REG(sii->osh, sbr, v);
+
+	if (PCMCIA(sii)) {
+		tmp = 0;
+		OSL_PCMCIA_WRITE_ATTR(sii->osh, MEM_SEG, &tmp, 1);
+		INTR_RESTORE(sii, intr_val);
+	}
+}
+
+uint
+sb_coreid(si_t *sih)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	return ((R_SBREG(sii, &sb->sbidhigh) & SBIDH_CC_MASK) >> SBIDH_CC_SHIFT);
+}
+
+uint
+sb_flag(si_t *sih)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	return R_SBREG(sii, &sb->sbtpsflag) & SBTPS_NUM0_MASK;
+}
+
+void
+sb_setint(si_t *sih, int siflag)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+	uint32 vec;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	if (siflag == -1)
+		vec = 0;
+	else
+		vec = 1 << siflag;
+	W_SBREG(sii, &sb->sbintvec, vec);
+}
+
+/* return core index of the core with address 'sba' */
+static uint
+_sb_coreidx(si_info_t *sii, uint32 sba)
+{
+	uint i;
+
+	for (i = 0; i < sii->numcores; i ++)
+		if (sba == sii->common_info->coresba[i])
+			return i;
+	return BADIDX;
+}
+
+/* return core address of the current core */
+static uint32
+_sb_coresba(si_info_t *sii)
+{
+	uint32 sbaddr;
+
+
+	switch (BUSTYPE(sii->pub.bustype)) {
+	case SI_BUS: {
+		sbconfig_t *sb = REGS2SB(sii->curmap);
+		sbaddr = sb_base(R_SBREG(sii, &sb->sbadmatch0));
+		break;
+	}
+
+	case PCI_BUS:
+		sbaddr = OSL_PCI_READ_CONFIG(sii->osh, PCI_BAR0_WIN, sizeof(uint32));
+		break;
+
+	case PCMCIA_BUS: {
+		uint8 tmp = 0;
+		OSL_PCMCIA_READ_ATTR(sii->osh, PCMCIA_ADDR0, &tmp, 1);
+		sbaddr  = (uint32)tmp << 12;
+		OSL_PCMCIA_READ_ATTR(sii->osh, PCMCIA_ADDR1, &tmp, 1);
+		sbaddr |= (uint32)tmp << 16;
+		OSL_PCMCIA_READ_ATTR(sii->osh, PCMCIA_ADDR2, &tmp, 1);
+		sbaddr |= (uint32)tmp << 24;
+		break;
+	}
+
+	case SPI_BUS:
+	case SDIO_BUS:
+		sbaddr = (uint32)(uintptr)sii->curmap;
+		break;
+
+
+	default:
+		sbaddr = BADCOREADDR;
+		break;
+	}
+
+	return sbaddr;
+}
+
+uint
+sb_corevendor(si_t *sih)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	return ((R_SBREG(sii, &sb->sbidhigh) & SBIDH_VC_MASK) >> SBIDH_VC_SHIFT);
+}
+
+uint
+sb_corerev(si_t *sih)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+	uint sbidh;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+	sbidh = R_SBREG(sii, &sb->sbidhigh);
+
+	return (SBCOREREV(sbidh));
+}
+
+/* set core-specific control flags */
+void
+sb_core_cflags_wo(si_t *sih, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+	uint32 w;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	ASSERT((val & ~mask) == 0);
+
+	/* mask and set */
+	w = (R_SBREG(sii, &sb->sbtmstatelow) & ~(mask << SBTML_SICF_SHIFT)) |
+	        (val << SBTML_SICF_SHIFT);
+	W_SBREG(sii, &sb->sbtmstatelow, w);
+}
+
+/* set/clear core-specific control flags */
+uint32
+sb_core_cflags(si_t *sih, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+	uint32 w;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	ASSERT((val & ~mask) == 0);
+
+	/* mask and set */
+	if (mask || val) {
+		w = (R_SBREG(sii, &sb->sbtmstatelow) & ~(mask << SBTML_SICF_SHIFT)) |
+		        (val << SBTML_SICF_SHIFT);
+		W_SBREG(sii, &sb->sbtmstatelow, w);
+	}
+
+	/* return the new value
+	 * for write operation, the following readback ensures the completion of write opration.
+	 */
+	return (R_SBREG(sii, &sb->sbtmstatelow) >> SBTML_SICF_SHIFT);
+}
+
+/* set/clear core-specific status flags */
+uint32
+sb_core_sflags(si_t *sih, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+	uint32 w;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	ASSERT((val & ~mask) == 0);
+	ASSERT((mask & ~SISF_CORE_BITS) == 0);
+
+	/* mask and set */
+	if (mask || val) {
+		w = (R_SBREG(sii, &sb->sbtmstatehigh) & ~(mask << SBTMH_SISF_SHIFT)) |
+		        (val << SBTMH_SISF_SHIFT);
+		W_SBREG(sii, &sb->sbtmstatehigh, w);
+	}
+
+	/* return the new value */
+	return (R_SBREG(sii, &sb->sbtmstatehigh) >> SBTMH_SISF_SHIFT);
+}
+
+bool
+sb_iscoreup(si_t *sih)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	return ((R_SBREG(sii, &sb->sbtmstatelow) &
+	         (SBTML_RESET | SBTML_REJ_MASK | (SICF_CLOCK_EN << SBTML_SICF_SHIFT))) ==
+	        (SICF_CLOCK_EN << SBTML_SICF_SHIFT));
+}
+
+/*
+ * Switch to 'coreidx', issue a single arbitrary 32bit register mask&set operation,
+ * switch back to the original core, and return the new value.
+ *
+ * When using the silicon backplane, no fidleing with interrupts or core switches are needed.
+ *
+ * Also, when using pci/pcie, we can optimize away the core switching for pci registers
+ * and (on newer pci cores) chipcommon registers.
+ */
+uint
+sb_corereg(si_t *sih, uint coreidx, uint regoff, uint mask, uint val)
+{
+	uint origidx = 0;
+	uint32 *r = NULL;
+	uint w;
+	uint intr_val = 0;
+	bool fast = FALSE;
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	ASSERT(GOODIDX(coreidx));
+	ASSERT(regoff < SI_CORE_SIZE);
+	ASSERT((val & ~mask) == 0);
+
+	if (coreidx >= SI_MAXCORES)
+		return 0;
+
+	if (BUSTYPE(sii->pub.bustype) == SI_BUS) {
+		/* If internal bus, we can always get at everything */
+		fast = TRUE;
+		/* map if does not exist */
+		if (!sii->common_info->regs[coreidx]) {
+			sii->common_info->regs[coreidx] =
+			    REG_MAP(sii->common_info->coresba[coreidx], SI_CORE_SIZE);
+			ASSERT(GOODREGS(sii->common_info->regs[coreidx]));
+		}
+		r = (uint32 *)((uchar *)sii->common_info->regs[coreidx] + regoff);
+	} else if (BUSTYPE(sii->pub.bustype) == PCI_BUS) {
+		/* If pci/pcie, we can get at pci/pcie regs and on newer cores to chipc */
+
+		if ((sii->common_info->coreid[coreidx] == CC_CORE_ID) && SI_FAST(sii)) {
+			/* Chipc registers are mapped at 12KB */
+
+			fast = TRUE;
+			r = (uint32 *)((char *)sii->curmap + PCI_16KB0_CCREGS_OFFSET + regoff);
+		} else if (sii->pub.buscoreidx == coreidx) {
+			/* pci registers are at either in the last 2KB of an 8KB window
+			 * or, in pcie and pci rev 13 at 8KB
+			 */
+			fast = TRUE;
+			if (SI_FAST(sii))
+				r = (uint32 *)((char *)sii->curmap +
+				               PCI_16KB0_PCIREGS_OFFSET + regoff);
+			else
+				r = (uint32 *)((char *)sii->curmap +
+				               ((regoff >= SBCONFIGOFF) ?
+				                PCI_BAR0_PCISBR_OFFSET : PCI_BAR0_PCIREGS_OFFSET) +
+				               regoff);
+		}
+	}
+
+	if (!fast) {
+		INTR_OFF(sii, intr_val);
+
+		/* save current core index */
+		origidx = si_coreidx(&sii->pub);
+
+		/* switch core */
+		r = (uint32*) ((uchar*)sb_setcoreidx(&sii->pub, coreidx) + regoff);
+	}
+	ASSERT(r != NULL);
+
+	/* mask and set */
+	if (mask || val) {
+		if (regoff >= SBCONFIGOFF) {
+			w = (R_SBREG(sii, r) & ~mask) | val;
+			W_SBREG(sii, r, w);
+		} else {
+			w = (R_REG(sii->osh, r) & ~mask) | val;
+			W_REG(sii->osh, r, w);
+		}
+	}
+
+	/* readback */
+	if (regoff >= SBCONFIGOFF)
+		w = R_SBREG(sii, r);
+	else {
+		if ((CHIPID(sii->pub.chip) == BCM5354_CHIP_ID) &&
+		    (coreidx == SI_CC_IDX) &&
+		    (regoff == OFFSETOF(chipcregs_t, watchdog))) {
+			w = val;
+		} else
+			w = R_REG(sii->osh, r);
+	}
+
+	if (!fast) {
+		/* restore core index */
+		if (origidx != coreidx)
+			sb_setcoreidx(&sii->pub, origidx);
+
+		INTR_RESTORE(sii, intr_val);
+	}
+
+	return (w);
+}
+
+/* Scan the enumeration space to find all cores starting from the given
+ * bus 'sbba'. Append coreid and other info to the lists in 'si'. 'sba'
+ * is the default core address at chip POR time and 'regs' is the virtual
+ * address that the default core is mapped at. 'ncores' is the number of
+ * cores expected on bus 'sbba'. It returns the total number of cores
+ * starting from bus 'sbba', inclusive.
+ */
+#define SB_MAXBUSES	2
+static uint
+_sb_scan(si_info_t *sii, uint32 sba, void *regs, uint bus, uint32 sbba, uint numcores)
+{
+	uint next;
+	uint ncc = 0;
+	uint i;
+
+	if (bus >= SB_MAXBUSES) {
+		SI_ERROR(("_sb_scan: bus 0x%08x at level %d is too deep to scan\n", sbba, bus));
+		return 0;
+	}
+	SI_MSG(("_sb_scan: scan bus 0x%08x assume %u cores\n", sbba, numcores));
+
+	/* Scan all cores on the bus starting from core 0.
+	 * Core addresses must be contiguous on each bus.
+	 */
+	for (i = 0, next = sii->numcores; i < numcores && next < SB_BUS_MAXCORES; i++, next++) {
+		sii->common_info->coresba[next] = sbba + (i * SI_CORE_SIZE);
+
+		/* keep and reuse the initial register mapping */
+		if ((BUSTYPE(sii->pub.bustype) == SI_BUS) &&
+			(sii->common_info->coresba[next] == sba)) {
+			SI_MSG(("_sb_scan: reuse mapped regs %p for core %u\n", regs, next));
+			sii->common_info->regs[next] = regs;
+		}
+
+		/* change core to 'next' and read its coreid */
+		sii->curmap = _sb_setcoreidx(sii, next);
+		sii->curidx = next;
+
+		sii->common_info->coreid[next] = sb_coreid(&sii->pub);
+
+		/* core specific processing... */
+		/* chipc provides # cores */
+		if (sii->common_info->coreid[next] == CC_CORE_ID) {
+			chipcregs_t *cc = (chipcregs_t *)sii->curmap;
+			uint32 ccrev = sb_corerev(&sii->pub);
+
+			/* determine numcores - this is the total # cores in the chip */
+			if (((ccrev == 4) || (ccrev >= 6)))
+				numcores = (R_REG(sii->osh, &cc->chipid) & CID_CC_MASK) >>
+				        CID_CC_SHIFT;
+			else {
+				/* Older chips */
+				uint chip = sii->pub.chip;
+
+				if (chip == BCM4306_CHIP_ID)	/* < 4306c0 */
+					numcores = 6;
+				else if (chip == BCM4704_CHIP_ID)
+					numcores = 9;
+				else if (chip == BCM5365_CHIP_ID)
+					numcores = 7;
+				else {
+					SI_ERROR(("sb_chip2numcores: unsupported chip 0x%x\n",
+					          chip));
+					ASSERT(0);
+					numcores = 1;
+				}
+			}
+			SI_MSG(("_sb_scan: there are %u cores in the chip %s\n", numcores,
+				sii->pub.issim ? "QT" : ""));
+		}
+		/* scan bridged SB(s) and add results to the end of the list */
+		else if (sii->common_info->coreid[next] == OCP_CORE_ID) {
+			sbconfig_t *sb = REGS2SB(sii->curmap);
+			uint32 nsbba = R_SBREG(sii, &sb->sbadmatch1);
+			uint nsbcc;
+
+			sii->numcores = next + 1;
+
+			if ((nsbba & 0xfff00000) != SI_ENUM_BASE)
+				continue;
+			nsbba &= 0xfffff000;
+			if (_sb_coreidx(sii, nsbba) != BADIDX)
+				continue;
+
+			nsbcc = (R_SBREG(sii, &sb->sbtmstatehigh) & 0x000f0000) >> 16;
+			nsbcc = _sb_scan(sii, sba, regs, bus + 1, nsbba, nsbcc);
+			if (sbba == SI_ENUM_BASE)
+				numcores -= nsbcc;
+			ncc += nsbcc;
+		}
+	}
+
+	SI_MSG(("_sb_scan: found %u cores on bus 0x%08x\n", i, sbba));
+
+	sii->numcores = i + ncc;
+	return sii->numcores;
+}
+
+/* scan the sb enumerated space to identify all cores */
+void
+sb_scan(si_t *sih, void *regs, uint devid)
+{
+	si_info_t *sii;
+	uint32 origsba;
+
+	sii = SI_INFO(sih);
+
+	/* Save the current core info and validate it later till we know
+	 * for sure what is good and what is bad.
+	 */
+	origsba = _sb_coresba(sii);
+
+	/* scan all SB(s) starting from SI_ENUM_BASE */
+	sii->numcores = _sb_scan(sii, origsba, regs, 0, SI_ENUM_BASE, 1);
+}
+
+/*
+ * This function changes logical "focus" to the indicated core;
+ * must be called with interrupts off.
+ * Moreover, callers should keep interrupts off during switching out of and back to d11 core
+ */
+void *
+sb_setcoreidx(si_t *sih, uint coreidx)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	if (coreidx >= sii->numcores)
+		return (NULL);
+
+	/*
+	 * If the user has provided an interrupt mask enabled function,
+	 * then assert interrupts are disabled before switching the core.
+	 */
+	ASSERT((sii->intrsenabled_fn == NULL) || !(*(sii)->intrsenabled_fn)((sii)->intr_arg));
+
+	sii->curmap = _sb_setcoreidx(sii, coreidx);
+	sii->curidx = coreidx;
+
+	return (sii->curmap);
+}
+
+/* This function changes the logical "focus" to the indicated core.
+ * Return the current core's virtual address.
+ */
+static void *
+_sb_setcoreidx(si_info_t *sii, uint coreidx)
+{
+	uint32 sbaddr = sii->common_info->coresba[coreidx];
+	void *regs;
+
+	switch (BUSTYPE(sii->pub.bustype)) {
+	case SI_BUS:
+		/* map new one */
+		if (!sii->common_info->regs[coreidx]) {
+			sii->common_info->regs[coreidx] = REG_MAP(sbaddr, SI_CORE_SIZE);
+			ASSERT(GOODREGS(sii->common_info->regs[coreidx]));
+		}
+		regs = sii->common_info->regs[coreidx];
+		break;
+
+	case PCI_BUS:
+		/* point bar0 window */
+		OSL_PCI_WRITE_CONFIG(sii->osh, PCI_BAR0_WIN, 4, sbaddr);
+		regs = sii->curmap;
+		break;
+
+	case PCMCIA_BUS: {
+		uint8 tmp = (sbaddr >> 12) & 0x0f;
+		OSL_PCMCIA_WRITE_ATTR(sii->osh, PCMCIA_ADDR0, &tmp, 1);
+		tmp = (sbaddr >> 16) & 0xff;
+		OSL_PCMCIA_WRITE_ATTR(sii->osh, PCMCIA_ADDR1, &tmp, 1);
+		tmp = (sbaddr >> 24) & 0xff;
+		OSL_PCMCIA_WRITE_ATTR(sii->osh, PCMCIA_ADDR2, &tmp, 1);
+		regs = sii->curmap;
+		break;
+	}
+	case SPI_BUS:
+	case SDIO_BUS:
+		/* map new one */
+		if (!sii->common_info->regs[coreidx]) {
+			sii->common_info->regs[coreidx] = (void *)(uintptr)sbaddr;
+			ASSERT(GOODREGS(sii->common_info->regs[coreidx]));
+		}
+		regs = sii->common_info->regs[coreidx];
+		break;
+
+
+	default:
+		ASSERT(0);
+		regs = NULL;
+		break;
+	}
+
+	return regs;
+}
+
+/* Return the address of sbadmatch0/1/2/3 register */
+static volatile uint32 *
+sb_admatch(si_info_t *sii, uint asidx)
+{
+	sbconfig_t *sb;
+	volatile uint32 *addrm;
+
+	sb = REGS2SB(sii->curmap);
+
+	switch (asidx) {
+	case 0:
+		addrm =  &sb->sbadmatch0;
+		break;
+
+	case 1:
+		addrm =  &sb->sbadmatch1;
+		break;
+
+	case 2:
+		addrm =  &sb->sbadmatch2;
+		break;
+
+	case 3:
+		addrm =  &sb->sbadmatch3;
+		break;
+
+	default:
+		SI_ERROR(("%s: Address space index (%d) out of range\n", __FUNCTION__, asidx));
+		return 0;
+	}
+
+	return (addrm);
+}
+
+/* Return the number of address spaces in current core */
+int
+sb_numaddrspaces(si_t *sih)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	/* + 1 because of enumeration space */
+	return ((R_SBREG(sii, &sb->sbidlow) & SBIDL_AR_MASK) >> SBIDL_AR_SHIFT) + 1;
+}
+
+/* Return the address of the nth address space in the current core */
+uint32
+sb_addrspace(si_t *sih, uint asidx)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	return (sb_base(R_SBREG(sii, sb_admatch(sii, asidx))));
+}
+
+/* Return the size of the nth address space in the current core */
+uint32
+sb_addrspacesize(si_t *sih, uint asidx)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	return (sb_size(R_SBREG(sii, sb_admatch(sii, asidx))));
+}
+
+
+/* do buffered registers update */
+void
+sb_commit(si_t *sih)
+{
+	si_info_t *sii;
+	uint origidx;
+	uint intr_val = 0;
+
+	sii = SI_INFO(sih);
+
+	origidx = sii->curidx;
+	ASSERT(GOODIDX(origidx));
+
+	INTR_OFF(sii, intr_val);
+
+	/* switch over to chipcommon core if there is one, else use pci */
+	if (sii->pub.ccrev != NOREV) {
+		chipcregs_t *ccregs = (chipcregs_t *)si_setcore(sih, CC_CORE_ID, 0);
+
+		/* do the buffer registers update */
+		W_REG(sii->osh, &ccregs->broadcastaddress, SB_COMMIT);
+		W_REG(sii->osh, &ccregs->broadcastdata, 0x0);
+	} else
+		ASSERT(0);
+
+	/* restore core index */
+	sb_setcoreidx(sih, origidx);
+	INTR_RESTORE(sii, intr_val);
+}
+
+void
+sb_core_disable(si_t *sih, uint32 bits)
+{
+	si_info_t *sii;
+	volatile uint32 dummy;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+
+	ASSERT(GOODREGS(sii->curmap));
+	sb = REGS2SB(sii->curmap);
+
+	/* if core is already in reset, just return */
+	if (R_SBREG(sii, &sb->sbtmstatelow) & SBTML_RESET)
+		return;
+
+	/* if clocks are not enabled, put into reset and return */
+	if ((R_SBREG(sii, &sb->sbtmstatelow) & (SICF_CLOCK_EN << SBTML_SICF_SHIFT)) == 0)
+		goto disable;
+
+	/* set target reject and spin until busy is clear (preserve core-specific bits) */
+	OR_SBREG(sii, &sb->sbtmstatelow, SBTML_REJ);
+	dummy = R_SBREG(sii, &sb->sbtmstatelow);
+	OSL_DELAY(1);
+	SPINWAIT((R_SBREG(sii, &sb->sbtmstatehigh) & SBTMH_BUSY), 100000);
+	if (R_SBREG(sii, &sb->sbtmstatehigh) & SBTMH_BUSY)
+		SI_ERROR(("%s: target state still busy\n", __FUNCTION__));
+
+	if (R_SBREG(sii, &sb->sbidlow) & SBIDL_INIT) {
+		OR_SBREG(sii, &sb->sbimstate, SBIM_RJ);
+		dummy = R_SBREG(sii, &sb->sbimstate);
+		OSL_DELAY(1);
+		SPINWAIT((R_SBREG(sii, &sb->sbimstate) & SBIM_BY), 100000);
+	}
+
+	/* set reset and reject while enabling the clocks */
+	W_SBREG(sii, &sb->sbtmstatelow,
+	        (((bits | SICF_FGC | SICF_CLOCK_EN) << SBTML_SICF_SHIFT) |
+	         SBTML_REJ | SBTML_RESET));
+	dummy = R_SBREG(sii, &sb->sbtmstatelow);
+	OSL_DELAY(10);
+
+	/* don't forget to clear the initiator reject bit */
+	if (R_SBREG(sii, &sb->sbidlow) & SBIDL_INIT)
+		AND_SBREG(sii, &sb->sbimstate, ~SBIM_RJ);
+
+disable:
+	/* leave reset and reject asserted */
+	W_SBREG(sii, &sb->sbtmstatelow, ((bits << SBTML_SICF_SHIFT) | SBTML_REJ | SBTML_RESET));
+	OSL_DELAY(1);
+}
+
+/* reset and re-enable a core
+ * inputs:
+ * bits - core specific bits that are set during and after reset sequence
+ * resetbits - core specific bits that are set only during reset sequence
+ */
+void
+sb_core_reset(si_t *sih, uint32 bits, uint32 resetbits)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+	volatile uint32 dummy;
+
+	sii = SI_INFO(sih);
+	ASSERT(GOODREGS(sii->curmap));
+	sb = REGS2SB(sii->curmap);
+
+	/*
+	 * Must do the disable sequence first to work for arbitrary current core state.
+	 */
+	sb_core_disable(sih, (bits | resetbits));
+
+	/*
+	 * Now do the initialization sequence.
+	 */
+
+	/* set reset while enabling the clock and forcing them on throughout the core */
+	W_SBREG(sii, &sb->sbtmstatelow,
+	        (((bits | resetbits | SICF_FGC | SICF_CLOCK_EN) << SBTML_SICF_SHIFT) |
+	         SBTML_RESET));
+	dummy = R_SBREG(sii, &sb->sbtmstatelow);
+	OSL_DELAY(1);
+
+	if (R_SBREG(sii, &sb->sbtmstatehigh) & SBTMH_SERR) {
+		W_SBREG(sii, &sb->sbtmstatehigh, 0);
+	}
+	if ((dummy = R_SBREG(sii, &sb->sbimstate)) & (SBIM_IBE | SBIM_TO)) {
+		AND_SBREG(sii, &sb->sbimstate, ~(SBIM_IBE | SBIM_TO));
+	}
+
+	/* clear reset and allow it to propagate throughout the core */
+	W_SBREG(sii, &sb->sbtmstatelow,
+	        ((bits | resetbits | SICF_FGC | SICF_CLOCK_EN) << SBTML_SICF_SHIFT));
+	dummy = R_SBREG(sii, &sb->sbtmstatelow);
+	OSL_DELAY(1);
+
+	/* leave clock enabled */
+	W_SBREG(sii, &sb->sbtmstatelow, ((bits | SICF_CLOCK_EN) << SBTML_SICF_SHIFT));
+	dummy = R_SBREG(sii, &sb->sbtmstatelow);
+	OSL_DELAY(1);
+}
+
+void
+sb_core_tofixup(si_t *sih)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+
+	if ((BUSTYPE(sii->pub.bustype) != PCI_BUS) || PCIE(sii) ||
+	    (PCI(sii) && (sii->pub.buscorerev >= 5)))
+		return;
+
+	ASSERT(GOODREGS(sii->curmap));
+	sb = REGS2SB(sii->curmap);
+
+	if (BUSTYPE(sii->pub.bustype) == SI_BUS) {
+		SET_SBREG(sii, &sb->sbimconfiglow,
+		          SBIMCL_RTO_MASK | SBIMCL_STO_MASK,
+		          (0x5 << SBIMCL_RTO_SHIFT) | 0x3);
+	} else {
+		if (sb_coreid(sih) == PCI_CORE_ID) {
+			SET_SBREG(sii, &sb->sbimconfiglow,
+			          SBIMCL_RTO_MASK | SBIMCL_STO_MASK,
+			          (0x3 << SBIMCL_RTO_SHIFT) | 0x2);
+		} else {
+			SET_SBREG(sii, &sb->sbimconfiglow, (SBIMCL_RTO_MASK | SBIMCL_STO_MASK), 0);
+		}
+	}
+
+	sb_commit(sih);
+}
+
+/*
+ * Set the initiator timeout for the "master core".
+ * The master core is defined to be the core in control
+ * of the chip and so it issues accesses to non-memory
+ * locations (Because of dma *any* core can access memeory).
+ *
+ * The routine uses the bus to decide who is the master:
+ *	SI_BUS => mips
+ *	JTAG_BUS => chipc
+ *	PCI_BUS => pci or pcie
+ *	PCMCIA_BUS => pcmcia
+ *	SDIO_BUS => pcmcia
+ *
+ * This routine exists so callers can disable initiator
+ * timeouts so accesses to very slow devices like otp
+ * won't cause an abort. The routine allows arbitrary
+ * settings of the service and request timeouts, though.
+ *
+ * Returns the timeout state before changing it or -1
+ * on error.
+ */
+
+#define	TO_MASK	(SBIMCL_RTO_MASK | SBIMCL_STO_MASK)
+
+uint32
+sb_set_initiator_to(si_t *sih, uint32 to, uint idx)
+{
+	si_info_t *sii;
+	uint origidx;
+	uint intr_val = 0;
+	uint32 tmp, ret = 0xffffffff;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+
+	if ((to & ~TO_MASK) != 0)
+		return ret;
+
+	/* Figure out the master core */
+	if (idx == BADIDX) {
+		switch (BUSTYPE(sii->pub.bustype)) {
+		case PCI_BUS:
+			idx = sii->pub.buscoreidx;
+			break;
+		case JTAG_BUS:
+			idx = SI_CC_IDX;
+			break;
+		case PCMCIA_BUS:
+		case SDIO_BUS:
+			idx = si_findcoreidx(sih, PCMCIA_CORE_ID, 0);
+			break;
+		case SI_BUS:
+			idx = si_findcoreidx(sih, MIPS33_CORE_ID, 0);
+			break;
+		default:
+			ASSERT(0);
+		}
+		if (idx == BADIDX)
+			return ret;
+	}
+
+	INTR_OFF(sii, intr_val);
+	origidx = si_coreidx(sih);
+
+	sb = REGS2SB(sb_setcoreidx(sih, idx));
+
+	tmp = R_SBREG(sii, &sb->sbimconfiglow);
+	ret = tmp & TO_MASK;
+	W_SBREG(sii, &sb->sbimconfiglow, (tmp & ~TO_MASK) | to);
+
+	sb_commit(sih);
+	sb_setcoreidx(sih, origidx);
+	INTR_RESTORE(sii, intr_val);
+	return ret;
+}
+
+uint32
+sb_base(uint32 admatch)
+{
+	uint32 base;
+	uint type;
+
+	type = admatch & SBAM_TYPE_MASK;
+	ASSERT(type < 3);
+
+	base = 0;
+
+	if (type == 0) {
+		base = admatch & SBAM_BASE0_MASK;
+	} else if (type == 1) {
+		ASSERT(!(admatch & SBAM_ADNEG));	/* neg not supported */
+		base = admatch & SBAM_BASE1_MASK;
+	} else if (type == 2) {
+		ASSERT(!(admatch & SBAM_ADNEG));	/* neg not supported */
+		base = admatch & SBAM_BASE2_MASK;
+	}
+
+	return (base);
+}
+
+uint32
+sb_size(uint32 admatch)
+{
+	uint32 size;
+	uint type;
+
+	type = admatch & SBAM_TYPE_MASK;
+	ASSERT(type < 3);
+
+	size = 0;
+
+	if (type == 0) {
+		size = 1 << (((admatch & SBAM_ADINT0_MASK) >> SBAM_ADINT0_SHIFT) + 1);
+	} else if (type == 1) {
+		ASSERT(!(admatch & SBAM_ADNEG));	/* neg not supported */
+		size = 1 << (((admatch & SBAM_ADINT1_MASK) >> SBAM_ADINT1_SHIFT) + 1);
+	} else if (type == 2) {
+		ASSERT(!(admatch & SBAM_ADNEG));	/* neg not supported */
+		size = 1 << (((admatch & SBAM_ADINT2_MASK) >> SBAM_ADINT2_SHIFT) + 1);
+	}
+
+	return (size);
+}
diff --git a/drivers/net/wireless/bcm4329/siutils.c b/drivers/net/wireless/bcm4329/siutils.c
new file mode 100644
index 000000000000..1814db0f9dd6
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/siutils.c
@@ -0,0 +1,1527 @@
+/*
+ * Misc utility routines for accessing chip-specific features
+ * of the SiliconBackplane-based Broadcom chips.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: siutils.c,v 1.662.4.4.4.16.4.28 2010/06/23 21:37:54 Exp $
+ */
+
+#include <typedefs.h>
+#include <bcmdefs.h>
+#include <osl.h>
+#include <bcmutils.h>
+#include <siutils.h>
+#include <bcmdevs.h>
+#include <hndsoc.h>
+#include <sbchipc.h>
+#include <pcicfg.h>
+#include <sbpcmcia.h>
+#include <sbsocram.h>
+#include <bcmsdh.h>
+#include <sdio.h>
+#include <sbsdio.h>
+#include <sbhnddma.h>
+#include <sbsdpcmdev.h>
+#include <bcmsdpcm.h>
+#include <hndpmu.h>
+
+#include "siutils_priv.h"
+
+/* local prototypes */
+static si_info_t *si_doattach(si_info_t *sii, uint devid, osl_t *osh, void *regs,
+                              uint bustype, void *sdh, char **vars, uint *varsz);
+static bool si_buscore_prep(si_info_t *sii, uint bustype, uint devid, void *sdh);
+static bool si_buscore_setup(si_info_t *sii, chipcregs_t *cc, uint bustype, uint32 savewin,
+	uint *origidx, void *regs);
+
+
+/* global variable to indicate reservation/release of gpio's */
+static uint32 si_gpioreservation = 0;
+static void *common_info_alloced = NULL;
+
+/* global flag to prevent shared resources from being initialized multiple times in si_attach() */
+
+/*
+ * Allocate a si handle.
+ * devid - pci device id (used to determine chip#)
+ * osh - opaque OS handle
+ * regs - virtual address of initial core registers
+ * bustype - pci/pcmcia/sb/sdio/etc
+ * vars - pointer to a pointer area for "environment" variables
+ * varsz - pointer to int to return the size of the vars
+ */
+si_t *
+si_attach(uint devid, osl_t *osh, void *regs,
+                       uint bustype, void *sdh, char **vars, uint *varsz)
+{
+	si_info_t *sii;
+
+	/* alloc si_info_t */
+	if ((sii = MALLOC(osh, sizeof (si_info_t))) == NULL) {
+		SI_ERROR(("si_attach: malloc failed! malloced %d bytes\n", MALLOCED(osh)));
+		return (NULL);
+	}
+
+	if (si_doattach(sii, devid, osh, regs, bustype, sdh, vars, varsz) == NULL) {
+		if (NULL != sii->common_info)
+			MFREE(osh, sii->common_info, sizeof(si_common_info_t));
+		MFREE(osh, sii, sizeof(si_info_t));
+		return (NULL);
+	}
+	sii->vars = vars ? *vars : NULL;
+	sii->varsz = varsz ? *varsz : 0;
+
+	return (si_t *)sii;
+}
+
+/* global kernel resource */
+static si_info_t ksii;
+
+static uint32	wd_msticks;		/* watchdog timer ticks normalized to ms */
+
+/* generic kernel variant of si_attach() */
+si_t *
+si_kattach(osl_t *osh)
+{
+	static bool ksii_attached = FALSE;
+
+	if (!ksii_attached) {
+		void *regs = REG_MAP(SI_ENUM_BASE, SI_CORE_SIZE);
+
+		if (si_doattach(&ksii, BCM4710_DEVICE_ID, osh, regs,
+		                SI_BUS, NULL,
+		                osh != SI_OSH ? &ksii.vars : NULL,
+		                osh != SI_OSH ? &ksii.varsz : NULL) == NULL) {
+			if (NULL != ksii.common_info)
+				MFREE(osh, ksii.common_info, sizeof(si_common_info_t));
+			SI_ERROR(("si_kattach: si_doattach failed\n"));
+			REG_UNMAP(regs);
+			return NULL;
+		}
+		REG_UNMAP(regs);
+
+		/* save ticks normalized to ms for si_watchdog_ms() */
+		if (PMUCTL_ENAB(&ksii.pub)) {
+			/* based on 32KHz ILP clock */
+			wd_msticks = 32;
+		} else {
+			wd_msticks = ALP_CLOCK / 1000;
+		}
+
+		ksii_attached = TRUE;
+		SI_MSG(("si_kattach done. ccrev = %d, wd_msticks = %d\n",
+		        ksii.pub.ccrev, wd_msticks));
+	}
+
+	return &ksii.pub;
+}
+
+
+static bool
+si_buscore_prep(si_info_t *sii, uint bustype, uint devid, void *sdh)
+{
+	/* need to set memseg flag for CF card first before any sb registers access */
+	if (BUSTYPE(bustype) == PCMCIA_BUS)
+		sii->memseg = TRUE;
+
+
+	if (BUSTYPE(bustype) == SDIO_BUS) {
+		int err;
+		uint8 clkset;
+
+		/* Try forcing SDIO core to do ALPAvail request only */
+		clkset = SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_ALP_AVAIL_REQ;
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, clkset, &err);
+		if (!err) {
+			uint8 clkval;
+
+			/* If register supported, wait for ALPAvail and then force ALP */
+			clkval = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, NULL);
+			if ((clkval & ~SBSDIO_AVBITS) == clkset) {
+				SPINWAIT(((clkval = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+					SBSDIO_FUNC1_CHIPCLKCSR, NULL)), !SBSDIO_ALPAV(clkval)),
+					PMU_MAX_TRANSITION_DLY);
+				if (!SBSDIO_ALPAV(clkval)) {
+					SI_ERROR(("timeout on ALPAV wait, clkval 0x%02x\n",
+						clkval));
+					return FALSE;
+				}
+				clkset = SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_FORCE_ALP;
+				bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
+					clkset, &err);
+				OSL_DELAY(65);
+			}
+		}
+
+		/* Also, disable the extra SDIO pull-ups */
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_SDIOPULLUP, 0, NULL);
+	}
+
+
+	return TRUE;
+}
+
+static bool
+si_buscore_setup(si_info_t *sii, chipcregs_t *cc, uint bustype, uint32 savewin,
+	uint *origidx, void *regs)
+{
+	bool pci, pcie;
+	uint i;
+	uint pciidx, pcieidx, pcirev, pcierev;
+
+	cc = si_setcoreidx(&sii->pub, SI_CC_IDX);
+	ASSERT((uintptr)cc);
+
+	/* get chipcommon rev */
+	sii->pub.ccrev = (int)si_corerev(&sii->pub);
+
+	/* get chipcommon chipstatus */
+	if (sii->pub.ccrev >= 11)
+		sii->pub.chipst = R_REG(sii->osh, &cc->chipstatus);
+
+	/* get chipcommon capabilites */
+	sii->pub.cccaps = R_REG(sii->osh, &cc->capabilities);
+
+	/* get pmu rev and caps */
+	if (sii->pub.cccaps & CC_CAP_PMU) {
+		sii->pub.pmucaps = R_REG(sii->osh, &cc->pmucapabilities);
+		sii->pub.pmurev = sii->pub.pmucaps & PCAP_REV_MASK;
+	}
+
+	SI_MSG(("Chipc: rev %d, caps 0x%x, chipst 0x%x pmurev %d, pmucaps 0x%x\n",
+		sii->pub.ccrev, sii->pub.cccaps, sii->pub.chipst, sii->pub.pmurev,
+		sii->pub.pmucaps));
+
+	/* figure out bus/orignal core idx */
+	sii->pub.buscoretype = NODEV_CORE_ID;
+	sii->pub.buscorerev = NOREV;
+	sii->pub.buscoreidx = BADIDX;
+
+	pci = pcie = FALSE;
+	pcirev = pcierev = NOREV;
+	pciidx = pcieidx = BADIDX;
+
+	for (i = 0; i < sii->numcores; i++) {
+		uint cid, crev;
+
+		si_setcoreidx(&sii->pub, i);
+		cid = si_coreid(&sii->pub);
+		crev = si_corerev(&sii->pub);
+
+		/* Display cores found */
+		SI_MSG(("CORE[%d]: id 0x%x rev %d base 0x%x regs 0x%p\n",
+		        i, cid, crev, sii->common_info->coresba[i], sii->common_info->regs[i]));
+
+		if (BUSTYPE(bustype) == PCI_BUS) {
+			if (cid == PCI_CORE_ID) {
+				pciidx = i;
+				pcirev = crev;
+				pci = TRUE;
+			} else if (cid == PCIE_CORE_ID) {
+				pcieidx = i;
+				pcierev = crev;
+				pcie = TRUE;
+			}
+		} else if ((BUSTYPE(bustype) == PCMCIA_BUS) &&
+		           (cid == PCMCIA_CORE_ID)) {
+			sii->pub.buscorerev = crev;
+			sii->pub.buscoretype = cid;
+			sii->pub.buscoreidx = i;
+		}
+		else if (((BUSTYPE(bustype) == SDIO_BUS) ||
+		          (BUSTYPE(bustype) == SPI_BUS)) &&
+		         ((cid == PCMCIA_CORE_ID) ||
+		          (cid == SDIOD_CORE_ID))) {
+			sii->pub.buscorerev = crev;
+			sii->pub.buscoretype = cid;
+			sii->pub.buscoreidx = i;
+		}
+
+		/* find the core idx before entering this func. */
+		if ((savewin && (savewin == sii->common_info->coresba[i])) ||
+		    (regs == sii->common_info->regs[i]))
+			*origidx = i;
+	}
+
+
+	SI_MSG(("Buscore id/type/rev %d/0x%x/%d\n", sii->pub.buscoreidx, sii->pub.buscoretype,
+		sii->pub.buscorerev));
+
+	if (BUSTYPE(sii->pub.bustype) == SI_BUS && (CHIPID(sii->pub.chip) == BCM4712_CHIP_ID) &&
+	    (sii->pub.chippkg != BCM4712LARGE_PKG_ID) && (sii->pub.chiprev <= 3))
+		OR_REG(sii->osh, &cc->slow_clk_ctl, SCC_SS_XTAL);
+
+
+	/* Make sure any on-chip ARM is off (in case strapping is wrong), or downloaded code was
+	 * already running.
+	 */
+	if ((BUSTYPE(bustype) == SDIO_BUS) || (BUSTYPE(bustype) == SPI_BUS)) {
+		if (si_setcore(&sii->pub, ARM7S_CORE_ID, 0) ||
+		    si_setcore(&sii->pub, ARMCM3_CORE_ID, 0))
+			si_core_disable(&sii->pub, 0);
+	}
+
+	/* return to the original core */
+	si_setcoreidx(&sii->pub, *origidx);
+
+	return TRUE;
+}
+
+
+
+static si_info_t *
+si_doattach(si_info_t *sii, uint devid, osl_t *osh, void *regs,
+                       uint bustype, void *sdh, char **vars, uint *varsz)
+{
+	struct si_pub *sih = &sii->pub;
+	uint32 w, savewin;
+	chipcregs_t *cc;
+	char *pvars = NULL;
+	uint origidx;
+
+	ASSERT(GOODREGS(regs));
+
+	bzero((uchar*)sii, sizeof(si_info_t));
+
+
+	{
+		if (NULL == (common_info_alloced = (void *)MALLOC(osh, sizeof(si_common_info_t)))) {
+			SI_ERROR(("si_doattach: malloc failed! malloced %dbytes\n", MALLOCED(osh)));
+			return (NULL);
+		}
+		bzero((uchar*)(common_info_alloced), sizeof(si_common_info_t));
+	}
+	sii->common_info = (si_common_info_t *)common_info_alloced;
+	sii->common_info->attach_count++;
+
+	savewin = 0;
+
+	sih->buscoreidx = BADIDX;
+
+	sii->curmap = regs;
+	sii->sdh = sdh;
+	sii->osh = osh;
+
+
+	/* find Chipcommon address */
+	if (bustype == PCI_BUS) {
+		savewin = OSL_PCI_READ_CONFIG(sii->osh, PCI_BAR0_WIN, sizeof(uint32));
+		if (!GOODCOREADDR(savewin, SI_ENUM_BASE))
+			savewin = SI_ENUM_BASE;
+		OSL_PCI_WRITE_CONFIG(sii->osh, PCI_BAR0_WIN, 4, SI_ENUM_BASE);
+		cc = (chipcregs_t *)regs;
+	} else
+	if ((bustype == SDIO_BUS) || (bustype == SPI_BUS)) {
+		cc = (chipcregs_t *)sii->curmap;
+	} else {
+		cc = (chipcregs_t *)REG_MAP(SI_ENUM_BASE, SI_CORE_SIZE);
+	}
+
+	sih->bustype = bustype;
+	if (bustype != BUSTYPE(bustype)) {
+		SI_ERROR(("si_doattach: bus type %d does not match configured bus type %d\n",
+			bustype, BUSTYPE(bustype)));
+		return NULL;
+	}
+
+	/* bus/core/clk setup for register access */
+	if (!si_buscore_prep(sii, bustype, devid, sdh)) {
+		SI_ERROR(("si_doattach: si_core_clk_prep failed %d\n", bustype));
+		return NULL;
+	}
+
+	/* ChipID recognition.
+	 *   We assume we can read chipid at offset 0 from the regs arg.
+	 *   If we add other chiptypes (or if we need to support old sdio hosts w/o chipcommon),
+	 *   some way of recognizing them needs to be added here.
+	 */
+	w = R_REG(osh, &cc->chipid);
+	sih->socitype = (w & CID_TYPE_MASK) >> CID_TYPE_SHIFT;
+	/* Might as wll fill in chip id rev & pkg */
+	sih->chip = w & CID_ID_MASK;
+	sih->chiprev = (w & CID_REV_MASK) >> CID_REV_SHIFT;
+	sih->chippkg = (w & CID_PKG_MASK) >> CID_PKG_SHIFT;
+	if ((CHIPID(sih->chip) == BCM4329_CHIP_ID) && (sih->chippkg != BCM4329_289PIN_PKG_ID))
+		sih->chippkg = BCM4329_182PIN_PKG_ID;
+	sih->issim = IS_SIM(sih->chippkg);
+
+	/* scan for cores */
+	if (CHIPTYPE(sii->pub.socitype) == SOCI_SB) {
+		SI_MSG(("Found chip type SB (0x%08x)\n", w));
+		sb_scan(&sii->pub, regs, devid);
+	} else if (CHIPTYPE(sii->pub.socitype) == SOCI_AI) {
+		SI_MSG(("Found chip type AI (0x%08x)\n", w));
+		/* pass chipc address instead of original core base */
+		ai_scan(&sii->pub, (void *)cc, devid);
+	} else {
+		SI_ERROR(("Found chip of unkown type (0x%08x)\n", w));
+		return NULL;
+	}
+	/* no cores found, bail out */
+	if (sii->numcores == 0) {
+		SI_ERROR(("si_doattach: could not find any cores\n"));
+		return NULL;
+	}
+	/* bus/core/clk setup */
+	origidx = SI_CC_IDX;
+	if (!si_buscore_setup(sii, cc, bustype, savewin, &origidx, regs)) {
+		SI_ERROR(("si_doattach: si_buscore_setup failed\n"));
+		return NULL;
+	}
+
+	pvars = NULL;
+
+
+
+		if (sii->pub.ccrev >= 20) {
+			cc = (chipcregs_t *)si_setcore(sih, CC_CORE_ID, 0);
+			W_REG(osh, &cc->gpiopullup, 0);
+			W_REG(osh, &cc->gpiopulldown, 0);
+			si_setcoreidx(sih, origidx);
+		}
+
+		/* Skip PMU initialization from the Dongle Host.
+		 * Firmware will take care of it when it comes up.
+		 */
+
+
+
+	return (sii);
+}
+
+/* may be called with core in reset */
+void
+si_detach(si_t *sih)
+{
+	si_info_t *sii;
+	uint idx;
+
+	sii = SI_INFO(sih);
+
+	if (sii == NULL)
+		return;
+
+	if (BUSTYPE(sih->bustype) == SI_BUS)
+		for (idx = 0; idx < SI_MAXCORES; idx++)
+			if (sii->common_info->regs[idx]) {
+				REG_UNMAP(sii->common_info->regs[idx]);
+				sii->common_info->regs[idx] = NULL;
+			}
+
+
+	if (1 == sii->common_info->attach_count--) {
+		MFREE(sii->osh, sii->common_info, sizeof(si_common_info_t));
+		common_info_alloced = NULL;
+	}
+
+#if !defined(BCMBUSTYPE) || (BCMBUSTYPE == SI_BUS)
+	if (sii != &ksii)
+#endif	/* !BCMBUSTYPE || (BCMBUSTYPE == SI_BUS) */
+		MFREE(sii->osh, sii, sizeof(si_info_t));
+}
+
+void *
+si_osh(si_t *sih)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	return sii->osh;
+}
+
+void
+si_setosh(si_t *sih, osl_t *osh)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	if (sii->osh != NULL) {
+		SI_ERROR(("osh is already set....\n"));
+		ASSERT(!sii->osh);
+	}
+	sii->osh = osh;
+}
+
+/* register driver interrupt disabling and restoring callback functions */
+void
+si_register_intr_callback(si_t *sih, void *intrsoff_fn, void *intrsrestore_fn,
+                          void *intrsenabled_fn, void *intr_arg)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	sii->intr_arg = intr_arg;
+	sii->intrsoff_fn = (si_intrsoff_t)intrsoff_fn;
+	sii->intrsrestore_fn = (si_intrsrestore_t)intrsrestore_fn;
+	sii->intrsenabled_fn = (si_intrsenabled_t)intrsenabled_fn;
+	/* save current core id.  when this function called, the current core
+	 * must be the core which provides driver functions(il, et, wl, etc.)
+	 */
+	sii->dev_coreid = sii->common_info->coreid[sii->curidx];
+}
+
+void
+si_deregister_intr_callback(si_t *sih)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	sii->intrsoff_fn = NULL;
+}
+
+uint
+si_intflag(si_t *sih)
+{
+	si_info_t *sii = SI_INFO(sih);
+	if (CHIPTYPE(sih->socitype) == SOCI_SB) {
+		sbconfig_t *ccsbr = (sbconfig_t *)((uintptr)((ulong)
+			    (sii->common_info->coresba[SI_CC_IDX]) + SBCONFIGOFF));
+		return R_REG(sii->osh, &ccsbr->sbflagst);
+	} else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return R_REG(sii->osh, ((uint32 *)(uintptr)
+			    (sii->common_info->oob_router + OOB_STATUSA)));
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+uint
+si_flag(si_t *sih)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_flag(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_flag(sih);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+void
+si_setint(si_t *sih, int siflag)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		sb_setint(sih, siflag);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		ai_setint(sih, siflag);
+	else
+		ASSERT(0);
+}
+
+uint
+si_coreid(si_t *sih)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	return sii->common_info->coreid[sii->curidx];
+}
+
+uint
+si_coreidx(si_t *sih)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	return sii->curidx;
+}
+
+/* return the core-type instantiation # of the current core */
+uint
+si_coreunit(si_t *sih)
+{
+	si_info_t *sii;
+	uint idx;
+	uint coreid;
+	uint coreunit;
+	uint i;
+
+	sii = SI_INFO(sih);
+	coreunit = 0;
+
+	idx = sii->curidx;
+
+	ASSERT(GOODREGS(sii->curmap));
+	coreid = si_coreid(sih);
+
+	/* count the cores of our type */
+	for (i = 0; i < idx; i++)
+		if (sii->common_info->coreid[i] == coreid)
+			coreunit++;
+
+	return (coreunit);
+}
+
+uint
+si_corevendor(si_t *sih)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_corevendor(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_corevendor(sih);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+bool
+si_backplane64(si_t *sih)
+{
+	return ((sih->cccaps & CC_CAP_BKPLN64) != 0);
+}
+
+uint
+si_corerev(si_t *sih)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_corerev(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_corerev(sih);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+/* return index of coreid or BADIDX if not found */
+uint
+si_findcoreidx(si_t *sih, uint coreid, uint coreunit)
+{
+	si_info_t *sii;
+	uint found;
+	uint i;
+
+	sii = SI_INFO(sih);
+
+	found = 0;
+
+	for (i = 0; i < sii->numcores; i++)
+		if (sii->common_info->coreid[i] == coreid) {
+			if (found == coreunit)
+				return (i);
+			found++;
+		}
+
+	return (BADIDX);
+}
+
+/* return list of found cores */
+uint
+si_corelist(si_t *sih, uint coreid[])
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	bcopy((uchar*)sii->common_info->coreid, (uchar*)coreid, (sii->numcores * sizeof(uint)));
+	return (sii->numcores);
+}
+
+/* return current register mapping */
+void *
+si_coreregs(si_t *sih)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	ASSERT(GOODREGS(sii->curmap));
+
+	return (sii->curmap);
+}
+
+/*
+ * This function changes logical "focus" to the indicated core;
+ * must be called with interrupts off.
+ * Moreover, callers should keep interrupts off during switching out of and back to d11 core
+ */
+void *
+si_setcore(si_t *sih, uint coreid, uint coreunit)
+{
+	uint idx;
+
+	idx = si_findcoreidx(sih, coreid, coreunit);
+	if (!GOODIDX(idx))
+		return (NULL);
+
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_setcoreidx(sih, idx);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_setcoreidx(sih, idx);
+	else {
+		ASSERT(0);
+		return NULL;
+	}
+}
+
+void *
+si_setcoreidx(si_t *sih, uint coreidx)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_setcoreidx(sih, coreidx);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_setcoreidx(sih, coreidx);
+	else {
+		ASSERT(0);
+		return NULL;
+	}
+}
+
+/* Turn off interrupt as required by sb_setcore, before switch core */
+void *si_switch_core(si_t *sih, uint coreid, uint *origidx, uint *intr_val)
+{
+	void *cc;
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	INTR_OFF(sii, *intr_val);
+	*origidx = sii->curidx;
+	cc = si_setcore(sih, coreid, 0);
+	ASSERT(cc != NULL);
+
+	return cc;
+}
+
+/* restore coreidx and restore interrupt */
+void si_restore_core(si_t *sih, uint coreid, uint intr_val)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	si_setcoreidx(sih, coreid);
+	INTR_RESTORE(sii, intr_val);
+}
+
+int
+si_numaddrspaces(si_t *sih)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_numaddrspaces(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_numaddrspaces(sih);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+uint32
+si_addrspace(si_t *sih, uint asidx)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_addrspace(sih, asidx);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_addrspace(sih, asidx);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+uint32
+si_addrspacesize(si_t *sih, uint asidx)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_addrspacesize(sih, asidx);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_addrspacesize(sih, asidx);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+uint32
+si_core_cflags(si_t *sih, uint32 mask, uint32 val)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_core_cflags(sih, mask, val);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_core_cflags(sih, mask, val);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+void
+si_core_cflags_wo(si_t *sih, uint32 mask, uint32 val)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		sb_core_cflags_wo(sih, mask, val);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		ai_core_cflags_wo(sih, mask, val);
+	else
+		ASSERT(0);
+}
+
+uint32
+si_core_sflags(si_t *sih, uint32 mask, uint32 val)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_core_sflags(sih, mask, val);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_core_sflags(sih, mask, val);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+bool
+si_iscoreup(si_t *sih)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_iscoreup(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_iscoreup(sih);
+	else {
+		ASSERT(0);
+		return FALSE;
+	}
+}
+
+void
+si_write_wrapperreg(si_t *sih, uint32 offset, uint32 val)
+{
+	/* only for 4319, no requirement for SOCI_SB */
+	if (CHIPTYPE(sih->socitype) == SOCI_AI) {
+		ai_write_wrap_reg(sih, offset, val);
+	}
+	else
+		return;
+
+	return;
+}
+
+uint
+si_corereg(si_t *sih, uint coreidx, uint regoff, uint mask, uint val)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_corereg(sih, coreidx, regoff, mask, val);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_corereg(sih, coreidx, regoff, mask, val);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+void
+si_core_disable(si_t *sih, uint32 bits)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		sb_core_disable(sih, bits);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		ai_core_disable(sih, bits);
+}
+
+void
+si_core_reset(si_t *sih, uint32 bits, uint32 resetbits)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		sb_core_reset(sih, bits, resetbits);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		ai_core_reset(sih, bits, resetbits);
+}
+
+void
+si_core_tofixup(si_t *sih)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		sb_core_tofixup(sih);
+}
+
+/* Run bist on current core. Caller needs to take care of core-specific bist hazards */
+int
+si_corebist(si_t *sih)
+{
+	uint32 cflags;
+	int result = 0;
+
+	/* Read core control flags */
+	cflags = si_core_cflags(sih, 0, 0);
+
+	/* Set bist & fgc */
+	si_core_cflags(sih, 0, (SICF_BIST_EN | SICF_FGC));
+
+	/* Wait for bist done */
+	SPINWAIT(((si_core_sflags(sih, 0, 0) & SISF_BIST_DONE) == 0), 100000);
+
+	if (si_core_sflags(sih, 0, 0) & SISF_BIST_ERROR)
+		result = BCME_ERROR;
+
+	/* Reset core control flags */
+	si_core_cflags(sih, 0xffff, cflags);
+
+	return result;
+}
+
+static uint32
+factor6(uint32 x)
+{
+	switch (x) {
+	case CC_F6_2:	return 2;
+	case CC_F6_3:	return 3;
+	case CC_F6_4:	return 4;
+	case CC_F6_5:	return 5;
+	case CC_F6_6:	return 6;
+	case CC_F6_7:	return 7;
+	default:	return 0;
+	}
+}
+
+/* calculate the speed the SI would run at given a set of clockcontrol values */
+uint32
+si_clock_rate(uint32 pll_type, uint32 n, uint32 m)
+{
+	uint32 n1, n2, clock, m1, m2, m3, mc;
+
+	n1 = n & CN_N1_MASK;
+	n2 = (n & CN_N2_MASK) >> CN_N2_SHIFT;
+
+	if (pll_type == PLL_TYPE6) {
+		if (m & CC_T6_MMASK)
+			return CC_T6_M1;
+		else
+			return CC_T6_M0;
+	} else if ((pll_type == PLL_TYPE1) ||
+	           (pll_type == PLL_TYPE3) ||
+	           (pll_type == PLL_TYPE4) ||
+	           (pll_type == PLL_TYPE7)) {
+		n1 = factor6(n1);
+		n2 += CC_F5_BIAS;
+	} else if (pll_type == PLL_TYPE2) {
+		n1 += CC_T2_BIAS;
+		n2 += CC_T2_BIAS;
+		ASSERT((n1 >= 2) && (n1 <= 7));
+		ASSERT((n2 >= 5) && (n2 <= 23));
+	} else if (pll_type == PLL_TYPE5) {
+		return (100000000);
+	} else
+		ASSERT(0);
+	/* PLL types 3 and 7 use BASE2 (25Mhz) */
+	if ((pll_type == PLL_TYPE3) ||
+	    (pll_type == PLL_TYPE7)) {
+		clock = CC_CLOCK_BASE2 * n1 * n2;
+	} else
+		clock = CC_CLOCK_BASE1 * n1 * n2;
+
+	if (clock == 0)
+		return 0;
+
+	m1 = m & CC_M1_MASK;
+	m2 = (m & CC_M2_MASK) >> CC_M2_SHIFT;
+	m3 = (m & CC_M3_MASK) >> CC_M3_SHIFT;
+	mc = (m & CC_MC_MASK) >> CC_MC_SHIFT;
+
+	if ((pll_type == PLL_TYPE1) ||
+	    (pll_type == PLL_TYPE3) ||
+	    (pll_type == PLL_TYPE4) ||
+	    (pll_type == PLL_TYPE7)) {
+		m1 = factor6(m1);
+		if ((pll_type == PLL_TYPE1) || (pll_type == PLL_TYPE3))
+			m2 += CC_F5_BIAS;
+		else
+			m2 = factor6(m2);
+		m3 = factor6(m3);
+
+		switch (mc) {
+		case CC_MC_BYPASS:	return (clock);
+		case CC_MC_M1:		return (clock / m1);
+		case CC_MC_M1M2:	return (clock / (m1 * m2));
+		case CC_MC_M1M2M3:	return (clock / (m1 * m2 * m3));
+		case CC_MC_M1M3:	return (clock / (m1 * m3));
+		default:		return (0);
+		}
+	} else {
+		ASSERT(pll_type == PLL_TYPE2);
+
+		m1 += CC_T2_BIAS;
+		m2 += CC_T2M2_BIAS;
+		m3 += CC_T2_BIAS;
+		ASSERT((m1 >= 2) && (m1 <= 7));
+		ASSERT((m2 >= 3) && (m2 <= 10));
+		ASSERT((m3 >= 2) && (m3 <= 7));
+
+		if ((mc & CC_T2MC_M1BYP) == 0)
+			clock /= m1;
+		if ((mc & CC_T2MC_M2BYP) == 0)
+			clock /= m2;
+		if ((mc & CC_T2MC_M3BYP) == 0)
+			clock /= m3;
+
+		return (clock);
+	}
+}
+
+
+/* set chip watchdog reset timer to fire in 'ticks' */
+void
+si_watchdog(si_t *sih, uint ticks)
+{
+	if (PMUCTL_ENAB(sih)) {
+
+		if ((sih->chip == BCM4319_CHIP_ID) && (sih->chiprev == 0) && (ticks != 0)) {
+			si_corereg(sih, SI_CC_IDX, OFFSETOF(chipcregs_t, clk_ctl_st), ~0, 0x2);
+			si_setcore(sih, USB20D_CORE_ID, 0);
+			si_core_disable(sih, 1);
+			si_setcore(sih, CC_CORE_ID, 0);
+		}
+
+		if (ticks == 1)
+			ticks = 2;
+		si_corereg(sih, SI_CC_IDX, OFFSETOF(chipcregs_t, pmuwatchdog), ~0, ticks);
+	} else {
+		/* instant NMI */
+		si_corereg(sih, SI_CC_IDX, OFFSETOF(chipcregs_t, watchdog), ~0, ticks);
+	}
+}
+
+#if !defined(BCMBUSTYPE) || (BCMBUSTYPE == SI_BUS)
+/* trigger watchdog reset after ms milliseconds */
+void
+si_watchdog_ms(si_t *sih, uint32 ms)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	si_watchdog(sih, wd_msticks * ms);
+}
+#endif
+
+
+
+/* initialize the sdio core */
+void
+si_sdio_init(si_t *sih)
+{
+	si_info_t *sii = SI_INFO(sih);
+
+	if (((sih->buscoretype == PCMCIA_CORE_ID) && (sih->buscorerev >= 8)) ||
+	    (sih->buscoretype == SDIOD_CORE_ID)) {
+		uint idx;
+		sdpcmd_regs_t *sdpregs;
+
+		/* get the current core index */
+		idx = sii->curidx;
+		ASSERT(idx == si_findcoreidx(sih, D11_CORE_ID, 0));
+
+		/* switch to sdio core */
+		if (!(sdpregs = (sdpcmd_regs_t *)si_setcore(sih, PCMCIA_CORE_ID, 0)))
+			sdpregs = (sdpcmd_regs_t *)si_setcore(sih, SDIOD_CORE_ID, 0);
+		ASSERT(sdpregs);
+
+		SI_MSG(("si_sdio_init: For PCMCIA/SDIO Corerev %d, enable ints from core %d "
+		        "through SD core %d (%p)\n",
+		        sih->buscorerev, idx, sii->curidx, sdpregs));
+
+		/* enable backplane error and core interrupts */
+		W_REG(sii->osh, &sdpregs->hostintmask, I_SBINT);
+		W_REG(sii->osh, &sdpregs->sbintmask, (I_SB_SERR | I_SB_RESPERR | (1 << idx)));
+
+		/* switch back to previous core */
+		si_setcoreidx(sih, idx);
+	}
+
+	/* enable interrupts */
+	bcmsdh_intr_enable(sii->sdh);
+
+}
+
+
+/* change logical "focus" to the gpio core for optimized access */
+void *
+si_gpiosetcore(si_t *sih)
+{
+	return (si_setcoreidx(sih, SI_CC_IDX));
+}
+
+/* mask&set gpiocontrol bits */
+uint32
+si_gpiocontrol(si_t *sih, uint32 mask, uint32 val, uint8 priority)
+{
+	uint regoff;
+
+	regoff = 0;
+
+	/* gpios could be shared on router platforms
+	 * ignore reservation if it's high priority (e.g., test apps)
+	 */
+	if ((priority != GPIO_HI_PRIORITY) &&
+	    (BUSTYPE(sih->bustype) == SI_BUS) && (val || mask)) {
+		mask = priority ? (si_gpioreservation & mask) :
+			((si_gpioreservation | mask) & ~(si_gpioreservation));
+		val &= mask;
+	}
+
+	regoff = OFFSETOF(chipcregs_t, gpiocontrol);
+	return (si_corereg(sih, SI_CC_IDX, regoff, mask, val));
+}
+
+/* mask&set gpio output enable bits */
+uint32
+si_gpioouten(si_t *sih, uint32 mask, uint32 val, uint8 priority)
+{
+	uint regoff;
+
+	regoff = 0;
+
+	/* gpios could be shared on router platforms
+	 * ignore reservation if it's high priority (e.g., test apps)
+	 */
+	if ((priority != GPIO_HI_PRIORITY) &&
+	    (BUSTYPE(sih->bustype) == SI_BUS) && (val || mask)) {
+		mask = priority ? (si_gpioreservation & mask) :
+			((si_gpioreservation | mask) & ~(si_gpioreservation));
+		val &= mask;
+	}
+
+	regoff = OFFSETOF(chipcregs_t, gpioouten);
+	return (si_corereg(sih, SI_CC_IDX, regoff, mask, val));
+}
+
+/* mask&set gpio output bits */
+uint32
+si_gpioout(si_t *sih, uint32 mask, uint32 val, uint8 priority)
+{
+	uint regoff;
+
+	regoff = 0;
+
+	/* gpios could be shared on router platforms
+	 * ignore reservation if it's high priority (e.g., test apps)
+	 */
+	if ((priority != GPIO_HI_PRIORITY) &&
+	    (BUSTYPE(sih->bustype) == SI_BUS) && (val || mask)) {
+		mask = priority ? (si_gpioreservation & mask) :
+			((si_gpioreservation | mask) & ~(si_gpioreservation));
+		val &= mask;
+	}
+
+	regoff = OFFSETOF(chipcregs_t, gpioout);
+	return (si_corereg(sih, SI_CC_IDX, regoff, mask, val));
+}
+
+/* reserve one gpio */
+uint32
+si_gpioreserve(si_t *sih, uint32 gpio_bitmask, uint8 priority)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	/* only cores on SI_BUS share GPIO's and only applcation users need to
+	 * reserve/release GPIO
+	 */
+	if ((BUSTYPE(sih->bustype) != SI_BUS) || (!priority)) {
+		ASSERT((BUSTYPE(sih->bustype) == SI_BUS) && (priority));
+		return -1;
+	}
+	/* make sure only one bit is set */
+	if ((!gpio_bitmask) || ((gpio_bitmask) & (gpio_bitmask - 1))) {
+		ASSERT((gpio_bitmask) && !((gpio_bitmask) & (gpio_bitmask - 1)));
+		return -1;
+	}
+
+	/* already reserved */
+	if (si_gpioreservation & gpio_bitmask)
+		return -1;
+	/* set reservation */
+	si_gpioreservation |= gpio_bitmask;
+
+	return si_gpioreservation;
+}
+
+/* release one gpio */
+/*
+ * releasing the gpio doesn't change the current value on the GPIO last write value
+ * persists till some one overwrites it
+ */
+
+uint32
+si_gpiorelease(si_t *sih, uint32 gpio_bitmask, uint8 priority)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	/* only cores on SI_BUS share GPIO's and only applcation users need to
+	 * reserve/release GPIO
+	 */
+	if ((BUSTYPE(sih->bustype) != SI_BUS) || (!priority)) {
+		ASSERT((BUSTYPE(sih->bustype) == SI_BUS) && (priority));
+		return -1;
+	}
+	/* make sure only one bit is set */
+	if ((!gpio_bitmask) || ((gpio_bitmask) & (gpio_bitmask - 1))) {
+		ASSERT((gpio_bitmask) && !((gpio_bitmask) & (gpio_bitmask - 1)));
+		return -1;
+	}
+
+	/* already released */
+	if (!(si_gpioreservation & gpio_bitmask))
+		return -1;
+
+	/* clear reservation */
+	si_gpioreservation &= ~gpio_bitmask;
+
+	return si_gpioreservation;
+}
+
+/* return the current gpioin register value */
+uint32
+si_gpioin(si_t *sih)
+{
+	si_info_t *sii;
+	uint regoff;
+
+	sii = SI_INFO(sih);
+	regoff = 0;
+
+	regoff = OFFSETOF(chipcregs_t, gpioin);
+	return (si_corereg(sih, SI_CC_IDX, regoff, 0, 0));
+}
+
+/* mask&set gpio interrupt polarity bits */
+uint32
+si_gpiointpolarity(si_t *sih, uint32 mask, uint32 val, uint8 priority)
+{
+	si_info_t *sii;
+	uint regoff;
+
+	sii = SI_INFO(sih);
+	regoff = 0;
+
+	/* gpios could be shared on router platforms */
+	if ((BUSTYPE(sih->bustype) == SI_BUS) && (val || mask)) {
+		mask = priority ? (si_gpioreservation & mask) :
+			((si_gpioreservation | mask) & ~(si_gpioreservation));
+		val &= mask;
+	}
+
+	regoff = OFFSETOF(chipcregs_t, gpiointpolarity);
+	return (si_corereg(sih, SI_CC_IDX, regoff, mask, val));
+}
+
+/* mask&set gpio interrupt mask bits */
+uint32
+si_gpiointmask(si_t *sih, uint32 mask, uint32 val, uint8 priority)
+{
+	si_info_t *sii;
+	uint regoff;
+
+	sii = SI_INFO(sih);
+	regoff = 0;
+
+	/* gpios could be shared on router platforms */
+	if ((BUSTYPE(sih->bustype) == SI_BUS) && (val || mask)) {
+		mask = priority ? (si_gpioreservation & mask) :
+			((si_gpioreservation | mask) & ~(si_gpioreservation));
+		val &= mask;
+	}
+
+	regoff = OFFSETOF(chipcregs_t, gpiointmask);
+	return (si_corereg(sih, SI_CC_IDX, regoff, mask, val));
+}
+
+/* assign the gpio to an led */
+uint32
+si_gpioled(si_t *sih, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	if (sih->ccrev < 16)
+		return -1;
+
+	/* gpio led powersave reg */
+	return (si_corereg(sih, SI_CC_IDX, OFFSETOF(chipcregs_t, gpiotimeroutmask), mask, val));
+}
+
+/* mask&set gpio timer val */
+uint32
+si_gpiotimerval(si_t *sih, uint32 mask, uint32 gpiotimerval)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	if (sih->ccrev < 16)
+		return -1;
+
+	return (si_corereg(sih, SI_CC_IDX,
+		OFFSETOF(chipcregs_t, gpiotimerval), mask, gpiotimerval));
+}
+
+uint32
+si_gpiopull(si_t *sih, bool updown, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	uint offs;
+
+	sii = SI_INFO(sih);
+	if (sih->ccrev < 20)
+		return -1;
+
+	offs = (updown ? OFFSETOF(chipcregs_t, gpiopulldown) : OFFSETOF(chipcregs_t, gpiopullup));
+	return (si_corereg(sih, SI_CC_IDX, offs, mask, val));
+}
+
+uint32
+si_gpioevent(si_t *sih, uint regtype, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	uint offs;
+
+	sii = SI_INFO(sih);
+	if (sih->ccrev < 11)
+		return -1;
+
+	if (regtype == GPIO_REGEVT)
+		offs = OFFSETOF(chipcregs_t, gpioevent);
+	else if (regtype == GPIO_REGEVT_INTMSK)
+		offs = OFFSETOF(chipcregs_t, gpioeventintmask);
+	else if (regtype == GPIO_REGEVT_INTPOL)
+		offs = OFFSETOF(chipcregs_t, gpioeventintpolarity);
+	else
+		return -1;
+
+	return (si_corereg(sih, SI_CC_IDX, offs, mask, val));
+}
+
+void *
+si_gpio_handler_register(si_t *sih, uint32 event,
+	bool level, gpio_handler_t cb, void *arg)
+{
+	si_info_t *sii;
+	gpioh_item_t *gi;
+
+	ASSERT(event);
+	ASSERT(cb != NULL);
+
+	sii = SI_INFO(sih);
+	if (sih->ccrev < 11)
+		return NULL;
+
+	if ((gi = MALLOC(sii->osh, sizeof(gpioh_item_t))) == NULL)
+		return NULL;
+
+	bzero(gi, sizeof(gpioh_item_t));
+	gi->event = event;
+	gi->handler = cb;
+	gi->arg = arg;
+	gi->level = level;
+
+	gi->next = sii->gpioh_head;
+	sii->gpioh_head = gi;
+
+	return (void *)(gi);
+}
+
+void
+si_gpio_handler_unregister(si_t *sih, void *gpioh)
+{
+	si_info_t *sii;
+	gpioh_item_t *p, *n;
+
+	sii = SI_INFO(sih);
+	if (sih->ccrev < 11)
+		return;
+
+	ASSERT(sii->gpioh_head != NULL);
+	if ((void*)sii->gpioh_head == gpioh) {
+		sii->gpioh_head = sii->gpioh_head->next;
+		MFREE(sii->osh, gpioh, sizeof(gpioh_item_t));
+		return;
+	} else {
+		p = sii->gpioh_head;
+		n = p->next;
+		while (n) {
+			if ((void*)n == gpioh) {
+				p->next = n->next;
+				MFREE(sii->osh, gpioh, sizeof(gpioh_item_t));
+				return;
+			}
+			p = n;
+			n = n->next;
+		}
+	}
+
+	ASSERT(0); /* Not found in list */
+}
+
+void
+si_gpio_handler_process(si_t *sih)
+{
+	si_info_t *sii;
+	gpioh_item_t *h;
+	uint32 status;
+	uint32 level = si_gpioin(sih);
+	uint32 edge = si_gpioevent(sih, GPIO_REGEVT, 0, 0);
+
+	sii = SI_INFO(sih);
+	for (h = sii->gpioh_head; h != NULL; h = h->next) {
+		if (h->handler) {
+			status = (h->level ? level : edge);
+
+			if (status & h->event)
+				h->handler(status, h->arg);
+		}
+	}
+
+	si_gpioevent(sih, GPIO_REGEVT, edge, edge); /* clear edge-trigger status */
+}
+
+uint32
+si_gpio_int_enable(si_t *sih, bool enable)
+{
+	si_info_t *sii;
+	uint offs;
+
+	sii = SI_INFO(sih);
+	if (sih->ccrev < 11)
+		return -1;
+
+	offs = OFFSETOF(chipcregs_t, intmask);
+	return (si_corereg(sih, SI_CC_IDX, offs, CI_GPIO, (enable ? CI_GPIO : 0)));
+}
+
+
+/* Return the RAM size of the SOCRAM core */
+uint32
+si_socram_size(si_t *sih)
+{
+	si_info_t *sii;
+	uint origidx;
+	uint intr_val = 0;
+
+	sbsocramregs_t *regs;
+	bool wasup;
+	uint corerev;
+	uint32 coreinfo;
+	uint memsize = 0;
+
+	sii = SI_INFO(sih);
+
+	/* Block ints and save current core */
+	INTR_OFF(sii, intr_val);
+	origidx = si_coreidx(sih);
+
+	/* Switch to SOCRAM core */
+	if (!(regs = si_setcore(sih, SOCRAM_CORE_ID, 0)))
+		goto done;
+
+	/* Get info for determining size */
+	if (!(wasup = si_iscoreup(sih)))
+		si_core_reset(sih, 0, 0);
+	corerev = si_corerev(sih);
+	coreinfo = R_REG(sii->osh, &regs->coreinfo);
+
+	/* Calculate size from coreinfo based on rev */
+	if (corerev == 0)
+		memsize = 1 << (16 + (coreinfo & SRCI_MS0_MASK));
+	else if (corerev < 3) {
+		memsize = 1 << (SR_BSZ_BASE + (coreinfo & SRCI_SRBSZ_MASK));
+		memsize *= (coreinfo & SRCI_SRNB_MASK) >> SRCI_SRNB_SHIFT;
+	} else {
+		uint nb = (coreinfo & SRCI_SRNB_MASK) >> SRCI_SRNB_SHIFT;
+		uint bsz = (coreinfo & SRCI_SRBSZ_MASK);
+		uint lss = (coreinfo & SRCI_LSS_MASK) >> SRCI_LSS_SHIFT;
+		if (lss != 0)
+			nb --;
+		memsize = nb * (1 << (bsz + SR_BSZ_BASE));
+		if (lss != 0)
+			memsize += (1 << ((lss - 1) + SR_BSZ_BASE));
+	}
+
+	/* Return to previous state and core */
+	if (!wasup)
+		si_core_disable(sih, 0);
+	si_setcoreidx(sih, origidx);
+
+done:
+	INTR_RESTORE(sii, intr_val);
+
+	return memsize;
+}
+
+
+void
+si_btcgpiowar(si_t *sih)
+{
+	si_info_t *sii;
+	uint origidx;
+	uint intr_val = 0;
+	chipcregs_t *cc;
+
+	sii = SI_INFO(sih);
+
+	/* Make sure that there is ChipCommon core present &&
+	 * UART_TX is strapped to 1
+	 */
+	if (!(sih->cccaps & CC_CAP_UARTGPIO))
+		return;
+
+	/* si_corereg cannot be used as we have to guarantee 8-bit read/writes */
+	INTR_OFF(sii, intr_val);
+
+	origidx = si_coreidx(sih);
+
+	cc = (chipcregs_t *)si_setcore(sih, CC_CORE_ID, 0);
+	ASSERT(cc != NULL);
+
+	W_REG(sii->osh, &cc->uart0mcr, R_REG(sii->osh, &cc->uart0mcr) | 0x04);
+
+	/* restore the original index */
+	si_setcoreidx(sih, origidx);
+
+	INTR_RESTORE(sii, intr_val);
+}
+
+/* check if the device is removed */
+bool
+si_deviceremoved(si_t *sih)
+{
+	uint32 w;
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	switch (BUSTYPE(sih->bustype)) {
+	case PCI_BUS:
+		ASSERT(sii->osh != NULL);
+		w = OSL_PCI_READ_CONFIG(sii->osh, PCI_CFG_VID, sizeof(uint32));
+		if ((w & 0xFFFF) != VENDOR_BROADCOM)
+			return TRUE;
+		else
+			return FALSE;
+	default:
+		return FALSE;
+	}
+	return FALSE;
+}
diff --git a/drivers/net/wireless/bcm4329/siutils_priv.h b/drivers/net/wireless/bcm4329/siutils_priv.h
new file mode 100644
index 000000000000..e8ad7e50958a
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/siutils_priv.h
@@ -0,0 +1,213 @@
+/*
+ * Include file private to the SOC Interconnect support files.
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: siutils_priv.h,v 1.3.10.5.4.2 2009/09/22 13:28:16 Exp $
+ */
+
+#ifndef	_siutils_priv_h_
+#define	_siutils_priv_h_
+
+/* debug/trace */
+#define	SI_ERROR(args)
+
+#define	SI_MSG(args)
+
+#define	IS_SIM(chippkg)	((chippkg == HDLSIM_PKG_ID) || (chippkg == HWSIM_PKG_ID))
+
+typedef uint32 (*si_intrsoff_t)(void *intr_arg);
+typedef void (*si_intrsrestore_t)(void *intr_arg, uint32 arg);
+typedef bool (*si_intrsenabled_t)(void *intr_arg);
+
+typedef struct gpioh_item {
+	void			*arg;
+	bool			level;
+	gpio_handler_t		handler;
+	uint32			event;
+	struct gpioh_item	*next;
+} gpioh_item_t;
+
+/* misc si info needed by some of the routines */
+typedef struct si_common_info {
+	void	*regs[SI_MAXCORES];	/* other regs va */
+	void	*regs2[SI_MAXCORES];	/* va of each core second register set (usbh20) */
+	uint	coreid[SI_MAXCORES];	/* id of each core */
+	uint32	cia[SI_MAXCORES];	/* erom cia entry for each core */
+	uint32	cib[SI_MAXCORES];	/* erom cia entry for each core */
+	uint32	coresba_size[SI_MAXCORES]; /* backplane address space size */
+	uint32	coresba2_size[SI_MAXCORES]; /* second address space size */
+	uint32	coresba[SI_MAXCORES];	/* backplane address of each core */
+	uint32	coresba2[SI_MAXCORES];	/* address of each core second register set (usbh20) */
+	void	*wrappers[SI_MAXCORES];	/* other cores wrapper va */
+	uint32	wrapba[SI_MAXCORES];	/* address of controlling wrapper */
+	uint32	oob_router;		/* oob router registers for axi */
+	uint8	attach_count;
+} si_common_info_t;
+
+typedef struct si_info {
+	struct si_pub pub;		/* back plane public state (must be first field) */
+
+	void	*osh;			/* osl os handle */
+	void	*sdh;			/* bcmsdh handle */
+	void *pch;			/* PCI/E core handle */
+	uint	dev_coreid;		/* the core provides driver functions */
+	void	*intr_arg;		/* interrupt callback function arg */
+	si_intrsoff_t intrsoff_fn;	/* turns chip interrupts off */
+	si_intrsrestore_t intrsrestore_fn; /* restore chip interrupts */
+	si_intrsenabled_t intrsenabled_fn; /* check if interrupts are enabled */
+
+
+	gpioh_item_t *gpioh_head; 	/* GPIO event handlers list */
+
+	bool	memseg;			/* flag to toggle MEM_SEG register */
+
+	char *vars;
+	uint varsz;
+
+	void	*curmap;		/* current regs va */
+
+	uint	curidx;			/* current core index */
+	uint	numcores;		/* # discovered cores */
+	void	*curwrap;		/* current wrapper va */
+	si_common_info_t	*common_info;	/* Common information for all the cores in a chip */
+} si_info_t;
+
+#define	SI_INFO(sih)	(si_info_t *)(uintptr)sih
+
+#define	GOODCOREADDR(x, b) (((x) >= (b)) && ((x) < ((b) + SI_MAXCORES * SI_CORE_SIZE)) && \
+		ISALIGNED((x), SI_CORE_SIZE))
+#define	GOODREGS(regs)	((regs) != NULL && ISALIGNED((uintptr)(regs), SI_CORE_SIZE))
+#define BADCOREADDR	0
+#define	GOODIDX(idx)	(((uint)idx) < SI_MAXCORES)
+#define	BADIDX		(SI_MAXCORES + 1)
+#define	NOREV		-1		/* Invalid rev */
+
+#define PCI(si)		((BUSTYPE((si)->pub.bustype) == PCI_BUS) &&	\
+			 ((si)->pub.buscoretype == PCI_CORE_ID))
+#define PCIE(si)	((BUSTYPE((si)->pub.bustype) == PCI_BUS) &&	\
+			 ((si)->pub.buscoretype == PCIE_CORE_ID))
+#define PCMCIA(si)	((BUSTYPE((si)->pub.bustype) == PCMCIA_BUS) && ((si)->memseg == TRUE))
+
+/* Newer chips can access PCI/PCIE and CC core without requiring to change
+ * PCI BAR0 WIN
+ */
+#define SI_FAST(si) (((si)->pub.buscoretype == PCIE_CORE_ID) ||	\
+		     (((si)->pub.buscoretype == PCI_CORE_ID) && (si)->pub.buscorerev >= 13))
+
+#define PCIEREGS(si) (((char *)((si)->curmap) + PCI_16KB0_PCIREGS_OFFSET))
+#define CCREGS_FAST(si) (((char *)((si)->curmap) + PCI_16KB0_CCREGS_OFFSET))
+
+/*
+ * Macros to disable/restore function core(D11, ENET, ILINE20, etc) interrupts before/
+ * after core switching to avoid invalid register accesss inside ISR.
+ */
+#define INTR_OFF(si, intr_val) \
+	if ((si)->intrsoff_fn && (si)->common_info->coreid[(si)->curidx] == (si)->dev_coreid) {	\
+		intr_val = (*(si)->intrsoff_fn)((si)->intr_arg); }
+#define INTR_RESTORE(si, intr_val) \
+	if ((si)->intrsrestore_fn && (si)->common_info->coreid[(si)->curidx] == (si)->dev_coreid) {\
+		(*(si)->intrsrestore_fn)((si)->intr_arg, intr_val); }
+
+/* dynamic clock control defines */
+#define	LPOMINFREQ		25000		/* low power oscillator min */
+#define	LPOMAXFREQ		43000		/* low power oscillator max */
+#define	XTALMINFREQ		19800000	/* 20 MHz - 1% */
+#define	XTALMAXFREQ		20200000	/* 20 MHz + 1% */
+#define	PCIMINFREQ		25000000	/* 25 MHz */
+#define	PCIMAXFREQ		34000000	/* 33 MHz + fudge */
+
+#define	ILP_DIV_5MHZ		0		/* ILP = 5 MHz */
+#define	ILP_DIV_1MHZ		4		/* ILP = 1 MHz */
+
+#define PCI_FORCEHT(si)	\
+	(((PCIE(si)) && (si->pub.chip == BCM4311_CHIP_ID) && ((si->pub.chiprev <= 1))) || \
+	((PCI(si) || PCIE(si)) && (si->pub.chip == BCM4321_CHIP_ID)))
+
+/* GPIO Based LED powersave defines */
+#define DEFAULT_GPIO_ONTIME	10		/* Default: 10% on */
+#define DEFAULT_GPIO_OFFTIME	90		/* Default: 10% on */
+
+#ifndef DEFAULT_GPIOTIMERVAL
+#define DEFAULT_GPIOTIMERVAL  ((DEFAULT_GPIO_ONTIME << GPIO_ONTIME_SHIFT) | DEFAULT_GPIO_OFFTIME)
+#endif
+
+/* Silicon Backplane externs */
+extern void sb_scan(si_t *sih, void *regs, uint devid);
+extern uint sb_coreid(si_t *sih);
+extern uint sb_flag(si_t *sih);
+extern void sb_setint(si_t *sih, int siflag);
+extern uint sb_corevendor(si_t *sih);
+extern uint sb_corerev(si_t *sih);
+extern uint sb_corereg(si_t *sih, uint coreidx, uint regoff, uint mask, uint val);
+extern bool sb_iscoreup(si_t *sih);
+extern void *sb_setcoreidx(si_t *sih, uint coreidx);
+extern uint32 sb_core_cflags(si_t *sih, uint32 mask, uint32 val);
+extern void sb_core_cflags_wo(si_t *sih, uint32 mask, uint32 val);
+extern uint32 sb_core_sflags(si_t *sih, uint32 mask, uint32 val);
+extern void sb_commit(si_t *sih);
+extern uint32 sb_base(uint32 admatch);
+extern uint32 sb_size(uint32 admatch);
+extern void sb_core_reset(si_t *sih, uint32 bits, uint32 resetbits);
+extern void sb_core_tofixup(si_t *sih);
+extern void sb_core_disable(si_t *sih, uint32 bits);
+extern uint32 sb_addrspace(si_t *sih, uint asidx);
+extern uint32 sb_addrspacesize(si_t *sih, uint asidx);
+extern int sb_numaddrspaces(si_t *sih);
+
+extern uint32 sb_set_initiator_to(si_t *sih, uint32 to, uint idx);
+
+
+
+/* Wake-on-wireless-LAN (WOWL) */
+extern bool sb_pci_pmecap(si_t *sih);
+struct osl_info;
+extern bool sb_pci_fastpmecap(struct osl_info *osh);
+extern bool sb_pci_pmeclr(si_t *sih);
+extern void sb_pci_pmeen(si_t *sih);
+extern uint sb_pcie_readreg(void *sih, uint addrtype, uint offset);
+
+/* AMBA Interconnect exported externs */
+extern si_t *ai_attach(uint pcidev, osl_t *osh, void *regs, uint bustype,
+                       void *sdh, char **vars, uint *varsz);
+extern si_t *ai_kattach(osl_t *osh);
+extern void ai_scan(si_t *sih, void *regs, uint devid);
+
+extern uint ai_flag(si_t *sih);
+extern void ai_setint(si_t *sih, int siflag);
+extern uint ai_coreidx(si_t *sih);
+extern uint ai_corevendor(si_t *sih);
+extern uint ai_corerev(si_t *sih);
+extern bool ai_iscoreup(si_t *sih);
+extern void *ai_setcoreidx(si_t *sih, uint coreidx);
+extern uint32 ai_core_cflags(si_t *sih, uint32 mask, uint32 val);
+extern void ai_core_cflags_wo(si_t *sih, uint32 mask, uint32 val);
+extern uint32 ai_core_sflags(si_t *sih, uint32 mask, uint32 val);
+extern uint ai_corereg(si_t *sih, uint coreidx, uint regoff, uint mask, uint val);
+extern void ai_core_reset(si_t *sih, uint32 bits, uint32 resetbits);
+extern void ai_core_disable(si_t *sih, uint32 bits);
+extern int ai_numaddrspaces(si_t *sih);
+extern uint32 ai_addrspace(si_t *sih, uint asidx);
+extern uint32 ai_addrspacesize(si_t *sih, uint asidx);
+extern void ai_write_wrap_reg(si_t *sih, uint32 offset, uint32 val);
+
+
+#endif	/* _siutils_priv_h_ */
diff --git a/drivers/net/wireless/bcm4329/wl_iw.c b/drivers/net/wireless/bcm4329/wl_iw.c
new file mode 100644
index 000000000000..8cb0f3af84ee
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/wl_iw.c
@@ -0,0 +1,8457 @@
+/*
+ * Linux Wireless Extensions support
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wl_iw.c,v 1.51.4.9.2.6.4.142.4.78 2011/02/11 21:27:52 Exp $
+ */
+
+
+#include <typedefs.h>
+#include <linuxver.h>
+#include <osl.h>
+
+#include <bcmutils.h>
+#include <bcmendian.h>
+#include <proto/ethernet.h>
+
+#include <linux/if_arp.h>
+#include <asm/uaccess.h>
+
+#include <dngl_stats.h>
+#include <dhd.h>
+#include <dhdioctl.h>
+
+typedef void wlc_info_t;
+typedef void wl_info_t;
+typedef const struct si_pub  si_t;
+#include <wlioctl.h>
+
+#include <proto/ethernet.h>
+#include <dngl_stats.h>
+#include <dhd.h>
+#define WL_ERROR(x) printf x
+#define WL_TRACE(x)
+#define WL_ASSOC(x)
+#define WL_INFORM(x)
+#define WL_WSEC(x)
+#define WL_SCAN(x)
+#define WL_PNO(x)
+#define WL_TRACE_COEX(x)
+
+#include <wl_iw.h>
+
+
+
+#ifndef IW_ENCODE_ALG_SM4
+#define IW_ENCODE_ALG_SM4 0x20
+#endif
+
+#ifndef IW_AUTH_WAPI_ENABLED
+#define IW_AUTH_WAPI_ENABLED 0x20
+#endif
+
+#ifndef IW_AUTH_WAPI_VERSION_1
+#define IW_AUTH_WAPI_VERSION_1	0x00000008
+#endif
+
+#ifndef IW_AUTH_CIPHER_SMS4
+#define IW_AUTH_CIPHER_SMS4	0x00000020
+#endif
+
+#ifndef IW_AUTH_KEY_MGMT_WAPI_PSK
+#define IW_AUTH_KEY_MGMT_WAPI_PSK 4
+#endif
+
+#ifndef IW_AUTH_KEY_MGMT_WAPI_CERT
+#define IW_AUTH_KEY_MGMT_WAPI_CERT 8
+#endif
+
+
+#define IW_WSEC_ENABLED(wsec)	((wsec) & (WEP_ENABLED | TKIP_ENABLED | AES_ENABLED | SMS4_ENABLED))
+
+#include <linux/rtnetlink.h>
+#include <linux/mutex.h>
+
+#define WL_IW_USE_ISCAN  1
+#define ENABLE_ACTIVE_PASSIVE_SCAN_SUPPRESS  1
+
+#if defined(SOFTAP)
+#define WL_SOFTAP(x) printk x
+static struct net_device *priv_dev;
+static bool ap_cfg_running = FALSE;
+bool ap_fw_loaded = FALSE;
+static long ap_cfg_pid = -1;
+struct net_device *ap_net_dev = NULL;
+struct semaphore  ap_eth_sema;
+static struct completion ap_cfg_exited;
+static int wl_iw_set_ap_security(struct net_device *dev, struct ap_profile *ap);
+static int wl_iw_softap_deassoc_stations(struct net_device *dev, u8 *mac);
+#endif
+
+#define WL_IW_IOCTL_CALL(func_call) \
+	do {				\
+		func_call;		\
+	} while (0)
+
+static int		g_onoff = G_WLAN_SET_ON;
+wl_iw_extra_params_t	g_wl_iw_params;
+static struct mutex	wl_cache_lock;
+
+extern bool wl_iw_conn_status_str(uint32 event_type, uint32 status,
+	uint32 reason, char* stringBuf, uint buflen);
+#include <bcmsdbus.h>
+extern void dhd_customer_gpio_wlan_ctrl(int onoff);
+extern uint dhd_dev_reset(struct net_device *dev, uint8 flag);
+extern void dhd_dev_init_ioctl(struct net_device *dev);
+int dev_iw_write_cfg1_bss_var(struct net_device *dev, int val);
+
+uint wl_msg_level = WL_ERROR_VAL;
+
+#define MAX_WLIW_IOCTL_LEN 1024
+
+
+#if defined(IL_BIGENDIAN)
+#include <bcmendian.h>
+#define htod32(i) (bcmswap32(i))
+#define htod16(i) (bcmswap16(i))
+#define dtoh32(i) (bcmswap32(i))
+#define dtoh16(i) (bcmswap16(i))
+#define htodchanspec(i) htod16(i)
+#define dtohchanspec(i) dtoh16(i)
+#else
+#define htod32(i) i
+#define htod16(i) i
+#define dtoh32(i) i
+#define dtoh16(i) i
+#define htodchanspec(i) i
+#define dtohchanspec(i) i
+#endif
+
+#ifdef CONFIG_WIRELESS_EXT
+
+extern struct iw_statistics *dhd_get_wireless_stats(struct net_device *dev);
+extern int dhd_wait_pend8021x(struct net_device *dev);
+#endif 
+
+#if WIRELESS_EXT < 19
+#define IW_IOCTL_IDX(cmd)	((cmd) - SIOCIWFIRST)
+#define IW_EVENT_IDX(cmd)	((cmd) - IWEVFIRST)
+#endif 
+
+static void *g_scan = NULL;
+static volatile uint g_scan_specified_ssid;
+static wlc_ssid_t g_specific_ssid;
+
+static wlc_ssid_t g_ssid;
+
+bool btcoex_is_sco_active(struct net_device *dev);
+static wl_iw_ss_cache_ctrl_t g_ss_cache_ctrl;
+#if defined(CONFIG_FIRST_SCAN)
+static volatile uint g_first_broadcast_scan;
+static volatile uint g_first_counter_scans;
+#define MAX_ALLOWED_BLOCK_SCAN_FROM_FIRST_SCAN 3
+#endif
+
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+#define DAEMONIZE(a) daemonize(a); \
+	allow_signal(SIGKILL); \
+	allow_signal(SIGTERM);
+#else 
+#define RAISE_RX_SOFTIRQ() \
+	cpu_raise_softirq(smp_processor_id(), NET_RX_SOFTIRQ)
+#define DAEMONIZE(a) daemonize(); \
+	do { if (a) \
+		strncpy(current->comm, a, MIN(sizeof(current->comm), (strlen(a) + 1))); \
+	} while (0);
+#endif 
+
+#if defined(WL_IW_USE_ISCAN)
+#if !defined(CSCAN)
+static void wl_iw_free_ss_cache(void);
+static int   wl_iw_run_ss_cache_timer(int kick_off);
+#endif
+#if defined(CONFIG_FIRST_SCAN)
+int  wl_iw_iscan_set_scan_broadcast_prep(struct net_device *dev, uint flag);
+#endif
+static int dev_wlc_bufvar_set(struct net_device *dev, char *name, char *buf, int len);
+#define ISCAN_STATE_IDLE   0
+#define ISCAN_STATE_SCANING 1
+
+#define WLC_IW_ISCAN_MAXLEN   2048
+typedef struct iscan_buf {
+	struct iscan_buf * next;
+	char   iscan_buf[WLC_IW_ISCAN_MAXLEN];
+} iscan_buf_t;
+
+typedef struct iscan_info {
+	struct net_device *dev;
+	struct timer_list timer;
+	uint32 timer_ms;
+	uint32 timer_on;
+	int    iscan_state;
+	iscan_buf_t * list_hdr;
+	iscan_buf_t * list_cur;
+
+	
+	long sysioc_pid;
+	struct semaphore sysioc_sem;
+	struct completion sysioc_exited;
+
+	uint32 scan_flag;	
+#if defined CSCAN
+	char ioctlbuf[WLC_IOCTL_MEDLEN];
+#else
+	char ioctlbuf[WLC_IOCTL_SMLEN];
+#endif
+	wl_iscan_params_t *iscan_ex_params_p;
+	int iscan_ex_param_size;
+} iscan_info_t;
+#define COEX_DHCP 1
+
+#define BT_DHCP_eSCO_FIX
+#define BT_DHCP_USE_FLAGS
+#define BT_DHCP_OPPORTUNITY_WINDOW_TIME	 2500
+#define BT_DHCP_FLAG_FORCE_TIME 5500
+static void wl_iw_bt_flag_set(struct net_device *dev, bool set);
+static void wl_iw_bt_release(void);
+
+typedef enum bt_coex_status {
+	BT_DHCP_IDLE = 0,
+	BT_DHCP_START,
+	BT_DHCP_OPPORTUNITY_WINDOW,
+	BT_DHCP_FLAG_FORCE_TIMEOUT
+} coex_status_t;
+
+typedef struct bt_info {
+	struct net_device *dev;
+	struct timer_list timer;
+	uint32 timer_ms;
+	uint32 timer_on;
+	bool   dhcp_done;
+	int    bt_state;
+
+	long   bt_pid;
+	struct semaphore bt_sem;
+	struct completion bt_exited;
+} bt_info_t;
+
+bt_info_t *g_bt = NULL;
+static void wl_iw_bt_timerfunc(ulong data);
+iscan_info_t *g_iscan = NULL;
+static void wl_iw_timerfunc(ulong data);
+static void wl_iw_set_event_mask(struct net_device *dev);
+static int
+wl_iw_iscan(iscan_info_t *iscan, wlc_ssid_t *ssid, uint16 action);
+#endif 
+static int
+wl_iw_set_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+);
+
+#ifndef CSCAN
+static int
+wl_iw_get_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+);
+
+static uint
+wl_iw_get_scan_prep(
+	wl_scan_results_t *list,
+	struct iw_request_info *info,
+	char *extra,
+	short max_size
+);
+#endif
+
+static void swap_key_from_BE(
+	        wl_wsec_key_t *key
+)
+{
+	key->index = htod32(key->index);
+	key->len = htod32(key->len);
+	key->algo = htod32(key->algo);
+	key->flags = htod32(key->flags);
+	key->rxiv.hi = htod32(key->rxiv.hi);
+	key->rxiv.lo = htod16(key->rxiv.lo);
+	key->iv_initialized = htod32(key->iv_initialized);
+}
+
+static void swap_key_to_BE(
+	        wl_wsec_key_t *key
+)
+{
+	key->index = dtoh32(key->index);
+	key->len = dtoh32(key->len);
+	key->algo = dtoh32(key->algo);
+	key->flags = dtoh32(key->flags);
+	key->rxiv.hi = dtoh32(key->rxiv.hi);
+	key->rxiv.lo = dtoh16(key->rxiv.lo);
+	key->iv_initialized = dtoh32(key->iv_initialized);
+}
+
+static int
+dev_wlc_ioctl(
+	struct net_device *dev,
+	int cmd,
+	void *arg,
+	int len
+)
+{
+	struct ifreq ifr;
+	wl_ioctl_t ioc;
+	mm_segment_t fs;
+	int ret = -EINVAL;
+
+	if (!dev) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return ret;
+	}
+
+	net_os_wake_lock(dev);
+
+	WL_INFORM(("\n%s, PID:%x: send Local IOCTL -> dhd: cmd:0x%x, buf:%p, len:%d ,\n",
+		__FUNCTION__, current->pid, cmd, arg, len));
+
+	if (g_onoff == G_WLAN_SET_ON) {
+		memset(&ioc, 0, sizeof(ioc));
+		ioc.cmd = cmd;
+		ioc.buf = arg;
+		ioc.len = len;
+
+		strcpy(ifr.ifr_name, dev->name);
+		ifr.ifr_data = (caddr_t) &ioc;
+
+		ret = dev_open(dev);
+		if (ret) {
+			WL_ERROR(("%s: Error dev_open: %d\n", __func__, ret));
+			net_os_wake_unlock(dev);
+			return ret;
+		}
+
+		fs = get_fs();
+		set_fs(get_ds());
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 31))
+		ret = dev->do_ioctl(dev, &ifr, SIOCDEVPRIVATE);
+#else
+		ret = dev->netdev_ops->ndo_do_ioctl(dev, &ifr, SIOCDEVPRIVATE);
+#endif
+		set_fs(fs);
+	}
+	else {
+		WL_TRACE(("%s: call after driver stop : ignored\n", __FUNCTION__));
+	}
+
+	net_os_wake_unlock(dev);
+
+	return ret;
+}
+
+
+static int
+dev_wlc_intvar_get_reg(
+	struct net_device *dev,
+	char *name,
+	uint  reg,
+	int *retval)
+{
+	union {
+		char buf[WLC_IOCTL_SMLEN];
+		int val;
+	} var;
+	int error;
+
+	uint len;
+	len = bcm_mkiovar(name, (char *)(&reg), sizeof(reg), (char *)(&var), sizeof(var.buf));
+	ASSERT(len);
+	error = dev_wlc_ioctl(dev, WLC_GET_VAR, (void *)&var, len);
+
+	*retval = dtoh32(var.val);
+	return (error);
+}
+
+
+static int
+dev_wlc_intvar_set_reg(
+	struct net_device *dev,
+	char *name,
+	char *addr,
+	char * val)
+{
+	char reg_addr[8];
+
+	memset(reg_addr, 0, sizeof(reg_addr));
+	memcpy((char *)&reg_addr[0], (char *)addr, 4);
+	memcpy((char *)&reg_addr[4], (char *)val, 4);
+
+	return (dev_wlc_bufvar_set(dev, name,  (char *)&reg_addr[0], sizeof(reg_addr)));
+}
+
+
+static int
+dev_wlc_intvar_set(
+	struct net_device *dev,
+	char *name,
+	int val)
+{
+	char buf[WLC_IOCTL_SMLEN];
+	uint len;
+
+	val = htod32(val);
+	len = bcm_mkiovar(name, (char *)(&val), sizeof(val), buf, sizeof(buf));
+	ASSERT(len);
+
+	return (dev_wlc_ioctl(dev, WLC_SET_VAR, buf, len));
+}
+
+#if defined(WL_IW_USE_ISCAN)
+static int
+dev_iw_iovar_setbuf(
+	struct net_device *dev,
+	char *iovar,
+	void *param,
+	int paramlen,
+	void *bufptr,
+	int buflen)
+{
+	int iolen;
+
+	iolen = bcm_mkiovar(iovar, param, paramlen, bufptr, buflen);
+	ASSERT(iolen);
+
+	if (iolen == 0)
+		return 0;
+
+	return (dev_wlc_ioctl(dev, WLC_SET_VAR, bufptr, iolen));
+}
+
+static int
+dev_iw_iovar_getbuf(
+	struct net_device *dev,
+	char *iovar,
+	void *param,
+	int paramlen,
+	void *bufptr,
+	int buflen)
+{
+	int iolen;
+
+	iolen = bcm_mkiovar(iovar, param, paramlen, bufptr, buflen);
+	ASSERT(iolen);
+
+	return (dev_wlc_ioctl(dev, WLC_GET_VAR, bufptr, buflen));
+}
+#endif 
+
+
+#if WIRELESS_EXT > 17
+static int
+dev_wlc_bufvar_set(
+	struct net_device *dev,
+	char *name,
+	char *buf, int len)
+{
+	static char ioctlbuf[MAX_WLIW_IOCTL_LEN];
+	uint buflen;
+
+	buflen = bcm_mkiovar(name, buf, len, ioctlbuf, sizeof(ioctlbuf));
+	ASSERT(buflen);
+
+	return (dev_wlc_ioctl(dev, WLC_SET_VAR, ioctlbuf, buflen));
+}
+#endif
+
+
+static int
+dev_wlc_bufvar_get(
+	struct net_device *dev,
+	char *name,
+	char *buf, int buflen)
+{
+	static char ioctlbuf[MAX_WLIW_IOCTL_LEN];
+	int error;
+	uint len;
+
+	len = bcm_mkiovar(name, NULL, 0, ioctlbuf, sizeof(ioctlbuf));
+	ASSERT(len);
+	error = dev_wlc_ioctl(dev, WLC_GET_VAR, (void *)ioctlbuf, MAX_WLIW_IOCTL_LEN);
+	if (!error)
+		bcopy(ioctlbuf, buf, buflen);
+
+	return (error);
+}
+
+
+
+static int
+dev_wlc_intvar_get(
+	struct net_device *dev,
+	char *name,
+	int *retval)
+{
+	union {
+		char buf[WLC_IOCTL_SMLEN];
+		int val;
+	} var;
+	int error;
+
+	uint len;
+	uint data_null;
+
+	len = bcm_mkiovar(name, (char *)(&data_null), 0, (char *)(&var), sizeof(var.buf));
+	ASSERT(len);
+	error = dev_wlc_ioctl(dev, WLC_GET_VAR, (void *)&var, len);
+
+	*retval = dtoh32(var.val);
+
+	return (error);
+}
+
+
+#if WIRELESS_EXT > 12
+static int
+wl_iw_set_active_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int as = 0;
+	int error = 0;
+	char *p = extra;
+
+#if defined(WL_IW_USE_ISCAN)
+	if (g_iscan->iscan_state == ISCAN_STATE_IDLE)
+#endif 
+		error = dev_wlc_ioctl(dev, WLC_SET_PASSIVE_SCAN, &as, sizeof(as));
+#if defined(WL_IW_USE_ISCAN)
+	else
+		g_iscan->scan_flag = as;
+#endif 
+	p += snprintf(p, MAX_WX_STRING, "OK");
+
+	wrqu->data.length = p - extra + 1;
+	return error;
+}
+
+static int
+wl_iw_set_passive_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int ps = 1;
+	int error = 0;
+	char *p = extra;
+
+#if defined(WL_IW_USE_ISCAN)
+	if (g_iscan->iscan_state == ISCAN_STATE_IDLE) {
+#endif 
+
+		 
+		if (g_scan_specified_ssid == 0) {
+			error = dev_wlc_ioctl(dev, WLC_SET_PASSIVE_SCAN, &ps, sizeof(ps));
+		}
+#if defined(WL_IW_USE_ISCAN)
+	}
+	else
+		g_iscan->scan_flag = ps;
+#endif 
+
+	p += snprintf(p, MAX_WX_STRING, "OK");
+
+	wrqu->data.length = p - extra + 1;
+	return error;
+}
+
+
+static int
+wl_iw_set_txpower(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = 0;
+	char *p = extra;
+	int txpower = -1;
+
+	txpower = bcm_atoi(extra + strlen(TXPOWER_SET_CMD) + 1);
+	if ((txpower >= 0) && (txpower <= 127)) {
+		txpower |= WL_TXPWR_OVERRIDE;
+		txpower = htod32(txpower);
+
+		error = dev_wlc_intvar_set(dev, "qtxpower", txpower);
+		p += snprintf(p, MAX_WX_STRING, "OK");
+		WL_TRACE(("%s: set TXpower 0x%X is OK\n", __FUNCTION__, txpower));
+	} else {
+		WL_ERROR(("%s: set tx power failed\n", __FUNCTION__));
+		p += snprintf(p, MAX_WX_STRING, "FAIL");
+	}
+
+	wrqu->data.length = p - extra + 1;
+	return error;
+}
+
+static int
+wl_iw_get_macaddr(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error;
+	char buf[128];
+	struct ether_addr *id;
+	char *p = extra;
+
+	
+	strcpy(buf, "cur_etheraddr");
+	error = dev_wlc_ioctl(dev, WLC_GET_VAR, buf, sizeof(buf));
+	id = (struct ether_addr *) buf;
+	p += snprintf(p, MAX_WX_STRING, "Macaddr = %02X:%02X:%02X:%02X:%02X:%02X\n",
+		id->octet[0], id->octet[1], id->octet[2],
+		id->octet[3], id->octet[4], id->octet[5]);
+	wrqu->data.length = p - extra + 1;
+
+	return error;
+}
+
+
+static int
+wl_iw_set_country(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	char country_code[WLC_CNTRY_BUF_SZ];
+	int error = 0;
+	char *p = extra;
+	int country_offset;
+	int country_code_size;
+	wl_country_t cspec = {{0}, 0, {0}};
+	char smbuf[WLC_IOCTL_SMLEN];
+
+	cspec.rev = -1;
+	memset(country_code, 0, sizeof(country_code));
+	memset(smbuf, 0, sizeof(smbuf));
+
+	country_offset = strcspn(extra, " ");
+	country_code_size = strlen(extra) - country_offset;
+
+	if (country_offset != 0) {
+		strncpy(country_code, extra + country_offset +1,
+			MIN(country_code_size, sizeof(country_code)));
+
+
+		memcpy(cspec.country_abbrev, country_code, WLC_CNTRY_BUF_SZ);
+		memcpy(cspec.ccode, country_code, WLC_CNTRY_BUF_SZ);
+
+		get_customized_country_code((char *)&cspec.country_abbrev, &cspec);
+
+		if ((error = dev_iw_iovar_setbuf(dev, "country", &cspec, \
+			sizeof(cspec), smbuf, sizeof(smbuf))) >= 0) {
+			p += snprintf(p, MAX_WX_STRING, "OK");
+			WL_ERROR(("%s: set country for %s as %s rev %d is OK\n", \
+				__FUNCTION__, country_code, cspec.ccode, cspec.rev));
+			dhd_bus_country_set(dev, &cspec);
+			goto exit;
+		}
+	}
+
+	WL_ERROR(("%s: set country for %s as %s rev %d failed\n", \
+			__FUNCTION__, country_code, cspec.ccode, cspec.rev));
+
+	p += snprintf(p, MAX_WX_STRING, "FAIL");
+
+exit:
+	wrqu->data.length = p - extra + 1;
+	return error;
+}
+
+#ifdef CUSTOMER_HW2
+static int
+wl_iw_set_power_mode(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = 0;
+	char *p = extra;
+	static int pm = PM_FAST;
+	int pm_local = PM_OFF;
+	char powermode_val = 0;
+
+	WL_TRACE_COEX(("%s: DHCP session cmd:%s\n", __FUNCTION__, extra));
+
+	strncpy((char *)&powermode_val, extra + strlen("POWERMODE") + 1, 1);
+
+	if (strnicmp((char *)&powermode_val, "1", strlen("1")) == 0) {
+
+		dev_wlc_ioctl(dev, WLC_GET_PM, &pm, sizeof(pm));
+		dev_wlc_ioctl(dev, WLC_SET_PM, &pm_local, sizeof(pm_local));
+
+		/* Disable packet filtering if necessary */
+		net_os_set_packet_filter(dev, 0);
+
+		g_bt->dhcp_done = false;
+		WL_TRACE_COEX(("%s: DHCP start, pm:%d changed to pm:%d\n",
+			__FUNCTION__, pm, pm_local));
+
+	} else if (strnicmp((char *)&powermode_val, "0", strlen("0")) == 0) {
+
+		dev_wlc_ioctl(dev, WLC_SET_PM, &pm, sizeof(pm));
+
+		/* Enable packet filtering if was turned off */
+		net_os_set_packet_filter(dev, 1);
+
+		g_bt->dhcp_done = true;
+
+	} else {
+		WL_ERROR(("%s Unkwown yet power setting, ignored\n",
+			__FUNCTION__));
+	}
+
+	p += snprintf(p, MAX_WX_STRING, "OK");
+
+	wrqu->data.length = p - extra + 1;
+
+	return error;
+}
+#endif
+
+
+bool btcoex_is_sco_active(struct net_device *dev)
+{
+	int ioc_res = 0;
+	bool res = false;
+	int sco_id_cnt = 0;
+	int param27;
+	int i;
+
+	for (i = 0; i < 12; i++) {
+
+		ioc_res = dev_wlc_intvar_get_reg(dev, "btc_params", 27, &param27);
+
+		WL_TRACE_COEX(("%s, sample[%d], btc params: 27:%x\n",
+			__FUNCTION__, i, param27));
+
+		if (ioc_res < 0) {
+			WL_ERROR(("%s ioc read btc params error\n", __FUNCTION__));
+			break;
+		}
+
+		if ((param27 & 0x6) == 2) {
+			sco_id_cnt++;
+		}
+
+		if (sco_id_cnt > 2) {
+			WL_TRACE_COEX(("%s, sco/esco detected, pkt id_cnt:%d  samples:%d\n",
+				__FUNCTION__, sco_id_cnt, i));
+			res = true;
+			break;
+		}
+
+		msleep(5);
+	}
+
+	return res;
+}
+
+#if defined(BT_DHCP_eSCO_FIX)
+
+static int set_btc_esco_params(struct net_device *dev, bool trump_sco)
+{
+	static bool saved_status = false;
+
+	char buf_reg50va_dhcp_on[8] = { 50, 00, 00, 00, 0x22, 0x80, 0x00, 0x00 };
+	char buf_reg51va_dhcp_on[8] = { 51, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
+	char buf_reg64va_dhcp_on[8] = { 64, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
+	char buf_reg65va_dhcp_on[8] = { 65, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
+	char buf_reg71va_dhcp_on[8] = { 71, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
+
+	uint32 regaddr;
+	static uint32 saved_reg50;
+	static uint32 saved_reg51;
+	static uint32 saved_reg64;
+	static uint32 saved_reg65;
+	static uint32 saved_reg71;
+
+	if (trump_sco) {
+
+		WL_TRACE_COEX(("Do new SCO/eSCO coex algo {save & override} \n"));
+
+		if  ((!dev_wlc_intvar_get_reg(dev, "btc_params", 50,  &saved_reg50)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 51,  &saved_reg51)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 64,  &saved_reg64)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 65,  &saved_reg65)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 71,  &saved_reg71))) {
+
+			saved_status = TRUE;
+			WL_TRACE_COEX(("%s saved bt_params[50,51,64,65,71]:"
+				" 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+				__FUNCTION__, saved_reg50, saved_reg51,
+				saved_reg64, saved_reg65, saved_reg71));
+
+		} else {
+			WL_ERROR((":%s: save btc_params failed\n",
+				__FUNCTION__));
+			saved_status = false;
+			return -1;
+		}
+
+		WL_TRACE_COEX(("override with [50,51,64,65,71]:"
+			" 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+			*(u32 *)(buf_reg50va_dhcp_on+4),
+			*(u32 *)(buf_reg51va_dhcp_on+4),
+			*(u32 *)(buf_reg64va_dhcp_on+4),
+			*(u32 *)(buf_reg65va_dhcp_on+4),
+			*(u32 *)(buf_reg71va_dhcp_on+4)));
+
+		dev_wlc_bufvar_set(dev, "btc_params", (char *)&buf_reg50va_dhcp_on[0], 8);
+		dev_wlc_bufvar_set(dev, "btc_params", (char *)&buf_reg51va_dhcp_on[0], 8);
+		dev_wlc_bufvar_set(dev, "btc_params", (char *)&buf_reg64va_dhcp_on[0], 8);
+		dev_wlc_bufvar_set(dev, "btc_params", (char *)&buf_reg65va_dhcp_on[0], 8);
+		dev_wlc_bufvar_set(dev, "btc_params", (char *)&buf_reg71va_dhcp_on[0], 8);
+
+		saved_status = true;
+
+	} else if (saved_status) {
+
+		WL_TRACE_COEX(("Do new SCO/eSCO coex algo {save & override} \n"));
+
+		regaddr = 50;
+		dev_wlc_intvar_set_reg(dev, "btc_params",
+			(char *)&regaddr, (char *)&saved_reg50);
+		regaddr = 51;
+		dev_wlc_intvar_set_reg(dev, "btc_params",
+			(char *)&regaddr, (char *)&saved_reg51);
+		regaddr = 64;
+		dev_wlc_intvar_set_reg(dev, "btc_params",
+			(char *)&regaddr, (char *)&saved_reg64);
+		regaddr = 65;
+		dev_wlc_intvar_set_reg(dev, "btc_params",
+			(char *)&regaddr, (char *)&saved_reg65);
+		regaddr = 71;
+		dev_wlc_intvar_set_reg(dev, "btc_params",
+			(char *)&regaddr, (char *)&saved_reg71);
+
+		WL_TRACE_COEX(("restore bt_params[50,51,64,65,71]: 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+			saved_reg50, saved_reg51, saved_reg64,
+			saved_reg65, saved_reg71));
+
+		saved_status = false;
+	} else {
+		WL_ERROR((":%s att to restore not saved BTCOEX params\n",
+			__FUNCTION__));
+		return -1;
+	}
+	return 0;
+}
+#endif
+
+static int
+wl_iw_get_power_mode(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error;
+	char *p = extra;
+	int pm_local = PM_FAST;
+
+	error = dev_wlc_ioctl(dev, WLC_GET_PM, &pm_local, sizeof(pm_local));
+	if (!error) {
+		WL_TRACE(("%s: Powermode = %d\n", __func__, pm_local));
+		if (pm_local == PM_OFF)
+			pm_local = 1; /* Active */
+		else
+			pm_local = 0; /* Auto */
+		p += snprintf(p, MAX_WX_STRING, "powermode = %d", pm_local);
+	}
+	else {
+		WL_TRACE(("%s: Error = %d\n", __func__, error));
+		p += snprintf(p, MAX_WX_STRING, "FAIL");
+	}
+	wrqu->data.length = p - extra + 1;
+	return error;
+}
+
+static int
+wl_iw_set_btcoex_dhcp(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = 0;
+	char *p = extra;
+#ifndef CUSTOMER_HW2
+	static int  pm = PM_FAST;
+	int  pm_local = PM_OFF;
+#endif
+	char powermode_val = 0;
+	char buf_reg66va_dhcp_on[8] = { 66, 00, 00, 00, 0x10, 0x27, 0x00, 0x00 };
+	char buf_reg41va_dhcp_on[8] = { 41, 00, 00, 00, 0x33, 0x00, 0x00, 0x00 };
+	char buf_reg68va_dhcp_on[8] = { 68, 00, 00, 00, 0x90, 0x01, 0x00, 0x00 };
+
+	uint32 regaddr;
+	static uint32 saved_reg66;
+	static uint32 saved_reg41;
+	static uint32 saved_reg68;
+	static bool saved_status = FALSE;
+
+	char buf_flag7_default[8] =   { 7, 00, 00, 00, 0x0, 0x00, 0x00, 0x00};
+
+#ifdef CUSTOMER_HW2
+	strncpy((char *)&powermode_val, extra + strlen("BTCOEXMODE") + 1, 1);
+#else
+	strncpy((char *)&powermode_val, extra + strlen("POWERMODE") + 1, 1);
+#endif
+
+	if (strnicmp((char *)&powermode_val, "1", strlen("1")) == 0) {
+
+		WL_TRACE_COEX(("%s: DHCP session start, cmd:%s\n", __FUNCTION__, extra));
+
+		if ((saved_status == FALSE) &&
+#ifndef CUSTOMER_HW2
+		   (!dev_wlc_ioctl(dev, WLC_GET_PM, &pm, sizeof(pm))) &&
+#endif
+		   (!dev_wlc_intvar_get_reg(dev, "btc_params", 66,  &saved_reg66)) &&
+		   (!dev_wlc_intvar_get_reg(dev, "btc_params", 41,  &saved_reg41)) &&
+		   (!dev_wlc_intvar_get_reg(dev, "btc_params", 68,  &saved_reg68))) {
+			WL_TRACE_COEX(("save regs {66,41,68} ->: 0x%x 0x%x 0x%x\n", \
+				saved_reg66, saved_reg41, saved_reg68));
+
+#ifndef CUSTOMER_HW2
+			dev_wlc_ioctl(dev, WLC_SET_PM, &pm_local, sizeof(pm_local));
+#endif
+
+				if (btcoex_is_sco_active(dev)) {
+
+					dev_wlc_bufvar_set(dev, "btc_params", \
+						(char *)&buf_reg66va_dhcp_on[0], \
+						 sizeof(buf_reg66va_dhcp_on));
+
+					dev_wlc_bufvar_set(dev, "btc_params", \
+						(char *)&buf_reg41va_dhcp_on[0], \
+						 sizeof(buf_reg41va_dhcp_on));
+
+					dev_wlc_bufvar_set(dev, "btc_params", \
+						(char *)&buf_reg68va_dhcp_on[0], \
+						 sizeof(buf_reg68va_dhcp_on));
+					saved_status = TRUE;
+
+					g_bt->bt_state = BT_DHCP_START;
+					g_bt->timer_on = 1;
+					mod_timer(&g_bt->timer, g_bt->timer.expires);
+					WL_TRACE_COEX(("%s enable BT DHCP Timer\n", \
+					__FUNCTION__));
+			}
+		}
+		else if (saved_status == TRUE) {
+			WL_ERROR(("%s was called w/o DHCP OFF. Continue\n", __FUNCTION__));
+		}
+	}
+#ifdef CUSTOMER_HW2
+	else if (strnicmp((char *)&powermode_val, "2", strlen("2")) == 0) {
+#else
+	else if (strnicmp((char *)&powermode_val, "0", strlen("0")) == 0) {
+#endif
+
+#ifndef CUSTOMER_HW2
+		dev_wlc_ioctl(dev, WLC_SET_PM, &pm, sizeof(pm));
+#endif
+
+		WL_TRACE_COEX(("%s disable BT DHCP Timer\n", __FUNCTION__));
+		if (g_bt->timer_on) {
+			g_bt->timer_on = 0;
+			del_timer_sync(&g_bt->timer);
+
+			if (g_bt->bt_state != BT_DHCP_IDLE) {
+				WL_TRACE_COEX(("%s bt->bt_state:%d\n",
+					__FUNCTION__, g_bt->bt_state));
+
+				up(&g_bt->bt_sem);
+			}
+		}
+
+		if (saved_status == TRUE) {
+			dev_wlc_bufvar_set(dev, "btc_flags", \
+				(char *)&buf_flag7_default[0], sizeof(buf_flag7_default));
+
+			regaddr = 66;
+			dev_wlc_intvar_set_reg(dev, "btc_params", \
+				(char *)&regaddr, (char *)&saved_reg66);
+			regaddr = 41;
+			dev_wlc_intvar_set_reg(dev, "btc_params", \
+				(char *)&regaddr, (char *)&saved_reg41);
+			regaddr = 68;
+			dev_wlc_intvar_set_reg(dev, "btc_params", \
+				(char *)&regaddr, (char *)&saved_reg68);
+
+			WL_TRACE_COEX(("restore regs {66,41,68} <- 0x%x 0x%x 0x%x\n", \
+					saved_reg66, saved_reg41, saved_reg68));
+		}
+		saved_status = FALSE;
+	}
+	else {
+		WL_ERROR(("%s Unkwown yet power setting, ignored\n",
+			__FUNCTION__));
+	}
+
+	p += snprintf(p, MAX_WX_STRING, "OK");
+
+	wrqu->data.length = p - extra + 1;
+
+	return error;
+}
+
+static int
+wl_iw_set_suspend(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int suspend_flag;
+	int ret_now;
+	int ret = 0;
+
+	suspend_flag = *(extra + strlen(SETSUSPEND_CMD) + 1) - '0';
+
+	if (suspend_flag != 0)
+		suspend_flag = 1;
+
+	ret_now = net_os_set_suspend_disable(dev, suspend_flag);
+
+	if (ret_now != suspend_flag) {
+		if (!(ret = net_os_set_suspend(dev, ret_now)))
+			WL_ERROR(("%s: Suspend Flag %d -> %d\n", \
+					__FUNCTION__, ret_now, suspend_flag));
+		else
+			WL_ERROR(("%s: failed %d\n", __FUNCTION__, ret));
+	}
+
+	return ret;
+}
+
+
+int
+wl_format_ssid(char* ssid_buf, uint8* ssid, int ssid_len)
+{
+	int i, c;
+	char *p = ssid_buf;
+
+	if (ssid_len > 32) ssid_len = 32;
+
+	for (i = 0; i < ssid_len; i++) {
+		c = (int)ssid[i];
+		if (c == '\\') {
+			*p++ = '\\';
+			*p++ = '\\';
+		} else if (isprint((uchar)c)) {
+			*p++ = (char)c;
+		} else {
+			p += sprintf(p, "\\x%02X", c);
+		}
+	}
+	*p = '\0';
+
+	return p - ssid_buf;
+}
+
+static int
+wl_iw_get_link_speed(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = 0;
+	char *p = extra;
+	static int link_speed;
+
+	net_os_wake_lock(dev);
+	if (g_onoff == G_WLAN_SET_ON) {
+		error = dev_wlc_ioctl(dev, WLC_GET_RATE, &link_speed, sizeof(link_speed));
+		link_speed *= 500000;
+	}
+
+	p += snprintf(p, MAX_WX_STRING, "LinkSpeed %d", link_speed/1000000);
+
+	wrqu->data.length = p - extra + 1;
+
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+
+static int
+wl_iw_get_dtim_skip(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = -1;
+	char *p = extra;
+	char iovbuf[32];
+
+	net_os_wake_lock(dev);
+	if (g_onoff == G_WLAN_SET_ON) {
+
+			memset(iovbuf, 0, sizeof(iovbuf));
+			strcpy(iovbuf, "bcn_li_dtim");
+
+			if ((error = dev_wlc_ioctl(dev, WLC_GET_VAR,
+				&iovbuf, sizeof(iovbuf))) >= 0) {
+
+				p += snprintf(p, MAX_WX_STRING, "Dtim_skip %d", iovbuf[0]);
+				WL_TRACE(("%s: get dtim_skip = %d\n", __FUNCTION__, iovbuf[0]));
+				wrqu->data.length = p - extra + 1;
+			}
+			else
+				WL_ERROR(("%s: get dtim_skip failed code %d\n", \
+					__FUNCTION__, error));
+	}
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+
+static int
+wl_iw_set_dtim_skip(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = -1;
+	char *p = extra;
+	int bcn_li_dtim;
+	char iovbuf[32];
+
+	net_os_wake_lock(dev);
+	if (g_onoff == G_WLAN_SET_ON) {
+
+		bcn_li_dtim = htod32((uint)*(extra + strlen(DTIM_SKIP_SET_CMD) + 1) - '0');
+
+		if ((bcn_li_dtim >= 0) || ((bcn_li_dtim <= 5))) {
+
+			memset(iovbuf, 0, sizeof(iovbuf));
+			bcm_mkiovar("bcn_li_dtim", (char *)&bcn_li_dtim,
+				4, iovbuf, sizeof(iovbuf));
+
+			if ((error = dev_wlc_ioctl(dev, WLC_SET_VAR,
+				&iovbuf, sizeof(iovbuf))) >= 0) {
+				p += snprintf(p, MAX_WX_STRING, "OK");
+
+				net_os_set_dtim_skip(dev, bcn_li_dtim);
+
+				WL_TRACE(("%s: set dtim_skip %d OK\n", __FUNCTION__, \
+					bcn_li_dtim));
+				goto exit;
+			}
+			else  WL_ERROR(("%s: set dtim_skip %d failed code %d\n", \
+				__FUNCTION__, bcn_li_dtim, error));
+		}
+		else  WL_ERROR(("%s Incorrect dtim_skip setting %d, ignored\n", \
+			__FUNCTION__, bcn_li_dtim));
+	}
+
+	p += snprintf(p, MAX_WX_STRING, "FAIL");
+
+exit:
+	wrqu->data.length = p - extra + 1;
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+
+static int
+wl_iw_get_band(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = -1;
+	char *p = extra;
+	static int band;
+
+	net_os_wake_lock(dev);
+
+	if (g_onoff == G_WLAN_SET_ON) {
+		error = dev_wlc_ioctl(dev, WLC_GET_BAND, &band, sizeof(band));
+
+		p += snprintf(p, MAX_WX_STRING, "Band %d", band);
+
+		wrqu->data.length = p - extra + 1;
+	}
+
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+
+static int
+wl_iw_set_band(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = -1;
+	char *p = extra;
+	uint band;
+
+	net_os_wake_lock(dev);
+
+	if (g_onoff == G_WLAN_SET_ON) {
+
+		band = htod32((uint)*(extra + strlen(BAND_SET_CMD) + 1) - '0');
+
+		if ((band == WLC_BAND_AUTO) || (band == WLC_BAND_5G) || (band == WLC_BAND_2G)) {
+
+			if ((error = dev_wlc_ioctl(dev, WLC_SET_BAND,
+				&band, sizeof(band))) >= 0) {
+				p += snprintf(p, MAX_WX_STRING, "OK");
+				WL_TRACE(("%s: set band %d OK\n", __FUNCTION__, band));
+				goto exit;
+			}
+			else WL_ERROR(("%s: set band %d failed code %d\n", __FUNCTION__, \
+					band, error));
+		}
+		else WL_ERROR(("%s Incorrect band setting %d, ignored\n", __FUNCTION__, band));
+	}
+
+	p += snprintf(p, MAX_WX_STRING, "FAIL");
+
+exit:
+	wrqu->data.length = p - extra + 1;
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+#ifdef PNO_SUPPORT
+
+static int
+wl_iw_set_pno_reset(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = -1;
+	char *p = extra;
+
+	net_os_wake_lock(dev);
+	if ((g_onoff == G_WLAN_SET_ON) && (dev != NULL)) {
+
+		if ((error = dhd_dev_pno_reset(dev)) >= 0) {
+				p += snprintf(p, MAX_WX_STRING, "OK");
+				WL_TRACE(("%s: set OK\n", __FUNCTION__));
+				goto exit;
+		}
+		else  WL_ERROR(("%s: failed code %d\n", __FUNCTION__, error));
+	}
+
+	p += snprintf(p, MAX_WX_STRING, "FAIL");
+
+exit:
+	wrqu->data.length = p - extra + 1;
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+
+
+static int
+wl_iw_set_pno_enable(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = -1;
+	char *p = extra;
+	int pfn_enabled;
+
+	net_os_wake_lock(dev);
+	pfn_enabled = htod32((uint)*(extra + strlen(PNOENABLE_SET_CMD) + 1) - '0');
+
+	if ((g_onoff == G_WLAN_SET_ON) && (dev != NULL)) {
+
+		if ((error = dhd_dev_pno_enable(dev, pfn_enabled)) >= 0) {
+				p += snprintf(p, MAX_WX_STRING, "OK");
+				WL_TRACE(("%s: set OK\n", __FUNCTION__));
+				goto exit;
+		}
+		else  WL_ERROR(("%s: failed code %d\n", __FUNCTION__, error));
+	}
+
+	p += snprintf(p, MAX_WX_STRING, "FAIL");
+
+exit:
+	wrqu->data.length = p - extra + 1;
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+
+
+static int
+wl_iw_set_pno_set(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int res = -1;
+	wlc_ssid_t ssids_local[MAX_PFN_LIST_COUNT];
+	int nssid = 0;
+	cmd_tlv_t *cmd_tlv_temp;
+	char *str_ptr;
+	int tlv_size_left;
+	int pno_time;
+	int pno_repeat;
+	int pno_freq_expo_max;
+
+#ifdef PNO_SET_DEBUG
+	int i;
+	char pno_in_example[] = {'P', 'N', 'O', 'S', 'E', 'T', 'U', 'P', ' ', \
+							'S', '1', '2', '0',
+							'S',
+							0x04,
+							'B', 'R', 'C', 'M',
+							'S',
+							0x04,
+							'G', 'O', 'O', 'G',
+							'T',
+							'1','E',
+							'R',
+							'2',
+							'M',
+							'2',
+							0x00
+							};
+#endif
+
+	net_os_wake_lock(dev);
+	WL_ERROR(("\n### %s: info->cmd:%x, info->flags:%x, u.data=0x%p, u.len=%d\n",
+		__FUNCTION__, info->cmd, info->flags,
+		wrqu->data.pointer, wrqu->data.length));
+
+	if (g_onoff == G_WLAN_SET_OFF) {
+		WL_TRACE(("%s: driver is not up yet after START\n", __FUNCTION__));
+		goto exit_proc;
+	}
+
+	if (wrqu->data.length < (strlen(PNOSETUP_SET_CMD) + sizeof(cmd_tlv_t))) {
+		WL_ERROR(("%s aggument=%d  less %d\n", __FUNCTION__, \
+			wrqu->data.length, strlen(PNOSETUP_SET_CMD) + sizeof(cmd_tlv_t)));
+		goto exit_proc;
+	}
+
+#ifdef PNO_SET_DEBUG
+	if (!(extra = kmalloc(sizeof(pno_in_example) +100, GFP_KERNEL))) {
+		res = -ENOMEM;
+		goto exit_proc;
+	}
+	memcpy(extra, pno_in_example, sizeof(pno_in_example));
+	wrqu->data.length = sizeof(pno_in_example);
+	for (i = 0; i < wrqu->data.length; i++)
+		printf("%02X ", extra[i]);
+	printf("\n");
+#endif
+
+	str_ptr = extra;
+#ifdef PNO_SET_DEBUG
+	str_ptr +=  strlen("PNOSETUP ");
+	tlv_size_left = wrqu->data.length - strlen("PNOSETUP ");
+#else
+	str_ptr +=  strlen(PNOSETUP_SET_CMD);
+	tlv_size_left = wrqu->data.length - strlen(PNOSETUP_SET_CMD);
+#endif
+
+	cmd_tlv_temp = (cmd_tlv_t *)str_ptr;
+	memset(ssids_local, 0, sizeof(ssids_local));
+	pno_repeat = pno_freq_expo_max = 0;
+
+	if ((cmd_tlv_temp->prefix == PNO_TLV_PREFIX) && \
+		(cmd_tlv_temp->version == PNO_TLV_VERSION) && \
+		(cmd_tlv_temp->subver == PNO_TLV_SUBVERSION))
+	{
+		str_ptr += sizeof(cmd_tlv_t);
+		tlv_size_left  -= sizeof(cmd_tlv_t);
+
+		if ((nssid = wl_iw_parse_ssid_list_tlv(&str_ptr, ssids_local, \
+				MAX_PFN_LIST_COUNT, &tlv_size_left)) <= 0) {
+			WL_ERROR(("SSID is not presented or corrupted ret=%d\n", nssid));
+			goto exit_proc;
+		}
+		else {
+			if ((str_ptr[0] != PNO_TLV_TYPE_TIME) || (tlv_size_left <= 1)) {
+				WL_ERROR(("%s scan duration corrupted field size %d\n", \
+						__FUNCTION__, tlv_size_left));
+				goto exit_proc;
+			}
+			str_ptr++;
+			pno_time = simple_strtoul(str_ptr, &str_ptr, 16);
+			WL_PNO(("%s: pno_time=%d\n", __FUNCTION__, pno_time));
+
+			if (str_ptr[0] != 0) {
+				if ((str_ptr[0] != PNO_TLV_FREQ_REPEAT)) {
+					WL_ERROR(("%s pno repeat : corrupted field\n", \
+						__FUNCTION__));
+					goto exit_proc;
+				}
+				str_ptr++;
+				pno_repeat = simple_strtoul(str_ptr, &str_ptr, 16);
+				WL_PNO(("%s :got pno_repeat=%d\n", __FUNCTION__, pno_repeat));
+				if (str_ptr[0] != PNO_TLV_FREQ_EXPO_MAX) {
+					WL_ERROR(("%s FREQ_EXPO_MAX corrupted field size\n", \
+							__FUNCTION__));
+					goto exit_proc;
+				}
+				str_ptr++;
+				pno_freq_expo_max = simple_strtoul(str_ptr, &str_ptr, 16);
+				WL_PNO(("%s: pno_freq_expo_max=%d\n", \
+							__FUNCTION__, pno_freq_expo_max));
+			}
+		}
+	}
+	else {
+		WL_ERROR(("%s get wrong TLV command\n", __FUNCTION__));
+		goto exit_proc;
+	}
+
+	res = dhd_dev_pno_set(dev, ssids_local, nssid, pno_time, pno_repeat, pno_freq_expo_max);
+
+exit_proc:
+	net_os_wake_unlock(dev);
+	return res;
+}
+#endif
+
+static int
+wl_iw_get_rssi(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	static int rssi = 0;
+	static wlc_ssid_t ssid = {0};
+	int error = 0;
+	char *p = extra;
+	static char ssidbuf[SSID_FMT_BUF_LEN];
+	scb_val_t scb_val;
+
+	net_os_wake_lock(dev);
+
+	bzero(&scb_val, sizeof(scb_val_t));
+
+	if (g_onoff == G_WLAN_SET_ON) {
+		error = dev_wlc_ioctl(dev, WLC_GET_RSSI, &scb_val, sizeof(scb_val_t));
+		if (error) {
+			WL_ERROR(("%s: Fails %d\n", __FUNCTION__, error));
+		} else {
+			rssi = dtoh32(scb_val.val);
+
+			error = dev_wlc_ioctl(dev, WLC_GET_SSID, &ssid, sizeof(ssid));
+			if (!error) {
+				ssid.SSID_len = dtoh32(ssid.SSID_len);
+				wl_format_ssid(ssidbuf, ssid.SSID, dtoh32(ssid.SSID_len));
+			}
+		}
+	}
+
+	WL_ASSOC(("%s ssid_len:%d, rssi:%d\n", __FUNCTION__, ssid.SSID_len, rssi));
+
+	if (error || (ssid.SSID_len == 0)) {
+		p += snprintf(p, MAX_WX_STRING, "FAIL");
+	} else {
+		p += snprintf(p, MAX_WX_STRING, "%s rssi %d ", ssidbuf, rssi);
+	}
+	wrqu->data.length = p - extra + 1;
+
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+int
+wl_iw_send_priv_event(
+	struct net_device *dev,
+	char *flag
+)
+{
+	union iwreq_data wrqu;
+	char extra[IW_CUSTOM_MAX + 1];
+	int cmd;
+
+	cmd = IWEVCUSTOM;
+	memset(&wrqu, 0, sizeof(wrqu));
+	if (strlen(flag) > sizeof(extra))
+		return -1;
+
+	strcpy(extra, flag);
+	wrqu.data.length = strlen(extra);
+	wireless_send_event(dev, cmd, &wrqu, extra);
+	net_os_wake_lock_timeout_enable(dev);
+	WL_TRACE(("Send IWEVCUSTOM Event as %s\n", extra));
+
+	return 0;
+}
+
+
+int
+wl_control_wl_start(struct net_device *dev)
+{
+	int ret = 0;
+	wl_iw_t *iw;
+
+	WL_TRACE(("Enter %s \n", __FUNCTION__));
+
+	if (!dev) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return -1;
+	}
+
+	iw = *(wl_iw_t **)netdev_priv(dev);
+
+	if (!iw) {
+		WL_ERROR(("%s: wl is null\n", __FUNCTION__));
+		return -1;
+	}
+	dhd_os_start_lock(iw->pub);
+
+	if (g_onoff == G_WLAN_SET_OFF) {
+		dhd_customer_gpio_wlan_ctrl(WLAN_RESET_ON);
+
+#if defined(BCMLXSDMMC)
+		sdioh_start(NULL, 0);
+#endif
+
+		ret = dhd_dev_reset(dev, 0);
+
+		if (ret == BCME_OK) {
+#if defined(BCMLXSDMMC)
+			sdioh_start(NULL, 1);
+#endif
+			dhd_dev_init_ioctl(dev);
+			g_onoff = G_WLAN_SET_ON;
+		}
+	}
+	WL_TRACE(("Exited %s \n", __FUNCTION__));
+
+	dhd_os_start_unlock(iw->pub);
+	return ret;
+}
+
+
+static int
+wl_iw_control_wl_off(
+	struct net_device *dev,
+	struct iw_request_info *info
+)
+{
+	int ret = 0;
+	wl_iw_t *iw;
+
+	WL_TRACE(("Enter %s\n", __FUNCTION__));
+
+	if (!dev) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return -1;
+	}
+
+	iw = *(wl_iw_t **)netdev_priv(dev);
+	if (!iw) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return -1;
+	}
+	dhd_os_start_lock(iw->pub);
+
+#ifdef SOFTAP
+	ap_cfg_running = FALSE;
+#endif
+
+	if (g_onoff == G_WLAN_SET_ON) {
+		g_onoff = G_WLAN_SET_OFF;
+#if defined(WL_IW_USE_ISCAN)
+		g_iscan->iscan_state = ISCAN_STATE_IDLE;
+#endif
+
+		dhd_dev_reset(dev, 1);
+
+#if defined(WL_IW_USE_ISCAN)
+#if !defined(CSCAN)
+		wl_iw_free_ss_cache();
+		wl_iw_run_ss_cache_timer(0);
+
+		g_ss_cache_ctrl.m_link_down = 1;
+#endif
+		memset(g_scan, 0, G_SCAN_RESULTS);
+		g_scan_specified_ssid = 0;
+#if defined(CONFIG_FIRST_SCAN)
+		g_first_broadcast_scan = BROADCAST_SCAN_FIRST_IDLE;
+		g_first_counter_scans = 0;
+#endif
+#endif
+
+#if defined(BCMLXSDMMC)
+		sdioh_stop(NULL);
+#endif
+
+		net_os_set_dtim_skip(dev, 0);
+
+		dhd_customer_gpio_wlan_ctrl(WLAN_RESET_OFF);
+
+		wl_iw_send_priv_event(dev, "STOP");
+	}
+
+	dhd_os_start_unlock(iw->pub);
+
+	WL_TRACE(("Exited %s\n", __FUNCTION__));
+
+	return ret;
+}
+
+static int
+wl_iw_control_wl_on(
+	struct net_device *dev,
+	struct iw_request_info *info
+)
+{
+	int ret = 0;
+
+	WL_TRACE(("Enter %s \n", __FUNCTION__));
+
+	if ((ret = wl_control_wl_start(dev)) != BCME_OK) {
+		WL_ERROR(("%s failed first attemp\n", __FUNCTION__));
+		dhd_customer_gpio_wlan_ctrl(WLAN_RESET_OFF);
+		if ((ret = wl_control_wl_start(dev)) != BCME_OK) {
+			WL_ERROR(("%s failed second attemp\n", __FUNCTION__));
+			net_os_send_hang_message(dev);
+			return ret;
+		}
+	}
+
+	wl_iw_send_priv_event(dev, "START");
+
+#ifdef SOFTAP
+	if (!ap_fw_loaded) {
+		wl_iw_iscan_set_scan_broadcast_prep(dev, 0);
+	}
+#else
+	wl_iw_iscan_set_scan_broadcast_prep(dev, 0);
+#endif
+
+	WL_TRACE(("Exited %s \n", __FUNCTION__));
+
+	return ret;
+}
+
+#ifdef SOFTAP
+static struct ap_profile my_ap;
+static int set_ap_cfg(struct net_device *dev, struct ap_profile *ap);
+static int get_assoc_sta_list(struct net_device *dev, char *buf, int len);
+static int set_ap_mac_list(struct net_device *dev, void *buf);
+
+#define PTYPE_STRING	0
+#define PTYPE_INTDEC	1
+#define PTYPE_INTHEX	2
+#define PTYPE_STR_HEX	3
+int get_parmeter_from_string(
+	char **str_ptr, const char *token, int param_type, void  *dst, int param_max_len);
+
+#endif
+
+int hex2num(char c)
+{
+	if (c >= '0' && c <= '9')
+		return c - '0';
+	if (c >= 'a' && c <= 'f')
+		return c - 'a' + 10;
+	if (c >= 'A' && c <= 'F')
+		return c - 'A' + 10;
+	return -1;
+}
+
+int hex2byte(const char *hex)
+{
+	int a, b;
+	a = hex2num(*hex++);
+	if (a < 0)
+		return -1;
+	b = hex2num(*hex++);
+	if (b < 0)
+		return -1;
+	return (a << 4) | b;
+}
+
+
+
+int hstr_2_buf(const char *txt, u8 *buf, int len)
+{
+	int i;
+
+	for (i = 0; i < len; i++) {
+		int a, b;
+
+		a = hex2num(*txt++);
+		if (a < 0)
+			return -1;
+		b = hex2num(*txt++);
+		if (b < 0)
+			return -1;
+		*buf++ = (a << 4) | b;
+	}
+
+	return 0;
+}
+
+#if defined(SOFTAP) && defined(SOFTAP_TLV_CFG)
+
+static int wl_iw_softap_cfg_tlv(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int res = -1;
+	char *str_ptr;
+	int tlv_size_left;
+
+
+#define SOFTAP_TLV_DEBUG  1
+#ifdef SOFTAP_TLV_DEBUG
+char softap_cmd_example[] = {
+
+	'S', 'O', 'F', 'T', 'A', 'P', 'S', 'E', 'T', ' ',
+
+	SOFTAP_TLV_PREFIX, SOFTAP_TLV_VERSION,
+	SOFTAP_TLV_SUBVERSION, SOFTAP_TLV_RESERVED,
+
+	TLV_TYPE_SSID,		9, 'B', 'R', 'C', 'M', ',', 'G', 'O', 'O', 'G',
+
+	TLV_TYPE_SECUR,		4, 'O', 'P', 'E', 'N',
+
+	TLV_TYPE_KEY,		4, 0x31, 0x32, 0x33, 0x34,
+
+	TLV_TYPE_CHANNEL,	4, 0x06, 0x00, 0x00, 0x00
+};
+#endif
+
+
+#ifdef SOFTAP_TLV_DEBUG
+	{
+	int i;
+	if (!(extra = kmalloc(sizeof(softap_cmd_example) +10, GFP_KERNEL)))
+		return -ENOMEM;
+	memcpy(extra, softap_cmd_example, sizeof(softap_cmd_example));
+	wrqu->data.length = sizeof(softap_cmd_example);
+	print_buf(extra, wrqu->data.length, 16);
+	for (i = 0; i < wrqu->data.length; i++)
+		printf("%c ", extra[i]);
+	printf("\n");
+	}
+#endif
+
+	WL_ERROR(("\n### %s: info->cmd:%x, info->flags:%x, u.data=0x%p, u.len=%d\n",
+		__FUNCTION__, info->cmd, info->flags,
+		wrqu->data.pointer, wrqu->data.length));
+
+	if (g_onoff == G_WLAN_SET_OFF) {
+		WL_TRACE(("%s: driver is not up yet after START\n", __FUNCTION__));
+		return -1;
+	}
+
+	if (wrqu->data.length < (strlen(SOFTAP_SET_CMD) + sizeof(cmd_tlv_t))) {
+		WL_ERROR(("%s argument=%d  less %d\n", __FUNCTION__,
+			wrqu->data.length, strlen(SOFTAP_SET_CMD) + sizeof(cmd_tlv_t)));
+		return -1;
+	}
+
+	str_ptr =  extra + strlen(SOFTAP_SET_CMD)+1; 
+	tlv_size_left = wrqu->data.length - (strlen(SOFTAP_SET_CMD)+1);
+
+	memset(&my_ap, 0, sizeof(my_ap));
+
+	return res;
+}
+#endif
+
+
+#ifdef SOFTAP
+int init_ap_profile_from_string(char *param_str, struct ap_profile *ap_cfg)
+{
+	char *str_ptr = param_str;
+	char sub_cmd[16];
+	int ret = 0;
+
+	memset(sub_cmd, 0, sizeof(sub_cmd));
+	memset(ap_cfg, 0, sizeof(struct ap_profile));
+
+	if (get_parmeter_from_string(&str_ptr, "ASCII_CMD=",
+		PTYPE_STRING, sub_cmd, SSID_LEN) != 0) {
+		return -1;
+	}
+	if (strncmp(sub_cmd, "AP_CFG", 6)) {
+		WL_ERROR(("ERROR: sub_cmd:%s != 'AP_CFG'!\n", sub_cmd));
+		return -1;
+	}
+
+	ret = get_parmeter_from_string(&str_ptr, "SSID=", PTYPE_STRING, ap_cfg->ssid, SSID_LEN);
+
+	ret |= get_parmeter_from_string(&str_ptr, "SEC=", PTYPE_STRING,  ap_cfg->sec, SEC_LEN);
+
+	ret |= get_parmeter_from_string(&str_ptr, "KEY=", PTYPE_STRING,  ap_cfg->key, KEY_LEN);
+
+	ret |= get_parmeter_from_string(&str_ptr, "CHANNEL=", PTYPE_INTDEC, &ap_cfg->channel, 5);
+
+	get_parmeter_from_string(&str_ptr, "PREAMBLE=", PTYPE_INTDEC, &ap_cfg->preamble, 5);
+
+	get_parmeter_from_string(&str_ptr, "MAX_SCB=", PTYPE_INTDEC,  &ap_cfg->max_scb, 5);
+
+	get_parmeter_from_string(&str_ptr, "HIDDEN=", PTYPE_INTDEC, &ap_cfg->closednet, 5);
+
+	get_parmeter_from_string(&str_ptr, "COUNTRY=", PTYPE_STRING, &ap_cfg->country_code, 3);
+
+	return ret;
+}
+#endif
+
+
+#ifdef SOFTAP
+static int iwpriv_set_ap_config(struct net_device *dev,
+		struct iw_request_info *info,
+		union iwreq_data *wrqu,
+		char *ext)
+{
+	int res = 0;
+	char  *extra = NULL;
+	struct ap_profile *ap_cfg = &my_ap;
+
+	WL_TRACE(("%s: info->cmd:%x, info->flags:%x, u.data:%p, u.len:%d\n",
+		__FUNCTION__,
+		info->cmd, info->flags,
+		wrqu->data.pointer, wrqu->data.length));
+
+	if (wrqu->data.length != 0) {
+
+		char *str_ptr;
+
+		if (!(extra = kmalloc(wrqu->data.length+1, GFP_KERNEL)))
+			return -ENOMEM;
+
+		if (copy_from_user(extra, wrqu->data.pointer, wrqu->data.length)) {
+			kfree(extra);
+			return -EFAULT;
+		}
+
+		extra[wrqu->data.length] = 0;
+		WL_SOFTAP((" Got str param in iw_point:\n %s\n", extra));
+
+		memset(ap_cfg, 0, sizeof(struct ap_profile));
+
+		str_ptr = extra;
+
+		if ((res = init_ap_profile_from_string(extra, ap_cfg)) < 0) {
+			WL_ERROR(("%s failed to parse %d\n", __FUNCTION__, res));
+			kfree(extra);
+			return -1;
+		}
+
+	} else {
+		WL_ERROR(("IWPRIV argument len = 0 \n"));
+		return -1;
+	}
+
+	if ((res = set_ap_cfg(dev, ap_cfg)) < 0)
+		WL_ERROR(("%s failed to set_ap_cfg %d\n", __FUNCTION__, res));
+
+	kfree(extra);
+
+	return res;
+}
+#endif
+
+
+#ifdef SOFTAP
+static int iwpriv_get_assoc_list(struct net_device *dev,
+		struct iw_request_info *info,
+		union iwreq_data *p_iwrq,
+		char *extra)
+{
+	int i, ret = 0;
+	char mac_buf[256];
+	struct maclist *sta_maclist = (struct maclist *)mac_buf;
+
+	char mac_lst[384];
+	char *p_mac_str;
+	char *p_mac_str_end;
+
+	if ((!dev) || (!extra)) {
+		return -EINVAL;
+	}
+
+	net_os_wake_lock(dev);
+
+	WL_TRACE(("\n %s: IWPRIV IOCTL: cmd:%hx, flags:%hx, extra:%p, iwp.len:%d, \
+		iwp.len:%p, iwp.flags:%x  \n", __FUNCTION__, info->cmd, info->flags, \
+		extra, p_iwrq->data.length, p_iwrq->data.pointer, p_iwrq->data.flags));
+
+	memset(sta_maclist, 0, sizeof(mac_buf));
+
+	sta_maclist->count = 8;
+
+	WL_SOFTAP(("%s: net device:%s, buf_sz:%d\n",
+		__FUNCTION__, dev->name, sizeof(mac_buf)));
+
+	if ((ret = get_assoc_sta_list(dev, mac_buf, sizeof(mac_buf))) < 0) {
+		WL_ERROR(("%s: sta list ioctl error:%d\n",
+			__FUNCTION__, ret));
+		goto func_exit;
+	}
+
+	WL_SOFTAP(("%s: got %d stations\n", __FUNCTION__,
+		sta_maclist->count));
+
+	memset(mac_lst, 0, sizeof(mac_lst));
+	p_mac_str = mac_lst;
+	p_mac_str_end = &mac_lst[sizeof(mac_lst)-1];
+
+	for (i = 0; i < 8; i++) {
+		struct ether_addr *id = &sta_maclist->ea[i];
+		if (!ETHER_ISNULLADDR(id->octet)) {
+			scb_val_t scb_val;
+			int rssi = 0;
+
+			bzero(&scb_val, sizeof(scb_val_t));
+
+			if ((p_mac_str_end - p_mac_str) <= 36) {
+				WL_ERROR(("%s: mac list buf is < 36 for item[%i] item\n",
+					__FUNCTION__, i));
+				break;
+			}
+
+			p_mac_str += snprintf(p_mac_str, MAX_WX_STRING,
+			"\nMac[%d]=%02X:%02X:%02X:%02X:%02X:%02X,", i,
+			id->octet[0], id->octet[1], id->octet[2],
+			id->octet[3], id->octet[4], id->octet[5]);
+
+			bcopy(id->octet, &scb_val.ea, 6);
+			ret = dev_wlc_ioctl(dev, WLC_GET_RSSI, &scb_val, sizeof(scb_val_t));
+			if (ret  < 0) {
+				snprintf(p_mac_str, MAX_WX_STRING, "RSSI:ERR");
+				WL_ERROR(("%s: RSSI ioctl error:%d\n",
+					__FUNCTION__, ret));
+				break;
+			}
+
+			rssi = dtoh32(scb_val.val);
+			p_mac_str += snprintf(p_mac_str, MAX_WX_STRING,
+			"RSSI:%d", rssi);
+		}
+	}
+
+	p_iwrq->data.length = strlen(mac_lst) + 1;
+
+	WL_SOFTAP(("%s: data to user:\n%s\n usr_ptr:%p\n", __FUNCTION__,
+		mac_lst, p_iwrq->data.pointer));
+
+	if (p_iwrq->data.length) {
+		bcopy(mac_lst, extra, p_iwrq->data.length);
+	}
+
+func_exit:
+	net_os_wake_unlock(dev);
+
+	WL_TRACE(("Exited %s \n", __FUNCTION__));
+	return ret;
+}
+#endif
+
+
+#ifdef SOFTAP
+#define MAC_FILT_MAX 8
+static int iwpriv_set_mac_filters(struct net_device *dev,
+		struct iw_request_info *info,
+		union iwreq_data *wrqu,
+		char *ext)
+{
+	int i, ret = -1;
+	char  * extra = NULL;
+	int mac_cnt = 0;
+	int mac_mode = 0;
+	struct ether_addr *p_ea;
+	struct mac_list_set mflist_set;
+
+	WL_SOFTAP((">>> Got IWPRIV SET_MAC_FILTER IOCTL:  info->cmd:%x, \
+			info->flags:%x, u.data:%p, u.len:%d\n",
+			info->cmd, info->flags,
+			wrqu->data.pointer, wrqu->data.length));
+
+	if (wrqu->data.length != 0) {
+
+		char *str_ptr;
+
+		if (!(extra = kmalloc(wrqu->data.length+1, GFP_KERNEL)))
+			return -ENOMEM;
+
+		if (copy_from_user(extra, wrqu->data.pointer, wrqu->data.length)) {
+			kfree(extra);
+			return -EFAULT;
+		}
+
+		extra[wrqu->data.length] = 0;
+		WL_SOFTAP((" Got parameter string in iw_point:\n %s \n", extra));
+
+		memset(&mflist_set, 0, sizeof(mflist_set));
+
+		str_ptr = extra;
+
+		if (get_parmeter_from_string(&str_ptr, "MAC_MODE=",
+			PTYPE_INTDEC, &mac_mode, 4) != 0) {
+			WL_ERROR(("ERROR: 'MAC_MODE=' token is missing\n"));
+			goto exit_proc;
+		}
+
+		p_ea = &mflist_set.mac_list.ea[0];
+
+		if (get_parmeter_from_string(&str_ptr, "MAC_CNT=",
+			PTYPE_INTDEC, &mac_cnt, 4) != 0) {
+			WL_ERROR(("ERROR: 'MAC_CNT=' token param is missing \n"));
+			goto exit_proc;
+		}
+
+		if (mac_cnt > MAC_FILT_MAX) {
+			WL_ERROR(("ERROR: number of MAC filters > MAX\n"));
+			goto exit_proc;
+		}
+
+		for (i=0; i < mac_cnt; i++)
+			if (get_parmeter_from_string(&str_ptr, "MAC=",
+				PTYPE_STR_HEX, &p_ea[i], 12) != 0) {
+				WL_ERROR(("ERROR: MAC_filter[%d] is missing !\n", i));
+				goto exit_proc;
+			}
+
+		WL_SOFTAP(("MAC_MODE=:%d, MAC_CNT=%d, MACs:..\n", mac_mode, mac_cnt));
+		for (i = 0; i < mac_cnt; i++) {
+		   WL_SOFTAP(("mac_filt[%d]:", i));
+		   print_buf(&p_ea[i], 6, 0);
+		}
+
+		mflist_set.mode = mac_mode;
+		mflist_set.mac_list.count = mac_cnt;
+		set_ap_mac_list(dev, &mflist_set);
+
+		wrqu->data.pointer = NULL;
+		wrqu->data.length = 0;
+		ret = 0;
+
+	} else {
+		WL_ERROR(("IWPRIV argument len is 0\n"));
+		return -1;
+	}
+
+	exit_proc:
+	kfree(extra);
+	return ret;
+}
+#endif
+
+
+#ifdef SOFTAP
+static int iwpriv_set_ap_sta_disassoc(struct net_device *dev,
+        struct iw_request_info *info,
+        union iwreq_data *wrqu,
+        char *ext)
+{
+	int res = 0;
+	char sta_mac[6] = {0, 0, 0, 0, 0, 0};
+	char cmd_buf[256];
+	char *str_ptr = cmd_buf;
+
+	WL_SOFTAP((">>%s called\n args: info->cmd:%x,"
+		" info->flags:%x, u.data.p:%p, u.data.len:%d\n",
+		__FUNCTION__, info->cmd, info->flags,
+		wrqu->data.pointer, wrqu->data.length));
+
+	if (wrqu->data.length != 0) {
+
+		if (copy_from_user(cmd_buf, wrqu->data.pointer, wrqu->data.length)) {
+			return -EFAULT;
+		}
+
+		if (get_parmeter_from_string(&str_ptr,
+			"MAC=", PTYPE_STR_HEX, sta_mac, 12) == 0) {
+			res = wl_iw_softap_deassoc_stations(dev, sta_mac);
+		} else  {
+			WL_ERROR(("ERROR: STA_MAC= token not found\n"));
+		}
+	}
+
+	return res;
+}
+#endif
+
+#endif
+
+
+#if WIRELESS_EXT < 13
+struct iw_request_info
+{
+	__u16		cmd;		
+	__u16		flags;		
+};
+
+typedef int (*iw_handler)(struct net_device *dev,
+		struct iw_request_info *info,
+		void *wrqu,
+		char *extra);
+#endif 
+
+static int
+wl_iw_config_commit(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	void *zwrq,
+	char *extra
+)
+{
+	wlc_ssid_t ssid;
+	int error;
+	struct sockaddr bssid;
+
+	WL_TRACE(("%s: SIOCSIWCOMMIT\n", dev->name));
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_SSID, &ssid, sizeof(ssid))))
+		return error;
+
+	ssid.SSID_len = dtoh32(ssid.SSID_len);
+
+	if (!ssid.SSID_len)
+		return 0;
+
+	bzero(&bssid, sizeof(struct sockaddr));
+	if ((error = dev_wlc_ioctl(dev, WLC_REASSOC, &bssid, ETHER_ADDR_LEN))) {
+		WL_ERROR(("%s: WLC_REASSOC to %s failed \n", __FUNCTION__, ssid.SSID));
+		return error;
+	}
+
+	return 0;
+}
+
+static int
+wl_iw_get_name(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	char *cwrq,
+	char *extra
+)
+{
+	WL_TRACE(("%s: SIOCGIWNAME\n", dev->name));
+
+	strcpy(cwrq, "IEEE 802.11-DS");
+
+	return 0;
+}
+
+static int
+wl_iw_set_freq(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_freq *fwrq,
+	char *extra
+)
+{
+	int error, chan;
+	uint sf = 0;
+
+	WL_TRACE(("%s %s: SIOCSIWFREQ\n", __FUNCTION__, dev->name));
+
+#if defined(SOFTAP)
+	if (ap_cfg_running) {
+		WL_TRACE(("%s:>> not executed, 'SOFT_AP is active' \n", __FUNCTION__));
+		return 0;
+	}
+#endif
+
+	
+	if (fwrq->e == 0 && fwrq->m < MAXCHANNEL) {
+		chan = fwrq->m;
+	}
+
+	
+	else {
+		
+		if (fwrq->e >= 6) {
+			fwrq->e -= 6;
+			while (fwrq->e--)
+				fwrq->m *= 10;
+		} else if (fwrq->e < 6) {
+			while (fwrq->e++ < 6)
+				fwrq->m /= 10;
+		}
+	
+	if (fwrq->m > 4000 && fwrq->m < 5000)
+		sf = WF_CHAN_FACTOR_4_G; 
+
+		chan = wf_mhz2channel(fwrq->m, sf);
+	}
+	chan = htod32(chan);
+	if ((error = dev_wlc_ioctl(dev, WLC_SET_CHANNEL, &chan, sizeof(chan))))
+		return error;
+
+	g_wl_iw_params.target_channel = chan;
+	
+	return -EINPROGRESS;
+}
+
+static int
+wl_iw_get_freq(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_freq *fwrq,
+	char *extra
+)
+{
+	channel_info_t ci;
+	int error;
+
+	WL_TRACE(("%s: SIOCGIWFREQ\n", dev->name));
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_CHANNEL, &ci, sizeof(ci))))
+		return error;
+
+	fwrq->m = dtoh32(ci.hw_channel);
+	fwrq->e = dtoh32(0);
+	return 0;
+}
+
+static int
+wl_iw_set_mode(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	__u32 *uwrq,
+	char *extra
+)
+{
+	int infra = 0, ap = 0, error = 0;
+
+	WL_TRACE(("%s: SIOCSIWMODE\n", dev->name));
+
+	switch (*uwrq) {
+	case IW_MODE_MASTER:
+		infra = ap = 1;
+		break;
+	case IW_MODE_ADHOC:
+	case IW_MODE_AUTO:
+		break;
+	case IW_MODE_INFRA:
+		infra = 1;
+		break;
+	default:
+		return -EINVAL;
+	}
+	infra = htod32(infra);
+	ap = htod32(ap);
+
+	if ((error = dev_wlc_ioctl(dev, WLC_SET_INFRA, &infra, sizeof(infra))) ||
+	    (error = dev_wlc_ioctl(dev, WLC_SET_AP, &ap, sizeof(ap))))
+		return error;
+
+	
+	return -EINPROGRESS;
+}
+
+static int
+wl_iw_get_mode(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	__u32 *uwrq,
+	char *extra
+)
+{
+	int error, infra = 0, ap = 0;
+
+	WL_TRACE(("%s: SIOCGIWMODE\n", dev->name));
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_INFRA, &infra, sizeof(infra))) ||
+	    (error = dev_wlc_ioctl(dev, WLC_GET_AP, &ap, sizeof(ap))))
+		return error;
+
+	infra = dtoh32(infra);
+	ap = dtoh32(ap);
+	*uwrq = infra ? ap ? IW_MODE_MASTER : IW_MODE_INFRA : IW_MODE_ADHOC;
+
+	return 0;
+}
+
+static int
+wl_iw_get_range(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	struct iw_range *range = (struct iw_range *) extra;
+	wl_uint32_list_t *list;
+	wl_rateset_t rateset;
+	int8 *channels;
+	int error, i, k;
+	uint sf, ch;
+
+	int phytype;
+	int bw_cap = 0, sgi_tx = 0, nmode = 0;
+	channel_info_t ci;
+	uint8 nrate_list2copy = 0;
+	uint16 nrate_list[4][8] = { {13, 26, 39, 52, 78, 104, 117, 130},
+		{14, 29, 43, 58, 87, 116, 130, 144},
+		{27, 54, 81, 108, 162, 216, 243, 270},
+		{30, 60, 90, 120, 180, 240, 270, 300}};
+
+	WL_TRACE(("%s: SIOCGIWRANGE\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	channels = kmalloc((MAXCHANNEL+1)*4, GFP_KERNEL);
+	if (!channels) {
+		WL_ERROR(("Could not alloc channels\n"));
+		return -ENOMEM;
+	}
+	list = (wl_uint32_list_t *)channels;
+
+	dwrq->length = sizeof(struct iw_range);
+	memset(range, 0, sizeof(range));
+
+	range->min_nwid = range->max_nwid = 0;
+
+	list->count = htod32(MAXCHANNEL);
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_VALID_CHANNELS, channels, (MAXCHANNEL+1)*4))) {
+		kfree(channels);
+		return error;
+	}
+	for (i = 0; i < dtoh32(list->count) && i < IW_MAX_FREQUENCIES; i++) {
+		range->freq[i].i = dtoh32(list->element[i]);
+
+		ch = dtoh32(list->element[i]);
+		if (ch <= CH_MAX_2G_CHANNEL)
+			sf = WF_CHAN_FACTOR_2_4_G;
+		else
+			sf = WF_CHAN_FACTOR_5_G;
+
+		range->freq[i].m = wf_channel2mhz(ch, sf);
+		range->freq[i].e = 6;
+	}
+	range->num_frequency = range->num_channels = i;
+
+	range->max_qual.qual = 5;
+	
+	range->max_qual.level = 0x100 - 200;	
+	
+	range->max_qual.noise = 0x100 - 200;	
+	
+	range->sensitivity = 65535;
+
+#if WIRELESS_EXT > 11
+	
+	range->avg_qual.qual = 3;
+	
+	range->avg_qual.level = 0x100 + WL_IW_RSSI_GOOD;
+	
+	range->avg_qual.noise = 0x100 - 75;	
+#endif 
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_CURR_RATESET, &rateset, sizeof(rateset)))) {
+		kfree(channels);
+		return error;
+	}
+	rateset.count = dtoh32(rateset.count);
+	range->num_bitrates = rateset.count;
+	for (i = 0; i < rateset.count && i < IW_MAX_BITRATES; i++)
+		range->bitrate[i] = (rateset.rates[i]& 0x7f) * 500000; 
+	dev_wlc_intvar_get(dev, "nmode", &nmode);
+	dev_wlc_ioctl(dev, WLC_GET_PHYTYPE, &phytype, sizeof(phytype));
+
+	if (nmode == 1 && phytype == WLC_PHY_TYPE_SSN) {
+		dev_wlc_intvar_get(dev, "mimo_bw_cap", &bw_cap);
+		dev_wlc_intvar_get(dev, "sgi_tx", &sgi_tx);
+		dev_wlc_ioctl(dev, WLC_GET_CHANNEL, &ci, sizeof(channel_info_t));
+		ci.hw_channel = dtoh32(ci.hw_channel);
+
+		if (bw_cap == 0 ||
+			(bw_cap == 2 && ci.hw_channel <= 14)) {
+			if (sgi_tx == 0)
+				nrate_list2copy = 0;
+			else
+				nrate_list2copy = 1;
+		}
+		if (bw_cap == 1 ||
+			(bw_cap == 2 && ci.hw_channel >= 36)) {
+			if (sgi_tx == 0)
+				nrate_list2copy = 2;
+			else
+				nrate_list2copy = 3;
+		}
+		range->num_bitrates += 8;
+		for (k = 0; i < range->num_bitrates; k++, i++) {
+			
+			range->bitrate[i] = (nrate_list[nrate_list2copy][k]) * 500000;
+		}
+	}
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_PHYTYPE, &i, sizeof(i)))) {
+		kfree(channels);
+		return error;
+	}
+	i = dtoh32(i);
+	if (i == WLC_PHY_TYPE_A)
+		range->throughput = 24000000;	
+	else
+		range->throughput = 1500000;	
+
+	range->min_rts = 0;
+	range->max_rts = 2347;
+	range->min_frag = 256;
+	range->max_frag = 2346;
+
+	range->max_encoding_tokens = DOT11_MAX_DEFAULT_KEYS;
+	range->num_encoding_sizes = 4;
+	range->encoding_size[0] = WEP1_KEY_SIZE;
+	range->encoding_size[1] = WEP128_KEY_SIZE;
+#if WIRELESS_EXT > 17
+	range->encoding_size[2] = TKIP_KEY_SIZE;
+#else
+	range->encoding_size[2] = 0;
+#endif
+	range->encoding_size[3] = AES_KEY_SIZE;
+
+	range->min_pmp = 0;
+	range->max_pmp = 0;
+	range->min_pmt = 0;
+	range->max_pmt = 0;
+	range->pmp_flags = 0;
+	range->pm_capa = 0;
+
+	range->num_txpower = 2;
+	range->txpower[0] = 1;
+	range->txpower[1] = 255;
+	range->txpower_capa = IW_TXPOW_MWATT;
+
+#if WIRELESS_EXT > 10
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 19;
+
+	range->retry_capa = IW_RETRY_LIMIT;
+	range->retry_flags = IW_RETRY_LIMIT;
+	range->r_time_flags = 0;
+	
+	range->min_retry = 1;
+	range->max_retry = 255;
+	
+	range->min_r_time = 0;
+	range->max_r_time = 0;
+#endif 
+
+#if WIRELESS_EXT > 17
+	range->enc_capa = IW_ENC_CAPA_WPA;
+	range->enc_capa |= IW_ENC_CAPA_CIPHER_TKIP;
+	range->enc_capa |= IW_ENC_CAPA_CIPHER_CCMP;
+#ifdef BCMWPA2
+	range->enc_capa |= IW_ENC_CAPA_WPA2;
+#endif
+
+	IW_EVENT_CAPA_SET_KERNEL(range->event_capa);
+
+	IW_EVENT_CAPA_SET(range->event_capa, SIOCGIWAP);
+	IW_EVENT_CAPA_SET(range->event_capa, SIOCGIWSCAN);
+	IW_EVENT_CAPA_SET(range->event_capa, IWEVTXDROP);
+	IW_EVENT_CAPA_SET(range->event_capa, IWEVMICHAELMICFAILURE);
+#ifdef BCMWPA2
+	IW_EVENT_CAPA_SET(range->event_capa, IWEVPMKIDCAND);
+#endif
+#endif 
+
+	kfree(channels);
+
+	return 0;
+}
+
+static int
+rssi_to_qual(int rssi)
+{
+	if (rssi <= WL_IW_RSSI_NO_SIGNAL)
+		return 0;
+	else if (rssi <= WL_IW_RSSI_VERY_LOW)
+		return 1;
+	else if (rssi <= WL_IW_RSSI_LOW)
+		return 2;
+	else if (rssi <= WL_IW_RSSI_GOOD)
+		return 3;
+	else if (rssi <= WL_IW_RSSI_VERY_GOOD)
+		return 4;
+	else
+		return 5;
+}
+
+static int
+wl_iw_set_spy(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_iw_t *iw = *(wl_iw_t **)netdev_priv(dev);
+	struct sockaddr *addr = (struct sockaddr *) extra;
+	int i;
+
+	WL_TRACE(("%s: SIOCSIWSPY\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	iw->spy_num = MIN(ARRAYSIZE(iw->spy_addr), dwrq->length);
+	for (i = 0; i < iw->spy_num; i++)
+		memcpy(&iw->spy_addr[i], addr[i].sa_data, ETHER_ADDR_LEN);
+	memset(iw->spy_qual, 0, sizeof(iw->spy_qual));
+
+	return 0;
+}
+
+static int
+wl_iw_get_spy(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_iw_t *iw = *(wl_iw_t **)netdev_priv(dev);
+	struct sockaddr *addr = (struct sockaddr *) extra;
+	struct iw_quality *qual = (struct iw_quality *) &addr[iw->spy_num];
+	int i;
+
+	WL_TRACE(("%s: SIOCGIWSPY\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	dwrq->length = iw->spy_num;
+	for (i = 0; i < iw->spy_num; i++) {
+		memcpy(addr[i].sa_data, &iw->spy_addr[i], ETHER_ADDR_LEN);
+		addr[i].sa_family = AF_UNIX;
+		memcpy(&qual[i], &iw->spy_qual[i], sizeof(struct iw_quality));
+		iw->spy_qual[i].updated = 0;
+	}
+
+	return 0;
+}
+
+
+static int
+wl_iw_ch_to_chanspec(int ch, wl_join_params_t *join_params, int *join_params_size)
+{
+	chanspec_t chanspec = 0;
+
+	if (ch != 0) {
+
+		join_params->params.chanspec_num = 1;
+		join_params->params.chanspec_list[0] = ch;
+
+		if (join_params->params.chanspec_list[0])
+			chanspec |= WL_CHANSPEC_BAND_2G;
+		else
+			chanspec |= WL_CHANSPEC_BAND_5G;
+
+		chanspec |= WL_CHANSPEC_BW_20;
+		chanspec |= WL_CHANSPEC_CTL_SB_NONE;
+
+		*join_params_size += WL_ASSOC_PARAMS_FIXED_SIZE +
+			join_params->params.chanspec_num * sizeof(chanspec_t);
+
+		join_params->params.chanspec_list[0]  &= WL_CHANSPEC_CHAN_MASK;
+		join_params->params.chanspec_list[0] |= chanspec;
+		join_params->params.chanspec_list[0] =
+		        htodchanspec(join_params->params.chanspec_list[0]);
+
+		join_params->params.chanspec_num = htod32(join_params->params.chanspec_num);
+
+		WL_TRACE(("%s  join_params->params.chanspec_list[0]= %X\n", \
+			__FUNCTION__, join_params->params.chanspec_list[0]));
+	}
+	return 1;
+}
+
+static int
+wl_iw_set_wap(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct sockaddr *awrq,
+	char *extra
+)
+{
+	int error = -EINVAL;
+	wl_join_params_t join_params;
+	int join_params_size;
+
+	WL_TRACE(("%s: SIOCSIWAP\n", dev->name));
+
+	if (awrq->sa_family != ARPHRD_ETHER) {
+		WL_ERROR(("Invalid Header...sa_family\n"));
+		return -EINVAL;
+	}
+
+	
+	if (ETHER_ISBCAST(awrq->sa_data) || ETHER_ISNULLADDR(awrq->sa_data)) {
+		scb_val_t scbval;
+		
+		bzero(&scbval, sizeof(scb_val_t));
+		
+		(void) dev_wlc_ioctl(dev, WLC_DISASSOC, &scbval, sizeof(scb_val_t));
+		return 0;
+	}
+
+
+	
+	memset(&join_params, 0, sizeof(join_params));
+	join_params_size = sizeof(join_params.ssid);
+
+	memcpy(join_params.ssid.SSID, g_ssid.SSID, g_ssid.SSID_len);
+	join_params.ssid.SSID_len = htod32(g_ssid.SSID_len);
+	memcpy(&join_params.params.bssid, awrq->sa_data, ETHER_ADDR_LEN);
+
+	WL_ASSOC(("%s  target_channel=%d\n", __FUNCTION__, g_wl_iw_params.target_channel));
+	wl_iw_ch_to_chanspec(g_wl_iw_params.target_channel, &join_params, &join_params_size);
+
+	if ((error = dev_wlc_ioctl(dev, WLC_SET_SSID, &join_params, join_params_size))) {
+		WL_ERROR(("%s Invalid ioctl data=%d\n", __FUNCTION__, error));
+		return error;
+	}
+
+	if (g_ssid.SSID_len) {
+		WL_ASSOC(("%s: join SSID=%s BSSID="MACSTR" ch=%d\n", __FUNCTION__,  \
+			g_ssid.SSID, MAC2STR((u8 *)awrq->sa_data), \
+			g_wl_iw_params.target_channel));
+	}
+
+	
+	memset(&g_ssid, 0, sizeof(g_ssid));
+	return 0;
+}
+
+static int
+wl_iw_get_wap(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct sockaddr *awrq,
+	char *extra
+)
+{
+	WL_TRACE(("%s: SIOCGIWAP\n", dev->name));
+
+	awrq->sa_family = ARPHRD_ETHER;
+	memset(awrq->sa_data, 0, ETHER_ADDR_LEN);
+
+	
+	(void) dev_wlc_ioctl(dev, WLC_GET_BSSID, awrq->sa_data, ETHER_ADDR_LEN);
+
+	return 0;
+}
+
+#if WIRELESS_EXT > 17
+static int
+wl_iw_mlme(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct sockaddr *awrq,
+	char *extra
+)
+{
+	struct iw_mlme *mlme;
+	scb_val_t scbval;
+	int error  = -EINVAL;
+
+	WL_TRACE(("%s: SIOCSIWMLME DISASSOC/DEAUTH\n", dev->name));
+
+	mlme = (struct iw_mlme *)extra;
+	if (mlme == NULL) {
+		WL_ERROR(("Invalid ioctl data.\n"));
+		return error;
+	}
+
+	scbval.val = mlme->reason_code;
+	bcopy(&mlme->addr.sa_data, &scbval.ea, ETHER_ADDR_LEN);
+
+	if (mlme->cmd == IW_MLME_DISASSOC) {
+		scbval.val = htod32(scbval.val);
+		error = dev_wlc_ioctl(dev, WLC_DISASSOC, &scbval, sizeof(scb_val_t));
+	}
+	else if (mlme->cmd == IW_MLME_DEAUTH) {
+		scbval.val = htod32(scbval.val);
+		error = dev_wlc_ioctl(dev, WLC_SCB_DEAUTHENTICATE_FOR_REASON, &scbval,
+			sizeof(scb_val_t));
+	}
+	else {
+		WL_ERROR(("Invalid ioctl data.\n"));
+		return error;
+	}
+
+	return error;
+}
+#endif 
+
+#ifndef WL_IW_USE_ISCAN
+static int
+wl_iw_get_aplist(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_scan_results_t *list;
+	struct sockaddr *addr = (struct sockaddr *) extra;
+	struct iw_quality qual[IW_MAX_AP];
+	wl_bss_info_t *bi = NULL;
+	int error, i;
+	uint buflen = dwrq->length;
+
+	WL_TRACE(("%s: SIOCGIWAPLIST\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	list = kmalloc(buflen, GFP_KERNEL);
+	if (!list)
+		return -ENOMEM;
+	memset(list, 0, buflen);
+	list->buflen = htod32(buflen);
+	if ((error = dev_wlc_ioctl(dev, WLC_SCAN_RESULTS, list, buflen))) {
+		WL_ERROR(("%d: Scan results error %d\n", __LINE__, error));
+		kfree(list);
+		return error;
+	}
+	list->buflen = dtoh32(list->buflen);
+	list->version = dtoh32(list->version);
+	list->count = dtoh32(list->count);
+	if (list->version != WL_BSS_INFO_VERSION) {
+		WL_ERROR(("%s: list->version %d != WL_BSS_INFO_VERSION\n", \
+			 __FUNCTION__, list->version));
+		kfree(list);
+		return -EINVAL;
+	}
+
+	for (i = 0, dwrq->length = 0; i < list->count && dwrq->length < IW_MAX_AP; i++) {
+		bi = bi ? (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length)) : list->bss_info;
+
+		if ((dtoh32(bi->length) > buflen) ||
+		    (((uintptr)bi + dtoh32(bi->length)) > ((uintptr)list + buflen))) {
+			WL_ERROR(("%s: Scan results out of bounds: %u\n",__FUNCTION__,dtoh32(bi->length)));
+			kfree(list);
+			return -E2BIG;
+		}
+
+		if (!(dtoh16(bi->capability) & DOT11_CAP_ESS))
+			continue;
+
+		memcpy(addr[dwrq->length].sa_data, &bi->BSSID, ETHER_ADDR_LEN);
+		addr[dwrq->length].sa_family = ARPHRD_ETHER;
+		qual[dwrq->length].qual = rssi_to_qual(dtoh16(bi->RSSI));
+		qual[dwrq->length].level = 0x100 + dtoh16(bi->RSSI);
+		qual[dwrq->length].noise = 0x100 + bi->phy_noise;
+
+#if WIRELESS_EXT > 18
+		qual[dwrq->length].updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
+#else
+		qual[dwrq->length].updated = 7;
+#endif 
+
+		dwrq->length++;
+	}
+
+	kfree(list);
+
+	if (dwrq->length) {
+		memcpy(&addr[dwrq->length], qual, sizeof(struct iw_quality) * dwrq->length);
+		
+		dwrq->flags = 1;
+	}
+	return 0;
+}
+#endif
+
+#ifdef WL_IW_USE_ISCAN
+static int
+wl_iw_iscan_get_aplist(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_scan_results_t *list;
+	iscan_buf_t * buf;
+	iscan_info_t *iscan = g_iscan;
+
+	struct sockaddr *addr = (struct sockaddr *) extra;
+	struct iw_quality qual[IW_MAX_AP];
+	wl_bss_info_t *bi = NULL;
+	int i;
+
+	WL_TRACE(("%s: SIOCGIWAPLIST\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	if ((!iscan) || (iscan->sysioc_pid < 0)) {
+		WL_ERROR(("%s error\n", __FUNCTION__));
+		return 0;
+	}
+
+	buf = iscan->list_hdr;
+	
+	while (buf) {
+		list = &((wl_iscan_results_t*)buf->iscan_buf)->results;
+		if (list->version != WL_BSS_INFO_VERSION) {
+			WL_ERROR(("%s : list->version %d != WL_BSS_INFO_VERSION\n", \
+				__FUNCTION__, list->version));
+			return -EINVAL;
+		}
+
+		bi = NULL;
+		for (i = 0, dwrq->length = 0; i < list->count && dwrq->length < IW_MAX_AP; i++) {
+			bi = bi ? (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length))
+			          : list->bss_info;
+
+			if ((dtoh32(bi->length) > WLC_IW_ISCAN_MAXLEN) ||
+			    (((uintptr)bi + dtoh32(bi->length)) > ((uintptr)list + WLC_IW_ISCAN_MAXLEN))) {
+				WL_ERROR(("%s: Scan results out of bounds: %u\n",__FUNCTION__,dtoh32(bi->length)));
+				return -E2BIG;
+			}
+
+			if (!(dtoh16(bi->capability) & DOT11_CAP_ESS))
+				continue;
+
+			memcpy(addr[dwrq->length].sa_data, &bi->BSSID, ETHER_ADDR_LEN);
+			addr[dwrq->length].sa_family = ARPHRD_ETHER;
+			qual[dwrq->length].qual = rssi_to_qual(dtoh16(bi->RSSI));
+			qual[dwrq->length].level = 0x100 + dtoh16(bi->RSSI);
+			qual[dwrq->length].noise = 0x100 + bi->phy_noise;
+
+#if WIRELESS_EXT > 18
+			qual[dwrq->length].updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
+#else
+			qual[dwrq->length].updated = 7;
+#endif 
+
+			dwrq->length++;
+		}
+		buf = buf->next;
+	}
+	if (dwrq->length) {
+		memcpy(&addr[dwrq->length], qual, sizeof(struct iw_quality) * dwrq->length);
+		
+		dwrq->flags = 1;
+	}
+	return 0;
+}
+
+static int
+wl_iw_iscan_prep(wl_scan_params_t *params, wlc_ssid_t *ssid)
+{
+	int err = 0;
+
+	memcpy(&params->bssid, &ether_bcast, ETHER_ADDR_LEN);
+	params->bss_type = DOT11_BSSTYPE_ANY;
+	params->scan_type = 0;
+	params->nprobes = -1;
+	params->active_time = -1;
+	params->passive_time = -1;
+	params->home_time = -1;
+	params->channel_num = 0;
+#if defined(CONFIG_FIRST_SCAN)
+	if (g_first_broadcast_scan == BROADCAST_SCAN_FIRST_STARTED)
+		params->passive_time = 30;
+#endif
+	params->nprobes = htod32(params->nprobes);
+	params->active_time = htod32(params->active_time);
+	params->passive_time = htod32(params->passive_time);
+	params->home_time = htod32(params->home_time);
+	if (ssid && ssid->SSID_len)
+		memcpy(&params->ssid, ssid, sizeof(wlc_ssid_t));
+
+	return err;
+}
+
+static int
+wl_iw_iscan(iscan_info_t *iscan, wlc_ssid_t *ssid, uint16 action)
+{
+	int err = 0;
+
+	iscan->iscan_ex_params_p->version = htod32(ISCAN_REQ_VERSION);
+	iscan->iscan_ex_params_p->action = htod16(action);
+	iscan->iscan_ex_params_p->scan_duration = htod16(0);
+
+	WL_SCAN(("%s : nprobes=%d\n", __FUNCTION__, iscan->iscan_ex_params_p->params.nprobes));
+	WL_SCAN(("active_time=%d\n", iscan->iscan_ex_params_p->params.active_time));
+	WL_SCAN(("passive_time=%d\n", iscan->iscan_ex_params_p->params.passive_time));
+	WL_SCAN(("home_time=%d\n", iscan->iscan_ex_params_p->params.home_time));
+	WL_SCAN(("scan_type=%d\n", iscan->iscan_ex_params_p->params.scan_type));
+	WL_SCAN(("bss_type=%d\n", iscan->iscan_ex_params_p->params.bss_type));
+
+	if ((err = dev_iw_iovar_setbuf(iscan->dev, "iscan", iscan->iscan_ex_params_p, \
+		iscan->iscan_ex_param_size, iscan->ioctlbuf, sizeof(iscan->ioctlbuf)))) {
+			WL_ERROR(("Set ISCAN for %s failed with %d\n", __FUNCTION__, err));
+			err = -1;
+	}
+
+	return err;
+}
+
+static void
+wl_iw_timerfunc(ulong data)
+{
+	iscan_info_t *iscan = (iscan_info_t *)data;
+	if (iscan) {
+		iscan->timer_on = 0;
+		if (iscan->iscan_state != ISCAN_STATE_IDLE) {
+			WL_SCAN(("timer trigger\n"));
+			up(&iscan->sysioc_sem);
+		}
+	}
+}
+static void wl_iw_set_event_mask(struct net_device *dev)
+{
+	char eventmask[WL_EVENTING_MASK_LEN];
+	char iovbuf[WL_EVENTING_MASK_LEN + 12];	
+
+	dev_iw_iovar_getbuf(dev, "event_msgs", "", 0, iovbuf, sizeof(iovbuf));
+	bcopy(iovbuf, eventmask, WL_EVENTING_MASK_LEN);
+	setbit(eventmask, WLC_E_SCAN_COMPLETE);
+	dev_iw_iovar_setbuf(dev, "event_msgs", eventmask, WL_EVENTING_MASK_LEN,
+		iovbuf, sizeof(iovbuf));
+}
+
+static uint32
+wl_iw_iscan_get(iscan_info_t *iscan)
+{
+	iscan_buf_t * buf;
+	iscan_buf_t * ptr;
+	wl_iscan_results_t * list_buf;
+	wl_iscan_results_t list;
+	wl_scan_results_t *results;
+	uint32 status;
+	int res;
+
+	mutex_lock(&wl_cache_lock);
+	if (iscan->list_cur) {
+		buf = iscan->list_cur;
+		iscan->list_cur = buf->next;
+	}
+	else {
+		buf = kmalloc(sizeof(iscan_buf_t), GFP_KERNEL);
+		if (!buf) {
+			WL_ERROR(("%s can't alloc iscan_buf_t : going to abort currect iscan\n", \
+						__FUNCTION__));
+			mutex_unlock(&wl_cache_lock);
+			return WL_SCAN_RESULTS_NO_MEM;
+		}
+		buf->next = NULL;
+		if (!iscan->list_hdr)
+			iscan->list_hdr = buf;
+		else {
+			ptr = iscan->list_hdr;
+			while (ptr->next) {
+				ptr = ptr->next;
+			}
+			ptr->next = buf;
+		}
+	}
+	memset(buf->iscan_buf, 0, WLC_IW_ISCAN_MAXLEN);
+	list_buf = (wl_iscan_results_t*)buf->iscan_buf;
+	results = &list_buf->results;
+	results->buflen = WL_ISCAN_RESULTS_FIXED_SIZE;
+	results->version = 0;
+	results->count = 0;
+
+	memset(&list, 0, sizeof(list));
+	list.results.buflen = htod32(WLC_IW_ISCAN_MAXLEN);
+	res = dev_iw_iovar_getbuf(
+		iscan->dev,
+		"iscanresults",
+		&list,
+		WL_ISCAN_RESULTS_FIXED_SIZE,
+		buf->iscan_buf,
+		WLC_IW_ISCAN_MAXLEN);
+	if (res == 0) {
+		results->buflen = dtoh32(results->buflen);
+		results->version = dtoh32(results->version);
+		results->count = dtoh32(results->count);
+		WL_SCAN(("results->count = %d\n", results->count));
+
+		WL_SCAN(("results->buflen = %d\n", results->buflen));
+		status = dtoh32(list_buf->status);
+	} else {
+		WL_ERROR(("%s returns error %d\n", __FUNCTION__, res));
+		status = WL_SCAN_RESULTS_NO_MEM;
+	}
+	mutex_unlock(&wl_cache_lock);
+	return status;
+}
+
+static void wl_iw_force_specific_scan(iscan_info_t *iscan)
+{
+	WL_SCAN(("%s force Specific SCAN for %s\n", __FUNCTION__, g_specific_ssid.SSID));
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	rtnl_lock();
+#endif
+	(void) dev_wlc_ioctl(iscan->dev, WLC_SCAN, &g_specific_ssid, sizeof(g_specific_ssid));
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	rtnl_unlock();
+#endif
+}
+
+static void wl_iw_send_scan_complete(iscan_info_t *iscan)
+{
+#ifndef SANDGATE2G
+	union iwreq_data wrqu;
+
+	memset(&wrqu, 0, sizeof(wrqu));
+
+	wireless_send_event(iscan->dev, SIOCGIWSCAN, &wrqu, NULL);
+#if defined(CONFIG_FIRST_SCAN)
+	if (g_first_broadcast_scan == BROADCAST_SCAN_FIRST_STARTED)
+		g_first_broadcast_scan = BROADCAST_SCAN_FIRST_RESULT_READY;
+#endif
+	WL_SCAN(("Send Event ISCAN complete\n"));
+#endif 
+}
+
+static int
+_iscan_sysioc_thread(void *data)
+{
+	uint32 status;
+	iscan_info_t *iscan = (iscan_info_t *)data;
+	static bool iscan_pass_abort = FALSE;
+
+	DAEMONIZE("iscan_sysioc");
+
+	status = WL_SCAN_RESULTS_PARTIAL;
+	while (down_interruptible(&iscan->sysioc_sem) == 0) {
+
+		net_os_wake_lock(iscan->dev);
+
+#if defined(SOFTAP)
+		if (ap_cfg_running) {
+			WL_SCAN(("%s skipping SCAN ops in AP mode !!!\n", __FUNCTION__));
+			net_os_wake_unlock(iscan->dev);
+			continue;
+		}
+#endif
+
+		if (iscan->timer_on) {
+			iscan->timer_on = 0;
+			del_timer_sync(&iscan->timer);
+		}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+		rtnl_lock();
+#endif
+		status = wl_iw_iscan_get(iscan);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+		rtnl_unlock();
+#endif
+
+		if (g_scan_specified_ssid && (iscan_pass_abort == TRUE)) {
+			WL_SCAN(("%s Get results from specific scan status=%d\n", __FUNCTION__, status));
+			wl_iw_send_scan_complete(iscan);
+			iscan_pass_abort = FALSE;
+			status  = -1;
+		}
+
+		switch (status) {
+			case WL_SCAN_RESULTS_PARTIAL:
+				WL_SCAN(("iscanresults incomplete\n"));
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+				rtnl_lock();
+#endif
+
+				wl_iw_iscan(iscan, NULL, WL_SCAN_ACTION_CONTINUE);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+				rtnl_unlock();
+#endif
+
+				mod_timer(&iscan->timer, jiffies + iscan->timer_ms*HZ/1000);
+				iscan->timer_on = 1;
+				break;
+			case WL_SCAN_RESULTS_SUCCESS:
+				WL_SCAN(("iscanresults complete\n"));
+				iscan->iscan_state = ISCAN_STATE_IDLE;
+				wl_iw_send_scan_complete(iscan);
+				break;
+			case WL_SCAN_RESULTS_PENDING:
+				WL_SCAN(("iscanresults pending\n"));
+
+				mod_timer(&iscan->timer, jiffies + iscan->timer_ms*HZ/1000);
+				iscan->timer_on = 1;
+				break;
+			case WL_SCAN_RESULTS_ABORTED:
+				WL_SCAN(("iscanresults aborted\n"));
+				iscan->iscan_state = ISCAN_STATE_IDLE;
+				if (g_scan_specified_ssid == 0)
+					wl_iw_send_scan_complete(iscan);
+				else {
+					iscan_pass_abort = TRUE;
+					wl_iw_force_specific_scan(iscan);
+				}
+				break;
+			case WL_SCAN_RESULTS_NO_MEM:
+				WL_SCAN(("iscanresults can't alloc memory: skip\n"));
+				iscan->iscan_state = ISCAN_STATE_IDLE;
+				break;
+			default:
+				WL_SCAN(("iscanresults returned unknown status %d\n", status));
+				break;
+		}
+
+		net_os_wake_unlock(iscan->dev);
+	}
+
+	if (iscan->timer_on) {
+		iscan->timer_on = 0;
+		del_timer_sync(&iscan->timer);
+	}
+
+	complete_and_exit(&iscan->sysioc_exited, 0);
+}
+#endif 
+
+#if !defined(CSCAN)
+
+static void
+wl_iw_set_ss_cache_timer_flag(void)
+{
+	g_ss_cache_ctrl.m_timer_expired = 1;
+	WL_TRACE(("%s called\n", __FUNCTION__));
+}
+
+static int
+wl_iw_init_ss_cache_ctrl(void)
+{
+	WL_TRACE(("%s :\n", __FUNCTION__));
+	g_ss_cache_ctrl.m_prev_scan_mode = 0;
+	g_ss_cache_ctrl.m_cons_br_scan_cnt = 0;
+	g_ss_cache_ctrl.m_cache_head = NULL;
+	g_ss_cache_ctrl.m_link_down = 0;
+	g_ss_cache_ctrl.m_timer_expired = 0;
+	memset(g_ss_cache_ctrl.m_active_bssid, 0, ETHER_ADDR_LEN);
+
+	g_ss_cache_ctrl.m_timer = kmalloc(sizeof(struct timer_list), GFP_KERNEL);
+	if (!g_ss_cache_ctrl.m_timer) {
+		return -ENOMEM;
+	}
+	g_ss_cache_ctrl.m_timer->function = (void *)wl_iw_set_ss_cache_timer_flag;
+	init_timer(g_ss_cache_ctrl.m_timer);
+
+	return 0;
+}
+
+
+
+static void
+wl_iw_free_ss_cache(void)
+{
+	wl_iw_ss_cache_t *node, *cur;
+	wl_iw_ss_cache_t **spec_scan_head;
+
+	WL_TRACE(("%s called\n", __FUNCTION__));
+
+	mutex_lock(&wl_cache_lock);
+	spec_scan_head = &g_ss_cache_ctrl.m_cache_head;
+	node = *spec_scan_head;
+
+	for (;node;) {
+		WL_TRACE(("%s : SSID - %s\n", __FUNCTION__, node->bss_info->SSID));
+		cur = node;
+		node = cur->next;
+		kfree(cur);
+	}
+	*spec_scan_head = NULL;
+	mutex_unlock(&wl_cache_lock);
+}
+
+
+
+static int
+wl_iw_run_ss_cache_timer(int kick_off)
+{
+	struct timer_list **timer;
+
+	timer = &g_ss_cache_ctrl.m_timer;
+
+	if (*timer) {
+		if (kick_off) {
+			(*timer)->expires = jiffies + 30000 * HZ / 1000;	
+			add_timer(*timer);
+			WL_TRACE(("%s : timer starts \n", __FUNCTION__));
+		} else {
+			del_timer_sync(*timer);
+			WL_TRACE(("%s : timer stops \n", __FUNCTION__));
+		}
+	}
+
+	return 0;
+}
+
+
+void
+wl_iw_release_ss_cache_ctrl(void)
+{
+	WL_TRACE(("%s :\n", __FUNCTION__));
+	wl_iw_free_ss_cache();
+	wl_iw_run_ss_cache_timer(0);
+	if (g_ss_cache_ctrl.m_timer) {
+		kfree(g_ss_cache_ctrl.m_timer);
+	}
+}
+
+
+
+static void
+wl_iw_reset_ss_cache(void)
+{
+	wl_iw_ss_cache_t *node, *prev, *cur;
+	wl_iw_ss_cache_t **spec_scan_head;
+
+	mutex_lock(&wl_cache_lock);
+	spec_scan_head = &g_ss_cache_ctrl.m_cache_head;
+	node = *spec_scan_head;
+	prev = node;
+
+	for (;node;) {
+		WL_TRACE(("%s : node SSID %s \n", __FUNCTION__, node->bss_info->SSID));
+		if (!node->dirty) {
+			cur = node;
+			if (cur == *spec_scan_head) {
+				*spec_scan_head = cur->next;
+				prev = *spec_scan_head;
+			}
+			else {
+				prev->next = cur->next;
+			}
+			node = cur->next;
+
+			WL_TRACE(("%s : Del node : SSID %s\n", __FUNCTION__, cur->bss_info->SSID));
+			kfree(cur);
+			continue;
+		}
+
+		node->dirty = 0;
+		prev = node;
+		node = node->next;
+	}
+	mutex_unlock(&wl_cache_lock);
+}
+
+
+static int
+wl_iw_add_bss_to_ss_cache(wl_scan_results_t *ss_list)
+{
+
+	wl_iw_ss_cache_t *node, *prev, *leaf;
+	wl_iw_ss_cache_t **spec_scan_head;
+	wl_bss_info_t *bi = NULL;
+	int i;
+
+	if (!ss_list->count) {
+		return 0;
+	}
+
+	mutex_lock(&wl_cache_lock);
+	spec_scan_head = &g_ss_cache_ctrl.m_cache_head;
+
+	for (i = 0; i < ss_list->count; i++) {
+
+		node = *spec_scan_head;
+		prev = node;
+
+		bi = bi ? (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length)) : ss_list->bss_info;
+
+		WL_TRACE(("%s : find %d with specific SSID %s\n", __FUNCTION__, i, bi->SSID));
+		for (;node;) {
+			if (!memcmp(&node->bss_info->BSSID, &bi->BSSID, ETHER_ADDR_LEN)) {
+				
+				WL_TRACE(("dirty marked : SSID %s\n", bi->SSID));
+				node->dirty = 1;
+				break;
+			}
+			prev = node;
+			node = node->next;
+		}
+
+		if (node) {
+			continue;
+		}
+		leaf = kmalloc(bi->length + WLC_IW_SS_CACHE_CTRL_FIELD_MAXLEN, GFP_KERNEL);
+		if (!leaf) {
+			WL_ERROR(("Memory alloc failure %d\n", \
+				bi->length + WLC_IW_SS_CACHE_CTRL_FIELD_MAXLEN));
+			mutex_unlock(&wl_cache_lock);
+			return -ENOMEM;
+		}
+
+		memcpy(leaf->bss_info, bi, bi->length);
+		leaf->next = NULL;
+		leaf->dirty = 1;
+		leaf->count = 1;
+		leaf->version = ss_list->version;
+
+		if (!prev) {
+			*spec_scan_head = leaf;
+		}
+		else {
+			prev->next = leaf;
+		}
+	}
+	mutex_unlock(&wl_cache_lock);
+	return 0;
+}
+
+
+static int
+wl_iw_merge_scan_cache(struct iw_request_info *info, char *extra, uint buflen_from_user,
+__u16 *merged_len)
+{
+	wl_iw_ss_cache_t *node;
+	wl_scan_results_t *list_merge;
+
+	mutex_lock(&wl_cache_lock);
+	node = g_ss_cache_ctrl.m_cache_head;
+	for (;node;) {
+		list_merge = (wl_scan_results_t *)&node->buflen;
+		WL_TRACE(("%s: Cached Specific APs list=%d\n", __FUNCTION__, list_merge->count));
+		if (buflen_from_user - *merged_len > 0) {
+			*merged_len += (__u16) wl_iw_get_scan_prep(list_merge, info,
+				extra + *merged_len, buflen_from_user - *merged_len);
+		}
+		else {
+			WL_TRACE(("%s: exit with break\n", __FUNCTION__));
+			break;
+		}
+		node = node->next;
+	}
+	mutex_unlock(&wl_cache_lock);
+	return 0;
+}
+
+
+static int
+wl_iw_delete_bss_from_ss_cache(void *addr)
+{
+
+	wl_iw_ss_cache_t *node, *prev;
+	wl_iw_ss_cache_t **spec_scan_head;
+
+	mutex_lock(&wl_cache_lock);
+	spec_scan_head = &g_ss_cache_ctrl.m_cache_head;
+	node = *spec_scan_head;
+	prev = node;
+	for (;node;) {
+		if (!memcmp(&node->bss_info->BSSID, addr, ETHER_ADDR_LEN)) {
+			if (node == *spec_scan_head) {
+				*spec_scan_head = node->next;
+			}
+			else {
+				prev->next = node->next;
+			}
+
+			WL_TRACE(("%s : Del node : %s\n", __FUNCTION__, node->bss_info->SSID));
+			kfree(node);
+			break;
+		}
+
+		prev = node;
+		node = node->next;
+	}
+
+	memset(addr, 0, ETHER_ADDR_LEN);
+	mutex_unlock(&wl_cache_lock);
+	return 0;
+}
+
+#endif
+
+
+static int
+wl_iw_set_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error;
+	WL_TRACE(("%s dev:%s: SIOCSIWSCAN : SCAN\n", __FUNCTION__, dev->name));
+
+#if defined(CSCAN)
+	WL_ERROR(("%s: Scan from SIOCGIWSCAN not supported\n", __FUNCTION__));
+	return -EINVAL;
+#endif 
+
+#if defined(SOFTAP)
+	if (ap_cfg_running) {
+		WL_TRACE(("\n>%s: Not executed, reason -'SOFTAP is active'\n", __FUNCTION__));
+		return 0;
+	}
+#endif
+
+	if (g_onoff == G_WLAN_SET_OFF)
+		return 0;
+
+	memset(&g_specific_ssid, 0, sizeof(g_specific_ssid));
+#ifndef WL_IW_USE_ISCAN
+	g_scan_specified_ssid = 0;
+#endif 
+
+#if WIRELESS_EXT > 17
+	
+	if (wrqu->data.length == sizeof(struct iw_scan_req)) {
+		if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
+			struct iw_scan_req *req = (struct iw_scan_req *)extra;
+#if defined(CONFIG_FIRST_SCAN)
+			if (g_first_broadcast_scan != BROADCAST_SCAN_FIRST_RESULT_CONSUMED) {
+				WL_ERROR(("%s Ignoring SC %s first BC is not done = %d\n", \
+						__FUNCTION__, req->essid, \
+						g_first_broadcast_scan));
+				return -EBUSY;
+			}
+#endif
+			if (g_scan_specified_ssid) {
+				WL_SCAN(("%s Specific SCAN is not done ignore scan for = %s \n", \
+					__FUNCTION__, req->essid));
+				return -EBUSY;
+			}
+			else {
+				g_specific_ssid.SSID_len = MIN(sizeof(g_specific_ssid.SSID), \
+										req->essid_len);
+				memcpy(g_specific_ssid.SSID, req->essid, g_specific_ssid.SSID_len);
+				g_specific_ssid.SSID_len = htod32(g_specific_ssid.SSID_len);
+				g_scan_specified_ssid = 1;
+				WL_TRACE(("### Specific scan ssid=%s len=%d\n", \
+						g_specific_ssid.SSID, g_specific_ssid.SSID_len));
+			}
+		}
+	}
+#endif 
+	
+	if ((error = dev_wlc_ioctl(dev, WLC_SCAN, &g_specific_ssid, sizeof(g_specific_ssid)))) {
+		WL_SCAN(("Set SCAN for %s failed with %d\n", g_specific_ssid.SSID, error));
+		g_scan_specified_ssid = 0;
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
+#ifdef WL_IW_USE_ISCAN
+int
+wl_iw_iscan_set_scan_broadcast_prep(struct net_device *dev, uint flag)
+{
+	wlc_ssid_t ssid;
+	iscan_info_t *iscan = g_iscan;
+
+#if defined(CONFIG_FIRST_SCAN)
+	if (g_first_broadcast_scan == BROADCAST_SCAN_FIRST_IDLE) {
+		g_first_broadcast_scan = BROADCAST_SCAN_FIRST_STARTED;
+		WL_SCAN(("%s: First Brodcast scan was forced\n", __FUNCTION__));
+	}
+	else if (g_first_broadcast_scan == BROADCAST_SCAN_FIRST_STARTED) {
+		WL_SCAN(("%s: ignore ISCAN request first BS is not done yet\n", __FUNCTION__));
+		return 0;
+	}
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	if (flag)
+		rtnl_lock();
+#endif
+
+	dev_wlc_ioctl(dev, WLC_SET_PASSIVE_SCAN, &iscan->scan_flag, sizeof(iscan->scan_flag));
+	wl_iw_set_event_mask(dev);
+
+	WL_SCAN(("+++: Set Broadcast ISCAN\n"));
+	
+	memset(&ssid, 0, sizeof(ssid));
+
+	iscan->list_cur = iscan->list_hdr;
+	iscan->iscan_state = ISCAN_STATE_SCANING;
+
+	memset(&iscan->iscan_ex_params_p->params, 0, iscan->iscan_ex_param_size);
+	wl_iw_iscan_prep(&iscan->iscan_ex_params_p->params, &ssid);
+	wl_iw_iscan(iscan, &ssid, WL_SCAN_ACTION_START);
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	if (flag)
+		rtnl_unlock();
+#endif
+
+	mod_timer(&iscan->timer, jiffies + iscan->timer_ms*HZ/1000);
+
+	iscan->timer_on = 1;
+
+	return 0;
+}
+
+static int
+wl_iw_iscan_set_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	wlc_ssid_t ssid;
+	iscan_info_t *iscan = g_iscan;
+	int ret = 0;
+
+	WL_SCAN(("%s: SIOCSIWSCAN : ISCAN\n", dev->name));
+
+#if defined(CSCAN)
+	WL_ERROR(("%s: Scan from SIOCGIWSCAN not supported\n", __FUNCTION__));
+	return -EINVAL;
+#endif
+
+	net_os_wake_lock(dev);
+
+#if defined(SOFTAP)
+	if (ap_cfg_running) {
+		WL_SCAN(("\n>%s: Not executed, reason -'SOFTAP is active'\n", __FUNCTION__));
+		goto set_scan_end;
+	}
+#endif
+
+	if (g_onoff == G_WLAN_SET_OFF) {
+		WL_SCAN(("%s: driver is not up yet after START\n", __FUNCTION__));
+		goto set_scan_end;
+	}
+
+#ifdef PNO_SUPPORT
+	if  (dhd_dev_get_pno_status(dev)) {
+		WL_SCAN(("%s: Scan called when PNO is active\n", __FUNCTION__));
+	}
+#endif
+
+	if ((!iscan) || (iscan->sysioc_pid < 0)) {
+		WL_ERROR(("%s error\n", __FUNCTION__));
+		goto set_scan_end;
+	}
+
+	if (g_scan_specified_ssid) {
+		WL_SCAN(("%s Specific SCAN already running ignoring BC scan\n", \
+				__FUNCTION__));
+		ret = EBUSY;
+		goto set_scan_end;
+	}
+
+	memset(&ssid, 0, sizeof(ssid));
+
+#if WIRELESS_EXT > 17
+	
+	if (wrqu->data.length == sizeof(struct iw_scan_req)) {
+		if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
+			int as = 0;
+			struct iw_scan_req *req = (struct iw_scan_req *)extra;
+			ssid.SSID_len = MIN(sizeof(ssid.SSID), req->essid_len);
+			memcpy(ssid.SSID, req->essid, ssid.SSID_len);
+			ssid.SSID_len = htod32(ssid.SSID_len);
+			dev_wlc_ioctl(dev, WLC_SET_PASSIVE_SCAN, &as, sizeof(as));
+			wl_iw_set_event_mask(dev);
+			ret = wl_iw_set_scan(dev, info, wrqu, extra);
+			goto set_scan_end;
+		}
+		else {
+			g_scan_specified_ssid = 0;
+
+			if (iscan->iscan_state == ISCAN_STATE_SCANING) {
+				WL_SCAN(("%s ISCAN already in progress \n", __FUNCTION__));
+				goto set_scan_end;
+			}
+		}
+	}
+#endif 
+
+#if defined(CONFIG_FIRST_SCAN) && !defined(CSCAN)
+	if (g_first_broadcast_scan < BROADCAST_SCAN_FIRST_RESULT_CONSUMED) {
+		if (++g_first_counter_scans == MAX_ALLOWED_BLOCK_SCAN_FROM_FIRST_SCAN) {
+
+			WL_ERROR(("%s Clean up First scan flag which is %d\n", \
+				 __FUNCTION__, g_first_broadcast_scan));
+			g_first_broadcast_scan = BROADCAST_SCAN_FIRST_RESULT_CONSUMED;
+		}
+		else {
+			WL_ERROR(("%s Ignoring Broadcast Scan:First Scan is not done yet %d\n", \
+					__FUNCTION__, g_first_counter_scans));
+			ret = -EBUSY;
+			goto set_scan_end;
+		}
+	}
+#endif
+
+	wl_iw_iscan_set_scan_broadcast_prep(dev, 0);
+
+set_scan_end:
+	net_os_wake_unlock(dev);
+	return ret;
+}
+#endif 
+
+#if WIRELESS_EXT > 17
+static bool
+ie_is_wpa_ie(uint8 **wpaie, uint8 **tlvs, int *tlvs_len)
+{
+	uint8 *ie = *wpaie;
+
+	if ((ie[1] >= 6) &&
+		!bcmp((const void *)&ie[2], (const void *)(WPA_OUI "\x01"), 4)) {
+		return TRUE;
+	}
+
+	ie += ie[1] + 2;
+	
+	*tlvs_len -= (int)(ie - *tlvs);
+	
+	*tlvs = ie;
+	return FALSE;
+}
+
+static bool
+ie_is_wps_ie(uint8 **wpsie, uint8 **tlvs, int *tlvs_len)
+{
+	uint8 *ie = *wpsie;
+
+	if ((ie[1] >= 4) &&
+		!bcmp((const void *)&ie[2], (const void *)(WPA_OUI "\x04"), 4)) {
+		return TRUE;
+	}
+
+	ie += ie[1] + 2;
+	
+	*tlvs_len -= (int)(ie - *tlvs);
+	
+	*tlvs = ie;
+	return FALSE;
+}
+#endif 
+
+static inline int _wpa_snprintf_hex(char *buf, size_t buf_size, const u8 *data,
+	size_t len, int uppercase)
+{
+	size_t i;
+	char *pos = buf, *end = buf + buf_size;
+	int ret;
+	if (buf_size == 0)
+		return 0;
+	for (i = 0; i < len; i++) {
+		ret = snprintf(pos, end - pos, uppercase ? "%02X" : "%02x",
+			data[i]);
+		if (ret < 0 || ret >= end - pos) {
+			end[-1] = '\0';
+			return pos - buf;
+		}
+		pos += ret;
+	}
+	end[-1] = '\0';
+	return pos - buf;
+}
+
+
+int wpa_snprintf_hex(char *buf, size_t buf_size, const u8 *data, size_t len)
+{
+	return _wpa_snprintf_hex(buf, buf_size, data, len, 0);
+}
+
+static int
+wl_iw_handle_scanresults_ies(char **event_p, char *end,
+	struct iw_request_info *info, wl_bss_info_t *bi)
+{
+#if WIRELESS_EXT > 17
+	struct iw_event	iwe;
+	char *event;
+	char *buf;
+	int custom_event_len;
+
+	event = *event_p;
+	if (bi->ie_length) {
+		
+		bcm_tlv_t *ie;
+		uint8 *ptr = ((uint8 *)bi) + sizeof(wl_bss_info_t);
+		int ptr_len = bi->ie_length;
+
+#ifdef BCMWPA2
+		if ((ie = bcm_parse_tlvs(ptr, ptr_len, DOT11_MNG_RSN_ID))) {
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = ie->len + 2;
+			event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)ie);
+		}
+		ptr = ((uint8 *)bi) + sizeof(wl_bss_info_t);
+#endif 
+
+		while ((ie = bcm_parse_tlvs(ptr, ptr_len, DOT11_MNG_WPA_ID))) {
+			
+			if (ie_is_wps_ie(((uint8 **)&ie), &ptr, &ptr_len)) {
+				iwe.cmd = IWEVGENIE;
+				iwe.u.data.length = ie->len + 2;
+				event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)ie);
+				break;
+			}
+		}
+
+		ptr = ((uint8 *)bi) + sizeof(wl_bss_info_t);
+		ptr_len = bi->ie_length;
+		while ((ie = bcm_parse_tlvs(ptr, ptr_len, DOT11_MNG_WPA_ID))) {
+			if (ie_is_wpa_ie(((uint8 **)&ie), &ptr, &ptr_len)) {
+				iwe.cmd = IWEVGENIE;
+				iwe.u.data.length = ie->len + 2;
+				event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)ie);
+				break;
+			}
+		}
+
+		ptr = ((uint8 *)bi) + sizeof(wl_bss_info_t);
+		ptr_len = bi->ie_length;
+
+		while ((ie = bcm_parse_tlvs(ptr, ptr_len, DOT11_MNG_WAPI_ID))) {
+			WL_TRACE(("%s: found a WAPI IE...\n", __FUNCTION__));
+#ifdef WAPI_IE_USE_GENIE
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = ie->len + 2;
+			event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)ie);
+#else 
+			iwe.cmd = IWEVCUSTOM;
+			custom_event_len = strlen("wapi_ie=") + 2*(ie->len + 2);
+			iwe.u.data.length = custom_event_len;
+
+			buf = kmalloc(custom_event_len+1, GFP_KERNEL);
+			if (buf == NULL)
+			{
+				WL_ERROR(("malloc(%d) returned NULL...\n", custom_event_len));
+				break;
+			}
+
+			memcpy(buf, "wapi_ie=", 8);
+			wpa_snprintf_hex(buf + 8, 2+1, &(ie->id), 1);
+			wpa_snprintf_hex(buf + 10, 2+1, &(ie->len), 1);
+			wpa_snprintf_hex(buf + 12, 2*ie->len+1, ie->data, ie->len);
+			event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, buf);
+			kfree(buf);
+#endif 
+			break;
+		}
+	*event_p = event;
+	}
+#endif 
+
+	return 0;
+}
+
+#ifndef CSCAN
+static uint
+wl_iw_get_scan_prep(
+	wl_scan_results_t *list,
+	struct iw_request_info *info,
+	char *extra,
+	short max_size)
+{
+	int  i, j;
+	struct iw_event  iwe;
+	wl_bss_info_t *bi = NULL;
+	char *event = extra, *end = extra + max_size - WE_ADD_EVENT_FIX, *value;
+	int	ret = 0;
+	int channel;
+
+	if (!list) {
+		WL_ERROR(("%s: Null list pointer",__FUNCTION__));
+		return ret;
+	}
+
+	for (i = 0; i < list->count && i < IW_MAX_AP; i++)
+	{
+		if (list->version != WL_BSS_INFO_VERSION) {
+			WL_ERROR(("%s : list->version %d != WL_BSS_INFO_VERSION\n", \
+				__FUNCTION__, list->version));
+			return ret;
+		}
+
+		bi = bi ? (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length)) : list->bss_info;
+
+		WL_TRACE(("%s : %s\n", __FUNCTION__, bi->SSID));
+
+		iwe.cmd = SIOCGIWAP;
+		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+		memcpy(iwe.u.ap_addr.sa_data, &bi->BSSID, ETHER_ADDR_LEN);
+		event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_ADDR_LEN);
+		
+		iwe.u.data.length = dtoh32(bi->SSID_len);
+		iwe.cmd = SIOCGIWESSID;
+		iwe.u.data.flags = 1;
+		event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, bi->SSID);
+
+		if (dtoh16(bi->capability) & (DOT11_CAP_ESS | DOT11_CAP_IBSS)) {
+			iwe.cmd = SIOCGIWMODE;
+			if (dtoh16(bi->capability) & DOT11_CAP_ESS)
+				iwe.u.mode = IW_MODE_INFRA;
+			else
+				iwe.u.mode = IW_MODE_ADHOC;
+			event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_UINT_LEN);
+		}
+
+		iwe.cmd = SIOCGIWFREQ;
+		channel = (bi->ctl_ch == 0) ? CHSPEC_CHANNEL(bi->chanspec) : bi->ctl_ch;
+		iwe.u.freq.m = wf_channel2mhz(channel,
+			channel <= CH_MAX_2G_CHANNEL ?
+			WF_CHAN_FACTOR_2_4_G : WF_CHAN_FACTOR_5_G);
+		iwe.u.freq.e = 6;
+		event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_FREQ_LEN);
+
+		iwe.cmd = IWEVQUAL;
+		iwe.u.qual.qual = rssi_to_qual(dtoh16(bi->RSSI));
+		iwe.u.qual.level = 0x100 + dtoh16(bi->RSSI);
+		iwe.u.qual.noise = 0x100 + bi->phy_noise;
+		event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_QUAL_LEN);
+
+		wl_iw_handle_scanresults_ies(&event, end, info, bi);
+
+		iwe.cmd = SIOCGIWENCODE;
+		if (dtoh16(bi->capability) & DOT11_CAP_PRIVACY)
+			iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+		else
+			iwe.u.data.flags = IW_ENCODE_DISABLED;
+		iwe.u.data.length = 0;
+		event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)event);
+
+		if (bi->rateset.count) {
+			if (((event -extra) + IW_EV_LCP_LEN) <= (uintptr)end) {
+				value = event + IW_EV_LCP_LEN;
+				iwe.cmd = SIOCGIWRATE;
+
+				iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
+				for (j = 0; j < bi->rateset.count && j < IW_MAX_BITRATES; j++) {
+					iwe.u.bitrate.value = (bi->rateset.rates[j] & 0x7f) * 500000;
+					value = IWE_STREAM_ADD_VALUE(info, event, value, end, &iwe,
+						IW_EV_PARAM_LEN);
+				}
+				event = value;
+			}
+		}
+	} 
+
+	if ((ret = (event - extra)) < 0) {
+		WL_ERROR(("==> Wrong size\n"));
+		ret = 0;
+	}
+	WL_TRACE(("%s: size=%d bytes prepared \n", __FUNCTION__, (unsigned int)(event - extra)));
+	return (uint)ret;
+}
+
+static int
+wl_iw_get_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	channel_info_t ci;
+	wl_scan_results_t *list_merge;
+	wl_scan_results_t *list = (wl_scan_results_t *) g_scan;
+	int error;
+	uint buflen_from_user = dwrq->length;
+	uint len =  G_SCAN_RESULTS;
+	__u16 len_ret = 0;
+#if !defined(CSCAN)
+	__u16 merged_len = 0;
+#endif
+#if defined(WL_IW_USE_ISCAN)
+	iscan_info_t *iscan = g_iscan;
+	iscan_buf_t * p_buf;
+#if  !defined(CSCAN)
+	uint32 counter = 0;
+#endif
+#endif
+	WL_TRACE(("%s: buflen_from_user %d: \n", dev->name, buflen_from_user));
+
+	if (!extra) {
+		WL_TRACE(("%s: wl_iw_get_scan return -EINVAL\n", dev->name));
+		return -EINVAL;
+	}
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_CHANNEL, &ci, sizeof(ci))))
+		return error;
+	ci.scan_channel = dtoh32(ci.scan_channel);
+	if (ci.scan_channel)
+		return -EAGAIN;
+
+#if !defined(CSCAN)
+	if (g_ss_cache_ctrl.m_timer_expired) {
+		wl_iw_free_ss_cache();
+		g_ss_cache_ctrl.m_timer_expired ^= 1;
+	}
+	if ((!g_scan_specified_ssid && g_ss_cache_ctrl.m_prev_scan_mode) ||
+		g_ss_cache_ctrl.m_cons_br_scan_cnt > 4) {
+		g_ss_cache_ctrl.m_cons_br_scan_cnt = 0;
+		
+		wl_iw_reset_ss_cache();
+	}
+	g_ss_cache_ctrl.m_prev_scan_mode = g_scan_specified_ssid;
+	if (g_scan_specified_ssid) {
+		g_ss_cache_ctrl.m_cons_br_scan_cnt = 0;
+	}
+	else {
+		g_ss_cache_ctrl.m_cons_br_scan_cnt++;
+	}
+#endif
+
+	if (g_scan_specified_ssid) {
+		
+		list = kmalloc(len, GFP_KERNEL);
+		if (!list) {
+			WL_TRACE(("%s: wl_iw_get_scan return -ENOMEM\n", dev->name));
+			g_scan_specified_ssid = 0;
+			return -ENOMEM;
+		}
+	}
+
+	memset(list, 0, len);
+	list->buflen = htod32(len);
+	if ((error = dev_wlc_ioctl(dev, WLC_SCAN_RESULTS, list, len))) {
+		WL_ERROR(("%s: %s : Scan_results ERROR %d\n", dev->name, __FUNCTION__, error));
+		dwrq->length = len;
+		if (g_scan_specified_ssid) {
+			g_scan_specified_ssid = 0;
+			kfree(list);
+		}
+		return 0;
+	}
+	list->buflen = dtoh32(list->buflen);
+	list->version = dtoh32(list->version);
+	list->count = dtoh32(list->count);
+
+	if (list->version != WL_BSS_INFO_VERSION) {
+		WL_ERROR(("%s : list->version %d != WL_BSS_INFO_VERSION\n",
+				__FUNCTION__, list->version));
+		if (g_scan_specified_ssid) {
+			g_scan_specified_ssid = 0;
+			kfree(list);
+		}
+		return -EINVAL;
+	}
+
+#if !defined(CSCAN)
+	if (g_scan_specified_ssid) {
+		
+		wl_iw_add_bss_to_ss_cache(list);
+		kfree(list);
+	}
+
+	mutex_lock(&wl_cache_lock);
+#if defined(WL_IW_USE_ISCAN)
+	if (g_scan_specified_ssid)
+		WL_TRACE(("%s: Specified scan APs from scan=%d\n", __FUNCTION__, list->count));
+	p_buf = iscan->list_hdr;
+	
+	while (p_buf != iscan->list_cur) {
+		list_merge = &((wl_iscan_results_t*)p_buf->iscan_buf)->results;
+		WL_TRACE(("%s: Bcast APs list=%d\n", __FUNCTION__, list_merge->count));
+		counter += list_merge->count;
+		if (list_merge->count > 0)
+			len_ret += (__u16) wl_iw_get_scan_prep(list_merge, info,
+				extra+len_ret, buflen_from_user -len_ret);
+		p_buf = p_buf->next;
+	}
+	WL_TRACE(("%s merged with total Bcast APs=%d\n", __FUNCTION__, counter));
+#else
+	list_merge = (wl_scan_results_t *) g_scan;
+	len_ret = (__u16) wl_iw_get_scan_prep(list_merge, info, extra, buflen_from_user);
+#endif
+	mutex_unlock(&wl_cache_lock);
+	if (g_ss_cache_ctrl.m_link_down) {
+		wl_iw_delete_bss_from_ss_cache(g_ss_cache_ctrl.m_active_bssid);
+	}
+
+	wl_iw_merge_scan_cache(info, extra+len_ret, buflen_from_user-len_ret, &merged_len);
+	len_ret += merged_len;
+	wl_iw_run_ss_cache_timer(0);
+	wl_iw_run_ss_cache_timer(1);
+#else
+
+	if (g_scan_specified_ssid) {
+		WL_TRACE(("%s: Specified scan APs in the list =%d\n", __FUNCTION__, list->count));
+		len_ret = (__u16) wl_iw_get_scan_prep(list, info, extra, buflen_from_user);
+		kfree(list);
+
+#if defined(WL_IW_USE_ISCAN)
+		p_buf = iscan->list_hdr;
+		
+		while (p_buf != iscan->list_cur) {
+			list_merge = &((wl_iscan_results_t*)p_buf->iscan_buf)->results;
+			WL_TRACE(("%s: Bcast APs list=%d\n", __FUNCTION__, list_merge->count));
+			if (list_merge->count > 0)
+				len_ret += (__u16) wl_iw_get_scan_prep(list_merge, info,
+				    extra+len_ret, buflen_from_user -len_ret);
+			p_buf = p_buf->next;
+		}
+#else
+		list_merge = (wl_scan_results_t *) g_scan;
+		WL_TRACE(("%s: Bcast APs list=%d\n", __FUNCTION__, list_merge->count));
+		if (list_merge->count > 0)
+			len_ret += (__u16) wl_iw_get_scan_prep(list_merge, info, extra+len_ret,
+				buflen_from_user -len_ret);
+#endif
+	}
+	else {
+		list = (wl_scan_results_t *) g_scan;
+		len_ret = (__u16) wl_iw_get_scan_prep(list, info, extra, buflen_from_user);
+	}
+#endif
+
+#if defined(WL_IW_USE_ISCAN)
+	
+	g_scan_specified_ssid = 0;
+#endif 
+	
+	if ((len_ret + WE_ADD_EVENT_FIX) < buflen_from_user)
+		len = len_ret;
+
+	dwrq->length = len;
+	dwrq->flags = 0;	
+
+	WL_TRACE(("%s return to WE %d bytes APs=%d\n", __FUNCTION__, dwrq->length, list->count));
+	return 0;
+}
+#endif
+
+#if defined(WL_IW_USE_ISCAN)
+static int
+wl_iw_iscan_get_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_scan_results_t *list;
+	struct iw_event	iwe;
+	wl_bss_info_t *bi = NULL;
+	int ii, j;
+	int apcnt;
+	char *event = extra, *end = extra + dwrq->length, *value;
+	iscan_info_t *iscan = g_iscan;
+	iscan_buf_t * p_buf;
+	uint32  counter = 0;
+	uint8   channel;
+#if !defined(CSCAN)
+	__u16 merged_len = 0;
+	uint buflen_from_user = dwrq->length;
+#endif
+
+	WL_SCAN(("%s %s buflen_from_user %d:\n", dev->name, __FUNCTION__, dwrq->length));
+
+#if defined(SOFTAP)
+	if (ap_cfg_running) {
+		WL_TRACE(("%s: Not executed, reason -'SOFTAP is active'\n", __FUNCTION__));
+		return -EINVAL;
+	}
+#endif
+
+	if (!extra) {
+		WL_TRACE(("%s: INVALID SIOCGIWSCAN GET bad parameter\n", dev->name));
+		return -EINVAL;
+	}
+
+#if defined(CONFIG_FIRST_SCAN)
+	if (g_first_broadcast_scan < BROADCAST_SCAN_FIRST_RESULT_READY) {
+		WL_TRACE(("%s %s: first ISCAN results are NOT ready yet \n", \
+			 dev->name, __FUNCTION__));
+		return -EAGAIN;
+	}
+#endif
+
+	if ((!iscan) || (iscan->sysioc_pid < 0)) {
+		WL_ERROR(("%ssysioc_pid\n", __FUNCTION__));
+		return -EAGAIN;
+	}
+
+#if !defined(CSCAN)
+	if (g_ss_cache_ctrl.m_timer_expired) {
+		wl_iw_free_ss_cache();
+		g_ss_cache_ctrl.m_timer_expired ^= 1;
+	}
+	if (g_scan_specified_ssid) {
+		return wl_iw_get_scan(dev, info, dwrq, extra);
+	}
+	else {
+		if (g_ss_cache_ctrl.m_link_down) {
+			wl_iw_delete_bss_from_ss_cache(g_ss_cache_ctrl.m_active_bssid);
+		}
+		if (g_ss_cache_ctrl.m_prev_scan_mode || g_ss_cache_ctrl.m_cons_br_scan_cnt > 4) {
+			g_ss_cache_ctrl.m_cons_br_scan_cnt = 0;
+
+			wl_iw_reset_ss_cache();
+		}
+		g_ss_cache_ctrl.m_prev_scan_mode = g_scan_specified_ssid;
+		g_ss_cache_ctrl.m_cons_br_scan_cnt++;
+	}
+#endif
+
+	WL_TRACE(("%s: SIOCGIWSCAN GET broadcast results\n", dev->name));
+	apcnt = 0;
+	p_buf = iscan->list_hdr;
+	
+	while (p_buf != iscan->list_cur) {
+	    list = &((wl_iscan_results_t*)p_buf->iscan_buf)->results;
+
+	    counter += list->count;
+
+	    if (list->version != WL_BSS_INFO_VERSION) {
+		WL_ERROR(("%s : list->version %d != WL_BSS_INFO_VERSION\n",
+			 __FUNCTION__, list->version));
+		return -EINVAL;
+	    }
+
+	    bi = NULL;
+	    for (ii = 0; ii < list->count && apcnt < IW_MAX_AP; apcnt++, ii++) {
+		bi = bi ? (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length)) : list->bss_info;
+
+		if ((dtoh32(bi->length) > WLC_IW_ISCAN_MAXLEN) ||
+		    (((uintptr)bi + dtoh32(bi->length)) > ((uintptr)list + WLC_IW_ISCAN_MAXLEN))) {
+			WL_ERROR(("%s: Scan results out of bounds: %u\n",__FUNCTION__,dtoh32(bi->length)));
+			return -E2BIG;
+		}
+
+		if (event + ETHER_ADDR_LEN + bi->SSID_len + IW_EV_UINT_LEN + IW_EV_FREQ_LEN +
+			IW_EV_QUAL_LEN >= end)
+			return -E2BIG;
+
+		iwe.cmd = SIOCGIWAP;
+		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+		memcpy(iwe.u.ap_addr.sa_data, &bi->BSSID, ETHER_ADDR_LEN);
+		event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_ADDR_LEN);
+
+		iwe.u.data.length = dtoh32(bi->SSID_len);
+		iwe.cmd = SIOCGIWESSID;
+		iwe.u.data.flags = 1;
+		event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, bi->SSID);
+
+		if (dtoh16(bi->capability) & (DOT11_CAP_ESS | DOT11_CAP_IBSS)) {
+			iwe.cmd = SIOCGIWMODE;
+			if (dtoh16(bi->capability) & DOT11_CAP_ESS)
+				iwe.u.mode = IW_MODE_INFRA;
+			else
+				iwe.u.mode = IW_MODE_ADHOC;
+			event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_UINT_LEN);
+		}
+
+		iwe.cmd = SIOCGIWFREQ;
+		channel = (bi->ctl_ch == 0) ? CHSPEC_CHANNEL(bi->chanspec) : bi->ctl_ch;
+		iwe.u.freq.m = wf_channel2mhz(channel,
+			channel <= CH_MAX_2G_CHANNEL ?
+			WF_CHAN_FACTOR_2_4_G : WF_CHAN_FACTOR_5_G);
+		iwe.u.freq.e = 6;
+		event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_FREQ_LEN);
+
+		iwe.cmd = IWEVQUAL;
+		iwe.u.qual.qual = rssi_to_qual(dtoh16(bi->RSSI));
+		iwe.u.qual.level = 0x100 + dtoh16(bi->RSSI);
+		iwe.u.qual.noise = 0x100 + bi->phy_noise;
+		event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_QUAL_LEN);
+
+		wl_iw_handle_scanresults_ies(&event, end, info, bi);
+
+		iwe.cmd = SIOCGIWENCODE;
+		if (dtoh16(bi->capability) & DOT11_CAP_PRIVACY)
+			iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+		else
+			iwe.u.data.flags = IW_ENCODE_DISABLED;
+		iwe.u.data.length = 0;
+		event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)event);
+
+		if (bi->rateset.count) {
+			if (event + IW_MAX_BITRATES*IW_EV_PARAM_LEN >= end)
+				return -E2BIG;
+
+			value = event + IW_EV_LCP_LEN;
+			iwe.cmd = SIOCGIWRATE;
+			
+			iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
+			for (j = 0; j < bi->rateset.count && j < IW_MAX_BITRATES; j++) {
+				iwe.u.bitrate.value = (bi->rateset.rates[j] & 0x7f) * 500000;
+				value = IWE_STREAM_ADD_VALUE(info, event, value, end, &iwe,
+					IW_EV_PARAM_LEN);
+			}
+			event = value;
+		}
+	    }
+	    p_buf = p_buf->next;
+	} 
+
+	dwrq->length = event - extra;
+	dwrq->flags = 0;	
+
+#if !defined(CSCAN)
+	wl_iw_merge_scan_cache(info, event, buflen_from_user - dwrq->length, &merged_len);
+	dwrq->length += merged_len;
+	wl_iw_run_ss_cache_timer(0);
+	wl_iw_run_ss_cache_timer(1);
+#endif /* CSCAN */
+#if defined(CONFIG_FIRST_SCAN)
+	g_first_broadcast_scan = BROADCAST_SCAN_FIRST_RESULT_CONSUMED;
+#endif
+
+	WL_TRACE(("%s return to WE %d bytes APs=%d\n", __FUNCTION__, dwrq->length, counter));
+
+	return 0;
+}
+#endif 
+
+static int
+wl_iw_set_essid(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	int error;
+	wl_join_params_t join_params;
+	int join_params_size;
+
+	WL_TRACE(("%s: SIOCSIWESSID\n", dev->name));
+
+	
+	memset(&g_ssid, 0, sizeof(g_ssid));
+
+	CHECK_EXTRA_FOR_NULL(extra);
+
+	if (dwrq->length && extra) {
+#if WIRELESS_EXT > 20
+		g_ssid.SSID_len = MIN(sizeof(g_ssid.SSID), dwrq->length);
+#else
+		g_ssid.SSID_len = MIN(sizeof(g_ssid.SSID), dwrq->length-1);
+#endif
+		memcpy(g_ssid.SSID, extra, g_ssid.SSID_len);
+	} else {
+		
+		g_ssid.SSID_len = 0;
+	}
+	g_ssid.SSID_len = htod32(g_ssid.SSID_len);
+
+	memset(&join_params, 0, sizeof(join_params));
+	join_params_size = sizeof(join_params.ssid);
+
+	memcpy(&join_params.ssid.SSID, g_ssid.SSID, g_ssid.SSID_len);
+	join_params.ssid.SSID_len = htod32(g_ssid.SSID_len);
+	memcpy(&join_params.params.bssid, &ether_bcast, ETHER_ADDR_LEN);
+
+	wl_iw_ch_to_chanspec(g_wl_iw_params.target_channel, &join_params, &join_params_size);
+
+	if ((error = dev_wlc_ioctl(dev, WLC_SET_SSID, &join_params, join_params_size))) {
+		WL_ERROR(("Invalid ioctl data=%d\n", error));
+		return error;
+	}
+
+	if (g_ssid.SSID_len) {
+		WL_TRACE(("%s: join SSID=%s ch=%d\n", __FUNCTION__, \
+			g_ssid.SSID,  g_wl_iw_params.target_channel));
+	}
+	return 0;
+}
+
+static int
+wl_iw_get_essid(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wlc_ssid_t ssid;
+	int error;
+
+	WL_TRACE(("%s: SIOCGIWESSID\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	memset(&ssid, 0, sizeof(ssid));
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_SSID, &ssid, sizeof(ssid)))) {
+		WL_ERROR(("Error getting the SSID\n"));
+		return error;
+	}
+
+	ssid.SSID_len = dtoh32(ssid.SSID_len);
+
+	memcpy(extra, ssid.SSID, ssid.SSID_len);
+
+	dwrq->length = ssid.SSID_len;
+
+	dwrq->flags = 1; 
+
+	return 0;
+}
+
+static int
+wl_iw_set_nick(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_iw_t *iw = *(wl_iw_t **)netdev_priv(dev);
+
+	WL_TRACE(("%s: SIOCSIWNICKN\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	if (dwrq->length > sizeof(iw->nickname))
+		return -E2BIG;
+
+	memcpy(iw->nickname, extra, dwrq->length);
+	iw->nickname[dwrq->length - 1] = '\0';
+
+	return 0;
+}
+
+static int
+wl_iw_get_nick(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_iw_t *iw = *(wl_iw_t **)netdev_priv(dev);
+
+	WL_TRACE(("%s: SIOCGIWNICKN\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	strcpy(extra, iw->nickname);
+	dwrq->length = strlen(extra) + 1;
+
+	return 0;
+}
+
+static int wl_iw_set_rate(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	wl_rateset_t rateset;
+	int error, rate, i, error_bg, error_a;
+
+	WL_TRACE(("%s: SIOCSIWRATE\n", dev->name));
+
+	
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_CURR_RATESET, &rateset, sizeof(rateset))))
+		return error;
+
+	rateset.count = dtoh32(rateset.count);
+
+	if (vwrq->value < 0) {
+		
+		rate = rateset.rates[rateset.count - 1] & 0x7f;
+	} else if (vwrq->value < rateset.count) {
+		
+		rate = rateset.rates[vwrq->value] & 0x7f;
+	} else {
+		
+		rate = vwrq->value / 500000;
+	}
+
+	if (vwrq->fixed) {
+		
+		error_bg = dev_wlc_intvar_set(dev, "bg_rate", rate);
+		error_a = dev_wlc_intvar_set(dev, "a_rate", rate);
+
+		if (error_bg && error_a)
+			return (error_bg | error_a);
+	} else {
+		
+		
+		error_bg = dev_wlc_intvar_set(dev, "bg_rate", 0);
+		
+		error_a = dev_wlc_intvar_set(dev, "a_rate", 0);
+
+		if (error_bg && error_a)
+			return (error_bg | error_a);
+
+		
+		for (i = 0; i < rateset.count; i++)
+			if ((rateset.rates[i] & 0x7f) > rate)
+				break;
+		rateset.count = htod32(i);
+
+		
+		if ((error = dev_wlc_ioctl(dev, WLC_SET_RATESET, &rateset, sizeof(rateset))))
+			return error;
+	}
+
+	return 0;
+}
+
+static int wl_iw_get_rate(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, rate;
+
+	WL_TRACE(("%s: SIOCGIWRATE\n", dev->name));
+
+	
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_RATE, &rate, sizeof(rate))))
+		return error;
+	rate = dtoh32(rate);
+	vwrq->value = rate * 500000;
+
+	return 0;
+}
+
+static int
+wl_iw_set_rts(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, rts;
+
+	WL_TRACE(("%s: SIOCSIWRTS\n", dev->name));
+
+	if (vwrq->disabled)
+		rts = DOT11_DEFAULT_RTS_LEN;
+	else if (vwrq->value < 0 || vwrq->value > DOT11_DEFAULT_RTS_LEN)
+		return -EINVAL;
+	else
+		rts = vwrq->value;
+
+	if ((error = dev_wlc_intvar_set(dev, "rtsthresh", rts)))
+		return error;
+
+	return 0;
+}
+
+static int
+wl_iw_get_rts(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, rts;
+
+	WL_TRACE(("%s: SIOCGIWRTS\n", dev->name));
+
+	if ((error = dev_wlc_intvar_get(dev, "rtsthresh", &rts)))
+		return error;
+
+	vwrq->value = rts;
+	vwrq->disabled = (rts >= DOT11_DEFAULT_RTS_LEN);
+	vwrq->fixed = 1;
+
+	return 0;
+}
+
+static int
+wl_iw_set_frag(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, frag;
+
+	WL_TRACE(("%s: SIOCSIWFRAG\n", dev->name));
+
+	if (vwrq->disabled)
+		frag = DOT11_DEFAULT_FRAG_LEN;
+	else if (vwrq->value < 0 || vwrq->value > DOT11_DEFAULT_FRAG_LEN)
+		return -EINVAL;
+	else
+		frag = vwrq->value;
+
+	if ((error = dev_wlc_intvar_set(dev, "fragthresh", frag)))
+		return error;
+
+	return 0;
+}
+
+static int
+wl_iw_get_frag(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, fragthreshold;
+
+	WL_TRACE(("%s: SIOCGIWFRAG\n", dev->name));
+
+	if ((error = dev_wlc_intvar_get(dev, "fragthresh", &fragthreshold)))
+		return error;
+
+	vwrq->value = fragthreshold;
+	vwrq->disabled = (fragthreshold >= DOT11_DEFAULT_FRAG_LEN);
+	vwrq->fixed = 1;
+
+	return 0;
+}
+
+static int
+wl_iw_set_txpow(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, disable;
+	uint16 txpwrmw;
+	WL_TRACE(("%s: SIOCSIWTXPOW\n", dev->name));
+
+	
+	disable = vwrq->disabled ? WL_RADIO_SW_DISABLE : 0;
+	disable += WL_RADIO_SW_DISABLE << 16;
+
+	disable = htod32(disable);
+	if ((error = dev_wlc_ioctl(dev, WLC_SET_RADIO, &disable, sizeof(disable))))
+		return error;
+
+	
+	if (disable & WL_RADIO_SW_DISABLE)
+		return 0;
+
+	
+	if (!(vwrq->flags & IW_TXPOW_MWATT))
+		return -EINVAL;
+
+	
+	if (vwrq->value < 0)
+		return 0;
+
+	if (vwrq->value > 0xffff) txpwrmw = 0xffff;
+	else txpwrmw = (uint16)vwrq->value;
+
+
+	error = dev_wlc_intvar_set(dev, "qtxpower", (int)(bcm_mw_to_qdbm(txpwrmw)));
+	return error;
+}
+
+static int
+wl_iw_get_txpow(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, disable, txpwrdbm;
+	uint8 result;
+
+	WL_TRACE(("%s: SIOCGIWTXPOW\n", dev->name));
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_RADIO, &disable, sizeof(disable))) ||
+	    (error = dev_wlc_intvar_get(dev, "qtxpower", &txpwrdbm)))
+		return error;
+
+	disable = dtoh32(disable);
+	result = (uint8)(txpwrdbm & ~WL_TXPWR_OVERRIDE);
+	vwrq->value = (int32)bcm_qdbm_to_mw(result);
+	vwrq->fixed = 0;
+	vwrq->disabled = (disable & (WL_RADIO_SW_DISABLE | WL_RADIO_HW_DISABLE)) ? 1 : 0;
+	vwrq->flags = IW_TXPOW_MWATT;
+
+	return 0;
+}
+
+#if WIRELESS_EXT > 10
+static int
+wl_iw_set_retry(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, lrl, srl;
+
+	WL_TRACE(("%s: SIOCSIWRETRY\n", dev->name));
+
+	
+	if (vwrq->disabled || (vwrq->flags & IW_RETRY_LIFETIME))
+		return -EINVAL;
+
+	
+	if (vwrq->flags & IW_RETRY_LIMIT) {
+
+		
+#if WIRELESS_EXT > 20
+	if ((vwrq->flags & IW_RETRY_LONG) ||(vwrq->flags & IW_RETRY_MAX) ||
+		!((vwrq->flags & IW_RETRY_SHORT) || (vwrq->flags & IW_RETRY_MIN))) {
+#else
+	if ((vwrq->flags & IW_RETRY_MAX) || !(vwrq->flags & IW_RETRY_MIN)) {
+#endif 
+			lrl = htod32(vwrq->value);
+			if ((error = dev_wlc_ioctl(dev, WLC_SET_LRL, &lrl, sizeof(lrl))))
+				return error;
+		}
+
+		
+#if WIRELESS_EXT > 20
+	if ((vwrq->flags & IW_RETRY_SHORT) ||(vwrq->flags & IW_RETRY_MIN) ||
+		!((vwrq->flags & IW_RETRY_LONG) || (vwrq->flags & IW_RETRY_MAX))) {
+#else
+		if ((vwrq->flags & IW_RETRY_MIN) || !(vwrq->flags & IW_RETRY_MAX)) {
+#endif 
+			srl = htod32(vwrq->value);
+			if ((error = dev_wlc_ioctl(dev, WLC_SET_SRL, &srl, sizeof(srl))))
+				return error;
+		}
+	}
+	return 0;
+}
+
+static int
+wl_iw_get_retry(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, lrl, srl;
+
+	WL_TRACE(("%s: SIOCGIWRETRY\n", dev->name));
+
+	vwrq->disabled = 0;      
+
+	
+	if ((vwrq->flags & IW_RETRY_TYPE) == IW_RETRY_LIFETIME)
+		return -EINVAL;
+
+	
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_LRL, &lrl, sizeof(lrl))) ||
+	    (error = dev_wlc_ioctl(dev, WLC_GET_SRL, &srl, sizeof(srl))))
+		return error;
+
+	lrl = dtoh32(lrl);
+	srl = dtoh32(srl);
+
+	
+	if (vwrq->flags & IW_RETRY_MAX) {
+		vwrq->flags = IW_RETRY_LIMIT | IW_RETRY_MAX;
+		vwrq->value = lrl;
+	} else {
+		vwrq->flags = IW_RETRY_LIMIT;
+		vwrq->value = srl;
+		if (srl != lrl)
+			vwrq->flags |= IW_RETRY_MIN;
+	}
+
+	return 0;
+}
+#endif 
+
+static int
+wl_iw_set_encode(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_wsec_key_t key;
+	int error, val, wsec;
+
+	WL_TRACE(("%s: SIOCSIWENCODE\n", dev->name));
+
+	memset(&key, 0, sizeof(key));
+
+	if ((dwrq->flags & IW_ENCODE_INDEX) == 0) {
+		
+		for (key.index = 0; key.index < DOT11_MAX_DEFAULT_KEYS; key.index++) {
+			val = htod32(key.index);
+			if ((error = dev_wlc_ioctl(dev, WLC_GET_KEY_PRIMARY, &val, sizeof(val))))
+				return error;
+			val = dtoh32(val);
+			if (val)
+				break;
+		}
+		
+		if (key.index == DOT11_MAX_DEFAULT_KEYS)
+			key.index = 0;
+	} else {
+		key.index = (dwrq->flags & IW_ENCODE_INDEX) - 1;
+		if (key.index >= DOT11_MAX_DEFAULT_KEYS)
+			return -EINVAL;
+	}
+
+	
+	if (!extra || !dwrq->length || (dwrq->flags & IW_ENCODE_NOKEY)) {
+		
+		val = htod32(key.index);
+		if ((error = dev_wlc_ioctl(dev, WLC_SET_KEY_PRIMARY, &val, sizeof(val))))
+			return error;
+	} else {
+		key.len = dwrq->length;
+
+		if (dwrq->length > sizeof(key.data))
+			return -EINVAL;
+
+		memcpy(key.data, extra, dwrq->length);
+
+		key.flags = WL_PRIMARY_KEY;
+		switch (key.len) {
+		case WEP1_KEY_SIZE:
+			key.algo = CRYPTO_ALGO_WEP1;
+			break;
+		case WEP128_KEY_SIZE:
+			key.algo = CRYPTO_ALGO_WEP128;
+			break;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 14)
+		case TKIP_KEY_SIZE:
+			key.algo = CRYPTO_ALGO_TKIP;
+			break;
+#endif
+		case AES_KEY_SIZE:
+			key.algo = CRYPTO_ALGO_AES_CCM;
+			break;
+		default:
+			return -EINVAL;
+		}
+
+		
+		swap_key_from_BE(&key);
+		if ((error = dev_wlc_ioctl(dev, WLC_SET_KEY, &key, sizeof(key))))
+			return error;
+	}
+
+	
+	val = (dwrq->flags & IW_ENCODE_DISABLED) ? 0 : WEP_ENABLED;
+
+	if ((error = dev_wlc_intvar_get(dev, "wsec", &wsec)))
+		return error;
+
+	wsec  &= ~(WEP_ENABLED);
+	wsec |= val;
+
+	if ((error = dev_wlc_intvar_set(dev, "wsec", wsec)))
+		return error;
+
+	
+	val = (dwrq->flags & IW_ENCODE_RESTRICTED) ? 1 : 0;
+	val = htod32(val);
+	if ((error = dev_wlc_ioctl(dev, WLC_SET_AUTH, &val, sizeof(val))))
+		return error;
+
+	return 0;
+}
+
+static int
+wl_iw_get_encode(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_wsec_key_t key;
+	int error, val, wsec, auth;
+
+	WL_TRACE(("%s: SIOCGIWENCODE\n", dev->name));
+
+	
+	bzero(&key, sizeof(wl_wsec_key_t));
+
+	if ((dwrq->flags & IW_ENCODE_INDEX) == 0) {
+		
+		for (key.index = 0; key.index < DOT11_MAX_DEFAULT_KEYS; key.index++) {
+			val = key.index;
+			if ((error = dev_wlc_ioctl(dev, WLC_GET_KEY_PRIMARY, &val, sizeof(val))))
+				return error;
+			val = dtoh32(val);
+			if (val)
+				break;
+		}
+	} else
+		key.index = (dwrq->flags & IW_ENCODE_INDEX) - 1;
+
+	if (key.index >= DOT11_MAX_DEFAULT_KEYS)
+		key.index = 0;
+
+	
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_WSEC, &wsec, sizeof(wsec))) ||
+	    (error = dev_wlc_ioctl(dev, WLC_GET_AUTH, &auth, sizeof(auth))))
+		return error;
+
+	swap_key_to_BE(&key);
+
+	wsec = dtoh32(wsec);
+	auth = dtoh32(auth);
+	
+	dwrq->length = MIN(DOT11_MAX_KEY_SIZE, key.len);
+
+	
+	dwrq->flags = key.index + 1;
+	if (!(wsec & (WEP_ENABLED | TKIP_ENABLED | AES_ENABLED))) {
+		
+		dwrq->flags |= IW_ENCODE_DISABLED;
+	}
+	if (auth) {
+		
+		dwrq->flags |= IW_ENCODE_RESTRICTED;
+	}
+
+	
+	if (dwrq->length && extra)
+		memcpy(extra, key.data, dwrq->length);
+
+	return 0;
+}
+
+static int
+wl_iw_set_power(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, pm;
+
+	WL_TRACE(("%s: SIOCSIWPOWER\n", dev->name));
+
+	pm = vwrq->disabled ? PM_OFF : PM_MAX;
+
+	pm = htod32(pm);
+	if ((error = dev_wlc_ioctl(dev, WLC_SET_PM, &pm, sizeof(pm))))
+		return error;
+
+	return 0;
+}
+
+static int
+wl_iw_get_power(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, pm;
+
+	WL_TRACE(("%s: SIOCGIWPOWER\n", dev->name));
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_PM, &pm, sizeof(pm))))
+		return error;
+
+	pm = dtoh32(pm);
+	vwrq->disabled = pm ? 0 : 1;
+	vwrq->flags = IW_POWER_ALL_R;
+
+	return 0;
+}
+
+#if WIRELESS_EXT > 17
+static int
+wl_iw_set_wpaie(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *iwp,
+	char *extra
+)
+{
+	uchar buf[WLC_IOCTL_SMLEN] = {0};
+	uchar *p = buf;
+	int wapi_ie_size;
+
+	WL_TRACE(("%s: SIOCSIWGENIE\n", dev->name));
+
+	CHECK_EXTRA_FOR_NULL(extra);
+
+	if (extra[0] == DOT11_MNG_WAPI_ID)
+	{
+		wapi_ie_size = iwp->length;
+		memcpy(p, extra, iwp->length);
+		dev_wlc_bufvar_set(dev, "wapiie", buf, wapi_ie_size);
+	}
+	else
+		dev_wlc_bufvar_set(dev, "wpaie", extra, iwp->length);
+
+	return 0;
+}
+
+static int
+wl_iw_get_wpaie(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *iwp,
+	char *extra
+)
+{
+	WL_TRACE(("%s: SIOCGIWGENIE\n", dev->name));
+	iwp->length = 64;
+	dev_wlc_bufvar_get(dev, "wpaie", extra, iwp->length);
+	return 0;
+}
+
+static int
+wl_iw_set_encodeext(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_wsec_key_t key;
+	int error;
+	struct iw_encode_ext *iwe;
+
+	WL_WSEC(("%s: SIOCSIWENCODEEXT\n", dev->name));
+
+	CHECK_EXTRA_FOR_NULL(extra);
+
+	memset(&key, 0, sizeof(key));
+	iwe = (struct iw_encode_ext *)extra;
+
+	
+	if (dwrq->flags & IW_ENCODE_DISABLED) {
+
+	}
+
+	
+	key.index = 0;
+	if (dwrq->flags & IW_ENCODE_INDEX)
+		key.index = (dwrq->flags & IW_ENCODE_INDEX) - 1;
+
+	key.len = iwe->key_len;
+
+	
+	if (!ETHER_ISMULTI(iwe->addr.sa_data))
+		bcopy((void *)&iwe->addr.sa_data, (char *)&key.ea, ETHER_ADDR_LEN);
+
+	
+	if (key.len == 0) {
+		if (iwe->ext_flags & IW_ENCODE_EXT_SET_TX_KEY) {
+			WL_WSEC(("Changing the the primary Key to %d\n", key.index));
+			
+			key.index = htod32(key.index);
+			error = dev_wlc_ioctl(dev, WLC_SET_KEY_PRIMARY,
+				&key.index, sizeof(key.index));
+			if (error)
+				return error;
+		}
+		
+		else {
+			swap_key_from_BE(&key);
+			dev_wlc_ioctl(dev, WLC_SET_KEY, &key, sizeof(key));
+		}
+	}
+	else {
+		if (iwe->key_len > sizeof(key.data))
+			return -EINVAL;
+
+		WL_WSEC(("Setting the key index %d\n", key.index));
+		if (iwe->ext_flags & IW_ENCODE_EXT_SET_TX_KEY) {
+			WL_WSEC(("key is a Primary Key\n"));
+			key.flags = WL_PRIMARY_KEY;
+		}
+
+		bcopy((void *)iwe->key, key.data, iwe->key_len);
+
+		if (iwe->alg == IW_ENCODE_ALG_TKIP) {
+			uint8 keybuf[8];
+			bcopy(&key.data[24], keybuf, sizeof(keybuf));
+			bcopy(&key.data[16], &key.data[24], sizeof(keybuf));
+			bcopy(keybuf, &key.data[16], sizeof(keybuf));
+		}
+
+		
+		if (iwe->ext_flags & IW_ENCODE_EXT_RX_SEQ_VALID) {
+			uchar *ivptr;
+			ivptr = (uchar *)iwe->rx_seq;
+			key.rxiv.hi = (ivptr[5] << 24) | (ivptr[4] << 16) |
+				(ivptr[3] << 8) | ivptr[2];
+			key.rxiv.lo = (ivptr[1] << 8) | ivptr[0];
+			key.iv_initialized = TRUE;
+		}
+
+		switch (iwe->alg) {
+			case IW_ENCODE_ALG_NONE:
+				key.algo = CRYPTO_ALGO_OFF;
+				break;
+			case IW_ENCODE_ALG_WEP:
+				if (iwe->key_len == WEP1_KEY_SIZE)
+					key.algo = CRYPTO_ALGO_WEP1;
+				else
+					key.algo = CRYPTO_ALGO_WEP128;
+				break;
+			case IW_ENCODE_ALG_TKIP:
+				key.algo = CRYPTO_ALGO_TKIP;
+				break;
+			case IW_ENCODE_ALG_CCMP:
+				key.algo = CRYPTO_ALGO_AES_CCM;
+				break;
+			case IW_ENCODE_ALG_SM4:
+				key.algo = CRYPTO_ALGO_SMS4;
+				if (iwe->ext_flags & IW_ENCODE_EXT_GROUP_KEY) {
+					key.flags &= ~WL_PRIMARY_KEY;
+				}
+				break;
+			default:
+				break;
+		}
+		swap_key_from_BE(&key);
+
+		dhd_wait_pend8021x(dev);
+
+		error = dev_wlc_ioctl(dev, WLC_SET_KEY, &key, sizeof(key));
+		if (error)
+			return error;
+	}
+	return 0;
+}
+
+#if WIRELESS_EXT > 17
+#ifdef BCMWPA2
+struct {
+	pmkid_list_t pmkids;
+	pmkid_t foo[MAXPMKID-1];
+} pmkid_list;
+
+static int
+wl_iw_set_pmksa(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	struct iw_pmksa *iwpmksa;
+	uint i;
+	int ret = 0;
+	char eabuf[ETHER_ADDR_STR_LEN];
+	pmkid_t * pmkid_array = pmkid_list.pmkids.pmkid;
+
+	WL_WSEC(("%s: SIOCSIWPMKSA\n", dev->name));
+	CHECK_EXTRA_FOR_NULL(extra);
+
+	iwpmksa = (struct iw_pmksa *)extra;
+	bzero((char *)eabuf, ETHER_ADDR_STR_LEN);
+
+	if (iwpmksa->cmd == IW_PMKSA_FLUSH) {
+		WL_WSEC(("wl_iw_set_pmksa - IW_PMKSA_FLUSH\n"));
+		bzero((char *)&pmkid_list, sizeof(pmkid_list));
+	}
+
+	else if (iwpmksa->cmd == IW_PMKSA_REMOVE) {
+		{
+			pmkid_list_t pmkid, *pmkidptr;
+			uint j;
+			pmkidptr = &pmkid;
+
+			bcopy(&iwpmksa->bssid.sa_data[0], &pmkidptr->pmkid[0].BSSID, ETHER_ADDR_LEN);
+			bcopy(&iwpmksa->pmkid[0], &pmkidptr->pmkid[0].PMKID, WPA2_PMKID_LEN);
+
+			WL_WSEC(("wl_iw_set_pmksa,IW_PMKSA_REMOVE - PMKID: %s = ",
+				bcm_ether_ntoa(&pmkidptr->pmkid[0].BSSID,
+				eabuf)));
+			for (j = 0; j < WPA2_PMKID_LEN; j++)
+				WL_WSEC(("%02x ", pmkidptr->pmkid[0].PMKID[j]));
+			WL_WSEC(("\n"));
+		}
+
+		for (i = 0; i < pmkid_list.pmkids.npmkid; i++)
+			if (!bcmp(&iwpmksa->bssid.sa_data[0], &pmkid_array[i].BSSID,
+				ETHER_ADDR_LEN))
+				break;
+
+		if ((pmkid_list.pmkids.npmkid > 0) && (i < pmkid_list.pmkids.npmkid)) {
+			bzero(&pmkid_array[i], sizeof(pmkid_t));
+			for (; i < (pmkid_list.pmkids.npmkid - 1); i++) {
+				bcopy(&pmkid_array[i+1].BSSID,
+					&pmkid_array[i].BSSID,
+					ETHER_ADDR_LEN);
+				bcopy(&pmkid_array[i+1].PMKID,
+					&pmkid_array[i].PMKID,
+					WPA2_PMKID_LEN);
+			}
+			pmkid_list.pmkids.npmkid--;
+		}
+		else
+			ret = -EINVAL;
+	}
+
+	else if (iwpmksa->cmd == IW_PMKSA_ADD) {
+		for (i = 0; i < pmkid_list.pmkids.npmkid; i++)
+			if (!bcmp(&iwpmksa->bssid.sa_data[0], &pmkid_array[i].BSSID,
+				ETHER_ADDR_LEN))
+				break;
+		if (i < MAXPMKID) {
+			bcopy(&iwpmksa->bssid.sa_data[0],
+				&pmkid_array[i].BSSID,
+				ETHER_ADDR_LEN);
+			bcopy(&iwpmksa->pmkid[0], &pmkid_array[i].PMKID,
+				WPA2_PMKID_LEN);
+			if (i == pmkid_list.pmkids.npmkid)
+				pmkid_list.pmkids.npmkid++;
+		}
+		else
+			ret = -EINVAL;
+
+		{
+			uint j;
+			uint k;
+			k = pmkid_list.pmkids.npmkid;
+			WL_WSEC(("wl_iw_set_pmksa,IW_PMKSA_ADD - PMKID: %s = ",
+				bcm_ether_ntoa(&pmkid_array[k].BSSID,
+				eabuf)));
+			for (j = 0; j < WPA2_PMKID_LEN; j++)
+				WL_WSEC(("%02x ", pmkid_array[k].PMKID[j]));
+			WL_WSEC(("\n"));
+		}
+	}
+	WL_WSEC(("PRINTING pmkid LIST - No of elements %d, ret = %d\n", pmkid_list.pmkids.npmkid, ret));
+	for (i = 0; i < pmkid_list.pmkids.npmkid; i++) {
+		uint j;
+		WL_WSEC(("PMKID[%d]: %s = ", i,
+			bcm_ether_ntoa(&pmkid_array[i].BSSID,
+			eabuf)));
+		for (j = 0; j < WPA2_PMKID_LEN; j++)
+			WL_WSEC(("%02x ", pmkid_array[i].PMKID[j]));
+		WL_WSEC(("\n"));
+	}
+	WL_WSEC(("\n"));
+
+	if (!ret)
+		ret = dev_wlc_bufvar_set(dev, "pmkid_info", (char *)&pmkid_list, sizeof(pmkid_list));
+	return ret;
+}
+#endif 
+#endif 
+
+static int
+wl_iw_get_encodeext(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	WL_WSEC(("%s: SIOCGIWENCODEEXT\n", dev->name));
+	return 0;
+}
+
+static int
+wl_iw_set_wpaauth(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error = 0;
+	int paramid;
+	int paramval;
+	int val = 0;
+	wl_iw_t *iw = *(wl_iw_t **)netdev_priv(dev);
+
+	WL_WSEC(("%s: SIOCSIWAUTH\n", dev->name));
+
+#if defined(SOFTAP)
+	if (ap_cfg_running) {
+		WL_TRACE(("%s: Not executed, reason -'SOFTAP is active'\n", __FUNCTION__));
+		return 0;
+	}
+#endif
+
+	paramid = vwrq->flags & IW_AUTH_INDEX;
+	paramval = vwrq->value;
+
+	WL_WSEC(("%s: SIOCSIWAUTH, paramid = 0x%0x, paramval = 0x%0x\n",
+		dev->name, paramid, paramval));
+
+	switch (paramid) {
+	case IW_AUTH_WPA_VERSION:
+		
+		if (paramval & IW_AUTH_WPA_VERSION_DISABLED)
+			val = WPA_AUTH_DISABLED;
+		else if (paramval & (IW_AUTH_WPA_VERSION_WPA))
+			val = WPA_AUTH_PSK | WPA_AUTH_UNSPECIFIED;
+#ifdef BCMWPA2
+		else if (paramval & IW_AUTH_WPA_VERSION_WPA2)
+			val = WPA2_AUTH_PSK | WPA2_AUTH_UNSPECIFIED;
+#endif 
+		else if (paramval & IW_AUTH_WAPI_VERSION_1)
+			val = WPA_AUTH_WAPI;
+		WL_WSEC(("%s: %d: setting wpa_auth to 0x%0x\n", __FUNCTION__, __LINE__, val));
+		if ((error = dev_wlc_intvar_set(dev, "wpa_auth", val)))
+			return error;
+		break;
+	case IW_AUTH_CIPHER_PAIRWISE:
+	case IW_AUTH_CIPHER_GROUP:
+		
+		
+		if (paramval & (IW_AUTH_CIPHER_WEP40 | IW_AUTH_CIPHER_WEP104))
+			val = 	WEP_ENABLED;
+		if (paramval & IW_AUTH_CIPHER_TKIP)
+			val = TKIP_ENABLED;
+		if (paramval & IW_AUTH_CIPHER_CCMP)
+			val = AES_ENABLED;
+		if (paramval & IW_AUTH_CIPHER_SMS4)
+			val = SMS4_ENABLED;
+
+		if (paramid == IW_AUTH_CIPHER_PAIRWISE) {
+			iw->pwsec = val;
+			val |= iw->gwsec;
+		}
+		else {
+			iw->gwsec = val;
+			val |= iw->pwsec;
+		}
+
+		if (iw->privacy_invoked && !val) {
+			WL_WSEC(("%s: %s: 'Privacy invoked' TRUE but clearing wsec, assuming "
+				"we're a WPS enrollee\n", dev->name, __FUNCTION__));
+			if ((error = dev_wlc_intvar_set(dev, "is_WPS_enrollee", TRUE))) {
+				WL_ERROR(("Failed to set iovar is_WPS_enrollee\n"));
+				return error;
+			}
+		} else if (val) {
+			if ((error = dev_wlc_intvar_set(dev, "is_WPS_enrollee", FALSE))) {
+				WL_ERROR(("Failed to clear iovar is_WPS_enrollee\n"));
+				return error;
+			}
+		}
+
+		if ((error = dev_wlc_intvar_set(dev, "wsec", val))) {
+			WL_ERROR(("Failed to set 'wsec'iovar\n"));
+			return error;
+		}
+
+		break;
+
+	case IW_AUTH_KEY_MGMT:
+		if ((error = dev_wlc_intvar_get(dev, "wpa_auth", &val))) {
+			WL_ERROR(("Failed to get 'wpa_auth'iovar\n"));
+			return error;
+		}
+
+		if (val & (WPA_AUTH_PSK | WPA_AUTH_UNSPECIFIED)) {
+			if (paramval & IW_AUTH_KEY_MGMT_PSK)
+				val = WPA_AUTH_PSK;
+			else
+				val = WPA_AUTH_UNSPECIFIED;
+		}
+#ifdef BCMWPA2
+		else if (val & (WPA2_AUTH_PSK | WPA2_AUTH_UNSPECIFIED)) {
+			if (paramval & IW_AUTH_KEY_MGMT_PSK)
+				val = WPA2_AUTH_PSK;
+			else
+				val = WPA2_AUTH_UNSPECIFIED;
+		}
+#endif 
+		if (paramval & (IW_AUTH_KEY_MGMT_WAPI_PSK | IW_AUTH_KEY_MGMT_WAPI_CERT))
+			val = WPA_AUTH_WAPI;
+		WL_WSEC(("%s: %d: setting wpa_auth to %d\n", __FUNCTION__, __LINE__, val));
+		if ((error = dev_wlc_intvar_set(dev, "wpa_auth", val))) {
+			WL_ERROR(("Failed to set 'wpa_auth'iovar\n"));
+			return error;
+		}
+
+		break;
+	case IW_AUTH_TKIP_COUNTERMEASURES:
+		if ((error = dev_wlc_bufvar_set(dev, "tkip_countermeasures", \
+						(char *)&paramval, sizeof(paramval))))
+			WL_WSEC(("%s: tkip_countermeasures failed %d\n", __FUNCTION__, error));
+		break;
+
+	case IW_AUTH_80211_AUTH_ALG:
+		
+		WL_WSEC(("Setting the D11auth %d\n", paramval));
+		if (paramval == IW_AUTH_ALG_OPEN_SYSTEM)
+			val = 0;
+		else if (paramval == IW_AUTH_ALG_SHARED_KEY)
+			val = 1;
+		else if (paramval == (IW_AUTH_ALG_OPEN_SYSTEM | IW_AUTH_ALG_SHARED_KEY))
+			val = 2;
+		else
+			error = 1;
+		if (!error && (error = dev_wlc_intvar_set(dev, "auth", val)))
+			return error;
+		break;
+
+	case IW_AUTH_WPA_ENABLED:
+		if (paramval == 0) {
+			iw->pwsec = 0;
+			iw->gwsec = 0;
+			if ((error = dev_wlc_intvar_get(dev, "wsec", &val))) {
+				WL_ERROR(("Failed to get 'wsec'iovar\n"));
+				return error;
+			}
+			if (val & (TKIP_ENABLED | AES_ENABLED)) {
+				val &= ~(TKIP_ENABLED | AES_ENABLED);
+				dev_wlc_intvar_set(dev, "wsec", val);
+			}
+			val = 0;
+
+			WL_INFORM(("%s: %d: setting wpa_auth to %d\n",
+				__FUNCTION__, __LINE__, val));
+			error = dev_wlc_intvar_set(dev, "wpa_auth", 0);
+			if (error)
+				WL_ERROR(("Failed to set 'wpa_auth'iovar\n"));
+			return error;
+		}
+
+		
+		break;
+
+	case IW_AUTH_DROP_UNENCRYPTED:
+		error = dev_wlc_bufvar_set(dev, "wsec_restrict", \
+				   (char *)&paramval, sizeof(paramval));
+		if (error)
+			WL_ERROR(("%s: wsec_restrict %d\n", __FUNCTION__, error));
+		break;
+
+	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+		error = dev_wlc_bufvar_set(dev, "rx_unencrypted_eapol", \
+				   (char *)&paramval, sizeof(paramval));
+		if (error)
+			WL_WSEC(("%s: rx_unencrypted_eapol %d\n", __FUNCTION__, error));
+		break;
+
+#if WIRELESS_EXT > 17
+	case IW_AUTH_ROAMING_CONTROL:
+		WL_INFORM(("%s: IW_AUTH_ROAMING_CONTROL\n", __FUNCTION__));
+		
+		break;
+	case IW_AUTH_PRIVACY_INVOKED: {
+		int wsec;
+
+		if (paramval == 0) {
+			iw->privacy_invoked = FALSE;
+			if ((error = dev_wlc_intvar_set(dev, "is_WPS_enrollee", FALSE))) {
+				WL_WSEC(("Failed to clear iovar is_WPS_enrollee\n"));
+				return error;
+			}
+		} else {
+			iw->privacy_invoked = TRUE;
+			if ((error = dev_wlc_intvar_get(dev, "wsec", &wsec)))
+				return error;
+
+			if (!(IW_WSEC_ENABLED(wsec))) {
+
+				if ((error = dev_wlc_intvar_set(dev, "is_WPS_enrollee", TRUE))) {
+					WL_WSEC(("Failed to set iovar is_WPS_enrollee\n"));
+					return error;
+				}
+			} else {
+				if ((error = dev_wlc_intvar_set(dev, "is_WPS_enrollee", FALSE))) {
+					WL_WSEC(("Failed to clear iovar is_WPS_enrollee\n"));
+					return error;
+				}
+			}
+		}
+		break;
+	}
+#endif
+	case IW_AUTH_WAPI_ENABLED:
+		if ((error = dev_wlc_intvar_get(dev, "wsec", &val)))
+			return error;
+		if (paramval) {
+			val |= SMS4_ENABLED;
+			if ((error = dev_wlc_intvar_set(dev, "wsec", val))) {
+				WL_ERROR(("%s: setting wsec to 0x%0x returned error %d\n",
+					__FUNCTION__, val, error));
+				return error;
+			}
+			if ((error = dev_wlc_intvar_set(dev, "wpa_auth", WPA_AUTH_WAPI))) {
+				WL_ERROR(("%s: setting wpa_auth(WPA_AUTH_WAPI) returned %d\n",
+					__FUNCTION__, error));
+				return error;
+			}
+		}
+
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+#ifdef BCMWPA2
+#define VAL_PSK(_val) (((_val) & WPA_AUTH_PSK) || ((_val) & WPA2_AUTH_PSK))
+#else
+#define VAL_PSK(_val) (((_val) & WPA_AUTH_PSK))
+#endif 
+
+static int
+wl_iw_get_wpaauth(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error;
+	int paramid;
+	int paramval = 0;
+	int val;
+	wl_iw_t *iw = *(wl_iw_t **)netdev_priv(dev);
+
+	WL_TRACE(("%s: SIOCGIWAUTH\n", dev->name));
+
+	paramid = vwrq->flags & IW_AUTH_INDEX;
+
+	switch (paramid) {
+	case IW_AUTH_WPA_VERSION:
+		
+		if ((error = dev_wlc_intvar_get(dev, "wpa_auth", &val)))
+			return error;
+		if (val & (WPA_AUTH_NONE | WPA_AUTH_DISABLED))
+			paramval = IW_AUTH_WPA_VERSION_DISABLED;
+		else if (val & (WPA_AUTH_PSK | WPA_AUTH_UNSPECIFIED))
+			paramval = IW_AUTH_WPA_VERSION_WPA;
+#ifdef BCMWPA2
+		else if (val & (WPA2_AUTH_PSK | WPA2_AUTH_UNSPECIFIED))
+			paramval = IW_AUTH_WPA_VERSION_WPA2;
+#endif 
+		break;
+	case IW_AUTH_CIPHER_PAIRWISE:
+	case IW_AUTH_CIPHER_GROUP:
+		if (paramid == IW_AUTH_CIPHER_PAIRWISE)
+			val = iw->pwsec;
+		else
+			val = iw->gwsec;
+
+		paramval = 0;
+		if (val) {
+			if (val & WEP_ENABLED)
+				paramval |= (IW_AUTH_CIPHER_WEP40 | IW_AUTH_CIPHER_WEP104);
+			if (val & TKIP_ENABLED)
+				paramval |= (IW_AUTH_CIPHER_TKIP);
+			if (val & AES_ENABLED)
+				paramval |= (IW_AUTH_CIPHER_CCMP);
+		}
+		else
+			paramval = IW_AUTH_CIPHER_NONE;
+		break;
+	case IW_AUTH_KEY_MGMT:
+		
+		if ((error = dev_wlc_intvar_get(dev, "wpa_auth", &val)))
+			return error;
+		if (VAL_PSK(val))
+			paramval = IW_AUTH_KEY_MGMT_PSK;
+		else
+			paramval = IW_AUTH_KEY_MGMT_802_1X;
+
+		break;
+	case IW_AUTH_TKIP_COUNTERMEASURES:
+		error = dev_wlc_bufvar_get(dev, "tkip_countermeasures", \
+							(char *)&paramval, sizeof(paramval));
+		if (error)
+			WL_ERROR(("%s get tkip_countermeasures %d\n", __FUNCTION__, error));
+		break;
+
+	case IW_AUTH_DROP_UNENCRYPTED:
+		error = dev_wlc_bufvar_get(dev, "wsec_restrict", \
+					   (char *)&paramval, sizeof(paramval));
+		if (error)
+			WL_ERROR(("%s get wsec_restrict %d\n", __FUNCTION__, error));
+		break;
+
+	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+		error = dev_wlc_bufvar_get(dev, "rx_unencrypted_eapol", \
+						   (char *)&paramval, sizeof(paramval));
+		if (error)
+			WL_ERROR(("%s get rx_unencrypted_eapol %d\n", __FUNCTION__, error));
+		break;
+
+	case IW_AUTH_80211_AUTH_ALG:
+		
+		if ((error = dev_wlc_intvar_get(dev, "auth", &val)))
+			return error;
+		if (!val)
+			paramval = IW_AUTH_ALG_OPEN_SYSTEM;
+		else
+			paramval = IW_AUTH_ALG_SHARED_KEY;
+		break;
+	case IW_AUTH_WPA_ENABLED:
+		if ((error = dev_wlc_intvar_get(dev, "wpa_auth", &val)))
+			return error;
+		if (val)
+			paramval = TRUE;
+		else
+			paramval = FALSE;
+		break;
+#if WIRELESS_EXT > 17
+	case IW_AUTH_ROAMING_CONTROL:
+		WL_ERROR(("%s: IW_AUTH_ROAMING_CONTROL\n", __FUNCTION__));
+		
+		break;
+	case IW_AUTH_PRIVACY_INVOKED:
+		paramval = iw->privacy_invoked;
+		break;
+#endif 
+	}
+	vwrq->value = paramval;
+	return 0;
+}
+#endif
+
+
+#ifdef SOFTAP
+
+static int ap_macmode = MACLIST_MODE_DISABLED;
+static struct mflist ap_black_list;
+static int
+wl_iw_parse_wep(char *keystr, wl_wsec_key_t *key)
+{
+	char hex[] = "XX";
+	unsigned char *data = key->data;
+
+	switch (strlen(keystr)) {
+	case 5:
+	case 13:
+	case 16:
+		key->len = strlen(keystr);
+		memcpy(data, keystr, key->len + 1);
+		break;
+	case 12:
+	case 28:
+	case 34:
+	case 66:
+		if (!strnicmp(keystr, "0x", 2))
+			keystr += 2;
+		else
+			return -1;
+	case 10:
+	case 26:
+	case 32:
+	case 64:
+		key->len = strlen(keystr) / 2;
+		while (*keystr) {
+			strncpy(hex, keystr, 2);
+			*data++ = (char) bcm_strtoul(hex, NULL, 16);
+			keystr += 2;
+		}
+		break;
+	default:
+		return -1;
+	}
+
+	switch (key->len) {
+	case 5:
+		key->algo = CRYPTO_ALGO_WEP1;
+		break;
+	case 13:
+		key->algo = CRYPTO_ALGO_WEP128;
+		break;
+	case 16:
+		key->algo = CRYPTO_ALGO_AES_CCM;
+		break;
+	case 32:
+		key->algo = CRYPTO_ALGO_TKIP;
+		break;
+	default:
+		return -1;
+	}
+
+	key->flags |= WL_PRIMARY_KEY;
+
+	return 0;
+}
+
+#ifdef EXT_WPA_CRYPTO
+#define SHA1HashSize 20
+extern void pbkdf2_sha1(const char *passphrase, const char *ssid, size_t ssid_len,
+				int iterations, u8 *buf, size_t buflen);
+
+#else
+
+#define SHA1HashSize 20
+int pbkdf2_sha1(const char *passphrase, const char *ssid, size_t ssid_len,
+				int iterations, u8 *buf, size_t buflen)
+{
+	WL_ERROR(("WARNING: %s is not implemented !!!\n", __FUNCTION__));
+	return -1;
+}
+
+#endif 
+
+
+int dev_iw_write_cfg1_bss_var(struct net_device *dev, int val)
+{
+	struct {
+		int cfg;
+		int val;
+	} bss_setbuf;
+
+	int bss_set_res;
+	char smbuf[WLC_IOCTL_SMLEN];
+	memset(smbuf, 0, sizeof(smbuf));
+
+	bss_setbuf.cfg = 1;
+	bss_setbuf.val = val;
+
+	bss_set_res = dev_iw_iovar_setbuf(dev, "bss",
+		&bss_setbuf, sizeof(bss_setbuf), smbuf, sizeof(smbuf));
+	WL_TRACE(("%s: bss_set_result:%d set with %d\n", __FUNCTION__, bss_set_res, val));
+
+	return bss_set_res;
+}
+
+
+int dev_iw_read_cfg1_bss_var(struct net_device *dev, int *val)
+{
+	int bsscfg_idx = 1;
+	int bss_set_res;
+	char smbuf[WLC_IOCTL_SMLEN];
+	memset(smbuf, 0, sizeof(smbuf));
+
+	bss_set_res = dev_iw_iovar_getbuf(dev, "bss", \
+		 &bsscfg_idx, sizeof(bsscfg_idx), smbuf, sizeof(smbuf));
+	*val = *(int*)smbuf;
+	*val = dtoh32(*val);
+	WL_TRACE(("%s: status=%d bss_get_result=%d\n", __FUNCTION__, bss_set_res, *val));
+	return bss_set_res;
+}
+
+
+#ifndef AP_ONLY
+static int wl_bssiovar_mkbuf(
+			const char *iovar,
+			int bssidx,
+			void *param,
+			int paramlen,
+			void *bufptr,
+			int buflen,
+			int *perr)
+{
+	const char *prefix = "bsscfg:";
+	int8 *p;
+	uint prefixlen;
+	uint namelen;
+	uint iolen;
+
+	prefixlen = strlen(prefix);
+	namelen = strlen(iovar) + 1;
+	iolen = prefixlen + namelen + sizeof(int) + paramlen;
+
+	if (buflen < 0 || iolen > (uint)buflen) {
+		*perr = BCME_BUFTOOSHORT;
+		return 0;
+	}
+
+	p = (int8 *)bufptr;
+
+	memcpy(p, prefix, prefixlen);
+	p += prefixlen;
+
+	memcpy(p, iovar, namelen);
+	p += namelen;
+
+	bssidx = htod32(bssidx);
+	memcpy(p, &bssidx, sizeof(int32));
+	p += sizeof(int32);
+
+	if (paramlen)
+		memcpy(p, param, paramlen);
+
+	*perr = 0;
+	return iolen;
+}
+#endif 
+
+
+int get_user_params(char *user_params, struct iw_point *dwrq)
+{
+	int ret = 0;
+
+	if (copy_from_user(user_params, dwrq->pointer, dwrq->length)) {
+		WL_ERROR(("\n%s: no user params: uptr:%p, ulen:%d\n",
+			__FUNCTION__, dwrq->pointer, dwrq->length));
+		return -EFAULT;
+	}
+
+	WL_TRACE(("\n%s: iwpriv user params:%s\n", __FUNCTION__, user_params));
+
+	return ret;
+}
+
+
+#define strtoul(nptr, endptr, base) bcm_strtoul((nptr), (endptr), (base))
+
+#if defined(CSCAN)
+
+static int
+wl_iw_combined_scan_set(struct net_device *dev, wlc_ssid_t* ssids_local, int nssid, int nchan)
+{
+	int params_size = WL_SCAN_PARAMS_FIXED_SIZE + WL_NUMCHANNELS * sizeof(uint16);
+	int err = 0;
+	char *p;
+	int i;
+	iscan_info_t *iscan = g_iscan;
+
+	WL_SCAN(("%s nssid=%d nchan=%d\n", __FUNCTION__, nssid, nchan));
+
+	if ((!dev) && (!g_iscan) && (!iscan->iscan_ex_params_p)) {
+		WL_ERROR(("%s error exit\n", __FUNCTION__));
+		err = -1;
+		goto exit;
+	}
+
+#ifdef PNO_SUPPORT
+	if  (dhd_dev_get_pno_status(dev)) {
+		WL_ERROR(("%s: Scan called when PNO is active\n", __FUNCTION__));
+	}
+#endif
+
+	params_size += WL_SCAN_PARAMS_SSID_MAX * sizeof(wlc_ssid_t);
+
+	if (nssid > 0) {
+		i = OFFSETOF(wl_scan_params_t, channel_list) + nchan * sizeof(uint16);
+		i = ROUNDUP(i, sizeof(uint32));
+		if (i + nssid * sizeof(wlc_ssid_t) > params_size) {
+			printf("additional ssids exceed params_size\n");
+			err = -1;
+			goto exit;
+		}
+
+		p = ((char*)&iscan->iscan_ex_params_p->params) + i;
+		memcpy(p, ssids_local, nssid * sizeof(wlc_ssid_t));
+		p += nssid * sizeof(wlc_ssid_t);
+	} else {
+		p = (char*)iscan->iscan_ex_params_p->params.channel_list + nchan * sizeof(uint16);
+	}
+
+	iscan->iscan_ex_params_p->params.channel_num = \
+		htod32((nssid << WL_SCAN_PARAMS_NSSID_SHIFT) | \
+					(nchan & WL_SCAN_PARAMS_COUNT_MASK));
+
+	nssid = \
+	(uint)((iscan->iscan_ex_params_p->params.channel_num >> WL_SCAN_PARAMS_NSSID_SHIFT) & \
+		               WL_SCAN_PARAMS_COUNT_MASK);
+
+	params_size = (int) (p - (char*)iscan->iscan_ex_params_p + nssid * sizeof(wlc_ssid_t));
+	iscan->iscan_ex_param_size = params_size;
+
+	iscan->list_cur = iscan->list_hdr;
+	iscan->iscan_state = ISCAN_STATE_SCANING;
+	wl_iw_set_event_mask(dev);
+	mod_timer(&iscan->timer, jiffies + iscan->timer_ms*HZ/1000);
+
+	iscan->timer_on = 1;
+
+#ifdef SCAN_DUMP
+	{
+		int i;
+		WL_SCAN(("\n### List of SSIDs to scan ###\n"));
+		for (i = 0; i < nssid; i++) {
+			if (!ssids_local[i].SSID_len)
+				WL_SCAN(("%d: Broadcast scan\n", i));
+			else
+			WL_SCAN(("%d: scan  for  %s size =%d\n", i, \
+				ssids_local[i].SSID, ssids_local[i].SSID_len));
+		}
+		WL_SCAN(("### List of channels to scan ###\n"));
+		for (i = 0; i < nchan; i++)
+		{
+			WL_SCAN(("%d ", iscan->iscan_ex_params_p->params.channel_list[i]));
+		}
+		WL_SCAN(("\nnprobes=%d\n", iscan->iscan_ex_params_p->params.nprobes));
+		WL_SCAN(("active_time=%d\n", iscan->iscan_ex_params_p->params.active_time));
+		WL_SCAN(("passive_time=%d\n", iscan->iscan_ex_params_p->params.passive_time));
+		WL_SCAN(("home_time=%d\n", iscan->iscan_ex_params_p->params.home_time));
+		WL_SCAN(("scan_type=%d\n", iscan->iscan_ex_params_p->params.scan_type));
+		WL_SCAN(("\n###################\n"));
+	}
+#endif
+
+	if (params_size > WLC_IOCTL_MEDLEN) {
+			WL_ERROR(("Set ISCAN for %s due to params_size=%d  \n", \
+				__FUNCTION__, params_size));
+			err = -1;
+	}
+
+	if ((err = dev_iw_iovar_setbuf(dev, "iscan", iscan->iscan_ex_params_p, \
+			iscan->iscan_ex_param_size, \
+			iscan->ioctlbuf, sizeof(iscan->ioctlbuf)))) {
+			WL_ERROR(("Set ISCAN for %s failed with %d\n", __FUNCTION__, err));
+			err = -1;
+	}
+
+exit:
+
+	return err;
+}
+
+
+static int iwpriv_set_cscan(struct net_device *dev, struct iw_request_info *info, \
+				union iwreq_data *wrqu, char *ext)
+{
+	int res = 0;
+	char  *extra = NULL;
+	iscan_info_t *iscan = g_iscan;
+	wlc_ssid_t ssids_local[WL_SCAN_PARAMS_SSID_MAX];
+	int nssid = 0;
+	int nchan = 0;
+
+	WL_TRACE(("\%s: info->cmd:%x, info->flags:%x, u.data=0x%p, u.len=%d\n",
+		__FUNCTION__, info->cmd, info->flags,
+		wrqu->data.pointer, wrqu->data.length));
+
+	if (g_onoff == G_WLAN_SET_OFF) {
+		WL_TRACE(("%s: driver is not up yet after START\n", __FUNCTION__));
+		return -1;
+	}
+
+#ifdef PNO_SET_DEBUG
+	wl_iw_set_pno_set(dev, info, wrqu, extra);
+	return 0;
+#endif
+
+	if (wrqu->data.length != 0) {
+
+		char *str_ptr;
+
+		if (!iscan->iscan_ex_params_p) {
+			return -EFAULT;
+		}
+
+		if (!(extra = kmalloc(wrqu->data.length+1, GFP_KERNEL)))
+			return -ENOMEM;
+
+		if (copy_from_user(extra, wrqu->data.pointer, wrqu->data.length)) {
+			kfree(extra);
+			return -EFAULT;
+		}
+
+		extra[wrqu->data.length] = 0;
+		WL_ERROR(("Got str param in iw_point:\n %s\n", extra));
+
+		str_ptr = extra;
+
+		if (strncmp(str_ptr, GET_SSID, strlen(GET_SSID))) {
+			WL_ERROR(("%s Error: extracting SSID='' string\n", __FUNCTION__));
+			goto exit_proc;
+		}
+		str_ptr += strlen(GET_SSID);
+		nssid = wl_iw_parse_ssid_list(&str_ptr, ssids_local, nssid, \
+						WL_SCAN_PARAMS_SSID_MAX);
+		if (nssid == -1) {
+			WL_ERROR(("%s wrong ssid list", __FUNCTION__));
+			return -1;
+		}
+
+		if (iscan->iscan_ex_param_size > WLC_IOCTL_MAXLEN) {
+			WL_ERROR(("%s wrong ex_param_size %d", \
+				__FUNCTION__, iscan->iscan_ex_param_size));
+			return -1;
+		}
+		memset(iscan->iscan_ex_params_p, 0, iscan->iscan_ex_param_size);
+
+		
+		wl_iw_iscan_prep(&iscan->iscan_ex_params_p->params, NULL);
+		iscan->iscan_ex_params_p->version = htod32(ISCAN_REQ_VERSION);
+		iscan->iscan_ex_params_p->action = htod16(WL_SCAN_ACTION_START);
+		iscan->iscan_ex_params_p->scan_duration = htod16(0);
+
+		
+		if ((nchan = wl_iw_parse_channel_list(&str_ptr, \
+					&iscan->iscan_ex_params_p->params.channel_list[0], \
+					WL_NUMCHANNELS)) == -1) {
+			WL_ERROR(("%s missing channel list\n", __FUNCTION__));
+			return -1;
+		}
+
+		
+		get_parmeter_from_string(&str_ptr, \
+				GET_NPROBE, PTYPE_INTDEC, \
+				&iscan->iscan_ex_params_p->params.nprobes, 2);
+
+		get_parmeter_from_string(&str_ptr, GET_ACTIVE_ASSOC_DWELL, PTYPE_INTDEC, \
+						&iscan->iscan_ex_params_p->params.active_time, 4);
+
+		get_parmeter_from_string(&str_ptr, GET_PASSIVE_ASSOC_DWELL, PTYPE_INTDEC, \
+						&iscan->iscan_ex_params_p->params.passive_time, 4);
+
+		get_parmeter_from_string(&str_ptr, GET_HOME_DWELL, PTYPE_INTDEC, \
+					&iscan->iscan_ex_params_p->params.home_time, 4);
+
+		get_parmeter_from_string(&str_ptr, GET_SCAN_TYPE, PTYPE_INTDEC, \
+					&iscan->iscan_ex_params_p->params.scan_type, 1);
+
+		res = wl_iw_combined_scan_set(dev, ssids_local, nssid, nchan);
+
+	} else {
+		  WL_ERROR(("IWPRIV argument len = 0 \n"));
+		  return -1;
+	}
+
+exit_proc:
+
+	kfree(extra);
+
+	return res;
+}
+
+
+static int
+wl_iw_set_cscan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int res = -1;
+	iscan_info_t *iscan = g_iscan;
+	wlc_ssid_t ssids_local[WL_SCAN_PARAMS_SSID_MAX];
+	int nssid = 0;
+	int nchan = 0;
+	cscan_tlv_t *cscan_tlv_temp;
+	char type;
+	char *str_ptr;
+	int tlv_size_left;
+#ifdef TLV_DEBUG
+	int i;
+	char tlv_in_example[] = {			'C', 'S', 'C', 'A', 'N', ' ', \
+							0x53, 0x01, 0x00, 0x00,
+							'S',	  
+							0x00, 
+							'S',    
+							0x04, 
+							'B', 'R', 'C', 'M',
+							'C',
+							0x06, 
+							'P', 
+							0x94,
+							0x11,
+							'T',     
+							0x01  
+							};
+#endif 
+
+	WL_TRACE(("\n### %s: info->cmd:%x, info->flags:%x, u.data=0x%p, u.len=%d\n",
+		__FUNCTION__, info->cmd, info->flags,
+		wrqu->data.pointer, wrqu->data.length));
+
+	net_os_wake_lock(dev);
+
+	if (g_onoff == G_WLAN_SET_OFF) {
+		WL_TRACE(("%s: driver is not up yet after START\n", __FUNCTION__));
+		goto exit_proc;
+	}
+
+
+	if (wrqu->data.length < (strlen(CSCAN_COMMAND) + sizeof(cscan_tlv_t))) {
+		WL_ERROR(("%s aggument=%d  less %d\n", __FUNCTION__, \
+			wrqu->data.length, strlen(CSCAN_COMMAND) + sizeof(cscan_tlv_t)));
+		goto exit_proc;
+	}
+
+#ifdef TLV_DEBUG
+	memcpy(extra, tlv_in_example, sizeof(tlv_in_example));
+	wrqu->data.length = sizeof(tlv_in_example);
+	for (i = 0; i < wrqu->data.length; i++)
+		printf("%02X ", extra[i]);
+	printf("\n");
+#endif 
+
+	str_ptr = extra;
+	str_ptr +=  strlen(CSCAN_COMMAND);
+	tlv_size_left = wrqu->data.length - strlen(CSCAN_COMMAND);
+
+	cscan_tlv_temp = (cscan_tlv_t *)str_ptr;
+	memset(ssids_local, 0, sizeof(ssids_local));
+	
+	if ((cscan_tlv_temp->prefix == CSCAN_TLV_PREFIX) && \
+		(cscan_tlv_temp->version == CSCAN_TLV_VERSION) && \
+		(cscan_tlv_temp->subver == CSCAN_TLV_SUBVERSION))
+	{
+		str_ptr += sizeof(cscan_tlv_t);
+		tlv_size_left  -= sizeof(cscan_tlv_t);
+
+		
+		if ((nssid = wl_iw_parse_ssid_list_tlv(&str_ptr, ssids_local, \
+				WL_SCAN_PARAMS_SSID_MAX, &tlv_size_left)) <= 0) {
+			WL_ERROR(("SSID is not presented or corrupted ret=%d\n", nssid));
+			goto exit_proc;
+		}
+		else {
+			
+			memset(iscan->iscan_ex_params_p, 0, iscan->iscan_ex_param_size);
+
+			
+			wl_iw_iscan_prep(&iscan->iscan_ex_params_p->params, NULL);
+			iscan->iscan_ex_params_p->version = htod32(ISCAN_REQ_VERSION);
+			iscan->iscan_ex_params_p->action = htod16(WL_SCAN_ACTION_START);
+			iscan->iscan_ex_params_p->scan_duration = htod16(0);
+
+			
+			while (tlv_size_left > 0)
+			{
+			type = str_ptr[0];
+			switch (type) {
+				case CSCAN_TLV_TYPE_CHANNEL_IE:
+					
+					if ((nchan = wl_iw_parse_channel_list_tlv(&str_ptr, \
+					&iscan->iscan_ex_params_p->params.channel_list[0], \
+					WL_NUMCHANNELS, &tlv_size_left)) == -1) {
+					WL_ERROR(("%s missing channel list\n", \
+						 __FUNCTION__));
+						goto exit_proc;
+					}
+				break;
+				case CSCAN_TLV_TYPE_NPROBE_IE:
+					if ((res = wl_iw_parse_data_tlv(&str_ptr, \
+						&iscan->iscan_ex_params_p->params.nprobes, \
+						sizeof(iscan->iscan_ex_params_p->params.nprobes), \
+						type, sizeof(char), &tlv_size_left)) == -1) {
+						WL_ERROR(("%s return %d\n", \
+							__FUNCTION__, res));
+							goto exit_proc;
+					}
+				break;
+				case CSCAN_TLV_TYPE_ACTIVE_IE:
+					if ((res = wl_iw_parse_data_tlv(&str_ptr, \
+					&iscan->iscan_ex_params_p->params.active_time, \
+					sizeof(iscan->iscan_ex_params_p->params.active_time), \
+					type, sizeof(short), &tlv_size_left)) == -1) {
+						WL_ERROR(("%s return %d\n", \
+						__FUNCTION__, res));
+						goto exit_proc;
+					}
+				break;
+				case CSCAN_TLV_TYPE_PASSIVE_IE:
+					if ((res = wl_iw_parse_data_tlv(&str_ptr, \
+					&iscan->iscan_ex_params_p->params.passive_time, \
+					sizeof(iscan->iscan_ex_params_p->params.passive_time), \
+					type, sizeof(short), &tlv_size_left)) == -1) {
+						WL_ERROR(("%s return %d\n", \
+						__FUNCTION__, res));
+						goto exit_proc;
+					}
+				break;
+				case CSCAN_TLV_TYPE_HOME_IE:
+					if ((res = wl_iw_parse_data_tlv(&str_ptr, \
+					&iscan->iscan_ex_params_p->params.home_time, \
+					sizeof(iscan->iscan_ex_params_p->params.home_time), \
+					type, sizeof(short), &tlv_size_left)) == -1) {
+						WL_ERROR(("%s return %d\n", \
+						__FUNCTION__, res));
+						goto exit_proc;
+					}
+				break;
+				case CSCAN_TLV_TYPE_STYPE_IE:
+					if ((res = wl_iw_parse_data_tlv(&str_ptr, \
+					&iscan->iscan_ex_params_p->params.scan_type, \
+					sizeof(iscan->iscan_ex_params_p->params.scan_type), \
+					type, sizeof(char), &tlv_size_left)) == -1) {
+					WL_ERROR(("%s return %d\n", \
+						__FUNCTION__, res));
+						goto exit_proc;
+					}
+				break;
+
+				default :
+					WL_ERROR(("%s get unkwown type %X\n", \
+						__FUNCTION__, type));
+					goto exit_proc;
+				break;
+				}
+			} 
+			}
+		}
+		else {
+			WL_ERROR(("%s get wrong TLV command\n", __FUNCTION__));
+			goto exit_proc;
+		}
+
+#if defined(CONFIG_FIRST_SCAN)
+		if (g_first_broadcast_scan < BROADCAST_SCAN_FIRST_RESULT_CONSUMED) {
+			if (++g_first_counter_scans == MAX_ALLOWED_BLOCK_SCAN_FROM_FIRST_SCAN) {
+
+				WL_ERROR(("%s Clean up First scan flag which is %d\n", \
+						 __FUNCTION__, g_first_broadcast_scan));
+				g_first_broadcast_scan = BROADCAST_SCAN_FIRST_RESULT_CONSUMED;
+			}
+			else {
+				WL_ERROR(("%s Ignoring CSCAN : First Scan is not done yet %d\n", \
+						__FUNCTION__, g_first_counter_scans));
+				res = -EBUSY;
+				goto exit_proc;
+			}
+		}
+#endif
+
+		res = wl_iw_combined_scan_set(dev, ssids_local, nssid, nchan);
+
+exit_proc:
+	net_os_wake_unlock(dev);
+	return res;
+}
+
+#endif 
+
+#ifdef SOFTAP
+#ifndef AP_ONLY
+
+static int thr_wait_for_2nd_eth_dev(void *data)
+{
+	struct net_device *dev = (struct net_device *)data;
+	wl_iw_t *iw;
+	int ret = 0;
+	unsigned long flags;
+
+	net_os_wake_lock(dev);
+
+	DAEMONIZE("wl0_eth_wthread");
+
+	WL_TRACE(("\n>%s thread started:, PID:%x\n", __FUNCTION__, current->pid));
+	iw = *(wl_iw_t **)netdev_priv(dev);
+	if (!iw) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		ret = -1;
+		goto fail;
+	}
+
+#ifndef BCMSDIOH_STD
+	if (down_timeout(&ap_eth_sema,  msecs_to_jiffies(5000)) != 0) {
+		WL_ERROR(("\n%s: sap_eth_sema timeout \n", __FUNCTION__));
+		ret = -1;
+		goto fail;
+	}
+#endif
+
+	flags = dhd_os_spin_lock(iw->pub);
+	if (!ap_net_dev) {
+		WL_ERROR((" ap_net_dev is null !!!"));
+		ret = -1;
+		dhd_os_spin_unlock(iw->pub, flags);
+		goto fail;
+	}
+
+	WL_TRACE(("\n>%s: Thread:'softap ethdev IF:%s is detected !!!'\n\n",
+		__FUNCTION__, ap_net_dev->name));
+
+	ap_cfg_running = TRUE;
+
+	dhd_os_spin_unlock(iw->pub, flags);
+
+	bcm_mdelay(500);
+
+	wl_iw_send_priv_event(priv_dev, "AP_SET_CFG_OK");
+
+fail:
+	WL_TRACE(("\n>%s, thread completed\n", __FUNCTION__));
+
+	net_os_wake_unlock(dev);
+
+	complete_and_exit(&ap_cfg_exited, 0);
+	return ret;
+}
+#endif 
+#ifndef AP_ONLY
+static int last_auto_channel = 6;
+#endif
+static int get_softap_auto_channel(struct net_device *dev, struct ap_profile *ap)
+{
+	int chosen = 0;
+	wl_uint32_list_t request;
+	int rescan = 0;
+	int retry = 0;
+	int updown = 0;
+	int ret = 0;
+	wlc_ssid_t null_ssid;
+	int res = 0;
+#ifndef AP_ONLY
+	int iolen = 0;
+	int mkvar_err = 0;
+	int bsscfg_index = 1;
+	char buf[WLC_IOCTL_SMLEN];
+#endif
+	WL_SOFTAP(("Enter %s\n", __FUNCTION__));
+
+#ifndef AP_ONLY
+	if (ap_cfg_running) {
+		ap->channel = last_auto_channel;
+		return res;
+	}
+#endif
+	memset(&null_ssid, 0, sizeof(wlc_ssid_t));
+	res |= dev_wlc_ioctl(dev, WLC_UP, &updown, sizeof(updown));
+#ifdef AP_ONLY
+	res |= dev_wlc_ioctl(dev, WLC_SET_SSID, &null_ssid, sizeof(null_ssid));
+#else
+	iolen = wl_bssiovar_mkbuf("ssid", bsscfg_index, (char *)(&null_ssid), \
+		null_ssid.SSID_len+4, buf, sizeof(buf), &mkvar_err);
+	ASSERT(iolen);
+	res |= dev_wlc_ioctl(dev, WLC_SET_VAR, buf, iolen);
+#endif
+	auto_channel_retry:
+			request.count = htod32(0);
+			ret = dev_wlc_ioctl(dev, WLC_START_CHANNEL_SEL, &request, sizeof(request));
+			if (ret < 0) {
+				WL_ERROR(("can't start auto channel scan\n"));
+				goto fail;
+			}
+
+	get_channel_retry:
+			bcm_mdelay(500);
+
+			ret = dev_wlc_ioctl(dev, WLC_GET_CHANNEL_SEL, &chosen, sizeof(chosen));
+			if (ret < 0 || dtoh32(chosen) == 0) {
+				if (retry++ < 3)
+					goto get_channel_retry;
+				else {
+					WL_ERROR(("can't get auto channel sel, err = %d, \
+						chosen = %d\n", ret, chosen));
+					goto fail;
+				}
+			}
+			if ((chosen == 1) && (!rescan++))
+				goto auto_channel_retry;
+			WL_SOFTAP(("Set auto channel = %d\n", chosen));
+			ap->channel = chosen;
+			if ((res = dev_wlc_ioctl(dev, WLC_DOWN, &updown, sizeof(updown))) < 0) {
+				WL_ERROR(("%s fail to set up err =%d\n", __FUNCTION__, res));
+				goto fail;
+			}
+#ifndef AP_ONLY
+	if (!res)
+		last_auto_channel = ap->channel;
+#endif
+
+fail :
+	return res;
+}
+
+
+static int set_ap_cfg(struct net_device *dev, struct ap_profile *ap)
+{
+	int updown = 0;
+	int channel = 0;
+
+	wlc_ssid_t ap_ssid;
+	int max_assoc = 8;
+
+	int res = 0;
+	int apsta_var = 0;
+#ifndef AP_ONLY
+	int mpc = 0;
+	int iolen = 0;
+	int mkvar_err = 0;
+	int bsscfg_index = 1;
+	char buf[WLC_IOCTL_SMLEN];
+#endif
+
+	if (!dev) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return -1;
+	}
+
+	net_os_wake_lock(dev);
+
+	WL_SOFTAP(("wl_iw: set ap profile:\n"));
+	WL_SOFTAP(("	ssid = '%s'\n", ap->ssid));
+	WL_SOFTAP(("	security = '%s'\n", ap->sec));
+	if (ap->key[0] != '\0')
+		WL_SOFTAP(("	key = '%s'\n", ap->key));
+	WL_SOFTAP(("	channel = %d\n", ap->channel));
+	WL_SOFTAP(("	max scb = %d\n", ap->max_scb));
+
+#ifdef AP_ONLY
+	if (ap_cfg_running) {
+		wl_iw_softap_deassoc_stations(dev, NULL);
+		ap_cfg_running = FALSE;
+	}
+#endif
+
+	if (ap_cfg_running == FALSE) {
+
+#ifndef AP_ONLY
+		sema_init(&ap_eth_sema, 0);
+
+		mpc = 0;
+		if ((res = dev_wlc_intvar_set(dev, "mpc", mpc))) {
+			WL_ERROR(("%s fail to set mpc\n", __FUNCTION__));
+			goto fail;
+		}
+#endif
+
+		updown = 0;
+		if ((res = dev_wlc_ioctl(dev, WLC_DOWN, &updown, sizeof(updown)))) {
+			WL_ERROR(("%s fail to set updown\n", __FUNCTION__));
+			goto fail;
+		}
+
+#ifdef AP_ONLY
+		apsta_var = 0;
+		if ((res = dev_wlc_ioctl(dev, WLC_SET_AP, &apsta_var, sizeof(apsta_var)))) {
+			WL_ERROR(("%s fail to set apsta_var 0\n", __FUNCTION__));
+			goto fail;
+		}
+		apsta_var = 1;
+		if ((res = dev_wlc_ioctl(dev, WLC_SET_AP, &apsta_var, sizeof(apsta_var)))) {
+			WL_ERROR(("%s fail to set apsta_var 1\n", __FUNCTION__));
+			goto fail;
+		}
+		res = dev_wlc_ioctl(dev, WLC_GET_AP, &apsta_var, sizeof(apsta_var));
+#else
+		apsta_var = 1;
+		iolen = wl_bssiovar_mkbuf("apsta",
+			bsscfg_index,  &apsta_var, sizeof(apsta_var)+4,
+			buf, sizeof(buf), &mkvar_err);
+
+		if (iolen <= 0)
+			goto fail;
+
+		if ((res = dev_wlc_ioctl(dev, WLC_SET_VAR, buf, iolen)) < 0) {
+			WL_ERROR(("%s fail to set apsta \n", __FUNCTION__));
+			goto fail;
+		}
+		WL_TRACE(("\n>in %s: apsta set result: %d \n", __FUNCTION__, res));
+#endif
+
+		updown = 1;
+		if ((res = dev_wlc_ioctl(dev, WLC_UP, &updown, sizeof(updown))) < 0) {
+			WL_ERROR(("%s fail to set apsta \n", __FUNCTION__));
+			goto fail;
+		}
+
+	} else {
+		
+		if (!ap_net_dev) {
+			WL_ERROR(("%s: ap_net_dev is null\n", __FUNCTION__));
+			goto fail;
+		}
+
+		res = wl_iw_softap_deassoc_stations(ap_net_dev, NULL);
+
+		
+		if ((res = dev_iw_write_cfg1_bss_var(dev, 0)) < 0) {
+			WL_ERROR(("%s fail to set bss down\n", __FUNCTION__));
+			goto fail;
+		}
+	}
+
+	if (strlen(ap->country_code)) {
+		WL_ERROR(("%s: Igonored: Country MUST be specified \
+				  COUNTRY command with \n",	__FUNCTION__));
+	} else {
+		WL_SOFTAP(("%s: Country code is not specified,"
+			" will use Radio's default\n",
+			__FUNCTION__));
+	}
+
+	iolen = wl_bssiovar_mkbuf("closednet",
+		bsscfg_index,  &ap->closednet, sizeof(ap->closednet)+4,
+		buf, sizeof(buf), &mkvar_err);
+	ASSERT(iolen);
+	if ((res = dev_wlc_ioctl(dev, WLC_SET_VAR, buf, iolen)) < 0) {
+		WL_ERROR(("%s failed to set 'closednet'for apsta \n", __FUNCTION__));
+		goto fail;
+	}
+
+	
+	if ((ap->channel == 0) && (get_softap_auto_channel(dev, ap) < 0)) {
+		ap->channel = 1;
+		WL_ERROR(("%s auto channel failed, pick up channel=%d\n", \
+			__FUNCTION__, ap->channel));
+	}
+
+	channel = ap->channel;
+	if ((res = dev_wlc_ioctl(dev, WLC_SET_CHANNEL, &channel, sizeof(channel)))) {
+			WL_ERROR(("%s fail to set channel\n", __FUNCTION__));
+			goto fail;
+	}
+
+	if (ap_cfg_running == FALSE) {
+		updown = 0;
+		if ((res = dev_wlc_ioctl(dev, WLC_UP, &updown, sizeof(updown)))) {
+			WL_ERROR(("%s fail to set up\n", __FUNCTION__));
+			goto fail;
+		}
+	}
+
+	max_assoc = ap->max_scb;
+	if ((res = dev_wlc_intvar_set(dev, "maxassoc", max_assoc))) {
+			WL_ERROR(("%s fail to set maxassoc\n", __FUNCTION__));
+			goto fail;
+	}
+
+	ap_ssid.SSID_len = strlen(ap->ssid);
+	strncpy(ap_ssid.SSID, ap->ssid, ap_ssid.SSID_len);
+
+#ifdef AP_ONLY
+	if ((res = wl_iw_set_ap_security(dev, &my_ap)) != 0) {
+		WL_ERROR(("ERROR:%d in:%s, wl_iw_set_ap_security is skipped\n", \
+		res, __FUNCTION__));
+		goto fail;
+	}
+	wl_iw_send_priv_event(dev, "ASCII_CMD=AP_BSS_START");
+	ap_cfg_running = TRUE;
+#else
+	iolen = wl_bssiovar_mkbuf("ssid", bsscfg_index, (char *)(&ap_ssid),
+		ap_ssid.SSID_len+4, buf, sizeof(buf), &mkvar_err);
+	ASSERT(iolen);
+	if ((res = dev_wlc_ioctl(dev, WLC_SET_VAR, buf, iolen)) != 0) {
+		WL_ERROR(("ERROR:%d in:%s, Security & BSS reconfiguration is skipped\n", \
+		res, __FUNCTION__));
+		goto fail;
+	}
+	if (ap_cfg_running == FALSE) {
+		init_completion(&ap_cfg_exited);
+		ap_cfg_pid = kernel_thread(thr_wait_for_2nd_eth_dev, dev, 0);
+	} else {
+		ap_cfg_pid = -1;
+		if (ap_net_dev == NULL) {
+			WL_ERROR(("%s ERROR: ap_net_dev is NULL !!!\n", __FUNCTION__));
+			goto fail;
+		}
+
+		WL_ERROR(("%s: %s Configure security & restart AP bss \n", \
+			 __FUNCTION__, ap_net_dev->name));
+
+		if ((res = wl_iw_set_ap_security(ap_net_dev, &my_ap)) < 0) {
+			WL_ERROR(("%s fail to set security : %d\n", __FUNCTION__, res));
+			goto fail;
+		}
+
+		if ((res = dev_iw_write_cfg1_bss_var(dev, 1)) < 0) {
+			WL_ERROR(("%s fail to set bss up\n", __FUNCTION__));
+			goto fail;
+		}
+	}
+#endif 
+fail:
+	WL_SOFTAP(("%s exit with %d\n", __FUNCTION__, res));
+
+	net_os_wake_unlock(dev);
+
+	return res;
+}
+
+
+static int wl_iw_set_ap_security(struct net_device *dev, struct ap_profile *ap)
+{
+	int wsec = 0;
+	int wpa_auth = 0;
+	int res = 0;
+	int i;
+	char *ptr;
+#ifdef AP_ONLY
+	int mpc = 0;
+	wlc_ssid_t ap_ssid;
+#endif
+	wl_wsec_key_t key;
+
+	WL_SOFTAP(("\nsetting SOFTAP security mode:\n"));
+	WL_SOFTAP(("wl_iw: set ap profile:\n"));
+	WL_SOFTAP(("	ssid = '%s'\n", ap->ssid));
+	WL_SOFTAP(("	security = '%s'\n", ap->sec));
+	if (ap->key[0] != '\0') {
+		WL_SOFTAP(("	key = '%s'\n", ap->key));
+	}
+	WL_SOFTAP(("	channel = %d\n", ap->channel));
+	WL_SOFTAP(("	max scb = %d\n", ap->max_scb));
+
+	if (strnicmp(ap->sec, "open", strlen("open")) == 0) {
+		wsec = 0;
+		res = dev_wlc_intvar_set(dev, "wsec", wsec);
+		wpa_auth = WPA_AUTH_DISABLED;
+		res |= dev_wlc_intvar_set(dev, "wpa_auth", wpa_auth);
+
+		WL_SOFTAP(("=====================\n"));
+		WL_SOFTAP((" wsec & wpa_auth set 'OPEN', result:&d %d\n", res));
+		WL_SOFTAP(("=====================\n"));
+
+	} else if (strnicmp(ap->sec, "wep", strlen("wep")) == 0) {
+
+		memset(&key, 0, sizeof(key));
+
+		wsec = WEP_ENABLED;
+		res = dev_wlc_intvar_set(dev, "wsec", wsec);
+
+		key.index = 0;
+		if (wl_iw_parse_wep(ap->key, &key)) {
+			WL_SOFTAP(("wep key parse err!\n"));
+			return -1;
+		}
+
+		key.index = htod32(key.index);
+		key.len = htod32(key.len);
+		key.algo = htod32(key.algo);
+		key.flags = htod32(key.flags);
+
+		res |= dev_wlc_ioctl(dev, WLC_SET_KEY, &key, sizeof(key));
+
+		wpa_auth = WPA_AUTH_DISABLED;
+		res |= dev_wlc_intvar_set(dev, "wpa_auth", wpa_auth);
+
+		WL_SOFTAP(("=====================\n"));
+		WL_SOFTAP((" wsec & auth set 'WEP', result:&d %d\n", res));
+		WL_SOFTAP(("=====================\n"));
+
+	} else if (strnicmp(ap->sec, "wpa2-psk", strlen("wpa2-psk")) == 0) {
+		wsec_pmk_t psk;
+		size_t key_len;
+
+		wsec = AES_ENABLED;
+		dev_wlc_intvar_set(dev, "wsec", wsec);
+
+		key_len = strlen(ap->key);
+		if (key_len < WSEC_MIN_PSK_LEN || key_len > WSEC_MAX_PSK_LEN) {
+			WL_SOFTAP(("passphrase must be between %d and %d characters long\n",
+			WSEC_MIN_PSK_LEN, WSEC_MAX_PSK_LEN));
+			return -1;
+		}
+
+		if (key_len < WSEC_MAX_PSK_LEN) {
+			unsigned char output[2*SHA1HashSize];
+			char key_str_buf[WSEC_MAX_PSK_LEN+1];
+
+			memset(output, 0, sizeof(output));
+			pbkdf2_sha1(ap->key, ap->ssid, strlen(ap->ssid), 4096, output, 32);
+
+			ptr = key_str_buf;
+			for (i = 0; i < (WSEC_MAX_PSK_LEN/8); i++) {
+				sprintf(ptr, "%02x%02x%02x%02x", (uint)output[i*4], \
+					 (uint)output[i*4+1], (uint)output[i*4+2], \
+					 (uint)output[i*4+3]);
+				ptr += 8;
+			}
+			WL_SOFTAP(("%s: passphase = %s\n", __FUNCTION__, key_str_buf));
+
+			psk.key_len = htod16((ushort)WSEC_MAX_PSK_LEN);
+			memcpy(psk.key, key_str_buf, psk.key_len);
+		} else {
+			psk.key_len = htod16((ushort) key_len);
+			memcpy(psk.key, ap->key, key_len);
+		}
+		psk.flags = htod16(WSEC_PASSPHRASE);
+		dev_wlc_ioctl(dev, WLC_SET_WSEC_PMK, &psk, sizeof(psk));
+
+		wpa_auth = WPA2_AUTH_PSK;
+		dev_wlc_intvar_set(dev, "wpa_auth", wpa_auth);
+
+	} else if (strnicmp(ap->sec, "wpa-psk", strlen("wpa-psk")) == 0) {
+
+		wsec_pmk_t psk;
+		size_t key_len;
+
+		wsec = TKIP_ENABLED;
+		res = dev_wlc_intvar_set(dev, "wsec", wsec);
+
+		key_len = strlen(ap->key);
+		if (key_len < WSEC_MIN_PSK_LEN || key_len > WSEC_MAX_PSK_LEN) {
+			WL_SOFTAP(("passphrase must be between %d and %d characters long\n",
+			WSEC_MIN_PSK_LEN, WSEC_MAX_PSK_LEN));
+			return -1;
+		}
+
+		if (key_len < WSEC_MAX_PSK_LEN) {
+			unsigned char output[2*SHA1HashSize];
+			char key_str_buf[WSEC_MAX_PSK_LEN+1];
+			bzero(output, 2*SHA1HashSize);
+
+			WL_SOFTAP(("%s: do passhash...\n", __FUNCTION__));
+
+			pbkdf2_sha1(ap->key, ap->ssid, strlen(ap->ssid), 4096, output, 32);
+
+			ptr = key_str_buf;
+			for (i = 0; i < (WSEC_MAX_PSK_LEN/8); i++) {
+				WL_SOFTAP(("[%02d]: %08x\n", i, *((unsigned int *)&output[i*4])));
+
+				sprintf(ptr, "%02x%02x%02x%02x", (uint)output[i*4],
+					(uint)output[i*4+1], (uint)output[i*4+2],
+					(uint)output[i*4+3]);
+				ptr += 8;
+			}
+			WL_SOFTAP(("%s: passphase = %s\n", __FUNCTION__, key_str_buf));
+
+			psk.key_len = htod16((ushort)WSEC_MAX_PSK_LEN);
+			memcpy(psk.key, key_str_buf, psk.key_len);
+		} else {
+			psk.key_len = htod16((ushort) key_len);
+			memcpy(psk.key, ap->key, key_len);
+		}
+
+		psk.flags = htod16(WSEC_PASSPHRASE);
+		res |= dev_wlc_ioctl(dev, WLC_SET_WSEC_PMK, &psk, sizeof(psk));
+
+		wpa_auth = WPA_AUTH_PSK;
+		res |= dev_wlc_intvar_set(dev, "wpa_auth", wpa_auth);
+
+		WL_SOFTAP((" wsec & auth set 'wpa-psk' (TKIP), result:&d %d\n", res));
+	}
+
+#ifdef AP_ONLY
+		ap_ssid.SSID_len = strlen(ap->ssid);
+		strncpy(ap_ssid.SSID, ap->ssid, ap_ssid.SSID_len);
+		res |= dev_wlc_ioctl(dev, WLC_SET_SSID, &ap_ssid, sizeof(ap_ssid));
+		mpc = 0;
+		res |= dev_wlc_intvar_set(dev, "mpc", mpc);
+		if (strnicmp(ap->sec, "wep", strlen("wep")) == 0) {
+			res |= dev_wlc_ioctl(dev, WLC_SET_KEY, &key, sizeof(key));
+		}
+#endif
+	return res;
+}
+
+
+
+int get_parmeter_from_string(
+			char **str_ptr, const char *token,
+			int param_type, void  *dst, int param_max_len)
+{
+	char int_str[7] = "0";
+	int parm_str_len;
+	char  *param_str_begin;
+	char  *param_str_end;
+
+	if ((*str_ptr) && !strncmp(*str_ptr, token, strlen(token))) {
+
+		strsep(str_ptr, "=,");
+		param_str_begin = *str_ptr;
+		strsep(str_ptr, "=,");
+
+		if (*str_ptr == NULL) {
+			parm_str_len = strlen(param_str_begin);
+		} else {
+			param_str_end = *str_ptr-1;
+			parm_str_len = param_str_end - param_str_begin;
+		}
+
+		WL_TRACE((" 'token:%s', len:%d, ", token, parm_str_len));
+
+		if (parm_str_len > param_max_len) {
+			WL_TRACE((" WARNING: extracted param len:%d is > MAX:%d\n",
+				parm_str_len, param_max_len));
+
+			parm_str_len = param_max_len;
+		}
+
+		switch (param_type) {
+
+		case PTYPE_INTDEC: {
+			int *pdst_int = dst;
+			char *eptr;
+
+			if (parm_str_len > sizeof(int_str))
+				 parm_str_len = sizeof(int_str);
+
+			memcpy(int_str, param_str_begin, parm_str_len);
+
+			*pdst_int = simple_strtoul(int_str, &eptr, 10);
+
+			WL_TRACE((" written as integer:%d\n",  *pdst_int));
+			}
+			break;
+		case PTYPE_STR_HEX: {
+			u8 *buf = dst;
+
+			param_max_len = param_max_len >> 1;
+			hstr_2_buf(param_str_begin, buf, param_max_len);
+			print_buf(buf, param_max_len, 0);
+			}
+			break;
+		default:
+			memcpy(dst, param_str_begin, parm_str_len);
+			*((char *)dst + parm_str_len) = 0;
+			WL_TRACE((" written as a string:%s\n", (char *)dst));
+			break;
+		}
+
+		return 0;
+	} else {
+		WL_ERROR(("\n %s: No token:%s in str:%s\n",
+			__FUNCTION__, token, *str_ptr));
+
+		return -1;
+	}
+}
+
+static int wl_iw_softap_deassoc_stations(struct net_device *dev, u8 *mac)
+{
+	int i;
+	int res = 0;
+	char mac_buf[128] = {0};
+	char z_mac[6] = {0, 0, 0, 0, 0, 0};
+	char *sta_mac;
+	struct maclist *assoc_maclist = (struct maclist *) mac_buf;
+	bool deauth_all = false;
+
+	if (mac == NULL) {
+		deauth_all = true;
+		sta_mac = z_mac;
+	} else {
+		sta_mac = mac;
+	}
+
+	memset(assoc_maclist, 0, sizeof(mac_buf));
+	assoc_maclist->count = 8;
+
+	res = dev_wlc_ioctl(dev, WLC_GET_ASSOCLIST, assoc_maclist, 128);
+	if (res != 0) {
+		WL_SOFTAP(("%s: Error:%d Couldn't get ASSOC List\n", __FUNCTION__, res));
+		return res;
+	}
+
+	if (assoc_maclist->count) {
+		for (i = 0; i < assoc_maclist->count; i++) {
+			scb_val_t scbval;
+
+			scbval.val = htod32(1);
+			bcopy(&assoc_maclist->ea[i], &scbval.ea, ETHER_ADDR_LEN);
+
+			if (deauth_all || (memcmp(&scbval.ea, sta_mac, ETHER_ADDR_LEN) == 0)) {
+				WL_SOFTAP(("%s, deauth STA:%d \n", __FUNCTION__, i));
+				res |= dev_wlc_ioctl(dev, WLC_SCB_DEAUTHENTICATE_FOR_REASON,
+					&scbval, sizeof(scb_val_t));
+			}
+		}
+	} else {
+		WL_SOFTAP((" STA ASSOC list is empty\n"));
+	}
+
+	if (res != 0) {
+		WL_ERROR(("%s: Error:%d\n", __FUNCTION__, res));
+	} else if (assoc_maclist->count) {
+		bcm_mdelay(200);
+	}
+	return res;
+}
+
+
+static int iwpriv_softap_stop(struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *ext)
+{
+	int res = 0;
+
+	WL_SOFTAP(("got iwpriv AP_BSS_STOP\n"));
+
+	if ((!dev) && (!ap_net_dev)) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return res;
+	}
+
+	net_os_wake_lock(dev);
+
+	if ((ap_cfg_running == TRUE)) {
+#ifdef AP_ONLY
+		wl_iw_softap_deassoc_stations(dev, NULL);
+#else
+		wl_iw_softap_deassoc_stations(ap_net_dev, NULL);
+
+		if ((res = dev_iw_write_cfg1_bss_var(dev, 2)) < 0)
+			WL_ERROR(("%s failed to del BSS err = %d", __FUNCTION__, res));
+#endif
+
+		bcm_mdelay(100);
+
+		wrqu->data.length = 0;
+		ap_cfg_running = FALSE;
+	}
+	else
+		WL_ERROR(("%s: was called when SoftAP is OFF : move on\n", __FUNCTION__));
+
+	WL_SOFTAP(("%s Done with %d\n", __FUNCTION__, res));
+
+	net_os_wake_unlock(dev);
+
+	return res;
+}
+
+
+static int iwpriv_fw_reload(struct net_device *dev,
+		struct iw_request_info *info,
+		union iwreq_data *wrqu,
+		char *ext)
+{
+	int ret = -1;
+	char extra[256];
+	char *fwstr = fw_path;
+
+	WL_SOFTAP(("current firmware_path[]=%s\n", fwstr));
+
+	WL_TRACE((">Got FW_RELOAD cmd:"
+				"info->cmd:%x, info->flags:%x, u.data:%p, u.len:%d, \
+				fw_path:%p, len:%d \n",
+				info->cmd, info->flags,
+				wrqu->data.pointer, wrqu->data.length, fwstr, strlen(fwstr)));
+
+	if ((wrqu->data.length > 4) && (wrqu->data.length < sizeof(extra))) {
+
+		char *str_ptr;
+
+		if (copy_from_user(extra, wrqu->data.pointer, wrqu->data.length)) {
+			ret = -EFAULT;
+			goto exit_proc;
+		}
+
+		extra[wrqu->data.length] = 8;
+		str_ptr = extra;
+
+		if (get_parmeter_from_string(&str_ptr, "FW_PATH=", PTYPE_STRING, fwstr, 255) != 0) {
+			WL_ERROR(("Error: extracting FW_PATH='' string\n"));
+			goto exit_proc;
+		}
+
+		if (strstr(fwstr, "apsta") != NULL) {
+			WL_SOFTAP(("GOT APSTA FIRMWARE\n"));
+			ap_fw_loaded = TRUE;
+		} else {
+			WL_SOFTAP(("GOT STA FIRMWARE\n"));
+			ap_fw_loaded = FALSE;
+		}
+
+		WL_SOFTAP(("SET firmware_path[]=%s , str_p:%p\n", fwstr, fwstr));
+		ret = 0;
+	} else {
+		WL_ERROR(("Error: ivalid param len:%d\n", wrqu->data.length));
+	}
+
+exit_proc:
+	return ret;
+}
+#endif
+
+#ifdef SOFTAP
+static int iwpriv_wpasupp_loop_tst(struct net_device *dev,
+		struct iw_request_info *info,
+		union iwreq_data *wrqu,
+		char *ext)
+{
+	int res = 0;
+	char  *params = NULL;
+
+	WL_TRACE((">Got IWPRIV  wp_supp loopback cmd test:"
+				"info->cmd:%x, info->flags:%x, u.data:%p, u.len:%d\n",
+				info->cmd, info->flags,
+				wrqu->data.pointer, wrqu->data.length));
+
+	if (wrqu->data.length != 0) {
+
+		if (!(params = kmalloc(wrqu->data.length+1, GFP_KERNEL)))
+			return -ENOMEM;
+
+		if (copy_from_user(params, wrqu->data.pointer, wrqu->data.length)) {
+			kfree(params);
+			return -EFAULT;
+		}
+
+		params[wrqu->data.length] = 0;
+		WL_SOFTAP(("\n>> copied from user:\n %s\n", params));
+	} else {
+		WL_ERROR(("ERROR param length is 0\n"));
+		return -EFAULT;
+	}
+
+	res = wl_iw_send_priv_event(dev, params);
+	kfree(params);
+
+	return res;
+}
+#endif
+
+
+static int
+iwpriv_en_ap_bss(
+		struct net_device *dev,
+		struct iw_request_info *info,
+		void *wrqu,
+		char *extra)
+{
+	int res = 0;
+
+	if (!dev) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return -1;
+	}
+
+	net_os_wake_lock(dev);
+
+	WL_SOFTAP(("%s: rcvd IWPRIV IOCTL:  for dev:%s\n", __FUNCTION__, dev->name));
+
+#ifndef AP_ONLY
+	if (ap_cfg_pid >= 0) {
+		wait_for_completion(&ap_cfg_exited);
+		ap_cfg_pid = -1;
+	}
+
+	if ((res = wl_iw_set_ap_security(dev, &my_ap)) != 0) {
+		WL_ERROR((" %s ERROR setting SOFTAP security in :%d\n", __FUNCTION__, res));
+	}
+	else {
+		if ((res = dev_iw_write_cfg1_bss_var(dev, 1)) < 0)
+			WL_ERROR(("%s fail to set bss up err=%d\n", __FUNCTION__, res));
+		else
+			bcm_mdelay(100);
+	}
+
+#endif 
+	WL_SOFTAP(("%s done with res %d \n", __FUNCTION__, res));
+
+	net_os_wake_unlock(dev);
+
+	return res;
+}
+
+static int
+get_assoc_sta_list(struct net_device *dev, char *buf, int len)
+{
+	WL_TRACE(("%s: dev_wlc_ioctl(dev:%p, cmd:%d, buf:%p, len:%d)\n",
+		__FUNCTION__, dev, WLC_GET_ASSOCLIST, buf, len));
+
+	return dev_wlc_ioctl(dev, WLC_GET_ASSOCLIST, buf, len);
+
+}
+
+
+void check_error(int res, const char *msg, const char *func, int line)
+{
+	if (res != 0)
+		WL_ERROR(("%s, %d function:%s, line:%d\n", msg, res, func, line));
+}
+
+static int
+set_ap_mac_list(struct net_device *dev, void *buf)
+{
+	struct mac_list_set *mac_list_set = (struct mac_list_set *)buf;
+	struct maclist *maclist = (struct maclist *)&mac_list_set->mac_list;
+	int length;
+	int i;
+	int mac_mode = mac_list_set->mode;
+	int ioc_res = 0;
+	ap_macmode = mac_list_set->mode;
+
+	bzero(&ap_black_list, sizeof(struct mflist));
+
+	if (mac_mode == MACLIST_MODE_DISABLED) {
+
+		ioc_res = dev_wlc_ioctl(dev, WLC_SET_MACMODE, &mac_mode, sizeof(mac_mode));
+		check_error(ioc_res, "ioctl ERROR:", __FUNCTION__, __LINE__);
+		WL_SOFTAP(("%s: MAC filtering disabled\n", __FUNCTION__));
+	} else {
+
+		scb_val_t scbval;
+		char mac_buf[256] = {0};
+		struct maclist *assoc_maclist = (struct maclist *) mac_buf;
+
+		bcopy(maclist, &ap_black_list, sizeof(ap_black_list));
+
+		ioc_res = dev_wlc_ioctl(dev, WLC_SET_MACMODE, &mac_mode, sizeof(mac_mode));
+		check_error(ioc_res, "ioctl ERROR:", __FUNCTION__, __LINE__);
+
+		length = sizeof(maclist->count) + maclist->count*ETHER_ADDR_LEN;
+		dev_wlc_ioctl(dev, WLC_SET_MACLIST, maclist, length);
+
+		WL_SOFTAP(("%s: applied MAC List, mode:%d, length %d:\n",
+			__FUNCTION__, mac_mode, length));
+		for (i = 0; i < maclist->count; i++)
+			WL_SOFTAP(("mac %d: %02X:%02X:%02X:%02X:%02X:%02X\n",
+				i, maclist->ea[i].octet[0], maclist->ea[i].octet[1], \
+				maclist->ea[i].octet[2], \
+				maclist->ea[i].octet[3], maclist->ea[i].octet[4], \
+				maclist->ea[i].octet[5]));
+
+		assoc_maclist->count = 8;
+		ioc_res = dev_wlc_ioctl(dev, WLC_GET_ASSOCLIST, assoc_maclist, 256);
+		check_error(ioc_res, "ioctl ERROR:", __FUNCTION__, __LINE__);
+		WL_SOFTAP((" Cur assoc clients:%d\n", assoc_maclist->count));
+
+		if (assoc_maclist->count)
+			for (i = 0; i < assoc_maclist->count; i++) {
+				int j;
+				bool assoc_mac_matched = false;
+
+				WL_SOFTAP(("\n Cheking assoc STA: "));
+				print_buf(&assoc_maclist->ea[i], 6, 7);
+				WL_SOFTAP(("with the b/w list:"));
+
+				for (j = 0; j < maclist->count; j++)
+					if (!bcmp(&assoc_maclist->ea[i], &maclist->ea[j],
+						ETHER_ADDR_LEN)) {
+
+						assoc_mac_matched = true;
+						break;
+					}
+
+				if (((mac_mode == MACLIST_MODE_ALLOW) && !assoc_mac_matched) ||
+					((mac_mode == MACLIST_MODE_DENY) && assoc_mac_matched)) {
+
+					WL_SOFTAP(("b-match or w-mismatch,"
+								" do deauth/disassoc \n"));
+							scbval.val = htod32(1);
+							bcopy(&assoc_maclist->ea[i], &scbval.ea, \
+							ETHER_ADDR_LEN);
+							ioc_res = dev_wlc_ioctl(dev,
+								WLC_SCB_DEAUTHENTICATE_FOR_REASON,
+								&scbval, sizeof(scb_val_t));
+							check_error(ioc_res,
+								"ioctl ERROR:",
+								__FUNCTION__, __LINE__);
+
+				} else {
+					WL_SOFTAP((" no b/w list hits, let it be\n"));
+				}
+		} else {
+			WL_SOFTAP(("No ASSOC CLIENTS\n"));
+		}
+	} 
+
+	WL_SOFTAP(("%s iocres:%d\n", __FUNCTION__, ioc_res));
+	return ioc_res;
+}
+#endif
+
+
+#ifdef SOFTAP
+int set_macfilt_from_string(struct mflist *pmflist, char **param_str)
+{
+	return 0;
+}
+#endif
+
+
+#ifdef SOFTAP
+#define PARAM_OFFSET PROFILE_OFFSET
+
+int wl_iw_process_private_ascii_cmd(
+			struct net_device *dev,
+			struct iw_request_info *info,
+			union iwreq_data *dwrq,
+			char *cmd_str)
+{
+	int ret = 0;
+	char *sub_cmd = cmd_str + PROFILE_OFFSET + strlen("ASCII_CMD=");
+
+	WL_SOFTAP(("\n %s: ASCII_CMD: offs_0:%s, offset_32:\n'%s'\n",
+		__FUNCTION__, cmd_str, cmd_str + PROFILE_OFFSET));
+
+	if (strnicmp(sub_cmd, "AP_CFG", strlen("AP_CFG")) == 0) {
+
+		WL_SOFTAP((" AP_CFG \n"));
+
+
+		if (init_ap_profile_from_string(cmd_str+PROFILE_OFFSET, &my_ap) != 0) {
+			WL_ERROR(("ERROR: SoftAP CFG prams !\n"));
+			ret = -1;
+		} else {
+			ret = set_ap_cfg(dev, &my_ap);
+		}
+
+	} else if (strnicmp(sub_cmd, "AP_BSS_START", strlen("AP_BSS_START")) == 0) {
+
+		WL_SOFTAP(("\n SOFTAP - ENABLE BSS \n"));
+
+		WL_SOFTAP(("\n!!! got 'WL_AP_EN_BSS' from WPA supplicant, dev:%s\n", dev->name));
+
+#ifndef AP_ONLY
+		if (ap_net_dev == NULL) {
+			printf("\n ERROR: SOFTAP net_dev* is NULL !!!\n");
+		} else {
+			if ((ret = iwpriv_en_ap_bss(ap_net_dev, info, dwrq, cmd_str)) < 0)
+				WL_ERROR(("%s line %d fail to set bss up\n", \
+					__FUNCTION__, __LINE__));
+		}
+#else
+		if ((ret = iwpriv_en_ap_bss(dev, info, dwrq, cmd_str)) < 0)
+				WL_ERROR(("%s line %d fail to set bss up\n", \
+					__FUNCTION__, __LINE__));
+#endif
+	} else if (strnicmp(sub_cmd, "ASSOC_LST", strlen("ASSOC_LST")) == 0) {
+		/* no code yet */
+	} else if (strnicmp(sub_cmd, "AP_BSS_STOP", strlen("AP_BSS_STOP")) == 0) {
+		WL_SOFTAP((" \n temp DOWN SOFTAP\n"));
+#ifndef AP_ONLY
+		if ((ret = dev_iw_write_cfg1_bss_var(dev, 0)) < 0) {
+				WL_ERROR(("%s line %d fail to set bss down\n", \
+					__FUNCTION__, __LINE__));
+		}
+#endif
+	}
+
+	return ret;
+}
+#endif
+
+static int wl_iw_set_priv(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *ext
+)
+{
+	int ret = 0;
+	char * extra;
+
+	if (!(extra = kmalloc(dwrq->length, GFP_KERNEL)))
+	    return -ENOMEM;
+
+	if (copy_from_user(extra, dwrq->pointer, dwrq->length)) {
+	    kfree(extra);
+	    return -EFAULT;
+	}
+
+	WL_TRACE(("%s: SIOCSIWPRIV request %s, info->cmd:%x, info->flags:%d\n dwrq->length:%d",
+		dev->name, extra, info->cmd, info->flags, dwrq->length));
+
+	net_os_wake_lock(dev);
+	
+	if (dwrq->length && extra) {
+		if (strnicmp(extra, "START", strlen("START")) == 0) {
+			wl_iw_control_wl_on(dev, info);
+			WL_TRACE(("%s, Received regular START command\n", __FUNCTION__));
+		}
+
+		if (g_onoff == G_WLAN_SET_OFF) {
+			WL_TRACE(("%s, missing START, Fail\n", __FUNCTION__));
+			kfree(extra);
+			net_os_wake_unlock(dev);
+			return -EFAULT;
+		}
+
+		if (strnicmp(extra, "SCAN-ACTIVE", strlen("SCAN-ACTIVE")) == 0) {
+#ifdef ENABLE_ACTIVE_PASSIVE_SCAN_SUPPRESS
+			WL_TRACE(("%s: active scan setting suppressed\n", dev->name));
+#else
+			ret = wl_iw_set_active_scan(dev, info, (union iwreq_data *)dwrq, extra);
+#endif
+		} else if (strnicmp(extra, "SCAN-PASSIVE", strlen("SCAN-PASSIVE")) == 0)
+#ifdef ENABLE_ACTIVE_PASSIVE_SCAN_SUPPRESS
+			WL_TRACE(("%s: passive scan setting suppressed\n", dev->name));
+#else
+			ret = wl_iw_set_passive_scan(dev, info, (union iwreq_data *)dwrq, extra);
+#endif
+		else if (strnicmp(extra, "RSSI", strlen("RSSI")) == 0)
+			ret = wl_iw_get_rssi(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, "LINKSPEED", strlen("LINKSPEED")) == 0)
+			ret = wl_iw_get_link_speed(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, "MACADDR", strlen("MACADDR")) == 0)
+			ret = wl_iw_get_macaddr(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, "COUNTRY", strlen("COUNTRY")) == 0)
+			ret = wl_iw_set_country(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, "STOP", strlen("STOP")) == 0)
+			ret = wl_iw_control_wl_off(dev, info);
+		else if (strnicmp(extra, BAND_GET_CMD, strlen(BAND_GET_CMD)) == 0)
+			ret = wl_iw_get_band(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, BAND_SET_CMD, strlen(BAND_SET_CMD)) == 0)
+			ret = wl_iw_set_band(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, DTIM_SKIP_GET_CMD, strlen(DTIM_SKIP_GET_CMD)) == 0)
+			ret = wl_iw_get_dtim_skip(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, DTIM_SKIP_SET_CMD, strlen(DTIM_SKIP_SET_CMD)) == 0)
+			ret = wl_iw_set_dtim_skip(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, SETSUSPEND_CMD, strlen(SETSUSPEND_CMD)) == 0)
+			ret = wl_iw_set_suspend(dev, info, (union iwreq_data *)dwrq, extra);
+	    else if (strnicmp(extra, TXPOWER_SET_CMD, strlen(TXPOWER_SET_CMD)) == 0)
+			ret = wl_iw_set_txpower(dev, info, (union iwreq_data *)dwrq, extra);
+#if defined(PNO_SUPPORT)
+		else if (strnicmp(extra, PNOSSIDCLR_SET_CMD, strlen(PNOSSIDCLR_SET_CMD)) == 0)
+			ret = wl_iw_set_pno_reset(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, PNOSETUP_SET_CMD, strlen(PNOSETUP_SET_CMD)) == 0)
+			ret = wl_iw_set_pno_set(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, PNOENABLE_SET_CMD, strlen(PNOENABLE_SET_CMD)) == 0)
+			ret = wl_iw_set_pno_enable(dev, info, (union iwreq_data *)dwrq, extra);
+#endif
+#if defined(CSCAN)
+		else if (strnicmp(extra, CSCAN_COMMAND, strlen(CSCAN_COMMAND)) == 0)
+			ret = wl_iw_set_cscan(dev, info, (union iwreq_data *)dwrq, extra);
+#endif
+#ifdef CUSTOMER_HW2
+		else if (strnicmp(extra, "POWERMODE", strlen("POWERMODE")) == 0)
+			ret = wl_iw_set_power_mode(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, "BTCOEXMODE", strlen("BTCOEXMODE")) == 0) {
+			WL_TRACE_COEX(("%s:got Framwrork cmd: 'BTCOEXMODE'\n", __FUNCTION__));
+			ret = wl_iw_set_btcoex_dhcp(dev, info, (union iwreq_data *)dwrq, extra);
+		}
+#else
+		else if (strnicmp(extra, "POWERMODE", strlen("POWERMODE")) == 0)
+			ret = wl_iw_set_btcoex_dhcp(dev, info, (union iwreq_data *)dwrq, extra);
+#endif
+		else if (strnicmp(extra, "GETPOWER", strlen("GETPOWER")) == 0)
+			ret = wl_iw_get_power_mode(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, RXFILTER_START_CMD, strlen(RXFILTER_START_CMD)) == 0)
+			ret = net_os_set_packet_filter(dev, 1);
+		else if (strnicmp(extra, RXFILTER_STOP_CMD, strlen(RXFILTER_STOP_CMD)) == 0)
+			ret = net_os_set_packet_filter(dev, 0);
+		else if (strnicmp(extra, RXFILTER_ADD_CMD, strlen(RXFILTER_ADD_CMD)) == 0) {
+			int filter_num = *(extra + strlen(RXFILTER_ADD_CMD) + 1) - '0';
+			ret = net_os_rxfilter_add_remove(dev, TRUE, filter_num);
+		}
+		else if (strnicmp(extra, RXFILTER_REMOVE_CMD, strlen(RXFILTER_REMOVE_CMD)) == 0) {
+			int filter_num = *(extra + strlen(RXFILTER_REMOVE_CMD) + 1) - '0';
+			ret = net_os_rxfilter_add_remove(dev, FALSE, filter_num);
+		}
+#ifdef SOFTAP
+#ifdef SOFTAP_TLV_CFG
+		else if (strnicmp(extra, SOFTAP_SET_CMD, strlen(SOFTAP_SET_CMD)) == 0) {
+		    wl_iw_softap_cfg_tlv(dev, info, (union iwreq_data *)dwrq, extra);
+		}
+#endif
+		else if (strnicmp(extra, "ASCII_CMD", strlen("ASCII_CMD")) == 0) {
+			wl_iw_process_private_ascii_cmd(dev, info, (union iwreq_data *)dwrq, extra);
+		} else if (strnicmp(extra, "AP_MAC_LIST_SET", strlen("AP_MAC_LIST_SET")) == 0) {
+			WL_SOFTAP(("penguin, set AP_MAC_LIST_SET\n"));
+			set_ap_mac_list(dev, (extra + PROFILE_OFFSET));
+		}
+#endif
+		else {
+			WL_TRACE(("Unknown PRIVATE command: %s: ignored\n", extra));
+			snprintf(extra, MAX_WX_STRING, "OK");
+			dwrq->length = strlen("OK") + 1;
+		}
+	}
+
+	net_os_wake_unlock(dev);
+
+	if (extra) {
+		if (copy_to_user(dwrq->pointer, extra, dwrq->length)) {
+			kfree(extra);
+			return -EFAULT;
+		}
+
+		kfree(extra);
+	}
+
+	return ret;
+}
+
+static const iw_handler wl_iw_handler[] =
+{
+	(iw_handler) wl_iw_config_commit,	
+	(iw_handler) wl_iw_get_name,		
+	(iw_handler) NULL,			
+	(iw_handler) NULL,			
+	(iw_handler) wl_iw_set_freq,		
+	(iw_handler) wl_iw_get_freq,		
+	(iw_handler) wl_iw_set_mode,		
+	(iw_handler) wl_iw_get_mode,		
+	(iw_handler) NULL,
+	(iw_handler) NULL,
+	(iw_handler) NULL,
+	(iw_handler) wl_iw_get_range,
+	(iw_handler) wl_iw_set_priv,
+	(iw_handler) NULL,
+	(iw_handler) NULL,			
+	(iw_handler) NULL,			
+	(iw_handler) wl_iw_set_spy,		
+	(iw_handler) wl_iw_get_spy,		
+	(iw_handler) NULL,			
+	(iw_handler) NULL,			
+	(iw_handler) wl_iw_set_wap,		
+	(iw_handler) wl_iw_get_wap,		
+#if WIRELESS_EXT > 17
+	(iw_handler) wl_iw_mlme,		
+#else
+	(iw_handler) NULL,			
+#endif
+#if defined(WL_IW_USE_ISCAN)
+	(iw_handler) wl_iw_iscan_get_aplist,	
+#else
+	(iw_handler) wl_iw_get_aplist,		
+#endif 
+#if WIRELESS_EXT > 13
+#if defined(WL_IW_USE_ISCAN)
+	(iw_handler) wl_iw_iscan_set_scan,	
+	(iw_handler) wl_iw_iscan_get_scan,	
+#else
+	(iw_handler) wl_iw_set_scan,		
+	(iw_handler) wl_iw_get_scan,		
+#endif
+#else	
+	(iw_handler) NULL,			
+	(iw_handler) NULL,			
+#endif	
+	(iw_handler) wl_iw_set_essid,		
+	(iw_handler) wl_iw_get_essid,		
+	(iw_handler) wl_iw_set_nick,
+	(iw_handler) wl_iw_get_nick,		
+	(iw_handler) NULL,			
+	(iw_handler) NULL,			
+	(iw_handler) wl_iw_set_rate,		
+	(iw_handler) wl_iw_get_rate,		
+	(iw_handler) wl_iw_set_rts,		
+	(iw_handler) wl_iw_get_rts,		
+	(iw_handler) wl_iw_set_frag,		
+	(iw_handler) wl_iw_get_frag,		
+	(iw_handler) wl_iw_set_txpow,		
+	(iw_handler) wl_iw_get_txpow,		
+#if WIRELESS_EXT > 10
+	(iw_handler) wl_iw_set_retry,		
+	(iw_handler) wl_iw_get_retry,		
+#endif 
+	(iw_handler) wl_iw_set_encode,		
+	(iw_handler) wl_iw_get_encode,		
+	(iw_handler) wl_iw_set_power,		
+	(iw_handler) wl_iw_get_power,		
+#if WIRELESS_EXT > 17
+	(iw_handler) NULL,			
+	(iw_handler) NULL,			
+	(iw_handler) wl_iw_set_wpaie,		
+	(iw_handler) wl_iw_get_wpaie,		
+	(iw_handler) wl_iw_set_wpaauth,		
+	(iw_handler) wl_iw_get_wpaauth,		
+	(iw_handler) wl_iw_set_encodeext,	
+	(iw_handler) wl_iw_get_encodeext,	
+#ifdef BCMWPA2
+	(iw_handler) wl_iw_set_pmksa,			
+#endif
+#endif 
+};
+
+#if WIRELESS_EXT > 12
+static const iw_handler wl_iw_priv_handler[] = {
+	NULL,
+	(iw_handler)wl_iw_set_active_scan,
+	NULL,
+	(iw_handler)wl_iw_get_rssi,
+	NULL,
+	(iw_handler)wl_iw_set_passive_scan,
+	NULL,
+	(iw_handler)wl_iw_get_link_speed,
+	NULL,
+	(iw_handler)wl_iw_get_macaddr,
+	NULL,
+	(iw_handler)wl_iw_control_wl_off,
+	NULL,
+	(iw_handler)wl_iw_control_wl_on,
+#ifdef SOFTAP
+	NULL,
+	(iw_handler)iwpriv_set_ap_config,
+
+	NULL,
+	(iw_handler)iwpriv_get_assoc_list,
+
+	NULL,
+	(iw_handler)iwpriv_set_mac_filters,
+
+	NULL,
+	(iw_handler)iwpriv_en_ap_bss,
+
+	NULL,
+	(iw_handler)iwpriv_wpasupp_loop_tst,
+
+	NULL,
+	(iw_handler)iwpriv_softap_stop,
+
+	NULL,
+	(iw_handler)iwpriv_fw_reload,
+
+	NULL,
+	(iw_handler)iwpriv_set_ap_sta_disassoc,
+#endif
+#if defined(CSCAN)
+
+	NULL,
+	(iw_handler)iwpriv_set_cscan
+#endif 	
+};
+
+static const struct iw_priv_args wl_iw_priv_args[] = {
+	{
+		WL_IW_SET_ACTIVE_SCAN,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"SCAN-ACTIVE"
+	},
+	{
+		WL_IW_GET_RSSI,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"RSSI"
+	},
+	{
+		WL_IW_SET_PASSIVE_SCAN,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"SCAN-PASSIVE"
+	},
+	{
+		WL_IW_GET_LINK_SPEED,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"LINKSPEED"
+	},
+	{
+		WL_IW_GET_CURR_MACADDR,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"Macaddr"
+	},
+	{
+		WL_IW_SET_STOP,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"STOP"
+	},
+	{
+		WL_IW_SET_START,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"START"
+	},
+
+#ifdef SOFTAP
+	{
+		WL_SET_AP_CFG,
+		IW_PRIV_TYPE_CHAR |  256,
+		0,
+		"AP_SET_CFG"
+	},
+
+	{
+		WL_AP_STA_LIST,
+		IW_PRIV_TYPE_CHAR | 0,
+		IW_PRIV_TYPE_CHAR | 1024,
+		"AP_GET_STA_LIST"
+	},
+
+	{
+		WL_AP_MAC_FLTR,
+		IW_PRIV_TYPE_CHAR | 256,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 0,
+		"AP_SET_MAC_FLTR"
+	},
+
+	{
+		WL_AP_BSS_START,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"AP_BSS_START"
+	},
+
+	{
+		AP_LPB_CMD,
+		IW_PRIV_TYPE_CHAR | 256,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 0,
+		"AP_LPB_CMD"
+	},
+
+	{
+		WL_AP_STOP,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 0,
+		"AP_BSS_STOP"
+	},
+
+	{
+		WL_FW_RELOAD,
+		IW_PRIV_TYPE_CHAR | 256,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 0,
+		"WL_FW_RELOAD"
+	},
+
+	{
+		WL_AP_STA_DISASSOC,
+		IW_PRIV_TYPE_CHAR | 256,
+		IW_PRIV_TYPE_CHAR | 0,
+		"AP_STA_DISASSOC"
+	},
+#endif
+#if defined(CSCAN)
+	{
+		WL_COMBO_SCAN,
+		IW_PRIV_TYPE_CHAR | 1024,
+		0,
+		"CSCAN"
+	},
+#endif
+};
+
+const struct iw_handler_def wl_iw_handler_def =
+{
+	.num_standard = ARRAYSIZE(wl_iw_handler),
+	.standard = (iw_handler *) wl_iw_handler,
+	.num_private = ARRAYSIZE(wl_iw_priv_handler),
+	.num_private_args = ARRAY_SIZE(wl_iw_priv_args),
+	.private = (iw_handler *)wl_iw_priv_handler,
+	.private_args = (void *) wl_iw_priv_args,
+
+#if WIRELESS_EXT >= 19
+	get_wireless_stats: dhd_get_wireless_stats,
+#endif 
+};
+#endif 
+
+
+int wl_iw_ioctl(
+	struct net_device *dev,
+	struct ifreq *rq,
+	int cmd
+)
+{
+	struct iwreq *wrq = (struct iwreq *) rq;
+	struct iw_request_info info;
+	iw_handler handler;
+	char *extra = NULL;
+	int token_size = 1, max_tokens = 0, ret = 0;
+
+	net_os_wake_lock(dev);
+
+	WL_TRACE(("%s: cmd:%x alled via dhd->do_ioctl()entry point\n", __FUNCTION__, cmd));
+	if (cmd < SIOCIWFIRST ||
+		IW_IOCTL_IDX(cmd) >= ARRAYSIZE(wl_iw_handler) ||
+		!(handler = wl_iw_handler[IW_IOCTL_IDX(cmd)])) {
+			WL_ERROR(("%s: error in cmd=%x : not supported\n", __FUNCTION__, cmd));
+			net_os_wake_unlock(dev);
+			return -EOPNOTSUPP;
+	}
+
+	switch (cmd) {
+
+	case SIOCSIWESSID:
+	case SIOCGIWESSID:
+	case SIOCSIWNICKN:
+	case SIOCGIWNICKN:
+		max_tokens = IW_ESSID_MAX_SIZE + 1;
+		break;
+
+	case SIOCSIWENCODE:
+	case SIOCGIWENCODE:
+#if WIRELESS_EXT > 17
+	case SIOCSIWENCODEEXT:
+	case SIOCGIWENCODEEXT:
+#endif
+		max_tokens = wrq->u.data.length;
+		break;
+
+	case SIOCGIWRANGE:
+		max_tokens = sizeof(struct iw_range) + 500;
+		break;
+
+	case SIOCGIWAPLIST:
+		token_size = sizeof(struct sockaddr) + sizeof(struct iw_quality);
+		max_tokens = IW_MAX_AP;
+		break;
+
+#if WIRELESS_EXT > 13
+	case SIOCGIWSCAN:
+#if defined(WL_IW_USE_ISCAN)
+	if (g_iscan)
+		max_tokens = wrq->u.data.length;
+	else
+#endif
+		max_tokens = IW_SCAN_MAX_DATA;
+		break;
+#endif 
+
+	case SIOCSIWSPY:
+		token_size = sizeof(struct sockaddr);
+		max_tokens = IW_MAX_SPY;
+		break;
+
+	case SIOCGIWSPY:
+		token_size = sizeof(struct sockaddr) + sizeof(struct iw_quality);
+		max_tokens = IW_MAX_SPY;
+		break;
+
+#if WIRELESS_EXT > 17
+	case SIOCSIWPMKSA:
+	case SIOCSIWGENIE:
+#endif 
+	case SIOCSIWPRIV:
+		max_tokens = wrq->u.data.length;
+		break;
+	}
+
+	if (max_tokens && wrq->u.data.pointer) {
+		if (wrq->u.data.length > max_tokens) {
+			WL_ERROR(("%s: error in cmd=%x wrq->u.data.length=%d  > max_tokens=%d\n", \
+				__FUNCTION__, cmd, wrq->u.data.length, max_tokens));
+			ret = -E2BIG;
+			goto wl_iw_ioctl_done;
+		}
+		if (!(extra = kmalloc(max_tokens * token_size, GFP_KERNEL))) {
+			ret = -ENOMEM;
+			goto wl_iw_ioctl_done;
+		}
+
+		if (copy_from_user(extra, wrq->u.data.pointer, wrq->u.data.length * token_size)) {
+			kfree(extra);
+			ret = -EFAULT;
+			goto wl_iw_ioctl_done;
+		}
+	}
+
+	info.cmd = cmd;
+	info.flags = 0;
+
+	ret = handler(dev, &info, &wrq->u, extra);
+
+	if (extra) {
+		if (copy_to_user(wrq->u.data.pointer, extra, wrq->u.data.length * token_size)) {
+			kfree(extra);
+			ret = -EFAULT;
+			goto wl_iw_ioctl_done;
+		}
+
+		kfree(extra);
+	}
+
+wl_iw_ioctl_done:
+
+	net_os_wake_unlock(dev);
+
+	return ret;
+}
+
+
+bool
+wl_iw_conn_status_str(uint32 event_type, uint32 status, uint32 reason,
+	char* stringBuf, uint buflen)
+{
+	typedef struct conn_fail_event_map_t {
+		uint32 inEvent;			
+		uint32 inStatus;		
+		uint32 inReason;		
+		const char* outName;	
+		const char* outCause;	
+	} conn_fail_event_map_t;
+
+	
+#	define WL_IW_DONT_CARE	9999
+	const conn_fail_event_map_t event_map [] = {
+		
+		
+		{WLC_E_SET_SSID,     WLC_E_STATUS_SUCCESS,   WL_IW_DONT_CARE,
+		"Conn", "Success"},
+		{WLC_E_SET_SSID,     WLC_E_STATUS_NO_NETWORKS, WL_IW_DONT_CARE,
+		"Conn", "NoNetworks"},
+		{WLC_E_SET_SSID,     WLC_E_STATUS_FAIL,      WL_IW_DONT_CARE,
+		"Conn", "ConfigMismatch"},
+		{WLC_E_PRUNE,        WL_IW_DONT_CARE,        WLC_E_PRUNE_ENCR_MISMATCH,
+		"Conn", "EncrypMismatch"},
+		{WLC_E_PRUNE,        WL_IW_DONT_CARE,        WLC_E_RSN_MISMATCH,
+		"Conn", "RsnMismatch"},
+		{WLC_E_AUTH,         WLC_E_STATUS_TIMEOUT,   WL_IW_DONT_CARE,
+		"Conn", "AuthTimeout"},
+		{WLC_E_AUTH,         WLC_E_STATUS_FAIL,      WL_IW_DONT_CARE,
+		"Conn", "AuthFail"},
+		{WLC_E_AUTH,         WLC_E_STATUS_NO_ACK,    WL_IW_DONT_CARE,
+		"Conn", "AuthNoAck"},
+		{WLC_E_REASSOC,      WLC_E_STATUS_FAIL,      WL_IW_DONT_CARE,
+		"Conn", "ReassocFail"},
+		{WLC_E_REASSOC,      WLC_E_STATUS_TIMEOUT,   WL_IW_DONT_CARE,
+		"Conn", "ReassocTimeout"},
+		{WLC_E_REASSOC,      WLC_E_STATUS_ABORT,     WL_IW_DONT_CARE,
+		"Conn", "ReassocAbort"},
+		{WLC_E_PSK_SUP,      WLC_SUP_KEYED,          WL_IW_DONT_CARE,
+		"Sup", "ConnSuccess"},
+		{WLC_E_PSK_SUP,      WL_IW_DONT_CARE,        WL_IW_DONT_CARE,
+		"Sup", "WpaHandshakeFail"},
+		{WLC_E_DEAUTH_IND,   WL_IW_DONT_CARE,        WL_IW_DONT_CARE,
+		"Conn", "Deauth"},
+		{WLC_E_DISASSOC_IND, WL_IW_DONT_CARE,        WL_IW_DONT_CARE,
+		"Conn", "DisassocInd"},
+		{WLC_E_DISASSOC,     WL_IW_DONT_CARE,        WL_IW_DONT_CARE,
+		"Conn", "Disassoc"}
+	};
+
+	const char* name = "";
+	const char* cause = NULL;
+	int i;
+
+	
+	for (i = 0;  i < sizeof(event_map)/sizeof(event_map[0]);  i++) {
+		const conn_fail_event_map_t* row = &event_map[i];
+		if (row->inEvent == event_type &&
+		    (row->inStatus == status || row->inStatus == WL_IW_DONT_CARE) &&
+		    (row->inReason == reason || row->inReason == WL_IW_DONT_CARE)) {
+			name = row->outName;
+			cause = row->outCause;
+			break;
+		}
+	}
+
+	
+	if (cause) {
+		memset(stringBuf, 0, buflen);
+		snprintf(stringBuf, buflen, "%s %s %02d %02d",
+			name, cause, status, reason);
+		WL_INFORM(("Connection status: %s\n", stringBuf));
+		return TRUE;
+	} else {
+		return FALSE;
+	}
+}
+
+#if WIRELESS_EXT > 14
+
+static bool
+wl_iw_check_conn_fail(wl_event_msg_t *e, char* stringBuf, uint buflen)
+{
+	uint32 event = ntoh32(e->event_type);
+	uint32 status =  ntoh32(e->status);
+	uint32 reason =  ntoh32(e->reason);
+
+	if (wl_iw_conn_status_str(event, status, reason, stringBuf, buflen)) {
+		return TRUE;
+	}
+	else
+		return FALSE;
+}
+#endif
+
+#ifndef IW_CUSTOM_MAX
+#define IW_CUSTOM_MAX 256 
+#endif
+
+void
+wl_iw_event(struct net_device *dev, wl_event_msg_t *e, void* data)
+{
+#if WIRELESS_EXT > 13
+	union iwreq_data wrqu;
+	char extra[IW_CUSTOM_MAX + 1];
+	int cmd = 0;
+	uint32 event_type = ntoh32(e->event_type);
+	uint16 flags =  ntoh16(e->flags);
+	uint32 datalen = ntoh32(e->datalen);
+	uint32 status =  ntoh32(e->status);
+	uint32 toto;
+#if defined(ROAM_NOT_USED)
+	static uint32 roam_no_success = 0;
+	static bool roam_no_success_send = FALSE;
+#endif
+	memset(&wrqu, 0, sizeof(wrqu));
+	memset(extra, 0, sizeof(extra));
+
+	if (!dev) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return;
+	}
+
+	net_os_wake_lock(dev);
+
+	WL_TRACE(("%s: dev=%s event=%d \n", __FUNCTION__, dev->name, event_type));
+
+	switch (event_type) {
+
+	case WLC_E_RELOAD:
+		WL_ERROR(("%s: Firmware ERROR %d\n", __FUNCTION__, status));
+		net_os_send_hang_message(dev);
+		goto wl_iw_event_end;
+
+#if defined(SOFTAP)
+	case WLC_E_PRUNE:
+		if (ap_cfg_running) {
+			char *macaddr = (char *)&e->addr;
+			WL_SOFTAP(("PRUNE received, %02X:%02X:%02X:%02X:%02X:%02X!\n",
+				macaddr[0], macaddr[1], macaddr[2], macaddr[3], \
+				macaddr[4], macaddr[5]));
+
+			if (ap_macmode) {
+				int i;
+				for (i = 0; i < ap_black_list.count; i++) {
+					if (!bcmp(macaddr, &ap_black_list.ea[i], \
+						sizeof(struct ether_addr))) {
+						WL_SOFTAP(("mac in black list, ignore it\n"));
+						break;
+					}
+				}
+
+				if (i == ap_black_list.count) {
+					char mac_buf[32] = {0};
+					sprintf(mac_buf, "STA_BLOCK %02X:%02X:%02X:%02X:%02X:%02X",
+						macaddr[0], macaddr[1], macaddr[2],
+						macaddr[3], macaddr[4], macaddr[5]);
+					wl_iw_send_priv_event(priv_dev, mac_buf);
+				}
+			}
+		}
+		break;
+#endif
+	case WLC_E_TXFAIL:
+		cmd = IWEVTXDROP;
+		memcpy(wrqu.addr.sa_data, &e->addr, ETHER_ADDR_LEN);
+		wrqu.addr.sa_family = ARPHRD_ETHER;
+		break;
+#if WIRELESS_EXT > 14
+	case WLC_E_JOIN:
+	case WLC_E_ASSOC_IND:
+	case WLC_E_REASSOC_IND:
+#if defined(SOFTAP)
+		WL_SOFTAP(("STA connect received %d\n", event_type));
+		if (ap_cfg_running) {
+			wl_iw_send_priv_event(priv_dev, "STA_JOIN");
+			goto wl_iw_event_end;
+		}
+#endif
+		memcpy(wrqu.addr.sa_data, &e->addr, ETHER_ADDR_LEN);
+		wrqu.addr.sa_family = ARPHRD_ETHER;
+		cmd = IWEVREGISTERED;
+		break;
+	case WLC_E_ROAM:
+		if (status == WLC_E_STATUS_SUCCESS) {
+			WL_ASSOC(("%s: WLC_E_ROAM: success\n", __FUNCTION__));
+#if defined(ROAM_NOT_USED)
+			roam_no_success_send = FALSE;
+			roam_no_success = 0;
+#endif
+			goto wl_iw_event_end;
+		}
+#if defined(ROAM_NOT_USED)
+		else if (status == WLC_E_STATUS_NO_NETWORKS) {
+			roam_no_success++;
+			if ((roam_no_success == 5) && (roam_no_success_send == FALSE)) {
+				roam_no_success_send = TRUE;
+				bzero(wrqu.addr.sa_data, ETHER_ADDR_LEN);
+				bzero(&extra, ETHER_ADDR_LEN);
+				cmd = SIOCGIWAP;
+				WL_ERROR(("%s  ROAMING did not succeeded , send Link Down\n", \
+					__FUNCTION__));
+			} else {
+				WL_TRACE(("##### ROAMING did not succeeded %d\n", roam_no_success));
+				goto wl_iw_event_end;
+			}
+		}
+#endif
+		break;
+	case WLC_E_DEAUTH_IND:
+	case WLC_E_DISASSOC_IND:
+#if defined(SOFTAP)
+		WL_SOFTAP(("STA disconnect received %d\n", event_type));
+		if (ap_cfg_running) {
+			wl_iw_send_priv_event(priv_dev, "STA_LEAVE");
+			goto wl_iw_event_end;
+		}
+#endif
+		cmd = SIOCGIWAP;
+		bzero(wrqu.addr.sa_data, ETHER_ADDR_LEN);
+		wrqu.addr.sa_family = ARPHRD_ETHER;
+		bzero(&extra, ETHER_ADDR_LEN);
+		break;
+	case WLC_E_LINK:
+	case WLC_E_NDIS_LINK:
+		cmd = SIOCGIWAP;
+		if (!(flags & WLC_EVENT_MSG_LINK)) {
+#ifdef SOFTAP
+#ifdef AP_ONLY
+		if (ap_cfg_running) {
+#else
+		if (ap_cfg_running && !strncmp(dev->name, "wl0.1", 5)) {
+#endif	
+				WL_SOFTAP(("AP DOWN %d\n", event_type));
+				wl_iw_send_priv_event(priv_dev, "AP_DOWN");
+			} else {
+				WL_TRACE(("STA_Link Down\n"));
+				g_ss_cache_ctrl.m_link_down = 1;
+			}
+#else
+			g_ss_cache_ctrl.m_link_down = 1;
+#endif
+			WL_TRACE(("Link Down\n"));
+
+			bzero(wrqu.addr.sa_data, ETHER_ADDR_LEN);
+			bzero(&extra, ETHER_ADDR_LEN);
+		}
+		else {
+			memcpy(wrqu.addr.sa_data, &e->addr, ETHER_ADDR_LEN);
+			g_ss_cache_ctrl.m_link_down = 0;
+
+			memcpy(g_ss_cache_ctrl.m_active_bssid, &e->addr, ETHER_ADDR_LEN);
+
+#ifdef SOFTAP
+#ifdef AP_ONLY
+			if (ap_cfg_running) {
+#else
+			if (ap_cfg_running && !strncmp(dev->name, "wl0.1", 5)) {
+#endif
+				WL_SOFTAP(("AP UP %d\n", event_type));
+				wl_iw_send_priv_event(priv_dev, "AP_UP");
+			} else {
+				WL_TRACE(("STA_LINK_UP\n"));
+#if defined(ROAM_NOT_USED)
+				roam_no_success_send = FALSE;
+				roam_no_success = 0;
+#endif
+			}
+#endif
+			WL_TRACE(("Link UP\n"));
+
+		}
+		net_os_wake_lock_timeout_enable(dev);
+		wrqu.addr.sa_family = ARPHRD_ETHER;
+		break;
+	case WLC_E_ACTION_FRAME:
+		cmd = IWEVCUSTOM;
+		if (datalen + 1 <= sizeof(extra)) {
+			wrqu.data.length = datalen + 1;
+			extra[0] = WLC_E_ACTION_FRAME;
+			memcpy(&extra[1], data, datalen);
+			WL_TRACE(("WLC_E_ACTION_FRAME len %d \n", wrqu.data.length));
+		}
+		break;
+
+	case WLC_E_ACTION_FRAME_COMPLETE:
+		cmd = IWEVCUSTOM;
+		memcpy(&toto, data, 4);
+		if (sizeof(status) + 1 <= sizeof(extra)) {
+			wrqu.data.length = sizeof(status) + 1;
+			extra[0] = WLC_E_ACTION_FRAME_COMPLETE;
+			memcpy(&extra[1], &status, sizeof(status));
+			printf("wl_iw_event status %d PacketId %d \n", status, toto);
+			printf("WLC_E_ACTION_FRAME_COMPLETE len %d \n", wrqu.data.length);
+		}
+		break;
+#endif 
+#if WIRELESS_EXT > 17
+	case WLC_E_MIC_ERROR: {
+		struct	iw_michaelmicfailure  *micerrevt = (struct  iw_michaelmicfailure  *)&extra;
+		cmd = IWEVMICHAELMICFAILURE;
+		wrqu.data.length = sizeof(struct iw_michaelmicfailure);
+		if (flags & WLC_EVENT_MSG_GROUP)
+			micerrevt->flags |= IW_MICFAILURE_GROUP;
+		else
+			micerrevt->flags |= IW_MICFAILURE_PAIRWISE;
+		memcpy(micerrevt->src_addr.sa_data, &e->addr, ETHER_ADDR_LEN);
+		micerrevt->src_addr.sa_family = ARPHRD_ETHER;
+
+		break;
+	}
+#ifdef BCMWPA2
+	case WLC_E_PMKID_CACHE: {
+		if (data)
+		{
+			struct iw_pmkid_cand *iwpmkidcand = (struct iw_pmkid_cand *)&extra;
+			pmkid_cand_list_t *pmkcandlist;
+			pmkid_cand_t	*pmkidcand;
+			int count;
+
+			cmd = IWEVPMKIDCAND;
+			pmkcandlist = data;
+			count = ntoh32_ua((uint8 *)&pmkcandlist->npmkid_cand);
+			ASSERT(count >= 0);
+			wrqu.data.length = sizeof(struct iw_pmkid_cand);
+			pmkidcand = pmkcandlist->pmkid_cand;
+			while (count) {
+				bzero(iwpmkidcand, sizeof(struct iw_pmkid_cand));
+				if (pmkidcand->preauth)
+					iwpmkidcand->flags |= IW_PMKID_CAND_PREAUTH;
+				bcopy(&pmkidcand->BSSID, &iwpmkidcand->bssid.sa_data,
+					ETHER_ADDR_LEN);
+#ifndef SANDGATE2G
+				wireless_send_event(dev, cmd, &wrqu, extra);
+#endif
+				pmkidcand++;
+				count--;
+			}
+		}
+		goto wl_iw_event_end;
+	}
+#endif 
+#endif 
+
+	case WLC_E_SCAN_COMPLETE:
+#if defined(WL_IW_USE_ISCAN)
+		if ((g_iscan) && (g_iscan->sysioc_pid >= 0) &&
+			(g_iscan->iscan_state != ISCAN_STATE_IDLE))
+		{
+			up(&g_iscan->sysioc_sem);
+		} else {
+			cmd = SIOCGIWSCAN;
+			wrqu.data.length = strlen(extra);
+			WL_TRACE(("Event WLC_E_SCAN_COMPLETE from specific scan %d\n", \
+				g_iscan->iscan_state));
+		}
+#else
+		cmd = SIOCGIWSCAN;
+		wrqu.data.length = strlen(extra);
+		WL_TRACE(("Event WLC_E_SCAN_COMPLETE\n"));
+#endif
+		break;
+
+	case WLC_E_PFN_NET_FOUND:
+	{
+		wlc_ssid_t	* ssid;
+		ssid = (wlc_ssid_t *)data;
+		WL_TRACE(("%s Event WLC_E_PFN_NET_FOUND, send %s up : find %s len=%d\n", \
+			__FUNCTION__, PNO_EVENT_UP, ssid->SSID, ssid->SSID_len));
+		net_os_wake_lock_timeout_enable(dev);
+		cmd = IWEVCUSTOM;
+		memset(&wrqu, 0, sizeof(wrqu));
+		strcpy(extra, PNO_EVENT_UP);
+		wrqu.data.length = strlen(extra);
+	}
+	break;
+
+	default:
+		
+		WL_TRACE(("Unknown Event %d: ignoring\n", event_type));
+		break;
+	}
+#ifndef SANDGATE2G
+	if (cmd) {
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 31))
+		if (cmd == SIOCGIWSCAN)
+			wireless_send_event(dev, cmd, &wrqu, NULL);
+		else
+#endif
+		wireless_send_event(dev, cmd, &wrqu, extra);
+	}
+#endif
+
+#if WIRELESS_EXT > 14
+	memset(extra, 0, sizeof(extra));
+	if (wl_iw_check_conn_fail(e, extra, sizeof(extra))) {
+		cmd = IWEVCUSTOM;
+		wrqu.data.length = strlen(extra);
+#ifndef SANDGATE2G
+		wireless_send_event(dev, cmd, &wrqu, extra);
+#endif
+	}
+#endif
+wl_iw_event_end:
+	net_os_wake_unlock(dev);
+#endif
+}
+
+int wl_iw_get_wireless_stats(struct net_device *dev, struct iw_statistics *wstats)
+{
+	int res = 0;
+	wl_cnt_t cnt;
+	int phy_noise;
+	int rssi;
+	scb_val_t scb_val;
+
+	phy_noise = 0;
+	if ((res = dev_wlc_ioctl(dev, WLC_GET_PHY_NOISE, &phy_noise, sizeof(phy_noise))))
+		goto done;
+
+	phy_noise = dtoh32(phy_noise);
+	WL_TRACE(("wl_iw_get_wireless_stats phy noise=%d\n", phy_noise));
+
+	bzero(&scb_val, sizeof(scb_val_t));
+	if ((res = dev_wlc_ioctl(dev, WLC_GET_RSSI, &scb_val, sizeof(scb_val_t))))
+		goto done;
+
+	rssi = dtoh32(scb_val.val);
+	WL_TRACE(("wl_iw_get_wireless_stats rssi=%d\n", rssi));
+	if (rssi <= WL_IW_RSSI_NO_SIGNAL)
+		wstats->qual.qual = 0;
+	else if (rssi <= WL_IW_RSSI_VERY_LOW)
+		wstats->qual.qual = 1;
+	else if (rssi <= WL_IW_RSSI_LOW)
+		wstats->qual.qual = 2;
+	else if (rssi <= WL_IW_RSSI_GOOD)
+		wstats->qual.qual = 3;
+	else if (rssi <= WL_IW_RSSI_VERY_GOOD)
+		wstats->qual.qual = 4;
+	else
+		wstats->qual.qual = 5;
+
+	
+	wstats->qual.level = 0x100 + rssi;
+	wstats->qual.noise = 0x100 + phy_noise;
+#if WIRELESS_EXT > 18
+	wstats->qual.updated |= (IW_QUAL_ALL_UPDATED | IW_QUAL_DBM);
+#else
+	wstats->qual.updated |= 7;
+#endif 
+
+#if WIRELESS_EXT > 11
+	WL_TRACE(("wl_iw_get_wireless_stats counters=%d\n", (int)sizeof(wl_cnt_t)));
+
+	memset(&cnt, 0, sizeof(wl_cnt_t));
+	res = dev_wlc_bufvar_get(dev, "counters", (char *)&cnt, sizeof(wl_cnt_t));
+	if (res)
+	{
+		WL_ERROR(("wl_iw_get_wireless_stats counters failed error=%d\n", res));
+		goto done;
+	}
+
+	cnt.version = dtoh16(cnt.version);
+	if (cnt.version != WL_CNT_T_VERSION) {
+		WL_TRACE(("\tIncorrect version of counters struct: expected %d; got %d\n",
+			WL_CNT_T_VERSION, cnt.version));
+		goto done;
+	}
+
+	wstats->discard.nwid = 0;
+	wstats->discard.code = dtoh32(cnt.rxundec);
+	wstats->discard.fragment = dtoh32(cnt.rxfragerr);
+	wstats->discard.retries = dtoh32(cnt.txfail);
+	wstats->discard.misc = dtoh32(cnt.rxrunt) + dtoh32(cnt.rxgiant);
+	wstats->miss.beacon = 0;
+
+	WL_TRACE(("wl_iw_get_wireless_stats counters txframe=%d txbyte=%d\n",
+		dtoh32(cnt.txframe), dtoh32(cnt.txbyte)));
+	WL_TRACE(("wl_iw_get_wireless_stats counters rxfrmtoolong=%d\n", dtoh32(cnt.rxfrmtoolong)));
+	WL_TRACE(("wl_iw_get_wireless_stats counters rxbadplcp=%d\n", dtoh32(cnt.rxbadplcp)));
+	WL_TRACE(("wl_iw_get_wireless_stats counters rxundec=%d\n", dtoh32(cnt.rxundec)));
+	WL_TRACE(("wl_iw_get_wireless_stats counters rxfragerr=%d\n", dtoh32(cnt.rxfragerr)));
+	WL_TRACE(("wl_iw_get_wireless_stats counters txfail=%d\n", dtoh32(cnt.txfail)));
+	WL_TRACE(("wl_iw_get_wireless_stats counters rxrunt=%d\n", dtoh32(cnt.rxrunt)));
+	WL_TRACE(("wl_iw_get_wireless_stats counters rxgiant=%d\n", dtoh32(cnt.rxgiant)));
+
+#endif 
+
+done:
+	return res;
+}
+static void
+wl_iw_bt_flag_set(
+	struct net_device *dev,
+	bool set)
+{
+#if defined(BT_DHCP_USE_FLAGS)
+	char buf_flag7_dhcp_on[8] = { 7, 00, 00, 00, 0x1, 0x0, 0x00, 0x00 };
+	char buf_flag7_default[8]   = { 7, 00, 00, 00, 0x0, 0x00, 0x00, 0x00};
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	rtnl_lock();
+#endif
+
+#if defined(BT_DHCP_eSCO_FIX)
+	set_btc_esco_params(dev, set);
+#endif
+
+#if defined(BT_DHCP_USE_FLAGS)
+	WL_TRACE_COEX(("WI-FI priority boost via bt flags, set:%d\n", set));
+	if (set == TRUE) {
+		dev_wlc_bufvar_set(dev, "btc_flags",
+					(char *)&buf_flag7_dhcp_on[0], sizeof(buf_flag7_dhcp_on));
+	}
+	else  {
+		dev_wlc_bufvar_set(dev, "btc_flags",
+					(char *)&buf_flag7_default[0], sizeof(buf_flag7_default));
+	}
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	rtnl_unlock();
+#endif
+}
+
+static void
+wl_iw_bt_timerfunc(ulong data)
+{
+	bt_info_t  *bt_local = (bt_info_t *)data;
+	bt_local->timer_on = 0;
+	WL_TRACE(("%s\n", __FUNCTION__));
+
+	up(&bt_local->bt_sem);
+}
+
+static int
+_bt_dhcp_sysioc_thread(void *data)
+{
+	DAEMONIZE("dhcp_sysioc");
+
+	while (down_interruptible(&g_bt->bt_sem) == 0) {
+
+		net_os_wake_lock(g_bt->dev);
+
+		if (g_bt->timer_on) {
+			g_bt->timer_on = 0;
+			del_timer_sync(&g_bt->timer);
+		}
+
+		switch (g_bt->bt_state) {
+			case BT_DHCP_START:
+				WL_TRACE_COEX(("%s bt_dhcp stm: started \n", __FUNCTION__));
+				g_bt->bt_state = BT_DHCP_OPPORTUNITY_WINDOW;
+				mod_timer(&g_bt->timer, jiffies + BT_DHCP_OPPORTUNITY_WINDOW_TIME*HZ/1000);
+				g_bt->timer_on = 1;
+				break;
+
+			case BT_DHCP_OPPORTUNITY_WINDOW:
+				if (g_bt->dhcp_done) {
+					WL_TRACE_COEX(("%s DHCP Done before T1 expiration\n", \
+						__FUNCTION__));
+					g_bt->bt_state = BT_DHCP_IDLE;
+					g_bt->timer_on = 0;
+					break;
+				}
+
+				WL_TRACE_COEX(("%s DHCP T1:%d expired\n", \
+						__FUNCTION__, BT_DHCP_OPPORTUNITY_WINDOW_TIME));
+				if (g_bt->dev) wl_iw_bt_flag_set(g_bt->dev, TRUE);
+				g_bt->bt_state = BT_DHCP_FLAG_FORCE_TIMEOUT;
+				mod_timer(&g_bt->timer, jiffies + BT_DHCP_FLAG_FORCE_TIME*HZ/1000);
+				g_bt->timer_on = 1;
+				break;
+
+			case BT_DHCP_FLAG_FORCE_TIMEOUT:
+				if (g_bt->dhcp_done) {
+					WL_TRACE_COEX(("%s DHCP Done before T2 expiration\n", \
+						__FUNCTION__));
+				} else {
+					WL_TRACE_COEX(("%s DHCP wait interval T2:%d msec expired\n",
+						__FUNCTION__, BT_DHCP_FLAG_FORCE_TIME));
+				}
+
+				if (g_bt->dev)  wl_iw_bt_flag_set(g_bt->dev, FALSE);
+				g_bt->bt_state = BT_DHCP_IDLE;
+				g_bt->timer_on = 0;
+				break;
+
+			default:
+				WL_ERROR(("%s error g_status=%d !!!\n", __FUNCTION__, \
+				          g_bt->bt_state));
+				if (g_bt->dev) wl_iw_bt_flag_set(g_bt->dev, FALSE);
+				g_bt->bt_state = BT_DHCP_IDLE;
+				g_bt->timer_on = 0;
+				break;
+		}
+
+		net_os_wake_unlock(g_bt->dev);
+	}
+
+	if (g_bt->timer_on) {
+		g_bt->timer_on = 0;
+		del_timer_sync(&g_bt->timer);
+	}
+
+	complete_and_exit(&g_bt->bt_exited, 0);
+}
+
+static void
+wl_iw_bt_release(void)
+{
+	bt_info_t *bt_local = g_bt;
+
+	if (!bt_local) {
+		return;
+	}
+
+	if (bt_local->bt_pid >= 0) {
+		KILL_PROC(bt_local->bt_pid, SIGTERM);
+		wait_for_completion(&bt_local->bt_exited);
+	}
+	kfree(bt_local);
+	g_bt = NULL;
+}
+
+static int
+wl_iw_bt_init(struct net_device *dev)
+{
+	bt_info_t *bt_dhcp = NULL;
+
+	bt_dhcp = kmalloc(sizeof(bt_info_t), GFP_KERNEL);
+	if (!bt_dhcp)
+		return -ENOMEM;
+
+	memset(bt_dhcp, 0, sizeof(bt_info_t));
+	bt_dhcp->bt_pid = -1;
+	g_bt = bt_dhcp;
+	bt_dhcp->dev = dev;
+	bt_dhcp->bt_state = BT_DHCP_IDLE;
+
+	
+	bt_dhcp->timer_ms    = 10;
+	init_timer(&bt_dhcp->timer);
+	bt_dhcp->timer.data = (ulong)bt_dhcp;
+	bt_dhcp->timer.function = wl_iw_bt_timerfunc;
+
+	sema_init(&bt_dhcp->bt_sem, 0);
+	init_completion(&bt_dhcp->bt_exited);
+	bt_dhcp->bt_pid = kernel_thread(_bt_dhcp_sysioc_thread, bt_dhcp, 0);
+	if (bt_dhcp->bt_pid < 0) {
+		WL_ERROR(("Failed in %s\n", __FUNCTION__));
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+int wl_iw_attach(struct net_device *dev, void *dhdp)
+{
+	int params_size;
+	wl_iw_t *iw;
+#if defined(WL_IW_USE_ISCAN)
+	iscan_info_t *iscan = NULL;
+#endif
+
+	mutex_init(&wl_cache_lock);
+
+#if defined(WL_IW_USE_ISCAN)
+	if (!dev)
+		return 0;
+
+	memset(&g_wl_iw_params, 0, sizeof(wl_iw_extra_params_t));
+
+#ifdef CSCAN
+	params_size = (WL_SCAN_PARAMS_FIXED_SIZE + OFFSETOF(wl_iscan_params_t, params)) +
+	    (WL_NUMCHANNELS * sizeof(uint16)) + WL_SCAN_PARAMS_SSID_MAX * sizeof(wlc_ssid_t);
+#else
+	params_size = (WL_SCAN_PARAMS_FIXED_SIZE + OFFSETOF(wl_iscan_params_t, params));
+#endif 
+	iscan = kmalloc(sizeof(iscan_info_t), GFP_KERNEL);
+	if (!iscan)
+		return -ENOMEM;
+	memset(iscan, 0, sizeof(iscan_info_t));
+	
+	iscan->iscan_ex_params_p = (wl_iscan_params_t*)kmalloc(params_size, GFP_KERNEL);
+	if (!iscan->iscan_ex_params_p)
+		return -ENOMEM;
+	iscan->iscan_ex_param_size = params_size;
+	iscan->sysioc_pid = -1;
+	
+	g_iscan = iscan;
+	iscan->dev = dev;
+	iscan->iscan_state = ISCAN_STATE_IDLE;
+#if defined(CONFIG_FIRST_SCAN)
+	g_first_broadcast_scan = BROADCAST_SCAN_FIRST_IDLE;
+	g_first_counter_scans = 0;
+	g_iscan->scan_flag = 0;
+#endif
+
+	iscan->timer_ms    = 8000;
+	init_timer(&iscan->timer);
+	iscan->timer.data = (ulong)iscan;
+	iscan->timer.function = wl_iw_timerfunc;
+
+	sema_init(&iscan->sysioc_sem, 0);
+	init_completion(&iscan->sysioc_exited);
+	iscan->sysioc_pid = kernel_thread(_iscan_sysioc_thread, iscan, 0);
+	if (iscan->sysioc_pid < 0)
+		return -ENOMEM;
+#endif
+
+	iw = *(wl_iw_t **)netdev_priv(dev);
+	iw->pub = (dhd_pub_t *)dhdp;
+#ifdef SOFTAP
+	priv_dev = dev;
+#endif 
+	g_scan = NULL;
+
+	g_scan = (void *)kmalloc(G_SCAN_RESULTS, GFP_KERNEL);
+	if (!g_scan)
+		return -ENOMEM;
+
+	memset(g_scan, 0, G_SCAN_RESULTS);
+	g_scan_specified_ssid = 0;
+
+#if !defined(CSCAN)
+	wl_iw_init_ss_cache_ctrl();
+#endif
+
+	wl_iw_bt_init(dev);
+
+	return 0;
+}
+
+void wl_iw_detach(void)
+{
+#if defined(WL_IW_USE_ISCAN)
+	iscan_buf_t  *buf;
+	iscan_info_t *iscan = g_iscan;
+
+	if (!iscan)
+		return;
+	if (iscan->sysioc_pid >= 0) {
+		KILL_PROC(iscan->sysioc_pid, SIGTERM);
+		wait_for_completion(&iscan->sysioc_exited);
+	}
+	mutex_lock(&wl_cache_lock);
+	while (iscan->list_hdr) {
+		buf = iscan->list_hdr->next;
+		kfree(iscan->list_hdr);
+		iscan->list_hdr = buf;
+	}
+	kfree(iscan->iscan_ex_params_p);
+	kfree(iscan);
+	g_iscan = NULL;
+	mutex_unlock(&wl_cache_lock);
+#endif
+
+	if (g_scan)
+		kfree(g_scan);
+
+	g_scan = NULL;
+#if !defined(CSCAN)
+	wl_iw_release_ss_cache_ctrl();
+#endif
+	wl_iw_bt_release();
+#ifdef SOFTAP
+	if (ap_cfg_running) {
+		WL_TRACE(("\n%s AP is going down\n", __FUNCTION__));
+		wl_iw_send_priv_event(priv_dev, "AP_DOWN");
+	}
+#endif
+}
diff --git a/drivers/net/wireless/bcm4329/wl_iw.h b/drivers/net/wireless/bcm4329/wl_iw.h
new file mode 100644
index 000000000000..ee6c699936ea
--- /dev/null
+++ b/drivers/net/wireless/bcm4329/wl_iw.h
@@ -0,0 +1,309 @@
+/*
+ * Linux Wireless Extensions support
+ *
+ * Copyright (C) 1999-2010, Broadcom Corporation
+ * 
+ *      Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wl_iw.h,v 1.5.34.1.6.36.4.18 2011/02/10 19:33:12 Exp $
+ */
+
+
+#ifndef _wl_iw_h_
+#define _wl_iw_h_
+
+#include <linux/wireless.h>
+
+#include <typedefs.h>
+#include <proto/ethernet.h>
+#include <wlioctl.h>
+
+#define WL_SCAN_PARAMS_SSID_MAX 	10
+#define GET_SSID			"SSID="
+#define GET_CHANNEL			"CH="
+#define GET_NPROBE 			"NPROBE="
+#define GET_ACTIVE_ASSOC_DWELL  	"ACTIVE="
+#define GET_PASSIVE_ASSOC_DWELL  	"PASSIVE="
+#define GET_HOME_DWELL  		"HOME="
+#define GET_SCAN_TYPE			"TYPE="
+
+#define BAND_GET_CMD				"GETBAND"
+#define BAND_SET_CMD				"SETBAND"
+#define DTIM_SKIP_GET_CMD			"DTIMSKIPGET"
+#define DTIM_SKIP_SET_CMD			"DTIMSKIPSET"
+#define SETSUSPEND_CMD				"SETSUSPENDOPT"
+#define PNOSSIDCLR_SET_CMD			"PNOSSIDCLR"
+#define PNOSETUP_SET_CMD			"PNOSETUP "
+#define PNOENABLE_SET_CMD			"PNOFORCE"
+#define PNODEBUG_SET_CMD			"PNODEBUG"
+#define TXPOWER_SET_CMD				"TXPOWER"
+#define RXFILTER_START_CMD			"RXFILTER-START"
+#define RXFILTER_STOP_CMD			"RXFILTER-STOP"
+#define RXFILTER_ADD_CMD			"RXFILTER-ADD"
+#define RXFILTER_REMOVE_CMD			"RXFILTER-REMOVE"
+
+#define MAC2STR(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5]
+#define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x"
+
+
+typedef struct wl_iw_extra_params {
+	int 	target_channel;
+} wl_iw_extra_params_t;
+
+struct cntry_locales_custom {
+	char iso_abbrev[WLC_CNTRY_BUF_SZ];
+	char custom_locale[WLC_CNTRY_BUF_SZ];
+	int32 custom_locale_rev;
+};
+
+#define	WL_IW_RSSI_MINVAL	-200
+#define	WL_IW_RSSI_NO_SIGNAL	-91
+#define	WL_IW_RSSI_VERY_LOW	-80
+#define	WL_IW_RSSI_LOW		-70
+#define	WL_IW_RSSI_GOOD		-68
+#define	WL_IW_RSSI_VERY_GOOD	-58
+#define	WL_IW_RSSI_EXCELLENT	-57
+#define	WL_IW_RSSI_INVALID	 0
+#define MAX_WX_STRING		80
+#define isprint(c)		bcm_isprint(c)
+#define WL_IW_SET_ACTIVE_SCAN	(SIOCIWFIRSTPRIV+1)
+#define WL_IW_GET_RSSI		(SIOCIWFIRSTPRIV+3)
+#define WL_IW_SET_PASSIVE_SCAN	(SIOCIWFIRSTPRIV+5)
+#define WL_IW_GET_LINK_SPEED	(SIOCIWFIRSTPRIV+7)
+#define WL_IW_GET_CURR_MACADDR	(SIOCIWFIRSTPRIV+9)
+#define WL_IW_SET_STOP		(SIOCIWFIRSTPRIV+11)
+#define WL_IW_SET_START		(SIOCIWFIRSTPRIV+13)
+
+
+#define WL_SET_AP_CFG           (SIOCIWFIRSTPRIV+15)
+#define WL_AP_STA_LIST          (SIOCIWFIRSTPRIV+17)
+#define WL_AP_MAC_FLTR	        (SIOCIWFIRSTPRIV+19)
+#define WL_AP_BSS_START         (SIOCIWFIRSTPRIV+21)
+#define AP_LPB_CMD              (SIOCIWFIRSTPRIV+23)
+#define WL_AP_STOP              (SIOCIWFIRSTPRIV+25)
+#define WL_FW_RELOAD            (SIOCIWFIRSTPRIV+27)
+#define WL_AP_STA_DISASSOC		(SIOCIWFIRSTPRIV+29)
+#define WL_COMBO_SCAN           (SIOCIWFIRSTPRIV+31)
+
+#define G_SCAN_RESULTS		(8*1024)
+#define WE_ADD_EVENT_FIX	0x80
+#define G_WLAN_SET_ON		0
+#define G_WLAN_SET_OFF		1
+
+#define CHECK_EXTRA_FOR_NULL(extra) \
+if (!extra) { \
+	WL_ERROR(("%s: error : extra is null pointer\n", __FUNCTION__)); \
+	return -EINVAL; \
+}
+
+typedef struct wl_iw {
+	char nickname[IW_ESSID_MAX_SIZE];
+
+	struct iw_statistics wstats;
+
+	int spy_num;
+	uint32 pwsec;
+	uint32 gwsec;
+	bool privacy_invoked;
+
+	struct ether_addr spy_addr[IW_MAX_SPY];
+	struct iw_quality spy_qual[IW_MAX_SPY];
+	void  *wlinfo;
+	dhd_pub_t * pub;
+} wl_iw_t;
+
+#define WLC_IW_SS_CACHE_MAXLEN				2048
+#define WLC_IW_SS_CACHE_CTRL_FIELD_MAXLEN	32
+#define WLC_IW_BSS_INFO_MAXLEN 				\
+	(WLC_IW_SS_CACHE_MAXLEN - WLC_IW_SS_CACHE_CTRL_FIELD_MAXLEN)
+
+typedef struct wl_iw_ss_cache {
+	struct wl_iw_ss_cache *next;
+	int dirty;
+	uint32 buflen;
+	uint32 version;
+	uint32 count;
+	wl_bss_info_t bss_info[1];
+} wl_iw_ss_cache_t;
+
+typedef struct wl_iw_ss_cache_ctrl {
+	wl_iw_ss_cache_t *m_cache_head;
+	int m_link_down;
+	int m_timer_expired;
+	char m_active_bssid[ETHER_ADDR_LEN];
+	uint m_prev_scan_mode;
+	uint m_cons_br_scan_cnt;
+	struct timer_list *m_timer;
+} wl_iw_ss_cache_ctrl_t;
+
+typedef enum broadcast_first_scan {
+	BROADCAST_SCAN_FIRST_IDLE = 0,
+	BROADCAST_SCAN_FIRST_STARTED,
+	BROADCAST_SCAN_FIRST_RESULT_READY,
+	BROADCAST_SCAN_FIRST_RESULT_CONSUMED
+} broadcast_first_scan_t;
+#ifdef SOFTAP
+#define SSID_LEN	33
+#define SEC_LEN		16
+#define KEY_LEN		65
+#define PROFILE_OFFSET	32
+struct ap_profile {
+	uint8	ssid[SSID_LEN];
+	uint8	sec[SEC_LEN];
+	uint8	key[KEY_LEN];
+	uint32	channel;
+	uint32	preamble;
+	uint32	max_scb;
+	uint32  closednet;
+	char country_code[WLC_CNTRY_BUF_SZ];
+};
+
+
+#define MACLIST_MODE_DISABLED		0
+#define MACLIST_MODE_DENY		1
+#define MACLIST_MODE_ALLOW		2
+struct mflist {
+	uint count;
+	struct ether_addr ea[16];
+};
+
+struct mac_list_set {
+	uint32	mode;
+	struct mflist mac_list;
+};
+#endif
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+extern const struct iw_handler_def wl_iw_handler_def;
+#endif
+
+extern int wl_iw_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
+extern void wl_iw_event(struct net_device *dev, wl_event_msg_t *e, void* data);
+extern int wl_iw_get_wireless_stats(struct net_device *dev, struct iw_statistics *wstats);
+int wl_iw_attach(struct net_device *dev, void * dhdp);
+void wl_iw_detach(void);
+int wl_control_wl_start(struct net_device *dev);
+
+extern int net_os_wake_lock(struct net_device *dev);
+extern int net_os_wake_unlock(struct net_device *dev);
+extern int net_os_wake_lock_timeout(struct net_device *dev);
+extern int net_os_wake_lock_timeout_enable(struct net_device *dev);
+extern int net_os_set_suspend_disable(struct net_device *dev, int val);
+extern int net_os_set_suspend(struct net_device *dev, int val);
+extern int net_os_set_dtim_skip(struct net_device *dev, int val);
+extern void get_customized_country_code(char *country_iso_code, wl_country_t *cspec);
+extern char *dhd_bus_country_get(struct net_device *dev);
+extern int dhd_get_dtim_skip(dhd_pub_t *dhd);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)
+#define IWE_STREAM_ADD_EVENT(info, stream, ends, iwe, extra) \
+	iwe_stream_add_event(info, stream, ends, iwe, extra)
+#define IWE_STREAM_ADD_VALUE(info, event, value, ends, iwe, event_len) \
+	iwe_stream_add_value(info, event, value, ends, iwe, event_len)
+#define IWE_STREAM_ADD_POINT(info, stream, ends, iwe, extra) \
+	iwe_stream_add_point(info, stream, ends, iwe, extra)
+#else
+#define IWE_STREAM_ADD_EVENT(info, stream, ends, iwe, extra) \
+	iwe_stream_add_event(stream, ends, iwe, extra)
+#define IWE_STREAM_ADD_VALUE(info, event, value, ends, iwe, event_len) \
+	iwe_stream_add_value(event, value, ends, iwe, event_len)
+#define IWE_STREAM_ADD_POINT(info, stream, ends, iwe, extra) \
+	iwe_stream_add_point(stream, ends, iwe, extra)
+#endif
+
+extern int dhd_pno_enable(dhd_pub_t *dhd, int pfn_enabled);
+extern int dhd_pno_clean(dhd_pub_t *dhd);
+extern int dhd_pno_set(dhd_pub_t *dhd, wlc_ssid_t* ssids_local, int nssid, \
+					    ushort  scan_fr, int pno_repeat, int pno_freq_expo_max);
+extern int dhd_pno_get_status(dhd_pub_t *dhd);
+extern int dhd_dev_pno_reset(struct net_device *dev);
+extern int dhd_dev_pno_set(struct net_device *dev, wlc_ssid_t* ssids_local, \
+				 int nssid, ushort  scan_fr, int pno_repeat, int pno_freq_expo_max);
+extern int dhd_dev_pno_enable(struct net_device *dev,  int pfn_enabled);
+extern int dhd_dev_get_pno_status(struct net_device *dev);
+extern void dhd_bus_country_set(struct net_device *dev, wl_country_t *cspec);
+
+#define PNO_TLV_PREFIX			'S'
+#define PNO_TLV_VERSION			'1'
+#define PNO_TLV_SUBVERSION 		'2'
+#define PNO_TLV_RESERVED		'0'
+#define PNO_TLV_TYPE_SSID_IE		'S'
+#define PNO_TLV_TYPE_TIME		'T'
+#define PNO_TLV_FREQ_REPEAT		'R'
+#define PNO_TLV_FREQ_EXPO_MAX	'M'
+#define PNO_EVENT_UP			"PNO_EVENT"
+
+typedef struct cmd_tlv {
+	char prefix;
+	char version;
+	char subver;
+	char reserved;
+} cmd_tlv_t;
+
+#ifdef SOFTAP_TLV_CFG
+#define SOFTAP_SET_CMD				"SOFTAPSET "
+#define SOFTAP_TLV_PREFIX			'A'
+#define SOFTAP_TLV_VERSION			'1'
+#define SOFTAP_TLV_SUBVERSION			'0'
+#define SOFTAP_TLV_RESERVED			'0'
+
+#define TLV_TYPE_SSID				'S'
+#define TLV_TYPE_SECUR				'E'
+#define TLV_TYPE_KEY				'K'
+#define TLV_TYPE_CHANNEL			'C'
+#endif
+
+#if defined(CSCAN)
+
+typedef struct cscan_tlv {
+	char prefix;
+	char version;
+	char subver;
+	char reserved;
+} cscan_tlv_t;
+
+#define CSCAN_COMMAND				"CSCAN "
+#define CSCAN_TLV_PREFIX 			'S'
+#define CSCAN_TLV_VERSION			1
+#define CSCAN_TLV_SUBVERSION			0
+#define CSCAN_TLV_TYPE_SSID_IE			'S'
+#define CSCAN_TLV_TYPE_CHANNEL_IE		'C'
+#define CSCAN_TLV_TYPE_NPROBE_IE		'N'
+#define CSCAN_TLV_TYPE_ACTIVE_IE		'A'
+#define CSCAN_TLV_TYPE_PASSIVE_IE		'P'
+#define CSCAN_TLV_TYPE_HOME_IE			'H'
+#define CSCAN_TLV_TYPE_STYPE_IE			'T'
+
+extern int wl_iw_parse_channel_list_tlv(char** list_str, uint16* channel_list, \
+					int channel_num, int *bytes_left);
+
+extern int wl_iw_parse_data_tlv(char** list_str, void  *dst, int dst_size, \
+					const char token, int input_size, int *bytes_left);
+
+extern int wl_iw_parse_ssid_list_tlv(char** list_str, wlc_ssid_t* ssid, \
+					int max, int *bytes_left);
+
+extern int wl_iw_parse_ssid_list(char** list_str, wlc_ssid_t* ssid, int idx, int max);
+
+extern int wl_iw_parse_channel_list(char** list_str, uint16* channel_list, int channel_num);
+
+#endif
+
+#endif
diff --git a/drivers/net/wireless/bcmdhd/Kconfig b/drivers/net/wireless/bcmdhd/Kconfig
new file mode 100644
index 000000000000..b02a173f98a5
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/Kconfig
@@ -0,0 +1,97 @@
+config BCMDHD
+	tristate "Broadcom 4329/30 wireless cards support"
+	depends on MMC
+	---help---
+	  This module adds support for wireless adapters based on
+	  Broadcom 4329/30 chipset.
+
+	  This driver uses the kernel's wireless extensions subsystem.
+
+	  If you choose to build a module, it'll be called dhd. Say M if
+	  unsure.
+
+config BCMDHD_FW_DIR
+	depends on BCMDHD
+	string "Firmware path"
+	default "/system/vendor/firmware"
+	---help---
+	  Path to the firmware file.
+
+config BCMDHD_NVRAM_DIR
+	depends on BCMDHD
+	string "NVRAM path"
+	default "/system/etc"
+	---help---
+	  Path to the calibration file.
+
+config BCMDHD_WEXT
+	bool "Enable WEXT support"
+	depends on BCMDHD
+	select WIRELESS_EXT
+	select WEXT_PRIV
+	help
+	  Enables WEXT support
+
+config DHD_USE_STATIC_BUF
+       bool "Enable memory preallocation"
+       depends on BCMDHD
+       default n
+       ---help---
+         Use memory preallocated in platform
+
+config DHD_USE_SCHED_SCAN
+	bool "Use CFG80211 sched scan"
+	depends on BCMDHD && CFG80211
+	default n
+	---help---
+	Use CFG80211 sched scan
+
+config DHD_ENABLE_P2P
+	bool "Enable Wifi Direct"
+	depends on BCMDHD && CFG80211
+	default n
+	---help---
+	Use Enable Wifi Direct
+
+config BCMDHD_CFG80211
+	bool "Enable CFG80211 support"
+	depends on BCMDHD && CFG80211
+	default n
+	help
+	  Enables CFG80211 support
+
+config BCMDHD_WIFI_CONTROL_FUNC
+	bool "Use bcmdhd_wlan device"
+	depends on BCMDHD
+	default n
+	---help---
+	  Use this option to get various parameters from architecture specific
+	  bcmdhd_wlan platform device. Say n if unsure.
+
+config BCMDHD_HW_OOB
+	bool "Use out of band interrupt"
+	depends on BCMDHD
+	default n
+	---help---
+	  Use out of band interrupt for card interrupt and wake on wireless.
+
+config BCMDHD_CSCAN_ENABLE
+	bool "Enable Combo Scan"
+	depends on BCMDHD
+	default n
+	---help---
+	Enable Combo Scan
+
+config BCMDHD_INSMOD_NO_FW_LOAD
+	bool "Enable delayed firmware load"
+	depends on BCMDHD
+	default n
+	---help---
+	Enable delayes firmware
+
+config BCMDHD_CUSTOM_REGULATORY_DOMAIN
+	bool "Enable Custom Regulatory Domain"
+	depends on BCMDHD
+	default y
+	---help---
+	Use Custom Regulatory Domain set by driver.
diff --git a/drivers/net/wireless/bcmdhd/Makefile b/drivers/net/wireless/bcmdhd/Makefile
new file mode 100644
index 000000000000..4ec987da11f8
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/Makefile
@@ -0,0 +1,74 @@
+# bcmdhd
+DHDCFLAGS = -Wall -Wstrict-prototypes -Dlinux -DBCMDRIVER                     \
+	-DBCMDONGLEHOST -DUNRELEASEDCHIP -DBCMDMA32 -DWLBTAMP -DBCMFILEIMAGE  \
+	-DDHDTHREAD -DDHD_GPL -DDHD_SCHED -DDHD_DEBUG -DSDTEST -DBDC -DTOE    \
+	-DDHD_BCMEVENTS -DSHOW_EVENTS -DDONGLEOVERLAYS -DBCMDBG               \
+	-DCUSTOMER_HW2    \
+	-DMMC_SDIO_ABORT -DBCMSDIO -DBCMLXSDMMC -DBCMPLATFORM_BUS -DWLP2P     \
+	-DNEW_COMPAT_WIRELESS -DWIFI_ACT_FRAME -DARP_OFFLOAD_SUPPORT          \
+	-DKEEP_ALIVE -DPKT_FILTER_SUPPORT     \
+	-DEMBEDDED_PLATFORM           \
+	-Idrivers/net/wireless/bcmdhd -Idrivers/net/wireless/bcmdhd/include
+
+# Only for JB (disable on ICS)
+DHDCFLAGS += -DSET_RANDOM_MAC_SOFTAP -DWL_CFG80211_STA_EVENT
+
+# for WFD IE support
+DHDCFLAGS += -DWLWFDIE
+
+
+ifeq ($(CONFIG_BCMDHD_WIFI_CONTROL_FUNC),y)
+DHDCFLAGS += -DCONFIG_WIFI_CONTROL_FUNC
+else
+DHDCFLAGS += -DCUSTOM_OOB_GPIO_NUM=2
+endif
+
+ifeq ($(CONFIG_BCMDHD_HW_OOB),y)
+DHDCFLAGS += -DHW_OOB -DOOB_INTR_ONLY
+else
+DHDCFLAGS += -DSDIO_ISR_THREAD
+endif
+
+ifeq ($(CONFIG_BCMDHD_CSCAN_ENABLE),y)
+DHDCFLAGS += -DCSCAN -DPNO_SUPPORT
+endif
+
+ifeq ($(CONFIG_BCMDHD_INSMOD_NO_FW_LOAD),y)
+DHDCFLAGS += -DENABLE_INSMOD_NO_FW_LOAD
+endif
+
+ifeq ($(CONFIG_BCMDHD_CUSTOM_REGULATORY_DOMAIN),y)
+DHDCFLAGS += -DENABLE_CUSTOM_REGULATORY_DOMAIN
+endif
+
+DHDOFILES = aiutils.o bcmsdh_sdmmc_linux.o dhd_linux.o siutils.o bcmutils.o   \
+	dhd_linux_sched.o bcmwifi.o dhd_sdio.o bcmevent.o dhd_bta.o hndpmu.o  \
+	bcmsdh.o dhd_cdc.o bcmsdh_linux.o dhd_common.o linux_osl.o            \
+	bcmsdh_sdmmc.o dhd_custom_gpio.o sbutils.o wldev_common.o wl_android.o
+
+obj-$(CONFIG_BCMDHD) += bcmdhd.o
+bcmdhd-objs += $(DHDOFILES)
+
+ifeq ($(CONFIG_BCMDHD_WEXT),y)
+bcmdhd-objs += wl_iw.o
+DHDCFLAGS += -DSOFTAP -DWL_WIRELESS_EXT
+endif
+
+ifneq ($(CONFIG_BCMDHD_CFG80211),)
+bcmdhd-objs += wl_cfg80211.o dhd_cfg80211.o wl_cfgp2p.o wl_linux_mon.o
+DHDCFLAGS += -DWL_CFG80211
+endif
+
+ifneq ($(CONFIG_DHD_USE_SCHED_SCAN),)
+DHDCFLAGS += -DWL_SCHED_SCAN
+endif
+
+ifneq ($(CONFIG_DHD_ENABLE_P2P),)
+DHDCFLAGS += -DWL_ENABLE_P2P_IF
+endif
+
+EXTRA_CFLAGS = $(DHDCFLAGS)
+
+ifeq ($(CONFIG_BCMDHD),m)
+EXTRA_LDFLAGS += --strip-debug
+endif
diff --git a/drivers/net/wireless/bcmdhd/aiutils.c b/drivers/net/wireless/bcmdhd/aiutils.c
new file mode 100644
index 000000000000..5ca0993c9333
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/aiutils.c
@@ -0,0 +1,675 @@
+/*
+ * Misc utility routines for accessing chip-specific features
+ * of the SiliconBackplane-based Broadcom chips.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: aiutils.c,v 1.26.2.1 2010-03-09 18:41:21 $
+ */
+
+
+#include <typedefs.h>
+#include <bcmdefs.h>
+#include <osl.h>
+#include <bcmutils.h>
+#include <siutils.h>
+#include <hndsoc.h>
+#include <sbchipc.h>
+#include <pcicfg.h>
+
+#include "siutils_priv.h"
+
+
+
+
+
+static uint32
+get_erom_ent(si_t *sih, uint32 **eromptr, uint32 mask, uint32 match)
+{
+	uint32 ent;
+	uint inv = 0, nom = 0;
+
+	while (TRUE) {
+		ent = R_REG(si_osh(sih), *eromptr);
+		(*eromptr)++;
+
+		if (mask == 0)
+			break;
+
+		if ((ent & ER_VALID) == 0) {
+			inv++;
+			continue;
+		}
+
+		if (ent == (ER_END | ER_VALID))
+			break;
+
+		if ((ent & mask) == match)
+			break;
+
+		nom++;
+	}
+
+	SI_VMSG(("%s: Returning ent 0x%08x\n", __FUNCTION__, ent));
+	if (inv + nom) {
+		SI_VMSG(("  after %d invalid and %d non-matching entries\n", inv, nom));
+	}
+	return ent;
+}
+
+static uint32
+get_asd(si_t *sih, uint32 **eromptr, uint sp, uint ad, uint st, uint32 *addrl, uint32 *addrh,
+        uint32 *sizel, uint32 *sizeh)
+{
+	uint32 asd, sz, szd;
+
+	asd = get_erom_ent(sih, eromptr, ER_VALID, ER_VALID);
+	if (((asd & ER_TAG1) != ER_ADD) ||
+	    (((asd & AD_SP_MASK) >> AD_SP_SHIFT) != sp) ||
+	    ((asd & AD_ST_MASK) != st)) {
+		
+		(*eromptr)--;
+		return 0;
+	}
+	*addrl = asd & AD_ADDR_MASK;
+	if (asd & AD_AG32)
+		*addrh = get_erom_ent(sih, eromptr, 0, 0);
+	else
+		*addrh = 0;
+	*sizeh = 0;
+	sz = asd & AD_SZ_MASK;
+	if (sz == AD_SZ_SZD) {
+		szd = get_erom_ent(sih, eromptr, 0, 0);
+		*sizel = szd & SD_SZ_MASK;
+		if (szd & SD_SG32)
+			*sizeh = get_erom_ent(sih, eromptr, 0, 0);
+	} else
+		*sizel = AD_SZ_BASE << (sz >> AD_SZ_SHIFT);
+
+	SI_VMSG(("  SP %d, ad %d: st = %d, 0x%08x_0x%08x @ 0x%08x_0x%08x\n",
+	        sp, ad, st, *sizeh, *sizel, *addrh, *addrl));
+
+	return asd;
+}
+
+static void
+ai_hwfixup(si_info_t *sii)
+{
+}
+
+
+void
+ai_scan(si_t *sih, void *regs, uint devid)
+{
+	si_info_t *sii = SI_INFO(sih);
+	chipcregs_t *cc = (chipcregs_t *)regs;
+	uint32 erombase, *eromptr, *eromlim;
+
+	erombase = R_REG(sii->osh, &cc->eromptr);
+
+	switch (BUSTYPE(sih->bustype)) {
+	case SI_BUS:
+		eromptr = (uint32 *)REG_MAP(erombase, SI_CORE_SIZE);
+		break;
+
+	case PCI_BUS:
+		
+		sii->curwrap = (void *)((uintptr)regs + SI_CORE_SIZE);
+
+		
+		OSL_PCI_WRITE_CONFIG(sii->osh, PCI_BAR0_WIN, 4, erombase);
+		eromptr = regs;
+		break;
+
+	case SPI_BUS:
+	case SDIO_BUS:
+		eromptr = (uint32 *)(uintptr)erombase;
+		break;
+
+	case PCMCIA_BUS:
+	default:
+		SI_ERROR(("Don't know how to do AXI enumertion on bus %d\n", sih->bustype));
+		ASSERT(0);
+		return;
+	}
+	eromlim = eromptr + (ER_REMAPCONTROL / sizeof(uint32));
+
+	SI_VMSG(("ai_scan: regs = 0x%p, erombase = 0x%08x, eromptr = 0x%p, eromlim = 0x%p\n",
+	         regs, erombase, eromptr, eromlim));
+	while (eromptr < eromlim) {
+		uint32 cia, cib, cid, mfg, crev, nmw, nsw, nmp, nsp;
+		uint32 mpd, asd, addrl, addrh, sizel, sizeh;
+		uint32 *base;
+		uint i, j, idx;
+		bool br;
+
+		br = FALSE;
+
+		
+		cia = get_erom_ent(sih, &eromptr, ER_TAG, ER_CI);
+		if (cia == (ER_END | ER_VALID)) {
+			SI_VMSG(("Found END of erom after %d cores\n", sii->numcores));
+			ai_hwfixup(sii);
+			return;
+		}
+		base = eromptr - 1;
+		cib = get_erom_ent(sih, &eromptr, 0, 0);
+
+		if ((cib & ER_TAG) != ER_CI) {
+			SI_ERROR(("CIA not followed by CIB\n"));
+			goto error;
+		}
+
+		cid = (cia & CIA_CID_MASK) >> CIA_CID_SHIFT;
+		mfg = (cia & CIA_MFG_MASK) >> CIA_MFG_SHIFT;
+		crev = (cib & CIB_REV_MASK) >> CIB_REV_SHIFT;
+		nmw = (cib & CIB_NMW_MASK) >> CIB_NMW_SHIFT;
+		nsw = (cib & CIB_NSW_MASK) >> CIB_NSW_SHIFT;
+		nmp = (cib & CIB_NMP_MASK) >> CIB_NMP_SHIFT;
+		nsp = (cib & CIB_NSP_MASK) >> CIB_NSP_SHIFT;
+
+		SI_VMSG(("Found component 0x%04x/0x%04x rev %d at erom addr 0x%p, with nmw = %d, "
+		         "nsw = %d, nmp = %d & nsp = %d\n",
+		         mfg, cid, crev, base, nmw, nsw, nmp, nsp));
+
+		if (((mfg == MFGID_ARM) && (cid == DEF_AI_COMP)) || (nsp == 0))
+			continue;
+		if ((nmw + nsw == 0)) {
+			
+			if (cid == OOB_ROUTER_CORE_ID) {
+				asd = get_asd(sih, &eromptr, 0, 0, AD_ST_SLAVE,
+					&addrl, &addrh, &sizel, &sizeh);
+				if (asd != 0) {
+					sii->oob_router = addrl;
+				}
+			}
+			continue;
+		}
+
+		idx = sii->numcores;
+
+		sii->cia[idx] = cia;
+		sii->cib[idx] = cib;
+		sii->coreid[idx] = cid;
+
+		for (i = 0; i < nmp; i++) {
+			mpd = get_erom_ent(sih, &eromptr, ER_VALID, ER_VALID);
+			if ((mpd & ER_TAG) != ER_MP) {
+				SI_ERROR(("Not enough MP entries for component 0x%x\n", cid));
+				goto error;
+			}
+			SI_VMSG(("  Master port %d, mp: %d id: %d\n", i,
+			         (mpd & MPD_MP_MASK) >> MPD_MP_SHIFT,
+			         (mpd & MPD_MUI_MASK) >> MPD_MUI_SHIFT));
+		}
+
+		
+		asd = get_asd(sih, &eromptr, 0, 0, AD_ST_SLAVE, &addrl, &addrh, &sizel, &sizeh);
+		if (asd == 0) {
+			
+			asd = get_asd(sih, &eromptr, 0, 0, AD_ST_BRIDGE, &addrl, &addrh,
+			              &sizel, &sizeh);
+			if (asd != 0)
+				br = TRUE;
+			else
+				if ((addrh != 0) || (sizeh != 0) || (sizel != SI_CORE_SIZE)) {
+					SI_ERROR(("First Slave ASD for core 0x%04x malformed "
+					          "(0x%08x)\n", cid, asd));
+					goto error;
+				}
+		}
+		sii->coresba[idx] = addrl;
+		sii->coresba_size[idx] = sizel;
+		
+		j = 1;
+		do {
+			asd = get_asd(sih, &eromptr, 0, j, AD_ST_SLAVE, &addrl, &addrh,
+			              &sizel, &sizeh);
+			if ((asd != 0) && (j == 1) && (sizel == SI_CORE_SIZE)) {
+				sii->coresba2[idx] = addrl;
+				sii->coresba2_size[idx] = sizel;
+			}
+			j++;
+		} while (asd != 0);
+
+		
+		for (i = 1; i < nsp; i++) {
+			j = 0;
+			do {
+				asd = get_asd(sih, &eromptr, i, j++, AD_ST_SLAVE, &addrl, &addrh,
+				              &sizel, &sizeh);
+			} while (asd != 0);
+			if (j == 0) {
+				SI_ERROR((" SP %d has no address descriptors\n", i));
+				goto error;
+			}
+		}
+
+		
+		for (i = 0; i < nmw; i++) {
+			asd = get_asd(sih, &eromptr, i, 0, AD_ST_MWRAP, &addrl, &addrh,
+			              &sizel, &sizeh);
+			if (asd == 0) {
+				SI_ERROR(("Missing descriptor for MW %d\n", i));
+				goto error;
+			}
+			if ((sizeh != 0) || (sizel != SI_CORE_SIZE)) {
+				SI_ERROR(("Master wrapper %d is not 4KB\n", i));
+				goto error;
+			}
+			if (i == 0)
+				sii->wrapba[idx] = addrl;
+		}
+
+		
+		for (i = 0; i < nsw; i++) {
+			uint fwp = (nsp == 1) ? 0 : 1;
+			asd = get_asd(sih, &eromptr, fwp + i, 0, AD_ST_SWRAP, &addrl, &addrh,
+			              &sizel, &sizeh);
+			if (asd == 0) {
+				SI_ERROR(("Missing descriptor for SW %d\n", i));
+				goto error;
+			}
+			if ((sizeh != 0) || (sizel != SI_CORE_SIZE)) {
+				SI_ERROR(("Slave wrapper %d is not 4KB\n", i));
+				goto error;
+			}
+			if ((nmw == 0) && (i == 0))
+				sii->wrapba[idx] = addrl;
+		}
+
+		
+		if (br)
+			continue;
+
+		
+		sii->numcores++;
+	}
+
+	SI_ERROR(("Reached end of erom without finding END"));
+
+error:
+	sii->numcores = 0;
+	return;
+}
+
+
+void *
+ai_setcoreidx(si_t *sih, uint coreidx)
+{
+	si_info_t *sii = SI_INFO(sih);
+	uint32 addr = sii->coresba[coreidx];
+	uint32 wrap = sii->wrapba[coreidx];
+	void *regs;
+
+	if (coreidx >= sii->numcores)
+		return (NULL);
+
+	
+	ASSERT((sii->intrsenabled_fn == NULL) || !(*(sii)->intrsenabled_fn)((sii)->intr_arg));
+
+	switch (BUSTYPE(sih->bustype)) {
+	case SI_BUS:
+		
+		if (!sii->regs[coreidx]) {
+			sii->regs[coreidx] = REG_MAP(addr, SI_CORE_SIZE);
+			ASSERT(GOODREGS(sii->regs[coreidx]));
+		}
+		sii->curmap = regs = sii->regs[coreidx];
+		if (!sii->wrappers[coreidx]) {
+			sii->wrappers[coreidx] = REG_MAP(wrap, SI_CORE_SIZE);
+			ASSERT(GOODREGS(sii->wrappers[coreidx]));
+		}
+		sii->curwrap = sii->wrappers[coreidx];
+		break;
+
+
+	case SPI_BUS:
+	case SDIO_BUS:
+		sii->curmap = regs = (void *)((uintptr)addr);
+		sii->curwrap = (void *)((uintptr)wrap);
+		break;
+
+	case PCMCIA_BUS:
+	default:
+		ASSERT(0);
+		regs = NULL;
+		break;
+	}
+
+	sii->curmap = regs;
+	sii->curidx = coreidx;
+
+	return regs;
+}
+
+
+int
+ai_numaddrspaces(si_t *sih)
+{
+	return 2;
+}
+
+
+uint32
+ai_addrspace(si_t *sih, uint asidx)
+{
+	si_info_t *sii;
+	uint cidx;
+
+	sii = SI_INFO(sih);
+	cidx = sii->curidx;
+
+	if (asidx == 0)
+		return sii->coresba[cidx];
+	else if (asidx == 1)
+		return sii->coresba2[cidx];
+	else {
+		SI_ERROR(("%s: Need to parse the erom again to find addr space %d\n",
+		          __FUNCTION__, asidx));
+		return 0;
+	}
+}
+
+
+uint32
+ai_addrspacesize(si_t *sih, uint asidx)
+{
+	si_info_t *sii;
+	uint cidx;
+
+	sii = SI_INFO(sih);
+	cidx = sii->curidx;
+
+	if (asidx == 0)
+		return sii->coresba_size[cidx];
+	else if (asidx == 1)
+		return sii->coresba2_size[cidx];
+	else {
+		SI_ERROR(("%s: Need to parse the erom again to find addr space %d\n",
+		          __FUNCTION__, asidx));
+		return 0;
+	}
+}
+
+uint
+ai_flag(si_t *sih)
+{
+	si_info_t *sii;
+	aidmp_t *ai;
+
+	sii = SI_INFO(sih);
+	ai = sii->curwrap;
+
+	return (R_REG(sii->osh, &ai->oobselouta30) & 0x1f);
+}
+
+void
+ai_setint(si_t *sih, int siflag)
+{
+}
+
+uint
+ai_wrap_reg(si_t *sih, uint32 offset, uint32 mask, uint32 val)
+{
+	si_info_t *sii = SI_INFO(sih);
+	uint32 *map = (uint32 *) sii->curwrap;
+
+	if (mask || val) {
+		uint32 w = R_REG(sii->osh, map+(offset/4));
+		w &= ~mask;
+		w |= val;
+		W_REG(sii->osh, map+(offset/4), val);
+	}
+
+	return (R_REG(sii->osh, map+(offset/4)));
+}
+
+uint
+ai_corevendor(si_t *sih)
+{
+	si_info_t *sii;
+	uint32 cia;
+
+	sii = SI_INFO(sih);
+	cia = sii->cia[sii->curidx];
+	return ((cia & CIA_MFG_MASK) >> CIA_MFG_SHIFT);
+}
+
+uint
+ai_corerev(si_t *sih)
+{
+	si_info_t *sii;
+	uint32 cib;
+
+	sii = SI_INFO(sih);
+	cib = sii->cib[sii->curidx];
+	return ((cib & CIB_REV_MASK) >> CIB_REV_SHIFT);
+}
+
+bool
+ai_iscoreup(si_t *sih)
+{
+	si_info_t *sii;
+	aidmp_t *ai;
+
+	sii = SI_INFO(sih);
+	ai = sii->curwrap;
+
+	return (((R_REG(sii->osh, &ai->ioctrl) & (SICF_FGC | SICF_CLOCK_EN)) == SICF_CLOCK_EN) &&
+	        ((R_REG(sii->osh, &ai->resetctrl) & AIRC_RESET) == 0));
+}
+
+
+uint
+ai_corereg(si_t *sih, uint coreidx, uint regoff, uint mask, uint val)
+{
+	uint origidx = 0;
+	uint32 *r = NULL;
+	uint w;
+	uint intr_val = 0;
+	bool fast = FALSE;
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	ASSERT(GOODIDX(coreidx));
+	ASSERT(regoff < SI_CORE_SIZE);
+	ASSERT((val & ~mask) == 0);
+
+	if (coreidx >= SI_MAXCORES)
+		return 0;
+
+	if (BUSTYPE(sih->bustype) == SI_BUS) {
+		
+		fast = TRUE;
+		
+		if (!sii->regs[coreidx]) {
+			sii->regs[coreidx] = REG_MAP(sii->coresba[coreidx],
+			                            SI_CORE_SIZE);
+			ASSERT(GOODREGS(sii->regs[coreidx]));
+		}
+		r = (uint32 *)((uchar *)sii->regs[coreidx] + regoff);
+	} else if (BUSTYPE(sih->bustype) == PCI_BUS) {
+		
+
+		if ((sii->coreid[coreidx] == CC_CORE_ID) && SI_FAST(sii)) {
+			
+
+			fast = TRUE;
+			r = (uint32 *)((char *)sii->curmap + PCI_16KB0_CCREGS_OFFSET + regoff);
+		} else if (sii->pub.buscoreidx == coreidx) {
+			
+			fast = TRUE;
+			if (SI_FAST(sii))
+				r = (uint32 *)((char *)sii->curmap +
+				               PCI_16KB0_PCIREGS_OFFSET + regoff);
+			else
+				r = (uint32 *)((char *)sii->curmap +
+				               ((regoff >= SBCONFIGOFF) ?
+				                PCI_BAR0_PCISBR_OFFSET : PCI_BAR0_PCIREGS_OFFSET) +
+				               regoff);
+		}
+	}
+
+	if (!fast) {
+		INTR_OFF(sii, intr_val);
+
+		
+		origidx = si_coreidx(&sii->pub);
+
+		
+		r = (uint32*) ((uchar*) ai_setcoreidx(&sii->pub, coreidx) + regoff);
+	}
+	ASSERT(r != NULL);
+
+	
+	if (mask || val) {
+		w = (R_REG(sii->osh, r) & ~mask) | val;
+		W_REG(sii->osh, r, w);
+	}
+
+	
+	w = R_REG(sii->osh, r);
+
+	if (!fast) {
+		
+		if (origidx != coreidx)
+			ai_setcoreidx(&sii->pub, origidx);
+
+		INTR_RESTORE(sii, intr_val);
+	}
+
+	return (w);
+}
+
+void
+ai_core_disable(si_t *sih, uint32 bits)
+{
+	si_info_t *sii;
+	volatile uint32 dummy;
+	aidmp_t *ai;
+
+	sii = SI_INFO(sih);
+
+	ASSERT(GOODREGS(sii->curwrap));
+	ai = sii->curwrap;
+
+	
+	if (R_REG(sii->osh, &ai->resetctrl) & AIRC_RESET)
+		return;
+
+	W_REG(sii->osh, &ai->ioctrl, bits);
+	dummy = R_REG(sii->osh, &ai->ioctrl);
+	OSL_DELAY(10);
+
+	W_REG(sii->osh, &ai->resetctrl, AIRC_RESET);
+	OSL_DELAY(1);
+}
+
+
+void
+ai_core_reset(si_t *sih, uint32 bits, uint32 resetbits)
+{
+	si_info_t *sii;
+	aidmp_t *ai;
+	volatile uint32 dummy;
+
+	sii = SI_INFO(sih);
+	ASSERT(GOODREGS(sii->curwrap));
+	ai = sii->curwrap;
+
+	
+	ai_core_disable(sih, (bits | resetbits));
+
+	
+	W_REG(sii->osh, &ai->ioctrl, (bits | SICF_FGC | SICF_CLOCK_EN));
+	dummy = R_REG(sii->osh, &ai->ioctrl);
+	W_REG(sii->osh, &ai->resetctrl, 0);
+	OSL_DELAY(1);
+
+	W_REG(sii->osh, &ai->ioctrl, (bits | SICF_CLOCK_EN));
+	dummy = R_REG(sii->osh, &ai->ioctrl);
+	OSL_DELAY(1);
+}
+
+
+void
+ai_core_cflags_wo(si_t *sih, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	aidmp_t *ai;
+	uint32 w;
+
+	sii = SI_INFO(sih);
+	ASSERT(GOODREGS(sii->curwrap));
+	ai = sii->curwrap;
+
+	ASSERT((val & ~mask) == 0);
+
+	if (mask || val) {
+		w = ((R_REG(sii->osh, &ai->ioctrl) & ~mask) | val);
+		W_REG(sii->osh, &ai->ioctrl, w);
+	}
+}
+
+uint32
+ai_core_cflags(si_t *sih, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	aidmp_t *ai;
+	uint32 w;
+
+	sii = SI_INFO(sih);
+	ASSERT(GOODREGS(sii->curwrap));
+	ai = sii->curwrap;
+
+	ASSERT((val & ~mask) == 0);
+
+	if (mask || val) {
+		w = ((R_REG(sii->osh, &ai->ioctrl) & ~mask) | val);
+		W_REG(sii->osh, &ai->ioctrl, w);
+	}
+
+	return R_REG(sii->osh, &ai->ioctrl);
+}
+
+uint32
+ai_core_sflags(si_t *sih, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	aidmp_t *ai;
+	uint32 w;
+
+	sii = SI_INFO(sih);
+	ASSERT(GOODREGS(sii->curwrap));
+	ai = sii->curwrap;
+
+	ASSERT((val & ~mask) == 0);
+	ASSERT((mask & ~SISF_CORE_BITS) == 0);
+
+	if (mask || val) {
+		w = ((R_REG(sii->osh, &ai->iostatus) & ~mask) | val);
+		W_REG(sii->osh, &ai->iostatus, w);
+	}
+
+	return R_REG(sii->osh, &ai->iostatus);
+}
diff --git a/drivers/net/wireless/bcmdhd/bcmevent.c b/drivers/net/wireless/bcmdhd/bcmevent.c
new file mode 100644
index 000000000000..84ab7564ad7d
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/bcmevent.c
@@ -0,0 +1,126 @@
+/*
+ * bcmevent read-only data shared by kernel or app layers
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: bcmevent.c,v 1.8.2.7 2011-02-01 06:23:39 $
+ */
+
+#include <typedefs.h>
+#include <bcmutils.h>
+#include <proto/ethernet.h>
+#include <proto/bcmeth.h>
+#include <proto/bcmevent.h>
+
+#if WLC_E_LAST != 87
+#error "You need to add an entry to bcmevent_names[] for the new event"
+#endif
+
+const bcmevent_name_t bcmevent_names[] = {
+	{ WLC_E_SET_SSID, "SET_SSID" },
+	{ WLC_E_JOIN, "JOIN" },
+	{ WLC_E_START, "START" },
+	{ WLC_E_AUTH, "AUTH" },
+	{ WLC_E_AUTH_IND, "AUTH_IND" },
+	{ WLC_E_DEAUTH, "DEAUTH" },
+	{ WLC_E_DEAUTH_IND, "DEAUTH_IND" },
+	{ WLC_E_ASSOC, "ASSOC" },
+	{ WLC_E_ASSOC_IND, "ASSOC_IND" },
+	{ WLC_E_REASSOC, "REASSOC" },
+	{ WLC_E_REASSOC_IND, "REASSOC_IND" },
+	{ WLC_E_DISASSOC, "DISASSOC" },
+	{ WLC_E_DISASSOC_IND, "DISASSOC_IND" },
+	{ WLC_E_QUIET_START, "START_QUIET" },
+	{ WLC_E_QUIET_END, "END_QUIET" },
+	{ WLC_E_BEACON_RX, "BEACON_RX" },
+	{ WLC_E_LINK, "LINK" },
+	{ WLC_E_MIC_ERROR, "MIC_ERROR" },
+	{ WLC_E_NDIS_LINK, "NDIS_LINK" },
+	{ WLC_E_ROAM, "ROAM" },
+	{ WLC_E_PMKID_CACHE, "PMKID_CACHE" },
+	{ WLC_E_RETROGRADE_TSF, "RETROGRADE_TSF" },
+	{ WLC_E_PRUNE, "PRUNE" },
+	{ WLC_E_AUTOAUTH, "AUTOAUTH" },
+	{ WLC_E_EAPOL_MSG, "EAPOL_MSG" },
+	{ WLC_E_SCAN_COMPLETE, "SCAN_COMPLETE" },
+	{ WLC_E_ADDTS_IND, "ADDTS_IND" },
+	{ WLC_E_DELTS_IND, "DELTS_IND" },
+	{ WLC_E_BCNSENT_IND, "BCNSENT_IND" },
+	{ WLC_E_BCNRX_MSG, "BCNRX_MSG" },
+	{ WLC_E_BCNLOST_MSG, "BCNLOST_IND" },
+	{ WLC_E_ROAM_PREP, "ROAM_PREP" },
+	{ WLC_E_PFN_NET_FOUND, "PFNFOUND_IND" },
+	{ WLC_E_PFN_NET_LOST, "PFNLOST_IND" },
+#if defined(IBSS_PEER_DISCOVERY_EVENT)
+	{ WLC_E_IBSS_ASSOC, "IBSS_ASSOC" },
+#endif /* defined(IBSS_PEER_DISCOVERY_EVENT) */
+	{ WLC_E_RADIO, "RADIO" },
+	{ WLC_E_PSM_WATCHDOG, "PSM_WATCHDOG" },
+	{ WLC_E_PROBREQ_MSG, "PROBE_REQ_MSG" },
+	{ WLC_E_SCAN_CONFIRM_IND, "SCAN_CONFIRM_IND" },
+	{ WLC_E_PSK_SUP, "PSK_SUP" },
+	{ WLC_E_COUNTRY_CODE_CHANGED, "CNTRYCODE_IND" },
+	{ WLC_E_EXCEEDED_MEDIUM_TIME, "EXCEEDED_MEDIUM_TIME" },
+	{ WLC_E_ICV_ERROR, "ICV_ERROR" },
+	{ WLC_E_UNICAST_DECODE_ERROR, "UNICAST_DECODE_ERROR" },
+	{ WLC_E_MULTICAST_DECODE_ERROR, "MULTICAST_DECODE_ERROR" },
+	{ WLC_E_TRACE, "TRACE" },
+	{ WLC_E_BTA_HCI_EVENT, "BTA_HCI_EVENT" },
+	{ WLC_E_IF, "IF" },
+#ifdef WLP2P
+	{ WLC_E_P2P_DISC_LISTEN_COMPLETE, "WLC_E_P2P_DISC_LISTEN_COMPLETE" },
+#endif
+	{ WLC_E_RSSI, "RSSI" },
+	{ WLC_E_PFN_SCAN_COMPLETE, "SCAN_COMPLETE" },
+	{ WLC_E_EXTLOG_MSG, "EXTERNAL LOG MESSAGE" },
+#ifdef WIFI_ACT_FRAME
+	{ WLC_E_ACTION_FRAME, "ACTION_FRAME" },
+	{ WLC_E_ACTION_FRAME_RX, "ACTION_FRAME_RX" },
+	{ WLC_E_ACTION_FRAME_COMPLETE, "ACTION_FRAME_COMPLETE" },
+#endif
+	{ WLC_E_ESCAN_RESULT, "WLC_E_ESCAN_RESULT" },
+	{ WLC_E_ACTION_FRAME_OFF_CHAN_COMPLETE, "WLC_E_AF_OFF_CHAN_COMPLETE" },
+#ifdef WLP2P
+	{ WLC_E_PROBRESP_MSG, "PROBE_RESP_MSG" },
+	{ WLC_E_P2P_PROBREQ_MSG, "P2P PROBE_REQ_MSG" },
+#endif
+#ifdef PROP_TXSTATUS
+	{ WLC_E_FIFO_CREDIT_MAP, "FIFO_CREDIT_MAP" },
+#endif
+	{ WLC_E_WAKE_EVENT, "WAKE_EVENT" },
+	{ WLC_E_DCS_REQUEST, "DCS_REQUEST" },
+	{ WLC_E_RM_COMPLETE, "RM_COMPLETE" },
+#ifdef WLMEDIA_HTSF
+	{ WLC_E_HTSFSYNC, "HTSF_SYNC_EVENT" },
+#endif
+	{ WLC_E_OVERLAY_REQ, "OVERLAY_REQ_EVENT" },
+	{ WLC_E_CSA_COMPLETE_IND, "WLC_E_CSA_COMPLETE_IND" },
+	{ WLC_E_EXCESS_PM_WAKE_EVENT, "EXCESS_PM_WAKE_EVENT" },
+	{ WLC_E_PFN_SCAN_NONE, "PFN_SCAN_NONE" },
+	{ WLC_E_PFN_SCAN_ALLGONE, "PFN_SCAN_ALLGONE" },
+#ifdef SOFTAP
+	{ WLC_E_GTK_PLUMBED, "GTK_PLUMBED" },
+#endif
+	{ WLC_E_ASSOC_REQ_IE, "ASSOC_REQ_IE" },
+	{ WLC_E_ASSOC_RESP_IE, "ASSOC_RESP_IE" }
+};
+
+
+const int bcmevent_names_size = ARRAYSIZE(bcmevent_names);
diff --git a/drivers/net/wireless/bcmdhd/bcmsdh.c b/drivers/net/wireless/bcmdhd/bcmsdh.c
new file mode 100644
index 000000000000..f67b13a03a1c
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/bcmsdh.c
@@ -0,0 +1,690 @@
+/*
+ *  BCMSDH interface glue
+ *  implement bcmsdh API for SDIOH driver
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdh.c 300445 2011-12-03 05:37:20Z $
+ */
+
+/**
+ * @file bcmsdh.c
+ */
+
+/* ****************** BCMSDH Interface Functions *************************** */
+
+#include <typedefs.h>
+#include <bcmdevs.h>
+#include <bcmendian.h>
+#include <bcmutils.h>
+#include <hndsoc.h>
+#include <siutils.h>
+#include <osl.h>
+
+#include <bcmsdh.h>	/* BRCM API for SDIO clients (such as wl, dhd) */
+#include <bcmsdbus.h>	/* common SDIO/controller interface */
+#include <sbsdio.h>	/* SDIO device core hardware definitions. */
+
+#include <sdio.h>	/* SDIO Device and Protocol Specs */
+
+#define SDIOH_API_ACCESS_RETRY_LIMIT	2
+const uint bcmsdh_msglevel = BCMSDH_ERROR_VAL;
+
+/**
+ * BCMSDH API context
+ */
+struct bcmsdh_info
+{
+	bool	init_success;	/* underlying driver successfully attached */
+	void	*sdioh;		/* handler for sdioh */
+	uint32  vendevid;	/* Target Vendor and Device ID on SD bus */
+	osl_t   *osh;
+	bool	regfail;	/* Save status of last reg_read/reg_write call */
+	uint32	sbwad;		/* Save backplane window address */
+};
+/* local copy of bcm sd handler */
+bcmsdh_info_t * l_bcmsdh = NULL;
+
+#if defined(OOB_INTR_ONLY) && defined(HW_OOB)
+extern int
+sdioh_enable_hw_oob_intr(void *sdioh, bool enable);
+
+void
+bcmsdh_enable_hw_oob_intr(bcmsdh_info_t *sdh, bool enable)
+{
+	sdioh_enable_hw_oob_intr(sdh->sdioh, enable);
+}
+#endif
+
+/* Attach BCMSDH layer to SDIO Host Controller Driver
+ *
+ * @param osh OSL Handle.
+ * @param cfghdl Configuration Handle.
+ * @param regsva Virtual address of controller registers.
+ * @param irq Interrupt number of SDIO controller.
+ *
+ * @return bcmsdh_info_t Handle to BCMSDH context.
+ */
+bcmsdh_info_t *
+bcmsdh_attach(osl_t *osh, void *cfghdl, void **regsva, uint irq)
+{
+	bcmsdh_info_t *bcmsdh;
+
+	if ((bcmsdh = (bcmsdh_info_t *)MALLOC(osh, sizeof(bcmsdh_info_t))) == NULL) {
+		BCMSDH_ERROR(("bcmsdh_attach: out of memory, malloced %d bytes\n", MALLOCED(osh)));
+		return NULL;
+	}
+	bzero((char *)bcmsdh, sizeof(bcmsdh_info_t));
+
+	/* save the handler locally */
+	l_bcmsdh = bcmsdh;
+
+	if (!(bcmsdh->sdioh = sdioh_attach(osh, cfghdl, irq))) {
+		bcmsdh_detach(osh, bcmsdh);
+		return NULL;
+	}
+
+	bcmsdh->osh = osh;
+	bcmsdh->init_success = TRUE;
+
+	*regsva = (uint32 *)SI_ENUM_BASE;
+
+	/* Report the BAR, to fix if needed */
+	bcmsdh->sbwad = SI_ENUM_BASE;
+	return bcmsdh;
+}
+
+int
+bcmsdh_detach(osl_t *osh, void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	if (bcmsdh != NULL) {
+		if (bcmsdh->sdioh) {
+			sdioh_detach(osh, bcmsdh->sdioh);
+			bcmsdh->sdioh = NULL;
+		}
+		MFREE(osh, bcmsdh, sizeof(bcmsdh_info_t));
+	}
+
+	l_bcmsdh = NULL;
+	return 0;
+}
+
+int
+bcmsdh_iovar_op(void *sdh, const char *name,
+                void *params, int plen, void *arg, int len, bool set)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	return sdioh_iovar_op(bcmsdh->sdioh, name, params, plen, arg, len, set);
+}
+
+bool
+bcmsdh_intr_query(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	bool on;
+
+	ASSERT(bcmsdh);
+	status = sdioh_interrupt_query(bcmsdh->sdioh, &on);
+	if (SDIOH_API_SUCCESS(status))
+		return FALSE;
+	else
+		return on;
+}
+
+int
+bcmsdh_intr_enable(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	ASSERT(bcmsdh);
+
+	status = sdioh_interrupt_set(bcmsdh->sdioh, TRUE);
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_ERROR);
+}
+
+int
+bcmsdh_intr_disable(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	ASSERT(bcmsdh);
+
+	status = sdioh_interrupt_set(bcmsdh->sdioh, FALSE);
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_ERROR);
+}
+
+int
+bcmsdh_intr_reg(void *sdh, bcmsdh_cb_fn_t fn, void *argh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	ASSERT(bcmsdh);
+
+	status = sdioh_interrupt_register(bcmsdh->sdioh, fn, argh);
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_ERROR);
+}
+
+int
+bcmsdh_intr_dereg(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	ASSERT(bcmsdh);
+
+	status = sdioh_interrupt_deregister(bcmsdh->sdioh);
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_ERROR);
+}
+
+#if defined(DHD_DEBUG)
+bool
+bcmsdh_intr_pending(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	ASSERT(sdh);
+	return sdioh_interrupt_pending(bcmsdh->sdioh);
+}
+#endif
+
+
+int
+bcmsdh_devremove_reg(void *sdh, bcmsdh_cb_fn_t fn, void *argh)
+{
+	ASSERT(sdh);
+
+	/* don't support yet */
+	return BCME_UNSUPPORTED;
+}
+
+/**
+ * Read from SDIO Configuration Space
+ * @param sdh SDIO Host context.
+ * @param func_num Function number to read from.
+ * @param addr Address to read from.
+ * @param err Error return.
+ * @return value read from SDIO configuration space.
+ */
+uint8
+bcmsdh_cfg_read(void *sdh, uint fnc_num, uint32 addr, int *err)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+#ifdef SDIOH_API_ACCESS_RETRY_LIMIT
+	int32 retry = 0;
+#endif
+	uint8 data = 0;
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	ASSERT(bcmsdh->init_success);
+
+#ifdef SDIOH_API_ACCESS_RETRY_LIMIT
+	do {
+		if (retry)	/* wait for 1 ms till bus get settled down */
+			OSL_DELAY(1000);
+#endif
+	status = sdioh_cfg_read(bcmsdh->sdioh, fnc_num, addr, (uint8 *)&data);
+#ifdef SDIOH_API_ACCESS_RETRY_LIMIT
+	} while (!SDIOH_API_SUCCESS(status) && (retry++ < SDIOH_API_ACCESS_RETRY_LIMIT));
+#endif
+	if (err)
+		*err = (SDIOH_API_SUCCESS(status) ? 0 : BCME_SDIO_ERROR);
+
+	BCMSDH_INFO(("%s:fun = %d, addr = 0x%x, uint8data = 0x%x\n", __FUNCTION__,
+	            fnc_num, addr, data));
+
+	return data;
+}
+
+void
+bcmsdh_cfg_write(void *sdh, uint fnc_num, uint32 addr, uint8 data, int *err)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+#ifdef SDIOH_API_ACCESS_RETRY_LIMIT
+	int32 retry = 0;
+#endif
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	ASSERT(bcmsdh->init_success);
+
+#ifdef SDIOH_API_ACCESS_RETRY_LIMIT
+	do {
+		if (retry)	/* wait for 1 ms till bus get settled down */
+			OSL_DELAY(1000);
+#endif
+	status = sdioh_cfg_write(bcmsdh->sdioh, fnc_num, addr, (uint8 *)&data);
+#ifdef SDIOH_API_ACCESS_RETRY_LIMIT
+	} while (!SDIOH_API_SUCCESS(status) && (retry++ < SDIOH_API_ACCESS_RETRY_LIMIT));
+#endif
+	if (err)
+		*err = SDIOH_API_SUCCESS(status) ? 0 : BCME_SDIO_ERROR;
+
+	BCMSDH_INFO(("%s:fun = %d, addr = 0x%x, uint8data = 0x%x\n", __FUNCTION__,
+	            fnc_num, addr, data));
+}
+
+uint32
+bcmsdh_cfg_read_word(void *sdh, uint fnc_num, uint32 addr, int *err)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	uint32 data = 0;
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	ASSERT(bcmsdh->init_success);
+
+	status = sdioh_request_word(bcmsdh->sdioh, SDIOH_CMD_TYPE_NORMAL, SDIOH_READ, fnc_num,
+	                            addr, &data, 4);
+
+	if (err)
+		*err = (SDIOH_API_SUCCESS(status) ? 0 : BCME_SDIO_ERROR);
+
+	BCMSDH_INFO(("%s:fun = %d, addr = 0x%x, uint32data = 0x%x\n", __FUNCTION__,
+	            fnc_num, addr, data));
+
+	return data;
+}
+
+void
+bcmsdh_cfg_write_word(void *sdh, uint fnc_num, uint32 addr, uint32 data, int *err)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	ASSERT(bcmsdh->init_success);
+
+	status = sdioh_request_word(bcmsdh->sdioh, SDIOH_CMD_TYPE_NORMAL, SDIOH_WRITE, fnc_num,
+	                            addr, &data, 4);
+
+	if (err)
+		*err = (SDIOH_API_SUCCESS(status) ? 0 : BCME_SDIO_ERROR);
+
+	BCMSDH_INFO(("%s:fun = %d, addr = 0x%x, uint32data = 0x%x\n", __FUNCTION__, fnc_num,
+	             addr, data));
+}
+
+
+int
+bcmsdh_cis_read(void *sdh, uint func, uint8 *cis, uint length)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+
+	uint8 *tmp_buf, *tmp_ptr;
+	uint8 *ptr;
+	bool ascii = func & ~0xf;
+	func &= 0x7;
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	ASSERT(bcmsdh->init_success);
+	ASSERT(cis);
+	ASSERT(length <= SBSDIO_CIS_SIZE_LIMIT);
+
+	status = sdioh_cis_read(bcmsdh->sdioh, func, cis, length);
+
+	if (ascii) {
+		/* Move binary bits to tmp and format them into the provided buffer. */
+		if ((tmp_buf = (uint8 *)MALLOC(bcmsdh->osh, length)) == NULL) {
+			BCMSDH_ERROR(("%s: out of memory\n", __FUNCTION__));
+			return BCME_NOMEM;
+		}
+		bcopy(cis, tmp_buf, length);
+		for (tmp_ptr = tmp_buf, ptr = cis; ptr < (cis + length - 4); tmp_ptr++) {
+			ptr += sprintf((char*)ptr, "%.2x ", *tmp_ptr & 0xff);
+			if ((((tmp_ptr - tmp_buf) + 1) & 0xf) == 0)
+				ptr += sprintf((char *)ptr, "\n");
+		}
+		MFREE(bcmsdh->osh, tmp_buf, length);
+	}
+
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_ERROR);
+}
+
+
+int
+bcmsdhsdio_set_sbaddr_window(void *sdh, uint32 address, bool force_set)
+{
+	int err = 0;
+	uint bar0 = address & ~SBSDIO_SB_OFT_ADDR_MASK;
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	if (bar0 != bcmsdh->sbwad || force_set) {
+		bcmsdh_cfg_write(bcmsdh, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRLOW,
+			(address >> 8) & SBSDIO_SBADDRLOW_MASK, &err);
+		if (!err)
+			bcmsdh_cfg_write(bcmsdh, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRMID,
+				(address >> 16) & SBSDIO_SBADDRMID_MASK, &err);
+		if (!err)
+			bcmsdh_cfg_write(bcmsdh, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRHIGH,
+				(address >> 24) & SBSDIO_SBADDRHIGH_MASK, &err);
+
+		if (!err)
+			bcmsdh->sbwad = bar0;
+		else
+			/* invalidate cached window var */
+			bcmsdh->sbwad = 0;
+
+	}
+
+	return err;
+}
+
+uint32
+bcmsdh_reg_read(void *sdh, uint32 addr, uint size)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	uint32 word = 0;
+
+	BCMSDH_INFO(("%s:fun = 1, addr = 0x%x, ", __FUNCTION__, addr));
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	ASSERT(bcmsdh->init_success);
+
+	if (bcmsdhsdio_set_sbaddr_window(bcmsdh, addr, FALSE))
+		return 0xFFFFFFFF;
+
+	addr &= SBSDIO_SB_OFT_ADDR_MASK;
+	if (size == 4)
+		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
+
+	status = sdioh_request_word(bcmsdh->sdioh, SDIOH_CMD_TYPE_NORMAL,
+		SDIOH_READ, SDIO_FUNC_1, addr, &word, size);
+
+	bcmsdh->regfail = !(SDIOH_API_SUCCESS(status));
+
+	BCMSDH_INFO(("uint32data = 0x%x\n", word));
+
+	/* if ok, return appropriately masked word */
+	if (SDIOH_API_SUCCESS(status)) {
+		switch (size) {
+			case sizeof(uint8):
+				return (word & 0xff);
+			case sizeof(uint16):
+				return (word & 0xffff);
+			case sizeof(uint32):
+				return word;
+			default:
+				bcmsdh->regfail = TRUE;
+
+		}
+	}
+
+	/* otherwise, bad sdio access or invalid size */
+	BCMSDH_ERROR(("%s: error reading addr 0x%04x size %d\n", __FUNCTION__, addr, size));
+	return 0xFFFFFFFF;
+}
+
+uint32
+bcmsdh_reg_write(void *sdh, uint32 addr, uint size, uint32 data)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	int err = 0;
+
+	BCMSDH_INFO(("%s:fun = 1, addr = 0x%x, uint%ddata = 0x%x\n",
+	             __FUNCTION__, addr, size*8, data));
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	ASSERT(bcmsdh->init_success);
+
+	if ((err = bcmsdhsdio_set_sbaddr_window(bcmsdh, addr, FALSE)))
+		return err;
+
+	addr &= SBSDIO_SB_OFT_ADDR_MASK;
+	if (size == 4)
+		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
+	status = sdioh_request_word(bcmsdh->sdioh, SDIOH_CMD_TYPE_NORMAL, SDIOH_WRITE, SDIO_FUNC_1,
+	                            addr, &data, size);
+	bcmsdh->regfail = !(SDIOH_API_SUCCESS(status));
+
+	if (SDIOH_API_SUCCESS(status))
+		return 0;
+
+	BCMSDH_ERROR(("%s: error writing 0x%08x to addr 0x%04x size %d\n",
+	              __FUNCTION__, data, addr, size));
+	return 0xFFFFFFFF;
+}
+
+bool
+bcmsdh_regfail(void *sdh)
+{
+	return ((bcmsdh_info_t *)sdh)->regfail;
+}
+
+int
+bcmsdh_recv_buf(void *sdh, uint32 addr, uint fn, uint flags,
+                uint8 *buf, uint nbytes, void *pkt,
+                bcmsdh_cmplt_fn_t complete_fn, void *handle)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	uint incr_fix;
+	uint width;
+	int err = 0;
+
+	ASSERT(bcmsdh);
+	ASSERT(bcmsdh->init_success);
+
+	BCMSDH_INFO(("%s:fun = %d, addr = 0x%x, size = %d\n",
+	             __FUNCTION__, fn, addr, nbytes));
+
+	/* Async not implemented yet */
+	ASSERT(!(flags & SDIO_REQ_ASYNC));
+	if (flags & SDIO_REQ_ASYNC)
+		return BCME_UNSUPPORTED;
+
+	if ((err = bcmsdhsdio_set_sbaddr_window(bcmsdh, addr, FALSE)))
+		return err;
+
+	addr &= SBSDIO_SB_OFT_ADDR_MASK;
+
+	incr_fix = (flags & SDIO_REQ_FIXED) ? SDIOH_DATA_FIX : SDIOH_DATA_INC;
+	width = (flags & SDIO_REQ_4BYTE) ? 4 : 2;
+	if (width == 4)
+		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
+
+	status = sdioh_request_buffer(bcmsdh->sdioh, SDIOH_DATA_PIO, incr_fix,
+	                              SDIOH_READ, fn, addr, width, nbytes, buf, pkt);
+
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_SDIO_ERROR);
+}
+
+int
+bcmsdh_send_buf(void *sdh, uint32 addr, uint fn, uint flags,
+                uint8 *buf, uint nbytes, void *pkt,
+                bcmsdh_cmplt_fn_t complete_fn, void *handle)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+	uint incr_fix;
+	uint width;
+	int err = 0;
+
+	ASSERT(bcmsdh);
+	ASSERT(bcmsdh->init_success);
+
+	BCMSDH_INFO(("%s:fun = %d, addr = 0x%x, size = %d\n",
+	            __FUNCTION__, fn, addr, nbytes));
+
+	/* Async not implemented yet */
+	ASSERT(!(flags & SDIO_REQ_ASYNC));
+	if (flags & SDIO_REQ_ASYNC)
+		return BCME_UNSUPPORTED;
+
+	if ((err = bcmsdhsdio_set_sbaddr_window(bcmsdh, addr, FALSE)))
+		return err;
+
+	addr &= SBSDIO_SB_OFT_ADDR_MASK;
+
+	incr_fix = (flags & SDIO_REQ_FIXED) ? SDIOH_DATA_FIX : SDIOH_DATA_INC;
+	width = (flags & SDIO_REQ_4BYTE) ? 4 : 2;
+	if (width == 4)
+		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
+
+	status = sdioh_request_buffer(bcmsdh->sdioh, SDIOH_DATA_PIO, incr_fix,
+	                              SDIOH_WRITE, fn, addr, width, nbytes, buf, pkt);
+
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_ERROR);
+}
+
+int
+bcmsdh_rwdata(void *sdh, uint rw, uint32 addr, uint8 *buf, uint nbytes)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	SDIOH_API_RC status;
+
+	ASSERT(bcmsdh);
+	ASSERT(bcmsdh->init_success);
+	ASSERT((addr & SBSDIO_SBWINDOW_MASK) == 0);
+
+	addr &= SBSDIO_SB_OFT_ADDR_MASK;
+	addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
+
+	status = sdioh_request_buffer(bcmsdh->sdioh, SDIOH_DATA_PIO, SDIOH_DATA_INC,
+	                              (rw ? SDIOH_WRITE : SDIOH_READ), SDIO_FUNC_1,
+	                              addr, 4, nbytes, buf, NULL);
+
+	return (SDIOH_API_SUCCESS(status) ? 0 : BCME_ERROR);
+}
+
+int
+bcmsdh_abort(void *sdh, uint fn)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	return sdioh_abort(bcmsdh->sdioh, fn);
+}
+
+int
+bcmsdh_start(void *sdh, int stage)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	return sdioh_start(bcmsdh->sdioh, stage);
+}
+
+int
+bcmsdh_stop(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	return sdioh_stop(bcmsdh->sdioh);
+}
+
+int
+bcmsdh_waitlockfree(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	return sdioh_waitlockfree(bcmsdh->sdioh);
+}
+
+
+int
+bcmsdh_query_device(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+	bcmsdh->vendevid = (VENDOR_BROADCOM << 16) | 0;
+	return (bcmsdh->vendevid);
+}
+
+uint
+bcmsdh_query_iofnum(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	return (sdioh_query_iofnum(bcmsdh->sdioh));
+}
+
+int
+bcmsdh_reset(bcmsdh_info_t *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	return sdioh_sdio_reset(bcmsdh->sdioh);
+}
+
+void *bcmsdh_get_sdioh(bcmsdh_info_t *sdh)
+{
+	ASSERT(sdh);
+	return sdh->sdioh;
+}
+
+/* Function to pass device-status bits to DHD. */
+uint32
+bcmsdh_get_dstatus(void *sdh)
+{
+	return 0;
+}
+uint32
+bcmsdh_cur_sbwad(void *sdh)
+{
+	bcmsdh_info_t *bcmsdh = (bcmsdh_info_t *)sdh;
+
+	if (!bcmsdh)
+		bcmsdh = l_bcmsdh;
+
+	return (bcmsdh->sbwad);
+}
+
+void
+bcmsdh_chipinfo(void *sdh, uint32 chip, uint32 chiprev)
+{
+	return;
+}
+
+
+int
+bcmsdh_sleep(void *sdh, bool enab)
+{
+#ifdef SDIOH_SLEEP_ENABLED
+	bcmsdh_info_t *p = (bcmsdh_info_t *)sdh;
+	sdioh_info_t *sd = (sdioh_info_t *)(p->sdioh);
+
+	return sdioh_sleep(sd, enab);
+#else
+	return BCME_UNSUPPORTED;
+#endif
+}
diff --git a/drivers/net/wireless/bcmdhd/bcmsdh_linux.c b/drivers/net/wireless/bcmdhd/bcmsdh_linux.c
new file mode 100644
index 000000000000..237af4ae8417
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/bcmsdh_linux.c
@@ -0,0 +1,730 @@
+/*
+ * SDIO access interface for drivers - linux specific (pci only)
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdh_linux.c 312788 2012-02-03 23:06:32Z $
+ */
+
+/**
+ * @file bcmsdh_linux.c
+ */
+
+#define __UNDEF_NO_VERSION__
+
+#include <typedefs.h>
+#include <linuxver.h>
+
+#include <linux/pci.h>
+#include <linux/completion.h>
+#include <linux/mmc/sdio_func.h>
+
+#include <osl.h>
+#include <pcicfg.h>
+#include <bcmdefs.h>
+#include <bcmdevs.h>
+
+#if defined(OOB_INTR_ONLY)
+#include <linux/irq.h>
+extern void dhdsdio_isr(void * args);
+#include <bcmutils.h>
+#include <dngl_stats.h>
+#include <dhd.h>
+#endif /* defined(OOB_INTR_ONLY) */
+
+/**
+ * SDIO Host Controller info
+ */
+typedef struct bcmsdh_hc bcmsdh_hc_t;
+
+struct bcmsdh_hc {
+	bcmsdh_hc_t *next;
+#ifdef BCMPLATFORM_BUS
+	struct device *dev;			/* platform device handle */
+#else
+	struct pci_dev *dev;		/* pci device handle */
+#endif /* BCMPLATFORM_BUS */
+	osl_t *osh;
+	void *regs;			/* SDIO Host Controller address */
+	bcmsdh_info_t *sdh;		/* SDIO Host Controller handle */
+	void *ch;
+	unsigned int oob_irq;
+	unsigned long oob_flags; /* OOB Host specifiction as edge and etc */
+	bool oob_irq_registered;
+	bool oob_irq_enable_flag;
+#if defined(OOB_INTR_ONLY)
+	spinlock_t irq_lock;
+#endif
+};
+static bcmsdh_hc_t *sdhcinfo = NULL;
+
+/* driver info, initialized when bcmsdh_register is called */
+static bcmsdh_driver_t drvinfo = {NULL, NULL};
+
+/* debugging macros */
+#define SDLX_MSG(x)
+
+/**
+ * Checks to see if vendor and device IDs match a supported SDIO Host Controller.
+ */
+bool
+bcmsdh_chipmatch(uint16 vendor, uint16 device)
+{
+	/* Add other vendors and devices as required */
+
+#ifdef BCMSDIOH_STD
+	/* Check for Arasan host controller */
+	if (vendor == VENDOR_SI_IMAGE) {
+		return (TRUE);
+	}
+	/* Check for BRCM 27XX Standard host controller */
+	if (device == BCM27XX_SDIOH_ID && vendor == VENDOR_BROADCOM) {
+		return (TRUE);
+	}
+	/* Check for BRCM Standard host controller */
+	if (device == SDIOH_FPGA_ID && vendor == VENDOR_BROADCOM) {
+		return (TRUE);
+	}
+	/* Check for TI PCIxx21 Standard host controller */
+	if (device == PCIXX21_SDIOH_ID && vendor == VENDOR_TI) {
+		return (TRUE);
+	}
+	if (device == PCIXX21_SDIOH0_ID && vendor == VENDOR_TI) {
+		return (TRUE);
+	}
+	/* Ricoh R5C822 Standard SDIO Host */
+	if (device == R5C822_SDIOH_ID && vendor == VENDOR_RICOH) {
+		return (TRUE);
+	}
+	/* JMicron Standard SDIO Host */
+	if (device == JMICRON_SDIOH_ID && vendor == VENDOR_JMICRON) {
+		return (TRUE);
+	}
+
+#endif /* BCMSDIOH_STD */
+#ifdef BCMSDIOH_SPI
+	/* This is the PciSpiHost. */
+	if (device == SPIH_FPGA_ID && vendor == VENDOR_BROADCOM) {
+		printf("Found PCI SPI Host Controller\n");
+		return (TRUE);
+	}
+
+#endif /* BCMSDIOH_SPI */
+
+	return (FALSE);
+}
+
+#if defined(BCMPLATFORM_BUS)
+#if defined(BCMLXSDMMC)
+/* forward declarations */
+int bcmsdh_probe_bcmdhd(struct device *dev);
+int bcmsdh_remove_bcmdhd(struct device *dev);
+
+EXPORT_SYMBOL(bcmsdh_probe_bcmdhd);
+EXPORT_SYMBOL(bcmsdh_remove_bcmdhd);
+
+#else
+/* forward declarations */
+static int __devinit bcmsdh_probe_bcmdhd(struct device *dev);
+static int __devexit bcmsdh_remove_bcmdhd(struct device *dev);
+#endif /* BCMLXSDMMC */
+
+#ifndef BCMLXSDMMC
+static
+#endif /* BCMLXSDMMC */
+int bcmsdh_probe_bcmdhd(struct device *dev)
+{
+	osl_t *osh = NULL;
+	bcmsdh_hc_t *sdhc = NULL;
+	ulong regs = 0;
+	bcmsdh_info_t *sdh = NULL;
+	struct sdio_func *func = container_of(dev, struct sdio_func, dev);
+#if !defined(BCMLXSDMMC) && defined(BCMPLATFORM_BUS)
+	struct platform_device *pdev;
+	struct resource *r;
+#endif /* BCMLXSDMMC */
+	int irq = 0;
+	uint32 vendevid;
+	unsigned long irq_flags = 0;
+
+#if !defined(BCMLXSDMMC) && defined(BCMPLATFORM_BUS)
+	pdev = to_platform_device(dev);
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq = platform_get_irq(pdev, 0);
+	if (!r || irq == NO_IRQ)
+		return -ENXIO;
+#endif /* BCMLXSDMMC */
+
+#if defined(OOB_INTR_ONLY)
+#ifdef HW_OOB
+	irq_flags =
+		IORESOURCE_IRQ | IORESOURCE_IRQ_HIGHLEVEL | IORESOURCE_IRQ_SHAREABLE;
+#else
+	 irq_flags = IRQF_TRIGGER_FALLING;
+#endif /* HW_OOB */
+
+	/* Get customer specific OOB IRQ parametres: IRQ number as IRQ type */
+	irq = dhd_customer_oob_irq_map(&irq_flags);
+	if  (irq < 0) {
+		SDLX_MSG(("%s: Host irq is not defined\n", __FUNCTION__));
+		return 1;
+	}
+#endif /* defined(OOB_INTR_ONLY) */
+	/* allocate SDIO Host Controller state info */
+	if (!(osh = osl_attach(dev, PCI_BUS, FALSE))) {
+		SDLX_MSG(("%s: osl_attach failed\n", __FUNCTION__));
+		goto err;
+	}
+	if (!(sdhc = MALLOC(osh, sizeof(bcmsdh_hc_t)))) {
+		SDLX_MSG(("%s: out of memory, allocated %d bytes\n",
+			__FUNCTION__,
+			MALLOCED(osh)));
+		goto err;
+	}
+	bzero(sdhc, sizeof(bcmsdh_hc_t));
+	sdhc->osh = osh;
+
+	sdhc->dev = (void *)dev;
+
+#ifdef BCMLXSDMMC
+	if (!(sdh = bcmsdh_attach(osh, (void *)0,
+	                          (void **)&regs, irq))) {
+		SDLX_MSG(("%s: bcmsdh_attach failed\n", __FUNCTION__));
+		goto err;
+	}
+#else
+	if (!(sdh = bcmsdh_attach(osh, (void *)r->start,
+	                          (void **)&regs, irq))) {
+		SDLX_MSG(("%s: bcmsdh_attach failed\n", __FUNCTION__));
+		goto err;
+	}
+#endif /* BCMLXSDMMC */
+	sdhc->sdh = sdh;
+	sdhc->oob_irq = irq;
+	sdhc->oob_flags = irq_flags;
+	sdhc->oob_irq_registered = FALSE;	/* to make sure.. */
+	sdhc->oob_irq_enable_flag = FALSE;
+#if defined(OOB_INTR_ONLY)
+	spin_lock_init(&sdhc->irq_lock);
+#endif
+
+	/* chain SDIO Host Controller info together */
+	sdhc->next = sdhcinfo;
+	sdhcinfo = sdhc;
+	/* Read the vendor/device ID from the CIS */
+	vendevid = bcmsdh_query_device(sdh);
+
+
+	/* try to attach to the target device */
+	if (!(sdhc->ch = drvinfo.attach((vendevid >> 16),
+					func->device, 0, 0, 0, 0,
+					(void *)regs, NULL, sdh, dev))) {
+		SDLX_MSG(("%s: device attach failed\n", __FUNCTION__));
+		goto err;
+	}
+
+	return 0;
+
+	/* error handling */
+err:
+	if (sdhc) {
+		if (sdhc->sdh)
+			bcmsdh_detach(sdhc->osh, sdhc->sdh);
+		MFREE(osh, sdhc, sizeof(bcmsdh_hc_t));
+	}
+	if (osh)
+		osl_detach(osh);
+	return -ENODEV;
+}
+
+#ifndef BCMLXSDMMC
+static
+#endif /* BCMLXSDMMC */
+int bcmsdh_remove_bcmdhd(struct device *dev)
+{
+	bcmsdh_hc_t *sdhc, *prev;
+	osl_t *osh;
+
+	sdhc = sdhcinfo;
+	drvinfo.detach(sdhc->ch);
+	bcmsdh_detach(sdhc->osh, sdhc->sdh);
+
+	/* find the SDIO Host Controller state for this pdev and take it out from the list */
+	for (sdhc = sdhcinfo, prev = NULL; sdhc; sdhc = sdhc->next) {
+		if (sdhc->dev == (void *)dev) {
+			if (prev)
+				prev->next = sdhc->next;
+			else
+				sdhcinfo = NULL;
+			break;
+		}
+		prev = sdhc;
+	}
+	if (!sdhc) {
+		SDLX_MSG(("%s: failed\n", __FUNCTION__));
+		return 0;
+	}
+
+	/* release SDIO Host Controller info */
+	osh = sdhc->osh;
+	MFREE(osh, sdhc, sizeof(bcmsdh_hc_t));
+	osl_detach(osh);
+
+#if !defined(BCMLXSDMMC) || defined(OOB_INTR_ONLY)
+	dev_set_drvdata(dev, NULL);
+#endif /* !defined(BCMLXSDMMC) || defined(OOB_INTR_ONLY) */
+
+	return 0;
+}
+
+#else /* BCMPLATFORM_BUS */
+
+#if !defined(BCMLXSDMMC)
+/* forward declarations for PCI probe and remove functions. */
+static int __devinit bcmsdh_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent);
+static void __devexit bcmsdh_pci_remove(struct pci_dev *pdev);
+
+/**
+ * pci id table
+ */
+static struct pci_device_id bcmsdh_pci_devid[] __devinitdata = {
+	{ vendor: PCI_ANY_ID,
+	device: PCI_ANY_ID,
+	subvendor: PCI_ANY_ID,
+	subdevice: PCI_ANY_ID,
+	class: 0,
+	class_mask: 0,
+	driver_data: 0,
+	},
+	{ 0, }
+};
+MODULE_DEVICE_TABLE(pci, bcmsdh_pci_devid);
+
+/**
+ * SDIO Host Controller pci driver info
+ */
+static struct pci_driver bcmsdh_pci_driver = {
+	node:		{},
+	name:		"bcmsdh",
+	id_table:	bcmsdh_pci_devid,
+	probe:		bcmsdh_pci_probe,
+	remove:		bcmsdh_pci_remove,
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
+	save_state:	NULL,
+#endif
+	suspend:	NULL,
+	resume:		NULL,
+	};
+
+
+extern uint sd_pci_slot;	/* Force detection to a particular PCI */
+							/* slot only . Allows for having multiple */
+							/* WL devices at once in a PC */
+							/* Only one instance of dhd will be */
+							/* usable at a time */
+							/* Upper word is bus number, */
+							/* lower word is slot number */
+							/* Default value of 0xffffffff turns this */
+							/* off */
+module_param(sd_pci_slot, uint, 0);
+
+
+/**
+ * Detect supported SDIO Host Controller and attach if found.
+ *
+ * Determine if the device described by pdev is a supported SDIO Host
+ * Controller.  If so, attach to it and attach to the target device.
+ */
+static int __devinit
+bcmsdh_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	osl_t *osh = NULL;
+	bcmsdh_hc_t *sdhc = NULL;
+	ulong regs;
+	bcmsdh_info_t *sdh = NULL;
+	int rc;
+
+	if (sd_pci_slot != 0xFFFFffff) {
+		if (pdev->bus->number != (sd_pci_slot>>16) ||
+			PCI_SLOT(pdev->devfn) != (sd_pci_slot&0xffff)) {
+			SDLX_MSG(("%s: %s: bus %X, slot %X, vend %X, dev %X\n",
+				__FUNCTION__,
+				bcmsdh_chipmatch(pdev->vendor, pdev->device)
+				?"Found compatible SDIOHC"
+				:"Probing unknown device",
+				pdev->bus->number, PCI_SLOT(pdev->devfn), pdev->vendor,
+				pdev->device));
+			return -ENODEV;
+		}
+		SDLX_MSG(("%s: %s: bus %X, slot %X, vendor %X, device %X (good PCI location)\n",
+			__FUNCTION__,
+			bcmsdh_chipmatch(pdev->vendor, pdev->device)
+			?"Using compatible SDIOHC"
+			:"WARNING, forced use of unkown device",
+			pdev->bus->number, PCI_SLOT(pdev->devfn), pdev->vendor, pdev->device));
+	}
+
+	if ((pdev->vendor == VENDOR_TI) && ((pdev->device == PCIXX21_FLASHMEDIA_ID) ||
+	    (pdev->device == PCIXX21_FLASHMEDIA0_ID))) {
+		uint32 config_reg;
+
+		SDLX_MSG(("%s: Disabling TI FlashMedia Controller.\n", __FUNCTION__));
+		if (!(osh = osl_attach(pdev, PCI_BUS, FALSE))) {
+			SDLX_MSG(("%s: osl_attach failed\n", __FUNCTION__));
+			goto err;
+		}
+
+		config_reg = OSL_PCI_READ_CONFIG(osh, 0x4c, 4);
+
+		/*
+		 * Set MMC_SD_DIS bit in FlashMedia Controller.
+		 * Disbling the SD/MMC Controller in the FlashMedia Controller
+		 * allows the Standard SD Host Controller to take over control
+		 * of the SD Slot.
+		 */
+		config_reg |= 0x02;
+		OSL_PCI_WRITE_CONFIG(osh, 0x4c, 4, config_reg);
+		osl_detach(osh);
+	}
+	/* match this pci device with what we support */
+	/* we can't solely rely on this to believe it is our SDIO Host Controller! */
+	if (!bcmsdh_chipmatch(pdev->vendor, pdev->device)) {
+		return -ENODEV;
+	}
+
+	/* this is a pci device we might support */
+	SDLX_MSG(("%s: Found possible SDIO Host Controller: bus %d slot %d func %d irq %d\n",
+		__FUNCTION__,
+		pdev->bus->number, PCI_SLOT(pdev->devfn),
+		PCI_FUNC(pdev->devfn), pdev->irq));
+
+	/* use bcmsdh_query_device() to get the vendor ID of the target device so
+	 * it will eventually appear in the Broadcom string on the console
+	 */
+
+	/* allocate SDIO Host Controller state info */
+	if (!(osh = osl_attach(pdev, PCI_BUS, FALSE))) {
+		SDLX_MSG(("%s: osl_attach failed\n", __FUNCTION__));
+		goto err;
+	}
+	if (!(sdhc = MALLOC(osh, sizeof(bcmsdh_hc_t)))) {
+		SDLX_MSG(("%s: out of memory, allocated %d bytes\n",
+			__FUNCTION__,
+			MALLOCED(osh)));
+		goto err;
+	}
+	bzero(sdhc, sizeof(bcmsdh_hc_t));
+	sdhc->osh = osh;
+
+	sdhc->dev = pdev;
+
+	/* map to address where host can access */
+	pci_set_master(pdev);
+	rc = pci_enable_device(pdev);
+	if (rc) {
+		SDLX_MSG(("%s: Cannot enable PCI device\n", __FUNCTION__));
+		goto err;
+	}
+	if (!(sdh = bcmsdh_attach(osh, (void *)(uintptr)pci_resource_start(pdev, 0),
+	                          (void **)&regs, pdev->irq))) {
+		SDLX_MSG(("%s: bcmsdh_attach failed\n", __FUNCTION__));
+		goto err;
+	}
+
+	sdhc->sdh = sdh;
+
+	/* try to attach to the target device */
+	if (!(sdhc->ch = drvinfo.attach(VENDOR_BROADCOM, /* pdev->vendor, */
+	                                bcmsdh_query_device(sdh) & 0xFFFF, 0, 0, 0, 0,
+	                                (void *)regs, NULL, sdh, pdev->dev))) {
+		SDLX_MSG(("%s: device attach failed\n", __FUNCTION__));
+		goto err;
+	}
+
+	/* chain SDIO Host Controller info together */
+	sdhc->next = sdhcinfo;
+	sdhcinfo = sdhc;
+
+	return 0;
+
+	/* error handling */
+err:
+	if (sdhc) {
+		if (sdhc->sdh)
+			bcmsdh_detach(sdhc->osh, sdhc->sdh);
+		MFREE(osh, sdhc, sizeof(bcmsdh_hc_t));
+	}
+	if (osh)
+		osl_detach(osh);
+	return -ENODEV;
+}
+
+
+/**
+ * Detach from target devices and SDIO Host Controller
+ */
+static void __devexit
+bcmsdh_pci_remove(struct pci_dev *pdev)
+{
+	bcmsdh_hc_t *sdhc, *prev;
+	osl_t *osh;
+
+	/* find the SDIO Host Controller state for this pdev and take it out from the list */
+	for (sdhc = sdhcinfo, prev = NULL; sdhc; sdhc = sdhc->next) {
+		if (sdhc->dev == pdev) {
+			if (prev)
+				prev->next = sdhc->next;
+			else
+				sdhcinfo = NULL;
+			break;
+		}
+		prev = sdhc;
+	}
+	if (!sdhc)
+		return;
+
+	drvinfo.detach(sdhc->ch);
+
+	bcmsdh_detach(sdhc->osh, sdhc->sdh);
+
+	/* release SDIO Host Controller info */
+	osh = sdhc->osh;
+	MFREE(osh, sdhc, sizeof(bcmsdh_hc_t));
+	osl_detach(osh);
+}
+#endif /* BCMLXSDMMC */
+#endif /* BCMPLATFORM_BUS */
+
+extern int sdio_function_init(void);
+
+int
+bcmsdh_register(bcmsdh_driver_t *driver)
+{
+	int error = 0;
+
+	drvinfo = *driver;
+
+#if defined(BCMPLATFORM_BUS)
+	SDLX_MSG(("Linux Kernel SDIO/MMC Driver\n"));
+	error = sdio_function_init();
+	return error;
+#endif /* defined(BCMPLATFORM_BUS) */
+
+#if !defined(BCMPLATFORM_BUS) && !defined(BCMLXSDMMC)
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
+	if (!(error = pci_module_init(&bcmsdh_pci_driver)))
+		return 0;
+#else
+	if (!(error = pci_register_driver(&bcmsdh_pci_driver)))
+		return 0;
+#endif
+
+	SDLX_MSG(("%s: pci_module_init failed 0x%x\n", __FUNCTION__, error));
+#endif /* BCMPLATFORM_BUS */
+
+	return error;
+}
+
+extern void sdio_function_cleanup(void);
+
+void
+bcmsdh_unregister(void)
+{
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
+	if (bcmsdh_pci_driver.node.next)
+#endif
+
+#if defined(BCMLXSDMMC)
+	sdio_function_cleanup();
+#endif /* BCMLXSDMMC */
+
+#if !defined(BCMPLATFORM_BUS) && !defined(BCMLXSDMMC)
+	pci_unregister_driver(&bcmsdh_pci_driver);
+#endif /* BCMPLATFORM_BUS */
+}
+
+#if defined(OOB_INTR_ONLY)
+void bcmsdh_oob_intr_set(bool enable)
+{
+	static bool curstate = 1;
+	unsigned long flags;
+
+	spin_lock_irqsave(&sdhcinfo->irq_lock, flags);
+	if (curstate != enable) {
+		if (enable)
+			enable_irq(sdhcinfo->oob_irq);
+		else
+			disable_irq_nosync(sdhcinfo->oob_irq);
+		curstate = enable;
+	}
+	spin_unlock_irqrestore(&sdhcinfo->irq_lock, flags);
+}
+
+static irqreturn_t wlan_oob_irq(int irq, void *dev_id)
+{
+	dhd_pub_t *dhdp;
+
+	dhdp = (dhd_pub_t *)dev_get_drvdata(sdhcinfo->dev);
+
+	bcmsdh_oob_intr_set(0);
+
+	if (dhdp == NULL) {
+		SDLX_MSG(("Out of band GPIO interrupt fired way too early\n"));
+		return IRQ_HANDLED;
+	}
+
+	dhdsdio_isr((void *)dhdp->bus);
+
+	return IRQ_HANDLED;
+}
+
+int bcmsdh_register_oob_intr(void * dhdp)
+{
+	int error = 0;
+
+	SDLX_MSG(("%s Enter \n", __FUNCTION__));
+
+	/* IORESOURCE_IRQ | IORESOURCE_IRQ_HIGHLEVEL | IORESOURCE_IRQ_SHAREABLE; */
+
+	dev_set_drvdata(sdhcinfo->dev, dhdp);
+
+	if (!sdhcinfo->oob_irq_registered) {
+		SDLX_MSG(("%s IRQ=%d Type=%X \n", __FUNCTION__,
+			(int)sdhcinfo->oob_irq, (int)sdhcinfo->oob_flags));
+		/* Refer to customer Host IRQ docs about proper irqflags definition */
+		error = request_irq(sdhcinfo->oob_irq, wlan_oob_irq, sdhcinfo->oob_flags,
+			"bcmsdh_sdmmc", NULL);
+		if (error)
+			return -ENODEV;
+
+		enable_irq_wake(sdhcinfo->oob_irq);
+		sdhcinfo->oob_irq_registered = TRUE;
+		sdhcinfo->oob_irq_enable_flag = TRUE;
+	}
+
+	return 0;
+}
+
+void bcmsdh_set_irq(int flag)
+{
+	if (sdhcinfo->oob_irq_registered && sdhcinfo->oob_irq_enable_flag != flag) {
+		SDLX_MSG(("%s Flag = %d", __FUNCTION__, flag));
+		sdhcinfo->oob_irq_enable_flag = flag;
+		if (flag) {
+			enable_irq(sdhcinfo->oob_irq);
+			enable_irq_wake(sdhcinfo->oob_irq);
+		} else {
+			disable_irq_wake(sdhcinfo->oob_irq);
+			disable_irq(sdhcinfo->oob_irq);
+		}
+	}
+}
+
+void bcmsdh_unregister_oob_intr(void)
+{
+	SDLX_MSG(("%s: Enter\n", __FUNCTION__));
+
+	if (sdhcinfo->oob_irq_registered == TRUE) {
+		bcmsdh_set_irq(FALSE);
+		free_irq(sdhcinfo->oob_irq, NULL);
+		sdhcinfo->oob_irq_registered = FALSE;
+	}
+}
+#endif /* defined(OOB_INTR_ONLY) */
+
+#if defined(BCMLXSDMMC)
+void *bcmsdh_get_drvdata(void)
+{
+	if (!sdhcinfo)
+		return NULL;
+	return dev_get_drvdata(sdhcinfo->dev);
+}
+#endif
+
+/* Module parameters specific to each host-controller driver */
+
+extern uint sd_msglevel;	/* Debug message level */
+module_param(sd_msglevel, uint, 0);
+
+extern uint sd_power;	/* 0 = SD Power OFF, 1 = SD Power ON. */
+module_param(sd_power, uint, 0);
+
+extern uint sd_clock;	/* SD Clock Control, 0 = SD Clock OFF, 1 = SD Clock ON */
+module_param(sd_clock, uint, 0);
+
+extern uint sd_divisor;	/* Divisor (-1 means external clock) */
+module_param(sd_divisor, uint, 0);
+
+extern uint sd_sdmode;	/* Default is SD4, 0=SPI, 1=SD1, 2=SD4 */
+module_param(sd_sdmode, uint, 0);
+
+extern uint sd_hiok;	/* Ok to use hi-speed mode */
+module_param(sd_hiok, uint, 0);
+
+extern uint sd_f2_blocksize;
+module_param(sd_f2_blocksize, int, 0);
+
+#ifdef BCMSDIOH_STD
+extern int sd_uhsimode;
+module_param(sd_uhsimode, int, 0);
+#endif
+
+#ifdef BCMSDH_MODULE
+EXPORT_SYMBOL(bcmsdh_attach);
+EXPORT_SYMBOL(bcmsdh_detach);
+EXPORT_SYMBOL(bcmsdh_intr_query);
+EXPORT_SYMBOL(bcmsdh_intr_enable);
+EXPORT_SYMBOL(bcmsdh_intr_disable);
+EXPORT_SYMBOL(bcmsdh_intr_reg);
+EXPORT_SYMBOL(bcmsdh_intr_dereg);
+
+#if defined(DHD_DEBUG)
+EXPORT_SYMBOL(bcmsdh_intr_pending);
+#endif
+
+EXPORT_SYMBOL(bcmsdh_devremove_reg);
+EXPORT_SYMBOL(bcmsdh_cfg_read);
+EXPORT_SYMBOL(bcmsdh_cfg_write);
+EXPORT_SYMBOL(bcmsdh_cis_read);
+EXPORT_SYMBOL(bcmsdh_reg_read);
+EXPORT_SYMBOL(bcmsdh_reg_write);
+EXPORT_SYMBOL(bcmsdh_regfail);
+EXPORT_SYMBOL(bcmsdh_send_buf);
+EXPORT_SYMBOL(bcmsdh_recv_buf);
+
+EXPORT_SYMBOL(bcmsdh_rwdata);
+EXPORT_SYMBOL(bcmsdh_abort);
+EXPORT_SYMBOL(bcmsdh_query_device);
+EXPORT_SYMBOL(bcmsdh_query_iofnum);
+EXPORT_SYMBOL(bcmsdh_iovar_op);
+EXPORT_SYMBOL(bcmsdh_register);
+EXPORT_SYMBOL(bcmsdh_unregister);
+EXPORT_SYMBOL(bcmsdh_chipmatch);
+EXPORT_SYMBOL(bcmsdh_reset);
+EXPORT_SYMBOL(bcmsdh_waitlockfree);
+
+EXPORT_SYMBOL(bcmsdh_get_dstatus);
+EXPORT_SYMBOL(bcmsdh_cfg_read_word);
+EXPORT_SYMBOL(bcmsdh_cfg_write_word);
+EXPORT_SYMBOL(bcmsdh_cur_sbwad);
+EXPORT_SYMBOL(bcmsdh_chipinfo);
+
+#endif /* BCMSDH_MODULE */
diff --git a/drivers/net/wireless/bcmdhd/bcmsdh_sdmmc.c b/drivers/net/wireless/bcmdhd/bcmsdh_sdmmc.c
new file mode 100644
index 000000000000..ffcb49bda290
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/bcmsdh_sdmmc.c
@@ -0,0 +1,1421 @@
+/*
+ * BCMSDH Function Driver for the native SDIO/MMC driver in the Linux Kernel
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdh_sdmmc.c 314904 2012-02-14 21:36:04Z $
+ */
+#include <typedefs.h>
+
+#include <bcmdevs.h>
+#include <bcmendian.h>
+#include <bcmutils.h>
+#include <osl.h>
+#include <sdio.h>	/* SDIO Device and Protocol Specs */
+#include <sdioh.h>	/* Standard SDIO Host Controller Specification */
+#include <bcmsdbus.h>	/* bcmsdh to/from specific controller APIs */
+#include <sdiovar.h>	/* ioctl/iovars */
+
+#include <linux/mmc/core.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/sdio_ids.h>
+
+#include <dngl_stats.h>
+#include <dhd.h>
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP)
+#include <linux/suspend.h>
+extern volatile bool dhd_mmc_suspend;
+#endif
+#include "bcmsdh_sdmmc.h"
+
+#ifndef BCMSDH_MODULE
+extern int sdio_function_init(void);
+extern void sdio_function_cleanup(void);
+#endif /* BCMSDH_MODULE */
+
+#if !defined(OOB_INTR_ONLY)
+static void IRQHandler(struct sdio_func *func);
+static void IRQHandlerF2(struct sdio_func *func);
+#endif /* !defined(OOB_INTR_ONLY) */
+static int sdioh_sdmmc_get_cisaddr(sdioh_info_t *sd, uint32 regaddr);
+extern int sdio_reset_comm(struct mmc_card *card);
+
+extern PBCMSDH_SDMMC_INSTANCE gInstance;
+
+uint sd_sdmode = SDIOH_MODE_SD4;	/* Use SD4 mode by default */
+uint sd_f2_blocksize = 512;		/* Default blocksize */
+
+uint sd_divisor = 2;			/* Default 48MHz/2 = 24MHz */
+
+uint sd_power = 1;		/* Default to SD Slot powered ON */
+uint sd_clock = 1;		/* Default to SD Clock turned ON */
+uint sd_hiok = FALSE;	/* Don't use hi-speed mode by default */
+uint sd_msglevel = 0x01;
+uint sd_use_dma = TRUE;
+DHD_PM_RESUME_WAIT_INIT(sdioh_request_byte_wait);
+DHD_PM_RESUME_WAIT_INIT(sdioh_request_word_wait);
+DHD_PM_RESUME_WAIT_INIT(sdioh_request_packet_wait);
+DHD_PM_RESUME_WAIT_INIT(sdioh_request_buffer_wait);
+
+#define DMA_ALIGN_MASK	0x03
+
+int sdioh_sdmmc_card_regread(sdioh_info_t *sd, int func, uint32 regaddr, int regsize, uint32 *data);
+
+static int
+sdioh_sdmmc_card_enablefuncs(sdioh_info_t *sd)
+{
+	int err_ret;
+	uint32 fbraddr;
+	uint8 func;
+
+	sd_trace(("%s\n", __FUNCTION__));
+
+	/* Get the Card's common CIS address */
+	sd->com_cis_ptr = sdioh_sdmmc_get_cisaddr(sd, SDIOD_CCCR_CISPTR_0);
+	sd->func_cis_ptr[0] = sd->com_cis_ptr;
+	sd_info(("%s: Card's Common CIS Ptr = 0x%x\n", __FUNCTION__, sd->com_cis_ptr));
+
+	/* Get the Card's function CIS (for each function) */
+	for (fbraddr = SDIOD_FBR_STARTADDR, func = 1;
+	     func <= sd->num_funcs; func++, fbraddr += SDIOD_FBR_SIZE) {
+		sd->func_cis_ptr[func] = sdioh_sdmmc_get_cisaddr(sd, SDIOD_FBR_CISPTR_0 + fbraddr);
+		sd_info(("%s: Function %d CIS Ptr = 0x%x\n",
+		         __FUNCTION__, func, sd->func_cis_ptr[func]));
+	}
+
+	sd->func_cis_ptr[0] = sd->com_cis_ptr;
+	sd_info(("%s: Card's Common CIS Ptr = 0x%x\n", __FUNCTION__, sd->com_cis_ptr));
+
+	/* Enable Function 1 */
+	sdio_claim_host(gInstance->func[1]);
+	err_ret = sdio_enable_func(gInstance->func[1]);
+	sdio_release_host(gInstance->func[1]);
+	if (err_ret) {
+		sd_err(("bcmsdh_sdmmc: Failed to enable F1 Err: 0x%08x", err_ret));
+	}
+
+	return FALSE;
+}
+
+/*
+ *	Public entry points & extern's
+ */
+extern sdioh_info_t *
+sdioh_attach(osl_t *osh, void *bar0, uint irq)
+{
+	sdioh_info_t *sd;
+	int err_ret;
+
+	sd_trace(("%s\n", __FUNCTION__));
+
+	if (gInstance == NULL) {
+		sd_err(("%s: SDIO Device not present\n", __FUNCTION__));
+		return NULL;
+	}
+
+	if ((sd = (sdioh_info_t *)MALLOC(osh, sizeof(sdioh_info_t))) == NULL) {
+		sd_err(("sdioh_attach: out of memory, malloced %d bytes\n", MALLOCED(osh)));
+		return NULL;
+	}
+	bzero((char *)sd, sizeof(sdioh_info_t));
+	sd->osh = osh;
+	if (sdioh_sdmmc_osinit(sd) != 0) {
+		sd_err(("%s:sdioh_sdmmc_osinit() failed\n", __FUNCTION__));
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+		return NULL;
+	}
+
+	sd->num_funcs = 2;
+	sd->sd_blockmode = TRUE;
+	sd->use_client_ints = TRUE;
+	sd->client_block_size[0] = 64;
+	sd->use_rxchain = FALSE;
+
+	gInstance->sd = sd;
+
+	/* Claim host controller */
+	sdio_claim_host(gInstance->func[1]);
+
+	sd->client_block_size[1] = 64;
+	err_ret = sdio_set_block_size(gInstance->func[1], 64);
+	if (err_ret) {
+		sd_err(("bcmsdh_sdmmc: Failed to set F1 blocksize\n"));
+	}
+
+	/* Release host controller F1 */
+	sdio_release_host(gInstance->func[1]);
+
+	if (gInstance->func[2]) {
+		/* Claim host controller F2 */
+		sdio_claim_host(gInstance->func[2]);
+
+		sd->client_block_size[2] = sd_f2_blocksize;
+		err_ret = sdio_set_block_size(gInstance->func[2], sd_f2_blocksize);
+		if (err_ret) {
+			sd_err(("bcmsdh_sdmmc: Failed to set F2 blocksize to %d\n",
+				sd_f2_blocksize));
+		}
+
+		/* Release host controller F2 */
+		sdio_release_host(gInstance->func[2]);
+	}
+
+	sdioh_sdmmc_card_enablefuncs(sd);
+
+	sd_trace(("%s: Done\n", __FUNCTION__));
+	return sd;
+}
+
+
+extern SDIOH_API_RC
+sdioh_detach(osl_t *osh, sdioh_info_t *sd)
+{
+	sd_trace(("%s\n", __FUNCTION__));
+
+	if (sd) {
+
+		/* Disable Function 2 */
+		sdio_claim_host(gInstance->func[2]);
+		sdio_disable_func(gInstance->func[2]);
+		sdio_release_host(gInstance->func[2]);
+
+		/* Disable Function 1 */
+		if (gInstance->func[1]) {
+			sdio_claim_host(gInstance->func[1]);
+			sdio_disable_func(gInstance->func[1]);
+			sdio_release_host(gInstance->func[1]);
+		}
+
+		gInstance->func[1] = NULL;
+		gInstance->func[2] = NULL;
+
+		/* deregister irq */
+		sdioh_sdmmc_osfree(sd);
+
+		MFREE(sd->osh, sd, sizeof(sdioh_info_t));
+	}
+	return SDIOH_API_RC_SUCCESS;
+}
+
+#if defined(OOB_INTR_ONLY) && defined(HW_OOB)
+
+extern SDIOH_API_RC
+sdioh_enable_func_intr(void)
+{
+	uint8 reg;
+	int err;
+
+	if (gInstance->func[0]) {
+		sdio_claim_host(gInstance->func[0]);
+
+		reg = sdio_readb(gInstance->func[0], SDIOD_CCCR_INTEN, &err);
+		if (err) {
+			sd_err(("%s: error for read SDIO_CCCR_IENx : 0x%x\n", __FUNCTION__, err));
+			sdio_release_host(gInstance->func[0]);
+			return SDIOH_API_RC_FAIL;
+		}
+
+		/* Enable F1 and F2 interrupts, set master enable */
+		reg |= (INTR_CTL_FUNC1_EN | INTR_CTL_FUNC2_EN | INTR_CTL_MASTER_EN);
+
+		sdio_writeb(gInstance->func[0], reg, SDIOD_CCCR_INTEN, &err);
+		sdio_release_host(gInstance->func[0]);
+
+		if (err) {
+			sd_err(("%s: error for write SDIO_CCCR_IENx : 0x%x\n", __FUNCTION__, err));
+			return SDIOH_API_RC_FAIL;
+		}
+	}
+
+	return SDIOH_API_RC_SUCCESS;
+}
+
+#endif /* defined(OOB_INTR_ONLY) && defined(HW_OOB) */
+
+extern SDIOH_API_RC
+sdioh_disable_func_intr(int func)
+{
+	uint8 reg;
+	int err;
+
+	if (gInstance->func[func]) {
+		sdio_claim_host(gInstance->func[func]);
+		reg = sdio_readb(gInstance->func[func], SDIOD_CCCR_INTEN, &err);
+		if (err) {
+			sd_err(("%s: error for read SDIO_CCCR_IENx : 0x%x\n", __FUNCTION__, err));
+			sdio_release_host(gInstance->func[func]);
+			return SDIOH_API_RC_FAIL;
+		}
+#if defined(HW_OOB)
+		reg &= ~(INTR_CTL_FUNC1_EN | INTR_CTL_FUNC2_EN);
+#else
+		reg &= ~(1 << func);
+#endif
+		/* Disable master interrupt with the last function interrupt */
+		if (!(reg & 0xFE))
+			reg = 0;
+		sdio_writeb(gInstance->func[func], reg, SDIOD_CCCR_INTEN, &err);
+
+		sdio_release_host(gInstance->func[func]);
+		if (err) {
+			sd_err(("%s: error for write SDIO_CCCR_IENx : 0x%x\n", __FUNCTION__, err));
+			return SDIOH_API_RC_FAIL;
+		}
+	}
+	return SDIOH_API_RC_SUCCESS;
+}
+
+
+/* Configure callback to client when we recieve client interrupt */
+extern SDIOH_API_RC
+sdioh_interrupt_register(sdioh_info_t *sd, sdioh_cb_fn_t fn, void *argh)
+{
+	sd_trace(("%s: Entering\n", __FUNCTION__));
+	if (fn == NULL) {
+		sd_err(("%s: interrupt handler is NULL, not registering\n", __FUNCTION__));
+		return SDIOH_API_RC_FAIL;
+	}
+#if !defined(OOB_INTR_ONLY)
+	sd->intr_handler = fn;
+	sd->intr_handler_arg = argh;
+	sd->intr_handler_valid = TRUE;
+
+	/* register and unmask irq */
+	if (gInstance->func[2]) {
+		sdio_claim_host(gInstance->func[2]);
+		sdio_claim_irq(gInstance->func[2], IRQHandlerF2);
+		sdio_release_host(gInstance->func[2]);
+	}
+
+	if (gInstance->func[1]) {
+		sdio_claim_host(gInstance->func[1]);
+		sdio_claim_irq(gInstance->func[1], IRQHandler);
+		sdio_release_host(gInstance->func[1]);
+	}
+#elif defined(HW_OOB)
+	sdioh_enable_func_intr();
+#endif /* !defined(OOB_INTR_ONLY) */
+
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_interrupt_deregister(sdioh_info_t *sd)
+{
+	sd_trace(("%s: Entering\n", __FUNCTION__));
+
+#if !defined(OOB_INTR_ONLY)
+	if (gInstance->func[1]) {
+		sdioh_disable_func_intr(1);
+		/*Wait for the pending interrupts to be cleared*/
+		msleep(300);
+		/* register and unmask irq */
+		sdio_claim_host(gInstance->func[1]);
+		sdio_release_irq(gInstance->func[1]);
+		sdio_release_host(gInstance->func[1]);
+	}
+
+	if (gInstance->func[2]) {
+		sdioh_disable_func_intr(2);
+		/*Wait for the pending interrupts to be cleared*/
+		msleep(300);
+		/* Claim host controller F2 */
+		sdio_claim_host(gInstance->func[2]);
+		sdio_release_irq(gInstance->func[2]);
+		/* Release host controller F2 */
+		sdio_release_host(gInstance->func[2]);
+	}
+
+	sd->intr_handler_valid = FALSE;
+	sd->intr_handler = NULL;
+	sd->intr_handler_arg = NULL;
+#elif defined(HW_OOB)
+	sdioh_disable_func_intr(0);
+#endif /* !defined(OOB_INTR_ONLY) */
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_interrupt_query(sdioh_info_t *sd, bool *onoff)
+{
+	sd_trace(("%s: Entering\n", __FUNCTION__));
+	*onoff = sd->client_intr_enabled;
+	return SDIOH_API_RC_SUCCESS;
+}
+
+#if defined(DHD_DEBUG)
+extern bool
+sdioh_interrupt_pending(sdioh_info_t *sd)
+{
+	return (0);
+}
+#endif
+
+uint
+sdioh_query_iofnum(sdioh_info_t *sd)
+{
+	return sd->num_funcs;
+}
+
+/* IOVar table */
+enum {
+	IOV_MSGLEVEL = 1,
+	IOV_BLOCKMODE,
+	IOV_BLOCKSIZE,
+	IOV_DMA,
+	IOV_USEINTS,
+	IOV_NUMINTS,
+	IOV_NUMLOCALINTS,
+	IOV_HOSTREG,
+	IOV_DEVREG,
+	IOV_DIVISOR,
+	IOV_SDMODE,
+	IOV_HISPEED,
+	IOV_HCIREGS,
+	IOV_POWER,
+	IOV_CLOCK,
+	IOV_RXCHAIN
+};
+
+const bcm_iovar_t sdioh_iovars[] = {
+	{"sd_msglevel", IOV_MSGLEVEL,	0,	IOVT_UINT32,	0 },
+	{"sd_blockmode", IOV_BLOCKMODE, 0,	IOVT_BOOL,	0 },
+	{"sd_blocksize", IOV_BLOCKSIZE, 0,	IOVT_UINT32,	0 }, /* ((fn << 16) | size) */
+	{"sd_dma",	IOV_DMA,	0,	IOVT_BOOL,	0 },
+	{"sd_ints", 	IOV_USEINTS,	0,	IOVT_BOOL,	0 },
+	{"sd_numints",	IOV_NUMINTS,	0,	IOVT_UINT32,	0 },
+	{"sd_numlocalints", IOV_NUMLOCALINTS, 0, IOVT_UINT32,	0 },
+	{"sd_hostreg",	IOV_HOSTREG,	0,	IOVT_BUFFER,	sizeof(sdreg_t) },
+	{"sd_devreg",	IOV_DEVREG, 	0,	IOVT_BUFFER,	sizeof(sdreg_t) },
+	{"sd_divisor",	IOV_DIVISOR,	0,	IOVT_UINT32,	0 },
+	{"sd_power",	IOV_POWER,	0,	IOVT_UINT32,	0 },
+	{"sd_clock",	IOV_CLOCK,	0,	IOVT_UINT32,	0 },
+	{"sd_mode", 	IOV_SDMODE, 	0,	IOVT_UINT32,	100},
+	{"sd_highspeed", IOV_HISPEED,	0,	IOVT_UINT32,	0 },
+	{"sd_rxchain",  IOV_RXCHAIN,    0, 	IOVT_BOOL,	0 },
+	{NULL, 0, 0, 0, 0 }
+};
+
+int
+sdioh_iovar_op(sdioh_info_t *si, const char *name,
+                           void *params, int plen, void *arg, int len, bool set)
+{
+	const bcm_iovar_t *vi = NULL;
+	int bcmerror = 0;
+	int val_size;
+	int32 int_val = 0;
+	bool bool_val;
+	uint32 actionid;
+
+	ASSERT(name);
+	ASSERT(len >= 0);
+
+	/* Get must have return space; Set does not take qualifiers */
+	ASSERT(set || (arg && len));
+	ASSERT(!set || (!params && !plen));
+
+	sd_trace(("%s: Enter (%s %s)\n", __FUNCTION__, (set ? "set" : "get"), name));
+
+	if ((vi = bcm_iovar_lookup(sdioh_iovars, name)) == NULL) {
+		bcmerror = BCME_UNSUPPORTED;
+		goto exit;
+	}
+
+	if ((bcmerror = bcm_iovar_lencheck(vi, arg, len, set)) != 0)
+		goto exit;
+
+	/* Set up params so get and set can share the convenience variables */
+	if (params == NULL) {
+		params = arg;
+		plen = len;
+	}
+
+	if (vi->type == IOVT_VOID)
+		val_size = 0;
+	else if (vi->type == IOVT_BUFFER)
+		val_size = len;
+	else
+		val_size = sizeof(int);
+
+	if (plen >= (int)sizeof(int_val))
+		bcopy(params, &int_val, sizeof(int_val));
+
+	bool_val = (int_val != 0) ? TRUE : FALSE;
+	BCM_REFERENCE(bool_val);
+
+	actionid = set ? IOV_SVAL(vi->varid) : IOV_GVAL(vi->varid);
+	switch (actionid) {
+	case IOV_GVAL(IOV_MSGLEVEL):
+		int_val = (int32)sd_msglevel;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_MSGLEVEL):
+		sd_msglevel = int_val;
+		break;
+
+	case IOV_GVAL(IOV_BLOCKMODE):
+		int_val = (int32)si->sd_blockmode;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_BLOCKMODE):
+		si->sd_blockmode = (bool)int_val;
+		/* Haven't figured out how to make non-block mode with DMA */
+		break;
+
+	case IOV_GVAL(IOV_BLOCKSIZE):
+		if ((uint32)int_val > si->num_funcs) {
+			bcmerror = BCME_BADARG;
+			break;
+		}
+		int_val = (int32)si->client_block_size[int_val];
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_BLOCKSIZE):
+	{
+		uint func = ((uint32)int_val >> 16);
+		uint blksize = (uint16)int_val;
+		uint maxsize;
+
+		if (func > si->num_funcs) {
+			bcmerror = BCME_BADARG;
+			break;
+		}
+
+		switch (func) {
+		case 0: maxsize = 32; break;
+		case 1: maxsize = BLOCK_SIZE_4318; break;
+		case 2: maxsize = BLOCK_SIZE_4328; break;
+		default: maxsize = 0;
+		}
+		if (blksize > maxsize) {
+			bcmerror = BCME_BADARG;
+			break;
+		}
+		if (!blksize) {
+			blksize = maxsize;
+		}
+
+		/* Now set it */
+		si->client_block_size[func] = blksize;
+
+		break;
+	}
+
+	case IOV_GVAL(IOV_RXCHAIN):
+		int_val = (int32)si->use_rxchain;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_DMA):
+		int_val = (int32)si->sd_use_dma;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_DMA):
+		si->sd_use_dma = (bool)int_val;
+		break;
+
+	case IOV_GVAL(IOV_USEINTS):
+		int_val = (int32)si->use_client_ints;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_USEINTS):
+		si->use_client_ints = (bool)int_val;
+		if (si->use_client_ints)
+			si->intmask |= CLIENT_INTR;
+		else
+			si->intmask &= ~CLIENT_INTR;
+
+		break;
+
+	case IOV_GVAL(IOV_DIVISOR):
+		int_val = (uint32)sd_divisor;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_DIVISOR):
+		sd_divisor = int_val;
+		break;
+
+	case IOV_GVAL(IOV_POWER):
+		int_val = (uint32)sd_power;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_POWER):
+		sd_power = int_val;
+		break;
+
+	case IOV_GVAL(IOV_CLOCK):
+		int_val = (uint32)sd_clock;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_CLOCK):
+		sd_clock = int_val;
+		break;
+
+	case IOV_GVAL(IOV_SDMODE):
+		int_val = (uint32)sd_sdmode;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_SDMODE):
+		sd_sdmode = int_val;
+		break;
+
+	case IOV_GVAL(IOV_HISPEED):
+		int_val = (uint32)sd_hiok;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_HISPEED):
+		sd_hiok = int_val;
+		break;
+
+	case IOV_GVAL(IOV_NUMINTS):
+		int_val = (int32)si->intrcount;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_NUMLOCALINTS):
+		int_val = (int32)0;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_HOSTREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+
+		if (sd_ptr->offset < SD_SysAddr || sd_ptr->offset > SD_MaxCurCap) {
+			sd_err(("%s: bad offset 0x%x\n", __FUNCTION__, sd_ptr->offset));
+			bcmerror = BCME_BADARG;
+			break;
+		}
+
+		sd_trace(("%s: rreg%d at offset %d\n", __FUNCTION__,
+		                  (sd_ptr->offset & 1) ? 8 : ((sd_ptr->offset & 2) ? 16 : 32),
+		                  sd_ptr->offset));
+		if (sd_ptr->offset & 1)
+			int_val = 8; /* sdioh_sdmmc_rreg8(si, sd_ptr->offset); */
+		else if (sd_ptr->offset & 2)
+			int_val = 16; /* sdioh_sdmmc_rreg16(si, sd_ptr->offset); */
+		else
+			int_val = 32; /* sdioh_sdmmc_rreg(si, sd_ptr->offset); */
+
+		bcopy(&int_val, arg, sizeof(int_val));
+		break;
+	}
+
+	case IOV_SVAL(IOV_HOSTREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+
+		if (sd_ptr->offset < SD_SysAddr || sd_ptr->offset > SD_MaxCurCap) {
+			sd_err(("%s: bad offset 0x%x\n", __FUNCTION__, sd_ptr->offset));
+			bcmerror = BCME_BADARG;
+			break;
+		}
+
+		sd_trace(("%s: wreg%d value 0x%08x at offset %d\n", __FUNCTION__, sd_ptr->value,
+		                  (sd_ptr->offset & 1) ? 8 : ((sd_ptr->offset & 2) ? 16 : 32),
+		                  sd_ptr->offset));
+		break;
+	}
+
+	case IOV_GVAL(IOV_DEVREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+		uint8 data = 0;
+
+		if (sdioh_cfg_read(si, sd_ptr->func, sd_ptr->offset, &data)) {
+			bcmerror = BCME_SDIO_ERROR;
+			break;
+		}
+
+		int_val = (int)data;
+		bcopy(&int_val, arg, sizeof(int_val));
+		break;
+	}
+
+	case IOV_SVAL(IOV_DEVREG):
+	{
+		sdreg_t *sd_ptr = (sdreg_t *)params;
+		uint8 data = (uint8)sd_ptr->value;
+
+		if (sdioh_cfg_write(si, sd_ptr->func, sd_ptr->offset, &data)) {
+			bcmerror = BCME_SDIO_ERROR;
+			break;
+		}
+		break;
+	}
+
+	default:
+		bcmerror = BCME_UNSUPPORTED;
+		break;
+	}
+exit:
+
+	return bcmerror;
+}
+
+#if defined(OOB_INTR_ONLY) && defined(HW_OOB)
+
+SDIOH_API_RC
+sdioh_enable_hw_oob_intr(sdioh_info_t *sd, bool enable)
+{
+	SDIOH_API_RC status;
+	uint8 data;
+
+	if (enable)
+		data = SDIO_SEPINT_MASK | SDIO_SEPINT_OE | SDIO_SEPINT_ACT_HI;
+	else
+		data = SDIO_SEPINT_ACT_HI;
+
+	status = sdioh_request_byte(sd, SDIOH_WRITE, 0, SDIOD_CCCR_BRCM_SEPINT, &data);
+	return status;
+}
+#endif /* defined(OOB_INTR_ONLY) && defined(HW_OOB) */
+
+extern SDIOH_API_RC
+sdioh_cfg_read(sdioh_info_t *sd, uint fnc_num, uint32 addr, uint8 *data)
+{
+	SDIOH_API_RC status;
+	/* No lock needed since sdioh_request_byte does locking */
+	status = sdioh_request_byte(sd, SDIOH_READ, fnc_num, addr, data);
+	return status;
+}
+
+extern SDIOH_API_RC
+sdioh_cfg_write(sdioh_info_t *sd, uint fnc_num, uint32 addr, uint8 *data)
+{
+	/* No lock needed since sdioh_request_byte does locking */
+	SDIOH_API_RC status;
+	status = sdioh_request_byte(sd, SDIOH_WRITE, fnc_num, addr, data);
+	return status;
+}
+
+static int
+sdioh_sdmmc_get_cisaddr(sdioh_info_t *sd, uint32 regaddr)
+{
+	/* read 24 bits and return valid 17 bit addr */
+	int i;
+	uint32 scratch, regdata;
+	uint8 *ptr = (uint8 *)&scratch;
+	for (i = 0; i < 3; i++) {
+		if ((sdioh_sdmmc_card_regread (sd, 0, regaddr, 1, &regdata)) != SUCCESS)
+			sd_err(("%s: Can't read!\n", __FUNCTION__));
+
+		*ptr++ = (uint8) regdata;
+		regaddr++;
+	}
+
+	/* Only the lower 17-bits are valid */
+	scratch = ltoh32(scratch);
+	scratch &= 0x0001FFFF;
+	return (scratch);
+}
+
+extern SDIOH_API_RC
+sdioh_cis_read(sdioh_info_t *sd, uint func, uint8 *cisd, uint32 length)
+{
+	uint32 count;
+	int offset;
+	uint32 foo;
+	uint8 *cis = cisd;
+
+	sd_trace(("%s: Func = %d\n", __FUNCTION__, func));
+
+	if (!sd->func_cis_ptr[func]) {
+		bzero(cis, length);
+		sd_err(("%s: no func_cis_ptr[%d]\n", __FUNCTION__, func));
+		return SDIOH_API_RC_FAIL;
+	}
+
+	sd_err(("%s: func_cis_ptr[%d]=0x%04x\n", __FUNCTION__, func, sd->func_cis_ptr[func]));
+
+	for (count = 0; count < length; count++) {
+		offset =  sd->func_cis_ptr[func] + count;
+		if (sdioh_sdmmc_card_regread (sd, 0, offset, 1, &foo) < 0) {
+			sd_err(("%s: regread failed: Can't read CIS\n", __FUNCTION__));
+			return SDIOH_API_RC_FAIL;
+		}
+
+		*cis = (uint8)(foo & 0xff);
+		cis++;
+	}
+
+	return SDIOH_API_RC_SUCCESS;
+}
+
+extern SDIOH_API_RC
+sdioh_request_byte(sdioh_info_t *sd, uint rw, uint func, uint regaddr, uint8 *byte)
+{
+	int err_ret;
+
+	sd_info(("%s: rw=%d, func=%d, addr=0x%05x\n", __FUNCTION__, rw, func, regaddr));
+
+	DHD_PM_RESUME_WAIT(sdioh_request_byte_wait);
+	DHD_PM_RESUME_RETURN_ERROR(SDIOH_API_RC_FAIL);
+	if(rw) { /* CMD52 Write */
+		if (func == 0) {
+			/* Can only directly write to some F0 registers.  Handle F2 enable
+			 * as a special case.
+			 */
+			if (regaddr == SDIOD_CCCR_IOEN) {
+				if (gInstance->func[2]) {
+					sdio_claim_host(gInstance->func[2]);
+					if (*byte & SDIO_FUNC_ENABLE_2) {
+						/* Enable Function 2 */
+						err_ret = sdio_enable_func(gInstance->func[2]);
+						if (err_ret) {
+							sd_err(("bcmsdh_sdmmc: enable F2 failed:%d",
+								err_ret));
+						}
+					} else {
+						/* Disable Function 2 */
+						err_ret = sdio_disable_func(gInstance->func[2]);
+						if (err_ret) {
+							sd_err(("bcmsdh_sdmmc: Disab F2 failed:%d",
+								err_ret));
+						}
+					}
+					sdio_release_host(gInstance->func[2]);
+				}
+			}
+#if defined(MMC_SDIO_ABORT)
+			/* to allow abort command through F1 */
+			else if (regaddr == SDIOD_CCCR_IOABORT) {
+				sdio_claim_host(gInstance->func[func]);
+				/*
+				* this sdio_f0_writeb() can be replaced with another api
+				* depending upon MMC driver change.
+				* As of this time, this is temporaray one
+				*/
+				sdio_writeb(gInstance->func[func], *byte, regaddr, &err_ret);
+				sdio_release_host(gInstance->func[func]);
+			}
+#endif /* MMC_SDIO_ABORT */
+			else if (regaddr < 0xF0) {
+				sd_err(("bcmsdh_sdmmc: F0 Wr:0x%02x: write disallowed\n", regaddr));
+			} else {
+				/* Claim host controller, perform F0 write, and release */
+				sdio_claim_host(gInstance->func[func]);
+				sdio_f0_writeb(gInstance->func[func], *byte, regaddr, &err_ret);
+				sdio_release_host(gInstance->func[func]);
+			}
+		} else {
+			/* Claim host controller, perform Fn write, and release */
+			sdio_claim_host(gInstance->func[func]);
+			sdio_writeb(gInstance->func[func], *byte, regaddr, &err_ret);
+			sdio_release_host(gInstance->func[func]);
+		}
+	} else { /* CMD52 Read */
+		/* Claim host controller, perform Fn read, and release */
+		sdio_claim_host(gInstance->func[func]);
+
+		if (func == 0) {
+			*byte = sdio_f0_readb(gInstance->func[func], regaddr, &err_ret);
+		} else {
+			*byte = sdio_readb(gInstance->func[func], regaddr, &err_ret);
+		}
+
+		sdio_release_host(gInstance->func[func]);
+	}
+
+	if (err_ret) {
+		sd_err(("bcmsdh_sdmmc: Failed to %s byte F%d:@0x%05x=%02x, Err: %d\n",
+		                        rw ? "Write" : "Read", func, regaddr, *byte, err_ret));
+	}
+
+	return ((err_ret == 0) ? SDIOH_API_RC_SUCCESS : SDIOH_API_RC_FAIL);
+}
+
+extern SDIOH_API_RC
+sdioh_request_word(sdioh_info_t *sd, uint cmd_type, uint rw, uint func, uint addr,
+                                   uint32 *word, uint nbytes)
+{
+	int err_ret = SDIOH_API_RC_FAIL;
+
+	if (func == 0) {
+		sd_err(("%s: Only CMD52 allowed to F0.\n", __FUNCTION__));
+		return SDIOH_API_RC_FAIL;
+	}
+
+	sd_info(("%s: cmd_type=%d, rw=%d, func=%d, addr=0x%05x, nbytes=%d\n",
+	         __FUNCTION__, cmd_type, rw, func, addr, nbytes));
+
+	DHD_PM_RESUME_WAIT(sdioh_request_word_wait);
+	DHD_PM_RESUME_RETURN_ERROR(SDIOH_API_RC_FAIL);
+	/* Claim host controller */
+	sdio_claim_host(gInstance->func[func]);
+
+	if(rw) { /* CMD52 Write */
+		if (nbytes == 4) {
+			sdio_writel(gInstance->func[func], *word, addr, &err_ret);
+		} else if (nbytes == 2) {
+			sdio_writew(gInstance->func[func], (*word & 0xFFFF), addr, &err_ret);
+		} else {
+			sd_err(("%s: Invalid nbytes: %d\n", __FUNCTION__, nbytes));
+		}
+	} else { /* CMD52 Read */
+		if (nbytes == 4) {
+			*word = sdio_readl(gInstance->func[func], addr, &err_ret);
+		} else if (nbytes == 2) {
+			*word = sdio_readw(gInstance->func[func], addr, &err_ret) & 0xFFFF;
+		} else {
+			sd_err(("%s: Invalid nbytes: %d\n", __FUNCTION__, nbytes));
+		}
+	}
+
+	/* Release host controller */
+	sdio_release_host(gInstance->func[func]);
+
+	if (err_ret) {
+		sd_err(("bcmsdh_sdmmc: Failed to %s word, Err: 0x%08x",
+		                        rw ? "Write" : "Read", err_ret));
+	}
+
+	return ((err_ret == 0) ? SDIOH_API_RC_SUCCESS : SDIOH_API_RC_FAIL);
+}
+
+static SDIOH_API_RC
+sdioh_request_packet(sdioh_info_t *sd, uint fix_inc, uint write, uint func,
+                     uint addr, void *pkt)
+{
+	bool fifo = (fix_inc == SDIOH_DATA_FIX);
+	uint32	SGCount = 0;
+	int err_ret = 0;
+	void *pnext, *pprev;
+	uint ttl_len, dma_len, lft_len, xfred_len, pkt_len;
+	uint blk_num;
+	struct mmc_request mmc_req;
+	struct mmc_command mmc_cmd;
+	struct mmc_data mmc_dat;
+
+	sd_trace(("%s: Enter\n", __FUNCTION__));
+
+	ASSERT(pkt);
+	DHD_PM_RESUME_WAIT(sdioh_request_packet_wait);
+	DHD_PM_RESUME_RETURN_ERROR(SDIOH_API_RC_FAIL);
+
+	ttl_len = xfred_len = 0;
+	/* at least 4 bytes alignment of skb buff is guaranteed */
+	for (pnext = pkt; pnext; pnext = PKTNEXT(sd->osh, pnext))
+		ttl_len += PKTLEN(sd->osh, pnext);
+
+	if (!sd->use_rxchain || ttl_len <= sd->client_block_size[func]) {
+		blk_num = 0;
+		dma_len = 0;
+	} else {
+		blk_num = ttl_len / sd->client_block_size[func];
+		dma_len = blk_num * sd->client_block_size[func];
+	}
+	lft_len = ttl_len - dma_len;
+
+	sd_trace(("%s: %s %dB to func%d:%08x, %d blks with DMA, %dB leftover\n",
+		__FUNCTION__, write ? "W" : "R",
+		ttl_len, func, addr, blk_num, lft_len));
+
+	if (0 != dma_len) {
+		memset(&mmc_req, 0, sizeof(struct mmc_request));
+		memset(&mmc_cmd, 0, sizeof(struct mmc_command));
+		memset(&mmc_dat, 0, sizeof(struct mmc_data));
+
+		/* Set up DMA descriptors */
+		pprev = pkt;
+		for (pnext = pkt;
+		     pnext && dma_len;
+		     pnext = PKTNEXT(sd->osh, pnext)) {
+			pkt_len = PKTLEN(sd->osh, pnext);
+
+			if (dma_len > pkt_len)
+				dma_len -= pkt_len;
+			else {
+				pkt_len = xfred_len = dma_len;
+				dma_len = 0;
+				pkt = pnext;
+			}
+
+			sg_set_buf(&sd->sg_list[SGCount++],
+				(uint8*)PKTDATA(sd->osh, pnext),
+				pkt_len);
+
+			if (SGCount >= SDIOH_SDMMC_MAX_SG_ENTRIES) {
+				sd_err(("%s: sg list entries exceed limit\n",
+					__FUNCTION__));
+				return (SDIOH_API_RC_FAIL);
+			}
+		}
+
+		mmc_dat.sg = sd->sg_list;
+		mmc_dat.sg_len = SGCount;
+		mmc_dat.blksz = sd->client_block_size[func];
+		mmc_dat.blocks = blk_num;
+		mmc_dat.flags = write ? MMC_DATA_WRITE : MMC_DATA_READ;
+
+		mmc_cmd.opcode = 53;		/* SD_IO_RW_EXTENDED */
+		mmc_cmd.arg = write ? 1<<31 : 0;
+		mmc_cmd.arg |= (func & 0x7) << 28;
+		mmc_cmd.arg |= 1<<27;
+		mmc_cmd.arg |= fifo ? 0 : 1<<26;
+		mmc_cmd.arg |= (addr & 0x1FFFF) << 9;
+		mmc_cmd.arg |= blk_num & 0x1FF;
+		mmc_cmd.flags = MMC_RSP_SPI_R5 | MMC_RSP_R5 | MMC_CMD_ADTC;
+
+		mmc_req.cmd = &mmc_cmd;
+		mmc_req.data = &mmc_dat;
+
+		sdio_claim_host(gInstance->func[func]);
+		mmc_set_data_timeout(&mmc_dat, gInstance->func[func]->card);
+		mmc_wait_for_req(gInstance->func[func]->card->host, &mmc_req);
+		sdio_release_host(gInstance->func[func]);
+
+		err_ret = mmc_cmd.error? mmc_cmd.error : mmc_dat.error;
+		if (0 != err_ret) {
+			sd_err(("%s:CMD53 %s failed with code %d\n",
+			       __FUNCTION__,
+			       write ? "write" : "read",
+			       err_ret));
+			sd_err(("%s:Disabling rxchain and fire it with PIO\n",
+			       __FUNCTION__));
+			sd->use_rxchain = FALSE;
+			pkt = pprev;
+			lft_len = ttl_len;
+		} else if (!fifo) {
+			addr = addr + ttl_len - lft_len - dma_len;
+		}
+	}
+
+	/* PIO mode */
+	if (0 != lft_len) {
+		/* Claim host controller */
+		sdio_claim_host(gInstance->func[func]);
+		for (pnext = pkt; pnext; pnext = PKTNEXT(sd->osh, pnext)) {
+			uint8 *buf = (uint8*)PKTDATA(sd->osh, pnext) +
+				xfred_len;
+			pkt_len = PKTLEN(sd->osh, pnext);
+			if (0 != xfred_len) {
+				pkt_len -= xfred_len;
+				xfred_len = 0;
+			}
+			pkt_len = (pkt_len + 3) & 0xFFFFFFFC;
+#ifdef CONFIG_MMC_MSM7X00A
+			if ((pkt_len % 64) == 32) {
+				sd_trace(("%s: Rounding up TX packet +=32\n", __FUNCTION__));
+				pkt_len += 32;
+			}
+#endif /* CONFIG_MMC_MSM7X00A */
+
+			if ((write) && (!fifo))
+				err_ret = sdio_memcpy_toio(
+						gInstance->func[func],
+						addr, buf, pkt_len);
+			else if (write)
+				err_ret = sdio_memcpy_toio(
+						gInstance->func[func],
+						addr, buf, pkt_len);
+			else if (fifo)
+				err_ret = sdio_readsb(
+						gInstance->func[func],
+						buf, addr, pkt_len);
+			else
+				err_ret = sdio_memcpy_fromio(
+						gInstance->func[func],
+						buf, addr, pkt_len);
+
+			if (err_ret)
+				sd_err(("%s: %s FAILED %p[%d], addr=0x%05x, pkt_len=%d, ERR=%d\n",
+				       __FUNCTION__,
+				       (write) ? "TX" : "RX",
+				       pnext, SGCount, addr, pkt_len, err_ret));
+			else
+				sd_trace(("%s: %s xfr'd %p[%d], addr=0x%05x, len=%d\n",
+					__FUNCTION__,
+					(write) ? "TX" : "RX",
+					pnext, SGCount, addr, pkt_len));
+
+			if (!fifo)
+				addr += pkt_len;
+			SGCount ++;
+		}
+		sdio_release_host(gInstance->func[func]);
+	}
+
+	sd_trace(("%s: Exit\n", __FUNCTION__));
+	return ((err_ret == 0) ? SDIOH_API_RC_SUCCESS : SDIOH_API_RC_FAIL);
+}
+
+
+/*
+ * This function takes a buffer or packet, and fixes everything up so that in the
+ * end, a DMA-able packet is created.
+ *
+ * A buffer does not have an associated packet pointer, and may or may not be aligned.
+ * A packet may consist of a single packet, or a packet chain.  If it is a packet chain,
+ * then all the packets in the chain must be properly aligned.  If the packet data is not
+ * aligned, then there may only be one packet, and in this case, it is copied to a new
+ * aligned packet.
+ *
+ */
+extern SDIOH_API_RC
+sdioh_request_buffer(sdioh_info_t *sd, uint pio_dma, uint fix_inc, uint write, uint func,
+                     uint addr, uint reg_width, uint buflen_u, uint8 *buffer, void *pkt)
+{
+	SDIOH_API_RC Status;
+	void *mypkt = NULL;
+
+	sd_trace(("%s: Enter\n", __FUNCTION__));
+
+	DHD_PM_RESUME_WAIT(sdioh_request_buffer_wait);
+	DHD_PM_RESUME_RETURN_ERROR(SDIOH_API_RC_FAIL);
+	/* Case 1: we don't have a packet. */
+	if (pkt == NULL) {
+		sd_data(("%s: Creating new %s Packet, len=%d\n",
+		         __FUNCTION__, write ? "TX" : "RX", buflen_u));
+#ifdef CONFIG_DHD_USE_STATIC_BUF
+		if (!(mypkt = PKTGET_STATIC(sd->osh, buflen_u, write ? TRUE : FALSE))) {
+#else
+		if (!(mypkt = PKTGET(sd->osh, buflen_u, write ? TRUE : FALSE))) {
+#endif /* CONFIG_DHD_USE_STATIC_BUF */
+			sd_err(("%s: PKTGET failed: len %d\n",
+			           __FUNCTION__, buflen_u));
+			return SDIOH_API_RC_FAIL;
+		}
+
+		/* For a write, copy the buffer data into the packet. */
+		if (write) {
+			bcopy(buffer, PKTDATA(sd->osh, mypkt), buflen_u);
+		}
+
+		Status = sdioh_request_packet(sd, fix_inc, write, func, addr, mypkt);
+
+		/* For a read, copy the packet data back to the buffer. */
+		if (!write) {
+			bcopy(PKTDATA(sd->osh, mypkt), buffer, buflen_u);
+		}
+#ifdef CONFIG_DHD_USE_STATIC_BUF
+		PKTFREE_STATIC(sd->osh, mypkt, write ? TRUE : FALSE);
+#else
+		PKTFREE(sd->osh, mypkt, write ? TRUE : FALSE);
+#endif /* CONFIG_DHD_USE_STATIC_BUF */
+	} else if (((uint32)(PKTDATA(sd->osh, pkt)) & DMA_ALIGN_MASK) != 0) {
+		/* Case 2: We have a packet, but it is unaligned. */
+
+		/* In this case, we cannot have a chain. */
+		ASSERT(PKTNEXT(sd->osh, pkt) == NULL);
+
+		sd_data(("%s: Creating aligned %s Packet, len=%d\n",
+		         __FUNCTION__, write ? "TX" : "RX", PKTLEN(sd->osh, pkt)));
+#ifdef CONFIG_DHD_USE_STATIC_BUF
+		if (!(mypkt = PKTGET_STATIC(sd->osh, PKTLEN(sd->osh, pkt), write ? TRUE : FALSE))) {
+#else
+		if (!(mypkt = PKTGET(sd->osh, PKTLEN(sd->osh, pkt), write ? TRUE : FALSE))) {
+#endif /* CONFIG_DHD_USE_STATIC_BUF */
+			sd_err(("%s: PKTGET failed: len %d\n",
+			           __FUNCTION__, PKTLEN(sd->osh, pkt)));
+			return SDIOH_API_RC_FAIL;
+		}
+
+		/* For a write, copy the buffer data into the packet. */
+		if (write) {
+			bcopy(PKTDATA(sd->osh, pkt),
+			      PKTDATA(sd->osh, mypkt),
+			      PKTLEN(sd->osh, pkt));
+		}
+
+		Status = sdioh_request_packet(sd, fix_inc, write, func, addr, mypkt);
+
+		/* For a read, copy the packet data back to the buffer. */
+		if (!write) {
+			bcopy(PKTDATA(sd->osh, mypkt),
+			      PKTDATA(sd->osh, pkt),
+			      PKTLEN(sd->osh, mypkt));
+		}
+#ifdef CONFIG_DHD_USE_STATIC_BUF
+		PKTFREE_STATIC(sd->osh, mypkt, write ? TRUE : FALSE);
+#else
+		PKTFREE(sd->osh, mypkt, write ? TRUE : FALSE);
+#endif /* CONFIG_DHD_USE_STATIC_BUF */
+	} else { /* case 3: We have a packet and it is aligned. */
+		sd_data(("%s: Aligned %s Packet, direct DMA\n",
+		         __FUNCTION__, write ? "Tx" : "Rx"));
+		Status = sdioh_request_packet(sd, fix_inc, write, func, addr, pkt);
+	}
+
+	return (Status);
+}
+
+/* this function performs "abort" for both of host & device */
+extern int
+sdioh_abort(sdioh_info_t *sd, uint func)
+{
+#if defined(MMC_SDIO_ABORT)
+	char t_func = (char) func;
+#endif /* defined(MMC_SDIO_ABORT) */
+	sd_trace(("%s: Enter\n", __FUNCTION__));
+
+#if defined(MMC_SDIO_ABORT)
+	/* issue abort cmd52 command through F1 */
+	sdioh_request_byte(sd, SD_IO_OP_WRITE, SDIO_FUNC_0, SDIOD_CCCR_IOABORT, &t_func);
+#endif /* defined(MMC_SDIO_ABORT) */
+
+	sd_trace(("%s: Exit\n", __FUNCTION__));
+	return SDIOH_API_RC_SUCCESS;
+}
+
+/* Reset and re-initialize the device */
+int sdioh_sdio_reset(sdioh_info_t *si)
+{
+	sd_trace(("%s: Enter\n", __FUNCTION__));
+	sd_trace(("%s: Exit\n", __FUNCTION__));
+	return SDIOH_API_RC_SUCCESS;
+}
+
+/* Disable device interrupt */
+void
+sdioh_sdmmc_devintr_off(sdioh_info_t *sd)
+{
+	sd_trace(("%s: %d\n", __FUNCTION__, sd->use_client_ints));
+	sd->intmask &= ~CLIENT_INTR;
+}
+
+/* Enable device interrupt */
+void
+sdioh_sdmmc_devintr_on(sdioh_info_t *sd)
+{
+	sd_trace(("%s: %d\n", __FUNCTION__, sd->use_client_ints));
+	sd->intmask |= CLIENT_INTR;
+}
+
+/* Read client card reg */
+int
+sdioh_sdmmc_card_regread(sdioh_info_t *sd, int func, uint32 regaddr, int regsize, uint32 *data)
+{
+
+	if ((func == 0) || (regsize == 1)) {
+		uint8 temp = 0;
+
+		sdioh_request_byte(sd, SDIOH_READ, func, regaddr, &temp);
+		*data = temp;
+		*data &= 0xff;
+		sd_data(("%s: byte read data=0x%02x\n",
+		         __FUNCTION__, *data));
+	} else {
+		sdioh_request_word(sd, 0, SDIOH_READ, func, regaddr, data, regsize);
+		if (regsize == 2)
+			*data &= 0xffff;
+
+		sd_data(("%s: word read data=0x%08x\n",
+		         __FUNCTION__, *data));
+	}
+
+	return SUCCESS;
+}
+
+#if !defined(OOB_INTR_ONLY)
+/* bcmsdh_sdmmc interrupt handler */
+static void IRQHandler(struct sdio_func *func)
+{
+	sdioh_info_t *sd;
+
+	sd_trace(("bcmsdh_sdmmc: ***IRQHandler\n"));
+	sd = gInstance->sd;
+
+	ASSERT(sd != NULL);
+	sdio_release_host(gInstance->func[0]);
+
+	if (sd->use_client_ints) {
+		sd->intrcount++;
+		ASSERT(sd->intr_handler);
+		ASSERT(sd->intr_handler_arg);
+		(sd->intr_handler)(sd->intr_handler_arg);
+	} else {
+		sd_err(("bcmsdh_sdmmc: ***IRQHandler\n"));
+
+		sd_err(("%s: Not ready for intr: enabled %d, handler %p\n",
+		        __FUNCTION__, sd->client_intr_enabled, sd->intr_handler));
+	}
+
+	sdio_claim_host(gInstance->func[0]);
+}
+
+/* bcmsdh_sdmmc interrupt handler for F2 (dummy handler) */
+static void IRQHandlerF2(struct sdio_func *func)
+{
+	sdioh_info_t *sd;
+
+	sd_trace(("bcmsdh_sdmmc: ***IRQHandlerF2\n"));
+
+	sd = gInstance->sd;
+
+	ASSERT(sd != NULL);
+	BCM_REFERENCE(sd);
+}
+#endif /* !defined(OOB_INTR_ONLY) */
+
+#ifdef NOTUSED
+/* Write client card reg */
+static int
+sdioh_sdmmc_card_regwrite(sdioh_info_t *sd, int func, uint32 regaddr, int regsize, uint32 data)
+{
+
+	if ((func == 0) || (regsize == 1)) {
+		uint8 temp;
+
+		temp = data & 0xff;
+		sdioh_request_byte(sd, SDIOH_READ, func, regaddr, &temp);
+		sd_data(("%s: byte write data=0x%02x\n",
+		         __FUNCTION__, data));
+	} else {
+		if (regsize == 2)
+			data &= 0xffff;
+
+		sdioh_request_word(sd, 0, SDIOH_READ, func, regaddr, &data, regsize);
+
+		sd_data(("%s: word write data=0x%08x\n",
+		         __FUNCTION__, data));
+	}
+
+	return SUCCESS;
+}
+#endif /* NOTUSED */
+
+int
+sdioh_start(sdioh_info_t *si, int stage)
+{
+	int ret;
+	sdioh_info_t *sd = gInstance->sd;
+
+	/* Need to do this stages as we can't enable the interrupt till
+		downloading of the firmware is complete, other wise polling
+		sdio access will come in way
+	*/
+	if (gInstance->func[0]) {
+			if (stage == 0) {
+		/* Since the power to the chip is killed, we will have
+			re enumerate the device again. Set the block size
+			and enable the fucntion 1 for in preparation for
+			downloading the code
+		*/
+		/* sdio_reset_comm() - has been fixed in latest kernel/msm.git for Linux
+		   2.6.27. The implementation prior to that is buggy, and needs broadcom's
+		   patch for it
+		*/
+		if ((ret = mmc_power_restore_host((gInstance->func[0])->card->host))) {
+			sd_err(("%s Failed, error = %d\n", __FUNCTION__, ret));
+			return ret;
+		}
+		else {
+			sd->num_funcs = 2;
+			sd->sd_blockmode = TRUE;
+			sd->use_client_ints = TRUE;
+			sd->client_block_size[0] = 64;
+
+			/* Claim host controller */
+			sdio_claim_host(gInstance->func[1]);
+
+			sd->client_block_size[1] = 64;
+			if (sdio_set_block_size(gInstance->func[1], 64)) {
+				sd_err(("bcmsdh_sdmmc: Failed to set F1 blocksize\n"));
+			}
+
+			/* Release host controller F1 */
+			sdio_release_host(gInstance->func[1]);
+
+			if (gInstance->func[2]) {
+				/* Claim host controller F2 */
+				sdio_claim_host(gInstance->func[2]);
+
+				sd->client_block_size[2] = sd_f2_blocksize;
+				if (sdio_set_block_size(gInstance->func[2],
+					sd_f2_blocksize)) {
+					sd_err(("bcmsdh_sdmmc: Failed to set F2 "
+						"blocksize to %d\n", sd_f2_blocksize));
+				}
+
+				/* Release host controller F2 */
+				sdio_release_host(gInstance->func[2]);
+			}
+
+			sdioh_sdmmc_card_enablefuncs(sd);
+			}
+		} else {
+#if !defined(OOB_INTR_ONLY)
+			sdio_claim_host(gInstance->func[0]);
+			sdio_claim_irq(gInstance->func[2], IRQHandlerF2);
+			sdio_claim_irq(gInstance->func[1], IRQHandler);
+			sdio_release_host(gInstance->func[0]);
+#else /* defined(OOB_INTR_ONLY) */
+#if defined(HW_OOB)
+			sdioh_enable_func_intr();
+#endif
+			bcmsdh_oob_intr_set(TRUE);
+#endif /* !defined(OOB_INTR_ONLY) */
+		}
+	}
+	else
+		sd_err(("%s Failed\n", __FUNCTION__));
+
+	return (0);
+}
+
+int
+sdioh_stop(sdioh_info_t *si)
+{
+	/* MSM7201A Android sdio stack has bug with interrupt
+		So internaly within SDIO stack they are polling
+		which cause issue when device is turned off. So
+		unregister interrupt with SDIO stack to stop the
+		polling
+	*/
+	if (gInstance->func[0]) {
+#if !defined(OOB_INTR_ONLY)
+		sdio_claim_host(gInstance->func[0]);
+		sdio_release_irq(gInstance->func[1]);
+		sdio_release_irq(gInstance->func[2]);
+		sdio_release_host(gInstance->func[0]);
+#else /* defined(OOB_INTR_ONLY) */
+#if defined(HW_OOB)
+		sdioh_disable_func_intr(0);
+#endif
+		bcmsdh_oob_intr_set(FALSE);
+#endif /* !defined(OOB_INTR_ONLY) */
+		if (mmc_power_save_host((gInstance->func[0])->card->host))
+			sd_err(("%s card power save fail\n", __FUNCTION__));
+	}
+	else
+		sd_err(("%s Failed\n", __FUNCTION__));
+	return (0);
+}
+
+int
+sdioh_waitlockfree(sdioh_info_t *sd)
+{
+	return (1);
+}
diff --git a/drivers/net/wireless/bcmdhd/bcmsdh_sdmmc_linux.c b/drivers/net/wireless/bcmdhd/bcmsdh_sdmmc_linux.c
new file mode 100644
index 000000000000..6ca85311b4c4
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/bcmsdh_sdmmc_linux.c
@@ -0,0 +1,387 @@
+/*
+ * BCMSDH Function Driver for the native SDIO/MMC driver in the Linux Kernel
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdh_sdmmc_linux.c 312783 2012-02-03 22:53:56Z $
+ */
+
+#include <typedefs.h>
+#include <bcmutils.h>
+#include <sdio.h>	/* SDIO Device and Protocol Specs */
+#include <bcmsdbus.h>	/* bcmsdh to/from specific controller APIs */
+#include <sdiovar.h>	/* to get msglevel bit values */
+
+#include <linux/sched.h>	/* request_irq() */
+
+#include <linux/mmc/core.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/sdio_ids.h>
+
+#if !defined(SDIO_VENDOR_ID_BROADCOM)
+#define SDIO_VENDOR_ID_BROADCOM		0x02d0
+#endif /* !defined(SDIO_VENDOR_ID_BROADCOM) */
+
+#define SDIO_DEVICE_ID_BROADCOM_DEFAULT	0x0000
+
+#if !defined(SDIO_DEVICE_ID_BROADCOM_4325_SDGWB)
+#define SDIO_DEVICE_ID_BROADCOM_4325_SDGWB	0x0492	/* BCM94325SDGWB */
+#endif /* !defined(SDIO_DEVICE_ID_BROADCOM_4325_SDGWB) */
+#if !defined(SDIO_DEVICE_ID_BROADCOM_4325)
+#define SDIO_DEVICE_ID_BROADCOM_4325	0x0493
+#endif /* !defined(SDIO_DEVICE_ID_BROADCOM_4325) */
+#if !defined(SDIO_DEVICE_ID_BROADCOM_4319)
+#define SDIO_DEVICE_ID_BROADCOM_4319	0x4319
+#endif /* !defined(SDIO_DEVICE_ID_BROADCOM_4319) */
+#if !defined(SDIO_DEVICE_ID_BROADCOM_4330)
+#define SDIO_DEVICE_ID_BROADCOM_4330	0x4330
+#endif /* !defined(SDIO_DEVICE_ID_BROADCOM_4330) */
+#if !defined(SDIO_DEVICE_ID_BROADCOM_4334)
+#define SDIO_DEVICE_ID_BROADCOM_4334    0x4334
+#endif /* !defined(SDIO_DEVICE_ID_BROADCOM_4334) */
+#if !defined(SDIO_DEVICE_ID_BROADCOM_4324)
+#define SDIO_DEVICE_ID_BROADCOM_4324    0x4324
+#endif /* !defined(SDIO_DEVICE_ID_BROADCOM_4324) */
+#if !defined(SDIO_DEVICE_ID_BROADCOM_43239)
+#define SDIO_DEVICE_ID_BROADCOM_43239    43239
+#endif /* !defined(SDIO_DEVICE_ID_BROADCOM_43239) */
+
+#include <bcmsdh_sdmmc.h>
+
+#include <dhd_dbg.h>
+
+#ifdef WL_CFG80211
+extern void wl_cfg80211_set_parent_dev(void *dev);
+#endif
+
+extern void sdioh_sdmmc_devintr_off(sdioh_info_t *sd);
+extern void sdioh_sdmmc_devintr_on(sdioh_info_t *sd);
+extern int dhd_os_check_wakelock(void *dhdp);
+extern int dhd_os_check_if_up(void *dhdp);
+extern void *bcmsdh_get_drvdata(void);
+
+int sdio_function_init(void);
+void sdio_function_cleanup(void);
+
+#define DESCRIPTION "bcmsdh_sdmmc Driver"
+#define AUTHOR "Broadcom Corporation"
+
+/* module param defaults */
+static int clockoverride = 0;
+
+module_param(clockoverride, int, 0644);
+MODULE_PARM_DESC(clockoverride, "SDIO card clock override");
+
+PBCMSDH_SDMMC_INSTANCE gInstance;
+
+/* Maximum number of bcmsdh_sdmmc devices supported by driver */
+#define BCMSDH_SDMMC_MAX_DEVICES 1
+
+extern int bcmsdh_probe_bcmdhd(struct device *dev);
+extern int bcmsdh_remove_bcmdhd(struct device *dev);
+
+extern volatile bool dhd_mmc_suspend;
+
+static int bcmsdh_sdmmc_probe(struct sdio_func *func,
+                              const struct sdio_device_id *id)
+{
+	int ret = 0;
+	static struct sdio_func sdio_func_0;
+	sd_trace(("bcmsdh_sdmmc: %s Enter\n", __FUNCTION__));
+	sd_trace(("sdio_bcmsdh: func->class=%x\n", func->class));
+	sd_trace(("sdio_vendor: 0x%04x\n", func->vendor));
+	sd_trace(("sdio_device: 0x%04x\n", func->device));
+	sd_trace(("Function#: 0x%04x\n", func->num));
+
+	if (func->num == 1) {
+		sdio_func_0.num = 0;
+		sdio_func_0.card = func->card;
+		gInstance->func[0] = &sdio_func_0;
+		if(func->device == 0x4) { /* 4318 */
+			gInstance->func[2] = NULL;
+			sd_trace(("NIC found, calling bcmsdh_probe_bcmdhd...\n"));
+			ret = bcmsdh_probe_bcmdhd(&func->dev);
+		}
+	}
+
+	gInstance->func[func->num] = func;
+
+	if (func->num == 2) {
+#ifdef WL_CFG80211
+		wl_cfg80211_set_parent_dev(&func->dev);
+#endif
+		sd_trace(("F2 found, calling bcmsdh_probe_bcmdhd...\n"));
+		ret = bcmsdh_probe_bcmdhd(&func->dev);
+#ifndef DHDTHREAD
+		if (mmc_power_save_host(func->card->host))
+			sd_err(("%s: card power save fail", __FUNCTION__));
+#endif
+	}
+
+	return ret;
+}
+
+static void bcmsdh_sdmmc_remove(struct sdio_func *func)
+{
+	sd_trace(("bcmsdh_sdmmc: %s Enter\n", __FUNCTION__));
+	sd_info(("sdio_bcmsdh: func->class=%x\n", func->class));
+	sd_info(("sdio_vendor: 0x%04x\n", func->vendor));
+	sd_info(("sdio_device: 0x%04x\n", func->device));
+	sd_info(("Function#: 0x%04x\n", func->num));
+
+	if (func->num == 2) {
+		sd_trace(("F2 found, calling bcmsdh_remove_bcmdhd...\n"));
+		bcmsdh_remove_bcmdhd(&func->dev);
+	} else if (func->num == 1) {
+		sdio_claim_host(func);
+		sdio_disable_func(func);
+		sdio_release_host(func);
+		gInstance->func[1] = NULL;
+	}
+}
+
+/* devices we support, null terminated */
+static const struct sdio_device_id bcmsdh_sdmmc_ids[] = {
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_DEFAULT) },
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4325_SDGWB) },
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4325) },
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4319) },
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4330) },
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4334) },
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4324) },
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_43239) },
+	{ /* end: all zeroes */				},
+};
+
+MODULE_DEVICE_TABLE(sdio, bcmsdh_sdmmc_ids);
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 39)) && defined(CONFIG_PM)
+static int bcmsdh_sdmmc_suspend(struct device *pdev)
+{
+	struct sdio_func *func = dev_to_sdio_func(pdev);
+	mmc_pm_flag_t sdio_flags;
+	int ret = 0;
+
+	if (func->num != 2)
+		return 0;
+
+	sd_trace(("%s Enter\n", __FUNCTION__));
+
+	if (dhd_os_check_wakelock(bcmsdh_get_drvdata()))
+		return -EBUSY;
+
+
+	sdio_flags = sdio_get_host_pm_caps(func);
+
+	if (!(sdio_flags & MMC_PM_KEEP_POWER)) {
+		sd_err(("%s: can't keep power while host is suspended\n", __FUNCTION__));
+		return  -EINVAL;
+	}
+
+	/* keep power while host suspended */
+	ret = sdio_set_host_pm_flags(func, MMC_PM_KEEP_POWER);
+	if (ret) {
+		sd_err(("%s: error while trying to keep power\n", __FUNCTION__));
+		return ret;
+	}
+
+#if defined(OOB_INTR_ONLY)
+	bcmsdh_oob_intr_set(0);
+#endif /* defined(OOB_INTR_ONLY) */
+
+	sdio_flags = sdio_get_host_pm_caps(func);
+
+	if (!(sdio_flags & MMC_PM_KEEP_POWER)) {
+		sd_err(("can't keep power while host "
+				"is suspended\n", __FUNCTION__));
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* keep power while host suspended */
+	ret = sdio_set_host_pm_flags(func, MMC_PM_KEEP_POWER);
+	if (ret) {
+		sd_err(("error while trying to keep power\n", __FUNCTION__));
+		goto out;
+	}
+
+	dhd_mmc_suspend = TRUE;
+	smp_mb();
+
+out:
+	return ret;
+}
+
+static int bcmsdh_sdmmc_resume(struct device *pdev)
+{
+#if defined(OOB_INTR_ONLY)
+	struct sdio_func *func = dev_to_sdio_func(pdev);
+#endif
+	sd_trace(("%s Enter\n", __FUNCTION__));
+	dhd_mmc_suspend = FALSE;
+#if defined(OOB_INTR_ONLY)
+	if ((func->num == 2) && dhd_os_check_if_up(bcmsdh_get_drvdata()))
+		bcmsdh_oob_intr_set(1);
+#endif /* (OOB_INTR_ONLY) */
+
+	smp_mb();
+	return 0;
+}
+
+static const struct dev_pm_ops bcmsdh_sdmmc_pm_ops = {
+	.suspend	= bcmsdh_sdmmc_suspend,
+	.resume		= bcmsdh_sdmmc_resume,
+};
+#endif  /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 39)) && defined(CONFIG_PM) */
+
+static struct sdio_driver bcmsdh_sdmmc_driver = {
+	.probe		= bcmsdh_sdmmc_probe,
+	.remove		= bcmsdh_sdmmc_remove,
+	.name		= "bcmsdh_sdmmc",
+	.id_table	= bcmsdh_sdmmc_ids,
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 39)) && defined(CONFIG_PM)
+	.drv = {
+		.pm	= &bcmsdh_sdmmc_pm_ops,
+	},
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 39)) && defined(CONFIG_PM) */
+};
+
+struct sdos_info {
+	sdioh_info_t *sd;
+	spinlock_t lock;
+};
+
+
+int
+sdioh_sdmmc_osinit(sdioh_info_t *sd)
+{
+	struct sdos_info *sdos;
+
+	sdos = (struct sdos_info*)MALLOC(sd->osh, sizeof(struct sdos_info));
+	sd->sdos_info = (void*)sdos;
+	if (sdos == NULL)
+		return BCME_NOMEM;
+
+	sdos->sd = sd;
+	spin_lock_init(&sdos->lock);
+	return BCME_OK;
+}
+
+void
+sdioh_sdmmc_osfree(sdioh_info_t *sd)
+{
+	struct sdos_info *sdos;
+	ASSERT(sd && sd->sdos_info);
+
+	sdos = (struct sdos_info *)sd->sdos_info;
+	MFREE(sd->osh, sdos, sizeof(struct sdos_info));
+}
+
+/* Interrupt enable/disable */
+SDIOH_API_RC
+sdioh_interrupt_set(sdioh_info_t *sd, bool enable)
+{
+	ulong flags;
+	struct sdos_info *sdos;
+
+	sd_trace(("%s: %s\n", __FUNCTION__, enable ? "Enabling" : "Disabling"));
+
+	sdos = (struct sdos_info *)sd->sdos_info;
+	ASSERT(sdos);
+
+#if !defined(OOB_INTR_ONLY)
+	if (enable && !(sd->intr_handler && sd->intr_handler_arg)) {
+		sd_err(("%s: no handler registered, will not enable\n", __FUNCTION__));
+		return SDIOH_API_RC_FAIL;
+	}
+#endif /* !defined(OOB_INTR_ONLY) */
+
+	/* Ensure atomicity for enable/disable calls */
+	spin_lock_irqsave(&sdos->lock, flags);
+
+	sd->client_intr_enabled = enable;
+	if (enable) {
+		sdioh_sdmmc_devintr_on(sd);
+	} else {
+		sdioh_sdmmc_devintr_off(sd);
+	}
+
+	spin_unlock_irqrestore(&sdos->lock, flags);
+
+	return SDIOH_API_RC_SUCCESS;
+}
+
+
+#ifdef BCMSDH_MODULE
+static int __init
+bcmsdh_module_init(void)
+{
+	int error = 0;
+	sdio_function_init();
+	return error;
+}
+
+static void __exit
+bcmsdh_module_cleanup(void)
+{
+	sdio_function_cleanup();
+}
+
+module_init(bcmsdh_module_init);
+module_exit(bcmsdh_module_cleanup);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION(DESCRIPTION);
+MODULE_AUTHOR(AUTHOR);
+
+#endif /* BCMSDH_MODULE */
+/*
+ * module init
+*/
+int sdio_function_init(void)
+{
+	int error = 0;
+	sd_trace(("bcmsdh_sdmmc: %s Enter\n", __FUNCTION__));
+
+	gInstance = kzalloc(sizeof(BCMSDH_SDMMC_INSTANCE), GFP_KERNEL);
+	if (!gInstance)
+		return -ENOMEM;
+
+	error = sdio_register_driver(&bcmsdh_sdmmc_driver);
+
+	return error;
+}
+
+/*
+ * module cleanup
+*/
+void sdio_function_cleanup(void)
+{
+	sd_trace(("%s Enter\n", __FUNCTION__));
+
+
+	sdio_unregister_driver(&bcmsdh_sdmmc_driver);
+
+	if (gInstance)
+		kfree(gInstance);
+}
diff --git a/drivers/net/wireless/bcmdhd/bcmutils.c b/drivers/net/wireless/bcmdhd/bcmutils.c
new file mode 100644
index 000000000000..6b578e653648
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/bcmutils.c
@@ -0,0 +1,1965 @@
+/*
+ * Driver O/S-independent utility routines
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: bcmutils.c,v 1.277.2.18 2011-01-26 02:32:08 $
+ */
+
+#include <typedefs.h>
+#include <bcmdefs.h>
+#include <stdarg.h>
+
+#ifdef BCMDRIVER
+
+#include <osl.h>
+#include <bcmutils.h>
+#include <siutils.h>
+
+#else /* !BCMDRIVER */
+
+#include <stdio.h>
+#include <string.h>
+#include <bcmutils.h>
+
+#if defined(BCMEXTSUP)
+#include <bcm_osl.h>
+#endif
+
+
+#endif /* !BCMDRIVER */
+
+#include <bcmendian.h>
+#include <bcmdevs.h>
+#include <proto/ethernet.h>
+#include <proto/vlan.h>
+#include <proto/bcmip.h>
+#include <proto/802.1d.h>
+#include <proto/802.11.h>
+
+void *_bcmutils_dummy_fn = NULL;
+
+#ifdef BCMDRIVER
+
+
+
+/* copy a pkt buffer chain into a buffer */
+uint
+pktcopy(osl_t *osh, void *p, uint offset, int len, uchar *buf)
+{
+	uint n, ret = 0;
+
+	if (len < 0)
+		len = 4096; /* "infinite" */
+
+	/* skip 'offset' bytes */
+	for (; p && offset; p = PKTNEXT(osh, p)) {
+		if (offset < (uint)PKTLEN(osh, p))
+			break;
+		offset -= PKTLEN(osh, p);
+	}
+
+	if (!p)
+		return 0;
+
+	/* copy the data */
+	for (; p && len; p = PKTNEXT(osh, p)) {
+		n = MIN((uint)PKTLEN(osh, p) - offset, (uint)len);
+		bcopy(PKTDATA(osh, p) + offset, buf, n);
+		buf += n;
+		len -= n;
+		ret += n;
+		offset = 0;
+	}
+
+	return ret;
+}
+
+/* copy a buffer into a pkt buffer chain */
+uint
+pktfrombuf(osl_t *osh, void *p, uint offset, int len, uchar *buf)
+{
+	uint n, ret = 0;
+
+	/* skip 'offset' bytes */
+	for (; p && offset; p = PKTNEXT(osh, p)) {
+		if (offset < (uint)PKTLEN(osh, p))
+			break;
+		offset -= PKTLEN(osh, p);
+	}
+
+	if (!p)
+		return 0;
+
+	/* copy the data */
+	for (; p && len; p = PKTNEXT(osh, p)) {
+		n = MIN((uint)PKTLEN(osh, p) - offset, (uint)len);
+		bcopy(buf, PKTDATA(osh, p) + offset, n);
+		buf += n;
+		len -= n;
+		ret += n;
+		offset = 0;
+	}
+
+	return ret;
+}
+
+
+
+/* return total length of buffer chain */
+uint BCMFASTPATH
+pkttotlen(osl_t *osh, void *p)
+{
+	uint total;
+
+	total = 0;
+	for (; p; p = PKTNEXT(osh, p))
+		total += PKTLEN(osh, p);
+	return (total);
+}
+
+/* return the last buffer of chained pkt */
+void *
+pktlast(osl_t *osh, void *p)
+{
+	for (; PKTNEXT(osh, p); p = PKTNEXT(osh, p))
+		;
+
+	return (p);
+}
+
+/* count segments of a chained packet */
+uint BCMFASTPATH
+pktsegcnt(osl_t *osh, void *p)
+{
+	uint cnt;
+
+	for (cnt = 0; p; p = PKTNEXT(osh, p))
+		cnt++;
+
+	return cnt;
+}
+
+
+/*
+ * osl multiple-precedence packet queue
+ * hi_prec is always >= the number of the highest non-empty precedence
+ */
+void * BCMFASTPATH
+pktq_penq(struct pktq *pq, int prec, void *p)
+{
+	struct pktq_prec *q;
+
+	ASSERT(prec >= 0 && prec < pq->num_prec);
+	ASSERT(PKTLINK(p) == NULL);         /* queueing chains not allowed */
+
+	ASSERT(!pktq_full(pq));
+	ASSERT(!pktq_pfull(pq, prec));
+
+	q = &pq->q[prec];
+
+	if (q->head)
+		PKTSETLINK(q->tail, p);
+	else
+		q->head = p;
+
+	q->tail = p;
+	q->len++;
+
+	pq->len++;
+
+	if (pq->hi_prec < prec)
+		pq->hi_prec = (uint8)prec;
+
+	return p;
+}
+
+void * BCMFASTPATH
+pktq_penq_head(struct pktq *pq, int prec, void *p)
+{
+	struct pktq_prec *q;
+
+	ASSERT(prec >= 0 && prec < pq->num_prec);
+	ASSERT(PKTLINK(p) == NULL);         /* queueing chains not allowed */
+
+	ASSERT(!pktq_full(pq));
+	ASSERT(!pktq_pfull(pq, prec));
+
+	q = &pq->q[prec];
+
+	if (q->head == NULL)
+		q->tail = p;
+
+	PKTSETLINK(p, q->head);
+	q->head = p;
+	q->len++;
+
+	pq->len++;
+
+	if (pq->hi_prec < prec)
+		pq->hi_prec = (uint8)prec;
+
+	return p;
+}
+
+void * BCMFASTPATH
+pktq_pdeq(struct pktq *pq, int prec)
+{
+	struct pktq_prec *q;
+	void *p;
+
+	ASSERT(prec >= 0 && prec < pq->num_prec);
+
+	q = &pq->q[prec];
+
+	if ((p = q->head) == NULL)
+		return NULL;
+
+	if ((q->head = PKTLINK(p)) == NULL)
+		q->tail = NULL;
+
+	q->len--;
+
+	pq->len--;
+
+	PKTSETLINK(p, NULL);
+
+	return p;
+}
+
+void * BCMFASTPATH
+pktq_pdeq_tail(struct pktq *pq, int prec)
+{
+	struct pktq_prec *q;
+	void *p, *prev;
+
+	ASSERT(prec >= 0 && prec < pq->num_prec);
+
+	q = &pq->q[prec];
+
+	if ((p = q->head) == NULL)
+		return NULL;
+
+	for (prev = NULL; p != q->tail; p = PKTLINK(p))
+		prev = p;
+
+	if (prev)
+		PKTSETLINK(prev, NULL);
+	else
+		q->head = NULL;
+
+	q->tail = prev;
+	q->len--;
+
+	pq->len--;
+
+	return p;
+}
+
+void
+pktq_pflush(osl_t *osh, struct pktq *pq, int prec, bool dir, ifpkt_cb_t fn, int arg)
+{
+	struct pktq_prec *q;
+	void *p, *prev = NULL;
+
+	q = &pq->q[prec];
+	p = q->head;
+	while (p) {
+		if (fn == NULL || (*fn)(p, arg)) {
+			bool head = (p == q->head);
+			if (head)
+				q->head = PKTLINK(p);
+			else
+				PKTSETLINK(prev, PKTLINK(p));
+			PKTSETLINK(p, NULL);
+			PKTFREE(osh, p, dir);
+			q->len--;
+			pq->len--;
+			p = (head ? q->head : PKTLINK(prev));
+		} else {
+			prev = p;
+			p = PKTLINK(p);
+		}
+	}
+
+	if (q->head == NULL) {
+		ASSERT(q->len == 0);
+		q->tail = NULL;
+	}
+}
+
+bool BCMFASTPATH
+pktq_pdel(struct pktq *pq, void *pktbuf, int prec)
+{
+	struct pktq_prec *q;
+	void *p;
+
+	ASSERT(prec >= 0 && prec < pq->num_prec);
+
+	if (!pktbuf)
+		return FALSE;
+
+	q = &pq->q[prec];
+
+	if (q->head == pktbuf) {
+		if ((q->head = PKTLINK(pktbuf)) == NULL)
+			q->tail = NULL;
+	} else {
+		for (p = q->head; p && PKTLINK(p) != pktbuf; p = PKTLINK(p))
+			;
+		if (p == NULL)
+			return FALSE;
+
+		PKTSETLINK(p, PKTLINK(pktbuf));
+		if (q->tail == pktbuf)
+			q->tail = p;
+	}
+
+	q->len--;
+	pq->len--;
+	PKTSETLINK(pktbuf, NULL);
+	return TRUE;
+}
+
+void
+pktq_init(struct pktq *pq, int num_prec, int max_len)
+{
+	int prec;
+
+	ASSERT(num_prec > 0 && num_prec <= PKTQ_MAX_PREC);
+
+	/* pq is variable size; only zero out what's requested */
+	bzero(pq, OFFSETOF(struct pktq, q) + (sizeof(struct pktq_prec) * num_prec));
+
+	pq->num_prec = (uint16)num_prec;
+
+	pq->max = (uint16)max_len;
+
+	for (prec = 0; prec < num_prec; prec++)
+		pq->q[prec].max = pq->max;
+}
+
+void * BCMFASTPATH
+pktq_deq(struct pktq *pq, int *prec_out)
+{
+	struct pktq_prec *q;
+	void *p;
+	int prec;
+
+	if (pq->len == 0)
+		return NULL;
+
+	while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
+		pq->hi_prec--;
+
+	q = &pq->q[prec];
+
+	if ((p = q->head) == NULL)
+		return NULL;
+
+	if ((q->head = PKTLINK(p)) == NULL)
+		q->tail = NULL;
+
+	q->len--;
+
+	pq->len--;
+
+	if (prec_out)
+		*prec_out = prec;
+
+	PKTSETLINK(p, NULL);
+
+	return p;
+}
+
+void * BCMFASTPATH
+pktq_deq_tail(struct pktq *pq, int *prec_out)
+{
+	struct pktq_prec *q;
+	void *p, *prev;
+	int prec;
+
+	if (pq->len == 0)
+		return NULL;
+
+	for (prec = 0; prec < pq->hi_prec; prec++)
+		if (pq->q[prec].head)
+			break;
+
+	q = &pq->q[prec];
+
+	if ((p = q->head) == NULL)
+		return NULL;
+
+	for (prev = NULL; p != q->tail; p = PKTLINK(p))
+		prev = p;
+
+	if (prev)
+		PKTSETLINK(prev, NULL);
+	else
+		q->head = NULL;
+
+	q->tail = prev;
+	q->len--;
+
+	pq->len--;
+
+	if (prec_out)
+		*prec_out = prec;
+
+	PKTSETLINK(p, NULL);
+
+	return p;
+}
+
+void *
+pktq_peek(struct pktq *pq, int *prec_out)
+{
+	int prec;
+
+	if (pq->len == 0)
+		return NULL;
+
+	while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
+		pq->hi_prec--;
+
+	if (prec_out)
+		*prec_out = prec;
+
+	return (pq->q[prec].head);
+}
+
+void *
+pktq_peek_tail(struct pktq *pq, int *prec_out)
+{
+	int prec;
+
+	if (pq->len == 0)
+		return NULL;
+
+	for (prec = 0; prec < pq->hi_prec; prec++)
+		if (pq->q[prec].head)
+			break;
+
+	if (prec_out)
+		*prec_out = prec;
+
+	return (pq->q[prec].tail);
+}
+
+void
+pktq_flush(osl_t *osh, struct pktq *pq, bool dir, ifpkt_cb_t fn, int arg)
+{
+	int prec;
+	for (prec = 0; prec < pq->num_prec; prec++)
+		pktq_pflush(osh, pq, prec, dir, fn, arg);
+	if (fn == NULL)
+		ASSERT(pq->len == 0);
+}
+
+/* Return sum of lengths of a specific set of precedences */
+int
+pktq_mlen(struct pktq *pq, uint prec_bmp)
+{
+	int prec, len;
+
+	len = 0;
+
+	for (prec = 0; prec <= pq->hi_prec; prec++)
+		if (prec_bmp & (1 << prec))
+			len += pq->q[prec].len;
+
+	return len;
+}
+
+/* Priority dequeue from a specific set of precedences */
+void * BCMFASTPATH
+pktq_mdeq(struct pktq *pq, uint prec_bmp, int *prec_out)
+{
+	struct pktq_prec *q;
+	void *p;
+	int prec;
+
+	if (pq->len == 0)
+		return NULL;
+
+	while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
+		pq->hi_prec--;
+
+	while ((prec_bmp & (1 << prec)) == 0 || pq->q[prec].head == NULL)
+		if (prec-- == 0)
+			return NULL;
+
+	q = &pq->q[prec];
+
+	if ((p = q->head) == NULL)
+		return NULL;
+
+	if ((q->head = PKTLINK(p)) == NULL)
+		q->tail = NULL;
+
+	q->len--;
+
+	if (prec_out)
+		*prec_out = prec;
+
+	pq->len--;
+
+	PKTSETLINK(p, NULL);
+
+	return p;
+}
+
+#endif /* BCMDRIVER */
+
+const unsigned char bcm_ctype[] = {
+
+	_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,            /* 0-7 */
+	_BCM_C, _BCM_C|_BCM_S, _BCM_C|_BCM_S, _BCM_C|_BCM_S, _BCM_C|_BCM_S, _BCM_C|_BCM_S, _BCM_C,
+	_BCM_C, /* 8-15 */
+	_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,            /* 16-23 */
+	_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,            /* 24-31 */
+	_BCM_S|_BCM_SP,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,        /* 32-39 */
+	_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,            /* 40-47 */
+	_BCM_D,_BCM_D,_BCM_D,_BCM_D,_BCM_D,_BCM_D,_BCM_D,_BCM_D,            /* 48-55 */
+	_BCM_D,_BCM_D,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,            /* 56-63 */
+	_BCM_P, _BCM_U|_BCM_X, _BCM_U|_BCM_X, _BCM_U|_BCM_X, _BCM_U|_BCM_X, _BCM_U|_BCM_X,
+	_BCM_U|_BCM_X, _BCM_U, /* 64-71 */
+	_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,            /* 72-79 */
+	_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,            /* 80-87 */
+	_BCM_U,_BCM_U,_BCM_U,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,            /* 88-95 */
+	_BCM_P, _BCM_L|_BCM_X, _BCM_L|_BCM_X, _BCM_L|_BCM_X, _BCM_L|_BCM_X, _BCM_L|_BCM_X,
+	_BCM_L|_BCM_X, _BCM_L, /* 96-103 */
+	_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L, /* 104-111 */
+	_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L, /* 112-119 */
+	_BCM_L,_BCM_L,_BCM_L,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_C, /* 120-127 */
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,     /* 128-143 */
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,     /* 144-159 */
+	_BCM_S|_BCM_SP, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P,
+	_BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, /* 160-175 */
+	_BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P,
+	_BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, /* 176-191 */
+	_BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U,
+	_BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, /* 192-207 */
+	_BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_P, _BCM_U, _BCM_U, _BCM_U,
+	_BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_L, /* 208-223 */
+	_BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L,
+	_BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, /* 224-239 */
+	_BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_P, _BCM_L, _BCM_L, _BCM_L,
+	_BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L /* 240-255 */
+};
+
+ulong
+bcm_strtoul(char *cp, char **endp, uint base)
+{
+	ulong result, last_result = 0, value;
+	bool minus;
+
+	minus = FALSE;
+
+	while (bcm_isspace(*cp))
+		cp++;
+
+	if (cp[0] == '+')
+		cp++;
+	else if (cp[0] == '-') {
+		minus = TRUE;
+		cp++;
+	}
+
+	if (base == 0) {
+		if (cp[0] == '0') {
+			if ((cp[1] == 'x') || (cp[1] == 'X')) {
+				base = 16;
+				cp = &cp[2];
+			} else {
+				base = 8;
+				cp = &cp[1];
+			}
+		} else
+			base = 10;
+	} else if (base == 16 && (cp[0] == '0') && ((cp[1] == 'x') || (cp[1] == 'X'))) {
+		cp = &cp[2];
+	}
+
+	result = 0;
+
+	while (bcm_isxdigit(*cp) &&
+		(value = bcm_isdigit(*cp) ? *cp-'0' : bcm_toupper(*cp)-'A'+10) < base)
+	{
+		result = result*base + value;
+		/* Detected overflow */
+		if (result < last_result && !minus)
+			return (ulong)-1;
+		last_result = result;
+		cp++;
+	}
+
+	if (minus)
+		result = (ulong)(-(long)result);
+
+	if (endp)
+		*endp = (char *)cp;
+
+	return (result);
+}
+
+int
+bcm_atoi(char *s)
+{
+	return (int)bcm_strtoul(s, NULL, 10);
+}
+
+/* return pointer to location of substring 'needle' in 'haystack' */
+char*
+bcmstrstr(char *haystack, char *needle)
+{
+	int len, nlen;
+	int i;
+
+	if ((haystack == NULL) || (needle == NULL))
+		return (haystack);
+
+	nlen = strlen(needle);
+	len = strlen(haystack) - nlen + 1;
+
+	for (i = 0; i < len; i++)
+		if (memcmp(needle, &haystack[i], nlen) == 0)
+			return (&haystack[i]);
+	return (NULL);
+}
+
+char*
+bcmstrcat(char *dest, const char *src)
+{
+	char *p;
+
+	p = dest + strlen(dest);
+
+	while ((*p++ = *src++) != '\0')
+		;
+
+	return (dest);
+}
+
+char*
+bcmstrncat(char *dest, const char *src, uint size)
+{
+	char *endp;
+	char *p;
+
+	p = dest + strlen(dest);
+	endp = p + size;
+
+	while (p != endp && (*p++ = *src++) != '\0')
+		;
+
+	return (dest);
+}
+
+
+/****************************************************************************
+* Function:   bcmstrtok
+*
+* Purpose:
+*  Tokenizes a string. This function is conceptually similiar to ANSI C strtok(),
+*  but allows strToken() to be used by different strings or callers at the same
+*  time. Each call modifies '*string' by substituting a NULL character for the
+*  first delimiter that is encountered, and updates 'string' to point to the char
+*  after the delimiter. Leading delimiters are skipped.
+*
+* Parameters:
+*  string      (mod) Ptr to string ptr, updated by token.
+*  delimiters  (in)  Set of delimiter characters.
+*  tokdelim    (out) Character that delimits the returned token. (May
+*                    be set to NULL if token delimiter is not required).
+*
+* Returns:  Pointer to the next token found. NULL when no more tokens are found.
+*****************************************************************************
+*/
+char *
+bcmstrtok(char **string, const char *delimiters, char *tokdelim)
+{
+	unsigned char *str;
+	unsigned long map[8];
+	int count;
+	char *nextoken;
+
+	if (tokdelim != NULL) {
+		/* Prime the token delimiter */
+		*tokdelim = '\0';
+	}
+
+	/* Clear control map */
+	for (count = 0; count < 8; count++) {
+		map[count] = 0;
+	}
+
+	/* Set bits in delimiter table */
+	do {
+		map[*delimiters >> 5] |= (1 << (*delimiters & 31));
+	}
+	while (*delimiters++);
+
+	str = (unsigned char*)*string;
+
+	/* Find beginning of token (skip over leading delimiters). Note that
+	 * there is no token iff this loop sets str to point to the terminal
+	 * null (*str == '\0')
+	 */
+	while (((map[*str >> 5] & (1 << (*str & 31))) && *str) || (*str == ' ')) {
+		str++;
+	}
+
+	nextoken = (char*)str;
+
+	/* Find the end of the token. If it is not the end of the string,
+	 * put a null there.
+	 */
+	for (; *str; str++) {
+		if (map[*str >> 5] & (1 << (*str & 31))) {
+			if (tokdelim != NULL) {
+				*tokdelim = *str;
+			}
+
+			*str++ = '\0';
+			break;
+		}
+	}
+
+	*string = (char*)str;
+
+	/* Determine if a token has been found. */
+	if (nextoken == (char *) str) {
+		return NULL;
+	}
+	else {
+		return nextoken;
+	}
+}
+
+
+#define xToLower(C) \
+	((C >= 'A' && C <= 'Z') ? (char)((int)C - (int)'A' + (int)'a') : C)
+
+
+/****************************************************************************
+* Function:   bcmstricmp
+*
+* Purpose:    Compare to strings case insensitively.
+*
+* Parameters: s1 (in) First string to compare.
+*             s2 (in) Second string to compare.
+*
+* Returns:    Return 0 if the two strings are equal, -1 if t1 < t2 and 1 if
+*             t1 > t2, when ignoring case sensitivity.
+*****************************************************************************
+*/
+int
+bcmstricmp(const char *s1, const char *s2)
+{
+	char dc, sc;
+
+	while (*s2 && *s1) {
+		dc = xToLower(*s1);
+		sc = xToLower(*s2);
+		if (dc < sc) return -1;
+		if (dc > sc) return 1;
+		s1++;
+		s2++;
+	}
+
+	if (*s1 && !*s2) return 1;
+	if (!*s1 && *s2) return -1;
+	return 0;
+}
+
+
+/****************************************************************************
+* Function:   bcmstrnicmp
+*
+* Purpose:    Compare to strings case insensitively, upto a max of 'cnt'
+*             characters.
+*
+* Parameters: s1  (in) First string to compare.
+*             s2  (in) Second string to compare.
+*             cnt (in) Max characters to compare.
+*
+* Returns:    Return 0 if the two strings are equal, -1 if t1 < t2 and 1 if
+*             t1 > t2, when ignoring case sensitivity.
+*****************************************************************************
+*/
+int
+bcmstrnicmp(const char* s1, const char* s2, int cnt)
+{
+	char dc, sc;
+
+	while (*s2 && *s1 && cnt) {
+		dc = xToLower(*s1);
+		sc = xToLower(*s2);
+		if (dc < sc) return -1;
+		if (dc > sc) return 1;
+		s1++;
+		s2++;
+		cnt--;
+	}
+
+	if (!cnt) return 0;
+	if (*s1 && !*s2) return 1;
+	if (!*s1 && *s2) return -1;
+	return 0;
+}
+
+/* parse a xx:xx:xx:xx:xx:xx format ethernet address */
+int
+bcm_ether_atoe(char *p, struct ether_addr *ea)
+{
+	int i = 0;
+
+	for (;;) {
+		ea->octet[i++] = (char) bcm_strtoul(p, &p, 16);
+		if (!*p++ || i == 6)
+			break;
+	}
+
+	return (i == 6);
+}
+
+
+#if defined(CONFIG_USBRNDIS_RETAIL) || defined(NDIS_MINIPORT_DRIVER)
+/* registry routine buffer preparation utility functions:
+ * parameter order is like strncpy, but returns count
+ * of bytes copied. Minimum bytes copied is null char(1)/wchar(2)
+ */
+ulong
+wchar2ascii(char *abuf, ushort *wbuf, ushort wbuflen, ulong abuflen)
+{
+	ulong copyct = 1;
+	ushort i;
+
+	if (abuflen == 0)
+		return 0;
+
+	/* wbuflen is in bytes */
+	wbuflen /= sizeof(ushort);
+
+	for (i = 0; i < wbuflen; ++i) {
+		if (--abuflen == 0)
+			break;
+		*abuf++ = (char) *wbuf++;
+		++copyct;
+	}
+	*abuf = '\0';
+
+	return copyct;
+}
+#endif /* CONFIG_USBRNDIS_RETAIL || NDIS_MINIPORT_DRIVER */
+
+char *
+bcm_ether_ntoa(const struct ether_addr *ea, char *buf)
+{
+	static const char template[] = "%02x:%02x:%02x:%02x:%02x:%02x";
+	snprintf(buf, 18, template,
+		ea->octet[0]&0xff, ea->octet[1]&0xff, ea->octet[2]&0xff,
+		ea->octet[3]&0xff, ea->octet[4]&0xff, ea->octet[5]&0xff);
+	return (buf);
+}
+
+char *
+bcm_ip_ntoa(struct ipv4_addr *ia, char *buf)
+{
+	snprintf(buf, 16, "%d.%d.%d.%d",
+		ia->addr[0], ia->addr[1], ia->addr[2], ia->addr[3]);
+	return (buf);
+}
+
+#ifdef BCMDRIVER
+
+void
+bcm_mdelay(uint ms)
+{
+	uint i;
+
+	for (i = 0; i < ms; i++) {
+		OSL_DELAY(1000);
+	}
+}
+
+
+
+
+
+#if defined(DHD_DEBUG)
+/* pretty hex print a pkt buffer chain */
+void
+prpkt(const char *msg, osl_t *osh, void *p0)
+{
+	void *p;
+
+	if (msg && (msg[0] != '\0'))
+		printf("%s:\n", msg);
+
+	for (p = p0; p; p = PKTNEXT(osh, p))
+		prhex(NULL, PKTDATA(osh, p), PKTLEN(osh, p));
+}
+#endif  
+
+/* Takes an Ethernet frame and sets out-of-bound PKTPRIO.
+ * Also updates the inplace vlan tag if requested.
+ * For debugging, it returns an indication of what it did.
+ */
+uint BCMFASTPATH
+pktsetprio(void *pkt, bool update_vtag)
+{
+	struct ether_header *eh;
+	struct ethervlan_header *evh;
+	uint8 *pktdata;
+	int priority = 0;
+	int rc = 0;
+
+	pktdata = (uint8 *) PKTDATA(NULL, pkt);
+	ASSERT(ISALIGNED((uintptr)pktdata, sizeof(uint16)));
+
+	eh = (struct ether_header *) pktdata;
+
+	if (ntoh16(eh->ether_type) == ETHER_TYPE_8021Q) {
+		uint16 vlan_tag;
+		int vlan_prio, dscp_prio = 0;
+
+		evh = (struct ethervlan_header *)eh;
+
+		vlan_tag = ntoh16(evh->vlan_tag);
+		vlan_prio = (int) (vlan_tag >> VLAN_PRI_SHIFT) & VLAN_PRI_MASK;
+
+		if (ntoh16(evh->ether_type) == ETHER_TYPE_IP) {
+			uint8 *ip_body = pktdata + sizeof(struct ethervlan_header);
+			uint8 tos_tc = IP_TOS46(ip_body);
+			dscp_prio = (int)(tos_tc >> IPV4_TOS_PREC_SHIFT);
+		}
+
+		/* DSCP priority gets precedence over 802.1P (vlan tag) */
+		if (dscp_prio != 0) {
+			priority = dscp_prio;
+			rc |= PKTPRIO_VDSCP;
+		} else {
+			priority = vlan_prio;
+			rc |= PKTPRIO_VLAN;
+		}
+		/*
+		 * If the DSCP priority is not the same as the VLAN priority,
+		 * then overwrite the priority field in the vlan tag, with the
+		 * DSCP priority value. This is required for Linux APs because
+		 * the VLAN driver on Linux, overwrites the skb->priority field
+		 * with the priority value in the vlan tag
+		 */
+		if (update_vtag && (priority != vlan_prio)) {
+			vlan_tag &= ~(VLAN_PRI_MASK << VLAN_PRI_SHIFT);
+			vlan_tag |= (uint16)priority << VLAN_PRI_SHIFT;
+			evh->vlan_tag = hton16(vlan_tag);
+			rc |= PKTPRIO_UPD;
+		}
+	} else if (ntoh16(eh->ether_type) == ETHER_TYPE_IP) {
+		uint8 *ip_body = pktdata + sizeof(struct ether_header);
+		uint8 tos_tc = IP_TOS46(ip_body);
+		priority = (int)(tos_tc >> IPV4_TOS_PREC_SHIFT);
+		rc |= PKTPRIO_DSCP;
+	}
+
+	ASSERT(priority >= 0 && priority <= MAXPRIO);
+	PKTSETPRIO(pkt, priority);
+	return (rc | priority);
+}
+
+
+static char bcm_undeferrstr[32];
+static const char *bcmerrorstrtable[] = BCMERRSTRINGTABLE;
+
+/* Convert the error codes into related error strings  */
+const char *
+bcmerrorstr(int bcmerror)
+{
+	/* check if someone added a bcmerror code but forgot to add errorstring */
+	ASSERT(ABS(BCME_LAST) == (ARRAYSIZE(bcmerrorstrtable) - 1));
+
+	if (bcmerror > 0 || bcmerror < BCME_LAST) {
+		snprintf(bcm_undeferrstr, sizeof(bcm_undeferrstr), "Undefined error %d", bcmerror);
+		return bcm_undeferrstr;
+	}
+
+	ASSERT(strlen(bcmerrorstrtable[-bcmerror]) < BCME_STRLEN);
+
+	return bcmerrorstrtable[-bcmerror];
+}
+
+
+
+
+/* iovar table lookup */
+const bcm_iovar_t*
+bcm_iovar_lookup(const bcm_iovar_t *table, const char *name)
+{
+	const bcm_iovar_t *vi;
+	const char *lookup_name;
+
+	/* skip any ':' delimited option prefixes */
+	lookup_name = strrchr(name, ':');
+	if (lookup_name != NULL)
+		lookup_name++;
+	else
+		lookup_name = name;
+
+	ASSERT(table != NULL);
+
+	for (vi = table; vi->name; vi++) {
+		if (!strcmp(vi->name, lookup_name))
+			return vi;
+	}
+	/* ran to end of table */
+
+	return NULL; /* var name not found */
+}
+
+int
+bcm_iovar_lencheck(const bcm_iovar_t *vi, void *arg, int len, bool set)
+{
+	int bcmerror = 0;
+
+	/* length check on io buf */
+	switch (vi->type) {
+	case IOVT_BOOL:
+	case IOVT_INT8:
+	case IOVT_INT16:
+	case IOVT_INT32:
+	case IOVT_UINT8:
+	case IOVT_UINT16:
+	case IOVT_UINT32:
+		/* all integers are int32 sized args at the ioctl interface */
+		if (len < (int)sizeof(int)) {
+			bcmerror = BCME_BUFTOOSHORT;
+		}
+		break;
+
+	case IOVT_BUFFER:
+		/* buffer must meet minimum length requirement */
+		if (len < vi->minlen) {
+			bcmerror = BCME_BUFTOOSHORT;
+		}
+		break;
+
+	case IOVT_VOID:
+		if (!set) {
+			/* Cannot return nil... */
+			bcmerror = BCME_UNSUPPORTED;
+		} else if (len) {
+			/* Set is an action w/o parameters */
+			bcmerror = BCME_BUFTOOLONG;
+		}
+		break;
+
+	default:
+		/* unknown type for length check in iovar info */
+		ASSERT(0);
+		bcmerror = BCME_UNSUPPORTED;
+	}
+
+	return bcmerror;
+}
+
+#endif  /* BCMDRIVER */
+
+
+/*******************************************************************************
+ * crc8
+ *
+ * Computes a crc8 over the input data using the polynomial:
+ *
+ *       x^8 + x^7 +x^6 + x^4 + x^2 + 1
+ *
+ * The caller provides the initial value (either CRC8_INIT_VALUE
+ * or the previous returned value) to allow for processing of
+ * discontiguous blocks of data.  When generating the CRC the
+ * caller is responsible for complementing the final return value
+ * and inserting it into the byte stream.  When checking, a final
+ * return value of CRC8_GOOD_VALUE indicates a valid CRC.
+ *
+ * Reference: Dallas Semiconductor Application Note 27
+ *   Williams, Ross N., "A Painless Guide to CRC Error Detection Algorithms",
+ *     ver 3, Aug 1993, ross@guest.adelaide.edu.au, Rocksoft Pty Ltd.,
+ *     ftp://ftp.rocksoft.com/clients/rocksoft/papers/crc_v3.txt
+ *
+ * ****************************************************************************
+ */
+
+static const uint8 crc8_table[256] = {
+	0x00, 0xF7, 0xB9, 0x4E, 0x25, 0xD2, 0x9C, 0x6B,
+	0x4A, 0xBD, 0xF3, 0x04, 0x6F, 0x98, 0xD6, 0x21,
+	0x94, 0x63, 0x2D, 0xDA, 0xB1, 0x46, 0x08, 0xFF,
+	0xDE, 0x29, 0x67, 0x90, 0xFB, 0x0C, 0x42, 0xB5,
+	0x7F, 0x88, 0xC6, 0x31, 0x5A, 0xAD, 0xE3, 0x14,
+	0x35, 0xC2, 0x8C, 0x7B, 0x10, 0xE7, 0xA9, 0x5E,
+	0xEB, 0x1C, 0x52, 0xA5, 0xCE, 0x39, 0x77, 0x80,
+	0xA1, 0x56, 0x18, 0xEF, 0x84, 0x73, 0x3D, 0xCA,
+	0xFE, 0x09, 0x47, 0xB0, 0xDB, 0x2C, 0x62, 0x95,
+	0xB4, 0x43, 0x0D, 0xFA, 0x91, 0x66, 0x28, 0xDF,
+	0x6A, 0x9D, 0xD3, 0x24, 0x4F, 0xB8, 0xF6, 0x01,
+	0x20, 0xD7, 0x99, 0x6E, 0x05, 0xF2, 0xBC, 0x4B,
+	0x81, 0x76, 0x38, 0xCF, 0xA4, 0x53, 0x1D, 0xEA,
+	0xCB, 0x3C, 0x72, 0x85, 0xEE, 0x19, 0x57, 0xA0,
+	0x15, 0xE2, 0xAC, 0x5B, 0x30, 0xC7, 0x89, 0x7E,
+	0x5F, 0xA8, 0xE6, 0x11, 0x7A, 0x8D, 0xC3, 0x34,
+	0xAB, 0x5C, 0x12, 0xE5, 0x8E, 0x79, 0x37, 0xC0,
+	0xE1, 0x16, 0x58, 0xAF, 0xC4, 0x33, 0x7D, 0x8A,
+	0x3F, 0xC8, 0x86, 0x71, 0x1A, 0xED, 0xA3, 0x54,
+	0x75, 0x82, 0xCC, 0x3B, 0x50, 0xA7, 0xE9, 0x1E,
+	0xD4, 0x23, 0x6D, 0x9A, 0xF1, 0x06, 0x48, 0xBF,
+	0x9E, 0x69, 0x27, 0xD0, 0xBB, 0x4C, 0x02, 0xF5,
+	0x40, 0xB7, 0xF9, 0x0E, 0x65, 0x92, 0xDC, 0x2B,
+	0x0A, 0xFD, 0xB3, 0x44, 0x2F, 0xD8, 0x96, 0x61,
+	0x55, 0xA2, 0xEC, 0x1B, 0x70, 0x87, 0xC9, 0x3E,
+	0x1F, 0xE8, 0xA6, 0x51, 0x3A, 0xCD, 0x83, 0x74,
+	0xC1, 0x36, 0x78, 0x8F, 0xE4, 0x13, 0x5D, 0xAA,
+	0x8B, 0x7C, 0x32, 0xC5, 0xAE, 0x59, 0x17, 0xE0,
+	0x2A, 0xDD, 0x93, 0x64, 0x0F, 0xF8, 0xB6, 0x41,
+	0x60, 0x97, 0xD9, 0x2E, 0x45, 0xB2, 0xFC, 0x0B,
+	0xBE, 0x49, 0x07, 0xF0, 0x9B, 0x6C, 0x22, 0xD5,
+	0xF4, 0x03, 0x4D, 0xBA, 0xD1, 0x26, 0x68, 0x9F
+};
+
+#define CRC_INNER_LOOP(n, c, x) \
+	(c) = ((c) >> 8) ^ crc##n##_table[((c) ^ (x)) & 0xff]
+
+uint8
+hndcrc8(
+	uint8 *pdata,   /* pointer to array of data to process */
+	uint  nbytes,   /* number of input data bytes to process */
+	uint8 crc   /* either CRC8_INIT_VALUE or previous return value */
+)
+{
+	/* hard code the crc loop instead of using CRC_INNER_LOOP macro
+	 * to avoid the undefined and unnecessary (uint8 >> 8) operation.
+	 */
+	while (nbytes-- > 0)
+		crc = crc8_table[(crc ^ *pdata++) & 0xff];
+
+	return crc;
+}
+
+/*******************************************************************************
+ * crc16
+ *
+ * Computes a crc16 over the input data using the polynomial:
+ *
+ *       x^16 + x^12 +x^5 + 1
+ *
+ * The caller provides the initial value (either CRC16_INIT_VALUE
+ * or the previous returned value) to allow for processing of
+ * discontiguous blocks of data.  When generating the CRC the
+ * caller is responsible for complementing the final return value
+ * and inserting it into the byte stream.  When checking, a final
+ * return value of CRC16_GOOD_VALUE indicates a valid CRC.
+ *
+ * Reference: Dallas Semiconductor Application Note 27
+ *   Williams, Ross N., "A Painless Guide to CRC Error Detection Algorithms",
+ *     ver 3, Aug 1993, ross@guest.adelaide.edu.au, Rocksoft Pty Ltd.,
+ *     ftp://ftp.rocksoft.com/clients/rocksoft/papers/crc_v3.txt
+ *
+ * ****************************************************************************
+ */
+
+static const uint16 crc16_table[256] = {
+	0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
+	0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7,
+	0x1081, 0x0108, 0x3393, 0x221A, 0x56A5, 0x472C, 0x75B7, 0x643E,
+	0x9CC9, 0x8D40, 0xBFDB, 0xAE52, 0xDAED, 0xCB64, 0xF9FF, 0xE876,
+	0x2102, 0x308B, 0x0210, 0x1399, 0x6726, 0x76AF, 0x4434, 0x55BD,
+	0xAD4A, 0xBCC3, 0x8E58, 0x9FD1, 0xEB6E, 0xFAE7, 0xC87C, 0xD9F5,
+	0x3183, 0x200A, 0x1291, 0x0318, 0x77A7, 0x662E, 0x54B5, 0x453C,
+	0xBDCB, 0xAC42, 0x9ED9, 0x8F50, 0xFBEF, 0xEA66, 0xD8FD, 0xC974,
+	0x4204, 0x538D, 0x6116, 0x709F, 0x0420, 0x15A9, 0x2732, 0x36BB,
+	0xCE4C, 0xDFC5, 0xED5E, 0xFCD7, 0x8868, 0x99E1, 0xAB7A, 0xBAF3,
+	0x5285, 0x430C, 0x7197, 0x601E, 0x14A1, 0x0528, 0x37B3, 0x263A,
+	0xDECD, 0xCF44, 0xFDDF, 0xEC56, 0x98E9, 0x8960, 0xBBFB, 0xAA72,
+	0x6306, 0x728F, 0x4014, 0x519D, 0x2522, 0x34AB, 0x0630, 0x17B9,
+	0xEF4E, 0xFEC7, 0xCC5C, 0xDDD5, 0xA96A, 0xB8E3, 0x8A78, 0x9BF1,
+	0x7387, 0x620E, 0x5095, 0x411C, 0x35A3, 0x242A, 0x16B1, 0x0738,
+	0xFFCF, 0xEE46, 0xDCDD, 0xCD54, 0xB9EB, 0xA862, 0x9AF9, 0x8B70,
+	0x8408, 0x9581, 0xA71A, 0xB693, 0xC22C, 0xD3A5, 0xE13E, 0xF0B7,
+	0x0840, 0x19C9, 0x2B52, 0x3ADB, 0x4E64, 0x5FED, 0x6D76, 0x7CFF,
+	0x9489, 0x8500, 0xB79B, 0xA612, 0xD2AD, 0xC324, 0xF1BF, 0xE036,
+	0x18C1, 0x0948, 0x3BD3, 0x2A5A, 0x5EE5, 0x4F6C, 0x7DF7, 0x6C7E,
+	0xA50A, 0xB483, 0x8618, 0x9791, 0xE32E, 0xF2A7, 0xC03C, 0xD1B5,
+	0x2942, 0x38CB, 0x0A50, 0x1BD9, 0x6F66, 0x7EEF, 0x4C74, 0x5DFD,
+	0xB58B, 0xA402, 0x9699, 0x8710, 0xF3AF, 0xE226, 0xD0BD, 0xC134,
+	0x39C3, 0x284A, 0x1AD1, 0x0B58, 0x7FE7, 0x6E6E, 0x5CF5, 0x4D7C,
+	0xC60C, 0xD785, 0xE51E, 0xF497, 0x8028, 0x91A1, 0xA33A, 0xB2B3,
+	0x4A44, 0x5BCD, 0x6956, 0x78DF, 0x0C60, 0x1DE9, 0x2F72, 0x3EFB,
+	0xD68D, 0xC704, 0xF59F, 0xE416, 0x90A9, 0x8120, 0xB3BB, 0xA232,
+	0x5AC5, 0x4B4C, 0x79D7, 0x685E, 0x1CE1, 0x0D68, 0x3FF3, 0x2E7A,
+	0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1,
+	0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9,
+	0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330,
+	0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78
+};
+
+uint16
+hndcrc16(
+	uint8 *pdata,  /* pointer to array of data to process */
+	uint nbytes, /* number of input data bytes to process */
+	uint16 crc     /* either CRC16_INIT_VALUE or previous return value */
+)
+{
+	while (nbytes-- > 0)
+		CRC_INNER_LOOP(16, crc, *pdata++);
+	return crc;
+}
+
+static const uint32 crc32_table[256] = {
+	0x00000000, 0x77073096, 0xEE0E612C, 0x990951BA,
+	0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3,
+	0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988,
+	0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, 0x90BF1D91,
+	0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE,
+	0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7,
+	0x136C9856, 0x646BA8C0, 0xFD62F97A, 0x8A65C9EC,
+	0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5,
+	0x3B6E20C8, 0x4C69105E, 0xD56041E4, 0xA2677172,
+	0x3C03E4D1, 0x4B04D447, 0xD20D85FD, 0xA50AB56B,
+	0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940,
+	0x32D86CE3, 0x45DF5C75, 0xDCD60DCF, 0xABD13D59,
+	0x26D930AC, 0x51DE003A, 0xC8D75180, 0xBFD06116,
+	0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F,
+	0x2802B89E, 0x5F058808, 0xC60CD9B2, 0xB10BE924,
+	0x2F6F7C87, 0x58684C11, 0xC1611DAB, 0xB6662D3D,
+	0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A,
+	0x71B18589, 0x06B6B51F, 0x9FBFE4A5, 0xE8B8D433,
+	0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818,
+	0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01,
+	0x6B6B51F4, 0x1C6C6162, 0x856530D8, 0xF262004E,
+	0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457,
+	0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C,
+	0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3, 0xFBD44C65,
+	0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2,
+	0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB,
+	0x4369E96A, 0x346ED9FC, 0xAD678846, 0xDA60B8D0,
+	0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9,
+	0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086,
+	0x5768B525, 0x206F85B3, 0xB966D409, 0xCE61E49F,
+	0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4,
+	0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD,
+	0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A,
+	0xEAD54739, 0x9DD277AF, 0x04DB2615, 0x73DC1683,
+	0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8,
+	0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1,
+	0xF00F9344, 0x8708A3D2, 0x1E01F268, 0x6906C2FE,
+	0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7,
+	0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC,
+	0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43, 0x60B08ED5,
+	0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252,
+	0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B,
+	0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6, 0x41047A60,
+	0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79,
+	0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236,
+	0xCC0C7795, 0xBB0B4703, 0x220216B9, 0x5505262F,
+	0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04,
+	0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D,
+	0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A,
+	0x9C0906A9, 0xEB0E363F, 0x72076785, 0x05005713,
+	0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38,
+	0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21,
+	0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8, 0x1FDA836E,
+	0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777,
+	0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C,
+	0x8F659EFF, 0xF862AE69, 0x616BFFD3, 0x166CCF45,
+	0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2,
+	0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB,
+	0xAED16A4A, 0xD9D65ADC, 0x40DF0B66, 0x37D83BF0,
+	0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9,
+	0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6,
+	0xBAD03605, 0xCDD70693, 0x54DE5729, 0x23D967BF,
+	0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94,
+	0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D
+};
+
+/*
+ * crc input is CRC32_INIT_VALUE for a fresh start, or previous return value if
+ * accumulating over multiple pieces.
+ */
+uint32
+hndcrc32(uint8 *pdata, uint nbytes, uint32 crc)
+{
+	uint8 *pend;
+#ifdef __mips__
+	uint8 tmp[4];
+	ulong *tptr = (ulong *)tmp;
+
+	/* in case the beginning of the buffer isn't aligned */
+	pend = (uint8 *)((uint)(pdata + 3) & 0xfffffffc);
+	nbytes -= (pend - pdata);
+	while (pdata < pend)
+		CRC_INNER_LOOP(32, crc, *pdata++);
+
+	/* handle bulk of data as 32-bit words */
+	pend = pdata + (nbytes & 0xfffffffc);
+	while (pdata < pend) {
+		*tptr = *(ulong *)pdata;
+		pdata += sizeof(ulong *);
+		CRC_INNER_LOOP(32, crc, tmp[0]);
+		CRC_INNER_LOOP(32, crc, tmp[1]);
+		CRC_INNER_LOOP(32, crc, tmp[2]);
+		CRC_INNER_LOOP(32, crc, tmp[3]);
+	}
+
+	/* 1-3 bytes at end of buffer */
+	pend = pdata + (nbytes & 0x03);
+	while (pdata < pend)
+		CRC_INNER_LOOP(32, crc, *pdata++);
+#else
+	pend = pdata + nbytes;
+	while (pdata < pend)
+		CRC_INNER_LOOP(32, crc, *pdata++);
+#endif /* __mips__ */
+
+	return crc;
+}
+
+#ifdef notdef
+#define CLEN    1499    /*  CRC Length */
+#define CBUFSIZ     (CLEN+4)
+#define CNBUFS      5 /* # of bufs */
+
+void
+testcrc32(void)
+{
+	uint j, k, l;
+	uint8 *buf;
+	uint len[CNBUFS];
+	uint32 crcr;
+	uint32 crc32tv[CNBUFS] =
+		{0xd2cb1faa, 0xd385c8fa, 0xf5b4f3f3, 0x55789e20, 0x00343110};
+
+	ASSERT((buf = MALLOC(CBUFSIZ*CNBUFS)) != NULL);
+
+	/* step through all possible alignments */
+	for (l = 0; l <= 4; l++) {
+		for (j = 0; j < CNBUFS; j++) {
+			len[j] = CLEN;
+			for (k = 0; k < len[j]; k++)
+				*(buf + j*CBUFSIZ + (k+l)) = (j+k) & 0xff;
+		}
+
+		for (j = 0; j < CNBUFS; j++) {
+			crcr = crc32(buf + j*CBUFSIZ + l, len[j], CRC32_INIT_VALUE);
+			ASSERT(crcr == crc32tv[j]);
+		}
+	}
+
+	MFREE(buf, CBUFSIZ*CNBUFS);
+	return;
+}
+#endif /* notdef */
+
+/*
+ * Advance from the current 1-byte tag/1-byte length/variable-length value
+ * triple, to the next, returning a pointer to the next.
+ * If the current or next TLV is invalid (does not fit in given buffer length),
+ * NULL is returned.
+ * *buflen is not modified if the TLV elt parameter is invalid, or is decremented
+ * by the TLV parameter's length if it is valid.
+ */
+bcm_tlv_t *
+bcm_next_tlv(bcm_tlv_t *elt, int *buflen)
+{
+	int len;
+
+	/* validate current elt */
+	if (!bcm_valid_tlv(elt, *buflen))
+		return NULL;
+
+	/* advance to next elt */
+	len = elt->len;
+	elt = (bcm_tlv_t*)(elt->data + len);
+	*buflen -= (2 + len);
+
+	/* validate next elt */
+	if (!bcm_valid_tlv(elt, *buflen))
+		return NULL;
+
+	return elt;
+}
+
+/*
+ * Traverse a string of 1-byte tag/1-byte length/variable-length value
+ * triples, returning a pointer to the substring whose first element
+ * matches tag
+ */
+bcm_tlv_t *
+bcm_parse_tlvs(void *buf, int buflen, uint key)
+{
+	bcm_tlv_t *elt;
+	int totlen;
+
+	elt = (bcm_tlv_t*)buf;
+	totlen = buflen;
+
+	/* find tagged parameter */
+	while (totlen >= 2) {
+		int len = elt->len;
+
+		/* validate remaining totlen */
+		if ((elt->id == key) && (totlen >= (len + 2)))
+			return (elt);
+
+		elt = (bcm_tlv_t*)((uint8*)elt + (len + 2));
+		totlen -= (len + 2);
+	}
+
+	return NULL;
+}
+
+/*
+ * Traverse a string of 1-byte tag/1-byte length/variable-length value
+ * triples, returning a pointer to the substring whose first element
+ * matches tag.  Stop parsing when we see an element whose ID is greater
+ * than the target key.
+ */
+bcm_tlv_t *
+bcm_parse_ordered_tlvs(void *buf, int buflen, uint key)
+{
+	bcm_tlv_t *elt;
+	int totlen;
+
+	elt = (bcm_tlv_t*)buf;
+	totlen = buflen;
+
+	/* find tagged parameter */
+	while (totlen >= 2) {
+		uint id = elt->id;
+		int len = elt->len;
+
+		/* Punt if we start seeing IDs > than target key */
+		if (id > key)
+			return (NULL);
+
+		/* validate remaining totlen */
+		if ((id == key) && (totlen >= (len + 2)))
+			return (elt);
+
+		elt = (bcm_tlv_t*)((uint8*)elt + (len + 2));
+		totlen -= (len + 2);
+	}
+	return NULL;
+}
+
+#if defined(WLMSG_PRHDRS) || defined(WLMSG_PRPKT) || defined(WLMSG_ASSOC) || \
+	defined(DHD_DEBUG)
+int
+bcm_format_flags(const bcm_bit_desc_t *bd, uint32 flags, char* buf, int len)
+{
+	int i;
+	char* p = buf;
+	char hexstr[16];
+	int slen = 0, nlen = 0;
+	uint32 bit;
+	const char* name;
+
+	if (len < 2 || !buf)
+		return 0;
+
+	buf[0] = '\0';
+
+	for (i = 0; flags != 0; i++) {
+		bit = bd[i].bit;
+		name = bd[i].name;
+		if (bit == 0 && flags != 0) {
+			/* print any unnamed bits */
+			snprintf(hexstr, 16, "0x%X", flags);
+			name = hexstr;
+			flags = 0;  /* exit loop */
+		} else if ((flags & bit) == 0)
+			continue;
+		flags &= ~bit;
+		nlen = strlen(name);
+		slen += nlen;
+		/* count btwn flag space */
+		if (flags != 0)
+			slen += 1;
+		/* need NULL char as well */
+		if (len <= slen)
+			break;
+		/* copy NULL char but don't count it */
+		strncpy(p, name, nlen + 1);
+		p += nlen;
+		/* copy btwn flag space and NULL char */
+		if (flags != 0)
+			p += snprintf(p, 2, " ");
+		len -= slen;
+	}
+
+	/* indicate the str was too short */
+	if (flags != 0) {
+		if (len < 2)
+			p -= 2 - len;   /* overwrite last char */
+		p += snprintf(p, 2, ">");
+	}
+
+	return (int)(p - buf);
+}
+#endif 
+
+#if defined(WLMSG_PRHDRS) || defined(WLMSG_PRPKT) || defined(WLMSG_ASSOC) || \
+	defined(DHD_DEBUG) || defined(WLMEDIA_PEAKRATE)
+/* print bytes formatted as hex to a string. return the resulting string length */
+int
+bcm_format_hex(char *str, const void *bytes, int len)
+{
+	int i;
+	char *p = str;
+	const uint8 *src = (const uint8*)bytes;
+
+	for (i = 0; i < len; i++) {
+		p += snprintf(p, 3, "%02X", *src);
+		src++;
+	}
+	return (int)(p - str);
+}
+#endif 
+
+/* pretty hex print a contiguous buffer */
+void
+prhex(const char *msg, uchar *buf, uint nbytes)
+{
+	char line[128], *p;
+	int len = sizeof(line);
+	int nchar;
+	uint i;
+
+	if (msg && (msg[0] != '\0'))
+		printf("%s:\n", msg);
+
+	p = line;
+	for (i = 0; i < nbytes; i++) {
+		if (i % 16 == 0) {
+			nchar = snprintf(p, len, "  %04d: ", i);    /* line prefix */
+			p += nchar;
+			len -= nchar;
+		}
+		if (len > 0) {
+			nchar = snprintf(p, len, "%02x ", buf[i]);
+			p += nchar;
+			len -= nchar;
+		}
+
+		if (i % 16 == 15) {
+			printf("%s\n", line);       /* flush line */
+			p = line;
+			len = sizeof(line);
+		}
+	}
+
+	/* flush last partial line */
+	if (p != line)
+		printf("%s\n", line);
+}
+
+static const char *crypto_algo_names[] = {
+	"NONE",
+	"WEP1",
+	"TKIP",
+	"WEP128",
+	"AES_CCM",
+	"AES_OCB_MSDU",
+	"AES_OCB_MPDU",
+	"NALG"
+	"UNDEF",
+	"UNDEF",
+	"UNDEF",
+	"UNDEF"
+};
+
+const char *
+bcm_crypto_algo_name(uint algo)
+{
+	return (algo < ARRAYSIZE(crypto_algo_names)) ? crypto_algo_names[algo] : "ERR";
+}
+
+
+char *
+bcm_chipname(uint chipid, char *buf, uint len)
+{
+	const char *fmt;
+
+	fmt = ((chipid > 0xa000) || (chipid < 0x4000)) ? "%d" : "%x";
+	snprintf(buf, len, fmt, chipid);
+	return buf;
+}
+
+/* Produce a human-readable string for boardrev */
+char *
+bcm_brev_str(uint32 brev, char *buf)
+{
+	if (brev < 0x100)
+		snprintf(buf, 8, "%d.%d", (brev & 0xf0) >> 4, brev & 0xf);
+	else
+		snprintf(buf, 8, "%c%03x", ((brev & 0xf000) == 0x1000) ? 'P' : 'A', brev & 0xfff);
+
+	return (buf);
+}
+
+#define BUFSIZE_TODUMP_ATONCE 512 /* Buffer size */
+
+/* dump large strings to console */
+void
+printbig(char *buf)
+{
+	uint len, max_len;
+	char c;
+
+	len = strlen(buf);
+
+	max_len = BUFSIZE_TODUMP_ATONCE;
+
+	while (len > max_len) {
+		c = buf[max_len];
+		buf[max_len] = '\0';
+		printf("%s", buf);
+		buf[max_len] = c;
+
+		buf += max_len;
+		len -= max_len;
+	}
+	/* print the remaining string */
+	printf("%s\n", buf);
+	return;
+}
+
+/* routine to dump fields in a fileddesc structure */
+uint
+bcmdumpfields(bcmutl_rdreg_rtn read_rtn, void *arg0, uint arg1, struct fielddesc *fielddesc_array,
+	char *buf, uint32 bufsize)
+{
+	uint  filled_len;
+	int len;
+	struct fielddesc *cur_ptr;
+
+	filled_len = 0;
+	cur_ptr = fielddesc_array;
+
+	while (bufsize > 1) {
+		if (cur_ptr->nameandfmt == NULL)
+			break;
+		len = snprintf(buf, bufsize, cur_ptr->nameandfmt,
+			read_rtn(arg0, arg1, cur_ptr->offset));
+		/* check for snprintf overflow or error */
+		if (len < 0 || (uint32)len >= bufsize)
+			len = bufsize - 1;
+		buf += len;
+		bufsize -= len;
+		filled_len += len;
+		cur_ptr++;
+	}
+	return filled_len;
+}
+
+uint
+bcm_mkiovar(char *name, char *data, uint datalen, char *buf, uint buflen)
+{
+	uint len;
+
+	len = strlen(name) + 1;
+
+	if ((len + datalen) > buflen)
+		return 0;
+
+	strncpy(buf, name, buflen);
+
+	/* append data onto the end of the name string */
+	memcpy(&buf[len], data, datalen);
+	len += datalen;
+
+	return len;
+}
+
+/* Quarter dBm units to mW
+ * Table starts at QDBM_OFFSET, so the first entry is mW for qdBm=153
+ * Table is offset so the last entry is largest mW value that fits in
+ * a uint16.
+ */
+
+#define QDBM_OFFSET 153     /* Offset for first entry */
+#define QDBM_TABLE_LEN 40   /* Table size */
+
+/* Smallest mW value that will round up to the first table entry, QDBM_OFFSET.
+ * Value is ( mW(QDBM_OFFSET - 1) + mW(QDBM_OFFSET) ) / 2
+ */
+#define QDBM_TABLE_LOW_BOUND 6493 /* Low bound */
+
+/* Largest mW value that will round down to the last table entry,
+ * QDBM_OFFSET + QDBM_TABLE_LEN-1.
+ * Value is ( mW(QDBM_OFFSET + QDBM_TABLE_LEN - 1) + mW(QDBM_OFFSET + QDBM_TABLE_LEN) ) / 2.
+ */
+#define QDBM_TABLE_HIGH_BOUND 64938 /* High bound */
+
+static const uint16 nqdBm_to_mW_map[QDBM_TABLE_LEN] = {
+/* qdBm:    +0  +1  +2  +3  +4  +5  +6  +7 */
+/* 153: */      6683,   7079,   7499,   7943,   8414,   8913,   9441,   10000,
+/* 161: */      10593,  11220,  11885,  12589,  13335,  14125,  14962,  15849,
+/* 169: */      16788,  17783,  18836,  19953,  21135,  22387,  23714,  25119,
+/* 177: */      26607,  28184,  29854,  31623,  33497,  35481,  37584,  39811,
+/* 185: */      42170,  44668,  47315,  50119,  53088,  56234,  59566,  63096
+};
+
+uint16
+bcm_qdbm_to_mw(uint8 qdbm)
+{
+	uint factor = 1;
+	int idx = qdbm - QDBM_OFFSET;
+
+	if (idx >= QDBM_TABLE_LEN) {
+		/* clamp to max uint16 mW value */
+		return 0xFFFF;
+	}
+
+	/* scale the qdBm index up to the range of the table 0-40
+	 * where an offset of 40 qdBm equals a factor of 10 mW.
+	 */
+	while (idx < 0) {
+		idx += 40;
+		factor *= 10;
+	}
+
+	/* return the mW value scaled down to the correct factor of 10,
+	 * adding in factor/2 to get proper rounding.
+	 */
+	return ((nqdBm_to_mW_map[idx] + factor/2) / factor);
+}
+
+uint8
+bcm_mw_to_qdbm(uint16 mw)
+{
+	uint8 qdbm;
+	int offset;
+	uint mw_uint = mw;
+	uint boundary;
+
+	/* handle boundary case */
+	if (mw_uint <= 1)
+		return 0;
+
+	offset = QDBM_OFFSET;
+
+	/* move mw into the range of the table */
+	while (mw_uint < QDBM_TABLE_LOW_BOUND) {
+		mw_uint *= 10;
+		offset -= 40;
+	}
+
+	for (qdbm = 0; qdbm < QDBM_TABLE_LEN-1; qdbm++) {
+		boundary = nqdBm_to_mW_map[qdbm] + (nqdBm_to_mW_map[qdbm+1] -
+			nqdBm_to_mW_map[qdbm])/2;
+		if (mw_uint < boundary)
+			break;
+	}
+
+	qdbm += (uint8)offset;
+
+	return (qdbm);
+}
+
+
+uint
+bcm_bitcount(uint8 *bitmap, uint length)
+{
+	uint bitcount = 0, i;
+	uint8 tmp;
+	for (i = 0; i < length; i++) {
+		tmp = bitmap[i];
+		while (tmp) {
+			bitcount++;
+			tmp &= (tmp - 1);
+		}
+	}
+	return bitcount;
+}
+
+#ifdef BCMDRIVER
+
+/* Initialization of bcmstrbuf structure */
+void
+bcm_binit(struct bcmstrbuf *b, char *buf, uint size)
+{
+	b->origsize = b->size = size;
+	b->origbuf = b->buf = buf;
+}
+
+/* Buffer sprintf wrapper to guard against buffer overflow */
+int
+bcm_bprintf(struct bcmstrbuf *b, const char *fmt, ...)
+{
+	va_list ap;
+	int r;
+
+	va_start(ap, fmt);
+	r = vsnprintf(b->buf, b->size, fmt, ap);
+
+	/* Non Ansi C99 compliant returns -1,
+	 * Ansi compliant return r >= b->size,
+	 * bcmstdlib returns 0, handle all
+	 */
+	if ((r == -1) || (r >= (int)b->size) || (r == 0)) {
+		b->size = 0;
+	} else {
+		b->size -= r;
+		b->buf += r;
+	}
+
+	va_end(ap);
+
+	return r;
+}
+
+void
+bcm_inc_bytes(uchar *num, int num_bytes, uint8 amount)
+{
+	int i;
+
+	for (i = 0; i < num_bytes; i++) {
+		num[i] += amount;
+		if (num[i] >= amount)
+			break;
+		amount = 1;
+	}
+}
+
+int
+bcm_cmp_bytes(uchar *arg1, uchar *arg2, uint8 nbytes)
+{
+	int i;
+
+	for (i = nbytes - 1; i >= 0; i--) {
+		if (arg1[i] != arg2[i])
+			return (arg1[i] - arg2[i]);
+	}
+	return 0;
+}
+
+void
+bcm_print_bytes(char *name, const uchar *data, int len)
+{
+	int i;
+	int per_line = 0;
+
+	printf("%s: %d \n", name ? name : "", len);
+	for (i = 0; i < len; i++) {
+		printf("%02x ", *data++);
+		per_line++;
+		if (per_line == 16) {
+			per_line = 0;
+			printf("\n");
+		}
+	}
+	printf("\n");
+}
+#if defined(WLTINYDUMP) || defined(WLMSG_INFORM) || defined(WLMSG_ASSOC) || \
+	defined(WLMSG_PRPKT) || defined(WLMSG_WSEC)
+#define SSID_FMT_BUF_LEN    ((4 * DOT11_MAX_SSID_LEN) + 1)
+
+int
+bcm_format_ssid(char* buf, const uchar ssid[], uint ssid_len)
+{
+	uint i, c;
+	char *p = buf;
+	char *endp = buf + SSID_FMT_BUF_LEN;
+
+	if (ssid_len > DOT11_MAX_SSID_LEN) ssid_len = DOT11_MAX_SSID_LEN;
+
+	for (i = 0; i < ssid_len; i++) {
+		c = (uint)ssid[i];
+		if (c == '\\') {
+			*p++ = '\\';
+			*p++ = '\\';
+		} else if (bcm_isprint((uchar)c)) {
+			*p++ = (char)c;
+		} else {
+			p += snprintf(p, (endp - p), "\\x%02X", c);
+		}
+	}
+	*p = '\0';
+	ASSERT(p < endp);
+
+	return (int)(p - buf);
+}
+#endif 
+
+#endif /* BCMDRIVER */
+
+/*
+ * ProcessVars:Takes a buffer of "<var>=<value>\n" lines read from a file and ending in a NUL.
+ * also accepts nvram files which are already in the format of <var1>=<value>\0\<var2>=<value2>\0
+ * Removes carriage returns, empty lines, comment lines, and converts newlines to NULs.
+ * Shortens buffer as needed and pads with NULs.  End of buffer is marked by two NULs.
+*/
+
+unsigned int
+process_nvram_vars(char *varbuf, unsigned int len)
+{
+	char *dp;
+	bool findNewline;
+	int column;
+	unsigned int buf_len, n;
+	unsigned int pad = 0;
+
+	dp = varbuf;
+
+	findNewline = FALSE;
+	column = 0;
+
+	for (n = 0; n < len; n++) {
+		if (varbuf[n] == '\r')
+			continue;
+		if (findNewline && varbuf[n] != '\n')
+			continue;
+		findNewline = FALSE;
+		if (varbuf[n] == '#') {
+			findNewline = TRUE;
+			continue;
+		}
+		if (varbuf[n] == '\n') {
+			if (column == 0)
+				continue;
+			*dp++ = 0;
+			column = 0;
+			continue;
+		}
+		*dp++ = varbuf[n];
+		column++;
+	}
+	buf_len = (unsigned int)(dp - varbuf);
+	if (buf_len % 4) {
+		pad = 4 - buf_len % 4;
+		if (pad && (buf_len + pad <= len)) {
+			buf_len += pad;
+		}
+	}
+
+	while (dp < varbuf + n)
+		*dp++ = 0;
+
+	return buf_len;
+}
diff --git a/drivers/net/wireless/bcmdhd/bcmwifi.c b/drivers/net/wireless/bcmdhd/bcmwifi.c
new file mode 100644
index 000000000000..70722170bdfd
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/bcmwifi.c
@@ -0,0 +1,274 @@
+/*
+ * Misc utility routines used by kernel or app-level.
+ * Contents are wifi-specific, used by any kernel or app-level
+ * software that might want wifi things as it grows.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: bcmwifi.c,v 1.31.8.1 2010-08-03 17:47:05 Exp $
+ */
+
+
+#include <typedefs.h>
+
+#ifdef BCMDRIVER
+#include <osl.h>
+#include <bcmutils.h>
+#define strtoul(nptr, endptr, base) bcm_strtoul((nptr), (endptr), (base))
+#define tolower(c) (bcm_isupper((c)) ? ((c) + 'a' - 'A') : (c))
+#else
+#include <stdio.h>
+#include <stdlib.h>
+#include <ctype.h>
+#ifndef ASSERT
+#define ASSERT(exp)
+#endif
+#endif 
+#include <bcmwifi.h>
+
+#if defined(WIN32) && (defined(BCMDLL) || defined(WLMDLL))
+#include <bcmstdlib.h> 	
+#endif
+
+
+
+
+
+char *
+wf_chspec_ntoa(chanspec_t chspec, char *buf)
+{
+	const char *band, *bw, *sb;
+	uint channel;
+
+	band = "";
+	bw = "";
+	sb = "";
+	channel = CHSPEC_CHANNEL(chspec);
+	
+	if ((CHSPEC_IS2G(chspec) && channel > CH_MAX_2G_CHANNEL) ||
+	    (CHSPEC_IS5G(chspec) && channel <= CH_MAX_2G_CHANNEL))
+		band = (CHSPEC_IS2G(chspec)) ? "b" : "a";
+	if (CHSPEC_IS40(chspec)) {
+		if (CHSPEC_SB_UPPER(chspec)) {
+			sb = "u";
+			channel += CH_10MHZ_APART;
+		} else {
+			sb = "l";
+			channel -= CH_10MHZ_APART;
+		}
+	} else if (CHSPEC_IS10(chspec)) {
+		bw = "n";
+	}
+
+	
+	snprintf(buf, 6, "%d%s%s%s", channel, band, bw, sb);
+	return (buf);
+}
+
+
+chanspec_t
+wf_chspec_aton(char *a)
+{
+	char *endp = NULL;
+	uint channel, band, bw, ctl_sb;
+	char c;
+
+	channel = strtoul(a, &endp, 10);
+
+	
+	if (endp == a)
+		return 0;
+
+	if (channel > MAXCHANNEL)
+		return 0;
+
+	band = ((channel <= CH_MAX_2G_CHANNEL) ? WL_CHANSPEC_BAND_2G : WL_CHANSPEC_BAND_5G);
+	bw = WL_CHANSPEC_BW_20;
+	ctl_sb = WL_CHANSPEC_CTL_SB_NONE;
+
+	a = endp;
+
+	c = tolower(a[0]);
+	if (c == '\0')
+		goto done;
+
+	
+	if (c == 'a' || c == 'b') {
+		band = (c == 'a') ? WL_CHANSPEC_BAND_5G : WL_CHANSPEC_BAND_2G;
+		a++;
+		c = tolower(a[0]);
+		if (c == '\0')
+			goto done;
+	}
+
+	
+	if (c == 'n') {
+		bw = WL_CHANSPEC_BW_10;
+	} else if (c == 'l') {
+		bw = WL_CHANSPEC_BW_40;
+		ctl_sb = WL_CHANSPEC_CTL_SB_LOWER;
+		
+		if (channel <= (MAXCHANNEL - CH_20MHZ_APART))
+			channel += CH_10MHZ_APART;
+		else
+			return 0;
+	} else if (c == 'u') {
+		bw = WL_CHANSPEC_BW_40;
+		ctl_sb = WL_CHANSPEC_CTL_SB_UPPER;
+		
+		if (channel > CH_20MHZ_APART)
+			channel -= CH_10MHZ_APART;
+		else
+			return 0;
+	} else {
+		return 0;
+	}
+
+done:
+	return (channel | band | bw | ctl_sb);
+}
+
+
+bool
+wf_chspec_malformed(chanspec_t chanspec)
+{
+	
+	if (!CHSPEC_IS5G(chanspec) && !CHSPEC_IS2G(chanspec))
+		return TRUE;
+	
+	if (!CHSPEC_IS40(chanspec) && !CHSPEC_IS20(chanspec))
+		return TRUE;
+
+	
+	if (CHSPEC_IS20_UNCOND(chanspec)) {
+		if (!CHSPEC_SB_NONE(chanspec))
+			return TRUE;
+	} else {
+		if (!CHSPEC_SB_UPPER(chanspec) && !CHSPEC_SB_LOWER(chanspec))
+		return TRUE;
+	}
+
+	return FALSE;
+}
+
+
+uint8
+wf_chspec_ctlchan(chanspec_t chspec)
+{
+	uint8 ctl_chan;
+
+	
+	if (CHSPEC_CTL_SB(chspec) == WL_CHANSPEC_CTL_SB_NONE) {
+		return CHSPEC_CHANNEL(chspec);
+	} else {
+		
+		ASSERT(CHSPEC_BW(chspec) == WL_CHANSPEC_BW_40);
+		
+		if (CHSPEC_CTL_SB(chspec) == WL_CHANSPEC_CTL_SB_UPPER) {
+			
+			ctl_chan = UPPER_20_SB(CHSPEC_CHANNEL(chspec));
+		} else {
+			ASSERT(CHSPEC_CTL_SB(chspec) == WL_CHANSPEC_CTL_SB_LOWER);
+			
+			ctl_chan = LOWER_20_SB(CHSPEC_CHANNEL(chspec));
+		}
+	}
+
+	return ctl_chan;
+}
+
+chanspec_t
+wf_chspec_ctlchspec(chanspec_t chspec)
+{
+	chanspec_t ctl_chspec = 0;
+	uint8 channel;
+
+	ASSERT(!wf_chspec_malformed(chspec));
+
+	
+	if (CHSPEC_CTL_SB(chspec) == WL_CHANSPEC_CTL_SB_NONE) {
+		return chspec;
+	} else {
+		if (CHSPEC_CTL_SB(chspec) == WL_CHANSPEC_CTL_SB_UPPER) {
+			channel = UPPER_20_SB(CHSPEC_CHANNEL(chspec));
+		} else {
+			channel = LOWER_20_SB(CHSPEC_CHANNEL(chspec));
+		}
+		ctl_chspec = channel | WL_CHANSPEC_BW_20 | WL_CHANSPEC_CTL_SB_NONE;
+		ctl_chspec |= CHSPEC_BAND(chspec);
+	}
+	return ctl_chspec;
+}
+
+
+int
+wf_mhz2channel(uint freq, uint start_factor)
+{
+	int ch = -1;
+	uint base;
+	int offset;
+
+	
+	if (start_factor == 0) {
+		if (freq >= 2400 && freq <= 2500)
+			start_factor = WF_CHAN_FACTOR_2_4_G;
+		else if (freq >= 5000 && freq <= 6000)
+			start_factor = WF_CHAN_FACTOR_5_G;
+	}
+
+	if (freq == 2484 && start_factor == WF_CHAN_FACTOR_2_4_G)
+		return 14;
+
+	base = start_factor / 2;
+
+	
+	if ((freq < base) || (freq > base + 1000))
+		return -1;
+
+	offset = freq - base;
+	ch = offset / 5;
+
+	
+	if (offset != (ch * 5))
+		return -1;
+
+	
+	if (start_factor == WF_CHAN_FACTOR_2_4_G && (ch < 1 || ch > 13))
+		return -1;
+
+	return ch;
+}
+
+
+int
+wf_channel2mhz(uint ch, uint start_factor)
+{
+	int freq;
+
+	if ((start_factor == WF_CHAN_FACTOR_2_4_G && (ch < 1 || ch > 14)) ||
+	    (ch > 200))
+		freq = -1;
+	else if ((start_factor == WF_CHAN_FACTOR_2_4_G) && (ch == 14))
+		freq = 2484;
+	else
+		freq = ch * 5 + start_factor / 2;
+
+	return freq;
+}
diff --git a/drivers/net/wireless/bcmdhd/dhd.h b/drivers/net/wireless/bcmdhd/dhd.h
new file mode 100644
index 000000000000..871c2bfb51d2
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd.h
@@ -0,0 +1,758 @@
+/*
+ * Header file describing the internal (inter-module) DHD interfaces.
+ *
+ * Provides type definitions and function prototypes used to link the
+ * DHD OS, bus, and protocol modules.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd.h 333052 2012-05-12 02:09:28Z $
+ */
+
+/****************
+ * Common types *
+ */
+
+#ifndef _dhd_h_
+#define _dhd_h_
+
+#if defined(CHROMIUMOS_COMPAT_WIRELESS)
+#include <linux/sched.h>
+#endif
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/random.h>
+#include <linux/spinlock.h>
+#include <linux/ethtool.h>
+#include <asm/uaccess.h>
+#include <asm/unaligned.h>
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_HAS_WAKELOCK)
+#include <linux/wakelock.h>
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined (CONFIG_HAS_WAKELOCK) */
+/* The kernel threading is sdio-specific */
+struct task_struct;
+struct sched_param;
+int setScheduler(struct task_struct *p, int policy, struct sched_param *param);
+
+#define ALL_INTERFACES	0xff
+
+#include <wlioctl.h>
+
+
+/* Forward decls */
+struct dhd_bus;
+struct dhd_prot;
+struct dhd_info;
+struct dhd_cmn;
+
+/* The level of bus communication with the dongle */
+enum dhd_bus_state {
+	DHD_BUS_DOWN,		/* Not ready for frame transfers */
+	DHD_BUS_LOAD,		/* Download access only (CPU reset) */
+	DHD_BUS_DATA		/* Ready for frame transfers */
+};
+
+/* Firmware requested operation mode */
+#define STA_MASK			0x0001
+#define HOSTAPD_MASK		0x0002
+#define WFD_MASK			0x0004
+#define SOFTAP_FW_MASK	0x0008
+#define P2P_GO_ENABLED		0x0010
+#define P2P_GC_ENABLED		0x0020
+#define CONCURENT_MASK		0x00F0
+
+#define MANUFACTRING_FW 	"WLTEST"
+
+/* max sequential rxcntl timeouts to set HANG event */
+#define MAX_CNTL_TIMEOUT  2
+
+#define DHD_SCAN_ACTIVE_TIME	 40 /* ms : Embedded default Active setting from DHD Driver */
+#define DHD_SCAN_PASSIVE_TIME	130 /* ms: Embedded default Passive setting from DHD Driver */
+
+#define DHD_BEACON_TIMEOUT_NORMAL	4
+#define DHD_BEACON_TIMEOUT_HIGH		10
+
+enum dhd_bus_wake_state {
+	WAKE_LOCK_OFF,
+	WAKE_LOCK_PRIV,
+	WAKE_LOCK_DPC,
+	WAKE_LOCK_IOCTL,
+	WAKE_LOCK_DOWNLOAD,
+	WAKE_LOCK_TMOUT,
+	WAKE_LOCK_WATCHDOG,
+	WAKE_LOCK_LINK_DOWN_TMOUT,
+	WAKE_LOCK_PNO_FIND_TMOUT,
+	WAKE_LOCK_SOFTAP_SET,
+	WAKE_LOCK_SOFTAP_STOP,
+	WAKE_LOCK_SOFTAP_START,
+	WAKE_LOCK_SOFTAP_THREAD,
+	WAKE_LOCK_MAX
+};
+
+enum dhd_prealloc_index {
+	DHD_PREALLOC_PROT = 0,
+	DHD_PREALLOC_RXBUF,
+	DHD_PREALLOC_DATABUF,
+	DHD_PREALLOC_OSL_BUF
+};
+
+typedef enum  {
+	DHD_IF_NONE = 0,
+	DHD_IF_ADD,
+	DHD_IF_DEL,
+	DHD_IF_CHANGE,
+	DHD_IF_DELETING
+} dhd_if_state_t;
+
+#if defined(CONFIG_DHD_USE_STATIC_BUF)
+
+uint8* dhd_os_prealloc(void *osh, int section, uint size);
+void dhd_os_prefree(void *osh, void *addr, uint size);
+#define DHD_OS_PREALLOC(osh, section, size) dhd_os_prealloc(osh, section, size)
+#define DHD_OS_PREFREE(osh, addr, size) dhd_os_prefree(osh, addr, size)
+
+#else
+
+#define DHD_OS_PREALLOC(osh, section, size) MALLOC(osh, size)
+#define DHD_OS_PREFREE(osh, addr, size) MFREE(osh, addr, size)
+
+#endif /* defined(CONFIG_DHD_USE_STATIC_BUF) */
+
+/* Packet alignment for most efficient SDIO (can change based on platform) */
+#ifndef DHD_SDALIGN
+#define DHD_SDALIGN	32
+#endif
+
+/* Common structure for module and instance linkage */
+typedef struct dhd_pub {
+	/* Linkage ponters */
+	osl_t *osh;		/* OSL handle */
+	struct dhd_bus *bus;	/* Bus module handle */
+	struct dhd_prot *prot;	/* Protocol module handle */
+	struct dhd_info  *info; /* Info module handle */
+	struct dhd_cmn	*cmn;	/* dhd_common module handle */
+
+	/* Internal dhd items */
+	bool up;		/* Driver up/down (to OS) */
+	bool txoff;		/* Transmit flow-controlled */
+	bool dongle_reset;  /* TRUE = DEVRESET put dongle into reset */
+	enum dhd_bus_state busstate;
+	uint hdrlen;		/* Total DHD header length (proto + bus) */
+	uint maxctl;		/* Max size rxctl request from proto to bus */
+	uint rxsz;		/* Rx buffer size bus module should use */
+	uint8 wme_dp;	/* wme discard priority */
+
+	/* Dongle media info */
+	bool iswl;		/* Dongle-resident driver is wl */
+	ulong drv_version;	/* Version of dongle-resident driver */
+	struct ether_addr mac;	/* MAC address obtained from dongle */
+	dngl_stats_t dstats;	/* Stats for dongle-based data */
+
+	/* Additional stats for the bus level */
+	ulong tx_packets;	/* Data packets sent to dongle */
+	ulong tx_multicast;	/* Multicast data packets sent to dongle */
+	ulong tx_errors;	/* Errors in sending data to dongle */
+	ulong tx_ctlpkts;	/* Control packets sent to dongle */
+	ulong tx_ctlerrs;	/* Errors sending control frames to dongle */
+	ulong rx_packets;	/* Packets sent up the network interface */
+	ulong rx_multicast;	/* Multicast packets sent up the network interface */
+	ulong rx_errors;	/* Errors processing rx data packets */
+	ulong rx_ctlpkts;	/* Control frames processed from dongle */
+	ulong rx_ctlerrs;	/* Errors in processing rx control frames */
+	ulong rx_dropped;	/* Packets dropped locally (no memory) */
+	ulong rx_flushed;  /* Packets flushed due to unscheduled sendup thread */
+	ulong wd_dpc_sched;   /* Number of times dhd dpc scheduled by watchdog timer */
+
+	ulong rx_readahead_cnt;	/* Number of packets where header read-ahead was used. */
+	ulong tx_realloc;	/* Number of tx packets we had to realloc for headroom */
+	ulong fc_packets;       /* Number of flow control pkts recvd */
+
+	/* Last error return */
+	int bcmerror;
+	uint tickcnt;
+
+	/* Last error from dongle */
+	int dongle_error;
+
+	/* Suspend disable flag and "in suspend" flag */
+	int suspend_disable_flag; /* "1" to disable all extra powersaving during suspend */
+	int in_suspend;			/* flag set to 1 when early suspend called */
+#ifdef PNO_SUPPORT
+	int pno_enable;                 /* pno status : "1" is pno enable */
+#endif /* PNO_SUPPORT */
+	int dtim_skip;         /* dtim skip , default 0 means wake each dtim */
+
+	/* Pkt filter defination */
+	char * pktfilter[100];
+	int pktfilter_count;
+
+	wl_country_t dhd_cspec;		/* Current Locale info */
+	char eventmask[WL_EVENTING_MASK_LEN];
+	int	op_mode;				/* STA, HostAPD, WFD, SoftAP */
+
+/* Set this to 1 to use a seperate interface (p2p0) for p2p operations.
+ *  For ICS MR1 releases it should be disable to be compatable with ICS MR1 Framework
+ *  see target dhd-cdc-sdmmc-panda-cfg80211-icsmr1-gpl-debug in Makefile
+ */
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25)) && 1
+	struct mutex 	wl_start_stop_lock; /* lock/unlock for Android start/stop */
+	struct mutex 	wl_softap_lock;		 /* lock/unlock for any SoftAP/STA settings */
+#endif 
+
+	uint16	maxdatablks;
+#ifdef PROP_TXSTATUS
+	int   wlfc_enabled;
+	void* wlfc_state;
+#endif
+	bool	dongle_isolation;
+	int   hang_was_sent;
+	int   rxcnt_timeout;		/* counter rxcnt timeout to send HANG */
+	int   txcnt_timeout;		/* counter txcnt timeout to send HANG */
+#ifdef WLMEDIA_HTSF
+	uint8 htsfdlystat_sz; /* Size of delay stats, max 255B */
+#endif
+} dhd_pub_t;
+
+typedef struct dhd_cmn {
+	osl_t *osh;		/* OSL handle */
+	dhd_pub_t *dhd;
+} dhd_cmn_t;
+
+
+	#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP)
+
+	#define DHD_PM_RESUME_WAIT_INIT(a) DECLARE_WAIT_QUEUE_HEAD(a);
+	#define _DHD_PM_RESUME_WAIT(a, b) do {\
+			int retry = 0; \
+			SMP_RD_BARRIER_DEPENDS(); \
+			while (dhd_mmc_suspend && retry++ != b) { \
+				SMP_RD_BARRIER_DEPENDS(); \
+				wait_event_interruptible_timeout(a, !dhd_mmc_suspend, HZ/100); \
+			} \
+		} while (0)
+	#define DHD_PM_RESUME_WAIT(a) 		_DHD_PM_RESUME_WAIT(a, 200)
+	#define DHD_PM_RESUME_WAIT_FOREVER(a) 	_DHD_PM_RESUME_WAIT(a, ~0)
+	#define DHD_PM_RESUME_RETURN_ERROR(a)	do { if (dhd_mmc_suspend) return a; } while (0)
+	#define DHD_PM_RESUME_RETURN		do { if (dhd_mmc_suspend) return; } while (0)
+
+	#define DHD_SPINWAIT_SLEEP_INIT(a) DECLARE_WAIT_QUEUE_HEAD(a);
+	#define SPINWAIT_SLEEP(a, exp, us) do { \
+		uint countdown = (us) + 9999; \
+		while ((exp) && (countdown >= 10000)) { \
+			wait_event_interruptible_timeout(a, FALSE, HZ/100); \
+			countdown -= 10000; \
+		} \
+	} while (0)
+
+	#else
+
+	#define DHD_PM_RESUME_WAIT_INIT(a)
+	#define DHD_PM_RESUME_WAIT(a)
+	#define DHD_PM_RESUME_WAIT_FOREVER(a)
+	#define DHD_PM_RESUME_RETURN_ERROR(a)
+	#define DHD_PM_RESUME_RETURN
+
+	#define DHD_SPINWAIT_SLEEP_INIT(a)
+	#define SPINWAIT_SLEEP(a, exp, us)  do { \
+		uint countdown = (us) + 9; \
+		while ((exp) && (countdown >= 10)) { \
+			OSL_DELAY(10);  \
+			countdown -= 10;  \
+		} \
+	} while (0)
+
+	#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) */
+#ifndef DHDTHREAD
+#undef	SPINWAIT_SLEEP
+#define SPINWAIT_SLEEP(a, exp, us) SPINWAIT(exp, us)
+#endif /* DHDTHREAD */
+#define DHD_IF_VIF	0x01	/* Virtual IF (Hidden from user) */
+
+unsigned long dhd_os_spin_lock(dhd_pub_t *pub);
+void dhd_os_spin_unlock(dhd_pub_t *pub, unsigned long flags);
+
+/*  Wakelock Functions */
+extern int dhd_os_wake_lock(dhd_pub_t *pub);
+extern int dhd_os_wake_unlock(dhd_pub_t *pub);
+extern int dhd_os_wake_lock_timeout(dhd_pub_t *pub);
+extern int dhd_os_wake_lock_rx_timeout_enable(dhd_pub_t *pub, int val);
+extern int dhd_os_wake_lock_ctrl_timeout_enable(dhd_pub_t *pub, int val);
+
+inline static void MUTEX_LOCK_SOFTAP_SET_INIT(dhd_pub_t * dhdp)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25)) && 1
+	mutex_init(&dhdp->wl_softap_lock);
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
+}
+
+inline static void MUTEX_LOCK_SOFTAP_SET(dhd_pub_t * dhdp)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25)) && 1
+	mutex_lock(&dhdp->wl_softap_lock);
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
+}
+
+inline static void MUTEX_UNLOCK_SOFTAP_SET(dhd_pub_t * dhdp)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25)) && 1
+	mutex_unlock(&dhdp->wl_softap_lock);
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
+}
+
+#define DHD_OS_WAKE_LOCK(pub) 			dhd_os_wake_lock(pub)
+#define DHD_OS_WAKE_UNLOCK(pub) 		dhd_os_wake_unlock(pub)
+#define DHD_OS_WAKE_LOCK_TIMEOUT(pub)		dhd_os_wake_lock_timeout(pub)
+#define DHD_OS_WAKE_LOCK_RX_TIMEOUT_ENABLE(pub, val)	dhd_os_wake_lock_rx_timeout_enable(pub, val)
+#define DHD_OS_WAKE_LOCK_CTRL_TIMEOUT_ENABLE(pub, val)	dhd_os_wake_lock_ctrl_timeout_enable(pub, val)
+#define DHD_PACKET_TIMEOUT_MS	1000
+#define DHD_EVENT_TIMEOUT_MS	1500
+
+/* interface operations (register, remove) should be atomic, use this lock to prevent race
+ * condition among wifi on/off and interface operation functions
+ */
+void dhd_net_if_lock(struct net_device *dev);
+void dhd_net_if_unlock(struct net_device *dev);
+
+typedef struct dhd_if_event {
+	uint8 ifidx;
+	uint8 action;
+	uint8 flags;
+	uint8 bssidx;
+	uint8 is_AP;
+} dhd_if_event_t;
+
+typedef enum dhd_attach_states
+{
+	DHD_ATTACH_STATE_INIT = 0x0,
+	DHD_ATTACH_STATE_NET_ALLOC = 0x1,
+	DHD_ATTACH_STATE_DHD_ALLOC = 0x2,
+	DHD_ATTACH_STATE_ADD_IF = 0x4,
+	DHD_ATTACH_STATE_PROT_ATTACH = 0x8,
+	DHD_ATTACH_STATE_WL_ATTACH = 0x10,
+	DHD_ATTACH_STATE_THREADS_CREATED = 0x20,
+	DHD_ATTACH_STATE_WAKELOCKS_INIT = 0x40,
+	DHD_ATTACH_STATE_CFG80211 = 0x80,
+	DHD_ATTACH_STATE_EARLYSUSPEND_DONE = 0x100,
+	DHD_ATTACH_STATE_DONE = 0x200
+} dhd_attach_states_t;
+
+/* Value -1 means we are unsuccessful in creating the kthread. */
+#define DHD_PID_KT_INVALID 	-1
+/* Value -2 means we are unsuccessful in both creating the kthread and tasklet */
+#define DHD_PID_KT_TL_INVALID	-2
+
+/*
+ * Exported from dhd OS modules (dhd_linux/dhd_ndis)
+ */
+
+/* To allow osl_attach/detach calls from os-independent modules */
+osl_t *dhd_osl_attach(void *pdev, uint bustype);
+void dhd_osl_detach(osl_t *osh);
+
+/* Indication from bus module regarding presence/insertion of dongle.
+ * Return dhd_pub_t pointer, used as handle to OS module in later calls.
+ * Returned structure should have bus and prot pointers filled in.
+ * bus_hdrlen specifies required headroom for bus module header.
+ */
+extern dhd_pub_t *dhd_attach(osl_t *osh, struct dhd_bus *bus, uint bus_hdrlen, void *dev);
+extern int dhd_net_attach(dhd_pub_t *dhdp, int idx);
+
+/* Indication from bus module regarding removal/absence of dongle */
+extern void dhd_detach(dhd_pub_t *dhdp);
+#if defined(WLP2P) && defined(WL_CFG80211)
+/* To allow attach/detach calls corresponding to p2p0 interface  */
+extern int dhd_attach_p2p(dhd_pub_t *);
+extern int dhd_detach_p2p(dhd_pub_t *);
+#endif /* WLP2P && WL_CFG80211 */
+
+extern void dhd_free(dhd_pub_t *dhdp);
+
+/* Indication from bus module to change flow-control state */
+extern void dhd_txflowcontrol(dhd_pub_t *dhdp, int ifidx, bool on);
+
+extern bool dhd_prec_enq(dhd_pub_t *dhdp, struct pktq *q, void *pkt, int prec);
+
+/* Receive frame for delivery to OS.  Callee disposes of rxp. */
+extern void dhd_rx_frame(dhd_pub_t *dhdp, int ifidx, void *rxp, int numpkt, uint8 chan);
+
+/* Return pointer to interface name */
+extern char *dhd_ifname(dhd_pub_t *dhdp, int idx);
+
+/* Request scheduling of the bus dpc */
+extern void dhd_sched_dpc(dhd_pub_t *dhdp);
+
+/* Notify tx completion */
+extern void dhd_txcomplete(dhd_pub_t *dhdp, void *txp, bool success);
+
+/* OS independent layer functions */
+extern int dhd_os_proto_block(dhd_pub_t * pub);
+extern int dhd_os_proto_unblock(dhd_pub_t * pub);
+extern int dhd_os_ioctl_resp_wait(dhd_pub_t * pub, uint * condition, bool * pending);
+extern int dhd_os_ioctl_resp_wake(dhd_pub_t * pub);
+extern unsigned int dhd_os_get_ioctl_resp_timeout(void);
+extern void dhd_os_set_ioctl_resp_timeout(unsigned int timeout_msec);
+extern void * dhd_os_open_image(char * filename);
+extern int dhd_os_get_image_block(char * buf, int len, void * image);
+extern void dhd_os_close_image(void * image);
+extern void dhd_os_wd_timer(void *bus, uint wdtick);
+extern void dhd_os_sdlock(dhd_pub_t * pub);
+extern void dhd_os_sdunlock(dhd_pub_t * pub);
+extern void dhd_os_sdlock_txq(dhd_pub_t * pub);
+extern void dhd_os_sdunlock_txq(dhd_pub_t * pub);
+extern void dhd_os_sdlock_rxq(dhd_pub_t * pub);
+extern void dhd_os_sdunlock_rxq(dhd_pub_t * pub);
+extern void dhd_os_sdlock_sndup_rxq(dhd_pub_t * pub);
+extern void dhd_customer_gpio_wlan_ctrl(int onoff);
+extern int dhd_custom_get_mac_address(unsigned char *buf);
+extern void dhd_os_sdunlock_sndup_rxq(dhd_pub_t * pub);
+extern void dhd_os_sdlock_eventq(dhd_pub_t * pub);
+extern void dhd_os_sdunlock_eventq(dhd_pub_t * pub);
+extern bool dhd_os_check_hang(dhd_pub_t *dhdp, int ifidx, int ret);
+
+#ifdef PNO_SUPPORT
+extern int dhd_pno_enable(dhd_pub_t *dhd, int pfn_enabled);
+extern int dhd_pno_clean(dhd_pub_t *dhd);
+extern int dhd_pno_set(dhd_pub_t *dhd, wlc_ssid_t* ssids_local, int nssid,
+                       ushort  scan_fr, int pno_repeat, int pno_freq_expo_max);
+extern int dhd_pno_set_ex(dhd_pub_t *dhd, wl_pfn_t* ssidnet, int nssid,
+				ushort pno_interval, int pno_repeat, int pno_expo_max, int pno_lost_time);
+extern int dhd_pno_get_status(dhd_pub_t *dhd);
+extern int dhd_dev_pno_reset(struct net_device *dev);
+extern int dhd_dev_pno_set(struct net_device *dev, wlc_ssid_t* ssids_local,
+                           int nssid, ushort  scan_fr, int pno_repeat, int pno_freq_expo_max);
+extern int dhd_dev_pno_set_ex(struct net_device *dev, wl_pfn_t* ssidnet, int nssid,
+				ushort	pno_interval, int pno_repeat, int pno_expo_max, int pno_lost_time);
+extern int dhd_dev_pno_enable(struct net_device *dev,  int pfn_enabled);
+extern int dhd_dev_get_pno_status(struct net_device *dev);
+#endif /* PNO_SUPPORT */
+
+#define DHD_UNICAST_FILTER_NUM		0
+#define DHD_BROADCAST_FILTER_NUM	1
+#define DHD_MULTICAST4_FILTER_NUM	2
+#define DHD_MULTICAST6_FILTER_NUM	3
+#define DHD_MDNS_FILTER_NUM		4
+extern int dhd_os_set_packet_filter(dhd_pub_t *dhdp, int val);
+extern int net_os_set_packet_filter(struct net_device *dev, int val);
+extern int net_os_rxfilter_add_remove(struct net_device *dev, int val, int num);
+
+extern int dhd_get_dtim_skip(dhd_pub_t *dhd);
+extern bool dhd_check_ap_wfd_mode_set(dhd_pub_t *dhd);
+
+#ifdef DHD_DEBUG
+extern int write_to_file(dhd_pub_t *dhd, uint8 *buf, int size);
+#endif /* DHD_DEBUG */
+#if defined(OOB_INTR_ONLY)
+extern int dhd_customer_oob_irq_map(unsigned long *irq_flags_ptr);
+#endif /* defined(OOB_INTR_ONLY) */
+extern void dhd_os_sdtxlock(dhd_pub_t * pub);
+extern void dhd_os_sdtxunlock(dhd_pub_t * pub);
+
+typedef struct {
+	uint32 limit;		/* Expiration time (usec) */
+	uint32 increment;	/* Current expiration increment (usec) */
+	uint32 elapsed;		/* Current elapsed time (usec) */
+	uint32 tick;		/* O/S tick time (usec) */
+} dhd_timeout_t;
+
+extern void dhd_timeout_start(dhd_timeout_t *tmo, uint usec);
+extern int dhd_timeout_expired(dhd_timeout_t *tmo);
+
+extern int dhd_ifname2idx(struct dhd_info *dhd, char *name);
+extern int dhd_net2idx(struct dhd_info *dhd, struct net_device *net);
+extern struct net_device * dhd_idx2net(void *pub, int ifidx);
+extern int wl_host_event(dhd_pub_t *dhd_pub, int *idx, void *pktdata,
+                         wl_event_msg_t *, void **data_ptr);
+extern void wl_event_to_host_order(wl_event_msg_t * evt);
+
+extern int dhd_wl_ioctl(dhd_pub_t *dhd_pub, int ifindex, wl_ioctl_t *ioc, void *buf, int len);
+extern int dhd_wl_ioctl_cmd(dhd_pub_t *dhd_pub, int cmd, void *arg, int len, uint8 set,
+                            int ifindex);
+
+extern struct dhd_cmn *dhd_common_init(uint16 devid, osl_t *osh);
+extern void dhd_common_deinit(dhd_pub_t *dhd_pub, dhd_cmn_t *sa_cmn);
+
+extern int dhd_do_driver_init(struct net_device *net);
+extern int dhd_add_if(struct dhd_info *dhd, int ifidx, void *handle,
+	char *name, uint8 *mac_addr, uint32 flags, uint8 bssidx);
+extern void dhd_del_if(struct dhd_info *dhd, int ifidx);
+
+extern void dhd_vif_add(struct dhd_info *dhd, int ifidx, char * name);
+extern void dhd_vif_del(struct dhd_info *dhd, int ifidx);
+
+extern void dhd_event(struct dhd_info *dhd, char *evpkt, int evlen, int ifidx);
+extern void dhd_vif_sendup(struct dhd_info *dhd, int ifidx, uchar *cp, int len);
+
+
+/* Send packet to dongle via data channel */
+extern int dhd_sendpkt(dhd_pub_t *dhdp, int ifidx, void *pkt);
+
+/* send up locally generated event */
+extern void dhd_sendup_event_common(dhd_pub_t *dhdp, wl_event_msg_t *event, void *data);
+/* Send event to host */
+extern void dhd_sendup_event(dhd_pub_t *dhdp, wl_event_msg_t *event, void *data);
+extern int dhd_bus_devreset(dhd_pub_t *dhdp, uint8 flag);
+extern uint dhd_bus_status(dhd_pub_t *dhdp);
+extern int  dhd_bus_start(dhd_pub_t *dhdp);
+extern int dhd_bus_membytes(dhd_pub_t *dhdp, bool set, uint32 address, uint8 *data, uint size);
+extern void dhd_print_buf(void *pbuf, int len, int bytes_per_line);
+extern bool dhd_is_associated(dhd_pub_t *dhd, void *bss_buf, int *retval);
+extern uint dhd_bus_chip_id(dhd_pub_t *dhdp);
+
+#if defined(KEEP_ALIVE)
+extern int dhd_keep_alive_onoff(dhd_pub_t *dhd);
+#endif /* KEEP_ALIVE */
+
+#ifdef ARP_OFFLOAD_SUPPORT
+extern void dhd_arp_offload_set(dhd_pub_t * dhd, int arp_mode);
+extern void dhd_arp_offload_enable(dhd_pub_t * dhd, int arp_enable);
+#endif /* ARP_OFFLOAD_SUPPORT */
+
+
+typedef enum cust_gpio_modes {
+	WLAN_RESET_ON,
+	WLAN_RESET_OFF,
+	WLAN_POWER_ON,
+	WLAN_POWER_OFF
+} cust_gpio_modes_t;
+
+extern int wl_iw_iscan_set_scan_broadcast_prep(struct net_device *dev, uint flag);
+extern int wl_iw_send_priv_event(struct net_device *dev, char *flag);
+/*
+ * Insmod parameters for debug/test
+ */
+
+/* Watchdog timer interval */
+extern uint dhd_watchdog_ms;
+
+#if defined(DHD_DEBUG)
+/* Console output poll interval */
+extern uint dhd_console_ms;
+extern uint wl_msg_level;
+#endif /* defined(DHD_DEBUG) */
+
+/* Use interrupts */
+extern uint dhd_intr;
+
+/* Use polling */
+extern uint dhd_poll;
+
+/* ARP offload agent mode */
+extern uint dhd_arp_mode;
+
+/* ARP offload enable */
+extern uint dhd_arp_enable;
+
+/* Pkt filte enable control */
+extern uint dhd_pkt_filter_enable;
+
+/*  Pkt filter init setup */
+extern uint dhd_pkt_filter_init;
+
+/* Pkt filter mode control */
+extern uint dhd_master_mode;
+
+/* Roaming mode control */
+extern uint dhd_roam_disable;
+
+/* Roaming mode control */
+extern uint dhd_radio_up;
+
+/* Initial idletime ticks (may be -1 for immediate idle, 0 for no idle) */
+extern int dhd_idletime;
+#define DHD_IDLETIME_TICKS 1
+
+/* SDIO Drive Strength */
+extern uint dhd_sdiod_drive_strength;
+
+/* Override to force tx queueing all the time */
+extern uint dhd_force_tx_queueing;
+/* Default KEEP_ALIVE Period is 55 sec to prevent AP from sending Keep Alive probe frame */
+#define KEEP_ALIVE_PERIOD 55000
+#define NULL_PKT_STR	"null_pkt"
+
+#ifdef SDTEST
+/* Echo packet generator (SDIO), pkts/s */
+extern uint dhd_pktgen;
+
+/* Echo packet len (0 => sawtooth, max 1800) */
+extern uint dhd_pktgen_len;
+#define MAX_PKTGEN_LEN 1800
+#endif
+
+
+/* optionally set by a module_param_string() */
+#define MOD_PARAM_PATHLEN	2048
+extern char fw_path[MOD_PARAM_PATHLEN];
+extern char nv_path[MOD_PARAM_PATHLEN];
+
+#ifdef SOFTAP
+extern char fw_path2[MOD_PARAM_PATHLEN];
+#endif
+
+/* Flag to indicate if we should download firmware on driver load */
+extern uint dhd_download_fw_on_driverload;
+
+/* For supporting multiple interfaces */
+#define DHD_MAX_IFS	16
+#define DHD_DEL_IF	-0xe
+#define DHD_BAD_IF	-0xf
+
+#ifdef PROP_TXSTATUS
+/* Please be mindful that total pkttag space is 32 octets only */
+typedef struct dhd_pkttag {
+	/*
+	b[11 ] - 1 = this packet was sent in response to one time packet request,
+	do not increment credit on status for this one. [WLFC_CTL_TYPE_MAC_REQUEST_PACKET].
+	b[10 ] - 1 = signal-only-packet to firmware [i.e. nothing to piggyback on]
+	b[9  ] - 1 = packet is host->firmware (transmit direction)
+	       - 0 = packet received from firmware (firmware->host)
+	b[8  ] - 1 = packet was sent due to credit_request (pspoll),
+	             packet does not count against FIFO credit.
+	       - 0 = normal transaction, packet counts against FIFO credit
+	b[7  ] - 1 = AP, 0 = STA
+	b[6:4] - AC FIFO number
+	b[3:0] - interface index
+	*/
+	uint16	if_flags;
+	/* destination MAC address for this packet so that not every
+	module needs to open the packet to find this
+	*/
+	uint8	dstn_ether[ETHER_ADDR_LEN];
+	/*
+	This 32-bit goes from host to device for every packet.
+	*/
+	uint32	htod_tag;
+	/* bus specific stuff */
+	union {
+		struct {
+			void* stuff;
+			uint32 thing1;
+			uint32 thing2;
+		} sd;
+		struct {
+			void* bus;
+			void* urb;
+		} usb;
+	} bus_specific;
+} dhd_pkttag_t;
+
+#define DHD_PKTTAG_SET_H2DTAG(tag, h2dvalue)	((dhd_pkttag_t*)(tag))->htod_tag = (h2dvalue)
+#define DHD_PKTTAG_H2DTAG(tag)					(((dhd_pkttag_t*)(tag))->htod_tag)
+
+#define DHD_PKTTAG_IFMASK		0xf
+#define DHD_PKTTAG_IFTYPE_MASK	0x1
+#define DHD_PKTTAG_IFTYPE_SHIFT	7
+#define DHD_PKTTAG_FIFO_MASK	0x7
+#define DHD_PKTTAG_FIFO_SHIFT	4
+
+#define DHD_PKTTAG_SIGNALONLY_MASK			0x1
+#define DHD_PKTTAG_SIGNALONLY_SHIFT			10
+
+#define DHD_PKTTAG_ONETIMEPKTRQST_MASK		0x1
+#define DHD_PKTTAG_ONETIMEPKTRQST_SHIFT		11
+
+#define DHD_PKTTAG_PKTDIR_MASK			0x1
+#define DHD_PKTTAG_PKTDIR_SHIFT			9
+
+#define DHD_PKTTAG_CREDITCHECK_MASK		0x1
+#define DHD_PKTTAG_CREDITCHECK_SHIFT	8
+
+#define DHD_PKTTAG_INVALID_FIFOID 0x7
+
+#define DHD_PKTTAG_SETFIFO(tag, fifo)	((dhd_pkttag_t*)(tag))->if_flags = \
+	(((dhd_pkttag_t*)(tag))->if_flags & ~(DHD_PKTTAG_FIFO_MASK << DHD_PKTTAG_FIFO_SHIFT)) | \
+	(((fifo) & DHD_PKTTAG_FIFO_MASK) << DHD_PKTTAG_FIFO_SHIFT)
+#define DHD_PKTTAG_FIFO(tag)			((((dhd_pkttag_t*)(tag))->if_flags >> \
+	DHD_PKTTAG_FIFO_SHIFT) & DHD_PKTTAG_FIFO_MASK)
+
+#define DHD_PKTTAG_SETIF(tag, if)	((dhd_pkttag_t*)(tag))->if_flags = \
+	(((dhd_pkttag_t*)(tag))->if_flags & ~DHD_PKTTAG_IFMASK) | ((if) & DHD_PKTTAG_IFMASK)
+#define DHD_PKTTAG_IF(tag)	(((dhd_pkttag_t*)(tag))->if_flags & DHD_PKTTAG_IFMASK)
+
+#define DHD_PKTTAG_SETIFTYPE(tag, isAP)	((dhd_pkttag_t*)(tag))->if_flags = \
+	(((dhd_pkttag_t*)(tag))->if_flags & \
+	~(DHD_PKTTAG_IFTYPE_MASK << DHD_PKTTAG_IFTYPE_SHIFT)) | \
+	(((isAP) & DHD_PKTTAG_IFTYPE_MASK) << DHD_PKTTAG_IFTYPE_SHIFT)
+#define DHD_PKTTAG_IFTYPE(tag)	((((dhd_pkttag_t*)(tag))->if_flags >> \
+	DHD_PKTTAG_IFTYPE_SHIFT) & DHD_PKTTAG_IFTYPE_MASK)
+
+#define DHD_PKTTAG_SETCREDITCHECK(tag, check)	((dhd_pkttag_t*)(tag))->if_flags = \
+	(((dhd_pkttag_t*)(tag))->if_flags & \
+	~(DHD_PKTTAG_CREDITCHECK_MASK << DHD_PKTTAG_CREDITCHECK_SHIFT)) | \
+	(((check) & DHD_PKTTAG_CREDITCHECK_MASK) << DHD_PKTTAG_CREDITCHECK_SHIFT)
+#define DHD_PKTTAG_CREDITCHECK(tag)	((((dhd_pkttag_t*)(tag))->if_flags >> \
+	DHD_PKTTAG_CREDITCHECK_SHIFT) & DHD_PKTTAG_CREDITCHECK_MASK)
+
+#define DHD_PKTTAG_SETPKTDIR(tag, dir)	((dhd_pkttag_t*)(tag))->if_flags = \
+	(((dhd_pkttag_t*)(tag))->if_flags & \
+	~(DHD_PKTTAG_PKTDIR_MASK << DHD_PKTTAG_PKTDIR_SHIFT)) | \
+	(((dir) & DHD_PKTTAG_PKTDIR_MASK) << DHD_PKTTAG_PKTDIR_SHIFT)
+#define DHD_PKTTAG_PKTDIR(tag)	((((dhd_pkttag_t*)(tag))->if_flags >> \
+	DHD_PKTTAG_PKTDIR_SHIFT) & DHD_PKTTAG_PKTDIR_MASK)
+
+#define DHD_PKTTAG_SETSIGNALONLY(tag, signalonly)	((dhd_pkttag_t*)(tag))->if_flags = \
+	(((dhd_pkttag_t*)(tag))->if_flags & \
+	~(DHD_PKTTAG_SIGNALONLY_MASK << DHD_PKTTAG_SIGNALONLY_SHIFT)) | \
+	(((signalonly) & DHD_PKTTAG_SIGNALONLY_MASK) << DHD_PKTTAG_SIGNALONLY_SHIFT)
+#define DHD_PKTTAG_SIGNALONLY(tag)	((((dhd_pkttag_t*)(tag))->if_flags >> \
+	DHD_PKTTAG_SIGNALONLY_SHIFT) & DHD_PKTTAG_SIGNALONLY_MASK)
+
+#define DHD_PKTTAG_SETONETIMEPKTRQST(tag)	((dhd_pkttag_t*)(tag))->if_flags = \
+	(((dhd_pkttag_t*)(tag))->if_flags & \
+	~(DHD_PKTTAG_ONETIMEPKTRQST_MASK << DHD_PKTTAG_ONETIMEPKTRQST_SHIFT)) | \
+	(1 << DHD_PKTTAG_ONETIMEPKTRQST_SHIFT)
+#define DHD_PKTTAG_ONETIMEPKTRQST(tag)	((((dhd_pkttag_t*)(tag))->if_flags >> \
+	DHD_PKTTAG_ONETIMEPKTRQST_SHIFT) & DHD_PKTTAG_ONETIMEPKTRQST_MASK)
+
+#define DHD_PKTTAG_SETDSTN(tag, dstn_MAC_ea)	memcpy(((dhd_pkttag_t*)((tag)))->dstn_ether, \
+	(dstn_MAC_ea), ETHER_ADDR_LEN)
+#define DHD_PKTTAG_DSTN(tag)	((dhd_pkttag_t*)(tag))->dstn_ether
+
+typedef int (*f_commitpkt_t)(void* ctx, void* p);
+
+#ifdef PROP_TXSTATUS_DEBUG
+#define DHD_WLFC_CTRINC_MAC_CLOSE(entry)	do { (entry)->closed_ct++; } while (0)
+#define DHD_WLFC_CTRINC_MAC_OPEN(entry)		do { (entry)->opened_ct++; } while (0)
+#else
+#define DHD_WLFC_CTRINC_MAC_CLOSE(entry)	do {} while (0)
+#define DHD_WLFC_CTRINC_MAC_OPEN(entry)		do {} while (0)
+#endif
+
+#endif /* PROP_TXSTATUS */
+
+extern void dhd_wait_for_event(dhd_pub_t *dhd, bool *lockvar);
+extern void dhd_wait_event_wakeup(dhd_pub_t*dhd);
+
+#ifdef ARP_OFFLOAD_SUPPORT
+#define MAX_IPV4_ENTRIES	8
+/* dhd_commn arp offload wrapers */
+void dhd_aoe_hostip_clr(dhd_pub_t *dhd);
+void dhd_aoe_arp_clr(dhd_pub_t *dhd);
+int dhd_arp_get_arp_hostip_table(dhd_pub_t *dhd, void *buf, int buflen);
+void dhd_arp_offload_add_ip(dhd_pub_t *dhd, uint32 ipaddr);
+#endif /* ARP_OFFLOAD_SUPPORT */
+
+#endif /* _dhd_h_ */
diff --git a/drivers/net/wireless/bcmdhd/dhd_bta.c b/drivers/net/wireless/bcmdhd/dhd_bta.c
new file mode 100644
index 000000000000..6b782ea4a4d2
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd_bta.c
@@ -0,0 +1,335 @@
+/*
+ * BT-AMP support routines
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_bta.c,v 1.10.4.2 2010-12-22 23:47:23 Exp $
+ */
+
+#include <typedefs.h>
+#include <osl.h>
+#include <bcmcdc.h>
+#include <bcmutils.h>
+#include <bcmendian.h>
+#include <proto/802.11.h>
+#include <proto/802.11_bta.h>
+#include <proto/bt_amp_hci.h>
+#include <dngl_stats.h>
+#include <dhd.h>
+#include <dhd_bus.h>
+#include <dhd_proto.h>
+#include <dhdioctl.h>
+#include <dhd_dbg.h>
+
+#include <dhd_bta.h>
+
+
+#ifdef SEND_HCI_CMD_VIA_IOCTL
+#define BTA_HCI_CMD_MAX_LEN HCI_CMD_PREAMBLE_SIZE + HCI_CMD_DATA_SIZE
+
+/* Send HCI cmd via wl iovar HCI_cmd to the dongle. */
+int
+dhd_bta_docmd(dhd_pub_t *pub, void *cmd_buf, uint cmd_len)
+{
+	amp_hci_cmd_t *cmd = (amp_hci_cmd_t *)cmd_buf;
+	uint8 buf[BTA_HCI_CMD_MAX_LEN + 16];
+	uint len = sizeof(buf);
+	wl_ioctl_t ioc;
+
+	if (cmd_len < HCI_CMD_PREAMBLE_SIZE)
+		return BCME_BADLEN;
+
+	if ((uint)cmd->plen + HCI_CMD_PREAMBLE_SIZE > cmd_len)
+		return BCME_BADLEN;
+
+	len = bcm_mkiovar("HCI_cmd",
+		(char *)cmd, (uint)cmd->plen + HCI_CMD_PREAMBLE_SIZE, (char *)buf, len);
+
+
+	memset(&ioc, 0, sizeof(ioc));
+
+	ioc.cmd = WLC_SET_VAR;
+	ioc.buf = buf;
+	ioc.len = len;
+	ioc.set = TRUE;
+
+	return dhd_wl_ioctl(pub, &ioc, ioc.buf, ioc.len);
+}
+#else /* !SEND_HCI_CMD_VIA_IOCTL */
+
+static void
+dhd_bta_flush_hcidata(dhd_pub_t *pub, uint16 llh)
+{
+	int prec;
+	struct pktq *q;
+	uint count = 0;
+
+	q = dhd_bus_txq(pub->bus);
+	if (q == NULL)
+		return;
+
+	DHD_BTA(("dhd: flushing HCI ACL data for logical link %u...\n", llh));
+
+	dhd_os_sdlock_txq(pub);
+
+	/* Walk through the txq and toss all HCI ACL data packets */
+	PKTQ_PREC_ITER(q, prec) {
+		void *head_pkt = NULL;
+
+		while (pktq_ppeek(q, prec) != head_pkt) {
+			void *pkt = pktq_pdeq(q, prec);
+			int ifidx;
+
+			PKTPULL(pub->osh, pkt, dhd_bus_hdrlen(pub->bus));
+			dhd_prot_hdrpull(pub, &ifidx, pkt);
+
+			if (PKTLEN(pub->osh, pkt) >= RFC1042_HDR_LEN) {
+				struct ether_header *eh =
+				        (struct ether_header *)PKTDATA(pub->osh, pkt);
+
+				if (ntoh16(eh->ether_type) < ETHER_TYPE_MIN) {
+					struct dot11_llc_snap_header *lsh =
+					        (struct dot11_llc_snap_header *)&eh[1];
+
+					if (bcmp(lsh, BT_SIG_SNAP_MPROT,
+					         DOT11_LLC_SNAP_HDR_LEN - 2) == 0 &&
+					    ntoh16(lsh->type) == BTA_PROT_L2CAP) {
+						amp_hci_ACL_data_t *ACL_data =
+						        (amp_hci_ACL_data_t *)&lsh[1];
+						uint16 handle = ltoh16(ACL_data->handle);
+
+						if (HCI_ACL_DATA_HANDLE(handle) == llh) {
+							PKTFREE(pub->osh, pkt, TRUE);
+							count ++;
+							continue;
+						}
+					}
+				}
+			}
+
+			dhd_prot_hdrpush(pub, ifidx, pkt);
+			PKTPUSH(pub->osh, pkt, dhd_bus_hdrlen(pub->bus));
+
+			if (head_pkt == NULL)
+				head_pkt = pkt;
+			pktq_penq(q, prec, pkt);
+		}
+	}
+
+	dhd_os_sdunlock_txq(pub);
+
+	DHD_BTA(("dhd: flushed %u packet(s) for logical link %u...\n", count, llh));
+}
+
+/* Handle HCI cmd locally.
+ * Return 0: continue to send the cmd across SDIO
+ *        < 0: stop, fail
+ *        > 0: stop, succuess
+ */
+static int
+_dhd_bta_docmd(dhd_pub_t *pub, amp_hci_cmd_t *cmd)
+{
+	int status = 0;
+
+	switch (ltoh16_ua((uint8 *)&cmd->opcode)) {
+	case HCI_Enhanced_Flush: {
+		eflush_cmd_parms_t *cmdparms = (eflush_cmd_parms_t *)cmd->parms;
+		dhd_bta_flush_hcidata(pub, ltoh16_ua(cmdparms->llh));
+		break;
+	}
+	default:
+		break;
+	}
+
+	return status;
+}
+
+/* Send HCI cmd encapsulated in BT-SIG frame via data channel to the dongle. */
+int
+dhd_bta_docmd(dhd_pub_t *pub, void *cmd_buf, uint cmd_len)
+{
+	amp_hci_cmd_t *cmd = (amp_hci_cmd_t *)cmd_buf;
+	struct ether_header *eh;
+	struct dot11_llc_snap_header *lsh;
+	osl_t *osh = pub->osh;
+	uint len;
+	void *p;
+	int status;
+
+	if (cmd_len < HCI_CMD_PREAMBLE_SIZE) {
+		DHD_ERROR(("dhd_bta_docmd: short command, cmd_len %u\n", cmd_len));
+		return BCME_BADLEN;
+	}
+
+	if ((len = (uint)cmd->plen + HCI_CMD_PREAMBLE_SIZE) > cmd_len) {
+		DHD_ERROR(("dhd_bta_docmd: malformed command, len %u cmd_len %u\n",
+		           len, cmd_len));
+		/* return BCME_BADLEN; */
+	}
+
+	p = PKTGET(osh, pub->hdrlen + RFC1042_HDR_LEN + len, TRUE);
+	if (p == NULL) {
+		DHD_ERROR(("dhd_bta_docmd: out of memory\n"));
+		return BCME_NOMEM;
+	}
+
+
+	/* intercept and handle the HCI cmd locally */
+	if ((status = _dhd_bta_docmd(pub, cmd)) > 0)
+		return 0;
+	else if (status < 0)
+		return status;
+
+	/* copy in HCI cmd */
+	PKTPULL(osh, p, pub->hdrlen + RFC1042_HDR_LEN);
+	bcopy(cmd, PKTDATA(osh, p), len);
+
+	/* copy in partial Ethernet header with BT-SIG LLC/SNAP header */
+	PKTPUSH(osh, p, RFC1042_HDR_LEN);
+	eh = (struct ether_header *)PKTDATA(osh, p);
+	bzero(eh->ether_dhost, ETHER_ADDR_LEN);
+	ETHER_SET_LOCALADDR(eh->ether_dhost);
+	bcopy(&pub->mac, eh->ether_shost, ETHER_ADDR_LEN);
+	eh->ether_type = hton16(len + DOT11_LLC_SNAP_HDR_LEN);
+	lsh = (struct dot11_llc_snap_header *)&eh[1];
+	bcopy(BT_SIG_SNAP_MPROT, lsh, DOT11_LLC_SNAP_HDR_LEN - 2);
+	lsh->type = 0;
+
+	return dhd_sendpkt(pub, 0, p);
+}
+#endif /* !SEND_HCI_CMD_VIA_IOCTL */
+
+/* Send HCI ACL data to dongle via data channel */
+int
+dhd_bta_tx_hcidata(dhd_pub_t *pub, void *data_buf, uint data_len)
+{
+	amp_hci_ACL_data_t *data = (amp_hci_ACL_data_t *)data_buf;
+	struct ether_header *eh;
+	struct dot11_llc_snap_header *lsh;
+	osl_t *osh = pub->osh;
+	uint len;
+	void *p;
+
+	if (data_len < HCI_ACL_DATA_PREAMBLE_SIZE) {
+		DHD_ERROR(("dhd_bta_tx_hcidata: short data_buf, data_len %u\n", data_len));
+		return BCME_BADLEN;
+	}
+
+	if ((len = (uint)ltoh16(data->dlen) + HCI_ACL_DATA_PREAMBLE_SIZE) > data_len) {
+		DHD_ERROR(("dhd_bta_tx_hcidata: malformed hci data, len %u data_len %u\n",
+		           len, data_len));
+		/* return BCME_BADLEN; */
+	}
+
+	p = PKTGET(osh, pub->hdrlen + RFC1042_HDR_LEN + len, TRUE);
+	if (p == NULL) {
+		DHD_ERROR(("dhd_bta_tx_hcidata: out of memory\n"));
+		return BCME_NOMEM;
+	}
+
+
+	/* copy in HCI ACL data header and HCI ACL data */
+	PKTPULL(osh, p, pub->hdrlen + RFC1042_HDR_LEN);
+	bcopy(data, PKTDATA(osh, p), len);
+
+	/* copy in partial Ethernet header with BT-SIG LLC/SNAP header */
+	PKTPUSH(osh, p, RFC1042_HDR_LEN);
+	eh = (struct ether_header *)PKTDATA(osh, p);
+	bzero(eh->ether_dhost, ETHER_ADDR_LEN);
+	bcopy(&pub->mac, eh->ether_shost, ETHER_ADDR_LEN);
+	eh->ether_type = hton16(len + DOT11_LLC_SNAP_HDR_LEN);
+	lsh = (struct dot11_llc_snap_header *)&eh[1];
+	bcopy(BT_SIG_SNAP_MPROT, lsh, DOT11_LLC_SNAP_HDR_LEN - 2);
+	lsh->type = HTON16(BTA_PROT_L2CAP);
+
+	return dhd_sendpkt(pub, 0, p);
+}
+
+/* txcomplete callback */
+void
+dhd_bta_tx_hcidata_complete(dhd_pub_t *dhdp, void *txp, bool success)
+{
+	uint8 *pktdata = (uint8 *)PKTDATA(dhdp->osh, txp);
+	amp_hci_ACL_data_t *ACL_data = (amp_hci_ACL_data_t *)(pktdata + RFC1042_HDR_LEN);
+	uint16 handle = ltoh16(ACL_data->handle);
+	uint16 llh = HCI_ACL_DATA_HANDLE(handle);
+
+	wl_event_msg_t event;
+	uint8 data[HCI_EVT_PREAMBLE_SIZE + sizeof(num_completed_data_blocks_evt_parms_t)];
+	amp_hci_event_t *evt;
+	num_completed_data_blocks_evt_parms_t *parms;
+
+	uint16 len = HCI_EVT_PREAMBLE_SIZE + sizeof(num_completed_data_blocks_evt_parms_t);
+
+	/* update the event struct */
+	memset(&event, 0, sizeof(event));
+	event.version = hton16(BCM_EVENT_MSG_VERSION);
+	event.event_type = hton32(WLC_E_BTA_HCI_EVENT);
+	event.status = 0;
+	event.reason = 0;
+	event.auth_type = 0;
+	event.datalen = hton32(len);
+	event.flags = 0;
+
+	/* generate Number of Completed Blocks event */
+	evt = (amp_hci_event_t *)data;
+	evt->ecode = HCI_Number_of_Completed_Data_Blocks;
+	evt->plen = sizeof(num_completed_data_blocks_evt_parms_t);
+
+	parms = (num_completed_data_blocks_evt_parms_t *)evt->parms;
+	htol16_ua_store(dhdp->maxdatablks, (uint8 *)&parms->num_blocks);
+	parms->num_handles = 1;
+	htol16_ua_store(llh, (uint8 *)&parms->completed[0].handle);
+	parms->completed[0].pkts = 1;
+	parms->completed[0].blocks = 1;
+
+	dhd_sendup_event_common(dhdp, &event, data);
+}
+
+/* event callback */
+void
+dhd_bta_doevt(dhd_pub_t *dhdp, void *data_buf, uint data_len)
+{
+	amp_hci_event_t *evt = (amp_hci_event_t *)data_buf;
+
+	switch (evt->ecode) {
+	case HCI_Command_Complete: {
+		cmd_complete_parms_t *parms = (cmd_complete_parms_t *)evt->parms;
+		switch (ltoh16_ua((uint8 *)&parms->opcode)) {
+		case HCI_Read_Data_Block_Size: {
+			read_data_block_size_evt_parms_t *parms2 =
+			        (read_data_block_size_evt_parms_t *)parms->parms;
+			dhdp->maxdatablks = ltoh16_ua((uint8 *)&parms2->data_block_num);
+			break;
+		}
+		}
+		break;
+	}
+
+	case HCI_Flush_Occurred: {
+		flush_occurred_evt_parms_t *evt_parms = (flush_occurred_evt_parms_t *)evt->parms;
+		dhd_bta_flush_hcidata(dhdp, ltoh16_ua((uint8 *)&evt_parms->handle));
+		break;
+	}
+	default:
+		break;
+	}
+}
diff --git a/drivers/net/wireless/bcmdhd/dhd_bta.h b/drivers/net/wireless/bcmdhd/dhd_bta.h
new file mode 100644
index 000000000000..07d9cebb883a
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd_bta.h
@@ -0,0 +1,39 @@
+/*
+ * BT-AMP support routines
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_bta.h,v 1.2 2009-02-26 22:35:56 Exp $
+ */
+#ifndef __dhd_bta_h__
+#define __dhd_bta_h__
+
+struct dhd_pub;
+
+extern int dhd_bta_docmd(struct dhd_pub *pub, void *cmd_buf, uint cmd_len);
+
+extern void dhd_bta_doevt(struct dhd_pub *pub, void *data_buf, uint data_len);
+
+extern int dhd_bta_tx_hcidata(struct dhd_pub *pub, void *data_buf, uint data_len);
+extern void dhd_bta_tx_hcidata_complete(struct dhd_pub *dhdp, void *txp, bool success);
+
+
+#endif /* __dhd_bta_h__ */
diff --git a/drivers/net/wireless/bcmdhd/dhd_bus.h b/drivers/net/wireless/bcmdhd/dhd_bus.h
new file mode 100644
index 000000000000..bccb8b6603f8
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd_bus.h
@@ -0,0 +1,99 @@
+/*
+ * Header file describing the internal (inter-module) DHD interfaces.
+ *
+ * Provides type definitions and function prototypes used to link the
+ * DHD OS, bus, and protocol modules.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_bus.h,v 1.14.28.1 2010-12-23 01:13:17 Exp $
+ */
+
+#ifndef _dhd_bus_h_
+#define _dhd_bus_h_
+
+/*
+ * Exported from dhd bus module (dhd_usb, dhd_sdio)
+ */
+
+/* Indicate (dis)interest in finding dongles. */
+extern int dhd_bus_register(void);
+extern void dhd_bus_unregister(void);
+
+/* Download firmware image and nvram image */
+extern bool dhd_bus_download_firmware(struct dhd_bus *bus, osl_t *osh,
+	char *fw_path, char *nv_path);
+
+/* Stop bus module: clear pending frames, disable data flow */
+extern void dhd_bus_stop(struct dhd_bus *bus, bool enforce_mutex);
+
+/* Initialize bus module: prepare for communication w/dongle */
+extern int dhd_bus_init(dhd_pub_t *dhdp, bool enforce_mutex);
+
+/* Get the Bus Idle Time */
+extern void dhd_bus_getidletime(dhd_pub_t *dhdp, int *idletime);
+
+/* Set the Bus Idle Time*/
+extern void dhd_bus_setidletime(dhd_pub_t *dhdp, int idle_time);
+/* Send a data frame to the dongle.  Callee disposes of txp. */
+extern int dhd_bus_txdata(struct dhd_bus *bus, void *txp);
+
+/* Send/receive a control message to/from the dongle.
+ * Expects caller to enforce a single outstanding transaction.
+ */
+extern int dhd_bus_txctl(struct dhd_bus *bus, uchar *msg, uint msglen);
+extern int dhd_bus_rxctl(struct dhd_bus *bus, uchar *msg, uint msglen);
+
+/* Watchdog timer function */
+extern bool dhd_bus_watchdog(dhd_pub_t *dhd);
+extern void dhd_disable_intr(dhd_pub_t *dhd);
+
+#if defined(DHD_DEBUG)
+/* Device console input function */
+extern int dhd_bus_console_in(dhd_pub_t *dhd, uchar *msg, uint msglen);
+#endif /* defined(DHD_DEBUG) */
+
+/* Deferred processing for the bus, return TRUE requests reschedule */
+extern bool dhd_bus_dpc(struct dhd_bus *bus);
+extern void dhd_bus_isr(bool * InterruptRecognized, bool * QueueMiniportHandleInterrupt, void *arg);
+
+
+/* Check for and handle local prot-specific iovar commands */
+extern int dhd_bus_iovar_op(dhd_pub_t *dhdp, const char *name,
+                            void *params, int plen, void *arg, int len, bool set);
+
+/* Add bus dump output to a buffer */
+extern void dhd_bus_dump(dhd_pub_t *dhdp, struct bcmstrbuf *strbuf);
+
+/* Clear any bus counters */
+extern void dhd_bus_clearcounts(dhd_pub_t *dhdp);
+
+/* return the dongle chipid */
+extern uint dhd_bus_chip(struct dhd_bus *bus);
+
+/* Set user-specified nvram parameters. */
+extern void dhd_bus_set_nvram_params(struct dhd_bus * bus, const char *nvram_params);
+
+extern void *dhd_bus_pub(struct dhd_bus *bus);
+extern void *dhd_bus_txq(struct dhd_bus *bus);
+extern uint dhd_bus_hdrlen(struct dhd_bus *bus);
+
+#endif /* _dhd_bus_h_ */
diff --git a/drivers/net/wireless/bcmdhd/dhd_cdc.c b/drivers/net/wireless/bcmdhd/dhd_cdc.c
new file mode 100644
index 000000000000..f16d81c9e198
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd_cdc.c
@@ -0,0 +1,2534 @@
+/*
+ * DHD Protocol Module for CDC and BDC.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_cdc.c 324280 2012-03-28 19:01:17Z $
+ *
+ * BDC is like CDC, except it includes a header for data packets to convey
+ * packet priority over the bus, and flags (e.g. to indicate checksum status
+ * for dongle offload.)
+ */
+
+#include <typedefs.h>
+#include <osl.h>
+
+#include <bcmutils.h>
+#include <bcmcdc.h>
+#include <bcmendian.h>
+
+#include <dngl_stats.h>
+#include <dhd.h>
+#include <dhd_proto.h>
+#include <dhd_bus.h>
+#include <dhd_dbg.h>
+
+
+#ifdef PROP_TXSTATUS
+#include <wlfc_proto.h>
+#include <dhd_wlfc.h>
+#endif
+
+
+#define RETRIES 2		/* # of retries to retrieve matching ioctl response */
+#define BUS_HEADER_LEN	(16+DHD_SDALIGN)	/* Must be at least SDPCM_RESERVE
+				 * defined in dhd_sdio.c (amount of header tha might be added)
+				 * plus any space that might be needed for alignment padding.
+				 */
+#define ROUND_UP_MARGIN	2048	/* Biggest SDIO block size possible for
+				 * round off at the end of buffer
+				 */
+
+#define BUS_RETRIES 1	/* # of retries before aborting a bus tx operation */
+
+#ifdef PROP_TXSTATUS
+typedef struct dhd_wlfc_commit_info {
+	uint8					needs_hdr;
+	uint8					ac_fifo_credit_spent;
+	ewlfc_packet_state_t	pkt_type;
+	wlfc_mac_descriptor_t*	mac_entry;
+	void*					p;
+} dhd_wlfc_commit_info_t;
+#endif /* PROP_TXSTATUS */
+
+typedef struct dhd_prot {
+	uint16 reqid;
+	uint8 pending;
+	uint32 lastcmd;
+	uint8 bus_header[BUS_HEADER_LEN];
+	cdc_ioctl_t msg;
+	unsigned char buf[WLC_IOCTL_MAXLEN + ROUND_UP_MARGIN];
+} dhd_prot_t;
+
+extern int dhd_dbus_txdata(dhd_pub_t *dhdp, void *pktbuf);
+
+static int
+dhdcdc_msg(dhd_pub_t *dhd)
+{
+	int err = 0;
+	dhd_prot_t *prot = dhd->prot;
+	int len = ltoh32(prot->msg.len) + sizeof(cdc_ioctl_t);
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	DHD_OS_WAKE_LOCK(dhd);
+
+	/* NOTE : cdc->msg.len holds the desired length of the buffer to be
+	 *        returned. Only up to CDC_MAX_MSG_SIZE of this buffer area
+	 *	  is actually sent to the dongle
+	 */
+	if (len > CDC_MAX_MSG_SIZE)
+		len = CDC_MAX_MSG_SIZE;
+
+	/* Send request */
+	err = dhd_bus_txctl(dhd->bus, (uchar*)&prot->msg, len);
+
+	DHD_OS_WAKE_UNLOCK(dhd);
+	return err;
+}
+
+static int
+dhdcdc_cmplt(dhd_pub_t *dhd, uint32 id, uint32 len)
+{
+	int ret;
+	int cdc_len = len+sizeof(cdc_ioctl_t);
+	dhd_prot_t *prot = dhd->prot;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	do {
+		ret = dhd_bus_rxctl(dhd->bus, (uchar*)&prot->msg, cdc_len);
+		if (ret < 0)
+			break;
+	} while (CDC_IOC_ID(ltoh32(prot->msg.flags)) != id);
+
+	return ret;
+}
+
+static int
+dhdcdc_query_ioctl(dhd_pub_t *dhd, int ifidx, uint cmd, void *buf, uint len, uint8 action)
+{
+	dhd_prot_t *prot = dhd->prot;
+	cdc_ioctl_t *msg = &prot->msg;
+	void *info;
+	int ret = 0, retries = 0;
+	uint32 id, flags = 0;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+	DHD_CTL(("%s: cmd %d len %d\n", __FUNCTION__, cmd, len));
+
+
+	/* Respond "bcmerror" and "bcmerrorstr" with local cache */
+	if (cmd == WLC_GET_VAR && buf)
+	{
+		if (!strcmp((char *)buf, "bcmerrorstr"))
+		{
+			strncpy((char *)buf, bcmerrorstr(dhd->dongle_error), BCME_STRLEN);
+			goto done;
+		}
+		else if (!strcmp((char *)buf, "bcmerror"))
+		{
+			*(int *)buf = dhd->dongle_error;
+			goto done;
+		}
+	}
+
+	memset(msg, 0, sizeof(cdc_ioctl_t));
+
+	msg->cmd = htol32(cmd);
+	msg->len = htol32(len);
+	msg->flags = (++prot->reqid << CDCF_IOC_ID_SHIFT);
+	CDC_SET_IF_IDX(msg, ifidx);
+	/* add additional action bits */
+	action &= WL_IOCTL_ACTION_MASK;
+	msg->flags |= (action << CDCF_IOC_ACTION_SHIFT);
+	msg->flags = htol32(msg->flags);
+
+	if (buf)
+		memcpy(prot->buf, buf, len);
+
+	if ((ret = dhdcdc_msg(dhd)) < 0) {
+		if (!dhd->hang_was_sent)
+			DHD_ERROR(("dhdcdc_query_ioctl: dhdcdc_msg failed w/status %d\n", ret));
+		goto done;
+	}
+
+retry:
+	/* wait for interrupt and get first fragment */
+	if ((ret = dhdcdc_cmplt(dhd, prot->reqid, len)) < 0)
+		goto done;
+
+	flags = ltoh32(msg->flags);
+	id = (flags & CDCF_IOC_ID_MASK) >> CDCF_IOC_ID_SHIFT;
+
+	if ((id < prot->reqid) && (++retries < RETRIES))
+		goto retry;
+	if (id != prot->reqid) {
+		DHD_ERROR(("%s: %s: unexpected request id %d (expected %d)\n",
+		           dhd_ifname(dhd, ifidx), __FUNCTION__, id, prot->reqid));
+		ret = -EINVAL;
+		goto done;
+	}
+
+	/* Check info buffer */
+	info = (void*)&msg[1];
+
+	/* Copy info buffer */
+	if (buf)
+	{
+		if (ret < (int)len)
+			len = ret;
+		memcpy(buf, info, len);
+	}
+
+	/* Check the ERROR flag */
+	if (flags & CDCF_IOC_ERROR)
+	{
+		ret = ltoh32(msg->status);
+		/* Cache error from dongle */
+		dhd->dongle_error = ret;
+	}
+
+done:
+	return ret;
+}
+
+static int
+dhdcdc_set_ioctl(dhd_pub_t *dhd, int ifidx, uint cmd, void *buf, uint len, uint8 action)
+{
+	dhd_prot_t *prot = dhd->prot;
+	cdc_ioctl_t *msg = &prot->msg;
+	int ret = 0;
+	uint32 flags, id;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+	DHD_CTL(("%s: cmd %d len %d\n", __FUNCTION__, cmd, len));
+
+	if (dhd->busstate == DHD_BUS_DOWN) {
+		DHD_ERROR(("%s : bus is down. we have nothing to do\n", __FUNCTION__));
+		return -EIO;
+	}
+
+	/* don't talk to the dongle if fw is about to be reloaded */
+	if (dhd->hang_was_sent) {
+		DHD_ERROR(("%s: HANG was sent up earlier. Not talking to the chip\n",
+			__FUNCTION__));
+		return -EIO;
+	}
+
+	memset(msg, 0, sizeof(cdc_ioctl_t));
+
+	msg->cmd = htol32(cmd);
+	msg->len = htol32(len);
+	msg->flags = (++prot->reqid << CDCF_IOC_ID_SHIFT);
+	CDC_SET_IF_IDX(msg, ifidx);
+	/* add additional action bits */
+	action &= WL_IOCTL_ACTION_MASK;
+	msg->flags |= (action << CDCF_IOC_ACTION_SHIFT) | CDCF_IOC_SET;
+	msg->flags = htol32(msg->flags);
+
+	if (buf)
+		memcpy(prot->buf, buf, len);
+
+	if ((ret = dhdcdc_msg(dhd)) < 0) {
+		DHD_ERROR(("%s: dhdcdc_msg failed w/status %d\n", __FUNCTION__, ret));
+		goto done;
+	}
+
+	if ((ret = dhdcdc_cmplt(dhd, prot->reqid, len)) < 0)
+		goto done;
+
+	flags = ltoh32(msg->flags);
+	id = (flags & CDCF_IOC_ID_MASK) >> CDCF_IOC_ID_SHIFT;
+
+	if (id != prot->reqid) {
+		DHD_ERROR(("%s: %s: unexpected request id %d (expected %d)\n",
+		           dhd_ifname(dhd, ifidx), __FUNCTION__, id, prot->reqid));
+		ret = -EINVAL;
+		goto done;
+	}
+
+	/* Check the ERROR flag */
+	if (flags & CDCF_IOC_ERROR)
+	{
+		ret = ltoh32(msg->status);
+		/* Cache error from dongle */
+		dhd->dongle_error = ret;
+	}
+
+done:
+	return ret;
+}
+
+
+int
+dhd_prot_ioctl(dhd_pub_t *dhd, int ifidx, wl_ioctl_t * ioc, void * buf, int len)
+{
+	dhd_prot_t *prot = dhd->prot;
+	int ret = -1;
+	uint8 action;
+
+	if ((dhd->busstate == DHD_BUS_DOWN) || dhd->hang_was_sent) {
+		DHD_ERROR(("%s : bus is down. we have nothing to do\n", __FUNCTION__));
+		goto done;
+	}
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	ASSERT(len <= WLC_IOCTL_MAXLEN);
+
+	if (len > WLC_IOCTL_MAXLEN)
+		goto done;
+
+	if (prot->pending == TRUE) {
+		DHD_ERROR(("CDC packet is pending!!!! cmd=0x%x (%lu) lastcmd=0x%x (%lu)\n",
+			ioc->cmd, (unsigned long)ioc->cmd, prot->lastcmd,
+			(unsigned long)prot->lastcmd));
+		if ((ioc->cmd == WLC_SET_VAR) || (ioc->cmd == WLC_GET_VAR)) {
+			DHD_TRACE(("iovar cmd=%s\n", (char*)buf));
+		}
+		goto done;
+	}
+
+	prot->pending = TRUE;
+	prot->lastcmd = ioc->cmd;
+	action = ioc->set;
+	if (action & WL_IOCTL_ACTION_SET)
+		ret = dhdcdc_set_ioctl(dhd, ifidx, ioc->cmd, buf, len, action);
+	else {
+		ret = dhdcdc_query_ioctl(dhd, ifidx, ioc->cmd, buf, len, action);
+		if (ret > 0)
+			ioc->used = ret - sizeof(cdc_ioctl_t);
+	}
+
+	/* Too many programs assume ioctl() returns 0 on success */
+	if (ret >= 0)
+		ret = 0;
+	else {
+		cdc_ioctl_t *msg = &prot->msg;
+		ioc->needed = ltoh32(msg->len); /* len == needed when set/query fails from dongle */
+	}
+
+	/* Intercept the wme_dp ioctl here */
+	if ((!ret) && (ioc->cmd == WLC_SET_VAR) && (!strcmp(buf, "wme_dp"))) {
+		int slen, val = 0;
+
+		slen = strlen("wme_dp") + 1;
+		if (len >= (int)(slen + sizeof(int)))
+			bcopy(((char *)buf + slen), &val, sizeof(int));
+		dhd->wme_dp = (uint8) ltoh32(val);
+	}
+
+	prot->pending = FALSE;
+
+done:
+	return ret;
+}
+
+int
+dhd_prot_iovar_op(dhd_pub_t *dhdp, const char *name,
+                  void *params, int plen, void *arg, int len, bool set)
+{
+	return BCME_UNSUPPORTED;
+}
+
+#ifdef PROP_TXSTATUS
+void
+dhd_wlfc_dump(dhd_pub_t *dhdp, struct bcmstrbuf *strbuf)
+{
+	int i;
+	uint8* ea;
+	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
+		dhdp->wlfc_state;
+	wlfc_hanger_t* h;
+	wlfc_mac_descriptor_t* mac_table;
+	wlfc_mac_descriptor_t* interfaces;
+	char* iftypes[] = {"STA", "AP", "WDS", "p2pGO", "p2pCL"};
+
+	if (wlfc == NULL) {
+		bcm_bprintf(strbuf, "wlfc not initialized yet\n");
+		return;
+	}
+	h = (wlfc_hanger_t*)wlfc->hanger;
+	if (h == NULL) {
+		bcm_bprintf(strbuf, "wlfc-hanger not initialized yet\n");
+	}
+
+	mac_table = wlfc->destination_entries.nodes;
+	interfaces = wlfc->destination_entries.interfaces;
+	bcm_bprintf(strbuf, "---- wlfc stats ----\n");
+	if (h) {
+		bcm_bprintf(strbuf, "wlfc hanger (pushed,popped,f_push,"
+			"f_pop,f_slot, pending) = (%d,%d,%d,%d,%d,%d)\n",
+			h->pushed,
+			h->popped,
+			h->failed_to_push,
+			h->failed_to_pop,
+			h->failed_slotfind,
+			(h->pushed - h->popped));
+	}
+
+	bcm_bprintf(strbuf, "wlfc fail(tlv,credit_rqst,mac_update,psmode_update), "
+		"(dq_full,sendq_full, rollback_fail) = (%d,%d,%d,%d), (%d,%d,%d)\n",
+		wlfc->stats.tlv_parse_failed,
+		wlfc->stats.credit_request_failed,
+		wlfc->stats.mac_update_failed,
+		wlfc->stats.psmode_update_failed,
+		wlfc->stats.delayq_full_error,
+		wlfc->stats.sendq_full_error,
+		wlfc->stats.rollback_failed);
+
+	bcm_bprintf(strbuf, "SENDQ (len,credit,sent) "
+		"(AC0[%d,%d,%d],AC1[%d,%d,%d],AC2[%d,%d,%d],AC3[%d,%d,%d],BC_MC[%d,%d,%d])\n",
+		wlfc->SENDQ.q[0].len, wlfc->FIFO_credit[0], wlfc->stats.sendq_pkts[0],
+		wlfc->SENDQ.q[1].len, wlfc->FIFO_credit[1], wlfc->stats.sendq_pkts[1],
+		wlfc->SENDQ.q[2].len, wlfc->FIFO_credit[2], wlfc->stats.sendq_pkts[2],
+		wlfc->SENDQ.q[3].len, wlfc->FIFO_credit[3], wlfc->stats.sendq_pkts[3],
+		wlfc->SENDQ.q[4].len, wlfc->FIFO_credit[4], wlfc->stats.sendq_pkts[4]);
+
+#ifdef PROP_TXSTATUS_DEBUG
+	bcm_bprintf(strbuf, "SENDQ dropped: AC[0-3]:(%d,%d,%d,%d), (bcmc,atim):(%d,%d)\n",
+		wlfc->stats.dropped_qfull[0], wlfc->stats.dropped_qfull[1],
+		wlfc->stats.dropped_qfull[2], wlfc->stats.dropped_qfull[3],
+		wlfc->stats.dropped_qfull[4], wlfc->stats.dropped_qfull[5]);
+#endif
+
+	bcm_bprintf(strbuf, "\n");
+	for (i = 0; i < WLFC_MAX_IFNUM; i++) {
+		if (interfaces[i].occupied) {
+			char* iftype_desc;
+
+			if (interfaces[i].iftype > WLC_E_IF_ROLE_P2P_CLIENT)
+				iftype_desc = "<Unknown";
+			else
+				iftype_desc = iftypes[interfaces[i].iftype];
+
+			ea = interfaces[i].ea;
+			bcm_bprintf(strbuf, "INTERFACE[%d].ea = "
+				"[%02x:%02x:%02x:%02x:%02x:%02x], if:%d, type: %s\n", i,
+				ea[0], ea[1], ea[2], ea[3], ea[4], ea[5],
+				interfaces[i].interface_id,
+				iftype_desc);
+
+			bcm_bprintf(strbuf, "INTERFACE[%d].DELAYQ(len,state,credit)"
+				"= (%d,%s,%d)\n",
+				i,
+				interfaces[i].psq.len,
+				((interfaces[i].state ==
+				WLFC_STATE_OPEN) ? " OPEN":"CLOSE"),
+				interfaces[i].requested_credit);
+
+			bcm_bprintf(strbuf, "INTERFACE[%d].DELAYQ"
+				"(sup,ac0),(sup,ac1),(sup,ac2),(sup,ac3) = "
+				"(%d,%d),(%d,%d),(%d,%d),(%d,%d)\n",
+				i,
+				interfaces[i].psq.q[0].len,
+				interfaces[i].psq.q[1].len,
+				interfaces[i].psq.q[2].len,
+				interfaces[i].psq.q[3].len,
+				interfaces[i].psq.q[4].len,
+				interfaces[i].psq.q[5].len,
+				interfaces[i].psq.q[6].len,
+				interfaces[i].psq.q[7].len);
+		}
+	}
+
+	bcm_bprintf(strbuf, "\n");
+	for (i = 0; i < WLFC_MAC_DESC_TABLE_SIZE; i++) {
+		if (mac_table[i].occupied) {
+			ea = mac_table[i].ea;
+			bcm_bprintf(strbuf, "MAC_table[%d].ea = "
+				"[%02x:%02x:%02x:%02x:%02x:%02x], if:%d\n", i,
+				ea[0], ea[1], ea[2], ea[3], ea[4], ea[5],
+				mac_table[i].interface_id);
+
+			bcm_bprintf(strbuf, "MAC_table[%d].DELAYQ(len,state,credit)"
+				"= (%d,%s,%d)\n",
+				i,
+				mac_table[i].psq.len,
+				((mac_table[i].state ==
+				WLFC_STATE_OPEN) ? " OPEN":"CLOSE"),
+				mac_table[i].requested_credit);
+#ifdef PROP_TXSTATUS_DEBUG
+			bcm_bprintf(strbuf, "MAC_table[%d]: (opened, closed) = (%d, %d)\n",
+				i, mac_table[i].opened_ct, mac_table[i].closed_ct);
+#endif
+			bcm_bprintf(strbuf, "MAC_table[%d].DELAYQ"
+				"(sup,ac0),(sup,ac1),(sup,ac2),(sup,ac3) = "
+				"(%d,%d),(%d,%d),(%d,%d),(%d,%d)\n",
+				i,
+				mac_table[i].psq.q[0].len,
+				mac_table[i].psq.q[1].len,
+				mac_table[i].psq.q[2].len,
+				mac_table[i].psq.q[3].len,
+				mac_table[i].psq.q[4].len,
+				mac_table[i].psq.q[5].len,
+				mac_table[i].psq.q[6].len,
+				mac_table[i].psq.q[7].len);
+		}
+	}
+
+#ifdef PROP_TXSTATUS_DEBUG
+	{
+		int avg;
+		int moving_avg = 0;
+		int moving_samples;
+
+		if (wlfc->stats.latency_sample_count) {
+			moving_samples = sizeof(wlfc->stats.deltas)/sizeof(uint32);
+
+			for (i = 0; i < moving_samples; i++)
+				moving_avg += wlfc->stats.deltas[i];
+			moving_avg /= moving_samples;
+
+			avg = (100 * wlfc->stats.total_status_latency) /
+				wlfc->stats.latency_sample_count;
+			bcm_bprintf(strbuf, "txstatus latency (average, last, moving[%d]) = "
+				"(%d.%d, %03d, %03d)\n",
+				moving_samples, avg/100, (avg - (avg/100)*100),
+				wlfc->stats.latency_most_recent,
+				moving_avg);
+		}
+	}
+
+	bcm_bprintf(strbuf, "wlfc- fifo[0-5] credit stats: sent = (%d,%d,%d,%d,%d,%d), "
+		"back = (%d,%d,%d,%d,%d,%d)\n",
+		wlfc->stats.fifo_credits_sent[0],
+		wlfc->stats.fifo_credits_sent[1],
+		wlfc->stats.fifo_credits_sent[2],
+		wlfc->stats.fifo_credits_sent[3],
+		wlfc->stats.fifo_credits_sent[4],
+		wlfc->stats.fifo_credits_sent[5],
+
+		wlfc->stats.fifo_credits_back[0],
+		wlfc->stats.fifo_credits_back[1],
+		wlfc->stats.fifo_credits_back[2],
+		wlfc->stats.fifo_credits_back[3],
+		wlfc->stats.fifo_credits_back[4],
+		wlfc->stats.fifo_credits_back[5]);
+	{
+		uint32 fifo_cr_sent = 0;
+		uint32 fifo_cr_acked = 0;
+		uint32 request_cr_sent = 0;
+		uint32 request_cr_ack = 0;
+		uint32 bc_mc_cr_ack = 0;
+
+		for (i = 0; i < sizeof(wlfc->stats.fifo_credits_sent)/sizeof(uint32); i++) {
+			fifo_cr_sent += wlfc->stats.fifo_credits_sent[i];
+		}
+
+		for (i = 0; i < sizeof(wlfc->stats.fifo_credits_back)/sizeof(uint32); i++) {
+			fifo_cr_acked += wlfc->stats.fifo_credits_back[i];
+		}
+
+		for (i = 0; i < WLFC_MAC_DESC_TABLE_SIZE; i++) {
+			if (wlfc->destination_entries.nodes[i].occupied) {
+				request_cr_sent +=
+					wlfc->destination_entries.nodes[i].dstncredit_sent_packets;
+			}
+		}
+		for (i = 0; i < WLFC_MAX_IFNUM; i++) {
+			if (wlfc->destination_entries.interfaces[i].occupied) {
+				request_cr_sent +=
+				wlfc->destination_entries.interfaces[i].dstncredit_sent_packets;
+			}
+		}
+		for (i = 0; i < WLFC_MAC_DESC_TABLE_SIZE; i++) {
+			if (wlfc->destination_entries.nodes[i].occupied) {
+				request_cr_ack +=
+					wlfc->destination_entries.nodes[i].dstncredit_acks;
+			}
+		}
+		for (i = 0; i < WLFC_MAX_IFNUM; i++) {
+			if (wlfc->destination_entries.interfaces[i].occupied) {
+				request_cr_ack +=
+					wlfc->destination_entries.interfaces[i].dstncredit_acks;
+			}
+		}
+		bcm_bprintf(strbuf, "wlfc- (sent, status) => pq(%d,%d), vq(%d,%d),"
+			"other:%d, bc_mc:%d, signal-only, (sent,freed): (%d,%d)",
+			fifo_cr_sent, fifo_cr_acked,
+			request_cr_sent, request_cr_ack,
+			wlfc->destination_entries.other.dstncredit_acks,
+			bc_mc_cr_ack,
+			wlfc->stats.signal_only_pkts_sent, wlfc->stats.signal_only_pkts_freed);
+	}
+#endif /* PROP_TXSTATUS_DEBUG */
+	bcm_bprintf(strbuf, "\n");
+	bcm_bprintf(strbuf, "wlfc- pkt((in,2bus,txstats,hdrpull),(dropped,hdr_only,wlc_tossed)"
+		"(freed,free_err,rollback)) = "
+		"((%d,%d,%d,%d),(%d,%d,%d),(%d,%d,%d))\n",
+		wlfc->stats.pktin,
+		wlfc->stats.pkt2bus,
+		wlfc->stats.txstatus_in,
+		wlfc->stats.dhd_hdrpulls,
+
+		wlfc->stats.pktdropped,
+		wlfc->stats.wlfc_header_only_pkt,
+		wlfc->stats.wlc_tossed_pkts,
+
+		wlfc->stats.pkt_freed,
+		wlfc->stats.pkt_free_err, wlfc->stats.rollback);
+
+	bcm_bprintf(strbuf, "wlfc- suppress((d11,wlc,err),enq(d11,wl,hq,mac?),retx(d11,wlc,hq)) = "
+		"((%d,%d,%d),(%d,%d,%d,%d),(%d,%d,%d))\n",
+
+		wlfc->stats.d11_suppress,
+		wlfc->stats.wl_suppress,
+		wlfc->stats.bad_suppress,
+
+		wlfc->stats.psq_d11sup_enq,
+		wlfc->stats.psq_wlsup_enq,
+		wlfc->stats.psq_hostq_enq,
+		wlfc->stats.mac_handle_notfound,
+
+		wlfc->stats.psq_d11sup_retx,
+		wlfc->stats.psq_wlsup_retx,
+		wlfc->stats.psq_hostq_retx);
+	return;
+}
+
+/* Create a place to store all packet pointers submitted to the firmware until
+	a status comes back, suppress or otherwise.
+
+	hang-er: noun, a contrivance on which things are hung, as a hook.
+*/
+static void*
+dhd_wlfc_hanger_create(osl_t *osh, int max_items)
+{
+	int i;
+	wlfc_hanger_t* hanger;
+
+	/* allow only up to a specific size for now */
+	ASSERT(max_items == WLFC_HANGER_MAXITEMS);
+
+	if ((hanger = (wlfc_hanger_t*)MALLOC(osh, WLFC_HANGER_SIZE(max_items))) == NULL)
+		return NULL;
+
+	memset(hanger, 0, WLFC_HANGER_SIZE(max_items));
+	hanger->max_items = max_items;
+
+	for (i = 0; i < hanger->max_items; i++) {
+		hanger->items[i].state = WLFC_HANGER_ITEM_STATE_FREE;
+	}
+	return hanger;
+}
+
+static int
+dhd_wlfc_hanger_delete(osl_t *osh, void* hanger)
+{
+	wlfc_hanger_t* h = (wlfc_hanger_t*)hanger;
+
+	if (h) {
+		MFREE(osh, h, WLFC_HANGER_SIZE(h->max_items));
+		return BCME_OK;
+	}
+	return BCME_BADARG;
+}
+
+static uint16
+dhd_wlfc_hanger_get_free_slot(void* hanger)
+{
+	int i;
+	wlfc_hanger_t* h = (wlfc_hanger_t*)hanger;
+
+	if (h) {
+		for (i = 0; i < h->max_items; i++) {
+			if (h->items[i].state == WLFC_HANGER_ITEM_STATE_FREE)
+				return (uint16)i;
+		}
+		h->failed_slotfind++;
+	}
+	return WLFC_HANGER_MAXITEMS;
+}
+
+static int
+dhd_wlfc_hanger_pushpkt(void* hanger, void* pkt, uint32 slot_id)
+{
+	int rc = BCME_OK;
+	wlfc_hanger_t* h = (wlfc_hanger_t*)hanger;
+
+	if (h && (slot_id < WLFC_HANGER_MAXITEMS)) {
+		if (h->items[slot_id].state == WLFC_HANGER_ITEM_STATE_FREE) {
+			h->items[slot_id].state = WLFC_HANGER_ITEM_STATE_INUSE;
+			h->items[slot_id].pkt = pkt;
+			h->items[slot_id].identifier = slot_id;
+			h->pushed++;
+		}
+		else {
+			h->failed_to_push++;
+			rc = BCME_NOTFOUND;
+		}
+	}
+	else
+		rc = BCME_BADARG;
+	return rc;
+}
+
+static int
+dhd_wlfc_hanger_poppkt(void* hanger, uint32 slot_id, void** pktout, int remove_from_hanger)
+{
+	int rc = BCME_OK;
+	wlfc_hanger_t* h = (wlfc_hanger_t*)hanger;
+
+	/* this packet was not pushed at the time it went to the firmware */
+	if (slot_id == WLFC_HANGER_MAXITEMS)
+		return BCME_NOTFOUND;
+
+	if (h) {
+		if (h->items[slot_id].state == WLFC_HANGER_ITEM_STATE_INUSE) {
+			*pktout = h->items[slot_id].pkt;
+			if (remove_from_hanger) {
+				h->items[slot_id].state =
+					WLFC_HANGER_ITEM_STATE_FREE;
+				h->items[slot_id].pkt = NULL;
+				h->items[slot_id].identifier = 0;
+				h->popped++;
+			}
+		}
+		else {
+			h->failed_to_pop++;
+			rc = BCME_NOTFOUND;
+		}
+	}
+	else
+		rc = BCME_BADARG;
+	return rc;
+}
+
+static int
+_dhd_wlfc_pushheader(athost_wl_status_info_t* ctx, void* p, bool tim_signal,
+	uint8 tim_bmp, uint8 mac_handle, uint32 htodtag)
+{
+	uint32 wl_pktinfo = 0;
+	uint8* wlh;
+	uint8 dataOffset;
+	uint8 fillers;
+	uint8 tim_signal_len = 0;
+
+	struct bdc_header *h;
+
+	if (tim_signal) {
+		tim_signal_len = 1 + 1 + WLFC_CTL_VALUE_LEN_PENDING_TRAFFIC_BMP;
+	}
+
+	/* +2 is for Type[1] and Len[1] in TLV, plus TIM signal */
+	dataOffset = WLFC_CTL_VALUE_LEN_PKTTAG + 2 + tim_signal_len;
+	fillers = ROUNDUP(dataOffset, 4) - dataOffset;
+	dataOffset += fillers;
+
+	PKTPUSH(ctx->osh, p, dataOffset);
+	wlh = (uint8*) PKTDATA(ctx->osh, p);
+
+	wl_pktinfo = htol32(htodtag);
+
+	wlh[0] = WLFC_CTL_TYPE_PKTTAG;
+	wlh[1] = WLFC_CTL_VALUE_LEN_PKTTAG;
+	memcpy(&wlh[2], &wl_pktinfo, sizeof(uint32));
+
+	if (tim_signal_len) {
+		wlh[dataOffset - fillers - tim_signal_len ] =
+			WLFC_CTL_TYPE_PENDING_TRAFFIC_BMP;
+		wlh[dataOffset - fillers - tim_signal_len + 1] =
+			WLFC_CTL_VALUE_LEN_PENDING_TRAFFIC_BMP;
+		wlh[dataOffset - fillers - tim_signal_len + 2] = mac_handle;
+		wlh[dataOffset - fillers - tim_signal_len + 3] = tim_bmp;
+	}
+	if (fillers)
+		memset(&wlh[dataOffset - fillers], WLFC_CTL_TYPE_FILLER, fillers);
+
+	PKTPUSH(ctx->osh, p, BDC_HEADER_LEN);
+	h = (struct bdc_header *)PKTDATA(ctx->osh, p);
+	h->flags = (BDC_PROTO_VER << BDC_FLAG_VER_SHIFT);
+	if (PKTSUMNEEDED(p))
+		h->flags |= BDC_FLAG_SUM_NEEDED;
+
+
+	h->priority = (PKTPRIO(p) & BDC_PRIORITY_MASK);
+	h->flags2 = 0;
+	h->dataOffset = dataOffset >> 2;
+	BDC_SET_IF_IDX(h, DHD_PKTTAG_IF(PKTTAG(p)));
+	return BCME_OK;
+}
+
+static int
+_dhd_wlfc_pullheader(athost_wl_status_info_t* ctx, void* pktbuf)
+{
+	struct bdc_header *h;
+
+	if (PKTLEN(ctx->osh, pktbuf) < BDC_HEADER_LEN) {
+		WLFC_DBGMESG(("%s: rx data too short (%d < %d)\n", __FUNCTION__,
+		           PKTLEN(ctx->osh, pktbuf), BDC_HEADER_LEN));
+		return BCME_ERROR;
+	}
+	h = (struct bdc_header *)PKTDATA(ctx->osh, pktbuf);
+
+	/* pull BDC header */
+	PKTPULL(ctx->osh, pktbuf, BDC_HEADER_LEN);
+	/* pull wl-header */
+	PKTPULL(ctx->osh, pktbuf, (h->dataOffset << 2));
+	return BCME_OK;
+}
+
+static wlfc_mac_descriptor_t*
+_dhd_wlfc_find_table_entry(athost_wl_status_info_t* ctx, void* p)
+{
+	int i;
+	wlfc_mac_descriptor_t* table = ctx->destination_entries.nodes;
+	uint8 ifid = DHD_PKTTAG_IF(PKTTAG(p));
+	uint8* dstn = DHD_PKTTAG_DSTN(PKTTAG(p));
+
+	/* no lookup necessary, only if this packet belongs to STA interface */
+	if (((ctx->destination_entries.interfaces[ifid].iftype == WLC_E_IF_ROLE_STA) ||
+		ETHER_ISMULTI(dstn) ||
+		(ctx->destination_entries.interfaces[ifid].iftype == WLC_E_IF_ROLE_P2P_CLIENT)) &&
+		(ctx->destination_entries.interfaces[ifid].occupied)) {
+			return &ctx->destination_entries.interfaces[ifid];
+	}
+
+	for (i = 0; i < WLFC_MAC_DESC_TABLE_SIZE; i++) {
+		if (table[i].occupied) {
+			if (table[i].interface_id == ifid) {
+				if (!memcmp(table[i].ea, dstn, ETHER_ADDR_LEN))
+					return &table[i];
+			}
+		}
+	}
+	return &ctx->destination_entries.other;
+}
+
+static int
+_dhd_wlfc_rollback_packet_toq(athost_wl_status_info_t* ctx,
+	void* p, ewlfc_packet_state_t pkt_type, uint32 hslot)
+{
+	/*
+	put the packet back to the head of queue
+
+	- a packet from send-q will need to go back to send-q and not delay-q
+	since that will change the order of packets.
+	- suppressed packet goes back to suppress sub-queue
+	- pull out the header, if new or delayed packet
+
+	Note: hslot is used only when header removal is done.
+	*/
+	wlfc_mac_descriptor_t* entry;
+	void* pktout;
+	int rc = BCME_OK;
+	int prec;
+
+	entry = _dhd_wlfc_find_table_entry(ctx, p);
+	prec = DHD_PKTTAG_FIFO(PKTTAG(p));
+	if (entry != NULL) {
+		if (pkt_type == eWLFC_PKTTYPE_SUPPRESSED) {
+			/* wl-header is saved for suppressed packets */
+			if (WLFC_PKTQ_PENQ_HEAD(&entry->psq, ((prec << 1) + 1), p) == NULL) {
+				WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
+				rc = BCME_ERROR;
+			}
+		}
+		else {
+			/* remove header first */
+			_dhd_wlfc_pullheader(ctx, p);
+
+			if (pkt_type == eWLFC_PKTTYPE_DELAYED) {
+				/* delay-q packets are going to delay-q */
+				if (WLFC_PKTQ_PENQ_HEAD(&entry->psq, (prec << 1), p) == NULL) {
+					WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
+					rc = BCME_ERROR;
+				}
+			}
+			else {
+				/* these are going to SENDQ */
+				if (WLFC_PKTQ_PENQ_HEAD(&ctx->SENDQ, prec, p) == NULL) {
+					WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
+					rc = BCME_ERROR;
+				}
+			}
+			/* free the hanger slot */
+			dhd_wlfc_hanger_poppkt(ctx->hanger, hslot, &pktout, 1);
+
+			/* decrement sequence count */
+			WLFC_DECR_SEQCOUNT(entry, prec);
+		}
+		/*
+		if this packet did not count against FIFO credit, it must have
+		taken a requested_credit from the firmware (for pspoll etc.)
+		*/
+		if (!DHD_PKTTAG_CREDITCHECK(PKTTAG(p))) {
+			entry->requested_credit++;
+		}
+	}
+	else {
+		WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
+		rc = BCME_ERROR;
+	}
+	if (rc != BCME_OK)
+		ctx->stats.rollback_failed++;
+	else
+		ctx->stats.rollback++;
+
+	return rc;
+}
+
+static void
+_dhd_wlfc_flow_control_check(athost_wl_status_info_t* ctx, struct pktq* pq, uint8 if_id)
+{
+	if ((pq->len <= WLFC_FLOWCONTROL_LOWATER) && (ctx->hostif_flow_state[if_id] == ON)) {
+		/* start traffic */
+		ctx->hostif_flow_state[if_id] = OFF;
+		/*
+		WLFC_DBGMESG(("qlen:%02d, if:%02d, ->OFF, start traffic %s()\n",
+		pq->len, if_id, __FUNCTION__));
+		*/
+		WLFC_DBGMESG(("F"));
+		/* dhd_txflowcontrol(ctx->dhdp, if_id, OFF); */
+		ctx->toggle_host_if = 0;
+	}
+	if ((pq->len >= WLFC_FLOWCONTROL_HIWATER) && (ctx->hostif_flow_state[if_id] == OFF)) {
+		/* stop traffic */
+		ctx->hostif_flow_state[if_id] = ON;
+		/*
+		WLFC_DBGMESG(("qlen:%02d, if:%02d, ->ON, stop traffic   %s()\n",
+		pq->len, if_id, __FUNCTION__));
+		*/
+		WLFC_DBGMESG(("N"));
+		/* dhd_txflowcontrol(ctx->dhdp, if_id, ON); */
+		ctx->host_ifidx = if_id;
+		ctx->toggle_host_if = 1;
+	}
+	return;
+}
+
+static int
+_dhd_wlfc_send_signalonly_packet(athost_wl_status_info_t* ctx, wlfc_mac_descriptor_t* entry,
+	uint8 ta_bmp)
+{
+	int rc = BCME_OK;
+	void* p = NULL;
+	int dummylen = ((dhd_pub_t *)ctx->dhdp)->hdrlen+ 12;
+
+	/* allocate a dummy packet */
+	p = PKTGET(ctx->osh, dummylen, TRUE);
+	if (p) {
+		PKTPULL(ctx->osh, p, dummylen);
+		DHD_PKTTAG_SET_H2DTAG(PKTTAG(p), 0);
+		_dhd_wlfc_pushheader(ctx, p, TRUE, ta_bmp, entry->mac_handle, 0);
+		DHD_PKTTAG_SETSIGNALONLY(PKTTAG(p), 1);
+#ifdef PROP_TXSTATUS_DEBUG
+		ctx->stats.signal_only_pkts_sent++;
+#endif
+		rc = dhd_bus_txdata(((dhd_pub_t *)ctx->dhdp)->bus, p);
+		if (rc != BCME_OK) {
+			PKTFREE(ctx->osh, p, TRUE);
+		}
+	}
+	else {
+		DHD_ERROR(("%s: couldn't allocate new %d-byte packet\n",
+		           __FUNCTION__, dummylen));
+		rc = BCME_NOMEM;
+	}
+	return rc;
+}
+
+/* Return TRUE if traffic availability changed */
+static bool
+_dhd_wlfc_traffic_pending_check(athost_wl_status_info_t* ctx, wlfc_mac_descriptor_t* entry,
+	int prec)
+{
+	bool rc = FALSE;
+
+	if (entry->state == WLFC_STATE_CLOSE) {
+		if ((pktq_plen(&entry->psq, (prec << 1)) == 0) &&
+			(pktq_plen(&entry->psq, ((prec << 1) + 1)) == 0)) {
+
+			if (entry->traffic_pending_bmp & NBITVAL(prec)) {
+				rc = TRUE;
+				entry->traffic_pending_bmp =
+					entry->traffic_pending_bmp & ~ NBITVAL(prec);
+			}
+		}
+		else {
+			if (!(entry->traffic_pending_bmp & NBITVAL(prec))) {
+				rc = TRUE;
+				entry->traffic_pending_bmp =
+					entry->traffic_pending_bmp | NBITVAL(prec);
+			}
+		}
+	}
+	if (rc) {
+		/* request a TIM update to firmware at the next piggyback opportunity */
+		if (entry->traffic_lastreported_bmp != entry->traffic_pending_bmp) {
+			entry->send_tim_signal = 1;
+			_dhd_wlfc_send_signalonly_packet(ctx, entry, entry->traffic_pending_bmp);
+			entry->traffic_lastreported_bmp = entry->traffic_pending_bmp;
+			entry->send_tim_signal = 0;
+		}
+		else {
+			rc = FALSE;
+		}
+	}
+	return rc;
+}
+
+static int
+_dhd_wlfc_enque_suppressed(athost_wl_status_info_t* ctx, int prec, void* p)
+{
+	wlfc_mac_descriptor_t* entry;
+
+	entry = _dhd_wlfc_find_table_entry(ctx, p);
+	if (entry == NULL) {
+		WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
+		return BCME_NOTFOUND;
+	}
+	/*
+	- suppressed packets go to sub_queue[2*prec + 1] AND
+	- delayed packets go to sub_queue[2*prec + 0] to ensure
+	order of delivery.
+	*/
+	if (WLFC_PKTQ_PENQ(&entry->psq, ((prec << 1) + 1), p) == NULL) {
+		ctx->stats.delayq_full_error++;
+		/* WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__)); */
+		WLFC_DBGMESG(("s"));
+		return BCME_ERROR;
+	}
+	/* A packet has been pushed, update traffic availability bitmap, if applicable */
+	_dhd_wlfc_traffic_pending_check(ctx, entry, prec);
+	_dhd_wlfc_flow_control_check(ctx, &entry->psq, DHD_PKTTAG_IF(PKTTAG(p)));
+	return BCME_OK;
+}
+
+static int
+_dhd_wlfc_pretx_pktprocess(athost_wl_status_info_t* ctx,
+	wlfc_mac_descriptor_t* entry, void* p, int header_needed, uint32* slot)
+{
+	int rc = BCME_OK;
+	int hslot = WLFC_HANGER_MAXITEMS;
+	bool send_tim_update = FALSE;
+	uint32 htod = 0;
+	uint8 free_ctr;
+
+	*slot = hslot;
+
+	if (entry == NULL) {
+		entry = _dhd_wlfc_find_table_entry(ctx, p);
+	}
+
+	if (entry == NULL) {
+		WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
+		return BCME_ERROR;
+	}
+	if (entry->send_tim_signal) {
+		send_tim_update = TRUE;
+		entry->send_tim_signal = 0;
+		entry->traffic_lastreported_bmp = entry->traffic_pending_bmp;
+	}
+	if (header_needed) {
+		hslot = dhd_wlfc_hanger_get_free_slot(ctx->hanger);
+		free_ctr = WLFC_SEQCOUNT(entry, DHD_PKTTAG_FIFO(PKTTAG(p)));
+		DHD_PKTTAG_SET_H2DTAG(PKTTAG(p), htod);
+	}
+	else {
+		hslot = WLFC_PKTID_HSLOT_GET(DHD_PKTTAG_H2DTAG(PKTTAG(p)));
+		free_ctr = WLFC_PKTID_FREERUNCTR_GET(DHD_PKTTAG_H2DTAG(PKTTAG(p)));
+	}
+	WLFC_PKTID_HSLOT_SET(htod, hslot);
+	WLFC_PKTID_FREERUNCTR_SET(htod, free_ctr);
+	DHD_PKTTAG_SETPKTDIR(PKTTAG(p), 1);
+	WL_TXSTATUS_SET_FLAGS(htod, WLFC_PKTFLAG_PKTFROMHOST);
+	WL_TXSTATUS_SET_FIFO(htod, DHD_PKTTAG_FIFO(PKTTAG(p)));
+	WLFC_PKTFLAG_SET_GENERATION(htod, entry->generation);
+
+	if (!DHD_PKTTAG_CREDITCHECK(PKTTAG(p))) {
+		/*
+		Indicate that this packet is being sent in response to an
+		explicit request from the firmware side.
+		*/
+		WLFC_PKTFLAG_SET_PKTREQUESTED(htod);
+	}
+	else {
+		WLFC_PKTFLAG_CLR_PKTREQUESTED(htod);
+	}
+	if (header_needed) {
+		rc = _dhd_wlfc_pushheader(ctx, p, send_tim_update,
+			entry->traffic_lastreported_bmp, entry->mac_handle, htod);
+		if (rc == BCME_OK) {
+			DHD_PKTTAG_SET_H2DTAG(PKTTAG(p), htod);
+			/*
+			a new header was created for this packet.
+			push to hanger slot and scrub q. Since bus
+			send succeeded, increment seq number as well.
+			*/
+			rc = dhd_wlfc_hanger_pushpkt(ctx->hanger, p, hslot);
+			if (rc == BCME_OK) {
+				/* increment free running sequence count */
+				WLFC_INCR_SEQCOUNT(entry, DHD_PKTTAG_FIFO(PKTTAG(p)));
+#ifdef PROP_TXSTATUS_DEBUG
+				((wlfc_hanger_t*)(ctx->hanger))->items[hslot].push_time =
+					OSL_SYSUPTIME();
+#endif
+			}
+			else {
+				WLFC_DBGMESG(("%s() hanger_pushpkt() failed, rc: %d\n",
+					__FUNCTION__, rc));
+			}
+		}
+	}
+	else {
+		/* remove old header */
+		_dhd_wlfc_pullheader(ctx, p);
+
+		hslot = WLFC_PKTID_HSLOT_GET(DHD_PKTTAG_H2DTAG(PKTTAG(p)));
+		free_ctr = WLFC_PKTID_FREERUNCTR_GET(DHD_PKTTAG_H2DTAG(PKTTAG(p)));
+		/* push new header */
+		_dhd_wlfc_pushheader(ctx, p, send_tim_update,
+			entry->traffic_lastreported_bmp, entry->mac_handle, htod);
+	}
+	*slot = hslot;
+	return rc;
+}
+
+static int
+_dhd_wlfc_is_destination_closed(athost_wl_status_info_t* ctx,
+	wlfc_mac_descriptor_t* entry, int prec)
+{
+	if (ctx->destination_entries.interfaces[entry->interface_id].iftype ==
+		WLC_E_IF_ROLE_P2P_GO) {
+		/* - destination interface is of type p2p GO.
+		For a p2pGO interface, if the destination is OPEN but the interface is
+		CLOSEd, do not send traffic. But if the dstn is CLOSEd while there is
+		destination-specific-credit left send packets. This is because the
+		firmware storing the destination-specific-requested packet in queue.
+		*/
+		if ((entry->state == WLFC_STATE_CLOSE) && (entry->requested_credit == 0) &&
+			(entry->requested_packet == 0))
+			return 1;
+	}
+	/* AP, p2p_go -> unicast desc entry, STA/p2p_cl -> interface desc. entry */
+	if (((entry->state == WLFC_STATE_CLOSE) && (entry->requested_credit == 0) &&
+		(entry->requested_packet == 0)) ||
+		(!(entry->ac_bitmap & (1 << prec))))
+		return 1;
+
+	return 0;
+}
+
+static void*
+_dhd_wlfc_deque_delayedq(athost_wl_status_info_t* ctx,
+	int prec, uint8* ac_credit_spent, uint8* needs_hdr, wlfc_mac_descriptor_t** entry_out)
+{
+	wlfc_mac_descriptor_t* entry;
+	wlfc_mac_descriptor_t* table;
+	uint8 token_pos;
+	int total_entries;
+	void* p = NULL;
+	int pout;
+	int i;
+
+	*entry_out = NULL;
+	token_pos = ctx->token_pos[prec];
+	/* most cases a packet will count against FIFO credit */
+	*ac_credit_spent = 1;
+	*needs_hdr = 1;
+
+	/* search all entries, include nodes as well as interfaces */
+	table = (wlfc_mac_descriptor_t*)&ctx->destination_entries;
+	total_entries = sizeof(ctx->destination_entries)/sizeof(wlfc_mac_descriptor_t);
+
+	for (i = 0; i < total_entries; i++) {
+		entry = &table[(token_pos + i) % total_entries];
+		if (entry->occupied) {
+			if (!_dhd_wlfc_is_destination_closed(ctx, entry, prec)) {
+				p = pktq_mdeq(&entry->psq,
+					/* higher precedence will be picked up first,
+					i.e. suppressed packets before delayed ones
+					*/
+					(NBITVAL((prec << 1) + 1) | NBITVAL((prec << 1))),
+					&pout);
+				if (p != NULL) {
+					/* did the packet come from suppress sub-queue? */
+					if (pout == ((prec << 1) + 1)) {
+						/*
+						this packet was suppressed and was sent on the bus
+						previously; this already has a header
+						*/
+						*needs_hdr = 0;
+					}
+					if (entry->requested_credit > 0) {
+						entry->requested_credit--;
+#ifdef PROP_TXSTATUS_DEBUG
+						entry->dstncredit_sent_packets++;
+#endif
+						/*
+						if the packet was pulled out while destination is in
+						closed state but had a non-zero packets requested,
+						then this should not count against the FIFO credit.
+						That is due to the fact that the firmware will
+						most likely hold onto this packet until a suitable
+						time later to push it to the appropriate  AC FIFO.
+						*/
+						if (entry->state == WLFC_STATE_CLOSE)
+							*ac_credit_spent = 0;
+					}
+					else if (entry->requested_packet > 0) {
+						entry->requested_packet--;
+						DHD_PKTTAG_SETONETIMEPKTRQST(PKTTAG(p));
+						if (entry->state == WLFC_STATE_CLOSE)
+							*ac_credit_spent = 0;
+					}
+					/* move token to ensure fair round-robin */
+					ctx->token_pos[prec] =
+						(token_pos + i + 1) % total_entries;
+					*entry_out = entry;
+					_dhd_wlfc_flow_control_check(ctx, &entry->psq,
+						DHD_PKTTAG_IF(PKTTAG(p)));
+					/*
+					A packet has been picked up, update traffic
+					availability bitmap, if applicable
+					*/
+					_dhd_wlfc_traffic_pending_check(ctx, entry, prec);
+					return p;
+				}
+			}
+		}
+	}
+	return NULL;
+}
+
+static void*
+_dhd_wlfc_deque_sendq(athost_wl_status_info_t* ctx, int prec, uint8* ac_credit_spent)
+{
+	wlfc_mac_descriptor_t* entry;
+	void* p;
+
+	/* most cases a packet will count against FIFO credit */
+	*ac_credit_spent = 1;
+
+	p = pktq_pdeq(&ctx->SENDQ, prec);
+	if (p != NULL) {
+		if (ETHER_ISMULTI(DHD_PKTTAG_DSTN(PKTTAG(p))))
+			/* bc/mc packets do not have a delay queue */
+			return p;
+
+		entry = _dhd_wlfc_find_table_entry(ctx, p);
+
+		if (entry == NULL) {
+			WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
+			return p;
+		}
+
+		while ((p != NULL) && _dhd_wlfc_is_destination_closed(ctx, entry, prec)) {
+			/*
+			- suppressed packets go to sub_queue[2*prec + 1] AND
+			- delayed packets go to sub_queue[2*prec + 0] to ensure
+			order of delivery.
+			*/
+			if (WLFC_PKTQ_PENQ(&entry->psq, (prec << 1), p) == NULL) {
+				WLFC_DBGMESG(("D"));
+				/* dhd_txcomplete(ctx->dhdp, p, FALSE); */
+				PKTFREE(ctx->osh, p, TRUE);
+				ctx->stats.delayq_full_error++;
+			}
+			/*
+			A packet has been pushed, update traffic availability bitmap,
+			if applicable
+			*/
+			_dhd_wlfc_traffic_pending_check(ctx, entry, prec);
+			_dhd_wlfc_flow_control_check(ctx, &entry->psq, DHD_PKTTAG_IF(PKTTAG(p)));
+			p = pktq_pdeq(&ctx->SENDQ, prec);
+			if (p == NULL)
+				break;
+
+			entry = _dhd_wlfc_find_table_entry(ctx, p);
+
+			if ((entry == NULL) || (ETHER_ISMULTI(DHD_PKTTAG_DSTN(PKTTAG(p))))) {
+				return p;
+			}
+		}
+		if (p) {
+			if (entry->requested_packet == 0) {
+				if (entry->requested_credit > 0)
+					entry->requested_credit--;
+			}
+			else {
+				entry->requested_packet--;
+				DHD_PKTTAG_SETONETIMEPKTRQST(PKTTAG(p));
+			}
+			if (entry->state == WLFC_STATE_CLOSE)
+				*ac_credit_spent = 0;
+#ifdef PROP_TXSTATUS_DEBUG
+			entry->dstncredit_sent_packets++;
+#endif
+		}
+		if (p)
+			_dhd_wlfc_flow_control_check(ctx, &ctx->SENDQ, DHD_PKTTAG_IF(PKTTAG(p)));
+	}
+	return p;
+}
+
+static int
+_dhd_wlfc_mac_entry_update(athost_wl_status_info_t* ctx, wlfc_mac_descriptor_t* entry,
+	ewlfc_mac_entry_action_t action, uint8 ifid, uint8 iftype, uint8* ea)
+{
+	int rc = BCME_OK;
+
+	if (action == eWLFC_MAC_ENTRY_ACTION_ADD) {
+		entry->occupied = 1;
+		entry->state = WLFC_STATE_OPEN;
+		entry->requested_credit = 0;
+		entry->interface_id = ifid;
+		entry->iftype = iftype;
+		entry->ac_bitmap = 0xff; /* update this when handling APSD */
+		/* for an interface entry we may not care about the MAC address */
+		if (ea != NULL)
+			memcpy(&entry->ea[0], ea, ETHER_ADDR_LEN);
+		pktq_init(&entry->psq, WLFC_PSQ_PREC_COUNT, WLFC_PSQ_LEN);
+	}
+	else if (action == eWLFC_MAC_ENTRY_ACTION_DEL) {
+		entry->occupied = 0;
+		entry->state = WLFC_STATE_CLOSE;
+		entry->requested_credit = 0;
+		/* enable after packets are queued-deqeued properly.
+		pktq_flush(dhd->osh, &entry->psq, FALSE, NULL, 0);
+		*/
+	}
+	return rc;
+}
+
+int
+_dhd_wlfc_borrow_credit(athost_wl_status_info_t* ctx, uint8 available_credit_map, int borrower_ac)
+{
+	int lender_ac;
+	int rc = BCME_ERROR;
+
+	if (ctx == NULL || available_credit_map == 0) {
+		WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
+		return BCME_BADARG;
+	}
+
+	/* Borrow from lowest priority available AC (including BC/MC credits) */
+	for (lender_ac = 0; lender_ac <= AC_COUNT; lender_ac++) {
+		if ((available_credit_map && (1 << lender_ac)) &&
+		   (ctx->FIFO_credit[lender_ac] > 0)) {
+			ctx->credits_borrowed[borrower_ac][lender_ac]++;
+			ctx->FIFO_credit[lender_ac]--;
+			rc = BCME_OK;
+			break;
+		}
+	}
+
+	return rc;
+}
+
+int
+dhd_wlfc_interface_entry_update(void* state,
+	ewlfc_mac_entry_action_t action, uint8 ifid, uint8 iftype, uint8* ea)
+{
+	athost_wl_status_info_t* ctx = (athost_wl_status_info_t*)state;
+	wlfc_mac_descriptor_t* entry;
+
+	if (ifid >= WLFC_MAX_IFNUM)
+		return BCME_BADARG;
+
+	entry = &ctx->destination_entries.interfaces[ifid];
+	return _dhd_wlfc_mac_entry_update(ctx, entry, action, ifid, iftype, ea);
+}
+
+int
+dhd_wlfc_FIFOcreditmap_update(void* state, uint8* credits)
+{
+	athost_wl_status_info_t* ctx = (athost_wl_status_info_t*)state;
+
+	/* update the AC FIFO credit map */
+	ctx->FIFO_credit[0] = credits[0];
+	ctx->FIFO_credit[1] = credits[1];
+	ctx->FIFO_credit[2] = credits[2];
+	ctx->FIFO_credit[3] = credits[3];
+	/* credit for bc/mc packets */
+	ctx->FIFO_credit[4] = credits[4];
+	/* credit for ATIM FIFO is not used yet. */
+	ctx->FIFO_credit[5] = 0;
+	return BCME_OK;
+}
+
+int
+dhd_wlfc_enque_sendq(void* state, int prec, void* p)
+{
+	athost_wl_status_info_t* ctx = (athost_wl_status_info_t*)state;
+
+	if ((state == NULL) ||
+		/* prec = AC_COUNT is used for bc/mc queue */
+		(prec > AC_COUNT) ||
+		(p == NULL)) {
+		WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
+		return BCME_BADARG;
+	}
+	if (FALSE == dhd_prec_enq(ctx->dhdp, &ctx->SENDQ, p, prec)) {
+		ctx->stats.sendq_full_error++;
+		/*
+		WLFC_DBGMESG(("Error: %s():%d, qlen:%d\n",
+		__FUNCTION__, __LINE__, ctx->SENDQ.len));
+		*/
+		WLFC_HOST_FIFO_DROPPEDCTR_INC(ctx, prec);
+		WLFC_DBGMESG(("Q"));
+		PKTFREE(ctx->osh, p, TRUE);
+		return BCME_ERROR;
+	}
+	ctx->stats.pktin++;
+	/* _dhd_wlfc_flow_control_check(ctx, &ctx->SENDQ, DHD_PKTTAG_IF(PKTTAG(p))); */
+	return BCME_OK;
+}
+
+int
+_dhd_wlfc_handle_packet_commit(athost_wl_status_info_t* ctx, int ac,
+    dhd_wlfc_commit_info_t *commit_info, f_commitpkt_t fcommit, void* commit_ctx)
+{
+	uint32 hslot;
+	int	rc;
+
+	/*
+		if ac_fifo_credit_spent = 0
+
+		This packet will not count against the FIFO credit.
+		To ensure the txstatus corresponding to this packet
+		does not provide an implied credit (default behavior)
+		mark the packet accordingly.
+
+		if ac_fifo_credit_spent = 1
+
+		This is a normal packet and it counts against the FIFO
+		credit count.
+	*/
+	DHD_PKTTAG_SETCREDITCHECK(PKTTAG(commit_info->p), commit_info->ac_fifo_credit_spent);
+	rc = _dhd_wlfc_pretx_pktprocess(ctx, commit_info->mac_entry, commit_info->p,
+	     commit_info->needs_hdr, &hslot);
+
+	if (rc == BCME_OK)
+		rc = fcommit(commit_ctx, commit_info->p);
+	else
+		ctx->stats.generic_error++;
+
+	if (rc == BCME_OK) {
+		ctx->stats.pkt2bus++;
+		if (commit_info->ac_fifo_credit_spent) {
+			ctx->stats.sendq_pkts[ac]++;
+			WLFC_HOST_FIFO_CREDIT_INC_SENTCTRS(ctx, ac);
+		}
+	}
+	else {
+		/*
+		   bus commit has failed, rollback.
+		   - remove wl-header for a delayed packet
+		   - save wl-header header for suppressed packets
+		*/
+		rc = _dhd_wlfc_rollback_packet_toq(ctx,	commit_info->p,
+		     (commit_info->pkt_type), hslot);
+		if (rc != BCME_OK)
+			ctx->stats.rollback_failed++;
+
+		rc = BCME_ERROR;
+	}
+
+	return rc;
+}
+
+int
+dhd_wlfc_commit_packets(void* state, f_commitpkt_t fcommit, void* commit_ctx)
+{
+	int ac;
+	int credit;
+	int rc;
+	dhd_wlfc_commit_info_t  commit_info;
+	athost_wl_status_info_t* ctx = (athost_wl_status_info_t*)state;
+	int credit_count = 0;
+	int bus_retry_count = 0;
+	uint8 ac_available = 0;  /* Bitmask for 4 ACs + BC/MC */
+
+	if ((state == NULL) ||
+		(fcommit == NULL)) {
+		WLFC_DBGMESG(("Error: %s():%d\n", __FUNCTION__, __LINE__));
+		return BCME_BADARG;
+	}
+
+	memset(&commit_info, 0, sizeof(commit_info));
+
+	/*
+	Commit packets for regular AC traffic. Higher priority first.
+	First, use up FIFO credits available to each AC. Based on distribution
+	and credits left, borrow from other ACs as applicable
+
+	-NOTE:
+	If the bus between the host and firmware is overwhelmed by the
+	traffic from host, it is possible that higher priority traffic
+	starves the lower priority queue. If that occurs often, we may
+	have to employ weighted round-robin or ucode scheme to avoid
+	low priority packet starvation.
+	*/
+
+	for (ac = AC_COUNT; ac >= 0; ac--) {
+
+		int initial_credit_count = ctx->FIFO_credit[ac];
+
+		for (credit = 0; credit < ctx->FIFO_credit[ac];) {
+			commit_info.p = _dhd_wlfc_deque_delayedq(ctx, ac,
+			                &(commit_info.ac_fifo_credit_spent),
+			                &(commit_info.needs_hdr),
+			                &(commit_info.mac_entry));
+
+			if (commit_info.p == NULL)
+				break;
+
+			commit_info.pkt_type = (commit_info.needs_hdr) ? eWLFC_PKTTYPE_DELAYED :
+				eWLFC_PKTTYPE_SUPPRESSED;
+
+			rc = _dhd_wlfc_handle_packet_commit(ctx, ac, &commit_info,
+			     fcommit, commit_ctx);
+
+			/* Bus commits may fail (e.g. flow control); abort after retries */
+			if (rc == BCME_OK) {
+				if (commit_info.ac_fifo_credit_spent) {
+					credit++;
+				}
+			}
+			else {
+				bus_retry_count++;
+				if (bus_retry_count >= BUS_RETRIES) {
+					DHD_ERROR(("dhd_wlfc_commit_packets(): bus error\n"));
+					ctx->FIFO_credit[ac] -= credit;
+					return rc;
+				}
+			}
+		}
+
+		ctx->FIFO_credit[ac] -= credit;
+
+		/* packets from SENDQ are fresh and they'd need header and have no MAC entry */
+		commit_info.needs_hdr = 1;
+		commit_info.mac_entry = NULL;
+		commit_info.pkt_type = eWLFC_PKTTYPE_NEW;
+
+		for (credit = 0; credit < ctx->FIFO_credit[ac];) {
+			commit_info.p = _dhd_wlfc_deque_sendq(ctx, ac,
+			                &(commit_info.ac_fifo_credit_spent));
+			if (commit_info.p == NULL)
+				break;
+
+			rc = _dhd_wlfc_handle_packet_commit(ctx, ac, &commit_info,
+			     fcommit, commit_ctx);
+
+			/* Bus commits may fail (e.g. flow control); abort after retries */
+			if (rc == BCME_OK) {
+				if (commit_info.ac_fifo_credit_spent) {
+					credit++;
+				}
+			}
+			else {
+				bus_retry_count++;
+				if (bus_retry_count >= BUS_RETRIES) {
+					DHD_ERROR(("dhd_wlfc_commit_packets(): bus error\n"));
+					ctx->FIFO_credit[ac] -= credit;
+					return rc;
+				}
+			}
+		}
+
+		ctx->FIFO_credit[ac] -= credit;
+
+		/* If no credits were used, the queue is idle and can be re-used
+		   Note that resv credits cannot be borrowed
+		   */
+		if (initial_credit_count == ctx->FIFO_credit[ac]) {
+			ac_available |= (1 << ac);
+			credit_count += ctx->FIFO_credit[ac];
+		}
+	}
+
+	/* We borrow only for AC_BE and only if no other traffic seen for DEFER_PERIOD
+
+	   Note that (ac_available & WLFC_AC_BE_TRAFFIC_ONLY) is done to:
+	   a) ignore BC/MC for deferring borrow
+	   b) ignore AC_BE being available along with other ACs
+		  (this should happen only for pure BC/MC traffic)
+
+	   i.e. AC_VI, AC_VO, AC_BK all MUST be available (i.e. no traffic) and
+	   we do not care if AC_BE and BC/MC are available or not
+	   */
+	if ((ac_available & WLFC_AC_BE_TRAFFIC_ONLY) == WLFC_AC_BE_TRAFFIC_ONLY) {
+
+		if (ctx->allow_credit_borrow) {
+			ac = 1;  /* Set ac to AC_BE and borrow credits */
+		}
+		else {
+			int delta;
+			int curr_t = OSL_SYSUPTIME();
+
+			if (curr_t > ctx->borrow_defer_timestamp)
+				delta = curr_t - ctx->borrow_defer_timestamp;
+			else
+				delta = 0xffffffff + curr_t - ctx->borrow_defer_timestamp;
+
+			if (delta >= WLFC_BORROW_DEFER_PERIOD_MS) {
+				/* Reset borrow but defer to next iteration (defensive borrowing) */
+				ctx->allow_credit_borrow = TRUE;
+				ctx->borrow_defer_timestamp = 0;
+			}
+			return BCME_OK;
+		}
+	}
+	else {
+		/* If we have multiple AC traffic, turn off borrowing, mark time and bail out */
+		ctx->allow_credit_borrow = FALSE;
+		ctx->borrow_defer_timestamp = OSL_SYSUPTIME();
+		return BCME_OK;
+	}
+
+	/* At this point, borrow all credits only for "ac" (which should be set above to AC_BE)
+	   Generically use "ac" only in case we extend to all ACs in future
+	   */
+	for (; (credit_count > 0);) {
+
+		commit_info.p = _dhd_wlfc_deque_delayedq(ctx, ac,
+		                &(commit_info.ac_fifo_credit_spent),
+		                &(commit_info.needs_hdr),
+		                &(commit_info.mac_entry));
+		if (commit_info.p == NULL)
+			break;
+
+		commit_info.pkt_type = (commit_info.needs_hdr) ? eWLFC_PKTTYPE_DELAYED :
+			eWLFC_PKTTYPE_SUPPRESSED;
+
+		rc = _dhd_wlfc_handle_packet_commit(ctx, ac, &commit_info,
+		     fcommit, commit_ctx);
+
+		/* Bus commits may fail (e.g. flow control); abort after retries */
+		if (rc == BCME_OK) {
+			if (commit_info.ac_fifo_credit_spent) {
+				(void) _dhd_wlfc_borrow_credit(ctx, ac_available, ac);
+				credit_count--;
+			}
+		}
+		else {
+			bus_retry_count++;
+			if (bus_retry_count >= BUS_RETRIES) {
+				DHD_ERROR(("dhd_wlfc_commit_packets(): bus error\n"));
+				return rc;
+			}
+		}
+	}
+
+	/* packets from SENDQ are fresh and they'd need header and have no MAC entry */
+	commit_info.needs_hdr = 1;
+	commit_info.mac_entry = NULL;
+	commit_info.pkt_type = eWLFC_PKTTYPE_NEW;
+
+	for (; (credit_count > 0);) {
+
+		commit_info.p = _dhd_wlfc_deque_sendq(ctx, ac,
+		                &(commit_info.ac_fifo_credit_spent));
+		if (commit_info.p == NULL)
+			break;
+
+		rc = _dhd_wlfc_handle_packet_commit(ctx, ac, &commit_info,
+		     fcommit, commit_ctx);
+
+		/* Bus commits may fail (e.g. flow control); abort after retries */
+		if (rc == BCME_OK) {
+			if (commit_info.ac_fifo_credit_spent) {
+				(void) _dhd_wlfc_borrow_credit(ctx, ac_available, ac);
+				credit_count--;
+			}
+		}
+		else {
+			bus_retry_count++;
+			if (bus_retry_count >= BUS_RETRIES) {
+				DHD_ERROR(("dhd_wlfc_commit_packets(): bus error\n"));
+				return rc;
+			}
+		}
+	}
+
+	return BCME_OK;
+}
+
+static uint8
+dhd_wlfc_find_mac_desc_id_from_mac(dhd_pub_t *dhdp, uint8* ea)
+{
+	wlfc_mac_descriptor_t* table =
+		((athost_wl_status_info_t*)dhdp->wlfc_state)->destination_entries.nodes;
+	uint8 table_index;
+
+	if (ea != NULL) {
+		for (table_index = 0; table_index < WLFC_MAC_DESC_TABLE_SIZE; table_index++) {
+			if ((memcmp(ea, &table[table_index].ea[0], ETHER_ADDR_LEN) == 0) &&
+				table[table_index].occupied)
+				return table_index;
+		}
+	}
+	return WLFC_MAC_DESC_ID_INVALID;
+}
+
+void
+dhd_wlfc_txcomplete(dhd_pub_t *dhd, void *txp, bool success)
+{
+	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
+		dhd->wlfc_state;
+	void* p;
+	int fifo_id;
+
+	if (DHD_PKTTAG_SIGNALONLY(PKTTAG(txp))) {
+#ifdef PROP_TXSTATUS_DEBUG
+		wlfc->stats.signal_only_pkts_freed++;
+#endif
+		/* is this a signal-only packet? */
+		PKTFREE(wlfc->osh, txp, TRUE);
+		return;
+	}
+	if (!success) {
+		WLFC_DBGMESG(("At: %s():%d, bus_complete() failure for %p, htod_tag:0x%08x\n",
+			__FUNCTION__, __LINE__, txp, DHD_PKTTAG_H2DTAG(PKTTAG(txp))));
+		dhd_wlfc_hanger_poppkt(wlfc->hanger, WLFC_PKTID_HSLOT_GET(DHD_PKTTAG_H2DTAG
+			(PKTTAG(txp))), &p, 1);
+
+		/* indicate failure and free the packet */
+		dhd_txcomplete(dhd, txp, FALSE);
+
+		/* return the credit, if necessary */
+		if (DHD_PKTTAG_CREDITCHECK(PKTTAG(txp))) {
+			int lender, credit_returned = 0; /* Note that borrower is fifo_id */
+
+			fifo_id = DHD_PKTTAG_FIFO(PKTTAG(txp));
+
+			/* Return credits to highest priority lender first */
+			for (lender = AC_COUNT; lender >= 0; lender--) {
+				if (wlfc->credits_borrowed[fifo_id][lender] > 0) {
+					wlfc->FIFO_credit[lender]++;
+					wlfc->credits_borrowed[fifo_id][lender]--;
+					credit_returned = 1;
+					break;
+				}
+			}
+
+			if (!credit_returned) {
+				wlfc->FIFO_credit[fifo_id]++;
+			}
+		}
+
+		PKTFREE(wlfc->osh, txp, TRUE);
+	}
+	return;
+}
+
+/* Handle discard or suppress indication */
+static int
+dhd_wlfc_txstatus_update(dhd_pub_t *dhd, uint8* pkt_info)
+{
+	uint8 	status_flag;
+	uint32	status;
+	int		ret;
+	int		remove_from_hanger = 1;
+	void*	pktbuf;
+	uint8	fifo_id;
+	wlfc_mac_descriptor_t* entry = NULL;
+	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
+		dhd->wlfc_state;
+
+	memcpy(&status, pkt_info, sizeof(uint32));
+	status_flag = WL_TXSTATUS_GET_FLAGS(status);
+	wlfc->stats.txstatus_in++;
+
+	if (status_flag == WLFC_CTL_PKTFLAG_DISCARD) {
+		wlfc->stats.pkt_freed++;
+	}
+
+	else if (status_flag == WLFC_CTL_PKTFLAG_D11SUPPRESS) {
+		wlfc->stats.d11_suppress++;
+		remove_from_hanger = 0;
+	}
+
+	else if (status_flag == WLFC_CTL_PKTFLAG_WLSUPPRESS) {
+		wlfc->stats.wl_suppress++;
+		remove_from_hanger = 0;
+	}
+
+	else if (status_flag == WLFC_CTL_PKTFLAG_TOSSED_BYWLC) {
+		wlfc->stats.wlc_tossed_pkts++;
+	}
+
+	ret = dhd_wlfc_hanger_poppkt(wlfc->hanger,
+		WLFC_PKTID_HSLOT_GET(status), &pktbuf, remove_from_hanger);
+	if (ret != BCME_OK) {
+		/* do something */
+		return ret;
+	}
+
+	if (!remove_from_hanger) {
+		/* this packet was suppressed */
+
+		entry = _dhd_wlfc_find_table_entry(wlfc, pktbuf);
+		entry->generation = WLFC_PKTID_GEN(status);
+	}
+
+#ifdef PROP_TXSTATUS_DEBUG
+	{
+		uint32 new_t = OSL_SYSUPTIME();
+		uint32 old_t;
+		uint32 delta;
+		old_t = ((wlfc_hanger_t*)(wlfc->hanger))->items[
+			WLFC_PKTID_HSLOT_GET(status)].push_time;
+
+
+		wlfc->stats.latency_sample_count++;
+		if (new_t > old_t)
+			delta = new_t - old_t;
+		else
+			delta = 0xffffffff + new_t - old_t;
+		wlfc->stats.total_status_latency += delta;
+		wlfc->stats.latency_most_recent = delta;
+
+		wlfc->stats.deltas[wlfc->stats.idx_delta++] = delta;
+		if (wlfc->stats.idx_delta == sizeof(wlfc->stats.deltas)/sizeof(uint32))
+			wlfc->stats.idx_delta = 0;
+	}
+#endif /* PROP_TXSTATUS_DEBUG */
+
+	fifo_id = DHD_PKTTAG_FIFO(PKTTAG(pktbuf));
+
+	/* pick up the implicit credit from this packet */
+	if (DHD_PKTTAG_CREDITCHECK(PKTTAG(pktbuf))) {
+		if (wlfc->proptxstatus_mode == WLFC_FCMODE_IMPLIED_CREDIT) {
+
+			int lender, credit_returned = 0; /* Note that borrower is fifo_id */
+
+			/* Return credits to highest priority lender first */
+			for (lender = AC_COUNT; lender >= 0; lender--)	{
+				if (wlfc->credits_borrowed[fifo_id][lender] > 0) {
+					wlfc->FIFO_credit[lender]++;
+					wlfc->credits_borrowed[fifo_id][lender]--;
+					credit_returned = 1;
+					break;
+				}
+			}
+
+			if (!credit_returned) {
+				wlfc->FIFO_credit[fifo_id]++;
+			}
+		}
+	}
+	else {
+		/*
+		if this packet did not count against FIFO credit, it must have
+		taken a requested_credit from the destination entry (for pspoll etc.)
+		*/
+		if (!entry) {
+
+			entry = _dhd_wlfc_find_table_entry(wlfc, pktbuf);
+		}
+		if (!DHD_PKTTAG_ONETIMEPKTRQST(PKTTAG(pktbuf)))
+			entry->requested_credit++;
+#ifdef PROP_TXSTATUS_DEBUG
+		entry->dstncredit_acks++;
+#endif
+	}
+	if ((status_flag == WLFC_CTL_PKTFLAG_D11SUPPRESS) ||
+		(status_flag == WLFC_CTL_PKTFLAG_WLSUPPRESS)) {
+		ret = _dhd_wlfc_enque_suppressed(wlfc, fifo_id, pktbuf);
+		if (ret != BCME_OK) {
+			/* delay q is full, drop this packet */
+			dhd_wlfc_hanger_poppkt(wlfc->hanger, WLFC_PKTID_HSLOT_GET(status),
+			&pktbuf, 1);
+
+			/* indicate failure and free the packet */
+			dhd_txcomplete(dhd, pktbuf, FALSE);
+			PKTFREE(wlfc->osh, pktbuf, TRUE);
+		}
+	}
+	else {
+		dhd_txcomplete(dhd, pktbuf, TRUE);
+		/* free the packet */
+		PKTFREE(wlfc->osh, pktbuf, TRUE);
+	}
+	return BCME_OK;
+}
+
+static int
+dhd_wlfc_fifocreditback_indicate(dhd_pub_t *dhd, uint8* credits)
+{
+	int i;
+	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
+		dhd->wlfc_state;
+	for (i = 0; i < WLFC_CTL_VALUE_LEN_FIFO_CREDITBACK; i++) {
+#ifdef PROP_TXSTATUS_DEBUG
+		wlfc->stats.fifo_credits_back[i] += credits[i];
+#endif
+		/* update FIFO credits */
+		if (wlfc->proptxstatus_mode == WLFC_FCMODE_EXPLICIT_CREDIT)
+		{
+			int lender; /* Note that borrower is i */
+
+			/* Return credits to highest priority lender first */
+			for (lender = AC_COUNT; (lender >= 0) && (credits[i] > 0); lender--) {
+				if (wlfc->credits_borrowed[i][lender] > 0) {
+					if (credits[i] >= wlfc->credits_borrowed[i][lender]) {
+						credits[i] -= wlfc->credits_borrowed[i][lender];
+						wlfc->FIFO_credit[lender] +=
+						    wlfc->credits_borrowed[i][lender];
+						wlfc->credits_borrowed[i][lender] = 0;
+					}
+					else {
+						wlfc->credits_borrowed[i][lender] -= credits[i];
+						wlfc->FIFO_credit[lender] += credits[i];
+						credits[i] = 0;
+					}
+				}
+			}
+
+			/* If we have more credits left over, these must belong to the AC */
+			if (credits[i] > 0) {
+				wlfc->FIFO_credit[i] += credits[i];
+			}
+		}
+	}
+
+	return BCME_OK;
+}
+
+static int
+dhd_wlfc_rssi_indicate(dhd_pub_t *dhd, uint8* rssi)
+{
+	(void)dhd;
+	(void)rssi;
+	return BCME_OK;
+}
+
+static int
+dhd_wlfc_mac_table_update(dhd_pub_t *dhd, uint8* value, uint8 type)
+{
+	int rc;
+	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
+		dhd->wlfc_state;
+	wlfc_mac_descriptor_t* table;
+	uint8 existing_index;
+	uint8 table_index;
+	uint8 ifid;
+	uint8* ea;
+
+	WLFC_DBGMESG(("%s(), mac [%02x:%02x:%02x:%02x:%02x:%02x],%s,idx:%d,id:0x%02x\n",
+		__FUNCTION__, value[2], value[3], value[4], value[5], value[6], value[7],
+		((type == WLFC_CTL_TYPE_MACDESC_ADD) ? "ADD":"DEL"),
+		WLFC_MAC_DESC_GET_LOOKUP_INDEX(value[0]), value[0]));
+
+	table = wlfc->destination_entries.nodes;
+	table_index = WLFC_MAC_DESC_GET_LOOKUP_INDEX(value[0]);
+	ifid = value[1];
+	ea = &value[2];
+
+	if (type == WLFC_CTL_TYPE_MACDESC_ADD) {
+		existing_index = dhd_wlfc_find_mac_desc_id_from_mac(dhd, &value[2]);
+		if (existing_index == WLFC_MAC_DESC_ID_INVALID) {
+			/* this MAC entry does not exist, create one */
+			if (!table[table_index].occupied) {
+				table[table_index].mac_handle = value[0];
+				rc = _dhd_wlfc_mac_entry_update(wlfc, &table[table_index],
+				eWLFC_MAC_ENTRY_ACTION_ADD, ifid,
+				wlfc->destination_entries.interfaces[ifid].iftype,
+				ea);
+			}
+			else {
+				/* the space should have been empty, but it's not */
+				wlfc->stats.mac_update_failed++;
+			}
+		}
+		else {
+			/*
+			there is an existing entry, move it to new index
+			if necessary.
+			*/
+			if (existing_index != table_index) {
+				/* if we already have an entry, free the old one */
+				table[existing_index].occupied = 0;
+				table[existing_index].state = WLFC_STATE_CLOSE;
+				table[existing_index].requested_credit = 0;
+				table[existing_index].interface_id = 0;
+			}
+		}
+	}
+	if (type == WLFC_CTL_TYPE_MACDESC_DEL) {
+		if (table[table_index].occupied) {
+				rc = _dhd_wlfc_mac_entry_update(wlfc, &table[table_index],
+					eWLFC_MAC_ENTRY_ACTION_DEL, ifid,
+					wlfc->destination_entries.interfaces[ifid].iftype,
+					ea);
+		}
+		else {
+			/* the space should have been occupied, but it's not */
+			wlfc->stats.mac_update_failed++;
+		}
+	}
+	return BCME_OK;
+}
+
+static int
+dhd_wlfc_psmode_update(dhd_pub_t *dhd, uint8* value, uint8 type)
+{
+	/* Handle PS on/off indication */
+	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
+		dhd->wlfc_state;
+	wlfc_mac_descriptor_t* table;
+	wlfc_mac_descriptor_t* desc;
+	uint8 mac_handle = value[0];
+	int i;
+
+	table = wlfc->destination_entries.nodes;
+	desc = &table[WLFC_MAC_DESC_GET_LOOKUP_INDEX(mac_handle)];
+	if (desc->occupied) {
+		/* a fresh PS mode should wipe old ps credits? */
+		desc->requested_credit = 0;
+		if (type == WLFC_CTL_TYPE_MAC_OPEN) {
+			desc->state = WLFC_STATE_OPEN;
+			DHD_WLFC_CTRINC_MAC_OPEN(desc);
+		}
+		else {
+			desc->state = WLFC_STATE_CLOSE;
+			DHD_WLFC_CTRINC_MAC_CLOSE(desc);
+			/*
+			Indicate to firmware if there is any traffic pending.
+			*/
+			for (i = AC_BE; i < AC_COUNT; i++) {
+				_dhd_wlfc_traffic_pending_check(wlfc, desc, i);
+			}
+		}
+	}
+	else {
+		wlfc->stats.psmode_update_failed++;
+	}
+	return BCME_OK;
+}
+
+static int
+dhd_wlfc_interface_update(dhd_pub_t *dhd, uint8* value, uint8 type)
+{
+	/* Handle PS on/off indication */
+	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
+		dhd->wlfc_state;
+	wlfc_mac_descriptor_t* table;
+	uint8 if_id = value[0];
+
+	if (if_id < WLFC_MAX_IFNUM) {
+		table = wlfc->destination_entries.interfaces;
+		if (table[if_id].occupied) {
+			if (type == WLFC_CTL_TYPE_INTERFACE_OPEN) {
+				table[if_id].state = WLFC_STATE_OPEN;
+				/* WLFC_DBGMESG(("INTERFACE[%d] OPEN\n", if_id)); */
+			}
+			else {
+				table[if_id].state = WLFC_STATE_CLOSE;
+				/* WLFC_DBGMESG(("INTERFACE[%d] CLOSE\n", if_id)); */
+			}
+			return BCME_OK;
+		}
+	}
+	wlfc->stats.interface_update_failed++;
+
+	return BCME_OK;
+}
+
+static int
+dhd_wlfc_credit_request(dhd_pub_t *dhd, uint8* value)
+{
+	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
+		dhd->wlfc_state;
+	wlfc_mac_descriptor_t* table;
+	wlfc_mac_descriptor_t* desc;
+	uint8 mac_handle;
+	uint8 credit;
+
+	table = wlfc->destination_entries.nodes;
+	mac_handle = value[1];
+	credit = value[0];
+
+	desc = &table[WLFC_MAC_DESC_GET_LOOKUP_INDEX(mac_handle)];
+	if (desc->occupied) {
+		desc->requested_credit = credit;
+
+		desc->ac_bitmap = value[2];
+	}
+	else {
+		wlfc->stats.credit_request_failed++;
+	}
+	return BCME_OK;
+}
+
+static int
+dhd_wlfc_packet_request(dhd_pub_t *dhd, uint8* value)
+{
+	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
+		dhd->wlfc_state;
+	wlfc_mac_descriptor_t* table;
+	wlfc_mac_descriptor_t* desc;
+	uint8 mac_handle;
+	uint8 packet_count;
+
+	table = wlfc->destination_entries.nodes;
+	mac_handle = value[1];
+	packet_count = value[0];
+
+	desc = &table[WLFC_MAC_DESC_GET_LOOKUP_INDEX(mac_handle)];
+	if (desc->occupied) {
+		desc->requested_packet = packet_count;
+
+		desc->ac_bitmap = value[2];
+	}
+	else {
+		wlfc->stats.packet_request_failed++;
+	}
+	return BCME_OK;
+}
+
+static int
+dhd_wlfc_parse_header_info(dhd_pub_t *dhd, void* pktbuf, int tlv_hdr_len)
+{
+	uint8 type, len;
+	uint8* value;
+	uint8* tmpbuf;
+	uint16 remainder = tlv_hdr_len;
+	uint16 processed = 0;
+	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
+		dhd->wlfc_state;
+	tmpbuf = (uint8*)PKTDATA(dhd->osh, pktbuf);
+	if (remainder) {
+		while ((processed < (WLFC_MAX_PENDING_DATALEN * 2)) && (remainder > 0)) {
+			type = tmpbuf[processed];
+			if (type == WLFC_CTL_TYPE_FILLER) {
+				remainder -= 1;
+				processed += 1;
+				continue;
+			}
+
+			len  = tmpbuf[processed + 1];
+			value = &tmpbuf[processed + 2];
+
+			if (remainder < (2 + len))
+				break;
+
+			remainder -= 2 + len;
+			processed += 2 + len;
+			if (type == WLFC_CTL_TYPE_TXSTATUS)
+				dhd_wlfc_txstatus_update(dhd, value);
+
+			else if (type == WLFC_CTL_TYPE_FIFO_CREDITBACK)
+				dhd_wlfc_fifocreditback_indicate(dhd, value);
+
+			else if (type == WLFC_CTL_TYPE_RSSI)
+				dhd_wlfc_rssi_indicate(dhd, value);
+
+			else if (type == WLFC_CTL_TYPE_MAC_REQUEST_CREDIT)
+				dhd_wlfc_credit_request(dhd, value);
+
+			else if (type == WLFC_CTL_TYPE_MAC_REQUEST_PACKET)
+				dhd_wlfc_packet_request(dhd, value);
+
+			else if ((type == WLFC_CTL_TYPE_MAC_OPEN) ||
+				(type == WLFC_CTL_TYPE_MAC_CLOSE))
+				dhd_wlfc_psmode_update(dhd, value, type);
+
+			else if ((type == WLFC_CTL_TYPE_MACDESC_ADD) ||
+				(type == WLFC_CTL_TYPE_MACDESC_DEL))
+				dhd_wlfc_mac_table_update(dhd, value, type);
+
+			else if ((type == WLFC_CTL_TYPE_INTERFACE_OPEN) ||
+				(type == WLFC_CTL_TYPE_INTERFACE_CLOSE)) {
+				dhd_wlfc_interface_update(dhd, value, type);
+			}
+		}
+		if (remainder != 0) {
+			/* trouble..., something is not right */
+			wlfc->stats.tlv_parse_failed++;
+		}
+	}
+	return BCME_OK;
+}
+
+int
+dhd_wlfc_init(dhd_pub_t *dhd)
+{
+	char iovbuf[12]; /* Room for "tlv" + '\0' + parameter */
+	/* enable all signals & indicate host proptxstatus logic is active */
+	uint32 tlv = dhd->wlfc_enabled?
+		WLFC_FLAGS_RSSI_SIGNALS |
+		WLFC_FLAGS_XONXOFF_SIGNALS |
+		WLFC_FLAGS_CREDIT_STATUS_SIGNALS |
+		WLFC_FLAGS_HOST_PROPTXSTATUS_ACTIVE : 0;
+
+	dhd->wlfc_state  = NULL;
+
+	/*
+	try to enable/disable signaling by sending "tlv" iovar. if that fails,
+	fallback to no flow control? Print a message for now.
+	*/
+
+	/* enable proptxtstatus signaling by default */
+	bcm_mkiovar("tlv", (char *)&tlv, 4, iovbuf, sizeof(iovbuf));
+	if (dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0) < 0) {
+		DHD_ERROR(("dhd_wlfc_init(): failed to enable/disable bdcv2 tlv signaling\n"));
+	}
+	else {
+		/*
+		Leaving the message for now, it should be removed after a while; once
+		the tlv situation is stable.
+		*/
+		DHD_ERROR(("dhd_wlfc_init(): successfully %s bdcv2 tlv signaling, %d\n",
+			dhd->wlfc_enabled?"enabled":"disabled", tlv));
+	}
+	return BCME_OK;
+}
+
+int
+dhd_wlfc_enable(dhd_pub_t *dhd)
+{
+	int i;
+	athost_wl_status_info_t* wlfc;
+
+	if (!dhd->wlfc_enabled || dhd->wlfc_state)
+		return BCME_OK;
+
+	/* allocate space to track txstatus propagated from firmware */
+	dhd->wlfc_state = MALLOC(dhd->osh, sizeof(athost_wl_status_info_t));
+	if (dhd->wlfc_state == NULL)
+		return BCME_NOMEM;
+
+	/* initialize state space */
+	wlfc = (athost_wl_status_info_t*)dhd->wlfc_state;
+	memset(wlfc, 0, sizeof(athost_wl_status_info_t));
+
+	/* remember osh & dhdp */
+	wlfc->osh = dhd->osh;
+	wlfc->dhdp = dhd;
+
+	wlfc->hanger =
+		dhd_wlfc_hanger_create(dhd->osh, WLFC_HANGER_MAXITEMS);
+	if (wlfc->hanger == NULL) {
+		MFREE(dhd->osh, dhd->wlfc_state, sizeof(athost_wl_status_info_t));
+		dhd->wlfc_state = NULL;
+		return BCME_NOMEM;
+	}
+
+	/* initialize all interfaces to accept traffic */
+	for (i = 0; i < WLFC_MAX_IFNUM; i++) {
+		wlfc->hostif_flow_state[i] = OFF;
+	}
+
+	/*
+	create the SENDQ containing
+	sub-queues for all AC precedences + 1 for bc/mc traffic
+	*/
+	pktq_init(&wlfc->SENDQ, (AC_COUNT + 1), WLFC_SENDQ_LEN);
+
+	wlfc->destination_entries.other.state = WLFC_STATE_OPEN;
+	/* bc/mc FIFO is always open [credit aside], i.e. b[5] */
+	wlfc->destination_entries.other.ac_bitmap = 0x1f;
+	wlfc->destination_entries.other.interface_id = 0;
+
+	wlfc->proptxstatus_mode = WLFC_FCMODE_EXPLICIT_CREDIT;
+
+	wlfc->allow_credit_borrow = TRUE;
+	wlfc->borrow_defer_timestamp = 0;
+
+	return BCME_OK;
+}
+
+/* release all packet resources */
+void
+dhd_wlfc_cleanup(dhd_pub_t *dhd)
+{
+	int i;
+	int total_entries;
+	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
+		dhd->wlfc_state;
+	wlfc_mac_descriptor_t* table;
+	wlfc_hanger_t* h;
+
+	if (dhd->wlfc_state == NULL)
+		return;
+
+	total_entries = sizeof(wlfc->destination_entries)/sizeof(wlfc_mac_descriptor_t);
+	/* search all entries, include nodes as well as interfaces */
+	table = (wlfc_mac_descriptor_t*)&wlfc->destination_entries;
+
+	for (i = 0; i < total_entries; i++) {
+		if (table[i].occupied) {
+			if (table[i].psq.len) {
+				WLFC_DBGMESG(("%s(): DELAYQ[%d].len = %d\n",
+					__FUNCTION__, i, table[i].psq.len));
+				/* release packets held in DELAYQ */
+				pktq_flush(wlfc->osh, &table[i].psq, TRUE, NULL, 0);
+			}
+			table[i].occupied = 0;
+		}
+	}
+	/* release packets held in SENDQ */
+	if (wlfc->SENDQ.len)
+		pktq_flush(wlfc->osh, &wlfc->SENDQ, TRUE, NULL, 0);
+	/* any in the hanger? */
+	h = (wlfc_hanger_t*)wlfc->hanger;
+	for (i = 0; i < h->max_items; i++) {
+		if (h->items[i].state == WLFC_HANGER_ITEM_STATE_INUSE) {
+			PKTFREE(wlfc->osh, h->items[i].pkt, TRUE);
+		}
+	}
+	return;
+}
+
+void
+dhd_wlfc_deinit(dhd_pub_t *dhd)
+{
+	/* cleanup all psq related resources */
+	athost_wl_status_info_t* wlfc = (athost_wl_status_info_t*)
+		dhd->wlfc_state;
+
+	if (dhd->wlfc_state == NULL)
+		return;
+
+#ifdef PROP_TXSTATUS_DEBUG
+	{
+		int i;
+		wlfc_hanger_t* h = (wlfc_hanger_t*)wlfc->hanger;
+		for (i = 0; i < h->max_items; i++) {
+			if (h->items[i].state == WLFC_HANGER_ITEM_STATE_INUSE) {
+				WLFC_DBGMESG(("%s() pkt[%d] = 0x%p, FIFO_credit_used:%d\n",
+					__FUNCTION__, i, h->items[i].pkt,
+					DHD_PKTTAG_CREDITCHECK(PKTTAG(h->items[i].pkt))));
+			}
+		}
+	}
+#endif
+	/* delete hanger */
+	dhd_wlfc_hanger_delete(dhd->osh, wlfc->hanger);
+
+	/* free top structure */
+	MFREE(dhd->osh, dhd->wlfc_state, sizeof(athost_wl_status_info_t));
+	dhd->wlfc_state = NULL;
+	return;
+}
+#endif /* PROP_TXSTATUS */
+
+void
+dhd_prot_dump(dhd_pub_t *dhdp, struct bcmstrbuf *strbuf)
+{
+	bcm_bprintf(strbuf, "Protocol CDC: reqid %d\n", dhdp->prot->reqid);
+#ifdef PROP_TXSTATUS
+	if (dhdp->wlfc_state)
+		dhd_wlfc_dump(dhdp, strbuf);
+#endif
+}
+
+void
+dhd_prot_hdrpush(dhd_pub_t *dhd, int ifidx, void *pktbuf)
+{
+#ifdef BDC
+	struct bdc_header *h;
+#endif /* BDC */
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+#ifdef BDC
+	/* Push BDC header used to convey priority for buses that don't */
+
+	PKTPUSH(dhd->osh, pktbuf, BDC_HEADER_LEN);
+
+	h = (struct bdc_header *)PKTDATA(dhd->osh, pktbuf);
+
+	h->flags = (BDC_PROTO_VER << BDC_FLAG_VER_SHIFT);
+	if (PKTSUMNEEDED(pktbuf))
+		h->flags |= BDC_FLAG_SUM_NEEDED;
+
+
+	h->priority = (PKTPRIO(pktbuf) & BDC_PRIORITY_MASK);
+	h->flags2 = 0;
+	h->dataOffset = 0;
+#endif /* BDC */
+	BDC_SET_IF_IDX(h, ifidx);
+}
+
+int
+dhd_prot_hdrpull(dhd_pub_t *dhd, int *ifidx, void *pktbuf)
+{
+#ifdef BDC
+	struct bdc_header *h;
+#endif
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+#ifdef BDC
+	/* Pop BDC header used to convey priority for buses that don't */
+
+	if (PKTLEN(dhd->osh, pktbuf) < BDC_HEADER_LEN) {
+		DHD_ERROR(("%s: rx data too short (%d < %d)\n", __FUNCTION__,
+		           PKTLEN(dhd->osh, pktbuf), BDC_HEADER_LEN));
+		return BCME_ERROR;
+	}
+
+	h = (struct bdc_header *)PKTDATA(dhd->osh, pktbuf);
+
+	if ((*ifidx = BDC_GET_IF_IDX(h)) >= DHD_MAX_IFS) {
+		DHD_ERROR(("%s: rx data ifnum out of range (%d)\n",
+		           __FUNCTION__, *ifidx));
+		return BCME_ERROR;
+	}
+
+	if (((h->flags & BDC_FLAG_VER_MASK) >> BDC_FLAG_VER_SHIFT) != BDC_PROTO_VER) {
+		DHD_ERROR(("%s: non-BDC packet received, flags = 0x%x\n",
+		           dhd_ifname(dhd, *ifidx), h->flags));
+		if (((h->flags & BDC_FLAG_VER_MASK) >> BDC_FLAG_VER_SHIFT) == BDC_PROTO_VER_1)
+			h->dataOffset = 0;
+		else
+			return BCME_ERROR;
+	}
+
+	if (h->flags & BDC_FLAG_SUM_GOOD) {
+		DHD_INFO(("%s: BDC packet received with good rx-csum, flags 0x%x\n",
+		          dhd_ifname(dhd, *ifidx), h->flags));
+		PKTSETSUMGOOD(pktbuf, TRUE);
+	}
+
+	PKTSETPRIO(pktbuf, (h->priority & BDC_PRIORITY_MASK));
+	PKTPULL(dhd->osh, pktbuf, BDC_HEADER_LEN);
+#endif /* BDC */
+
+	if (PKTLEN(dhd->osh, pktbuf) < (uint32) (h->dataOffset << 2)) {
+		DHD_ERROR(("%s: rx data too short (%d < %d)\n", __FUNCTION__,
+		           PKTLEN(dhd->osh, pktbuf), (h->dataOffset * 4)));
+		return BCME_ERROR;
+	}
+
+#ifdef PROP_TXSTATUS
+	if (dhd->wlfc_state &&
+		((athost_wl_status_info_t*)dhd->wlfc_state)->proptxstatus_mode
+		!= WLFC_FCMODE_NONE &&
+		(!DHD_PKTTAG_PKTDIR(PKTTAG(pktbuf)))) {
+		/*
+		- parse txstatus only for packets that came from the firmware
+		*/
+		dhd_os_wlfc_block(dhd);
+		dhd_wlfc_parse_header_info(dhd, pktbuf, (h->dataOffset << 2));
+		((athost_wl_status_info_t*)dhd->wlfc_state)->stats.dhd_hdrpulls++;
+		dhd_wlfc_commit_packets(dhd->wlfc_state, (f_commitpkt_t)dhd_bus_txdata,
+			(void *)dhd->bus);
+		dhd_os_wlfc_unblock(dhd);
+	}
+#endif /* PROP_TXSTATUS */
+	PKTPULL(dhd->osh, pktbuf, (h->dataOffset << 2));
+	return 0;
+}
+
+int
+dhd_prot_attach(dhd_pub_t *dhd)
+{
+	dhd_prot_t *cdc;
+
+	if (!(cdc = (dhd_prot_t *)DHD_OS_PREALLOC(dhd->osh, DHD_PREALLOC_PROT,
+		sizeof(dhd_prot_t)))) {
+		DHD_ERROR(("%s: kmalloc failed\n", __FUNCTION__));
+		goto fail;
+	}
+	memset(cdc, 0, sizeof(dhd_prot_t));
+
+	/* ensure that the msg buf directly follows the cdc msg struct */
+	if ((uintptr)(&cdc->msg + 1) != (uintptr)cdc->buf) {
+		DHD_ERROR(("dhd_prot_t is not correctly defined\n"));
+		goto fail;
+	}
+
+	dhd->prot = cdc;
+#ifdef BDC
+	dhd->hdrlen += BDC_HEADER_LEN;
+#endif
+	dhd->maxctl = WLC_IOCTL_MAXLEN + sizeof(cdc_ioctl_t) + ROUND_UP_MARGIN;
+	return 0;
+
+fail:
+#ifndef CONFIG_DHD_USE_STATIC_BUF
+	if (cdc != NULL)
+		MFREE(dhd->osh, cdc, sizeof(dhd_prot_t));
+#endif /* CONFIG_DHD_USE_STATIC_BUF */
+	return BCME_NOMEM;
+}
+
+/* ~NOTE~ What if another thread is waiting on the semaphore?  Holding it? */
+void
+dhd_prot_detach(dhd_pub_t *dhd)
+{
+#ifdef PROP_TXSTATUS
+	dhd_wlfc_deinit(dhd);
+#endif
+#ifndef CONFIG_DHD_USE_STATIC_BUF
+	MFREE(dhd->osh, dhd->prot, sizeof(dhd_prot_t));
+#endif /* CONFIG_DHD_USE_STATIC_BUF */
+	dhd->prot = NULL;
+}
+
+void
+dhd_prot_dstats(dhd_pub_t *dhd)
+{
+	/* No stats from dongle added yet, copy bus stats */
+	dhd->dstats.tx_packets = dhd->tx_packets;
+	dhd->dstats.tx_errors = dhd->tx_errors;
+	dhd->dstats.rx_packets = dhd->rx_packets;
+	dhd->dstats.rx_errors = dhd->rx_errors;
+	dhd->dstats.rx_dropped = dhd->rx_dropped;
+	dhd->dstats.multicast = dhd->rx_multicast;
+	return;
+}
+
+int
+dhd_prot_init(dhd_pub_t *dhd)
+{
+	int ret = 0;
+	wlc_rev_info_t revinfo;
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+
+	/* Get the device rev info */
+	memset(&revinfo, 0, sizeof(revinfo));
+	ret = dhd_wl_ioctl_cmd(dhd, WLC_GET_REVINFO, &revinfo, sizeof(revinfo), FALSE, 0);
+	if (ret < 0)
+		goto done;
+
+
+#ifdef PROP_TXSTATUS
+	ret = dhd_wlfc_init(dhd);
+#endif
+
+#if defined(WL_CFG80211)
+	if (dhd_download_fw_on_driverload)
+#endif /* defined(WL_CFG80211) */
+	ret = dhd_preinit_ioctls(dhd);
+
+	/* Always assumes wl for now */
+	dhd->iswl = TRUE;
+
+done:
+	return ret;
+}
+
+void
+dhd_prot_stop(dhd_pub_t *dhd)
+{
+	/* Nothing to do for CDC */
+}
diff --git a/drivers/net/wireless/bcmdhd/dhd_cfg80211.c b/drivers/net/wireless/bcmdhd/dhd_cfg80211.c
new file mode 100644
index 000000000000..bc1be94802b4
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd_cfg80211.c
@@ -0,0 +1,655 @@
+/*
+ * Linux cfg80211 driver - Dongle Host Driver (DHD) related
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wl_cfg80211.c,v 1.1.4.1.2.14 2011/02/09 01:40:07 Exp $
+ */
+
+#include <net/rtnetlink.h>
+
+#include <bcmutils.h>
+#include <wldev_common.h>
+#include <wl_cfg80211.h>
+#include <dhd_cfg80211.h>
+extern struct wl_priv *wlcfg_drv_priv;
+static int dhd_dongle_up = FALSE;
+
+#include <dngl_stats.h>
+#include <dhd.h>
+#include <dhdioctl.h>
+#include <wlioctl.h>
+#include <dhd_cfg80211.h>
+
+static s32 wl_dongle_up(struct net_device *ndev, u32 up);
+
+/**
+ * Function implementations
+ */
+
+s32 dhd_cfg80211_init(struct wl_priv *wl)
+{
+	dhd_dongle_up = FALSE;
+	return 0;
+}
+
+s32 dhd_cfg80211_deinit(struct wl_priv *wl)
+{
+	dhd_dongle_up = FALSE;
+	return 0;
+}
+
+s32 dhd_cfg80211_get_opmode(struct wl_priv *wl)
+{
+	dhd_pub_t *dhd =  (dhd_pub_t *)(wl->pub);
+	return dhd->op_mode;
+}
+
+s32 dhd_cfg80211_down(struct wl_priv *wl)
+{
+	dhd_dongle_up = FALSE;
+	return 0;
+}
+
+s32 dhd_cfg80211_set_p2p_info(struct wl_priv *wl, int val)
+{
+	dhd_pub_t *dhd =  (dhd_pub_t *)(wl->pub);
+	int bcn_timeout = DHD_BEACON_TIMEOUT_HIGH;
+	char iovbuf[30];
+
+	dhd->op_mode |= val;
+	WL_ERR(("Set : op_mode=%d\n", dhd->op_mode));
+
+#ifdef ARP_OFFLOAD_SUPPORT
+	/* IF P2P is enabled, disable arpoe */
+	dhd_arp_offload_set(dhd, 0);
+	dhd_arp_offload_enable(dhd, false);
+#endif /* ARP_OFFLOAD_SUPPORT */
+
+	dhd_os_set_packet_filter(dhd, 0);
+
+	/* Setup timeout if Beacons are lost and roam is off to report link down */
+	bcm_mkiovar("bcn_timeout", (char *)&bcn_timeout, 4, iovbuf, sizeof(iovbuf));
+	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
+
+
+	return 0;
+}
+
+s32 dhd_cfg80211_clean_p2p_info(struct wl_priv *wl)
+{
+	dhd_pub_t *dhd =  (dhd_pub_t *)(wl->pub);
+	int bcn_timeout = DHD_BEACON_TIMEOUT_NORMAL;
+	char iovbuf[30];
+
+	dhd->op_mode &= ~CONCURENT_MASK;
+	WL_ERR(("Clean : op_mode=%d\n", dhd->op_mode));
+
+#ifdef ARP_OFFLOAD_SUPPORT
+	/* IF P2P is disabled, enable arpoe back for STA mode. */
+	dhd_arp_offload_set(dhd, dhd_arp_mode);
+	dhd_arp_offload_enable(dhd, true);
+#endif /* ARP_OFFLOAD_SUPPORT */
+
+	dhd_os_set_packet_filter(dhd, 1);
+
+	/* Setup timeout if Beacons are lost and roam is off to report link down */
+	bcm_mkiovar("bcn_timeout", (char *)&bcn_timeout, 4, iovbuf, sizeof(iovbuf));
+	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
+
+	return 0;
+}
+
+static s32 wl_dongle_up(struct net_device *ndev, u32 up)
+{
+	s32 err = 0;
+
+	err = wldev_ioctl(ndev, WLC_UP, &up, sizeof(up), true);
+	if (unlikely(err)) {
+		WL_ERR(("WLC_UP error (%d)\n", err));
+	}
+	return err;
+}
+
+s32 dhd_config_dongle(struct wl_priv *wl, bool need_lock)
+{
+#ifndef DHD_SDALIGN
+#define DHD_SDALIGN	32
+#endif
+	struct net_device *ndev;
+	s32 err = 0;
+
+	WL_TRACE(("In\n"));
+	if (dhd_dongle_up) {
+		WL_ERR(("Dongle is already up\n"));
+		return err;
+	}
+
+	ndev = wl_to_prmry_ndev(wl);
+
+	if (need_lock)
+		rtnl_lock();
+
+	err = wl_dongle_up(ndev, 0);
+	if (unlikely(err)) {
+		WL_ERR(("wl_dongle_up failed\n"));
+		goto default_conf_out;
+	}
+	dhd_dongle_up = true;
+
+default_conf_out:
+	if (need_lock)
+		rtnl_unlock();
+	return err;
+
+}
+
+
+/* TODO: clean up the BT-Coex code, it still have some legacy ioctl/iovar functions */
+#define COEX_DHCP
+
+#if defined(COEX_DHCP)
+
+/* use New SCO/eSCO smart YG suppression */
+#define BT_DHCP_eSCO_FIX
+/* this flag boost wifi pkt priority to max, caution: -not fair to sco */
+#define BT_DHCP_USE_FLAGS
+/* T1 start SCO/ESCo priority suppression */
+#define BT_DHCP_OPPR_WIN_TIME	2500
+/* T2 turn off SCO/SCO supperesion is (timeout) */
+#define BT_DHCP_FLAG_FORCE_TIME 5500
+
+enum wl_cfg80211_btcoex_status {
+	BT_DHCP_IDLE,
+	BT_DHCP_START,
+	BT_DHCP_OPPR_WIN,
+	BT_DHCP_FLAG_FORCE_TIMEOUT
+};
+
+/*
+ * get named driver variable to uint register value and return error indication
+ * calling example: dev_wlc_intvar_get_reg(dev, "btc_params",66, &reg_value)
+ */
+static int
+dev_wlc_intvar_get_reg(struct net_device *dev, char *name,
+	uint reg, int *retval)
+{
+	union {
+		char buf[WLC_IOCTL_SMLEN];
+		int val;
+	} var;
+	int error;
+
+	bcm_mkiovar(name, (char *)(&reg), sizeof(reg),
+		(char *)(&var), sizeof(var.buf));
+	error = wldev_ioctl(dev, WLC_GET_VAR, (char *)(&var), sizeof(var.buf), false);
+
+	*retval = dtoh32(var.val);
+	return (error);
+}
+
+static int
+dev_wlc_bufvar_set(struct net_device *dev, char *name, char *buf, int len)
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31)
+	char ioctlbuf_local[1024];
+#else
+	static char ioctlbuf_local[1024];
+#endif /* LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31) */
+
+	bcm_mkiovar(name, buf, len, ioctlbuf_local, sizeof(ioctlbuf_local));
+
+	return (wldev_ioctl(dev, WLC_SET_VAR, ioctlbuf_local, sizeof(ioctlbuf_local), true));
+}
+/*
+get named driver variable to uint register value and return error indication
+calling example: dev_wlc_intvar_set_reg(dev, "btc_params",66, value)
+*/
+static int
+dev_wlc_intvar_set_reg(struct net_device *dev, char *name, char *addr, char * val)
+{
+	char reg_addr[8];
+
+	memset(reg_addr, 0, sizeof(reg_addr));
+	memcpy((char *)&reg_addr[0], (char *)addr, 4);
+	memcpy((char *)&reg_addr[4], (char *)val, 4);
+
+	return (dev_wlc_bufvar_set(dev, name, (char *)&reg_addr[0], sizeof(reg_addr)));
+}
+
+static bool btcoex_is_sco_active(struct net_device *dev)
+{
+	int ioc_res = 0;
+	bool res = FALSE;
+	int sco_id_cnt = 0;
+	int param27;
+	int i;
+
+	for (i = 0; i < 12; i++) {
+
+		ioc_res = dev_wlc_intvar_get_reg(dev, "btc_params", 27, &param27);
+
+		WL_TRACE(("%s, sample[%d], btc params: 27:%x\n",
+			__FUNCTION__, i, param27));
+
+		if (ioc_res < 0) {
+			WL_ERR(("%s ioc read btc params error\n", __FUNCTION__));
+			break;
+		}
+
+		if ((param27 & 0x6) == 2) { /* count both sco & esco  */
+			sco_id_cnt++;
+		}
+
+		if (sco_id_cnt > 2) {
+			WL_TRACE(("%s, sco/esco detected, pkt id_cnt:%d  samples:%d\n",
+				__FUNCTION__, sco_id_cnt, i));
+			res = TRUE;
+			break;
+		}
+
+		msleep(5);
+	}
+
+	return res;
+}
+
+#if defined(BT_DHCP_eSCO_FIX)
+/* Enhanced BT COEX settings for eSCO compatibility during DHCP window */
+static int set_btc_esco_params(struct net_device *dev, bool trump_sco)
+{
+	static bool saved_status = FALSE;
+
+	char buf_reg50va_dhcp_on[8] =
+		{ 50, 00, 00, 00, 0x22, 0x80, 0x00, 0x00 };
+	char buf_reg51va_dhcp_on[8] =
+		{ 51, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
+	char buf_reg64va_dhcp_on[8] =
+		{ 64, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
+	char buf_reg65va_dhcp_on[8] =
+		{ 65, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
+	char buf_reg71va_dhcp_on[8] =
+		{ 71, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
+	uint32 regaddr;
+	static uint32 saved_reg50;
+	static uint32 saved_reg51;
+	static uint32 saved_reg64;
+	static uint32 saved_reg65;
+	static uint32 saved_reg71;
+
+	if (trump_sco) {
+		/* this should reduce eSCO agressive retransmit
+		 * w/o breaking it
+		 */
+
+		/* 1st save current */
+		WL_TRACE(("Do new SCO/eSCO coex algo {save &"
+			  "override}\n"));
+		if ((!dev_wlc_intvar_get_reg(dev, "btc_params", 50, &saved_reg50)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 51, &saved_reg51)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 64, &saved_reg64)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 65, &saved_reg65)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 71, &saved_reg71))) {
+			saved_status = TRUE;
+			WL_TRACE(("%s saved bt_params[50,51,64,65,71]:"
+				  "0x%x 0x%x 0x%x 0x%x 0x%x\n",
+				  __FUNCTION__, saved_reg50, saved_reg51,
+				  saved_reg64, saved_reg65, saved_reg71));
+		} else {
+			WL_ERR((":%s: save btc_params failed\n",
+				__FUNCTION__));
+			saved_status = FALSE;
+			return -1;
+		}
+
+		WL_TRACE(("override with [50,51,64,65,71]:"
+			  "0x%x 0x%x 0x%x 0x%x 0x%x\n",
+			  *(u32 *)(buf_reg50va_dhcp_on+4),
+			  *(u32 *)(buf_reg51va_dhcp_on+4),
+			  *(u32 *)(buf_reg64va_dhcp_on+4),
+			  *(u32 *)(buf_reg65va_dhcp_on+4),
+			  *(u32 *)(buf_reg71va_dhcp_on+4)));
+
+		dev_wlc_bufvar_set(dev, "btc_params",
+			(char *)&buf_reg50va_dhcp_on[0], 8);
+		dev_wlc_bufvar_set(dev, "btc_params",
+			(char *)&buf_reg51va_dhcp_on[0], 8);
+		dev_wlc_bufvar_set(dev, "btc_params",
+			(char *)&buf_reg64va_dhcp_on[0], 8);
+		dev_wlc_bufvar_set(dev, "btc_params",
+			(char *)&buf_reg65va_dhcp_on[0], 8);
+		dev_wlc_bufvar_set(dev, "btc_params",
+			(char *)&buf_reg71va_dhcp_on[0], 8);
+
+		saved_status = TRUE;
+	} else if (saved_status) {
+		/* restore previously saved bt params */
+		WL_TRACE(("Do new SCO/eSCO coex algo {save &"
+			  "override}\n"));
+
+		regaddr = 50;
+		dev_wlc_intvar_set_reg(dev, "btc_params",
+			(char *)&regaddr, (char *)&saved_reg50);
+		regaddr = 51;
+		dev_wlc_intvar_set_reg(dev, "btc_params",
+			(char *)&regaddr, (char *)&saved_reg51);
+		regaddr = 64;
+		dev_wlc_intvar_set_reg(dev, "btc_params",
+			(char *)&regaddr, (char *)&saved_reg64);
+		regaddr = 65;
+		dev_wlc_intvar_set_reg(dev, "btc_params",
+			(char *)&regaddr, (char *)&saved_reg65);
+		regaddr = 71;
+		dev_wlc_intvar_set_reg(dev, "btc_params",
+			(char *)&regaddr, (char *)&saved_reg71);
+
+		WL_TRACE(("restore bt_params[50,51,64,65,71]:"
+			"0x%x 0x%x 0x%x 0x%x 0x%x\n",
+			saved_reg50, saved_reg51, saved_reg64,
+			saved_reg65, saved_reg71));
+
+		saved_status = FALSE;
+	} else {
+		WL_ERR((":%s att to restore not saved BTCOEX params\n",
+			__FUNCTION__));
+		return -1;
+	}
+	return 0;
+}
+#endif /* BT_DHCP_eSCO_FIX */
+
+static void
+wl_cfg80211_bt_setflag(struct net_device *dev, bool set)
+{
+#if defined(BT_DHCP_USE_FLAGS)
+	char buf_flag7_dhcp_on[8] = { 7, 00, 00, 00, 0x1, 0x0, 0x00, 0x00 };
+	char buf_flag7_default[8]   = { 7, 00, 00, 00, 0x0, 0x00, 0x00, 0x00};
+#endif
+
+
+#if defined(BT_DHCP_eSCO_FIX)
+	/* set = 1, save & turn on  0 - off & restore prev settings */
+	set_btc_esco_params(dev, set);
+#endif
+
+#if defined(BT_DHCP_USE_FLAGS)
+	WL_TRACE(("WI-FI priority boost via bt flags, set:%d\n", set));
+	if (set == TRUE)
+		/* Forcing bt_flag7  */
+		dev_wlc_bufvar_set(dev, "btc_flags",
+			(char *)&buf_flag7_dhcp_on[0],
+			sizeof(buf_flag7_dhcp_on));
+	else
+		/* Restoring default bt flag7 */
+		dev_wlc_bufvar_set(dev, "btc_flags",
+			(char *)&buf_flag7_default[0],
+			sizeof(buf_flag7_default));
+#endif
+}
+
+static void wl_cfg80211_bt_timerfunc(ulong data)
+{
+	struct btcoex_info *bt_local = (struct btcoex_info *)data;
+	WL_TRACE(("%s\n", __FUNCTION__));
+	bt_local->timer_on = 0;
+	schedule_work(&bt_local->work);
+}
+
+static void wl_cfg80211_bt_handler(struct work_struct *work)
+{
+	struct btcoex_info *btcx_inf;
+
+	btcx_inf = container_of(work, struct btcoex_info, work);
+
+	if (btcx_inf->timer_on) {
+		btcx_inf->timer_on = 0;
+		del_timer_sync(&btcx_inf->timer);
+	}
+
+	switch (btcx_inf->bt_state) {
+		case BT_DHCP_START:
+			/* DHCP started
+			 * provide OPPORTUNITY window to get DHCP address
+			 */
+			WL_TRACE(("%s bt_dhcp stm: started \n",
+				__FUNCTION__));
+			btcx_inf->bt_state = BT_DHCP_OPPR_WIN;
+			mod_timer(&btcx_inf->timer,
+				jiffies + BT_DHCP_OPPR_WIN_TIME*HZ/1000);
+			btcx_inf->timer_on = 1;
+			break;
+
+		case BT_DHCP_OPPR_WIN:
+			if (btcx_inf->dhcp_done) {
+				WL_TRACE(("%s DHCP Done before T1 expiration\n",
+					__FUNCTION__));
+				goto btc_coex_idle;
+			}
+
+			/* DHCP is not over yet, start lowering BT priority
+			 * enforce btc_params + flags if necessary
+			 */
+			WL_TRACE(("%s DHCP T1:%d expired\n", __FUNCTION__,
+				BT_DHCP_OPPR_WIN_TIME));
+			if (btcx_inf->dev)
+				wl_cfg80211_bt_setflag(btcx_inf->dev, TRUE);
+			btcx_inf->bt_state = BT_DHCP_FLAG_FORCE_TIMEOUT;
+			mod_timer(&btcx_inf->timer,
+				jiffies + BT_DHCP_FLAG_FORCE_TIME*HZ/1000);
+			btcx_inf->timer_on = 1;
+			break;
+
+		case BT_DHCP_FLAG_FORCE_TIMEOUT:
+			if (btcx_inf->dhcp_done) {
+				WL_TRACE(("%s DHCP Done before T2 expiration\n",
+					__FUNCTION__));
+			} else {
+				/* Noo dhcp during T1+T2, restore BT priority */
+				WL_TRACE(("%s DHCP wait interval T2:%d"
+					  "msec expired\n", __FUNCTION__,
+					  BT_DHCP_FLAG_FORCE_TIME));
+			}
+
+			/* Restoring default bt priority */
+			if (btcx_inf->dev)
+				wl_cfg80211_bt_setflag(btcx_inf->dev, FALSE);
+btc_coex_idle:
+			btcx_inf->bt_state = BT_DHCP_IDLE;
+			btcx_inf->timer_on = 0;
+			break;
+
+		default:
+			WL_ERR(("%s error g_status=%d !!!\n", __FUNCTION__,
+				btcx_inf->bt_state));
+			if (btcx_inf->dev)
+				wl_cfg80211_bt_setflag(btcx_inf->dev, FALSE);
+			btcx_inf->bt_state = BT_DHCP_IDLE;
+			btcx_inf->timer_on = 0;
+			break;
+	}
+
+	net_os_wake_unlock(btcx_inf->dev);
+}
+
+int wl_cfg80211_btcoex_init(struct wl_priv *wl)
+{
+	struct btcoex_info *btco_inf = NULL;
+
+	btco_inf = kmalloc(sizeof(struct btcoex_info), GFP_KERNEL);
+	if (!btco_inf)
+		return -ENOMEM;
+
+	btco_inf->bt_state = BT_DHCP_IDLE;
+	btco_inf->ts_dhcp_start = 0;
+	btco_inf->ts_dhcp_ok = 0;
+	/* Set up timer for BT  */
+	btco_inf->timer_ms = 10;
+	init_timer(&btco_inf->timer);
+	btco_inf->timer.data = (ulong)btco_inf;
+	btco_inf->timer.function = wl_cfg80211_bt_timerfunc;
+
+	btco_inf->dev = wl->wdev->netdev;
+
+	INIT_WORK(&btco_inf->work, wl_cfg80211_bt_handler);
+
+	wl->btcoex_info = btco_inf;
+	return 0;
+}
+
+void wl_cfg80211_btcoex_deinit(struct wl_priv *wl)
+{
+	if (!wl->btcoex_info)
+		return;
+
+	if (!wl->btcoex_info->timer_on) {
+		wl->btcoex_info->timer_on = 0;
+		del_timer_sync(&wl->btcoex_info->timer);
+	}
+
+	cancel_work_sync(&wl->btcoex_info->work);
+
+	kfree(wl->btcoex_info);
+	wl->btcoex_info = NULL;
+}
+
+int wl_cfg80211_set_btcoex_dhcp(struct net_device *dev, char *command)
+{
+
+	struct wl_priv *wl = wlcfg_drv_priv;
+	char powermode_val = 0;
+	char buf_reg66va_dhcp_on[8] = { 66, 00, 00, 00, 0x10, 0x27, 0x00, 0x00 };
+	char buf_reg41va_dhcp_on[8] = { 41, 00, 00, 00, 0x33, 0x00, 0x00, 0x00 };
+	char buf_reg68va_dhcp_on[8] = { 68, 00, 00, 00, 0x90, 0x01, 0x00, 0x00 };
+
+	uint32 regaddr;
+	static uint32 saved_reg66;
+	static uint32 saved_reg41;
+	static uint32 saved_reg68;
+	static bool saved_status = FALSE;
+
+#ifdef COEX_DHCP
+	char buf_flag7_default[8] =   { 7, 00, 00, 00, 0x0, 0x00, 0x00, 0x00};
+	struct btcoex_info *btco_inf = wl->btcoex_info;
+#endif /* COEX_DHCP */
+
+	/* Figure out powermode 1 or o command */
+	strncpy((char *)&powermode_val, command + strlen("BTCOEXMODE") +1, 1);
+
+	if (strnicmp((char *)&powermode_val, "1", strlen("1")) == 0) {
+
+		WL_TRACE(("%s: DHCP session starts\n", __FUNCTION__));
+
+		/* Retrieve and saved orig regs value */
+		if ((saved_status == FALSE) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 66,  &saved_reg66)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 41,  &saved_reg41)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 68,  &saved_reg68)))   {
+				saved_status = TRUE;
+				WL_TRACE(("Saved 0x%x 0x%x 0x%x\n",
+					saved_reg66, saved_reg41, saved_reg68));
+
+				/* Disable PM mode during dhpc session */
+
+				/* Disable PM mode during dhpc session */
+#ifdef COEX_DHCP
+				/* Start  BT timer only for SCO connection */
+				if (btcoex_is_sco_active(dev)) {
+					/* btc_params 66 */
+					dev_wlc_bufvar_set(dev, "btc_params",
+						(char *)&buf_reg66va_dhcp_on[0],
+						sizeof(buf_reg66va_dhcp_on));
+					/* btc_params 41 0x33 */
+					dev_wlc_bufvar_set(dev, "btc_params",
+						(char *)&buf_reg41va_dhcp_on[0],
+						sizeof(buf_reg41va_dhcp_on));
+					/* btc_params 68 0x190 */
+					dev_wlc_bufvar_set(dev, "btc_params",
+						(char *)&buf_reg68va_dhcp_on[0],
+						sizeof(buf_reg68va_dhcp_on));
+					saved_status = TRUE;
+
+					btco_inf->bt_state = BT_DHCP_START;
+					btco_inf->timer_on = 1;
+					mod_timer(&btco_inf->timer, btco_inf->timer.expires);
+					WL_TRACE(("%s enable BT DHCP Timer\n",
+					__FUNCTION__));
+				}
+#endif /* COEX_DHCP */
+		}
+		else if (saved_status == TRUE) {
+			WL_ERR(("%s was called w/o DHCP OFF. Continue\n", __FUNCTION__));
+		}
+	}
+	else if (strnicmp((char *)&powermode_val, "2", strlen("2")) == 0) {
+
+
+		/* Restoring PM mode */
+
+#ifdef COEX_DHCP
+		/* Stop any bt timer because DHCP session is done */
+		WL_TRACE(("%s disable BT DHCP Timer\n", __FUNCTION__));
+		if (btco_inf->timer_on) {
+			btco_inf->timer_on = 0;
+			del_timer_sync(&btco_inf->timer);
+
+			if (btco_inf->bt_state != BT_DHCP_IDLE) {
+			/* need to restore original btc flags & extra btc params */
+				WL_TRACE(("%s bt->bt_state:%d\n",
+					__FUNCTION__, btco_inf->bt_state));
+				/* wake up btcoex thread to restore btlags+params  */
+				schedule_work(&btco_inf->work);
+			}
+		}
+
+		/* Restoring btc_flag paramter anyway */
+		if (saved_status == TRUE)
+			dev_wlc_bufvar_set(dev, "btc_flags",
+				(char *)&buf_flag7_default[0], sizeof(buf_flag7_default));
+#endif /* COEX_DHCP */
+
+		/* Restore original values */
+		if (saved_status == TRUE) {
+			regaddr = 66;
+			dev_wlc_intvar_set_reg(dev, "btc_params",
+				(char *)&regaddr, (char *)&saved_reg66);
+			regaddr = 41;
+			dev_wlc_intvar_set_reg(dev, "btc_params",
+				(char *)&regaddr, (char *)&saved_reg41);
+			regaddr = 68;
+			dev_wlc_intvar_set_reg(dev, "btc_params",
+				(char *)&regaddr, (char *)&saved_reg68);
+
+			WL_TRACE(("restore regs {66,41,68} <- 0x%x 0x%x 0x%x\n",
+				saved_reg66, saved_reg41, saved_reg68));
+		}
+		saved_status = FALSE;
+
+	}
+	else {
+		WL_ERR(("%s Unkwown yet power setting, ignored\n",
+			__FUNCTION__));
+	}
+
+	snprintf(command, 3, "OK");
+
+	return (strlen("OK"));
+}
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/dhd_cfg80211.h b/drivers/net/wireless/bcmdhd/dhd_cfg80211.h
new file mode 100644
index 000000000000..ced46dbdb96c
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd_cfg80211.h
@@ -0,0 +1,45 @@
+/*
+ * Linux cfg80211 driver - Dongle Host Driver (DHD) related
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wl_cfg80211.c,v 1.1.4.1.2.14 2011/02/09 01:40:07 Exp $
+ */
+
+
+#ifndef __DHD_CFG80211__
+#define __DHD_CFG80211__
+
+#include <wl_cfg80211.h>
+#include <wl_cfgp2p.h>
+
+s32 dhd_cfg80211_init(struct wl_priv *wl);
+s32 dhd_cfg80211_deinit(struct wl_priv *wl);
+s32 dhd_cfg80211_get_opmode(struct wl_priv *wl);
+s32 dhd_cfg80211_down(struct wl_priv *wl);
+s32 dhd_cfg80211_set_p2p_info(struct wl_priv *wl, int val);
+s32 dhd_cfg80211_clean_p2p_info(struct wl_priv *wl);
+s32 dhd_config_dongle(struct wl_priv *wl, bool need_lock);
+
+int wl_cfg80211_btcoex_init(struct wl_priv *wl);
+void wl_cfg80211_btcoex_deinit(struct wl_priv *wl);
+
+#endif /* __DHD_CFG80211__ */
diff --git a/drivers/net/wireless/bcmdhd/dhd_common.c b/drivers/net/wireless/bcmdhd/dhd_common.c
new file mode 100644
index 000000000000..3ccc55d87d4b
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd_common.c
@@ -0,0 +1,2457 @@
+/*
+ * Broadcom Dongle Host Driver (DHD), common DHD core.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_common.c 331276 2012-05-04 08:05:57Z $
+ */
+#include <typedefs.h>
+#include <osl.h>
+
+#include <epivers.h>
+#include <bcmutils.h>
+
+#include <bcmendian.h>
+#include <dngl_stats.h>
+#include <wlioctl.h>
+#include <dhd.h>
+
+#include <proto/bcmevent.h>
+
+#include <dhd_bus.h>
+#include <dhd_proto.h>
+#include <dhd_dbg.h>
+#include <msgtrace.h>
+
+#ifdef WL_CFG80211
+#include <wl_cfg80211.h>
+#endif
+#include <proto/bt_amp_hci.h>
+#include <dhd_bta.h>
+#ifdef SET_RANDOM_MAC_SOFTAP
+#include <linux/random.h>
+#include <linux/jiffies.h>
+#endif
+
+#ifdef PROP_TXSTATUS
+#include <wlfc_proto.h>
+#include <dhd_wlfc.h>
+#endif
+
+
+#ifdef WLMEDIA_HTSF
+extern void htsf_update(struct dhd_info *dhd, void *data);
+#endif
+int dhd_msg_level = DHD_ERROR_VAL;
+
+
+#include <wl_iw.h>
+
+char fw_path[MOD_PARAM_PATHLEN];
+char nv_path[MOD_PARAM_PATHLEN];
+
+#ifdef SOFTAP
+char fw_path2[MOD_PARAM_PATHLEN];
+extern bool softap_enabled;
+#endif
+
+/* Last connection success/failure status */
+uint32 dhd_conn_event;
+uint32 dhd_conn_status;
+uint32 dhd_conn_reason;
+
+#define htod32(i) i
+#define htod16(i) i
+#define dtoh32(i) i
+#define dtoh16(i) i
+extern int dhd_iscan_request(void * dhdp, uint16 action);
+extern void dhd_ind_scan_confirm(void *h, bool status);
+extern int dhd_iscan_in_progress(void *h);
+void dhd_iscan_lock(void);
+void dhd_iscan_unlock(void);
+extern int dhd_change_mtu(dhd_pub_t *dhd, int new_mtu, int ifidx);
+bool ap_cfg_running = FALSE;
+bool ap_fw_loaded = FALSE;
+
+
+#ifdef DHD_DEBUG
+const char dhd_version[] = "Dongle Host Driver, version " EPI_VERSION_STR "\nCompiled on "
+	__DATE__ " at " __TIME__;
+#else
+const char dhd_version[] = "Dongle Host Driver, version " EPI_VERSION_STR;
+#endif
+
+void dhd_set_timer(void *bus, uint wdtick);
+
+/* IOVar table */
+enum {
+	IOV_VERSION = 1,
+	IOV_MSGLEVEL,
+	IOV_BCMERRORSTR,
+	IOV_BCMERROR,
+	IOV_WDTICK,
+	IOV_DUMP,
+	IOV_CLEARCOUNTS,
+	IOV_LOGDUMP,
+	IOV_LOGCAL,
+	IOV_LOGSTAMP,
+	IOV_GPIOOB,
+	IOV_IOCTLTIMEOUT,
+	IOV_HCI_CMD,		/* HCI command */
+	IOV_HCI_ACL_DATA,	/* HCI data packet */
+#if defined(DHD_DEBUG)
+	IOV_CONS,
+	IOV_DCONSOLE_POLL,
+#endif /* defined(DHD_DEBUG) */
+#ifdef PROP_TXSTATUS
+	IOV_PROPTXSTATUS_ENABLE,
+	IOV_PROPTXSTATUS_MODE,
+#endif
+	IOV_BUS_TYPE,
+#ifdef WLMEDIA_HTSF
+	IOV_WLPKTDLYSTAT_SZ,
+#endif
+	IOV_CHANGEMTU,
+	IOV_LAST
+};
+
+const bcm_iovar_t dhd_iovars[] = {
+	{"version",	IOV_VERSION,	0,	IOVT_BUFFER,	sizeof(dhd_version) },
+#ifdef DHD_DEBUG
+	{"msglevel",	IOV_MSGLEVEL,	0,	IOVT_UINT32,	0 },
+#endif /* DHD_DEBUG */
+	{"bcmerrorstr", IOV_BCMERRORSTR, 0, IOVT_BUFFER,	BCME_STRLEN },
+	{"bcmerror",	IOV_BCMERROR,	0,	IOVT_INT8,	0 },
+	{"wdtick",	IOV_WDTICK, 0,	IOVT_UINT32,	0 },
+	{"dump",	IOV_DUMP,	0,	IOVT_BUFFER,	DHD_IOCTL_MAXLEN },
+#ifdef DHD_DEBUG
+	{"cons",	IOV_CONS,	0,	IOVT_BUFFER,	0 },
+	{"dconpoll",	IOV_DCONSOLE_POLL, 0,	IOVT_UINT32,	0 },
+#endif
+	{"clearcounts", IOV_CLEARCOUNTS, 0, IOVT_VOID,	0 },
+	{"gpioob",	IOV_GPIOOB,	0,	IOVT_UINT32,	0 },
+	{"ioctl_timeout",	IOV_IOCTLTIMEOUT,	0,	IOVT_UINT32,	0 },
+	{"HCI_cmd",	IOV_HCI_CMD,	0,	IOVT_BUFFER,	0},
+	{"HCI_ACL_data", IOV_HCI_ACL_DATA, 0,	IOVT_BUFFER,	0},
+#ifdef PROP_TXSTATUS
+	{"proptx",	IOV_PROPTXSTATUS_ENABLE,	0,	IOVT_UINT32,	0 },
+	/*
+	set the proptxtstatus operation mode:
+	0 - Do not do any proptxtstatus flow control
+	1 - Use implied credit from a packet status
+	2 - Use explicit credit
+	*/
+	{"ptxmode",	IOV_PROPTXSTATUS_MODE,	0,	IOVT_UINT32,	0 },
+#endif
+	{"bustype", IOV_BUS_TYPE, 0, IOVT_UINT32, 0},
+#ifdef WLMEDIA_HTSF
+	{"pktdlystatsz", IOV_WLPKTDLYSTAT_SZ, 0, IOVT_UINT8, 0 },
+#endif
+	{"changemtu", IOV_CHANGEMTU, 0, IOVT_UINT32, 0 },
+	{NULL, 0, 0, 0, 0 }
+};
+
+struct dhd_cmn *
+dhd_common_init(uint16 devid, osl_t *osh)
+{
+	dhd_cmn_t *cmn;
+
+	/* Init global variables at run-time, not as part of the declaration.
+	 * This is required to support init/de-init of the driver. Initialization
+	 * of globals as part of the declaration results in non-deterministic
+	 * behavior since the value of the globals may be different on the
+	 * first time that the driver is initialized vs subsequent initializations.
+	 */
+	/* Allocate private bus interface state */
+	if (!(cmn = MALLOC(osh, sizeof(dhd_cmn_t)))) {
+		DHD_ERROR(("%s: MALLOC failed\n", __FUNCTION__));
+		return NULL;
+	}
+	memset(cmn, 0, sizeof(dhd_cmn_t));
+	cmn->osh = osh;
+
+#ifdef CONFIG_BCMDHD_FW_PATH
+	bcm_strncpy_s(fw_path, sizeof(fw_path), CONFIG_BCMDHD_FW_PATH, MOD_PARAM_PATHLEN-1);
+#elif defined(CONFIG_BCMDHD_FW_DIR)  /* CONFIG_BCMDHD_FW_PATH */
+	sprintf(fw_path, "%s/bcm%x/fw_bcmdhd.bin", CONFIG_BCMDHD_FW_DIR, devid);
+#else
+	fw_path[0] = '\0';
+#endif /* CONFIG_BCMDHD_FW_DIR */
+#ifdef CONFIG_BCMDHD_NVRAM_PATH
+	bcm_strncpy_s(nv_path, sizeof(nv_path), CONFIG_BCMDHD_NVRAM_PATH, MOD_PARAM_PATHLEN-1);
+#elif defined(CONFIG_BCMDHD_NVRAM_DIR) /* CONFIG_BCMDHD_NVRAM_PATH */
+	sprintf(nv_path, "%s/nvram_%x.txt", CONFIG_BCMDHD_NVRAM_DIR, devid);
+#else
+	nv_path[0] = '\0';
+#endif /* CONFIG_BCMDHD_NVRAM_PATH */
+#ifdef SOFTAP
+	fw_path2[0] = '\0';
+#endif
+	DHD_ERROR(("bcmdhd: fw_path: %s nvram_path: %s\n", fw_path, nv_path));
+	return cmn;
+}
+
+void
+dhd_common_deinit(dhd_pub_t *dhd_pub, dhd_cmn_t *sa_cmn)
+{
+	osl_t *osh;
+	dhd_cmn_t *cmn;
+
+	if (dhd_pub != NULL)
+		cmn = dhd_pub->cmn;
+	else
+		cmn = sa_cmn;
+
+	if (!cmn)
+		return;
+
+	osh = cmn->osh;
+
+	if (dhd_pub != NULL)
+	dhd_pub->cmn = NULL;
+	MFREE(osh, cmn, sizeof(dhd_cmn_t));
+}
+
+static int
+dhd_dump(dhd_pub_t *dhdp, char *buf, int buflen)
+{
+	char eabuf[ETHER_ADDR_STR_LEN];
+
+	struct bcmstrbuf b;
+	struct bcmstrbuf *strbuf = &b;
+
+	bcm_binit(strbuf, buf, buflen);
+
+	/* Base DHD info */
+	bcm_bprintf(strbuf, "%s\n", dhd_version);
+	bcm_bprintf(strbuf, "\n");
+	bcm_bprintf(strbuf, "pub.up %d pub.txoff %d pub.busstate %d\n",
+	            dhdp->up, dhdp->txoff, dhdp->busstate);
+	bcm_bprintf(strbuf, "pub.hdrlen %d pub.maxctl %d pub.rxsz %d\n",
+	            dhdp->hdrlen, dhdp->maxctl, dhdp->rxsz);
+	bcm_bprintf(strbuf, "pub.iswl %d pub.drv_version %ld pub.mac %s\n",
+	            dhdp->iswl, dhdp->drv_version, bcm_ether_ntoa(&dhdp->mac, eabuf));
+	bcm_bprintf(strbuf, "pub.bcmerror %d tickcnt %d\n", dhdp->bcmerror, dhdp->tickcnt);
+
+	bcm_bprintf(strbuf, "dongle stats:\n");
+	bcm_bprintf(strbuf, "tx_packets %ld tx_bytes %ld tx_errors %ld tx_dropped %ld\n",
+	            dhdp->dstats.tx_packets, dhdp->dstats.tx_bytes,
+	            dhdp->dstats.tx_errors, dhdp->dstats.tx_dropped);
+	bcm_bprintf(strbuf, "rx_packets %ld rx_bytes %ld rx_errors %ld rx_dropped %ld\n",
+	            dhdp->dstats.rx_packets, dhdp->dstats.rx_bytes,
+	            dhdp->dstats.rx_errors, dhdp->dstats.rx_dropped);
+	bcm_bprintf(strbuf, "multicast %ld\n", dhdp->dstats.multicast);
+
+	bcm_bprintf(strbuf, "bus stats:\n");
+	bcm_bprintf(strbuf, "tx_packets %ld tx_multicast %ld tx_errors %ld\n",
+	            dhdp->tx_packets, dhdp->tx_multicast, dhdp->tx_errors);
+	bcm_bprintf(strbuf, "tx_ctlpkts %ld tx_ctlerrs %ld\n",
+	            dhdp->tx_ctlpkts, dhdp->tx_ctlerrs);
+	bcm_bprintf(strbuf, "rx_packets %ld rx_multicast %ld rx_errors %ld \n",
+	            dhdp->rx_packets, dhdp->rx_multicast, dhdp->rx_errors);
+	bcm_bprintf(strbuf, "rx_ctlpkts %ld rx_ctlerrs %ld rx_dropped %ld\n",
+	            dhdp->rx_ctlpkts, dhdp->rx_ctlerrs, dhdp->rx_dropped);
+	bcm_bprintf(strbuf, "rx_readahead_cnt %ld tx_realloc %ld\n",
+	            dhdp->rx_readahead_cnt, dhdp->tx_realloc);
+	bcm_bprintf(strbuf, "\n");
+
+	/* Add any prot info */
+	dhd_prot_dump(dhdp, strbuf);
+	bcm_bprintf(strbuf, "\n");
+
+	/* Add any bus info */
+	dhd_bus_dump(dhdp, strbuf);
+
+	return (!strbuf->size ? BCME_BUFTOOSHORT : 0);
+}
+
+int
+dhd_wl_ioctl_cmd(dhd_pub_t *dhd_pub, int cmd, void *arg, int len, uint8 set, int ifindex)
+{
+	wl_ioctl_t ioc;
+
+	ioc.cmd = cmd;
+	ioc.buf = arg;
+	ioc.len = len;
+	ioc.set = set;
+
+	return dhd_wl_ioctl(dhd_pub, ifindex, &ioc, arg, len);
+}
+
+
+int
+dhd_wl_ioctl(dhd_pub_t *dhd_pub, int ifindex, wl_ioctl_t *ioc, void *buf, int len)
+{
+	int ret;
+
+	dhd_os_proto_block(dhd_pub);
+
+	ret = dhd_prot_ioctl(dhd_pub, ifindex, ioc, buf, len);
+	if (ret)
+		dhd_os_check_hang(dhd_pub, ifindex, ret);
+
+	dhd_os_proto_unblock(dhd_pub);
+	return ret;
+}
+
+static int
+dhd_doiovar(dhd_pub_t *dhd_pub, const bcm_iovar_t *vi, uint32 actionid, const char *name,
+            void *params, int plen, void *arg, int len, int val_size)
+{
+	int bcmerror = 0;
+	int32 int_val = 0;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+	DHD_TRACE(("%s: actionid = %d; name %s\n", __FUNCTION__, actionid, name));
+
+	if ((bcmerror = bcm_iovar_lencheck(vi, arg, len, IOV_ISSET(actionid))) != 0)
+		goto exit;
+
+	if (plen >= (int)sizeof(int_val))
+		bcopy(params, &int_val, sizeof(int_val));
+
+	switch (actionid) {
+	case IOV_GVAL(IOV_VERSION):
+		/* Need to have checked buffer length */
+		bcm_strncpy_s((char*)arg, len, dhd_version, len);
+		break;
+
+	case IOV_GVAL(IOV_MSGLEVEL):
+		int_val = (int32)dhd_msg_level;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_MSGLEVEL):
+		dhd_msg_level = int_val;
+#ifdef WL_CFG80211
+		/* Enable DHD and WL logs in oneshot */
+		if (dhd_msg_level & DHD_WL_VAL)
+			wl_cfg80211_enable_trace(dhd_msg_level);
+#endif
+		break;
+	case IOV_GVAL(IOV_BCMERRORSTR):
+		bcm_strncpy_s((char *)arg, len, bcmerrorstr(dhd_pub->bcmerror), BCME_STRLEN);
+		((char *)arg)[BCME_STRLEN - 1] = 0x00;
+		break;
+
+	case IOV_GVAL(IOV_BCMERROR):
+		int_val = (int32)dhd_pub->bcmerror;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_WDTICK):
+		int_val = (int32)dhd_watchdog_ms;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_WDTICK):
+		if (!dhd_pub->up) {
+			bcmerror = BCME_NOTUP;
+			break;
+		}
+		dhd_os_wd_timer(dhd_pub, (uint)int_val);
+		break;
+
+	case IOV_GVAL(IOV_DUMP):
+		bcmerror = dhd_dump(dhd_pub, arg, len);
+		break;
+
+#ifdef DHD_DEBUG
+	case IOV_GVAL(IOV_DCONSOLE_POLL):
+		int_val = (int32)dhd_console_ms;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_DCONSOLE_POLL):
+		dhd_console_ms = (uint)int_val;
+		break;
+
+	case IOV_SVAL(IOV_CONS):
+		if (len > 0)
+			bcmerror = dhd_bus_console_in(dhd_pub, arg, len - 1);
+		break;
+#endif /* DHD_DEBUG */
+
+	case IOV_SVAL(IOV_CLEARCOUNTS):
+		dhd_pub->tx_packets = dhd_pub->rx_packets = 0;
+		dhd_pub->tx_errors = dhd_pub->rx_errors = 0;
+		dhd_pub->tx_ctlpkts = dhd_pub->rx_ctlpkts = 0;
+		dhd_pub->tx_ctlerrs = dhd_pub->rx_ctlerrs = 0;
+		dhd_pub->rx_dropped = 0;
+		dhd_pub->rx_readahead_cnt = 0;
+		dhd_pub->tx_realloc = 0;
+		dhd_pub->wd_dpc_sched = 0;
+		memset(&dhd_pub->dstats, 0, sizeof(dhd_pub->dstats));
+		dhd_bus_clearcounts(dhd_pub);
+#ifdef PROP_TXSTATUS
+		/* clear proptxstatus related counters */
+		if (dhd_pub->wlfc_state) {
+			athost_wl_status_info_t *wlfc =
+			        (athost_wl_status_info_t*)dhd_pub->wlfc_state;
+			wlfc_hanger_t* hanger;
+
+			memset(&wlfc->stats, 0, sizeof(athost_wl_stat_counters_t));
+
+			hanger = (wlfc_hanger_t*)wlfc->hanger;
+			hanger->pushed = 0;
+			hanger->popped = 0;
+			hanger->failed_slotfind = 0;
+			hanger->failed_to_pop = 0;
+			hanger->failed_to_push = 0;
+		}
+#endif /* PROP_TXSTATUS */
+		break;
+
+
+	case IOV_GVAL(IOV_IOCTLTIMEOUT): {
+		int_val = (int32)dhd_os_get_ioctl_resp_timeout();
+		bcopy(&int_val, arg, sizeof(int_val));
+		break;
+	}
+
+	case IOV_SVAL(IOV_IOCTLTIMEOUT): {
+		if (int_val <= 0)
+			bcmerror = BCME_BADARG;
+		else
+			dhd_os_set_ioctl_resp_timeout((unsigned int)int_val);
+		break;
+	}
+
+	case IOV_SVAL(IOV_HCI_CMD): {
+		amp_hci_cmd_t *cmd = (amp_hci_cmd_t *)arg;
+
+		/* sanity check: command preamble present */
+		if (len < HCI_CMD_PREAMBLE_SIZE)
+			return BCME_BUFTOOSHORT;
+
+		/* sanity check: command parameters are present */
+		if (len < (int)(HCI_CMD_PREAMBLE_SIZE + cmd->plen))
+			return BCME_BUFTOOSHORT;
+
+		dhd_bta_docmd(dhd_pub, cmd, len);
+		break;
+	}
+
+	case IOV_SVAL(IOV_HCI_ACL_DATA): {
+		amp_hci_ACL_data_t *ACL_data = (amp_hci_ACL_data_t *)arg;
+
+		/* sanity check: HCI header present */
+		if (len < HCI_ACL_DATA_PREAMBLE_SIZE)
+			return BCME_BUFTOOSHORT;
+
+		/* sanity check: ACL data is present */
+		if (len < (int)(HCI_ACL_DATA_PREAMBLE_SIZE + ACL_data->dlen))
+			return BCME_BUFTOOSHORT;
+
+		dhd_bta_tx_hcidata(dhd_pub, ACL_data, len);
+		break;
+	}
+
+#ifdef PROP_TXSTATUS
+	case IOV_GVAL(IOV_PROPTXSTATUS_ENABLE):
+		int_val = dhd_pub->wlfc_enabled? 1 : 0;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_PROPTXSTATUS_ENABLE):
+		dhd_pub->wlfc_enabled = int_val? 1 : 0;
+		break;
+
+	case IOV_GVAL(IOV_PROPTXSTATUS_MODE): {
+		athost_wl_status_info_t *wlfc =
+		        (athost_wl_status_info_t*)dhd_pub->wlfc_state;
+		int_val = dhd_pub->wlfc_state ? (int32)wlfc->proptxstatus_mode : 0;
+		bcopy(&int_val, arg, val_size);
+		break;
+	}
+
+	case IOV_SVAL(IOV_PROPTXSTATUS_MODE):
+		if (dhd_pub->wlfc_state) {
+			athost_wl_status_info_t *wlfc =
+			        (athost_wl_status_info_t*)dhd_pub->wlfc_state;
+			wlfc->proptxstatus_mode = int_val & 0xff;
+		}
+		break;
+#endif /* PROP_TXSTATUS */
+
+	case IOV_GVAL(IOV_BUS_TYPE):
+		/* The dhd application queries the driver to check if its usb or sdio.  */
+#ifdef BCMDHDUSB
+		int_val = BUS_TYPE_USB;
+#endif
+		int_val = BUS_TYPE_SDIO;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+
+#ifdef WLMEDIA_HTSF
+	case IOV_GVAL(IOV_WLPKTDLYSTAT_SZ):
+		int_val = dhd_pub->htsfdlystat_sz;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_WLPKTDLYSTAT_SZ):
+		dhd_pub->htsfdlystat_sz = int_val & 0xff;
+		printf("Setting tsfdlystat_sz:%d\n", dhd_pub->htsfdlystat_sz);
+		break;
+#endif
+	case IOV_SVAL(IOV_CHANGEMTU):
+		int_val &= 0xffff;
+		bcmerror = dhd_change_mtu(dhd_pub, int_val, 0);
+		break;
+
+	default:
+		bcmerror = BCME_UNSUPPORTED;
+		break;
+	}
+
+exit:
+	DHD_TRACE(("%s: actionid %d, bcmerror %d\n", __FUNCTION__, actionid, bcmerror));
+	return bcmerror;
+}
+
+/* Store the status of a connection attempt for later retrieval by an iovar */
+void
+dhd_store_conn_status(uint32 event, uint32 status, uint32 reason)
+{
+	/* Do not overwrite a WLC_E_PRUNE with a WLC_E_SET_SSID
+	 * because an encryption/rsn mismatch results in both events, and
+	 * the important information is in the WLC_E_PRUNE.
+	 */
+	if (!(event == WLC_E_SET_SSID && status == WLC_E_STATUS_FAIL &&
+	      dhd_conn_event == WLC_E_PRUNE)) {
+		dhd_conn_event = event;
+		dhd_conn_status = status;
+		dhd_conn_reason = reason;
+	}
+}
+
+bool
+dhd_prec_enq(dhd_pub_t *dhdp, struct pktq *q, void *pkt, int prec)
+{
+	void *p;
+	int eprec = -1;		/* precedence to evict from */
+	bool discard_oldest;
+
+	/* Fast case, precedence queue is not full and we are also not
+	 * exceeding total queue length
+	 */
+	if (!pktq_pfull(q, prec) && !pktq_full(q)) {
+		pktq_penq(q, prec, pkt);
+		return TRUE;
+	}
+
+	/* Determine precedence from which to evict packet, if any */
+	if (pktq_pfull(q, prec))
+		eprec = prec;
+	else if (pktq_full(q)) {
+		p = pktq_peek_tail(q, &eprec);
+		ASSERT(p);
+		if (eprec > prec || eprec < 0)
+			return FALSE;
+	}
+
+	/* Evict if needed */
+	if (eprec >= 0) {
+		/* Detect queueing to unconfigured precedence */
+		ASSERT(!pktq_pempty(q, eprec));
+		discard_oldest = AC_BITMAP_TST(dhdp->wme_dp, eprec);
+		if (eprec == prec && !discard_oldest)
+			return FALSE;		/* refuse newer (incoming) packet */
+		/* Evict packet according to discard policy */
+		p = discard_oldest ? pktq_pdeq(q, eprec) : pktq_pdeq_tail(q, eprec);
+		ASSERT(p);
+
+		PKTFREE(dhdp->osh, p, TRUE);
+	}
+
+	/* Enqueue */
+	p = pktq_penq(q, prec, pkt);
+	ASSERT(p);
+
+	return TRUE;
+}
+
+static int
+dhd_iovar_op(dhd_pub_t *dhd_pub, const char *name,
+	void *params, int plen, void *arg, int len, bool set)
+{
+	int bcmerror = 0;
+	int val_size;
+	const bcm_iovar_t *vi = NULL;
+	uint32 actionid;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	ASSERT(name);
+	ASSERT(len >= 0);
+
+	/* Get MUST have return space */
+	ASSERT(set || (arg && len));
+
+	/* Set does NOT take qualifiers */
+	ASSERT(!set || (!params && !plen));
+
+	if ((vi = bcm_iovar_lookup(dhd_iovars, name)) == NULL) {
+		bcmerror = BCME_UNSUPPORTED;
+		goto exit;
+	}
+
+	DHD_CTL(("%s: %s %s, len %d plen %d\n", __FUNCTION__,
+		name, (set ? "set" : "get"), len, plen));
+
+	/* set up 'params' pointer in case this is a set command so that
+	 * the convenience int and bool code can be common to set and get
+	 */
+	if (params == NULL) {
+		params = arg;
+		plen = len;
+	}
+
+	if (vi->type == IOVT_VOID)
+		val_size = 0;
+	else if (vi->type == IOVT_BUFFER)
+		val_size = len;
+	else
+		/* all other types are integer sized */
+		val_size = sizeof(int);
+
+	actionid = set ? IOV_SVAL(vi->varid) : IOV_GVAL(vi->varid);
+
+	bcmerror = dhd_doiovar(dhd_pub, vi, actionid, name, params, plen, arg, len, val_size);
+
+exit:
+	return bcmerror;
+}
+
+int
+dhd_ioctl(dhd_pub_t * dhd_pub, dhd_ioctl_t *ioc, void * buf, uint buflen)
+{
+	int bcmerror = 0;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (!buf) {
+		return BCME_BADARG;
+	}
+
+	switch (ioc->cmd) {
+	case DHD_GET_MAGIC:
+		if (buflen < sizeof(int))
+			bcmerror = BCME_BUFTOOSHORT;
+		else
+			*(int*)buf = DHD_IOCTL_MAGIC;
+		break;
+
+	case DHD_GET_VERSION:
+		if (buflen < sizeof(int))
+			bcmerror = -BCME_BUFTOOSHORT;
+		else
+			*(int*)buf = DHD_IOCTL_VERSION;
+		break;
+
+	case DHD_GET_VAR:
+	case DHD_SET_VAR: {
+		char *arg;
+		uint arglen;
+
+		/* scan past the name to any arguments */
+		for (arg = buf, arglen = buflen; *arg && arglen; arg++, arglen--)
+			;
+
+		if (*arg) {
+			bcmerror = BCME_BUFTOOSHORT;
+			break;
+		}
+
+		/* account for the NUL terminator */
+		arg++, arglen--;
+
+		/* call with the appropriate arguments */
+		if (ioc->cmd == DHD_GET_VAR)
+			bcmerror = dhd_iovar_op(dhd_pub, buf, arg, arglen,
+			buf, buflen, IOV_GET);
+		else
+			bcmerror = dhd_iovar_op(dhd_pub, buf, NULL, 0, arg, arglen, IOV_SET);
+		if (bcmerror != BCME_UNSUPPORTED)
+			break;
+
+		/* not in generic table, try protocol module */
+		if (ioc->cmd == DHD_GET_VAR)
+			bcmerror = dhd_prot_iovar_op(dhd_pub, buf, arg,
+				arglen, buf, buflen, IOV_GET);
+		else
+			bcmerror = dhd_prot_iovar_op(dhd_pub, buf,
+				NULL, 0, arg, arglen, IOV_SET);
+		if (bcmerror != BCME_UNSUPPORTED)
+			break;
+
+		/* if still not found, try bus module */
+		if (ioc->cmd == DHD_GET_VAR) {
+			bcmerror = dhd_bus_iovar_op(dhd_pub, buf,
+				arg, arglen, buf, buflen, IOV_GET);
+		} else {
+			bcmerror = dhd_bus_iovar_op(dhd_pub, buf,
+				NULL, 0, arg, arglen, IOV_SET);
+		}
+
+		break;
+	}
+
+	default:
+		bcmerror = BCME_UNSUPPORTED;
+	}
+
+	return bcmerror;
+}
+
+#ifdef SHOW_EVENTS
+static void
+wl_show_host_event(wl_event_msg_t *event, void *event_data)
+{
+	uint i, status, reason;
+	bool group = FALSE, flush_txq = FALSE, link = FALSE;
+	const char *auth_str;
+	const char *event_name;
+	uchar *buf;
+	char err_msg[256], eabuf[ETHER_ADDR_STR_LEN];
+	uint event_type, flags, auth_type, datalen;
+
+	event_type = ntoh32(event->event_type);
+	flags = ntoh16(event->flags);
+	status = ntoh32(event->status);
+	reason = ntoh32(event->reason);
+	auth_type = ntoh32(event->auth_type);
+	datalen = ntoh32(event->datalen);
+
+	/* debug dump of event messages */
+	sprintf(eabuf, "%02x:%02x:%02x:%02x:%02x:%02x",
+	        (uchar)event->addr.octet[0]&0xff,
+	        (uchar)event->addr.octet[1]&0xff,
+	        (uchar)event->addr.octet[2]&0xff,
+	        (uchar)event->addr.octet[3]&0xff,
+	        (uchar)event->addr.octet[4]&0xff,
+	        (uchar)event->addr.octet[5]&0xff);
+
+	event_name = "UNKNOWN";
+	for (i = 0; i < (uint)bcmevent_names_size; i++)
+		if (bcmevent_names[i].event == event_type)
+			event_name = bcmevent_names[i].name;
+
+	if (flags & WLC_EVENT_MSG_LINK)
+		link = TRUE;
+	if (flags & WLC_EVENT_MSG_GROUP)
+		group = TRUE;
+	if (flags & WLC_EVENT_MSG_FLUSHTXQ)
+		flush_txq = TRUE;
+
+	switch (event_type) {
+	case WLC_E_START:
+	case WLC_E_DEAUTH:
+	case WLC_E_DISASSOC:
+		DHD_EVENT(("MACEVENT: %s, MAC %s\n", event_name, eabuf));
+		break;
+
+	case WLC_E_ASSOC_IND:
+	case WLC_E_REASSOC_IND:
+
+		DHD_EVENT(("MACEVENT: %s, MAC %s\n", event_name, eabuf));
+		break;
+
+	case WLC_E_ASSOC:
+	case WLC_E_REASSOC:
+		if (status == WLC_E_STATUS_SUCCESS) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, SUCCESS\n", event_name, eabuf));
+		} else if (status == WLC_E_STATUS_TIMEOUT) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, TIMEOUT\n", event_name, eabuf));
+		} else if (status == WLC_E_STATUS_FAIL) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, FAILURE, reason %d\n",
+			       event_name, eabuf, (int)reason));
+		} else {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, unexpected status %d\n",
+			       event_name, eabuf, (int)status));
+		}
+		break;
+
+	case WLC_E_DEAUTH_IND:
+	case WLC_E_DISASSOC_IND:
+		DHD_EVENT(("MACEVENT: %s, MAC %s, reason %d\n", event_name, eabuf, (int)reason));
+		break;
+
+	case WLC_E_AUTH:
+	case WLC_E_AUTH_IND:
+		if (auth_type == DOT11_OPEN_SYSTEM)
+			auth_str = "Open System";
+		else if (auth_type == DOT11_SHARED_KEY)
+			auth_str = "Shared Key";
+		else {
+			sprintf(err_msg, "AUTH unknown: %d", (int)auth_type);
+			auth_str = err_msg;
+		}
+		if (event_type == WLC_E_AUTH_IND) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, %s\n", event_name, eabuf, auth_str));
+		} else if (status == WLC_E_STATUS_SUCCESS) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, %s, SUCCESS\n",
+				event_name, eabuf, auth_str));
+		} else if (status == WLC_E_STATUS_TIMEOUT) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, %s, TIMEOUT\n",
+				event_name, eabuf, auth_str));
+		} else if (status == WLC_E_STATUS_FAIL) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s, %s, FAILURE, reason %d\n",
+			       event_name, eabuf, auth_str, (int)reason));
+		}
+
+		break;
+
+	case WLC_E_JOIN:
+	case WLC_E_ROAM:
+	case WLC_E_SET_SSID:
+		if (status == WLC_E_STATUS_SUCCESS) {
+			DHD_EVENT(("MACEVENT: %s, MAC %s\n", event_name, eabuf));
+		} else if (status == WLC_E_STATUS_FAIL) {
+			DHD_EVENT(("MACEVENT: %s, failed\n", event_name));
+		} else if (status == WLC_E_STATUS_NO_NETWORKS) {
+			DHD_EVENT(("MACEVENT: %s, no networks found\n", event_name));
+		} else {
+			DHD_EVENT(("MACEVENT: %s, unexpected status %d\n",
+				event_name, (int)status));
+		}
+		break;
+
+	case WLC_E_BEACON_RX:
+		if (status == WLC_E_STATUS_SUCCESS) {
+			DHD_EVENT(("MACEVENT: %s, SUCCESS\n", event_name));
+		} else if (status == WLC_E_STATUS_FAIL) {
+			DHD_EVENT(("MACEVENT: %s, FAIL\n", event_name));
+		} else {
+			DHD_EVENT(("MACEVENT: %s, status %d\n", event_name, status));
+		}
+		break;
+
+	case WLC_E_LINK:
+		DHD_EVENT(("MACEVENT: %s %s\n", event_name, link?"UP":"DOWN"));
+		break;
+
+	case WLC_E_MIC_ERROR:
+		DHD_EVENT(("MACEVENT: %s, MAC %s, Group %d, Flush %d\n",
+		       event_name, eabuf, group, flush_txq));
+		break;
+
+	case WLC_E_ICV_ERROR:
+	case WLC_E_UNICAST_DECODE_ERROR:
+	case WLC_E_MULTICAST_DECODE_ERROR:
+		DHD_EVENT(("MACEVENT: %s, MAC %s\n",
+		       event_name, eabuf));
+		break;
+
+	case WLC_E_TXFAIL:
+		DHD_EVENT(("MACEVENT: %s, RA %s\n", event_name, eabuf));
+		break;
+
+	case WLC_E_SCAN_COMPLETE:
+	case WLC_E_ASSOC_REQ_IE:
+	case WLC_E_ASSOC_RESP_IE:
+	case WLC_E_PMKID_CACHE:
+		DHD_EVENT(("MACEVENT: %s\n", event_name));
+		break;
+
+	case WLC_E_PFN_NET_FOUND:
+	case WLC_E_PFN_NET_LOST:
+	case WLC_E_PFN_SCAN_COMPLETE:
+	case WLC_E_PFN_SCAN_NONE:
+	case WLC_E_PFN_SCAN_ALLGONE:
+		DHD_EVENT(("PNOEVENT: %s\n", event_name));
+		break;
+
+	case WLC_E_PSK_SUP:
+	case WLC_E_PRUNE:
+		DHD_EVENT(("MACEVENT: %s, status %d, reason %d\n",
+		           event_name, (int)status, (int)reason));
+		break;
+
+#ifdef WIFI_ACT_FRAME
+	case WLC_E_ACTION_FRAME:
+		DHD_TRACE(("MACEVENT: %s Bssid %s\n", event_name, eabuf));
+		break;
+#endif /* WIFI_ACT_FRAME */
+
+	case WLC_E_TRACE: {
+		static uint32 seqnum_prev = 0;
+		msgtrace_hdr_t hdr;
+		uint32 nblost;
+		char *s, *p;
+
+		buf = (uchar *) event_data;
+		memcpy(&hdr, buf, MSGTRACE_HDRLEN);
+
+		if (hdr.version != MSGTRACE_VERSION) {
+			printf("\nMACEVENT: %s [unsupported version --> "
+			       "dhd version:%d dongle version:%d]\n",
+			       event_name, MSGTRACE_VERSION, hdr.version);
+			/* Reset datalen to avoid display below */
+			datalen = 0;
+			break;
+		}
+
+		/* There are 2 bytes available at the end of data */
+		buf[MSGTRACE_HDRLEN + ntoh16(hdr.len)] = '\0';
+
+		if (ntoh32(hdr.discarded_bytes) || ntoh32(hdr.discarded_printf)) {
+			printf("\nWLC_E_TRACE: [Discarded traces in dongle -->"
+			       "discarded_bytes %d discarded_printf %d]\n",
+			       ntoh32(hdr.discarded_bytes), ntoh32(hdr.discarded_printf));
+		}
+
+		nblost = ntoh32(hdr.seqnum) - seqnum_prev - 1;
+		if (nblost > 0) {
+			printf("\nWLC_E_TRACE: [Event lost --> seqnum %d nblost %d\n",
+			       ntoh32(hdr.seqnum), nblost);
+		}
+		seqnum_prev = ntoh32(hdr.seqnum);
+
+		/* Display the trace buffer. Advance from \n to \n to avoid display big
+		 * printf (issue with Linux printk )
+		 */
+		p = (char *)&buf[MSGTRACE_HDRLEN];
+		while ((s = strstr(p, "\n")) != NULL) {
+			*s = '\0';
+			printf("%s\n", p);
+			p = s+1;
+		}
+		printf("%s\n", p);
+
+		/* Reset datalen to avoid display below */
+		datalen = 0;
+		break;
+	}
+
+
+	case WLC_E_RSSI:
+		DHD_EVENT(("MACEVENT: %s %d\n", event_name, ntoh32(*((int *)event_data))));
+		break;
+
+	default:
+		DHD_EVENT(("MACEVENT: %s %d, MAC %s, status %d, reason %d, auth %d\n",
+		       event_name, event_type, eabuf, (int)status, (int)reason,
+		       (int)auth_type));
+		break;
+	}
+
+	/* show any appended data */
+	if (datalen) {
+		buf = (uchar *) event_data;
+		DHD_EVENT((" data (%d) : ", datalen));
+		for (i = 0; i < datalen; i++)
+			DHD_EVENT((" 0x%02x ", *buf++));
+		DHD_EVENT(("\n"));
+	}
+}
+#endif /* SHOW_EVENTS */
+
+int
+wl_host_event(dhd_pub_t *dhd_pub, int *ifidx, void *pktdata,
+              wl_event_msg_t *event, void **data_ptr)
+{
+	/* check whether packet is a BRCM event pkt */
+	bcm_event_t *pvt_data = (bcm_event_t *)pktdata;
+	uint8 *event_data;
+	uint32 type, status, reason, datalen;
+	uint16 flags;
+	int evlen;
+
+	if (bcmp(BRCM_OUI, &pvt_data->bcm_hdr.oui[0], DOT11_OUI_LEN)) {
+		DHD_ERROR(("%s: mismatched OUI, bailing\n", __FUNCTION__));
+		return (BCME_ERROR);
+	}
+
+	/* BRCM event pkt may be unaligned - use xxx_ua to load user_subtype. */
+	if (ntoh16_ua((void *)&pvt_data->bcm_hdr.usr_subtype) != BCMILCP_BCM_SUBTYPE_EVENT) {
+		DHD_ERROR(("%s: mismatched subtype, bailing\n", __FUNCTION__));
+		return (BCME_ERROR);
+	}
+
+	*data_ptr = &pvt_data[1];
+	event_data = *data_ptr;
+
+	/* memcpy since BRCM event pkt may be unaligned. */
+	memcpy(event, &pvt_data->event, sizeof(wl_event_msg_t));
+
+	type = ntoh32_ua((void *)&event->event_type);
+	flags = ntoh16_ua((void *)&event->flags);
+	status = ntoh32_ua((void *)&event->status);
+	reason = ntoh32_ua((void *)&event->reason);
+	datalen = ntoh32_ua((void *)&event->datalen);
+	evlen = datalen + sizeof(bcm_event_t);
+
+	DHD_TRACE(("RX: event_type:%d flags:%d status:%d reason:%d \n",
+								type, flags, status, reason));
+
+	switch (type) {
+#ifdef PROP_TXSTATUS
+	case WLC_E_FIFO_CREDIT_MAP:
+		dhd_wlfc_event(dhd_pub->info);
+		dhd_wlfc_FIFOcreditmap_event(dhd_pub->info, event_data);
+		WLFC_DBGMESG(("WLC_E_FIFO_CREDIT_MAP:(AC0,AC1,AC2,AC3),(BC_MC),(OTHER): "
+			"(%d,%d,%d,%d),(%d),(%d)\n", event_data[0], event_data[1],
+			event_data[2],
+			event_data[3], event_data[4], event_data[5]));
+		break;
+#endif
+
+	case WLC_E_IF:
+		{
+			dhd_if_event_t *ifevent = (dhd_if_event_t *)event_data;
+#ifdef PROP_TXSTATUS
+			{
+				uint8* ea = pvt_data->eth.ether_dhost;
+				WLFC_DBGMESG(("WLC_E_IF: idx:%d, action:%s, iftype:%s, "
+						"[%02x:%02x:%02x:%02x:%02x:%02x]\n",
+						ifevent->ifidx,
+						((ifevent->action == WLC_E_IF_ADD) ? "ADD":"DEL"),
+						((ifevent->is_AP == 0) ? "STA":"AP "),
+						ea[0], ea[1], ea[2], ea[3], ea[4], ea[5]));
+				(void)ea;
+
+				dhd_wlfc_interface_event(dhd_pub->info,
+					((ifevent->action == WLC_E_IF_ADD) ?
+					eWLFC_MAC_ENTRY_ACTION_ADD : eWLFC_MAC_ENTRY_ACTION_DEL),
+					ifevent->ifidx, ifevent->is_AP, ea);
+
+				/* dhd already has created an interface by default, for 0 */
+				if (ifevent->ifidx == 0)
+					break;
+			}
+#endif /* PROP_TXSTATUS */
+
+#ifdef WL_CFG80211
+			if (wl_cfg80211_is_progress_ifchange()) {
+				DHD_ERROR(("%s:  ifidx %d for %s action %d\n",
+					__FUNCTION__, ifevent->ifidx,
+					event->ifname, ifevent->action));
+				if (ifevent->action == WLC_E_IF_ADD)
+					wl_cfg80211_notify_ifchange();
+				return (BCME_OK);
+			}
+#endif /* WL_CFG80211 */
+				if (ifevent->ifidx > 0 && ifevent->ifidx < DHD_MAX_IFS) {
+					if (ifevent->action == WLC_E_IF_ADD) {
+						if (dhd_add_if(dhd_pub->info, ifevent->ifidx,
+							NULL, event->ifname,
+							event->addr.octet,
+							ifevent->flags, ifevent->bssidx)) {
+							DHD_ERROR(("%s: dhd_add_if failed!!"
+									" ifidx: %d for %s\n",
+									__FUNCTION__,
+									ifevent->ifidx,
+									event->ifname));
+							return (BCME_ERROR);
+						}
+					}
+			else
+				dhd_del_if(dhd_pub->info, ifevent->ifidx);
+		} else {
+#ifndef PROP_TXSTATUS
+			DHD_ERROR(("%s: Invalid ifidx %d for %s\n",
+			           __FUNCTION__, ifevent->ifidx, event->ifname));
+#endif /* !PROP_TXSTATUS */
+		}
+			}
+			/* send up the if event: btamp user needs it */
+			*ifidx = dhd_ifname2idx(dhd_pub->info, event->ifname);
+			/* push up to external supp/auth */
+			dhd_event(dhd_pub->info, (char *)pvt_data, evlen, *ifidx);
+			break;
+
+
+#ifdef WLMEDIA_HTSF
+	case WLC_E_HTSFSYNC:
+		htsf_update(dhd_pub->info, event_data);
+		break;
+#endif /* WLMEDIA_HTSF */
+	case WLC_E_NDIS_LINK: {
+		uint32 temp = hton32(WLC_E_LINK);
+
+		memcpy((void *)(&pvt_data->event.event_type), &temp,
+		       sizeof(pvt_data->event.event_type));
+	}
+		/* These are what external supplicant/authenticator wants */
+		/* fall through */
+	case WLC_E_LINK:
+	case WLC_E_DEAUTH:
+	case WLC_E_DEAUTH_IND:
+	case WLC_E_DISASSOC:
+	case WLC_E_DISASSOC_IND:
+		DHD_EVENT(("%s: Link event %d, flags %x, status %x\n",
+		           __FUNCTION__, type, flags, status));
+		/* fall through */
+	default:
+		*ifidx = dhd_ifname2idx(dhd_pub->info, event->ifname);
+		/* push up to external supp/auth */
+		dhd_event(dhd_pub->info, (char *)pvt_data, evlen, *ifidx);
+		DHD_TRACE(("%s: MAC event %d, flags %x, status %x\n",
+		           __FUNCTION__, type, flags, status));
+
+		/* put it back to WLC_E_NDIS_LINK */
+		if (type == WLC_E_NDIS_LINK) {
+			uint32 temp;
+
+			temp = ntoh32_ua((void *)&event->event_type);
+			DHD_TRACE(("Converted to WLC_E_LINK type %d\n", temp));
+
+			temp = ntoh32(WLC_E_NDIS_LINK);
+			memcpy((void *)(&pvt_data->event.event_type), &temp,
+			       sizeof(pvt_data->event.event_type));
+		}
+		break;
+	}
+
+#ifdef SHOW_EVENTS
+	wl_show_host_event(event, (void *)event_data);
+#endif /* SHOW_EVENTS */
+
+	return (BCME_OK);
+}
+
+void
+wl_event_to_host_order(wl_event_msg_t * evt)
+{
+	/* Event struct members passed from dongle to host are stored in network
+	 * byte order. Convert all members to host-order.
+	 */
+	evt->event_type = ntoh32(evt->event_type);
+	evt->flags = ntoh16(evt->flags);
+	evt->status = ntoh32(evt->status);
+	evt->reason = ntoh32(evt->reason);
+	evt->auth_type = ntoh32(evt->auth_type);
+	evt->datalen = ntoh32(evt->datalen);
+	evt->version = ntoh16(evt->version);
+}
+
+void
+dhd_print_buf(void *pbuf, int len, int bytes_per_line)
+{
+#ifdef DHD_DEBUG
+	int i, j = 0;
+	unsigned char *buf = pbuf;
+
+	if (bytes_per_line == 0) {
+		bytes_per_line = len;
+	}
+
+	for (i = 0; i < len; i++) {
+		printf("%2.2x", *buf++);
+		j++;
+		if (j == bytes_per_line) {
+			printf("\n");
+			j = 0;
+		} else {
+			printf(":");
+		}
+	}
+	printf("\n");
+#endif /* DHD_DEBUG */
+}
+
+#define strtoul(nptr, endptr, base) bcm_strtoul((nptr), (endptr), (base))
+
+/* Convert user's input in hex pattern to byte-size mask */
+static int
+wl_pattern_atoh(char *src, char *dst)
+{
+	int i;
+	if (strncmp(src, "0x", 2) != 0 &&
+	    strncmp(src, "0X", 2) != 0) {
+		DHD_ERROR(("Mask invalid format. Needs to start with 0x\n"));
+		return -1;
+	}
+	src = src + 2; /* Skip past 0x */
+	if (strlen(src) % 2 != 0) {
+		DHD_ERROR(("Mask invalid format. Needs to be of even length\n"));
+		return -1;
+	}
+	for (i = 0; *src != '\0'; i++) {
+		char num[3];
+		bcm_strncpy_s(num, sizeof(num), src, 2);
+		num[2] = '\0';
+		dst[i] = (uint8)strtoul(num, NULL, 16);
+		src += 2;
+	}
+	return i;
+}
+
+void
+dhd_pktfilter_offload_enable(dhd_pub_t * dhd, char *arg, int enable, int master_mode)
+{
+	char				*argv[8];
+	int					i = 0;
+	const char 			*str;
+	int					buf_len;
+	int					str_len;
+	char				*arg_save = 0, *arg_org = 0;
+	int					rc;
+	char				buf[128];
+	wl_pkt_filter_enable_t	enable_parm;
+	wl_pkt_filter_enable_t	* pkt_filterp;
+
+	if (!arg)
+		return;
+
+	if (!(arg_save = MALLOC(dhd->osh, strlen(arg) + 1))) {
+		DHD_ERROR(("%s: kmalloc failed\n", __FUNCTION__));
+		goto fail;
+	}
+	arg_org = arg_save;
+	memcpy(arg_save, arg, strlen(arg) + 1);
+
+	argv[i] = bcmstrtok(&arg_save, " ", 0);
+
+	i = 0;
+	if (argv[i] == NULL) {
+		DHD_ERROR(("No args provided\n"));
+		goto fail;
+	}
+
+	str = "pkt_filter_enable";
+	str_len = strlen(str);
+	bcm_strncpy_s(buf, sizeof(buf), str, str_len);
+	buf[str_len] = '\0';
+	buf_len = str_len + 1;
+
+	pkt_filterp = (wl_pkt_filter_enable_t *)(buf + str_len + 1);
+
+	/* Parse packet filter id. */
+	enable_parm.id = htod32(strtoul(argv[i], NULL, 0));
+
+	/* Parse enable/disable value. */
+	enable_parm.enable = htod32(enable);
+
+	buf_len += sizeof(enable_parm);
+	memcpy((char *)pkt_filterp,
+	       &enable_parm,
+	       sizeof(enable_parm));
+
+	/* Enable/disable the specified filter. */
+	rc = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, buf, buf_len, TRUE, 0);
+	rc = rc >= 0 ? 0 : rc;
+	if (rc)
+		DHD_TRACE(("%s: failed to add pktfilter %s, retcode = %d\n",
+		__FUNCTION__, arg, rc));
+	else
+		DHD_TRACE(("%s: successfully added pktfilter %s\n",
+		__FUNCTION__, arg));
+
+	/* Contorl the master mode */
+	bcm_mkiovar("pkt_filter_mode", (char *)&master_mode, 4, buf, sizeof(buf));
+	rc = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, buf, sizeof(buf), TRUE, 0);
+	rc = rc >= 0 ? 0 : rc;
+	if (rc)
+		DHD_TRACE(("%s: failed to add pktfilter %s, retcode = %d\n",
+		__FUNCTION__, arg, rc));
+
+fail:
+	if (arg_org)
+		MFREE(dhd->osh, arg_org, strlen(arg) + 1);
+}
+
+void
+dhd_pktfilter_offload_set(dhd_pub_t * dhd, char *arg)
+{
+	const char 			*str;
+	wl_pkt_filter_t		pkt_filter;
+	wl_pkt_filter_t		*pkt_filterp;
+	int					buf_len;
+	int					str_len;
+	int 				rc;
+	uint32				mask_size;
+	uint32				pattern_size;
+	char				*argv[8], * buf = 0;
+	int					i = 0;
+	char				*arg_save = 0, *arg_org = 0;
+#define BUF_SIZE		2048
+
+	if (!arg)
+		return;
+
+	if (!(arg_save = MALLOC(dhd->osh, strlen(arg) + 1))) {
+		DHD_ERROR(("%s: kmalloc failed\n", __FUNCTION__));
+		goto fail;
+	}
+
+	arg_org = arg_save;
+
+	if (!(buf = MALLOC(dhd->osh, BUF_SIZE))) {
+		DHD_ERROR(("%s: kmalloc failed\n", __FUNCTION__));
+		goto fail;
+	}
+
+	memcpy(arg_save, arg, strlen(arg) + 1);
+
+	if (strlen(arg) > BUF_SIZE) {
+		DHD_ERROR(("Not enough buffer %d < %d\n", (int)strlen(arg), (int)sizeof(buf)));
+		goto fail;
+	}
+
+	argv[i] = bcmstrtok(&arg_save, " ", 0);
+	while (argv[i++])
+		argv[i] = bcmstrtok(&arg_save, " ", 0);
+
+	i = 0;
+	if (argv[i] == NULL) {
+		DHD_ERROR(("No args provided\n"));
+		goto fail;
+	}
+
+	str = "pkt_filter_add";
+	str_len = strlen(str);
+	bcm_strncpy_s(buf, BUF_SIZE, str, str_len);
+	buf[ str_len ] = '\0';
+	buf_len = str_len + 1;
+
+	pkt_filterp = (wl_pkt_filter_t *) (buf + str_len + 1);
+
+	/* Parse packet filter id. */
+	pkt_filter.id = htod32(strtoul(argv[i], NULL, 0));
+
+	if (argv[++i] == NULL) {
+		DHD_ERROR(("Polarity not provided\n"));
+		goto fail;
+	}
+
+	/* Parse filter polarity. */
+	pkt_filter.negate_match = htod32(strtoul(argv[i], NULL, 0));
+
+	if (argv[++i] == NULL) {
+		DHD_ERROR(("Filter type not provided\n"));
+		goto fail;
+	}
+
+	/* Parse filter type. */
+	pkt_filter.type = htod32(strtoul(argv[i], NULL, 0));
+
+	if (argv[++i] == NULL) {
+		DHD_ERROR(("Offset not provided\n"));
+		goto fail;
+	}
+
+	/* Parse pattern filter offset. */
+	pkt_filter.u.pattern.offset = htod32(strtoul(argv[i], NULL, 0));
+
+	if (argv[++i] == NULL) {
+		DHD_ERROR(("Bitmask not provided\n"));
+		goto fail;
+	}
+
+	/* Parse pattern filter mask. */
+	mask_size =
+		htod32(wl_pattern_atoh(argv[i], (char *) pkt_filterp->u.pattern.mask_and_pattern));
+
+	if (argv[++i] == NULL) {
+		DHD_ERROR(("Pattern not provided\n"));
+		goto fail;
+	}
+
+	/* Parse pattern filter pattern. */
+	pattern_size =
+		htod32(wl_pattern_atoh(argv[i],
+	         (char *) &pkt_filterp->u.pattern.mask_and_pattern[mask_size]));
+
+	if (mask_size != pattern_size) {
+		DHD_ERROR(("Mask and pattern not the same size\n"));
+		goto fail;
+	}
+
+	pkt_filter.u.pattern.size_bytes = mask_size;
+	buf_len += WL_PKT_FILTER_FIXED_LEN;
+	buf_len += (WL_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,
+	       WL_PKT_FILTER_FIXED_LEN + WL_PKT_FILTER_PATTERN_FIXED_LEN);
+
+	rc = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, buf, buf_len, TRUE, 0);
+	rc = rc >= 0 ? 0 : rc;
+
+	if (rc)
+		DHD_TRACE(("%s: failed to add pktfilter %s, retcode = %d\n",
+		__FUNCTION__, arg, rc));
+	else
+		DHD_TRACE(("%s: successfully added pktfilter %s\n",
+		__FUNCTION__, arg));
+
+fail:
+	if (arg_org)
+		MFREE(dhd->osh, arg_org, strlen(arg) + 1);
+
+	if (buf)
+		MFREE(dhd->osh, buf, BUF_SIZE);
+}
+
+/* ========================== */
+/* ==== ARP OFFLOAD SUPPORT = */
+/* ========================== */
+#ifdef ARP_OFFLOAD_SUPPORT
+void
+dhd_arp_offload_set(dhd_pub_t * dhd, int arp_mode)
+{
+	char iovbuf[32];
+	int retcode;
+
+	bcm_mkiovar("arp_ol", (char *)&arp_mode, 4, iovbuf, sizeof(iovbuf));
+	retcode = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
+	retcode = retcode >= 0 ? 0 : retcode;
+	if (retcode)
+		DHD_TRACE(("%s: failed to set ARP offload mode to 0x%x, retcode = %d\n",
+		__FUNCTION__, arp_mode, retcode));
+	else
+		DHD_TRACE(("%s: successfully set ARP offload mode to 0x%x\n",
+		__FUNCTION__, arp_mode));
+}
+
+void
+dhd_arp_offload_enable(dhd_pub_t * dhd, int arp_enable)
+{
+	char iovbuf[32];
+	int retcode;
+
+	bcm_mkiovar("arpoe", (char *)&arp_enable, 4, iovbuf, sizeof(iovbuf));
+	retcode = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
+	retcode = retcode >= 0 ? 0 : retcode;
+	if (retcode)
+		DHD_TRACE(("%s: failed to enabe ARP offload to %d, retcode = %d\n",
+		__FUNCTION__, arp_enable, retcode));
+	else
+		DHD_TRACE(("%s: successfully enabed ARP offload to %d\n",
+		__FUNCTION__, arp_enable));
+}
+
+void
+dhd_aoe_arp_clr(dhd_pub_t *dhd)
+{
+	int ret = 0;
+	int iov_len = 0;
+	char iovbuf[128];
+
+	if (dhd == NULL) return;
+
+	iov_len = bcm_mkiovar("arp_table_clear", 0, 0, iovbuf, sizeof(iovbuf));
+	if ((ret  = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, iov_len, TRUE, 0) < 0))
+		DHD_ERROR(("%s failed code %d\n", __FUNCTION__, ret));
+}
+
+void
+dhd_aoe_hostip_clr(dhd_pub_t *dhd)
+{
+	int ret = 0;
+	int iov_len = 0;
+	char iovbuf[128];
+
+	if (dhd == NULL) return;
+
+	iov_len = bcm_mkiovar("arp_hostip_clear", 0, 0, iovbuf, sizeof(iovbuf));
+	if ((ret  = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, iov_len, TRUE, 0)) < 0)
+		DHD_ERROR(("%s failed code %d\n", __FUNCTION__, ret));
+}
+
+void
+dhd_arp_offload_add_ip(dhd_pub_t *dhd, uint32 ipaddr)
+{
+	int iov_len = 0;
+	char iovbuf[32];
+	int retcode;
+
+	iov_len = bcm_mkiovar("arp_hostip", (char *)&ipaddr, 4, iovbuf, sizeof(iovbuf));
+	retcode = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, iov_len, TRUE, 0);
+
+	if (retcode)
+		DHD_TRACE(("%s: ARP ip addr add failed, retcode = %d\n",
+		__FUNCTION__, retcode));
+	else
+		DHD_TRACE(("%s: sARP H ipaddr entry added \n",
+		__FUNCTION__));
+}
+
+int
+dhd_arp_get_arp_hostip_table(dhd_pub_t *dhd, void *buf, int buflen)
+{
+	int retcode, i;
+	int iov_len = 0;
+	uint32 *ptr32 = buf;
+	bool clr_bottom = FALSE;
+
+	if (!buf)
+		return -1;
+
+	iov_len = bcm_mkiovar("arp_hostip", 0, 0, buf, buflen);
+	retcode = dhd_wl_ioctl_cmd(dhd, WLC_GET_VAR, buf, buflen, FALSE, 0);
+
+	if (retcode) {
+		DHD_TRACE(("%s: ioctl WLC_GET_VAR error %d\n",
+		__FUNCTION__, retcode));
+
+		return -1;
+	}
+
+	/* clean up the buf, ascii reminder */
+	for (i = 0; i < MAX_IPV4_ENTRIES; i++) {
+		if (!clr_bottom) {
+			if (*ptr32 == 0)
+				clr_bottom = TRUE;
+		} else {
+			*ptr32 = 0;
+		}
+		ptr32++;
+	}
+
+	return 0;
+}
+#endif /* ARP_OFFLOAD_SUPPORT  */
+
+/* send up locally generated event */
+void
+dhd_sendup_event_common(dhd_pub_t *dhdp, wl_event_msg_t *event, void *data)
+{
+	switch (ntoh32(event->event_type)) {
+	case WLC_E_BTA_HCI_EVENT:
+		break;
+	default:
+		break;
+	}
+
+	/* Call per-port handler. */
+	dhd_sendup_event(dhdp, event, data);
+}
+
+#ifdef SIMPLE_ISCAN
+
+uint iscan_thread_id = 0;
+iscan_buf_t * iscan_chain = 0;
+
+iscan_buf_t *
+dhd_iscan_allocate_buf(dhd_pub_t *dhd, iscan_buf_t **iscanbuf)
+{
+	iscan_buf_t *iscanbuf_alloc = 0;
+	iscan_buf_t *iscanbuf_head;
+
+	DHD_ISCAN(("%s: Entered\n", __FUNCTION__));
+	dhd_iscan_lock();
+
+	iscanbuf_alloc = (iscan_buf_t*)MALLOC(dhd->osh, sizeof(iscan_buf_t));
+	if (iscanbuf_alloc == NULL)
+		goto fail;
+
+	iscanbuf_alloc->next = NULL;
+	iscanbuf_head = *iscanbuf;
+
+	DHD_ISCAN(("%s: addr of allocated node = 0x%X"
+		   "addr of iscanbuf_head = 0x%X dhd = 0x%X\n",
+		   __FUNCTION__, iscanbuf_alloc, iscanbuf_head, dhd));
+
+	if (iscanbuf_head == NULL) {
+		*iscanbuf = iscanbuf_alloc;
+		DHD_ISCAN(("%s: Head is allocated\n", __FUNCTION__));
+		goto fail;
+	}
+
+	while (iscanbuf_head->next)
+		iscanbuf_head = iscanbuf_head->next;
+
+	iscanbuf_head->next = iscanbuf_alloc;
+
+fail:
+	dhd_iscan_unlock();
+	return iscanbuf_alloc;
+}
+
+void
+dhd_iscan_free_buf(void *dhdp, iscan_buf_t *iscan_delete)
+{
+	iscan_buf_t *iscanbuf_free = 0;
+	iscan_buf_t *iscanbuf_prv = 0;
+	iscan_buf_t *iscanbuf_cur;
+	dhd_pub_t *dhd = dhd_bus_pub(dhdp);
+	DHD_ISCAN(("%s: Entered\n", __FUNCTION__));
+
+	dhd_iscan_lock();
+
+	iscanbuf_cur = iscan_chain;
+
+	/* If iscan_delete is null then delete the entire
+	 * chain or else delete specific one provided
+	 */
+	if (!iscan_delete) {
+		while (iscanbuf_cur) {
+			iscanbuf_free = iscanbuf_cur;
+			iscanbuf_cur = iscanbuf_cur->next;
+			iscanbuf_free->next = 0;
+			MFREE(dhd->osh, iscanbuf_free, sizeof(iscan_buf_t));
+		}
+		iscan_chain = 0;
+	} else {
+		while (iscanbuf_cur) {
+			if (iscanbuf_cur == iscan_delete)
+				break;
+			iscanbuf_prv = iscanbuf_cur;
+			iscanbuf_cur = iscanbuf_cur->next;
+		}
+		if (iscanbuf_prv)
+			iscanbuf_prv->next = iscan_delete->next;
+
+		iscan_delete->next = 0;
+		MFREE(dhd->osh, iscan_delete, sizeof(iscan_buf_t));
+
+		if (!iscanbuf_prv)
+			iscan_chain = 0;
+	}
+	dhd_iscan_unlock();
+}
+
+iscan_buf_t *
+dhd_iscan_result_buf(void)
+{
+	return iscan_chain;
+}
+
+int
+dhd_iscan_issue_request(void * dhdp, wl_iscan_params_t *pParams, uint32 size)
+{
+	int rc = -1;
+	dhd_pub_t *dhd = dhd_bus_pub(dhdp);
+	char *buf;
+	char iovar[] = "iscan";
+	uint32 allocSize = 0;
+	wl_ioctl_t ioctl;
+
+	if (pParams) {
+		allocSize = (size + strlen(iovar) + 1);
+		if ((allocSize < size) || (allocSize < strlen(iovar)))
+		{
+			DHD_ERROR(("%s: overflow - allocation size too large %d < %d + %d!\n",
+				__FUNCTION__, allocSize, size, strlen(iovar)));
+			goto cleanUp;
+		}
+		buf = MALLOC(dhd->osh, allocSize);
+
+		if (buf == NULL)
+			{
+			DHD_ERROR(("%s: malloc of size %d failed!\n", __FUNCTION__, allocSize));
+			goto cleanUp;
+			}
+		ioctl.cmd = WLC_SET_VAR;
+		bcm_mkiovar(iovar, (char *)pParams, size, buf, allocSize);
+		rc = dhd_wl_ioctl(dhd, 0, &ioctl, buf, allocSize);
+	}
+
+cleanUp:
+	if (buf) {
+		MFREE(dhd->osh, buf, allocSize);
+	}
+
+	return rc;
+}
+
+static int
+dhd_iscan_get_partial_result(void *dhdp, uint *scan_count)
+{
+	wl_iscan_results_t *list_buf;
+	wl_iscan_results_t list;
+	wl_scan_results_t *results;
+	iscan_buf_t *iscan_cur;
+	int status = -1;
+	dhd_pub_t *dhd = dhd_bus_pub(dhdp);
+	int rc;
+	wl_ioctl_t ioctl;
+
+	DHD_ISCAN(("%s: Enter\n", __FUNCTION__));
+
+	iscan_cur = dhd_iscan_allocate_buf(dhd, &iscan_chain);
+	if (!iscan_cur) {
+		DHD_ERROR(("%s: Failed to allocate node\n", __FUNCTION__));
+		dhd_iscan_free_buf(dhdp, 0);
+		dhd_iscan_request(dhdp, WL_SCAN_ACTION_ABORT);
+		dhd_ind_scan_confirm(dhdp, FALSE);
+		goto fail;
+	}
+
+	dhd_iscan_lock();
+
+	memset(iscan_cur->iscan_buf, 0, WLC_IW_ISCAN_MAXLEN);
+	list_buf = (wl_iscan_results_t*)iscan_cur->iscan_buf;
+	results = &list_buf->results;
+	results->buflen = WL_ISCAN_RESULTS_FIXED_SIZE;
+	results->version = 0;
+	results->count = 0;
+
+	memset(&list, 0, sizeof(list));
+	list.results.buflen = htod32(WLC_IW_ISCAN_MAXLEN);
+	bcm_mkiovar("iscanresults", (char *)&list, WL_ISCAN_RESULTS_FIXED_SIZE,
+		iscan_cur->iscan_buf, WLC_IW_ISCAN_MAXLEN);
+	ioctl.cmd = WLC_GET_VAR;
+	ioctl.set = FALSE;
+	rc = dhd_wl_ioctl(dhd, 0, &ioctl, iscan_cur->iscan_buf, WLC_IW_ISCAN_MAXLEN);
+
+	results->buflen = dtoh32(results->buflen);
+	results->version = dtoh32(results->version);
+	*scan_count = results->count = dtoh32(results->count);
+	status = dtoh32(list_buf->status);
+	DHD_ISCAN(("%s: Got %d resuls status = (%x)\n", __FUNCTION__, results->count, status));
+
+	dhd_iscan_unlock();
+
+	if (!(*scan_count)) {
+		 /* TODO: race condition when FLUSH already called */
+		dhd_iscan_free_buf(dhdp, 0);
+	}
+fail:
+	return status;
+}
+
+#endif /* SIMPLE_ISCAN */
+
+/*
+ * returns = TRUE if associated, FALSE if not associated
+ * third paramter retval can return error from error
+ */
+bool dhd_is_associated(dhd_pub_t *dhd, void *bss_buf, int *retval)
+{
+	char bssid[6], zbuf[6];
+	int ret;
+
+	bzero(bssid, 6);
+	bzero(zbuf, 6);
+
+	ret  = dhd_wl_ioctl_cmd(dhd, WLC_GET_BSSID, (char *)&bssid, ETHER_ADDR_LEN, FALSE, 0);
+	DHD_TRACE((" %s WLC_GET_BSSID ioctl res = %d\n", __FUNCTION__, ret));
+
+	if (retval)
+		*retval = ret;
+
+	if (ret == BCME_NOTASSOCIATED) {
+		DHD_TRACE(("%s: not associated! res:%d\n", __FUNCTION__, ret));
+	}
+
+	if (ret < 0)
+		return FALSE;
+
+	if ((memcmp(bssid, zbuf, ETHER_ADDR_LEN) != 0)) {
+		/*  STA is assocoated BSSID is non zero */
+
+		if (bss_buf) {
+			/* return bss if caller provided buf */
+			memcpy(bss_buf, bssid, ETHER_ADDR_LEN);
+		}
+		return TRUE;
+	} else {
+		DHD_TRACE(("%s: WLC_GET_BSSID ioctl returned zero bssid\n", __FUNCTION__));
+		return FALSE;
+	}
+}
+
+
+/* Function to estimate possible DTIM_SKIP value */
+int
+dhd_get_dtim_skip(dhd_pub_t *dhd)
+{
+	int bcn_li_dtim;
+	int ret = -1;
+	int dtim_assoc = 0;
+
+	if ((dhd->dtim_skip == 0) || (dhd->dtim_skip == 1))
+		bcn_li_dtim = 3;
+	else
+		bcn_li_dtim = dhd->dtim_skip;
+
+	/* Check if associated */
+	if (dhd_is_associated(dhd, NULL, NULL) == FALSE) {
+		DHD_TRACE(("%s NOT assoc ret %d\n", __FUNCTION__, ret));
+		goto exit;
+	}
+
+	/* if assoc grab ap's dtim value */
+	if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_GET_DTIMPRD,
+		&dtim_assoc, sizeof(dtim_assoc), FALSE, 0)) < 0) {
+		DHD_ERROR(("%s failed code %d\n", __FUNCTION__, ret));
+		goto exit;
+	}
+
+	DHD_ERROR(("%s bcn_li_dtim=%d DTIM=%d Listen=%d\n",
+		__FUNCTION__, bcn_li_dtim, dtim_assoc, LISTEN_INTERVAL));
+
+	/* if not assocated just eixt */
+	if (dtim_assoc == 0) {
+		goto exit;
+	}
+
+	/* check if sta listen interval fits into AP dtim */
+	if (dtim_assoc > LISTEN_INTERVAL) {
+		/* AP DTIM to big for our Listen Interval : no dtim skiping */
+		bcn_li_dtim = 1;
+		DHD_ERROR(("%s DTIM=%d > Listen=%d : too big ...\n",
+			__FUNCTION__, dtim_assoc, LISTEN_INTERVAL));
+		goto exit;
+	}
+
+	if ((bcn_li_dtim * dtim_assoc) > LISTEN_INTERVAL) {
+		/* Round up dtim_skip to fit into STAs Listen Interval */
+		bcn_li_dtim = (int)(LISTEN_INTERVAL / dtim_assoc);
+		DHD_TRACE(("%s agjust dtim_skip as %d\n", __FUNCTION__, bcn_li_dtim));
+	}
+
+exit:
+	return bcn_li_dtim;
+}
+
+/* Check if HostAPD or WFD mode setup */
+bool dhd_check_ap_wfd_mode_set(dhd_pub_t *dhd)
+{
+#ifdef  WL_CFG80211
+#ifndef WL_ENABLE_P2P_IF
+	/* To be back compatble with ICS MR1 release where p2p interface
+	 * disable but wlan0 used for p2p
+	 */
+	if (((dhd->op_mode & HOSTAPD_MASK) == HOSTAPD_MASK) ||
+		((dhd->op_mode & WFD_MASK) == WFD_MASK)) {
+		return TRUE;
+	}
+	else
+#else
+	/* concurent mode with p2p interface for wfd and wlan0 for sta */
+	if (((dhd->op_mode & P2P_GO_ENABLED) == P2P_GO_ENABLED) ||
+		((dhd->op_mode & P2P_GC_ENABLED) == P2P_GC_ENABLED)) {
+		DHD_ERROR(("%s P2P enabled for  mode=%d\n", __FUNCTION__, dhd->op_mode));
+		return TRUE;
+	}
+	else
+#endif /* WL_ENABLE_P2P_IF */
+#endif /* WL_CFG80211 */
+		return FALSE;
+}
+
+#ifdef PNO_SUPPORT
+int
+dhd_pno_clean(dhd_pub_t *dhd)
+{
+	char iovbuf[128];
+	int pfn_enabled = 0;
+	int iov_len = 0;
+	int ret;
+
+	/* Disable pfn */
+	iov_len = bcm_mkiovar("pfn", (char *)&pfn_enabled, 4, iovbuf, sizeof(iovbuf));
+	if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0)) >= 0) {
+		/* clear pfn */
+		iov_len = bcm_mkiovar("pfnclear", 0, 0, iovbuf, sizeof(iovbuf));
+		if (iov_len) {
+			if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf,
+			                            iov_len, TRUE, 0)) < 0) {
+				DHD_ERROR(("%s failed code %d\n", __FUNCTION__, ret));
+			}
+		}
+		else {
+			ret = -1;
+			DHD_ERROR(("%s failed code %d\n", __FUNCTION__, iov_len));
+		}
+	}
+	else
+		DHD_ERROR(("%s failed code %d\n", __FUNCTION__, ret));
+
+	return ret;
+}
+
+int
+dhd_pno_enable(dhd_pub_t *dhd, int pfn_enabled)
+{
+	char iovbuf[128];
+	int ret = -1;
+
+	if ((!dhd) && ((pfn_enabled != 0) || (pfn_enabled != 1))) {
+		DHD_ERROR(("%s error exit\n", __FUNCTION__));
+		return ret;
+	}
+
+
+	memset(iovbuf, 0, sizeof(iovbuf));
+
+#ifndef WL_SCHED_SCAN
+	if (dhd_check_ap_wfd_mode_set(dhd) == TRUE)
+		return (ret);
+
+	if ((pfn_enabled) && (dhd_is_associated(dhd, NULL, NULL) == TRUE)) {
+		DHD_ERROR(("%s pno is NOT enable : called in assoc mode , ignore\n", __FUNCTION__));
+		return ret;
+	}
+#endif /* !WL_SCHED_SCAN */
+
+	/* Enable/disable PNO */
+	if ((ret = bcm_mkiovar("pfn", (char *)&pfn_enabled, 4, iovbuf, sizeof(iovbuf))) > 0) {
+		if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR,
+			iovbuf, sizeof(iovbuf), TRUE, 0)) < 0) {
+			DHD_ERROR(("%s failed for error=%d\n", __FUNCTION__, ret));
+			return ret;
+		}
+		else {
+			dhd->pno_enable = pfn_enabled;
+			DHD_TRACE(("%s set pno as %s\n",
+				__FUNCTION__, dhd->pno_enable ? "Enable" : "Disable"));
+		}
+	}
+	else DHD_ERROR(("%s failed err=%d\n", __FUNCTION__, ret));
+
+	return ret;
+}
+
+/* Function to execute combined scan */
+int
+dhd_pno_set(dhd_pub_t *dhd, wlc_ssid_t* ssids_local, int nssid, ushort scan_fr,
+	int pno_repeat, int pno_freq_expo_max)
+{
+	int err = -1;
+	char iovbuf[128];
+	int k, i;
+	wl_pfn_param_t pfn_param;
+	wl_pfn_t	pfn_element;
+	uint len = 0;
+
+	DHD_TRACE(("%s nssid=%d nchan=%d\n", __FUNCTION__, nssid, scan_fr));
+
+	if ((!dhd) && (!ssids_local)) {
+		DHD_ERROR(("%s error exit\n", __FUNCTION__));
+		err = -1;
+		return err;
+	}
+#ifndef WL_SCHED_SCAN
+	if (dhd_check_ap_wfd_mode_set(dhd) == TRUE)
+		return (err);
+#endif /* !WL_SCHED_SCAN */
+
+	/* Check for broadcast ssid */
+	for (k = 0; k < nssid; k++) {
+		if (!ssids_local[k].SSID_len) {
+			DHD_ERROR(("%d: Broadcast SSID is ilegal for PNO setting\n", k));
+			return err;
+		}
+	}
+/* #define  PNO_DUMP 1 */
+#ifdef PNO_DUMP
+	{
+		int j;
+		for (j = 0; j < nssid; j++) {
+			DHD_ERROR(("%d: scan  for  %s size =%d\n", j,
+				ssids_local[j].SSID, ssids_local[j].SSID_len));
+		}
+	}
+#endif /* PNO_DUMP */
+
+	/* clean up everything */
+	if  ((err = dhd_pno_clean(dhd)) < 0) {
+		DHD_ERROR(("%s failed error=%d\n", __FUNCTION__, err));
+		return err;
+	}
+	memset(iovbuf, 0, sizeof(iovbuf));
+	memset(&pfn_param, 0, sizeof(pfn_param));
+	memset(&pfn_element, 0, sizeof(pfn_element));
+
+	/* set pfn parameters */
+	pfn_param.version = htod32(PFN_VERSION);
+	pfn_param.flags = htod16((PFN_LIST_ORDER << SORT_CRITERIA_BIT));
+
+	/* check and set extra pno params */
+	if ((pno_repeat != 0) || (pno_freq_expo_max != 0)) {
+		pfn_param.flags |= htod16(ENABLE << ENABLE_ADAPTSCAN_BIT);
+		pfn_param.repeat = (uchar) (pno_repeat);
+		pfn_param.exp = (uchar) (pno_freq_expo_max);
+	}
+	/* set up pno scan fr */
+	if (scan_fr  != 0)
+		pfn_param.scan_freq = htod32(scan_fr);
+
+	if (pfn_param.scan_freq > PNO_SCAN_MAX_FW_SEC) {
+		DHD_ERROR(("%s pno freq above %d sec\n", __FUNCTION__, PNO_SCAN_MAX_FW_SEC));
+		return err;
+	}
+	if (pfn_param.scan_freq < PNO_SCAN_MIN_FW_SEC) {
+		DHD_ERROR(("%s pno freq less %d sec\n", __FUNCTION__, PNO_SCAN_MIN_FW_SEC));
+		return err;
+	}
+
+	len = bcm_mkiovar("pfn_set", (char *)&pfn_param, sizeof(pfn_param), iovbuf, sizeof(iovbuf));
+	if ((err = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, len, TRUE, 0)) < 0) {
+				DHD_ERROR(("%s pfn_set failed for error=%d\n",
+					__FUNCTION__, err));
+				return err;
+	}
+
+	/* set all pfn ssid */
+	for (i = 0; i < nssid; i++) {
+
+		pfn_element.infra = htod32(DOT11_BSSTYPE_INFRASTRUCTURE);
+		pfn_element.auth = (DOT11_OPEN_SYSTEM);
+		pfn_element.wpa_auth = htod32(WPA_AUTH_PFN_ANY);
+		pfn_element.wsec = htod32(0);
+		pfn_element.infra = htod32(1);
+		pfn_element.flags = htod32(ENABLE << WL_PFN_HIDDEN_BIT);
+		memcpy((char *)pfn_element.ssid.SSID, ssids_local[i].SSID, ssids_local[i].SSID_len);
+		pfn_element.ssid.SSID_len = ssids_local[i].SSID_len;
+
+		if ((len =
+		bcm_mkiovar("pfn_add", (char *)&pfn_element,
+			sizeof(pfn_element), iovbuf, sizeof(iovbuf))) > 0) {
+			if ((err =
+			dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, len, TRUE, 0)) < 0) {
+				DHD_ERROR(("%s failed for i=%d error=%d\n",
+					__FUNCTION__, i, err));
+				return err;
+			}
+			else
+				DHD_TRACE(("%s set OK with PNO time=%d repeat=%d max_adjust=%d\n",
+					__FUNCTION__, pfn_param.scan_freq,
+					pfn_param.repeat, pfn_param.exp));
+		}
+		else DHD_ERROR(("%s failed err=%d\n", __FUNCTION__, err));
+	}
+
+	/* Enable PNO */
+	/* dhd_pno_enable(dhd, 1); */
+	return err;
+}
+
+int
+dhd_pno_set_ex(dhd_pub_t *dhd, wl_pfn_t* ssidnet, int nssid, ushort pno_interval,
+	int pno_repeat, int pno_expo_max, int pno_lost_time)
+{
+	int err = -1;
+	char iovbuf[128];
+	int k, i;
+	wl_pfn_param_t pfn_param;
+	wl_pfn_t	pfn_element;
+	uint len = 0;
+
+	DHD_TRACE(("%s nssid=%d pno_interval=%d\n", __FUNCTION__, nssid, pno_interval));
+
+	if ((!dhd) && (!ssidnet)) {
+		DHD_ERROR(("%s error exit\n", __FUNCTION__));
+		err = -1;
+		return err;
+	}
+
+	if (dhd_check_ap_wfd_mode_set(dhd) == TRUE)
+		return (err);
+
+	/* Check for broadcast ssid */
+	for (k = 0; k < nssid; k++) {
+		if (!ssidnet[k].ssid.SSID_len) {
+			DHD_ERROR(("%d: Broadcast SSID is ilegal for PNO setting\n", k));
+			return err;
+		}
+	}
+/* #define  PNO_DUMP 1 */
+#ifdef PNO_DUMP
+	{
+		int j;
+		for (j = 0; j < nssid; j++) {
+			DHD_ERROR(("%d: scan  for  %s size =%d\n", j,
+				ssidnet[j].ssid.SSID, ssidnet[j].ssid.SSID_len));
+		}
+	}
+#endif /* PNO_DUMP */
+
+	/* clean up everything */
+	if  ((err = dhd_pno_clean(dhd)) < 0) {
+		DHD_ERROR(("%s failed error=%d\n", __FUNCTION__, err));
+		return err;
+	}
+	memset(iovbuf, 0, sizeof(iovbuf));
+	memset(&pfn_param, 0, sizeof(pfn_param));
+	memset(&pfn_element, 0, sizeof(pfn_element));
+
+	/* set pfn parameters */
+	pfn_param.version = htod32(PFN_VERSION);
+	pfn_param.flags = htod16((PFN_LIST_ORDER << SORT_CRITERIA_BIT));
+
+	/* check and set extra pno params */
+	if ((pno_repeat != 0) || (pno_expo_max != 0)) {
+		pfn_param.flags |= htod16(ENABLE << ENABLE_ADAPTSCAN_BIT);
+		pfn_param.repeat = (uchar) (pno_repeat);
+		pfn_param.exp = (uchar) (pno_expo_max);
+	}
+
+	/* set up pno scan fr */
+	if (pno_interval  != 0)
+		pfn_param.scan_freq = htod32(pno_interval);
+
+	if (pfn_param.scan_freq > PNO_SCAN_MAX_FW_SEC) {
+		DHD_ERROR(("%s pno freq above %d sec\n", __FUNCTION__, PNO_SCAN_MAX_FW_SEC));
+		return err;
+	}
+	if (pfn_param.scan_freq < PNO_SCAN_MIN_FW_SEC) {
+		DHD_ERROR(("%s pno freq less %d sec\n", __FUNCTION__, PNO_SCAN_MIN_FW_SEC));
+		return err;
+	}
+
+	/* network lost time */
+	pfn_param.lost_network_timeout = htod32(pno_lost_time);
+
+	len = bcm_mkiovar("pfn_set", (char *)&pfn_param, sizeof(pfn_param), iovbuf, sizeof(iovbuf));
+	if ((err = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, len, TRUE, 0)) < 0) {
+				DHD_ERROR(("%s pfn_set failed for error=%d\n",
+					__FUNCTION__, err));
+				return err;
+	} else {
+		DHD_TRACE(("%s pfn_set OK with PNO time=%d repeat=%d max_adjust=%d\n",
+			__FUNCTION__, pfn_param.scan_freq,
+			pfn_param.repeat, pfn_param.exp));
+	}
+
+	/* set all pfn ssid */
+	for (i = 0; i < nssid; i++) {
+		pfn_element.flags = htod32(ssidnet[i].flags);
+		pfn_element.infra = htod32(ssidnet[i].infra);
+		pfn_element.auth = htod32(ssidnet[i].auth);
+		pfn_element.wpa_auth = htod32(ssidnet[i].wpa_auth);
+		pfn_element.wsec = htod32(ssidnet[i].wsec);
+
+		memcpy((char *)pfn_element.ssid.SSID, ssidnet[i].ssid.SSID, ssidnet[i].ssid.SSID_len);
+		pfn_element.ssid.SSID_len = htod32(ssidnet[i].ssid.SSID_len);
+
+		if ((len =
+			bcm_mkiovar("pfn_add", (char *)&pfn_element,
+			sizeof(pfn_element), iovbuf, sizeof(iovbuf))) > 0) {
+			if ((err =
+				dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, len, TRUE, 0)) < 0) {
+				DHD_ERROR(("%s pfn_add failed with ssidnet[%d] error=%d\n",
+					__FUNCTION__, i, err));
+				return err;
+			} else {
+				DHD_TRACE(("%s pfn_add OK with ssidnet[%d]\n", __FUNCTION__, i));
+			}
+		} else {
+			DHD_ERROR(("%s bcm_mkiovar failed with ssidnet[%d]\n", __FUNCTION__, i));
+		}
+	}
+
+	return err;
+}
+
+int
+dhd_pno_get_status(dhd_pub_t *dhd)
+{
+	int ret = -1;
+
+	if (!dhd)
+		return ret;
+	else
+		return (dhd->pno_enable);
+}
+
+#endif /* PNO_SUPPORT */
+
+#if defined(KEEP_ALIVE)
+int dhd_keep_alive_onoff(dhd_pub_t *dhd)
+{
+	char 				buf[256];
+	const char 			*str;
+	wl_mkeep_alive_pkt_t	mkeep_alive_pkt;
+	wl_mkeep_alive_pkt_t	*mkeep_alive_pktp;
+	int					buf_len;
+	int					str_len;
+	int res 				= -1;
+
+	if (dhd_check_ap_wfd_mode_set(dhd) == TRUE)
+		return (res);
+
+	DHD_TRACE(("%s execution\n", __FUNCTION__));
+
+	str = "mkeep_alive";
+	str_len = strlen(str);
+	strncpy(buf, str, str_len);
+	buf[ str_len ] = '\0';
+	mkeep_alive_pktp = (wl_mkeep_alive_pkt_t *) (buf + str_len + 1);
+	mkeep_alive_pkt.period_msec = KEEP_ALIVE_PERIOD;
+	buf_len = str_len + 1;
+	mkeep_alive_pkt.version = htod16(WL_MKEEP_ALIVE_VERSION);
+	mkeep_alive_pkt.length = htod16(WL_MKEEP_ALIVE_FIXED_LEN);
+	/* Setup keep alive zero for null packet generation */
+	mkeep_alive_pkt.keep_alive_id = 0;
+	mkeep_alive_pkt.len_bytes = 0;
+	buf_len += WL_MKEEP_ALIVE_FIXED_LEN;
+	/* Keep-alive attributes are set in local variable (mkeep_alive_pkt), and
+	 * then memcpy'ed into buffer (mkeep_alive_pktp) since there is no
+	 * guarantee that the buffer is properly aligned.
+	 */
+	memcpy((char *)mkeep_alive_pktp, &mkeep_alive_pkt, WL_MKEEP_ALIVE_FIXED_LEN);
+
+	res = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, buf, buf_len, TRUE, 0);
+
+	return res;
+}
+#endif /* defined(KEEP_ALIVE) */
+
+/* Android ComboSCAN support */
+
+/*
+ *  data parsing from ComboScan tlv list
+*/
+int
+wl_iw_parse_data_tlv(char** list_str, void *dst, int dst_size, const char token,
+                     int input_size, int *bytes_left)
+{
+	char* str = *list_str;
+	uint16 short_temp;
+	uint32 int_temp;
+
+	if ((list_str == NULL) || (*list_str == NULL) ||(bytes_left == NULL) || (*bytes_left < 0)) {
+		DHD_ERROR(("%s error paramters\n", __FUNCTION__));
+		return -1;
+	}
+
+	/* Clean all dest bytes */
+	memset(dst, 0, dst_size);
+	while (*bytes_left > 0) {
+
+		if (str[0] != token) {
+			DHD_TRACE(("%s NOT Type=%d get=%d left_parse=%d \n",
+				__FUNCTION__, token, str[0], *bytes_left));
+			return -1;
+		}
+
+		*bytes_left -= 1;
+		str += 1;
+
+		if (input_size == 1) {
+			memcpy(dst, str, input_size);
+		}
+		else if (input_size == 2) {
+			memcpy(dst, (char *)htod16(memcpy(&short_temp, str, input_size)),
+				input_size);
+		}
+		else if (input_size == 4) {
+			memcpy(dst, (char *)htod32(memcpy(&int_temp, str, input_size)),
+				input_size);
+		}
+
+		*bytes_left -= input_size;
+		str += input_size;
+		*list_str = str;
+		return 1;
+	}
+	return 1;
+}
+
+/*
+ *  channel list parsing from cscan tlv list
+*/
+int
+wl_iw_parse_channel_list_tlv(char** list_str, uint16* channel_list,
+                             int channel_num, int *bytes_left)
+{
+	char* str = *list_str;
+	int idx = 0;
+
+	if ((list_str == NULL) || (*list_str == NULL) ||(bytes_left == NULL) || (*bytes_left < 0)) {
+		DHD_ERROR(("%s error paramters\n", __FUNCTION__));
+		return -1;
+	}
+
+	while (*bytes_left > 0) {
+
+		if (str[0] != CSCAN_TLV_TYPE_CHANNEL_IE) {
+			*list_str = str;
+			DHD_TRACE(("End channel=%d left_parse=%d %d\n", idx, *bytes_left, str[0]));
+			return idx;
+		}
+		/* Get proper CSCAN_TLV_TYPE_CHANNEL_IE */
+		*bytes_left -= 1;
+		str += 1;
+
+		if (str[0] == 0) {
+			/* All channels */
+			channel_list[idx] = 0x0;
+		}
+		else {
+			channel_list[idx] = (uint16)str[0];
+			DHD_TRACE(("%s channel=%d \n", __FUNCTION__,  channel_list[idx]));
+		}
+		*bytes_left -= 1;
+		str += 1;
+
+		if (idx++ > 255) {
+			DHD_ERROR(("%s Too many channels \n", __FUNCTION__));
+			return -1;
+		}
+	}
+
+	*list_str = str;
+	return idx;
+}
+
+/*
+ *  SSIDs list parsing from cscan tlv list
+ */
+int
+wl_iw_parse_ssid_list_tlv(char** list_str, wlc_ssid_t* ssid, int max, int *bytes_left)
+{
+	char* str;
+	int idx = 0;
+
+	if ((list_str == NULL) || (*list_str == NULL) || (*bytes_left < 0)) {
+		DHD_ERROR(("%s error paramters\n", __FUNCTION__));
+		return -1;
+	}
+	str = *list_str;
+	while (*bytes_left > 0) {
+
+		if (str[0] != CSCAN_TLV_TYPE_SSID_IE) {
+			*list_str = str;
+			DHD_TRACE(("nssid=%d left_parse=%d %d\n", idx, *bytes_left, str[0]));
+			return idx;
+		}
+
+		/* Get proper CSCAN_TLV_TYPE_SSID_IE */
+		*bytes_left -= 1;
+		str += 1;
+
+		if (str[0] == 0) {
+			/* Broadcast SSID */
+			ssid[idx].SSID_len = 0;
+			memset((char*)ssid[idx].SSID, 0x0, DOT11_MAX_SSID_LEN);
+			*bytes_left -= 1;
+			str += 1;
+
+			DHD_TRACE(("BROADCAST SCAN  left=%d\n", *bytes_left));
+		}
+		else if (str[0] <= DOT11_MAX_SSID_LEN) {
+			/* Get proper SSID size */
+			ssid[idx].SSID_len = str[0];
+			*bytes_left -= 1;
+			str += 1;
+
+			/* Get SSID */
+			if (ssid[idx].SSID_len > *bytes_left) {
+				DHD_ERROR(("%s out of memory range len=%d but left=%d\n",
+				__FUNCTION__, ssid[idx].SSID_len, *bytes_left));
+				return -1;
+			}
+
+			memcpy((char*)ssid[idx].SSID, str, ssid[idx].SSID_len);
+
+			*bytes_left -= ssid[idx].SSID_len;
+			str += ssid[idx].SSID_len;
+
+			DHD_TRACE(("%s :size=%d left=%d\n",
+				(char*)ssid[idx].SSID, ssid[idx].SSID_len, *bytes_left));
+		}
+		else {
+			DHD_ERROR(("### SSID size more that %d\n", str[0]));
+			return -1;
+		}
+
+		if (idx++ >  max) {
+			DHD_ERROR(("%s number of SSIDs more that %d\n", __FUNCTION__, idx));
+			return -1;
+		}
+	}
+
+	*list_str = str;
+	return idx;
+}
+
+/* Parse a comma-separated list from list_str into ssid array, starting
+ * at index idx.  Max specifies size of the ssid array.  Parses ssids
+ * and returns updated idx; if idx >= max not all fit, the excess have
+ * not been copied.  Returns -1 on empty string, or on ssid too long.
+ */
+int
+wl_iw_parse_ssid_list(char** list_str, wlc_ssid_t* ssid, int idx, int max)
+{
+	char* str, *ptr;
+
+	if ((list_str == NULL) || (*list_str == NULL))
+		return -1;
+
+	for (str = *list_str; str != NULL; str = ptr) {
+
+		/* check for next TAG */
+		if (!strncmp(str, GET_CHANNEL, strlen(GET_CHANNEL))) {
+			*list_str	 = str + strlen(GET_CHANNEL);
+			return idx;
+		}
+
+		if ((ptr = strchr(str, ',')) != NULL) {
+			*ptr++ = '\0';
+		}
+
+		if (strlen(str) > DOT11_MAX_SSID_LEN) {
+			DHD_ERROR(("ssid <%s> exceeds %d\n", str, DOT11_MAX_SSID_LEN));
+			return -1;
+		}
+
+		if (strlen(str) == 0)
+			ssid[idx].SSID_len = 0;
+
+		if (idx < max) {
+			bcm_strcpy_s((char*)ssid[idx].SSID, sizeof(ssid[idx].SSID), str);
+			ssid[idx].SSID_len = strlen(str);
+		}
+		idx++;
+	}
+	return idx;
+}
+
+/*
+ * Parse channel list from iwpriv CSCAN
+ */
+int
+wl_iw_parse_channel_list(char** list_str, uint16* channel_list, int channel_num)
+{
+	int num;
+	int val;
+	char* str;
+	char* endptr = NULL;
+
+	if ((list_str == NULL)||(*list_str == NULL))
+		return -1;
+
+	str = *list_str;
+	num = 0;
+	while (strncmp(str, GET_NPROBE, strlen(GET_NPROBE))) {
+		val = (int)strtoul(str, &endptr, 0);
+		if (endptr == str) {
+			printf("could not parse channel number starting at"
+				" substring \"%s\" in list:\n%s\n",
+				str, *list_str);
+			return -1;
+		}
+		str = endptr + strspn(endptr, " ,");
+
+		if (num == channel_num) {
+			DHD_ERROR(("too many channels (more than %d) in channel list:\n%s\n",
+				channel_num, *list_str));
+			return -1;
+		}
+
+		channel_list[num++] = (uint16)val;
+	}
+	*list_str = str;
+	return num;
+}
diff --git a/drivers/net/wireless/bcmdhd/dhd_custom_gpio.c b/drivers/net/wireless/bcmdhd/dhd_custom_gpio.c
new file mode 100644
index 000000000000..9750eeb23bce
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd_custom_gpio.c
@@ -0,0 +1,293 @@
+/*
+* Customer code to add GPIO control during WLAN start/stop
+* Copyright (C) 1999-2011, Broadcom Corporation
+* 
+*         Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2 (the "GPL"),
+* available at http://www.broadcom.com/licenses/GPLv2.php, with the
+* following added to such license:
+* 
+*      As a special exception, the copyright holders of this software give you
+* permission to link this software with independent modules, and to copy and
+* distribute the resulting executable under terms of your choice, provided that
+* you also meet, for each linked independent module, the terms and conditions of
+* the license of that module.  An independent module is a module which is not
+* derived from this software.  The special exception does not apply to any
+* modifications of the software.
+* 
+*      Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a license
+* other than the GPL, without Broadcom's express prior written consent.
+*
+* $Id: dhd_custom_gpio.c,v 1.2.42.1 2010-10-19 00:41:09 Exp $
+*/
+
+#include <typedefs.h>
+#include <linuxver.h>
+#include <osl.h>
+#include <bcmutils.h>
+
+#include <dngl_stats.h>
+#include <dhd.h>
+
+#include <wlioctl.h>
+#include <wl_iw.h>
+
+#define WL_ERROR(x) printf x
+#define WL_TRACE(x)
+
+#ifdef CUSTOMER_HW
+extern  void bcm_wlan_power_off(int);
+extern  void bcm_wlan_power_on(int);
+#endif /* CUSTOMER_HW */
+#if defined(CUSTOMER_HW2)
+#ifdef CONFIG_WIFI_CONTROL_FUNC
+int wifi_set_power(int on, unsigned long msec);
+int wifi_get_irq_number(unsigned long *irq_flags_ptr);
+int wifi_get_mac_addr(unsigned char *buf);
+void *wifi_get_country_code(char *ccode);
+#else
+int wifi_set_power(int on, unsigned long msec) { return -1; }
+int wifi_get_irq_number(unsigned long *irq_flags_ptr) { return -1; }
+int wifi_get_mac_addr(unsigned char *buf) { return -1; }
+void *wifi_get_country_code(char *ccode) { return NULL; }
+#endif /* CONFIG_WIFI_CONTROL_FUNC */
+#endif /* CUSTOMER_HW2 */
+
+#if defined(OOB_INTR_ONLY)
+
+#if defined(BCMLXSDMMC)
+extern int sdioh_mmc_irq(int irq);
+#endif /* (BCMLXSDMMC)  */
+
+#ifdef CUSTOMER_HW3
+#include <mach/gpio.h>
+#endif
+
+/* Customer specific Host GPIO defintion  */
+static int dhd_oob_gpio_num = -1;
+
+module_param(dhd_oob_gpio_num, int, 0644);
+MODULE_PARM_DESC(dhd_oob_gpio_num, "DHD oob gpio number");
+
+/* This function will return:
+ *  1) return :  Host gpio interrupt number per customer platform
+ *  2) irq_flags_ptr : Type of Host interrupt as Level or Edge
+ *
+ *  NOTE :
+ *  Customer should check his platform definitions
+ *  and his Host Interrupt spec
+ *  to figure out the proper setting for his platform.
+ *  Broadcom provides just reference settings as example.
+ *
+ */
+int dhd_customer_oob_irq_map(unsigned long *irq_flags_ptr)
+{
+	int  host_oob_irq = 0;
+
+#ifdef CUSTOMER_HW2
+	host_oob_irq = wifi_get_irq_number(irq_flags_ptr);
+
+#else
+#if defined(CUSTOM_OOB_GPIO_NUM)
+	if (dhd_oob_gpio_num < 0) {
+		dhd_oob_gpio_num = CUSTOM_OOB_GPIO_NUM;
+	}
+#endif /* CUSTOMER_HW2 */
+
+	if (dhd_oob_gpio_num < 0) {
+		WL_ERROR(("%s: ERROR customer specific Host GPIO is NOT defined \n",
+			__FUNCTION__));
+		return (dhd_oob_gpio_num);
+	}
+
+	WL_ERROR(("%s: customer specific Host GPIO number is (%d)\n",
+	         __FUNCTION__, dhd_oob_gpio_num));
+
+#if defined CUSTOMER_HW
+	host_oob_irq = MSM_GPIO_TO_INT(dhd_oob_gpio_num);
+#elif defined CUSTOMER_HW3
+	gpio_request(dhd_oob_gpio_num, "oob irq");
+	host_oob_irq = gpio_to_irq(dhd_oob_gpio_num);
+	gpio_direction_input(dhd_oob_gpio_num);
+#endif /* CUSTOMER_HW */
+#endif /* CUSTOMER_HW2 */
+
+	return (host_oob_irq);
+}
+#endif /* defined(OOB_INTR_ONLY) */
+
+/* Customer function to control hw specific wlan gpios */
+void
+dhd_customer_gpio_wlan_ctrl(int onoff)
+{
+	switch (onoff) {
+		case WLAN_RESET_OFF:
+			WL_TRACE(("%s: call customer specific GPIO to insert WLAN RESET\n",
+				__FUNCTION__));
+#ifdef CUSTOMER_HW
+			bcm_wlan_power_off(2);
+#endif /* CUSTOMER_HW */
+#ifdef CUSTOMER_HW2
+			wifi_set_power(0, 0);
+#endif
+			WL_ERROR(("=========== WLAN placed in RESET ========\n"));
+		break;
+
+		case WLAN_RESET_ON:
+			WL_TRACE(("%s: callc customer specific GPIO to remove WLAN RESET\n",
+				__FUNCTION__));
+#ifdef CUSTOMER_HW
+			bcm_wlan_power_on(2);
+#endif /* CUSTOMER_HW */
+#ifdef CUSTOMER_HW2
+			wifi_set_power(1, 0);
+#endif
+			WL_ERROR(("=========== WLAN going back to live  ========\n"));
+		break;
+
+		case WLAN_POWER_OFF:
+			WL_TRACE(("%s: call customer specific GPIO to turn off WL_REG_ON\n",
+				__FUNCTION__));
+#ifdef CUSTOMER_HW
+			bcm_wlan_power_off(1);
+#endif /* CUSTOMER_HW */
+		break;
+
+		case WLAN_POWER_ON:
+			WL_TRACE(("%s: call customer specific GPIO to turn on WL_REG_ON\n",
+				__FUNCTION__));
+#ifdef CUSTOMER_HW
+			bcm_wlan_power_on(1);
+			/* Lets customer power to get stable */
+			OSL_DELAY(200);
+#endif /* CUSTOMER_HW */
+		break;
+	}
+}
+
+#ifdef GET_CUSTOM_MAC_ENABLE
+/* Function to get custom MAC address */
+int
+dhd_custom_get_mac_address(unsigned char *buf)
+{
+	int ret = 0;
+
+	WL_TRACE(("%s Enter\n", __FUNCTION__));
+	if (!buf)
+		return -EINVAL;
+
+	/* Customer access to MAC address stored outside of DHD driver */
+#if defined(CUSTOMER_HW2) && (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35))
+	ret = wifi_get_mac_addr(buf);
+#endif
+
+#ifdef EXAMPLE_GET_MAC
+	/* EXAMPLE code */
+	{
+		struct ether_addr ea_example = {{0x00, 0x11, 0x22, 0x33, 0x44, 0xFF}};
+		bcopy((char *)&ea_example, buf, sizeof(struct ether_addr));
+	}
+#endif /* EXAMPLE_GET_MAC */
+
+	return ret;
+}
+#endif /* GET_CUSTOM_MAC_ENABLE */
+
+/* Customized Locale table : OPTIONAL feature */
+const struct cntry_locales_custom translate_custom_table[] = {
+/* Table should be filled out based on custom platform regulatory requirement */
+#ifdef EXAMPLE_TABLE
+	{"",   "XY", 4},  /* Universal if Country code is unknown or empty */
+	{"US", "US", 69}, /* input ISO "US" to : US regrev 69 */
+	{"CA", "US", 69}, /* input ISO "CA" to : US regrev 69 */
+	{"EU", "EU", 5},  /* European union countries to : EU regrev 05 */
+	{"AT", "EU", 5},
+	{"BE", "EU", 5},
+	{"BG", "EU", 5},
+	{"CY", "EU", 5},
+	{"CZ", "EU", 5},
+	{"DK", "EU", 5},
+	{"EE", "EU", 5},
+	{"FI", "EU", 5},
+	{"FR", "EU", 5},
+	{"DE", "EU", 5},
+	{"GR", "EU", 5},
+	{"HU", "EU", 5},
+	{"IE", "EU", 5},
+	{"IT", "EU", 5},
+	{"LV", "EU", 5},
+	{"LI", "EU", 5},
+	{"LT", "EU", 5},
+	{"LU", "EU", 5},
+	{"MT", "EU", 5},
+	{"NL", "EU", 5},
+	{"PL", "EU", 5},
+	{"PT", "EU", 5},
+	{"RO", "EU", 5},
+	{"SK", "EU", 5},
+	{"SI", "EU", 5},
+	{"ES", "EU", 5},
+	{"SE", "EU", 5},
+	{"GB", "EU", 5},
+	{"KR", "XY", 3},
+	{"AU", "XY", 3},
+	{"CN", "XY", 3}, /* input ISO "CN" to : XY regrev 03 */
+	{"TW", "XY", 3},
+	{"AR", "XY", 3},
+	{"MX", "XY", 3},
+	{"IL", "IL", 0},
+	{"CH", "CH", 0},
+	{"TR", "TR", 0},
+	{"NO", "NO", 0},
+#endif /* EXMAPLE_TABLE */
+};
+
+
+/* Customized Locale convertor
+*  input : ISO 3166-1 country abbreviation
+*  output: customized cspec
+*/
+void get_customized_country_code(char *country_iso_code, wl_country_t *cspec)
+{
+#if defined(CUSTOMER_HW2) && (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 39))
+
+	struct cntry_locales_custom *cloc_ptr;
+
+	if (!cspec)
+		return;
+
+	cloc_ptr = wifi_get_country_code(country_iso_code);
+	if (cloc_ptr) {
+		strlcpy(cspec->ccode, cloc_ptr->custom_locale, WLC_CNTRY_BUF_SZ);
+		cspec->rev = cloc_ptr->custom_locale_rev;
+	}
+	return;
+#else
+	int size, i;
+
+	size = ARRAYSIZE(translate_custom_table);
+
+	if (cspec == 0)
+		 return;
+
+	if (size == 0)
+		 return;
+
+	for (i = 0; i < size; i++) {
+		if (strcmp(country_iso_code, translate_custom_table[i].iso_abbrev) == 0) {
+			memcpy(cspec->ccode,
+				translate_custom_table[i].custom_locale, WLC_CNTRY_BUF_SZ);
+			cspec->rev = translate_custom_table[i].custom_locale_rev;
+			return;
+		}
+	}
+#ifdef EXAMPLE_TABLE
+	/* if no country code matched return first universal code from translate_custom_table */
+	memcpy(cspec->ccode, translate_custom_table[0].custom_locale, WLC_CNTRY_BUF_SZ);
+	cspec->rev = translate_custom_table[0].custom_locale_rev;
+#endif /* EXMAPLE_TABLE */
+	return;
+#endif /* defined(CUSTOMER_HW2) && (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 39)) */
+}
diff --git a/drivers/net/wireless/bcmdhd/dhd_dbg.h b/drivers/net/wireless/bcmdhd/dhd_dbg.h
new file mode 100644
index 000000000000..01be6a1f056f
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd_dbg.h
@@ -0,0 +1,105 @@
+/*
+ * Debug/trace/assert driver definitions for Dongle Host Driver.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_dbg.h 285933 2011-09-23 21:45:31Z $
+ */
+
+#ifndef _dhd_dbg_
+#define _dhd_dbg_
+
+#if defined(DHD_DEBUG)
+
+#define DHD_ERROR(args)		do {if ((dhd_msg_level & DHD_ERROR_VAL) && (net_ratelimit())) \
+							printf args;} while (0)
+#define DHD_TRACE(args)		do {if (dhd_msg_level & DHD_TRACE_VAL) printf args;} while (0)
+#define DHD_INFO(args)		do {if (dhd_msg_level & DHD_INFO_VAL) printf args;} while (0)
+#define DHD_DATA(args)		do {if (dhd_msg_level & DHD_DATA_VAL) printf args;} while (0)
+#define DHD_CTL(args)		do {if (dhd_msg_level & DHD_CTL_VAL) printf args;} while (0)
+#define DHD_TIMER(args)		do {if (dhd_msg_level & DHD_TIMER_VAL) printf args;} while (0)
+#define DHD_HDRS(args)		do {if (dhd_msg_level & DHD_HDRS_VAL) printf args;} while (0)
+#define DHD_BYTES(args)		do {if (dhd_msg_level & DHD_BYTES_VAL) printf args;} while (0)
+#define DHD_INTR(args)		do {if (dhd_msg_level & DHD_INTR_VAL) printf args;} while (0)
+#define DHD_GLOM(args)		do {if (dhd_msg_level & DHD_GLOM_VAL) printf args;} while (0)
+#define DHD_EVENT(args)		do {if (dhd_msg_level & DHD_EVENT_VAL) printf args;} while (0)
+#define DHD_BTA(args)		do {if (dhd_msg_level & DHD_BTA_VAL) printf args;} while (0)
+#define DHD_ISCAN(args)		do {if (dhd_msg_level & DHD_ISCAN_VAL) printf args;} while (0)
+#define DHD_ARPOE(args)		do {if (dhd_msg_level & DHD_ARPOE_VAL) printf args;} while (0)
+
+#define DHD_ERROR_ON()		(dhd_msg_level & DHD_ERROR_VAL)
+#define DHD_TRACE_ON()		(dhd_msg_level & DHD_TRACE_VAL)
+#define DHD_INFO_ON()		(dhd_msg_level & DHD_INFO_VAL)
+#define DHD_DATA_ON()		(dhd_msg_level & DHD_DATA_VAL)
+#define DHD_CTL_ON()		(dhd_msg_level & DHD_CTL_VAL)
+#define DHD_TIMER_ON()		(dhd_msg_level & DHD_TIMER_VAL)
+#define DHD_HDRS_ON()		(dhd_msg_level & DHD_HDRS_VAL)
+#define DHD_BYTES_ON()		(dhd_msg_level & DHD_BYTES_VAL)
+#define DHD_INTR_ON()		(dhd_msg_level & DHD_INTR_VAL)
+#define DHD_GLOM_ON()		(dhd_msg_level & DHD_GLOM_VAL)
+#define DHD_EVENT_ON()		(dhd_msg_level & DHD_EVENT_VAL)
+#define DHD_BTA_ON()		(dhd_msg_level & DHD_BTA_VAL)
+#define DHD_ISCAN_ON()		(dhd_msg_level & DHD_ISCAN_VAL)
+#define DHD_ARPOE_ON()		(dhd_msg_level & DHD_ARPOE_VAL)
+
+#else /* defined(BCMDBG) || defined(DHD_DEBUG) */
+
+#define DHD_ERROR(args)    	do {if (net_ratelimit()) printf args;} while (0)
+#define DHD_TRACE(args)
+#define DHD_INFO(args)
+#define DHD_DATA(args)
+#define DHD_CTL(args)
+#define DHD_TIMER(args)
+#define DHD_HDRS(args)
+#define DHD_BYTES(args)
+#define DHD_INTR(args)
+#define DHD_GLOM(args)
+#define DHD_EVENT(args)
+#define DHD_BTA(args)
+#define DHD_ISCAN(args)
+#define DHD_ARPOE(args)
+
+#define DHD_ERROR_ON()		0
+#define DHD_TRACE_ON()		0
+#define DHD_INFO_ON()		0
+#define DHD_DATA_ON()		0
+#define DHD_CTL_ON()		0
+#define DHD_TIMER_ON()		0
+#define DHD_HDRS_ON()		0
+#define DHD_BYTES_ON()		0
+#define DHD_INTR_ON()		0
+#define DHD_GLOM_ON()		0
+#define DHD_EVENT_ON()		0
+#define DHD_BTA_ON()		0
+#define DHD_ISCAN_ON()		0
+#define DHD_ARPOE_ON()		0
+#endif 
+
+#define DHD_LOG(args)
+
+#define DHD_BLOG(cp, size)
+#define DHD_NONE(args)
+extern int dhd_msg_level;
+
+/* Defines msg bits */
+#include <dhdioctl.h>
+
+#endif /* _dhd_dbg_ */
diff --git a/drivers/net/wireless/bcmdhd/dhd_linux.c b/drivers/net/wireless/bcmdhd/dhd_linux.c
new file mode 100644
index 000000000000..a6b6dc8be91f
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd_linux.c
@@ -0,0 +1,5366 @@
+/*
+ * Broadcom Dongle Host Driver (DHD), Linux-specific network interface
+ * Basically selected code segments from usb-cdc.c and usb-rndis.c
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_linux.c 333885 2012-05-18 00:39:03Z $
+ */
+
+#include <typedefs.h>
+#include <linuxver.h>
+#include <osl.h>
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/inetdevice.h>
+#include <linux/rtnetlink.h>
+#include <linux/etherdevice.h>
+#include <linux/random.h>
+#include <linux/spinlock.h>
+#include <linux/ethtool.h>
+#include <linux/fcntl.h>
+#include <linux/fs.h>
+
+#include <asm/uaccess.h>
+#include <asm/unaligned.h>
+
+#include <epivers.h>
+#include <bcmutils.h>
+#include <bcmendian.h>
+#include <bcmdevs.h>
+
+#include <proto/ethernet.h>
+#include <dngl_stats.h>
+#include <dhd.h>
+#include <dhd_bus.h>
+#include <dhd_proto.h>
+#include <dhd_dbg.h>
+#ifdef CONFIG_HAS_WAKELOCK
+#include <linux/wakelock.h>
+#endif
+#ifdef WL_CFG80211
+#include <wl_cfg80211.h>
+#endif
+
+#include <proto/802.11_bta.h>
+#include <proto/bt_amp_hci.h>
+#include <dhd_bta.h>
+
+#ifdef WLMEDIA_HTSF
+#include <linux/time.h>
+#include <htsf.h>
+
+#define HTSF_MINLEN 200    /* min. packet length to timestamp */
+#define HTSF_BUS_DELAY 150 /* assume a fix propagation in us  */
+#define TSMAX  1000        /* max no. of timing record kept   */
+#define NUMBIN 34
+
+static uint32 tsidx = 0;
+static uint32 htsf_seqnum = 0;
+uint32 tsfsync;
+struct timeval tsync;
+static uint32 tsport = 5010;
+
+typedef struct histo_ {
+	uint32 bin[NUMBIN];
+} histo_t;
+
+#if !ISPOWEROF2(DHD_SDALIGN)
+#error DHD_SDALIGN is not a power of 2!
+#endif
+
+static histo_t vi_d1, vi_d2, vi_d3, vi_d4;
+#endif /* WLMEDIA_HTSF */
+
+#if defined(SOFTAP)
+extern bool ap_cfg_running;
+extern bool ap_fw_loaded;
+#endif
+
+/* enable HOSTIP cache update from the host side when an eth0:N is up */
+#define AOE_IP_ALIAS_SUPPORT 1
+
+#ifdef PROP_TXSTATUS
+#include <wlfc_proto.h>
+#include <dhd_wlfc.h>
+#endif
+
+#include <wl_android.h>
+
+#ifdef ARP_OFFLOAD_SUPPORT
+void aoe_update_host_ipv4_table(dhd_pub_t *dhd_pub, u32 ipa, bool add);
+static int dhd_device_event(struct notifier_block *this,
+	unsigned long event,
+	void *ptr);
+
+static struct notifier_block dhd_notifier = {
+	.notifier_call = dhd_device_event
+};
+#endif /* ARP_OFFLOAD_SUPPORT */
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP)
+#include <linux/suspend.h>
+volatile bool dhd_mmc_suspend = FALSE;
+DECLARE_WAIT_QUEUE_HEAD(dhd_dpc_wait);
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) */
+
+#if defined(OOB_INTR_ONLY)
+extern void dhd_enable_oob_intr(struct dhd_bus *bus, bool enable);
+#endif /* defined(OOB_INTR_ONLY) */
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+static void dhd_hang_process(struct work_struct *work);
+#endif 
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+MODULE_LICENSE("GPL v2");
+#endif /* LinuxVer */
+
+#include <dhd_bus.h>
+
+#ifndef PROP_TXSTATUS
+#define DBUS_RX_BUFFER_SIZE_DHD(net)	(net->mtu + net->hard_header_len + dhd->pub.hdrlen)
+#else
+#define DBUS_RX_BUFFER_SIZE_DHD(net)	(net->mtu + net->hard_header_len + dhd->pub.hdrlen + 128)
+#endif
+
+#if LINUX_VERSION_CODE == KERNEL_VERSION(2, 6, 15)
+const char *
+print_tainted()
+{
+	return "";
+}
+#endif	/* LINUX_VERSION_CODE == KERNEL_VERSION(2, 6, 15) */
+
+/* Linux wireless extension support */
+#if defined(CONFIG_BCMDHD_WEXT)
+#include <wl_iw.h>
+extern wl_iw_extra_params_t  g_wl_iw_params;
+#endif /* defined(CONFIG_BCMDHD_WEXT) */
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) && defined(DHD_USE_EARLYSUSPEND)
+#include <linux/earlysuspend.h>
+#endif /* defined(CONFIG_HAS_EARLYSUSPEND) */
+extern int dhd_get_dtim_skip(dhd_pub_t *dhd);
+
+#ifdef PKT_FILTER_SUPPORT
+extern void dhd_pktfilter_offload_set(dhd_pub_t * dhd, char *arg);
+extern void dhd_pktfilter_offload_enable(dhd_pub_t * dhd, char *arg, int enable, int master_mode);
+#endif
+
+/* Interface control information */
+typedef struct dhd_if {
+	struct dhd_info *info;			/* back pointer to dhd_info */
+	/* OS/stack specifics */
+	struct net_device *net;
+	struct net_device_stats stats;
+	int 			idx;			/* iface idx in dongle */
+	dhd_if_state_t	state;			/* interface state */
+	uint 			subunit;		/* subunit */
+	uint8			mac_addr[ETHER_ADDR_LEN];	/* assigned MAC address */
+	bool			attached;		/* Delayed attachment when unset */
+	bool			txflowcontrol;	/* Per interface flow control indicator */
+	char			name[IFNAMSIZ+1]; /* linux interface name */
+	uint8			bssidx;			/* bsscfg index for the interface */
+	bool			set_multicast;
+} dhd_if_t;
+
+#ifdef WLMEDIA_HTSF
+typedef struct {
+	uint32 low;
+	uint32 high;
+} tsf_t;
+
+typedef struct {
+	uint32 last_cycle;
+	uint32 last_sec;
+	uint32 last_tsf;
+	uint32 coef;     /* scaling factor */
+	uint32 coefdec1; /* first decimal  */
+	uint32 coefdec2; /* second decimal */
+} htsf_t;
+
+typedef struct {
+	uint32 t1;
+	uint32 t2;
+	uint32 t3;
+	uint32 t4;
+} tstamp_t;
+
+static tstamp_t ts[TSMAX];
+static tstamp_t maxdelayts;
+static uint32 maxdelay = 0, tspktcnt = 0, maxdelaypktno = 0;
+
+#endif  /* WLMEDIA_HTSF */
+
+/* Local private structure (extension of pub) */
+typedef struct dhd_info {
+#if defined(CONFIG_BCMDHD_WEXT)
+	wl_iw_t		iw;		/* wireless extensions state (must be first) */
+#endif /* defined(CONFIG_BCMDHD_WEXT) */
+
+	dhd_pub_t pub;
+
+	/* For supporting multiple interfaces */
+	dhd_if_t *iflist[DHD_MAX_IFS];
+
+	struct semaphore proto_sem;
+#ifdef PROP_TXSTATUS
+	spinlock_t	wlfc_spinlock;
+#endif /* PROP_TXSTATUS */
+#ifdef WLMEDIA_HTSF
+	htsf_t  htsf;
+#endif
+	wait_queue_head_t ioctl_resp_wait;
+	struct timer_list timer;
+	bool wd_timer_valid;
+	struct tasklet_struct tasklet;
+	spinlock_t	sdlock;
+	spinlock_t	txqlock;
+	spinlock_t	dhd_lock;
+#ifdef DHDTHREAD
+	/* Thread based operation */
+	bool threads_only;
+	struct semaphore sdsem;
+
+	tsk_ctl_t	thr_dpc_ctl;
+	tsk_ctl_t	thr_wdt_ctl;
+
+#else
+	bool dhd_tasklet_create;
+#endif /* DHDTHREAD */
+	tsk_ctl_t	thr_sysioc_ctl;
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	struct work_struct work_hang;
+#endif 
+
+	/* Wakelocks */
+#if defined(CONFIG_HAS_WAKELOCK) && (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	struct wake_lock wl_wifi;   /* Wifi wakelock */
+	struct wake_lock wl_rxwake; /* Wifi rx wakelock */
+	struct wake_lock wl_ctrlwake; /* Wifi ctrl wakelock */
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
+	/* net_device interface lock, prevent race conditions among net_dev interface
+	 * calls and wifi_on or wifi_off
+	 */
+	struct mutex dhd_net_if_mutex;
+	struct mutex dhd_suspend_mutex;
+#endif
+	spinlock_t wakelock_spinlock;
+	int wakelock_counter;
+	int wakelock_rx_timeout_enable;
+	int wakelock_ctrl_timeout_enable;
+
+	/* Thread to issue ioctl for multicast */
+	bool set_macaddress;
+	struct ether_addr macvalue;
+	wait_queue_head_t ctrl_wait;
+	atomic_t pend_8021x_cnt;
+	dhd_attach_states_t dhd_state;
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) && defined(DHD_USE_EARLYSUSPEND)
+	struct early_suspend early_suspend;
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+
+#ifdef ARP_OFFLOAD_SUPPORT
+	u32 pend_ipaddr;
+#endif /* ARP_OFFLOAD_SUPPORT */
+} dhd_info_t;
+
+/* Definitions to provide path to the firmware and nvram
+ * example nvram_path[MOD_PARAM_PATHLEN]="/projects/wlan/nvram.txt"
+ */
+char firmware_path[MOD_PARAM_PATHLEN];
+char nvram_path[MOD_PARAM_PATHLEN];
+
+int op_mode = 0;
+module_param(op_mode, int, 0644);
+extern int wl_control_wl_start(struct net_device *dev);
+extern int net_os_send_hang_message(struct net_device *dev);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+struct semaphore dhd_registration_sem;
+#define DHD_REGISTRATION_TIMEOUT  12000  /* msec : allowed time to finished dhd registration */
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
+
+/* Spawn a thread for system ioctls (set mac, set mcast) */
+uint dhd_sysioc = TRUE;
+module_param(dhd_sysioc, uint, 0);
+
+/* Error bits */
+module_param(dhd_msg_level, int, 0);
+
+/* load firmware and/or nvram values from the filesystem */
+module_param_string(firmware_path, firmware_path, MOD_PARAM_PATHLEN, 0660);
+module_param_string(nvram_path, nvram_path, MOD_PARAM_PATHLEN, 0);
+
+/* Watchdog interval */
+uint dhd_watchdog_ms = 10;
+module_param(dhd_watchdog_ms, uint, 0);
+
+#if defined(DHD_DEBUG)
+/* Console poll interval */
+uint dhd_console_ms = 0;
+module_param(dhd_console_ms, uint, 0644);
+#endif /* defined(DHD_DEBUG) */
+
+/* ARP offload agent mode : Enable ARP Host Auto-Reply and ARP Peer Auto-Reply */
+uint dhd_arp_mode = 0xb;
+module_param(dhd_arp_mode, uint, 0);
+
+/* ARP offload enable */
+uint dhd_arp_enable = TRUE;
+module_param(dhd_arp_enable, uint, 0);
+
+/* Global Pkt filter enable control */
+uint dhd_pkt_filter_enable = TRUE;
+module_param(dhd_pkt_filter_enable, uint, 0);
+
+/*  Pkt filter init setup */
+uint dhd_pkt_filter_init = 0;
+module_param(dhd_pkt_filter_init, uint, 0);
+
+/* Pkt filter mode control */
+uint dhd_master_mode = TRUE;
+module_param(dhd_master_mode, uint, 0);
+
+#ifdef DHDTHREAD
+/* Watchdog thread priority, -1 to use kernel timer */
+int dhd_watchdog_prio = 97;
+module_param(dhd_watchdog_prio, int, 0);
+
+/* DPC thread priority, -1 to use tasklet */
+int dhd_dpc_prio = 98;
+module_param(dhd_dpc_prio, int, 0);
+
+/* DPC thread priority, -1 to use tasklet */
+extern int dhd_dongle_memsize;
+module_param(dhd_dongle_memsize, int, 0);
+#endif /* DHDTHREAD */
+/* Control fw roaming */
+uint dhd_roam_disable = 0;
+
+/* Control radio state */
+uint dhd_radio_up = 1;
+
+/* Network inteface name */
+char iface_name[IFNAMSIZ] = {'\0'};
+module_param_string(iface_name, iface_name, IFNAMSIZ, 0);
+
+/* The following are specific to the SDIO dongle */
+
+/* IOCTL response timeout */
+int dhd_ioctl_timeout_msec = IOCTL_RESP_TIMEOUT;
+
+/* Idle timeout for backplane clock */
+int dhd_idletime = DHD_IDLETIME_TICKS;
+module_param(dhd_idletime, int, 0);
+
+/* Use polling */
+uint dhd_poll = FALSE;
+module_param(dhd_poll, uint, 0);
+
+/* Use interrupts */
+uint dhd_intr = TRUE;
+module_param(dhd_intr, uint, 0);
+
+/* SDIO Drive Strength (in milliamps) */
+uint dhd_sdiod_drive_strength = 6;
+module_param(dhd_sdiod_drive_strength, uint, 0);
+
+/* Tx/Rx bounds */
+extern uint dhd_txbound;
+extern uint dhd_rxbound;
+module_param(dhd_txbound, uint, 0);
+module_param(dhd_rxbound, uint, 0);
+
+/* Deferred transmits */
+extern uint dhd_deferred_tx;
+module_param(dhd_deferred_tx, uint, 0);
+
+#ifdef BCMDBGFS
+extern void dhd_dbg_init(dhd_pub_t *dhdp);
+extern void dhd_dbg_remove(void);
+#endif /* BCMDBGFS */
+
+
+
+#ifdef SDTEST
+/* Echo packet generator (pkts/s) */
+uint dhd_pktgen = 0;
+module_param(dhd_pktgen, uint, 0);
+
+/* Echo packet len (0 => sawtooth, max 2040) */
+uint dhd_pktgen_len = 0;
+module_param(dhd_pktgen_len, uint, 0);
+#endif /* SDTEST */
+
+/* Version string to report */
+#ifdef DHD_DEBUG
+#ifndef SRCBASE
+#define SRCBASE        "drivers/net/wireless/bcmdhd"
+#endif
+#define DHD_COMPILED "\nCompiled in " SRCBASE
+#else
+#define DHD_COMPILED
+#endif /* DHD_DEBUG */
+
+static char dhd_version[] = "Dongle Host Driver, version " EPI_VERSION_STR
+#ifdef DHD_DEBUG
+"\nCompiled in " SRCBASE " on " __DATE__ " at " __TIME__
+#endif
+;
+static void dhd_net_if_lock_local(dhd_info_t *dhd);
+static void dhd_net_if_unlock_local(dhd_info_t *dhd);
+static void dhd_suspend_lock(dhd_pub_t *dhdp);
+static void dhd_suspend_unlock(dhd_pub_t *dhdp);
+#if !defined(AP) && defined(WLP2P) && defined(WL_ENABLE_P2P_IF)
+static u32 dhd_concurrent_fw(dhd_pub_t *dhd);
+#endif 
+
+#ifdef WLMEDIA_HTSF
+void htsf_update(dhd_info_t *dhd, void *data);
+tsf_t prev_tsf, cur_tsf;
+
+uint32 dhd_get_htsf(dhd_info_t *dhd, int ifidx);
+static int dhd_ioctl_htsf_get(dhd_info_t *dhd, int ifidx);
+static void dhd_dump_latency(void);
+static void dhd_htsf_addtxts(dhd_pub_t *dhdp, void *pktbuf);
+static void dhd_htsf_addrxts(dhd_pub_t *dhdp, void *pktbuf);
+static void dhd_dump_htsfhisto(histo_t *his, char *s);
+#endif /* WLMEDIA_HTSF */
+
+/* Monitor interface */
+int dhd_monitor_init(void *dhd_pub);
+int dhd_monitor_uninit(void);
+
+
+#if defined(CONFIG_BCMDHD_WEXT)
+struct iw_statistics *dhd_get_wireless_stats(struct net_device *dev);
+#endif /* defined(CONFIG_BCMDHD_WEXT) */
+
+static void dhd_dpc(ulong data);
+/* forward decl */
+extern int dhd_wait_pend8021x(struct net_device *dev);
+
+#ifdef TOE
+#ifndef BDC
+#error TOE requires BDC
+#endif /* !BDC */
+static int dhd_toe_get(dhd_info_t *dhd, int idx, uint32 *toe_ol);
+static int dhd_toe_set(dhd_info_t *dhd, int idx, uint32 toe_ol);
+#endif /* TOE */
+
+static int dhd_wl_host_event(dhd_info_t *dhd, int *ifidx, void *pktdata,
+                             wl_event_msg_t *event_ptr, void **data_ptr);
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP)
+static int dhd_sleep_pm_callback(struct notifier_block *nfb, unsigned long action, void *ignored)
+{
+	int ret = NOTIFY_DONE;
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 39))
+	switch (action) {
+		case PM_HIBERNATION_PREPARE:
+		case PM_SUSPEND_PREPARE:
+			dhd_mmc_suspend = TRUE;
+			ret = NOTIFY_OK;
+		break;
+		case PM_POST_HIBERNATION:
+		case PM_POST_SUSPEND:
+			dhd_mmc_suspend = FALSE;
+			ret = NOTIFY_OK;
+		break;
+	}
+	smp_mb();
+#endif
+	return ret;
+}
+
+static struct notifier_block dhd_sleep_pm_notifier = {
+	.notifier_call = dhd_sleep_pm_callback,
+	.priority = 10
+};
+extern int register_pm_notifier(struct notifier_block *nb);
+extern int unregister_pm_notifier(struct notifier_block *nb);
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) */
+
+static void dhd_set_packet_filter(int value, dhd_pub_t *dhd)
+{
+#ifdef PKT_FILTER_SUPPORT
+	DHD_TRACE(("%s: %d\n", __FUNCTION__, value));
+	/* 1 - Enable packet filter, only allow unicast packet to send up */
+	/* 0 - Disable packet filter */
+	if (dhd_pkt_filter_enable && (!value ||
+	    (dhd_check_ap_wfd_mode_set(dhd) == FALSE))) {
+		int i;
+
+		for (i = 0; i < dhd->pktfilter_count; i++) {
+			dhd_pktfilter_offload_set(dhd, dhd->pktfilter[i]);
+			dhd_pktfilter_offload_enable(dhd, dhd->pktfilter[i],
+				value, dhd_master_mode);
+		}
+	}
+#endif
+}
+
+static int dhd_set_suspend(int value, dhd_pub_t *dhd)
+{
+	int power_mode = PM_MAX;
+	/* wl_pkt_filter_enable_t	enable_parm; */
+	char iovbuf[32];
+	int bcn_li_dtim = 3;
+	uint roamvar = 1;
+
+	DHD_TRACE(("%s: enter, value = %d in_suspend=%d\n",
+		__FUNCTION__, value, dhd->in_suspend));
+
+	dhd_suspend_lock(dhd);
+	if (dhd && dhd->up) {
+		if (value && dhd->in_suspend) {
+
+			/* Kernel suspended */
+			DHD_ERROR(("%s: force extra Suspend setting \n", __FUNCTION__));
+
+			dhd_wl_ioctl_cmd(dhd, WLC_SET_PM, (char *)&power_mode,
+			                 sizeof(power_mode), TRUE, 0);
+
+			/* Enable packet filter, only allow unicast packet to send up */
+			dhd_set_packet_filter(1, dhd);
+
+			/* If DTIM skip is set up as default, force it to wake
+			 * each third DTIM for better power savings.  Note that
+			 * one side effect is a chance to miss BC/MC packet.
+			 */
+			bcn_li_dtim = dhd_get_dtim_skip(dhd);
+			bcm_mkiovar("bcn_li_dtim", (char *)&bcn_li_dtim,
+				4, iovbuf, sizeof(iovbuf));
+			dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
+
+			/* Disable firmware roaming during suspend */
+			bcm_mkiovar("roam_off", (char *)&roamvar, 4,
+				iovbuf, sizeof(iovbuf));
+			dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
+		} else {
+
+			/* Kernel resumed  */
+			DHD_TRACE(("%s: Remove extra suspend setting \n", __FUNCTION__));
+
+			power_mode = PM_FAST;
+			dhd_wl_ioctl_cmd(dhd, WLC_SET_PM, (char *)&power_mode,
+			                 sizeof(power_mode), TRUE, 0);
+
+			/* disable pkt filter */
+			dhd_set_packet_filter(0, dhd);
+
+			/* restore pre-suspend setting for dtim_skip */
+			bcm_mkiovar("bcn_li_dtim", (char *)&dhd->dtim_skip,
+				4, iovbuf, sizeof(iovbuf));
+
+			dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
+			roamvar = dhd_roam_disable;
+			bcm_mkiovar("roam_off", (char *)&roamvar, 4, iovbuf,
+				sizeof(iovbuf));
+			dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
+		}
+	}
+	dhd_suspend_unlock(dhd);
+	return 0;
+}
+
+static int dhd_suspend_resume_helper(struct dhd_info *dhd, int val, int force)
+{
+	dhd_pub_t *dhdp = &dhd->pub;
+	int ret = 0;
+
+	DHD_OS_WAKE_LOCK(dhdp);
+	/* Set flag when early suspend was called */
+	dhdp->in_suspend = val;
+	if ((force || !dhdp->suspend_disable_flag) &&
+	    (dhd_check_ap_wfd_mode_set(dhdp) == FALSE)) {
+		ret = dhd_set_suspend(val, dhdp);
+	}
+	DHD_OS_WAKE_UNLOCK(dhdp);
+	return ret;
+}
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) && defined(DHD_USE_EARLYSUSPEND)
+static void dhd_early_suspend(struct early_suspend *h)
+{
+	struct dhd_info *dhd = container_of(h, struct dhd_info, early_suspend);
+
+	DHD_TRACE(("%s: enter\n", __FUNCTION__));
+
+	if (dhd)
+		dhd_suspend_resume_helper(dhd, 1, 0);
+}
+
+static void dhd_late_resume(struct early_suspend *h)
+{
+	struct dhd_info *dhd = container_of(h, struct dhd_info, early_suspend);
+
+	DHD_TRACE(("%s: enter\n", __FUNCTION__));
+
+	if (dhd)
+		dhd_suspend_resume_helper(dhd, 0, 0);
+}
+#endif /* defined(CONFIG_HAS_EARLYSUSPEND) */
+
+/*
+ * Generalized timeout mechanism.  Uses spin sleep with exponential back-off until
+ * the sleep time reaches one jiffy, then switches over to task delay.  Usage:
+ *
+ *      dhd_timeout_start(&tmo, usec);
+ *      while (!dhd_timeout_expired(&tmo))
+ *              if (poll_something())
+ *                      break;
+ *      if (dhd_timeout_expired(&tmo))
+ *              fatal();
+ */
+
+void
+dhd_timeout_start(dhd_timeout_t *tmo, uint usec)
+{
+	tmo->limit = usec;
+	tmo->increment = 0;
+	tmo->elapsed = 0;
+	tmo->tick = 1000000 / HZ;
+}
+
+int
+dhd_timeout_expired(dhd_timeout_t *tmo)
+{
+	/* Does nothing the first call */
+	if (tmo->increment == 0) {
+		tmo->increment = 1;
+		return 0;
+	}
+
+	if (tmo->elapsed >= tmo->limit)
+		return 1;
+
+	/* Add the delay that's about to take place */
+	tmo->elapsed += tmo->increment;
+
+	if (tmo->increment < tmo->tick) {
+		OSL_DELAY(tmo->increment);
+		tmo->increment *= 2;
+		if (tmo->increment > tmo->tick)
+			tmo->increment = tmo->tick;
+	} else {
+		wait_queue_head_t delay_wait;
+		DECLARE_WAITQUEUE(wait, current);
+		init_waitqueue_head(&delay_wait);
+		add_wait_queue(&delay_wait, &wait);
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(1);
+		remove_wait_queue(&delay_wait, &wait);
+		set_current_state(TASK_RUNNING);
+	}
+
+	return 0;
+}
+
+int
+dhd_net2idx(dhd_info_t *dhd, struct net_device *net)
+{
+	int i = 0;
+
+	ASSERT(dhd);
+	while (i < DHD_MAX_IFS) {
+		if (dhd->iflist[i] && (dhd->iflist[i]->net == net))
+			return i;
+		i++;
+	}
+
+	return DHD_BAD_IF;
+}
+
+struct net_device * dhd_idx2net(void *pub, int ifidx)
+{
+	struct dhd_pub *dhd_pub = (struct dhd_pub *)pub;
+	struct dhd_info *dhd_info;
+
+	if (!dhd_pub || ifidx < 0 || ifidx >= DHD_MAX_IFS)
+		return NULL;
+	dhd_info = dhd_pub->info;
+	if (dhd_info && dhd_info->iflist[ifidx])
+		return dhd_info->iflist[ifidx]->net;
+	return NULL;
+}
+
+int
+dhd_ifname2idx(dhd_info_t *dhd, char *name)
+{
+	int i = DHD_MAX_IFS;
+
+	ASSERT(dhd);
+
+	if (name == NULL || *name == '\0')
+		return 0;
+
+	while (--i > 0)
+		if (dhd->iflist[i] && !strncmp(dhd->iflist[i]->name, name, IFNAMSIZ))
+				break;
+
+	DHD_TRACE(("%s: return idx %d for \"%s\"\n", __FUNCTION__, i, name));
+
+	return i;	/* default - the primary interface */
+}
+
+char *
+dhd_ifname(dhd_pub_t *dhdp, int ifidx)
+{
+	dhd_info_t *dhd = (dhd_info_t *)dhdp->info;
+
+	ASSERT(dhd);
+
+	if (ifidx < 0 || ifidx >= DHD_MAX_IFS) {
+		DHD_ERROR(("%s: ifidx %d out of range\n", __FUNCTION__, ifidx));
+		return "<if_bad>";
+	}
+
+	if (dhd->iflist[ifidx] == NULL) {
+		DHD_ERROR(("%s: null i/f %d\n", __FUNCTION__, ifidx));
+		return "<if_null>";
+	}
+
+	if (dhd->iflist[ifidx]->net)
+		return dhd->iflist[ifidx]->net->name;
+
+	return "<if_none>";
+}
+
+uint8 *
+dhd_bssidx2bssid(dhd_pub_t *dhdp, int idx)
+{
+	int i;
+	dhd_info_t *dhd = (dhd_info_t *)dhdp;
+
+	ASSERT(dhd);
+	for (i = 0; i < DHD_MAX_IFS; i++)
+	if (dhd->iflist[i] && dhd->iflist[i]->bssidx == idx)
+		return dhd->iflist[i]->mac_addr;
+
+	return NULL;
+}
+
+
+static void
+_dhd_set_multicast_list(dhd_info_t *dhd, int ifidx)
+{
+	struct net_device *dev;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35)
+	struct netdev_hw_addr *ha;
+#else
+	struct dev_mc_list *mclist;
+#endif
+	uint32 allmulti, cnt;
+
+	wl_ioctl_t ioc;
+	char *buf, *bufp;
+	uint buflen;
+	int ret;
+
+	ASSERT(dhd && dhd->iflist[ifidx]);
+	dev = dhd->iflist[ifidx]->net;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)
+	netif_addr_lock_bh(dev);
+#endif
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35)
+	cnt = netdev_mc_count(dev);
+#else
+	cnt = dev->mc_count;
+#endif
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)
+	netif_addr_unlock_bh(dev);
+#endif
+
+	/* Determine initial value of allmulti flag */
+	allmulti = (dev->flags & IFF_ALLMULTI) ? TRUE : FALSE;
+
+	/* Send down the multicast list first. */
+
+
+	buflen = sizeof("mcast_list") + sizeof(cnt) + (cnt * ETHER_ADDR_LEN);
+	if (!(bufp = buf = MALLOC(dhd->pub.osh, buflen))) {
+		DHD_ERROR(("%s: out of memory for mcast_list, cnt %d\n",
+		           dhd_ifname(&dhd->pub, ifidx), cnt));
+		return;
+	}
+
+	strcpy(bufp, "mcast_list");
+	bufp += strlen("mcast_list") + 1;
+
+	cnt = htol32(cnt);
+	memcpy(bufp, &cnt, sizeof(cnt));
+	bufp += sizeof(cnt);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)
+	netif_addr_lock_bh(dev);
+#endif
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35)
+	netdev_for_each_mc_addr(ha, dev) {
+		if (!cnt)
+			break;
+		memcpy(bufp, ha->addr, ETHER_ADDR_LEN);
+		bufp += ETHER_ADDR_LEN;
+		cnt--;
+	}
+#else
+	for (mclist = dev->mc_list; (mclist && (cnt > 0)); cnt--, mclist = mclist->next) {
+		memcpy(bufp, (void *)mclist->dmi_addr, ETHER_ADDR_LEN);
+		bufp += ETHER_ADDR_LEN;
+	}
+#endif
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)
+	netif_addr_unlock_bh(dev);
+#endif
+
+	memset(&ioc, 0, sizeof(ioc));
+	ioc.cmd = WLC_SET_VAR;
+	ioc.buf = buf;
+	ioc.len = buflen;
+	ioc.set = TRUE;
+
+	ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len);
+	if (ret < 0) {
+		DHD_ERROR(("%s: set mcast_list failed, cnt %d\n",
+			dhd_ifname(&dhd->pub, ifidx), cnt));
+		allmulti = cnt ? TRUE : allmulti;
+	}
+
+	MFREE(dhd->pub.osh, buf, buflen);
+
+	/* Now send the allmulti setting.  This is based on the setting in the
+	 * net_device flags, but might be modified above to be turned on if we
+	 * were trying to set some addresses and dongle rejected it...
+	 */
+
+	buflen = sizeof("allmulti") + sizeof(allmulti);
+	if (!(buf = MALLOC(dhd->pub.osh, buflen))) {
+		DHD_ERROR(("%s: out of memory for allmulti\n", dhd_ifname(&dhd->pub, ifidx)));
+		return;
+	}
+	allmulti = htol32(allmulti);
+
+	if (!bcm_mkiovar("allmulti", (void*)&allmulti, sizeof(allmulti), buf, buflen)) {
+		DHD_ERROR(("%s: mkiovar failed for allmulti, datalen %d buflen %u\n",
+		           dhd_ifname(&dhd->pub, ifidx), (int)sizeof(allmulti), buflen));
+		MFREE(dhd->pub.osh, buf, buflen);
+		return;
+	}
+
+
+	memset(&ioc, 0, sizeof(ioc));
+	ioc.cmd = WLC_SET_VAR;
+	ioc.buf = buf;
+	ioc.len = buflen;
+	ioc.set = TRUE;
+
+	ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len);
+	if (ret < 0) {
+		DHD_ERROR(("%s: set allmulti %d failed\n",
+		           dhd_ifname(&dhd->pub, ifidx), ltoh32(allmulti)));
+	}
+
+	MFREE(dhd->pub.osh, buf, buflen);
+
+	/* Finally, pick up the PROMISC flag as well, like the NIC driver does */
+
+	allmulti = (dev->flags & IFF_PROMISC) ? TRUE : FALSE;
+	allmulti = htol32(allmulti);
+
+	memset(&ioc, 0, sizeof(ioc));
+	ioc.cmd = WLC_SET_PROMISC;
+	ioc.buf = &allmulti;
+	ioc.len = sizeof(allmulti);
+	ioc.set = TRUE;
+
+	ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len);
+	if (ret < 0) {
+		DHD_ERROR(("%s: set promisc %d failed\n",
+		           dhd_ifname(&dhd->pub, ifidx), ltoh32(allmulti)));
+	}
+}
+
+static int
+_dhd_set_mac_address(dhd_info_t *dhd, int ifidx, struct ether_addr *addr)
+{
+	char buf[32];
+	wl_ioctl_t ioc;
+	int ret;
+
+	if (!bcm_mkiovar("cur_etheraddr", (char*)addr, ETHER_ADDR_LEN, buf, 32)) {
+		DHD_ERROR(("%s: mkiovar failed for cur_etheraddr\n", dhd_ifname(&dhd->pub, ifidx)));
+		return -1;
+	}
+	memset(&ioc, 0, sizeof(ioc));
+	ioc.cmd = WLC_SET_VAR;
+	ioc.buf = buf;
+	ioc.len = 32;
+	ioc.set = TRUE;
+
+	ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len);
+	if (ret < 0) {
+		DHD_ERROR(("%s: set cur_etheraddr failed\n", dhd_ifname(&dhd->pub, ifidx)));
+	} else {
+		memcpy(dhd->iflist[ifidx]->net->dev_addr, addr, ETHER_ADDR_LEN);
+		memcpy(dhd->pub.mac.octet, addr, ETHER_ADDR_LEN);
+	}
+
+	return ret;
+}
+
+#ifdef SOFTAP
+extern struct net_device *ap_net_dev;
+extern tsk_ctl_t ap_eth_ctl; /* ap netdev heper thread ctl */
+#endif
+
+static void
+dhd_op_if(dhd_if_t *ifp)
+{
+	dhd_info_t *dhd;
+	int ret = 0, err = 0;
+#ifdef SOFTAP
+	unsigned long flags;
+#endif
+
+	if (!ifp || !ifp->info || !ifp->idx)
+		return;
+	ASSERT(ifp && ifp->info && ifp->idx);	/* Virtual interfaces only */
+	dhd = ifp->info;
+
+	DHD_TRACE(("%s: idx %d, state %d\n", __FUNCTION__, ifp->idx, ifp->state));
+
+#ifdef WL_CFG80211
+	if (wl_cfg80211_is_progress_ifchange())
+			return;
+
+#endif
+	switch (ifp->state) {
+	case DHD_IF_ADD:
+		/*
+		 * Delete the existing interface before overwriting it
+		 * in case we missed the WLC_E_IF_DEL event.
+		 */
+		if (ifp->net != NULL) {
+			DHD_ERROR(("%s: ERROR: netdev:%s already exists, try free & unregister \n",
+			 __FUNCTION__, ifp->net->name));
+			netif_stop_queue(ifp->net);
+			unregister_netdev(ifp->net);
+			free_netdev(ifp->net);
+		}
+		/* Allocate etherdev, including space for private structure */
+		if (!(ifp->net = alloc_etherdev(sizeof(dhd)))) {
+			DHD_ERROR(("%s: OOM - alloc_etherdev\n", __FUNCTION__));
+			ret = -ENOMEM;
+		}
+		if (ret == 0) {
+			strncpy(ifp->net->name, ifp->name, IFNAMSIZ);
+			ifp->net->name[IFNAMSIZ - 1] = '\0';
+			memcpy(netdev_priv(ifp->net), &dhd, sizeof(dhd));
+#ifdef WL_CFG80211
+			if (dhd->dhd_state & DHD_ATTACH_STATE_CFG80211)
+				if (!wl_cfg80211_notify_ifadd(ifp->net, ifp->idx, ifp->bssidx,
+					(void*)dhd_net_attach)) {
+					ifp->state = DHD_IF_NONE;
+					return;
+				}
+#endif
+			if ((err = dhd_net_attach(&dhd->pub, ifp->idx)) != 0) {
+				DHD_ERROR(("%s: dhd_net_attach failed, err %d\n",
+					__FUNCTION__, err));
+				ret = -EOPNOTSUPP;
+			} else {
+#if defined(SOFTAP)
+		if (ap_fw_loaded && !(dhd->dhd_state & DHD_ATTACH_STATE_CFG80211)) {
+				 /* semaphore that the soft AP CODE waits on */
+				flags = dhd_os_spin_lock(&dhd->pub);
+
+				/* save ptr to wl0.1 netdev for use in wl_iw.c  */
+				ap_net_dev = ifp->net;
+				 /* signal to the SOFTAP 'sleeper' thread, wl0.1 is ready */
+				up(&ap_eth_ctl.sema);
+				dhd_os_spin_unlock(&dhd->pub, flags);
+		}
+#endif
+				DHD_TRACE(("\n ==== pid:%x, net_device for if:%s created ===\n\n",
+					current->pid, ifp->net->name));
+				ifp->state = DHD_IF_NONE;
+			}
+		}
+		break;
+	case DHD_IF_DEL:
+		/* Make sure that we don't enter again here if .. */
+		/* dhd_op_if is called again from some other context */
+		ifp->state = DHD_IF_DELETING;
+		if (ifp->net != NULL) {
+			DHD_TRACE(("\n%s: got 'DHD_IF_DEL' state\n", __FUNCTION__));
+#ifdef WL_CFG80211
+			if (dhd->dhd_state & DHD_ATTACH_STATE_CFG80211) {
+				wl_cfg80211_ifdel_ops(ifp->net);
+			}
+#endif
+			netif_stop_queue(ifp->net);
+			unregister_netdev(ifp->net);
+			ret = DHD_DEL_IF;
+
+#ifdef WL_CFG80211
+			if (dhd->dhd_state & DHD_ATTACH_STATE_CFG80211) {
+				wl_cfg80211_notify_ifdel();
+			}
+#endif
+		}
+		break;
+	case DHD_IF_DELETING:
+		break;
+	default:
+		DHD_ERROR(("%s: bad op %d\n", __FUNCTION__, ifp->state));
+		ASSERT(!ifp->state);
+		break;
+	}
+
+	if (ret < 0) {
+		ifp->set_multicast = FALSE;
+		if (ifp->net) {
+			free_netdev(ifp->net);
+			ifp->net = NULL;
+		}
+		dhd->iflist[ifp->idx] = NULL;
+#ifdef SOFTAP
+		flags = dhd_os_spin_lock(&dhd->pub);
+		if (ifp->net == ap_net_dev)
+			ap_net_dev = NULL;   /*  NULL  SOFTAP global wl0.1 as well */
+		dhd_os_spin_unlock(&dhd->pub, flags);
+#endif /*  SOFTAP */
+		MFREE(dhd->pub.osh, ifp, sizeof(*ifp));
+	}
+}
+
+static int
+_dhd_sysioc_thread(void *data)
+{
+	tsk_ctl_t *tsk = (tsk_ctl_t *)data;
+	dhd_info_t *dhd = (dhd_info_t *)tsk->parent;
+
+
+	int i;
+#ifdef SOFTAP
+	bool in_ap = FALSE;
+	unsigned long flags;
+#endif
+
+	DAEMONIZE("dhd_sysioc");
+
+	complete(&tsk->completed);
+
+	while (down_interruptible(&tsk->sema) == 0) {
+
+		SMP_RD_BARRIER_DEPENDS();
+		if (tsk->terminated) {
+			break;
+		}
+
+		dhd_net_if_lock_local(dhd);
+		DHD_OS_WAKE_LOCK(&dhd->pub);
+
+		for (i = 0; i < DHD_MAX_IFS; i++) {
+			if (dhd->iflist[i]) {
+				DHD_TRACE(("%s: interface %d\n", __FUNCTION__, i));
+#ifdef SOFTAP
+				flags = dhd_os_spin_lock(&dhd->pub);
+				in_ap = (ap_net_dev != NULL);
+				dhd_os_spin_unlock(&dhd->pub, flags);
+#endif /* SOFTAP */
+				if (dhd->iflist[i] && dhd->iflist[i]->state)
+					dhd_op_if(dhd->iflist[i]);
+
+				if (dhd->iflist[i] == NULL) {
+					DHD_TRACE(("\n\n %s: interface %d just been removed,"
+						"!\n\n", __FUNCTION__, i));
+					continue;
+				}
+#ifdef SOFTAP
+				if (in_ap && dhd->set_macaddress)  {
+					DHD_TRACE(("attempt to set MAC for %s in AP Mode,"
+						"blocked. \n", dhd->iflist[i]->net->name));
+					dhd->set_macaddress = FALSE;
+					continue;
+				}
+
+				if (in_ap && dhd->iflist[i]->set_multicast)  {
+					DHD_TRACE(("attempt to set MULTICAST list for %s"
+					 "in AP Mode, blocked. \n", dhd->iflist[i]->net->name));
+					dhd->iflist[i]->set_multicast = FALSE;
+					continue;
+				}
+#endif /* SOFTAP */
+				if (dhd->iflist[i]->set_multicast) {
+					dhd->iflist[i]->set_multicast = FALSE;
+					_dhd_set_multicast_list(dhd, i);
+				}
+				if (dhd->set_macaddress) {
+					dhd->set_macaddress = FALSE;
+					_dhd_set_mac_address(dhd, i, &dhd->macvalue);
+				}
+			}
+		}
+
+		DHD_OS_WAKE_UNLOCK(&dhd->pub);
+		dhd_net_if_unlock_local(dhd);
+	}
+	DHD_TRACE(("%s: stopped\n", __FUNCTION__));
+	complete_and_exit(&tsk->completed, 0);
+}
+
+static int
+dhd_set_mac_address(struct net_device *dev, void *addr)
+{
+	int ret = 0;
+
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	struct sockaddr *sa = (struct sockaddr *)addr;
+	int ifidx;
+
+	ifidx = dhd_net2idx(dhd, dev);
+	if (ifidx == DHD_BAD_IF)
+		return -1;
+
+	ASSERT(dhd->thr_sysioc_ctl.thr_pid >= 0);
+	memcpy(&dhd->macvalue, sa->sa_data, ETHER_ADDR_LEN);
+	dhd->set_macaddress = TRUE;
+	up(&dhd->thr_sysioc_ctl.sema);
+
+	return ret;
+}
+
+static void
+dhd_set_multicast_list(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ifidx;
+
+	ifidx = dhd_net2idx(dhd, dev);
+	if (ifidx == DHD_BAD_IF)
+		return;
+
+	ASSERT(dhd->thr_sysioc_ctl.thr_pid >= 0);
+	dhd->iflist[ifidx]->set_multicast = TRUE;
+	up(&dhd->thr_sysioc_ctl.sema);
+}
+
+#ifdef PROP_TXSTATUS
+int
+dhd_os_wlfc_block(dhd_pub_t *pub)
+{
+	dhd_info_t *di = (dhd_info_t *)(pub->info);
+	ASSERT(di != NULL);
+
+	spin_lock_bh(&di->wlfc_spinlock);
+	return 1;
+}
+
+int
+dhd_os_wlfc_unblock(dhd_pub_t *pub)
+{
+	dhd_info_t *di = (dhd_info_t *)(pub->info);
+	ASSERT(di != NULL);
+	spin_unlock_bh(&di->wlfc_spinlock);
+	return 1;
+}
+
+const uint8 wme_fifo2ac[] = { 0, 1, 2, 3, 1, 1 };
+uint8 prio2fifo[8] = { 1, 0, 0, 1, 2, 2, 3, 3 };
+#define WME_PRIO2AC(prio)	wme_fifo2ac[prio2fifo[(prio)]]
+
+#endif /* PROP_TXSTATUS */
+int
+dhd_sendpkt(dhd_pub_t *dhdp, int ifidx, void *pktbuf)
+{
+	int ret;
+	dhd_info_t *dhd = (dhd_info_t *)(dhdp->info);
+	struct ether_header *eh = NULL;
+
+	/* Reject if down */
+	if (!dhdp->up || (dhdp->busstate == DHD_BUS_DOWN)) {
+		/* free the packet here since the caller won't */
+		PKTFREE(dhdp->osh, pktbuf, TRUE);
+		return -ENODEV;
+	}
+
+	/* Update multicast statistic */
+	if (PKTLEN(dhdp->osh, pktbuf) >= ETHER_HDR_LEN) {
+		uint8 *pktdata = (uint8 *)PKTDATA(dhdp->osh, pktbuf);
+		eh = (struct ether_header *)pktdata;
+
+		if (ETHER_ISMULTI(eh->ether_dhost))
+			dhdp->tx_multicast++;
+		if (ntoh16(eh->ether_type) == ETHER_TYPE_802_1X)
+			atomic_inc(&dhd->pend_8021x_cnt);
+	} else {
+		PKTFREE(dhd->pub.osh, pktbuf, TRUE);
+		return BCME_ERROR;
+	}
+
+	/* Look into the packet and update the packet priority */
+	if (PKTPRIO(pktbuf) == 0)
+		pktsetprio(pktbuf, FALSE);
+
+#ifdef PROP_TXSTATUS
+	if (dhdp->wlfc_state) {
+		/* store the interface ID */
+		DHD_PKTTAG_SETIF(PKTTAG(pktbuf), ifidx);
+
+		/* store destination MAC in the tag as well */
+		DHD_PKTTAG_SETDSTN(PKTTAG(pktbuf), eh->ether_dhost);
+
+		/* decide which FIFO this packet belongs to */
+		if (ETHER_ISMULTI(eh->ether_dhost))
+			/* one additional queue index (highest AC + 1) is used for bc/mc queue */
+			DHD_PKTTAG_SETFIFO(PKTTAG(pktbuf), AC_COUNT);
+		else
+			DHD_PKTTAG_SETFIFO(PKTTAG(pktbuf), WME_PRIO2AC(PKTPRIO(pktbuf)));
+	} else
+#endif /* PROP_TXSTATUS */
+	/* If the protocol uses a data header, apply it */
+	dhd_prot_hdrpush(dhdp, ifidx, pktbuf);
+
+	/* Use bus module to send data frame */
+#ifdef WLMEDIA_HTSF
+	dhd_htsf_addtxts(dhdp, pktbuf);
+#endif
+#ifdef PROP_TXSTATUS
+	if (dhdp->wlfc_state && ((athost_wl_status_info_t*)dhdp->wlfc_state)->proptxstatus_mode
+			!= WLFC_FCMODE_NONE) {
+		dhd_os_wlfc_block(dhdp);
+		ret = dhd_wlfc_enque_sendq(dhdp->wlfc_state, DHD_PKTTAG_FIFO(PKTTAG(pktbuf)),
+			pktbuf);
+		dhd_wlfc_commit_packets(dhdp->wlfc_state,  (f_commitpkt_t)dhd_bus_txdata,
+			dhdp->bus);
+		if (((athost_wl_status_info_t*)dhdp->wlfc_state)->toggle_host_if) {
+			((athost_wl_status_info_t*)dhdp->wlfc_state)->toggle_host_if = 0;
+		}
+		dhd_os_wlfc_unblock(dhdp);
+	}
+	else
+		/* non-proptxstatus way */
+	ret = dhd_bus_txdata(dhdp->bus, pktbuf);
+#else
+	ret = dhd_bus_txdata(dhdp->bus, pktbuf);
+#endif /* PROP_TXSTATUS */
+
+
+	return ret;
+}
+
+int
+dhd_start_xmit(struct sk_buff *skb, struct net_device *net)
+{
+	int ret;
+	void *pktbuf;
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
+	int ifidx;
+#ifdef WLMEDIA_HTSF
+	uint8 htsfdlystat_sz = dhd->pub.htsfdlystat_sz;
+#else
+	uint8 htsfdlystat_sz = 0;
+#endif
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	DHD_OS_WAKE_LOCK(&dhd->pub);
+
+	/* Reject if down */
+	if (!dhd->pub.up || (dhd->pub.busstate == DHD_BUS_DOWN)) {
+		DHD_ERROR(("%s: xmit rejected pub.up=%d busstate=%d \n",
+			__FUNCTION__, dhd->pub.up, dhd->pub.busstate));
+		netif_stop_queue(net);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+		/* Send Event when bus down detected during data session */
+		if (dhd->pub.busstate == DHD_BUS_DOWN)  {
+			DHD_ERROR(("%s: Event HANG sent up\n", __FUNCTION__));
+			net_os_send_hang_message(net);
+		}
+#endif
+		DHD_OS_WAKE_UNLOCK(&dhd->pub);
+		return -ENODEV;
+	}
+
+	ifidx = dhd_net2idx(dhd, net);
+	if (ifidx == DHD_BAD_IF) {
+		DHD_ERROR(("%s: bad ifidx %d\n", __FUNCTION__, ifidx));
+		netif_stop_queue(net);
+		DHD_OS_WAKE_UNLOCK(&dhd->pub);
+		return -ENODEV;
+	}
+
+	/* Make sure there's enough room for any header */
+
+	if (skb_headroom(skb) < dhd->pub.hdrlen + htsfdlystat_sz) {
+		struct sk_buff *skb2;
+
+		DHD_INFO(("%s: insufficient headroom\n",
+		          dhd_ifname(&dhd->pub, ifidx)));
+		dhd->pub.tx_realloc++;
+
+		skb2 = skb_realloc_headroom(skb, dhd->pub.hdrlen + htsfdlystat_sz);
+
+		dev_kfree_skb(skb);
+		if ((skb = skb2) == NULL) {
+			DHD_ERROR(("%s: skb_realloc_headroom failed\n",
+			           dhd_ifname(&dhd->pub, ifidx)));
+			ret = -ENOMEM;
+			goto done;
+		}
+	}
+
+	/* Convert to packet */
+	if (!(pktbuf = PKTFRMNATIVE(dhd->pub.osh, skb))) {
+		DHD_ERROR(("%s: PKTFRMNATIVE failed\n",
+		           dhd_ifname(&dhd->pub, ifidx)));
+		dev_kfree_skb_any(skb);
+		ret = -ENOMEM;
+		goto done;
+	}
+#ifdef WLMEDIA_HTSF
+	if (htsfdlystat_sz && PKTLEN(dhd->pub.osh, pktbuf) >= ETHER_ADDR_LEN) {
+		uint8 *pktdata = (uint8 *)PKTDATA(dhd->pub.osh, pktbuf);
+		struct ether_header *eh = (struct ether_header *)pktdata;
+
+		if (!ETHER_ISMULTI(eh->ether_dhost) &&
+			(ntoh16(eh->ether_type) == ETHER_TYPE_IP)) {
+			eh->ether_type = hton16(ETHER_TYPE_BRCM_PKTDLYSTATS);
+		}
+	}
+#endif
+
+	ret = dhd_sendpkt(&dhd->pub, ifidx, pktbuf);
+
+
+done:
+	if (ret)
+		dhd->pub.dstats.tx_dropped++;
+	else
+		dhd->pub.tx_packets++;
+
+	DHD_OS_WAKE_UNLOCK(&dhd->pub);
+
+	/* Return ok: we always eat the packet */
+	return 0;
+}
+
+void
+dhd_txflowcontrol(dhd_pub_t *dhdp, int ifidx, bool state)
+{
+	struct net_device *net;
+	dhd_info_t *dhd = dhdp->info;
+	int i;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	dhdp->txoff = state;
+	ASSERT(dhd);
+
+	if (ifidx == ALL_INTERFACES) {
+		/* Flow control on all active interfaces */
+		for (i = 0; i < DHD_MAX_IFS; i++) {
+			if (dhd->iflist[i]) {
+				net = dhd->iflist[i]->net;
+				if (state == ON)
+					netif_stop_queue(net);
+				else
+					netif_wake_queue(net);
+			}
+		}
+	}
+	else {
+		if (dhd->iflist[ifidx]) {
+			net = dhd->iflist[ifidx]->net;
+			if (state == ON)
+				netif_stop_queue(net);
+			else
+				netif_wake_queue(net);
+		}
+	}
+}
+
+void
+dhd_rx_frame(dhd_pub_t *dhdp, int ifidx, void *pktbuf, int numpkt, uint8 chan)
+{
+	dhd_info_t *dhd = (dhd_info_t *)dhdp->info;
+	struct sk_buff *skb;
+	uchar *eth;
+	uint len;
+	void *data, *pnext = NULL, *save_pktbuf;
+	int i;
+	dhd_if_t *ifp;
+	wl_event_msg_t event;
+	int tout_rx = 0;
+	int tout_ctrl = 0;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	save_pktbuf = pktbuf;
+
+	for (i = 0; pktbuf && i < numpkt; i++, pktbuf = pnext) {
+		struct ether_header *eh;
+		struct dot11_llc_snap_header *lsh;
+
+		ifp = dhd->iflist[ifidx];
+		if (ifp == NULL) {
+			DHD_ERROR(("%s: ifp is NULL. drop packet\n",
+				__FUNCTION__));
+			PKTFREE(dhdp->osh, pktbuf, TRUE);
+			continue;
+		}
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0)
+		/* Dropping packets before registering net device to avoid kernel panic */
+		if (!ifp->net || ifp->net->reg_state != NETREG_REGISTERED ||
+			!dhd->pub.up) {
+			DHD_ERROR(("%s: net device is NOT registered yet. drop packet\n",
+			__FUNCTION__));
+			PKTFREE(dhdp->osh, pktbuf, TRUE);
+			continue;
+		}
+#endif
+
+		pnext = PKTNEXT(dhdp->osh, pktbuf);
+		PKTSETNEXT(wl->sh.osh, pktbuf, NULL);
+
+		eh = (struct ether_header *)PKTDATA(wl->sh.osh, pktbuf);
+		lsh = (struct dot11_llc_snap_header *)&eh[1];
+
+		if ((ntoh16(eh->ether_type) < ETHER_TYPE_MIN) &&
+		    (PKTLEN(wl->sh.osh, pktbuf) >= RFC1042_HDR_LEN) &&
+		    bcmp(lsh, BT_SIG_SNAP_MPROT, DOT11_LLC_SNAP_HDR_LEN - 2) == 0 &&
+		    lsh->type == HTON16(BTA_PROT_L2CAP)) {
+			amp_hci_ACL_data_t *ACL_data = (amp_hci_ACL_data_t *)
+			        ((uint8 *)eh + RFC1042_HDR_LEN);
+			ACL_data = NULL;
+		}
+
+#ifdef PROP_TXSTATUS
+		if (dhdp->wlfc_state && PKTLEN(wl->sh.osh, pktbuf) == 0) {
+			/* WLFC may send header only packet when
+			there is an urgent message but no packet to
+			piggy-back on
+			*/
+			((athost_wl_status_info_t*)dhdp->wlfc_state)->stats.wlfc_header_only_pkt++;
+			PKTFREE(dhdp->osh, pktbuf, TRUE);
+			DHD_TRACE(("RX: wlfc header \n"));
+			continue;
+		}
+#endif
+
+		skb = PKTTONATIVE(dhdp->osh, pktbuf);
+
+		/* Get the protocol, maintain skb around eth_type_trans()
+		 * The main reason for this hack is for the limitation of
+		 * Linux 2.4 where 'eth_type_trans' uses the 'net->hard_header_len'
+		 * to perform skb_pull inside vs ETH_HLEN. Since to avoid
+		 * coping of the packet coming from the network stack to add
+		 * BDC, Hardware header etc, during network interface registration
+		 * we set the 'net->hard_header_len' to ETH_HLEN + extra space required
+		 * for BDC, Hardware header etc. and not just the ETH_HLEN
+		 */
+		eth = skb->data;
+		len = skb->len;
+
+		ifp = dhd->iflist[ifidx];
+		if (ifp == NULL)
+			ifp = dhd->iflist[0];
+
+		ASSERT(ifp);
+		skb->dev = ifp->net;
+		skb->protocol = eth_type_trans(skb, skb->dev);
+
+		if (skb->pkt_type == PACKET_MULTICAST) {
+			dhd->pub.rx_multicast++;
+		}
+
+		skb->data = eth;
+		skb->len = len;
+
+#ifdef WLMEDIA_HTSF
+	dhd_htsf_addrxts(dhdp, pktbuf);
+#endif
+		/* Strip header, count, deliver upward */
+		skb_pull(skb, ETH_HLEN);
+
+		/* Process special event packets and then discard them */
+		if (ntoh16(skb->protocol) == ETHER_TYPE_BRCM) {
+			dhd_wl_host_event(dhd, &ifidx,
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 22)
+			skb->mac_header,
+#else
+			skb->mac.raw,
+#endif
+			&event,
+			&data);
+
+			wl_event_to_host_order(&event);
+			if (!tout_ctrl)
+				tout_ctrl = DHD_PACKET_TIMEOUT_MS;
+			if (event.event_type == WLC_E_BTA_HCI_EVENT) {
+				dhd_bta_doevt(dhdp, data, event.datalen);
+			}
+#ifdef PNO_SUPPORT
+			if (event.event_type == WLC_E_PFN_NET_FOUND) {
+				tout_ctrl *= 2;
+			}
+#endif /* PNO_SUPPORT */
+		} else {
+			tout_rx = DHD_PACKET_TIMEOUT_MS;
+		}
+
+		ASSERT(ifidx < DHD_MAX_IFS && dhd->iflist[ifidx]);
+		if (dhd->iflist[ifidx] && !dhd->iflist[ifidx]->state)
+			ifp = dhd->iflist[ifidx];
+
+		if (ifp->net)
+			ifp->net->last_rx = jiffies;
+
+		dhdp->dstats.rx_bytes += skb->len;
+		dhdp->rx_packets++; /* Local count */
+
+		if (in_interrupt()) {
+			netif_rx(skb);
+		} else {
+			/* If the receive is not processed inside an ISR,
+			 * the softirqd must be woken explicitly to service
+			 * the NET_RX_SOFTIRQ.  In 2.6 kernels, this is handled
+			 * by netif_rx_ni(), but in earlier kernels, we need
+			 * to do it manually.
+			 */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0)
+			netif_rx_ni(skb);
+#else
+			ulong flags;
+			netif_rx(skb);
+			local_irq_save(flags);
+			RAISE_RX_SOFTIRQ();
+			local_irq_restore(flags);
+#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0) */
+		}
+	}
+	DHD_OS_WAKE_LOCK_RX_TIMEOUT_ENABLE(dhdp, tout_rx);
+	DHD_OS_WAKE_LOCK_CTRL_TIMEOUT_ENABLE(dhdp, tout_ctrl);
+}
+
+void
+dhd_event(struct dhd_info *dhd, char *evpkt, int evlen, int ifidx)
+{
+	/* Linux version has nothing to do */
+	return;
+}
+
+void
+dhd_txcomplete(dhd_pub_t *dhdp, void *txp, bool success)
+{
+	uint ifidx;
+	dhd_info_t *dhd = (dhd_info_t *)(dhdp->info);
+	struct ether_header *eh;
+	uint16 type;
+	uint len;
+
+	dhd_prot_hdrpull(dhdp, &ifidx, txp);
+
+	eh = (struct ether_header *)PKTDATA(dhdp->osh, txp);
+	type  = ntoh16(eh->ether_type);
+
+	if (type == ETHER_TYPE_802_1X)
+		atomic_dec(&dhd->pend_8021x_cnt);
+
+	/* Crack open the packet and check to see if it is BT HCI ACL data packet.
+	 * If yes generate packet completion event.
+	 */
+	len = PKTLEN(dhdp->osh, txp);
+
+	/* Generate ACL data tx completion event locally to avoid SDIO bus transaction */
+	if ((type < ETHER_TYPE_MIN) && (len >= RFC1042_HDR_LEN)) {
+		struct dot11_llc_snap_header *lsh = (struct dot11_llc_snap_header *)&eh[1];
+
+		if (bcmp(lsh, BT_SIG_SNAP_MPROT, DOT11_LLC_SNAP_HDR_LEN - 2) == 0 &&
+		    ntoh16(lsh->type) == BTA_PROT_L2CAP) {
+
+			dhd_bta_tx_hcidata_complete(dhdp, txp, success);
+		}
+	}
+}
+
+static struct net_device_stats *
+dhd_get_stats(struct net_device *net)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
+	dhd_if_t *ifp;
+	int ifidx;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	ifidx = dhd_net2idx(dhd, net);
+	if (ifidx == DHD_BAD_IF) {
+		DHD_ERROR(("%s: BAD_IF\n", __FUNCTION__));
+		return NULL;
+	}
+
+	ifp = dhd->iflist[ifidx];
+	ASSERT(dhd && ifp);
+
+	if (dhd->pub.up) {
+		/* Use the protocol to get dongle stats */
+		dhd_prot_dstats(&dhd->pub);
+	}
+
+	/* Copy dongle stats to net device stats */
+	ifp->stats.rx_packets = dhd->pub.dstats.rx_packets;
+	ifp->stats.tx_packets = dhd->pub.dstats.tx_packets;
+	ifp->stats.rx_bytes = dhd->pub.dstats.rx_bytes;
+	ifp->stats.tx_bytes = dhd->pub.dstats.tx_bytes;
+	ifp->stats.rx_errors = dhd->pub.dstats.rx_errors;
+	ifp->stats.tx_errors = dhd->pub.dstats.tx_errors;
+	ifp->stats.rx_dropped = dhd->pub.dstats.rx_dropped;
+	ifp->stats.tx_dropped = dhd->pub.dstats.tx_dropped;
+	ifp->stats.multicast = dhd->pub.dstats.multicast;
+
+	return &ifp->stats;
+}
+
+#ifdef DHDTHREAD
+static int
+dhd_watchdog_thread(void *data)
+{
+	tsk_ctl_t *tsk = (tsk_ctl_t *)data;
+	dhd_info_t *dhd = (dhd_info_t *)tsk->parent;
+	/* This thread doesn't need any user-level access,
+	 * so get rid of all our resources
+	 */
+	if (dhd_watchdog_prio > 0) {
+		struct sched_param param;
+		param.sched_priority = (dhd_watchdog_prio < MAX_RT_PRIO)?
+			dhd_watchdog_prio:(MAX_RT_PRIO-1);
+		setScheduler(current, SCHED_FIFO, &param);
+	}
+
+	DAEMONIZE("dhd_watchdog");
+
+	/* Run until signal received */
+	complete(&tsk->completed);
+
+	while (1)
+		if (down_interruptible (&tsk->sema) == 0) {
+			unsigned long flags;
+
+			SMP_RD_BARRIER_DEPENDS();
+			if (tsk->terminated) {
+				break;
+			}
+
+			dhd_os_sdlock(&dhd->pub);
+			if (dhd->pub.dongle_reset == FALSE) {
+				DHD_TIMER(("%s:\n", __FUNCTION__));
+
+				/* Call the bus module watchdog */
+				dhd_bus_watchdog(&dhd->pub);
+
+				flags = dhd_os_spin_lock(&dhd->pub);
+				/* Count the tick for reference */
+				dhd->pub.tickcnt++;
+				/* Reschedule the watchdog */
+				if (dhd->wd_timer_valid)
+					mod_timer(&dhd->timer,
+					jiffies + dhd_watchdog_ms * HZ / 1000);
+				dhd_os_spin_unlock(&dhd->pub, flags);
+			}
+			dhd_os_sdunlock(&dhd->pub);
+			DHD_OS_WAKE_UNLOCK(&dhd->pub);
+		} else {
+			break;
+		}
+
+	complete_and_exit(&tsk->completed, 0);
+}
+#endif /* DHDTHREAD */
+
+static void dhd_watchdog(ulong data)
+{
+	dhd_info_t *dhd = (dhd_info_t *)data;
+	unsigned long flags;
+
+	DHD_OS_WAKE_LOCK(&dhd->pub);
+	if (dhd->pub.dongle_reset) {
+		DHD_OS_WAKE_UNLOCK(&dhd->pub);
+		return;
+	}
+
+#ifdef DHDTHREAD
+	if (dhd->thr_wdt_ctl.thr_pid >= 0) {
+		up(&dhd->thr_wdt_ctl.sema);
+		return;
+	}
+#endif /* DHDTHREAD */
+
+	dhd_os_sdlock(&dhd->pub);
+	/* Call the bus module watchdog */
+	dhd_bus_watchdog(&dhd->pub);
+
+	flags = dhd_os_spin_lock(&dhd->pub);
+	/* Count the tick for reference */
+	dhd->pub.tickcnt++;
+
+	/* Reschedule the watchdog */
+	if (dhd->wd_timer_valid)
+		mod_timer(&dhd->timer, jiffies + dhd_watchdog_ms * HZ / 1000);
+	dhd_os_spin_unlock(&dhd->pub, flags);
+	dhd_os_sdunlock(&dhd->pub);
+	DHD_OS_WAKE_UNLOCK(&dhd->pub);
+}
+
+#ifdef DHDTHREAD
+static int
+dhd_dpc_thread(void *data)
+{
+	tsk_ctl_t *tsk = (tsk_ctl_t *)data;
+	dhd_info_t *dhd = (dhd_info_t *)tsk->parent;
+
+	/* This thread doesn't need any user-level access,
+	 * so get rid of all our resources
+	 */
+	if (dhd_dpc_prio > 0)
+	{
+		struct sched_param param;
+		param.sched_priority = (dhd_dpc_prio < MAX_RT_PRIO)?dhd_dpc_prio:(MAX_RT_PRIO-1);
+		setScheduler(current, SCHED_FIFO, &param);
+	}
+
+	DAEMONIZE("dhd_dpc");
+	/* DHD_OS_WAKE_LOCK is called in dhd_sched_dpc[dhd_linux.c] down below  */
+
+	/*  signal: thread has started */
+	complete(&tsk->completed);
+
+	/* Run until signal received */
+	while (1) {
+		if (down_interruptible(&tsk->sema) == 0) {
+
+			SMP_RD_BARRIER_DEPENDS();
+			if (tsk->terminated) {
+				break;
+			}
+
+			/* Call bus dpc unless it indicated down (then clean stop) */
+			if (dhd->pub.busstate != DHD_BUS_DOWN) {
+				if (dhd_bus_dpc(dhd->pub.bus)) {
+					up(&tsk->sema);
+				}
+				else {
+					DHD_OS_WAKE_UNLOCK(&dhd->pub);
+				}
+			} else {
+				if (dhd->pub.up)
+					dhd_bus_stop(dhd->pub.bus, TRUE);
+				DHD_OS_WAKE_UNLOCK(&dhd->pub);
+			}
+		}
+		else
+			break;
+	}
+
+	complete_and_exit(&tsk->completed, 0);
+}
+#endif /* DHDTHREAD */
+
+static void
+dhd_dpc(ulong data)
+{
+	dhd_info_t *dhd;
+
+	dhd = (dhd_info_t *)data;
+
+	/* this (tasklet) can be scheduled in dhd_sched_dpc[dhd_linux.c]
+	 * down below , wake lock is set,
+	 * the tasklet is initialized in dhd_attach()
+	 */
+	/* Call bus dpc unless it indicated down (then clean stop) */
+	if (dhd->pub.busstate != DHD_BUS_DOWN) {
+		if (dhd_bus_dpc(dhd->pub.bus))
+			tasklet_schedule(&dhd->tasklet);
+		else
+			DHD_OS_WAKE_UNLOCK(&dhd->pub);
+	} else {
+		dhd_bus_stop(dhd->pub.bus, TRUE);
+		DHD_OS_WAKE_UNLOCK(&dhd->pub);
+	}
+}
+
+void
+dhd_sched_dpc(dhd_pub_t *dhdp)
+{
+	dhd_info_t *dhd = (dhd_info_t *)dhdp->info;
+
+	DHD_OS_WAKE_LOCK(dhdp);
+#ifdef DHDTHREAD
+	if (dhd->thr_dpc_ctl.thr_pid >= 0) {
+		up(&dhd->thr_dpc_ctl.sema);
+		return;
+	}
+#endif /* DHDTHREAD */
+
+	tasklet_schedule(&dhd->tasklet);
+}
+
+#ifdef TOE
+/* Retrieve current toe component enables, which are kept as a bitmap in toe_ol iovar */
+static int
+dhd_toe_get(dhd_info_t *dhd, int ifidx, uint32 *toe_ol)
+{
+	wl_ioctl_t ioc;
+	char buf[32];
+	int ret;
+
+	memset(&ioc, 0, sizeof(ioc));
+
+	ioc.cmd = WLC_GET_VAR;
+	ioc.buf = buf;
+	ioc.len = (uint)sizeof(buf);
+	ioc.set = FALSE;
+
+	strcpy(buf, "toe_ol");
+	if ((ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len)) < 0) {
+		/* Check for older dongle image that doesn't support toe_ol */
+		if (ret == -EIO) {
+			DHD_ERROR(("%s: toe not supported by device\n",
+				dhd_ifname(&dhd->pub, ifidx)));
+			return -EOPNOTSUPP;
+		}
+
+		DHD_INFO(("%s: could not get toe_ol: ret=%d\n", dhd_ifname(&dhd->pub, ifidx), ret));
+		return ret;
+	}
+
+	memcpy(toe_ol, buf, sizeof(uint32));
+	return 0;
+}
+
+/* Set current toe component enables in toe_ol iovar, and set toe global enable iovar */
+static int
+dhd_toe_set(dhd_info_t *dhd, int ifidx, uint32 toe_ol)
+{
+	wl_ioctl_t ioc;
+	char buf[32];
+	int toe, ret;
+
+	memset(&ioc, 0, sizeof(ioc));
+
+	ioc.cmd = WLC_SET_VAR;
+	ioc.buf = buf;
+	ioc.len = (uint)sizeof(buf);
+	ioc.set = TRUE;
+
+	/* Set toe_ol as requested */
+
+	strcpy(buf, "toe_ol");
+	memcpy(&buf[sizeof("toe_ol")], &toe_ol, sizeof(uint32));
+
+	if ((ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len)) < 0) {
+		DHD_ERROR(("%s: could not set toe_ol: ret=%d\n",
+			dhd_ifname(&dhd->pub, ifidx), ret));
+		return ret;
+	}
+
+	/* Enable toe globally only if any components are enabled. */
+
+	toe = (toe_ol != 0);
+
+	strcpy(buf, "toe");
+	memcpy(&buf[sizeof("toe")], &toe, sizeof(uint32));
+
+	if ((ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len)) < 0) {
+		DHD_ERROR(("%s: could not set toe: ret=%d\n", dhd_ifname(&dhd->pub, ifidx), ret));
+		return ret;
+	}
+
+	return 0;
+}
+#endif /* TOE */
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24)
+static void
+dhd_ethtool_get_drvinfo(struct net_device *net, struct ethtool_drvinfo *info)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
+
+	sprintf(info->driver, "wl");
+	sprintf(info->version, "%lu", dhd->pub.drv_version);
+}
+
+struct ethtool_ops dhd_ethtool_ops = {
+	.get_drvinfo = dhd_ethtool_get_drvinfo
+};
+#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24) */
+
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 4, 2)
+static int
+dhd_ethtool(dhd_info_t *dhd, void *uaddr)
+{
+	struct ethtool_drvinfo info;
+	char drvname[sizeof(info.driver)];
+	uint32 cmd;
+#ifdef TOE
+	struct ethtool_value edata;
+	uint32 toe_cmpnt, csum_dir;
+	int ret;
+#endif
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	/* all ethtool calls start with a cmd word */
+	if (copy_from_user(&cmd, uaddr, sizeof (uint32)))
+		return -EFAULT;
+
+	switch (cmd) {
+	case ETHTOOL_GDRVINFO:
+		/* Copy out any request driver name */
+		if (copy_from_user(&info, uaddr, sizeof(info)))
+			return -EFAULT;
+		strncpy(drvname, info.driver, sizeof(info.driver));
+		drvname[sizeof(info.driver)-1] = '\0';
+
+		/* clear struct for return */
+		memset(&info, 0, sizeof(info));
+		info.cmd = cmd;
+
+		/* if dhd requested, identify ourselves */
+		if (strcmp(drvname, "?dhd") == 0) {
+			sprintf(info.driver, "dhd");
+			strcpy(info.version, EPI_VERSION_STR);
+		}
+
+		/* otherwise, require dongle to be up */
+		else if (!dhd->pub.up) {
+			DHD_ERROR(("%s: dongle is not up\n", __FUNCTION__));
+			return -ENODEV;
+		}
+
+		/* finally, report dongle driver type */
+		else if (dhd->pub.iswl)
+			sprintf(info.driver, "wl");
+		else
+			sprintf(info.driver, "xx");
+
+		sprintf(info.version, "%lu", dhd->pub.drv_version);
+		if (copy_to_user(uaddr, &info, sizeof(info)))
+			return -EFAULT;
+		DHD_CTL(("%s: given %*s, returning %s\n", __FUNCTION__,
+		         (int)sizeof(drvname), drvname, info.driver));
+		break;
+
+#ifdef TOE
+	/* Get toe offload components from dongle */
+	case ETHTOOL_GRXCSUM:
+	case ETHTOOL_GTXCSUM:
+		if ((ret = dhd_toe_get(dhd, 0, &toe_cmpnt)) < 0)
+			return ret;
+
+		csum_dir = (cmd == ETHTOOL_GTXCSUM) ? TOE_TX_CSUM_OL : TOE_RX_CSUM_OL;
+
+		edata.cmd = cmd;
+		edata.data = (toe_cmpnt & csum_dir) ? 1 : 0;
+
+		if (copy_to_user(uaddr, &edata, sizeof(edata)))
+			return -EFAULT;
+		break;
+
+	/* Set toe offload components in dongle */
+	case ETHTOOL_SRXCSUM:
+	case ETHTOOL_STXCSUM:
+		if (copy_from_user(&edata, uaddr, sizeof(edata)))
+			return -EFAULT;
+
+		/* Read the current settings, update and write back */
+		if ((ret = dhd_toe_get(dhd, 0, &toe_cmpnt)) < 0)
+			return ret;
+
+		csum_dir = (cmd == ETHTOOL_STXCSUM) ? TOE_TX_CSUM_OL : TOE_RX_CSUM_OL;
+
+		if (edata.data != 0)
+			toe_cmpnt |= csum_dir;
+		else
+			toe_cmpnt &= ~csum_dir;
+
+		if ((ret = dhd_toe_set(dhd, 0, toe_cmpnt)) < 0)
+			return ret;
+
+		/* If setting TX checksum mode, tell Linux the new mode */
+		if (cmd == ETHTOOL_STXCSUM) {
+			if (edata.data)
+				dhd->iflist[0]->net->features |= NETIF_F_IP_CSUM;
+			else
+				dhd->iflist[0]->net->features &= ~NETIF_F_IP_CSUM;
+		}
+
+		break;
+#endif /* TOE */
+
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+#endif /* LINUX_VERSION_CODE > KERNEL_VERSION(2, 4, 2) */
+
+static bool dhd_check_hang(struct net_device *net, dhd_pub_t *dhdp, int error)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	if (!dhdp)
+		return FALSE;
+	if ((error == -ETIMEDOUT) || ((dhdp->busstate == DHD_BUS_DOWN) &&
+		(!dhdp->dongle_reset))) {
+		DHD_ERROR(("%s: Event HANG send up due to  re=%d te=%d e=%d s=%d\n", __FUNCTION__,
+			dhdp->rxcnt_timeout, dhdp->txcnt_timeout, error, dhdp->busstate));
+		net_os_send_hang_message(net);
+		return TRUE;
+	}
+#endif
+	return FALSE;
+}
+
+static int
+dhd_ioctl_entry(struct net_device *net, struct ifreq *ifr, int cmd)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
+	dhd_ioctl_t ioc;
+	int bcmerror = 0;
+	int buflen = 0;
+	void *buf = NULL;
+	uint driver = 0;
+	int ifidx;
+	int ret;
+
+	DHD_OS_WAKE_LOCK(&dhd->pub);
+
+	/* send to dongle only if we are not waiting for reload already */
+	if (dhd->pub.hang_was_sent) {
+		DHD_ERROR(("%s: HANG was sent up earlier\n", __FUNCTION__));
+		DHD_OS_WAKE_LOCK_CTRL_TIMEOUT_ENABLE(&dhd->pub, DHD_EVENT_TIMEOUT_MS);
+		DHD_OS_WAKE_UNLOCK(&dhd->pub);
+		return OSL_ERROR(BCME_DONGLE_DOWN);
+	}
+
+	ifidx = dhd_net2idx(dhd, net);
+	DHD_TRACE(("%s: ifidx %d, cmd 0x%04x\n", __FUNCTION__, ifidx, cmd));
+
+	if (ifidx == DHD_BAD_IF) {
+		DHD_ERROR(("%s: BAD IF\n", __FUNCTION__));
+		DHD_OS_WAKE_UNLOCK(&dhd->pub);
+		return -1;
+	}
+
+#if defined(CONFIG_BCMDHD_WEXT)
+	/* linux wireless extensions */
+	if ((cmd >= SIOCIWFIRST) && (cmd <= SIOCIWLAST)) {
+		/* may recurse, do NOT lock */
+		ret = wl_iw_ioctl(net, ifr, cmd);
+		DHD_OS_WAKE_UNLOCK(&dhd->pub);
+		return ret;
+	}
+#endif /* defined(CONFIG_BCMDHD_WEXT) */
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 4, 2)
+	if (cmd == SIOCETHTOOL) {
+		ret = dhd_ethtool(dhd, (void*)ifr->ifr_data);
+		DHD_OS_WAKE_UNLOCK(&dhd->pub);
+		return ret;
+	}
+#endif /* LINUX_VERSION_CODE > KERNEL_VERSION(2, 4, 2) */
+
+	if (cmd == SIOCDEVPRIVATE+1) {
+		ret = wl_android_priv_cmd(net, ifr, cmd);
+		dhd_check_hang(net, &dhd->pub, ret);
+		DHD_OS_WAKE_UNLOCK(&dhd->pub);
+		return ret;
+	}
+
+	if (cmd != SIOCDEVPRIVATE) {
+		DHD_OS_WAKE_UNLOCK(&dhd->pub);
+		return -EOPNOTSUPP;
+	}
+
+	memset(&ioc, 0, sizeof(ioc));
+
+	/* Copy the ioc control structure part of ioctl request */
+	if (copy_from_user(&ioc, ifr->ifr_data, sizeof(wl_ioctl_t))) {
+		bcmerror = -BCME_BADADDR;
+		goto done;
+	}
+
+	/* Copy out any buffer passed */
+	if (ioc.buf) {
+		buflen = MIN(ioc.len, DHD_IOCTL_MAXLEN);
+		/* optimization for direct ioctl calls from kernel */
+		/*
+		if (segment_eq(get_fs(), KERNEL_DS)) {
+			buf = ioc.buf;
+		} else {
+		*/
+		{
+			if (!(buf = (char*)MALLOC(dhd->pub.osh, buflen))) {
+				bcmerror = -BCME_NOMEM;
+				goto done;
+			}
+			if (copy_from_user(buf, ioc.buf, buflen)) {
+				bcmerror = -BCME_BADADDR;
+				goto done;
+			}
+		}
+	}
+
+	/* To differentiate between wl and dhd read 4 more byes */
+	if ((copy_from_user(&driver, (char *)ifr->ifr_data + sizeof(wl_ioctl_t),
+		sizeof(uint)) != 0)) {
+		bcmerror = -BCME_BADADDR;
+		goto done;
+	}
+
+	if (!capable(CAP_NET_ADMIN)) {
+		bcmerror = -BCME_EPERM;
+		goto done;
+	}
+
+	/* check for local dhd ioctl and handle it */
+	if (driver == DHD_IOCTL_MAGIC) {
+		bcmerror = dhd_ioctl((void *)&dhd->pub, &ioc, buf, buflen);
+		if (bcmerror)
+			dhd->pub.bcmerror = bcmerror;
+		goto done;
+	}
+
+	/* send to dongle (must be up, and wl). */
+	if (dhd->pub.busstate != DHD_BUS_DATA) {
+		bcmerror = BCME_DONGLE_DOWN;
+		goto done;
+	}
+
+	if (!dhd->pub.iswl) {
+		bcmerror = BCME_DONGLE_DOWN;
+		goto done;
+	}
+
+	/*
+	 * Flush the TX queue if required for proper message serialization:
+	 * Intercept WLC_SET_KEY IOCTL - serialize M4 send and set key IOCTL to
+	 * prevent M4 encryption and
+	 * intercept WLC_DISASSOC IOCTL - serialize WPS-DONE and WLC_DISASSOC IOCTL to
+	 * prevent disassoc frame being sent before WPS-DONE frame.
+	 */
+	if (ioc.cmd == WLC_SET_KEY ||
+	    (ioc.cmd == WLC_SET_VAR && ioc.buf != NULL &&
+	     strncmp("wsec_key", ioc.buf, 9) == 0) ||
+	    (ioc.cmd == WLC_SET_VAR && ioc.buf != NULL &&
+	     strncmp("bsscfg:wsec_key", ioc.buf, 15) == 0) ||
+	    ioc.cmd == WLC_DISASSOC)
+		dhd_wait_pend8021x(net);
+
+#ifdef WLMEDIA_HTSF
+	if (ioc.buf) {
+		/*  short cut wl ioctl calls here  */
+		if (strcmp("htsf", ioc.buf) == 0) {
+			dhd_ioctl_htsf_get(dhd, 0);
+			return BCME_OK;
+		}
+
+		if (strcmp("htsflate", ioc.buf) == 0) {
+			if (ioc.set) {
+				memset(ts, 0, sizeof(tstamp_t)*TSMAX);
+				memset(&maxdelayts, 0, sizeof(tstamp_t));
+				maxdelay = 0;
+				tspktcnt = 0;
+				maxdelaypktno = 0;
+				memset(&vi_d1.bin, 0, sizeof(uint32)*NUMBIN);
+				memset(&vi_d2.bin, 0, sizeof(uint32)*NUMBIN);
+				memset(&vi_d3.bin, 0, sizeof(uint32)*NUMBIN);
+				memset(&vi_d4.bin, 0, sizeof(uint32)*NUMBIN);
+			} else {
+				dhd_dump_latency();
+			}
+			return BCME_OK;
+		}
+		if (strcmp("htsfclear", ioc.buf) == 0) {
+			memset(&vi_d1.bin, 0, sizeof(uint32)*NUMBIN);
+			memset(&vi_d2.bin, 0, sizeof(uint32)*NUMBIN);
+			memset(&vi_d3.bin, 0, sizeof(uint32)*NUMBIN);
+			memset(&vi_d4.bin, 0, sizeof(uint32)*NUMBIN);
+			htsf_seqnum = 0;
+			return BCME_OK;
+		}
+		if (strcmp("htsfhis", ioc.buf) == 0) {
+			dhd_dump_htsfhisto(&vi_d1, "H to D");
+			dhd_dump_htsfhisto(&vi_d2, "D to D");
+			dhd_dump_htsfhisto(&vi_d3, "D to H");
+			dhd_dump_htsfhisto(&vi_d4, "H to H");
+			return BCME_OK;
+		}
+		if (strcmp("tsport", ioc.buf) == 0) {
+			if (ioc.set) {
+				memcpy(&tsport, ioc.buf + 7, 4);
+			} else {
+				DHD_ERROR(("current timestamp port: %d \n", tsport));
+			}
+			return BCME_OK;
+		}
+	}
+#endif /* WLMEDIA_HTSF */
+
+	bcmerror = dhd_wl_ioctl(&dhd->pub, ifidx, (wl_ioctl_t *)&ioc, buf, buflen);
+
+done:
+	dhd_check_hang(net, &dhd->pub, bcmerror);
+
+	if (!bcmerror && buf && ioc.buf) {
+		if (copy_to_user(ioc.buf, buf, buflen))
+			bcmerror = -EFAULT;
+	}
+
+	if (buf)
+		MFREE(dhd->pub.osh, buf, buflen);
+
+	DHD_OS_WAKE_UNLOCK(&dhd->pub);
+
+	return OSL_ERROR(bcmerror);
+}
+
+#ifdef WL_CFG80211
+static int
+dhd_cleanup_virt_ifaces(dhd_info_t *dhd)
+{
+	int i = 1; /* Leave ifidx 0 [Primary Interface] */
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	int rollback_lock = FALSE;
+#endif
+
+	DHD_TRACE(("%s: Enter \n", __func__));
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	/* release lock for unregister_netdev */
+	if (rtnl_is_locked()) {
+		rtnl_unlock();
+		rollback_lock = TRUE;
+	}
+#endif
+
+	for (i = 1; i < DHD_MAX_IFS; i++) {
+		dhd_net_if_lock_local(dhd);
+		if (dhd->iflist[i]) {
+			DHD_TRACE(("Deleting IF: %d \n", i));
+			if ((dhd->iflist[i]->state != DHD_IF_DEL) &&
+				(dhd->iflist[i]->state != DHD_IF_DELETING)) {
+				dhd->iflist[i]->state = DHD_IF_DEL;
+				dhd->iflist[i]->idx = i;
+				dhd_op_if(dhd->iflist[i]);
+			}
+		}
+		dhd_net_if_unlock_local(dhd);
+	}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	if (rollback_lock)
+		rtnl_lock();
+#endif
+
+	return 0;
+}
+#endif /* WL_CFG80211 */
+
+static int
+dhd_stop(struct net_device *net)
+{
+	int ifidx = 0;
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
+	DHD_OS_WAKE_LOCK(&dhd->pub);
+	DHD_TRACE(("%s: Enter %p\n", __FUNCTION__, net));
+	if (dhd->pub.up == 0) {
+		goto exit;
+	}
+	ifidx = dhd_net2idx(dhd, net);
+
+#ifdef WL_CFG80211
+	if (ifidx == 0) {
+		wl_cfg80211_down(NULL);
+
+		/*
+		 * For CFG80211: Clean up all the left over virtual interfaces
+		 * when the primary Interface is brought down. [ifconfig wlan0 down]
+		 */
+		if ((dhd->dhd_state & DHD_ATTACH_STATE_ADD_IF) &&
+			(dhd->dhd_state & DHD_ATTACH_STATE_CFG80211)) {
+			dhd_cleanup_virt_ifaces(dhd);
+		}
+	}
+#endif
+
+#ifdef PROP_TXSTATUS
+	dhd_wlfc_cleanup(&dhd->pub);
+#endif
+	/* Set state and stop OS transmissions */
+	dhd->pub.up = 0;
+	netif_stop_queue(net);
+
+	/* Stop the protocol module */
+	dhd_prot_stop(&dhd->pub);
+
+	OLD_MOD_DEC_USE_COUNT;
+exit:
+#if defined(WL_CFG80211)
+	if (ifidx == 0 && !dhd_download_fw_on_driverload)
+		wl_android_wifi_off(net);
+#endif
+	dhd->pub.rxcnt_timeout = 0;
+	dhd->pub.txcnt_timeout = 0;
+
+	DHD_OS_WAKE_UNLOCK(&dhd->pub);
+	return 0;
+}
+
+static int
+dhd_open(struct net_device *net)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(net);
+
+#ifdef TOE
+	uint32 toe_ol;
+#endif
+	int ifidx;
+	int32 ret = 0;
+
+	DHD_OS_WAKE_LOCK(&dhd->pub);
+	/* Update FW path if it was changed */
+	if ((firmware_path != NULL) && (firmware_path[0] != '\0')) {
+		if (firmware_path[strlen(firmware_path)-1] == '\n')
+			firmware_path[strlen(firmware_path)-1] = '\0';
+		strcpy(fw_path, firmware_path);
+		firmware_path[0] = '\0';
+	}
+
+	dhd->pub.hang_was_sent = 0;
+#if !defined(WL_CFG80211)
+	/*
+	 * Force start if ifconfig_up gets called before START command
+	 * We keep WEXT's wl_control_wl_start to provide backward compatibility
+	 * This should be removed in the future
+	 */
+	ret = wl_control_wl_start(net);
+	if (ret != 0) {
+		DHD_ERROR(("%s: failed with code %d\n", __FUNCTION__, ret));
+		ret = -1;
+		goto exit;
+	}
+#endif
+
+	ifidx = dhd_net2idx(dhd, net);
+	DHD_TRACE(("%s: ifidx %d\n", __FUNCTION__, ifidx));
+
+	if (ifidx < 0) {
+		DHD_ERROR(("%s: Error: called with invalid IF\n", __FUNCTION__));
+		ret = -1;
+		goto exit;
+	}
+
+	if (!dhd->iflist[ifidx] || dhd->iflist[ifidx]->state == DHD_IF_DEL) {
+		DHD_ERROR(("%s: Error: called when IF already deleted\n", __FUNCTION__));
+		ret = -1;
+		goto exit;
+	}
+
+	if (ifidx == 0) {
+		atomic_set(&dhd->pend_8021x_cnt, 0);
+#if defined(WL_CFG80211)
+		DHD_ERROR(("\n%s\n", dhd_version));
+		if (!dhd_download_fw_on_driverload) {
+			ret = wl_android_wifi_on(net);
+			if (ret != 0) {
+				DHD_ERROR(("%s: failed with code %d\n", __FUNCTION__, ret));
+				ret = -1;
+				goto exit;
+			}
+		}
+#endif /* defined(WL_CFG80211) */
+
+		if (dhd->pub.busstate != DHD_BUS_DATA) {
+
+			/* try to bring up bus */
+			if ((ret = dhd_bus_start(&dhd->pub)) != 0) {
+				DHD_ERROR(("%s: failed with code %d\n", __FUNCTION__, ret));
+				ret = -1;
+				goto exit;
+			}
+
+		}
+
+		/* dhd_prot_init has been called in dhd_bus_start or wl_android_wifi_on */
+		memcpy(net->dev_addr, dhd->pub.mac.octet, ETHER_ADDR_LEN);
+
+#ifdef TOE
+		/* Get current TOE mode from dongle */
+		if (dhd_toe_get(dhd, ifidx, &toe_ol) >= 0 && (toe_ol & TOE_TX_CSUM_OL) != 0)
+			dhd->iflist[ifidx]->net->features |= NETIF_F_IP_CSUM;
+		else
+			dhd->iflist[ifidx]->net->features &= ~NETIF_F_IP_CSUM;
+#endif /* TOE */
+
+#if defined(WL_CFG80211)
+		if (unlikely(wl_cfg80211_up(NULL))) {
+			DHD_ERROR(("%s: failed to bring up cfg80211\n", __FUNCTION__));
+			ret = -1;
+			goto exit;
+		}
+#endif /* WL_CFG80211 */
+	}
+
+	/* Allow transmit calls */
+	netif_start_queue(net);
+	dhd->pub.up = 1;
+
+#ifdef BCMDBGFS
+	dhd_dbg_init(&dhd->pub);
+#endif
+
+	OLD_MOD_INC_USE_COUNT;
+exit:
+	if (ret)
+		dhd_stop(net);
+
+	DHD_OS_WAKE_UNLOCK(&dhd->pub);
+	return ret;
+}
+
+int dhd_do_driver_init(struct net_device *net)
+{
+	dhd_info_t *dhd = NULL;
+
+	if (!net) {
+		DHD_ERROR(("Primary Interface not initialized \n"));
+		return -EINVAL;
+	}
+
+	dhd = *(dhd_info_t **)netdev_priv(net);
+
+	/* If driver is already initialized, do nothing
+	 */
+	if (dhd->pub.busstate == DHD_BUS_DATA) {
+		DHD_TRACE(("Driver already Inititalized. Nothing to do"));
+		return 0;
+	}
+
+	if (dhd_open(net) < 0) {
+		DHD_ERROR(("Driver Init Failed \n"));
+		return -1;
+	}
+
+	return 0;
+}
+
+osl_t *
+dhd_osl_attach(void *pdev, uint bustype)
+{
+	return osl_attach(pdev, bustype, TRUE);
+}
+
+void
+dhd_osl_detach(osl_t *osh)
+{
+	if (MALLOCED(osh)) {
+		DHD_ERROR(("%s: MEMORY LEAK %d bytes\n", __FUNCTION__, MALLOCED(osh)));
+	}
+	osl_detach(osh);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	up(&dhd_registration_sem);
+#endif
+}
+
+int
+dhd_add_if(dhd_info_t *dhd, int ifidx, void *handle, char *name,
+	uint8 *mac_addr, uint32 flags, uint8 bssidx)
+{
+	dhd_if_t *ifp;
+
+	DHD_TRACE(("%s: idx %d, handle->%p\n", __FUNCTION__, ifidx, handle));
+
+	ASSERT(dhd && (ifidx < DHD_MAX_IFS));
+
+	ifp = dhd->iflist[ifidx];
+	if (ifp != NULL) {
+		if (ifp->net != NULL) {
+			netif_stop_queue(ifp->net);
+			unregister_netdev(ifp->net);
+			free_netdev(ifp->net);
+		}
+	} else
+		if ((ifp = MALLOC(dhd->pub.osh, sizeof(dhd_if_t))) == NULL) {
+			DHD_ERROR(("%s: OOM - dhd_if_t\n", __FUNCTION__));
+			return -ENOMEM;
+		}
+
+	memset(ifp, 0, sizeof(dhd_if_t));
+	ifp->info = dhd;
+	dhd->iflist[ifidx] = ifp;
+	strncpy(ifp->name, name, IFNAMSIZ);
+	ifp->name[IFNAMSIZ] = '\0';
+	if (mac_addr != NULL)
+		memcpy(&ifp->mac_addr, mac_addr, ETHER_ADDR_LEN);
+
+	if (handle == NULL) {
+		ifp->state = DHD_IF_ADD;
+		ifp->idx = ifidx;
+		ifp->bssidx = bssidx;
+		ASSERT(dhd->thr_sysioc_ctl.thr_pid >= 0);
+		up(&dhd->thr_sysioc_ctl.sema);
+	} else
+		ifp->net = (struct net_device *)handle;
+
+	return 0;
+}
+
+void
+dhd_del_if(dhd_info_t *dhd, int ifidx)
+{
+	dhd_if_t *ifp;
+
+	DHD_TRACE(("%s: idx %d\n", __FUNCTION__, ifidx));
+
+	ASSERT(dhd && ifidx && (ifidx < DHD_MAX_IFS));
+	ifp = dhd->iflist[ifidx];
+	if (!ifp) {
+		DHD_ERROR(("%s: Null interface\n", __FUNCTION__));
+		return;
+	}
+
+	ifp->state = DHD_IF_DEL;
+	ifp->idx = ifidx;
+	ASSERT(dhd->thr_sysioc_ctl.thr_pid >= 0);
+	up(&dhd->thr_sysioc_ctl.sema);
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 31))
+static struct net_device_ops dhd_ops_pri = {
+       .ndo_open = dhd_open,
+       .ndo_stop = dhd_stop,
+       .ndo_get_stats = dhd_get_stats,
+       .ndo_do_ioctl = dhd_ioctl_entry,
+       .ndo_start_xmit = dhd_start_xmit,
+       .ndo_set_mac_address = dhd_set_mac_address,
+       .ndo_set_multicast_list = dhd_set_multicast_list,
+};
+
+static struct net_device_ops dhd_ops_virt = {
+       .ndo_get_stats = dhd_get_stats,
+       .ndo_do_ioctl = dhd_ioctl_entry,
+       .ndo_start_xmit = dhd_start_xmit,
+       .ndo_set_mac_address = dhd_set_mac_address,
+       .ndo_set_multicast_list = dhd_set_multicast_list,
+};
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 31)) */
+
+dhd_pub_t *
+dhd_attach(osl_t *osh, struct dhd_bus *bus, uint bus_hdrlen, void *dev)
+{
+	dhd_info_t *dhd = NULL;
+	struct net_device *net = NULL;
+
+	dhd_attach_states_t dhd_state = DHD_ATTACH_STATE_INIT;
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	/* updates firmware nvram path if it was provided as module parameters */
+	if ((firmware_path != NULL) && (firmware_path[0] != '\0'))
+		strcpy(fw_path, firmware_path);
+	if ((nvram_path != NULL) && (nvram_path[0] != '\0'))
+		strcpy(nv_path, nvram_path);
+
+	/* Allocate etherdev, including space for private structure */
+	if (!(net = alloc_etherdev(sizeof(dhd)))) {
+		DHD_ERROR(("%s: OOM - alloc_etherdev\n", __FUNCTION__));
+		goto fail;
+	}
+	dhd_state |= DHD_ATTACH_STATE_NET_ALLOC;
+
+	/* Allocate primary dhd_info */
+	if (!(dhd = MALLOC(osh, sizeof(dhd_info_t)))) {
+		DHD_ERROR(("%s: OOM - alloc dhd_info\n", __FUNCTION__));
+		goto fail;
+	}
+	memset(dhd, 0, sizeof(dhd_info_t));
+
+#ifdef DHDTHREAD
+	dhd->thr_dpc_ctl.thr_pid = DHD_PID_KT_TL_INVALID;
+	dhd->thr_wdt_ctl.thr_pid = DHD_PID_KT_INVALID;
+#else
+	dhd->dhd_tasklet_create = FALSE;
+#endif /* DHDTHREAD */
+	dhd->thr_sysioc_ctl.thr_pid = DHD_PID_KT_INVALID;
+	dhd_state |= DHD_ATTACH_STATE_DHD_ALLOC;
+
+	/*
+	 * Save the dhd_info into the priv
+	 */
+	memcpy((void *)netdev_priv(net), &dhd, sizeof(dhd));
+	dhd->pub.osh = osh;
+
+	/* Link to info module */
+	dhd->pub.info = dhd;
+	/* Link to bus module */
+	dhd->pub.bus = bus;
+	dhd->pub.hdrlen = bus_hdrlen;
+
+	/* Set network interface name if it was provided as module parameter */
+	if (iface_name[0]) {
+		int len;
+		char ch;
+		strncpy(net->name, iface_name, IFNAMSIZ);
+		net->name[IFNAMSIZ - 1] = 0;
+		len = strlen(net->name);
+		ch = net->name[len - 1];
+		if ((ch > '9' || ch < '0') && (len < IFNAMSIZ - 2))
+			strcat(net->name, "%d");
+	}
+
+	if (dhd_add_if(dhd, 0, (void *)net, net->name, NULL, 0, 0) == DHD_BAD_IF)
+		goto fail;
+	dhd_state |= DHD_ATTACH_STATE_ADD_IF;
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31))
+	net->open = NULL;
+#else
+	net->netdev_ops = NULL;
+#endif
+
+	sema_init(&dhd->proto_sem, 1);
+
+#ifdef PROP_TXSTATUS
+	spin_lock_init(&dhd->wlfc_spinlock);
+	dhd->pub.wlfc_enabled = TRUE;
+#endif /* PROP_TXSTATUS */
+
+	/* Initialize other structure content */
+	init_waitqueue_head(&dhd->ioctl_resp_wait);
+	init_waitqueue_head(&dhd->ctrl_wait);
+
+	/* Initialize the spinlocks */
+	spin_lock_init(&dhd->sdlock);
+	spin_lock_init(&dhd->txqlock);
+	spin_lock_init(&dhd->dhd_lock);
+
+	/* Initialize Wakelock stuff */
+	spin_lock_init(&dhd->wakelock_spinlock);
+	dhd->wakelock_counter = 0;
+	dhd->wakelock_rx_timeout_enable = 0;
+	dhd->wakelock_ctrl_timeout_enable = 0;
+#ifdef CONFIG_HAS_WAKELOCK
+	wake_lock_init(&dhd->wl_wifi, WAKE_LOCK_SUSPEND, "wlan_wake");
+	wake_lock_init(&dhd->wl_rxwake, WAKE_LOCK_SUSPEND, "wlan_rx_wake");
+	wake_lock_init(&dhd->wl_ctrlwake, WAKE_LOCK_SUSPEND, "wlan_ctrl_wake");
+#endif
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
+	mutex_init(&dhd->dhd_net_if_mutex);
+	mutex_init(&dhd->dhd_suspend_mutex);
+#endif
+	dhd_state |= DHD_ATTACH_STATE_WAKELOCKS_INIT;
+
+	/* Attach and link in the protocol */
+	if (dhd_prot_attach(&dhd->pub) != 0) {
+		DHD_ERROR(("dhd_prot_attach failed\n"));
+		goto fail;
+	}
+	dhd_state |= DHD_ATTACH_STATE_PROT_ATTACH;
+
+#ifdef WL_CFG80211
+	/* Attach and link in the cfg80211 */
+	if (unlikely(wl_cfg80211_attach(net, &dhd->pub))) {
+		DHD_ERROR(("wl_cfg80211_attach failed\n"));
+		goto fail;
+	}
+
+	dhd_monitor_init(&dhd->pub);
+	dhd_state |= DHD_ATTACH_STATE_CFG80211;
+#endif
+#if defined(CONFIG_BCMDHD_WEXT)
+	/* Attach and link in the iw */
+	if (!(dhd_state &  DHD_ATTACH_STATE_CFG80211)) {
+		if (wl_iw_attach(net, (void *)&dhd->pub) != 0) {
+			DHD_ERROR(("wl_iw_attach failed\n"));
+			goto fail;
+		}
+	dhd_state |= DHD_ATTACH_STATE_WL_ATTACH;
+	}
+#endif /* defined(CONFIG_BCMDHD_WEXT) */
+
+
+	/* Set up the watchdog timer */
+	init_timer(&dhd->timer);
+	dhd->timer.data = (ulong)dhd;
+	dhd->timer.function = dhd_watchdog;
+
+#ifdef DHDTHREAD
+	/* Initialize thread based operation and lock */
+	sema_init(&dhd->sdsem, 1);
+	if ((dhd_watchdog_prio >= 0) && (dhd_dpc_prio >= 0)) {
+		dhd->threads_only = TRUE;
+	}
+	else {
+		dhd->threads_only = FALSE;
+	}
+
+	if (dhd_dpc_prio >= 0) {
+		/* Initialize watchdog thread */
+		PROC_START(dhd_watchdog_thread, dhd, &dhd->thr_wdt_ctl, 0);
+	} else {
+		dhd->thr_wdt_ctl.thr_pid = -1;
+	}
+
+	/* Set up the bottom half handler */
+	if (dhd_dpc_prio >= 0) {
+		/* Initialize DPC thread */
+		PROC_START(dhd_dpc_thread, dhd, &dhd->thr_dpc_ctl, 0);
+	} else {
+		/*  use tasklet for dpc */
+		tasklet_init(&dhd->tasklet, dhd_dpc, (ulong)dhd);
+		dhd->thr_dpc_ctl.thr_pid = -1;
+	}
+#else
+	/* Set up the bottom half handler */
+	tasklet_init(&dhd->tasklet, dhd_dpc, (ulong)dhd);
+	dhd->dhd_tasklet_create = TRUE;
+#endif /* DHDTHREAD */
+
+	if (dhd_sysioc) {
+		PROC_START(_dhd_sysioc_thread, dhd, &dhd->thr_sysioc_ctl, 0);
+	} else {
+		dhd->thr_sysioc_ctl.thr_pid = -1;
+	}
+	dhd_state |= DHD_ATTACH_STATE_THREADS_CREATED;
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	INIT_WORK(&dhd->work_hang, dhd_hang_process);
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))  */
+	/*
+	 * Save the dhd_info into the priv
+	 */
+	memcpy(netdev_priv(net), &dhd, sizeof(dhd));
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP)
+	register_pm_notifier(&dhd_sleep_pm_notifier);
+#endif /*  (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) */
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) && defined(DHD_USE_EARLYSUSPEND)
+	dhd->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 20;
+	dhd->early_suspend.suspend = dhd_early_suspend;
+	dhd->early_suspend.resume = dhd_late_resume;
+	register_early_suspend(&dhd->early_suspend);
+	dhd_state |= DHD_ATTACH_STATE_EARLYSUSPEND_DONE;
+#endif
+
+#ifdef ARP_OFFLOAD_SUPPORT
+	dhd->pend_ipaddr = 0;
+	register_inetaddr_notifier(&dhd_notifier);
+#endif /* ARP_OFFLOAD_SUPPORT */
+
+	dhd_state |= DHD_ATTACH_STATE_DONE;
+	dhd->dhd_state = dhd_state;
+	return &dhd->pub;
+
+fail:
+	if (dhd_state < DHD_ATTACH_STATE_DHD_ALLOC) {
+		if (net) free_netdev(net);
+	} else {
+		DHD_TRACE(("%s: Calling dhd_detach dhd_state 0x%x &dhd->pub %p\n",
+			__FUNCTION__, dhd_state, &dhd->pub));
+		dhd->dhd_state = dhd_state;
+		dhd_detach(&dhd->pub);
+		dhd_free(&dhd->pub);
+	}
+
+	return NULL;
+}
+
+int
+dhd_bus_start(dhd_pub_t *dhdp)
+{
+	int ret = -1;
+	dhd_info_t *dhd = (dhd_info_t*)dhdp->info;
+	unsigned long flags;
+
+	ASSERT(dhd);
+
+	DHD_TRACE(("Enter %s:\n", __FUNCTION__));
+
+#ifdef DHDTHREAD
+	if (dhd->threads_only)
+		dhd_os_sdlock(dhdp);
+#endif /* DHDTHREAD */
+
+	/* try to download image and nvram to the dongle */
+	if  ((dhd->pub.busstate == DHD_BUS_DOWN) &&
+		(fw_path != NULL) && (fw_path[0] != '\0') &&
+		(nv_path != NULL) && (nv_path[0] != '\0')) {
+		/* wake lock moved to dhdsdio_download_firmware */
+		if (!(dhd_bus_download_firmware(dhd->pub.bus, dhd->pub.osh,
+		                                fw_path, nv_path))) {
+			DHD_ERROR(("%s: dhdsdio_probe_download failed. firmware = %s nvram = %s\n",
+			           __FUNCTION__, fw_path, nv_path));
+#ifdef DHDTHREAD
+			if (dhd->threads_only)
+				dhd_os_sdunlock(dhdp);
+#endif /* DHDTHREAD */
+			return -1;
+		}
+	}
+	if (dhd->pub.busstate != DHD_BUS_LOAD) {
+#ifdef DHDTHREAD
+		if (dhd->threads_only)
+			dhd_os_sdunlock(dhdp);
+#endif /* DHDTHREAD */
+		return -ENETDOWN;
+	}
+
+	/* Start the watchdog timer */
+	dhd->pub.tickcnt = 0;
+	dhd_os_wd_timer(&dhd->pub, dhd_watchdog_ms);
+
+	/* Bring up the bus */
+	if ((ret = dhd_bus_init(&dhd->pub, FALSE)) != 0) {
+
+		DHD_ERROR(("%s, dhd_bus_init failed %d\n", __FUNCTION__, ret));
+#ifdef DHDTHREAD
+		if (dhd->threads_only)
+			dhd_os_sdunlock(dhdp);
+#endif /* DHDTHREAD */
+		return ret;
+	}
+#if defined(OOB_INTR_ONLY)
+	/* Host registration for OOB interrupt */
+	if (bcmsdh_register_oob_intr(dhdp)) {
+		/* deactivate timer and wait for the handler to finish */
+
+		flags = dhd_os_spin_lock(&dhd->pub);
+		dhd->wd_timer_valid = FALSE;
+		dhd_os_spin_unlock(&dhd->pub, flags);
+		del_timer_sync(&dhd->timer);
+
+		DHD_ERROR(("%s Host failed to register for OOB\n", __FUNCTION__));
+#ifdef DHDTHREAD
+		if (dhd->threads_only)
+			dhd_os_sdunlock(dhdp);
+#endif /* DHDTHREAD */
+		return -ENODEV;
+	}
+
+	/* Enable oob at firmware */
+	dhd_enable_oob_intr(dhd->pub.bus, TRUE);
+#endif /* defined(OOB_INTR_ONLY) */
+
+	/* If bus is not ready, can't come up */
+	if (dhd->pub.busstate != DHD_BUS_DATA) {
+		flags = dhd_os_spin_lock(&dhd->pub);
+		dhd->wd_timer_valid = FALSE;
+		dhd_os_spin_unlock(&dhd->pub, flags);
+		del_timer_sync(&dhd->timer);
+		DHD_ERROR(("%s failed bus is not ready\n", __FUNCTION__));
+#ifdef DHDTHREAD
+		if (dhd->threads_only)
+			dhd_os_sdunlock(dhdp);
+#endif /* DHDTHREAD */
+		return -ENODEV;
+	}
+
+#ifdef DHDTHREAD
+	if (dhd->threads_only)
+		dhd_os_sdunlock(dhdp);
+#endif /* DHDTHREAD */
+
+#ifdef READ_MACADDR
+	dhd_read_macaddr(dhd);
+#endif
+
+	/* Bus is ready, do any protocol initialization */
+	if ((ret = dhd_prot_init(&dhd->pub)) < 0)
+		return ret;
+
+#ifdef WRITE_MACADDR
+	dhd_write_macaddr(dhd->pub.mac.octet);
+#endif
+
+#ifdef ARP_OFFLOAD_SUPPORT
+	if (dhd->pend_ipaddr) {
+#ifdef AOE_IP_ALIAS_SUPPORT
+		aoe_update_host_ipv4_table(&dhd->pub, dhd->pend_ipaddr, TRUE);
+#endif /* AOE_IP_ALIAS_SUPPORT */
+		dhd->pend_ipaddr = 0;
+	}
+#endif /* ARP_OFFLOAD_SUPPORT */
+
+	return 0;
+}
+
+#if !defined(AP) && defined(WLP2P) && defined(WL_ENABLE_P2P_IF)
+/* For Android ICS MR2 release, the concurrent mode is enabled by default and the firmware
+ * name would be fw_bcmdhd.bin. So we need to determine whether P2P is enabled in the STA
+ * firmware and accordingly enable concurrent mode (Apply P2P settings). SoftAP firmware
+ * would still be named as fw_bcmdhd_apsta.
+ */
+static u32
+dhd_concurrent_fw(dhd_pub_t *dhd)
+{
+	int ret = 0;
+	char buf[WLC_IOCTL_SMLEN];
+
+	if ((!op_mode) && (strstr(fw_path, "_p2p") == NULL) &&
+		(strstr(fw_path, "_apsta") == NULL)) {
+		/* Given path is for the STA firmware. Check whether P2P support is present in
+		 * the firmware. If so, set mode as P2P (concurrent support).
+		 */
+		memset(buf, 0, sizeof(buf));
+		bcm_mkiovar("p2p", 0, 0, buf, sizeof(buf));
+		if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_GET_VAR, buf, sizeof(buf),
+			FALSE, 0)) < 0) {
+			DHD_ERROR(("%s: Get P2P failed (error=%d)\n", __FUNCTION__, ret));
+		} else if (buf[0] == 1) {
+			DHD_TRACE(("%s: P2P is supported\n", __FUNCTION__));
+			return 1;
+		}
+	}
+	return ret;
+}
+#endif 
+
+/*
+ *   dhd_preinit_ioctls makes special pre-setting in the firmware before radio turns on
+ *   returns : 0 if all settings passed or negative value if anything failed
+*/
+int
+dhd_preinit_ioctls(dhd_pub_t *dhd)
+{
+	int ret = 0;
+	char eventmask[WL_EVENTING_MASK_LEN];
+	char iovbuf[WL_EVENTING_MASK_LEN + 12];	/*  Room for "event_msgs" + '\0' + bitvec  */
+#if !defined(WL_CFG80211)
+	uint up = 0;
+#endif /* defined(WL_CFG80211) */
+	uint power_mode = PM_FAST;
+	uint32 dongle_align = DHD_SDALIGN;
+	uint32 glom = 0;
+	uint bcn_timeout = DHD_BEACON_TIMEOUT_NORMAL;
+
+	uint retry_max = 3;
+#if defined(ARP_OFFLOAD_SUPPORT)
+	int arpoe = 1;
+#endif
+#if defined(KEEP_ALIVE)
+	int res;
+#endif /* defined(KEEP_ALIVE) */
+	int scan_assoc_time = DHD_SCAN_ACTIVE_TIME;
+	int scan_unassoc_time = 40;
+	int scan_passive_time = DHD_SCAN_PASSIVE_TIME;
+	char buf[WLC_IOCTL_SMLEN];
+	char *ptr;
+	uint32 listen_interval = LISTEN_INTERVAL; /* Default Listen Interval in Beacons */
+	uint16 chipID;
+#if defined(SOFTAP)
+	uint dtim = 1;
+#endif
+#if (defined(AP) && !defined(WLP2P)) || (!defined(AP) && defined(WL_CFG80211))
+	uint32 mpc = 0; /* Turn MPC off for AP/APSTA mode */
+#endif
+#if defined(AP) || defined(WLP2P)
+	uint32 apsta = 1; /* Enable APSTA mode */
+#endif /* defined(AP) || defined(WLP2P) */
+#ifdef GET_CUSTOM_MAC_ENABLE
+	struct ether_addr ea_addr;
+#endif /* GET_CUSTOM_MAC_ENABLE */
+	DHD_TRACE(("Enter %s\n", __FUNCTION__));
+	dhd->op_mode = 0;
+#ifdef GET_CUSTOM_MAC_ENABLE
+	ret = dhd_custom_get_mac_address(ea_addr.octet);
+	if (!ret) {
+		memset(buf, 0, sizeof(buf));
+		bcm_mkiovar("cur_etheraddr", (void *)&ea_addr, ETHER_ADDR_LEN, buf, sizeof(buf));
+		ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, buf, sizeof(buf), TRUE, 0);
+		if (ret < 0) {
+			DHD_ERROR(("%s: can't set custom MAC address , error=%d\n", __FUNCTION__, ret));
+			return BCME_NOTUP;
+		}
+		memcpy(dhd->mac.octet, ea_addr.octet, ETHER_ADDR_LEN);
+	} else {
+#endif /* GET_CUSTOM_MAC_ENABLE */
+		/* Get the default device MAC address directly from firmware */
+		memset(buf, 0, sizeof(buf));
+		bcm_mkiovar("cur_etheraddr", 0, 0, buf, sizeof(buf));
+		if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_GET_VAR, buf, sizeof(buf),
+			FALSE, 0)) < 0) {
+			DHD_ERROR(("%s: can't get MAC address , error=%d\n", __FUNCTION__, ret));
+			return BCME_NOTUP;
+		}
+		/* Update public MAC address after reading from Firmware */
+		memcpy(dhd->mac.octet, buf, ETHER_ADDR_LEN);
+#ifdef GET_CUSTOM_MAC_ENABLE
+	}
+#endif /* GET_CUSTOM_MAC_ENABLE */
+
+#ifdef SET_RANDOM_MAC_SOFTAP
+	if ((!op_mode && strstr(fw_path, "_apsta") != NULL) || (op_mode == HOSTAPD_MASK)) {
+		uint rand_mac;
+
+		srandom32((uint)jiffies);
+		rand_mac = random32();
+		iovbuf[0] = 0x02;              /* locally administered bit */
+		iovbuf[1] = 0x1A;
+		iovbuf[2] = 0x11;
+		iovbuf[3] = (unsigned char)(rand_mac & 0x0F) | 0xF0;
+		iovbuf[4] = (unsigned char)(rand_mac >> 8);
+		iovbuf[5] = (unsigned char)(rand_mac >> 16);
+
+		bcm_mkiovar("cur_etheraddr", (void *)iovbuf, ETHER_ADDR_LEN, buf, sizeof(buf));
+		ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, buf, sizeof(buf), TRUE, 0);
+		if (ret < 0) {
+			DHD_ERROR(("%s: can't set MAC address , error=%d\n", __FUNCTION__, ret));
+		} else
+			memcpy(dhd->mac.octet, iovbuf, ETHER_ADDR_LEN);
+	}
+#endif /* SET_RANDOM_MAC_SOFTAP */
+
+	DHD_TRACE(("Firmware = %s\n", fw_path));
+
+#if !defined(AP)  && defined(WLP2P)
+	/* Check if firmware with WFD support used */
+#if defined(WL_ENABLE_P2P_IF)
+	if ((ret = dhd_concurrent_fw(dhd)) < 0) {
+		DHD_ERROR(("%s error : firmware can't support p2p mode\n", __FUNCTION__));
+		goto done;
+	}
+#endif /* (WL_ENABLE_P2P_IF) */
+
+	if ((!op_mode && strstr(fw_path, "_p2p") != NULL)
+#if defined(WL_ENABLE_P2P_IF)
+			|| (op_mode == WFD_MASK) || (dhd_concurrent_fw(dhd) == 1)
+#endif
+		) {
+		bcm_mkiovar("apsta", (char *)&apsta, 4, iovbuf, sizeof(iovbuf));
+		if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR,
+			iovbuf, sizeof(iovbuf), TRUE, 0)) < 0) {
+			DHD_ERROR(("%s APSTA setting failed ret= %d\n", __FUNCTION__, ret));
+		} else {
+			dhd->op_mode |= WFD_MASK;
+#if !defined(WL_ENABLE_P2P_IF)
+			/* ICS back capability : disable any packet filtering for p2p only mode */
+			dhd_pkt_filter_enable = FALSE;
+#endif /*!defined(WL_ENABLE_P2P_IF) */
+		}
+	}
+#endif
+
+#if !defined(AP) && defined(WL_CFG80211)
+	/* Check if firmware with HostAPD support used */
+	if ((!op_mode && strstr(fw_path, "_apsta") != NULL) || (op_mode == HOSTAPD_MASK)) {
+			/* Disable A-band for HostAPD  */
+			uint band = WLC_BAND_2G;
+			if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_BAND, (char *)&band, sizeof(band),
+				TRUE, 0)) < 0) {
+				DHD_ERROR(("%s:set band failed error (%d)\n", __FUNCTION__, ret));
+			}
+
+			/* Turn off wme if we are having only g ONLY firmware */
+			bcm_mkiovar("nmode", 0, 0, buf, sizeof(buf));
+			if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_GET_VAR, buf, sizeof(buf),
+				FALSE, 0)) < 0) {
+				DHD_ERROR(("%s:get nmode failed error (%d)\n", __FUNCTION__, ret));
+			}
+			else {
+				DHD_TRACE(("%s:get nmode returned %d\n", __FUNCTION__,buf[0]));
+			}
+			if (buf[0] == 0) {
+				int wme = 0;
+				bcm_mkiovar("wme", (char *)&wme, 4, iovbuf, sizeof(iovbuf));
+				if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf,
+					sizeof(iovbuf), TRUE, 0)) < 0) {
+					DHD_ERROR(("%s set wme for HostAPD failed  %d\n", __FUNCTION__, ret));
+				}
+				else {
+					DHD_TRACE(("%s set wme succeeded for g ONLY firmware\n", __FUNCTION__));
+				}
+			}
+			/* Turn off MPC in AP mode */
+			bcm_mkiovar("mpc", (char *)&mpc, 4, iovbuf, sizeof(iovbuf));
+			if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf,
+				sizeof(iovbuf), TRUE, 0)) < 0) {
+				DHD_ERROR(("%s mpc for HostAPD failed  %d\n", __FUNCTION__, ret));
+			} else {
+				dhd->op_mode |= HOSTAPD_MASK;
+#if defined(ARP_OFFLOAD_SUPPORT)
+				arpoe = 0;
+#endif /* (ARP_OFFLOAD_SUPPORT) */
+				/* disable any filtering for SoftAP mode */
+				dhd_pkt_filter_enable = FALSE;
+			}
+	}
+#endif
+
+#if !defined(WL_ENABLE_P2P_IF)
+	/* ICS mode setting for sta */
+	if ((dhd->op_mode != WFD_MASK) && (dhd->op_mode != HOSTAPD_MASK)) {
+		/* STA only operation mode */
+		dhd->op_mode |= STA_MASK;
+		dhd_pkt_filter_enable = TRUE;
+	}
+#endif /* !defined(WL_ENABLE_P2P_IF) */
+
+	DHD_ERROR(("Firmware up: fw_path=%s op_mode=%d, "
+			"Broadcom Dongle Host Driver mac=%.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n",
+			fw_path,
+			dhd->op_mode,
+			dhd->mac.octet[0], dhd->mac.octet[1], dhd->mac.octet[2],
+			dhd->mac.octet[3], dhd->mac.octet[4], dhd->mac.octet[5]));
+
+	/* Set Country code  */
+	if (dhd->dhd_cspec.ccode[0] != 0) {
+		bcm_mkiovar("country", (char *)&dhd->dhd_cspec,
+			sizeof(wl_country_t), iovbuf, sizeof(iovbuf));
+		if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0)) < 0)
+			DHD_ERROR(("%s: country code setting failed\n", __FUNCTION__));
+	}
+
+	/* Set Listen Interval */
+	bcm_mkiovar("assoc_listen", (char *)&listen_interval, 4, iovbuf, sizeof(iovbuf));
+	if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0)) < 0)
+		DHD_ERROR(("%s assoc_listen failed %d\n", __FUNCTION__, ret));
+
+	/* Set PowerSave mode */
+	dhd_wl_ioctl_cmd(dhd, WLC_SET_PM, (char *)&power_mode, sizeof(power_mode), TRUE, 0);
+
+	/* Match Host and Dongle rx alignment */
+	bcm_mkiovar("bus:txglomalign", (char *)&dongle_align, 4, iovbuf, sizeof(iovbuf));
+	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
+
+	/* disable glom option for some chips */
+	chipID = (uint16)dhd_bus_chip_id(dhd);
+	if  ((chipID == BCM4330_CHIP_ID) || (chipID == BCM4329_CHIP_ID)) {
+		DHD_INFO(("%s disable glom for chipID=0x%X\n", __FUNCTION__, chipID));
+		bcm_mkiovar("bus:txglom", (char *)&glom, 4, iovbuf, sizeof(iovbuf));
+		dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
+	}
+
+	/* Setup timeout if Beacons are lost and roam is off to report link down */
+	bcm_mkiovar("bcn_timeout", (char *)&bcn_timeout, 4, iovbuf, sizeof(iovbuf));
+	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
+	/* Setup assoc_retry_max count to reconnect target AP in dongle */
+	bcm_mkiovar("assoc_retry_max", (char *)&retry_max, 4, iovbuf, sizeof(iovbuf));
+	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
+
+#if defined(AP) && !defined(WLP2P)
+	/* Turn off MPC in AP mode */
+	bcm_mkiovar("mpc", (char *)&mpc, 4, iovbuf, sizeof(iovbuf));
+	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
+	bcm_mkiovar("apsta", (char *)&apsta, 4, iovbuf, sizeof(iovbuf));
+	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
+#endif /* defined(AP) && !defined(WLP2P) */
+
+#if defined(SOFTAP)
+	if (ap_fw_loaded == TRUE) {
+		dhd_wl_ioctl_cmd(dhd, WLC_SET_DTIMPRD, (char *)&dtim, sizeof(dtim), TRUE, 0);
+	}
+#endif
+
+#if defined(KEEP_ALIVE)
+	/* Set Keep Alive : be sure to use FW with -keepalive */
+#if defined(SOFTAP)
+	if (ap_fw_loaded == FALSE)
+#endif
+		if ((res = dhd_keep_alive_onoff(dhd)) < 0)
+			DHD_ERROR(("%s set keeplive failed %d\n",
+			__FUNCTION__, res));
+#endif /* defined(KEEP_ALIVE) */
+
+	/* Read event_msgs mask */
+	bcm_mkiovar("event_msgs", eventmask, WL_EVENTING_MASK_LEN, iovbuf, sizeof(iovbuf));
+	if ((ret  = dhd_wl_ioctl_cmd(dhd, WLC_GET_VAR, iovbuf, sizeof(iovbuf), FALSE, 0)) < 0) {
+		DHD_ERROR(("%s read Event mask failed %d\n", __FUNCTION__, ret));
+		goto done;
+	}
+	bcopy(iovbuf, eventmask, WL_EVENTING_MASK_LEN);
+
+	/* Setup event_msgs */
+	setbit(eventmask, WLC_E_SET_SSID);
+	setbit(eventmask, WLC_E_PRUNE);
+	setbit(eventmask, WLC_E_AUTH);
+	setbit(eventmask, WLC_E_REASSOC);
+	setbit(eventmask, WLC_E_REASSOC_IND);
+	setbit(eventmask, WLC_E_DEAUTH);
+	setbit(eventmask, WLC_E_DEAUTH_IND);
+	setbit(eventmask, WLC_E_DISASSOC_IND);
+	setbit(eventmask, WLC_E_DISASSOC);
+	setbit(eventmask, WLC_E_JOIN);
+	setbit(eventmask, WLC_E_ASSOC_IND);
+	setbit(eventmask, WLC_E_PSK_SUP);
+	setbit(eventmask, WLC_E_LINK);
+	setbit(eventmask, WLC_E_NDIS_LINK);
+	setbit(eventmask, WLC_E_MIC_ERROR);
+	setbit(eventmask, WLC_E_ASSOC_REQ_IE);
+	setbit(eventmask, WLC_E_ASSOC_RESP_IE);
+	setbit(eventmask, WLC_E_PMKID_CACHE);
+	setbit(eventmask, WLC_E_JOIN_START);
+	setbit(eventmask, WLC_E_SCAN_COMPLETE);
+#ifdef WLMEDIA_HTSF
+	setbit(eventmask, WLC_E_HTSFSYNC);
+#endif /* WLMEDIA_HTSF */
+#ifdef PNO_SUPPORT
+	setbit(eventmask, WLC_E_PFN_NET_FOUND);
+#endif /* PNO_SUPPORT */
+	/* enable dongle roaming event */
+	setbit(eventmask, WLC_E_ROAM);
+#ifdef WL_CFG80211
+	setbit(eventmask, WLC_E_ESCAN_RESULT);
+	if ((dhd->op_mode & WFD_MASK) == WFD_MASK) {
+		setbit(eventmask, WLC_E_ACTION_FRAME_RX);
+		setbit(eventmask, WLC_E_ACTION_FRAME_COMPLETE);
+		setbit(eventmask, WLC_E_ACTION_FRAME_OFF_CHAN_COMPLETE);
+		setbit(eventmask, WLC_E_P2P_PROBREQ_MSG);
+		setbit(eventmask, WLC_E_P2P_DISC_LISTEN_COMPLETE);
+	}
+#endif /* WL_CFG80211 */
+
+	/* Write updated Event mask */
+	bcm_mkiovar("event_msgs", eventmask, WL_EVENTING_MASK_LEN, iovbuf, sizeof(iovbuf));
+	if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0)) < 0) {
+		DHD_ERROR(("%s Set Event mask failed %d\n", __FUNCTION__, ret));
+		goto done;
+	}
+
+	dhd_wl_ioctl_cmd(dhd, WLC_SET_SCAN_CHANNEL_TIME, (char *)&scan_assoc_time,
+		sizeof(scan_assoc_time), TRUE, 0);
+	dhd_wl_ioctl_cmd(dhd, WLC_SET_SCAN_UNASSOC_TIME, (char *)&scan_unassoc_time,
+		sizeof(scan_unassoc_time), TRUE, 0);
+	dhd_wl_ioctl_cmd(dhd, WLC_SET_SCAN_PASSIVE_TIME, (char *)&scan_passive_time,
+		sizeof(scan_passive_time), TRUE, 0);
+
+#ifdef ARP_OFFLOAD_SUPPORT
+	/* Set and enable ARP offload feature for STA only  */
+#if defined(SOFTAP)
+	if (arpoe && !ap_fw_loaded) {
+#else
+	if (arpoe) {
+#endif 
+		dhd_arp_offload_set(dhd, dhd_arp_mode);
+		dhd_arp_offload_enable(dhd, arpoe);
+	} else {
+		dhd_arp_offload_set(dhd, 0);
+		dhd_arp_offload_enable(dhd, FALSE);
+	}
+#endif /* ARP_OFFLOAD_SUPPORT */
+
+#ifdef PKT_FILTER_SUPPORT
+	/* Setup defintions for pktfilter , enable in suspend */
+	dhd->pktfilter_count = 5;
+	/* Setup filter to allow only unicast */
+	dhd->pktfilter[0] = "100 0 0 0 0x01 0x00";
+	dhd->pktfilter[1] = NULL;
+	dhd->pktfilter[2] = NULL;
+	dhd->pktfilter[3] = NULL;
+	/* Add filter to pass multicastDNS packet and NOT filter out as Broadcast */
+	dhd->pktfilter[4] = "104 0 0 0 0xFFFFFFFFFFFF 0x01005E0000FB";
+#if defined(SOFTAP)
+	if (ap_fw_loaded) {
+		int i;
+		for (i = 0; i < dhd->pktfilter_count; i++) {
+			dhd_pktfilter_offload_enable(dhd, dhd->pktfilter[i],
+				0, dhd_master_mode);
+		}
+	}
+#endif /* defined(SOFTAP) */
+#endif /* PKT_FILTER_SUPPORT */
+
+#if !defined(WL_CFG80211)
+	/* Force STA UP */
+	if ((ret = dhd_wl_ioctl_cmd(dhd, WLC_UP, (char *)&up, sizeof(up), TRUE, 0)) < 0) {
+		DHD_ERROR(("%s Setting WL UP failed %d\n", __FUNCTION__, ret));
+		goto done;
+	}
+#endif
+	/* query for 'ver' to get version info from firmware */
+	memset(buf, 0, sizeof(buf));
+	ptr = buf;
+	bcm_mkiovar("ver", (char *)&buf, 4, buf, sizeof(buf));
+	if ((ret  = dhd_wl_ioctl_cmd(dhd, WLC_GET_VAR, buf, sizeof(buf), FALSE, 0)) < 0)
+		DHD_ERROR(("%s failed %d\n", __FUNCTION__, ret));
+	else {
+		bcmstrtok(&ptr, "\n", 0);
+		/* Print fw version info */
+		DHD_ERROR(("Firmware version = %s\n", buf));
+
+		DHD_BLOG(buf, strlen(buf) + 1);
+		DHD_BLOG(dhd_version, strlen(dhd_version) + 1);
+
+		/* Check and adjust IOCTL response timeout for Manufactring firmware */
+		if (strstr(buf, MANUFACTRING_FW) != NULL) {
+			dhd_os_set_ioctl_resp_timeout(IOCTL_RESP_TIMEOUT * 10);
+			DHD_ERROR(("%s : adjust IOCTL response time for Manufactring Firmware\n", __FUNCTION__));
+		}
+	}
+
+done:
+	return ret;
+}
+
+
+int
+dhd_iovar(dhd_pub_t *pub, int ifidx, char *name, char *cmd_buf, uint cmd_len, int set)
+{
+	char buf[strlen(name) + 1 + cmd_len];
+	int len = sizeof(buf);
+	wl_ioctl_t ioc;
+	int ret;
+
+	len = bcm_mkiovar(name, cmd_buf, cmd_len, buf, len);
+
+	memset(&ioc, 0, sizeof(ioc));
+
+	ioc.cmd = set? WLC_SET_VAR : WLC_GET_VAR;
+	ioc.buf = buf;
+	ioc.len = len;
+	ioc.set = TRUE;
+
+	ret = dhd_wl_ioctl(pub, ifidx, &ioc, ioc.buf, ioc.len);
+	if (!set && ret >= 0)
+		memcpy(cmd_buf, buf, cmd_len);
+
+	return ret;
+}
+
+int dhd_change_mtu(dhd_pub_t *dhdp, int new_mtu, int ifidx)
+{
+	struct dhd_info *dhd = dhdp->info;
+	struct net_device *dev = NULL;
+
+	ASSERT(dhd && dhd->iflist[ifidx]);
+	dev = dhd->iflist[ifidx]->net;
+	ASSERT(dev);
+
+	if (netif_running(dev)) {
+		DHD_ERROR(("%s: Must be down to change its MTU", dev->name));
+		return BCME_NOTDOWN;
+	}
+
+#define DHD_MIN_MTU 1500
+#define DHD_MAX_MTU 1752
+
+	if ((new_mtu < DHD_MIN_MTU) || (new_mtu > DHD_MAX_MTU)) {
+		DHD_ERROR(("%s: MTU size %d is invalid.\n", __FUNCTION__, new_mtu));
+		return BCME_BADARG;
+	}
+
+	dev->mtu = new_mtu;
+	return 0;
+}
+
+#ifdef ARP_OFFLOAD_SUPPORT
+/* add or remove AOE host ip(s) (up to 8 IPs on the interface)  */
+void
+aoe_update_host_ipv4_table(dhd_pub_t *dhd_pub, u32 ipa, bool add)
+{
+	u32 ipv4_buf[MAX_IPV4_ENTRIES]; /* temp save for AOE host_ip table */
+	int i;
+	int ret;
+
+	bzero(ipv4_buf, sizeof(ipv4_buf));
+
+	/* display what we've got */
+	ret = dhd_arp_get_arp_hostip_table(dhd_pub, ipv4_buf, sizeof(ipv4_buf));
+	DHD_ARPOE(("%s: hostip table read from Dongle:\n", __FUNCTION__));
+#ifdef AOE_DBG
+	dhd_print_buf(ipv4_buf, 32, 4); /* max 8 IPs 4b each */
+#endif
+	/* now we saved hoste_ip table, clr it in the dongle AOE */
+	dhd_aoe_hostip_clr(dhd_pub);
+
+	if (ret) {
+		DHD_ERROR(("%s failed\n", __FUNCTION__));
+		return;
+	}
+
+	for (i = 0; i < MAX_IPV4_ENTRIES; i++) {
+		if (add && (ipv4_buf[i] == 0)) {
+				ipv4_buf[i] = ipa;
+				add = FALSE; /* added ipa to local table  */
+				DHD_ARPOE(("%s: Saved new IP in temp arp_hostip[%d]\n",
+				__FUNCTION__, i));
+		} else if (ipv4_buf[i] == ipa) {
+			ipv4_buf[i]	= 0;
+			DHD_ARPOE(("%s: removed IP:%x from temp table %d\n",
+				__FUNCTION__, ipa, i));
+		}
+
+		if (ipv4_buf[i] != 0) {
+			/* add back host_ip entries from our local cache */
+			dhd_arp_offload_add_ip(dhd_pub, ipv4_buf[i]);
+			DHD_ARPOE(("%s: added IP:%x to dongle arp_hostip[%d]\n\n",
+				__FUNCTION__, ipv4_buf[i], i));
+		}
+	}
+#ifdef AOE_DBG
+	/* see the resulting hostip table */
+	dhd_arp_get_arp_hostip_table(dhd_pub, ipv4_buf, sizeof(ipv4_buf));
+	DHD_ARPOE(("%s: read back arp_hostip table:\n", __FUNCTION__));
+	dhd_print_buf(ipv4_buf, 32, 4); /* max 8 IPs 4b each */
+#endif
+}
+
+static int dhd_device_event(struct notifier_block *this,
+	unsigned long event,
+	void *ptr)
+{
+	struct in_ifaddr *ifa = (struct in_ifaddr *)ptr;
+
+	dhd_info_t *dhd;
+	dhd_pub_t *dhd_pub;
+
+	if (!ifa)
+		return NOTIFY_DONE;
+
+	dhd = *(dhd_info_t **)netdev_priv(ifa->ifa_dev->dev);
+	dhd_pub = &dhd->pub;
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 31))
+	if (ifa->ifa_dev->dev->netdev_ops == &dhd_ops_pri) {
+#else
+	if (ifa->ifa_dev->dev) {
+#endif
+		switch (event) {
+		case NETDEV_UP:
+			DHD_ARPOE(("%s: [%s] Up IP: 0x%x\n",
+				__FUNCTION__, ifa->ifa_label, ifa->ifa_address));
+
+			if (dhd->pub.busstate != DHD_BUS_DATA) {
+				DHD_ERROR(("%s: bus not ready, exit\n", __FUNCTION__));
+				if (dhd->pend_ipaddr) {
+					DHD_ERROR(("%s: overwrite pending ipaddr: 0x%x\n",
+						__FUNCTION__, dhd->pend_ipaddr));
+				}
+				dhd->pend_ipaddr = ifa->ifa_address;
+				break;
+			}
+
+#ifdef AOE_IP_ALIAS_SUPPORT
+			if (ifa->ifa_label[strlen(ifa->ifa_label)-2] == 0x3a) {
+				DHD_ARPOE(("%s:add aliased IP to AOE hostip cache\n",
+					__FUNCTION__));
+				aoe_update_host_ipv4_table(dhd_pub, ifa->ifa_address, TRUE);
+			}
+			else
+				aoe_update_host_ipv4_table(dhd_pub, ifa->ifa_address, TRUE);
+#endif
+			break;
+
+		case NETDEV_DOWN:
+			DHD_ARPOE(("%s: [%s] Down IP: 0x%x\n",
+				__FUNCTION__, ifa->ifa_label, ifa->ifa_address));
+			dhd->pend_ipaddr = 0;
+#ifdef AOE_IP_ALIAS_SUPPORT
+		if (!(ifa->ifa_label[strlen(ifa->ifa_label)-2] == 0x3a)) {
+				DHD_ARPOE(("%s: primary interface is down, AOE clr all\n",
+				           __FUNCTION__));
+				dhd_aoe_hostip_clr(&dhd->pub);
+				dhd_aoe_arp_clr(&dhd->pub);
+		} else
+			aoe_update_host_ipv4_table(dhd_pub, ifa->ifa_address, FALSE);
+#else
+			dhd_aoe_hostip_clr(&dhd->pub);
+			dhd_aoe_arp_clr(&dhd->pub);
+#endif
+			break;
+
+		default:
+			DHD_ARPOE(("%s: do noting for [%s] Event: %lu\n",
+				__func__, ifa->ifa_label, event));
+			break;
+		}
+	}
+	return NOTIFY_DONE;
+}
+#endif /* ARP_OFFLOAD_SUPPORT */
+
+int
+dhd_net_attach(dhd_pub_t *dhdp, int ifidx)
+{
+	dhd_info_t *dhd = (dhd_info_t *)dhdp->info;
+	struct net_device *net = NULL;
+	int err = 0;
+	uint8 temp_addr[ETHER_ADDR_LEN] = { 0x00, 0x90, 0x4c, 0x11, 0x22, 0x33 };
+
+	DHD_TRACE(("%s: ifidx %d\n", __FUNCTION__, ifidx));
+
+	ASSERT(dhd && dhd->iflist[ifidx]);
+
+	net = dhd->iflist[ifidx]->net;
+	ASSERT(net);
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31))
+	ASSERT(!net->open);
+	net->get_stats = dhd_get_stats;
+	net->do_ioctl = dhd_ioctl_entry;
+	net->hard_start_xmit = dhd_start_xmit;
+	net->set_mac_address = dhd_set_mac_address;
+	net->set_multicast_list = dhd_set_multicast_list;
+	net->open = net->stop = NULL;
+#else
+	ASSERT(!net->netdev_ops);
+	net->netdev_ops = &dhd_ops_virt;
+#endif
+
+	/* Ok, link into the network layer... */
+	if (ifidx == 0) {
+		/*
+		 * device functions for the primary interface only
+		 */
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31))
+		net->open = dhd_open;
+		net->stop = dhd_stop;
+#else
+		net->netdev_ops = &dhd_ops_pri;
+#endif
+	} else {
+		/*
+		 * We have to use the primary MAC for virtual interfaces
+		 */
+		memcpy(temp_addr, dhd->iflist[ifidx]->mac_addr, ETHER_ADDR_LEN);
+		/*
+		 * Android sets the locally administered bit to indicate that this is a
+		 * portable hotspot.  This will not work in simultaneous AP/STA mode,
+		 * nor with P2P.  Need to set the Donlge's MAC address, and then use that.
+		 */
+		if (!memcmp(temp_addr, dhd->iflist[0]->mac_addr,
+			ETHER_ADDR_LEN)) {
+			DHD_ERROR(("%s interface [%s]: set locally administered bit in MAC\n",
+			__func__, net->name));
+			temp_addr[0] |= 0x02;
+		}
+	}
+
+	net->hard_header_len = ETH_HLEN + dhd->pub.hdrlen;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24)
+	net->ethtool_ops = &dhd_ethtool_ops;
+#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 24) */
+
+#if defined(CONFIG_BCMDHD_WEXT)
+#if WIRELESS_EXT < 19
+	net->get_wireless_stats = dhd_get_wireless_stats;
+#endif /* WIRELESS_EXT < 19 */
+#if WIRELESS_EXT > 12
+	net->wireless_handlers = (struct iw_handler_def *)&wl_iw_handler_def;
+#endif /* WIRELESS_EXT > 12 */
+#endif /* defined(CONFIG_BCMDHD_WEXT) */
+
+	dhd->pub.rxsz = DBUS_RX_BUFFER_SIZE_DHD(net);
+
+	memcpy(net->dev_addr, temp_addr, ETHER_ADDR_LEN);
+
+	if ((err = register_netdev(net)) != 0) {
+		DHD_ERROR(("couldn't register the net device, err %d\n", err));
+		goto fail;
+	}
+	printf("Broadcom Dongle Host Driver: register interface [%s]"
+		" MAC: %.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n",
+		net->name,
+		net->dev_addr[0], net->dev_addr[1], net->dev_addr[2],
+		net->dev_addr[3], net->dev_addr[4], net->dev_addr[5]);
+
+#if defined(SOFTAP) && defined(CONFIG_BCMDHD_WEXT) && !defined(WL_CFG80211)
+		wl_iw_iscan_set_scan_broadcast_prep(net, 1);
+#endif
+
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	if (ifidx == 0) {
+		up(&dhd_registration_sem);
+	}
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
+	return 0;
+
+fail:
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31)
+	net->open = NULL;
+#else
+	net->netdev_ops = NULL;
+#endif
+	return err;
+}
+
+void
+dhd_bus_detach(dhd_pub_t *dhdp)
+{
+	dhd_info_t *dhd;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (dhdp) {
+		dhd = (dhd_info_t *)dhdp->info;
+		if (dhd) {
+
+			/*
+			 * In case of Android cfg80211 driver, the bus is down in dhd_stop,
+			 * calling stop again will cuase SD read/write errors.
+			 */
+			if (dhd->pub.busstate != DHD_BUS_DOWN) {
+				/* Stop the protocol module */
+				dhd_prot_stop(&dhd->pub);
+
+				/* Stop the bus module */
+				dhd_bus_stop(dhd->pub.bus, TRUE);
+			}
+
+#if defined(OOB_INTR_ONLY)
+			bcmsdh_unregister_oob_intr();
+#endif /* defined(OOB_INTR_ONLY) */
+		}
+	}
+}
+
+
+void dhd_detach(dhd_pub_t *dhdp)
+{
+	dhd_info_t *dhd;
+	unsigned long flags;
+	int timer_valid = FALSE;
+
+	if (!dhdp)
+		return;
+
+	dhd = (dhd_info_t *)dhdp->info;
+	if (!dhd)
+		return;
+
+	DHD_TRACE(("%s: Enter state 0x%x\n", __FUNCTION__, dhd->dhd_state));
+
+	if (!(dhd->dhd_state & DHD_ATTACH_STATE_DONE)) {
+		/* Give sufficient time for threads to start running in case
+		 * dhd_attach() has failed
+		 */
+		osl_delay(1000*100);
+	}
+
+#ifdef ARP_OFFLOAD_SUPPORT
+	unregister_inetaddr_notifier(&dhd_notifier);
+#endif /* ARP_OFFLOAD_SUPPORT */
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) && defined(DHD_USE_EARLYSUSPEND)
+	if (dhd->dhd_state & DHD_ATTACH_STATE_EARLYSUSPEND_DONE) {
+		if (dhd->early_suspend.suspend)
+			unregister_early_suspend(&dhd->early_suspend);
+	}
+#endif /* defined(CONFIG_HAS_EARLYSUSPEND) */
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	dhd->pub.hang_was_sent = 1;
+	cancel_work_sync(&dhd->work_hang);
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))  */
+
+#if defined(CONFIG_BCMDHD_WEXT)
+	if (dhd->dhd_state & DHD_ATTACH_STATE_WL_ATTACH) {
+		/* Detatch and unlink in the iw */
+		wl_iw_detach();
+	}
+#endif /* defined(CONFIG_BCMDHD_WEXT) */
+
+	if (dhd->thr_sysioc_ctl.thr_pid >= 0) {
+		PROC_STOP(&dhd->thr_sysioc_ctl);
+	}
+
+	/* delete all interfaces, start with virtual  */
+	if (dhd->dhd_state & DHD_ATTACH_STATE_ADD_IF) {
+		int i = 1;
+		dhd_if_t *ifp;
+
+		/* Cleanup virtual interfaces */
+		for (i = 1; i < DHD_MAX_IFS; i++) {
+			dhd_net_if_lock_local(dhd);
+			if (dhd->iflist[i]) {
+				dhd->iflist[i]->state = DHD_IF_DEL;
+				dhd->iflist[i]->idx = i;
+				dhd_op_if(dhd->iflist[i]);
+			}
+			dhd_net_if_unlock_local(dhd);
+		}
+		/*  delete primary interface 0 */
+		ifp = dhd->iflist[0];
+		ASSERT(ifp);
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31))
+		if (ifp->net->open)
+#else
+		if (ifp->net->netdev_ops == &dhd_ops_pri)
+#endif
+		{
+			if (ifp->net) {
+				unregister_netdev(ifp->net);
+				free_netdev(ifp->net);
+				ifp->net = NULL;
+			}
+			MFREE(dhd->pub.osh, ifp, sizeof(*ifp));
+			dhd->iflist[0] = NULL;
+		}
+	}
+
+	/* Clear the watchdog timer */
+	flags = dhd_os_spin_lock(&dhd->pub);
+	timer_valid = dhd->wd_timer_valid;
+	dhd->wd_timer_valid = FALSE;
+	dhd_os_spin_unlock(&dhd->pub, flags);
+	if (timer_valid)
+		del_timer_sync(&dhd->timer);
+
+	if (dhd->dhd_state & DHD_ATTACH_STATE_THREADS_CREATED) {
+#ifdef DHDTHREAD
+		if (dhd->thr_wdt_ctl.thr_pid >= 0) {
+			PROC_STOP(&dhd->thr_wdt_ctl);
+		}
+
+		if (dhd->thr_dpc_ctl.thr_pid >= 0) {
+			PROC_STOP(&dhd->thr_dpc_ctl);
+		}
+		else
+#endif /* DHDTHREAD */
+		tasklet_kill(&dhd->tasklet);
+	}
+	if (dhd->dhd_state & DHD_ATTACH_STATE_PROT_ATTACH) {
+		dhd_bus_detach(dhdp);
+
+		if (dhdp->prot)
+			dhd_prot_detach(dhdp);
+	}
+
+#ifdef WL_CFG80211
+	if (dhd->dhd_state & DHD_ATTACH_STATE_CFG80211) {
+		wl_cfg80211_detach(NULL);
+		dhd_monitor_uninit();
+	}
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP)
+		unregister_pm_notifier(&dhd_sleep_pm_notifier);
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_PM_SLEEP) */
+
+	if (dhd->dhd_state & DHD_ATTACH_STATE_WAKELOCKS_INIT) {
+#ifdef CONFIG_HAS_WAKELOCK
+		wake_lock_destroy(&dhd->wl_wifi);
+		wake_lock_destroy(&dhd->wl_rxwake);
+		wake_lock_destroy(&dhd->wl_ctrlwake);
+#endif
+	}
+}
+
+
+void
+dhd_free(dhd_pub_t *dhdp)
+{
+	dhd_info_t *dhd;
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (dhdp) {
+		dhd = (dhd_info_t *)dhdp->info;
+		if (dhd)
+			MFREE(dhd->pub.osh, dhd, sizeof(*dhd));
+	}
+}
+
+static void __exit
+dhd_module_cleanup(void)
+{
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	dhd_bus_unregister();
+
+#if defined(CONFIG_WIFI_CONTROL_FUNC)
+	wl_android_wifictrl_func_del();
+#endif /* CONFIG_WIFI_CONTROL_FUNC */
+	wl_android_exit();
+
+	/* Call customer gpio to turn off power with WL_REG_ON signal */
+	dhd_customer_gpio_wlan_ctrl(WLAN_POWER_OFF);
+}
+
+static int __init
+dhd_module_init(void)
+{
+	int error = 0;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	wl_android_init();
+
+#ifdef DHDTHREAD
+	/* Sanity check on the module parameters */
+	do {
+		/* Both watchdog and DPC as tasklets are ok */
+		if ((dhd_watchdog_prio < 0) && (dhd_dpc_prio < 0))
+			break;
+
+		/* If both watchdog and DPC are threads, TX must be deferred */
+		if ((dhd_watchdog_prio >= 0) && (dhd_dpc_prio >= 0) && dhd_deferred_tx)
+			break;
+
+		DHD_ERROR(("Invalid module parameters.\n"));
+		return -EINVAL;
+	} while (0);
+#endif /* DHDTHREAD */
+
+	/* Call customer gpio to turn on power with WL_REG_ON signal */
+	dhd_customer_gpio_wlan_ctrl(WLAN_POWER_ON);
+
+#if defined(CONFIG_WIFI_CONTROL_FUNC)
+	if (wl_android_wifictrl_func_add() < 0)
+		goto fail_1;
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+		sema_init(&dhd_registration_sem, 0);
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) */
+	error = dhd_bus_register();
+
+	if (!error)
+		printf("\n%s\n", dhd_version);
+	else {
+		DHD_ERROR(("%s: sdio_register_driver failed\n", __FUNCTION__));
+		goto fail_1;
+	}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	/*
+	 * Wait till MMC sdio_register_driver callback called and made driver attach.
+	 * It's needed to make sync up exit from dhd insmod  and
+	 * Kernel MMC sdio device callback registration
+	 */
+	if (down_timeout(&dhd_registration_sem,  msecs_to_jiffies(DHD_REGISTRATION_TIMEOUT)) != 0) {
+		error = -ENODEV;
+		DHD_ERROR(("%s: sdio_register_driver timeout\n", __FUNCTION__));
+		goto fail_2;
+	}
+#endif
+#if defined(WL_CFG80211)
+	wl_android_post_init();
+#endif /* defined(WL_CFG80211) */
+
+	return error;
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+fail_2:
+	dhd_bus_unregister();
+#endif
+fail_1:
+#if defined(CONFIG_WIFI_CONTROL_FUNC)
+	wl_android_wifictrl_func_del();
+#endif 
+
+	/* Call customer gpio to turn off power with WL_REG_ON signal */
+	dhd_customer_gpio_wlan_ctrl(WLAN_POWER_OFF);
+
+	return error;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0)
+late_initcall(dhd_module_init);
+#else
+module_init(dhd_module_init);
+#endif
+module_exit(dhd_module_cleanup);
+
+/*
+ * OS specific functions required to implement DHD driver in OS independent way
+ */
+int
+dhd_os_proto_block(dhd_pub_t *pub)
+{
+	dhd_info_t * dhd = (dhd_info_t *)(pub->info);
+
+	if (dhd) {
+		down(&dhd->proto_sem);
+		return 1;
+	}
+
+	return 0;
+}
+
+int
+dhd_os_proto_unblock(dhd_pub_t *pub)
+{
+	dhd_info_t * dhd = (dhd_info_t *)(pub->info);
+
+	if (dhd) {
+		up(&dhd->proto_sem);
+		return 1;
+	}
+
+	return 0;
+}
+
+unsigned int
+dhd_os_get_ioctl_resp_timeout(void)
+{
+	return ((unsigned int)dhd_ioctl_timeout_msec);
+}
+
+void
+dhd_os_set_ioctl_resp_timeout(unsigned int timeout_msec)
+{
+	dhd_ioctl_timeout_msec = (int)timeout_msec;
+}
+
+int
+dhd_os_ioctl_resp_wait(dhd_pub_t *pub, uint *condition, bool *pending)
+{
+	dhd_info_t * dhd = (dhd_info_t *)(pub->info);
+	int timeout = dhd_ioctl_timeout_msec;
+
+	/* Convert timeout in millsecond to jiffies */
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	timeout = msecs_to_jiffies(timeout);
+#else
+	timeout = timeout * HZ / 1000;
+#endif
+
+	timeout = wait_event_timeout(dhd->ioctl_resp_wait, (*condition), timeout);
+	return timeout;
+}
+
+int
+dhd_os_ioctl_resp_wake(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+
+	if (waitqueue_active(&dhd->ioctl_resp_wait)) {
+		wake_up(&dhd->ioctl_resp_wait);
+	}
+
+	return 0;
+}
+
+void
+dhd_os_wd_timer(void *bus, uint wdtick)
+{
+	dhd_pub_t *pub = bus;
+	dhd_info_t *dhd = (dhd_info_t *)pub->info;
+	unsigned long flags;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	flags = dhd_os_spin_lock(pub);
+
+	/* don't start the wd until fw is loaded */
+	if (pub->busstate == DHD_BUS_DOWN) {
+		dhd_os_spin_unlock(pub, flags);
+		return;
+	}
+
+	/* Totally stop the timer */
+	if (!wdtick && dhd->wd_timer_valid == TRUE) {
+		dhd->wd_timer_valid = FALSE;
+		dhd_os_spin_unlock(pub, flags);
+#ifdef DHDTHREAD
+		del_timer_sync(&dhd->timer);
+#else
+		del_timer(&dhd->timer);
+#endif /* DHDTHREAD */
+		return;
+	}
+
+	if (wdtick) {
+		dhd_watchdog_ms = (uint)wdtick;
+		/* Re arm the timer, at last watchdog period */
+		mod_timer(&dhd->timer, jiffies + dhd_watchdog_ms * HZ / 1000);
+		dhd->wd_timer_valid = TRUE;
+	}
+	dhd_os_spin_unlock(pub, flags);
+}
+
+void *
+dhd_os_open_image(char *filename)
+{
+	struct file *fp;
+
+	fp = filp_open(filename, O_RDONLY, 0);
+	/*
+	 * 2.6.11 (FC4) supports filp_open() but later revs don't?
+	 * Alternative:
+	 * fp = open_namei(AT_FDCWD, filename, O_RD, 0);
+	 * ???
+	 */
+	 if (IS_ERR(fp))
+		 fp = NULL;
+
+	 return fp;
+}
+
+int
+dhd_os_get_image_block(char *buf, int len, void *image)
+{
+	struct file *fp = (struct file *)image;
+	int rdlen;
+
+	if (!image)
+		return 0;
+
+	rdlen = kernel_read(fp, fp->f_pos, buf, len);
+	if (rdlen > 0)
+		fp->f_pos += rdlen;
+
+	return rdlen;
+}
+
+void
+dhd_os_close_image(void *image)
+{
+	if (image)
+		filp_close((struct file *)image, NULL);
+}
+
+
+void
+dhd_os_sdlock(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd;
+
+	dhd = (dhd_info_t *)(pub->info);
+
+#ifdef DHDTHREAD
+	if (dhd->threads_only)
+		down(&dhd->sdsem);
+	else
+#endif /* DHDTHREAD */
+	spin_lock_bh(&dhd->sdlock);
+}
+
+void
+dhd_os_sdunlock(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd;
+
+	dhd = (dhd_info_t *)(pub->info);
+
+#ifdef DHDTHREAD
+	if (dhd->threads_only)
+		up(&dhd->sdsem);
+	else
+#endif /* DHDTHREAD */
+	spin_unlock_bh(&dhd->sdlock);
+}
+
+void
+dhd_os_sdlock_txq(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd;
+
+	dhd = (dhd_info_t *)(pub->info);
+	spin_lock_bh(&dhd->txqlock);
+}
+
+void
+dhd_os_sdunlock_txq(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd;
+
+	dhd = (dhd_info_t *)(pub->info);
+	spin_unlock_bh(&dhd->txqlock);
+}
+
+void
+dhd_os_sdlock_rxq(dhd_pub_t *pub)
+{
+}
+
+void
+dhd_os_sdunlock_rxq(dhd_pub_t *pub)
+{
+}
+
+void
+dhd_os_sdtxlock(dhd_pub_t *pub)
+{
+	dhd_os_sdlock(pub);
+}
+
+void
+dhd_os_sdtxunlock(dhd_pub_t *pub)
+{
+	dhd_os_sdunlock(pub);
+}
+
+#if defined(CONFIG_DHD_USE_STATIC_BUF)
+uint8* dhd_os_prealloc(void *osh, int section, uint size)
+{
+	return (uint8*)wl_android_prealloc(section, size);
+}
+
+void dhd_os_prefree(void *osh, void *addr, uint size)
+{
+}
+#endif /* defined(CONFIG_DHD_USE_STATIC_BUF) */
+
+#if defined(CONFIG_BCMDHD_WEXT)
+struct iw_statistics *
+dhd_get_wireless_stats(struct net_device *dev)
+{
+	int res = 0;
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	if (!dhd->pub.up) {
+		return NULL;
+	}
+
+	res = wl_iw_get_wireless_stats(dev, &dhd->iw.wstats);
+
+	if (res == 0)
+		return &dhd->iw.wstats;
+	else
+		return NULL;
+}
+#endif /* defined(CONFIG_BCMDHD_WEXT) */
+
+static int
+dhd_wl_host_event(dhd_info_t *dhd, int *ifidx, void *pktdata,
+	wl_event_msg_t *event, void **data)
+{
+	int bcmerror = 0;
+	ASSERT(dhd != NULL);
+
+	bcmerror = wl_host_event(&dhd->pub, ifidx, pktdata, event, data);
+	if (bcmerror != BCME_OK)
+		return (bcmerror);
+
+#if defined(CONFIG_BCMDHD_WEXT)
+	if (event->bsscfgidx == 0) {
+		/*
+		 * Wireless ext is on primary interface only
+		 */
+
+		ASSERT(dhd->iflist[*ifidx] != NULL);
+		ASSERT(dhd->iflist[*ifidx]->net != NULL);
+
+		if (dhd->iflist[*ifidx]->net) {
+			wl_iw_event(dhd->iflist[*ifidx]->net, event, *data);
+		}
+	}
+#endif /* defined(CONFIG_BCMDHD_WEXT)  */
+
+#ifdef WL_CFG80211
+	if ((ntoh32(event->event_type) == WLC_E_IF) &&
+		(((dhd_if_event_t *)*data)->action == WLC_E_IF_ADD))
+		/* If ADD_IF has been called directly by wl utility then we
+		* should not report this. In case if ADD_IF was called from
+		* CFG stack, then too this event need not be reported back
+		*/
+               return (BCME_OK);
+	if ((wl_cfg80211_is_progress_ifchange() ||
+		wl_cfg80211_is_progress_ifadd()) && (*ifidx != 0)) {
+		/*
+		 * If IF_ADD/CHANGE operation is going on,
+		 *  discard any event received on the virtual I/F
+		 */
+		return (BCME_OK);
+	}
+
+	ASSERT(dhd->iflist[*ifidx] != NULL);
+	ASSERT(dhd->iflist[*ifidx]->net != NULL);
+	if (dhd->iflist[*ifidx]->net) {
+		wl_cfg80211_event(dhd->iflist[*ifidx]->net, event, *data);
+	}
+#endif /* defined(WL_CFG80211) */
+
+	return (bcmerror);
+}
+
+/* send up locally generated event */
+void
+dhd_sendup_event(dhd_pub_t *dhdp, wl_event_msg_t *event, void *data)
+{
+	switch (ntoh32(event->event_type)) {
+	/* Send up locally generated AMP HCI Events */
+	case WLC_E_BTA_HCI_EVENT: {
+		struct sk_buff *p, *skb;
+		bcm_event_t *msg;
+		wl_event_msg_t *p_bcm_event;
+		char *ptr;
+		uint32 len;
+		uint32 pktlen;
+		dhd_if_t *ifp;
+		dhd_info_t *dhd;
+		uchar *eth;
+		int ifidx;
+
+		len = ntoh32(event->datalen);
+		pktlen = sizeof(bcm_event_t) + len + 2;
+		dhd = dhdp->info;
+		ifidx = dhd_ifname2idx(dhd, event->ifname);
+
+		if ((p = PKTGET(dhdp->osh, pktlen, FALSE))) {
+			ASSERT(ISALIGNED((uintptr)PKTDATA(dhdp->osh, p), sizeof(uint32)));
+
+			msg = (bcm_event_t *) PKTDATA(dhdp->osh, p);
+
+			bcopy(&dhdp->mac, &msg->eth.ether_dhost, ETHER_ADDR_LEN);
+			bcopy(&dhdp->mac, &msg->eth.ether_shost, ETHER_ADDR_LEN);
+			ETHER_TOGGLE_LOCALADDR(&msg->eth.ether_shost);
+
+			msg->eth.ether_type = hton16(ETHER_TYPE_BRCM);
+
+			/* BCM Vendor specific header... */
+			msg->bcm_hdr.subtype = hton16(BCMILCP_SUBTYPE_VENDOR_LONG);
+			msg->bcm_hdr.version = BCMILCP_BCM_SUBTYPEHDR_VERSION;
+			bcopy(BRCM_OUI, &msg->bcm_hdr.oui[0], DOT11_OUI_LEN);
+
+			/* vendor spec header length + pvt data length (private indication
+			 *  hdr + actual message itself)
+			 */
+			msg->bcm_hdr.length = hton16(BCMILCP_BCM_SUBTYPEHDR_MINLENGTH +
+				BCM_MSG_LEN + sizeof(wl_event_msg_t) + (uint16)len);
+			msg->bcm_hdr.usr_subtype = hton16(BCMILCP_BCM_SUBTYPE_EVENT);
+
+			PKTSETLEN(dhdp->osh, p, (sizeof(bcm_event_t) + len + 2));
+
+			/* copy  wl_event_msg_t into sk_buf */
+
+			/* pointer to wl_event_msg_t in sk_buf */
+			p_bcm_event = &msg->event;
+			bcopy(event, p_bcm_event, sizeof(wl_event_msg_t));
+
+			/* copy hci event into sk_buf */
+			bcopy(data, (p_bcm_event + 1), len);
+
+			msg->bcm_hdr.length  = hton16(sizeof(wl_event_msg_t) +
+				ntoh16(msg->bcm_hdr.length));
+			PKTSETLEN(dhdp->osh, p, (sizeof(bcm_event_t) + len + 2));
+
+			ptr = (char *)(msg + 1);
+			/* Last 2 bytes of the message are 0x00 0x00 to signal that there
+			 * are no ethertypes which are following this
+			 */
+			ptr[len+0] = 0x00;
+			ptr[len+1] = 0x00;
+
+			skb = PKTTONATIVE(dhdp->osh, p);
+			eth = skb->data;
+			len = skb->len;
+
+			ifp = dhd->iflist[ifidx];
+			if (ifp == NULL)
+			     ifp = dhd->iflist[0];
+
+			ASSERT(ifp);
+			skb->dev = ifp->net;
+			skb->protocol = eth_type_trans(skb, skb->dev);
+
+			skb->data = eth;
+			skb->len = len;
+
+			/* Strip header, count, deliver upward */
+			skb_pull(skb, ETH_HLEN);
+
+			/* Send the packet */
+			if (in_interrupt()) {
+				netif_rx(skb);
+			} else {
+				netif_rx_ni(skb);
+			}
+		}
+		else {
+			/* Could not allocate a sk_buf */
+			DHD_ERROR(("%s: unable to alloc sk_buf", __FUNCTION__));
+		}
+		break;
+	} /* case WLC_E_BTA_HCI_EVENT */
+
+	default:
+		break;
+	}
+}
+
+void dhd_wait_for_event(dhd_pub_t *dhd, bool *lockvar)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+	struct dhd_info *dhdinfo =  dhd->info;
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	int timeout = msecs_to_jiffies(2000);
+#else
+	int timeout = 2 * HZ;
+#endif
+	dhd_os_sdunlock(dhd);
+	wait_event_timeout(dhdinfo->ctrl_wait, (*lockvar == FALSE), timeout);
+	dhd_os_sdlock(dhd);
+#endif
+	return;
+}
+
+void dhd_wait_event_wakeup(dhd_pub_t *dhd)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+	struct dhd_info *dhdinfo =  dhd->info;
+	if (waitqueue_active(&dhdinfo->ctrl_wait))
+		wake_up(&dhdinfo->ctrl_wait);
+#endif
+	return;
+}
+
+int
+dhd_dev_reset(struct net_device *dev, uint8 flag)
+{
+	int ret;
+
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	ret = dhd_bus_devreset(&dhd->pub, flag);
+	if (ret) {
+		DHD_ERROR(("%s: dhd_bus_devreset: %d\n", __FUNCTION__, ret));
+		return ret;
+	}
+
+	return ret;
+}
+
+int net_os_set_suspend_disable(struct net_device *dev, int val)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ret = 0;
+
+	if (dhd) {
+		ret = dhd->pub.suspend_disable_flag;
+		dhd->pub.suspend_disable_flag = val;
+	}
+	return ret;
+}
+
+int net_os_set_suspend(struct net_device *dev, int val, int force)
+{
+	int ret = 0;
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	if (dhd) {
+#if defined(CONFIG_HAS_EARLYSUSPEND) && defined(DHD_USE_EARLYSUSPEND)
+		ret = dhd_set_suspend(val, &dhd->pub);
+#else
+		ret = dhd_suspend_resume_helper(dhd, val, force);
+#endif
+	}
+	return ret;
+}
+
+int net_os_set_dtim_skip(struct net_device *dev, int val)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	if (dhd)
+		dhd->pub.dtim_skip = val;
+
+	return 0;
+}
+
+int net_os_rxfilter_add_remove(struct net_device *dev, int add_remove, int num)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	char *filterp = NULL;
+	int ret = 0;
+
+	if (!dhd || (num == DHD_UNICAST_FILTER_NUM) ||
+	    (num == DHD_MDNS_FILTER_NUM))
+		return ret;
+	if (num >= dhd->pub.pktfilter_count)
+		return -EINVAL;
+	if (add_remove) {
+		switch (num) {
+		case DHD_BROADCAST_FILTER_NUM:
+			filterp = "101 0 0 0 0xFFFFFFFFFFFF 0xFFFFFFFFFFFF";
+			break;
+		case DHD_MULTICAST4_FILTER_NUM:
+			filterp = "102 0 0 0 0xFFFFFF 0x01005E";
+			break;
+		case DHD_MULTICAST6_FILTER_NUM:
+			filterp = "103 0 0 0 0xFFFF 0x3333";
+			break;
+		default:
+			return -EINVAL;
+		}
+	}
+	dhd->pub.pktfilter[num] = filterp;
+	return ret;
+}
+
+int dhd_os_set_packet_filter(dhd_pub_t *dhdp, int val)
+{
+	int ret = 0;
+
+	/* Packet filtering is set only if we still in early-suspend and
+	 * we need either to turn it ON or turn it OFF
+	 * We can always turn it OFF in case of early-suspend, but we turn it
+	 * back ON only if suspend_disable_flag was not set
+	*/
+	if (dhdp && dhdp->up) {
+		if (dhdp->in_suspend) {
+			if (!val || (val && !dhdp->suspend_disable_flag))
+				dhd_set_packet_filter(val, dhdp);
+		}
+	}
+	return ret;
+
+}
+
+int net_os_set_packet_filter(struct net_device *dev, int val)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	return dhd_os_set_packet_filter(&dhd->pub, val);
+}
+
+int
+dhd_dev_init_ioctl(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	return dhd_preinit_ioctls(&dhd->pub);
+}
+
+#ifdef PNO_SUPPORT
+/* Linux wrapper to call common dhd_pno_clean */
+int
+dhd_dev_pno_reset(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	return (dhd_pno_clean(&dhd->pub));
+}
+
+
+/* Linux wrapper to call common dhd_pno_enable */
+int
+dhd_dev_pno_enable(struct net_device *dev,  int pfn_enabled)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	return (dhd_pno_enable(&dhd->pub, pfn_enabled));
+}
+
+
+/* Linux wrapper to call common dhd_pno_set */
+int
+dhd_dev_pno_set(struct net_device *dev, wlc_ssid_t* ssids_local, int nssid,
+	ushort  scan_fr, int pno_repeat, int pno_freq_expo_max)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	return (dhd_pno_set(&dhd->pub, ssids_local, nssid, scan_fr, pno_repeat, pno_freq_expo_max));
+}
+
+/* Linux wrapper to call common dhd_pno_set_ex */
+int
+dhd_dev_pno_set_ex(struct net_device *dev, wl_pfn_t* ssidnet, int nssid,
+	ushort  pno_interval, int pno_repeat, int pno_expo_max, int pno_lost_time)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	return (dhd_pno_set_ex(&dhd->pub, ssidnet, nssid,
+					pno_interval, pno_repeat, pno_expo_max, pno_lost_time));
+}
+
+/* Linux wrapper to get  pno status */
+int
+dhd_dev_get_pno_status(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	return (dhd_pno_get_status(&dhd->pub));
+}
+
+#endif /* PNO_SUPPORT */
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+static void dhd_hang_process(struct work_struct *work)
+{
+	dhd_info_t *dhd;
+	struct net_device *dev;
+
+	dhd = (dhd_info_t *)container_of(work, dhd_info_t, work_hang);
+	dev = dhd->iflist[0]->net;
+
+	if (dev) {
+		rtnl_lock();
+		dev_close(dev);
+		rtnl_unlock();
+#if defined(WL_WIRELESS_EXT)
+		wl_iw_send_priv_event(dev, "HANG");
+#endif
+#if defined(WL_CFG80211)
+		wl_cfg80211_hang(dev, WLAN_REASON_UNSPECIFIED);
+#endif
+	}
+}
+
+int net_os_send_hang_message(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ret = 0;
+
+	if (dhd) {
+		if (!dhd->pub.hang_was_sent) {
+			dhd->pub.hang_was_sent = 1;
+			schedule_work(&dhd->work_hang);
+		}
+	}
+	return ret;
+}
+#endif
+
+void dhd_bus_country_set(struct net_device *dev, wl_country_t *cspec)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+
+	if (dhd && dhd->pub.up)
+		memcpy(&dhd->pub.dhd_cspec, cspec, sizeof(wl_country_t));
+}
+
+void dhd_net_if_lock(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	dhd_net_if_lock_local(dhd);
+}
+
+void dhd_net_if_unlock(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	dhd_net_if_unlock_local(dhd);
+}
+
+static void dhd_net_if_lock_local(dhd_info_t *dhd)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
+	if (dhd)
+		mutex_lock(&dhd->dhd_net_if_mutex);
+#endif
+}
+
+static void dhd_net_if_unlock_local(dhd_info_t *dhd)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
+	if (dhd)
+		mutex_unlock(&dhd->dhd_net_if_mutex);
+#endif
+}
+
+static void dhd_suspend_lock(dhd_pub_t *pub)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+	if (dhd)
+		mutex_lock(&dhd->dhd_suspend_mutex);
+#endif
+}
+
+static void dhd_suspend_unlock(dhd_pub_t *pub)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+	if (dhd)
+		mutex_unlock(&dhd->dhd_suspend_mutex);
+#endif
+}
+
+unsigned long dhd_os_spin_lock(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+	unsigned long flags = 0;
+
+	if (dhd)
+		spin_lock_irqsave(&dhd->dhd_lock, flags);
+
+	return flags;
+}
+
+void dhd_os_spin_unlock(dhd_pub_t *pub, unsigned long flags)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+
+	if (dhd)
+		spin_unlock_irqrestore(&dhd->dhd_lock, flags);
+}
+
+static int
+dhd_get_pend_8021x_cnt(dhd_info_t *dhd)
+{
+	return (atomic_read(&dhd->pend_8021x_cnt));
+}
+
+#define MAX_WAIT_FOR_8021X_TX	10
+
+int
+dhd_wait_pend8021x(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int timeout = 10 * HZ / 1000;
+	int ntimes = MAX_WAIT_FOR_8021X_TX;
+	int pend = dhd_get_pend_8021x_cnt(dhd);
+
+	while (ntimes && pend) {
+		if (pend) {
+			set_current_state(TASK_INTERRUPTIBLE);
+			schedule_timeout(timeout);
+			set_current_state(TASK_RUNNING);
+			ntimes--;
+		}
+		pend = dhd_get_pend_8021x_cnt(dhd);
+	}
+	return pend;
+}
+
+#ifdef DHD_DEBUG
+int
+write_to_file(dhd_pub_t *dhd, uint8 *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) {
+		printf("%s: open file error\n", __FUNCTION__);
+		ret = -1;
+		goto exit;
+	}
+
+	/* Write buf to file */
+	fp->f_op->write(fp, buf, size, &pos);
+
+exit:
+	/* free buf before return */
+	MFREE(dhd->osh, buf, size);
+	/* close file before return */
+	if (fp)
+		filp_close(fp, current->files);
+	/* restore previous address limit */
+	set_fs(old_fs);
+
+	return ret;
+}
+#endif /* DHD_DEBUG */
+
+int dhd_os_wake_lock_timeout(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+	unsigned long flags;
+	int ret = 0;
+
+	if (dhd) {
+		spin_lock_irqsave(&dhd->wakelock_spinlock, flags);
+		ret = dhd->wakelock_rx_timeout_enable > dhd->wakelock_ctrl_timeout_enable ?
+			dhd->wakelock_rx_timeout_enable : dhd->wakelock_ctrl_timeout_enable;
+#ifdef CONFIG_HAS_WAKELOCK
+		if (dhd->wakelock_rx_timeout_enable)
+			wake_lock_timeout(&dhd->wl_rxwake,
+				msecs_to_jiffies(dhd->wakelock_rx_timeout_enable));
+		if (dhd->wakelock_ctrl_timeout_enable)
+			wake_lock_timeout(&dhd->wl_ctrlwake,
+				msecs_to_jiffies(dhd->wakelock_ctrl_timeout_enable));
+#endif
+		dhd->wakelock_rx_timeout_enable = 0;
+		dhd->wakelock_ctrl_timeout_enable = 0;
+		spin_unlock_irqrestore(&dhd->wakelock_spinlock, flags);
+	}
+	return ret;
+}
+
+int net_os_wake_lock_timeout(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ret = 0;
+
+	if (dhd)
+		ret = dhd_os_wake_lock_timeout(&dhd->pub);
+	return ret;
+}
+
+int dhd_os_wake_lock_rx_timeout_enable(dhd_pub_t *pub, int val)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+	unsigned long flags;
+
+	if (dhd) {
+		spin_lock_irqsave(&dhd->wakelock_spinlock, flags);
+		if (val > dhd->wakelock_rx_timeout_enable)
+			dhd->wakelock_rx_timeout_enable = val;
+		spin_unlock_irqrestore(&dhd->wakelock_spinlock, flags);
+	}
+	return 0;
+}
+
+int dhd_os_wake_lock_ctrl_timeout_enable(dhd_pub_t *pub, int val)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+	unsigned long flags;
+
+	if (dhd) {
+		spin_lock_irqsave(&dhd->wakelock_spinlock, flags);
+		if (val > dhd->wakelock_ctrl_timeout_enable)
+			dhd->wakelock_ctrl_timeout_enable = val;
+		spin_unlock_irqrestore(&dhd->wakelock_spinlock, flags);
+	}
+	return 0;
+}
+
+int net_os_wake_lock_rx_timeout_enable(struct net_device *dev, int val)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ret = 0;
+
+	if (dhd)
+		ret = dhd_os_wake_lock_rx_timeout_enable(&dhd->pub, val);
+	return ret;
+}
+
+int net_os_wake_lock_ctrl_timeout_enable(struct net_device *dev, int val)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ret = 0;
+
+	if (dhd)
+		ret = dhd_os_wake_lock_ctrl_timeout_enable(&dhd->pub, val);
+	return ret;
+}
+
+int dhd_os_wake_lock(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+	unsigned long flags;
+	int ret = 0;
+
+	if (dhd) {
+		spin_lock_irqsave(&dhd->wakelock_spinlock, flags);
+#ifdef CONFIG_HAS_WAKELOCK
+		if (!dhd->wakelock_counter)
+			wake_lock(&dhd->wl_wifi);
+#endif
+		dhd->wakelock_counter++;
+		ret = dhd->wakelock_counter;
+		spin_unlock_irqrestore(&dhd->wakelock_spinlock, flags);
+	}
+	return ret;
+}
+
+int net_os_wake_lock(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ret = 0;
+
+	if (dhd)
+		ret = dhd_os_wake_lock(&dhd->pub);
+	return ret;
+}
+
+int dhd_os_wake_unlock(dhd_pub_t *pub)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(pub->info);
+	unsigned long flags;
+	int ret = 0;
+
+	dhd_os_wake_lock_timeout(pub);
+	if (dhd) {
+		spin_lock_irqsave(&dhd->wakelock_spinlock, flags);
+		if (dhd->wakelock_counter) {
+			dhd->wakelock_counter--;
+#ifdef CONFIG_HAS_WAKELOCK
+			if (!dhd->wakelock_counter)
+				wake_unlock(&dhd->wl_wifi);
+#endif
+			ret = dhd->wakelock_counter;
+		}
+		spin_unlock_irqrestore(&dhd->wakelock_spinlock, flags);
+	}
+	return ret;
+}
+
+int dhd_os_check_wakelock(void *dhdp)
+{
+#ifdef CONFIG_HAS_WAKELOCK
+	dhd_pub_t *pub = (dhd_pub_t *)dhdp;
+	dhd_info_t *dhd;
+
+	if (!pub)
+		return 0;
+	dhd = (dhd_info_t *)(pub->info);
+
+	if (dhd && wake_lock_active(&dhd->wl_wifi))
+		return 1;
+#endif
+	return 0;
+}
+
+int net_os_wake_unlock(struct net_device *dev)
+{
+	dhd_info_t *dhd = *(dhd_info_t **)netdev_priv(dev);
+	int ret = 0;
+
+	if (dhd)
+		ret = dhd_os_wake_unlock(&dhd->pub);
+	return ret;
+}
+
+int dhd_os_check_if_up(void *dhdp)
+{
+	dhd_pub_t *pub = (dhd_pub_t *)dhdp;
+
+	if (!pub)
+		return 0;
+	return pub->up;
+}
+
+int dhd_ioctl_entry_local(struct net_device *net, wl_ioctl_t *ioc, int cmd)
+{
+	int ifidx;
+	int ret = 0;
+	dhd_info_t *dhd = NULL;
+
+	if (!net || !netdev_priv(net)) {
+		DHD_ERROR(("%s invalid parameter\n", __FUNCTION__));
+		return -EINVAL;
+	}
+
+	dhd = *(dhd_info_t **)netdev_priv(net);
+	ifidx = dhd_net2idx(dhd, net);
+	if (ifidx == DHD_BAD_IF) {
+		DHD_ERROR(("%s bad ifidx\n", __FUNCTION__));
+		return -ENODEV;
+	}
+
+	DHD_OS_WAKE_LOCK(&dhd->pub);
+	ret = dhd_wl_ioctl(&dhd->pub, ifidx, ioc, ioc->buf, ioc->len);
+	dhd_check_hang(net, &dhd->pub, ret);
+	DHD_OS_WAKE_UNLOCK(&dhd->pub);
+
+	return ret;
+}
+
+bool dhd_os_check_hang(dhd_pub_t *dhdp, int ifidx, int ret)
+{
+	struct net_device *net;
+
+	net = dhd_idx2net(dhdp, ifidx);
+	return dhd_check_hang(net, dhdp, ret);
+}
+
+#ifdef PROP_TXSTATUS
+extern int dhd_wlfc_interface_entry_update(void* state,	ewlfc_mac_entry_action_t action, uint8 ifid,
+	uint8 iftype, uint8* ea);
+extern int dhd_wlfc_FIFOcreditmap_update(void* state, uint8* credits);
+
+int dhd_wlfc_interface_event(struct dhd_info *dhd,
+	ewlfc_mac_entry_action_t action, uint8 ifid, uint8 iftype, uint8* ea)
+{
+	if (dhd->pub.wlfc_state == NULL)
+		return BCME_OK;
+
+	return dhd_wlfc_interface_entry_update(dhd->pub.wlfc_state, action, ifid, iftype, ea);
+}
+
+int dhd_wlfc_FIFOcreditmap_event(struct dhd_info *dhd, uint8* event_data)
+{
+	if (dhd->pub.wlfc_state == NULL)
+		return BCME_OK;
+
+	return dhd_wlfc_FIFOcreditmap_update(dhd->pub.wlfc_state, event_data);
+}
+
+int dhd_wlfc_event(struct dhd_info *dhd)
+{
+	return dhd_wlfc_enable(&dhd->pub);
+}
+#endif /* PROP_TXSTATUS */
+
+#ifdef BCMDBGFS
+
+#include <linux/debugfs.h>
+
+extern uint32 dhd_readregl(void *bp, uint32 addr);
+extern uint32 dhd_writeregl(void *bp, uint32 addr, uint32 data);
+
+typedef struct dhd_dbgfs {
+	struct dentry	*debugfs_dir;
+	struct dentry	*debugfs_mem;
+	dhd_pub_t 	*dhdp;
+	uint32 		size;
+} dhd_dbgfs_t;
+
+dhd_dbgfs_t g_dbgfs;
+
+static int
+dhd_dbg_state_open(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+
+static ssize_t
+dhd_dbg_state_read(struct file *file, char __user *ubuf,
+                       size_t count, loff_t *ppos)
+{
+	ssize_t rval;
+	uint32 tmp;
+	loff_t pos = *ppos;
+	size_t ret;
+
+	if (pos < 0)
+		return -EINVAL;
+	if (pos >= g_dbgfs.size || !count)
+		return 0;
+	if (count > g_dbgfs.size - pos)
+		count = g_dbgfs.size - pos;
+
+	/* Basically enforce aligned 4 byte reads. It's up to the user to work out the details */
+	tmp = dhd_readregl(g_dbgfs.dhdp->bus, file->f_pos & (~3));
+
+	ret = copy_to_user(ubuf, &tmp, 4);
+	if (ret == count)
+		return -EFAULT;
+
+	count -= ret;
+	*ppos = pos + count;
+	rval = count;
+
+	return rval;
+}
+
+
+static ssize_t
+dhd_debugfs_write(struct file *file, const char __user *ubuf, size_t count, loff_t *ppos)
+{
+	loff_t pos = *ppos;
+	size_t ret;
+	uint32 buf;
+
+	if (pos < 0)
+		return -EINVAL;
+	if (pos >= g_dbgfs.size || !count)
+		return 0;
+	if (count > g_dbgfs.size - pos)
+		count = g_dbgfs.size - pos;
+
+	ret = copy_from_user(&buf, ubuf, sizeof(uint32));
+	if (ret == count)
+		return -EFAULT;
+
+	/* Basically enforce aligned 4 byte writes. It's up to the user to work out the details */
+	dhd_writeregl(g_dbgfs.dhdp->bus, file->f_pos & (~3), buf);
+
+	return count;
+}
+
+
+loff_t
+dhd_debugfs_lseek(struct file *file, loff_t off, int whence)
+{
+	loff_t pos = -1;
+
+	switch (whence) {
+		case 0:
+			pos = off;
+			break;
+		case 1:
+			pos = file->f_pos + off;
+			break;
+		case 2:
+			pos = g_dbgfs.size - off;
+	}
+	return (pos < 0 || pos > g_dbgfs.size) ? -EINVAL : (file->f_pos = pos);
+}
+
+static const struct file_operations dhd_dbg_state_ops = {
+	.read   = dhd_dbg_state_read,
+	.write	= dhd_debugfs_write,
+	.open   = dhd_dbg_state_open,
+	.llseek	= dhd_debugfs_lseek
+};
+
+static void dhd_dbg_create(void)
+{
+	if (g_dbgfs.debugfs_dir) {
+		g_dbgfs.debugfs_mem = debugfs_create_file("mem", 0644, g_dbgfs.debugfs_dir,
+			NULL, &dhd_dbg_state_ops);
+	}
+}
+
+void dhd_dbg_init(dhd_pub_t *dhdp)
+{
+	int err;
+
+	g_dbgfs.dhdp = dhdp;
+	g_dbgfs.size = 0x20000000; /* Allow access to various cores regs */
+
+	g_dbgfs.debugfs_dir = debugfs_create_dir("dhd", 0);
+	if (IS_ERR(g_dbgfs.debugfs_dir)) {
+		err = PTR_ERR(g_dbgfs.debugfs_dir);
+		g_dbgfs.debugfs_dir = NULL;
+		return;
+	}
+
+	dhd_dbg_create();
+
+	return;
+}
+
+void dhd_dbg_remove(void)
+{
+	debugfs_remove(g_dbgfs.debugfs_mem);
+	debugfs_remove(g_dbgfs.debugfs_dir);
+
+	bzero((unsigned char *) &g_dbgfs, sizeof(g_dbgfs));
+
+}
+#endif /* ifdef BCMDBGFS */
+
+#ifdef WLMEDIA_HTSF
+
+static
+void dhd_htsf_addtxts(dhd_pub_t *dhdp, void *pktbuf)
+{
+	dhd_info_t *dhd = (dhd_info_t *)(dhdp->info);
+	struct sk_buff *skb;
+	uint32 htsf = 0;
+	uint16 dport = 0, oldmagic = 0xACAC;
+	char *p1;
+	htsfts_t ts;
+
+	/*  timestamp packet  */
+
+	p1 = (char*) PKTDATA(dhdp->osh, pktbuf);
+
+	if (PKTLEN(dhdp->osh, pktbuf) > HTSF_MINLEN) {
+/*		memcpy(&proto, p1+26, 4);  	*/
+		memcpy(&dport, p1+40, 2);
+/* 	proto = ((ntoh32(proto))>> 16) & 0xFF;  */
+		dport = ntoh16(dport);
+	}
+
+	/* timestamp only if  icmp or udb iperf with port 5555 */
+/*	if (proto == 17 && dport == tsport) { */
+	if (dport >= tsport && dport <= tsport + 20) {
+
+		skb = (struct sk_buff *) pktbuf;
+
+		htsf = dhd_get_htsf(dhd, 0);
+		memset(skb->data + 44, 0, 2); /* clear checksum */
+		memcpy(skb->data+82, &oldmagic, 2);
+		memcpy(skb->data+84, &htsf, 4);
+
+		memset(&ts, 0, sizeof(htsfts_t));
+		ts.magic  = HTSFMAGIC;
+		ts.prio   = PKTPRIO(pktbuf);
+		ts.seqnum = htsf_seqnum++;
+		ts.c10    = get_cycles();
+		ts.t10    = htsf;
+		ts.endmagic = HTSFENDMAGIC;
+
+		memcpy(skb->data + HTSF_HOSTOFFSET, &ts, sizeof(ts));
+	}
+}
+
+static void dhd_dump_htsfhisto(histo_t *his, char *s)
+{
+	int pktcnt = 0, curval = 0, i;
+	for (i = 0; i < (NUMBIN-2); i++) {
+		curval += 500;
+		printf("%d ",  his->bin[i]);
+		pktcnt += his->bin[i];
+	}
+	printf(" max: %d TotPkt: %d neg: %d [%s]\n", his->bin[NUMBIN-2], pktcnt,
+		his->bin[NUMBIN-1], s);
+}
+
+static
+void sorttobin(int value, histo_t *histo)
+{
+	int i, binval = 0;
+
+	if (value < 0) {
+		histo->bin[NUMBIN-1]++;
+		return;
+	}
+	if (value > histo->bin[NUMBIN-2])  /* store the max value  */
+		histo->bin[NUMBIN-2] = value;
+
+	for (i = 0; i < (NUMBIN-2); i++) {
+		binval += 500; /* 500m s bins */
+		if (value <= binval) {
+			histo->bin[i]++;
+			return;
+		}
+	}
+	histo->bin[NUMBIN-3]++;
+}
+
+static
+void dhd_htsf_addrxts(dhd_pub_t *dhdp, void *pktbuf)
+{
+	dhd_info_t *dhd = (dhd_info_t *)dhdp->info;
+	struct sk_buff *skb;
+	char *p1;
+	uint16 old_magic;
+	int d1, d2, d3, end2end;
+	htsfts_t *htsf_ts;
+	uint32 htsf;
+
+	skb = PKTTONATIVE(dhdp->osh, pktbuf);
+	p1 = (char*)PKTDATA(dhdp->osh, pktbuf);
+
+	if (PKTLEN(osh, pktbuf) > HTSF_MINLEN) {
+		memcpy(&old_magic, p1+78, 2);
+		htsf_ts = (htsfts_t*) (p1 + HTSF_HOSTOFFSET - 4);
+	}
+	else
+		return;
+
+	if (htsf_ts->magic == HTSFMAGIC) {
+		htsf_ts->tE0 = dhd_get_htsf(dhd, 0);
+		htsf_ts->cE0 = get_cycles();
+	}
+
+	if (old_magic == 0xACAC) {
+
+		tspktcnt++;
+		htsf = dhd_get_htsf(dhd, 0);
+		memcpy(skb->data+92, &htsf, sizeof(uint32));
+
+		memcpy(&ts[tsidx].t1, skb->data+80, 16);
+
+		d1 = ts[tsidx].t2 - ts[tsidx].t1;
+		d2 = ts[tsidx].t3 - ts[tsidx].t2;
+		d3 = ts[tsidx].t4 - ts[tsidx].t3;
+		end2end = ts[tsidx].t4 - ts[tsidx].t1;
+
+		sorttobin(d1, &vi_d1);
+		sorttobin(d2, &vi_d2);
+		sorttobin(d3, &vi_d3);
+		sorttobin(end2end, &vi_d4);
+
+		if (end2end > 0 && end2end >  maxdelay) {
+			maxdelay = end2end;
+			maxdelaypktno = tspktcnt;
+			memcpy(&maxdelayts, &ts[tsidx], 16);
+		}
+		if (++tsidx >= TSMAX)
+			tsidx = 0;
+	}
+}
+
+uint32 dhd_get_htsf(dhd_info_t *dhd, int ifidx)
+{
+	uint32 htsf = 0, cur_cycle, delta, delta_us;
+	uint32    factor, baseval, baseval2;
+	cycles_t t;
+
+	t = get_cycles();
+	cur_cycle = t;
+
+	if (cur_cycle >  dhd->htsf.last_cycle)
+		delta = cur_cycle -  dhd->htsf.last_cycle;
+	else {
+		delta = cur_cycle + (0xFFFFFFFF -  dhd->htsf.last_cycle);
+	}
+
+	delta = delta >> 4;
+
+	if (dhd->htsf.coef) {
+		/* times ten to get the first digit */
+	        factor = (dhd->htsf.coef*10 + dhd->htsf.coefdec1);
+		baseval  = (delta*10)/factor;
+		baseval2 = (delta*10)/(factor+1);
+		delta_us  = (baseval -  (((baseval - baseval2) * dhd->htsf.coefdec2)) / 10);
+		htsf = (delta_us << 4) +  dhd->htsf.last_tsf + HTSF_BUS_DELAY;
+	}
+	else {
+		DHD_ERROR(("-------dhd->htsf.coef = 0 -------\n"));
+	}
+
+	return htsf;
+}
+
+static void dhd_dump_latency(void)
+{
+	int i, max = 0;
+	int d1, d2, d3, d4, d5;
+
+	printf("T1       T2       T3       T4           d1  d2   t4-t1     i    \n");
+	for (i = 0; i < TSMAX; i++) {
+		d1 = ts[i].t2 - ts[i].t1;
+		d2 = ts[i].t3 - ts[i].t2;
+		d3 = ts[i].t4 - ts[i].t3;
+		d4 = ts[i].t4 - ts[i].t1;
+		d5 = ts[max].t4-ts[max].t1;
+		if (d4 > d5 && d4 > 0)  {
+			max = i;
+		}
+		printf("%08X %08X %08X %08X \t%d %d %d   %d i=%d\n",
+			ts[i].t1, ts[i].t2, ts[i].t3, ts[i].t4,
+			d1, d2, d3, d4, i);
+	}
+
+	printf("current idx = %d \n", tsidx);
+
+	printf("Highest latency %d pkt no.%d total=%d\n", maxdelay, maxdelaypktno, tspktcnt);
+	printf("%08X %08X %08X %08X \t%d %d %d   %d\n",
+	maxdelayts.t1, maxdelayts.t2, maxdelayts.t3, maxdelayts.t4,
+	maxdelayts.t2 - maxdelayts.t1,
+	maxdelayts.t3 - maxdelayts.t2,
+	maxdelayts.t4 - maxdelayts.t3,
+	maxdelayts.t4 - maxdelayts.t1);
+}
+
+
+static int
+dhd_ioctl_htsf_get(dhd_info_t *dhd, int ifidx)
+{
+	wl_ioctl_t ioc;
+	char buf[32];
+	int ret;
+	uint32 s1, s2;
+
+	struct tsf {
+		uint32 low;
+		uint32 high;
+	} tsf_buf;
+
+	memset(&ioc, 0, sizeof(ioc));
+	memset(&tsf_buf, 0, sizeof(tsf_buf));
+
+	ioc.cmd = WLC_GET_VAR;
+	ioc.buf = buf;
+	ioc.len = (uint)sizeof(buf);
+	ioc.set = FALSE;
+
+	strcpy(buf, "tsf");
+	s1 = dhd_get_htsf(dhd, 0);
+	if ((ret = dhd_wl_ioctl(&dhd->pub, ifidx, &ioc, ioc.buf, ioc.len)) < 0) {
+		if (ret == -EIO) {
+			DHD_ERROR(("%s: tsf is not supported by device\n",
+				dhd_ifname(&dhd->pub, ifidx)));
+			return -EOPNOTSUPP;
+		}
+		return ret;
+	}
+	s2 = dhd_get_htsf(dhd, 0);
+
+	memcpy(&tsf_buf, buf, sizeof(tsf_buf));
+	printf(" TSF_h=%04X lo=%08X Calc:htsf=%08X, coef=%d.%d%d delta=%d ",
+		tsf_buf.high, tsf_buf.low, s2, dhd->htsf.coef, dhd->htsf.coefdec1,
+		dhd->htsf.coefdec2, s2-tsf_buf.low);
+	printf("lasttsf=%08X lastcycle=%08X\n", dhd->htsf.last_tsf, dhd->htsf.last_cycle);
+	return 0;
+}
+
+void htsf_update(dhd_info_t *dhd, void *data)
+{
+	static ulong  cur_cycle = 0, prev_cycle = 0;
+	uint32 htsf, tsf_delta = 0;
+	uint32 hfactor = 0, cyc_delta, dec1 = 0, dec2, dec3, tmp;
+	ulong b, a;
+	cycles_t t;
+
+	/* cycles_t in inlcude/mips/timex.h */
+
+	t = get_cycles();
+
+	prev_cycle = cur_cycle;
+	cur_cycle = t;
+
+	if (cur_cycle > prev_cycle)
+		cyc_delta = cur_cycle - prev_cycle;
+	else {
+		b = cur_cycle;
+		a = prev_cycle;
+		cyc_delta = cur_cycle + (0xFFFFFFFF - prev_cycle);
+	}
+
+	if (data == NULL)
+		printf(" tsf update ata point er is null \n");
+
+	memcpy(&prev_tsf, &cur_tsf, sizeof(tsf_t));
+	memcpy(&cur_tsf, data, sizeof(tsf_t));
+
+	if (cur_tsf.low == 0) {
+		DHD_INFO((" ---- 0 TSF, do not update, return\n"));
+		return;
+	}
+
+	if (cur_tsf.low > prev_tsf.low)
+		tsf_delta = (cur_tsf.low - prev_tsf.low);
+	else {
+		DHD_INFO((" ---- tsf low is smaller cur_tsf= %08X, prev_tsf=%08X, \n",
+		 cur_tsf.low, prev_tsf.low));
+		if (cur_tsf.high > prev_tsf.high) {
+			tsf_delta = cur_tsf.low + (0xFFFFFFFF - prev_tsf.low);
+			DHD_INFO((" ---- Wrap around tsf coutner  adjusted TSF=%08X\n", tsf_delta));
+		}
+		else
+			return; /* do not update */
+	}
+
+	if (tsf_delta)  {
+		hfactor = cyc_delta / tsf_delta;
+		tmp  = 	(cyc_delta - (hfactor * tsf_delta))*10;
+		dec1 =  tmp/tsf_delta;
+		dec2 =  ((tmp - dec1*tsf_delta)*10) / tsf_delta;
+		tmp  = 	(tmp   - (dec1*tsf_delta))*10;
+		dec3 =  ((tmp - dec2*tsf_delta)*10) / tsf_delta;
+
+		if (dec3 > 4) {
+			if (dec2 == 9) {
+				dec2 = 0;
+				if (dec1 == 9) {
+					dec1 = 0;
+					hfactor++;
+				}
+				else {
+					dec1++;
+				}
+			}
+			else
+				dec2++;
+		}
+	}
+
+	if (hfactor) {
+		htsf = ((cyc_delta * 10)  / (hfactor*10+dec1)) + prev_tsf.low;
+		dhd->htsf.coef = hfactor;
+		dhd->htsf.last_cycle = cur_cycle;
+		dhd->htsf.last_tsf = cur_tsf.low;
+		dhd->htsf.coefdec1 = dec1;
+		dhd->htsf.coefdec2 = dec2;
+	}
+	else {
+		htsf = prev_tsf.low;
+	}
+}
+
+#endif /* WLMEDIA_HTSF */
diff --git a/drivers/net/wireless/bcmdhd/dhd_linux_mon.c b/drivers/net/wireless/bcmdhd/dhd_linux_mon.c
new file mode 100644
index 000000000000..d4dca2607212
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd_linux_mon.c
@@ -0,0 +1,409 @@
+/*
+ * Broadcom Dongle Host Driver (DHD), Linux monitor network interface
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_linux_mon.c 297563 2011-11-20 15:38:29Z $
+ */
+
+#include <linux/string.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/if_arp.h>
+#include <linux/ieee80211.h>
+#include <linux/rtnetlink.h>
+#include <net/ieee80211_radiotap.h>
+
+#include <wlioctl.h>
+#include <bcmutils.h>
+#include <linux_osl.h>
+#include <dhd_dbg.h>
+#include <dngl_stats.h>
+#include <dhd.h>
+
+typedef enum monitor_states
+{
+	MONITOR_STATE_DEINIT = 0x0,
+	MONITOR_STATE_INIT = 0x1,
+	MONITOR_STATE_INTERFACE_ADDED = 0x2,
+	MONITOR_STATE_INTERFACE_DELETED = 0x4
+} monitor_states_t;
+extern int dhd_start_xmit(struct sk_buff *skb, struct net_device *net);
+
+/**
+ * Local declarations and defintions (not exposed)
+ */
+#define MON_PRINT(format, ...) printk("DHD-MON: %s " format, __func__, ##__VA_ARGS__)
+#define MON_TRACE MON_PRINT
+
+typedef struct monitor_interface {
+	int radiotap_enabled;
+	struct net_device* real_ndev;	/* The real interface that the monitor is on */
+	struct net_device* mon_ndev;
+} monitor_interface;
+
+typedef struct dhd_linux_monitor {
+	void *dhd_pub;
+	monitor_states_t monitor_state;
+	monitor_interface mon_if[DHD_MAX_IFS];
+	struct mutex lock;		/* lock to protect mon_if */
+} dhd_linux_monitor_t;
+
+static dhd_linux_monitor_t g_monitor;
+
+static struct net_device* lookup_real_netdev(char *name);
+static monitor_interface* ndev_to_monif(struct net_device *ndev);
+static int dhd_mon_if_open(struct net_device *ndev);
+static int dhd_mon_if_stop(struct net_device *ndev);
+static int dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev);
+static void dhd_mon_if_set_multicast_list(struct net_device *ndev);
+static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr);
+
+static const struct net_device_ops dhd_mon_if_ops = {
+	.ndo_open		= dhd_mon_if_open,
+	.ndo_stop		= dhd_mon_if_stop,
+	.ndo_start_xmit		= dhd_mon_if_subif_start_xmit,
+	.ndo_set_multicast_list = dhd_mon_if_set_multicast_list,
+	.ndo_set_mac_address 	= dhd_mon_if_change_mac,
+};
+
+/**
+ * Local static function defintions
+ */
+
+/* Look up dhd's net device table to find a match (e.g. interface "eth0" is a match for "mon.eth0"
+ * "p2p-eth0-0" is a match for "mon.p2p-eth0-0")
+ */
+static struct net_device* lookup_real_netdev(char *name)
+{
+	int i;
+	int len = 0;
+	int last_name_len = 0;
+	struct net_device *ndev;
+	struct net_device *ndev_found = NULL;
+
+	/* We need to find interface "p2p-p2p-0" corresponding to monitor interface "mon-p2p-0",
+	 * Once mon iface name reaches IFNAMSIZ, it is reset to p2p0-0 and corresponding mon
+	 * iface would be mon-p2p0-0.
+	 */
+	for (i = 0; i < DHD_MAX_IFS; i++) {
+		ndev = dhd_idx2net(g_monitor.dhd_pub, i);
+
+		/* Skip "p2p" and look for "-p2p0-x" in monitor interface name. If it
+		 * it matches, then this netdev is the corresponding real_netdev.
+		 */
+		if (ndev && strstr(ndev->name, "p2p-p2p0")) {
+			len = strlen("p2p");
+		} else {
+		/* if p2p- is not present, then the IFNAMSIZ have reached and name
+		 * would have got reset. In this casse,look for p2p0-x in mon-p2p0-x
+		 */
+			len = 0;
+		}
+		if (ndev && strstr(name, (ndev->name + len))) {
+			if (strlen(ndev->name) > last_name_len) {
+				ndev_found = ndev;
+				last_name_len = strlen(ndev->name);
+			}
+		}
+	}
+
+	return ndev_found;
+}
+
+static monitor_interface* ndev_to_monif(struct net_device *ndev)
+{
+	int i;
+
+	for (i = 0; i < DHD_MAX_IFS; i++) {
+		if (g_monitor.mon_if[i].mon_ndev == ndev)
+			return &g_monitor.mon_if[i];
+	}
+
+	return NULL;
+}
+
+static int dhd_mon_if_open(struct net_device *ndev)
+{
+	int ret = 0;
+
+	MON_PRINT("enter\n");
+	return ret;
+}
+
+static int dhd_mon_if_stop(struct net_device *ndev)
+{
+	int ret = 0;
+
+	MON_PRINT("enter\n");
+	return ret;
+}
+
+static int dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev)
+{
+	int ret = 0;
+	int rtap_len;
+	int qos_len = 0;
+	int dot11_hdr_len = 24;
+	int snap_len = 6;
+	unsigned char *pdata;
+	unsigned short frame_ctl;
+	unsigned char src_mac_addr[6];
+	unsigned char dst_mac_addr[6];
+	struct ieee80211_hdr *dot11_hdr;
+	struct ieee80211_radiotap_header *rtap_hdr;
+	monitor_interface* mon_if;
+
+	MON_PRINT("enter\n");
+
+	mon_if = ndev_to_monif(ndev);
+	if (mon_if == NULL || mon_if->real_ndev == NULL) {
+		MON_PRINT(" cannot find matched net dev, skip the packet\n");
+		goto fail;
+	}
+
+	if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header)))
+		goto fail;
+
+	rtap_hdr = (struct ieee80211_radiotap_header *)skb->data;
+	if (unlikely(rtap_hdr->it_version))
+		goto fail;
+
+	rtap_len = ieee80211_get_radiotap_len(skb->data);
+	if (unlikely(skb->len < rtap_len))
+		goto fail;
+
+	MON_PRINT("radiotap len (should be 14): %d\n", rtap_len);
+
+	/* Skip the ratio tap header */
+	skb_pull(skb, rtap_len);
+
+	dot11_hdr = (struct ieee80211_hdr *)skb->data;
+	frame_ctl = le16_to_cpu(dot11_hdr->frame_control);
+	/* Check if the QoS bit is set */
+	if ((frame_ctl & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) {
+		/* Check if this ia a Wireless Distribution System (WDS) frame
+		 * which has 4 MAC addresses
+		 */
+		if (dot11_hdr->frame_control & 0x0080)
+			qos_len = 2;
+		if ((dot11_hdr->frame_control & 0x0300) == 0x0300)
+			dot11_hdr_len += 6;
+
+		memcpy(dst_mac_addr, dot11_hdr->addr1, sizeof(dst_mac_addr));
+		memcpy(src_mac_addr, dot11_hdr->addr2, sizeof(src_mac_addr));
+
+		/* Skip the 802.11 header, QoS (if any) and SNAP, but leave spaces for
+		 * for two MAC addresses
+		 */
+		skb_pull(skb, dot11_hdr_len + qos_len + snap_len - sizeof(src_mac_addr) * 2);
+		pdata = (unsigned char*)skb->data;
+		memcpy(pdata, dst_mac_addr, sizeof(dst_mac_addr));
+		memcpy(pdata + sizeof(dst_mac_addr), src_mac_addr, sizeof(src_mac_addr));
+
+		MON_PRINT("if name: %s, matched if name %s\n", ndev->name, mon_if->real_ndev->name);
+
+		/* Use the real net device to transmit the packet */
+		ret = dhd_start_xmit(skb, mon_if->real_ndev);
+
+		return ret;
+	}
+fail:
+	dev_kfree_skb(skb);
+	return 0;
+}
+
+static void dhd_mon_if_set_multicast_list(struct net_device *ndev)
+{
+	monitor_interface* mon_if;
+
+	mon_if = ndev_to_monif(ndev);
+	if (mon_if == NULL || mon_if->real_ndev == NULL) {
+		MON_PRINT(" cannot find matched net dev, skip the packet\n");
+	} else {
+		MON_PRINT("enter, if name: %s, matched if name %s\n",
+		ndev->name, mon_if->real_ndev->name);
+	}
+}
+
+static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr)
+{
+	int ret = 0;
+	monitor_interface* mon_if;
+
+	mon_if = ndev_to_monif(ndev);
+	if (mon_if == NULL || mon_if->real_ndev == NULL) {
+		MON_PRINT(" cannot find matched net dev, skip the packet\n");
+	} else {
+		MON_PRINT("enter, if name: %s, matched if name %s\n",
+		ndev->name, mon_if->real_ndev->name);
+	}
+	return ret;
+}
+
+/**
+ * Global function definitions (declared in dhd_linux_mon.h)
+ */
+
+int dhd_add_monitor(char *name, struct net_device **new_ndev)
+{
+	int i;
+	int idx = -1;
+	int ret = 0;
+	struct net_device* ndev = NULL;
+	dhd_linux_monitor_t **dhd_mon;
+
+	mutex_lock(&g_monitor.lock);
+
+	MON_TRACE("enter, if name: %s\n", name);
+	if (!name || !new_ndev) {
+		MON_PRINT("invalid parameters\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/*
+	 * Find a vacancy
+	 */
+	for (i = 0; i < DHD_MAX_IFS; i++)
+		if (g_monitor.mon_if[i].mon_ndev == NULL) {
+			idx = i;
+			break;
+		}
+	if (idx == -1) {
+		MON_PRINT("exceeds maximum interfaces\n");
+		ret = -EFAULT;
+		goto out;
+	}
+
+	ndev = alloc_etherdev(sizeof(dhd_linux_monitor_t*));
+	if (!ndev) {
+		MON_PRINT("failed to allocate memory\n");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ndev->type = ARPHRD_IEEE80211_RADIOTAP;
+	strncpy(ndev->name, name, IFNAMSIZ);
+	ndev->name[IFNAMSIZ - 1] = 0;
+	ndev->netdev_ops = &dhd_mon_if_ops;
+
+	ret = register_netdevice(ndev);
+	if (ret) {
+		MON_PRINT(" register_netdevice failed (%d)\n", ret);
+		goto out;
+	}
+
+	*new_ndev = ndev;
+	g_monitor.mon_if[idx].radiotap_enabled = TRUE;
+	g_monitor.mon_if[idx].mon_ndev = ndev;
+	g_monitor.mon_if[idx].real_ndev = lookup_real_netdev(name);
+	dhd_mon = (dhd_linux_monitor_t **)netdev_priv(ndev);
+	*dhd_mon = &g_monitor;
+	g_monitor.monitor_state = MONITOR_STATE_INTERFACE_ADDED;
+	MON_PRINT("net device returned: 0x%p\n", ndev);
+	MON_PRINT("found a matched net device, name %s\n", g_monitor.mon_if[idx].real_ndev->name);
+
+out:
+	if (ret && ndev)
+		free_netdev(ndev);
+
+	mutex_unlock(&g_monitor.lock);
+	return ret;
+
+}
+
+int dhd_del_monitor(struct net_device *ndev)
+{
+	int i;
+	bool rollback_lock = false;
+	if (!ndev)
+		return -EINVAL;
+	mutex_lock(&g_monitor.lock);
+	for (i = 0; i < DHD_MAX_IFS; i++) {
+		if (g_monitor.mon_if[i].mon_ndev == ndev ||
+			g_monitor.mon_if[i].real_ndev == ndev) {
+			g_monitor.mon_if[i].real_ndev = NULL;
+			if (rtnl_is_locked()) {
+				rtnl_unlock();
+				rollback_lock = true;
+			}
+			unregister_netdev(g_monitor.mon_if[i].mon_ndev);
+			free_netdev(g_monitor.mon_if[i].mon_ndev);
+			g_monitor.mon_if[i].mon_ndev = NULL;
+			g_monitor.monitor_state = MONITOR_STATE_INTERFACE_DELETED;
+			break;
+		}
+	}
+	if (rollback_lock) {
+		rtnl_lock();
+		rollback_lock = false;
+	}
+
+	if (g_monitor.monitor_state !=
+	MONITOR_STATE_INTERFACE_DELETED)
+		MON_PRINT("interface not found in monitor IF array, is this a monitor IF? 0x%p\n",
+			ndev);
+	mutex_unlock(&g_monitor.lock);
+
+	return 0;
+}
+
+int dhd_monitor_init(void *dhd_pub)
+{
+	if (g_monitor.monitor_state == MONITOR_STATE_DEINIT) {
+		g_monitor.dhd_pub = dhd_pub;
+		mutex_init(&g_monitor.lock);
+		g_monitor.monitor_state = MONITOR_STATE_INIT;
+	}
+	return 0;
+}
+
+int dhd_monitor_uninit(void)
+{
+	int i;
+	struct net_device *ndev;
+	bool rollback_lock = false;
+	mutex_lock(&g_monitor.lock);
+	if (g_monitor.monitor_state != MONITOR_STATE_DEINIT) {
+		for (i = 0; i < DHD_MAX_IFS; i++) {
+			ndev = g_monitor.mon_if[i].mon_ndev;
+			if (ndev) {
+				if (rtnl_is_locked()) {
+					rtnl_unlock();
+					rollback_lock = true;
+				}
+				unregister_netdev(ndev);
+				free_netdev(ndev);
+				g_monitor.mon_if[i].real_ndev = NULL;
+				g_monitor.mon_if[i].mon_ndev = NULL;
+				if (rollback_lock) {
+					rtnl_lock();
+					rollback_lock = false;
+				}
+			}
+		}
+		g_monitor.monitor_state = MONITOR_STATE_DEINIT;
+	}
+	mutex_unlock(&g_monitor.lock);
+	return 0;
+}
diff --git a/drivers/net/wireless/bcmdhd/dhd_linux_sched.c b/drivers/net/wireless/bcmdhd/dhd_linux_sched.c
new file mode 100644
index 000000000000..aadd122f5b07
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd_linux_sched.c
@@ -0,0 +1,39 @@
+/*
+ * Expose some of the kernel scheduler routines
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_linux_sched.c,v 1.3 2009-04-10 04:14:49 Exp $
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <typedefs.h>
+#include <linuxver.h>
+
+int setScheduler(struct task_struct *p, int policy, struct sched_param *param)
+{
+	int rc = 0;
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+	rc = sched_setscheduler(p, policy, param);
+#endif /* LinuxVer */
+	return rc;
+}
diff --git a/drivers/net/wireless/bcmdhd/dhd_proto.h b/drivers/net/wireless/bcmdhd/dhd_proto.h
new file mode 100644
index 000000000000..bb1d7365ea94
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd_proto.h
@@ -0,0 +1,105 @@
+/*
+ * Header file describing the internal (inter-module) DHD interfaces.
+ *
+ * Provides type definitions and function prototypes used to link the
+ * DHD OS, bus, and protocol modules.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_proto.h,v 1.8.10.6 2010-12-22 23:47:24 Exp $
+ */
+
+#ifndef _dhd_proto_h_
+#define _dhd_proto_h_
+
+#include <dhdioctl.h>
+#include <wlioctl.h>
+
+#ifndef IOCTL_RESP_TIMEOUT
+#define IOCTL_RESP_TIMEOUT  2000 /* In milli second */
+#endif
+
+/*
+ * Exported from the dhd protocol module (dhd_cdc, dhd_rndis)
+ */
+
+/* Linkage, sets prot link and updates hdrlen in pub */
+extern int dhd_prot_attach(dhd_pub_t *dhdp);
+
+/* Unlink, frees allocated protocol memory (including dhd_prot) */
+extern void dhd_prot_detach(dhd_pub_t *dhdp);
+
+/* Initialize protocol: sync w/dongle state.
+ * Sets dongle media info (iswl, drv_version, mac address).
+ */
+extern int dhd_prot_init(dhd_pub_t *dhdp);
+
+/* Stop protocol: sync w/dongle state. */
+extern void dhd_prot_stop(dhd_pub_t *dhdp);
+
+/* Add any protocol-specific data header.
+ * Caller must reserve prot_hdrlen prepend space.
+ */
+extern void dhd_prot_hdrpush(dhd_pub_t *, int ifidx, void *txp);
+
+/* Remove any protocol-specific data header. */
+extern int dhd_prot_hdrpull(dhd_pub_t *, int *ifidx, void *rxp);
+
+/* Use protocol to issue ioctl to dongle */
+extern int dhd_prot_ioctl(dhd_pub_t *dhd, int ifidx, wl_ioctl_t * ioc, void * buf, int len);
+
+/* Handles a protocol control response asynchronously */
+extern int dhd_prot_ctl_complete(dhd_pub_t *dhd);
+
+/* Check for and handle local prot-specific iovar commands */
+extern int dhd_prot_iovar_op(dhd_pub_t *dhdp, const char *name,
+                             void *params, int plen, void *arg, int len, bool set);
+
+/* Add prot dump output to a buffer */
+extern void dhd_prot_dump(dhd_pub_t *dhdp, struct bcmstrbuf *strbuf);
+
+/* Update local copy of dongle statistics */
+extern void dhd_prot_dstats(dhd_pub_t *dhdp);
+
+extern int dhd_ioctl(dhd_pub_t * dhd_pub, dhd_ioctl_t *ioc, void * buf, uint buflen);
+
+extern int dhd_preinit_ioctls(dhd_pub_t *dhd);
+
+#ifdef PROP_TXSTATUS
+extern int dhd_wlfc_enque_sendq(void* state, int prec, void* p);
+extern int dhd_wlfc_commit_packets(void* state, f_commitpkt_t fcommit, void* commit_ctx);
+extern void dhd_wlfc_cleanup(dhd_pub_t *dhd);
+#endif /* PROP_TXSTATUS */
+
+/********************************
+ * For version-string expansion *
+ */
+#if defined(BDC)
+#define DHD_PROTOCOL "bdc"
+#elif defined(CDC)
+#define DHD_PROTOCOL "cdc"
+#elif defined(RNDIS)
+#define DHD_PROTOCOL "rndis"
+#else
+#define DHD_PROTOCOL "unknown"
+#endif /* proto */
+
+#endif /* _dhd_proto_h_ */
diff --git a/drivers/net/wireless/bcmdhd/dhd_sdio.c b/drivers/net/wireless/bcmdhd/dhd_sdio.c
new file mode 100644
index 000000000000..2cac95f2def4
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd_sdio.c
@@ -0,0 +1,6347 @@
+/*
+ * DHD Bus Module for SDIO
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhd_sdio.c 326662 2012-04-10 06:38:08Z $
+ */
+
+#include <typedefs.h>
+#include <osl.h>
+#include <bcmsdh.h>
+
+#ifdef BCMEMBEDIMAGE
+#include BCMEMBEDIMAGE
+#endif /* BCMEMBEDIMAGE */
+
+#include <bcmdefs.h>
+#include <bcmutils.h>
+#include <bcmendian.h>
+#include <bcmdevs.h>
+
+#include <siutils.h>
+#include <hndpmu.h>
+#include <hndsoc.h>
+#include <bcmsdpcm.h>
+#if defined(DHD_DEBUG)
+#include <hndrte_armtrap.h>
+#include <hndrte_cons.h>
+#endif /* defined(DHD_DEBUG) */
+#include <sbchipc.h>
+#include <sbhnddma.h>
+
+#include <sdio.h>
+#include <sbsdio.h>
+#include <sbsdpcmdev.h>
+#include <bcmsdpcm.h>
+#include <bcmsdbus.h>
+
+#include <proto/ethernet.h>
+#include <proto/802.1d.h>
+#include <proto/802.11.h>
+
+#include <dngl_stats.h>
+#include <dhd.h>
+#include <dhd_bus.h>
+#include <dhd_proto.h>
+#include <dhd_dbg.h>
+#include <dhdioctl.h>
+#include <sdiovar.h>
+
+#ifndef DHDSDIO_MEM_DUMP_FNAME
+#define DHDSDIO_MEM_DUMP_FNAME         "mem_dump"
+#endif
+
+#define QLEN		256	/* bulk rx and tx queue lengths */
+#define FCHI		(QLEN - 10)
+#define FCLOW		(FCHI / 2)
+#define PRIOMASK	7
+
+#define TXRETRIES	2	/* # of retries for tx frames */
+
+#define DHD_RXBOUND	50	/* Default for max rx frames in one scheduling */
+
+#define DHD_TXBOUND	20	/* Default for max tx frames in one scheduling */
+
+#define DHD_TXMINMAX	1	/* Max tx frames if rx still pending */
+
+#define MEMBLOCK	2048		/* Block size used for downloading of dongle image */
+#define MAX_NVRAMBUF_SIZE	4096	/* max nvram buf size */
+#define MAX_DATA_BUF	(32 * 1024)	/* Must be large enough to hold biggest possible glom */
+
+#ifndef DHD_FIRSTREAD
+#define DHD_FIRSTREAD   32
+#endif
+#if !ISPOWEROF2(DHD_FIRSTREAD)
+#error DHD_FIRSTREAD is not a power of 2!
+#endif
+
+/* Total length of frame header for dongle protocol */
+#define SDPCM_HDRLEN	(SDPCM_FRAMETAG_LEN + SDPCM_SWHEADER_LEN)
+#ifdef SDTEST
+#define SDPCM_RESERVE	(SDPCM_HDRLEN + SDPCM_TEST_HDRLEN + DHD_SDALIGN)
+#else
+#define SDPCM_RESERVE	(SDPCM_HDRLEN + DHD_SDALIGN)
+#endif
+
+/* Space for header read, limit for data packets */
+#ifndef MAX_HDR_READ
+#define MAX_HDR_READ	32
+#endif
+#if !ISPOWEROF2(MAX_HDR_READ)
+#error MAX_HDR_READ is not a power of 2!
+#endif
+
+#define MAX_RX_DATASZ	2048
+
+/* Maximum milliseconds to wait for F2 to come up */
+#define DHD_WAIT_F2RDY	3000
+
+/* Bump up limit on waiting for HT to account for first startup;
+ * if the image is doing a CRC calculation before programming the PMU
+ * for HT availability, it could take a couple hundred ms more, so
+ * max out at a 1 second (1000000us).
+ */
+#if (PMU_MAX_TRANSITION_DLY <= 1000000)
+#undef PMU_MAX_TRANSITION_DLY
+#define PMU_MAX_TRANSITION_DLY 1000000
+#endif
+
+/* Value for ChipClockCSR during initial setup */
+#define DHD_INIT_CLKCTL1	(SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_ALP_AVAIL_REQ)
+#define DHD_INIT_CLKCTL2	(SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_FORCE_ALP)
+
+/* Flags for SDH calls */
+#define F2SYNC	(SDIO_REQ_4BYTE | SDIO_REQ_FIXED)
+
+/* Packet free applicable unconditionally for sdio and sdspi.  Conditional if
+ * bufpool was present for gspi bus.
+ */
+#define PKTFREE2()		if ((bus->bus != SPI_BUS) || bus->usebufpool) \
+					PKTFREE(bus->dhd->osh, pkt, FALSE);
+DHD_SPINWAIT_SLEEP_INIT(sdioh_spinwait_sleep);
+#if defined(OOB_INTR_ONLY)
+extern void bcmsdh_set_irq(int flag);
+#endif /* defined(OOB_INTR_ONLY) */
+#ifdef PROP_TXSTATUS
+extern void dhd_wlfc_txcomplete(dhd_pub_t *dhd, void *txp, bool success);
+#endif
+
+#ifdef DHD_DEBUG
+/* Device console log buffer state */
+#define CONSOLE_LINE_MAX	192
+#define CONSOLE_BUFFER_MAX	2024
+typedef struct dhd_console {
+	uint		count;			/* Poll interval msec counter */
+	uint		log_addr;		/* Log struct address (fixed) */
+	hndrte_log_t	log;			/* Log struct (host copy) */
+	uint		bufsize;		/* Size of log buffer */
+	uint8		*buf;			/* Log buffer (host copy) */
+	uint		last;			/* Last buffer read index */
+} dhd_console_t;
+#endif /* DHD_DEBUG */
+
+/* Private data for SDIO bus interaction */
+typedef struct dhd_bus {
+	dhd_pub_t	*dhd;
+
+	bcmsdh_info_t	*sdh;			/* Handle for BCMSDH calls */
+	si_t		*sih;			/* Handle for SI calls */
+	char		*vars;			/* Variables (from CIS and/or other) */
+	uint		varsz;			/* Size of variables buffer */
+	uint32		sbaddr;			/* Current SB window pointer (-1, invalid) */
+
+	sdpcmd_regs_t	*regs;			/* Registers for SDIO core */
+	uint		sdpcmrev;		/* SDIO core revision */
+	uint		armrev;			/* CPU core revision */
+	uint		ramrev;			/* SOCRAM core revision */
+	uint32		ramsize;		/* Size of RAM in SOCRAM (bytes) */
+	uint32		orig_ramsize;		/* Size of RAM in SOCRAM (bytes) */
+
+	uint32		bus;			/* gSPI or SDIO bus */
+	uint32		hostintmask;		/* Copy of Host Interrupt Mask */
+	uint32		intstatus;		/* Intstatus bits (events) pending */
+	bool		dpc_sched;		/* Indicates DPC schedule (intrpt rcvd) */
+	bool		fcstate;		/* State of dongle flow-control */
+
+	uint16		cl_devid;		/* cached devid for dhdsdio_probe_attach() */
+	char		*fw_path; /* module_param: path to firmware image */
+	char		*nv_path; /* module_param: path to nvram vars file */
+	const char      *nvram_params;		/* user specified nvram params. */
+
+	uint		blocksize;		/* Block size of SDIO transfers */
+	uint		roundup;		/* Max roundup limit */
+
+	struct pktq	txq;			/* Queue length used for flow-control */
+	uint8		flowcontrol;		/* per prio flow control bitmask */
+	uint8		tx_seq;			/* Transmit sequence number (next) */
+	uint8		tx_max;			/* Maximum transmit sequence allowed */
+
+	uint8		hdrbuf[MAX_HDR_READ + DHD_SDALIGN];
+	uint8		*rxhdr;			/* Header of current rx frame (in hdrbuf) */
+	uint16		nextlen;		/* Next Read Len from last header */
+	uint8		rx_seq;			/* Receive sequence number (expected) */
+	bool		rxskip;			/* Skip receive (awaiting NAK ACK) */
+
+	void		*glomd;			/* Packet containing glomming descriptor */
+	void		*glom;			/* Packet chain for glommed superframe */
+	uint		glomerr;		/* Glom packet read errors */
+
+	uint8		*rxbuf;			/* Buffer for receiving control packets */
+	uint		rxblen;			/* Allocated length of rxbuf */
+	uint8		*rxctl;			/* Aligned pointer into rxbuf */
+	uint8		*databuf;		/* Buffer for receiving big glom packet */
+	uint8		*dataptr;		/* Aligned pointer into databuf */
+	uint		rxlen;			/* Length of valid data in buffer */
+
+	uint8		sdpcm_ver;		/* Bus protocol reported by dongle */
+
+	bool		intr;			/* Use interrupts */
+	bool		poll;			/* Use polling */
+	bool		ipend;			/* Device interrupt is pending */
+	bool		intdis;			/* Interrupts disabled by isr */
+	uint 		intrcount;		/* Count of device interrupt callbacks */
+	uint		lastintrs;		/* Count as of last watchdog timer */
+	uint		spurious;		/* Count of spurious interrupts */
+	uint		pollrate;		/* Ticks between device polls */
+	uint		polltick;		/* Tick counter */
+	uint		pollcnt;		/* Count of active polls */
+
+#ifdef DHD_DEBUG
+	dhd_console_t	console;		/* Console output polling support */
+	uint		console_addr;		/* Console address from shared struct */
+#endif /* DHD_DEBUG */
+
+	uint		regfails;		/* Count of R_REG/W_REG failures */
+
+	uint		clkstate;		/* State of sd and backplane clock(s) */
+	bool		activity;		/* Activity flag for clock down */
+	int32		idletime;		/* Control for activity timeout */
+	int32		idlecount;		/* Activity timeout counter */
+	int32		idleclock;		/* How to set bus driver when idle */
+	int32		sd_divisor;		/* Speed control to bus driver */
+	int32		sd_mode;		/* Mode control to bus driver */
+	int32		sd_rxchain;		/* If bcmsdh api accepts PKT chains */
+	bool		use_rxchain;		/* If dhd should use PKT chains */
+	bool		sleeping;		/* Is SDIO bus sleeping? */
+	bool		rxflow_mode;	/* Rx flow control mode */
+	bool		rxflow;			/* Is rx flow control on */
+	uint		prev_rxlim_hit;		/* Is prev rx limit exceeded (per dpc schedule) */
+	bool		alp_only;		/* Don't use HT clock (ALP only) */
+	/* Field to decide if rx of control frames happen in rxbuf or lb-pool */
+	bool		usebufpool;
+
+#ifdef SDTEST
+	/* external loopback */
+	bool		ext_loop;
+	uint8		loopid;
+
+	/* pktgen configuration */
+	uint		pktgen_freq;		/* Ticks between bursts */
+	uint		pktgen_count;		/* Packets to send each burst */
+	uint		pktgen_print;		/* Bursts between count displays */
+	uint		pktgen_total;		/* Stop after this many */
+	uint		pktgen_minlen;		/* Minimum packet data len */
+	uint		pktgen_maxlen;		/* Maximum packet data len */
+	uint		pktgen_mode;		/* Configured mode: tx, rx, or echo */
+	uint		pktgen_stop;		/* Number of tx failures causing stop */
+
+	/* active pktgen fields */
+	uint		pktgen_tick;		/* Tick counter for bursts */
+	uint		pktgen_ptick;		/* Burst counter for printing */
+	uint		pktgen_sent;		/* Number of test packets generated */
+	uint		pktgen_rcvd;		/* Number of test packets received */
+	uint		pktgen_fail;		/* Number of failed send attempts */
+	uint16		pktgen_len;		/* Length of next packet to send */
+#define PKTGEN_RCV_IDLE     (0)
+#define PKTGEN_RCV_ONGOING  (1)
+	uint16		pktgen_rcv_state;		/* receive state */
+	uint		pktgen_rcvd_rcvsession;	/* test pkts rcvd per rcv session. */
+#endif /* SDTEST */
+
+	/* Some additional counters */
+	uint		tx_sderrs;		/* Count of tx attempts with sd errors */
+	uint		fcqueued;		/* Tx packets that got queued */
+	uint		rxrtx;			/* Count of rtx requests (NAK to dongle) */
+	uint		rx_toolong;		/* Receive frames too long to receive */
+	uint		rxc_errors;		/* SDIO errors when reading control frames */
+	uint		rx_hdrfail;		/* SDIO errors on header reads */
+	uint		rx_badhdr;		/* Bad received headers (roosync?) */
+	uint		rx_badseq;		/* Mismatched rx sequence number */
+	uint		fc_rcvd;		/* Number of flow-control events received */
+	uint		fc_xoff;		/* Number which turned on flow-control */
+	uint		fc_xon;			/* Number which turned off flow-control */
+	uint		rxglomfail;		/* Failed deglom attempts */
+	uint		rxglomframes;		/* Number of glom frames (superframes) */
+	uint		rxglompkts;		/* Number of packets from glom frames */
+	uint		f2rxhdrs;		/* Number of header reads */
+	uint		f2rxdata;		/* Number of frame data reads */
+	uint		f2txdata;		/* Number of f2 frame writes */
+	uint		f1regdata;		/* Number of f1 register accesses */
+
+	uint8		*ctrl_frame_buf;
+	uint32		ctrl_frame_len;
+	bool		ctrl_frame_stat;
+	uint32		rxint_mode;	/* rx interrupt mode */
+} dhd_bus_t;
+
+/* clkstate */
+#define CLK_NONE	0
+#define CLK_SDONLY	1
+#define CLK_PENDING	2	/* Not used yet */
+#define CLK_AVAIL	3
+
+#define DHD_NOPMU(dhd)	(FALSE)
+
+#ifdef DHD_DEBUG
+static int qcount[NUMPRIO];
+static int tx_packets[NUMPRIO];
+#endif /* DHD_DEBUG */
+
+/* Deferred transmit */
+const uint dhd_deferred_tx = 1;
+
+extern uint dhd_watchdog_ms;
+extern void dhd_os_wd_timer(void *bus, uint wdtick);
+
+/* Tx/Rx bounds */
+uint dhd_txbound;
+uint dhd_rxbound;
+uint dhd_txminmax = DHD_TXMINMAX;
+
+/* override the RAM size if possible */
+#define DONGLE_MIN_MEMSIZE (128 *1024)
+int dhd_dongle_memsize;
+
+static bool dhd_doflow;
+static bool dhd_alignctl;
+
+static bool sd1idle;
+
+static bool retrydata;
+#define RETRYCHAN(chan) (((chan) == SDPCM_EVENT_CHANNEL) || retrydata)
+
+static const uint watermark = 8;
+static const uint firstread = DHD_FIRSTREAD;
+
+#define HDATLEN (firstread - (SDPCM_HDRLEN))
+
+/* Retry count for register access failures */
+static const uint retry_limit = 2;
+
+/* Force even SD lengths (some host controllers mess up on odd bytes) */
+static bool forcealign;
+
+/* Flag to indicate if we should download firmware on driver load */
+uint dhd_download_fw_on_driverload = TRUE;
+
+#define ALIGNMENT  4
+
+#if defined(OOB_INTR_ONLY) && defined(HW_OOB)
+extern void bcmsdh_enable_hw_oob_intr(void *sdh, bool enable);
+#endif
+
+#if defined(OOB_INTR_ONLY) && defined(SDIO_ISR_THREAD)
+#error OOB_INTR_ONLY is NOT working with SDIO_ISR_THREAD
+#endif /* defined(OOB_INTR_ONLY) && defined(SDIO_ISR_THREAD) */
+#define PKTALIGN(osh, p, len, align)					\
+	do {								\
+		uint datalign;						\
+		datalign = (uintptr)PKTDATA((osh), (p));		\
+		datalign = ROUNDUP(datalign, (align)) - datalign;	\
+		ASSERT(datalign < (align));				\
+		ASSERT(PKTLEN((osh), (p)) >= ((len) + datalign));	\
+		if (datalign)						\
+			PKTPULL((osh), (p), datalign);			\
+		PKTSETLEN((osh), (p), (len));				\
+	} while (0)
+
+/* Limit on rounding up frames */
+static const uint max_roundup = 512;
+
+/* Try doing readahead */
+static bool dhd_readahead;
+
+/* To check if there's window offered */
+#define DATAOK(bus) \
+	(((uint8)(bus->tx_max - bus->tx_seq) > 1) && \
+	(((uint8)(bus->tx_max - bus->tx_seq) & 0x80) == 0))
+
+/* To check if there's window offered for ctrl frame */
+#define TXCTLOK(bus) \
+	(((uint8)(bus->tx_max - bus->tx_seq) != 0) && \
+	(((uint8)(bus->tx_max - bus->tx_seq) & 0x80) == 0))
+
+/* Macros to get register read/write status */
+/* NOTE: these assume a local dhdsdio_bus_t *bus! */
+#define R_SDREG(regvar, regaddr, retryvar) \
+do { \
+	retryvar = 0; \
+	do { \
+		regvar = R_REG(bus->dhd->osh, regaddr); \
+	} while (bcmsdh_regfail(bus->sdh) && (++retryvar <= retry_limit)); \
+	if (retryvar) { \
+		bus->regfails += (retryvar-1); \
+		if (retryvar > retry_limit) { \
+			DHD_ERROR(("%s: FAILED" #regvar "READ, LINE %d\n", \
+			           __FUNCTION__, __LINE__)); \
+			regvar = 0; \
+		} \
+	} \
+} while (0)
+
+#define W_SDREG(regval, regaddr, retryvar) \
+do { \
+	retryvar = 0; \
+	do { \
+		W_REG(bus->dhd->osh, regaddr, regval); \
+	} while (bcmsdh_regfail(bus->sdh) && (++retryvar <= retry_limit)); \
+	if (retryvar) { \
+		bus->regfails += (retryvar-1); \
+		if (retryvar > retry_limit) \
+			DHD_ERROR(("%s: FAILED REGISTER WRITE, LINE %d\n", \
+			           __FUNCTION__, __LINE__)); \
+	} \
+} while (0)
+
+#define BUS_WAKE(bus) \
+	do { \
+		if ((bus)->sleeping) \
+			dhdsdio_bussleep((bus), FALSE); \
+	} while (0);
+
+/*
+ * pktavail interrupts from dongle to host can be managed in 3 different ways
+ * whenever there is a packet available in dongle to transmit to host.
+ *
+ * Mode 0:	Dongle writes the software host mailbox and host is interrupted.
+ * Mode 1:	(sdiod core rev >= 4)
+ *		Device sets a new bit in the intstatus whenever there is a packet
+ *		available in fifo.  Host can't clear this specific status bit until all the
+ *		packets are read from the FIFO.  No need to ack dongle intstatus.
+ * Mode 2:	(sdiod core rev >= 4)
+ *		Device sets a bit in the intstatus, and host acks this by writing
+ *		one to this bit.  Dongle won't generate anymore packet interrupts
+ *		until host reads all the packets from the dongle and reads a zero to
+ *		figure that there are no more packets.  No need to disable host ints.
+ *		Need to ack the intstatus.
+ */
+
+#define SDIO_DEVICE_HMB_RXINT		0	/* default old way */
+#define SDIO_DEVICE_RXDATAINT_MODE_0	1	/* from sdiod rev 4 */
+#define SDIO_DEVICE_RXDATAINT_MODE_1	2	/* from sdiod rev 4 */
+
+
+#define FRAME_AVAIL_MASK(bus) 	\
+	((bus->rxint_mode == SDIO_DEVICE_HMB_RXINT) ? I_HMB_FRAME_IND : I_XMTDATA_AVAIL)
+
+#define DHD_BUS			SDIO_BUS
+
+#define PKT_AVAILABLE(bus, intstatus)	((intstatus) & (FRAME_AVAIL_MASK(bus)))
+
+#define HOSTINTMASK		(I_HMB_SW_MASK | I_CHIPACTIVE)
+
+#define GSPI_PR55150_BAILOUT
+
+
+#ifdef SDTEST
+static void dhdsdio_testrcv(dhd_bus_t *bus, void *pkt, uint seq);
+static void dhdsdio_sdtest_set(dhd_bus_t *bus, uint8 count);
+#endif
+
+#ifdef DHD_DEBUG
+static int dhdsdio_checkdied(dhd_bus_t *bus, char *data, uint size);
+static int dhd_serialconsole(dhd_bus_t *bus, bool get, bool enable, int *bcmerror);
+#endif /* DHD_DEBUG */
+
+static int dhdsdio_download_state(dhd_bus_t *bus, bool enter);
+
+static void dhdsdio_release(dhd_bus_t *bus, osl_t *osh);
+static void dhdsdio_release_malloc(dhd_bus_t *bus, osl_t *osh);
+static void dhdsdio_disconnect(void *ptr);
+static bool dhdsdio_chipmatch(uint16 chipid);
+static bool dhdsdio_probe_attach(dhd_bus_t *bus, osl_t *osh, void *sdh,
+                                 void * regsva, uint16  devid);
+static bool dhdsdio_probe_malloc(dhd_bus_t *bus, osl_t *osh, void *sdh);
+static bool dhdsdio_probe_init(dhd_bus_t *bus, osl_t *osh, void *sdh);
+static void dhdsdio_release_dongle(dhd_bus_t *bus, osl_t *osh, bool dongle_isolation,
+	bool reset_flag);
+
+static void dhd_dongle_setmemsize(struct dhd_bus *bus, int mem_size);
+static int dhd_bcmsdh_recv_buf(dhd_bus_t *bus, uint32 addr, uint fn, uint flags,
+	uint8 *buf, uint nbytes,
+	void *pkt, bcmsdh_cmplt_fn_t complete, void *handle);
+static int dhd_bcmsdh_send_buf(dhd_bus_t *bus, uint32 addr, uint fn, uint flags,
+	uint8 *buf, uint nbytes,
+	void *pkt, bcmsdh_cmplt_fn_t complete, void *handle);
+
+static bool dhdsdio_download_firmware(dhd_bus_t *bus, osl_t *osh, void *sdh);
+static int _dhdsdio_download_firmware(dhd_bus_t *bus);
+
+static int dhdsdio_download_code_file(dhd_bus_t *bus, char *image_path);
+static int dhdsdio_download_nvram(dhd_bus_t *bus);
+#ifdef BCMEMBEDIMAGE
+static int dhdsdio_download_code_array(dhd_bus_t *bus);
+#endif
+
+#ifdef WLMEDIA_HTSF
+#include <htsf.h>
+extern uint32 dhd_get_htsf(void *dhd, int ifidx);
+#endif /* WLMEDIA_HTSF */
+
+static void
+dhd_dongle_setmemsize(struct dhd_bus *bus, int mem_size)
+{
+	int32 min_size =  DONGLE_MIN_MEMSIZE;
+	/* Restrict the memsize to user specified limit */
+	DHD_ERROR(("user: Restrict the dongle ram size to %d, min accepted %d\n",
+		dhd_dongle_memsize, min_size));
+	if ((dhd_dongle_memsize > min_size) &&
+		(dhd_dongle_memsize < (int32)bus->orig_ramsize))
+		bus->ramsize = dhd_dongle_memsize;
+}
+
+static int
+dhdsdio_set_siaddr_window(dhd_bus_t *bus, uint32 address)
+{
+	int err = 0;
+	bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRLOW,
+	                 (address >> 8) & SBSDIO_SBADDRLOW_MASK, &err);
+	if (!err)
+		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRMID,
+		                 (address >> 16) & SBSDIO_SBADDRMID_MASK, &err);
+	if (!err)
+		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRHIGH,
+		                 (address >> 24) & SBSDIO_SBADDRHIGH_MASK, &err);
+	return err;
+}
+
+
+/* Turn backplane clock on or off */
+static int
+dhdsdio_htclk(dhd_bus_t *bus, bool on, bool pendok)
+{
+	int err;
+	uint8 clkctl, clkreq, devctl;
+	bcmsdh_info_t *sdh;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+#if defined(OOB_INTR_ONLY)
+	pendok = FALSE;
+#endif
+	clkctl = 0;
+	sdh = bus->sdh;
+
+
+	if (on) {
+		/* Request HT Avail */
+		clkreq = bus->alp_only ? SBSDIO_ALP_AVAIL_REQ : SBSDIO_HT_AVAIL_REQ;
+
+
+
+
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, clkreq, &err);
+		if (err) {
+			DHD_ERROR(("%s: HT Avail request error: %d\n", __FUNCTION__, err));
+			return BCME_ERROR;
+		}
+
+		if (pendok &&
+		    ((bus->sih->buscoretype == PCMCIA_CORE_ID) && (bus->sih->buscorerev == 9))) {
+			uint32 dummy, retries;
+			R_SDREG(dummy, &bus->regs->clockctlstatus, retries);
+		}
+
+		/* Check current status */
+		clkctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, &err);
+		if (err) {
+			DHD_ERROR(("%s: HT Avail read error: %d\n", __FUNCTION__, err));
+			return BCME_ERROR;
+		}
+
+		/* Go to pending and await interrupt if appropriate */
+		if (!SBSDIO_CLKAV(clkctl, bus->alp_only) && pendok) {
+			/* Allow only clock-available interrupt */
+			devctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, &err);
+			if (err) {
+				DHD_ERROR(("%s: Devctl access error setting CA: %d\n",
+				           __FUNCTION__, err));
+				return BCME_ERROR;
+			}
+
+			devctl |= SBSDIO_DEVCTL_CA_INT_ONLY;
+			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, devctl, &err);
+			DHD_INFO(("CLKCTL: set PENDING\n"));
+			bus->clkstate = CLK_PENDING;
+			return BCME_OK;
+		} else if (bus->clkstate == CLK_PENDING) {
+			/* Cancel CA-only interrupt filter */
+			devctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, &err);
+			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
+			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, devctl, &err);
+		}
+
+		/* Otherwise, wait here (polling) for HT Avail */
+		if (!SBSDIO_CLKAV(clkctl, bus->alp_only)) {
+			SPINWAIT_SLEEP(sdioh_spinwait_sleep,
+				((clkctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+			                                    SBSDIO_FUNC1_CHIPCLKCSR, &err)),
+			          !SBSDIO_CLKAV(clkctl, bus->alp_only)), PMU_MAX_TRANSITION_DLY);
+		}
+		if (err) {
+			DHD_ERROR(("%s: HT Avail request error: %d\n", __FUNCTION__, err));
+			return BCME_ERROR;
+		}
+		if (!SBSDIO_CLKAV(clkctl, bus->alp_only)) {
+			DHD_ERROR(("%s: HT Avail timeout (%d): clkctl 0x%02x\n",
+			           __FUNCTION__, PMU_MAX_TRANSITION_DLY, clkctl));
+			return BCME_ERROR;
+		}
+
+
+		/* Mark clock available */
+		bus->clkstate = CLK_AVAIL;
+		DHD_INFO(("CLKCTL: turned ON\n"));
+
+#if defined(DHD_DEBUG)
+		if (bus->alp_only == TRUE) {
+#if !defined(BCMLXSDMMC)
+			if (!SBSDIO_ALPONLY(clkctl)) {
+				DHD_ERROR(("%s: HT Clock, when ALP Only\n", __FUNCTION__));
+			}
+#endif /* !defined(BCMLXSDMMC) */
+		} else {
+			if (SBSDIO_ALPONLY(clkctl)) {
+				DHD_ERROR(("%s: HT Clock should be on.\n", __FUNCTION__));
+			}
+		}
+#endif /* defined (DHD_DEBUG) */
+
+		bus->activity = TRUE;
+	} else {
+		clkreq = 0;
+
+		if (bus->clkstate == CLK_PENDING) {
+			/* Cancel CA-only interrupt filter */
+			devctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, &err);
+			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
+			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, devctl, &err);
+		}
+
+		bus->clkstate = CLK_SDONLY;
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, clkreq, &err);
+		DHD_INFO(("CLKCTL: turned OFF\n"));
+		if (err) {
+			DHD_ERROR(("%s: Failed access turning clock off: %d\n",
+			           __FUNCTION__, err));
+			return BCME_ERROR;
+		}
+	}
+	return BCME_OK;
+}
+
+/* Change idle/active SD state */
+static int
+dhdsdio_sdclk(dhd_bus_t *bus, bool on)
+{
+	int err;
+	int32 iovalue;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (on) {
+		if (bus->idleclock == DHD_IDLE_STOP) {
+			/* Turn on clock and restore mode */
+			iovalue = 1;
+			err = bcmsdh_iovar_op(bus->sdh, "sd_clock", NULL, 0,
+			                      &iovalue, sizeof(iovalue), TRUE);
+			if (err) {
+				DHD_ERROR(("%s: error enabling sd_clock: %d\n",
+				           __FUNCTION__, err));
+				return BCME_ERROR;
+			}
+
+			iovalue = bus->sd_mode;
+			err = bcmsdh_iovar_op(bus->sdh, "sd_mode", NULL, 0,
+			                      &iovalue, sizeof(iovalue), TRUE);
+			if (err) {
+				DHD_ERROR(("%s: error changing sd_mode: %d\n",
+				           __FUNCTION__, err));
+				return BCME_ERROR;
+			}
+		} else if (bus->idleclock != DHD_IDLE_ACTIVE) {
+			/* Restore clock speed */
+			iovalue = bus->sd_divisor;
+			err = bcmsdh_iovar_op(bus->sdh, "sd_divisor", NULL, 0,
+			                      &iovalue, sizeof(iovalue), TRUE);
+			if (err) {
+				DHD_ERROR(("%s: error restoring sd_divisor: %d\n",
+				           __FUNCTION__, err));
+				return BCME_ERROR;
+			}
+		}
+		bus->clkstate = CLK_SDONLY;
+	} else {
+		/* Stop or slow the SD clock itself */
+		if ((bus->sd_divisor == -1) || (bus->sd_mode == -1)) {
+			DHD_TRACE(("%s: can't idle clock, divisor %d mode %d\n",
+			           __FUNCTION__, bus->sd_divisor, bus->sd_mode));
+			return BCME_ERROR;
+		}
+		if (bus->idleclock == DHD_IDLE_STOP) {
+			if (sd1idle) {
+				/* Change to SD1 mode and turn off clock */
+				iovalue = 1;
+				err = bcmsdh_iovar_op(bus->sdh, "sd_mode", NULL, 0,
+				                      &iovalue, sizeof(iovalue), TRUE);
+				if (err) {
+					DHD_ERROR(("%s: error changing sd_clock: %d\n",
+					           __FUNCTION__, err));
+					return BCME_ERROR;
+				}
+			}
+
+			iovalue = 0;
+			err = bcmsdh_iovar_op(bus->sdh, "sd_clock", NULL, 0,
+			                      &iovalue, sizeof(iovalue), TRUE);
+			if (err) {
+				DHD_ERROR(("%s: error disabling sd_clock: %d\n",
+				           __FUNCTION__, err));
+				return BCME_ERROR;
+			}
+		} else if (bus->idleclock != DHD_IDLE_ACTIVE) {
+			/* Set divisor to idle value */
+			iovalue = bus->idleclock;
+			err = bcmsdh_iovar_op(bus->sdh, "sd_divisor", NULL, 0,
+			                      &iovalue, sizeof(iovalue), TRUE);
+			if (err) {
+				DHD_ERROR(("%s: error changing sd_divisor: %d\n",
+				           __FUNCTION__, err));
+				return BCME_ERROR;
+			}
+		}
+		bus->clkstate = CLK_NONE;
+	}
+
+	return BCME_OK;
+}
+
+/* Transition SD and backplane clock readiness */
+static int
+dhdsdio_clkctl(dhd_bus_t *bus, uint target, bool pendok)
+{
+	int ret = BCME_OK;
+#ifdef DHD_DEBUG
+	uint oldstate = bus->clkstate;
+#endif /* DHD_DEBUG */
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	/* Early exit if we're already there */
+	if (bus->clkstate == target) {
+		if (target == CLK_AVAIL) {
+			dhd_os_wd_timer(bus->dhd, dhd_watchdog_ms);
+			bus->activity = TRUE;
+		}
+		return ret;
+	}
+
+	switch (target) {
+	case CLK_AVAIL:
+		/* Make sure SD clock is available */
+		if (bus->clkstate == CLK_NONE)
+			dhdsdio_sdclk(bus, TRUE);
+		/* Now request HT Avail on the backplane */
+		ret = dhdsdio_htclk(bus, TRUE, pendok);
+		if (ret == BCME_OK) {
+			dhd_os_wd_timer(bus->dhd, dhd_watchdog_ms);
+			bus->activity = TRUE;
+		}
+		break;
+
+	case CLK_SDONLY:
+		/* Remove HT request, or bring up SD clock */
+		if (bus->clkstate == CLK_NONE)
+			ret = dhdsdio_sdclk(bus, TRUE);
+		else if (bus->clkstate == CLK_AVAIL)
+			ret = dhdsdio_htclk(bus, FALSE, FALSE);
+		else
+			DHD_ERROR(("dhdsdio_clkctl: request for %d -> %d\n",
+			           bus->clkstate, target));
+		if (ret == BCME_OK) {
+			dhd_os_wd_timer(bus->dhd, dhd_watchdog_ms);
+		}
+		break;
+
+	case CLK_NONE:
+		/* Make sure to remove HT request */
+		if (bus->clkstate == CLK_AVAIL)
+			ret = dhdsdio_htclk(bus, FALSE, FALSE);
+		/* Now remove the SD clock */
+		ret = dhdsdio_sdclk(bus, FALSE);
+#ifdef DHD_DEBUG
+		if (dhd_console_ms == 0)
+#endif /* DHD_DEBUG */
+		if (bus->poll == 0)
+			dhd_os_wd_timer(bus->dhd, 0);
+		break;
+	}
+#ifdef DHD_DEBUG
+	DHD_INFO(("dhdsdio_clkctl: %d -> %d\n", oldstate, bus->clkstate));
+#endif /* DHD_DEBUG */
+
+	return ret;
+}
+
+static int
+dhdsdio_bussleep(dhd_bus_t *bus, bool sleep)
+{
+	bcmsdh_info_t *sdh = bus->sdh;
+	sdpcmd_regs_t *regs = bus->regs;
+	uint retries = 0;
+
+	DHD_INFO(("dhdsdio_bussleep: request %s (currently %s)\n",
+	          (sleep ? "SLEEP" : "WAKE"),
+	          (bus->sleeping ? "SLEEP" : "WAKE")));
+
+	/* Done if we're already in the requested state */
+	if (sleep == bus->sleeping)
+		return BCME_OK;
+
+	/* Going to sleep: set the alarm and turn off the lights... */
+	if (sleep) {
+		/* Don't sleep if something is pending */
+		if (bus->dpc_sched || bus->rxskip || pktq_len(&bus->txq))
+			return BCME_BUSY;
+
+
+		/* Disable SDIO interrupts (no longer interested) */
+		bcmsdh_intr_disable(bus->sdh);
+
+		/* Make sure the controller has the bus up */
+		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+
+		/* Tell device to start using OOB wakeup */
+		W_SDREG(SMB_USE_OOB, &regs->tosbmailbox, retries);
+		if (retries > retry_limit)
+			DHD_ERROR(("CANNOT SIGNAL CHIP, WILL NOT WAKE UP!!\n"));
+
+		/* Turn off our contribution to the HT clock request */
+		dhdsdio_clkctl(bus, CLK_SDONLY, FALSE);
+
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
+		                 SBSDIO_FORCE_HW_CLKREQ_OFF, NULL);
+
+		/* Isolate the bus */
+		if (bus->sih->chip != BCM4329_CHIP_ID && bus->sih->chip != BCM4319_CHIP_ID) {
+			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL,
+				SBSDIO_DEVCTL_PADS_ISO, NULL);
+		}
+
+		/* Change state */
+		bus->sleeping = TRUE;
+
+	} else {
+		/* Waking up: bus power up is ok, set local state */
+
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
+		                 0, NULL);
+
+		/* Force pad isolation off if possible (in case power never toggled) */
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, 0, NULL);
+
+
+		/* Make sure the controller has the bus up */
+		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+
+		/* Send misc interrupt to indicate OOB not needed */
+		W_SDREG(0, &regs->tosbmailboxdata, retries);
+		if (retries <= retry_limit)
+			W_SDREG(SMB_DEV_INT, &regs->tosbmailbox, retries);
+
+		if (retries > retry_limit)
+			DHD_ERROR(("CANNOT SIGNAL CHIP TO CLEAR OOB!!\n"));
+
+		/* Make sure we have SD bus access */
+		dhdsdio_clkctl(bus, CLK_SDONLY, FALSE);
+
+		/* Change state */
+		bus->sleeping = FALSE;
+
+		/* Enable interrupts again */
+		if (bus->intr && (bus->dhd->busstate == DHD_BUS_DATA)) {
+			bus->intdis = FALSE;
+			bcmsdh_intr_enable(bus->sdh);
+		}
+	}
+
+	return BCME_OK;
+}
+
+#if defined(OOB_INTR_ONLY)
+void
+dhd_enable_oob_intr(struct dhd_bus *bus, bool enable)
+{
+#if defined(HW_OOB)
+	bcmsdh_enable_hw_oob_intr(bus->sdh, enable);
+#else
+	sdpcmd_regs_t *regs = bus->regs;
+	uint retries = 0;
+
+	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+	if (enable == TRUE) {
+
+		/* Tell device to start using OOB wakeup */
+		W_SDREG(SMB_USE_OOB, &regs->tosbmailbox, retries);
+		if (retries > retry_limit)
+			DHD_ERROR(("CANNOT SIGNAL CHIP, WILL NOT WAKE UP!!\n"));
+
+	} else {
+		/* Send misc interrupt to indicate OOB not needed */
+		W_SDREG(0, &regs->tosbmailboxdata, retries);
+		if (retries <= retry_limit)
+			W_SDREG(SMB_DEV_INT, &regs->tosbmailbox, retries);
+	}
+
+	/* Turn off our contribution to the HT clock request */
+	dhdsdio_clkctl(bus, CLK_SDONLY, FALSE);
+#endif /* !defined(HW_OOB) */
+}
+#endif /* defined(OOB_INTR_ONLY) */
+
+/* Writes a HW/SW header into the packet and sends it. */
+/* Assumes: (a) header space already there, (b) caller holds lock */
+static int
+dhdsdio_txpkt(dhd_bus_t *bus, void *pkt, uint chan, bool free_pkt)
+{
+	int ret;
+	osl_t *osh;
+	uint8 *frame;
+	uint16 len, pad1 = 0;
+	uint32 swheader;
+	uint retries = 0;
+	bcmsdh_info_t *sdh;
+	void *new;
+	int i;
+#ifdef WLMEDIA_HTSF
+	char *p;
+	htsfts_t *htsf_ts;
+#endif
+
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	sdh = bus->sdh;
+	osh = bus->dhd->osh;
+
+	if (bus->dhd->dongle_reset) {
+		ret = BCME_NOTREADY;
+		goto done;
+	}
+
+	frame = (uint8*)PKTDATA(osh, pkt);
+
+#ifdef WLMEDIA_HTSF
+	if (PKTLEN(osh, pkt) >= 100) {
+		p = PKTDATA(osh, pkt);
+		htsf_ts = (htsfts_t*) (p + HTSF_HOSTOFFSET + 12);
+		if (htsf_ts->magic == HTSFMAGIC) {
+			htsf_ts->c20 = get_cycles();
+			htsf_ts->t20 = dhd_get_htsf(bus->dhd->info, 0);
+		}
+	}
+#endif /* WLMEDIA_HTSF */
+
+	/* Add alignment padding, allocate new packet if needed */
+	if ((pad1 = ((uintptr)frame % DHD_SDALIGN))) {
+		if (PKTHEADROOM(osh, pkt) < pad1) {
+			DHD_INFO(("%s: insufficient headroom %d for %d pad1\n",
+			          __FUNCTION__, (int)PKTHEADROOM(osh, pkt), pad1));
+			bus->dhd->tx_realloc++;
+			new = PKTGET(osh, (PKTLEN(osh, pkt) + DHD_SDALIGN), TRUE);
+			if (!new) {
+				DHD_ERROR(("%s: couldn't allocate new %d-byte packet\n",
+				           __FUNCTION__, PKTLEN(osh, pkt) + DHD_SDALIGN));
+				ret = BCME_NOMEM;
+				goto done;
+			}
+
+			PKTALIGN(osh, new, PKTLEN(osh, pkt), DHD_SDALIGN);
+			bcopy(PKTDATA(osh, pkt), PKTDATA(osh, new), PKTLEN(osh, pkt));
+			if (free_pkt)
+				PKTFREE(osh, pkt, TRUE);
+			/* free the pkt if canned one is not used */
+			free_pkt = TRUE;
+			pkt = new;
+			frame = (uint8*)PKTDATA(osh, pkt);
+			ASSERT(((uintptr)frame % DHD_SDALIGN) == 0);
+			pad1 = 0;
+		} else {
+			PKTPUSH(osh, pkt, pad1);
+			frame = (uint8*)PKTDATA(osh, pkt);
+
+			ASSERT((pad1 + SDPCM_HDRLEN) <= (int) PKTLEN(osh, pkt));
+			bzero(frame, pad1 + SDPCM_HDRLEN);
+		}
+	}
+	ASSERT(pad1 < DHD_SDALIGN);
+
+	/* Hardware tag: 2 byte len followed by 2 byte ~len check (all LE) */
+	len = (uint16)PKTLEN(osh, pkt);
+	*(uint16*)frame = htol16(len);
+	*(((uint16*)frame) + 1) = htol16(~len);
+
+	/* Software tag: channel, sequence number, data offset */
+	swheader = ((chan << SDPCM_CHANNEL_SHIFT) & SDPCM_CHANNEL_MASK) | bus->tx_seq |
+	        (((pad1 + SDPCM_HDRLEN) << SDPCM_DOFFSET_SHIFT) & SDPCM_DOFFSET_MASK);
+	htol32_ua_store(swheader, frame + SDPCM_FRAMETAG_LEN);
+	htol32_ua_store(0, frame + SDPCM_FRAMETAG_LEN + sizeof(swheader));
+
+#ifdef DHD_DEBUG
+	if (PKTPRIO(pkt) < ARRAYSIZE(tx_packets)) {
+		tx_packets[PKTPRIO(pkt)]++;
+	}
+	if (DHD_BYTES_ON() &&
+	    (((DHD_CTL_ON() && (chan == SDPCM_CONTROL_CHANNEL)) ||
+	      (DHD_DATA_ON() && (chan != SDPCM_CONTROL_CHANNEL))))) {
+		prhex("Tx Frame", frame, len);
+	} else if (DHD_HDRS_ON()) {
+		prhex("TxHdr", frame, MIN(len, 16));
+	}
+#endif
+
+	/* Raise len to next SDIO block to eliminate tail command */
+	if (bus->roundup && bus->blocksize && (len > bus->blocksize)) {
+		uint16 pad2 = bus->blocksize - (len % bus->blocksize);
+		if ((pad2 <= bus->roundup) && (pad2 < bus->blocksize))
+#ifdef NOTUSED
+			if (pad2 <= PKTTAILROOM(osh, pkt))
+#endif /* NOTUSED */
+				len += pad2;
+	} else if (len % DHD_SDALIGN) {
+		len += DHD_SDALIGN - (len % DHD_SDALIGN);
+	}
+
+	/* Some controllers have trouble with odd bytes -- round to even */
+	if (forcealign && (len & (ALIGNMENT - 1))) {
+#ifdef NOTUSED
+		if (PKTTAILROOM(osh, pkt))
+#endif
+			len = ROUNDUP(len, ALIGNMENT);
+#ifdef NOTUSED
+		else
+			DHD_ERROR(("%s: sending unrounded %d-byte packet\n", __FUNCTION__, len));
+#endif
+	}
+
+	do {
+		ret = dhd_bcmsdh_send_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
+		                          frame, len, pkt, NULL, NULL);
+		bus->f2txdata++;
+		ASSERT(ret != BCME_PENDING);
+
+		if (ret < 0) {
+			/* On failure, abort the command and terminate the frame */
+			DHD_INFO(("%s: sdio error %d, abort command and terminate frame.\n",
+			          __FUNCTION__, ret));
+			bus->tx_sderrs++;
+
+			bcmsdh_abort(sdh, SDIO_FUNC_2);
+			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_FRAMECTRL,
+			                 SFC_WF_TERM, NULL);
+			bus->f1regdata++;
+
+			for (i = 0; i < 3; i++) {
+				uint8 hi, lo;
+				hi = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+				                     SBSDIO_FUNC1_WFRAMEBCHI, NULL);
+				lo = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+				                     SBSDIO_FUNC1_WFRAMEBCLO, NULL);
+				bus->f1regdata += 2;
+				if ((hi == 0) && (lo == 0))
+					break;
+			}
+
+		}
+		if (ret == 0) {
+			bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQUENCE_WRAP;
+		}
+	} while ((ret < 0) && retrydata && retries++ < TXRETRIES);
+
+done:
+	/* restore pkt buffer pointer before calling tx complete routine */
+	PKTPULL(osh, pkt, SDPCM_HDRLEN + pad1);
+#ifdef PROP_TXSTATUS
+	if (bus->dhd->wlfc_state) {
+		dhd_os_sdunlock(bus->dhd);
+		dhd_wlfc_txcomplete(bus->dhd, pkt, ret == 0);
+		dhd_os_sdlock(bus->dhd);
+	} else {
+#endif /* PROP_TXSTATUS */
+	dhd_txcomplete(bus->dhd, pkt, ret != 0);
+	if (free_pkt)
+		PKTFREE(osh, pkt, TRUE);
+
+#ifdef PROP_TXSTATUS
+	}
+#endif
+	return ret;
+}
+
+int
+dhd_bus_txdata(struct dhd_bus *bus, void *pkt)
+{
+	int ret = BCME_ERROR;
+	osl_t *osh;
+	uint datalen, prec;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	osh = bus->dhd->osh;
+	datalen = PKTLEN(osh, pkt);
+
+#ifdef SDTEST
+	/* Push the test header if doing loopback */
+	if (bus->ext_loop) {
+		uint8* data;
+		PKTPUSH(osh, pkt, SDPCM_TEST_HDRLEN);
+		data = PKTDATA(osh, pkt);
+		*data++ = SDPCM_TEST_ECHOREQ;
+		*data++ = (uint8)bus->loopid++;
+		*data++ = (datalen >> 0);
+		*data++ = (datalen >> 8);
+		datalen += SDPCM_TEST_HDRLEN;
+	}
+#endif /* SDTEST */
+
+	/* Add space for the header */
+	PKTPUSH(osh, pkt, SDPCM_HDRLEN);
+	ASSERT(ISALIGNED((uintptr)PKTDATA(osh, pkt), 2));
+
+	prec = PRIO2PREC((PKTPRIO(pkt) & PRIOMASK));
+#ifndef DHDTHREAD
+	/* Lock: we're about to use shared data/code (and SDIO) */
+	dhd_os_sdlock(bus->dhd);
+#endif /* DHDTHREAD */
+
+	/* Check for existing queue, current flow-control, pending event, or pending clock */
+	if (dhd_deferred_tx || bus->fcstate || pktq_len(&bus->txq) || bus->dpc_sched ||
+	    (!DATAOK(bus)) || (bus->flowcontrol & NBITVAL(prec)) ||
+	    (bus->clkstate != CLK_AVAIL)) {
+		DHD_TRACE(("%s: deferring pktq len %d\n", __FUNCTION__,
+			pktq_len(&bus->txq)));
+		bus->fcqueued++;
+
+		/* Priority based enq */
+		dhd_os_sdlock_txq(bus->dhd);
+		if (dhd_prec_enq(bus->dhd, &bus->txq, pkt, prec) == FALSE) {
+			PKTPULL(osh, pkt, SDPCM_HDRLEN);
+#ifndef DHDTHREAD
+			/* Need to also release txqlock before releasing sdlock.
+			 * This thread still has txqlock and releases sdlock.
+			 * Deadlock happens when dpc() grabs sdlock first then
+			 * attempts to grab txqlock.
+			 */
+			dhd_os_sdunlock_txq(bus->dhd);
+			dhd_os_sdunlock(bus->dhd);
+#endif
+#ifdef PROP_TXSTATUS
+			if (bus->dhd->wlfc_state)
+				dhd_wlfc_txcomplete(bus->dhd, pkt, FALSE);
+			else
+#endif
+			dhd_txcomplete(bus->dhd, pkt, FALSE);
+#ifndef DHDTHREAD
+			dhd_os_sdlock(bus->dhd);
+			dhd_os_sdlock_txq(bus->dhd);
+#endif
+#ifdef PROP_TXSTATUS
+			/* let the caller decide whether to free the packet */
+		if (!bus->dhd->wlfc_state)
+#endif
+			PKTFREE(osh, pkt, TRUE);
+			ret = BCME_NORESOURCE;
+		}
+		else
+			ret = BCME_OK;
+		dhd_os_sdunlock_txq(bus->dhd);
+
+		if ((pktq_len(&bus->txq) >= FCHI) && dhd_doflow)
+			dhd_txflowcontrol(bus->dhd, ALL_INTERFACES, ON);
+
+#ifdef DHD_DEBUG
+		if (pktq_plen(&bus->txq, prec) > qcount[prec])
+			qcount[prec] = pktq_plen(&bus->txq, prec);
+#endif
+		/* Schedule DPC if needed to send queued packet(s) */
+		if (dhd_deferred_tx && !bus->dpc_sched) {
+			bus->dpc_sched = TRUE;
+			dhd_sched_dpc(bus->dhd);
+		}
+	} else {
+#ifdef DHDTHREAD
+		/* Lock: we're about to use shared data/code (and SDIO) */
+		dhd_os_sdlock(bus->dhd);
+#endif /* DHDTHREAD */
+
+		/* Otherwise, send it now */
+		BUS_WAKE(bus);
+		/* Make sure back plane ht clk is on, no pending allowed */
+		dhdsdio_clkctl(bus, CLK_AVAIL, TRUE);
+#ifndef SDTEST
+		ret = dhdsdio_txpkt(bus, pkt, SDPCM_DATA_CHANNEL, TRUE);
+#else
+		ret = dhdsdio_txpkt(bus, pkt,
+		        (bus->ext_loop ? SDPCM_TEST_CHANNEL : SDPCM_DATA_CHANNEL), TRUE);
+#endif
+		if (ret)
+			bus->dhd->tx_errors++;
+		else
+			bus->dhd->dstats.tx_bytes += datalen;
+
+		if ((bus->idletime == DHD_IDLE_IMMEDIATE) && !bus->dpc_sched) {
+			bus->activity = FALSE;
+			dhdsdio_clkctl(bus, CLK_NONE, TRUE);
+		}
+
+#ifdef DHDTHREAD
+		dhd_os_sdunlock(bus->dhd);
+#endif /* DHDTHREAD */
+	}
+
+#ifndef DHDTHREAD
+	dhd_os_sdunlock(bus->dhd);
+#endif /* DHDTHREAD */
+
+	return ret;
+}
+
+static uint
+dhdsdio_sendfromq(dhd_bus_t *bus, uint maxframes)
+{
+	void *pkt;
+	uint32 intstatus = 0;
+	uint retries = 0;
+	int ret = 0, prec_out;
+	uint cnt = 0;
+	uint datalen;
+	uint8 tx_prec_map;
+
+	dhd_pub_t *dhd = bus->dhd;
+	sdpcmd_regs_t *regs = bus->regs;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	tx_prec_map = ~bus->flowcontrol;
+
+	/* Send frames until the limit or some other event */
+	for (cnt = 0; (cnt < maxframes) && DATAOK(bus); cnt++) {
+		dhd_os_sdlock_txq(bus->dhd);
+		if ((pkt = pktq_mdeq(&bus->txq, tx_prec_map, &prec_out)) == NULL) {
+			dhd_os_sdunlock_txq(bus->dhd);
+			break;
+		}
+		dhd_os_sdunlock_txq(bus->dhd);
+		datalen = PKTLEN(bus->dhd->osh, pkt) - SDPCM_HDRLEN;
+
+#ifndef SDTEST
+		ret = dhdsdio_txpkt(bus, pkt, SDPCM_DATA_CHANNEL, TRUE);
+#else
+		ret = dhdsdio_txpkt(bus, pkt,
+		        (bus->ext_loop ? SDPCM_TEST_CHANNEL : SDPCM_DATA_CHANNEL), TRUE);
+#endif
+		if (ret)
+			bus->dhd->tx_errors++;
+		else
+			bus->dhd->dstats.tx_bytes += datalen;
+
+		/* In poll mode, need to check for other events */
+		if (!bus->intr && cnt)
+		{
+			/* Check device status, signal pending interrupt */
+			R_SDREG(intstatus, &regs->intstatus, retries);
+			bus->f2txdata++;
+			if (bcmsdh_regfail(bus->sdh))
+				break;
+			if (intstatus & bus->hostintmask)
+				bus->ipend = TRUE;
+		}
+	}
+
+	/* Deflow-control stack if needed */
+	if (dhd_doflow && dhd->up && (dhd->busstate == DHD_BUS_DATA) &&
+	    dhd->txoff && (pktq_len(&bus->txq) < FCLOW))
+		dhd_txflowcontrol(dhd, ALL_INTERFACES, OFF);
+
+	return cnt;
+}
+
+int
+dhd_bus_txctl(struct dhd_bus *bus, uchar *msg, uint msglen)
+{
+	uint8 *frame;
+	uint16 len;
+	uint32 swheader;
+	uint retries = 0;
+	bcmsdh_info_t *sdh = bus->sdh;
+	uint8 doff = 0;
+	int ret = -1;
+	int i;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (bus->dhd->dongle_reset)
+		return -EIO;
+
+	/* Back the pointer to make a room for bus header */
+	frame = msg - SDPCM_HDRLEN;
+	len = (msglen += SDPCM_HDRLEN);
+
+	/* Add alignment padding (optional for ctl frames) */
+	if (dhd_alignctl) {
+		if ((doff = ((uintptr)frame % DHD_SDALIGN))) {
+			frame -= doff;
+			len += doff;
+			msglen += doff;
+			bzero(frame, doff + SDPCM_HDRLEN);
+		}
+		ASSERT(doff < DHD_SDALIGN);
+	}
+	doff += SDPCM_HDRLEN;
+
+	/* Round send length to next SDIO block */
+	if (bus->roundup && bus->blocksize && (len > bus->blocksize)) {
+		uint16 pad = bus->blocksize - (len % bus->blocksize);
+		if ((pad <= bus->roundup) && (pad < bus->blocksize))
+			len += pad;
+	} else if (len % DHD_SDALIGN) {
+		len += DHD_SDALIGN - (len % DHD_SDALIGN);
+	}
+
+	/* Satisfy length-alignment requirements */
+	if (forcealign && (len & (ALIGNMENT - 1)))
+		len = ROUNDUP(len, ALIGNMENT);
+
+	ASSERT(ISALIGNED((uintptr)frame, 2));
+
+
+	/* Need to lock here to protect txseq and SDIO tx calls */
+	dhd_os_sdlock(bus->dhd);
+
+	BUS_WAKE(bus);
+
+	/* Make sure backplane clock is on */
+	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+
+	/* Hardware tag: 2 byte len followed by 2 byte ~len check (all LE) */
+	*(uint16*)frame = htol16((uint16)msglen);
+	*(((uint16*)frame) + 1) = htol16(~msglen);
+
+	/* Software tag: channel, sequence number, data offset */
+	swheader = ((SDPCM_CONTROL_CHANNEL << SDPCM_CHANNEL_SHIFT) & SDPCM_CHANNEL_MASK)
+	        | bus->tx_seq | ((doff << SDPCM_DOFFSET_SHIFT) & SDPCM_DOFFSET_MASK);
+	htol32_ua_store(swheader, frame + SDPCM_FRAMETAG_LEN);
+	htol32_ua_store(0, frame + SDPCM_FRAMETAG_LEN + sizeof(swheader));
+
+	if (!TXCTLOK(bus)) {
+		DHD_INFO(("%s: No bus credit bus->tx_max %d, bus->tx_seq %d\n",
+			__FUNCTION__, bus->tx_max, bus->tx_seq));
+		bus->ctrl_frame_stat = TRUE;
+		/* Send from dpc */
+		bus->ctrl_frame_buf = frame;
+		bus->ctrl_frame_len = len;
+		dhd_wait_for_event(bus->dhd, &bus->ctrl_frame_stat);
+		if (bus->ctrl_frame_stat == FALSE) {
+			DHD_INFO(("%s: ctrl_frame_stat == FALSE\n", __FUNCTION__));
+			ret = 0;
+		} else {
+			bus->dhd->txcnt_timeout++;
+			if (!bus->dhd->hang_was_sent)
+				DHD_ERROR(("%s: ctrl_frame_stat == TRUE txcnt_timeout=%d\n",
+					__FUNCTION__, bus->dhd->txcnt_timeout));
+			ret = -1;
+			bus->ctrl_frame_stat = FALSE;
+			goto done;
+		}
+	}
+
+	bus->dhd->txcnt_timeout = 0;
+
+	if (ret == -1) {
+#ifdef DHD_DEBUG
+		if (DHD_BYTES_ON() && DHD_CTL_ON()) {
+			prhex("Tx Frame", frame, len);
+		} else if (DHD_HDRS_ON()) {
+			prhex("TxHdr", frame, MIN(len, 16));
+		}
+#endif
+
+		do {
+			ret = dhd_bcmsdh_send_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
+			                          frame, len, NULL, NULL, NULL);
+			ASSERT(ret != BCME_PENDING);
+
+			if (ret < 0) {
+			/* On failure, abort the command and terminate the frame */
+				DHD_INFO(("%s: sdio error %d, abort command and terminate frame.\n",
+				          __FUNCTION__, ret));
+				bus->tx_sderrs++;
+
+				bcmsdh_abort(sdh, SDIO_FUNC_2);
+
+				bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_FRAMECTRL,
+				                 SFC_WF_TERM, NULL);
+				bus->f1regdata++;
+
+				for (i = 0; i < 3; i++) {
+					uint8 hi, lo;
+					hi = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+					                     SBSDIO_FUNC1_WFRAMEBCHI, NULL);
+					lo = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+					                     SBSDIO_FUNC1_WFRAMEBCLO, NULL);
+					bus->f1regdata += 2;
+					if ((hi == 0) && (lo == 0))
+						break;
+				}
+
+			}
+			if (ret == 0) {
+				bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQUENCE_WRAP;
+			}
+		} while ((ret < 0) && retries++ < TXRETRIES);
+	}
+
+done:
+	if ((bus->idletime == DHD_IDLE_IMMEDIATE) && !bus->dpc_sched) {
+		bus->activity = FALSE;
+		dhdsdio_clkctl(bus, CLK_NONE, TRUE);
+	}
+
+	dhd_os_sdunlock(bus->dhd);
+
+	if (ret)
+		bus->dhd->tx_ctlerrs++;
+	else
+		bus->dhd->tx_ctlpkts++;
+
+	if (bus->dhd->txcnt_timeout >= MAX_CNTL_TIMEOUT)
+		return -ETIMEDOUT;
+
+	return ret ? -EIO : 0;
+}
+
+int
+dhd_bus_rxctl(struct dhd_bus *bus, uchar *msg, uint msglen)
+{
+	int timeleft;
+	uint rxlen = 0;
+	bool pending = FALSE;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (bus->dhd->dongle_reset)
+		return -EIO;
+
+	/* Wait until control frame is available */
+	timeleft = dhd_os_ioctl_resp_wait(bus->dhd, &bus->rxlen, &pending);
+
+	dhd_os_sdlock(bus->dhd);
+	rxlen = bus->rxlen;
+	bcopy(bus->rxctl, msg, MIN(msglen, rxlen));
+	bus->rxlen = 0;
+	dhd_os_sdunlock(bus->dhd);
+
+	if (rxlen) {
+		DHD_CTL(("%s: resumed on rxctl frame, got %d expected %d\n",
+		         __FUNCTION__, rxlen, msglen));
+	} else if (timeleft == 0) {
+		DHD_ERROR(("%s: resumed on timeout\n", __FUNCTION__));
+#ifdef DHD_DEBUG
+		dhd_os_sdlock(bus->dhd);
+		dhdsdio_checkdied(bus, NULL, 0);
+		dhd_os_sdunlock(bus->dhd);
+#endif /* DHD_DEBUG */
+	} else if (pending == TRUE) {
+		/* signal pending */
+		DHD_ERROR(("%s: signal pending\n", __FUNCTION__));
+		return -EINTR;
+	} else {
+		DHD_CTL(("%s: resumed for unknown reason?\n", __FUNCTION__));
+#ifdef DHD_DEBUG
+		dhd_os_sdlock(bus->dhd);
+		dhdsdio_checkdied(bus, NULL, 0);
+		dhd_os_sdunlock(bus->dhd);
+#endif /* DHD_DEBUG */
+	}
+	if (timeleft == 0) {
+		bus->dhd->rxcnt_timeout++;
+		DHD_ERROR(("%s: rxcnt_timeout=%d\n", __FUNCTION__, bus->dhd->rxcnt_timeout));
+	}
+	else
+		bus->dhd->rxcnt_timeout = 0;
+
+	if (rxlen)
+		bus->dhd->rx_ctlpkts++;
+	else
+		bus->dhd->rx_ctlerrs++;
+
+	if (bus->dhd->rxcnt_timeout >= MAX_CNTL_TIMEOUT)
+		return -ETIMEDOUT;
+
+	return rxlen ? (int)rxlen : -EIO;
+}
+
+/* IOVar table */
+enum {
+	IOV_INTR = 1,
+	IOV_POLLRATE,
+	IOV_SDREG,
+	IOV_SBREG,
+	IOV_SDCIS,
+	IOV_MEMBYTES,
+	IOV_MEMSIZE,
+#ifdef DHD_DEBUG
+	IOV_CHECKDIED,
+	IOV_SERIALCONS,
+#endif /* DHD_DEBUG */
+	IOV_DOWNLOAD,
+	IOV_SOCRAM_STATE,
+	IOV_FORCEEVEN,
+	IOV_SDIOD_DRIVE,
+	IOV_READAHEAD,
+	IOV_SDRXCHAIN,
+	IOV_ALIGNCTL,
+	IOV_SDALIGN,
+	IOV_DEVRESET,
+	IOV_CPU,
+#ifdef SDTEST
+	IOV_PKTGEN,
+	IOV_EXTLOOP,
+#endif /* SDTEST */
+	IOV_SPROM,
+	IOV_TXBOUND,
+	IOV_RXBOUND,
+	IOV_TXMINMAX,
+	IOV_IDLETIME,
+	IOV_IDLECLOCK,
+	IOV_SD1IDLE,
+	IOV_SLEEP,
+	IOV_DONGLEISOLATION,
+	IOV_VARS,
+#ifdef SOFTAP
+	IOV_FWPATH
+#endif
+};
+
+const bcm_iovar_t dhdsdio_iovars[] = {
+	{"intr",	IOV_INTR,	0,	IOVT_BOOL,	0 },
+	{"sleep",	IOV_SLEEP,	0,	IOVT_BOOL,	0 },
+	{"pollrate",	IOV_POLLRATE,	0,	IOVT_UINT32,	0 },
+	{"idletime",	IOV_IDLETIME,	0,	IOVT_INT32,	0 },
+	{"idleclock",	IOV_IDLECLOCK,	0,	IOVT_INT32,	0 },
+	{"sd1idle",	IOV_SD1IDLE,	0,	IOVT_BOOL,	0 },
+	{"membytes",	IOV_MEMBYTES,	0,	IOVT_BUFFER,	2 * sizeof(int) },
+	{"memsize",	IOV_MEMSIZE,	0,	IOVT_UINT32,	0 },
+	{"download",	IOV_DOWNLOAD,	0,	IOVT_BOOL,	0 },
+	{"socram_state",	IOV_SOCRAM_STATE,	0,	IOVT_BOOL,	0 },
+	{"vars",	IOV_VARS,	0,	IOVT_BUFFER,	0 },
+	{"sdiod_drive",	IOV_SDIOD_DRIVE, 0,	IOVT_UINT32,	0 },
+	{"readahead",	IOV_READAHEAD,	0,	IOVT_BOOL,	0 },
+	{"sdrxchain",	IOV_SDRXCHAIN,	0,	IOVT_BOOL,	0 },
+	{"alignctl",	IOV_ALIGNCTL,	0,	IOVT_BOOL,	0 },
+	{"sdalign",	IOV_SDALIGN,	0,	IOVT_BOOL,	0 },
+	{"devreset",	IOV_DEVRESET,	0,	IOVT_BOOL,	0 },
+#ifdef DHD_DEBUG
+	{"sdreg",	IOV_SDREG,	0,	IOVT_BUFFER,	sizeof(sdreg_t) },
+	{"sbreg",	IOV_SBREG,	0,	IOVT_BUFFER,	sizeof(sdreg_t) },
+	{"sd_cis",	IOV_SDCIS,	0,	IOVT_BUFFER,	DHD_IOCTL_MAXLEN },
+	{"forcealign",	IOV_FORCEEVEN,	0,	IOVT_BOOL,	0 },
+	{"txbound",	IOV_TXBOUND,	0,	IOVT_UINT32,	0 },
+	{"rxbound",	IOV_RXBOUND,	0,	IOVT_UINT32,	0 },
+	{"txminmax",	IOV_TXMINMAX,	0,	IOVT_UINT32,	0 },
+	{"cpu",		IOV_CPU,	0,	IOVT_BOOL,	0 },
+#ifdef DHD_DEBUG
+	{"checkdied",	IOV_CHECKDIED,	0,	IOVT_BUFFER,	0 },
+	{"serial",	IOV_SERIALCONS,	0,	IOVT_UINT32,	0 },
+#endif /* DHD_DEBUG  */
+#endif /* DHD_DEBUG */
+#ifdef SDTEST
+	{"extloop",	IOV_EXTLOOP,	0,	IOVT_BOOL,	0 },
+	{"pktgen",	IOV_PKTGEN,	0,	IOVT_BUFFER,	sizeof(dhd_pktgen_t) },
+#endif /* SDTEST */
+	{"dngl_isolation", IOV_DONGLEISOLATION,	0,	IOVT_UINT32,	0 },
+#ifdef SOFTAP
+	{"fwpath", IOV_FWPATH, 0, IOVT_BUFFER, 0 },
+#endif
+	{NULL, 0, 0, 0, 0 }
+};
+
+static void
+dhd_dump_pct(struct bcmstrbuf *strbuf, char *desc, uint num, uint div)
+{
+	uint q1, q2;
+
+	if (!div) {
+		bcm_bprintf(strbuf, "%s N/A", desc);
+	} else {
+		q1 = num / div;
+		q2 = (100 * (num - (q1 * div))) / div;
+		bcm_bprintf(strbuf, "%s %d.%02d", desc, q1, q2);
+	}
+}
+
+void
+dhd_bus_dump(dhd_pub_t *dhdp, struct bcmstrbuf *strbuf)
+{
+	dhd_bus_t *bus = dhdp->bus;
+
+	bcm_bprintf(strbuf, "Bus SDIO structure:\n");
+	bcm_bprintf(strbuf, "hostintmask 0x%08x intstatus 0x%08x sdpcm_ver %d\n",
+	            bus->hostintmask, bus->intstatus, bus->sdpcm_ver);
+	bcm_bprintf(strbuf, "fcstate %d qlen %d tx_seq %d, max %d, rxskip %d rxlen %d rx_seq %d\n",
+	            bus->fcstate, pktq_len(&bus->txq), bus->tx_seq, bus->tx_max, bus->rxskip,
+	            bus->rxlen, bus->rx_seq);
+	bcm_bprintf(strbuf, "intr %d intrcount %d lastintrs %d spurious %d\n",
+	            bus->intr, bus->intrcount, bus->lastintrs, bus->spurious);
+	bcm_bprintf(strbuf, "pollrate %d pollcnt %d regfails %d\n",
+	            bus->pollrate, bus->pollcnt, bus->regfails);
+
+	bcm_bprintf(strbuf, "\nAdditional counters:\n");
+	bcm_bprintf(strbuf, "tx_sderrs %d fcqueued %d rxrtx %d rx_toolong %d rxc_errors %d\n",
+	            bus->tx_sderrs, bus->fcqueued, bus->rxrtx, bus->rx_toolong,
+	            bus->rxc_errors);
+	bcm_bprintf(strbuf, "rx_hdrfail %d badhdr %d badseq %d\n",
+	            bus->rx_hdrfail, bus->rx_badhdr, bus->rx_badseq);
+	bcm_bprintf(strbuf, "fc_rcvd %d, fc_xoff %d, fc_xon %d\n",
+	            bus->fc_rcvd, bus->fc_xoff, bus->fc_xon);
+	bcm_bprintf(strbuf, "rxglomfail %d, rxglomframes %d, rxglompkts %d\n",
+	            bus->rxglomfail, bus->rxglomframes, bus->rxglompkts);
+	bcm_bprintf(strbuf, "f2rx (hdrs/data) %d (%d/%d), f2tx %d f1regs %d\n",
+	            (bus->f2rxhdrs + bus->f2rxdata), bus->f2rxhdrs, bus->f2rxdata,
+	            bus->f2txdata, bus->f1regdata);
+	{
+		dhd_dump_pct(strbuf, "\nRx: pkts/f2rd", bus->dhd->rx_packets,
+		             (bus->f2rxhdrs + bus->f2rxdata));
+		dhd_dump_pct(strbuf, ", pkts/f1sd", bus->dhd->rx_packets, bus->f1regdata);
+		dhd_dump_pct(strbuf, ", pkts/sd", bus->dhd->rx_packets,
+		             (bus->f2rxhdrs + bus->f2rxdata + bus->f1regdata));
+		dhd_dump_pct(strbuf, ", pkts/int", bus->dhd->rx_packets, bus->intrcount);
+		bcm_bprintf(strbuf, "\n");
+
+		dhd_dump_pct(strbuf, "Rx: glom pct", (100 * bus->rxglompkts),
+		             bus->dhd->rx_packets);
+		dhd_dump_pct(strbuf, ", pkts/glom", bus->rxglompkts, bus->rxglomframes);
+		bcm_bprintf(strbuf, "\n");
+
+		dhd_dump_pct(strbuf, "Tx: pkts/f2wr", bus->dhd->tx_packets, bus->f2txdata);
+		dhd_dump_pct(strbuf, ", pkts/f1sd", bus->dhd->tx_packets, bus->f1regdata);
+		dhd_dump_pct(strbuf, ", pkts/sd", bus->dhd->tx_packets,
+		             (bus->f2txdata + bus->f1regdata));
+		dhd_dump_pct(strbuf, ", pkts/int", bus->dhd->tx_packets, bus->intrcount);
+		bcm_bprintf(strbuf, "\n");
+
+		dhd_dump_pct(strbuf, "Total: pkts/f2rw",
+		             (bus->dhd->tx_packets + bus->dhd->rx_packets),
+		             (bus->f2txdata + bus->f2rxhdrs + bus->f2rxdata));
+		dhd_dump_pct(strbuf, ", pkts/f1sd",
+		             (bus->dhd->tx_packets + bus->dhd->rx_packets), bus->f1regdata);
+		dhd_dump_pct(strbuf, ", pkts/sd",
+		             (bus->dhd->tx_packets + bus->dhd->rx_packets),
+		             (bus->f2txdata + bus->f2rxhdrs + bus->f2rxdata + bus->f1regdata));
+		dhd_dump_pct(strbuf, ", pkts/int",
+		             (bus->dhd->tx_packets + bus->dhd->rx_packets), bus->intrcount);
+		bcm_bprintf(strbuf, "\n\n");
+	}
+
+#ifdef SDTEST
+	if (bus->pktgen_count) {
+		bcm_bprintf(strbuf, "pktgen config and count:\n");
+		bcm_bprintf(strbuf, "freq %d count %d print %d total %d min %d len %d\n",
+		            bus->pktgen_freq, bus->pktgen_count, bus->pktgen_print,
+		            bus->pktgen_total, bus->pktgen_minlen, bus->pktgen_maxlen);
+		bcm_bprintf(strbuf, "send attempts %d rcvd %d fail %d\n",
+		            bus->pktgen_sent, bus->pktgen_rcvd, bus->pktgen_fail);
+	}
+#endif /* SDTEST */
+#ifdef DHD_DEBUG
+	bcm_bprintf(strbuf, "dpc_sched %d host interrupt%spending\n",
+	            bus->dpc_sched, (bcmsdh_intr_pending(bus->sdh) ? " " : " not "));
+	bcm_bprintf(strbuf, "blocksize %d roundup %d\n", bus->blocksize, bus->roundup);
+#endif /* DHD_DEBUG */
+	bcm_bprintf(strbuf, "clkstate %d activity %d idletime %d idlecount %d sleeping %d\n",
+	            bus->clkstate, bus->activity, bus->idletime, bus->idlecount, bus->sleeping);
+}
+
+void
+dhd_bus_clearcounts(dhd_pub_t *dhdp)
+{
+	dhd_bus_t *bus = (dhd_bus_t *)dhdp->bus;
+
+	bus->intrcount = bus->lastintrs = bus->spurious = bus->regfails = 0;
+	bus->rxrtx = bus->rx_toolong = bus->rxc_errors = 0;
+	bus->rx_hdrfail = bus->rx_badhdr = bus->rx_badseq = 0;
+	bus->tx_sderrs = bus->fc_rcvd = bus->fc_xoff = bus->fc_xon = 0;
+	bus->rxglomfail = bus->rxglomframes = bus->rxglompkts = 0;
+	bus->f2rxhdrs = bus->f2rxdata = bus->f2txdata = bus->f1regdata = 0;
+}
+
+#ifdef SDTEST
+static int
+dhdsdio_pktgen_get(dhd_bus_t *bus, uint8 *arg)
+{
+	dhd_pktgen_t pktgen;
+
+	pktgen.version = DHD_PKTGEN_VERSION;
+	pktgen.freq = bus->pktgen_freq;
+	pktgen.count = bus->pktgen_count;
+	pktgen.print = bus->pktgen_print;
+	pktgen.total = bus->pktgen_total;
+	pktgen.minlen = bus->pktgen_minlen;
+	pktgen.maxlen = bus->pktgen_maxlen;
+	pktgen.numsent = bus->pktgen_sent;
+	pktgen.numrcvd = bus->pktgen_rcvd;
+	pktgen.numfail = bus->pktgen_fail;
+	pktgen.mode = bus->pktgen_mode;
+	pktgen.stop = bus->pktgen_stop;
+
+	bcopy(&pktgen, arg, sizeof(pktgen));
+
+	return 0;
+}
+
+static int
+dhdsdio_pktgen_set(dhd_bus_t *bus, uint8 *arg)
+{
+	dhd_pktgen_t pktgen;
+	uint oldcnt, oldmode;
+
+	bcopy(arg, &pktgen, sizeof(pktgen));
+	if (pktgen.version != DHD_PKTGEN_VERSION)
+		return BCME_BADARG;
+
+	oldcnt = bus->pktgen_count;
+	oldmode = bus->pktgen_mode;
+
+	bus->pktgen_freq = pktgen.freq;
+	bus->pktgen_count = pktgen.count;
+	bus->pktgen_print = pktgen.print;
+	bus->pktgen_total = pktgen.total;
+	bus->pktgen_minlen = pktgen.minlen;
+	bus->pktgen_maxlen = pktgen.maxlen;
+	bus->pktgen_mode = pktgen.mode;
+	bus->pktgen_stop = pktgen.stop;
+
+	bus->pktgen_tick = bus->pktgen_ptick = 0;
+	bus->pktgen_len = MAX(bus->pktgen_len, bus->pktgen_minlen);
+	bus->pktgen_len = MIN(bus->pktgen_len, bus->pktgen_maxlen);
+
+	/* Clear counts for a new pktgen (mode change, or was stopped) */
+	if (bus->pktgen_count && (!oldcnt || oldmode != bus->pktgen_mode))
+		bus->pktgen_sent = bus->pktgen_rcvd = bus->pktgen_fail = 0;
+
+	return 0;
+}
+#endif /* SDTEST */
+
+static int
+dhdsdio_membytes(dhd_bus_t *bus, bool write, uint32 address, uint8 *data, uint size)
+{
+	int bcmerror = 0;
+	uint32 sdaddr;
+	uint dsize;
+
+	/* Determine initial transfer parameters */
+	sdaddr = address & SBSDIO_SB_OFT_ADDR_MASK;
+	if ((sdaddr + size) & SBSDIO_SBWINDOW_MASK)
+		dsize = (SBSDIO_SB_OFT_ADDR_LIMIT - sdaddr);
+	else
+		dsize = size;
+
+	/* Set the backplane window to include the start address */
+	if ((bcmerror = dhdsdio_set_siaddr_window(bus, address))) {
+		DHD_ERROR(("%s: window change failed\n", __FUNCTION__));
+		goto xfer_done;
+	}
+
+	/* Do the transfer(s) */
+	while (size) {
+		DHD_INFO(("%s: %s %d bytes at offset 0x%08x in window 0x%08x\n",
+		          __FUNCTION__, (write ? "write" : "read"), dsize, sdaddr,
+		          (address & SBSDIO_SBWINDOW_MASK)));
+		if ((bcmerror = bcmsdh_rwdata(bus->sdh, write, sdaddr, data, dsize))) {
+			DHD_ERROR(("%s: membytes transfer failed\n", __FUNCTION__));
+			break;
+		}
+
+		/* Adjust for next transfer (if any) */
+		if ((size -= dsize)) {
+			data += dsize;
+			address += dsize;
+			if ((bcmerror = dhdsdio_set_siaddr_window(bus, address))) {
+				DHD_ERROR(("%s: window change failed\n", __FUNCTION__));
+				break;
+			}
+			sdaddr = 0;
+			dsize = MIN(SBSDIO_SB_OFT_ADDR_LIMIT, size);
+		}
+
+	}
+
+xfer_done:
+	/* Return the window to backplane enumeration space for core access */
+	if (dhdsdio_set_siaddr_window(bus, bcmsdh_cur_sbwad(bus->sdh))) {
+		DHD_ERROR(("%s: FAILED to set window back to 0x%x\n", __FUNCTION__,
+			bcmsdh_cur_sbwad(bus->sdh)));
+	}
+
+	return bcmerror;
+}
+
+#ifdef DHD_DEBUG
+static int
+dhdsdio_readshared(dhd_bus_t *bus, sdpcm_shared_t *sh)
+{
+	uint32 addr;
+	int rv;
+
+	/* Read last word in memory to determine address of sdpcm_shared structure */
+	if ((rv = dhdsdio_membytes(bus, FALSE, bus->ramsize - 4, (uint8 *)&addr, 4)) < 0)
+		return rv;
+
+	addr = ltoh32(addr);
+
+	DHD_INFO(("sdpcm_shared address 0x%08X\n", addr));
+
+	/*
+	 * Check if addr is valid.
+	 * NVRAM length at the end of memory should have been overwritten.
+	 */
+	if (addr == 0 || ((~addr >> 16) & 0xffff) == (addr & 0xffff)) {
+		DHD_ERROR(("%s: address (0x%08x) of sdpcm_shared invalid\n", __FUNCTION__, addr));
+		return BCME_ERROR;
+	}
+
+	/* Read hndrte_shared structure */
+	if ((rv = dhdsdio_membytes(bus, FALSE, addr, (uint8 *)sh, sizeof(sdpcm_shared_t))) < 0)
+		return rv;
+
+	/* Endianness */
+	sh->flags = ltoh32(sh->flags);
+	sh->trap_addr = ltoh32(sh->trap_addr);
+	sh->assert_exp_addr = ltoh32(sh->assert_exp_addr);
+	sh->assert_file_addr = ltoh32(sh->assert_file_addr);
+	sh->assert_line = ltoh32(sh->assert_line);
+	sh->console_addr = ltoh32(sh->console_addr);
+	sh->msgtrace_addr = ltoh32(sh->msgtrace_addr);
+
+	if ((sh->flags & SDPCM_SHARED_VERSION_MASK) == 3 && SDPCM_SHARED_VERSION == 1)
+		return BCME_OK;
+
+	if ((sh->flags & SDPCM_SHARED_VERSION_MASK) != SDPCM_SHARED_VERSION) {
+		DHD_ERROR(("%s: sdpcm_shared version %d in dhd "
+		           "is different than sdpcm_shared version %d in dongle\n",
+		           __FUNCTION__, SDPCM_SHARED_VERSION,
+		           sh->flags & SDPCM_SHARED_VERSION_MASK));
+		return BCME_ERROR;
+	}
+
+	return BCME_OK;
+}
+
+
+static int
+dhdsdio_readconsole(dhd_bus_t *bus)
+{
+	dhd_console_t *c = &bus->console;
+	uint8 line[CONSOLE_LINE_MAX], ch;
+	uint32 n, idx, addr;
+	int rv;
+
+	/* Don't do anything until FWREADY updates console address */
+	if (bus->console_addr == 0)
+		return 0;
+
+	/* Read console log struct */
+	addr = bus->console_addr + OFFSETOF(hndrte_cons_t, log);
+	if ((rv = dhdsdio_membytes(bus, FALSE, addr, (uint8 *)&c->log, sizeof(c->log))) < 0)
+		return rv;
+
+	/* Allocate console buffer (one time only) */
+	if (c->buf == NULL) {
+		c->bufsize = ltoh32(c->log.buf_size);
+		if ((c->buf = MALLOC(bus->dhd->osh, c->bufsize)) == NULL)
+			return BCME_NOMEM;
+	}
+
+	idx = ltoh32(c->log.idx);
+
+	/* Protect against corrupt value */
+	if (idx > c->bufsize)
+		return BCME_ERROR;
+
+	/* Skip reading the console buffer if the index pointer has not moved */
+	if (idx == c->last)
+		return BCME_OK;
+
+	/* Read the console buffer */
+	addr = ltoh32(c->log.buf);
+	if ((rv = dhdsdio_membytes(bus, FALSE, addr, c->buf, c->bufsize)) < 0)
+		return rv;
+
+	while (c->last != idx) {
+		for (n = 0; n < CONSOLE_LINE_MAX - 2; n++) {
+			if (c->last == idx) {
+				/* This would output a partial line.  Instead, back up
+				 * the buffer pointer and output this line next time around.
+				 */
+				if (c->last >= n)
+					c->last -= n;
+				else
+					c->last = c->bufsize - n;
+				goto break2;
+			}
+			ch = c->buf[c->last];
+			c->last = (c->last + 1) % c->bufsize;
+			if (ch == '\n')
+				break;
+			line[n] = ch;
+		}
+
+		if (n > 0) {
+			if (line[n - 1] == '\r')
+				n--;
+			line[n] = 0;
+			printf("CONSOLE: %s\n", line);
+		}
+	}
+break2:
+
+	return BCME_OK;
+}
+
+static int
+dhdsdio_checkdied(dhd_bus_t *bus, char *data, uint size)
+{
+	int bcmerror = 0;
+	uint msize = 512;
+	char *mbuffer = NULL;
+	char *console_buffer = NULL;
+	uint maxstrlen = 256;
+	char *str = NULL;
+	trap_t tr;
+	sdpcm_shared_t sdpcm_shared;
+	struct bcmstrbuf strbuf;
+	uint32 console_ptr, console_size, console_index;
+	uint8 line[CONSOLE_LINE_MAX], ch;
+	uint32 n, i, addr;
+	int rv;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (data == NULL) {
+		/*
+		 * Called after a rx ctrl timeout. "data" is NULL.
+		 * allocate memory to trace the trap or assert.
+		 */
+		size = msize;
+		mbuffer = data = MALLOC(bus->dhd->osh, msize);
+		if (mbuffer == NULL) {
+			DHD_ERROR(("%s: MALLOC(%d) failed \n", __FUNCTION__, msize));
+			bcmerror = BCME_NOMEM;
+			goto done;
+		}
+	}
+
+	if ((str = MALLOC(bus->dhd->osh, maxstrlen)) == NULL) {
+		DHD_ERROR(("%s: MALLOC(%d) failed \n", __FUNCTION__, maxstrlen));
+		bcmerror = BCME_NOMEM;
+		goto done;
+	}
+
+	if ((bcmerror = dhdsdio_readshared(bus, &sdpcm_shared)) < 0)
+		goto done;
+
+	bcm_binit(&strbuf, data, size);
+
+	bcm_bprintf(&strbuf, "msgtrace address : 0x%08X\nconsole address  : 0x%08X\n",
+	            sdpcm_shared.msgtrace_addr, sdpcm_shared.console_addr);
+
+	if ((sdpcm_shared.flags & SDPCM_SHARED_ASSERT_BUILT) == 0) {
+		/* NOTE: Misspelled assert is intentional - DO NOT FIX.
+		 * (Avoids conflict with real asserts for programmatic parsing of output.)
+		 */
+		bcm_bprintf(&strbuf, "Assrt not built in dongle\n");
+	}
+
+	if ((sdpcm_shared.flags & (SDPCM_SHARED_ASSERT|SDPCM_SHARED_TRAP)) == 0) {
+		/* NOTE: Misspelled assert is intentional - DO NOT FIX.
+		 * (Avoids conflict with real asserts for programmatic parsing of output.)
+		 */
+		bcm_bprintf(&strbuf, "No trap%s in dongle",
+		          (sdpcm_shared.flags & SDPCM_SHARED_ASSERT_BUILT)
+		          ?"/assrt" :"");
+	} else {
+		if (sdpcm_shared.flags & SDPCM_SHARED_ASSERT) {
+			/* Download assert */
+			bcm_bprintf(&strbuf, "Dongle assert");
+			if (sdpcm_shared.assert_exp_addr != 0) {
+				str[0] = '\0';
+				if ((bcmerror = dhdsdio_membytes(bus, FALSE,
+				                                 sdpcm_shared.assert_exp_addr,
+				                                 (uint8 *)str, maxstrlen)) < 0)
+					goto done;
+
+				str[maxstrlen - 1] = '\0';
+				bcm_bprintf(&strbuf, " expr \"%s\"", str);
+			}
+
+			if (sdpcm_shared.assert_file_addr != 0) {
+				str[0] = '\0';
+				if ((bcmerror = dhdsdio_membytes(bus, FALSE,
+				                                 sdpcm_shared.assert_file_addr,
+				                                 (uint8 *)str, maxstrlen)) < 0)
+					goto done;
+
+				str[maxstrlen - 1] = '\0';
+				bcm_bprintf(&strbuf, " file \"%s\"", str);
+			}
+
+			bcm_bprintf(&strbuf, " line %d ", sdpcm_shared.assert_line);
+		}
+
+		if (sdpcm_shared.flags & SDPCM_SHARED_TRAP) {
+			if ((bcmerror = dhdsdio_membytes(bus, FALSE,
+			                                 sdpcm_shared.trap_addr,
+			                                 (uint8*)&tr, sizeof(trap_t))) < 0)
+				goto done;
+
+			bcm_bprintf(&strbuf,
+			"Dongle trap type 0x%x @ epc 0x%x, cpsr 0x%x, spsr 0x%x, sp 0x%x,"
+			"lp 0x%x, rpc 0x%x Trap offset 0x%x, "
+			"r0 0x%x, r1 0x%x, r2 0x%x, r3 0x%x, "
+			"r4 0x%x, r5 0x%x, r6 0x%x, r7 0x%x\n\n",
+			ltoh32(tr.type), ltoh32(tr.epc), ltoh32(tr.cpsr), ltoh32(tr.spsr),
+			ltoh32(tr.r13), ltoh32(tr.r14), ltoh32(tr.pc),
+			ltoh32(sdpcm_shared.trap_addr),
+			ltoh32(tr.r0), ltoh32(tr.r1), ltoh32(tr.r2), ltoh32(tr.r3),
+			ltoh32(tr.r4), ltoh32(tr.r5), ltoh32(tr.r6), ltoh32(tr.r7));
+
+			addr = sdpcm_shared.console_addr + OFFSETOF(hndrte_cons_t, log);
+			if ((rv = dhdsdio_membytes(bus, FALSE, addr,
+				(uint8 *)&console_ptr, sizeof(console_ptr))) < 0)
+				goto printbuf;
+
+			addr = sdpcm_shared.console_addr + OFFSETOF(hndrte_cons_t, log.buf_size);
+			if ((rv = dhdsdio_membytes(bus, FALSE, addr,
+				(uint8 *)&console_size, sizeof(console_size))) < 0)
+				goto printbuf;
+
+			addr = sdpcm_shared.console_addr + OFFSETOF(hndrte_cons_t, log.idx);
+			if ((rv = dhdsdio_membytes(bus, FALSE, addr,
+				(uint8 *)&console_index, sizeof(console_index))) < 0)
+				goto printbuf;
+
+			console_ptr = ltoh32(console_ptr);
+			console_size = ltoh32(console_size);
+			console_index = ltoh32(console_index);
+
+			if (console_size > CONSOLE_BUFFER_MAX ||
+				!(console_buffer = MALLOC(bus->dhd->osh, console_size)))
+				goto printbuf;
+
+			if ((rv = dhdsdio_membytes(bus, FALSE, console_ptr,
+				(uint8 *)console_buffer, console_size)) < 0)
+				goto printbuf;
+
+			for (i = 0, n = 0; i < console_size; i += n + 1) {
+				for (n = 0; n < CONSOLE_LINE_MAX - 2; n++) {
+					ch = console_buffer[(console_index + i + n) % console_size];
+					if (ch == '\n')
+						break;
+					line[n] = ch;
+				}
+
+
+				if (n > 0) {
+					if (line[n - 1] == '\r')
+						n--;
+					line[n] = 0;
+					/* Don't use DHD_ERROR macro since we print
+					 * a lot of information quickly. The macro
+					 * will truncate a lot of the printfs
+					 */
+
+					if (dhd_msg_level & DHD_ERROR_VAL) {
+						printf("CONSOLE: %s\n", line);
+						DHD_BLOG(line, strlen(line) + 1);
+					}
+				}
+			}
+		}
+	}
+
+printbuf:
+	if (sdpcm_shared.flags & (SDPCM_SHARED_ASSERT | SDPCM_SHARED_TRAP)) {
+		DHD_ERROR(("%s: %s\n", __FUNCTION__, strbuf.origbuf));
+	}
+
+
+done:
+	if (mbuffer)
+		MFREE(bus->dhd->osh, mbuffer, msize);
+	if (str)
+		MFREE(bus->dhd->osh, str, maxstrlen);
+	if (console_buffer)
+		MFREE(bus->dhd->osh, console_buffer, console_size);
+
+	return bcmerror;
+}
+#endif /* #ifdef DHD_DEBUG */
+
+
+int
+dhdsdio_downloadvars(dhd_bus_t *bus, void *arg, int len)
+{
+	int bcmerror = BCME_OK;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	/* Basic sanity checks */
+	if (bus->dhd->up) {
+		bcmerror = BCME_NOTDOWN;
+		goto err;
+	}
+	if (!len) {
+		bcmerror = BCME_BUFTOOSHORT;
+		goto err;
+	}
+
+	/* Free the old ones and replace with passed variables */
+	if (bus->vars)
+		MFREE(bus->dhd->osh, bus->vars, bus->varsz);
+
+	bus->vars = MALLOC(bus->dhd->osh, len);
+	bus->varsz = bus->vars ? len : 0;
+	if (bus->vars == NULL) {
+		bcmerror = BCME_NOMEM;
+		goto err;
+	}
+
+	/* Copy the passed variables, which should include the terminating double-null */
+	bcopy(arg, bus->vars, bus->varsz);
+err:
+	return bcmerror;
+}
+
+#ifdef DHD_DEBUG
+
+#define CC_PLL_CHIPCTRL_SERIAL_ENAB	(1  << 24)
+static int
+dhd_serialconsole(dhd_bus_t *bus, bool set, bool enable, int *bcmerror)
+{
+	int int_val;
+	uint32 addr, data;
+
+
+	addr = SI_ENUM_BASE + OFFSETOF(chipcregs_t, chipcontrol_addr);
+	data = SI_ENUM_BASE + OFFSETOF(chipcregs_t, chipcontrol_data);
+	*bcmerror = 0;
+
+	bcmsdh_reg_write(bus->sdh, addr, 4, 1);
+	if (bcmsdh_regfail(bus->sdh)) {
+		*bcmerror = BCME_SDIO_ERROR;
+		return -1;
+	}
+	int_val = bcmsdh_reg_read(bus->sdh, data, 4);
+	if (bcmsdh_regfail(bus->sdh)) {
+		*bcmerror = BCME_SDIO_ERROR;
+		return -1;
+	}
+	if (!set)
+		return (int_val & CC_PLL_CHIPCTRL_SERIAL_ENAB);
+	if (enable)
+		int_val |= CC_PLL_CHIPCTRL_SERIAL_ENAB;
+	else
+		int_val &= ~CC_PLL_CHIPCTRL_SERIAL_ENAB;
+	bcmsdh_reg_write(bus->sdh, data, 4, int_val);
+	if (bcmsdh_regfail(bus->sdh)) {
+		*bcmerror = BCME_SDIO_ERROR;
+		return -1;
+	}
+	if (bus->sih->chip == BCM4330_CHIP_ID) {
+		uint32 chipcontrol;
+		addr = SI_ENUM_BASE + OFFSETOF(chipcregs_t, chipcontrol);
+		chipcontrol = bcmsdh_reg_read(bus->sdh, addr, 4);
+		chipcontrol &= ~0x8;
+		if (enable) {
+			chipcontrol |=  0x8;
+			chipcontrol &= ~0x3;
+		}
+		bcmsdh_reg_write(bus->sdh, addr, 4, chipcontrol);
+	}
+
+	return (int_val & CC_PLL_CHIPCTRL_SERIAL_ENAB);
+}
+#endif 
+
+static int
+dhdsdio_doiovar(dhd_bus_t *bus, const bcm_iovar_t *vi, uint32 actionid, const char *name,
+                void *params, int plen, void *arg, int len, int val_size)
+{
+	int bcmerror = 0;
+	int32 int_val = 0;
+	bool bool_val = 0;
+
+	DHD_ERROR(("%s: Enter, action %d name %s params %p plen %d arg %p len %d val_size %d\n",
+	           __FUNCTION__, actionid, name, params, plen, arg, len, val_size));
+
+	if ((bcmerror = bcm_iovar_lencheck(vi, arg, len, IOV_ISSET(actionid))) != 0)
+		goto exit;
+
+	if (plen >= (int)sizeof(int_val))
+		bcopy(params, &int_val, sizeof(int_val));
+
+	bool_val = (int_val != 0) ? TRUE : FALSE;
+
+
+	/* Some ioctls use the bus */
+	dhd_os_sdlock(bus->dhd);
+
+	/* Check if dongle is in reset. If so, only allow DEVRESET iovars */
+	if (bus->dhd->dongle_reset && !(actionid == IOV_SVAL(IOV_DEVRESET) ||
+	                                actionid == IOV_GVAL(IOV_DEVRESET))) {
+		bcmerror = BCME_NOTREADY;
+		goto exit;
+	}
+
+	/* Handle sleep stuff before any clock mucking */
+	if (vi->varid == IOV_SLEEP) {
+		if (IOV_ISSET(actionid)) {
+			bcmerror = dhdsdio_bussleep(bus, bool_val);
+		} else {
+			int_val = (int32)bus->sleeping;
+			bcopy(&int_val, arg, val_size);
+		}
+		goto exit;
+	}
+
+	/* Request clock to allow SDIO accesses */
+	if (!bus->dhd->dongle_reset) {
+		BUS_WAKE(bus);
+		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+	}
+
+	switch (actionid) {
+	case IOV_GVAL(IOV_INTR):
+		int_val = (int32)bus->intr;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_INTR):
+		bus->intr = bool_val;
+		bus->intdis = FALSE;
+		if (bus->dhd->up) {
+			if (bus->intr) {
+				DHD_INTR(("%s: enable SDIO device interrupts\n", __FUNCTION__));
+				bcmsdh_intr_enable(bus->sdh);
+			} else {
+				DHD_INTR(("%s: disable SDIO interrupts\n", __FUNCTION__));
+				bcmsdh_intr_disable(bus->sdh);
+			}
+		}
+		break;
+
+	case IOV_GVAL(IOV_POLLRATE):
+		int_val = (int32)bus->pollrate;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_POLLRATE):
+		bus->pollrate = (uint)int_val;
+		bus->poll = (bus->pollrate != 0);
+		break;
+
+	case IOV_GVAL(IOV_IDLETIME):
+		int_val = bus->idletime;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_IDLETIME):
+		if ((int_val < 0) && (int_val != DHD_IDLE_IMMEDIATE)) {
+			bcmerror = BCME_BADARG;
+		} else {
+			bus->idletime = int_val;
+		}
+		break;
+
+	case IOV_GVAL(IOV_IDLECLOCK):
+		int_val = (int32)bus->idleclock;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_IDLECLOCK):
+		bus->idleclock = int_val;
+		break;
+
+	case IOV_GVAL(IOV_SD1IDLE):
+		int_val = (int32)sd1idle;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_SD1IDLE):
+		sd1idle = bool_val;
+		break;
+
+
+	case IOV_SVAL(IOV_MEMBYTES):
+	case IOV_GVAL(IOV_MEMBYTES):
+	{
+		uint32 address;
+		uint size, dsize;
+		uint8 *data;
+
+		bool set = (actionid == IOV_SVAL(IOV_MEMBYTES));
+
+		ASSERT(plen >= 2*sizeof(int));
+
+		address = (uint32)int_val;
+		bcopy((char *)params + sizeof(int_val), &int_val, sizeof(int_val));
+		size = (uint)int_val;
+
+		/* Do some validation */
+		dsize = set ? plen - (2 * sizeof(int)) : len;
+		if (dsize < size) {
+			DHD_ERROR(("%s: error on %s membytes, addr 0x%08x size %d dsize %d\n",
+			           __FUNCTION__, (set ? "set" : "get"), address, size, dsize));
+			bcmerror = BCME_BADARG;
+			break;
+		}
+
+		DHD_INFO(("%s: Request to %s %d bytes at address 0x%08x\n", __FUNCTION__,
+		          (set ? "write" : "read"), size, address));
+
+		/* If we know about SOCRAM, check for a fit */
+		if ((bus->orig_ramsize) &&
+		    ((address > bus->orig_ramsize) || (address + size > bus->orig_ramsize)))
+		{
+			uint8 enable, protect;
+			si_socdevram(bus->sih, FALSE, &enable, &protect);
+			if (!enable || protect) {
+				DHD_ERROR(("%s: ramsize 0x%08x doesn't have %d bytes at 0x%08x\n",
+					__FUNCTION__, bus->orig_ramsize, size, address));
+				DHD_ERROR(("%s: socram enable %d, protect %d\n",
+					__FUNCTION__, enable, protect));
+				bcmerror = BCME_BADARG;
+				break;
+			}
+			if (enable && (bus->sih->chip == BCM4330_CHIP_ID)) {
+				uint32 devramsize = si_socdevram_size(bus->sih);
+				if ((address < SOCDEVRAM_4330_ARM_ADDR) ||
+					(address + size > (SOCDEVRAM_4330_ARM_ADDR + devramsize))) {
+					DHD_ERROR(("%s: bad address 0x%08x, size 0x%08x\n",
+						__FUNCTION__, address, size));
+					DHD_ERROR(("%s: socram range 0x%08x,size 0x%08x\n",
+						__FUNCTION__, SOCDEVRAM_4330_ARM_ADDR, devramsize));
+					bcmerror = BCME_BADARG;
+					break;
+				}
+				/* move it such that address is real now */
+				address -= SOCDEVRAM_4330_ARM_ADDR;
+				address += SOCDEVRAM_4330_BP_ADDR;
+				DHD_INFO(("%s: Request to %s %d bytes @ Mapped address 0x%08x\n",
+					__FUNCTION__, (set ? "write" : "read"), size, address));
+			}
+		}
+
+		/* Generate the actual data pointer */
+		data = set ? (uint8*)params + 2 * sizeof(int): (uint8*)arg;
+
+		/* Call to do the transfer */
+		bcmerror = dhdsdio_membytes(bus, set, address, data, size);
+
+		break;
+	}
+
+	case IOV_GVAL(IOV_MEMSIZE):
+		int_val = (int32)bus->ramsize;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_GVAL(IOV_SDIOD_DRIVE):
+		int_val = (int32)dhd_sdiod_drive_strength;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_SDIOD_DRIVE):
+		dhd_sdiod_drive_strength = int_val;
+		si_sdiod_drive_strength_init(bus->sih, bus->dhd->osh, dhd_sdiod_drive_strength);
+		break;
+
+	case IOV_SVAL(IOV_DOWNLOAD):
+		bcmerror = dhdsdio_download_state(bus, bool_val);
+		break;
+
+	case IOV_SVAL(IOV_SOCRAM_STATE):
+		bcmerror = dhdsdio_download_state(bus, bool_val);
+		break;
+
+	case IOV_SVAL(IOV_VARS):
+		bcmerror = dhdsdio_downloadvars(bus, arg, len);
+		break;
+
+	case IOV_GVAL(IOV_READAHEAD):
+		int_val = (int32)dhd_readahead;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_READAHEAD):
+		if (bool_val && !dhd_readahead)
+			bus->nextlen = 0;
+		dhd_readahead = bool_val;
+		break;
+
+	case IOV_GVAL(IOV_SDRXCHAIN):
+		int_val = (int32)bus->use_rxchain;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_SDRXCHAIN):
+		if (bool_val && !bus->sd_rxchain)
+			bcmerror = BCME_UNSUPPORTED;
+		else
+			bus->use_rxchain = bool_val;
+		break;
+	case IOV_GVAL(IOV_ALIGNCTL):
+		int_val = (int32)dhd_alignctl;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_ALIGNCTL):
+		dhd_alignctl = bool_val;
+		break;
+
+	case IOV_GVAL(IOV_SDALIGN):
+		int_val = DHD_SDALIGN;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+#ifdef DHD_DEBUG
+	case IOV_GVAL(IOV_VARS):
+		if (bus->varsz < (uint)len)
+			bcopy(bus->vars, arg, bus->varsz);
+		else
+			bcmerror = BCME_BUFTOOSHORT;
+		break;
+#endif /* DHD_DEBUG */
+
+#ifdef DHD_DEBUG
+	case IOV_GVAL(IOV_SDREG):
+	{
+		sdreg_t *sd_ptr;
+		uint32 addr, size;
+
+		sd_ptr = (sdreg_t *)params;
+
+		addr = (uintptr)bus->regs + sd_ptr->offset;
+		size = sd_ptr->func;
+		int_val = (int32)bcmsdh_reg_read(bus->sdh, addr, size);
+		if (bcmsdh_regfail(bus->sdh))
+			bcmerror = BCME_SDIO_ERROR;
+		bcopy(&int_val, arg, sizeof(int32));
+		break;
+	}
+
+	case IOV_SVAL(IOV_SDREG):
+	{
+		sdreg_t *sd_ptr;
+		uint32 addr, size;
+
+		sd_ptr = (sdreg_t *)params;
+
+		addr = (uintptr)bus->regs + sd_ptr->offset;
+		size = sd_ptr->func;
+		bcmsdh_reg_write(bus->sdh, addr, size, sd_ptr->value);
+		if (bcmsdh_regfail(bus->sdh))
+			bcmerror = BCME_SDIO_ERROR;
+		break;
+	}
+
+	/* Same as above, but offset is not backplane (not SDIO core) */
+	case IOV_GVAL(IOV_SBREG):
+	{
+		sdreg_t sdreg;
+		uint32 addr, size;
+
+		bcopy(params, &sdreg, sizeof(sdreg));
+
+		addr = SI_ENUM_BASE + sdreg.offset;
+		size = sdreg.func;
+		int_val = (int32)bcmsdh_reg_read(bus->sdh, addr, size);
+		if (bcmsdh_regfail(bus->sdh))
+			bcmerror = BCME_SDIO_ERROR;
+		bcopy(&int_val, arg, sizeof(int32));
+		break;
+	}
+
+	case IOV_SVAL(IOV_SBREG):
+	{
+		sdreg_t sdreg;
+		uint32 addr, size;
+
+		bcopy(params, &sdreg, sizeof(sdreg));
+
+		addr = SI_ENUM_BASE + sdreg.offset;
+		size = sdreg.func;
+		bcmsdh_reg_write(bus->sdh, addr, size, sdreg.value);
+		if (bcmsdh_regfail(bus->sdh))
+			bcmerror = BCME_SDIO_ERROR;
+		break;
+	}
+
+	case IOV_GVAL(IOV_SDCIS):
+	{
+		*(char *)arg = 0;
+
+		bcmstrcat(arg, "\nFunc 0\n");
+		bcmsdh_cis_read(bus->sdh, 0x10, (uint8 *)arg + strlen(arg), SBSDIO_CIS_SIZE_LIMIT);
+		bcmstrcat(arg, "\nFunc 1\n");
+		bcmsdh_cis_read(bus->sdh, 0x11, (uint8 *)arg + strlen(arg), SBSDIO_CIS_SIZE_LIMIT);
+		bcmstrcat(arg, "\nFunc 2\n");
+		bcmsdh_cis_read(bus->sdh, 0x12, (uint8 *)arg + strlen(arg), SBSDIO_CIS_SIZE_LIMIT);
+		break;
+	}
+
+	case IOV_GVAL(IOV_FORCEEVEN):
+		int_val = (int32)forcealign;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_FORCEEVEN):
+		forcealign = bool_val;
+		break;
+
+	case IOV_GVAL(IOV_TXBOUND):
+		int_val = (int32)dhd_txbound;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_TXBOUND):
+		dhd_txbound = (uint)int_val;
+		break;
+
+	case IOV_GVAL(IOV_RXBOUND):
+		int_val = (int32)dhd_rxbound;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_RXBOUND):
+		dhd_rxbound = (uint)int_val;
+		break;
+
+	case IOV_GVAL(IOV_TXMINMAX):
+		int_val = (int32)dhd_txminmax;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_TXMINMAX):
+		dhd_txminmax = (uint)int_val;
+		break;
+
+	case IOV_GVAL(IOV_SERIALCONS):
+		int_val = dhd_serialconsole(bus, FALSE, 0, &bcmerror);
+		if (bcmerror != 0)
+			break;
+
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_SERIALCONS):
+		dhd_serialconsole(bus, TRUE, bool_val, &bcmerror);
+		break;
+
+
+
+#endif /* DHD_DEBUG */
+
+
+#ifdef SDTEST
+	case IOV_GVAL(IOV_EXTLOOP):
+		int_val = (int32)bus->ext_loop;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_EXTLOOP):
+		bus->ext_loop = bool_val;
+		break;
+
+	case IOV_GVAL(IOV_PKTGEN):
+		bcmerror = dhdsdio_pktgen_get(bus, arg);
+		break;
+
+	case IOV_SVAL(IOV_PKTGEN):
+		bcmerror = dhdsdio_pktgen_set(bus, arg);
+		break;
+#endif /* SDTEST */
+
+
+	case IOV_GVAL(IOV_DONGLEISOLATION):
+		int_val = bus->dhd->dongle_isolation;
+		bcopy(&int_val, arg, val_size);
+		break;
+
+	case IOV_SVAL(IOV_DONGLEISOLATION):
+		bus->dhd->dongle_isolation = bool_val;
+		break;
+
+	case IOV_SVAL(IOV_DEVRESET):
+		DHD_TRACE(("%s: Called set IOV_DEVRESET=%d dongle_reset=%d busstate=%d\n",
+		           __FUNCTION__, bool_val, bus->dhd->dongle_reset,
+		           bus->dhd->busstate));
+
+		ASSERT(bus->dhd->osh);
+		/* ASSERT(bus->cl_devid); */
+
+		dhd_bus_devreset(bus->dhd, (uint8)bool_val);
+
+		break;
+#ifdef SOFTAP
+	case IOV_GVAL(IOV_FWPATH):
+	{
+		uint32  fw_path_len;
+
+		fw_path_len = strlen(bus->fw_path);
+		DHD_INFO(("[softap] get fwpath, l=%d\n", len));
+
+		if (fw_path_len > len-1) {
+			bcmerror = BCME_BUFTOOSHORT;
+			break;
+		}
+
+		if (fw_path_len) {
+			bcopy(bus->fw_path, arg, fw_path_len);
+			((uchar*)arg)[fw_path_len] = 0;
+		}
+		break;
+	}
+
+	case IOV_SVAL(IOV_FWPATH):
+		DHD_INFO(("[softap] set fwpath, idx=%d\n", int_val));
+
+		switch (int_val) {
+		case 1:
+			bus->fw_path = fw_path; /* ordinary one */
+			break;
+		case 2:
+			bus->fw_path = fw_path2;
+			break;
+		default:
+			bcmerror = BCME_BADARG;
+			break;
+		}
+
+		DHD_INFO(("[softap] new fw path: %s\n", (bus->fw_path[0] ? bus->fw_path : "NULL")));
+		break;
+
+#endif /* SOFTAP */
+	case IOV_GVAL(IOV_DEVRESET):
+		DHD_TRACE(("%s: Called get IOV_DEVRESET\n", __FUNCTION__));
+
+		/* Get its status */
+		int_val = (bool) bus->dhd->dongle_reset;
+		bcopy(&int_val, arg, val_size);
+
+		break;
+
+	default:
+		bcmerror = BCME_UNSUPPORTED;
+		break;
+	}
+
+exit:
+	if ((bus->idletime == DHD_IDLE_IMMEDIATE) && !bus->dpc_sched) {
+		bus->activity = FALSE;
+		dhdsdio_clkctl(bus, CLK_NONE, TRUE);
+	}
+
+	dhd_os_sdunlock(bus->dhd);
+
+	if (actionid == IOV_SVAL(IOV_DEVRESET) && bool_val == FALSE)
+		dhd_preinit_ioctls((dhd_pub_t *) bus->dhd);
+
+	return bcmerror;
+}
+
+static int
+dhdsdio_write_vars(dhd_bus_t *bus)
+{
+	int bcmerror = 0;
+	uint32 varsize;
+	uint32 varaddr;
+	uint8 *vbuffer;
+	uint32 varsizew;
+#ifdef DHD_DEBUG
+	uint8 *nvram_ularray;
+#endif /* DHD_DEBUG */
+
+	/* Even if there are no vars are to be written, we still need to set the ramsize. */
+	varsize = bus->varsz ? ROUNDUP(bus->varsz, 4) : 0;
+	varaddr = (bus->ramsize - 4) - varsize;
+
+	if (bus->vars) {
+		if ((bus->sih->buscoretype == SDIOD_CORE_ID) && (bus->sdpcmrev == 7)) {
+			if (((varaddr & 0x3C) == 0x3C) && (varsize > 4)) {
+				DHD_ERROR(("PR85623WAR in place\n"));
+				varsize += 4;
+				varaddr -= 4;
+			}
+		}
+
+		vbuffer = (uint8 *)MALLOC(bus->dhd->osh, varsize);
+		if (!vbuffer)
+			return BCME_NOMEM;
+
+		bzero(vbuffer, varsize);
+		bcopy(bus->vars, vbuffer, bus->varsz);
+
+		/* Write the vars list */
+		bcmerror = dhdsdio_membytes(bus, TRUE, varaddr, vbuffer, varsize);
+#ifdef DHD_DEBUG
+		/* Verify NVRAM bytes */
+		DHD_INFO(("Compare NVRAM dl & ul; varsize=%d\n", varsize));
+		nvram_ularray = (uint8*)MALLOC(bus->dhd->osh, varsize);
+		if (!nvram_ularray)
+			return BCME_NOMEM;
+
+		/* Upload image to verify downloaded contents. */
+		memset(nvram_ularray, 0xaa, varsize);
+
+		/* Read the vars list to temp buffer for comparison */
+		bcmerror = dhdsdio_membytes(bus, FALSE, varaddr, nvram_ularray, varsize);
+		if (bcmerror) {
+				DHD_ERROR(("%s: error %d on reading %d nvram bytes at 0x%08x\n",
+					__FUNCTION__, bcmerror, varsize, varaddr));
+		}
+		/* Compare the org NVRAM with the one read from RAM */
+		if (memcmp(vbuffer, nvram_ularray, varsize)) {
+			DHD_ERROR(("%s: Downloaded NVRAM image is corrupted.\n", __FUNCTION__));
+		} else
+			DHD_ERROR(("%s: Download, Upload and compare of NVRAM succeeded.\n",
+			__FUNCTION__));
+
+		MFREE(bus->dhd->osh, nvram_ularray, varsize);
+#endif /* DHD_DEBUG */
+
+		MFREE(bus->dhd->osh, vbuffer, varsize);
+	}
+
+	/* adjust to the user specified RAM */
+	DHD_INFO(("Physical memory size: %d, usable memory size: %d\n",
+		bus->orig_ramsize, bus->ramsize));
+	DHD_INFO(("Vars are at %d, orig varsize is %d\n",
+		varaddr, varsize));
+	varsize = ((bus->orig_ramsize - 4) - varaddr);
+
+	/*
+	 * Determine the length token:
+	 * Varsize, converted to words, in lower 16-bits, checksum in upper 16-bits.
+	 */
+	if (bcmerror) {
+		varsizew = 0;
+	} else {
+		varsizew = varsize / 4;
+		varsizew = (~varsizew << 16) | (varsizew & 0x0000FFFF);
+		varsizew = htol32(varsizew);
+	}
+
+	DHD_INFO(("New varsize is %d, length token=0x%08x\n", varsize, varsizew));
+
+	/* Write the length token to the last word */
+	bcmerror = dhdsdio_membytes(bus, TRUE, (bus->orig_ramsize - 4),
+		(uint8*)&varsizew, 4);
+
+	return bcmerror;
+}
+
+static int
+dhdsdio_download_state(dhd_bus_t *bus, bool enter)
+{
+	uint retries;
+	int bcmerror = 0;
+
+	if (!bus->sih)
+		return BCME_ERROR;
+
+	/* To enter download state, disable ARM and reset SOCRAM.
+	 * To exit download state, simply reset ARM (default is RAM boot).
+	 */
+	if (enter) {
+		bus->alp_only = TRUE;
+
+		if (!(si_setcore(bus->sih, ARM7S_CORE_ID, 0)) &&
+		    !(si_setcore(bus->sih, ARMCM3_CORE_ID, 0))) {
+			DHD_ERROR(("%s: Failed to find ARM core!\n", __FUNCTION__));
+			bcmerror = BCME_ERROR;
+			goto fail;
+		}
+
+		si_core_disable(bus->sih, 0);
+		if (bcmsdh_regfail(bus->sdh)) {
+			bcmerror = BCME_SDIO_ERROR;
+			goto fail;
+		}
+
+		if (!(si_setcore(bus->sih, SOCRAM_CORE_ID, 0))) {
+			DHD_ERROR(("%s: Failed to find SOCRAM core!\n", __FUNCTION__));
+			bcmerror = BCME_ERROR;
+			goto fail;
+		}
+
+		si_core_reset(bus->sih, 0, 0);
+		if (bcmsdh_regfail(bus->sdh)) {
+			DHD_ERROR(("%s: Failure trying reset SOCRAM core?\n", __FUNCTION__));
+			bcmerror = BCME_SDIO_ERROR;
+			goto fail;
+		}
+
+		/* Clear the top bit of memory */
+		if (bus->ramsize) {
+			uint32 zeros = 0;
+			if (dhdsdio_membytes(bus, TRUE, bus->ramsize - 4, (uint8*)&zeros, 4) < 0) {
+				bcmerror = BCME_SDIO_ERROR;
+				goto fail;
+			}
+		}
+	} else {
+		if (!(si_setcore(bus->sih, SOCRAM_CORE_ID, 0))) {
+			DHD_ERROR(("%s: Failed to find SOCRAM core!\n", __FUNCTION__));
+			bcmerror = BCME_ERROR;
+			goto fail;
+		}
+
+		if (!si_iscoreup(bus->sih)) {
+			DHD_ERROR(("%s: SOCRAM core is down after reset?\n", __FUNCTION__));
+			bcmerror = BCME_ERROR;
+			goto fail;
+		}
+
+		if ((bcmerror = dhdsdio_write_vars(bus))) {
+			DHD_ERROR(("%s: could not write vars to RAM\n", __FUNCTION__));
+			goto fail;
+		}
+
+		if (!si_setcore(bus->sih, PCMCIA_CORE_ID, 0) &&
+		    !si_setcore(bus->sih, SDIOD_CORE_ID, 0)) {
+			DHD_ERROR(("%s: Can't change back to SDIO core?\n", __FUNCTION__));
+			bcmerror = BCME_ERROR;
+			goto fail;
+		}
+		W_SDREG(0xFFFFFFFF, &bus->regs->intstatus, retries);
+
+
+		if (!(si_setcore(bus->sih, ARM7S_CORE_ID, 0)) &&
+		    !(si_setcore(bus->sih, ARMCM3_CORE_ID, 0))) {
+			DHD_ERROR(("%s: Failed to find ARM core!\n", __FUNCTION__));
+			bcmerror = BCME_ERROR;
+			goto fail;
+		}
+
+		si_core_reset(bus->sih, 0, 0);
+		if (bcmsdh_regfail(bus->sdh)) {
+			DHD_ERROR(("%s: Failure trying to reset ARM core?\n", __FUNCTION__));
+			bcmerror = BCME_SDIO_ERROR;
+			goto fail;
+		}
+
+		/* Allow HT Clock now that the ARM is running. */
+		bus->alp_only = FALSE;
+
+		bus->dhd->busstate = DHD_BUS_LOAD;
+	}
+
+fail:
+	/* Always return to SDIOD core */
+	if (!si_setcore(bus->sih, PCMCIA_CORE_ID, 0))
+		si_setcore(bus->sih, SDIOD_CORE_ID, 0);
+
+	return bcmerror;
+}
+
+int
+dhd_bus_iovar_op(dhd_pub_t *dhdp, const char *name,
+                 void *params, int plen, void *arg, int len, bool set)
+{
+	dhd_bus_t *bus = dhdp->bus;
+	const bcm_iovar_t *vi = NULL;
+	int bcmerror = 0;
+	int val_size;
+	uint32 actionid;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	ASSERT(name);
+	ASSERT(len >= 0);
+
+	/* Get MUST have return space */
+	ASSERT(set || (arg && len));
+
+	/* Set does NOT take qualifiers */
+	ASSERT(!set || (!params && !plen));
+
+	/* Look up var locally; if not found pass to host driver */
+	if ((vi = bcm_iovar_lookup(dhdsdio_iovars, name)) == NULL) {
+		dhd_os_sdlock(bus->dhd);
+
+		BUS_WAKE(bus);
+
+		/* Turn on clock in case SD command needs backplane */
+		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+
+		bcmerror = bcmsdh_iovar_op(bus->sdh, name, params, plen, arg, len, set);
+
+		/* Check for bus configuration changes of interest */
+
+		/* If it was divisor change, read the new one */
+		if (set && strcmp(name, "sd_divisor") == 0) {
+			if (bcmsdh_iovar_op(bus->sdh, "sd_divisor", NULL, 0,
+			                    &bus->sd_divisor, sizeof(int32), FALSE) != BCME_OK) {
+				bus->sd_divisor = -1;
+				DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, name));
+			} else {
+				DHD_INFO(("%s: noted %s update, value now %d\n",
+				          __FUNCTION__, name, bus->sd_divisor));
+			}
+		}
+		/* If it was a mode change, read the new one */
+		if (set && strcmp(name, "sd_mode") == 0) {
+			if (bcmsdh_iovar_op(bus->sdh, "sd_mode", NULL, 0,
+			                    &bus->sd_mode, sizeof(int32), FALSE) != BCME_OK) {
+				bus->sd_mode = -1;
+				DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, name));
+			} else {
+				DHD_INFO(("%s: noted %s update, value now %d\n",
+				          __FUNCTION__, name, bus->sd_mode));
+			}
+		}
+		/* Similar check for blocksize change */
+		if (set && strcmp(name, "sd_blocksize") == 0) {
+			int32 fnum = 2;
+			if (bcmsdh_iovar_op(bus->sdh, "sd_blocksize", &fnum, sizeof(int32),
+			                    &bus->blocksize, sizeof(int32), FALSE) != BCME_OK) {
+				bus->blocksize = 0;
+				DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, "sd_blocksize"));
+			} else {
+				DHD_INFO(("%s: noted %s update, value now %d\n",
+				          __FUNCTION__, "sd_blocksize", bus->blocksize));
+			}
+		}
+		bus->roundup = MIN(max_roundup, bus->blocksize);
+
+		if ((bus->idletime == DHD_IDLE_IMMEDIATE) && !bus->dpc_sched) {
+			bus->activity = FALSE;
+			dhdsdio_clkctl(bus, CLK_NONE, TRUE);
+		}
+
+		dhd_os_sdunlock(bus->dhd);
+		goto exit;
+	}
+
+	DHD_CTL(("%s: %s %s, len %d plen %d\n", __FUNCTION__,
+	         name, (set ? "set" : "get"), len, plen));
+
+	/* set up 'params' pointer in case this is a set command so that
+	 * the convenience int and bool code can be common to set and get
+	 */
+	if (params == NULL) {
+		params = arg;
+		plen = len;
+	}
+
+	if (vi->type == IOVT_VOID)
+		val_size = 0;
+	else if (vi->type == IOVT_BUFFER)
+		val_size = len;
+	else
+		/* all other types are integer sized */
+		val_size = sizeof(int);
+
+	actionid = set ? IOV_SVAL(vi->varid) : IOV_GVAL(vi->varid);
+	bcmerror = dhdsdio_doiovar(bus, vi, actionid, name, params, plen, arg, len, val_size);
+
+exit:
+	return bcmerror;
+}
+
+void
+dhd_bus_stop(struct dhd_bus *bus, bool enforce_mutex)
+{
+	osl_t *osh;
+	uint32 local_hostintmask;
+	uint8 saveclk;
+	uint retries;
+	int err;
+	if (!bus->dhd)
+		return;
+
+	osh = bus->dhd->osh;
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	bcmsdh_waitlockfree(NULL);
+
+	if (enforce_mutex)
+		dhd_os_sdlock(bus->dhd);
+
+	BUS_WAKE(bus);
+
+	/* Change our idea of bus state */
+	bus->dhd->busstate = DHD_BUS_DOWN;
+
+	/* Enable clock for device interrupts */
+	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+
+	/* Disable and clear interrupts at the chip level also */
+	W_SDREG(0, &bus->regs->hostintmask, retries);
+	local_hostintmask = bus->hostintmask;
+	bus->hostintmask = 0;
+
+	/* Force clocks on backplane to be sure F2 interrupt propagates */
+	saveclk = bcmsdh_cfg_read(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, &err);
+	if (!err) {
+		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
+		                 (saveclk | SBSDIO_FORCE_HT), &err);
+	}
+	if (err) {
+		DHD_ERROR(("%s: Failed to force clock for F2: err %d\n", __FUNCTION__, err));
+	}
+
+	/* Turn off the bus (F2), free any pending packets */
+	DHD_INTR(("%s: disable SDIO interrupts\n", __FUNCTION__));
+	bcmsdh_intr_disable(bus->sdh);
+	bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_0, SDIOD_CCCR_IOEN, SDIO_FUNC_ENABLE_1, NULL);
+
+	/* Clear any pending interrupts now that F2 is disabled */
+	W_SDREG(local_hostintmask, &bus->regs->intstatus, retries);
+
+	/* Turn off the backplane clock (only) */
+	dhdsdio_clkctl(bus, CLK_SDONLY, FALSE);
+
+	/* Clear the data packet queues */
+	pktq_flush(osh, &bus->txq, TRUE, NULL, 0);
+
+	/* Clear any held glomming stuff */
+	if (bus->glomd)
+		PKTFREE(osh, bus->glomd, FALSE);
+
+	if (bus->glom)
+		PKTFREE(osh, bus->glom, FALSE);
+
+	bus->glom = bus->glomd = NULL;
+
+	/* Clear rx control and wake any waiters */
+	bus->rxlen = 0;
+	dhd_os_ioctl_resp_wake(bus->dhd);
+
+	/* Reset some F2 state stuff */
+	bus->rxskip = FALSE;
+	bus->tx_seq = bus->rx_seq = 0;
+
+	/* Set to a safe default.  It gets updated when we
+	 * receive a packet from the fw but when we reset,
+	 * we need a safe default to be able to send the
+	 * initial mac address.
+	 */
+	bus->tx_max = 4;
+
+	if (enforce_mutex)
+		dhd_os_sdunlock(bus->dhd);
+}
+
+
+int
+dhd_bus_init(dhd_pub_t *dhdp, bool enforce_mutex)
+{
+	dhd_bus_t *bus = dhdp->bus;
+	dhd_timeout_t tmo;
+	uint retries = 0;
+	uint8 ready, enable;
+	int err, ret = 0;
+	uint8 saveclk;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	ASSERT(bus->dhd);
+	if (!bus->dhd)
+		return 0;
+
+	if (enforce_mutex)
+		dhd_os_sdlock(bus->dhd);
+
+	/* Make sure backplane clock is on, needed to generate F2 interrupt */
+	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+	if (bus->clkstate != CLK_AVAIL) {
+		DHD_ERROR(("%s: clock state is wrong. state = %d\n", __FUNCTION__, bus->clkstate));
+		goto exit;
+	}
+
+
+	/* Force clocks on backplane to be sure F2 interrupt propagates */
+	saveclk = bcmsdh_cfg_read(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, &err);
+	if (!err) {
+		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
+		                 (saveclk | SBSDIO_FORCE_HT), &err);
+	}
+	if (err) {
+		DHD_ERROR(("%s: Failed to force clock for F2: err %d\n", __FUNCTION__, err));
+		goto exit;
+	}
+
+	/* Enable function 2 (frame transfers) */
+	W_SDREG((SDPCM_PROT_VERSION << SMB_DATA_VERSION_SHIFT),
+	        &bus->regs->tosbmailboxdata, retries);
+	enable = (SDIO_FUNC_ENABLE_1 | SDIO_FUNC_ENABLE_2);
+
+	bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_0, SDIOD_CCCR_IOEN, enable, NULL);
+
+	/* Give the dongle some time to do its thing and set IOR2 */
+	dhd_timeout_start(&tmo, DHD_WAIT_F2RDY * 1000);
+
+	ready = 0;
+	do {
+		ready = bcmsdh_cfg_read(bus->sdh, SDIO_FUNC_0, SDIOD_CCCR_IORDY, NULL);
+	} while (ready != enable && !dhd_timeout_expired(&tmo));
+
+	DHD_INFO(("%s: enable 0x%02x, ready 0x%02x (waited %uus)\n",
+	          __FUNCTION__, enable, ready, tmo.elapsed));
+
+
+	/* If F2 successfully enabled, set core and enable interrupts */
+	if (ready == enable) {
+		/* Make sure we're talking to the core. */
+		if (!(bus->regs = si_setcore(bus->sih, PCMCIA_CORE_ID, 0)))
+			bus->regs = si_setcore(bus->sih, SDIOD_CORE_ID, 0);
+		ASSERT(bus->regs != NULL);
+
+		/* Set up the interrupt mask and enable interrupts */
+		bus->hostintmask = HOSTINTMASK;
+		/* corerev 4 could use the newer interrupt logic to detect the frames */
+		if ((bus->sih->buscoretype == SDIOD_CORE_ID) && (bus->sdpcmrev == 4) &&
+			(bus->rxint_mode != SDIO_DEVICE_HMB_RXINT)) {
+			bus->hostintmask &= ~I_HMB_FRAME_IND;
+			bus->hostintmask |= I_XMTDATA_AVAIL;
+		}
+		W_SDREG(bus->hostintmask, &bus->regs->hostintmask, retries);
+
+		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_WATERMARK, (uint8)watermark, &err);
+
+		/* Set bus state according to enable result */
+		dhdp->busstate = DHD_BUS_DATA;
+
+		/* bcmsdh_intr_unmask(bus->sdh); */
+
+		bus->intdis = FALSE;
+		if (bus->intr) {
+			DHD_INTR(("%s: enable SDIO device interrupts\n", __FUNCTION__));
+			bcmsdh_intr_enable(bus->sdh);
+		} else {
+			DHD_INTR(("%s: disable SDIO interrupts\n", __FUNCTION__));
+			bcmsdh_intr_disable(bus->sdh);
+		}
+
+	}
+
+
+	else {
+		/* Disable F2 again */
+		enable = SDIO_FUNC_ENABLE_1;
+		bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_0, SDIOD_CCCR_IOEN, enable, NULL);
+	}
+
+	/* Restore previous clock setting */
+	bcmsdh_cfg_write(bus->sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, saveclk, &err);
+
+
+	/* If we didn't come up, turn off backplane clock */
+	if (dhdp->busstate != DHD_BUS_DATA)
+		dhdsdio_clkctl(bus, CLK_NONE, FALSE);
+
+exit:
+	if (enforce_mutex)
+		dhd_os_sdunlock(bus->dhd);
+
+	return ret;
+}
+
+static void
+dhdsdio_rxfail(dhd_bus_t *bus, bool abort, bool rtx)
+{
+	bcmsdh_info_t *sdh = bus->sdh;
+	sdpcmd_regs_t *regs = bus->regs;
+	uint retries = 0;
+	uint16 lastrbc;
+	uint8 hi, lo;
+	int err;
+
+	DHD_ERROR(("%s: %sterminate frame%s\n", __FUNCTION__,
+	           (abort ? "abort command, " : ""), (rtx ? ", send NAK" : "")));
+
+	if (abort) {
+		bcmsdh_abort(sdh, SDIO_FUNC_2);
+	}
+
+	bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_FRAMECTRL, SFC_RF_TERM, &err);
+	bus->f1regdata++;
+
+	/* Wait until the packet has been flushed (device/FIFO stable) */
+	for (lastrbc = retries = 0xffff; retries > 0; retries--) {
+		hi = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_RFRAMEBCHI, NULL);
+		lo = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_RFRAMEBCLO, NULL);
+		bus->f1regdata += 2;
+
+		if ((hi == 0) && (lo == 0))
+			break;
+
+		if ((hi > (lastrbc >> 8)) && (lo > (lastrbc & 0x00ff))) {
+			DHD_ERROR(("%s: count growing: last 0x%04x now 0x%04x\n",
+			           __FUNCTION__, lastrbc, ((hi << 8) + lo)));
+		}
+		lastrbc = (hi << 8) + lo;
+	}
+
+	if (!retries) {
+		DHD_ERROR(("%s: count never zeroed: last 0x%04x\n", __FUNCTION__, lastrbc));
+	} else {
+		DHD_INFO(("%s: flush took %d iterations\n", __FUNCTION__, (0xffff - retries)));
+	}
+
+	if (rtx) {
+		bus->rxrtx++;
+		W_SDREG(SMB_NAK, &regs->tosbmailbox, retries);
+		bus->f1regdata++;
+		if (retries <= retry_limit) {
+			bus->rxskip = TRUE;
+		}
+	}
+
+	/* Clear partial in any case */
+	bus->nextlen = 0;
+
+	/* If we can't reach the device, signal failure */
+	if (err || bcmsdh_regfail(sdh))
+		bus->dhd->busstate = DHD_BUS_DOWN;
+}
+
+static void
+dhdsdio_read_control(dhd_bus_t *bus, uint8 *hdr, uint len, uint doff)
+{
+	bcmsdh_info_t *sdh = bus->sdh;
+	uint rdlen, pad;
+
+	int sdret;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	/* Control data already received in aligned rxctl */
+	if ((bus->bus == SPI_BUS) && (!bus->usebufpool))
+		goto gotpkt;
+
+	ASSERT(bus->rxbuf);
+	/* Set rxctl for frame (w/optional alignment) */
+	bus->rxctl = bus->rxbuf;
+	if (dhd_alignctl) {
+		bus->rxctl += firstread;
+		if ((pad = ((uintptr)bus->rxctl % DHD_SDALIGN)))
+			bus->rxctl += (DHD_SDALIGN - pad);
+		bus->rxctl -= firstread;
+	}
+	ASSERT(bus->rxctl >= bus->rxbuf);
+
+	/* Copy the already-read portion over */
+	bcopy(hdr, bus->rxctl, firstread);
+	if (len <= firstread)
+		goto gotpkt;
+
+	/* Copy the full data pkt in gSPI case and process ioctl. */
+	if (bus->bus == SPI_BUS) {
+		bcopy(hdr, bus->rxctl, len);
+		goto gotpkt;
+	}
+
+	/* Raise rdlen to next SDIO block to avoid tail command */
+	rdlen = len - firstread;
+	if (bus->roundup && bus->blocksize && (rdlen > bus->blocksize)) {
+		pad = bus->blocksize - (rdlen % bus->blocksize);
+		if ((pad <= bus->roundup) && (pad < bus->blocksize) &&
+		    ((len + pad) < bus->dhd->maxctl))
+			rdlen += pad;
+	} else if (rdlen % DHD_SDALIGN) {
+		rdlen += DHD_SDALIGN - (rdlen % DHD_SDALIGN);
+	}
+
+	/* Satisfy length-alignment requirements */
+	if (forcealign && (rdlen & (ALIGNMENT - 1)))
+		rdlen = ROUNDUP(rdlen, ALIGNMENT);
+
+	/* Drop if the read is too big or it exceeds our maximum */
+	if ((rdlen + firstread) > bus->dhd->maxctl) {
+		DHD_ERROR(("%s: %d-byte control read exceeds %d-byte buffer\n",
+		           __FUNCTION__, rdlen, bus->dhd->maxctl));
+		bus->dhd->rx_errors++;
+		dhdsdio_rxfail(bus, FALSE, FALSE);
+		goto done;
+	}
+
+	if ((len - doff) > bus->dhd->maxctl) {
+		DHD_ERROR(("%s: %d-byte ctl frame (%d-byte ctl data) exceeds %d-byte limit\n",
+		           __FUNCTION__, len, (len - doff), bus->dhd->maxctl));
+		bus->dhd->rx_errors++; bus->rx_toolong++;
+		dhdsdio_rxfail(bus, FALSE, FALSE);
+		goto done;
+	}
+
+
+	/* Read remainder of frame body into the rxctl buffer */
+	sdret = dhd_bcmsdh_recv_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
+	                            (bus->rxctl + firstread), rdlen, NULL, NULL, NULL);
+	bus->f2rxdata++;
+	ASSERT(sdret != BCME_PENDING);
+
+	/* Control frame failures need retransmission */
+	if (sdret < 0) {
+		DHD_ERROR(("%s: read %d control bytes failed: %d\n", __FUNCTION__, rdlen, sdret));
+		bus->rxc_errors++; /* dhd.rx_ctlerrs is higher level */
+		dhdsdio_rxfail(bus, TRUE, TRUE);
+		goto done;
+	}
+
+gotpkt:
+
+#ifdef DHD_DEBUG
+	if (DHD_BYTES_ON() && DHD_CTL_ON()) {
+		prhex("RxCtrl", bus->rxctl, len);
+	}
+#endif
+
+	/* Point to valid data and indicate its length */
+	bus->rxctl += doff;
+	bus->rxlen = len - doff;
+
+done:
+	/* Awake any waiters */
+	dhd_os_ioctl_resp_wake(bus->dhd);
+}
+
+static uint8
+dhdsdio_rxglom(dhd_bus_t *bus, uint8 rxseq)
+{
+	uint16 dlen, totlen;
+	uint8 *dptr, num = 0;
+
+	uint16 sublen, check;
+	void *pfirst, *plast, *pnext, *save_pfirst;
+	osl_t *osh = bus->dhd->osh;
+
+	int errcode;
+	uint8 chan, seq, doff, sfdoff;
+	uint8 txmax;
+
+	int ifidx = 0;
+	bool usechain = bus->use_rxchain;
+
+	/* If packets, issue read(s) and send up packet chain */
+	/* Return sequence numbers consumed? */
+
+	DHD_TRACE(("dhdsdio_rxglom: start: glomd %p glom %p\n", bus->glomd, bus->glom));
+
+	/* If there's a descriptor, generate the packet chain */
+	if (bus->glomd) {
+		dhd_os_sdlock_rxq(bus->dhd);
+
+		pfirst = plast = pnext = NULL;
+		dlen = (uint16)PKTLEN(osh, bus->glomd);
+		dptr = PKTDATA(osh, bus->glomd);
+		if (!dlen || (dlen & 1)) {
+			DHD_ERROR(("%s: bad glomd len (%d), ignore descriptor\n",
+			           __FUNCTION__, dlen));
+			dlen = 0;
+		}
+
+		for (totlen = num = 0; dlen; num++) {
+			/* Get (and move past) next length */
+			sublen = ltoh16_ua(dptr);
+			dlen -= sizeof(uint16);
+			dptr += sizeof(uint16);
+			if ((sublen < SDPCM_HDRLEN) ||
+			    ((num == 0) && (sublen < (2 * SDPCM_HDRLEN)))) {
+				DHD_ERROR(("%s: descriptor len %d bad: %d\n",
+				           __FUNCTION__, num, sublen));
+				pnext = NULL;
+				break;
+			}
+			if (sublen % DHD_SDALIGN) {
+				DHD_ERROR(("%s: sublen %d not a multiple of %d\n",
+				           __FUNCTION__, sublen, DHD_SDALIGN));
+				usechain = FALSE;
+			}
+			totlen += sublen;
+
+			/* For last frame, adjust read len so total is a block multiple */
+			if (!dlen) {
+				sublen += (ROUNDUP(totlen, bus->blocksize) - totlen);
+				totlen = ROUNDUP(totlen, bus->blocksize);
+			}
+
+			/* Allocate/chain packet for next subframe */
+			if ((pnext = PKTGET(osh, sublen + DHD_SDALIGN, FALSE)) == NULL) {
+				DHD_ERROR(("%s: PKTGET failed, num %d len %d\n",
+				           __FUNCTION__, num, sublen));
+				break;
+			}
+			ASSERT(!PKTLINK(pnext));
+			if (!pfirst) {
+				ASSERT(!plast);
+				pfirst = plast = pnext;
+			} else {
+				ASSERT(plast);
+				PKTSETNEXT(osh, plast, pnext);
+				plast = pnext;
+			}
+
+			/* Adhere to start alignment requirements */
+			PKTALIGN(osh, pnext, sublen, DHD_SDALIGN);
+		}
+
+		/* If all allocations succeeded, save packet chain in bus structure */
+		if (pnext) {
+			DHD_GLOM(("%s: allocated %d-byte packet chain for %d subframes\n",
+			          __FUNCTION__, totlen, num));
+			if (DHD_GLOM_ON() && bus->nextlen) {
+				if (totlen != bus->nextlen) {
+					DHD_GLOM(("%s: glomdesc mismatch: nextlen %d glomdesc %d "
+					          "rxseq %d\n", __FUNCTION__, bus->nextlen,
+					          totlen, rxseq));
+				}
+			}
+			bus->glom = pfirst;
+			pfirst = pnext = NULL;
+		} else {
+			if (pfirst)
+				PKTFREE(osh, pfirst, FALSE);
+			bus->glom = NULL;
+			num = 0;
+		}
+
+		/* Done with descriptor packet */
+		PKTFREE(osh, bus->glomd, FALSE);
+		bus->glomd = NULL;
+		bus->nextlen = 0;
+
+		dhd_os_sdunlock_rxq(bus->dhd);
+	}
+
+	/* Ok -- either we just generated a packet chain, or had one from before */
+	if (bus->glom) {
+		if (DHD_GLOM_ON()) {
+			DHD_GLOM(("%s: attempt superframe read, packet chain:\n", __FUNCTION__));
+			for (pnext = bus->glom; pnext; pnext = PKTNEXT(osh, pnext)) {
+				DHD_GLOM(("    %p: %p len 0x%04x (%d)\n",
+				          pnext, (uint8*)PKTDATA(osh, pnext),
+				          PKTLEN(osh, pnext), PKTLEN(osh, pnext)));
+			}
+		}
+
+		pfirst = bus->glom;
+		dlen = (uint16)pkttotlen(osh, pfirst);
+
+		/* Do an SDIO read for the superframe.  Configurable iovar to
+		 * read directly into the chained packet, or allocate a large
+		 * packet and and copy into the chain.
+		 */
+		if (usechain) {
+			errcode = dhd_bcmsdh_recv_buf(bus,
+			                              bcmsdh_cur_sbwad(bus->sdh), SDIO_FUNC_2,
+			                              F2SYNC, (uint8*)PKTDATA(osh, pfirst),
+			                              dlen, pfirst, NULL, NULL);
+		} else if (bus->dataptr) {
+			errcode = dhd_bcmsdh_recv_buf(bus,
+			                              bcmsdh_cur_sbwad(bus->sdh), SDIO_FUNC_2,
+			                              F2SYNC, bus->dataptr,
+			                              dlen, NULL, NULL, NULL);
+			sublen = (uint16)pktfrombuf(osh, pfirst, 0, dlen, bus->dataptr);
+			if (sublen != dlen) {
+				DHD_ERROR(("%s: FAILED TO COPY, dlen %d sublen %d\n",
+				           __FUNCTION__, dlen, sublen));
+				errcode = -1;
+			}
+			pnext = NULL;
+		} else {
+			DHD_ERROR(("COULDN'T ALLOC %d-BYTE GLOM, FORCE FAILURE\n", dlen));
+			errcode = -1;
+		}
+		bus->f2rxdata++;
+		ASSERT(errcode != BCME_PENDING);
+
+		/* On failure, kill the superframe, allow a couple retries */
+		if (errcode < 0) {
+			DHD_ERROR(("%s: glom read of %d bytes failed: %d\n",
+			           __FUNCTION__, dlen, errcode));
+			bus->dhd->rx_errors++;
+
+			if (bus->glomerr++ < 3) {
+				dhdsdio_rxfail(bus, TRUE, TRUE);
+			} else {
+				bus->glomerr = 0;
+				dhdsdio_rxfail(bus, TRUE, FALSE);
+				dhd_os_sdlock_rxq(bus->dhd);
+				PKTFREE(osh, bus->glom, FALSE);
+				dhd_os_sdunlock_rxq(bus->dhd);
+				bus->rxglomfail++;
+				bus->glom = NULL;
+			}
+			return 0;
+		}
+
+#ifdef DHD_DEBUG
+		if (DHD_GLOM_ON()) {
+			prhex("SUPERFRAME", PKTDATA(osh, pfirst),
+			      MIN(PKTLEN(osh, pfirst), 48));
+		}
+#endif
+
+
+		/* Validate the superframe header */
+		dptr = (uint8 *)PKTDATA(osh, pfirst);
+		sublen = ltoh16_ua(dptr);
+		check = ltoh16_ua(dptr + sizeof(uint16));
+
+		chan = SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN]);
+		seq = SDPCM_PACKET_SEQUENCE(&dptr[SDPCM_FRAMETAG_LEN]);
+		bus->nextlen = dptr[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
+		if ((bus->nextlen << 4) > MAX_RX_DATASZ) {
+			DHD_INFO(("%s: got frame w/nextlen too large (%d) seq %d\n",
+			          __FUNCTION__, bus->nextlen, seq));
+			bus->nextlen = 0;
+		}
+		doff = SDPCM_DOFFSET_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
+		txmax = SDPCM_WINDOW_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
+
+		errcode = 0;
+		if ((uint16)~(sublen^check)) {
+			DHD_ERROR(("%s (superframe): HW hdr error: len/check 0x%04x/0x%04x\n",
+			           __FUNCTION__, sublen, check));
+			errcode = -1;
+		} else if (ROUNDUP(sublen, bus->blocksize) != dlen) {
+			DHD_ERROR(("%s (superframe): len 0x%04x, rounded 0x%04x, expect 0x%04x\n",
+			           __FUNCTION__, sublen, ROUNDUP(sublen, bus->blocksize), dlen));
+			errcode = -1;
+		} else if (SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN]) != SDPCM_GLOM_CHANNEL) {
+			DHD_ERROR(("%s (superframe): bad channel %d\n", __FUNCTION__,
+			           SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN])));
+			errcode = -1;
+		} else if (SDPCM_GLOMDESC(&dptr[SDPCM_FRAMETAG_LEN])) {
+			DHD_ERROR(("%s (superframe): got second descriptor?\n", __FUNCTION__));
+			errcode = -1;
+		} else if ((doff < SDPCM_HDRLEN) ||
+		           (doff > (PKTLEN(osh, pfirst) - SDPCM_HDRLEN))) {
+			DHD_ERROR(("%s (superframe): Bad data offset %d: HW %d pkt %d min %d\n",
+			           __FUNCTION__, doff, sublen, PKTLEN(osh, pfirst), SDPCM_HDRLEN));
+			errcode = -1;
+		}
+
+		/* Check sequence number of superframe SW header */
+		if (rxseq != seq) {
+			DHD_INFO(("%s: (superframe) rx_seq %d, expected %d\n",
+			          __FUNCTION__, seq, rxseq));
+			bus->rx_badseq++;
+			rxseq = seq;
+		}
+
+		/* Check window for sanity */
+		if ((uint8)(txmax - bus->tx_seq) > 0x40) {
+			DHD_ERROR(("%s: got unlikely tx max %d with tx_seq %d\n",
+			           __FUNCTION__, txmax, bus->tx_seq));
+			txmax = bus->tx_max;
+		}
+		bus->tx_max = txmax;
+
+		/* Remove superframe header, remember offset */
+		PKTPULL(osh, pfirst, doff);
+		sfdoff = doff;
+
+		/* Validate all the subframe headers */
+		for (num = 0, pnext = pfirst; pnext && !errcode;
+		     num++, pnext = PKTNEXT(osh, pnext)) {
+			dptr = (uint8 *)PKTDATA(osh, pnext);
+			dlen = (uint16)PKTLEN(osh, pnext);
+			sublen = ltoh16_ua(dptr);
+			check = ltoh16_ua(dptr + sizeof(uint16));
+			chan = SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN]);
+			doff = SDPCM_DOFFSET_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
+#ifdef DHD_DEBUG
+			if (DHD_GLOM_ON()) {
+				prhex("subframe", dptr, 32);
+			}
+#endif
+
+			if ((uint16)~(sublen^check)) {
+				DHD_ERROR(("%s (subframe %d): HW hdr error: "
+				           "len/check 0x%04x/0x%04x\n",
+				           __FUNCTION__, num, sublen, check));
+				errcode = -1;
+			} else if ((sublen > dlen) || (sublen < SDPCM_HDRLEN)) {
+				DHD_ERROR(("%s (subframe %d): length mismatch: "
+				           "len 0x%04x, expect 0x%04x\n",
+				           __FUNCTION__, num, sublen, dlen));
+				errcode = -1;
+			} else if ((chan != SDPCM_DATA_CHANNEL) &&
+			           (chan != SDPCM_EVENT_CHANNEL)) {
+				DHD_ERROR(("%s (subframe %d): bad channel %d\n",
+				           __FUNCTION__, num, chan));
+				errcode = -1;
+			} else if ((doff < SDPCM_HDRLEN) || (doff > sublen)) {
+				DHD_ERROR(("%s (subframe %d): Bad data offset %d: HW %d min %d\n",
+				           __FUNCTION__, num, doff, sublen, SDPCM_HDRLEN));
+				errcode = -1;
+			}
+		}
+
+		if (errcode) {
+			/* Terminate frame on error, request a couple retries */
+			if (bus->glomerr++ < 3) {
+				/* Restore superframe header space */
+				PKTPUSH(osh, pfirst, sfdoff);
+				dhdsdio_rxfail(bus, TRUE, TRUE);
+			} else {
+				bus->glomerr = 0;
+				dhdsdio_rxfail(bus, TRUE, FALSE);
+				dhd_os_sdlock_rxq(bus->dhd);
+				PKTFREE(osh, bus->glom, FALSE);
+				dhd_os_sdunlock_rxq(bus->dhd);
+				bus->rxglomfail++;
+				bus->glom = NULL;
+			}
+			bus->nextlen = 0;
+			return 0;
+		}
+
+		/* Basic SD framing looks ok - process each packet (header) */
+		save_pfirst = pfirst;
+		bus->glom = NULL;
+		plast = NULL;
+
+		dhd_os_sdlock_rxq(bus->dhd);
+		for (num = 0; pfirst; rxseq++, pfirst = pnext) {
+			pnext = PKTNEXT(osh, pfirst);
+			PKTSETNEXT(osh, pfirst, NULL);
+
+			dptr = (uint8 *)PKTDATA(osh, pfirst);
+			sublen = ltoh16_ua(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]);
+
+			DHD_GLOM(("%s: Get subframe %d, %p(%p/%d), sublen %d chan %d seq %d\n",
+			          __FUNCTION__, num, pfirst, PKTDATA(osh, pfirst),
+			          PKTLEN(osh, pfirst), sublen, chan, seq));
+
+			ASSERT((chan == SDPCM_DATA_CHANNEL) || (chan == SDPCM_EVENT_CHANNEL));
+
+			if (rxseq != seq) {
+				DHD_GLOM(("%s: rx_seq %d, expected %d\n",
+				          __FUNCTION__, seq, rxseq));
+				bus->rx_badseq++;
+				rxseq = seq;
+			}
+
+#ifdef DHD_DEBUG
+			if (DHD_BYTES_ON() && DHD_DATA_ON()) {
+				prhex("Rx Subframe Data", dptr, dlen);
+			}
+#endif
+
+			PKTSETLEN(osh, pfirst, sublen);
+			PKTPULL(osh, pfirst, doff);
+
+			if (PKTLEN(osh, pfirst) == 0) {
+				PKTFREE(bus->dhd->osh, pfirst, FALSE);
+				if (plast) {
+					PKTSETNEXT(osh, plast, pnext);
+				} else {
+					ASSERT(save_pfirst == pfirst);
+					save_pfirst = pnext;
+				}
+				continue;
+			} else if (dhd_prot_hdrpull(bus->dhd, &ifidx, pfirst) != 0) {
+				DHD_ERROR(("%s: rx protocol error\n", __FUNCTION__));
+				bus->dhd->rx_errors++;
+				PKTFREE(osh, pfirst, FALSE);
+				if (plast) {
+					PKTSETNEXT(osh, plast, pnext);
+				} else {
+					ASSERT(save_pfirst == pfirst);
+					save_pfirst = pnext;
+				}
+				continue;
+			}
+
+			/* this packet will go up, link back into chain and count it */
+			PKTSETNEXT(osh, pfirst, pnext);
+			plast = pfirst;
+			num++;
+
+#ifdef DHD_DEBUG
+			if (DHD_GLOM_ON()) {
+				DHD_GLOM(("%s subframe %d to stack, %p(%p/%d) nxt/lnk %p/%p\n",
+				          __FUNCTION__, num, pfirst,
+				          PKTDATA(osh, pfirst), PKTLEN(osh, pfirst),
+				          PKTNEXT(osh, pfirst), PKTLINK(pfirst)));
+				prhex("", (uint8 *)PKTDATA(osh, pfirst),
+				      MIN(PKTLEN(osh, pfirst), 32));
+			}
+#endif /* DHD_DEBUG */
+		}
+		dhd_os_sdunlock_rxq(bus->dhd);
+		if (num) {
+			dhd_os_sdunlock(bus->dhd);
+			dhd_rx_frame(bus->dhd, ifidx, save_pfirst, num, 0);
+			dhd_os_sdlock(bus->dhd);
+		}
+
+		bus->rxglomframes++;
+		bus->rxglompkts += num;
+	}
+	return num;
+}
+
+/* Return TRUE if there may be more frames to read */
+static uint
+dhdsdio_readframes(dhd_bus_t *bus, uint maxframes, bool *finished)
+{
+	osl_t *osh = bus->dhd->osh;
+	bcmsdh_info_t *sdh = bus->sdh;
+
+	uint16 len, check;	/* Extracted hardware header fields */
+	uint8 chan, seq, doff;	/* Extracted software header fields */
+	uint8 fcbits;		/* Extracted fcbits from software header */
+	uint8 delta;
+
+	void *pkt;	/* Packet for event or data frames */
+	uint16 pad;	/* Number of pad bytes to read */
+	uint16 rdlen;	/* Total number of bytes to read */
+	uint8 rxseq;	/* Next sequence number to expect */
+	uint rxleft = 0;	/* Remaining number of frames allowed */
+	int sdret;	/* Return code from bcmsdh calls */
+	uint8 txmax;	/* Maximum tx sequence offered */
+	bool len_consistent; /* Result of comparing readahead len and len from hw-hdr */
+	uint8 *rxbuf;
+	int ifidx = 0;
+	uint rxcount = 0; /* Total frames read */
+
+#if defined(DHD_DEBUG) || defined(SDTEST)
+	bool sdtest = FALSE;	/* To limit message spew from test mode */
+#endif
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	ASSERT(maxframes);
+
+#ifdef SDTEST
+	/* Allow pktgen to override maxframes */
+	if (bus->pktgen_count && (bus->pktgen_mode == DHD_PKTGEN_RECV)) {
+		maxframes = bus->pktgen_count;
+		sdtest = TRUE;
+	}
+#endif
+
+	/* Not finished unless we encounter no more frames indication */
+	*finished = FALSE;
+
+
+	for (rxseq = bus->rx_seq, rxleft = maxframes;
+	     !bus->rxskip && rxleft && bus->dhd->busstate != DHD_BUS_DOWN;
+	     rxseq++, rxleft--) {
+
+#ifdef DHDTHREAD
+		/* tx more to improve rx performance */
+		if ((bus->clkstate == CLK_AVAIL) && !bus->fcstate &&
+			pktq_mlen(&bus->txq, ~bus->flowcontrol) && DATAOK(bus)) {
+			dhdsdio_sendfromq(bus, dhd_txbound);
+		}
+#endif /* DHDTHREAD */
+
+		/* Handle glomming separately */
+		if (bus->glom || bus->glomd) {
+			uint8 cnt;
+			DHD_GLOM(("%s: calling rxglom: glomd %p, glom %p\n",
+			          __FUNCTION__, bus->glomd, bus->glom));
+			cnt = dhdsdio_rxglom(bus, rxseq);
+			DHD_GLOM(("%s: rxglom returned %d\n", __FUNCTION__, cnt));
+			rxseq += cnt - 1;
+			rxleft = (rxleft > cnt) ? (rxleft - cnt) : 1;
+			continue;
+		}
+
+		/* Try doing single read if we can */
+		if (dhd_readahead && bus->nextlen) {
+			uint16 nextlen = bus->nextlen;
+			bus->nextlen = 0;
+
+			if (bus->bus == SPI_BUS) {
+				rdlen = len = nextlen;
+			}
+			else {
+				rdlen = len = nextlen << 4;
+
+				/* Pad read to blocksize for efficiency */
+				if (bus->roundup && bus->blocksize && (rdlen > bus->blocksize)) {
+					pad = bus->blocksize - (rdlen % bus->blocksize);
+					if ((pad <= bus->roundup) && (pad < bus->blocksize) &&
+						((rdlen + pad + firstread) < MAX_RX_DATASZ))
+						rdlen += pad;
+				} else if (rdlen % DHD_SDALIGN) {
+					rdlen += DHD_SDALIGN - (rdlen % DHD_SDALIGN);
+				}
+			}
+
+			/* We use bus->rxctl buffer in WinXP for initial control pkt receives.
+			 * Later we use buffer-poll for data as well as control packets.
+			 * This is required because dhd receives full frame in gSPI unlike SDIO.
+			 * After the frame is received we have to distinguish whether it is data
+			 * or non-data frame.
+			 */
+			/* Allocate a packet buffer */
+			dhd_os_sdlock_rxq(bus->dhd);
+			if (!(pkt = PKTGET(osh, rdlen + DHD_SDALIGN, FALSE))) {
+				if (bus->bus == SPI_BUS) {
+					bus->usebufpool = FALSE;
+					bus->rxctl = bus->rxbuf;
+					if (dhd_alignctl) {
+						bus->rxctl += firstread;
+						if ((pad = ((uintptr)bus->rxctl % DHD_SDALIGN)))
+							bus->rxctl += (DHD_SDALIGN - pad);
+						bus->rxctl -= firstread;
+					}
+					ASSERT(bus->rxctl >= bus->rxbuf);
+					rxbuf = bus->rxctl;
+					/* Read the entire frame */
+					sdret = dhd_bcmsdh_recv_buf(bus,
+					                            bcmsdh_cur_sbwad(sdh),
+					                            SDIO_FUNC_2,
+					                            F2SYNC, rxbuf, rdlen,
+					                            NULL, NULL, NULL);
+					bus->f2rxdata++;
+					ASSERT(sdret != BCME_PENDING);
+
+
+					/* Control frame failures need retransmission */
+					if (sdret < 0) {
+						DHD_ERROR(("%s: read %d control bytes failed: %d\n",
+						   __FUNCTION__, rdlen, sdret));
+						/* dhd.rx_ctlerrs is higher level */
+						bus->rxc_errors++;
+						dhd_os_sdunlock_rxq(bus->dhd);
+						dhdsdio_rxfail(bus, TRUE,
+						    (bus->bus == SPI_BUS) ? FALSE : TRUE);
+						continue;
+					}
+				} else {
+					/* Give up on data, request rtx of events */
+					DHD_ERROR(("%s (nextlen): PKTGET failed: len %d rdlen %d "
+					           "expected rxseq %d\n",
+					           __FUNCTION__, len, rdlen, rxseq));
+					/* Just go try again w/normal header read */
+					dhd_os_sdunlock_rxq(bus->dhd);
+					continue;
+				}
+			} else {
+				if (bus->bus == SPI_BUS)
+					bus->usebufpool = TRUE;
+
+				ASSERT(!PKTLINK(pkt));
+				PKTALIGN(osh, pkt, rdlen, DHD_SDALIGN);
+				rxbuf = (uint8 *)PKTDATA(osh, pkt);
+				/* Read the entire frame */
+				sdret = dhd_bcmsdh_recv_buf(bus, bcmsdh_cur_sbwad(sdh),
+				                            SDIO_FUNC_2,
+				                            F2SYNC, rxbuf, rdlen,
+				                            pkt, NULL, NULL);
+				bus->f2rxdata++;
+				ASSERT(sdret != BCME_PENDING);
+
+				if (sdret < 0) {
+					DHD_ERROR(("%s (nextlen): read %d bytes failed: %d\n",
+					   __FUNCTION__, rdlen, sdret));
+					PKTFREE(bus->dhd->osh, pkt, FALSE);
+					bus->dhd->rx_errors++;
+					dhd_os_sdunlock_rxq(bus->dhd);
+					/* Force retry w/normal header read.  Don't attempt NAK for
+					 * gSPI
+					 */
+					dhdsdio_rxfail(bus, TRUE,
+					      (bus->bus == SPI_BUS) ? FALSE : TRUE);
+					continue;
+				}
+			}
+			dhd_os_sdunlock_rxq(bus->dhd);
+
+			/* Now check the header */
+			bcopy(rxbuf, bus->rxhdr, SDPCM_HDRLEN);
+
+			/* Extract hardware header fields */
+			len = ltoh16_ua(bus->rxhdr);
+			check = ltoh16_ua(bus->rxhdr + sizeof(uint16));
+
+			/* All zeros means readahead info was bad */
+			if (!(len|check)) {
+				DHD_INFO(("%s (nextlen): read zeros in HW header???\n",
+				           __FUNCTION__));
+				dhd_os_sdlock_rxq(bus->dhd);
+				PKTFREE2();
+				dhd_os_sdunlock_rxq(bus->dhd);
+				GSPI_PR55150_BAILOUT;
+				continue;
+			}
+
+			/* Validate check bytes */
+			if ((uint16)~(len^check)) {
+				DHD_ERROR(("%s (nextlen): HW hdr error: nextlen/len/check"
+				           " 0x%04x/0x%04x/0x%04x\n", __FUNCTION__, nextlen,
+				           len, check));
+				dhd_os_sdlock_rxq(bus->dhd);
+				PKTFREE2();
+				dhd_os_sdunlock_rxq(bus->dhd);
+				bus->rx_badhdr++;
+				dhdsdio_rxfail(bus, FALSE, FALSE);
+				GSPI_PR55150_BAILOUT;
+				continue;
+			}
+
+			/* Validate frame length */
+			if (len < SDPCM_HDRLEN) {
+				DHD_ERROR(("%s (nextlen): HW hdr length invalid: %d\n",
+				           __FUNCTION__, len));
+				dhd_os_sdlock_rxq(bus->dhd);
+				PKTFREE2();
+				dhd_os_sdunlock_rxq(bus->dhd);
+				GSPI_PR55150_BAILOUT;
+				continue;
+			}
+
+			/* Check for consistency with readahead info */
+				len_consistent = (nextlen != (ROUNDUP(len, 16) >> 4));
+			if (len_consistent) {
+				/* Mismatch, force retry w/normal header (may be >4K) */
+				DHD_ERROR(("%s (nextlen): mismatch, nextlen %d len %d rnd %d; "
+				           "expected rxseq %d\n",
+				           __FUNCTION__, nextlen, len, ROUNDUP(len, 16), rxseq));
+				dhd_os_sdlock_rxq(bus->dhd);
+				PKTFREE2();
+				dhd_os_sdunlock_rxq(bus->dhd);
+				dhdsdio_rxfail(bus, TRUE, (bus->bus == SPI_BUS) ? FALSE : TRUE);
+				GSPI_PR55150_BAILOUT;
+				continue;
+			}
+
+
+			/* Extract software header fields */
+			chan = SDPCM_PACKET_CHANNEL(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+			seq = SDPCM_PACKET_SEQUENCE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+			doff = SDPCM_DOFFSET_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+			txmax = SDPCM_WINDOW_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+
+				bus->nextlen =
+				         bus->rxhdr[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
+				if ((bus->nextlen << 4) > MAX_RX_DATASZ) {
+					DHD_INFO(("%s (nextlen): got frame w/nextlen too large"
+					          " (%d), seq %d\n", __FUNCTION__, bus->nextlen,
+					          seq));
+					bus->nextlen = 0;
+				}
+
+				bus->dhd->rx_readahead_cnt ++;
+			/* Handle Flow Control */
+			fcbits = SDPCM_FCMASK_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+
+			delta = 0;
+			if (~bus->flowcontrol & fcbits) {
+				bus->fc_xoff++;
+				delta = 1;
+			}
+			if (bus->flowcontrol & ~fcbits) {
+				bus->fc_xon++;
+				delta = 1;
+			}
+
+			if (delta) {
+				bus->fc_rcvd++;
+				bus->flowcontrol = fcbits;
+			}
+
+			/* Check and update sequence number */
+			if (rxseq != seq) {
+				DHD_INFO(("%s (nextlen): rx_seq %d, expected %d\n",
+				          __FUNCTION__, seq, rxseq));
+				bus->rx_badseq++;
+				rxseq = seq;
+			}
+
+			/* Check window for sanity */
+			if ((uint8)(txmax - bus->tx_seq) > 0x40) {
+					DHD_ERROR(("%s: got unlikely tx max %d with tx_seq %d\n",
+						__FUNCTION__, txmax, bus->tx_seq));
+					txmax = bus->tx_max;
+			}
+			bus->tx_max = txmax;
+
+#ifdef DHD_DEBUG
+			if (DHD_BYTES_ON() && DHD_DATA_ON()) {
+				prhex("Rx Data", rxbuf, len);
+			} else if (DHD_HDRS_ON()) {
+				prhex("RxHdr", bus->rxhdr, SDPCM_HDRLEN);
+			}
+#endif
+
+			if (chan == SDPCM_CONTROL_CHANNEL) {
+				if (bus->bus == SPI_BUS) {
+					dhdsdio_read_control(bus, rxbuf, len, doff);
+					if (bus->usebufpool) {
+						dhd_os_sdlock_rxq(bus->dhd);
+						PKTFREE(bus->dhd->osh, pkt, FALSE);
+						dhd_os_sdunlock_rxq(bus->dhd);
+					}
+					continue;
+				} else {
+					DHD_ERROR(("%s (nextlen): readahead on control"
+					           " packet %d?\n", __FUNCTION__, seq));
+					/* Force retry w/normal header read */
+					bus->nextlen = 0;
+					dhdsdio_rxfail(bus, FALSE, TRUE);
+					dhd_os_sdlock_rxq(bus->dhd);
+					PKTFREE2();
+					dhd_os_sdunlock_rxq(bus->dhd);
+					continue;
+				}
+			}
+
+			if ((bus->bus == SPI_BUS) && !bus->usebufpool) {
+				DHD_ERROR(("Received %d bytes on %d channel. Running out of "
+				           "rx pktbuf's or not yet malloced.\n", len, chan));
+				continue;
+			}
+
+			/* Validate data offset */
+			if ((doff < SDPCM_HDRLEN) || (doff > len)) {
+				DHD_ERROR(("%s (nextlen): bad data offset %d: HW len %d min %d\n",
+				           __FUNCTION__, doff, len, SDPCM_HDRLEN));
+				dhd_os_sdlock_rxq(bus->dhd);
+				PKTFREE2();
+				dhd_os_sdunlock_rxq(bus->dhd);
+				ASSERT(0);
+				dhdsdio_rxfail(bus, FALSE, FALSE);
+				continue;
+			}
+
+			/* All done with this one -- now deliver the packet */
+			goto deliver;
+		}
+		/* gSPI frames should not be handled in fractions */
+		if (bus->bus == SPI_BUS) {
+			break;
+		}
+
+		/* Read frame header (hardware and software) */
+		sdret = dhd_bcmsdh_recv_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
+		                            bus->rxhdr, firstread, NULL, NULL, NULL);
+		bus->f2rxhdrs++;
+		ASSERT(sdret != BCME_PENDING);
+
+		if (sdret < 0) {
+			DHD_ERROR(("%s: RXHEADER FAILED: %d\n", __FUNCTION__, sdret));
+			bus->rx_hdrfail++;
+			dhdsdio_rxfail(bus, TRUE, TRUE);
+			continue;
+		}
+
+#ifdef DHD_DEBUG
+		if (DHD_BYTES_ON() || DHD_HDRS_ON()) {
+			prhex("RxHdr", bus->rxhdr, SDPCM_HDRLEN);
+		}
+#endif
+
+		/* Extract hardware header fields */
+		len = ltoh16_ua(bus->rxhdr);
+		check = ltoh16_ua(bus->rxhdr + sizeof(uint16));
+
+		/* All zeros means no more frames */
+		if (!(len|check)) {
+			*finished = TRUE;
+			break;
+		}
+
+		/* Validate check bytes */
+		if ((uint16)~(len^check)) {
+			DHD_ERROR(("%s: HW hdr error: len/check 0x%04x/0x%04x\n",
+			           __FUNCTION__, len, check));
+			bus->rx_badhdr++;
+			dhdsdio_rxfail(bus, FALSE, FALSE);
+			continue;
+		}
+
+		/* Validate frame length */
+		if (len < SDPCM_HDRLEN) {
+			DHD_ERROR(("%s: HW hdr length invalid: %d\n", __FUNCTION__, len));
+			continue;
+		}
+
+		/* Extract software header fields */
+		chan = SDPCM_PACKET_CHANNEL(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+		seq = SDPCM_PACKET_SEQUENCE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+		doff = SDPCM_DOFFSET_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+		txmax = SDPCM_WINDOW_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+
+		/* Validate data offset */
+		if ((doff < SDPCM_HDRLEN) || (doff > len)) {
+			DHD_ERROR(("%s: Bad data offset %d: HW len %d, min %d seq %d\n",
+			           __FUNCTION__, doff, len, SDPCM_HDRLEN, seq));
+			bus->rx_badhdr++;
+			ASSERT(0);
+			dhdsdio_rxfail(bus, FALSE, FALSE);
+			continue;
+		}
+
+		/* Save the readahead length if there is one */
+		bus->nextlen = bus->rxhdr[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
+		if ((bus->nextlen << 4) > MAX_RX_DATASZ) {
+			DHD_INFO(("%s (nextlen): got frame w/nextlen too large (%d), seq %d\n",
+			          __FUNCTION__, bus->nextlen, seq));
+			bus->nextlen = 0;
+		}
+
+		/* Handle Flow Control */
+		fcbits = SDPCM_FCMASK_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
+
+		delta = 0;
+		if (~bus->flowcontrol & fcbits) {
+			bus->fc_xoff++;
+			delta = 1;
+		}
+		if (bus->flowcontrol & ~fcbits) {
+			bus->fc_xon++;
+			delta = 1;
+		}
+
+		if (delta) {
+			bus->fc_rcvd++;
+			bus->flowcontrol = fcbits;
+		}
+
+		/* Check and update sequence number */
+		if (rxseq != seq) {
+			DHD_INFO(("%s: rx_seq %d, expected %d\n", __FUNCTION__, seq, rxseq));
+			bus->rx_badseq++;
+			rxseq = seq;
+		}
+
+		/* Check window for sanity */
+		if ((uint8)(txmax - bus->tx_seq) > 0x40) {
+			DHD_ERROR(("%s: got unlikely tx max %d with tx_seq %d\n",
+			           __FUNCTION__, txmax, bus->tx_seq));
+			txmax = bus->tx_max;
+		}
+		bus->tx_max = txmax;
+
+		/* Call a separate function for control frames */
+		if (chan == SDPCM_CONTROL_CHANNEL) {
+			dhdsdio_read_control(bus, bus->rxhdr, len, doff);
+			continue;
+		}
+
+		ASSERT((chan == SDPCM_DATA_CHANNEL) || (chan == SDPCM_EVENT_CHANNEL) ||
+		       (chan == SDPCM_TEST_CHANNEL) || (chan == SDPCM_GLOM_CHANNEL));
+
+		/* Length to read */
+		rdlen = (len > firstread) ? (len - firstread) : 0;
+
+		/* May pad read to blocksize for efficiency */
+		if (bus->roundup && bus->blocksize && (rdlen > bus->blocksize)) {
+			pad = bus->blocksize - (rdlen % bus->blocksize);
+			if ((pad <= bus->roundup) && (pad < bus->blocksize) &&
+			    ((rdlen + pad + firstread) < MAX_RX_DATASZ))
+				rdlen += pad;
+		} else if (rdlen % DHD_SDALIGN) {
+			rdlen += DHD_SDALIGN - (rdlen % DHD_SDALIGN);
+		}
+
+		/* Satisfy length-alignment requirements */
+		if (forcealign && (rdlen & (ALIGNMENT - 1)))
+			rdlen = ROUNDUP(rdlen, ALIGNMENT);
+
+		if ((rdlen + firstread) > MAX_RX_DATASZ) {
+			/* Too long -- skip this frame */
+			DHD_ERROR(("%s: too long: len %d rdlen %d\n", __FUNCTION__, len, rdlen));
+			bus->dhd->rx_errors++; bus->rx_toolong++;
+			dhdsdio_rxfail(bus, FALSE, FALSE);
+			continue;
+		}
+
+		dhd_os_sdlock_rxq(bus->dhd);
+		if (!(pkt = PKTGET(osh, (rdlen + firstread + DHD_SDALIGN), FALSE))) {
+			/* Give up on data, request rtx of events */
+			DHD_ERROR(("%s: PKTGET failed: rdlen %d chan %d\n",
+			           __FUNCTION__, rdlen, chan));
+			bus->dhd->rx_dropped++;
+			dhd_os_sdunlock_rxq(bus->dhd);
+			dhdsdio_rxfail(bus, FALSE, RETRYCHAN(chan));
+			continue;
+		}
+		dhd_os_sdunlock_rxq(bus->dhd);
+
+		ASSERT(!PKTLINK(pkt));
+
+		/* Leave room for what we already read, and align remainder */
+		ASSERT(firstread < (PKTLEN(osh, pkt)));
+		PKTPULL(osh, pkt, firstread);
+		PKTALIGN(osh, pkt, rdlen, DHD_SDALIGN);
+
+		/* Read the remaining frame data */
+		sdret = dhd_bcmsdh_recv_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
+		                            ((uint8 *)PKTDATA(osh, pkt)), rdlen, pkt, NULL, NULL);
+		bus->f2rxdata++;
+		ASSERT(sdret != BCME_PENDING);
+
+		if (sdret < 0) {
+			DHD_ERROR(("%s: read %d %s bytes failed: %d\n", __FUNCTION__, rdlen,
+			           ((chan == SDPCM_EVENT_CHANNEL) ? "event" :
+			            ((chan == SDPCM_DATA_CHANNEL) ? "data" : "test")), sdret));
+			dhd_os_sdlock_rxq(bus->dhd);
+			PKTFREE(bus->dhd->osh, pkt, FALSE);
+			dhd_os_sdunlock_rxq(bus->dhd);
+			bus->dhd->rx_errors++;
+			dhdsdio_rxfail(bus, TRUE, RETRYCHAN(chan));
+			continue;
+		}
+
+		/* Copy the already-read portion */
+		PKTPUSH(osh, pkt, firstread);
+		bcopy(bus->rxhdr, PKTDATA(osh, pkt), firstread);
+
+#ifdef DHD_DEBUG
+		if (DHD_BYTES_ON() && DHD_DATA_ON()) {
+			prhex("Rx Data", PKTDATA(osh, pkt), len);
+		}
+#endif
+
+deliver:
+		/* Save superframe descriptor and allocate packet frame */
+		if (chan == SDPCM_GLOM_CHANNEL) {
+			if (SDPCM_GLOMDESC(&bus->rxhdr[SDPCM_FRAMETAG_LEN])) {
+				DHD_GLOM(("%s: got glom descriptor, %d bytes:\n",
+				          __FUNCTION__, len));
+#ifdef DHD_DEBUG
+				if (DHD_GLOM_ON()) {
+					prhex("Glom Data", PKTDATA(osh, pkt), len);
+				}
+#endif
+				PKTSETLEN(osh, pkt, len);
+				ASSERT(doff == SDPCM_HDRLEN);
+				PKTPULL(osh, pkt, SDPCM_HDRLEN);
+				bus->glomd = pkt;
+			} else {
+				DHD_ERROR(("%s: glom superframe w/o descriptor!\n", __FUNCTION__));
+				dhdsdio_rxfail(bus, FALSE, FALSE);
+			}
+			continue;
+		}
+
+		/* Fill in packet len and prio, deliver upward */
+		PKTSETLEN(osh, pkt, len);
+		PKTPULL(osh, pkt, doff);
+
+#ifdef SDTEST
+		/* Test channel packets are processed separately */
+		if (chan == SDPCM_TEST_CHANNEL) {
+			dhdsdio_testrcv(bus, pkt, seq);
+			continue;
+		}
+#endif /* SDTEST */
+
+		if (PKTLEN(osh, pkt) == 0) {
+			dhd_os_sdlock_rxq(bus->dhd);
+			PKTFREE(bus->dhd->osh, pkt, FALSE);
+			dhd_os_sdunlock_rxq(bus->dhd);
+			continue;
+		} else if (dhd_prot_hdrpull(bus->dhd, &ifidx, pkt) != 0) {
+			DHD_ERROR(("%s: rx protocol error\n", __FUNCTION__));
+			dhd_os_sdlock_rxq(bus->dhd);
+			PKTFREE(bus->dhd->osh, pkt, FALSE);
+			dhd_os_sdunlock_rxq(bus->dhd);
+			bus->dhd->rx_errors++;
+			continue;
+		}
+
+
+		/* Unlock during rx call */
+		dhd_os_sdunlock(bus->dhd);
+		dhd_rx_frame(bus->dhd, ifidx, pkt, 1, chan);
+		dhd_os_sdlock(bus->dhd);
+	}
+	rxcount = maxframes - rxleft;
+#ifdef DHD_DEBUG
+	/* Message if we hit the limit */
+	if (!rxleft && !sdtest)
+		DHD_DATA(("%s: hit rx limit of %d frames\n", __FUNCTION__, maxframes));
+	else
+#endif /* DHD_DEBUG */
+	DHD_DATA(("%s: processed %d frames\n", __FUNCTION__, rxcount));
+	/* Back off rxseq if awaiting rtx, update rx_seq */
+	if (bus->rxskip)
+		rxseq--;
+	bus->rx_seq = rxseq;
+
+	return rxcount;
+}
+
+static uint32
+dhdsdio_hostmail(dhd_bus_t *bus)
+{
+	sdpcmd_regs_t *regs = bus->regs;
+	uint32 intstatus = 0;
+	uint32 hmb_data;
+	uint8 fcbits;
+	uint retries = 0;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	/* Read mailbox data and ack that we did so */
+	R_SDREG(hmb_data, &regs->tohostmailboxdata, retries);
+	if (retries <= retry_limit)
+		W_SDREG(SMB_INT_ACK, &regs->tosbmailbox, retries);
+	bus->f1regdata += 2;
+
+	/* Dongle recomposed rx frames, accept them again */
+	if (hmb_data & HMB_DATA_NAKHANDLED) {
+		DHD_INFO(("Dongle reports NAK handled, expect rtx of %d\n", bus->rx_seq));
+		if (!bus->rxskip) {
+			DHD_ERROR(("%s: unexpected NAKHANDLED!\n", __FUNCTION__));
+		}
+		bus->rxskip = FALSE;
+		intstatus |= FRAME_AVAIL_MASK(bus);
+	}
+
+	/*
+	 * DEVREADY does not occur with gSPI.
+	 */
+	if (hmb_data & (HMB_DATA_DEVREADY | HMB_DATA_FWREADY)) {
+		bus->sdpcm_ver = (hmb_data & HMB_DATA_VERSION_MASK) >> HMB_DATA_VERSION_SHIFT;
+		if (bus->sdpcm_ver != SDPCM_PROT_VERSION)
+			DHD_ERROR(("Version mismatch, dongle reports %d, expecting %d\n",
+			           bus->sdpcm_ver, SDPCM_PROT_VERSION));
+		else
+			DHD_INFO(("Dongle ready, protocol version %d\n", bus->sdpcm_ver));
+		/* make sure for the SDIO_DEVICE_RXDATAINT_MODE_1 corecontrol is proper */
+		if ((bus->sih->buscoretype == SDIOD_CORE_ID) && (bus->sdpcmrev >= 4) &&
+		    (bus->rxint_mode  == SDIO_DEVICE_RXDATAINT_MODE_1)) {
+			uint32 val;
+
+			val = R_REG(bus->dhd->osh, &bus->regs->corecontrol);
+			val &= ~CC_XMTDATAAVAIL_MODE;
+			val |= CC_XMTDATAAVAIL_CTRL;
+			W_REG(bus->dhd->osh, &bus->regs->corecontrol, val);
+
+			val = R_REG(bus->dhd->osh, &bus->regs->corecontrol);
+		}
+
+#ifdef DHD_DEBUG
+		/* Retrieve console state address now that firmware should have updated it */
+		{
+			sdpcm_shared_t shared;
+			if (dhdsdio_readshared(bus, &shared) == 0)
+				bus->console_addr = shared.console_addr;
+		}
+#endif /* DHD_DEBUG */
+	}
+
+	/*
+	 * Flow Control has been moved into the RX headers and this out of band
+	 * method isn't used any more.  Leave this here for possibly remaining backward
+	 * compatible with older dongles
+	 */
+	if (hmb_data & HMB_DATA_FC) {
+		fcbits = (hmb_data & HMB_DATA_FCDATA_MASK) >> HMB_DATA_FCDATA_SHIFT;
+
+		if (fcbits & ~bus->flowcontrol)
+			bus->fc_xoff++;
+		if (bus->flowcontrol & ~fcbits)
+			bus->fc_xon++;
+
+		bus->fc_rcvd++;
+		bus->flowcontrol = fcbits;
+	}
+
+#ifdef DHD_DEBUG
+	/* At least print a message if FW halted */
+	if (hmb_data & HMB_DATA_FWHALT) {
+		DHD_ERROR(("INTERNAL ERROR: FIRMWARE HALTED\n"));
+		dhdsdio_checkdied(bus, NULL, 0);
+	}
+#endif /* DHD_DEBUG */
+
+	/* Shouldn't be any others */
+	if (hmb_data & ~(HMB_DATA_DEVREADY |
+	                 HMB_DATA_FWHALT |
+	                 HMB_DATA_NAKHANDLED |
+	                 HMB_DATA_FC |
+	                 HMB_DATA_FWREADY |
+	                 HMB_DATA_FCDATA_MASK |
+	                 HMB_DATA_VERSION_MASK)) {
+		DHD_ERROR(("Unknown mailbox data content: 0x%02x\n", hmb_data));
+	}
+
+	return intstatus;
+}
+
+static bool
+dhdsdio_dpc(dhd_bus_t *bus)
+{
+	bcmsdh_info_t *sdh = bus->sdh;
+	sdpcmd_regs_t *regs = bus->regs;
+	uint32 intstatus, newstatus = 0;
+	uint retries = 0;
+	uint rxlimit = dhd_rxbound; /* Rx frames to read before resched */
+	uint txlimit = dhd_txbound; /* Tx frames to send before resched */
+	uint framecnt = 0;		  /* Temporary counter of tx/rx frames */
+	bool rxdone = TRUE;		  /* Flag for no more read data */
+	bool resched = FALSE;	  /* Flag indicating resched wanted */
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (bus->dhd->busstate == DHD_BUS_DOWN) {
+		DHD_ERROR(("%s: Bus down, ret\n", __FUNCTION__));
+		bus->intstatus = 0;
+		return 0;
+	}
+
+	/* Start with leftover status bits */
+	intstatus = bus->intstatus;
+
+	dhd_os_sdlock(bus->dhd);
+
+	/* If waiting for HTAVAIL, check status */
+	if (bus->clkstate == CLK_PENDING) {
+		int err;
+		uint8 clkctl, devctl = 0;
+
+#ifdef DHD_DEBUG
+		/* Check for inconsistent device control */
+		devctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, &err);
+		if (err) {
+			DHD_ERROR(("%s: error reading DEVCTL: %d\n", __FUNCTION__, err));
+			bus->dhd->busstate = DHD_BUS_DOWN;
+		} else {
+			ASSERT(devctl & SBSDIO_DEVCTL_CA_INT_ONLY);
+		}
+#endif /* DHD_DEBUG */
+
+		/* Read CSR, if clock on switch to AVAIL, else ignore */
+		clkctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, &err);
+		if (err) {
+			DHD_ERROR(("%s: error reading CSR: %d\n", __FUNCTION__, err));
+			bus->dhd->busstate = DHD_BUS_DOWN;
+		}
+
+		DHD_INFO(("DPC: PENDING, devctl 0x%02x clkctl 0x%02x\n", devctl, clkctl));
+
+		if (SBSDIO_HTAV(clkctl)) {
+			devctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, &err);
+			if (err) {
+				DHD_ERROR(("%s: error reading DEVCTL: %d\n",
+				           __FUNCTION__, err));
+				bus->dhd->busstate = DHD_BUS_DOWN;
+			}
+			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
+			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_DEVICE_CTL, devctl, &err);
+			if (err) {
+				DHD_ERROR(("%s: error writing DEVCTL: %d\n",
+				           __FUNCTION__, err));
+				bus->dhd->busstate = DHD_BUS_DOWN;
+			}
+			bus->clkstate = CLK_AVAIL;
+		} else {
+			goto clkwait;
+		}
+	}
+
+	BUS_WAKE(bus);
+
+	/* Make sure backplane clock is on */
+	dhdsdio_clkctl(bus, CLK_AVAIL, TRUE);
+	if (bus->clkstate != CLK_AVAIL)
+		goto clkwait;
+
+	/* Pending interrupt indicates new device status */
+	if (bus->ipend) {
+		bus->ipend = FALSE;
+		R_SDREG(newstatus, &regs->intstatus, retries);
+		bus->f1regdata++;
+		if (bcmsdh_regfail(bus->sdh))
+			newstatus = 0;
+		newstatus &= bus->hostintmask;
+		bus->fcstate = !!(newstatus & I_HMB_FC_STATE);
+		if (newstatus) {
+			bus->f1regdata++;
+			if ((bus->rxint_mode == SDIO_DEVICE_RXDATAINT_MODE_0) &&
+				(newstatus == I_XMTDATA_AVAIL)) {
+			}
+			else
+				W_SDREG(newstatus, &regs->intstatus, retries);
+		}
+	}
+
+	/* Merge new bits with previous */
+	intstatus |= newstatus;
+	bus->intstatus = 0;
+
+	/* Handle flow-control change: read new state in case our ack
+	 * crossed another change interrupt.  If change still set, assume
+	 * FC ON for safety, let next loop through do the debounce.
+	 */
+	if (intstatus & I_HMB_FC_CHANGE) {
+		intstatus &= ~I_HMB_FC_CHANGE;
+		W_SDREG(I_HMB_FC_CHANGE, &regs->intstatus, retries);
+		R_SDREG(newstatus, &regs->intstatus, retries);
+		bus->f1regdata += 2;
+		bus->fcstate = !!(newstatus & (I_HMB_FC_STATE | I_HMB_FC_CHANGE));
+		intstatus |= (newstatus & bus->hostintmask);
+	}
+
+	/* Just being here means nothing more to do for chipactive */
+	if (intstatus & I_CHIPACTIVE) {
+		/* ASSERT(bus->clkstate == CLK_AVAIL); */
+		intstatus &= ~I_CHIPACTIVE;
+	}
+
+	/* Handle host mailbox indication */
+	if (intstatus & I_HMB_HOST_INT) {
+		intstatus &= ~I_HMB_HOST_INT;
+		intstatus |= dhdsdio_hostmail(bus);
+	}
+
+	/* Generally don't ask for these, can get CRC errors... */
+	if (intstatus & I_WR_OOSYNC) {
+		DHD_ERROR(("Dongle reports WR_OOSYNC\n"));
+		intstatus &= ~I_WR_OOSYNC;
+	}
+
+	if (intstatus & I_RD_OOSYNC) {
+		DHD_ERROR(("Dongle reports RD_OOSYNC\n"));
+		intstatus &= ~I_RD_OOSYNC;
+	}
+
+	if (intstatus & I_SBINT) {
+		DHD_ERROR(("Dongle reports SBINT\n"));
+		intstatus &= ~I_SBINT;
+	}
+
+	/* Would be active due to wake-wlan in gSPI */
+	if (intstatus & I_CHIPACTIVE) {
+		DHD_INFO(("Dongle reports CHIPACTIVE\n"));
+		intstatus &= ~I_CHIPACTIVE;
+	}
+
+	/* Ignore frame indications if rxskip is set */
+	if (bus->rxskip) {
+		intstatus &= ~FRAME_AVAIL_MASK(bus);
+	}
+
+	/* On frame indication, read available frames */
+	if (PKT_AVAILABLE(bus, intstatus)) {
+		framecnt = dhdsdio_readframes(bus, rxlimit, &rxdone);
+		if (rxdone || bus->rxskip)
+			intstatus  &= ~FRAME_AVAIL_MASK(bus);
+		rxlimit -= MIN(framecnt, rxlimit);
+	}
+
+	/* Keep still-pending events for next scheduling */
+	bus->intstatus = intstatus;
+
+clkwait:
+	/* Re-enable interrupts to detect new device events (mailbox, rx frame)
+	 * or clock availability.  (Allows tx loop to check ipend if desired.)
+	 * (Unless register access seems hosed, as we may not be able to ACK...)
+	 */
+	if (bus->intr && bus->intdis && !bcmsdh_regfail(sdh)) {
+		DHD_INTR(("%s: enable SDIO interrupts, rxdone %d framecnt %d\n",
+		          __FUNCTION__, rxdone, framecnt));
+		bus->intdis = FALSE;
+#if defined(OOB_INTR_ONLY)
+	bcmsdh_oob_intr_set(1);
+#endif /* (OOB_INTR_ONLY) */
+		bcmsdh_intr_enable(sdh);
+	}
+
+#if defined(OOB_INTR_ONLY) && !defined(HW_OOB)
+	/* In case of SW-OOB(using edge trigger),
+	 * Check interrupt status in the dongle again after enable irq on the host.
+	 * and rechedule dpc if interrupt is pended in the dongle.
+	 * There is a chance to miss OOB interrupt while irq is disabled on the host.
+	 * No need to do this with HW-OOB(level trigger)
+	 */
+	R_SDREG(newstatus, &regs->intstatus, retries);
+	if (bcmsdh_regfail(bus->sdh))
+		newstatus = 0;
+	if (newstatus & bus->hostintmask) {
+		bus->ipend = TRUE;
+		resched = TRUE;
+	}
+#endif /* defined(OOB_INTR_ONLY) && !defined(HW_OOB) */
+
+	if (TXCTLOK(bus) && bus->ctrl_frame_stat && (bus->clkstate == CLK_AVAIL))  {
+		int ret, i;
+		uint8* frame_seq = bus->ctrl_frame_buf + SDPCM_FRAMETAG_LEN;
+
+		if (*frame_seq != bus->tx_seq) {
+			DHD_INFO(("%s IOCTL frame seq lag detected!"
+				" frm_seq:%d != bus->tx_seq:%d, corrected\n",
+				__FUNCTION__, *frame_seq, bus->tx_seq));
+			*frame_seq = bus->tx_seq;
+		}
+
+		ret = dhd_bcmsdh_send_buf(bus, bcmsdh_cur_sbwad(sdh), SDIO_FUNC_2, F2SYNC,
+		                      (uint8 *)bus->ctrl_frame_buf, (uint32)bus->ctrl_frame_len,
+			NULL, NULL, NULL);
+		ASSERT(ret != BCME_PENDING);
+
+		if (ret < 0) {
+			/* On failure, abort the command and terminate the frame */
+			DHD_INFO(("%s: sdio error %d, abort command and terminate frame.\n",
+			          __FUNCTION__, ret));
+			bus->tx_sderrs++;
+
+			bcmsdh_abort(sdh, SDIO_FUNC_2);
+
+			bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_FRAMECTRL,
+			                 SFC_WF_TERM, NULL);
+			bus->f1regdata++;
+
+			for (i = 0; i < 3; i++) {
+				uint8 hi, lo;
+				hi = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+				                     SBSDIO_FUNC1_WFRAMEBCHI, NULL);
+				lo = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+				                     SBSDIO_FUNC1_WFRAMEBCLO, NULL);
+				bus->f1regdata += 2;
+				if ((hi == 0) && (lo == 0))
+					break;
+			}
+		}
+		if (ret == 0) {
+			bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQUENCE_WRAP;
+		}
+
+		bus->ctrl_frame_stat = FALSE;
+		dhd_wait_event_wakeup(bus->dhd);
+	}
+	/* Send queued frames (limit 1 if rx may still be pending) */
+	else if ((bus->clkstate == CLK_AVAIL) && !bus->fcstate &&
+	    pktq_mlen(&bus->txq, ~bus->flowcontrol) && txlimit && DATAOK(bus)) {
+		framecnt = rxdone ? txlimit : MIN(txlimit, dhd_txminmax);
+		framecnt = dhdsdio_sendfromq(bus, framecnt);
+		txlimit -= framecnt;
+	}
+	/* Resched the DPC if ctrl cmd is pending on bus credit */
+	if (bus->ctrl_frame_stat)
+		resched = TRUE;
+
+	/* Resched if events or tx frames are pending, else await next interrupt */
+	/* On failed register access, all bets are off: no resched or interrupts */
+	if ((bus->dhd->busstate == DHD_BUS_DOWN) || bcmsdh_regfail(sdh)) {
+		DHD_ERROR(("%s: failed backplane access over SDIO, halting operation %d \n",
+		           __FUNCTION__, bcmsdh_regfail(sdh)));
+		bus->dhd->busstate = DHD_BUS_DOWN;
+		bus->intstatus = 0;
+	} else if (bus->clkstate == CLK_PENDING) {
+		/* Awaiting I_CHIPACTIVE; don't resched */
+	} else if (bus->intstatus || bus->ipend ||
+	           (!bus->fcstate && pktq_mlen(&bus->txq, ~bus->flowcontrol) && DATAOK(bus)) ||
+			PKT_AVAILABLE(bus, bus->intstatus)) {  /* Read multiple frames */
+		resched = TRUE;
+	}
+
+	bus->dpc_sched = resched;
+
+	/* If we're done for now, turn off clock request. */
+	if ((bus->idletime == DHD_IDLE_IMMEDIATE) && (bus->clkstate != CLK_PENDING)) {
+		bus->activity = FALSE;
+		dhdsdio_clkctl(bus, CLK_NONE, FALSE);
+	}
+
+	dhd_os_sdunlock(bus->dhd);
+	return resched;
+}
+
+bool
+dhd_bus_dpc(struct dhd_bus *bus)
+{
+	bool resched;
+
+	/* Call the DPC directly. */
+	DHD_TRACE(("Calling dhdsdio_dpc() from %s\n", __FUNCTION__));
+	resched = dhdsdio_dpc(bus);
+
+	return resched;
+}
+
+void
+dhdsdio_isr(void *arg)
+{
+	dhd_bus_t *bus = (dhd_bus_t*)arg;
+	bcmsdh_info_t *sdh;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (!bus) {
+		DHD_ERROR(("%s : bus is null pointer , exit \n", __FUNCTION__));
+		return;
+	}
+	sdh = bus->sdh;
+
+	if (bus->dhd->busstate == DHD_BUS_DOWN) {
+		DHD_ERROR(("%s : bus is down. we have nothing to do\n", __FUNCTION__));
+		return;
+	}
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	/* Count the interrupt call */
+	bus->intrcount++;
+	bus->ipend = TRUE;
+
+	/* Shouldn't get this interrupt if we're sleeping? */
+	if (bus->sleeping) {
+		DHD_ERROR(("INTERRUPT WHILE SLEEPING??\n"));
+		return;
+	}
+
+	/* Disable additional interrupts (is this needed now)? */
+	if (bus->intr) {
+		DHD_INTR(("%s: disable SDIO interrupts\n", __FUNCTION__));
+	} else {
+		DHD_ERROR(("dhdsdio_isr() w/o interrupt configured!\n"));
+	}
+
+	bcmsdh_intr_disable(sdh);
+	bus->intdis = TRUE;
+
+#if defined(SDIO_ISR_THREAD)
+	DHD_TRACE(("Calling dhdsdio_dpc() from %s\n", __FUNCTION__));
+	DHD_OS_WAKE_LOCK(bus->dhd);
+	while (dhdsdio_dpc(bus));
+	DHD_OS_WAKE_UNLOCK(bus->dhd);
+#else
+	bus->dpc_sched = TRUE;
+	dhd_sched_dpc(bus->dhd);
+#endif 
+
+}
+
+#ifdef SDTEST
+static void
+dhdsdio_pktgen_init(dhd_bus_t *bus)
+{
+	/* Default to specified length, or full range */
+	if (dhd_pktgen_len) {
+		bus->pktgen_maxlen = MIN(dhd_pktgen_len, MAX_PKTGEN_LEN);
+		bus->pktgen_minlen = bus->pktgen_maxlen;
+	} else {
+		bus->pktgen_maxlen = MAX_PKTGEN_LEN;
+		bus->pktgen_minlen = 0;
+	}
+	bus->pktgen_len = (uint16)bus->pktgen_minlen;
+
+	/* Default to per-watchdog burst with 10s print time */
+	bus->pktgen_freq = 1;
+	bus->pktgen_print = 10000 / dhd_watchdog_ms;
+	bus->pktgen_count = (dhd_pktgen * dhd_watchdog_ms + 999) / 1000;
+
+	/* Default to echo mode */
+	bus->pktgen_mode = DHD_PKTGEN_ECHO;
+	bus->pktgen_stop = 1;
+}
+
+static void
+dhdsdio_pktgen(dhd_bus_t *bus)
+{
+	void *pkt;
+	uint8 *data;
+	uint pktcount;
+	uint fillbyte;
+	osl_t *osh = bus->dhd->osh;
+	uint16 len;
+
+	/* Display current count if appropriate */
+	if (bus->pktgen_print && (++bus->pktgen_ptick >= bus->pktgen_print)) {
+		bus->pktgen_ptick = 0;
+		printf("%s: send attempts %d rcvd %d\n",
+		       __FUNCTION__, bus->pktgen_sent, bus->pktgen_rcvd);
+	}
+
+	/* For recv mode, just make sure dongle has started sending */
+	if (bus->pktgen_mode == DHD_PKTGEN_RECV) {
+		if (bus->pktgen_rcv_state == PKTGEN_RCV_IDLE) {
+			bus->pktgen_rcv_state = PKTGEN_RCV_ONGOING;
+			dhdsdio_sdtest_set(bus, (uint8)bus->pktgen_total);
+		}
+		return;
+	}
+
+	/* Otherwise, generate or request the specified number of packets */
+	for (pktcount = 0; pktcount < bus->pktgen_count; pktcount++) {
+		/* Stop if total has been reached */
+		if (bus->pktgen_total && (bus->pktgen_sent >= bus->pktgen_total)) {
+			bus->pktgen_count = 0;
+			break;
+		}
+
+		/* Allocate an appropriate-sized packet */
+		len = bus->pktgen_len;
+		if (!(pkt = PKTGET(osh, (len + SDPCM_HDRLEN + SDPCM_TEST_HDRLEN + DHD_SDALIGN),
+		                   TRUE))) {;
+			DHD_ERROR(("%s: PKTGET failed!\n", __FUNCTION__));
+			break;
+		}
+		PKTALIGN(osh, pkt, (len + SDPCM_HDRLEN + SDPCM_TEST_HDRLEN), DHD_SDALIGN);
+		data = (uint8*)PKTDATA(osh, pkt) + SDPCM_HDRLEN;
+
+		/* Write test header cmd and extra based on mode */
+		switch (bus->pktgen_mode) {
+		case DHD_PKTGEN_ECHO:
+			*data++ = SDPCM_TEST_ECHOREQ;
+			*data++ = (uint8)bus->pktgen_sent;
+			break;
+
+		case DHD_PKTGEN_SEND:
+			*data++ = SDPCM_TEST_DISCARD;
+			*data++ = (uint8)bus->pktgen_sent;
+			break;
+
+		case DHD_PKTGEN_RXBURST:
+			*data++ = SDPCM_TEST_BURST;
+			*data++ = (uint8)bus->pktgen_count;
+			break;
+
+		default:
+			DHD_ERROR(("Unrecognized pktgen mode %d\n", bus->pktgen_mode));
+			PKTFREE(osh, pkt, TRUE);
+			bus->pktgen_count = 0;
+			return;
+		}
+
+		/* Write test header length field */
+		*data++ = (len >> 0);
+		*data++ = (len >> 8);
+
+		/* Then fill in the remainder -- N/A for burst, but who cares... */
+		for (fillbyte = 0; fillbyte < len; fillbyte++)
+			*data++ = SDPCM_TEST_FILL(fillbyte, (uint8)bus->pktgen_sent);
+
+#ifdef DHD_DEBUG
+		if (DHD_BYTES_ON() && DHD_DATA_ON()) {
+			data = (uint8*)PKTDATA(osh, pkt) + SDPCM_HDRLEN;
+			prhex("dhdsdio_pktgen: Tx Data", data, PKTLEN(osh, pkt) - SDPCM_HDRLEN);
+		}
+#endif
+
+		/* Send it */
+		if (dhdsdio_txpkt(bus, pkt, SDPCM_TEST_CHANNEL, TRUE)) {
+			bus->pktgen_fail++;
+			if (bus->pktgen_stop && bus->pktgen_stop == bus->pktgen_fail)
+				bus->pktgen_count = 0;
+		}
+		bus->pktgen_sent++;
+
+		/* Bump length if not fixed, wrap at max */
+		if (++bus->pktgen_len > bus->pktgen_maxlen)
+			bus->pktgen_len = (uint16)bus->pktgen_minlen;
+
+		/* Special case for burst mode: just send one request! */
+		if (bus->pktgen_mode == DHD_PKTGEN_RXBURST)
+			break;
+	}
+}
+
+static void
+dhdsdio_sdtest_set(dhd_bus_t *bus, uint8 count)
+{
+	void *pkt;
+	uint8 *data;
+	osl_t *osh = bus->dhd->osh;
+
+	/* Allocate the packet */
+	if (!(pkt = PKTGET(osh, SDPCM_HDRLEN + SDPCM_TEST_HDRLEN + DHD_SDALIGN, TRUE))) {
+		DHD_ERROR(("%s: PKTGET failed!\n", __FUNCTION__));
+		return;
+	}
+	PKTALIGN(osh, pkt, (SDPCM_HDRLEN + SDPCM_TEST_HDRLEN), DHD_SDALIGN);
+	data = (uint8*)PKTDATA(osh, pkt) + SDPCM_HDRLEN;
+
+	/* Fill in the test header */
+	*data++ = SDPCM_TEST_SEND;
+	*data++ = count;
+	*data++ = (bus->pktgen_maxlen >> 0);
+	*data++ = (bus->pktgen_maxlen >> 8);
+
+	/* Send it */
+	if (dhdsdio_txpkt(bus, pkt, SDPCM_TEST_CHANNEL, TRUE))
+		bus->pktgen_fail++;
+}
+
+
+static void
+dhdsdio_testrcv(dhd_bus_t *bus, void *pkt, uint seq)
+{
+	osl_t *osh = bus->dhd->osh;
+	uint8 *data;
+	uint pktlen;
+
+	uint8 cmd;
+	uint8 extra;
+	uint16 len;
+	uint16 offset;
+
+	/* Check for min length */
+	if ((pktlen = PKTLEN(osh, pkt)) < SDPCM_TEST_HDRLEN) {
+		DHD_ERROR(("dhdsdio_restrcv: toss runt frame, pktlen %d\n", pktlen));
+		PKTFREE(osh, pkt, FALSE);
+		return;
+	}
+
+	/* Extract header fields */
+	data = PKTDATA(osh, pkt);
+	cmd = *data++;
+	extra = *data++;
+	len = *data++; len += *data++ << 8;
+	DHD_TRACE(("%s:cmd:%d, xtra:%d,len:%d\n", __FUNCTION__, cmd, extra, len));
+	/* Check length for relevant commands */
+	if (cmd == SDPCM_TEST_DISCARD || cmd == SDPCM_TEST_ECHOREQ || cmd == SDPCM_TEST_ECHORSP) {
+		if (pktlen != len + SDPCM_TEST_HDRLEN) {
+			DHD_ERROR(("dhdsdio_testrcv: frame length mismatch, pktlen %d seq %d"
+			           " cmd %d extra %d len %d\n", pktlen, seq, cmd, extra, len));
+			PKTFREE(osh, pkt, FALSE);
+			return;
+		}
+	}
+
+	/* Process as per command */
+	switch (cmd) {
+	case SDPCM_TEST_ECHOREQ:
+		/* Rx->Tx turnaround ok (even on NDIS w/current implementation) */
+		*(uint8 *)(PKTDATA(osh, pkt)) = SDPCM_TEST_ECHORSP;
+		if (dhdsdio_txpkt(bus, pkt, SDPCM_TEST_CHANNEL, TRUE) == 0) {
+			bus->pktgen_sent++;
+		} else {
+			bus->pktgen_fail++;
+			PKTFREE(osh, pkt, FALSE);
+		}
+		bus->pktgen_rcvd++;
+		break;
+
+	case SDPCM_TEST_ECHORSP:
+		if (bus->ext_loop) {
+			PKTFREE(osh, pkt, FALSE);
+			bus->pktgen_rcvd++;
+			break;
+		}
+
+		for (offset = 0; offset < len; offset++, data++) {
+			if (*data != SDPCM_TEST_FILL(offset, extra)) {
+				DHD_ERROR(("dhdsdio_testrcv: echo data mismatch: "
+				           "offset %d (len %d) expect 0x%02x rcvd 0x%02x\n",
+				           offset, len, SDPCM_TEST_FILL(offset, extra), *data));
+				break;
+			}
+		}
+		PKTFREE(osh, pkt, FALSE);
+		bus->pktgen_rcvd++;
+		break;
+
+	case SDPCM_TEST_DISCARD:
+		{
+			int i = 0;
+			uint8 *prn = data;
+			uint8 testval = extra;
+			for (i = 0; i < len; i++) {
+				if (*prn != testval) {
+					DHD_ERROR(("DIErr@Pkt#:%d,Ix:%d, expected:0x%x, got:0x%x\n",
+						i, bus->pktgen_rcvd_rcvsession, testval, *prn));
+					prn++; testval++;
+				}
+			}
+		}
+		PKTFREE(osh, pkt, FALSE);
+		bus->pktgen_rcvd++;
+		break;
+
+	case SDPCM_TEST_BURST:
+	case SDPCM_TEST_SEND:
+	default:
+		DHD_INFO(("dhdsdio_testrcv: unsupported or unknown command, pktlen %d seq %d"
+		          " cmd %d extra %d len %d\n", pktlen, seq, cmd, extra, len));
+		PKTFREE(osh, pkt, FALSE);
+		break;
+	}
+
+	/* For recv mode, stop at limit (and tell dongle to stop sending) */
+	if (bus->pktgen_mode == DHD_PKTGEN_RECV) {
+		if (bus->pktgen_rcv_state != PKTGEN_RCV_IDLE) {
+			bus->pktgen_rcvd_rcvsession++;
+
+			if (bus->pktgen_total &&
+				(bus->pktgen_rcvd_rcvsession >= bus->pktgen_total)) {
+			bus->pktgen_count = 0;
+	            DHD_ERROR(("Pktgen:rcv test complete!\n"));
+	            bus->pktgen_rcv_state = PKTGEN_RCV_IDLE;
+			dhdsdio_sdtest_set(bus, FALSE);
+				bus->pktgen_rcvd_rcvsession = 0;
+			}
+		}
+	}
+}
+#endif /* SDTEST */
+
+extern void
+dhd_disable_intr(dhd_pub_t *dhdp)
+{
+	dhd_bus_t *bus;
+	bus = dhdp->bus;
+	bcmsdh_intr_disable(bus->sdh);
+}
+
+extern bool
+dhd_bus_watchdog(dhd_pub_t *dhdp)
+{
+	dhd_bus_t *bus;
+
+	DHD_TIMER(("%s: Enter\n", __FUNCTION__));
+
+	bus = dhdp->bus;
+
+	if (bus->dhd->dongle_reset)
+		return FALSE;
+
+	/* Ignore the timer if simulating bus down */
+	if (bus->sleeping)
+		return FALSE;
+
+	if (dhdp->busstate == DHD_BUS_DOWN)
+		return FALSE;
+
+	/* Poll period: check device if appropriate. */
+	if (bus->poll && (++bus->polltick >= bus->pollrate)) {
+		uint32 intstatus = 0;
+
+		/* Reset poll tick */
+		bus->polltick = 0;
+
+		/* Check device if no interrupts */
+		if (!bus->intr || (bus->intrcount == bus->lastintrs)) {
+
+			if (!bus->dpc_sched) {
+				uint8 devpend;
+				devpend = bcmsdh_cfg_read(bus->sdh, SDIO_FUNC_0,
+				                          SDIOD_CCCR_INTPEND, NULL);
+				intstatus = devpend & (INTR_STATUS_FUNC1 | INTR_STATUS_FUNC2);
+			}
+
+			/* If there is something, make like the ISR and schedule the DPC */
+			if (intstatus) {
+				bus->pollcnt++;
+				bus->ipend = TRUE;
+				if (bus->intr) {
+					bcmsdh_intr_disable(bus->sdh);
+				}
+				bus->dpc_sched = TRUE;
+				dhd_sched_dpc(bus->dhd);
+
+			}
+		}
+
+		/* Update interrupt tracking */
+		bus->lastintrs = bus->intrcount;
+	}
+
+#ifdef DHD_DEBUG
+	/* Poll for console output periodically */
+	if (dhdp->busstate == DHD_BUS_DATA && dhd_console_ms != 0) {
+		bus->console.count += dhd_watchdog_ms;
+		if (bus->console.count >= dhd_console_ms) {
+			bus->console.count -= dhd_console_ms;
+			/* Make sure backplane clock is on */
+			dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+			if (dhdsdio_readconsole(bus) < 0)
+				dhd_console_ms = 0;	/* On error, stop trying */
+		}
+	}
+#endif /* DHD_DEBUG */
+
+#ifdef SDTEST
+	/* Generate packets if configured */
+	if (bus->pktgen_count && (++bus->pktgen_tick >= bus->pktgen_freq)) {
+		/* Make sure backplane clock is on */
+		dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+		bus->pktgen_tick = 0;
+		dhdsdio_pktgen(bus);
+	}
+#endif
+
+	/* On idle timeout clear activity flag and/or turn off clock */
+	if ((bus->idletime > 0) && (bus->clkstate == CLK_AVAIL)) {
+		if (++bus->idlecount >= bus->idletime) {
+			bus->idlecount = 0;
+			if (bus->activity) {
+				bus->activity = FALSE;
+				dhdsdio_clkctl(bus, CLK_NONE, FALSE);
+			}
+		}
+	}
+
+	return bus->ipend;
+}
+
+#ifdef DHD_DEBUG
+extern int
+dhd_bus_console_in(dhd_pub_t *dhdp, uchar *msg, uint msglen)
+{
+	dhd_bus_t *bus = dhdp->bus;
+	uint32 addr, val;
+	int rv;
+	void *pkt;
+
+	/* Address could be zero if CONSOLE := 0 in dongle Makefile */
+	if (bus->console_addr == 0)
+		return BCME_UNSUPPORTED;
+
+	/* Exclusive bus access */
+	dhd_os_sdlock(bus->dhd);
+
+	/* Don't allow input if dongle is in reset */
+	if (bus->dhd->dongle_reset) {
+		dhd_os_sdunlock(bus->dhd);
+		return BCME_NOTREADY;
+	}
+
+	/* Request clock to allow SDIO accesses */
+	BUS_WAKE(bus);
+	/* No pend allowed since txpkt is called later, ht clk has to be on */
+	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+
+	/* Zero cbuf_index */
+	addr = bus->console_addr + OFFSETOF(hndrte_cons_t, cbuf_idx);
+	val = htol32(0);
+	if ((rv = dhdsdio_membytes(bus, TRUE, addr, (uint8 *)&val, sizeof(val))) < 0)
+		goto done;
+
+	/* Write message into cbuf */
+	addr = bus->console_addr + OFFSETOF(hndrte_cons_t, cbuf);
+	if ((rv = dhdsdio_membytes(bus, TRUE, addr, (uint8 *)msg, msglen)) < 0)
+		goto done;
+
+	/* Write length into vcons_in */
+	addr = bus->console_addr + OFFSETOF(hndrte_cons_t, vcons_in);
+	val = htol32(msglen);
+	if ((rv = dhdsdio_membytes(bus, TRUE, addr, (uint8 *)&val, sizeof(val))) < 0)
+		goto done;
+
+	/* Bump dongle by sending an empty packet on the event channel.
+	 * sdpcm_sendup (RX) checks for virtual console input.
+	 */
+	if ((pkt = PKTGET(bus->dhd->osh, 4 + SDPCM_RESERVE, TRUE)) != NULL)
+		dhdsdio_txpkt(bus, pkt, SDPCM_EVENT_CHANNEL, TRUE);
+
+done:
+	if ((bus->idletime == DHD_IDLE_IMMEDIATE) && !bus->dpc_sched) {
+		bus->activity = FALSE;
+		dhdsdio_clkctl(bus, CLK_NONE, TRUE);
+	}
+
+	dhd_os_sdunlock(bus->dhd);
+
+	return rv;
+}
+#endif /* DHD_DEBUG */
+
+#ifdef DHD_DEBUG
+static void
+dhd_dump_cis(uint fn, uint8 *cis)
+{
+	uint byte, tag, tdata;
+	DHD_INFO(("Function %d CIS:\n", fn));
+
+	for (tdata = byte = 0; byte < SBSDIO_CIS_SIZE_LIMIT; byte++) {
+		if ((byte % 16) == 0)
+			DHD_INFO(("    "));
+		DHD_INFO(("%02x ", cis[byte]));
+		if ((byte % 16) == 15)
+			DHD_INFO(("\n"));
+		if (!tdata--) {
+			tag = cis[byte];
+			if (tag == 0xff)
+				break;
+			else if (!tag)
+				tdata = 0;
+			else if ((byte + 1) < SBSDIO_CIS_SIZE_LIMIT)
+				tdata = cis[byte + 1] + 1;
+			else
+				DHD_INFO(("]"));
+		}
+	}
+	if ((byte % 16) != 15)
+		DHD_INFO(("\n"));
+}
+#endif /* DHD_DEBUG */
+
+static bool
+dhdsdio_chipmatch(uint16 chipid)
+{
+	if (chipid == BCM4325_CHIP_ID)
+		return TRUE;
+	if (chipid == BCM4329_CHIP_ID)
+		return TRUE;
+	if (chipid == BCM4315_CHIP_ID)
+		return TRUE;
+	if (chipid == BCM4319_CHIP_ID)
+		return TRUE;
+	if (chipid == BCM4330_CHIP_ID)
+		return TRUE;
+	if (chipid == BCM43239_CHIP_ID)
+		return TRUE;
+	if (chipid == BCM4336_CHIP_ID)
+		return TRUE;
+	if (chipid == BCM43237_CHIP_ID)
+		return TRUE;
+	if (chipid == BCM43362_CHIP_ID)
+		return TRUE;
+
+	return FALSE;
+}
+
+static void *
+dhdsdio_probe(uint16 venid, uint16 devid, uint16 bus_no, uint16 slot,
+	uint16 func, uint bustype, void *regsva, osl_t * osh, void *sdh, void *dev)
+{
+	int ret;
+	dhd_bus_t *bus;
+	dhd_cmn_t *cmn;
+#ifdef GET_CUSTOM_MAC_ENABLE
+	struct ether_addr ea_addr;
+#endif /* GET_CUSTOM_MAC_ENABLE */
+#ifdef PROP_TXSTATUS
+	uint up = 0;
+#endif
+
+	/* Init global variables at run-time, not as part of the declaration.
+	 * This is required to support init/de-init of the driver. Initialization
+	 * of globals as part of the declaration results in non-deterministic
+	 * behavior since the value of the globals may be different on the
+	 * first time that the driver is initialized vs subsequent initializations.
+	 */
+	dhd_txbound = DHD_TXBOUND;
+	dhd_rxbound = DHD_RXBOUND;
+	dhd_alignctl = TRUE;
+	sd1idle = TRUE;
+	dhd_readahead = TRUE;
+	retrydata = FALSE;
+	dhd_doflow = FALSE;
+	dhd_dongle_memsize = 0;
+	dhd_txminmax = DHD_TXMINMAX;
+
+	forcealign = TRUE;
+
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+	DHD_INFO(("%s: venid 0x%04x devid 0x%04x\n", __FUNCTION__, venid, devid));
+
+	/* We make assumptions about address window mappings */
+	ASSERT((uintptr)regsva == SI_ENUM_BASE);
+
+	/* BCMSDH passes venid and devid based on CIS parsing -- but low-power start
+	 * means early parse could fail, so here we should get either an ID
+	 * we recognize OR (-1) indicating we must request power first.
+	 */
+	/* Check the Vendor ID */
+	switch (venid) {
+		case 0x0000:
+		case VENDOR_BROADCOM:
+			break;
+		default:
+			DHD_ERROR(("%s: unknown vendor: 0x%04x\n",
+			           __FUNCTION__, venid));
+			return NULL;
+	}
+
+	/* Check the Device ID and make sure it's one that we support */
+	switch (devid) {
+		case BCM4325_D11DUAL_ID:		/* 4325 802.11a/g id */
+		case BCM4325_D11G_ID:			/* 4325 802.11g 2.4Ghz band id */
+		case BCM4325_D11A_ID:			/* 4325 802.11a 5Ghz band id */
+			DHD_INFO(("%s: found 4325 Dongle\n", __FUNCTION__));
+			break;
+		case BCM4329_D11N_ID:		/* 4329 802.11n dualband device */
+		case BCM4329_D11N2G_ID:		/* 4329 802.11n 2.4G device */
+		case BCM4329_D11N5G_ID:		/* 4329 802.11n 5G device */
+		case 0x4329:
+			DHD_INFO(("%s: found 4329 Dongle\n", __FUNCTION__));
+			break;
+		case BCM4315_D11DUAL_ID:		/* 4315 802.11a/g id */
+		case BCM4315_D11G_ID:			/* 4315 802.11g id */
+		case BCM4315_D11A_ID:			/* 4315 802.11a id */
+			DHD_INFO(("%s: found 4315 Dongle\n", __FUNCTION__));
+			break;
+		case BCM4319_D11N_ID:			/* 4319 802.11n id */
+		case BCM4319_D11N2G_ID:			/* 4319 802.11n2g id */
+		case BCM4319_D11N5G_ID:			/* 4319 802.11n5g id */
+			DHD_INFO(("%s: found 4319 Dongle\n", __FUNCTION__));
+			break;
+		case 0:
+			DHD_INFO(("%s: allow device id 0, will check chip internals\n",
+			          __FUNCTION__));
+			break;
+
+		default:
+			DHD_ERROR(("%s: skipping 0x%04x/0x%04x, not a dongle\n",
+			           __FUNCTION__, venid, devid));
+			return NULL;
+	}
+
+	if (osh == NULL) {
+		/* Ask the OS interface part for an OSL handle */
+		if (!(osh = dhd_osl_attach(sdh, DHD_BUS))) {
+			DHD_ERROR(("%s: osl_attach failed!\n", __FUNCTION__));
+			return NULL;
+		}
+	}
+
+	/* Allocate private bus interface state */
+	if (!(bus = MALLOC(osh, sizeof(dhd_bus_t)))) {
+		DHD_ERROR(("%s: MALLOC of dhd_bus_t failed\n", __FUNCTION__));
+		goto fail;
+	}
+	bzero(bus, sizeof(dhd_bus_t));
+	bus->sdh = sdh;
+	bus->cl_devid = (uint16)devid;
+	bus->bus = DHD_BUS;
+	bus->tx_seq = SDPCM_SEQUENCE_WRAP - 1;
+	bus->usebufpool = FALSE; /* Use bufpool if allocated, else use locally malloced rxbuf */
+
+	/* attach the common module */
+	if (!(cmn = dhd_common_init(bus->cl_devid, osh))) {
+		DHD_ERROR(("%s: dhd_common_init failed\n", __FUNCTION__));
+		goto fail;
+	}
+
+	/* attempt to attach to the dongle */
+	if (!(dhdsdio_probe_attach(bus, osh, sdh, regsva, devid))) {
+		DHD_ERROR(("%s: dhdsdio_probe_attach failed\n", __FUNCTION__));
+		dhd_common_deinit(NULL, cmn);
+		goto fail;
+	}
+
+	/* Attach to the dhd/OS/network interface */
+	if (!(bus->dhd = dhd_attach(osh, bus, SDPCM_RESERVE, dev))) {
+		DHD_ERROR(("%s: dhd_attach failed\n", __FUNCTION__));
+		goto fail;
+	}
+
+	bus->dhd->cmn = cmn;
+	cmn->dhd = bus->dhd;
+
+	/* Allocate buffers */
+	if (!(dhdsdio_probe_malloc(bus, osh, sdh))) {
+		DHD_ERROR(("%s: dhdsdio_probe_malloc failed\n", __FUNCTION__));
+		goto fail;
+	}
+
+	if (!(dhdsdio_probe_init(bus, osh, sdh))) {
+		DHD_ERROR(("%s: dhdsdio_probe_init failed\n", __FUNCTION__));
+		goto fail;
+	}
+
+	if (bus->intr) {
+		/* Register interrupt callback, but mask it (not operational yet). */
+		DHD_INTR(("%s: disable SDIO interrupts (not interested yet)\n", __FUNCTION__));
+		bcmsdh_intr_disable(sdh);
+		if ((ret = bcmsdh_intr_reg(sdh, dhdsdio_isr, bus)) != 0) {
+			DHD_ERROR(("%s: FAILED: bcmsdh_intr_reg returned %d\n",
+			           __FUNCTION__, ret));
+			goto fail;
+		}
+		DHD_INTR(("%s: registered SDIO interrupt function ok\n", __FUNCTION__));
+	} else {
+		DHD_INFO(("%s: SDIO interrupt function is NOT registered due to polling mode\n",
+		           __FUNCTION__));
+	}
+
+	DHD_INFO(("%s: completed!!\n", __FUNCTION__));
+
+#ifdef GET_CUSTOM_MAC_ENABLE
+	/* Read MAC address from external customer place 	*/
+	memset(&ea_addr, 0, sizeof(ea_addr));
+	ret = dhd_custom_get_mac_address(ea_addr.octet);
+	if (!ret) {
+		memcpy(bus->dhd->mac.octet, (void *)&ea_addr, ETHER_ADDR_LEN);
+	}
+#endif /* GET_CUSTOM_MAC_ENABLE */
+
+	/* if firmware path present try to download and bring up bus */
+	if (dhd_download_fw_on_driverload && (ret = dhd_bus_start(bus->dhd)) != 0) {
+		DHD_ERROR(("%s: dhd_bus_start failed\n", __FUNCTION__));
+		if (ret == BCME_NOTUP)
+			goto fail;
+	}
+
+	/* Ok, have the per-port tell the stack we're open for business */
+	if (dhd_net_attach(bus->dhd, 0) != 0) {
+		DHD_ERROR(("%s: Net attach failed!!\n", __FUNCTION__));
+		goto fail;
+	}
+
+#ifdef PROP_TXSTATUS
+	if (dhd_download_fw_on_driverload)
+		dhd_wl_ioctl_cmd(bus->dhd, WLC_UP, (char *)&up, sizeof(up), TRUE, 0);
+#endif
+	return bus;
+
+fail:
+	dhdsdio_release(bus, osh);
+	return NULL;
+}
+
+static bool
+dhdsdio_probe_attach(struct dhd_bus *bus, osl_t *osh, void *sdh, void *regsva,
+                     uint16 devid)
+{
+	int err = 0;
+	uint8 clkctl = 0;
+
+	bus->alp_only = TRUE;
+
+	/* Return the window to backplane enumeration space for core access */
+	if (dhdsdio_set_siaddr_window(bus, SI_ENUM_BASE)) {
+		DHD_ERROR(("%s: FAILED to return to SI_ENUM_BASE\n", __FUNCTION__));
+	}
+
+#ifdef DHD_DEBUG
+	DHD_ERROR(("F1 signature read @0x18000000=0x%4x\n",
+	       bcmsdh_reg_read(bus->sdh, SI_ENUM_BASE, 4)));
+
+#endif /* DHD_DEBUG */
+
+
+	/* Force PLL off until si_attach() programs PLL control regs */
+
+
+
+	bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, DHD_INIT_CLKCTL1, &err);
+	if (!err)
+		clkctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, &err);
+
+	if (err || ((clkctl & ~SBSDIO_AVBITS) != DHD_INIT_CLKCTL1)) {
+		DHD_ERROR(("dhdsdio_probe: ChipClkCSR access: err %d wrote 0x%02x read 0x%02x\n",
+		           err, DHD_INIT_CLKCTL1, clkctl));
+		goto fail;
+	}
+
+
+#ifdef DHD_DEBUG
+	if (DHD_INFO_ON()) {
+		uint fn, numfn;
+		uint8 *cis[SDIOD_MAX_IOFUNCS];
+		int err = 0;
+
+		numfn = bcmsdh_query_iofnum(sdh);
+		ASSERT(numfn <= SDIOD_MAX_IOFUNCS);
+
+		/* Make sure ALP is available before trying to read CIS */
+		SPINWAIT(((clkctl = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+		                                    SBSDIO_FUNC1_CHIPCLKCSR, NULL)),
+		          !SBSDIO_ALPAV(clkctl)), PMU_MAX_TRANSITION_DLY);
+
+		/* Now request ALP be put on the bus */
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
+		                 DHD_INIT_CLKCTL2, &err);
+		OSL_DELAY(65);
+
+		for (fn = 0; fn <= numfn; fn++) {
+			if (!(cis[fn] = MALLOC(osh, SBSDIO_CIS_SIZE_LIMIT))) {
+				DHD_INFO(("dhdsdio_probe: fn %d cis malloc failed\n", fn));
+				break;
+			}
+			bzero(cis[fn], SBSDIO_CIS_SIZE_LIMIT);
+
+			if ((err = bcmsdh_cis_read(sdh, fn, cis[fn], SBSDIO_CIS_SIZE_LIMIT))) {
+				DHD_INFO(("dhdsdio_probe: fn %d cis read err %d\n", fn, err));
+				MFREE(osh, cis[fn], SBSDIO_CIS_SIZE_LIMIT);
+				break;
+			}
+			dhd_dump_cis(fn, cis[fn]);
+		}
+
+		while (fn-- > 0) {
+			ASSERT(cis[fn]);
+			MFREE(osh, cis[fn], SBSDIO_CIS_SIZE_LIMIT);
+		}
+
+		if (err) {
+			DHD_ERROR(("dhdsdio_probe: failure reading or parsing CIS\n"));
+			goto fail;
+		}
+	}
+#endif /* DHD_DEBUG */
+
+	/* si_attach() will provide an SI handle and scan the backplane */
+	if (!(bus->sih = si_attach((uint)devid, osh, regsva, DHD_BUS, sdh,
+	                           &bus->vars, &bus->varsz))) {
+		DHD_ERROR(("%s: si_attach failed!\n", __FUNCTION__));
+		goto fail;
+	}
+
+	bcmsdh_chipinfo(sdh, bus->sih->chip, bus->sih->chiprev);
+
+	if (!dhdsdio_chipmatch((uint16)bus->sih->chip)) {
+		DHD_ERROR(("%s: unsupported chip: 0x%04x\n",
+		           __FUNCTION__, bus->sih->chip));
+		goto fail;
+	}
+
+
+	si_sdiod_drive_strength_init(bus->sih, osh, dhd_sdiod_drive_strength);
+
+
+	/* Get info on the ARM and SOCRAM cores... */
+	if (!DHD_NOPMU(bus)) {
+		if ((si_setcore(bus->sih, ARM7S_CORE_ID, 0)) ||
+		    (si_setcore(bus->sih, ARMCM3_CORE_ID, 0))) {
+			bus->armrev = si_corerev(bus->sih);
+		} else {
+			DHD_ERROR(("%s: failed to find ARM core!\n", __FUNCTION__));
+			goto fail;
+		}
+		if (!(bus->orig_ramsize = si_socram_size(bus->sih))) {
+			DHD_ERROR(("%s: failed to find SOCRAM memory!\n", __FUNCTION__));
+			goto fail;
+		}
+		bus->ramsize = bus->orig_ramsize;
+		if (dhd_dongle_memsize)
+			dhd_dongle_setmemsize(bus, dhd_dongle_memsize);
+
+		DHD_ERROR(("DHD: dongle ram size is set to %d(orig %d)\n",
+			bus->ramsize, bus->orig_ramsize));
+	}
+
+	/* ...but normally deal with the SDPCMDEV core */
+	if (!(bus->regs = si_setcore(bus->sih, PCMCIA_CORE_ID, 0)) &&
+	    !(bus->regs = si_setcore(bus->sih, SDIOD_CORE_ID, 0))) {
+		DHD_ERROR(("%s: failed to find SDIODEV core!\n", __FUNCTION__));
+		goto fail;
+	}
+	bus->sdpcmrev = si_corerev(bus->sih);
+
+	/* Set core control so an SDIO reset does a backplane reset */
+	OR_REG(osh, &bus->regs->corecontrol, CC_BPRESEN);
+	bus->rxint_mode = SDIO_DEVICE_HMB_RXINT;
+
+	if ((bus->sih->buscoretype == SDIOD_CORE_ID) && (bus->sdpcmrev >= 4) &&
+		(bus->rxint_mode  == SDIO_DEVICE_RXDATAINT_MODE_1))
+	{
+		uint32 val;
+
+		val = R_REG(osh, &bus->regs->corecontrol);
+		val &= ~CC_XMTDATAAVAIL_MODE;
+		val |= CC_XMTDATAAVAIL_CTRL;
+		W_REG(osh, &bus->regs->corecontrol, val);
+	}
+
+
+	pktq_init(&bus->txq, (PRIOMASK + 1), QLEN);
+
+	/* Locate an appropriately-aligned portion of hdrbuf */
+	bus->rxhdr = (uint8 *)ROUNDUP((uintptr)&bus->hdrbuf[0], DHD_SDALIGN);
+
+	/* Set the poll and/or interrupt flags */
+	bus->intr = (bool)dhd_intr;
+	if ((bus->poll = (bool)dhd_poll))
+		bus->pollrate = 1;
+
+	return TRUE;
+
+fail:
+	if (bus->sih != NULL) {
+		si_detach(bus->sih);
+		bus->sih = NULL;
+	}
+	return FALSE;
+}
+
+static bool
+dhdsdio_probe_malloc(dhd_bus_t *bus, osl_t *osh, void *sdh)
+{
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (bus->dhd->maxctl) {
+		bus->rxblen = ROUNDUP((bus->dhd->maxctl + SDPCM_HDRLEN), ALIGNMENT) + DHD_SDALIGN;
+		if (!(bus->rxbuf = DHD_OS_PREALLOC(osh, DHD_PREALLOC_RXBUF, bus->rxblen))) {
+			DHD_ERROR(("%s: MALLOC of %d-byte rxbuf failed\n",
+			           __FUNCTION__, bus->rxblen));
+			goto fail;
+		}
+	}
+	/* Allocate buffer to receive glomed packet */
+	if (!(bus->databuf = DHD_OS_PREALLOC(osh, DHD_PREALLOC_DATABUF, MAX_DATA_BUF))) {
+		DHD_ERROR(("%s: MALLOC of %d-byte databuf failed\n",
+			__FUNCTION__, MAX_DATA_BUF));
+		/* release rxbuf which was already located as above */
+		if (!bus->rxblen)
+			DHD_OS_PREFREE(osh, bus->rxbuf, bus->rxblen);
+		goto fail;
+	}
+
+	/* Align the buffer */
+	if ((uintptr)bus->databuf % DHD_SDALIGN)
+		bus->dataptr = bus->databuf + (DHD_SDALIGN - ((uintptr)bus->databuf % DHD_SDALIGN));
+	else
+		bus->dataptr = bus->databuf;
+
+	return TRUE;
+
+fail:
+	return FALSE;
+}
+
+static bool
+dhdsdio_probe_init(dhd_bus_t *bus, osl_t *osh, void *sdh)
+{
+	int32 fnum;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+#ifdef SDTEST
+	dhdsdio_pktgen_init(bus);
+#endif /* SDTEST */
+
+	/* Disable F2 to clear any intermediate frame state on the dongle */
+	bcmsdh_cfg_write(sdh, SDIO_FUNC_0, SDIOD_CCCR_IOEN, SDIO_FUNC_ENABLE_1, NULL);
+
+	bus->dhd->busstate = DHD_BUS_DOWN;
+	bus->sleeping = FALSE;
+	bus->rxflow = FALSE;
+	bus->prev_rxlim_hit = 0;
+
+
+	/* Done with backplane-dependent accesses, can drop clock... */
+	bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, 0, NULL);
+
+	/* ...and initialize clock/power states */
+	bus->clkstate = CLK_SDONLY;
+	bus->idletime = (int32)dhd_idletime;
+	bus->idleclock = DHD_IDLE_ACTIVE;
+
+	/* Query the SD clock speed */
+	if (bcmsdh_iovar_op(sdh, "sd_divisor", NULL, 0,
+	                    &bus->sd_divisor, sizeof(int32), FALSE) != BCME_OK) {
+		DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, "sd_divisor"));
+		bus->sd_divisor = -1;
+	} else {
+		DHD_INFO(("%s: Initial value for %s is %d\n",
+		          __FUNCTION__, "sd_divisor", bus->sd_divisor));
+	}
+
+	/* Query the SD bus mode */
+	if (bcmsdh_iovar_op(sdh, "sd_mode", NULL, 0,
+	                    &bus->sd_mode, sizeof(int32), FALSE) != BCME_OK) {
+		DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, "sd_mode"));
+		bus->sd_mode = -1;
+	} else {
+		DHD_INFO(("%s: Initial value for %s is %d\n",
+		          __FUNCTION__, "sd_mode", bus->sd_mode));
+	}
+
+	/* Query the F2 block size, set roundup accordingly */
+	fnum = 2;
+	if (bcmsdh_iovar_op(sdh, "sd_blocksize", &fnum, sizeof(int32),
+	                    &bus->blocksize, sizeof(int32), FALSE) != BCME_OK) {
+		bus->blocksize = 0;
+		DHD_ERROR(("%s: fail on %s get\n", __FUNCTION__, "sd_blocksize"));
+	} else {
+		DHD_INFO(("%s: Initial value for %s is %d\n",
+		          __FUNCTION__, "sd_blocksize", bus->blocksize));
+	}
+	bus->roundup = MIN(max_roundup, bus->blocksize);
+
+	/* Query if bus module supports packet chaining, default to use if supported */
+	if (bcmsdh_iovar_op(sdh, "sd_rxchain", NULL, 0,
+	                    &bus->sd_rxchain, sizeof(int32), FALSE) != BCME_OK) {
+		bus->sd_rxchain = FALSE;
+	} else {
+		DHD_INFO(("%s: bus module (through bcmsdh API) %s chaining\n",
+		          __FUNCTION__, (bus->sd_rxchain ? "supports" : "does not support")));
+	}
+	bus->use_rxchain = (bool)bus->sd_rxchain;
+
+	return TRUE;
+}
+
+bool
+dhd_bus_download_firmware(struct dhd_bus *bus, osl_t *osh,
+                          char *pfw_path, char *pnv_path)
+{
+	bool ret;
+	bus->fw_path = pfw_path;
+	bus->nv_path = pnv_path;
+
+	ret = dhdsdio_download_firmware(bus, osh, bus->sdh);
+
+
+	return ret;
+}
+
+static bool
+dhdsdio_download_firmware(struct dhd_bus *bus, osl_t *osh, void *sdh)
+{
+	bool ret;
+
+	/* Download the firmware */
+	DHD_OS_WAKE_LOCK(bus->dhd);
+	dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+
+	ret = _dhdsdio_download_firmware(bus) == 0;
+
+	dhdsdio_clkctl(bus, CLK_SDONLY, FALSE);
+	DHD_OS_WAKE_UNLOCK(bus->dhd);
+	return ret;
+}
+
+/* Detach and free everything */
+static void
+dhdsdio_release(dhd_bus_t *bus, osl_t *osh)
+{
+	bool dongle_isolation = FALSE;
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (bus) {
+		ASSERT(osh);
+
+		/* De-register interrupt handler */
+		bcmsdh_intr_disable(bus->sdh);
+		bcmsdh_intr_dereg(bus->sdh);
+
+		if (bus->dhd) {
+			dhd_common_deinit(bus->dhd, NULL);
+			dongle_isolation = bus->dhd->dongle_isolation;
+			dhd_detach(bus->dhd);
+			dhdsdio_release_dongle(bus, osh, dongle_isolation, TRUE);
+			dhd_free(bus->dhd);
+			bus->dhd = NULL;
+		}
+
+		dhdsdio_release_malloc(bus, osh);
+
+#ifdef DHD_DEBUG
+		if (bus->console.buf != NULL)
+			MFREE(osh, bus->console.buf, bus->console.bufsize);
+#endif
+
+		MFREE(osh, bus, sizeof(dhd_bus_t));
+	}
+
+	if (osh)
+		dhd_osl_detach(osh);
+
+	DHD_TRACE(("%s: Disconnected\n", __FUNCTION__));
+}
+
+static void
+dhdsdio_release_malloc(dhd_bus_t *bus, osl_t *osh)
+{
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (bus->dhd && bus->dhd->dongle_reset)
+		return;
+
+	if (bus->rxbuf) {
+#ifndef CONFIG_DHD_USE_STATIC_BUF
+		MFREE(osh, bus->rxbuf, bus->rxblen);
+#endif
+		bus->rxctl = bus->rxbuf = NULL;
+		bus->rxlen = 0;
+	}
+
+	if (bus->databuf) {
+#ifndef CONFIG_DHD_USE_STATIC_BUF
+		MFREE(osh, bus->databuf, MAX_DATA_BUF);
+#endif
+		bus->databuf = NULL;
+	}
+
+	if (bus->vars && bus->varsz) {
+		MFREE(osh, bus->vars, bus->varsz);
+		bus->vars = NULL;
+	}
+
+}
+
+
+static void
+dhdsdio_release_dongle(dhd_bus_t *bus, osl_t *osh, bool dongle_isolation, bool reset_flag)
+{
+	DHD_TRACE(("%s: Enter bus->dhd %p bus->dhd->dongle_reset %d \n", __FUNCTION__,
+		bus->dhd, bus->dhd->dongle_reset));
+
+	if ((bus->dhd && bus->dhd->dongle_reset) && reset_flag)
+		return;
+
+	if (bus->sih) {
+		if (bus->dhd) {
+			dhdsdio_clkctl(bus, CLK_AVAIL, FALSE);
+		}
+#if !defined(BCMLXSDMMC)
+		if (dongle_isolation == FALSE)
+			si_watchdog(bus->sih, 4);
+#endif /* !defined(BCMLXSDMMC) */
+		if (bus->dhd) {
+			dhdsdio_clkctl(bus, CLK_NONE, FALSE);
+		}
+		si_detach(bus->sih);
+		bus->sih = NULL;
+		if (bus->vars && bus->varsz)
+			MFREE(osh, bus->vars, bus->varsz);
+		bus->vars = NULL;
+	}
+
+	DHD_TRACE(("%s: Disconnected\n", __FUNCTION__));
+}
+
+static void
+dhdsdio_disconnect(void *ptr)
+{
+	dhd_bus_t *bus = (dhd_bus_t *)ptr;
+
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	if (bus) {
+		ASSERT(bus->dhd);
+		dhdsdio_release(bus, bus->dhd->osh);
+	}
+
+	DHD_TRACE(("%s: Disconnected\n", __FUNCTION__));
+}
+
+
+/* Register/Unregister functions are called by the main DHD entry
+ * point (e.g. module insertion) to link with the bus driver, in
+ * order to look for or await the device.
+ */
+
+static bcmsdh_driver_t dhd_sdio = {
+	dhdsdio_probe,
+	dhdsdio_disconnect
+};
+
+int
+dhd_bus_register(void)
+{
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	return bcmsdh_register(&dhd_sdio);
+}
+
+void
+dhd_bus_unregister(void)
+{
+	DHD_TRACE(("%s: Enter\n", __FUNCTION__));
+
+	bcmsdh_unregister();
+}
+
+#ifdef BCMEMBEDIMAGE
+static int
+dhdsdio_download_code_array(struct dhd_bus *bus)
+{
+	int bcmerror = -1;
+	int offset = 0;
+	unsigned char *ularray = NULL;
+
+	DHD_INFO(("%s: download embedded firmware...\n", __FUNCTION__));
+
+	/* Download image */
+	while ((offset + MEMBLOCK) < sizeof(dlarray)) {
+		bcmerror = dhdsdio_membytes(bus, TRUE, offset,
+			(uint8 *) (dlarray + offset), MEMBLOCK);
+		if (bcmerror) {
+			DHD_ERROR(("%s: error %d on writing %d membytes at 0x%08x\n",
+			        __FUNCTION__, bcmerror, MEMBLOCK, offset));
+			goto err;
+		}
+
+		offset += MEMBLOCK;
+	}
+
+	if (offset < sizeof(dlarray)) {
+		bcmerror = dhdsdio_membytes(bus, TRUE, offset,
+			(uint8 *) (dlarray + offset), sizeof(dlarray) - offset);
+		if (bcmerror) {
+			DHD_ERROR(("%s: error %d on writing %d membytes at 0x%08x\n",
+			        __FUNCTION__, bcmerror, sizeof(dlarray) - offset, offset));
+			goto err;
+		}
+	}
+
+#ifdef DHD_DEBUG
+	/* Upload and compare the downloaded code */
+	{
+		ularray = MALLOC(bus->dhd->osh, bus->ramsize);
+		/* Upload image to verify downloaded contents. */
+		offset = 0;
+		memset(ularray, 0xaa, bus->ramsize);
+		while ((offset + MEMBLOCK) < sizeof(dlarray)) {
+			bcmerror = dhdsdio_membytes(bus, FALSE, offset, ularray + offset, MEMBLOCK);
+			if (bcmerror) {
+				DHD_ERROR(("%s: error %d on reading %d membytes at 0x%08x\n",
+					__FUNCTION__, bcmerror, MEMBLOCK, offset));
+				goto err;
+			}
+
+			offset += MEMBLOCK;
+		}
+
+		if (offset < sizeof(dlarray)) {
+			bcmerror = dhdsdio_membytes(bus, FALSE, offset,
+				ularray + offset, sizeof(dlarray) - offset);
+			if (bcmerror) {
+				DHD_ERROR(("%s: error %d on reading %d membytes at 0x%08x\n",
+					__FUNCTION__, bcmerror, sizeof(dlarray) - offset, offset));
+				goto err;
+			}
+		}
+
+		if (memcmp(dlarray, ularray, sizeof(dlarray))) {
+			DHD_ERROR(("%s: Downloaded image is corrupted (%s, %s, %s).\n",
+			           __FUNCTION__, dlimagename, dlimagever, dlimagedate));
+			goto err;
+		} else
+			DHD_ERROR(("%s: Download, Upload and compare succeeded (%s, %s, %s).\n",
+			           __FUNCTION__, dlimagename, dlimagever, dlimagedate));
+
+	}
+#endif /* DHD_DEBUG */
+
+err:
+	if (ularray)
+		MFREE(bus->dhd->osh, ularray, bus->ramsize);
+	return bcmerror;
+}
+#endif /* BCMEMBEDIMAGE */
+
+static int
+dhdsdio_download_code_file(struct dhd_bus *bus, char *pfw_path)
+{
+	int bcmerror = -1;
+	int offset = 0;
+	uint len;
+	void *image = NULL;
+	uint8 *memblock = NULL, *memptr;
+
+	DHD_INFO(("%s: download firmware %s\n", __FUNCTION__, pfw_path));
+
+	image = dhd_os_open_image(pfw_path);
+	if (image == NULL)
+		goto err;
+
+	memptr = memblock = MALLOC(bus->dhd->osh, MEMBLOCK + DHD_SDALIGN);
+	if (memblock == NULL) {
+		DHD_ERROR(("%s: Failed to allocate memory %d bytes\n", __FUNCTION__, MEMBLOCK));
+		goto err;
+	}
+	if ((uint32)(uintptr)memblock % DHD_SDALIGN)
+		memptr += (DHD_SDALIGN - ((uint32)(uintptr)memblock % DHD_SDALIGN));
+
+	/* Download image */
+	while ((len = dhd_os_get_image_block((char*)memptr, MEMBLOCK, image))) {
+		bcmerror = dhdsdio_membytes(bus, TRUE, offset, memptr, len);
+		if (bcmerror) {
+			DHD_ERROR(("%s: error %d on writing %d membytes at 0x%08x\n",
+			        __FUNCTION__, bcmerror, MEMBLOCK, offset));
+			goto err;
+		}
+
+		offset += MEMBLOCK;
+	}
+
+err:
+	if (memblock)
+		MFREE(bus->dhd->osh, memblock, MEMBLOCK + DHD_SDALIGN);
+
+	if (image)
+		dhd_os_close_image(image);
+
+	return bcmerror;
+}
+
+/*
+	EXAMPLE: nvram_array
+	nvram_arry format:
+	name=value
+	Use carriage return at the end of each assignment, and an empty string with
+	carriage return at the end of array.
+
+	For example:
+	unsigned char  nvram_array[] = {"name1=value1\n", "name2=value2\n", "\n"};
+	Hex values start with 0x, and mac addr format: xx:xx:xx:xx:xx:xx.
+
+	Search "EXAMPLE: nvram_array" to see how the array is activated.
+*/
+
+void
+dhd_bus_set_nvram_params(struct dhd_bus * bus, const char *nvram_params)
+{
+	bus->nvram_params = nvram_params;
+}
+
+static int
+dhdsdio_download_nvram(struct dhd_bus *bus)
+{
+	int bcmerror = -1;
+	uint len;
+	void * image = NULL;
+	char * memblock = NULL;
+	char *bufp;
+	char *pnv_path;
+	bool nvram_file_exists;
+
+	pnv_path = bus->nv_path;
+
+	nvram_file_exists = ((pnv_path != NULL) && (pnv_path[0] != '\0'));
+	if (!nvram_file_exists && (bus->nvram_params == NULL))
+		return (0);
+
+	if (nvram_file_exists) {
+		image = dhd_os_open_image(pnv_path);
+		if (image == NULL)
+			goto err;
+	}
+
+	memblock = MALLOC(bus->dhd->osh, MAX_NVRAMBUF_SIZE);
+	if (memblock == NULL) {
+		DHD_ERROR(("%s: Failed to allocate memory %d bytes\n",
+		           __FUNCTION__, MAX_NVRAMBUF_SIZE));
+		goto err;
+	}
+
+	/* Download variables */
+	if (nvram_file_exists) {
+		len = dhd_os_get_image_block(memblock, MAX_NVRAMBUF_SIZE, image);
+	}
+	else {
+		len = strlen(bus->nvram_params);
+		ASSERT(len <= MAX_NVRAMBUF_SIZE);
+		memcpy(memblock, bus->nvram_params, len);
+	}
+	if (len > 0 && len < MAX_NVRAMBUF_SIZE) {
+		bufp = (char *)memblock;
+		bufp[len] = 0;
+		len = process_nvram_vars(bufp, len);
+		if (len % 4) {
+			len += 4 - (len % 4);
+		}
+		bufp += len;
+		*bufp++ = 0;
+		if (len)
+			bcmerror = dhdsdio_downloadvars(bus, memblock, len + 1);
+		if (bcmerror) {
+			DHD_ERROR(("%s: error downloading vars: %d\n",
+			           __FUNCTION__, bcmerror));
+		}
+	}
+	else {
+		DHD_ERROR(("%s: error reading nvram file: %d\n",
+		           __FUNCTION__, len));
+		bcmerror = BCME_SDIO_ERROR;
+	}
+
+err:
+	if (memblock)
+		MFREE(bus->dhd->osh, memblock, MAX_NVRAMBUF_SIZE);
+
+	if (image)
+		dhd_os_close_image(image);
+
+	return bcmerror;
+}
+
+static int
+_dhdsdio_download_firmware(struct dhd_bus *bus)
+{
+	int bcmerror = -1;
+	char *p;
+
+	bool embed = FALSE;	/* download embedded firmware */
+	bool dlok = FALSE;	/* download firmware succeeded */
+
+	/* Out immediately if no image to download */
+	if ((bus->fw_path == NULL) || (bus->fw_path[0] == '\0')) {
+#ifdef BCMEMBEDIMAGE
+		embed = TRUE;
+#else
+		return 0;
+#endif
+	}
+
+	/* Keep arm in reset */
+	if (dhdsdio_download_state(bus, TRUE)) {
+		DHD_ERROR(("%s: error placing ARM core in reset\n", __FUNCTION__));
+		goto err;
+	}
+
+	/* External image takes precedence if specified */
+	if ((bus->fw_path != NULL) && (bus->fw_path[0] != '\0')) {
+
+		/* replace bcm43xx with bcm4330 or bcm4329 */
+		if ((p = strstr(bus->fw_path, "bcm43xx"))) {
+			if (bus->cl_devid == 0x4329) {
+				*(p + 5)='2';
+				*(p + 6)='9';
+			}
+			if (bus->cl_devid == 0x4330) {
+				*(p + 5)='3';
+				*(p + 6)='0';
+			}
+		}
+
+		if (dhdsdio_download_code_file(bus, bus->fw_path)) {
+			DHD_ERROR(("%s: dongle image file download failed\n", __FUNCTION__));
+#ifdef BCMEMBEDIMAGE
+			embed = TRUE;
+#else
+			goto err;
+#endif
+		}
+		else {
+			embed = FALSE;
+			dlok = TRUE;
+		}
+	}
+#ifdef BCMEMBEDIMAGE
+	if (embed) {
+		if (dhdsdio_download_code_array(bus)) {
+			DHD_ERROR(("%s: dongle image array download failed\n", __FUNCTION__));
+			goto err;
+		}
+		else {
+			dlok = TRUE;
+		}
+	}
+#endif
+	if (!dlok) {
+		DHD_ERROR(("%s: dongle image download failed\n", __FUNCTION__));
+		goto err;
+	}
+
+	/* EXAMPLE: nvram_array */
+	/* If a valid nvram_arry is specified as above, it can be passed down to dongle */
+	/* dhd_bus_set_nvram_params(bus, (char *)&nvram_array); */
+
+	/* External nvram takes precedence if specified */
+	if (dhdsdio_download_nvram(bus)) {
+		DHD_ERROR(("%s: dongle nvram file download failed\n", __FUNCTION__));
+		goto err;
+	}
+
+	/* Take arm out of reset */
+	if (dhdsdio_download_state(bus, FALSE)) {
+		DHD_ERROR(("%s: error getting out of ARM core reset\n", __FUNCTION__));
+		goto err;
+	}
+
+	bcmerror = 0;
+
+err:
+	return bcmerror;
+}
+
+static int
+dhd_bcmsdh_recv_buf(dhd_bus_t *bus, uint32 addr, uint fn, uint flags, uint8 *buf, uint nbytes,
+	void *pkt, bcmsdh_cmplt_fn_t complete, void *handle)
+{
+	int status;
+
+	status = bcmsdh_recv_buf(bus->sdh, addr, fn, flags, buf, nbytes, pkt, complete, handle);
+
+	return status;
+}
+
+static int
+dhd_bcmsdh_send_buf(dhd_bus_t *bus, uint32 addr, uint fn, uint flags, uint8 *buf, uint nbytes,
+	void *pkt, bcmsdh_cmplt_fn_t complete, void *handle)
+{
+	return (bcmsdh_send_buf(bus->sdh, addr, fn, flags, buf, nbytes, pkt, complete, handle));
+}
+
+uint
+dhd_bus_chip(struct dhd_bus *bus)
+{
+	ASSERT(bus);
+	ASSERT(bus->sih != NULL);
+	return bus->sih->chip;
+}
+
+void *
+dhd_bus_pub(struct dhd_bus *bus)
+{
+	ASSERT(bus);
+	return bus->dhd;
+}
+
+void *
+dhd_bus_txq(struct dhd_bus *bus)
+{
+	return &bus->txq;
+}
+
+uint
+dhd_bus_hdrlen(struct dhd_bus *bus)
+{
+	return SDPCM_HDRLEN;
+}
+
+int
+dhd_bus_devreset(dhd_pub_t *dhdp, uint8 flag)
+{
+	int bcmerror = 0;
+	dhd_bus_t *bus;
+
+	bus = dhdp->bus;
+
+	if (flag == TRUE) {
+		if (!bus->dhd->dongle_reset) {
+			dhd_os_sdlock(dhdp);
+			dhd_os_wd_timer(dhdp, 0);
+#if !defined(IGNORE_ETH0_DOWN)
+			/* Force flow control as protection when stop come before ifconfig_down */
+			dhd_txflowcontrol(bus->dhd, ALL_INTERFACES, ON);
+#endif /* !defined(IGNORE_ETH0_DOWN) */
+			/* Expect app to have torn down any connection before calling */
+			/* Stop the bus, disable F2 */
+			dhd_bus_stop(bus, FALSE);
+
+#if defined(OOB_INTR_ONLY)
+			/* Clean up any pending IRQ */
+			bcmsdh_set_irq(FALSE);
+#endif /* defined(OOB_INTR_ONLY) */
+
+			/* Clean tx/rx buffer pointers, detach from the dongle */
+			dhdsdio_release_dongle(bus, bus->dhd->osh, TRUE, TRUE);
+
+			bus->dhd->dongle_reset = TRUE;
+			bus->dhd->up = FALSE;
+			dhd_os_sdunlock(dhdp);
+			DHD_TRACE(("%s:  WLAN OFF DONE\n", __FUNCTION__));
+			/* App can now remove power from device */
+		} else
+			bcmerror = BCME_SDIO_ERROR;
+	} else {
+		/* App must have restored power to device before calling */
+
+		DHD_TRACE(("\n\n%s: == WLAN ON ==\n", __FUNCTION__));
+
+		if (bus->dhd->dongle_reset) {
+			/* Turn on WLAN */
+#ifdef DHDTHREAD
+			dhd_os_sdlock(dhdp);
+#endif /* DHDTHREAD */
+			/* Reset SD client */
+			bcmsdh_reset(bus->sdh);
+
+			/* Attempt to re-attach & download */
+			if (dhdsdio_probe_attach(bus, bus->dhd->osh, bus->sdh,
+			                        (uint32 *)SI_ENUM_BASE,
+			                        bus->cl_devid)) {
+				/* Attempt to download binary to the dongle */
+				if (dhdsdio_probe_init(bus, bus->dhd->osh, bus->sdh) &&
+					dhdsdio_download_firmware(bus, bus->dhd->osh, bus->sdh)) {
+
+					/* Re-init bus, enable F2 transfer */
+					bcmerror = dhd_bus_init((dhd_pub_t *) bus->dhd, FALSE);
+					if (bcmerror == BCME_OK) {
+#if defined(OOB_INTR_ONLY)
+						bcmsdh_set_irq(TRUE);
+						dhd_enable_oob_intr(bus, TRUE);
+#endif /* defined(OOB_INTR_ONLY) */
+
+						bus->dhd->dongle_reset = FALSE;
+						bus->dhd->up = TRUE;
+
+#if !defined(IGNORE_ETH0_DOWN)
+						/* Restore flow control  */
+						dhd_txflowcontrol(bus->dhd, ALL_INTERFACES, OFF);
+#endif 
+						dhd_os_wd_timer(dhdp, dhd_watchdog_ms);
+
+						DHD_TRACE(("%s: WLAN ON DONE\n", __FUNCTION__));
+					} else {
+						dhd_bus_stop(bus, FALSE);
+						dhdsdio_release_dongle(bus, bus->dhd->osh,
+							TRUE, FALSE);
+					}
+				} else
+					bcmerror = BCME_SDIO_ERROR;
+			} else
+				bcmerror = BCME_SDIO_ERROR;
+
+#ifdef DHDTHREAD
+			dhd_os_sdunlock(dhdp);
+#endif /* DHDTHREAD */
+		} else {
+			bcmerror = BCME_SDIO_ERROR;
+			DHD_INFO(("%s called when dongle is not in reset\n",
+				__FUNCTION__));
+			DHD_INFO(("Will call dhd_bus_start instead\n"));
+			sdioh_start(NULL, 1);
+			if ((bcmerror = dhd_bus_start(dhdp)) != 0)
+				DHD_ERROR(("%s: dhd_bus_start fail with %d\n",
+					__FUNCTION__, bcmerror));
+		}
+	}
+	return bcmerror;
+}
+
+/* Get Chip ID version */
+uint dhd_bus_chip_id(dhd_pub_t *dhdp)
+{
+	dhd_bus_t *bus = dhdp->bus;
+
+	return  bus->sih->chip;
+}
+
+int
+dhd_bus_membytes(dhd_pub_t *dhdp, bool set, uint32 address, uint8 *data, uint size)
+{
+	dhd_bus_t *bus;
+
+	bus = dhdp->bus;
+	return dhdsdio_membytes(bus, set, address, data, size);
+}
diff --git a/drivers/net/wireless/bcmdhd/dhd_wlfc.h b/drivers/net/wireless/bcmdhd/dhd_wlfc.h
new file mode 100644
index 000000000000..c4d251806d78
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dhd_wlfc.h
@@ -0,0 +1,276 @@
+/*
+* Copyright (C) 1999-2011, Broadcom Corporation
+* 
+*         Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2 (the "GPL"),
+* available at http://www.broadcom.com/licenses/GPLv2.php, with the
+* following added to such license:
+* 
+*      As a special exception, the copyright holders of this software give you
+* permission to link this software with independent modules, and to copy and
+* distribute the resulting executable under terms of your choice, provided that
+* you also meet, for each linked independent module, the terms and conditions of
+* the license of that module.  An independent module is a module which is not
+* derived from this software.  The special exception does not apply to any
+* modifications of the software.
+* 
+*      Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a license
+* other than the GPL, without Broadcom's express prior written consent.
+* $Id: dhd_wlfc.h 286994 2011-09-29 21:27:44Z $
+*
+*/
+#ifndef __wlfc_host_driver_definitions_h__
+#define __wlfc_host_driver_definitions_h__
+
+/* 16 bits will provide an absolute max of 65536 slots */
+#define WLFC_HANGER_MAXITEMS 1024
+
+#define WLFC_HANGER_ITEM_STATE_FREE		1
+#define WLFC_HANGER_ITEM_STATE_INUSE	2
+
+#define WLFC_PKTID_HSLOT_MASK			0xffff /* allow 16 bits only */
+#define WLFC_PKTID_HSLOT_SHIFT			8
+
+/* x -> TXSTATUS TAG to/from firmware */
+#define WLFC_PKTID_HSLOT_GET(x)			\
+	(((x) >> WLFC_PKTID_HSLOT_SHIFT) & WLFC_PKTID_HSLOT_MASK)
+#define WLFC_PKTID_HSLOT_SET(var, slot)	\
+	((var) = ((var) & ~(WLFC_PKTID_HSLOT_MASK << WLFC_PKTID_HSLOT_SHIFT)) | \
+	(((slot) & WLFC_PKTID_HSLOT_MASK) << WLFC_PKTID_HSLOT_SHIFT))
+
+#define WLFC_PKTID_FREERUNCTR_MASK	0xff
+
+#define WLFC_PKTID_FREERUNCTR_GET(x)	((x) & WLFC_PKTID_FREERUNCTR_MASK)
+#define WLFC_PKTID_FREERUNCTR_SET(var, ctr)	\
+	((var) = (((var) & ~WLFC_PKTID_FREERUNCTR_MASK) | \
+	(((ctr) & WLFC_PKTID_FREERUNCTR_MASK))))
+
+#define WLFC_PKTQ_PENQ(pq, prec, p) ((pktq_full((pq)) || pktq_pfull((pq), (prec)))? \
+	NULL : pktq_penq((pq), (prec), (p)))
+#define WLFC_PKTQ_PENQ_HEAD(pq, prec, p) ((pktq_full((pq)) || pktq_pfull((pq), (prec))) ? \
+	NULL : pktq_penq_head((pq), (prec), (p)))
+
+typedef enum ewlfc_packet_state {
+	eWLFC_PKTTYPE_NEW,
+	eWLFC_PKTTYPE_DELAYED,
+	eWLFC_PKTTYPE_SUPPRESSED,
+	eWLFC_PKTTYPE_MAX
+} ewlfc_packet_state_t;
+
+typedef enum ewlfc_mac_entry_action {
+	eWLFC_MAC_ENTRY_ACTION_ADD,
+	eWLFC_MAC_ENTRY_ACTION_DEL,
+	eWLFC_MAC_ENTRY_ACTION_MAX
+} ewlfc_mac_entry_action_t;
+
+typedef struct wlfc_hanger_item {
+	uint8	state;
+	uint8	pad[3];
+	uint32	identifier;
+	void*	pkt;
+#ifdef PROP_TXSTATUS_DEBUG
+	uint32	push_time;
+#endif
+} wlfc_hanger_item_t;
+
+typedef struct wlfc_hanger {
+	int max_items;
+	uint32 pushed;
+	uint32 popped;
+	uint32 failed_to_push;
+	uint32 failed_to_pop;
+	uint32 failed_slotfind;
+	wlfc_hanger_item_t items[1];
+} wlfc_hanger_t;
+
+#define WLFC_HANGER_SIZE(n)	((sizeof(wlfc_hanger_t) - \
+	sizeof(wlfc_hanger_item_t)) + ((n)*sizeof(wlfc_hanger_item_t)))
+
+#define WLFC_STATE_OPEN		1
+#define WLFC_STATE_CLOSE	2
+
+#define WLFC_PSQ_PREC_COUNT		((AC_COUNT + 1) * 2) /* 2 for each AC traffic and bc/mc */
+#define WLFC_PSQ_LEN			64
+#define WLFC_SENDQ_LEN			256
+
+#define WLFC_FLOWCONTROL_DELTA		8
+#define WLFC_FLOWCONTROL_HIWATER	(WLFC_PSQ_LEN - WLFC_FLOWCONTROL_DELTA)
+#define WLFC_FLOWCONTROL_LOWATER	(WLFC_FLOWCONTROL_HIWATER - WLFC_FLOWCONTROL_DELTA)
+
+typedef struct wlfc_mac_descriptor {
+	uint8 occupied;
+	uint8 interface_id;
+	uint8 iftype;
+	uint8 state;
+	uint8 ac_bitmap; /* for APSD */
+	uint8 requested_credit;
+	uint8 requested_packet;
+	uint8 ea[ETHER_ADDR_LEN];
+	/*
+	maintain (MAC,AC) based seq count for
+	packets going to the device. As well as bc/mc.
+	*/
+	uint8 seq[AC_COUNT + 1];
+	uint8 generation;
+	struct pktq	psq;
+	/* The AC pending bitmap that was reported to the fw at last change */
+	uint8 traffic_lastreported_bmp;
+	/* The new AC pending bitmap */
+	uint8 traffic_pending_bmp;
+	/* 1= send on next opportunity */
+	uint8 send_tim_signal;
+	uint8 mac_handle;
+#ifdef PROP_TXSTATUS_DEBUG
+	uint32 dstncredit_sent_packets;
+	uint32 dstncredit_acks;
+	uint32 opened_ct;
+	uint32 closed_ct;
+#endif
+} wlfc_mac_descriptor_t;
+
+#define WLFC_DECR_SEQCOUNT(entry, prec) do { if (entry->seq[(prec)] == 0) {\
+	entry->seq[prec] = 0xff; } else entry->seq[prec]--;} while (0)
+
+#define WLFC_INCR_SEQCOUNT(entry, prec) entry->seq[(prec)]++
+#define WLFC_SEQCOUNT(entry, prec) entry->seq[(prec)]
+
+typedef struct athost_wl_stat_counters {
+	uint32	pktin;
+	uint32	pkt2bus;
+	uint32	pktdropped;
+	uint32	tlv_parse_failed;
+	uint32	rollback;
+	uint32	rollback_failed;
+	uint32	sendq_full_error;
+	uint32	delayq_full_error;
+	uint32	credit_request_failed;
+	uint32	packet_request_failed;
+	uint32	mac_update_failed;
+	uint32	psmode_update_failed;
+	uint32	interface_update_failed;
+	uint32	wlfc_header_only_pkt;
+	uint32	txstatus_in;
+	uint32	d11_suppress;
+	uint32	wl_suppress;
+	uint32	bad_suppress;
+	uint32	pkt_freed;
+	uint32	pkt_free_err;
+	uint32	psq_wlsup_retx;
+	uint32	psq_wlsup_enq;
+	uint32	psq_d11sup_retx;
+	uint32	psq_d11sup_enq;
+	uint32	psq_hostq_retx;
+	uint32	psq_hostq_enq;
+	uint32	mac_handle_notfound;
+	uint32	wlc_tossed_pkts;
+	uint32	dhd_hdrpulls;
+	uint32	generic_error;
+	/* an extra one for bc/mc traffic */
+	uint32	sendq_pkts[AC_COUNT + 1];
+#ifdef PROP_TXSTATUS_DEBUG
+	/* all pkt2bus -> txstatus latency accumulated */
+	uint32	latency_sample_count;
+	uint32	total_status_latency;
+	uint32	latency_most_recent;
+	int		idx_delta;
+	uint32	deltas[10];
+	uint32	fifo_credits_sent[6];
+	uint32	fifo_credits_back[6];
+	uint32	dropped_qfull[6];
+	uint32	signal_only_pkts_sent;
+	uint32	signal_only_pkts_freed;
+#endif
+} athost_wl_stat_counters_t;
+
+#ifdef PROP_TXSTATUS_DEBUG
+#define WLFC_HOST_FIFO_CREDIT_INC_SENTCTRS(ctx, ac) do { \
+	(ctx)->stats.fifo_credits_sent[(ac)]++;} while (0)
+#define WLFC_HOST_FIFO_CREDIT_INC_BACKCTRS(ctx, ac) do { \
+	(ctx)->stats.fifo_credits_back[(ac)]++;} while (0)
+#define WLFC_HOST_FIFO_DROPPEDCTR_INC(ctx, ac) do { \
+	(ctx)->stats.dropped_qfull[(ac)]++;} while (0)
+#else
+#define WLFC_HOST_FIFO_CREDIT_INC_SENTCTRS(ctx, ac) do {} while (0)
+#define WLFC_HOST_FIFO_CREDIT_INC_BACKCTRS(ctx, ac) do {} while (0)
+#define WLFC_HOST_FIFO_DROPPEDCTR_INC(ctx, ac) do {} while (0)
+#endif
+
+#define WLFC_FCMODE_NONE				0
+#define WLFC_FCMODE_IMPLIED_CREDIT		1
+#define WLFC_FCMODE_EXPLICIT_CREDIT		2
+
+/* How long to defer borrowing in milliseconds */
+#define WLFC_BORROW_DEFER_PERIOD_MS 100
+
+/* Mask to represent available ACs (note: BC/MC is ignored */
+#define WLFC_AC_MASK 0xF
+
+/* Mask to check for only on-going AC_BE traffic */
+#define WLFC_AC_BE_TRAFFIC_ONLY 0xD
+
+typedef struct athost_wl_status_info {
+	uint8	last_seqid_to_wlc;
+
+	/* OSL handle */
+	osl_t*	osh;
+	/* dhd pub */
+	void*	dhdp;
+
+	/* stats */
+	athost_wl_stat_counters_t stats;
+
+	/* the additional ones are for bc/mc and ATIM FIFO */
+	int		FIFO_credit[AC_COUNT + 2];
+
+	/* Credit borrow counts for each FIFO from each of the other FIFOs */
+	int		credits_borrowed[AC_COUNT + 2][AC_COUNT + 2];
+
+	struct  pktq SENDQ;
+
+	/* packet hanger and MAC->handle lookup table */
+	void*	hanger;
+	struct {
+		/* table for individual nodes */
+		wlfc_mac_descriptor_t	nodes[WLFC_MAC_DESC_TABLE_SIZE];
+		/* table for interfaces */
+		wlfc_mac_descriptor_t	interfaces[WLFC_MAX_IFNUM];
+		/* OS may send packets to unknown (unassociated) destinations */
+		/* A place holder for bc/mc and packets to unknown destinations */
+		wlfc_mac_descriptor_t	other;
+	} destination_entries;
+	/* token position for different priority packets */
+	uint8   token_pos[AC_COUNT+1];
+	/* ON/OFF state for flow control to the host network interface */
+	uint8	hostif_flow_state[WLFC_MAX_IFNUM];
+	uint8	host_ifidx;
+	/* to flow control an OS interface */
+	uint8	toggle_host_if;
+
+	/*
+	Mode in which the dhd flow control shall operate. Must be set before
+	traffic starts to the device.
+	0 - Do not do any proptxtstatus flow control
+	1 - Use implied credit from a packet status
+	2 - Use explicit credit
+	*/
+	uint8	proptxstatus_mode;
+
+	/* To borrow credits */
+	uint8   allow_credit_borrow;
+
+	/* Timestamp to compute how long to defer borrowing for */
+	uint32  borrow_defer_timestamp;
+
+} athost_wl_status_info_t;
+
+int dhd_wlfc_enable(dhd_pub_t *dhd);
+int dhd_wlfc_interface_event(struct dhd_info *,
+	ewlfc_mac_entry_action_t action, uint8 ifid, uint8 iftype, uint8* ea);
+int dhd_wlfc_FIFOcreditmap_event(struct dhd_info *dhd, uint8* event_data);
+int dhd_wlfc_event(struct dhd_info *dhd);
+int dhd_os_wlfc_block(dhd_pub_t *pub);
+int dhd_os_wlfc_unblock(dhd_pub_t *pub);
+
+#endif /* __wlfc_host_driver_definitions_h__ */
diff --git a/drivers/net/wireless/bcmdhd/dngl_stats.h b/drivers/net/wireless/bcmdhd/dngl_stats.h
new file mode 100644
index 000000000000..9cdf718b3990
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dngl_stats.h
@@ -0,0 +1,43 @@
+/*
+ * Common stats definitions for clients of dongle
+ * ports
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dngl_stats.h,v 1.5 2008-06-02 16:56:20 Exp $
+ */
+
+#ifndef _dngl_stats_h_
+#define _dngl_stats_h_
+
+typedef struct {
+	unsigned long	rx_packets;		/* total packets received */
+	unsigned long	tx_packets;		/* total packets transmitted */
+	unsigned long	rx_bytes;		/* total bytes received */
+	unsigned long	tx_bytes;		/* total bytes transmitted */
+	unsigned long	rx_errors;		/* bad packets received */
+	unsigned long	tx_errors;		/* packet transmit problems */
+	unsigned long	rx_dropped;		/* packets dropped by dongle */
+	unsigned long	tx_dropped;		/* packets dropped by dongle */
+	unsigned long   multicast;      /* multicast packets received */
+} dngl_stats_t;
+
+#endif /* _dngl_stats_h_ */
diff --git a/drivers/net/wireless/bcmdhd/dngl_wlhdr.h b/drivers/net/wireless/bcmdhd/dngl_wlhdr.h
new file mode 100644
index 000000000000..8b39b9ecb584
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/dngl_wlhdr.h
@@ -0,0 +1,40 @@
+/*
+ * Dongle WL Header definitions
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dngl_wlhdr.h,v 1.1 2009-01-08 01:21:12 Exp $
+ */
+
+#ifndef _dngl_wlhdr_h_
+#define _dngl_wlhdr_h_
+
+typedef struct wl_header {
+    uint8   type;           /* Header type */
+    uint8   version;        /* Header version */
+	int8	rssi;			/* RSSI */
+	uint8	pad;			/* Unused */
+} wl_header_t;
+
+#define WL_HEADER_LEN   sizeof(wl_header_t)
+#define WL_HEADER_TYPE  0
+#define WL_HEADER_VER   1
+#endif /* _dngl_wlhdr_h_ */
diff --git a/drivers/net/wireless/bcmdhd/hndpmu.c b/drivers/net/wireless/bcmdhd/hndpmu.c
new file mode 100644
index 000000000000..0e493343c806
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/hndpmu.c
@@ -0,0 +1,196 @@
+/*
+ * Misc utility routines for accessing PMU corerev specific features
+ * of the SiliconBackplane-based Broadcom chips.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: hndpmu.c,v 1.228.2.56 2011-02-11 22:49:07 $
+ */
+
+#include <typedefs.h>
+#include <bcmdefs.h>
+#include <osl.h>
+#include <bcmutils.h>
+#include <siutils.h>
+#include <bcmdevs.h>
+#include <hndsoc.h>
+#include <sbchipc.h>
+#include <hndpmu.h>
+
+#define	PMU_ERROR(args)
+
+#define	PMU_MSG(args)
+
+/* To check in verbose debugging messages not intended
+ * to be on except on private builds.
+ */
+#define	PMU_NONE(args)
+
+
+/* SDIO Pad drive strength to select value mappings.
+ * The last strength value in each table must be 0 (the tri-state value).
+ */
+typedef struct {
+	uint8 strength;			/* Pad Drive Strength in mA */
+	uint8 sel;			/* Chip-specific select value */
+} sdiod_drive_str_t;
+
+/* SDIO Drive Strength to sel value table for PMU Rev 1 */
+static const sdiod_drive_str_t sdiod_drive_strength_tab1[] = {
+	{4, 0x2},
+	{2, 0x3},
+	{1, 0x0},
+	{0, 0x0} };
+
+/* SDIO Drive Strength to sel value table for PMU Rev 2, 3 */
+static const sdiod_drive_str_t sdiod_drive_strength_tab2[] = {
+	{12, 0x7},
+	{10, 0x6},
+	{8, 0x5},
+	{6, 0x4},
+	{4, 0x2},
+	{2, 0x1},
+	{0, 0x0} };
+
+/* SDIO Drive Strength to sel value table for PMU Rev 8 (1.8V) */
+static const sdiod_drive_str_t sdiod_drive_strength_tab3[] = {
+	{32, 0x7},
+	{26, 0x6},
+	{22, 0x5},
+	{16, 0x4},
+	{12, 0x3},
+	{8, 0x2},
+	{4, 0x1},
+	{0, 0x0} };
+
+/* SDIO Drive Strength to sel value table for PMU Rev 11 (1.8v) */
+static const sdiod_drive_str_t sdiod_drive_strength_tab4_1v8[] = {
+	{32, 0x6},
+	{26, 0x7},
+	{22, 0x4},
+	{16, 0x5},
+	{12, 0x2},
+	{8, 0x3},
+	{4, 0x0},
+	{0, 0x1} };
+
+/* SDIO Drive Strength to sel value table for PMU Rev 11 (1.2v) */
+
+/* SDIO Drive Strength to sel value table for PMU Rev 11 (2.5v) */
+
+/* SDIO Drive Strength to sel value table for PMU Rev 13 (1.8v) */
+static const sdiod_drive_str_t sdiod_drive_strength_tab5_1v8[] = {
+	{6, 0x7},
+	{5, 0x6},
+	{4, 0x5},
+	{3, 0x4},
+	{2, 0x2},
+	{1, 0x1},
+	{0, 0x0} };
+
+/* SDIO Drive Strength to sel value table for PMU Rev 13 (3.3v) */
+
+
+#define SDIOD_DRVSTR_KEY(chip, pmu)	(((chip) << 16) | (pmu))
+
+void
+si_sdiod_drive_strength_init(si_t *sih, osl_t *osh, uint32 drivestrength)
+{
+	chipcregs_t *cc;
+	uint origidx, intr_val = 0;
+	sdiod_drive_str_t *str_tab = NULL;
+	uint32 str_mask = 0;
+	uint32 str_shift = 0;
+
+	if (!(sih->cccaps & CC_CAP_PMU)) {
+		return;
+	}
+
+	/* Remember original core before switch to chipc */
+	cc = (chipcregs_t *) si_switch_core(sih, CC_CORE_ID, &origidx, &intr_val);
+
+	switch (SDIOD_DRVSTR_KEY(sih->chip, sih->pmurev)) {
+	case SDIOD_DRVSTR_KEY(BCM4325_CHIP_ID, 1):
+		str_tab = (sdiod_drive_str_t *)&sdiod_drive_strength_tab1;
+		str_mask = 0x30000000;
+		str_shift = 28;
+		break;
+	case SDIOD_DRVSTR_KEY(BCM4325_CHIP_ID, 2):
+	case SDIOD_DRVSTR_KEY(BCM4325_CHIP_ID, 3):
+	case SDIOD_DRVSTR_KEY(BCM4315_CHIP_ID, 4):
+		str_tab = (sdiod_drive_str_t *)&sdiod_drive_strength_tab2;
+		str_mask = 0x00003800;
+		str_shift = 11;
+		break;
+	case SDIOD_DRVSTR_KEY(BCM4336_CHIP_ID, 8):
+	case SDIOD_DRVSTR_KEY(BCM4336_CHIP_ID, 11):
+		if (sih->pmurev == 8) {
+			str_tab = (sdiod_drive_str_t *)&sdiod_drive_strength_tab3;
+		}
+		else if (sih->pmurev == 11) {
+			str_tab = (sdiod_drive_str_t *)&sdiod_drive_strength_tab4_1v8;
+		}
+		str_mask = 0x00003800;
+		str_shift = 11;
+		break;
+	case SDIOD_DRVSTR_KEY(BCM4330_CHIP_ID, 12):
+		str_tab = (sdiod_drive_str_t *)&sdiod_drive_strength_tab4_1v8;
+		str_mask = 0x00003800;
+		str_shift = 11;
+		break;
+	case SDIOD_DRVSTR_KEY(BCM43362_CHIP_ID, 13):
+		str_tab = (sdiod_drive_str_t *)&sdiod_drive_strength_tab5_1v8;
+		str_mask = 0x00003800;
+		str_shift = 11;
+		break;
+	default:
+		PMU_MSG(("No SDIO Drive strength init done for chip %s rev %d pmurev %d\n",
+		         bcm_chipname(sih->chip, chn, 8), sih->chiprev, sih->pmurev));
+
+		break;
+	}
+
+	if (str_tab != NULL) {
+		uint32 cc_data_temp;
+		int i;
+
+		/* Pick the lowest available drive strength equal or greater than the
+		 * requested strength.	Drive strength of 0 requests tri-state.
+		 */
+		for (i = 0; drivestrength < str_tab[i].strength; i++)
+			;
+
+		if (i > 0 && drivestrength > str_tab[i].strength)
+			i--;
+
+		W_REG(osh, &cc->chipcontrol_addr, 1);
+		cc_data_temp = R_REG(osh, &cc->chipcontrol_data);
+		cc_data_temp &= ~str_mask;
+		cc_data_temp |= str_tab[i].sel << str_shift;
+		W_REG(osh, &cc->chipcontrol_data, cc_data_temp);
+
+		PMU_MSG(("SDIO: %dmA drive strength requested; set to %dmA\n",
+		         drivestrength, str_tab[i].strength));
+	}
+
+	/* Return to original core */
+	si_restore_core(sih, origidx, intr_val);
+}
diff --git a/drivers/net/wireless/bcmdhd/include/Makefile b/drivers/net/wireless/bcmdhd/include/Makefile
new file mode 100644
index 000000000000..67c4906f5889
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/Makefile
@@ -0,0 +1,53 @@
+#!/bin/bash
+#
+# This script serves following purpose:
+#
+# 1. It generates native version information by querying
+#    automerger maintained database to see where src/include
+#    came from
+# 2. For select components, as listed in compvers.sh
+#    it generates component version files
+#
+# Copyright 2005, Broadcom, Inc.
+#
+# $Id: Makefile 241702 2011-02-19 00:41:03Z $
+#
+
+SRCBASE := ..
+
+TARGETS := epivers.h
+
+ifdef VERBOSE
+export VERBOSE
+endif
+
+all release: epivers compvers
+
+# Generate epivers.h for native branch version
+epivers:
+	bash epivers.sh
+
+# Generate epivers.h for native branch version
+compvers:
+	@if [ -s "compvers.sh" ]; then \
+		echo "Generating component versions, if any"; \
+		bash compvers.sh; \
+	else \
+		echo "Skipping component version generation"; \
+	fi
+
+# Generate epivers.h for native branch version
+clean_compvers:
+	@if [ -s "compvers.sh" ]; then \
+		echo "bash compvers.sh clean"; \
+		bash compvers.sh clean; \
+	else \
+		echo "Skipping component version clean"; \
+	fi
+
+clean:
+	rm -f $(TARGETS) *.prev
+
+clean_all: clean clean_compvers
+
+.PHONY: all release clean epivers compvers clean_compvers
diff --git a/drivers/net/wireless/bcmdhd/include/aidmp.h b/drivers/net/wireless/bcmdhd/include/aidmp.h
new file mode 100644
index 000000000000..b993a033abc2
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/aidmp.h
@@ -0,0 +1,377 @@
+/*
+ * Broadcom AMBA Interconnect definitions.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: aidmp.h 277737 2011-08-16 17:54:59Z $
+ */
+
+
+#ifndef	_AIDMP_H
+#define	_AIDMP_H
+
+
+#define	MFGID_ARM		0x43b
+#define	MFGID_BRCM		0x4bf
+#define	MFGID_MIPS		0x4a7
+
+
+#define	CC_SIM			0
+#define	CC_EROM			1
+#define	CC_CORESIGHT		9
+#define	CC_VERIF		0xb
+#define	CC_OPTIMO		0xd
+#define	CC_GEN			0xe
+#define	CC_PRIMECELL		0xf
+
+
+#define	ER_EROMENTRY		0x000
+#define	ER_REMAPCONTROL		0xe00
+#define	ER_REMAPSELECT		0xe04
+#define	ER_MASTERSELECT		0xe10
+#define	ER_ITCR			0xf00
+#define	ER_ITIP			0xf04
+
+
+#define	ER_TAG			0xe
+#define	ER_TAG1			0x6
+#define	ER_VALID		1
+#define	ER_CI			0
+#define	ER_MP			2
+#define	ER_ADD			4
+#define	ER_END			0xe
+#define	ER_BAD			0xffffffff
+
+
+#define	CIA_MFG_MASK		0xfff00000
+#define	CIA_MFG_SHIFT		20
+#define	CIA_CID_MASK		0x000fff00
+#define	CIA_CID_SHIFT		8
+#define	CIA_CCL_MASK		0x000000f0
+#define	CIA_CCL_SHIFT		4
+
+
+#define	CIB_REV_MASK		0xff000000
+#define	CIB_REV_SHIFT		24
+#define	CIB_NSW_MASK		0x00f80000
+#define	CIB_NSW_SHIFT		19
+#define	CIB_NMW_MASK		0x0007c000
+#define	CIB_NMW_SHIFT		14
+#define	CIB_NSP_MASK		0x00003e00
+#define	CIB_NSP_SHIFT		9
+#define	CIB_NMP_MASK		0x000001f0
+#define	CIB_NMP_SHIFT		4
+
+
+#define	MPD_MUI_MASK		0x0000ff00
+#define	MPD_MUI_SHIFT		8
+#define	MPD_MP_MASK		0x000000f0
+#define	MPD_MP_SHIFT		4
+
+
+#define	AD_ADDR_MASK		0xfffff000
+#define	AD_SP_MASK		0x00000f00
+#define	AD_SP_SHIFT		8
+#define	AD_ST_MASK		0x000000c0
+#define	AD_ST_SHIFT		6
+#define	AD_ST_SLAVE		0x00000000
+#define	AD_ST_BRIDGE		0x00000040
+#define	AD_ST_SWRAP		0x00000080
+#define	AD_ST_MWRAP		0x000000c0
+#define	AD_SZ_MASK		0x00000030
+#define	AD_SZ_SHIFT		4
+#define	AD_SZ_4K		0x00000000
+#define	AD_SZ_8K		0x00000010
+#define	AD_SZ_16K		0x00000020
+#define	AD_SZ_SZD		0x00000030
+#define	AD_AG32			0x00000008
+#define	AD_ADDR_ALIGN		0x00000fff
+#define	AD_SZ_BASE		0x00001000	
+
+
+#define	SD_SZ_MASK		0xfffff000
+#define	SD_SG32			0x00000008
+#define	SD_SZ_ALIGN		0x00000fff
+
+
+#ifndef _LANGUAGE_ASSEMBLY
+
+typedef volatile struct _aidmp {
+	uint32	oobselina30;	
+	uint32	oobselina74;	
+	uint32	PAD[6];
+	uint32	oobselinb30;	
+	uint32	oobselinb74;	
+	uint32	PAD[6];
+	uint32	oobselinc30;	
+	uint32	oobselinc74;	
+	uint32	PAD[6];
+	uint32	oobselind30;	
+	uint32	oobselind74;	
+	uint32	PAD[38];
+	uint32	oobselouta30;	
+	uint32	oobselouta74;	
+	uint32	PAD[6];
+	uint32	oobseloutb30;	
+	uint32	oobseloutb74;	
+	uint32	PAD[6];
+	uint32	oobseloutc30;	
+	uint32	oobseloutc74;	
+	uint32	PAD[6];
+	uint32	oobseloutd30;	
+	uint32	oobseloutd74;	
+	uint32	PAD[38];
+	uint32	oobsynca;	
+	uint32	oobseloutaen;	
+	uint32	PAD[6];
+	uint32	oobsyncb;	
+	uint32	oobseloutben;	
+	uint32	PAD[6];
+	uint32	oobsyncc;	
+	uint32	oobseloutcen;	
+	uint32	PAD[6];
+	uint32	oobsyncd;	
+	uint32	oobseloutden;	
+	uint32	PAD[38];
+	uint32	oobaextwidth;	
+	uint32	oobainwidth;	
+	uint32	oobaoutwidth;	
+	uint32	PAD[5];
+	uint32	oobbextwidth;	
+	uint32	oobbinwidth;	
+	uint32	oobboutwidth;	
+	uint32	PAD[5];
+	uint32	oobcextwidth;	
+	uint32	oobcinwidth;	
+	uint32	oobcoutwidth;	
+	uint32	PAD[5];
+	uint32	oobdextwidth;	
+	uint32	oobdinwidth;	
+	uint32	oobdoutwidth;	
+	uint32	PAD[37];
+	uint32	ioctrlset;	
+	uint32	ioctrlclear;	
+	uint32	ioctrl;		
+	uint32	PAD[61];
+	uint32	iostatus;	
+	uint32	PAD[127];
+	uint32	ioctrlwidth;	
+	uint32	iostatuswidth;	
+	uint32	PAD[62];
+	uint32	resetctrl;	
+	uint32	resetstatus;	
+	uint32	resetreadid;	
+	uint32	resetwriteid;	
+	uint32	PAD[60];
+	uint32	errlogctrl;	
+	uint32	errlogdone;	
+	uint32	errlogstatus;	
+	uint32	errlogaddrlo;	
+	uint32	errlogaddrhi;	
+	uint32	errlogid;	
+	uint32	errloguser;	
+	uint32	errlogflags;	
+	uint32	PAD[56];
+	uint32	intstatus;	
+	uint32	PAD[127];
+	uint32	config;		
+	uint32	PAD[63];
+	uint32	itcr;		
+	uint32	PAD[3];
+	uint32	itipooba;	
+	uint32	itipoobb;	
+	uint32	itipoobc;	
+	uint32	itipoobd;	
+	uint32	PAD[4];
+	uint32	itipoobaout;	
+	uint32	itipoobbout;	
+	uint32	itipoobcout;	
+	uint32	itipoobdout;	
+	uint32	PAD[4];
+	uint32	itopooba;	
+	uint32	itopoobb;	
+	uint32	itopoobc;	
+	uint32	itopoobd;	
+	uint32	PAD[4];
+	uint32	itopoobain;	
+	uint32	itopoobbin;	
+	uint32	itopoobcin;	
+	uint32	itopoobdin;	
+	uint32	PAD[4];
+	uint32	itopreset;	
+	uint32	PAD[15];
+	uint32	peripherialid4;	
+	uint32	peripherialid5;	
+	uint32	peripherialid6;	
+	uint32	peripherialid7;	
+	uint32	peripherialid0;	
+	uint32	peripherialid1;	
+	uint32	peripherialid2;	
+	uint32	peripherialid3;	
+	uint32	componentid0;	
+	uint32	componentid1;	
+	uint32	componentid2;	
+	uint32	componentid3;	
+} aidmp_t;
+
+#endif 
+
+
+#define	OOB_BUSCONFIG		0x020
+#define	OOB_STATUSA		0x100
+#define	OOB_STATUSB		0x104
+#define	OOB_STATUSC		0x108
+#define	OOB_STATUSD		0x10c
+#define	OOB_ENABLEA0		0x200
+#define	OOB_ENABLEA1		0x204
+#define	OOB_ENABLEA2		0x208
+#define	OOB_ENABLEA3		0x20c
+#define	OOB_ENABLEB0		0x280
+#define	OOB_ENABLEB1		0x284
+#define	OOB_ENABLEB2		0x288
+#define	OOB_ENABLEB3		0x28c
+#define	OOB_ENABLEC0		0x300
+#define	OOB_ENABLEC1		0x304
+#define	OOB_ENABLEC2		0x308
+#define	OOB_ENABLEC3		0x30c
+#define	OOB_ENABLED0		0x380
+#define	OOB_ENABLED1		0x384
+#define	OOB_ENABLED2		0x388
+#define	OOB_ENABLED3		0x38c
+#define	OOB_ITCR		0xf00
+#define	OOB_ITIPOOBA		0xf10
+#define	OOB_ITIPOOBB		0xf14
+#define	OOB_ITIPOOBC		0xf18
+#define	OOB_ITIPOOBD		0xf1c
+#define	OOB_ITOPOOBA		0xf30
+#define	OOB_ITOPOOBB		0xf34
+#define	OOB_ITOPOOBC		0xf38
+#define	OOB_ITOPOOBD		0xf3c
+
+
+#define	AI_OOBSELINA30		0x000
+#define	AI_OOBSELINA74		0x004
+#define	AI_OOBSELINB30		0x020
+#define	AI_OOBSELINB74		0x024
+#define	AI_OOBSELINC30		0x040
+#define	AI_OOBSELINC74		0x044
+#define	AI_OOBSELIND30		0x060
+#define	AI_OOBSELIND74		0x064
+#define	AI_OOBSELOUTA30		0x100
+#define	AI_OOBSELOUTA74		0x104
+#define	AI_OOBSELOUTB30		0x120
+#define	AI_OOBSELOUTB74		0x124
+#define	AI_OOBSELOUTC30		0x140
+#define	AI_OOBSELOUTC74		0x144
+#define	AI_OOBSELOUTD30		0x160
+#define	AI_OOBSELOUTD74		0x164
+#define	AI_OOBSYNCA		0x200
+#define	AI_OOBSELOUTAEN		0x204
+#define	AI_OOBSYNCB		0x220
+#define	AI_OOBSELOUTBEN		0x224
+#define	AI_OOBSYNCC		0x240
+#define	AI_OOBSELOUTCEN		0x244
+#define	AI_OOBSYNCD		0x260
+#define	AI_OOBSELOUTDEN		0x264
+#define	AI_OOBAEXTWIDTH		0x300
+#define	AI_OOBAINWIDTH		0x304
+#define	AI_OOBAOUTWIDTH		0x308
+#define	AI_OOBBEXTWIDTH		0x320
+#define	AI_OOBBINWIDTH		0x324
+#define	AI_OOBBOUTWIDTH		0x328
+#define	AI_OOBCEXTWIDTH		0x340
+#define	AI_OOBCINWIDTH		0x344
+#define	AI_OOBCOUTWIDTH		0x348
+#define	AI_OOBDEXTWIDTH		0x360
+#define	AI_OOBDINWIDTH		0x364
+#define	AI_OOBDOUTWIDTH		0x368
+
+
+#define	AI_IOCTRLSET		0x400
+#define	AI_IOCTRLCLEAR		0x404
+#define	AI_IOCTRL		0x408
+#define	AI_IOSTATUS		0x500
+#define	AI_RESETCTRL		0x800
+#define	AI_RESETSTATUS		0x804
+
+
+#define	AI_IOCTRLWIDTH		0x700
+#define	AI_IOSTATUSWIDTH	0x704
+
+#define	AI_RESETREADID		0x808
+#define	AI_RESETWRITEID		0x80c
+#define	AI_ERRLOGCTRL		0xa00
+#define	AI_ERRLOGDONE		0xa04
+#define	AI_ERRLOGSTATUS		0xa08
+#define	AI_ERRLOGADDRLO		0xa0c
+#define	AI_ERRLOGADDRHI		0xa10
+#define	AI_ERRLOGID		0xa14
+#define	AI_ERRLOGUSER		0xa18
+#define	AI_ERRLOGFLAGS		0xa1c
+#define	AI_INTSTATUS		0xa00
+#define	AI_CONFIG		0xe00
+#define	AI_ITCR			0xf00
+#define	AI_ITIPOOBA		0xf10
+#define	AI_ITIPOOBB		0xf14
+#define	AI_ITIPOOBC		0xf18
+#define	AI_ITIPOOBD		0xf1c
+#define	AI_ITIPOOBAOUT		0xf30
+#define	AI_ITIPOOBBOUT		0xf34
+#define	AI_ITIPOOBCOUT		0xf38
+#define	AI_ITIPOOBDOUT		0xf3c
+#define	AI_ITOPOOBA		0xf50
+#define	AI_ITOPOOBB		0xf54
+#define	AI_ITOPOOBC		0xf58
+#define	AI_ITOPOOBD		0xf5c
+#define	AI_ITOPOOBAIN		0xf70
+#define	AI_ITOPOOBBIN		0xf74
+#define	AI_ITOPOOBCIN		0xf78
+#define	AI_ITOPOOBDIN		0xf7c
+#define	AI_ITOPRESET		0xf90
+#define	AI_PERIPHERIALID4	0xfd0
+#define	AI_PERIPHERIALID5	0xfd4
+#define	AI_PERIPHERIALID6	0xfd8
+#define	AI_PERIPHERIALID7	0xfdc
+#define	AI_PERIPHERIALID0	0xfe0
+#define	AI_PERIPHERIALID1	0xfe4
+#define	AI_PERIPHERIALID2	0xfe8
+#define	AI_PERIPHERIALID3	0xfec
+#define	AI_COMPONENTID0		0xff0
+#define	AI_COMPONENTID1		0xff4
+#define	AI_COMPONENTID2		0xff8
+#define	AI_COMPONENTID3		0xffc
+
+
+#define	AIRC_RESET		1
+
+
+#define	AICFG_OOB		0x00000020
+#define	AICFG_IOS		0x00000010
+#define	AICFG_IOC		0x00000008
+#define	AICFG_TO		0x00000004
+#define	AICFG_ERRL		0x00000002
+#define	AICFG_RST		0x00000001
+
+
+#define OOB_SEL_OUTEN_B_5	15
+#define OOB_SEL_OUTEN_B_6	23
+
+#endif	
diff --git a/drivers/net/wireless/bcmdhd/include/bcmcdc.h b/drivers/net/wireless/bcmdhd/include/bcmcdc.h
new file mode 100644
index 000000000000..77a20f87b7ea
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/bcmcdc.h
@@ -0,0 +1,121 @@
+/*
+ * CDC network driver ioctl/indication encoding
+ * Broadcom 802.11abg Networking Device Driver
+ *
+ * Definitions subject to change without notice.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmcdc.h 277737 2011-08-16 17:54:59Z $
+ */
+
+#ifndef _bcmcdc_h_
+#define	_bcmcdc_h_
+#include <proto/ethernet.h>
+
+typedef struct cdc_ioctl {
+	uint32 cmd;      
+	uint32 len;      
+	uint32 flags;    
+	uint32 status;   
+} cdc_ioctl_t;
+
+
+#define CDC_MAX_MSG_SIZE   ETHER_MAX_LEN
+
+
+#define CDCL_IOC_OUTLEN_MASK   0x0000FFFF  
+					   
+#define CDCL_IOC_OUTLEN_SHIFT  0
+#define CDCL_IOC_INLEN_MASK    0xFFFF0000   
+#define CDCL_IOC_INLEN_SHIFT   16
+
+
+#define CDCF_IOC_ERROR		0x01	
+#define CDCF_IOC_SET		0x02	
+#define CDCF_IOC_OVL_IDX_MASK	0x3c	
+#define CDCF_IOC_OVL_RSV	0x40	
+#define CDCF_IOC_OVL		0x80	
+#define CDCF_IOC_ACTION_MASK	0xfe	
+#define CDCF_IOC_ACTION_SHIFT	1	
+#define CDCF_IOC_IF_MASK	0xF000	
+#define CDCF_IOC_IF_SHIFT	12
+#define CDCF_IOC_ID_MASK	0xFFFF0000	
+#define CDCF_IOC_ID_SHIFT	16		
+
+#define CDC_IOC_IF_IDX(flags)	(((flags) & CDCF_IOC_IF_MASK) >> CDCF_IOC_IF_SHIFT)
+#define CDC_IOC_ID(flags)	(((flags) & CDCF_IOC_ID_MASK) >> CDCF_IOC_ID_SHIFT)
+
+#define CDC_GET_IF_IDX(hdr) \
+	((int)((((hdr)->flags) & CDCF_IOC_IF_MASK) >> CDCF_IOC_IF_SHIFT))
+#define CDC_SET_IF_IDX(hdr, idx) \
+	((hdr)->flags = (((hdr)->flags & ~CDCF_IOC_IF_MASK) | ((idx) << CDCF_IOC_IF_SHIFT)))
+
+
+
+#define	BDC_HEADER_LEN		4
+
+#define BDC_PROTO_VER_1		1	
+#define BDC_PROTO_VER		2	
+
+#define BDC_FLAG_VER_MASK	0xf0	
+#define BDC_FLAG_VER_SHIFT	4	
+
+#define BDC_FLAG__UNUSED	0x03	
+#define BDC_FLAG_SUM_GOOD	0x04	
+#define BDC_FLAG_SUM_NEEDED	0x08	
+
+#define BDC_PRIORITY_MASK	0x7
+
+#define BDC_FLAG2_FC_FLAG	0x10	
+									
+#define BDC_PRIORITY_FC_SHIFT	4		
+
+#define BDC_FLAG2_IF_MASK	0x0f	
+#define BDC_FLAG2_IF_SHIFT	0
+#define BDC_FLAG2_PAD_MASK		0xf0
+#define BDC_FLAG_PAD_MASK		0x03
+#define BDC_FLAG2_PAD_SHIFT		2
+#define BDC_FLAG_PAD_SHIFT		0
+#define BDC_FLAG2_PAD_IDX		0x3c
+#define BDC_FLAG_PAD_IDX		0x03
+#define BDC_GET_PAD_LEN(hdr) \
+	((int)(((((hdr)->flags2) & BDC_FLAG2_PAD_MASK) >> BDC_FLAG2_PAD_SHIFT) | \
+	((((hdr)->flags) & BDC_FLAG_PAD_MASK) >> BDC_FLAG_PAD_SHIFT)))
+#define BDC_SET_PAD_LEN(hdr, idx) \
+	((hdr)->flags2 = (((hdr)->flags2 & ~BDC_FLAG2_PAD_MASK) | \
+	(((idx) & BDC_FLAG2_PAD_IDX) << BDC_FLAG2_PAD_SHIFT))); \
+	((hdr)->flags = (((hdr)->flags & ~BDC_FLAG_PAD_MASK) | \
+	(((idx) & BDC_FLAG_PAD_IDX) << BDC_FLAG_PAD_SHIFT)))
+
+#define BDC_GET_IF_IDX(hdr) \
+	((int)((((hdr)->flags2) & BDC_FLAG2_IF_MASK) >> BDC_FLAG2_IF_SHIFT))
+#define BDC_SET_IF_IDX(hdr, idx) \
+	((hdr)->flags2 = (((hdr)->flags2 & ~BDC_FLAG2_IF_MASK) | ((idx) << BDC_FLAG2_IF_SHIFT)))
+
+struct bdc_header {
+	uint8	flags;			
+	uint8	priority;		
+	uint8	flags2;
+	uint8	dataOffset;
+};
+
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/bcmdefs.h b/drivers/net/wireless/bcmdhd/include/bcmdefs.h
new file mode 100644
index 000000000000..17cc0e955f62
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/bcmdefs.h
@@ -0,0 +1,231 @@
+/*
+ * Misc system wide definitions
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmdefs.h 279282 2011-08-23 22:44:02Z $
+ */
+
+
+#ifndef	_bcmdefs_h_
+#define	_bcmdefs_h_
+
+
+
+
+#define BCM_REFERENCE(data)	((void)(data))
+
+
+
+#define bcmreclaimed 		0
+#define _data	_data
+#define _fn	_fn
+#define BCMPREATTACHDATA(_data)	_data
+#define BCMPREATTACHFN(_fn)	_fn
+#define _data	_data
+#define _fn		_fn
+#define _fn	_fn
+#define	BCMNMIATTACHFN(_fn)	_fn
+#define	BCMNMIATTACHDATA(_data)	_data
+#define BCMOVERLAY0DATA(_sym)	_sym
+#define BCMOVERLAY0FN(_fn)	_fn
+#define BCMOVERLAY1DATA(_sym)	_sym
+#define BCMOVERLAY1FN(_fn)	_fn
+#define BCMOVERLAYERRFN(_fn)	_fn
+#define CONST	const
+#define BCMFASTPATH
+
+
+
+
+#define _data	_data
+#define BCMROMDAT_NAME(_data)	_data
+#define _fn		_fn
+#define _fn	_fn
+#define STATIC	static
+#define BCMROMDAT_ARYSIZ(data)	ARRAYSIZE(data)
+#define BCMROMDAT_SIZEOF(data)	sizeof(data)
+#define BCMROMDAT_APATCH(data)
+#define BCMROMDAT_SPATCH(data)
+
+
+
+#define OVERLAY_INLINE
+#define OSTATIC			static
+#define BCMOVERLAYDATA(_ovly, _sym)	_sym
+#define BCMOVERLAYFN(_ovly, _fn)	_fn
+#define BCMOVERLAYERRFN(_fn)	_fn
+#define BCMROMOVERLAYDATA(_ovly, _data)	_data
+#define BCMROMOVERLAYFN(_ovly, _fn)		_fn
+#define BCMATTACHOVERLAYDATA(_ovly, _sym)	_sym
+#define BCMATTACHOVERLAYFN(_ovly, _fn)		_fn
+#define BCMINITOVERLAYDATA(_ovly, _sym)		_sym
+#define BCMINITOVERLAYFN(_ovly, _fn)		_fn
+#define BCMUNINITOVERLAYFN(_ovly, _fn)		_fn
+
+
+
+#define	SI_BUS			0	
+#define	PCI_BUS			1	
+#define	PCMCIA_BUS		2	
+#define SDIO_BUS		3	
+#define JTAG_BUS		4	
+#define USB_BUS			5	
+#define SPI_BUS			6	
+#define RPC_BUS			7	
+
+
+#ifdef BCMBUSTYPE
+#define BUSTYPE(bus) 	(BCMBUSTYPE)
+#else
+#define BUSTYPE(bus) 	(bus)
+#endif
+
+
+#ifdef BCMCHIPTYPE
+#define CHIPTYPE(bus) 	(BCMCHIPTYPE)
+#else
+#define CHIPTYPE(bus) 	(bus)
+#endif
+
+
+
+#if defined(BCMSPROMBUS)
+#define SPROMBUS	(BCMSPROMBUS)
+#elif defined(SI_PCMCIA_SROM)
+#define SPROMBUS	(PCMCIA_BUS)
+#else
+#define SPROMBUS	(PCI_BUS)
+#endif
+
+
+#ifdef BCMCHIPID
+#define CHIPID(chip)	(BCMCHIPID)
+#else
+#define CHIPID(chip)	(chip)
+#endif
+
+#ifdef BCMCHIPREV
+#define CHIPREV(rev)	(BCMCHIPREV)
+#else
+#define CHIPREV(rev)	(rev)
+#endif
+
+
+#define DMADDR_MASK_32 0x0		
+#define DMADDR_MASK_30 0xc0000000	
+#define DMADDR_MASK_0  0xffffffff	
+
+#define	DMADDRWIDTH_30  30 
+#define	DMADDRWIDTH_32  32 
+#define	DMADDRWIDTH_63  63 
+#define	DMADDRWIDTH_64  64 
+
+#ifdef BCMDMA64OSL
+typedef struct {
+	uint32 loaddr;
+	uint32 hiaddr;
+} dma64addr_t;
+
+typedef dma64addr_t dmaaddr_t;
+#define PHYSADDRHI(_pa) ((_pa).hiaddr)
+#define PHYSADDRHISET(_pa, _val) \
+	do { \
+		(_pa).hiaddr = (_val);		\
+	} while (0)
+#define PHYSADDRLO(_pa) ((_pa).loaddr)
+#define PHYSADDRLOSET(_pa, _val) \
+	do { \
+		(_pa).loaddr = (_val);		\
+	} while (0)
+
+#else
+typedef unsigned long dmaaddr_t;
+#define PHYSADDRHI(_pa) (0)
+#define PHYSADDRHISET(_pa, _val)
+#define PHYSADDRLO(_pa) ((_pa))
+#define PHYSADDRLOSET(_pa, _val) \
+	do { \
+		(_pa) = (_val);			\
+	} while (0)
+#endif 
+
+
+typedef struct  {
+	dmaaddr_t addr;
+	uint32	  length;
+} hnddma_seg_t;
+
+#define MAX_DMA_SEGS 4
+
+
+typedef struct {
+	void *oshdmah; 
+	uint origsize; 
+	uint nsegs;
+	hnddma_seg_t segs[MAX_DMA_SEGS];
+} hnddma_seg_map_t;
+
+
+
+
+#if defined(BCM_RPC_NOCOPY) || defined(BCM_RCP_TXNOCOPY)
+
+#define BCMEXTRAHDROOM 220
+#else
+#define BCMEXTRAHDROOM 172
+#endif
+
+
+#define BCMDONGLEHDRSZ 12
+#define BCMDONGLEPADSZ 16
+
+#define BCMDONGLEOVERHEAD	(BCMDONGLEHDRSZ + BCMDONGLEPADSZ)
+
+
+#if defined(BCMASSERT_LOG)
+#define BCMASSERT_SUPPORT
+#endif 
+
+
+#define BITFIELD_MASK(width) \
+		(((unsigned)1 << (width)) - 1)
+#define GFIELD(val, field) \
+		(((val) >> field ## _S) & field ## _M)
+#define SFIELD(val, field, bits) \
+		(((val) & (~(field ## _M << field ## _S))) | \
+		 ((unsigned)(bits) << field ## _S))
+
+
+#ifdef BCMSMALL
+#undef	BCMSPACE
+#define bcmspace	FALSE	
+#else
+#define	BCMSPACE
+#define bcmspace	TRUE	
+#endif
+
+
+#define	MAXSZ_NVRAM_VARS	4096
+
+#define LOCATOR_EXTERN static
+
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/bcmdevs.h b/drivers/net/wireless/bcmdhd/include/bcmdevs.h
new file mode 100644
index 000000000000..4d2f5763a3d9
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/bcmdevs.h
@@ -0,0 +1,747 @@
+/*
+ * Broadcom device-specific manifest constants.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmdevs.h 332966 2012-05-11 22:40:21Z $
+ */
+
+
+#ifndef	_BCMDEVS_H
+#define	_BCMDEVS_H
+
+
+#define	VENDOR_EPIGRAM		0xfeda
+#define	VENDOR_BROADCOM		0x14e4
+#define	VENDOR_3COM		0x10b7
+#define	VENDOR_NETGEAR		0x1385
+#define	VENDOR_DIAMOND		0x1092
+#define	VENDOR_INTEL		0x8086
+#define	VENDOR_DELL		0x1028
+#define	VENDOR_HP		0x103c
+#define	VENDOR_HP_COMPAQ	0x0e11
+#define	VENDOR_APPLE		0x106b
+#define VENDOR_SI_IMAGE		0x1095		
+#define VENDOR_BUFFALO		0x1154		
+#define VENDOR_TI		0x104c		
+#define VENDOR_RICOH		0x1180		
+#define VENDOR_JMICRON		0x197b
+
+
+
+#define	VENDOR_BROADCOM_PCMCIA	0x02d0
+
+
+#define	VENDOR_BROADCOM_SDIO	0x00BF
+
+
+#define BCM_DNGL_VID		0x0a5c
+#define BCM_DNGL_BL_PID_4328	0xbd12
+#define BCM_DNGL_BL_PID_4322	0xbd13
+#define BCM_DNGL_BL_PID_4319    0xbd16
+#define BCM_DNGL_BL_PID_43236   0xbd17
+#define BCM_DNGL_BL_PID_4332	0xbd18
+#define BCM_DNGL_BL_PID_4330	0xbd19
+#define BCM_DNGL_BL_PID_43239   0xbd1b
+#define BCM_DNGL_BDC_PID	0x0bdc
+#define BCM_DNGL_JTAG_PID	0x4a44
+#define BCM_DNGL_BL_PID_4324   0xbd1c
+
+
+#define BCM_HWUSB_PID_43239     43239
+
+
+#define	BCM4210_DEVICE_ID	0x1072		
+#define	BCM4230_DEVICE_ID	0x1086		
+#define	BCM4401_ENET_ID		0x170c		
+#define	BCM3352_DEVICE_ID	0x3352		
+#define	BCM3360_DEVICE_ID	0x3360		
+#define	BCM4211_DEVICE_ID	0x4211
+#define	BCM4231_DEVICE_ID	0x4231
+#define	BCM4303_D11B_ID		0x4303		
+#define	BCM4311_D11G_ID		0x4311		
+#define	BCM4311_D11DUAL_ID	0x4312		
+#define	BCM4311_D11A_ID		0x4313		
+#define	BCM4328_D11DUAL_ID	0x4314		
+#define	BCM4328_D11G_ID		0x4315		
+#define	BCM4328_D11A_ID		0x4316		
+#define	BCM4318_D11G_ID		0x4318		
+#define	BCM4318_D11DUAL_ID	0x4319		
+#define	BCM4318_D11A_ID		0x431a		
+#define	BCM4325_D11DUAL_ID	0x431b		
+#define	BCM4325_D11G_ID		0x431c		
+#define	BCM4325_D11A_ID		0x431d		
+#define	BCM4306_D11G_ID		0x4320		
+#define	BCM4306_D11A_ID		0x4321		
+#define	BCM4306_UART_ID		0x4322		
+#define	BCM4306_V90_ID		0x4323		
+#define	BCM4306_D11DUAL_ID	0x4324		
+#define	BCM4306_D11G_ID2	0x4325		
+#define	BCM4321_D11N_ID		0x4328		
+#define	BCM4321_D11N2G_ID	0x4329		
+#define	BCM4321_D11N5G_ID	0x432a		
+#define BCM4322_D11N_ID		0x432b		
+#define BCM4322_D11N2G_ID	0x432c		
+#define BCM4322_D11N5G_ID	0x432d		
+#define BCM4329_D11N_ID		0x432e		
+#define BCM4329_D11N2G_ID	0x432f		
+#define BCM4329_D11N5G_ID	0x4330		
+#define	BCM4315_D11DUAL_ID	0x4334		
+#define	BCM4315_D11G_ID		0x4335		
+#define	BCM4315_D11A_ID		0x4336		
+#define BCM4319_D11N_ID		0x4337		
+#define BCM4319_D11N2G_ID	0x4338		
+#define BCM4319_D11N5G_ID	0x4339		
+#define BCM43231_D11N2G_ID	0x4340		
+#define BCM43221_D11N2G_ID	0x4341		
+#define BCM43222_D11N_ID	0x4350		
+#define BCM43222_D11N2G_ID	0x4351		
+#define BCM43222_D11N5G_ID	0x4352		
+#define BCM43224_D11N_ID	0x4353		
+#define BCM43224_D11N_ID_VEN1	0x0576		
+#define BCM43226_D11N_ID	0x4354		
+#define BCM43236_D11N_ID	0x4346		
+#define BCM43236_D11N2G_ID	0x4347		
+#define BCM43236_D11N5G_ID	0x4348		
+#define BCM43225_D11N2G_ID	0x4357		
+#define BCM43421_D11N_ID	0xA99D		
+#define BCM4313_D11N2G_ID	0x4727		
+#define BCM4330_D11N_ID         0x4360          
+#define BCM4330_D11N2G_ID       0x4361          
+#define BCM4330_D11N5G_ID       0x4362          
+#define BCM4336_D11N_ID		0x4343		
+#define BCM6362_D11N_ID		0x435f		
+#define BCM4331_D11N_ID		0x4331		
+#define BCM4331_D11N2G_ID	0x4332		
+#define BCM4331_D11N5G_ID	0x4333		
+#define BCM43237_D11N_ID	0x4355		
+#define BCM43237_D11N5G_ID	0x4356		
+#define BCM43227_D11N2G_ID	0x4358		
+#define BCM43228_D11N_ID		0x4359		
+#define BCM43228_D11N5G_ID	0x435a		
+#define BCM43362_D11N_ID	0x4363		
+#define BCM43239_D11N_ID	0x4370		
+#define BCM4324_D11N_ID		0x4374		
+#define BCM43217_D11N2G_ID	0x43a9		
+#define BCM43131_D11N2G_ID	0x43aa		
+
+#define BCM4314_D11N2G_ID	0x4364		
+#define BCM43142_D11N2G_ID	0x4365		
+
+#define	BCMGPRS_UART_ID		0x4333		
+#define	BCMGPRS2_UART_ID	0x4344		
+#define FPGA_JTAGM_ID		0x43f0		
+#define BCM_JTAGM_ID		0x43f1		
+#define SDIOH_FPGA_ID		0x43f2		
+#define BCM_SDIOH_ID		0x43f3		
+#define SDIOD_FPGA_ID		0x43f4		
+#define SPIH_FPGA_ID		0x43f5		
+#define BCM_SPIH_ID		0x43f6		
+#define MIMO_FPGA_ID		0x43f8		
+#define BCM_JTAGM2_ID		0x43f9		
+#define SDHCI_FPGA_ID		0x43fa		
+#define	BCM4402_ENET_ID		0x4402		
+#define	BCM4402_V90_ID		0x4403		
+#define	BCM4410_DEVICE_ID	0x4410		
+#define	BCM4412_DEVICE_ID	0x4412		
+#define	BCM4430_DEVICE_ID	0x4430		
+#define	BCM4432_DEVICE_ID	0x4432		
+#define	BCM4704_ENET_ID		0x4706		
+#define	BCM4710_DEVICE_ID	0x4710		
+#define	BCM47XX_AUDIO_ID	0x4711		
+#define	BCM47XX_V90_ID		0x4712		
+#define	BCM47XX_ENET_ID		0x4713		
+#define	BCM47XX_EXT_ID		0x4714		
+#define	BCM47XX_GMAC_ID		0x4715		
+#define	BCM47XX_USBH_ID		0x4716		
+#define	BCM47XX_USBD_ID		0x4717		
+#define	BCM47XX_IPSEC_ID	0x4718		
+#define	BCM47XX_ROBO_ID		0x4719		
+#define	BCM47XX_USB20H_ID	0x471a		
+#define	BCM47XX_USB20D_ID	0x471b		
+#define	BCM47XX_ATA100_ID	0x471d		
+#define	BCM47XX_SATAXOR_ID	0x471e		
+#define	BCM47XX_GIGETH_ID	0x471f		
+#define	BCM4712_MIPS_ID		0x4720		
+#define	BCM4716_DEVICE_ID	0x4722		
+#define BCM47XX_SMBUS_EMU_ID	0x47fe		
+#define	BCM47XX_XOR_EMU_ID	0x47ff		
+#define	EPI41210_DEVICE_ID	0xa0fa		
+#define	EPI41230_DEVICE_ID	0xa10e		
+#define JINVANI_SDIOH_ID	0x4743		
+#define BCM27XX_SDIOH_ID	0x2702		
+#define PCIXX21_FLASHMEDIA_ID	0x803b		
+#define PCIXX21_SDIOH_ID	0x803c		
+#define R5C822_SDIOH_ID		0x0822		
+#define JMICRON_SDIOH_ID	0x2381		
+
+
+#define	BCM4306_CHIP_ID		0x4306		
+#define	BCM4311_CHIP_ID		0x4311		
+#define	BCM43111_CHIP_ID	43111		
+#define	BCM43112_CHIP_ID	43112		
+#define	BCM4312_CHIP_ID		0x4312		
+#define BCM4313_CHIP_ID		0x4313		
+#define	BCM43131_CHIP_ID	43131		
+#define	BCM4315_CHIP_ID		0x4315		
+#define	BCM4318_CHIP_ID		0x4318		
+#define	BCM4319_CHIP_ID		0x4319		
+#define	BCM4320_CHIP_ID		0x4320		
+#define	BCM4321_CHIP_ID		0x4321		
+#define	BCM43217_CHIP_ID	43217		
+#define	BCM4322_CHIP_ID		0x4322		
+#define	BCM43221_CHIP_ID	43221		
+#define	BCM43222_CHIP_ID	43222		
+#define	BCM43224_CHIP_ID	43224		
+#define	BCM43225_CHIP_ID	43225		
+#define	BCM43227_CHIP_ID	43227		
+#define	BCM43228_CHIP_ID	43228		
+#define	BCM43226_CHIP_ID	43226		
+#define	BCM43231_CHIP_ID	43231		
+#define	BCM43234_CHIP_ID	43234		
+#define	BCM43235_CHIP_ID	43235		
+#define	BCM43236_CHIP_ID	43236		
+#define	BCM43237_CHIP_ID	43237		
+#define	BCM43238_CHIP_ID	43238		
+#define	BCM43239_CHIP_ID	43239		
+#define	BCM43420_CHIP_ID	43420		
+#define	BCM43421_CHIP_ID	43421		
+#define	BCM43428_CHIP_ID	43428		
+#define	BCM43431_CHIP_ID	43431		
+#define	BCM4325_CHIP_ID		0x4325		
+#define	BCM4328_CHIP_ID		0x4328		
+#define	BCM4329_CHIP_ID		0x4329		
+#define	BCM4331_CHIP_ID		0x4331		
+#define BCM4336_CHIP_ID		0x4336		
+#define BCM43362_CHIP_ID	43362		
+#define BCM4330_CHIP_ID		0x4330		
+#define BCM6362_CHIP_ID		0x6362		
+#define BCM4314_CHIP_ID		0x4314		
+#define BCM43142_CHIP_ID	43142		
+#define BCM4324_CHIP_ID		0x4324		
+
+#define	BCM4342_CHIP_ID		4342		
+#define	BCM4402_CHIP_ID		0x4402		
+#define	BCM4704_CHIP_ID		0x4704		
+#define	BCM4710_CHIP_ID		0x4710		
+#define	BCM4712_CHIP_ID		0x4712		
+#define	BCM4716_CHIP_ID		0x4716		
+#define	BCM47162_CHIP_ID	47162		
+#define	BCM4748_CHIP_ID		0x4748		
+#define	BCM4749_CHIP_ID		0x4749		
+#define BCM4785_CHIP_ID		0x4785		
+#define	BCM5350_CHIP_ID		0x5350		
+#define	BCM5352_CHIP_ID		0x5352		
+#define	BCM5354_CHIP_ID		0x5354		
+#define BCM5365_CHIP_ID		0x5365		
+#define	BCM5356_CHIP_ID		0x5356		
+#define	BCM5357_CHIP_ID		0x5357		
+#define	BCM53572_CHIP_ID	53572		
+
+
+#define	BCM4303_PKG_ID		2		
+#define	BCM4309_PKG_ID		1		
+#define	BCM4712LARGE_PKG_ID	0		
+#define	BCM4712SMALL_PKG_ID	1		
+#define	BCM4712MID_PKG_ID	2		
+#define BCM4328USBD11G_PKG_ID	2		
+#define BCM4328USBDUAL_PKG_ID	3		
+#define BCM4328SDIOD11G_PKG_ID	4		
+#define BCM4328SDIODUAL_PKG_ID	5		
+#define BCM4329_289PIN_PKG_ID	0		
+#define BCM4329_182PIN_PKG_ID	1		
+#define BCM5354E_PKG_ID		1		
+#define	BCM4716_PKG_ID		8		
+#define	BCM4717_PKG_ID		9		
+#define	BCM4718_PKG_ID		10		
+#define BCM5356_PKG_NONMODE	1		
+#define BCM5358U_PKG_ID		8		
+#define BCM5358_PKG_ID		9		
+#define BCM47186_PKG_ID		10		
+#define BCM5357_PKG_ID		11		
+#define BCM5356U_PKG_ID		12		
+#define BCM53572_PKG_ID		8		
+#define BCM47188_PKG_ID		9		
+#define BCM4331TT_PKG_ID        8		
+#define BCM4331TN_PKG_ID        9		
+#define BCM4331TNA0_PKG_ID     0xb		
+
+
+#define HDLSIM5350_PKG_ID	1		
+#define HDLSIM_PKG_ID		14		
+#define HWSIM_PKG_ID		15		
+#define BCM43224_FAB_CSM	0x8		
+#define BCM43224_FAB_SMIC	0xa		
+#define BCM4336_WLBGA_PKG_ID	0x8
+#define BCM4330_WLBGA_PKG_ID	0x0
+#define BCM4314PCIE_ARM_PKG_ID		(8 | 0)	
+#define BCM4314SDIO_PKG_ID		(8 | 1)	
+#define BCM4314PCIE_PKG_ID		(8 | 2)	
+#define BCM4314SDIO_ARM_PKG_ID		(8 | 3)	
+#define BCM4314SDIO_FPBGA_PKG_ID	(8 | 4)	
+#define BCM4314DEV_PKG_ID		(8 | 6)	
+
+#define PCIXX21_FLASHMEDIA0_ID	0x8033		
+#define PCIXX21_SDIOH0_ID	0x8034		
+
+
+#define	BFL_BTC2WIRE		0x00000001  
+#define BFL_BTCOEX      0x00000001      
+#define	BFL_PACTRL		0x00000002  
+#define BFL_AIRLINEMODE	0x00000004  
+#define	BFL_ADCDIV		0x00000008  
+#define	BFL_ENETROBO		0x00000010  
+#define	BFL_NOPLLDOWN		0x00000020  
+#define	BFL_CCKHIPWR		0x00000040  
+#define	BFL_ENETADM		0x00000080  
+#define	BFL_ENETVLAN		0x00000100  
+#ifdef WLAFTERBURNER
+#define	BFL_AFTERBURNER		0x00000200  
+#endif 
+#define BFL_NOPCI		0x00000400  
+#define BFL_FEM			0x00000800  
+#define BFL_EXTLNA		0x00001000  
+#define BFL_HGPA		0x00002000  
+#define	BFL_BTC2WIRE_ALTGPIO	0x00004000  
+#define	BFL_ALTIQ		0x00008000  
+#define BFL_NOPA		0x00010000  
+#define BFL_RSSIINV		0x00020000  
+#define BFL_PAREF		0x00040000  
+#define BFL_3TSWITCH		0x00080000  
+#define BFL_PHASESHIFT		0x00100000  
+#define BFL_BUCKBOOST		0x00200000  
+#define BFL_FEM_BT		0x00400000  
+#define BFL_NOCBUCK		0x00800000  
+#define BFL_CCKFAVOREVM		0x01000000  
+#define BFL_PALDO		0x02000000  
+#define BFL_LNLDO2_2P5		0x04000000  
+#define BFL_FASTPWR		0x08000000
+#define BFL_UCPWRCTL_MININDX	0x08000000  
+#define BFL_EXTLNA_5GHz		0x10000000  
+#define BFL_TRSW_1by2		0x20000000  
+#define BFL_LO_TRSW_R_5GHz	0x40000000  
+#define BFL_ELNA_GAINDEF	0x80000000  
+#define BFL_EXTLNA_TX	0x20000000	
+
+
+#define BFL2_RXBB_INT_REG_DIS	0x00000001  
+#define BFL2_APLL_WAR		0x00000002  
+#define BFL2_TXPWRCTRL_EN	0x00000004  
+#define BFL2_2X4_DIV		0x00000008  
+#define BFL2_5G_PWRGAIN		0x00000010  
+#define BFL2_PCIEWAR_OVR	0x00000020  
+#define BFL2_CAESERS_BRD	0x00000040  
+#define BFL2_BTC3WIRE		0x00000080  
+#define BFL2_BTCLEGACY          0x00000080  
+#define BFL2_SKWRKFEM_BRD	0x00000100  
+#define BFL2_SPUR_WAR		0x00000200  
+#define BFL2_GPLL_WAR		0x00000400  
+#define BFL2_TRISTATE_LED	0x00000800  
+#define BFL2_SINGLEANT_CCK	0x00001000  
+#define BFL2_2G_SPUR_WAR	0x00002000  
+#define BFL2_BPHY_ALL_TXCORES	0x00004000  
+#define BFL2_FCC_BANDEDGE_WAR	0x00008000  
+#define BFL2_GPLL_WAR2	        0x00010000  
+#define BFL2_IPALVLSHIFT_3P3    0x00020000
+#define BFL2_INTERNDET_TXIQCAL  0x00040000  
+#define BFL2_XTALBUFOUTEN       0x00080000  
+#define BFL2_ANAPACTRL_2G	0x00100000  
+#define BFL2_ANAPACTRL_5G	0x00200000  
+#define BFL2_ELNACTRL_TRSW_2G	0x00400000  
+#define BFL2_BT_SHARE_ANT0	0x00800000  
+#define BFL2_TEMPSENSE_HIGHER	0x01000000  
+#define BFL2_BTC3WIREONLY       0x02000000  
+#define BFL2_PWR_NOMINAL	0x04000000  
+#define BFL2_EXTLNA_TX		0x08000000  
+						
+#define BFL2_4313_RADIOREG	0x10000000
+#define BFL2_SECI_LOPWR_DIS	0x20000000
+									   
+
+
+#define	BOARD_GPIO_BTC3W_IN	0x850	
+#define	BOARD_GPIO_BTC3W_OUT	0x020	
+#define	BOARD_GPIO_BTCMOD_IN	0x010	
+#define	BOARD_GPIO_BTCMOD_OUT	0x020	
+#define	BOARD_GPIO_BTC_IN	0x080	
+#define	BOARD_GPIO_BTC_OUT	0x100	
+#define	BOARD_GPIO_PACTRL	0x200	
+#define BOARD_GPIO_12		0x1000	
+#define BOARD_GPIO_13		0x2000	
+#define BOARD_GPIO_BTC4_IN	0x0800	
+#define BOARD_GPIO_BTC4_BT	0x2000	
+#define BOARD_GPIO_BTC4_STAT	0x4000	
+#define BOARD_GPIO_BTC4_WLAN	0x8000	
+#define	BOARD_GPIO_1_WLAN_PWR	0x2	
+#define	BOARD_GPIO_4_WLAN_PWR	0x10	
+
+#define GPIO_BTC4W_OUT_4312  0x010  
+#define GPIO_BTC4W_OUT_43224  0x020  
+#define GPIO_BTC4W_OUT_43224_SHARED  0x0e0  
+#define GPIO_BTC4W_OUT_43225  0x0e0  
+#define GPIO_BTC4W_OUT_43421  0x020  
+#define GPIO_BTC4W_OUT_4313  0x060  
+
+#define	PCI_CFG_GPIO_SCS	0x10	
+#define PCI_CFG_GPIO_HWRAD	0x20	
+#define PCI_CFG_GPIO_XTAL	0x40	
+#define PCI_CFG_GPIO_PLL	0x80	
+
+
+#define PLL_DELAY		150		
+#define FREF_DELAY		200		
+#define MIN_SLOW_CLK		32		
+#define	XTAL_ON_DELAY		1000		
+
+
+#define	BU4710_BOARD		0x0400
+#define	VSIM4710_BOARD		0x0401
+#define	QT4710_BOARD		0x0402
+
+#define	BU4309_BOARD		0x040a
+#define	BCM94309CB_BOARD	0x040b
+#define	BCM94309MP_BOARD	0x040c
+#define	BCM4309AP_BOARD		0x040d
+
+#define	BCM94302MP_BOARD	0x040e
+
+#define	BU4306_BOARD		0x0416
+#define	BCM94306CB_BOARD	0x0417
+#define	BCM94306MP_BOARD	0x0418
+
+#define	BCM94710D_BOARD		0x041a
+#define	BCM94710R1_BOARD	0x041b
+#define	BCM94710R4_BOARD	0x041c
+#define	BCM94710AP_BOARD	0x041d
+
+#define	BU2050_BOARD		0x041f
+
+#define	BCM94306P50_BOARD	0x0420
+
+#define	BCM94309G_BOARD		0x0421
+
+#define	BU4704_BOARD		0x0423
+#define	BU4702_BOARD		0x0424
+
+#define	BCM94306PC_BOARD	0x0425		
+
+#define	MPSG4306_BOARD		0x0427
+
+#define	BCM94702MN_BOARD	0x0428
+
+
+#define	BCM94702CPCI_BOARD	0x0429
+
+
+#define	BCM95380RR_BOARD	0x042a
+
+
+#define	BCM94306CBSG_BOARD	0x042b
+
+
+#define	PCSG94306_BOARD		0x042d
+
+
+#define	BU4704SD_BOARD		0x042e
+
+
+#define	BCM94704AGR_BOARD	0x042f
+
+
+#define	BCM94308MP_BOARD	0x0430
+
+
+#define	BCM94306GPRS_BOARD	0x0432
+
+
+#define BU5365_FPGA_BOARD	0x0433
+
+#define BU4712_BOARD		0x0444
+#define	BU4712SD_BOARD		0x045d
+#define	BU4712L_BOARD		0x045f
+
+
+#define BCM94712AP_BOARD	0x0445
+#define BCM94712P_BOARD		0x0446
+
+
+#define BU4318_BOARD		0x0447
+#define CB4318_BOARD		0x0448
+#define MPG4318_BOARD		0x0449
+#define MP4318_BOARD		0x044a
+#define SD4318_BOARD		0x044b
+
+
+#define BCM94313BU_BOARD	0x050f
+#define BCM94313HM_BOARD	0x0510
+#define BCM94313EPA_BOARD	0x0511
+#define BCM94313HMG_BOARD       0x051C
+
+
+#define BCM96338_BOARD		0x6338
+#define BCM96348_BOARD		0x6348
+#define BCM96358_BOARD		0x6358
+#define BCM96368_BOARD		0x6368
+
+
+#define	BCM94306P_BOARD		0x044c
+
+
+#define	BCM94303MP_BOARD	0x044e
+
+
+#define	BCM94306MPSGH_BOARD	0x044f
+
+
+#define BCM94306MPM		0x0450
+#define BCM94306MPL		0x0453
+
+
+#define	BCM94712AGR_BOARD	0x0451
+
+
+#define	PC4303_BOARD		0x0454
+
+
+#define	BCM95350K_BOARD		0x0455
+
+
+#define	BCM95350R_BOARD		0x0456
+
+
+#define	BCM94306MPLNA_BOARD	0x0457
+
+
+#define	BU4320_BOARD		0x0458
+#define	BU4320S_BOARD		0x0459
+#define	BCM94320PH_BOARD	0x045a
+
+
+#define	BCM94306MPH_BOARD	0x045b
+
+
+#define	BCM94306PCIV_BOARD	0x045c
+
+#define	BU4712SD_BOARD		0x045d
+
+#define	BCM94320PFLSH_BOARD	0x045e
+
+#define	BU4712L_BOARD		0x045f
+#define	BCM94712LGR_BOARD	0x0460
+#define	BCM94320R_BOARD		0x0461
+
+#define	BU5352_BOARD		0x0462
+
+#define	BCM94318MPGH_BOARD	0x0463
+
+#define	BU4311_BOARD		0x0464
+#define	BCM94311MC_BOARD	0x0465
+#define	BCM94311MCAG_BOARD	0x0466
+
+#define	BCM95352GR_BOARD	0x0467
+
+
+#define	BCM95351AGR_BOARD	0x0470
+
+
+#define	BCM94704MPCB_BOARD	0x0472
+
+
+#define BU4785_BOARD		0x0478
+
+
+#define BU4321_BOARD		0x046b
+#define BU4321E_BOARD		0x047c
+#define MP4321_BOARD		0x046c
+#define CB2_4321_BOARD		0x046d
+#define CB2_4321_AG_BOARD	0x0066
+#define MC4321_BOARD		0x046e
+
+
+#define BU4328_BOARD		0x0481
+#define BCM4328SDG_BOARD	0x0482
+#define BCM4328SDAG_BOARD	0x0483
+#define BCM4328UG_BOARD		0x0484
+#define BCM4328UAG_BOARD	0x0485
+#define BCM4328PC_BOARD		0x0486
+#define BCM4328CF_BOARD		0x0487
+
+
+#define BCM94325DEVBU_BOARD	0x0490
+#define BCM94325BGABU_BOARD	0x0491
+
+#define BCM94325SDGWB_BOARD	0x0492
+
+#define BCM94325SDGMDL_BOARD	0x04aa
+#define BCM94325SDGMDL2_BOARD	0x04c6
+#define BCM94325SDGMDL3_BOARD	0x04c9
+
+#define BCM94325SDABGWBA_BOARD	0x04e1
+
+
+#define BCM94322MC_SSID		0x04a4
+#define BCM94322USB_SSID	0x04a8	
+#define BCM94322HM_SSID		0x04b0
+#define BCM94322USB2D_SSID	0x04bf	
+
+
+#define	BCM4312MCGSG_BOARD	0x04b5
+
+
+#define BCM94315DEVBU_SSID	0x04c2
+#define BCM94315USBGP_SSID	0x04c7
+#define BCM94315BGABU_SSID	0x04ca
+#define BCM94315USBGP41_SSID	0x04cb
+
+
+#define BCM94319DEVBU_SSID	0X04e5
+#define BCM94319USB_SSID	0X04e6
+#define BCM94319SD_SSID		0X04e7
+
+
+#define BCM94716NR2_SSID	0x04cd
+
+
+#define BCM94319DEVBU_SSID	0X04e5
+#define BCM94319USBNP4L_SSID	0X04e6
+#define BCM94319WLUSBN4L_SSID	0X04e7
+#define BCM94319SDG_SSID	0X04ea
+#define BCM94319LCUSBSDN4L_SSID	0X04eb
+#define BCM94319USBB_SSID       0x04ee
+#define BCM94319LCSDN4L_SSID	0X0507
+#define BCM94319LSUSBN4L_SSID	0X0508
+#define BCM94319SDNA4L_SSID	0X0517
+#define BCM94319SDELNA4L_SSID	0X0518
+#define BCM94319SDELNA6L_SSID	0X0539
+#define BCM94319ARCADYAN_SSID	0X0546
+#define BCM94319WINDSOR_SSID    0x0561
+#define BCM94319MLAP_SSID       0x0562
+#define BCM94319SDNA_SSID       0x058b
+#define BCM94319BHEMU3_SSID     0x0563
+#define BCM94319SDHMB_SSID     0x058c
+#define BCM94319SDBREF_SSID     0x05a1
+#define BCM94319USBSDB_SSID     0x05a2
+
+
+
+#define BCM94329AGB_SSID	0X04b9
+#define BCM94329TDKMDL1_SSID	0X04ba
+#define BCM94329TDKMDL11_SSID	0X04fc
+#define BCM94329OLYMPICN18_SSID	0X04fd
+#define BCM94329OLYMPICN90_SSID	0X04fe
+#define BCM94329OLYMPICN90U_SSID 0X050c
+#define BCM94329OLYMPICN90M_SSID 0X050b
+#define BCM94329AGBF_SSID	0X04ff
+#define BCM94329OLYMPICX17_SSID	0X0504
+#define BCM94329OLYMPICX17M_SSID	0X050a
+#define BCM94329OLYMPICX17U_SSID	0X0509
+#define BCM94329OLYMPICUNO_SSID	0X0564
+#define BCM94329MOTOROLA_SSID   0X0565
+#define BCM94329OLYMPICLOCO_SSID	0X0568
+
+#define BCM94336SD_WLBGABU_SSID		0x0511
+#define BCM94336SD_WLBGAREF_SSID	0x0519
+#define BCM94336SDGP_SSID	0x0538
+#define BCM94336SDG_SSID	0x0519
+#define BCM94336SDGN_SSID	0x0538
+#define BCM94336SDGFC_SSID	0x056B
+
+
+#define BCM94330SDG_SSID	0x0528
+#define BCM94330SD_FCBGABU_SSID	0x052e
+#define BCM94330SD_WLBGABU_SSID	0x052f
+#define BCM94330SD_FCBGA_SSID	0x0530
+#define BCM94330FCSDAGB_SSID		0x0532
+#define BCM94330OLYMPICAMG_SSID		0x0549
+#define BCM94330OLYMPICAMGEPA_SSID		0x054F
+#define BCM94330OLYMPICUNO3_SSID	0x0551
+#define BCM94330WLSDAGB_SSID	0x0547
+#define BCM94330CSPSDAGBB_SSID	0x054A
+
+
+#define BCM943224X21        0x056e
+#define BCM943224X21_FCC    0x00d1
+
+
+#define BCM943228BU8_SSID	0x0540
+#define BCM943228BU9_SSID	0x0541
+#define BCM943228BU_SSID	0x0542
+#define BCM943227HM4L_SSID	0x0543
+#define BCM943227HMB_SSID	0x0544
+#define BCM943228HM4L_SSID	0x0545
+#define BCM943228SD_SSID	0x0573
+
+
+#define BCM943239MOD_SSID	0x05ac
+#define BCM943239REF_SSID	0x05aa
+
+
+#define BCM94331X19               0x00D6	
+#define BCM94331PCIEBT3Ax_SSID    0x00E4	
+#define BCM94331X12_2G_SSID       0x00EC	
+#define BCM94331X12_5G_SSID       0x00ED	
+#define BCM94331X29B              0x00EF	
+#define BCM94331BU_SSID           0x0523
+#define BCM94331S9BU_SSID         0x0524
+#define BCM94331MC_SSID           0x0525
+#define BCM94331MCI_SSID          0x0526
+#define BCM94331PCIEBT4_SSID      0x0527
+#define BCM94331HM_SSID           0x0574
+#define BCM94331PCIEDUAL_SSID     0x059B
+#define BCM94331MCH5_SSID         0x05A9
+#define BCM94331PCIEDUALV2_SSID   0x05B7
+#define BCM94331CS_SSID           0x05C6
+#define BCM94331CSAX_SSID         0x00EF
+
+
+#define BCM953572BU_SSID       0x058D
+#define BCM953572NR2_SSID      0x058E
+#define BCM947188NR2_SSID      0x058F
+#define BCM953572SDRNR2_SSID   0x0590
+
+
+#define BCM943236OLYMPICSULLEY_SSID 0x594
+#define BCM943236PREPROTOBLU2O3_SSID 0x5b9
+#define BCM943236USBELNA_SSID 0x5f8
+
+
+#define GPIO_NUMPINS		32
+
+
+#define RDL_RAM_BASE_4319 0x60000000
+#define RDL_RAM_BASE_4329 0x60000000
+#define RDL_RAM_SIZE_4319 0x48000
+#define RDL_RAM_SIZE_4329  0x48000
+#define RDL_RAM_SIZE_43236 0x70000
+#define RDL_RAM_BASE_43236 0x60000000
+#define RDL_RAM_SIZE_4328 0x60000
+#define RDL_RAM_BASE_4328 0x80000000
+#define RDL_RAM_SIZE_4322 0x60000
+#define RDL_RAM_BASE_4322 0x60000000
+
+
+#define MUXENAB_UART		0x00000001
+#define MUXENAB_GPIO		0x00000002
+#define MUXENAB_ERCX		0x00000004
+#define MUXENAB_JTAG		0x00000008
+#define MUXENAB_HOST_WAKE	0x00000010
+
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/bcmendian.h b/drivers/net/wireless/bcmdhd/include/bcmendian.h
new file mode 100644
index 000000000000..f3356a724b44
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/bcmendian.h
@@ -0,0 +1,279 @@
+/*
+ * Byte order utilities
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ *  $Id: bcmendian.h 277737 2011-08-16 17:54:59Z $
+ *
+ * This file by default provides proper behavior on little-endian architectures.
+ * On big-endian architectures, IL_BIGENDIAN should be defined.
+ */
+
+
+#ifndef _BCMENDIAN_H_
+#define _BCMENDIAN_H_
+
+#include <typedefs.h>
+
+
+#define BCMSWAP16(val) \
+	((uint16)((((uint16)(val) & (uint16)0x00ffU) << 8) | \
+		  (((uint16)(val) & (uint16)0xff00U) >> 8)))
+
+
+#define BCMSWAP32(val) \
+	((uint32)((((uint32)(val) & (uint32)0x000000ffU) << 24) | \
+		  (((uint32)(val) & (uint32)0x0000ff00U) <<  8) | \
+		  (((uint32)(val) & (uint32)0x00ff0000U) >>  8) | \
+		  (((uint32)(val) & (uint32)0xff000000U) >> 24)))
+
+
+#define BCMSWAP32BY16(val) \
+	((uint32)((((uint32)(val) & (uint32)0x0000ffffU) << 16) | \
+		  (((uint32)(val) & (uint32)0xffff0000U) >> 16)))
+
+
+#ifndef hton16
+#define HTON16(i) BCMSWAP16(i)
+#define	hton16(i) bcmswap16(i)
+#define	HTON32(i) BCMSWAP32(i)
+#define	hton32(i) bcmswap32(i)
+#define	NTOH16(i) BCMSWAP16(i)
+#define	ntoh16(i) bcmswap16(i)
+#define	NTOH32(i) BCMSWAP32(i)
+#define	ntoh32(i) bcmswap32(i)
+#define LTOH16(i) (i)
+#define ltoh16(i) (i)
+#define LTOH32(i) (i)
+#define ltoh32(i) (i)
+#define HTOL16(i) (i)
+#define htol16(i) (i)
+#define HTOL32(i) (i)
+#define htol32(i) (i)
+#endif 
+
+#define ltoh16_buf(buf, i)
+#define htol16_buf(buf, i)
+
+
+#define load32_ua(a)		ltoh32_ua(a)
+#define store32_ua(a, v)	htol32_ua_store(v, a)
+#define load16_ua(a)		ltoh16_ua(a)
+#define store16_ua(a, v)	htol16_ua_store(v, a)
+
+#define _LTOH16_UA(cp)	((cp)[0] | ((cp)[1] << 8))
+#define _LTOH32_UA(cp)	((cp)[0] | ((cp)[1] << 8) | ((cp)[2] << 16) | ((cp)[3] << 24))
+#define _NTOH16_UA(cp)	(((cp)[0] << 8) | (cp)[1])
+#define _NTOH32_UA(cp)	(((cp)[0] << 24) | ((cp)[1] << 16) | ((cp)[2] << 8) | (cp)[3])
+
+#define ltoh_ua(ptr) \
+	(sizeof(*(ptr)) == sizeof(uint8) ? *(const uint8 *)(ptr) : \
+	 sizeof(*(ptr)) == sizeof(uint16) ? _LTOH16_UA((const uint8 *)(ptr)) : \
+	 sizeof(*(ptr)) == sizeof(uint32) ? _LTOH32_UA((const uint8 *)(ptr)) : \
+	 *(uint8 *)0)
+
+#define ntoh_ua(ptr) \
+	(sizeof(*(ptr)) == sizeof(uint8) ? *(const uint8 *)(ptr) : \
+	 sizeof(*(ptr)) == sizeof(uint16) ? _NTOH16_UA((const uint8 *)(ptr)) : \
+	 sizeof(*(ptr)) == sizeof(uint32) ? _NTOH32_UA((const uint8 *)(ptr)) : \
+	 *(uint8 *)0)
+
+#ifdef __GNUC__
+
+
+
+#define bcmswap16(val) ({ \
+	uint16 _val = (val); \
+	BCMSWAP16(_val); \
+})
+
+#define bcmswap32(val) ({ \
+	uint32 _val = (val); \
+	BCMSWAP32(_val); \
+})
+
+#define bcmswap32by16(val) ({ \
+	uint32 _val = (val); \
+	BCMSWAP32BY16(_val); \
+})
+
+#define bcmswap16_buf(buf, len) ({ \
+	uint16 *_buf = (uint16 *)(buf); \
+	uint _wds = (len) / 2; \
+	while (_wds--) { \
+		*_buf = bcmswap16(*_buf); \
+		_buf++; \
+	} \
+})
+
+#define htol16_ua_store(val, bytes) ({ \
+	uint16 _val = (val); \
+	uint8 *_bytes = (uint8 *)(bytes); \
+	_bytes[0] = _val & 0xff; \
+	_bytes[1] = _val >> 8; \
+})
+
+#define htol32_ua_store(val, bytes) ({ \
+	uint32 _val = (val); \
+	uint8 *_bytes = (uint8 *)(bytes); \
+	_bytes[0] = _val & 0xff; \
+	_bytes[1] = (_val >> 8) & 0xff; \
+	_bytes[2] = (_val >> 16) & 0xff; \
+	_bytes[3] = _val >> 24; \
+})
+
+#define hton16_ua_store(val, bytes) ({ \
+	uint16 _val = (val); \
+	uint8 *_bytes = (uint8 *)(bytes); \
+	_bytes[0] = _val >> 8; \
+	_bytes[1] = _val & 0xff; \
+})
+
+#define hton32_ua_store(val, bytes) ({ \
+	uint32 _val = (val); \
+	uint8 *_bytes = (uint8 *)(bytes); \
+	_bytes[0] = _val >> 24; \
+	_bytes[1] = (_val >> 16) & 0xff; \
+	_bytes[2] = (_val >> 8) & 0xff; \
+	_bytes[3] = _val & 0xff; \
+})
+
+#define ltoh16_ua(bytes) ({ \
+	const uint8 *_bytes = (const uint8 *)(bytes); \
+	_LTOH16_UA(_bytes); \
+})
+
+#define ltoh32_ua(bytes) ({ \
+	const uint8 *_bytes = (const uint8 *)(bytes); \
+	_LTOH32_UA(_bytes); \
+})
+
+#define ntoh16_ua(bytes) ({ \
+	const uint8 *_bytes = (const uint8 *)(bytes); \
+	_NTOH16_UA(_bytes); \
+})
+
+#define ntoh32_ua(bytes) ({ \
+	const uint8 *_bytes = (const uint8 *)(bytes); \
+	_NTOH32_UA(_bytes); \
+})
+
+#else 
+
+
+static INLINE uint16
+bcmswap16(uint16 val)
+{
+	return BCMSWAP16(val);
+}
+
+static INLINE uint32
+bcmswap32(uint32 val)
+{
+	return BCMSWAP32(val);
+}
+
+static INLINE uint32
+bcmswap32by16(uint32 val)
+{
+	return BCMSWAP32BY16(val);
+}
+
+
+
+
+static INLINE void
+bcmswap16_buf(uint16 *buf, uint len)
+{
+	len = len / 2;
+
+	while (len--) {
+		*buf = bcmswap16(*buf);
+		buf++;
+	}
+}
+
+
+static INLINE void
+htol16_ua_store(uint16 val, uint8 *bytes)
+{
+	bytes[0] = val & 0xff;
+	bytes[1] = val >> 8;
+}
+
+
+static INLINE void
+htol32_ua_store(uint32 val, uint8 *bytes)
+{
+	bytes[0] = val & 0xff;
+	bytes[1] = (val >> 8) & 0xff;
+	bytes[2] = (val >> 16) & 0xff;
+	bytes[3] = val >> 24;
+}
+
+
+static INLINE void
+hton16_ua_store(uint16 val, uint8 *bytes)
+{
+	bytes[0] = val >> 8;
+	bytes[1] = val & 0xff;
+}
+
+
+static INLINE void
+hton32_ua_store(uint32 val, uint8 *bytes)
+{
+	bytes[0] = val >> 24;
+	bytes[1] = (val >> 16) & 0xff;
+	bytes[2] = (val >> 8) & 0xff;
+	bytes[3] = val & 0xff;
+}
+
+
+static INLINE uint16
+ltoh16_ua(const void *bytes)
+{
+	return _LTOH16_UA((const uint8 *)bytes);
+}
+
+
+static INLINE uint32
+ltoh32_ua(const void *bytes)
+{
+	return _LTOH32_UA((const uint8 *)bytes);
+}
+
+
+static INLINE uint16
+ntoh16_ua(const void *bytes)
+{
+	return _NTOH16_UA((const uint8 *)bytes);
+}
+
+
+static INLINE uint32
+ntoh32_ua(const void *bytes)
+{
+	return _NTOH32_UA((const uint8 *)bytes);
+}
+
+#endif 
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/bcmpcispi.h b/drivers/net/wireless/bcmdhd/include/bcmpcispi.h
new file mode 100644
index 000000000000..51e0427e7f60
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/bcmpcispi.h
@@ -0,0 +1,181 @@
+/*
+ * Broadcom PCI-SPI Host Controller Register Definitions
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmpcispi.h 277737 2011-08-16 17:54:59Z $
+ */
+#ifndef	_BCM_PCI_SPI_H
+#define	_BCM_PCI_SPI_H
+
+/* cpp contortions to concatenate w/arg prescan */
+#ifndef PAD
+#define	_PADLINE(line)	pad ## line
+#define	_XSTR(line)	_PADLINE(line)
+#define	PAD		_XSTR(__LINE__)
+#endif	/* PAD */
+
+
+typedef volatile struct {
+	uint32 spih_ctrl;		/* 0x00 SPI Control Register */
+	uint32 spih_stat;		/* 0x04 SPI Status Register */
+	uint32 spih_data;		/* 0x08 SPI Data Register, 32-bits wide */
+	uint32 spih_ext;		/* 0x0C SPI Extension Register */
+	uint32 PAD[4];			/* 0x10-0x1F PADDING */
+
+	uint32 spih_gpio_ctrl;		/* 0x20 SPI GPIO Control Register */
+	uint32 spih_gpio_data;		/* 0x24 SPI GPIO Data Register */
+	uint32 PAD[6];			/* 0x28-0x3F PADDING */
+
+	uint32 spih_int_edge;		/* 0x40 SPI Interrupt Edge Register (0=Level, 1=Edge) */
+	uint32 spih_int_pol;		/* 0x44 SPI Interrupt Polarity Register (0=Active Low, */
+							/* 1=Active High) */
+	uint32 spih_int_mask;		/* 0x48 SPI Interrupt Mask */
+	uint32 spih_int_status;		/* 0x4C SPI Interrupt Status */
+	uint32 PAD[4];			/* 0x50-0x5F PADDING */
+
+	uint32 spih_hex_disp;		/* 0x60 SPI 4-digit hex display value */
+	uint32 spih_current_ma;		/* 0x64 SPI SD card current consumption in mA */
+	uint32 PAD[1];			/* 0x68 PADDING */
+	uint32 spih_disp_sel;		/* 0x6c SPI 4-digit hex display mode select (1=current) */
+	uint32 PAD[4];			/* 0x70-0x7F PADDING */
+	uint32 PAD[8];			/* 0x80-0x9F PADDING */
+	uint32 PAD[8];			/* 0xA0-0xBF PADDING */
+	uint32 spih_pll_ctrl;	/* 0xC0 PLL Control Register */
+	uint32 spih_pll_status;	/* 0xC4 PLL Status Register */
+	uint32 spih_xtal_freq;	/* 0xC8 External Clock Frequency in units of 10000Hz */
+	uint32 spih_clk_count;	/* 0xCC External Clock Count Register */
+
+} spih_regs_t;
+
+typedef volatile struct {
+	uint32 cfg_space[0x40];		/* 0x000-0x0FF PCI Configuration Space (Read Only) */
+	uint32 P_IMG_CTRL0;		/* 0x100 PCI Image0 Control Register */
+
+	uint32 P_BA0;			/* 0x104 32 R/W PCI Image0 Base Address register */
+	uint32 P_AM0;			/* 0x108 32 R/W PCI Image0 Address Mask register */
+	uint32 P_TA0;			/* 0x10C 32 R/W PCI Image0 Translation Address register */
+	uint32 P_IMG_CTRL1;		/* 0x110 32 R/W PCI Image1 Control register */
+	uint32 P_BA1;			/* 0x114 32 R/W PCI Image1 Base Address register */
+	uint32 P_AM1;			/* 0x118 32 R/W PCI Image1 Address Mask register */
+	uint32 P_TA1;			/* 0x11C 32 R/W PCI Image1 Translation Address register */
+	uint32 P_IMG_CTRL2;		/* 0x120 32 R/W PCI Image2 Control register */
+	uint32 P_BA2;			/* 0x124 32 R/W PCI Image2 Base Address register */
+	uint32 P_AM2;			/* 0x128 32 R/W PCI Image2 Address Mask register */
+	uint32 P_TA2;			/* 0x12C 32 R/W PCI Image2 Translation Address register */
+	uint32 P_IMG_CTRL3;		/* 0x130 32 R/W PCI Image3 Control register */
+	uint32 P_BA3;			/* 0x134 32 R/W PCI Image3 Base Address register */
+	uint32 P_AM3;			/* 0x138 32 R/W PCI Image3 Address Mask register */
+	uint32 P_TA3;			/* 0x13C 32 R/W PCI Image3 Translation Address register */
+	uint32 P_IMG_CTRL4;		/* 0x140 32 R/W PCI Image4 Control register */
+	uint32 P_BA4;			/* 0x144 32 R/W PCI Image4 Base Address register */
+	uint32 P_AM4;			/* 0x148 32 R/W PCI Image4 Address Mask register */
+	uint32 P_TA4;			/* 0x14C 32 R/W PCI Image4 Translation Address register */
+	uint32 P_IMG_CTRL5;		/* 0x150 32 R/W PCI Image5 Control register */
+	uint32 P_BA5;			/* 0x154 32 R/W PCI Image5 Base Address register */
+	uint32 P_AM5;			/* 0x158 32 R/W PCI Image5 Address Mask register */
+	uint32 P_TA5;			/* 0x15C 32 R/W PCI Image5 Translation Address register */
+	uint32 P_ERR_CS;		/* 0x160 32 R/W PCI Error Control and Status register */
+	uint32 P_ERR_ADDR;		/* 0x164 32 R PCI Erroneous Address register */
+	uint32 P_ERR_DATA;		/* 0x168 32 R PCI Erroneous Data register */
+
+	uint32 PAD[5];			/* 0x16C-0x17F PADDING */
+
+	uint32 WB_CONF_SPC_BAR;		/* 0x180 32 R WISHBONE Configuration Space Base Address */
+	uint32 W_IMG_CTRL1;		/* 0x184 32 R/W WISHBONE Image1 Control register */
+	uint32 W_BA1;			/* 0x188 32 R/W WISHBONE Image1 Base Address register */
+	uint32 W_AM1;			/* 0x18C 32 R/W WISHBONE Image1 Address Mask register */
+	uint32 W_TA1;			/* 0x190 32 R/W WISHBONE Image1 Translation Address reg */
+	uint32 W_IMG_CTRL2;		/* 0x194 32 R/W WISHBONE Image2 Control register */
+	uint32 W_BA2;			/* 0x198 32 R/W WISHBONE Image2 Base Address register */
+	uint32 W_AM2;			/* 0x19C 32 R/W WISHBONE Image2 Address Mask register */
+	uint32 W_TA2;			/* 0x1A0 32 R/W WISHBONE Image2 Translation Address reg */
+	uint32 W_IMG_CTRL3;		/* 0x1A4 32 R/W WISHBONE Image3 Control register */
+	uint32 W_BA3;			/* 0x1A8 32 R/W WISHBONE Image3 Base Address register */
+	uint32 W_AM3;			/* 0x1AC 32 R/W WISHBONE Image3 Address Mask register */
+	uint32 W_TA3;			/* 0x1B0 32 R/W WISHBONE Image3 Translation Address reg */
+	uint32 W_IMG_CTRL4;		/* 0x1B4 32 R/W WISHBONE Image4 Control register */
+	uint32 W_BA4;			/* 0x1B8 32 R/W WISHBONE Image4 Base Address register */
+	uint32 W_AM4;			/* 0x1BC 32 R/W WISHBONE Image4 Address Mask register */
+	uint32 W_TA4;			/* 0x1C0 32 R/W WISHBONE Image4 Translation Address reg */
+	uint32 W_IMG_CTRL5;		/* 0x1C4 32 R/W WISHBONE Image5 Control register */
+	uint32 W_BA5;			/* 0x1C8 32 R/W WISHBONE Image5 Base Address register */
+	uint32 W_AM5;			/* 0x1CC 32 R/W WISHBONE Image5 Address Mask register */
+	uint32 W_TA5;			/* 0x1D0 32 R/W WISHBONE Image5 Translation Address reg */
+	uint32 W_ERR_CS;		/* 0x1D4 32 R/W WISHBONE Error Control and Status reg */
+	uint32 W_ERR_ADDR;		/* 0x1D8 32 R WISHBONE Erroneous Address register */
+	uint32 W_ERR_DATA;		/* 0x1DC 32 R WISHBONE Erroneous Data register */
+	uint32 CNF_ADDR;		/* 0x1E0 32 R/W Configuration Cycle register */
+	uint32 CNF_DATA;		/* 0x1E4 32 R/W Configuration Cycle Generation Data reg */
+
+	uint32 INT_ACK;			/* 0x1E8 32 R Interrupt Acknowledge register */
+	uint32 ICR;			/* 0x1EC 32 R/W Interrupt Control register */
+	uint32 ISR;			/* 0x1F0 32 R/W Interrupt Status register */
+} spih_pciregs_t;
+
+/*
+ * PCI Core interrupt enable and status bit definitions.
+ */
+
+/* PCI Core ICR Register bit definitions */
+#define PCI_INT_PROP_EN		(1 << 0)	/* Interrupt Propagation Enable */
+#define PCI_WB_ERR_INT_EN	(1 << 1)	/* Wishbone Error Interrupt Enable */
+#define PCI_PCI_ERR_INT_EN	(1 << 2)	/* PCI Error Interrupt Enable */
+#define PCI_PAR_ERR_INT_EN	(1 << 3)	/* Parity Error Interrupt Enable */
+#define PCI_SYS_ERR_INT_EN	(1 << 4)	/* System Error Interrupt Enable */
+#define PCI_SOFTWARE_RESET	(1U << 31)	/* Software reset of the PCI Core. */
+
+
+/* PCI Core ISR Register bit definitions */
+#define PCI_INT_PROP_ST		(1 << 0)	/* Interrupt Propagation Status */
+#define PCI_WB_ERR_INT_ST	(1 << 1)	/* Wishbone Error Interrupt Status */
+#define PCI_PCI_ERR_INT_ST	(1 << 2)	/* PCI Error Interrupt Status */
+#define PCI_PAR_ERR_INT_ST	(1 << 3)	/* Parity Error Interrupt Status */
+#define PCI_SYS_ERR_INT_ST	(1 << 4)	/* System Error Interrupt Status */
+
+
+/* Registers on the Wishbone bus */
+#define SPIH_CTLR_INTR		(1 << 0)	/* SPI Host Controller Core Interrupt */
+#define SPIH_DEV_INTR		(1 << 1)	/* SPI Device Interrupt */
+#define SPIH_WFIFO_INTR		(1 << 2)	/* SPI Tx FIFO Empty Intr (FPGA Rev >= 8) */
+
+/* GPIO Bit definitions */
+#define SPIH_CS			(1 << 0)	/* SPI Chip Select (active low) */
+#define SPIH_SLOT_POWER		(1 << 1)	/* SD Card Slot Power Enable */
+#define SPIH_CARD_DETECT	(1 << 2)	/* SD Card Detect */
+
+/* SPI Status Register Bit definitions */
+#define SPIH_STATE_MASK		0x30		/* SPI Transfer State Machine state mask */
+#define SPIH_STATE_SHIFT	4		/* SPI Transfer State Machine state shift */
+#define SPIH_WFFULL		(1 << 3)	/* SPI Write FIFO Full */
+#define SPIH_WFEMPTY		(1 << 2)	/* SPI Write FIFO Empty */
+#define SPIH_RFFULL		(1 << 1)	/* SPI Read FIFO Full */
+#define SPIH_RFEMPTY		(1 << 0)	/* SPI Read FIFO Empty */
+
+#define SPIH_EXT_CLK		(1U << 31)	/* Use External Clock as PLL Clock source. */
+
+#define SPIH_PLL_NO_CLK		(1 << 1)	/* Set to 1 if the PLL's input clock is lost. */
+#define SPIH_PLL_LOCKED		(1 << 3)	/* Set to 1 when the PLL is locked. */
+
+/* Spin bit loop bound check */
+#define SPI_SPIN_BOUND		0xf4240		/* 1 million */
+
+#endif /* _BCM_PCI_SPI_H */
diff --git a/drivers/net/wireless/bcmdhd/include/bcmperf.h b/drivers/net/wireless/bcmdhd/include/bcmperf.h
new file mode 100644
index 000000000000..a503edbd6226
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/bcmperf.h
@@ -0,0 +1,36 @@
+/*
+ * Performance counters software interface.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmperf.h 277737 2011-08-16 17:54:59Z $
+ */
+/* essai */
+#ifndef _BCMPERF_H_
+#define _BCMPERF_H_
+/* get cache hits and misses */
+#define BCMPERF_ENABLE_INSTRCOUNT()
+#define BCMPERF_ENABLE_ICACHE_MISS()
+#define BCMPERF_ENABLE_ICACHE_HIT()
+#define	BCMPERF_GETICACHE_MISS(x)	((x) = 0)
+#define	BCMPERF_GETICACHE_HIT(x)	((x) = 0)
+#define	BCMPERF_GETINSTRCOUNT(x)	((x) = 0)
+#endif /* _BCMPERF_H_ */
diff --git a/drivers/net/wireless/bcmdhd/include/bcmsdbus.h b/drivers/net/wireless/bcmdhd/include/bcmsdbus.h
new file mode 100644
index 000000000000..21a58b473e91
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/bcmsdbus.h
@@ -0,0 +1,128 @@
+/*
+ * Definitions for API from sdio common code (bcmsdh) to individual
+ * host controller drivers.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdbus.h 300017 2011-12-01 20:30:27Z $
+ */
+
+#ifndef	_sdio_api_h_
+#define	_sdio_api_h_
+
+
+#define SDIOH_API_RC_SUCCESS                          (0x00)
+#define SDIOH_API_RC_FAIL	                      (0x01)
+#define SDIOH_API_SUCCESS(status) (status == 0)
+
+#define SDIOH_READ              0	/* Read request */
+#define SDIOH_WRITE             1	/* Write request */
+
+#define SDIOH_DATA_FIX          0	/* Fixed addressing */
+#define SDIOH_DATA_INC          1	/* Incremental addressing */
+
+#define SDIOH_CMD_TYPE_NORMAL   0       /* Normal command */
+#define SDIOH_CMD_TYPE_APPEND   1       /* Append command */
+#define SDIOH_CMD_TYPE_CUTTHRU  2       /* Cut-through command */
+
+#define SDIOH_DATA_PIO          0       /* PIO mode */
+#define SDIOH_DATA_DMA          1       /* DMA mode */
+
+
+typedef int SDIOH_API_RC;
+
+/* SDio Host structure */
+typedef struct sdioh_info sdioh_info_t;
+
+/* callback function, taking one arg */
+typedef void (*sdioh_cb_fn_t)(void *);
+
+/* attach, return handler on success, NULL if failed.
+ *  The handler shall be provided by all subsequent calls. No local cache
+ *  cfghdl points to the starting address of pci device mapped memory
+ */
+extern sdioh_info_t * sdioh_attach(osl_t *osh, void *cfghdl, uint irq);
+extern SDIOH_API_RC sdioh_detach(osl_t *osh, sdioh_info_t *si);
+extern SDIOH_API_RC sdioh_interrupt_register(sdioh_info_t *si, sdioh_cb_fn_t fn, void *argh);
+extern SDIOH_API_RC sdioh_interrupt_deregister(sdioh_info_t *si);
+
+/* query whether SD interrupt is enabled or not */
+extern SDIOH_API_RC sdioh_interrupt_query(sdioh_info_t *si, bool *onoff);
+
+/* enable or disable SD interrupt */
+extern SDIOH_API_RC sdioh_interrupt_set(sdioh_info_t *si, bool enable_disable);
+
+#if defined(DHD_DEBUG)
+extern bool sdioh_interrupt_pending(sdioh_info_t *si);
+#endif
+
+/* read or write one byte using cmd52 */
+extern SDIOH_API_RC sdioh_request_byte(sdioh_info_t *si, uint rw, uint fnc, uint addr, uint8 *byte);
+
+/* read or write 2/4 bytes using cmd53 */
+extern SDIOH_API_RC sdioh_request_word(sdioh_info_t *si, uint cmd_type, uint rw, uint fnc,
+	uint addr, uint32 *word, uint nbyte);
+
+/* read or write any buffer using cmd53 */
+extern SDIOH_API_RC sdioh_request_buffer(sdioh_info_t *si, uint pio_dma, uint fix_inc,
+	uint rw, uint fnc_num, uint32 addr, uint regwidth, uint32 buflen, uint8 *buffer,
+	void *pkt);
+
+/* get cis data */
+extern SDIOH_API_RC sdioh_cis_read(sdioh_info_t *si, uint fuc, uint8 *cis, uint32 length);
+
+extern SDIOH_API_RC sdioh_cfg_read(sdioh_info_t *si, uint fuc, uint32 addr, uint8 *data);
+extern SDIOH_API_RC sdioh_cfg_write(sdioh_info_t *si, uint fuc, uint32 addr, uint8 *data);
+
+/* query number of io functions */
+extern uint sdioh_query_iofnum(sdioh_info_t *si);
+
+/* handle iovars */
+extern int sdioh_iovar_op(sdioh_info_t *si, const char *name,
+                          void *params, int plen, void *arg, int len, bool set);
+
+/* Issue abort to the specified function and clear controller as needed */
+extern int sdioh_abort(sdioh_info_t *si, uint fnc);
+
+/* Start and Stop SDIO without re-enumerating the SD card. */
+extern int sdioh_start(sdioh_info_t *si, int stage);
+extern int sdioh_stop(sdioh_info_t *si);
+
+/* Wait system lock free */
+extern int sdioh_waitlockfree(sdioh_info_t *si);
+
+/* Reset and re-initialize the device */
+extern int sdioh_sdio_reset(sdioh_info_t *si);
+
+/* Helper function */
+void *bcmsdh_get_sdioh(bcmsdh_info_t *sdh);
+
+
+
+extern SDIOH_API_RC sdioh_sleep(sdioh_info_t *si, bool enab);
+
+/* GPIO support */
+extern SDIOH_API_RC sdioh_gpio_init(sdioh_info_t *sd);
+extern bool sdioh_gpioin(sdioh_info_t *sd, uint32 gpio);
+extern SDIOH_API_RC sdioh_gpioouten(sdioh_info_t *sd, uint32 gpio);
+extern SDIOH_API_RC sdioh_gpioout(sdioh_info_t *sd, uint32 gpio, bool enab);
+
+#endif /* _sdio_api_h_ */
diff --git a/drivers/net/wireless/bcmdhd/include/bcmsdh.h b/drivers/net/wireless/bcmdhd/include/bcmsdh.h
new file mode 100644
index 000000000000..72ee65da8874
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/bcmsdh.h
@@ -0,0 +1,219 @@
+/*
+ * SDIO host client driver interface of Broadcom HNBU
+ *     export functions to client drivers
+ *     abstract OS and BUS specific details of SDIO
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdh.h 300017 2011-12-01 20:30:27Z $
+ */
+
+#ifndef	_bcmsdh_h_
+#define	_bcmsdh_h_
+
+#define BCMSDH_ERROR_VAL	0x0001 /* Error */
+#define BCMSDH_INFO_VAL		0x0002 /* Info */
+extern const uint bcmsdh_msglevel;
+
+#define BCMSDH_ERROR(x)
+#define BCMSDH_INFO(x)
+
+/* forward declarations */
+typedef struct bcmsdh_info bcmsdh_info_t;
+typedef void (*bcmsdh_cb_fn_t)(void *);
+
+/* Attach and build an interface to the underlying SD host driver.
+ *  - Allocates resources (structs, arrays, mem, OS handles, etc) needed by bcmsdh.
+ *  - Returns the bcmsdh handle and virtual address base for register access.
+ *    The returned handle should be used in all subsequent calls, but the bcmsh
+ *    implementation may maintain a single "default" handle (e.g. the first or
+ *    most recent one) to enable single-instance implementations to pass NULL.
+ */
+extern bcmsdh_info_t *bcmsdh_attach(osl_t *osh, void *cfghdl, void **regsva, uint irq);
+
+/* Detach - freeup resources allocated in attach */
+extern int bcmsdh_detach(osl_t *osh, void *sdh);
+
+/* Query if SD device interrupts are enabled */
+extern bool bcmsdh_intr_query(void *sdh);
+
+/* Enable/disable SD interrupt */
+extern int bcmsdh_intr_enable(void *sdh);
+extern int bcmsdh_intr_disable(void *sdh);
+
+/* Register/deregister device interrupt handler. */
+extern int bcmsdh_intr_reg(void *sdh, bcmsdh_cb_fn_t fn, void *argh);
+extern int bcmsdh_intr_dereg(void *sdh);
+
+#if defined(DHD_DEBUG)
+/* Query pending interrupt status from the host controller */
+extern bool bcmsdh_intr_pending(void *sdh);
+#endif
+
+#ifdef BCMLXSDMMC
+extern int bcmsdh_claim_host_and_lock(void *sdh);
+extern int bcmsdh_release_host_and_unlock(void *sdh);
+#endif /* BCMLXSDMMC */
+
+/* Register a callback to be called if and when bcmsdh detects
+ * device removal. No-op in the case of non-removable/hardwired devices.
+ */
+extern int bcmsdh_devremove_reg(void *sdh, bcmsdh_cb_fn_t fn, void *argh);
+
+/* Access SDIO address space (e.g. CCCR) using CMD52 (single-byte interface).
+ *   fn:   function number
+ *   addr: unmodified SDIO-space address
+ *   data: data byte to write
+ *   err:  pointer to error code (or NULL)
+ */
+extern uint8 bcmsdh_cfg_read(void *sdh, uint func, uint32 addr, int *err);
+extern void bcmsdh_cfg_write(void *sdh, uint func, uint32 addr, uint8 data, int *err);
+
+/* Read/Write 4bytes from/to cfg space */
+extern uint32 bcmsdh_cfg_read_word(void *sdh, uint fnc_num, uint32 addr, int *err);
+extern void bcmsdh_cfg_write_word(void *sdh, uint fnc_num, uint32 addr, uint32 data, int *err);
+
+/* Read CIS content for specified function.
+ *   fn:     function whose CIS is being requested (0 is common CIS)
+ *   cis:    pointer to memory location to place results
+ *   length: number of bytes to read
+ * Internally, this routine uses the values from the cis base regs (0x9-0xB)
+ * to form an SDIO-space address to read the data from.
+ */
+extern int bcmsdh_cis_read(void *sdh, uint func, uint8 *cis, uint length);
+
+/* Synchronous access to device (client) core registers via CMD53 to F1.
+ *   addr: backplane address (i.e. >= regsva from attach)
+ *   size: register width in bytes (2 or 4)
+ *   data: data for register write
+ */
+extern uint32 bcmsdh_reg_read(void *sdh, uint32 addr, uint size);
+extern uint32 bcmsdh_reg_write(void *sdh, uint32 addr, uint size, uint32 data);
+
+/* Indicate if last reg read/write failed */
+extern bool bcmsdh_regfail(void *sdh);
+
+/* Buffer transfer to/from device (client) core via cmd53.
+ *   fn:       function number
+ *   addr:     backplane address (i.e. >= regsva from attach)
+ *   flags:    backplane width, address increment, sync/async
+ *   buf:      pointer to memory data buffer
+ *   nbytes:   number of bytes to transfer to/from buf
+ *   pkt:      pointer to packet associated with buf (if any)
+ *   complete: callback function for command completion (async only)
+ *   handle:   handle for completion callback (first arg in callback)
+ * Returns 0 or error code.
+ * NOTE: Async operation is not currently supported.
+ */
+typedef void (*bcmsdh_cmplt_fn_t)(void *handle, int status, bool sync_waiting);
+extern int bcmsdh_send_buf(void *sdh, uint32 addr, uint fn, uint flags,
+                           uint8 *buf, uint nbytes, void *pkt,
+                           bcmsdh_cmplt_fn_t complete, void *handle);
+extern int bcmsdh_recv_buf(void *sdh, uint32 addr, uint fn, uint flags,
+                           uint8 *buf, uint nbytes, void *pkt,
+                           bcmsdh_cmplt_fn_t complete, void *handle);
+
+/* Flags bits */
+#define SDIO_REQ_4BYTE	0x1	/* Four-byte target (backplane) width (vs. two-byte) */
+#define SDIO_REQ_FIXED	0x2	/* Fixed address (FIFO) (vs. incrementing address) */
+#define SDIO_REQ_ASYNC	0x4	/* Async request (vs. sync request) */
+
+/* Pending (non-error) return code */
+#define BCME_PENDING	1
+
+/* Read/write to memory block (F1, no FIFO) via CMD53 (sync only).
+ *   rw:       read or write (0/1)
+ *   addr:     direct SDIO address
+ *   buf:      pointer to memory data buffer
+ *   nbytes:   number of bytes to transfer to/from buf
+ * Returns 0 or error code.
+ */
+extern int bcmsdh_rwdata(void *sdh, uint rw, uint32 addr, uint8 *buf, uint nbytes);
+
+/* Issue an abort to the specified function */
+extern int bcmsdh_abort(void *sdh, uint fn);
+
+/* Start SDIO Host Controller communication */
+extern int bcmsdh_start(void *sdh, int stage);
+
+/* Stop SDIO Host Controller communication */
+extern int bcmsdh_stop(void *sdh);
+
+/* Wait system lock free */
+extern int bcmsdh_waitlockfree(void *sdh);
+
+/* Returns the "Device ID" of target device on the SDIO bus. */
+extern int bcmsdh_query_device(void *sdh);
+
+/* Returns the number of IO functions reported by the device */
+extern uint bcmsdh_query_iofnum(void *sdh);
+
+/* Miscellaneous knob tweaker. */
+extern int bcmsdh_iovar_op(void *sdh, const char *name,
+                           void *params, int plen, void *arg, int len, bool set);
+
+/* Reset and reinitialize the device */
+extern int bcmsdh_reset(bcmsdh_info_t *sdh);
+
+/* helper functions */
+
+extern void *bcmsdh_get_sdioh(bcmsdh_info_t *sdh);
+
+/* callback functions */
+typedef struct {
+	/* attach to device */
+	void *(*attach)(uint16 vend_id, uint16 dev_id, uint16 bus, uint16 slot,
+	                uint16 func, uint bustype, void * regsva, osl_t * osh,
+	                void * param, void *dev);
+	/* detach from device */
+	void (*detach)(void *ch);
+} bcmsdh_driver_t;
+
+/* platform specific/high level functions */
+extern int bcmsdh_register(bcmsdh_driver_t *driver);
+extern void bcmsdh_unregister(void);
+extern bool bcmsdh_chipmatch(uint16 vendor, uint16 device);
+extern void bcmsdh_device_remove(void * sdh);
+
+#if defined(OOB_INTR_ONLY)
+extern int bcmsdh_register_oob_intr(void * dhdp);
+extern void bcmsdh_unregister_oob_intr(void);
+extern void bcmsdh_oob_intr_set(bool enable);
+#endif /* defined(OOB_INTR_ONLY) */
+/* Function to pass device-status bits to DHD. */
+extern uint32 bcmsdh_get_dstatus(void *sdh);
+
+/* Function to return current window addr */
+extern uint32 bcmsdh_cur_sbwad(void *sdh);
+
+/* Function to pass chipid and rev to lower layers for controlling pr's */
+extern void bcmsdh_chipinfo(void *sdh, uint32 chip, uint32 chiprev);
+
+
+extern int bcmsdh_sleep(void *sdh, bool enab);
+
+/* GPIO support */
+extern int bcmsdh_gpio_init(void *sd);
+extern bool bcmsdh_gpioin(void *sd, uint32 gpio);
+extern int bcmsdh_gpioouten(void *sd, uint32 gpio);
+extern int bcmsdh_gpioout(void *sd, uint32 gpio, bool enab);
+
+#endif	/* _bcmsdh_h_ */
diff --git a/drivers/net/wireless/bcmdhd/include/bcmsdh_sdmmc.h b/drivers/net/wireless/bcmdhd/include/bcmsdh_sdmmc.h
new file mode 100644
index 000000000000..db8ea596304c
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/bcmsdh_sdmmc.h
@@ -0,0 +1,123 @@
+/*
+ * BCMSDH Function Driver for the native SDIO/MMC driver in the Linux Kernel
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdh_sdmmc.h 314048 2012-02-09 20:31:56Z $
+ */
+
+#ifndef __BCMSDH_SDMMC_H__
+#define __BCMSDH_SDMMC_H__
+
+#define sd_err(x)
+#define sd_trace(x)
+#define sd_info(x)
+#define sd_debug(x)
+#define sd_data(x)
+#define sd_ctrl(x)
+
+#define sd_sync_dma(sd, read, nbytes)
+#define sd_init_dma(sd)
+#define sd_ack_intr(sd)
+#define sd_wakeup(sd);
+
+/* Allocate/init/free per-OS private data */
+extern int sdioh_sdmmc_osinit(sdioh_info_t *sd);
+extern void sdioh_sdmmc_osfree(sdioh_info_t *sd);
+
+#define sd_log(x)
+
+#define SDIOH_ASSERT(exp) \
+	do { if (!(exp)) \
+		printf("!!!ASSERT fail: file %s lines %d", __FILE__, __LINE__); \
+	} while (0)
+
+#define BLOCK_SIZE_4318 64
+#define BLOCK_SIZE_4328 512
+
+/* internal return code */
+#define SUCCESS	0
+#define ERROR	1
+
+/* private bus modes */
+#define SDIOH_MODE_SD4		2
+#define CLIENT_INTR 		0x100	/* Get rid of this! */
+
+struct sdioh_info {
+	osl_t 		*osh;			/* osh handler */
+	bool		client_intr_enabled;	/* interrupt connnected flag */
+	bool		intr_handler_valid;	/* client driver interrupt handler valid */
+	sdioh_cb_fn_t	intr_handler;		/* registered interrupt handler */
+	void		*intr_handler_arg;	/* argument to call interrupt handler */
+	uint16		intmask;		/* Current active interrupts */
+	void		*sdos_info;		/* Pointer to per-OS private data */
+
+	uint 		irq;			/* Client irq */
+	int 		intrcount;		/* Client interrupts */
+
+	bool		sd_use_dma;		/* DMA on CMD53 */
+	bool 		sd_blockmode;		/* sd_blockmode == FALSE => 64 Byte Cmd 53s. */
+						/*  Must be on for sd_multiblock to be effective */
+	bool 		use_client_ints;	/* If this is false, make sure to restore */
+	int 		sd_mode;		/* SD1/SD4/SPI */
+	int 		client_block_size[SDIOD_MAX_IOFUNCS];		/* Blocksize */
+	uint8 		num_funcs;		/* Supported funcs on client */
+	uint32 		com_cis_ptr;
+	uint32 		func_cis_ptr[SDIOD_MAX_IOFUNCS];
+
+#define SDIOH_SDMMC_MAX_SG_ENTRIES	32
+	struct scatterlist sg_list[SDIOH_SDMMC_MAX_SG_ENTRIES];
+	bool		use_rxchain;
+};
+
+/************************************************************
+ * Internal interfaces: per-port references into bcmsdh_sdmmc.c
+ */
+
+/* Global message bits */
+extern uint sd_msglevel;
+
+/* OS-independent interrupt handler */
+extern bool check_client_intr(sdioh_info_t *sd);
+
+/* Core interrupt enable/disable of device interrupts */
+extern void sdioh_sdmmc_devintr_on(sdioh_info_t *sd);
+extern void sdioh_sdmmc_devintr_off(sdioh_info_t *sd);
+
+
+/**************************************************************
+ * Internal interfaces: bcmsdh_sdmmc.c references to per-port code
+ */
+
+/* Register mapping routines */
+extern uint32 *sdioh_sdmmc_reg_map(osl_t *osh, int32 addr, int size);
+extern void sdioh_sdmmc_reg_unmap(osl_t *osh, int32 addr, int size);
+
+/* Interrupt (de)registration routines */
+extern int sdioh_sdmmc_register_irq(sdioh_info_t *sd, uint irq);
+extern void sdioh_sdmmc_free_irq(uint irq, sdioh_info_t *sd);
+
+typedef struct _BCMSDH_SDMMC_INSTANCE {
+	sdioh_info_t	*sd;
+	struct sdio_func *func[SDIOD_MAX_IOFUNCS];
+} BCMSDH_SDMMC_INSTANCE, *PBCMSDH_SDMMC_INSTANCE;
+
+#endif /* __BCMSDH_SDMMC_H__ */
diff --git a/drivers/net/wireless/bcmdhd/include/bcmsdpcm.h b/drivers/net/wireless/bcmdhd/include/bcmsdpcm.h
new file mode 100644
index 000000000000..1b9d39fee8fc
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/bcmsdpcm.h
@@ -0,0 +1,274 @@
+/*
+ * Broadcom SDIO/PCMCIA
+ * Software-specific definitions shared between device and host side
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdpcm.h 277737 2011-08-16 17:54:59Z $
+ */
+
+#ifndef	_bcmsdpcm_h_
+#define	_bcmsdpcm_h_
+
+/*
+ * Software allocation of To SB Mailbox resources
+ */
+
+/* intstatus bits */
+#define I_SMB_NAK	I_SMB_SW0	/* To SB Mailbox Frame NAK */
+#define I_SMB_INT_ACK	I_SMB_SW1	/* To SB Mailbox Host Interrupt ACK */
+#define I_SMB_USE_OOB	I_SMB_SW2	/* To SB Mailbox Use OOB Wakeup */
+#define I_SMB_DEV_INT	I_SMB_SW3	/* To SB Mailbox Miscellaneous Interrupt */
+
+#define I_TOSBMAIL      (I_SMB_NAK | I_SMB_INT_ACK | I_SMB_USE_OOB | I_SMB_DEV_INT)
+
+/* tosbmailbox bits corresponding to intstatus bits */
+#define SMB_NAK		(1 << 0)	/* To SB Mailbox Frame NAK */
+#define SMB_INT_ACK	(1 << 1)	/* To SB Mailbox Host Interrupt ACK */
+#define SMB_USE_OOB	(1 << 2)	/* To SB Mailbox Use OOB Wakeup */
+#define SMB_DEV_INT	(1 << 3)	/* To SB Mailbox Miscellaneous Interrupt */
+#define SMB_MASK	0x0000000f	/* To SB Mailbox Mask */
+
+/* tosbmailboxdata */
+#define SMB_DATA_VERSION_MASK	0x00ff0000	/* host protocol version (sent with F2 enable) */
+#define SMB_DATA_VERSION_SHIFT	16		/* host protocol version (sent with F2 enable) */
+
+/*
+ * Software allocation of To Host Mailbox resources
+ */
+
+/* intstatus bits */
+#define I_HMB_FC_STATE	I_HMB_SW0	/* To Host Mailbox Flow Control State */
+#define I_HMB_FC_CHANGE	I_HMB_SW1	/* To Host Mailbox Flow Control State Changed */
+#define I_HMB_FRAME_IND	I_HMB_SW2	/* To Host Mailbox Frame Indication */
+#define I_HMB_HOST_INT	I_HMB_SW3	/* To Host Mailbox Miscellaneous Interrupt */
+
+#define I_TOHOSTMAIL    (I_HMB_FC_CHANGE | I_HMB_FRAME_IND | I_HMB_HOST_INT)
+
+/* tohostmailbox bits corresponding to intstatus bits */
+#define HMB_FC_ON	(1 << 0)	/* To Host Mailbox Flow Control State */
+#define HMB_FC_CHANGE	(1 << 1)	/* To Host Mailbox Flow Control State Changed */
+#define HMB_FRAME_IND	(1 << 2)	/* To Host Mailbox Frame Indication */
+#define HMB_HOST_INT	(1 << 3)	/* To Host Mailbox Miscellaneous Interrupt */
+#define HMB_MASK	0x0000000f	/* To Host Mailbox Mask */
+
+/* tohostmailboxdata */
+#define HMB_DATA_NAKHANDLED	0x01	/* we're ready to retransmit NAK'd frame to host */
+#define HMB_DATA_DEVREADY	0x02	/* we're ready to to talk to host after enable */
+#define HMB_DATA_FC		0x04	/* per prio flowcontrol update flag to host */
+#define HMB_DATA_FWREADY	0x08	/* firmware is ready for protocol activity */
+#define HMB_DATA_FWHALT		0x10	/* firmware has halted operation */
+
+#define HMB_DATA_FCDATA_MASK	0xff000000	/* per prio flowcontrol data */
+#define HMB_DATA_FCDATA_SHIFT	24		/* per prio flowcontrol data */
+
+#define HMB_DATA_VERSION_MASK	0x00ff0000	/* device protocol version (with devready) */
+#define HMB_DATA_VERSION_SHIFT	16		/* device protocol version (with devready) */
+
+/*
+ * Software-defined protocol header
+ */
+
+/* Current protocol version */
+#define SDPCM_PROT_VERSION	4
+
+/* SW frame header */
+#define SDPCM_SEQUENCE_MASK		0x000000ff	/* Sequence Number Mask */
+#define SDPCM_PACKET_SEQUENCE(p) (((uint8 *)p)[0] & 0xff) /* p starts w/SW Header */
+
+#define SDPCM_CHANNEL_MASK		0x00000f00	/* Channel Number Mask */
+#define SDPCM_CHANNEL_SHIFT		8		/* Channel Number Shift */
+#define SDPCM_PACKET_CHANNEL(p) (((uint8 *)p)[1] & 0x0f) /* p starts w/SW Header */
+
+#define SDPCM_FLAGS_MASK		0x0000f000	/* Mask of flag bits */
+#define SDPCM_FLAGS_SHIFT		12		/* Flag bits shift */
+#define SDPCM_PACKET_FLAGS(p) ((((uint8 *)p)[1] & 0xf0) >> 4) /* p starts w/SW Header */
+
+/* Next Read Len: lookahead length of next frame, in 16-byte units (rounded up) */
+#define SDPCM_NEXTLEN_MASK		0x00ff0000	/* Next Read Len Mask */
+#define SDPCM_NEXTLEN_SHIFT		16		/* Next Read Len Shift */
+#define SDPCM_NEXTLEN_VALUE(p) ((((uint8 *)p)[2] & 0xff) << 4) /* p starts w/SW Header */
+#define SDPCM_NEXTLEN_OFFSET		2
+
+/* Data Offset from SOF (HW Tag, SW Tag, Pad) */
+#define SDPCM_DOFFSET_OFFSET		3		/* Data Offset */
+#define SDPCM_DOFFSET_VALUE(p) 		(((uint8 *)p)[SDPCM_DOFFSET_OFFSET] & 0xff)
+#define SDPCM_DOFFSET_MASK		0xff000000
+#define SDPCM_DOFFSET_SHIFT		24
+
+#define SDPCM_FCMASK_OFFSET		4		/* Flow control */
+#define SDPCM_FCMASK_VALUE(p)		(((uint8 *)p)[SDPCM_FCMASK_OFFSET ] & 0xff)
+#define SDPCM_WINDOW_OFFSET		5		/* Credit based fc */
+#define SDPCM_WINDOW_VALUE(p)		(((uint8 *)p)[SDPCM_WINDOW_OFFSET] & 0xff)
+#define SDPCM_VERSION_OFFSET		6		/* Version # */
+#define SDPCM_VERSION_VALUE(p)		(((uint8 *)p)[SDPCM_VERSION_OFFSET] & 0xff)
+#define SDPCM_UNUSED_OFFSET		7		/* Spare */
+#define SDPCM_UNUSED_VALUE(p)		(((uint8 *)p)[SDPCM_UNUSED_OFFSET] & 0xff)
+
+#define SDPCM_SWHEADER_LEN	8	/* SW header is 64 bits */
+
+/* logical channel numbers */
+#define SDPCM_CONTROL_CHANNEL	0	/* Control Request/Response Channel Id */
+#define SDPCM_EVENT_CHANNEL	1	/* Asyc Event Indication Channel Id */
+#define SDPCM_DATA_CHANNEL	2	/* Data Xmit/Recv Channel Id */
+#define SDPCM_GLOM_CHANNEL	3	/* For coalesced packets (superframes) */
+#define SDPCM_TEST_CHANNEL	15	/* Reserved for test/debug packets */
+#define SDPCM_MAX_CHANNEL	15
+
+#define SDPCM_SEQUENCE_WRAP	256	/* wrap-around val for eight-bit frame seq number */
+
+#define SDPCM_FLAG_RESVD0	0x01
+#define SDPCM_FLAG_RESVD1	0x02
+#define SDPCM_FLAG_GSPI_TXENAB	0x04
+#define SDPCM_FLAG_GLOMDESC	0x08	/* Superframe descriptor mask */
+
+/* For GLOM_CHANNEL frames, use a flag to indicate descriptor frame */
+#define SDPCM_GLOMDESC_FLAG	(SDPCM_FLAG_GLOMDESC << SDPCM_FLAGS_SHIFT)
+
+#define SDPCM_GLOMDESC(p)	(((uint8 *)p)[1] & 0x80)
+
+/* For TEST_CHANNEL packets, define another 4-byte header */
+#define SDPCM_TEST_HDRLEN	4	/* Generally: Cmd(1), Ext(1), Len(2);
+					 * Semantics of Ext byte depend on command.
+					 * Len is current or requested frame length, not
+					 * including test header; sent little-endian.
+					 */
+#define SDPCM_TEST_DISCARD	0x01	/* Receiver discards. Ext is a pattern id. */
+#define SDPCM_TEST_ECHOREQ	0x02	/* Echo request. Ext is a pattern id. */
+#define SDPCM_TEST_ECHORSP	0x03	/* Echo response. Ext is a pattern id. */
+#define SDPCM_TEST_BURST	0x04	/* Receiver to send a burst. Ext is a frame count */
+#define SDPCM_TEST_SEND		0x05	/* Receiver sets send mode. Ext is boolean on/off */
+
+/* Handy macro for filling in datagen packets with a pattern */
+#define SDPCM_TEST_FILL(byteno, id)	((uint8)(id + byteno))
+
+/*
+ * Software counters (first part matches hardware counters)
+ */
+
+typedef volatile struct {
+	uint32 cmd52rd;		/* Cmd52RdCount, SDIO: cmd52 reads */
+	uint32 cmd52wr;		/* Cmd52WrCount, SDIO: cmd52 writes */
+	uint32 cmd53rd;		/* Cmd53RdCount, SDIO: cmd53 reads */
+	uint32 cmd53wr;		/* Cmd53WrCount, SDIO: cmd53 writes */
+	uint32 abort;		/* AbortCount, SDIO: aborts */
+	uint32 datacrcerror;	/* DataCrcErrorCount, SDIO: frames w/CRC error */
+	uint32 rdoutofsync;	/* RdOutOfSyncCount, SDIO/PCMCIA: Rd Frm out of sync */
+	uint32 wroutofsync;	/* RdOutOfSyncCount, SDIO/PCMCIA: Wr Frm out of sync */
+	uint32 writebusy;	/* WriteBusyCount, SDIO: device asserted "busy" */
+	uint32 readwait;	/* ReadWaitCount, SDIO: no data ready for a read cmd */
+	uint32 readterm;	/* ReadTermCount, SDIO: read frame termination cmds */
+	uint32 writeterm;	/* WriteTermCount, SDIO: write frames termination cmds */
+	uint32 rxdescuflo;	/* receive descriptor underflows */
+	uint32 rxfifooflo;	/* receive fifo overflows */
+	uint32 txfifouflo;	/* transmit fifo underflows */
+	uint32 runt;		/* runt (too short) frames recv'd from bus */
+	uint32 badlen;		/* frame's rxh len does not match its hw tag len */
+	uint32 badcksum;	/* frame's hw tag chksum doesn't agree with len value */
+	uint32 seqbreak;	/* break in sequence # space from one rx frame to the next */
+	uint32 rxfcrc;		/* frame rx header indicates crc error */
+	uint32 rxfwoos;		/* frame rx header indicates write out of sync */
+	uint32 rxfwft;		/* frame rx header indicates write frame termination */
+	uint32 rxfabort;	/* frame rx header indicates frame aborted */
+	uint32 woosint;		/* write out of sync interrupt */
+	uint32 roosint;		/* read out of sync interrupt */
+	uint32 rftermint;	/* read frame terminate interrupt */
+	uint32 wftermint;	/* write frame terminate interrupt */
+} sdpcmd_cnt_t;
+
+/*
+ * Register Access Macros
+ */
+
+#define SDIODREV_IS(var, val)	((var) == (val))
+#define SDIODREV_GE(var, val)	((var) >= (val))
+#define SDIODREV_GT(var, val)	((var) > (val))
+#define SDIODREV_LT(var, val)	((var) < (val))
+#define SDIODREV_LE(var, val)	((var) <= (val))
+
+#define SDIODDMAREG32(h, dir, chnl) \
+	((dir) == DMA_TX ? \
+	 (void *)(uintptr)&((h)->regs->dma.sdiod32.dma32regs[chnl].xmt) : \
+	 (void *)(uintptr)&((h)->regs->dma.sdiod32.dma32regs[chnl].rcv))
+
+#define SDIODDMAREG64(h, dir, chnl) \
+	((dir) == DMA_TX ? \
+	 (void *)(uintptr)&((h)->regs->dma.sdiod64.dma64regs[chnl].xmt) : \
+	 (void *)(uintptr)&((h)->regs->dma.sdiod64.dma64regs[chnl].rcv))
+
+#define SDIODDMAREG(h, dir, chnl) \
+	(SDIODREV_LT((h)->corerev, 1) ? \
+	 SDIODDMAREG32((h), (dir), (chnl)) : \
+	 SDIODDMAREG64((h), (dir), (chnl)))
+
+#define PCMDDMAREG(h, dir, chnl) \
+	((dir) == DMA_TX ? \
+	 (void *)(uintptr)&((h)->regs->dma.pcm32.dmaregs.xmt) : \
+	 (void *)(uintptr)&((h)->regs->dma.pcm32.dmaregs.rcv))
+
+#define SDPCMDMAREG(h, dir, chnl, coreid) \
+	((coreid) == SDIOD_CORE_ID ? \
+	 SDIODDMAREG(h, dir, chnl) : \
+	 PCMDDMAREG(h, dir, chnl))
+
+#define SDIODFIFOREG(h, corerev) \
+	(SDIODREV_LT((corerev), 1) ? \
+	 ((dma32diag_t *)(uintptr)&((h)->regs->dma.sdiod32.dmafifo)) : \
+	 ((dma32diag_t *)(uintptr)&((h)->regs->dma.sdiod64.dmafifo)))
+
+#define PCMDFIFOREG(h) \
+	((dma32diag_t *)(uintptr)&((h)->regs->dma.pcm32.dmafifo))
+
+#define SDPCMFIFOREG(h, coreid, corerev) \
+	((coreid) == SDIOD_CORE_ID ? \
+	 SDIODFIFOREG(h, corerev) : \
+	 PCMDFIFOREG(h))
+
+/*
+ * Shared structure between dongle and the host.
+ * The structure contains pointers to trap or assert information.
+ */
+#define SDPCM_SHARED_VERSION       0x0001
+#define SDPCM_SHARED_VERSION_MASK  0x00FF
+#define SDPCM_SHARED_ASSERT_BUILT  0x0100
+#define SDPCM_SHARED_ASSERT        0x0200
+#define SDPCM_SHARED_TRAP          0x0400
+#define SDPCM_SHARED_IN_BRPT       0x0800
+#define SDPCM_SHARED_SET_BRPT      0x1000
+#define SDPCM_SHARED_PENDING_BRPT  0x2000
+
+typedef struct {
+	uint32	flags;
+	uint32  trap_addr;
+	uint32  assert_exp_addr;
+	uint32  assert_file_addr;
+	uint32  assert_line;
+	uint32	console_addr;		/* Address of hndrte_cons_t */
+	uint32  msgtrace_addr;
+	uint32  brpt_addr;
+} sdpcm_shared_t;
+
+extern sdpcm_shared_t sdpcm_shared;
+
+/* Function can be used to notify host of FW halt */
+extern void sdpcmd_fwhalt(void);
+
+#endif	/* _bcmsdpcm_h_ */
diff --git a/drivers/net/wireless/bcmdhd/include/bcmsdspi.h b/drivers/net/wireless/bcmdhd/include/bcmsdspi.h
new file mode 100644
index 000000000000..a62bee42b2ba
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/bcmsdspi.h
@@ -0,0 +1,135 @@
+/*
+ * SD-SPI Protocol Conversion - BCMSDH->SPI Translation Layer
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdspi.h 277737 2011-08-16 17:54:59Z $
+ */
+#ifndef	_BCM_SD_SPI_H
+#define	_BCM_SD_SPI_H
+
+/* global msglevel for debug messages - bitvals come from sdiovar.h */
+
+#define sd_err(x)
+#define sd_trace(x)
+#define sd_info(x)
+#define sd_debug(x)
+#define sd_data(x)
+#define sd_ctrl(x)
+
+#define sd_log(x)
+
+#define SDIOH_ASSERT(exp) \
+	do { if (!(exp)) \
+		printf("!!!ASSERT fail: file %s lines %d", __FILE__, __LINE__); \
+	} while (0)
+
+#define BLOCK_SIZE_4318 64
+#define BLOCK_SIZE_4328 512
+
+/* internal return code */
+#define SUCCESS	0
+#undef ERROR
+#define ERROR	1
+
+/* private bus modes */
+#define SDIOH_MODE_SPI		0
+
+#define USE_BLOCKMODE		0x2	/* Block mode can be single block or multi */
+#define USE_MULTIBLOCK		0x4
+
+struct sdioh_info {
+	uint cfg_bar;                   	/* pci cfg address for bar */
+	uint32 caps;                    	/* cached value of capabilities reg */
+	uint		bar0;			/* BAR0 for PCI Device */
+	osl_t 		*osh;			/* osh handler */
+	void		*controller;	/* Pointer to SPI Controller's private data struct */
+
+	uint		lockcount; 		/* nest count of sdspi_lock() calls */
+	bool		client_intr_enabled;	/* interrupt connnected flag */
+	bool		intr_handler_valid;	/* client driver interrupt handler valid */
+	sdioh_cb_fn_t	intr_handler;		/* registered interrupt handler */
+	void		*intr_handler_arg;	/* argument to call interrupt handler */
+	bool		initialized;		/* card initialized */
+	uint32		target_dev;		/* Target device ID */
+	uint32		intmask;		/* Current active interrupts */
+	void		*sdos_info;		/* Pointer to per-OS private data */
+
+	uint32		controller_type;	/* Host controller type */
+	uint8		version;		/* Host Controller Spec Compliance Version */
+	uint 		irq;			/* Client irq */
+	uint32 		intrcount;		/* Client interrupts */
+	uint32 		local_intrcount;	/* Controller interrupts */
+	bool 		host_init_done;		/* Controller initted */
+	bool 		card_init_done;		/* Client SDIO interface initted */
+	bool 		polled_mode;		/* polling for command completion */
+
+	bool		sd_use_dma;		/* DMA on CMD53 */
+	bool 		sd_blockmode;		/* sd_blockmode == FALSE => 64 Byte Cmd 53s. */
+						/*  Must be on for sd_multiblock to be effective */
+	bool 		use_client_ints;	/* If this is false, make sure to restore */
+	bool		got_hcint;		/* Host Controller interrupt. */
+						/*  polling hack in wl_linux.c:wl_timer() */
+	int 		adapter_slot;		/* Maybe dealing with multiple slots/controllers */
+	int 		sd_mode;		/* SD1/SD4/SPI */
+	int 		client_block_size[SDIOD_MAX_IOFUNCS];		/* Blocksize */
+	uint32 		data_xfer_count;	/* Current register transfer size */
+	uint32		cmd53_wr_data;		/* Used to pass CMD53 write data */
+	uint32		card_response;		/* Used to pass back response status byte */
+	uint32		card_rsp_data;		/* Used to pass back response data word */
+	uint16 		card_rca;		/* Current Address */
+	uint8 		num_funcs;		/* Supported funcs on client */
+	uint32 		com_cis_ptr;
+	uint32 		func_cis_ptr[SDIOD_MAX_IOFUNCS];
+	void		*dma_buf;
+	ulong		dma_phys;
+	int 		r_cnt;			/* rx count */
+	int 		t_cnt;			/* tx_count */
+};
+
+/************************************************************
+ * Internal interfaces: per-port references into bcmsdspi.c
+ */
+
+/* Global message bits */
+extern uint sd_msglevel;
+
+/**************************************************************
+ * Internal interfaces: bcmsdspi.c references to per-port code
+ */
+
+/* Register mapping routines */
+extern uint32 *spi_reg_map(osl_t *osh, uintptr addr, int size);
+extern void spi_reg_unmap(osl_t *osh, uintptr addr, int size);
+
+/* Interrupt (de)registration routines */
+extern int spi_register_irq(sdioh_info_t *sd, uint irq);
+extern void spi_free_irq(uint irq, sdioh_info_t *sd);
+
+/* OS-specific interrupt wrappers (atomic interrupt enable/disable) */
+extern void spi_lock(sdioh_info_t *sd);
+extern void spi_unlock(sdioh_info_t *sd);
+
+/* Allocate/init/free per-OS private data */
+extern int spi_osinit(sdioh_info_t *sd);
+extern void spi_osfree(sdioh_info_t *sd);
+
+#endif /* _BCM_SD_SPI_H */
diff --git a/drivers/net/wireless/bcmdhd/include/bcmsdstd.h b/drivers/net/wireless/bcmdhd/include/bcmsdstd.h
new file mode 100644
index 000000000000..c7382540b84f
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/bcmsdstd.h
@@ -0,0 +1,248 @@
+/*
+ *  'Standard' SDIO HOST CONTROLLER driver
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmsdstd.h 324819 2012-03-30 12:15:19Z $
+ */
+#ifndef	_BCM_SD_STD_H
+#define	_BCM_SD_STD_H
+
+/* global msglevel for debug messages - bitvals come from sdiovar.h */
+#define sd_err(x)	do { if (sd_msglevel & SDH_ERROR_VAL) printf x; } while (0)
+#define sd_trace(x)
+#define sd_info(x)
+#define sd_debug(x)
+#define sd_data(x)
+#define sd_ctrl(x)
+#define sd_dma(x)
+
+#define sd_sync_dma(sd, read, nbytes)
+#define sd_init_dma(sd)
+#define sd_ack_intr(sd)
+#define sd_wakeup(sd);
+/* Allocate/init/free per-OS private data */
+extern int sdstd_osinit(sdioh_info_t *sd);
+extern void sdstd_osfree(sdioh_info_t *sd);
+
+#define sd_log(x)
+
+#define SDIOH_ASSERT(exp) \
+	do { if (!(exp)) \
+		printf("!!!ASSERT fail: file %s lines %d", __FILE__, __LINE__); \
+	} while (0)
+
+#define BLOCK_SIZE_4318 64
+#define BLOCK_SIZE_4328 512
+
+/* internal return code */
+#define SUCCESS	0
+#define ERROR	1
+
+/* private bus modes */
+#define SDIOH_MODE_SPI		0
+#define SDIOH_MODE_SD1		1
+#define SDIOH_MODE_SD4		2
+
+#define MAX_SLOTS 6 	/* For PCI: Only 6 BAR entries => 6 slots */
+#define SDIOH_REG_WINSZ	0x100 /* Number of registers in Standard Host Controller */
+
+#define SDIOH_TYPE_ARASAN_HDK	1
+#define SDIOH_TYPE_BCM27XX	2
+#define SDIOH_TYPE_TI_PCIXX21	4	/* TI PCIxx21 Standard Host Controller */
+#define SDIOH_TYPE_RICOH_R5C822	5	/* Ricoh Co Ltd R5C822 SD/SDIO/MMC/MS/MSPro Host Adapter */
+#define SDIOH_TYPE_JMICRON	6	/* JMicron Standard SDIO Host Controller */
+
+/* For linux, allow yielding for dongle */
+#define BCMSDYIELD
+
+/* Expected card status value for CMD7 */
+#define SDIOH_CMD7_EXP_STATUS   0x00001E00
+
+#define RETRIES_LARGE 100000
+#define sdstd_os_yield(sd)	do {} while (0)
+#define RETRIES_SMALL 100
+
+
+#define USE_BLOCKMODE		0x2	/* Block mode can be single block or multi */
+#define USE_MULTIBLOCK		0x4
+
+#define USE_FIFO		0x8	/* Fifo vs non-fifo */
+
+#define CLIENT_INTR 		0x100	/* Get rid of this! */
+
+#define HC_INTR_RETUNING	0x1000
+
+
+struct sdioh_info {
+	uint cfg_bar;                   	/* pci cfg address for bar */
+	uint32 caps;                    	/* cached value of capabilities reg */
+	uint32 curr_caps;                    	/* max current capabilities reg */
+
+	osl_t 		*osh;			/* osh handler */
+	volatile char 	*mem_space;		/* pci device memory va */
+	uint		lockcount; 		/* nest count of sdstd_lock() calls */
+	bool		client_intr_enabled;	/* interrupt connnected flag */
+	bool		intr_handler_valid;	/* client driver interrupt handler valid */
+	sdioh_cb_fn_t	intr_handler;		/* registered interrupt handler */
+	void		*intr_handler_arg;	/* argument to call interrupt handler */
+	bool		initialized;		/* card initialized */
+	uint		target_dev;		/* Target device ID */
+	uint16		intmask;		/* Current active interrupts */
+	void		*sdos_info;		/* Pointer to per-OS private data */
+
+	uint32		controller_type;	/* Host controller type */
+	uint8		version;		/* Host Controller Spec Compliance Version */
+	uint 		irq;			/* Client irq */
+	int 		intrcount;		/* Client interrupts */
+	int 		local_intrcount;	/* Controller interrupts */
+	bool 		host_init_done;		/* Controller initted */
+	bool 		card_init_done;		/* Client SDIO interface initted */
+	bool 		polled_mode;		/* polling for command completion */
+
+	bool 		sd_blockmode;		/* sd_blockmode == FALSE => 64 Byte Cmd 53s. */
+						/*  Must be on for sd_multiblock to be effective */
+	bool 		use_client_ints;	/* If this is false, make sure to restore */
+						/*  polling hack in wl_linux.c:wl_timer() */
+	int 		adapter_slot;		/* Maybe dealing with multiple slots/controllers */
+	int 		sd_mode;		/* SD1/SD4/SPI */
+	int 		client_block_size[SDIOD_MAX_IOFUNCS];		/* Blocksize */
+	uint32 		data_xfer_count;	/* Current transfer */
+	uint16 		card_rca;		/* Current Address */
+	int8		sd_dma_mode;		/* DMA Mode (PIO, SDMA, ... ADMA2) on CMD53 */
+	uint8 		num_funcs;		/* Supported funcs on client */
+	uint32 		com_cis_ptr;
+	uint32 		func_cis_ptr[SDIOD_MAX_IOFUNCS];
+	void		*dma_buf;		/* DMA Buffer virtual address */
+	ulong		dma_phys;		/* DMA Buffer physical address */
+	void		*adma2_dscr_buf;	/* ADMA2 Descriptor Buffer virtual address */
+	ulong		adma2_dscr_phys;	/* ADMA2 Descriptor Buffer physical address */
+
+	/* adjustments needed to make the dma align properly */
+	void		*dma_start_buf;
+	ulong		dma_start_phys;
+	uint		alloced_dma_size;
+	void		*adma2_dscr_start_buf;
+	ulong		adma2_dscr_start_phys;
+	uint		alloced_adma2_dscr_size;
+
+	int 		r_cnt;			/* rx count */
+	int 		t_cnt;			/* tx_count */
+	bool		got_hcint;		/* local interrupt flag */
+	uint16		last_intrstatus;	/* to cache intrstatus */
+	int 	host_UHSISupported;		/* whether UHSI is supported for HC. */
+	int 	card_UHSI_voltage_Supported; 	/* whether UHSI is supported for
+						 * Card in terms of Voltage [1.8 or 3.3].
+						 */
+	int	global_UHSI_Supp;	/* type of UHSI support in both host and card.
+					 * HOST_SDR_UNSUPP: capabilities not supported/matched
+					 * HOST_SDR_12_25: SDR12 and SDR25 supported
+					 * HOST_SDR_50_104_DDR: one of SDR50/SDR104 or DDR50 supptd
+					 */
+	int	sd3_dat_state; 		/* data transfer state used for retuning check */
+	int	sd3_tun_state; 		/* tuning state used for retuning check */
+	bool	sd3_tuning_reqd; 	/* tuning requirement parameter */
+	uint32	caps3;			/* cached value of 32 MSbits capabilities reg (SDIO 3.0) */
+};
+
+#define DMA_MODE_NONE	0
+#define DMA_MODE_SDMA	1
+#define DMA_MODE_ADMA1	2
+#define DMA_MODE_ADMA2	3
+#define DMA_MODE_ADMA2_64 4
+#define DMA_MODE_AUTO	-1
+
+#define USE_DMA(sd)		((bool)((sd->sd_dma_mode > 0) ? TRUE : FALSE))
+
+/* States for Tuning and corr data */
+#define TUNING_IDLE 			0
+#define TUNING_START 			1
+#define TUNING_START_AFTER_DAT 	2
+#define TUNING_ONGOING 			3
+
+#define DATA_TRANSFER_IDLE 		0
+#define DATA_TRANSFER_ONGOING	1
+#define CHECK_TUNING_PRE_DATA   1
+#define CHECK_TUNING_POST_DATA  2
+
+
+/************************************************************
+ * Internal interfaces: per-port references into bcmsdstd.c
+ */
+
+/* Global message bits */
+extern uint sd_msglevel;
+
+/* OS-independent interrupt handler */
+extern bool check_client_intr(sdioh_info_t *sd);
+
+/* Core interrupt enable/disable of device interrupts */
+extern void sdstd_devintr_on(sdioh_info_t *sd);
+extern void sdstd_devintr_off(sdioh_info_t *sd);
+
+/* Enable/disable interrupts for local controller events */
+extern void sdstd_intrs_on(sdioh_info_t *sd, uint16 norm, uint16 err);
+extern void sdstd_intrs_off(sdioh_info_t *sd, uint16 norm, uint16 err);
+
+/* Wait for specified interrupt and error bits to be set */
+extern void sdstd_spinbits(sdioh_info_t *sd, uint16 norm, uint16 err);
+
+
+/**************************************************************
+ * Internal interfaces: bcmsdstd.c references to per-port code
+ */
+
+/* Register mapping routines */
+extern uint32 *sdstd_reg_map(osl_t *osh, int32 addr, int size);
+extern void sdstd_reg_unmap(osl_t *osh, int32 addr, int size);
+
+/* Interrupt (de)registration routines */
+extern int sdstd_register_irq(sdioh_info_t *sd, uint irq);
+extern void sdstd_free_irq(uint irq, sdioh_info_t *sd);
+
+/* OS-specific interrupt wrappers (atomic interrupt enable/disable) */
+extern void sdstd_lock(sdioh_info_t *sd);
+extern void sdstd_unlock(sdioh_info_t *sd);
+extern void sdstd_waitlockfree(sdioh_info_t *sd);
+
+/* OS-specific wait-for-interrupt-or-status */
+extern int sdstd_waitbits(sdioh_info_t *sd, uint16 norm, uint16 err, bool yield, uint16 *bits);
+
+/* used by bcmsdstd_linux [implemented in sdstd] */
+extern void sdstd_3_enable_retuning_int(sdioh_info_t *sd);
+extern void sdstd_3_disable_retuning_int(sdioh_info_t *sd);
+extern bool sdstd_3_is_retuning_int_set(sdioh_info_t *sd);
+extern void sdstd_3_check_and_do_tuning(sdioh_info_t *sd, int tuning_param);
+extern bool sdstd_3_check_and_set_retuning(sdioh_info_t *sd);
+extern int sdstd_3_get_tune_state(sdioh_info_t *sd);
+extern int sdstd_3_get_data_state(sdioh_info_t *sd);
+extern void sdstd_3_set_tune_state(sdioh_info_t *sd, int state);
+extern void sdstd_3_set_data_state(sdioh_info_t *sd, int state);
+extern uint8 sdstd_3_get_tuning_exp(sdioh_info_t *sd);
+extern uint32 sdstd_3_get_uhsi_clkmode(sdioh_info_t *sd);
+extern int sdstd_3_clk_tuning(sdioh_info_t *sd, uint32 sd3ClkMode);
+
+/* used by sdstd [implemented in bcmsdstd_linux/ndis] */
+extern void sdstd_3_start_tuning(sdioh_info_t *sd);
+extern void sdstd_3_osinit_tuning(sdioh_info_t *sd);
+extern void sdstd_3_osclean_tuning(sdioh_info_t *sd);
+
+#endif /* _BCM_SD_STD_H */
diff --git a/drivers/net/wireless/bcmdhd/include/bcmspi.h b/drivers/net/wireless/bcmdhd/include/bcmspi.h
new file mode 100644
index 000000000000..34a02d00c6bd
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/bcmspi.h
@@ -0,0 +1,40 @@
+/*
+ * Broadcom SPI Low-Level Hardware Driver API
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmspi.h 277737 2011-08-16 17:54:59Z $
+ */
+#ifndef	_BCM_SPI_H
+#define	_BCM_SPI_H
+
+extern void spi_devintr_off(sdioh_info_t *sd);
+extern void spi_devintr_on(sdioh_info_t *sd);
+extern bool spi_start_clock(sdioh_info_t *sd, uint16 new_sd_divisor);
+extern bool spi_controller_highspeed_mode(sdioh_info_t *sd, bool hsmode);
+extern bool spi_check_client_intr(sdioh_info_t *sd, int *is_dev_intr);
+extern bool spi_hw_attach(sdioh_info_t *sd);
+extern bool spi_hw_detach(sdioh_info_t *sd);
+extern void spi_sendrecv(sdioh_info_t *sd, uint8 *msg_out, uint8 *msg_in, int msglen);
+extern void spi_spinbits(sdioh_info_t *sd);
+extern void spi_waitbits(sdioh_info_t *sd, bool yield);
+
+#endif /* _BCM_SPI_H */
diff --git a/drivers/net/wireless/bcmdhd/include/bcmutils.h b/drivers/net/wireless/bcmdhd/include/bcmutils.h
new file mode 100644
index 000000000000..6849c26da837
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/bcmutils.h
@@ -0,0 +1,720 @@
+/*
+ * Misc useful os-independent macros and functions.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmutils.h 294991 2011-11-09 00:17:28Z $
+ */
+
+
+#ifndef	_bcmutils_h_
+#define	_bcmutils_h_
+
+#define bcm_strcpy_s(dst, noOfElements, src)            strcpy((dst), (src))
+#define bcm_strncpy_s(dst, noOfElements, src, count)    strncpy((dst), (src), (count))
+#define bcm_strcat_s(dst, noOfElements, src)            strcat((dst), (src))
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+#define _BCM_U	0x01	
+#define _BCM_L	0x02	
+#define _BCM_D	0x04	
+#define _BCM_C	0x08	
+#define _BCM_P	0x10	
+#define _BCM_S	0x20	
+#define _BCM_X	0x40	
+#define _BCM_SP	0x80	
+
+extern const unsigned char bcm_ctype[];
+#define bcm_ismask(x)	(bcm_ctype[(int)(unsigned char)(x)])
+
+#define bcm_isalnum(c)	((bcm_ismask(c)&(_BCM_U|_BCM_L|_BCM_D)) != 0)
+#define bcm_isalpha(c)	((bcm_ismask(c)&(_BCM_U|_BCM_L)) != 0)
+#define bcm_iscntrl(c)	((bcm_ismask(c)&(_BCM_C)) != 0)
+#define bcm_isdigit(c)	((bcm_ismask(c)&(_BCM_D)) != 0)
+#define bcm_isgraph(c)	((bcm_ismask(c)&(_BCM_P|_BCM_U|_BCM_L|_BCM_D)) != 0)
+#define bcm_islower(c)	((bcm_ismask(c)&(_BCM_L)) != 0)
+#define bcm_isprint(c)	((bcm_ismask(c)&(_BCM_P|_BCM_U|_BCM_L|_BCM_D|_BCM_SP)) != 0)
+#define bcm_ispunct(c)	((bcm_ismask(c)&(_BCM_P)) != 0)
+#define bcm_isspace(c)	((bcm_ismask(c)&(_BCM_S)) != 0)
+#define bcm_isupper(c)	((bcm_ismask(c)&(_BCM_U)) != 0)
+#define bcm_isxdigit(c)	((bcm_ismask(c)&(_BCM_D|_BCM_X)) != 0)
+#define bcm_tolower(c)	(bcm_isupper((c)) ? ((c) + 'a' - 'A') : (c))
+#define bcm_toupper(c)	(bcm_islower((c)) ? ((c) + 'A' - 'a') : (c))
+
+
+
+struct bcmstrbuf {
+	char *buf;	
+	unsigned int size;	
+	char *origbuf;	
+	unsigned int origsize;	
+};
+
+
+#ifdef BCMDRIVER
+#include <osl.h>
+
+#define GPIO_PIN_NOTDEFINED 	0x20	
+
+
+#define SPINWAIT(exp, us) { \
+	uint countdown = (us) + 9; \
+	while ((exp) && (countdown >= 10)) {\
+		OSL_DELAY(10); \
+		countdown -= 10; \
+	} \
+}
+
+
+#ifndef PKTQ_LEN_DEFAULT
+#define PKTQ_LEN_DEFAULT        128	
+#endif
+#ifndef PKTQ_MAX_PREC
+#define PKTQ_MAX_PREC           16	
+#endif
+
+typedef struct pktq_prec {
+	void *head;     
+	void *tail;     
+	uint16 len;     
+	uint16 max;     
+} pktq_prec_t;
+
+
+
+struct pktq {
+	uint16 num_prec;        
+	uint16 hi_prec;         
+	uint16 max;             
+	uint16 len;             
+	
+	struct pktq_prec q[PKTQ_MAX_PREC];
+};
+
+
+struct spktq {
+	uint16 num_prec;        
+	uint16 hi_prec;         
+	uint16 max;             
+	uint16 len;             
+	
+	struct pktq_prec q[1];
+};
+
+#define PKTQ_PREC_ITER(pq, prec)        for (prec = (pq)->num_prec - 1; prec >= 0; prec--)
+
+
+typedef bool (*ifpkt_cb_t)(void*, int);
+
+#ifdef BCMPKTPOOL
+#define POOL_ENAB(pool)		((pool) && (pool)->inited)
+#if defined(BCM4329C0)
+#define SHARED_POOL		(pktpool_shared_ptr)
+#else
+#define SHARED_POOL		(pktpool_shared)
+#endif 
+#else 
+#define POOL_ENAB(bus)		0
+#define SHARED_POOL		((struct pktpool *)NULL)
+#endif 
+
+#ifndef PKTPOOL_LEN_MAX
+#define PKTPOOL_LEN_MAX		40
+#endif
+#define PKTPOOL_CB_MAX		3
+
+struct pktpool;
+typedef void (*pktpool_cb_t)(struct pktpool *pool, void *arg);
+typedef struct {
+	pktpool_cb_t cb;
+	void *arg;
+} pktpool_cbinfo_t;
+
+#ifdef BCMDBG_POOL
+
+#define POOL_IDLE	0
+#define POOL_RXFILL	1
+#define POOL_RXDH	2
+#define POOL_RXD11	3
+#define POOL_TXDH	4
+#define POOL_TXD11	5
+#define POOL_AMPDU	6
+#define POOL_TXENQ	7
+
+typedef struct {
+	void *p;
+	uint32 cycles;
+	uint32 dur;
+} pktpool_dbg_t;
+
+typedef struct {
+	uint8 txdh;	
+	uint8 txd11;	
+	uint8 enq;	
+	uint8 rxdh;	
+	uint8 rxd11;	
+	uint8 rxfill;	
+	uint8 idle;	
+} pktpool_stats_t;
+#endif 
+
+typedef struct pktpool {
+	bool inited;
+	uint16 r;
+	uint16 w;
+	uint16 len;
+	uint16 maxlen;
+	uint16 plen;
+	bool istx;
+	bool empty;
+	uint8 cbtoggle;
+	uint8 cbcnt;
+	uint8 ecbcnt;
+	bool emptycb_disable;
+	pktpool_cbinfo_t cbs[PKTPOOL_CB_MAX];
+	pktpool_cbinfo_t ecbs[PKTPOOL_CB_MAX];
+	void *q[PKTPOOL_LEN_MAX + 1];
+
+#ifdef BCMDBG_POOL
+	uint8 dbg_cbcnt;
+	pktpool_cbinfo_t dbg_cbs[PKTPOOL_CB_MAX];
+	uint16 dbg_qlen;
+	pktpool_dbg_t dbg_q[PKTPOOL_LEN_MAX + 1];
+#endif
+} pktpool_t;
+
+#if defined(BCM4329C0)
+extern pktpool_t *pktpool_shared_ptr;
+#else
+extern pktpool_t *pktpool_shared;
+#endif 
+
+extern int pktpool_init(osl_t *osh, pktpool_t *pktp, int *pktplen, int plen, bool istx);
+extern int pktpool_deinit(osl_t *osh, pktpool_t *pktp);
+extern int pktpool_fill(osl_t *osh, pktpool_t *pktp, bool minimal);
+extern void* pktpool_get(pktpool_t *pktp);
+extern void pktpool_free(pktpool_t *pktp, void *p);
+extern int pktpool_add(pktpool_t *pktp, void *p);
+extern uint16 pktpool_avail(pktpool_t *pktp);
+extern int pktpool_avail_register(pktpool_t *pktp, pktpool_cb_t cb, void *arg);
+extern int pktpool_empty_register(pktpool_t *pktp, pktpool_cb_t cb, void *arg);
+extern int pktpool_setmaxlen(pktpool_t *pktp, uint16 maxlen);
+extern int pktpool_setmaxlen_strict(osl_t *osh, pktpool_t *pktp, uint16 maxlen);
+extern void pktpool_emptycb_disable(pktpool_t *pktp, bool disable);
+extern bool pktpool_emptycb_disabled(pktpool_t *pktp);
+
+#define POOLPTR(pp)			((pktpool_t *)(pp))
+#define pktpool_len(pp)			(POOLPTR(pp)->len - 1)
+#define pktpool_plen(pp)		(POOLPTR(pp)->plen)
+#define pktpool_maxlen(pp)		(POOLPTR(pp)->maxlen)
+
+#ifdef BCMDBG_POOL
+extern int pktpool_dbg_register(pktpool_t *pktp, pktpool_cb_t cb, void *arg);
+extern int pktpool_start_trigger(pktpool_t *pktp, void *p);
+extern int pktpool_dbg_dump(pktpool_t *pktp);
+extern int pktpool_dbg_notify(pktpool_t *pktp);
+extern int pktpool_stats_dump(pktpool_t *pktp, pktpool_stats_t *stats);
+#endif 
+
+
+
+struct ether_addr;
+
+extern int ether_isbcast(const void *ea);
+extern int ether_isnulladdr(const void *ea);
+
+
+
+#define pktq_psetmax(pq, prec, _max)    ((pq)->q[prec].max = (_max))
+#define pktq_plen(pq, prec)             ((pq)->q[prec].len)
+#define pktq_pavail(pq, prec)           ((pq)->q[prec].max - (pq)->q[prec].len)
+#define pktq_pfull(pq, prec)            ((pq)->q[prec].len >= (pq)->q[prec].max)
+#define pktq_pempty(pq, prec)           ((pq)->q[prec].len == 0)
+
+#define pktq_ppeek(pq, prec)            ((pq)->q[prec].head)
+#define pktq_ppeek_tail(pq, prec)       ((pq)->q[prec].tail)
+
+extern void *pktq_penq(struct pktq *pq, int prec, void *p);
+extern void *pktq_penq_head(struct pktq *pq, int prec, void *p);
+extern void *pktq_pdeq(struct pktq *pq, int prec);
+extern void *pktq_pdeq_tail(struct pktq *pq, int prec);
+
+extern void pktq_pflush(osl_t *osh, struct pktq *pq, int prec, bool dir,
+	ifpkt_cb_t fn, int arg);
+
+extern bool pktq_pdel(struct pktq *pq, void *p, int prec);
+
+
+
+extern int pktq_mlen(struct pktq *pq, uint prec_bmp);
+extern void *pktq_mdeq(struct pktq *pq, uint prec_bmp, int *prec_out);
+
+
+
+#define pktq_len(pq)                    ((int)(pq)->len)
+#define pktq_max(pq)                    ((int)(pq)->max)
+#define pktq_avail(pq)                  ((int)((pq)->max - (pq)->len))
+#define pktq_full(pq)                   ((pq)->len >= (pq)->max)
+#define pktq_empty(pq)                  ((pq)->len == 0)
+
+
+#define pktenq(pq, p)		pktq_penq(((struct pktq *)pq), 0, (p))
+#define pktenq_head(pq, p)	pktq_penq_head(((struct pktq *)pq), 0, (p))
+#define pktdeq(pq)		pktq_pdeq(((struct pktq *)pq), 0)
+#define pktdeq_tail(pq)		pktq_pdeq_tail(((struct pktq *)pq), 0)
+#define pktqinit(pq, len) pktq_init(((struct pktq *)pq), 1, len)
+
+extern void pktq_init(struct pktq *pq, int num_prec, int max_len);
+
+extern void *pktq_deq(struct pktq *pq, int *prec_out);
+extern void *pktq_deq_tail(struct pktq *pq, int *prec_out);
+extern void *pktq_peek(struct pktq *pq, int *prec_out);
+extern void *pktq_peek_tail(struct pktq *pq, int *prec_out);
+extern void pktq_flush(osl_t *osh, struct pktq *pq, bool dir, ifpkt_cb_t fn, int arg);
+
+
+
+extern uint pktcopy(osl_t *osh, void *p, uint offset, int len, uchar *buf);
+extern uint pktfrombuf(osl_t *osh, void *p, uint offset, int len, uchar *buf);
+extern uint pkttotlen(osl_t *osh, void *p);
+extern void *pktlast(osl_t *osh, void *p);
+extern uint pktsegcnt(osl_t *osh, void *p);
+
+
+extern uint pktsetprio(void *pkt, bool update_vtag);
+#define	PKTPRIO_VDSCP	0x100		
+#define	PKTPRIO_VLAN	0x200		
+#define	PKTPRIO_UPD	0x400		
+#define	PKTPRIO_DSCP	0x800		
+
+
+extern int bcm_atoi(char *s);
+extern ulong bcm_strtoul(char *cp, char **endp, uint base);
+extern char *bcmstrstr(char *haystack, char *needle);
+extern char *bcmstrcat(char *dest, const char *src);
+extern char *bcmstrncat(char *dest, const char *src, uint size);
+extern ulong wchar2ascii(char *abuf, ushort *wbuf, ushort wbuflen, ulong abuflen);
+char* bcmstrtok(char **string, const char *delimiters, char *tokdelim);
+int bcmstricmp(const char *s1, const char *s2);
+int bcmstrnicmp(const char* s1, const char* s2, int cnt);
+
+
+
+extern char *bcm_ether_ntoa(const struct ether_addr *ea, char *buf);
+extern int bcm_ether_atoe(char *p, struct ether_addr *ea);
+
+
+struct ipv4_addr;
+extern char *bcm_ip_ntoa(struct ipv4_addr *ia, char *buf);
+
+
+extern void bcm_mdelay(uint ms);
+
+#define NVRAM_RECLAIM_CHECK(name)
+
+extern char *getvar(char *vars, const char *name);
+extern int getintvar(char *vars, const char *name);
+extern int getintvararray(char *vars, const char *name, int index);
+extern int getintvararraysize(char *vars, const char *name);
+extern uint getgpiopin(char *vars, char *pin_name, uint def_pin);
+#define bcm_perf_enable()
+#define bcmstats(fmt)
+#define	bcmlog(fmt, a1, a2)
+#define	bcmdumplog(buf, size)	*buf = '\0'
+#define	bcmdumplogent(buf, idx)	-1
+
+#define bcmtslog(tstamp, fmt, a1, a2)
+#define bcmprinttslogs()
+#define bcmprinttstamp(us)
+
+extern char *bcm_nvram_vars(uint *length);
+extern int bcm_nvram_cache(void *sih);
+
+
+
+
+typedef struct bcm_iovar {
+	const char *name;	
+	uint16 varid;		
+	uint16 flags;		
+	uint16 type;		
+	uint16 minlen;		
+} bcm_iovar_t;
+
+
+
+
+#define IOV_GET 0 
+#define IOV_SET 1 
+
+
+#define IOV_GVAL(id)		((id)*2)
+#define IOV_SVAL(id)		(((id)*2)+IOV_SET)
+#define IOV_ISSET(actionid)	((actionid & IOV_SET) == IOV_SET)
+#define IOV_ID(actionid)	(actionid >> 1)
+
+
+
+extern const bcm_iovar_t *bcm_iovar_lookup(const bcm_iovar_t *table, const char *name);
+extern int bcm_iovar_lencheck(const bcm_iovar_t *table, void *arg, int len, bool set);
+#if defined(WLTINYDUMP) || defined(WLMSG_INFORM) || defined(WLMSG_ASSOC) || \
+	defined(WLMSG_PRPKT) || defined(WLMSG_WSEC)
+extern int bcm_format_ssid(char* buf, const uchar ssid[], uint ssid_len);
+#endif 
+#endif	
+
+
+#define IOVT_VOID	0	
+#define IOVT_BOOL	1	
+#define IOVT_INT8	2	
+#define IOVT_UINT8	3	
+#define IOVT_INT16	4	
+#define IOVT_UINT16	5	
+#define IOVT_INT32	6	
+#define IOVT_UINT32	7	
+#define IOVT_BUFFER	8	
+#define BCM_IOVT_VALID(type) (((unsigned int)(type)) <= IOVT_BUFFER)
+
+
+#define BCM_IOV_TYPE_INIT { \
+	"void", \
+	"bool", \
+	"int8", \
+	"uint8", \
+	"int16", \
+	"uint16", \
+	"int32", \
+	"uint32", \
+	"buffer", \
+	"" }
+
+#define BCM_IOVT_IS_INT(type) (\
+	(type == IOVT_BOOL) || \
+	(type == IOVT_INT8) || \
+	(type == IOVT_UINT8) || \
+	(type == IOVT_INT16) || \
+	(type == IOVT_UINT16) || \
+	(type == IOVT_INT32) || \
+	(type == IOVT_UINT32))
+
+
+
+#define BCME_STRLEN 		64	
+#define VALID_BCMERROR(e)  ((e <= 0) && (e >= BCME_LAST))
+
+
+
+
+#define BCME_OK				0	
+#define BCME_ERROR			-1	
+#define BCME_BADARG			-2	
+#define BCME_BADOPTION			-3	
+#define BCME_NOTUP			-4	
+#define BCME_NOTDOWN			-5	
+#define BCME_NOTAP			-6	
+#define BCME_NOTSTA			-7	
+#define BCME_BADKEYIDX			-8	
+#define BCME_RADIOOFF 			-9	
+#define BCME_NOTBANDLOCKED		-10	
+#define BCME_NOCLK			-11	
+#define BCME_BADRATESET			-12	
+#define BCME_BADBAND			-13	
+#define BCME_BUFTOOSHORT		-14	
+#define BCME_BUFTOOLONG			-15	
+#define BCME_BUSY			-16	
+#define BCME_NOTASSOCIATED		-17	
+#define BCME_BADSSIDLEN			-18	
+#define BCME_OUTOFRANGECHAN		-19	
+#define BCME_BADCHAN			-20	
+#define BCME_BADADDR			-21	
+#define BCME_NORESOURCE			-22	
+#define BCME_UNSUPPORTED		-23	
+#define BCME_BADLEN			-24	
+#define BCME_NOTREADY			-25	
+#define BCME_EPERM			-26	
+#define BCME_NOMEM			-27	
+#define BCME_ASSOCIATED			-28	
+#define BCME_RANGE			-29	
+#define BCME_NOTFOUND			-30	
+#define BCME_WME_NOT_ENABLED		-31	
+#define BCME_TSPEC_NOTFOUND		-32	
+#define BCME_ACM_NOTSUPPORTED		-33	
+#define BCME_NOT_WME_ASSOCIATION	-34	
+#define BCME_SDIO_ERROR			-35	
+#define BCME_DONGLE_DOWN		-36	
+#define BCME_VERSION			-37 	
+#define BCME_TXFAIL			-38 	
+#define BCME_RXFAIL			-39	
+#define BCME_NODEVICE			-40 	
+#define BCME_NMODE_DISABLED		-41 	
+#define BCME_NONRESIDENT		-42 
+#define BCME_LAST			BCME_NONRESIDENT
+
+
+#define BCMERRSTRINGTABLE {		\
+	"OK",				\
+	"Undefined error",		\
+	"Bad Argument",			\
+	"Bad Option",			\
+	"Not up",			\
+	"Not down",			\
+	"Not AP",			\
+	"Not STA",			\
+	"Bad Key Index",		\
+	"Radio Off",			\
+	"Not band locked",		\
+	"No clock",			\
+	"Bad Rate valueset",		\
+	"Bad Band",			\
+	"Buffer too short",		\
+	"Buffer too long",		\
+	"Busy",				\
+	"Not Associated",		\
+	"Bad SSID len",			\
+	"Out of Range Channel",		\
+	"Bad Channel",			\
+	"Bad Address",			\
+	"Not Enough Resources",		\
+	"Unsupported",			\
+	"Bad length",			\
+	"Not Ready",			\
+	"Not Permitted",		\
+	"No Memory",			\
+	"Associated",			\
+	"Not In Range",			\
+	"Not Found",			\
+	"WME Not Enabled",		\
+	"TSPEC Not Found",		\
+	"ACM Not Supported",		\
+	"Not WME Association",		\
+	"SDIO Bus Error",		\
+	"Dongle Not Accessible",	\
+	"Incorrect version",		\
+	"TX Failure",			\
+	"RX Failure",			\
+	"Device Not Present",		\
+	"NMODE Disabled",		\
+	"Nonresident overlay access", \
+}
+
+#ifndef ABS
+#define	ABS(a)			(((a) < 0)?-(a):(a))
+#endif 
+
+#ifndef MIN
+#define	MIN(a, b)		(((a) < (b))?(a):(b))
+#endif 
+
+#ifndef MAX
+#define	MAX(a, b)		(((a) > (b))?(a):(b))
+#endif 
+
+#define CEIL(x, y)		(((x) + ((y)-1)) / (y))
+#define	ROUNDUP(x, y)		((((x)+((y)-1))/(y))*(y))
+#define	ISALIGNED(a, x)		(((uintptr)(a) & ((x)-1)) == 0)
+#define ALIGN_ADDR(addr, boundary) (void *)(((uintptr)(addr) + (boundary) - 1) \
+	                                         & ~((boundary) - 1))
+#define	ISPOWEROF2(x)		((((x)-1)&(x)) == 0)
+#define VALID_MASK(mask)	!((mask) & ((mask) + 1))
+
+#ifndef OFFSETOF
+#ifdef __ARMCC_VERSION
+
+#include <stddef.h>
+#define	OFFSETOF(type, member)	offsetof(type, member)
+#else
+#define	OFFSETOF(type, member)	((uint)(uintptr)&((type *)0)->member)
+#endif 
+#endif 
+
+#ifndef ARRAYSIZE
+#define ARRAYSIZE(a)		(sizeof(a)/sizeof(a[0]))
+#endif
+
+
+extern void *_bcmutils_dummy_fn;
+#define REFERENCE_FUNCTION(f)	(_bcmutils_dummy_fn = (void *)(f))
+
+
+#ifndef setbit
+#ifndef NBBY		      
+#define	NBBY	8	
+#endif 
+#define	setbit(a, i)	(((uint8 *)a)[(i)/NBBY] |= 1<<((i)%NBBY))
+#define	clrbit(a, i)	(((uint8 *)a)[(i)/NBBY] &= ~(1<<((i)%NBBY)))
+#define	isset(a, i)	(((const uint8 *)a)[(i)/NBBY] & (1<<((i)%NBBY)))
+#define	isclr(a, i)	((((const uint8 *)a)[(i)/NBBY] & (1<<((i)%NBBY))) == 0)
+#endif 
+
+#define	NBITS(type)	(sizeof(type) * 8)
+#define NBITVAL(nbits)	(1 << (nbits))
+#define MAXBITVAL(nbits)	((1 << (nbits)) - 1)
+#define	NBITMASK(nbits)	MAXBITVAL(nbits)
+#define MAXNBVAL(nbyte)	MAXBITVAL((nbyte) * 8)
+
+
+#define MUX(pred, true, false) ((pred) ? (true) : (false))
+
+
+#define MODDEC(x, bound) MUX((x) == 0, (bound) - 1, (x) - 1)
+#define MODINC(x, bound) MUX((x) == (bound) - 1, 0, (x) + 1)
+
+
+#define MODDEC_POW2(x, bound) (((x) - 1) & ((bound) - 1))
+#define MODINC_POW2(x, bound) (((x) + 1) & ((bound) - 1))
+
+
+#define MODADD(x, y, bound) \
+    MUX((x) + (y) >= (bound), (x) + (y) - (bound), (x) + (y))
+#define MODSUB(x, y, bound) \
+    MUX(((int)(x)) - ((int)(y)) < 0, (x) - (y) + (bound), (x) - (y))
+
+
+#define MODADD_POW2(x, y, bound) (((x) + (y)) & ((bound) - 1))
+#define MODSUB_POW2(x, y, bound) (((x) - (y)) & ((bound) - 1))
+
+
+#define CRC8_INIT_VALUE  0xff		
+#define CRC8_GOOD_VALUE  0x9f		
+#define CRC16_INIT_VALUE 0xffff		
+#define CRC16_GOOD_VALUE 0xf0b8		
+#define CRC32_INIT_VALUE 0xffffffff	
+#define CRC32_GOOD_VALUE 0xdebb20e3	
+
+
+typedef struct bcm_bit_desc {
+	uint32	bit;
+	const char* name;
+} bcm_bit_desc_t;
+
+
+typedef struct bcm_tlv {
+	uint8	id;
+	uint8	len;
+	uint8	data[1];
+} bcm_tlv_t;
+
+
+#define bcm_valid_tlv(elt, buflen) ((buflen) >= 2 && (int)(buflen) >= (int)(2 + (elt)->len))
+
+
+#define ETHER_ADDR_STR_LEN	18	
+
+
+
+static INLINE void
+xor_128bit_block(const uint8 *src1, const uint8 *src2, uint8 *dst)
+{
+	if (
+#ifdef __i386__
+	    1 ||
+#endif
+	    (((uintptr)src1 | (uintptr)src2 | (uintptr)dst) & 3) == 0) {
+		
+		
+		((uint32 *)dst)[0] = ((const uint32 *)src1)[0] ^ ((const uint32 *)src2)[0];
+		((uint32 *)dst)[1] = ((const uint32 *)src1)[1] ^ ((const uint32 *)src2)[1];
+		((uint32 *)dst)[2] = ((const uint32 *)src1)[2] ^ ((const uint32 *)src2)[2];
+		((uint32 *)dst)[3] = ((const uint32 *)src1)[3] ^ ((const uint32 *)src2)[3];
+	} else {
+		
+		int k;
+		for (k = 0; k < 16; k++)
+			dst[k] = src1[k] ^ src2[k];
+	}
+}
+
+
+
+extern uint8 hndcrc8(uint8 *p, uint nbytes, uint8 crc);
+extern uint16 hndcrc16(uint8 *p, uint nbytes, uint16 crc);
+extern uint32 hndcrc32(uint8 *p, uint nbytes, uint32 crc);
+
+#if defined(DHD_DEBUG) || defined(WLMSG_PRHDRS) || defined(WLMSG_PRPKT) || \
+	defined(WLMSG_ASSOC)
+extern int bcm_format_flags(const bcm_bit_desc_t *bd, uint32 flags, char* buf, int len);
+#endif
+
+#if defined(DHD_DEBUG) || defined(WLMSG_PRHDRS) || defined(WLMSG_PRPKT) || \
+	defined(WLMSG_ASSOC) || defined(WLMEDIA_PEAKRATE)
+extern int bcm_format_hex(char *str, const void *bytes, int len);
+#endif
+
+extern const char *bcm_crypto_algo_name(uint algo);
+extern char *bcm_chipname(uint chipid, char *buf, uint len);
+extern char *bcm_brev_str(uint32 brev, char *buf);
+extern void printbig(char *buf);
+extern void prhex(const char *msg, uchar *buf, uint len);
+
+
+extern bcm_tlv_t *bcm_next_tlv(bcm_tlv_t *elt, int *buflen);
+extern bcm_tlv_t *bcm_parse_tlvs(void *buf, int buflen, uint key);
+extern bcm_tlv_t *bcm_parse_ordered_tlvs(void *buf, int buflen, uint key);
+
+
+extern const char *bcmerrorstr(int bcmerror);
+
+
+typedef uint32 mbool;
+#define mboolset(mb, bit)		((mb) |= (bit))		
+#define mboolclr(mb, bit)		((mb) &= ~(bit))	
+#define mboolisset(mb, bit)		(((mb) & (bit)) != 0)	
+#define	mboolmaskset(mb, mask, val)	((mb) = (((mb) & ~(mask)) | (val)))
+
+
+extern uint16 bcm_qdbm_to_mw(uint8 qdbm);
+extern uint8 bcm_mw_to_qdbm(uint16 mw);
+
+
+struct fielddesc {
+	const char *nameandfmt;
+	uint32 	offset;
+	uint32 	len;
+};
+
+extern void bcm_binit(struct bcmstrbuf *b, char *buf, uint size);
+extern int bcm_bprintf(struct bcmstrbuf *b, const char *fmt, ...);
+extern void bcm_inc_bytes(uchar *num, int num_bytes, uint8 amount);
+extern int bcm_cmp_bytes(uchar *arg1, uchar *arg2, uint8 nbytes);
+extern void bcm_print_bytes(char *name, const uchar *cdata, int len);
+
+typedef  uint32 (*bcmutl_rdreg_rtn)(void *arg0, uint arg1, uint32 offset);
+extern uint bcmdumpfields(bcmutl_rdreg_rtn func_ptr, void *arg0, uint arg1, struct fielddesc *str,
+                          char *buf, uint32 bufsize);
+
+extern uint bcm_mkiovar(char *name, char *data, uint datalen, char *buf, uint len);
+extern uint bcm_bitcount(uint8 *bitmap, uint bytelength);
+
+
+
+#define SSID_FMT_BUF_LEN	((4 * DOT11_MAX_SSID_LEN) + 1)
+
+unsigned int process_nvram_vars(char *varbuf, unsigned int len);
+
+#ifdef __cplusplus
+	}
+#endif
+
+#endif	
diff --git a/drivers/net/wireless/bcmdhd/include/bcmwifi.h b/drivers/net/wireless/bcmdhd/include/bcmwifi.h
new file mode 100644
index 000000000000..e5207e9c4086
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/bcmwifi.h
@@ -0,0 +1,165 @@
+/*
+ * Misc utility routines for WL and Apps
+ * This header file housing the define and function prototype use by
+ * both the wl driver, tools & Apps.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmwifi.h 277737 2011-08-16 17:54:59Z $
+ */
+
+
+#ifndef	_bcmwifi_h_
+#define	_bcmwifi_h_
+
+
+
+typedef uint16 chanspec_t;
+
+
+#define CH_UPPER_SB			0x01
+#define CH_LOWER_SB			0x02
+#define CH_EWA_VALID			0x04
+#define CH_20MHZ_APART			4
+#define CH_10MHZ_APART			2
+#define CH_5MHZ_APART			1	
+#define CH_MAX_2G_CHANNEL		14	
+#define WLC_MAX_2G_CHANNEL		CH_MAX_2G_CHANNEL 
+#define	MAXCHANNEL		224	
+
+#define WL_CHANSPEC_CHAN_MASK		0x00ff
+#define WL_CHANSPEC_CHAN_SHIFT		0
+
+#define WL_CHANSPEC_CTL_SB_MASK		0x0300
+#define WL_CHANSPEC_CTL_SB_SHIFT	     8
+#define WL_CHANSPEC_CTL_SB_LOWER	0x0100
+#define WL_CHANSPEC_CTL_SB_UPPER	0x0200
+#define WL_CHANSPEC_CTL_SB_NONE		0x0300
+
+#define WL_CHANSPEC_BW_MASK		0x0C00
+#define WL_CHANSPEC_BW_SHIFT		    10
+#define WL_CHANSPEC_BW_10		0x0400
+#define WL_CHANSPEC_BW_20		0x0800
+#define WL_CHANSPEC_BW_40		0x0C00
+
+#define WL_CHANSPEC_BAND_MASK		0xf000
+#define WL_CHANSPEC_BAND_SHIFT		12
+#define WL_CHANSPEC_BAND_5G		0x1000
+#define WL_CHANSPEC_BAND_2G		0x2000
+#define INVCHANSPEC			255
+
+
+#define WF_CHAN_FACTOR_2_4_G		4814	
+#define WF_CHAN_FACTOR_5_G		10000	
+#define WF_CHAN_FACTOR_4_G		8000	
+
+
+#define LOWER_20_SB(channel)	(((channel) > CH_10MHZ_APART) ? ((channel) - CH_10MHZ_APART) : 0)
+#define UPPER_20_SB(channel)	(((channel) < (MAXCHANNEL - CH_10MHZ_APART)) ? \
+				((channel) + CH_10MHZ_APART) : 0)
+#define CHSPEC_WLCBANDUNIT(chspec)	(CHSPEC_IS5G(chspec) ? BAND_5G_INDEX : BAND_2G_INDEX)
+#define CH20MHZ_CHSPEC(channel)	(chanspec_t)((chanspec_t)(channel) | WL_CHANSPEC_BW_20 | \
+				WL_CHANSPEC_CTL_SB_NONE | (((channel) <= CH_MAX_2G_CHANNEL) ? \
+				WL_CHANSPEC_BAND_2G : WL_CHANSPEC_BAND_5G))
+#define NEXT_20MHZ_CHAN(channel)	(((channel) < (MAXCHANNEL - CH_20MHZ_APART)) ? \
+					((channel) + CH_20MHZ_APART) : 0)
+#define CH40MHZ_CHSPEC(channel, ctlsb)	(chanspec_t) \
+					((channel) | (ctlsb) | WL_CHANSPEC_BW_40 | \
+					((channel) <= CH_MAX_2G_CHANNEL ? WL_CHANSPEC_BAND_2G : \
+					WL_CHANSPEC_BAND_5G))
+#define CHSPEC_CHANNEL(chspec)	((uint8)((chspec) & WL_CHANSPEC_CHAN_MASK))
+#define CHSPEC_BAND(chspec)	((chspec) & WL_CHANSPEC_BAND_MASK)
+
+
+#define CHSPEC_CTL_SB(chspec)  (chspec & WL_CHANSPEC_CTL_SB_MASK)
+#define CHSPEC_BW(chspec)      (chspec & WL_CHANSPEC_BW_MASK)
+
+#ifdef WL11N_20MHZONLY
+
+#define CHSPEC_IS10(chspec)	0
+#define CHSPEC_IS20(chspec)	1
+#ifndef CHSPEC_IS40
+#define CHSPEC_IS40(chspec)	0
+#endif
+
+#else 
+
+#define CHSPEC_IS10(chspec)	(((chspec) & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_10)
+#define CHSPEC_IS20(chspec)	(((chspec) & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_20)
+#ifndef CHSPEC_IS40
+#define CHSPEC_IS40(chspec)	(((chspec) & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_40)
+#endif
+
+#endif 
+
+#define CHSPEC_IS20_UNCOND(chspec)	(((chspec) & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_20)
+
+#define CHSPEC_IS5G(chspec)	(((chspec) & WL_CHANSPEC_BAND_MASK) == WL_CHANSPEC_BAND_5G)
+#define CHSPEC_IS2G(chspec)	(((chspec) & WL_CHANSPEC_BAND_MASK) == WL_CHANSPEC_BAND_2G)
+#define CHSPEC_SB_NONE(chspec)	(((chspec) & WL_CHANSPEC_CTL_SB_MASK) == WL_CHANSPEC_CTL_SB_NONE)
+#define CHSPEC_SB_UPPER(chspec)	(((chspec) & WL_CHANSPEC_CTL_SB_MASK) == WL_CHANSPEC_CTL_SB_UPPER)
+#define CHSPEC_SB_LOWER(chspec)	(((chspec) & WL_CHANSPEC_CTL_SB_MASK) == WL_CHANSPEC_CTL_SB_LOWER)
+#define CHSPEC_CTL_CHAN(chspec)  ((CHSPEC_SB_LOWER(chspec)) ? \
+				  (LOWER_20_SB(((chspec) & WL_CHANSPEC_CHAN_MASK))) : \
+				  (UPPER_20_SB(((chspec) & WL_CHANSPEC_CHAN_MASK))))
+#define CHSPEC2WLC_BAND(chspec) (CHSPEC_IS5G(chspec) ? WLC_BAND_5G : WLC_BAND_2G)
+
+#define CHANSPEC_STR_LEN    8
+
+
+#define WLC_MAXRATE	108	
+#define WLC_RATE_1M	2	
+#define WLC_RATE_2M	4	
+#define WLC_RATE_5M5	11	
+#define WLC_RATE_11M	22	
+#define WLC_RATE_6M	12	
+#define WLC_RATE_9M	18	
+#define WLC_RATE_12M	24	
+#define WLC_RATE_18M	36	
+#define WLC_RATE_24M	48	
+#define WLC_RATE_36M	72	
+#define WLC_RATE_48M	96	
+#define WLC_RATE_54M	108	
+
+#define WLC_2G_25MHZ_OFFSET		5	
+
+
+extern char * wf_chspec_ntoa(chanspec_t chspec, char *buf);
+
+
+extern chanspec_t wf_chspec_aton(char *a);
+
+
+extern bool wf_chspec_malformed(chanspec_t chanspec);
+
+
+extern uint8 wf_chspec_ctlchan(chanspec_t chspec);
+
+
+extern chanspec_t wf_chspec_ctlchspec(chanspec_t chspec);
+
+
+extern int wf_mhz2channel(uint freq, uint start_factor);
+
+
+extern int wf_channel2mhz(uint channel, uint start_factor);
+
+#endif	
diff --git a/drivers/net/wireless/bcmdhd/include/dhdioctl.h b/drivers/net/wireless/bcmdhd/include/dhdioctl.h
new file mode 100644
index 000000000000..175ff8545a0c
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/dhdioctl.h
@@ -0,0 +1,131 @@
+/*
+ * Definitions for ioctls to access DHD iovars.
+ * Based on wlioctl.h (for Broadcom 802.11abg driver).
+ * (Moves towards generic ioctls for BCM drivers/iovars.)
+ *
+ * Definitions subject to change without notice.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: dhdioctl.h 323572 2012-03-26 06:28:14Z $
+ */
+
+#ifndef _dhdioctl_h_
+#define	_dhdioctl_h_
+
+#include <typedefs.h>
+
+
+/* require default structure packing */
+#define BWL_DEFAULT_PACKING
+#include <packed_section_start.h>
+
+
+/* Linux network driver ioctl encoding */
+typedef struct dhd_ioctl {
+	uint cmd;	/* common ioctl definition */
+	void *buf;	/* pointer to user buffer */
+	uint len;	/* length of user buffer */
+	bool set;	/* get or set request (optional) */
+	uint used;	/* bytes read or written (optional) */
+	uint needed;	/* bytes needed (optional) */
+	uint driver;	/* to identify target driver */
+} dhd_ioctl_t;
+
+/* Underlying BUS definition */
+enum {
+	BUS_TYPE_USB = 0, /* for USB dongles */
+	BUS_TYPE_SDIO /* for SDIO dongles */
+};
+
+/* per-driver magic numbers */
+#define DHD_IOCTL_MAGIC		0x00444944
+
+/* bump this number if you change the ioctl interface */
+#define DHD_IOCTL_VERSION	1
+
+#define	DHD_IOCTL_MAXLEN	8192		/* max length ioctl buffer required */
+#define	DHD_IOCTL_SMLEN		256		/* "small" length ioctl buffer required */
+
+/* common ioctl definitions */
+#define DHD_GET_MAGIC				0
+#define DHD_GET_VERSION				1
+#define DHD_GET_VAR				2
+#define DHD_SET_VAR				3
+
+/* message levels */
+#define DHD_ERROR_VAL	0x0001
+#define DHD_TRACE_VAL	0x0002
+#define DHD_INFO_VAL	0x0004
+#define DHD_DATA_VAL	0x0008
+#define DHD_CTL_VAL	0x0010
+#define DHD_TIMER_VAL	0x0020
+#define DHD_HDRS_VAL	0x0040
+#define DHD_BYTES_VAL	0x0080
+#define DHD_INTR_VAL	0x0100
+#define DHD_LOG_VAL	0x0200
+#define DHD_GLOM_VAL	0x0400
+#define DHD_EVENT_VAL	0x0800
+#define DHD_BTA_VAL	0x1000
+#define DHD_ISCAN_VAL	0x2000
+#define DHD_ARPOE_VAL	0x4000
+#define DHD_REORDER_VAL 0x8000
+#define DHD_WL_VAL	0x10000
+
+#ifdef SDTEST
+/* For pktgen iovar */
+typedef struct dhd_pktgen {
+	uint version;		/* To allow structure change tracking */
+	uint freq;		/* Max ticks between tx/rx attempts */
+	uint count;		/* Test packets to send/rcv each attempt */
+	uint print;		/* Print counts every <print> attempts */
+	uint total;		/* Total packets (or bursts) */
+	uint minlen;		/* Minimum length of packets to send */
+	uint maxlen;		/* Maximum length of packets to send */
+	uint numsent;		/* Count of test packets sent */
+	uint numrcvd;		/* Count of test packets received */
+	uint numfail;		/* Count of test send failures */
+	uint mode;		/* Test mode (type of test packets) */
+	uint stop;		/* Stop after this many tx failures */
+} dhd_pktgen_t;
+
+/* Version in case structure changes */
+#define DHD_PKTGEN_VERSION 2
+
+/* Type of test packets to use */
+#define DHD_PKTGEN_ECHO		1 /* Send echo requests */
+#define DHD_PKTGEN_SEND 	2 /* Send discard packets */
+#define DHD_PKTGEN_RXBURST	3 /* Request dongle send N packets */
+#define DHD_PKTGEN_RECV		4 /* Continuous rx from continuous tx dongle */
+#endif /* SDTEST */
+
+/* Enter idle immediately (no timeout) */
+#define DHD_IDLE_IMMEDIATE	(-1)
+
+/* Values for idleclock iovar: other values are the sd_divisor to use when idle */
+#define DHD_IDLE_ACTIVE	0	/* Do not request any SD clock change when idle */
+#define DHD_IDLE_STOP   (-1)	/* Request SD clock be stopped (and use SD1 mode) */
+
+
+/* require default structure packing */
+#include <packed_section_end.h>
+
+#endif /* _dhdioctl_h_ */
diff --git a/drivers/net/wireless/bcmdhd/include/epivers.h b/drivers/net/wireless/bcmdhd/include/epivers.h
new file mode 100644
index 000000000000..5df25c16b7d9
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/epivers.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: epivers.h.in 277737 2011-08-16 17:54:59Z $
+ *
+*/
+
+
+#ifndef _epivers_h_
+#define _epivers_h_
+
+#define	EPI_MAJOR_VERSION	5
+
+#define	EPI_MINOR_VERSION	90
+
+#define	EPI_RC_NUMBER		195
+
+#define	EPI_INCREMENTAL_NUMBER	75
+
+#define	EPI_BUILD_NUMBER	0
+
+#define	EPI_VERSION		5, 90, 195, 75
+
+#define	EPI_VERSION_NUM		0x055ac34b
+
+#define EPI_VERSION_DEV		5.90.195
+
+
+#define	EPI_VERSION_STR		"5.90.195.75"
+
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/hndpmu.h b/drivers/net/wireless/bcmdhd/include/hndpmu.h
new file mode 100644
index 000000000000..9bfc8c9275a9
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/hndpmu.h
@@ -0,0 +1,37 @@
+/*
+ * HND SiliconBackplane PMU support.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: hndpmu.h 335486 2012-05-28 09:47:55Z $
+ */
+
+#ifndef _hndpmu_h_
+#define _hndpmu_h_
+
+
+extern void si_pmu_otp_power(si_t *sih, osl_t *osh, bool on);
+extern void si_sdiod_drive_strength_init(si_t *sih, osl_t *osh, uint32 drivestrength);
+
+extern void si_pmu_set_otp_wr_volts(si_t *sih);
+extern void si_pmu_set_otp_rd_volts(si_t *sih);
+
+#endif /* _hndpmu_h_ */
diff --git a/drivers/net/wireless/bcmdhd/include/hndrte_armtrap.h b/drivers/net/wireless/bcmdhd/include/hndrte_armtrap.h
new file mode 100644
index 000000000000..7d862c4deb21
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/hndrte_armtrap.h
@@ -0,0 +1,88 @@
+/*
+ * HNDRTE arm trap handling.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: hndrte_armtrap.h 277737 2011-08-16 17:54:59Z $
+ */
+
+#ifndef	_hndrte_armtrap_h
+#define	_hndrte_armtrap_h
+
+
+/* ARM trap handling */
+
+/* Trap types defined by ARM (see arminc.h) */
+
+/* Trap locations in lo memory */
+#define	TRAP_STRIDE	4
+#define FIRST_TRAP	TR_RST
+#define LAST_TRAP	(TR_FIQ * TRAP_STRIDE)
+
+#if defined(__ARM_ARCH_4T__)
+#define	MAX_TRAP_TYPE	(TR_FIQ + 1)
+#elif defined(__ARM_ARCH_7M__)
+#define	MAX_TRAP_TYPE	(TR_ISR + ARMCM3_NUMINTS)
+#endif	/* __ARM_ARCH_7M__ */
+
+/* The trap structure is defined here as offsets for assembly */
+#define	TR_TYPE		0x00
+#define	TR_EPC		0x04
+#define	TR_CPSR		0x08
+#define	TR_SPSR		0x0c
+#define	TR_REGS		0x10
+#define	TR_REG(n)	(TR_REGS + (n) * 4)
+#define	TR_SP		TR_REG(13)
+#define	TR_LR		TR_REG(14)
+#define	TR_PC		TR_REG(15)
+
+#define	TRAP_T_SIZE	80
+
+#ifndef	_LANGUAGE_ASSEMBLY
+
+#include <typedefs.h>
+
+typedef struct _trap_struct {
+	uint32		type;
+	uint32		epc;
+	uint32		cpsr;
+	uint32		spsr;
+	uint32		r0;
+	uint32		r1;
+	uint32		r2;
+	uint32		r3;
+	uint32		r4;
+	uint32		r5;
+	uint32		r6;
+	uint32		r7;
+	uint32		r8;
+	uint32		r9;
+	uint32		r10;
+	uint32		r11;
+	uint32		r12;
+	uint32		r13;
+	uint32		r14;
+	uint32		pc;
+} trap_t;
+
+#endif	/* !_LANGUAGE_ASSEMBLY */
+
+#endif	/* _hndrte_armtrap_h */
diff --git a/drivers/net/wireless/bcmdhd/include/hndrte_cons.h b/drivers/net/wireless/bcmdhd/include/hndrte_cons.h
new file mode 100644
index 000000000000..859ddc8953a8
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/hndrte_cons.h
@@ -0,0 +1,68 @@
+/*
+ * Console support for hndrte.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: hndrte_cons.h 277737 2011-08-16 17:54:59Z $
+ */
+
+#ifndef	_HNDRTE_CONS_H
+#define	_HNDRTE_CONS_H
+
+#include <typedefs.h>
+
+#define CBUF_LEN	(128)
+
+#define LOG_BUF_LEN	1024
+
+typedef struct {
+	uint32		buf;		/* Can't be pointer on (64-bit) hosts */
+	uint		buf_size;
+	uint		idx;
+	char		*_buf_compat;	/* redundant pointer for backward compat. */
+} hndrte_log_t;
+
+typedef struct {
+	/* Virtual UART
+	 *   When there is no UART (e.g. Quickturn), the host should write a complete
+	 *   input line directly into cbuf and then write the length into vcons_in.
+	 *   This may also be used when there is a real UART (at risk of conflicting with
+	 *   the real UART).  vcons_out is currently unused.
+	 */
+	volatile uint	vcons_in;
+	volatile uint	vcons_out;
+
+	/* Output (logging) buffer
+	 *   Console output is written to a ring buffer log_buf at index log_idx.
+	 *   The host may read the output when it sees log_idx advance.
+	 *   Output will be lost if the output wraps around faster than the host polls.
+	 */
+	hndrte_log_t	log;
+
+	/* Console input line buffer
+	 *   Characters are read one at a time into cbuf until <CR> is received, then
+	 *   the buffer is processed as a command line.  Also used for virtual UART.
+	 */
+	uint		cbuf_idx;
+	char		cbuf[CBUF_LEN];
+} hndrte_cons_t;
+
+#endif /* _HNDRTE_CONS_H */
diff --git a/drivers/net/wireless/bcmdhd/include/hndsoc.h b/drivers/net/wireless/bcmdhd/include/hndsoc.h
new file mode 100644
index 000000000000..34f927c6af80
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/hndsoc.h
@@ -0,0 +1,207 @@
+/*
+ * Broadcom HND chip & on-chip-interconnect-related definitions.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: hndsoc.h 277737 2011-08-16 17:54:59Z $
+ */
+
+#ifndef	_HNDSOC_H
+#define	_HNDSOC_H
+
+/* Include the soci specific files */
+#include <sbconfig.h>
+#include <aidmp.h>
+
+/*
+ * SOC Interconnect Address Map.
+ * All regions may not exist on all chips.
+ */
+#define SI_SDRAM_BASE		0x00000000	/* Physical SDRAM */
+#define SI_PCI_MEM		0x08000000	/* Host Mode sb2pcitranslation0 (64 MB) */
+#define SI_PCI_MEM_SZ		(64 * 1024 * 1024)
+#define SI_PCI_CFG		0x0c000000	/* Host Mode sb2pcitranslation1 (64 MB) */
+#define	SI_SDRAM_SWAPPED	0x10000000	/* Byteswapped Physical SDRAM */
+#define SI_SDRAM_R2		0x80000000	/* Region 2 for sdram (512 MB) */
+
+#define SI_ENUM_BASE    	0x18000000	/* Enumeration space base */
+
+#define SI_WRAP_BASE    	0x18100000	/* Wrapper space base */
+#define SI_CORE_SIZE    	0x1000		/* each core gets 4Kbytes for registers */
+#define	SI_MAXCORES		16		/* Max cores (this is arbitrary, for software
+						 * convenience and could be changed if we
+						 * make any larger chips
+						 */
+
+#define	SI_FASTRAM		0x19000000	/* On-chip RAM on chips that also have DDR */
+#define	SI_FASTRAM_SWAPPED	0x19800000
+
+#define	SI_FLASH2		0x1c000000	/* Flash Region 2 (region 1 shadowed here) */
+#define	SI_FLASH2_SZ		0x02000000	/* Size of Flash Region 2 */
+#define	SI_ARMCM3_ROM		0x1e000000	/* ARM Cortex-M3 ROM */
+#define	SI_FLASH1		0x1fc00000	/* MIPS Flash Region 1 */
+#define	SI_FLASH1_SZ		0x00400000	/* MIPS Size of Flash Region 1 */
+#define	SI_ARM7S_ROM		0x20000000	/* ARM7TDMI-S ROM */
+#define	SI_ARMCM3_SRAM2		0x60000000	/* ARM Cortex-M3 SRAM Region 2 */
+#define	SI_ARM7S_SRAM2		0x80000000	/* ARM7TDMI-S SRAM Region 2 */
+#define	SI_ARM_FLASH1		0xffff0000	/* ARM Flash Region 1 */
+#define	SI_ARM_FLASH1_SZ	0x00010000	/* ARM Size of Flash Region 1 */
+
+#define SI_PCI_DMA		0x40000000	/* Client Mode sb2pcitranslation2 (1 GB) */
+#define SI_PCI_DMA2		0x80000000	/* Client Mode sb2pcitranslation2 (1 GB) */
+#define SI_PCI_DMA_SZ		0x40000000	/* Client Mode sb2pcitranslation2 size in bytes */
+#define SI_PCIE_DMA_L32		0x00000000	/* PCIE Client Mode sb2pcitranslation2
+						 * (2 ZettaBytes), low 32 bits
+						 */
+#define SI_PCIE_DMA_H32		0x80000000	/* PCIE Client Mode sb2pcitranslation2
+						 * (2 ZettaBytes), high 32 bits
+						 */
+
+/* core codes */
+#define	NODEV_CORE_ID		0x700		/* Invalid coreid */
+#define	CC_CORE_ID		0x800		/* chipcommon core */
+#define	ILINE20_CORE_ID		0x801		/* iline20 core */
+#define	SRAM_CORE_ID		0x802		/* sram core */
+#define	SDRAM_CORE_ID		0x803		/* sdram core */
+#define	PCI_CORE_ID		0x804		/* pci core */
+#define	MIPS_CORE_ID		0x805		/* mips core */
+#define	ENET_CORE_ID		0x806		/* enet mac core */
+#define	CODEC_CORE_ID		0x807		/* v90 codec core */
+#define	USB_CORE_ID		0x808		/* usb 1.1 host/device core */
+#define	ADSL_CORE_ID		0x809		/* ADSL core */
+#define	ILINE100_CORE_ID	0x80a		/* iline100 core */
+#define	IPSEC_CORE_ID		0x80b		/* ipsec core */
+#define	UTOPIA_CORE_ID		0x80c		/* utopia core */
+#define	PCMCIA_CORE_ID		0x80d		/* pcmcia core */
+#define	SOCRAM_CORE_ID		0x80e		/* internal memory core */
+#define	MEMC_CORE_ID		0x80f		/* memc sdram core */
+#define	OFDM_CORE_ID		0x810		/* OFDM phy core */
+#define	EXTIF_CORE_ID		0x811		/* external interface core */
+#define	D11_CORE_ID		0x812		/* 802.11 MAC core */
+#define	APHY_CORE_ID		0x813		/* 802.11a phy core */
+#define	BPHY_CORE_ID		0x814		/* 802.11b phy core */
+#define	GPHY_CORE_ID		0x815		/* 802.11g phy core */
+#define	MIPS33_CORE_ID		0x816		/* mips3302 core */
+#define	USB11H_CORE_ID		0x817		/* usb 1.1 host core */
+#define	USB11D_CORE_ID		0x818		/* usb 1.1 device core */
+#define	USB20H_CORE_ID		0x819		/* usb 2.0 host core */
+#define	USB20D_CORE_ID		0x81a		/* usb 2.0 device core */
+#define	SDIOH_CORE_ID		0x81b		/* sdio host core */
+#define	ROBO_CORE_ID		0x81c		/* roboswitch core */
+#define	ATA100_CORE_ID		0x81d		/* parallel ATA core */
+#define	SATAXOR_CORE_ID		0x81e		/* serial ATA & XOR DMA core */
+#define	GIGETH_CORE_ID		0x81f		/* gigabit ethernet core */
+#define	PCIE_CORE_ID		0x820		/* pci express core */
+#define	NPHY_CORE_ID		0x821		/* 802.11n 2x2 phy core */
+#define	SRAMC_CORE_ID		0x822		/* SRAM controller core */
+#define	MINIMAC_CORE_ID		0x823		/* MINI MAC/phy core */
+#define	ARM11_CORE_ID		0x824		/* ARM 1176 core */
+#define	ARM7S_CORE_ID		0x825		/* ARM7tdmi-s core */
+#define	LPPHY_CORE_ID		0x826		/* 802.11a/b/g phy core */
+#define	PMU_CORE_ID		0x827		/* PMU core */
+#define	SSNPHY_CORE_ID		0x828		/* 802.11n single-stream phy core */
+#define	SDIOD_CORE_ID		0x829		/* SDIO device core */
+#define	ARMCM3_CORE_ID		0x82a		/* ARM Cortex M3 core */
+#define	HTPHY_CORE_ID		0x82b		/* 802.11n 4x4 phy core */
+#define	MIPS74K_CORE_ID		0x82c		/* mips 74k core */
+#define	GMAC_CORE_ID		0x82d		/* Gigabit MAC core */
+#define	DMEMC_CORE_ID		0x82e		/* DDR1/2 memory controller core */
+#define	PCIERC_CORE_ID		0x82f		/* PCIE Root Complex core */
+#define	OCP_CORE_ID		0x830		/* OCP2OCP bridge core */
+#define	SC_CORE_ID		0x831		/* shared common core */
+#define	AHB_CORE_ID		0x832		/* OCP2AHB bridge core */
+#define	SPIH_CORE_ID		0x833		/* SPI host core */
+#define	I2S_CORE_ID		0x834		/* I2S core */
+#define	DMEMS_CORE_ID		0x835		/* SDR/DDR1 memory controller core */
+#define	DEF_SHIM_COMP		0x837		/* SHIM component in ubus/6362 */
+#define OOB_ROUTER_CORE_ID	0x367		/* OOB router core ID */
+#define	DEF_AI_COMP		0xfff		/* Default component, in ai chips it maps all
+						 * unused address ranges
+						 */
+
+/* There are TWO constants on all HND chips: SI_ENUM_BASE above,
+ * and chipcommon being the first core:
+ */
+#define	SI_CC_IDX		0
+
+/* SOC Interconnect types (aka chip types) */
+#define	SOCI_SB			0
+#define	SOCI_AI			1
+#define	SOCI_UBUS		2
+
+/* Common core control flags */
+#define	SICF_BIST_EN		0x8000
+#define	SICF_PME_EN		0x4000
+#define	SICF_CORE_BITS		0x3ffc
+#define	SICF_FGC		0x0002
+#define	SICF_CLOCK_EN		0x0001
+
+/* Common core status flags */
+#define	SISF_BIST_DONE		0x8000
+#define	SISF_BIST_ERROR		0x4000
+#define	SISF_GATED_CLK		0x2000
+#define	SISF_DMA64		0x1000
+#define	SISF_CORE_BITS		0x0fff
+
+/* A register that is common to all cores to
+ * communicate w/PMU regarding clock control.
+ */
+#define SI_CLK_CTL_ST		0x1e0		/* clock control and status */
+
+/* clk_ctl_st register */
+#define	CCS_FORCEALP		0x00000001	/* force ALP request */
+#define	CCS_FORCEHT		0x00000002	/* force HT request */
+#define	CCS_FORCEILP		0x00000004	/* force ILP request */
+#define	CCS_ALPAREQ		0x00000008	/* ALP Avail Request */
+#define	CCS_HTAREQ		0x00000010	/* HT Avail Request */
+#define	CCS_FORCEHWREQOFF	0x00000020	/* Force HW Clock Request Off */
+#define CCS_ERSRC_REQ_MASK	0x00000700	/* external resource requests */
+#define CCS_ERSRC_REQ_SHIFT	8
+#define	CCS_ALPAVAIL		0x00010000	/* ALP is available */
+#define	CCS_HTAVAIL		0x00020000	/* HT is available */
+#define CCS_BP_ON_APL		0x00040000	/* RO: Backplane is running on ALP clock */
+#define CCS_BP_ON_HT		0x00080000	/* RO: Backplane is running on HT clock */
+#define CCS_ERSRC_STS_MASK	0x07000000	/* external resource status */
+#define CCS_ERSRC_STS_SHIFT	24
+
+#define	CCS0_HTAVAIL		0x00010000	/* HT avail in chipc and pcmcia on 4328a0 */
+#define	CCS0_ALPAVAIL		0x00020000	/* ALP avail in chipc and pcmcia on 4328a0 */
+
+/* Not really related to SOC Interconnect, but a couple of software
+ * conventions for the use the flash space:
+ */
+
+/* Minumum amount of flash we support */
+#define FLASH_MIN		0x00020000	/* Minimum flash size */
+
+/* A boot/binary may have an embedded block that describes its size  */
+#define	BISZ_OFFSET		0x3e0		/* At this offset into the binary */
+#define	BISZ_MAGIC		0x4249535a	/* Marked with this value: 'BISZ' */
+#define	BISZ_MAGIC_IDX		0		/* Word 0: magic */
+#define	BISZ_TXTST_IDX		1		/*	1: text start */
+#define	BISZ_TXTEND_IDX		2		/*	2: text end */
+#define	BISZ_DATAST_IDX		3		/*	3: data start */
+#define	BISZ_DATAEND_IDX	4		/*	4: data end */
+#define	BISZ_BSSST_IDX		5		/*	5: bss start */
+#define	BISZ_BSSEND_IDX		6		/*	6: bss end */
+#define BISZ_SIZE		7		/* descriptor size in 32-bit integers */
+
+#endif /* _HNDSOC_H */
diff --git a/drivers/net/wireless/bcmdhd/include/htsf.h b/drivers/net/wireless/bcmdhd/include/htsf.h
new file mode 100644
index 000000000000..d875edb816c9
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/htsf.h
@@ -0,0 +1,74 @@
+/*
+ * Time stamps for latency measurements 
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: htsf.h 277737 2011-08-16 17:54:59Z $
+ */
+#ifndef _HTSF_H_
+#define _HTSF_H_
+
+#define HTSFMAGIC       	0xCDCDABAB  /* in network order for tcpdump  */
+#define HTSFENDMAGIC    	0xEFEFABAB  /* to distinguish from RT2 magic */
+#define HTSF_HOSTOFFSET		102
+#define HTSF_DNGLOFFSET		HTSF_HOSTOFFSET	- 4
+#define HTSF_DNGLOFFSET2	HTSF_HOSTOFFSET	+ 106
+#define HTSF_MIN_PKTLEN 	200
+#define ETHER_TYPE_BRCM_PKTDLYSTATS     0x886d
+
+typedef enum htsfts_type {
+	T10,
+	T20,
+	T30,
+	T40,
+	T50,
+	T60,
+	T70,
+	T80,
+	T90,
+	TA0,
+	TE0
+} htsf_timestamp_t;
+
+typedef struct {
+	uint32 magic;
+	uint32 prio;
+	uint32 seqnum;
+	uint32 misc;
+	uint32 c10;
+	uint32 t10;
+	uint32 c20;
+	uint32 t20;
+	uint32 t30;
+	uint32 t40;
+	uint32 t50;
+	uint32 t60;
+	uint32 t70;
+	uint32 t80;
+	uint32 t90;
+	uint32 cA0;
+	uint32 tA0;
+	uint32 cE0;
+	uint32 tE0;
+	uint32 endmagic;
+} htsfts_t;
+
+#endif /* _HTSF_H_ */
diff --git a/drivers/net/wireless/bcmdhd/include/linux_osl.h b/drivers/net/wireless/bcmdhd/include/linux_osl.h
new file mode 100644
index 000000000000..830d351d882b
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/linux_osl.h
@@ -0,0 +1,431 @@
+/*
+ * Linux OS Independent Layer
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: linux_osl.h 301794 2011-12-08 20:41:35Z $
+ */
+
+
+#ifndef _linux_osl_h_
+#define _linux_osl_h_
+
+#include <typedefs.h>
+
+
+extern void * osl_os_open_image(char * filename);
+extern int osl_os_get_image_block(char * buf, int len, void * image);
+extern void osl_os_close_image(void * image);
+
+
+#ifdef BCMDRIVER
+
+
+extern osl_t *osl_attach(void *pdev, uint bustype, bool pkttag);
+extern void osl_detach(osl_t *osh);
+
+
+extern uint32 g_assert_type;
+
+
+#if defined(BCMASSERT_LOG)
+	#define ASSERT(exp) \
+	  do { if (!(exp)) osl_assert(#exp, __FILE__, __LINE__); } while (0)
+extern void osl_assert(char *exp, char *file, int line);
+#else
+	#ifdef __GNUC__
+		#define GCC_VERSION \
+			(__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__)
+		#if GCC_VERSION > 30100
+			#define ASSERT(exp)	do {} while (0)
+		#else
+			
+			#define ASSERT(exp)
+		#endif 
+	#endif 
+#endif 
+
+
+#define	OSL_DELAY(usec)		osl_delay(usec)
+extern void osl_delay(uint usec);
+
+#define	OSL_PCMCIA_READ_ATTR(osh, offset, buf, size) \
+	osl_pcmcia_read_attr((osh), (offset), (buf), (size))
+#define	OSL_PCMCIA_WRITE_ATTR(osh, offset, buf, size) \
+	osl_pcmcia_write_attr((osh), (offset), (buf), (size))
+extern void osl_pcmcia_read_attr(osl_t *osh, uint offset, void *buf, int size);
+extern void osl_pcmcia_write_attr(osl_t *osh, uint offset, void *buf, int size);
+
+
+#define	OSL_PCI_READ_CONFIG(osh, offset, size) \
+	osl_pci_read_config((osh), (offset), (size))
+#define	OSL_PCI_WRITE_CONFIG(osh, offset, size, val) \
+	osl_pci_write_config((osh), (offset), (size), (val))
+extern uint32 osl_pci_read_config(osl_t *osh, uint offset, uint size);
+extern void osl_pci_write_config(osl_t *osh, uint offset, uint size, uint val);
+
+
+#define OSL_PCI_BUS(osh)	osl_pci_bus(osh)
+#define OSL_PCI_SLOT(osh)	osl_pci_slot(osh)
+extern uint osl_pci_bus(osl_t *osh);
+extern uint osl_pci_slot(osl_t *osh);
+
+
+typedef struct {
+	bool pkttag;
+	uint pktalloced; 	
+	bool mmbus;		
+	pktfree_cb_fn_t tx_fn;  
+	void *tx_ctx;		
+} osl_pubinfo_t;
+
+#define PKTFREESETCB(osh, _tx_fn, _tx_ctx)		\
+	do {						\
+	   ((osl_pubinfo_t*)osh)->tx_fn = _tx_fn;	\
+	   ((osl_pubinfo_t*)osh)->tx_ctx = _tx_ctx;	\
+	} while (0)
+
+
+
+#define BUS_SWAP32(v)		(v)
+
+	#define MALLOC(osh, size)	osl_malloc((osh), (size))
+	#define MFREE(osh, addr, size)	osl_mfree((osh), (addr), (size))
+	#define MALLOCED(osh)		osl_malloced((osh))
+	extern void *osl_malloc(osl_t *osh, uint size);
+	extern void osl_mfree(osl_t *osh, void *addr, uint size);
+	extern uint osl_malloced(osl_t *osh);
+
+#define NATIVE_MALLOC(osh, size)		kmalloc(size, GFP_ATOMIC)
+#define NATIVE_MFREE(osh, addr, size)	kfree(addr)
+
+#define	MALLOC_FAILED(osh)	osl_malloc_failed((osh))
+extern uint osl_malloc_failed(osl_t *osh);
+
+
+#define	DMA_CONSISTENT_ALIGN	osl_dma_consistent_align()
+#define	DMA_ALLOC_CONSISTENT(osh, size, align, tot, pap, dmah) \
+	osl_dma_alloc_consistent((osh), (size), (align), (tot), (pap))
+#define	DMA_FREE_CONSISTENT(osh, va, size, pa, dmah) \
+	osl_dma_free_consistent((osh), (void*)(va), (size), (pa))
+extern uint osl_dma_consistent_align(void);
+extern void *osl_dma_alloc_consistent(osl_t *osh, uint size, uint16 align, uint *tot, ulong *pap);
+extern void osl_dma_free_consistent(osl_t *osh, void *va, uint size, ulong pa);
+
+
+#define	DMA_TX	1	
+#define	DMA_RX	2	
+
+
+#define	DMA_MAP(osh, va, size, direction, p, dmah) \
+	osl_dma_map((osh), (va), (size), (direction))
+#define	DMA_UNMAP(osh, pa, size, direction, p, dmah) \
+	osl_dma_unmap((osh), (pa), (size), (direction))
+extern uint osl_dma_map(osl_t *osh, void *va, uint size, int direction);
+extern void osl_dma_unmap(osl_t *osh, uint pa, uint size, int direction);
+
+
+#define OSL_DMADDRWIDTH(osh, addrwidth) do {} while (0)
+
+
+	#include <bcmsdh.h>
+	#define OSL_WRITE_REG(osh, r, v) (bcmsdh_reg_write(NULL, (uintptr)(r), sizeof(*(r)), (v)))
+	#define OSL_READ_REG(osh, r) (bcmsdh_reg_read(NULL, (uintptr)(r), sizeof(*(r))))
+
+	#define SELECT_BUS_WRITE(osh, mmap_op, bus_op) if (((osl_pubinfo_t*)(osh))->mmbus) \
+		mmap_op else bus_op
+	#define SELECT_BUS_READ(osh, mmap_op, bus_op) (((osl_pubinfo_t*)(osh))->mmbus) ? \
+		mmap_op : bus_op
+
+#define OSL_ERROR(bcmerror)	osl_error(bcmerror)
+extern int osl_error(int bcmerror);
+
+
+#define	PKTBUFSZ	2048   
+
+
+
+#define OSL_SYSUPTIME()		((uint32)jiffies * (1000 / HZ))
+#define	printf(fmt, args...)	printk(fmt , ## args)
+#include <linux/kernel.h>	
+#include <linux/string.h>	
+
+#define	bcopy(src, dst, len)	memcpy((dst), (src), (len))
+#define	bcmp(b1, b2, len)	memcmp((b1), (b2), (len))
+#define	bzero(b, len)		memset((b), '\0', (len))
+
+
+
+#ifndef __mips__
+#define R_REG(osh, r) (\
+	SELECT_BUS_READ(osh, sizeof(*(r)) == sizeof(uint8) ? readb((volatile uint8*)(r)) : \
+	sizeof(*(r)) == sizeof(uint16) ? readw((volatile uint16*)(r)) : \
+	readl((volatile uint32*)(r)), OSL_READ_REG(osh, r)) \
+)
+#else 
+#define R_REG(osh, r) (\
+	SELECT_BUS_READ(osh, \
+		({ \
+			__typeof(*(r)) __osl_v; \
+			__asm__ __volatile__("sync"); \
+			switch (sizeof(*(r))) { \
+				case sizeof(uint8):	__osl_v = \
+					readb((volatile uint8*)(r)); break; \
+				case sizeof(uint16):	__osl_v = \
+					readw((volatile uint16*)(r)); break; \
+				case sizeof(uint32):	__osl_v = \
+					readl((volatile uint32*)(r)); break; \
+			} \
+			__asm__ __volatile__("sync"); \
+			__osl_v; \
+		}), \
+		({ \
+			__typeof(*(r)) __osl_v; \
+			__asm__ __volatile__("sync"); \
+			__osl_v = OSL_READ_REG(osh, r); \
+			__asm__ __volatile__("sync"); \
+			__osl_v; \
+		})) \
+)
+#endif 
+
+#define W_REG(osh, r, v) do { \
+	SELECT_BUS_WRITE(osh,  \
+		switch (sizeof(*(r))) { \
+			case sizeof(uint8):	writeb((uint8)(v), (volatile uint8*)(r)); break; \
+			case sizeof(uint16):	writew((uint16)(v), (volatile uint16*)(r)); break; \
+			case sizeof(uint32):	writel((uint32)(v), (volatile uint32*)(r)); break; \
+		}, \
+		(OSL_WRITE_REG(osh, r, v))); \
+	} while (0)
+
+
+#define	AND_REG(osh, r, v)		W_REG(osh, (r), R_REG(osh, r) & (v))
+#define	OR_REG(osh, r, v)		W_REG(osh, (r), R_REG(osh, r) | (v))
+
+
+#define	bcopy(src, dst, len)	memcpy((dst), (src), (len))
+#define	bcmp(b1, b2, len)	memcmp((b1), (b2), (len))
+#define	bzero(b, len)		memset((b), '\0', (len))
+
+
+#ifdef __mips__
+#include <asm/addrspace.h>
+#define OSL_UNCACHED(va)	((void *)KSEG1ADDR((va)))
+#define OSL_CACHED(va)		((void *)KSEG0ADDR((va)))
+#else
+#define OSL_UNCACHED(va)	((void *)va)
+#define OSL_CACHED(va)		((void *)va)
+#endif 
+
+
+#if defined(__i386__)
+#define	OSL_GETCYCLES(x)	rdtscl((x))
+#else
+#define OSL_GETCYCLES(x)	((x) = 0)
+#endif 
+
+
+#define	BUSPROBE(val, addr)	({ (val) = R_REG(NULL, (addr)); 0; })
+
+
+#if !defined(CONFIG_MMC_MSM7X00A)
+#define	REG_MAP(pa, size)	ioremap_nocache((unsigned long)(pa), (unsigned long)(size))
+#else
+#define REG_MAP(pa, size)       (void *)(0)
+#endif 
+#define	REG_UNMAP(va)		iounmap((va))
+
+
+#define	R_SM(r)			*(r)
+#define	W_SM(r, v)		(*(r) = (v))
+#define	BZERO_SM(r, len)	memset((r), '\0', (len))
+
+
+#include <linuxver.h>		
+
+
+#define	PKTGET(osh, len, send)		osl_pktget((osh), (len))
+#define	PKTDUP(osh, skb)		osl_pktdup((osh), (skb))
+#define PKTLIST_DUMP(osh, buf)
+#define PKTDBG_TRACE(osh, pkt, bit)
+#define	PKTFREE(osh, skb, send)		osl_pktfree((osh), (skb), (send))
+#ifdef CONFIG_DHD_USE_STATIC_BUF
+#define	PKTGET_STATIC(osh, len, send)		osl_pktget_static((osh), (len))
+#define	PKTFREE_STATIC(osh, skb, send)		osl_pktfree_static((osh), (skb), (send))
+#endif
+#define	PKTDATA(osh, skb)		(((struct sk_buff*)(skb))->data)
+#define	PKTLEN(osh, skb)		(((struct sk_buff*)(skb))->len)
+#define PKTHEADROOM(osh, skb)		(PKTDATA(osh, skb)-(((struct sk_buff*)(skb))->head))
+#define PKTTAILROOM(osh, skb) ((((struct sk_buff*)(skb))->end)-(((struct sk_buff*)(skb))->tail))
+#define	PKTNEXT(osh, skb)		(((struct sk_buff*)(skb))->next)
+#define	PKTSETNEXT(osh, skb, x)		(((struct sk_buff*)(skb))->next = (struct sk_buff*)(x))
+#define	PKTSETLEN(osh, skb, len)	__skb_trim((struct sk_buff*)(skb), (len))
+#define	PKTPUSH(osh, skb, bytes)	skb_push((struct sk_buff*)(skb), (bytes))
+#define	PKTPULL(osh, skb, bytes)	skb_pull((struct sk_buff*)(skb), (bytes))
+#define	PKTTAG(skb)			((void*)(((struct sk_buff*)(skb))->cb))
+#define PKTALLOCED(osh)			((osl_pubinfo_t *)(osh))->pktalloced
+#define PKTSETPOOL(osh, skb, x, y)	do {} while (0)
+#define PKTPOOL(osh, skb)		FALSE
+#define PKTSHRINK(osh, m)		(m)
+
+#ifdef CTFPOOL
+#define	CTFPOOL_REFILL_THRESH	3
+typedef struct ctfpool {
+	void		*head;
+	spinlock_t	lock;
+	uint		max_obj;
+	uint		curr_obj;
+	uint		obj_size;
+	uint		refills;
+	uint		fast_allocs;
+	uint 		fast_frees;
+	uint 		slow_allocs;
+} ctfpool_t;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 22)
+#define	FASTBUF	(1 << 4)
+#define	CTFBUF	(1 << 5)
+#define	PKTSETFAST(osh, skb)	((((struct sk_buff*)(skb))->mac_len) |= FASTBUF)
+#define	PKTCLRFAST(osh, skb)	((((struct sk_buff*)(skb))->mac_len) &= (~FASTBUF))
+#define	PKTSETCTF(osh, skb)	((((struct sk_buff*)(skb))->mac_len) |= CTFBUF)
+#define	PKTCLRCTF(osh, skb)	((((struct sk_buff*)(skb))->mac_len) &= (~CTFBUF))
+#define	PKTISFAST(osh, skb)	((((struct sk_buff*)(skb))->mac_len) & FASTBUF)
+#define	PKTISCTF(osh, skb)	((((struct sk_buff*)(skb))->mac_len) & CTFBUF)
+#define	PKTFAST(osh, skb)	(((struct sk_buff*)(skb))->mac_len)
+#else
+#define	FASTBUF	(1 << 0)
+#define	CTFBUF	(1 << 1)
+#define	PKTSETFAST(osh, skb)	((((struct sk_buff*)(skb))->__unused) |= FASTBUF)
+#define	PKTCLRFAST(osh, skb)	((((struct sk_buff*)(skb))->__unused) &= (~FASTBUF))
+#define	PKTSETCTF(osh, skb)	((((struct sk_buff*)(skb))->__unused) |= CTFBUF)
+#define	PKTCLRCTF(osh, skb)	((((struct sk_buff*)(skb))->__unused) &= (~CTFBUF))
+#define	PKTISFAST(osh, skb)	((((struct sk_buff*)(skb))->__unused) & FASTBUF)
+#define	PKTISCTF(osh, skb)	((((struct sk_buff*)(skb))->__unused) & CTFBUF)
+#define	PKTFAST(osh, skb)	(((struct sk_buff*)(skb))->__unused)
+#endif 
+
+#define	CTFPOOLPTR(osh, skb)	(((struct sk_buff*)(skb))->sk)
+#define	CTFPOOLHEAD(osh, skb)	(((ctfpool_t *)((struct sk_buff*)(skb))->sk)->head)
+
+extern void *osl_ctfpool_add(osl_t *osh);
+extern void osl_ctfpool_replenish(osl_t *osh, uint thresh);
+extern int32 osl_ctfpool_init(osl_t *osh, uint numobj, uint size);
+extern void osl_ctfpool_cleanup(osl_t *osh);
+extern void osl_ctfpool_stats(osl_t *osh, void *b);
+#endif 
+
+#ifdef HNDCTF
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 22)
+#define	SKIPCT	(1 << 6)
+#define	PKTSETSKIPCT(osh, skb)	(((struct sk_buff*)(skb))->mac_len |= SKIPCT)
+#define	PKTCLRSKIPCT(osh, skb)	(((struct sk_buff*)(skb))->mac_len &= (~SKIPCT))
+#define	PKTSKIPCT(osh, skb)	(((struct sk_buff*)(skb))->mac_len & SKIPCT)
+#else 
+#define	SKIPCT	(1 << 2)
+#define	PKTSETSKIPCT(osh, skb)	(((struct sk_buff*)(skb))->__unused |= SKIPCT)
+#define	PKTCLRSKIPCT(osh, skb)	(((struct sk_buff*)(skb))->__unused &= (~SKIPCT))
+#define	PKTSKIPCT(osh, skb)	(((struct sk_buff*)(skb))->__unused & SKIPCT)
+#endif 
+#else 
+#define	PKTSETSKIPCT(osh, skb)
+#define	PKTCLRSKIPCT(osh, skb)
+#define	PKTSKIPCT(osh, skb)
+#endif 
+
+extern void osl_pktfree(osl_t *osh, void *skb, bool send);
+extern void *osl_pktget_static(osl_t *osh, uint len);
+extern void osl_pktfree_static(osl_t *osh, void *skb, bool send);
+
+extern void *osl_pktget(osl_t *osh, uint len);
+extern void *osl_pktdup(osl_t *osh, void *skb);
+
+
+static INLINE void *
+osl_pkt_frmnative(osl_pubinfo_t *osh, void *pkt)
+{
+	struct sk_buff *nskb;
+
+	if (osh->pkttag)
+		bzero((void*)((struct sk_buff*)pkt)->cb, OSL_PKTTAG_SZ);
+
+	
+	for (nskb = (struct sk_buff *)pkt; nskb; nskb = nskb->next) {
+		osh->pktalloced++;
+	}
+
+	return (void *)pkt;
+}
+#define PKTFRMNATIVE(osh, skb)	osl_pkt_frmnative(((osl_pubinfo_t *)osh), (struct sk_buff*)(skb))
+
+
+static INLINE struct sk_buff *
+osl_pkt_tonative(osl_pubinfo_t *osh, void *pkt)
+{
+	struct sk_buff *nskb;
+
+	if (osh->pkttag)
+		bzero(((struct sk_buff*)pkt)->cb, OSL_PKTTAG_SZ);
+
+	
+	for (nskb = (struct sk_buff *)pkt; nskb; nskb = nskb->next) {
+		osh->pktalloced--;
+	}
+
+	return (struct sk_buff *)pkt;
+}
+#define PKTTONATIVE(osh, pkt)		osl_pkt_tonative((osl_pubinfo_t *)(osh), (pkt))
+
+#define	PKTLINK(skb)			(((struct sk_buff*)(skb))->prev)
+#define	PKTSETLINK(skb, x)		(((struct sk_buff*)(skb))->prev = (struct sk_buff*)(x))
+#define	PKTPRIO(skb)			(((struct sk_buff*)(skb))->priority)
+#define	PKTSETPRIO(skb, x)		(((struct sk_buff*)(skb))->priority = (x))
+#define PKTSUMNEEDED(skb)		(((struct sk_buff*)(skb))->ip_summed == CHECKSUM_HW)
+#define PKTSETSUMGOOD(skb, x)		(((struct sk_buff*)(skb))->ip_summed = \
+						((x) ? CHECKSUM_UNNECESSARY : CHECKSUM_NONE))
+
+#define PKTSHARED(skb)                  (((struct sk_buff*)(skb))->cloned)
+
+
+
+#else 
+
+
+
+	#define ASSERT(exp)	do {} while (0)
+
+
+#define MALLOC(o, l) malloc(l)
+#define MFREE(o, p, l) free(p)
+#include <stdlib.h>
+
+
+#include <string.h>
+
+
+#include <stdio.h>
+
+
+extern void bcopy(const void *src, void *dst, size_t len);
+extern int bcmp(const void *b1, const void *b2, size_t len);
+extern void bzero(void *b, size_t len);
+#endif 
+
+#endif	
diff --git a/drivers/net/wireless/bcmdhd/include/linuxver.h b/drivers/net/wireless/bcmdhd/include/linuxver.h
new file mode 100644
index 000000000000..54d88ee923b2
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/linuxver.h
@@ -0,0 +1,614 @@
+/*
+ * Linux-specific abstractions to gain some independence from linux kernel versions.
+ * Pave over some 2.2 versus 2.4 versus 2.6 kernel differences.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: linuxver.h 312264 2012-02-02 00:49:43Z $
+ */
+
+
+#ifndef _linuxver_h_
+#define _linuxver_h_
+
+#include <linux/version.h>
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
+#include <linux/config.h>
+#else
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 33))
+#include <generated/autoconf.h>
+#else
+#include <linux/autoconf.h>
+#endif
+#endif 
+#include <linux/module.h>
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 0))
+
+#ifdef __UNDEF_NO_VERSION__
+#undef __NO_VERSION__
+#else
+#define __NO_VERSION__
+#endif
+#endif	
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 5, 0)
+#define module_param(_name_, _type_, _perm_)	MODULE_PARM(_name_, "i")
+#define module_param_string(_name_, _string_, _size_, _perm_) \
+		MODULE_PARM(_string_, "c" __MODULE_STRING(_size_))
+#endif
+
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 4, 9))
+#include <linux/malloc.h>
+#else
+#include <linux/slab.h>
+#endif
+
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/pci.h>
+#include <linux/interrupt.h>
+#include <linux/netdevice.h>
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+#include <linux/semaphore.h>
+#endif 
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 28))
+#undef IP_TOS
+#endif 
+#include <asm/io.h>
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2, 5, 41))
+#include <linux/workqueue.h>
+#else
+#include <linux/tqueue.h>
+#ifndef work_struct
+#define work_struct tq_struct
+#endif
+#ifndef INIT_WORK
+#define INIT_WORK(_work, _func, _data) INIT_TQUEUE((_work), (_func), (_data))
+#endif
+#ifndef schedule_work
+#define schedule_work(_work) schedule_task((_work))
+#endif
+#ifndef flush_scheduled_work
+#define flush_scheduled_work() flush_scheduled_tasks()
+#endif
+#endif	
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+#define	MY_INIT_WORK(_work, _func)	INIT_WORK(_work, _func)
+#else
+#define	MY_INIT_WORK(_work, _func)	INIT_WORK(_work, _func, _work)
+typedef void (*work_func_t)(void *work);
+#endif	
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
+
+#ifndef IRQ_NONE
+typedef void irqreturn_t;
+#define IRQ_NONE
+#define IRQ_HANDLED
+#define IRQ_RETVAL(x)
+#endif
+#else
+typedef irqreturn_t(*FN_ISR) (int irq, void *dev_id, struct pt_regs *ptregs);
+#endif	
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18)
+#define IRQF_SHARED	SA_SHIRQ
+#endif 
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 17)
+#ifdef	CONFIG_NET_RADIO
+#define	CONFIG_WIRELESS_EXT
+#endif
+#endif	
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 5, 67)
+#define MOD_INC_USE_COUNT
+#define MOD_DEC_USE_COUNT
+#endif 
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 32)
+#include <linux/sched.h>
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29)
+#include <net/lib80211.h>
+#endif
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29)
+#include <linux/ieee80211.h>
+#else
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 14)
+#include <net/ieee80211.h>
+#endif
+#endif 
+
+
+#ifndef __exit
+#define __exit
+#endif
+#ifndef __devexit
+#define __devexit
+#endif
+#ifndef __devinit
+#define __devinit	__init
+#endif
+#ifndef __devinitdata
+#define __devinitdata
+#endif
+#ifndef __devexit_p
+#define __devexit_p(x)	x
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 4, 0))
+
+#define pci_get_drvdata(dev)		(dev)->sysdata
+#define pci_set_drvdata(dev, value)	(dev)->sysdata = (value)
+
+
+
+struct pci_device_id {
+	unsigned int vendor, device;		
+	unsigned int subvendor, subdevice;	
+	unsigned int class, class_mask;		
+	unsigned long driver_data;		
+};
+
+struct pci_driver {
+	struct list_head node;
+	char *name;
+	const struct pci_device_id *id_table;	
+	int (*probe)(struct pci_dev *dev,
+	             const struct pci_device_id *id); 
+	void (*remove)(struct pci_dev *dev);	
+	void (*suspend)(struct pci_dev *dev);	
+	void (*resume)(struct pci_dev *dev);	
+};
+
+#define MODULE_DEVICE_TABLE(type, name)
+#define PCI_ANY_ID (~0)
+
+
+#define pci_module_init pci_register_driver
+extern int pci_register_driver(struct pci_driver *drv);
+extern void pci_unregister_driver(struct pci_driver *drv);
+
+#endif 
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 18))
+#define pci_module_init pci_register_driver
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 2, 18))
+#ifdef MODULE
+#define module_init(x) int init_module(void) { return x(); }
+#define module_exit(x) void cleanup_module(void) { x(); }
+#else
+#define module_init(x)	__initcall(x);
+#define module_exit(x)	__exitcall(x);
+#endif
+#endif	
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 31)
+#define WL_USE_NETDEV_OPS
+#else
+#undef WL_USE_NETDEV_OPS
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31)) && defined(CONFIG_RFKILL_INPUT)
+#define WL_CONFIG_RFKILL_INPUT
+#else
+#undef WL_CONFIG_RFKILL_INPUT
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 48))
+#define list_for_each(pos, head) \
+	for (pos = (head)->next; pos != (head); pos = pos->next)
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 13))
+#define pci_resource_start(dev, bar)	((dev)->base_address[(bar)])
+#elif (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 44))
+#define pci_resource_start(dev, bar)	((dev)->resource[(bar)].start)
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 23))
+#define pci_enable_device(dev) do { } while (0)
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 14))
+#define net_device device
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 42))
+
+
+
+#ifndef PCI_DMA_TODEVICE
+#define	PCI_DMA_TODEVICE	1
+#define	PCI_DMA_FROMDEVICE	2
+#endif
+
+typedef u32 dma_addr_t;
+
+
+static inline int get_order(unsigned long size)
+{
+	int order;
+
+	size = (size-1) >> (PAGE_SHIFT-1);
+	order = -1;
+	do {
+		size >>= 1;
+		order++;
+	} while (size);
+	return order;
+}
+
+static inline void *pci_alloc_consistent(struct pci_dev *hwdev, size_t size,
+                                         dma_addr_t *dma_handle)
+{
+	void *ret;
+	int gfp = GFP_ATOMIC | GFP_DMA;
+
+	ret = (void *)__get_free_pages(gfp, get_order(size));
+
+	if (ret != NULL) {
+		memset(ret, 0, size);
+		*dma_handle = virt_to_bus(ret);
+	}
+	return ret;
+}
+static inline void pci_free_consistent(struct pci_dev *hwdev, size_t size,
+                                       void *vaddr, dma_addr_t dma_handle)
+{
+	free_pages((unsigned long)vaddr, get_order(size));
+}
+#define pci_map_single(cookie, address, size, dir)	virt_to_bus(address)
+#define pci_unmap_single(cookie, address, size, dir)
+
+#endif 
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 3, 43))
+
+#define dev_kfree_skb_any(a)		dev_kfree_skb(a)
+#define netif_down(dev)			do { (dev)->start = 0; } while (0)
+
+
+#ifndef _COMPAT_NETDEVICE_H
+
+
+
+#define dev_kfree_skb_irq(a)	dev_kfree_skb(a)
+#define netif_wake_queue(dev) \
+		do { clear_bit(0, &(dev)->tbusy); mark_bh(NET_BH); } while (0)
+#define netif_stop_queue(dev)	set_bit(0, &(dev)->tbusy)
+
+static inline void netif_start_queue(struct net_device *dev)
+{
+	dev->tbusy = 0;
+	dev->interrupt = 0;
+	dev->start = 1;
+}
+
+#define netif_queue_stopped(dev)	(dev)->tbusy
+#define netif_running(dev)		(dev)->start
+
+#endif 
+
+#define netif_device_attach(dev)	netif_start_queue(dev)
+#define netif_device_detach(dev)	netif_stop_queue(dev)
+
+
+#define tasklet_struct				tq_struct
+static inline void tasklet_schedule(struct tasklet_struct *tasklet)
+{
+	queue_task(tasklet, &tq_immediate);
+	mark_bh(IMMEDIATE_BH);
+}
+
+static inline void tasklet_init(struct tasklet_struct *tasklet,
+                                void (*func)(unsigned long),
+                                unsigned long data)
+{
+	tasklet->next = NULL;
+	tasklet->sync = 0;
+	tasklet->routine = (void (*)(void *))func;
+	tasklet->data = (void *)data;
+}
+#define tasklet_kill(tasklet)	{ do {} while (0); }
+
+
+#define del_timer_sync(timer) del_timer(timer)
+
+#else
+
+#define netif_down(dev)
+
+#endif 
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 4, 3))
+
+
+#define PREPARE_TQUEUE(_tq, _routine, _data)			\
+	do {							\
+		(_tq)->routine = _routine;			\
+		(_tq)->data = _data;				\
+	} while (0)
+
+
+#define INIT_TQUEUE(_tq, _routine, _data)			\
+	do {							\
+		INIT_LIST_HEAD(&(_tq)->list);			\
+		(_tq)->sync = 0;				\
+		PREPARE_TQUEUE((_tq), (_routine), (_data));	\
+	} while (0)
+
+#endif	
+
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 9)
+#define	PCI_SAVE_STATE(a, b)	pci_save_state(a)
+#define	PCI_RESTORE_STATE(a, b)	pci_restore_state(a)
+#else
+#define	PCI_SAVE_STATE(a, b)	pci_save_state(a, b)
+#define	PCI_RESTORE_STATE(a, b)	pci_restore_state(a, b)
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 4, 6))
+static inline int
+pci_save_state(struct pci_dev *dev, u32 *buffer)
+{
+	int i;
+	if (buffer) {
+		for (i = 0; i < 16; i++)
+			pci_read_config_dword(dev, i * 4, &buffer[i]);
+	}
+	return 0;
+}
+
+static inline int
+pci_restore_state(struct pci_dev *dev, u32 *buffer)
+{
+	int i;
+
+	if (buffer) {
+		for (i = 0; i < 16; i++)
+			pci_write_config_dword(dev, i * 4, buffer[i]);
+	}
+	
+	else {
+		for (i = 0; i < 6; i ++)
+			pci_write_config_dword(dev,
+			                       PCI_BASE_ADDRESS_0 + (i * 4),
+			                       pci_resource_start(dev, i));
+		pci_write_config_byte(dev, PCI_INTERRUPT_LINE, dev->irq);
+	}
+	return 0;
+}
+#endif 
+
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 4, 19))
+#define read_c0_count() read_32bit_cp0_register(CP0_COUNT)
+#endif
+
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 24))
+#ifndef SET_MODULE_OWNER
+#define SET_MODULE_OWNER(dev)		do {} while (0)
+#define OLD_MOD_INC_USE_COUNT		MOD_INC_USE_COUNT
+#define OLD_MOD_DEC_USE_COUNT		MOD_DEC_USE_COUNT
+#else
+#define OLD_MOD_INC_USE_COUNT		do {} while (0)
+#define OLD_MOD_DEC_USE_COUNT		do {} while (0)
+#endif
+#else 
+#ifndef SET_MODULE_OWNER
+#define SET_MODULE_OWNER(dev)		do {} while (0)
+#endif
+#ifndef MOD_INC_USE_COUNT
+#define MOD_INC_USE_COUNT			do {} while (0)
+#endif
+#ifndef MOD_DEC_USE_COUNT
+#define MOD_DEC_USE_COUNT			do {} while (0)
+#endif
+#define OLD_MOD_INC_USE_COUNT		MOD_INC_USE_COUNT
+#define OLD_MOD_DEC_USE_COUNT		MOD_DEC_USE_COUNT
+#endif 
+
+#ifndef SET_NETDEV_DEV
+#define SET_NETDEV_DEV(net, pdev)	do {} while (0)
+#endif
+
+#ifndef HAVE_FREE_NETDEV
+#define free_netdev(dev)		kfree(dev)
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
+
+#define af_packet_priv			data
+#endif
+
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 11)
+#define DRV_SUSPEND_STATE_TYPE pm_message_t
+#else
+#define DRV_SUSPEND_STATE_TYPE uint32
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+#define CHECKSUM_HW	CHECKSUM_PARTIAL
+#endif
+
+typedef struct {
+	void 	*parent;  
+	struct	task_struct *p_task;
+	long 	thr_pid;
+	int 	prio; 
+	struct	semaphore sema;
+	bool	terminated;
+	struct	completion completed;
+} tsk_ctl_t;
+
+
+
+
+#ifdef DHD_DEBUG
+#define DBG_THR(x) printk x
+#else
+#define DBG_THR(x)
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+#define SMP_RD_BARRIER_DEPENDS(x) smp_read_barrier_depends(x)
+#else
+#define SMP_RD_BARRIER_DEPENDS(x) smp_rmb(x)
+#endif
+
+
+#define PROC_START(thread_func, owner, tsk_ctl, flags) \
+{ \
+	sema_init(&((tsk_ctl)->sema), 0); \
+	init_completion(&((tsk_ctl)->completed)); \
+	(tsk_ctl)->parent = owner; \
+	(tsk_ctl)->terminated = FALSE; \
+	(tsk_ctl)->thr_pid = kernel_thread(thread_func, tsk_ctl, flags); \
+	if ((tsk_ctl)->thr_pid > 0) \
+		wait_for_completion(&((tsk_ctl)->completed)); \
+	DBG_THR(("%s thr:%lx started\n", __FUNCTION__, (tsk_ctl)->thr_pid)); \
+}
+
+#define PROC_STOP(tsk_ctl) \
+{ \
+	(tsk_ctl)->terminated = TRUE; \
+	smp_wmb(); \
+	up(&((tsk_ctl)->sema));	\
+	wait_for_completion(&((tsk_ctl)->completed)); \
+	DBG_THR(("%s thr:%lx terminated OK\n", __FUNCTION__, (tsk_ctl)->thr_pid)); \
+	(tsk_ctl)->thr_pid = -1; \
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+#define DAEMONIZE(a) daemonize(a); \
+	allow_signal(SIGKILL); \
+	allow_signal(SIGTERM);
+#else /* Linux 2.4 (w/o preemption patch) */
+#define RAISE_RX_SOFTIRQ() \
+	cpu_raise_softirq(smp_processor_id(), NET_RX_SOFTIRQ)
+#define DAEMONIZE(a) daemonize(); \
+	do { if (a) \
+		strncpy(current->comm, a, MIN(sizeof(current->comm), (strlen(a) + 1))); \
+	} while (0);
+#endif /* LINUX_VERSION_CODE  */
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+#define BLOCKABLE()	(!in_atomic())
+#else
+#define BLOCKABLE()	(!in_interrupt())
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 31))
+#define KILL_PROC(nr, sig) \
+{ \
+struct task_struct *tsk; \
+struct pid *pid;    \
+pid = find_get_pid((pid_t)nr);    \
+tsk = pid_task(pid, PIDTYPE_PID);    \
+if (tsk) send_sig(sig, tsk, 1); \
+}
+#else
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && (LINUX_VERSION_CODE <= \
+	KERNEL_VERSION(2, 6, 30))
+#define KILL_PROC(pid, sig) \
+{ \
+	struct task_struct *tsk; \
+	tsk = find_task_by_vpid(pid); \
+	if (tsk) send_sig(sig, tsk, 1); \
+}
+#else
+#define KILL_PROC(pid, sig) \
+{ \
+	kill_proc(pid, sig, 1); \
+}
+#endif
+#endif 
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+#include <linux/time.h>
+#include <linux/wait.h>
+#else
+#include <linux/sched.h>
+
+#define __wait_event_interruptible_timeout(wq, condition, ret)		\
+do {									\
+	wait_queue_t __wait;						\
+	init_waitqueue_entry(&__wait, current);				\
+									\
+	add_wait_queue(&wq, &__wait);					\
+	for (;;) {							\
+		set_current_state(TASK_INTERRUPTIBLE);			\
+		if (condition)						\
+			break;						\
+		if (!signal_pending(current)) {				\
+			ret = schedule_timeout(ret);			\
+			if (!ret)					\
+				break;					\
+			continue;					\
+		}							\
+		ret = -ERESTARTSYS;					\
+		break;							\
+	}								\
+	current->state = TASK_RUNNING;					\
+	remove_wait_queue(&wq, &__wait);				\
+} while (0)
+
+#define wait_event_interruptible_timeout(wq, condition, timeout)	\
+({									\
+	long __ret = timeout;						\
+	if (!(condition))						\
+		__wait_event_interruptible_timeout(wq, condition, __ret); \
+	__ret;								\
+})
+
+#endif 
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 29)
+#define WL_DEV_IF(dev)          ((wl_if_t*)netdev_priv(dev))
+#else
+#define WL_DEV_IF(dev)          ((wl_if_t*)(dev)->priv)
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 20)
+#define WL_ISR(i, d, p)         wl_isr((i), (d))
+#else
+#define WL_ISR(i, d, p)         wl_isr((i), (d), (p))
+#endif  
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 0))
+#define netdev_priv(dev) dev->priv
+#endif 
+
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/miniopt.h b/drivers/net/wireless/bcmdhd/include/miniopt.h
new file mode 100644
index 000000000000..77eace6252d7
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/miniopt.h
@@ -0,0 +1,77 @@
+/*
+ * Command line options parser.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: miniopt.h 277737 2011-08-16 17:54:59Z $
+ */
+
+
+#ifndef MINI_OPT_H
+#define MINI_OPT_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* ---- Include Files ---------------------------------------------------- */
+/* ---- Constants and Types ---------------------------------------------- */
+
+#define MINIOPT_MAXKEY	128	/* Max options */
+typedef struct miniopt {
+
+	/* These are persistent after miniopt_init() */
+	const char* name;		/* name for prompt in error strings */
+	const char* flags;		/* option chars that take no args */
+	bool longflags;		/* long options may be flags */
+	bool opt_end;		/* at end of options (passed a "--") */
+
+	/* These are per-call to miniopt() */
+
+	int consumed;		/* number of argv entries cosumed in
+				 * the most recent call to miniopt()
+				 */
+	bool positional;
+	bool good_int;		/* 'val' member is the result of a sucessful
+				 * strtol conversion of the option value
+				 */
+	char opt;
+	char key[MINIOPT_MAXKEY];
+	char* valstr;		/* positional param, or value for the option,
+				 * or null if the option had
+				 * no accompanying value
+				 */
+	uint uval;		/* strtol translation of valstr */
+	int  val;		/* strtol translation of valstr */
+} miniopt_t;
+
+void miniopt_init(miniopt_t *t, const char* name, const char* flags, bool longflags);
+int miniopt(miniopt_t *t, char **argv);
+
+
+/* ---- Variable Externs ------------------------------------------------- */
+/* ---- Function Prototypes ---------------------------------------------- */
+
+
+#ifdef __cplusplus
+	}
+#endif
+
+#endif  /* MINI_OPT_H  */
diff --git a/drivers/net/wireless/bcmdhd/include/msgtrace.h b/drivers/net/wireless/bcmdhd/include/msgtrace.h
new file mode 100644
index 000000000000..088f1e845a43
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/msgtrace.h
@@ -0,0 +1,74 @@
+/*
+ * Trace messages sent over HBUS
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: msgtrace.h 277737 2011-08-16 17:54:59Z $
+ */
+
+#ifndef	_MSGTRACE_H
+#define	_MSGTRACE_H
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+
+/* This marks the start of a packed structure section. */
+#include <packed_section_start.h>
+
+#define MSGTRACE_VERSION 1
+
+/* Message trace header */
+typedef BWL_PRE_PACKED_STRUCT struct msgtrace_hdr {
+	uint8	version;
+	uint8   spare;
+	uint16	len;	/* Len of the trace */
+	uint32	seqnum;	/* Sequence number of message. Useful if the messsage has been lost
+			 * because of DMA error or a bus reset (ex: SDIO Func2)
+			 */
+	uint32  discarded_bytes;  /* Number of discarded bytes because of trace overflow  */
+	uint32  discarded_printf; /* Number of discarded printf because of trace overflow */
+} BWL_POST_PACKED_STRUCT msgtrace_hdr_t;
+
+#define MSGTRACE_HDRLEN 	sizeof(msgtrace_hdr_t)
+
+/* The hbus driver generates traces when sending a trace message. This causes endless traces. 
+ * This flag must be set to TRUE in any hbus traces. The flag is reset in the function msgtrace_put.
+ * This prevents endless traces but generates hasardous lost of traces only in bus device code.
+ * It is recommendat to set this flag in macro SD_TRACE but not in SD_ERROR for avoiding missing
+ * hbus error traces. hbus error trace should not generates endless traces.
+ */
+extern bool msgtrace_hbus_trace;
+
+typedef void (*msgtrace_func_send_t)(void *hdl1, void *hdl2, uint8 *hdr,
+                                     uint16 hdrlen, uint8 *buf, uint16 buflen);
+extern void msgtrace_start(void);
+extern void msgtrace_stop(void);
+extern void msgtrace_sent(void);
+extern void msgtrace_put(char *buf, int count);
+extern void msgtrace_init(void *hdl1, void *hdl2, msgtrace_func_send_t func_send);
+extern bool msgtrace_event_enabled(void);
+
+/* This marks the end of a packed structure section. */
+#include <packed_section_end.h>
+
+#endif	/* _MSGTRACE_H */
diff --git a/drivers/net/wireless/bcmdhd/include/osl.h b/drivers/net/wireless/bcmdhd/include/osl.h
new file mode 100644
index 000000000000..b8cc2569f506
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/osl.h
@@ -0,0 +1,66 @@
+/*
+ * OS Abstraction Layer
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: osl.h 277737 2011-08-16 17:54:59Z $
+ */
+
+
+#ifndef _osl_h_
+#define _osl_h_
+
+
+typedef struct osl_info osl_t;
+typedef struct osl_dmainfo osldma_t;
+
+#define OSL_PKTTAG_SZ	32 
+
+
+typedef void (*pktfree_cb_fn_t)(void *ctx, void *pkt, unsigned int status);
+
+
+#include <linux_osl.h>
+
+#ifndef PKTDBG_TRACE
+#define PKTDBG_TRACE(osh, pkt, bit)
+#endif
+
+
+
+#define	SET_REG(osh, r, mask, val)	W_REG((osh), (r), ((R_REG((osh), r) & ~(mask)) | (val)))
+
+#ifndef AND_REG
+#define AND_REG(osh, r, v)		W_REG(osh, (r), R_REG(osh, r) & (v))
+#endif   
+
+#ifndef OR_REG
+#define OR_REG(osh, r, v)		W_REG(osh, (r), R_REG(osh, r) | (v))
+#endif   
+
+#if !defined(OSL_SYSUPTIME)
+#define OSL_SYSUPTIME() (0)
+#define OSL_SYSUPTIME_SUPPORT FALSE
+#else
+#define OSL_SYSUPTIME_SUPPORT TRUE
+#endif 
+
+#endif	
diff --git a/drivers/net/wireless/bcmdhd/include/packed_section_end.h b/drivers/net/wireless/bcmdhd/include/packed_section_end.h
new file mode 100644
index 000000000000..71f8b2e13b3b
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/packed_section_end.h
@@ -0,0 +1,54 @@
+/*
+ * Declare directives for structure packing. No padding will be provided
+ * between the members of packed structures, and therefore, there is no
+ * guarantee that structure members will be aligned.
+ *
+ * Declaring packed structures is compiler specific. In order to handle all
+ * cases, packed structures should be delared as:
+ *
+ * #include <packed_section_start.h>
+ *
+ * typedef BWL_PRE_PACKED_STRUCT struct foobar_t {
+ *    some_struct_members;
+ * } BWL_POST_PACKED_STRUCT foobar_t;
+ *
+ * #include <packed_section_end.h>
+ *
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: packed_section_end.h 277737 2011-08-16 17:54:59Z $
+ */
+
+
+
+
+#ifdef BWL_PACKED_SECTION
+	#undef BWL_PACKED_SECTION
+#else
+	#error "BWL_PACKED_SECTION is NOT defined!"
+#endif
+
+
+
+
+
+#undef	BWL_PRE_PACKED_STRUCT
+#undef	BWL_POST_PACKED_STRUCT
diff --git a/drivers/net/wireless/bcmdhd/include/packed_section_start.h b/drivers/net/wireless/bcmdhd/include/packed_section_start.h
new file mode 100644
index 000000000000..afc2ba32fd93
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/packed_section_start.h
@@ -0,0 +1,61 @@
+/*
+ * Declare directives for structure packing. No padding will be provided
+ * between the members of packed structures, and therefore, there is no
+ * guarantee that structure members will be aligned.
+ *
+ * Declaring packed structures is compiler specific. In order to handle all
+ * cases, packed structures should be delared as:
+ *
+ * #include <packed_section_start.h>
+ *
+ * typedef BWL_PRE_PACKED_STRUCT struct foobar_t {
+ *    some_struct_members;
+ * } BWL_POST_PACKED_STRUCT foobar_t;
+ *
+ * #include <packed_section_end.h>
+ *
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: packed_section_start.h 277737 2011-08-16 17:54:59Z $
+ */
+
+
+
+
+#ifdef BWL_PACKED_SECTION
+	#error "BWL_PACKED_SECTION is already defined!"
+#else
+	#define BWL_PACKED_SECTION
+#endif
+
+
+
+
+
+#if defined(__GNUC__)
+	#define	BWL_PRE_PACKED_STRUCT
+	#define	BWL_POST_PACKED_STRUCT	__attribute__ ((packed))
+#elif defined(__CC_ARM)
+	#define	BWL_PRE_PACKED_STRUCT	__packed
+	#define	BWL_POST_PACKED_STRUCT
+#else
+	#error "Unknown compiler!"
+#endif
diff --git a/drivers/net/wireless/bcmdhd/include/pcicfg.h b/drivers/net/wireless/bcmdhd/include/pcicfg.h
new file mode 100644
index 000000000000..66199431fb92
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/pcicfg.h
@@ -0,0 +1,78 @@
+/*
+ * pcicfg.h: PCI configuration constants and structures.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: pcicfg.h 277737 2011-08-16 17:54:59Z $
+ */
+
+
+#ifndef	_h_pcicfg_
+#define	_h_pcicfg_
+
+
+#define	PCI_CFG_VID		0
+#define	PCI_CFG_CMD		4
+#define	PCI_CFG_REV		8
+#define	PCI_CFG_BAR0		0x10
+#define	PCI_CFG_BAR1		0x14
+#define	PCI_BAR0_WIN		0x80	
+#define	PCI_INT_STATUS		0x90	
+#define	PCI_INT_MASK		0x94	
+
+#define PCIE_EXTCFG_OFFSET	0x100
+#define	PCI_SPROM_CONTROL	0x88	
+#define	PCI_BAR1_CONTROL	0x8c	
+#define PCI_TO_SB_MB		0x98	
+#define PCI_BACKPLANE_ADDR	0xa0	
+#define PCI_BACKPLANE_DATA	0xa4	
+#define	PCI_CLK_CTL_ST		0xa8	
+#define	PCI_BAR0_WIN2		0xac	
+#define	PCI_GPIO_IN		0xb0	
+#define	PCI_GPIO_OUT		0xb4	
+#define	PCI_GPIO_OUTEN		0xb8	
+
+#define	PCI_BAR0_SHADOW_OFFSET	(2 * 1024)	
+#define	PCI_BAR0_SPROM_OFFSET	(4 * 1024)	
+#define	PCI_BAR0_PCIREGS_OFFSET	(6 * 1024)	
+#define	PCI_BAR0_PCISBR_OFFSET	(4 * 1024)	
+
+#define PCI_BAR0_WINSZ		(16 * 1024)	
+
+
+#define	PCI_16KB0_PCIREGS_OFFSET (8 * 1024)	
+#define	PCI_16KB0_CCREGS_OFFSET	(12 * 1024)	
+#define PCI_16KBB0_WINSZ	(16 * 1024)	
+
+
+#define	PCI_16KB0_WIN2_OFFSET	(4 * 1024)	
+
+
+
+#define SPROM_SZ_MSK		0x02	
+#define SPROM_LOCKED		0x08	
+#define	SPROM_BLANK		0x04	
+#define SPROM_WRITEEN		0x10	
+#define SPROM_BOOTROM_WE	0x20	
+#define SPROM_BACKPLANE_EN	0x40	
+#define SPROM_OTPIN_USE		0x80	
+
+#endif	
diff --git a/drivers/net/wireless/bcmdhd/include/proto/802.11.h b/drivers/net/wireless/bcmdhd/include/proto/802.11.h
new file mode 100644
index 000000000000..fd69aac41309
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/proto/802.11.h
@@ -0,0 +1,2032 @@
+/*
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * Fundamental types and constants relating to 802.11
+ *
+ * $Id: 802.11.h 304058 2011-12-21 00:39:12Z $
+ */
+
+
+#ifndef _802_11_H_
+#define _802_11_H_
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+#ifndef _NET_ETHERNET_H_
+#include <proto/ethernet.h>
+#endif
+
+#include <proto/wpa.h>
+
+
+#include <packed_section_start.h>
+
+
+#define DOT11_TU_TO_US          1024    
+
+
+#define DOT11_A3_HDR_LEN        24  
+#define DOT11_A4_HDR_LEN        30  
+#define DOT11_MAC_HDR_LEN       DOT11_A3_HDR_LEN    
+#define DOT11_FCS_LEN           4   
+#define DOT11_ICV_LEN           4   
+#define DOT11_ICV_AES_LEN       8   
+#define DOT11_QOS_LEN           2   
+#define DOT11_HTC_LEN           4   
+
+#define DOT11_KEY_INDEX_SHIFT       6   
+#define DOT11_IV_LEN            4   
+#define DOT11_IV_TKIP_LEN       8   
+#define DOT11_IV_AES_OCB_LEN        4   
+#define DOT11_IV_AES_CCM_LEN        8   
+#define DOT11_IV_MAX_LEN        8   
+
+
+#define DOT11_MAX_MPDU_BODY_LEN     2304    
+
+#define DOT11_MAX_MPDU_LEN      (DOT11_A4_HDR_LEN + \
+					 DOT11_QOS_LEN + \
+					 DOT11_IV_AES_CCM_LEN + \
+					 DOT11_MAX_MPDU_BODY_LEN + \
+					 DOT11_ICV_LEN + \
+					 DOT11_FCS_LEN) 
+
+#define DOT11_MAX_SSID_LEN      32  
+
+
+#define DOT11_DEFAULT_RTS_LEN       2347    
+#define DOT11_MAX_RTS_LEN       2347    
+
+
+#define DOT11_MIN_FRAG_LEN      256 
+#define DOT11_MAX_FRAG_LEN      2346    
+#define DOT11_DEFAULT_FRAG_LEN      2346    
+
+
+#define DOT11_MIN_BEACON_PERIOD     1   
+#define DOT11_MAX_BEACON_PERIOD     0xFFFF  
+
+
+#define DOT11_MIN_DTIM_PERIOD       1   
+#define DOT11_MAX_DTIM_PERIOD       0xFF    
+
+
+#define DOT11_LLC_SNAP_HDR_LEN      8   
+#define DOT11_OUI_LEN           3   
+BWL_PRE_PACKED_STRUCT struct dot11_llc_snap_header {
+	uint8   dsap;               
+	uint8   ssap;               
+	uint8   ctl;                
+	uint8   oui[DOT11_OUI_LEN];     
+	uint16  type;               
+} BWL_POST_PACKED_STRUCT;
+
+
+#define RFC1042_HDR_LEN (ETHER_HDR_LEN + DOT11_LLC_SNAP_HDR_LEN)    
+
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_header {
+	uint16          fc;     
+	uint16          durid;      
+	struct ether_addr   a1;     
+	struct ether_addr   a2;     
+	struct ether_addr   a3;     
+	uint16          seq;        
+	struct ether_addr   a4;     
+} BWL_POST_PACKED_STRUCT;
+
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_rts_frame {
+	uint16          fc;     
+	uint16          durid;      
+	struct ether_addr   ra;     
+	struct ether_addr   ta;     
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_RTS_LEN       16      
+
+BWL_PRE_PACKED_STRUCT struct dot11_cts_frame {
+	uint16          fc;     
+	uint16          durid;      
+	struct ether_addr   ra;     
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_CTS_LEN       10      
+
+BWL_PRE_PACKED_STRUCT struct dot11_ack_frame {
+	uint16          fc;     
+	uint16          durid;      
+	struct ether_addr   ra;     
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_ACK_LEN       10      
+
+BWL_PRE_PACKED_STRUCT struct dot11_ps_poll_frame {
+	uint16          fc;     
+	uint16          durid;      
+	struct ether_addr   bssid;      
+	struct ether_addr   ta;     
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_PS_POLL_LEN   16      
+
+BWL_PRE_PACKED_STRUCT struct dot11_cf_end_frame {
+	uint16          fc;     
+	uint16          durid;      
+	struct ether_addr   ra;     
+	struct ether_addr   bssid;      
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_CS_END_LEN    16      
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_action_wifi_vendor_specific {
+	uint8   category;
+	uint8   OUI[3];
+	uint8   type;
+	uint8   subtype;
+	uint8   data[1040];
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_action_wifi_vendor_specific dot11_action_wifi_vendor_specific_t;
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_action_vs_frmhdr {
+	uint8   category;
+	uint8   OUI[3];
+	uint8   type;
+	uint8   subtype;
+	uint8   data[1];
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_action_vs_frmhdr dot11_action_vs_frmhdr_t;
+#define DOT11_ACTION_VS_HDR_LEN 6
+
+#define BCM_ACTION_OUI_BYTE0    0x00
+#define BCM_ACTION_OUI_BYTE1    0x90
+#define BCM_ACTION_OUI_BYTE2    0x4c
+
+
+#define DOT11_BA_CTL_POLICY_NORMAL  0x0000  
+#define DOT11_BA_CTL_POLICY_NOACK   0x0001  
+#define DOT11_BA_CTL_POLICY_MASK    0x0001  
+
+#define DOT11_BA_CTL_MTID       0x0002  
+#define DOT11_BA_CTL_COMPRESSED     0x0004  
+
+#define DOT11_BA_CTL_NUMMSDU_MASK   0x0FC0  
+#define DOT11_BA_CTL_NUMMSDU_SHIFT  6   
+
+#define DOT11_BA_CTL_TID_MASK       0xF000  
+#define DOT11_BA_CTL_TID_SHIFT      12  
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_ctl_header {
+	uint16          fc;     
+	uint16          durid;      
+	struct ether_addr   ra;     
+	struct ether_addr   ta;     
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_CTL_HDR_LEN   16      
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_bar {
+	uint16          bar_control;    
+	uint16          seqnum;     
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_BAR_LEN       4       
+
+#define DOT11_BA_BITMAP_LEN 128     
+#define DOT11_BA_CMP_BITMAP_LEN 8       
+
+BWL_PRE_PACKED_STRUCT struct dot11_ba {
+	uint16          ba_control; 
+	uint16          seqnum;     
+	uint8           bitmap[DOT11_BA_BITMAP_LEN];    
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_BA_LEN        4       
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_management_header {
+	uint16          fc;     
+	uint16          durid;      
+	struct ether_addr   da;     
+	struct ether_addr   sa;     
+	struct ether_addr   bssid;      
+	uint16          seq;        
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_MGMT_HDR_LEN  24      
+
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_bcn_prb {
+	uint32          timestamp[2];
+	uint16          beacon_interval;
+	uint16          capability;
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_BCN_PRB_LEN   12      
+#define DOT11_BCN_PRB_FIXED_LEN 12      
+
+BWL_PRE_PACKED_STRUCT struct dot11_auth {
+	uint16          alg;        
+	uint16          seq;        
+	uint16          status;     
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_AUTH_FIXED_LEN    6       
+
+BWL_PRE_PACKED_STRUCT struct dot11_assoc_req {
+	uint16          capability; 
+	uint16          listen;     
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_ASSOC_REQ_FIXED_LEN   4   
+
+BWL_PRE_PACKED_STRUCT struct dot11_reassoc_req {
+	uint16          capability; 
+	uint16          listen;     
+	struct ether_addr   ap;     
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_REASSOC_REQ_FIXED_LEN 10  
+
+BWL_PRE_PACKED_STRUCT struct dot11_assoc_resp {
+	uint16          capability; 
+	uint16          status;     
+	uint16          aid;        
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_ASSOC_RESP_FIXED_LEN  6   
+
+BWL_PRE_PACKED_STRUCT struct dot11_action_measure {
+	uint8   category;
+	uint8   action;
+	uint8   token;
+	uint8   data[1];
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_ACTION_MEASURE_LEN    3   
+
+BWL_PRE_PACKED_STRUCT struct dot11_action_ht_ch_width {
+	uint8   category;
+	uint8   action;
+	uint8   ch_width;
+} BWL_POST_PACKED_STRUCT;
+
+BWL_PRE_PACKED_STRUCT struct dot11_action_ht_mimops {
+	uint8   category;
+	uint8   action;
+	uint8   control;
+} BWL_POST_PACKED_STRUCT;
+
+#define SM_PWRSAVE_ENABLE   1
+#define SM_PWRSAVE_MODE     2
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_power_cnst {
+	uint8 id;
+	uint8 len;
+	uint8 power;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_power_cnst dot11_power_cnst_t;
+
+BWL_PRE_PACKED_STRUCT struct dot11_power_cap {
+	uint8 min;
+	uint8 max;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_power_cap dot11_power_cap_t;
+
+BWL_PRE_PACKED_STRUCT struct dot11_tpc_rep {
+	uint8 id;
+	uint8 len;
+	uint8 tx_pwr;
+	uint8 margin;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_tpc_rep dot11_tpc_rep_t;
+#define DOT11_MNG_IE_TPC_REPORT_LEN 2   
+
+BWL_PRE_PACKED_STRUCT struct dot11_supp_channels {
+	uint8 id;
+	uint8 len;
+	uint8 first_channel;
+	uint8 num_channels;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_supp_channels dot11_supp_channels_t;
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_extch {
+	uint8   id;     
+	uint8   len;        
+	uint8   extch;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_extch dot11_extch_ie_t;
+
+BWL_PRE_PACKED_STRUCT struct dot11_brcm_extch {
+	uint8   id;     
+	uint8   len;        
+	uint8   oui[3];     
+	uint8   type;           
+	uint8   extch;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_brcm_extch dot11_brcm_extch_ie_t;
+
+#define BRCM_EXTCH_IE_LEN   5
+#define BRCM_EXTCH_IE_TYPE  53  
+#define DOT11_EXTCH_IE_LEN  1
+#define DOT11_EXT_CH_MASK   0x03    
+#define DOT11_EXT_CH_UPPER  0x01    
+#define DOT11_EXT_CH_LOWER  0x03    
+#define DOT11_EXT_CH_NONE   0x00    
+
+BWL_PRE_PACKED_STRUCT struct dot11_action_frmhdr {
+	uint8   category;
+	uint8   action;
+	uint8   data[1];
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_ACTION_FRMHDR_LEN 2
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_channel_switch {
+	uint8 id;   
+	uint8 len;  
+	uint8 mode; 
+	uint8 channel;  
+	uint8 count;    
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_channel_switch dot11_chan_switch_ie_t;
+
+#define DOT11_SWITCH_IE_LEN 3   
+
+#define DOT11_CSA_MODE_ADVISORY     0   
+#define DOT11_CSA_MODE_NO_TX        1   
+
+BWL_PRE_PACKED_STRUCT struct dot11_action_switch_channel {
+	uint8   category;
+	uint8   action;
+	dot11_chan_switch_ie_t chan_switch_ie;  
+	dot11_brcm_extch_ie_t extch_ie;     
+} BWL_POST_PACKED_STRUCT;
+
+BWL_PRE_PACKED_STRUCT struct dot11_csa_body {
+	uint8 mode; 
+	uint8 reg;  
+	uint8 channel;  
+	uint8 count;    
+} BWL_POST_PACKED_STRUCT;
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_ext_csa {
+	uint8 id;   
+	uint8 len;  
+	struct dot11_csa_body b;    
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_ext_csa dot11_ext_csa_ie_t;
+#define DOT11_EXT_CSA_IE_LEN    4   
+
+BWL_PRE_PACKED_STRUCT struct dot11_action_ext_csa {
+	uint8   category;
+	uint8   action;
+	dot11_ext_csa_ie_t chan_switch_ie;  
+} BWL_POST_PACKED_STRUCT;
+
+BWL_PRE_PACKED_STRUCT struct dot11y_action_ext_csa {
+	uint8   category;
+	uint8   action;
+	struct dot11_csa_body b;    
+} BWL_POST_PACKED_STRUCT;
+
+BWL_PRE_PACKED_STRUCT struct dot11_obss_coex {
+	uint8   id;
+	uint8   len;
+	uint8   info;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_obss_coex dot11_obss_coex_t;
+#define DOT11_OBSS_COEXINFO_LEN 1   
+
+#define DOT11_OBSS_COEX_INFO_REQ        0x01
+#define DOT11_OBSS_COEX_40MHZ_INTOLERANT    0x02
+#define DOT11_OBSS_COEX_20MHZ_WIDTH_REQ 0x04
+
+BWL_PRE_PACKED_STRUCT struct dot11_obss_chanlist {
+	uint8   id;
+	uint8   len;
+	uint8   regclass;
+	uint8   chanlist[1];
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_obss_chanlist dot11_obss_chanlist_t;
+#define DOT11_OBSS_CHANLIST_FIXED_LEN   1   
+
+BWL_PRE_PACKED_STRUCT struct dot11_extcap_ie {
+	uint8 id;
+	uint8 len;
+	uint8 cap[1];
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_extcap_ie dot11_extcap_ie_t;
+#define DOT11_EXTCAP_LEN    1
+#define DOT11_EXTCAP_LEN_TDLS	5
+
+BWL_PRE_PACKED_STRUCT struct dot11_extcap {
+	uint8 extcap[DOT11_EXTCAP_LEN_TDLS];
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_extcap dot11_extcap_t;
+
+
+#define TDLS_CAP_TDLS			37		
+#define TDLS_CAP_PU_BUFFER_STA	28		
+#define TDLS_CAP_PEER_PSM		20		
+#define TDLS_CAP_CH_SW			30		
+#define TDLS_CAP_PROH			38		
+#define TDLS_CAP_CH_SW_PROH		39		
+
+#define TDLS_CAP_MAX_BIT		39		
+
+
+
+#define DOT11_MEASURE_TYPE_BASIC    0   
+#define DOT11_MEASURE_TYPE_CCA      1   
+#define DOT11_MEASURE_TYPE_RPI      2   
+#define DOT11_MEASURE_TYPE_CHLOAD       3   
+#define DOT11_MEASURE_TYPE_NOISE        4   
+#define DOT11_MEASURE_TYPE_BEACON       5   
+#define DOT11_MEASURE_TYPE_FRAME    6   
+#define DOT11_MEASURE_TYPE_STATS        7   
+#define DOT11_MEASURE_TYPE_LCI      8   
+#define DOT11_MEASURE_TYPE_TXSTREAM     9   
+#define DOT11_MEASURE_TYPE_PAUSE        255 
+
+
+#define DOT11_MEASURE_MODE_PARALLEL     (1<<0)  
+#define DOT11_MEASURE_MODE_ENABLE   (1<<1)  
+#define DOT11_MEASURE_MODE_REQUEST  (1<<2)  
+#define DOT11_MEASURE_MODE_REPORT   (1<<3)  
+#define DOT11_MEASURE_MODE_DUR  (1<<4)  
+
+#define DOT11_MEASURE_MODE_LATE     (1<<0)  
+#define DOT11_MEASURE_MODE_INCAPABLE    (1<<1)  
+#define DOT11_MEASURE_MODE_REFUSED  (1<<2)  
+
+#define DOT11_MEASURE_BASIC_MAP_BSS ((uint8)(1<<0)) 
+#define DOT11_MEASURE_BASIC_MAP_OFDM    ((uint8)(1<<1)) 
+#define DOT11_MEASURE_BASIC_MAP_UKNOWN  ((uint8)(1<<2)) 
+#define DOT11_MEASURE_BASIC_MAP_RADAR   ((uint8)(1<<3)) 
+#define DOT11_MEASURE_BASIC_MAP_UNMEAS  ((uint8)(1<<4)) 
+
+BWL_PRE_PACKED_STRUCT struct dot11_meas_req {
+	uint8 id;
+	uint8 len;
+	uint8 token;
+	uint8 mode;
+	uint8 type;
+	uint8 channel;
+	uint8 start_time[8];
+	uint16 duration;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_meas_req dot11_meas_req_t;
+#define DOT11_MNG_IE_MREQ_LEN 14    
+
+#define DOT11_MNG_IE_MREQ_FIXED_LEN 3   
+
+BWL_PRE_PACKED_STRUCT struct dot11_meas_rep {
+	uint8 id;
+	uint8 len;
+	uint8 token;
+	uint8 mode;
+	uint8 type;
+	BWL_PRE_PACKED_STRUCT union
+	{
+		BWL_PRE_PACKED_STRUCT struct {
+			uint8 channel;
+			uint8 start_time[8];
+			uint16 duration;
+			uint8 map;
+		} BWL_POST_PACKED_STRUCT basic;
+		uint8 data[1];
+	} BWL_POST_PACKED_STRUCT rep;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_meas_rep dot11_meas_rep_t;
+
+
+#define DOT11_MNG_IE_MREP_FIXED_LEN 3   
+
+BWL_PRE_PACKED_STRUCT struct dot11_meas_rep_basic {
+	uint8 channel;
+	uint8 start_time[8];
+	uint16 duration;
+	uint8 map;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_meas_rep_basic dot11_meas_rep_basic_t;
+#define DOT11_MEASURE_BASIC_REP_LEN 12  
+
+BWL_PRE_PACKED_STRUCT struct dot11_quiet {
+	uint8 id;
+	uint8 len;
+	uint8 count;    
+	uint8 period;   
+	uint16 duration;    
+	uint16 offset;  
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_quiet dot11_quiet_t;
+
+BWL_PRE_PACKED_STRUCT struct chan_map_tuple {
+	uint8 channel;
+	uint8 map;
+} BWL_POST_PACKED_STRUCT;
+typedef struct chan_map_tuple chan_map_tuple_t;
+
+BWL_PRE_PACKED_STRUCT struct dot11_ibss_dfs {
+	uint8 id;
+	uint8 len;
+	uint8 eaddr[ETHER_ADDR_LEN];
+	uint8 interval;
+	chan_map_tuple_t map[1];
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_ibss_dfs dot11_ibss_dfs_t;
+
+
+#define WME_OUI         "\x00\x50\xf2"  
+#define WME_OUI_LEN     3
+#define WME_OUI_TYPE        2   
+#define WME_VER         1   
+#define WME_TYPE        2   
+#define WME_SUBTYPE_IE      0   
+#define WME_SUBTYPE_PARAM_IE    1   
+#define WME_SUBTYPE_TSPEC   2   
+#define WME_VERSION_LEN	1
+#define WME_PARAMETER_IE_LEN		24
+
+
+
+#define AC_BE           0   
+#define AC_BK           1   
+#define AC_VI           2   
+#define AC_VO           3   
+#define AC_COUNT        4   
+
+typedef uint8 ac_bitmap_t;  
+
+#define AC_BITMAP_NONE      0x0 
+#define AC_BITMAP_ALL       0xf 
+#define AC_BITMAP_TST(ab, ac)   (((ab) & (1 << (ac))) != 0)
+#define AC_BITMAP_SET(ab, ac)   (((ab) |= (1 << (ac))))
+#define AC_BITMAP_RESET(ab, ac) (((ab) &= ~(1 << (ac))))
+
+
+BWL_PRE_PACKED_STRUCT struct wme_ie {
+	uint8 oui[3];
+	uint8 type;
+	uint8 subtype;
+	uint8 version;
+	uint8 qosinfo;
+} BWL_POST_PACKED_STRUCT;
+typedef struct wme_ie wme_ie_t;
+#define WME_IE_LEN 7    
+
+BWL_PRE_PACKED_STRUCT struct edcf_acparam {
+	uint8   ACI;
+	uint8   ECW;
+	uint16  TXOP;       
+} BWL_POST_PACKED_STRUCT;
+typedef struct edcf_acparam edcf_acparam_t;
+
+
+BWL_PRE_PACKED_STRUCT struct wme_param_ie {
+	uint8 oui[3];
+	uint8 type;
+	uint8 subtype;
+	uint8 version;
+	uint8 qosinfo;
+	uint8 rsvd;
+	edcf_acparam_t acparam[AC_COUNT];
+} BWL_POST_PACKED_STRUCT;
+typedef struct wme_param_ie wme_param_ie_t;
+#define WME_PARAM_IE_LEN            24          
+
+
+#define WME_QI_AP_APSD_MASK         0x80        
+#define WME_QI_AP_APSD_SHIFT        7           
+#define WME_QI_AP_COUNT_MASK        0x0f        
+#define WME_QI_AP_COUNT_SHIFT       0           
+
+
+#define WME_QI_STA_MAXSPLEN_MASK    0x60        
+#define WME_QI_STA_MAXSPLEN_SHIFT   5           
+#define WME_QI_STA_APSD_ALL_MASK    0xf         
+#define WME_QI_STA_APSD_ALL_SHIFT   0           
+#define WME_QI_STA_APSD_BE_MASK     0x8         
+#define WME_QI_STA_APSD_BE_SHIFT    3           
+#define WME_QI_STA_APSD_BK_MASK     0x4         
+#define WME_QI_STA_APSD_BK_SHIFT    2           
+#define WME_QI_STA_APSD_VI_MASK     0x2         
+#define WME_QI_STA_APSD_VI_SHIFT    1           
+#define WME_QI_STA_APSD_VO_MASK     0x1         
+#define WME_QI_STA_APSD_VO_SHIFT    0           
+
+
+#define EDCF_AIFSN_MIN               1           
+#define EDCF_AIFSN_MAX               15          
+#define EDCF_AIFSN_MASK              0x0f        
+#define EDCF_ACM_MASK                0x10        
+#define EDCF_ACI_MASK                0x60        
+#define EDCF_ACI_SHIFT               5           
+#define EDCF_AIFSN_SHIFT             12          
+
+
+#define EDCF_ECW_MIN                 0           
+#define EDCF_ECW_MAX                 15          
+#define EDCF_ECW2CW(exp)             ((1 << (exp)) - 1)
+#define EDCF_ECWMIN_MASK             0x0f        
+#define EDCF_ECWMAX_MASK             0xf0        
+#define EDCF_ECWMAX_SHIFT            4           
+
+
+#define EDCF_TXOP_MIN                0           
+#define EDCF_TXOP_MAX                65535       
+#define EDCF_TXOP2USEC(txop)         ((txop) << 5)
+
+
+#define NON_EDCF_AC_BE_ACI_STA          0x02
+
+
+#define EDCF_AC_BE_ACI_STA           0x03   
+#define EDCF_AC_BE_ECW_STA           0xA4   
+#define EDCF_AC_BE_TXOP_STA          0x0000 
+#define EDCF_AC_BK_ACI_STA           0x27   
+#define EDCF_AC_BK_ECW_STA           0xA4   
+#define EDCF_AC_BK_TXOP_STA          0x0000 
+#define EDCF_AC_VI_ACI_STA           0x42   
+#define EDCF_AC_VI_ECW_STA           0x43   
+#define EDCF_AC_VI_TXOP_STA          0x005e 
+#define EDCF_AC_VO_ACI_STA           0x62   
+#define EDCF_AC_VO_ECW_STA           0x32   
+#define EDCF_AC_VO_TXOP_STA          0x002f 
+
+
+#define EDCF_AC_BE_ACI_AP            0x03   
+#define EDCF_AC_BE_ECW_AP            0x64   
+#define EDCF_AC_BE_TXOP_AP           0x0000 
+#define EDCF_AC_BK_ACI_AP            0x27   
+#define EDCF_AC_BK_ECW_AP            0xA4   
+#define EDCF_AC_BK_TXOP_AP           0x0000 
+#define EDCF_AC_VI_ACI_AP            0x41   
+#define EDCF_AC_VI_ECW_AP            0x43   
+#define EDCF_AC_VI_TXOP_AP           0x005e 
+#define EDCF_AC_VO_ACI_AP            0x61   
+#define EDCF_AC_VO_ECW_AP            0x32   
+#define EDCF_AC_VO_TXOP_AP           0x002f 
+
+
+BWL_PRE_PACKED_STRUCT struct edca_param_ie {
+	uint8 qosinfo;
+	uint8 rsvd;
+	edcf_acparam_t acparam[AC_COUNT];
+} BWL_POST_PACKED_STRUCT;
+typedef struct edca_param_ie edca_param_ie_t;
+#define EDCA_PARAM_IE_LEN            18          
+
+
+BWL_PRE_PACKED_STRUCT struct qos_cap_ie {
+	uint8 qosinfo;
+} BWL_POST_PACKED_STRUCT;
+typedef struct qos_cap_ie qos_cap_ie_t;
+
+BWL_PRE_PACKED_STRUCT struct dot11_qbss_load_ie {
+	uint8 id;           
+	uint8 length;
+	uint16 station_count;       
+	uint8 channel_utilization;  
+	uint16 aac;             
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_qbss_load_ie dot11_qbss_load_ie_t;
+
+
+#define FIXED_MSDU_SIZE 0x8000      
+#define MSDU_SIZE_MASK  0x7fff      
+
+
+
+#define INTEGER_SHIFT   13  
+#define FRACTION_MASK   0x1FFF  
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_management_notification {
+	uint8 category;         
+	uint8 action;
+	uint8 token;
+	uint8 status;
+	uint8 data[1];          
+} BWL_POST_PACKED_STRUCT;
+#define DOT11_MGMT_NOTIFICATION_LEN 4   
+
+
+BWL_PRE_PACKED_STRUCT struct ti_ie {
+	uint8 ti_type;
+	uint32 ti_val;
+} BWL_POST_PACKED_STRUCT;
+typedef struct ti_ie ti_ie_t;
+#define TI_TYPE_REASSOC_DEADLINE	1
+#define TI_TYPE_KEY_LIFETIME		2
+
+
+#define WME_ADDTS_REQUEST   0   
+#define WME_ADDTS_RESPONSE  1   
+#define WME_DELTS_REQUEST   2   
+
+
+#define WME_ADMISSION_ACCEPTED      0   
+#define WME_INVALID_PARAMETERS      1   
+#define WME_ADMISSION_REFUSED       3   
+
+
+#define BCN_PRB_SSID(body) ((char*)(body) + DOT11_BCN_PRB_LEN)
+
+
+#define DOT11_OPEN_SYSTEM   0   
+#define DOT11_SHARED_KEY    1   
+#define DOT11_FAST_BSS		2	
+#define DOT11_CHALLENGE_LEN 128 
+
+
+#define FC_PVER_MASK        0x3 
+#define FC_PVER_SHIFT       0   
+#define FC_TYPE_MASK        0xC 
+#define FC_TYPE_SHIFT       2   
+#define FC_SUBTYPE_MASK     0xF0    
+#define FC_SUBTYPE_SHIFT    4   
+#define FC_TODS         0x100   
+#define FC_TODS_SHIFT       8   
+#define FC_FROMDS       0x200   
+#define FC_FROMDS_SHIFT     9   
+#define FC_MOREFRAG     0x400   
+#define FC_MOREFRAG_SHIFT   10  
+#define FC_RETRY        0x800   
+#define FC_RETRY_SHIFT      11  
+#define FC_PM           0x1000  
+#define FC_PM_SHIFT     12  
+#define FC_MOREDATA     0x2000  
+#define FC_MOREDATA_SHIFT   13  
+#define FC_WEP          0x4000  
+#define FC_WEP_SHIFT        14  
+#define FC_ORDER        0x8000  
+#define FC_ORDER_SHIFT      15  
+
+
+#define SEQNUM_SHIFT        4   
+#define SEQNUM_MAX      0x1000  
+#define FRAGNUM_MASK        0xF 
+
+
+
+
+#define FC_TYPE_MNG     0   
+#define FC_TYPE_CTL     1   
+#define FC_TYPE_DATA        2   
+
+
+#define FC_SUBTYPE_ASSOC_REQ        0   
+#define FC_SUBTYPE_ASSOC_RESP       1   
+#define FC_SUBTYPE_REASSOC_REQ      2   
+#define FC_SUBTYPE_REASSOC_RESP     3   
+#define FC_SUBTYPE_PROBE_REQ        4   
+#define FC_SUBTYPE_PROBE_RESP       5   
+#define FC_SUBTYPE_BEACON       8   
+#define FC_SUBTYPE_ATIM         9   
+#define FC_SUBTYPE_DISASSOC     10  
+#define FC_SUBTYPE_AUTH         11  
+#define FC_SUBTYPE_DEAUTH       12  
+#define FC_SUBTYPE_ACTION       13  
+#define FC_SUBTYPE_ACTION_NOACK     14  
+
+
+#define FC_SUBTYPE_CTL_WRAPPER      7   
+#define FC_SUBTYPE_BLOCKACK_REQ     8   
+#define FC_SUBTYPE_BLOCKACK     9   
+#define FC_SUBTYPE_PS_POLL      10  
+#define FC_SUBTYPE_RTS          11  
+#define FC_SUBTYPE_CTS          12  
+#define FC_SUBTYPE_ACK          13  
+#define FC_SUBTYPE_CF_END       14  
+#define FC_SUBTYPE_CF_END_ACK       15  
+
+
+#define FC_SUBTYPE_DATA         0   
+#define FC_SUBTYPE_DATA_CF_ACK      1   
+#define FC_SUBTYPE_DATA_CF_POLL     2   
+#define FC_SUBTYPE_DATA_CF_ACK_POLL 3   
+#define FC_SUBTYPE_NULL         4   
+#define FC_SUBTYPE_CF_ACK       5   
+#define FC_SUBTYPE_CF_POLL      6   
+#define FC_SUBTYPE_CF_ACK_POLL      7   
+#define FC_SUBTYPE_QOS_DATA     8   
+#define FC_SUBTYPE_QOS_DATA_CF_ACK  9   
+#define FC_SUBTYPE_QOS_DATA_CF_POLL 10  
+#define FC_SUBTYPE_QOS_DATA_CF_ACK_POLL 11  
+#define FC_SUBTYPE_QOS_NULL     12  
+#define FC_SUBTYPE_QOS_CF_POLL      14  
+#define FC_SUBTYPE_QOS_CF_ACK_POLL  15  
+
+
+#define FC_SUBTYPE_ANY_QOS(s)       (((s) & 8) != 0)
+#define FC_SUBTYPE_ANY_NULL(s)      (((s) & 4) != 0)
+#define FC_SUBTYPE_ANY_CF_POLL(s)   (((s) & 2) != 0)
+#define FC_SUBTYPE_ANY_CF_ACK(s)    (((s) & 1) != 0)
+
+
+#define FC_KIND_MASK        (FC_TYPE_MASK | FC_SUBTYPE_MASK)    
+
+#define FC_KIND(t, s)   (((t) << FC_TYPE_SHIFT) | ((s) << FC_SUBTYPE_SHIFT))    
+
+#define FC_SUBTYPE(fc)  (((fc) & FC_SUBTYPE_MASK) >> FC_SUBTYPE_SHIFT)  
+#define FC_TYPE(fc) (((fc) & FC_TYPE_MASK) >> FC_TYPE_SHIFT)    
+
+#define FC_ASSOC_REQ    FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_ASSOC_REQ)  
+#define FC_ASSOC_RESP   FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_ASSOC_RESP) 
+#define FC_REASSOC_REQ  FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_REASSOC_REQ)    
+#define FC_REASSOC_RESP FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_REASSOC_RESP)   
+#define FC_PROBE_REQ    FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_PROBE_REQ)  
+#define FC_PROBE_RESP   FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_PROBE_RESP) 
+#define FC_BEACON   FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_BEACON)     
+#define FC_DISASSOC FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_DISASSOC)   
+#define FC_AUTH     FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_AUTH)       
+#define FC_DEAUTH   FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_DEAUTH)     
+#define FC_ACTION   FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_ACTION)     
+#define FC_ACTION_NOACK FC_KIND(FC_TYPE_MNG, FC_SUBTYPE_ACTION_NOACK)   
+
+#define FC_CTL_WRAPPER  FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_CTL_WRAPPER)    
+#define FC_BLOCKACK_REQ FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_BLOCKACK_REQ)   
+#define FC_BLOCKACK FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_BLOCKACK)   
+#define FC_PS_POLL  FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_PS_POLL)    
+#define FC_RTS      FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_RTS)        
+#define FC_CTS      FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_CTS)        
+#define FC_ACK      FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_ACK)        
+#define FC_CF_END   FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_CF_END)     
+#define FC_CF_END_ACK   FC_KIND(FC_TYPE_CTL, FC_SUBTYPE_CF_END_ACK) 
+
+#define FC_DATA     FC_KIND(FC_TYPE_DATA, FC_SUBTYPE_DATA)      
+#define FC_NULL_DATA    FC_KIND(FC_TYPE_DATA, FC_SUBTYPE_NULL)      
+#define FC_DATA_CF_ACK  FC_KIND(FC_TYPE_DATA, FC_SUBTYPE_DATA_CF_ACK)   
+#define FC_QOS_DATA FC_KIND(FC_TYPE_DATA, FC_SUBTYPE_QOS_DATA)  
+#define FC_QOS_NULL FC_KIND(FC_TYPE_DATA, FC_SUBTYPE_QOS_NULL)  
+
+
+
+
+#define QOS_PRIO_SHIFT      0   
+#define QOS_PRIO_MASK       0x0007  
+#define QOS_PRIO(qos)       (((qos) & QOS_PRIO_MASK) >> QOS_PRIO_SHIFT) 
+
+
+#define QOS_TID_SHIFT       0   
+#define QOS_TID_MASK        0x000f  
+#define QOS_TID(qos)        (((qos) & QOS_TID_MASK) >> QOS_TID_SHIFT)   
+
+
+#define QOS_EOSP_SHIFT      4   
+#define QOS_EOSP_MASK       0x0010  
+#define QOS_EOSP(qos)       (((qos) & QOS_EOSP_MASK) >> QOS_EOSP_SHIFT) 
+
+
+#define QOS_ACK_NORMAL_ACK  0   
+#define QOS_ACK_NO_ACK      1   
+#define QOS_ACK_NO_EXP_ACK  2   
+#define QOS_ACK_BLOCK_ACK   3   
+#define QOS_ACK_SHIFT       5   
+#define QOS_ACK_MASK        0x0060  
+#define QOS_ACK(qos)        (((qos) & QOS_ACK_MASK) >> QOS_ACK_SHIFT)   
+
+
+#define QOS_AMSDU_SHIFT     7   
+#define QOS_AMSDU_MASK      0x0080  
+
+
+
+
+
+
+#define DOT11_MNG_AUTH_ALGO_LEN     2   
+#define DOT11_MNG_AUTH_SEQ_LEN      2   
+#define DOT11_MNG_BEACON_INT_LEN    2   
+#define DOT11_MNG_CAP_LEN       2   
+#define DOT11_MNG_AP_ADDR_LEN       6   
+#define DOT11_MNG_LISTEN_INT_LEN    2   
+#define DOT11_MNG_REASON_LEN        2   
+#define DOT11_MNG_AID_LEN       2   
+#define DOT11_MNG_STATUS_LEN        2   
+#define DOT11_MNG_TIMESTAMP_LEN     8   
+
+
+#define DOT11_AID_MASK          0x3fff  
+
+
+#define DOT11_RC_RESERVED       0   
+#define DOT11_RC_UNSPECIFIED        1   
+#define DOT11_RC_AUTH_INVAL     2   
+#define DOT11_RC_DEAUTH_LEAVING     3   
+#define DOT11_RC_INACTIVITY     4   
+#define DOT11_RC_BUSY           5   
+#define DOT11_RC_INVAL_CLASS_2      6   
+#define DOT11_RC_INVAL_CLASS_3      7   
+#define DOT11_RC_DISASSOC_LEAVING   8   
+#define DOT11_RC_NOT_AUTH       9   
+#define DOT11_RC_BAD_PC         10  
+#define DOT11_RC_BAD_CHANNELS       11  
+
+
+
+#define DOT11_RC_UNSPECIFIED_QOS    32  
+#define DOT11_RC_INSUFFCIENT_BW     33  
+#define DOT11_RC_EXCESSIVE_FRAMES   34  
+#define DOT11_RC_TX_OUTSIDE_TXOP    35  
+#define DOT11_RC_LEAVING_QBSS       36  
+#define DOT11_RC_BAD_MECHANISM      37  
+#define DOT11_RC_SETUP_NEEDED       38  
+#define DOT11_RC_TIMEOUT        39  
+
+#define DOT11_RC_MAX            23  
+
+#define DOT11_RC_TDLS_PEER_UNREACH	25
+#define DOT11_RC_TDLS_DOWN_UNSPECIFIED	26
+
+
+#define DOT11_SC_SUCCESS        0   
+#define DOT11_SC_FAILURE        1   
+#define DOT11_SC_TDLS_WAKEUP_SCH_ALT 2	
+					
+#define DOT11_SC_TDLS_WAKEUP_SCH_REJ 3	
+#define DOT11_SC_TDLS_SEC_DISABLED	5	
+#define DOT11_SC_LIFETIME_REJ		6	
+#define DOT11_SC_NOT_SAME_BSS		7	
+#define DOT11_SC_CAP_MISMATCH       10  
+#define DOT11_SC_REASSOC_FAIL       11  
+#define DOT11_SC_ASSOC_FAIL     12  
+#define DOT11_SC_AUTH_MISMATCH      13  
+#define DOT11_SC_AUTH_SEQ       14  
+#define DOT11_SC_AUTH_CHALLENGE_FAIL    15  
+#define DOT11_SC_AUTH_TIMEOUT       16  
+#define DOT11_SC_ASSOC_BUSY_FAIL    17  
+#define DOT11_SC_ASSOC_RATE_MISMATCH    18  
+#define DOT11_SC_ASSOC_SHORT_REQUIRED   19  
+#define DOT11_SC_ASSOC_PBCC_REQUIRED    20  
+#define DOT11_SC_ASSOC_AGILITY_REQUIRED 21  
+#define DOT11_SC_ASSOC_SPECTRUM_REQUIRED    22  
+#define DOT11_SC_ASSOC_BAD_POWER_CAP    23  
+#define DOT11_SC_ASSOC_BAD_SUP_CHANNELS 24  
+#define DOT11_SC_ASSOC_SHORTSLOT_REQUIRED   25  
+#define DOT11_SC_ASSOC_ERPBCC_REQUIRED  26  
+#define DOT11_SC_ASSOC_DSSOFDM_REQUIRED 27  
+#define DOT11_SC_ASSOC_R0KH_UNREACHABLE	28	
+#define DOT11_SC_ASSOC_TRY_LATER	30	
+#define DOT11_SC_ASSOC_MFP_VIOLATION	31	
+
+#define	DOT11_SC_DECLINED		37	
+#define	DOT11_SC_INVALID_PARAMS		38	
+#define DOT11_SC_INVALID_PAIRWISE_CIPHER	42 
+#define	DOT11_SC_INVALID_AKMP		43	
+#define DOT11_SC_INVALID_RSNIE_CAP	45	
+#define	DOT11_SC_INVALID_PMKID		53	
+#define	DOT11_SC_INVALID_MDID		54	
+#define	DOT11_SC_INVALID_FTIE		55	
+
+#define DOT11_SC_UNEXP_MSG			70	
+#define DOT11_SC_INVALID_SNONCE		71	
+#define DOT11_SC_INVALID_RSNIE		72	
+
+#define DOT11_MNG_DS_PARAM_LEN          1   
+#define DOT11_MNG_IBSS_PARAM_LEN        2   
+
+
+#define DOT11_MNG_TIM_FIXED_LEN         3   
+#define DOT11_MNG_TIM_DTIM_COUNT        0   
+#define DOT11_MNG_TIM_DTIM_PERIOD       1   
+#define DOT11_MNG_TIM_BITMAP_CTL        2   
+#define DOT11_MNG_TIM_PVB           3   
+
+
+#define TLV_TAG_OFF     0   
+#define TLV_LEN_OFF     1   
+#define TLV_HDR_LEN     2   
+#define TLV_BODY_OFF        2   
+
+
+#define DOT11_MNG_SSID_ID           0   
+#define DOT11_MNG_RATES_ID          1   
+#define DOT11_MNG_FH_PARMS_ID           2   
+#define DOT11_MNG_DS_PARMS_ID           3   
+#define DOT11_MNG_CF_PARMS_ID           4   
+#define DOT11_MNG_TIM_ID            5   
+#define DOT11_MNG_IBSS_PARMS_ID         6   
+#define DOT11_MNG_COUNTRY_ID            7   
+#define DOT11_MNG_HOPPING_PARMS_ID      8   
+#define DOT11_MNG_HOPPING_TABLE_ID      9   
+#define DOT11_MNG_REQUEST_ID            10  
+#define DOT11_MNG_QBSS_LOAD_ID          11  
+#define DOT11_MNG_EDCA_PARAM_ID         12  
+#define DOT11_MNG_CHALLENGE_ID          16  
+#define DOT11_MNG_PWR_CONSTRAINT_ID     32  
+#define DOT11_MNG_PWR_CAP_ID            33  
+#define DOT11_MNG_TPC_REQUEST_ID        34  
+#define DOT11_MNG_TPC_REPORT_ID         35  
+#define DOT11_MNG_SUPP_CHANNELS_ID      36  
+#define DOT11_MNG_CHANNEL_SWITCH_ID     37  
+#define DOT11_MNG_MEASURE_REQUEST_ID        38  
+#define DOT11_MNG_MEASURE_REPORT_ID     39  
+#define DOT11_MNG_QUIET_ID          40  
+#define DOT11_MNG_IBSS_DFS_ID           41  
+#define DOT11_MNG_ERP_ID            42  
+#define DOT11_MNG_TS_DELAY_ID           43  
+#define DOT11_MNG_HT_CAP            45  
+#define DOT11_MNG_QOS_CAP_ID            46  
+#define DOT11_MNG_NONERP_ID         47  
+#define DOT11_MNG_RSN_ID            48  
+#define DOT11_MNG_EXT_RATES_ID          50  
+#define DOT11_MNG_AP_CHREP_ID       51  
+#define DOT11_MNG_NBR_REP_ID        52  
+#define DOT11_MNG_MDIE_ID       54  
+#define DOT11_MNG_FTIE_ID       55  
+#define DOT11_MNG_FT_TI_ID      56  
+#define DOT11_MNG_RDE_ID			57	
+#define DOT11_MNG_REGCLASS_ID           59  
+#define DOT11_MNG_EXT_CSA_ID            60  
+#define DOT11_MNG_HT_ADD            61  
+#define DOT11_MNG_EXT_CHANNEL_OFFSET        62  
+
+
+#define DOT11_MNG_RRM_CAP_ID        70  
+#define DOT11_MNG_HT_BSS_COEXINFO_ID        72  
+#define DOT11_MNG_HT_BSS_CHANNEL_REPORT_ID  73  
+#define DOT11_MNG_HT_OBSS_ID            74  
+#define DOT11_MNG_CHANNEL_USAGE			97 
+#define DOT11_MNG_LINK_IDENTIFIER_ID	101	
+#define DOT11_MNG_WAKEUP_SCHEDULE_ID	102 
+#define DOT11_MNG_CHANNEL_SWITCH_TIMING_ID	104 
+#define DOT11_MNG_PTI_CONTROL_ID		105	
+#define DOT11_MNG_PU_BUFFER_STATUS_ID	106	
+#define DOT11_MNG_EXT_CAP_ID           127 
+#define DOT11_MNG_WPA_ID            221 
+#define DOT11_MNG_PROPR_ID          221 
+
+#define DOT11_MNG_VS_ID             221 
+
+
+#define DOT11_RATE_BASIC            0x80    
+#define DOT11_RATE_MASK             0x7F    
+
+
+#define DOT11_MNG_ERP_LEN           1   
+#define DOT11_MNG_NONERP_PRESENT        0x01    
+#define DOT11_MNG_USE_PROTECTION        0x02    
+#define DOT11_MNG_BARKER_PREAMBLE       0x04    
+
+#define DOT11_MGN_TS_DELAY_LEN      4   
+#define TS_DELAY_FIELD_SIZE         4   
+
+
+#define DOT11_CAP_ESS               0x0001  
+#define DOT11_CAP_IBSS              0x0002  
+#define DOT11_CAP_POLLABLE          0x0004  
+#define DOT11_CAP_POLL_RQ           0x0008  
+#define DOT11_CAP_PRIVACY           0x0010  
+#define DOT11_CAP_SHORT             0x0020  
+#define DOT11_CAP_PBCC              0x0040  
+#define DOT11_CAP_AGILITY           0x0080  
+#define DOT11_CAP_SPECTRUM          0x0100  
+#define DOT11_CAP_SHORTSLOT         0x0400  
+#define DOT11_CAP_RRM           0x1000  
+#define DOT11_CAP_CCK_OFDM          0x2000  
+
+
+#define DOT11_OBSS_COEX_MNG_SUPPORT 0x01    
+
+
+#define DOT11_ACTION_HDR_LEN        2   
+#define DOT11_ACTION_CAT_OFF        0   
+#define DOT11_ACTION_ACT_OFF        1   
+
+
+#define DOT11_ACTION_CAT_ERR_MASK   0x80    
+#define DOT11_ACTION_CAT_MASK       0x7F    
+#define DOT11_ACTION_CAT_SPECT_MNG  0   
+#define DOT11_ACTION_CAT_QOS        1   
+#define DOT11_ACTION_CAT_DLS        2   
+#define DOT11_ACTION_CAT_BLOCKACK   3   
+#define DOT11_ACTION_CAT_PUBLIC     4   
+#define DOT11_ACTION_CAT_RRM        5   
+#define DOT11_ACTION_CAT_FBT    6   
+#define DOT11_ACTION_CAT_HT     7   
+#if defined(MFP) || defined(WLFBT) || defined(WLWNM)
+#define	DOT11_ACTION_CAT_SA_QUERY	8	
+#define	DOT11_ACTION_CAT_PDPA		9	
+#define DOT11_ACTION_CAT_BSSMGMT    10  
+#define DOT11_ACTION_NOTIFICATION   17
+#define DOT11_ACTION_CAT_VSP		126	
+#endif 
+#define DOT11_ACTION_NOTIFICATION	17
+#define DOT11_ACTION_CAT_VS     127 
+
+
+#define DOT11_SM_ACTION_M_REQ       0   
+#define DOT11_SM_ACTION_M_REP       1   
+#define DOT11_SM_ACTION_TPC_REQ     2   
+#define DOT11_SM_ACTION_TPC_REP     3   
+#define DOT11_SM_ACTION_CHANNEL_SWITCH  4   
+#define DOT11_SM_ACTION_EXT_CSA     5   
+
+
+#define DOT11_ACTION_ID_HT_CH_WIDTH 0   
+#define DOT11_ACTION_ID_HT_MIMO_PS  1   
+
+
+#define DOT11_PUB_ACTION_BSS_COEX_MNG   0   
+#define DOT11_PUB_ACTION_CHANNEL_SWITCH 4   
+
+
+#define DOT11_BA_ACTION_ADDBA_REQ   0   
+#define DOT11_BA_ACTION_ADDBA_RESP  1   
+#define DOT11_BA_ACTION_DELBA       2   
+
+
+#define DOT11_ADDBA_PARAM_AMSDU_SUP 0x0001  
+#define DOT11_ADDBA_PARAM_POLICY_MASK   0x0002  
+#define DOT11_ADDBA_PARAM_POLICY_SHIFT  1   
+#define DOT11_ADDBA_PARAM_TID_MASK  0x003c  
+#define DOT11_ADDBA_PARAM_TID_SHIFT 2   
+#define DOT11_ADDBA_PARAM_BSIZE_MASK    0xffc0  
+#define DOT11_ADDBA_PARAM_BSIZE_SHIFT   6   
+
+#define DOT11_ADDBA_POLICY_DELAYED  0   
+#define DOT11_ADDBA_POLICY_IMMEDIATE    1   
+
+
+#define DOT11_FT_ACTION_FT_RESERVED		0
+#define DOT11_FT_ACTION_FT_REQ			1	
+#define DOT11_FT_ACTION_FT_RES			2	
+#define DOT11_FT_ACTION_FT_CON			3	
+#define DOT11_FT_ACTION_FT_ACK			4	
+
+
+
+#define DOT11_WNM_ACTION_EVENT_REQ			0
+#define DOT11_WNM_ACTION_EVENT_REP			1
+#define DOT11_WNM_ACTION_DIAG_REQ			2
+#define DOT11_WNM_ACTION_DIAG_REP			3
+#define DOT11_WNM_ACTION_LOC_CFG_REQ		4
+#define DOT11_WNM_ACTION_LOC_RFG_RESP		5
+#define DOT11_WNM_ACTION_BSS_TRANS_QURY		6
+#define DOT11_WNM_ACTION_BSS_TRANS_REQ		7
+#define DOT11_WNM_ACTION_BSS_TRANS_RESP		8
+#define DOT11_WNM_ACTION_FMS_REQ			9
+#define DOT11_WNM_ACTION_FMS_RESP			10
+#define DOT11_WNM_ACTION_COL_INTRFRNCE_REQ	11
+#define DOT11_WNM_ACTION_COL_INTRFRNCE_REP	12
+#define DOT11_WNM_ACTION_TFS_REQ			13
+#define DOT11_WNM_ACTION_TFS_RESP			14
+#define DOT11_WNM_ACTION_TFS_NOTIFY			15
+#define DOT11_WNM_ACTION_WNM_SLEEP_REQ		16
+#define DOT11_WNM_ACTION_WNM_SLEEP_RESP		17
+#define DOT11_WNM_ACTION_TIM_BCAST_REQ		18
+#define DOT11_WNM_ACTION_TIM_BCAST_RESP		19
+#define DOT11_WNM_ACTION_QOS_TRFC_CAP_UPD	20
+#define DOT11_WNM_ACTION_CHAN_USAGE_REQ		21
+#define DOT11_WNM_ACTION_CHAN_USAGE_RESP	22
+#define DOT11_WNM_ACTION_DMS_REQ			23
+#define DOT11_WNM_ACTION_DMS_RESP			24
+#define DOT11_WNM_ACTION_TMNG_MEASUR_REQ	25
+#define DOT11_WNM_ACTION_NOTFCTN_REQ		26
+#define DOT11_WNM_ACTION_NOTFCTN_RES		27
+
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_bss_trans_query {
+	uint8 category;				
+	uint8 action;				
+	uint8 token;				
+	uint8 reason;				
+	uint8 data[1];				
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_bss_trans_query dot11_bss_trans_query_t;
+#define DOT11_BSS_TRANS_QUERY_LEN 4	
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_bss_trans_req {
+	uint8 category;				
+	uint8 action;				
+	uint8 token;				
+	uint8 reqmode;				
+	uint16 disassoc_tmr;		
+	uint8 validity_intrvl;		
+	uint8 data[1];				
+								
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_bss_trans_req dot11_bss_trans_req_t;
+#define DOT11_BSS_TRANS_REQ_LEN 7	
+
+#define DOT11_BSS_TERM_DUR_LEN 12	
+
+
+
+#define DOT11_BSS_TRNS_REQMODE_PREF_LIST_INCL		0x01
+#define DOT11_BSS_TRNS_REQMODE_ABRIDGED				0x02
+#define DOT11_BSS_TRNS_REQMODE_DISASSOC_IMMINENT	0x04
+#define DOT11_BSS_TRNS_REQMODE_BSS_TERM_INCL		0x08
+#define DOT11_BSS_TRNS_REQMODE_ESS_DISASSOC_IMNT	0x10
+
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_bss_trans_res {
+	uint8 category;				
+	uint8 action;				
+	uint8 token;				
+	uint8 status;				
+	uint8 term_delay;			
+	uint8 data[1];				
+								
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_bss_trans_res dot11_bss_trans_res_t;
+#define DOT11_BSS_TRANS_RES_LEN 5	
+
+
+#define DOT11_BSS_TRNS_RES_STATUS_ACCEPT				0
+#define DOT11_BSS_TRNS_RES_STATUS_REJECT				1
+#define DOT11_BSS_TRNS_RES_STATUS_REJ_INSUFF_BCN		2
+#define DOT11_BSS_TRNS_RES_STATUS_REJ_INSUFF_CAP		3
+#define DOT11_BSS_TRNS_RES_STATUS_REJ_TERM_UNDESIRED	4
+#define DOT11_BSS_TRNS_RES_STATUS_REJ_TERM_DELAY_REQ	5
+#define DOT11_BSS_TRNS_RES_STATUS_REJ_BSS_LIST_PROVIDED	6
+#define DOT11_BSS_TRNS_RES_STATUS_REJ_NO_SUITABLE_BSS	7
+#define DOT11_BSS_TRNS_RES_STATUS_REJ_LEAVING_ESS		8
+
+
+
+#define DOT11_NBR_RPRT_BSSID_INFO_REACHABILTY		0x0003
+#define DOT11_NBR_RPRT_BSSID_INFO_SEC				0x0004
+#define DOT11_NBR_RPRT_BSSID_INFO_KEY_SCOPE			0x0008
+#define DOT11_NBR_RPRT_BSSID_INFO_CAP				0x03f0
+
+#define DOT11_NBR_RPRT_BSSID_INFO_CAP_SPEC_MGMT		0x0010
+#define DOT11_NBR_RPRT_BSSID_INFO_CAP_QOS			0x0020
+#define DOT11_NBR_RPRT_BSSID_INFO_CAP_APSD			0x0040
+#define DOT11_NBR_RPRT_BSSID_INFO_CAP_RDIO_MSMT		0x0080
+#define DOT11_NBR_RPRT_BSSID_INFO_CAP_DEL_BA		0x0100
+#define DOT11_NBR_RPRT_BSSID_INFO_CAP_IMM_BA		0x0200
+
+
+#define DOT11_NBR_RPRT_SUBELEM_BSS_CANDDT_PREF_ID	3
+BWL_PRE_PACKED_STRUCT struct dot11_addba_req {
+	uint8 category;             
+	uint8 action;               
+	uint8 token;                
+	uint16 addba_param_set;         
+	uint16 timeout;             
+	uint16 start_seqnum;            
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_addba_req dot11_addba_req_t;
+#define DOT11_ADDBA_REQ_LEN     9   
+
+BWL_PRE_PACKED_STRUCT struct dot11_addba_resp {
+	uint8 category;             
+	uint8 action;               
+	uint8 token;                
+	uint16 status;              
+	uint16 addba_param_set;         
+	uint16 timeout;             
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_addba_resp dot11_addba_resp_t;
+#define DOT11_ADDBA_RESP_LEN        9   
+
+
+#define DOT11_DELBA_PARAM_INIT_MASK 0x0800  
+#define DOT11_DELBA_PARAM_INIT_SHIFT    11  
+#define DOT11_DELBA_PARAM_TID_MASK  0xf000  
+#define DOT11_DELBA_PARAM_TID_SHIFT 12  
+
+BWL_PRE_PACKED_STRUCT struct dot11_delba {
+	uint8 category;             
+	uint8 action;               
+	uint16 delba_param_set;         
+	uint16 reason;              
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_delba dot11_delba_t;
+#define DOT11_DELBA_LEN         6   
+
+
+#define SA_QUERY_REQUEST		0
+#define SA_QUERY_RESPONSE		1
+
+
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_ft_req {
+	uint8 category;			
+	uint8 action;			
+	uint8 sta_addr[ETHER_ADDR_LEN];
+	uint8 tgt_ap_addr[ETHER_ADDR_LEN];
+	uint8 data[1];			
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_ft_req dot11_ft_req_t;
+#define DOT11_FT_REQ_FIXED_LEN 14
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_ft_res {
+	uint8 category;			
+	uint8 action;			
+	uint8 sta_addr[ETHER_ADDR_LEN];
+	uint8 tgt_ap_addr[ETHER_ADDR_LEN];
+	uint16 status;			
+	uint8 data[1];			
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_ft_res dot11_ft_res_t;
+#define DOT11_FT_RES_FIXED_LEN 16
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_rde_ie {
+	uint8 id; 			
+	uint8 length;
+	uint8 rde_id;			
+	uint8 rd_count;			
+	uint16 status;			
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_rde_ie dot11_rde_ie_t;
+
+
+#define DOT11_MNG_RDE_IE_LEN sizeof(dot11_rde_ie_t)
+
+
+
+
+
+#define DOT11_RRM_CAP_LEN       5   
+BWL_PRE_PACKED_STRUCT struct dot11_rrm_cap_ie {
+	uint8 cap[DOT11_RRM_CAP_LEN];
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_rrm_cap_ie dot11_rrm_cap_ie_t;
+
+
+#define DOT11_RRM_CAP_LINK          0
+#define DOT11_RRM_CAP_NEIGHBOR_REPORT   1
+#define DOT11_RRM_CAP_PARALLEL      2
+#define DOT11_RRM_CAP_REPEATED      3
+#define DOT11_RRM_CAP_BCN_PASSIVE   4
+#define DOT11_RRM_CAP_BCN_ACTIVE    5
+#define DOT11_RRM_CAP_BCN_TABLE     6
+#define DOT11_RRM_CAP_BCN_REP_COND  7
+#define DOT11_RRM_CAP_AP_CHANREP    16
+
+
+
+#define DOT11_EXT_CAP_LEN		4	
+BWL_PRE_PACKED_STRUCT struct dot11_ext_cap_ie {
+	uint8 cap[DOT11_EXT_CAP_LEN];
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_ext_cap_ie dot11_ext_cap_ie_t;
+
+
+#define DOT11_EXT_CAP_BSS_TRANSITION_MGMT	19
+
+
+#define DOT11_OP_CLASS_NONE			255
+
+BWL_PRE_PACKED_STRUCT struct do11_ap_chrep {
+	uint8 id;
+	uint8 len;
+	uint8 reg;
+	uint8 chanlist[1];
+} BWL_POST_PACKED_STRUCT;
+typedef struct do11_ap_chrep dot11_ap_chrep_t;
+
+
+#define DOT11_RM_ACTION_RM_REQ      0   
+#define DOT11_RM_ACTION_RM_REP      1   
+#define DOT11_RM_ACTION_LM_REQ      2   
+#define DOT11_RM_ACTION_LM_REP      3   
+#define DOT11_RM_ACTION_NR_REQ      4   
+#define DOT11_RM_ACTION_NR_REP      5   
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_rm_action {
+	uint8 category;             
+	uint8 action;               
+	uint8 token;                
+	uint8 data[1];
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_rm_action dot11_rm_action_t;
+#define DOT11_RM_ACTION_LEN 3
+
+BWL_PRE_PACKED_STRUCT struct dot11_rmreq {
+	uint8 category;             
+	uint8 action;               
+	uint8 token;                
+	uint16 reps;                
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_rmreq dot11_rmreq_t;
+#define DOT11_RMREQ_LEN 5
+
+BWL_PRE_PACKED_STRUCT struct dot11_rm_ie {
+	uint8 id;
+	uint8 len;
+	uint8 token;
+	uint8 mode;
+	uint8 type;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_rm_ie dot11_rm_ie_t;
+#define DOT11_RM_IE_LEN 5
+
+
+#define DOT11_RMREQ_MODE_PARALLEL   1
+#define DOT11_RMREQ_MODE_ENABLE     2
+#define DOT11_RMREQ_MODE_REQUEST    4
+#define DOT11_RMREQ_MODE_REPORT     8
+#define DOT11_RMREQ_MODE_DURMAND    0x10    
+
+
+#define DOT11_RMREP_MODE_LATE       1
+#define DOT11_RMREP_MODE_INCAPABLE  2
+#define DOT11_RMREP_MODE_REFUSED    4
+
+BWL_PRE_PACKED_STRUCT struct dot11_rmreq_bcn {
+	uint8 id;
+	uint8 len;
+	uint8 token;
+	uint8 mode;
+	uint8 type;
+	uint8 reg;
+	uint8 channel;
+	uint16 interval;
+	uint16 duration;
+	uint8 bcn_mode;
+	struct ether_addr   bssid;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_rmreq_bcn dot11_rmreq_bcn_t;
+#define DOT11_RMREQ_BCN_LEN 18
+
+BWL_PRE_PACKED_STRUCT struct dot11_rmrep_bcn {
+	uint8 reg;
+	uint8 channel;
+	uint32 starttime[2];
+	uint16 duration;
+	uint8 frame_info;
+	uint8 rcpi;
+	uint8 rsni;
+	struct ether_addr   bssid;
+	uint8 antenna_id;
+	uint32 parent_tsf;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_rmrep_bcn dot11_rmrep_bcn_t;
+#define DOT11_RMREP_BCN_LEN 26
+
+
+#define DOT11_RMREQ_BCN_PASSIVE 0
+#define DOT11_RMREQ_BCN_ACTIVE  1
+#define DOT11_RMREQ_BCN_TABLE   2
+
+
+#define DOT11_RMREQ_BCN_SSID_ID 0
+#define DOT11_RMREQ_BCN_REPINFO_ID  1
+#define DOT11_RMREQ_BCN_REPDET_ID   2
+#define DOT11_RMREQ_BCN_REQUEST_ID  10
+#define DOT11_RMREQ_BCN_APCHREP_ID  DOT11_MNG_AP_CHREP_ID
+
+
+#define DOT11_RMREQ_BCN_REPDET_FIXED    0   
+#define DOT11_RMREQ_BCN_REPDET_REQUEST  1   
+#define DOT11_RMREQ_BCN_REPDET_ALL  2   
+
+
+#define DOT11_RMREP_BCN_FRM_BODY    1
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_rmrep_nbr {
+	struct ether_addr   bssid;
+	uint32  bssid_info;
+	uint8 reg;
+	uint8 channel;
+	uint8 phytype;
+	uchar sub_elements[1]; 	
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_rmrep_nbr dot11_rmrep_nbr_t;
+#define DOT11_RMREP_NBR_LEN 13
+
+
+#define DOT11_BSSTYPE_INFRASTRUCTURE        0   
+#define DOT11_BSSTYPE_INDEPENDENT       1   
+#define DOT11_BSSTYPE_ANY           2   
+#define DOT11_SCANTYPE_ACTIVE           0   
+#define DOT11_SCANTYPE_PASSIVE          1   
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_lmreq {
+	uint8 category;             
+	uint8 action;               
+	uint8 token;                
+	uint8 txpwr;                
+	uint8 maxtxpwr;             
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_lmreq dot11_lmreq_t;
+#define DOT11_LMREQ_LEN 5
+
+BWL_PRE_PACKED_STRUCT struct dot11_lmrep {
+	uint8 category;             
+	uint8 action;               
+	uint8 token;                
+	dot11_tpc_rep_t tpc;            
+	uint8 rxant;                
+	uint8 txant;                
+	uint8 rcpi;             
+	uint8 rsni;             
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_lmrep dot11_lmrep_t;
+#define DOT11_LMREP_LEN 11
+
+
+#define PREN_PREAMBLE       24  
+#define PREN_MM_EXT     12  
+#define PREN_PREAMBLE_EXT   4   
+
+
+#define RIFS_11N_TIME       2   
+
+
+
+#define HT_SIG1_MCS_MASK        0x00007F
+#define HT_SIG1_CBW             0x000080
+#define HT_SIG1_HT_LENGTH       0xFFFF00
+
+
+#define HT_SIG2_SMOOTHING       0x000001
+#define HT_SIG2_NOT_SOUNDING    0x000002
+#define HT_SIG2_RESERVED        0x000004
+#define HT_SIG2_AGGREGATION     0x000008
+#define HT_SIG2_STBC_MASK       0x000030
+#define HT_SIG2_STBC_SHIFT      4
+#define HT_SIG2_FEC_CODING      0x000040
+#define HT_SIG2_SHORT_GI        0x000080
+#define HT_SIG2_ESS_MASK        0x000300
+#define HT_SIG2_ESS_SHIFT       8
+#define HT_SIG2_CRC             0x03FC00
+#define HT_SIG2_TAIL            0x1C0000
+
+
+#define APHY_SLOT_TIME      9   
+#define APHY_SIFS_TIME      16  
+#define APHY_DIFS_TIME      (APHY_SIFS_TIME + (2 * APHY_SLOT_TIME)) 
+#define APHY_PREAMBLE_TIME  16  
+#define APHY_SIGNAL_TIME    4   
+#define APHY_SYMBOL_TIME    4   
+#define APHY_SERVICE_NBITS  16  
+#define APHY_TAIL_NBITS     6   
+#define APHY_CWMIN      15  
+
+
+#define BPHY_SLOT_TIME      20  
+#define BPHY_SIFS_TIME      10  
+#define BPHY_DIFS_TIME      50  
+#define BPHY_PLCP_TIME      192 
+#define BPHY_PLCP_SHORT_TIME    96  
+#define BPHY_CWMIN      31  
+
+
+#define DOT11_OFDM_SIGNAL_EXTENSION 6   
+
+#define PHY_CWMAX       1023    
+
+#define DOT11_MAXNUMFRAGS   16  
+
+
+typedef struct d11cnt {
+	uint32      txfrag;     
+	uint32      txmulti;    
+	uint32      txfail;     
+	uint32      txretry;    
+	uint32      txretrie;   
+	uint32      rxdup;      
+	uint32      txrts;      
+	uint32      txnocts;    
+	uint32      txnoack;    
+	uint32      rxfrag;     
+	uint32      rxmulti;    
+	uint32      rxcrc;      
+	uint32      txfrmsnt;   
+	uint32      rxundec;    
+} d11cnt_t;
+
+
+#define BRCM_PROP_OUI       "\x00\x90\x4C"  
+
+
+
+#define BRCM_OUI        "\x00\x10\x18"  
+
+
+BWL_PRE_PACKED_STRUCT struct brcm_ie {
+	uint8   id;     
+	uint8   len;        
+	uint8   oui[3];     
+	uint8   ver;        
+	uint8   assoc;      
+	uint8   flags;      
+	uint8   flags1;     
+	uint16  amsdu_mtu_pref; 
+} BWL_POST_PACKED_STRUCT;
+typedef struct brcm_ie brcm_ie_t;
+#define BRCM_IE_LEN     11  
+#define BRCM_IE_VER     2   
+#define BRCM_IE_LEGACY_AES_VER  1   
+
+
+#ifdef WLAFTERBURNER
+#define BRF_ABCAP       0x1 
+#define BRF_ABRQRD      0x2 
+#define BRF_ABCOUNTER_MASK  0xf0    
+#define BRF_ABCOUNTER_SHIFT 4   
+#endif 
+#define BRF_LZWDS       0x4 
+#define BRF_BLOCKACK        0x8 
+
+
+#define BRF1_AMSDU      0x1 
+#define BRF1_WMEPS      0x4 
+#define BRF1_PSOFIX     0x8 
+#define BRF1_RX_LARGE_AGG   0x10    
+#define BRF1_SOFTAP             0x40    
+
+#ifdef WLAFTERBURNER
+#define AB_WDS_TIMEOUT_MAX  15  
+#define AB_WDS_TIMEOUT_MIN  1   
+#endif
+
+#define AB_GUARDCOUNT   10      
+
+
+BWL_PRE_PACKED_STRUCT struct vndr_ie {
+	uchar id;
+	uchar len;
+	uchar oui [3];
+	uchar data [1];     
+} BWL_POST_PACKED_STRUCT;
+typedef struct vndr_ie vndr_ie_t;
+
+#define VNDR_IE_HDR_LEN     2   
+#define VNDR_IE_MIN_LEN     3   
+#define VNDR_IE_MAX_LEN     256 
+
+
+#define MCSSET_LEN  16  
+#define MAX_MCS_NUM (128)   
+
+BWL_PRE_PACKED_STRUCT struct ht_cap_ie {
+	uint16  cap;
+	uint8   params;
+	uint8   supp_mcs[MCSSET_LEN];
+	uint16  ext_htcap;
+	uint32  txbf_cap;
+	uint8   as_cap;
+} BWL_POST_PACKED_STRUCT;
+typedef struct ht_cap_ie ht_cap_ie_t;
+
+
+
+BWL_PRE_PACKED_STRUCT struct ht_prop_cap_ie {
+	uint8   id;     
+	uint8   len;        
+	uint8   oui[3];     
+	uint8   type;           
+	ht_cap_ie_t cap_ie;
+} BWL_POST_PACKED_STRUCT;
+typedef struct ht_prop_cap_ie ht_prop_cap_ie_t;
+
+#define HT_PROP_IE_OVERHEAD 4   
+#define HT_CAP_IE_LEN       26  
+#define HT_CAP_IE_TYPE      51
+
+#define HT_CAP_LDPC_CODING  0x0001  
+#define HT_CAP_40MHZ        0x0002  
+#define HT_CAP_MIMO_PS_MASK 0x000C  
+#define HT_CAP_MIMO_PS_SHIFT    0x0002  
+#define HT_CAP_MIMO_PS_OFF  0x0003  
+#define HT_CAP_MIMO_PS_RTS  0x0001  
+#define HT_CAP_MIMO_PS_ON   0x0000  
+#define HT_CAP_GF       0x0010  
+#define HT_CAP_SHORT_GI_20  0x0020  
+#define HT_CAP_SHORT_GI_40  0x0040  
+#define HT_CAP_TX_STBC      0x0080  
+#define HT_CAP_RX_STBC_MASK 0x0300  
+#define HT_CAP_RX_STBC_SHIFT    8   
+#define HT_CAP_DELAYED_BA   0x0400  
+#define HT_CAP_MAX_AMSDU    0x0800  
+#define HT_CAP_DSSS_CCK 0x1000  
+#define HT_CAP_PSMP     0x2000  
+#define HT_CAP_40MHZ_INTOLERANT 0x4000  
+#define HT_CAP_LSIG_TXOP    0x8000  
+
+#define HT_CAP_RX_STBC_NO       0x0 
+#define HT_CAP_RX_STBC_ONE_STREAM   0x1 
+#define HT_CAP_RX_STBC_TWO_STREAM   0x2 
+#define HT_CAP_RX_STBC_THREE_STREAM 0x3 
+
+#define HT_MAX_AMSDU        7935    
+#define HT_MIN_AMSDU        3835    
+
+#define HT_PARAMS_RX_FACTOR_MASK    0x03    
+#define HT_PARAMS_DENSITY_MASK      0x1C    
+#define HT_PARAMS_DENSITY_SHIFT 2   
+
+
+#define AMPDU_MAX_MPDU_DENSITY  7   
+#define AMPDU_RX_FACTOR_8K  0   
+#define AMPDU_RX_FACTOR_16K 1   
+#define AMPDU_RX_FACTOR_32K 2   
+#define AMPDU_RX_FACTOR_64K 3   
+#define AMPDU_RX_FACTOR_BASE    8*1024  
+
+#define AMPDU_DELIMITER_LEN 4   
+#define AMPDU_DELIMITER_LEN_MAX 63  
+
+BWL_PRE_PACKED_STRUCT struct ht_add_ie {
+	uint8   ctl_ch;         
+	uint8   byte1;          
+	uint16  opmode;         
+	uint16  misc_bits;      
+	uint8   basic_mcs[MCSSET_LEN];  
+} BWL_POST_PACKED_STRUCT;
+typedef struct ht_add_ie ht_add_ie_t;
+
+
+
+BWL_PRE_PACKED_STRUCT struct ht_prop_add_ie {
+	uint8   id;     
+	uint8   len;        
+	uint8   oui[3];     
+	uint8   type;       
+	ht_add_ie_t add_ie;
+} BWL_POST_PACKED_STRUCT;
+typedef struct ht_prop_add_ie ht_prop_add_ie_t;
+
+#define HT_ADD_IE_LEN   22
+#define HT_ADD_IE_TYPE  52
+
+
+#define HT_BW_ANY       0x04    
+#define HT_RIFS_PERMITTED       0x08    
+
+
+#define HT_OPMODE_MASK          0x0003  
+#define HT_OPMODE_SHIFT     0   
+#define HT_OPMODE_PURE      0x0000  
+#define HT_OPMODE_OPTIONAL  0x0001  
+#define HT_OPMODE_HT20IN40  0x0002  
+#define HT_OPMODE_MIXED 0x0003  
+#define HT_OPMODE_NONGF 0x0004  
+#define DOT11N_TXBURST      0x0008  
+#define DOT11N_OBSS_NONHT   0x0010  
+
+
+#define HT_BASIC_STBC_MCS   0x007f  
+#define HT_DUAL_STBC_PROT   0x0080  
+#define HT_SECOND_BCN       0x0100  
+#define HT_LSIG_TXOP        0x0200  
+#define HT_PCO_ACTIVE       0x0400  
+#define HT_PCO_PHASE        0x0800  
+
+
+#define DOT11N_2G_TXBURST_LIMIT 6160    
+#define DOT11N_5G_TXBURST_LIMIT 3080    
+
+
+#define GET_HT_OPMODE(add_ie)       ((ltoh16_ua(&add_ie->opmode) & HT_OPMODE_MASK) \
+					>> HT_OPMODE_SHIFT)
+#define HT_MIXEDMODE_PRESENT(add_ie)    ((ltoh16_ua(&add_ie->opmode) & HT_OPMODE_MASK) \
+					== HT_OPMODE_MIXED) 
+#define HT_HT20_PRESENT(add_ie) ((ltoh16_ua(&add_ie->opmode) & HT_OPMODE_MASK) \
+					== HT_OPMODE_HT20IN40)  
+#define HT_OPTIONAL_PRESENT(add_ie) ((ltoh16_ua(&add_ie->opmode) & HT_OPMODE_MASK) \
+					== HT_OPMODE_OPTIONAL)  
+#define HT_USE_PROTECTION(add_ie)   (HT_HT20_PRESENT((add_ie)) || \
+					HT_MIXEDMODE_PRESENT((add_ie))) 
+#define HT_NONGF_PRESENT(add_ie)    ((ltoh16_ua(&add_ie->opmode) & HT_OPMODE_NONGF) \
+					== HT_OPMODE_NONGF) 
+#define DOT11N_TXBURST_PRESENT(add_ie)  ((ltoh16_ua(&add_ie->opmode) & DOT11N_TXBURST) \
+					== DOT11N_TXBURST)  
+#define DOT11N_OBSS_NONHT_PRESENT(add_ie)   ((ltoh16_ua(&add_ie->opmode) & DOT11N_OBSS_NONHT) \
+					== DOT11N_OBSS_NONHT)   
+
+BWL_PRE_PACKED_STRUCT struct obss_params {
+	uint16  passive_dwell;
+	uint16  active_dwell;
+	uint16  bss_widthscan_interval;
+	uint16  passive_total;
+	uint16  active_total;
+	uint16  chanwidth_transition_dly;
+	uint16  activity_threshold;
+} BWL_POST_PACKED_STRUCT;
+typedef struct obss_params obss_params_t;
+
+BWL_PRE_PACKED_STRUCT struct dot11_obss_ie {
+	uint8   id;
+	uint8   len;
+	obss_params_t obss_params;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_obss_ie dot11_obss_ie_t;
+#define DOT11_OBSS_SCAN_IE_LEN  sizeof(obss_params_t)   
+
+
+#define HT_CTRL_LA_TRQ      0x00000002  
+#define HT_CTRL_LA_MAI      0x0000003C  
+#define HT_CTRL_LA_MAI_SHIFT    2
+#define HT_CTRL_LA_MAI_MRQ  0x00000004  
+#define HT_CTRL_LA_MAI_MSI  0x00000038  
+#define HT_CTRL_LA_MFSI     0x000001C0  
+#define HT_CTRL_LA_MFSI_SHIFT   6
+#define HT_CTRL_LA_MFB_ASELC    0x0000FE00  
+#define HT_CTRL_LA_MFB_ASELC_SH 9
+#define HT_CTRL_LA_ASELC_CMD    0x00000C00  
+#define HT_CTRL_LA_ASELC_DATA   0x0000F000  
+#define HT_CTRL_CAL_POS     0x00030000  
+#define HT_CTRL_CAL_SEQ     0x000C0000  
+#define HT_CTRL_CSI_STEERING    0x00C00000  
+#define HT_CTRL_CSI_STEER_SHIFT 22
+#define HT_CTRL_CSI_STEER_NFB   0       
+#define HT_CTRL_CSI_STEER_CSI   1       
+#define HT_CTRL_CSI_STEER_NCOM  2       
+#define HT_CTRL_CSI_STEER_COM   3       
+#define HT_CTRL_NDP_ANNOUNCE    0x01000000  
+#define HT_CTRL_AC_CONSTRAINT   0x40000000  
+#define HT_CTRL_RDG_MOREPPDU    0x80000000  
+
+#define HT_OPMODE_OPTIONAL  0x0001  
+#define HT_OPMODE_HT20IN40  0x0002  
+#define HT_OPMODE_MIXED 0x0003  
+#define HT_OPMODE_NONGF 0x0004  
+#define DOT11N_TXBURST      0x0008  
+#define DOT11N_OBSS_NONHT   0x0010  
+
+
+
+#define WPA_OUI         "\x00\x50\xF2"  
+#define WPA_OUI_LEN     3       
+#define WPA_OUI_TYPE        1
+#define WPA_VERSION     1   
+#define WPA2_OUI        "\x00\x0F\xAC"  
+#define WPA2_OUI_LEN        3       
+#define WPA2_VERSION        1   
+#define WPA2_VERSION_LEN    2   
+
+
+#define WPS_OUI         "\x00\x50\xF2"  
+#define WPS_OUI_LEN     3       
+#define WPS_OUI_TYPE        4
+
+
+#define WFA_OUI         "\x50\x6F\x9A"  
+#define WFA_OUI_LEN 3   
+
+#define WFA_OUI_TYPE_WPA    1
+#define WFA_OUI_TYPE_WPS    4
+#define WFA_OUI_TYPE_TPC    8
+#define WFA_OUI_TYPE_P2P    9
+
+
+#define RSN_AKM_NONE        0   
+#define RSN_AKM_UNSPECIFIED 1   
+#define RSN_AKM_PSK     2   
+#define RSN_AKM_FBT_1X      3   
+#define RSN_AKM_FBT_PSK     4   
+#define RSN_AKM_MFP_1X		5	
+#define RSN_AKM_MFP_PSK		6	
+#define RSN_AKM_TPK			7	
+
+
+#define DOT11_MAX_DEFAULT_KEYS  4   
+#define DOT11_MAX_KEY_SIZE  32  
+#define DOT11_MAX_IV_SIZE   16  
+#define DOT11_EXT_IV_FLAG   (1<<5)  
+#define DOT11_WPA_KEY_RSC_LEN   8       
+
+#define WEP1_KEY_SIZE       5   
+#define WEP1_KEY_HEX_SIZE   10  
+#define WEP128_KEY_SIZE     13  
+#define WEP128_KEY_HEX_SIZE 26  
+#define TKIP_MIC_SIZE       8   
+#define TKIP_EOM_SIZE       7   
+#define TKIP_EOM_FLAG       0x5a    
+#define TKIP_KEY_SIZE       32  
+#define TKIP_MIC_AUTH_TX    16  
+#define TKIP_MIC_AUTH_RX    24  
+#define TKIP_MIC_SUP_RX     TKIP_MIC_AUTH_TX    
+#define TKIP_MIC_SUP_TX     TKIP_MIC_AUTH_RX    
+#define AES_KEY_SIZE        16  
+#define AES_MIC_SIZE        8   
+
+
+#define WCN_OUI         "\x00\x50\xf2"  
+#define WCN_TYPE        4   
+
+
+
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_mdid_ie {
+	uint8 id;
+	uint8 len;
+	uint16 mdid;        
+	uint8 cap;
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_mdid_ie dot11_mdid_ie_t;
+
+#define FBT_MDID_CAP_OVERDS 0x01    
+#define FBT_MDID_CAP_RRP    0x02    
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_ft_ie {
+	uint8 id;
+	uint8 len;
+	uint16 mic_control;     
+	uint8 mic[16];
+	uint8 anonce[32];
+	uint8 snonce[32];
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_ft_ie dot11_ft_ie_t;
+
+
+BWL_PRE_PACKED_STRUCT struct dot11_gtk_ie {
+	uint8 id;
+	uint8 len;
+	uint16 key_info;
+	uint8 key_len;
+	uint8 rsc[8];
+	uint8 data[1];
+} BWL_POST_PACKED_STRUCT;
+typedef struct dot11_gtk_ie dot11_gtk_ie_t;
+
+#define BSSID_INVALID           "\x00\x00\x00\x00\x00\x00"
+#define BSSID_BROADCAST         "\xFF\xFF\xFF\xFF\xFF\xFF"
+
+
+
+BWL_PRE_PACKED_STRUCT struct link_id_ie {
+	uint8 id;
+	uint8 len;
+	struct ether_addr	bssid;
+	struct ether_addr	tdls_init_mac;
+	struct ether_addr	tdls_resp_mac;
+} BWL_POST_PACKED_STRUCT;
+typedef struct link_id_ie link_id_ie_t;
+#define TDLS_LINK_ID_IE_LEN		18
+
+
+BWL_PRE_PACKED_STRUCT struct wakeup_sch_ie {
+	uint8 id;
+	uint8 len;
+	uint32 offset;			
+	uint32 interval;		
+	uint32 awake_win_slots;	
+	uint32 max_wake_win;	
+	uint16 idle_cnt;		
+} BWL_POST_PACKED_STRUCT;
+typedef struct wakeup_sch_ie wakeup_sch_ie_t;
+#define TDLS_WAKEUP_SCH_IE_LEN		18
+
+
+BWL_PRE_PACKED_STRUCT struct channel_switch_timing_ie {
+	uint8 id;
+	uint8 len;
+	uint16 switch_time;		
+	uint16 switch_timeout;	
+} BWL_POST_PACKED_STRUCT;
+typedef struct channel_switch_timing_ie channel_switch_timing_ie_t;
+#define TDLS_CHANNEL_SWITCH_TIMING_IE_LEN		4
+
+
+BWL_PRE_PACKED_STRUCT struct pti_control_ie {
+	uint8 id;
+	uint8 len;
+	uint8 tid;
+	uint16 seq_control;
+} BWL_POST_PACKED_STRUCT;
+typedef struct pti_control_ie pti_control_ie_t;
+#define TDLS_PTI_CONTROL_IE_LEN		3
+
+
+BWL_PRE_PACKED_STRUCT struct pu_buffer_status_ie {
+	uint8 id;
+	uint8 len;
+	uint8 status;
+} BWL_POST_PACKED_STRUCT;
+typedef struct pu_buffer_status_ie pu_buffer_status_ie_t;
+#define TDLS_PU_BUFFER_STATUS_IE_LEN	1
+#define TDLS_PU_BUFFER_STATUS_AC_BK		1
+#define TDLS_PU_BUFFER_STATUS_AC_BE		2
+#define TDLS_PU_BUFFER_STATUS_AC_VI		4
+#define TDLS_PU_BUFFER_STATUS_AC_VO		8
+
+
+#include <packed_section_end.h>
+
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/proto/802.11_bta.h b/drivers/net/wireless/bcmdhd/include/proto/802.11_bta.h
new file mode 100644
index 000000000000..cbdd05e624bc
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/proto/802.11_bta.h
@@ -0,0 +1,45 @@
+/*
+ * BT-AMP (BlueTooth Alternate Mac and Phy) 802.11 PAL (Protocol Adaptation Layer)
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: 802.11_bta.h 277737 2011-08-16 17:54:59Z $
+*/
+
+#ifndef _802_11_BTA_H_
+#define _802_11_BTA_H_
+
+#define BT_SIG_SNAP_MPROT		"\xAA\xAA\x03\x00\x19\x58"
+
+/* BT-AMP 802.11 PAL Protocols */
+#define BTA_PROT_L2CAP				1
+#define	BTA_PROT_ACTIVITY_REPORT		2
+#define BTA_PROT_SECURITY			3
+#define BTA_PROT_LINK_SUPERVISION_REQUEST	4
+#define BTA_PROT_LINK_SUPERVISION_REPLY		5
+
+/* BT-AMP 802.11 PAL AMP_ASSOC Type IDs */
+#define BTA_TYPE_ID_MAC_ADDRESS			1
+#define BTA_TYPE_ID_PREFERRED_CHANNELS		2
+#define BTA_TYPE_ID_CONNECTED_CHANNELS		3
+#define BTA_TYPE_ID_CAPABILITIES		4
+#define BTA_TYPE_ID_VERSION			5
+#endif /* _802_11_bta_h_ */
diff --git a/drivers/net/wireless/bcmdhd/include/proto/802.11e.h b/drivers/net/wireless/bcmdhd/include/proto/802.11e.h
new file mode 100644
index 000000000000..0e070a475b64
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/proto/802.11e.h
@@ -0,0 +1,131 @@
+/*
+ * 802.11e protocol header file
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: 802.11e.h 277737 2011-08-16 17:54:59Z $
+ */
+
+#ifndef _802_11e_H_
+#define _802_11e_H_
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+/* This marks the start of a packed structure section. */
+#include <packed_section_start.h>
+
+
+/* WME Traffic Specification (TSPEC) element */
+#define WME_TSPEC_HDR_LEN           2           /* WME TSPEC header length */
+#define WME_TSPEC_BODY_OFF          2           /* WME TSPEC body offset */
+
+#define WME_CATEGORY_CODE_OFFSET	0		/* WME Category code offset */
+#define WME_ACTION_CODE_OFFSET		1		/* WME Action code offset */
+#define WME_TOKEN_CODE_OFFSET		2		/* WME Token code offset */
+#define WME_STATUS_CODE_OFFSET		3		/* WME Status code offset */
+
+BWL_PRE_PACKED_STRUCT struct tsinfo {
+	uint8 octets[3];
+} BWL_POST_PACKED_STRUCT;
+
+typedef struct tsinfo tsinfo_t;
+
+/* 802.11e TSPEC IE */
+typedef BWL_PRE_PACKED_STRUCT struct tspec {
+	uint8 oui[DOT11_OUI_LEN];	/* WME_OUI */
+	uint8 type;					/* WME_TYPE */
+	uint8 subtype;				/* WME_SUBTYPE_TSPEC */
+	uint8 version;				/* WME_VERSION */
+	tsinfo_t tsinfo;			/* TS Info bit field */
+	uint16 nom_msdu_size;		/* (Nominal or fixed) MSDU Size (bytes) */
+	uint16 max_msdu_size;		/* Maximum MSDU Size (bytes) */
+	uint32 min_srv_interval;	/* Minimum Service Interval (us) */
+	uint32 max_srv_interval;	/* Maximum Service Interval (us) */
+	uint32 inactivity_interval;	/* Inactivity Interval (us) */
+	uint32 suspension_interval; /* Suspension Interval (us) */
+	uint32 srv_start_time;		/* Service Start Time (us) */
+	uint32 min_data_rate;		/* Minimum Data Rate (bps) */
+	uint32 mean_data_rate;		/* Mean Data Rate (bps) */
+	uint32 peak_data_rate;		/* Peak Data Rate (bps) */
+	uint32 max_burst_size;		/* Maximum Burst Size (bytes) */
+	uint32 delay_bound;			/* Delay Bound (us) */
+	uint32 min_phy_rate;		/* Minimum PHY Rate (bps) */
+	uint16 surplus_bw;			/* Surplus Bandwidth Allowance (range 1.0-8.0) */
+	uint16 medium_time;			/* Medium Time (32 us/s periods) */
+} BWL_POST_PACKED_STRUCT tspec_t;
+
+#define WME_TSPEC_LEN	(sizeof(tspec_t))		/* not including 2-bytes of header */
+
+/* ts_info */
+/* 802.1D priority is duplicated - bits 13-11 AND bits 3-1 */
+#define TS_INFO_TID_SHIFT		1	/* TS info. TID shift */
+#define TS_INFO_TID_MASK		(0xf << TS_INFO_TID_SHIFT)	/* TS info. TID mask */
+#define TS_INFO_CONTENTION_SHIFT	7	/* TS info. contention shift */
+#define TS_INFO_CONTENTION_MASK	(0x1 << TS_INFO_CONTENTION_SHIFT) /* TS info. contention mask */
+#define TS_INFO_DIRECTION_SHIFT	5	/* TS info. direction shift */
+#define TS_INFO_DIRECTION_MASK	(0x3 << TS_INFO_DIRECTION_SHIFT) /* TS info. direction mask */
+#define TS_INFO_PSB_SHIFT		2		/* TS info. PSB bit Shift */
+#define TS_INFO_PSB_MASK		(1 << TS_INFO_PSB_SHIFT)	/* TS info. PSB mask */
+#define TS_INFO_UPLINK			(0 << TS_INFO_DIRECTION_SHIFT)	/* TS info. uplink */
+#define TS_INFO_DOWNLINK		(1 << TS_INFO_DIRECTION_SHIFT)	/* TS info. downlink */
+#define TS_INFO_BIDIRECTIONAL	(3 << TS_INFO_DIRECTION_SHIFT)	/* TS info. bidirectional */
+#define TS_INFO_USER_PRIO_SHIFT	3	/* TS info. user priority shift */
+/* TS info. user priority mask */
+#define TS_INFO_USER_PRIO_MASK	(0x7 << TS_INFO_USER_PRIO_SHIFT)
+
+/* Macro to get/set bit(s) field in TSINFO */
+#define WLC_CAC_GET_TID(pt)	((((pt).octets[0]) & TS_INFO_TID_MASK) >> TS_INFO_TID_SHIFT)
+#define WLC_CAC_GET_DIR(pt)	((((pt).octets[0]) & \
+	TS_INFO_DIRECTION_MASK) >> TS_INFO_DIRECTION_SHIFT)
+#define WLC_CAC_GET_PSB(pt)	((((pt).octets[1]) & TS_INFO_PSB_MASK) >> TS_INFO_PSB_SHIFT)
+#define WLC_CAC_GET_USER_PRIO(pt)	((((pt).octets[1]) & \
+	TS_INFO_USER_PRIO_MASK) >> TS_INFO_USER_PRIO_SHIFT)
+
+#define WLC_CAC_SET_TID(pt, id)	((((pt).octets[0]) & (~TS_INFO_TID_MASK)) | \
+	((id) << TS_INFO_TID_SHIFT))
+#define WLC_CAC_SET_USER_PRIO(pt, prio)	((((pt).octets[0]) & (~TS_INFO_USER_PRIO_MASK)) | \
+	((prio) << TS_INFO_USER_PRIO_SHIFT))
+
+/* 802.11e QBSS Load IE */
+#define QBSS_LOAD_IE_LEN		5	/* QBSS Load IE length */
+#define QBSS_LOAD_AAC_OFF		3	/* AAC offset in IE */
+
+#define CAC_ADDTS_RESP_TIMEOUT		300	/* default ADDTS response timeout in ms */
+
+/* 802.11e ADDTS status code */
+#define DOT11E_STATUS_ADMISSION_ACCEPTED	0	/* TSPEC Admission accepted status */
+#define DOT11E_STATUS_ADDTS_INVALID_PARAM	1	/* TSPEC invalid parameter status */
+#define DOT11E_STATUS_ADDTS_REFUSED_NSBW	3	/* ADDTS refused (non-sufficient BW) */
+#define DOT11E_STATUS_ADDTS_REFUSED_AWHILE	47	/* ADDTS refused but could retry later */
+
+/* 802.11e DELTS status code */
+#define DOT11E_STATUS_QSTA_LEAVE_QBSS		36	/* STA leave QBSS */
+#define DOT11E_STATUS_END_TS				37	/* END TS */
+#define DOT11E_STATUS_UNKNOWN_TS			38	/* UNKNOWN TS */
+#define DOT11E_STATUS_QSTA_REQ_TIMEOUT		39	/* STA ADDTS request timeout */
+
+
+/* This marks the end of a packed structure section. */
+#include <packed_section_end.h>
+
+#endif /* _802_11e_CAC_H_ */
diff --git a/drivers/net/wireless/bcmdhd/include/proto/802.1d.h b/drivers/net/wireless/bcmdhd/include/proto/802.1d.h
new file mode 100644
index 000000000000..c7e07bd5e7c3
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/proto/802.1d.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * Fundamental types and constants relating to 802.1D
+ *
+ * $Id: 802.1d.h 277737 2011-08-16 17:54:59Z $
+ */
+
+
+#ifndef _802_1_D_
+#define _802_1_D_
+
+
+#define	PRIO_8021D_NONE		2	
+#define	PRIO_8021D_BK		1	
+#define	PRIO_8021D_BE		0	
+#define	PRIO_8021D_EE		3	
+#define	PRIO_8021D_CL		4	
+#define	PRIO_8021D_VI		5	
+#define	PRIO_8021D_VO		6	
+#define	PRIO_8021D_NC		7	
+#define	MAXPRIO			7	
+#define NUMPRIO			(MAXPRIO + 1)
+
+#define ALLPRIO		-1	
+
+
+#define PRIO2PREC(prio) \
+	(((prio) == PRIO_8021D_NONE || (prio) == PRIO_8021D_BE) ? ((prio^2)) : (prio))
+
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/proto/bcmeth.h b/drivers/net/wireless/bcmdhd/include/proto/bcmeth.h
new file mode 100644
index 000000000000..0f75d3c8f1d6
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/proto/bcmeth.h
@@ -0,0 +1,83 @@
+/*
+ * Broadcom Ethernettype  protocol definitions
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bcmeth.h 277737 2011-08-16 17:54:59Z $
+ */
+
+
+
+
+#ifndef _BCMETH_H_
+#define _BCMETH_H_
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+
+#include <packed_section_start.h>
+
+
+
+
+
+
+
+#define	BCMILCP_SUBTYPE_RATE		1
+#define	BCMILCP_SUBTYPE_LINK		2
+#define	BCMILCP_SUBTYPE_CSA		3
+#define	BCMILCP_SUBTYPE_LARQ		4
+#define BCMILCP_SUBTYPE_VENDOR		5
+#define	BCMILCP_SUBTYPE_FLH		17
+
+#define BCMILCP_SUBTYPE_VENDOR_LONG	32769
+#define BCMILCP_SUBTYPE_CERT		32770
+#define BCMILCP_SUBTYPE_SES		32771
+
+
+#define BCMILCP_BCM_SUBTYPE_RESERVED		0
+#define BCMILCP_BCM_SUBTYPE_EVENT		1
+#define BCMILCP_BCM_SUBTYPE_SES			2
+
+
+#define BCMILCP_BCM_SUBTYPE_DPT			4
+
+#define BCMILCP_BCM_SUBTYPEHDR_MINLENGTH	8
+#define BCMILCP_BCM_SUBTYPEHDR_VERSION		0
+
+
+typedef BWL_PRE_PACKED_STRUCT struct bcmeth_hdr
+{
+	uint16	subtype;	
+	uint16	length;
+	uint8	version;	
+	uint8	oui[3];		
+	
+	uint16	usr_subtype;
+} BWL_POST_PACKED_STRUCT bcmeth_hdr_t;
+
+
+
+#include <packed_section_end.h>
+
+#endif	
diff --git a/drivers/net/wireless/bcmdhd/include/proto/bcmevent.h b/drivers/net/wireless/bcmdhd/include/proto/bcmevent.h
new file mode 100644
index 000000000000..e8c2387dd10b
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/proto/bcmevent.h
@@ -0,0 +1,317 @@
+/*
+ * Broadcom Event  protocol definitions
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * Dependencies: proto/bcmeth.h
+ *
+ * $Id: bcmevent.h 288077 2011-10-06 00:08:47Z $
+ *
+ */
+
+
+
+
+#ifndef _BCMEVENT_H_
+#define _BCMEVENT_H_
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+
+#include <packed_section_start.h>
+
+#define BCM_EVENT_MSG_VERSION		2	
+#define BCM_MSG_IFNAME_MAX		16	
+
+
+#define WLC_EVENT_MSG_LINK		0x01	
+#define WLC_EVENT_MSG_FLUSHTXQ		0x02	
+#define WLC_EVENT_MSG_GROUP		0x04	
+#define WLC_EVENT_MSG_UNKBSS		0x08	
+#define WLC_EVENT_MSG_UNKIF		0x10	
+
+
+
+
+typedef BWL_PRE_PACKED_STRUCT struct
+{
+	uint16	version;
+	uint16	flags;			
+	uint32	event_type;		
+	uint32	status;			
+	uint32	reason;			
+	uint32	auth_type;		
+	uint32	datalen;		
+	struct ether_addr	addr;	
+	char	ifname[BCM_MSG_IFNAME_MAX]; 
+} BWL_POST_PACKED_STRUCT wl_event_msg_v1_t;
+
+
+typedef BWL_PRE_PACKED_STRUCT struct
+{
+	uint16	version;
+	uint16	flags;			
+	uint32	event_type;		
+	uint32	status;			
+	uint32	reason;			
+	uint32	auth_type;		
+	uint32	datalen;		
+	struct ether_addr	addr;	
+	char	ifname[BCM_MSG_IFNAME_MAX]; 
+	uint8	ifidx;			
+	uint8	bsscfgidx;		
+} BWL_POST_PACKED_STRUCT wl_event_msg_t;
+
+
+typedef BWL_PRE_PACKED_STRUCT struct bcm_event {
+	struct ether_header eth;
+	bcmeth_hdr_t		bcm_hdr;
+	wl_event_msg_t		event;
+	
+} BWL_POST_PACKED_STRUCT bcm_event_t;
+
+#define BCM_MSG_LEN	(sizeof(bcm_event_t) - sizeof(bcmeth_hdr_t) - sizeof(struct ether_header))
+
+
+#define WLC_E_SET_SSID		0	
+#define WLC_E_JOIN		1	
+#define WLC_E_START		2	
+#define WLC_E_AUTH		3	
+#define WLC_E_AUTH_IND		4	
+#define WLC_E_DEAUTH		5	
+#define WLC_E_DEAUTH_IND	6	
+#define WLC_E_ASSOC		7	
+#define WLC_E_ASSOC_IND		8	
+#define WLC_E_REASSOC		9	
+#define WLC_E_REASSOC_IND	10	
+#define WLC_E_DISASSOC		11	
+#define WLC_E_DISASSOC_IND	12	
+#define WLC_E_QUIET_START	13	
+#define WLC_E_QUIET_END		14	
+#define WLC_E_BEACON_RX		15	
+#define WLC_E_LINK		16	
+#define WLC_E_MIC_ERROR		17	
+#define WLC_E_NDIS_LINK		18	
+#define WLC_E_ROAM		19	
+#define WLC_E_TXFAIL		20	
+#define WLC_E_PMKID_CACHE	21	
+#define WLC_E_RETROGRADE_TSF	22	
+#define WLC_E_PRUNE		23	
+#define WLC_E_AUTOAUTH		24	
+#define WLC_E_EAPOL_MSG		25	
+#define WLC_E_SCAN_COMPLETE	26	
+#define WLC_E_ADDTS_IND		27	
+#define WLC_E_DELTS_IND		28	
+#define WLC_E_BCNSENT_IND	29	
+#define WLC_E_BCNRX_MSG		30	
+#define WLC_E_BCNLOST_MSG	31	
+#define WLC_E_ROAM_PREP		32	
+#define WLC_E_PFN_NET_FOUND	33	
+#define WLC_E_PFN_NET_LOST	34	
+#define WLC_E_RESET_COMPLETE	35
+#define WLC_E_JOIN_START	36
+#define WLC_E_ROAM_START	37
+#define WLC_E_ASSOC_START	38
+#define WLC_E_IBSS_ASSOC	39
+#define WLC_E_RADIO		40
+#define WLC_E_PSM_WATCHDOG	41	
+#define WLC_E_PROBREQ_MSG       44      
+#define WLC_E_SCAN_CONFIRM_IND  45
+#define WLC_E_PSK_SUP		46	
+#define WLC_E_COUNTRY_CODE_CHANGED 47
+#define	WLC_E_EXCEEDED_MEDIUM_TIME 48	
+#define WLC_E_ICV_ERROR		49	
+#define WLC_E_UNICAST_DECODE_ERROR 50	
+#define WLC_E_MULTICAST_DECODE_ERROR 51 
+#define WLC_E_TRACE		52
+#define WLC_E_BTA_HCI_EVENT	53	
+#define WLC_E_IF		54	
+#ifdef WLP2P
+#define WLC_E_P2P_DISC_LISTEN_COMPLETE 	55	
+#endif
+#define WLC_E_RSSI		56	
+#define WLC_E_PFN_SCAN_COMPLETE	57	
+#define WLC_E_EXTLOG_MSG	58
+#define WLC_E_ACTION_FRAME      59 	
+#define WLC_E_ACTION_FRAME_COMPLETE 60	
+#define WLC_E_PRE_ASSOC_IND	61	
+#define WLC_E_PRE_REASSOC_IND	62	
+#define WLC_E_CHANNEL_ADOPTED	63
+#define WLC_E_AP_STARTED	64	
+#define WLC_E_DFS_AP_STOP	65	
+#define WLC_E_DFS_AP_RESUME	66	
+#define WLC_E_WAI_STA_EVENT	67	
+#define WLC_E_WAI_MSG 		68	
+#define WLC_E_ESCAN_RESULT 	69	
+#define WLC_E_ACTION_FRAME_OFF_CHAN_COMPLETE 	70	
+#if defined(WLP2P)
+#define WLC_E_PROBRESP_MSG	71	
+#define WLC_E_P2P_PROBREQ_MSG	72	
+#endif
+#define WLC_E_DCS_REQUEST 73
+
+#define WLC_E_FIFO_CREDIT_MAP	74 
+
+#define WLC_E_ACTION_FRAME_RX	75	
+#define WLC_E_WAKE_EVENT	76	
+#define WLC_E_RM_COMPLETE	77	
+#define WLC_E_HTSFSYNC		78	
+#define WLC_E_OVERLAY_REQ	79	
+#define WLC_E_CSA_COMPLETE_IND  80
+#define WLC_E_EXCESS_PM_WAKE_EVENT	81	
+#define WLC_E_PFN_SCAN_NONE		82	
+#define WLC_E_PFN_SCAN_ALLGONE	83	
+#define WLC_E_GTK_PLUMBED 84
+#define WLC_E_ASSOC_REQ_IE 85
+#define WLC_E_ASSOC_RESP_IE 86
+#define WLC_E_LAST	87	
+
+
+
+typedef struct {
+	uint event;
+	const char *name;
+} bcmevent_name_t;
+
+extern const bcmevent_name_t	bcmevent_names[];
+extern const int		bcmevent_names_size;
+
+
+#define WLC_E_STATUS_SUCCESS		0	
+#define WLC_E_STATUS_FAIL		1	
+#define WLC_E_STATUS_TIMEOUT		2	
+#define WLC_E_STATUS_NO_NETWORKS	3	
+#define WLC_E_STATUS_ABORT		4	
+#define WLC_E_STATUS_NO_ACK		5	
+#define WLC_E_STATUS_UNSOLICITED	6	
+#define WLC_E_STATUS_ATTEMPT		7	
+#define WLC_E_STATUS_PARTIAL		8	
+#define WLC_E_STATUS_NEWSCAN		9	
+#define WLC_E_STATUS_NEWASSOC		10	
+#define WLC_E_STATUS_11HQUIET		11	
+#define WLC_E_STATUS_SUPPRESS		12	
+#define WLC_E_STATUS_NOCHANS		13	
+#define WLC_E_STATUS_CS_ABORT		15	
+#define WLC_E_STATUS_ERROR		16	
+
+
+#define WLC_E_REASON_INITIAL_ASSOC	0	
+#define WLC_E_REASON_LOW_RSSI		1	
+#define WLC_E_REASON_DEAUTH		2	
+#define WLC_E_REASON_DISASSOC		3	
+#define WLC_E_REASON_BCNS_LOST		4	
+#define WLC_E_REASON_MINTXRATE		9	
+#define WLC_E_REASON_TXFAIL		10	
+
+
+#define WLC_E_REASON_FAST_ROAM_FAILED	5	
+#define WLC_E_REASON_DIRECTED_ROAM	6	
+#define WLC_E_REASON_TSPEC_REJECTED	7	
+#define WLC_E_REASON_BETTER_AP		8	
+
+#define WLC_E_REASON_REQUESTED_ROAM 11	
+
+
+#define WLC_E_PRUNE_ENCR_MISMATCH	1	
+#define WLC_E_PRUNE_BCAST_BSSID		2	
+#define WLC_E_PRUNE_MAC_DENY		3	
+#define WLC_E_PRUNE_MAC_NA		4	
+#define WLC_E_PRUNE_REG_PASSV		5	
+#define WLC_E_PRUNE_SPCT_MGMT		6	
+#define WLC_E_PRUNE_RADAR		7	
+#define WLC_E_RSN_MISMATCH		8	
+#define WLC_E_PRUNE_NO_COMMON_RATES	9	
+#define WLC_E_PRUNE_BASIC_RATES		10	
+#define WLC_E_PRUNE_CIPHER_NA		12	
+#define WLC_E_PRUNE_KNOWN_STA		13	
+#define WLC_E_PRUNE_WDS_PEER		15	
+#define WLC_E_PRUNE_QBSS_LOAD		16	
+#define WLC_E_PRUNE_HOME_AP		17	
+
+
+#define WLC_E_SUP_OTHER			0	
+#define WLC_E_SUP_DECRYPT_KEY_DATA	1	
+#define WLC_E_SUP_BAD_UCAST_WEP128	2	
+#define WLC_E_SUP_BAD_UCAST_WEP40	3	
+#define WLC_E_SUP_UNSUP_KEY_LEN		4	
+#define WLC_E_SUP_PW_KEY_CIPHER		5	
+#define WLC_E_SUP_MSG3_TOO_MANY_IE	6	
+#define WLC_E_SUP_MSG3_IE_MISMATCH	7	
+#define WLC_E_SUP_NO_INSTALL_FLAG	8	
+#define WLC_E_SUP_MSG3_NO_GTK		9	
+#define WLC_E_SUP_GRP_KEY_CIPHER	10	
+#define WLC_E_SUP_GRP_MSG1_NO_GTK	11	
+#define WLC_E_SUP_GTK_DECRYPT_FAIL	12	
+#define WLC_E_SUP_SEND_FAIL		13	
+#define WLC_E_SUP_DEAUTH		14	
+#define WLC_E_SUP_WPA_PSK_TMO		15	
+
+
+
+typedef BWL_PRE_PACKED_STRUCT struct wl_event_rx_frame_data {
+	uint16	version;
+	uint16	channel;	
+	int32	rssi;
+	uint32	mactime;
+	uint32	rate;
+} BWL_POST_PACKED_STRUCT wl_event_rx_frame_data_t;
+
+#define BCM_RX_FRAME_DATA_VERSION 1
+
+
+typedef struct wl_event_data_if {
+	uint8 ifidx;
+	uint8 opcode;		
+	uint8 reserved;
+	uint8 bssidx;		
+	uint8 role;		
+} wl_event_data_if_t;
+
+
+#define WLC_E_IF_ADD		1	
+#define WLC_E_IF_DEL		2	
+#define WLC_E_IF_CHANGE		3	
+
+
+#define WLC_E_IF_ROLE_STA		0	
+#define WLC_E_IF_ROLE_AP		1	
+#define WLC_E_IF_ROLE_WDS		2	
+#define WLC_E_IF_ROLE_P2P_GO		3	
+#define WLC_E_IF_ROLE_P2P_CLIENT	4	
+#define WLC_E_IF_ROLE_BTA_CREATOR	5	
+#define WLC_E_IF_ROLE_BTA_ACCEPTOR	6	
+
+
+#define WLC_E_LINK_BCN_LOSS	1	
+#define WLC_E_LINK_DISASSOC	2	
+#define WLC_E_LINK_ASSOC_REC	3	
+#define WLC_E_LINK_BSSCFG_DIS	4	
+
+
+#define WLC_E_OVL_DOWNLOAD	0	
+#define WLC_E_OVL_UPDATE_IND	1	
+
+
+#include <packed_section_end.h>
+
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/proto/bcmip.h b/drivers/net/wireless/bcmdhd/include/proto/bcmip.h
new file mode 100644
index 000000000000..55eff247c492
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/proto/bcmip.h
@@ -0,0 +1,154 @@
+/*
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * Fundamental constants relating to IP Protocol
+ *
+ * $Id: bcmip.h 277737 2011-08-16 17:54:59Z $
+ */
+
+
+#ifndef _bcmip_h_
+#define _bcmip_h_
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+
+#include <packed_section_start.h>
+
+
+
+#define IP_VER_OFFSET		0x0	
+#define IP_VER_MASK		0xf0	
+#define IP_VER_SHIFT		4	
+#define IP_VER_4		4	
+#define IP_VER_6		6	
+
+#define IP_VER(ip_body) \
+	((((uint8 *)(ip_body))[IP_VER_OFFSET] & IP_VER_MASK) >> IP_VER_SHIFT)
+
+#define IP_PROT_ICMP		0x1	
+#define IP_PROT_TCP		0x6	
+#define IP_PROT_UDP		0x11	
+
+
+#define IPV4_VER_HL_OFFSET	0	
+#define IPV4_TOS_OFFSET		1	
+#define IPV4_PKTLEN_OFFSET	2	
+#define IPV4_PKTFLAG_OFFSET	6	
+#define IPV4_PROT_OFFSET	9	
+#define IPV4_CHKSUM_OFFSET	10	
+#define IPV4_SRC_IP_OFFSET	12	
+#define IPV4_DEST_IP_OFFSET	16	
+#define IPV4_OPTIONS_OFFSET	20	
+
+
+#define IPV4_VER_MASK		0xf0	
+#define IPV4_VER_SHIFT		4	
+
+#define IPV4_HLEN_MASK		0x0f	
+#define IPV4_HLEN(ipv4_body)	(4 * (((uint8 *)(ipv4_body))[IPV4_VER_HL_OFFSET] & IPV4_HLEN_MASK))
+
+#define IPV4_ADDR_LEN		4	
+
+#define IPV4_ADDR_NULL(a)	((((uint8 *)(a))[0] | ((uint8 *)(a))[1] | \
+				  ((uint8 *)(a))[2] | ((uint8 *)(a))[3]) == 0)
+
+#define IPV4_ADDR_BCAST(a)	((((uint8 *)(a))[0] & ((uint8 *)(a))[1] & \
+				  ((uint8 *)(a))[2] & ((uint8 *)(a))[3]) == 0xff)
+
+#define	IPV4_TOS_DSCP_MASK	0xfc	
+#define	IPV4_TOS_DSCP_SHIFT	2	
+
+#define	IPV4_TOS(ipv4_body)	(((uint8 *)(ipv4_body))[IPV4_TOS_OFFSET])
+
+#define	IPV4_TOS_PREC_MASK	0xe0	
+#define	IPV4_TOS_PREC_SHIFT	5	
+
+#define IPV4_TOS_LOWDELAY	0x10	
+#define IPV4_TOS_THROUGHPUT	0x8	
+#define IPV4_TOS_RELIABILITY	0x4	
+
+#define IPV4_PROT(ipv4_body)	(((uint8 *)(ipv4_body))[IPV4_PROT_OFFSET])
+
+#define IPV4_FRAG_RESV		0x8000	
+#define IPV4_FRAG_DONT		0x4000	
+#define IPV4_FRAG_MORE		0x2000	
+#define IPV4_FRAG_OFFSET_MASK	0x1fff	
+
+#define IPV4_ADDR_STR_LEN	16	
+
+
+BWL_PRE_PACKED_STRUCT struct ipv4_addr {
+	uint8	addr[IPV4_ADDR_LEN];
+} BWL_POST_PACKED_STRUCT;
+
+BWL_PRE_PACKED_STRUCT struct ipv4_hdr {
+	uint8	version_ihl;		
+	uint8	tos;			
+	uint16	tot_len;		
+	uint16	id;
+	uint16	frag;			
+	uint8	ttl;			
+	uint8	prot;			
+	uint16	hdr_chksum;		
+	uint8	src_ip[IPV4_ADDR_LEN];	
+	uint8	dst_ip[IPV4_ADDR_LEN];	
+} BWL_POST_PACKED_STRUCT;
+
+
+#define IPV6_PAYLOAD_LEN_OFFSET	4	
+#define IPV6_NEXT_HDR_OFFSET	6	
+#define IPV6_HOP_LIMIT_OFFSET	7	
+#define IPV6_SRC_IP_OFFSET	8	
+#define IPV6_DEST_IP_OFFSET	24	
+
+
+#define IPV6_TRAFFIC_CLASS(ipv6_body) \
+	(((((uint8 *)(ipv6_body))[0] & 0x0f) << 4) | \
+	 ((((uint8 *)(ipv6_body))[1] & 0xf0) >> 4))
+
+#define IPV6_FLOW_LABEL(ipv6_body) \
+	(((((uint8 *)(ipv6_body))[1] & 0x0f) << 16) | \
+	 (((uint8 *)(ipv6_body))[2] << 8) | \
+	 (((uint8 *)(ipv6_body))[3]))
+
+#define IPV6_PAYLOAD_LEN(ipv6_body) \
+	((((uint8 *)(ipv6_body))[IPV6_PAYLOAD_LEN_OFFSET + 0] << 8) | \
+	 ((uint8 *)(ipv6_body))[IPV6_PAYLOAD_LEN_OFFSET + 1])
+
+#define IPV6_NEXT_HDR(ipv6_body) \
+	(((uint8 *)(ipv6_body))[IPV6_NEXT_HDR_OFFSET])
+
+#define IPV6_PROT(ipv6_body)	IPV6_NEXT_HDR(ipv6_body)
+
+#define IPV6_ADDR_LEN		16	
+
+
+#define IP_TOS46(ip_body) \
+	(IP_VER(ip_body) == IP_VER_4 ? IPV4_TOS(ip_body) : \
+	 IP_VER(ip_body) == IP_VER_6 ? IPV6_TRAFFIC_CLASS(ip_body) : 0)
+
+
+#include <packed_section_end.h>
+
+#endif	
diff --git a/drivers/net/wireless/bcmdhd/include/proto/bt_amp_hci.h b/drivers/net/wireless/bcmdhd/include/proto/bt_amp_hci.h
new file mode 100644
index 000000000000..91ab4fe538f2
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/proto/bt_amp_hci.h
@@ -0,0 +1,442 @@
+/*
+ * BT-AMP (BlueTooth Alternate Mac and Phy) HCI (Host/Controller Interface)
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: bt_amp_hci.h 277737 2011-08-16 17:54:59Z $
+*/
+
+#ifndef _bt_amp_hci_h
+#define _bt_amp_hci_h
+
+/* This marks the start of a packed structure section. */
+#include <packed_section_start.h>
+
+
+/* AMP HCI CMD packet format */
+typedef BWL_PRE_PACKED_STRUCT struct amp_hci_cmd {
+	uint16 opcode;
+	uint8 plen;
+	uint8 parms[1];
+} BWL_POST_PACKED_STRUCT amp_hci_cmd_t;
+
+#define HCI_CMD_PREAMBLE_SIZE		OFFSETOF(amp_hci_cmd_t, parms)
+#define HCI_CMD_DATA_SIZE		255
+
+/* AMP HCI CMD opcode layout */
+#define HCI_CMD_OPCODE(ogf, ocf)	((((ogf) & 0x3F) << 10) | ((ocf) & 0x03FF))
+#define HCI_CMD_OGF(opcode)		((uint8)(((opcode) >> 10) & 0x3F))
+#define HCI_CMD_OCF(opcode)		((opcode) & 0x03FF)
+
+/* AMP HCI command opcodes */
+#define HCI_Read_Failed_Contact_Counter		HCI_CMD_OPCODE(0x05, 0x0001)
+#define HCI_Reset_Failed_Contact_Counter	HCI_CMD_OPCODE(0x05, 0x0002)
+#define HCI_Read_Link_Quality			HCI_CMD_OPCODE(0x05, 0x0003)
+#define HCI_Read_Local_AMP_Info			HCI_CMD_OPCODE(0x05, 0x0009)
+#define HCI_Read_Local_AMP_ASSOC		HCI_CMD_OPCODE(0x05, 0x000A)
+#define HCI_Write_Remote_AMP_ASSOC		HCI_CMD_OPCODE(0x05, 0x000B)
+#define HCI_Create_Physical_Link		HCI_CMD_OPCODE(0x01, 0x0035)
+#define HCI_Accept_Physical_Link_Request	HCI_CMD_OPCODE(0x01, 0x0036)
+#define HCI_Disconnect_Physical_Link		HCI_CMD_OPCODE(0x01, 0x0037)
+#define HCI_Create_Logical_Link			HCI_CMD_OPCODE(0x01, 0x0038)
+#define HCI_Accept_Logical_Link			HCI_CMD_OPCODE(0x01, 0x0039)
+#define HCI_Disconnect_Logical_Link		HCI_CMD_OPCODE(0x01, 0x003A)
+#define HCI_Logical_Link_Cancel			HCI_CMD_OPCODE(0x01, 0x003B)
+#define HCI_Flow_Spec_Modify			HCI_CMD_OPCODE(0x01, 0x003C)
+#define HCI_Write_Flow_Control_Mode		HCI_CMD_OPCODE(0x01, 0x0067)
+#define HCI_Read_Best_Effort_Flush_Timeout	HCI_CMD_OPCODE(0x01, 0x0069)
+#define HCI_Write_Best_Effort_Flush_Timeout	HCI_CMD_OPCODE(0x01, 0x006A)
+#define HCI_Short_Range_Mode			HCI_CMD_OPCODE(0x01, 0x006B)
+#define HCI_Reset				HCI_CMD_OPCODE(0x03, 0x0003)
+#define HCI_Read_Connection_Accept_Timeout	HCI_CMD_OPCODE(0x03, 0x0015)
+#define HCI_Write_Connection_Accept_Timeout	HCI_CMD_OPCODE(0x03, 0x0016)
+#define HCI_Read_Link_Supervision_Timeout	HCI_CMD_OPCODE(0x03, 0x0036)
+#define HCI_Write_Link_Supervision_Timeout	HCI_CMD_OPCODE(0x03, 0x0037)
+#define HCI_Enhanced_Flush			HCI_CMD_OPCODE(0x03, 0x005F)
+#define HCI_Read_Logical_Link_Accept_Timeout	HCI_CMD_OPCODE(0x03, 0x0061)
+#define HCI_Write_Logical_Link_Accept_Timeout	HCI_CMD_OPCODE(0x03, 0x0062)
+#define HCI_Set_Event_Mask_Page_2		HCI_CMD_OPCODE(0x03, 0x0063)
+#define HCI_Read_Location_Data_Command		HCI_CMD_OPCODE(0x03, 0x0064)
+#define HCI_Write_Location_Data_Command		HCI_CMD_OPCODE(0x03, 0x0065)
+#define HCI_Read_Local_Version_Info		HCI_CMD_OPCODE(0x04, 0x0001)
+#define HCI_Read_Local_Supported_Commands	HCI_CMD_OPCODE(0x04, 0x0002)
+#define HCI_Read_Buffer_Size			HCI_CMD_OPCODE(0x04, 0x0005)
+#define HCI_Read_Data_Block_Size		HCI_CMD_OPCODE(0x04, 0x000A)
+
+/* AMP HCI command parameters */
+typedef BWL_PRE_PACKED_STRUCT struct read_local_cmd_parms {
+	uint8 plh;
+	uint8 offset[2];			/* length so far */
+	uint8 max_remote[2];
+} BWL_POST_PACKED_STRUCT read_local_cmd_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct write_remote_cmd_parms {
+	uint8 plh;
+	uint8 offset[2];
+	uint8 len[2];
+	uint8 frag[1];
+} BWL_POST_PACKED_STRUCT write_remote_cmd_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct phy_link_cmd_parms {
+	uint8 plh;
+	uint8 key_length;
+	uint8 key_type;
+	uint8 key[1];
+} BWL_POST_PACKED_STRUCT phy_link_cmd_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct dis_phy_link_cmd_parms {
+	uint8 plh;
+	uint8 reason;
+} BWL_POST_PACKED_STRUCT dis_phy_link_cmd_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct log_link_cmd_parms {
+	uint8 plh;
+	uint8 txflow[16];
+	uint8 rxflow[16];
+} BWL_POST_PACKED_STRUCT log_link_cmd_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct ext_flow_spec {
+	uint8 id;
+	uint8 service_type;
+	uint8 max_sdu[2];
+	uint8 sdu_ia_time[4];
+	uint8 access_latency[4];
+	uint8 flush_timeout[4];
+} BWL_POST_PACKED_STRUCT ext_flow_spec_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct log_link_cancel_cmd_parms {
+	uint8 plh;
+	uint8 tx_fs_ID;
+} BWL_POST_PACKED_STRUCT log_link_cancel_cmd_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct flow_spec_mod_cmd_parms {
+	uint8 llh[2];
+	uint8 txflow[16];
+	uint8 rxflow[16];
+} BWL_POST_PACKED_STRUCT flow_spec_mod_cmd_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct plh_pad {
+	uint8 plh;
+	uint8 pad;
+} BWL_POST_PACKED_STRUCT plh_pad_t;
+
+typedef BWL_PRE_PACKED_STRUCT union hci_handle {
+	uint16 bredr;
+	plh_pad_t amp;
+} BWL_POST_PACKED_STRUCT hci_handle_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct ls_to_cmd_parms {
+	hci_handle_t handle;
+	uint8 timeout[2];
+} BWL_POST_PACKED_STRUCT ls_to_cmd_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct befto_cmd_parms {
+	uint8 llh[2];
+	uint8 befto[4];
+} BWL_POST_PACKED_STRUCT befto_cmd_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct srm_cmd_parms {
+	uint8 plh;
+	uint8 srm;
+} BWL_POST_PACKED_STRUCT srm_cmd_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct ld_cmd_parms {
+	uint8 ld_aware;
+	uint8 ld[2];
+	uint8 ld_opts;
+	uint8 l_opts;
+} BWL_POST_PACKED_STRUCT ld_cmd_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct eflush_cmd_parms {
+	uint8 llh[2];
+	uint8 packet_type;
+} BWL_POST_PACKED_STRUCT eflush_cmd_parms_t;
+
+/* Generic AMP extended flow spec service types */
+#define EFS_SVCTYPE_NO_TRAFFIC		0
+#define EFS_SVCTYPE_BEST_EFFORT		1
+#define EFS_SVCTYPE_GUARANTEED		2
+
+/* AMP HCI event packet format */
+typedef BWL_PRE_PACKED_STRUCT struct amp_hci_event {
+	uint8 ecode;
+	uint8 plen;
+	uint8 parms[1];
+} BWL_POST_PACKED_STRUCT amp_hci_event_t;
+
+#define HCI_EVT_PREAMBLE_SIZE			OFFSETOF(amp_hci_event_t, parms)
+
+/* AMP HCI event codes */
+#define HCI_Command_Complete			0x0E
+#define HCI_Command_Status			0x0F
+#define HCI_Flush_Occurred			0x11
+#define HCI_Enhanced_Flush_Complete		0x39
+#define HCI_Physical_Link_Complete		0x40
+#define HCI_Channel_Select			0x41
+#define HCI_Disconnect_Physical_Link_Complete	0x42
+#define HCI_Logical_Link_Complete		0x45
+#define HCI_Disconnect_Logical_Link_Complete	0x46
+#define HCI_Flow_Spec_Modify_Complete		0x47
+#define HCI_Number_of_Completed_Data_Blocks	0x48
+#define HCI_Short_Range_Mode_Change_Complete	0x4C
+#define HCI_Status_Change_Event			0x4D
+#define HCI_Vendor_Specific			0xFF
+
+/* AMP HCI event mask bit positions */
+#define HCI_Physical_Link_Complete_Event_Mask			0x0001
+#define HCI_Channel_Select_Event_Mask				0x0002
+#define HCI_Disconnect_Physical_Link_Complete_Event_Mask	0x0004
+#define HCI_Logical_Link_Complete_Event_Mask			0x0020
+#define HCI_Disconnect_Logical_Link_Complete_Event_Mask		0x0040
+#define HCI_Flow_Spec_Modify_Complete_Event_Mask		0x0080
+#define HCI_Number_of_Completed_Data_Blocks_Event_Mask		0x0100
+#define HCI_Short_Range_Mode_Change_Complete_Event_Mask		0x1000
+#define HCI_Status_Change_Event_Mask				0x2000
+#define HCI_All_Event_Mask					0x31e7
+
+/* AMP HCI event parameters */
+typedef BWL_PRE_PACKED_STRUCT struct cmd_status_parms {
+	uint8 status;
+	uint8 cmdpkts;
+	uint16 opcode;
+} BWL_POST_PACKED_STRUCT cmd_status_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct cmd_complete_parms {
+	uint8 cmdpkts;
+	uint16 opcode;
+	uint8 parms[1];
+} BWL_POST_PACKED_STRUCT cmd_complete_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct flush_occurred_evt_parms {
+	uint16 handle;
+} BWL_POST_PACKED_STRUCT flush_occurred_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct write_remote_evt_parms {
+	uint8 status;
+	uint8 plh;
+} BWL_POST_PACKED_STRUCT write_remote_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct read_local_evt_parms {
+	uint8 status;
+	uint8 plh;
+	uint16 len;
+	uint8 frag[1];
+} BWL_POST_PACKED_STRUCT read_local_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct read_local_info_evt_parms {
+	uint8 status;
+	uint8 AMP_status;
+	uint32 bandwidth;
+	uint32 gbandwidth;
+	uint32 latency;
+	uint32 PDU_size;
+	uint8 ctrl_type;
+	uint16 PAL_cap;
+	uint16 AMP_ASSOC_len;
+	uint32 max_flush_timeout;
+	uint32 be_flush_timeout;
+} BWL_POST_PACKED_STRUCT read_local_info_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct log_link_evt_parms {
+	uint8 status;
+	uint16 llh;
+	uint8 plh;
+	uint8 tx_fs_ID;
+} BWL_POST_PACKED_STRUCT log_link_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct disc_log_link_evt_parms {
+	uint8 status;
+	uint16 llh;
+	uint8 reason;
+} BWL_POST_PACKED_STRUCT disc_log_link_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct log_link_cancel_evt_parms {
+	uint8 status;
+	uint8 plh;
+	uint8 tx_fs_ID;
+} BWL_POST_PACKED_STRUCT log_link_cancel_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct flow_spec_mod_evt_parms {
+	uint8 status;
+	uint16 llh;
+} BWL_POST_PACKED_STRUCT flow_spec_mod_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct phy_link_evt_parms {
+	uint8 status;
+	uint8 plh;
+} BWL_POST_PACKED_STRUCT phy_link_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct dis_phy_link_evt_parms {
+	uint8 status;
+	uint8 plh;
+	uint8 reason;
+} BWL_POST_PACKED_STRUCT dis_phy_link_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct read_ls_to_evt_parms {
+	uint8 status;
+	hci_handle_t handle;
+	uint16 timeout;
+} BWL_POST_PACKED_STRUCT read_ls_to_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct read_lla_ca_to_evt_parms {
+	uint8 status;
+	uint16 timeout;
+} BWL_POST_PACKED_STRUCT read_lla_ca_to_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct read_data_block_size_evt_parms {
+	uint8 status;
+	uint16 ACL_pkt_len;
+	uint16 data_block_len;
+	uint16 data_block_num;
+} BWL_POST_PACKED_STRUCT read_data_block_size_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct data_blocks {
+	uint16 handle;
+	uint16 pkts;
+	uint16 blocks;
+} BWL_POST_PACKED_STRUCT data_blocks_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct num_completed_data_blocks_evt_parms {
+	uint16 num_blocks;
+	uint8 num_handles;
+	data_blocks_t completed[1];
+} BWL_POST_PACKED_STRUCT num_completed_data_blocks_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct befto_evt_parms {
+	uint8 status;
+	uint32 befto;
+} BWL_POST_PACKED_STRUCT befto_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct srm_evt_parms {
+	uint8 status;
+	uint8 plh;
+	uint8 srm;
+} BWL_POST_PACKED_STRUCT srm_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct contact_counter_evt_parms {
+	uint8 status;
+	uint8 llh[2];
+	uint16 counter;
+} BWL_POST_PACKED_STRUCT contact_counter_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct contact_counter_reset_evt_parms {
+	uint8 status;
+	uint8 llh[2];
+} BWL_POST_PACKED_STRUCT contact_counter_reset_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct read_linkq_evt_parms {
+	uint8 status;
+	hci_handle_t handle;
+	uint8 link_quality;
+} BWL_POST_PACKED_STRUCT read_linkq_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct ld_evt_parms {
+	uint8 status;
+	uint8 ld_aware;
+	uint8 ld[2];
+	uint8 ld_opts;
+	uint8 l_opts;
+} BWL_POST_PACKED_STRUCT ld_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct eflush_complete_evt_parms {
+	uint16 handle;
+} BWL_POST_PACKED_STRUCT eflush_complete_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct vendor_specific_evt_parms {
+	uint8 len;
+	uint8 parms[1];
+} BWL_POST_PACKED_STRUCT vendor_specific_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct local_version_info_evt_parms {
+	uint8 status;
+	uint8 hci_version;
+	uint16 hci_revision;
+	uint8 pal_version;
+	uint16 mfg_name;
+	uint16 pal_subversion;
+} BWL_POST_PACKED_STRUCT local_version_info_evt_parms_t;
+
+#define MAX_SUPPORTED_CMD_BYTE	64
+typedef BWL_PRE_PACKED_STRUCT struct local_supported_cmd_evt_parms {
+	uint8 status;
+	uint8 cmd[MAX_SUPPORTED_CMD_BYTE];
+} BWL_POST_PACKED_STRUCT local_supported_cmd_evt_parms_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct status_change_evt_parms {
+	uint8 status;
+	uint8 amp_status;
+} BWL_POST_PACKED_STRUCT status_change_evt_parms_t;
+
+/* AMP HCI error codes */
+#define HCI_SUCCESS				0x00
+#define HCI_ERR_ILLEGAL_COMMAND			0x01
+#define HCI_ERR_NO_CONNECTION			0x02
+#define HCI_ERR_MEMORY_FULL			0x07
+#define HCI_ERR_CONNECTION_TIMEOUT		0x08
+#define HCI_ERR_MAX_NUM_OF_CONNECTIONS		0x09
+#define HCI_ERR_CONNECTION_EXISTS		0x0B
+#define HCI_ERR_CONNECTION_DISALLOWED		0x0C
+#define HCI_ERR_CONNECTION_ACCEPT_TIMEOUT	0x10
+#define HCI_ERR_UNSUPPORTED_VALUE		0x11
+#define HCI_ERR_ILLEGAL_PARAMETER_FMT		0x12
+#define HCI_ERR_CONN_TERM_BY_LOCAL_HOST		0x16
+#define HCI_ERR_UNSPECIFIED			0x1F
+#define HCI_ERR_UNIT_KEY_USED			0x26
+#define HCI_ERR_QOS_REJECTED			0x2D
+#define HCI_ERR_PARAM_OUT_OF_RANGE		0x30
+#define HCI_ERR_NO_SUITABLE_CHANNEL		0x39
+#define HCI_ERR_CHANNEL_MOVE			0xFF
+
+/* AMP HCI ACL Data packet format */
+typedef BWL_PRE_PACKED_STRUCT struct amp_hci_ACL_data {
+	uint16	handle;			/* 12-bit connection handle + 2-bit PB and 2-bit BC flags */
+	uint16	dlen;			/* data total length */
+	uint8 data[1];
+} BWL_POST_PACKED_STRUCT amp_hci_ACL_data_t;
+
+#define HCI_ACL_DATA_PREAMBLE_SIZE	OFFSETOF(amp_hci_ACL_data_t, data)
+
+#define HCI_ACL_DATA_BC_FLAGS		(0x0 << 14)
+#define HCI_ACL_DATA_PB_FLAGS		(0x3 << 12)
+
+#define HCI_ACL_DATA_HANDLE(handle)	((handle) & 0x0fff)
+#define HCI_ACL_DATA_FLAGS(handle)	((handle) >> 12)
+
+/* AMP Activity Report packet formats */
+typedef BWL_PRE_PACKED_STRUCT struct amp_hci_activity_report {
+	uint8	ScheduleKnown;
+	uint8	NumReports;
+	uint8	data[1];
+} BWL_POST_PACKED_STRUCT amp_hci_activity_report_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct amp_hci_activity_report_triple {
+	uint32	StartTime;
+	uint32	Duration;
+	uint32	Periodicity;
+} BWL_POST_PACKED_STRUCT amp_hci_activity_report_triple_t;
+
+#define HCI_AR_SCHEDULE_KNOWN		0x01
+
+
+/* This marks the end of a packed structure section. */
+#include <packed_section_end.h>
+
+#endif /* _bt_amp_hci_h_ */
diff --git a/drivers/net/wireless/bcmdhd/include/proto/eapol.h b/drivers/net/wireless/bcmdhd/include/proto/eapol.h
new file mode 100644
index 000000000000..92634c1221a6
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/proto/eapol.h
@@ -0,0 +1,173 @@
+/*
+ * 802.1x EAPOL definitions
+ *
+ * See
+ * IEEE Std 802.1X-2001
+ * IEEE 802.1X RADIUS Usage Guidelines
+ *
+ * Copyright (C) 2002 Broadcom Corporation
+ *
+ * $Id: eapol.h 277737 2011-08-16 17:54:59Z $
+ */
+
+#ifndef _eapol_h_
+#define _eapol_h_
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+/* This marks the start of a packed structure section. */
+#include <packed_section_start.h>
+
+#include <bcmcrypto/aeskeywrap.h>
+
+/* EAPOL for 802.3/Ethernet */
+typedef struct {
+	struct ether_header eth;	/* 802.3/Ethernet header */
+	unsigned char version;		/* EAPOL protocol version */
+	unsigned char type;		/* EAPOL type */
+	unsigned short length;		/* Length of body */
+	unsigned char body[1];		/* Body (optional) */
+} eapol_header_t;
+
+#define EAPOL_HEADER_LEN 18
+
+/* EAPOL version */
+#define WPA2_EAPOL_VERSION	2
+#define WPA_EAPOL_VERSION	1
+#define LEAP_EAPOL_VERSION	1
+#define SES_EAPOL_VERSION	1
+
+/* EAPOL types */
+#define EAP_PACKET		0
+#define EAPOL_START		1
+#define EAPOL_LOGOFF		2
+#define EAPOL_KEY		3
+#define EAPOL_ASF		4
+
+/* EAPOL-Key types */
+#define EAPOL_RC4_KEY		1
+#define EAPOL_WPA2_KEY		2	/* 802.11i/WPA2 */
+#define EAPOL_WPA_KEY		254	/* WPA */
+
+/* RC4 EAPOL-Key header field sizes */
+#define EAPOL_KEY_REPLAY_LEN	8
+#define EAPOL_KEY_IV_LEN	16
+#define EAPOL_KEY_SIG_LEN	16
+
+/* RC4 EAPOL-Key */
+typedef BWL_PRE_PACKED_STRUCT struct {
+	unsigned char type;			/* Key Descriptor Type */
+	unsigned short length;			/* Key Length (unaligned) */
+	unsigned char replay[EAPOL_KEY_REPLAY_LEN];	/* Replay Counter */
+	unsigned char iv[EAPOL_KEY_IV_LEN];		/* Key IV */
+	unsigned char index;				/* Key Flags & Index */
+	unsigned char signature[EAPOL_KEY_SIG_LEN];	/* Key Signature */
+	unsigned char key[1];				/* Key (optional) */
+} BWL_POST_PACKED_STRUCT eapol_key_header_t;
+
+#define EAPOL_KEY_HEADER_LEN 	44
+
+/* RC4 EAPOL-Key flags */
+#define EAPOL_KEY_FLAGS_MASK	0x80
+#define EAPOL_KEY_BROADCAST	0
+#define EAPOL_KEY_UNICAST	0x80
+
+/* RC4 EAPOL-Key index */
+#define EAPOL_KEY_INDEX_MASK	0x7f
+
+/* WPA/802.11i/WPA2 EAPOL-Key header field sizes */
+#define EAPOL_WPA_KEY_REPLAY_LEN	8
+#define EAPOL_WPA_KEY_NONCE_LEN		32
+#define EAPOL_WPA_KEY_IV_LEN		16
+#define EAPOL_WPA_KEY_RSC_LEN		8
+#define EAPOL_WPA_KEY_ID_LEN		8
+#define EAPOL_WPA_KEY_MIC_LEN		16
+#define EAPOL_WPA_KEY_DATA_LEN		(EAPOL_WPA_MAX_KEY_SIZE + AKW_BLOCK_LEN)
+#define EAPOL_WPA_MAX_KEY_SIZE		32
+
+/* WPA EAPOL-Key */
+typedef BWL_PRE_PACKED_STRUCT struct {
+	unsigned char type;		/* Key Descriptor Type */
+	unsigned short key_info;	/* Key Information (unaligned) */
+	unsigned short key_len;		/* Key Length (unaligned) */
+	unsigned char replay[EAPOL_WPA_KEY_REPLAY_LEN];	/* Replay Counter */
+	unsigned char nonce[EAPOL_WPA_KEY_NONCE_LEN];	/* Nonce */
+	unsigned char iv[EAPOL_WPA_KEY_IV_LEN];		/* Key IV */
+	unsigned char rsc[EAPOL_WPA_KEY_RSC_LEN];	/* Key RSC */
+	unsigned char id[EAPOL_WPA_KEY_ID_LEN];		/* WPA:Key ID, 802.11i/WPA2: Reserved */
+	unsigned char mic[EAPOL_WPA_KEY_MIC_LEN];	/* Key MIC */
+	unsigned short data_len;			/* Key Data Length */
+	unsigned char data[EAPOL_WPA_KEY_DATA_LEN];	/* Key data */
+} BWL_POST_PACKED_STRUCT eapol_wpa_key_header_t;
+
+#define EAPOL_WPA_KEY_LEN 		95
+
+/* WPA/802.11i/WPA2 KEY KEY_INFO bits */
+#define WPA_KEY_DESC_V1		0x01
+#define WPA_KEY_DESC_V2		0x02
+#define WPA_KEY_DESC_V3		0x03
+#define WPA_KEY_PAIRWISE	0x08
+#define WPA_KEY_INSTALL		0x40
+#define WPA_KEY_ACK		0x80
+#define WPA_KEY_MIC		0x100
+#define WPA_KEY_SECURE		0x200
+#define WPA_KEY_ERROR		0x400
+#define WPA_KEY_REQ		0x800
+
+/* WPA-only KEY KEY_INFO bits */
+#define WPA_KEY_INDEX_0		0x00
+#define WPA_KEY_INDEX_1		0x10
+#define WPA_KEY_INDEX_2		0x20
+#define WPA_KEY_INDEX_3		0x30
+#define WPA_KEY_INDEX_MASK	0x30
+#define WPA_KEY_INDEX_SHIFT	0x04
+
+/* 802.11i/WPA2-only KEY KEY_INFO bits */
+#define WPA_KEY_ENCRYPTED_DATA	0x1000
+
+/* Key Data encapsulation */
+typedef BWL_PRE_PACKED_STRUCT struct {
+	uint8 type;
+	uint8 length;
+	uint8 oui[3];
+	uint8 subtype;
+	uint8 data[1];
+} BWL_POST_PACKED_STRUCT eapol_wpa2_encap_data_t;
+
+#define EAPOL_WPA2_ENCAP_DATA_HDR_LEN 	6
+
+#define WPA2_KEY_DATA_SUBTYPE_GTK	1
+#define WPA2_KEY_DATA_SUBTYPE_STAKEY	2
+#define WPA2_KEY_DATA_SUBTYPE_MAC	3
+#define WPA2_KEY_DATA_SUBTYPE_PMKID	4
+
+/* GTK encapsulation */
+typedef BWL_PRE_PACKED_STRUCT struct {
+	uint8	flags;
+	uint8	reserved;
+	uint8	gtk[EAPOL_WPA_MAX_KEY_SIZE];
+} BWL_POST_PACKED_STRUCT eapol_wpa2_key_gtk_encap_t;
+
+#define EAPOL_WPA2_KEY_GTK_ENCAP_HDR_LEN 	2
+
+#define WPA2_GTK_INDEX_MASK	0x03
+#define WPA2_GTK_INDEX_SHIFT	0x00
+
+#define WPA2_GTK_TRANSMIT	0x04
+
+/* STAKey encapsulation */
+typedef BWL_PRE_PACKED_STRUCT struct {
+	uint8	reserved[2];
+	uint8	mac[ETHER_ADDR_LEN];
+	uint8	stakey[EAPOL_WPA_MAX_KEY_SIZE];
+} BWL_POST_PACKED_STRUCT eapol_wpa2_key_stakey_encap_t;
+
+#define WPA2_KEY_DATA_PAD	0xdd
+
+
+/* This marks the end of a packed structure section. */
+#include <packed_section_end.h>
+
+#endif /* _eapol_h_ */
diff --git a/drivers/net/wireless/bcmdhd/include/proto/ethernet.h b/drivers/net/wireless/bcmdhd/include/proto/ethernet.h
new file mode 100644
index 000000000000..20865dc5a231
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/proto/ethernet.h
@@ -0,0 +1,163 @@
+/*
+ * From FreeBSD 2.2.7: Fundamental constants relating to ethernet.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: ethernet.h 285437 2011-09-21 22:16:56Z $
+ */
+
+
+#ifndef _NET_ETHERNET_H_	      
+#define _NET_ETHERNET_H_
+
+#ifndef _TYPEDEFS_H_
+#include "typedefs.h"
+#endif
+
+
+#include <packed_section_start.h>
+
+
+
+#define	ETHER_ADDR_LEN		6
+
+
+#define	ETHER_TYPE_LEN		2
+
+
+#define	ETHER_CRC_LEN		4
+
+
+#define	ETHER_HDR_LEN		(ETHER_ADDR_LEN * 2 + ETHER_TYPE_LEN)
+
+
+#define	ETHER_MIN_LEN		64
+
+
+#define	ETHER_MIN_DATA		46
+
+
+#define	ETHER_MAX_LEN		1518
+
+
+#define	ETHER_MAX_DATA		1500
+
+
+#define ETHER_TYPE_MIN		0x0600		
+#define	ETHER_TYPE_IP		0x0800		
+#define ETHER_TYPE_ARP		0x0806		
+#define ETHER_TYPE_8021Q	0x8100		
+#define	ETHER_TYPE_BRCM		0x886c		
+#define	ETHER_TYPE_802_1X	0x888e		
+#define	ETHER_TYPE_802_1X_PREAUTH 0x88c7	
+#define ETHER_TYPE_WAI		0x88b4		
+#define ETHER_TYPE_89_0D	0x890d		
+
+
+
+#define	ETHER_BRCM_SUBTYPE_LEN	4	
+#define	ETHER_BRCM_CRAM		1	
+
+
+#define ETHER_DEST_OFFSET	(0 * ETHER_ADDR_LEN)	
+#define ETHER_SRC_OFFSET	(1 * ETHER_ADDR_LEN)	
+#define ETHER_TYPE_OFFSET	(2 * ETHER_ADDR_LEN)	
+
+
+#define	ETHER_IS_VALID_LEN(foo)	\
+	((foo) >= ETHER_MIN_LEN && (foo) <= ETHER_MAX_LEN)
+
+#define ETHER_FILL_MCAST_ADDR_FROM_IP(ea, mgrp_ip) {		\
+		((uint8 *)ea)[0] = 0x01;			\
+		((uint8 *)ea)[1] = 0x00;			\
+		((uint8 *)ea)[2] = 0x5e;			\
+		((uint8 *)ea)[3] = ((mgrp_ip) >> 16) & 0x7f;	\
+		((uint8 *)ea)[4] = ((mgrp_ip) >>  8) & 0xff;	\
+		((uint8 *)ea)[5] = ((mgrp_ip) >>  0) & 0xff;	\
+}
+
+#ifndef __INCif_etherh       
+
+BWL_PRE_PACKED_STRUCT struct ether_header {
+	uint8	ether_dhost[ETHER_ADDR_LEN];
+	uint8	ether_shost[ETHER_ADDR_LEN];
+	uint16	ether_type;
+} BWL_POST_PACKED_STRUCT;
+
+
+BWL_PRE_PACKED_STRUCT struct	ether_addr {
+	uint8 octet[ETHER_ADDR_LEN];
+} BWL_POST_PACKED_STRUCT;
+#endif	
+
+
+#define ETHER_SET_LOCALADDR(ea)	(((uint8 *)(ea))[0] = (((uint8 *)(ea))[0] | 2))
+#define ETHER_IS_LOCALADDR(ea) 	(((uint8 *)(ea))[0] & 2)
+#define ETHER_CLR_LOCALADDR(ea)	(((uint8 *)(ea))[0] = (((uint8 *)(ea))[0] & 0xd))
+#define ETHER_TOGGLE_LOCALADDR(ea)	(((uint8 *)(ea))[0] = (((uint8 *)(ea))[0] ^ 2))
+
+
+#define ETHER_SET_UNICAST(ea)	(((uint8 *)(ea))[0] = (((uint8 *)(ea))[0] & ~1))
+
+
+#define ETHER_ISMULTI(ea) (((const uint8 *)(ea))[0] & 1)
+
+
+
+#define	ether_cmp(a, b)	(!(((short*)a)[0] == ((short*)b)[0]) | \
+			 !(((short*)a)[1] == ((short*)b)[1]) | \
+			 !(((short*)a)[2] == ((short*)b)[2]))
+
+
+#define	ether_copy(s, d) { \
+		((short*)d)[0] = ((short*)s)[0]; \
+		((short*)d)[1] = ((short*)s)[1]; \
+		((short*)d)[2] = ((short*)s)[2]; }
+
+
+static const struct ether_addr ether_bcast = {{255, 255, 255, 255, 255, 255}};
+static const struct ether_addr ether_null = {{0, 0, 0, 0, 0, 0}};
+
+#define ETHER_ISBCAST(ea)	((((uint8 *)(ea))[0] &		\
+	                          ((uint8 *)(ea))[1] &		\
+				  ((uint8 *)(ea))[2] &		\
+				  ((uint8 *)(ea))[3] &		\
+				  ((uint8 *)(ea))[4] &		\
+				  ((uint8 *)(ea))[5]) == 0xff)
+#define ETHER_ISNULLADDR(ea)	((((uint8 *)(ea))[0] |		\
+				  ((uint8 *)(ea))[1] |		\
+				  ((uint8 *)(ea))[2] |		\
+				  ((uint8 *)(ea))[3] |		\
+				  ((uint8 *)(ea))[4] |		\
+				  ((uint8 *)(ea))[5]) == 0)
+
+
+#define ETHER_MOVE_HDR(d, s) \
+do { \
+	struct ether_header t; \
+	t = *(struct ether_header *)(s); \
+	*(struct ether_header *)(d) = t; \
+} while (0)
+
+
+#include <packed_section_end.h>
+
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/proto/p2p.h b/drivers/net/wireless/bcmdhd/include/proto/p2p.h
new file mode 100644
index 000000000000..d2bf3f20688c
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/proto/p2p.h
@@ -0,0 +1,512 @@
+/*
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * Fundamental types and constants relating to WFA P2P (aka WiFi Direct)
+ *
+ * $Id: p2p.h 277737 2011-08-16 17:54:59Z $
+ */
+
+#ifndef _P2P_H_
+#define _P2P_H_
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+#include <wlioctl.h>
+#include <proto/802.11.h>
+
+/* This marks the start of a packed structure section. */
+#include <packed_section_start.h>
+
+
+/* WiFi P2P OUI values */
+#define P2P_OUI         WFA_OUI             /* WiFi P2P OUI */
+#define P2P_VER         WFA_OUI_TYPE_P2P    /* P2P version: 9=WiFi P2P v1.0 */
+
+#define P2P_IE_ID       0xdd            /* P2P IE element ID */
+
+/* WiFi P2P IE */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_ie {
+	uint8   id;     /* IE ID: 0xDD */
+	uint8   len;        /* IE length */
+	uint8   OUI[3];     /* WiFi P2P specific OUI: P2P_OUI */
+	uint8   oui_type;   /* Identifies P2P version: P2P_VER */
+	uint8   subelts[1]; /* variable length subelements */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_ie wifi_p2p_ie_t;
+
+#define P2P_IE_FIXED_LEN    6
+
+#define P2P_ATTR_ID_OFF         0
+#define P2P_ATTR_LEN_OFF        1
+#define P2P_ATTR_DATA_OFF       3
+
+#define P2P_ATTR_HDR_LEN        3 /* ID + 2-byte length field spec 1.02 */
+
+/* P2P IE Subelement IDs from WiFi P2P Technical Spec 1.00 */
+#define P2P_SEID_STATUS         0   /* Status */
+#define P2P_SEID_MINOR_RC       1   /* Minor Reason Code */
+#define P2P_SEID_P2P_INFO       2   /* P2P Capability (capabilities info) */
+#define P2P_SEID_DEV_ID         3   /* P2P Device ID */
+#define P2P_SEID_INTENT         4   /* Group Owner Intent */
+#define P2P_SEID_CFG_TIMEOUT        5   /* Configuration Timeout */
+#define P2P_SEID_CHANNEL        6   /* Channel */
+#define P2P_SEID_GRP_BSSID      7   /* P2P Group BSSID */
+#define P2P_SEID_XT_TIMING      8   /* Extended Listen Timing */
+#define P2P_SEID_INTINTADDR     9   /* Intended P2P Interface Address */
+#define P2P_SEID_P2P_MGBTY      10  /* P2P Manageability */
+#define P2P_SEID_CHAN_LIST      11  /* Channel List */
+#define P2P_SEID_ABSENCE        12  /* Notice of Absence */
+#define P2P_SEID_DEV_INFO       13  /* Device Info */
+#define P2P_SEID_GROUP_INFO     14  /* Group Info */
+#define P2P_SEID_GROUP_ID       15  /* Group ID */
+#define P2P_SEID_P2P_IF         16  /* P2P Interface */
+#define P2P_SEID_VNDR           221 /* Vendor-specific subelement */
+
+#define P2P_SE_VS_ID_SERVICES   0x1b /* BRCM proprietary subel: L2 Services */
+
+
+/* WiFi P2P IE subelement: P2P Capability (capabilities info) */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_info_se_s {
+	uint8   eltId;      /* SE ID: P2P_SEID_P2P_INFO */
+	uint8   len[2];     /* SE length not including eltId, len fields */
+	uint8   dev;        /* Device Capability Bitmap */
+	uint8   group;      /* Group Capability Bitmap */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_info_se_s wifi_p2p_info_se_t;
+
+/* P2P Capability subelement's Device Capability Bitmap bit values */
+#define P2P_CAPSE_DEV_SERVICE_DIS   0x1 /* Service Discovery */
+#define P2P_CAPSE_DEV_CLIENT_DIS    0x2 /* Client Discoverability */
+#define P2P_CAPSE_DEV_CONCURRENT    0x4 /* Concurrent Operation */
+#define P2P_CAPSE_DEV_INFRA_MAN     0x8 /* P2P Infrastructure Managed */
+#define P2P_CAPSE_DEV_LIMIT         0x10 /* P2P Device Limit */
+#define P2P_CAPSE_INVITE_PROC       0x20 /* P2P Invitation Procedure */
+
+/* P2P Capability subelement's Group Capability Bitmap bit values */
+#define P2P_CAPSE_GRP_OWNER         0x1 /* P2P Group Owner */
+#define P2P_CAPSE_PERSIST_GRP       0x2 /* Persistent P2P Group */
+#define P2P_CAPSE_GRP_LIMIT         0x4 /* P2P Group Limit */
+#define P2P_CAPSE_GRP_INTRA_BSS     0x8 /* Intra-BSS Distribution */
+#define P2P_CAPSE_GRP_X_CONNECT     0x10 /* Cross Connection */
+#define P2P_CAPSE_GRP_PERSISTENT    0x20 /* Persistent Reconnect */
+#define P2P_CAPSE_GRP_FORMATION     0x40 /* Group Formation */
+
+
+/* WiFi P2P IE subelement: Group Owner Intent */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_intent_se_s {
+	uint8   eltId;      /* SE ID: P2P_SEID_INTENT */
+	uint8   len[2];     /* SE length not including eltId, len fields */
+	uint8   intent;     /* Intent Value 0...15 (0=legacy 15=master only) */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_intent_se_s wifi_p2p_intent_se_t;
+
+/* WiFi P2P IE subelement: Configuration Timeout */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_cfg_tmo_se_s {
+	uint8   eltId;      /* SE ID: P2P_SEID_CFG_TIMEOUT */
+	uint8   len[2];     /* SE length not including eltId, len fields */
+	uint8   go_tmo;     /* GO config timeout in units of 10 ms */
+	uint8   client_tmo; /* Client config timeout in units of 10 ms */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_cfg_tmo_se_s wifi_p2p_cfg_tmo_se_t;
+
+
+/* WiFi P2P IE subelement: Status */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_status_se_s {
+	uint8   eltId;      /* SE ID: P2P_SEID_STATUS */
+	uint8   len[2];     /* SE length not including eltId, len fields */
+	uint8   status;     /* Status Code: P2P_STATSE_* */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_status_se_s wifi_p2p_status_se_t;
+
+/* Status subelement Status Code definitions */
+#define P2P_STATSE_SUCCESS              0
+				/* Success */
+#define P2P_STATSE_FAIL_INFO_CURR_UNAVAIL   1
+				/* Failed, information currently unavailable */
+#define P2P_STATSE_PASSED_UP            P2P_STATSE_FAIL_INFO_CURR_UNAVAIL
+				/* Old name for above in P2P spec 1.08 and older */
+#define P2P_STATSE_FAIL_INCOMPAT_PARAMS     2
+				/* Failed, incompatible parameters */
+#define P2P_STATSE_FAIL_LIMIT_REACHED       3
+				/* Failed, limit reached */
+#define P2P_STATSE_FAIL_INVALID_PARAMS      4
+				/* Failed, invalid parameters */
+#define P2P_STATSE_FAIL_UNABLE_TO_ACCOM     5
+				/* Failed, unable to accomodate request */
+#define P2P_STATSE_FAIL_PROTO_ERROR         6
+				/* Failed, previous protocol error or disruptive behaviour */
+#define P2P_STATSE_FAIL_NO_COMMON_CHAN      7
+				/* Failed, no common channels */
+#define P2P_STATSE_FAIL_UNKNOWN_GROUP       8
+				/* Failed, unknown P2P Group */
+#define P2P_STATSE_FAIL_INTENT          9
+				/* Failed, both peers indicated Intent 15 in GO Negotiation */
+#define P2P_STATSE_FAIL_INCOMPAT_PROVIS     10
+				/* Failed, incompatible provisioning method */
+#define P2P_STATSE_FAIL_USER_REJECT     11
+				/* Failed, rejected by user */
+
+/* WiFi P2P IE attribute: Extended Listen Timing */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_ext_se_s {
+	uint8   eltId;      /* ID: P2P_SEID_EXT_TIMING */
+	uint8   len[2];     /* length not including eltId, len fields */
+	uint8   avail[2];   /* availibility period */
+	uint8   interval[2];    /* availibility interval */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_ext_se_s wifi_p2p_ext_se_t;
+
+#define P2P_EXT_MIN 10  /* minimum 10ms */
+
+/* WiFi P2P IE subelement: Intended P2P Interface Address */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_intintad_se_s {
+	uint8   eltId;      /* SE ID: P2P_SEID_INTINTADDR */
+	uint8   len[2];     /* SE length not including eltId, len fields */
+	uint8   mac[6];     /* intended P2P interface MAC address */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_intintad_se_s wifi_p2p_intintad_se_t;
+
+/* WiFi P2P IE subelement: Channel */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_channel_se_s {
+	uint8   eltId;      /* SE ID: P2P_SEID_STATUS */
+	uint8   len[2];     /* SE length not including eltId, len fields */
+	uint8   band;       /* Regulatory Class (band) */
+	uint8   channel;    /* Channel */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_channel_se_s wifi_p2p_channel_se_t;
+
+
+/* Channel Entry structure within the Channel List SE */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_chanlist_entry_s {
+	uint8   band;                       /* Regulatory Class (band) */
+	uint8   num_channels;               /* # of channels in the channel list */
+	uint8   channels[WL_NUMCHANNELS];   /* Channel List */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_chanlist_entry_s wifi_p2p_chanlist_entry_t;
+#define WIFI_P2P_CHANLIST_SE_MAX_ENTRIES 2
+
+/* WiFi P2P IE subelement: Channel List */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_chanlist_se_s {
+	uint8   eltId;      /* SE ID: P2P_SEID_STATUS */
+	uint8   len[2];     /* SE length not including eltId, len fields */
+	uint8   country[3]; /* Country String */
+	uint8   num_entries;    /* # of channel entries */
+	wifi_p2p_chanlist_entry_t   entries[WIFI_P2P_CHANLIST_SE_MAX_ENTRIES];
+						/* Channel Entry List */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_chanlist_se_s wifi_p2p_chanlist_se_t;
+
+/* WiFi P2P IE's Device Info subelement */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_devinfo_se_s {
+	uint8   eltId;          /* SE ID: P2P_SEID_DEVINFO */
+	uint8   len[2];         /* SE length not including eltId, len fields */
+	uint8   mac[6];         /* P2P Device MAC address */
+	uint16  wps_cfg_meths;      /* Config Methods: reg_prototlv.h WPS_CONFMET_* */
+	uint8   pri_devtype[8];     /* Primary Device Type */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_devinfo_se_s wifi_p2p_devinfo_se_t;
+
+#define P2P_DEV_TYPE_LEN    8
+
+/* WiFi P2P IE's Group Info subelement Client Info Descriptor */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_cid_fixed_s {
+	uint8   len;
+	uint8   devaddr[ETHER_ADDR_LEN];    /* P2P Device Address */
+	uint8   ifaddr[ETHER_ADDR_LEN];     /* P2P Interface Address */
+	uint8   devcap;             /* Device Capability */
+	uint8   cfg_meths[2];           /* Config Methods: reg_prototlv.h WPS_CONFMET_* */
+	uint8   pridt[P2P_DEV_TYPE_LEN];    /* Primary Device Type */
+	uint8   secdts;             /* Number of Secondary Device Types */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_cid_fixed_s wifi_p2p_cid_fixed_t;
+
+/* WiFi P2P IE's Device ID subelement */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_devid_se_s {
+	uint8   eltId;
+	uint8   len[2];
+	struct ether_addr   addr;           /* P2P Device MAC address */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_devid_se_s wifi_p2p_devid_se_t;
+
+/* WiFi P2P IE subelement: P2P Manageability */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_mgbt_se_s {
+	uint8   eltId;      /* SE ID: P2P_SEID_P2P_MGBTY */
+	uint8   len[2];     /* SE length not including eltId, len fields */
+	uint8   mg_bitmap;  /* manageability bitmap */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_mgbt_se_s wifi_p2p_mgbt_se_t;
+/* mg_bitmap field bit values */
+#define P2P_MGBTSE_P2PDEVMGMT_FLAG   0x1 /* AP supports Managed P2P Device */
+
+/* WiFi P2P IE subelement: Group Info */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_grpinfo_se_s {
+	uint8   eltId;          /* SE ID: P2P_SEID_GROUP_INFO */
+	uint8   len[2];         /* SE length not including eltId, len fields */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_grpinfo_se_s wifi_p2p_grpinfo_se_t;
+
+
+/* WiFi P2P Action Frame */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_action_frame {
+	uint8   category;   /* P2P_AF_CATEGORY */
+	uint8   OUI[3];     /* OUI - P2P_OUI */
+	uint8   type;       /* OUI Type - P2P_VER */
+	uint8   subtype;    /* OUI Subtype - P2P_AF_* */
+	uint8   dialog_token;   /* nonzero, identifies req/resp tranaction */
+	uint8   elts[1];    /* Variable length information elements.  Max size =
+				 * ACTION_FRAME_SIZE - sizeof(this structure) - 1
+				 */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_action_frame wifi_p2p_action_frame_t;
+#define P2P_AF_CATEGORY     0x7f
+
+#define P2P_AF_FIXED_LEN    7
+
+/* WiFi P2P Action Frame OUI Subtypes */
+#define P2P_AF_NOTICE_OF_ABSENCE    0   /* Notice of Absence */
+#define P2P_AF_PRESENCE_REQ     1   /* P2P Presence Request */
+#define P2P_AF_PRESENCE_RSP     2   /* P2P Presence Response */
+#define P2P_AF_GO_DISC_REQ      3   /* GO Discoverability Request */
+
+
+/* WiFi P2P Public Action Frame */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_pub_act_frame {
+	uint8   category;   /* P2P_PUB_AF_CATEGORY */
+	uint8   action;     /* P2P_PUB_AF_ACTION */
+	uint8   oui[3];     /* P2P_OUI */
+	uint8   oui_type;   /* OUI type - P2P_VER */
+	uint8   subtype;    /* OUI subtype - P2P_TYPE_* */
+	uint8   dialog_token;   /* nonzero, identifies req/rsp transaction */
+	uint8   elts[1];    /* Variable length information elements.  Max size =
+				 * ACTION_FRAME_SIZE - sizeof(this structure) - 1
+				 */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_pub_act_frame wifi_p2p_pub_act_frame_t;
+#define P2P_PUB_AF_FIXED_LEN    8
+#define P2P_PUB_AF_CATEGORY 0x04
+#define P2P_PUB_AF_ACTION   0x09
+
+/* WiFi P2P Public Action Frame OUI Subtypes */
+#define P2P_PAF_GON_REQ     0   /* Group Owner Negotiation Req */
+#define P2P_PAF_GON_RSP     1   /* Group Owner Negotiation Rsp */
+#define P2P_PAF_GON_CONF    2   /* Group Owner Negotiation Confirm */
+#define P2P_PAF_INVITE_REQ  3   /* P2P Invitation Request */
+#define P2P_PAF_INVITE_RSP  4   /* P2P Invitation Response */
+#define P2P_PAF_DEVDIS_REQ  5   /* Device Discoverability Request */
+#define P2P_PAF_DEVDIS_RSP  6   /* Device Discoverability Response */
+#define P2P_PAF_PROVDIS_REQ 7   /* Provision Discovery Request */
+#define P2P_PAF_PROVDIS_RSP 8   /* Provision Discovery Request */
+
+/* TODO: Stop using these obsolete aliases for P2P_PAF_GON_* */
+#define P2P_TYPE_MNREQ      P2P_PAF_GON_REQ
+#define P2P_TYPE_MNRSP      P2P_PAF_GON_RSP
+#define P2P_TYPE_MNCONF     P2P_PAF_GON_CONF
+
+/* WiFi P2P IE subelement: Notice of Absence */
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_noa_desc {
+	uint8   cnt_type;   /* Count/Type */
+	uint32  duration;   /* Duration */
+	uint32  interval;   /* Interval */
+	uint32  start;      /* Start Time */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_noa_desc wifi_p2p_noa_desc_t;
+
+BWL_PRE_PACKED_STRUCT struct wifi_p2p_noa_se {
+	uint8   eltId;      /* Subelement ID */
+	uint8   len[2];     /* Length */
+	uint8   index;      /* Index */
+	uint8   ops_ctw_parms;  /* CTWindow and OppPS Parameters */
+	wifi_p2p_noa_desc_t desc[1];    /* Notice of Absence Descriptor(s) */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2p_noa_se wifi_p2p_noa_se_t;
+
+#define P2P_NOA_SE_FIXED_LEN    5
+
+/* cnt_type field values */
+#define P2P_NOA_DESC_CNT_RESERVED   0   /* reserved and should not be used */
+#define P2P_NOA_DESC_CNT_REPEAT     255 /* continuous schedule */
+#define P2P_NOA_DESC_TYPE_PREFERRED 1   /* preferred values */
+#define P2P_NOA_DESC_TYPE_ACCEPTABLE    2   /* acceptable limits */
+
+/* ctw_ops_parms field values */
+#define P2P_NOA_CTW_MASK    0x7f
+#define P2P_NOA_OPS_MASK    0x80
+#define P2P_NOA_OPS_SHIFT   7
+
+#define P2P_CTW_MIN 10  /* minimum 10TU */
+
+/*
+ * P2P Service Discovery related
+ */
+#define P2PSD_ACTION_CATEGORY       0x04
+				/* Public action frame */
+#define P2PSD_ACTION_ID_GAS_IREQ    0x0a
+				/* Action value for GAS Initial Request AF */
+#define P2PSD_ACTION_ID_GAS_IRESP   0x0b
+				/* Action value for GAS Initial Response AF */
+#define P2PSD_ACTION_ID_GAS_CREQ    0x0c
+				/* Action value for GAS Comback Request AF */
+#define P2PSD_ACTION_ID_GAS_CRESP   0x0d
+				/* Action value for GAS Comback Response AF */
+#define P2PSD_AD_EID                0x6c
+				/* Advertisement Protocol IE ID */
+#define P2PSD_ADP_TUPLE_QLMT_PAMEBI 0x00
+				/* Query Response Length Limit 7 bits plus PAME-BI 1 bit */
+#define P2PSD_ADP_PROTO_ID          0x00
+				/* Advertisement Protocol ID. Always 0 for P2P SD */
+#define P2PSD_GAS_OUI               P2P_OUI
+				/* WFA OUI */
+#define P2PSD_GAS_OUI_SUBTYPE       P2P_VER
+				/* OUI Subtype for GAS IE */
+#define P2PSD_GAS_NQP_INFOID        0xDDDD
+				/* NQP Query Info ID: 56797 */
+#define P2PSD_GAS_COMEBACKDEALY     0x00
+				/* Not used in the Native GAS protocol */
+
+/* Service Protocol Type */
+typedef enum p2psd_svc_protype {
+	SVC_RPOTYPE_ALL = 0,
+	SVC_RPOTYPE_BONJOUR = 1,
+	SVC_RPOTYPE_UPNP = 2,
+	SVC_RPOTYPE_WSD = 3,
+	SVC_RPOTYPE_VENDOR = 255
+} p2psd_svc_protype_t;
+
+/* Service Discovery response status code */
+typedef enum {
+	P2PSD_RESP_STATUS_SUCCESS = 0,
+	P2PSD_RESP_STATUS_PROTYPE_NA = 1,
+	P2PSD_RESP_STATUS_DATA_NA = 2,
+	P2PSD_RESP_STATUS_BAD_REQUEST = 3
+} p2psd_resp_status_t;
+
+/* Advertisement Protocol IE tuple field */
+BWL_PRE_PACKED_STRUCT struct wifi_p2psd_adp_tpl {
+	uint8   llm_pamebi; /* Query Response Length Limit bit 0-6, set to 0 plus
+				* Pre-Associated Message Exchange BSSID Independent bit 7, set to 0
+				*/
+	uint8   adp_id;     /* Advertisement Protocol ID: 0 for NQP Native Query Protocol */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2psd_adp_tpl wifi_p2psd_adp_tpl_t;
+
+/* Advertisement Protocol IE */
+BWL_PRE_PACKED_STRUCT struct wifi_p2psd_adp_ie {
+	uint8   id;     /* IE ID: 0x6c - 108 */
+	uint8   len;    /* IE length */
+	wifi_p2psd_adp_tpl_t adp_tpl;  /* Advertisement Protocol Tuple field. Only one
+				* tuple is defined for P2P Service Discovery
+				*/
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2psd_adp_ie wifi_p2psd_adp_ie_t;
+
+/* NQP Vendor-specific Content */
+BWL_PRE_PACKED_STRUCT struct wifi_p2psd_nqp_query_vsc {
+	uint8   oui_subtype;    /* OUI Subtype: 0x09 */
+	uint16  svc_updi;       /* Service Update Indicator */
+	uint8   svc_tlvs[1];    /* wifi_p2psd_qreq_tlv_t type for service request,
+				* wifi_p2psd_qresp_tlv_t type for service response
+				*/
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2psd_nqp_query_vsc wifi_p2psd_nqp_query_vsc_t;
+
+/* Service Request TLV */
+BWL_PRE_PACKED_STRUCT struct wifi_p2psd_qreq_tlv {
+	uint16  len;            /* Length: 5 plus size of Query Data */
+	uint8   svc_prot;       /* Service Protocol Type */
+	uint8   svc_tscid;      /* Service Transaction ID */
+	uint8   query_data[1];  /* Query Data, passed in from above Layer 2 */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2psd_qreq_tlv wifi_p2psd_qreq_tlv_t;
+
+/* Query Request Frame, defined in generic format, instead of NQP specific */
+BWL_PRE_PACKED_STRUCT struct wifi_p2psd_qreq_frame {
+	uint16  info_id;    /* Info ID: 0xDDDD */
+	uint16  len;        /* Length of service request TLV, 5 plus the size of request data */
+	uint8   oui[3];     /* WFA OUI: 0x0050F2 */
+	uint8   qreq_vsc[1]; /* Vendor-specific Content: wifi_p2psd_nqp_query_vsc_t type for NQP */
+
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2psd_qreq_frame wifi_p2psd_qreq_frame_t;
+
+/* GAS Initial Request AF body, "elts" in wifi_p2p_pub_act_frame */
+BWL_PRE_PACKED_STRUCT struct wifi_p2psd_gas_ireq_frame {
+	wifi_p2psd_adp_ie_t     adp_ie;     /* Advertisement Protocol IE */
+	uint16                  qreq_len;   /* Query Request Length */
+	uint8   qreq_frm[1];    /* Query Request Frame wifi_p2psd_qreq_frame_t */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2psd_gas_ireq_frame wifi_p2psd_gas_ireq_frame_t;
+
+/* Service Response TLV */
+BWL_PRE_PACKED_STRUCT struct wifi_p2psd_qresp_tlv {
+	uint16  len;                /* Length: 5 plus size of Query Data */
+	uint8   svc_prot;           /* Service Protocol Type */
+	uint8   svc_tscid;          /* Service Transaction ID */
+	uint8   status;             /* Value defined in Table 57 of P2P spec. */
+	uint8   query_data[1];      /* Response Data, passed in from above Layer 2 */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2psd_qresp_tlv wifi_p2psd_qresp_tlv_t;
+
+/* Query Response Frame, defined in generic format, instead of NQP specific */
+BWL_PRE_PACKED_STRUCT struct wifi_p2psd_qresp_frame {
+	uint16  info_id;    /* Info ID: 0xDDDD */
+	uint16  len;        /* Lenth of service response TLV, 6 plus the size of resp data */
+	uint8   oui[3];     /* WFA OUI: 0x0050F2 */
+	uint8   qresp_vsc[1]; /* Vendor-specific Content: wifi_p2psd_qresp_tlv_t type for NQP */
+
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2psd_qresp_frame wifi_p2psd_qresp_frame_t;
+
+/* GAS Initial Response AF body, "elts" in wifi_p2p_pub_act_frame */
+BWL_PRE_PACKED_STRUCT struct wifi_p2psd_gas_iresp_frame {
+	uint16      status;     /* Value defined in Table 7-23 of IEEE P802.11u */
+	uint16  cb_delay;       /* GAS Comeback Delay */
+	wifi_p2psd_adp_ie_t adp_ie;     /* Advertisement Protocol IE */
+	uint16      qresp_len;  /* Query Response Length */
+	uint8   qresp_frm[1];   /* Query Response Frame wifi_p2psd_qresp_frame_t */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2psd_gas_iresp_frame wifi_p2psd_gas_iresp_frame_t;
+
+/* GAS Comeback Response AF body, "elts" in wifi_p2p_pub_act_frame */
+BWL_PRE_PACKED_STRUCT struct wifi_p2psd_gas_cresp_frame {
+	uint16  status;         /* Value defined in Table 7-23 of IEEE P802.11u */
+	uint8   fragment_id;    /* Fragmentation ID */
+	uint16  cb_delay;       /* GAS Comeback Delay */
+	wifi_p2psd_adp_ie_t adp_ie;     /* Advertisement Protocol IE */
+	uint16  qresp_len;      /* Query Response Length */
+	uint8   qresp_frm[1];   /* Query Response Frame wifi_p2psd_qresp_frame_t */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2psd_gas_cresp_frame wifi_p2psd_gas_cresp_frame_t;
+
+/* Wi-Fi GAS Public Action Frame */
+BWL_PRE_PACKED_STRUCT struct wifi_p2psd_gas_pub_act_frame {
+	uint8   category;       /* 0x04 Public Action Frame */
+	uint8   action;         /* 0x6c Advertisement Protocol */
+	uint8   dialog_token;   /* nonzero, identifies req/rsp transaction */
+	uint8   query_data[1];  /* Query Data. wifi_p2psd_gas_ireq_frame_t
+					 * or wifi_p2psd_gas_iresp_frame_t format
+					 */
+} BWL_POST_PACKED_STRUCT;
+typedef struct wifi_p2psd_gas_pub_act_frame wifi_p2psd_gas_pub_act_frame_t;
+
+/* This marks the end of a packed structure section. */
+#include <packed_section_end.h>
+
+#endif /* _P2P_H_ */
diff --git a/drivers/net/wireless/bcmdhd/include/proto/sdspi.h b/drivers/net/wireless/bcmdhd/include/proto/sdspi.h
new file mode 100644
index 000000000000..7353ff0d7c73
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/proto/sdspi.h
@@ -0,0 +1,76 @@
+/*
+ * SD-SPI Protocol Standard
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sdspi.h 277737 2011-08-16 17:54:59Z $
+ */
+
+#ifndef	_SD_SPI_H
+#define	_SD_SPI_H
+
+#define SPI_START_M		BITFIELD_MASK(1)	/* Bit [31] 	- Start Bit */
+#define SPI_START_S		31
+#define SPI_DIR_M		BITFIELD_MASK(1)	/* Bit [30] 	- Direction */
+#define SPI_DIR_S		30
+#define SPI_CMD_INDEX_M		BITFIELD_MASK(6)	/* Bits [29:24] - Command number */
+#define SPI_CMD_INDEX_S		24
+#define SPI_RW_M		BITFIELD_MASK(1)	/* Bit [23] 	- Read=0, Write=1 */
+#define SPI_RW_S		23
+#define SPI_FUNC_M		BITFIELD_MASK(3)	/* Bits [22:20]	- Function Number */
+#define SPI_FUNC_S		20
+#define SPI_RAW_M		BITFIELD_MASK(1)	/* Bit [19] 	- Read After Wr */
+#define SPI_RAW_S		19
+#define SPI_STUFF_M		BITFIELD_MASK(1)	/* Bit [18] 	- Stuff bit */
+#define SPI_STUFF_S		18
+#define SPI_BLKMODE_M		BITFIELD_MASK(1)	/* Bit [19] 	- Blockmode 1=blk */
+#define SPI_BLKMODE_S		19
+#define SPI_OPCODE_M		BITFIELD_MASK(1)	/* Bit [18] 	- OP Code */
+#define SPI_OPCODE_S		18
+#define SPI_ADDR_M		BITFIELD_MASK(17)	/* Bits [17:1] 	- Address */
+#define SPI_ADDR_S		1
+#define SPI_STUFF0_M		BITFIELD_MASK(1)	/* Bit [0] 	- Stuff bit */
+#define SPI_STUFF0_S		0
+
+#define SPI_RSP_START_M		BITFIELD_MASK(1)	/* Bit [7] 	- Start Bit (always 0) */
+#define SPI_RSP_START_S		7
+#define SPI_RSP_PARAM_ERR_M	BITFIELD_MASK(1)	/* Bit [6] 	- Parameter Error */
+#define SPI_RSP_PARAM_ERR_S	6
+#define SPI_RSP_RFU5_M		BITFIELD_MASK(1)	/* Bit [5] 	- RFU (Always 0) */
+#define SPI_RSP_RFU5_S		5
+#define SPI_RSP_FUNC_ERR_M	BITFIELD_MASK(1)	/* Bit [4] 	- Function number error */
+#define SPI_RSP_FUNC_ERR_S	4
+#define SPI_RSP_CRC_ERR_M	BITFIELD_MASK(1)	/* Bit [3] 	- COM CRC Error */
+#define SPI_RSP_CRC_ERR_S	3
+#define SPI_RSP_ILL_CMD_M	BITFIELD_MASK(1)	/* Bit [2] 	- Illegal Command error */
+#define SPI_RSP_ILL_CMD_S	2
+#define SPI_RSP_RFU1_M		BITFIELD_MASK(1)	/* Bit [1] 	- RFU (Always 0) */
+#define SPI_RSP_RFU1_S		1
+#define SPI_RSP_IDLE_M		BITFIELD_MASK(1)	/* Bit [0] 	- In idle state */
+#define SPI_RSP_IDLE_S		0
+
+/* SD-SPI Protocol Definitions */
+#define SDSPI_COMMAND_LEN	6	/* Number of bytes in an SD command */
+#define SDSPI_START_BLOCK	0xFE	/* SD Start Block Token */
+#define SDSPI_IDLE_PAD		0xFF	/* SD-SPI idle value for MOSI */
+#define SDSPI_START_BIT_MASK	0x80
+
+#endif /* _SD_SPI_H */
diff --git a/drivers/net/wireless/bcmdhd/include/proto/vlan.h b/drivers/net/wireless/bcmdhd/include/proto/vlan.h
new file mode 100644
index 000000000000..27f005537604
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/proto/vlan.h
@@ -0,0 +1,70 @@
+/*
+ * 802.1Q VLAN protocol definitions
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: vlan.h 277737 2011-08-16 17:54:59Z $
+ */
+
+
+#ifndef _vlan_h_
+#define _vlan_h_
+
+#ifndef _TYPEDEFS_H_
+#include <typedefs.h>
+#endif
+
+
+#include <packed_section_start.h>
+
+#define VLAN_VID_MASK		0xfff	
+#define	VLAN_CFI_SHIFT		12	
+#define VLAN_PRI_SHIFT		13	
+
+#define VLAN_PRI_MASK		7	
+
+#define	VLAN_TAG_LEN		4
+#define	VLAN_TAG_OFFSET		(2 * ETHER_ADDR_LEN)	
+
+#define VLAN_TPID		0x8100	
+
+struct ethervlan_header {
+	uint8	ether_dhost[ETHER_ADDR_LEN];
+	uint8	ether_shost[ETHER_ADDR_LEN];
+	uint16	vlan_type;		
+	uint16	vlan_tag;		
+	uint16	ether_type;
+};
+
+#define	ETHERVLAN_HDR_LEN	(ETHER_HDR_LEN + VLAN_TAG_LEN)
+
+
+
+#include <packed_section_end.h>
+
+#define ETHERVLAN_MOVE_HDR(d, s) \
+do { \
+	struct ethervlan_header t; \
+	t = *(struct ethervlan_header *)(s); \
+	*(struct ethervlan_header *)(d) = t; \
+} while (0)
+
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/proto/wpa.h b/drivers/net/wireless/bcmdhd/include/proto/wpa.h
new file mode 100644
index 000000000000..7361cbf20b06
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/proto/wpa.h
@@ -0,0 +1,168 @@
+/*
+ * Fundamental types and constants relating to WPA
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wpa.h 285437 2011-09-21 22:16:56Z $
+ */
+
+
+#ifndef _proto_wpa_h_
+#define _proto_wpa_h_
+
+#include <typedefs.h>
+#include <proto/ethernet.h>
+
+
+
+#include <packed_section_start.h>
+
+
+
+
+#define DOT11_RC_INVALID_WPA_IE		13	
+#define DOT11_RC_MIC_FAILURE		14	
+#define DOT11_RC_4WH_TIMEOUT		15	
+#define DOT11_RC_GTK_UPDATE_TIMEOUT	16	
+#define DOT11_RC_WPA_IE_MISMATCH	17	
+#define DOT11_RC_INVALID_MC_CIPHER	18	
+#define DOT11_RC_INVALID_UC_CIPHER	19	
+#define DOT11_RC_INVALID_AKMP		20	
+#define DOT11_RC_BAD_WPA_VERSION	21	
+#define DOT11_RC_INVALID_WPA_CAP	22	
+#define DOT11_RC_8021X_AUTH_FAIL	23	
+
+#define WPA2_PMKID_LEN	16
+
+
+typedef BWL_PRE_PACKED_STRUCT struct
+{
+	uint8 tag;	
+	uint8 length;	
+	uint8 oui[3];	
+	uint8 oui_type;	
+	BWL_PRE_PACKED_STRUCT struct {
+		uint8 low;
+		uint8 high;
+	} BWL_POST_PACKED_STRUCT version;	
+} BWL_POST_PACKED_STRUCT wpa_ie_fixed_t;
+#define WPA_IE_OUITYPE_LEN	4
+#define WPA_IE_FIXED_LEN	8
+#define WPA_IE_TAG_FIXED_LEN	6
+
+typedef BWL_PRE_PACKED_STRUCT struct {
+	uint8 tag;	
+	uint8 length;	
+	BWL_PRE_PACKED_STRUCT struct {
+		uint8 low;
+		uint8 high;
+	} BWL_POST_PACKED_STRUCT version;	
+} BWL_POST_PACKED_STRUCT wpa_rsn_ie_fixed_t;
+#define WPA_RSN_IE_FIXED_LEN	4
+#define WPA_RSN_IE_TAG_FIXED_LEN	2
+typedef uint8 wpa_pmkid_t[WPA2_PMKID_LEN];
+
+
+typedef BWL_PRE_PACKED_STRUCT struct
+{
+	uint8 oui[3];
+	uint8 type;
+} BWL_POST_PACKED_STRUCT wpa_suite_t, wpa_suite_mcast_t;
+#define WPA_SUITE_LEN	4
+
+
+typedef BWL_PRE_PACKED_STRUCT struct
+{
+	BWL_PRE_PACKED_STRUCT struct {
+		uint8 low;
+		uint8 high;
+	} BWL_POST_PACKED_STRUCT count;
+	wpa_suite_t list[1];
+} BWL_POST_PACKED_STRUCT wpa_suite_ucast_t, wpa_suite_auth_key_mgmt_t;
+#define WPA_IE_SUITE_COUNT_LEN	2
+typedef BWL_PRE_PACKED_STRUCT struct
+{
+	BWL_PRE_PACKED_STRUCT struct {
+		uint8 low;
+		uint8 high;
+	} BWL_POST_PACKED_STRUCT count;
+	wpa_pmkid_t list[1];
+} BWL_POST_PACKED_STRUCT wpa_pmkid_list_t;
+
+
+#define WPA_CIPHER_NONE		0	
+#define WPA_CIPHER_WEP_40	1	
+#define WPA_CIPHER_TKIP		2	
+#define WPA_CIPHER_AES_OCB	3	
+#define WPA_CIPHER_AES_CCM	4	
+#define WPA_CIPHER_WEP_104	5	
+#define WPA_CIPHER_BIP		6	
+#define WPA_CIPHER_TPK		7	
+
+
+#define IS_WPA_CIPHER(cipher)	((cipher) == WPA_CIPHER_NONE || \
+				 (cipher) == WPA_CIPHER_WEP_40 || \
+				 (cipher) == WPA_CIPHER_WEP_104 || \
+				 (cipher) == WPA_CIPHER_TKIP || \
+				 (cipher) == WPA_CIPHER_AES_OCB || \
+				 (cipher) == WPA_CIPHER_AES_CCM || \
+				 (cipher) == WPA_CIPHER_TPK)
+
+
+
+#define WPA_TKIP_CM_DETECT	60	
+#define WPA_TKIP_CM_BLOCK	60	
+
+
+#define RSN_CAP_LEN		2	
+
+
+#define RSN_CAP_PREAUTH			0x0001
+#define RSN_CAP_NOPAIRWISE		0x0002
+#define RSN_CAP_PTK_REPLAY_CNTR_MASK	0x000C
+#define RSN_CAP_PTK_REPLAY_CNTR_SHIFT	2
+#define RSN_CAP_GTK_REPLAY_CNTR_MASK	0x0030
+#define RSN_CAP_GTK_REPLAY_CNTR_SHIFT	4
+#define RSN_CAP_1_REPLAY_CNTR		0
+#define RSN_CAP_2_REPLAY_CNTRS		1
+#define RSN_CAP_4_REPLAY_CNTRS		2
+#define RSN_CAP_16_REPLAY_CNTRS		3
+
+
+#define WPA_CAP_4_REPLAY_CNTRS		RSN_CAP_4_REPLAY_CNTRS
+#define WPA_CAP_16_REPLAY_CNTRS		RSN_CAP_16_REPLAY_CNTRS
+#define WPA_CAP_REPLAY_CNTR_SHIFT	RSN_CAP_PTK_REPLAY_CNTR_SHIFT
+#define WPA_CAP_REPLAY_CNTR_MASK	RSN_CAP_PTK_REPLAY_CNTR_MASK
+
+
+#define WPA_CAP_PEER_KEY_ENABLE		(0x1 << 1)	
+
+
+#define WPA_CAP_LEN	RSN_CAP_LEN	
+#define WPA_PMKID_CNT_LEN	2	
+
+#define	WPA_CAP_WPA2_PREAUTH		RSN_CAP_PREAUTH
+
+
+
+#include <packed_section_end.h>
+
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/sbchipc.h b/drivers/net/wireless/bcmdhd/include/sbchipc.h
new file mode 100644
index 000000000000..53bd2a1d5580
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/sbchipc.h
@@ -0,0 +1,1778 @@
+/*
+ * SiliconBackplane Chipcommon core hardware definitions.
+ *
+ * The chipcommon core provides chip identification, SB control,
+ * JTAG, 0/1/2 UARTs, clock frequency control, a watchdog interrupt timer,
+ * GPIO interface, extbus, and support for serial and parallel flashes.
+ *
+ * $Id: sbchipc.h 333924 2012-05-18 04:48:52Z $
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ */
+
+
+#ifndef	_SBCHIPC_H
+#define	_SBCHIPC_H
+
+#ifndef _LANGUAGE_ASSEMBLY
+
+
+#ifndef PAD
+#define	_PADLINE(line)	pad ## line
+#define	_XSTR(line)	_PADLINE(line)
+#define	PAD		_XSTR(__LINE__)
+#endif	
+
+typedef struct eci_prerev35 {
+	uint32	eci_output;
+	uint32	eci_control;
+	uint32	eci_inputlo;
+	uint32	eci_inputmi;
+	uint32	eci_inputhi;
+	uint32	eci_inputintpolaritylo;
+	uint32	eci_inputintpolaritymi;
+	uint32	eci_inputintpolarityhi;
+	uint32	eci_intmasklo;
+	uint32	eci_intmaskmi;
+	uint32	eci_intmaskhi;
+	uint32	eci_eventlo;
+	uint32	eci_eventmi;
+	uint32	eci_eventhi;
+	uint32	eci_eventmasklo;
+	uint32	eci_eventmaskmi;
+	uint32	eci_eventmaskhi;
+	uint32	PAD[3];
+} eci_prerev35_t;
+
+typedef struct eci_rev35 {
+	uint32	eci_outputlo;
+	uint32	eci_outputhi;
+	uint32	eci_controllo;
+	uint32	eci_controlhi;
+	uint32	eci_inputlo;
+	uint32	eci_inputhi;
+	uint32	eci_inputintpolaritylo;
+	uint32	eci_inputintpolarityhi;
+	uint32	eci_intmasklo;
+	uint32	eci_intmaskhi;
+	uint32	eci_eventlo;
+	uint32	eci_eventhi;
+	uint32	eci_eventmasklo;
+	uint32	eci_eventmaskhi;
+	uint32	eci_auxtx;
+	uint32	eci_auxrx;
+	uint32	eci_datatag;
+	uint32	eci_uartescvalue;
+	uint32	eci_autobaudctr;
+	uint32	eci_uartfifolevel;
+} eci_rev35_t;
+
+typedef volatile struct {
+	uint32	chipid;			
+	uint32	capabilities;
+	uint32	corecontrol;		
+	uint32	bist;
+
+	
+	uint32	otpstatus;		
+	uint32	otpcontrol;
+	uint32	otpprog;
+	uint32	otplayout;		
+
+	
+	uint32	intstatus;		
+	uint32	intmask;
+
+	
+	uint32	chipcontrol;		
+	uint32	chipstatus;		
+
+	
+	uint32	jtagcmd;		
+	uint32	jtagir;
+	uint32	jtagdr;
+	uint32	jtagctrl;
+
+	
+	uint32	flashcontrol;		
+	uint32	flashaddress;
+	uint32	flashdata;
+	uint32	PAD[1];
+
+	
+	uint32	broadcastaddress;	
+	uint32	broadcastdata;
+
+	
+	uint32	gpiopullup;		
+	uint32	gpiopulldown;		
+	uint32	gpioin;			
+	uint32	gpioout;		
+	uint32	gpioouten;		
+	uint32	gpiocontrol;		
+	uint32	gpiointpolarity;	
+	uint32	gpiointmask;		
+
+	
+	uint32	gpioevent;
+	uint32	gpioeventintmask;
+
+	
+	uint32	watchdog;		
+
+	
+	uint32	gpioeventintpolarity;
+
+	
+	uint32  gpiotimerval;		
+	uint32  gpiotimeroutmask;
+
+	
+	uint32	clockcontrol_n;		
+	uint32	clockcontrol_sb;	
+	uint32	clockcontrol_pci;	
+	uint32	clockcontrol_m2;	
+	uint32	clockcontrol_m3;	
+	uint32	clkdiv;			
+	uint32	gpiodebugsel;		
+	uint32	capabilities_ext;               	
+
+	
+	uint32	pll_on_delay;		
+	uint32	fref_sel_delay;
+	uint32	slow_clk_ctl;		
+	uint32	PAD;
+
+	
+	uint32	system_clk_ctl;		
+	uint32	clkstatestretch;
+	uint32	PAD[2];
+
+	
+	uint32	bp_addrlow;		
+	uint32	bp_addrhigh;
+	uint32	bp_data;
+	uint32	PAD;
+	uint32	bp_indaccess;
+	
+	uint32	gsioctrl;
+	uint32	gsioaddress;
+	uint32	gsiodata;
+
+	
+	uint32	clkdiv2;
+	uint32	PAD[2];
+
+	
+	uint32	eromptr;		
+
+	
+	uint32	pcmcia_config;		
+	uint32	pcmcia_memwait;
+	uint32	pcmcia_attrwait;
+	uint32	pcmcia_iowait;
+	uint32	ide_config;
+	uint32	ide_memwait;
+	uint32	ide_attrwait;
+	uint32	ide_iowait;
+	uint32	prog_config;
+	uint32	prog_waitcount;
+	uint32	flash_config;
+	uint32	flash_waitcount;
+	uint32  SECI_config;		
+	uint32	SECI_status;
+	uint32	SECI_statusmask;
+	uint32	SECI_rxnibchanged;
+
+	uint32	PAD[20];
+
+	
+	uint32	sromcontrol;		
+	uint32	sromaddress;
+	uint32	sromdata;
+	uint32	PAD[9];		
+	uint32  seci_uart_data;		
+	uint32  seci_uart_bauddiv;
+	uint32  seci_uart_fcr;
+	uint32  seci_uart_lcr;
+	uint32  seci_uart_mcr;
+	uint32  seci_uart_lsr;
+	uint32  seci_uart_msr;
+	uint32  seci_uart_baudadj;
+	
+	uint32	clk_ctl_st;		
+	uint32	hw_war;
+	uint32	PAD[70];
+
+	
+	uint8	uart0data;		
+	uint8	uart0imr;
+	uint8	uart0fcr;
+	uint8	uart0lcr;
+	uint8	uart0mcr;
+	uint8	uart0lsr;
+	uint8	uart0msr;
+	uint8	uart0scratch;
+	uint8	PAD[248];		
+
+	uint8	uart1data;		
+	uint8	uart1imr;
+	uint8	uart1fcr;
+	uint8	uart1lcr;
+	uint8	uart1mcr;
+	uint8	uart1lsr;
+	uint8	uart1msr;
+	uint8	uart1scratch;
+	uint32	PAD[126];
+
+	
+	
+	uint32	pmucontrol;		
+	uint32	pmucapabilities;
+	uint32	pmustatus;
+	uint32	res_state;
+	uint32	res_pending;
+	uint32	pmutimer;
+	uint32	min_res_mask;
+	uint32	max_res_mask;
+	uint32	res_table_sel;
+	uint32	res_dep_mask;
+	uint32	res_updn_timer;
+	uint32	res_timer;
+	uint32	clkstretch;
+	uint32	pmuwatchdog;
+	uint32	gpiosel;		
+	uint32	gpioenable;		
+	uint32	res_req_timer_sel;
+	uint32	res_req_timer;
+	uint32	res_req_mask;
+	uint32	PAD;
+	uint32	chipcontrol_addr;	
+	uint32	chipcontrol_data;	
+	uint32	regcontrol_addr;
+	uint32	regcontrol_data;
+	uint32	pllcontrol_addr;
+	uint32	pllcontrol_data;
+	uint32	pmustrapopt;		
+	uint32	pmu_xtalfreq;		
+	uint32	PAD[100];
+	uint16	sromotp[768];
+} chipcregs_t;
+
+#endif 
+
+
+#define	CC_CHIPID		0
+#define	CC_CAPABILITIES		4
+#define	CC_CHIPST		0x2c
+#define	CC_EROMPTR		0xfc
+
+
+#define CC_OTPST		0x10
+#define	CC_JTAGCMD		0x30
+#define	CC_JTAGIR		0x34
+#define	CC_JTAGDR		0x38
+#define	CC_JTAGCTRL		0x3c
+#define	CC_GPIOPU		0x58
+#define	CC_GPIOPD		0x5c
+#define	CC_GPIOIN		0x60
+#define	CC_GPIOOUT		0x64
+#define	CC_GPIOOUTEN		0x68
+#define	CC_GPIOCTRL		0x6c
+#define	CC_GPIOPOL		0x70
+#define	CC_GPIOINTM		0x74
+#define	CC_WATCHDOG		0x80
+#define	CC_CLKC_N		0x90
+#define	CC_CLKC_M0		0x94
+#define	CC_CLKC_M1		0x98
+#define	CC_CLKC_M2		0x9c
+#define	CC_CLKC_M3		0xa0
+#define	CC_CLKDIV		0xa4
+#define	CC_SYS_CLK_CTL		0xc0
+#define	CC_CLK_CTL_ST		SI_CLK_CTL_ST
+#define	PMU_CTL			0x600
+#define	PMU_CAP			0x604
+#define	PMU_ST			0x608
+#define PMU_RES_STATE		0x60c
+#define PMU_TIMER		0x614
+#define	PMU_MIN_RES_MASK	0x618
+#define	PMU_MAX_RES_MASK	0x61c
+#define CC_CHIPCTL_ADDR         0x650
+#define CC_CHIPCTL_DATA         0x654
+#define PMU_REG_CONTROL_ADDR	0x658
+#define PMU_REG_CONTROL_DATA	0x65C
+#define PMU_PLL_CONTROL_ADDR 	0x660
+#define PMU_PLL_CONTROL_DATA 	0x664
+#define	CC_SROM_OTP		0x800		
+
+
+#define	CID_ID_MASK		0x0000ffff	
+#define	CID_REV_MASK		0x000f0000	
+#define	CID_REV_SHIFT		16		
+#define	CID_PKG_MASK		0x00f00000	
+#define	CID_PKG_SHIFT		20		
+#define	CID_CC_MASK		0x0f000000	
+#define CID_CC_SHIFT		24
+#define	CID_TYPE_MASK		0xf0000000	
+#define CID_TYPE_SHIFT		28
+
+
+#define	CC_CAP_UARTS_MASK	0x00000003	
+#define CC_CAP_MIPSEB		0x00000004	
+#define CC_CAP_UCLKSEL		0x00000018	
+#define CC_CAP_UINTCLK		0x00000008	
+#define CC_CAP_UARTGPIO		0x00000020	
+#define CC_CAP_EXTBUS_MASK	0x000000c0	
+#define CC_CAP_EXTBUS_NONE	0x00000000	
+#define CC_CAP_EXTBUS_FULL	0x00000040	
+#define CC_CAP_EXTBUS_PROG	0x00000080	
+#define	CC_CAP_FLASH_MASK	0x00000700	
+#define	CC_CAP_PLL_MASK		0x00038000	
+#define CC_CAP_PWR_CTL		0x00040000	
+#define CC_CAP_OTPSIZE		0x00380000	
+#define CC_CAP_OTPSIZE_SHIFT	19		
+#define CC_CAP_OTPSIZE_BASE	5		
+#define CC_CAP_JTAGP		0x00400000	
+#define CC_CAP_ROM		0x00800000	
+#define CC_CAP_BKPLN64		0x08000000	
+#define	CC_CAP_PMU		0x10000000	
+#define	CC_CAP_ECI		0x20000000	
+#define	CC_CAP_SROM		0x40000000	
+#define	CC_CAP_NFLASH		0x80000000	
+
+#define	CC_CAP2_SECI		0x00000001	
+#define	CC_CAP2_GSIO		0x00000002	
+
+
+#define CC_CAP_EXT_SECI_PRESENT   0x00000001    
+
+
+#define PLL_NONE		0x00000000
+#define PLL_TYPE1		0x00010000	
+#define PLL_TYPE2		0x00020000	
+#define PLL_TYPE3		0x00030000	
+#define PLL_TYPE4		0x00008000	
+#define PLL_TYPE5		0x00018000	
+#define PLL_TYPE6		0x00028000	
+#define PLL_TYPE7		0x00038000	
+
+
+#define	ILP_CLOCK		32000
+
+
+#define	ALP_CLOCK		20000000
+
+
+#define	HT_CLOCK		80000000
+
+
+#define CC_UARTCLKO		0x00000001	
+#define	CC_SE			0x00000002	
+#define CC_ASYNCGPIO	0x00000004	
+#define CC_UARTCLKEN		0x00000008	
+
+
+#define CHIPCTRL_4321A0_DEFAULT	0x3a4
+#define CHIPCTRL_4321A1_DEFAULT	0x0a4
+#define CHIPCTRL_4321_PLL_DOWN	0x800000	
+
+
+#define OTPS_OL_MASK		0x000000ff
+#define OTPS_OL_MFG		0x00000001	
+#define OTPS_OL_OR1		0x00000002	
+#define OTPS_OL_OR2		0x00000004	
+#define OTPS_OL_GU		0x00000008	
+#define OTPS_GUP_MASK		0x00000f00
+#define OTPS_GUP_SHIFT		8
+#define OTPS_GUP_HW		0x00000100	
+#define OTPS_GUP_SW		0x00000200	
+#define OTPS_GUP_CI		0x00000400	
+#define OTPS_GUP_FUSE		0x00000800	
+#define OTPS_READY		0x00001000
+#define OTPS_RV(x)		(1 << (16 + (x)))	
+#define OTPS_RV_MASK		0x0fff0000
+
+
+#define OTPC_PROGSEL		0x00000001
+#define OTPC_PCOUNT_MASK	0x0000000e
+#define OTPC_PCOUNT_SHIFT	1
+#define OTPC_VSEL_MASK		0x000000f0
+#define OTPC_VSEL_SHIFT		4
+#define OTPC_TMM_MASK		0x00000700
+#define OTPC_TMM_SHIFT		8
+#define OTPC_ODM		0x00000800
+#define OTPC_PROGEN		0x80000000
+
+
+#define OTPP_COL_MASK		0x000000ff
+#define OTPP_COL_SHIFT		0
+#define OTPP_ROW_MASK		0x0000ff00
+#define OTPP_ROW_SHIFT		8
+#define OTPP_OC_MASK		0x0f000000
+#define OTPP_OC_SHIFT		24
+#define OTPP_READERR		0x10000000
+#define OTPP_VALUE_MASK		0x20000000
+#define OTPP_VALUE_SHIFT	29
+#define OTPP_START_BUSY		0x80000000
+#define	OTPP_READ		0x40000000	
+
+
+#define OTP_CISFORMAT_NEW	0x80000000
+
+
+#define OTPPOC_READ		0
+#define OTPPOC_BIT_PROG		1
+#define OTPPOC_VERIFY		3
+#define OTPPOC_INIT		4
+#define OTPPOC_SET		5
+#define OTPPOC_RESET		6
+#define OTPPOC_OCST		7
+#define OTPPOC_ROW_LOCK		8
+#define OTPPOC_PRESCN_TEST	9
+
+
+
+#define	JTAGM_CREV_OLD		10	
+#define	JTAGM_CREV_IRP		22	
+#define	JTAGM_CREV_RTI		28	
+
+
+#define JCMD_START		0x80000000
+#define JCMD_BUSY		0x80000000
+#define JCMD_STATE_MASK		0x60000000
+#define JCMD_STATE_TLR		0x00000000	
+#define JCMD_STATE_PIR		0x20000000	
+#define JCMD_STATE_PDR		0x40000000	
+#define JCMD_STATE_RTI		0x60000000	
+#define JCMD0_ACC_MASK		0x0000f000
+#define JCMD0_ACC_IRDR		0x00000000
+#define JCMD0_ACC_DR		0x00001000
+#define JCMD0_ACC_IR		0x00002000
+#define JCMD0_ACC_RESET		0x00003000
+#define JCMD0_ACC_IRPDR		0x00004000
+#define JCMD0_ACC_PDR		0x00005000
+#define JCMD0_IRW_MASK		0x00000f00
+#define JCMD_ACC_MASK		0x000f0000	
+#define JCMD_ACC_IRDR		0x00000000
+#define JCMD_ACC_DR		0x00010000
+#define JCMD_ACC_IR		0x00020000
+#define JCMD_ACC_RESET		0x00030000
+#define JCMD_ACC_IRPDR		0x00040000
+#define JCMD_ACC_PDR		0x00050000
+#define JCMD_ACC_PIR		0x00060000
+#define JCMD_ACC_IRDR_I		0x00070000	
+#define JCMD_ACC_DR_I		0x00080000	
+#define JCMD_IRW_MASK		0x00001f00
+#define JCMD_IRW_SHIFT		8
+#define JCMD_DRW_MASK		0x0000003f
+
+
+#define JCTRL_FORCE_CLK		4		
+#define JCTRL_EXT_EN		2		
+#define JCTRL_EN		1		
+
+
+#define	CLKD_SFLASH		0x0f000000
+#define	CLKD_SFLASH_SHIFT	24
+#define	CLKD_OTP		0x000f0000
+#define	CLKD_OTP_SHIFT		16
+#define	CLKD_JTAG		0x00000f00
+#define	CLKD_JTAG_SHIFT		8
+#define	CLKD_UART		0x000000ff
+
+#define	CLKD2_SROM		0x00000003
+
+
+#define	CI_GPIO			0x00000001	
+#define	CI_EI			0x00000002	
+#define	CI_TEMP			0x00000004	
+#define	CI_SIRQ			0x00000008	
+#define	CI_ECI			0x00000010	
+#define	CI_PMU			0x00000020	
+#define	CI_UART			0x00000040	
+#define	CI_WDRESET		0x80000000	
+
+
+#define SCC_SS_MASK		0x00000007	
+#define	SCC_SS_LPO		0x00000000	
+#define	SCC_SS_XTAL		0x00000001	
+#define	SCC_SS_PCI		0x00000002	
+#define SCC_LF			0x00000200	
+#define SCC_LP			0x00000400	
+#define SCC_FS			0x00000800	
+#define SCC_IP			0x00001000	
+#define SCC_XC			0x00002000	
+#define SCC_XP			0x00004000	
+#define SCC_CD_MASK		0xffff0000	
+#define SCC_CD_SHIFT		16
+
+
+#define	SYCC_IE			0x00000001	
+#define	SYCC_AE			0x00000002	
+#define	SYCC_FP			0x00000004	
+#define	SYCC_AR			0x00000008	
+#define	SYCC_HR			0x00000010	
+#define SYCC_CD_MASK		0xffff0000	
+#define SYCC_CD_SHIFT		16
+
+
+#define	BPIA_BYTEEN		0x0000000f
+#define	BPIA_SZ1		0x00000001
+#define	BPIA_SZ2		0x00000003
+#define	BPIA_SZ4		0x00000007
+#define	BPIA_SZ8		0x0000000f
+#define	BPIA_WRITE		0x00000100
+#define	BPIA_START		0x00000200
+#define	BPIA_BUSY		0x00000200
+#define	BPIA_ERROR		0x00000400
+
+
+#define	CF_EN			0x00000001	
+#define	CF_EM_MASK		0x0000000e	
+#define	CF_EM_SHIFT		1
+#define	CF_EM_FLASH		0		
+#define	CF_EM_SYNC		2		
+#define	CF_EM_PCMCIA		4		
+#define	CF_DS			0x00000010	
+#define	CF_BS			0x00000020	
+#define	CF_CD_MASK		0x000000c0	
+#define	CF_CD_SHIFT		6
+#define	CF_CD_DIV2		0x00000000	
+#define	CF_CD_DIV3		0x00000040	
+#define	CF_CD_DIV4		0x00000080	
+#define	CF_CE			0x00000100	
+#define	CF_SB			0x00000200	
+
+
+#define	PM_W0_MASK		0x0000003f	
+#define	PM_W1_MASK		0x00001f00	
+#define	PM_W1_SHIFT		8
+#define	PM_W2_MASK		0x001f0000	
+#define	PM_W2_SHIFT		16
+#define	PM_W3_MASK		0x1f000000	
+#define	PM_W3_SHIFT		24
+
+
+#define	PA_W0_MASK		0x0000003f	
+#define	PA_W1_MASK		0x00001f00	
+#define	PA_W1_SHIFT		8
+#define	PA_W2_MASK		0x001f0000	
+#define	PA_W2_SHIFT		16
+#define	PA_W3_MASK		0x1f000000	
+#define	PA_W3_SHIFT		24
+
+
+#define	PI_W0_MASK		0x0000003f	
+#define	PI_W1_MASK		0x00001f00	
+#define	PI_W1_SHIFT		8
+#define	PI_W2_MASK		0x001f0000	
+#define	PI_W2_SHIFT		16
+#define	PI_W3_MASK		0x1f000000	
+#define	PI_W3_SHIFT		24
+
+
+#define	PW_W0_MASK		0x0000001f	
+#define	PW_W1_MASK		0x00001f00	
+#define	PW_W1_SHIFT		8
+#define	PW_W2_MASK		0x001f0000	
+#define	PW_W2_SHIFT		16
+#define	PW_W3_MASK		0x1f000000	
+#define	PW_W3_SHIFT		24
+
+#define PW_W0       		0x0000000c
+#define PW_W1       		0x00000a00
+#define PW_W2       		0x00020000
+#define PW_W3       		0x01000000
+
+
+#define	FW_W0_MASK		0x0000003f	
+#define	FW_W1_MASK		0x00001f00	
+#define	FW_W1_SHIFT		8
+#define	FW_W2_MASK		0x001f0000	
+#define	FW_W2_SHIFT		16
+#define	FW_W3_MASK		0x1f000000	
+#define	FW_W3_SHIFT		24
+
+
+#define	SRC_START		0x80000000
+#define	SRC_BUSY		0x80000000
+#define	SRC_OPCODE		0x60000000
+#define	SRC_OP_READ		0x00000000
+#define	SRC_OP_WRITE		0x20000000
+#define	SRC_OP_WRDIS		0x40000000
+#define	SRC_OP_WREN		0x60000000
+#define	SRC_OTPSEL		0x00000010
+#define	SRC_LOCK		0x00000008
+#define	SRC_SIZE_MASK		0x00000006
+#define	SRC_SIZE_1K		0x00000000
+#define	SRC_SIZE_4K		0x00000002
+#define	SRC_SIZE_16K		0x00000004
+#define	SRC_SIZE_SHIFT		1
+#define	SRC_PRESENT		0x00000001
+
+
+#define	PCTL_ILP_DIV_MASK	0xffff0000
+#define	PCTL_ILP_DIV_SHIFT	16
+#define PCTL_PLL_PLLCTL_UPD	0x00000400	
+#define PCTL_NOILP_ON_WAIT	0x00000200	
+#define	PCTL_HT_REQ_EN		0x00000100
+#define	PCTL_ALP_REQ_EN		0x00000080
+#define	PCTL_XTALFREQ_MASK	0x0000007c
+#define	PCTL_XTALFREQ_SHIFT	2
+#define	PCTL_ILP_DIV_EN		0x00000002
+#define	PCTL_LPO_SEL		0x00000001
+
+
+#define CSTRETCH_HT		0xffff0000
+#define CSTRETCH_ALP		0x0000ffff
+
+
+#define GPIO_ONTIME_SHIFT	16
+
+
+#define	CN_N1_MASK		0x3f		
+#define	CN_N2_MASK		0x3f00		
+#define	CN_N2_SHIFT		8
+#define	CN_PLLC_MASK		0xf0000		
+#define	CN_PLLC_SHIFT		16
+
+
+#define	CC_M1_MASK		0x3f		
+#define	CC_M2_MASK		0x3f00		
+#define	CC_M2_SHIFT		8
+#define	CC_M3_MASK		0x3f0000	
+#define	CC_M3_SHIFT		16
+#define	CC_MC_MASK		0x1f000000	
+#define	CC_MC_SHIFT		24
+
+
+#define	CC_F6_2			0x02		
+#define	CC_F6_3			0x03		
+#define	CC_F6_4			0x05		
+#define	CC_F6_5			0x09
+#define	CC_F6_6			0x11
+#define	CC_F6_7			0x21
+
+#define	CC_F5_BIAS		5		
+
+#define	CC_MC_BYPASS		0x08
+#define	CC_MC_M1		0x04
+#define	CC_MC_M1M2		0x02
+#define	CC_MC_M1M2M3		0x01
+#define	CC_MC_M1M3		0x11
+
+
+#define	CC_T2_BIAS		2		
+#define	CC_T2M2_BIAS		3		
+
+#define	CC_T2MC_M1BYP		1
+#define	CC_T2MC_M2BYP		2
+#define	CC_T2MC_M3BYP		4
+
+
+#define	CC_T6_MMASK		1		
+#define	CC_T6_M0		120000000	
+#define	CC_T6_M1		100000000	
+#define	SB2MIPS_T6(sb)		(2 * (sb))
+
+
+#define	CC_CLOCK_BASE1		24000000	
+#define CC_CLOCK_BASE2		12500000	
+
+
+#define	CLKC_5350_N		0x0311
+#define	CLKC_5350_M		0x04020009
+
+
+#define FLASH_NONE		0x000		
+#define SFLASH_ST		0x100		
+#define SFLASH_AT		0x200		
+#define	PFLASH			0x700		
+
+
+#define	CC_CFG_EN		0x0001		
+#define	CC_CFG_EM_MASK		0x000e		
+#define	CC_CFG_EM_ASYNC		0x0000		
+#define	CC_CFG_EM_SYNC		0x0002		
+#define	CC_CFG_EM_PCMCIA	0x0004		
+#define	CC_CFG_EM_IDE		0x0006		
+#define	CC_CFG_DS		0x0010		
+#define	CC_CFG_CD_MASK		0x00e0		
+#define	CC_CFG_CE		0x0100		
+#define	CC_CFG_SB		0x0200		
+#define	CC_CFG_IS		0x0400		
+
+
+#define	CC_EB_BASE		0x1a000000	
+#define	CC_EB_PCMCIA_MEM	0x1a000000	
+#define	CC_EB_PCMCIA_IO		0x1a200000	
+#define	CC_EB_PCMCIA_CFG	0x1a400000	
+#define	CC_EB_IDE		0x1a800000	
+#define	CC_EB_PCMCIA1_MEM	0x1a800000	
+#define	CC_EB_PCMCIA1_IO	0x1aa00000	
+#define	CC_EB_PCMCIA1_CFG	0x1ac00000	
+#define	CC_EB_PROGIF		0x1b000000	
+
+
+
+#define SFLASH_OPCODE		0x000000ff
+#define SFLASH_ACTION		0x00000700
+#define	SFLASH_CS_ACTIVE	0x00001000	
+#define SFLASH_START		0x80000000
+#define SFLASH_BUSY		SFLASH_START
+
+
+#define	SFLASH_ACT_OPONLY	0x0000		
+#define	SFLASH_ACT_OP1D		0x0100		
+#define	SFLASH_ACT_OP3A		0x0200		
+#define	SFLASH_ACT_OP3A1D	0x0300		
+#define	SFLASH_ACT_OP3A4D	0x0400		
+#define	SFLASH_ACT_OP3A4X4D	0x0500		
+#define	SFLASH_ACT_OP3A1X4D	0x0700		
+
+
+#define SFLASH_ST_WREN		0x0006		
+#define SFLASH_ST_WRDIS		0x0004		
+#define SFLASH_ST_RDSR		0x0105		
+#define SFLASH_ST_WRSR		0x0101		
+#define SFLASH_ST_READ		0x0303		
+#define SFLASH_ST_PP		0x0302		
+#define SFLASH_ST_SE		0x02d8		
+#define SFLASH_ST_BE		0x00c7		
+#define SFLASH_ST_DP		0x00b9		
+#define SFLASH_ST_RES		0x03ab		
+#define SFLASH_ST_CSA		0x1000		
+#define SFLASH_ST_SSE		0x0220		
+
+
+#define SFLASH_ST_WIP		0x01		
+#define SFLASH_ST_WEL		0x02		
+#define SFLASH_ST_BP_MASK	0x1c		
+#define SFLASH_ST_BP_SHIFT	2
+#define SFLASH_ST_SRWD		0x80		
+
+
+#define SFLASH_AT_READ				0x07e8
+#define SFLASH_AT_PAGE_READ			0x07d2
+#define SFLASH_AT_BUF1_READ
+#define SFLASH_AT_BUF2_READ
+#define SFLASH_AT_STATUS			0x01d7
+#define SFLASH_AT_BUF1_WRITE			0x0384
+#define SFLASH_AT_BUF2_WRITE			0x0387
+#define SFLASH_AT_BUF1_ERASE_PROGRAM		0x0283
+#define SFLASH_AT_BUF2_ERASE_PROGRAM		0x0286
+#define SFLASH_AT_BUF1_PROGRAM			0x0288
+#define SFLASH_AT_BUF2_PROGRAM			0x0289
+#define SFLASH_AT_PAGE_ERASE			0x0281
+#define SFLASH_AT_BLOCK_ERASE			0x0250
+#define SFLASH_AT_BUF1_WRITE_ERASE_PROGRAM	0x0382
+#define SFLASH_AT_BUF2_WRITE_ERASE_PROGRAM	0x0385
+#define SFLASH_AT_BUF1_LOAD			0x0253
+#define SFLASH_AT_BUF2_LOAD			0x0255
+#define SFLASH_AT_BUF1_COMPARE			0x0260
+#define SFLASH_AT_BUF2_COMPARE			0x0261
+#define SFLASH_AT_BUF1_REPROGRAM		0x0258
+#define SFLASH_AT_BUF2_REPROGRAM		0x0259
+
+
+#define SFLASH_AT_READY				0x80
+#define SFLASH_AT_MISMATCH			0x40
+#define SFLASH_AT_ID_MASK			0x38
+#define SFLASH_AT_ID_SHIFT			3
+
+
+#define GSIO_START			0x80000000
+#define GSIO_BUSY			GSIO_START
+
+
+
+#define UART_RX		0	
+#define UART_TX		0	
+#define UART_DLL	0	
+#define UART_IER	1	
+#define UART_DLM	1	
+#define UART_IIR	2	
+#define UART_FCR	2	
+#define UART_LCR	3	
+#define UART_MCR	4	
+#define UART_LSR	5	
+#define UART_MSR	6	
+#define UART_SCR	7	
+#define UART_LCR_DLAB	0x80	
+#define UART_LCR_WLEN8	0x03	
+#define UART_MCR_OUT2	0x08	
+#define UART_MCR_LOOP	0x10	
+#define UART_LSR_RX_FIFO 	0x80	
+#define UART_LSR_TDHR		0x40	
+#define UART_LSR_THRE		0x20	
+#define UART_LSR_BREAK		0x10	
+#define UART_LSR_FRAMING	0x08	
+#define UART_LSR_PARITY		0x04	
+#define UART_LSR_OVERRUN	0x02	
+#define UART_LSR_RXRDY		0x01	
+#define UART_FCR_FIFO_ENABLE 1	
+
+
+#define UART_IIR_FIFO_MASK	0xc0	
+#define UART_IIR_INT_MASK	0xf	
+#define UART_IIR_MDM_CHG	0x0	
+#define UART_IIR_NOINT		0x1	
+#define UART_IIR_THRE		0x2	
+#define UART_IIR_RCVD_DATA	0x4	
+#define UART_IIR_RCVR_STATUS 	0x6	
+#define UART_IIR_CHAR_TIME 	0xc	
+
+
+#define UART_IER_EDSSI	8	
+#define UART_IER_ELSI	4	
+#define UART_IER_ETBEI  2	
+#define UART_IER_ERBFI	1	
+
+
+#define PST_EXTLPOAVAIL	0x0100
+#define PST_WDRESET	0x0080
+#define	PST_INTPEND	0x0040
+#define	PST_SBCLKST	0x0030
+#define	PST_SBCLKST_ILP	0x0010
+#define	PST_SBCLKST_ALP	0x0020
+#define	PST_SBCLKST_HT	0x0030
+#define	PST_ALPAVAIL	0x0008
+#define	PST_HTAVAIL	0x0004
+#define	PST_RESINIT	0x0003
+
+
+#define PCAP_REV_MASK	0x000000ff
+#define PCAP_RC_MASK	0x00001f00
+#define PCAP_RC_SHIFT	8
+#define PCAP_TC_MASK	0x0001e000
+#define PCAP_TC_SHIFT	13
+#define PCAP_PC_MASK	0x001e0000
+#define PCAP_PC_SHIFT	17
+#define PCAP_VC_MASK	0x01e00000
+#define PCAP_VC_SHIFT	21
+#define PCAP_CC_MASK	0x1e000000
+#define PCAP_CC_SHIFT	25
+#define PCAP5_PC_MASK	0x003e0000	
+#define PCAP5_PC_SHIFT	17
+#define PCAP5_VC_MASK	0x07c00000
+#define PCAP5_VC_SHIFT	22
+#define PCAP5_CC_MASK	0xf8000000
+#define PCAP5_CC_SHIFT	27
+
+
+
+#define	PRRT_TIME_MASK	0x03ff
+#define	PRRT_INTEN	0x0400
+#define	PRRT_REQ_ACTIVE	0x0800
+#define	PRRT_ALP_REQ	0x1000
+#define	PRRT_HT_REQ	0x2000
+
+
+#define PMURES_BIT(bit)	(1 << (bit))
+
+
+#define PMURES_MAX_RESNUM	30
+
+
+#define	PMU_CHIPCTL0		0
+
+
+#define PMU_CC1_CLKREQ_TYPE_SHIFT	19
+#define PMU_CC1_CLKREQ_TYPE_MASK	(1 << PMU_CC1_CLKREQ_TYPE_SHIFT)
+
+#define CLKREQ_TYPE_CONFIG_OPENDRAIN		0
+#define CLKREQ_TYPE_CONFIG_PUSHPULL		1
+
+
+#define	PMU_CHIPCTL1			1
+#define	PMU_CC1_RXC_DLL_BYPASS		0x00010000
+
+#define PMU_CC1_IF_TYPE_MASK   		0x00000030
+#define PMU_CC1_IF_TYPE_RMII    	0x00000000
+#define PMU_CC1_IF_TYPE_MII     	0x00000010
+#define PMU_CC1_IF_TYPE_RGMII   	0x00000020
+
+#define PMU_CC1_SW_TYPE_MASK    	0x000000c0
+#define PMU_CC1_SW_TYPE_EPHY    	0x00000000
+#define PMU_CC1_SW_TYPE_EPHYMII 	0x00000040
+#define PMU_CC1_SW_TYPE_EPHYRMII	0x00000080
+#define PMU_CC1_SW_TYPE_RGMII   	0x000000c0
+
+
+
+
+
+#define	PMU0_PLL0_PLLCTL0		0
+#define	PMU0_PLL0_PC0_PDIV_MASK		1
+#define	PMU0_PLL0_PC0_PDIV_FREQ		25000
+#define PMU0_PLL0_PC0_DIV_ARM_MASK	0x00000038
+#define PMU0_PLL0_PC0_DIV_ARM_SHIFT	3
+#define PMU0_PLL0_PC0_DIV_ARM_BASE	8
+
+
+#define PMU0_PLL0_PC0_DIV_ARM_110MHZ	0
+#define PMU0_PLL0_PC0_DIV_ARM_97_7MHZ	1
+#define PMU0_PLL0_PC0_DIV_ARM_88MHZ	2
+#define PMU0_PLL0_PC0_DIV_ARM_80MHZ	3 
+#define PMU0_PLL0_PC0_DIV_ARM_73_3MHZ	4
+#define PMU0_PLL0_PC0_DIV_ARM_67_7MHZ	5
+#define PMU0_PLL0_PC0_DIV_ARM_62_9MHZ	6
+#define PMU0_PLL0_PC0_DIV_ARM_58_6MHZ	7
+
+
+#define	PMU0_PLL0_PLLCTL1		1
+#define	PMU0_PLL0_PC1_WILD_INT_MASK	0xf0000000
+#define	PMU0_PLL0_PC1_WILD_INT_SHIFT	28
+#define	PMU0_PLL0_PC1_WILD_FRAC_MASK	0x0fffff00
+#define	PMU0_PLL0_PC1_WILD_FRAC_SHIFT	8
+#define	PMU0_PLL0_PC1_STOP_MOD		0x00000040
+
+
+#define	PMU0_PLL0_PLLCTL2		2
+#define	PMU0_PLL0_PC2_WILD_INT_MASK	0xf
+#define	PMU0_PLL0_PC2_WILD_INT_SHIFT	4
+
+
+
+#define PMU1_PLL0_PLLCTL0		0
+#define PMU1_PLL0_PC0_P1DIV_MASK	0x00f00000
+#define PMU1_PLL0_PC0_P1DIV_SHIFT	20
+#define PMU1_PLL0_PC0_P2DIV_MASK	0x0f000000
+#define PMU1_PLL0_PC0_P2DIV_SHIFT	24
+
+
+#define PMU1_PLL0_PLLCTL1		1
+#define PMU1_PLL0_PC1_M1DIV_MASK	0x000000ff
+#define PMU1_PLL0_PC1_M1DIV_SHIFT	0
+#define PMU1_PLL0_PC1_M2DIV_MASK	0x0000ff00
+#define PMU1_PLL0_PC1_M2DIV_SHIFT	8
+#define PMU1_PLL0_PC1_M3DIV_MASK	0x00ff0000
+#define PMU1_PLL0_PC1_M3DIV_SHIFT	16
+#define PMU1_PLL0_PC1_M4DIV_MASK	0xff000000
+#define PMU1_PLL0_PC1_M4DIV_SHIFT	24
+#define PMU1_PLL0_PC1_M4DIV_BY_9	9
+#define PMU1_PLL0_PC1_M4DIV_BY_18	0x12
+#define PMU1_PLL0_PC1_M4DIV_BY_36	0x24
+
+#define DOT11MAC_880MHZ_CLK_DIVISOR_SHIFT 8
+#define DOT11MAC_880MHZ_CLK_DIVISOR_MASK (0xFF << DOT11MAC_880MHZ_CLK_DIVISOR_SHIFT)
+#define DOT11MAC_880MHZ_CLK_DIVISOR_VAL  (0xE << DOT11MAC_880MHZ_CLK_DIVISOR_SHIFT)
+
+
+#define PMU1_PLL0_PLLCTL2		2
+#define PMU1_PLL0_PC2_M5DIV_MASK	0x000000ff
+#define PMU1_PLL0_PC2_M5DIV_SHIFT	0
+#define PMU1_PLL0_PC2_M5DIV_BY_12	0xc
+#define PMU1_PLL0_PC2_M5DIV_BY_18	0x12
+#define PMU1_PLL0_PC2_M5DIV_BY_36	0x24
+#define PMU1_PLL0_PC2_M6DIV_MASK	0x0000ff00
+#define PMU1_PLL0_PC2_M6DIV_SHIFT	8
+#define PMU1_PLL0_PC2_M6DIV_BY_18	0x12
+#define PMU1_PLL0_PC2_M6DIV_BY_36	0x24
+#define PMU1_PLL0_PC2_NDIV_MODE_MASK	0x000e0000
+#define PMU1_PLL0_PC2_NDIV_MODE_SHIFT	17
+#define PMU1_PLL0_PC2_NDIV_MODE_MASH	1
+#define PMU1_PLL0_PC2_NDIV_MODE_MFB	2	
+#define PMU1_PLL0_PC2_NDIV_INT_MASK	0x1ff00000
+#define PMU1_PLL0_PC2_NDIV_INT_SHIFT	20
+
+
+#define PMU1_PLL0_PLLCTL3		3
+#define PMU1_PLL0_PC3_NDIV_FRAC_MASK	0x00ffffff
+#define PMU1_PLL0_PC3_NDIV_FRAC_SHIFT	0
+
+
+#define PMU1_PLL0_PLLCTL4		4
+
+
+#define PMU1_PLL0_PLLCTL5		5
+#define PMU1_PLL0_PC5_CLK_DRV_MASK 0xffffff00
+#define PMU1_PLL0_PC5_CLK_DRV_SHIFT 8
+
+
+#define PMU2_PHY_PLL_PLLCTL		4
+#define PMU2_SI_PLL_PLLCTL		10
+
+
+
+
+#define PMU2_PLL_PLLCTL0		0
+#define PMU2_PLL_PC0_P1DIV_MASK 	0x00f00000
+#define PMU2_PLL_PC0_P1DIV_SHIFT	20
+#define PMU2_PLL_PC0_P2DIV_MASK 	0x0f000000
+#define PMU2_PLL_PC0_P2DIV_SHIFT	24
+
+
+#define PMU2_PLL_PLLCTL1		1
+#define PMU2_PLL_PC1_M1DIV_MASK 	0x000000ff
+#define PMU2_PLL_PC1_M1DIV_SHIFT	0
+#define PMU2_PLL_PC1_M2DIV_MASK 	0x0000ff00
+#define PMU2_PLL_PC1_M2DIV_SHIFT	8
+#define PMU2_PLL_PC1_M3DIV_MASK 	0x00ff0000
+#define PMU2_PLL_PC1_M3DIV_SHIFT	16
+#define PMU2_PLL_PC1_M4DIV_MASK 	0xff000000
+#define PMU2_PLL_PC1_M4DIV_SHIFT	24
+
+
+#define PMU2_PLL_PLLCTL2		2
+#define PMU2_PLL_PC2_M5DIV_MASK 	0x000000ff
+#define PMU2_PLL_PC2_M5DIV_SHIFT	0
+#define PMU2_PLL_PC2_M6DIV_MASK 	0x0000ff00
+#define PMU2_PLL_PC2_M6DIV_SHIFT	8
+#define PMU2_PLL_PC2_NDIV_MODE_MASK	0x000e0000
+#define PMU2_PLL_PC2_NDIV_MODE_SHIFT	17
+#define PMU2_PLL_PC2_NDIV_INT_MASK	0x1ff00000
+#define PMU2_PLL_PC2_NDIV_INT_SHIFT	20
+
+
+#define PMU2_PLL_PLLCTL3		3
+#define PMU2_PLL_PC3_NDIV_FRAC_MASK	0x00ffffff
+#define PMU2_PLL_PC3_NDIV_FRAC_SHIFT	0
+
+
+#define PMU2_PLL_PLLCTL4		4
+
+
+#define PMU2_PLL_PLLCTL5		5
+#define PMU2_PLL_PC5_CLKDRIVE_CH1_MASK	0x00000f00
+#define PMU2_PLL_PC5_CLKDRIVE_CH1_SHIFT	8
+#define PMU2_PLL_PC5_CLKDRIVE_CH2_MASK	0x0000f000
+#define PMU2_PLL_PC5_CLKDRIVE_CH2_SHIFT	12
+#define PMU2_PLL_PC5_CLKDRIVE_CH3_MASK	0x000f0000
+#define PMU2_PLL_PC5_CLKDRIVE_CH3_SHIFT	16
+#define PMU2_PLL_PC5_CLKDRIVE_CH4_MASK	0x00f00000
+#define PMU2_PLL_PC5_CLKDRIVE_CH4_SHIFT	20
+#define PMU2_PLL_PC5_CLKDRIVE_CH5_MASK	0x0f000000
+#define PMU2_PLL_PC5_CLKDRIVE_CH5_SHIFT	24
+#define PMU2_PLL_PC5_CLKDRIVE_CH6_MASK	0xf0000000
+#define PMU2_PLL_PC5_CLKDRIVE_CH6_SHIFT	28
+
+
+#define	PMU5_PLL_P1P2_OFF		0
+#define	PMU5_PLL_P1_MASK		0x0f000000
+#define	PMU5_PLL_P1_SHIFT		24
+#define	PMU5_PLL_P2_MASK		0x00f00000
+#define	PMU5_PLL_P2_SHIFT		20
+#define	PMU5_PLL_M14_OFF		1
+#define	PMU5_PLL_MDIV_MASK		0x000000ff
+#define	PMU5_PLL_MDIV_WIDTH		8
+#define	PMU5_PLL_NM5_OFF		2
+#define	PMU5_PLL_NDIV_MASK		0xfff00000
+#define	PMU5_PLL_NDIV_SHIFT		20
+#define	PMU5_PLL_NDIV_MODE_MASK		0x000e0000
+#define	PMU5_PLL_NDIV_MODE_SHIFT	17
+#define	PMU5_PLL_FMAB_OFF		3
+#define	PMU5_PLL_MRAT_MASK		0xf0000000
+#define	PMU5_PLL_MRAT_SHIFT		28
+#define	PMU5_PLL_ABRAT_MASK		0x08000000
+#define	PMU5_PLL_ABRAT_SHIFT		27
+#define	PMU5_PLL_FDIV_MASK		0x07ffffff
+#define	PMU5_PLL_PLLCTL_OFF		4
+#define	PMU5_PLL_PCHI_OFF		5
+#define	PMU5_PLL_PCHI_MASK		0x0000003f
+
+
+#define	PMU_XTALFREQ_REG_ILPCTR_MASK	0x00001FFF
+#define	PMU_XTALFREQ_REG_MEASURE_MASK	0x80000000
+#define	PMU_XTALFREQ_REG_MEASURE_SHIFT	31
+
+
+#define	PMU5_MAINPLL_CPU		1
+#define	PMU5_MAINPLL_MEM		2
+#define	PMU5_MAINPLL_SI			3
+
+#define PMU7_PLL_PLLCTL7                7
+#define PMU7_PLL_CTL7_M4DIV_MASK	0xff000000
+#define PMU7_PLL_CTL7_M4DIV_SHIFT 	24
+#define PMU7_PLL_CTL7_M4DIV_BY_6	6
+#define PMU7_PLL_CTL7_M4DIV_BY_12	0xc
+#define PMU7_PLL_CTL7_M4DIV_BY_24	0x18
+#define PMU7_PLL_PLLCTL8                8
+#define PMU7_PLL_CTL8_M5DIV_MASK	0x000000ff
+#define PMU7_PLL_CTL8_M5DIV_SHIFT	0
+#define PMU7_PLL_CTL8_M5DIV_BY_8	8
+#define PMU7_PLL_CTL8_M5DIV_BY_12	0xc
+#define PMU7_PLL_CTL8_M5DIV_BY_24	0x18
+#define PMU7_PLL_CTL8_M6DIV_MASK	0x0000ff00
+#define PMU7_PLL_CTL8_M6DIV_SHIFT	8
+#define PMU7_PLL_CTL8_M6DIV_BY_12	0xc
+#define PMU7_PLL_CTL8_M6DIV_BY_24	0x18
+#define PMU7_PLL_PLLCTL11		11
+#define PMU7_PLL_PLLCTL11_MASK		0xffffff00
+#define PMU7_PLL_PLLCTL11_VAL		0x22222200
+
+
+#define	PMU4716_MAINPLL_PLL0		12
+
+
+#define	PMU5356_MAINPLL_PLL0		0
+#define	PMU5357_MAINPLL_PLL0		0
+
+
+#define RES4716_PROC_PLL_ON		0x00000040
+#define RES4716_PROC_HT_AVAIL		0x00000080
+
+
+#define CCTRL_471X_I2S_PINS_ENABLE	0x0080 
+
+
+
+#define CCTRL_5357_I2S_PINS_ENABLE	0x00040000 
+#define CCTRL_5357_I2CSPI_PINS_ENABLE	0x00080000 
+
+
+#define RES5354_EXT_SWITCHER_PWM	0	
+#define RES5354_BB_SWITCHER_PWM		1	
+#define RES5354_BB_SWITCHER_BURST	2	
+#define RES5354_BB_EXT_SWITCHER_BURST	3	
+#define RES5354_ILP_REQUEST		4	
+#define RES5354_RADIO_SWITCHER_PWM	5	
+#define RES5354_RADIO_SWITCHER_BURST	6	
+#define RES5354_ROM_SWITCH		7	
+#define RES5354_PA_REF_LDO		8	
+#define RES5354_RADIO_LDO		9	
+#define RES5354_AFE_LDO			10	
+#define RES5354_PLL_LDO			11	
+#define RES5354_BG_FILTBYP		12	
+#define RES5354_TX_FILTBYP		13	
+#define RES5354_RX_FILTBYP		14	
+#define RES5354_XTAL_PU			15	
+#define RES5354_XTAL_EN			16	
+#define RES5354_BB_PLL_FILTBYP		17	
+#define RES5354_RF_PLL_FILTBYP		18	
+#define RES5354_BB_PLL_PU		19	
+
+
+#define CCTRL5357_EXTPA                 (1<<14)  
+#define CCTRL5357_ANT_MUX_2o3		(1<<15)  
+
+
+#define RES4328_EXT_SWITCHER_PWM	0	
+#define RES4328_BB_SWITCHER_PWM		1	
+#define RES4328_BB_SWITCHER_BURST	2	
+#define RES4328_BB_EXT_SWITCHER_BURST	3	
+#define RES4328_ILP_REQUEST		4	
+#define RES4328_RADIO_SWITCHER_PWM	5	
+#define RES4328_RADIO_SWITCHER_BURST	6	
+#define RES4328_ROM_SWITCH		7	
+#define RES4328_PA_REF_LDO		8	
+#define RES4328_RADIO_LDO		9	
+#define RES4328_AFE_LDO			10	
+#define RES4328_PLL_LDO			11	
+#define RES4328_BG_FILTBYP		12	
+#define RES4328_TX_FILTBYP		13	
+#define RES4328_RX_FILTBYP		14	
+#define RES4328_XTAL_PU			15	
+#define RES4328_XTAL_EN			16	
+#define RES4328_BB_PLL_FILTBYP		17	
+#define RES4328_RF_PLL_FILTBYP		18	
+#define RES4328_BB_PLL_PU		19	
+
+
+#define RES4325_BUCK_BOOST_BURST	0	
+#define RES4325_CBUCK_BURST		1	
+#define RES4325_CBUCK_PWM		2	
+#define RES4325_CLDO_CBUCK_BURST	3	
+#define RES4325_CLDO_CBUCK_PWM		4	
+#define RES4325_BUCK_BOOST_PWM		5	
+#define RES4325_ILP_REQUEST		6	
+#define RES4325_ABUCK_BURST		7	
+#define RES4325_ABUCK_PWM		8	
+#define RES4325_LNLDO1_PU		9	
+#define RES4325_OTP_PU			10	
+#define RES4325_LNLDO3_PU		11	
+#define RES4325_LNLDO4_PU		12	
+#define RES4325_XTAL_PU			13	
+#define RES4325_ALP_AVAIL		14	
+#define RES4325_RX_PWRSW_PU		15	
+#define RES4325_TX_PWRSW_PU		16	
+#define RES4325_RFPLL_PWRSW_PU		17	
+#define RES4325_LOGEN_PWRSW_PU		18	
+#define RES4325_AFE_PWRSW_PU		19	
+#define RES4325_BBPLL_PWRSW_PU		20	
+#define RES4325_HT_AVAIL		21	
+
+
+#define RES4325B0_CBUCK_LPOM		1	
+#define RES4325B0_CBUCK_BURST		2	
+#define RES4325B0_CBUCK_PWM		3	
+#define RES4325B0_CLDO_PU		4	
+
+
+#define RES4325C1_LNLDO2_PU		12	
+
+
+#define CST4325_SPROM_OTP_SEL_MASK	0x00000003
+#define CST4325_DEFCIS_SEL		0	
+#define CST4325_SPROM_SEL		1	
+#define CST4325_OTP_SEL			2	
+#define CST4325_OTP_PWRDN		3	
+#define CST4325_SDIO_USB_MODE_MASK	0x00000004
+#define CST4325_SDIO_USB_MODE_SHIFT	2
+#define CST4325_RCAL_VALID_MASK		0x00000008
+#define CST4325_RCAL_VALID_SHIFT	3
+#define CST4325_RCAL_VALUE_MASK		0x000001f0
+#define CST4325_RCAL_VALUE_SHIFT	4
+#define CST4325_PMUTOP_2B_MASK 		0x00000200	
+#define CST4325_PMUTOP_2B_SHIFT   	9
+
+#define RES4329_RESERVED0		0	
+#define RES4329_CBUCK_LPOM		1	
+#define RES4329_CBUCK_BURST		2	
+#define RES4329_CBUCK_PWM		3	
+#define RES4329_CLDO_PU			4	
+#define RES4329_PALDO_PU		5	
+#define RES4329_ILP_REQUEST		6	
+#define RES4329_RESERVED7		7	
+#define RES4329_RESERVED8		8	
+#define RES4329_LNLDO1_PU		9	
+#define RES4329_OTP_PU			10	
+#define RES4329_RESERVED11		11	
+#define RES4329_LNLDO2_PU		12	
+#define RES4329_XTAL_PU			13	
+#define RES4329_ALP_AVAIL		14	
+#define RES4329_RX_PWRSW_PU		15	
+#define RES4329_TX_PWRSW_PU		16	
+#define RES4329_RFPLL_PWRSW_PU		17	
+#define RES4329_LOGEN_PWRSW_PU		18	
+#define RES4329_AFE_PWRSW_PU		19	
+#define RES4329_BBPLL_PWRSW_PU		20	
+#define RES4329_HT_AVAIL		21	
+
+#define CST4329_SPROM_OTP_SEL_MASK	0x00000003
+#define CST4329_DEFCIS_SEL		0	
+#define CST4329_SPROM_SEL		1	
+#define CST4329_OTP_SEL			2	
+#define CST4329_OTP_PWRDN		3	
+#define CST4329_SPI_SDIO_MODE_MASK	0x00000004
+#define CST4329_SPI_SDIO_MODE_SHIFT	2
+
+
+#define CST4312_SPROM_OTP_SEL_MASK	0x00000003
+#define CST4312_DEFCIS_SEL		0	
+#define CST4312_SPROM_SEL		1	
+#define CST4312_OTP_SEL			2	
+#define CST4312_OTP_BAD			3	
+
+
+#define RES4312_SWITCHER_BURST		0	
+#define RES4312_SWITCHER_PWM    	1	
+#define RES4312_PA_REF_LDO		2	
+#define RES4312_CORE_LDO_BURST		3	
+#define RES4312_CORE_LDO_PWM		4	
+#define RES4312_RADIO_LDO		5	
+#define RES4312_ILP_REQUEST		6	
+#define RES4312_BG_FILTBYP		7	
+#define RES4312_TX_FILTBYP		8	
+#define RES4312_RX_FILTBYP		9	
+#define RES4312_XTAL_PU			10	
+#define RES4312_ALP_AVAIL		11	
+#define RES4312_BB_PLL_FILTBYP		12	
+#define RES4312_RF_PLL_FILTBYP		13	
+#define RES4312_HT_AVAIL		14	
+
+
+#define RES4322_RF_LDO			0
+#define RES4322_ILP_REQUEST		1
+#define RES4322_XTAL_PU			2
+#define RES4322_ALP_AVAIL		3
+#define RES4322_SI_PLL_ON		4
+#define RES4322_HT_SI_AVAIL		5
+#define RES4322_PHY_PLL_ON		6
+#define RES4322_HT_PHY_AVAIL		7
+#define RES4322_OTP_PU			8
+
+
+#define CST4322_XTAL_FREQ_20_40MHZ	0x00000020
+#define CST4322_SPROM_OTP_SEL_MASK	0x000000c0
+#define CST4322_SPROM_OTP_SEL_SHIFT	6
+#define CST4322_NO_SPROM_OTP		0	
+#define CST4322_SPROM_PRESENT		1	
+#define CST4322_OTP_PRESENT		2	
+#define CST4322_PCI_OR_USB		0x00000100
+#define CST4322_BOOT_MASK		0x00000600
+#define CST4322_BOOT_SHIFT		9
+#define CST4322_BOOT_FROM_SRAM		0	
+#define CST4322_BOOT_FROM_ROM		1	
+#define CST4322_BOOT_FROM_FLASH		2	
+#define CST4322_BOOT_FROM_INVALID	3
+#define CST4322_ILP_DIV_EN		0x00000800
+#define CST4322_FLASH_TYPE_MASK		0x00001000
+#define CST4322_FLASH_TYPE_SHIFT	12
+#define CST4322_FLASH_TYPE_SHIFT_ST	0	
+#define CST4322_FLASH_TYPE_SHIFT_ATMEL	1	
+#define CST4322_ARM_TAP_SEL		0x00002000
+#define CST4322_RES_INIT_MODE_MASK	0x0000c000
+#define CST4322_RES_INIT_MODE_SHIFT	14
+#define CST4322_RES_INIT_MODE_ILPAVAIL	0	
+#define CST4322_RES_INIT_MODE_ILPREQ	1	
+#define CST4322_RES_INIT_MODE_ALPAVAIL	2	
+#define CST4322_RES_INIT_MODE_HTAVAIL	3	
+#define CST4322_PCIPLLCLK_GATING	0x00010000
+#define CST4322_CLK_SWITCH_PCI_TO_ALP	0x00020000
+#define CST4322_PCI_CARDBUS_MODE	0x00040000
+
+
+#define CCTRL43224_GPIO_TOGGLE          0x8000 
+#define CCTRL_43224A0_12MA_LED_DRIVE    0x00F000F0 
+#define CCTRL_43224B0_12MA_LED_DRIVE    0xF0    
+
+
+#define RES43236_REGULATOR		0
+#define RES43236_ILP_REQUEST		1
+#define RES43236_XTAL_PU		2
+#define RES43236_ALP_AVAIL		3
+#define RES43236_SI_PLL_ON		4
+#define RES43236_HT_SI_AVAIL		5
+
+
+#define CCTRL43236_BT_COEXIST		(1<<0)	
+#define CCTRL43236_SECI			(1<<1)	
+#define CCTRL43236_EXT_LNA		(1<<2)	
+#define CCTRL43236_ANT_MUX_2o3          (1<<3)	
+#define CCTRL43236_GSIO			(1<<4)	
+
+
+#define CST43236_SFLASH_MASK		0x00000040
+#define CST43236_OTP_SEL_MASK		0x00000080
+#define CST43236_OTP_SEL_SHIFT		7
+#define CST43236_HSIC_MASK		0x00000100	
+#define CST43236_BP_CLK			0x00000200	
+#define CST43236_BOOT_MASK		0x00001800
+#define CST43236_BOOT_SHIFT		11
+#define CST43236_BOOT_FROM_SRAM		0	
+#define CST43236_BOOT_FROM_ROM		1	
+#define CST43236_BOOT_FROM_FLASH	2	
+#define CST43236_BOOT_FROM_INVALID	3
+
+
+#define RES43237_REGULATOR		0
+#define RES43237_ILP_REQUEST		1
+#define RES43237_XTAL_PU		2
+#define RES43237_ALP_AVAIL		3
+#define RES43237_SI_PLL_ON		4
+#define RES43237_HT_SI_AVAIL		5
+
+
+#define CCTRL43237_BT_COEXIST		(1<<0)	
+#define CCTRL43237_SECI			(1<<1)	
+#define CCTRL43237_EXT_LNA		(1<<2)	
+#define CCTRL43237_ANT_MUX_2o3          (1<<3)	
+#define CCTRL43237_GSIO			(1<<4)	
+
+
+#define CST43237_SFLASH_MASK		0x00000040
+#define CST43237_OTP_SEL_MASK		0x00000080
+#define CST43237_OTP_SEL_SHIFT		7
+#define CST43237_HSIC_MASK		0x00000100	
+#define CST43237_BP_CLK			0x00000200	
+#define CST43237_BOOT_MASK		0x00001800
+#define CST43237_BOOT_SHIFT		11
+#define CST43237_BOOT_FROM_SRAM		0	
+#define CST43237_BOOT_FROM_ROM		1	
+#define CST43237_BOOT_FROM_FLASH	2	
+#define CST43237_BOOT_FROM_INVALID	3
+
+
+#define RES43239_CBUCK_LPOM		0
+#define RES43239_CBUCK_BURST		1
+#define RES43239_CBUCK_LP_PWM		2
+#define RES43239_CBUCK_PWM		3
+#define RES43239_CLDO_PU		4
+#define RES43239_DIS_INT_RESET_PD	5
+#define RES43239_ILP_REQUEST		6
+#define RES43239_LNLDO_PU		7
+#define RES43239_LDO3P3_PU		8
+#define RES43239_OTP_PU			9
+#define RES43239_XTAL_PU		10
+#define RES43239_ALP_AVAIL		11
+#define RES43239_RADIO_PU		12
+#define RES43239_MACPHY_CLKAVAIL	23
+#define RES43239_HT_AVAIL		24
+#define RES43239_XOLDO_PU		25
+#define RES43239_WL_XTAL_CTL_SEL	26
+#define RES43239_SR_CLK_STABLE		27
+#define RES43239_SR_SAVE_RESTORE	28
+#define RES43239_SR_PHY_PIC		29
+#define RES43239_SR_PHY_PWR_SW		30
+
+
+#define CST43239_SPROM_MASK			0x00000002
+#define CST43239_SFLASH_MASK		0x00000004
+#define	CST43239_RES_INIT_MODE_SHIFT	7
+#define	CST43239_RES_INIT_MODE_MASK		0x000001f0
+#define CST43239_CHIPMODE_SDIOD(cs)	((cs) & (1 << 15))	
+#define CST43239_CHIPMODE_USB20D(cs)	(~(cs) & (1 << 15))	
+#define CST43239_CHIPMODE_SDIO(cs)	(((cs) & (1 << 0)) == 0)	
+#define CST43239_CHIPMODE_GSPI(cs)	(((cs) & (1 << 0)) == (1 << 0))	
+
+
+#define CCTRL43239_XTAL_STRENGTH(ctl)	((ctl & 0x3F) << 12)
+
+
+#define RES4331_REGULATOR		0
+#define RES4331_ILP_REQUEST		1
+#define RES4331_XTAL_PU			2
+#define RES4331_ALP_AVAIL		3
+#define RES4331_SI_PLL_ON		4
+#define RES4331_HT_SI_AVAIL		5
+
+
+#define CCTRL4331_BT_COEXIST		(1<<0)	
+#define CCTRL4331_SECI			(1<<1)	
+#define CCTRL4331_EXT_LNA_G		(1<<2)	
+#define CCTRL4331_SPROM_GPIO13_15       (1<<3)  
+#define CCTRL4331_EXTPA_EN		(1<<4)	
+#define CCTRL4331_GPIOCLK_ON_SPROMCS	(1<<5)
+#define CCTRL4331_PCIE_MDIO_ON_SPROMCS	(1<<6)	
+#define CCTRL4331_EXTPA_ON_GPIO2_5	(1<<7)	
+#define CCTRL4331_OVR_PIPEAUXCLKEN	(1<<8)	
+#define CCTRL4331_OVR_PIPEAUXPWRDOWN	(1<<9)	
+#define CCTRL4331_PCIE_AUXCLKEN		(1<<10)
+#define CCTRL4331_PCIE_PIPE_PLLDOWN	(1<<11)
+#define CCTRL4331_EXTPA_EN2		(1<<12)
+#define CCTRL4331_EXT_LNA_A		(1<<13)
+#define CCTRL4331_BT_SHD0_ON_GPIO4	(1<<16)
+#define CCTRL4331_BT_SHD1_ON_GPIO5	(1<<17)
+#define CCTRL4331_EXTPA_ANA_EN		(1<<24)
+
+
+#define	CST4331_XTAL_FREQ		0x00000001	
+#define	CST4331_SPROM_OTP_SEL_MASK	0x00000006
+#define	CST4331_SPROM_OTP_SEL_SHIFT	1
+#define	CST4331_SPROM_PRESENT		0x00000002
+#define	CST4331_OTP_PRESENT		0x00000004
+#define	CST4331_LDO_RF			0x00000008
+#define	CST4331_LDO_PAR			0x00000010
+
+
+#define RES4315_CBUCK_LPOM		1	
+#define RES4315_CBUCK_BURST		2	
+#define RES4315_CBUCK_PWM		3	
+#define RES4315_CLDO_PU			4	
+#define RES4315_PALDO_PU		5	
+#define RES4315_ILP_REQUEST		6	
+#define RES4315_LNLDO1_PU		9	
+#define RES4315_OTP_PU			10	
+#define RES4315_LNLDO2_PU		12	
+#define RES4315_XTAL_PU			13	
+#define RES4315_ALP_AVAIL		14	
+#define RES4315_RX_PWRSW_PU		15	
+#define RES4315_TX_PWRSW_PU		16	
+#define RES4315_RFPLL_PWRSW_PU		17	
+#define RES4315_LOGEN_PWRSW_PU		18	
+#define RES4315_AFE_PWRSW_PU		19	
+#define RES4315_BBPLL_PWRSW_PU		20	
+#define RES4315_HT_AVAIL		21	
+
+
+#define CST4315_SPROM_OTP_SEL_MASK	0x00000003	
+#define CST4315_DEFCIS_SEL		0x00000000	
+#define CST4315_SPROM_SEL		0x00000001	
+#define CST4315_OTP_SEL			0x00000002	
+#define CST4315_OTP_PWRDN		0x00000003	
+#define CST4315_SDIO_MODE		0x00000004	
+#define CST4315_RCAL_VALID		0x00000008
+#define CST4315_RCAL_VALUE_MASK		0x000001f0
+#define CST4315_RCAL_VALUE_SHIFT	4
+#define CST4315_PALDO_EXTPNP		0x00000200	
+#define CST4315_CBUCK_MODE_MASK		0x00000c00
+#define CST4315_CBUCK_MODE_BURST	0x00000400
+#define CST4315_CBUCK_MODE_LPBURST	0x00000c00
+
+
+#define RES4319_CBUCK_LPOM		1	
+#define RES4319_CBUCK_BURST		2	
+#define RES4319_CBUCK_PWM		3	
+#define RES4319_CLDO_PU			4	
+#define RES4319_PALDO_PU		5	
+#define RES4319_ILP_REQUEST		6	
+#define RES4319_LNLDO1_PU		9	
+#define RES4319_OTP_PU			10	
+#define RES4319_LNLDO2_PU		12	
+#define RES4319_XTAL_PU			13	
+#define RES4319_ALP_AVAIL		14	
+#define RES4319_RX_PWRSW_PU		15	
+#define RES4319_TX_PWRSW_PU		16	
+#define RES4319_RFPLL_PWRSW_PU		17	
+#define RES4319_LOGEN_PWRSW_PU		18	
+#define RES4319_AFE_PWRSW_PU		19	
+#define RES4319_BBPLL_PWRSW_PU		20	
+#define RES4319_HT_AVAIL		21	
+
+
+#define	CST4319_SPI_CPULESSUSB		0x00000001
+#define	CST4319_SPI_CLK_POL		0x00000002
+#define	CST4319_SPI_CLK_PH		0x00000008
+#define	CST4319_SPROM_OTP_SEL_MASK	0x000000c0	
+#define	CST4319_SPROM_OTP_SEL_SHIFT	6
+#define	CST4319_DEFCIS_SEL		0x00000000	
+#define	CST4319_SPROM_SEL		0x00000040	
+#define	CST4319_OTP_SEL			0x00000080      
+#define	CST4319_OTP_PWRDN		0x000000c0      
+#define	CST4319_SDIO_USB_MODE		0x00000100	
+#define	CST4319_REMAP_SEL_MASK		0x00000600
+#define	CST4319_ILPDIV_EN		0x00000800
+#define	CST4319_XTAL_PD_POL		0x00001000
+#define	CST4319_LPO_SEL			0x00002000
+#define	CST4319_RES_INIT_MODE		0x0000c000
+#define	CST4319_PALDO_EXTPNP		0x00010000	
+#define	CST4319_CBUCK_MODE_MASK		0x00060000
+#define CST4319_CBUCK_MODE_BURST	0x00020000
+#define CST4319_CBUCK_MODE_LPBURST	0x00060000
+#define	CST4319_RCAL_VALID		0x01000000
+#define	CST4319_RCAL_VALUE_MASK		0x3e000000
+#define	CST4319_RCAL_VALUE_SHIFT	25
+
+#define PMU1_PLL0_CHIPCTL0		0
+#define PMU1_PLL0_CHIPCTL1		1
+#define PMU1_PLL0_CHIPCTL2		2
+#define CCTL_4319USB_XTAL_SEL_MASK	0x00180000
+#define CCTL_4319USB_XTAL_SEL_SHIFT	19
+#define CCTL_4319USB_48MHZ_PLL_SEL	1
+#define CCTL_4319USB_24MHZ_PLL_SEL	2
+
+
+#define	RES4336_CBUCK_LPOM		0
+#define	RES4336_CBUCK_BURST		1
+#define	RES4336_CBUCK_LP_PWM		2
+#define	RES4336_CBUCK_PWM		3
+#define	RES4336_CLDO_PU			4
+#define	RES4336_DIS_INT_RESET_PD	5
+#define	RES4336_ILP_REQUEST		6
+#define	RES4336_LNLDO_PU		7
+#define	RES4336_LDO3P3_PU		8
+#define	RES4336_OTP_PU			9
+#define	RES4336_XTAL_PU			10
+#define	RES4336_ALP_AVAIL		11
+#define	RES4336_RADIO_PU		12
+#define	RES4336_BG_PU			13
+#define	RES4336_VREG1p4_PU_PU		14
+#define	RES4336_AFE_PWRSW_PU		15
+#define	RES4336_RX_PWRSW_PU		16
+#define	RES4336_TX_PWRSW_PU		17
+#define	RES4336_BB_PWRSW_PU		18
+#define	RES4336_SYNTH_PWRSW_PU		19
+#define	RES4336_MISC_PWRSW_PU		20
+#define	RES4336_LOGEN_PWRSW_PU		21
+#define	RES4336_BBPLL_PWRSW_PU		22
+#define	RES4336_MACPHY_CLKAVAIL		23
+#define	RES4336_HT_AVAIL		24
+#define	RES4336_RSVD			25
+
+
+#define	CST4336_SPI_MODE_MASK		0x00000001
+#define	CST4336_SPROM_PRESENT		0x00000002
+#define	CST4336_OTP_PRESENT		0x00000004
+#define	CST4336_ARMREMAP_0		0x00000008
+#define	CST4336_ILPDIV_EN_MASK		0x00000010
+#define	CST4336_ILPDIV_EN_SHIFT		4
+#define	CST4336_XTAL_PD_POL_MASK	0x00000020
+#define	CST4336_XTAL_PD_POL_SHIFT	5
+#define	CST4336_LPO_SEL_MASK		0x00000040
+#define	CST4336_LPO_SEL_SHIFT		6
+#define	CST4336_RES_INIT_MODE_MASK	0x00000180
+#define	CST4336_RES_INIT_MODE_SHIFT	7
+#define	CST4336_CBUCK_MODE_MASK		0x00000600
+#define	CST4336_CBUCK_MODE_SHIFT	9
+
+
+#define PCTL_4336_SERIAL_ENAB	(1  << 24)
+
+
+#define	RES4330_CBUCK_LPOM		0
+#define	RES4330_CBUCK_BURST		1
+#define	RES4330_CBUCK_LP_PWM		2
+#define	RES4330_CBUCK_PWM		3
+#define	RES4330_CLDO_PU			4
+#define	RES4330_DIS_INT_RESET_PD	5
+#define	RES4330_ILP_REQUEST		6
+#define	RES4330_LNLDO_PU		7
+#define	RES4330_LDO3P3_PU		8
+#define	RES4330_OTP_PU			9
+#define	RES4330_XTAL_PU			10
+#define	RES4330_ALP_AVAIL		11
+#define	RES4330_RADIO_PU		12
+#define	RES4330_BG_PU			13
+#define	RES4330_VREG1p4_PU_PU		14
+#define	RES4330_AFE_PWRSW_PU		15
+#define	RES4330_RX_PWRSW_PU		16
+#define	RES4330_TX_PWRSW_PU		17
+#define	RES4330_BB_PWRSW_PU		18
+#define	RES4330_SYNTH_PWRSW_PU		19
+#define	RES4330_MISC_PWRSW_PU		20
+#define	RES4330_LOGEN_PWRSW_PU		21
+#define	RES4330_BBPLL_PWRSW_PU		22
+#define	RES4330_MACPHY_CLKAVAIL		23
+#define	RES4330_HT_AVAIL		24
+#define	RES4330_5gRX_PWRSW_PU		25
+#define	RES4330_5gTX_PWRSW_PU		26
+#define	RES4330_5g_LOGEN_PWRSW_PU	27
+
+
+#define CST4330_CHIPMODE_SDIOD(cs)	(((cs) & 0x7) < 6)	
+#define CST4330_CHIPMODE_USB20D(cs)	(((cs) & 0x7) >= 6)	
+#define CST4330_CHIPMODE_SDIO(cs)	(((cs) & 0x4) == 0)	
+#define CST4330_CHIPMODE_GSPI(cs)	(((cs) & 0x6) == 4)	
+#define CST4330_CHIPMODE_USB(cs)	(((cs) & 0x7) == 6)	
+#define CST4330_CHIPMODE_USBDA(cs)	(((cs) & 0x7) == 7)	
+#define	CST4330_OTP_PRESENT		0x00000010
+#define	CST4330_LPO_AUTODET_EN		0x00000020
+#define	CST4330_ARMREMAP_0		0x00000040
+#define	CST4330_SPROM_PRESENT		0x00000080	
+#define	CST4330_ILPDIV_EN		0x00000100
+#define	CST4330_LPO_SEL			0x00000200
+#define	CST4330_RES_INIT_MODE_SHIFT	10
+#define	CST4330_RES_INIT_MODE_MASK	0x00000c00
+#define CST4330_CBUCK_MODE_SHIFT	12
+#define CST4330_CBUCK_MODE_MASK		0x00003000
+#define	CST4330_CBUCK_POWER_OK		0x00004000
+#define	CST4330_BB_PLL_LOCKED		0x00008000
+#define SOCDEVRAM_4330_BP_ADDR		0x1E000000
+#define SOCDEVRAM_4330_ARM_ADDR		0x00800000
+
+
+#define PCTL_4330_SERIAL_ENAB	(1  << 24)
+
+
+#define CCTRL_4330_GPIO_SEL		0x00000001    
+#define CCTRL_4330_ERCX_SEL		0x00000002    
+#define CCTRL_4330_SDIO_HOST_WAKE	0x00000004    
+#define CCTRL_4330_JTAG_DISABLE	0x00000008    
+
+
+#define CCTRL_43239_GPIO_SEL		0x00000002    
+#define CCTRL_43239_SDIO_HOST_WAKE	0x00000004    
+
+#define	RES4313_BB_PU_RSRC		0
+#define	RES4313_ILP_REQ_RSRC		1
+#define	RES4313_XTAL_PU_RSRC		2
+#define	RES4313_ALP_AVAIL_RSRC		3
+#define	RES4313_RADIO_PU_RSRC		4
+#define	RES4313_BG_PU_RSRC		5
+#define	RES4313_VREG1P4_PU_RSRC		6
+#define	RES4313_AFE_PWRSW_RSRC		7
+#define	RES4313_RX_PWRSW_RSRC		8
+#define	RES4313_TX_PWRSW_RSRC		9
+#define	RES4313_BB_PWRSW_RSRC		10
+#define	RES4313_SYNTH_PWRSW_RSRC	11
+#define	RES4313_MISC_PWRSW_RSRC		12
+#define	RES4313_BB_PLL_PWRSW_RSRC	13
+#define	RES4313_HT_AVAIL_RSRC		14
+#define	RES4313_MACPHY_CLK_AVAIL_RSRC	15
+
+
+#define	CST4313_SPROM_PRESENT			1
+#define	CST4313_OTP_PRESENT			2
+#define	CST4313_SPROM_OTP_SEL_MASK		0x00000002
+#define	CST4313_SPROM_OTP_SEL_SHIFT		0
+
+
+#define CCTRL_4313_12MA_LED_DRIVE    0x00000007    
+
+
+#define RES43228_NOT_USED		0
+#define RES43228_ILP_REQUEST		1
+#define RES43228_XTAL_PU		2
+#define RES43228_ALP_AVAIL		3
+#define RES43228_PLL_EN			4
+#define RES43228_HT_PHY_AVAIL		5
+
+
+#define CST43228_ILP_DIV_EN		0x1
+#define	CST43228_OTP_PRESENT		0x2
+#define	CST43228_SERDES_REFCLK_PADSEL	0x4
+#define	CST43228_SDIO_MODE		0x8
+#define	CST43228_SDIO_OTP_PRESENT	0x10
+#define	CST43228_SDIO_RESET		0x20
+
+
+#define PMU_MAX_TRANSITION_DLY	15000
+
+
+#define PMURES_UP_TRANSITION	2
+
+
+
+#define SECI_MODE_UART			0x0
+#define SECI_MODE_SECI			0x1
+#define SECI_MODE_LEGACY_3WIRE_BT	0x2
+#define SECI_MODE_LEGACY_3WIRE_WLAN	0x3
+#define SECI_MODE_HALF_SECI		0x4
+
+#define SECI_RESET		(1 << 0)
+#define SECI_RESET_BAR_UART	(1 << 1)
+#define SECI_ENAB_SECI_ECI	(1 << 2)
+#define SECI_ENAB_SECIOUT_DIS	(1 << 3)
+#define SECI_MODE_MASK		0x7
+#define SECI_MODE_SHIFT		4 
+#define SECI_UPD_SECI		(1 << 7)
+
+
+#define CLKCTL_STS_SECI_CLK_REQ		(1 << 8)
+#define CLKCTL_STS_SECI_CLK_AVAIL	(1 << 24)
+
+#define SECI_UART_MSR_CTS_STATE		(1 << 0)
+#define SECI_UART_MSR_RTS_STATE		(1 << 1)
+#define SECI_UART_SECI_IN_STATE		(1 << 2)
+#define SECI_UART_SECI_IN2_STATE	(1 << 3)
+
+
+#define SECI_UART_LCR_STOP_BITS		(1 << 0) 
+#define SECI_UART_LCR_PARITY_EN		(1 << 1)
+#define SECI_UART_LCR_PARITY		(1 << 2) 
+#define SECI_UART_LCR_RX_EN		(1 << 3)
+#define SECI_UART_LCR_LBRK_CTRL		(1 << 4) 
+#define SECI_UART_LCR_TXO_EN		(1 << 5)
+#define SECI_UART_LCR_RTSO_EN		(1 << 6)
+#define SECI_UART_LCR_SLIPMODE_EN	(1 << 7)
+#define SECI_UART_LCR_RXCRC_CHK		(1 << 8)
+#define SECI_UART_LCR_TXCRC_INV		(1 << 9)
+#define SECI_UART_LCR_TXCRC_LSBF	(1 << 10)
+#define SECI_UART_LCR_TXCRC_EN		(1 << 11)
+
+#define SECI_UART_MCR_TX_EN		(1 << 0)
+#define SECI_UART_MCR_PRTS		(1 << 1)
+#define SECI_UART_MCR_SWFLCTRL_EN	(1 << 2)
+#define SECI_UART_MCR_HIGHRATE_EN	(1 << 3)
+#define SECI_UART_MCR_LOOPBK_EN		(1 << 4)
+#define SECI_UART_MCR_AUTO_RTS		(1 << 5)
+#define SECI_UART_MCR_AUTO_TX_DIS	(1 << 6)
+#define SECI_UART_MCR_BAUD_ADJ_EN	(1 << 7)
+#define SECI_UART_MCR_XONOFF_RPT	(1 << 9)
+
+
+
+
+#define ECI_BW_20   0x0
+#define ECI_BW_25   0x1
+#define ECI_BW_30   0x2
+#define ECI_BW_35   0x3
+#define ECI_BW_40   0x4
+#define ECI_BW_45   0x5
+#define ECI_BW_50   0x6
+#define ECI_BW_ALL  0x7
+
+
+#define WLAN_NUM_ANT1 TXANT_0
+#define WLAN_NUM_ANT2 TXANT_1
+
+#endif	
diff --git a/drivers/net/wireless/bcmdhd/include/sbconfig.h b/drivers/net/wireless/bcmdhd/include/sbconfig.h
new file mode 100644
index 000000000000..f45351a586cb
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/sbconfig.h
@@ -0,0 +1,276 @@
+/*
+ * Broadcom SiliconBackplane hardware register definitions.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sbconfig.h 277737 2011-08-16 17:54:59Z $
+ */
+
+
+#ifndef	_SBCONFIG_H
+#define	_SBCONFIG_H
+
+
+#ifndef PAD
+#define	_PADLINE(line)	pad ## line
+#define	_XSTR(line)	_PADLINE(line)
+#define	PAD		_XSTR(__LINE__)
+#endif
+
+
+#define SB_BUS_SIZE		0x10000		
+#define SB_BUS_BASE(b)		(SI_ENUM_BASE + (b) * SB_BUS_SIZE)
+#define	SB_BUS_MAXCORES		(SB_BUS_SIZE / SI_CORE_SIZE)	
+
+
+#define	SBCONFIGOFF		0xf00		
+#define	SBCONFIGSIZE		256		
+
+#define SBIPSFLAG		0x08
+#define SBTPSFLAG		0x18
+#define	SBTMERRLOGA		0x48		
+#define	SBTMERRLOG		0x50		
+#define SBADMATCH3		0x60
+#define SBADMATCH2		0x68
+#define SBADMATCH1		0x70
+#define SBIMSTATE		0x90
+#define SBINTVEC		0x94
+#define SBTMSTATELOW		0x98
+#define SBTMSTATEHIGH		0x9c
+#define SBBWA0			0xa0
+#define SBIMCONFIGLOW		0xa8
+#define SBIMCONFIGHIGH		0xac
+#define SBADMATCH0		0xb0
+#define SBTMCONFIGLOW		0xb8
+#define SBTMCONFIGHIGH		0xbc
+#define SBBCONFIG		0xc0
+#define SBBSTATE		0xc8
+#define SBACTCNFG		0xd8
+#define	SBFLAGST		0xe8
+#define SBIDLOW			0xf8
+#define SBIDHIGH		0xfc
+
+
+
+#define SBIMERRLOGA		0xea8
+#define SBIMERRLOG		0xeb0
+#define SBTMPORTCONNID0		0xed8
+#define SBTMPORTLOCK0		0xef8
+
+#ifndef _LANGUAGE_ASSEMBLY
+
+typedef volatile struct _sbconfig {
+	uint32	PAD[2];
+	uint32	sbipsflag;		
+	uint32	PAD[3];
+	uint32	sbtpsflag;		
+	uint32	PAD[11];
+	uint32	sbtmerrloga;		
+	uint32	PAD;
+	uint32	sbtmerrlog;		
+	uint32	PAD[3];
+	uint32	sbadmatch3;		
+	uint32	PAD;
+	uint32	sbadmatch2;		
+	uint32	PAD;
+	uint32	sbadmatch1;		
+	uint32	PAD[7];
+	uint32	sbimstate;		
+	uint32	sbintvec;		
+	uint32	sbtmstatelow;		
+	uint32	sbtmstatehigh;		
+	uint32	sbbwa0;			
+	uint32	PAD;
+	uint32	sbimconfiglow;		
+	uint32	sbimconfighigh;		
+	uint32	sbadmatch0;		
+	uint32	PAD;
+	uint32	sbtmconfiglow;		
+	uint32	sbtmconfighigh;		
+	uint32	sbbconfig;		
+	uint32	PAD;
+	uint32	sbbstate;		
+	uint32	PAD[3];
+	uint32	sbactcnfg;		
+	uint32	PAD[3];
+	uint32	sbflagst;		
+	uint32	PAD[3];
+	uint32	sbidlow;		
+	uint32	sbidhigh;		
+} sbconfig_t;
+
+#endif 
+
+
+#define	SBIPS_INT1_MASK		0x3f		
+#define	SBIPS_INT1_SHIFT	0
+#define	SBIPS_INT2_MASK		0x3f00		
+#define	SBIPS_INT2_SHIFT	8
+#define	SBIPS_INT3_MASK		0x3f0000	
+#define	SBIPS_INT3_SHIFT	16
+#define	SBIPS_INT4_MASK		0x3f000000	
+#define	SBIPS_INT4_SHIFT	24
+
+
+#define	SBTPS_NUM0_MASK		0x3f		
+#define	SBTPS_F0EN0		0x40		
+
+
+#define	SBTMEL_CM		0x00000007	
+#define	SBTMEL_CI		0x0000ff00	
+#define	SBTMEL_EC		0x0f000000	
+#define	SBTMEL_ME		0x80000000	
+
+
+#define	SBIM_PC			0xf		
+#define	SBIM_AP_MASK		0x30		
+#define	SBIM_AP_BOTH		0x00		
+#define	SBIM_AP_TS		0x10		
+#define	SBIM_AP_TK		0x20		
+#define	SBIM_AP_RSV		0x30		
+#define	SBIM_IBE		0x20000		
+#define	SBIM_TO			0x40000		
+#define	SBIM_BY			0x01800000	
+#define	SBIM_RJ			0x02000000	
+
+
+#define	SBTML_RESET		0x0001		
+#define	SBTML_REJ_MASK		0x0006		
+#define	SBTML_REJ		0x0002		
+#define	SBTML_TMPREJ		0x0004		
+
+#define	SBTML_SICF_SHIFT	16		
+
+
+#define	SBTMH_SERR		0x0001		
+#define	SBTMH_INT		0x0002		
+#define	SBTMH_BUSY		0x0004		
+#define	SBTMH_TO		0x0020		
+
+#define	SBTMH_SISF_SHIFT	16		
+
+
+#define	SBBWA_TAB0_MASK		0xffff		
+#define	SBBWA_TAB1_MASK		0xffff		
+#define	SBBWA_TAB1_SHIFT	16
+
+
+#define	SBIMCL_STO_MASK		0x7		
+#define	SBIMCL_RTO_MASK		0x70		
+#define	SBIMCL_RTO_SHIFT	4
+#define	SBIMCL_CID_MASK		0xff0000	
+#define	SBIMCL_CID_SHIFT	16
+
+
+#define	SBIMCH_IEM_MASK		0xc		
+#define	SBIMCH_TEM_MASK		0x30		
+#define	SBIMCH_TEM_SHIFT	4
+#define	SBIMCH_BEM_MASK		0xc0		
+#define	SBIMCH_BEM_SHIFT	6
+
+
+#define	SBAM_TYPE_MASK		0x3		
+#define	SBAM_AD64		0x4		
+#define	SBAM_ADINT0_MASK	0xf8		
+#define	SBAM_ADINT0_SHIFT	3
+#define	SBAM_ADINT1_MASK	0x1f8		
+#define	SBAM_ADINT1_SHIFT	3
+#define	SBAM_ADINT2_MASK	0x1f8		
+#define	SBAM_ADINT2_SHIFT	3
+#define	SBAM_ADEN		0x400		
+#define	SBAM_ADNEG		0x800		
+#define	SBAM_BASE0_MASK		0xffffff00	
+#define	SBAM_BASE0_SHIFT	8
+#define	SBAM_BASE1_MASK		0xfffff000	
+#define	SBAM_BASE1_SHIFT	12
+#define	SBAM_BASE2_MASK		0xffff0000	
+#define	SBAM_BASE2_SHIFT	16
+
+
+#define	SBTMCL_CD_MASK		0xff		
+#define	SBTMCL_CO_MASK		0xf800		
+#define	SBTMCL_CO_SHIFT		11
+#define	SBTMCL_IF_MASK		0xfc0000	
+#define	SBTMCL_IF_SHIFT		18
+#define	SBTMCL_IM_MASK		0x3000000	
+#define	SBTMCL_IM_SHIFT		24
+
+
+#define	SBTMCH_BM_MASK		0x3		
+#define	SBTMCH_RM_MASK		0x3		
+#define	SBTMCH_RM_SHIFT		2
+#define	SBTMCH_SM_MASK		0x30		
+#define	SBTMCH_SM_SHIFT		4
+#define	SBTMCH_EM_MASK		0x300		
+#define	SBTMCH_EM_SHIFT		8
+#define	SBTMCH_IM_MASK		0xc00		
+#define	SBTMCH_IM_SHIFT		10
+
+
+#define	SBBC_LAT_MASK		0x3		
+#define	SBBC_MAX0_MASK		0xf0000		
+#define	SBBC_MAX0_SHIFT		16
+#define	SBBC_MAX1_MASK		0xf00000	
+#define	SBBC_MAX1_SHIFT		20
+
+
+#define	SBBS_SRD		0x1		
+#define	SBBS_HRD		0x2		
+
+
+#define	SBIDL_CS_MASK		0x3		
+#define	SBIDL_AR_MASK		0x38		
+#define	SBIDL_AR_SHIFT		3
+#define	SBIDL_SYNCH		0x40		
+#define	SBIDL_INIT		0x80		
+#define	SBIDL_MINLAT_MASK	0xf00		
+#define	SBIDL_MINLAT_SHIFT	8
+#define	SBIDL_MAXLAT		0xf000		
+#define	SBIDL_MAXLAT_SHIFT	12
+#define	SBIDL_FIRST		0x10000		
+#define	SBIDL_CW_MASK		0xc0000		
+#define	SBIDL_CW_SHIFT		18
+#define	SBIDL_TP_MASK		0xf00000	
+#define	SBIDL_TP_SHIFT		20
+#define	SBIDL_IP_MASK		0xf000000	
+#define	SBIDL_IP_SHIFT		24
+#define	SBIDL_RV_MASK		0xf0000000	
+#define	SBIDL_RV_SHIFT		28
+#define	SBIDL_RV_2_2		0x00000000	
+#define	SBIDL_RV_2_3		0x10000000	
+
+
+#define	SBIDH_RC_MASK		0x000f		
+#define	SBIDH_RCE_MASK		0x7000		
+#define	SBIDH_RCE_SHIFT		8
+#define	SBCOREREV(sbidh) \
+	((((sbidh) & SBIDH_RCE_MASK) >> SBIDH_RCE_SHIFT) | ((sbidh) & SBIDH_RC_MASK))
+#define	SBIDH_CC_MASK		0x8ff0		
+#define	SBIDH_CC_SHIFT		4
+#define	SBIDH_VC_MASK		0xffff0000	
+#define	SBIDH_VC_SHIFT		16
+
+#define	SB_COMMIT		0xfd8		
+
+
+#define	SB_VEND_BCM		0x4243		
+
+#endif	
diff --git a/drivers/net/wireless/bcmdhd/include/sbhnddma.h b/drivers/net/wireless/bcmdhd/include/sbhnddma.h
new file mode 100644
index 000000000000..77c413f75f0d
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/sbhnddma.h
@@ -0,0 +1,327 @@
+/*
+ * Generic Broadcom Home Networking Division (HND) DMA engine HW interface
+ * This supports the following chips: BCM42xx, 44xx, 47xx .
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sbhnddma.h 278779 2011-08-19 22:07:18Z $
+ */
+
+
+#ifndef	_sbhnddma_h_
+#define	_sbhnddma_h_
+
+
+
+
+
+
+
+typedef volatile struct {
+	uint32	control;		
+	uint32	addr;			
+	uint32	ptr;			
+	uint32	status;			
+} dma32regs_t;
+
+typedef volatile struct {
+	dma32regs_t	xmt;		
+	dma32regs_t	rcv;		
+} dma32regp_t;
+
+typedef volatile struct {	
+	uint32	fifoaddr;		
+	uint32	fifodatalow;		
+	uint32	fifodatahigh;		
+	uint32	pad;			
+} dma32diag_t;
+
+
+typedef volatile struct {
+	uint32	ctrl;		
+	uint32	addr;		
+} dma32dd_t;
+
+
+#define	D32RINGALIGN_BITS	12
+#define	D32MAXRINGSZ		(1 << D32RINGALIGN_BITS)
+#define	D32RINGALIGN		(1 << D32RINGALIGN_BITS)
+
+#define	D32MAXDD	(D32MAXRINGSZ / sizeof (dma32dd_t))
+
+
+#define	XC_XE		((uint32)1 << 0)	
+#define	XC_SE		((uint32)1 << 1)	
+#define	XC_LE		((uint32)1 << 2)	
+#define	XC_FL		((uint32)1 << 4)	
+#define	XC_PD		((uint32)1 << 11)	
+#define	XC_AE		((uint32)3 << 16)	
+#define	XC_AE_SHIFT	16
+#define XC_BL_MASK	0x001C0000			
+#define XC_BL_SHIFT	18
+
+
+#define	XP_LD_MASK	0xfff			
+
+
+#define	XS_CD_MASK	0x0fff			
+#define	XS_XS_MASK	0xf000			
+#define	XS_XS_SHIFT	12
+#define	XS_XS_DISABLED	0x0000			
+#define	XS_XS_ACTIVE	0x1000			
+#define	XS_XS_IDLE	0x2000			
+#define	XS_XS_STOPPED	0x3000			
+#define	XS_XS_SUSP	0x4000			
+#define	XS_XE_MASK	0xf0000			
+#define	XS_XE_SHIFT	16
+#define	XS_XE_NOERR	0x00000			
+#define	XS_XE_DPE	0x10000			
+#define	XS_XE_DFU	0x20000			
+#define	XS_XE_BEBR	0x30000			
+#define	XS_XE_BEDA	0x40000			
+#define	XS_AD_MASK	0xfff00000		
+#define	XS_AD_SHIFT	20
+
+
+#define	RC_RE		((uint32)1 << 0)	
+#define	RC_RO_MASK	0xfe			
+#define	RC_RO_SHIFT	1
+#define	RC_FM		((uint32)1 << 8)	
+#define	RC_SH		((uint32)1 << 9)	
+#define	RC_OC		((uint32)1 << 10)	
+#define	RC_PD		((uint32)1 << 11)	
+#define	RC_AE		((uint32)3 << 16)	
+#define	RC_AE_SHIFT	16
+#define RC_BL_MASK	0x001C0000			
+#define RC_BL_SHIFT	18
+
+
+#define	RP_LD_MASK	0xfff			
+
+
+#define	RS_CD_MASK	0x0fff			
+#define	RS_RS_MASK	0xf000			
+#define	RS_RS_SHIFT	12
+#define	RS_RS_DISABLED	0x0000			
+#define	RS_RS_ACTIVE	0x1000			
+#define	RS_RS_IDLE	0x2000			
+#define	RS_RS_STOPPED	0x3000			
+#define	RS_RE_MASK	0xf0000			
+#define	RS_RE_SHIFT	16
+#define	RS_RE_NOERR	0x00000			
+#define	RS_RE_DPE	0x10000			
+#define	RS_RE_DFO	0x20000			
+#define	RS_RE_BEBW	0x30000			
+#define	RS_RE_BEDA	0x40000			
+#define	RS_AD_MASK	0xfff00000		
+#define	RS_AD_SHIFT	20
+
+
+#define	FA_OFF_MASK	0xffff			
+#define	FA_SEL_MASK	0xf0000			
+#define	FA_SEL_SHIFT	16
+#define	FA_SEL_XDD	0x00000			
+#define	FA_SEL_XDP	0x10000			
+#define	FA_SEL_RDD	0x40000			
+#define	FA_SEL_RDP	0x50000			
+#define	FA_SEL_XFD	0x80000			
+#define	FA_SEL_XFP	0x90000			
+#define	FA_SEL_RFD	0xc0000			
+#define	FA_SEL_RFP	0xd0000			
+#define	FA_SEL_RSD	0xe0000			
+#define	FA_SEL_RSP	0xf0000			
+
+
+#define	CTRL_BC_MASK	0x00001fff		
+#define	CTRL_AE		((uint32)3 << 16)	
+#define	CTRL_AE_SHIFT	16
+#define	CTRL_PARITY	((uint32)3 << 18)	
+#define	CTRL_EOT	((uint32)1 << 28)	
+#define	CTRL_IOC	((uint32)1 << 29)	
+#define	CTRL_EOF	((uint32)1 << 30)	
+#define	CTRL_SOF	((uint32)1 << 31)	
+
+
+#define	CTRL_CORE_MASK	0x0ff00000
+
+
+
+
+typedef volatile struct {
+	uint32	control;		
+	uint32	ptr;			
+	uint32	addrlow;		
+	uint32	addrhigh;		
+	uint32	status0;		
+	uint32	status1;		
+} dma64regs_t;
+
+typedef volatile struct {
+	dma64regs_t	tx;		
+	dma64regs_t	rx;		
+} dma64regp_t;
+
+typedef volatile struct {		
+	uint32	fifoaddr;		
+	uint32	fifodatalow;		
+	uint32	fifodatahigh;		
+	uint32	pad;			
+} dma64diag_t;
+
+
+typedef volatile struct {
+	uint32	ctrl1;		
+	uint32	ctrl2;		
+	uint32	addrlow;	
+	uint32	addrhigh;	
+} dma64dd_t;
+
+
+#define D64RINGALIGN_BITS	13
+#define	D64MAXRINGSZ		(1 << D64RINGALIGN_BITS)
+#define	D64RINGALIGN		(1 << D64RINGALIGN_BITS)
+
+#define	D64MAXDD	(D64MAXRINGSZ / sizeof (dma64dd_t))
+
+
+#define D64_DEF_USBBURSTLEN		2
+#define D64_DEF_SDIOBURSTLEN	1
+
+
+#define	D64_XC_XE		0x00000001	
+#define	D64_XC_SE		0x00000002	
+#define	D64_XC_LE		0x00000004	
+#define	D64_XC_FL		0x00000010	
+#define	D64_XC_PD		0x00000800	
+#define	D64_XC_AE		0x00030000	
+#define	D64_XC_AE_SHIFT		16
+#define D64_XC_BL_MASK	0x001C0000	
+#define D64_XC_BL_SHIFT		18
+
+
+#define	D64_XP_LD_MASK		0x00001fff	
+
+
+#define	D64_XS0_CD_MASK		0x00001fff	
+#define	D64_XS0_XS_MASK		0xf0000000     	
+#define	D64_XS0_XS_SHIFT		28
+#define	D64_XS0_XS_DISABLED	0x00000000	
+#define	D64_XS0_XS_ACTIVE	0x10000000	
+#define	D64_XS0_XS_IDLE		0x20000000	
+#define	D64_XS0_XS_STOPPED	0x30000000	
+#define	D64_XS0_XS_SUSP		0x40000000	
+
+#define	D64_XS1_AD_MASK		0x00001fff	
+#define	D64_XS1_XE_MASK		0xf0000000     	
+#define	D64_XS1_XE_SHIFT		28
+#define	D64_XS1_XE_NOERR	0x00000000	
+#define	D64_XS1_XE_DPE		0x10000000	
+#define	D64_XS1_XE_DFU		0x20000000	
+#define	D64_XS1_XE_DTE		0x30000000	
+#define	D64_XS1_XE_DESRE	0x40000000	
+#define	D64_XS1_XE_COREE	0x50000000	
+
+
+#define	D64_RC_RE		0x00000001	
+#define	D64_RC_RO_MASK		0x000000fe	
+#define	D64_RC_RO_SHIFT		1
+#define	D64_RC_FM		0x00000100	
+#define	D64_RC_SH		0x00000200	
+#define	D64_RC_OC		0x00000400	
+#define	D64_RC_PD		0x00000800	
+#define	D64_RC_AE		0x00030000	
+#define	D64_RC_AE_SHIFT		16
+#define D64_RC_BL_MASK	0x001C0000	
+#define D64_RC_BL_SHIFT		18
+
+
+#define DMA_CTRL_PEN		(1 << 0)	
+#define DMA_CTRL_ROC		(1 << 1)	
+#define DMA_CTRL_RXMULTI	(1 << 2)	
+#define DMA_CTRL_UNFRAMED	(1 << 3)	
+#define DMA_CTRL_USB_BOUNDRY4KB_WAR (1 << 4)
+
+
+#define	D64_RP_LD_MASK		0x00001fff	
+
+
+#define	D64_RS0_CD_MASK		0x00001fff	
+#define	D64_RS0_RS_MASK		0xf0000000     	
+#define	D64_RS0_RS_SHIFT		28
+#define	D64_RS0_RS_DISABLED	0x00000000	
+#define	D64_RS0_RS_ACTIVE	0x10000000	
+#define	D64_RS0_RS_IDLE		0x20000000	
+#define	D64_RS0_RS_STOPPED	0x30000000	
+#define	D64_RS0_RS_SUSP		0x40000000	
+
+#define	D64_RS1_AD_MASK		0x0001ffff	
+#define	D64_RS1_RE_MASK		0xf0000000     	
+#define	D64_RS1_RE_SHIFT		28
+#define	D64_RS1_RE_NOERR	0x00000000	
+#define	D64_RS1_RE_DPO		0x10000000	
+#define	D64_RS1_RE_DFU		0x20000000	
+#define	D64_RS1_RE_DTE		0x30000000	
+#define	D64_RS1_RE_DESRE	0x40000000	
+#define	D64_RS1_RE_COREE	0x50000000	
+
+
+#define	D64_FA_OFF_MASK		0xffff		
+#define	D64_FA_SEL_MASK		0xf0000		
+#define	D64_FA_SEL_SHIFT	16
+#define	D64_FA_SEL_XDD		0x00000		
+#define	D64_FA_SEL_XDP		0x10000		
+#define	D64_FA_SEL_RDD		0x40000		
+#define	D64_FA_SEL_RDP		0x50000		
+#define	D64_FA_SEL_XFD		0x80000		
+#define	D64_FA_SEL_XFP		0x90000		
+#define	D64_FA_SEL_RFD		0xc0000		
+#define	D64_FA_SEL_RFP		0xd0000		
+#define	D64_FA_SEL_RSD		0xe0000		
+#define	D64_FA_SEL_RSP		0xf0000		
+
+
+#define D64_CTRL_COREFLAGS	0x0ff00000	
+#define	D64_CTRL1_EOT		((uint32)1 << 28)	
+#define	D64_CTRL1_IOC		((uint32)1 << 29)	
+#define	D64_CTRL1_EOF		((uint32)1 << 30)	
+#define	D64_CTRL1_SOF		((uint32)1 << 31)	
+
+
+#define	D64_CTRL2_BC_MASK	0x00007fff	
+#define	D64_CTRL2_AE		0x00030000	
+#define	D64_CTRL2_AE_SHIFT	16
+#define D64_CTRL2_PARITY	0x00040000      
+
+
+#define	D64_CTRL_CORE_MASK	0x0ff00000
+
+#define D64_RX_FRM_STS_LEN	0x0000ffff	
+#define D64_RX_FRM_STS_OVFL	0x00800000	
+#define D64_RX_FRM_STS_DSCRCNT	0x0f000000	
+#define D64_RX_FRM_STS_DATATYPE	0xf0000000	
+
+
+typedef volatile struct {
+	uint16 len;
+	uint16 flags;
+} dma_rxh_t;
+
+#endif	
diff --git a/drivers/net/wireless/bcmdhd/include/sbpcmcia.h b/drivers/net/wireless/bcmdhd/include/sbpcmcia.h
new file mode 100644
index 000000000000..d84f69ab5617
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/sbpcmcia.h
@@ -0,0 +1,109 @@
+/*
+ * BCM43XX Sonics SiliconBackplane PCMCIA core hardware definitions.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sbpcmcia.h 277737 2011-08-16 17:54:59Z $
+ */
+
+
+#ifndef	_SBPCMCIA_H
+#define	_SBPCMCIA_H
+
+
+
+
+#define	PCMCIA_FCR		(0x700 / 2)
+
+#define	FCR0_OFF		0
+#define	FCR1_OFF		(0x40 / 2)
+#define	FCR2_OFF		(0x80 / 2)
+#define	FCR3_OFF		(0xc0 / 2)
+
+#define	PCMCIA_FCR0		(0x700 / 2)
+#define	PCMCIA_FCR1		(0x740 / 2)
+#define	PCMCIA_FCR2		(0x780 / 2)
+#define	PCMCIA_FCR3		(0x7c0 / 2)
+
+
+
+#define	PCMCIA_COR		0
+
+#define	COR_RST			0x80
+#define	COR_LEV			0x40
+#define	COR_IRQEN		0x04
+#define	COR_BLREN		0x01
+#define	COR_FUNEN		0x01
+
+
+#define	PCICIA_FCSR		(2 / 2)
+#define	PCICIA_PRR		(4 / 2)
+#define	PCICIA_SCR		(6 / 2)
+#define	PCICIA_ESR		(8 / 2)
+
+
+#define PCM_MEMOFF		0x0000
+#define F0_MEMOFF		0x1000
+#define F1_MEMOFF		0x2000
+#define F2_MEMOFF		0x3000
+#define F3_MEMOFF		0x4000
+
+
+#define MEM_ADDR0		(0x728 / 2)
+#define MEM_ADDR1		(0x72a / 2)
+#define MEM_ADDR2		(0x72c / 2)
+
+
+#define PCMCIA_ADDR0		(0x072e / 2)
+#define PCMCIA_ADDR1		(0x0730 / 2)
+#define PCMCIA_ADDR2		(0x0732 / 2)
+
+#define MEM_SEG			(0x0734 / 2)
+#define SROM_CS			(0x0736 / 2)
+#define SROM_DATAL		(0x0738 / 2)
+#define SROM_DATAH		(0x073a / 2)
+#define SROM_ADDRL		(0x073c / 2)
+#define SROM_ADDRH		(0x073e / 2)
+#define	SROM_INFO2		(0x0772 / 2)	
+#define	SROM_INFO		(0x07be / 2)	
+
+
+#define SROM_IDLE		0
+#define SROM_WRITE		1
+#define SROM_READ		2
+#define SROM_WEN		4
+#define SROM_WDS		7
+#define SROM_DONE		8
+
+
+#define	SRI_SZ_MASK		0x03
+#define	SRI_BLANK		0x04
+#define	SRI_OTP			0x80
+
+
+
+#define SBTML_INT_ACK		0x40000		
+#define SBTML_INT_EN		0x20000		
+
+
+#define SBTMH_INT_STATUS	0x40000		
+
+#endif	
diff --git a/drivers/net/wireless/bcmdhd/include/sbsdio.h b/drivers/net/wireless/bcmdhd/include/sbsdio.h
new file mode 100644
index 000000000000..7aaeb73f0100
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/sbsdio.h
@@ -0,0 +1,166 @@
+/*
+ * SDIO device core hardware definitions.
+ * sdio is a portion of the pcmcia core in core rev 3 - rev 8
+ *
+ * SDIO core support 1bit, 4 bit SDIO mode as well as SPI mode.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sbsdio.h 277737 2011-08-16 17:54:59Z $
+ */
+
+#ifndef	_SBSDIO_H
+#define	_SBSDIO_H
+
+#define SBSDIO_NUM_FUNCTION		3	/* as of sdiod rev 0, supports 3 functions */
+
+/* function 1 miscellaneous registers */
+#define SBSDIO_SPROM_CS			0x10000		/* sprom command and status */
+#define SBSDIO_SPROM_INFO		0x10001		/* sprom info register */
+#define SBSDIO_SPROM_DATA_LOW		0x10002		/* sprom indirect access data byte 0 */
+#define SBSDIO_SPROM_DATA_HIGH		0x10003 	/* sprom indirect access data byte 1 */
+#define SBSDIO_SPROM_ADDR_LOW		0x10004		/* sprom indirect access addr byte 0 */
+#define SBSDIO_SPROM_ADDR_HIGH		0x10005		/* sprom indirect access addr byte 0 */
+#define SBSDIO_CHIP_CTRL_DATA		0x10006		/* xtal_pu (gpio) output */
+#define SBSDIO_CHIP_CTRL_EN		0x10007		/* xtal_pu (gpio) enable */
+#define SBSDIO_WATERMARK		0x10008		/* rev < 7, watermark for sdio device */
+#define SBSDIO_DEVICE_CTL		0x10009		/* control busy signal generation */
+
+/* registers introduced in rev 8, some content (mask/bits) defs in sbsdpcmdev.h */
+#define SBSDIO_FUNC1_SBADDRLOW		0x1000A		/* SB Address Window Low (b15) */
+#define SBSDIO_FUNC1_SBADDRMID		0x1000B		/* SB Address Window Mid (b23:b16) */
+#define SBSDIO_FUNC1_SBADDRHIGH		0x1000C		/* SB Address Window High (b31:b24)    */
+#define SBSDIO_FUNC1_FRAMECTRL		0x1000D		/* Frame Control (frame term/abort) */
+#define SBSDIO_FUNC1_CHIPCLKCSR		0x1000E		/* ChipClockCSR (ALP/HT ctl/status) */
+#define SBSDIO_FUNC1_SDIOPULLUP 	0x1000F		/* SdioPullUp (on cmd, d0-d2) */
+#define SBSDIO_FUNC1_WFRAMEBCLO		0x10019		/* Write Frame Byte Count Low */
+#define SBSDIO_FUNC1_WFRAMEBCHI		0x1001A		/* Write Frame Byte Count High */
+#define SBSDIO_FUNC1_RFRAMEBCLO		0x1001B		/* Read Frame Byte Count Low */
+#define SBSDIO_FUNC1_RFRAMEBCHI		0x1001C		/* Read Frame Byte Count High */
+
+#define SBSDIO_FUNC1_MISC_REG_START	0x10000 	/* f1 misc register start */
+#define SBSDIO_FUNC1_MISC_REG_LIMIT	0x1001C 	/* f1 misc register end */
+
+/* SBSDIO_SPROM_CS */
+#define SBSDIO_SPROM_IDLE		0
+#define SBSDIO_SPROM_WRITE		1
+#define SBSDIO_SPROM_READ		2
+#define SBSDIO_SPROM_WEN		4
+#define SBSDIO_SPROM_WDS		7
+#define SBSDIO_SPROM_DONE		8
+
+/* SBSDIO_SPROM_INFO */
+#define SROM_SZ_MASK			0x03		/* SROM size, 1: 4k, 2: 16k */
+#define SROM_BLANK			0x04		/* depreciated in corerev 6 */
+#define	SROM_OTP			0x80		/* OTP present */
+
+/* SBSDIO_CHIP_CTRL */
+#define SBSDIO_CHIP_CTRL_XTAL		0x01		/* or'd with onchip xtal_pu,
+							 * 1: power on oscillator
+							 * (for 4318 only)
+							 */
+/* SBSDIO_WATERMARK */
+#define SBSDIO_WATERMARK_MASK		0x7f		/* number of words - 1 for sd device
+							 * to wait before sending data to host
+							 */
+
+/* SBSDIO_DEVICE_CTL */
+#define SBSDIO_DEVCTL_SETBUSY		0x01		/* 1: device will assert busy signal when
+							 * receiving CMD53
+							 */
+#define SBSDIO_DEVCTL_SPI_INTR_SYNC	0x02		/* 1: assertion of sdio interrupt is
+							 * synchronous to the sdio clock
+							 */
+#define SBSDIO_DEVCTL_CA_INT_ONLY	0x04		/* 1: mask all interrupts to host
+							 * except the chipActive (rev 8)
+							 */
+#define SBSDIO_DEVCTL_PADS_ISO		0x08		/* 1: isolate internal sdio signals, put
+							 * external pads in tri-state; requires
+							 * sdio bus power cycle to clear (rev 9)
+							 */
+#define SBSDIO_DEVCTL_SB_RST_CTL	0x30		/* Force SD->SB reset mapping (rev 11) */
+#define SBSDIO_DEVCTL_RST_CORECTL	0x00		/*   Determined by CoreControl bit */
+#define SBSDIO_DEVCTL_RST_BPRESET	0x10		/*   Force backplane reset */
+#define SBSDIO_DEVCTL_RST_NOBPRESET	0x20		/*   Force no backplane reset */
+
+
+/* SBSDIO_FUNC1_CHIPCLKCSR */
+#define SBSDIO_FORCE_ALP		0x01		/* Force ALP request to backplane */
+#define SBSDIO_FORCE_HT			0x02		/* Force HT request to backplane */
+#define SBSDIO_FORCE_ILP		0x04		/* Force ILP request to backplane */
+#define SBSDIO_ALP_AVAIL_REQ		0x08		/* Make ALP ready (power up xtal) */
+#define SBSDIO_HT_AVAIL_REQ		0x10		/* Make HT ready (power up PLL) */
+#define SBSDIO_FORCE_HW_CLKREQ_OFF	0x20		/* Squelch clock requests from HW */
+#define SBSDIO_ALP_AVAIL		0x40		/* Status: ALP is ready */
+#define SBSDIO_HT_AVAIL			0x80		/* Status: HT is ready */
+/* In rev8, actual avail bits followed original docs */
+#define SBSDIO_Rev8_HT_AVAIL		0x40
+#define SBSDIO_Rev8_ALP_AVAIL		0x80
+
+#define SBSDIO_AVBITS			(SBSDIO_HT_AVAIL | SBSDIO_ALP_AVAIL)
+#define SBSDIO_ALPAV(regval)		((regval) & SBSDIO_AVBITS)
+#define SBSDIO_HTAV(regval)		(((regval) & SBSDIO_AVBITS) == SBSDIO_AVBITS)
+#define SBSDIO_ALPONLY(regval)		(SBSDIO_ALPAV(regval) && !SBSDIO_HTAV(regval))
+#define SBSDIO_CLKAV(regval, alponly)	(SBSDIO_ALPAV(regval) && \
+					(alponly ? 1 : SBSDIO_HTAV(regval)))
+
+/* SBSDIO_FUNC1_SDIOPULLUP */
+#define SBSDIO_PULLUP_D0		0x01		/* Enable D0/MISO pullup */
+#define SBSDIO_PULLUP_D1		0x02		/* Enable D1/INT# pullup */
+#define SBSDIO_PULLUP_D2		0x04		/* Enable D2 pullup */
+#define SBSDIO_PULLUP_CMD		0x08		/* Enable CMD/MOSI pullup */
+#define SBSDIO_PULLUP_ALL		0x0f		/* All valid bits */
+
+/* function 1 OCP space */
+#define SBSDIO_SB_OFT_ADDR_MASK		0x07FFF		/* sb offset addr is <= 15 bits, 32k */
+#define SBSDIO_SB_OFT_ADDR_LIMIT	0x08000
+#define SBSDIO_SB_ACCESS_2_4B_FLAG	0x08000		/* with b15, maps to 32-bit SB access */
+
+/* some duplication with sbsdpcmdev.h here */
+/* valid bits in SBSDIO_FUNC1_SBADDRxxx regs */
+#define SBSDIO_SBADDRLOW_MASK		0x80		/* Valid bits in SBADDRLOW */
+#define SBSDIO_SBADDRMID_MASK		0xff		/* Valid bits in SBADDRMID */
+#define SBSDIO_SBADDRHIGH_MASK		0xffU		/* Valid bits in SBADDRHIGH */
+#define SBSDIO_SBWINDOW_MASK		0xffff8000	/* Address bits from SBADDR regs */
+
+/* direct(mapped) cis space */
+#define SBSDIO_CIS_BASE_COMMON		0x1000		/* MAPPED common CIS address */
+#define SBSDIO_CIS_SIZE_LIMIT		0x200		/* maximum bytes in one CIS */
+#define SBSDIO_OTP_CIS_SIZE_LIMIT       0x078           /* maximum bytes OTP CIS */
+
+#define SBSDIO_CIS_OFT_ADDR_MASK	0x1FFFF		/* cis offset addr is < 17 bits */
+
+#define SBSDIO_CIS_MANFID_TUPLE_LEN	6		/* manfid tuple length, include tuple,
+							 * link bytes
+							 */
+
+/* indirect cis access (in sprom) */
+#define SBSDIO_SPROM_CIS_OFFSET		0x8		/* 8 control bytes first, CIS starts from
+							 * 8th byte
+							 */
+
+#define SBSDIO_BYTEMODE_DATALEN_MAX	64		/* sdio byte mode: maximum length of one
+							 * data comamnd
+							 */
+
+#define SBSDIO_CORE_ADDR_MASK		0x1FFFF		/* sdio core function one address mask */
+
+#endif	/* _SBSDIO_H */
diff --git a/drivers/net/wireless/bcmdhd/include/sbsdpcmdev.h b/drivers/net/wireless/bcmdhd/include/sbsdpcmdev.h
new file mode 100644
index 000000000000..e5176483e9a1
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/sbsdpcmdev.h
@@ -0,0 +1,293 @@
+/*
+ * Broadcom SiliconBackplane SDIO/PCMCIA hardware-specific
+ * device core support
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sbsdpcmdev.h 282638 2011-09-08 21:18:10Z $
+ */
+
+#ifndef	_sbsdpcmdev_h_
+#define	_sbsdpcmdev_h_
+
+/* cpp contortions to concatenate w/arg prescan */
+#ifndef PAD
+#define	_PADLINE(line)	pad ## line
+#define	_XSTR(line)	_PADLINE(line)
+#define	PAD		_XSTR(__LINE__)
+#endif	/* PAD */
+
+
+typedef volatile struct {
+	dma64regs_t	xmt;		/* dma tx */
+	uint32 PAD[2];
+	dma64regs_t	rcv;		/* dma rx */
+	uint32 PAD[2];
+} dma64p_t;
+
+/* dma64 sdiod corerev >= 1 */
+typedef volatile struct {
+	dma64p_t dma64regs[2];
+	dma64diag_t dmafifo;		/* DMA Diagnostic Regs, 0x280-0x28c */
+	uint32 PAD[92];
+} sdiodma64_t;
+
+/* dma32 sdiod corerev == 0 */
+typedef volatile struct {
+	dma32regp_t dma32regs[2];	/* dma tx & rx, 0x200-0x23c */
+	dma32diag_t dmafifo;		/* DMA Diagnostic Regs, 0x240-0x24c */
+	uint32 PAD[108];
+} sdiodma32_t;
+
+/* dma32 regs for pcmcia core */
+typedef volatile struct {
+	dma32regp_t dmaregs;		/* DMA Regs, 0x200-0x21c, rev8 */
+	dma32diag_t dmafifo;		/* DMA Diagnostic Regs, 0x220-0x22c */
+	uint32 PAD[116];
+} pcmdma32_t;
+
+/* core registers */
+typedef volatile struct {
+	uint32 corecontrol;		/* CoreControl, 0x000, rev8 */
+	uint32 corestatus;		/* CoreStatus, 0x004, rev8  */
+	uint32 PAD[1];
+	uint32 biststatus;		/* BistStatus, 0x00c, rev8  */
+
+	/* PCMCIA access */
+	uint16 pcmciamesportaladdr;	/* PcmciaMesPortalAddr, 0x010, rev8   */
+	uint16 PAD[1];
+	uint16 pcmciamesportalmask;	/* PcmciaMesPortalMask, 0x014, rev8   */
+	uint16 PAD[1];
+	uint16 pcmciawrframebc;		/* PcmciaWrFrameBC, 0x018, rev8   */
+	uint16 PAD[1];
+	uint16 pcmciaunderflowtimer;	/* PcmciaUnderflowTimer, 0x01c, rev8   */
+	uint16 PAD[1];
+
+	/* interrupt */
+	uint32 intstatus;		/* IntStatus, 0x020, rev8   */
+	uint32 hostintmask;		/* IntHostMask, 0x024, rev8   */
+	uint32 intmask;			/* IntSbMask, 0x028, rev8   */
+	uint32 sbintstatus;		/* SBIntStatus, 0x02c, rev8   */
+	uint32 sbintmask;		/* SBIntMask, 0x030, rev8   */
+	uint32 funcintmask;		/* SDIO Function Interrupt Mask, SDIO rev4 */
+	uint32 PAD[2];
+	uint32 tosbmailbox;		/* ToSBMailbox, 0x040, rev8   */
+	uint32 tohostmailbox;		/* ToHostMailbox, 0x044, rev8   */
+	uint32 tosbmailboxdata;		/* ToSbMailboxData, 0x048, rev8   */
+	uint32 tohostmailboxdata;	/* ToHostMailboxData, 0x04c, rev8   */
+
+	/* synchronized access to registers in SDIO clock domain */
+	uint32 sdioaccess;		/* SdioAccess, 0x050, rev8   */
+	uint32 PAD[3];
+
+	/* PCMCIA frame control */
+	uint8 pcmciaframectrl;		/* pcmciaFrameCtrl, 0x060, rev8   */
+	uint8 PAD[3];
+	uint8 pcmciawatermark;		/* pcmciaWaterMark, 0x064, rev8   */
+	uint8 PAD[155];
+
+	/* interrupt batching control */
+	uint32 intrcvlazy;		/* IntRcvLazy, 0x100, rev8 */
+	uint32 PAD[3];
+
+	/* counters */
+	uint32 cmd52rd;			/* Cmd52RdCount, 0x110, rev8, SDIO: cmd52 reads */
+	uint32 cmd52wr;			/* Cmd52WrCount, 0x114, rev8, SDIO: cmd52 writes */
+	uint32 cmd53rd;			/* Cmd53RdCount, 0x118, rev8, SDIO: cmd53 reads */
+	uint32 cmd53wr;			/* Cmd53WrCount, 0x11c, rev8, SDIO: cmd53 writes */
+	uint32 abort;			/* AbortCount, 0x120, rev8, SDIO: aborts */
+	uint32 datacrcerror;		/* DataCrcErrorCount, 0x124, rev8, SDIO: frames w/bad CRC */
+	uint32 rdoutofsync;		/* RdOutOfSyncCount, 0x128, rev8, SDIO/PCMCIA: Rd Frm OOS */
+	uint32 wroutofsync;		/* RdOutOfSyncCount, 0x12c, rev8, SDIO/PCMCIA: Wr Frm OOS */
+	uint32 writebusy;		/* WriteBusyCount, 0x130, rev8, SDIO: dev asserted "busy" */
+	uint32 readwait;		/* ReadWaitCount, 0x134, rev8, SDIO: read: no data avail */
+	uint32 readterm;		/* ReadTermCount, 0x138, rev8, SDIO: rd frm terminates */
+	uint32 writeterm;		/* WriteTermCount, 0x13c, rev8, SDIO: wr frm terminates */
+	uint32 PAD[40];
+	uint32 clockctlstatus;		/* ClockCtlStatus, 0x1e0, rev8 */
+	uint32 PAD[7];
+
+	/* DMA engines */
+	volatile union {
+		pcmdma32_t pcm32;
+		sdiodma32_t sdiod32;
+		sdiodma64_t sdiod64;
+	} dma;
+
+	/* SDIO/PCMCIA CIS region */
+	char cis[512];			/* 512 byte CIS, 0x400-0x5ff, rev6 */
+
+	/* PCMCIA function control registers */
+	char pcmciafcr[256];		/* PCMCIA FCR, 0x600-6ff, rev6 */
+	uint16 PAD[55];
+
+	/* PCMCIA backplane access */
+	uint16 backplanecsr;		/* BackplaneCSR, 0x76E, rev6 */
+	uint16 backplaneaddr0;		/* BackplaneAddr0, 0x770, rev6 */
+	uint16 backplaneaddr1;		/* BackplaneAddr1, 0x772, rev6 */
+	uint16 backplaneaddr2;		/* BackplaneAddr2, 0x774, rev6 */
+	uint16 backplaneaddr3;		/* BackplaneAddr3, 0x776, rev6 */
+	uint16 backplanedata0;		/* BackplaneData0, 0x778, rev6 */
+	uint16 backplanedata1;		/* BackplaneData1, 0x77a, rev6 */
+	uint16 backplanedata2;		/* BackplaneData2, 0x77c, rev6 */
+	uint16 backplanedata3;		/* BackplaneData3, 0x77e, rev6 */
+	uint16 PAD[31];
+
+	/* sprom "size" & "blank" info */
+	uint16 spromstatus;		/* SPROMStatus, 0x7BE, rev2 */
+	uint32 PAD[464];
+
+	/* Sonics SiliconBackplane registers */
+	sbconfig_t sbconfig;		/* SbConfig Regs, 0xf00-0xfff, rev8 */
+} sdpcmd_regs_t;
+
+/* corecontrol */
+#define CC_CISRDY		(1 << 0)	/* CIS Ready */
+#define CC_BPRESEN		(1 << 1)	/* CCCR RES signal causes backplane reset */
+#define CC_F2RDY		(1 << 2)	/* set CCCR IOR2 bit */
+#define CC_CLRPADSISO		(1 << 3)	/* clear SDIO pads isolation bit (rev 11) */
+#define CC_XMTDATAAVAIL_MODE	(1 << 4)	/* data avail generates an interrupt */
+#define CC_XMTDATAAVAIL_CTRL	(1 << 5)	/* data avail interrupt ctrl */
+
+/* corestatus */
+#define CS_PCMCIAMODE	(1 << 0)	/* Device Mode; 0=SDIO, 1=PCMCIA */
+#define CS_SMARTDEV	(1 << 1)	/* 1=smartDev enabled */
+#define CS_F2ENABLED	(1 << 2)	/* 1=host has enabled the device */
+
+#define PCMCIA_MES_PA_MASK	0x7fff	/* PCMCIA Message Portal Address Mask */
+#define PCMCIA_MES_PM_MASK	0x7fff	/* PCMCIA Message Portal Mask Mask */
+#define PCMCIA_WFBC_MASK	0xffff	/* PCMCIA Write Frame Byte Count Mask */
+#define PCMCIA_UT_MASK		0x07ff	/* PCMCIA Underflow Timer Mask */
+
+/* intstatus */
+#define I_SMB_SW0	(1 << 0)	/* To SB Mail S/W interrupt 0 */
+#define I_SMB_SW1	(1 << 1)	/* To SB Mail S/W interrupt 1 */
+#define I_SMB_SW2	(1 << 2)	/* To SB Mail S/W interrupt 2 */
+#define I_SMB_SW3	(1 << 3)	/* To SB Mail S/W interrupt 3 */
+#define I_SMB_SW_MASK	0x0000000f	/* To SB Mail S/W interrupts mask */
+#define I_SMB_SW_SHIFT	0		/* To SB Mail S/W interrupts shift */
+#define I_HMB_SW0	(1 << 4)	/* To Host Mail S/W interrupt 0 */
+#define I_HMB_SW1	(1 << 5)	/* To Host Mail S/W interrupt 1 */
+#define I_HMB_SW2	(1 << 6)	/* To Host Mail S/W interrupt 2 */
+#define I_HMB_SW3	(1 << 7)	/* To Host Mail S/W interrupt 3 */
+#define I_HMB_SW_MASK	0x000000f0	/* To Host Mail S/W interrupts mask */
+#define I_HMB_SW_SHIFT	4		/* To Host Mail S/W interrupts shift */
+#define I_WR_OOSYNC	(1 << 8)	/* Write Frame Out Of Sync */
+#define I_RD_OOSYNC	(1 << 9)	/* Read Frame Out Of Sync */
+#define	I_PC		(1 << 10)	/* descriptor error */
+#define	I_PD		(1 << 11)	/* data error */
+#define	I_DE		(1 << 12)	/* Descriptor protocol Error */
+#define	I_RU		(1 << 13)	/* Receive descriptor Underflow */
+#define	I_RO		(1 << 14)	/* Receive fifo Overflow */
+#define	I_XU		(1 << 15)	/* Transmit fifo Underflow */
+#define	I_RI		(1 << 16)	/* Receive Interrupt */
+#define I_BUSPWR	(1 << 17)	/* SDIO Bus Power Change (rev 9) */
+#define I_XMTDATA_AVAIL (1 << 23)	/* bits in fifo */
+#define	I_XI		(1 << 24)	/* Transmit Interrupt */
+#define I_RF_TERM	(1 << 25)	/* Read Frame Terminate */
+#define I_WF_TERM	(1 << 26)	/* Write Frame Terminate */
+#define I_PCMCIA_XU	(1 << 27)	/* PCMCIA Transmit FIFO Underflow */
+#define I_SBINT		(1 << 28)	/* sbintstatus Interrupt */
+#define I_CHIPACTIVE	(1 << 29)	/* chip transitioned from doze to active state */
+#define I_SRESET	(1 << 30)	/* CCCR RES interrupt */
+#define I_IOE2		(1U << 31)	/* CCCR IOE2 Bit Changed */
+#define	I_ERRORS	(I_PC | I_PD | I_DE | I_RU | I_RO | I_XU)	/* DMA Errors */
+#define I_DMA		(I_RI | I_XI | I_ERRORS)
+
+/* sbintstatus */
+#define I_SB_SERR	(1 << 8)	/* Backplane SError (write) */
+#define I_SB_RESPERR	(1 << 9)	/* Backplane Response Error (read) */
+#define I_SB_SPROMERR	(1 << 10)	/* Error accessing the sprom */
+
+/* sdioaccess */
+#define SDA_DATA_MASK	0x000000ff	/* Read/Write Data Mask */
+#define SDA_ADDR_MASK	0x000fff00	/* Read/Write Address Mask */
+#define SDA_ADDR_SHIFT	8		/* Read/Write Address Shift */
+#define SDA_WRITE	0x01000000	/* Write bit  */
+#define SDA_READ	0x00000000	/* Write bit cleared for Read */
+#define SDA_BUSY	0x80000000	/* Busy bit */
+
+/* sdioaccess-accessible register address spaces */
+#define SDA_CCCR_SPACE		0x000	/* sdioAccess CCCR register space */
+#define SDA_F1_FBR_SPACE	0x100	/* sdioAccess F1 FBR register space */
+#define SDA_F2_FBR_SPACE	0x200	/* sdioAccess F2 FBR register space */
+#define SDA_F1_REG_SPACE	0x300	/* sdioAccess F1 core-specific register space */
+
+/* SDA_F1_REG_SPACE sdioaccess-accessible F1 reg space register offsets */
+#define SDA_CHIPCONTROLDATA	0x006	/* ChipControlData */
+#define SDA_CHIPCONTROLENAB	0x007	/* ChipControlEnable */
+#define SDA_F2WATERMARK		0x008	/* Function 2 Watermark */
+#define SDA_DEVICECONTROL	0x009	/* DeviceControl */
+#define SDA_SBADDRLOW		0x00a	/* SbAddrLow */
+#define SDA_SBADDRMID		0x00b	/* SbAddrMid */
+#define SDA_SBADDRHIGH		0x00c	/* SbAddrHigh */
+#define SDA_FRAMECTRL		0x00d	/* FrameCtrl */
+#define SDA_CHIPCLOCKCSR	0x00e	/* ChipClockCSR */
+#define SDA_SDIOPULLUP		0x00f	/* SdioPullUp */
+#define SDA_SDIOWRFRAMEBCLOW	0x019	/* SdioWrFrameBCLow */
+#define SDA_SDIOWRFRAMEBCHIGH	0x01a	/* SdioWrFrameBCHigh */
+#define SDA_SDIORDFRAMEBCLOW	0x01b	/* SdioRdFrameBCLow */
+#define SDA_SDIORDFRAMEBCHIGH	0x01c	/* SdioRdFrameBCHigh */
+
+/* SDA_F2WATERMARK */
+#define SDA_F2WATERMARK_MASK	0x7f	/* F2Watermark Mask */
+
+/* SDA_SBADDRLOW */
+#define SDA_SBADDRLOW_MASK	0x80	/* SbAddrLow Mask */
+
+/* SDA_SBADDRMID */
+#define SDA_SBADDRMID_MASK	0xff	/* SbAddrMid Mask */
+
+/* SDA_SBADDRHIGH */
+#define SDA_SBADDRHIGH_MASK	0xff	/* SbAddrHigh Mask */
+
+/* SDA_FRAMECTRL */
+#define SFC_RF_TERM	(1 << 0)	/* Read Frame Terminate */
+#define SFC_WF_TERM	(1 << 1)	/* Write Frame Terminate */
+#define SFC_CRC4WOOS	(1 << 2)	/* HW reports CRC error for write out of sync */
+#define SFC_ABORTALL	(1 << 3)	/* Abort cancels all in-progress frames */
+
+/* pcmciaframectrl */
+#define PFC_RF_TERM	(1 << 0)	/* Read Frame Terminate */
+#define PFC_WF_TERM	(1 << 1)	/* Write Frame Terminate */
+
+/* intrcvlazy */
+#define	IRL_TO_MASK	0x00ffffff	/* timeout */
+#define	IRL_FC_MASK	0xff000000	/* frame count */
+#define	IRL_FC_SHIFT	24		/* frame count */
+
+/* rx header */
+typedef volatile struct {
+	uint16 len;
+	uint16 flags;
+} sdpcmd_rxh_t;
+
+/* rx header flags */
+#define RXF_CRC		0x0001		/* CRC error detected */
+#define RXF_WOOS	0x0002		/* write frame out of sync */
+#define RXF_WF_TERM	0x0004		/* write frame terminated */
+#define RXF_ABORT	0x0008		/* write frame aborted */
+#define RXF_DISCARD	(RXF_CRC | RXF_WOOS | RXF_WF_TERM | RXF_ABORT)	/* bad frame */
+
+/* HW frame tag */
+#define SDPCM_FRAMETAG_LEN	4	/* HW frametag: 2 bytes len, 2 bytes check val */
+
+#endif	/* _sbsdpcmdev_h_ */
diff --git a/drivers/net/wireless/bcmdhd/include/sbsocram.h b/drivers/net/wireless/bcmdhd/include/sbsocram.h
new file mode 100644
index 000000000000..45c4dc208bda
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/sbsocram.h
@@ -0,0 +1,186 @@
+/*
+ * BCM47XX Sonics SiliconBackplane embedded ram core
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sbsocram.h 277737 2011-08-16 17:54:59Z $
+ */
+
+
+#ifndef	_SBSOCRAM_H
+#define	_SBSOCRAM_H
+
+#ifndef _LANGUAGE_ASSEMBLY
+
+
+#ifndef PAD
+#define	_PADLINE(line)	pad ## line
+#define	_XSTR(line)	_PADLINE(line)
+#define	PAD		_XSTR(__LINE__)
+#endif	
+
+
+typedef volatile struct sbsocramregs {
+	uint32	coreinfo;
+	uint32	bwalloc;
+	uint32	extracoreinfo;
+	uint32	biststat;
+	uint32	bankidx;
+	uint32	standbyctrl;
+
+	uint32	errlogstatus;	
+	uint32	errlogaddr;	
+	
+	uint32	cambankidx;
+	uint32	cambankstandbyctrl;
+	uint32	cambankpatchctrl;
+	uint32	cambankpatchtblbaseaddr;
+	uint32	cambankcmdreg;
+	uint32	cambankdatareg;
+	uint32	cambankmaskreg;
+	uint32	PAD[1];
+	uint32	bankinfo;	
+	uint32	PAD[15];
+	uint32	extmemconfig;
+	uint32	extmemparitycsr;
+	uint32	extmemparityerrdata;
+	uint32	extmemparityerrcnt;
+	uint32	extmemwrctrlandsize;
+	uint32	PAD[84];
+	uint32	workaround;
+	uint32	pwrctl;		
+	uint32  PAD[133];
+	uint32  sr_control;     
+	uint32  sr_status;      
+	uint32  sr_address;     
+	uint32  sr_data;        
+} sbsocramregs_t;
+
+#endif	
+
+
+#define	SR_COREINFO		0x00
+#define	SR_BWALLOC		0x04
+#define	SR_BISTSTAT		0x0c
+#define	SR_BANKINDEX		0x10
+#define	SR_BANKSTBYCTL		0x14
+#define SR_PWRCTL		0x1e8
+
+
+#define	SRCI_PT_MASK		0x00070000	
+#define	SRCI_PT_SHIFT		16
+
+#define SRCI_PT_OCP_OCP		0
+#define SRCI_PT_AXI_OCP		1
+#define SRCI_PT_ARM7AHB_OCP	2
+#define SRCI_PT_CM3AHB_OCP	3
+#define SRCI_PT_AXI_AXI		4
+#define SRCI_PT_AHB_AXI		5
+
+#define SRCI_LSS_MASK		0x00f00000
+#define SRCI_LSS_SHIFT		20
+#define SRCI_LRS_MASK		0x0f000000
+#define SRCI_LRS_SHIFT		24
+
+
+#define	SRCI_MS0_MASK		0xf
+#define SR_MS0_BASE		16
+
+
+#define	SRCI_ROMNB_MASK		0xf000
+#define	SRCI_ROMNB_SHIFT	12
+#define	SRCI_ROMBSZ_MASK	0xf00
+#define	SRCI_ROMBSZ_SHIFT	8
+#define	SRCI_SRNB_MASK		0xf0
+#define	SRCI_SRNB_SHIFT		4
+#define	SRCI_SRBSZ_MASK		0xf
+#define	SRCI_SRBSZ_SHIFT	0
+
+#define SR_BSZ_BASE		14
+
+
+#define	SRSC_SBYOVR_MASK	0x80000000
+#define	SRSC_SBYOVR_SHIFT	31
+#define	SRSC_SBYOVRVAL_MASK	0x60000000
+#define	SRSC_SBYOVRVAL_SHIFT	29
+#define	SRSC_SBYEN_MASK		0x01000000	
+#define	SRSC_SBYEN_SHIFT	24
+
+
+#define SRPC_PMU_STBYDIS_MASK	0x00000010	
+#define SRPC_PMU_STBYDIS_SHIFT	4
+#define SRPC_STBYOVRVAL_MASK	0x00000008
+#define SRPC_STBYOVRVAL_SHIFT	3
+#define SRPC_STBYOVR_MASK	0x00000007
+#define SRPC_STBYOVR_SHIFT	0
+
+
+#define SRECC_NUM_BANKS_MASK   0x000000F0
+#define SRECC_NUM_BANKS_SHIFT  4
+#define SRECC_BANKSIZE_MASK    0x0000000F
+#define SRECC_BANKSIZE_SHIFT   0
+
+#define SRECC_BANKSIZE(value)	 (1 << (value))
+
+
+#define SRCBPC_PATCHENABLE 0x80000000
+
+#define SRP_ADDRESS   0x0001FFFC
+#define SRP_VALID     0x8000
+
+
+#define SRCMD_WRITE  0x00020000
+#define SRCMD_READ   0x00010000
+#define SRCMD_DONE   0x80000000
+
+#define SRCMD_DONE_DLY	1000
+
+
+#define SOCRAM_BANKINFO_SZMASK		0x3f
+#define SOCRAM_BANKIDX_ROM_MASK		0x100
+
+#define SOCRAM_BANKIDX_MEMTYPE_SHIFT	8
+
+#define SOCRAM_MEMTYPE_RAM		0
+#define SOCRAM_MEMTYPE_R0M		1
+#define SOCRAM_MEMTYPE_DEVRAM		2
+
+#define	SOCRAM_BANKINFO_REG		0x40
+#define	SOCRAM_BANKIDX_REG		0x10
+#define	SOCRAM_BANKINFO_STDBY_MASK	0x400
+#define	SOCRAM_BANKINFO_STDBY_TIMER	0x800
+
+
+#define SOCRAM_BANKINFO_DEVRAMSEL_SHIFT	13
+#define SOCRAM_BANKINFO_DEVRAMSEL_MASK	0x2000
+#define SOCRAM_BANKINFO_DEVRAMPRO_SHIFT	14
+#define SOCRAM_BANKINFO_DEVRAMPRO_MASK	0x4000
+
+
+#define SOCRAM_DEVRAMBANK_MASK		0xF000
+#define SOCRAM_DEVRAMBANK_SHIFT		12
+
+
+#define	SOCRAM_BANKINFO_SZBASE		8192
+#define SOCRAM_BANKSIZE_SHIFT		13	
+
+
+#endif	
diff --git a/drivers/net/wireless/bcmdhd/include/sdio.h b/drivers/net/wireless/bcmdhd/include/sdio.h
new file mode 100644
index 000000000000..c8ac7b773fb9
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/sdio.h
@@ -0,0 +1,612 @@
+/*
+ * SDIO spec header file
+ * Protocol and standard (common) device definitions
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sdio.h 277737 2011-08-16 17:54:59Z $
+ */
+
+#ifndef	_SDIO_H
+#define	_SDIO_H
+
+
+/* CCCR structure for function 0 */
+typedef volatile struct {
+	uint8	cccr_sdio_rev;		/* RO, cccr and sdio revision */
+	uint8	sd_rev;			/* RO, sd spec revision */
+	uint8	io_en;			/* I/O enable */
+	uint8	io_rdy;			/* I/O ready reg */
+	uint8	intr_ctl;		/* Master and per function interrupt enable control */
+	uint8	intr_status;		/* RO, interrupt pending status */
+	uint8	io_abort;		/* read/write abort or reset all functions */
+	uint8	bus_inter;		/* bus interface control */
+	uint8	capability;		/* RO, card capability */
+
+	uint8	cis_base_low;		/* 0x9 RO, common CIS base address, LSB */
+	uint8	cis_base_mid;
+	uint8	cis_base_high;		/* 0xB RO, common CIS base address, MSB */
+
+	/* suspend/resume registers */
+	uint8	bus_suspend;		/* 0xC */
+	uint8	func_select;		/* 0xD */
+	uint8	exec_flag;		/* 0xE */
+	uint8	ready_flag;		/* 0xF */
+
+	uint8	fn0_blk_size[2];	/* 0x10(LSB), 0x11(MSB) */
+
+	uint8	power_control;		/* 0x12 (SDIO version 1.10) */
+
+	uint8	speed_control;		/* 0x13 */
+} sdio_regs_t;
+
+/* SDIO Device CCCR offsets */
+#define SDIOD_CCCR_REV			0x00
+#define SDIOD_CCCR_SDREV		0x01
+#define SDIOD_CCCR_IOEN			0x02
+#define SDIOD_CCCR_IORDY		0x03
+#define SDIOD_CCCR_INTEN		0x04
+#define SDIOD_CCCR_INTPEND		0x05
+#define SDIOD_CCCR_IOABORT		0x06
+#define SDIOD_CCCR_BICTRL		0x07
+#define SDIOD_CCCR_CAPABLITIES		0x08
+#define SDIOD_CCCR_CISPTR_0		0x09
+#define SDIOD_CCCR_CISPTR_1		0x0A
+#define SDIOD_CCCR_CISPTR_2		0x0B
+#define SDIOD_CCCR_BUSSUSP		0x0C
+#define SDIOD_CCCR_FUNCSEL		0x0D
+#define SDIOD_CCCR_EXECFLAGS		0x0E
+#define SDIOD_CCCR_RDYFLAGS		0x0F
+#define SDIOD_CCCR_BLKSIZE_0		0x10
+#define SDIOD_CCCR_BLKSIZE_1		0x11
+#define SDIOD_CCCR_POWER_CONTROL	0x12
+#define SDIOD_CCCR_SPEED_CONTROL	0x13
+#define SDIOD_CCCR_UHSI_SUPPORT		0x14
+#define SDIOD_CCCR_DRIVER_STRENGTH	0x15
+#define SDIOD_CCCR_INTR_EXTN		0x16
+
+/* Broadcom extensions (corerev >= 1) */
+#define SDIOD_CCCR_BRCM_SEPINT		0xf2
+
+/* cccr_sdio_rev */
+#define SDIO_REV_SDIOID_MASK	0xf0	/* SDIO spec revision number */
+#define SDIO_REV_CCCRID_MASK	0x0f	/* CCCR format version number */
+
+/* sd_rev */
+#define SD_REV_PHY_MASK		0x0f	/* SD format version number */
+
+/* io_en */
+#define SDIO_FUNC_ENABLE_1	0x02	/* function 1 I/O enable */
+#define SDIO_FUNC_ENABLE_2	0x04	/* function 2 I/O enable */
+
+/* io_rdys */
+#define SDIO_FUNC_READY_1	0x02	/* function 1 I/O ready */
+#define SDIO_FUNC_READY_2	0x04	/* function 2 I/O ready */
+
+/* intr_ctl */
+#define INTR_CTL_MASTER_EN	0x1	/* interrupt enable master */
+#define INTR_CTL_FUNC1_EN	0x2	/* interrupt enable for function 1 */
+#define INTR_CTL_FUNC2_EN	0x4	/* interrupt enable for function 2 */
+
+/* intr_status */
+#define INTR_STATUS_FUNC1	0x2	/* interrupt pending for function 1 */
+#define INTR_STATUS_FUNC2	0x4	/* interrupt pending for function 2 */
+
+/* io_abort */
+#define IO_ABORT_RESET_ALL	0x08	/* I/O card reset */
+#define IO_ABORT_FUNC_MASK	0x07	/* abort selction: function x */
+
+/* bus_inter */
+#define BUS_CARD_DETECT_DIS	0x80	/* Card Detect disable */
+#define BUS_SPI_CONT_INTR_CAP	0x40	/* support continuous SPI interrupt */
+#define BUS_SPI_CONT_INTR_EN	0x20	/* continuous SPI interrupt enable */
+#define BUS_SD_DATA_WIDTH_MASK	0x03	/* bus width mask */
+#define BUS_SD_DATA_WIDTH_4BIT	0x02	/* bus width 4-bit mode */
+#define BUS_SD_DATA_WIDTH_1BIT	0x00	/* bus width 1-bit mode */
+
+/* capability */
+#define SDIO_CAP_4BLS		0x80	/* 4-bit support for low speed card */
+#define SDIO_CAP_LSC		0x40	/* low speed card */
+#define SDIO_CAP_E4MI		0x20	/* enable interrupt between block of data in 4-bit mode */
+#define SDIO_CAP_S4MI		0x10	/* support interrupt between block of data in 4-bit mode */
+#define SDIO_CAP_SBS		0x08	/* support suspend/resume */
+#define SDIO_CAP_SRW		0x04	/* support read wait */
+#define SDIO_CAP_SMB		0x02	/* support multi-block transfer */
+#define SDIO_CAP_SDC		0x01	/* Support Direct commands during multi-byte transfer */
+
+/* power_control */
+#define SDIO_POWER_SMPC		0x01	/* supports master power control (RO) */
+#define SDIO_POWER_EMPC		0x02	/* enable master power control (allow > 200mA) (RW) */
+
+/* speed_control (control device entry into high-speed clocking mode) */
+#define SDIO_SPEED_SHS		0x01	/* supports high-speed [clocking] mode (RO) */
+#define SDIO_SPEED_EHS		0x02	/* enable high-speed [clocking] mode (RW) */
+
+/* for setting bus speed in card: 0x13h */
+#define SDIO_BUS_SPEED_UHSISEL_M	BITFIELD_MASK(3)
+#define SDIO_BUS_SPEED_UHSISEL_S	1
+
+/* for getting bus speed cap in card: 0x14h */
+#define SDIO_BUS_SPEED_UHSICAP_M	BITFIELD_MASK(3)
+#define SDIO_BUS_SPEED_UHSICAP_S	0
+
+/* for getting driver type CAP in card: 0x15h */
+#define SDIO_BUS_DRVR_TYPE_CAP_M	BITFIELD_MASK(3)
+#define SDIO_BUS_DRVR_TYPE_CAP_S	0
+
+/* for setting driver type selection in card: 0x15h */
+#define SDIO_BUS_DRVR_TYPE_SEL_M	BITFIELD_MASK(2)
+#define SDIO_BUS_DRVR_TYPE_SEL_S	4
+
+/* for getting async int support in card: 0x16h */
+#define SDIO_BUS_ASYNCINT_CAP_M	BITFIELD_MASK(1)
+#define SDIO_BUS_ASYNCINT_CAP_S	0
+
+/* for setting async int selection in card: 0x16h */
+#define SDIO_BUS_ASYNCINT_SEL_M	BITFIELD_MASK(1)
+#define SDIO_BUS_ASYNCINT_SEL_S	1
+
+/* brcm sepint */
+#define SDIO_SEPINT_MASK	0x01	/* route sdpcmdev intr onto separate pad (chip-specific) */
+#define SDIO_SEPINT_OE		0x02	/* 1 asserts output enable for above pad */
+#define SDIO_SEPINT_ACT_HI	0x04	/* use active high interrupt level instead of active low */
+
+/* FBR structure for function 1-7, FBR addresses and register offsets */
+typedef volatile struct {
+	uint8	devctr;			/* device interface, CSA control */
+	uint8	ext_dev;		/* extended standard I/O device type code */
+	uint8	pwr_sel;		/* power selection support */
+	uint8	PAD[6];			/* reserved */
+
+	uint8	cis_low;		/* CIS LSB */
+	uint8	cis_mid;
+	uint8	cis_high;		/* CIS MSB */
+	uint8	csa_low;		/* code storage area, LSB */
+	uint8	csa_mid;
+	uint8	csa_high;		/* code storage area, MSB */
+	uint8	csa_dat_win;		/* data access window to function */
+
+	uint8	fnx_blk_size[2];	/* block size, little endian */
+} sdio_fbr_t;
+
+/* Maximum number of I/O funcs */
+#define SDIOD_MAX_IOFUNCS		7
+
+/* SDIO Device FBR Start Address  */
+#define SDIOD_FBR_STARTADDR		0x100
+
+/* SDIO Device FBR Size */
+#define SDIOD_FBR_SIZE			0x100
+
+/* Macro to calculate FBR register base */
+#define SDIOD_FBR_BASE(n)		((n) * 0x100)
+
+/* Function register offsets */
+#define SDIOD_FBR_DEVCTR		0x00	/* basic info for function */
+#define SDIOD_FBR_EXT_DEV		0x01	/* extended I/O device code */
+#define SDIOD_FBR_PWR_SEL		0x02	/* power selection bits */
+
+/* SDIO Function CIS ptr offset */
+#define SDIOD_FBR_CISPTR_0		0x09
+#define SDIOD_FBR_CISPTR_1		0x0A
+#define SDIOD_FBR_CISPTR_2		0x0B
+
+/* Code Storage Area pointer */
+#define SDIOD_FBR_CSA_ADDR_0		0x0C
+#define SDIOD_FBR_CSA_ADDR_1		0x0D
+#define SDIOD_FBR_CSA_ADDR_2		0x0E
+#define SDIOD_FBR_CSA_DATA		0x0F
+
+/* SDIO Function I/O Block Size */
+#define SDIOD_FBR_BLKSIZE_0		0x10
+#define SDIOD_FBR_BLKSIZE_1		0x11
+
+/* devctr */
+#define SDIOD_FBR_DEVCTR_DIC	0x0f	/* device interface code */
+#define SDIOD_FBR_DECVTR_CSA	0x40	/* CSA support flag */
+#define SDIOD_FBR_DEVCTR_CSA_EN	0x80	/* CSA enabled */
+/* interface codes */
+#define SDIOD_DIC_NONE		0	/* SDIO standard interface is not supported */
+#define SDIOD_DIC_UART		1
+#define SDIOD_DIC_BLUETOOTH_A	2
+#define SDIOD_DIC_BLUETOOTH_B	3
+#define SDIOD_DIC_GPS		4
+#define SDIOD_DIC_CAMERA	5
+#define SDIOD_DIC_PHS		6
+#define SDIOD_DIC_WLAN		7
+#define SDIOD_DIC_EXT		0xf	/* extended device interface, read ext_dev register */
+
+/* pwr_sel */
+#define SDIOD_PWR_SEL_SPS	0x01	/* supports power selection */
+#define SDIOD_PWR_SEL_EPS	0x02	/* enable power selection (low-current mode) */
+
+/* misc defines */
+#define SDIO_FUNC_0		0
+#define SDIO_FUNC_1		1
+#define SDIO_FUNC_2		2
+#define SDIO_FUNC_3		3
+#define SDIO_FUNC_4		4
+#define SDIO_FUNC_5		5
+#define SDIO_FUNC_6		6
+#define SDIO_FUNC_7		7
+
+#define SD_CARD_TYPE_UNKNOWN	0	/* bad type or unrecognized */
+#define SD_CARD_TYPE_IO		1	/* IO only card */
+#define SD_CARD_TYPE_MEMORY	2	/* memory only card */
+#define SD_CARD_TYPE_COMBO	3	/* IO and memory combo card */
+
+#define SDIO_MAX_BLOCK_SIZE	2048	/* maximum block size for block mode operation */
+#define SDIO_MIN_BLOCK_SIZE	1	/* minimum block size for block mode operation */
+
+/* Card registers: status bit position */
+#define CARDREG_STATUS_BIT_OUTOFRANGE		31
+#define CARDREG_STATUS_BIT_COMCRCERROR		23
+#define CARDREG_STATUS_BIT_ILLEGALCOMMAND	22
+#define CARDREG_STATUS_BIT_ERROR		19
+#define CARDREG_STATUS_BIT_IOCURRENTSTATE3	12
+#define CARDREG_STATUS_BIT_IOCURRENTSTATE2	11
+#define CARDREG_STATUS_BIT_IOCURRENTSTATE1	10
+#define CARDREG_STATUS_BIT_IOCURRENTSTATE0	9
+#define CARDREG_STATUS_BIT_FUN_NUM_ERROR	4
+
+
+
+#define SD_CMD_GO_IDLE_STATE		0	/* mandatory for SDIO */
+#define SD_CMD_SEND_OPCOND		1
+#define SD_CMD_MMC_SET_RCA		3
+#define SD_CMD_IO_SEND_OP_COND		5	/* mandatory for SDIO */
+#define SD_CMD_SELECT_DESELECT_CARD	7
+#define SD_CMD_SEND_CSD			9
+#define SD_CMD_SEND_CID			10
+#define SD_CMD_STOP_TRANSMISSION	12
+#define SD_CMD_SEND_STATUS		13
+#define SD_CMD_GO_INACTIVE_STATE	15
+#define SD_CMD_SET_BLOCKLEN		16
+#define SD_CMD_READ_SINGLE_BLOCK	17
+#define SD_CMD_READ_MULTIPLE_BLOCK	18
+#define SD_CMD_WRITE_BLOCK		24
+#define SD_CMD_WRITE_MULTIPLE_BLOCK	25
+#define SD_CMD_PROGRAM_CSD		27
+#define SD_CMD_SET_WRITE_PROT		28
+#define SD_CMD_CLR_WRITE_PROT		29
+#define SD_CMD_SEND_WRITE_PROT		30
+#define SD_CMD_ERASE_WR_BLK_START	32
+#define SD_CMD_ERASE_WR_BLK_END		33
+#define SD_CMD_ERASE			38
+#define SD_CMD_LOCK_UNLOCK		42
+#define SD_CMD_IO_RW_DIRECT		52	/* mandatory for SDIO */
+#define SD_CMD_IO_RW_EXTENDED		53	/* mandatory for SDIO */
+#define SD_CMD_APP_CMD			55
+#define SD_CMD_GEN_CMD			56
+#define SD_CMD_READ_OCR			58
+#define SD_CMD_CRC_ON_OFF		59	/* mandatory for SDIO */
+#define SD_ACMD_SD_STATUS		13
+#define SD_ACMD_SEND_NUM_WR_BLOCKS	22
+#define SD_ACMD_SET_WR_BLOCK_ERASE_CNT	23
+#define SD_ACMD_SD_SEND_OP_COND		41
+#define SD_ACMD_SET_CLR_CARD_DETECT	42
+#define SD_ACMD_SEND_SCR		51
+
+/* argument for SD_CMD_IO_RW_DIRECT and SD_CMD_IO_RW_EXTENDED */
+#define SD_IO_OP_READ		0   /* Read_Write: Read */
+#define SD_IO_OP_WRITE		1   /* Read_Write: Write */
+#define SD_IO_RW_NORMAL		0   /* no RAW */
+#define SD_IO_RW_RAW		1   /* RAW */
+#define SD_IO_BYTE_MODE		0   /* Byte Mode */
+#define SD_IO_BLOCK_MODE	1   /* BlockMode */
+#define SD_IO_FIXED_ADDRESS	0   /* fix Address */
+#define SD_IO_INCREMENT_ADDRESS	1   /* IncrementAddress */
+
+/* build SD_CMD_IO_RW_DIRECT Argument */
+#define SDIO_IO_RW_DIRECT_ARG(rw, raw, func, addr, data) \
+	((((rw) & 1) << 31) | (((func) & 0x7) << 28) | (((raw) & 1) << 27) | \
+	 (((addr) & 0x1FFFF) << 9) | ((data) & 0xFF))
+
+/* build SD_CMD_IO_RW_EXTENDED Argument */
+#define SDIO_IO_RW_EXTENDED_ARG(rw, blk, func, addr, inc_addr, count) \
+	((((rw) & 1) << 31) | (((func) & 0x7) << 28) | (((blk) & 1) << 27) | \
+	 (((inc_addr) & 1) << 26) | (((addr) & 0x1FFFF) << 9) | ((count) & 0x1FF))
+
+/* SDIO response parameters */
+#define SD_RSP_NO_NONE			0
+#define SD_RSP_NO_1			1
+#define SD_RSP_NO_2			2
+#define SD_RSP_NO_3			3
+#define SD_RSP_NO_4			4
+#define SD_RSP_NO_5			5
+#define SD_RSP_NO_6			6
+
+	/* Modified R6 response (to CMD3) */
+#define SD_RSP_MR6_COM_CRC_ERROR	0x8000
+#define SD_RSP_MR6_ILLEGAL_COMMAND	0x4000
+#define SD_RSP_MR6_ERROR		0x2000
+
+	/* Modified R1 in R4 Response (to CMD5) */
+#define SD_RSP_MR1_SBIT			0x80
+#define SD_RSP_MR1_PARAMETER_ERROR	0x40
+#define SD_RSP_MR1_RFU5			0x20
+#define SD_RSP_MR1_FUNC_NUM_ERROR	0x10
+#define SD_RSP_MR1_COM_CRC_ERROR	0x08
+#define SD_RSP_MR1_ILLEGAL_COMMAND	0x04
+#define SD_RSP_MR1_RFU1			0x02
+#define SD_RSP_MR1_IDLE_STATE		0x01
+
+	/* R5 response (to CMD52 and CMD53) */
+#define SD_RSP_R5_COM_CRC_ERROR		0x80
+#define SD_RSP_R5_ILLEGAL_COMMAND	0x40
+#define SD_RSP_R5_IO_CURRENTSTATE1	0x20
+#define SD_RSP_R5_IO_CURRENTSTATE0	0x10
+#define SD_RSP_R5_ERROR			0x08
+#define SD_RSP_R5_RFU			0x04
+#define SD_RSP_R5_FUNC_NUM_ERROR	0x02
+#define SD_RSP_R5_OUT_OF_RANGE		0x01
+
+#define SD_RSP_R5_ERRBITS		0xCB
+
+
+/* ------------------------------------------------
+ *  SDIO Commands and responses
+ *
+ *  I/O only commands are:
+ *      CMD0, CMD3, CMD5, CMD7, CMD14, CMD15, CMD52, CMD53
+ * ------------------------------------------------
+ */
+
+/* SDIO Commands */
+#define SDIOH_CMD_0		0
+#define SDIOH_CMD_3		3
+#define SDIOH_CMD_5		5
+#define SDIOH_CMD_7		7
+#define SDIOH_CMD_11		11
+#define SDIOH_CMD_14		14
+#define SDIOH_CMD_15		15
+#define SDIOH_CMD_19		19
+#define SDIOH_CMD_52		52
+#define SDIOH_CMD_53		53
+#define SDIOH_CMD_59		59
+
+/* SDIO Command Responses */
+#define SDIOH_RSP_NONE		0
+#define SDIOH_RSP_R1		1
+#define SDIOH_RSP_R2		2
+#define SDIOH_RSP_R3		3
+#define SDIOH_RSP_R4		4
+#define SDIOH_RSP_R5		5
+#define SDIOH_RSP_R6		6
+
+/*
+ *  SDIO Response Error flags
+ */
+#define SDIOH_RSP5_ERROR_FLAGS	0xCB
+
+/* ------------------------------------------------
+ * SDIO Command structures. I/O only commands are:
+ *
+ * 	CMD0, CMD3, CMD5, CMD7, CMD15, CMD52, CMD53
+ * ------------------------------------------------
+ */
+
+#define CMD5_OCR_M		BITFIELD_MASK(24)
+#define CMD5_OCR_S		0
+
+#define CMD5_S18R_M		BITFIELD_MASK(1)
+#define CMD5_S18R_S		24
+
+#define CMD7_RCA_M		BITFIELD_MASK(16)
+#define CMD7_RCA_S		16
+
+#define CMD14_RCA_M		BITFIELD_MASK(16)
+#define CMD14_RCA_S		16
+#define CMD14_SLEEP_M		BITFIELD_MASK(1)
+#define CMD14_SLEEP_S		15
+
+#define CMD_15_RCA_M		BITFIELD_MASK(16)
+#define CMD_15_RCA_S		16
+
+#define CMD52_DATA_M		BITFIELD_MASK(8)  /* Bits [7:0]    - Write Data/Stuff bits of CMD52
+						   */
+#define CMD52_DATA_S		0
+#define CMD52_REG_ADDR_M	BITFIELD_MASK(17) /* Bits [25:9]   - register address */
+#define CMD52_REG_ADDR_S	9
+#define CMD52_RAW_M		BITFIELD_MASK(1)  /* Bit  27       - Read after Write flag */
+#define CMD52_RAW_S		27
+#define CMD52_FUNCTION_M	BITFIELD_MASK(3)  /* Bits [30:28]  - Function number */
+#define CMD52_FUNCTION_S	28
+#define CMD52_RW_FLAG_M		BITFIELD_MASK(1)  /* Bit  31       - R/W flag */
+#define CMD52_RW_FLAG_S		31
+
+
+#define CMD53_BYTE_BLK_CNT_M	BITFIELD_MASK(9) /* Bits [8:0]     - Byte/Block Count of CMD53 */
+#define CMD53_BYTE_BLK_CNT_S	0
+#define CMD53_REG_ADDR_M	BITFIELD_MASK(17) /* Bits [25:9]   - register address */
+#define CMD53_REG_ADDR_S	9
+#define CMD53_OP_CODE_M		BITFIELD_MASK(1)  /* Bit  26       - R/W Operation Code */
+#define CMD53_OP_CODE_S		26
+#define CMD53_BLK_MODE_M	BITFIELD_MASK(1)  /* Bit  27       - Block Mode */
+#define CMD53_BLK_MODE_S	27
+#define CMD53_FUNCTION_M	BITFIELD_MASK(3)  /* Bits [30:28]  - Function number */
+#define CMD53_FUNCTION_S	28
+#define CMD53_RW_FLAG_M		BITFIELD_MASK(1)  /* Bit  31       - R/W flag */
+#define CMD53_RW_FLAG_S		31
+
+/* ------------------------------------------------------
+ * SDIO Command Response structures for SD1 and SD4 modes
+ *  -----------------------------------------------------
+ */
+#define RSP4_IO_OCR_M		BITFIELD_MASK(24) /* Bits [23:0]  - Card's OCR Bits [23:0] */
+#define RSP4_IO_OCR_S		0
+
+#define RSP4_S18A_M			BITFIELD_MASK(1) /* Bits [23:0]  - Card's OCR Bits [23:0] */
+#define RSP4_S18A_S			24
+
+#define RSP4_STUFF_M		BITFIELD_MASK(3)  /* Bits [26:24] - Stuff bits */
+#define RSP4_STUFF_S		24
+#define RSP4_MEM_PRESENT_M	BITFIELD_MASK(1)  /* Bit  27      - Memory present */
+#define RSP4_MEM_PRESENT_S	27
+#define RSP4_NUM_FUNCS_M	BITFIELD_MASK(3)  /* Bits [30:28] - Number of I/O funcs */
+#define RSP4_NUM_FUNCS_S	28
+#define RSP4_CARD_READY_M	BITFIELD_MASK(1)  /* Bit  31      - SDIO card ready */
+#define RSP4_CARD_READY_S	31
+
+#define RSP6_STATUS_M		BITFIELD_MASK(16) /* Bits [15:0]  - Card status bits [19,22,23,12:0]
+						   */
+#define RSP6_STATUS_S		0
+#define RSP6_IO_RCA_M		BITFIELD_MASK(16) /* Bits [31:16] - RCA bits[31-16] */
+#define RSP6_IO_RCA_S		16
+
+#define RSP1_AKE_SEQ_ERROR_M	BITFIELD_MASK(1)  /* Bit 3       - Authentication seq error */
+#define RSP1_AKE_SEQ_ERROR_S	3
+#define RSP1_APP_CMD_M		BITFIELD_MASK(1)  /* Bit 5       - Card expects ACMD */
+#define RSP1_APP_CMD_S		5
+#define RSP1_READY_FOR_DATA_M	BITFIELD_MASK(1)  /* Bit 8       - Ready for data (buff empty) */
+#define RSP1_READY_FOR_DATA_S	8
+#define RSP1_CURR_STATE_M	BITFIELD_MASK(4)  /* Bits [12:9] - State of card
+						   * when Cmd was received
+						   */
+#define RSP1_CURR_STATE_S	9
+#define RSP1_EARSE_RESET_M	BITFIELD_MASK(1)  /* Bit 13   - Erase seq cleared */
+#define RSP1_EARSE_RESET_S	13
+#define RSP1_CARD_ECC_DISABLE_M	BITFIELD_MASK(1)  /* Bit 14   - Card ECC disabled */
+#define RSP1_CARD_ECC_DISABLE_S	14
+#define RSP1_WP_ERASE_SKIP_M	BITFIELD_MASK(1)  /* Bit 15   - Partial blocks erased due to W/P */
+#define RSP1_WP_ERASE_SKIP_S	15
+#define RSP1_CID_CSD_OVERW_M	BITFIELD_MASK(1)  /* Bit 16   - Illegal write to CID or R/O bits
+						   * of CSD
+						   */
+#define RSP1_CID_CSD_OVERW_S	16
+#define RSP1_ERROR_M		BITFIELD_MASK(1)  /* Bit 19   - General/Unknown error */
+#define RSP1_ERROR_S		19
+#define RSP1_CC_ERROR_M		BITFIELD_MASK(1)  /* Bit 20   - Internal Card Control error */
+#define RSP1_CC_ERROR_S		20
+#define RSP1_CARD_ECC_FAILED_M	BITFIELD_MASK(1)  /* Bit 21   - Card internal ECC failed
+						   * to correct data
+						   */
+#define RSP1_CARD_ECC_FAILED_S	21
+#define RSP1_ILLEGAL_CMD_M	BITFIELD_MASK(1)  /* Bit 22   - Cmd not legal for the card state */
+#define RSP1_ILLEGAL_CMD_S	22
+#define RSP1_COM_CRC_ERROR_M	BITFIELD_MASK(1)  /* Bit 23   - CRC check of previous command failed
+						   */
+#define RSP1_COM_CRC_ERROR_S	23
+#define RSP1_LOCK_UNLOCK_FAIL_M	BITFIELD_MASK(1)  /* Bit 24   - Card lock-unlock Cmd Seq error */
+#define RSP1_LOCK_UNLOCK_FAIL_S	24
+#define RSP1_CARD_LOCKED_M	BITFIELD_MASK(1)  /* Bit 25   - Card locked by the host */
+#define RSP1_CARD_LOCKED_S	25
+#define RSP1_WP_VIOLATION_M	BITFIELD_MASK(1)  /* Bit 26   - Attempt to program
+						   * write-protected blocks
+						   */
+#define RSP1_WP_VIOLATION_S	26
+#define RSP1_ERASE_PARAM_M	BITFIELD_MASK(1)  /* Bit 27   - Invalid erase blocks */
+#define RSP1_ERASE_PARAM_S	27
+#define RSP1_ERASE_SEQ_ERR_M	BITFIELD_MASK(1)  /* Bit 28   - Erase Cmd seq error */
+#define RSP1_ERASE_SEQ_ERR_S	28
+#define RSP1_BLK_LEN_ERR_M	BITFIELD_MASK(1)  /* Bit 29   - Block length error */
+#define RSP1_BLK_LEN_ERR_S	29
+#define RSP1_ADDR_ERR_M		BITFIELD_MASK(1)  /* Bit 30   - Misaligned address */
+#define RSP1_ADDR_ERR_S		30
+#define RSP1_OUT_OF_RANGE_M	BITFIELD_MASK(1)  /* Bit 31   - Cmd arg was out of range */
+#define RSP1_OUT_OF_RANGE_S	31
+
+
+#define RSP5_DATA_M		BITFIELD_MASK(8)  /* Bits [0:7]   - data */
+#define RSP5_DATA_S		0
+#define RSP5_FLAGS_M		BITFIELD_MASK(8)  /* Bit  [15:8]  - Rsp flags */
+#define RSP5_FLAGS_S		8
+#define RSP5_STUFF_M		BITFIELD_MASK(16) /* Bits [31:16] - Stuff bits */
+#define RSP5_STUFF_S		16
+
+/* ----------------------------------------------
+ * SDIO Command Response structures for SPI mode
+ * ----------------------------------------------
+ */
+#define SPIRSP4_IO_OCR_M	BITFIELD_MASK(16) /* Bits [15:0]    - Card's OCR Bits [23:8] */
+#define SPIRSP4_IO_OCR_S	0
+#define SPIRSP4_STUFF_M		BITFIELD_MASK(3)  /* Bits [18:16]   - Stuff bits */
+#define SPIRSP4_STUFF_S		16
+#define SPIRSP4_MEM_PRESENT_M	BITFIELD_MASK(1)  /* Bit  19        - Memory present */
+#define SPIRSP4_MEM_PRESENT_S	19
+#define SPIRSP4_NUM_FUNCS_M	BITFIELD_MASK(3)  /* Bits [22:20]   - Number of I/O funcs */
+#define SPIRSP4_NUM_FUNCS_S	20
+#define SPIRSP4_CARD_READY_M	BITFIELD_MASK(1)  /* Bit  23        - SDIO card ready */
+#define SPIRSP4_CARD_READY_S	23
+#define SPIRSP4_IDLE_STATE_M	BITFIELD_MASK(1)  /* Bit  24        - idle state */
+#define SPIRSP4_IDLE_STATE_S	24
+#define SPIRSP4_ILLEGAL_CMD_M	BITFIELD_MASK(1)  /* Bit  26        - Illegal Cmd error */
+#define SPIRSP4_ILLEGAL_CMD_S	26
+#define SPIRSP4_COM_CRC_ERROR_M	BITFIELD_MASK(1)  /* Bit  27        - COM CRC error */
+#define SPIRSP4_COM_CRC_ERROR_S	27
+#define SPIRSP4_FUNC_NUM_ERROR_M	BITFIELD_MASK(1)  /* Bit  28        - Function number error
+							   */
+#define SPIRSP4_FUNC_NUM_ERROR_S	28
+#define SPIRSP4_PARAM_ERROR_M	BITFIELD_MASK(1)  /* Bit  30        - Parameter Error Bit */
+#define SPIRSP4_PARAM_ERROR_S	30
+#define SPIRSP4_START_BIT_M	BITFIELD_MASK(1)  /* Bit  31        - Start Bit */
+#define SPIRSP4_START_BIT_S	31
+
+#define SPIRSP5_DATA_M			BITFIELD_MASK(8)  /* Bits [23:16]   - R/W Data */
+#define SPIRSP5_DATA_S			16
+#define SPIRSP5_IDLE_STATE_M		BITFIELD_MASK(1)  /* Bit  24        - Idle state */
+#define SPIRSP5_IDLE_STATE_S		24
+#define SPIRSP5_ILLEGAL_CMD_M		BITFIELD_MASK(1)  /* Bit  26        - Illegal Cmd error */
+#define SPIRSP5_ILLEGAL_CMD_S		26
+#define SPIRSP5_COM_CRC_ERROR_M		BITFIELD_MASK(1)  /* Bit  27        - COM CRC error */
+#define SPIRSP5_COM_CRC_ERROR_S		27
+#define SPIRSP5_FUNC_NUM_ERROR_M	BITFIELD_MASK(1)  /* Bit  28        - Function number error
+							   */
+#define SPIRSP5_FUNC_NUM_ERROR_S	28
+#define SPIRSP5_PARAM_ERROR_M		BITFIELD_MASK(1)  /* Bit  30        - Parameter Error Bit */
+#define SPIRSP5_PARAM_ERROR_S		30
+#define SPIRSP5_START_BIT_M		BITFIELD_MASK(1)  /* Bit  31        - Start Bit */
+#define SPIRSP5_START_BIT_S		31
+
+/* RSP6 card status format; Pg 68 Physical Layer spec v 1.10 */
+#define RSP6STAT_AKE_SEQ_ERROR_M	BITFIELD_MASK(1)  /* Bit 3	- Authentication seq error
+							   */
+#define RSP6STAT_AKE_SEQ_ERROR_S	3
+#define RSP6STAT_APP_CMD_M		BITFIELD_MASK(1)  /* Bit 5	- Card expects ACMD */
+#define RSP6STAT_APP_CMD_S		5
+#define RSP6STAT_READY_FOR_DATA_M	BITFIELD_MASK(1)  /* Bit 8	- Ready for data
+							   * (buff empty)
+							   */
+#define RSP6STAT_READY_FOR_DATA_S	8
+#define RSP6STAT_CURR_STATE_M		BITFIELD_MASK(4)  /* Bits [12:9] - Card state at
+							   * Cmd reception
+							   */
+#define RSP6STAT_CURR_STATE_S		9
+#define RSP6STAT_ERROR_M		BITFIELD_MASK(1)  /* Bit 13  - General/Unknown error Bit 19
+							   */
+#define RSP6STAT_ERROR_S		13
+#define RSP6STAT_ILLEGAL_CMD_M		BITFIELD_MASK(1)  /* Bit 14  - Illegal cmd for
+							   * card state Bit 22
+							   */
+#define RSP6STAT_ILLEGAL_CMD_S		14
+#define RSP6STAT_COM_CRC_ERROR_M	BITFIELD_MASK(1)  /* Bit 15  - CRC previous command
+							   * failed Bit 23
+							   */
+#define RSP6STAT_COM_CRC_ERROR_S	15
+
+#define SDIOH_XFER_TYPE_READ    SD_IO_OP_READ
+#define SDIOH_XFER_TYPE_WRITE   SD_IO_OP_WRITE
+
+/* command issue options */
+#define CMD_OPTION_DEFAULT	0
+#define CMD_OPTION_TUNING	1
+
+#endif /* _SDIO_H */
diff --git a/drivers/net/wireless/bcmdhd/include/sdioh.h b/drivers/net/wireless/bcmdhd/include/sdioh.h
new file mode 100644
index 000000000000..5f47d6f88cea
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/sdioh.h
@@ -0,0 +1,442 @@
+/*
+ * SDIO Host Controller Spec header file
+ * Register map and definitions for the Standard Host Controller
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sdioh.h 300017 2011-12-01 20:30:27Z $
+ */
+
+#ifndef	_SDIOH_H
+#define	_SDIOH_H
+
+#define SD_SysAddr			0x000
+#define SD_BlockSize			0x004
+#define SD_BlockCount 			0x006
+#define SD_Arg0				0x008
+#define SD_Arg1 			0x00A
+#define SD_TransferMode			0x00C
+#define SD_Command 			0x00E
+#define SD_Response0			0x010
+#define SD_Response1 			0x012
+#define SD_Response2			0x014
+#define SD_Response3 			0x016
+#define SD_Response4			0x018
+#define SD_Response5 			0x01A
+#define SD_Response6			0x01C
+#define SD_Response7 			0x01E
+#define SD_BufferDataPort0		0x020
+#define SD_BufferDataPort1 		0x022
+#define SD_PresentState			0x024
+#define SD_HostCntrl			0x028
+#define SD_PwrCntrl			0x029
+#define SD_BlockGapCntrl 		0x02A
+#define SD_WakeupCntrl 			0x02B
+#define SD_ClockCntrl			0x02C
+#define SD_TimeoutCntrl 		0x02E
+#define SD_SoftwareReset		0x02F
+#define SD_IntrStatus			0x030
+#define SD_ErrorIntrStatus 		0x032
+#define SD_IntrStatusEnable		0x034
+#define SD_ErrorIntrStatusEnable 	0x036
+#define SD_IntrSignalEnable		0x038
+#define SD_ErrorIntrSignalEnable 	0x03A
+#define SD_CMD12ErrorStatus		0x03C
+#define SD_Capabilities			0x040
+#define SD_Capabilities3		0x044
+#define SD_MaxCurCap			0x048
+#define SD_MaxCurCap_Reserved		0x04C
+#define SD_ADMA_ErrStatus		0x054
+#define SD_ADMA_SysAddr			0x58
+#define SD_SlotInterruptStatus		0x0FC
+#define SD_HostControllerVersion 	0x0FE
+#define	SD_GPIO_Reg			0x100
+#define	SD_GPIO_OE			0x104
+#define	SD_GPIO_Enable			0x108
+
+
+/* SD specific registers in PCI config space */
+#define SD_SlotInfo	0x40
+
+/* HC 3.0 specific registers and offsets */
+#define SD3_HostCntrl2			0x03E
+/* preset regsstart and count */
+#define SD3_PresetValStart		0x060
+#define SD3_PresetValCount		8
+/* preset-indiv regs */
+#define SD3_PresetVal_init		0x060
+#define SD3_PresetVal_default	0x062
+#define SD3_PresetVal_HS		0x064
+#define SD3_PresetVal_SDR12		0x066
+#define SD3_PresetVal_SDR25		0x068
+#define SD3_PresetVal_SDR50		0x06a
+#define SD3_PresetVal_SDR104	0x06c
+#define SD3_PresetVal_DDR50		0x06e
+
+/* preset value indices */
+#define SD3_PRESETVAL_INITIAL_IX	0
+#define SD3_PRESETVAL_DESPEED_IX	1
+#define SD3_PRESETVAL_HISPEED_IX	2
+#define SD3_PRESETVAL_SDR12_IX		3
+#define SD3_PRESETVAL_SDR25_IX		4
+#define SD3_PRESETVAL_SDR50_IX		5
+#define SD3_PRESETVAL_SDR104_IX		6
+#define SD3_PRESETVAL_DDR50_IX		7
+
+/* SD_Capabilities reg (0x040) */
+#define CAP_TO_CLKFREQ_M 	BITFIELD_MASK(6)
+#define CAP_TO_CLKFREQ_S 	0
+#define CAP_TO_CLKUNIT_M  	BITFIELD_MASK(1)
+#define CAP_TO_CLKUNIT_S 	7
+/* Note: for sdio-2.0 case, this mask has to be 6 bits, but msb 2
+	bits are reserved. going ahead with 8 bits, as it is req for 3.0
+*/
+#define CAP_BASECLK_M 		BITFIELD_MASK(8)
+#define CAP_BASECLK_S 		8
+#define CAP_MAXBLOCK_M 		BITFIELD_MASK(2)
+#define CAP_MAXBLOCK_S		16
+#define CAP_ADMA2_M		BITFIELD_MASK(1)
+#define CAP_ADMA2_S		19
+#define CAP_ADMA1_M		BITFIELD_MASK(1)
+#define CAP_ADMA1_S		20
+#define CAP_HIGHSPEED_M		BITFIELD_MASK(1)
+#define CAP_HIGHSPEED_S		21
+#define CAP_DMA_M		BITFIELD_MASK(1)
+#define CAP_DMA_S		22
+#define CAP_SUSPEND_M		BITFIELD_MASK(1)
+#define CAP_SUSPEND_S		23
+#define CAP_VOLT_3_3_M		BITFIELD_MASK(1)
+#define CAP_VOLT_3_3_S		24
+#define CAP_VOLT_3_0_M		BITFIELD_MASK(1)
+#define CAP_VOLT_3_0_S		25
+#define CAP_VOLT_1_8_M		BITFIELD_MASK(1)
+#define CAP_VOLT_1_8_S		26
+#define CAP_64BIT_HOST_M	BITFIELD_MASK(1)
+#define CAP_64BIT_HOST_S	28
+
+#define SDIO_OCR_READ_FAIL	(2)
+
+
+#define CAP_ASYNCINT_SUP_M	BITFIELD_MASK(1)
+#define CAP_ASYNCINT_SUP_S	29
+
+#define CAP_SLOTTYPE_M		BITFIELD_MASK(2)
+#define CAP_SLOTTYPE_S		30
+
+#define CAP3_MSBits_OFFSET	(32)
+/* note: following are caps MSB32 bits.
+	So the bits start from 0, instead of 32. that is why
+	CAP3_MSBits_OFFSET is subtracted.
+*/
+#define CAP3_SDR50_SUP_M		BITFIELD_MASK(1)
+#define CAP3_SDR50_SUP_S		(32 - CAP3_MSBits_OFFSET)
+
+#define CAP3_SDR104_SUP_M	BITFIELD_MASK(1)
+#define CAP3_SDR104_SUP_S	(33 - CAP3_MSBits_OFFSET)
+
+#define CAP3_DDR50_SUP_M	BITFIELD_MASK(1)
+#define CAP3_DDR50_SUP_S	(34 - CAP3_MSBits_OFFSET)
+
+/* for knowing the clk caps in a single read */
+#define CAP3_30CLKCAP_M		BITFIELD_MASK(3)
+#define CAP3_30CLKCAP_S		(32 - CAP3_MSBits_OFFSET)
+
+#define CAP3_DRIVTYPE_A_M	BITFIELD_MASK(1)
+#define CAP3_DRIVTYPE_A_S	(36 - CAP3_MSBits_OFFSET)
+
+#define CAP3_DRIVTYPE_C_M	BITFIELD_MASK(1)
+#define CAP3_DRIVTYPE_C_S	(37 - CAP3_MSBits_OFFSET)
+
+#define CAP3_DRIVTYPE_D_M	BITFIELD_MASK(1)
+#define CAP3_DRIVTYPE_D_S	(38 - CAP3_MSBits_OFFSET)
+
+#define CAP3_RETUNING_TC_M	BITFIELD_MASK(4)
+#define CAP3_RETUNING_TC_S	(40 - CAP3_MSBits_OFFSET)
+
+#define CAP3_TUNING_SDR50_M	BITFIELD_MASK(1)
+#define CAP3_TUNING_SDR50_S	(45 - CAP3_MSBits_OFFSET)
+
+#define CAP3_RETUNING_MODES_M	BITFIELD_MASK(2)
+#define CAP3_RETUNING_MODES_S	(46 - CAP3_MSBits_OFFSET)
+
+#define CAP3_CLK_MULT_M		BITFIELD_MASK(8)
+#define CAP3_CLK_MULT_S		(48 - CAP3_MSBits_OFFSET)
+
+#define PRESET_DRIVR_SELECT_M	BITFIELD_MASK(2)
+#define PRESET_DRIVR_SELECT_S	14
+
+#define PRESET_CLK_DIV_M	BITFIELD_MASK(10)
+#define PRESET_CLK_DIV_S	0
+
+/* SD_MaxCurCap reg (0x048) */
+#define CAP_CURR_3_3_M		BITFIELD_MASK(8)
+#define CAP_CURR_3_3_S		0
+#define CAP_CURR_3_0_M		BITFIELD_MASK(8)
+#define CAP_CURR_3_0_S		8
+#define CAP_CURR_1_8_M		BITFIELD_MASK(8)
+#define CAP_CURR_1_8_S		16
+
+/* SD_SysAddr: Offset 0x0000, Size 4 bytes */
+
+/* SD_BlockSize: Offset 0x004, Size 2 bytes */
+#define BLKSZ_BLKSZ_M		BITFIELD_MASK(12)
+#define BLKSZ_BLKSZ_S		0
+#define BLKSZ_BNDRY_M		BITFIELD_MASK(3)
+#define BLKSZ_BNDRY_S		12
+
+/* SD_BlockCount: Offset 0x006, size 2 bytes */
+
+/* SD_Arg0: Offset 0x008, size = 4 bytes  */
+/* SD_TransferMode Offset 0x00C, size = 2 bytes */
+#define XFER_DMA_ENABLE_M   	BITFIELD_MASK(1)
+#define XFER_DMA_ENABLE_S	0
+#define XFER_BLK_COUNT_EN_M 	BITFIELD_MASK(1)
+#define XFER_BLK_COUNT_EN_S	1
+#define XFER_CMD_12_EN_M    	BITFIELD_MASK(1)
+#define XFER_CMD_12_EN_S 	2
+#define XFER_DATA_DIRECTION_M	BITFIELD_MASK(1)
+#define XFER_DATA_DIRECTION_S	4
+#define XFER_MULTI_BLOCK_M	BITFIELD_MASK(1)
+#define XFER_MULTI_BLOCK_S	5
+
+/* SD_Command: Offset 0x00E, size = 2 bytes */
+/* resp_type field */
+#define RESP_TYPE_NONE 		0
+#define RESP_TYPE_136  		1
+#define RESP_TYPE_48   		2
+#define RESP_TYPE_48_BUSY	3
+/* type field */
+#define CMD_TYPE_NORMAL		0
+#define CMD_TYPE_SUSPEND	1
+#define CMD_TYPE_RESUME		2
+#define CMD_TYPE_ABORT		3
+
+#define CMD_RESP_TYPE_M		BITFIELD_MASK(2)	/* Bits [0-1] 	- Response type */
+#define CMD_RESP_TYPE_S		0
+#define CMD_CRC_EN_M		BITFIELD_MASK(1)	/* Bit 3 	- CRC enable */
+#define CMD_CRC_EN_S		3
+#define CMD_INDEX_EN_M		BITFIELD_MASK(1)	/* Bit 4 	- Enable index checking */
+#define CMD_INDEX_EN_S		4
+#define CMD_DATA_EN_M		BITFIELD_MASK(1)	/* Bit 5 	- Using DAT line */
+#define CMD_DATA_EN_S		5
+#define CMD_TYPE_M		BITFIELD_MASK(2)	/* Bit [6-7] 	- Normal, abort, resume, etc
+							 */
+#define CMD_TYPE_S		6
+#define CMD_INDEX_M		BITFIELD_MASK(6)	/* Bits [8-13] 	- Command number */
+#define CMD_INDEX_S		8
+
+/* SD_BufferDataPort0	: Offset 0x020, size = 2 or 4 bytes */
+/* SD_BufferDataPort1 	: Offset 0x022, size = 2 bytes */
+/* SD_PresentState	: Offset 0x024, size = 4 bytes */
+#define PRES_CMD_INHIBIT_M	BITFIELD_MASK(1)	/* Bit 0	May use CMD */
+#define PRES_CMD_INHIBIT_S	0
+#define PRES_DAT_INHIBIT_M	BITFIELD_MASK(1)	/* Bit 1	May use DAT */
+#define PRES_DAT_INHIBIT_S	1
+#define PRES_DAT_BUSY_M		BITFIELD_MASK(1)	/* Bit 2	DAT is busy */
+#define PRES_DAT_BUSY_S		2
+#define PRES_PRESENT_RSVD_M	BITFIELD_MASK(5)	/* Bit [3-7]	rsvd */
+#define PRES_PRESENT_RSVD_S	3
+#define PRES_WRITE_ACTIVE_M	BITFIELD_MASK(1)	/* Bit 8	Write is active */
+#define PRES_WRITE_ACTIVE_S	8
+#define PRES_READ_ACTIVE_M	BITFIELD_MASK(1)	/* Bit 9	Read is active */
+#define PRES_READ_ACTIVE_S	9
+#define PRES_WRITE_DATA_RDY_M	BITFIELD_MASK(1)	/* Bit 10	Write buf is avail */
+#define PRES_WRITE_DATA_RDY_S	10
+#define PRES_READ_DATA_RDY_M	BITFIELD_MASK(1)	/* Bit 11	Read buf data avail */
+#define PRES_READ_DATA_RDY_S	11
+#define PRES_CARD_PRESENT_M	BITFIELD_MASK(1)	/* Bit 16	Card present - debounced */
+#define PRES_CARD_PRESENT_S	16
+#define PRES_CARD_STABLE_M	BITFIELD_MASK(1)	/* Bit 17	Debugging */
+#define PRES_CARD_STABLE_S	17
+#define PRES_CARD_PRESENT_RAW_M	BITFIELD_MASK(1)	/* Bit 18	Not debounced */
+#define PRES_CARD_PRESENT_RAW_S	18
+#define PRES_WRITE_ENABLED_M	BITFIELD_MASK(1)	/* Bit 19	Write protected? */
+#define PRES_WRITE_ENABLED_S	19
+#define PRES_DAT_SIGNAL_M	BITFIELD_MASK(4)	/* Bit [20-23]	Debugging */
+#define PRES_DAT_SIGNAL_S	20
+#define PRES_CMD_SIGNAL_M	BITFIELD_MASK(1)	/* Bit 24	Debugging */
+#define PRES_CMD_SIGNAL_S	24
+
+/* SD_HostCntrl: Offset 0x028, size = 1 bytes */
+#define HOST_LED_M		BITFIELD_MASK(1)	/* Bit 0	LED On/Off */
+#define HOST_LED_S		0
+#define HOST_DATA_WIDTH_M	BITFIELD_MASK(1)	/* Bit 1	4 bit enable */
+#define HOST_DATA_WIDTH_S	1
+#define HOST_HI_SPEED_EN_M	BITFIELD_MASK(1)	/* Bit 2	High speed vs low speed */
+#define HOST_DMA_SEL_S		3
+#define HOST_DMA_SEL_M		BITFIELD_MASK(2)	/* Bit 4:3	DMA Select */
+#define HOST_HI_SPEED_EN_S	2
+
+/* Host Control2: */
+#define HOSTCtrl2_PRESVAL_EN_M	BITFIELD_MASK(1)	/* 1 bit */
+#define HOSTCtrl2_PRESVAL_EN_S	15					/* bit# */
+
+#define HOSTCtrl2_ASYINT_EN_M	BITFIELD_MASK(1)	/* 1 bit */
+#define HOSTCtrl2_ASYINT_EN_S	14					/* bit# */
+
+#define HOSTCtrl2_SAMPCLK_SEL_M	BITFIELD_MASK(1)	/* 1 bit */
+#define HOSTCtrl2_SAMPCLK_SEL_S	7					/* bit# */
+
+#define HOSTCtrl2_EXEC_TUNING_M	BITFIELD_MASK(1)	/* 1 bit */
+#define HOSTCtrl2_EXEC_TUNING_S	6					/* bit# */
+
+#define HOSTCtrl2_DRIVSTRENGTH_SEL_M	BITFIELD_MASK(2)	/* 2 bit */
+#define HOSTCtrl2_DRIVSTRENGTH_SEL_S	4					/* bit# */
+
+#define HOSTCtrl2_1_8SIG_EN_M	BITFIELD_MASK(1)	/* 1 bit */
+#define HOSTCtrl2_1_8SIG_EN_S	3					/* bit# */
+
+#define HOSTCtrl2_UHSMODE_SEL_M	BITFIELD_MASK(3)	/* 3 bit */
+#define HOSTCtrl2_UHSMODE_SEL_S	0					/* bit# */
+
+#define HOST_CONTR_VER_2		(1)
+#define HOST_CONTR_VER_3		(2)
+
+/* misc defines */
+#define SD1_MODE 		0x1	/* SD Host Cntrlr Spec */
+#define SD4_MODE 		0x2	/* SD Host Cntrlr Spec */
+
+/* SD_PwrCntrl: Offset 0x029, size = 1 bytes */
+#define PWR_BUS_EN_M		BITFIELD_MASK(1)	/* Bit 0	Power the bus */
+#define PWR_BUS_EN_S		0
+#define PWR_VOLTS_M		BITFIELD_MASK(3)	/* Bit [1-3]	Voltage Select */
+#define PWR_VOLTS_S		1
+
+/* SD_SoftwareReset: Offset 0x02F, size = 1 byte */
+#define SW_RESET_ALL_M		BITFIELD_MASK(1)	/* Bit 0	Reset All */
+#define SW_RESET_ALL_S		0
+#define SW_RESET_CMD_M		BITFIELD_MASK(1)	/* Bit 1	CMD Line Reset */
+#define SW_RESET_CMD_S		1
+#define SW_RESET_DAT_M		BITFIELD_MASK(1)	/* Bit 2	DAT Line Reset */
+#define SW_RESET_DAT_S		2
+
+/* SD_IntrStatus: Offset 0x030, size = 2 bytes */
+/* Defs also serve SD_IntrStatusEnable and SD_IntrSignalEnable */
+#define INTSTAT_CMD_COMPLETE_M		BITFIELD_MASK(1)	/* Bit 0 */
+#define INTSTAT_CMD_COMPLETE_S		0
+#define INTSTAT_XFER_COMPLETE_M		BITFIELD_MASK(1)
+#define INTSTAT_XFER_COMPLETE_S		1
+#define INTSTAT_BLOCK_GAP_EVENT_M	BITFIELD_MASK(1)
+#define INTSTAT_BLOCK_GAP_EVENT_S	2
+#define INTSTAT_DMA_INT_M		BITFIELD_MASK(1)
+#define INTSTAT_DMA_INT_S		3
+#define INTSTAT_BUF_WRITE_READY_M	BITFIELD_MASK(1)
+#define INTSTAT_BUF_WRITE_READY_S	4
+#define INTSTAT_BUF_READ_READY_M	BITFIELD_MASK(1)
+#define INTSTAT_BUF_READ_READY_S	5
+#define INTSTAT_CARD_INSERTION_M	BITFIELD_MASK(1)
+#define INTSTAT_CARD_INSERTION_S	6
+#define INTSTAT_CARD_REMOVAL_M		BITFIELD_MASK(1)
+#define INTSTAT_CARD_REMOVAL_S		7
+#define INTSTAT_CARD_INT_M		BITFIELD_MASK(1)
+#define INTSTAT_CARD_INT_S		8
+#define INTSTAT_RETUNING_INT_M		BITFIELD_MASK(1)	/* Bit 12 */
+#define INTSTAT_RETUNING_INT_S		12
+#define INTSTAT_ERROR_INT_M		BITFIELD_MASK(1)	/* Bit 15 */
+#define INTSTAT_ERROR_INT_S		15
+
+/* SD_ErrorIntrStatus: Offset 0x032, size = 2 bytes */
+/* Defs also serve SD_ErrorIntrStatusEnable and SD_ErrorIntrSignalEnable */
+#define ERRINT_CMD_TIMEOUT_M		BITFIELD_MASK(1)
+#define ERRINT_CMD_TIMEOUT_S		0
+#define ERRINT_CMD_CRC_M		BITFIELD_MASK(1)
+#define ERRINT_CMD_CRC_S		1
+#define ERRINT_CMD_ENDBIT_M		BITFIELD_MASK(1)
+#define ERRINT_CMD_ENDBIT_S		2
+#define ERRINT_CMD_INDEX_M		BITFIELD_MASK(1)
+#define ERRINT_CMD_INDEX_S		3
+#define ERRINT_DATA_TIMEOUT_M		BITFIELD_MASK(1)
+#define ERRINT_DATA_TIMEOUT_S		4
+#define ERRINT_DATA_CRC_M		BITFIELD_MASK(1)
+#define ERRINT_DATA_CRC_S		5
+#define ERRINT_DATA_ENDBIT_M		BITFIELD_MASK(1)
+#define ERRINT_DATA_ENDBIT_S		6
+#define ERRINT_CURRENT_LIMIT_M		BITFIELD_MASK(1)
+#define ERRINT_CURRENT_LIMIT_S		7
+#define ERRINT_AUTO_CMD12_M		BITFIELD_MASK(1)
+#define ERRINT_AUTO_CMD12_S		8
+#define ERRINT_VENDOR_M			BITFIELD_MASK(4)
+#define ERRINT_VENDOR_S			12
+#define ERRINT_ADMA_M			BITFIELD_MASK(1)
+#define ERRINT_ADMA_S			9
+
+/* Also provide definitions in "normal" form to allow combined masks */
+#define ERRINT_CMD_TIMEOUT_BIT		0x0001
+#define ERRINT_CMD_CRC_BIT		0x0002
+#define ERRINT_CMD_ENDBIT_BIT		0x0004
+#define ERRINT_CMD_INDEX_BIT		0x0008
+#define ERRINT_DATA_TIMEOUT_BIT		0x0010
+#define ERRINT_DATA_CRC_BIT		0x0020
+#define ERRINT_DATA_ENDBIT_BIT		0x0040
+#define ERRINT_CURRENT_LIMIT_BIT	0x0080
+#define ERRINT_AUTO_CMD12_BIT		0x0100
+#define ERRINT_ADMA_BIT		0x0200
+
+/* Masks to select CMD vs. DATA errors */
+#define ERRINT_CMD_ERRS		(ERRINT_CMD_TIMEOUT_BIT | ERRINT_CMD_CRC_BIT |\
+				 ERRINT_CMD_ENDBIT_BIT | ERRINT_CMD_INDEX_BIT)
+#define ERRINT_DATA_ERRS	(ERRINT_DATA_TIMEOUT_BIT | ERRINT_DATA_CRC_BIT |\
+				 ERRINT_DATA_ENDBIT_BIT | ERRINT_ADMA_BIT)
+#define ERRINT_TRANSFER_ERRS	(ERRINT_CMD_ERRS | ERRINT_DATA_ERRS)
+
+/* SD_WakeupCntr_BlockGapCntrl : Offset 0x02A , size = bytes */
+/* SD_ClockCntrl	: Offset 0x02C , size = bytes */
+/* SD_SoftwareReset_TimeoutCntrl 	: Offset 0x02E , size = bytes */
+/* SD_IntrStatus	: Offset 0x030 , size = bytes */
+/* SD_ErrorIntrStatus 	: Offset 0x032 , size = bytes */
+/* SD_IntrStatusEnable	: Offset 0x034 , size = bytes */
+/* SD_ErrorIntrStatusEnable : Offset 0x036 , size = bytes */
+/* SD_IntrSignalEnable	: Offset 0x038 , size = bytes */
+/* SD_ErrorIntrSignalEnable : Offset 0x03A , size = bytes */
+/* SD_CMD12ErrorStatus	: Offset 0x03C , size = bytes */
+/* SD_Capabilities	: Offset 0x040 , size = bytes */
+/* SD_MaxCurCap		: Offset 0x048 , size = bytes */
+/* SD_MaxCurCap_Reserved: Offset 0x04C , size = bytes */
+/* SD_SlotInterruptStatus: Offset 0x0FC , size = bytes */
+/* SD_HostControllerVersion : Offset 0x0FE , size = bytes */
+
+/* SDIO Host Control Register DMA Mode Definitions */
+#define SDIOH_SDMA_MODE			0
+#define SDIOH_ADMA1_MODE		1
+#define SDIOH_ADMA2_MODE		2
+#define SDIOH_ADMA2_64_MODE		3
+
+#define ADMA2_ATTRIBUTE_VALID		(1 << 0)	/* ADMA Descriptor line valid */
+#define ADMA2_ATTRIBUTE_END			(1 << 1)	/* End of Descriptor */
+#define ADMA2_ATTRIBUTE_INT			(1 << 2)	/* Interrupt when line is done */
+#define ADMA2_ATTRIBUTE_ACT_NOP		(0 << 4)	/* Skip current line, go to next. */
+#define ADMA2_ATTRIBUTE_ACT_RSV		(1 << 4)	/* Same as NOP */
+#define ADMA1_ATTRIBUTE_ACT_SET		(1 << 4)	/* ADMA1 Only - set transfer length */
+#define ADMA2_ATTRIBUTE_ACT_TRAN	(2 << 4)	/* Transfer Data of one descriptor line. */
+#define ADMA2_ATTRIBUTE_ACT_LINK	(3 << 4)	/* Link Descriptor */
+
+/* ADMA2 Descriptor Table Entry for 32-bit Address */
+typedef struct adma2_dscr_32b {
+	uint32 len_attr;
+	uint32 phys_addr;
+} adma2_dscr_32b_t;
+
+/* ADMA1 Descriptor Table Entry */
+typedef struct adma1_dscr {
+	uint32 phys_addr_attr;
+} adma1_dscr_t;
+
+#endif /* _SDIOH_H */
diff --git a/drivers/net/wireless/bcmdhd/include/sdiovar.h b/drivers/net/wireless/bcmdhd/include/sdiovar.h
new file mode 100644
index 000000000000..55a3d3490c30
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/sdiovar.h
@@ -0,0 +1,58 @@
+/*
+ * Structure used by apps whose drivers access SDIO drivers.
+ * Pulled out separately so dhdu and wlu can both use it.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sdiovar.h 277737 2011-08-16 17:54:59Z $
+ */
+
+#ifndef _sdiovar_h_
+#define _sdiovar_h_
+
+#include <typedefs.h>
+
+/* require default structure packing */
+#define BWL_DEFAULT_PACKING
+#include <packed_section_start.h>
+
+typedef struct sdreg {
+	int func;
+	int offset;
+	int value;
+} sdreg_t;
+
+/* Common msglevel constants */
+#define SDH_ERROR_VAL		0x0001	/* Error */
+#define SDH_TRACE_VAL		0x0002	/* Trace */
+#define SDH_INFO_VAL		0x0004	/* Info */
+#define SDH_DEBUG_VAL		0x0008	/* Debug */
+#define SDH_DATA_VAL		0x0010	/* Data */
+#define SDH_CTRL_VAL		0x0020	/* Control Regs */
+#define SDH_LOG_VAL		0x0040	/* Enable bcmlog */
+#define SDH_DMA_VAL		0x0080	/* DMA */
+
+#define NUM_PREV_TRANSACTIONS	16
+
+
+#include <packed_section_end.h>
+
+#endif /* _sdiovar_h_ */
diff --git a/drivers/net/wireless/bcmdhd/include/siutils.h b/drivers/net/wireless/bcmdhd/include/siutils.h
new file mode 100644
index 000000000000..4e7aeb71cb02
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/siutils.h
@@ -0,0 +1,277 @@
+/*
+ * Misc utility routines for accessing the SOC Interconnects
+ * of Broadcom HNBU chips.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: siutils.h 335486 2012-05-28 09:47:55Z $
+ */
+
+
+#ifndef	_siutils_h_
+#define	_siutils_h_
+
+
+struct si_pub {
+	uint	socitype;		
+
+	uint	bustype;		
+	uint	buscoretype;		
+	uint	buscorerev;		
+	uint	buscoreidx;		
+	int	ccrev;			
+	uint32	cccaps;			
+	uint32  cccaps_ext;			
+	int	pmurev;			
+	uint32	pmucaps;		
+	uint	boardtype;		
+	uint	boardvendor;		
+	uint	boardflags;		
+	uint	boardflags2;		
+	uint	chip;			
+	uint	chiprev;		
+	uint	chippkg;		
+	uint32	chipst;			
+	bool	issim;			
+	uint    socirev;		
+	bool	pci_pr32414;
+
+};
+
+
+typedef const struct si_pub si_t;
+
+
+
+#define	SI_OSH		NULL	
+
+#define	BADIDX		(SI_MAXCORES + 1)
+
+
+#define	XTAL			0x1	
+#define	PLL			0x2	
+
+
+#define	CLK_FAST		0	
+#define	CLK_DYNAMIC		2	
+
+
+#define GPIO_DRV_PRIORITY	0	
+#define GPIO_APP_PRIORITY	1	
+#define GPIO_HI_PRIORITY	2	
+
+
+#define GPIO_PULLUP		0
+#define GPIO_PULLDN		1
+
+
+#define GPIO_REGEVT		0	
+#define GPIO_REGEVT_INTMSK	1	
+#define GPIO_REGEVT_INTPOL	2	
+
+
+#define SI_DEVPATH_BUFSZ	16	
+
+
+#define	SI_DOATTACH	1
+#define SI_PCIDOWN	2
+#define SI_PCIUP	3
+
+#define	ISSIM_ENAB(sih)	0
+
+
+#if defined(BCMPMUCTL)
+#define PMUCTL_ENAB(sih)	(BCMPMUCTL)
+#else
+#define PMUCTL_ENAB(sih)	((sih)->cccaps & CC_CAP_PMU)
+#endif
+
+
+#if defined(BCMPMUCTL) && BCMPMUCTL
+#define CCCTL_ENAB(sih)		(0)
+#define CCPLL_ENAB(sih)		(0)
+#else
+#define CCCTL_ENAB(sih)		((sih)->cccaps & CC_CAP_PWR_CTL)
+#define CCPLL_ENAB(sih)		((sih)->cccaps & CC_CAP_PLL_MASK)
+#endif
+
+typedef void (*gpio_handler_t)(uint32 stat, void *arg);
+
+
+
+extern si_t *si_attach(uint pcidev, osl_t *osh, void *regs, uint bustype,
+                       void *sdh, char **vars, uint *varsz);
+extern si_t *si_kattach(osl_t *osh);
+extern void si_detach(si_t *sih);
+extern bool si_pci_war16165(si_t *sih);
+
+extern uint si_corelist(si_t *sih, uint coreid[]);
+extern uint si_coreid(si_t *sih);
+extern uint si_flag(si_t *sih);
+extern uint si_intflag(si_t *sih);
+extern uint si_coreidx(si_t *sih);
+extern uint si_coreunit(si_t *sih);
+extern uint si_corevendor(si_t *sih);
+extern uint si_corerev(si_t *sih);
+extern void *si_osh(si_t *sih);
+extern void si_setosh(si_t *sih, osl_t *osh);
+extern uint si_corereg(si_t *sih, uint coreidx, uint regoff, uint mask, uint val);
+extern void *si_coreregs(si_t *sih);
+extern uint si_wrapperreg(si_t *sih, uint32 offset, uint32 mask, uint32 val);
+extern uint32 si_core_cflags(si_t *sih, uint32 mask, uint32 val);
+extern void si_core_cflags_wo(si_t *sih, uint32 mask, uint32 val);
+extern uint32 si_core_sflags(si_t *sih, uint32 mask, uint32 val);
+extern bool si_iscoreup(si_t *sih);
+extern uint si_findcoreidx(si_t *sih, uint coreid, uint coreunit);
+extern void *si_setcoreidx(si_t *sih, uint coreidx);
+extern void *si_setcore(si_t *sih, uint coreid, uint coreunit);
+extern void *si_switch_core(si_t *sih, uint coreid, uint *origidx, uint *intr_val);
+extern void si_restore_core(si_t *sih, uint coreid, uint intr_val);
+extern int si_numaddrspaces(si_t *sih);
+extern uint32 si_addrspace(si_t *sih, uint asidx);
+extern uint32 si_addrspacesize(si_t *sih, uint asidx);
+extern int si_corebist(si_t *sih);
+extern void si_core_reset(si_t *sih, uint32 bits, uint32 resetbits);
+extern void si_core_disable(si_t *sih, uint32 bits);
+extern uint32 si_clock_rate(uint32 pll_type, uint32 n, uint32 m);
+extern bool si_read_pmu_autopll(si_t *sih);
+extern uint32 si_clock(si_t *sih);
+extern uint32 si_alp_clock(si_t *sih);
+extern uint32 si_ilp_clock(si_t *sih);
+extern void si_pci_setup(si_t *sih, uint coremask);
+extern void si_pcmcia_init(si_t *sih);
+extern void si_setint(si_t *sih, int siflag);
+extern bool si_backplane64(si_t *sih);
+extern void si_register_intr_callback(si_t *sih, void *intrsoff_fn, void *intrsrestore_fn,
+	void *intrsenabled_fn, void *intr_arg);
+extern void si_deregister_intr_callback(si_t *sih);
+extern void si_clkctl_init(si_t *sih);
+extern uint16 si_clkctl_fast_pwrup_delay(si_t *sih);
+extern bool si_clkctl_cc(si_t *sih, uint mode);
+extern int si_clkctl_xtal(si_t *sih, uint what, bool on);
+extern uint32 si_gpiotimerval(si_t *sih, uint32 mask, uint32 val);
+extern void si_btcgpiowar(si_t *sih);
+extern bool si_deviceremoved(si_t *sih);
+extern uint32 si_socram_size(si_t *sih);
+extern uint32 si_socdevram_size(si_t *sih);
+extern void si_socdevram(si_t *sih, bool set, uint8 *ennable, uint8 *protect);
+extern bool si_socdevram_pkg(si_t *sih);
+
+extern void si_watchdog(si_t *sih, uint ticks);
+extern void si_watchdog_ms(si_t *sih, uint32 ms);
+extern void *si_gpiosetcore(si_t *sih);
+extern uint32 si_gpiocontrol(si_t *sih, uint32 mask, uint32 val, uint8 priority);
+extern uint32 si_gpioouten(si_t *sih, uint32 mask, uint32 val, uint8 priority);
+extern uint32 si_gpioout(si_t *sih, uint32 mask, uint32 val, uint8 priority);
+extern uint32 si_gpioin(si_t *sih);
+extern uint32 si_gpiointpolarity(si_t *sih, uint32 mask, uint32 val, uint8 priority);
+extern uint32 si_gpiointmask(si_t *sih, uint32 mask, uint32 val, uint8 priority);
+extern uint32 si_gpioled(si_t *sih, uint32 mask, uint32 val);
+extern uint32 si_gpioreserve(si_t *sih, uint32 gpio_num, uint8 priority);
+extern uint32 si_gpiorelease(si_t *sih, uint32 gpio_num, uint8 priority);
+extern uint32 si_gpiopull(si_t *sih, bool updown, uint32 mask, uint32 val);
+extern uint32 si_gpioevent(si_t *sih, uint regtype, uint32 mask, uint32 val);
+extern uint32 si_gpio_int_enable(si_t *sih, bool enable);
+
+
+extern void *si_gpio_handler_register(si_t *sih, uint32 e, bool lev, gpio_handler_t cb, void *arg);
+extern void si_gpio_handler_unregister(si_t *sih, void* gpioh);
+extern void si_gpio_handler_process(si_t *sih);
+
+
+extern bool si_pci_pmecap(si_t *sih);
+struct osl_info;
+extern bool si_pci_fastpmecap(struct osl_info *osh);
+extern bool si_pci_pmestat(si_t *sih);
+extern void si_pci_pmeclr(si_t *sih);
+extern void si_pci_pmeen(si_t *sih);
+extern uint si_pcie_readreg(void *sih, uint addrtype, uint offset);
+
+extern void si_sdio_init(si_t *sih);
+
+extern uint16 si_d11_devid(si_t *sih);
+extern int si_corepciid(si_t *sih, uint func, uint16 *pcivendor, uint16 *pcidevice,
+	uint8 *pciclass, uint8 *pcisubclass, uint8 *pciprogif, uint8 *pciheader);
+
+#define si_eci(sih) 0
+#define si_eci_init(sih) (0)
+#define si_eci_notify_bt(sih, type, val)  (0)
+#define si_seci(sih) 0
+static INLINE void * si_seci_init(si_t *sih, uint8 use_seci) {return NULL;}
+#define si_seci_down(sih) do { } while (0)
+
+
+extern bool si_is_otp_disabled(si_t *sih);
+extern bool si_is_otp_powered(si_t *sih);
+extern void si_otp_power(si_t *sih, bool on);
+extern void si_set_otp_wr_volts(si_t *sih);
+extern void si_set_otp_rd_volts(si_t *sih);
+
+
+extern bool si_is_sprom_available(si_t *sih);
+extern bool si_is_sprom_enabled(si_t *sih);
+extern void si_sprom_enable(si_t *sih, bool enable);
+
+
+extern int si_cis_source(si_t *sih);
+#define CIS_DEFAULT	0
+#define CIS_SROM	1
+#define CIS_OTP		2
+
+
+#define	DEFAULT_FAB	0x0	
+#define	CSM_FAB7	0x1	
+#define	TSMC_FAB12	0x2	
+#define	SMIC_FAB4	0x3	
+extern int si_otp_fabid(si_t *sih, uint16 *fabid, bool rw);
+extern uint16 si_fabid(si_t *sih);
+
+
+extern int si_devpath(si_t *sih, char *path, int size);
+
+extern char *si_getdevpathvar(si_t *sih, const char *name);
+extern int si_getdevpathintvar(si_t *sih, const char *name);
+
+
+extern uint8 si_pcieclkreq(si_t *sih, uint32 mask, uint32 val);
+extern uint32 si_pcielcreg(si_t *sih, uint32 mask, uint32 val);
+extern void si_war42780_clkreq(si_t *sih, bool clkreq);
+extern void si_pci_sleep(si_t *sih);
+extern void si_pci_down(si_t *sih);
+extern void si_pci_up(si_t *sih);
+extern void si_pcie_war_ovr_update(si_t *sih, uint8 aspm);
+extern void si_pcie_extendL1timer(si_t *sih, bool extend);
+extern int si_pci_fixcfg(si_t *sih);
+extern uint si_pll_reset(si_t *sih);
+
+
+
+extern bool si_taclear(si_t *sih, bool details);
+
+
+
+extern uint32 si_pciereg(si_t *sih, uint32 offset, uint32 mask, uint32 val, uint type);
+extern uint32 si_pcieserdesreg(si_t *sih, uint32 mdioslave, uint32 offset, uint32 mask, uint32 val);
+
+char *si_getnvramflvar(si_t *sih, const char *name);
+
+
+#endif	
diff --git a/drivers/net/wireless/bcmdhd/include/trxhdr.h b/drivers/net/wireless/bcmdhd/include/trxhdr.h
new file mode 100644
index 000000000000..b52fb15ba5c0
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/trxhdr.h
@@ -0,0 +1,53 @@
+/*
+ * TRX image file header format.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: trxhdr.h 286295 2011-09-27 06:39:43Z $
+ */
+
+#ifndef	_TRX_HDR_H_
+#define	_TRX_HDR_H_
+
+#include <typedefs.h>
+
+#define TRX_MAGIC	0x30524448	/* "HDR0" */
+#define TRX_VERSION	1		/* Version 1 */
+#define TRX_MAX_LEN	0x3B0000	/* Max length */
+#define TRX_NO_HEADER	1		/* Do not write TRX header */
+#define TRX_GZ_FILES	0x2     /* Contains up to TRX_MAX_OFFSET individual gzip files */
+#define TRX_OVERLAYS	0x4     /* Contains an overlay header after the trx header */
+#define TRX_MAX_OFFSET	3		/* Max number of individual files */
+#define TRX_UNCOMP_IMAGE	0x20	/* Trx contains uncompressed rtecdc.bin image */
+#define TRX_ROMSIM_IMAGE	0x10	/* Trx contains ROM simulation image */
+
+struct trx_header {
+	uint32 magic;		/* "HDR0" */
+	uint32 len;		/* Length of file including header */
+	uint32 crc32;		/* 32-bit CRC from flag_version to end of file */
+	uint32 flag_version;	/* 0:15 flags, 16:31 version */
+	uint32 offsets[TRX_MAX_OFFSET];	/* Offsets of partitions from start of header */
+};
+
+/* Compatibility */
+typedef struct trx_header TRXHDR, *PTRXHDR;
+
+#endif /* _TRX_HDR_H_ */
diff --git a/drivers/net/wireless/bcmdhd/include/typedefs.h b/drivers/net/wireless/bcmdhd/include/typedefs.h
new file mode 100644
index 000000000000..d0902fe80891
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/typedefs.h
@@ -0,0 +1,312 @@
+/*
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ * $Id: typedefs.h 290055 2011-10-15 21:26:26Z $
+ */
+
+
+#ifndef _TYPEDEFS_H_
+#define _TYPEDEFS_H_
+
+#ifdef SITE_TYPEDEFS
+
+
+
+#include "site_typedefs.h"
+
+#else
+
+
+
+#ifdef __cplusplus
+
+#define TYPEDEF_BOOL
+#ifndef FALSE
+#define FALSE	false
+#endif
+#ifndef TRUE
+#define TRUE	true
+#endif
+
+#else	
+
+
+#endif	
+
+#if defined(__x86_64__)
+#define TYPEDEF_UINTPTR
+typedef unsigned long long int uintptr;
+#endif
+
+
+
+
+
+#if defined(_NEED_SIZE_T_)
+typedef long unsigned int size_t;
+#endif
+
+
+
+
+
+#if defined(__sparc__)
+#define TYPEDEF_ULONG
+#endif
+
+
+
+#if !defined(LINUX_HYBRID) || defined(LINUX_PORT)
+#define TYPEDEF_UINT
+#ifndef TARGETENV_android
+#define TYPEDEF_USHORT
+#define TYPEDEF_ULONG
+#endif 
+#ifdef __KERNEL__
+#include <linux/version.h>
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19))
+#define TYPEDEF_BOOL
+#endif	
+
+#if (LINUX_VERSION_CODE == KERNEL_VERSION(2, 6, 18))
+#include <linux/compiler.h>
+#ifdef noinline_for_stack
+#define TYPEDEF_BOOL
+#endif
+#endif	
+#endif	
+#endif  
+
+
+
+
+
+#if defined(__GNUC__) && defined(__STRICT_ANSI__)
+#define TYPEDEF_INT64
+#define TYPEDEF_UINT64
+#endif
+
+
+#if defined(__ICL)
+
+#define TYPEDEF_INT64
+
+#if defined(__STDC__)
+#define TYPEDEF_UINT64
+#endif
+
+#endif 
+
+#if !defined(__DJGPP__)
+
+
+#if defined(__KERNEL__)
+
+
+#if !defined(LINUX_HYBRID) || defined(LINUX_PORT)
+#include <linux/types.h>	
+#endif 
+
+#else
+
+
+#include <sys/types.h>
+
+#endif 
+
+#endif 
+
+
+
+
+#define USE_TYPEDEF_DEFAULTS
+
+#endif 
+
+
+
+
+#ifdef USE_TYPEDEF_DEFAULTS
+#undef USE_TYPEDEF_DEFAULTS
+
+#ifndef TYPEDEF_BOOL
+typedef	 unsigned char	bool;
+#endif
+
+
+
+#ifndef TYPEDEF_UCHAR
+typedef unsigned char	uchar;
+#endif
+
+#ifndef TYPEDEF_USHORT
+typedef unsigned short	ushort;
+#endif
+
+#ifndef TYPEDEF_UINT
+typedef unsigned int	uint;
+#endif
+
+#ifndef TYPEDEF_ULONG
+typedef unsigned long	ulong;
+#endif
+
+
+
+#ifndef TYPEDEF_UINT8
+typedef unsigned char	uint8;
+#endif
+
+#ifndef TYPEDEF_UINT16
+typedef unsigned short	uint16;
+#endif
+
+#ifndef TYPEDEF_UINT32
+typedef unsigned int	uint32;
+#endif
+
+#ifndef TYPEDEF_UINT64
+typedef unsigned long long uint64;
+#endif
+
+#ifndef TYPEDEF_UINTPTR
+typedef unsigned int	uintptr;
+#endif
+
+#ifndef TYPEDEF_INT8
+typedef signed char	int8;
+#endif
+
+#ifndef TYPEDEF_INT16
+typedef signed short	int16;
+#endif
+
+#ifndef TYPEDEF_INT32
+typedef signed int	int32;
+#endif
+
+#ifndef TYPEDEF_INT64
+typedef signed long long int64;
+#endif
+
+
+
+#ifndef TYPEDEF_FLOAT32
+typedef float		float32;
+#endif
+
+#ifndef TYPEDEF_FLOAT64
+typedef double		float64;
+#endif
+
+
+
+#ifndef TYPEDEF_FLOAT_T
+
+#if defined(FLOAT32)
+typedef float32 float_t;
+#else 
+typedef float64 float_t;
+#endif
+
+#endif 
+
+
+
+#ifndef FALSE
+#define FALSE	0
+#endif
+
+#ifndef TRUE
+#define TRUE	1  
+#endif
+
+#ifndef NULL
+#define	NULL	0
+#endif
+
+#ifndef OFF
+#define	OFF	0
+#endif
+
+#ifndef ON
+#define	ON	1  
+#endif
+
+#define	AUTO	(-1) 
+
+
+
+#ifndef PTRSZ
+#define	PTRSZ	sizeof(char*)
+#endif
+
+
+
+#if defined(__GNUC__)
+	#define BWL_COMPILER_GNU
+#elif defined(__CC_ARM) && __CC_ARM
+	#define BWL_COMPILER_ARMCC
+#else
+	#error "Unknown compiler!"
+#endif 
+
+
+#ifndef INLINE
+	#if defined(BWL_COMPILER_MICROSOFT)
+		#define INLINE __inline
+	#elif defined(BWL_COMPILER_GNU)
+		#define INLINE __inline__
+	#elif defined(BWL_COMPILER_ARMCC)
+		#define INLINE	__inline
+	#else
+		#define INLINE
+	#endif 
+#endif 
+
+#undef TYPEDEF_BOOL
+#undef TYPEDEF_UCHAR
+#undef TYPEDEF_USHORT
+#undef TYPEDEF_UINT
+#undef TYPEDEF_ULONG
+#undef TYPEDEF_UINT8
+#undef TYPEDEF_UINT16
+#undef TYPEDEF_UINT32
+#undef TYPEDEF_UINT64
+#undef TYPEDEF_UINTPTR
+#undef TYPEDEF_INT8
+#undef TYPEDEF_INT16
+#undef TYPEDEF_INT32
+#undef TYPEDEF_INT64
+#undef TYPEDEF_FLOAT32
+#undef TYPEDEF_FLOAT64
+#undef TYPEDEF_FLOAT_T
+
+#endif 
+
+
+#define UNUSED_PARAMETER(x) (void)(x)
+
+
+#define DISCARD_QUAL(ptr, type) ((type *)(uintptr)(ptr))
+
+
+#include <bcmdefs.h>
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/include/wlfc_proto.h b/drivers/net/wireless/bcmdhd/include/wlfc_proto.h
new file mode 100644
index 000000000000..d37105165bab
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/wlfc_proto.h
@@ -0,0 +1,198 @@
+/*
+* Copyright (C) 1999-2011, Broadcom Corporation
+* 
+*         Unless you and Broadcom execute a separate written software license
+* agreement governing use of this software, this software is licensed to you
+* under the terms of the GNU General Public License version 2 (the "GPL"),
+* available at http://www.broadcom.com/licenses/GPLv2.php, with the
+* following added to such license:
+* 
+*      As a special exception, the copyright holders of this software give you
+* permission to link this software with independent modules, and to copy and
+* distribute the resulting executable under terms of your choice, provided that
+* you also meet, for each linked independent module, the terms and conditions of
+* the license of that module.  An independent module is a module which is not
+* derived from this software.  The special exception does not apply to any
+* modifications of the software.
+* 
+*      Notwithstanding the above, under no circumstances may you combine this
+* software in any way with any other Broadcom software provided under a license
+* other than the GPL, without Broadcom's express prior written consent.
+* $Id: wlfc_proto.h 277737 2011-08-16 17:54:59Z $
+*
+*/
+#ifndef __wlfc_proto_definitions_h__
+#define __wlfc_proto_definitions_h__
+
+	/* Use TLV to convey WLFC information.
+	 ---------------------------------------------------------------------------
+	| Type |  Len | value                    | Description
+	 ---------------------------------------------------------------------------
+	|  1   |   1  | (handle)                 | MAC OPEN
+	 ---------------------------------------------------------------------------
+	|  2   |   1  | (handle)                 | MAC CLOSE
+	 ---------------------------------------------------------------------------
+	|  3   |   2  | (count, handle, prec_bmp)| Set the credit depth for a MAC dstn
+	 ---------------------------------------------------------------------------
+	|  4   |   4  | see pkttag comments      | TXSTATUS
+	 ---------------------------------------------------------------------------
+	|  5   |   4  | see pkttag comments      | PKKTTAG [host->firmware]
+	 ---------------------------------------------------------------------------
+	|  6   |   8  | (handle, ifid, MAC)      | MAC ADD
+	 ---------------------------------------------------------------------------
+	|  7   |   8  | (handle, ifid, MAC)      | MAC DEL
+	 ---------------------------------------------------------------------------
+	|  8   |   1  | (rssi)                   | RSSI - RSSI value for the packet.
+	 ---------------------------------------------------------------------------
+	|  9   |   1  | (interface ID)           | Interface OPEN
+	 ---------------------------------------------------------------------------
+	|  10  |   1  | (interface ID)           | Interface CLOSE
+	 ---------------------------------------------------------------------------
+	|  11  |   8  | fifo credit returns map  | FIFO credits back to the host
+	|      |      |                          |
+	|      |      |                          | --------------------------------------
+	|      |      |                          | | ac0 | ac1 | ac2 | ac3 | bcmc | atim |
+	|      |      |                          | --------------------------------------
+	|      |      |                          |
+	 ---------------------------------------------------------------------------
+	|  12  |   2  | MAC handle,              | Host provides a bitmap of pending
+	|      |      | AC[0-3] traffic bitmap   | unicast traffic for MAC-handle dstn.
+	|      |      |                          | [host->firmware]
+	 ---------------------------------------------------------------------------
+	|  13  |   3  | (count, handle, prec_bmp)| One time request for packet to a specific
+	|      |      |                          | MAC destination.
+	 ---------------------------------------------------------------------------
+	| 255  |  N/A |  N/A                     | FILLER - This is a special type
+	|      |      |                          | that has no length or value.
+	|      |      |                          | Typically used for padding.
+	 ---------------------------------------------------------------------------
+	*/
+
+#define WLFC_CTL_TYPE_MAC_OPEN				1
+#define WLFC_CTL_TYPE_MAC_CLOSE				2
+#define WLFC_CTL_TYPE_MAC_REQUEST_CREDIT	3
+#define WLFC_CTL_TYPE_TXSTATUS				4
+#define WLFC_CTL_TYPE_PKTTAG				5
+
+#define WLFC_CTL_TYPE_MACDESC_ADD			6
+#define WLFC_CTL_TYPE_MACDESC_DEL			7
+#define WLFC_CTL_TYPE_RSSI					8
+
+#define WLFC_CTL_TYPE_INTERFACE_OPEN		9
+#define WLFC_CTL_TYPE_INTERFACE_CLOSE		10
+
+#define WLFC_CTL_TYPE_FIFO_CREDITBACK		11
+
+#define WLFC_CTL_TYPE_PENDING_TRAFFIC_BMP	12
+#define WLFC_CTL_TYPE_MAC_REQUEST_PACKET	13
+
+#define WLFC_CTL_TYPE_FILLER				255
+
+#define WLFC_CTL_VALUE_LEN_MACDESC			8 /* handle, interface, MAC */
+
+#define WLFC_CTL_VALUE_LEN_MAC		1	/* MAC-handle */
+#define WLFC_CTL_VALUE_LEN_RSSI		1
+
+#define WLFC_CTL_VALUE_LEN_INTERFACE	1
+#define WLFC_CTL_VALUE_LEN_PENDING_TRAFFIC_BMP	2
+
+#define WLFC_CTL_VALUE_LEN_TXSTATUS	4
+#define WLFC_CTL_VALUE_LEN_PKTTAG	4
+
+/* enough space to host all 4 ACs, bc/mc and atim fifo credit */
+#define WLFC_CTL_VALUE_LEN_FIFO_CREDITBACK	6
+
+#define WLFC_CTL_VALUE_LEN_REQUEST_CREDIT 3 /* credit, MAC-handle, prec_bitmap */
+#define WLFC_CTL_VALUE_LEN_REQUEST_PACKET 3 /* credit, MAC-handle, prec_bitmap */
+
+
+
+#define WLFC_PKTID_GEN_MASK		0x80000000
+#define WLFC_PKTID_GEN_SHIFT	31
+
+#define WLFC_PKTID_GEN(x)	(((x) & WLFC_PKTID_GEN_MASK) >> WLFC_PKTID_GEN_SHIFT)
+#define WLFC_PKTID_SETGEN(x, gen)	(x) = ((x) & ~WLFC_PKTID_GEN_MASK) | \
+	(((gen) << WLFC_PKTID_GEN_SHIFT) & WLFC_PKTID_GEN_MASK)
+
+#define WLFC_PKTFLAG_PKTFROMHOST	0x01
+#define WLFC_PKTFLAG_PKT_REQUESTED	0x02
+
+#define WL_TXSTATUS_FLAGS_MASK			0xf /* allow 4 bits only */
+#define WL_TXSTATUS_FLAGS_SHIFT			27
+
+#define WL_TXSTATUS_SET_FLAGS(x, flags)	((x)  = \
+	((x) & ~(WL_TXSTATUS_FLAGS_MASK << WL_TXSTATUS_FLAGS_SHIFT)) | \
+	(((flags) & WL_TXSTATUS_FLAGS_MASK) << WL_TXSTATUS_FLAGS_SHIFT))
+#define WL_TXSTATUS_GET_FLAGS(x)		(((x) >> WL_TXSTATUS_FLAGS_SHIFT) & \
+	WL_TXSTATUS_FLAGS_MASK)
+
+#define WL_TXSTATUS_FIFO_MASK			0x7 /* allow 3 bits for FIFO ID */
+#define WL_TXSTATUS_FIFO_SHIFT			24
+
+#define WL_TXSTATUS_SET_FIFO(x, flags)	((x)  = \
+	((x) & ~(WL_TXSTATUS_FIFO_MASK << WL_TXSTATUS_FIFO_SHIFT)) | \
+	(((flags) & WL_TXSTATUS_FIFO_MASK) << WL_TXSTATUS_FIFO_SHIFT))
+#define WL_TXSTATUS_GET_FIFO(x)		(((x) >> WL_TXSTATUS_FIFO_SHIFT) & WL_TXSTATUS_FIFO_MASK)
+
+#define WL_TXSTATUS_PKTID_MASK			0xffffff /* allow 24 bits */
+#define WL_TXSTATUS_SET_PKTID(x, num)	((x) = \
+	((x) & ~WL_TXSTATUS_PKTID_MASK) | (num))
+#define WL_TXSTATUS_GET_PKTID(x)		((x) & WL_TXSTATUS_PKTID_MASK)
+
+/* 32 STA should be enough??, 6 bits; Must be power of 2 */
+#define WLFC_MAC_DESC_TABLE_SIZE	32
+#define WLFC_MAX_IFNUM				16
+#define WLFC_MAC_DESC_ID_INVALID	0xff
+
+/* b[7:5] -reuse guard, b[4:0] -value */
+#define WLFC_MAC_DESC_GET_LOOKUP_INDEX(x) ((x) & 0x1f)
+
+#define WLFC_PKTFLAG_SET_PKTREQUESTED(x)	(x) |= \
+	(WLFC_PKTFLAG_PKT_REQUESTED << WL_TXSTATUS_FLAGS_SHIFT)
+
+#define WLFC_PKTFLAG_CLR_PKTREQUESTED(x)	(x) &= \
+	~(WLFC_PKTFLAG_PKT_REQUESTED << WL_TXSTATUS_FLAGS_SHIFT)
+
+#define WL_TXSTATUS_GENERATION_MASK			1
+#define WL_TXSTATUS_GENERATION_SHIFT		31
+
+#define WLFC_PKTFLAG_SET_GENERATION(x, gen)	((x) = \
+	((x) & ~(WL_TXSTATUS_GENERATION_MASK << WL_TXSTATUS_GENERATION_SHIFT)) | \
+	(((gen) & WL_TXSTATUS_GENERATION_MASK) << WL_TXSTATUS_GENERATION_SHIFT))
+
+#define WLFC_PKTFLAG_GENERATION(x)	(((x) >> WL_TXSTATUS_GENERATION_SHIFT) & \
+	WL_TXSTATUS_GENERATION_MASK)
+
+#define WLFC_MAX_PENDING_DATALEN	120
+
+/* host is free to discard the packet */
+#define WLFC_CTL_PKTFLAG_DISCARD		0
+/* D11 suppressed a packet */
+#define WLFC_CTL_PKTFLAG_D11SUPPRESS	1
+/* WL firmware suppressed a packet because MAC is 
+	already in PSMode (short time window)
+*/
+#define WLFC_CTL_PKTFLAG_WLSUPPRESS		2
+/* Firmware tossed this packet */
+#define WLFC_CTL_PKTFLAG_TOSSED_BYWLC	3
+
+#define WLFC_D11_STATUS_INTERPRET(txs)	((((txs)->status & TX_STATUS_SUPR_MASK) >> \
+		TX_STATUS_SUPR_SHIFT)) ? WLFC_CTL_PKTFLAG_D11SUPPRESS : WLFC_CTL_PKTFLAG_DISCARD
+
+#ifdef PROP_TXSTATUS_DEBUG
+#define WLFC_DBGMESG(x) printf x
+/* wlfc-breadcrumb */
+#define WLFC_BREADCRUMB(x) do {if ((x) == NULL) \
+	{printf("WLFC: %s():%d:caller:%p\n", \
+	__FUNCTION__, __LINE__, __builtin_return_address(0));}} while (0)
+#define WLFC_PRINTMAC(banner, ea) do {printf("%s MAC: [%02x:%02x:%02x:%02x:%02x:%02x]\n", \
+	banner, ea[0], 	ea[1], 	ea[2], 	ea[3], 	ea[4], 	ea[5]); } while (0)
+#define WLFC_WHEREIS(s) printf("WLFC: at %s():%d, %s\n", __FUNCTION__, __LINE__, (s))
+#else
+#define WLFC_DBGMESG(x)
+#define WLFC_BREADCRUMB(x)
+#define WLFC_PRINTMAC(banner, ea)
+#define WLFC_WHEREIS(s)
+#endif
+
+#endif /* __wlfc_proto_definitions_h__ */
diff --git a/drivers/net/wireless/bcmdhd/include/wlioctl.h b/drivers/net/wireless/bcmdhd/include/wlioctl.h
new file mode 100644
index 000000000000..5ec0c9ade197
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/include/wlioctl.h
@@ -0,0 +1,2811 @@
+/*
+ * Custom OID/ioctl definitions for
+ * Broadcom 802.11abg Networking Device Driver
+ *
+ * Definitions subject to change without notice.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wlioctl.h 331292 2012-05-04 09:04:23Z $
+ */
+
+
+#ifndef _wlioctl_h_
+#define _wlioctl_h_
+
+#include <typedefs.h>
+#include <proto/ethernet.h>
+#include <proto/bcmeth.h>
+#include <proto/bcmevent.h>
+#include <proto/802.11.h>
+#include <bcmwifi.h>
+
+#include <bcmcdc.h>
+
+#ifndef INTF_NAME_SIZ
+#define INTF_NAME_SIZ   16
+#endif
+
+
+typedef struct remote_ioctl {
+	cdc_ioctl_t     msg;
+	uint        data_len;
+	char            intf_name[INTF_NAME_SIZ];
+} rem_ioctl_t;
+#define REMOTE_SIZE sizeof(rem_ioctl_t)
+
+#define ACTION_FRAME_SIZE 1040
+
+typedef struct wl_action_frame {
+	struct ether_addr   da;
+	uint16          len;
+	uint32          packetId;
+	uint8           data[ACTION_FRAME_SIZE];
+} wl_action_frame_t;
+
+#define WL_WIFI_ACTION_FRAME_SIZE sizeof(struct wl_action_frame)
+
+typedef struct ssid_info
+{
+	uint8		ssid_len;	
+	uint8		ssid[32];	
+} ssid_info_t;
+
+typedef struct wl_af_params {
+	uint32          channel;
+	int32           dwell_time;
+	struct ether_addr   BSSID;
+	wl_action_frame_t   action_frame;
+} wl_af_params_t;
+
+#define WL_WIFI_AF_PARAMS_SIZE sizeof(struct wl_af_params)
+
+
+#define BWL_DEFAULT_PACKING
+#include <packed_section_start.h>
+
+
+
+
+
+#define LEGACY2_WL_BSS_INFO_VERSION 108     
+
+
+typedef struct wl_bss_info_108 {
+	uint32      version;        
+	uint32      length;         
+	struct ether_addr BSSID;
+	uint16      beacon_period;      
+	uint16      capability;     
+	uint8       SSID_len;
+	uint8       SSID[32];
+	struct {
+		uint    count;          
+		uint8   rates[16];      
+	} rateset;              
+	chanspec_t  chanspec;       
+	uint16      atim_window;        
+	uint8       dtim_period;        
+	int16       RSSI;           
+	int8        phy_noise;      
+
+	uint8       n_cap;          
+	uint32      nbss_cap;       
+	uint8       ctl_ch;         
+	uint32      reserved32[1];      
+	uint8       flags;          
+	uint8       reserved[3];        
+	uint8       basic_mcs[MCSSET_LEN];  
+
+	uint16      ie_offset;      
+	uint32      ie_length;      
+	
+	
+} wl_bss_info_108_t;
+
+#define WL_BSS_INFO_VERSION 109     
+
+
+typedef struct wl_bss_info {
+	uint32      version;        
+	uint32      length;         
+	struct ether_addr BSSID;
+	uint16      beacon_period;      
+	uint16      capability;     
+	uint8       SSID_len;
+	uint8       SSID[32];
+	struct {
+		uint    count;          
+		uint8   rates[16];      
+	} rateset;              
+	chanspec_t  chanspec;       
+	uint16      atim_window;        
+	uint8       dtim_period;        
+	int16       RSSI;           
+	int8        phy_noise;      
+
+	uint8       n_cap;          
+	uint32      nbss_cap;       
+	uint8       ctl_ch;         
+	uint32      reserved32[1];      
+	uint8       flags;          
+	uint8       reserved[3];        
+	uint8       basic_mcs[MCSSET_LEN];  
+
+	uint16      ie_offset;      
+	uint32      ie_length;      
+	int16       SNR;            
+	
+	
+} wl_bss_info_t;
+
+typedef struct wl_bsscfg {
+	uint32  wsec;
+	uint32  WPA_auth;
+	uint32  wsec_index;
+	uint32  associated;
+	uint32  BSS;
+	uint32  phytest_on;
+	struct ether_addr   prev_BSSID;
+	struct ether_addr   BSSID;
+} wl_bsscfg_t;
+
+typedef struct wl_bss_config {
+	uint32  atim_window;
+	uint32  beacon_period;
+	uint32  chanspec;
+} wl_bss_config_t;
+
+
+typedef struct wlc_ssid {
+	uint32      SSID_len;
+	uchar       SSID[32];
+} wlc_ssid_t;
+
+
+#define WL_BSSTYPE_INFRA 1
+#define WL_BSSTYPE_INDEP 0
+#define WL_BSSTYPE_ANY   2
+
+
+#define WL_SCANFLAGS_PASSIVE 0x01       
+#define WL_SCANFLAGS_RESERVED 0x02      
+#define WL_SCANFLAGS_PROHIBITED 0x04    
+
+#define WL_SCAN_PARAMS_SSID_MAX 	10
+
+typedef struct wl_scan_params {
+	wlc_ssid_t ssid;        
+	struct ether_addr bssid;    
+	int8 bss_type;          
+	uint8 scan_type;        
+	int32 nprobes;          
+	int32 active_time;      
+	int32 passive_time;     
+	int32 home_time;        
+	int32 channel_num;      
+	uint16 channel_list[1];     
+} wl_scan_params_t;
+
+
+#define WL_SCAN_PARAMS_FIXED_SIZE 64
+
+
+#define WL_SCAN_PARAMS_COUNT_MASK 0x0000ffff
+#define WL_SCAN_PARAMS_NSSID_SHIFT 16
+
+#define WL_SCAN_ACTION_START      1
+#define WL_SCAN_ACTION_CONTINUE   2
+#define WL_SCAN_ACTION_ABORT      3
+
+#define ISCAN_REQ_VERSION 1
+
+
+typedef struct wl_iscan_params {
+	uint32 version;
+	uint16 action;
+	uint16 scan_duration;
+	wl_scan_params_t params;
+} wl_iscan_params_t;
+
+
+#define WL_ISCAN_PARAMS_FIXED_SIZE (OFFSETOF(wl_iscan_params_t, params) + sizeof(wlc_ssid_t))
+
+typedef struct wl_scan_results {
+	uint32 buflen;
+	uint32 version;
+	uint32 count;
+	wl_bss_info_t bss_info[1];
+} wl_scan_results_t;
+
+
+#define WL_SCAN_RESULTS_FIXED_SIZE (sizeof(wl_scan_results_t) - sizeof(wl_bss_info_t))
+
+
+#define WL_SCAN_RESULTS_SUCCESS 0
+#define WL_SCAN_RESULTS_PARTIAL 1
+#define WL_SCAN_RESULTS_PENDING 2
+#define WL_SCAN_RESULTS_ABORTED 3
+#define WL_SCAN_RESULTS_NO_MEM  4
+
+
+#define DNGL_RXCTXT_SIZE	45
+
+#if defined(SIMPLE_ISCAN)
+#define ISCAN_RETRY_CNT   5
+#define ISCAN_STATE_IDLE   0
+#define ISCAN_STATE_SCANING 1
+#define ISCAN_STATE_PENDING 2
+
+
+#define WLC_IW_ISCAN_MAXLEN   2048
+typedef struct iscan_buf {
+	struct iscan_buf * next;
+	char   iscan_buf[WLC_IW_ISCAN_MAXLEN];
+} iscan_buf_t;
+#endif 
+
+#define ESCAN_REQ_VERSION 1
+
+typedef struct wl_escan_params {
+	uint32 version;
+	uint16 action;
+	uint16 sync_id;
+	wl_scan_params_t params;
+} wl_escan_params_t;
+
+#define WL_ESCAN_PARAMS_FIXED_SIZE (OFFSETOF(wl_escan_params_t, params) + sizeof(wlc_ssid_t))
+
+typedef struct wl_escan_result {
+	uint32 buflen;
+	uint32 version;
+	uint16 sync_id;
+	uint16 bss_count;
+	wl_bss_info_t bss_info[1];
+} wl_escan_result_t;
+
+#define WL_ESCAN_RESULTS_FIXED_SIZE (sizeof(wl_escan_result_t) - sizeof(wl_bss_info_t))
+
+
+typedef struct wl_iscan_results {
+	uint32 status;
+	wl_scan_results_t results;
+} wl_iscan_results_t;
+
+
+#define WL_ISCAN_RESULTS_FIXED_SIZE \
+	(WL_SCAN_RESULTS_FIXED_SIZE + OFFSETOF(wl_iscan_results_t, results))
+
+typedef struct wl_probe_params {
+	wlc_ssid_t ssid;
+	struct ether_addr bssid;
+	struct ether_addr mac;
+} wl_probe_params_t;
+
+#define WL_NUMRATES     16  
+typedef struct wl_rateset {
+	uint32  count;          
+	uint8   rates[WL_NUMRATES]; 
+} wl_rateset_t;
+
+typedef struct wl_rateset_args {
+	uint32  count;          
+	uint8   rates[WL_NUMRATES]; 
+	uint8   mcs[MCSSET_LEN];        
+} wl_rateset_args_t;
+
+
+typedef struct wl_uint32_list {
+	
+	uint32 count;
+	
+	uint32 element[1];
+} wl_uint32_list_t;
+
+
+typedef struct wl_assoc_params {
+	struct ether_addr bssid;    
+	uint16 bssid_cnt;       
+	int32 chanspec_num;     
+	chanspec_t chanspec_list[1];    
+} wl_assoc_params_t;
+#define WL_ASSOC_PARAMS_FIXED_SIZE  (sizeof(wl_assoc_params_t) - sizeof(chanspec_t))
+
+
+typedef wl_assoc_params_t wl_reassoc_params_t;
+#define WL_REASSOC_PARAMS_FIXED_SIZE    WL_ASSOC_PARAMS_FIXED_SIZE
+
+
+typedef wl_assoc_params_t wl_join_assoc_params_t;
+#define WL_JOIN_ASSOC_PARAMS_FIXED_SIZE        WL_ASSOC_PARAMS_FIXED_SIZE
+
+
+typedef struct wl_join_params {
+	wlc_ssid_t ssid;
+	wl_assoc_params_t params;   
+} wl_join_params_t;
+#define WL_JOIN_PARAMS_FIXED_SIZE   (sizeof(wl_join_params_t) - sizeof(chanspec_t))
+
+
+typedef struct wl_join_scan_params {
+	uint8 scan_type;                
+	int32 nprobes;                  
+	int32 active_time;              
+	int32 passive_time;             
+	int32 home_time;                
+} wl_join_scan_params_t;
+
+
+typedef struct wl_extjoin_params {
+	wlc_ssid_t ssid;                
+	wl_join_scan_params_t scan;
+	wl_join_assoc_params_t assoc;   
+} wl_extjoin_params_t;
+#define WL_EXTJOIN_PARAMS_FIXED_SIZE    (sizeof(wl_extjoin_params_t) - sizeof(chanspec_t))
+
+typedef struct {
+	uint32 num;
+	chanspec_t list[1];
+} chanspec_list_t;
+
+
+#define NRATE_MCS_INUSE 0x00000080  
+#define NRATE_RATE_MASK 0x0000007f  
+#define NRATE_STF_MASK  0x0000ff00  
+#define NRATE_STF_SHIFT 8       
+#define NRATE_OVERRIDE  0x80000000  
+#define NRATE_OVERRIDE_MCS_ONLY 0x40000000 
+#define NRATE_SGI_MASK  0x00800000      
+#define NRATE_SGI_SHIFT 23              
+#define NRATE_LDPC_CODING 0x00400000    
+#define NRATE_LDPC_SHIFT 22             
+#define NRATE_BCMC_OVERRIDE 0x00200000    
+#define NRATE_BCMC_SHIFT 21             
+
+#define NRATE_STF_SISO  0       
+#define NRATE_STF_CDD   1       
+#define NRATE_STF_STBC  2       
+#define NRATE_STF_SDM   3       
+
+#define ANTENNA_NUM_1   1       
+#define ANTENNA_NUM_2   2
+#define ANTENNA_NUM_3   3
+#define ANTENNA_NUM_4   4
+
+#define ANT_SELCFG_AUTO     0x80    
+#define ANT_SELCFG_MASK     0x33    
+#define ANT_SELCFG_MAX      4   
+#define ANT_SELCFG_TX_UNICAST   0   
+#define ANT_SELCFG_RX_UNICAST   1   
+#define ANT_SELCFG_TX_DEF   2   
+#define ANT_SELCFG_RX_DEF   3   
+
+#define MAX_STREAMS_SUPPORTED   4   
+
+typedef struct {
+	uint8 ant_config[ANT_SELCFG_MAX];   
+	uint8 num_antcfg;   
+} wlc_antselcfg_t;
+
+#define HIGHEST_SINGLE_STREAM_MCS   7 
+
+#define MAX_CCA_CHANNELS 38 
+#define MAX_CCA_SECS     60 
+
+#define IBSS_MED        15  
+#define IBSS_HI         25  
+#define OBSS_MED        12
+#define OBSS_HI         25
+#define INTERFER_MED    5
+#define INTERFER_HI     10
+
+#define  CCA_FLAG_2G_ONLY       0x01    
+#define  CCA_FLAG_5G_ONLY       0x02    
+#define  CCA_FLAG_IGNORE_DURATION   0x04    
+#define  CCA_FLAGS_PREFER_1_6_11    0x10
+#define  CCA_FLAG_IGNORE_INTERFER   0x20 
+
+#define CCA_ERRNO_BAND      1   
+#define CCA_ERRNO_DURATION  2   
+#define CCA_ERRNO_PREF_CHAN 3   
+#define CCA_ERRNO_INTERFER  4   
+#define CCA_ERRNO_TOO_FEW   5   
+
+typedef struct {
+	uint32 duration;    
+	uint32 congest_ibss;    
+				
+	uint32 congest_obss;    
+	uint32 interference;    
+	uint32 timestamp;   
+} cca_congest_t;
+
+typedef struct {
+	chanspec_t chanspec;    
+	uint8 num_secs;     
+	cca_congest_t  secs[1]; 
+} cca_congest_channel_req_t;
+
+#define WLC_CNTRY_BUF_SZ    4       
+
+typedef struct wl_country {
+	char country_abbrev[WLC_CNTRY_BUF_SZ];  
+	int32 rev;              
+	char ccode[WLC_CNTRY_BUF_SZ];       
+} wl_country_t;
+
+typedef struct wl_channels_in_country {
+	uint32 buflen;
+	uint32 band;
+	char country_abbrev[WLC_CNTRY_BUF_SZ];
+	uint32 count;
+	uint32 channel[1];
+} wl_channels_in_country_t;
+
+typedef struct wl_country_list {
+	uint32 buflen;
+	uint32 band_set;
+	uint32 band;
+	uint32 count;
+	char country_abbrev[1];
+} wl_country_list_t;
+
+#define WL_NUM_RPI_BINS     8
+#define WL_RM_TYPE_BASIC    1
+#define WL_RM_TYPE_CCA      2
+#define WL_RM_TYPE_RPI      3
+
+#define WL_RM_FLAG_PARALLEL (1<<0)
+
+#define WL_RM_FLAG_LATE     (1<<1)
+#define WL_RM_FLAG_INCAPABLE    (1<<2)
+#define WL_RM_FLAG_REFUSED  (1<<3)
+
+typedef struct wl_rm_req_elt {
+	int8    type;
+	int8    flags;
+	chanspec_t  chanspec;
+	uint32  token;      
+	uint32  tsf_h;      
+	uint32  tsf_l;      
+	uint32  dur;        
+} wl_rm_req_elt_t;
+
+typedef struct wl_rm_req {
+	uint32  token;      
+	uint32  count;      
+	void    *cb;        
+	void    *cb_arg;    
+	wl_rm_req_elt_t req[1]; 
+} wl_rm_req_t;
+#define WL_RM_REQ_FIXED_LEN OFFSETOF(wl_rm_req_t, req)
+
+typedef struct wl_rm_rep_elt {
+	int8    type;
+	int8    flags;
+	chanspec_t  chanspec;
+	uint32  token;      
+	uint32  tsf_h;      
+	uint32  tsf_l;      
+	uint32  dur;        
+	uint32  len;        
+	uint8   data[1];    
+} wl_rm_rep_elt_t;
+#define WL_RM_REP_ELT_FIXED_LEN 24  
+
+#define WL_RPI_REP_BIN_NUM 8
+typedef struct wl_rm_rpi_rep {
+	uint8   rpi[WL_RPI_REP_BIN_NUM];
+	int8    rpi_max[WL_RPI_REP_BIN_NUM];
+} wl_rm_rpi_rep_t;
+
+typedef struct wl_rm_rep {
+	uint32  token;      
+	uint32  len;        
+	wl_rm_rep_elt_t rep[1]; 
+} wl_rm_rep_t;
+#define WL_RM_REP_FIXED_LEN 8
+
+
+typedef enum sup_auth_status {
+	
+	WLC_SUP_DISCONNECTED = 0,
+	WLC_SUP_CONNECTING,
+	WLC_SUP_IDREQUIRED,
+	WLC_SUP_AUTHENTICATING,
+	WLC_SUP_AUTHENTICATED,
+	WLC_SUP_KEYXCHANGE,
+	WLC_SUP_KEYED,
+	WLC_SUP_TIMEOUT,
+	WLC_SUP_LAST_BASIC_STATE,
+
+	
+	
+	WLC_SUP_KEYXCHANGE_WAIT_M1 = WLC_SUP_AUTHENTICATED,
+	
+	WLC_SUP_KEYXCHANGE_PREP_M2 = WLC_SUP_KEYXCHANGE,
+	
+	WLC_SUP_KEYXCHANGE_WAIT_M3 = WLC_SUP_LAST_BASIC_STATE,
+	WLC_SUP_KEYXCHANGE_PREP_M4, 
+	WLC_SUP_KEYXCHANGE_WAIT_G1, 
+	WLC_SUP_KEYXCHANGE_PREP_G2  
+} sup_auth_status_t;
+
+
+#define CRYPTO_ALGO_OFF         0
+#define CRYPTO_ALGO_WEP1        1
+#define CRYPTO_ALGO_TKIP        2
+#define CRYPTO_ALGO_WEP128      3
+#define CRYPTO_ALGO_AES_CCM     4
+#define CRYPTO_ALGO_AES_OCB_MSDU    5
+#define CRYPTO_ALGO_AES_OCB_MPDU    6
+#define CRYPTO_ALGO_NALG        7
+#define CRYPTO_ALGO_PMK			12	
+
+#define WSEC_GEN_MIC_ERROR  0x0001
+#define WSEC_GEN_REPLAY     0x0002
+#define WSEC_GEN_ICV_ERROR  0x0004
+
+#define WL_SOFT_KEY (1 << 0)    
+#define WL_PRIMARY_KEY  (1 << 1)    
+#define WL_KF_RES_4 (1 << 4)    
+#define WL_KF_RES_5 (1 << 5)    
+#define WL_IBSS_PEER_GROUP_KEY  (1 << 6)    
+
+typedef struct wl_wsec_key {
+	uint32      index;      
+	uint32      len;        
+	uint8       data[DOT11_MAX_KEY_SIZE];   
+	uint32      pad_1[18];
+	uint32      algo;       
+	uint32      flags;      
+	uint32      pad_2[2];
+	int     pad_3;
+	int     iv_initialized; 
+	int     pad_4;
+	
+	struct {
+		uint32  hi;     
+		uint16  lo;     
+	} rxiv;
+	uint32      pad_5[2];
+	struct ether_addr ea;       
+} wl_wsec_key_t;
+
+#define WSEC_MIN_PSK_LEN    8
+#define WSEC_MAX_PSK_LEN    64
+
+
+#define WSEC_PASSPHRASE     (1<<0)
+
+
+typedef struct {
+	ushort  key_len;        
+	ushort  flags;          
+	uint8   key[WSEC_MAX_PSK_LEN];  
+} wsec_pmk_t;
+
+
+#define WEP_ENABLED     0x0001
+#define TKIP_ENABLED        0x0002
+#define AES_ENABLED     0x0004
+#define WSEC_SWFLAG     0x0008
+#define SES_OW_ENABLED      0x0040  
+
+
+#define WPA_AUTH_DISABLED   0x0000  
+#define WPA_AUTH_NONE       0x0001  
+#define WPA_AUTH_UNSPECIFIED    0x0002  
+#define WPA_AUTH_PSK        0x0004  
+ 
+#define WPA2_AUTH_UNSPECIFIED   0x0040  
+#define WPA2_AUTH_PSK       0x0080  
+#define BRCM_AUTH_PSK           0x0100  
+#define BRCM_AUTH_DPT       0x0200  
+#define WPA2_AUTH_MFP           0x1000  
+#define WPA2_AUTH_TPK		0x2000 	
+#define WPA2_AUTH_FT		0x4000 	
+
+
+#define MAXPMKID        16
+
+typedef struct _pmkid {
+	struct ether_addr   BSSID;
+	uint8           PMKID[WPA2_PMKID_LEN];
+} pmkid_t;
+
+typedef struct _pmkid_list {
+	uint32  npmkid;
+	pmkid_t pmkid[1];
+} pmkid_list_t;
+
+typedef struct _pmkid_cand {
+	struct ether_addr   BSSID;
+	uint8           preauth;
+} pmkid_cand_t;
+
+typedef struct _pmkid_cand_list {
+	uint32  npmkid_cand;
+	pmkid_cand_t    pmkid_cand[1];
+} pmkid_cand_list_t;
+
+typedef struct wl_assoc_info {
+	uint32      req_len;
+	uint32      resp_len;
+	uint32      flags;
+	struct dot11_assoc_req req;
+	struct ether_addr reassoc_bssid;	
+	struct dot11_assoc_resp resp;
+} wl_assoc_info_t;
+
+
+#define WLC_ASSOC_REQ_IS_REASSOC 0x01	
+
+
+typedef struct {
+	uint16          ver;        
+	uint16          len;        
+	uint16          cap;        
+	uint32          flags;      
+	uint32          idle;       
+	struct ether_addr   ea;     
+	wl_rateset_t        rateset;    
+	uint32          in;     
+	uint32          listen_interval_inms; 
+	uint32          tx_pkts;    
+	uint32          tx_failures;    
+	uint32          rx_ucast_pkts;  
+	uint32          rx_mcast_pkts;  
+	uint32          tx_rate;    
+	uint32          rx_rate;    
+	uint32          rx_decrypt_succeeds;    
+	uint32          rx_decrypt_failures;    
+} sta_info_t;
+
+#define WL_OLD_STAINFO_SIZE OFFSETOF(sta_info_t, tx_pkts)
+
+#define WL_STA_VER      3
+
+
+#define WL_STA_BRCM     0x1     
+#define WL_STA_WME      0x2     
+#define WL_STA_ABCAP        0x4
+#define WL_STA_AUTHE        0x8     
+#define WL_STA_ASSOC        0x10        
+#define WL_STA_AUTHO        0x20        
+#define WL_STA_WDS      0x40        
+#define WL_STA_WDS_LINKUP   0x80        
+#define WL_STA_PS       0x100       
+#define WL_STA_APSD_BE      0x200       
+#define WL_STA_APSD_BK      0x400       
+#define WL_STA_APSD_VI      0x800       
+#define WL_STA_APSD_VO      0x1000      
+#define WL_STA_N_CAP        0x2000      
+#define WL_STA_SCBSTATS     0x4000      
+
+#define WL_WDS_LINKUP       WL_STA_WDS_LINKUP   
+
+
+#define WLC_TXFILTER_OVERRIDE_DISABLED  0
+#define WLC_TXFILTER_OVERRIDE_ENABLED   1
+
+
+typedef struct {
+	uint32  val;
+	struct ether_addr ea;
+} scb_val_t;
+
+
+typedef struct {
+	uint32 code;
+	scb_val_t ioctl_args;
+} authops_t;
+
+
+typedef struct channel_info {
+	int hw_channel;
+	int target_channel;
+	int scan_channel;
+} channel_info_t;
+
+
+struct maclist {
+	uint count;         
+	struct ether_addr ea[1];    
+};
+
+
+typedef struct get_pktcnt {
+	uint rx_good_pkt;
+	uint rx_bad_pkt;
+	uint tx_good_pkt;
+	uint tx_bad_pkt;
+	uint rx_ocast_good_pkt; 
+} get_pktcnt_t;
+
+#define WL_IOCTL_ACTION_GET				0x0
+#define WL_IOCTL_ACTION_SET				0x1
+#define WL_IOCTL_ACTION_OVL_IDX_MASK	0x1e
+#define WL_IOCTL_ACTION_OVL_RSV			0x20
+#define WL_IOCTL_ACTION_OVL				0x40
+#define WL_IOCTL_ACTION_MASK			0x7e
+#define WL_IOCTL_ACTION_OVL_SHIFT		1
+
+
+typedef struct wl_ioctl {
+	uint cmd;   
+	void *buf;  
+	uint len;   
+	uint8 set;		
+	uint used;  
+	uint needed;    
+} wl_ioctl_t;
+
+
+#define ioctl_subtype   set     
+#define ioctl_pid   used        
+#define ioctl_status    needed      
+
+
+typedef struct wlc_rev_info {
+	uint        vendorid;   
+	uint        deviceid;   
+	uint        radiorev;   
+	uint        chiprev;    
+	uint        corerev;    
+	uint        boardid;    
+	uint        boardvendor;    
+	uint        boardrev;   
+	uint        driverrev;  
+	uint        ucoderev;   
+	uint        bus;        
+	uint        chipnum;    
+	uint        phytype;    
+	uint        phyrev;     
+	uint        anarev;     
+	uint        chippkg;    
+} wlc_rev_info_t;
+
+#define WL_REV_INFO_LEGACY_LENGTH   48
+
+#define WL_BRAND_MAX 10
+typedef struct wl_instance_info {
+	uint instance;
+	char brand[WL_BRAND_MAX];
+} wl_instance_info_t;
+
+
+typedef struct wl_txfifo_sz {
+	uint16  magic;
+	uint16  fifo;
+	uint16  size;
+} wl_txfifo_sz_t;
+
+#define WL_TXFIFO_SZ_MAGIC  0xa5a5
+
+
+
+#define WLC_IOV_NAME_LEN 30
+typedef struct wlc_iov_trx_s {
+	uint8 module;
+	uint8 type;
+	char name[WLC_IOV_NAME_LEN];
+} wlc_iov_trx_t;
+
+
+#define WLC_IOCTL_MAGIC     0x14e46c77
+
+
+#define WLC_IOCTL_VERSION   1
+
+#define WLC_IOCTL_MAXLEN        8192    
+#define WLC_IOCTL_SMLEN         256 
+#define WLC_IOCTL_MEDLEN        1536    
+#ifdef WLC_HIGH_ONLY
+#define WLC_SAMPLECOLLECT_MAXLEN    1024    
+#define WLC_SAMPLECOLLECT_MAXLEN_LCN40  1024
+#else
+#if defined(LCNCONF) || defined(LCN40CONF)
+#define WLC_SAMPLECOLLECT_MAXLEN	8192	
+#else
+#define WLC_SAMPLECOLLECT_MAXLEN	10240	
+#endif
+#define WLC_SAMPLECOLLECT_MAXLEN_LCN40  8192
+#endif 
+
+
+#define WLC_GET_MAGIC               0
+#define WLC_GET_VERSION             1
+#define WLC_UP                  2
+#define WLC_DOWN                3
+#define WLC_GET_LOOP                4
+#define WLC_SET_LOOP                5
+#define WLC_DUMP                6
+#define WLC_GET_MSGLEVEL            7
+#define WLC_SET_MSGLEVEL            8
+#define WLC_GET_PROMISC             9
+#define WLC_SET_PROMISC             10
+#define WLC_OVERLAY_IOCTL           11
+#define WLC_GET_RATE                12
+ 
+#define WLC_GET_INSTANCE            14
+ 
+ 
+ 
+ 
+#define WLC_GET_INFRA               19
+#define WLC_SET_INFRA               20
+#define WLC_GET_AUTH                21
+#define WLC_SET_AUTH                22
+#define WLC_GET_BSSID               23
+#define WLC_SET_BSSID               24
+#define WLC_GET_SSID                25
+#define WLC_SET_SSID                26
+#define WLC_RESTART             27
+#define WLC_TERMINATED             28
+ 
+#define WLC_GET_CHANNEL             29
+#define WLC_SET_CHANNEL             30
+#define WLC_GET_SRL             31
+#define WLC_SET_SRL             32
+#define WLC_GET_LRL             33
+#define WLC_SET_LRL             34
+#define WLC_GET_PLCPHDR             35
+#define WLC_SET_PLCPHDR             36
+#define WLC_GET_RADIO               37
+#define WLC_SET_RADIO               38
+#define WLC_GET_PHYTYPE             39
+#define WLC_DUMP_RATE               40
+#define WLC_SET_RATE_PARAMS         41
+#define WLC_GET_FIXRATE             42
+#define WLC_SET_FIXRATE             43
+ 
+ 
+#define WLC_GET_KEY             44
+#define WLC_SET_KEY             45
+#define WLC_GET_REGULATORY          46
+#define WLC_SET_REGULATORY          47
+#define WLC_GET_PASSIVE_SCAN            48
+#define WLC_SET_PASSIVE_SCAN            49
+#define WLC_SCAN                50
+#define WLC_SCAN_RESULTS            51
+#define WLC_DISASSOC                52
+#define WLC_REASSOC             53
+#define WLC_GET_ROAM_TRIGGER            54
+#define WLC_SET_ROAM_TRIGGER            55
+#define WLC_GET_ROAM_DELTA          56
+#define WLC_SET_ROAM_DELTA          57
+#define WLC_GET_ROAM_SCAN_PERIOD        58
+#define WLC_SET_ROAM_SCAN_PERIOD        59
+#define WLC_EVM                 60  
+#define WLC_GET_TXANT               61
+#define WLC_SET_TXANT               62
+#define WLC_GET_ANTDIV              63
+#define WLC_SET_ANTDIV              64
+ 
+ 
+#define WLC_GET_CLOSED              67
+#define WLC_SET_CLOSED              68
+#define WLC_GET_MACLIST             69
+#define WLC_SET_MACLIST             70
+#define WLC_GET_RATESET             71
+#define WLC_SET_RATESET             72
+ 
+#define WLC_LONGTRAIN               74
+#define WLC_GET_BCNPRD              75
+#define WLC_SET_BCNPRD              76
+#define WLC_GET_DTIMPRD             77
+#define WLC_SET_DTIMPRD             78
+#define WLC_GET_SROM                79
+#define WLC_SET_SROM                80
+#define WLC_GET_WEP_RESTRICT            81
+#define WLC_SET_WEP_RESTRICT            82
+#define WLC_GET_COUNTRY             83
+#define WLC_SET_COUNTRY             84
+#define WLC_GET_PM              85
+#define WLC_SET_PM              86
+#define WLC_GET_WAKE                87
+#define WLC_SET_WAKE                88
+ 
+#define WLC_GET_FORCELINK           90  
+#define WLC_SET_FORCELINK           91  
+#define WLC_FREQ_ACCURACY           92  
+#define WLC_CARRIER_SUPPRESS            93  
+#define WLC_GET_PHYREG              94
+#define WLC_SET_PHYREG              95
+#define WLC_GET_RADIOREG            96
+#define WLC_SET_RADIOREG            97
+#define WLC_GET_REVINFO             98
+#define WLC_GET_UCANTDIV            99
+#define WLC_SET_UCANTDIV            100
+#define WLC_R_REG               101
+#define WLC_W_REG               102
+
+ 
+#define WLC_GET_MACMODE             105
+#define WLC_SET_MACMODE             106
+#define WLC_GET_MONITOR             107
+#define WLC_SET_MONITOR             108
+#define WLC_GET_GMODE               109
+#define WLC_SET_GMODE               110
+#define WLC_GET_LEGACY_ERP          111
+#define WLC_SET_LEGACY_ERP          112
+#define WLC_GET_RX_ANT              113
+#define WLC_GET_CURR_RATESET            114 
+#define WLC_GET_SCANSUPPRESS            115
+#define WLC_SET_SCANSUPPRESS            116
+#define WLC_GET_AP              117
+#define WLC_SET_AP              118
+#define WLC_GET_EAP_RESTRICT            119
+#define WLC_SET_EAP_RESTRICT            120
+#define WLC_SCB_AUTHORIZE           121
+#define WLC_SCB_DEAUTHORIZE         122
+#define WLC_GET_WDSLIST             123
+#define WLC_SET_WDSLIST             124
+#define WLC_GET_ATIM                125
+#define WLC_SET_ATIM                126
+#define WLC_GET_RSSI                127
+#define WLC_GET_PHYANTDIV           128
+#define WLC_SET_PHYANTDIV           129
+#define WLC_AP_RX_ONLY              130
+#define WLC_GET_TX_PATH_PWR         131
+#define WLC_SET_TX_PATH_PWR         132
+#define WLC_GET_WSEC                133
+#define WLC_SET_WSEC                134
+#define WLC_GET_PHY_NOISE           135
+#define WLC_GET_BSS_INFO            136
+#define WLC_GET_PKTCNTS             137
+#define WLC_GET_LAZYWDS             138
+#define WLC_SET_LAZYWDS             139
+#define WLC_GET_BANDLIST            140
+#define WLC_GET_BAND                141
+#define WLC_SET_BAND                142
+#define WLC_SCB_DEAUTHENTICATE          143
+#define WLC_GET_SHORTSLOT           144
+#define WLC_GET_SHORTSLOT_OVERRIDE      145
+#define WLC_SET_SHORTSLOT_OVERRIDE      146
+#define WLC_GET_SHORTSLOT_RESTRICT      147
+#define WLC_SET_SHORTSLOT_RESTRICT      148
+#define WLC_GET_GMODE_PROTECTION        149
+#define WLC_GET_GMODE_PROTECTION_OVERRIDE   150
+#define WLC_SET_GMODE_PROTECTION_OVERRIDE   151
+#define WLC_UPGRADE             152
+ 
+ 
+#define WLC_GET_IGNORE_BCNS         155
+#define WLC_SET_IGNORE_BCNS         156
+#define WLC_GET_SCB_TIMEOUT         157
+#define WLC_SET_SCB_TIMEOUT         158
+#define WLC_GET_ASSOCLIST           159
+#define WLC_GET_CLK             160
+#define WLC_SET_CLK             161
+#define WLC_GET_UP              162
+#define WLC_OUT                 163
+#define WLC_GET_WPA_AUTH            164
+#define WLC_SET_WPA_AUTH            165
+#define WLC_GET_UCFLAGS             166
+#define WLC_SET_UCFLAGS             167
+#define WLC_GET_PWRIDX              168
+#define WLC_SET_PWRIDX              169
+#define WLC_GET_TSSI                170
+#define WLC_GET_SUP_RATESET_OVERRIDE        171
+#define WLC_SET_SUP_RATESET_OVERRIDE        172
+ 
+ 
+ 
+ 
+ 
+#define WLC_GET_PROTECTION_CONTROL      178
+#define WLC_SET_PROTECTION_CONTROL      179
+#define WLC_GET_PHYLIST             180
+#define WLC_ENCRYPT_STRENGTH            181 
+#define WLC_DECRYPT_STATUS          182 
+#define WLC_GET_KEY_SEQ             183
+#define WLC_GET_SCAN_CHANNEL_TIME       184
+#define WLC_SET_SCAN_CHANNEL_TIME       185
+#define WLC_GET_SCAN_UNASSOC_TIME       186
+#define WLC_SET_SCAN_UNASSOC_TIME       187
+#define WLC_GET_SCAN_HOME_TIME          188
+#define WLC_SET_SCAN_HOME_TIME          189
+#define WLC_GET_SCAN_NPROBES            190
+#define WLC_SET_SCAN_NPROBES            191
+#define WLC_GET_PRB_RESP_TIMEOUT        192
+#define WLC_SET_PRB_RESP_TIMEOUT        193
+#define WLC_GET_ATTEN               194
+#define WLC_SET_ATTEN               195
+#define WLC_GET_SHMEM               196 
+#define WLC_SET_SHMEM               197 
+ 
+ 
+#define WLC_SET_WSEC_TEST           200
+#define WLC_SCB_DEAUTHENTICATE_FOR_REASON   201
+#define WLC_TKIP_COUNTERMEASURES        202
+#define WLC_GET_PIOMODE             203
+#define WLC_SET_PIOMODE             204
+#define WLC_SET_ASSOC_PREFER            205
+#define WLC_GET_ASSOC_PREFER            206
+#define WLC_SET_ROAM_PREFER         207
+#define WLC_GET_ROAM_PREFER         208
+#define WLC_SET_LED             209
+#define WLC_GET_LED             210
+#define WLC_GET_INTERFERENCE_MODE       211
+#define WLC_SET_INTERFERENCE_MODE       212
+#define WLC_GET_CHANNEL_QA          213
+#define WLC_START_CHANNEL_QA            214
+#define WLC_GET_CHANNEL_SEL         215
+#define WLC_START_CHANNEL_SEL           216
+#define WLC_GET_VALID_CHANNELS          217
+#define WLC_GET_FAKEFRAG            218
+#define WLC_SET_FAKEFRAG            219
+#define WLC_GET_PWROUT_PERCENTAGE       220
+#define WLC_SET_PWROUT_PERCENTAGE       221
+#define WLC_SET_BAD_FRAME_PREEMPT       222
+#define WLC_GET_BAD_FRAME_PREEMPT       223
+#define WLC_SET_LEAP_LIST           224
+#define WLC_GET_LEAP_LIST           225
+#define WLC_GET_CWMIN               226
+#define WLC_SET_CWMIN               227
+#define WLC_GET_CWMAX               228
+#define WLC_SET_CWMAX               229
+#define WLC_GET_WET             230
+#define WLC_SET_WET             231
+#define WLC_GET_PUB             232
+ 
+ 
+#define WLC_GET_KEY_PRIMARY         235
+#define WLC_SET_KEY_PRIMARY         236
+ 
+#define WLC_GET_ACI_ARGS            238
+#define WLC_SET_ACI_ARGS            239
+#define WLC_UNSET_CALLBACK          240
+#define WLC_SET_CALLBACK            241
+#define WLC_GET_RADAR               242
+#define WLC_SET_RADAR               243
+#define WLC_SET_SPECT_MANAGMENT         244
+#define WLC_GET_SPECT_MANAGMENT         245
+#define WLC_WDS_GET_REMOTE_HWADDR       246 
+#define WLC_WDS_GET_WPA_SUP         247
+#define WLC_SET_CS_SCAN_TIMER           248
+#define WLC_GET_CS_SCAN_TIMER           249
+#define WLC_MEASURE_REQUEST         250
+#define WLC_INIT                251
+#define WLC_SEND_QUIET              252
+#define WLC_KEEPALIVE           253
+#define WLC_SEND_PWR_CONSTRAINT         254
+#define WLC_UPGRADE_STATUS          255
+#define WLC_CURRENT_PWR             256
+#define WLC_GET_SCAN_PASSIVE_TIME       257
+#define WLC_SET_SCAN_PASSIVE_TIME       258
+#define WLC_LEGACY_LINK_BEHAVIOR        259
+#define WLC_GET_CHANNELS_IN_COUNTRY     260
+#define WLC_GET_COUNTRY_LIST            261
+#define WLC_GET_VAR             262 
+#define WLC_SET_VAR             263 
+#define WLC_NVRAM_GET               264 
+#define WLC_NVRAM_SET               265
+#define WLC_NVRAM_DUMP              266
+#define WLC_REBOOT              267
+#define WLC_SET_WSEC_PMK            268
+#define WLC_GET_AUTH_MODE           269
+#define WLC_SET_AUTH_MODE           270
+#define WLC_GET_WAKEENTRY           271
+#define WLC_SET_WAKEENTRY           272
+#define WLC_NDCONFIG_ITEM           273 
+#define WLC_NVOTPW              274
+#define WLC_OTPW                275
+#define WLC_IOV_BLOCK_GET           276
+#define WLC_IOV_MODULES_GET         277
+#define WLC_SOFT_RESET              278
+#define WLC_GET_ALLOW_MODE          279
+#define WLC_SET_ALLOW_MODE          280
+#define WLC_GET_DESIRED_BSSID           281
+#define WLC_SET_DESIRED_BSSID           282
+#define WLC_DISASSOC_MYAP           283
+#define WLC_GET_NBANDS              284 
+#define WLC_GET_BANDSTATES          285 
+#define WLC_GET_WLC_BSS_INFO            286 
+#define WLC_GET_ASSOC_INFO          287 
+#define WLC_GET_OID_PHY             288 
+#define WLC_SET_OID_PHY             289 
+#define WLC_SET_ASSOC_TIME          290 
+#define WLC_GET_DESIRED_SSID            291 
+#define WLC_GET_CHANSPEC            292 
+#define WLC_GET_ASSOC_STATE         293 
+#define WLC_SET_PHY_STATE           294 
+#define WLC_GET_SCAN_PENDING            295 
+#define WLC_GET_SCANREQ_PENDING         296 
+#define WLC_GET_PREV_ROAM_REASON        297 
+#define WLC_SET_PREV_ROAM_REASON        298 
+#define WLC_GET_BANDSTATES_PI           299 
+#define WLC_GET_PHY_STATE           300 
+#define WLC_GET_BSS_WPA_RSN         301 
+#define WLC_GET_BSS_WPA2_RSN            302 
+#define WLC_GET_BSS_BCN_TS          303 
+#define WLC_GET_INT_DISASSOC            304 
+#define WLC_SET_NUM_PEERS           305     
+#define WLC_GET_NUM_BSS             306 
+#define WLC_NPHY_SAMPLE_COLLECT         307 
+#define WLC_UM_PRIV             308 
+#define WLC_GET_CMD             309
+  
+#define WLC_SET_INTERFERENCE_OVERRIDE_MODE  311 
+#define WLC_GET_INTERFERENCE_OVERRIDE_MODE  312 
+#define WLC_GET_WAI_RESTRICT            313 
+#define WLC_SET_WAI_RESTRICT            314 
+#define WLC_SET_WAI_REKEY           315 
+#define WLC_SET_PEAKRATE            316 
+#define WLC_GET_PEAKRATE            317
+#define WLC_LAST                318
+
+#ifndef EPICTRL_COOKIE
+#define EPICTRL_COOKIE      0xABADCEDE
+#endif
+
+
+#define CMN_IOCTL_OFF 0x180
+
+
+
+
+#define WL_OID_BASE     0xFFE41420
+
+
+#define OID_WL_GETINSTANCE  (WL_OID_BASE + WLC_GET_INSTANCE)
+#define OID_WL_GET_FORCELINK    (WL_OID_BASE + WLC_GET_FORCELINK)
+#define OID_WL_SET_FORCELINK    (WL_OID_BASE + WLC_SET_FORCELINK)
+#define OID_WL_ENCRYPT_STRENGTH (WL_OID_BASE + WLC_ENCRYPT_STRENGTH)
+#define OID_WL_DECRYPT_STATUS   (WL_OID_BASE + WLC_DECRYPT_STATUS)
+#define OID_LEGACY_LINK_BEHAVIOR (WL_OID_BASE + WLC_LEGACY_LINK_BEHAVIOR)
+#define OID_WL_NDCONFIG_ITEM    (WL_OID_BASE + WLC_NDCONFIG_ITEM)
+
+
+#define OID_STA_CHANSPEC    (WL_OID_BASE + WLC_GET_CHANSPEC)
+#define OID_STA_NBANDS      (WL_OID_BASE + WLC_GET_NBANDS)
+#define OID_STA_GET_PHY     (WL_OID_BASE + WLC_GET_OID_PHY)
+#define OID_STA_SET_PHY     (WL_OID_BASE + WLC_SET_OID_PHY)
+#define OID_STA_ASSOC_TIME  (WL_OID_BASE + WLC_SET_ASSOC_TIME)
+#define OID_STA_DESIRED_SSID    (WL_OID_BASE + WLC_GET_DESIRED_SSID)
+#define OID_STA_SET_PHY_STATE   (WL_OID_BASE + WLC_SET_PHY_STATE)
+#define OID_STA_SCAN_PENDING    (WL_OID_BASE + WLC_GET_SCAN_PENDING)
+#define OID_STA_SCANREQ_PENDING (WL_OID_BASE + WLC_GET_SCANREQ_PENDING)
+#define OID_STA_GET_ROAM_REASON (WL_OID_BASE + WLC_GET_PREV_ROAM_REASON)
+#define OID_STA_SET_ROAM_REASON (WL_OID_BASE + WLC_SET_PREV_ROAM_REASON)
+#define OID_STA_GET_PHY_STATE   (WL_OID_BASE + WLC_GET_PHY_STATE)
+#define OID_STA_INT_DISASSOC    (WL_OID_BASE + WLC_GET_INT_DISASSOC)
+#define OID_STA_SET_NUM_PEERS   (WL_OID_BASE + WLC_SET_NUM_PEERS)
+#define OID_STA_GET_NUM_BSS (WL_OID_BASE + WLC_GET_NUM_BSS)
+
+#define WL_DECRYPT_STATUS_SUCCESS   1
+#define WL_DECRYPT_STATUS_FAILURE   2
+#define WL_DECRYPT_STATUS_UNKNOWN   3
+
+
+#define WLC_UPGRADE_SUCCESS         0
+#define WLC_UPGRADE_PENDING         1
+
+#ifdef CONFIG_USBRNDIS_RETAIL
+
+typedef struct {
+	char *name;
+	void *param;
+} ndconfig_item_t;
+#endif
+
+
+
+#define WL_AUTH_OPEN_SYSTEM     0   
+#define WL_AUTH_SHARED_KEY      1   
+#define WL_AUTH_OPEN_SHARED		2	
+
+
+#define WL_RADIO_SW_DISABLE     (1<<0)
+#define WL_RADIO_HW_DISABLE     (1<<1)
+#define WL_RADIO_MPC_DISABLE        (1<<2)
+#define WL_RADIO_COUNTRY_DISABLE    (1<<3)  
+
+#define WL_SPURAVOID_OFF    0
+#define WL_SPURAVOID_ON1    1
+#define WL_SPURAVOID_ON2    2
+
+
+#define WL_TXPWR_OVERRIDE   (1U<<31)
+#define WL_TXPWR_NEG   (1U<<30)
+
+#define WL_PHY_PAVARS_LEN   6   
+
+#define WL_PHY_PAVARS2_NUM	3	
+#define WL_PHY_PAVAR_VER	1	
+typedef struct wl_pavars2 {
+	uint16 ver;		
+	uint16 len;		
+	uint16 inuse;		
+	uint16 phy_type;	
+	uint16 bandrange;
+	uint16 chain;
+	uint16 inpa[WL_PHY_PAVARS2_NUM];	
+} wl_pavars2_t;
+
+typedef struct wl_po {
+	uint16  phy_type;   
+	uint16  band;
+	uint16  cckpo;
+	uint32  ofdmpo;
+	uint16  mcspo[8];
+} wl_po_t;
+
+
+#define WLC_TXPWR_MAX       (127)   
+
+
+#define WL_DIAG_INTERRUPT           1   
+#define WL_DIAG_LOOPBACK            2   
+#define WL_DIAG_MEMORY              3   
+#define WL_DIAG_LED             4   
+#define WL_DIAG_REG             5   
+#define WL_DIAG_SROM                6   
+#define WL_DIAG_DMA             7   
+
+#define WL_DIAGERR_SUCCESS          0
+#define WL_DIAGERR_FAIL_TO_RUN          1   
+#define WL_DIAGERR_NOT_SUPPORTED        2   
+#define WL_DIAGERR_INTERRUPT_FAIL       3   
+#define WL_DIAGERR_LOOPBACK_FAIL        4   
+#define WL_DIAGERR_SROM_FAIL            5   
+#define WL_DIAGERR_SROM_BADCRC          6   
+#define WL_DIAGERR_REG_FAIL         7   
+#define WL_DIAGERR_MEMORY_FAIL          8   
+#define WL_DIAGERR_NOMEM            9   
+#define WL_DIAGERR_DMA_FAIL         10  
+
+#define WL_DIAGERR_MEMORY_TIMEOUT       11  
+#define WL_DIAGERR_MEMORY_BADPATTERN        12  
+
+
+#define WLC_BAND_AUTO       0   
+#define WLC_BAND_5G     1   
+#define WLC_BAND_2G     2   
+#define WLC_BAND_ALL        3   
+
+
+#define WL_CHAN_FREQ_RANGE_2G      0
+#define WL_CHAN_FREQ_RANGE_5GL     1
+#define WL_CHAN_FREQ_RANGE_5GM     2
+#define WL_CHAN_FREQ_RANGE_5GH     3
+
+#define WL_CHAN_FREQ_RANGE_5GLL_5BAND    4
+#define WL_CHAN_FREQ_RANGE_5GLH_5BAND    5
+#define WL_CHAN_FREQ_RANGE_5GML_5BAND    6
+#define WL_CHAN_FREQ_RANGE_5GMH_5BAND    7
+#define WL_CHAN_FREQ_RANGE_5GH_5BAND     8
+
+#define WL_CHAN_FREQ_RANGE_5G_BAND0     1
+#define WL_CHAN_FREQ_RANGE_5G_BAND1     2
+#define WL_CHAN_FREQ_RANGE_5G_BAND2     3
+#define WL_CHAN_FREQ_RANGE_5G_BAND3     4
+
+
+#define WLC_PHY_TYPE_A      0
+#define WLC_PHY_TYPE_B      1
+#define WLC_PHY_TYPE_G      2
+#define WLC_PHY_TYPE_N      4
+#define WLC_PHY_TYPE_LP     5
+#define WLC_PHY_TYPE_SSN    6
+#define WLC_PHY_TYPE_HT     7
+#define WLC_PHY_TYPE_LCN    8
+#define WLC_PHY_TYPE_NULL   0xf
+
+
+#define WLC_MACMODE_DISABLED    0   
+#define WLC_MACMODE_DENY    1   
+#define WLC_MACMODE_ALLOW   2   
+
+
+#define GMODE_LEGACY_B      0
+#define GMODE_AUTO      1
+#define GMODE_ONLY      2
+#define GMODE_B_DEFERRED    3
+#define GMODE_PERFORMANCE   4
+#define GMODE_LRS       5
+#define GMODE_MAX       6
+
+
+#define WLC_PLCP_AUTO   -1
+#define WLC_PLCP_SHORT  0
+#define WLC_PLCP_LONG   1
+
+
+#define WLC_PROTECTION_AUTO     -1
+#define WLC_PROTECTION_OFF      0
+#define WLC_PROTECTION_ON       1
+#define WLC_PROTECTION_MMHDR_ONLY   2
+#define WLC_PROTECTION_CTS_ONLY     3
+
+
+#define WLC_PROTECTION_CTL_OFF      0
+#define WLC_PROTECTION_CTL_LOCAL    1
+#define WLC_PROTECTION_CTL_OVERLAP  2
+
+
+#define WLC_N_PROTECTION_OFF        0
+#define WLC_N_PROTECTION_OPTIONAL   1
+#define WLC_N_PROTECTION_20IN40     2
+#define WLC_N_PROTECTION_MIXEDMODE  3
+
+
+#define WLC_N_PREAMBLE_MIXEDMODE    0
+#define WLC_N_PREAMBLE_GF       1
+#define WLC_N_PREAMBLE_GF_BRCM          2
+
+
+#define WLC_N_BW_20ALL          0
+#define WLC_N_BW_40ALL          1
+#define WLC_N_BW_20IN2G_40IN5G      2
+
+
+#define WLC_N_TXRX_CHAIN0       0
+#define WLC_N_TXRX_CHAIN1       1
+
+
+#define WLC_N_SGI_20            0x01
+#define WLC_N_SGI_40            0x02
+
+
+#define PM_OFF  0
+#define PM_MAX  1
+#define PM_FAST 2
+
+#define LISTEN_INTERVAL			10
+
+#define INTERFERE_OVRRIDE_OFF   -1  
+#define INTERFERE_NONE  0   
+#define NON_WLAN    1   
+#define WLAN_MANUAL 2   
+#define WLAN_AUTO   3   
+#define WLAN_AUTO_W_NOISE   4   
+#define AUTO_ACTIVE (1 << 7) 
+
+typedef struct wl_aci_args {
+	int enter_aci_thresh; 
+	int exit_aci_thresh; 
+	int usec_spin; 
+	int glitch_delay; 
+	uint16 nphy_adcpwr_enter_thresh;    
+	uint16 nphy_adcpwr_exit_thresh; 
+	uint16 nphy_repeat_ctr;     
+	uint16 nphy_num_samples;    
+	uint16 nphy_undetect_window_sz; 
+	uint16 nphy_b_energy_lo_aci;    
+	uint16 nphy_b_energy_md_aci;    
+	uint16 nphy_b_energy_hi_aci;    
+	uint16 nphy_noise_noassoc_glitch_th_up; 
+	uint16 nphy_noise_noassoc_glitch_th_dn;
+	uint16 nphy_noise_assoc_glitch_th_up;
+	uint16 nphy_noise_assoc_glitch_th_dn;
+	uint16 nphy_noise_assoc_aci_glitch_th_up;
+	uint16 nphy_noise_assoc_aci_glitch_th_dn;
+	uint16 nphy_noise_assoc_enter_th;
+	uint16 nphy_noise_noassoc_enter_th;
+	uint16 nphy_noise_assoc_rx_glitch_badplcp_enter_th;
+	uint16 nphy_noise_noassoc_crsidx_incr;
+	uint16 nphy_noise_assoc_crsidx_incr;
+	uint16 nphy_noise_crsidx_decr;
+} wl_aci_args_t;
+
+#define TRIGGER_NOW             0
+#define TRIGGER_CRS             0x01
+#define TRIGGER_CRSDEASSERT         0x02
+#define TRIGGER_GOODFCS             0x04
+#define TRIGGER_BADFCS              0x08
+#define TRIGGER_BADPLCP             0x10
+#define TRIGGER_CRSGLITCH           0x20
+#define WL_ACI_ARGS_LEGACY_LENGTH   16  
+#define	WL_SAMPLECOLLECT_T_VERSION	2	
+typedef struct wl_samplecollect_args {
+	
+	uint8 coll_us;
+	int cores;
+	
+	uint16 version;     
+	uint16 length;      
+	int8 trigger;
+	uint16 timeout;
+	uint16 mode;
+	uint32 pre_dur;
+	uint32 post_dur;
+	uint8 gpio_sel;
+	bool downsamp;
+	bool be_deaf;
+	bool agc;       
+	bool filter;        
+	
+	uint8 trigger_state;
+	uint8 module_sel1;
+	uint8 module_sel2;
+	uint16 nsamps;
+} wl_samplecollect_args_t;
+
+#define WL_SAMPLEDATA_HEADER_TYPE   1
+#define WL_SAMPLEDATA_HEADER_SIZE   80  
+#define WL_SAMPLEDATA_TYPE      2
+#define WL_SAMPLEDATA_SEQ       0xff    
+#define WL_SAMPLEDATA_MORE_DATA     0x100   
+#define WL_SAMPLEDATA_T_VERSION     1   
+
+#define WL_SAMPLEDATA_T_VERSION_SPEC_AN 2
+
+typedef struct wl_sampledata {
+	uint16 version; 
+	uint16 size;    
+	uint16 tag; 
+	uint16 length;  
+	uint32 flag;    
+} wl_sampledata_t;
+
+
+#define WL_CHAN_VALID_HW    (1 << 0)
+#define WL_CHAN_VALID_SW    (1 << 1)
+#define WL_CHAN_BAND_5G     (1 << 2)
+#define WL_CHAN_RADAR       (1 << 3)
+#define WL_CHAN_INACTIVE    (1 << 4)
+#define WL_CHAN_PASSIVE     (1 << 5)
+#define WL_CHAN_RESTRICTED  (1 << 6)
+
+
+#define WL_ERROR_VAL        0x00000001
+#define WL_TRACE_VAL        0x00000002
+#define WL_PRHDRS_VAL       0x00000004
+#define WL_PRPKT_VAL        0x00000008
+#define WL_INFORM_VAL       0x00000010
+#define WL_TMP_VAL      0x00000020
+#define WL_OID_VAL      0x00000040
+#define WL_RATE_VAL     0x00000080
+#define WL_ASSOC_VAL        0x00000100
+#define WL_PRUSR_VAL        0x00000200
+#define WL_PS_VAL       0x00000400
+#define WL_TXPWR_VAL        0x00000800  
+#define WL_PORT_VAL     0x00001000
+#define WL_DUAL_VAL     0x00002000
+#define WL_WSEC_VAL     0x00004000
+#define WL_WSEC_DUMP_VAL    0x00008000
+#define WL_LOG_VAL      0x00010000
+#define WL_NRSSI_VAL        0x00020000  
+#define WL_LOFT_VAL     0x00040000  
+#define WL_REGULATORY_VAL   0x00080000
+#define WL_PHYCAL_VAL       0x00100000  
+#define WL_RADAR_VAL        0x00200000  
+#define WL_MPC_VAL      0x00400000
+#define WL_APSTA_VAL        0x00800000
+#define WL_DFS_VAL      0x01000000
+#define WL_BA_VAL       0x02000000
+#define WL_ACI_VAL      0x04000000
+#define WL_MBSS_VAL     0x04000000
+#define WL_CAC_VAL      0x08000000
+#define WL_AMSDU_VAL        0x10000000
+#define WL_AMPDU_VAL        0x20000000
+#define WL_FFPLD_VAL        0x40000000
+
+
+#define WL_DPT_VAL      0x00000001
+#define WL_SCAN_VAL     0x00000002
+#define WL_WOWL_VAL     0x00000004
+#define WL_COEX_VAL     0x00000008
+#define WL_RTDC_VAL     0x00000010
+#define WL_PROTO_VAL        0x00000020
+#define WL_BTA_VAL      0x00000040
+#define WL_CHANINT_VAL      0x00000080
+#define WL_THERMAL_VAL      0x00000100  
+#define WL_P2P_VAL      0x00000200
+#define WL_TXRX_VAL		0x00000400
+#define WL_MCHAN_VAL            0x00000800
+#define WL_TDLS_VAL		0x00001000
+
+
+#define WL_LED_NUMGPIO      16  
+
+
+#define WL_LED_OFF      0       
+#define WL_LED_ON       1       
+#define WL_LED_ACTIVITY     2       
+#define WL_LED_RADIO        3       
+#define WL_LED_ARADIO       4       
+#define WL_LED_BRADIO       5       
+#define WL_LED_BGMODE       6       
+#define WL_LED_WI1      7
+#define WL_LED_WI2      8
+#define WL_LED_WI3      9
+#define WL_LED_ASSOC        10      
+#define WL_LED_INACTIVE     11      
+#define WL_LED_ASSOCACT     12      
+#define WL_LED_WI4      13
+#define WL_LED_WI5      14
+#define WL_LED_BLINKSLOW    15      
+#define WL_LED_BLINKMED     16      
+#define WL_LED_BLINKFAST    17      
+#define WL_LED_BLINKCUSTOM  18      
+#define WL_LED_BLINKPERIODIC    19      
+#define WL_LED_ASSOC_WITH_SEC   20      
+						
+#define WL_LED_START_OFF    21      
+#define WL_LED_NUMBEHAVIOR  22
+
+
+#define WL_LED_BEH_MASK     0x7f        
+#define WL_LED_AL_MASK      0x80        
+
+
+#define WL_NUMCHANNELS      64
+#define WL_NUMCHANSPECS     100
+
+
+#define WL_WDS_WPA_ROLE_AUTH    0   
+#define WL_WDS_WPA_ROLE_SUP 1   
+#define WL_WDS_WPA_ROLE_AUTO    255 
+
+
+#define WL_EVENTING_MASK_LEN    16
+
+
+
+
+#define WL_JOIN_PREF_RSSI   1   
+#define WL_JOIN_PREF_WPA    2   
+#define WL_JOIN_PREF_BAND   3   
+#define WL_JOIN_PREF_RSSI_DELTA 4   
+#define WL_JOIN_PREF_TRANS_PREF	5	
+
+
+#define WLJP_BAND_ASSOC_PREF    255 
+
+
+#define WL_WPA_ACP_MCS_ANY  "\x00\x00\x00\x00"
+
+struct tsinfo_arg {
+	uint8 octets[3];
+};
+
+#define NFIFO           6   
+
+#define WL_CNT_T_VERSION    6   
+
+typedef struct {
+	uint16  version;    
+	uint16  length;     
+
+	
+	uint32  txframe;    
+	uint32  txbyte;     
+	uint32  txretrans;  
+	uint32  txerror;    
+	uint32  txctl;      
+	uint32  txprshort;  
+	uint32  txserr;     
+	uint32  txnobuf;    
+	uint32  txnoassoc;  
+	uint32  txrunt;     
+	uint32  txchit;     
+	uint32  txcmiss;    
+
+	
+	uint32  txuflo;     
+	uint32  txphyerr;   
+	uint32  txphycrs;
+
+	
+	uint32  rxframe;    
+	uint32  rxbyte;     
+	uint32  rxerror;    
+	uint32  rxctl;      
+	uint32  rxnobuf;    
+	uint32  rxnondata;  
+	uint32  rxbadds;    
+	uint32  rxbadcm;    
+	uint32  rxfragerr;  
+	uint32  rxrunt;     
+	uint32  rxgiant;    
+	uint32  rxnoscb;    
+	uint32  rxbadproto; 
+	uint32  rxbadsrcmac;    
+	uint32  rxbadda;    
+	uint32  rxfilter;   
+
+	
+	uint32  rxoflo;     
+	uint32  rxuflo[NFIFO];  
+
+	uint32  d11cnt_txrts_off;   
+	uint32  d11cnt_rxcrc_off;   
+	uint32  d11cnt_txnocts_off; 
+
+	
+	uint32  dmade;      
+	uint32  dmada;      
+	uint32  dmape;      
+	uint32  reset;      
+	uint32  tbtt;       
+	uint32  txdmawar;
+	uint32  pkt_callback_reg_fail;  
+
+	
+	uint32  txallfrm;   
+	uint32  txrtsfrm;   
+	uint32  txctsfrm;   
+	uint32  txackfrm;   
+	uint32  txdnlfrm;   
+	uint32  txbcnfrm;   
+	uint32  txfunfl[8]; 
+	uint32  txtplunfl;  
+	uint32  txphyerror; 
+	uint32  rxfrmtoolong;   
+	uint32  rxfrmtooshrt;   
+	uint32  rxinvmachdr;    
+	uint32  rxbadfcs;   
+	uint32  rxbadplcp;  
+	uint32  rxcrsglitch;    
+	uint32  rxstrt;     
+	uint32  rxdfrmucastmbss; 
+	uint32  rxmfrmucastmbss; 
+	uint32  rxcfrmucast;    
+	uint32  rxrtsucast; 
+	uint32  rxctsucast; 
+	uint32  rxackucast; 
+	uint32  rxdfrmocast;    
+	uint32  rxmfrmocast;    
+	uint32  rxcfrmocast;    
+	uint32  rxrtsocast; 
+	uint32  rxctsocast; 
+	uint32  rxdfrmmcast;    
+	uint32  rxmfrmmcast;    
+	uint32  rxcfrmmcast;    
+	uint32  rxbeaconmbss;   
+	uint32  rxdfrmucastobss; 
+	uint32  rxbeaconobss;   
+	uint32  rxrsptmout; 
+	uint32  bcntxcancl; 
+	uint32  rxf0ovfl;   
+	uint32  rxf1ovfl;   
+	uint32  rxf2ovfl;   
+	uint32  txsfovfl;   
+	uint32  pmqovfl;    
+	uint32  rxcgprqfrm; 
+	uint32  rxcgprsqovfl;   
+	uint32  txcgprsfail;    
+	uint32  txcgprssuc; 
+	uint32  prs_timeout;    
+	uint32  rxnack;
+	uint32  frmscons;
+	uint32  txnack;
+	uint32  txglitch_nack;  
+	uint32  txburst;    
+
+	
+	uint32  txfrag;     
+	uint32  txmulti;    
+	uint32  txfail;     
+	uint32  txretry;    
+	uint32  txretrie;   
+	uint32  rxdup;      
+	uint32  txrts;      
+	uint32  txnocts;    
+	uint32  txnoack;    
+	uint32  rxfrag;     
+	uint32  rxmulti;    
+	uint32  rxcrc;      
+	uint32  txfrmsnt;   
+	uint32  rxundec;    
+
+	
+	uint32  tkipmicfaill;   
+	uint32  tkipcntrmsr;    
+	uint32  tkipreplay; 
+	uint32  ccmpfmterr; 
+	uint32  ccmpreplay; 
+	uint32  ccmpundec;  
+	uint32  fourwayfail;    
+	uint32  wepundec;   
+	uint32  wepicverr;  
+	uint32  decsuccess; 
+	uint32  tkipicverr; 
+	uint32  wepexcluded;    
+
+	uint32  rxundec_mcst;   
+
+	
+	uint32  tkipmicfaill_mcst;  
+	uint32  tkipcntrmsr_mcst;   
+	uint32  tkipreplay_mcst;    
+	uint32  ccmpfmterr_mcst;    
+	uint32  ccmpreplay_mcst;    
+	uint32  ccmpundec_mcst; 
+	uint32  fourwayfail_mcst;   
+	uint32  wepundec_mcst;  
+	uint32  wepicverr_mcst; 
+	uint32  decsuccess_mcst;    
+	uint32  tkipicverr_mcst;    
+	uint32  wepexcluded_mcst;   
+
+	uint32  txchanrej;  
+	uint32  txexptime;  
+	uint32  psmwds;     
+	uint32  phywatchdog;    
+
+	
+	uint32  prq_entries_handled;    
+	uint32  prq_undirected_entries; 
+	uint32  prq_bad_entries;    
+	uint32  atim_suppress_count;    
+	uint32  bcn_template_not_ready; 
+	uint32  bcn_template_not_ready_done; 
+	uint32  late_tbtt_dpc;  
+
+	
+	uint32  rx1mbps;    
+	uint32  rx2mbps;    
+	uint32  rx5mbps5;   
+	uint32  rx6mbps;    
+	uint32  rx9mbps;    
+	uint32  rx11mbps;   
+	uint32  rx12mbps;   
+	uint32  rx18mbps;   
+	uint32  rx24mbps;   
+	uint32  rx36mbps;   
+	uint32  rx48mbps;   
+	uint32  rx54mbps;   
+	uint32  rx108mbps;  
+	uint32  rx162mbps;  
+	uint32  rx216mbps;  
+	uint32  rx270mbps;  
+	uint32  rx324mbps;  
+	uint32  rx378mbps;  
+	uint32  rx432mbps;  
+	uint32  rx486mbps;  
+	uint32  rx540mbps;  
+
+	
+	uint32  pktengrxducast; 
+	uint32  pktengrxdmcast; 
+
+	uint32  rfdisable;  
+	uint32  bphy_rxcrsglitch;   
+
+	uint32  txmpdu_sgi; 
+	uint32  rxmpdu_sgi; 
+	uint32  txmpdu_stbc;    
+	uint32  rxmpdu_stbc;    
+} wl_cnt_t;
+
+
+#define WL_WME_CNT_VERSION  1   
+
+typedef struct {
+	uint32 packets;
+	uint32 bytes;
+} wl_traffic_stats_t;
+
+typedef struct {
+	uint16  version;    
+	uint16  length;     
+
+	wl_traffic_stats_t tx[AC_COUNT];    
+	wl_traffic_stats_t tx_failed[AC_COUNT]; 
+	wl_traffic_stats_t rx[AC_COUNT];    
+	wl_traffic_stats_t rx_failed[AC_COUNT]; 
+
+	wl_traffic_stats_t forward[AC_COUNT];   
+
+	wl_traffic_stats_t tx_expired[AC_COUNT];    
+
+} wl_wme_cnt_t;
+
+struct wl_msglevel2 {
+	uint32 low;
+	uint32 high;
+};
+
+typedef struct wl_mkeep_alive_pkt {
+	uint16	version; 
+	uint16	length; 
+	uint32	period_msec;
+	uint16	len_bytes;
+	uint8	keep_alive_id; 
+	uint8	data[1];
+} wl_mkeep_alive_pkt_t;
+
+#define WL_MKEEP_ALIVE_VERSION		1
+#define WL_MKEEP_ALIVE_FIXED_LEN	OFFSETOF(wl_mkeep_alive_pkt_t, data)
+#define WL_MKEEP_ALIVE_PRECISION	500
+
+
+
+#define WLC_ROAM_TRIGGER_DEFAULT    0 
+#define WLC_ROAM_TRIGGER_BANDWIDTH  1 
+#define WLC_ROAM_TRIGGER_DISTANCE   2 
+#define WLC_ROAM_TRIGGER_AUTO       3 
+#define WLC_ROAM_TRIGGER_MAX_VALUE  3 
+
+
+#define WPA_AUTH_PFN_ANY	0xffffffff	
+
+enum {
+	PFN_LIST_ORDER,
+	PFN_RSSI
+};
+
+enum {
+	DISABLE,
+	ENABLE
+};
+
+enum {
+	OFF_ADAPT,
+	SMART_ADAPT,
+	STRICT_ADAPT,
+	SLOW_ADAPT
+};
+
+#define SORT_CRITERIA_BIT		0
+#define AUTO_NET_SWITCH_BIT		1
+#define ENABLE_BKGRD_SCAN_BIT	2
+#define IMMEDIATE_SCAN_BIT		3
+#define	AUTO_CONNECT_BIT		4
+#define	ENABLE_BD_SCAN_BIT		5
+#define ENABLE_ADAPTSCAN_BIT	6
+#define IMMEDIATE_EVENT_BIT		8
+
+#define SORT_CRITERIA_MASK		0x0001
+#define AUTO_NET_SWITCH_MASK	0x0002
+#define ENABLE_BKGRD_SCAN_MASK	0x0004
+#define IMMEDIATE_SCAN_MASK		0x0008
+#define	AUTO_CONNECT_MASK		0x0010
+#define ENABLE_BD_SCAN_MASK		0x0020
+#define ENABLE_ADAPTSCAN_MASK	0x00c0
+#define IMMEDIATE_EVENT_MASK	0x0100
+
+#define PFN_VERSION				2
+#define PFN_SCANRESULT_VERSION	1
+#define MAX_PFN_LIST_COUNT	16
+
+#define PFN_COMPLETE			1
+#define PFN_INCOMPLETE			0
+
+#define DEFAULT_BESTN			2
+#define DEFAULT_MSCAN			0
+#define DEFAULT_REPEAT			10
+#define DEFAULT_EXP				2
+
+
+typedef struct wl_pfn_subnet_info {
+	struct ether_addr BSSID;
+	uint8	channel; 
+	uint8	SSID_len;
+	uint8	SSID[32];
+} wl_pfn_subnet_info_t;
+
+typedef struct wl_pfn_net_info {
+	wl_pfn_subnet_info_t pfnsubnet;
+	int16	RSSI; 
+	uint16	timestamp; 
+} wl_pfn_net_info_t;
+
+typedef struct wl_pfn_scanresults {
+	uint32 version;
+	uint32 status;
+	uint32 count;
+	wl_pfn_net_info_t netinfo[1];
+} wl_pfn_scanresults_t;
+
+
+typedef struct wl_pfn_param {
+	int32 version;			
+	int32 scan_freq;		
+	int32 lost_network_timeout;	
+	int16 flags;			
+	int16 rssi_margin;		
+	uint8 bestn; 
+	uint8 mscan; 
+	uint8 repeat; 
+	uint8 exp; 
+	int32 slow_freq; 
+} wl_pfn_param_t;
+
+typedef struct wl_pfn_bssid {
+	struct ether_addr 	macaddr;
+	
+	uint16				flags;
+} wl_pfn_bssid_t;
+#define WL_PFN_SUPPRESSFOUND_MASK	0x08
+#define WL_PFN_SUPPRESSLOST_MASK	0x10
+
+typedef struct wl_pfn_cfg {
+	uint32				reporttype;
+	int32				channel_num;
+	uint16				channel_list[WL_NUMCHANNELS];
+} wl_pfn_cfg_t;
+#define WL_PFN_REPORT_ALLNET 	0
+#define WL_PFN_REPORT_SSIDNET 	1
+#define WL_PFN_REPORT_BSSIDNET 	2
+
+typedef struct wl_pfn {
+	wlc_ssid_t		ssid;			
+	int32			flags;			
+	int32			infra;			
+	int32			auth;			
+	int32			wpa_auth;		
+	int32			wsec;			
+} wl_pfn_t;
+#define WL_PFN_HIDDEN_BIT		2
+#define PNO_SCAN_MAX_FW		508*1000	
+#define PNO_SCAN_MAX_FW_SEC	PNO_SCAN_MAX_FW/1000 
+#define PNO_SCAN_MIN_FW_SEC	10			
+#define WL_PFN_HIDDEN_MASK		0x4
+
+
+#define TOE_TX_CSUM_OL      0x00000001
+#define TOE_RX_CSUM_OL      0x00000002
+
+
+#define TOE_ERRTEST_TX_CSUM 0x00000001
+#define TOE_ERRTEST_RX_CSUM 0x00000002
+#define TOE_ERRTEST_RX_CSUM2    0x00000004
+
+struct toe_ol_stats_t {
+	
+	uint32 tx_summed;
+
+	
+	uint32 tx_iph_fill;
+	uint32 tx_tcp_fill;
+	uint32 tx_udp_fill;
+	uint32 tx_icmp_fill;
+
+	
+	uint32 rx_iph_good;
+	uint32 rx_iph_bad;
+	uint32 rx_tcp_good;
+	uint32 rx_tcp_bad;
+	uint32 rx_udp_good;
+	uint32 rx_udp_bad;
+	uint32 rx_icmp_good;
+	uint32 rx_icmp_bad;
+
+	
+	uint32 tx_tcp_errinj;
+	uint32 tx_udp_errinj;
+	uint32 tx_icmp_errinj;
+
+	
+	uint32 rx_tcp_errinj;
+	uint32 rx_udp_errinj;
+	uint32 rx_icmp_errinj;
+};
+
+
+#define ARP_OL_AGENT        0x00000001
+#define ARP_OL_SNOOP        0x00000002
+#define ARP_OL_HOST_AUTO_REPLY  0x00000004
+#define ARP_OL_PEER_AUTO_REPLY  0x00000008
+
+
+#define ARP_ERRTEST_REPLY_PEER  0x1
+#define ARP_ERRTEST_REPLY_HOST  0x2
+
+#define ARP_MULTIHOMING_MAX 8   
+
+
+struct arp_ol_stats_t {
+	uint32  host_ip_entries;    
+	uint32  host_ip_overflow;   
+
+	uint32  arp_table_entries;  
+	uint32  arp_table_overflow; 
+
+	uint32  host_request;       
+	uint32  host_reply;     
+	uint32  host_service;       
+
+	uint32  peer_request;       
+	uint32  peer_request_drop;  
+	uint32  peer_reply;     
+	uint32  peer_reply_drop;    
+	uint32  peer_service;       
+};
+
+
+
+
+typedef struct wl_keep_alive_pkt {
+	uint32  period_msec;    
+	uint16  len_bytes;  
+	uint8   data[1];    
+} wl_keep_alive_pkt_t;
+
+#define WL_KEEP_ALIVE_FIXED_LEN     OFFSETOF(wl_keep_alive_pkt_t, data)
+
+
+
+
+#define MAX_WAKE_PACKET_BYTES 128
+
+
+typedef struct pm_wake_packet {
+	uint32	status;		
+	uint32	pattern_id;	
+	uint32	original_packet_size;
+	uint32	saved_packet_size;
+	uchar	packet[MAX_WAKE_PACKET_BYTES];
+} pm_wake_packet_t;
+
+
+
+#define PKT_FILTER_MODE_FORWARD_ON_MATCH      		1
+
+#define PKT_FILTER_MODE_DISABLE					2
+
+#define PKT_FILTER_MODE_PKT_CACHE_ON_MATCH    		4
+
+#define PKT_FILTER_MODE_PKT_FORWARD_OFF_DEFAULT 8
+
+
+typedef enum wl_pkt_filter_type {
+	WL_PKT_FILTER_TYPE_PATTERN_MATCH,	
+	WL_PKT_FILTER_TYPE_MAGIC_PATTERN_MATCH	
+} wl_pkt_filter_type_t;
+
+#define WL_PKT_FILTER_TYPE wl_pkt_filter_type_t
+
+
+typedef struct wl_pkt_filter_pattern {
+	uint32  offset;     
+	uint32  size_bytes; 
+	uint8   mask_and_pattern[1]; 
+} wl_pkt_filter_pattern_t;
+
+
+typedef struct wl_pkt_filter {
+	uint32  id;     
+	uint32  type;       
+	uint32  negate_match;   
+	union {         
+		wl_pkt_filter_pattern_t pattern;    
+	} u;
+} wl_pkt_filter_t;
+
+#define WL_PKT_FILTER_FIXED_LEN       OFFSETOF(wl_pkt_filter_t, u)
+#define WL_PKT_FILTER_PATTERN_FIXED_LEN   OFFSETOF(wl_pkt_filter_pattern_t, mask_and_pattern)
+
+
+typedef struct wl_pkt_filter_enable {
+	uint32  id;     
+	uint32  enable;     
+} wl_pkt_filter_enable_t;
+
+
+typedef struct wl_pkt_filter_list {
+	uint32  num;        
+	wl_pkt_filter_t filter[1];  
+} wl_pkt_filter_list_t;
+
+#define WL_PKT_FILTER_LIST_FIXED_LEN      OFFSETOF(wl_pkt_filter_list_t, filter)
+
+
+typedef struct wl_pkt_filter_stats {
+	uint32  num_pkts_matched;   
+	uint32  num_pkts_forwarded; 
+	uint32  num_pkts_discarded; 
+} wl_pkt_filter_stats_t;
+
+
+typedef struct wl_seq_cmd_ioctl {
+	uint32 cmd;     
+	uint32 len;     
+} wl_seq_cmd_ioctl_t;
+
+#define WL_SEQ_CMD_ALIGN_BYTES  4
+
+
+#define WL_SEQ_CMDS_GET_IOCTL_FILTER(cmd) \
+	(((cmd) == WLC_GET_MAGIC) || \
+	 ((cmd) == WLC_GET_VERSION) || \
+	 ((cmd) == WLC_GET_AP) || \
+	 ((cmd) == WLC_GET_INSTANCE))
+
+
+
+#define WL_PKTENG_PER_TX_START          0x01
+#define WL_PKTENG_PER_TX_STOP           0x02
+#define WL_PKTENG_PER_RX_START          0x04
+#define WL_PKTENG_PER_RX_WITH_ACK_START     0x05
+#define WL_PKTENG_PER_TX_WITH_ACK_START     0x06
+#define WL_PKTENG_PER_RX_STOP           0x08
+#define WL_PKTENG_PER_MASK          0xff
+
+#define WL_PKTENG_SYNCHRONOUS           0x100   
+
+typedef struct wl_pkteng {
+	uint32 flags;
+	uint32 delay;           
+	uint32 nframes;         
+	uint32 length;          
+	uint8  seqno;           
+	struct ether_addr dest;     
+	struct ether_addr src;      
+} wl_pkteng_t;
+
+#define NUM_80211b_RATES    4
+#define NUM_80211ag_RATES   8
+#define NUM_80211n_RATES    32
+#define NUM_80211_RATES     (NUM_80211b_RATES+NUM_80211ag_RATES+NUM_80211n_RATES)
+typedef struct wl_pkteng_stats {
+	uint32 lostfrmcnt;      
+	int32 rssi;         
+	int32 snr;          
+	uint16 rxpktcnt[NUM_80211_RATES+1];
+} wl_pkteng_stats_t;
+
+
+#define WL_WOWL_MAGIC   (1 << 0)    
+#define WL_WOWL_NET (1 << 1)    
+#define WL_WOWL_DIS (1 << 2)    
+#define WL_WOWL_RETR    (1 << 3)    
+#define WL_WOWL_BCN (1 << 4)    
+#define WL_WOWL_TST (1 << 5)    
+#define WL_WOWL_M1      (1 << 6)        
+#define WL_WOWL_EAPID   (1 << 7)        
+#define WL_WOWL_KEYROT  (1 << 14)       
+#define WL_WOWL_BCAST   (1 << 15)   
+
+#define MAGIC_PKT_MINLEN 102    
+
+typedef struct {
+	uint masksize;      
+	uint offset;        
+	uint patternoffset; 
+	uint patternsize;   
+	ulong id;       
+	
+	
+} wl_wowl_pattern_t;
+
+typedef struct {
+	uint            count;
+	wl_wowl_pattern_t   pattern[1];
+} wl_wowl_pattern_list_t;
+
+typedef struct {
+	uint8   pci_wakeind;    
+	uint16  ucode_wakeind;  
+} wl_wowl_wakeind_t;
+
+
+typedef struct wl_txrate_class {
+	uint8       init_rate;
+	uint8       min_rate;
+	uint8       max_rate;
+} wl_txrate_class_t;
+
+
+
+
+#define WLC_OBSS_SCAN_PASSIVE_DWELL_DEFAULT     20  
+#define WLC_OBSS_SCAN_PASSIVE_DWELL_MIN         5   
+#define WLC_OBSS_SCAN_PASSIVE_DWELL_MAX         1000    
+#define WLC_OBSS_SCAN_ACTIVE_DWELL_DEFAULT      10  
+#define WLC_OBSS_SCAN_ACTIVE_DWELL_MIN          10  
+#define WLC_OBSS_SCAN_ACTIVE_DWELL_MAX          1000    
+#define WLC_OBSS_SCAN_WIDTHSCAN_INTERVAL_DEFAULT    300 
+#define WLC_OBSS_SCAN_WIDTHSCAN_INTERVAL_MIN        10  
+#define WLC_OBSS_SCAN_WIDTHSCAN_INTERVAL_MAX        900 
+#define WLC_OBSS_SCAN_CHANWIDTH_TRANSITION_DLY_DEFAULT  5
+#define WLC_OBSS_SCAN_CHANWIDTH_TRANSITION_DLY_MIN  5
+#define WLC_OBSS_SCAN_CHANWIDTH_TRANSITION_DLY_MAX  100
+#define WLC_OBSS_SCAN_PASSIVE_TOTAL_PER_CHANNEL_DEFAULT 200 
+#define WLC_OBSS_SCAN_PASSIVE_TOTAL_PER_CHANNEL_MIN 200 
+#define WLC_OBSS_SCAN_PASSIVE_TOTAL_PER_CHANNEL_MAX 10000   
+#define WLC_OBSS_SCAN_ACTIVE_TOTAL_PER_CHANNEL_DEFAULT  20  
+#define WLC_OBSS_SCAN_ACTIVE_TOTAL_PER_CHANNEL_MIN  20  
+#define WLC_OBSS_SCAN_ACTIVE_TOTAL_PER_CHANNEL_MAX  10000   
+#define WLC_OBSS_SCAN_ACTIVITY_THRESHOLD_DEFAULT    25  
+#define WLC_OBSS_SCAN_ACTIVITY_THRESHOLD_MIN        0   
+#define WLC_OBSS_SCAN_ACTIVITY_THRESHOLD_MAX        100 
+
+
+typedef struct wl_obss_scan_arg {
+	int16   passive_dwell;
+	int16   active_dwell;
+	int16   bss_widthscan_interval;
+	int16   passive_total;
+	int16   active_total;
+	int16   chanwidth_transition_delay;
+	int16   activity_threshold;
+} wl_obss_scan_arg_t;
+
+#define WL_OBSS_SCAN_PARAM_LEN  sizeof(wl_obss_scan_arg_t)
+#define WL_MIN_NUM_OBSS_SCAN_ARG 7  
+
+#define WL_COEX_INFO_MASK       0x07
+#define WL_COEX_INFO_REQ        0x01
+#define WL_COEX_40MHZ_INTOLERANT    0x02
+#define WL_COEX_WIDTH20         0x04
+
+#define WLC_RSSI_INVALID     0  
+
+#define MAX_RSSI_LEVELS 8
+
+
+typedef struct wl_rssi_event {
+	uint32 rate_limit_msec;     
+	uint8 num_rssi_levels;      
+	int8 rssi_levels[MAX_RSSI_LEVELS];  
+} wl_rssi_event_t;
+
+typedef struct wl_action_obss_coex_req {
+	uint8 info;
+	uint8 num;
+	uint8 ch_list[1];
+} wl_action_obss_coex_req_t;
+
+
+#define EXTLOG_CUR_VER      0x0100
+
+#define MAX_ARGSTR_LEN      18 
+
+
+#define LOG_MODULE_COMMON   0x0001
+#define LOG_MODULE_ASSOC    0x0002
+#define LOG_MODULE_EVENT    0x0004
+#define LOG_MODULE_MAX      3           
+
+
+#define WL_LOG_LEVEL_DISABLE    0
+#define WL_LOG_LEVEL_ERR    1
+#define WL_LOG_LEVEL_WARN   2
+#define WL_LOG_LEVEL_INFO   3
+#define WL_LOG_LEVEL_MAX    WL_LOG_LEVEL_INFO   
+
+
+#define LOG_FLAG_EVENT      1
+
+
+#define LOG_ARGTYPE_NULL    0
+#define LOG_ARGTYPE_STR     1   
+#define LOG_ARGTYPE_INT     2   
+#define LOG_ARGTYPE_INT_STR 3   
+#define LOG_ARGTYPE_STR_INT 4   
+
+typedef struct wlc_extlog_cfg {
+	int max_number;
+	uint16 module;  
+	uint8 level;
+	uint8 flag;
+	uint16 version;
+} wlc_extlog_cfg_t;
+
+typedef struct log_record {
+	uint32 time;
+	uint16 module;
+	uint16 id;
+	uint8 level;
+	uint8 sub_unit;
+	uint8 seq_num;
+	int32 arg;
+	char str[MAX_ARGSTR_LEN];
+} log_record_t;
+
+typedef struct wlc_extlog_req {
+	uint32 from_last;
+	uint32 num;
+} wlc_extlog_req_t;
+
+typedef struct wlc_extlog_results {
+	uint16 version;
+	uint16 record_len;
+	uint32 num;
+	log_record_t logs[1];
+} wlc_extlog_results_t;
+
+typedef struct log_idstr {
+	uint16  id;
+	uint16  flag;
+	uint8   arg_type;
+	const char  *fmt_str;
+} log_idstr_t;
+
+#define FMTSTRF_USER        1
+
+
+typedef enum {
+	FMTSTR_DRIVER_UP_ID = 0,
+	FMTSTR_DRIVER_DOWN_ID = 1,
+	FMTSTR_SUSPEND_MAC_FAIL_ID = 2,
+	FMTSTR_NO_PROGRESS_ID = 3,
+	FMTSTR_RFDISABLE_ID = 4,
+	FMTSTR_REG_PRINT_ID = 5,
+	FMTSTR_EXPTIME_ID = 6,
+	FMTSTR_JOIN_START_ID = 7,
+	FMTSTR_JOIN_COMPLETE_ID = 8,
+	FMTSTR_NO_NETWORKS_ID = 9,
+	FMTSTR_SECURITY_MISMATCH_ID = 10,
+	FMTSTR_RATE_MISMATCH_ID = 11,
+	FMTSTR_AP_PRUNED_ID = 12,
+	FMTSTR_KEY_INSERTED_ID = 13,
+	FMTSTR_DEAUTH_ID = 14,
+	FMTSTR_DISASSOC_ID = 15,
+	FMTSTR_LINK_UP_ID = 16,
+	FMTSTR_LINK_DOWN_ID = 17,
+	FMTSTR_RADIO_HW_OFF_ID = 18,
+	FMTSTR_RADIO_HW_ON_ID = 19,
+	FMTSTR_EVENT_DESC_ID = 20,
+	FMTSTR_PNP_SET_POWER_ID = 21,
+	FMTSTR_RADIO_SW_OFF_ID = 22,
+	FMTSTR_RADIO_SW_ON_ID = 23,
+	FMTSTR_PWD_MISMATCH_ID = 24,
+	FMTSTR_FATAL_ERROR_ID = 25,
+	FMTSTR_AUTH_FAIL_ID = 26,
+	FMTSTR_ASSOC_FAIL_ID = 27,
+	FMTSTR_IBSS_FAIL_ID = 28,
+	FMTSTR_EXTAP_FAIL_ID = 29,
+	FMTSTR_MAX_ID
+} log_fmtstr_id_t;
+
+#ifdef DONGLEOVERLAYS
+typedef struct {
+	uint32 flags_idx;	
+	uint32 offset;		
+	uint32 len;			
+	
+} wl_ioctl_overlay_t;
+
+#define OVERLAY_IDX_MASK		0x000000ff
+#define OVERLAY_IDX_SHIFT		0
+#define OVERLAY_FLAGS_MASK		0xffffff00
+#define OVERLAY_FLAGS_SHIFT		8
+
+#define OVERLAY_FLAG_POSTLOAD	0x100
+
+#define OVERLAY_FLAG_DEFER_DL	0x200
+
+#define OVERLAY_FLAG_PRESLEEP	0x400
+
+#define OVERLAY_DOWNLOAD_CHUNKSIZE	1024
+#endif 
+
+
+#include <packed_section_end.h>
+
+
+#include <packed_section_start.h>
+
+#define VNDR_IE_CMD_LEN     4   
+
+
+#define VNDR_IE_BEACON_FLAG 0x1
+#define VNDR_IE_PRBRSP_FLAG 0x2
+#define VNDR_IE_ASSOCRSP_FLAG   0x4
+#define VNDR_IE_AUTHRSP_FLAG    0x8
+#define VNDR_IE_PRBREQ_FLAG 0x10
+#define VNDR_IE_ASSOCREQ_FLAG   0x20
+#define VNDR_IE_CUSTOM_FLAG 0x100 
+
+#define VNDR_IE_INFO_HDR_LEN    (sizeof(uint32))
+
+typedef BWL_PRE_PACKED_STRUCT struct {
+	uint32 pktflag;         
+	vndr_ie_t vndr_ie_data;     
+} BWL_POST_PACKED_STRUCT vndr_ie_info_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct {
+	int iecount;            
+	vndr_ie_info_t vndr_ie_list[1]; 
+} BWL_POST_PACKED_STRUCT vndr_ie_buf_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct {
+	char cmd[VNDR_IE_CMD_LEN];  
+	vndr_ie_buf_t vndr_ie_buffer;   
+} BWL_POST_PACKED_STRUCT vndr_ie_setbuf_t;
+
+
+
+typedef BWL_PRE_PACKED_STRUCT struct sta_prbreq_wps_ie_hdr {
+	struct ether_addr staAddr;
+	uint16 ieLen;
+} BWL_POST_PACKED_STRUCT sta_prbreq_wps_ie_hdr_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct sta_prbreq_wps_ie_data {
+	sta_prbreq_wps_ie_hdr_t hdr;
+	uint8 ieData[1];
+} BWL_POST_PACKED_STRUCT sta_prbreq_wps_ie_data_t;
+
+typedef BWL_PRE_PACKED_STRUCT struct sta_prbreq_wps_ie_list {
+	uint32 totLen;
+	uint8 ieDataList[1];
+} BWL_POST_PACKED_STRUCT sta_prbreq_wps_ie_list_t;
+
+
+#ifdef WLMEDIA_TXFAILEVENT
+typedef BWL_PRE_PACKED_STRUCT struct {
+	char   dest[ETHER_ADDR_LEN]; 
+	uint8  prio;            
+	uint8  flags;           
+	uint32 tsf_l;           
+	uint32 tsf_h;           
+	uint16 rates;           
+	uint16 txstatus;        
+} BWL_POST_PACKED_STRUCT txfailinfo_t;
+#endif 
+
+#include <packed_section_end.h>
+
+
+#define ASSERTLOG_CUR_VER   0x0100
+#define MAX_ASSRTSTR_LEN    64
+
+typedef struct assert_record {
+	uint32 time;
+	uint8 seq_num;
+	char str[MAX_ASSRTSTR_LEN];
+} assert_record_t;
+
+typedef struct assertlog_results {
+	uint16 version;
+	uint16 record_len;
+	uint32 num;
+	assert_record_t logs[1];
+} assertlog_results_t;
+
+#define LOGRRC_FIX_LEN  8
+#define IOBUF_ALLOWED_NUM_OF_LOGREC(type, len) ((len - LOGRRC_FIX_LEN)/sizeof(type))
+
+
+
+
+
+#define CHANIM_DISABLE  0   
+#define CHANIM_DETECT   1   
+#define CHANIM_ACT  2   
+#define CHANIM_MODE_MAX 2
+
+
+#define APCS_IOCTL      1
+#define APCS_CHANIM     2
+#define APCS_CSTIMER    3
+#define APCS_BTA        4
+
+
+#define CHANIM_ACS_RECORD           10
+
+
+typedef struct {
+	bool valid;
+	uint8 trigger;
+	chanspec_t selected_chspc;
+	uint32 glitch_cnt;
+	uint8 ccastats;
+	uint timestamp;
+} chanim_acs_record_t;
+
+typedef struct {
+	chanim_acs_record_t acs_record[CHANIM_ACS_RECORD];
+	uint8 count;
+	uint timestamp;
+} wl_acs_record_t;
+
+
+
+#define SMFS_VERSION 1
+
+typedef struct wl_smfs_elem {
+	uint32 count;
+	uint16 code;  
+} wl_smfs_elem_t;
+
+typedef struct wl_smf_stats {
+	uint32 version;
+	uint16 length;  
+	uint8 type;
+	uint8 codetype;
+	uint32 ignored_cnt;
+	uint32 malformed_cnt;
+	uint32 count_total; 
+	wl_smfs_elem_t elem[1];
+} wl_smf_stats_t;
+
+#define WL_SMFSTATS_FIXED_LEN OFFSETOF(wl_smf_stats_t, elem);
+
+enum {
+	SMFS_CODETYPE_SC,
+	SMFS_CODETYPE_RC
+};
+
+
+#define SMFS_CODE_MALFORMED 0xFFFE
+#define SMFS_CODE_IGNORED   0xFFFD
+
+typedef enum smfs_type {
+	SMFS_TYPE_AUTH,
+	SMFS_TYPE_ASSOC,
+	SMFS_TYPE_REASSOC,
+	SMFS_TYPE_DISASSOC_TX,
+	SMFS_TYPE_DISASSOC_RX,
+	SMFS_TYPE_DEAUTH_TX,
+	SMFS_TYPE_DEAUTH_RX,
+	SMFS_TYPE_MAX
+} smfs_type_t;
+
+#ifdef PHYMON
+
+#define PHYMON_VERSION 1
+
+typedef struct wl_phycal_core_state {
+	
+	int16 tx_iqlocal_a;
+	int16 tx_iqlocal_b;
+	int8 tx_iqlocal_ci;
+	int8 tx_iqlocal_cq;
+	int8 tx_iqlocal_di;
+	int8 tx_iqlocal_dq;
+	int8 tx_iqlocal_ei;
+	int8 tx_iqlocal_eq;
+	int8 tx_iqlocal_fi;
+	int8 tx_iqlocal_fq;
+
+	
+	int16 rx_iqcal_a;
+	int16 rx_iqcal_b;
+
+	uint8 tx_iqlocal_pwridx; 
+	uint32 papd_epsilon_table[64]; 
+	int16 papd_epsilon_offset; 
+	uint8 curr_tx_pwrindex; 
+	int8 idle_tssi; 
+	int8 est_tx_pwr; 
+	int8 est_rx_pwr; 
+	uint16 rx_gaininfo; 
+	uint16 init_gaincode; 
+	int8 estirr_tx;
+	int8 estirr_rx;
+
+} wl_phycal_core_state_t;
+
+typedef struct wl_phycal_state {
+	int version;
+	int8 num_phy_cores; 
+	int8 curr_temperature; 
+	chanspec_t chspec; 
+	bool aci_state; 
+	uint16 crsminpower; 
+	uint16 crsminpowerl; 
+	uint16 crsminpoweru; 
+	wl_phycal_core_state_t phycal_core[1];
+} wl_phycal_state_t;
+
+#define WL_PHYCAL_STAT_FIXED_LEN OFFSETOF(wl_phycal_state_t, phycal_core)
+#endif 
+
+#ifdef WLDSTA
+typedef struct wl_dsta_if {
+	struct ether_addr addr;
+} wl_dsta_if_t;
+#endif
+
+#ifdef WLP2P
+
+typedef struct wl_p2p_disc_st {
+	uint8 state;    
+	chanspec_t chspec;  
+	uint16 dwell;   
+} wl_p2p_disc_st_t;
+
+
+#define WL_P2P_DISC_ST_SCAN 0
+#define WL_P2P_DISC_ST_LISTEN   1
+#define WL_P2P_DISC_ST_SEARCH   2
+
+
+typedef struct wl_p2p_scan {
+	uint8 type;     
+	uint8 reserved[3];
+	
+} wl_p2p_scan_t;
+
+
+typedef struct wl_p2p_if {
+	struct ether_addr addr;
+	uint8 type; 
+	chanspec_t chspec;  
+} wl_p2p_if_t;
+
+
+#define WL_P2P_IF_CLIENT    0
+#define WL_P2P_IF_GO        1
+#define WL_P2P_IF_DYNBCN_GO     2
+#define WL_P2P_IF_DEV       3
+
+
+typedef struct wl_p2p_ifq {
+	uint bsscfgidx;
+	char ifname[BCM_MSG_IFNAME_MAX];
+} wl_p2p_ifq_t;
+
+
+typedef struct wl_p2p_ops {
+	uint8 ops;  
+	uint8 ctw;  
+} wl_p2p_ops_t;
+
+
+typedef struct wl_p2p_sched_desc {
+	uint32 start;
+	uint32 interval;
+	uint32 duration;
+	uint32 count;   
+} wl_p2p_sched_desc_t;
+
+
+#define WL_P2P_SCHED_RSVD   0
+#define WL_P2P_SCHED_REPEAT 255 
+
+typedef struct wl_p2p_sched {
+	uint8 type; 
+	uint8 action;   
+	uint8 option;   
+	wl_p2p_sched_desc_t desc[1];
+} wl_p2p_sched_t;
+#define WL_P2P_SCHED_FIXED_LEN      3
+
+
+#define WL_P2P_SCHED_TYPE_ABS       0   
+#define WL_P2P_SCHED_TYPE_REQ_ABS   1   
+
+
+#define WL_P2P_SCHED_ACTION_NONE    0   
+#define WL_P2P_SCHED_ACTION_DOZE    1   
+
+#define WL_P2P_SCHED_ACTION_GOOFF   2   
+
+#define WL_P2P_SCHED_ACTION_RESET   255 
+
+
+#define WL_P2P_SCHED_OPTION_NORMAL  0   
+#define WL_P2P_SCHED_OPTION_BCNPCT  1   
+
+#define WL_P2P_SCHED_OPTION_TSFOFS  2   
+
+
+#define WL_P2P_FEAT_GO_CSA      (1 << 0)        
+#define WL_P2P_FEAT_GO_NOLEGACY (1 << 1)        
+#define WL_P2P_FEAT_RESTRICT_DEV_RESP (1 << 2)  
+#endif 
+
+
+#define BCM_ACTION_RFAWARE      0x77
+#define BCM_ACTION_RFAWARE_DCS  0x01
+
+
+
+#define WL_11N_2x2          1
+#define WL_11N_3x3          3
+#define WL_11N_4x4          4
+
+
+#define WLFEATURE_DISABLE_11N       0x00000001
+#define WLFEATURE_DISABLE_11N_STBC_TX   0x00000002
+#define WLFEATURE_DISABLE_11N_STBC_RX   0x00000004
+#define WLFEATURE_DISABLE_11N_SGI_TX    0x00000008
+#define WLFEATURE_DISABLE_11N_SGI_RX    0x00000010
+#define WLFEATURE_DISABLE_11N_AMPDU_TX  0x00000020
+#define WLFEATURE_DISABLE_11N_AMPDU_RX  0x00000040
+#define WLFEATURE_DISABLE_11N_GF    0x00000080
+
+
+#define LQ_IDX_LAST             3
+#define MCS_INDEX_SIZE          33
+
+#define LQ_IDX_MIN              0
+#define LQ_IDX_MAX              1
+#define LQ_IDX_AVG              2
+#define LQ_IDX_SUM              2
+#define LQ_IDX_LAST             3
+#define LQ_STOP_MONITOR         0
+#define LQ_START_MONITOR        1
+
+#define LINKQUAL_V1     0x01
+
+struct  wl_lq {
+	int32 enable;
+	int32 rssi[LQ_IDX_LAST];    
+	int32 rssicnt;
+	int32 snr[LQ_IDX_LAST];     
+	uint32 nsamples;            
+	uint8 isvalid;              
+	uint8 version;
+}; 
+
+typedef struct wl_lq wl_lq_t;
+typedef struct wl_lq  wl_lq_stats_t;
+
+typedef struct {
+	struct  ether_addr ea;  
+	uint8   ac_cat; 
+	uint8   num_pkts;   
+} wl_mac_ratehisto_cmd_t;   
+
+
+typedef struct {
+	uint32  rate[WLC_MAXRATE + 1];  
+	uint32  mcs_index[MCS_INDEX_SIZE];  
+	uint32  tsf_timer[2][2];    
+} wl_mac_ratehisto_res_t;   
+
+#ifdef PROP_TXSTATUS
+
+
+#define WLFC_FLAGS_RSSI_SIGNALS                         1
+
+
+#define WLFC_FLAGS_XONXOFF_SIGNALS                      2
+
+
+#define WLFC_FLAGS_CREDIT_STATUS_SIGNALS        4
+
+#define WLFC_FLAGS_HOST_PROPTXSTATUS_ACTIVE     8
+#define WLFC_FLAGS_PSQ_GENERATIONFSM_ENABLE     16
+#define WLFC_FLAGS_PSQ_ZERO_BUFFER_ENABLE	32
+#endif 
+
+#define BTA_STATE_LOG_SZ    64
+
+
+enum {
+	HCIReset = 1,
+	HCIReadLocalAMPInfo,
+	HCIReadLocalAMPASSOC,
+	HCIWriteRemoteAMPASSOC,
+	HCICreatePhysicalLink,
+	HCIAcceptPhysicalLinkRequest,
+	HCIDisconnectPhysicalLink,
+	HCICreateLogicalLink,
+	HCIAcceptLogicalLink,
+	HCIDisconnectLogicalLink,
+	HCILogicalLinkCancel,
+	HCIAmpStateChange,
+	HCIWriteLogicalLinkAcceptTimeout
+};
+
+typedef struct flush_txfifo {
+	uint32 txfifobmp;
+	uint32 hwtxfifoflush;
+	struct ether_addr ea;
+} flush_txfifo_t;
+
+#define CHANNEL_5G_LOW_START    36      
+#define CHANNEL_5G_MID_START    52      
+#define CHANNEL_5G_HIGH_START   100     
+#define CHANNEL_5G_UPPER_START  149     
+
+enum {
+	SPATIAL_MODE_2G_IDX = 0,
+	SPATIAL_MODE_5G_LOW_IDX,
+	SPATIAL_MODE_5G_MID_IDX,
+	SPATIAL_MODE_5G_HIGH_IDX,
+	SPATIAL_MODE_5G_UPPER_IDX,
+	SPATIAL_MODE_MAX_IDX
+};
+
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/linux_osl.c b/drivers/net/wireless/bcmdhd/linux_osl.c
new file mode 100644
index 000000000000..4ef7bf7b24dc
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/linux_osl.c
@@ -0,0 +1,924 @@
+/*
+ * Linux OS Independent Layer
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: linux_osl.c,v 1.168.2.7 2011-01-27 17:01:13 $
+ */
+
+
+#define LINUX_PORT
+
+#include <typedefs.h>
+#include <bcmendian.h>
+#include <linuxver.h>
+#include <bcmdefs.h>
+#include <osl.h>
+#include <bcmutils.h>
+#include <linux/delay.h>
+#include <pcicfg.h>
+
+#ifdef BCMASSERT_LOG
+#include <bcm_assert_log.h>
+#endif
+
+#include <linux/fs.h>
+
+#define PCI_CFG_RETRY 		10
+
+#define OS_HANDLE_MAGIC		0x1234abcd	
+#define BCM_MEM_FILENAME_LEN 	24		
+
+#ifdef CONFIG_DHD_USE_STATIC_BUF
+#define STATIC_BUF_MAX_NUM	16
+#define STATIC_BUF_SIZE		(PAGE_SIZE * 2)
+#define STATIC_BUF_TOTAL_LEN	(STATIC_BUF_MAX_NUM * STATIC_BUF_SIZE)
+
+typedef struct bcm_static_buf {
+	struct semaphore static_sem;
+	unsigned char *buf_ptr;
+	unsigned char buf_use[STATIC_BUF_MAX_NUM];
+} bcm_static_buf_t;
+
+static bcm_static_buf_t *bcm_static_buf = 0;
+
+#define STATIC_PKT_MAX_NUM	8
+
+typedef struct bcm_static_pkt {
+	struct sk_buff *skb_4k[STATIC_PKT_MAX_NUM];
+	struct sk_buff *skb_8k[STATIC_PKT_MAX_NUM];
+	struct semaphore osl_pkt_sem;
+	unsigned char pkt_use[STATIC_PKT_MAX_NUM * 2];
+} bcm_static_pkt_t;
+
+static bcm_static_pkt_t *bcm_static_skb = 0;
+#endif
+
+typedef struct bcm_mem_link {
+	struct bcm_mem_link *prev;
+	struct bcm_mem_link *next;
+	uint	size;
+	int	line;
+	char	file[BCM_MEM_FILENAME_LEN];
+} bcm_mem_link_t;
+
+struct osl_info {
+	osl_pubinfo_t pub;
+#ifdef CTFPOOL
+	ctfpool_t *ctfpool;
+#endif 
+	uint magic;
+	void *pdev;
+	atomic_t malloced;
+	uint failed;
+	uint bustype;
+	bcm_mem_link_t *dbgmem_list;
+};
+
+
+
+
+uint32 g_assert_type = FALSE;
+
+static int16 linuxbcmerrormap[] =
+{	0, 			
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL,		
+	-EINVAL, 		
+	-EINVAL, 		
+	-EINVAL, 		
+	-E2BIG,			
+	-E2BIG,			
+	-EBUSY, 		
+	-EINVAL, 		
+	-EINVAL, 		
+	-EINVAL, 		
+	-EINVAL, 		
+	-EFAULT, 		
+	-ENOMEM, 		
+	-EOPNOTSUPP,		
+	-EMSGSIZE,		
+	-EINVAL,		
+	-EPERM,			
+	-ENOMEM, 		
+	-EINVAL, 		
+	-ERANGE, 		
+	-EINVAL, 		
+	-EINVAL, 		
+	-EINVAL, 		
+	-EINVAL, 		
+	-EINVAL,		
+	-EIO,			
+	-ENODEV,		
+	-EINVAL,		
+	-EIO,			
+	-EIO,			
+	-ENODEV,		
+	-EINVAL,		
+	-ENODATA,		
+
+
+
+#if BCME_LAST != -42
+#error "You need to add a OS error translation in the linuxbcmerrormap \
+	for new error code defined in bcmutils.h"
+#endif
+};
+
+
+int
+osl_error(int bcmerror)
+{
+	if (bcmerror > 0)
+		bcmerror = 0;
+	else if (bcmerror < BCME_LAST)
+		bcmerror = BCME_ERROR;
+
+	
+	return linuxbcmerrormap[-bcmerror];
+}
+
+extern uint8* dhd_os_prealloc(void *osh, int section, int size);
+
+osl_t *
+osl_attach(void *pdev, uint bustype, bool pkttag)
+{
+	osl_t *osh;
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
+	gfp_t flags;
+
+	flags = (in_atomic()) ? GFP_ATOMIC : GFP_KERNEL;
+	osh = kmalloc(sizeof(osl_t), flags);
+#else
+	osh = kmalloc(sizeof(osl_t), GFP_ATOMIC);
+#endif 
+	ASSERT(osh);
+
+	bzero(osh, sizeof(osl_t));
+
+	
+	ASSERT(ABS(BCME_LAST) == (ARRAYSIZE(linuxbcmerrormap) - 1));
+
+	osh->magic = OS_HANDLE_MAGIC;
+	atomic_set(&osh->malloced, 0);
+	osh->failed = 0;
+	osh->dbgmem_list = NULL;
+	osh->pdev = pdev;
+	osh->pub.pkttag = pkttag;
+	osh->bustype = bustype;
+
+	switch (bustype) {
+		case PCI_BUS:
+		case SI_BUS:
+		case PCMCIA_BUS:
+			osh->pub.mmbus = TRUE;
+			break;
+		case JTAG_BUS:
+		case SDIO_BUS:
+		case USB_BUS:
+		case SPI_BUS:
+		case RPC_BUS:
+			osh->pub.mmbus = FALSE;
+			break;
+		default:
+			ASSERT(FALSE);
+			break;
+	}
+
+#if defined(CONFIG_DHD_USE_STATIC_BUF)
+	if (!bcm_static_buf) {
+		if (!(bcm_static_buf = (bcm_static_buf_t *)dhd_os_prealloc(osh, 3, STATIC_BUF_SIZE+
+			STATIC_BUF_TOTAL_LEN))) {
+			printk("can not alloc static buf!\n");
+		}
+		else
+			printk("alloc static buf at %x!\n", (unsigned int)bcm_static_buf);
+
+		sema_init(&bcm_static_buf->static_sem, 1);
+
+		bcm_static_buf->buf_ptr = (unsigned char *)bcm_static_buf + STATIC_BUF_SIZE;
+	}
+
+	if (!bcm_static_skb) {
+		int i;
+		void *skb_buff_ptr = 0;
+		bcm_static_skb = (bcm_static_pkt_t *)((char *)bcm_static_buf + 2048);
+		skb_buff_ptr = dhd_os_prealloc(osh, 4, 0);
+
+		bcopy(skb_buff_ptr, bcm_static_skb, sizeof(struct sk_buff *) * 16);
+		for (i = 0; i < STATIC_PKT_MAX_NUM * 2; i++)
+			bcm_static_skb->pkt_use[i] = 0;
+
+		sema_init(&bcm_static_skb->osl_pkt_sem, 1);
+	}
+#endif
+
+	return osh;
+}
+
+void
+osl_detach(osl_t *osh)
+{
+	if (osh == NULL)
+		return;
+
+	ASSERT(osh->magic == OS_HANDLE_MAGIC);
+	kfree(osh);
+}
+
+static struct sk_buff *osl_alloc_skb(unsigned int len)
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25)
+	gfp_t flags = (in_atomic()) ? GFP_ATOMIC : GFP_KERNEL;
+
+	return __dev_alloc_skb(len, flags);
+#else
+	return dev_alloc_skb(len);
+#endif
+}
+
+#ifdef CTFPOOL
+
+void *
+osl_ctfpool_add(osl_t *osh)
+{
+	struct sk_buff *skb;
+
+	if ((osh == NULL) || (osh->ctfpool == NULL))
+		return NULL;
+
+	spin_lock_bh(&osh->ctfpool->lock);
+	ASSERT(osh->ctfpool->curr_obj <= osh->ctfpool->max_obj);
+
+	
+	if (osh->ctfpool->curr_obj == osh->ctfpool->max_obj) {
+		spin_unlock_bh(&osh->ctfpool->lock);
+		return NULL;
+	}
+
+	
+	skb = osl_alloc_skb(osh->ctfpool->obj_size);
+	if (skb == NULL) {
+		printf("%s: skb alloc of len %d failed\n", __FUNCTION__,
+		       osh->ctfpool->obj_size);
+		spin_unlock_bh(&osh->ctfpool->lock);
+		return NULL;
+	}
+
+	
+	skb->next = (struct sk_buff *)osh->ctfpool->head;
+	osh->ctfpool->head = skb;
+	osh->ctfpool->fast_frees++;
+	osh->ctfpool->curr_obj++;
+
+	
+	CTFPOOLPTR(osh, skb) = (void *)osh->ctfpool;
+
+	
+	PKTFAST(osh, skb) = FASTBUF;
+
+	spin_unlock_bh(&osh->ctfpool->lock);
+
+	return skb;
+}
+
+
+void
+osl_ctfpool_replenish(osl_t *osh, uint thresh)
+{
+	if ((osh == NULL) || (osh->ctfpool == NULL))
+		return;
+
+	
+	while ((osh->ctfpool->refills > 0) && (thresh--)) {
+		osl_ctfpool_add(osh);
+		osh->ctfpool->refills--;
+	}
+}
+
+
+int32
+osl_ctfpool_init(osl_t *osh, uint numobj, uint size)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
+	gfp_t flags;
+
+	flags = (in_atomic()) ? GFP_ATOMIC : GFP_KERNEL;
+	osh->ctfpool = kmalloc(sizeof(ctfpool_t), flags);
+#else
+	osh->ctfpool = kmalloc(sizeof(ctfpool_t), GFP_ATOMIC);
+#endif 
+	ASSERT(osh->ctfpool);
+	bzero(osh->ctfpool, sizeof(ctfpool_t));
+
+	osh->ctfpool->max_obj = numobj;
+	osh->ctfpool->obj_size = size;
+
+	spin_lock_init(&osh->ctfpool->lock);
+
+	while (numobj--) {
+		if (!osl_ctfpool_add(osh))
+			return -1;
+		osh->ctfpool->fast_frees--;
+	}
+
+	return 0;
+}
+
+
+void
+osl_ctfpool_cleanup(osl_t *osh)
+{
+	struct sk_buff *skb, *nskb;
+
+	if ((osh == NULL) || (osh->ctfpool == NULL))
+		return;
+
+	spin_lock_bh(&osh->ctfpool->lock);
+
+	skb = osh->ctfpool->head;
+
+	while (skb != NULL) {
+		nskb = skb->next;
+		dev_kfree_skb(skb);
+		skb = nskb;
+		osh->ctfpool->curr_obj--;
+	}
+
+	ASSERT(osh->ctfpool->curr_obj == 0);
+	osh->ctfpool->head = NULL;
+	spin_unlock_bh(&osh->ctfpool->lock);
+
+	kfree(osh->ctfpool);
+	osh->ctfpool = NULL;
+}
+
+void
+osl_ctfpool_stats(osl_t *osh, void *b)
+{
+	struct bcmstrbuf *bb;
+
+	if ((osh == NULL) || (osh->ctfpool == NULL))
+		return;
+
+#ifdef CONFIG_DHD_USE_STATIC_BUF
+	if (bcm_static_buf) {
+		bcm_static_buf = 0;
+	}
+	if (bcm_static_skb) {
+		bcm_static_skb = 0;
+	}
+#endif 
+
+	bb = b;
+
+	ASSERT((osh != NULL) && (bb != NULL));
+
+	bcm_bprintf(bb, "max_obj %d obj_size %d curr_obj %d refills %d\n",
+	            osh->ctfpool->max_obj, osh->ctfpool->obj_size,
+	            osh->ctfpool->curr_obj, osh->ctfpool->refills);
+	bcm_bprintf(bb, "fast_allocs %d fast_frees %d slow_allocs %d\n",
+	            osh->ctfpool->fast_allocs, osh->ctfpool->fast_frees,
+	            osh->ctfpool->slow_allocs);
+}
+
+static inline struct sk_buff *
+osl_pktfastget(osl_t *osh, uint len)
+{
+	struct sk_buff *skb;
+
+	
+	if (osh->ctfpool == NULL)
+		return NULL;
+
+	spin_lock_bh(&osh->ctfpool->lock);
+	if (osh->ctfpool->head == NULL) {
+		ASSERT(osh->ctfpool->curr_obj == 0);
+		osh->ctfpool->slow_allocs++;
+		spin_unlock_bh(&osh->ctfpool->lock);
+		return NULL;
+	}
+
+	ASSERT(len <= osh->ctfpool->obj_size);
+
+	
+	skb = (struct sk_buff *)osh->ctfpool->head;
+	osh->ctfpool->head = (void *)skb->next;
+
+	osh->ctfpool->fast_allocs++;
+	osh->ctfpool->curr_obj--;
+	ASSERT(CTFPOOLHEAD(osh, skb) == (struct sock *)osh->ctfpool->head);
+	spin_unlock_bh(&osh->ctfpool->lock);
+
+	
+	skb->next = skb->prev = NULL;
+	skb->data = skb->head + 16;
+	skb->tail = skb->head + 16;
+
+	skb->len = 0;
+	skb->cloned = 0;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 14)
+	skb->list = NULL;
+#endif
+	atomic_set(&skb->users, 1);
+
+	return skb;
+}
+#endif
+
+
+void * BCMFASTPATH
+osl_pktget(osl_t *osh, uint len)
+{
+	struct sk_buff *skb;
+
+#ifdef CTFPOOL
+	skb = osl_pktfastget(osh, len);
+	if ((skb != NULL) || ((skb = osl_alloc_skb(len)) != NULL)) {
+#else
+	if ((skb = osl_alloc_skb(len))) {
+#endif
+		skb_put(skb, len);
+		skb->priority = 0;
+
+		osh->pub.pktalloced++;
+	}
+
+	return ((void*) skb);
+}
+
+#ifdef CTFPOOL
+static inline void
+osl_pktfastfree(osl_t *osh, struct sk_buff *skb)
+{
+	ctfpool_t *ctfpool;
+
+	ctfpool = (ctfpool_t *)CTFPOOLPTR(osh, skb);
+	ASSERT(ctfpool != NULL);
+
+	
+	spin_lock_bh(&ctfpool->lock);
+	skb->next = (struct sk_buff *)ctfpool->head;
+	ctfpool->head = (void *)skb;
+
+	ctfpool->fast_frees++;
+	ctfpool->curr_obj++;
+
+	ASSERT(ctfpool->curr_obj <= ctfpool->max_obj);
+	spin_unlock_bh(&ctfpool->lock);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 14)
+	skb->tstamp.tv.sec = 0;
+#else
+	skb->stamp.tv_sec = 0;
+#endif
+
+	
+	skb->dev = NULL;
+	skb->dst = NULL;
+	memset(skb->cb, 0, sizeof(skb->cb));
+	skb->ip_summed = 0;
+	skb->destructor = NULL;
+}
+#endif 
+
+
+void BCMFASTPATH
+osl_pktfree(osl_t *osh, void *p, bool send)
+{
+	struct sk_buff *skb, *nskb;
+
+	skb = (struct sk_buff*) p;
+
+	if (send && osh->pub.tx_fn)
+		osh->pub.tx_fn(osh->pub.tx_ctx, p, 0);
+
+	
+	while (skb) {
+		nskb = skb->next;
+		skb->next = NULL;
+
+
+#ifdef CTFPOOL
+		if (PKTISFAST(osh, skb))
+			osl_pktfastfree(osh, skb);
+		else {
+#else 
+		{
+#endif 
+
+			if (skb->destructor)
+				
+				dev_kfree_skb_any(skb);
+			else
+				
+				dev_kfree_skb(skb);
+		}
+
+		osh->pub.pktalloced--;
+
+		skb = nskb;
+	}
+}
+
+#ifdef CONFIG_DHD_USE_STATIC_BUF
+void *
+osl_pktget_static(osl_t *osh, uint len)
+{
+	int i;
+	struct sk_buff *skb;
+
+	if (!bcm_static_skb || (len > (PAGE_SIZE * 2))) {
+		printk("%s: attempt to allocate huge packet (0x%x)\n", __FUNCTION__, len);
+		return osl_pktget(osh, len);
+	}
+
+	down(&bcm_static_skb->osl_pkt_sem);
+
+	if (len <= PAGE_SIZE) {
+		for (i = 0; i < STATIC_PKT_MAX_NUM; i++) {
+			if (bcm_static_skb->pkt_use[i] == 0)
+				break;
+		}
+
+		if (i != STATIC_PKT_MAX_NUM) {
+			bcm_static_skb->pkt_use[i] = 1;
+			skb = bcm_static_skb->skb_4k[i];
+			skb->tail = skb->data + len;
+			skb->len = len;
+			up(&bcm_static_skb->osl_pkt_sem);
+			return skb;
+		}
+	}
+
+
+	for (i = 0; i < STATIC_PKT_MAX_NUM; i++) {
+		if (bcm_static_skb->pkt_use[i+STATIC_PKT_MAX_NUM] == 0)
+			break;
+	}
+
+	if (i != STATIC_PKT_MAX_NUM) {
+		bcm_static_skb->pkt_use[i+STATIC_PKT_MAX_NUM] = 1;
+		skb = bcm_static_skb->skb_8k[i];
+		skb->tail = skb->data + len;
+		skb->len = len;
+		up(&bcm_static_skb->osl_pkt_sem);
+		return skb;
+	}
+
+	up(&bcm_static_skb->osl_pkt_sem);
+	printk("%s: all static pkt in use!\n", __FUNCTION__);
+	return osl_pktget(osh, len);
+}
+
+void
+osl_pktfree_static(osl_t *osh, void *p, bool send)
+{
+	int i;
+
+	if (!bcm_static_skb) {
+		osl_pktfree(osh, p, send);
+		return;
+	}
+
+	down(&bcm_static_skb->osl_pkt_sem);
+	for (i = 0; i < STATIC_PKT_MAX_NUM; i++) {
+		if (p == bcm_static_skb->skb_4k[i]) {
+			bcm_static_skb->pkt_use[i] = 0;
+			up(&bcm_static_skb->osl_pkt_sem);
+			return;
+		}
+	}
+
+	for (i = 0; i < STATIC_PKT_MAX_NUM; i++) {
+		if (p == bcm_static_skb->skb_8k[i]) {
+			bcm_static_skb->pkt_use[i + STATIC_PKT_MAX_NUM] = 0;
+			up(&bcm_static_skb->osl_pkt_sem);
+			return;
+		}
+	}
+	up(&bcm_static_skb->osl_pkt_sem);
+
+	osl_pktfree(osh, p, send);
+	return;
+}
+#endif 
+
+uint32
+osl_pci_read_config(osl_t *osh, uint offset, uint size)
+{
+	uint val = 0;
+	uint retry = PCI_CFG_RETRY;
+
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+
+	
+	ASSERT(size == 4);
+
+	do {
+		pci_read_config_dword(osh->pdev, offset, &val);
+		if (val != 0xffffffff)
+			break;
+	} while (retry--);
+
+
+	return (val);
+}
+
+void
+osl_pci_write_config(osl_t *osh, uint offset, uint size, uint val)
+{
+	uint retry = PCI_CFG_RETRY;
+
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+
+	
+	ASSERT(size == 4);
+
+	do {
+		pci_write_config_dword(osh->pdev, offset, val);
+		if (offset != PCI_BAR0_WIN)
+			break;
+		if (osl_pci_read_config(osh, offset, size) == val)
+			break;
+	} while (retry--);
+
+}
+
+
+uint
+osl_pci_bus(osl_t *osh)
+{
+	ASSERT(osh && (osh->magic == OS_HANDLE_MAGIC) && osh->pdev);
+
+	return ((struct pci_dev *)osh->pdev)->bus->number;
+}
+
+
+uint
+osl_pci_slot(osl_t *osh)
+{
+	ASSERT(osh && (osh->magic == OS_HANDLE_MAGIC) && osh->pdev);
+
+	return PCI_SLOT(((struct pci_dev *)osh->pdev)->devfn);
+}
+
+static void
+osl_pcmcia_attr(osl_t *osh, uint offset, char *buf, int size, bool write)
+{
+}
+
+void
+osl_pcmcia_read_attr(osl_t *osh, uint offset, void *buf, int size)
+{
+	osl_pcmcia_attr(osh, offset, (char *) buf, size, FALSE);
+}
+
+void
+osl_pcmcia_write_attr(osl_t *osh, uint offset, void *buf, int size)
+{
+	osl_pcmcia_attr(osh, offset, (char *) buf, size, TRUE);
+}
+
+void *
+osl_malloc(osl_t *osh, uint size)
+{
+	void *addr;
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
+	gfp_t flags;
+
+	
+	if (osh)
+		ASSERT(osh->magic == OS_HANDLE_MAGIC);
+
+	flags = (in_atomic()) ? GFP_ATOMIC : GFP_KERNEL;
+	if ((addr = kmalloc(size, flags)) == NULL) {
+#else
+	if ((addr = kmalloc(size, GFP_ATOMIC)) == NULL) {
+#endif 
+		if (osh)
+			osh->failed++;
+		return (NULL);
+	}
+	if (osh)
+		atomic_add(size, &osh->malloced);
+
+	return (addr);
+}
+
+void
+osl_mfree(osl_t *osh, void *addr, uint size)
+{
+	if (osh) {
+		ASSERT(osh->magic == OS_HANDLE_MAGIC);
+		atomic_sub(size, &osh->malloced);
+	}
+	kfree(addr);
+}
+
+uint
+osl_malloced(osl_t *osh)
+{
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+	return (atomic_read(&osh->malloced));
+}
+
+uint
+osl_malloc_failed(osl_t *osh)
+{
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+	return (osh->failed);
+}
+
+
+
+uint
+osl_dma_consistent_align(void)
+{
+	return (PAGE_SIZE);
+}
+
+void*
+osl_dma_alloc_consistent(osl_t *osh, uint size, uint16 align_bits, uint *alloced, ulong *pap)
+{
+	uint16 align = (1 << align_bits);
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+
+	if (!ISALIGNED(DMA_CONSISTENT_ALIGN, align))
+		size += align;
+	*alloced = size;
+
+	return (pci_alloc_consistent(osh->pdev, size, (dma_addr_t*)pap));
+}
+
+void
+osl_dma_free_consistent(osl_t *osh, void *va, uint size, ulong pa)
+{
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+
+	pci_free_consistent(osh->pdev, size, va, (dma_addr_t)pa);
+}
+
+uint BCMFASTPATH
+osl_dma_map(osl_t *osh, void *va, uint size, int direction)
+{
+	int dir;
+
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+	dir = (direction == DMA_TX)? PCI_DMA_TODEVICE: PCI_DMA_FROMDEVICE;
+	return (pci_map_single(osh->pdev, va, size, dir));
+}
+
+void BCMFASTPATH
+osl_dma_unmap(osl_t *osh, uint pa, uint size, int direction)
+{
+	int dir;
+
+	ASSERT((osh && (osh->magic == OS_HANDLE_MAGIC)));
+	dir = (direction == DMA_TX)? PCI_DMA_TODEVICE: PCI_DMA_FROMDEVICE;
+	pci_unmap_single(osh->pdev, (uint32)pa, size, dir);
+}
+
+#if defined(BCMASSERT_LOG)
+void
+osl_assert(char *exp, char *file, int line)
+{
+	char tempbuf[256];
+	char *basename;
+
+	basename = strrchr(file, '/');
+	
+	if (basename)
+		basename++;
+
+	if (!basename)
+		basename = file;
+
+#ifdef BCMASSERT_LOG
+	snprintf(tempbuf, 64, "\"%s\": file \"%s\", line %d\n",
+		exp, basename, line);
+
+	bcm_assert_log(tempbuf);
+#endif 
+
+
+}
+#endif 
+
+void
+osl_delay(uint usec)
+{
+	uint d;
+
+	while (usec > 0) {
+		d = MIN(usec, 1000);
+		udelay(d);
+		usec -= d;
+	}
+}
+
+
+
+void *
+osl_pktdup(osl_t *osh, void *skb)
+{
+	void * p;
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25))
+	gfp_t flags;
+
+	flags = (in_atomic()) ? GFP_ATOMIC : GFP_KERNEL;
+	if ((p = skb_clone((struct sk_buff *)skb, flags)) == NULL)
+#else
+	if ((p = skb_clone((struct sk_buff*)skb, GFP_ATOMIC)) == NULL)
+#endif 
+		return NULL;
+
+#ifdef CTFPOOL
+	if (PKTISFAST(osh, skb)) {
+		ctfpool_t *ctfpool;
+
+		
+		ctfpool = (ctfpool_t *)CTFPOOLPTR(osh, skb);
+		ASSERT(ctfpool != NULL);
+		PKTCLRFAST(osh, p);
+		PKTCLRFAST(osh, skb);
+		ctfpool->refills++;
+	}
+#endif 
+
+	
+	if (osh->pub.pkttag)
+		bzero((void*)((struct sk_buff *)p)->cb, OSL_PKTTAG_SZ);
+
+	
+	osh->pub.pktalloced++;
+	return (p);
+}
+
+
+
+
+
+
+
+void *
+osl_os_open_image(char *filename)
+{
+	struct file *fp;
+
+	fp = filp_open(filename, O_RDONLY, 0);
+	
+	 if (IS_ERR(fp))
+		 fp = NULL;
+
+	 return fp;
+}
+
+int
+osl_os_get_image_block(char *buf, int len, void *image)
+{
+	struct file *fp = (struct file *)image;
+	int rdlen;
+
+	if (!image)
+		return 0;
+
+	rdlen = kernel_read(fp, fp->f_pos, buf, len);
+	if (rdlen > 0)
+		fp->f_pos += rdlen;
+
+	return rdlen;
+}
+
+void
+osl_os_close_image(void *image)
+{
+	if (image)
+		filp_close((struct file *)image, NULL);
+}
diff --git a/drivers/net/wireless/bcmdhd/sbutils.c b/drivers/net/wireless/bcmdhd/sbutils.c
new file mode 100644
index 000000000000..02d1bc0a79d1
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/sbutils.c
@@ -0,0 +1,992 @@
+/*
+ * Misc utility routines for accessing chip-specific features
+ * of the SiliconBackplane-based Broadcom chips.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: sbutils.c,v 1.687.2.1 2010-11-29 20:21:56 Exp $
+ */
+
+#include <typedefs.h>
+#include <bcmdefs.h>
+#include <osl.h>
+#include <bcmutils.h>
+#include <siutils.h>
+#include <bcmdevs.h>
+#include <hndsoc.h>
+#include <sbchipc.h>
+#include <pcicfg.h>
+#include <sbpcmcia.h>
+
+#include "siutils_priv.h"
+
+
+/* local prototypes */
+static uint _sb_coreidx(si_info_t *sii, uint32 sba);
+static uint _sb_scan(si_info_t *sii, uint32 sba, void *regs, uint bus, uint32 sbba,
+                     uint ncores);
+static uint32 _sb_coresba(si_info_t *sii);
+static void *_sb_setcoreidx(si_info_t *sii, uint coreidx);
+
+#define	SET_SBREG(sii, r, mask, val)	\
+		W_SBREG((sii), (r), ((R_SBREG((sii), (r)) & ~(mask)) | (val)))
+#define	REGS2SB(va)	(sbconfig_t*) ((int8*)(va) + SBCONFIGOFF)
+
+/* sonicsrev */
+#define	SONICS_2_2	(SBIDL_RV_2_2 >> SBIDL_RV_SHIFT)
+#define	SONICS_2_3	(SBIDL_RV_2_3 >> SBIDL_RV_SHIFT)
+
+#define	R_SBREG(sii, sbr)	sb_read_sbreg((sii), (sbr))
+#define	W_SBREG(sii, sbr, v)	sb_write_sbreg((sii), (sbr), (v))
+#define	AND_SBREG(sii, sbr, v)	W_SBREG((sii), (sbr), (R_SBREG((sii), (sbr)) & (v)))
+#define	OR_SBREG(sii, sbr, v)	W_SBREG((sii), (sbr), (R_SBREG((sii), (sbr)) | (v)))
+
+static uint32
+sb_read_sbreg(si_info_t *sii, volatile uint32 *sbr)
+{
+	uint8 tmp;
+	uint32 val, intr_val = 0;
+
+
+	/*
+	 * compact flash only has 11 bits address, while we needs 12 bits address.
+	 * MEM_SEG will be OR'd with other 11 bits address in hardware,
+	 * so we program MEM_SEG with 12th bit when necessary(access sb regsiters).
+	 * For normal PCMCIA bus(CFTable_regwinsz > 2k), do nothing special
+	 */
+	if (PCMCIA(sii)) {
+		INTR_OFF(sii, intr_val);
+		tmp = 1;
+		OSL_PCMCIA_WRITE_ATTR(sii->osh, MEM_SEG, &tmp, 1);
+		sbr = (volatile uint32 *)((uintptr)sbr & ~(1 << 11)); /* mask out bit 11 */
+	}
+
+	val = R_REG(sii->osh, sbr);
+
+	if (PCMCIA(sii)) {
+		tmp = 0;
+		OSL_PCMCIA_WRITE_ATTR(sii->osh, MEM_SEG, &tmp, 1);
+		INTR_RESTORE(sii, intr_val);
+	}
+
+	return (val);
+}
+
+static void
+sb_write_sbreg(si_info_t *sii, volatile uint32 *sbr, uint32 v)
+{
+	uint8 tmp;
+	volatile uint32 dummy;
+	uint32 intr_val = 0;
+
+
+	/*
+	 * compact flash only has 11 bits address, while we needs 12 bits address.
+	 * MEM_SEG will be OR'd with other 11 bits address in hardware,
+	 * so we program MEM_SEG with 12th bit when necessary(access sb regsiters).
+	 * For normal PCMCIA bus(CFTable_regwinsz > 2k), do nothing special
+	 */
+	if (PCMCIA(sii)) {
+		INTR_OFF(sii, intr_val);
+		tmp = 1;
+		OSL_PCMCIA_WRITE_ATTR(sii->osh, MEM_SEG, &tmp, 1);
+		sbr = (volatile uint32 *)((uintptr)sbr & ~(1 << 11)); /* mask out bit 11 */
+	}
+
+	if (BUSTYPE(sii->pub.bustype) == PCMCIA_BUS) {
+		dummy = R_REG(sii->osh, sbr);
+		W_REG(sii->osh, (volatile uint16 *)sbr, (uint16)(v & 0xffff));
+		dummy = R_REG(sii->osh, sbr);
+		W_REG(sii->osh, ((volatile uint16 *)sbr + 1), (uint16)((v >> 16) & 0xffff));
+	} else
+		W_REG(sii->osh, sbr, v);
+
+	if (PCMCIA(sii)) {
+		tmp = 0;
+		OSL_PCMCIA_WRITE_ATTR(sii->osh, MEM_SEG, &tmp, 1);
+		INTR_RESTORE(sii, intr_val);
+	}
+}
+
+uint
+sb_coreid(si_t *sih)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	return ((R_SBREG(sii, &sb->sbidhigh) & SBIDH_CC_MASK) >> SBIDH_CC_SHIFT);
+}
+
+uint
+sb_intflag(si_t *sih)
+{
+	si_info_t *sii;
+	void *corereg;
+	sbconfig_t *sb;
+	uint origidx, intflag, intr_val = 0;
+
+	sii = SI_INFO(sih);
+
+	INTR_OFF(sii, intr_val);
+	origidx = si_coreidx(sih);
+	corereg = si_setcore(sih, CC_CORE_ID, 0);
+	ASSERT(corereg != NULL);
+	sb = REGS2SB(corereg);
+	intflag = R_SBREG(sii, &sb->sbflagst);
+	sb_setcoreidx(sih, origidx);
+	INTR_RESTORE(sii, intr_val);
+
+	return intflag;
+}
+
+uint
+sb_flag(si_t *sih)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	return R_SBREG(sii, &sb->sbtpsflag) & SBTPS_NUM0_MASK;
+}
+
+void
+sb_setint(si_t *sih, int siflag)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+	uint32 vec;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	if (siflag == -1)
+		vec = 0;
+	else
+		vec = 1 << siflag;
+	W_SBREG(sii, &sb->sbintvec, vec);
+}
+
+/* return core index of the core with address 'sba' */
+static uint
+_sb_coreidx(si_info_t *sii, uint32 sba)
+{
+	uint i;
+
+	for (i = 0; i < sii->numcores; i ++)
+		if (sba == sii->coresba[i])
+			return i;
+	return BADIDX;
+}
+
+/* return core address of the current core */
+static uint32
+_sb_coresba(si_info_t *sii)
+{
+	uint32 sbaddr;
+
+
+	switch (BUSTYPE(sii->pub.bustype)) {
+	case SI_BUS: {
+		sbconfig_t *sb = REGS2SB(sii->curmap);
+		sbaddr = sb_base(R_SBREG(sii, &sb->sbadmatch0));
+		break;
+	}
+
+	case PCI_BUS:
+		sbaddr = OSL_PCI_READ_CONFIG(sii->osh, PCI_BAR0_WIN, sizeof(uint32));
+		break;
+
+	case PCMCIA_BUS: {
+		uint8 tmp = 0;
+		OSL_PCMCIA_READ_ATTR(sii->osh, PCMCIA_ADDR0, &tmp, 1);
+		sbaddr  = (uint32)tmp << 12;
+		OSL_PCMCIA_READ_ATTR(sii->osh, PCMCIA_ADDR1, &tmp, 1);
+		sbaddr |= (uint32)tmp << 16;
+		OSL_PCMCIA_READ_ATTR(sii->osh, PCMCIA_ADDR2, &tmp, 1);
+		sbaddr |= (uint32)tmp << 24;
+		break;
+	}
+
+	case SPI_BUS:
+	case SDIO_BUS:
+		sbaddr = (uint32)(uintptr)sii->curmap;
+		break;
+
+
+	default:
+		sbaddr = BADCOREADDR;
+		break;
+	}
+
+	return sbaddr;
+}
+
+uint
+sb_corevendor(si_t *sih)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	return ((R_SBREG(sii, &sb->sbidhigh) & SBIDH_VC_MASK) >> SBIDH_VC_SHIFT);
+}
+
+uint
+sb_corerev(si_t *sih)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+	uint sbidh;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+	sbidh = R_SBREG(sii, &sb->sbidhigh);
+
+	return (SBCOREREV(sbidh));
+}
+
+/* set core-specific control flags */
+void
+sb_core_cflags_wo(si_t *sih, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+	uint32 w;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	ASSERT((val & ~mask) == 0);
+
+	/* mask and set */
+	w = (R_SBREG(sii, &sb->sbtmstatelow) & ~(mask << SBTML_SICF_SHIFT)) |
+	        (val << SBTML_SICF_SHIFT);
+	W_SBREG(sii, &sb->sbtmstatelow, w);
+}
+
+/* set/clear core-specific control flags */
+uint32
+sb_core_cflags(si_t *sih, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+	uint32 w;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	ASSERT((val & ~mask) == 0);
+
+	/* mask and set */
+	if (mask || val) {
+		w = (R_SBREG(sii, &sb->sbtmstatelow) & ~(mask << SBTML_SICF_SHIFT)) |
+		        (val << SBTML_SICF_SHIFT);
+		W_SBREG(sii, &sb->sbtmstatelow, w);
+	}
+
+	/* return the new value
+	 * for write operation, the following readback ensures the completion of write opration.
+	 */
+	return (R_SBREG(sii, &sb->sbtmstatelow) >> SBTML_SICF_SHIFT);
+}
+
+/* set/clear core-specific status flags */
+uint32
+sb_core_sflags(si_t *sih, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+	uint32 w;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	ASSERT((val & ~mask) == 0);
+	ASSERT((mask & ~SISF_CORE_BITS) == 0);
+
+	/* mask and set */
+	if (mask || val) {
+		w = (R_SBREG(sii, &sb->sbtmstatehigh) & ~(mask << SBTMH_SISF_SHIFT)) |
+		        (val << SBTMH_SISF_SHIFT);
+		W_SBREG(sii, &sb->sbtmstatehigh, w);
+	}
+
+	/* return the new value */
+	return (R_SBREG(sii, &sb->sbtmstatehigh) >> SBTMH_SISF_SHIFT);
+}
+
+bool
+sb_iscoreup(si_t *sih)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	return ((R_SBREG(sii, &sb->sbtmstatelow) &
+	         (SBTML_RESET | SBTML_REJ_MASK | (SICF_CLOCK_EN << SBTML_SICF_SHIFT))) ==
+	        (SICF_CLOCK_EN << SBTML_SICF_SHIFT));
+}
+
+/*
+ * Switch to 'coreidx', issue a single arbitrary 32bit register mask&set operation,
+ * switch back to the original core, and return the new value.
+ *
+ * When using the silicon backplane, no fidleing with interrupts or core switches are needed.
+ *
+ * Also, when using pci/pcie, we can optimize away the core switching for pci registers
+ * and (on newer pci cores) chipcommon registers.
+ */
+uint
+sb_corereg(si_t *sih, uint coreidx, uint regoff, uint mask, uint val)
+{
+	uint origidx = 0;
+	uint32 *r = NULL;
+	uint w;
+	uint intr_val = 0;
+	bool fast = FALSE;
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	ASSERT(GOODIDX(coreidx));
+	ASSERT(regoff < SI_CORE_SIZE);
+	ASSERT((val & ~mask) == 0);
+
+	if (coreidx >= SI_MAXCORES)
+		return 0;
+
+	if (BUSTYPE(sii->pub.bustype) == SI_BUS) {
+		/* If internal bus, we can always get at everything */
+		fast = TRUE;
+		/* map if does not exist */
+		if (!sii->regs[coreidx]) {
+			sii->regs[coreidx] = REG_MAP(sii->coresba[coreidx],
+			                            SI_CORE_SIZE);
+			ASSERT(GOODREGS(sii->regs[coreidx]));
+		}
+		r = (uint32 *)((uchar *)sii->regs[coreidx] + regoff);
+	} else if (BUSTYPE(sii->pub.bustype) == PCI_BUS) {
+		/* If pci/pcie, we can get at pci/pcie regs and on newer cores to chipc */
+
+		if ((sii->coreid[coreidx] == CC_CORE_ID) && SI_FAST(sii)) {
+			/* Chipc registers are mapped at 12KB */
+
+			fast = TRUE;
+			r = (uint32 *)((char *)sii->curmap + PCI_16KB0_CCREGS_OFFSET + regoff);
+		} else if (sii->pub.buscoreidx == coreidx) {
+			/* pci registers are at either in the last 2KB of an 8KB window
+			 * or, in pcie and pci rev 13 at 8KB
+			 */
+			fast = TRUE;
+			if (SI_FAST(sii))
+				r = (uint32 *)((char *)sii->curmap +
+				               PCI_16KB0_PCIREGS_OFFSET + regoff);
+			else
+				r = (uint32 *)((char *)sii->curmap +
+				               ((regoff >= SBCONFIGOFF) ?
+				                PCI_BAR0_PCISBR_OFFSET : PCI_BAR0_PCIREGS_OFFSET) +
+				               regoff);
+		}
+	}
+
+	if (!fast) {
+		INTR_OFF(sii, intr_val);
+
+		/* save current core index */
+		origidx = si_coreidx(&sii->pub);
+
+		/* switch core */
+		r = (uint32*) ((uchar*)sb_setcoreidx(&sii->pub, coreidx) + regoff);
+	}
+	ASSERT(r != NULL);
+
+	/* mask and set */
+	if (mask || val) {
+		if (regoff >= SBCONFIGOFF) {
+			w = (R_SBREG(sii, r) & ~mask) | val;
+			W_SBREG(sii, r, w);
+		} else {
+			w = (R_REG(sii->osh, r) & ~mask) | val;
+			W_REG(sii->osh, r, w);
+		}
+	}
+
+	/* readback */
+	if (regoff >= SBCONFIGOFF)
+		w = R_SBREG(sii, r);
+	else {
+		if ((CHIPID(sii->pub.chip) == BCM5354_CHIP_ID) &&
+		    (coreidx == SI_CC_IDX) &&
+		    (regoff == OFFSETOF(chipcregs_t, watchdog))) {
+			w = val;
+		} else
+			w = R_REG(sii->osh, r);
+	}
+
+	if (!fast) {
+		/* restore core index */
+		if (origidx != coreidx)
+			sb_setcoreidx(&sii->pub, origidx);
+
+		INTR_RESTORE(sii, intr_val);
+	}
+
+	return (w);
+}
+
+/* Scan the enumeration space to find all cores starting from the given
+ * bus 'sbba'. Append coreid and other info to the lists in 'si'. 'sba'
+ * is the default core address at chip POR time and 'regs' is the virtual
+ * address that the default core is mapped at. 'ncores' is the number of
+ * cores expected on bus 'sbba'. It returns the total number of cores
+ * starting from bus 'sbba', inclusive.
+ */
+#define SB_MAXBUSES	2
+static uint
+_sb_scan(si_info_t *sii, uint32 sba, void *regs, uint bus, uint32 sbba, uint numcores)
+{
+	uint next;
+	uint ncc = 0;
+	uint i;
+
+	if (bus >= SB_MAXBUSES) {
+		SI_ERROR(("_sb_scan: bus 0x%08x at level %d is too deep to scan\n", sbba, bus));
+		return 0;
+	}
+	SI_MSG(("_sb_scan: scan bus 0x%08x assume %u cores\n", sbba, numcores));
+
+	/* Scan all cores on the bus starting from core 0.
+	 * Core addresses must be contiguous on each bus.
+	 */
+	for (i = 0, next = sii->numcores; i < numcores && next < SB_BUS_MAXCORES; i++, next++) {
+		sii->coresba[next] = sbba + (i * SI_CORE_SIZE);
+
+		/* keep and reuse the initial register mapping */
+		if ((BUSTYPE(sii->pub.bustype) == SI_BUS) && (sii->coresba[next] == sba)) {
+			SI_VMSG(("_sb_scan: reuse mapped regs %p for core %u\n", regs, next));
+			sii->regs[next] = regs;
+		}
+
+		/* change core to 'next' and read its coreid */
+		sii->curmap = _sb_setcoreidx(sii, next);
+		sii->curidx = next;
+
+		sii->coreid[next] = sb_coreid(&sii->pub);
+
+		/* core specific processing... */
+		/* chipc provides # cores */
+		if (sii->coreid[next] == CC_CORE_ID) {
+			chipcregs_t *cc = (chipcregs_t *)sii->curmap;
+			uint32 ccrev = sb_corerev(&sii->pub);
+
+			/* determine numcores - this is the total # cores in the chip */
+			if (((ccrev == 4) || (ccrev >= 6)))
+				numcores = (R_REG(sii->osh, &cc->chipid) & CID_CC_MASK) >>
+				        CID_CC_SHIFT;
+			else {
+				/* Older chips */
+				uint chip = CHIPID(sii->pub.chip);
+
+				if (chip == BCM4306_CHIP_ID)	/* < 4306c0 */
+					numcores = 6;
+				else if (chip == BCM4704_CHIP_ID)
+					numcores = 9;
+				else if (chip == BCM5365_CHIP_ID)
+					numcores = 7;
+				else {
+					SI_ERROR(("sb_chip2numcores: unsupported chip 0x%x\n",
+					          chip));
+					ASSERT(0);
+					numcores = 1;
+				}
+			}
+			SI_VMSG(("_sb_scan: there are %u cores in the chip %s\n", numcores,
+				sii->pub.issim ? "QT" : ""));
+		}
+		/* scan bridged SB(s) and add results to the end of the list */
+		else if (sii->coreid[next] == OCP_CORE_ID) {
+			sbconfig_t *sb = REGS2SB(sii->curmap);
+			uint32 nsbba = R_SBREG(sii, &sb->sbadmatch1);
+			uint nsbcc;
+
+			sii->numcores = next + 1;
+
+			if ((nsbba & 0xfff00000) != SI_ENUM_BASE)
+				continue;
+			nsbba &= 0xfffff000;
+			if (_sb_coreidx(sii, nsbba) != BADIDX)
+				continue;
+
+			nsbcc = (R_SBREG(sii, &sb->sbtmstatehigh) & 0x000f0000) >> 16;
+			nsbcc = _sb_scan(sii, sba, regs, bus + 1, nsbba, nsbcc);
+			if (sbba == SI_ENUM_BASE)
+				numcores -= nsbcc;
+			ncc += nsbcc;
+		}
+	}
+
+	SI_MSG(("_sb_scan: found %u cores on bus 0x%08x\n", i, sbba));
+
+	sii->numcores = i + ncc;
+	return sii->numcores;
+}
+
+/* scan the sb enumerated space to identify all cores */
+void
+sb_scan(si_t *sih, void *regs, uint devid)
+{
+	si_info_t *sii;
+	uint32 origsba;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	sii->pub.socirev = (R_SBREG(sii, &sb->sbidlow) & SBIDL_RV_MASK) >> SBIDL_RV_SHIFT;
+
+	/* Save the current core info and validate it later till we know
+	 * for sure what is good and what is bad.
+	 */
+	origsba = _sb_coresba(sii);
+
+	/* scan all SB(s) starting from SI_ENUM_BASE */
+	sii->numcores = _sb_scan(sii, origsba, regs, 0, SI_ENUM_BASE, 1);
+}
+
+/*
+ * This function changes logical "focus" to the indicated core;
+ * must be called with interrupts off.
+ * Moreover, callers should keep interrupts off during switching out of and back to d11 core
+ */
+void *
+sb_setcoreidx(si_t *sih, uint coreidx)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	if (coreidx >= sii->numcores)
+		return (NULL);
+
+	/*
+	 * If the user has provided an interrupt mask enabled function,
+	 * then assert interrupts are disabled before switching the core.
+	 */
+	ASSERT((sii->intrsenabled_fn == NULL) || !(*(sii)->intrsenabled_fn)((sii)->intr_arg));
+
+	sii->curmap = _sb_setcoreidx(sii, coreidx);
+	sii->curidx = coreidx;
+
+	return (sii->curmap);
+}
+
+/* This function changes the logical "focus" to the indicated core.
+ * Return the current core's virtual address.
+ */
+static void *
+_sb_setcoreidx(si_info_t *sii, uint coreidx)
+{
+	uint32 sbaddr = sii->coresba[coreidx];
+	void *regs;
+
+	switch (BUSTYPE(sii->pub.bustype)) {
+	case SI_BUS:
+		/* map new one */
+		if (!sii->regs[coreidx]) {
+			sii->regs[coreidx] = REG_MAP(sbaddr, SI_CORE_SIZE);
+			ASSERT(GOODREGS(sii->regs[coreidx]));
+		}
+		regs = sii->regs[coreidx];
+		break;
+
+	case PCI_BUS:
+		/* point bar0 window */
+		OSL_PCI_WRITE_CONFIG(sii->osh, PCI_BAR0_WIN, 4, sbaddr);
+		regs = sii->curmap;
+		break;
+
+	case PCMCIA_BUS: {
+		uint8 tmp = (sbaddr >> 12) & 0x0f;
+		OSL_PCMCIA_WRITE_ATTR(sii->osh, PCMCIA_ADDR0, &tmp, 1);
+		tmp = (sbaddr >> 16) & 0xff;
+		OSL_PCMCIA_WRITE_ATTR(sii->osh, PCMCIA_ADDR1, &tmp, 1);
+		tmp = (sbaddr >> 24) & 0xff;
+		OSL_PCMCIA_WRITE_ATTR(sii->osh, PCMCIA_ADDR2, &tmp, 1);
+		regs = sii->curmap;
+		break;
+	}
+	case SPI_BUS:
+	case SDIO_BUS:
+		/* map new one */
+		if (!sii->regs[coreidx]) {
+			sii->regs[coreidx] = (void *)(uintptr)sbaddr;
+			ASSERT(GOODREGS(sii->regs[coreidx]));
+		}
+		regs = sii->regs[coreidx];
+		break;
+
+
+	default:
+		ASSERT(0);
+		regs = NULL;
+		break;
+	}
+
+	return regs;
+}
+
+/* Return the address of sbadmatch0/1/2/3 register */
+static volatile uint32 *
+sb_admatch(si_info_t *sii, uint asidx)
+{
+	sbconfig_t *sb;
+	volatile uint32 *addrm;
+
+	sb = REGS2SB(sii->curmap);
+
+	switch (asidx) {
+	case 0:
+		addrm =  &sb->sbadmatch0;
+		break;
+
+	case 1:
+		addrm =  &sb->sbadmatch1;
+		break;
+
+	case 2:
+		addrm =  &sb->sbadmatch2;
+		break;
+
+	case 3:
+		addrm =  &sb->sbadmatch3;
+		break;
+
+	default:
+		SI_ERROR(("%s: Address space index (%d) out of range\n", __FUNCTION__, asidx));
+		return 0;
+	}
+
+	return (addrm);
+}
+
+/* Return the number of address spaces in current core */
+int
+sb_numaddrspaces(si_t *sih)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+	sb = REGS2SB(sii->curmap);
+
+	/* + 1 because of enumeration space */
+	return ((R_SBREG(sii, &sb->sbidlow) & SBIDL_AR_MASK) >> SBIDL_AR_SHIFT) + 1;
+}
+
+/* Return the address of the nth address space in the current core */
+uint32
+sb_addrspace(si_t *sih, uint asidx)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	return (sb_base(R_SBREG(sii, sb_admatch(sii, asidx))));
+}
+
+/* Return the size of the nth address space in the current core */
+uint32
+sb_addrspacesize(si_t *sih, uint asidx)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	return (sb_size(R_SBREG(sii, sb_admatch(sii, asidx))));
+}
+
+
+/* do buffered registers update */
+void
+sb_commit(si_t *sih)
+{
+	si_info_t *sii;
+	uint origidx;
+	uint intr_val = 0;
+
+	sii = SI_INFO(sih);
+
+	origidx = sii->curidx;
+	ASSERT(GOODIDX(origidx));
+
+	INTR_OFF(sii, intr_val);
+
+	/* switch over to chipcommon core if there is one, else use pci */
+	if (sii->pub.ccrev != NOREV) {
+		chipcregs_t *ccregs = (chipcregs_t *)si_setcore(sih, CC_CORE_ID, 0);
+		ASSERT(ccregs != NULL);
+
+		/* do the buffer registers update */
+		W_REG(sii->osh, &ccregs->broadcastaddress, SB_COMMIT);
+		W_REG(sii->osh, &ccregs->broadcastdata, 0x0);
+	} else
+		ASSERT(0);
+
+	/* restore core index */
+	sb_setcoreidx(sih, origidx);
+	INTR_RESTORE(sii, intr_val);
+}
+
+void
+sb_core_disable(si_t *sih, uint32 bits)
+{
+	si_info_t *sii;
+	volatile uint32 dummy;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+
+	ASSERT(GOODREGS(sii->curmap));
+	sb = REGS2SB(sii->curmap);
+
+	/* if core is already in reset, just return */
+	if (R_SBREG(sii, &sb->sbtmstatelow) & SBTML_RESET)
+		return;
+
+	/* if clocks are not enabled, put into reset and return */
+	if ((R_SBREG(sii, &sb->sbtmstatelow) & (SICF_CLOCK_EN << SBTML_SICF_SHIFT)) == 0)
+		goto disable;
+
+	/* set target reject and spin until busy is clear (preserve core-specific bits) */
+	OR_SBREG(sii, &sb->sbtmstatelow, SBTML_REJ);
+	dummy = R_SBREG(sii, &sb->sbtmstatelow);
+	OSL_DELAY(1);
+	SPINWAIT((R_SBREG(sii, &sb->sbtmstatehigh) & SBTMH_BUSY), 100000);
+	if (R_SBREG(sii, &sb->sbtmstatehigh) & SBTMH_BUSY)
+		SI_ERROR(("%s: target state still busy\n", __FUNCTION__));
+
+	if (R_SBREG(sii, &sb->sbidlow) & SBIDL_INIT) {
+		OR_SBREG(sii, &sb->sbimstate, SBIM_RJ);
+		dummy = R_SBREG(sii, &sb->sbimstate);
+		OSL_DELAY(1);
+		SPINWAIT((R_SBREG(sii, &sb->sbimstate) & SBIM_BY), 100000);
+	}
+
+	/* set reset and reject while enabling the clocks */
+	W_SBREG(sii, &sb->sbtmstatelow,
+	        (((bits | SICF_FGC | SICF_CLOCK_EN) << SBTML_SICF_SHIFT) |
+	         SBTML_REJ | SBTML_RESET));
+	dummy = R_SBREG(sii, &sb->sbtmstatelow);
+	OSL_DELAY(10);
+
+	/* don't forget to clear the initiator reject bit */
+	if (R_SBREG(sii, &sb->sbidlow) & SBIDL_INIT)
+		AND_SBREG(sii, &sb->sbimstate, ~SBIM_RJ);
+
+disable:
+	/* leave reset and reject asserted */
+	W_SBREG(sii, &sb->sbtmstatelow, ((bits << SBTML_SICF_SHIFT) | SBTML_REJ | SBTML_RESET));
+	OSL_DELAY(1);
+}
+
+/* reset and re-enable a core
+ * inputs:
+ * bits - core specific bits that are set during and after reset sequence
+ * resetbits - core specific bits that are set only during reset sequence
+ */
+void
+sb_core_reset(si_t *sih, uint32 bits, uint32 resetbits)
+{
+	si_info_t *sii;
+	sbconfig_t *sb;
+	volatile uint32 dummy;
+
+	sii = SI_INFO(sih);
+	ASSERT(GOODREGS(sii->curmap));
+	sb = REGS2SB(sii->curmap);
+
+	/*
+	 * Must do the disable sequence first to work for arbitrary current core state.
+	 */
+	sb_core_disable(sih, (bits | resetbits));
+
+	/*
+	 * Now do the initialization sequence.
+	 */
+
+	/* set reset while enabling the clock and forcing them on throughout the core */
+	W_SBREG(sii, &sb->sbtmstatelow,
+	        (((bits | resetbits | SICF_FGC | SICF_CLOCK_EN) << SBTML_SICF_SHIFT) |
+	         SBTML_RESET));
+	dummy = R_SBREG(sii, &sb->sbtmstatelow);
+	OSL_DELAY(1);
+
+	if (R_SBREG(sii, &sb->sbtmstatehigh) & SBTMH_SERR) {
+		W_SBREG(sii, &sb->sbtmstatehigh, 0);
+	}
+	if ((dummy = R_SBREG(sii, &sb->sbimstate)) & (SBIM_IBE | SBIM_TO)) {
+		AND_SBREG(sii, &sb->sbimstate, ~(SBIM_IBE | SBIM_TO));
+	}
+
+	/* clear reset and allow it to propagate throughout the core */
+	W_SBREG(sii, &sb->sbtmstatelow,
+	        ((bits | resetbits | SICF_FGC | SICF_CLOCK_EN) << SBTML_SICF_SHIFT));
+	dummy = R_SBREG(sii, &sb->sbtmstatelow);
+	OSL_DELAY(1);
+
+	/* leave clock enabled */
+	W_SBREG(sii, &sb->sbtmstatelow, ((bits | SICF_CLOCK_EN) << SBTML_SICF_SHIFT));
+	dummy = R_SBREG(sii, &sb->sbtmstatelow);
+	OSL_DELAY(1);
+}
+
+/*
+ * Set the initiator timeout for the "master core".
+ * The master core is defined to be the core in control
+ * of the chip and so it issues accesses to non-memory
+ * locations (Because of dma *any* core can access memeory).
+ *
+ * The routine uses the bus to decide who is the master:
+ *	SI_BUS => mips
+ *	JTAG_BUS => chipc
+ *	PCI_BUS => pci or pcie
+ *	PCMCIA_BUS => pcmcia
+ *	SDIO_BUS => pcmcia
+ *
+ * This routine exists so callers can disable initiator
+ * timeouts so accesses to very slow devices like otp
+ * won't cause an abort. The routine allows arbitrary
+ * settings of the service and request timeouts, though.
+ *
+ * Returns the timeout state before changing it or -1
+ * on error.
+ */
+
+#define	TO_MASK	(SBIMCL_RTO_MASK | SBIMCL_STO_MASK)
+
+uint32
+sb_set_initiator_to(si_t *sih, uint32 to, uint idx)
+{
+	si_info_t *sii;
+	uint origidx;
+	uint intr_val = 0;
+	uint32 tmp, ret = 0xffffffff;
+	sbconfig_t *sb;
+
+	sii = SI_INFO(sih);
+
+	if ((to & ~TO_MASK) != 0)
+		return ret;
+
+	/* Figure out the master core */
+	if (idx == BADIDX) {
+		switch (BUSTYPE(sii->pub.bustype)) {
+		case PCI_BUS:
+			idx = sii->pub.buscoreidx;
+			break;
+		case JTAG_BUS:
+			idx = SI_CC_IDX;
+			break;
+		case PCMCIA_BUS:
+		case SDIO_BUS:
+			idx = si_findcoreidx(sih, PCMCIA_CORE_ID, 0);
+			break;
+		case SI_BUS:
+			idx = si_findcoreidx(sih, MIPS33_CORE_ID, 0);
+			break;
+		default:
+			ASSERT(0);
+		}
+		if (idx == BADIDX)
+			return ret;
+	}
+
+	INTR_OFF(sii, intr_val);
+	origidx = si_coreidx(sih);
+
+	sb = REGS2SB(sb_setcoreidx(sih, idx));
+
+	tmp = R_SBREG(sii, &sb->sbimconfiglow);
+	ret = tmp & TO_MASK;
+	W_SBREG(sii, &sb->sbimconfiglow, (tmp & ~TO_MASK) | to);
+
+	sb_commit(sih);
+	sb_setcoreidx(sih, origidx);
+	INTR_RESTORE(sii, intr_val);
+	return ret;
+}
+
+uint32
+sb_base(uint32 admatch)
+{
+	uint32 base;
+	uint type;
+
+	type = admatch & SBAM_TYPE_MASK;
+	ASSERT(type < 3);
+
+	base = 0;
+
+	if (type == 0) {
+		base = admatch & SBAM_BASE0_MASK;
+	} else if (type == 1) {
+		ASSERT(!(admatch & SBAM_ADNEG));	/* neg not supported */
+		base = admatch & SBAM_BASE1_MASK;
+	} else if (type == 2) {
+		ASSERT(!(admatch & SBAM_ADNEG));	/* neg not supported */
+		base = admatch & SBAM_BASE2_MASK;
+	}
+
+	return (base);
+}
+
+uint32
+sb_size(uint32 admatch)
+{
+	uint32 size;
+	uint type;
+
+	type = admatch & SBAM_TYPE_MASK;
+	ASSERT(type < 3);
+
+	size = 0;
+
+	if (type == 0) {
+		size = 1 << (((admatch & SBAM_ADINT0_MASK) >> SBAM_ADINT0_SHIFT) + 1);
+	} else if (type == 1) {
+		ASSERT(!(admatch & SBAM_ADNEG));	/* neg not supported */
+		size = 1 << (((admatch & SBAM_ADINT1_MASK) >> SBAM_ADINT1_SHIFT) + 1);
+	} else if (type == 2) {
+		ASSERT(!(admatch & SBAM_ADNEG));	/* neg not supported */
+		size = 1 << (((admatch & SBAM_ADINT2_MASK) >> SBAM_ADINT2_SHIFT) + 1);
+	}
+
+	return (size);
+}
diff --git a/drivers/net/wireless/bcmdhd/siutils.c b/drivers/net/wireless/bcmdhd/siutils.c
new file mode 100644
index 000000000000..a655ac4ef141
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/siutils.c
@@ -0,0 +1,1915 @@
+/*
+ * Misc utility routines for accessing chip-specific features
+ * of the SiliconBackplane-based Broadcom chips.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: siutils.c,v 1.813.2.36 2011-02-10 23:43:55 $
+ */
+
+#include <typedefs.h>
+#include <bcmdefs.h>
+#include <osl.h>
+#include <bcmutils.h>
+#include <siutils.h>
+#include <bcmdevs.h>
+#include <hndsoc.h>
+#include <sbchipc.h>
+#include <pcicfg.h>
+#include <sbpcmcia.h>
+#include <sbsocram.h>
+#include <bcmsdh.h>
+#include <sdio.h>
+#include <sbsdio.h>
+#include <sbhnddma.h>
+#include <sbsdpcmdev.h>
+#include <bcmsdpcm.h>
+#include <hndpmu.h>
+
+#include "siutils_priv.h"
+
+/* local prototypes */
+static si_info_t *si_doattach(si_info_t *sii, uint devid, osl_t *osh, void *regs,
+                              uint bustype, void *sdh, char **vars, uint *varsz);
+static bool si_buscore_prep(si_info_t *sii, uint bustype, uint devid, void *sdh);
+static bool si_buscore_setup(si_info_t *sii, chipcregs_t *cc, uint bustype, uint32 savewin,
+	uint *origidx, void *regs);
+
+
+/* global variable to indicate reservation/release of gpio's */
+static uint32 si_gpioreservation = 0;
+
+/* global flag to prevent shared resources from being initialized multiple times in si_attach() */
+
+/*
+ * Allocate a si handle.
+ * devid - pci device id (used to determine chip#)
+ * osh - opaque OS handle
+ * regs - virtual address of initial core registers
+ * bustype - pci/pcmcia/sb/sdio/etc
+ * vars - pointer to a pointer area for "environment" variables
+ * varsz - pointer to int to return the size of the vars
+ */
+si_t *
+si_attach(uint devid, osl_t *osh, void *regs,
+                       uint bustype, void *sdh, char **vars, uint *varsz)
+{
+	si_info_t *sii;
+
+	/* alloc si_info_t */
+	if ((sii = MALLOC(osh, sizeof (si_info_t))) == NULL) {
+		SI_ERROR(("si_attach: malloc failed! malloced %d bytes\n", MALLOCED(osh)));
+		return (NULL);
+	}
+
+	if (si_doattach(sii, devid, osh, regs, bustype, sdh, vars, varsz) == NULL) {
+		MFREE(osh, sii, sizeof(si_info_t));
+		return (NULL);
+	}
+	sii->vars = vars ? *vars : NULL;
+	sii->varsz = varsz ? *varsz : 0;
+
+	return (si_t *)sii;
+}
+
+/* global kernel resource */
+static si_info_t ksii;
+
+static uint32	wd_msticks;		/* watchdog timer ticks normalized to ms */
+
+/* generic kernel variant of si_attach() */
+si_t *
+si_kattach(osl_t *osh)
+{
+	static bool ksii_attached = FALSE;
+
+	if (!ksii_attached) {
+		void *regs;
+		regs = REG_MAP(SI_ENUM_BASE, SI_CORE_SIZE);
+
+		if (si_doattach(&ksii, BCM4710_DEVICE_ID, osh, regs,
+		                SI_BUS, NULL,
+		                osh != SI_OSH ? &ksii.vars : NULL,
+		                osh != SI_OSH ? &ksii.varsz : NULL) == NULL) {
+			SI_ERROR(("si_kattach: si_doattach failed\n"));
+			REG_UNMAP(regs);
+			return NULL;
+		}
+		REG_UNMAP(regs);
+
+		/* save ticks normalized to ms for si_watchdog_ms() */
+		if (PMUCTL_ENAB(&ksii.pub)) {
+			/* based on 32KHz ILP clock */
+			wd_msticks = 32;
+		} else {
+			wd_msticks = ALP_CLOCK / 1000;
+		}
+
+		ksii_attached = TRUE;
+		SI_MSG(("si_kattach done. ccrev = %d, wd_msticks = %d\n",
+		        ksii.pub.ccrev, wd_msticks));
+	}
+
+	return &ksii.pub;
+}
+
+
+static bool
+si_buscore_prep(si_info_t *sii, uint bustype, uint devid, void *sdh)
+{
+	/* need to set memseg flag for CF card first before any sb registers access */
+	if (BUSTYPE(bustype) == PCMCIA_BUS)
+		sii->memseg = TRUE;
+
+
+	if (BUSTYPE(bustype) == SDIO_BUS) {
+		int err;
+		uint8 clkset;
+
+		/* Try forcing SDIO core to do ALPAvail request only */
+		clkset = SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_ALP_AVAIL_REQ;
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, clkset, &err);
+		if (!err) {
+			uint8 clkval;
+
+			/* If register supported, wait for ALPAvail and then force ALP */
+			clkval = bcmsdh_cfg_read(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, NULL);
+			if ((clkval & ~SBSDIO_AVBITS) == clkset) {
+				SPINWAIT(((clkval = bcmsdh_cfg_read(sdh, SDIO_FUNC_1,
+					SBSDIO_FUNC1_CHIPCLKCSR, NULL)), !SBSDIO_ALPAV(clkval)),
+					PMU_MAX_TRANSITION_DLY);
+				if (!SBSDIO_ALPAV(clkval)) {
+					SI_ERROR(("timeout on ALPAV wait, clkval 0x%02x\n",
+						clkval));
+					return FALSE;
+				}
+				clkset = SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_FORCE_ALP;
+				bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
+					clkset, &err);
+				OSL_DELAY(65);
+			}
+		}
+
+		/* Also, disable the extra SDIO pull-ups */
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, SBSDIO_FUNC1_SDIOPULLUP, 0, NULL);
+	}
+
+
+	return TRUE;
+}
+
+static bool
+si_buscore_setup(si_info_t *sii, chipcregs_t *cc, uint bustype, uint32 savewin,
+	uint *origidx, void *regs)
+{
+	bool pci, pcie;
+	uint i;
+	uint pciidx, pcieidx, pcirev, pcierev;
+
+	cc = si_setcoreidx(&sii->pub, SI_CC_IDX);
+	ASSERT((uintptr)cc);
+
+	/* get chipcommon rev */
+	sii->pub.ccrev = (int)si_corerev(&sii->pub);
+
+	/* get chipcommon chipstatus */
+	if (sii->pub.ccrev >= 11)
+		sii->pub.chipst = R_REG(sii->osh, &cc->chipstatus);
+
+	/* get chipcommon capabilites */
+	sii->pub.cccaps = R_REG(sii->osh, &cc->capabilities);
+	/* get chipcommon extended capabilities */
+
+	if (sii->pub.ccrev >= 35)
+		sii->pub.cccaps_ext = R_REG(sii->osh, &cc->capabilities_ext);
+
+	/* get pmu rev and caps */
+	if (sii->pub.cccaps & CC_CAP_PMU) {
+		sii->pub.pmucaps = R_REG(sii->osh, &cc->pmucapabilities);
+		sii->pub.pmurev = sii->pub.pmucaps & PCAP_REV_MASK;
+	}
+
+	SI_MSG(("Chipc: rev %d, caps 0x%x, chipst 0x%x pmurev %d, pmucaps 0x%x\n",
+		sii->pub.ccrev, sii->pub.cccaps, sii->pub.chipst, sii->pub.pmurev,
+		sii->pub.pmucaps));
+
+	/* figure out bus/orignal core idx */
+	sii->pub.buscoretype = NODEV_CORE_ID;
+	sii->pub.buscorerev = (uint)NOREV;
+	sii->pub.buscoreidx = BADIDX;
+
+	pci = pcie = FALSE;
+	pcirev = pcierev = (uint)NOREV;
+	pciidx = pcieidx = BADIDX;
+
+	for (i = 0; i < sii->numcores; i++) {
+		uint cid, crev;
+
+		si_setcoreidx(&sii->pub, i);
+		cid = si_coreid(&sii->pub);
+		crev = si_corerev(&sii->pub);
+
+		/* Display cores found */
+		SI_VMSG(("CORE[%d]: id 0x%x rev %d base 0x%x regs 0x%p\n",
+		        i, cid, crev, sii->coresba[i], sii->regs[i]));
+
+		if (BUSTYPE(bustype) == PCI_BUS) {
+			if (cid == PCI_CORE_ID) {
+				pciidx = i;
+				pcirev = crev;
+				pci = TRUE;
+			} else if (cid == PCIE_CORE_ID) {
+				pcieidx = i;
+				pcierev = crev;
+				pcie = TRUE;
+			}
+		} else if ((BUSTYPE(bustype) == PCMCIA_BUS) &&
+		           (cid == PCMCIA_CORE_ID)) {
+			sii->pub.buscorerev = crev;
+			sii->pub.buscoretype = cid;
+			sii->pub.buscoreidx = i;
+		}
+		else if (((BUSTYPE(bustype) == SDIO_BUS) ||
+		          (BUSTYPE(bustype) == SPI_BUS)) &&
+		         ((cid == PCMCIA_CORE_ID) ||
+		          (cid == SDIOD_CORE_ID))) {
+			sii->pub.buscorerev = crev;
+			sii->pub.buscoretype = cid;
+			sii->pub.buscoreidx = i;
+		}
+
+		/* find the core idx before entering this func. */
+		if ((savewin && (savewin == sii->coresba[i])) ||
+		    (regs == sii->regs[i]))
+			*origidx = i;
+	}
+
+	if (pci) {
+		sii->pub.buscoretype = PCI_CORE_ID;
+		sii->pub.buscorerev = pcirev;
+		sii->pub.buscoreidx = pciidx;
+	} else if (pcie) {
+		sii->pub.buscoretype = PCIE_CORE_ID;
+		sii->pub.buscorerev = pcierev;
+		sii->pub.buscoreidx = pcieidx;
+	}
+
+	SI_VMSG(("Buscore id/type/rev %d/0x%x/%d\n", sii->pub.buscoreidx, sii->pub.buscoretype,
+	         sii->pub.buscorerev));
+
+	if (BUSTYPE(sii->pub.bustype) == SI_BUS && (CHIPID(sii->pub.chip) == BCM4712_CHIP_ID) &&
+	    (sii->pub.chippkg != BCM4712LARGE_PKG_ID) && (CHIPREV(sii->pub.chiprev) <= 3))
+		OR_REG(sii->osh, &cc->slow_clk_ctl, SCC_SS_XTAL);
+
+
+	/* Make sure any on-chip ARM is off (in case strapping is wrong), or downloaded code was
+	 * already running.
+	 */
+	if ((BUSTYPE(bustype) == SDIO_BUS) || (BUSTYPE(bustype) == SPI_BUS)) {
+		if (si_setcore(&sii->pub, ARM7S_CORE_ID, 0) ||
+		    si_setcore(&sii->pub, ARMCM3_CORE_ID, 0))
+			si_core_disable(&sii->pub, 0);
+	}
+
+	/* return to the original core */
+	si_setcoreidx(&sii->pub, *origidx);
+
+	return TRUE;
+}
+
+
+
+static si_info_t *
+si_doattach(si_info_t *sii, uint devid, osl_t *osh, void *regs,
+                       uint bustype, void *sdh, char **vars, uint *varsz)
+{
+	struct si_pub *sih = &sii->pub;
+	uint32 w, savewin;
+	chipcregs_t *cc;
+	char *pvars = NULL;
+	uint origidx;
+
+	ASSERT(GOODREGS(regs));
+
+	bzero((uchar*)sii, sizeof(si_info_t));
+
+	savewin = 0;
+
+	sih->buscoreidx = BADIDX;
+
+	sii->curmap = regs;
+	sii->sdh = sdh;
+	sii->osh = osh;
+
+
+
+	/* find Chipcommon address */
+	if (bustype == PCI_BUS) {
+		savewin = OSL_PCI_READ_CONFIG(sii->osh, PCI_BAR0_WIN, sizeof(uint32));
+		if (!GOODCOREADDR(savewin, SI_ENUM_BASE))
+			savewin = SI_ENUM_BASE;
+		OSL_PCI_WRITE_CONFIG(sii->osh, PCI_BAR0_WIN, 4, SI_ENUM_BASE);
+		cc = (chipcregs_t *)regs;
+	} else if ((bustype == SDIO_BUS) || (bustype == SPI_BUS)) {
+		cc = (chipcregs_t *)sii->curmap;
+	} else {
+		cc = (chipcregs_t *)REG_MAP(SI_ENUM_BASE, SI_CORE_SIZE);
+	}
+
+	sih->bustype = bustype;
+	if (bustype != BUSTYPE(bustype)) {
+		SI_ERROR(("si_doattach: bus type %d does not match configured bus type %d\n",
+			bustype, BUSTYPE(bustype)));
+		return NULL;
+	}
+
+	/* bus/core/clk setup for register access */
+	if (!si_buscore_prep(sii, bustype, devid, sdh)) {
+		SI_ERROR(("si_doattach: si_core_clk_prep failed %d\n", bustype));
+		return NULL;
+	}
+
+	/* ChipID recognition.
+	 *   We assume we can read chipid at offset 0 from the regs arg.
+	 *   If we add other chiptypes (or if we need to support old sdio hosts w/o chipcommon),
+	 *   some way of recognizing them needs to be added here.
+	 */
+	w = R_REG(osh, &cc->chipid);
+	sih->socitype = (w & CID_TYPE_MASK) >> CID_TYPE_SHIFT;
+	/* Might as wll fill in chip id rev & pkg */
+	sih->chip = w & CID_ID_MASK;
+	sih->chiprev = (w & CID_REV_MASK) >> CID_REV_SHIFT;
+	sih->chippkg = (w & CID_PKG_MASK) >> CID_PKG_SHIFT;
+	if (CHIPID(sih->chip) == BCM4322_CHIP_ID && (((sih->chipst & CST4322_SPROM_OTP_SEL_MASK)
+		>> CST4322_SPROM_OTP_SEL_SHIFT) == (CST4322_OTP_PRESENT |
+		CST4322_SPROM_PRESENT))) {
+		SI_ERROR(("%s: Invalid setting: both SPROM and OTP strapped.\n", __FUNCTION__));
+		return NULL;
+	}
+
+#if defined(HW_OOB)
+	if (CHIPID(sih->chip) == BCM43362_CHIP_ID) {
+		uint32 gpiocontrol, addr;
+		addr = SI_ENUM_BASE + OFFSETOF(chipcregs_t, gpiocontrol);
+		gpiocontrol = bcmsdh_reg_read(sdh, addr, 4);
+		gpiocontrol |= 0x2;
+		bcmsdh_reg_write(sdh, addr, 4, gpiocontrol);
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, 0x10005, 0xf, NULL);
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, 0x10006, 0x0, NULL);
+		bcmsdh_cfg_write(sdh, SDIO_FUNC_1, 0x10007, 0x2, NULL);
+	}
+#endif
+
+	if ((CHIPID(sih->chip) == BCM4329_CHIP_ID) && (sih->chiprev == 0) &&
+		(sih->chippkg != BCM4329_289PIN_PKG_ID)) {
+		sih->chippkg = BCM4329_182PIN_PKG_ID;
+	}
+
+	sih->issim = IS_SIM(sih->chippkg);
+
+	/* scan for cores */
+	if (CHIPTYPE(sii->pub.socitype) == SOCI_SB) {
+		SI_MSG(("Found chip type SB (0x%08x)\n", w));
+		sb_scan(&sii->pub, regs, devid);
+	} else if (CHIPTYPE(sii->pub.socitype) == SOCI_AI) {
+		SI_MSG(("Found chip type AI (0x%08x)\n", w));
+		/* pass chipc address instead of original core base */
+		ai_scan(&sii->pub, (void *)(uintptr)cc, devid);
+	} else if (CHIPTYPE(sii->pub.socitype) == SOCI_UBUS) {
+		SI_MSG(("Found chip type UBUS (0x%08x), chip id = 0x%4x\n", w, sih->chip));
+		/* pass chipc address instead of original core base */
+		ub_scan(&sii->pub, (void *)(uintptr)cc, devid);
+	} else {
+		SI_ERROR(("Found chip of unknown type (0x%08x)\n", w));
+		return NULL;
+	}
+	/* no cores found, bail out */
+	if (sii->numcores == 0) {
+		SI_ERROR(("si_doattach: could not find any cores\n"));
+		return NULL;
+	}
+	/* bus/core/clk setup */
+	origidx = SI_CC_IDX;
+	if (!si_buscore_setup(sii, cc, bustype, savewin, &origidx, regs)) {
+		SI_ERROR(("si_doattach: si_buscore_setup failed\n"));
+		goto exit;
+	}
+
+	/* assume current core is CC */
+	if ((sii->pub.ccrev == 0x25) && ((CHIPID(sih->chip) == BCM43234_CHIP_ID ||
+	                                  CHIPID(sih->chip) == BCM43235_CHIP_ID ||
+	                                  CHIPID(sih->chip) == BCM43236_CHIP_ID ||
+	                                  CHIPID(sih->chip) == BCM43238_CHIP_ID) &&
+	                                 (CHIPREV(sii->pub.chiprev) == 0))) {
+
+		if ((cc->chipstatus & CST43236_BP_CLK) != 0) {
+			uint clkdiv;
+			clkdiv = R_REG(osh, &cc->clkdiv);
+			/* otp_clk_div is even number, 120/14 < 9mhz */
+			clkdiv = (clkdiv & ~CLKD_OTP) | (14 << CLKD_OTP_SHIFT);
+			W_REG(osh, &cc->clkdiv, clkdiv);
+			SI_ERROR(("%s: set clkdiv to %x\n", __FUNCTION__, clkdiv));
+		}
+		OSL_DELAY(10);
+	}
+
+
+	pvars = NULL;
+
+
+
+		if (sii->pub.ccrev >= 20) {
+			cc = (chipcregs_t *)si_setcore(sih, CC_CORE_ID, 0);
+			ASSERT(cc != NULL);
+			W_REG(osh, &cc->gpiopullup, 0);
+			W_REG(osh, &cc->gpiopulldown, 0);
+			si_setcoreidx(sih, origidx);
+		}
+
+
+
+
+	return (sii);
+
+exit:
+
+	return NULL;
+}
+
+/* may be called with core in reset */
+void
+si_detach(si_t *sih)
+{
+	si_info_t *sii;
+	uint idx;
+
+
+	sii = SI_INFO(sih);
+
+	if (sii == NULL)
+		return;
+
+	if (BUSTYPE(sih->bustype) == SI_BUS)
+		for (idx = 0; idx < SI_MAXCORES; idx++)
+			if (sii->regs[idx]) {
+				REG_UNMAP(sii->regs[idx]);
+				sii->regs[idx] = NULL;
+			}
+
+
+
+#if !defined(BCMBUSTYPE) || (BCMBUSTYPE == SI_BUS)
+	if (sii != &ksii)
+#endif	/* !BCMBUSTYPE || (BCMBUSTYPE == SI_BUS) */
+		MFREE(sii->osh, sii, sizeof(si_info_t));
+}
+
+void *
+si_osh(si_t *sih)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	return sii->osh;
+}
+
+void
+si_setosh(si_t *sih, osl_t *osh)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	if (sii->osh != NULL) {
+		SI_ERROR(("osh is already set....\n"));
+		ASSERT(!sii->osh);
+	}
+	sii->osh = osh;
+}
+
+/* register driver interrupt disabling and restoring callback functions */
+void
+si_register_intr_callback(si_t *sih, void *intrsoff_fn, void *intrsrestore_fn,
+                          void *intrsenabled_fn, void *intr_arg)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	sii->intr_arg = intr_arg;
+	sii->intrsoff_fn = (si_intrsoff_t)intrsoff_fn;
+	sii->intrsrestore_fn = (si_intrsrestore_t)intrsrestore_fn;
+	sii->intrsenabled_fn = (si_intrsenabled_t)intrsenabled_fn;
+	/* save current core id.  when this function called, the current core
+	 * must be the core which provides driver functions(il, et, wl, etc.)
+	 */
+	sii->dev_coreid = sii->coreid[sii->curidx];
+}
+
+void
+si_deregister_intr_callback(si_t *sih)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	sii->intrsoff_fn = NULL;
+}
+
+uint
+si_intflag(si_t *sih)
+{
+	si_info_t *sii = SI_INFO(sih);
+
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_intflag(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return R_REG(sii->osh, ((uint32 *)(uintptr)
+			    (sii->oob_router + OOB_STATUSA)));
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+uint
+si_flag(si_t *sih)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_flag(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_flag(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		return ub_flag(sih);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+void
+si_setint(si_t *sih, int siflag)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		sb_setint(sih, siflag);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		ai_setint(sih, siflag);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		ub_setint(sih, siflag);
+	else
+		ASSERT(0);
+}
+
+uint
+si_coreid(si_t *sih)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	return sii->coreid[sii->curidx];
+}
+
+uint
+si_coreidx(si_t *sih)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	return sii->curidx;
+}
+
+/* return the core-type instantiation # of the current core */
+uint
+si_coreunit(si_t *sih)
+{
+	si_info_t *sii;
+	uint idx;
+	uint coreid;
+	uint coreunit;
+	uint i;
+
+	sii = SI_INFO(sih);
+	coreunit = 0;
+
+	idx = sii->curidx;
+
+	ASSERT(GOODREGS(sii->curmap));
+	coreid = si_coreid(sih);
+
+	/* count the cores of our type */
+	for (i = 0; i < idx; i++)
+		if (sii->coreid[i] == coreid)
+			coreunit++;
+
+	return (coreunit);
+}
+
+uint
+si_corevendor(si_t *sih)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_corevendor(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_corevendor(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		return ub_corevendor(sih);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+bool
+si_backplane64(si_t *sih)
+{
+	return ((sih->cccaps & CC_CAP_BKPLN64) != 0);
+}
+
+uint
+si_corerev(si_t *sih)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_corerev(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_corerev(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		return ub_corerev(sih);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+/* return index of coreid or BADIDX if not found */
+uint
+si_findcoreidx(si_t *sih, uint coreid, uint coreunit)
+{
+	si_info_t *sii;
+	uint found;
+	uint i;
+
+	sii = SI_INFO(sih);
+
+	found = 0;
+
+	for (i = 0; i < sii->numcores; i++)
+		if (sii->coreid[i] == coreid) {
+			if (found == coreunit)
+				return (i);
+			found++;
+		}
+
+	return (BADIDX);
+}
+
+/* return list of found cores */
+uint
+si_corelist(si_t *sih, uint coreid[])
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	bcopy((uchar*)sii->coreid, (uchar*)coreid, (sii->numcores * sizeof(uint)));
+	return (sii->numcores);
+}
+
+/* return current register mapping */
+void *
+si_coreregs(si_t *sih)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	ASSERT(GOODREGS(sii->curmap));
+
+	return (sii->curmap);
+}
+
+/*
+ * This function changes logical "focus" to the indicated core;
+ * must be called with interrupts off.
+ * Moreover, callers should keep interrupts off during switching out of and back to d11 core
+ */
+void *
+si_setcore(si_t *sih, uint coreid, uint coreunit)
+{
+	uint idx;
+
+	idx = si_findcoreidx(sih, coreid, coreunit);
+	if (!GOODIDX(idx))
+		return (NULL);
+
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_setcoreidx(sih, idx);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_setcoreidx(sih, idx);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		return ub_setcoreidx(sih, idx);
+	else {
+		ASSERT(0);
+		return NULL;
+	}
+}
+
+void *
+si_setcoreidx(si_t *sih, uint coreidx)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_setcoreidx(sih, coreidx);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_setcoreidx(sih, coreidx);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		return ub_setcoreidx(sih, coreidx);
+	else {
+		ASSERT(0);
+		return NULL;
+	}
+}
+
+/* Turn off interrupt as required by sb_setcore, before switch core */
+void *
+si_switch_core(si_t *sih, uint coreid, uint *origidx, uint *intr_val)
+{
+	void *cc;
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	if (SI_FAST(sii)) {
+		/* Overloading the origidx variable to remember the coreid,
+		 * this works because the core ids cannot be confused with
+		 * core indices.
+		 */
+		*origidx = coreid;
+		if (coreid == CC_CORE_ID)
+			return (void *)CCREGS_FAST(sii);
+		else if (coreid == sih->buscoretype)
+			return (void *)PCIEREGS(sii);
+	}
+	INTR_OFF(sii, *intr_val);
+	*origidx = sii->curidx;
+	cc = si_setcore(sih, coreid, 0);
+	ASSERT(cc != NULL);
+
+	return cc;
+}
+
+/* restore coreidx and restore interrupt */
+void
+si_restore_core(si_t *sih, uint coreid, uint intr_val)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	if (SI_FAST(sii) && ((coreid == CC_CORE_ID) || (coreid == sih->buscoretype)))
+		return;
+
+	si_setcoreidx(sih, coreid);
+	INTR_RESTORE(sii, intr_val);
+}
+
+int
+si_numaddrspaces(si_t *sih)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_numaddrspaces(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_numaddrspaces(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		return ub_numaddrspaces(sih);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+uint32
+si_addrspace(si_t *sih, uint asidx)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_addrspace(sih, asidx);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_addrspace(sih, asidx);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		return ub_addrspace(sih, asidx);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+uint32
+si_addrspacesize(si_t *sih, uint asidx)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_addrspacesize(sih, asidx);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_addrspacesize(sih, asidx);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		return ub_addrspacesize(sih, asidx);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+uint32
+si_core_cflags(si_t *sih, uint32 mask, uint32 val)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_core_cflags(sih, mask, val);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_core_cflags(sih, mask, val);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		return ub_core_cflags(sih, mask, val);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+void
+si_core_cflags_wo(si_t *sih, uint32 mask, uint32 val)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		sb_core_cflags_wo(sih, mask, val);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		ai_core_cflags_wo(sih, mask, val);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		ub_core_cflags_wo(sih, mask, val);
+	else
+		ASSERT(0);
+}
+
+uint32
+si_core_sflags(si_t *sih, uint32 mask, uint32 val)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_core_sflags(sih, mask, val);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_core_sflags(sih, mask, val);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		return ub_core_sflags(sih, mask, val);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+bool
+si_iscoreup(si_t *sih)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_iscoreup(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_iscoreup(sih);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		return ub_iscoreup(sih);
+	else {
+		ASSERT(0);
+		return FALSE;
+	}
+}
+
+uint
+si_wrapperreg(si_t *sih, uint32 offset, uint32 mask, uint32 val)
+{
+	/* only for AI back plane chips */
+	if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return (ai_wrap_reg(sih, offset, mask, val));
+	return 0;
+}
+
+uint
+si_corereg(si_t *sih, uint coreidx, uint regoff, uint mask, uint val)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		return sb_corereg(sih, coreidx, regoff, mask, val);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		return ai_corereg(sih, coreidx, regoff, mask, val);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		return ub_corereg(sih, coreidx, regoff, mask, val);
+	else {
+		ASSERT(0);
+		return 0;
+	}
+}
+
+void
+si_core_disable(si_t *sih, uint32 bits)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		sb_core_disable(sih, bits);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		ai_core_disable(sih, bits);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		ub_core_disable(sih, bits);
+}
+
+void
+si_core_reset(si_t *sih, uint32 bits, uint32 resetbits)
+{
+	if (CHIPTYPE(sih->socitype) == SOCI_SB)
+		sb_core_reset(sih, bits, resetbits);
+	else if (CHIPTYPE(sih->socitype) == SOCI_AI)
+		ai_core_reset(sih, bits, resetbits);
+	else if (CHIPTYPE(sih->socitype) == SOCI_UBUS)
+		ub_core_reset(sih, bits, resetbits);
+}
+
+/* Run bist on current core. Caller needs to take care of core-specific bist hazards */
+int
+si_corebist(si_t *sih)
+{
+	uint32 cflags;
+	int result = 0;
+
+	/* Read core control flags */
+	cflags = si_core_cflags(sih, 0, 0);
+
+	/* Set bist & fgc */
+	si_core_cflags(sih, ~0, (SICF_BIST_EN | SICF_FGC));
+
+	/* Wait for bist done */
+	SPINWAIT(((si_core_sflags(sih, 0, 0) & SISF_BIST_DONE) == 0), 100000);
+
+	if (si_core_sflags(sih, 0, 0) & SISF_BIST_ERROR)
+		result = BCME_ERROR;
+
+	/* Reset core control flags */
+	si_core_cflags(sih, 0xffff, cflags);
+
+	return result;
+}
+
+static uint32
+factor6(uint32 x)
+{
+	switch (x) {
+	case CC_F6_2:	return 2;
+	case CC_F6_3:	return 3;
+	case CC_F6_4:	return 4;
+	case CC_F6_5:	return 5;
+	case CC_F6_6:	return 6;
+	case CC_F6_7:	return 7;
+	default:	return 0;
+	}
+}
+
+/* calculate the speed the SI would run at given a set of clockcontrol values */
+uint32
+si_clock_rate(uint32 pll_type, uint32 n, uint32 m)
+{
+	uint32 n1, n2, clock, m1, m2, m3, mc;
+
+	n1 = n & CN_N1_MASK;
+	n2 = (n & CN_N2_MASK) >> CN_N2_SHIFT;
+
+	if (pll_type == PLL_TYPE6) {
+		if (m & CC_T6_MMASK)
+			return CC_T6_M1;
+		else
+			return CC_T6_M0;
+	} else if ((pll_type == PLL_TYPE1) ||
+	           (pll_type == PLL_TYPE3) ||
+	           (pll_type == PLL_TYPE4) ||
+	           (pll_type == PLL_TYPE7)) {
+		n1 = factor6(n1);
+		n2 += CC_F5_BIAS;
+	} else if (pll_type == PLL_TYPE2) {
+		n1 += CC_T2_BIAS;
+		n2 += CC_T2_BIAS;
+		ASSERT((n1 >= 2) && (n1 <= 7));
+		ASSERT((n2 >= 5) && (n2 <= 23));
+	} else if (pll_type == PLL_TYPE5) {
+		return (100000000);
+	} else
+		ASSERT(0);
+	/* PLL types 3 and 7 use BASE2 (25Mhz) */
+	if ((pll_type == PLL_TYPE3) ||
+	    (pll_type == PLL_TYPE7)) {
+		clock = CC_CLOCK_BASE2 * n1 * n2;
+	} else
+		clock = CC_CLOCK_BASE1 * n1 * n2;
+
+	if (clock == 0)
+		return 0;
+
+	m1 = m & CC_M1_MASK;
+	m2 = (m & CC_M2_MASK) >> CC_M2_SHIFT;
+	m3 = (m & CC_M3_MASK) >> CC_M3_SHIFT;
+	mc = (m & CC_MC_MASK) >> CC_MC_SHIFT;
+
+	if ((pll_type == PLL_TYPE1) ||
+	    (pll_type == PLL_TYPE3) ||
+	    (pll_type == PLL_TYPE4) ||
+	    (pll_type == PLL_TYPE7)) {
+		m1 = factor6(m1);
+		if ((pll_type == PLL_TYPE1) || (pll_type == PLL_TYPE3))
+			m2 += CC_F5_BIAS;
+		else
+			m2 = factor6(m2);
+		m3 = factor6(m3);
+
+		switch (mc) {
+		case CC_MC_BYPASS:	return (clock);
+		case CC_MC_M1:		return (clock / m1);
+		case CC_MC_M1M2:	return (clock / (m1 * m2));
+		case CC_MC_M1M2M3:	return (clock / (m1 * m2 * m3));
+		case CC_MC_M1M3:	return (clock / (m1 * m3));
+		default:		return (0);
+		}
+	} else {
+		ASSERT(pll_type == PLL_TYPE2);
+
+		m1 += CC_T2_BIAS;
+		m2 += CC_T2M2_BIAS;
+		m3 += CC_T2_BIAS;
+		ASSERT((m1 >= 2) && (m1 <= 7));
+		ASSERT((m2 >= 3) && (m2 <= 10));
+		ASSERT((m3 >= 2) && (m3 <= 7));
+
+		if ((mc & CC_T2MC_M1BYP) == 0)
+			clock /= m1;
+		if ((mc & CC_T2MC_M2BYP) == 0)
+			clock /= m2;
+		if ((mc & CC_T2MC_M3BYP) == 0)
+			clock /= m3;
+
+		return (clock);
+	}
+}
+
+
+/* set chip watchdog reset timer to fire in 'ticks' */
+void
+si_watchdog(si_t *sih, uint ticks)
+{
+	uint nb, maxt;
+
+	if (PMUCTL_ENAB(sih)) {
+
+		if ((CHIPID(sih->chip) == BCM4319_CHIP_ID) &&
+		    (CHIPREV(sih->chiprev) == 0) && (ticks != 0)) {
+			si_corereg(sih, SI_CC_IDX, OFFSETOF(chipcregs_t, clk_ctl_st), ~0, 0x2);
+			si_setcore(sih, USB20D_CORE_ID, 0);
+			si_core_disable(sih, 1);
+			si_setcore(sih, CC_CORE_ID, 0);
+		}
+
+		nb = (sih->ccrev < 26) ? 16 : ((sih->ccrev >= 37) ? 32 : 24);
+		/* The mips compiler uses the sllv instruction,
+		 * so we specially handle the 32-bit case.
+		 */
+		if (nb == 32)
+			maxt = 0xffffffff;
+		else
+			maxt = ((1 << nb) - 1);
+
+		if (ticks == 1)
+			ticks = 2;
+		else if (ticks > maxt)
+			ticks = maxt;
+
+		si_corereg(sih, SI_CC_IDX, OFFSETOF(chipcregs_t, pmuwatchdog), ~0, ticks);
+	} else {
+		maxt = (1 << 28) - 1;
+		if (ticks > maxt)
+			ticks = maxt;
+
+		si_corereg(sih, SI_CC_IDX, OFFSETOF(chipcregs_t, watchdog), ~0, ticks);
+	}
+}
+
+/* trigger watchdog reset after ms milliseconds */
+void
+si_watchdog_ms(si_t *sih, uint32 ms)
+{
+	si_watchdog(sih, wd_msticks * ms);
+}
+
+
+
+
+/* return the slow clock source - LPO, XTAL, or PCI */
+static uint
+si_slowclk_src(si_info_t *sii)
+{
+	chipcregs_t *cc;
+
+	ASSERT(SI_FAST(sii) || si_coreid(&sii->pub) == CC_CORE_ID);
+
+	if (sii->pub.ccrev < 6) {
+		if ((BUSTYPE(sii->pub.bustype) == PCI_BUS) &&
+		    (OSL_PCI_READ_CONFIG(sii->osh, PCI_GPIO_OUT, sizeof(uint32)) &
+		     PCI_CFG_GPIO_SCS))
+			return (SCC_SS_PCI);
+		else
+			return (SCC_SS_XTAL);
+	} else if (sii->pub.ccrev < 10) {
+		cc = (chipcregs_t *)si_setcoreidx(&sii->pub, sii->curidx);
+		return (R_REG(sii->osh, &cc->slow_clk_ctl) & SCC_SS_MASK);
+	} else	/* Insta-clock */
+		return (SCC_SS_XTAL);
+}
+
+/* return the ILP (slowclock) min or max frequency */
+static uint
+si_slowclk_freq(si_info_t *sii, bool max_freq, chipcregs_t *cc)
+{
+	uint32 slowclk;
+	uint div;
+
+	ASSERT(SI_FAST(sii) || si_coreid(&sii->pub) == CC_CORE_ID);
+
+	/* shouldn't be here unless we've established the chip has dynamic clk control */
+	ASSERT(R_REG(sii->osh, &cc->capabilities) & CC_CAP_PWR_CTL);
+
+	slowclk = si_slowclk_src(sii);
+	if (sii->pub.ccrev < 6) {
+		if (slowclk == SCC_SS_PCI)
+			return (max_freq ? (PCIMAXFREQ / 64) : (PCIMINFREQ / 64));
+		else
+			return (max_freq ? (XTALMAXFREQ / 32) : (XTALMINFREQ / 32));
+	} else if (sii->pub.ccrev < 10) {
+		div = 4 *
+		        (((R_REG(sii->osh, &cc->slow_clk_ctl) & SCC_CD_MASK) >> SCC_CD_SHIFT) + 1);
+		if (slowclk == SCC_SS_LPO)
+			return (max_freq ? LPOMAXFREQ : LPOMINFREQ);
+		else if (slowclk == SCC_SS_XTAL)
+			return (max_freq ? (XTALMAXFREQ / div) : (XTALMINFREQ / div));
+		else if (slowclk == SCC_SS_PCI)
+			return (max_freq ? (PCIMAXFREQ / div) : (PCIMINFREQ / div));
+		else
+			ASSERT(0);
+	} else {
+		/* Chipc rev 10 is InstaClock */
+		div = R_REG(sii->osh, &cc->system_clk_ctl) >> SYCC_CD_SHIFT;
+		div = 4 * (div + 1);
+		return (max_freq ? XTALMAXFREQ : (XTALMINFREQ / div));
+	}
+	return (0);
+}
+
+static void
+si_clkctl_setdelay(si_info_t *sii, void *chipcregs)
+{
+	chipcregs_t *cc = (chipcregs_t *)chipcregs;
+	uint slowmaxfreq, pll_delay, slowclk;
+	uint pll_on_delay, fref_sel_delay;
+
+	pll_delay = PLL_DELAY;
+
+	/* If the slow clock is not sourced by the xtal then add the xtal_on_delay
+	 * since the xtal will also be powered down by dynamic clk control logic.
+	 */
+
+	slowclk = si_slowclk_src(sii);
+	if (slowclk != SCC_SS_XTAL)
+		pll_delay += XTAL_ON_DELAY;
+
+	/* Starting with 4318 it is ILP that is used for the delays */
+	slowmaxfreq = si_slowclk_freq(sii, (sii->pub.ccrev >= 10) ? FALSE : TRUE, cc);
+
+	pll_on_delay = ((slowmaxfreq * pll_delay) + 999999) / 1000000;
+	fref_sel_delay = ((slowmaxfreq * FREF_DELAY) + 999999) / 1000000;
+
+	W_REG(sii->osh, &cc->pll_on_delay, pll_on_delay);
+	W_REG(sii->osh, &cc->fref_sel_delay, fref_sel_delay);
+}
+
+/* initialize power control delay registers */
+void
+si_clkctl_init(si_t *sih)
+{
+	si_info_t *sii;
+	uint origidx = 0;
+	chipcregs_t *cc;
+	bool fast;
+
+	if (!CCCTL_ENAB(sih))
+		return;
+
+	sii = SI_INFO(sih);
+	fast = SI_FAST(sii);
+	if (!fast) {
+		origidx = sii->curidx;
+		if ((cc = (chipcregs_t *)si_setcore(sih, CC_CORE_ID, 0)) == NULL)
+			return;
+	} else if ((cc = (chipcregs_t *)CCREGS_FAST(sii)) == NULL)
+		return;
+	ASSERT(cc != NULL);
+
+	/* set all Instaclk chip ILP to 1 MHz */
+	if (sih->ccrev >= 10)
+		SET_REG(sii->osh, &cc->system_clk_ctl, SYCC_CD_MASK,
+		        (ILP_DIV_1MHZ << SYCC_CD_SHIFT));
+
+	si_clkctl_setdelay(sii, (void *)(uintptr)cc);
+
+	if (!fast)
+		si_setcoreidx(sih, origidx);
+}
+
+/* change logical "focus" to the gpio core for optimized access */
+void *
+si_gpiosetcore(si_t *sih)
+{
+	return (si_setcoreidx(sih, SI_CC_IDX));
+}
+
+/* mask&set gpiocontrol bits */
+uint32
+si_gpiocontrol(si_t *sih, uint32 mask, uint32 val, uint8 priority)
+{
+	uint regoff;
+
+	regoff = 0;
+
+	/* gpios could be shared on router platforms
+	 * ignore reservation if it's high priority (e.g., test apps)
+	 */
+	if ((priority != GPIO_HI_PRIORITY) &&
+	    (BUSTYPE(sih->bustype) == SI_BUS) && (val || mask)) {
+		mask = priority ? (si_gpioreservation & mask) :
+			((si_gpioreservation | mask) & ~(si_gpioreservation));
+		val &= mask;
+	}
+
+	regoff = OFFSETOF(chipcregs_t, gpiocontrol);
+	return (si_corereg(sih, SI_CC_IDX, regoff, mask, val));
+}
+
+/* mask&set gpio output enable bits */
+uint32
+si_gpioouten(si_t *sih, uint32 mask, uint32 val, uint8 priority)
+{
+	uint regoff;
+
+	regoff = 0;
+
+	/* gpios could be shared on router platforms
+	 * ignore reservation if it's high priority (e.g., test apps)
+	 */
+	if ((priority != GPIO_HI_PRIORITY) &&
+	    (BUSTYPE(sih->bustype) == SI_BUS) && (val || mask)) {
+		mask = priority ? (si_gpioreservation & mask) :
+			((si_gpioreservation | mask) & ~(si_gpioreservation));
+		val &= mask;
+	}
+
+	regoff = OFFSETOF(chipcregs_t, gpioouten);
+	return (si_corereg(sih, SI_CC_IDX, regoff, mask, val));
+}
+
+/* mask&set gpio output bits */
+uint32
+si_gpioout(si_t *sih, uint32 mask, uint32 val, uint8 priority)
+{
+	uint regoff;
+
+	regoff = 0;
+
+	/* gpios could be shared on router platforms
+	 * ignore reservation if it's high priority (e.g., test apps)
+	 */
+	if ((priority != GPIO_HI_PRIORITY) &&
+	    (BUSTYPE(sih->bustype) == SI_BUS) && (val || mask)) {
+		mask = priority ? (si_gpioreservation & mask) :
+			((si_gpioreservation | mask) & ~(si_gpioreservation));
+		val &= mask;
+	}
+
+	regoff = OFFSETOF(chipcregs_t, gpioout);
+	return (si_corereg(sih, SI_CC_IDX, regoff, mask, val));
+}
+
+/* reserve one gpio */
+uint32
+si_gpioreserve(si_t *sih, uint32 gpio_bitmask, uint8 priority)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	/* only cores on SI_BUS share GPIO's and only applcation users need to
+	 * reserve/release GPIO
+	 */
+	if ((BUSTYPE(sih->bustype) != SI_BUS) || (!priority)) {
+		ASSERT((BUSTYPE(sih->bustype) == SI_BUS) && (priority));
+		return 0xffffffff;
+	}
+	/* make sure only one bit is set */
+	if ((!gpio_bitmask) || ((gpio_bitmask) & (gpio_bitmask - 1))) {
+		ASSERT((gpio_bitmask) && !((gpio_bitmask) & (gpio_bitmask - 1)));
+		return 0xffffffff;
+	}
+
+	/* already reserved */
+	if (si_gpioreservation & gpio_bitmask)
+		return 0xffffffff;
+	/* set reservation */
+	si_gpioreservation |= gpio_bitmask;
+
+	return si_gpioreservation;
+}
+
+/* release one gpio */
+/*
+ * releasing the gpio doesn't change the current value on the GPIO last write value
+ * persists till some one overwrites it
+ */
+
+uint32
+si_gpiorelease(si_t *sih, uint32 gpio_bitmask, uint8 priority)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	/* only cores on SI_BUS share GPIO's and only applcation users need to
+	 * reserve/release GPIO
+	 */
+	if ((BUSTYPE(sih->bustype) != SI_BUS) || (!priority)) {
+		ASSERT((BUSTYPE(sih->bustype) == SI_BUS) && (priority));
+		return 0xffffffff;
+	}
+	/* make sure only one bit is set */
+	if ((!gpio_bitmask) || ((gpio_bitmask) & (gpio_bitmask - 1))) {
+		ASSERT((gpio_bitmask) && !((gpio_bitmask) & (gpio_bitmask - 1)));
+		return 0xffffffff;
+	}
+
+	/* already released */
+	if (!(si_gpioreservation & gpio_bitmask))
+		return 0xffffffff;
+
+	/* clear reservation */
+	si_gpioreservation &= ~gpio_bitmask;
+
+	return si_gpioreservation;
+}
+
+/* return the current gpioin register value */
+uint32
+si_gpioin(si_t *sih)
+{
+	si_info_t *sii;
+	uint regoff;
+
+	sii = SI_INFO(sih);
+	regoff = 0;
+
+	regoff = OFFSETOF(chipcregs_t, gpioin);
+	return (si_corereg(sih, SI_CC_IDX, regoff, 0, 0));
+}
+
+/* mask&set gpio interrupt polarity bits */
+uint32
+si_gpiointpolarity(si_t *sih, uint32 mask, uint32 val, uint8 priority)
+{
+	si_info_t *sii;
+	uint regoff;
+
+	sii = SI_INFO(sih);
+	regoff = 0;
+
+	/* gpios could be shared on router platforms */
+	if ((BUSTYPE(sih->bustype) == SI_BUS) && (val || mask)) {
+		mask = priority ? (si_gpioreservation & mask) :
+			((si_gpioreservation | mask) & ~(si_gpioreservation));
+		val &= mask;
+	}
+
+	regoff = OFFSETOF(chipcregs_t, gpiointpolarity);
+	return (si_corereg(sih, SI_CC_IDX, regoff, mask, val));
+}
+
+/* mask&set gpio interrupt mask bits */
+uint32
+si_gpiointmask(si_t *sih, uint32 mask, uint32 val, uint8 priority)
+{
+	si_info_t *sii;
+	uint regoff;
+
+	sii = SI_INFO(sih);
+	regoff = 0;
+
+	/* gpios could be shared on router platforms */
+	if ((BUSTYPE(sih->bustype) == SI_BUS) && (val || mask)) {
+		mask = priority ? (si_gpioreservation & mask) :
+			((si_gpioreservation | mask) & ~(si_gpioreservation));
+		val &= mask;
+	}
+
+	regoff = OFFSETOF(chipcregs_t, gpiointmask);
+	return (si_corereg(sih, SI_CC_IDX, regoff, mask, val));
+}
+
+/* assign the gpio to an led */
+uint32
+si_gpioled(si_t *sih, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+	if (sih->ccrev < 16)
+		return 0xffffffff;
+
+	/* gpio led powersave reg */
+	return (si_corereg(sih, SI_CC_IDX, OFFSETOF(chipcregs_t, gpiotimeroutmask), mask, val));
+}
+
+/* mask&set gpio timer val */
+uint32
+si_gpiotimerval(si_t *sih, uint32 mask, uint32 gpiotimerval)
+{
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	if (sih->ccrev < 16)
+		return 0xffffffff;
+
+	return (si_corereg(sih, SI_CC_IDX,
+		OFFSETOF(chipcregs_t, gpiotimerval), mask, gpiotimerval));
+}
+
+uint32
+si_gpiopull(si_t *sih, bool updown, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	uint offs;
+
+	sii = SI_INFO(sih);
+	if (sih->ccrev < 20)
+		return 0xffffffff;
+
+	offs = (updown ? OFFSETOF(chipcregs_t, gpiopulldown) : OFFSETOF(chipcregs_t, gpiopullup));
+	return (si_corereg(sih, SI_CC_IDX, offs, mask, val));
+}
+
+uint32
+si_gpioevent(si_t *sih, uint regtype, uint32 mask, uint32 val)
+{
+	si_info_t *sii;
+	uint offs;
+
+	sii = SI_INFO(sih);
+	if (sih->ccrev < 11)
+		return 0xffffffff;
+
+	if (regtype == GPIO_REGEVT)
+		offs = OFFSETOF(chipcregs_t, gpioevent);
+	else if (regtype == GPIO_REGEVT_INTMSK)
+		offs = OFFSETOF(chipcregs_t, gpioeventintmask);
+	else if (regtype == GPIO_REGEVT_INTPOL)
+		offs = OFFSETOF(chipcregs_t, gpioeventintpolarity);
+	else
+		return 0xffffffff;
+
+	return (si_corereg(sih, SI_CC_IDX, offs, mask, val));
+}
+
+void *
+si_gpio_handler_register(si_t *sih, uint32 event,
+	bool level, gpio_handler_t cb, void *arg)
+{
+	si_info_t *sii;
+	gpioh_item_t *gi;
+
+	ASSERT(event);
+	ASSERT(cb != NULL);
+
+	sii = SI_INFO(sih);
+	if (sih->ccrev < 11)
+		return NULL;
+
+	if ((gi = MALLOC(sii->osh, sizeof(gpioh_item_t))) == NULL)
+		return NULL;
+
+	bzero(gi, sizeof(gpioh_item_t));
+	gi->event = event;
+	gi->handler = cb;
+	gi->arg = arg;
+	gi->level = level;
+
+	gi->next = sii->gpioh_head;
+	sii->gpioh_head = gi;
+
+	return (void *)(gi);
+}
+
+void
+si_gpio_handler_unregister(si_t *sih, void *gpioh)
+{
+	si_info_t *sii;
+	gpioh_item_t *p, *n;
+
+	sii = SI_INFO(sih);
+	if (sih->ccrev < 11)
+		return;
+
+	ASSERT(sii->gpioh_head != NULL);
+	if ((void*)sii->gpioh_head == gpioh) {
+		sii->gpioh_head = sii->gpioh_head->next;
+		MFREE(sii->osh, gpioh, sizeof(gpioh_item_t));
+		return;
+	} else {
+		p = sii->gpioh_head;
+		n = p->next;
+		while (n) {
+			if ((void*)n == gpioh) {
+				p->next = n->next;
+				MFREE(sii->osh, gpioh, sizeof(gpioh_item_t));
+				return;
+			}
+			p = n;
+			n = n->next;
+		}
+	}
+
+	ASSERT(0); /* Not found in list */
+}
+
+void
+si_gpio_handler_process(si_t *sih)
+{
+	si_info_t *sii;
+	gpioh_item_t *h;
+	uint32 level = si_gpioin(sih);
+	uint32 levelp = si_gpiointpolarity(sih, 0, 0, 0);
+	uint32 edge = si_gpioevent(sih, GPIO_REGEVT, 0, 0);
+	uint32 edgep = si_gpioevent(sih, GPIO_REGEVT_INTPOL, 0, 0);
+
+	sii = SI_INFO(sih);
+	for (h = sii->gpioh_head; h != NULL; h = h->next) {
+		if (h->handler) {
+			uint32 status = (h->level ? level : edge) & h->event;
+			uint32 polarity = (h->level ? levelp : edgep) & h->event;
+
+			/* polarity bitval is opposite of status bitval */
+			if (status ^ polarity)
+				h->handler(status, h->arg);
+		}
+	}
+
+	si_gpioevent(sih, GPIO_REGEVT, edge, edge); /* clear edge-trigger status */
+}
+
+uint32
+si_gpio_int_enable(si_t *sih, bool enable)
+{
+	si_info_t *sii;
+	uint offs;
+
+	sii = SI_INFO(sih);
+	if (sih->ccrev < 11)
+		return 0xffffffff;
+
+	offs = OFFSETOF(chipcregs_t, intmask);
+	return (si_corereg(sih, SI_CC_IDX, offs, CI_GPIO, (enable ? CI_GPIO : 0)));
+}
+
+
+/* Return the size of the specified SOCRAM bank */
+static uint
+socram_banksize(si_info_t *sii, sbsocramregs_t *regs, uint8 index, uint8 mem_type)
+{
+	uint banksize, bankinfo;
+	uint bankidx = index | (mem_type << SOCRAM_BANKIDX_MEMTYPE_SHIFT);
+
+	ASSERT(mem_type <= SOCRAM_MEMTYPE_DEVRAM);
+
+	W_REG(sii->osh, &regs->bankidx, bankidx);
+	bankinfo = R_REG(sii->osh, &regs->bankinfo);
+	banksize = SOCRAM_BANKINFO_SZBASE * ((bankinfo & SOCRAM_BANKINFO_SZMASK) + 1);
+	return banksize;
+}
+
+void
+si_socdevram(si_t *sih, bool set, uint8 *enable, uint8 *protect)
+{
+	si_info_t *sii;
+	uint origidx;
+	uint intr_val = 0;
+	sbsocramregs_t *regs;
+	bool wasup;
+	uint corerev;
+
+	sii = SI_INFO(sih);
+
+	/* Block ints and save current core */
+	INTR_OFF(sii, intr_val);
+	origidx = si_coreidx(sih);
+
+	if (!set)
+		*enable = *protect = 0;
+
+	/* Switch to SOCRAM core */
+	if (!(regs = si_setcore(sih, SOCRAM_CORE_ID, 0)))
+		goto done;
+
+	/* Get info for determining size */
+	if (!(wasup = si_iscoreup(sih)))
+		si_core_reset(sih, 0, 0);
+
+	corerev = si_corerev(sih);
+	if (corerev >= 10) {
+		uint32 extcinfo;
+		uint8 nb;
+		uint8 i;
+		uint32 bankidx, bankinfo;
+
+		extcinfo = R_REG(sii->osh, &regs->extracoreinfo);
+		nb = ((extcinfo & SOCRAM_DEVRAMBANK_MASK) >> SOCRAM_DEVRAMBANK_SHIFT);
+		for (i = 0; i < nb; i++) {
+			bankidx = i | (SOCRAM_MEMTYPE_DEVRAM << SOCRAM_BANKIDX_MEMTYPE_SHIFT);
+			W_REG(sii->osh, &regs->bankidx, bankidx);
+			bankinfo = R_REG(sii->osh, &regs->bankinfo);
+			if (set) {
+				bankinfo &= ~SOCRAM_BANKINFO_DEVRAMSEL_MASK;
+				bankinfo &= ~SOCRAM_BANKINFO_DEVRAMPRO_MASK;
+				if (*enable) {
+					bankinfo |= (1 << SOCRAM_BANKINFO_DEVRAMSEL_SHIFT);
+					if (*protect)
+						bankinfo |= (1 << SOCRAM_BANKINFO_DEVRAMPRO_SHIFT);
+				}
+				W_REG(sii->osh, &regs->bankinfo, bankinfo);
+			}
+			else if (i == 0) {
+				if (bankinfo & SOCRAM_BANKINFO_DEVRAMSEL_MASK) {
+					*enable = 1;
+					if (bankinfo & SOCRAM_BANKINFO_DEVRAMPRO_MASK)
+						*protect = 1;
+				}
+			}
+		}
+	}
+
+	/* Return to previous state and core */
+	if (!wasup)
+		si_core_disable(sih, 0);
+	si_setcoreidx(sih, origidx);
+
+done:
+	INTR_RESTORE(sii, intr_val);
+}
+
+bool
+si_socdevram_pkg(si_t *sih)
+{
+	if (si_socdevram_size(sih) > 0)
+		return TRUE;
+	else
+		return FALSE;
+}
+
+uint32
+si_socdevram_size(si_t *sih)
+{
+	si_info_t *sii;
+	uint origidx;
+	uint intr_val = 0;
+	uint32 memsize = 0;
+	sbsocramregs_t *regs;
+	bool wasup;
+	uint corerev;
+
+	sii = SI_INFO(sih);
+
+	/* Block ints and save current core */
+	INTR_OFF(sii, intr_val);
+	origidx = si_coreidx(sih);
+
+	/* Switch to SOCRAM core */
+	if (!(regs = si_setcore(sih, SOCRAM_CORE_ID, 0)))
+		goto done;
+
+	/* Get info for determining size */
+	if (!(wasup = si_iscoreup(sih)))
+		si_core_reset(sih, 0, 0);
+
+	corerev = si_corerev(sih);
+	if (corerev >= 10) {
+		uint32 extcinfo;
+		uint8 nb;
+		uint8 i;
+
+		extcinfo = R_REG(sii->osh, &regs->extracoreinfo);
+		nb = (((extcinfo & SOCRAM_DEVRAMBANK_MASK) >> SOCRAM_DEVRAMBANK_SHIFT));
+		for (i = 0; i < nb; i++)
+			memsize += socram_banksize(sii, regs, i, SOCRAM_MEMTYPE_DEVRAM);
+	}
+
+	/* Return to previous state and core */
+	if (!wasup)
+		si_core_disable(sih, 0);
+	si_setcoreidx(sih, origidx);
+
+done:
+	INTR_RESTORE(sii, intr_val);
+
+	return memsize;
+}
+
+/* Return the RAM size of the SOCRAM core */
+uint32
+si_socram_size(si_t *sih)
+{
+	si_info_t *sii;
+	uint origidx;
+	uint intr_val = 0;
+
+	sbsocramregs_t *regs;
+	bool wasup;
+	uint corerev;
+	uint32 coreinfo;
+	uint memsize = 0;
+
+	sii = SI_INFO(sih);
+
+	/* Block ints and save current core */
+	INTR_OFF(sii, intr_val);
+	origidx = si_coreidx(sih);
+
+	/* Switch to SOCRAM core */
+	if (!(regs = si_setcore(sih, SOCRAM_CORE_ID, 0)))
+		goto done;
+
+	/* Get info for determining size */
+	if (!(wasup = si_iscoreup(sih)))
+		si_core_reset(sih, 0, 0);
+	corerev = si_corerev(sih);
+	coreinfo = R_REG(sii->osh, &regs->coreinfo);
+
+	/* Calculate size from coreinfo based on rev */
+	if (corerev == 0)
+		memsize = 1 << (16 + (coreinfo & SRCI_MS0_MASK));
+	else if (corerev < 3) {
+		memsize = 1 << (SR_BSZ_BASE + (coreinfo & SRCI_SRBSZ_MASK));
+		memsize *= (coreinfo & SRCI_SRNB_MASK) >> SRCI_SRNB_SHIFT;
+	} else if ((corerev <= 7) || (corerev == 12)) {
+		uint nb = (coreinfo & SRCI_SRNB_MASK) >> SRCI_SRNB_SHIFT;
+		uint bsz = (coreinfo & SRCI_SRBSZ_MASK);
+		uint lss = (coreinfo & SRCI_LSS_MASK) >> SRCI_LSS_SHIFT;
+		if (lss != 0)
+			nb --;
+		memsize = nb * (1 << (bsz + SR_BSZ_BASE));
+		if (lss != 0)
+			memsize += (1 << ((lss - 1) + SR_BSZ_BASE));
+	} else {
+		uint8 i;
+		uint nb = (coreinfo & SRCI_SRNB_MASK) >> SRCI_SRNB_SHIFT;
+		for (i = 0; i < nb; i++)
+			memsize += socram_banksize(sii, regs, i, SOCRAM_MEMTYPE_RAM);
+	}
+
+	/* Return to previous state and core */
+	if (!wasup)
+		si_core_disable(sih, 0);
+	si_setcoreidx(sih, origidx);
+
+done:
+	INTR_RESTORE(sii, intr_val);
+
+	return memsize;
+}
+
+
+void
+si_btcgpiowar(si_t *sih)
+{
+	si_info_t *sii;
+	uint origidx;
+	uint intr_val = 0;
+	chipcregs_t *cc;
+
+	sii = SI_INFO(sih);
+
+	/* Make sure that there is ChipCommon core present &&
+	 * UART_TX is strapped to 1
+	 */
+	if (!(sih->cccaps & CC_CAP_UARTGPIO))
+		return;
+
+	/* si_corereg cannot be used as we have to guarantee 8-bit read/writes */
+	INTR_OFF(sii, intr_val);
+
+	origidx = si_coreidx(sih);
+
+	cc = (chipcregs_t *)si_setcore(sih, CC_CORE_ID, 0);
+	ASSERT(cc != NULL);
+
+	W_REG(sii->osh, &cc->uart0mcr, R_REG(sii->osh, &cc->uart0mcr) | 0x04);
+
+	/* restore the original index */
+	si_setcoreidx(sih, origidx);
+
+	INTR_RESTORE(sii, intr_val);
+}
+
+uint
+si_pll_reset(si_t *sih)
+{
+	uint err = 0;
+
+	return (err);
+}
+
+/* check if the device is removed */
+bool
+si_deviceremoved(si_t *sih)
+{
+	uint32 w;
+	si_info_t *sii;
+
+	sii = SI_INFO(sih);
+
+	switch (BUSTYPE(sih->bustype)) {
+	case PCI_BUS:
+		ASSERT(sii->osh != NULL);
+		w = OSL_PCI_READ_CONFIG(sii->osh, PCI_CFG_VID, sizeof(uint32));
+		if ((w & 0xFFFF) != VENDOR_BROADCOM)
+			return TRUE;
+		break;
+	}
+	return FALSE;
+}
+
+bool
+si_is_sprom_available(si_t *sih)
+{
+	if (sih->ccrev >= 31) {
+		si_info_t *sii;
+		uint origidx;
+		chipcregs_t *cc;
+		uint32 sromctrl;
+
+		if ((sih->cccaps & CC_CAP_SROM) == 0)
+			return FALSE;
+
+		sii = SI_INFO(sih);
+		origidx = sii->curidx;
+		cc = si_setcoreidx(sih, SI_CC_IDX);
+		sromctrl = R_REG(sii->osh, &cc->sromcontrol);
+		si_setcoreidx(sih, origidx);
+		return (sromctrl & SRC_PRESENT);
+	}
+
+	switch (CHIPID(sih->chip)) {
+	case BCM4312_CHIP_ID:
+		return ((sih->chipst & CST4312_SPROM_OTP_SEL_MASK) != CST4312_OTP_SEL);
+	case BCM4325_CHIP_ID:
+		return (sih->chipst & CST4325_SPROM_SEL) != 0;
+	case BCM4322_CHIP_ID:
+	case BCM43221_CHIP_ID:
+	case BCM43231_CHIP_ID:
+	case BCM43222_CHIP_ID:
+	case BCM43111_CHIP_ID:
+	case BCM43112_CHIP_ID:
+	case BCM4342_CHIP_ID:
+	{
+		uint32 spromotp;
+		spromotp = (sih->chipst & CST4322_SPROM_OTP_SEL_MASK) >>
+		        CST4322_SPROM_OTP_SEL_SHIFT;
+		return (spromotp & CST4322_SPROM_PRESENT) != 0;
+	}
+	case BCM4329_CHIP_ID:
+		return (sih->chipst & CST4329_SPROM_SEL) != 0;
+	case BCM4315_CHIP_ID:
+		return (sih->chipst & CST4315_SPROM_SEL) != 0;
+	case BCM4319_CHIP_ID:
+		return (sih->chipst & CST4319_SPROM_SEL) != 0;
+
+	case BCM4336_CHIP_ID:
+	case BCM43362_CHIP_ID:
+		return (sih->chipst & CST4336_SPROM_PRESENT) != 0;
+
+	case BCM4330_CHIP_ID:
+		return (sih->chipst & CST4330_SPROM_PRESENT) != 0;
+	case BCM4313_CHIP_ID:
+		return (sih->chipst & CST4313_SPROM_PRESENT) != 0;
+	case BCM43239_CHIP_ID:
+		return ((sih->chipst & CST43239_SPROM_MASK) &&
+			!(sih->chipst & CST43239_SFLASH_MASK));
+	default:
+		return TRUE;
+	}
+}
diff --git a/drivers/net/wireless/bcmdhd/siutils_priv.h b/drivers/net/wireless/bcmdhd/siutils_priv.h
new file mode 100644
index 000000000000..d80246e01d1b
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/siutils_priv.h
@@ -0,0 +1,235 @@
+/*
+ * Include file private to the SOC Interconnect support files.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: siutils_priv.h,v 1.17.4.3 2010-10-25 16:56:56 Exp $
+ */
+
+#ifndef	_siutils_priv_h_
+#define	_siutils_priv_h_
+
+#define	SI_ERROR(args)
+
+#define	SI_MSG(args)
+
+/* Define SI_VMSG to printf for verbose debugging, but don't check it in */
+#define	SI_VMSG(args)
+
+#define	IS_SIM(chippkg)	((chippkg == HDLSIM_PKG_ID) || (chippkg == HWSIM_PKG_ID))
+
+typedef uint32 (*si_intrsoff_t)(void *intr_arg);
+typedef void (*si_intrsrestore_t)(void *intr_arg, uint32 arg);
+typedef bool (*si_intrsenabled_t)(void *intr_arg);
+
+typedef struct gpioh_item {
+	void			*arg;
+	bool			level;
+	gpio_handler_t		handler;
+	uint32			event;
+	struct gpioh_item	*next;
+} gpioh_item_t;
+
+/* misc si info needed by some of the routines */
+typedef struct si_info {
+	struct si_pub pub;		/* back plane public state (must be first field) */
+
+	void	*osh;			/* osl os handle */
+	void	*sdh;			/* bcmsdh handle */
+
+	uint	dev_coreid;		/* the core provides driver functions */
+	void	*intr_arg;		/* interrupt callback function arg */
+	si_intrsoff_t intrsoff_fn;	/* turns chip interrupts off */
+	si_intrsrestore_t intrsrestore_fn; /* restore chip interrupts */
+	si_intrsenabled_t intrsenabled_fn; /* check if interrupts are enabled */
+
+	void *pch;			/* PCI/E core handle */
+
+	gpioh_item_t *gpioh_head; 	/* GPIO event handlers list */
+
+	bool	memseg;			/* flag to toggle MEM_SEG register */
+
+	char *vars;
+	uint varsz;
+
+	void	*curmap;		/* current regs va */
+	void	*regs[SI_MAXCORES];	/* other regs va */
+
+	uint	curidx;			/* current core index */
+	uint	numcores;		/* # discovered cores */
+	uint	coreid[SI_MAXCORES];	/* id of each core */
+	uint32	coresba[SI_MAXCORES];	/* backplane address of each core */
+	void	*regs2[SI_MAXCORES];	/* va of each core second register set (usbh20) */
+	uint32	coresba2[SI_MAXCORES];	/* address of each core second register set (usbh20) */
+	uint32	coresba_size[SI_MAXCORES]; /* backplane address space size */
+	uint32	coresba2_size[SI_MAXCORES]; /* second address space size */
+
+	void	*curwrap;		/* current wrapper va */
+	void	*wrappers[SI_MAXCORES];	/* other cores wrapper va */
+	uint32	wrapba[SI_MAXCORES];	/* address of controlling wrapper */
+
+	uint32	cia[SI_MAXCORES];	/* erom cia entry for each core */
+	uint32	cib[SI_MAXCORES];	/* erom cia entry for each core */
+	uint32	oob_router;		/* oob router registers for axi */
+} si_info_t;
+
+#define	SI_INFO(sih)	(si_info_t *)(uintptr)sih
+
+#define	GOODCOREADDR(x, b) (((x) >= (b)) && ((x) < ((b) + SI_MAXCORES * SI_CORE_SIZE)) && \
+		ISALIGNED((x), SI_CORE_SIZE))
+#define	GOODREGS(regs)	((regs) != NULL && ISALIGNED((uintptr)(regs), SI_CORE_SIZE))
+#define BADCOREADDR	0
+#define	GOODIDX(idx)	(((uint)idx) < SI_MAXCORES)
+#define	NOREV		-1		/* Invalid rev */
+
+#define PCI(si)		((BUSTYPE((si)->pub.bustype) == PCI_BUS) &&	\
+			 ((si)->pub.buscoretype == PCI_CORE_ID))
+#define PCIE(si)	((BUSTYPE((si)->pub.bustype) == PCI_BUS) &&	\
+			 ((si)->pub.buscoretype == PCIE_CORE_ID))
+#define PCMCIA(si)	((BUSTYPE((si)->pub.bustype) == PCMCIA_BUS) && ((si)->memseg == TRUE))
+
+/* Newer chips can access PCI/PCIE and CC core without requiring to change
+ * PCI BAR0 WIN
+ */
+#define SI_FAST(si) (((si)->pub.buscoretype == PCIE_CORE_ID) ||	\
+		     (((si)->pub.buscoretype == PCI_CORE_ID) && (si)->pub.buscorerev >= 13))
+
+#define PCIEREGS(si) (((char *)((si)->curmap) + PCI_16KB0_PCIREGS_OFFSET))
+#define CCREGS_FAST(si) (((char *)((si)->curmap) + PCI_16KB0_CCREGS_OFFSET))
+
+/*
+ * Macros to disable/restore function core(D11, ENET, ILINE20, etc) interrupts before/
+ * after core switching to avoid invalid register accesss inside ISR.
+ */
+#define INTR_OFF(si, intr_val) \
+	if ((si)->intrsoff_fn && (si)->coreid[(si)->curidx] == (si)->dev_coreid) {	\
+		intr_val = (*(si)->intrsoff_fn)((si)->intr_arg); }
+#define INTR_RESTORE(si, intr_val) \
+	if ((si)->intrsrestore_fn && (si)->coreid[(si)->curidx] == (si)->dev_coreid) {	\
+		(*(si)->intrsrestore_fn)((si)->intr_arg, intr_val); }
+
+/* dynamic clock control defines */
+#define	LPOMINFREQ		25000		/* low power oscillator min */
+#define	LPOMAXFREQ		43000		/* low power oscillator max */
+#define	XTALMINFREQ		19800000	/* 20 MHz - 1% */
+#define	XTALMAXFREQ		20200000	/* 20 MHz + 1% */
+#define	PCIMINFREQ		25000000	/* 25 MHz */
+#define	PCIMAXFREQ		34000000	/* 33 MHz + fudge */
+
+#define	ILP_DIV_5MHZ		0		/* ILP = 5 MHz */
+#define	ILP_DIV_1MHZ		4		/* ILP = 1 MHz */
+
+#define PCI_FORCEHT(si)	\
+	(((PCIE(si)) && (si->pub.chip == BCM4311_CHIP_ID) && ((si->pub.chiprev <= 1))) || \
+	((PCI(si) || PCIE(si)) && (si->pub.chip == BCM4321_CHIP_ID)) || \
+	(PCIE(si) && (si->pub.chip == BCM4716_CHIP_ID)))
+
+/* GPIO Based LED powersave defines */
+#define DEFAULT_GPIO_ONTIME	10		/* Default: 10% on */
+#define DEFAULT_GPIO_OFFTIME	90		/* Default: 10% on */
+
+#ifndef DEFAULT_GPIOTIMERVAL
+#define DEFAULT_GPIOTIMERVAL  ((DEFAULT_GPIO_ONTIME << GPIO_ONTIME_SHIFT) | DEFAULT_GPIO_OFFTIME)
+#endif
+
+/* Silicon Backplane externs */
+extern void sb_scan(si_t *sih, void *regs, uint devid);
+extern uint sb_coreid(si_t *sih);
+extern uint sb_intflag(si_t *sih);
+extern uint sb_flag(si_t *sih);
+extern void sb_setint(si_t *sih, int siflag);
+extern uint sb_corevendor(si_t *sih);
+extern uint sb_corerev(si_t *sih);
+extern uint sb_corereg(si_t *sih, uint coreidx, uint regoff, uint mask, uint val);
+extern bool sb_iscoreup(si_t *sih);
+extern void *sb_setcoreidx(si_t *sih, uint coreidx);
+extern uint32 sb_core_cflags(si_t *sih, uint32 mask, uint32 val);
+extern void sb_core_cflags_wo(si_t *sih, uint32 mask, uint32 val);
+extern uint32 sb_core_sflags(si_t *sih, uint32 mask, uint32 val);
+extern void sb_commit(si_t *sih);
+extern uint32 sb_base(uint32 admatch);
+extern uint32 sb_size(uint32 admatch);
+extern void sb_core_reset(si_t *sih, uint32 bits, uint32 resetbits);
+extern void sb_core_disable(si_t *sih, uint32 bits);
+extern uint32 sb_addrspace(si_t *sih, uint asidx);
+extern uint32 sb_addrspacesize(si_t *sih, uint asidx);
+extern int sb_numaddrspaces(si_t *sih);
+
+extern uint32 sb_set_initiator_to(si_t *sih, uint32 to, uint idx);
+
+extern bool sb_taclear(si_t *sih, bool details);
+
+
+/* Wake-on-wireless-LAN (WOWL) */
+extern bool sb_pci_pmecap(si_t *sih);
+struct osl_info;
+extern bool sb_pci_fastpmecap(struct osl_info *osh);
+extern bool sb_pci_pmeclr(si_t *sih);
+extern void sb_pci_pmeen(si_t *sih);
+extern uint sb_pcie_readreg(void *sih, uint addrtype, uint offset);
+
+/* AMBA Interconnect exported externs */
+extern si_t *ai_attach(uint pcidev, osl_t *osh, void *regs, uint bustype,
+                       void *sdh, char **vars, uint *varsz);
+extern si_t *ai_kattach(osl_t *osh);
+extern void ai_scan(si_t *sih, void *regs, uint devid);
+
+extern uint ai_flag(si_t *sih);
+extern void ai_setint(si_t *sih, int siflag);
+extern uint ai_coreidx(si_t *sih);
+extern uint ai_corevendor(si_t *sih);
+extern uint ai_corerev(si_t *sih);
+extern bool ai_iscoreup(si_t *sih);
+extern void *ai_setcoreidx(si_t *sih, uint coreidx);
+extern uint32 ai_core_cflags(si_t *sih, uint32 mask, uint32 val);
+extern void ai_core_cflags_wo(si_t *sih, uint32 mask, uint32 val);
+extern uint32 ai_core_sflags(si_t *sih, uint32 mask, uint32 val);
+extern uint ai_corereg(si_t *sih, uint coreidx, uint regoff, uint mask, uint val);
+extern void ai_core_reset(si_t *sih, uint32 bits, uint32 resetbits);
+extern void ai_core_disable(si_t *sih, uint32 bits);
+extern int ai_numaddrspaces(si_t *sih);
+extern uint32 ai_addrspace(si_t *sih, uint asidx);
+extern uint32 ai_addrspacesize(si_t *sih, uint asidx);
+extern uint ai_wrap_reg(si_t *sih, uint32 offset, uint32 mask, uint32 val);
+
+
+
+#define ub_scan(a, b, c) do {} while (0)
+#define ub_flag(a) (0)
+#define ub_setint(a, b) do {} while (0)
+#define ub_coreidx(a) (0)
+#define ub_corevendor(a) (0)
+#define ub_corerev(a) (0)
+#define ub_iscoreup(a) (0)
+#define ub_setcoreidx(a, b) (0)
+#define ub_core_cflags(a, b, c) (0)
+#define ub_core_cflags_wo(a, b, c) do {} while (0)
+#define ub_core_sflags(a, b, c) (0)
+#define ub_corereg(a, b, c, d, e) (0)
+#define ub_core_reset(a, b, c) do {} while (0)
+#define ub_core_disable(a, b) do {} while (0)
+#define ub_numaddrspaces(a) (0)
+#define ub_addrspace(a, b)  (0)
+#define ub_addrspacesize(a, b) (0)
+#define ub_view(a, b) do {} while (0)
+#define ub_dumpregs(a, b) do {} while (0)
+
+#endif	/* _siutils_priv_h_ */
diff --git a/drivers/net/wireless/bcmdhd/uamp_api.h b/drivers/net/wireless/bcmdhd/uamp_api.h
new file mode 100644
index 000000000000..c51c68cd0eed
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/uamp_api.h
@@ -0,0 +1,176 @@
+/*
+ *  Name:       uamp_api.h
+ *
+ *  Description: Universal AMP API
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: uamp_api.h,v 1.2.8.1 2011-02-05 00:16:14 Exp $
+ *
+ */
+#ifndef UAMP_API_H
+#define UAMP_API_H
+
+
+#include "typedefs.h"
+
+
+/*****************************************************************************
+**  Constant and Type Definitions
+******************************************************************************
+*/
+
+#define BT_API
+
+/* Types. */
+typedef bool	BOOLEAN;
+typedef uint8	UINT8;
+typedef uint16	UINT16;
+
+
+/* UAMP identifiers */
+#define UAMP_ID_1   1
+#define UAMP_ID_2   2
+typedef UINT8 tUAMP_ID;
+
+/* UAMP event ids (used by UAMP_CBACK) */
+#define UAMP_EVT_RX_READY           0   /* Data from AMP controller is ready to be read */
+#define UAMP_EVT_CTLR_REMOVED       1   /* Controller removed */
+#define UAMP_EVT_CTLR_READY         2   /* Controller added/ready */
+typedef UINT8 tUAMP_EVT;
+
+
+/* UAMP Channels */
+#define UAMP_CH_HCI_CMD            0   /* HCI Command channel */
+#define UAMP_CH_HCI_EVT            1   /* HCI Event channel */
+#define UAMP_CH_HCI_DATA           2   /* HCI ACL Data channel */
+typedef UINT8 tUAMP_CH;
+
+/* tUAMP_EVT_DATA: union for event-specific data, used by UAMP_CBACK */
+typedef union {
+    tUAMP_CH channel;       /* UAMP_EVT_RX_READY: channel for which rx occured */
+} tUAMP_EVT_DATA;
+
+
+/*****************************************************************************
+**
+** Function:    UAMP_CBACK
+**
+** Description: Callback for events. Register callback using UAMP_Init.
+**
+** Parameters   amp_id:         AMP device identifier that generated the event
+**              amp_evt:        event id
+**              p_amp_evt_data: pointer to event-specific data
+**
+******************************************************************************
+*/
+typedef void (*tUAMP_CBACK)(tUAMP_ID amp_id, tUAMP_EVT amp_evt, tUAMP_EVT_DATA *p_amp_evt_data);
+
+/*****************************************************************************
+**  external function declarations
+******************************************************************************
+*/
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/*****************************************************************************
+**
+** Function:    UAMP_Init
+**
+** Description: Initialize UAMP driver
+**
+** Parameters   p_cback:    Callback function for UAMP event notification
+**
+******************************************************************************
+*/
+BT_API BOOLEAN UAMP_Init(tUAMP_CBACK p_cback);
+
+
+/*****************************************************************************
+**
+** Function:    UAMP_Open
+**
+** Description: Open connection to local AMP device.
+**
+** Parameters   app_id: Application specific AMP identifer. This value
+**                      will be included in AMP messages sent to the
+**                      BTU task, to identify source of the message
+**
+******************************************************************************
+*/
+BT_API BOOLEAN UAMP_Open(tUAMP_ID amp_id);
+
+/*****************************************************************************
+**
+** Function:    UAMP_Close
+**
+** Description: Close connection to local AMP device.
+**
+** Parameters   app_id: Application specific AMP identifer.
+**
+******************************************************************************
+*/
+BT_API void UAMP_Close(tUAMP_ID amp_id);
+
+
+/*****************************************************************************
+**
+** Function:    UAMP_Write
+**
+** Description: Send buffer to AMP device. Frees GKI buffer when done.
+**
+**
+** Parameters:  app_id:     AMP identifer.
+**              p_buf:      pointer to buffer to write
+**              num_bytes:  number of bytes to write
+**              channel:    UAMP_CH_HCI_ACL, or UAMP_CH_HCI_CMD
+**
+** Returns:     number of bytes written
+**
+******************************************************************************
+*/
+BT_API UINT16 UAMP_Write(tUAMP_ID amp_id, UINT8 *p_buf, UINT16 num_bytes, tUAMP_CH channel);
+
+/*****************************************************************************
+**
+** Function:    UAMP_Read
+**
+** Description: Read incoming data from AMP. Call after receiving a
+**              UAMP_EVT_RX_READY callback event.
+**
+** Parameters:  app_id:     AMP identifer.
+**              p_buf:      pointer to buffer for holding incoming AMP data
+**              buf_size:   size of p_buf
+**              channel:    UAMP_CH_HCI_ACL, or UAMP_CH_HCI_EVT
+**
+** Returns:     number of bytes read
+**
+******************************************************************************
+*/
+BT_API UINT16 UAMP_Read(tUAMP_ID amp_id, UINT8 *p_buf, UINT16 buf_size, tUAMP_CH channel);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* UAMP_API_H */
diff --git a/drivers/net/wireless/bcmdhd/wl_android.c b/drivers/net/wireless/bcmdhd/wl_android.c
new file mode 100644
index 000000000000..4fcdcd32a3ff
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/wl_android.c
@@ -0,0 +1,858 @@
+/*
+ * Linux cfg80211 driver - Android related functions
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wl_android.c,v 1.1.4.1.2.14 2011/02/09 01:40:07 Exp $
+ */
+
+#include <linux/module.h>
+#include <linux/netdevice.h>
+
+#include <wl_android.h>
+#include <wldev_common.h>
+#include <wlioctl.h>
+#include <bcmutils.h>
+#include <linux_osl.h>
+#include <dhd_dbg.h>
+#include <dngl_stats.h>
+#include <dhd.h>
+#include <bcmsdbus.h>
+#ifdef WL_CFG80211
+#include <wl_cfg80211.h>
+#endif
+#if defined(CONFIG_WIFI_CONTROL_FUNC)
+#include <linux/platform_device.h>
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35))
+#include <linux/wlan_plat.h>
+#else
+#include <linux/wifi_tiwlan.h>
+#endif
+#endif /* CONFIG_WIFI_CONTROL_FUNC */
+
+/*
+ * Android private command strings, PLEASE define new private commands here
+ * so they can be updated easily in the future (if needed)
+ */
+
+#define CMD_START				"START"
+#define CMD_STOP				"STOP"
+#define CMD_SCAN_ACTIVE			"SCAN-ACTIVE"
+#define CMD_SCAN_PASSIVE		"SCAN-PASSIVE"
+#define CMD_RSSI				"RSSI"
+#define CMD_LINKSPEED			"LINKSPEED"
+#define CMD_RXFILTER_START		"RXFILTER-START"
+#define CMD_RXFILTER_STOP		"RXFILTER-STOP"
+#define CMD_RXFILTER_ADD		"RXFILTER-ADD"
+#define CMD_RXFILTER_REMOVE		"RXFILTER-REMOVE"
+#define CMD_BTCOEXSCAN_START	"BTCOEXSCAN-START"
+#define CMD_BTCOEXSCAN_STOP		"BTCOEXSCAN-STOP"
+#define CMD_BTCOEXMODE			"BTCOEXMODE"
+#define CMD_SETSUSPENDOPT		"SETSUSPENDOPT"
+#define CMD_SETSUSPENDMODE		"SETSUSPENDMODE"
+#define CMD_P2P_DEV_ADDR		"P2P_DEV_ADDR"
+#define CMD_SETFWPATH			"SETFWPATH"
+#define CMD_SETBAND				"SETBAND"
+#define CMD_GETBAND				"GETBAND"
+#define CMD_COUNTRY				"COUNTRY"
+#define CMD_P2P_SET_NOA			"P2P_SET_NOA"
+#if !defined WL_ENABLE_P2P_IF
+#define CMD_P2P_GET_NOA			"P2P_GET_NOA"
+#endif
+#define CMD_P2P_SET_PS			"P2P_SET_PS"
+#define CMD_SET_AP_WPS_P2P_IE	"SET_AP_WPS_P2P_IE"
+
+
+#ifdef PNO_SUPPORT
+#define CMD_PNOSSIDCLR_SET	"PNOSSIDCLR"
+#define CMD_PNOSETUP_SET	"PNOSETUP "
+#define CMD_PNOENABLE_SET	"PNOFORCE"
+#define CMD_PNODEBUG_SET	"PNODEBUG"
+
+#define PNO_TLV_PREFIX			'S'
+#define PNO_TLV_VERSION			'1'
+#define PNO_TLV_SUBVERSION 		'2'
+#define PNO_TLV_RESERVED		'0'
+#define PNO_TLV_TYPE_SSID_IE		'S'
+#define PNO_TLV_TYPE_TIME		'T'
+#define PNO_TLV_FREQ_REPEAT		'R'
+#define PNO_TLV_FREQ_EXPO_MAX		'M'
+
+typedef struct cmd_tlv {
+	char prefix;
+	char version;
+	char subver;
+	char reserved;
+} cmd_tlv_t;
+#endif /* PNO_SUPPORT */
+
+typedef struct android_wifi_priv_cmd {
+	char *buf;
+	int used_len;
+	int total_len;
+} android_wifi_priv_cmd;
+
+/**
+ * Extern function declarations (TODO: move them to dhd_linux.h)
+ */
+void dhd_customer_gpio_wlan_ctrl(int onoff);
+uint dhd_dev_reset(struct net_device *dev, uint8 flag);
+int dhd_dev_init_ioctl(struct net_device *dev);
+#ifdef WL_CFG80211
+int wl_cfg80211_get_p2p_dev_addr(struct net_device *net, struct ether_addr *p2pdev_addr);
+int wl_cfg80211_set_btcoex_dhcp(struct net_device *dev, char *command);
+#else
+int wl_cfg80211_get_p2p_dev_addr(struct net_device *net, struct ether_addr *p2pdev_addr)
+{ return 0; }
+int wl_cfg80211_set_p2p_noa(struct net_device *net, char* buf, int len)
+{ return 0; }
+int wl_cfg80211_get_p2p_noa(struct net_device *net, char* buf, int len)
+{ return 0; }
+int wl_cfg80211_set_p2p_ps(struct net_device *net, char* buf, int len)
+{ return 0; }
+#endif
+extern int dhd_os_check_if_up(void *dhdp);
+extern void *bcmsdh_get_drvdata(void);
+
+extern bool ap_fw_loaded;
+#ifdef CUSTOMER_HW2
+extern char iface_name[IFNAMSIZ];
+#endif
+
+/**
+ * Local (static) functions and variables
+ */
+
+/* Initialize g_wifi_on to 1 so dhd_bus_start will be called for the first
+ * time (only) in dhd_open, subsequential wifi on will be handled by
+ * wl_android_wifi_on
+ */
+static int g_wifi_on = TRUE;
+
+/**
+ * Local (static) function definitions
+ */
+static int wl_android_get_link_speed(struct net_device *net, char *command, int total_len)
+{
+	int link_speed;
+	int bytes_written;
+	int error;
+
+	error = wldev_get_link_speed(net, &link_speed);
+	if (error)
+		return -1;
+
+	/* Convert Kbps to Android Mbps */
+	link_speed = link_speed / 1000;
+	bytes_written = snprintf(command, total_len, "LinkSpeed %d", link_speed);
+	DHD_INFO(("%s: command result is %s\n", __FUNCTION__, command));
+	return bytes_written;
+}
+
+static int wl_android_get_rssi(struct net_device *net, char *command, int total_len)
+{
+	wlc_ssid_t ssid = {0};
+	int rssi;
+	int bytes_written = 0;
+	int error;
+
+	error = wldev_get_rssi(net, &rssi);
+	if (error)
+		return -1;
+
+	error = wldev_get_ssid(net, &ssid);
+	if (error)
+		return -1;
+	if ((ssid.SSID_len == 0) || (ssid.SSID_len > DOT11_MAX_SSID_LEN)) {
+		DHD_ERROR(("%s: wldev_get_ssid failed\n", __FUNCTION__));
+	} else {
+		memcpy(command, ssid.SSID, ssid.SSID_len);
+		bytes_written = ssid.SSID_len;
+	}
+	bytes_written += snprintf(&command[bytes_written], total_len, " rssi %d", rssi);
+	DHD_INFO(("%s: command result is %s (%d)\n", __FUNCTION__, command, bytes_written));
+	return bytes_written;
+}
+
+static int wl_android_set_suspendopt(struct net_device *dev, char *command, int total_len)
+{
+	int suspend_flag;
+	int ret_now;
+	int ret = 0;
+
+	suspend_flag = *(command + strlen(CMD_SETSUSPENDOPT) + 1) - '0';
+
+	if (suspend_flag != 0)
+		suspend_flag = 1;
+	ret_now = net_os_set_suspend_disable(dev, suspend_flag);
+
+	if (ret_now != suspend_flag) {
+		if (!(ret = net_os_set_suspend(dev, ret_now, 1)))
+			DHD_INFO(("%s: Suspend Flag %d -> %d\n",
+				__FUNCTION__, ret_now, suspend_flag));
+		else
+			DHD_ERROR(("%s: failed %d\n", __FUNCTION__, ret));
+	}
+	return ret;
+}
+
+static int wl_android_set_suspendmode(struct net_device *dev, char *command, int total_len)
+{
+	int ret = 0;
+
+#if !defined(CONFIG_HAS_EARLYSUSPEND) || !defined(DHD_USE_EARLYSUSPEND)
+	int suspend_flag;
+
+	suspend_flag = *(command + strlen(CMD_SETSUSPENDMODE) + 1) - '0';
+
+	if (suspend_flag != 0)
+		suspend_flag = 1;
+
+	if (!(ret = net_os_set_suspend(dev, suspend_flag, 0)))
+		DHD_INFO(("%s: Suspend Mode %d\n",__FUNCTION__,suspend_flag));
+	else
+		DHD_ERROR(("%s: failed %d\n",__FUNCTION__,ret));
+#endif
+	return ret;
+}
+
+static int wl_android_get_band(struct net_device *dev, char *command, int total_len)
+{
+	uint band;
+	int bytes_written;
+	int error;
+
+	error = wldev_get_band(dev, &band);
+	if (error)
+		return -1;
+	bytes_written = snprintf(command, total_len, "Band %d", band);
+	return bytes_written;
+}
+
+#if defined(PNO_SUPPORT) && !defined(WL_SCHED_SCAN)
+static int wl_android_set_pno_setup(struct net_device *dev, char *command, int total_len)
+{
+	wlc_ssid_t ssids_local[MAX_PFN_LIST_COUNT];
+	int res = -1;
+	int nssid = 0;
+	cmd_tlv_t *cmd_tlv_temp;
+	char *str_ptr;
+	int tlv_size_left;
+	int pno_time = 0;
+	int pno_repeat = 0;
+	int pno_freq_expo_max = 0;
+
+#ifdef PNO_SET_DEBUG
+	int i;
+	char pno_in_example[] = {
+		'P', 'N', 'O', 'S', 'E', 'T', 'U', 'P', ' ',
+		'S', '1', '2', '0',
+		'S',
+		0x05,
+		'd', 'l', 'i', 'n', 'k',
+		'S',
+		0x04,
+		'G', 'O', 'O', 'G',
+		'T',
+		'0', 'B',
+		'R',
+		'2',
+		'M',
+		'2',
+		0x00
+		};
+#endif /* PNO_SET_DEBUG */
+
+	DHD_INFO(("%s: command=%s, len=%d\n", __FUNCTION__, command, total_len));
+
+	if (total_len < (strlen(CMD_PNOSETUP_SET) + sizeof(cmd_tlv_t))) {
+		DHD_ERROR(("%s argument=%d less min size\n", __FUNCTION__, total_len));
+		goto exit_proc;
+	}
+
+#ifdef PNO_SET_DEBUG
+	memcpy(command, pno_in_example, sizeof(pno_in_example));
+	for (i = 0; i < sizeof(pno_in_example); i++)
+		printf("%02X ", command[i]);
+	printf("\n");
+	total_len = sizeof(pno_in_example);
+#endif
+
+	str_ptr = command + strlen(CMD_PNOSETUP_SET);
+	tlv_size_left = total_len - strlen(CMD_PNOSETUP_SET);
+
+	cmd_tlv_temp = (cmd_tlv_t *)str_ptr;
+	memset(ssids_local, 0, sizeof(ssids_local));
+
+	if ((cmd_tlv_temp->prefix == PNO_TLV_PREFIX) &&
+		(cmd_tlv_temp->version == PNO_TLV_VERSION) &&
+		(cmd_tlv_temp->subver == PNO_TLV_SUBVERSION)) {
+
+		str_ptr += sizeof(cmd_tlv_t);
+		tlv_size_left -= sizeof(cmd_tlv_t);
+
+		if ((nssid = wl_iw_parse_ssid_list_tlv(&str_ptr, ssids_local,
+			MAX_PFN_LIST_COUNT, &tlv_size_left)) <= 0) {
+			DHD_ERROR(("SSID is not presented or corrupted ret=%d\n", nssid));
+			goto exit_proc;
+		} else {
+			if ((str_ptr[0] != PNO_TLV_TYPE_TIME) || (tlv_size_left <= 1)) {
+				DHD_ERROR(("%s scan duration corrupted field size %d\n",
+					__FUNCTION__, tlv_size_left));
+				goto exit_proc;
+			}
+			str_ptr++;
+			pno_time = simple_strtoul(str_ptr, &str_ptr, 16);
+			DHD_INFO(("%s: pno_time=%d\n", __FUNCTION__, pno_time));
+
+			if (str_ptr[0] != 0) {
+				if ((str_ptr[0] != PNO_TLV_FREQ_REPEAT)) {
+					DHD_ERROR(("%s pno repeat : corrupted field\n",
+						__FUNCTION__));
+					goto exit_proc;
+				}
+				str_ptr++;
+				pno_repeat = simple_strtoul(str_ptr, &str_ptr, 16);
+				DHD_INFO(("%s :got pno_repeat=%d\n", __FUNCTION__, pno_repeat));
+				if (str_ptr[0] != PNO_TLV_FREQ_EXPO_MAX) {
+					DHD_ERROR(("%s FREQ_EXPO_MAX corrupted field size\n",
+						__FUNCTION__));
+					goto exit_proc;
+				}
+				str_ptr++;
+				pno_freq_expo_max = simple_strtoul(str_ptr, &str_ptr, 16);
+				DHD_INFO(("%s: pno_freq_expo_max=%d\n",
+					__FUNCTION__, pno_freq_expo_max));
+			}
+		}
+	} else {
+		DHD_ERROR(("%s get wrong TLV command\n", __FUNCTION__));
+		goto exit_proc;
+	}
+
+	res = dhd_dev_pno_set(dev, ssids_local, nssid, pno_time, pno_repeat, pno_freq_expo_max);
+
+exit_proc:
+	return res;
+}
+#endif /* PNO_SUPPORT && !WL_SCHED_SCAN */
+
+static int wl_android_get_p2p_dev_addr(struct net_device *ndev, char *command, int total_len)
+{
+	int ret;
+	int bytes_written = 0;
+
+	ret = wl_cfg80211_get_p2p_dev_addr(ndev, (struct ether_addr*)command);
+	if (ret)
+		return 0;
+	bytes_written = sizeof(struct ether_addr);
+	return bytes_written;
+}
+
+/**
+ * Global function definitions (declared in wl_android.h)
+ */
+
+int wl_android_wifi_on(struct net_device *dev)
+{
+	int ret = 0;
+
+	printk("%s in\n", __FUNCTION__);
+	if (!dev) {
+		DHD_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return -EINVAL;
+	}
+
+	dhd_net_if_lock(dev);
+	if (!g_wifi_on) {
+		dhd_customer_gpio_wlan_ctrl(WLAN_RESET_ON);
+		sdioh_start(NULL, 0);
+		ret = dhd_dev_reset(dev, FALSE);
+		sdioh_start(NULL, 1);
+		if (!ret) {
+			if (dhd_dev_init_ioctl(dev) < 0)
+				ret = -EFAULT;
+		}
+		g_wifi_on = 1;
+	}
+	dhd_net_if_unlock(dev);
+
+	return ret;
+}
+
+int wl_android_wifi_off(struct net_device *dev)
+{
+	int ret = 0;
+
+	printk("%s in\n", __FUNCTION__);
+	if (!dev) {
+		DHD_TRACE(("%s: dev is null\n", __FUNCTION__));
+		return -EINVAL;
+	}
+
+	dhd_net_if_lock(dev);
+	if (g_wifi_on) {
+		ret = dhd_dev_reset(dev, TRUE);
+		sdioh_stop(NULL);
+		dhd_customer_gpio_wlan_ctrl(WLAN_RESET_OFF);
+		g_wifi_on = 0;
+	}
+	dhd_net_if_unlock(dev);
+
+	return ret;
+}
+
+static int wl_android_set_fwpath(struct net_device *net, char *command, int total_len)
+{
+	if ((strlen(command) - strlen(CMD_SETFWPATH)) > MOD_PARAM_PATHLEN)
+		return -1;
+	bcm_strncpy_s(fw_path, sizeof(fw_path),
+		command + strlen(CMD_SETFWPATH) + 1, MOD_PARAM_PATHLEN - 1);
+	if (strstr(fw_path, "apsta") != NULL) {
+		DHD_INFO(("GOT APSTA FIRMWARE\n"));
+		ap_fw_loaded = TRUE;
+	} else {
+		DHD_INFO(("GOT STA FIRMWARE\n"));
+		ap_fw_loaded = FALSE;
+	}
+	return 0;
+}
+
+int wl_android_priv_cmd(struct net_device *net, struct ifreq *ifr, int cmd)
+{
+	int ret = 0;
+	char *command = NULL;
+	int bytes_written = 0;
+	android_wifi_priv_cmd priv_cmd;
+
+	net_os_wake_lock(net);
+
+	if (!ifr->ifr_data) {
+		ret = -EINVAL;
+		goto exit;
+	}
+	if (copy_from_user(&priv_cmd, ifr->ifr_data, sizeof(android_wifi_priv_cmd))) {
+		ret = -EFAULT;
+		goto exit;
+	}
+	command = kmalloc(priv_cmd.total_len, GFP_KERNEL);
+	if (!command)
+	{
+		DHD_ERROR(("%s: failed to allocate memory\n", __FUNCTION__));
+		ret = -ENOMEM;
+		goto exit;
+	}
+	if (copy_from_user(command, priv_cmd.buf, priv_cmd.total_len)) {
+		ret = -EFAULT;
+		goto exit;
+	}
+
+	DHD_INFO(("%s: Android private cmd \"%s\" on %s\n", __FUNCTION__, command, ifr->ifr_name));
+
+	if (strnicmp(command, CMD_START, strlen(CMD_START)) == 0) {
+		DHD_INFO(("%s, Received regular START command\n", __FUNCTION__));
+		bytes_written = wl_android_wifi_on(net);
+	}
+	else if (strnicmp(command, CMD_SETFWPATH, strlen(CMD_SETFWPATH)) == 0) {
+		bytes_written = wl_android_set_fwpath(net, command, priv_cmd.total_len);
+	}
+
+	if (!g_wifi_on) {
+		DHD_ERROR(("%s: Ignore private cmd \"%s\" - iface %s is down\n",
+			__FUNCTION__, command, ifr->ifr_name));
+		ret = 0;
+		goto exit;
+	}
+
+	if (strnicmp(command, CMD_STOP, strlen(CMD_STOP)) == 0) {
+		bytes_written = wl_android_wifi_off(net);
+	}
+	else if (strnicmp(command, CMD_SCAN_ACTIVE, strlen(CMD_SCAN_ACTIVE)) == 0) {
+		/* TBD: SCAN-ACTIVE */
+	}
+	else if (strnicmp(command, CMD_SCAN_PASSIVE, strlen(CMD_SCAN_PASSIVE)) == 0) {
+		/* TBD: SCAN-PASSIVE */
+	}
+	else if (strnicmp(command, CMD_RSSI, strlen(CMD_RSSI)) == 0) {
+		bytes_written = wl_android_get_rssi(net, command, priv_cmd.total_len);
+	}
+	else if (strnicmp(command, CMD_LINKSPEED, strlen(CMD_LINKSPEED)) == 0) {
+		bytes_written = wl_android_get_link_speed(net, command, priv_cmd.total_len);
+	}
+	else if (strnicmp(command, CMD_RXFILTER_START, strlen(CMD_RXFILTER_START)) == 0) {
+		bytes_written = net_os_set_packet_filter(net, 1);
+	}
+	else if (strnicmp(command, CMD_RXFILTER_STOP, strlen(CMD_RXFILTER_STOP)) == 0) {
+		bytes_written = net_os_set_packet_filter(net, 0);
+	}
+	else if (strnicmp(command, CMD_RXFILTER_ADD, strlen(CMD_RXFILTER_ADD)) == 0) {
+		int filter_num = *(command + strlen(CMD_RXFILTER_ADD) + 1) - '0';
+		bytes_written = net_os_rxfilter_add_remove(net, TRUE, filter_num);
+	}
+	else if (strnicmp(command, CMD_RXFILTER_REMOVE, strlen(CMD_RXFILTER_REMOVE)) == 0) {
+		int filter_num = *(command + strlen(CMD_RXFILTER_REMOVE) + 1) - '0';
+		bytes_written = net_os_rxfilter_add_remove(net, FALSE, filter_num);
+	}
+	else if (strnicmp(command, CMD_BTCOEXSCAN_START, strlen(CMD_BTCOEXSCAN_START)) == 0) {
+		/* TBD: BTCOEXSCAN-START */
+	}
+	else if (strnicmp(command, CMD_BTCOEXSCAN_STOP, strlen(CMD_BTCOEXSCAN_STOP)) == 0) {
+		/* TBD: BTCOEXSCAN-STOP */
+	}
+	else if (strnicmp(command, CMD_BTCOEXMODE, strlen(CMD_BTCOEXMODE)) == 0) {
+		uint mode = *(command + strlen(CMD_BTCOEXMODE) + 1) - '0';
+
+		if (mode == 1)
+			net_os_set_packet_filter(net, 0); /* DHCP starts */
+		else
+			net_os_set_packet_filter(net, 1); /* DHCP ends */
+#ifdef WL_CFG80211
+		bytes_written = wl_cfg80211_set_btcoex_dhcp(net, command);
+#endif
+	}
+	else if (strnicmp(command, CMD_SETSUSPENDOPT, strlen(CMD_SETSUSPENDOPT)) == 0) {
+		bytes_written = wl_android_set_suspendopt(net, command, priv_cmd.total_len);
+	}
+	else if (strnicmp(command, CMD_SETSUSPENDMODE, strlen(CMD_SETSUSPENDMODE)) == 0) {
+		bytes_written = wl_android_set_suspendmode(net, command, priv_cmd.total_len);
+	}
+	else if (strnicmp(command, CMD_SETBAND, strlen(CMD_SETBAND)) == 0) {
+		uint band = *(command + strlen(CMD_SETBAND) + 1) - '0';
+		bytes_written = wldev_set_band(net, band);
+	}
+	else if (strnicmp(command, CMD_GETBAND, strlen(CMD_GETBAND)) == 0) {
+		bytes_written = wl_android_get_band(net, command, priv_cmd.total_len);
+	}
+	else if (strnicmp(command, CMD_COUNTRY, strlen(CMD_COUNTRY)) == 0) {
+		char *country_code = command + strlen(CMD_COUNTRY) + 1;
+		bytes_written = wldev_set_country(net, country_code);
+	}
+#if defined(PNO_SUPPORT) && !defined(WL_SCHED_SCAN)
+	else if (strnicmp(command, CMD_PNOSSIDCLR_SET, strlen(CMD_PNOSSIDCLR_SET)) == 0) {
+		bytes_written = dhd_dev_pno_reset(net);
+	}
+	else if (strnicmp(command, CMD_PNOSETUP_SET, strlen(CMD_PNOSETUP_SET)) == 0) {
+		bytes_written = wl_android_set_pno_setup(net, command, priv_cmd.total_len);
+	}
+	else if (strnicmp(command, CMD_PNOENABLE_SET, strlen(CMD_PNOENABLE_SET)) == 0) {
+		uint pfn_enabled = *(command + strlen(CMD_PNOENABLE_SET) + 1) - '0';
+		bytes_written = dhd_dev_pno_enable(net, pfn_enabled);
+	}
+#endif
+	else if (strnicmp(command, CMD_P2P_DEV_ADDR, strlen(CMD_P2P_DEV_ADDR)) == 0) {
+		bytes_written = wl_android_get_p2p_dev_addr(net, command, priv_cmd.total_len);
+	}
+	else if (strnicmp(command, CMD_P2P_SET_NOA, strlen(CMD_P2P_SET_NOA)) == 0) {
+		int skip = strlen(CMD_P2P_SET_NOA) + 1;
+		bytes_written = wl_cfg80211_set_p2p_noa(net, command + skip,
+			priv_cmd.total_len - skip);
+	}
+#if !defined WL_ENABLE_P2P_IF
+	else if (strnicmp(command, CMD_P2P_GET_NOA, strlen(CMD_P2P_GET_NOA)) == 0) {
+		bytes_written = wl_cfg80211_get_p2p_noa(net, command, priv_cmd.total_len);
+	}
+#endif
+	else if (strnicmp(command, CMD_P2P_SET_PS, strlen(CMD_P2P_SET_PS)) == 0) {
+		int skip = strlen(CMD_P2P_SET_PS) + 1;
+		bytes_written = wl_cfg80211_set_p2p_ps(net, command + skip,
+			priv_cmd.total_len - skip);
+	}
+#ifdef WL_CFG80211
+	else if (strnicmp(command, CMD_SET_AP_WPS_P2P_IE,
+		strlen(CMD_SET_AP_WPS_P2P_IE)) == 0) {
+		int skip = strlen(CMD_SET_AP_WPS_P2P_IE) + 3;
+		bytes_written = wl_cfg80211_set_wps_p2p_ie(net, command + skip,
+			priv_cmd.total_len - skip, *(command + skip - 2) - '0');
+	}
+#endif /* WL_CFG80211 */
+	else {
+		DHD_ERROR(("Unknown PRIVATE command %s - ignored\n", command));
+		snprintf(command, 3, "OK");
+		bytes_written = strlen("OK");
+	}
+
+	if (bytes_written >= 0) {
+		if ((bytes_written == 0) && (priv_cmd.total_len > 0))
+			command[0] = '\0';
+		if (bytes_written >= priv_cmd.total_len) {
+			DHD_ERROR(("%s: bytes_written = %d\n", __FUNCTION__, bytes_written));
+			bytes_written = priv_cmd.total_len;
+		} else {
+			bytes_written++;
+		}
+		priv_cmd.used_len = bytes_written;
+		if (copy_to_user(priv_cmd.buf, command, bytes_written)) {
+			DHD_ERROR(("%s: failed to copy data to user buffer\n", __FUNCTION__));
+			ret = -EFAULT;
+		}
+	}
+	else {
+		ret = bytes_written;
+	}
+
+exit:
+	net_os_wake_unlock(net);
+	if (command) {
+		kfree(command);
+	}
+
+	return ret;
+}
+
+int wl_android_init(void)
+{
+	int ret = 0;
+
+	dhd_msg_level |= DHD_ERROR_VAL;
+#ifdef ENABLE_INSMOD_NO_FW_LOAD
+	dhd_download_fw_on_driverload = FALSE;
+#endif /* ENABLE_INSMOD_NO_FW_LOAD */
+#ifdef CUSTOMER_HW2
+	if (!iface_name[0]) {
+		memset(iface_name, 0, IFNAMSIZ);
+		bcm_strncpy_s(iface_name, IFNAMSIZ, "wlan", IFNAMSIZ);
+	}
+#endif /* CUSTOMER_HW2 */
+	return ret;
+}
+
+int wl_android_exit(void)
+{
+	int ret = 0;
+
+	return ret;
+}
+
+void wl_android_post_init(void)
+{
+	if (!dhd_download_fw_on_driverload) {
+		/* Call customer gpio to turn off power with WL_REG_ON signal */
+		dhd_customer_gpio_wlan_ctrl(WLAN_RESET_OFF);
+		g_wifi_on = 0;
+	}
+}
+/**
+ * Functions for Android WiFi card detection
+ */
+#if defined(CONFIG_WIFI_CONTROL_FUNC)
+
+static int g_wifidev_registered = 0;
+static struct semaphore wifi_control_sem;
+static struct wifi_platform_data *wifi_control_data = NULL;
+static struct resource *wifi_irqres = NULL;
+
+static int wifi_add_dev(void);
+static void wifi_del_dev(void);
+
+int wl_android_wifictrl_func_add(void)
+{
+	int ret = 0;
+	sema_init(&wifi_control_sem, 0);
+
+	ret = wifi_add_dev();
+	if (ret) {
+		DHD_ERROR(("%s: platform_driver_register failed\n", __FUNCTION__));
+		return ret;
+	}
+	g_wifidev_registered = 1;
+
+	/* Waiting callback after platform_driver_register is done or exit with error */
+	if (down_timeout(&wifi_control_sem,  msecs_to_jiffies(1000)) != 0) {
+		ret = -EINVAL;
+		DHD_ERROR(("%s: platform_driver_register timeout\n", __FUNCTION__));
+	}
+
+	return ret;
+}
+
+void wl_android_wifictrl_func_del(void)
+{
+	if (g_wifidev_registered)
+	{
+		wifi_del_dev();
+		g_wifidev_registered = 0;
+	}
+}
+
+void* wl_android_prealloc(int section, unsigned long size)
+{
+	void *alloc_ptr = NULL;
+	if (wifi_control_data && wifi_control_data->mem_prealloc) {
+		alloc_ptr = wifi_control_data->mem_prealloc(section, size);
+		if (alloc_ptr) {
+			DHD_INFO(("success alloc section %d\n", section));
+			if (size != 0L)
+				bzero(alloc_ptr, size);
+			return alloc_ptr;
+		}
+	}
+
+	DHD_ERROR(("can't alloc section %d\n", section));
+	return NULL;
+}
+
+int wifi_get_irq_number(unsigned long *irq_flags_ptr)
+{
+	if (wifi_irqres) {
+		*irq_flags_ptr = wifi_irqres->flags & IRQF_TRIGGER_MASK;
+		return (int)wifi_irqres->start;
+	}
+#ifdef CUSTOM_OOB_GPIO_NUM
+	return CUSTOM_OOB_GPIO_NUM;
+#else
+	return -1;
+#endif
+}
+
+int wifi_set_power(int on, unsigned long msec)
+{
+	DHD_ERROR(("%s = %d\n", __FUNCTION__, on));
+	if (wifi_control_data && wifi_control_data->set_power) {
+		wifi_control_data->set_power(on);
+	}
+	if (msec)
+		msleep(msec);
+	return 0;
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35))
+int wifi_get_mac_addr(unsigned char *buf)
+{
+	DHD_ERROR(("%s\n", __FUNCTION__));
+	if (!buf)
+		return -EINVAL;
+	if (wifi_control_data && wifi_control_data->get_mac_addr) {
+		return wifi_control_data->get_mac_addr(buf);
+	}
+	return -EOPNOTSUPP;
+}
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 35)) */
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 39))
+void *wifi_get_country_code(char *ccode)
+{
+	DHD_TRACE(("%s\n", __FUNCTION__));
+	if (!ccode)
+		return NULL;
+	if (wifi_control_data && wifi_control_data->get_country_code) {
+		return wifi_control_data->get_country_code(ccode);
+	}
+	return NULL;
+}
+#endif /* (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 39)) */
+
+static int wifi_set_carddetect(int on)
+{
+	DHD_ERROR(("%s = %d\n", __FUNCTION__, on));
+	if (wifi_control_data && wifi_control_data->set_carddetect) {
+		wifi_control_data->set_carddetect(on);
+	}
+	return 0;
+}
+
+static int wifi_probe(struct platform_device *pdev)
+{
+	struct wifi_platform_data *wifi_ctrl =
+		(struct wifi_platform_data *)(pdev->dev.platform_data);
+
+	DHD_ERROR(("## %s\n", __FUNCTION__));
+	wifi_irqres = platform_get_resource_byname(pdev, IORESOURCE_IRQ, "bcmdhd_wlan_irq");
+	if (wifi_irqres == NULL)
+		wifi_irqres = platform_get_resource_byname(pdev,
+			IORESOURCE_IRQ, "bcm4329_wlan_irq");
+	wifi_control_data = wifi_ctrl;
+
+	wifi_set_power(1, 0);	/* Power On */
+	wifi_set_carddetect(1);	/* CardDetect (0->1) */
+
+	up(&wifi_control_sem);
+	return 0;
+}
+
+static int wifi_remove(struct platform_device *pdev)
+{
+	struct wifi_platform_data *wifi_ctrl =
+		(struct wifi_platform_data *)(pdev->dev.platform_data);
+
+	DHD_ERROR(("## %s\n", __FUNCTION__));
+	wifi_control_data = wifi_ctrl;
+
+	wifi_set_power(0, 0);	/* Power Off */
+	wifi_set_carddetect(0);	/* CardDetect (1->0) */
+
+	up(&wifi_control_sem);
+	return 0;
+}
+
+static int wifi_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	DHD_TRACE(("##> %s\n", __FUNCTION__));
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 39)) && defined(OOB_INTR_ONLY)
+	bcmsdh_oob_intr_set(0);
+#endif
+	return 0;
+}
+
+static int wifi_resume(struct platform_device *pdev)
+{
+	DHD_TRACE(("##> %s\n", __FUNCTION__));
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 39)) && defined(OOB_INTR_ONLY)
+	if (dhd_os_check_if_up(bcmsdh_get_drvdata()))
+		bcmsdh_oob_intr_set(1);
+#endif
+	return 0;
+}
+
+static struct platform_driver wifi_device = {
+	.probe          = wifi_probe,
+	.remove         = wifi_remove,
+	.suspend        = wifi_suspend,
+	.resume         = wifi_resume,
+	.driver         = {
+	.name   = "bcmdhd_wlan",
+	}
+};
+
+static struct platform_driver wifi_device_legacy = {
+	.probe          = wifi_probe,
+	.remove         = wifi_remove,
+	.suspend        = wifi_suspend,
+	.resume         = wifi_resume,
+	.driver         = {
+	.name   = "bcm4329_wlan",
+	}
+};
+
+static int wifi_add_dev(void)
+{
+	DHD_TRACE(("## Calling platform_driver_register\n"));
+	platform_driver_register(&wifi_device);
+	platform_driver_register(&wifi_device_legacy);
+	return 0;
+}
+
+static void wifi_del_dev(void)
+{
+	DHD_TRACE(("## Unregister platform_driver_register\n"));
+	platform_driver_unregister(&wifi_device);
+	platform_driver_unregister(&wifi_device_legacy);
+}
+#endif /* defined(CONFIG_WIFI_CONTROL_FUNC) */
diff --git a/drivers/net/wireless/bcmdhd/wl_android.h b/drivers/net/wireless/bcmdhd/wl_android.h
new file mode 100644
index 000000000000..3983306cfe38
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/wl_android.h
@@ -0,0 +1,57 @@
+/*
+ * Linux cfg80211 driver - Android related functions
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ *
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ *
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ *
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wl_android.c,v 1.1.4.1.2.14 2011/02/09 01:40:07 Exp $
+ */
+
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <wldev_common.h>
+
+/**
+ * Android platform dependent functions, feel free to add Android specific functions here
+ * (save the macros in dhd). Please do NOT declare functions that are NOT exposed to dhd
+ * or cfg, define them as static in wl_android.c
+ */
+
+/**
+ * wl_android_init will be called from module init function (dhd_module_init now), similarly
+ * wl_android_exit will be called from module exit function (dhd_module_cleanup now)
+ */
+int wl_android_init(void);
+int wl_android_exit(void);
+void wl_android_post_init(void);
+int wl_android_wifi_on(struct net_device *dev);
+int wl_android_wifi_off(struct net_device *dev);
+int wl_android_priv_cmd(struct net_device *net, struct ifreq *ifr, int cmd);
+
+#if defined(CONFIG_WIFI_CONTROL_FUNC)
+int wl_android_wifictrl_func_add(void);
+void wl_android_wifictrl_func_del(void);
+void* wl_android_prealloc(int section, unsigned long size);
+
+int wifi_get_irq_number(unsigned long *irq_flags_ptr);
+int wifi_set_power(int on, unsigned long msec);
+int wifi_get_mac_addr(unsigned char *buf);
+void *wifi_get_country_code(char *ccode);
+#endif /* CONFIG_WIFI_CONTROL_FUNC */
diff --git a/drivers/net/wireless/bcmdhd/wl_cfg80211.c b/drivers/net/wireless/bcmdhd/wl_cfg80211.c
new file mode 100644
index 000000000000..2c4e9a63dd9e
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/wl_cfg80211.c
@@ -0,0 +1,7776 @@
+/*
+ * Linux cfg80211 driver
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wl_cfg80211.c,v 1.1.4.1.2.14 2011/02/09 01:40:07 Exp $
+ */
+
+#include <typedefs.h>
+#include <linuxver.h>
+#include <osl.h>
+#include <linux/kernel.h>
+
+#include <bcmutils.h>
+#include <bcmwifi.h>
+#include <bcmendian.h>
+#include <proto/ethernet.h>
+#include <proto/802.11.h>
+#include <linux/if_arp.h>
+#include <asm/uaccess.h>
+
+#include <dngl_stats.h>
+#include <dhd.h>
+#include <dhdioctl.h>
+#include <wlioctl.h>
+#include <dhd_cfg80211.h>
+
+#include <proto/ethernet.h>
+#include <linux/kernel.h>
+#include <linux/kthread.h>
+#include <linux/netdevice.h>
+#include <linux/sched.h>
+#include <linux/etherdevice.h>
+#include <linux/wireless.h>
+#include <linux/ieee80211.h>
+#include <linux/wait.h>
+#include <net/cfg80211.h>
+#include <net/rtnetlink.h>
+
+#include <wlioctl.h>
+#include <wldev_common.h>
+#include <wl_cfg80211.h>
+#include <wl_cfgp2p.h>
+
+static struct device *cfg80211_parent_dev = NULL;
+static int vsdb_supported = 0;
+struct wl_priv *wlcfg_drv_priv = NULL;
+
+u32 wl_dbg_level = WL_DBG_ERR;
+
+#define MAC2STR(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5]
+#define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x"
+#define MAX_WAIT_TIME 1500
+#define WL_SCAN_ACTIVE_TIME	 40
+#define WL_SCAN_PASSIVE_TIME	130
+#define WL_FRAME_LEN			300
+
+#define DNGL_FUNC(func, parameters) func parameters;
+#define COEX_DHCP
+
+
+/* This is to override regulatory domains defined in cfg80211 module (reg.c)
+ * By default world regulatory domain defined in reg.c puts the flags NL80211_RRF_PASSIVE_SCAN
+ * and NL80211_RRF_NO_IBSS for 5GHz channels (for 36..48 and 149..165).
+ * With respect to these flags, wpa_supplicant doesn't start p2p operations on 5GHz channels.
+ * All the chnages in world regulatory domain are to be done here.
+ */
+static const struct ieee80211_regdomain brcm_regdom = {
+	.n_reg_rules = 4,
+	.alpha2 =  "99",
+	.reg_rules = {
+		/* IEEE 802.11b/g, channels 1..11 */
+		REG_RULE(2412-10, 2472+10, 40, 6, 20, 0),
+		/* IEEE 802.11b/g, channels 12..13. No HT40
+		 * channel fits here.
+		 */
+		/* If any */
+		/*
+		 * IEEE 802.11 channel 14 - is for JP only,
+		 * we need cfg80211 to allow it (reg_flags = 0); so that
+		 * hostapd could request auto channel by sending down ch 14
+		 */
+		REG_RULE(2484-10, 2484+10, 20, 6, 20,
+		NL80211_RRF_PASSIVE_SCAN |
+		NL80211_RRF_NO_IBSS |
+		NL80211_RRF_NO_OFDM),
+		/* IEEE 802.11a, channel 36..64 */
+		REG_RULE(5150-10, 5350+10, 40, 6, 20, 0),
+		/* IEEE 802.11a, channel 100..165 */
+		REG_RULE(5470-10, 5850+10, 40, 6, 20, 0), }
+};
+
+
+/* Data Element Definitions */
+#define WPS_ID_CONFIG_METHODS     0x1008
+#define WPS_ID_REQ_TYPE           0x103A
+#define WPS_ID_DEVICE_NAME        0x1011
+#define WPS_ID_VERSION            0x104A
+#define WPS_ID_DEVICE_PWD_ID      0x1012
+#define WPS_ID_REQ_DEV_TYPE       0x106A
+#define WPS_ID_SELECTED_REGISTRAR_CONFIG_METHODS 0x1053
+#define WPS_ID_PRIM_DEV_TYPE      0x1054
+
+/* Device Password ID */
+#define DEV_PW_DEFAULT 0x0000
+#define DEV_PW_USER_SPECIFIED 0x0001,
+#define DEV_PW_MACHINE_SPECIFIED 0x0002
+#define DEV_PW_REKEY 0x0003
+#define DEV_PW_PUSHBUTTON 0x0004
+#define DEV_PW_REGISTRAR_SPECIFIED 0x0005
+
+/* Config Methods */
+#define WPS_CONFIG_USBA 0x0001
+#define WPS_CONFIG_ETHERNET 0x0002
+#define WPS_CONFIG_LABEL 0x0004
+#define WPS_CONFIG_DISPLAY 0x0008
+#define WPS_CONFIG_EXT_NFC_TOKEN 0x0010
+#define WPS_CONFIG_INT_NFC_TOKEN 0x0020
+#define WPS_CONFIG_NFC_INTERFACE 0x0040
+#define WPS_CONFIG_PUSHBUTTON 0x0080
+#define WPS_CONFIG_KEYPAD 0x0100
+#define WPS_CONFIG_VIRT_PUSHBUTTON 0x0280
+#define WPS_CONFIG_PHY_PUSHBUTTON 0x0480
+#define WPS_CONFIG_VIRT_DISPLAY 0x2008
+#define WPS_CONFIG_PHY_DISPLAY 0x4008
+
+/*
+ * cfg80211_ops api/callback list
+ */
+static s32 wl_frame_get_mgmt(u16 fc, const struct ether_addr *da,
+	const struct ether_addr *sa, const struct ether_addr *bssid,
+	u8 **pheader, u32 *body_len, u8 *pbody);
+static s32 __wl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
+	struct cfg80211_scan_request *request,
+	struct cfg80211_ssid *this_ssid);
+static s32 wl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
+	struct cfg80211_scan_request *request);
+static s32 wl_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed);
+static s32 wl_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
+	struct cfg80211_ibss_params *params);
+static s32 wl_cfg80211_leave_ibss(struct wiphy *wiphy,
+	struct net_device *dev);
+static s32 wl_cfg80211_get_station(struct wiphy *wiphy,
+	struct net_device *dev, u8 *mac,
+	struct station_info *sinfo);
+static s32 wl_cfg80211_set_power_mgmt(struct wiphy *wiphy,
+	struct net_device *dev, bool enabled,
+	s32 timeout);
+static int wl_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
+	struct cfg80211_connect_params *sme);
+static s32 wl_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *dev,
+	u16 reason_code);
+static s32 wl_cfg80211_set_tx_power(struct wiphy *wiphy,
+	enum nl80211_tx_power_setting type,
+	s32 dbm);
+static s32 wl_cfg80211_get_tx_power(struct wiphy *wiphy, s32 *dbm);
+static s32 wl_cfg80211_config_default_key(struct wiphy *wiphy,
+	struct net_device *dev,
+	u8 key_idx, bool unicast, bool multicast);
+static s32 wl_cfg80211_add_key(struct wiphy *wiphy, struct net_device *dev,
+	u8 key_idx, bool pairwise, const u8 *mac_addr,
+	struct key_params *params);
+static s32 wl_cfg80211_del_key(struct wiphy *wiphy, struct net_device *dev,
+	u8 key_idx, bool pairwise, const u8 *mac_addr);
+static s32 wl_cfg80211_get_key(struct wiphy *wiphy, struct net_device *dev,
+	u8 key_idx, bool pairwise, const u8 *mac_addr,
+	void *cookie, void (*callback) (void *cookie,
+	struct key_params *params));
+static s32 wl_cfg80211_config_default_mgmt_key(struct wiphy *wiphy,
+	struct net_device *dev,	u8 key_idx);
+static s32 wl_cfg80211_resume(struct wiphy *wiphy);
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 39)
+static s32 wl_cfg80211_suspend(struct wiphy *wiphy, struct cfg80211_wowlan *wow);
+#else
+static s32 wl_cfg80211_suspend(struct wiphy *wiphy);
+#endif
+static s32 wl_cfg80211_set_pmksa(struct wiphy *wiphy, struct net_device *dev,
+	struct cfg80211_pmksa *pmksa);
+static s32 wl_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *dev,
+	struct cfg80211_pmksa *pmksa);
+static s32 wl_cfg80211_flush_pmksa(struct wiphy *wiphy,
+	struct net_device *dev);
+static s32 wl_notify_escan_complete(struct wl_priv *wl,
+	struct net_device *ndev, bool aborted, bool fw_abort);
+/*
+ * event & event Q handlers for cfg80211 interfaces
+ */
+static s32 wl_create_event_handler(struct wl_priv *wl);
+static void wl_destroy_event_handler(struct wl_priv *wl);
+static s32 wl_event_handler(void *data);
+static void wl_init_eq(struct wl_priv *wl);
+static void wl_flush_eq(struct wl_priv *wl);
+static unsigned long wl_lock_eq(struct wl_priv *wl);
+static void wl_unlock_eq(struct wl_priv *wl, unsigned long flags);
+static void wl_init_eq_lock(struct wl_priv *wl);
+static void wl_init_event_handler(struct wl_priv *wl);
+static struct wl_event_q *wl_deq_event(struct wl_priv *wl);
+static s32 wl_enq_event(struct wl_priv *wl, struct net_device *ndev, u32 type,
+	const wl_event_msg_t *msg, void *data);
+static void wl_put_event(struct wl_event_q *e);
+static void wl_wakeup_event(struct wl_priv *wl);
+static s32 wl_notify_connect_status_ap(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data);
+static s32 wl_notify_connect_status(struct wl_priv *wl,
+	struct net_device *ndev,
+	const wl_event_msg_t *e, void *data);
+static s32 wl_notify_roaming_status(struct wl_priv *wl,
+	struct net_device *ndev,
+	const wl_event_msg_t *e, void *data);
+static s32 wl_notify_scan_status(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data);
+static s32 wl_bss_connect_done(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data, bool completed);
+static s32 wl_ibss_join_done(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data, bool completed);
+static s32 wl_bss_roaming_done(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data);
+static s32 wl_notify_mic_status(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data);
+#ifdef WL_SCHED_SCAN
+static s32
+wl_notify_sched_scan_results(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data);
+#endif /* WL_SCHED_SCAN */
+#ifdef PNO_SUPPORT
+static s32 wl_notify_pfn_status(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data);
+#endif /* PNO_SUPPORT */
+/*
+ * register/deregister parent device
+ */
+static void wl_cfg80211_clear_parent_dev(void);
+
+/*
+ * cfg80211 set_wiphy_params utilities
+ */
+static s32 wl_set_frag(struct net_device *dev, u32 frag_threshold);
+static s32 wl_set_rts(struct net_device *dev, u32 frag_threshold);
+static s32 wl_set_retry(struct net_device *dev, u32 retry, bool l);
+
+/*
+ * wl profile utilities
+ */
+static s32 wl_update_prof(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data, s32 item);
+static void *wl_read_prof(struct wl_priv *wl, struct net_device *ndev, s32 item);
+static void wl_init_prof(struct wl_priv *wl, struct net_device *ndev);
+
+/*
+ * cfg80211 connect utilites
+ */
+static s32 wl_set_wpa_version(struct net_device *dev,
+	struct cfg80211_connect_params *sme);
+static s32 wl_set_auth_type(struct net_device *dev,
+	struct cfg80211_connect_params *sme);
+static s32 wl_set_set_cipher(struct net_device *dev,
+	struct cfg80211_connect_params *sme);
+static s32 wl_set_key_mgmt(struct net_device *dev,
+	struct cfg80211_connect_params *sme);
+static s32 wl_set_set_sharedkey(struct net_device *dev,
+	struct cfg80211_connect_params *sme);
+static s32 wl_get_assoc_ies(struct wl_priv *wl, struct net_device *ndev);
+static void wl_ch_to_chanspec(int ch,
+	struct wl_join_params *join_params, size_t *join_params_size);
+
+/*
+ * information element utilities
+ */
+static void wl_rst_ie(struct wl_priv *wl);
+static __used s32 wl_add_ie(struct wl_priv *wl, u8 t, u8 l, u8 *v);
+static s32 wl_mrg_ie(struct wl_priv *wl, u8 *ie_stream, u16 ie_size);
+static s32 wl_cp_ie(struct wl_priv *wl, u8 *dst, u16 dst_size);
+static u32 wl_get_ielen(struct wl_priv *wl);
+
+
+static s32 wl_setup_wiphy(struct wireless_dev *wdev, struct device *dev);
+static void wl_free_wdev(struct wl_priv *wl);
+
+static s32 wl_inform_bss(struct wl_priv *wl);
+static s32 wl_inform_single_bss(struct wl_priv *wl, struct wl_bss_info *bi);
+static s32 wl_inform_ibss(struct wl_priv *wl, const u8 *bssid);
+static s32 wl_update_bss_info(struct wl_priv *wl, struct net_device *ndev);
+static chanspec_t wl_cfg80211_get_shared_freq(struct wiphy *wiphy);
+
+static s32 wl_add_keyext(struct wiphy *wiphy, struct net_device *dev,
+	u8 key_idx, const u8 *mac_addr,
+	struct key_params *params);
+/*
+ * key indianess swap utilities
+ */
+static void swap_key_from_BE(struct wl_wsec_key *key);
+static void swap_key_to_BE(struct wl_wsec_key *key);
+
+/*
+ * wl_priv memory init/deinit utilities
+ */
+static s32 wl_init_priv_mem(struct wl_priv *wl);
+static void wl_deinit_priv_mem(struct wl_priv *wl);
+
+static void wl_delay(u32 ms);
+
+/*
+ * ibss mode utilities
+ */
+static bool wl_is_ibssmode(struct wl_priv *wl, struct net_device *ndev);
+static __used bool wl_is_ibssstarter(struct wl_priv *wl);
+
+/*
+ * link up/down , default configuration utilities
+ */
+static s32 __wl_cfg80211_up(struct wl_priv *wl);
+static s32 __wl_cfg80211_down(struct wl_priv *wl);
+static s32 wl_add_remove_eventmsg(struct net_device *ndev, u16 event, bool add);
+static bool wl_is_linkdown(struct wl_priv *wl, const wl_event_msg_t *e);
+static bool wl_is_linkup(struct wl_priv *wl, const wl_event_msg_t *e, struct net_device *ndev);
+static bool wl_is_nonetwork(struct wl_priv *wl, const wl_event_msg_t *e);
+static void wl_link_up(struct wl_priv *wl);
+static void wl_link_down(struct wl_priv *wl);
+static s32 wl_config_ifmode(struct wl_priv *wl, struct net_device *ndev, s32 iftype);
+static void wl_init_conf(struct wl_conf *conf);
+
+/*
+ * iscan handler
+ */
+static void wl_iscan_timer(unsigned long data);
+static void wl_term_iscan(struct wl_priv *wl);
+static s32 wl_init_scan(struct wl_priv *wl);
+static s32 wl_iscan_thread(void *data);
+static s32 wl_run_iscan(struct wl_iscan_ctrl *iscan, struct cfg80211_scan_request *request,
+	u16 action);
+static s32 wl_do_iscan(struct wl_priv *wl,  struct cfg80211_scan_request *request);
+static s32 wl_wakeup_iscan(struct wl_iscan_ctrl *iscan);
+static s32 wl_invoke_iscan(struct wl_priv *wl);
+static s32 wl_get_iscan_results(struct wl_iscan_ctrl *iscan, u32 *status,
+	struct wl_scan_results **bss_list);
+static void wl_notify_iscan_complete(struct wl_iscan_ctrl *iscan, bool aborted);
+static void wl_init_iscan_handler(struct wl_iscan_ctrl *iscan);
+static s32 wl_iscan_done(struct wl_priv *wl);
+static s32 wl_iscan_pending(struct wl_priv *wl);
+static s32 wl_iscan_inprogress(struct wl_priv *wl);
+static s32 wl_iscan_aborted(struct wl_priv *wl);
+
+/*
+ * find most significant bit set
+ */
+static __used u32 wl_find_msb(u16 bit16);
+
+/*
+ * rfkill support
+ */
+static int wl_setup_rfkill(struct wl_priv *wl, bool setup);
+static int wl_rfkill_set(void *data, bool blocked);
+
+static wl_scan_params_t *wl_cfg80211_scan_alloc_params(int channel,
+	int nprobes, int *out_params_size);
+static void get_primary_mac(struct wl_priv *wl, struct ether_addr *mac);
+
+/*
+ * Some external functions, TODO: move them to dhd_linux.h
+ */
+int dhd_add_monitor(char *name, struct net_device **new_ndev);
+int dhd_del_monitor(struct net_device *ndev);
+int dhd_monitor_init(void *dhd_pub);
+int dhd_monitor_uninit(void);
+int dhd_start_xmit(struct sk_buff *skb, struct net_device *net);
+
+#define CHECK_SYS_UP(wlpriv)							\
+do {									\
+	struct net_device *ndev = wl_to_prmry_ndev(wlpriv);       \
+	if (unlikely(!wl_get_drv_status(wlpriv, READY, ndev))) {	\
+		WL_INFO(("device is not ready\n"));		\
+			return -EIO;					\
+	}								\
+} while (0)
+
+
+#define IS_WPA_AKM(akm) ((akm) == RSN_AKM_NONE || \
+				 (akm) == RSN_AKM_UNSPECIFIED || \
+				 (akm) == RSN_AKM_PSK)
+
+
+extern int dhd_wait_pend8021x(struct net_device *dev);
+
+#if (WL_DBG_LEVEL > 0)
+#define WL_DBG_ESTR_MAX	50
+static s8 wl_dbg_estr[][WL_DBG_ESTR_MAX] = {
+	"SET_SSID", "JOIN", "START", "AUTH", "AUTH_IND",
+	"DEAUTH", "DEAUTH_IND", "ASSOC", "ASSOC_IND", "REASSOC",
+	"REASSOC_IND", "DISASSOC", "DISASSOC_IND", "QUIET_START", "QUIET_END",
+	"BEACON_RX", "LINK", "MIC_ERROR", "NDIS_LINK", "ROAM",
+	"TXFAIL", "PMKID_CACHE", "RETROGRADE_TSF", "PRUNE", "AUTOAUTH",
+	"EAPOL_MSG", "SCAN_COMPLETE", "ADDTS_IND", "DELTS_IND", "BCNSENT_IND",
+	"BCNRX_MSG", "BCNLOST_MSG", "ROAM_PREP", "PFN_NET_FOUND",
+	"PFN_NET_LOST",
+	"RESET_COMPLETE", "JOIN_START", "ROAM_START", "ASSOC_START",
+	"IBSS_ASSOC",
+	"RADIO", "PSM_WATCHDOG", "WLC_E_CCX_ASSOC_START", "WLC_E_CCX_ASSOC_ABORT",
+	"PROBREQ_MSG",
+	"SCAN_CONFIRM_IND", "PSK_SUP", "COUNTRY_CODE_CHANGED",
+	"EXCEEDED_MEDIUM_TIME", "ICV_ERROR",
+	"UNICAST_DECODE_ERROR", "MULTICAST_DECODE_ERROR", "TRACE",
+	"WLC_E_BTA_HCI_EVENT", "IF", "WLC_E_P2P_DISC_LISTEN_COMPLETE",
+	"RSSI", "PFN_SCAN_COMPLETE", "WLC_E_EXTLOG_MSG",
+	"ACTION_FRAME", "ACTION_FRAME_COMPLETE", "WLC_E_PRE_ASSOC_IND",
+	"WLC_E_PRE_REASSOC_IND", "WLC_E_CHANNEL_ADOPTED", "WLC_E_AP_STARTED",
+	"WLC_E_DFS_AP_STOP", "WLC_E_DFS_AP_RESUME", "WLC_E_WAI_STA_EVENT",
+	"WLC_E_WAI_MSG", "WLC_E_ESCAN_RESULT", "WLC_E_ACTION_FRAME_OFF_CHAN_COMPLETE",
+	"WLC_E_PROBRESP_MSG", "WLC_E_P2P_PROBREQ_MSG", "WLC_E_DCS_REQUEST", "WLC_E_FIFO_CREDIT_MAP",
+	"WLC_E_ACTION_FRAME_RX", "WLC_E_WAKE_EVENT", "WLC_E_RM_COMPLETE"
+};
+#endif				/* WL_DBG_LEVEL */
+
+#define CHAN2G(_channel, _freq, _flags) {			\
+	.band			= IEEE80211_BAND_2GHZ,		\
+	.center_freq		= (_freq),			\
+	.hw_value		= (_channel),			\
+	.flags			= (_flags),			\
+	.max_antenna_gain	= 0,				\
+	.max_power		= 30,				\
+}
+
+#define CHAN5G(_channel, _flags) {				\
+	.band			= IEEE80211_BAND_5GHZ,		\
+	.center_freq		= 5000 + (5 * (_channel)),	\
+	.hw_value		= (_channel),			\
+	.flags			= (_flags),			\
+	.max_antenna_gain	= 0,				\
+	.max_power		= 30,				\
+}
+
+#define RATE_TO_BASE100KBPS(rate)   (((rate) * 10) / 2)
+#define RATETAB_ENT(_rateid, _flags) \
+	{								\
+		.bitrate	= RATE_TO_BASE100KBPS(_rateid),     \
+		.hw_value	= (_rateid),			    \
+		.flags	  = (_flags),			     \
+	}
+
+static struct ieee80211_rate __wl_rates[] = {
+	RATETAB_ENT(WLC_RATE_1M, 0),
+	RATETAB_ENT(WLC_RATE_2M, IEEE80211_RATE_SHORT_PREAMBLE),
+	RATETAB_ENT(WLC_RATE_5M5, IEEE80211_RATE_SHORT_PREAMBLE),
+	RATETAB_ENT(WLC_RATE_11M, IEEE80211_RATE_SHORT_PREAMBLE),
+	RATETAB_ENT(WLC_RATE_6M, 0),
+	RATETAB_ENT(WLC_RATE_9M, 0),
+	RATETAB_ENT(WLC_RATE_12M, 0),
+	RATETAB_ENT(WLC_RATE_18M, 0),
+	RATETAB_ENT(WLC_RATE_24M, 0),
+	RATETAB_ENT(WLC_RATE_36M, 0),
+	RATETAB_ENT(WLC_RATE_48M, 0),
+	RATETAB_ENT(WLC_RATE_54M, 0)
+};
+
+#define wl_a_rates		(__wl_rates + 4)
+#define wl_a_rates_size	8
+#define wl_g_rates		(__wl_rates + 0)
+#define wl_g_rates_size	12
+
+static struct ieee80211_channel __wl_2ghz_channels[] = {
+	CHAN2G(1, 2412, 0),
+	CHAN2G(2, 2417, 0),
+	CHAN2G(3, 2422, 0),
+	CHAN2G(4, 2427, 0),
+	CHAN2G(5, 2432, 0),
+	CHAN2G(6, 2437, 0),
+	CHAN2G(7, 2442, 0),
+	CHAN2G(8, 2447, 0),
+	CHAN2G(9, 2452, 0),
+	CHAN2G(10, 2457, 0),
+	CHAN2G(11, 2462, 0),
+	CHAN2G(12, 2467, 0),
+	CHAN2G(13, 2472, 0),
+	CHAN2G(14, 2484, 0)
+};
+
+static struct ieee80211_channel __wl_5ghz_a_channels[] = {
+	CHAN5G(34, 0), CHAN5G(36, 0),
+	CHAN5G(38, 0), CHAN5G(40, 0),
+	CHAN5G(42, 0), CHAN5G(44, 0),
+	CHAN5G(46, 0), CHAN5G(48, 0),
+	CHAN5G(52, 0), CHAN5G(56, 0),
+	CHAN5G(60, 0), CHAN5G(64, 0),
+	CHAN5G(100, 0), CHAN5G(104, 0),
+	CHAN5G(108, 0), CHAN5G(112, 0),
+	CHAN5G(116, 0), CHAN5G(120, 0),
+	CHAN5G(124, 0), CHAN5G(128, 0),
+	CHAN5G(132, 0), CHAN5G(136, 0),
+	CHAN5G(140, 0), CHAN5G(149, 0),
+	CHAN5G(153, 0), CHAN5G(157, 0),
+	CHAN5G(161, 0), CHAN5G(165, 0)
+};
+
+static struct ieee80211_supported_band __wl_band_2ghz = {
+	.band = IEEE80211_BAND_2GHZ,
+	.channels = __wl_2ghz_channels,
+	.n_channels = ARRAY_SIZE(__wl_2ghz_channels),
+	.bitrates = wl_g_rates,
+	.n_bitrates = wl_g_rates_size
+};
+
+static struct ieee80211_supported_band __wl_band_5ghz_a = {
+	.band = IEEE80211_BAND_5GHZ,
+	.channels = __wl_5ghz_a_channels,
+	.n_channels = ARRAY_SIZE(__wl_5ghz_a_channels),
+	.bitrates = wl_a_rates,
+	.n_bitrates = wl_a_rates_size
+};
+
+static const u32 __wl_cipher_suites[] = {
+	WLAN_CIPHER_SUITE_WEP40,
+	WLAN_CIPHER_SUITE_WEP104,
+	WLAN_CIPHER_SUITE_TKIP,
+	WLAN_CIPHER_SUITE_CCMP,
+	WLAN_CIPHER_SUITE_AES_CMAC,
+};
+
+/* There isn't a lot of sense in it, but you can transmit anything you like */
+static const struct ieee80211_txrx_stypes
+wl_cfg80211_default_mgmt_stypes[NUM_NL80211_IFTYPES] = {
+	[NL80211_IFTYPE_ADHOC] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ACTION >> 4)
+	},
+	[NL80211_IFTYPE_STATION] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+		BIT(IEEE80211_STYPE_PROBE_REQ >> 4)
+	},
+	[NL80211_IFTYPE_AP] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
+		BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
+		BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
+		BIT(IEEE80211_STYPE_DISASSOC >> 4) |
+		BIT(IEEE80211_STYPE_AUTH >> 4) |
+		BIT(IEEE80211_STYPE_DEAUTH >> 4) |
+		BIT(IEEE80211_STYPE_ACTION >> 4)
+	},
+	[NL80211_IFTYPE_AP_VLAN] = {
+		/* copy AP */
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
+		BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
+		BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
+		BIT(IEEE80211_STYPE_DISASSOC >> 4) |
+		BIT(IEEE80211_STYPE_AUTH >> 4) |
+		BIT(IEEE80211_STYPE_DEAUTH >> 4) |
+		BIT(IEEE80211_STYPE_ACTION >> 4)
+	},
+	[NL80211_IFTYPE_P2P_CLIENT] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+		BIT(IEEE80211_STYPE_PROBE_REQ >> 4)
+	},
+	[NL80211_IFTYPE_P2P_GO] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
+		BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
+		BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
+		BIT(IEEE80211_STYPE_DISASSOC >> 4) |
+		BIT(IEEE80211_STYPE_AUTH >> 4) |
+		BIT(IEEE80211_STYPE_DEAUTH >> 4) |
+		BIT(IEEE80211_STYPE_ACTION >> 4)
+	}
+};
+
+static void swap_key_from_BE(struct wl_wsec_key *key)
+{
+	key->index = htod32(key->index);
+	key->len = htod32(key->len);
+	key->algo = htod32(key->algo);
+	key->flags = htod32(key->flags);
+	key->rxiv.hi = htod32(key->rxiv.hi);
+	key->rxiv.lo = htod16(key->rxiv.lo);
+	key->iv_initialized = htod32(key->iv_initialized);
+}
+
+static void swap_key_to_BE(struct wl_wsec_key *key)
+{
+	key->index = dtoh32(key->index);
+	key->len = dtoh32(key->len);
+	key->algo = dtoh32(key->algo);
+	key->flags = dtoh32(key->flags);
+	key->rxiv.hi = dtoh32(key->rxiv.hi);
+	key->rxiv.lo = dtoh16(key->rxiv.lo);
+	key->iv_initialized = dtoh32(key->iv_initialized);
+}
+
+/* For debug: Dump the contents of the encoded wps ie buffe */
+static void
+wl_validate_wps_ie(char *wps_ie, bool *pbc)
+{
+	#define WPS_IE_FIXED_LEN 6
+	u16 len = (u16) wps_ie[TLV_LEN_OFF];
+	u8 *subel = wps_ie+  WPS_IE_FIXED_LEN;
+	u16 subelt_id;
+	u16 subelt_len;
+	u16 val;
+	u8 *valptr = (uint8*) &val;
+
+	WL_DBG(("wps_ie len=%d\n", len));
+
+	len -= 4;	/* for the WPS IE's OUI, oui_type fields */
+
+	while (len >= 4) {		/* must have attr id, attr len fields */
+		valptr[0] = *subel++;
+		valptr[1] = *subel++;
+		subelt_id = HTON16(val);
+
+		valptr[0] = *subel++;
+		valptr[1] = *subel++;
+		subelt_len = HTON16(val);
+
+		len -= 4;			/* for the attr id, attr len fields */
+		len -= subelt_len;	/* for the remaining fields in this attribute */
+		WL_DBG((" subel=%p, subelt_id=0x%x subelt_len=%u\n",
+			subel, subelt_id, subelt_len));
+
+		if (subelt_id == WPS_ID_VERSION) {
+			WL_DBG(("  attr WPS_ID_VERSION: %u\n", *subel));
+		} else if (subelt_id == WPS_ID_REQ_TYPE) {
+			WL_DBG(("  attr WPS_ID_REQ_TYPE: %u\n", *subel));
+		} else if (subelt_id == WPS_ID_CONFIG_METHODS) {
+			valptr[0] = *subel;
+			valptr[1] = *(subel + 1);
+			WL_DBG(("  attr WPS_ID_CONFIG_METHODS: %x\n", HTON16(val)));
+		} else if (subelt_id == WPS_ID_DEVICE_NAME) {
+			char devname[100];
+			memcpy(devname, subel, subelt_len);
+			devname[subelt_len] = '\0';
+			WL_DBG(("  attr WPS_ID_DEVICE_NAME: %s (len %u)\n",
+				devname, subelt_len));
+		} else if (subelt_id == WPS_ID_DEVICE_PWD_ID) {
+			valptr[0] = *subel;
+			valptr[1] = *(subel + 1);
+			WL_DBG(("  attr WPS_ID_DEVICE_PWD_ID: %u\n", HTON16(val)));
+			*pbc = (HTON16(val) == DEV_PW_PUSHBUTTON) ? true : false;
+		} else if (subelt_id == WPS_ID_PRIM_DEV_TYPE) {
+			valptr[0] = *subel;
+			valptr[1] = *(subel + 1);
+			WL_DBG(("  attr WPS_ID_PRIM_DEV_TYPE: cat=%u \n", HTON16(val)));
+			valptr[0] = *(subel + 6);
+			valptr[1] = *(subel + 7);
+			WL_DBG(("  attr WPS_ID_PRIM_DEV_TYPE: subcat=%u\n", HTON16(val)));
+		} else if (subelt_id == WPS_ID_REQ_DEV_TYPE) {
+			valptr[0] = *subel;
+			valptr[1] = *(subel + 1);
+			WL_DBG(("  attr WPS_ID_REQ_DEV_TYPE: cat=%u\n", HTON16(val)));
+			valptr[0] = *(subel + 6);
+			valptr[1] = *(subel + 7);
+			WL_DBG(("  attr WPS_ID_REQ_DEV_TYPE: subcat=%u\n", HTON16(val)));
+		} else if (subelt_id == WPS_ID_SELECTED_REGISTRAR_CONFIG_METHODS) {
+			valptr[0] = *subel;
+			valptr[1] = *(subel + 1);
+			WL_DBG(("  attr WPS_ID_SELECTED_REGISTRAR_CONFIG_METHODS"
+				": cat=%u\n", HTON16(val)));
+		} else {
+			WL_DBG(("  unknown attr 0x%x\n", subelt_id));
+		}
+
+		subel += subelt_len;
+	}
+}
+
+static chanspec_t wl_cfg80211_get_shared_freq(struct wiphy *wiphy)
+{
+	if (vsdb_supported) {
+		return wf_chspec_aton(WL_P2P_TEMP_CHAN);
+	}
+	else {
+		chanspec_t chspec;
+		int err = 0;
+		struct wl_priv *wl = wiphy_priv(wiphy);
+		struct net_device *dev = wl_to_prmry_ndev(wl);
+		struct ether_addr bssid;
+		struct wl_bss_info *bss = NULL;
+		if ((err = wldev_ioctl(dev, WLC_GET_BSSID, &bssid, sizeof(bssid), false))) {
+			/* STA interface is not associated. So start the new interface on a temp
+			 * channel . Later proper channel will be applied by the above framework
+			 * via set_channel (cfg80211 API).
+			 */
+			WL_DBG(("Not associated. Return a temp channel. \n"));
+			return wf_chspec_aton(WL_P2P_TEMP_CHAN);
+		}
+
+
+		*(u32 *) wl->extra_buf = htod32(WL_EXTRA_BUF_MAX);
+		if ((err = wldev_ioctl(dev, WLC_GET_BSS_INFO, wl->extra_buf,
+			WL_EXTRA_BUF_MAX, false))) {
+				WL_ERR(("Failed to get associated bss info, use temp channel \n"));
+				chspec = wf_chspec_aton(WL_P2P_TEMP_CHAN);
+		}
+		else {
+				bss = (struct wl_bss_info *) (wl->extra_buf + 4);
+				chspec =  bss->chanspec;
+				WL_DBG(("Valid BSS Found. chanspec:%d \n", bss->chanspec));
+		}
+		return chspec;
+	}
+}
+
+static struct net_device* wl_cfg80211_add_monitor_if(char *name)
+{
+	int ret = 0;
+	struct net_device* ndev = NULL;
+
+	ret = dhd_add_monitor(name, &ndev);
+	WL_INFO(("wl_cfg80211_add_monitor_if net device returned: 0x%p\n", ndev));
+	return ndev;
+}
+
+static struct net_device *
+wl_cfg80211_add_virtual_iface(struct wiphy *wiphy, char *name,
+	enum nl80211_iftype type, u32 *flags,
+	struct vif_params *params)
+{
+	s32 err;
+	s32 timeout = -1;
+	s32 wlif_type = -1;
+	s32 mode = 0;
+#if defined(WL_ENABLE_P2P_IF)
+	s32 dhd_mode = 0;
+#endif /* (WL_ENABLE_P2P_IF) */
+	chanspec_t chspec;
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	struct net_device *_ndev;
+	struct ether_addr primary_mac;
+	int (*net_attach)(void *dhdp, int ifidx);
+	bool rollback_lock = false;
+
+	/* Use primary I/F for sending cmds down to firmware */
+	_ndev = wl_to_prmry_ndev(wl);
+
+	WL_DBG(("if name: %s, type: %d\n", name, type));
+	switch (type) {
+	case NL80211_IFTYPE_ADHOC:
+	case NL80211_IFTYPE_AP_VLAN:
+	case NL80211_IFTYPE_WDS:
+	case NL80211_IFTYPE_MESH_POINT:
+		WL_ERR(("Unsupported interface type\n"));
+		mode = WL_MODE_IBSS;
+		return NULL;
+	case NL80211_IFTYPE_MONITOR:
+		return wl_cfg80211_add_monitor_if(name);
+	case NL80211_IFTYPE_P2P_CLIENT:
+	case NL80211_IFTYPE_STATION:
+		wlif_type = WL_P2P_IF_CLIENT;
+		mode = WL_MODE_BSS;
+		break;
+	case NL80211_IFTYPE_P2P_GO:
+	case NL80211_IFTYPE_AP:
+		wlif_type = WL_P2P_IF_GO;
+		mode = WL_MODE_AP;
+		break;
+	default:
+		WL_ERR(("Unsupported interface type\n"));
+		return NULL;
+		break;
+	}
+
+	if (!name) {
+		WL_ERR(("name is NULL\n"));
+		return NULL;
+	}
+	if (wl->iface_cnt == IFACE_MAX_CNT)
+		return ERR_PTR(-ENOMEM);
+	if (wl->p2p_supported && (wlif_type != -1)) {
+		if (wl_get_p2p_status(wl, IF_DELETING)) {
+			/* wait till IF_DEL is complete
+			 * release the lock for the unregister to proceed
+			 */
+			if (rtnl_is_locked()) {
+				rtnl_unlock();
+				rollback_lock = true;
+			}
+			WL_INFO(("%s: Released the lock and wait till IF_DEL is complete\n",
+				__func__));
+			timeout = wait_event_interruptible_timeout(wl->netif_change_event,
+				(wl_get_p2p_status(wl, IF_DELETING) == false),
+				msecs_to_jiffies(MAX_WAIT_TIME));
+
+			/* put back the rtnl_lock again */
+			if (rollback_lock) {
+				rtnl_lock();
+				rollback_lock = false;
+			}
+			if (timeout > 0) {
+				WL_ERR(("IF DEL is Success\n"));
+
+			} else {
+				WL_ERR(("timeount < 0, return -EAGAIN\n"));
+				return ERR_PTR(-EAGAIN);
+			}
+		}
+		if (wl->p2p && !wl->p2p->on && strstr(name, WL_P2P_INTERFACE_PREFIX)) {
+			p2p_on(wl) = true;
+			wl_cfgp2p_set_firm_p2p(wl);
+			wl_cfgp2p_init_discovery(wl);
+			get_primary_mac(wl, &primary_mac);
+			wl_cfgp2p_generate_bss_mac(&primary_mac,
+				&wl->p2p->dev_addr, &wl->p2p->int_addr);
+		}
+
+		memset(wl->p2p->vir_ifname, 0, IFNAMSIZ);
+		strncpy(wl->p2p->vir_ifname, name, IFNAMSIZ - 1);
+
+		wldev_iovar_setint(_ndev, "mpc", 0);
+		wl_notify_escan_complete(wl, _ndev, true, true);
+		/* In concurrency case, STA may be already associated in a particular channel.
+		 * so retrieve the current channel of primary interface and then start the virtual
+		 * interface on that.
+		 */
+		 chspec = wl_cfg80211_get_shared_freq(wiphy);
+
+		/* For P2P mode, use P2P-specific driver features to create the
+		 * bss: "wl p2p_ifadd"
+		 */
+		wl_set_p2p_status(wl, IF_ADD);
+		err = wl_cfgp2p_ifadd(wl, &wl->p2p->int_addr, htod32(wlif_type), chspec);
+
+		if (unlikely(err)) {
+			WL_ERR((" virtual iface add failed (%d) \n", err));
+			return ERR_PTR(-ENOMEM);
+		}
+
+		timeout = wait_event_interruptible_timeout(wl->netif_change_event,
+			(wl_get_p2p_status(wl, IF_ADD) == false),
+			msecs_to_jiffies(MAX_WAIT_TIME));
+		if (timeout > 0 && (!wl_get_p2p_status(wl, IF_ADD))) {
+
+			struct wireless_dev *vwdev;
+			vwdev = kzalloc(sizeof(*vwdev), GFP_KERNEL);
+			if (unlikely(!vwdev)) {
+				WL_ERR(("Could not allocate wireless device\n"));
+				return ERR_PTR(-ENOMEM);
+			}
+			vwdev->wiphy = wl->wdev->wiphy;
+			WL_INFO((" virtual interface(%s) is created memalloc done \n",
+				wl->p2p->vir_ifname));
+			vwdev->iftype = type;
+			_ndev =  wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION);
+			_ndev->ieee80211_ptr = vwdev;
+			SET_NETDEV_DEV(_ndev, wiphy_dev(vwdev->wiphy));
+			vwdev->netdev = _ndev;
+			wl_set_drv_status(wl, READY, _ndev);
+			wl->p2p->vif_created = true;
+			wl_set_mode_by_netdev(wl, _ndev, mode);
+			net_attach =  wl_to_p2p_bss_private(wl, P2PAPI_BSSCFG_CONNECTION);
+			if (rtnl_is_locked()) {
+				rtnl_unlock();
+				rollback_lock = true;
+			}
+			if (net_attach && !net_attach(wl->pub, _ndev->ifindex)) {
+				wl_alloc_netinfo(wl, _ndev, vwdev, mode);
+				WL_ERR((" virtual interface(%s) is "
+					"created net attach done\n", wl->p2p->vir_ifname));
+#if defined(WL_ENABLE_P2P_IF)
+				if (type == NL80211_IFTYPE_P2P_CLIENT)
+					dhd_mode = P2P_GC_ENABLED;
+				else if (type == NL80211_IFTYPE_P2P_GO)
+					dhd_mode = P2P_GO_ENABLED;
+				DNGL_FUNC(dhd_cfg80211_set_p2p_info, (wl, dhd_mode));
+#endif /* (WL_ENABLE_P2P_IF) */
+				/* Start the P2P I/F with PM disabled. Enable PM from
+				 * the framework
+				 */
+				 if ((type == NL80211_IFTYPE_P2P_CLIENT) || (
+					 type == NL80211_IFTYPE_P2P_GO))
+					vwdev->ps = NL80211_PS_DISABLED;
+			} else {
+				/* put back the rtnl_lock again */
+				if (rollback_lock)
+					rtnl_lock();
+				goto fail;
+			}
+			/* put back the rtnl_lock again */
+			if (rollback_lock)
+				rtnl_lock();
+			return _ndev;
+
+		} else {
+			wl_clr_p2p_status(wl, IF_ADD);
+			WL_ERR((" virtual interface(%s) is not created \n", wl->p2p->vir_ifname));
+			memset(wl->p2p->vir_ifname, '\0', IFNAMSIZ);
+			wl->p2p->vif_created = false;
+		}
+	}
+fail:
+	return ERR_PTR(-ENODEV);
+}
+
+static s32
+wl_cfg80211_del_virtual_iface(struct wiphy *wiphy, struct net_device *dev)
+{
+	struct ether_addr p2p_mac;
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	s32 timeout = -1;
+	s32 ret = 0;
+	WL_DBG(("Enter\n"));
+
+	if (wl->p2p_net == dev) {
+		/* Since there is no ifidx corresponding to p2p0, cmds to
+		 * firmware should be routed through primary I/F
+		 */
+		dev = wl_to_prmry_ndev(wl);
+	}
+
+	if (wl->p2p_supported) {
+		memcpy(p2p_mac.octet, wl->p2p->int_addr.octet, ETHER_ADDR_LEN);
+
+		/* Clear GO_NEG_PHASE bit to take care of GO-NEG-FAIL cases
+		 */
+		WL_DBG(("P2P: GO_NEG_PHASE status cleared "));
+		wl_clr_p2p_status(wl, GO_NEG_PHASE);
+		if (wl->p2p->vif_created) {
+			if (wl_get_drv_status(wl, SCANNING, dev)) {
+				wl_notify_escan_complete(wl, dev, true, true);
+			}
+			wldev_iovar_setint(dev, "mpc", 1);
+			wl_set_p2p_status(wl, IF_DELETING);
+			ret = wl_cfgp2p_ifdel(wl, &p2p_mac);
+			/* Firmware could not delete the interface so we will not get WLC_E_IF
+			* event for cleaning the dhd virtual nw interace
+			* So lets do it here. Failures from fw will ensure the application to do
+			* ifconfig <inter> down and up sequnce, which will reload the fw
+			* however we should cleanup the linux network virtual interfaces
+			*/
+			/* Request framework to RESET and clean up */
+			if (ret) {
+				struct net_device *ndev = wl_to_prmry_ndev(wl);
+				WL_ERR(("Firmware returned an error (%d) from p2p_ifdel"
+					"HANG Notification sent to %s\n", ret, ndev->name));
+				wl_cfg80211_hang(ndev, WLAN_REASON_UNSPECIFIED);
+			}
+
+			/* Wait for any pending scan req to get aborted from the sysioc context */
+			timeout = wait_event_interruptible_timeout(wl->netif_change_event,
+				(wl->p2p->vif_created == false),
+				msecs_to_jiffies(MAX_WAIT_TIME));
+			if (timeout > 0 && (wl->p2p->vif_created == false)) {
+				WL_DBG(("IFDEL operation done\n"));
+#if  defined(WL_ENABLE_P2P_IF)
+				DNGL_FUNC(dhd_cfg80211_clean_p2p_info, (wl));
+#endif /*  (WL_ENABLE_P2P_IF)) */
+			} else {
+				WL_ERR(("IFDEL didn't complete properly\n"));
+			}
+			ret = dhd_del_monitor(dev);
+		}
+	}
+	return ret;
+}
+
+static s32
+wl_cfg80211_change_virtual_iface(struct wiphy *wiphy, struct net_device *ndev,
+	enum nl80211_iftype type, u32 *flags,
+	struct vif_params *params)
+{
+	s32 ap = 0;
+	s32 infra = 0;
+	s32 err = BCME_OK;
+	s32 timeout = -1;
+	s32 wlif_type;
+	s32 mode = 0;
+	chanspec_t chspec;
+	struct wl_priv *wl = wiphy_priv(wiphy);
+
+	WL_DBG(("Enter type %d\n", type));
+	switch (type) {
+	case NL80211_IFTYPE_MONITOR:
+	case NL80211_IFTYPE_WDS:
+	case NL80211_IFTYPE_MESH_POINT:
+		ap = 1;
+		WL_ERR(("type (%d) : currently we do not support this type\n",
+			type));
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		mode = WL_MODE_IBSS;
+		break;
+	case NL80211_IFTYPE_STATION:
+	case NL80211_IFTYPE_P2P_CLIENT:
+		mode = WL_MODE_BSS;
+		infra = 1;
+		break;
+	case NL80211_IFTYPE_AP:
+	case NL80211_IFTYPE_AP_VLAN:
+	case NL80211_IFTYPE_P2P_GO:
+		mode = WL_MODE_AP;
+		ap = 1;
+		break;
+	default:
+		return -EINVAL;
+	}
+	WL_DBG(("%s : ap (%d), infra (%d), iftype: (%d)\n", ndev->name, ap, infra, type));
+
+	if (ap) {
+		wl_set_mode_by_netdev(wl, ndev, mode);
+		if (wl->p2p_supported && wl->p2p->vif_created) {
+			WL_DBG(("p2p_vif_created (%d) p2p_on (%d)\n", wl->p2p->vif_created,
+				p2p_on(wl)));
+			wldev_iovar_setint(ndev, "mpc", 0);
+			wl_notify_escan_complete(wl, ndev, true, true);
+
+			/* In concurrency case, STA may be already associated in a particular
+			 * channel. so retrieve the current channel of primary interface and
+			 * then start the virtual interface on that.
+			 */
+			chspec = wl_cfg80211_get_shared_freq(wiphy);
+
+			wlif_type = WL_P2P_IF_GO;
+			WL_ERR(("%s : ap (%d), infra (%d), iftype: (%d)\n",
+				ndev->name, ap, infra, type));
+			wl_set_p2p_status(wl, IF_CHANGING);
+			wl_clr_p2p_status(wl, IF_CHANGED);
+			err = wl_cfgp2p_ifchange(wl, &wl->p2p->int_addr, htod32(wlif_type), chspec);
+			timeout = wait_event_interruptible_timeout(wl->netif_change_event,
+				(wl_get_p2p_status(wl, IF_CHANGED) == true),
+				msecs_to_jiffies(MAX_WAIT_TIME));
+			wl_set_mode_by_netdev(wl, ndev, mode);
+			wl_clr_p2p_status(wl, IF_CHANGING);
+			wl_clr_p2p_status(wl, IF_CHANGED);
+		} else if (ndev == wl_to_prmry_ndev(wl) &&
+			!wl_get_drv_status(wl, AP_CREATED, ndev)) {
+			wl_set_drv_status(wl, AP_CREATING, ndev);
+			if (!wl->ap_info &&
+				!(wl->ap_info = kzalloc(sizeof(struct ap_info), GFP_KERNEL))) {
+				WL_ERR(("struct ap_saved_ie allocation failed\n"));
+				return -ENOMEM;
+			}
+		} else {
+			WL_ERR(("Cannot change the interface for GO or SOFTAP\n"));
+			return -EINVAL;
+		}
+	} else {
+		infra = htod32(infra);
+		err = wldev_ioctl(ndev, WLC_SET_INFRA, &infra, sizeof(s32), true);
+		if (err) {
+			WL_ERR(("WLC_SET_INFRA error (%d)\n", err));
+			return -EAGAIN;
+		}
+		wl_set_mode_by_netdev(wl, ndev, mode);
+	}
+
+	ndev->ieee80211_ptr->iftype = type;
+	return 0;
+}
+
+s32
+wl_cfg80211_notify_ifadd(struct net_device *ndev, s32 idx, s32 bssidx,
+	void* _net_attach)
+{
+	struct wl_priv *wl = wlcfg_drv_priv;
+	s32 ret = BCME_OK;
+	WL_DBG(("Enter"));
+	if (!ndev) {
+		WL_ERR(("net is NULL\n"));
+		return 0;
+	}
+	if (wl->p2p_supported && wl_get_p2p_status(wl, IF_ADD)) {
+		WL_DBG(("IF_ADD event called from dongle, old interface name: %s,"
+			"new name: %s\n", ndev->name, wl->p2p->vir_ifname));
+		/* Assign the net device to CONNECT BSSCFG */
+		strncpy(ndev->name, wl->p2p->vir_ifname, IFNAMSIZ - 1);
+		wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION) = ndev;
+		wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_CONNECTION) = bssidx;
+		wl_to_p2p_bss_private(wl, P2PAPI_BSSCFG_CONNECTION) = _net_attach;
+		ndev->ifindex = idx;
+		wl_clr_p2p_status(wl, IF_ADD);
+
+		wake_up_interruptible(&wl->netif_change_event);
+	} else {
+		ret = BCME_NOTREADY;
+	}
+	return ret;
+}
+
+s32
+wl_cfg80211_notify_ifdel(void)
+{
+	struct wl_priv *wl = wlcfg_drv_priv;
+
+	WL_DBG(("Enter \n"));
+	wl_clr_p2p_status(wl, IF_DELETING);
+
+	return 0;
+}
+
+s32
+wl_cfg80211_ifdel_ops(struct net_device *ndev)
+{
+	struct wl_priv *wl = wlcfg_drv_priv;
+	bool rollback_lock = false;
+	s32 index = 0;
+
+	if (!ndev || !ndev->name) {
+		WL_ERR(("net is NULL\n"));
+		return 0;
+	}
+
+	if (p2p_is_on(wl) && wl->p2p->vif_created &&
+		wl_get_p2p_status(wl, IF_DELETING)) {
+		if (wl->scan_request &&
+			(wl->escan_info.ndev == ndev)) {
+			/* Abort any pending scan requests */
+			wl->escan_info.escan_state = WL_ESCAN_STATE_IDLE;
+			if (!rtnl_is_locked()) {
+				rtnl_lock();
+				rollback_lock = true;
+			}
+			WL_DBG(("ESCAN COMPLETED\n"));
+			wl_notify_escan_complete(wl, ndev, true, false);
+			if (rollback_lock)
+				rtnl_unlock();
+		}
+		WL_ERR(("IF_DEL event called from dongle, net %x, vif name: %s\n",
+			(unsigned int)ndev, wl->p2p->vir_ifname));
+
+		memset(wl->p2p->vir_ifname, '\0', IFNAMSIZ);
+		index = wl_cfgp2p_find_idx(wl, ndev);
+		wl_to_p2p_bss_ndev(wl, index) = NULL;
+		wl_to_p2p_bss_bssidx(wl, index) = 0;
+		wl->p2p->vif_created = false;
+		wl_cfgp2p_clear_management_ie(wl,
+			index);
+		WL_DBG(("index : %d\n", index));
+
+	}
+
+	/* Wake up any waiting thread */
+	wake_up_interruptible(&wl->netif_change_event);
+
+	return 0;
+}
+
+s32
+wl_cfg80211_is_progress_ifadd(void)
+{
+	s32 is_progress = 0;
+	struct wl_priv *wl = wlcfg_drv_priv;
+	if (wl_get_p2p_status(wl, IF_ADD))
+		is_progress = 1;
+	return is_progress;
+}
+
+s32
+wl_cfg80211_is_progress_ifchange(void)
+{
+	s32 is_progress = 0;
+	struct wl_priv *wl = wlcfg_drv_priv;
+	if (wl_get_p2p_status(wl, IF_CHANGING))
+		is_progress = 1;
+	return is_progress;
+}
+
+
+s32
+wl_cfg80211_notify_ifchange(void)
+{
+	struct wl_priv *wl = wlcfg_drv_priv;
+	if (wl_get_p2p_status(wl, IF_CHANGING)) {
+		wl_set_p2p_status(wl, IF_CHANGED);
+		wake_up_interruptible(&wl->netif_change_event);
+	}
+	return 0;
+}
+
+static void wl_scan_prep(struct wl_scan_params *params, struct cfg80211_scan_request *request)
+{
+	u32 n_ssids;
+	u32 n_channels;
+	u16 channel;
+	chanspec_t chanspec;
+	s32 i, offset;
+	char *ptr;
+	wlc_ssid_t ssid;
+
+	memcpy(&params->bssid, &ether_bcast, ETHER_ADDR_LEN);
+	params->bss_type = DOT11_BSSTYPE_ANY;
+	params->scan_type = 0;
+	params->nprobes = -1;
+	params->active_time = -1;
+	params->passive_time = -1;
+	params->home_time = -1;
+	params->channel_num = 0;
+	memset(&params->ssid, 0, sizeof(wlc_ssid_t));
+
+	WL_SCAN(("Preparing Scan request\n"));
+	WL_SCAN(("nprobes=%d\n", params->nprobes));
+	WL_SCAN(("active_time=%d\n", params->active_time));
+	WL_SCAN(("passive_time=%d\n", params->passive_time));
+	WL_SCAN(("home_time=%d\n", params->home_time));
+	WL_SCAN(("scan_type=%d\n", params->scan_type));
+
+	params->nprobes = htod32(params->nprobes);
+	params->active_time = htod32(params->active_time);
+	params->passive_time = htod32(params->passive_time);
+	params->home_time = htod32(params->home_time);
+
+	/* if request is null just exit so it will be all channel broadcast scan */
+	if (!request)
+		return;
+
+	n_ssids = request->n_ssids;
+	n_channels = request->n_channels;
+
+	/* Copy channel array if applicable */
+	WL_SCAN(("### List of channelspecs to scan ###\n"));
+	if (n_channels > 0) {
+		for (i = 0; i < n_channels; i++) {
+			chanspec = 0;
+			channel = ieee80211_frequency_to_channel(request->channels[i]->center_freq);
+			if (request->channels[i]->band == IEEE80211_BAND_2GHZ)
+				chanspec |= WL_CHANSPEC_BAND_2G;
+			else
+				chanspec |= WL_CHANSPEC_BAND_5G;
+
+			if (request->channels[i]->flags & IEEE80211_CHAN_NO_HT40) {
+				chanspec |= WL_CHANSPEC_BW_20;
+				chanspec |= WL_CHANSPEC_CTL_SB_NONE;
+			} else {
+				chanspec |= WL_CHANSPEC_BW_40;
+				if (request->channels[i]->flags & IEEE80211_CHAN_NO_HT40PLUS)
+					chanspec |= WL_CHANSPEC_CTL_SB_LOWER;
+				else
+					chanspec |= WL_CHANSPEC_CTL_SB_UPPER;
+			}
+
+			params->channel_list[i] = channel;
+			params->channel_list[i] &= WL_CHANSPEC_CHAN_MASK;
+			params->channel_list[i] |= chanspec;
+			WL_SCAN(("Chan : %d, Channel spec: %x \n",
+				channel, params->channel_list[i]));
+			params->channel_list[i] = htod16(params->channel_list[i]);
+		}
+	} else {
+		WL_SCAN(("Scanning all channels\n"));
+	}
+
+	/* Copy ssid array if applicable */
+	WL_SCAN(("### List of SSIDs to scan ###\n"));
+	if (n_ssids > 0) {
+		offset = offsetof(wl_scan_params_t, channel_list) + n_channels * sizeof(u16);
+		offset = roundup(offset, sizeof(u32));
+		ptr = (char*)params + offset;
+		for (i = 0; i < n_ssids; i++) {
+			memset(&ssid, 0, sizeof(wlc_ssid_t));
+			ssid.SSID_len = request->ssids[i].ssid_len;
+			memcpy(ssid.SSID, request->ssids[i].ssid, ssid.SSID_len);
+			if (!ssid.SSID_len)
+				WL_SCAN(("%d: Broadcast scan\n", i));
+			else
+				WL_SCAN(("%d: scan  for  %s size =%d\n", i,
+				ssid.SSID, ssid.SSID_len));
+			memcpy(ptr, &ssid, sizeof(wlc_ssid_t));
+			ptr += sizeof(wlc_ssid_t);
+		}
+	} else {
+		WL_SCAN(("Broadcast scan\n"));
+	}
+	/* Adding mask to channel numbers */
+	params->channel_num =
+	        htod32((n_ssids << WL_SCAN_PARAMS_NSSID_SHIFT) |
+	               (n_channels & WL_SCAN_PARAMS_COUNT_MASK));
+}
+
+static s32
+wl_run_iscan(struct wl_iscan_ctrl *iscan, struct cfg80211_scan_request *request, u16 action)
+{
+	u32 n_channels;
+	u32 n_ssids;
+	s32 params_size =
+	    (WL_SCAN_PARAMS_FIXED_SIZE + offsetof(wl_iscan_params_t, params));
+	struct wl_iscan_params *params;
+	s32 err = 0;
+
+	if (request != NULL) {
+		n_channels = request->n_channels;
+		n_ssids = request->n_ssids;
+		/* Allocate space for populating ssids in wl_iscan_params struct */
+		if (n_channels % 2)
+			/* If n_channels is odd, add a padd of u16 */
+			params_size += sizeof(u16) * (n_channels + 1);
+		else
+			params_size += sizeof(u16) * n_channels;
+
+		/* Allocate space for populating ssids in wl_iscan_params struct */
+		params_size += sizeof(struct wlc_ssid) * n_ssids;
+	}
+	params = (struct wl_iscan_params *)kzalloc(params_size, GFP_KERNEL);
+	if (!params) {
+		return -ENOMEM;
+	}
+
+	wl_scan_prep(&params->params, request);
+
+	params->version = htod32(ISCAN_REQ_VERSION);
+	params->action = htod16(action);
+	params->scan_duration = htod16(0);
+
+	if (params_size + sizeof("iscan") >= WLC_IOCTL_MEDLEN) {
+		WL_ERR(("ioctl buffer length is not sufficient\n"));
+		err = -ENOMEM;
+		goto done;
+	}
+	err = wldev_iovar_setbuf(iscan->dev, "iscan", params, params_size,
+		iscan->ioctl_buf, WLC_IOCTL_MEDLEN, NULL);
+	if (unlikely(err)) {
+		if (err == -EBUSY) {
+			WL_ERR(("system busy : iscan canceled\n"));
+		} else {
+			WL_ERR(("error (%d)\n", err));
+		}
+	}
+done:
+	kfree(params);
+	return err;
+}
+
+static s32 wl_do_iscan(struct wl_priv *wl, struct cfg80211_scan_request *request)
+{
+	struct wl_iscan_ctrl *iscan = wl_to_iscan(wl);
+	struct net_device *ndev = wl_to_prmry_ndev(wl);
+	s32 passive_scan;
+	s32 err = 0;
+
+	iscan->state = WL_ISCAN_STATE_SCANING;
+
+	passive_scan = wl->active_scan ? 0 : 1;
+	err = wldev_ioctl(ndev, WLC_SET_PASSIVE_SCAN,
+		&passive_scan, sizeof(passive_scan), false);
+	if (unlikely(err)) {
+		WL_DBG(("error (%d)\n", err));
+		return err;
+	}
+	wl->iscan_kickstart = true;
+	wl_run_iscan(iscan, request, WL_SCAN_ACTION_START);
+	mod_timer(&iscan->timer, jiffies + iscan->timer_ms * HZ / 1000);
+	iscan->timer_on = 1;
+
+	return err;
+}
+
+static s32
+wl_get_valid_channels(struct net_device *ndev, u8 *valid_chan_list, s32 size)
+{
+	wl_uint32_list_t *list;
+	s32 err = BCME_OK;
+	if (valid_chan_list == NULL || size <= 0)
+		return -ENOMEM;
+
+	memset(valid_chan_list, 0, size);
+	list = (wl_uint32_list_t *)(void *) valid_chan_list;
+	list->count = htod32(WL_NUMCHANNELS);
+	err = wldev_ioctl(ndev, WLC_GET_VALID_CHANNELS, valid_chan_list, size, false);
+	if (err != 0) {
+		WL_ERR(("get channels failed with %d\n", err));
+	}
+
+	return err;
+}
+
+static s32
+wl_run_escan(struct wl_priv *wl, struct net_device *ndev,
+	struct cfg80211_scan_request *request, uint16 action)
+{
+	s32 err = BCME_OK;
+	u32 n_channels;
+	u32 n_ssids;
+	s32 params_size = (WL_SCAN_PARAMS_FIXED_SIZE + OFFSETOF(wl_escan_params_t, params));
+	wl_escan_params_t *params = NULL;
+	struct cfg80211_scan_request *scan_request = wl->scan_request;
+	u8 chan_buf[sizeof(u32)*(WL_NUMCHANNELS + 1)];
+	u32 num_chans = 0;
+	s32 channel;
+	s32 n_valid_chan;
+	s32 search_state = WL_P2P_DISC_ST_SCAN;
+	u32 i, j, n_nodfs = 0;
+	u16 *default_chan_list = NULL;
+	wl_uint32_list_t *list;
+	struct net_device *dev = NULL;
+	WL_DBG(("Enter \n"));
+
+
+	if (!wl->p2p_supported || ((ndev == wl_to_prmry_ndev(wl)) &&
+		!p2p_scan(wl))) {
+		/* LEGACY SCAN TRIGGER */
+		WL_SCAN((" LEGACY E-SCAN START\n"));
+
+		if (request != NULL) {
+			n_channels = request->n_channels;
+			n_ssids = request->n_ssids;
+			/* Allocate space for populating ssids in wl_iscan_params struct */
+			if (n_channels % 2)
+				/* If n_channels is odd, add a padd of u16 */
+				params_size += sizeof(u16) * (n_channels + 1);
+			else
+				params_size += sizeof(u16) * n_channels;
+
+			/* Allocate space for populating ssids in wl_iscan_params struct */
+			params_size += sizeof(struct wlc_ssid) * n_ssids;
+		}
+		params = (wl_escan_params_t *) kzalloc(params_size, GFP_KERNEL);
+		if (params == NULL) {
+			err = -ENOMEM;
+			goto exit;
+		}
+
+		wl_scan_prep(&params->params, request);
+		params->version = htod32(ESCAN_REQ_VERSION);
+		params->action =  htod16(action);
+		params->sync_id = htod16(0x1234);
+		if (params_size + sizeof("escan") >= WLC_IOCTL_MEDLEN) {
+			WL_ERR(("ioctl buffer length not sufficient\n"));
+			kfree(params);
+			err = -ENOMEM;
+			goto exit;
+		}
+		err = wldev_iovar_setbuf(ndev, "escan", params, params_size,
+			wl->escan_ioctl_buf, WLC_IOCTL_MEDLEN, NULL);
+		if (unlikely(err))
+			WL_ERR((" Escan set error (%d)\n", err));
+		kfree(params);
+	}
+	else if (p2p_is_on(wl) && p2p_scan(wl)) {
+		/* P2P SCAN TRIGGER */
+		s32 _freq = 0;
+		n_nodfs = 0;
+		if (scan_request && scan_request->n_channels) {
+			num_chans = scan_request->n_channels;
+			WL_SCAN((" chann number : %d\n", num_chans));
+			default_chan_list = kzalloc(num_chans * sizeof(*default_chan_list),
+				GFP_KERNEL);
+			if (default_chan_list == NULL) {
+				WL_ERR(("channel list allocation failed \n"));
+				err = -ENOMEM;
+				goto exit;
+			}
+			if (!wl_get_valid_channels(ndev, chan_buf, sizeof(chan_buf))) {
+				list = (wl_uint32_list_t *) chan_buf;
+				n_valid_chan = dtoh32(list->count);
+				for (i = 0; i < num_chans; i++)
+				{
+					_freq = scan_request->channels[i]->center_freq;
+					channel = ieee80211_frequency_to_channel(_freq);
+					/* remove DFS channels */
+					if (channel < 52 || channel > 140) {
+						for (j = 0; j < n_valid_chan; j++) {
+							/* allows only supported channel on
+							*  current reguatory
+							*/
+							if (channel == (dtoh32(list->element[j])))
+								default_chan_list[n_nodfs++] =
+									channel;
+						}
+					}
+
+				}
+			}
+			if (num_chans == 3 && (
+						(default_chan_list[0] == SOCIAL_CHAN_1) &&
+						(default_chan_list[1] == SOCIAL_CHAN_2) &&
+						(default_chan_list[2] == SOCIAL_CHAN_3))) {
+				/* SOCIAL CHANNELS 1, 6, 11 */
+				search_state = WL_P2P_DISC_ST_SEARCH;
+				WL_INFO(("P2P SEARCH PHASE START \n"));
+			} else if ((dev = wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION)) &&
+				(wl_get_mode_by_netdev(wl, dev) == WL_MODE_AP)) {
+				/* If you are already a GO, then do SEARCH only */
+				WL_INFO(("Already a GO. Do SEARCH Only"));
+				search_state = WL_P2P_DISC_ST_SEARCH;
+				num_chans = n_nodfs;
+
+			} else {
+				WL_INFO(("P2P SCAN STATE START \n"));
+				num_chans = n_nodfs;
+			}
+
+		}
+		err = wl_cfgp2p_escan(wl, ndev, wl->active_scan, num_chans, default_chan_list,
+			search_state, action,
+			wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE));
+		kfree(default_chan_list);
+	}
+exit:
+	if (unlikely(err)) {
+		WL_ERR(("error (%d)\n", err));
+	}
+	return err;
+}
+
+
+static s32
+wl_do_escan(struct wl_priv *wl, struct wiphy *wiphy, struct net_device *ndev,
+	struct cfg80211_scan_request *request)
+{
+	s32 err = BCME_OK;
+	s32 passive_scan;
+	wl_scan_results_t *results;
+	WL_SCAN(("Enter \n"));
+	wl->escan_info.ndev = ndev;
+	wl->escan_info.wiphy = wiphy;
+	wl->escan_info.escan_state = WL_ESCAN_STATE_SCANING;
+	passive_scan = wl->active_scan ? 0 : 1;
+	err = wldev_ioctl(ndev, WLC_SET_PASSIVE_SCAN,
+		&passive_scan, sizeof(passive_scan), false);
+	if (unlikely(err)) {
+		WL_ERR(("error (%d)\n", err));
+		return err;
+	}
+	results = (wl_scan_results_t *) wl->escan_info.escan_buf;
+	results->version = 0;
+	results->count = 0;
+	results->buflen = WL_SCAN_RESULTS_FIXED_SIZE;
+
+	err = wl_run_escan(wl, ndev, request, WL_SCAN_ACTION_START);
+	return err;
+}
+
+static s32
+__wl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
+	struct cfg80211_scan_request *request,
+	struct cfg80211_ssid *this_ssid)
+{
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	struct cfg80211_ssid *ssids;
+	struct wl_scan_req *sr = wl_to_sr(wl);
+	struct ether_addr primary_mac;
+	wpa_ie_fixed_t *wps_ie;
+	s32 passive_scan;
+	bool iscan_req;
+	bool escan_req = false;
+	bool p2p_ssid;
+	s32 err = 0;
+	s32 i;
+	u32 wpsie_len = 0;
+	u8 wpsie[IE_MAX_LEN];
+
+	/* If scan req comes for p2p0, send it over primary I/F
+	 * Scan results will be delivered corresponding to cfg80211_scan_request
+	 */
+	if (ndev == wl->p2p_net) {
+		ndev = wl_to_prmry_ndev(wl);
+	}
+
+	WL_DBG(("Enter wiphy (%p)\n", wiphy));
+	if (wl_get_drv_status_all(wl, SCANNING)) {
+		WL_ERR(("Scanning already\n"));
+		return -EAGAIN;
+	}
+	if (wl_get_drv_status(wl, SCAN_ABORTING, ndev)) {
+		WL_ERR(("Scanning being aborted\n"));
+		return -EAGAIN;
+	}
+	if (request && request->n_ssids > WL_SCAN_PARAMS_SSID_MAX) {
+		WL_ERR(("request null or n_ssids > WL_SCAN_PARAMS_SSID_MAX\n"));
+		return -EOPNOTSUPP;
+	}
+
+	/* Arm scan timeout timer */
+	mod_timer(&wl->scan_timeout, jiffies + WL_SCAN_TIMER_INTERVAL_MS * HZ / 1000);
+	iscan_req = false;
+	if (request) {		/* scan bss */
+		ssids = request->ssids;
+		if (wl->iscan_on && (!ssids || !ssids->ssid_len || request->n_ssids != 1)) {
+			iscan_req = true;
+		} else if (wl->escan_on) {
+			escan_req = true;
+			p2p_ssid = false;
+			for (i = 0; i < request->n_ssids; i++) {
+				if (ssids[i].ssid_len && IS_P2P_SSID(ssids[i].ssid)) {
+					p2p_ssid = true;
+					break;
+				}
+			}
+			if (p2p_ssid) {
+				if (wl->p2p_supported) {
+					/* p2p scan trigger */
+					if (p2p_on(wl) == false) {
+						/* p2p on at the first time */
+						p2p_on(wl) = true;
+						wl_cfgp2p_set_firm_p2p(wl);
+						get_primary_mac(wl, &primary_mac);
+						wl_cfgp2p_generate_bss_mac(&primary_mac,
+							&wl->p2p->dev_addr, &wl->p2p->int_addr);
+					}
+					wl_clr_p2p_status(wl, GO_NEG_PHASE);
+					WL_DBG(("P2P: GO_NEG_PHASE status cleared \n"));
+					p2p_scan(wl) = true;
+				}
+			} else {
+				/* legacy scan trigger
+				 * So, we have to disable p2p discovery if p2p discovery is on
+				 */
+				if (wl->p2p_supported) {
+					p2p_scan(wl) = false;
+					/* If Netdevice is not equals to primary and p2p is on
+					*  , we will do p2p scan using P2PAPI_BSSCFG_DEVICE.
+					*/
+					if (p2p_on(wl) && (ndev != wl_to_prmry_ndev(wl)))
+						p2p_scan(wl) = true;
+
+					if (p2p_scan(wl) == false) {
+						if (wl_get_p2p_status(wl, DISCOVERY_ON)) {
+							err = wl_cfgp2p_discover_enable_search(wl,
+							false);
+							if (unlikely(err)) {
+								goto scan_out;
+							}
+
+						}
+					}
+				}
+				if (!wl->p2p_supported || !p2p_scan(wl)) {
+					if (ndev == wl_to_prmry_ndev(wl)) {
+						/* find the WPSIE */
+						memset(wpsie, 0, sizeof(wpsie));
+						if ((wps_ie = wl_cfgp2p_find_wpsie(
+							(u8 *)request->ie,
+							request->ie_len)) != NULL) {
+							wpsie_len =
+							wps_ie->length + WPA_RSN_IE_TAG_FIXED_LEN;
+							memcpy(wpsie, wps_ie, wpsie_len);
+						} else {
+							wpsie_len = 0;
+						}
+						if (wpsie_len > 0) {
+							err = wl_cfgp2p_set_management_ie(wl,
+								ndev, -1, VNDR_IE_PRBREQ_FLAG,
+								wpsie, wpsie_len);
+							if (unlikely(err)) {
+								goto scan_out;
+							}
+						}
+					}
+				}
+			}
+		}
+	} else {		/* scan in ibss */
+		/* we don't do iscan in ibss */
+		ssids = this_ssid;
+	}
+	wl->scan_request = request;
+	wl_set_drv_status(wl, SCANNING, ndev);
+	if (iscan_req) {
+		err = wl_do_iscan(wl, request);
+		if (likely(!err))
+			return err;
+		else
+			goto scan_out;
+	} else if (escan_req) {
+		if (wl->p2p_supported) {
+			if (p2p_on(wl) && p2p_scan(wl)) {
+
+				err = wl_cfgp2p_enable_discovery(wl, ndev,
+				request->ie, request->ie_len);
+
+				if (unlikely(err)) {
+					goto scan_out;
+				}
+			}
+		}
+		err = wl_do_escan(wl, wiphy, ndev, request);
+		if (likely(!err))
+			return err;
+		else
+			goto scan_out;
+
+
+	} else {
+		memset(&sr->ssid, 0, sizeof(sr->ssid));
+		sr->ssid.SSID_len =
+			min_t(u8, sizeof(sr->ssid.SSID), ssids->ssid_len);
+		if (sr->ssid.SSID_len) {
+			memcpy(sr->ssid.SSID, ssids->ssid, sr->ssid.SSID_len);
+			sr->ssid.SSID_len = htod32(sr->ssid.SSID_len);
+			WL_SCAN(("Specific scan ssid=\"%s\" len=%d\n",
+				sr->ssid.SSID, sr->ssid.SSID_len));
+		} else {
+			WL_SCAN(("Broadcast scan\n"));
+		}
+		WL_SCAN(("sr->ssid.SSID_len (%d)\n", sr->ssid.SSID_len));
+		passive_scan = wl->active_scan ? 0 : 1;
+		err = wldev_ioctl(ndev, WLC_SET_PASSIVE_SCAN,
+			&passive_scan, sizeof(passive_scan), false);
+		if (unlikely(err)) {
+			WL_SCAN(("WLC_SET_PASSIVE_SCAN error (%d)\n", err));
+			goto scan_out;
+		}
+		err = wldev_ioctl(ndev, WLC_SCAN, &sr->ssid,
+			sizeof(sr->ssid), false);
+		if (err) {
+			if (err == -EBUSY) {
+				WL_ERR(("system busy : scan for \"%s\" "
+					"canceled\n", sr->ssid.SSID));
+			} else {
+				WL_ERR(("WLC_SCAN error (%d)\n", err));
+			}
+			goto scan_out;
+		}
+	}
+
+	return 0;
+
+scan_out:
+	wl_clr_drv_status(wl, SCANNING, ndev);
+	wl->scan_request = NULL;
+	return err;
+}
+
+static s32
+wl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
+	struct cfg80211_scan_request *request)
+{
+	s32 err = 0;
+	struct wl_priv *wl = wiphy_priv(wiphy);
+
+	WL_DBG(("Enter \n"));
+	CHECK_SYS_UP(wl);
+
+	err = __wl_cfg80211_scan(wiphy, ndev, request, NULL);
+	if (unlikely(err)) {
+		WL_ERR(("scan error (%d)\n", err));
+		return err;
+	}
+
+	return err;
+}
+
+static s32 wl_set_rts(struct net_device *dev, u32 rts_threshold)
+{
+	s32 err = 0;
+
+	err = wldev_iovar_setint(dev, "rtsthresh", rts_threshold);
+	if (unlikely(err)) {
+		WL_ERR(("Error (%d)\n", err));
+		return err;
+	}
+	return err;
+}
+
+static s32 wl_set_frag(struct net_device *dev, u32 frag_threshold)
+{
+	s32 err = 0;
+
+	err = wldev_iovar_setint_bsscfg(dev, "fragthresh", frag_threshold, 0);
+	if (unlikely(err)) {
+		WL_ERR(("Error (%d)\n", err));
+		return err;
+	}
+	return err;
+}
+
+static s32 wl_set_retry(struct net_device *dev, u32 retry, bool l)
+{
+	s32 err = 0;
+	u32 cmd = (l ? WLC_SET_LRL : WLC_SET_SRL);
+
+	retry = htod32(retry);
+	err = wldev_ioctl(dev, cmd, &retry, sizeof(retry), false);
+	if (unlikely(err)) {
+		WL_ERR(("cmd (%d) , error (%d)\n", cmd, err));
+		return err;
+	}
+	return err;
+}
+
+static s32 wl_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed)
+{
+	struct wl_priv *wl = (struct wl_priv *)wiphy_priv(wiphy);
+	struct net_device *ndev = wl_to_prmry_ndev(wl);
+	s32 err = 0;
+
+	CHECK_SYS_UP(wl);
+	WL_DBG(("Enter\n"));
+	if (changed & WIPHY_PARAM_RTS_THRESHOLD &&
+		(wl->conf->rts_threshold != wiphy->rts_threshold)) {
+		wl->conf->rts_threshold = wiphy->rts_threshold;
+		err = wl_set_rts(ndev, wl->conf->rts_threshold);
+		if (!err)
+			return err;
+	}
+	if (changed & WIPHY_PARAM_FRAG_THRESHOLD &&
+		(wl->conf->frag_threshold != wiphy->frag_threshold)) {
+		wl->conf->frag_threshold = wiphy->frag_threshold;
+		err = wl_set_frag(ndev, wl->conf->frag_threshold);
+		if (!err)
+			return err;
+	}
+	if (changed & WIPHY_PARAM_RETRY_LONG &&
+		(wl->conf->retry_long != wiphy->retry_long)) {
+		wl->conf->retry_long = wiphy->retry_long;
+		err = wl_set_retry(ndev, wl->conf->retry_long, true);
+		if (!err)
+			return err;
+	}
+	if (changed & WIPHY_PARAM_RETRY_SHORT &&
+		(wl->conf->retry_short != wiphy->retry_short)) {
+		wl->conf->retry_short = wiphy->retry_short;
+		err = wl_set_retry(ndev, wl->conf->retry_short, false);
+		if (!err) {
+			return err;
+		}
+	}
+	return err;
+}
+
+static s32
+wl_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
+	struct cfg80211_ibss_params *params)
+{
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	struct wl_join_params join_params;
+	struct wlc_ssid ssid;
+	struct ether_addr bssid;
+	size_t join_params_size = 0;
+	s32 wsec = 0;
+	s32 bcnprd;
+	s32 err = 0;
+
+	WL_TRACE(("In\n"));
+	CHECK_SYS_UP(wl);
+
+	/*
+	 * Cancel ongoing scan to sync up with sme state machine of cfg80211.
+	 */
+	if (wl->scan_request) {
+		wl_notify_escan_complete(wl, dev, true, true);
+	}
+	/* Clean BSSID */
+	bzero(&bssid, sizeof(bssid));
+	wl_update_prof(wl, dev, NULL, (void *)&bssid, WL_PROF_BSSID);
+
+	if (params->ssid)
+		WL_INFO(("SSID: %s\n", params->ssid));
+	else {
+		WL_ERR(("SSID: NULL, Not supported\n"));
+		err = -EOPNOTSUPP;
+		goto CleanUp;
+	}
+
+	if (params->bssid)
+		WL_INFO(("BSSID: %02X:%02X:%02X:%02X:%02X:%02X\n",
+			params->bssid[0], params->bssid[1], params->bssid[2],
+			params->bssid[3], params->bssid[4], params->bssid[5]));
+
+	if (params->channel)
+		WL_INFO(("channel: %d\n", params->channel->center_freq));
+
+	if (params->channel_fixed)
+		WL_INFO(("fixed channel required\n"));
+
+	if (params->ie && params->ie_len)
+		WL_INFO(("ie len: %d\n", params->ie_len));
+
+	if (params->beacon_interval)
+		WL_INFO(("beacon interval: %d\n", params->beacon_interval));
+
+	if (params->basic_rates)
+		WL_INFO(("basic rates: %08X\n", params->basic_rates));
+
+	if (params->privacy)
+		WL_INFO(("privacy required\n"));
+
+	wl_set_drv_status(wl, CONNECTING, dev);
+
+	/* Configure Privacy for starter */
+	if (params->privacy)
+		wsec |= WEP_ENABLED;
+
+	err = wldev_iovar_setint(dev, "wsec", wsec);
+	if (err) {
+		WL_ERR(("wsec failed (%d)\n", err));
+		goto CleanUp;
+	}
+
+	err = wldev_iovar_setint(dev, "auth", WL_AUTH_OPEN_SYSTEM);
+	if (err) {
+		WL_ERR(("auth failed (%d)\n", err));
+		goto CleanUp;
+	}
+
+	err = wldev_iovar_setint(dev, "wpa_auth", 0);
+	if (err) {
+		WL_ERR(("wpa_auth failed (%d)\n", err));
+		goto CleanUp;
+	}
+
+	/* Configure Beacon Interval for starter */
+	if (params->beacon_interval)
+		bcnprd = params->beacon_interval;
+	else
+		bcnprd = 100;
+
+	bcnprd = htod32(bcnprd);
+	err = wldev_ioctl(dev, WLC_SET_BCNPRD, &bcnprd, sizeof(bcnprd), true);
+	if (err) {
+		WL_ERR(("WLC_SET_BCNPRD failed (%d)\n", err));
+		goto CleanUp;
+	}
+
+	/* Configure required join parameter */
+	memset(&join_params, 0, sizeof(struct wl_join_params));
+
+	/* SSID */
+	memset(&ssid, 0, sizeof(struct wlc_ssid));
+	ssid.SSID_len = MIN(params->ssid_len, 32);
+	join_params.ssid.SSID_len = htod32(ssid.SSID_len);
+	memcpy(ssid.SSID, params->ssid, ssid.SSID_len);
+	memcpy(join_params.ssid.SSID, params->ssid, ssid.SSID_len);
+	join_params_size = sizeof(join_params.ssid);
+
+	wl_update_prof(wl, dev, NULL, &ssid, WL_PROF_SSID);
+
+	/* BSSID */
+	if (params->bssid) {
+		memcpy(&join_params.params.bssid, params->bssid, ETHER_ADDR_LEN);
+		join_params_size = sizeof(join_params.ssid) +
+			WL_ASSOC_PARAMS_FIXED_SIZE;
+
+		wl_update_prof(wl, dev, NULL, params->bssid, WL_PROF_BSSID);
+	} else {
+		memcpy(&join_params.params.bssid, &ether_bcast, ETHER_ADDR_LEN);
+	}
+
+	/* Channel */
+	if (params->channel) {
+		u32 target_channel;
+
+		target_channel = ieee80211_frequency_to_channel(
+							params->channel->center_freq);
+		if (params->channel_fixed) {
+			/* adding chanspec */
+			wl_ch_to_chanspec(target_channel,
+				&join_params, &join_params_size);
+		}
+
+		/* set channel for starter */
+		target_channel = htod32(target_channel);
+		err = wldev_ioctl(dev, WLC_SET_CHANNEL,
+			&target_channel, sizeof(target_channel), true);
+		if (err) {
+			WL_ERR(("WLC_SET_CHANNEL failed (%d)\n", err));
+			goto CleanUp;
+		}
+	}
+
+	wl->ibss_starter = false;
+
+	err = wldev_ioctl(dev, WLC_SET_SSID, &join_params, join_params_size, true);
+	if (err) {
+		WL_ERR(("WLC_SET_SSID failed (%d)\n", err));
+		goto CleanUp;
+	}
+
+CleanUp:
+
+	if (err)
+		wl_clr_drv_status(wl, CONNECTING, dev);
+
+	WL_TRACE(("Exit\n"));
+	return err;
+}
+
+static s32 wl_cfg80211_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
+{
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	scb_val_t scbval;
+	bool act = false;
+	s32 err = 0;
+	u8 *curbssid;
+
+	WL_TRACE(("Enter\n"));
+
+	CHECK_SYS_UP(wl);
+	act = *(bool *) wl_read_prof(wl, dev, WL_PROF_ACT);
+	curbssid = wl_read_prof(wl, dev, WL_PROF_BSSID);
+	if (act) {
+		/*
+		* Cancel ongoing scan to sync up with sme state machine of cfg80211.
+		*/
+		if (wl->scan_request) {
+			wl_notify_escan_complete(wl, dev, true, true);
+		}
+		wl_set_drv_status(wl, DISCONNECTING, dev);
+		scbval.val = DOT11_RC_DISASSOC_LEAVING;
+		memcpy(&scbval.ea, curbssid, ETHER_ADDR_LEN);
+		scbval.val = htod32(scbval.val);
+		err = wldev_ioctl(dev, WLC_DISASSOC, &scbval,
+				sizeof(scb_val_t), true);
+		if (unlikely(err)) {
+			wl_clr_drv_status(wl, DISCONNECTING, dev);
+			WL_ERR(("error (%d)\n", err));
+			return err;
+		}
+	}
+
+	WL_TRACE(("Exit\n"));
+	return err;
+}
+
+static s32
+wl_set_wpa_version(struct net_device *dev, struct cfg80211_connect_params *sme)
+{
+	struct wl_priv *wl = wlcfg_drv_priv;
+	struct wl_security *sec;
+	s32 val = 0;
+	s32 err = 0;
+	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
+
+	if (sme->crypto.wpa_versions & NL80211_WPA_VERSION_1)
+		val = WPA_AUTH_PSK | WPA_AUTH_UNSPECIFIED;
+	else if (sme->crypto.wpa_versions & NL80211_WPA_VERSION_2)
+		val = WPA2_AUTH_PSK| WPA2_AUTH_UNSPECIFIED;
+	else
+		val = WPA_AUTH_DISABLED;
+
+	if (is_wps_conn(sme))
+		val = WPA_AUTH_DISABLED;
+
+	WL_DBG(("setting wpa_auth to 0x%0x\n", val));
+	err = wldev_iovar_setint_bsscfg(dev, "wpa_auth", val, bssidx);
+	if (unlikely(err)) {
+		WL_ERR(("set wpa_auth failed (%d)\n", err));
+		return err;
+	}
+	sec = wl_read_prof(wl, dev, WL_PROF_SEC);
+	sec->wpa_versions = sme->crypto.wpa_versions;
+	return err;
+}
+
+static s32
+wl_set_auth_type(struct net_device *dev, struct cfg80211_connect_params *sme)
+{
+	struct wl_priv *wl = wlcfg_drv_priv;
+	struct wl_security *sec;
+	s32 val = 0;
+	s32 err = 0;
+	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
+	switch (sme->auth_type) {
+	case NL80211_AUTHTYPE_OPEN_SYSTEM:
+		val = WL_AUTH_OPEN_SYSTEM;
+		WL_DBG(("open system\n"));
+		break;
+	case NL80211_AUTHTYPE_SHARED_KEY:
+		val = WL_AUTH_SHARED_KEY;
+		WL_DBG(("shared key\n"));
+		break;
+	case NL80211_AUTHTYPE_AUTOMATIC:
+		val = WL_AUTH_OPEN_SHARED;
+		WL_DBG(("automatic\n"));
+		break;
+	case NL80211_AUTHTYPE_NETWORK_EAP:
+		WL_DBG(("network eap\n"));
+	default:
+		val = WL_AUTH_OPEN_SHARED;
+		WL_ERR(("invalid auth type (%d)\n", sme->auth_type));
+		break;
+	}
+
+	err = wldev_iovar_setint_bsscfg(dev, "auth", val, bssidx);
+	if (unlikely(err)) {
+		WL_ERR(("set auth failed (%d)\n", err));
+		return err;
+	}
+	sec = wl_read_prof(wl, dev, WL_PROF_SEC);
+	sec->auth_type = sme->auth_type;
+	return err;
+}
+
+static s32
+wl_set_set_cipher(struct net_device *dev, struct cfg80211_connect_params *sme)
+{
+	struct wl_priv *wl = wlcfg_drv_priv;
+	struct wl_security *sec;
+	s32 pval = 0;
+	s32 gval = 0;
+	s32 err = 0;
+	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
+
+	if (sme->crypto.n_ciphers_pairwise) {
+		switch (sme->crypto.ciphers_pairwise[0]) {
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+			pval = WEP_ENABLED;
+			break;
+		case WLAN_CIPHER_SUITE_TKIP:
+			pval = TKIP_ENABLED;
+			break;
+		case WLAN_CIPHER_SUITE_CCMP:
+			pval = AES_ENABLED;
+			break;
+		case WLAN_CIPHER_SUITE_AES_CMAC:
+			pval = AES_ENABLED;
+			break;
+		default:
+			WL_ERR(("invalid cipher pairwise (%d)\n",
+				sme->crypto.ciphers_pairwise[0]));
+			return -EINVAL;
+		}
+	}
+	if (sme->crypto.cipher_group) {
+		switch (sme->crypto.cipher_group) {
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+			gval = WEP_ENABLED;
+			break;
+		case WLAN_CIPHER_SUITE_TKIP:
+			gval = TKIP_ENABLED;
+			break;
+		case WLAN_CIPHER_SUITE_CCMP:
+			gval = AES_ENABLED;
+			break;
+		case WLAN_CIPHER_SUITE_AES_CMAC:
+			gval = AES_ENABLED;
+			break;
+		default:
+			WL_ERR(("invalid cipher group (%d)\n",
+				sme->crypto.cipher_group));
+			return -EINVAL;
+		}
+	}
+
+	WL_DBG(("pval (%d) gval (%d)\n", pval, gval));
+
+	if (is_wps_conn(sme)) {
+		if (sme->privacy)
+			err = wldev_iovar_setint_bsscfg(dev, "wsec", 4, bssidx);
+		else
+			/* WPS-2.0 allowes no security */
+			err = wldev_iovar_setint_bsscfg(dev, "wsec", 0, bssidx);
+	} else {
+		WL_DBG((" NO, is_wps_conn, Set pval | gval to WSEC"));
+		err = wldev_iovar_setint_bsscfg(dev, "wsec",
+				pval | gval, bssidx);
+	}
+	if (unlikely(err)) {
+		WL_ERR(("error (%d)\n", err));
+		return err;
+	}
+
+	sec = wl_read_prof(wl, dev, WL_PROF_SEC);
+	sec->cipher_pairwise = sme->crypto.ciphers_pairwise[0];
+	sec->cipher_group = sme->crypto.cipher_group;
+
+	return err;
+}
+
+static s32
+wl_set_key_mgmt(struct net_device *dev, struct cfg80211_connect_params *sme)
+{
+	struct wl_priv *wl = wlcfg_drv_priv;
+	struct wl_security *sec;
+	s32 val = 0;
+	s32 err = 0;
+	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
+
+	if (sme->crypto.n_akm_suites) {
+		err = wldev_iovar_getint(dev, "wpa_auth", &val);
+		if (unlikely(err)) {
+			WL_ERR(("could not get wpa_auth (%d)\n", err));
+			return err;
+		}
+		if (val & (WPA_AUTH_PSK | WPA_AUTH_UNSPECIFIED)) {
+			switch (sme->crypto.akm_suites[0]) {
+			case WLAN_AKM_SUITE_8021X:
+				val = WPA_AUTH_UNSPECIFIED;
+				break;
+			case WLAN_AKM_SUITE_PSK:
+				val = WPA_AUTH_PSK;
+				break;
+			default:
+				WL_ERR(("invalid cipher group (%d)\n",
+					sme->crypto.cipher_group));
+				return -EINVAL;
+			}
+		} else if (val & (WPA2_AUTH_PSK | WPA2_AUTH_UNSPECIFIED)) {
+			switch (sme->crypto.akm_suites[0]) {
+			case WLAN_AKM_SUITE_8021X:
+				val = WPA2_AUTH_UNSPECIFIED;
+				break;
+			case WLAN_AKM_SUITE_PSK:
+				val = WPA2_AUTH_PSK;
+				break;
+			default:
+				WL_ERR(("invalid cipher group (%d)\n",
+					sme->crypto.cipher_group));
+				return -EINVAL;
+			}
+		}
+		WL_DBG(("setting wpa_auth to %d\n", val));
+
+		err = wldev_iovar_setint_bsscfg(dev, "wpa_auth", val, bssidx);
+		if (unlikely(err)) {
+			WL_ERR(("could not set wpa_auth (%d)\n", err));
+			return err;
+		}
+	}
+	sec = wl_read_prof(wl, dev, WL_PROF_SEC);
+	sec->wpa_auth = sme->crypto.akm_suites[0];
+
+	return err;
+}
+
+static s32
+wl_set_set_sharedkey(struct net_device *dev,
+	struct cfg80211_connect_params *sme)
+{
+	struct wl_priv *wl = wlcfg_drv_priv;
+	struct wl_security *sec;
+	struct wl_wsec_key key;
+	s32 val;
+	s32 err = 0;
+	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
+
+	WL_DBG(("key len (%d)\n", sme->key_len));
+	if (sme->key_len) {
+		sec = wl_read_prof(wl, dev, WL_PROF_SEC);
+		WL_DBG(("wpa_versions 0x%x cipher_pairwise 0x%x\n",
+			sec->wpa_versions, sec->cipher_pairwise));
+		if (!(sec->wpa_versions & (NL80211_WPA_VERSION_1 |
+			NL80211_WPA_VERSION_2)) &&
+			(sec->cipher_pairwise & (WLAN_CIPHER_SUITE_WEP40 |
+		WLAN_CIPHER_SUITE_WEP104)))
+		{
+			memset(&key, 0, sizeof(key));
+			key.len = (u32) sme->key_len;
+			key.index = (u32) sme->key_idx;
+			if (unlikely(key.len > sizeof(key.data))) {
+				WL_ERR(("Too long key length (%u)\n", key.len));
+				return -EINVAL;
+			}
+			memcpy(key.data, sme->key, key.len);
+			key.flags = WL_PRIMARY_KEY;
+			switch (sec->cipher_pairwise) {
+			case WLAN_CIPHER_SUITE_WEP40:
+				key.algo = CRYPTO_ALGO_WEP1;
+				break;
+			case WLAN_CIPHER_SUITE_WEP104:
+				key.algo = CRYPTO_ALGO_WEP128;
+				break;
+			default:
+				WL_ERR(("Invalid algorithm (%d)\n",
+					sme->crypto.ciphers_pairwise[0]));
+				return -EINVAL;
+			}
+			/* Set the new key/index */
+			WL_DBG(("key length (%d) key index (%d) algo (%d)\n",
+				key.len, key.index, key.algo));
+			WL_DBG(("key \"%s\"\n", key.data));
+			swap_key_from_BE(&key);
+			err = wldev_iovar_setbuf_bsscfg(dev, "wsec_key", &key, sizeof(key),
+				wl->ioctl_buf, WLC_IOCTL_MAXLEN, bssidx, &wl->ioctl_buf_sync);
+			if (unlikely(err)) {
+				WL_ERR(("WLC_SET_KEY error (%d)\n", err));
+				return err;
+			}
+			if (sec->auth_type == NL80211_AUTHTYPE_SHARED_KEY) {
+				WL_DBG(("set auth_type to shared key\n"));
+				val = WL_AUTH_SHARED_KEY;	/* shared key */
+				err = wldev_iovar_setint_bsscfg(dev, "auth", val, bssidx);
+				if (unlikely(err)) {
+					WL_ERR(("set auth failed (%d)\n", err));
+					return err;
+				}
+			}
+		}
+	}
+	return err;
+}
+
+static s32
+wl_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
+	struct cfg80211_connect_params *sme)
+{
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	struct ieee80211_channel *chan = sme->channel;
+	wl_extjoin_params_t *ext_join_params;
+	struct wl_join_params join_params;
+	size_t join_params_size;
+	s32 err = 0;
+	wpa_ie_fixed_t *wpa_ie;
+	wpa_ie_fixed_t *wps_ie;
+	bcm_tlv_t *wpa2_ie;
+	u8* wpaie  = 0;
+	u32 wpaie_len = 0;
+	u32 wpsie_len = 0;
+	u32 chan_cnt = 0;
+	u8 wpsie[IE_MAX_LEN];
+	struct ether_addr bssid;
+
+	WL_DBG(("In\n"));
+	CHECK_SYS_UP(wl);
+
+	/*
+	 * Cancel ongoing scan to sync up with sme state machine of cfg80211.
+	 */
+	if (wl->scan_request) {
+		wl_notify_escan_complete(wl, dev, true, true);
+	}
+	/* Clean BSSID */
+	bzero(&bssid, sizeof(bssid));
+	wl_update_prof(wl, dev, NULL, (void *)&bssid, WL_PROF_BSSID);
+
+	if (IS_P2P_SSID(sme->ssid) && (dev != wl_to_prmry_ndev(wl))) {
+		/* we only allow to connect using virtual interface in case of P2P */
+		if (p2p_is_on(wl) && is_wps_conn(sme)) {
+			WL_DBG(("ASSOC1 p2p index : %d sme->ie_len %d\n",
+				wl_cfgp2p_find_idx(wl, dev), sme->ie_len));
+			/* Have to apply WPS IE + P2P IE in assoc req frame */
+			wl_cfgp2p_set_management_ie(wl, dev,
+				wl_cfgp2p_find_idx(wl, dev), VNDR_IE_PRBREQ_FLAG,
+				wl_to_p2p_bss_saved_ie(wl, P2PAPI_BSSCFG_DEVICE).p2p_probe_req_ie,
+				wl_to_p2p_bss_saved_ie(wl,
+				P2PAPI_BSSCFG_DEVICE).p2p_probe_req_ie_len);
+			wl_cfgp2p_set_management_ie(wl, dev, wl_cfgp2p_find_idx(wl, dev),
+				VNDR_IE_ASSOCREQ_FLAG, sme->ie, sme->ie_len);
+		} else if (p2p_is_on(wl) && (sme->crypto.wpa_versions & NL80211_WPA_VERSION_2)) {
+			/* This is the connect req after WPS is done [credentials exchanged]
+			 * currently identified with WPA_VERSION_2 .
+			 * Update the previously set IEs with
+			 * the newly received IEs from Supplicant. This will remove the WPS IE from
+			 * the Assoc Req.
+			 */
+			WL_DBG(("ASSOC2 p2p index : %d sme->ie_len %d\n",
+				wl_cfgp2p_find_idx(wl, dev), sme->ie_len));
+			wl_cfgp2p_set_management_ie(wl, dev,
+				wl_cfgp2p_find_idx(wl, dev), VNDR_IE_PRBREQ_FLAG,
+				sme->ie, sme->ie_len);
+			wl_cfgp2p_set_management_ie(wl, dev, wl_cfgp2p_find_idx(wl, dev),
+				VNDR_IE_ASSOCREQ_FLAG, sme->ie, sme->ie_len);
+		}
+
+	} else if (dev == wl_to_prmry_ndev(wl)) {
+			/* find the RSN_IE */
+			if ((wpa2_ie = bcm_parse_tlvs((u8 *)sme->ie, sme->ie_len,
+				DOT11_MNG_RSN_ID)) != NULL) {
+				WL_DBG((" WPA2 IE is found\n"));
+			}
+			/* find the WPA_IE */
+			if ((wpa_ie = wl_cfgp2p_find_wpaie((u8 *)sme->ie,
+				sme->ie_len)) != NULL) {
+				WL_DBG((" WPA IE is found\n"));
+			}
+			if (wpa_ie != NULL || wpa2_ie != NULL) {
+				wpaie = (wpa_ie != NULL) ? (u8 *)wpa_ie : (u8 *)wpa2_ie;
+				wpaie_len = (wpa_ie != NULL) ? wpa_ie->length : wpa2_ie->len;
+				wpaie_len += WPA_RSN_IE_TAG_FIXED_LEN;
+				wldev_iovar_setbuf(dev, "wpaie", wpaie, wpaie_len,
+					wl->ioctl_buf, WLC_IOCTL_MAXLEN, &wl->ioctl_buf_sync);
+			} else {
+				wldev_iovar_setbuf(dev, "wpaie", NULL, 0,
+					wl->ioctl_buf, WLC_IOCTL_MAXLEN, &wl->ioctl_buf_sync);
+			}
+
+			/* find the WPSIE */
+			memset(wpsie, 0, sizeof(wpsie));
+			if ((wps_ie = wl_cfgp2p_find_wpsie((u8 *)sme->ie,
+				sme->ie_len)) != NULL) {
+				wpsie_len = wps_ie->length +WPA_RSN_IE_TAG_FIXED_LEN;
+				memcpy(wpsie, wps_ie, wpsie_len);
+			} else {
+				wpsie_len = 0;
+			}
+			err = wl_cfgp2p_set_management_ie(wl, dev, -1,
+				VNDR_IE_ASSOCREQ_FLAG, wpsie, wpsie_len);
+			if (unlikely(err)) {
+				return err;
+			}
+	}
+	if (unlikely(!sme->ssid)) {
+		WL_ERR(("Invalid ssid\n"));
+		return -EOPNOTSUPP;
+	}
+	if (chan) {
+		wl->channel = ieee80211_frequency_to_channel(chan->center_freq);
+		chan_cnt = 1;
+		WL_DBG(("channel (%d), center_req (%d)\n", wl->channel,
+			chan->center_freq));
+	} else
+		wl->channel = 0;
+	WL_DBG(("ie (%p), ie_len (%zd)\n", sme->ie, sme->ie_len));
+	err = wl_set_wpa_version(dev, sme);
+	if (unlikely(err)) {
+		WL_ERR(("Invalid wpa_version\n"));
+		return err;
+	}
+
+	err = wl_set_auth_type(dev, sme);
+	if (unlikely(err)) {
+		WL_ERR(("Invalid auth type\n"));
+		return err;
+	}
+
+	err = wl_set_set_cipher(dev, sme);
+	if (unlikely(err)) {
+		WL_ERR(("Invalid ciper\n"));
+		return err;
+	}
+
+	err = wl_set_key_mgmt(dev, sme);
+	if (unlikely(err)) {
+		WL_ERR(("Invalid key mgmt\n"));
+		return err;
+	}
+
+	err = wl_set_set_sharedkey(dev, sme);
+	if (unlikely(err)) {
+		WL_ERR(("Invalid shared key\n"));
+		return err;
+	}
+
+	/*
+	 *  Join with specific BSSID and cached SSID
+	 *  If SSID is zero join based on BSSID only
+	 */
+	join_params_size = WL_EXTJOIN_PARAMS_FIXED_SIZE +
+		chan_cnt * sizeof(chanspec_t);
+	ext_join_params =  (wl_extjoin_params_t*)kzalloc(join_params_size, GFP_KERNEL);
+	if (ext_join_params == NULL) {
+		err = -ENOMEM;
+		wl_clr_drv_status(wl, CONNECTING, dev);
+		goto exit;
+	}
+	ext_join_params->ssid.SSID_len = min(sizeof(ext_join_params->ssid.SSID), sme->ssid_len);
+	memcpy(&ext_join_params->ssid.SSID, sme->ssid, ext_join_params->ssid.SSID_len);
+	ext_join_params->ssid.SSID_len = htod32(ext_join_params->ssid.SSID_len);
+	/* Set up join scan parameters */
+	ext_join_params->scan.scan_type = -1;
+	ext_join_params->scan.nprobes = 2;
+	/* increate dwell time to receive probe response or detect Beacon
+	* from target AP at a noisy air only during connect command
+	*/
+	ext_join_params->scan.active_time = WL_SCAN_ACTIVE_TIME*3;
+	ext_join_params->scan.passive_time = WL_SCAN_PASSIVE_TIME*3;
+	ext_join_params->scan.home_time = -1;
+
+	if (sme->bssid)
+		memcpy(&ext_join_params->assoc.bssid, sme->bssid, ETH_ALEN);
+	else
+		memcpy(&ext_join_params->assoc.bssid, &ether_bcast, ETH_ALEN);
+	ext_join_params->assoc.chanspec_num = chan_cnt;
+	if (chan_cnt) {
+		u16 channel, band, bw, ctl_sb;
+		chanspec_t chspec;
+		channel = wl->channel;
+		band = (channel <= CH_MAX_2G_CHANNEL) ? WL_CHANSPEC_BAND_2G
+			: WL_CHANSPEC_BAND_5G;
+		bw = WL_CHANSPEC_BW_20;
+		ctl_sb = WL_CHANSPEC_CTL_SB_NONE;
+		chspec = (channel | band | bw | ctl_sb);
+		ext_join_params->assoc.chanspec_list[0]  &= WL_CHANSPEC_CHAN_MASK;
+		ext_join_params->assoc.chanspec_list[0] |= chspec;
+		ext_join_params->assoc.chanspec_list[0] =
+			htodchanspec(ext_join_params->assoc.chanspec_list[0]);
+	}
+	ext_join_params->assoc.chanspec_num = htod32(ext_join_params->assoc.chanspec_num);
+	if (ext_join_params->ssid.SSID_len < IEEE80211_MAX_SSID_LEN) {
+		WL_INFO(("ssid \"%s\", len (%d)\n", ext_join_params->ssid.SSID,
+			ext_join_params->ssid.SSID_len));
+	}
+	wl_set_drv_status(wl, CONNECTING, dev);
+	err = wldev_iovar_setbuf_bsscfg(dev, "join", ext_join_params, join_params_size,
+		wl->ioctl_buf, WLC_IOCTL_MAXLEN, wl_cfgp2p_find_idx(wl, dev), &wl->ioctl_buf_sync);
+	kfree(ext_join_params);
+	if (err) {
+		wl_clr_drv_status(wl, CONNECTING, dev);
+		if (err == BCME_UNSUPPORTED) {
+			WL_DBG(("join iovar is not supported\n"));
+			goto set_ssid;
+		} else
+			WL_ERR(("error (%d)\n", err));
+	} else
+		goto exit;
+
+set_ssid:
+	memset(&join_params, 0, sizeof(join_params));
+	join_params_size = sizeof(join_params.ssid);
+
+	join_params.ssid.SSID_len = min(sizeof(join_params.ssid.SSID), sme->ssid_len);
+	memcpy(&join_params.ssid.SSID, sme->ssid, join_params.ssid.SSID_len);
+	join_params.ssid.SSID_len = htod32(join_params.ssid.SSID_len);
+	wl_update_prof(wl, dev, NULL, &join_params.ssid, WL_PROF_SSID);
+	if (sme->bssid)
+		memcpy(&join_params.params.bssid, sme->bssid, ETH_ALEN);
+	else
+		memcpy(&join_params.params.bssid, &ether_bcast, ETH_ALEN);
+
+	wl_ch_to_chanspec(wl->channel, &join_params, &join_params_size);
+	WL_DBG(("join_param_size %d\n", join_params_size));
+
+	if (join_params.ssid.SSID_len < IEEE80211_MAX_SSID_LEN) {
+		WL_INFO(("ssid \"%s\", len (%d)\n", join_params.ssid.SSID,
+			join_params.ssid.SSID_len));
+	}
+	wl_set_drv_status(wl, CONNECTING, dev);
+	err = wldev_ioctl(dev, WLC_SET_SSID, &join_params, join_params_size, true);
+	if (err) {
+		WL_ERR(("error (%d)\n", err));
+		wl_clr_drv_status(wl, CONNECTING, dev);
+	}
+exit:
+	return err;
+}
+
+static s32
+wl_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *dev,
+	u16 reason_code)
+{
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	scb_val_t scbval;
+	bool act = false;
+	s32 err = 0;
+	u8 *curbssid;
+	WL_ERR(("Reason %d\n", reason_code));
+	CHECK_SYS_UP(wl);
+	act = *(bool *) wl_read_prof(wl, dev, WL_PROF_ACT);
+	curbssid = wl_read_prof(wl, dev, WL_PROF_BSSID);
+	if (act) {
+		/*
+		* Cancel ongoing scan to sync up with sme state machine of cfg80211.
+		*/
+		if (wl->scan_request) {
+			wl_notify_escan_complete(wl, dev, true, true);
+		}
+		wl_set_drv_status(wl, DISCONNECTING, dev);
+		scbval.val = reason_code;
+		memcpy(&scbval.ea, curbssid, ETHER_ADDR_LEN);
+		scbval.val = htod32(scbval.val);
+		err = wldev_ioctl(dev, WLC_DISASSOC, &scbval,
+			sizeof(scb_val_t), true);
+		if (unlikely(err)) {
+			wl_clr_drv_status(wl, DISCONNECTING, dev);
+			WL_ERR(("error (%d)\n", err));
+			return err;
+		}
+	}
+
+	return err;
+}
+
+static s32
+wl_cfg80211_set_tx_power(struct wiphy *wiphy,
+	enum nl80211_tx_power_setting type, s32 dbm)
+{
+
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	struct net_device *ndev = wl_to_prmry_ndev(wl);
+	u16 txpwrmw;
+	s32 err = 0;
+	s32 disable = 0;
+
+	CHECK_SYS_UP(wl);
+	switch (type) {
+	case NL80211_TX_POWER_AUTOMATIC:
+		break;
+	case NL80211_TX_POWER_LIMITED:
+		if (dbm < 0) {
+			WL_ERR(("TX_POWER_LIMITTED - dbm is negative\n"));
+			return -EINVAL;
+		}
+		break;
+	case NL80211_TX_POWER_FIXED:
+		if (dbm < 0) {
+			WL_ERR(("TX_POWER_FIXED - dbm is negative..\n"));
+			return -EINVAL;
+		}
+		break;
+	}
+	/* Make sure radio is off or on as far as software is concerned */
+	disable = WL_RADIO_SW_DISABLE << 16;
+	disable = htod32(disable);
+	err = wldev_ioctl(ndev, WLC_SET_RADIO, &disable, sizeof(disable), true);
+	if (unlikely(err)) {
+		WL_ERR(("WLC_SET_RADIO error (%d)\n", err));
+		return err;
+	}
+
+	if (dbm > 0xffff)
+		txpwrmw = 0xffff;
+	else
+		txpwrmw = (u16) dbm;
+	err = wldev_iovar_setint(ndev, "qtxpower",
+		(s32) (bcm_mw_to_qdbm(txpwrmw)));
+	if (unlikely(err)) {
+		WL_ERR(("qtxpower error (%d)\n", err));
+		return err;
+	}
+	wl->conf->tx_power = dbm;
+
+	return err;
+}
+
+static s32 wl_cfg80211_get_tx_power(struct wiphy *wiphy, s32 *dbm)
+{
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	struct net_device *ndev = wl_to_prmry_ndev(wl);
+	s32 txpwrdbm;
+	u8 result;
+	s32 err = 0;
+
+	CHECK_SYS_UP(wl);
+	err = wldev_iovar_getint(ndev, "qtxpower", &txpwrdbm);
+	if (unlikely(err)) {
+		WL_ERR(("error (%d)\n", err));
+		return err;
+	}
+	result = (u8) (txpwrdbm & ~WL_TXPWR_OVERRIDE);
+	*dbm = (s32) bcm_qdbm_to_mw(result);
+
+	return err;
+}
+
+static s32
+wl_cfg80211_config_default_key(struct wiphy *wiphy, struct net_device *dev,
+	u8 key_idx, bool unicast, bool multicast)
+{
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	u32 index;
+	s32 wsec;
+	s32 err = 0;
+	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
+
+	WL_DBG(("key index (%d)\n", key_idx));
+	CHECK_SYS_UP(wl);
+	err = wldev_iovar_getint_bsscfg(dev, "wsec", &wsec, bssidx);
+	if (unlikely(err)) {
+		WL_ERR(("WLC_GET_WSEC error (%d)\n", err));
+		return err;
+	}
+	if (wsec & WEP_ENABLED) {
+		/* Just select a new current key */
+		index = (u32) key_idx;
+		index = htod32(index);
+		err = wldev_ioctl(dev, WLC_SET_KEY_PRIMARY, &index,
+			sizeof(index), true);
+		if (unlikely(err)) {
+			WL_ERR(("error (%d)\n", err));
+		}
+	}
+	return err;
+}
+
+static s32
+wl_add_keyext(struct wiphy *wiphy, struct net_device *dev,
+	u8 key_idx, const u8 *mac_addr, struct key_params *params)
+{
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	struct wl_wsec_key key;
+	s32 err = 0;
+	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
+	s32 mode = wl_get_mode_by_netdev(wl, dev);
+	memset(&key, 0, sizeof(key));
+	key.index = (u32) key_idx;
+
+	if (!ETHER_ISMULTI(mac_addr))
+		memcpy((char *)&key.ea, (void *)mac_addr, ETHER_ADDR_LEN);
+	key.len = (u32) params->key_len;
+
+	/* check for key index change */
+	if (key.len == 0) {
+		/* key delete */
+		swap_key_from_BE(&key);
+		wldev_iovar_setbuf_bsscfg(dev, "wsec_key", &key, sizeof(key),
+			wl->ioctl_buf, WLC_IOCTL_MAXLEN, bssidx, &wl->ioctl_buf_sync);
+		if (unlikely(err)) {
+			WL_ERR(("key delete error (%d)\n", err));
+			return err;
+		}
+	} else {
+		if (key.len > sizeof(key.data)) {
+			WL_ERR(("Invalid key length (%d)\n", key.len));
+			return -EINVAL;
+		}
+		WL_DBG(("Setting the key index %d\n", key.index));
+		memcpy(key.data, params->key, key.len);
+
+		if ((mode == WL_MODE_BSS) &&
+			(params->cipher == WLAN_CIPHER_SUITE_TKIP)) {
+			u8 keybuf[8];
+			memcpy(keybuf, &key.data[24], sizeof(keybuf));
+			memcpy(&key.data[24], &key.data[16], sizeof(keybuf));
+			memcpy(&key.data[16], keybuf, sizeof(keybuf));
+		}
+
+		/* if IW_ENCODE_EXT_RX_SEQ_VALID set */
+		if (params->seq && params->seq_len == 6) {
+			/* rx iv */
+			u8 *ivptr;
+			ivptr = (u8 *) params->seq;
+			key.rxiv.hi = (ivptr[5] << 24) | (ivptr[4] << 16) |
+				(ivptr[3] << 8) | ivptr[2];
+			key.rxiv.lo = (ivptr[1] << 8) | ivptr[0];
+			key.iv_initialized = true;
+		}
+
+		switch (params->cipher) {
+		case WLAN_CIPHER_SUITE_WEP40:
+			key.algo = CRYPTO_ALGO_WEP1;
+			WL_DBG(("WLAN_CIPHER_SUITE_WEP40\n"));
+			break;
+		case WLAN_CIPHER_SUITE_WEP104:
+			key.algo = CRYPTO_ALGO_WEP128;
+			WL_DBG(("WLAN_CIPHER_SUITE_WEP104\n"));
+			break;
+		case WLAN_CIPHER_SUITE_TKIP:
+			key.algo = CRYPTO_ALGO_TKIP;
+			WL_DBG(("WLAN_CIPHER_SUITE_TKIP\n"));
+			break;
+		case WLAN_CIPHER_SUITE_AES_CMAC:
+			key.algo = CRYPTO_ALGO_AES_CCM;
+			WL_DBG(("WLAN_CIPHER_SUITE_AES_CMAC\n"));
+			break;
+		case WLAN_CIPHER_SUITE_CCMP:
+			key.algo = CRYPTO_ALGO_AES_CCM;
+			WL_DBG(("WLAN_CIPHER_SUITE_CCMP\n"));
+			break;
+		default:
+			WL_ERR(("Invalid cipher (0x%x)\n", params->cipher));
+			return -EINVAL;
+		}
+		swap_key_from_BE(&key);
+		wldev_iovar_setbuf_bsscfg(dev, "wsec_key", &key, sizeof(key),
+			wl->ioctl_buf, WLC_IOCTL_MAXLEN, bssidx, &wl->ioctl_buf_sync);
+		if (unlikely(err)) {
+			WL_ERR(("WLC_SET_KEY error (%d)\n", err));
+			return err;
+		}
+	}
+	return err;
+}
+
+static s32
+wl_cfg80211_add_key(struct wiphy *wiphy, struct net_device *dev,
+	u8 key_idx, bool pairwise, const u8 *mac_addr,
+	struct key_params *params)
+{
+	struct wl_wsec_key key;
+	s32 val = 0;
+	s32 wsec = 0;
+	s32 err = 0;
+	u8 keybuf[8];
+	s32 bssidx = 0;
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	s32 mode = wl_get_mode_by_netdev(wl, dev);
+	WL_DBG(("key index (%d)\n", key_idx));
+	CHECK_SYS_UP(wl);
+
+	bssidx = wl_cfgp2p_find_idx(wl, dev);
+
+	if (mac_addr) {
+		wl_add_keyext(wiphy, dev, key_idx, mac_addr, params);
+		goto exit;
+	}
+	memset(&key, 0, sizeof(key));
+
+	key.len = (u32) params->key_len;
+	key.index = (u32) key_idx;
+
+	if (unlikely(key.len > sizeof(key.data))) {
+		WL_ERR(("Too long key length (%u)\n", key.len));
+		return -EINVAL;
+	}
+	memcpy(key.data, params->key, key.len);
+
+	key.flags = WL_PRIMARY_KEY;
+	switch (params->cipher) {
+	case WLAN_CIPHER_SUITE_WEP40:
+		key.algo = CRYPTO_ALGO_WEP1;
+		val = WEP_ENABLED;
+		WL_DBG(("WLAN_CIPHER_SUITE_WEP40\n"));
+		break;
+	case WLAN_CIPHER_SUITE_WEP104:
+		key.algo = CRYPTO_ALGO_WEP128;
+		val = WEP_ENABLED;
+		WL_DBG(("WLAN_CIPHER_SUITE_WEP104\n"));
+		break;
+	case WLAN_CIPHER_SUITE_TKIP:
+		key.algo = CRYPTO_ALGO_TKIP;
+		val = TKIP_ENABLED;
+		/* wpa_supplicant switches the third and fourth quarters of the TKIP key */
+		if (mode == WL_MODE_BSS) {
+			bcopy(&key.data[24], keybuf, sizeof(keybuf));
+			bcopy(&key.data[16], &key.data[24], sizeof(keybuf));
+			bcopy(keybuf, &key.data[16], sizeof(keybuf));
+		}
+		WL_DBG(("WLAN_CIPHER_SUITE_TKIP\n"));
+		break;
+	case WLAN_CIPHER_SUITE_AES_CMAC:
+		key.algo = CRYPTO_ALGO_AES_CCM;
+		val = AES_ENABLED;
+		WL_DBG(("WLAN_CIPHER_SUITE_AES_CMAC\n"));
+		break;
+	case WLAN_CIPHER_SUITE_CCMP:
+		key.algo = CRYPTO_ALGO_AES_CCM;
+		val = AES_ENABLED;
+		WL_DBG(("WLAN_CIPHER_SUITE_CCMP\n"));
+		break;
+	default:
+		WL_ERR(("Invalid cipher (0x%x)\n", params->cipher));
+		return -EINVAL;
+	}
+
+	/* Set the new key/index */
+	swap_key_from_BE(&key);
+	err = wldev_iovar_setbuf_bsscfg(dev, "wsec_key", &key, sizeof(key), wl->ioctl_buf,
+		WLC_IOCTL_MAXLEN, bssidx, &wl->ioctl_buf_sync);
+	if (unlikely(err)) {
+		WL_ERR(("WLC_SET_KEY error (%d)\n", err));
+		return err;
+	}
+
+exit:
+	err = wldev_iovar_getint_bsscfg(dev, "wsec", &wsec, bssidx);
+	if (unlikely(err)) {
+		WL_ERR(("get wsec error (%d)\n", err));
+		return err;
+	}
+
+	wsec |= val;
+	err = wldev_iovar_setint_bsscfg(dev, "wsec", wsec, bssidx);
+	if (unlikely(err)) {
+		WL_ERR(("set wsec error (%d)\n", err));
+		return err;
+	}
+
+	return err;
+}
+
+static s32
+wl_cfg80211_del_key(struct wiphy *wiphy, struct net_device *dev,
+	u8 key_idx, bool pairwise, const u8 *mac_addr)
+{
+	struct wl_wsec_key key;
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	s32 err = 0;
+	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
+
+	WL_DBG(("Enter\n"));
+	CHECK_SYS_UP(wl);
+	memset(&key, 0, sizeof(key));
+
+	key.flags = WL_PRIMARY_KEY;
+	key.algo = CRYPTO_ALGO_OFF;
+	key.index = (u32) key_idx;
+
+	WL_DBG(("key index (%d)\n", key_idx));
+	/* Set the new key/index */
+	swap_key_from_BE(&key);
+	wldev_iovar_setbuf_bsscfg(dev, "wsec_key", &key, sizeof(key), wl->ioctl_buf,
+		WLC_IOCTL_MAXLEN, bssidx, &wl->ioctl_buf_sync);
+	if (unlikely(err)) {
+		if (err == -EINVAL) {
+			if (key.index >= DOT11_MAX_DEFAULT_KEYS) {
+				/* we ignore this key index in this case */
+				WL_DBG(("invalid key index (%d)\n", key_idx));
+			}
+		} else {
+			WL_ERR(("WLC_SET_KEY error (%d)\n", err));
+		}
+		return err;
+	}
+	return err;
+}
+
+static s32
+wl_cfg80211_get_key(struct wiphy *wiphy, struct net_device *dev,
+	u8 key_idx, bool pairwise, const u8 *mac_addr, void *cookie,
+	void (*callback) (void *cookie, struct key_params * params))
+{
+	struct key_params params;
+	struct wl_wsec_key key;
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	struct wl_security *sec;
+	s32 wsec;
+	s32 err = 0;
+	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
+
+	WL_DBG(("key index (%d)\n", key_idx));
+	CHECK_SYS_UP(wl);
+	memset(&key, 0, sizeof(key));
+	key.index = key_idx;
+	swap_key_to_BE(&key);
+	memset(&params, 0, sizeof(params));
+	params.key_len = (u8) min_t(u8, DOT11_MAX_KEY_SIZE, key.len);
+	memcpy(params.key, key.data, params.key_len);
+
+	wldev_iovar_getint_bsscfg(dev, "wsec", &wsec, bssidx);
+	if (unlikely(err)) {
+		WL_ERR(("WLC_GET_WSEC error (%d)\n", err));
+		return err;
+	}
+	switch (wsec & ~SES_OW_ENABLED) {
+		case WEP_ENABLED:
+			sec = wl_read_prof(wl, dev, WL_PROF_SEC);
+			if (sec->cipher_pairwise & WLAN_CIPHER_SUITE_WEP40) {
+				params.cipher = WLAN_CIPHER_SUITE_WEP40;
+				WL_DBG(("WLAN_CIPHER_SUITE_WEP40\n"));
+			} else if (sec->cipher_pairwise & WLAN_CIPHER_SUITE_WEP104) {
+				params.cipher = WLAN_CIPHER_SUITE_WEP104;
+				WL_DBG(("WLAN_CIPHER_SUITE_WEP104\n"));
+			}
+			break;
+		case TKIP_ENABLED:
+			params.cipher = WLAN_CIPHER_SUITE_TKIP;
+			WL_DBG(("WLAN_CIPHER_SUITE_TKIP\n"));
+			break;
+		case AES_ENABLED:
+			params.cipher = WLAN_CIPHER_SUITE_AES_CMAC;
+			WL_DBG(("WLAN_CIPHER_SUITE_AES_CMAC\n"));
+			break;
+		default:
+			WL_ERR(("Invalid algo (0x%x)\n", wsec));
+			return -EINVAL;
+	}
+
+	callback(cookie, &params);
+	return err;
+}
+
+static s32
+wl_cfg80211_config_default_mgmt_key(struct wiphy *wiphy,
+	struct net_device *dev, u8 key_idx)
+{
+	WL_INFO(("Not supported\n"));
+	return -EOPNOTSUPP;
+}
+
+static s32
+wl_cfg80211_get_station(struct wiphy *wiphy, struct net_device *dev,
+	u8 *mac, struct station_info *sinfo)
+{
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	scb_val_t scb_val;
+	s32 rssi;
+	s32 rate;
+	s32 err = 0;
+	sta_info_t *sta;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 0, 0)
+	s8 eabuf[ETHER_ADDR_STR_LEN];
+#endif
+	dhd_pub_t *dhd =  (dhd_pub_t *)(wl->pub);
+
+	CHECK_SYS_UP(wl);
+	if (wl_get_mode_by_netdev(wl, dev) == WL_MODE_AP) {
+		err = wldev_iovar_getbuf(dev, "sta_info", (struct ether_addr *)mac,
+			ETHER_ADDR_LEN, wl->ioctl_buf, WLC_IOCTL_MAXLEN, &wl->ioctl_buf_sync);
+		if (err < 0) {
+			WL_ERR(("GET STA INFO failed, %d\n", err));
+			return err;
+		}
+		sinfo->filled = STATION_INFO_INACTIVE_TIME;
+		sta = (sta_info_t *)wl->ioctl_buf;
+		sta->len = dtoh16(sta->len);
+		sta->cap = dtoh16(sta->cap);
+		sta->flags = dtoh32(sta->flags);
+		sta->idle = dtoh32(sta->idle);
+		sta->in = dtoh32(sta->in);
+		sinfo->inactive_time = sta->idle * 1000;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 0, 0)
+		if (sta->flags & WL_STA_ASSOC) {
+			sinfo->filled |= STATION_INFO_CONNECTED_TIME;
+			sinfo->connected_time = sta->in;
+		}
+		WL_INFO(("STA %s : idle time : %d sec, connected time :%d ms\n",
+			bcm_ether_ntoa((const struct ether_addr *)mac, eabuf), sinfo->inactive_time,
+			sta->idle * 1000));
+#endif
+	} else if (wl_get_mode_by_netdev(wl, dev) == WL_MODE_BSS) {
+		u8 *curmacp = wl_read_prof(wl, dev, WL_PROF_BSSID);
+		err = -ENODEV;
+		if (!wl_get_drv_status(wl, CONNECTED, dev) ||
+			(dhd_is_associated(dhd, NULL, &err) == FALSE)) {
+			WL_ERR(("NOT assoc: %d\n", err));
+			goto get_station_err;
+		}
+		if (memcmp(mac, curmacp, ETHER_ADDR_LEN)) {
+			WL_ERR(("Wrong Mac address: "MACSTR" != "MACSTR"\n",
+				MAC2STR(mac), MAC2STR(curmacp)));
+		}
+
+		/* Report the current tx rate */
+		err = wldev_ioctl(dev, WLC_GET_RATE, &rate, sizeof(rate), false);
+		if (err) {
+			WL_ERR(("Could not get rate (%d)\n", err));
+		} else {
+			rate = dtoh32(rate);
+			sinfo->filled |= STATION_INFO_TX_BITRATE;
+			sinfo->txrate.legacy = rate * 5;
+			WL_DBG(("Rate %d Mbps\n", (rate / 2)));
+		}
+
+		memset(&scb_val, 0, sizeof(scb_val));
+		scb_val.val = 0;
+		err = wldev_ioctl(dev, WLC_GET_RSSI, &scb_val,
+			sizeof(scb_val_t), false);
+		if (err) {
+			WL_ERR(("Could not get rssi (%d)\n", err));
+			goto get_station_err;
+		}
+		rssi = dtoh32(scb_val.val);
+		sinfo->filled |= STATION_INFO_SIGNAL;
+		sinfo->signal = rssi;
+		WL_DBG(("RSSI %d dBm\n", rssi));
+
+get_station_err:
+		if (err && (err != -ETIMEDOUT) && (err != -EIO)) {
+			/* Disconnect due to zero BSSID or error to get RSSI */
+			WL_ERR(("force cfg80211_disconnected: %d\n", err));
+			wl_clr_drv_status(wl, CONNECTED, dev);
+			cfg80211_disconnected(dev, 0, NULL, 0, GFP_KERNEL);
+			wl_link_down(wl);
+		}
+	}
+
+	return err;
+}
+
+static s32
+wl_cfg80211_set_power_mgmt(struct wiphy *wiphy, struct net_device *dev,
+	bool enabled, s32 timeout)
+{
+	s32 pm;
+	s32 err = 0;
+	struct wl_priv *wl = wiphy_priv(wiphy);
+
+	CHECK_SYS_UP(wl);
+
+	WL_DBG(("Enter : power save %s\n", (enabled ? "enable" : "disable")));
+	if (wl->p2p_net == dev) {
+		return err;
+	}
+
+	pm = enabled ? PM_FAST : PM_OFF;
+	pm = htod32(pm);
+	err = wldev_ioctl(dev, WLC_SET_PM, &pm, sizeof(pm), true);
+	if (unlikely(err)) {
+		if (err == -ENODEV)
+			WL_DBG(("net_device is not ready yet\n"));
+		else
+			WL_ERR(("error (%d)\n", err));
+		return err;
+	}
+	WL_DBG(("power save %s\n", (pm ? "enabled" : "disabled")));
+	return err;
+}
+
+static __used u32 wl_find_msb(u16 bit16)
+{
+	u32 ret = 0;
+
+	if (bit16 & 0xff00) {
+		ret += 8;
+		bit16 >>= 8;
+	}
+
+	if (bit16 & 0xf0) {
+		ret += 4;
+		bit16 >>= 4;
+	}
+
+	if (bit16 & 0xc) {
+		ret += 2;
+		bit16 >>= 2;
+	}
+
+	if (bit16 & 2)
+		ret += bit16 & 2;
+	else if (bit16)
+		ret += bit16;
+
+	return ret;
+}
+
+static s32 wl_cfg80211_resume(struct wiphy *wiphy)
+{
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	struct net_device *ndev = wl_to_prmry_ndev(wl);
+	s32 err = 0;
+
+	if (unlikely(!wl_get_drv_status(wl, READY, ndev))) {
+		WL_INFO(("device is not ready\n"));
+		return 0;
+	}
+
+	wl_invoke_iscan(wl);
+
+	return err;
+}
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 39)
+static s32 wl_cfg80211_suspend(struct wiphy *wiphy, struct cfg80211_wowlan *wow)
+#else
+static s32 wl_cfg80211_suspend(struct wiphy *wiphy)
+#endif
+{
+#ifdef DHD_CLEAR_ON_SUSPEND
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	struct net_info *iter, *next;
+	struct net_device *ndev = wl_to_prmry_ndev(wl);
+	unsigned long flags;
+
+	if (unlikely(!wl_get_drv_status(wl, READY, ndev))) {
+		WL_INFO(("device is not ready : status (%d)\n",
+			(int)wl->status));
+		return 0;
+	}
+	for_each_ndev(wl, iter, next)
+		wl_set_drv_status(wl, SCAN_ABORTING, iter->ndev);
+	wl_term_iscan(wl);
+	spin_lock_irqsave(&wl->cfgdrv_lock, flags);
+	if (wl->scan_request) {
+		cfg80211_scan_done(wl->scan_request, true);
+		wl->scan_request = NULL;
+	}
+	for_each_ndev(wl, iter, next) {
+		wl_clr_drv_status(wl, SCANNING, iter->ndev);
+		wl_clr_drv_status(wl, SCAN_ABORTING, iter->ndev);
+	}
+	spin_unlock_irqrestore(&wl->cfgdrv_lock, flags);
+	for_each_ndev(wl, iter, next) {
+		if (wl_get_drv_status(wl, CONNECTING, iter->ndev)) {
+			wl_bss_connect_done(wl, iter->ndev, NULL, NULL, false);
+		}
+	}
+#endif /* DHD_CLEAR_ON_SUSPEND */
+	return 0;
+}
+
+static s32
+wl_update_pmklist(struct net_device *dev, struct wl_pmk_list *pmk_list,
+	s32 err)
+{
+	int i, j;
+	struct wl_priv *wl = wlcfg_drv_priv;
+	struct net_device *primary_dev = wl_to_prmry_ndev(wl);
+
+	if (!pmk_list) {
+		printk("pmk_list is NULL\n");
+		return -EINVAL;
+	}
+	/* pmk list is supported only for STA interface i.e. primary interface
+	 * Refer code wlc_bsscfg.c->wlc_bsscfg_sta_init
+	 */
+	if (primary_dev != dev) {
+		WL_INFO(("Not supporting Flushing pmklist on virtual"
+			" interfaces than primary interface\n"));
+		return err;
+	}
+
+	WL_DBG(("No of elements %d\n", pmk_list->pmkids.npmkid));
+	for (i = 0; i < pmk_list->pmkids.npmkid; i++) {
+		WL_DBG(("PMKID[%d]: %pM =\n", i,
+			&pmk_list->pmkids.pmkid[i].BSSID));
+		for (j = 0; j < WPA2_PMKID_LEN; j++) {
+			WL_DBG(("%02x\n", pmk_list->pmkids.pmkid[i].PMKID[j]));
+		}
+	}
+	if (likely(!err)) {
+		err = wldev_iovar_setbuf(dev, "pmkid_info", (char *)pmk_list,
+			sizeof(*pmk_list), wl->ioctl_buf, WLC_IOCTL_MAXLEN, NULL);
+	}
+
+	return err;
+}
+
+static s32
+wl_cfg80211_set_pmksa(struct wiphy *wiphy, struct net_device *dev,
+	struct cfg80211_pmksa *pmksa)
+{
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	s32 err = 0;
+	int i;
+
+	CHECK_SYS_UP(wl);
+	for (i = 0; i < wl->pmk_list->pmkids.npmkid; i++)
+		if (!memcmp(pmksa->bssid, &wl->pmk_list->pmkids.pmkid[i].BSSID,
+			ETHER_ADDR_LEN))
+			break;
+	if (i < WL_NUM_PMKIDS_MAX) {
+		memcpy(&wl->pmk_list->pmkids.pmkid[i].BSSID, pmksa->bssid,
+			ETHER_ADDR_LEN);
+		memcpy(&wl->pmk_list->pmkids.pmkid[i].PMKID, pmksa->pmkid,
+			WPA2_PMKID_LEN);
+		if (i == wl->pmk_list->pmkids.npmkid)
+			wl->pmk_list->pmkids.npmkid++;
+	} else {
+		err = -EINVAL;
+	}
+	WL_DBG(("set_pmksa,IW_PMKSA_ADD - PMKID: %pM =\n",
+		&wl->pmk_list->pmkids.pmkid[wl->pmk_list->pmkids.npmkid - 1].BSSID));
+	for (i = 0; i < WPA2_PMKID_LEN; i++) {
+		WL_DBG(("%02x\n",
+			wl->pmk_list->pmkids.pmkid[wl->pmk_list->pmkids.npmkid - 1].
+			PMKID[i]));
+	}
+
+	err = wl_update_pmklist(dev, wl->pmk_list, err);
+
+	return err;
+}
+
+static s32
+wl_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *dev,
+	struct cfg80211_pmksa *pmksa)
+{
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	struct _pmkid_list pmkid;
+	s32 err = 0;
+	int i;
+
+	CHECK_SYS_UP(wl);
+	memcpy(&pmkid.pmkid[0].BSSID, pmksa->bssid, ETHER_ADDR_LEN);
+	memcpy(&pmkid.pmkid[0].PMKID, pmksa->pmkid, WPA2_PMKID_LEN);
+
+	WL_DBG(("del_pmksa,IW_PMKSA_REMOVE - PMKID: %pM =\n",
+		&pmkid.pmkid[0].BSSID));
+	for (i = 0; i < WPA2_PMKID_LEN; i++) {
+		WL_DBG(("%02x\n", pmkid.pmkid[0].PMKID[i]));
+	}
+
+	for (i = 0; i < wl->pmk_list->pmkids.npmkid; i++)
+		if (!memcmp
+		    (pmksa->bssid, &wl->pmk_list->pmkids.pmkid[i].BSSID,
+		     ETHER_ADDR_LEN))
+			break;
+
+	if ((wl->pmk_list->pmkids.npmkid > 0) &&
+		(i < wl->pmk_list->pmkids.npmkid)) {
+		memset(&wl->pmk_list->pmkids.pmkid[i], 0, sizeof(pmkid_t));
+		for (; i < (wl->pmk_list->pmkids.npmkid - 1); i++) {
+			memcpy(&wl->pmk_list->pmkids.pmkid[i].BSSID,
+				&wl->pmk_list->pmkids.pmkid[i + 1].BSSID,
+				ETHER_ADDR_LEN);
+			memcpy(&wl->pmk_list->pmkids.pmkid[i].PMKID,
+				&wl->pmk_list->pmkids.pmkid[i + 1].PMKID,
+				WPA2_PMKID_LEN);
+		}
+		wl->pmk_list->pmkids.npmkid--;
+	} else {
+		err = -EINVAL;
+	}
+
+	err = wl_update_pmklist(dev, wl->pmk_list, err);
+
+	return err;
+
+}
+
+static s32
+wl_cfg80211_flush_pmksa(struct wiphy *wiphy, struct net_device *dev)
+{
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	s32 err = 0;
+	CHECK_SYS_UP(wl);
+	memset(wl->pmk_list, 0, sizeof(*wl->pmk_list));
+	err = wl_update_pmklist(dev, wl->pmk_list, err);
+	return err;
+
+}
+
+static wl_scan_params_t *
+wl_cfg80211_scan_alloc_params(int channel, int nprobes, int *out_params_size)
+{
+	wl_scan_params_t *params;
+	int params_size;
+	int num_chans;
+
+	*out_params_size = 0;
+
+	/* Our scan params only need space for 1 channel and 0 ssids */
+	params_size = WL_SCAN_PARAMS_FIXED_SIZE + 1 * sizeof(uint16);
+	params = (wl_scan_params_t*) kzalloc(params_size, GFP_KERNEL);
+	if (params == NULL) {
+		WL_ERR(("%s: mem alloc failed (%d bytes)\n", __func__, params_size));
+		return params;
+	}
+	memset(params, 0, params_size);
+	params->nprobes = nprobes;
+
+	num_chans = (channel == 0) ? 0 : 1;
+
+	memcpy(&params->bssid, &ether_bcast, ETHER_ADDR_LEN);
+	params->bss_type = DOT11_BSSTYPE_ANY;
+	params->scan_type = DOT11_SCANTYPE_ACTIVE;
+	params->nprobes = htod32(1);
+	params->active_time = htod32(-1);
+	params->passive_time = htod32(-1);
+	params->home_time = htod32(10);
+	params->channel_list[0] = htodchanspec(channel);
+
+	/* Our scan params have 1 channel and 0 ssids */
+	params->channel_num = htod32((0 << WL_SCAN_PARAMS_NSSID_SHIFT) |
+		(num_chans & WL_SCAN_PARAMS_COUNT_MASK));
+
+	*out_params_size = params_size;	/* rtn size to the caller */
+	return params;
+}
+
+static s32
+wl_cfg80211_remain_on_channel(struct wiphy *wiphy, struct net_device *dev,
+	struct ieee80211_channel * channel,
+	enum nl80211_channel_type channel_type,
+	unsigned int duration, u64 *cookie)
+{
+	s32 target_channel;
+	u32 id;
+	struct ether_addr primary_mac;
+	struct net_device *ndev = NULL;
+
+	s32 err = BCME_OK;
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	WL_DBG(("Enter, netdev_ifidx: %d \n", dev->ifindex));
+
+	if (wl->p2p_net == dev) {
+		ndev = wl_to_prmry_ndev(wl);
+	} else {
+		ndev = dev;
+	}
+
+	if (wl_get_drv_status(wl, SCANNING, ndev)) {
+		wl_notify_escan_complete(wl, ndev, true, true);
+	}
+	target_channel = ieee80211_frequency_to_channel(channel->center_freq);
+	memcpy(&wl->remain_on_chan, channel, sizeof(struct ieee80211_channel));
+	wl->remain_on_chan_type = channel_type;
+	id = ++wl->last_roc_id;
+	if (id == 0)
+		id = ++wl->last_roc_id;
+	*cookie = id;
+	cfg80211_ready_on_channel(dev, *cookie, channel,
+		channel_type, duration, GFP_KERNEL);
+	if (wl->p2p && !wl->p2p->on) {
+		get_primary_mac(wl, &primary_mac);
+		wl_cfgp2p_generate_bss_mac(&primary_mac, &wl->p2p->dev_addr, &wl->p2p->int_addr);
+
+		/* In case of p2p_listen command, supplicant send remain_on_channel
+		 * without turning on P2P
+		 */
+
+		p2p_on(wl) = true;
+		err = wl_cfgp2p_enable_discovery(wl, ndev, NULL, 0);
+
+		if (unlikely(err)) {
+			goto exit;
+		}
+	}
+	if (p2p_is_on(wl))
+		wl_cfgp2p_discover_listen(wl, target_channel, duration);
+
+
+exit:
+	return err;
+}
+
+static s32
+wl_cfg80211_cancel_remain_on_channel(struct wiphy *wiphy, struct net_device *dev,
+	u64 cookie)
+{
+	s32 err = 0;
+	WL_DBG((" enter ) netdev_ifidx: %d \n", dev->ifindex));
+	return err;
+}
+
+static s32
+wl_cfg80211_send_pending_tx_act_frm(struct wl_priv *wl)
+{
+	wl_af_params_t *tx_act_frm;
+	struct net_device *dev = wl->afx_hdl->dev;
+	if (!p2p_is_on(wl))
+		return -1;
+
+	if (dev == wl->p2p_net) {
+		dev = wl_to_prmry_ndev(wl);
+	}
+
+	tx_act_frm = wl->afx_hdl->pending_tx_act_frm;
+	WL_DBG(("Sending the action frame\n"));
+	wl->afx_hdl->pending_tx_act_frm = NULL;
+	if (tx_act_frm != NULL) {
+		/* Suspend P2P discovery's search-listen to prevent it from
+		 * starting a scan or changing the channel.
+		 */
+		wl_clr_drv_status(wl, SENDING_ACT_FRM, wl->afx_hdl->dev);
+		wl_clr_drv_status(wl, SCANNING, wl->afx_hdl->dev);
+		wl_notify_escan_complete(wl, dev, true, true);
+		wl_cfgp2p_discover_enable_search(wl, false);
+		tx_act_frm->channel = wl->afx_hdl->peer_chan;
+		wl->afx_hdl->ack_recv = (wl_cfgp2p_tx_action_frame(wl, dev,
+			tx_act_frm, wl->afx_hdl->bssidx)) ? false : true;
+	}
+	return 0;
+}
+static void
+wl_cfg80211_afx_handler(struct work_struct *work)
+{
+
+	struct afx_hdl *afx_instance;
+	struct wl_priv *wl = wlcfg_drv_priv;
+	afx_instance = container_of(work, struct afx_hdl, work);
+	if (afx_instance != NULL) {
+		wl_cfgp2p_act_frm_search(wl, wl->afx_hdl->dev,
+			wl->afx_hdl->bssidx, 0);
+	}
+}
+
+static bool
+wl_cfg80211_send_at_common_channel(struct wl_priv *wl,
+	struct net_device *dev,
+	wl_af_params_t *af_params)
+{
+	WL_DBG((" enter ) \n"));
+	/* initialize afx_hdl */
+	wl->afx_hdl->pending_tx_act_frm = af_params;
+	wl->afx_hdl->bssidx = wl_cfgp2p_find_idx(wl, dev);
+	wl->afx_hdl->dev = dev;
+	wl->afx_hdl->retry = 0;
+	wl->afx_hdl->peer_chan = WL_INVALID;
+	wl->afx_hdl->ack_recv = false;
+	memcpy(wl->afx_hdl->pending_tx_dst_addr.octet,
+		af_params->action_frame.da.octet,
+		sizeof(wl->afx_hdl->pending_tx_dst_addr.octet));
+	/* Loop to wait until we have sent the pending tx action frame or the
+	 * pending action frame tx is cancelled.
+	 */
+	while ((wl->afx_hdl->retry < WL_CHANNEL_SYNC_RETRY) &&
+		(wl->afx_hdl->peer_chan == WL_INVALID)) {
+		wl_set_drv_status(wl, SENDING_ACT_FRM, dev);
+		wl_set_drv_status(wl, SCANNING, dev);
+		WL_DBG(("Scheduling the action frame for sending.. retry %d\n",
+			wl->afx_hdl->retry));
+		/* Do find_peer_for_action */
+		schedule_work(&wl->afx_hdl->work);
+		wait_for_completion(&wl->act_frm_scan);
+		wl->afx_hdl->retry++;
+	}
+	if (wl->afx_hdl->peer_chan != WL_INVALID)
+		wl_cfg80211_send_pending_tx_act_frm(wl);
+	else {
+		WL_ERR(("Couldn't find the peer " MACSTR " after %d retries\n",
+			MAC2STR(wl->afx_hdl->pending_tx_dst_addr.octet), wl->afx_hdl->retry));
+	}
+	wl->afx_hdl->dev = NULL;
+	wl->afx_hdl->bssidx = WL_INVALID;
+	wl_clr_drv_status(wl, SENDING_ACT_FRM, dev);
+	if (wl->afx_hdl->ack_recv)
+		return true; /* ACK */
+	else
+		return false; /* NO ACK */
+}
+
+static s32
+wl_cfg80211_mgmt_tx(struct wiphy *wiphy, struct net_device *ndev,
+	struct ieee80211_channel *channel, bool offchan,
+	enum nl80211_channel_type channel_type,
+	bool channel_type_valid, unsigned int wait,
+	const u8* buf, size_t len,
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 2, 0)
+	bool no_cck,
+#endif
+	u64 *cookie)
+{
+	wl_action_frame_t *action_frame;
+	wl_af_params_t *af_params;
+	wifi_p2p_ie_t *p2p_ie;
+	wpa_ie_fixed_t *wps_ie;
+#ifdef WLWFDIE
+	wifi_wfd_ie_t *wfd_ie;
+#endif /* WLWFDIE */
+	scb_val_t scb_val;
+	const struct ieee80211_mgmt *mgmt;
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	struct net_device *dev = NULL;
+	s32 err = BCME_OK;
+	s32 bssidx = 0;
+	u32 p2pie_len = 0;
+	u32 wpsie_len = 0;
+#ifdef WLWFDIE
+	u32 wfdie_len = 0;
+#endif /* WLWFDIE */
+	u32 id;
+	u32 retry = 0;
+	bool ack = false;
+	wifi_p2p_pub_act_frame_t *act_frm = NULL;
+	wifi_p2p_action_frame_t *p2p_act_frm = NULL;
+	wifi_p2psd_gas_pub_act_frame_t *sd_act_frm = NULL;
+	s8 eabuf[ETHER_ADDR_STR_LEN];
+	int retry_cnt = 0;
+
+	WL_DBG(("Enter \n"));
+
+	if (ndev == wl->p2p_net) {
+		dev = wl_to_prmry_ndev(wl);
+	} else {
+		/* If TX req is for any valid ifidx. Use as is */
+		dev = ndev;
+	}
+
+	/* find bssidx based on ndev */
+	bssidx = wl_cfgp2p_find_idx(wl, dev);
+	if (bssidx == -1) {
+
+		WL_ERR(("Can not find the bssidx for dev( %p )\n", dev));
+		return -ENODEV;
+	}
+	if (p2p_is_on(wl)) {
+		/* Suspend P2P discovery search-listen to prevent it from changing the
+		 * channel.
+		 */
+		if ((err = wl_cfgp2p_discover_enable_search(wl, false)) < 0) {
+			WL_ERR(("Can not disable discovery mode\n"));
+			return -EFAULT;
+		}
+	}
+	*cookie = 0;
+	id = wl->send_action_id++;
+	if (id == 0)
+		id = wl->send_action_id++;
+	*cookie = id;
+	mgmt = (const struct ieee80211_mgmt *)buf;
+	if (ieee80211_is_mgmt(mgmt->frame_control)) {
+		if (ieee80211_is_probe_resp(mgmt->frame_control)) {
+			s32 ie_offset =  DOT11_MGMT_HDR_LEN + DOT11_BCN_PRB_FIXED_LEN;
+			s32 ie_len = len - ie_offset;
+			if ((p2p_ie = wl_cfgp2p_find_p2pie((u8 *)(buf + ie_offset), ie_len))
+				!= NULL) {
+				/* Total length of P2P Information Element */
+				p2pie_len = p2p_ie->len + sizeof(p2p_ie->len) + sizeof(p2p_ie->id);
+			}
+#ifdef WLWFDIE
+			if ((wfd_ie = wl_cfgp2p_find_wfdie((u8 *)(buf + ie_offset), ie_len))
+				!= NULL) {
+				/* Total length of WFD Information Element */
+				wfdie_len = wfd_ie->len + sizeof(wfd_ie->len) + sizeof(wfd_ie->id);
+			}
+#endif /* WLWFDIE */
+			if ((wps_ie = wl_cfgp2p_find_wpsie((u8 *)(buf + ie_offset), ie_len))
+				!= NULL) {
+				/* Order of Vendor IE is 1) WPS IE +
+				 * 2) P2P IE created by supplicant
+				 *  So, it is ok to find start address of WPS IE
+				 *  to save IEs
+				 */
+				wpsie_len = wps_ie->length + sizeof(wps_ie->length) +
+					sizeof(wps_ie->tag);
+				wl_cfgp2p_set_management_ie(wl, dev, bssidx,
+					VNDR_IE_PRBRSP_FLAG,
+#ifdef WLWFDIE
+					(u8 *)wps_ie, wpsie_len + p2pie_len + wfdie_len);
+#else
+					(u8 *)wps_ie, wpsie_len + p2pie_len);
+#endif /* WLWFDIE */
+			}
+			cfg80211_mgmt_tx_status(ndev, *cookie, buf, len, true, GFP_KERNEL);
+			goto exit;
+		} else if (ieee80211_is_disassoc(mgmt->frame_control) ||
+			ieee80211_is_deauth(mgmt->frame_control)) {
+			memcpy(scb_val.ea.octet, mgmt->da, ETH_ALEN);
+			scb_val.val = mgmt->u.disassoc.reason_code;
+			wldev_ioctl(dev, WLC_SCB_DEAUTHENTICATE_FOR_REASON, &scb_val,
+				sizeof(scb_val_t), true);
+			WL_DBG(("Disconnect STA : %s scb_val.val %d\n",
+				bcm_ether_ntoa((const struct ether_addr *)mgmt->da, eabuf),
+				scb_val.val));
+			/* Wait for the deauth event to come, supplicant will do the
+			 * delete iface immediately and we will have problem in sending
+			 * deauth frame if we delete the bss in firmware
+			 */
+			wl_delay(400);
+			cfg80211_mgmt_tx_status(ndev, *cookie, buf, len, true, GFP_KERNEL);
+			goto exit;
+
+		} else if (ieee80211_is_action(mgmt->frame_control)) {
+			/* Abort the dwell time of any previous off-channel
+			* action frame that may be still in effect.  Sending
+			* off-channel action frames relies on the driver's
+			* scan engine.  If a previous off-channel action frame
+			* tx is still in progress (including the dwell time),
+			* then this new action frame will not be sent out.
+			*/
+			wl_notify_escan_complete(wl, dev, true, true);
+
+		}
+
+	} else {
+		WL_ERR(("Driver only allows MGMT packet type\n"));
+		goto exit;
+	}
+
+	af_params = (wl_af_params_t *) kzalloc(WL_WIFI_AF_PARAMS_SIZE, GFP_KERNEL);
+
+	if (af_params == NULL)
+	{
+		WL_ERR(("unable to allocate frame\n"));
+		return -ENOMEM;
+	}
+
+	action_frame = &af_params->action_frame;
+
+	/* Add the packet Id */
+	action_frame->packetId = *cookie;
+	WL_DBG(("action frame %d\n", action_frame->packetId));
+	/* Add BSSID */
+	memcpy(&action_frame->da, &mgmt->da[0], ETHER_ADDR_LEN);
+	memcpy(&af_params->BSSID, &mgmt->bssid[0], ETHER_ADDR_LEN);
+
+	/* Add the length exepted for 802.11 header  */
+	action_frame->len = len - DOT11_MGMT_HDR_LEN;
+	WL_DBG(("action_frame->len: %d\n", action_frame->len));
+
+	/* Add the channel */
+	af_params->channel =
+		ieee80211_frequency_to_channel(channel->center_freq);
+
+	if (channel->band == IEEE80211_BAND_5GHZ) {
+		WL_DBG(("5GHz channel %d", af_params->channel));
+		err = wldev_ioctl(dev, WLC_SET_CHANNEL,
+			&af_params->channel, sizeof(af_params->channel), true);
+		if (err < 0) {
+			WL_ERR(("WLC_SET_CHANNEL error %d\n", err));
+		}
+	}
+
+	/* Add the dwell time
+	 * Dwell time to stay off-channel to wait for a response action frame
+	 * after transmitting an GO Negotiation action frame
+	 */
+	af_params->dwell_time = WL_DWELL_TIME;
+
+	memcpy(action_frame->data, &buf[DOT11_MGMT_HDR_LEN], action_frame->len);
+	if (wl_cfgp2p_is_pub_action(action_frame->data, action_frame->len)) {
+		act_frm = (wifi_p2p_pub_act_frame_t *) (action_frame->data);
+		WL_DBG(("P2P PUB action_frame->len: %d chan %d category %d subtype %d\n",
+			action_frame->len, af_params->channel,
+			act_frm->category, act_frm->subtype));
+		if (act_frm && ((act_frm->subtype == P2P_PAF_GON_REQ) ||
+		  (act_frm->subtype == P2P_PAF_GON_RSP) ||
+		  (act_frm->subtype == P2P_PAF_GON_CONF) ||
+		  (act_frm->subtype == P2P_PAF_PROVDIS_REQ))) {
+			wldev_iovar_setint(dev, "mpc", 0);
+		}
+
+		if (act_frm->subtype == P2P_PAF_GON_REQ) {
+			WL_DBG(("P2P: GO_NEG_PHASE status set \n"));
+			wl_set_p2p_status(wl, GO_NEG_PHASE);
+		} else if (act_frm->subtype == P2P_PAF_GON_CONF) {
+			/* If we reached till GO Neg confirmation
+			 * reset the filter
+			 */
+			WL_DBG(("P2P: GO_NEG_PHASE status cleared \n"));
+			wl_clr_p2p_status(wl, GO_NEG_PHASE);
+		}
+
+		if (act_frm->subtype == P2P_PAF_GON_RSP)
+			retry_cnt = 1;
+		else retry_cnt = WL_ACT_FRAME_RETRY;
+
+		if (act_frm && act_frm->subtype == P2P_PAF_DEVDIS_REQ) {
+			af_params->dwell_time = WL_LONG_DWELL_TIME;
+		} else if (act_frm &&
+			(act_frm->subtype == P2P_PAF_PROVDIS_REQ ||
+			act_frm->subtype == P2P_PAF_PROVDIS_RSP ||
+			act_frm->subtype == P2P_PAF_GON_RSP)) {
+			af_params->dwell_time = WL_MED_DWELL_TIME;
+		}
+	} else if (wl_cfgp2p_is_p2p_action(action_frame->data, action_frame->len)) {
+		p2p_act_frm = (wifi_p2p_action_frame_t *) (action_frame->data);
+		WL_DBG(("P2P action_frame->len: %d chan %d category %d subtype %d\n",
+			action_frame->len, af_params->channel,
+			p2p_act_frm->category, p2p_act_frm->subtype));
+	} else if (wl_cfgp2p_is_gas_action(action_frame->data, action_frame->len)) {
+		sd_act_frm = (wifi_p2psd_gas_pub_act_frame_t *) (action_frame->data);
+		WL_DBG(("Service Discovery action_frame->len: %d chan %d category %d action %d\n",
+			action_frame->len, af_params->channel,
+			sd_act_frm->category, sd_act_frm->action));
+		af_params->dwell_time = WL_MED_DWELL_TIME;
+		retry_cnt = WL_ACT_FRAME_RETRY;
+	}
+	wl_cfgp2p_print_actframe(true, action_frame->data, action_frame->len);
+	/*
+	 * To make sure to send successfully action frame, we have to turn off mpc
+	 */
+	if (IS_P2P_SOCIAL(af_params->channel) &&
+		(IS_P2P_PUB_ACT_REQ(act_frm, &act_frm->elts[0], action_frame->len) ||
+		IS_GAS_REQ(sd_act_frm, action_frame->len)) &&
+		wl_to_p2p_bss_saved_ie(wl, P2PAPI_BSSCFG_DEVICE).p2p_probe_req_ie_len) {
+		/* channel offload require P2P IE for Probe request
+		 * otherwise, we will use wl_cfgp2p_tx_action_frame directly.
+		 * channel offload for action request frame
+		 */
+
+		/* channel offload for action request frame */
+		ack = wl_cfg80211_send_at_common_channel(wl, dev, af_params);
+		/* We need to retry Service discovery frames as they don't get retried immediately by supplicant*/
+		if ((!ack) && (IS_GAS_REQ(sd_act_frm, action_frame->len))) {
+			for (retry = 1; retry < retry_cnt; retry++) {
+				WL_DBG(("Service Discovery action_frame retry %d len: %d chan %d category %d action %d\n",
+					retry, action_frame->len, af_params->channel,
+					sd_act_frm->category, sd_act_frm->action));
+				ack = (wl_cfgp2p_tx_action_frame(wl, dev,
+					af_params, bssidx)) ? false : true;
+				if (ack)
+					break;
+			}
+		}
+	} else {
+		ack = (wl_cfgp2p_tx_action_frame(wl, dev, af_params, bssidx)) ? false : true;
+		if (!ack) {
+				for (retry = 1; retry < retry_cnt; retry++) {
+					ack = (wl_cfgp2p_tx_action_frame(wl, dev,
+						af_params, bssidx)) ? false : true;
+					if (ack)
+						break;
+				}
+		}
+
+	}
+	cfg80211_mgmt_tx_status(ndev, *cookie, buf, len, ack, GFP_KERNEL);
+	if (act_frm && act_frm->subtype == P2P_PAF_GON_CONF) {
+		wldev_iovar_setint(dev, "mpc", 1);
+	}
+	kfree(af_params);
+exit:
+	return err;
+}
+
+
+static void
+wl_cfg80211_mgmt_frame_register(struct wiphy *wiphy, struct net_device *dev,
+	u16 frame_type, bool reg)
+{
+
+	WL_DBG(("%s: frame_type: %x, reg: %d\n", __func__, frame_type, reg));
+
+	if (frame_type != (IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_PROBE_REQ))
+		return;
+
+	return;
+}
+
+
+static s32
+wl_cfg80211_change_bss(struct wiphy *wiphy,
+	struct net_device *dev,
+	struct bss_parameters *params)
+{
+	if (params->use_cts_prot >= 0) {
+	}
+
+	if (params->use_short_preamble >= 0) {
+	}
+
+	if (params->use_short_slot_time >= 0) {
+	}
+
+	if (params->basic_rates) {
+	}
+
+	if (params->ap_isolate >= 0) {
+	}
+
+	if (params->ht_opmode >= 0) {
+	}
+
+	return 0;
+}
+
+static s32
+wl_cfg80211_set_channel(struct wiphy *wiphy, struct net_device *dev,
+	struct ieee80211_channel *chan,
+	enum nl80211_channel_type channel_type)
+{
+	s32 channel;
+	s32 err = BCME_OK;
+	struct wl_priv *wl = wiphy_priv(wiphy);
+
+	if (wl->p2p_net == dev) {
+		dev = wl_to_prmry_ndev(wl);
+	}
+	channel = ieee80211_frequency_to_channel(chan->center_freq);
+
+	if (wl_get_drv_status(wl, AP_CREATING, dev)) {
+		WL_TRACE(("<0> %s: as!!! in AP creating mode, save chan num:%d\n",
+			__FUNCTION__, channel));
+		wl->hostapd_chan = channel;
+		if (channel == 14)
+			return err; /* hostapd requested ch auto-select, will be done later */
+	}
+
+	WL_DBG(("netdev_ifidx(%d), chan_type(%d) target channel(%d) \n",
+		dev->ifindex, channel_type, channel));
+	err = wldev_ioctl(dev, WLC_SET_CHANNEL, &channel, sizeof(channel), true);
+	if (err < 0) {
+		WL_ERR(("WLC_SET_CHANNEL error %d chip may not be supporting this channel\n", err));
+	}
+	return err;
+}
+
+static s32
+wl_validate_wpa2ie(struct net_device *dev, bcm_tlv_t *wpa2ie, s32 bssidx)
+{
+	s32 len = 0;
+	s32 err = BCME_OK;
+	u16 auth = WL_AUTH_OPEN_SYSTEM; /* d11 open authentication */
+	u32 wsec;
+	u32 pval = 0;
+	u32 gval = 0;
+	u32 wpa_auth = 0;
+	u8* tmp;
+	wpa_suite_mcast_t *mcast;
+	wpa_suite_ucast_t *ucast;
+	wpa_suite_auth_key_mgmt_t *mgmt;
+	if (wpa2ie == NULL)
+		goto exit;
+
+	WL_DBG(("Enter \n"));
+	len =  wpa2ie->len;
+	/* check the mcast cipher */
+	mcast = (wpa_suite_mcast_t *)&wpa2ie->data[WPA2_VERSION_LEN];
+	tmp = mcast->oui;
+	switch (tmp[DOT11_OUI_LEN]) {
+		case WPA_CIPHER_NONE:
+			gval = 0;
+			break;
+		case WPA_CIPHER_WEP_40:
+		case WPA_CIPHER_WEP_104:
+			gval = WEP_ENABLED;
+			break;
+		case WPA_CIPHER_TKIP:
+			gval = TKIP_ENABLED;
+			break;
+		case WPA_CIPHER_AES_CCM:
+			gval = AES_ENABLED;
+			break;
+		default:
+			WL_ERR(("No Security Info\n"));
+			break;
+	}
+	len -= WPA_SUITE_LEN;
+	/* check the unicast cipher */
+	ucast = (wpa_suite_ucast_t *)&mcast[1];
+	ltoh16_ua(&ucast->count);
+	tmp = ucast->list[0].oui;
+	switch (tmp[DOT11_OUI_LEN]) {
+		case WPA_CIPHER_NONE:
+			pval = 0;
+			break;
+		case WPA_CIPHER_WEP_40:
+		case WPA_CIPHER_WEP_104:
+			pval = WEP_ENABLED;
+			break;
+		case WPA_CIPHER_TKIP:
+			pval = TKIP_ENABLED;
+			break;
+		case WPA_CIPHER_AES_CCM:
+			pval = AES_ENABLED;
+			break;
+		default:
+			WL_ERR(("No Security Info\n"));
+	}
+	/* FOR WPS , set SEC_OW_ENABLED */
+	wsec = (pval | gval | SES_OW_ENABLED);
+	/* check the AKM */
+	mgmt = (wpa_suite_auth_key_mgmt_t *)&ucast->list[1];
+	ltoh16_ua(&mgmt->count);
+	tmp = (u8 *)&mgmt->list[0];
+	switch (tmp[DOT11_OUI_LEN]) {
+		case RSN_AKM_NONE:
+			wpa_auth = WPA_AUTH_NONE;
+			break;
+		case RSN_AKM_UNSPECIFIED:
+			wpa_auth = WPA2_AUTH_UNSPECIFIED;
+			break;
+		case RSN_AKM_PSK:
+			wpa_auth = WPA2_AUTH_PSK;
+			break;
+		default:
+			WL_ERR(("No Key Mgmt Info\n"));
+	}
+	/* set auth */
+	err = wldev_iovar_setint_bsscfg(dev, "auth", auth, bssidx);
+	if (err < 0) {
+		WL_ERR(("auth error %d\n", err));
+		return BCME_ERROR;
+	}
+	/* set wsec */
+	err = wldev_iovar_setint_bsscfg(dev, "wsec", wsec, bssidx);
+	if (err < 0) {
+		WL_ERR(("wsec error %d\n", err));
+		return BCME_ERROR;
+	}
+	/* set upper-layer auth */
+	err = wldev_iovar_setint_bsscfg(dev, "wpa_auth", wpa_auth, bssidx);
+	if (err < 0) {
+		WL_ERR(("wpa_auth error %d\n", err));
+		return BCME_ERROR;
+	}
+exit:
+	return 0;
+}
+
+static s32
+wl_validate_wpaie(struct net_device *dev, wpa_ie_fixed_t *wpaie, s32 bssidx)
+{
+	wpa_suite_mcast_t *mcast;
+	wpa_suite_ucast_t *ucast;
+	wpa_suite_auth_key_mgmt_t *mgmt;
+	u16 auth = WL_AUTH_OPEN_SYSTEM; /* d11 open authentication */
+	u16 count;
+	s32 err = BCME_OK;
+	s32 len = 0;
+	u32 i;
+	u32 wsec;
+	u32 pval = 0;
+	u32 gval = 0;
+	u32 wpa_auth = 0;
+	u32 tmp = 0;
+
+	if (wpaie == NULL)
+		goto exit;
+	WL_DBG(("Enter \n"));
+	len = wpaie->length;    /* value length */
+	len -= WPA_IE_TAG_FIXED_LEN;
+	/* check for multicast cipher suite */
+	if (len < WPA_SUITE_LEN) {
+		WL_INFO(("no multicast cipher suite\n"));
+		goto exit;
+	}
+
+	/* pick up multicast cipher */
+	mcast = (wpa_suite_mcast_t *)&wpaie[1];
+	len -= WPA_SUITE_LEN;
+	if (!bcmp(mcast->oui, WPA_OUI, WPA_OUI_LEN)) {
+		if (IS_WPA_CIPHER(mcast->type)) {
+			tmp = 0;
+			switch (mcast->type) {
+				case WPA_CIPHER_NONE:
+					tmp = 0;
+					break;
+				case WPA_CIPHER_WEP_40:
+				case WPA_CIPHER_WEP_104:
+					tmp = WEP_ENABLED;
+					break;
+				case WPA_CIPHER_TKIP:
+					tmp = TKIP_ENABLED;
+					break;
+				case WPA_CIPHER_AES_CCM:
+					tmp = AES_ENABLED;
+					break;
+				default:
+					WL_ERR(("No Security Info\n"));
+			}
+			gval |= tmp;
+		}
+	}
+	/* Check for unicast suite(s) */
+	if (len < WPA_IE_SUITE_COUNT_LEN) {
+		WL_INFO(("no unicast suite\n"));
+		goto exit;
+	}
+	/* walk thru unicast cipher list and pick up what we recognize */
+	ucast = (wpa_suite_ucast_t *)&mcast[1];
+	count = ltoh16_ua(&ucast->count);
+	len -= WPA_IE_SUITE_COUNT_LEN;
+	for (i = 0; i < count && len >= WPA_SUITE_LEN;
+		i++, len -= WPA_SUITE_LEN) {
+		if (!bcmp(ucast->list[i].oui, WPA_OUI, WPA_OUI_LEN)) {
+			if (IS_WPA_CIPHER(ucast->list[i].type)) {
+				tmp = 0;
+				switch (ucast->list[i].type) {
+					case WPA_CIPHER_NONE:
+						tmp = 0;
+						break;
+					case WPA_CIPHER_WEP_40:
+					case WPA_CIPHER_WEP_104:
+						tmp = WEP_ENABLED;
+						break;
+					case WPA_CIPHER_TKIP:
+						tmp = TKIP_ENABLED;
+						break;
+					case WPA_CIPHER_AES_CCM:
+						tmp = AES_ENABLED;
+						break;
+					default:
+						WL_ERR(("No Security Info\n"));
+				}
+				pval |= tmp;
+			}
+		}
+	}
+	len -= (count - i) * WPA_SUITE_LEN;
+	/* Check for auth key management suite(s) */
+	if (len < WPA_IE_SUITE_COUNT_LEN) {
+		WL_INFO((" no auth key mgmt suite\n"));
+		goto exit;
+	}
+	/* walk thru auth management suite list and pick up what we recognize */
+	mgmt = (wpa_suite_auth_key_mgmt_t *)&ucast->list[count];
+	count = ltoh16_ua(&mgmt->count);
+	len -= WPA_IE_SUITE_COUNT_LEN;
+	for (i = 0; i < count && len >= WPA_SUITE_LEN;
+		i++, len -= WPA_SUITE_LEN) {
+		if (!bcmp(mgmt->list[i].oui, WPA_OUI, WPA_OUI_LEN)) {
+			if (IS_WPA_AKM(mgmt->list[i].type)) {
+				tmp = 0;
+				switch (mgmt->list[i].type) {
+					case RSN_AKM_NONE:
+						tmp = WPA_AUTH_NONE;
+						break;
+					case RSN_AKM_UNSPECIFIED:
+						tmp = WPA_AUTH_UNSPECIFIED;
+						break;
+					case RSN_AKM_PSK:
+						tmp = WPA_AUTH_PSK;
+						break;
+					default:
+						WL_ERR(("No Key Mgmt Info\n"));
+				}
+				wpa_auth |= tmp;
+			}
+		}
+
+	}
+	/* FOR WPS , set SEC_OW_ENABLED */
+	wsec = (pval | gval | SES_OW_ENABLED);
+	/* set auth */
+	err = wldev_iovar_setint_bsscfg(dev, "auth", auth, bssidx);
+	if (err < 0) {
+		WL_ERR(("auth error %d\n", err));
+		return BCME_ERROR;
+	}
+	/* set wsec */
+	err = wldev_iovar_setint_bsscfg(dev, "wsec", wsec, bssidx);
+	if (err < 0) {
+		WL_ERR(("wsec error %d\n", err));
+		return BCME_ERROR;
+	}
+	/* set upper-layer auth */
+	err = wldev_iovar_setint_bsscfg(dev, "wpa_auth", wpa_auth, bssidx);
+	if (err < 0) {
+		WL_ERR(("wpa_auth error %d\n", err));
+		return BCME_ERROR;
+	}
+exit:
+	return 0;
+}
+
+static s32
+wl_cfg80211_add_set_beacon(struct wiphy *wiphy, struct net_device *dev,
+	struct beacon_parameters *info)
+{
+	s32 err = BCME_OK;
+	bcm_tlv_t *ssid_ie;
+	wlc_ssid_t ssid;
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	struct wl_join_params join_params;
+	wpa_ie_fixed_t *wps_ie;
+	wpa_ie_fixed_t *wpa_ie;
+	bcm_tlv_t *wpa2_ie;
+	wifi_p2p_ie_t *p2p_ie;
+#ifdef WLWFDIE
+	wifi_wfd_ie_t *wfd_ie;
+#endif /* WLWFDIE */
+	bool is_bssup = false;
+	bool update_bss = false;
+	bool pbc = false;
+	u16 wpsie_len = 0;
+	u16 p2pie_len = 0;
+#ifdef WLWFDIE
+	u32 wfdie_len = 0;
+#endif /* WLWFDIE */
+	u8 beacon_ie[IE_MAX_LEN];
+	s32 ie_offset = 0;
+	s32 bssidx = 0;
+	s32 infra = 1;
+	s32 join_params_size = 0;
+	s32 ap = 0;
+	WL_DBG(("interval (%d) dtim_period (%d) head_len (%d) tail_len (%d)\n",
+		info->interval, info->dtim_period, info->head_len, info->tail_len));
+
+	if (wl->p2p_net == dev) {
+		dev = wl_to_prmry_ndev(wl);
+	}
+
+	bssidx = wl_cfgp2p_find_idx(wl, dev);
+	if (p2p_is_on(wl) &&
+		(bssidx == wl_to_p2p_bss_bssidx(wl,
+		P2PAPI_BSSCFG_CONNECTION))) {
+		memset(beacon_ie, 0, sizeof(beacon_ie));
+		/* We don't need to set beacon for P2P_GO,
+		 * but need to parse ssid from beacon_parameters
+		 * because there is no way to set ssid
+		 */
+		ie_offset = DOT11_MGMT_HDR_LEN + DOT11_BCN_PRB_FIXED_LEN;
+		/* find the SSID */
+		if ((ssid_ie = bcm_parse_tlvs((u8 *)&info->head[ie_offset],
+			info->head_len - ie_offset,
+			DOT11_MNG_SSID_ID)) != NULL) {
+			memcpy(wl->p2p->ssid.SSID, ssid_ie->data, ssid_ie->len);
+			wl->p2p->ssid.SSID_len = ssid_ie->len;
+			WL_DBG(("SSID (%s) in Head \n", ssid_ie->data));
+
+		} else {
+			WL_ERR(("No SSID in beacon \n"));
+		}
+
+		/* find the WPSIE */
+		if ((wps_ie = wl_cfgp2p_find_wpsie((u8 *)info->tail, info->tail_len)) != NULL) {
+			wpsie_len = wps_ie->length + WPA_RSN_IE_TAG_FIXED_LEN;
+			/*
+			 * Should be compared with saved ie before saving it
+			 */
+			wl_validate_wps_ie((char *) wps_ie, &pbc);
+			memcpy(beacon_ie, wps_ie, wpsie_len);
+		} else {
+			WL_ERR(("No WPSIE in beacon \n"));
+		}
+
+
+		/* find the P2PIE */
+		if ((p2p_ie = wl_cfgp2p_find_p2pie((u8 *)info->tail, info->tail_len)) != NULL) {
+			/* Total length of P2P Information Element */
+			p2pie_len = p2p_ie->len + sizeof(p2p_ie->len) + sizeof(p2p_ie->id);
+			memcpy(&beacon_ie[wpsie_len], p2p_ie, p2pie_len);
+
+		} else {
+			WL_ERR(("No P2PIE in beacon \n"));
+		}
+#ifdef WLWFDIE
+		/* find the WFD IEs */
+		if ((wfd_ie = wl_cfgp2p_find_wfdie((u8 *)info->tail, info->tail_len)) != NULL) {
+			/* Total length of P2P Information Element */
+			wfdie_len = wfd_ie->len + sizeof(wfd_ie->len) + sizeof(wfd_ie->id);
+			if ((wpsie_len + p2pie_len + wfdie_len) < IE_MAX_LEN) {
+				memcpy(&beacon_ie[wpsie_len + p2pie_len], wfd_ie, wfdie_len);
+			} else {
+				WL_ERR(("Found WFD IE but there is no space, (%d)(%d)(%d)\n",
+					wpsie_len, p2pie_len, wfdie_len));
+				wfdie_len = 0;
+			}
+		} else {
+			WL_ERR(("No WFDIE in beacon \n"));
+		}
+#endif /* WLWFDIE */
+		/* add WLC_E_PROBREQ_MSG event to respose probe_request from STA */
+		wl_add_remove_eventmsg(dev, WLC_E_PROBREQ_MSG, pbc);
+		wl_cfgp2p_set_management_ie(wl, dev, bssidx, VNDR_IE_BEACON_FLAG,
+#ifdef WLWFDIE
+			beacon_ie, wpsie_len + p2pie_len + wfdie_len);
+#else
+			beacon_ie, wpsie_len + p2pie_len);
+#endif /* WLWFDIE */
+
+		/* find the RSN_IE */
+		if ((wpa2_ie = bcm_parse_tlvs((u8 *)info->tail, info->tail_len,
+			DOT11_MNG_RSN_ID)) != NULL) {
+			WL_DBG((" WPA2 IE is found\n"));
+		}
+		is_bssup = wl_cfgp2p_bss_isup(dev, bssidx);
+
+		if (!is_bssup && (wpa2_ie != NULL)) {
+			wldev_iovar_setint(dev, "mpc", 0);
+			if ((err = wl_validate_wpa2ie(dev, wpa2_ie, bssidx)) < 0) {
+				WL_ERR(("WPA2 IE parsing error"));
+				goto exit;
+			}
+			err = wldev_ioctl(dev, WLC_SET_INFRA, &infra, sizeof(s32), true);
+			if (err < 0) {
+				WL_ERR(("SET INFRA error %d\n", err));
+				goto exit;
+			}
+			err = wldev_iovar_setbuf_bsscfg(dev, "ssid", &wl->p2p->ssid,
+				sizeof(wl->p2p->ssid), wl->ioctl_buf, WLC_IOCTL_MAXLEN,
+				bssidx, &wl->ioctl_buf_sync);
+			if (err < 0) {
+				WL_ERR(("GO SSID setting error %d\n", err));
+				goto exit;
+			}
+			if ((err = wl_cfgp2p_bss(wl, dev, bssidx, 1)) < 0) {
+				WL_ERR(("GO Bring up error %d\n", err));
+				goto exit;
+			}
+		}
+	} else if (wl_get_drv_status(wl, AP_CREATING, dev)) {
+		ie_offset = DOT11_MGMT_HDR_LEN + DOT11_BCN_PRB_FIXED_LEN;
+		ap = 1;
+		/* find the SSID */
+		if ((ssid_ie = bcm_parse_tlvs((u8 *)&info->head[ie_offset],
+			info->head_len - ie_offset,
+			DOT11_MNG_SSID_ID)) != NULL) {
+			memset(&ssid, 0, sizeof(wlc_ssid_t));
+			memcpy(ssid.SSID, ssid_ie->data, ssid_ie->len);
+			WL_DBG(("SSID is (%s) in Head \n", ssid.SSID));
+			ssid.SSID_len = ssid_ie->len;
+			wldev_iovar_setint(dev, "mpc", 0);
+			wldev_ioctl(dev, WLC_DOWN, &ap, sizeof(s32), true);
+			wldev_ioctl(dev, WLC_SET_INFRA, &infra, sizeof(s32), true);
+			if ((err = wldev_ioctl(dev, WLC_SET_AP, &ap, sizeof(s32), true)) < 0) {
+				WL_ERR(("setting AP mode failed %d \n", err));
+				return err;
+			}
+
+			/* if requested, do softap ch autoselect  */
+			if (wl->hostapd_chan == 14) {
+				int auto_chan;
+				if ((err = wldev_get_auto_channel(dev, &auto_chan)) != 0) {
+					WL_ERR(("softap: auto chan select failed,"
+						" will use ch 6\n"));
+					auto_chan = 6;
+				} else {
+					printf("<0>softap: got auto ch:%d\n", auto_chan);
+				}
+				err = wldev_ioctl(dev, WLC_SET_CHANNEL,
+					&auto_chan, sizeof(auto_chan), true);
+				if (err < 0) {
+					WL_ERR(("softap: WLC_SET_CHANNEL error %d chip"
+						" may not be supporting this channel\n", err));
+					return err;
+				}
+			}
+
+			/* find the RSN_IE */
+			if ((wpa2_ie = bcm_parse_tlvs((u8 *)info->tail, info->tail_len,
+				DOT11_MNG_RSN_ID)) != NULL) {
+				WL_DBG((" WPA2 IE is found\n"));
+			}
+			/* find the WPA_IE */
+			if ((wpa_ie = wl_cfgp2p_find_wpaie((u8 *)info->tail,
+			info->tail_len)) != NULL) {
+				WL_DBG((" WPA IE is found\n"));
+			}
+			if ((wpa_ie != NULL || wpa2_ie != NULL)) {
+				if (wl_validate_wpa2ie(dev, wpa2_ie, bssidx)  < 0 ||
+					wl_validate_wpaie(dev, wpa_ie, bssidx) < 0) {
+					wl->ap_info->security_mode = false;
+					return BCME_ERROR;
+				}
+				wl->ap_info->security_mode = true;
+				if (wl->ap_info->rsn_ie) {
+					kfree(wl->ap_info->rsn_ie);
+					wl->ap_info->rsn_ie = NULL;
+				}
+				if (wl->ap_info->wpa_ie) {
+					kfree(wl->ap_info->wpa_ie);
+					wl->ap_info->wpa_ie = NULL;
+				}
+				if (wl->ap_info->wps_ie) {
+					kfree(wl->ap_info->wps_ie);
+					wl->ap_info->wps_ie = NULL;
+				}
+				if (wpa_ie != NULL) {
+					/* WPAIE */
+					wl->ap_info->rsn_ie = NULL;
+					wl->ap_info->wpa_ie = kmemdup(wpa_ie,
+						wpa_ie->length + WPA_RSN_IE_TAG_FIXED_LEN,
+						GFP_KERNEL);
+				} else {
+					/* RSNIE */
+					wl->ap_info->wpa_ie = NULL;
+					wl->ap_info->rsn_ie = kmemdup(wpa2_ie,
+						wpa2_ie->len + WPA_RSN_IE_TAG_FIXED_LEN,
+						GFP_KERNEL);
+				}
+			} else
+				wl->ap_info->security_mode = false;
+			/* find the WPSIE */
+			if ((wps_ie = wl_cfgp2p_find_wpsie((u8 *)info->tail,
+				info->tail_len)) != NULL) {
+				wpsie_len = wps_ie->length +WPA_RSN_IE_TAG_FIXED_LEN;
+				/*
+				* Should be compared with saved ie before saving it
+				*/
+				wl_validate_wps_ie((char *) wps_ie, &pbc);
+				memcpy(beacon_ie, wps_ie, wpsie_len);
+				wl_cfgp2p_set_management_ie(wl, dev, bssidx, VNDR_IE_BEACON_FLAG,
+				beacon_ie, wpsie_len);
+				wl->ap_info->wps_ie = kmemdup(wps_ie, wpsie_len, GFP_KERNEL);
+				/* add WLC_E_PROBREQ_MSG event to respose probe_request from STA */
+				wl_add_remove_eventmsg(dev, WLC_E_PROBREQ_MSG, pbc);
+			} else {
+				WL_DBG(("No WPSIE in beacon \n"));
+			}
+			if (info->interval) {
+				if ((err = wldev_ioctl(dev, WLC_SET_BCNPRD,
+					&info->interval, sizeof(s32), true)) < 0) {
+					WL_ERR(("Beacon Interval Set Error, %d\n", err));
+					return err;
+				}
+			}
+			if (info->dtim_period) {
+				if ((err = wldev_ioctl(dev, WLC_SET_DTIMPRD,
+					&info->dtim_period, sizeof(s32), true)) < 0) {
+					WL_ERR(("DTIM Interval Set Error, %d\n", err));
+					return err;
+				}
+			}
+			err = wldev_ioctl(dev, WLC_UP, &ap, sizeof(s32), true);
+			if (unlikely(err)) {
+				WL_ERR(("WLC_UP error (%d)\n", err));
+				return err;
+			}
+			memset(&join_params, 0, sizeof(join_params));
+			/* join parameters starts with ssid */
+			join_params_size = sizeof(join_params.ssid);
+			memcpy(join_params.ssid.SSID, ssid.SSID, ssid.SSID_len);
+			join_params.ssid.SSID_len = htod32(ssid.SSID_len);
+			/* create softap */
+			if ((err = wldev_ioctl(dev, WLC_SET_SSID, &join_params,
+				join_params_size, true)) == 0) {
+				wl_clr_drv_status(wl, AP_CREATING, dev);
+				wl_set_drv_status(wl, AP_CREATED, dev);
+			}
+		}
+	} else if (wl_get_drv_status(wl, AP_CREATED, dev)) {
+		ap = 1;
+		/* find the WPSIE */
+		if ((wps_ie = wl_cfgp2p_find_wpsie((u8 *)info->tail, info->tail_len)) != NULL) {
+			wpsie_len = wps_ie->length + WPA_RSN_IE_TAG_FIXED_LEN;
+			/*
+			 * Should be compared with saved ie before saving it
+			 */
+			wl_validate_wps_ie((char *) wps_ie, &pbc);
+			memcpy(beacon_ie, wps_ie, wpsie_len);
+			wl_cfgp2p_set_management_ie(wl, dev, bssidx, VNDR_IE_BEACON_FLAG,
+			beacon_ie, wpsie_len);
+			if (wl->ap_info->wps_ie &&
+				memcmp(wl->ap_info->wps_ie, wps_ie, wpsie_len)) {
+				WL_DBG((" WPS IE is changed\n"));
+				kfree(wl->ap_info->wps_ie);
+				wl->ap_info->wps_ie = kmemdup(wps_ie, wpsie_len, GFP_KERNEL);
+				/* add WLC_E_PROBREQ_MSG event to respose probe_request from STA */
+				wl_add_remove_eventmsg(dev, WLC_E_PROBREQ_MSG, pbc);
+			} else if (wl->ap_info->wps_ie == NULL) {
+				WL_DBG((" WPS IE is added\n"));
+				wl->ap_info->wps_ie = kmemdup(wps_ie, wpsie_len, GFP_KERNEL);
+				/* add WLC_E_PROBREQ_MSG event to respose probe_request from STA */
+				wl_add_remove_eventmsg(dev, WLC_E_PROBREQ_MSG, pbc);
+			}
+			/* find the RSN_IE */
+			if ((wpa2_ie = bcm_parse_tlvs((u8 *)info->tail, info->tail_len,
+				DOT11_MNG_RSN_ID)) != NULL) {
+				WL_DBG((" WPA2 IE is found\n"));
+			}
+			/* find the WPA_IE */
+			if ((wpa_ie = wl_cfgp2p_find_wpaie((u8 *)info->tail,
+				info->tail_len)) != NULL) {
+				WL_DBG((" WPA IE is found\n"));
+			}
+			if ((wpa_ie != NULL || wpa2_ie != NULL)) {
+				if (!wl->ap_info->security_mode) {
+					/* change from open mode to security mode */
+					update_bss = true;
+					if (wpa_ie != NULL) {
+						wl->ap_info->wpa_ie = kmemdup(wpa_ie,
+						wpa_ie->length + WPA_RSN_IE_TAG_FIXED_LEN,
+						GFP_KERNEL);
+					} else {
+						wl->ap_info->rsn_ie = kmemdup(wpa2_ie,
+						wpa2_ie->len + WPA_RSN_IE_TAG_FIXED_LEN,
+						GFP_KERNEL);
+					}
+				} else if (wl->ap_info->wpa_ie) {
+					/* change from WPA mode to WPA2 mode */
+					if (wpa2_ie != NULL) {
+						update_bss = true;
+						kfree(wl->ap_info->wpa_ie);
+						wl->ap_info->rsn_ie = kmemdup(wpa2_ie,
+						wpa2_ie->len + WPA_RSN_IE_TAG_FIXED_LEN,
+						GFP_KERNEL);
+						wl->ap_info->wpa_ie = NULL;
+					}
+					else if (memcmp(wl->ap_info->wpa_ie,
+						wpa_ie, wpa_ie->length +
+						WPA_RSN_IE_TAG_FIXED_LEN)) {
+						kfree(wl->ap_info->wpa_ie);
+						update_bss = true;
+						wl->ap_info->wpa_ie = kmemdup(wpa_ie,
+						wpa_ie->length + WPA_RSN_IE_TAG_FIXED_LEN,
+						GFP_KERNEL);
+						wl->ap_info->rsn_ie = NULL;
+					}
+				} else {
+					/* change from WPA2 mode to WPA mode */
+					if (wpa_ie != NULL) {
+						update_bss = true;
+						kfree(wl->ap_info->rsn_ie);
+						wl->ap_info->rsn_ie = NULL;
+						wl->ap_info->wpa_ie = kmemdup(wpa_ie,
+						wpa_ie->length + WPA_RSN_IE_TAG_FIXED_LEN,
+						GFP_KERNEL);
+					} else if (memcmp(wl->ap_info->rsn_ie,
+						wpa2_ie, wpa2_ie->len + WPA_RSN_IE_TAG_FIXED_LEN)) {
+						update_bss = true;
+						kfree(wl->ap_info->rsn_ie);
+						wl->ap_info->rsn_ie = kmemdup(wpa2_ie,
+						wpa2_ie->len + WPA_RSN_IE_TAG_FIXED_LEN,
+						GFP_KERNEL);
+						wl->ap_info->wpa_ie = NULL;
+					}
+				}
+				if (update_bss) {
+					wl->ap_info->security_mode = true;
+					wl_cfgp2p_bss(wl, dev, bssidx, 0);
+					if (wl_validate_wpa2ie(dev, wpa2_ie, bssidx)  < 0 ||
+						wl_validate_wpaie(dev, wpa_ie, bssidx) < 0) {
+						return BCME_ERROR;
+					}
+					wl_cfgp2p_bss(wl, dev, bssidx, 1);
+				}
+			}
+		} else {
+			WL_ERR(("No WPSIE in beacon \n"));
+		}
+	}
+exit:
+	if (err)
+		wldev_iovar_setint(dev, "mpc", 1);
+	return err;
+}
+
+#ifdef WL_SCHED_SCAN
+#define PNO_TIME	30
+#define PNO_REPEAT	4
+#define PNO_FREQ_EXPO_MAX	2
+int wl_cfg80211_sched_scan_start(struct wiphy *wiphy,
+                             struct net_device *dev,
+                             struct cfg80211_sched_scan_request *request)
+{
+	ushort pno_time = PNO_TIME;
+	int pno_repeat = PNO_REPEAT;
+	int pno_freq_expo_max = PNO_FREQ_EXPO_MAX;
+	wlc_ssid_t ssids_local[MAX_PFN_LIST_COUNT];
+	struct wl_priv *wl = wiphy_priv(wiphy);
+	struct cfg80211_ssid *ssid = NULL;
+	int ssid_count = 0;
+	int i;
+	int ret = 0;
+
+	WL_DBG(("Enter n_match_sets:%d   n_ssids:%d \n",
+		request->n_match_sets, request->n_ssids));
+	WL_DBG(("ssids:%d pno_time:%d pno_repeat:%d pno_freq:%d \n",
+		request->n_ssids, pno_time, pno_repeat, pno_freq_expo_max));
+
+#if defined(WL_ENABLE_P2P_IF)
+	/* While GO is operational, PNO is not supported */
+	if (dhd_cfg80211_get_opmode(wl) & P2P_GO_ENABLED) {
+		WL_DBG(("PNO not enabled! op_mode: P2P GO"));
+		return -1;
+	}
+#endif
+
+	if (!request || !request->n_ssids || !request->n_match_sets) {
+		WL_ERR(("Invalid sched scan req!! n_ssids:%d \n", request->n_ssids));
+		return -EINVAL;
+	}
+
+	memset(&ssids_local, 0, sizeof(ssids_local));
+
+	if (request->n_match_sets > 0) {
+		for (i = 0; i < request->n_match_sets; i++) {
+			ssid = &request->match_sets[i].ssid;
+			memcpy(ssids_local[i].SSID, ssid->ssid, ssid->ssid_len);
+			ssids_local[i].SSID_len = ssid->ssid_len;
+			WL_DBG((">>> PNO filter set for ssid (%s) \n", ssid->ssid));
+			ssid_count++;
+		}
+	}
+
+	if (request->n_ssids > 0) {
+		for (i = 0; i < request->n_ssids; i++) {
+			/* Active scan req for ssids */
+			WL_DBG((">>> Active scan req for ssid (%s) \n", request->ssids[i].ssid));
+
+			/* match_set ssids is a supert set of n_ssid list, so we need
+			 * not add these set seperately
+			 */
+		}
+	}
+
+	if (ssid_count) {
+		if ((ret = dhd_dev_pno_set(dev, ssids_local, request->n_match_sets,
+			pno_time, pno_repeat, pno_freq_expo_max)) < 0) {
+			WL_ERR(("PNO setup failed!! ret=%d \n", ret));
+			return -EINVAL;
+		}
+
+		/* Enable the PNO */
+		if (dhd_dev_pno_enable(dev, 1) < 0) {
+			WL_ERR(("PNO enable failed!! ret=%d \n", ret));
+			return -EINVAL;
+		}
+		wl->sched_scan_req = request;
+	} else {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int wl_cfg80211_sched_scan_stop(struct wiphy *wiphy, struct net_device *dev)
+{
+	struct wl_priv *wl = wiphy_priv(wiphy);
+
+	WL_DBG(("Enter \n"));
+
+	if (dhd_dev_pno_enable(dev, 0) < 0)
+		WL_ERR(("PNO disable failed"));
+
+	if (dhd_dev_pno_reset(dev) < 0)
+		WL_ERR(("PNO reset failed"));
+
+	if (wl->scan_request && wl->sched_scan_running) {
+		wl_notify_escan_complete(wl, dev, true, true);
+	}
+
+	 wl->sched_scan_req = NULL;
+	 wl->sched_scan_running = FALSE;
+
+	return 0;
+}
+#endif /* WL_SCHED_SCAN */
+
+static struct cfg80211_ops wl_cfg80211_ops = {
+	.add_virtual_intf = wl_cfg80211_add_virtual_iface,
+	.del_virtual_intf = wl_cfg80211_del_virtual_iface,
+	.change_virtual_intf = wl_cfg80211_change_virtual_iface,
+	.scan = wl_cfg80211_scan,
+	.set_wiphy_params = wl_cfg80211_set_wiphy_params,
+	.join_ibss = wl_cfg80211_join_ibss,
+	.leave_ibss = wl_cfg80211_leave_ibss,
+	.get_station = wl_cfg80211_get_station,
+	.set_tx_power = wl_cfg80211_set_tx_power,
+	.get_tx_power = wl_cfg80211_get_tx_power,
+	.add_key = wl_cfg80211_add_key,
+	.del_key = wl_cfg80211_del_key,
+	.get_key = wl_cfg80211_get_key,
+	.set_default_key = wl_cfg80211_config_default_key,
+	.set_default_mgmt_key = wl_cfg80211_config_default_mgmt_key,
+	.set_power_mgmt = wl_cfg80211_set_power_mgmt,
+	.connect = wl_cfg80211_connect,
+	.disconnect = wl_cfg80211_disconnect,
+	.suspend = wl_cfg80211_suspend,
+	.resume = wl_cfg80211_resume,
+	.set_pmksa = wl_cfg80211_set_pmksa,
+	.del_pmksa = wl_cfg80211_del_pmksa,
+	.flush_pmksa = wl_cfg80211_flush_pmksa,
+	.remain_on_channel = wl_cfg80211_remain_on_channel,
+	.cancel_remain_on_channel = wl_cfg80211_cancel_remain_on_channel,
+	.mgmt_tx = wl_cfg80211_mgmt_tx,
+	.mgmt_frame_register = wl_cfg80211_mgmt_frame_register,
+	.change_bss = wl_cfg80211_change_bss,
+	.set_channel = wl_cfg80211_set_channel,
+	.set_beacon = wl_cfg80211_add_set_beacon,
+	.add_beacon = wl_cfg80211_add_set_beacon,
+#ifdef WL_SCHED_SCAN
+	.sched_scan_start = wl_cfg80211_sched_scan_start,
+	.sched_scan_stop = wl_cfg80211_sched_scan_stop,
+#endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(3, 2, 0) */
+};
+
+s32 wl_mode_to_nl80211_iftype(s32 mode)
+{
+	s32 err = 0;
+
+	switch (mode) {
+	case WL_MODE_BSS:
+		return NL80211_IFTYPE_STATION;
+	case WL_MODE_IBSS:
+		return NL80211_IFTYPE_ADHOC;
+	case WL_MODE_AP:
+		return NL80211_IFTYPE_AP;
+	default:
+		return NL80211_IFTYPE_UNSPECIFIED;
+	}
+
+	return err;
+}
+
+static s32 wl_setup_wiphy(struct wireless_dev *wdev, struct device *sdiofunc_dev)
+{
+	s32 err = 0;
+	wdev->wiphy =
+	    wiphy_new(&wl_cfg80211_ops, sizeof(struct wl_priv));
+	if (unlikely(!wdev->wiphy)) {
+		WL_ERR(("Couldn not allocate wiphy device\n"));
+		err = -ENOMEM;
+		return err;
+	}
+	set_wiphy_dev(wdev->wiphy, sdiofunc_dev);
+	wdev->wiphy->max_scan_ie_len = WL_SCAN_IE_LEN_MAX;
+	/* Report  how many SSIDs Driver can support per Scan request */
+	wdev->wiphy->max_scan_ssids = WL_SCAN_PARAMS_SSID_MAX;
+	wdev->wiphy->max_num_pmkids = WL_NUM_PMKIDS_MAX;
+#ifdef WL_SCHED_SCAN
+	wdev->wiphy->max_sched_scan_ssids = MAX_PFN_LIST_COUNT;
+	wdev->wiphy->max_match_sets = MAX_PFN_LIST_COUNT;
+	wdev->wiphy->max_sched_scan_ie_len = WL_SCAN_IE_LEN_MAX;
+	wdev->wiphy->flags |= WIPHY_FLAG_SUPPORTS_SCHED_SCAN;
+#endif /* WL_SCHED_SCAN */
+	wdev->wiphy->interface_modes =
+		BIT(NL80211_IFTYPE_STATION)
+		| BIT(NL80211_IFTYPE_AP) | BIT(NL80211_IFTYPE_MONITOR);
+
+	wdev->wiphy->bands[IEEE80211_BAND_2GHZ] = &__wl_band_2ghz;
+	wdev->wiphy->bands[IEEE80211_BAND_5GHZ] = &__wl_band_5ghz_a;
+	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->max_remain_on_channel_duration = 5000;
+	wdev->wiphy->mgmt_stypes = wl_cfg80211_default_mgmt_stypes;
+#ifndef WL_POWERSAVE_DISABLED
+	wdev->wiphy->flags |= WIPHY_FLAG_PS_ON_BY_DEFAULT;
+#else
+	wdev->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
+#endif				/* !WL_POWERSAVE_DISABLED */
+	wdev->wiphy->flags |= WIPHY_FLAG_NETNS_OK |
+		WIPHY_FLAG_4ADDR_AP |
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 39)
+		WIPHY_FLAG_SUPPORTS_SEPARATE_DEFAULT_KEYS |
+#endif
+		WIPHY_FLAG_4ADDR_STATION;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 2, 0)
+	wdev->wiphy->flags |= WIPHY_FLAG_SUPPORTS_FW_ROAM;
+#endif
+
+#ifdef ENABLE_CUSTOM_REGULATORY_DOMAIN
+	WL_DBG(("Registering custom regulatory)\n"));
+	wdev->wiphy->flags |= WIPHY_FLAG_CUSTOM_REGULATORY;
+	wiphy_apply_custom_regulatory(wdev->wiphy, &brcm_regdom);
+#endif
+	/* Now we can register wiphy with cfg80211 module */
+	err = wiphy_register(wdev->wiphy);
+	if (unlikely(err < 0)) {
+		WL_ERR(("Couldn not register wiphy device (%d)\n", err));
+		wiphy_free(wdev->wiphy);
+	}
+	return err;
+}
+
+static void wl_free_wdev(struct wl_priv *wl)
+{
+	struct wireless_dev *wdev = wl->wdev;
+	struct wiphy *wiphy;
+	if (!wdev) {
+		WL_ERR(("wdev is invalid\n"));
+		return;
+	}
+	wiphy = wdev->wiphy;
+	wiphy_unregister(wdev->wiphy);
+	wdev->wiphy->dev.parent = NULL;
+
+	wl_delete_all_netinfo(wl);
+	wiphy_free(wiphy);
+	/* PLEASE do NOT call any function after wiphy_free, the driver's private structure "wl",
+	 * which is the private part of wiphy, has been freed in wiphy_free !!!!!!!!!!!
+	 */
+}
+
+static s32 wl_inform_bss(struct wl_priv *wl)
+{
+	struct wl_scan_results *bss_list;
+	struct wl_bss_info *bi = NULL;	/* must be initialized */
+	s32 err = 0;
+	s32 i;
+
+	bss_list = wl->bss_list;
+	WL_DBG(("scanned AP count (%d)\n", bss_list->count));
+	bi = next_bss(bss_list, bi);
+	for_each_bss(bss_list, bi, i) {
+		err = wl_inform_single_bss(wl, bi);
+		if (unlikely(err))
+			break;
+	}
+	return err;
+}
+
+static s32 wl_inform_single_bss(struct wl_priv *wl, struct wl_bss_info *bi)
+{
+	struct wiphy *wiphy = wl_to_wiphy(wl);
+	struct ieee80211_mgmt *mgmt;
+	struct ieee80211_channel *channel;
+	struct ieee80211_supported_band *band;
+	struct wl_cfg80211_bss_info *notif_bss_info;
+	struct wl_scan_req *sr = wl_to_sr(wl);
+	struct beacon_proberesp *beacon_proberesp;
+	struct cfg80211_bss *cbss = NULL;
+	s32 mgmt_type;
+	s32 signal;
+	u32 freq;
+	s32 err = 0;
+
+	if (unlikely(dtoh32(bi->length) > WL_BSS_INFO_MAX)) {
+		WL_DBG(("Beacon is larger than buffer. Discarding\n"));
+		return err;
+	}
+	notif_bss_info = kzalloc(sizeof(*notif_bss_info) + sizeof(*mgmt)
+		- sizeof(u8) + WL_BSS_INFO_MAX, GFP_KERNEL);
+	if (unlikely(!notif_bss_info)) {
+		WL_ERR(("notif_bss_info alloc failed\n"));
+		return -ENOMEM;
+	}
+	mgmt = (struct ieee80211_mgmt *)notif_bss_info->frame_buf;
+	notif_bss_info->channel =
+		bi->ctl_ch ? bi->ctl_ch : CHSPEC_CHANNEL(bi->chanspec);
+
+	if (notif_bss_info->channel <= CH_MAX_2G_CHANNEL)
+		band = wiphy->bands[IEEE80211_BAND_2GHZ];
+	else
+		band = wiphy->bands[IEEE80211_BAND_5GHZ];
+	if (!band) {
+		WL_ERR(("No valid band"));
+		kfree(notif_bss_info);
+		return -EINVAL;
+	}
+	notif_bss_info->rssi = dtoh16(bi->RSSI);
+	memcpy(mgmt->bssid, &bi->BSSID, ETHER_ADDR_LEN);
+	mgmt_type = wl->active_scan ?
+		IEEE80211_STYPE_PROBE_RESP : IEEE80211_STYPE_BEACON;
+	if (!memcmp(bi->SSID, sr->ssid.SSID, bi->SSID_len)) {
+	    mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT | mgmt_type);
+	}
+	beacon_proberesp = wl->active_scan ?
+		(struct beacon_proberesp *)&mgmt->u.probe_resp :
+		(struct beacon_proberesp *)&mgmt->u.beacon;
+	beacon_proberesp->timestamp = 0;
+	beacon_proberesp->beacon_int = cpu_to_le16(bi->beacon_period);
+	beacon_proberesp->capab_info = cpu_to_le16(bi->capability);
+	wl_rst_ie(wl);
+
+	wl_mrg_ie(wl, ((u8 *) bi) + bi->ie_offset, bi->ie_length);
+	wl_cp_ie(wl, beacon_proberesp->variable, WL_BSS_INFO_MAX -
+		offsetof(struct wl_cfg80211_bss_info, frame_buf));
+	notif_bss_info->frame_len = offsetof(struct ieee80211_mgmt,
+		u.beacon.variable) + wl_get_ielen(wl);
+#if LINUX_VERSION_CODE == KERNEL_VERSION(2, 6, 38) && !defined(WL_COMPAT_WIRELESS)
+	freq = ieee80211_channel_to_frequency(notif_bss_info->channel);
+#else
+	freq = ieee80211_channel_to_frequency(notif_bss_info->channel, band->band);
+#endif
+	channel = ieee80211_get_channel(wiphy, freq);
+	if (!channel) {
+		WL_ERR(("No valid channel"));
+		kfree(notif_bss_info);
+		return -EINVAL;
+	}
+
+	WL_DBG(("SSID : \"%s\", rssi %d, channel %d, capability : 0x04%x, bssid %pM "
+			"mgmt_type %d frame_len %d\n", bi->SSID,
+			notif_bss_info->rssi, notif_bss_info->channel,
+			mgmt->u.beacon.capab_info, &bi->BSSID, mgmt_type,
+			notif_bss_info->frame_len));
+
+	signal = notif_bss_info->rssi * 100;
+
+#if defined(WLP2P) && defined(WL_ENABLE_P2P_IF)
+	if (wl->p2p && wl->p2p_net && wl->scan_request &&
+		((wl->scan_request->dev == wl->p2p_net) ||
+		(wl->scan_request->dev == wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION)))){
+#else
+	if (p2p_is_on(wl) && ( p2p_scan(wl) ||
+		(wl->scan_request->dev == wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION)))) {
+#endif
+		/* find the P2PIE, if we do not find it, we will discard this frame */
+		wifi_p2p_ie_t * p2p_ie;
+		if ((p2p_ie = wl_cfgp2p_find_p2pie((u8 *)beacon_proberesp->variable,
+			wl_get_ielen(wl))) == NULL) {
+			WL_ERR(("Couldn't find P2PIE in probe response/beacon\n"));
+			kfree(notif_bss_info);
+			return err;
+		}
+		else if( wl_cfgp2p_retreive_p2pattrib(p2p_ie, P2P_SEID_DEV_INFO) == NULL)
+		{
+			WL_DBG(("Couldn't find P2P_SEID_DEV_INFO in probe response/beacon\n"));
+			kfree(notif_bss_info);
+			return err;
+		}
+	}
+	if (!mgmt->u.probe_resp.timestamp) {
+		struct timeval tv;
+
+		do_gettimeofday(&tv);
+		mgmt->u.probe_resp.timestamp = ((u64)tv.tv_sec * 1000000)
+						+ tv.tv_usec;
+	}
+
+	cbss = cfg80211_inform_bss_frame(wiphy, channel, mgmt,
+		le16_to_cpu(notif_bss_info->frame_len), signal, GFP_KERNEL);
+	if (unlikely(!cbss)) {
+		WL_ERR(("cfg80211_inform_bss_frame error\n"));
+		kfree(notif_bss_info);
+		return -EINVAL;
+	}
+
+	cfg80211_put_bss(cbss);
+	kfree(notif_bss_info);
+
+	return err;
+}
+
+static s32 wl_inform_ibss(struct wl_priv *wl, const u8 *bssid)
+{
+	struct net_device *ndev = wl_to_prmry_ndev(wl);
+	struct wiphy *wiphy = wl_to_wiphy(wl);
+	struct wl_bss_info *bi = NULL;
+	struct ieee80211_channel *notify_channel;
+	struct ieee80211_supported_band *band;
+	struct cfg80211_bss *bss;
+	s32 err = 0;
+	u16 channel;
+	u32 freq;
+	u32 wsec = 0;
+	u16 notify_capability;
+	u16 notify_interval;
+	u8 *notify_ie;
+	size_t notify_ielen;
+	s32 notify_signal;
+
+	WL_TRACE(("Enter\n"));
+
+	if (wl->scan_request) {
+		wl_notify_escan_complete(wl, ndev, true, true);
+	}
+
+	mutex_lock(&wl->usr_sync);
+
+	*(u32 *)wl->extra_buf = htod32(WL_EXTRA_BUF_MAX);
+	err = wldev_ioctl(ndev, WLC_GET_BSS_INFO, wl->extra_buf,
+					WL_EXTRA_BUF_MAX, false);
+	if (err) {
+		WL_ERR(("Failed to get bss info for IBSS\n"));
+		err = -EIO;
+		goto CleanUp;
+	}
+	bi = (struct wl_bss_info *)(wl->extra_buf + 4);
+
+	if (memcmp(bssid, &bi->BSSID, ETHER_ADDR_LEN)) {
+		WL_ERR(("BSSID mismatch: Inform %02x:%02x:%02x:%02x:%02x:%02x,"
+					"%02x:%02x:%02x:%02x:%02x:%02x\n",
+					bssid[0], bssid[1], bssid[2], bssid[3], bssid[4], bssid[5],
+					bi->BSSID.octet[0], bi->BSSID.octet[1], bi->BSSID.octet[2],
+					bi->BSSID.octet[3], bi->BSSID.octet[4],
+					bi->BSSID.octet[5]));
+		err = -EINVAL;
+		goto CleanUp;
+	}
+
+	err = wldev_iovar_getint(ndev, "wsec", &wsec);
+	if (err) {
+		WL_ERR(("wsec failed: %d\n", err));
+		err = -EIO;
+		goto CleanUp;
+	}
+
+	channel = bi->ctl_ch ? bi->ctl_ch :
+				CHSPEC_CHANNEL(dtohchanspec(bi->chanspec));
+	if (channel <= CH_MAX_2G_CHANNEL)
+		band = wiphy->bands[IEEE80211_BAND_2GHZ];
+	else
+		band = wiphy->bands[IEEE80211_BAND_5GHZ];
+
+#if LINUX_VERSION_CODE == KERNEL_VERSION(2, 6, 38) && !defined(WL_COMPAT_WIRELESS)
+	freq = ieee80211_channel_to_frequency(channel);
+	(void)band->band;
+#else
+	freq = ieee80211_channel_to_frequency(channel, band->band);
+#endif
+	notify_channel = ieee80211_get_channel(wiphy, freq);
+
+	notify_capability = dtoh16(bi->capability);
+	notify_interval = dtoh16(bi->beacon_period);
+	notify_ie = (u8 *)bi + dtoh16(bi->ie_offset);
+	notify_ielen = dtoh32(bi->ie_length);
+	notify_signal = (int16)dtoh16(bi->RSSI) * 100;
+
+	if (wl->p2p_supported) {
+		notify_capability |= DOT11_CAP_IBSS;
+		if (wsec)
+			notify_capability |= DOT11_CAP_PRIVACY;
+	}
+
+	WL_DBG(("BSSID %02x:%02x:%02x:%02x:%02x:%02x",
+			bssid[0], bssid[1], bssid[2], bssid[3], bssid[4], bssid[5]));
+	WL_INFO(("channel: %d(%d)\n", channel, freq));
+	WL_INFO(("capability: %X\n", notify_capability));
+	WL_INFO(("beacon interval: %d ms\n", notify_interval));
+	WL_INFO(("signal: %d dBm\n", notify_signal));
+	WL_INFO(("ie_len: %d\n", notify_ielen));
+	bss = cfg80211_inform_bss(wiphy, notify_channel, bssid, 0,
+				notify_capability, notify_interval,
+				notify_ie, notify_ielen, notify_signal, GFP_KERNEL);
+	if (!bss) {
+		WL_ERR(("cfg80211_inform_bss() Failed\n"));
+		err = -ENOMEM;
+		goto CleanUp;
+	}
+
+	cfg80211_put_bss(bss);
+	err = 0;
+
+CleanUp:
+
+	mutex_unlock(&wl->usr_sync);
+
+	WL_TRACE(("Exit\n"));
+	return err;
+}
+
+static bool wl_is_linkup(struct wl_priv *wl, const wl_event_msg_t *e, struct net_device *ndev)
+{
+	u32 event = ntoh32(e->event_type);
+	u32 status =  ntoh32(e->status);
+	u16 flags = ntoh16(e->flags);
+
+	WL_DBG(("event %d, status %d flags %x\n", event, status, flags));
+	if (event == WLC_E_SET_SSID) {
+		if (status == WLC_E_STATUS_SUCCESS) {
+			return true;
+		}
+	} else if (event == WLC_E_LINK) {
+		if (flags & WLC_EVENT_MSG_LINK)
+			if (!wl_is_ibssmode(wl, ndev))
+				return true;
+	}
+
+	WL_DBG(("wl_is_linkup false\n"));
+	return false;
+}
+
+static bool wl_is_linkdown(struct wl_priv *wl, const wl_event_msg_t *e)
+{
+	u32 event = ntoh32(e->event_type);
+	u16 flags = ntoh16(e->flags);
+
+	if (event == WLC_E_DEAUTH_IND ||
+	event == WLC_E_DISASSOC_IND ||
+	event == WLC_E_DISASSOC ||
+	event == WLC_E_DEAUTH) {
+		return true;
+	} else if (event == WLC_E_LINK) {
+		if (!(flags & WLC_EVENT_MSG_LINK))
+			return true;
+	}
+
+	return false;
+}
+
+static bool wl_is_nonetwork(struct wl_priv *wl, const wl_event_msg_t *e)
+{
+	u32 event = ntoh32(e->event_type);
+	u32 status = ntoh32(e->status);
+
+	if (event == WLC_E_LINK && status == WLC_E_STATUS_NO_NETWORKS)
+		return true;
+	if (event == WLC_E_SET_SSID && status != WLC_E_STATUS_SUCCESS)
+		return true;
+
+	return false;
+}
+
+/* The mainline kernel >= 3.2.0 has support for indicating new/del station
+ * to AP/P2P GO via events. If this change is backported to kernel for which
+ * this driver is being built, then define WL_CFG80211_STA_EVENT. You
+ * should use this new/del sta event mechanism for BRCM supplicant >= 22.
+ */
+static s32
+wl_notify_connect_status_ap(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data)
+{
+	s32 err = 0;
+	u32 event = ntoh32(e->event_type);
+	u32 reason = ntoh32(e->reason);
+	u32 len = ntoh32(e->datalen);
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 2, 0)) && !defined(WL_CFG80211_STA_EVENT)
+	bool isfree = false;
+	u8 *mgmt_frame;
+	u8 bsscfgidx = e->bsscfgidx;
+	s32 freq;
+	s32 channel;
+	u8 body[WL_FRAME_LEN];
+	u16 fc = 0;
+	struct ieee80211_supported_band *band;
+	struct ether_addr da;
+	struct ether_addr bssid;
+	struct wiphy *wiphy = wl_to_wiphy(wl);
+	channel_info_t ci;
+#else
+	struct station_info sinfo;
+#endif /* (LINUX_VERSION_CODE < KERNEL_VERSION(3, 2, 0)) && !WL_CFG80211_STA_EVENT */
+
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 2, 0)) && !defined(WL_CFG80211_STA_EVENT)
+	memset(body, 0, sizeof(body));
+	memset(&bssid, 0, ETHER_ADDR_LEN);
+	WL_DBG(("Enter event %d ndev %p\n", event, ndev));
+	if (wl_get_mode_by_netdev(wl, ndev) == WL_INVALID)
+		return WL_INVALID;
+
+	if (len > WL_FRAME_LEN) {
+		WL_ERR(("Received frame length %d from dongle is greater than"
+			" allocated body buffer len %d", len, WL_FRAME_LEN));
+		goto exit;
+	}
+	memcpy(body, data, len);
+	wldev_iovar_getbuf_bsscfg(ndev, "cur_etheraddr",
+		NULL, 0, wl->ioctl_buf, WLC_IOCTL_MAXLEN, bsscfgidx, &wl->ioctl_buf_sync);
+	memcpy(da.octet, wl->ioctl_buf, ETHER_ADDR_LEN);
+	err = wldev_ioctl(ndev, WLC_GET_BSSID, &bssid, ETHER_ADDR_LEN, false);
+	switch (event) {
+		case WLC_E_ASSOC_IND:
+			fc = FC_ASSOC_REQ;
+			break;
+		case WLC_E_REASSOC_IND:
+			fc = FC_REASSOC_REQ;
+			break;
+		case WLC_E_DISASSOC_IND:
+			fc = FC_DISASSOC;
+			break;
+		case WLC_E_DEAUTH_IND:
+			fc = FC_DISASSOC;
+			break;
+		case WLC_E_DEAUTH:
+			fc = FC_DISASSOC;
+			break;
+		default:
+			fc = 0;
+			goto exit;
+	}
+	if ((err = wldev_ioctl(ndev, WLC_GET_CHANNEL, &ci, sizeof(ci), false)))
+		return err;
+
+	channel = dtoh32(ci.hw_channel);
+	if (channel <= CH_MAX_2G_CHANNEL)
+		band = wiphy->bands[IEEE80211_BAND_2GHZ];
+	else
+		band = wiphy->bands[IEEE80211_BAND_5GHZ];
+	if (!band) {
+		WL_ERR(("No valid band"));
+		return -EINVAL;
+	}
+#if LINUX_VERSION_CODE == KERNEL_VERSION(2, 6, 38) && !defined(WL_COMPAT_WIRELESS)
+	freq = ieee80211_channel_to_frequency(channel);
+#else
+	freq = ieee80211_channel_to_frequency(channel, band->band);
+#endif
+
+	err = wl_frame_get_mgmt(fc, &da, &e->addr, &bssid,
+		&mgmt_frame, &len, body);
+	if (err < 0)
+		goto exit;
+	isfree = true;
+
+	if (event == WLC_E_ASSOC_IND && reason == DOT11_SC_SUCCESS) {
+		cfg80211_rx_mgmt(ndev, freq, mgmt_frame, len, GFP_ATOMIC);
+	} else if (event == WLC_E_DISASSOC_IND) {
+		cfg80211_rx_mgmt(ndev, freq, mgmt_frame, len, GFP_ATOMIC);
+	} else if ((event == WLC_E_DEAUTH_IND) || (event == WLC_E_DEAUTH)) {
+		cfg80211_rx_mgmt(ndev, freq, mgmt_frame, len, GFP_ATOMIC);
+	}
+
+exit:
+	if (isfree)
+		kfree(mgmt_frame);
+	return err;
+#else /* LINUX_VERSION_CODE < KERNEL_VERSION(3, 2, 0) && !WL_CFG80211_STA_EVENT */
+	sinfo.filled = 0;
+	if (((event == WLC_E_ASSOC_IND) || (event == WLC_E_REASSOC_IND)) &&
+		reason == DOT11_SC_SUCCESS) {
+		sinfo.filled = STATION_INFO_ASSOC_REQ_IES;
+		if (!data) {
+			WL_ERR(("No IEs present in ASSOC/REASSOC_IND"));
+			return -EINVAL;
+		}
+		sinfo.assoc_req_ies = data;
+		sinfo.assoc_req_ies_len = len;
+		cfg80211_new_sta(ndev, e->addr.octet, &sinfo, GFP_ATOMIC);
+	} else if (event == WLC_E_DISASSOC_IND) {
+		cfg80211_del_sta(ndev, e->addr.octet, GFP_ATOMIC);
+	} else if ((event == WLC_E_DEAUTH_IND) || (event == WLC_E_DEAUTH)) {
+		cfg80211_del_sta(ndev, e->addr.octet, GFP_ATOMIC);
+	}
+#endif /* LINUX_VERSION_CODE < KERNEL_VERSION(3, 2, 0) && !WL_CFG80211_STA_EVENT */
+	return err;
+}
+
+static s32
+wl_notify_connect_status(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data)
+{
+	bool act;
+	s32 err = 0;
+	u32 event = ntoh32(e->event_type);
+	u32 reason;
+
+	if (wl_get_mode_by_netdev(wl, ndev) == WL_MODE_AP) {
+		wl_notify_connect_status_ap(wl, ndev, e, data);
+	} else {
+		WL_DBG(("wl_notify_connect_status : event %d status : %d ndev %p\n",
+			ntoh32(e->event_type), ntoh32(e->status), ndev));
+		if((event == WLC_E_DEAUTH_IND) || (event == WLC_E_DISASSOC_IND)) {
+			reason = ntoh32(e->reason);
+			wl->deauth_reason = reason;
+			WL_ERR(("Received %s event with reason code: %d\n",
+			(event == WLC_E_DEAUTH_IND)?
+			"WLC_E_DEAUTH_IND":"WLC_E_DISASSOC_IND", reason));
+		}
+		if (wl_is_linkup(wl, e, ndev)) {
+			wl_link_up(wl);
+			act = true;
+			wl_update_prof(wl, ndev, e, &act, WL_PROF_ACT);
+			wl_update_prof(wl, ndev, NULL, (void *)&e->addr, WL_PROF_BSSID);
+			wl->deauth_reason = 0;
+			if (wl_is_ibssmode(wl, ndev)) {
+				wl_ibss_join_done(wl, ndev, e, data, true);
+				WL_DBG(("wl_ibss_join_done succeeded\n"));
+			} else {
+				if (!wl_get_drv_status(wl, DISCONNECTING, ndev)) {
+					printk("wl_bss_connect_done succeeded\n");
+					wl_bss_connect_done(wl, ndev, e, data, true);
+					WL_DBG(("joined in BSS network \"%s\"\n",
+					((struct wlc_ssid *)
+					 wl_read_prof(wl, ndev, WL_PROF_SSID))->SSID));
+				}
+			}
+		} else if (wl_is_linkdown(wl, e)) {
+			if (wl->scan_request) {
+				if (wl->escan_on) {
+					wl_notify_escan_complete(wl, ndev, true, true);
+				} else {
+					del_timer_sync(&wl->scan_timeout);
+					wl_iscan_aborted(wl);
+				}
+			}
+			if (wl_get_drv_status(wl, CONNECTED, ndev)) {
+				scb_val_t scbval;
+				u8 *curbssid = wl_read_prof(wl, ndev, WL_PROF_BSSID);
+				wl_clr_drv_status(wl, CONNECTED, ndev);
+				if (! wl_get_drv_status(wl, DISCONNECTING, ndev)) {
+					/* To make sure disconnect, explictly send dissassoc
+					*  for BSSID 00:00:00:00:00:00 issue
+					*/
+					scbval.val = WLAN_REASON_DEAUTH_LEAVING;
+
+					memcpy(&scbval.ea, curbssid, ETHER_ADDR_LEN);
+					scbval.val = htod32(scbval.val);
+					wldev_ioctl(ndev, WLC_DISASSOC, &scbval,
+						sizeof(scb_val_t), true);
+					WL_ERR(("link down, calling cfg80211_disconnected"
+					" with deauth_reason:%d\n", wl->deauth_reason));
+					if (!wl_is_ibssmode(wl, ndev))
+						cfg80211_disconnected(ndev, wl->deauth_reason,
+							NULL, 0, GFP_KERNEL);
+					wl_link_down(wl);
+					wl_init_prof(wl, ndev);
+				}
+			}
+			else if (wl_get_drv_status(wl, CONNECTING, ndev)) {
+				printk("link down, during connecting\n");
+				if (wl_is_ibssmode(wl, ndev))
+					wl_ibss_join_done(wl, ndev, e, data, false);
+				else
+					wl_bss_connect_done(wl, ndev, e, data, false);
+			}
+			wl_clr_drv_status(wl, DISCONNECTING, ndev);
+
+		} else if (wl_is_nonetwork(wl, e)) {
+			printk("connect failed event=%d e->status 0x%x\n",
+				event, (int)ntoh32(e->status));
+			/* Clean up any pending scan request */
+			if (wl->scan_request) {
+				if (wl->escan_on) {
+					wl_notify_escan_complete(wl, ndev, true, true);
+				} else {
+					del_timer_sync(&wl->scan_timeout);
+					wl_iscan_aborted(wl);
+				}
+			}
+			if (wl_get_drv_status(wl, CONNECTING, ndev))
+				wl_bss_connect_done(wl, ndev, e, data, false);
+		} else {
+			printk("%s nothing\n", __FUNCTION__);
+		}
+	}
+	return err;
+}
+
+static s32
+wl_notify_roaming_status(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data)
+{
+	bool act;
+	s32 err = 0;
+	u32 event = be32_to_cpu(e->event_type);
+	u32 status = be32_to_cpu(e->status);
+	WL_DBG(("Enter \n"));
+	if (event == WLC_E_ROAM && status == WLC_E_STATUS_SUCCESS) {
+		if (wl_get_drv_status(wl, CONNECTED, ndev))
+			wl_bss_roaming_done(wl, ndev, e, data);
+		else
+			wl_bss_connect_done(wl, ndev, e, data, true);
+		act = true;
+		wl_update_prof(wl, ndev, e, &act, WL_PROF_ACT);
+		wl_update_prof(wl, ndev, NULL, (void *)&e->addr, WL_PROF_BSSID);
+	}
+	return err;
+}
+
+static s32 wl_get_assoc_ies(struct wl_priv *wl, struct net_device *ndev)
+{
+	wl_assoc_info_t assoc_info;
+	struct wl_connect_info *conn_info = wl_to_conn(wl);
+	s32 err = 0;
+
+	WL_DBG(("Enter \n"));
+	err = wldev_iovar_getbuf(ndev, "assoc_info", NULL, 0, wl->extra_buf,
+		WL_ASSOC_INFO_MAX, NULL);
+	if (unlikely(err)) {
+		WL_ERR(("could not get assoc info (%d)\n", err));
+		return err;
+	}
+	memcpy(&assoc_info, wl->extra_buf, sizeof(wl_assoc_info_t));
+	assoc_info.req_len = htod32(assoc_info.req_len);
+	assoc_info.resp_len = htod32(assoc_info.resp_len);
+	assoc_info.flags = htod32(assoc_info.flags);
+	if (conn_info->req_ie_len) {
+		conn_info->req_ie_len = 0;
+		bzero(conn_info->req_ie, sizeof(conn_info->req_ie));
+	}
+	if (conn_info->resp_ie_len) {
+		conn_info->resp_ie_len = 0;
+		bzero(conn_info->resp_ie, sizeof(conn_info->resp_ie));
+	}
+	if (assoc_info.req_len) {
+		err = wldev_iovar_getbuf(ndev, "assoc_req_ies", NULL, 0, wl->extra_buf,
+			WL_ASSOC_INFO_MAX, NULL);
+		if (unlikely(err)) {
+			WL_ERR(("could not get assoc req (%d)\n", err));
+			return err;
+		}
+		conn_info->req_ie_len = assoc_info.req_len - sizeof(struct dot11_assoc_req);
+		if (assoc_info.flags & WLC_ASSOC_REQ_IS_REASSOC) {
+			conn_info->req_ie_len -= ETHER_ADDR_LEN;
+		}
+		if (conn_info->req_ie_len <= MAX_REQ_LINE)
+			memcpy(conn_info->req_ie, wl->extra_buf, conn_info->req_ie_len);
+		else {
+			WL_ERR(("%s IE size %d above max %d size \n",
+				__FUNCTION__, conn_info->req_ie_len, MAX_REQ_LINE));
+			return err;
+		}
+	} else {
+		conn_info->req_ie_len = 0;
+	}
+	if (assoc_info.resp_len) {
+		err = wldev_iovar_getbuf(ndev, "assoc_resp_ies", NULL, 0, wl->extra_buf,
+			WL_ASSOC_INFO_MAX, NULL);
+		if (unlikely(err)) {
+			WL_ERR(("could not get assoc resp (%d)\n", err));
+			return err;
+		}
+		conn_info->resp_ie_len = assoc_info.resp_len -sizeof(struct dot11_assoc_resp);
+		if (conn_info->resp_ie_len <= MAX_REQ_LINE)
+			memcpy(conn_info->resp_ie, wl->extra_buf, conn_info->resp_ie_len);
+		else {
+			WL_ERR(("%s IE size %d above max %d size \n",
+				__FUNCTION__, conn_info->resp_ie_len, MAX_REQ_LINE));
+			return err;
+		}
+	} else {
+		conn_info->resp_ie_len = 0;
+	}
+	WL_DBG(("req len (%d) resp len (%d)\n", conn_info->req_ie_len,
+		conn_info->resp_ie_len));
+
+	return err;
+}
+
+static void wl_ch_to_chanspec(int ch, struct wl_join_params *join_params,
+        size_t *join_params_size)
+{
+	chanspec_t chanspec = 0;
+
+	if (ch != 0) {
+		join_params->params.chanspec_num = 1;
+		join_params->params.chanspec_list[0] = ch;
+
+		if (join_params->params.chanspec_list[0] <= CH_MAX_2G_CHANNEL)
+			chanspec |= WL_CHANSPEC_BAND_2G;
+		else
+			chanspec |= WL_CHANSPEC_BAND_5G;
+
+		chanspec |= WL_CHANSPEC_BW_20;
+		chanspec |= WL_CHANSPEC_CTL_SB_NONE;
+
+		*join_params_size += WL_ASSOC_PARAMS_FIXED_SIZE +
+			join_params->params.chanspec_num * sizeof(chanspec_t);
+
+		join_params->params.chanspec_list[0]  &= WL_CHANSPEC_CHAN_MASK;
+		join_params->params.chanspec_list[0] |= chanspec;
+		join_params->params.chanspec_list[0] =
+			htodchanspec(join_params->params.chanspec_list[0]);
+
+		join_params->params.chanspec_num =
+			htod32(join_params->params.chanspec_num);
+
+		WL_DBG(("%s  join_params->params.chanspec_list[0]= %X\n",
+			__FUNCTION__, join_params->params.chanspec_list[0]));
+
+	}
+}
+
+static s32 wl_update_bss_info(struct wl_priv *wl, struct net_device *ndev)
+{
+	struct cfg80211_bss *bss;
+	struct wl_bss_info *bi;
+	struct wlc_ssid *ssid;
+	struct bcm_tlv *tim;
+	s32 beacon_interval;
+	s32 dtim_period;
+	size_t ie_len;
+	u8 *ie;
+	u8 *curbssid;
+	s32 err = 0;
+	struct wiphy *wiphy;
+
+	wiphy = wl_to_wiphy(wl);
+
+	if (wl_is_ibssmode(wl, ndev))
+		return err;
+
+	ssid = (struct wlc_ssid *)wl_read_prof(wl, ndev, WL_PROF_SSID);
+	curbssid = wl_read_prof(wl, ndev, WL_PROF_BSSID);
+	bss = cfg80211_get_bss(wiphy, NULL, curbssid,
+		ssid->SSID, ssid->SSID_len, WLAN_CAPABILITY_ESS,
+		WLAN_CAPABILITY_ESS);
+
+	mutex_lock(&wl->usr_sync);
+	if (!bss) {
+		WL_DBG(("Could not find the AP\n"));
+		*(u32 *) wl->extra_buf = htod32(WL_EXTRA_BUF_MAX);
+		err = wldev_ioctl(ndev, WLC_GET_BSS_INFO,
+			wl->extra_buf, WL_EXTRA_BUF_MAX, false);
+		if (unlikely(err)) {
+			WL_ERR(("Could not get bss info %d\n", err));
+			goto update_bss_info_out;
+		}
+		bi = (struct wl_bss_info *)(wl->extra_buf + 4);
+		if (memcmp(bi->BSSID.octet, curbssid, ETHER_ADDR_LEN)) {
+			err = -EIO;
+			goto update_bss_info_out;
+		}
+		err = wl_inform_single_bss(wl, bi);
+		if (unlikely(err))
+			goto update_bss_info_out;
+
+		ie = ((u8 *)bi) + bi->ie_offset;
+		ie_len = bi->ie_length;
+		beacon_interval = cpu_to_le16(bi->beacon_period);
+	} else {
+		WL_DBG(("Found the AP in the list - BSSID %pM\n", bss->bssid));
+		ie = bss->information_elements;
+		ie_len = bss->len_information_elements;
+		beacon_interval = bss->beacon_interval;
+		cfg80211_put_bss(bss);
+	}
+
+	tim = bcm_parse_tlvs(ie, ie_len, WLAN_EID_TIM);
+	if (tim) {
+		dtim_period = tim->data[1];
+	} else {
+		/*
+		* active scan was done so we could not get dtim
+		* information out of probe response.
+		* so we speficially query dtim information.
+		*/
+		err = wldev_ioctl(ndev, WLC_GET_DTIMPRD,
+			&dtim_period, sizeof(dtim_period), false);
+		if (unlikely(err)) {
+			WL_ERR(("WLC_GET_DTIMPRD error (%d)\n", err));
+			goto update_bss_info_out;
+		}
+	}
+
+	wl_update_prof(wl, ndev, NULL, &beacon_interval, WL_PROF_BEACONINT);
+	wl_update_prof(wl, ndev, NULL, &dtim_period, WL_PROF_DTIMPERIOD);
+
+update_bss_info_out:
+	mutex_unlock(&wl->usr_sync);
+	return err;
+}
+
+static s32
+wl_bss_roaming_done(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data)
+{
+	struct wl_connect_info *conn_info = wl_to_conn(wl);
+	s32 err = 0;
+	u8 *curbssid;
+
+	wl_get_assoc_ies(wl, ndev);
+	wl_update_prof(wl, ndev, NULL, (void *)(e->addr.octet), WL_PROF_BSSID);
+	curbssid = wl_read_prof(wl, ndev, WL_PROF_BSSID);
+	wl_update_bss_info(wl, ndev);
+	wl_update_pmklist(ndev, wl->pmk_list, err);
+	cfg80211_roamed(ndev,
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2, 6, 39)
+		NULL,
+#endif
+		curbssid,
+		conn_info->req_ie, conn_info->req_ie_len,
+		conn_info->resp_ie, conn_info->resp_ie_len, GFP_KERNEL);
+	WL_DBG(("Report roaming result\n"));
+
+	wl_set_drv_status(wl, CONNECTED, ndev);
+
+	return err;
+}
+
+static s32
+wl_bss_connect_done(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data, bool completed)
+{
+	struct wl_connect_info *conn_info = wl_to_conn(wl);
+	s32 err = 0;
+	u8 *curbssid = wl_read_prof(wl, ndev, WL_PROF_BSSID);
+
+	WL_DBG((" enter\n"));
+	if (wl->scan_request) {
+		wl_notify_escan_complete(wl, ndev, true, true);
+	}
+	if (wl_get_drv_status(wl, CONNECTING, ndev)) {
+		wl_clr_drv_status(wl, CONNECTING, ndev);
+		if (completed) {
+			wl_get_assoc_ies(wl, ndev);
+			wl_update_prof(wl, ndev, NULL, (void *)(e->addr.octet), WL_PROF_BSSID);
+			curbssid = wl_read_prof(wl, ndev, WL_PROF_BSSID);
+			wl_update_bss_info(wl, ndev);
+			wl_update_pmklist(ndev, wl->pmk_list, err);
+			wl_set_drv_status(wl, CONNECTED, ndev);
+		}
+		cfg80211_connect_result(ndev,
+			curbssid,
+			conn_info->req_ie,
+			conn_info->req_ie_len,
+			conn_info->resp_ie,
+			conn_info->resp_ie_len,
+			completed ? WLAN_STATUS_SUCCESS : WLAN_STATUS_AUTH_TIMEOUT,
+			GFP_KERNEL);
+		if (completed)
+			WL_INFO(("Report connect result - connection succeeded\n"));
+		else
+			WL_ERR(("Report connect result - connection failed\n"));
+	}
+	return err;
+}
+
+static s32
+wl_ibss_join_done(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data, bool completed)
+{
+	s32 err = 0;
+
+	WL_TRACE(("Enter\n"));
+
+	if (wl->scan_request) {
+		wl_notify_escan_complete(wl, ndev, true, true);
+	}
+	if (wl_get_drv_status(wl, CONNECTING, ndev)) {
+		wl_clr_drv_status(wl, CONNECTING, ndev);
+		if (completed) {
+			err = wl_inform_ibss(wl, (u8 *)&e->addr);
+			if (err) {
+				WL_ERR(("wl_inform_ibss() failed: %d\n", err));
+			}
+			wl_set_drv_status(wl, CONNECTED, ndev);
+
+			cfg80211_ibss_joined(ndev, (u8 *)&e->addr, GFP_KERNEL);
+			WL_DBG(("cfg80211_ibss_joined() called with valid BSSID\n"));
+		}
+	}
+
+	WL_TRACE(("Exit\n"));
+	return err;
+}
+
+static s32
+wl_notify_mic_status(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data)
+{
+	u16 flags = ntoh16(e->flags);
+	enum nl80211_key_type key_type;
+
+	mutex_lock(&wl->usr_sync);
+	if (flags & WLC_EVENT_MSG_GROUP)
+		key_type = NL80211_KEYTYPE_GROUP;
+	else
+		key_type = NL80211_KEYTYPE_PAIRWISE;
+
+	cfg80211_michael_mic_failure(ndev, (u8 *)&e->addr, key_type, -1,
+		NULL, GFP_KERNEL);
+	mutex_unlock(&wl->usr_sync);
+
+	return 0;
+}
+
+#ifdef PNO_SUPPORT
+static s32
+wl_notify_pfn_status(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data)
+{
+	WL_ERR((" PNO Event\n"));
+
+	mutex_lock(&wl->usr_sync);
+#ifndef WL_SCHED_SCAN
+	/* TODO: Use cfg80211_sched_scan_results(wiphy); */
+	cfg80211_disconnected(ndev, 0, NULL, 0, GFP_KERNEL);
+#else
+	/* If cfg80211 scheduled scan is supported, report the pno results via sched
+	 * scan results
+	 */
+	wl_notify_sched_scan_results(wl, ndev, e, data);
+#endif /* WL_SCHED_SCAN */
+	mutex_unlock(&wl->usr_sync);
+	return 0;
+}
+#endif /* PNO_SUPPORT */
+
+static s32
+wl_notify_scan_status(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data)
+{
+	struct channel_info channel_inform;
+	struct wl_scan_results *bss_list;
+	u32 len = WL_SCAN_BUF_MAX;
+	s32 err = 0;
+	unsigned long flags;
+
+	WL_DBG(("Enter \n"));
+	if (!wl_get_drv_status(wl, SCANNING, ndev)) {
+		WL_ERR(("scan is not ready \n"));
+		return err;
+	}
+	if (wl->iscan_on && wl->iscan_kickstart)
+		return wl_wakeup_iscan(wl_to_iscan(wl));
+
+	mutex_lock(&wl->usr_sync);
+	wl_clr_drv_status(wl, SCANNING, ndev);
+	err = wldev_ioctl(ndev, WLC_GET_CHANNEL, &channel_inform,
+		sizeof(channel_inform), false);
+	if (unlikely(err)) {
+		WL_ERR(("scan busy (%d)\n", err));
+		goto scan_done_out;
+	}
+	channel_inform.scan_channel = dtoh32(channel_inform.scan_channel);
+	if (unlikely(channel_inform.scan_channel)) {
+
+		WL_DBG(("channel_inform.scan_channel (%d)\n",
+			channel_inform.scan_channel));
+	}
+	wl->bss_list = wl->scan_results;
+	bss_list = wl->bss_list;
+	memset(bss_list, 0, len);
+	bss_list->buflen = htod32(len);
+	err = wldev_ioctl(ndev, WLC_SCAN_RESULTS, bss_list, len, false);
+	if (unlikely(err)) {
+		WL_ERR(("%s Scan_results error (%d)\n", ndev->name, err));
+		err = -EINVAL;
+		goto scan_done_out;
+	}
+	bss_list->buflen = dtoh32(bss_list->buflen);
+	bss_list->version = dtoh32(bss_list->version);
+	bss_list->count = dtoh32(bss_list->count);
+
+	err = wl_inform_bss(wl);
+
+scan_done_out:
+	del_timer_sync(&wl->scan_timeout);
+	spin_lock_irqsave(&wl->cfgdrv_lock, flags);
+	if (wl->scan_request) {
+		WL_DBG(("cfg80211_scan_done\n"));
+		cfg80211_scan_done(wl->scan_request, false);
+		wl->scan_request = NULL;
+	}
+	spin_unlock_irqrestore(&wl->cfgdrv_lock, flags);
+	mutex_unlock(&wl->usr_sync);
+	return err;
+}
+static s32
+wl_frame_get_mgmt(u16 fc, const struct ether_addr *da,
+	const struct ether_addr *sa, const struct ether_addr *bssid,
+	u8 **pheader, u32 *body_len, u8 *pbody)
+{
+	struct dot11_management_header *hdr;
+	u32 totlen = 0;
+	s32 err = 0;
+	u8 *offset;
+	u32 prebody_len = *body_len;
+	switch (fc) {
+		case FC_ASSOC_REQ:
+			/* capability , listen interval */
+			totlen = DOT11_ASSOC_REQ_FIXED_LEN;
+			*body_len += DOT11_ASSOC_REQ_FIXED_LEN;
+			break;
+
+		case FC_REASSOC_REQ:
+			/* capability, listen inteval, ap address */
+			totlen = DOT11_REASSOC_REQ_FIXED_LEN;
+			*body_len += DOT11_REASSOC_REQ_FIXED_LEN;
+			break;
+	}
+	totlen += DOT11_MGMT_HDR_LEN + prebody_len;
+	*pheader = kzalloc(totlen, GFP_KERNEL);
+	if (*pheader == NULL) {
+		WL_ERR(("memory alloc failed \n"));
+		return -ENOMEM;
+	}
+	hdr = (struct dot11_management_header *) (*pheader);
+	hdr->fc = htol16(fc);
+	hdr->durid = 0;
+	hdr->seq = 0;
+	offset = (u8*)(hdr + 1) + (totlen - DOT11_MGMT_HDR_LEN - prebody_len);
+	bcopy((const char*)da, (u8*)&hdr->da, ETHER_ADDR_LEN);
+	bcopy((const char*)sa, (u8*)&hdr->sa, ETHER_ADDR_LEN);
+	bcopy((const char*)bssid, (u8*)&hdr->bssid, ETHER_ADDR_LEN);
+	bcopy((const char*)pbody, offset, prebody_len);
+	*body_len = totlen;
+	return err;
+}
+static s32
+wl_notify_rx_mgmt_frame(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data)
+{
+	struct ieee80211_supported_band *band;
+	struct wiphy *wiphy = wl_to_wiphy(wl);
+	struct ether_addr da;
+	struct ether_addr bssid;
+	bool isfree = false;
+	s32 err = 0;
+	s32 freq;
+	struct net_device *dev = NULL;
+	wifi_p2p_pub_act_frame_t *act_frm = NULL;
+	wifi_p2p_action_frame_t *p2p_act_frm = NULL;
+	wifi_p2psd_gas_pub_act_frame_t *sd_act_frm = NULL;
+	wl_event_rx_frame_data_t *rxframe =
+		(wl_event_rx_frame_data_t*)data;
+	u32 event = ntoh32(e->event_type);
+	u8 *mgmt_frame;
+	u8 bsscfgidx = e->bsscfgidx;
+	u32 mgmt_frame_len = ntoh32(e->datalen) - sizeof(wl_event_rx_frame_data_t);
+	u16 channel = ((ntoh16(rxframe->channel) & WL_CHANSPEC_CHAN_MASK));
+
+	memset(&bssid, 0, ETHER_ADDR_LEN);
+
+	if (wl->p2p_net == ndev) {
+		dev = wl_to_prmry_ndev(wl);
+	} else {
+		dev = ndev;
+	}
+
+	if (channel <= CH_MAX_2G_CHANNEL)
+		band = wiphy->bands[IEEE80211_BAND_2GHZ];
+	else
+		band = wiphy->bands[IEEE80211_BAND_5GHZ];
+	if (!band) {
+		WL_ERR(("No valid band"));
+		return -EINVAL;
+	}
+
+	if ((event == WLC_E_P2P_PROBREQ_MSG) &&
+		wl->p2p && wl_get_p2p_status(wl, GO_NEG_PHASE)) {
+		WL_DBG(("Filtering P2P probe_req while being in GO-Neg state\n"));
+		goto exit;
+	}
+
+#if LINUX_VERSION_CODE == KERNEL_VERSION(2, 6, 38) && !defined(WL_COMPAT_WIRELESS)
+	freq = ieee80211_channel_to_frequency(channel);
+#else
+	freq = ieee80211_channel_to_frequency(channel, band->band);
+#endif
+	if (event == WLC_E_ACTION_FRAME_RX) {
+		wldev_iovar_getbuf_bsscfg(dev, "cur_etheraddr",
+			NULL, 0, wl->ioctl_buf, WLC_IOCTL_MAXLEN, bsscfgidx, &wl->ioctl_buf_sync);
+
+		wldev_ioctl(dev, WLC_GET_BSSID, &bssid, ETHER_ADDR_LEN, false);
+		memcpy(da.octet, wl->ioctl_buf, ETHER_ADDR_LEN);
+		err = wl_frame_get_mgmt(FC_ACTION, &da, &e->addr, &bssid,
+			&mgmt_frame, &mgmt_frame_len,
+			(u8 *)((wl_event_rx_frame_data_t *)rxframe + 1));
+		if (err < 0) {
+			WL_ERR(("%s: Error in receiving action frame len %d channel %d freq %d\n",
+				__func__, mgmt_frame_len, channel, freq));
+			goto exit;
+		}
+		isfree = true;
+		if (wl_cfgp2p_is_pub_action(&mgmt_frame[DOT11_MGMT_HDR_LEN],
+			mgmt_frame_len - DOT11_MGMT_HDR_LEN)) {
+			act_frm = (wifi_p2p_pub_act_frame_t *)
+					(&mgmt_frame[DOT11_MGMT_HDR_LEN]);
+		} else if (wl_cfgp2p_is_p2p_action(&mgmt_frame[DOT11_MGMT_HDR_LEN],
+			mgmt_frame_len - DOT11_MGMT_HDR_LEN)) {
+			p2p_act_frm = (wifi_p2p_action_frame_t *)
+					(&mgmt_frame[DOT11_MGMT_HDR_LEN]);
+			(void) p2p_act_frm;
+		} else if (wl_cfgp2p_is_gas_action(&mgmt_frame[DOT11_MGMT_HDR_LEN],
+			mgmt_frame_len - DOT11_MGMT_HDR_LEN)) {
+			sd_act_frm = (wifi_p2psd_gas_pub_act_frame_t *)
+					(&mgmt_frame[DOT11_MGMT_HDR_LEN]);
+			(void) sd_act_frm;
+		}
+		wl_cfgp2p_print_actframe(false, &mgmt_frame[DOT11_MGMT_HDR_LEN],
+			mgmt_frame_len - DOT11_MGMT_HDR_LEN);
+		/*
+		 * After complete GO Negotiation, roll back to mpc mode
+		 */
+		if (act_frm && ((act_frm->subtype == P2P_PAF_GON_CONF) ||
+			(act_frm->subtype == P2P_PAF_PROVDIS_RSP))) {
+			wldev_iovar_setint(dev, "mpc", 1);
+		}
+
+		if (act_frm && (act_frm->subtype == P2P_PAF_GON_CONF)) {
+			WL_DBG(("P2P: GO_NEG_PHASE status cleared \n"));
+			wl_clr_p2p_status(wl, GO_NEG_PHASE);
+		}
+	} else {
+		mgmt_frame = (u8 *)((wl_event_rx_frame_data_t *)rxframe + 1);
+	}
+
+	cfg80211_rx_mgmt(ndev, freq, mgmt_frame, mgmt_frame_len, GFP_ATOMIC);
+
+	WL_DBG(("%s: mgmt_frame_len (%d) , e->datalen (%d), channel (%d), freq (%d)\n", __func__,
+		mgmt_frame_len, ntoh32(e->datalen), channel, freq));
+
+	if (isfree)
+		kfree(mgmt_frame);
+exit:
+	return 0;
+}
+
+#ifdef WL_SCHED_SCAN
+/* If target scan is not reliable, set the below define to "1" to do a
+ * full escan
+ */
+#define FULL_ESCAN_ON_PFN_NET_FOUND		0
+static s32
+wl_notify_sched_scan_results(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data)
+{
+	wl_pfn_net_info_t *netinfo, *pnetinfo;
+	struct cfg80211_scan_request request;
+	struct wiphy *wiphy	= wl_to_wiphy(wl);
+	int err = 0;
+	struct cfg80211_ssid ssid[MAX_PFN_LIST_COUNT];
+	struct ieee80211_channel *channel = NULL;
+	int channel_req = 0;
+	int band = 0;
+	struct wl_pfn_scanresults *pfn_result = (struct wl_pfn_scanresults *)data;
+
+	WL_DBG(("Enter\n"));
+
+	if (e->event_type == WLC_E_PFN_NET_LOST) {
+		WL_DBG(("PFN NET LOST event. Do Nothing \n"));
+		return 0;
+	}
+	WL_DBG(("PFN NET FOUND event. count:%d \n", pfn_result->count));
+	if (pfn_result->count > 0) {
+		int i;
+
+		memset(&request, 0x00, sizeof(struct cfg80211_scan_request));
+		memset(&ssid, 0x00, sizeof(ssid));
+		request.wiphy = wiphy;
+
+		pnetinfo = (wl_pfn_net_info_t *)(data + sizeof(wl_pfn_scanresults_t)
+				- sizeof(wl_pfn_net_info_t));
+		channel = (struct ieee80211_channel *)kzalloc(
+			(sizeof(struct ieee80211_channel) * MAX_PFN_LIST_COUNT),
+			GFP_KERNEL);
+		if (!channel) {
+			WL_ERR(("No memory"));
+			err = -ENOMEM;
+			goto out_err;
+		}
+
+		for (i = 0; i < pfn_result->count; i++) {
+			netinfo = &pnetinfo[i];
+			if (!netinfo) {
+				WL_ERR(("Invalid netinfo ptr. index:%d", i));
+				err = -EINVAL;
+				goto out_err;
+			}
+			WL_DBG(("SSID:%s Channel:%d \n",
+				netinfo->pfnsubnet.SSID, netinfo->pfnsubnet.channel));
+			/* PFN result doesn't have all the info which are required by the supplicant
+			 * (For e.g IEs) Do a target Escan so that sched scan results are reported
+			 * via wl_inform_single_bss in the required format. Escan does require the
+			 * scan request in the form of cfg80211_scan_request. For timebeing, create
+			 * cfg80211_scan_request one out of the received PNO event.
+			 */
+			memcpy(ssid[i].ssid, netinfo->pfnsubnet.SSID,
+				netinfo->pfnsubnet.SSID_len);
+			ssid[i].ssid_len = netinfo->pfnsubnet.SSID_len;
+			request.n_ssids++;
+
+			channel_req = netinfo->pfnsubnet.channel;
+			band = (channel_req <= CH_MAX_2G_CHANNEL) ? NL80211_BAND_2GHZ
+				: NL80211_BAND_5GHZ;
+			channel[i].center_freq = ieee80211_channel_to_frequency(channel_req, band);
+			channel[i].band = band;
+			channel[i].flags |= IEEE80211_CHAN_NO_HT40;
+			request.channels[i] = &channel[i];
+			request.n_channels++;
+		}
+
+		/* assign parsed ssid array */
+		if (request.n_ssids)
+			request.ssids = &ssid[0];
+
+		if (wl_get_drv_status_all(wl, SCANNING)) {
+			/* Abort any on-going scan */
+			wl_notify_escan_complete(wl, ndev, true, true);
+		}
+
+		if (wl_get_p2p_status(wl, DISCOVERY_ON)) {
+			err = wl_cfgp2p_discover_enable_search(wl, false);
+			if (unlikely(err)) {
+				wl_clr_drv_status(wl, SCANNING, ndev);
+				goto out_err;
+			}
+		}
+
+		wl_set_drv_status(wl, SCANNING, ndev);
+#if FULL_ESCAN_ON_PFN_NET_FOUND
+		err = wl_do_escan(wl, wiphy, ndev, NULL);
+#else
+		err = wl_do_escan(wl, wiphy, ndev, &request);
+#endif
+		if (err) {
+			wl_clr_drv_status(wl, SCANNING, ndev);
+			goto out_err;
+		}
+		wl->sched_scan_running = TRUE;
+	}
+	else {
+		WL_ERR(("FALSE PNO Event. (pfn_count == 0) \n"));
+	}
+out_err:
+	if (channel)
+		kfree(channel);
+	return err;
+}
+#endif /* WL_SCHED_SCAN */
+
+static void wl_init_conf(struct wl_conf *conf)
+{
+	WL_DBG(("Enter \n"));
+	conf->frag_threshold = (u32)-1;
+	conf->rts_threshold = (u32)-1;
+	conf->retry_short = (u32)-1;
+	conf->retry_long = (u32)-1;
+	conf->tx_power = -1;
+}
+
+static void wl_init_prof(struct wl_priv *wl, struct net_device *ndev)
+{
+	unsigned long flags;
+	struct wl_profile *profile = wl_get_profile_by_netdev(wl, ndev);
+
+	spin_lock_irqsave(&wl->cfgdrv_lock, flags);
+	memset(profile, 0, sizeof(struct wl_profile));
+	spin_unlock_irqrestore(&wl->cfgdrv_lock, flags);
+}
+
+static void wl_init_event_handler(struct wl_priv *wl)
+{
+	memset(wl->evt_handler, 0, sizeof(wl->evt_handler));
+
+	wl->evt_handler[WLC_E_SCAN_COMPLETE] = wl_notify_scan_status;
+	wl->evt_handler[WLC_E_LINK] = wl_notify_connect_status;
+	wl->evt_handler[WLC_E_DEAUTH_IND] = wl_notify_connect_status;
+	wl->evt_handler[WLC_E_DEAUTH] = wl_notify_connect_status;
+	wl->evt_handler[WLC_E_DISASSOC_IND] = wl_notify_connect_status;
+	wl->evt_handler[WLC_E_ASSOC_IND] = wl_notify_connect_status;
+	wl->evt_handler[WLC_E_REASSOC_IND] = wl_notify_connect_status;
+	wl->evt_handler[WLC_E_ROAM] = wl_notify_roaming_status;
+	wl->evt_handler[WLC_E_MIC_ERROR] = wl_notify_mic_status;
+	wl->evt_handler[WLC_E_SET_SSID] = wl_notify_connect_status;
+	wl->evt_handler[WLC_E_ACTION_FRAME_RX] = wl_notify_rx_mgmt_frame;
+	wl->evt_handler[WLC_E_PROBREQ_MSG] = wl_notify_rx_mgmt_frame;
+	wl->evt_handler[WLC_E_P2P_PROBREQ_MSG] = wl_notify_rx_mgmt_frame;
+	wl->evt_handler[WLC_E_P2P_DISC_LISTEN_COMPLETE] = wl_cfgp2p_listen_complete;
+	wl->evt_handler[WLC_E_ACTION_FRAME_COMPLETE] = wl_cfgp2p_action_tx_complete;
+	wl->evt_handler[WLC_E_ACTION_FRAME_OFF_CHAN_COMPLETE] = wl_cfgp2p_action_tx_complete;
+#ifdef PNO_SUPPORT
+	wl->evt_handler[WLC_E_PFN_NET_FOUND] = wl_notify_pfn_status;
+#endif /* PNO_SUPPORT */
+}
+
+static s32 wl_init_priv_mem(struct wl_priv *wl)
+{
+	WL_DBG(("Enter \n"));
+	wl->scan_results = (void *)kzalloc(WL_SCAN_BUF_MAX, GFP_KERNEL);
+	if (unlikely(!wl->scan_results)) {
+		WL_ERR(("Scan results alloc failed\n"));
+		goto init_priv_mem_out;
+	}
+	wl->conf = (void *)kzalloc(sizeof(*wl->conf), GFP_KERNEL);
+	if (unlikely(!wl->conf)) {
+		WL_ERR(("wl_conf alloc failed\n"));
+		goto init_priv_mem_out;
+	}
+	wl->scan_req_int =
+	    (void *)kzalloc(sizeof(*wl->scan_req_int), GFP_KERNEL);
+	if (unlikely(!wl->scan_req_int)) {
+		WL_ERR(("Scan req alloc failed\n"));
+		goto init_priv_mem_out;
+	}
+	wl->ioctl_buf = (void *)kzalloc(WLC_IOCTL_MAXLEN, GFP_KERNEL);
+	if (unlikely(!wl->ioctl_buf)) {
+		WL_ERR(("Ioctl buf alloc failed\n"));
+		goto init_priv_mem_out;
+	}
+	wl->escan_ioctl_buf = (void *)kzalloc(WLC_IOCTL_MAXLEN, GFP_KERNEL);
+	if (unlikely(!wl->escan_ioctl_buf)) {
+		WL_ERR(("Ioctl buf alloc failed\n"));
+		goto init_priv_mem_out;
+	}
+	wl->extra_buf = (void *)kzalloc(WL_EXTRA_BUF_MAX, GFP_KERNEL);
+	if (unlikely(!wl->extra_buf)) {
+		WL_ERR(("Extra buf alloc failed\n"));
+		goto init_priv_mem_out;
+	}
+	wl->iscan = (void *)kzalloc(sizeof(*wl->iscan), GFP_KERNEL);
+	if (unlikely(!wl->iscan)) {
+		WL_ERR(("Iscan buf alloc failed\n"));
+		goto init_priv_mem_out;
+	}
+	wl->pmk_list = (void *)kzalloc(sizeof(*wl->pmk_list), GFP_KERNEL);
+	if (unlikely(!wl->pmk_list)) {
+		WL_ERR(("pmk list alloc failed\n"));
+		goto init_priv_mem_out;
+	}
+	wl->sta_info = (void *)kzalloc(sizeof(*wl->sta_info), GFP_KERNEL);
+	if (unlikely(!wl->sta_info)) {
+		WL_ERR(("sta info  alloc failed\n"));
+		goto init_priv_mem_out;
+	}
+	wl->afx_hdl = (void *)kzalloc(sizeof(*wl->afx_hdl), GFP_KERNEL);
+	if (unlikely(!wl->afx_hdl)) {
+		WL_ERR(("afx hdl  alloc failed\n"));
+		goto init_priv_mem_out;
+	} else {
+		init_completion(&wl->act_frm_scan);
+		INIT_WORK(&wl->afx_hdl->work, wl_cfg80211_afx_handler);
+	}
+	return 0;
+
+init_priv_mem_out:
+	wl_deinit_priv_mem(wl);
+
+	return -ENOMEM;
+}
+
+static void wl_deinit_priv_mem(struct wl_priv *wl)
+{
+	kfree(wl->scan_results);
+	wl->scan_results = NULL;
+	kfree(wl->conf);
+	wl->conf = NULL;
+	kfree(wl->scan_req_int);
+	wl->scan_req_int = NULL;
+	kfree(wl->ioctl_buf);
+	wl->ioctl_buf = NULL;
+	kfree(wl->escan_ioctl_buf);
+	wl->escan_ioctl_buf = NULL;
+	kfree(wl->extra_buf);
+	wl->extra_buf = NULL;
+	kfree(wl->iscan);
+	wl->iscan = NULL;
+	kfree(wl->pmk_list);
+	wl->pmk_list = NULL;
+	kfree(wl->sta_info);
+	wl->sta_info = NULL;
+	if (wl->afx_hdl) {
+		cancel_work_sync(&wl->afx_hdl->work);
+		kfree(wl->afx_hdl);
+		wl->afx_hdl = NULL;
+	}
+
+	if (wl->ap_info) {
+		kfree(wl->ap_info->wpa_ie);
+		kfree(wl->ap_info->rsn_ie);
+		kfree(wl->ap_info->wps_ie);
+		kfree(wl->ap_info);
+		wl->ap_info = NULL;
+	}
+}
+
+static s32 wl_create_event_handler(struct wl_priv *wl)
+{
+	int ret = 0;
+	WL_DBG(("Enter \n"));
+
+	/* Do not use DHD in cfg driver */
+	wl->event_tsk.thr_pid = -1;
+	PROC_START(wl_event_handler, wl, &wl->event_tsk, 0);
+	if (wl->event_tsk.thr_pid < 0)
+		ret = -ENOMEM;
+	return ret;
+}
+
+static void wl_destroy_event_handler(struct wl_priv *wl)
+{
+	if (wl->event_tsk.thr_pid >= 0)
+		PROC_STOP(&wl->event_tsk);
+}
+
+static void wl_term_iscan(struct wl_priv *wl)
+{
+	struct wl_iscan_ctrl *iscan = wl_to_iscan(wl);
+	WL_TRACE(("In\n"));
+	if (wl->iscan_on && iscan->tsk) {
+		iscan->state = WL_ISCAN_STATE_IDLE;
+		WL_INFO(("SIGTERM\n"));
+		send_sig(SIGTERM, iscan->tsk, 1);
+		WL_DBG(("kthread_stop\n"));
+		kthread_stop(iscan->tsk);
+		iscan->tsk = NULL;
+	}
+}
+
+static void wl_notify_iscan_complete(struct wl_iscan_ctrl *iscan, bool aborted)
+{
+	struct wl_priv *wl = iscan_to_wl(iscan);
+	struct net_device *ndev = wl_to_prmry_ndev(wl);
+	unsigned long flags;
+
+	WL_DBG(("Enter \n"));
+	if (!wl_get_drv_status(wl, SCANNING, ndev)) {
+		wl_clr_drv_status(wl, SCANNING, ndev);
+		WL_ERR(("Scan complete while device not scanning\n"));
+		return;
+	}
+	spin_lock_irqsave(&wl->cfgdrv_lock, flags);
+	wl_clr_drv_status(wl, SCANNING, ndev);
+	if (likely(wl->scan_request)) {
+		cfg80211_scan_done(wl->scan_request, aborted);
+		wl->scan_request = NULL;
+	}
+	spin_unlock_irqrestore(&wl->cfgdrv_lock, flags);
+	wl->iscan_kickstart = false;
+}
+
+static s32 wl_wakeup_iscan(struct wl_iscan_ctrl *iscan)
+{
+	if (likely(iscan->state != WL_ISCAN_STATE_IDLE)) {
+		WL_DBG(("wake up iscan\n"));
+		up(&iscan->sync);
+		return 0;
+	}
+
+	return -EIO;
+}
+
+static s32
+wl_get_iscan_results(struct wl_iscan_ctrl *iscan, u32 *status,
+	struct wl_scan_results **bss_list)
+{
+	struct wl_iscan_results list;
+	struct wl_scan_results *results;
+	struct wl_iscan_results *list_buf;
+	s32 err = 0;
+
+	WL_DBG(("Enter \n"));
+	memset(iscan->scan_buf, 0, WL_ISCAN_BUF_MAX);
+	list_buf = (struct wl_iscan_results *)iscan->scan_buf;
+	results = &list_buf->results;
+	results->buflen = WL_ISCAN_RESULTS_FIXED_SIZE;
+	results->version = 0;
+	results->count = 0;
+
+	memset(&list, 0, sizeof(list));
+	list.results.buflen = htod32(WL_ISCAN_BUF_MAX);
+	err = wldev_iovar_getbuf(iscan->dev, "iscanresults", &list,
+		WL_ISCAN_RESULTS_FIXED_SIZE, iscan->scan_buf,
+		WL_ISCAN_BUF_MAX, NULL);
+	if (unlikely(err)) {
+		WL_ERR(("error (%d)\n", err));
+		return err;
+	}
+	results->buflen = dtoh32(results->buflen);
+	results->version = dtoh32(results->version);
+	results->count = dtoh32(results->count);
+	WL_DBG(("results->count = %d\n", results->count));
+	WL_DBG(("results->buflen = %d\n", results->buflen));
+	*status = dtoh32(list_buf->status);
+	*bss_list = results;
+
+	return err;
+}
+
+static s32 wl_iscan_done(struct wl_priv *wl)
+{
+	struct wl_iscan_ctrl *iscan = wl->iscan;
+	s32 err = 0;
+
+	iscan->state = WL_ISCAN_STATE_IDLE;
+	mutex_lock(&wl->usr_sync);
+	wl_inform_bss(wl);
+	wl_notify_iscan_complete(iscan, false);
+	mutex_unlock(&wl->usr_sync);
+
+	return err;
+}
+
+static s32 wl_iscan_pending(struct wl_priv *wl)
+{
+	struct wl_iscan_ctrl *iscan = wl->iscan;
+	s32 err = 0;
+
+	/* Reschedule the timer */
+	mod_timer(&iscan->timer, jiffies + iscan->timer_ms * HZ / 1000);
+	iscan->timer_on = 1;
+
+	return err;
+}
+
+static s32 wl_iscan_inprogress(struct wl_priv *wl)
+{
+	struct wl_iscan_ctrl *iscan = wl->iscan;
+	s32 err = 0;
+
+	mutex_lock(&wl->usr_sync);
+	wl_inform_bss(wl);
+	wl_run_iscan(iscan, NULL, WL_SCAN_ACTION_CONTINUE);
+	mutex_unlock(&wl->usr_sync);
+	/* Reschedule the timer */
+	mod_timer(&iscan->timer, jiffies + iscan->timer_ms * HZ / 1000);
+	iscan->timer_on = 1;
+
+	return err;
+}
+
+static s32 wl_iscan_aborted(struct wl_priv *wl)
+{
+	struct wl_iscan_ctrl *iscan = wl->iscan;
+	s32 err = 0;
+
+	iscan->state = WL_ISCAN_STATE_IDLE;
+	mutex_lock(&wl->usr_sync);
+	wl_notify_iscan_complete(iscan, true);
+	mutex_unlock(&wl->usr_sync);
+
+	return err;
+}
+
+static s32 wl_iscan_thread(void *data)
+{
+	struct wl_iscan_ctrl *iscan = (struct wl_iscan_ctrl *)data;
+	struct wl_priv *wl = iscan_to_wl(iscan);
+	u32 status;
+	int err = 0;
+
+	allow_signal(SIGTERM);
+	status = WL_SCAN_RESULTS_PARTIAL;
+	while (likely(!down_interruptible(&iscan->sync))) {
+		if (kthread_should_stop())
+			break;
+		if (iscan->timer_on) {
+			del_timer_sync(&iscan->timer);
+			iscan->timer_on = 0;
+		}
+		mutex_lock(&wl->usr_sync);
+		err = wl_get_iscan_results(iscan, &status, &wl->bss_list);
+		if (unlikely(err)) {
+			status = WL_SCAN_RESULTS_ABORTED;
+			WL_ERR(("Abort iscan\n"));
+		}
+		mutex_unlock(&wl->usr_sync);
+		iscan->iscan_handler[status] (wl);
+	}
+	if (iscan->timer_on) {
+		del_timer_sync(&iscan->timer);
+		iscan->timer_on = 0;
+	}
+	WL_DBG(("%s was terminated\n", __func__));
+
+	return 0;
+}
+
+static void wl_scan_timeout(unsigned long data)
+{
+	struct wl_priv *wl = (struct wl_priv *)data;
+
+	if (wl->scan_request) {
+		WL_ERR(("timer expired\n"));
+		if (wl->escan_on)
+			wl_notify_escan_complete(wl, wl->escan_info.ndev, true, false);
+		else
+			wl_notify_iscan_complete(wl_to_iscan(wl), true);
+	}
+}
+
+static void wl_iscan_timer(unsigned long data)
+{
+	struct wl_iscan_ctrl *iscan = (struct wl_iscan_ctrl *)data;
+
+	if (iscan) {
+		iscan->timer_on = 0;
+		WL_DBG(("timer expired\n"));
+		wl_wakeup_iscan(iscan);
+	}
+}
+
+static s32 wl_invoke_iscan(struct wl_priv *wl)
+{
+	struct wl_iscan_ctrl *iscan = wl_to_iscan(wl);
+	int err = 0;
+
+	if (wl->iscan_on && !iscan->tsk) {
+		iscan->state = WL_ISCAN_STATE_IDLE;
+		sema_init(&iscan->sync, 0);
+		iscan->tsk = kthread_run(wl_iscan_thread, iscan, "wl_iscan");
+		if (IS_ERR(iscan->tsk)) {
+			WL_ERR(("Could not create iscan thread\n"));
+			iscan->tsk = NULL;
+			return -ENOMEM;
+		}
+	}
+
+	return err;
+}
+
+static void wl_init_iscan_handler(struct wl_iscan_ctrl *iscan)
+{
+	memset(iscan->iscan_handler, 0, sizeof(iscan->iscan_handler));
+	iscan->iscan_handler[WL_SCAN_RESULTS_SUCCESS] = wl_iscan_done;
+	iscan->iscan_handler[WL_SCAN_RESULTS_PARTIAL] = wl_iscan_inprogress;
+	iscan->iscan_handler[WL_SCAN_RESULTS_PENDING] = wl_iscan_pending;
+	iscan->iscan_handler[WL_SCAN_RESULTS_ABORTED] = wl_iscan_aborted;
+	iscan->iscan_handler[WL_SCAN_RESULTS_NO_MEM] = wl_iscan_aborted;
+}
+
+static s32
+wl_cfg80211_netdev_notifier_call(struct notifier_block * nb,
+	unsigned long state,
+	void *ndev)
+{
+	struct net_device *dev = ndev;
+	struct wireless_dev *wdev = dev->ieee80211_ptr;
+	struct wl_priv *wl = wlcfg_drv_priv;
+
+	WL_DBG(("Enter \n"));
+	if (!wdev || !wl || dev == wl_to_prmry_ndev(wl))
+		return NOTIFY_DONE;
+	switch (state) {
+		case NETDEV_UNREGISTER:
+			/* after calling list_del_rcu(&wdev->list) */
+			wl_dealloc_netinfo(wl, ndev);
+			break;
+		case NETDEV_GOING_DOWN:
+			/* At NETDEV_DOWN state, wdev_cleanup_work work will be called.
+			*  In front of door, the function checks
+			*  whether current scan is working or not.
+			*  If the scanning is still working, wdev_cleanup_work call WARN_ON and
+			*  make the scan done forcibly.
+			*/
+			if (wl_get_drv_status(wl, SCANNING, dev)) {
+				if (wl->escan_on) {
+					wl_notify_escan_complete(wl, dev, true, true);
+				}
+			}
+			break;
+	}
+	return NOTIFY_DONE;
+}
+static struct notifier_block wl_cfg80211_netdev_notifier = {
+	.notifier_call = wl_cfg80211_netdev_notifier_call,
+};
+
+static s32 wl_notify_escan_complete(struct wl_priv *wl,
+	struct net_device *ndev,
+	bool aborted, bool fw_abort)
+{
+	wl_scan_params_t *params = NULL;
+	s32 params_size = 0;
+	s32 err = BCME_OK;
+	unsigned long flags;
+	struct net_device *dev;
+
+	WL_DBG(("Enter \n"));
+
+	if (wl->scan_request) {
+		if (wl->scan_request->dev == wl->p2p_net)
+			dev = wl_to_prmry_ndev(wl);
+		else
+			dev = wl->scan_request->dev;
+	}
+	else {
+		WL_ERR(("wl->scan_request is NULL may be internal scan."
+			"doing scan_abort for ndev %p primary %p p2p_net %p",
+				ndev, wl_to_prmry_ndev(wl), wl->p2p_net));
+		dev = ndev;
+	}
+	if (fw_abort && !in_atomic()) {
+		/* Our scan params only need space for 1 channel and 0 ssids */
+		params = wl_cfg80211_scan_alloc_params(-1, 0, &params_size);
+		if (params == NULL) {
+			WL_ERR(("scan params allocation failed \n"));
+			err = -ENOMEM;
+		} else {
+			/* Do a scan abort to stop the driver's scan engine */
+			err = wldev_ioctl(dev, WLC_SCAN, params, params_size, true);
+			if (err < 0) {
+				WL_ERR(("scan abort  failed \n"));
+			}
+		}
+	}
+	if (timer_pending(&wl->scan_timeout))
+		del_timer_sync(&wl->scan_timeout);
+	spin_lock_irqsave(&wl->cfgdrv_lock, flags);
+
+#ifdef WL_SCHED_SCAN
+	if (wl->sched_scan_req && !wl->scan_request) {
+		WL_DBG((" REPORTING SCHED SCAN RESULTS \n"));
+		if (aborted)
+			cfg80211_sched_scan_stopped(wl->sched_scan_req->wiphy);
+		else
+			cfg80211_sched_scan_results(wl->sched_scan_req->wiphy);
+		wl->sched_scan_running = FALSE;
+		wl->sched_scan_req = NULL;
+	}
+#endif /* WL_SCHED_SCAN */
+
+	if (likely(wl->scan_request)) {
+		cfg80211_scan_done(wl->scan_request, aborted);
+		wl->scan_request = NULL;
+	}
+	if (p2p_is_on(wl))
+		wl_clr_p2p_status(wl, SCANNING);
+	wl_clr_drv_status(wl, SCANNING, dev);
+	spin_unlock_irqrestore(&wl->cfgdrv_lock, flags);
+	if (params)
+		kfree(params);
+
+	return err;
+}
+
+static s32 wl_escan_handler(struct wl_priv *wl,
+	struct net_device *ndev,
+	const wl_event_msg_t *e, void *data)
+{
+	s32 err = BCME_OK;
+	s32 status = ntoh32(e->status);
+	wl_bss_info_t *bi;
+	wl_escan_result_t *escan_result;
+	wl_bss_info_t *bss = NULL;
+	wl_scan_results_t *list;
+	u32 bi_length;
+	u32 i;
+	u8 *p2p_dev_addr = NULL;
+	WL_DBG((" enter event type : %d, status : %d \n",
+		ntoh32(e->event_type), ntoh32(e->status)));
+	/* P2P SCAN is coming from primary interface */
+	if (wl_get_p2p_status(wl, SCANNING)) {
+		if (wl_get_drv_status_all(wl, SENDING_ACT_FRM))
+			ndev = wl->afx_hdl->dev;
+		else
+			ndev = wl->escan_info.ndev;
+
+	}
+	if (!ndev || !wl->escan_on ||
+		!wl_get_drv_status(wl, SCANNING, ndev)) {
+		WL_ERR(("escan is not ready ndev %p wl->escan_on %d drv_status 0x%x\n",
+			ndev, wl->escan_on, wl_get_drv_status(wl, SCANNING, ndev)));
+		return err;
+	}
+
+	if (status == WLC_E_STATUS_PARTIAL) {
+		WL_INFO(("WLC_E_STATUS_PARTIAL \n"));
+		escan_result = (wl_escan_result_t *) data;
+		if (!escan_result) {
+			WL_ERR(("Invalid escan result (NULL pointer)\n"));
+			goto exit;
+		}
+		if (dtoh16(escan_result->bss_count) != 1) {
+			WL_ERR(("Invalid bss_count %d: ignoring\n", escan_result->bss_count));
+			goto exit;
+		}
+		bi = escan_result->bss_info;
+		if (!bi) {
+			WL_ERR(("Invalid escan bss info (NULL pointer)\n"));
+			goto exit;
+		}
+		bi_length = dtoh32(bi->length);
+		if (bi_length != (dtoh32(escan_result->buflen) - WL_ESCAN_RESULTS_FIXED_SIZE)) {
+			WL_ERR(("Invalid bss_info length %d: ignoring\n", bi_length));
+			goto exit;
+		}
+
+		if (!(wl_to_wiphy(wl)->interface_modes & BIT(NL80211_IFTYPE_ADHOC))) {
+			if (dtoh16(bi->capability) & DOT11_CAP_IBSS) {
+				WL_DBG(("Ignoring IBSS result\n"));
+				goto exit;
+			}
+		}
+
+		if (wl_get_drv_status_all(wl, SENDING_ACT_FRM)) {
+			p2p_dev_addr = wl_cfgp2p_retreive_p2p_dev_addr(bi, bi_length);
+			if (p2p_dev_addr && !memcmp(p2p_dev_addr,
+				wl->afx_hdl->pending_tx_dst_addr.octet, ETHER_ADDR_LEN)) {
+				s32 channel = CHSPEC_CHANNEL(dtohchanspec(bi->chanspec));
+				WL_DBG(("ACTION FRAME SCAN : Peer " MACSTR " found, channel : %d\n",
+					MAC2STR(wl->afx_hdl->pending_tx_dst_addr.octet), channel));
+				wl_clr_p2p_status(wl, SCANNING);
+				wl->afx_hdl->peer_chan = channel;
+				complete(&wl->act_frm_scan);
+				goto exit;
+			}
+
+		} else {
+			list = (wl_scan_results_t *)wl->escan_info.escan_buf;
+			if (bi_length > ESCAN_BUF_SIZE - list->buflen) {
+				WL_ERR(("Buffer is too small: ignoring\n"));
+				goto exit;
+			}
+#define WLC_BSS_RSSI_ON_CHANNEL 0x0002
+			for (i = 0; i < list->count; i++) {
+				bss = bss ? (wl_bss_info_t *)((uintptr)bss + dtoh32(bss->length))
+					: list->bss_info;
+
+				if (!bcmp(&bi->BSSID, &bss->BSSID, ETHER_ADDR_LEN) &&
+					CHSPEC_BAND(bi->chanspec) == CHSPEC_BAND(bss->chanspec) &&
+					bi->SSID_len == bss->SSID_len &&
+					!bcmp(bi->SSID, bss->SSID, bi->SSID_len)) {
+					if ((bss->flags & WLC_BSS_RSSI_ON_CHANNEL) ==
+						(bi->flags & WLC_BSS_RSSI_ON_CHANNEL)) {
+						/* preserve max RSSI if the measurements are
+						* both on-channel or both off-channel
+						*/
+						bss->RSSI = MAX(bss->RSSI, bi->RSSI);
+					} else if ((bss->flags & WLC_BSS_RSSI_ON_CHANNEL) &&
+						(bi->flags & WLC_BSS_RSSI_ON_CHANNEL) == 0) {
+						/* preserve the on-channel rssi measurement
+						* if the new measurement is off channel
+						*/
+						bss->RSSI = bi->RSSI;
+						bss->flags |= WLC_BSS_RSSI_ON_CHANNEL;
+					}
+
+					goto exit;
+				}
+			}
+			memcpy(&(wl->escan_info.escan_buf[list->buflen]), bi, bi_length);
+			list->version = dtoh32(bi->version);
+			list->buflen += bi_length;
+			list->count++;
+
+		}
+
+	}
+	else if (status == WLC_E_STATUS_SUCCESS) {
+		wl->escan_info.escan_state = WL_ESCAN_STATE_IDLE;
+		if (wl_get_drv_status_all(wl, SENDING_ACT_FRM)) {
+			WL_INFO(("ACTION FRAME SCAN DONE\n"));
+			wl_clr_p2p_status(wl, SCANNING);
+			wl_clr_drv_status(wl, SCANNING, wl->afx_hdl->dev);
+			if (wl->afx_hdl->peer_chan == WL_INVALID)
+				complete(&wl->act_frm_scan);
+		} else if ((likely(wl->scan_request)) || (wl->sched_scan_running)) {
+			mutex_lock(&wl->usr_sync);
+			WL_INFO(("ESCAN COMPLETED\n"));
+			wl->bss_list = (wl_scan_results_t *)wl->escan_info.escan_buf;
+			wl_inform_bss(wl);
+			wl_notify_escan_complete(wl, ndev, false, false);
+			mutex_unlock(&wl->usr_sync);
+		}
+	}
+	else if (status == WLC_E_STATUS_ABORT) {
+		wl->escan_info.escan_state = WL_ESCAN_STATE_IDLE;
+		if (wl_get_drv_status_all(wl, SENDING_ACT_FRM)) {
+			WL_INFO(("ACTION FRAME SCAN DONE\n"));
+			wl_clr_drv_status(wl, SCANNING, wl->afx_hdl->dev);
+			wl_clr_p2p_status(wl, SCANNING);
+			if (wl->afx_hdl->peer_chan == WL_INVALID)
+				complete(&wl->act_frm_scan);
+		} else if ((likely(wl->scan_request)) || (wl->sched_scan_running)) {
+			mutex_lock(&wl->usr_sync);
+			WL_INFO(("ESCAN ABORTED\n"));
+			wl->bss_list = (wl_scan_results_t *)wl->escan_info.escan_buf;
+			wl_inform_bss(wl);
+			wl_notify_escan_complete(wl, ndev, true, false);
+			mutex_unlock(&wl->usr_sync);
+		}
+	}
+	else {
+		WL_ERR(("unexpected Escan Event %d : abort\n", status));
+		wl->escan_info.escan_state = WL_ESCAN_STATE_IDLE;
+		if (wl_get_drv_status_all(wl, SENDING_ACT_FRM)) {
+			WL_INFO(("ACTION FRAME SCAN DONE\n"));
+			wl_clr_p2p_status(wl, SCANNING);
+			wl_clr_drv_status(wl, SCANNING, wl->afx_hdl->dev);
+			if (wl->afx_hdl->peer_chan == WL_INVALID)
+				complete(&wl->act_frm_scan);
+		} else if ((likely(wl->scan_request)) || (wl->sched_scan_running)) {
+			mutex_lock(&wl->usr_sync);
+			wl->bss_list = (wl_scan_results_t *)wl->escan_info.escan_buf;
+			wl_inform_bss(wl);
+			wl_notify_escan_complete(wl, ndev, true, false);
+			mutex_unlock(&wl->usr_sync);
+		}
+	}
+exit:
+	return err;
+}
+
+static s32 wl_init_scan(struct wl_priv *wl)
+{
+	struct wl_iscan_ctrl *iscan = wl_to_iscan(wl);
+	int err = 0;
+
+	if (wl->iscan_on) {
+		iscan->dev = wl_to_prmry_ndev(wl);
+		iscan->state = WL_ISCAN_STATE_IDLE;
+		wl_init_iscan_handler(iscan);
+		iscan->timer_ms = WL_ISCAN_TIMER_INTERVAL_MS;
+		init_timer(&iscan->timer);
+		iscan->timer.data = (unsigned long) iscan;
+		iscan->timer.function = wl_iscan_timer;
+		sema_init(&iscan->sync, 0);
+		iscan->tsk = kthread_run(wl_iscan_thread, iscan, "wl_iscan");
+		if (IS_ERR(iscan->tsk)) {
+			WL_ERR(("Could not create iscan thread\n"));
+			iscan->tsk = NULL;
+			return -ENOMEM;
+		}
+		iscan->data = wl;
+	} else if (wl->escan_on) {
+		wl->evt_handler[WLC_E_ESCAN_RESULT] = wl_escan_handler;
+		wl->escan_info.escan_state = WL_ESCAN_STATE_IDLE;
+	}
+	/* Init scan_timeout timer */
+	init_timer(&wl->scan_timeout);
+	wl->scan_timeout.data = (unsigned long) wl;
+	wl->scan_timeout.function = wl_scan_timeout;
+
+	return err;
+}
+
+static s32 wl_init_priv(struct wl_priv *wl)
+{
+	struct wiphy *wiphy = wl_to_wiphy(wl);
+	struct net_device *ndev = wl_to_prmry_ndev(wl);
+	s32 err = 0;
+
+	wl->scan_request = NULL;
+	wl->pwr_save = !!(wiphy->flags & WIPHY_FLAG_PS_ON_BY_DEFAULT);
+	wl->iscan_on = false;
+	wl->escan_on = true;
+	wl->roam_on = false;
+	wl->iscan_kickstart = false;
+	wl->active_scan = true;
+	wl->rf_blocked = false;
+	wl->deauth_reason = 0;
+	spin_lock_init(&wl->cfgdrv_lock);
+	mutex_init(&wl->ioctl_buf_sync);
+	init_waitqueue_head(&wl->netif_change_event);
+	wl_init_eq(wl);
+	err = wl_init_priv_mem(wl);
+	if (err)
+		return err;
+	if (wl_create_event_handler(wl))
+		return -ENOMEM;
+	wl_init_event_handler(wl);
+	mutex_init(&wl->usr_sync);
+	err = wl_init_scan(wl);
+	if (err)
+		return err;
+	wl_init_conf(wl->conf);
+	wl_init_prof(wl, ndev);
+	wl_link_down(wl);
+	DNGL_FUNC(dhd_cfg80211_init, (wl));
+
+	return err;
+}
+
+static void wl_deinit_priv(struct wl_priv *wl)
+{
+	DNGL_FUNC(dhd_cfg80211_deinit, (wl));
+	wl_destroy_event_handler(wl);
+	wl_flush_eq(wl);
+	wl_link_down(wl);
+	del_timer_sync(&wl->scan_timeout);
+	wl_term_iscan(wl);
+	wl_deinit_priv_mem(wl);
+	unregister_netdevice_notifier(&wl_cfg80211_netdev_notifier);
+}
+
+#if defined(WLP2P) && defined(WL_ENABLE_P2P_IF)
+static s32 wl_cfg80211_attach_p2p(void)
+{
+	struct wl_priv *wl = wlcfg_drv_priv;
+
+	WL_TRACE(("Enter \n"));
+
+	if (wl_cfgp2p_register_ndev(wl) < 0) {
+		WL_ERR(("%s: P2P attach failed. \n", __func__));
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static s32  wl_cfg80211_detach_p2p(void)
+{
+	struct wl_priv *wl = wlcfg_drv_priv;
+	struct wireless_dev *wdev = wl->p2p_wdev;
+
+	WL_DBG(("Enter \n"));
+	if (!wdev || !wl) {
+		WL_ERR(("Invalid Ptr\n"));
+		return -EINVAL;
+	}
+
+	wl_cfgp2p_unregister_ndev(wl);
+
+	wl->p2p_wdev = NULL;
+	wl->p2p_net = NULL;
+	WL_DBG(("Freeing 0x%08x \n", (unsigned int)wdev));
+	kfree(wdev);
+
+	return 0;
+}
+#endif /* defined(WLP2P) && defined(WL_ENABLE_P2P_IF) */
+
+s32 wl_cfg80211_attach_post(struct net_device *ndev)
+{
+	struct wl_priv * wl = NULL;
+	s32 err = 0;
+	WL_TRACE(("In\n"));
+	if (unlikely(!ndev)) {
+		WL_ERR(("ndev is invaild\n"));
+		return -ENODEV;
+	}
+	wl = wlcfg_drv_priv;
+	if (wl && !wl_get_drv_status(wl, READY, ndev)) {
+			if (wl->wdev &&
+				wl_cfgp2p_supported(wl, ndev)) {
+#if !defined(WL_ENABLE_P2P_IF)
+				wl->wdev->wiphy->interface_modes |=
+					(BIT(NL80211_IFTYPE_P2P_CLIENT)|
+					BIT(NL80211_IFTYPE_P2P_GO));
+#endif
+				if ((err = wl_cfgp2p_init_priv(wl)) != 0)
+					goto fail;
+
+#if defined(WLP2P) && defined(WL_ENABLE_P2P_IF)
+				if (wl->p2p_net) {
+					/* Update MAC addr for p2p0 interface here. */
+					memcpy(wl->p2p_net->dev_addr, ndev->dev_addr, ETH_ALEN);
+					wl->p2p_net->dev_addr[0] |= 0x02;
+					printk("%s: p2p_dev_addr="MACSTR "\n",
+						wl->p2p_net->name, MAC2STR(wl->p2p_net->dev_addr));
+				} else {
+					WL_ERR(("p2p_net not yet populated."
+					" Couldn't update the MAC Address for p2p0 \n"));
+					return -ENODEV;
+				}
+#endif /* defined(WLP2P) && (WL_ENABLE_P2P_IF) */
+
+				wl->p2p_supported = true;
+			}
+	} else
+		return -ENODEV;
+	wl_set_drv_status(wl, READY, ndev);
+fail:
+	return err;
+}
+
+s32 wl_cfg80211_attach(struct net_device *ndev, void *data)
+{
+	struct wireless_dev *wdev;
+	struct wl_priv *wl;
+	s32 err = 0;
+	struct device *dev;
+
+	WL_TRACE(("In\n"));
+	if (!ndev) {
+		WL_ERR(("ndev is invaild\n"));
+		return -ENODEV;
+	}
+	WL_DBG(("func %p\n", wl_cfg80211_get_parent_dev()));
+	dev = wl_cfg80211_get_parent_dev();
+
+	wdev = kzalloc(sizeof(*wdev), GFP_KERNEL);
+	if (unlikely(!wdev)) {
+		WL_ERR(("Could not allocate wireless device\n"));
+		return -ENOMEM;
+	}
+	err = wl_setup_wiphy(wdev, dev);
+	if (unlikely(err)) {
+		kfree(wdev);
+		return -ENOMEM;
+	}
+	wdev->iftype = wl_mode_to_nl80211_iftype(WL_MODE_BSS);
+	wl = (struct wl_priv *)wiphy_priv(wdev->wiphy);
+	wl->wdev = wdev;
+	wl->pub = data;
+	INIT_LIST_HEAD(&wl->net_list);
+	ndev->ieee80211_ptr = wdev;
+	SET_NETDEV_DEV(ndev, wiphy_dev(wdev->wiphy));
+	wdev->netdev = ndev;
+	err = wl_alloc_netinfo(wl, ndev, wdev, WL_MODE_BSS);
+	if (err) {
+		WL_ERR(("Failed to alloc net_info (%d)\n", err));
+		goto cfg80211_attach_out;
+	}
+	err = wl_init_priv(wl);
+	if (err) {
+		WL_ERR(("Failed to init iwm_priv (%d)\n", err));
+		goto cfg80211_attach_out;
+	}
+
+	err = wl_setup_rfkill(wl, TRUE);
+	if (err) {
+		WL_ERR(("Failed to setup rfkill %d\n", err));
+		goto cfg80211_attach_out;
+	}
+	err = register_netdevice_notifier(&wl_cfg80211_netdev_notifier);
+	if (err) {
+		WL_ERR(("Failed to register notifierl %d\n", err));
+		goto cfg80211_attach_out;
+	}
+#if defined(COEX_DHCP)
+	if (wl_cfg80211_btcoex_init(wl))
+		goto cfg80211_attach_out;
+#endif 
+
+	wlcfg_drv_priv = wl;
+
+#if defined(WLP2P) && defined(WL_ENABLE_P2P_IF)
+	err = wl_cfg80211_attach_p2p();
+	if (err)
+		goto cfg80211_attach_out;
+#endif
+
+	return err;
+
+cfg80211_attach_out:
+	err = wl_setup_rfkill(wl, FALSE);
+	wl_free_wdev(wl);
+	return err;
+}
+
+void wl_cfg80211_detach(void *para)
+{
+	struct wl_priv *wl;
+
+	wl = wlcfg_drv_priv;
+
+	WL_TRACE(("In\n"));
+
+#if defined(COEX_DHCP)
+	wl_cfg80211_btcoex_deinit(wl);
+#endif 
+
+#if defined(WLP2P) && defined(WL_ENABLE_P2P_IF)
+	wl_cfg80211_detach_p2p();
+#endif
+	wl_setup_rfkill(wl, FALSE);
+	if (wl->p2p_supported)
+		wl_cfgp2p_deinit_priv(wl);
+	wl_deinit_priv(wl);
+	wlcfg_drv_priv = NULL;
+	wl_cfg80211_clear_parent_dev();
+	wl_free_wdev(wl);
+	 /* PLEASE do NOT call any function after wl_free_wdev, the driver's private structure "wl",
+	  * which is the private part of wiphy, has been freed in wl_free_wdev !!!!!!!!!!!
+	  */
+}
+
+static void wl_wakeup_event(struct wl_priv *wl)
+{
+	if (wl->event_tsk.thr_pid >= 0) {
+		DHD_OS_WAKE_LOCK(wl->pub);
+		up(&wl->event_tsk.sema);
+	}
+}
+
+static int wl_is_p2p_event(struct wl_event_q *e)
+{
+	switch (e->etype) {
+	/* We have to seperate out the P2P events received
+	 * on primary interface so that it can be send up
+	 * via p2p0 interface.
+	*/
+	case WLC_E_P2P_PROBREQ_MSG:
+	case WLC_E_P2P_DISC_LISTEN_COMPLETE:
+	case WLC_E_ACTION_FRAME_RX:
+	case WLC_E_ACTION_FRAME_OFF_CHAN_COMPLETE:
+	case WLC_E_ACTION_FRAME_COMPLETE:
+
+		if (e->emsg.ifidx != 0) {
+			WL_TRACE(("P2P Event on Virtual I/F (ifidx:%d) \n",
+			e->emsg.ifidx));
+			/* We are only bothered about the P2P events received
+			 * on primary interface. For rest of them return false
+			 * so that it is sent over the interface corresponding
+			 * to the ifidx.
+			 */
+			return FALSE;
+		} else {
+			WL_TRACE(("P2P Event on Primary I/F (ifidx:%d)."
+				" Sent it to p2p0 \n", e->emsg.ifidx));
+			return TRUE;
+		}
+		break;
+
+	default:
+		WL_TRACE(("NON-P2P Event %d on ifidx (ifidx:%d) \n",
+			e->etype, e->emsg.ifidx));
+		return FALSE;
+	}
+}
+
+static s32 wl_event_handler(void *data)
+{
+	struct net_device *netdev;
+	struct wl_priv *wl = NULL;
+	struct wl_event_q *e;
+	tsk_ctl_t *tsk = (tsk_ctl_t *)data;
+
+	wl = (struct wl_priv *)tsk->parent;
+	DAEMONIZE("dhd_cfg80211_event");
+	complete(&tsk->completed);
+
+	while (down_interruptible (&tsk->sema) == 0) {
+		SMP_RD_BARRIER_DEPENDS();
+		if (tsk->terminated)
+			break;
+		while ((e = wl_deq_event(wl))) {
+			WL_DBG(("event type (%d), if idx: %d\n", e->etype, e->emsg.ifidx));
+			/* All P2P device address related events comes on primary interface since
+			 * there is no corresponding bsscfg for P2P interface. Map it to p2p0
+			 * interface.
+			 */
+			if ((wl_is_p2p_event(e) == TRUE) && (wl->p2p_net)) {
+				netdev = wl->p2p_net;
+			} else {
+				netdev = dhd_idx2net((struct dhd_pub *)(wl->pub), e->emsg.ifidx);
+			}
+			if (!netdev)
+				netdev = wl_to_prmry_ndev(wl);
+			if (e->etype < WLC_E_LAST && wl->evt_handler[e->etype]) {
+				wl->evt_handler[e->etype] (wl, netdev, &e->emsg, e->edata);
+			} else {
+				WL_DBG(("Unknown Event (%d): ignoring\n", e->etype));
+			}
+			wl_put_event(e);
+		}
+		DHD_OS_WAKE_UNLOCK(wl->pub);
+	}
+	WL_ERR(("%s was terminated\n", __func__));
+	complete_and_exit(&tsk->completed, 0);
+	return 0;
+}
+
+void
+wl_cfg80211_event(struct net_device *ndev, const wl_event_msg_t * e, void *data)
+{
+	u32 event_type = ntoh32(e->event_type);
+	struct wl_priv *wl = wlcfg_drv_priv;
+
+#if (WL_DBG_LEVEL > 0)
+	s8 *estr = (event_type <= sizeof(wl_dbg_estr) / WL_DBG_ESTR_MAX - 1) ?
+	    wl_dbg_estr[event_type] : (s8 *) "Unknown";
+	WL_DBG(("event_type (%d):" "WLC_E_" "%s\n", event_type, estr));
+#endif /* (WL_DBG_LEVEL > 0) */
+
+	if (likely(!wl_enq_event(wl, ndev, event_type, e, data)))
+		wl_wakeup_event(wl);
+}
+
+static void wl_init_eq(struct wl_priv *wl)
+{
+	wl_init_eq_lock(wl);
+	INIT_LIST_HEAD(&wl->eq_list);
+}
+
+static void wl_flush_eq(struct wl_priv *wl)
+{
+	struct wl_event_q *e;
+	unsigned long flags;
+
+	flags = wl_lock_eq(wl);
+	while (!list_empty(&wl->eq_list)) {
+		e = list_first_entry(&wl->eq_list, struct wl_event_q, eq_list);
+		list_del(&e->eq_list);
+		kfree(e);
+	}
+	wl_unlock_eq(wl, flags);
+}
+
+/*
+* retrieve first queued event from head
+*/
+
+static struct wl_event_q *wl_deq_event(struct wl_priv *wl)
+{
+	struct wl_event_q *e = NULL;
+	unsigned long flags;
+
+	flags = wl_lock_eq(wl);
+	if (likely(!list_empty(&wl->eq_list))) {
+		e = list_first_entry(&wl->eq_list, struct wl_event_q, eq_list);
+		list_del(&e->eq_list);
+	}
+	wl_unlock_eq(wl, flags);
+
+	return e;
+}
+
+/*
+ * push event to tail of the queue
+ */
+
+static s32
+wl_enq_event(struct wl_priv *wl, struct net_device *ndev, u32 event, const wl_event_msg_t *msg,
+	void *data)
+{
+	struct wl_event_q *e;
+	s32 err = 0;
+	uint32 evtq_size;
+	uint32 data_len;
+	unsigned long flags;
+	gfp_t aflags;
+
+	data_len = 0;
+	if (data)
+		data_len = ntoh32(msg->datalen);
+	evtq_size = sizeof(struct wl_event_q) + data_len;
+	aflags = (in_atomic()) ? GFP_ATOMIC : GFP_KERNEL;
+	e = kzalloc(evtq_size, aflags);
+	if (unlikely(!e)) {
+		WL_ERR(("event alloc failed\n"));
+		return -ENOMEM;
+	}
+	e->etype = event;
+	memcpy(&e->emsg, msg, sizeof(wl_event_msg_t));
+	if (data)
+		memcpy(e->edata, data, data_len);
+	flags = wl_lock_eq(wl);
+	list_add_tail(&e->eq_list, &wl->eq_list);
+	wl_unlock_eq(wl, flags);
+
+	return err;
+}
+
+static void wl_put_event(struct wl_event_q *e)
+{
+	kfree(e);
+}
+
+static s32 wl_config_ifmode(struct wl_priv *wl, struct net_device *ndev, s32 iftype)
+{
+	s32 infra = 0;
+	s32 err = 0;
+	s32 mode = 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:
+		mode = WL_MODE_IBSS;
+		break;
+	case NL80211_IFTYPE_STATION:
+	case NL80211_IFTYPE_P2P_CLIENT:
+		mode = WL_MODE_BSS;
+		infra = 1;
+		break;
+	case NL80211_IFTYPE_AP:
+	case NL80211_IFTYPE_P2P_GO:
+		mode = WL_MODE_AP;
+		infra = 1;
+		break;
+	default:
+		err = -EINVAL;
+		WL_ERR(("invalid type (%d)\n", iftype));
+		return err;
+	}
+	infra = htod32(infra);
+	err = wldev_ioctl(ndev, WLC_SET_INFRA, &infra, sizeof(infra), true);
+	if (unlikely(err)) {
+		WL_ERR(("WLC_SET_INFRA error (%d)\n", err));
+		return err;
+	}
+
+	wl_set_mode_by_netdev(wl, ndev, mode);
+
+	return 0;
+}
+
+static s32 wl_add_remove_eventmsg(struct net_device *ndev, u16 event, bool add)
+{
+	s8 iovbuf[WL_EVENTING_MASK_LEN + 12];
+
+	s8 eventmask[WL_EVENTING_MASK_LEN];
+	s32 err = 0;
+
+	/* Setup event_msgs */
+	bcm_mkiovar("event_msgs", NULL, 0, iovbuf,
+		sizeof(iovbuf));
+	err = wldev_ioctl(ndev, WLC_GET_VAR, iovbuf, sizeof(iovbuf), false);
+	if (unlikely(err)) {
+		WL_ERR(("Get event_msgs error (%d)\n", err));
+		goto eventmsg_out;
+	}
+	memcpy(eventmask, iovbuf, WL_EVENTING_MASK_LEN);
+	if (add) {
+		setbit(eventmask, event);
+	} else {
+		clrbit(eventmask, event);
+	}
+	bcm_mkiovar("event_msgs", eventmask, WL_EVENTING_MASK_LEN, iovbuf,
+		sizeof(iovbuf));
+	err = wldev_ioctl(ndev, WLC_SET_VAR, iovbuf, sizeof(iovbuf), true);
+	if (unlikely(err)) {
+		WL_ERR(("Set event_msgs error (%d)\n", err));
+		goto eventmsg_out;
+	}
+
+eventmsg_out:
+	return err;
+
+}
+
+static int wl_construct_reginfo(struct wl_priv *wl, s32 bw_cap)
+{
+	struct net_device *dev = wl_to_prmry_ndev(wl);
+	struct ieee80211_channel *band_chan_arr = NULL;
+	wl_uint32_list_t *list;
+	u32 i, j, index, n_2g, n_5g, band, channel, array_size;
+	u32 *n_cnt = NULL;
+	chanspec_t c = 0;
+	s32 err = BCME_OK;
+	bool update;
+	bool ht40_allowed;
+	u8 *pbuf = NULL;
+
+#define LOCAL_BUF_LEN 1024
+	pbuf = kzalloc(LOCAL_BUF_LEN, GFP_KERNEL);
+	if (pbuf == NULL) {
+		WL_ERR(("failed to allocate local buf\n"));
+		return -ENOMEM;
+	}
+
+	list = (wl_uint32_list_t *)(void *)pbuf;
+	list->count = htod32(WL_NUMCHANSPECS);
+
+	err = wldev_iovar_getbuf_bsscfg(dev, "chanspecs", NULL,
+		0, pbuf, LOCAL_BUF_LEN, 0, &wl->ioctl_buf_sync);
+	if (err != 0) {
+		WL_ERR(("get chanspecs failed with %d\n", err));
+		kfree(pbuf);
+		return err;
+	}
+#undef LOCAL_BUF_LEN
+
+	band = array_size = n_2g = n_5g = 0;
+	for (i = 0; i < dtoh32(list->count); i++) {
+		index = 0;
+		update = FALSE;
+		ht40_allowed = FALSE;
+		c = (chanspec_t)dtoh32(list->element[i]);
+		channel = CHSPEC_CHANNEL(c);
+		if (CHSPEC_IS40(c)) {
+			if (CHSPEC_SB_UPPER(c))
+				channel += CH_10MHZ_APART;
+			else
+				channel -= CH_10MHZ_APART;
+		}
+
+		if (CHSPEC_IS2G(c) && channel <= CH_MAX_2G_CHANNEL) {
+			band_chan_arr = __wl_2ghz_channels;
+			array_size = ARRAYSIZE(__wl_2ghz_channels);
+			n_cnt = &n_2g;
+			band = IEEE80211_BAND_2GHZ;
+			ht40_allowed = (bw_cap == WLC_N_BW_40ALL) ? TRUE : FALSE;
+		} else if (CHSPEC_IS5G(c) && channel > CH_MAX_2G_CHANNEL) {
+			band_chan_arr = __wl_5ghz_a_channels;
+			array_size = ARRAYSIZE(__wl_5ghz_a_channels);
+			n_cnt = &n_5g;
+			band = IEEE80211_BAND_5GHZ;
+			ht40_allowed = (bw_cap == WLC_N_BW_20ALL) ? FALSE : TRUE;
+		}
+
+		for (j = 0; (j < *n_cnt && (*n_cnt < array_size)); j++) {
+			if (band_chan_arr[j].hw_value == channel) {
+				update = TRUE;
+				break;
+			}
+		}
+
+		if (update)
+			index = j;
+		else
+			index = *n_cnt;
+
+		if (index <  array_size) {
+#if LINUX_VERSION_CODE == KERNEL_VERSION(2, 6, 38) && !defined(WL_COMPAT_WIRELESS)
+			band_chan_arr[index].center_freq =
+				ieee80211_channel_to_frequency(channel);
+#else
+			band_chan_arr[index].center_freq =
+				ieee80211_channel_to_frequency(channel, band);
+#endif
+			band_chan_arr[index].hw_value = channel;
+
+			if (CHSPEC_IS40(c) && ht40_allowed) {
+				u32 ht40_flag = band_chan_arr[index].flags & IEEE80211_CHAN_NO_HT40;
+				if (CHSPEC_SB_UPPER(c)) {
+					if (ht40_flag == IEEE80211_CHAN_NO_HT40)
+						band_chan_arr[index].flags &= ~IEEE80211_CHAN_NO_HT40;
+					band_chan_arr[index].flags |= IEEE80211_CHAN_NO_HT40PLUS;
+				} else {
+					band_chan_arr[index].flags &= ~IEEE80211_CHAN_NO_HT40;
+					if (ht40_flag == IEEE80211_CHAN_NO_HT40)
+						band_chan_arr[index].flags |= IEEE80211_CHAN_NO_HT40MINUS;
+				}
+			} else {
+				band_chan_arr[index].flags = IEEE80211_CHAN_NO_HT40;
+				if (band == IEEE80211_BAND_2GHZ)
+					channel |= WL_CHANSPEC_BAND_2G;
+				else
+					channel |= WL_CHANSPEC_BAND_5G;
+					err = wldev_iovar_getint(dev, "per_chan_info", &channel);
+				if (!err) {
+					if (channel & WL_CHAN_RADAR) {
+						band_chan_arr[index].flags |= IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS;
+					}
+					if (channel & WL_CHAN_PASSIVE) {
+						band_chan_arr[index].flags |= IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_IBSS;
+					}
+				}
+			}
+
+			if (!update)
+				(*n_cnt)++;
+		}
+	}
+
+	__wl_band_2ghz.n_channels = n_2g;
+	__wl_band_5ghz_a.n_channels = n_5g;
+
+	kfree(pbuf);
+	return err;
+}
+
+s32 wl_update_wiphybands(struct wl_priv *wl)
+{
+	struct wiphy *wiphy;
+	struct net_device *dev;
+	u32 bandlist[3];
+	u32 nband = 0;
+	u32 i = 0;
+	s32 err = 0;
+	int nmode = 0;
+	int bw_cap = 0;
+	int index = 0;
+
+	WL_DBG(("Entry"));
+
+	if (wl == NULL)
+		wl = wlcfg_drv_priv;
+	dev = wl_to_prmry_ndev(wl);
+
+	memset(bandlist, 0, sizeof(bandlist));
+	err = wldev_ioctl(dev, WLC_GET_BANDLIST, bandlist,
+		sizeof(bandlist), false);
+	if (unlikely(err)) {
+		WL_ERR(("error read bandlist (%d)\n", err));
+		return err;
+	}
+	wiphy = wl_to_wiphy(wl);
+	nband = bandlist[0];
+	wiphy->bands[IEEE80211_BAND_2GHZ] = &__wl_band_2ghz;
+	wiphy->bands[IEEE80211_BAND_5GHZ] = NULL;
+
+	err = wldev_iovar_getint(dev, "nmode", &nmode);
+	if (unlikely(err)) {
+		WL_ERR(("error reading nmode (%d)\n", err));
+	} else {
+		/* For nmodeonly  check bw cap */
+		err = wldev_iovar_getint(dev, "mimo_bw_cap", &bw_cap);
+		if (unlikely(err)) {
+			WL_ERR(("error get mimo_bw_cap (%d)\n", err));
+		}
+	}
+
+	err = wl_construct_reginfo(wl, bw_cap);
+	if (err) {
+		WL_ERR(("wl_construct_reginfo() fails err=%d\n", err));
+		return err;
+	}
+	for (i = 1; i <= nband && i < sizeof(bandlist)/sizeof(u32); i++) {
+		index = -1;
+		if (bandlist[i] == WLC_BAND_5G && __wl_band_5ghz_a.n_channels > 0) {
+			wiphy->bands[IEEE80211_BAND_5GHZ] =
+				&__wl_band_5ghz_a;
+			index = IEEE80211_BAND_5GHZ;
+			if (bw_cap == WLC_N_BW_40ALL || bw_cap == WLC_N_BW_20IN2G_40IN5G)
+				wiphy->bands[index]->ht_cap.cap |= IEEE80211_HT_CAP_SGI_40;
+		} else if (bandlist[i] == WLC_BAND_2G && __wl_band_2ghz.n_channels > 0) {
+			wiphy->bands[IEEE80211_BAND_2GHZ] =
+				&__wl_band_2ghz;
+			index = IEEE80211_BAND_2GHZ;
+			if (bandlist[i] == WLC_BAND_2G && bw_cap == WLC_N_BW_40ALL)
+				wiphy->bands[index]->ht_cap.cap |= IEEE80211_HT_CAP_SGI_40;
+		}
+		if ((index >= 0) && nmode) {
+			wiphy->bands[index]->ht_cap.cap |=
+				IEEE80211_HT_CAP_SGI_20 | IEEE80211_HT_CAP_DSSSCCK40;
+			wiphy->bands[index]->ht_cap.ht_supported = TRUE;
+			wiphy->bands[index]->ht_cap.ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
+			wiphy->bands[index]->ht_cap.ampdu_density = IEEE80211_HT_MPDU_DENSITY_16;
+		}
+	}
+
+	wiphy_apply_custom_regulatory(wiphy, &brcm_regdom);
+	return err;
+}
+
+static s32 __wl_cfg80211_up(struct wl_priv *wl)
+{
+	s32 err = 0;
+	struct net_device *ndev = wl_to_prmry_ndev(wl);
+	struct wireless_dev *wdev = ndev->ieee80211_ptr;
+
+	WL_DBG(("In\n"));
+
+	err = dhd_config_dongle(wl, false);
+	if (unlikely(err))
+		return err;
+
+	err = wl_config_ifmode(wl, ndev, wdev->iftype);
+	if (unlikely(err && err != -EINPROGRESS)) {
+		WL_ERR(("wl_config_ifmode failed\n"));
+	}
+	err = wl_update_wiphybands(wl);
+	if (unlikely(err)) {
+		WL_ERR(("wl_update_wiphybands failed\n"));
+	}
+
+	err = dhd_monitor_init(wl->pub);
+	err = wl_invoke_iscan(wl);
+	wl_set_drv_status(wl, READY, ndev);
+	return err;
+}
+
+static s32 __wl_cfg80211_down(struct wl_priv *wl)
+{
+	s32 err = 0;
+	unsigned long flags;
+	struct net_info *iter, *next;
+	struct net_device *ndev = wl_to_prmry_ndev(wl);
+#ifdef WL_ENABLE_P2P_IF
+	struct wiphy *wiphy = wl_to_prmry_ndev(wl)->ieee80211_ptr->wiphy;
+	struct net_device *p2p_net = wl->p2p_net;
+#endif
+
+	WL_DBG(("In\n"));
+	/* Check if cfg80211 interface is already down */
+	if (!wl_get_drv_status(wl, READY, ndev))
+		return err;	/* it is even not ready */
+	for_each_ndev(wl, iter, next)
+		wl_set_drv_status(wl, SCAN_ABORTING, iter->ndev);
+
+	wl_term_iscan(wl);
+	spin_lock_irqsave(&wl->cfgdrv_lock, flags);
+	if (wl->scan_request) {
+		cfg80211_scan_done(wl->scan_request, true);
+		wl->scan_request = NULL;
+	}
+	for_each_ndev(wl, iter, next) {
+		wl_clr_drv_status(wl, READY, iter->ndev);
+		wl_clr_drv_status(wl, SCANNING, iter->ndev);
+		wl_clr_drv_status(wl, SCAN_ABORTING, iter->ndev);
+		wl_clr_drv_status(wl, CONNECTING, iter->ndev);
+		wl_clr_drv_status(wl, CONNECTED, iter->ndev);
+		wl_clr_drv_status(wl, DISCONNECTING, iter->ndev);
+		wl_clr_drv_status(wl, AP_CREATED, iter->ndev);
+		wl_clr_drv_status(wl, AP_CREATING, iter->ndev);
+	}
+	wl_to_prmry_ndev(wl)->ieee80211_ptr->iftype =
+		NL80211_IFTYPE_STATION;
+#ifdef WL_ENABLE_P2P_IF
+	wiphy->interface_modes = (wiphy->interface_modes)
+					& (~(BIT(NL80211_IFTYPE_P2P_CLIENT)|
+					BIT(NL80211_IFTYPE_P2P_GO)));
+	if ((p2p_net) && (p2p_net->flags & IFF_UP)) {
+		/* p2p0 interface is still UP. Bring it down */
+		p2p_net->flags &= ~IFF_UP;
+	}
+#endif /* WL_ENABLE_P2P_IF */
+	spin_unlock_irqrestore(&wl->cfgdrv_lock, flags);
+
+	DNGL_FUNC(dhd_cfg80211_down, (wl));
+	wl_flush_eq(wl);
+	wl_link_down(wl);
+	if (wl->p2p_supported)
+		wl_cfgp2p_down(wl);
+	dhd_monitor_uninit();
+
+	return err;
+}
+
+s32 wl_cfg80211_up(void *para)
+{
+	struct wl_priv *wl;
+	s32 err = 0;
+
+	WL_DBG(("In\n"));
+	wl = wlcfg_drv_priv;
+	mutex_lock(&wl->usr_sync);
+	wl_cfg80211_attach_post(wl_to_prmry_ndev(wl));
+	err = __wl_cfg80211_up(wl);
+	if (err)
+		WL_ERR(("__wl_cfg80211_up failed\n"));
+	mutex_unlock(&wl->usr_sync);
+	return err;
+}
+
+/* Private Event to Supplicant with indication that chip hangs */
+int wl_cfg80211_hang(struct net_device *dev, u16 reason)
+{
+	struct wl_priv *wl;
+	wl = wlcfg_drv_priv;
+
+	WL_ERR(("In : chip crash eventing\n"));
+	cfg80211_disconnected(dev, reason, NULL, 0, GFP_KERNEL);
+	if (wl != NULL) {
+		wl_link_down(wl);
+	}
+	return 0;
+}
+
+s32 wl_cfg80211_down(void *para)
+{
+	struct wl_priv *wl;
+	s32 err = 0;
+
+	WL_DBG(("In\n"));
+	wl = wlcfg_drv_priv;
+	mutex_lock(&wl->usr_sync);
+	err = __wl_cfg80211_down(wl);
+	mutex_unlock(&wl->usr_sync);
+
+	return err;
+}
+
+static void *wl_read_prof(struct wl_priv *wl, struct net_device *ndev, s32 item)
+{
+	unsigned long flags;
+	void *rptr = NULL;
+	struct wl_profile *profile = wl_get_profile_by_netdev(wl, ndev);
+
+	if (!profile)
+		return NULL;
+	spin_lock_irqsave(&wl->cfgdrv_lock, flags);
+	switch (item) {
+	case WL_PROF_SEC:
+		rptr = &profile->sec;
+		break;
+	case WL_PROF_ACT:
+		rptr = &profile->active;
+		break;
+	case WL_PROF_BSSID:
+		rptr = profile->bssid;
+		break;
+	case WL_PROF_SSID:
+		rptr = &profile->ssid;
+		break;
+	}
+	spin_unlock_irqrestore(&wl->cfgdrv_lock, flags);
+	if (!rptr)
+		WL_ERR(("invalid item (%d)\n", item));
+	return rptr;
+}
+
+static s32
+wl_update_prof(struct wl_priv *wl, struct net_device *ndev,
+	const wl_event_msg_t *e, void *data, s32 item)
+{
+	s32 err = 0;
+	struct wlc_ssid *ssid;
+	unsigned long flags;
+	struct wl_profile *profile = wl_get_profile_by_netdev(wl, ndev);
+
+	if (!profile)
+		return WL_INVALID;
+	spin_lock_irqsave(&wl->cfgdrv_lock, flags);
+	switch (item) {
+	case WL_PROF_SSID:
+		ssid = (wlc_ssid_t *) data;
+		memset(profile->ssid.SSID, 0,
+			sizeof(profile->ssid.SSID));
+		memcpy(profile->ssid.SSID, ssid->SSID, ssid->SSID_len);
+		profile->ssid.SSID_len = ssid->SSID_len;
+		break;
+	case WL_PROF_BSSID:
+		if (data)
+			memcpy(profile->bssid, data, ETHER_ADDR_LEN);
+		else
+			memset(profile->bssid, 0, ETHER_ADDR_LEN);
+		break;
+	case WL_PROF_SEC:
+		memcpy(&profile->sec, data, sizeof(profile->sec));
+		break;
+	case WL_PROF_ACT:
+		profile->active = *(bool *)data;
+		break;
+	case WL_PROF_BEACONINT:
+		profile->beacon_interval = *(u16 *)data;
+		break;
+	case WL_PROF_DTIMPERIOD:
+		profile->dtim_period = *(u8 *)data;
+		break;
+	default:
+		WL_ERR(("unsupported item (%d)\n", item));
+		err = -EOPNOTSUPP;
+		break;
+	}
+	spin_unlock_irqrestore(&wl->cfgdrv_lock, flags);
+	return err;
+}
+
+void wl_cfg80211_dbg_level(u32 level)
+{
+	/*
+	* prohibit to change debug level
+	* by insmod parameter.
+	* eventually debug level will be configured
+	* in compile time by using CONFIG_XXX
+	*/
+	/* wl_dbg_level = level; */
+}
+
+static bool wl_is_ibssmode(struct wl_priv *wl, struct net_device *ndev)
+{
+	return wl_get_mode_by_netdev(wl, ndev) == WL_MODE_IBSS;
+}
+
+static __used bool wl_is_ibssstarter(struct wl_priv *wl)
+{
+	return wl->ibss_starter;
+}
+
+static void wl_rst_ie(struct wl_priv *wl)
+{
+	struct wl_ie *ie = wl_to_ie(wl);
+
+	ie->offset = 0;
+}
+
+static __used s32 wl_add_ie(struct wl_priv *wl, u8 t, u8 l, u8 *v)
+{
+	struct wl_ie *ie = wl_to_ie(wl);
+	s32 err = 0;
+
+	if (unlikely(ie->offset + l + 2 > WL_TLV_INFO_MAX)) {
+		WL_ERR(("ei crosses buffer boundary\n"));
+		return -ENOSPC;
+	}
+	ie->buf[ie->offset] = t;
+	ie->buf[ie->offset + 1] = l;
+	memcpy(&ie->buf[ie->offset + 2], v, l);
+	ie->offset += l + 2;
+
+	return err;
+}
+
+static s32 wl_mrg_ie(struct wl_priv *wl, u8 *ie_stream, u16 ie_size)
+{
+	struct wl_ie *ie = wl_to_ie(wl);
+	s32 err = 0;
+
+	if (unlikely(ie->offset + ie_size > WL_TLV_INFO_MAX)) {
+		WL_ERR(("ei_stream crosses buffer boundary\n"));
+		return -ENOSPC;
+	}
+	memcpy(&ie->buf[ie->offset], ie_stream, ie_size);
+	ie->offset += ie_size;
+
+	return err;
+}
+
+static s32 wl_cp_ie(struct wl_priv *wl, u8 *dst, u16 dst_size)
+{
+	struct wl_ie *ie = wl_to_ie(wl);
+	s32 err = 0;
+
+	if (unlikely(ie->offset > dst_size)) {
+		WL_ERR(("dst_size is not enough\n"));
+		return -ENOSPC;
+	}
+	memcpy(dst, &ie->buf[0], ie->offset);
+
+	return err;
+}
+
+static u32 wl_get_ielen(struct wl_priv *wl)
+{
+	struct wl_ie *ie = wl_to_ie(wl);
+
+	return ie->offset;
+}
+
+static void wl_link_up(struct wl_priv *wl)
+{
+	wl->link_up = true;
+}
+
+static void wl_link_down(struct wl_priv *wl)
+{
+	struct wl_connect_info *conn_info = wl_to_conn(wl);
+
+	WL_DBG(("In\n"));
+	wl->link_up = false;
+	conn_info->req_ie_len = 0;
+	conn_info->resp_ie_len = 0;
+}
+
+static unsigned long wl_lock_eq(struct wl_priv *wl)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&wl->eq_lock, flags);
+	return flags;
+}
+
+static void wl_unlock_eq(struct wl_priv *wl, unsigned long flags)
+{
+	spin_unlock_irqrestore(&wl->eq_lock, flags);
+}
+
+static void wl_init_eq_lock(struct wl_priv *wl)
+{
+	spin_lock_init(&wl->eq_lock);
+}
+
+static void wl_delay(u32 ms)
+{
+	if (in_atomic() || ms < 1000 / HZ) {
+		mdelay(ms);
+	} else {
+		msleep(ms);
+	}
+}
+
+s32 wl_cfg80211_get_p2p_dev_addr(struct net_device *net, struct ether_addr *p2pdev_addr)
+{
+	struct wl_priv *wl = wlcfg_drv_priv;
+	struct ether_addr p2pif_addr;
+	struct ether_addr primary_mac;
+
+	if (!wl->p2p)
+		return -1;
+	if (!p2p_is_on(wl)) {
+		get_primary_mac(wl, &primary_mac);
+		wl_cfgp2p_generate_bss_mac(&primary_mac, p2pdev_addr, &p2pif_addr);
+	} else {
+		memcpy(p2pdev_addr->octet,
+			wl->p2p->dev_addr.octet, ETHER_ADDR_LEN);
+	}
+
+	return 0;
+}
+s32 wl_cfg80211_set_p2p_noa(struct net_device *net, char* buf, int len)
+{
+	struct wl_priv *wl;
+
+	wl = wlcfg_drv_priv;
+
+	return wl_cfgp2p_set_p2p_noa(wl, net, buf, len);
+}
+
+s32 wl_cfg80211_get_p2p_noa(struct net_device *net, char* buf, int len)
+{
+	struct wl_priv *wl;
+	wl = wlcfg_drv_priv;
+
+	return wl_cfgp2p_get_p2p_noa(wl, net, buf, len);
+}
+
+s32 wl_cfg80211_set_p2p_ps(struct net_device *net, char* buf, int len)
+{
+	struct wl_priv *wl;
+	wl = wlcfg_drv_priv;
+
+	return wl_cfgp2p_set_p2p_ps(wl, net, buf, len);
+}
+
+s32 wl_cfg80211_set_wps_p2p_ie(struct net_device *net, char *buf, int len,
+	enum wl_management_type type)
+{
+	struct wl_priv *wl;
+	struct net_device *ndev = NULL;
+	struct ether_addr primary_mac;
+	s32 ret = 0;
+	s32 bssidx = 0;
+	s32 pktflag = 0;
+	wl = wlcfg_drv_priv;
+
+	if (wl_get_drv_status(wl, AP_CREATING, net) ||
+		wl_get_drv_status(wl, AP_CREATED, net)) {
+		ndev = net;
+		bssidx = 0;
+	} else if (wl->p2p) {
+	   if (net == wl->p2p_net) {
+			net = wl_to_prmry_ndev(wl);
+		}
+
+		if (!wl->p2p->on) {
+			get_primary_mac(wl, &primary_mac);
+			wl_cfgp2p_generate_bss_mac(&primary_mac, &wl->p2p->dev_addr,
+				&wl->p2p->int_addr);
+			/* In case of p2p_listen command, supplicant send remain_on_channel
+			* without turning on P2P
+			*/
+			p2p_on(wl) = true;
+			ret = wl_cfgp2p_enable_discovery(wl, ndev, NULL, 0);
+
+			if (unlikely(ret)) {
+				goto exit;
+			}
+		}
+		if (net  != wl_to_prmry_ndev(wl)) {
+			if (wl_get_mode_by_netdev(wl, net) == WL_MODE_AP) {
+				ndev = wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION);
+				bssidx = wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_CONNECTION);
+			}
+		} else {
+				ndev = wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_PRIMARY);
+				bssidx = wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE);
+		}
+	}
+	if (ndev != NULL) {
+		switch (type) {
+			case WL_BEACON:
+				pktflag = VNDR_IE_BEACON_FLAG;
+				break;
+			case WL_PROBE_RESP:
+				pktflag = VNDR_IE_PRBRSP_FLAG;
+				break;
+			case WL_ASSOC_RESP:
+				pktflag = VNDR_IE_ASSOCRSP_FLAG;
+				break;
+		}
+		if (pktflag)
+			ret = wl_cfgp2p_set_management_ie(wl, ndev, bssidx, pktflag, buf, len);
+	}
+exit:
+	return ret;
+}
+
+static const struct rfkill_ops wl_rfkill_ops = {
+	.set_block = wl_rfkill_set
+};
+
+static int wl_rfkill_set(void *data, bool blocked)
+{
+	struct wl_priv *wl = (struct wl_priv *)data;
+
+	WL_DBG(("Enter \n"));
+	WL_DBG(("RF %s\n", blocked ? "blocked" : "unblocked"));
+
+	if (!wl)
+		return -EINVAL;
+
+	wl->rf_blocked = blocked;
+
+	return 0;
+}
+
+static int wl_setup_rfkill(struct wl_priv *wl, bool setup)
+{
+	s32 err = 0;
+
+	WL_DBG(("Enter \n"));
+	if (!wl)
+		return -EINVAL;
+	if (setup) {
+		wl->rfkill = rfkill_alloc("brcmfmac-wifi",
+			wl_cfg80211_get_parent_dev(),
+			RFKILL_TYPE_WLAN, &wl_rfkill_ops, (void *)wl);
+
+		if (!wl->rfkill) {
+			err = -ENOMEM;
+			goto err_out;
+		}
+
+		err = rfkill_register(wl->rfkill);
+
+		if (err)
+			rfkill_destroy(wl->rfkill);
+	} else {
+		if (!wl->rfkill) {
+			err = -ENOMEM;
+			goto err_out;
+		}
+
+		rfkill_unregister(wl->rfkill);
+		rfkill_destroy(wl->rfkill);
+	}
+
+err_out:
+	return err;
+}
+
+struct device *wl_cfg80211_get_parent_dev(void)
+{
+	return cfg80211_parent_dev;
+}
+
+void wl_cfg80211_set_parent_dev(void *dev)
+{
+	cfg80211_parent_dev = dev;
+}
+
+static void wl_cfg80211_clear_parent_dev(void)
+{
+	cfg80211_parent_dev = NULL;
+}
+
+static void get_primary_mac(struct wl_priv *wl, struct ether_addr *mac)
+{
+	wldev_iovar_getbuf_bsscfg(wl_to_prmry_ndev(wl), "cur_etheraddr", NULL,
+		0, wl->ioctl_buf, WLC_IOCTL_MAXLEN, 0, &wl->ioctl_buf_sync);
+	memcpy(mac->octet, wl->ioctl_buf, ETHER_ADDR_LEN);
+}
+
+int wl_cfg80211_do_driver_init(struct net_device *net)
+{
+	struct wl_priv *wl = *(struct wl_priv **)netdev_priv(net);
+
+	if (!wl || !wl->wdev)
+		return -EINVAL;
+
+	if (dhd_do_driver_init(wl->wdev->netdev) < 0)
+		return -1;
+
+	return 0;
+}
+
+void wl_cfg80211_enable_trace(int level)
+{
+	wl_dbg_level |= WL_DBG_DBG;
+}
diff --git a/drivers/net/wireless/bcmdhd/wl_cfg80211.h b/drivers/net/wireless/bcmdhd/wl_cfg80211.h
new file mode 100644
index 000000000000..21446dc10708
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/wl_cfg80211.h
@@ -0,0 +1,672 @@
+/*
+ * Linux cfg80211 driver
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wl_cfg80211.h,v 1.1.4.1.2.8 2011/02/09 01:37:52 Exp $
+ */
+
+#ifndef _wl_cfg80211_h_
+#define _wl_cfg80211_h_
+
+#include <linux/wireless.h>
+#include <typedefs.h>
+#include <proto/ethernet.h>
+#include <wlioctl.h>
+#include <linux/wireless.h>
+#include <net/cfg80211.h>
+#include <linux/rfkill.h>
+
+#include <wl_cfgp2p.h>
+
+struct wl_conf;
+struct wl_iface;
+struct wl_priv;
+struct wl_security;
+struct wl_ibss;
+
+
+#define htod32(i) i
+#define htod16(i) i
+#define dtoh32(i) i
+#define dtoh16(i) i
+#define htodchanspec(i) i
+#define dtohchanspec(i) i
+
+#define WL_DBG_NONE	0
+#define WL_DBG_TRACE	(1 << 4)
+#define WL_DBG_SCAN 	(1 << 3)
+#define WL_DBG_DBG 	(1 << 2)
+#define WL_DBG_INFO	(1 << 1)
+#define WL_DBG_ERR	(1 << 0)
+
+/* 0 invalidates all debug messages.  default is 1 */
+#define WL_DBG_LEVEL 0xFF
+
+#define	WL_ERR(args)									\
+do {										\
+	if (wl_dbg_level & WL_DBG_ERR) {				\
+			printk(KERN_ERR "CFG80211-ERROR) %s : ", __func__);	\
+			printk args;						\
+		} 								\
+} while (0)
+#ifdef WL_INFO
+#undef WL_INFO
+#endif
+#define	WL_INFO(args)									\
+do {										\
+	if (wl_dbg_level & WL_DBG_INFO) {				\
+			printk(KERN_ERR "CFG80211-INFO) %s : ", __func__);	\
+			printk args;						\
+		}								\
+} while (0)
+#ifdef WL_SCAN
+#undef WL_SCAN
+#endif
+#define	WL_SCAN(args)								\
+do {									\
+	if (wl_dbg_level & WL_DBG_SCAN) {			\
+		printk(KERN_ERR "CFG80211-SCAN) %s :", __func__);	\
+		printk args;							\
+	}									\
+} while (0)
+#ifdef WL_TRACE
+#undef WL_TRACE
+#endif
+#define	WL_TRACE(args)								\
+do {									\
+	if (wl_dbg_level & WL_DBG_TRACE) {			\
+		printk(KERN_ERR "CFG80211-TRACE) %s :", __func__);	\
+		printk args;							\
+	}									\
+} while (0)
+#if (WL_DBG_LEVEL > 0)
+#define	WL_DBG(args)								\
+do {									\
+	if (wl_dbg_level & WL_DBG_DBG) {			\
+		printk(KERN_ERR "CFG80211-DEBUG) %s :", __func__);	\
+		printk args;							\
+	}									\
+} while (0)
+#else				/* !(WL_DBG_LEVEL > 0) */
+#define	WL_DBG(args)
+#endif				/* (WL_DBG_LEVEL > 0) */
+
+
+#define WL_SCAN_RETRY_MAX	3
+#define WL_NUM_PMKIDS_MAX	MAXPMKID
+#define WL_SCAN_BUF_MAX 	(1024 * 8)
+#define WL_TLV_INFO_MAX 	1024
+#define WL_SCAN_IE_LEN_MAX      2048
+#define WL_BSS_INFO_MAX		2048
+#define WL_ASSOC_INFO_MAX	512
+#define WL_IOCTL_LEN_MAX	1024
+#define WL_EXTRA_BUF_MAX	2048
+#define WL_ISCAN_BUF_MAX	2048
+#define WL_ISCAN_TIMER_INTERVAL_MS	3000
+#define WL_SCAN_ERSULTS_LAST 	(WL_SCAN_RESULTS_NO_MEM+1)
+#define WL_AP_MAX		256
+#define WL_FILE_NAME_MAX	256
+#define WL_DWELL_TIME 		200
+#define WL_MED_DWELL_TIME       400
+#define WL_LONG_DWELL_TIME 	1000
+#define IFACE_MAX_CNT 		2
+
+#define WL_SCAN_TIMER_INTERVAL_MS	8000 /* Scan timeout */
+#define WL_CHANNEL_SYNC_RETRY 	3
+#define WL_ACT_FRAME_RETRY 4
+
+#define WL_INVALID 		-1
+
+
+/* Bring down SCB Timeout to 20secs from 60secs default */
+#ifndef WL_SCB_TIMEOUT
+#define WL_SCB_TIMEOUT 20
+#endif
+
+/* driver status */
+enum wl_status {
+	WL_STATUS_READY = 0,
+	WL_STATUS_SCANNING,
+	WL_STATUS_SCAN_ABORTING,
+	WL_STATUS_CONNECTING,
+	WL_STATUS_CONNECTED,
+	WL_STATUS_DISCONNECTING,
+	WL_STATUS_AP_CREATING,
+	WL_STATUS_AP_CREATED,
+	WL_STATUS_SENDING_ACT_FRM
+};
+
+/* wi-fi mode */
+enum wl_mode {
+	WL_MODE_BSS,
+	WL_MODE_IBSS,
+	WL_MODE_AP
+};
+
+/* driver 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
+};
+
+/* driver iscan state */
+enum wl_iscan_state {
+	WL_ISCAN_STATE_IDLE,
+	WL_ISCAN_STATE_SCANING
+};
+
+/* donlge escan state */
+enum wl_escan_state {
+    WL_ESCAN_STATE_IDLE,
+    WL_ESCAN_STATE_SCANING
+};
+/* fw downloading status */
+enum wl_fw_status {
+	WL_FW_LOADING_DONE,
+	WL_NVRAM_LOADING_DONE
+};
+
+enum wl_management_type {
+	WL_BEACON = 0x1,
+	WL_PROBE_RESP = 0x2,
+	WL_ASSOC_RESP = 0x4
+};
+/* beacon / probe_response */
+struct beacon_proberesp {
+	__le64 timestamp;
+	__le16 beacon_int;
+	__le16 capab_info;
+	u8 variable[0];
+} __attribute__ ((packed));
+
+/* driver configuration */
+struct wl_conf {
+	u32 frag_threshold;
+	u32 rts_threshold;
+	u32 retry_short;
+	u32 retry_long;
+	s32 tx_power;
+	struct ieee80211_channel channel;
+};
+
+typedef s32(*EVENT_HANDLER) (struct wl_priv *wl,
+                            struct net_device *ndev, const wl_event_msg_t *e, void *data);
+
+/* bss inform structure for cfg80211 interface */
+struct wl_cfg80211_bss_info {
+	u16 band;
+	u16 channel;
+	s16 rssi;
+	u16 frame_len;
+	u8 frame_buf[1];
+};
+
+/* basic structure of scan request */
+struct wl_scan_req {
+	struct wlc_ssid ssid;
+};
+
+/* basic structure of information element */
+struct wl_ie {
+	u16 offset;
+	u8 buf[WL_TLV_INFO_MAX];
+};
+
+/* event queue for cfg80211 main event */
+struct wl_event_q {
+	struct list_head eq_list;
+	u32 etype;
+	wl_event_msg_t emsg;
+	s8 edata[1];
+};
+
+/* security information with currently associated ap */
+struct wl_security {
+	u32 wpa_versions;
+	u32 auth_type;
+	u32 cipher_pairwise;
+	u32 cipher_group;
+	u32 wpa_auth;
+};
+
+/* ibss information for currently joined ibss network */
+struct wl_ibss {
+	u8 beacon_interval;	/* in millisecond */
+	u8 atim;		/* in millisecond */
+	s8 join_only;
+	u8 band;
+	u8 channel;
+};
+
+/* wl driver profile */
+struct wl_profile {
+	u32 mode;
+	s32 band;
+	struct wlc_ssid ssid;
+	struct wl_security sec;
+	struct wl_ibss ibss;
+	u8 bssid[ETHER_ADDR_LEN];
+	u16 beacon_interval;
+	u8 dtim_period;
+	bool active;
+};
+
+struct net_info {
+	struct net_device *ndev;
+	struct wireless_dev *wdev;
+	struct wl_profile profile;
+	s32 mode;
+	unsigned long sme_state;
+	struct list_head list; /* list of all net_info structure */
+};
+typedef s32(*ISCAN_HANDLER) (struct wl_priv *wl);
+
+/* iscan controller */
+struct wl_iscan_ctrl {
+	struct net_device *dev;
+	struct timer_list timer;
+	u32 timer_ms;
+	u32 timer_on;
+	s32 state;
+	struct task_struct *tsk;
+	struct semaphore sync;
+	ISCAN_HANDLER iscan_handler[WL_SCAN_ERSULTS_LAST];
+	void *data;
+	s8 ioctl_buf[WLC_IOCTL_SMLEN];
+	s8 scan_buf[WL_ISCAN_BUF_MAX];
+};
+
+/* association inform */
+#define MAX_REQ_LINE 1024
+struct wl_connect_info {
+	u8 req_ie[MAX_REQ_LINE];
+	s32 req_ie_len;
+	u8 resp_ie[MAX_REQ_LINE];
+	s32 resp_ie_len;
+};
+
+/* firmware /nvram downloading controller */
+struct wl_fw_ctrl {
+	const struct firmware *fw_entry;
+	unsigned long status;
+	u32 ptr;
+	s8 fw_name[WL_FILE_NAME_MAX];
+	s8 nvram_name[WL_FILE_NAME_MAX];
+};
+
+/* assoc ie length */
+struct wl_assoc_ielen {
+	u32 req_len;
+	u32 resp_len;
+};
+
+/* wpa2 pmk list */
+struct wl_pmk_list {
+	pmkid_list_t pmkids;
+	pmkid_t foo[MAXPMKID - 1];
+};
+
+
+#define ESCAN_BUF_SIZE (64 * 1024)
+
+struct escan_info {
+	u32 escan_state;
+	u8 escan_buf[ESCAN_BUF_SIZE];
+	struct wiphy *wiphy;
+	struct net_device *ndev;
+};
+
+struct ap_info {
+/* Structure to hold WPS, WPA IEs for a AP */
+	u8   probe_res_ie[IE_MAX_LEN];
+	u8   beacon_ie[IE_MAX_LEN];
+	u32 probe_res_ie_len;
+	u32 beacon_ie_len;
+	u8 *wpa_ie;
+	u8 *rsn_ie;
+	u8 *wps_ie;
+	bool security_mode;
+};
+struct btcoex_info {
+	struct timer_list timer;
+	u32 timer_ms;
+	u32 timer_on;
+	u32 ts_dhcp_start;	/* ms ts ecord time stats */
+	u32 ts_dhcp_ok;		/* ms ts ecord time stats */
+	bool dhcp_done;	/* flag, indicates that host done with
+					 * dhcp before t1/t2 expiration
+					 */
+	s32 bt_state;
+	struct work_struct work;
+	struct net_device *dev;
+};
+
+struct sta_info {
+	/* Structure to hold WPS IE for a STA */
+	u8  probe_req_ie[IE_MAX_LEN];
+	u8  assoc_req_ie[IE_MAX_LEN];
+	u32 probe_req_ie_len;
+	u32 assoc_req_ie_len;
+};
+
+struct afx_hdl {
+	wl_af_params_t *pending_tx_act_frm;
+	struct ether_addr	pending_tx_dst_addr;
+	struct net_device *dev;
+	struct work_struct work;
+	u32 bssidx;
+	u32 retry;
+	s32 peer_chan;
+	bool ack_recv;
+};
+
+/* private data of cfg80211 interface */
+struct wl_priv {
+	struct wireless_dev *wdev;	/* representing wl cfg80211 device */
+
+	struct wireless_dev *p2p_wdev;	/* representing wl cfg80211 device for P2P */
+	struct net_device *p2p_net;    /* reference to p2p0 interface */
+
+	struct wl_conf *conf;
+	struct cfg80211_scan_request *scan_request;	/* scan request object */
+	EVENT_HANDLER evt_handler[WLC_E_LAST];
+	struct list_head eq_list;	/* used for event queue */
+	struct list_head net_list;     /* used for struct net_info */
+	spinlock_t eq_lock;	/* for event queue synchronization */
+	spinlock_t cfgdrv_lock;	/* to protect scan status (and others if needed) */
+	struct completion act_frm_scan;
+	struct mutex usr_sync;	/* maily for up/down synchronization */
+	struct wl_scan_results *bss_list;
+	struct wl_scan_results *scan_results;
+
+	/* scan request object for internal purpose */
+	struct wl_scan_req *scan_req_int;
+	/* information element object for internal purpose */
+	struct wl_ie ie;
+	struct wl_iscan_ctrl *iscan;	/* iscan controller */
+
+	/* association information container */
+	struct wl_connect_info conn_info;
+
+	struct wl_pmk_list *pmk_list;	/* wpa2 pmk list */
+	tsk_ctl_t event_tsk;  		/* task of main event handler thread */
+	void *pub;
+	u32 iface_cnt;
+	u32 channel;		/* current channel */
+	bool iscan_on;		/* iscan on/off switch */
+	bool iscan_kickstart;	/* indicate iscan already started */
+	bool escan_on;      /* escan on/off switch */
+	struct escan_info escan_info;   /* escan information */
+	bool active_scan;	/* current scan mode */
+	bool ibss_starter;	/* indicates this sta is ibss starter */
+	bool link_up;		/* link/connection up flag */
+
+	/* indicate whether chip to support power save mode */
+	bool pwr_save;
+	bool roam_on;		/* on/off switch for self-roaming */
+	bool scan_tried;	/* indicates if first scan attempted */
+	u8 *ioctl_buf;		/* ioctl buffer */
+	struct mutex ioctl_buf_sync;
+	u8 *escan_ioctl_buf;
+	u8 *extra_buf;	/* maily to grab assoc information */
+	struct dentry *debugfsdir;
+	struct rfkill *rfkill;
+	bool rf_blocked;
+	struct ieee80211_channel remain_on_chan;
+	enum nl80211_channel_type remain_on_chan_type;
+	u64 send_action_id;
+	u64 last_roc_id;
+	wait_queue_head_t netif_change_event;
+	struct afx_hdl *afx_hdl;
+	struct ap_info *ap_info;
+	struct sta_info *sta_info;
+	struct p2p_info *p2p;
+	bool p2p_supported;
+	struct btcoex_info *btcoex_info;
+	struct timer_list scan_timeout;   /* Timer for catch scan event timeout */
+#ifdef WL_SCHED_SCAN
+	struct cfg80211_sched_scan_request *sched_scan_req;	/* scheduled scan req */
+#endif /* WL_SCHED_SCAN */
+	bool sched_scan_running;	/* scheduled scan req status */
+	u16 hostapd_chan;            /* remember chan requested by framework for hostapd  */
+	u16 deauth_reason;           /* Place holder to save deauth/disassoc reasons */
+};
+
+static inline struct wl_bss_info *next_bss(struct wl_scan_results *list, struct wl_bss_info *bss)
+{
+	return bss = bss ?
+		(struct wl_bss_info *)((uintptr) bss + dtoh32(bss->length)) : list->bss_info;
+}
+
+static inline s32
+wl_alloc_netinfo(struct wl_priv *wl, struct net_device *ndev,
+	struct wireless_dev * wdev, s32 mode)
+{
+	struct net_info *_net_info;
+	s32 err = 0;
+	if (wl->iface_cnt == IFACE_MAX_CNT)
+		return -ENOMEM;
+	_net_info = kzalloc(sizeof(struct net_info), GFP_KERNEL);
+	if (!_net_info)
+		err = -ENOMEM;
+	else {
+		_net_info->mode = mode;
+		_net_info->ndev = ndev;
+		_net_info->wdev = wdev;
+		wl->iface_cnt++;
+		list_add(&_net_info->list, &wl->net_list);
+	}
+	return err;
+}
+
+static inline void
+wl_dealloc_netinfo(struct wl_priv *wl, struct net_device *ndev)
+{
+	struct net_info *_net_info, *next;
+
+	list_for_each_entry_safe(_net_info, next, &wl->net_list, list) {
+		if (ndev && (_net_info->ndev == ndev)) {
+			list_del(&_net_info->list);
+			wl->iface_cnt--;
+			if (_net_info->wdev) {
+				kfree(_net_info->wdev);
+				ndev->ieee80211_ptr = NULL;
+			}
+			kfree(_net_info);
+		}
+	}
+}
+
+static inline void
+wl_delete_all_netinfo(struct wl_priv *wl)
+{
+	struct net_info *_net_info, *next;
+
+	list_for_each_entry_safe(_net_info, next, &wl->net_list, list) {
+		list_del(&_net_info->list);
+			if (_net_info->wdev)
+				kfree(_net_info->wdev);
+			kfree(_net_info);
+	}
+	wl->iface_cnt = 0;
+}
+
+static inline bool
+wl_get_status_all(struct wl_priv *wl, s32 status)
+
+{
+	struct net_info *_net_info, *next;
+	u32 cnt = 0;
+	list_for_each_entry_safe(_net_info, next, &wl->net_list, list) {
+		if (_net_info->ndev &&
+			test_bit(status, &_net_info->sme_state))
+			cnt++;
+	}
+	return cnt? true: false;
+}
+
+static inline void
+wl_set_status_by_netdev(struct wl_priv *wl, s32 status,
+	struct net_device *ndev, u32 op)
+{
+
+	struct net_info *_net_info, *next;
+
+	list_for_each_entry_safe(_net_info, next, &wl->net_list, list) {
+		if (ndev && (_net_info->ndev == ndev)) {
+			switch (op) {
+				case 1:
+					set_bit(status, &_net_info->sme_state);
+					break;
+				case 2:
+					clear_bit(status, &_net_info->sme_state);
+					break;
+				case 4:
+					change_bit(status, &_net_info->sme_state);
+					break;
+			}
+		}
+
+	}
+}
+
+static inline u32
+wl_get_status_by_netdev(struct wl_priv *wl, s32 status,
+	struct net_device *ndev)
+{
+	struct net_info *_net_info, *next;
+
+	list_for_each_entry_safe(_net_info, next, &wl->net_list, list) {
+				if (ndev && (_net_info->ndev == ndev))
+					return test_bit(status, &_net_info->sme_state);
+	}
+	return 0;
+}
+
+static inline s32
+wl_get_mode_by_netdev(struct wl_priv *wl, struct net_device *ndev)
+{
+	struct net_info *_net_info, *next;
+
+	list_for_each_entry_safe(_net_info, next, &wl->net_list, list) {
+				if (ndev && (_net_info->ndev == ndev))
+					return _net_info->mode;
+	}
+	return -1;
+}
+
+static inline void
+wl_set_mode_by_netdev(struct wl_priv *wl, struct net_device *ndev,
+	s32 mode)
+{
+	struct net_info *_net_info, *next;
+
+	list_for_each_entry_safe(_net_info, next, &wl->net_list, list) {
+				if (ndev && (_net_info->ndev == ndev))
+					_net_info->mode = mode;
+	}
+}
+
+static inline struct wl_profile *
+wl_get_profile_by_netdev(struct wl_priv *wl, struct net_device *ndev)
+{
+	struct net_info *_net_info, *next;
+
+	list_for_each_entry_safe(_net_info, next, &wl->net_list, list) {
+				if (ndev && (_net_info->ndev == ndev))
+					return &_net_info->profile;
+	}
+	return NULL;
+}
+#define wl_to_wiphy(w) (w->wdev->wiphy)
+#define wl_to_prmry_ndev(w) (w->wdev->netdev)
+#define ndev_to_wl(n) (wdev_to_wl(n->ieee80211_ptr))
+#define wl_to_sr(w) (w->scan_req_int)
+#define wl_to_ie(w) (&w->ie)
+#define iscan_to_wl(i) ((struct wl_priv *)(i->data))
+#define wl_to_iscan(w) (w->iscan)
+#define wl_to_conn(w) (&w->conn_info)
+#define wl_get_drv_status_all(wl, stat) \
+	(wl_get_status_all(wl, WL_STATUS_ ## stat))
+#define wl_get_drv_status(wl, stat, ndev)  \
+	(wl_get_status_by_netdev(wl, WL_STATUS_ ## stat, ndev))
+#define wl_set_drv_status(wl, stat, ndev)  \
+	(wl_set_status_by_netdev(wl, WL_STATUS_ ## stat, ndev, 1))
+#define wl_clr_drv_status(wl, stat, ndev)  \
+	(wl_set_status_by_netdev(wl, WL_STATUS_ ## stat, ndev, 2))
+#define wl_chg_drv_status(wl, stat, ndev)  \
+	(wl_set_status_by_netdev(wl, WL_STATUS_ ## stat, ndev, 4))
+
+#define for_each_bss(list, bss, __i)	\
+	for (__i = 0; __i < list->count && __i < WL_AP_MAX; __i++, bss = next_bss(list, bss))
+
+#define for_each_ndev(wl, iter, next) \
+	list_for_each_entry_safe(iter, next, &wl->net_list, list)
+
+
+/* In case of WPS from wpa_supplicant, pairwise siute and group suite is 0.
+ * In addtion to that, wpa_version is WPA_VERSION_1
+ */
+#define is_wps_conn(_sme) \
+	((wl_cfgp2p_find_wpsie((u8 *)_sme->ie, _sme->ie_len) != NULL) && \
+	 (!_sme->crypto.n_ciphers_pairwise) && \
+	 (!_sme->crypto.cipher_group))
+extern s32 wl_cfg80211_attach(struct net_device *ndev, void *data);
+extern s32 wl_cfg80211_attach_post(struct net_device *ndev);
+extern void wl_cfg80211_detach(void *para);
+
+extern void wl_cfg80211_event(struct net_device *ndev, const wl_event_msg_t *e,
+            void *data);
+void wl_cfg80211_set_parent_dev(void *dev);
+struct device *wl_cfg80211_get_parent_dev(void);
+
+extern s32 wl_cfg80211_up(void *para);
+extern s32 wl_cfg80211_down(void *para);
+extern s32 wl_cfg80211_notify_ifadd(struct net_device *ndev, s32 idx, s32 bssidx,
+	void* _net_attach);
+extern s32 wl_cfg80211_ifdel_ops(struct net_device *net);
+extern s32 wl_cfg80211_notify_ifdel(void);
+extern s32 wl_cfg80211_is_progress_ifadd(void);
+extern s32 wl_cfg80211_is_progress_ifchange(void);
+extern s32 wl_cfg80211_is_progress_ifadd(void);
+extern s32 wl_cfg80211_notify_ifchange(void);
+extern void wl_cfg80211_dbg_level(u32 level);
+extern s32 wl_cfg80211_get_p2p_dev_addr(struct net_device *net, struct ether_addr *p2pdev_addr);
+extern s32 wl_cfg80211_set_p2p_noa(struct net_device *net, char* buf, int len);
+extern s32 wl_cfg80211_get_p2p_noa(struct net_device *net, char* buf, int len);
+extern s32 wl_cfg80211_set_wps_p2p_ie(struct net_device *net, char *buf, int len,
+	enum wl_management_type type);
+extern s32 wl_cfg80211_set_p2p_ps(struct net_device *net, char* buf, int len);
+extern int wl_cfg80211_hang(struct net_device *dev, u16 reason);
+extern s32 wl_mode_to_nl80211_iftype(s32 mode);
+int wl_cfg80211_do_driver_init(struct net_device *net);
+void wl_cfg80211_enable_trace(int level);
+extern s32 wl_update_wiphybands(struct wl_priv *wl);
+extern s32 wl_cfg80211_if_is_group_owner(void);
+#endif				/* _wl_cfg80211_h_ */
diff --git a/drivers/net/wireless/bcmdhd/wl_cfgp2p.c b/drivers/net/wireless/bcmdhd/wl_cfgp2p.c
new file mode 100644
index 000000000000..318e122004e7
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/wl_cfgp2p.c
@@ -0,0 +1,2031 @@
+/*
+ * Linux cfgp2p driver
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wl_cfgp2p.c,v 1.1.4.1.2.14 2011-02-09 01:40:07 $
+ *
+ */
+#include <typedefs.h>
+#include <linuxver.h>
+#include <osl.h>
+#include <linux/kernel.h>
+#include <linux/kthread.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/types.h>
+#include <linux/string.h>
+#include <linux/timer.h>
+#include <linux/if_arp.h>
+#include <asm/uaccess.h>
+
+#include <bcmutils.h>
+#include <bcmendian.h>
+#include <proto/ethernet.h>
+
+#include <wl_cfg80211.h>
+#include <wl_cfgp2p.h>
+#include <wldev_common.h>
+#include <wl_android.h>
+
+static s8 scanparambuf[WLC_IOCTL_SMLEN];
+
+static bool
+wl_cfgp2p_has_ie(u8 *ie, u8 **tlvs, u32 *tlvs_len, const u8 *oui, u32 oui_len, u8 type);
+
+static s32
+wl_cfgp2p_vndr_ie(struct wl_priv *wl, struct net_device *ndev, s32 bssidx, s32 pktflag,
+            s8 *oui, s32 ie_id, s8 *data, s32 data_len, s32 delete);
+
+static int wl_cfgp2p_start_xmit(struct sk_buff *skb, struct net_device *ndev);
+static int wl_cfgp2p_do_ioctl(struct net_device *net, struct ifreq *ifr, int cmd);
+static int wl_cfgp2p_if_open(struct net_device *net);
+static int wl_cfgp2p_if_stop(struct net_device *net);
+static s32 wl_cfgp2p_cancel_listen(struct wl_priv *wl, struct net_device *ndev,
+	bool notify);
+
+static const struct net_device_ops wl_cfgp2p_if_ops = {
+	.ndo_open		= wl_cfgp2p_if_open,
+	.ndo_stop		= wl_cfgp2p_if_stop,
+	.ndo_do_ioctl		= wl_cfgp2p_do_ioctl,
+	.ndo_start_xmit		= wl_cfgp2p_start_xmit,
+};
+
+bool wl_cfgp2p_is_pub_action(void *frame, u32 frame_len)
+{
+	wifi_p2p_pub_act_frame_t *pact_frm;
+
+	if (frame == NULL)
+		return false;
+	pact_frm = (wifi_p2p_pub_act_frame_t *)frame;
+	if (frame_len < sizeof(wifi_p2p_pub_act_frame_t) -1)
+		return false;
+
+	if (pact_frm->category == P2P_PUB_AF_CATEGORY &&
+		pact_frm->action == P2P_PUB_AF_ACTION &&
+		pact_frm->oui_type == P2P_VER &&
+		memcmp(pact_frm->oui, P2P_OUI, sizeof(pact_frm->oui)) == 0) {
+		return true;
+	}
+
+	return false;
+}
+
+bool wl_cfgp2p_is_p2p_action(void *frame, u32 frame_len)
+{
+	wifi_p2p_action_frame_t *act_frm;
+
+	if (frame == NULL)
+		return false;
+	act_frm = (wifi_p2p_action_frame_t *)frame;
+	if (frame_len < sizeof(wifi_p2p_action_frame_t) -1)
+		return false;
+
+	if (act_frm->category == P2P_AF_CATEGORY &&
+		act_frm->type  == P2P_VER &&
+		memcmp(act_frm->OUI, P2P_OUI, DOT11_OUI_LEN) == 0) {
+		return true;
+	}
+
+	return false;
+}
+
+bool wl_cfgp2p_is_gas_action(void *frame, u32 frame_len)
+{
+
+	wifi_p2psd_gas_pub_act_frame_t *sd_act_frm;
+
+	if (frame == NULL)
+		return false;
+
+	sd_act_frm = (wifi_p2psd_gas_pub_act_frame_t *)frame;
+	if (frame_len < sizeof(wifi_p2psd_gas_pub_act_frame_t) - 1)
+		return false;
+	if (sd_act_frm->category != P2PSD_ACTION_CATEGORY)
+		return false;
+
+	if (sd_act_frm->action == P2PSD_ACTION_ID_GAS_IREQ ||
+		sd_act_frm->action == P2PSD_ACTION_ID_GAS_IRESP ||
+		sd_act_frm->action == P2PSD_ACTION_ID_GAS_CREQ ||
+		sd_act_frm->action == P2PSD_ACTION_ID_GAS_CRESP)
+		return true;
+	else
+		return false;
+
+}
+
+void wl_cfgp2p_print_actframe(bool tx, void *frame, u32 frame_len)
+{
+	wifi_p2p_pub_act_frame_t *pact_frm;
+	wifi_p2p_action_frame_t *act_frm;
+	wifi_p2psd_gas_pub_act_frame_t *sd_act_frm;
+	if (!frame || frame_len <= 2)
+		return;
+
+	if (wl_cfgp2p_is_pub_action(frame, frame_len)) {
+		pact_frm = (wifi_p2p_pub_act_frame_t *)frame;
+		switch (pact_frm->subtype) {
+			case P2P_PAF_GON_REQ:
+				CFGP2P_DBG(("%s P2P Group Owner Negotiation Req Frame\n",
+					(tx)? "TX": "RX"));
+				break;
+			case P2P_PAF_GON_RSP:
+				CFGP2P_DBG(("%s P2P Group Owner Negotiation Rsp Frame\n",
+					(tx)? "TX": "RX"));
+				break;
+			case P2P_PAF_GON_CONF:
+				CFGP2P_DBG(("%s P2P Group Owner Negotiation Confirm Frame\n",
+					(tx)? "TX": "RX"));
+				break;
+			case P2P_PAF_INVITE_REQ:
+				CFGP2P_DBG(("%s P2P Invitation Request  Frame\n",
+					(tx)? "TX": "RX"));
+				break;
+			case P2P_PAF_INVITE_RSP:
+				CFGP2P_DBG(("%s P2P Invitation Response Frame\n",
+					(tx)? "TX": "RX"));
+				break;
+			case P2P_PAF_DEVDIS_REQ:
+				CFGP2P_DBG(("%s P2P Device Discoverability Request Frame\n",
+					(tx)? "TX": "RX"));
+				break;
+			case P2P_PAF_DEVDIS_RSP:
+				CFGP2P_DBG(("%s P2P Device Discoverability Response Frame\n",
+					(tx)? "TX": "RX"));
+				break;
+			case P2P_PAF_PROVDIS_REQ:
+				CFGP2P_DBG(("%s P2P Provision Discovery Request Frame\n",
+					(tx)? "TX": "RX"));
+				break;
+			case P2P_PAF_PROVDIS_RSP:
+				CFGP2P_DBG(("%s P2P Provision Discovery Response Frame\n",
+					(tx)? "TX": "RX"));
+				break;
+			default:
+				CFGP2P_DBG(("%s Unknown P2P Public Action Frame\n",
+					(tx)? "TX": "RX"));
+
+		}
+
+	} else if (wl_cfgp2p_is_p2p_action(frame, frame_len)) {
+		act_frm = (wifi_p2p_action_frame_t *)frame;
+		switch (act_frm->subtype) {
+			case P2P_AF_NOTICE_OF_ABSENCE:
+				CFGP2P_DBG(("%s P2P Notice of Absence Frame\n",
+					(tx)? "TX": "RX"));
+				break;
+			case P2P_AF_PRESENCE_REQ:
+				CFGP2P_DBG(("%s P2P Presence Request Frame\n",
+					(tx)? "TX": "RX"));
+				break;
+			case P2P_AF_PRESENCE_RSP:
+				CFGP2P_DBG(("%s P2P Presence Response Frame\n",
+					(tx)? "TX": "RX"));
+				break;
+			case P2P_AF_GO_DISC_REQ:
+				CFGP2P_DBG(("%s P2P Discoverability Request Frame\n",
+					(tx)? "TX": "RX"));
+				break;
+			default:
+				CFGP2P_DBG(("%s Unknown P2P Action Frame\n",
+					(tx)? "TX": "RX"));
+		}
+
+	} else if (wl_cfgp2p_is_gas_action(frame, frame_len)) {
+		sd_act_frm = (wifi_p2psd_gas_pub_act_frame_t *)frame;
+		switch (sd_act_frm->action) {
+			case P2PSD_ACTION_ID_GAS_IREQ:
+				CFGP2P_DBG(("%s P2P GAS Initial Request\n",
+					(tx)? "TX" : "RX"));
+				break;
+			case P2PSD_ACTION_ID_GAS_IRESP:
+				CFGP2P_DBG(("%s P2P GAS Initial Response\n",
+					(tx)? "TX" : "RX"));
+				break;
+			case P2PSD_ACTION_ID_GAS_CREQ:
+				CFGP2P_DBG(("%s P2P GAS Comback Request\n",
+					(tx)? "TX" : "RX"));
+				break;
+			case P2PSD_ACTION_ID_GAS_CRESP:
+				CFGP2P_DBG(("%s P2P GAS Comback Response\n",
+					(tx)? "TX" : "RX"));
+				break;
+			default:
+				CFGP2P_DBG(("%s Unknown P2P GAS Frame\n",
+					(tx)? "TX" : "RX"));
+		}
+	}
+}
+
+/*
+ *  Initialize variables related to P2P
+ *
+ */
+s32
+wl_cfgp2p_init_priv(struct wl_priv *wl)
+{
+	if (!(wl->p2p = kzalloc(sizeof(struct p2p_info), GFP_KERNEL))) {
+		CFGP2P_ERR(("struct p2p_info allocation failed\n"));
+		return -ENOMEM;
+	}
+#define INIT_IE(IE_TYPE, BSS_TYPE)		\
+	do {							\
+		memset(wl_to_p2p_bss_saved_ie(wl, BSS_TYPE).p2p_ ## IE_TYPE ## _ie, 0, \
+		   sizeof(wl_to_p2p_bss_saved_ie(wl, BSS_TYPE).p2p_ ## IE_TYPE ## _ie)); \
+		wl_to_p2p_bss_saved_ie(wl, BSS_TYPE).p2p_ ## IE_TYPE ## _ie_len = 0; \
+	} while (0);
+
+	INIT_IE(probe_req, P2PAPI_BSSCFG_PRIMARY);
+	INIT_IE(probe_res, P2PAPI_BSSCFG_PRIMARY);
+	INIT_IE(assoc_req, P2PAPI_BSSCFG_PRIMARY);
+	INIT_IE(assoc_res, P2PAPI_BSSCFG_PRIMARY);
+	INIT_IE(beacon,    P2PAPI_BSSCFG_PRIMARY);
+	INIT_IE(probe_req, P2PAPI_BSSCFG_DEVICE);
+	INIT_IE(probe_res, P2PAPI_BSSCFG_DEVICE);
+	INIT_IE(assoc_req, P2PAPI_BSSCFG_DEVICE);
+	INIT_IE(assoc_res, P2PAPI_BSSCFG_DEVICE);
+	INIT_IE(beacon,    P2PAPI_BSSCFG_DEVICE);
+	INIT_IE(probe_req, P2PAPI_BSSCFG_CONNECTION);
+	INIT_IE(probe_res, P2PAPI_BSSCFG_CONNECTION);
+	INIT_IE(assoc_req, P2PAPI_BSSCFG_CONNECTION);
+	INIT_IE(assoc_res, P2PAPI_BSSCFG_CONNECTION);
+	INIT_IE(beacon,    P2PAPI_BSSCFG_CONNECTION);
+#undef INIT_IE
+	wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_PRIMARY) = wl_to_prmry_ndev(wl);
+	wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_PRIMARY) = 0;
+	wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_DEVICE) = NULL;
+	wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE) = 0;
+	wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION) = NULL;
+	wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_CONNECTION) = 0;
+	return BCME_OK;
+
+}
+/*
+ *  Deinitialize variables related to P2P
+ *
+ */
+void
+wl_cfgp2p_deinit_priv(struct wl_priv *wl)
+{
+	CFGP2P_DBG(("In\n"));
+
+	if (wl->p2p) {
+		kfree(wl->p2p);
+		wl->p2p = NULL;
+	}
+	wl->p2p_supported = 0;
+}
+/*
+ * Set P2P functions into firmware
+ */
+s32
+wl_cfgp2p_set_firm_p2p(struct wl_priv *wl)
+{
+	struct net_device *ndev = wl_to_prmry_ndev(wl);
+	struct ether_addr null_eth_addr = { { 0, 0, 0, 0, 0, 0 } };
+	s32 ret = BCME_OK;
+	s32 val = 0;
+	/* Do we have to check whether APSTA is enabled or not ? */
+	wldev_iovar_getint(ndev, "apsta", &val);
+	if (val == 0) {
+		val = 1;
+		wldev_ioctl(ndev, WLC_DOWN, &val, sizeof(s32), true);
+		wldev_iovar_setint(ndev, "apsta", val);
+		wldev_ioctl(ndev, WLC_UP, &val, sizeof(s32), true);
+	}
+	val = 1;
+	/* Disable firmware roaming for P2P  */
+	wldev_iovar_setint(ndev, "roam_off", val);
+	/* In case of COB type, firmware has default mac address
+	 * After Initializing firmware, we have to set current mac address to
+	 * firmware for P2P device address
+	 */
+	ret = wldev_iovar_setbuf_bsscfg(ndev, "p2p_da_override", &null_eth_addr,
+		sizeof(null_eth_addr), wl->ioctl_buf, WLC_IOCTL_MAXLEN, 0, &wl->ioctl_buf_sync);
+	if (ret && ret != BCME_UNSUPPORTED) {
+		CFGP2P_ERR(("failed to update device address ret %d\n", ret));
+	}
+	return ret;
+}
+
+/* Create a new P2P BSS.
+ * Parameters:
+ * @mac      : MAC address of the BSS to create
+ * @if_type  : interface type: WL_P2P_IF_GO or WL_P2P_IF_CLIENT
+ * @chspec   : chspec to use if creating a GO BSS.
+ * Returns 0 if success.
+ */
+s32
+wl_cfgp2p_ifadd(struct wl_priv *wl, struct ether_addr *mac, u8 if_type,
+            chanspec_t chspec)
+{
+	wl_p2p_if_t ifreq;
+	s32 err;
+	struct net_device *ndev = wl_to_prmry_ndev(wl);
+	u32 scb_timeout = WL_SCB_TIMEOUT;
+
+	ifreq.type = if_type;
+	ifreq.chspec = chspec;
+	memcpy(ifreq.addr.octet, mac->octet, sizeof(ifreq.addr.octet));
+
+	CFGP2P_DBG(("---wl p2p_ifadd %02x:%02x:%02x:%02x:%02x:%02x %s %u\n",
+	    ifreq.addr.octet[0], ifreq.addr.octet[1], ifreq.addr.octet[2],
+		ifreq.addr.octet[3], ifreq.addr.octet[4], ifreq.addr.octet[5],
+		(if_type == WL_P2P_IF_GO) ? "go" : "client",
+	        (chspec & WL_CHANSPEC_CHAN_MASK) >> WL_CHANSPEC_CHAN_SHIFT));
+
+	err = wldev_iovar_setbuf(ndev, "p2p_ifadd", &ifreq, sizeof(ifreq),
+		wl->ioctl_buf, WLC_IOCTL_MAXLEN, &wl->ioctl_buf_sync);
+
+	if (unlikely(err < 0)) {
+		printk("'wl p2p_ifadd' error %d\n", err);
+	} else if (if_type == WL_P2P_IF_GO) {
+		err = wldev_ioctl(ndev, WLC_SET_SCB_TIMEOUT, &scb_timeout, sizeof(u32), true);
+		if (unlikely(err < 0))
+			printk("'wl scb_timeout' error %d\n", err);
+	}
+
+	return err;
+}
+
+/* Delete a P2P BSS.
+ * Parameters:
+ * @mac      : MAC address of the BSS to create
+ * Returns 0 if success.
+ */
+s32
+wl_cfgp2p_ifdel(struct wl_priv *wl, struct ether_addr *mac)
+{
+	s32 ret;
+	struct net_device *netdev = wl_to_prmry_ndev(wl);
+
+	CFGP2P_INFO(("------primary idx %d : wl p2p_ifdel %02x:%02x:%02x:%02x:%02x:%02x\n",
+	    netdev->ifindex, mac->octet[0], mac->octet[1], mac->octet[2],
+	    mac->octet[3], mac->octet[4], mac->octet[5]));
+	ret = wldev_iovar_setbuf(netdev, "p2p_ifdel", mac, sizeof(*mac),
+		wl->ioctl_buf, WLC_IOCTL_MAXLEN, &wl->ioctl_buf_sync);
+	if (unlikely(ret < 0)) {
+		printk("'wl p2p_ifdel' error %d\n", ret);
+	}
+	return ret;
+}
+
+/* Change a P2P Role.
+ * Parameters:
+ * @mac      : MAC address of the BSS to change a role
+ * Returns 0 if success.
+ */
+s32
+wl_cfgp2p_ifchange(struct wl_priv *wl, struct ether_addr *mac, u8 if_type,
+            chanspec_t chspec)
+{
+	wl_p2p_if_t ifreq;
+	s32 err;
+	u32 scb_timeout = WL_SCB_TIMEOUT;
+	struct net_device *netdev =  wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION);
+
+	ifreq.type = if_type;
+	ifreq.chspec = chspec;
+	memcpy(ifreq.addr.octet, mac->octet, sizeof(ifreq.addr.octet));
+
+	CFGP2P_INFO(("---wl p2p_ifchange %02x:%02x:%02x:%02x:%02x:%02x %s %u\n",
+	    ifreq.addr.octet[0], ifreq.addr.octet[1], ifreq.addr.octet[2],
+	    ifreq.addr.octet[3], ifreq.addr.octet[4], ifreq.addr.octet[5],
+	    (if_type == WL_P2P_IF_GO) ? "go" : "client",
+		(chspec & WL_CHANSPEC_CHAN_MASK) >> WL_CHANSPEC_CHAN_SHIFT));
+
+	err = wldev_iovar_setbuf(netdev, "p2p_ifupd", &ifreq, sizeof(ifreq),
+		wl->ioctl_buf, WLC_IOCTL_MAXLEN, &wl->ioctl_buf_sync);
+
+	if (unlikely(err < 0)) {
+		printk("'wl p2p_ifupd' error %d\n", err);
+	} else if (if_type == WL_P2P_IF_GO) {
+		err = wldev_ioctl(netdev, WLC_SET_SCB_TIMEOUT, &scb_timeout, sizeof(u32), true);
+		if (unlikely(err < 0))
+			printk("'wl scb_timeout' error %d\n", err);
+	}
+	return err;
+}
+
+
+/* Get the index of a created P2P BSS.
+ * Parameters:
+ * @mac      : MAC address of the created BSS
+ * @index    : output: index of created BSS
+ * Returns 0 if success.
+ */
+s32
+wl_cfgp2p_ifidx(struct wl_priv *wl, struct ether_addr *mac, s32 *index)
+{
+	s32 ret;
+	u8 getbuf[64];
+	struct net_device *dev = wl_to_prmry_ndev(wl);
+
+	CFGP2P_INFO(("---wl p2p_if %02x:%02x:%02x:%02x:%02x:%02x\n",
+	    mac->octet[0], mac->octet[1], mac->octet[2],
+	    mac->octet[3], mac->octet[4], mac->octet[5]));
+
+	ret = wldev_iovar_getbuf_bsscfg(dev, "p2p_if", mac, sizeof(*mac), getbuf,
+		sizeof(getbuf), wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_PRIMARY), NULL);
+
+	if (ret == 0) {
+		memcpy(index, getbuf, sizeof(index));
+		CFGP2P_INFO(("---wl p2p_if   ==> %d\n", *index));
+	}
+
+	return ret;
+}
+
+static s32
+wl_cfgp2p_set_discovery(struct wl_priv *wl, s32 on)
+{
+	s32 ret = BCME_OK;
+	struct net_device *ndev = wl_to_prmry_ndev(wl);
+	CFGP2P_DBG(("enter\n"));
+
+	ret = wldev_iovar_setint(ndev, "p2p_disc", on);
+
+	if (unlikely(ret < 0)) {
+		CFGP2P_ERR(("p2p_disc %d error %d\n", on, ret));
+	}
+
+	return ret;
+}
+
+/* Set the WL driver's P2P mode.
+ * Parameters :
+ * @mode      : is one of WL_P2P_DISC_ST_{SCAN,LISTEN,SEARCH}.
+ * @channel   : the channel to listen
+ * @listen_ms : the time (milli seconds) to wait
+ * @bssidx    : bss index for BSSCFG
+ * Returns 0 if success
+ */
+
+s32
+wl_cfgp2p_set_p2p_mode(struct wl_priv *wl, u8 mode, u32 channel, u16 listen_ms, int bssidx)
+{
+	wl_p2p_disc_st_t discovery_mode;
+	s32 ret;
+	struct net_device *dev;
+	CFGP2P_DBG(("enter\n"));
+
+	if (unlikely(bssidx >= P2PAPI_BSSCFG_MAX)) {
+		CFGP2P_ERR((" %d index out of range\n", bssidx));
+		return -1;
+	}
+
+	dev = wl_to_p2p_bss_ndev(wl, bssidx);
+	if (unlikely(dev == NULL)) {
+		CFGP2P_ERR(("bssidx %d is not assigned\n", bssidx));
+		return BCME_NOTFOUND;
+	}
+
+	/* Put the WL driver into P2P Listen Mode to respond to P2P probe reqs */
+	discovery_mode.state = mode;
+	discovery_mode.chspec = CH20MHZ_CHSPEC(channel);
+	discovery_mode.dwell = listen_ms;
+	ret = wldev_iovar_setbuf_bsscfg(dev, "p2p_state", &discovery_mode,
+		sizeof(discovery_mode), wl->ioctl_buf, WLC_IOCTL_MAXLEN,
+		bssidx, &wl->ioctl_buf_sync);
+
+	return ret;
+}
+
+/* Get the index of the P2P Discovery BSS */
+static s32
+wl_cfgp2p_get_disc_idx(struct wl_priv *wl, s32 *index)
+{
+	s32 ret;
+	struct net_device *dev = wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_PRIMARY);
+
+	ret = wldev_iovar_getint(dev, "p2p_dev", index);
+	CFGP2P_INFO(("p2p_dev bsscfg_idx=%d ret=%d\n", *index, ret));
+
+	if (unlikely(ret <  0)) {
+	    CFGP2P_ERR(("'p2p_dev' error %d\n", ret));
+		return ret;
+	}
+	return ret;
+}
+
+s32
+wl_cfgp2p_init_discovery(struct wl_priv *wl)
+{
+
+	s32 index = 0;
+	s32 ret = BCME_OK;
+
+	CFGP2P_DBG(("enter\n"));
+
+	if (wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE) != 0) {
+		CFGP2P_ERR(("do nothing, already initialized\n"));
+		return ret;
+	}
+
+	ret = wl_cfgp2p_set_discovery(wl, 1);
+	if (ret < 0) {
+		CFGP2P_ERR(("set discover error\n"));
+		return ret;
+	}
+	/* Enable P2P Discovery in the WL Driver */
+	ret = wl_cfgp2p_get_disc_idx(wl, &index);
+
+	if (ret < 0) {
+		return ret;
+	}
+	wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_DEVICE) =
+	    wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_PRIMARY);
+	wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE) = index;
+
+	/* Set the initial discovery state to SCAN */
+	ret = wl_cfgp2p_set_p2p_mode(wl, WL_P2P_DISC_ST_SCAN, 0, 0,
+		wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE));
+
+	if (unlikely(ret != 0)) {
+		CFGP2P_ERR(("unable to set WL_P2P_DISC_ST_SCAN\n"));
+		wl_cfgp2p_set_discovery(wl, 0);
+		wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE) = 0;
+		wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_DEVICE) = NULL;
+		return 0;
+	}
+	return ret;
+}
+
+/* Deinitialize P2P Discovery
+ * Parameters :
+ * @wl        : wl_private data
+ * Returns 0 if succes
+ */
+static s32
+wl_cfgp2p_deinit_discovery(struct wl_priv *wl)
+{
+	s32 ret = BCME_OK;
+	CFGP2P_DBG(("enter\n"));
+
+	if (wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE) == 0) {
+		CFGP2P_ERR(("do nothing, not initialized\n"));
+		return -1;
+	}
+	/* Set the discovery state to SCAN */
+	ret = wl_cfgp2p_set_p2p_mode(wl, WL_P2P_DISC_ST_SCAN, 0, 0,
+	            wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE));
+	/* Disable P2P discovery in the WL driver (deletes the discovery BSSCFG) */
+	ret = wl_cfgp2p_set_discovery(wl, 0);
+
+	/* Clear our saved WPS and P2P IEs for the discovery BSS.  The driver
+	 * deleted these IEs when wl_cfgp2p_set_discovery() deleted the discovery
+	 * BSS.
+	 */
+
+	/* Clear the saved bsscfg index of the discovery BSSCFG to indicate we
+	 * have no discovery BSS.
+	 */
+	wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE) = 0;
+	wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_DEVICE) = NULL;
+
+	return ret;
+
+}
+/* Enable P2P Discovery
+ * Parameters:
+ * @wl	: wl_private data
+ * @ie  : probe request ie (WPS IE + P2P IE)
+ * @ie_len   : probe request ie length
+ * Returns 0 if success.
+ */
+s32
+wl_cfgp2p_enable_discovery(struct wl_priv *wl, struct net_device *dev,
+	const u8 *ie, u32 ie_len)
+{
+	s32 ret = BCME_OK;
+	if (wl_get_p2p_status(wl, DISCOVERY_ON)) {
+		CFGP2P_INFO((" DISCOVERY is already initialized, we have nothing to do\n"));
+		goto set_ie;
+	}
+
+	wl_set_p2p_status(wl, DISCOVERY_ON);
+
+	CFGP2P_DBG(("enter\n"));
+
+	ret = wl_cfgp2p_init_discovery(wl);
+	if (unlikely(ret < 0)) {
+		CFGP2P_ERR((" init discovery error %d\n", ret));
+		goto exit;
+	}
+	/* Set wsec to any non-zero value in the discovery bsscfg to ensure our
+	 * P2P probe responses have the privacy bit set in the 802.11 WPA IE.
+	 * Some peer devices may not initiate WPS with us if this bit is not set.
+	 */
+	ret = wldev_iovar_setint_bsscfg(wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_DEVICE),
+			"wsec", AES_ENABLED, wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE));
+	if (unlikely(ret < 0)) {
+		CFGP2P_ERR((" wsec error %d\n", ret));
+	}
+set_ie:
+	ret = wl_cfgp2p_set_management_ie(wl, dev,
+	            wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE),
+	            VNDR_IE_PRBREQ_FLAG, ie, ie_len);
+
+	if (unlikely(ret < 0)) {
+		CFGP2P_ERR(("set probreq ie occurs error %d\n", ret));
+		goto exit;
+	}
+exit:
+	return ret;
+}
+
+/* Disable P2P Discovery
+ * Parameters:
+ * @wl       : wl_private_data
+ * Returns 0 if success.
+ */
+s32
+wl_cfgp2p_disable_discovery(struct wl_priv *wl)
+{
+	s32 ret = BCME_OK;
+	CFGP2P_DBG((" enter\n"));
+	wl_clr_p2p_status(wl, DISCOVERY_ON);
+
+	if (wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE) == 0) {
+		CFGP2P_ERR((" do nothing, not initialized\n"));
+		goto exit;
+	}
+
+	ret = wl_cfgp2p_set_p2p_mode(wl, WL_P2P_DISC_ST_SCAN, 0, 0,
+	            wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE));
+
+	if (unlikely(ret < 0)) {
+
+		CFGP2P_ERR(("unable to set WL_P2P_DISC_ST_SCAN\n"));
+	}
+	/* Do a scan abort to stop the driver's scan engine in case it is still
+	 * waiting out an action frame tx dwell time.
+	 */
+#ifdef NOT_YET
+	if (wl_get_p2p_status(wl, SCANNING)) {
+		p2pwlu_scan_abort(hdl, FALSE);
+	}
+#endif
+	wl_clr_p2p_status(wl, DISCOVERY_ON);
+	ret = wl_cfgp2p_deinit_discovery(wl);
+
+exit:
+	return ret;
+}
+
+s32
+wl_cfgp2p_escan(struct wl_priv *wl, struct net_device *dev, u16 active,
+	u32 num_chans, u16 *channels,
+	s32 search_state, u16 action, u32 bssidx)
+{
+	s32 ret = BCME_OK;
+	s32 memsize;
+	s32 eparams_size;
+	u32 i;
+	s8 *memblk;
+	wl_p2p_scan_t *p2p_params;
+	wl_escan_params_t *eparams;
+	wlc_ssid_t ssid;
+	/* Scan parameters */
+#define P2PAPI_SCAN_NPROBES 1
+#define P2PAPI_SCAN_DWELL_TIME_MS 50
+#define P2PAPI_SCAN_SOCIAL_DWELL_TIME_MS 40
+#define P2PAPI_SCAN_HOME_TIME_MS 60
+	struct net_device *pri_dev = wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_PRIMARY);
+	wl_set_p2p_status(wl, SCANNING);
+	/* Allocate scan params which need space for 3 channels and 0 ssids */
+	eparams_size = (WL_SCAN_PARAMS_FIXED_SIZE +
+	    OFFSETOF(wl_escan_params_t, params)) +
+		num_chans * sizeof(eparams->params.channel_list[0]);
+
+	memsize = sizeof(wl_p2p_scan_t) + eparams_size;
+	memblk = scanparambuf;
+	if (memsize > sizeof(scanparambuf)) {
+		CFGP2P_ERR((" scanpar buf too small (%u > %u)\n",
+		    memsize, sizeof(scanparambuf)));
+		return -1;
+	}
+	memset(memblk, 0, memsize);
+	memset(wl->ioctl_buf, 0, WLC_IOCTL_MAXLEN);
+	if (search_state == WL_P2P_DISC_ST_SEARCH) {
+		/*
+		 * If we in SEARCH STATE, we don't need to set SSID explictly
+		 * because dongle use P2P WILDCARD internally by default
+		 */
+		wl_cfgp2p_set_p2p_mode(wl, WL_P2P_DISC_ST_SEARCH, 0, 0, bssidx);
+		ssid.SSID_len = htod32(0);
+
+	} else if (search_state == WL_P2P_DISC_ST_SCAN) {
+		/* SCAN STATE 802.11 SCAN
+		 * WFD Supplicant has p2p_find command with (type=progressive, type= full)
+		 * So if P2P_find command with type=progressive,
+		 * we have to set ssid to P2P WILDCARD because
+		 * we just do broadcast scan unless setting SSID
+		 */
+		strcpy(ssid.SSID, WL_P2P_WILDCARD_SSID);
+		ssid.SSID_len = htod32(WL_P2P_WILDCARD_SSID_LEN);
+		wl_cfgp2p_set_p2p_mode(wl, WL_P2P_DISC_ST_SCAN, 0, 0, bssidx);
+	}
+
+
+	/* Fill in the P2P scan structure at the start of the iovar param block */
+	p2p_params = (wl_p2p_scan_t*) memblk;
+	p2p_params->type = 'E';
+	/* Fill in the Scan structure that follows the P2P scan structure */
+	eparams = (wl_escan_params_t*) (p2p_params + 1);
+	eparams->params.bss_type = DOT11_BSSTYPE_ANY;
+	if (active)
+		eparams->params.scan_type = DOT11_SCANTYPE_ACTIVE;
+	else
+		eparams->params.scan_type = DOT11_SCANTYPE_PASSIVE;
+
+	memcpy(&eparams->params.bssid, &ether_bcast, ETHER_ADDR_LEN);
+	if (ssid.SSID_len)
+		memcpy(&eparams->params.ssid, &ssid, sizeof(wlc_ssid_t));
+
+	eparams->params.nprobes = htod32(P2PAPI_SCAN_NPROBES);
+	eparams->params.home_time = htod32(P2PAPI_SCAN_HOME_TIME_MS);
+	if (wl_get_drv_status_all(wl, CONNECTED))
+		eparams->params.active_time = htod32(-1);
+	else if (num_chans == 3)
+		eparams->params.active_time = htod32(P2PAPI_SCAN_SOCIAL_DWELL_TIME_MS);
+	else
+		eparams->params.active_time = htod32(P2PAPI_SCAN_DWELL_TIME_MS);
+	eparams->params.passive_time = htod32(-1);
+	eparams->params.channel_num = htod32((0 << WL_SCAN_PARAMS_NSSID_SHIFT) |
+	    (num_chans & WL_SCAN_PARAMS_COUNT_MASK));
+
+	for (i = 0; i < num_chans; i++) {
+		eparams->params.channel_list[i] = htodchanspec(channels[i]);
+	}
+	eparams->version = htod32(ESCAN_REQ_VERSION);
+	eparams->action =  htod16(action);
+	eparams->sync_id = htod16(0x1234);
+	CFGP2P_INFO(("SCAN CHANNELS : "));
+
+	for (i = 0; i < num_chans; i++) {
+		if (i == 0) CFGP2P_INFO(("%d", channels[i]));
+		else CFGP2P_INFO((",%d", channels[i]));
+	}
+
+	CFGP2P_INFO(("\n"));
+
+	ret = wldev_iovar_setbuf_bsscfg(pri_dev, "p2p_scan",
+		memblk, memsize, wl->ioctl_buf, WLC_IOCTL_MAXLEN, bssidx, &wl->ioctl_buf_sync);
+	return ret;
+}
+
+/* search function to reach at common channel to send action frame
+ * Parameters:
+ * @wl       : wl_private data
+ * @ndev     : net device for bssidx
+ * @bssidx   : bssidx for BSS
+ * Returns 0 if success.
+ */
+s32
+wl_cfgp2p_act_frm_search(struct wl_priv *wl, struct net_device *ndev,
+	s32 bssidx, s32 channel)
+{
+	s32 ret = 0;
+	u32 chan_cnt = 0;
+	u16 *default_chan_list = NULL;
+	if (!p2p_is_on(wl))
+		return -BCME_ERROR;
+	CFGP2P_ERR((" Enter\n"));
+	if (bssidx == P2PAPI_BSSCFG_PRIMARY)
+		bssidx =  wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE);
+	if (channel)
+		chan_cnt = 1;
+	else
+		chan_cnt = SOCIAL_CHAN_CNT;
+	default_chan_list = kzalloc(chan_cnt * sizeof(*default_chan_list), GFP_KERNEL);
+	if (default_chan_list == NULL) {
+		CFGP2P_ERR(("channel list allocation failed \n"));
+		ret = -ENOMEM;
+		goto exit;
+	}
+	if (channel) {
+		default_chan_list[0] = channel;
+	} else {
+		default_chan_list[0] = SOCIAL_CHAN_1;
+		default_chan_list[1] = SOCIAL_CHAN_2;
+		default_chan_list[2] = SOCIAL_CHAN_3;
+	}
+	ret = wl_cfgp2p_escan(wl, ndev, true, SOCIAL_CHAN_CNT,
+		default_chan_list, WL_P2P_DISC_ST_SEARCH,
+		WL_SCAN_ACTION_START, bssidx);
+	kfree(default_chan_list);
+exit:
+	return ret;
+}
+
+/* Check whether pointed-to IE looks like WPA. */
+#define wl_cfgp2p_is_wpa_ie(ie, tlvs, len)	wl_cfgp2p_has_ie(ie, tlvs, len, \
+		(const uint8 *)WPS_OUI, WPS_OUI_LEN, WPA_OUI_TYPE)
+/* Check whether pointed-to IE looks like WPS. */
+#define wl_cfgp2p_is_wps_ie(ie, tlvs, len)	wl_cfgp2p_has_ie(ie, tlvs, len, \
+		(const uint8 *)WPS_OUI, WPS_OUI_LEN, WPS_OUI_TYPE)
+/* Check whether the given IE looks like WFA P2P IE. */
+#define wl_cfgp2p_is_p2p_ie(ie, tlvs, len)	wl_cfgp2p_has_ie(ie, tlvs, len, \
+		(const uint8 *)WFA_OUI, WFA_OUI_LEN, WFA_OUI_TYPE_P2P)
+#ifdef WLWFDIE
+ /* Check whether the given IE looks like WFA WFDisplay IE. */
+#define WFA_OUI_TYPE_WFD		 0x0a	 /* WiFi Display OUI TYPE */
+#define wl_cfgp2p_is_wfd_ie(ie, tlvs, len) 	 wl_cfgp2p_has_ie(ie, tlvs, len, \
+		 (const uint8 *)WFA_OUI, WFA_OUI_LEN, WFA_OUI_TYPE_WFD)
+#endif /* WLWFDIE */
+/* Delete and Set a management vndr ie to firmware
+ * Parameters:
+ * @wl       : wl_private data
+ * @ndev     : net device for bssidx
+ * @bssidx   : bssidx for BSS
+ * @pktflag  : packet flag for IE (VNDR_IE_PRBREQ_FLAG,VNDR_IE_PRBRSP_FLAG, VNDR_IE_ASSOCRSP_FLAG,
+ *                                 VNDR_IE_ASSOCREQ_FLAG)
+ * @ie       :  VNDR IE (such as P2P IE , WPS IE)
+ * @ie_len   : VNDR IE Length
+ * Returns 0 if success.
+ */
+
+s32
+wl_cfgp2p_set_management_ie(struct wl_priv *wl, struct net_device *ndev, s32 bssidx,
+    s32 pktflag, const u8 *vndr_ie, u32 vndr_ie_len)
+{
+	/* Vendor-specific Information Element ID */
+#define VNDR_SPEC_ELEMENT_ID 0xdd
+	s32 ret = BCME_OK;
+	u32 pos;
+	u8  *ie_buf;
+	u8  *mgmt_ie_buf = NULL;
+	u32 mgmt_ie_buf_len = 0;
+	u32 *mgmt_ie_len = 0;
+	u8 ie_id, ie_len;
+	u8 delete = 0;
+#define IE_TYPE(type, bsstype) (wl_to_p2p_bss_saved_ie(wl, bsstype).p2p_ ## type ## _ie)
+#define IE_TYPE_LEN(type, bsstype) (wl_to_p2p_bss_saved_ie(wl, bsstype).p2p_ ## type ## _ie_len)
+	if (p2p_is_on(wl) && bssidx != -1) {
+		if (bssidx == P2PAPI_BSSCFG_PRIMARY)
+			bssidx =  wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE);
+		switch (pktflag) {
+			case VNDR_IE_PRBREQ_FLAG :
+				mgmt_ie_buf = IE_TYPE(probe_req, bssidx);
+				mgmt_ie_len = &IE_TYPE_LEN(probe_req, bssidx);
+				mgmt_ie_buf_len = sizeof(IE_TYPE(probe_req, bssidx));
+				break;
+			case VNDR_IE_PRBRSP_FLAG :
+				mgmt_ie_buf = IE_TYPE(probe_res, bssidx);
+				mgmt_ie_len = &IE_TYPE_LEN(probe_res, bssidx);
+				mgmt_ie_buf_len = sizeof(IE_TYPE(probe_res, bssidx));
+				break;
+			case VNDR_IE_ASSOCREQ_FLAG :
+				mgmt_ie_buf = IE_TYPE(assoc_req, bssidx);
+				mgmt_ie_len = &IE_TYPE_LEN(assoc_req, bssidx);
+				mgmt_ie_buf_len = sizeof(IE_TYPE(assoc_req, bssidx));
+				break;
+			case VNDR_IE_ASSOCRSP_FLAG :
+				mgmt_ie_buf = IE_TYPE(assoc_res, bssidx);
+				mgmt_ie_len = &IE_TYPE_LEN(assoc_res, bssidx);
+				mgmt_ie_buf_len = sizeof(IE_TYPE(assoc_res, bssidx));
+				break;
+			case VNDR_IE_BEACON_FLAG :
+				mgmt_ie_buf = IE_TYPE(beacon, bssidx);
+				mgmt_ie_len = &IE_TYPE_LEN(beacon, bssidx);
+				mgmt_ie_buf_len = sizeof(IE_TYPE(beacon, bssidx));
+				break;
+			default:
+				mgmt_ie_buf = NULL;
+				mgmt_ie_len = NULL;
+				CFGP2P_ERR(("not suitable type\n"));
+				return -1;
+		}
+	} else if (wl_get_mode_by_netdev(wl, ndev) == WL_MODE_AP) {
+		switch (pktflag) {
+			case VNDR_IE_PRBRSP_FLAG :
+				mgmt_ie_buf = wl->ap_info->probe_res_ie;
+				mgmt_ie_len = &wl->ap_info->probe_res_ie_len;
+				mgmt_ie_buf_len = sizeof(wl->ap_info->probe_res_ie);
+				break;
+			case VNDR_IE_BEACON_FLAG :
+				mgmt_ie_buf = wl->ap_info->beacon_ie;
+				mgmt_ie_len = &wl->ap_info->beacon_ie_len;
+				mgmt_ie_buf_len = sizeof(wl->ap_info->beacon_ie);
+				break;
+			default:
+				mgmt_ie_buf = NULL;
+				mgmt_ie_len = NULL;
+				CFGP2P_ERR(("not suitable type\n"));
+				return -1;
+		}
+		bssidx = 0;
+	} else if (bssidx == -1 && wl_get_mode_by_netdev(wl, ndev) == WL_MODE_BSS) {
+		switch (pktflag) {
+			case VNDR_IE_PRBREQ_FLAG :
+				mgmt_ie_buf = wl->sta_info->probe_req_ie;
+				mgmt_ie_len = &wl->sta_info->probe_req_ie_len;
+				mgmt_ie_buf_len = sizeof(wl->sta_info->probe_req_ie);
+				break;
+			case VNDR_IE_ASSOCREQ_FLAG :
+				mgmt_ie_buf = wl->sta_info->assoc_req_ie;
+				mgmt_ie_len = &wl->sta_info->assoc_req_ie_len;
+				mgmt_ie_buf_len = sizeof(wl->sta_info->assoc_req_ie);
+				break;
+			default:
+				mgmt_ie_buf = NULL;
+				mgmt_ie_len = NULL;
+				CFGP2P_ERR(("not suitable type\n"));
+				return -1;
+		}
+		bssidx = 0;
+	} else {
+		CFGP2P_ERR(("not suitable type\n"));
+		return -1;
+	}
+
+	if (vndr_ie_len > mgmt_ie_buf_len) {
+		CFGP2P_ERR(("extra IE size too big\n"));
+		ret = -ENOMEM;
+	} else {
+		if (mgmt_ie_buf != NULL) {
+			if (vndr_ie_len && (vndr_ie_len == *mgmt_ie_len) &&
+			     (memcmp(mgmt_ie_buf, vndr_ie, vndr_ie_len) == 0)) {
+				CFGP2P_INFO(("Previous mgmt IE is equals to current IE"));
+				goto exit;
+			}
+			pos = 0;
+			delete = 1;
+			ie_buf = (u8 *) mgmt_ie_buf;
+			while (pos < *mgmt_ie_len) {
+				ie_id = ie_buf[pos++];
+				ie_len = ie_buf[pos++];
+				if ((ie_id == DOT11_MNG_VS_ID) &&
+				   (wl_cfgp2p_is_wps_ie(&ie_buf[pos-2], NULL, 0) ||
+#ifdef WLWFDIE
+					wl_cfgp2p_is_p2p_ie(&ie_buf[pos-2], NULL, 0) ||
+					wl_cfgp2p_is_wfd_ie(&ie_buf[pos-2], NULL, 0))) {
+#else
+					wl_cfgp2p_is_p2p_ie(&ie_buf[pos-2], NULL, 0))) {
+#endif /* WLWFDIE */
+					CFGP2P_INFO(("DELELED ID : %d, Len : %d , OUI :"
+						"%02x:%02x:%02x\n", ie_id, ie_len, ie_buf[pos],
+						ie_buf[pos+1], ie_buf[pos+2]));
+					ret = wl_cfgp2p_vndr_ie(wl, ndev, bssidx, pktflag,
+						ie_buf+pos, VNDR_SPEC_ELEMENT_ID, ie_buf+pos+3,
+						ie_len-3, delete);
+				}
+				pos += ie_len;
+			}
+
+		}
+		*mgmt_ie_len = 0;
+		/* Add if there is any extra IE */
+		if (vndr_ie && vndr_ie_len) {
+			/* save the current IE in wl struct */
+			memcpy(mgmt_ie_buf, vndr_ie, vndr_ie_len);
+			*mgmt_ie_len = vndr_ie_len;
+			pos = 0;
+			ie_buf = (u8 *) vndr_ie;
+			delete = 0;
+			while (pos < vndr_ie_len) {
+				ie_id = ie_buf[pos++];
+				ie_len = ie_buf[pos++];
+				if ((ie_id == DOT11_MNG_VS_ID) &&
+				   (wl_cfgp2p_is_wps_ie(&ie_buf[pos-2], NULL, 0) ||
+#ifdef WLWFDIE
+					wl_cfgp2p_is_p2p_ie(&ie_buf[pos-2], NULL, 0) ||
+					wl_cfgp2p_is_wfd_ie(&ie_buf[pos-2], NULL, 0))) {
+#else
+					wl_cfgp2p_is_p2p_ie(&ie_buf[pos-2], NULL, 0))) {
+#endif /* WLWFDIE */
+					CFGP2P_INFO(("ADDED ID : %d, Len : %d , OUI :"
+						"%02x:%02x:%02x\n", ie_id, ie_len, ie_buf[pos],
+						ie_buf[pos+1], ie_buf[pos+2]));
+					ret = wl_cfgp2p_vndr_ie(wl, ndev, bssidx, pktflag,
+						ie_buf+pos, VNDR_SPEC_ELEMENT_ID, ie_buf+pos+3,
+						ie_len-3, delete);
+				}
+				pos += ie_len;
+			}
+		}
+	}
+#undef IE_TYPE
+#undef IE_TYPE_LEN
+exit:
+	return ret;
+}
+
+/* Clear the manament IE buffer of BSSCFG
+ * Parameters:
+ * @wl       : wl_private data
+ * @bssidx   : bssidx for BSS
+ *
+ * Returns 0 if success.
+ */
+s32
+wl_cfgp2p_clear_management_ie(struct wl_priv *wl, s32 bssidx)
+{
+#define INIT_IE(IE_TYPE, BSS_TYPE)		\
+	do {							\
+		memset(wl_to_p2p_bss_saved_ie(wl, BSS_TYPE).p2p_ ## IE_TYPE ## _ie, 0, \
+		   sizeof(wl_to_p2p_bss_saved_ie(wl, BSS_TYPE).p2p_ ## IE_TYPE ## _ie)); \
+		wl_to_p2p_bss_saved_ie(wl, BSS_TYPE).p2p_ ## IE_TYPE ## _ie_len = 0; \
+	} while (0);
+	if (bssidx < 0) {
+		CFGP2P_ERR(("invalid bssidx\n"));
+		return BCME_BADARG;
+	}
+	INIT_IE(probe_req, bssidx);
+	INIT_IE(probe_res, bssidx);
+	INIT_IE(assoc_req, bssidx);
+	INIT_IE(assoc_res, bssidx);
+	INIT_IE(beacon, bssidx);
+	return BCME_OK;
+}
+
+
+/* Is any of the tlvs the expected entry? If
+ * not update the tlvs buffer pointer/length.
+ */
+static bool
+wl_cfgp2p_has_ie(u8 *ie, u8 **tlvs, u32 *tlvs_len, const u8 *oui, u32 oui_len, u8 type)
+{
+	/* If the contents match the OUI and the type */
+	if (ie[TLV_LEN_OFF] >= oui_len + 1 &&
+	        !bcmp(&ie[TLV_BODY_OFF], oui, oui_len) &&
+	        type == ie[TLV_BODY_OFF + oui_len]) {
+		return TRUE;
+	}
+
+	if (tlvs == NULL)
+		return FALSE;
+	/* point to the next ie */
+	ie += ie[TLV_LEN_OFF] + TLV_HDR_LEN;
+	/* calculate the length of the rest of the buffer */
+	*tlvs_len -= (int)(ie - *tlvs);
+	/* update the pointer to the start of the buffer */
+	*tlvs = ie;
+
+	return FALSE;
+}
+
+wpa_ie_fixed_t *
+wl_cfgp2p_find_wpaie(u8 *parse, u32 len)
+{
+	bcm_tlv_t *ie;
+
+	while ((ie = bcm_parse_tlvs(parse, (u32)len, DOT11_MNG_VS_ID))) {
+		if (wl_cfgp2p_is_wpa_ie((u8*)ie, &parse, &len)) {
+			return (wpa_ie_fixed_t *)ie;
+		}
+	}
+	return NULL;
+}
+
+wpa_ie_fixed_t *
+wl_cfgp2p_find_wpsie(u8 *parse, u32 len)
+{
+	bcm_tlv_t *ie;
+
+	while ((ie = bcm_parse_tlvs(parse, (u32)len, DOT11_MNG_VS_ID))) {
+		if (wl_cfgp2p_is_wps_ie((u8*)ie, &parse, &len)) {
+			return (wpa_ie_fixed_t *)ie;
+		}
+	}
+	return NULL;
+}
+
+wifi_p2p_ie_t *
+wl_cfgp2p_find_p2pie(u8 *parse, u32 len)
+{
+	bcm_tlv_t *ie;
+
+	while ((ie = bcm_parse_tlvs(parse, (int)len, DOT11_MNG_VS_ID))) {
+		if (wl_cfgp2p_is_p2p_ie((uint8*)ie, &parse, &len)) {
+			return (wifi_p2p_ie_t *)ie;
+		}
+	}
+	return NULL;
+}
+
+#ifdef WLWFDIE
+wifi_wfd_ie_t *
+wl_cfgp2p_find_wfdie(u8 *parse, u32 len)
+{
+	bcm_tlv_t *ie;
+
+	while ((ie = bcm_parse_tlvs(parse, (int)len, DOT11_MNG_VS_ID))) {
+		if (wl_cfgp2p_is_wfd_ie((uint8*)ie, &parse, &len)) {
+			return (wifi_wfd_ie_t *)ie;
+		}
+	}
+	return NULL;
+}
+#endif /* WLWFDIE */
+
+static s32
+wl_cfgp2p_vndr_ie(struct wl_priv *wl, struct net_device *ndev, s32 bssidx, s32 pktflag,
+            s8 *oui, s32 ie_id, s8 *data, s32 data_len, s32 delete)
+{
+	s32 err = BCME_OK;
+	s32 buf_len;
+	s32 iecount;
+
+	vndr_ie_setbuf_t *ie_setbuf;
+
+	/* Validate the pktflag parameter */
+	if ((pktflag & ~(VNDR_IE_BEACON_FLAG | VNDR_IE_PRBRSP_FLAG |
+	            VNDR_IE_ASSOCRSP_FLAG | VNDR_IE_AUTHRSP_FLAG |
+	            VNDR_IE_PRBREQ_FLAG | VNDR_IE_ASSOCREQ_FLAG))) {
+		CFGP2P_ERR(("p2pwl_vndr_ie: Invalid packet flag 0x%x\n", pktflag));
+		return -1;
+	}
+
+	buf_len = sizeof(vndr_ie_setbuf_t) + data_len - 1;
+	ie_setbuf = (vndr_ie_setbuf_t *) kzalloc(buf_len, GFP_KERNEL);
+
+	CFGP2P_INFO((" ie_id : %02x, data length : %d\n", ie_id, data_len));
+	if (!ie_setbuf) {
+
+		CFGP2P_ERR(("Error allocating buffer for IE\n"));
+		return -ENOMEM;
+	}
+	if (delete)
+		strcpy(ie_setbuf->cmd, "del");
+	else
+		strcpy(ie_setbuf->cmd, "add");
+	/* Buffer contains only 1 IE */
+	iecount = htod32(1);
+	memcpy((void *)&ie_setbuf->vndr_ie_buffer.iecount, &iecount, sizeof(int));
+	pktflag = htod32(pktflag);
+	memcpy((void *)&ie_setbuf->vndr_ie_buffer.vndr_ie_list[0].pktflag,
+	    &pktflag, sizeof(uint32));
+	ie_setbuf->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.id = ie_id;
+	ie_setbuf->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.len
+	        = (uchar)(data_len + VNDR_IE_MIN_LEN);
+	memcpy(ie_setbuf->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.oui, oui, 3);
+	memcpy(ie_setbuf->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.data, data, data_len);
+	err = wldev_iovar_setbuf_bsscfg(ndev, "vndr_ie", ie_setbuf, buf_len,
+		wl->ioctl_buf, WLC_IOCTL_MAXLEN, bssidx, &wl->ioctl_buf_sync);
+
+	CFGP2P_INFO(("vndr_ie iovar returns %d\n", err));
+	kfree(ie_setbuf);
+	return err;
+}
+
+/*
+ * Search the bssidx based on dev argument
+ * Parameters:
+ * @wl       : wl_private data
+ * @ndev     : net device to search bssidx
+ *  Returns bssidx for ndev
+ */
+s32
+wl_cfgp2p_find_idx(struct wl_priv *wl, struct net_device *ndev)
+{
+	u32 i;
+	s32 index = -1;
+
+	if (ndev == NULL) {
+		CFGP2P_ERR((" ndev is NULL\n"));
+		goto exit;
+	}
+	if (!wl->p2p_supported) {
+		return P2PAPI_BSSCFG_PRIMARY;
+	}
+	for (i = 0; i < P2PAPI_BSSCFG_MAX; i++) {
+		if (ndev == wl_to_p2p_bss_ndev(wl, i)) {
+			index = wl_to_p2p_bss_bssidx(wl, i);
+			break;
+		}
+	}
+	if (index == -1)
+		return P2PAPI_BSSCFG_PRIMARY;
+exit:
+	return index;
+}
+/*
+ * Callback function for WLC_E_P2P_DISC_LISTEN_COMPLETE
+ */
+s32
+wl_cfgp2p_listen_complete(struct wl_priv *wl, struct net_device *ndev,
+            const wl_event_msg_t *e, void *data)
+{
+	s32 ret = BCME_OK;
+
+	CFGP2P_DBG((" Enter\n"));
+
+	/* If p2p_info is de-initialized, do nothing  */
+	if (!wl->p2p)
+		return ret;
+
+	if (wl_get_p2p_status(wl, LISTEN_EXPIRED) == 0) {
+		wl_set_p2p_status(wl, LISTEN_EXPIRED);
+		if (timer_pending(&wl->p2p->listen_timer)) {
+			del_timer_sync(&wl->p2p->listen_timer);
+		}
+		cfg80211_remain_on_channel_expired(ndev, wl->last_roc_id, &wl->remain_on_chan,
+		    wl->remain_on_chan_type, GFP_KERNEL);
+	} else
+		wl_clr_p2p_status(wl, LISTEN_EXPIRED);
+
+	return ret;
+
+}
+
+/*
+ *  Timer expire callback function for LISTEN
+ *  We can't report cfg80211_remain_on_channel_expired from Timer ISR context,
+ *  so lets do it from thread context.
+ */
+static void
+wl_cfgp2p_listen_expired(unsigned long data)
+{
+	wl_event_msg_t msg;
+	struct wl_priv *wl = (struct wl_priv *) data;
+
+	CFGP2P_DBG((" Enter\n"));
+	memset(&msg, 0, sizeof(wl_event_msg_t));
+	msg.event_type =  hton32(WLC_E_P2P_DISC_LISTEN_COMPLETE);
+	wl_cfg80211_event(wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_DEVICE), &msg, NULL);
+}
+
+/*
+ *  Routine for cancelling the P2P LISTEN
+ */
+static s32
+wl_cfgp2p_cancel_listen(struct wl_priv *wl, struct net_device *ndev,
+                         bool notify)
+{
+	WL_DBG(("Enter \n"));
+
+	/* Irrespective of whether timer is running or not, reset
+	 * the LISTEN state.
+	 */
+	wl_cfgp2p_set_p2p_mode(wl, WL_P2P_DISC_ST_SCAN, 0, 0,
+		wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE));
+
+	if (timer_pending(&wl->p2p->listen_timer)) {
+		del_timer_sync(&wl->p2p->listen_timer);
+
+		if (notify)
+			cfg80211_remain_on_channel_expired(ndev, wl->last_roc_id,
+				&wl->remain_on_chan, wl->remain_on_chan_type, GFP_KERNEL);
+	}
+
+
+	return 0;
+}
+
+/*
+ * Do a P2P Listen on the given channel for the given duration.
+ * A listen consists of sitting idle and responding to P2P probe requests
+ * with a P2P probe response.
+ *
+ * This fn assumes dongle p2p device discovery is already enabled.
+ * Parameters   :
+ * @wl          : wl_private data
+ * @channel     : channel to listen
+ * @duration_ms : the time (milli seconds) to wait
+ */
+s32
+wl_cfgp2p_discover_listen(struct wl_priv *wl, s32 channel, u32 duration_ms)
+{
+#define INIT_TIMER(timer, func, duration, extra_delay)	\
+	do {                   \
+		init_timer(timer); \
+		timer->function = func; \
+		timer->expires = jiffies + msecs_to_jiffies(duration + extra_delay); \
+		timer->data = (unsigned long) wl; \
+		add_timer(timer); \
+	} while (0);
+
+	s32 ret = BCME_OK;
+	struct timer_list *_timer;
+	CFGP2P_DBG((" Enter Channel : %d, Duration : %d\n", channel, duration_ms));
+	if (unlikely(wl_get_p2p_status(wl, DISCOVERY_ON) == 0)) {
+
+		CFGP2P_ERR((" Discovery is not set, so we have noting to do\n"));
+
+		ret = BCME_NOTREADY;
+		goto exit;
+	}
+	if (timer_pending(&wl->p2p->listen_timer)) {
+		CFGP2P_DBG(("previous LISTEN is not completed yet\n"));
+		goto exit;
+
+	} else
+		wl_clr_p2p_status(wl, LISTEN_EXPIRED);
+
+	wl_cfgp2p_set_p2p_mode(wl, WL_P2P_DISC_ST_LISTEN, channel, (u16) duration_ms,
+	            wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE));
+	_timer = &wl->p2p->listen_timer;
+
+	/*  We will wait to receive WLC_E_P2P_DISC_LISTEN_COMPLETE from dongle ,
+	 *  otherwise we will wait up to duration_ms + 200ms
+	 */
+	INIT_TIMER(_timer, wl_cfgp2p_listen_expired, duration_ms, 200);
+
+#undef INIT_TIMER
+exit:
+	return ret;
+}
+
+
+s32
+wl_cfgp2p_discover_enable_search(struct wl_priv *wl, u8 enable)
+{
+	s32 ret = BCME_OK;
+	CFGP2P_DBG((" Enter\n"));
+	if (!wl_get_p2p_status(wl, DISCOVERY_ON)) {
+
+		CFGP2P_DBG((" do nothing, discovery is off\n"));
+		return ret;
+	}
+	if (wl_get_p2p_status(wl, SEARCH_ENABLED) == enable) {
+		CFGP2P_DBG(("already : %d\n", enable));
+		return ret;
+	}
+
+	wl_chg_p2p_status(wl, SEARCH_ENABLED);
+	/* When disabling Search, reset the WL driver's p2p discovery state to
+	 * WL_P2P_DISC_ST_SCAN.
+	 */
+	if (!enable) {
+		wl_clr_p2p_status(wl, SCANNING);
+		ret = wl_cfgp2p_set_p2p_mode(wl, WL_P2P_DISC_ST_SCAN, 0, 0,
+		            wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE));
+	}
+
+	return ret;
+}
+
+/*
+ * Callback function for WLC_E_ACTION_FRAME_COMPLETE, WLC_E_ACTION_FRAME_OFF_CHAN_COMPLETE
+ */
+s32
+wl_cfgp2p_action_tx_complete(struct wl_priv *wl, struct net_device *ndev,
+            const wl_event_msg_t *e, void *data)
+{
+	s32 ret = BCME_OK;
+	u32 event_type = ntoh32(e->event_type);
+	u32 status = ntoh32(e->status);
+	CFGP2P_DBG((" Enter\n"));
+	if (event_type == WLC_E_ACTION_FRAME_COMPLETE) {
+
+		CFGP2P_INFO((" WLC_E_ACTION_FRAME_COMPLETE is received : %d\n", status));
+		if (status == WLC_E_STATUS_SUCCESS) {
+			wl_set_p2p_status(wl, ACTION_TX_COMPLETED);
+		}
+		else {
+			wl_set_p2p_status(wl, ACTION_TX_NOACK);
+			CFGP2P_ERR(("WLC_E_ACTION_FRAME_COMPLETE : NO ACK\n"));
+		}
+	} else {
+		CFGP2P_INFO((" WLC_E_ACTION_FRAME_OFFCHAN_COMPLETE is received,"
+					"status : %d\n", status));
+		wake_up_interruptible(&wl->netif_change_event);
+	}
+	return ret;
+}
+/* Send an action frame immediately without doing channel synchronization.
+ *
+ * This function does not wait for a completion event before returning.
+ * The WLC_E_ACTION_FRAME_COMPLETE event will be received when the action
+ * frame is transmitted.
+ * The WLC_E_ACTION_FRAME_OFF_CHAN_COMPLETE event will be received when an
+ * 802.11 ack has been received for the sent action frame.
+ */
+s32
+wl_cfgp2p_tx_action_frame(struct wl_priv *wl, struct net_device *dev,
+	wl_af_params_t *af_params, s32 bssidx)
+{
+	s32 ret = BCME_OK;
+	s32 timeout = 0;
+
+
+	CFGP2P_INFO(("\n"));
+	CFGP2P_INFO(("channel : %u , dwell time : %u\n",
+	    af_params->channel, af_params->dwell_time));
+
+	wl_clr_p2p_status(wl, ACTION_TX_COMPLETED);
+	wl_clr_p2p_status(wl, ACTION_TX_NOACK);
+#define MAX_WAIT_TIME 2000
+	if (bssidx == P2PAPI_BSSCFG_PRIMARY)
+		bssidx =  wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_DEVICE);
+
+	ret = wldev_iovar_setbuf_bsscfg(dev, "actframe", af_params, sizeof(*af_params),
+		wl->ioctl_buf, WLC_IOCTL_MAXLEN, bssidx, &wl->ioctl_buf_sync);
+
+	if (ret < 0) {
+		CFGP2P_ERR((" sending action frame is failed\n"));
+		goto exit;
+	}
+	timeout = wait_event_interruptible_timeout(wl->netif_change_event,
+		(wl_get_p2p_status(wl, ACTION_TX_COMPLETED) || wl_get_p2p_status(wl, ACTION_TX_NOACK)),
+		msecs_to_jiffies(MAX_WAIT_TIME));
+
+	if (timeout > 0 && wl_get_p2p_status(wl, ACTION_TX_COMPLETED)) {
+		CFGP2P_INFO(("tx action frame operation is completed\n"));
+		ret = BCME_OK;
+	} else {
+		ret = BCME_ERROR;
+		CFGP2P_INFO(("tx action frame operation is failed\n"));
+	}
+exit:
+	CFGP2P_INFO((" via act frame iovar : status = %d\n", ret));
+#undef MAX_WAIT_TIME
+	return ret;
+}
+
+/* Generate our P2P Device Address and P2P Interface Address from our primary
+ * MAC address.
+ */
+void
+wl_cfgp2p_generate_bss_mac(struct ether_addr *primary_addr,
+            struct ether_addr *out_dev_addr, struct ether_addr *out_int_addr)
+{
+	memset(out_dev_addr, 0, sizeof(*out_dev_addr));
+	memset(out_int_addr, 0, sizeof(*out_int_addr));
+
+	/* Generate the P2P Device Address.  This consists of the device's
+	 * primary MAC address with the locally administered bit set.
+	 */
+	memcpy(out_dev_addr, primary_addr, sizeof(*out_dev_addr));
+	out_dev_addr->octet[0] |= 0x02;
+
+	/* Generate the P2P Interface Address.  If the discovery and connection
+	 * BSSCFGs need to simultaneously co-exist, then this address must be
+	 * different from the P2P Device Address.
+	 */
+	memcpy(out_int_addr, out_dev_addr, sizeof(*out_int_addr));
+	out_int_addr->octet[4] ^= 0x80;
+
+}
+
+/* P2P IF Address change to Virtual Interface MAC Address */
+void
+wl_cfg80211_change_ifaddr(u8* buf, struct ether_addr *p2p_int_addr, u8 element_id)
+{
+	wifi_p2p_ie_t *ie = (wifi_p2p_ie_t*) buf;
+	u16 len = ie->len;
+	u8 *subel;
+	u8 subelt_id;
+	u16 subelt_len;
+	CFGP2P_DBG((" Enter\n"));
+
+	/* Point subel to the P2P IE's subelt field.
+	 * Subtract the preceding fields (id, len, OUI, oui_type) from the length.
+	 */
+	subel = ie->subelts;
+	len -= 4;	/* exclude OUI + OUI_TYPE */
+
+	while (len >= 3) {
+	/* attribute id */
+		subelt_id = *subel;
+		subel += 1;
+		len -= 1;
+
+		/* 2-byte little endian */
+		subelt_len = *subel++;
+		subelt_len |= *subel++ << 8;
+
+		len -= 2;
+		len -= subelt_len;	/* for the remaining subelt fields */
+
+		if (subelt_id == element_id) {
+			if (subelt_id == P2P_SEID_INTINTADDR) {
+				memcpy(subel, p2p_int_addr->octet, ETHER_ADDR_LEN);
+				CFGP2P_INFO(("Intended P2P Interface Address ATTR FOUND\n"));
+			} else if (subelt_id == P2P_SEID_DEV_ID) {
+				memcpy(subel, p2p_int_addr->octet, ETHER_ADDR_LEN);
+				CFGP2P_INFO(("Device ID ATTR FOUND\n"));
+			} else if (subelt_id == P2P_SEID_DEV_INFO) {
+				memcpy(subel, p2p_int_addr->octet, ETHER_ADDR_LEN);
+				CFGP2P_INFO(("Device INFO ATTR FOUND\n"));
+			} else if (subelt_id == P2P_SEID_GROUP_ID) {
+				memcpy(subel, p2p_int_addr->octet, ETHER_ADDR_LEN);
+				CFGP2P_INFO(("GROUP ID ATTR FOUND\n"));
+			}			return;
+		} else {
+			CFGP2P_DBG(("OTHER id : %d\n", subelt_id));
+		}
+		subel += subelt_len;
+	}
+}
+/*
+ * Check if a BSS is up.
+ * This is a common implementation called by most OSL implementations of
+ * p2posl_bss_isup().  DO NOT call this function directly from the
+ * common code -- call p2posl_bss_isup() instead to allow the OSL to
+ * override the common implementation if necessary.
+ */
+bool
+wl_cfgp2p_bss_isup(struct net_device *ndev, int bsscfg_idx)
+{
+	s32 result, val;
+	bool isup = false;
+	s8 getbuf[64];
+
+	/* Check if the BSS is up */
+	*(int*)getbuf = -1;
+	result = wldev_iovar_getbuf_bsscfg(ndev, "bss", &bsscfg_idx,
+		sizeof(bsscfg_idx), getbuf, sizeof(getbuf), 0, NULL);
+	if (result != 0) {
+		CFGP2P_ERR(("'wl bss -C %d' failed: %d\n", bsscfg_idx, result));
+		CFGP2P_ERR(("NOTE: this ioctl error is normal "
+					"when the BSS has not been created yet.\n"));
+	} else {
+		val = *(int*)getbuf;
+		val = dtoh32(val);
+		CFGP2P_INFO(("---wl bss -C %d   ==> %d\n", bsscfg_idx, val));
+		isup = (val ? TRUE : FALSE);
+	}
+	return isup;
+}
+
+
+/* Bring up or down a BSS */
+s32
+wl_cfgp2p_bss(struct wl_priv *wl, struct net_device *ndev, s32 bsscfg_idx, s32 up)
+{
+	s32 ret = BCME_OK;
+	s32 val = up ? 1 : 0;
+
+	struct {
+		s32 cfg;
+		s32 val;
+	} bss_setbuf;
+
+	bss_setbuf.cfg = htod32(bsscfg_idx);
+	bss_setbuf.val = htod32(val);
+	CFGP2P_INFO(("---wl bss -C %d %s\n", bsscfg_idx, up ? "up" : "down"));
+	ret = wldev_iovar_setbuf(ndev, "bss", &bss_setbuf, sizeof(bss_setbuf),
+		wl->ioctl_buf, WLC_IOCTL_MAXLEN, &wl->ioctl_buf_sync);
+
+	if (ret != 0) {
+		CFGP2P_ERR(("'bss %d' failed with %d\n", up, ret));
+	}
+
+	return ret;
+}
+
+/* Check if 'p2p' is supported in the driver */
+s32
+wl_cfgp2p_supported(struct wl_priv *wl, struct net_device *ndev)
+{
+	s32 ret = BCME_OK;
+	s32 p2p_supported = 0;
+	ret = wldev_iovar_getint(ndev, "p2p",
+	               &p2p_supported);
+	if (ret < 0) {
+		CFGP2P_ERR(("wl p2p error %d\n", ret));
+		return 0;
+	}
+	if (p2p_supported == 1) {
+		CFGP2P_INFO(("p2p is supported\n"));
+	} else {
+		CFGP2P_INFO(("p2p is unsupported\n"));
+		p2p_supported = 0;
+	}
+	return p2p_supported;
+}
+
+/* Cleanup P2P resources */
+s32
+wl_cfgp2p_down(struct wl_priv *wl)
+{
+
+	wl_cfgp2p_cancel_listen(wl,
+		wl->p2p_net ? wl->p2p_net : wl_to_prmry_ndev(wl), TRUE);
+
+	wl_cfgp2p_deinit_priv(wl);
+	return 0;
+}
+
+s32
+wl_cfgp2p_set_p2p_noa(struct wl_priv *wl, struct net_device *ndev, char* buf, int len)
+{
+	s32 ret = -1;
+	int count, start, duration;
+	wl_p2p_sched_t dongle_noa;
+
+	CFGP2P_DBG((" Enter\n"));
+
+	memset(&dongle_noa, 0, sizeof(dongle_noa));
+
+	if (wl->p2p && wl->p2p->vif_created) {
+
+		wl->p2p->noa.desc[0].start = 0;
+
+		sscanf(buf, "%d %d %d", &count, &start, &duration);
+		CFGP2P_DBG(("set_p2p_noa count %d start %d duration %d\n",
+			count, start, duration));
+		if (count != -1)
+			wl->p2p->noa.desc[0].count = count;
+
+		/* supplicant gives interval as start */
+		if (start != -1)
+			wl->p2p->noa.desc[0].interval = start;
+
+		if (duration != -1)
+			wl->p2p->noa.desc[0].duration = duration;
+
+		if (wl->p2p->noa.desc[0].count != 255) {
+			wl->p2p->noa.desc[0].start = 200;
+			dongle_noa.type = WL_P2P_SCHED_TYPE_REQ_ABS;
+			dongle_noa.action = WL_P2P_SCHED_ACTION_GOOFF;
+			dongle_noa.option = WL_P2P_SCHED_OPTION_TSFOFS;
+		}
+		else {
+			/* Continuous NoA interval. */
+			dongle_noa.action = WL_P2P_SCHED_ACTION_NONE;
+			dongle_noa.type = WL_P2P_SCHED_TYPE_ABS;
+			if ((wl->p2p->noa.desc[0].interval == 102) ||
+				(wl->p2p->noa.desc[0].interval == 100)) {
+				wl->p2p->noa.desc[0].start = 100 -
+					wl->p2p->noa.desc[0].duration;
+				dongle_noa.option = WL_P2P_SCHED_OPTION_BCNPCT;
+			}
+			else {
+				dongle_noa.option = WL_P2P_SCHED_OPTION_NORMAL;
+			}
+		}
+		/* Put the noa descriptor in dongle format for dongle */
+		dongle_noa.desc[0].count = htod32(wl->p2p->noa.desc[0].count);
+		if (dongle_noa.option == WL_P2P_SCHED_OPTION_BCNPCT) {
+			dongle_noa.desc[0].start = htod32(wl->p2p->noa.desc[0].start);
+			dongle_noa.desc[0].duration = htod32(wl->p2p->noa.desc[0].duration);
+		}
+		else {
+			dongle_noa.desc[0].start = htod32(wl->p2p->noa.desc[0].start*1000);
+			dongle_noa.desc[0].duration = htod32(wl->p2p->noa.desc[0].duration*1000);
+		}
+		dongle_noa.desc[0].interval = htod32(wl->p2p->noa.desc[0].interval*1000);
+
+		ret = wldev_iovar_setbuf(wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION),
+			"p2p_noa", &dongle_noa, sizeof(dongle_noa), wl->ioctl_buf, WLC_IOCTL_MAXLEN,
+			&wl->ioctl_buf_sync);
+
+		if (ret < 0) {
+			CFGP2P_ERR(("fw set p2p_noa failed %d\n", ret));
+		}
+	}
+	else {
+		CFGP2P_ERR(("ERROR: set_noa in non-p2p mode\n"));
+	}
+	return ret;
+}
+
+s32
+wl_cfgp2p_get_p2p_noa(struct wl_priv *wl, struct net_device *ndev, char* buf, int buf_len)
+{
+	wifi_p2p_noa_desc_t *noa_desc;
+	int len = 0, i;
+	char _buf[200];
+
+	CFGP2P_DBG((" Enter\n"));
+	buf[0] = '\0';
+	if (wl->p2p && wl->p2p->vif_created) {
+		if (wl->p2p->noa.desc[0].count || wl->p2p->ops.ops) {
+			_buf[0] = 1; /* noa index */
+			_buf[1] = (wl->p2p->ops.ops ? 0x80: 0) |
+				(wl->p2p->ops.ctw & 0x7f); /* ops + ctw */
+			len += 2;
+			if (wl->p2p->noa.desc[0].count) {
+				noa_desc = (wifi_p2p_noa_desc_t*)&_buf[len];
+				noa_desc->cnt_type = wl->p2p->noa.desc[0].count;
+				noa_desc->duration = wl->p2p->noa.desc[0].duration;
+				noa_desc->interval = wl->p2p->noa.desc[0].interval;
+				noa_desc->start = wl->p2p->noa.desc[0].start;
+				len += sizeof(wifi_p2p_noa_desc_t);
+			}
+			if (buf_len <= len * 2) {
+				CFGP2P_ERR(("ERROR: buf_len %d in not enough for"
+					"returning noa in string format\n", buf_len));
+				return -1;
+			}
+			/* We have to convert the buffer data into ASCII strings */
+			for (i = 0; i < len; i++) {
+				sprintf(buf, "%02x", _buf[i]);
+				buf += 2;
+			}
+			buf[i*2] = '\0';
+		}
+	}
+	else {
+		CFGP2P_ERR(("ERROR: get_noa in non-p2p mode\n"));
+		return -1;
+	}
+	return len * 2;
+}
+
+s32
+wl_cfgp2p_set_p2p_ps(struct wl_priv *wl, struct net_device *ndev, char* buf, int len)
+{
+	int ps, ctw;
+	int ret = -1;
+	s32 legacy_ps;
+
+	CFGP2P_DBG((" Enter\n"));
+	if (wl->p2p && wl->p2p->vif_created) {
+		sscanf(buf, "%d %d %d", &legacy_ps, &ps, &ctw);
+		CFGP2P_DBG((" Enter legacy_ps %d ps %d ctw %d\n", legacy_ps, ps, ctw));
+		if (ctw != -1) {
+			wl->p2p->ops.ctw = ctw;
+			ret = 0;
+		}
+		if (ps != -1) {
+			wl->p2p->ops.ops = ps;
+			ret = wldev_iovar_setbuf(wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION),
+				"p2p_ops", &wl->p2p->ops, sizeof(wl->p2p->ops),
+				wl->ioctl_buf, WLC_IOCTL_MAXLEN, &wl->ioctl_buf_sync);
+			if (ret < 0) {
+				CFGP2P_ERR(("fw set p2p_ops failed %d\n", ret));
+			}
+		}
+
+		if (legacy_ps != -1) {
+			s32 pm = legacy_ps ? PM_MAX : PM_OFF;
+			ret = wldev_ioctl(wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION),
+				WLC_SET_PM, &pm, sizeof(pm), true);
+			if (unlikely(ret)) {
+				CFGP2P_ERR(("error (%d)\n", ret));
+			}
+		}
+	}
+	else {
+		CFGP2P_ERR(("ERROR: set_p2p_ps in non-p2p mode\n"));
+		ret = -1;
+	}
+	return ret;
+}
+
+u8 *
+wl_cfgp2p_retreive_p2pattrib(void *buf, u8 element_id)
+{
+	wifi_p2p_ie_t *ie = NULL;
+	u16 len = 0;
+	u8 *subel;
+	u8 subelt_id;
+	u16 subelt_len;
+
+	if (!buf) {
+		WL_ERR(("P2P IE not present"));
+		return 0;
+	}
+
+	ie = (wifi_p2p_ie_t*) buf;
+	len = ie->len;
+
+	/* Point subel to the P2P IE's subelt field.
+	 * Subtract the preceding fields (id, len, OUI, oui_type) from the length.
+	 */
+	subel = ie->subelts;
+	len -= 4;	/* exclude OUI + OUI_TYPE */
+
+	while (len >= 3) {
+		/* attribute id */
+		subelt_id = *subel;
+		subel += 1;
+		len -= 1;
+
+		/* 2-byte little endian */
+		subelt_len = *subel++;
+		subelt_len |= *subel++ << 8;
+
+		len -= 2;
+		len -= subelt_len;	/* for the remaining subelt fields */
+
+		if (subelt_id == element_id) {
+			/* This will point to start of subelement attrib after
+			 * attribute id & len
+			 */
+			return subel;
+		}
+
+		/* Go to next subelement */
+		subel += subelt_len;
+	}
+
+	/* Not Found */
+	return NULL;
+}
+
+#define P2P_GROUP_CAPAB_GO_BIT	0x01
+u8 *
+wl_cfgp2p_retreive_p2p_dev_addr(wl_bss_info_t *bi, u32 bi_length)
+{
+	wifi_p2p_ie_t * p2p_ie = NULL;
+	u8 *capability = NULL;
+	bool p2p_go	= 0;
+	u8 *ptr = NULL;
+
+	if (!(p2p_ie = wl_cfgp2p_find_p2pie(((u8 *) bi) + bi->ie_offset, bi->ie_length))) {
+		WL_ERR(("P2P IE not found"));
+		return NULL;
+	}
+
+	if (!(capability = wl_cfgp2p_retreive_p2pattrib(p2p_ie, P2P_SEID_P2P_INFO))) {
+		WL_ERR(("P2P Capability attribute not found"));
+		return NULL;
+	}
+
+	/* Check Group capability for Group Owner bit */
+	p2p_go = capability[1] & P2P_GROUP_CAPAB_GO_BIT;
+	if (!p2p_go) {
+		return bi->BSSID.octet;
+	}
+
+	/* In probe responses, DEVICE INFO attribute will be present */
+	if (!(ptr = wl_cfgp2p_retreive_p2pattrib(p2p_ie, P2P_SEID_DEV_INFO))) {
+		/* If DEVICE_INFO is not found, this might be a beacon frame.
+		 * check for DEVICE_ID in the beacon frame.
+		 */
+		ptr = wl_cfgp2p_retreive_p2pattrib(p2p_ie, P2P_SEID_DEV_ID);
+	}
+
+	if (!ptr)
+		WL_ERR((" Both DEVICE_ID & DEVICE_INFO attribute not present in P2P IE "));
+
+	return ptr;
+}
+
+s32
+wl_cfgp2p_register_ndev(struct wl_priv *wl)
+{
+	int ret = 0;
+	struct net_device* net = NULL;
+	struct wireless_dev *wdev;
+	uint8 temp_addr[ETHER_ADDR_LEN] = { 0x00, 0x90, 0x4c, 0x33, 0x22, 0x11 };
+
+	/* Allocate etherdev, including space for private structure */
+	if (!(net = alloc_etherdev(sizeof(wl)))) {
+		CFGP2P_ERR(("%s: OOM - alloc_etherdev\n", __FUNCTION__));
+		goto fail;
+	}
+
+	strcpy(net->name, "p2p%d");
+	net->name[IFNAMSIZ - 1] = '\0';
+
+	/* Copy the reference to wl_priv */
+	memcpy((void *)netdev_priv(net), &wl, sizeof(wl));
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31))
+	ASSERT(!net->open);
+	net->do_ioctl = wl_cfgp2p_do_ioctl;
+	net->hard_start_xmit = wl_cfgp2p_start_xmit;
+	net->open = wl_cfgp2p_if_open;
+	net->stop = wl_cfgp2p_if_stop;
+#else
+	ASSERT(!net->netdev_ops);
+	net->netdev_ops = &wl_cfgp2p_if_ops;
+#endif
+
+	/* Register with a dummy MAC addr */
+	memcpy(net->dev_addr, temp_addr, ETHER_ADDR_LEN);
+
+	wdev = kzalloc(sizeof(*wdev), GFP_KERNEL);
+	if (unlikely(!wdev)) {
+		WL_ERR(("Could not allocate wireless device\n"));
+		return -ENOMEM;
+	}
+
+	wdev->wiphy = wl->wdev->wiphy;
+
+	wdev->iftype = wl_mode_to_nl80211_iftype(WL_MODE_BSS);
+
+	net->ieee80211_ptr = wdev;
+
+	SET_NETDEV_DEV(net, wiphy_dev(wdev->wiphy));
+
+	/* Associate p2p0 network interface with new wdev */
+	wdev->netdev = net;
+
+	/* store p2p net ptr for further reference. Note that iflist won't have this
+	 * entry as there corresponding firmware interface is a "Hidden" interface.
+	 */
+	if (wl->p2p_net) {
+		CFGP2P_ERR(("p2p_net defined already.\n"));
+		return -EINVAL;
+	} else {
+		wl->p2p_wdev = wdev;
+		wl->p2p_net = net;
+	}
+
+	ret = register_netdev(net);
+	if (ret) {
+		CFGP2P_ERR((" register_netdevice failed (%d)\n", ret));
+		goto fail;
+	}
+
+	printk("%s: P2P Interface Registered\n", net->name);
+
+	return ret;
+fail:
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31)
+	net->open = NULL;
+#else
+	net->netdev_ops = NULL;
+#endif
+
+	if (net) {
+		unregister_netdev(net);
+		free_netdev(net);
+	}
+
+	return -ENODEV;
+}
+
+s32
+wl_cfgp2p_unregister_ndev(struct wl_priv *wl)
+{
+
+	if (!wl || !wl->p2p_net) {
+		CFGP2P_ERR(("Invalid Ptr\n"));
+		return -EINVAL;
+	}
+
+	unregister_netdev(wl->p2p_net);
+	free_netdev(wl->p2p_net);
+
+	return 0;
+}
+
+static int wl_cfgp2p_start_xmit(struct sk_buff *skb, struct net_device *ndev)
+{
+	CFGP2P_DBG(("(%s) is not used for data operations. Droping the packet. \n", ndev->name));
+	return 0;
+}
+
+static int wl_cfgp2p_do_ioctl(struct net_device *net, struct ifreq *ifr, int cmd)
+{
+	int ret = 0;
+	struct wl_priv *wl = *(struct wl_priv **)netdev_priv(net);
+	struct net_device *ndev = wl_to_prmry_ndev(wl);
+
+	/* There is no ifidx corresponding to p2p0 in our firmware. So we should
+	 * not Handle any IOCTL cmds on p2p0 other than ANDROID PRIVATE CMDs.
+	 * For Android PRIV CMD handling map it to primary I/F
+	 */
+	if (cmd == SIOCDEVPRIVATE+1) {
+		ret = wl_android_priv_cmd(ndev, ifr, cmd);
+
+	} else {
+		CFGP2P_ERR(("%s: IOCTL req 0x%x on p2p0 I/F. Ignoring. \n",
+		__FUNCTION__, cmd));
+		return -1;
+	}
+
+	return ret;
+}
+
+static int wl_cfgp2p_if_open(struct net_device *net)
+{
+	struct wireless_dev *wdev = net->ieee80211_ptr;
+
+	if (!wdev)
+		return -EINVAL;
+
+	/* If suppose F/W download (ifconfig wlan0 up) hasn't been done by now,
+	 * do it here. This will make sure that in concurrent mode, supplicant
+	 * is not dependent on a particular order of interface initialization.
+	 * i.e you may give wpa_supp -iwlan0 -N -ip2p0 or wpa_supp -ip2p0 -N
+	 * -iwlan0.
+	 */
+	wl_cfg80211_do_driver_init(net);
+
+	wdev->wiphy->interface_modes |= (BIT(NL80211_IFTYPE_P2P_CLIENT)
+		| BIT(NL80211_IFTYPE_P2P_GO));
+
+	return 0;
+}
+
+static int wl_cfgp2p_if_stop(struct net_device *net)
+{
+	struct wireless_dev *wdev = net->ieee80211_ptr;
+
+	if (!wdev)
+		return -EINVAL;
+
+	wdev->wiphy->interface_modes = (wdev->wiphy->interface_modes)
+					& (~(BIT(NL80211_IFTYPE_P2P_CLIENT)|
+					BIT(NL80211_IFTYPE_P2P_GO)));
+	return 0;
+}
diff --git a/drivers/net/wireless/bcmdhd/wl_cfgp2p.h b/drivers/net/wireless/bcmdhd/wl_cfgp2p.h
new file mode 100644
index 000000000000..40d9e5463fe0
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/wl_cfgp2p.h
@@ -0,0 +1,292 @@
+/*
+ * Linux cfgp2p driver
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wl_cfgp2p.h,v 1.1.4.1.2.8 2011/02/09 01:37:52 Exp $
+ */
+#ifndef _wl_cfgp2p_h_
+#define _wl_cfgp2p_h_
+#include <proto/802.11.h>
+#include <proto/p2p.h>
+
+struct wl_priv;
+extern u32 wl_dbg_level;
+
+#ifdef WLWFDIE
+typedef struct wifi_p2p_ie wifi_wfd_ie_t;
+#endif /* WLWFDIE */
+
+/* Enumeration of the usages of the BSSCFGs used by the P2P Library.  Do not
+ * confuse this with a bsscfg index.  This value is an index into the
+ * saved_ie[] array of structures which in turn contains a bsscfg index field.
+ */
+typedef enum {
+	P2PAPI_BSSCFG_PRIMARY, /* maps to driver's primary bsscfg */
+	P2PAPI_BSSCFG_DEVICE, /* maps to driver's P2P device discovery bsscfg */
+	P2PAPI_BSSCFG_CONNECTION, /* maps to driver's P2P connection bsscfg */
+	P2PAPI_BSSCFG_MAX
+} p2p_bsscfg_type_t;
+
+#define IE_MAX_LEN 512
+/* Structure to hold all saved P2P and WPS IEs for a BSSCFG */
+struct p2p_saved_ie {
+	u8  p2p_probe_req_ie[IE_MAX_LEN];
+	u8  p2p_probe_res_ie[IE_MAX_LEN];
+	u8  p2p_assoc_req_ie[IE_MAX_LEN];
+	u8  p2p_assoc_res_ie[IE_MAX_LEN];
+	u8  p2p_beacon_ie[IE_MAX_LEN];
+	u32 p2p_probe_req_ie_len;
+	u32 p2p_probe_res_ie_len;
+	u32 p2p_assoc_req_ie_len;
+	u32 p2p_assoc_res_ie_len;
+	u32 p2p_beacon_ie_len;
+};
+
+struct p2p_bss {
+	u32 bssidx;
+	struct net_device *dev;
+	struct p2p_saved_ie saved_ie;
+	void *private_data;
+};
+
+struct p2p_info {
+	bool on;    /* p2p on/off switch */
+	bool scan;
+	bool vif_created;
+	s8 vir_ifname[IFNAMSIZ];
+	unsigned long status;
+	struct ether_addr dev_addr;
+	struct ether_addr int_addr;
+	struct p2p_bss bss_idx[P2PAPI_BSSCFG_MAX];
+	struct timer_list listen_timer;
+	wl_p2p_sched_t noa;
+	wl_p2p_ops_t ops;
+	wlc_ssid_t ssid;
+};
+
+/* dongle status */
+enum wl_cfgp2p_status {
+	WLP2P_STATUS_DISCOVERY_ON = 0,
+	WLP2P_STATUS_SEARCH_ENABLED,
+	WLP2P_STATUS_IF_ADD,
+	WLP2P_STATUS_IF_DEL,
+	WLP2P_STATUS_IF_DELETING,
+	WLP2P_STATUS_IF_CHANGING,
+	WLP2P_STATUS_IF_CHANGED,
+	WLP2P_STATUS_LISTEN_EXPIRED,
+	WLP2P_STATUS_ACTION_TX_COMPLETED,
+	WLP2P_STATUS_ACTION_TX_NOACK,
+	WLP2P_STATUS_SCANNING,
+	WLP2P_STATUS_GO_NEG_PHASE
+};
+
+
+#define wl_to_p2p_bss_ndev(w, type) 	((wl)->p2p->bss_idx[type].dev)
+#define wl_to_p2p_bss_bssidx(w, type) 	((wl)->p2p->bss_idx[type].bssidx)
+#define wl_to_p2p_bss_saved_ie(w, type) 	((wl)->p2p->bss_idx[type].saved_ie)
+#define wl_to_p2p_bss_private(w, type) 	((wl)->p2p->bss_idx[type].private_data)
+#define wl_to_p2p_bss(wl, type) ((wl)->p2p->bss_idx[type])
+#define wl_get_p2p_status(wl, stat) ((!(wl)->p2p_supported) ? 0:test_bit(WLP2P_STATUS_ ## stat, \
+									&(wl)->p2p->status))
+#define wl_set_p2p_status(wl, stat) ((!(wl)->p2p_supported) ? 0:set_bit(WLP2P_STATUS_ ## stat, \
+									&(wl)->p2p->status))
+#define wl_clr_p2p_status(wl, stat) ((!(wl)->p2p_supported) ? 0:clear_bit(WLP2P_STATUS_ ## stat, \
+									&(wl)->p2p->status))
+#define wl_chg_p2p_status(wl, stat) ((!(wl)->p2p_supported) ? 0:change_bit(WLP2P_STATUS_ ## stat, \
+									&(wl)->p2p->status))
+#define p2p_on(wl) ((wl)->p2p->on)
+#define p2p_scan(wl) ((wl)->p2p->scan)
+#define p2p_is_on(wl) ((wl)->p2p && (wl)->p2p->on)
+
+/* dword align allocation */
+#define WLC_IOCTL_MAXLEN 8192
+#define MAC2STR(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5]
+#define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x"
+
+#define CFGP2P_ERR(args)									\
+	do {										\
+		if (wl_dbg_level & WL_DBG_ERR) {				\
+			printk(KERN_ERR "CFGP2P-ERROR) %s : ", __func__);	\
+			printk args;						\
+		}									\
+	} while (0)
+#define	CFGP2P_INFO(args)									\
+	do {										\
+		if (wl_dbg_level & WL_DBG_INFO) {				\
+			printk(KERN_ERR "CFGP2P-INFO) %s : ", __func__);	\
+			printk args;						\
+		}									\
+	} while (0)
+#define	CFGP2P_DBG(args)								\
+	do {									\
+		if (wl_dbg_level & WL_DBG_DBG) {			\
+			printk(KERN_ERR "CFGP2P-DEBUG) %s :", __func__);	\
+			printk args;							\
+		}									\
+	} while (0)
+
+extern bool
+wl_cfgp2p_is_pub_action(void *frame, u32 frame_len);
+extern bool
+wl_cfgp2p_is_p2p_action(void *frame, u32 frame_len);
+extern bool
+wl_cfgp2p_is_gas_action(void *frame, u32 frame_len);
+extern void
+wl_cfgp2p_print_actframe(bool tx, void *frame, u32 frame_len);
+extern s32
+wl_cfgp2p_init_priv(struct wl_priv *wl);
+extern void
+wl_cfgp2p_deinit_priv(struct wl_priv *wl);
+extern s32
+wl_cfgp2p_set_firm_p2p(struct wl_priv *wl);
+extern s32
+wl_cfgp2p_set_p2p_mode(struct wl_priv *wl, u8 mode,
+            u32 channel, u16 listen_ms, int bssidx);
+extern s32
+wl_cfgp2p_ifadd(struct wl_priv *wl, struct ether_addr *mac, u8 if_type,
+            chanspec_t chspec);
+extern s32
+wl_cfgp2p_ifdel(struct wl_priv *wl, struct ether_addr *mac);
+extern s32
+wl_cfgp2p_ifchange(struct wl_priv *wl, struct ether_addr *mac, u8 if_type, chanspec_t chspec);
+
+extern s32
+wl_cfgp2p_ifidx(struct wl_priv *wl, struct ether_addr *mac, s32 *index);
+
+extern s32
+wl_cfgp2p_init_discovery(struct wl_priv *wl);
+extern s32
+wl_cfgp2p_enable_discovery(struct wl_priv *wl, struct net_device *dev, const u8 *ie, u32 ie_len);
+extern s32
+wl_cfgp2p_disable_discovery(struct wl_priv *wl);
+extern s32
+wl_cfgp2p_escan(struct wl_priv *wl, struct net_device *dev, u16 active, u32 num_chans,
+	u16 *channels,
+	s32 search_state, u16 action, u32 bssidx);
+
+extern s32
+wl_cfgp2p_act_frm_search(struct wl_priv *wl, struct net_device *ndev,
+	s32 bssidx, s32 channel);
+
+extern wpa_ie_fixed_t *
+wl_cfgp2p_find_wpaie(u8 *parse, u32 len);
+
+extern wpa_ie_fixed_t *
+wl_cfgp2p_find_wpsie(u8 *parse, u32 len);
+
+extern wifi_p2p_ie_t *
+wl_cfgp2p_find_p2pie(u8 *parse, u32 len);
+
+#ifdef WLWFDIE
+extern wifi_wfd_ie_t *
+wl_cfgp2p_find_wfdie(u8 *parse, u32 len);
+#endif /* WLWFDIE */
+
+extern s32
+wl_cfgp2p_set_management_ie(struct wl_priv *wl, struct net_device *ndev, s32 bssidx,
+            s32 pktflag, const u8 *vndr_ie, u32 vndr_ie_len);
+extern s32
+wl_cfgp2p_clear_management_ie(struct wl_priv *wl, s32 bssidx);
+
+extern s32
+wl_cfgp2p_find_idx(struct wl_priv *wl, struct net_device *ndev);
+
+
+extern s32
+wl_cfgp2p_listen_complete(struct wl_priv *wl, struct net_device *ndev,
+            const wl_event_msg_t *e, void *data);
+extern s32
+wl_cfgp2p_discover_listen(struct wl_priv *wl, s32 channel, u32 duration_ms);
+
+extern s32
+wl_cfgp2p_discover_enable_search(struct wl_priv *wl, u8 enable);
+
+extern s32
+wl_cfgp2p_action_tx_complete(struct wl_priv *wl, struct net_device *ndev,
+            const wl_event_msg_t *e, void *data);
+extern s32
+wl_cfgp2p_tx_action_frame(struct wl_priv *wl, struct net_device *dev,
+	wl_af_params_t *af_params, s32 bssidx);
+
+extern void
+wl_cfgp2p_generate_bss_mac(struct ether_addr *primary_addr, struct ether_addr *out_dev_addr,
+            struct ether_addr *out_int_addr);
+
+extern void
+wl_cfg80211_change_ifaddr(u8* buf, struct ether_addr *p2p_int_addr, u8 element_id);
+extern bool
+wl_cfgp2p_bss_isup(struct net_device *ndev, int bsscfg_idx);
+
+extern s32
+wl_cfgp2p_bss(struct wl_priv *wl, struct net_device *ndev, s32 bsscfg_idx, s32 up);
+
+
+extern s32
+wl_cfgp2p_supported(struct wl_priv *wl, struct net_device *ndev);
+
+extern s32
+wl_cfgp2p_down(struct wl_priv *wl);
+
+extern s32
+wl_cfgp2p_set_p2p_noa(struct wl_priv *wl, struct net_device *ndev, char* buf, int len);
+
+extern s32
+wl_cfgp2p_get_p2p_noa(struct wl_priv *wl, struct net_device *ndev, char* buf, int len);
+
+extern s32
+wl_cfgp2p_set_p2p_ps(struct wl_priv *wl, struct net_device *ndev, char* buf, int len);
+
+extern u8 *
+wl_cfgp2p_retreive_p2pattrib(void *buf, u8 element_id);
+
+extern u8 *
+wl_cfgp2p_retreive_p2p_dev_addr(wl_bss_info_t *bi, u32 bi_length);
+
+extern s32
+wl_cfgp2p_register_ndev(struct wl_priv *wl);
+
+extern s32
+wl_cfgp2p_unregister_ndev(struct wl_priv *wl);
+
+/* WiFi Direct */
+#define SOCIAL_CHAN_1 1
+#define SOCIAL_CHAN_2 6
+#define SOCIAL_CHAN_3 11
+#define SOCIAL_CHAN_CNT 3
+#define WL_P2P_WILDCARD_SSID "DIRECT-"
+#define WL_P2P_WILDCARD_SSID_LEN 7
+#define WL_P2P_INTERFACE_PREFIX "p2p"
+#define WL_P2P_TEMP_CHAN "11"
+
+/* If the provision discovery is for JOIN operations, then we need not do an internal scan to find GO */
+#define IS_PROV_DISC_WITHOUT_GROUP_ID(p2p_ie, len) (wl_cfgp2p_retreive_p2pattrib(p2p_ie, P2P_SEID_GROUP_ID) == NULL )
+
+#define IS_GAS_REQ(frame, len) (wl_cfgp2p_is_gas_action(frame, len) && \
+					((frame->action == P2PSD_ACTION_ID_GAS_IREQ) || \
+					(frame->action == P2PSD_ACTION_ID_GAS_CREQ)))
+#define IS_P2P_PUB_ACT_REQ(frame, p2p_ie, len) (wl_cfgp2p_is_pub_action(frame, len) && \
+						((frame->subtype == P2P_PAF_GON_REQ) || \
+						(frame->subtype == P2P_PAF_INVITE_REQ) || \
+						((frame->subtype == P2P_PAF_PROVDIS_REQ) && IS_PROV_DISC_WITHOUT_GROUP_ID(p2p_ie, len))))
+#define IS_P2P_SOCIAL(ch) ((ch == SOCIAL_CHAN_1) || (ch == SOCIAL_CHAN_2) || (ch == SOCIAL_CHAN_3))
+#define IS_P2P_SSID(ssid) (memcmp(ssid, WL_P2P_WILDCARD_SSID, WL_P2P_WILDCARD_SSID_LEN) == 0)
+#endif				/* _wl_cfgp2p_h_ */
diff --git a/drivers/net/wireless/bcmdhd/wl_dbg.h b/drivers/net/wireless/bcmdhd/wl_dbg.h
new file mode 100644
index 000000000000..0b99557cbe8d
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/wl_dbg.h
@@ -0,0 +1,49 @@
+/*
+ * Minimal debug/trace/assert driver definitions for
+ * Broadcom 802.11 Networking Adapter.
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wl_dbg.h,v 1.115.6.3 2010-12-15 21:42:23 Exp $
+ */
+
+
+
+#ifndef _wl_dbg_h_
+#define _wl_dbg_h_
+
+
+extern uint32 wl_msg_level;
+extern uint32 wl_msg_level2;
+
+#define WL_PRINT(args)      printf args
+
+
+
+#define WL_NONE(args)
+
+#define WL_ERROR(args)
+#define WL_TRACE(args)
+
+
+extern uint32 wl_msg_level;
+extern uint32 wl_msg_level2;
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/wl_iw.c b/drivers/net/wireless/bcmdhd/wl_iw.c
new file mode 100644
index 000000000000..6bb07650b27e
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/wl_iw.c
@@ -0,0 +1,8895 @@
+/*
+ * Linux Wireless Extensions support
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wl_iw.c,v 1.132.2.18 2011-02-05 01:44:47 $
+ */
+
+#include <wlioctl.h>
+
+#include <typedefs.h>
+#include <linuxver.h>
+#include <osl.h>
+
+#include <bcmutils.h>
+#include <bcmendian.h>
+#include <proto/ethernet.h>
+
+#include <linux/if_arp.h>
+#include <asm/uaccess.h>
+
+#include <dngl_stats.h>
+#include <dhd.h>
+#include <dhdioctl.h>
+
+typedef void wlc_info_t;
+typedef void wl_info_t;
+typedef const struct si_pub  si_t;
+#include <wlioctl.h>
+
+#include <proto/ethernet.h>
+#include <dngl_stats.h>
+#include <dhd.h>
+#define WL_ERROR(x) printf x
+#define WL_TRACE(x)
+#define WL_ASSOC(x)
+#define WL_INFORM(x)
+#define WL_WSEC(x)
+#define WL_SCAN(x)
+
+
+#ifdef PNO_SET_DEBUG
+#define WL_PNO(x)	printf x
+#else
+#define WL_PNO(x)
+#endif
+
+
+#define JF2MS ((((jiffies / HZ) * 1000) + ((jiffies % HZ) * 1000) / HZ))
+
+#ifdef COEX_DBG       
+#define WL_TRACE_COEX(x) printf("TS:%lu ", JF2MS); \
+							printf x
+#else
+#define WL_TRACE_COEX(x)
+#endif
+
+#ifdef SCAN_DBG        
+#define WL_TRACE_SCAN(x) printf("TS:%lu ", JF2MS); \
+							printf x
+#else
+#define WL_TRACE_SCAN(x)
+#endif
+
+
+#include <wl_iw.h>
+
+
+
+
+#define IW_WSEC_ENABLED(wsec)	((wsec) & (WEP_ENABLED | TKIP_ENABLED | AES_ENABLED))
+
+#include <linux/rtnetlink.h>
+
+#define WL_IW_USE_ISCAN  1
+#define ENABLE_ACTIVE_PASSIVE_SCAN_SUPPRESS  1
+
+#ifdef OEM_CHROMIUMOS
+bool g_set_essid_before_scan = TRUE;
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25)) && 1
+	struct mutex  g_wl_ss_scan_lock; 
+#endif 
+
+#if defined(SOFTAP)
+#define WL_SOFTAP(x)
+static struct net_device *priv_dev;
+extern bool ap_cfg_running;
+extern bool ap_fw_loaded;
+struct net_device *ap_net_dev = NULL;
+tsk_ctl_t ap_eth_ctl;
+static int wl_iw_set_ap_security(struct net_device *dev, struct ap_profile *ap);
+static int wl_iw_softap_deassoc_stations(struct net_device *dev, u8 *mac);
+#endif 
+
+
+#define WL_IW_IOCTL_CALL(func_call) \
+	do {				\
+		func_call;		\
+	} while (0)
+
+#define RETURN_IF_EXTRA_NULL(extra) \
+	if (!extra) { \
+		WL_ERROR(("%s: error : extra is null pointer\n", __FUNCTION__)); \
+		return -EINVAL; \
+	}
+
+static int		g_onoff = G_WLAN_SET_ON;
+wl_iw_extra_params_t	g_wl_iw_params;
+
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 25)) && 1
+
+static struct mutex	wl_cache_lock;
+static struct mutex	wl_softap_lock;
+
+#define DHD_OS_MUTEX_INIT(a) mutex_init(a)
+#define DHD_OS_MUTEX_LOCK(a) mutex_lock(a)
+#define DHD_OS_MUTEX_UNLOCK(a) mutex_unlock(a)
+
+#else
+
+#define DHD_OS_MUTEX_INIT(a)
+#define DHD_OS_MUTEX_LOCK(a)
+#define DHD_OS_MUTEX_UNLOCK(a)
+
+#endif 
+
+#include <bcmsdbus.h>
+extern void dhd_customer_gpio_wlan_ctrl(int onoff);
+extern uint dhd_dev_reset(struct net_device *dev, uint8 flag);
+extern int dhd_dev_init_ioctl(struct net_device *dev);
+
+uint wl_msg_level = WL_ERROR_VAL;
+
+#define MAX_WLIW_IOCTL_LEN 1024
+
+
+#define htod32(i) i
+#define htod16(i) i
+#define dtoh32(i) i
+#define dtoh16(i) i
+#define htodchanspec(i) i
+#define dtohchanspec(i) i
+
+extern struct iw_statistics *dhd_get_wireless_stats(struct net_device *dev);
+extern int dhd_wait_pend8021x(struct net_device *dev);
+
+#if WIRELESS_EXT < 19
+#define IW_IOCTL_IDX(cmd)	((cmd) - SIOCIWFIRST)
+#define IW_EVENT_IDX(cmd)	((cmd) - IWEVFIRST)
+#endif 
+
+static void *g_scan = NULL;
+static volatile uint g_scan_specified_ssid;	
+static wlc_ssid_t g_specific_ssid;		
+
+static wlc_ssid_t g_ssid;
+
+#ifdef CONFIG_WPS2
+static char *g_wps_probe_req_ie;
+static int g_wps_probe_req_ie_len;
+#endif
+
+bool btcoex_is_sco_active(struct net_device *dev);  
+static wl_iw_ss_cache_ctrl_t g_ss_cache_ctrl;	
+#if defined(CONFIG_FIRST_SCAN)
+static volatile uint g_first_broadcast_scan;	
+static volatile uint g_first_counter_scans;
+#define MAX_ALLOWED_BLOCK_SCAN_FROM_FIRST_SCAN 3
+#endif 
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 0))
+#define DAEMONIZE(a) daemonize(a); \
+	allow_signal(SIGKILL); \
+	allow_signal(SIGTERM);
+#else 
+#define RAISE_RX_SOFTIRQ() \
+	cpu_raise_softirq(smp_processor_id(), NET_RX_SOFTIRQ)
+#define DAEMONIZE(a) daemonize(); \
+	do { if (a) \
+		strncpy(current->comm, a, MIN(sizeof(current->comm), (strlen(a) + 1))); \
+	} while (0);
+#endif 
+
+#if defined(WL_IW_USE_ISCAN)
+#if  !defined(CSCAN)
+static void wl_iw_free_ss_cache(void);
+static int   wl_iw_run_ss_cache_timer(int kick_off);
+#endif 
+#if defined(CONFIG_FIRST_SCAN)
+int  wl_iw_iscan_set_scan_broadcast_prep(struct net_device *dev, uint flag);
+#endif 
+static int dev_wlc_bufvar_set(struct net_device *dev, char *name, char *buf, int len);
+#define ISCAN_STATE_IDLE   0
+#define ISCAN_STATE_SCANING 1
+
+
+#define WLC_IW_ISCAN_MAXLEN   2048
+typedef struct iscan_buf {
+	struct iscan_buf * next;
+	char   iscan_buf[WLC_IW_ISCAN_MAXLEN];
+} iscan_buf_t;
+
+typedef struct iscan_info {
+	struct net_device *dev;
+	struct timer_list timer;
+	uint32 timer_ms;
+	uint32 timer_on;
+	int    iscan_state;
+	iscan_buf_t * list_hdr;
+	iscan_buf_t * list_cur;
+
+	
+	tsk_ctl_t tsk_ctl;
+
+	uint32 scan_flag;	
+#if defined CSCAN
+	char ioctlbuf[WLC_IOCTL_MEDLEN];
+#else
+	char ioctlbuf[WLC_IOCTL_SMLEN];
+#endif 
+	
+	wl_iscan_params_t *iscan_ex_params_p;
+	int iscan_ex_param_size;
+} iscan_info_t;
+
+
+
+#define  COEX_DHCP 1	
+#ifdef COEX_DHCP
+
+#define BT_DHCP_eSCO_FIX 
+#define BT_DHCP_USE_FLAGS  
+#define BT_DHCP_OPPORTUNITY_WINDOW_TIME	 2500 
+#define BT_DHCP_FLAG_FORCE_TIME 5500 
+
+
+
+static int wl_iw_set_btcoex_dhcp(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+);
+
+static void wl_iw_bt_flag_set(struct net_device *dev, bool set);
+static void wl_iw_bt_release(void);
+
+typedef enum bt_coex_status {
+	BT_DHCP_IDLE = 0,
+	BT_DHCP_START,
+	BT_DHCP_OPPORTUNITY_WINDOW,
+	BT_DHCP_FLAG_FORCE_TIMEOUT
+} coex_status_t;
+
+
+typedef struct bt_info {
+	struct net_device *dev;
+	struct timer_list timer;
+	uint32 timer_ms;
+	uint32 timer_on;
+	uint32 ts_dhcp_start; 
+	uint32 ts_dhcp_ok;    
+	bool	dhcp_done; 
+	int	bt_state;
+
+	
+	tsk_ctl_t tsk_ctl;
+
+} bt_info_t;
+
+bt_info_t *g_bt = NULL;
+static void wl_iw_bt_timerfunc(ulong data);
+#endif 
+iscan_info_t *g_iscan = NULL;
+void dhd_print_buf(void *pbuf, int len, int bytes_per_line);
+static void wl_iw_timerfunc(ulong data);
+static void wl_iw_set_event_mask(struct net_device *dev);
+static int
+wl_iw_iscan(iscan_info_t *iscan, wlc_ssid_t *ssid, uint16 action);
+#endif 
+
+static int
+wl_iw_set_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+);
+
+#ifndef CSCAN
+static int
+wl_iw_get_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+);
+
+static uint
+wl_iw_get_scan_prep(
+	wl_scan_results_t *list,
+	struct iw_request_info *info,
+	char *extra,
+	short max_size
+);
+#endif 
+
+static void
+swap_key_from_BE(
+	wl_wsec_key_t *key
+)
+{
+	key->index = htod32(key->index);
+	key->len = htod32(key->len);
+	key->algo = htod32(key->algo);
+	key->flags = htod32(key->flags);
+	key->rxiv.hi = htod32(key->rxiv.hi);
+	key->rxiv.lo = htod16(key->rxiv.lo);
+	key->iv_initialized = htod32(key->iv_initialized);
+}
+
+static void
+swap_key_to_BE(
+	wl_wsec_key_t *key
+)
+{
+	key->index = dtoh32(key->index);
+	key->len = dtoh32(key->len);
+	key->algo = dtoh32(key->algo);
+	key->flags = dtoh32(key->flags);
+	key->rxiv.hi = dtoh32(key->rxiv.hi);
+	key->rxiv.lo = dtoh16(key->rxiv.lo);
+	key->iv_initialized = dtoh32(key->iv_initialized);
+}
+
+static int
+dev_wlc_ioctl(
+	struct net_device *dev,
+	int cmd,
+	void *arg,
+	int len
+)
+{
+	struct ifreq ifr;
+	wl_ioctl_t ioc;
+	mm_segment_t fs;
+	int ret = -EINVAL;
+
+	if (!dev) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return ret;
+	}
+
+	net_os_wake_lock(dev);
+
+	WL_INFORM(("%s, PID:%x: send Local IOCTL -> dhd: cmd:0x%x, buf:%p, len:%d ,\n",
+		__FUNCTION__, current->pid, cmd, arg, len));
+
+	if (g_onoff == G_WLAN_SET_ON) {
+		memset(&ioc, 0, sizeof(ioc));
+		ioc.cmd = cmd;
+		ioc.buf = arg;
+		ioc.len = len;
+
+		strcpy(ifr.ifr_name, dev->name);
+		ifr.ifr_data = (caddr_t) &ioc;
+
+		
+		ret = dev_open(dev);
+		if (ret) {
+			WL_ERROR(("%s: Error dev_open: %d\n", __func__, ret));
+			net_os_wake_unlock(dev);
+			return ret;
+		}
+
+		fs = get_fs();
+		set_fs(get_ds());
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 31)
+		ret = dev->do_ioctl(dev, &ifr, SIOCDEVPRIVATE);
+#else
+		ret = dev->netdev_ops->ndo_do_ioctl(dev, &ifr, SIOCDEVPRIVATE);
+#endif 
+		set_fs(fs);
+	}
+	else {
+		WL_TRACE(("%s: call after driver stop : ignored\n", __FUNCTION__));
+	}
+
+	net_os_wake_unlock(dev);
+
+	return ret;
+}
+
+
+static int
+dev_wlc_intvar_get_reg(
+	struct net_device *dev,
+	char *name,
+	uint  reg,
+	int *retval)
+{
+	union {
+		char buf[WLC_IOCTL_SMLEN];
+		int val;
+	} var;
+	int error;
+
+	uint len;
+	len = bcm_mkiovar(name, (char *)(&reg), sizeof(reg), (char *)(&var), sizeof(var.buf));
+	ASSERT(len);
+	error = dev_wlc_ioctl(dev, WLC_GET_VAR, (void *)&var, len);
+
+	*retval = dtoh32(var.val);
+	return (error);
+}
+
+
+static int
+dev_wlc_intvar_set_reg(
+	struct net_device *dev,
+	char *name,
+	char *addr,
+	char * val)
+{
+	char reg_addr[8];
+
+	memset(reg_addr, 0, sizeof(reg_addr));
+	memcpy((char *)&reg_addr[0], (char *)addr, 4);
+	memcpy((char *)&reg_addr[4], (char *)val, 4);
+
+	return (dev_wlc_bufvar_set(dev, name,  (char *)&reg_addr[0], sizeof(reg_addr)));
+}
+
+
+
+
+static int
+dev_wlc_intvar_set(
+	struct net_device *dev,
+	char *name,
+	int val)
+{
+	char buf[WLC_IOCTL_SMLEN];
+	uint len;
+
+	val = htod32(val);
+	len = bcm_mkiovar(name, (char *)(&val), sizeof(val), buf, sizeof(buf));
+	ASSERT(len);
+
+	return (dev_wlc_ioctl(dev, WLC_SET_VAR, buf, len));
+}
+
+#if defined(WL_IW_USE_ISCAN)
+static int
+dev_iw_iovar_setbuf(
+	struct net_device *dev,
+	char *iovar,
+	void *param,
+	int paramlen,
+	void *bufptr,
+	int buflen)
+{
+	int iolen;
+
+	iolen = bcm_mkiovar(iovar, param, paramlen, bufptr, buflen);
+	ASSERT(iolen);
+
+	if (iolen == 0)
+		return 0;
+
+	return (dev_wlc_ioctl(dev, WLC_SET_VAR, bufptr, iolen));
+}
+
+static int
+dev_iw_iovar_getbuf(
+	struct net_device *dev,
+	char *iovar,
+	void *param,
+	int paramlen,
+	void *bufptr,
+	int buflen)
+{
+	int iolen;
+
+	iolen = bcm_mkiovar(iovar, param, paramlen, bufptr, buflen);
+	ASSERT(iolen);
+
+	return (dev_wlc_ioctl(dev, WLC_GET_VAR, bufptr, buflen));
+}
+#endif 
+
+
+#if WIRELESS_EXT > 17
+static int
+dev_wlc_bufvar_set(
+	struct net_device *dev,
+	char *name,
+	char *buf, int len)
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31)
+	char ioctlbuf[MAX_WLIW_IOCTL_LEN];
+#else
+	static char ioctlbuf[MAX_WLIW_IOCTL_LEN];
+#endif 
+	uint buflen;
+
+	buflen = bcm_mkiovar(name, buf, len, ioctlbuf, sizeof(ioctlbuf));
+	ASSERT(buflen);
+
+	return (dev_wlc_ioctl(dev, WLC_SET_VAR, ioctlbuf, buflen));
+}
+#endif 
+
+
+static int
+dev_wlc_bufvar_get(
+	struct net_device *dev,
+	char *name,
+	char *buf, int buflen)
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 31)
+	char ioctlbuf[MAX_WLIW_IOCTL_LEN];
+#else
+	static char ioctlbuf[MAX_WLIW_IOCTL_LEN];
+#endif 
+	int error;
+	uint len;
+
+	len = bcm_mkiovar(name, NULL, 0, ioctlbuf, sizeof(ioctlbuf));
+	ASSERT(len);
+	error = dev_wlc_ioctl(dev, WLC_GET_VAR, (void *)ioctlbuf, MAX_WLIW_IOCTL_LEN);
+	if (!error)
+		bcopy(ioctlbuf, buf, buflen);
+
+	return (error);
+}
+
+
+
+static int
+dev_wlc_intvar_get(
+	struct net_device *dev,
+	char *name,
+	int *retval)
+{
+	union {
+		char buf[WLC_IOCTL_SMLEN];
+		int val;
+	} var;
+	int error;
+
+	uint len;
+	uint data_null;
+
+	len = bcm_mkiovar(name, (char *)(&data_null), 0, (char *)(&var), sizeof(var.buf));
+	ASSERT(len);
+	error = dev_wlc_ioctl(dev, WLC_GET_VAR, (void *)&var, len);
+
+	*retval = dtoh32(var.val);
+
+	return (error);
+}
+
+
+#if WIRELESS_EXT > 12
+static int
+wl_iw_set_active_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int as = 0;
+	int error = 0;
+	char *p = extra;
+
+#if defined(WL_IW_USE_ISCAN)
+	if (g_iscan->iscan_state == ISCAN_STATE_IDLE)
+#endif 
+		error = dev_wlc_ioctl(dev, WLC_SET_PASSIVE_SCAN, &as, sizeof(as));
+#if defined(WL_IW_USE_ISCAN)
+	else
+		g_iscan->scan_flag = as;
+#endif 
+	p += snprintf(p, MAX_WX_STRING, "OK");
+
+	wrqu->data.length = p - extra + 1;
+	return error;
+}
+
+static int
+wl_iw_set_passive_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int ps = 1;
+	int error = 0;
+	char *p = extra;
+
+#if defined(WL_IW_USE_ISCAN)
+	if (g_iscan->iscan_state == ISCAN_STATE_IDLE) {
+#endif 
+
+		 
+		if (g_scan_specified_ssid == 0) {
+			error = dev_wlc_ioctl(dev, WLC_SET_PASSIVE_SCAN, &ps, sizeof(ps));
+		}
+#if defined(WL_IW_USE_ISCAN)
+	}
+	else
+		g_iscan->scan_flag = ps;
+#endif 
+
+	p += snprintf(p, MAX_WX_STRING, "OK");
+
+	wrqu->data.length = p - extra + 1;
+	return error;
+}
+
+
+static int
+wl_iw_set_txpower(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = 0;
+	char *p = extra;
+	int txpower = -1;
+
+	txpower = bcm_atoi(extra + strlen(TXPOWER_SET_CMD) + 1);
+	if ((txpower >= 0) && (txpower <= 127))
+	{
+		txpower |= WL_TXPWR_OVERRIDE;
+		txpower = htod32(txpower);
+
+		error = dev_wlc_intvar_set(dev, "qtxpower", txpower);
+		p += snprintf(p, MAX_WX_STRING, "OK");
+		WL_TRACE(("%s: set TXpower 0x%X is OK\n", __FUNCTION__, txpower));
+	} else {
+		WL_ERROR(("%s: set tx power failed\n", __FUNCTION__));
+		p += snprintf(p, MAX_WX_STRING, "FAIL");
+	}
+
+	wrqu->data.length = p - extra + 1;
+	return error;
+}
+
+static int
+wl_iw_get_macaddr(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error;
+	char buf[128];
+	struct ether_addr *id;
+	char *p = extra;
+
+	
+	strcpy(buf, "cur_etheraddr");
+	error = dev_wlc_ioctl(dev, WLC_GET_VAR, buf, sizeof(buf));
+	id = (struct ether_addr *) buf;
+	p += snprintf(p, MAX_WX_STRING, "Macaddr = %02X:%02X:%02X:%02X:%02X:%02X\n",
+		id->octet[0], id->octet[1], id->octet[2],
+		id->octet[3], id->octet[4], id->octet[5]);
+	wrqu->data.length = p - extra + 1;
+
+	return error;
+}
+
+
+
+static int
+wl_iw_set_country(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	char country_code[WLC_CNTRY_BUF_SZ];
+	int error = 0;
+	char *p = extra;
+	int country_offset;
+	int country_code_size;
+	wl_country_t cspec = {{0}, 0, {0}};
+	char smbuf[WLC_IOCTL_SMLEN];
+	scb_val_t scbval;
+
+	cspec.rev = -1;
+	memset(country_code, 0, sizeof(country_code));
+	memset(smbuf, 0, sizeof(smbuf));
+
+	
+	country_offset = strcspn(extra, " ");
+	country_code_size = strlen(extra) - country_offset;
+
+	
+	if (country_offset != 0) {
+		strncpy(country_code, extra + country_offset +1,
+			MIN(country_code_size, sizeof(country_code)));
+
+		
+		bzero(&scbval, sizeof(scb_val_t));
+		if ((error = dev_wlc_ioctl(dev, WLC_DISASSOC, &scbval, sizeof(scb_val_t)))) {
+			WL_ERROR(("%s: set country failed due to Disassoc error\n", __FUNCTION__));
+			goto exit_failed;
+		}
+
+		memcpy(cspec.country_abbrev, country_code, WLC_CNTRY_BUF_SZ);
+		memcpy(cspec.ccode, country_code, WLC_CNTRY_BUF_SZ);
+
+		get_customized_country_code((char *)&cspec.country_abbrev, &cspec);
+
+		
+		if ((error = dev_iw_iovar_setbuf(dev, "country", &cspec,
+			sizeof(cspec), smbuf, sizeof(smbuf))) >= 0) {
+			p += snprintf(p, MAX_WX_STRING, "OK");
+			WL_ERROR(("%s: set country for %s as %s rev %d is OK\n",
+				__FUNCTION__, country_code, cspec.ccode, cspec.rev));
+			dhd_bus_country_set(dev, &cspec);
+			goto exit;
+		}
+	}
+
+	WL_ERROR(("%s: set country for %s as %s rev %d failed\n",
+		__FUNCTION__, country_code, cspec.ccode, cspec.rev));
+
+exit_failed:
+	p += snprintf(p, MAX_WX_STRING, "FAIL");
+
+exit:
+	wrqu->data.length = p - extra + 1;
+	return error;
+}
+
+static int
+wl_iw_set_power_mode(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = 0;
+	char *p = extra;
+	static int  pm = PM_FAST;
+	int  pm_local = PM_OFF;
+	char powermode_val = 0;
+
+	WL_TRACE_COEX(("%s: DHCP session cmd:%s\n", __FUNCTION__, extra));
+
+	strncpy((char *)&powermode_val, extra + strlen("POWERMODE") +1, 1);
+
+	if (strnicmp((char *)&powermode_val, "1", strlen("1")) == 0) {
+
+		WL_TRACE(("%s: DHCP session starts\n", __FUNCTION__));
+
+		dev_wlc_ioctl(dev, WLC_GET_PM, &pm, sizeof(pm));
+		dev_wlc_ioctl(dev, WLC_SET_PM, &pm_local, sizeof(pm_local));
+
+		
+		net_os_set_packet_filter(dev, 0);
+
+#ifdef COEX_DHCP
+		g_bt->ts_dhcp_start = JF2MS;
+		g_bt->dhcp_done = FALSE;
+		WL_TRACE_COEX(("%s: DHCP start, pm:%d changed to pm:%d\n",
+			__FUNCTION__, pm, pm_local));
+
+#endif 
+	} else if (strnicmp((char *)&powermode_val, "0", strlen("0")) == 0) {
+		
+
+		dev_wlc_ioctl(dev, WLC_SET_PM, &pm, sizeof(pm));
+
+		
+		net_os_set_packet_filter(dev, 1);
+
+#ifdef COEX_DHCP
+		g_bt->dhcp_done = TRUE;
+		g_bt->ts_dhcp_ok = JF2MS;
+		WL_TRACE_COEX(("%s: DHCP done for:%d ms, restored pm:%d\n",
+			__FUNCTION__, (g_bt->ts_dhcp_ok - g_bt->ts_dhcp_start), pm));
+#endif 
+
+	} else {
+		WL_ERROR(("%s Unkwown yet power setting, ignored\n",
+			__FUNCTION__));
+	}
+
+	p += snprintf(p, MAX_WX_STRING, "OK");
+
+	wrqu->data.length = p - extra + 1;
+
+	return error;
+}
+
+
+bool btcoex_is_sco_active(struct net_device *dev)
+{
+	int ioc_res = 0;
+	bool res = FALSE;
+	int sco_id_cnt = 0;
+	int param27;
+	int i;
+
+	for (i = 0; i < 12; i++) {
+
+		ioc_res = dev_wlc_intvar_get_reg(dev, "btc_params", 27, &param27);
+
+		WL_TRACE_COEX(("%s, sample[%d], btc params: 27:%x\n",
+			__FUNCTION__, i, param27));
+
+		if (ioc_res < 0) {
+			WL_ERROR(("%s ioc read btc params error\n", __FUNCTION__));
+			break;
+		}
+
+		if ((param27 & 0x6) == 2) { 
+			sco_id_cnt++;
+		}
+
+		if (sco_id_cnt > 2) {
+			WL_TRACE_COEX(("%s, sco/esco detected, pkt id_cnt:%d  samples:%d\n",
+				__FUNCTION__, sco_id_cnt, i));
+			res = TRUE;
+			break;
+		}
+
+		msleep(5);
+	}
+
+	return res;
+}
+
+#if defined(BT_DHCP_eSCO_FIX)
+
+static int set_btc_esco_params(struct net_device *dev, bool trump_sco)
+{
+	static bool saved_status = FALSE;
+
+	char buf_reg50va_dhcp_on[8] = { 50, 00, 00, 00, 0x22, 0x80, 0x00, 0x00 };
+	char buf_reg51va_dhcp_on[8] = { 51, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
+	char buf_reg64va_dhcp_on[8] = { 64, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
+	char buf_reg65va_dhcp_on[8] = { 65, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
+	char buf_reg71va_dhcp_on[8] = { 71, 00, 00, 00, 0x00, 0x00, 0x00, 0x00 };
+
+	uint32 regaddr;
+	static uint32 saved_reg50;
+	static uint32 saved_reg51;
+	static uint32 saved_reg64;
+	static uint32 saved_reg65;
+	static uint32 saved_reg71;
+
+	if (trump_sco) { 
+
+		
+		WL_TRACE_COEX(("Do new SCO/eSCO coex algo {save & override} \n"));
+
+
+		if  ((!dev_wlc_intvar_get_reg(dev, "btc_params", 50,  &saved_reg50)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 51,  &saved_reg51)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 64,  &saved_reg64)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 65,  &saved_reg65)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 71,  &saved_reg71))) {
+
+			saved_status = TRUE;
+			WL_TRACE_COEX(("%s saved bt_params[50,51,64,65,71]:"
+				" 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+				__FUNCTION__, saved_reg50, saved_reg51,
+				saved_reg64, saved_reg65, saved_reg71));
+
+		} else {
+			WL_ERROR((":%s: save btc_params failed\n",
+				__FUNCTION__));
+			saved_status = FALSE;
+			return -1;
+		}
+
+		WL_TRACE_COEX(("override with [50,51,64,65,71]:"
+			" 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+			*(u32 *)(buf_reg50va_dhcp_on+4),
+			*(u32 *)(buf_reg51va_dhcp_on+4),
+			*(u32 *)(buf_reg64va_dhcp_on+4),
+			*(u32 *)(buf_reg65va_dhcp_on+4),
+			*(u32 *)(buf_reg71va_dhcp_on+4)));
+
+		dev_wlc_bufvar_set(dev, "btc_params", (char *)&buf_reg50va_dhcp_on[0], 8);
+		dev_wlc_bufvar_set(dev, "btc_params", (char *)&buf_reg51va_dhcp_on[0], 8);
+		dev_wlc_bufvar_set(dev, "btc_params", (char *)&buf_reg64va_dhcp_on[0], 8);
+		dev_wlc_bufvar_set(dev, "btc_params", (char *)&buf_reg65va_dhcp_on[0], 8);
+		dev_wlc_bufvar_set(dev, "btc_params", (char *)&buf_reg71va_dhcp_on[0], 8);
+
+		saved_status = TRUE;
+
+	} else if (saved_status) {
+		
+		WL_TRACE_COEX(("Do new SCO/eSCO coex algo {save & override} \n"));
+
+		regaddr = 50;
+		dev_wlc_intvar_set_reg(dev, "btc_params",
+			(char *)&regaddr, (char *)&saved_reg50);
+		regaddr = 51;
+		dev_wlc_intvar_set_reg(dev, "btc_params",
+			(char *)&regaddr, (char *)&saved_reg51);
+		regaddr = 64;
+		dev_wlc_intvar_set_reg(dev, "btc_params",
+			(char *)&regaddr, (char *)&saved_reg64);
+		regaddr = 65;
+		dev_wlc_intvar_set_reg(dev, "btc_params",
+			(char *)&regaddr, (char *)&saved_reg65);
+		regaddr = 71;
+		dev_wlc_intvar_set_reg(dev, "btc_params",
+			(char *)&regaddr, (char *)&saved_reg71);
+
+		WL_TRACE_COEX(("restore bt_params[50,51,64,65,71]: 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+			saved_reg50, saved_reg51, saved_reg64,
+			saved_reg65, saved_reg71));
+
+		saved_status = FALSE;
+	} else {
+		WL_ERROR((":%s att to restore not saved BTCOEX params\n",
+			__FUNCTION__));
+		return -1;
+	}
+	return 0;
+}
+#endif 
+
+
+static int
+wl_iw_get_power_mode(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = 0;
+	int pm_local;
+	char *p = extra;
+
+	error = dev_wlc_ioctl(dev, WLC_GET_PM, &pm_local, sizeof(pm_local));
+	if (!error) {
+		WL_TRACE(("%s: Powermode = %d\n", __func__, pm_local));
+		if (pm_local == PM_OFF)
+			pm_local = 1; 
+		else
+			pm_local = 0; 
+		p += snprintf(p, MAX_WX_STRING, "powermode = %d", pm_local);
+	}
+	else {
+		WL_TRACE(("%s: Error = %d\n", __func__, error));
+		p += snprintf(p, MAX_WX_STRING, "FAIL");
+	}
+	wrqu->data.length = p - extra + 1;
+	return error;
+}
+
+static int
+wl_iw_set_btcoex_dhcp(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = 0;
+	char *p = extra;
+	char powermode_val = 0;
+	char buf_reg66va_dhcp_on[8] = { 66, 00, 00, 00, 0x10, 0x27, 0x00, 0x00 };
+	char buf_reg41va_dhcp_on[8] = { 41, 00, 00, 00, 0x33, 0x00, 0x00, 0x00 };
+	char buf_reg68va_dhcp_on[8] = { 68, 00, 00, 00, 0x90, 0x01, 0x00, 0x00 };
+
+	uint32 regaddr;
+	static uint32 saved_reg66;
+	static uint32 saved_reg41;
+	static uint32 saved_reg68;
+	static bool saved_status = FALSE;
+
+#ifdef COEX_DHCP
+	char buf_flag7_default[8] =   { 7, 00, 00, 00, 0x0, 0x00, 0x00, 0x00};
+#endif 
+
+	
+	strncpy((char *)&powermode_val, extra + strlen("BTCOEXMODE") +1, 1);
+
+	if (strnicmp((char *)&powermode_val, "1", strlen("1")) == 0) {
+
+		WL_TRACE(("%s: DHCP session starts\n", __FUNCTION__));
+
+		
+		if ((saved_status == FALSE) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 66,  &saved_reg66)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 41,  &saved_reg41)) &&
+			(!dev_wlc_intvar_get_reg(dev, "btc_params", 68,  &saved_reg68)))   {
+				saved_status = TRUE;
+				WL_TRACE(("Saved 0x%x 0x%x 0x%x\n",
+					saved_reg66, saved_reg41, saved_reg68));
+
+				
+
+				
+#ifdef COEX_DHCP
+				
+				if (btcoex_is_sco_active(dev)) {
+					
+					dev_wlc_bufvar_set(dev, "btc_params",
+						(char *)&buf_reg66va_dhcp_on[0],
+						sizeof(buf_reg66va_dhcp_on));
+					
+					dev_wlc_bufvar_set(dev, "btc_params",
+						(char *)&buf_reg41va_dhcp_on[0],
+						sizeof(buf_reg41va_dhcp_on));
+					
+					dev_wlc_bufvar_set(dev, "btc_params",
+						(char *)&buf_reg68va_dhcp_on[0],
+						sizeof(buf_reg68va_dhcp_on));
+					saved_status = TRUE;
+
+					g_bt->bt_state = BT_DHCP_START;
+					g_bt->timer_on = 1;
+					mod_timer(&g_bt->timer, g_bt->timer.expires);
+					WL_TRACE_COEX(("%s enable BT DHCP Timer\n",
+					__FUNCTION__));
+				}
+#endif 
+		}
+		else if (saved_status == TRUE) {
+			WL_ERROR(("%s was called w/o DHCP OFF. Continue\n", __FUNCTION__));
+		}
+	}
+	else if (strnicmp((char *)&powermode_val, "2", strlen("2")) == 0) {
+
+
+		
+
+#ifdef COEX_DHCP
+		
+		WL_TRACE(("%s disable BT DHCP Timer\n", __FUNCTION__));
+		if (g_bt->timer_on) {
+			g_bt->timer_on = 0;
+			del_timer_sync(&g_bt->timer);
+
+			if (g_bt->bt_state != BT_DHCP_IDLE) {
+			
+				WL_TRACE_COEX(("%s bt->bt_state:%d\n",
+					__FUNCTION__, g_bt->bt_state));
+				
+				up(&g_bt->tsk_ctl.sema);
+			}
+		}
+
+		
+		if (saved_status == TRUE)
+			dev_wlc_bufvar_set(dev, "btc_flags",
+				(char *)&buf_flag7_default[0], sizeof(buf_flag7_default));
+#endif 
+
+		
+		if (saved_status == TRUE) {
+			regaddr = 66;
+			dev_wlc_intvar_set_reg(dev, "btc_params",
+				(char *)&regaddr, (char *)&saved_reg66);
+			regaddr = 41;
+			dev_wlc_intvar_set_reg(dev, "btc_params",
+				(char *)&regaddr, (char *)&saved_reg41);
+			regaddr = 68;
+			dev_wlc_intvar_set_reg(dev, "btc_params",
+				(char *)&regaddr, (char *)&saved_reg68);
+
+			WL_TRACE_COEX(("restore regs {66,41,68} <- 0x%x 0x%x 0x%x\n",
+				saved_reg66, saved_reg41, saved_reg68));
+		}
+		saved_status = FALSE;
+
+	}
+	else {
+		WL_ERROR(("%s Unkwown yet power setting, ignored\n",
+			__FUNCTION__));
+	}
+
+	p += snprintf(p, MAX_WX_STRING, "OK");
+
+	wrqu->data.length = p - extra + 1;
+
+	return error;
+}
+
+static int
+wl_iw_set_suspend_opt(
+struct net_device *dev,
+struct iw_request_info *info,
+union iwreq_data *wrqu,
+char *extra
+)
+{
+	int suspend_flag;
+	int ret_now;
+	int ret = 0;
+
+	suspend_flag = *(extra + strlen(SETSUSPENDOPT_CMD) + 1) - '0';
+
+	if (suspend_flag != 0)
+		suspend_flag = 1;
+
+	ret_now = net_os_set_suspend_disable(dev, suspend_flag);
+
+	if (ret_now != suspend_flag) {
+		if (!(ret = net_os_set_suspend(dev, ret_now, 1)))
+			WL_ERROR(("%s: Suspend Flag %d -> %d\n",
+			          __FUNCTION__, ret_now, suspend_flag));
+		else
+			WL_ERROR(("%s: failed %d\n", __FUNCTION__, ret));
+	}
+
+	return ret;
+}
+
+static int
+wl_iw_set_suspend_mode(
+struct net_device *dev,
+struct iw_request_info *info,
+union iwreq_data *wrqu,
+char *extra
+)
+{
+	int ret = 0;
+
+#if !defined(CONFIG_HAS_EARLYSUSPEND) || !defined(DHD_USE_EARLYSUSPEND)
+	int suspend_flag;
+
+	suspend_flag = *(extra + strlen(SETSUSPENDMODE_CMD) + 1) - '0';
+
+	if (suspend_flag != 0)
+		suspend_flag = 1;
+
+	if (!(ret = net_os_set_suspend(dev, suspend_flag, 0)))
+		WL_ERROR(("%s: Suspend Mode %d\n",__FUNCTION__,suspend_flag));
+	else
+		WL_ERROR(("%s: failed %d\n", __FUNCTION__, ret));
+#endif
+	return ret;
+}
+
+static int
+wl_format_ssid(char* ssid_buf, uint8* ssid, int ssid_len)
+{
+	int i, c;
+	char *p = ssid_buf;
+
+	if (ssid_len > 32) ssid_len = 32;
+
+	for (i = 0; i < ssid_len; i++) {
+		c = (int)ssid[i];
+		if (c == '\\') {
+			*p++ = '\\';
+			*p++ = '\\';
+		} else if (isprint((uchar)c)) {
+			*p++ = (char)c;
+		} else {
+			p += sprintf(p, "\\x%02X", c);
+		}
+	}
+	*p = '\0';
+
+	return p - ssid_buf;
+}
+
+static int
+wl_iw_get_link_speed(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = 0;
+	char *p = extra;
+	static int link_speed;
+
+	
+	net_os_wake_lock(dev);
+	if (g_onoff == G_WLAN_SET_ON) {
+		error = dev_wlc_ioctl(dev, WLC_GET_RATE, &link_speed, sizeof(link_speed));
+		link_speed *= 500000;
+	}
+
+	p += snprintf(p, MAX_WX_STRING, "LinkSpeed %d", link_speed/1000000);
+
+	wrqu->data.length = p - extra + 1;
+
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+
+static int
+wl_iw_get_dtim_skip(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = -1;
+	char *p = extra;
+	char iovbuf[32];
+
+	net_os_wake_lock(dev);
+	if (g_onoff == G_WLAN_SET_ON) {
+
+			memset(iovbuf, 0, sizeof(iovbuf));
+			strcpy(iovbuf, "bcn_li_dtim");
+
+			if ((error = dev_wlc_ioctl(dev, WLC_GET_VAR,
+				&iovbuf, sizeof(iovbuf))) >= 0) {
+
+				p += snprintf(p, MAX_WX_STRING, "Dtim_skip %d", iovbuf[0]);
+				WL_TRACE(("%s: get dtim_skip = %d\n", __FUNCTION__, iovbuf[0]));
+				wrqu->data.length = p - extra + 1;
+			}
+			else
+				WL_ERROR(("%s: get dtim_skip failed code %d\n",
+					__FUNCTION__, error));
+	}
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+
+static int
+wl_iw_set_dtim_skip(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = -1;
+	char *p = extra;
+	int bcn_li_dtim;
+	char iovbuf[32];
+
+	net_os_wake_lock(dev);
+	if (g_onoff == G_WLAN_SET_ON) {
+
+		bcn_li_dtim = htod32((uint)*(extra + strlen(DTIM_SKIP_SET_CMD) + 1) - '0');
+
+		if ((bcn_li_dtim >= 0) || ((bcn_li_dtim <= 5))) {
+
+			memset(iovbuf, 0, sizeof(iovbuf));
+			bcm_mkiovar("bcn_li_dtim", (char *)&bcn_li_dtim,
+				4, iovbuf, sizeof(iovbuf));
+
+			if ((error = dev_wlc_ioctl(dev, WLC_SET_VAR,
+				&iovbuf, sizeof(iovbuf))) >= 0) {
+				p += snprintf(p, MAX_WX_STRING, "OK");
+
+				
+				net_os_set_dtim_skip(dev, bcn_li_dtim);
+
+				WL_TRACE(("%s: set dtim_skip %d OK\n", __FUNCTION__,
+					bcn_li_dtim));
+				goto exit;
+			}
+			else  WL_ERROR(("%s: set dtim_skip %d failed code %d\n",
+				__FUNCTION__, bcn_li_dtim, error));
+		}
+		else  WL_ERROR(("%s Incorrect dtim_skip setting %d, ignored\n",
+			__FUNCTION__, bcn_li_dtim));
+	}
+
+	p += snprintf(p, MAX_WX_STRING, "FAIL");
+
+exit:
+	wrqu->data.length = p - extra + 1;
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+
+static int
+wl_iw_get_band(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = -1;
+	char *p = extra;
+	static int band;
+
+	net_os_wake_lock(dev);
+
+	if (g_onoff == G_WLAN_SET_ON) {
+		error = dev_wlc_ioctl(dev, WLC_GET_BAND, &band, sizeof(band));
+
+		p += snprintf(p, MAX_WX_STRING, "Band %d", band);
+
+		wrqu->data.length = p - extra + 1;
+	}
+
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+
+static int
+wl_iw_set_band(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = -1;
+	char *p = extra;
+	uint band;
+
+	net_os_wake_lock(dev);
+
+	if (g_onoff == G_WLAN_SET_ON) {
+
+		band = htod32((uint)*(extra + strlen(BAND_SET_CMD) + 1) - '0');
+
+		if ((band == WLC_BAND_AUTO) || (band == WLC_BAND_5G) || (band == WLC_BAND_2G)) {
+			
+			if ((error = dev_wlc_ioctl(dev, WLC_SET_BAND,
+				&band, sizeof(band))) >= 0) {
+				p += snprintf(p, MAX_WX_STRING, "OK");
+				WL_TRACE(("%s: set band %d OK\n", __FUNCTION__, band));
+				goto exit;
+			} else {
+				WL_ERROR(("%s: set band %d failed code %d\n", __FUNCTION__,
+				          band, error));
+			}
+		} else {
+			WL_ERROR(("%s Incorrect band setting %d, ignored\n", __FUNCTION__, band));
+		}
+	}
+
+	p += snprintf(p, MAX_WX_STRING, "FAIL");
+
+exit:
+	wrqu->data.length = p - extra + 1;
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+#ifdef PNO_SUPPORT
+
+static int
+wl_iw_set_pno_reset(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = -1;
+	char *p = extra;
+
+	net_os_wake_lock(dev);
+	if ((g_onoff == G_WLAN_SET_ON) && (dev != NULL)) {
+
+		if ((error = dhd_dev_pno_reset(dev)) >= 0) {
+				p += snprintf(p, MAX_WX_STRING, "OK");
+				WL_TRACE(("%s: set OK\n", __FUNCTION__));
+				goto exit;
+		}
+		else  WL_ERROR(("%s: failed code %d\n", __FUNCTION__, error));
+	}
+
+	p += snprintf(p, MAX_WX_STRING, "FAIL");
+
+exit:
+	wrqu->data.length = p - extra + 1;
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+
+
+static int
+wl_iw_set_pno_enable(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error = -1;
+	char *p = extra;
+	int pfn_enabled;
+
+	net_os_wake_lock(dev);
+	pfn_enabled = htod32((uint)*(extra + strlen(PNOENABLE_SET_CMD) + 1) - '0');
+
+	if ((g_onoff == G_WLAN_SET_ON) && (dev != NULL)) {
+
+		if ((error = dhd_dev_pno_enable(dev, pfn_enabled)) >= 0) {
+				p += snprintf(p, MAX_WX_STRING, "OK");
+				WL_TRACE(("%s: set OK\n", __FUNCTION__));
+				goto exit;
+		}
+		else  WL_ERROR(("%s: failed code %d\n", __FUNCTION__, error));
+	}
+
+	p += snprintf(p, MAX_WX_STRING, "FAIL");
+
+exit:
+	wrqu->data.length = p - extra + 1;
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+
+
+static int
+wl_iw_set_pno_set(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int res = -1;
+	wlc_ssid_t ssids_local[MAX_PFN_LIST_COUNT];
+	int nssid = 0;
+	cmd_tlv_t *cmd_tlv_temp;
+	char *str_ptr;
+	int tlv_size_left;
+	int pno_time;
+	int pno_repeat;
+	int pno_freq_expo_max;
+#ifdef PNO_SET_DEBUG
+	int i;
+	char pno_in_example[] = {
+		'P', 'N', 'O', 'S', 'E', 'T', 'U', 'P', ' ',
+		'S', '1', '2', '0',
+		'S',
+		0x04,
+		'B', 'R', 'C', 'M',
+		'S',
+		0x04,
+		'G', 'O', 'O', 'G',
+		'T',
+		'1', 'E',
+		'R',
+		'2',
+		'M',
+		'2',
+		0x00
+		};
+#endif 
+
+	net_os_wake_lock(dev);
+	WL_ERROR(("\n### %s: info->cmd:%x, info->flags:%x, u.data=0x%p, u.len=%d\n",
+		__FUNCTION__, info->cmd, info->flags,
+		wrqu->data.pointer, wrqu->data.length));
+
+	if (g_onoff == G_WLAN_SET_OFF) {
+		WL_TRACE(("%s: driver is not up yet after START\n", __FUNCTION__));
+		goto exit_proc;
+	}
+
+	if (wrqu->data.length < (strlen(PNOSETUP_SET_CMD) + sizeof(cmd_tlv_t))) {
+		WL_ERROR(("%s argument=%d less %d\n", __FUNCTION__,
+		          wrqu->data.length, (int)(strlen(PNOSETUP_SET_CMD) + sizeof(cmd_tlv_t))));
+		goto exit_proc;
+	}
+
+#ifdef PNO_SET_DEBUG
+	if (!(extra = kmalloc(sizeof(pno_in_example) +100, GFP_KERNEL))) {
+		res = -ENOMEM;
+		goto exit_proc;
+	}
+	memcpy(extra, pno_in_example, sizeof(pno_in_example));
+	wrqu->data.length = sizeof(pno_in_example);
+	for (i = 0; i < wrqu->data.length; i++)
+		printf("%02X ", extra[i]);
+	printf("\n");
+#endif 
+
+	str_ptr = extra;
+#ifdef PNO_SET_DEBUG
+	str_ptr +=  strlen("PNOSETUP ");
+	tlv_size_left = wrqu->data.length - strlen("PNOSETUP ");
+#else
+	str_ptr +=  strlen(PNOSETUP_SET_CMD);
+	tlv_size_left = wrqu->data.length - strlen(PNOSETUP_SET_CMD);
+#endif
+
+	cmd_tlv_temp = (cmd_tlv_t *)str_ptr;
+	memset(ssids_local, 0, sizeof(ssids_local));
+	pno_repeat = pno_freq_expo_max = 0;
+	
+	if ((cmd_tlv_temp->prefix == PNO_TLV_PREFIX) &&
+		(cmd_tlv_temp->version == PNO_TLV_VERSION) &&
+		(cmd_tlv_temp->subver == PNO_TLV_SUBVERSION))
+	{
+		str_ptr += sizeof(cmd_tlv_t);
+		tlv_size_left  -= sizeof(cmd_tlv_t);
+
+		
+		if ((nssid = wl_iw_parse_ssid_list_tlv(&str_ptr, ssids_local,
+			MAX_PFN_LIST_COUNT,
+			&tlv_size_left)) <= 0) {
+			WL_ERROR(("SSID is not presented or corrupted ret=%d\n", nssid));
+			goto exit_proc;
+		}
+		else {
+			if ((str_ptr[0] != PNO_TLV_TYPE_TIME) || (tlv_size_left <= 1)) {
+				WL_ERROR(("%s scan duration corrupted field size %d\n",
+					__FUNCTION__, tlv_size_left));
+				goto exit_proc;
+			}
+			str_ptr++;
+			pno_time = simple_strtoul(str_ptr, &str_ptr, 16);
+			WL_PNO(("%s: pno_time=%d\n", __FUNCTION__, pno_time));
+
+			
+			if (str_ptr[0] != 0) {
+				if ((str_ptr[0] != PNO_TLV_FREQ_REPEAT)) {
+					WL_ERROR(("%s pno repeat : corrupted field\n",
+						__FUNCTION__));
+					goto exit_proc;
+				}
+				str_ptr++;
+				pno_repeat = simple_strtoul(str_ptr, &str_ptr, 16);
+				WL_PNO(("%s :got pno_repeat=%d\n", __FUNCTION__, pno_repeat));
+				if (str_ptr[0] != PNO_TLV_FREQ_EXPO_MAX) {
+					WL_ERROR(("%s FREQ_EXPO_MAX corrupted field size\n",
+						__FUNCTION__));
+					goto exit_proc;
+				}
+				str_ptr++;
+				pno_freq_expo_max = simple_strtoul(str_ptr, &str_ptr, 16);
+				WL_PNO(("%s: pno_freq_expo_max=%d\n",
+					__FUNCTION__, pno_freq_expo_max));
+			}
+		}
+	}
+	else {
+		WL_ERROR(("%s get wrong TLV command\n", __FUNCTION__));
+		goto exit_proc;
+	}
+
+	
+	res = dhd_dev_pno_set(dev, ssids_local, nssid, pno_time, pno_repeat, pno_freq_expo_max);
+
+exit_proc:
+	net_os_wake_unlock(dev);
+	return res;
+}
+
+static int
+wl_iw_set_pno_setadd(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int ret = -1;
+	char *tmp_ptr;
+	int size, tmp_size;
+
+	net_os_wake_lock(dev);
+	WL_ERROR(("\n### %s: info->cmd:%x, info->flags:%x, u.data=0x%p, u.len=%d\n",
+		__FUNCTION__, info->cmd, info->flags,
+		wrqu->data.pointer, wrqu->data.length));
+
+	if (g_onoff == G_WLAN_SET_OFF) {
+		WL_TRACE(("%s: driver is not up yet after START\n", __FUNCTION__));
+		goto exit_proc;
+	}
+
+	if (wrqu->data.length <= strlen(PNOSETADD_SET_CMD) + sizeof(cmd_tlv_t)) {
+		WL_ERROR(("%s argument=%d less than %d\n", __FUNCTION__,
+		          wrqu->data.length, (int)(strlen(PNOSETADD_SET_CMD) + sizeof(cmd_tlv_t))));
+		goto exit_proc;
+	}
+
+	
+	bcopy(PNOSETUP_SET_CMD, extra, strlen(PNOSETUP_SET_CMD));
+
+	tmp_ptr = extra + strlen(PNOSETUP_SET_CMD);
+	size = wrqu->data.length - strlen(PNOSETUP_SET_CMD);
+	tmp_size = size;
+	
+	while (*tmp_ptr && tmp_size > 0) {
+		if ((*tmp_ptr == 'S') && (size - tmp_size) >= sizeof(cmd_tlv_t)) {
+			*(tmp_ptr + 1) = ((*(tmp_ptr + 1) - '0') << 4) + (*(tmp_ptr + 2) - '0');
+			memmove(tmp_ptr + 2, tmp_ptr + 3, tmp_size - 3);
+			tmp_size -= 2 + *(tmp_ptr + 1);
+			tmp_ptr += 2 + *(tmp_ptr + 1);
+			size--;
+		} else {
+			tmp_ptr++;
+			tmp_size--;
+		}
+	}
+
+	wrqu->data.length = strlen(PNOSETUP_SET_CMD) + size;
+	ret = wl_iw_set_pno_set(dev, info, wrqu, extra);
+
+exit_proc:
+	net_os_wake_unlock(dev);
+	return ret;
+
+}
+#endif 
+
+static int
+wl_iw_get_rssi(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	static int rssi = 0;
+	static wlc_ssid_t ssid = {0};
+	int error = 0;
+	char *p = extra;
+	static char ssidbuf[SSID_FMT_BUF_LEN];
+	scb_val_t scb_val;
+
+	net_os_wake_lock(dev);
+
+	bzero(&scb_val, sizeof(scb_val_t));
+
+	if (g_onoff == G_WLAN_SET_ON) {
+		error = dev_wlc_ioctl(dev, WLC_GET_RSSI, &scb_val, sizeof(scb_val_t));
+		if (error) {
+			WL_ERROR(("%s: Fails %d\n", __FUNCTION__, error));
+			net_os_wake_unlock(dev);
+			return error;
+		}
+		rssi = dtoh32(scb_val.val);
+
+		error = dev_wlc_ioctl(dev, WLC_GET_SSID, &ssid, sizeof(ssid));
+		if (!error) {
+			ssid.SSID_len = dtoh32(ssid.SSID_len);
+			wl_format_ssid(ssidbuf, ssid.SSID, dtoh32(ssid.SSID_len));
+		}
+	}
+
+	p += snprintf(p, MAX_WX_STRING, "%s rssi %d ", ssidbuf, rssi);
+	wrqu->data.length = p - extra + 1;
+
+	net_os_wake_unlock(dev);
+	return error;
+}
+
+int
+wl_iw_send_priv_event(
+	struct net_device *dev,
+	char *flag
+)
+{
+	union iwreq_data wrqu;
+	char extra[IW_CUSTOM_MAX + 1];
+	int cmd;
+
+	cmd = IWEVCUSTOM;
+	memset(&wrqu, 0, sizeof(wrqu));
+	if (strlen(flag) > sizeof(extra))
+		return -1;
+
+	strcpy(extra, flag);
+	wrqu.data.length = strlen(extra);
+	wireless_send_event(dev, cmd, &wrqu, extra);
+	net_os_wake_lock_ctrl_timeout_enable(dev, DHD_EVENT_TIMEOUT_MS);
+	WL_TRACE(("Send IWEVCUSTOM Event as %s\n", extra));
+
+	return 0;
+}
+
+
+int
+wl_control_wl_start(struct net_device *dev)
+{
+	wl_iw_t *iw;
+	int ret = 0;
+
+	WL_TRACE(("Enter %s \n", __FUNCTION__));
+
+	if (!dev) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return -1;
+	}
+
+	iw = *(wl_iw_t **)netdev_priv(dev);
+
+	if (!iw) {
+		WL_ERROR(("%s: wl is null\n", __FUNCTION__));
+		return -1;
+	}
+
+	dhd_net_if_lock(dev);
+
+	if (g_onoff == G_WLAN_SET_OFF) {
+		dhd_customer_gpio_wlan_ctrl(WLAN_RESET_ON);
+
+#if defined(BCMLXSDMMC)
+		sdioh_start(NULL, 0);
+#endif
+
+		ret = dhd_dev_reset(dev, 0);
+
+#if defined(BCMLXSDMMC)
+		sdioh_start(NULL, 1);
+#endif
+		if (!ret)
+			dhd_dev_init_ioctl(dev);
+
+		g_onoff = G_WLAN_SET_ON;
+	}
+	WL_TRACE(("Exited %s\n", __FUNCTION__));
+
+	dhd_net_if_unlock(dev);
+	return ret;
+}
+
+
+static int
+wl_iw_control_wl_off(
+	struct net_device *dev,
+	struct iw_request_info *info
+)
+{
+	wl_iw_t *iw;
+	int ret = 0;
+
+	WL_TRACE(("Enter %s\n", __FUNCTION__));
+
+	if (!dev) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return -1;
+	}
+
+	iw = *(wl_iw_t **)netdev_priv(dev);
+
+	if (!iw) {
+		WL_ERROR(("%s: wl is null\n", __FUNCTION__));
+		return -1;
+	}
+
+	dhd_net_if_lock(dev);
+
+#ifdef SOFTAP
+	ap_cfg_running = FALSE;
+#endif 
+
+	if (g_onoff == G_WLAN_SET_ON) {
+		g_onoff = G_WLAN_SET_OFF;
+
+#if defined(WL_IW_USE_ISCAN)
+		g_iscan->iscan_state = ISCAN_STATE_IDLE;
+#endif 
+
+		ret = dhd_dev_reset(dev, 1);
+
+#if defined(WL_IW_USE_ISCAN)
+#if !defined(CSCAN)
+		
+		wl_iw_free_ss_cache();
+		wl_iw_run_ss_cache_timer(0);
+		
+		g_ss_cache_ctrl.m_link_down = 1;
+#endif 
+		memset(g_scan, 0, G_SCAN_RESULTS);
+		g_scan_specified_ssid = 0;
+#if defined(CONFIG_FIRST_SCAN)
+		
+		g_first_broadcast_scan = BROADCAST_SCAN_FIRST_IDLE;
+		g_first_counter_scans = 0;
+#endif 
+#endif 
+
+#if defined(BCMLXSDMMC)
+		sdioh_stop(NULL);
+#endif
+
+		dhd_customer_gpio_wlan_ctrl(WLAN_RESET_OFF);
+
+		wl_iw_send_priv_event(dev, "STOP");
+	}
+
+	dhd_net_if_unlock(dev);
+
+	WL_TRACE(("Exited %s\n", __FUNCTION__));
+
+	return ret;
+}
+
+static int
+wl_iw_control_wl_on(
+	struct net_device *dev,
+	struct iw_request_info *info
+)
+{
+	int ret = 0;
+
+	WL_TRACE(("Enter %s \n", __FUNCTION__));
+
+	ret = wl_control_wl_start(dev);
+
+	wl_iw_send_priv_event(dev, "START");
+
+#ifdef SOFTAP
+	if (!ap_fw_loaded) {
+		wl_iw_iscan_set_scan_broadcast_prep(dev, 0);
+	}
+#else
+	wl_iw_iscan_set_scan_broadcast_prep(dev, 0);
+#endif
+
+	WL_TRACE(("Exited %s\n", __FUNCTION__));
+
+	return ret;
+}
+
+#ifdef SOFTAP
+static struct ap_profile my_ap;
+static int set_ap_cfg(struct net_device *dev, struct ap_profile *ap); 
+static int get_assoc_sta_list(struct net_device *dev, char *buf, int len);
+static int set_ap_mac_list(struct net_device *dev, void *buf);
+
+#define PTYPE_STRING 0
+#define PTYPE_INTDEC 1   
+#define PTYPE_INTHEX 2
+#define PTYPE_STR_HEX 3  
+
+static int get_parameter_from_string(
+	char **str_ptr, const char *token, int param_type, void  *dst, int param_max_len);
+
+static int
+hex2num(char c)
+{
+	if (c >= '0' && c <= '9')
+		return c - '0';
+	if (c >= 'a' && c <= 'f')
+		return c - 'a' + 10;
+	if (c >= 'A' && c <= 'F')
+		return c - 'A' + 10;
+	return -1;
+}
+
+
+
+static int
+hstr_2_buf(const char *txt, u8 *buf, int len)
+{
+	int i;
+
+	for (i = 0; i < len; i++) {
+		int a, b;
+
+		a = hex2num(*txt++);
+		if (a < 0)
+			return -1;
+		b = hex2num(*txt++);
+		if (b < 0)
+			return -1;
+		*buf++ = (a << 4) | b;
+	}
+
+	return 0;
+}
+
+
+
+static int
+init_ap_profile_from_string(char *param_str, struct ap_profile *ap_cfg)
+{
+	char *str_ptr = param_str;
+	char sub_cmd[16];
+	int ret = 0;
+
+	memset(sub_cmd, 0, sizeof(sub_cmd));
+	memset(ap_cfg, 0, sizeof(struct ap_profile));
+
+	
+	if (get_parameter_from_string(&str_ptr, "ASCII_CMD=",
+		PTYPE_STRING, sub_cmd, SSID_LEN) != 0) {
+	 return -1;
+	}
+	if (strncmp(sub_cmd, "AP_CFG", 6)) {
+	   WL_ERROR(("ERROR: sub_cmd:%s != 'AP_CFG'!\n", sub_cmd));
+		return -1;
+	}
+
+	
+	
+	ret = get_parameter_from_string(&str_ptr, "SSID=", PTYPE_STRING, ap_cfg->ssid, SSID_LEN);
+
+	ret |= get_parameter_from_string(&str_ptr, "SEC=", PTYPE_STRING,  ap_cfg->sec, SEC_LEN);
+
+	ret |= get_parameter_from_string(&str_ptr, "KEY=", PTYPE_STRING,  ap_cfg->key, KEY_LEN);
+
+	ret |= get_parameter_from_string(&str_ptr, "CHANNEL=", PTYPE_INTDEC, &ap_cfg->channel, 5);
+
+	
+	get_parameter_from_string(&str_ptr, "PREAMBLE=", PTYPE_INTDEC, &ap_cfg->preamble, 5);
+
+	
+	get_parameter_from_string(&str_ptr, "MAX_SCB=", PTYPE_INTDEC,  &ap_cfg->max_scb, 5);
+
+	
+	get_parameter_from_string(&str_ptr, "HIDDEN=",
+		PTYPE_INTDEC,  &ap_cfg->closednet, 5);
+
+	
+	get_parameter_from_string(&str_ptr, "COUNTRY=",
+		PTYPE_STRING,  &ap_cfg->country_code, 3);
+
+	return ret;
+}
+#endif 
+
+
+
+#ifdef SOFTAP
+static int
+iwpriv_set_ap_config(struct net_device *dev,
+            struct iw_request_info *info,
+            union iwreq_data *wrqu,
+            char *ext)
+{
+	int res = 0;
+	char  *extra = NULL;
+	struct ap_profile *ap_cfg = &my_ap;
+
+	WL_TRACE(("> Got IWPRIV SET_AP IOCTL: info->cmd:%x, info->flags:%x, u.data:%p, u.len:%d\n",
+		info->cmd, info->flags,
+		wrqu->data.pointer, wrqu->data.length));
+
+	if (!ap_fw_loaded) {
+		WL_ERROR(("Can't execute %s(), SOFTAP fw is not Loaded\n",
+			__FUNCTION__));
+		return -1;
+	}
+
+	if (wrqu->data.length != 0) {
+
+		char *str_ptr;
+
+		if (!(extra = kmalloc(wrqu->data.length+1, GFP_KERNEL)))
+			return -ENOMEM;
+
+		if (copy_from_user(extra, wrqu->data.pointer, wrqu->data.length)) {
+			kfree(extra);
+			return -EFAULT;
+		}
+
+		extra[wrqu->data.length] = 0;
+		WL_SOFTAP((" Got str param in iw_point:\n %s\n", extra));
+
+		memset(ap_cfg, 0, sizeof(struct ap_profile));
+
+		
+
+		str_ptr = extra;
+
+		if ((res = init_ap_profile_from_string(extra, ap_cfg)) < 0) {
+			WL_ERROR(("%s failed to parse %d\n", __FUNCTION__, res));
+			kfree(extra);
+			return -1;
+		}
+
+	} else {
+	 
+	  WL_ERROR(("IWPRIV argument len = 0 \n"));
+	  return -1;
+	}
+
+	if ((res = set_ap_cfg(dev, ap_cfg)) < 0)
+		WL_ERROR(("%s failed to set_ap_cfg %d\n", __FUNCTION__, res));
+
+	kfree(extra);
+
+	return res;
+}
+#endif 
+
+
+
+#ifdef SOFTAP
+static int iwpriv_get_assoc_list(struct net_device *dev,
+        struct iw_request_info *info,
+        union iwreq_data *p_iwrq,
+        char *extra)
+{
+	int i, ret = 0;
+	char mac_buf[256];
+	struct maclist *sta_maclist = (struct maclist *)mac_buf;
+
+	char mac_lst[384];
+	char *p_mac_str;
+	char *p_mac_str_end;
+	wl_iw_t *iw;
+
+	if ((!dev) || (!extra)) {
+		
+		return -EINVAL;
+	}
+
+
+	iw = *(wl_iw_t **)netdev_priv(dev);
+
+	net_os_wake_lock(dev);
+	DHD_OS_MUTEX_LOCK(&wl_softap_lock);
+
+	WL_TRACE(("\n %s: IWPRIV IOCTL: cmd:%hx, flags:%hx, extra:%p, iwp.len:%d,"
+		"iwp.len:%p, iwp.flags:%x  \n", __FUNCTION__, info->cmd, info->flags,
+		extra, p_iwrq->data.length, p_iwrq->data.pointer, p_iwrq->data.flags));
+
+
+	memset(sta_maclist, 0, sizeof(mac_buf));
+
+	sta_maclist->count = 8;
+
+	WL_SOFTAP(("%s: net device:%s, buf_sz:%d\n",
+		__FUNCTION__, dev->name, sizeof(mac_buf)));
+
+	if ((ret = get_assoc_sta_list(dev, mac_buf, sizeof(mac_buf))) < 0) {
+		WL_ERROR(("%s: sta list ioctl error:%d\n",
+			__FUNCTION__, ret));
+		goto func_exit;
+	}
+
+	WL_SOFTAP(("%s: got %d stations\n", __FUNCTION__,
+		sta_maclist->count));
+
+
+	
+	memset(mac_lst, 0, sizeof(mac_lst));
+	p_mac_str = mac_lst;
+	p_mac_str_end = &mac_lst[sizeof(mac_lst)-1];
+
+	for (i = 0; i < 8; i++) { 
+		struct ether_addr * id = &sta_maclist->ea[i];
+		if (!ETHER_ISNULLADDR(id->octet)) {
+			scb_val_t scb_val;
+			int rssi = 0;
+			bzero(&scb_val, sizeof(scb_val_t));
+
+			
+			if ((p_mac_str_end - p_mac_str) <= 36) {
+				WL_ERROR(("%s: mac list buf is < 36 for item[%i] item\n",
+					__FUNCTION__, i));
+				break;
+			}
+
+			p_mac_str += snprintf(p_mac_str, MAX_WX_STRING,
+			"\nMac[%d]=%02X:%02X:%02X:%02X:%02X:%02X,", i,
+			id->octet[0], id->octet[1], id->octet[2],
+			id->octet[3], id->octet[4], id->octet[5]);
+
+			
+			bcopy(id->octet, &scb_val.ea, 6);
+			ret = dev_wlc_ioctl(dev, WLC_GET_RSSI, &scb_val, sizeof(scb_val_t));
+			if (ret  < 0) {
+				snprintf(p_mac_str, MAX_WX_STRING, "RSSI:ERR");
+				WL_ERROR(("%s: RSSI ioctl error:%d\n",
+					__FUNCTION__, ret));
+				break;
+			}
+
+			rssi = dtoh32(scb_val.val);
+			p_mac_str += snprintf(p_mac_str, MAX_WX_STRING,
+			"RSSI:%d", rssi);
+		}
+	}
+
+	p_iwrq->data.length = strlen(mac_lst)+1; 
+
+	WL_SOFTAP(("%s: data to user:\n%s\n usr_ptr:%p\n", __FUNCTION__,
+		mac_lst, p_iwrq->data.pointer));
+
+	if (p_iwrq->data.length) {
+		bcopy(mac_lst, extra, p_iwrq->data.length);
+	}
+
+func_exit:
+
+	DHD_OS_MUTEX_UNLOCK(&wl_softap_lock);
+	net_os_wake_unlock(dev);
+
+	WL_SOFTAP(("%s: Exited\n", __FUNCTION__));
+	return ret;
+}
+#endif 
+
+
+#ifdef SOFTAP
+
+#define MAC_FILT_MAX 8
+static int iwpriv_set_mac_filters(struct net_device *dev,
+        struct iw_request_info *info,
+        union iwreq_data *wrqu,
+        char *ext)
+{
+	int i, ret = -1;
+	char  * extra = NULL;
+	int mac_cnt = 0; 
+	int mac_mode = 0;
+	struct ether_addr *p_ea;
+	struct mac_list_set mflist_set; 
+
+	WL_SOFTAP((">>> Got IWPRIV SET_MAC_FILTER IOCTL:  info->cmd:%x,"
+			"info->flags:%x, u.data:%p, u.len:%d\n",
+			info->cmd, info->flags,
+			wrqu->data.pointer, wrqu->data.length));
+
+	if (wrqu->data.length != 0) {
+
+		char *str_ptr;
+
+		if (!(extra = kmalloc(wrqu->data.length+1, GFP_KERNEL)))
+			return -ENOMEM;
+
+		if (copy_from_user(extra, wrqu->data.pointer, wrqu->data.length)) {
+			kfree(extra);
+			return -EFAULT;
+		}
+
+		extra[wrqu->data.length] = 0;
+		WL_SOFTAP((" Got parameter string in iw_point:\n %s \n", extra));
+
+		memset(&mflist_set, 0, sizeof(mflist_set));
+
+		
+		str_ptr = extra;
+
+
+		
+		if (get_parameter_from_string(&str_ptr, "MAC_MODE=",
+			PTYPE_INTDEC, &mac_mode, 4) != 0) {
+			WL_ERROR(("ERROR: 'MAC_MODE=' token is missing\n"));
+			goto exit_proc;
+		}
+
+		p_ea = &mflist_set.mac_list.ea[0];
+
+		if (get_parameter_from_string(&str_ptr, "MAC_CNT=",
+			PTYPE_INTDEC, &mac_cnt, 4) != 0) {
+			WL_ERROR(("ERROR: 'MAC_CNT=' token param is missing \n"));
+			goto exit_proc;
+		}
+
+		if (mac_cnt > MAC_FILT_MAX) {
+			WL_ERROR(("ERROR: number of MAC filters > MAX\n"));
+			goto exit_proc;
+		}
+
+		for (i=0; i< mac_cnt; i++)	
+		if (get_parameter_from_string(&str_ptr, "MAC=",
+			PTYPE_STR_HEX, &p_ea[i], 12) != 0) {
+			WL_ERROR(("ERROR: MAC_filter[%d] is missing !\n", i));
+			goto exit_proc;
+		}
+
+		WL_SOFTAP(("MAC_MODE=:%d, MAC_CNT=%d, MACs:..\n", mac_mode, mac_cnt));
+		for (i = 0; i < mac_cnt; i++) {
+		   WL_SOFTAP(("mac_filt[%d]:", i));
+		   dhd_print_buf(&p_ea[i], 6, 0);
+		}
+
+		
+		mflist_set.mode = mac_mode;
+		mflist_set.mac_list.count = mac_cnt;
+		set_ap_mac_list(dev, &mflist_set);
+
+		
+		wrqu->data.pointer = NULL;
+		wrqu->data.length = 0;
+		ret = 0;
+
+	} else {
+	 
+	  WL_ERROR(("IWPRIV argument len is 0\n"));
+	  return -1;
+	}
+
+	exit_proc:
+	kfree(extra);
+	return ret;
+}
+#endif 
+
+
+#ifdef SOFTAP
+
+static int iwpriv_set_ap_sta_disassoc(struct net_device *dev,
+        struct iw_request_info *info,
+        union iwreq_data *wrqu,
+        char *ext)
+{
+	int res = 0;
+	char sta_mac[6] = {0, 0, 0, 0, 0, 0};
+	char cmd_buf[256];
+	char *str_ptr = cmd_buf;
+
+	WL_SOFTAP((">>%s called\n args: info->cmd:%x,"
+		" info->flags:%x, u.data.p:%p, u.data.len:%d\n",
+		__FUNCTION__, info->cmd, info->flags,
+		wrqu->data.pointer, wrqu->data.length));
+
+	if (wrqu->data.length != 0) {
+
+		if (copy_from_user(cmd_buf, wrqu->data.pointer, wrqu->data.length)) {
+			return -EFAULT;
+		}
+
+		if (get_parameter_from_string(&str_ptr,
+			"MAC=", PTYPE_STR_HEX, sta_mac, 12) == 0) {
+			res = wl_iw_softap_deassoc_stations(dev, sta_mac);
+		} else  {
+			WL_ERROR(("ERROR: STA_MAC= token not found\n"));
+		}
+	}
+
+	return res;
+}
+#endif 
+
+#endif 
+
+#if WIRELESS_EXT < 13
+struct iw_request_info
+{
+	__u16		cmd;		
+	__u16		flags;		
+};
+
+typedef int (*iw_handler)(struct net_device *dev,
+                struct iw_request_info *info,
+                void *wrqu,
+                char *extra);
+#endif 
+
+static int
+wl_iw_config_commit(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	void *zwrq,
+	char *extra
+)
+{
+	wlc_ssid_t ssid;
+	int error;
+	struct sockaddr bssid;
+
+	WL_TRACE(("%s: SIOCSIWCOMMIT\n", dev->name));
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_SSID, &ssid, sizeof(ssid))))
+		return error;
+
+	ssid.SSID_len = dtoh32(ssid.SSID_len);
+
+	if (!ssid.SSID_len)
+		return 0;
+
+	bzero(&bssid, sizeof(struct sockaddr));
+	if ((error = dev_wlc_ioctl(dev, WLC_REASSOC, &bssid, ETHER_ADDR_LEN))) {
+		WL_ERROR(("%s: WLC_REASSOC to %s failed \n", __FUNCTION__, ssid.SSID));
+		return error;
+	}
+
+	return 0;
+}
+
+static int
+wl_iw_get_name(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	char *cwrq,
+	char *extra
+)
+{
+	WL_TRACE(("%s: SIOCGIWNAME\n", dev->name));
+
+	strcpy(cwrq, "IEEE 802.11-DS");
+
+	return 0;
+}
+
+static int
+wl_iw_set_freq(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_freq *fwrq,
+	char *extra
+)
+{
+	int error, chan;
+	uint sf = 0;
+
+	WL_TRACE(("%s %s: SIOCSIWFREQ\n", __FUNCTION__, dev->name));
+
+#if defined(SOFTAP)
+	if (ap_cfg_running) {
+		WL_TRACE(("%s:>> not executed, 'SOFT_AP is active' \n", __FUNCTION__));
+		return 0;
+	}
+#endif
+
+	
+	if (fwrq->e == 0 && fwrq->m < MAXCHANNEL) {
+		chan = fwrq->m;
+	}
+	
+	else {
+		
+		if (fwrq->e >= 6) {
+			fwrq->e -= 6;
+			while (fwrq->e--)
+				fwrq->m *= 10;
+		} else if (fwrq->e < 6) {
+			while (fwrq->e++ < 6)
+				fwrq->m /= 10;
+		}
+		
+		if (fwrq->m > 4000 && fwrq->m < 5000)
+			sf = WF_CHAN_FACTOR_4_G; 
+
+		chan = wf_mhz2channel(fwrq->m, sf);
+	}
+
+	chan = htod32(chan);
+
+	if ((error = dev_wlc_ioctl(dev, WLC_SET_CHANNEL, &chan, sizeof(chan))))
+		return error;
+
+	g_wl_iw_params.target_channel = chan;
+
+	
+	return -EINPROGRESS;
+}
+
+static int
+wl_iw_get_freq(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_freq *fwrq,
+	char *extra
+)
+{
+	channel_info_t ci;
+	int error;
+
+	WL_TRACE(("%s: SIOCGIWFREQ\n", dev->name));
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_CHANNEL, &ci, sizeof(ci))))
+		return error;
+
+	
+	fwrq->m = dtoh32(ci.hw_channel);
+	fwrq->e = dtoh32(0);
+	return 0;
+}
+
+static int
+wl_iw_set_mode(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	__u32 *uwrq,
+	char *extra
+)
+{
+	int infra = 0, ap = 0, error = 0;
+
+	WL_TRACE(("%s: SIOCSIWMODE\n", dev->name));
+
+	switch (*uwrq) {
+	case IW_MODE_MASTER:
+		infra = ap = 1;
+		break;
+	case IW_MODE_ADHOC:
+	case IW_MODE_AUTO:
+		break;
+	case IW_MODE_INFRA:
+		infra = 1;
+		break;
+	default:
+		return -EINVAL;
+	}
+	infra = htod32(infra);
+	ap = htod32(ap);
+
+	if ((error = dev_wlc_ioctl(dev, WLC_SET_INFRA, &infra, sizeof(infra))) ||
+	    (error = dev_wlc_ioctl(dev, WLC_SET_AP, &ap, sizeof(ap))))
+		return error;
+
+	
+	return -EINPROGRESS;
+}
+
+static int
+wl_iw_get_mode(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	__u32 *uwrq,
+	char *extra
+)
+{
+	int error, infra = 0, ap = 0;
+
+	WL_TRACE(("%s: SIOCGIWMODE\n", dev->name));
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_INFRA, &infra, sizeof(infra))) ||
+	    (error = dev_wlc_ioctl(dev, WLC_GET_AP, &ap, sizeof(ap))))
+		return error;
+
+	infra = dtoh32(infra);
+	ap = dtoh32(ap);
+	*uwrq = infra ? ap ? IW_MODE_MASTER : IW_MODE_INFRA : IW_MODE_ADHOC;
+
+	return 0;
+}
+
+static int
+wl_iw_get_range(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	struct iw_range *range = (struct iw_range *) extra;
+	wl_uint32_list_t *list;
+	wl_rateset_t rateset;
+	int8 *channels;
+	int error, i, k;
+	uint sf, ch;
+
+	int phytype;
+	int bw_cap = 0, sgi_tx = 0, nmode = 0;
+	channel_info_t ci;
+	uint8 nrate_list2copy = 0;
+	uint16 nrate_list[4][8] = { {13, 26, 39, 52, 78, 104, 117, 130},
+		{14, 29, 43, 58, 87, 116, 130, 144},
+		{27, 54, 81, 108, 162, 216, 243, 270},
+		{30, 60, 90, 120, 180, 240, 270, 300}};
+
+	WL_TRACE(("%s: SIOCGIWRANGE\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	channels = kmalloc((MAXCHANNEL+1)*4, GFP_KERNEL);
+	if (!channels) {
+		WL_ERROR(("Could not alloc channels\n"));
+		return -ENOMEM;
+	}
+	list = (wl_uint32_list_t *)channels;
+
+	dwrq->length = sizeof(struct iw_range);
+	memset(range, 0, sizeof(*range));
+
+	
+	range->min_nwid = range->max_nwid = 0;
+
+	
+	list->count = htod32(MAXCHANNEL);
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_VALID_CHANNELS, channels, (MAXCHANNEL+1)*4))) {
+		kfree(channels);
+		return error;
+	}
+	for (i = 0; i < dtoh32(list->count) && i < IW_MAX_FREQUENCIES; i++) {
+		range->freq[i].i = dtoh32(list->element[i]);
+
+		ch = dtoh32(list->element[i]);
+		if (ch <= CH_MAX_2G_CHANNEL)
+			sf = WF_CHAN_FACTOR_2_4_G;
+		else
+			sf = WF_CHAN_FACTOR_5_G;
+
+		range->freq[i].m = wf_channel2mhz(ch, sf);
+		range->freq[i].e = 6;
+	}
+	range->num_frequency = range->num_channels = i;
+
+	
+	range->max_qual.qual = 5;
+	
+	range->max_qual.level = 0x100 - 200;	
+	
+	range->max_qual.noise = 0x100 - 200;	
+	
+	range->sensitivity = 65535;
+
+#if WIRELESS_EXT > 11
+	
+	range->avg_qual.qual = 3;
+	
+	range->avg_qual.level = 0x100 + WL_IW_RSSI_GOOD;
+	
+	range->avg_qual.noise = 0x100 - 75;	
+#endif 
+
+	
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_CURR_RATESET, &rateset, sizeof(rateset)))) {
+		kfree(channels);
+		return error;
+	}
+	rateset.count = dtoh32(rateset.count);
+	range->num_bitrates = rateset.count;
+	for (i = 0; i < rateset.count && i < IW_MAX_BITRATES; i++)
+		range->bitrate[i] = (rateset.rates[i]& 0x7f) * 500000; 
+	dev_wlc_intvar_get(dev, "nmode", &nmode);
+	dev_wlc_ioctl(dev, WLC_GET_PHYTYPE, &phytype, sizeof(phytype));
+
+	if (nmode == 1 && phytype == WLC_PHY_TYPE_SSN) {
+		dev_wlc_intvar_get(dev, "mimo_bw_cap", &bw_cap);
+		dev_wlc_intvar_get(dev, "sgi_tx", &sgi_tx);
+		dev_wlc_ioctl(dev, WLC_GET_CHANNEL, &ci, sizeof(channel_info_t));
+		ci.hw_channel = dtoh32(ci.hw_channel);
+
+		if (bw_cap == 0 ||
+			(bw_cap == 2 && ci.hw_channel <= 14)) {
+			if (sgi_tx == 0)
+				nrate_list2copy = 0;
+			else
+				nrate_list2copy = 1;
+		}
+		if (bw_cap == 1 ||
+			(bw_cap == 2 && ci.hw_channel >= 36)) {
+			if (sgi_tx == 0)
+				nrate_list2copy = 2;
+			else
+				nrate_list2copy = 3;
+		}
+		range->num_bitrates += 8;
+		for (k = 0; i < range->num_bitrates; k++, i++) {
+			
+			range->bitrate[i] = (nrate_list[nrate_list2copy][k]) * 500000;
+		}
+	}
+
+	
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_PHYTYPE, &i, sizeof(i)))) {
+		kfree(channels);
+		return error;
+	}
+	i = dtoh32(i);
+	if (i == WLC_PHY_TYPE_A)
+		range->throughput = 24000000;	
+	else
+		range->throughput = 1500000;	
+
+	
+	range->min_rts = 0;
+	range->max_rts = 2347;
+	range->min_frag = 256;
+	range->max_frag = 2346;
+
+	range->max_encoding_tokens = DOT11_MAX_DEFAULT_KEYS;
+	range->num_encoding_sizes = 4;
+	range->encoding_size[0] = WEP1_KEY_SIZE;
+	range->encoding_size[1] = WEP128_KEY_SIZE;
+#if WIRELESS_EXT > 17
+	range->encoding_size[2] = TKIP_KEY_SIZE;
+#else
+	range->encoding_size[2] = 0;
+#endif
+	range->encoding_size[3] = AES_KEY_SIZE;
+
+	
+	range->min_pmp = 0;
+	range->max_pmp = 0;
+	range->min_pmt = 0;
+	range->max_pmt = 0;
+	range->pmp_flags = 0;
+	range->pm_capa = 0;
+
+	
+	range->num_txpower = 2;
+	range->txpower[0] = 1;
+	range->txpower[1] = 255;
+	range->txpower_capa = IW_TXPOW_MWATT;
+
+#if WIRELESS_EXT > 10
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 19;
+
+	
+	range->retry_capa = IW_RETRY_LIMIT;
+	range->retry_flags = IW_RETRY_LIMIT;
+	range->r_time_flags = 0;
+	
+	range->min_retry = 1;
+	range->max_retry = 255;
+	
+	range->min_r_time = 0;
+	range->max_r_time = 0;
+#endif 
+
+#if WIRELESS_EXT > 17
+	range->enc_capa = IW_ENC_CAPA_WPA;
+	range->enc_capa |= IW_ENC_CAPA_CIPHER_TKIP;
+	range->enc_capa |= IW_ENC_CAPA_CIPHER_CCMP;
+	range->enc_capa |= IW_ENC_CAPA_WPA2;
+
+	
+	IW_EVENT_CAPA_SET_KERNEL(range->event_capa);
+	
+	IW_EVENT_CAPA_SET(range->event_capa, SIOCGIWAP);
+	IW_EVENT_CAPA_SET(range->event_capa, SIOCGIWSCAN);
+	IW_EVENT_CAPA_SET(range->event_capa, IWEVTXDROP);
+	IW_EVENT_CAPA_SET(range->event_capa, IWEVMICHAELMICFAILURE);
+	IW_EVENT_CAPA_SET(range->event_capa, IWEVASSOCREQIE);
+	IW_EVENT_CAPA_SET(range->event_capa, IWEVASSOCRESPIE);
+	IW_EVENT_CAPA_SET(range->event_capa, IWEVPMKIDCAND);
+#endif 
+
+	kfree(channels);
+
+	return 0;
+}
+
+static int
+rssi_to_qual(int rssi)
+{
+	if (rssi <= WL_IW_RSSI_NO_SIGNAL)
+		return 0;
+	else if (rssi <= WL_IW_RSSI_VERY_LOW)
+		return 1;
+	else if (rssi <= WL_IW_RSSI_LOW)
+		return 2;
+	else if (rssi <= WL_IW_RSSI_GOOD)
+		return 3;
+	else if (rssi <= WL_IW_RSSI_VERY_GOOD)
+		return 4;
+	else
+		return 5;
+}
+
+static int
+wl_iw_set_spy(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_iw_t *iw = NETDEV_PRIV(dev);
+	struct sockaddr *addr = (struct sockaddr *) extra;
+	int i;
+
+	WL_TRACE(("%s: SIOCSIWSPY\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	iw->spy_num = MIN(ARRAYSIZE(iw->spy_addr), dwrq->length);
+	for (i = 0; i < iw->spy_num; i++)
+		memcpy(&iw->spy_addr[i], addr[i].sa_data, ETHER_ADDR_LEN);
+	memset(iw->spy_qual, 0, sizeof(iw->spy_qual));
+
+	return 0;
+}
+
+static int
+wl_iw_get_spy(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_iw_t *iw = NETDEV_PRIV(dev);
+	struct sockaddr *addr = (struct sockaddr *) extra;
+	struct iw_quality *qual = (struct iw_quality *) &addr[iw->spy_num];
+	int i;
+
+	WL_TRACE(("%s: SIOCGIWSPY\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	dwrq->length = iw->spy_num;
+	for (i = 0; i < iw->spy_num; i++) {
+		memcpy(addr[i].sa_data, &iw->spy_addr[i], ETHER_ADDR_LEN);
+		addr[i].sa_family = AF_UNIX;
+		memcpy(&qual[i], &iw->spy_qual[i], sizeof(struct iw_quality));
+		iw->spy_qual[i].updated = 0;
+	}
+
+	return 0;
+}
+
+
+static int
+wl_iw_ch_to_chanspec(int ch, wl_join_params_t *join_params, int *join_params_size)
+{
+	chanspec_t chanspec = 0;
+
+	if (ch != 0) {
+		
+		join_params->params.chanspec_num = 1;
+		join_params->params.chanspec_list[0] = ch;
+
+		if (join_params->params.chanspec_list[0])
+			chanspec |= WL_CHANSPEC_BAND_2G;
+		else
+			chanspec |= WL_CHANSPEC_BAND_5G;
+
+		chanspec |= WL_CHANSPEC_BW_20;
+		chanspec |= WL_CHANSPEC_CTL_SB_NONE;
+
+		
+		*join_params_size += WL_ASSOC_PARAMS_FIXED_SIZE +
+			join_params->params.chanspec_num * sizeof(chanspec_t);
+
+		
+		join_params->params.chanspec_list[0]  &= WL_CHANSPEC_CHAN_MASK;
+		join_params->params.chanspec_list[0] |= chanspec;
+		join_params->params.chanspec_list[0] =
+		        htodchanspec(join_params->params.chanspec_list[0]);
+
+		join_params->params.chanspec_num = htod32(join_params->params.chanspec_num);
+
+		WL_TRACE(("%s  join_params->params.chanspec_list[0]= %X\n",
+			__FUNCTION__, join_params->params.chanspec_list[0]));
+	}
+	return 1;
+}
+
+static int
+wl_iw_set_wap(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct sockaddr *awrq,
+	char *extra
+)
+{
+	int error = -EINVAL;
+	wl_join_params_t join_params;
+	int join_params_size;
+
+	WL_TRACE(("%s: SIOCSIWAP\n", dev->name));
+
+	if (awrq->sa_family != ARPHRD_ETHER) {
+		WL_ERROR(("Invalid Header...sa_family\n"));
+		return -EINVAL;
+	}
+
+	
+	if (ETHER_ISBCAST(awrq->sa_data) || ETHER_ISNULLADDR(awrq->sa_data)) {
+		scb_val_t scbval;
+		
+		bzero(&scbval, sizeof(scb_val_t));
+		
+		(void) dev_wlc_ioctl(dev, WLC_DISASSOC, &scbval, sizeof(scb_val_t));
+		return 0;
+	}
+
+
+	
+	memset(&join_params, 0, sizeof(join_params));
+	join_params_size = sizeof(join_params.ssid);
+
+	memcpy(join_params.ssid.SSID, g_ssid.SSID, g_ssid.SSID_len);
+	join_params.ssid.SSID_len = htod32(g_ssid.SSID_len);
+	memcpy(&join_params.params.bssid, awrq->sa_data, ETHER_ADDR_LEN);
+
+	
+	
+	WL_TRACE(("%s  target_channel=%d\n", __FUNCTION__, g_wl_iw_params.target_channel));
+	wl_iw_ch_to_chanspec(g_wl_iw_params.target_channel, &join_params, &join_params_size);
+
+	if ((error = dev_wlc_ioctl(dev, WLC_SET_SSID, &join_params, join_params_size))) {
+		WL_ERROR(("%s Invalid ioctl data=%d\n", __FUNCTION__, error));
+		return error;
+	}
+
+	if (g_ssid.SSID_len) {
+		WL_TRACE(("%s: join SSID=%s BSSID="MACSTR" ch=%d\n", __FUNCTION__,
+			g_ssid.SSID, MAC2STR((u8 *)awrq->sa_data),
+			g_wl_iw_params.target_channel));
+	}
+
+	
+	memset(&g_ssid, 0, sizeof(g_ssid));
+	return 0;
+}
+
+static int
+wl_iw_get_wap(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct sockaddr *awrq,
+	char *extra
+)
+{
+	WL_TRACE(("%s: SIOCGIWAP\n", dev->name));
+
+	awrq->sa_family = ARPHRD_ETHER;
+	memset(awrq->sa_data, 0, ETHER_ADDR_LEN);
+
+	
+	(void) dev_wlc_ioctl(dev, WLC_GET_BSSID, awrq->sa_data, ETHER_ADDR_LEN);
+
+	return 0;
+}
+
+#if WIRELESS_EXT > 17
+static int
+wl_iw_mlme(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct sockaddr *awrq,
+	char *extra
+)
+{
+	struct iw_mlme *mlme;
+	scb_val_t scbval;
+	int error  = -EINVAL;
+
+	WL_TRACE(("%s: SIOCSIWMLME DISASSOC/DEAUTH\n", dev->name));
+
+	mlme = (struct iw_mlme *)extra;
+	if (mlme == NULL) {
+		WL_ERROR(("Invalid ioctl data.\n"));
+		return error;
+	}
+
+	scbval.val = mlme->reason_code;
+	bcopy(&mlme->addr.sa_data, &scbval.ea, ETHER_ADDR_LEN);
+
+	if (mlme->cmd == IW_MLME_DISASSOC) {
+		scbval.val = htod32(scbval.val);
+		error = dev_wlc_ioctl(dev, WLC_DISASSOC, &scbval, sizeof(scb_val_t));
+	}
+	else if (mlme->cmd == IW_MLME_DEAUTH) {
+		scbval.val = htod32(scbval.val);
+		error = dev_wlc_ioctl(dev, WLC_SCB_DEAUTHENTICATE_FOR_REASON, &scbval,
+			sizeof(scb_val_t));
+	}
+	else {
+		WL_ERROR(("Invalid ioctl data.\n"));
+		return error;
+	}
+
+	return error;
+}
+#endif 
+
+#ifndef WL_IW_USE_ISCAN
+static int
+wl_iw_get_aplist(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_scan_results_t *list;
+	struct sockaddr *addr = (struct sockaddr *) extra;
+	struct iw_quality qual[IW_MAX_AP];
+	wl_bss_info_t *bi = NULL;
+	int error, i;
+	uint buflen = dwrq->length;
+
+	WL_TRACE(("%s: SIOCGIWAPLIST\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	
+	list = kmalloc(buflen, GFP_KERNEL);
+	if (!list)
+		return -ENOMEM;
+	memset(list, 0, buflen);
+	list->buflen = htod32(buflen);
+	if ((error = dev_wlc_ioctl(dev, WLC_SCAN_RESULTS, list, buflen))) {
+		WL_ERROR(("%d: Scan results error %d\n", __LINE__, error));
+		kfree(list);
+		return error;
+	}
+	list->buflen = dtoh32(list->buflen);
+	list->version = dtoh32(list->version);
+	list->count = dtoh32(list->count);
+	if (list->version != WL_BSS_INFO_VERSION) {
+		WL_ERROR(("%s : list->version %d != WL_BSS_INFO_VERSION\n",
+		          __FUNCTION__, list->version));
+		kfree(list);
+		return -EINVAL;
+	}
+
+	for (i = 0, dwrq->length = 0; i < list->count && dwrq->length < IW_MAX_AP; i++) {
+		bi = bi ? (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length)) : list->bss_info;
+		ASSERT(((uintptr)bi + dtoh32(bi->length)) <= ((uintptr)list +
+			buflen));
+
+		
+		if (!(dtoh16(bi->capability) & DOT11_CAP_ESS))
+			continue;
+
+		
+		memcpy(addr[dwrq->length].sa_data, &bi->BSSID, ETHER_ADDR_LEN);
+		addr[dwrq->length].sa_family = ARPHRD_ETHER;
+		qual[dwrq->length].qual = rssi_to_qual(dtoh16(bi->RSSI));
+		qual[dwrq->length].level = 0x100 + dtoh16(bi->RSSI);
+		qual[dwrq->length].noise = 0x100 + bi->phy_noise;
+
+		
+#if WIRELESS_EXT > 18
+		qual[dwrq->length].updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
+#else
+		qual[dwrq->length].updated = 7;
+#endif 
+
+		dwrq->length++;
+	}
+
+	kfree(list);
+
+	if (dwrq->length) {
+		memcpy(&addr[dwrq->length], qual, sizeof(struct iw_quality) * dwrq->length);
+		
+		dwrq->flags = 1;
+	}
+
+	return 0;
+}
+#endif 
+
+#ifdef WL_IW_USE_ISCAN
+static int
+wl_iw_iscan_get_aplist(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_scan_results_t *list;
+	iscan_buf_t * buf;
+	iscan_info_t *iscan = g_iscan;
+
+	struct sockaddr *addr = (struct sockaddr *) extra;
+	struct iw_quality qual[IW_MAX_AP];
+	wl_bss_info_t *bi = NULL;
+	int i;
+
+	WL_TRACE(("%s: SIOCGIWAPLIST\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	if ((!iscan) || (iscan->tsk_ctl.thr_pid < 0)) {
+		WL_ERROR(("%s error\n", __FUNCTION__));
+		return 0;
+	}
+
+	buf = iscan->list_hdr;
+	
+	while (buf) {
+		list = &((wl_iscan_results_t*)buf->iscan_buf)->results;
+		if (list->version != WL_BSS_INFO_VERSION) {
+			WL_ERROR(("%s : list->version %d != WL_BSS_INFO_VERSION\n",
+				__FUNCTION__, list->version));
+			return -EINVAL;
+		}
+
+		bi = NULL;
+		for (i = 0, dwrq->length = 0; i < list->count && dwrq->length < IW_MAX_AP; i++) {
+			bi = bi ? (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length))
+			          : list->bss_info;
+			ASSERT(((uintptr)bi + dtoh32(bi->length)) <= ((uintptr)list +
+				WLC_IW_ISCAN_MAXLEN));
+
+			
+			if (!(dtoh16(bi->capability) & DOT11_CAP_ESS))
+				continue;
+
+			
+			memcpy(addr[dwrq->length].sa_data, &bi->BSSID, ETHER_ADDR_LEN);
+			addr[dwrq->length].sa_family = ARPHRD_ETHER;
+			qual[dwrq->length].qual = rssi_to_qual(dtoh16(bi->RSSI));
+			qual[dwrq->length].level = 0x100 + dtoh16(bi->RSSI);
+			qual[dwrq->length].noise = 0x100 + bi->phy_noise;
+
+			
+#if WIRELESS_EXT > 18
+			qual[dwrq->length].updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
+#else
+			qual[dwrq->length].updated = 7;
+#endif 
+
+			dwrq->length++;
+		}
+		buf = buf->next;
+	}
+	if (dwrq->length) {
+		memcpy(&addr[dwrq->length], qual, sizeof(struct iw_quality) * dwrq->length);
+		
+		dwrq->flags = 1;
+	}
+
+	return 0;
+}
+
+static int
+wl_iw_iscan_prep(wl_scan_params_t *params, wlc_ssid_t *ssid)
+{
+	int err = 0;
+
+	memcpy(&params->bssid, &ether_bcast, ETHER_ADDR_LEN);
+	params->bss_type = DOT11_BSSTYPE_ANY;
+	params->scan_type = 0;
+	params->nprobes = -1;
+	params->active_time = -1;
+	params->passive_time = -1;
+	params->home_time = -1;
+	params->channel_num = 0;
+
+#if defined(CONFIG_FIRST_SCAN)
+	
+	if (g_first_broadcast_scan == BROADCAST_SCAN_FIRST_STARTED)
+		params->passive_time = 30;
+#endif 
+	params->nprobes = htod32(params->nprobes);
+	params->active_time = htod32(params->active_time);
+	params->passive_time = htod32(params->passive_time);
+	params->home_time = htod32(params->home_time);
+	if (ssid && ssid->SSID_len)
+		memcpy(&params->ssid, ssid, sizeof(wlc_ssid_t));
+
+	return err;
+}
+
+static int
+wl_iw_iscan(iscan_info_t *iscan, wlc_ssid_t *ssid, uint16 action)
+{
+	int err = 0;
+
+	iscan->iscan_ex_params_p->version = htod32(ISCAN_REQ_VERSION);
+	iscan->iscan_ex_params_p->action = htod16(action);
+	iscan->iscan_ex_params_p->scan_duration = htod16(0);
+
+	WL_SCAN(("%s : nprobes=%d\n", __FUNCTION__, iscan->iscan_ex_params_p->params.nprobes));
+	WL_SCAN(("active_time=%d\n", iscan->iscan_ex_params_p->params.active_time));
+	WL_SCAN(("passive_time=%d\n", iscan->iscan_ex_params_p->params.passive_time));
+	WL_SCAN(("home_time=%d\n", iscan->iscan_ex_params_p->params.home_time));
+	WL_SCAN(("scan_type=%d\n", iscan->iscan_ex_params_p->params.scan_type));
+	WL_SCAN(("bss_type=%d\n", iscan->iscan_ex_params_p->params.bss_type));
+
+	if ((dev_iw_iovar_setbuf(iscan->dev, "iscan", iscan->iscan_ex_params_p,
+		iscan->iscan_ex_param_size, iscan->ioctlbuf, sizeof(iscan->ioctlbuf)))) {
+			WL_ERROR(("Set ISCAN for %s failed with %d\n", __FUNCTION__, err));
+			err = -1;
+	}
+
+	return err;
+}
+
+static void
+wl_iw_timerfunc(ulong data)
+{
+	iscan_info_t *iscan = (iscan_info_t *)data;
+	if (iscan) {
+		iscan->timer_on = 0;
+		if (iscan->iscan_state != ISCAN_STATE_IDLE) {
+			WL_TRACE(("timer trigger\n"));
+			up(&iscan->tsk_ctl.sema);
+		}
+	}
+}
+
+static void
+wl_iw_set_event_mask(struct net_device *dev)
+{
+	char eventmask[WL_EVENTING_MASK_LEN];
+	char iovbuf[WL_EVENTING_MASK_LEN + 12];	
+
+	dev_iw_iovar_getbuf(dev, "event_msgs", "", 0, iovbuf, sizeof(iovbuf));
+	bcopy(iovbuf, eventmask, WL_EVENTING_MASK_LEN);
+	setbit(eventmask, WLC_E_SCAN_COMPLETE);
+	dev_iw_iovar_setbuf(dev, "event_msgs", eventmask, WL_EVENTING_MASK_LEN,
+		iovbuf, sizeof(iovbuf));
+}
+
+static uint32
+wl_iw_iscan_get(iscan_info_t *iscan)
+{
+	iscan_buf_t * buf;
+	iscan_buf_t * ptr;
+	wl_iscan_results_t * list_buf;
+	wl_iscan_results_t list;
+	wl_scan_results_t *results;
+	uint32 status;
+	int res = 0;
+
+	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
+	if (iscan->list_cur) {
+		buf = iscan->list_cur;
+		iscan->list_cur = buf->next;
+	}
+	else {
+		buf = kmalloc(sizeof(iscan_buf_t), GFP_KERNEL);
+		if (!buf) {
+			WL_ERROR(("%s can't alloc iscan_buf_t : going to abort currect iscan\n",
+			          __FUNCTION__));
+			DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
+			return WL_SCAN_RESULTS_NO_MEM;
+		}
+		buf->next = NULL;
+		if (!iscan->list_hdr)
+			iscan->list_hdr = buf;
+		else {
+			ptr = iscan->list_hdr;
+			while (ptr->next) {
+				ptr = ptr->next;
+			}
+			ptr->next = buf;
+		}
+	}
+	memset(buf->iscan_buf, 0, WLC_IW_ISCAN_MAXLEN);
+	list_buf = (wl_iscan_results_t*)buf->iscan_buf;
+	results = &list_buf->results;
+	results->buflen = WL_ISCAN_RESULTS_FIXED_SIZE;
+	results->version = 0;
+	results->count = 0;
+
+	memset(&list, 0, sizeof(list));
+	list.results.buflen = htod32(WLC_IW_ISCAN_MAXLEN);
+	res = dev_iw_iovar_getbuf(
+		iscan->dev,
+		"iscanresults",
+		&list,
+		WL_ISCAN_RESULTS_FIXED_SIZE,
+		buf->iscan_buf,
+		WLC_IW_ISCAN_MAXLEN);
+	if (res == 0) {
+		results->buflen = dtoh32(results->buflen);
+		results->version = dtoh32(results->version);
+		results->count = dtoh32(results->count);
+		WL_TRACE(("results->count = %d\n", results->count));
+		WL_TRACE(("results->buflen = %d\n", results->buflen));
+		status = dtoh32(list_buf->status);
+	} else {
+		WL_ERROR(("%s returns error %d\n", __FUNCTION__, res));
+		
+		status = WL_SCAN_RESULTS_NO_MEM;
+	}
+	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
+	return status;
+}
+
+static void
+wl_iw_force_specific_scan(iscan_info_t *iscan)
+{
+	WL_TRACE(("%s force Specific SCAN for %s\n", __FUNCTION__, g_specific_ssid.SSID));
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	rtnl_lock();
+#endif
+
+	(void) dev_wlc_ioctl(iscan->dev, WLC_SCAN, &g_specific_ssid, sizeof(g_specific_ssid));
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	rtnl_unlock();
+#endif
+}
+
+static void
+wl_iw_send_scan_complete(iscan_info_t *iscan)
+{
+	union iwreq_data wrqu;
+
+	memset(&wrqu, 0, sizeof(wrqu));
+
+	
+	wireless_send_event(iscan->dev, SIOCGIWSCAN, &wrqu, NULL);
+#if defined(CONFIG_FIRST_SCAN)
+		if (g_first_broadcast_scan == BROADCAST_SCAN_FIRST_STARTED)
+			g_first_broadcast_scan = BROADCAST_SCAN_FIRST_RESULT_READY;
+#endif 
+		WL_TRACE(("Send Event ISCAN complete\n"));
+}
+
+static int
+_iscan_sysioc_thread(void *data)
+{
+	uint32 status;
+
+	tsk_ctl_t *tsk_ctl = (tsk_ctl_t *)data;
+	iscan_info_t *iscan = (iscan_info_t *) tsk_ctl->parent;
+
+
+	static bool iscan_pass_abort = FALSE;
+
+	DAEMONIZE("iscan_sysioc");
+
+	status = WL_SCAN_RESULTS_PARTIAL;
+
+	
+	complete(&tsk_ctl->completed);
+
+	while (down_interruptible(&tsk_ctl->sema) == 0) {
+
+		SMP_RD_BARRIER_DEPENDS();
+		if (tsk_ctl->terminated) {
+			break;
+		}
+#if defined(SOFTAP)
+		
+		if (ap_cfg_running) {
+		 WL_TRACE(("%s skipping SCAN ops in AP mode !!!\n", __FUNCTION__));
+		 net_os_wake_unlock(iscan->dev);
+		 continue;
+		}
+#endif 
+
+		if (iscan->timer_on) {
+			
+			iscan->timer_on = 0;
+			del_timer_sync(&iscan->timer);
+		}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+		rtnl_lock();
+#endif
+		status = wl_iw_iscan_get(iscan);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+		rtnl_unlock();
+#endif
+
+	if  (g_scan_specified_ssid && (iscan_pass_abort == TRUE)) {
+		WL_TRACE(("%s Get results from specific scan status=%d\n", __FUNCTION__, status));
+			wl_iw_send_scan_complete(iscan);
+			iscan_pass_abort = FALSE;
+			status  = -1;
+		}
+
+		switch (status) {
+			case WL_SCAN_RESULTS_PARTIAL:
+				WL_TRACE(("iscanresults incomplete\n"));
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+				rtnl_lock();
+#endif
+				
+				wl_iw_iscan(iscan, NULL, WL_SCAN_ACTION_CONTINUE);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+				rtnl_unlock();
+#endif
+				
+				mod_timer(&iscan->timer, jiffies + iscan->timer_ms*HZ/1000);
+				iscan->timer_on = 1;
+				break;
+			case WL_SCAN_RESULTS_SUCCESS:
+				WL_TRACE(("iscanresults complete\n"));
+				iscan->iscan_state = ISCAN_STATE_IDLE;
+				wl_iw_send_scan_complete(iscan);
+				break;
+			case WL_SCAN_RESULTS_PENDING:
+				WL_TRACE(("iscanresults pending\n"));
+				
+				mod_timer(&iscan->timer, jiffies + iscan->timer_ms*HZ/1000);
+				iscan->timer_on = 1;
+				break;
+			case WL_SCAN_RESULTS_ABORTED:
+				WL_TRACE(("iscanresults aborted\n"));
+				iscan->iscan_state = ISCAN_STATE_IDLE;
+				if (g_scan_specified_ssid == 0)
+					wl_iw_send_scan_complete(iscan);
+				else {
+					iscan_pass_abort = TRUE;
+					wl_iw_force_specific_scan(iscan);
+				}
+				break;
+			case WL_SCAN_RESULTS_NO_MEM:
+				WL_TRACE(("iscanresults can't alloc memory: skip\n"));
+				iscan->iscan_state = ISCAN_STATE_IDLE;
+				break;
+			default:
+				WL_TRACE(("iscanresults returned unknown status %d\n", status));
+				break;
+		 }
+
+		net_os_wake_unlock(iscan->dev);
+	}
+
+	if (iscan->timer_on) {
+		iscan->timer_on = 0;
+		del_timer_sync(&iscan->timer);
+	}
+	complete_and_exit(&tsk_ctl->completed, 0);
+}
+#endif 
+
+#if !defined(CSCAN)
+
+static void
+wl_iw_set_ss_cache_timer_flag(void)
+{
+	g_ss_cache_ctrl.m_timer_expired = 1;
+	WL_TRACE(("%s called\n", __FUNCTION__));
+}
+
+
+static int
+wl_iw_init_ss_cache_ctrl(void)
+{
+	WL_TRACE(("%s :\n", __FUNCTION__));
+	g_ss_cache_ctrl.m_prev_scan_mode = 0;
+	g_ss_cache_ctrl.m_cons_br_scan_cnt = 0;
+	g_ss_cache_ctrl.m_cache_head = NULL;
+	g_ss_cache_ctrl.m_link_down = 0;
+	g_ss_cache_ctrl.m_timer_expired = 0;
+	memset(g_ss_cache_ctrl.m_active_bssid, 0, ETHER_ADDR_LEN);
+
+	g_ss_cache_ctrl.m_timer = kmalloc(sizeof(struct timer_list), GFP_KERNEL);
+	if (!g_ss_cache_ctrl.m_timer) {
+		return -ENOMEM;
+	}
+	g_ss_cache_ctrl.m_timer->function = (void *)wl_iw_set_ss_cache_timer_flag;
+	init_timer(g_ss_cache_ctrl.m_timer);
+
+	return 0;
+}
+
+
+
+static void
+wl_iw_free_ss_cache(void)
+{
+	wl_iw_ss_cache_t *node, *cur;
+	wl_iw_ss_cache_t **spec_scan_head;
+
+	WL_TRACE(("%s called\n", __FUNCTION__));
+
+	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
+	spec_scan_head = &g_ss_cache_ctrl.m_cache_head;
+	node = *spec_scan_head;
+
+	for (;node;) {
+		WL_TRACE(("%s : SSID - %s\n", __FUNCTION__, node->bss_info->SSID));
+		cur = node;
+		node = cur->next;
+		kfree(cur);
+	}
+	*spec_scan_head = NULL;
+	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
+}
+
+
+
+static int
+wl_iw_run_ss_cache_timer(int kick_off)
+{
+	struct timer_list **timer;
+
+	timer = &g_ss_cache_ctrl.m_timer;
+
+	if (*timer) {
+		if (kick_off) {
+#ifdef CONFIG_PRESCANNED
+			(*timer)->expires = jiffies + 70000 * HZ / 1000;
+#else
+			(*timer)->expires = jiffies + 30000 * HZ / 1000;	
+#endif
+			add_timer(*timer);
+			WL_TRACE(("%s : timer starts \n", __FUNCTION__));
+		} else {
+			del_timer_sync(*timer);
+			WL_TRACE(("%s : timer stops \n", __FUNCTION__));
+		}
+	}
+
+	return 0;
+}
+
+
+static void
+wl_iw_release_ss_cache_ctrl(void)
+{
+	WL_TRACE(("%s :\n", __FUNCTION__));
+	wl_iw_free_ss_cache();
+	wl_iw_run_ss_cache_timer(0);
+	if (g_ss_cache_ctrl.m_timer) {
+		kfree(g_ss_cache_ctrl.m_timer);
+	}
+}
+
+
+
+static void
+wl_iw_reset_ss_cache(void)
+{
+	wl_iw_ss_cache_t *node, *prev, *cur;
+	wl_iw_ss_cache_t **spec_scan_head;
+
+	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
+	spec_scan_head = &g_ss_cache_ctrl.m_cache_head;
+	node = *spec_scan_head;
+	prev = node;
+
+	for (;node;) {
+		WL_TRACE(("%s : node SSID %s \n", __FUNCTION__, node->bss_info->SSID));
+		if (!node->dirty) {
+			cur = node;
+			if (cur == *spec_scan_head) {
+				*spec_scan_head = cur->next;
+				prev = *spec_scan_head;
+			}
+			else {
+				prev->next = cur->next;
+			}
+			node = cur->next;
+
+			WL_TRACE(("%s : Del node : SSID %s\n", __FUNCTION__, cur->bss_info->SSID));
+			kfree(cur);
+			continue;
+		}
+
+		node->dirty = 0;
+		prev = node;
+		node = node->next;
+	}
+	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
+}
+
+
+static int
+wl_iw_add_bss_to_ss_cache(wl_scan_results_t *ss_list)
+{
+
+	wl_iw_ss_cache_t *node, *prev, *leaf;
+	wl_iw_ss_cache_t **spec_scan_head;
+	wl_bss_info_t *bi = NULL;
+	int i;
+
+	
+	if (!ss_list->count) {
+		return 0;
+	}
+
+	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
+	spec_scan_head = &g_ss_cache_ctrl.m_cache_head;
+
+	for (i = 0; i < ss_list->count; i++) {
+
+		node = *spec_scan_head;
+		prev = node;
+
+		bi = bi ? (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length)) : ss_list->bss_info;
+
+		WL_TRACE(("%s : find %d with specific SSID %s\n", __FUNCTION__, i, bi->SSID));
+		for (;node;) {
+			if (!memcmp(&node->bss_info->BSSID, &bi->BSSID, ETHER_ADDR_LEN)) {
+				
+				WL_TRACE(("dirty marked : SSID %s\n", bi->SSID));
+				node->dirty = 1;
+				break;
+			}
+			prev = node;
+			node = node->next;
+		}
+
+		if (node) {
+			continue;
+		}
+
+		leaf = kmalloc(bi->length + WLC_IW_SS_CACHE_CTRL_FIELD_MAXLEN, GFP_KERNEL);
+		if (!leaf) {
+			WL_ERROR(("Memory alloc failure %d\n",
+				bi->length + WLC_IW_SS_CACHE_CTRL_FIELD_MAXLEN));
+			DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
+			return -ENOMEM;
+		}
+
+		memcpy(leaf->bss_info, bi, bi->length);
+		leaf->next = NULL;
+		leaf->dirty = 1;
+		leaf->count = 1;
+		leaf->version = ss_list->version;
+
+		if (!prev) {
+			*spec_scan_head = leaf;
+		}
+		else {
+			prev->next = leaf;
+		}
+	}
+	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
+	return 0;
+}
+
+
+static int
+wl_iw_merge_scan_cache(struct iw_request_info *info, char *extra, uint buflen_from_user,
+__u16 *merged_len)
+{
+	wl_iw_ss_cache_t *node;
+	wl_scan_results_t *list_merge;
+
+	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
+	node = g_ss_cache_ctrl.m_cache_head;
+	for (;node;) {
+		list_merge = (wl_scan_results_t *)&node->buflen;
+		WL_TRACE(("%s: Cached Specific APs list=%d\n", __FUNCTION__, list_merge->count));
+		if (buflen_from_user - *merged_len > 0) {
+			*merged_len += (__u16) wl_iw_get_scan_prep(list_merge, info,
+				extra + *merged_len, buflen_from_user - *merged_len);
+		}
+		else {
+			WL_TRACE(("%s: exit with break\n", __FUNCTION__));
+			break;
+		}
+		node = node->next;
+	}
+	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
+	return 0;
+}
+
+
+static int
+wl_iw_delete_bss_from_ss_cache(void *addr)
+{
+
+	wl_iw_ss_cache_t *node, *prev;
+	wl_iw_ss_cache_t **spec_scan_head;
+
+	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
+	spec_scan_head = &g_ss_cache_ctrl.m_cache_head;
+	node = *spec_scan_head;
+	prev = node;
+	for (;node;) {
+		if (!memcmp(&node->bss_info->BSSID, addr, ETHER_ADDR_LEN)) {
+			if (node == *spec_scan_head) {
+				*spec_scan_head = node->next;
+			}
+			else {
+				prev->next = node->next;
+			}
+
+			WL_TRACE(("%s : Del node : %s\n", __FUNCTION__, node->bss_info->SSID));
+			kfree(node);
+			break;
+		}
+
+		prev = node;
+		node = node->next;
+	}
+
+	memset(addr, 0, ETHER_ADDR_LEN);
+	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
+	return 0;
+}
+
+#endif	
+
+static int
+wl_iw_set_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int error;
+	WL_TRACE(("\n:%s dev:%s: SIOCSIWSCAN : SCAN\n", __FUNCTION__, dev->name));
+
+#ifdef OEM_CHROMIUMOS
+	g_set_essid_before_scan = FALSE;
+#endif
+
+#if defined(CSCAN)
+		WL_ERROR(("%s: Scan from SIOCGIWSCAN not supported\n", __FUNCTION__));
+		return -EINVAL;
+#endif 
+
+#if defined(SOFTAP)
+	
+	if (ap_cfg_running) {
+		WL_TRACE(("\n>%s: Not executed, reason -'SOFTAP is active'\n", __FUNCTION__));
+		return 0;
+	}
+#endif 
+
+	
+	if (g_onoff == G_WLAN_SET_OFF)
+		return 0;
+
+	
+	memset(&g_specific_ssid, 0, sizeof(g_specific_ssid));
+#ifndef WL_IW_USE_ISCAN
+	
+	g_scan_specified_ssid = 0;
+#endif 
+
+#if WIRELESS_EXT > 17
+	
+	if (wrqu->data.length == sizeof(struct iw_scan_req)) {
+		if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
+			struct iw_scan_req *req = (struct iw_scan_req *)extra;
+#if defined(CONFIG_FIRST_SCAN)
+			if (g_first_broadcast_scan != BROADCAST_SCAN_FIRST_RESULT_CONSUMED) {
+				
+				WL_TRACE(("%s Ignoring SC %s first BC is not done = %d\n",
+				          __FUNCTION__, req->essid,
+				          g_first_broadcast_scan));
+				return -EBUSY;
+			}
+#endif	
+			if (g_scan_specified_ssid) {
+				WL_TRACE(("%s Specific SCAN is not done ignore scan for = %s \n",
+					__FUNCTION__, req->essid));
+				
+				return -EBUSY;
+			}
+			else {
+				g_specific_ssid.SSID_len = MIN(sizeof(g_specific_ssid.SSID),
+				                               req->essid_len);
+				memcpy(g_specific_ssid.SSID, req->essid, g_specific_ssid.SSID_len);
+				g_specific_ssid.SSID_len = htod32(g_specific_ssid.SSID_len);
+				g_scan_specified_ssid = 1;
+				WL_TRACE(("### Specific scan ssid=%s len=%d\n",
+				          g_specific_ssid.SSID, g_specific_ssid.SSID_len));
+			}
+		}
+	}
+#endif 
+	
+	if ((error = dev_wlc_ioctl(dev, WLC_SCAN, &g_specific_ssid, sizeof(g_specific_ssid)))) {
+		WL_TRACE(("#### Set SCAN for %s failed with %d\n", g_specific_ssid.SSID, error));
+		
+		g_scan_specified_ssid = 0;
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
+#ifdef WL_IW_USE_ISCAN
+int
+wl_iw_iscan_set_scan_broadcast_prep(struct net_device *dev, uint flag)
+{
+	wlc_ssid_t ssid;
+	iscan_info_t *iscan = g_iscan;
+
+#if defined(CONFIG_FIRST_SCAN)
+	
+	if (g_first_broadcast_scan == BROADCAST_SCAN_FIRST_IDLE) {
+		g_first_broadcast_scan = BROADCAST_SCAN_FIRST_STARTED;
+		WL_TRACE(("%s: First Brodcast scan was forced\n", __FUNCTION__));
+	}
+	else if (g_first_broadcast_scan == BROADCAST_SCAN_FIRST_STARTED) {
+		WL_TRACE(("%s: ignore ISCAN request first BS is not done yet\n", __FUNCTION__));
+		return 0;
+	}
+#endif 
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	if (flag)
+		rtnl_lock();
+#endif
+
+	dev_wlc_ioctl(dev, WLC_SET_PASSIVE_SCAN, &iscan->scan_flag, sizeof(iscan->scan_flag));
+	wl_iw_set_event_mask(dev);
+
+	WL_TRACE(("+++: Set Broadcast ISCAN\n"));
+	
+	memset(&ssid, 0, sizeof(ssid));
+
+	iscan->list_cur = iscan->list_hdr;
+	iscan->iscan_state = ISCAN_STATE_SCANING;
+
+	memset(&iscan->iscan_ex_params_p->params, 0, iscan->iscan_ex_param_size);
+	wl_iw_iscan_prep(&iscan->iscan_ex_params_p->params, &ssid);
+	wl_iw_iscan(iscan, &ssid, WL_SCAN_ACTION_START);
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	if (flag)
+		rtnl_unlock();
+#endif
+
+	mod_timer(&iscan->timer, jiffies + iscan->timer_ms*HZ/1000);
+
+	iscan->timer_on = 1;
+
+	return 0;
+}
+
+static int
+wl_iw_iscan_set_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	wlc_ssid_t ssid;
+	iscan_info_t *iscan = g_iscan;
+	int ret = 0;
+
+	WL_TRACE_SCAN(("%s: SIOCSIWSCAN : ISCAN\n", dev->name));
+
+#if defined(CSCAN)
+		WL_ERROR(("%s: Scan from SIOCGIWSCAN not supported\n", __FUNCTION__));
+		return -EINVAL;
+#endif 
+
+	net_os_wake_lock(dev);
+
+	
+#if defined(SOFTAP)
+	if (ap_cfg_running) {
+		WL_TRACE(("\n>%s: Not executed, reason -'SOFTAP is active'\n", __FUNCTION__));
+		goto set_scan_end;
+	}
+#endif
+	
+	if (g_onoff == G_WLAN_SET_OFF) {
+		WL_TRACE(("%s: driver is not up yet after START\n", __FUNCTION__));
+		goto set_scan_end;
+	}
+
+#ifdef PNO_SUPPORT
+	
+	if  (dhd_dev_get_pno_status(dev)) {
+		WL_ERROR(("%s: Scan called when PNO is active\n", __FUNCTION__));
+	}
+#endif 
+
+	
+	if ((!iscan) || (iscan->tsk_ctl.thr_pid < 0)) {
+		WL_ERROR(("%s error \n",  __FUNCTION__));
+		goto set_scan_end;
+	}
+
+	if (g_scan_specified_ssid) {
+		WL_TRACE(("%s Specific SCAN already running ignoring BC scan\n",
+		          __FUNCTION__));
+		ret = EBUSY;
+		goto set_scan_end;
+	}
+
+	
+	memset(&ssid, 0, sizeof(ssid));
+
+#if WIRELESS_EXT > 17
+	
+	if (wrqu->data.length == sizeof(struct iw_scan_req)) {
+		if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
+			int as = 0;
+			struct iw_scan_req *req = (struct iw_scan_req *)extra;
+			
+			ssid.SSID_len = MIN(sizeof(ssid.SSID), req->essid_len);
+			memcpy(ssid.SSID, req->essid, ssid.SSID_len);
+			ssid.SSID_len = htod32(ssid.SSID_len);
+			dev_wlc_ioctl(dev, WLC_SET_PASSIVE_SCAN, &as, sizeof(as));
+			wl_iw_set_event_mask(dev);
+			ret = wl_iw_set_scan(dev, info, wrqu, extra);
+			goto set_scan_end;
+		}
+		else {
+			g_scan_specified_ssid = 0;
+
+			if (iscan->iscan_state == ISCAN_STATE_SCANING) {
+				WL_TRACE(("%s ISCAN already in progress \n", __FUNCTION__));
+				goto set_scan_end;
+			}
+		}
+	}
+#endif 
+
+#if defined(CONFIG_FIRST_SCAN) && !defined(CSCAN)
+	if (g_first_broadcast_scan < BROADCAST_SCAN_FIRST_RESULT_CONSUMED) {
+		if (++g_first_counter_scans == MAX_ALLOWED_BLOCK_SCAN_FROM_FIRST_SCAN) {
+
+			WL_ERROR(("%s Clean up First scan flag which is %d\n",
+			          __FUNCTION__, g_first_broadcast_scan));
+			g_first_broadcast_scan = BROADCAST_SCAN_FIRST_RESULT_CONSUMED;
+		}
+		else {
+			WL_ERROR(("%s Ignoring Broadcast Scan:First Scan is not done yet %d\n",
+			          __FUNCTION__, g_first_counter_scans));
+			ret = -EBUSY;
+			goto set_scan_end;
+		}
+	}
+#endif
+
+	wl_iw_iscan_set_scan_broadcast_prep(dev, 0);
+
+set_scan_end:
+	net_os_wake_unlock(dev);
+	return ret;
+}
+#endif 
+
+#if WIRELESS_EXT > 17
+static bool
+ie_is_wpa_ie(uint8 **wpaie, uint8 **tlvs, int *tlvs_len)
+{
+
+
+	uint8 *ie = *wpaie;
+
+	
+	if ((ie[1] >= 6) &&
+		!bcmp((const void *)&ie[2], (const void *)(WPA_OUI "\x01"), 4)) {
+		return TRUE;
+	}
+
+	
+	ie += ie[1] + 2;
+	
+	*tlvs_len -= (int)(ie - *tlvs);
+	
+	*tlvs = ie;
+	return FALSE;
+}
+
+static bool
+ie_is_wps_ie(uint8 **wpsie, uint8 **tlvs, int *tlvs_len)
+{
+
+
+	uint8 *ie = *wpsie;
+
+	
+	if ((ie[1] >= 4) &&
+		!bcmp((const void *)&ie[2], (const void *)(WPA_OUI "\x04"), 4)) {
+		return TRUE;
+	}
+
+	
+	ie += ie[1] + 2;
+	
+	*tlvs_len -= (int)(ie - *tlvs);
+	
+	*tlvs = ie;
+	return FALSE;
+}
+#endif 
+
+
+static int
+wl_iw_handle_scanresults_ies(char **event_p, char *end,
+	struct iw_request_info *info, wl_bss_info_t *bi)
+{
+#if WIRELESS_EXT > 17
+	struct iw_event	iwe;
+	char *event;
+
+	event = *event_p;
+	if (bi->ie_length) {
+		
+		bcm_tlv_t *ie;
+		uint8 *ptr = ((uint8 *)bi) + sizeof(wl_bss_info_t);
+		int ptr_len = bi->ie_length;
+
+		if ((ie = bcm_parse_tlvs(ptr, ptr_len, DOT11_MNG_RSN_ID))) {
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = ie->len + 2;
+			event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)ie);
+		}
+		ptr = ((uint8 *)bi) + sizeof(wl_bss_info_t);
+
+		while ((ie = bcm_parse_tlvs(ptr, ptr_len, DOT11_MNG_WPA_ID))) {
+			
+			if (ie_is_wps_ie(((uint8 **)&ie), &ptr, &ptr_len)) {
+				iwe.cmd = IWEVGENIE;
+				iwe.u.data.length = ie->len + 2;
+				event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)ie);
+				break;
+			}
+		}
+
+		ptr = ((uint8 *)bi) + sizeof(wl_bss_info_t);
+		ptr_len = bi->ie_length;
+		while ((ie = bcm_parse_tlvs(ptr, ptr_len, DOT11_MNG_WPA_ID))) {
+			if (ie_is_wpa_ie(((uint8 **)&ie), &ptr, &ptr_len)) {
+				iwe.cmd = IWEVGENIE;
+				iwe.u.data.length = ie->len + 2;
+				event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)ie);
+				break;
+			}
+		}
+
+	*event_p = event;
+	}
+#endif 
+
+	return 0;
+}
+
+#ifndef CSCAN
+static uint
+wl_iw_get_scan_prep(
+	wl_scan_results_t *list,
+	struct iw_request_info *info,
+	char *extra,
+	short max_size)
+{
+	int  i, j;
+	struct iw_event  iwe;
+	wl_bss_info_t *bi = NULL;
+	char *event = extra, *end = extra + max_size - WE_ADD_EVENT_FIX, *value;
+	int	ret = 0;
+
+	if (!list) {
+		WL_ERROR(("%s: Null list pointer", __FUNCTION__));
+		return ret;
+	}
+
+	
+
+	for (i = 0; i < list->count && i < IW_MAX_AP; i++) {
+		if (list->version != WL_BSS_INFO_VERSION) {
+			WL_ERROR(("%s : list->version %d != WL_BSS_INFO_VERSION\n",
+			          __FUNCTION__, list->version));
+			return ret;
+		}
+
+		bi = bi ? (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length)) : list->bss_info;
+
+		WL_TRACE(("%s : %s\n", __FUNCTION__, bi->SSID));
+
+		
+		iwe.cmd = SIOCGIWAP;
+		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+		memcpy(iwe.u.ap_addr.sa_data, &bi->BSSID, ETHER_ADDR_LEN);
+		event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_ADDR_LEN);
+		
+		iwe.u.data.length = dtoh32(bi->SSID_len);
+		iwe.cmd = SIOCGIWESSID;
+		iwe.u.data.flags = 1;
+		event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, bi->SSID);
+
+		
+		if (dtoh16(bi->capability) & (DOT11_CAP_ESS | DOT11_CAP_IBSS)) {
+			iwe.cmd = SIOCGIWMODE;
+			if (dtoh16(bi->capability) & DOT11_CAP_ESS)
+				iwe.u.mode = IW_MODE_INFRA;
+			else
+				iwe.u.mode = IW_MODE_ADHOC;
+			event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_UINT_LEN);
+		}
+
+		
+		iwe.cmd = SIOCGIWFREQ;
+		iwe.u.freq.m = wf_channel2mhz(CHSPEC_CHANNEL(bi->chanspec),
+			CHSPEC_CHANNEL(bi->chanspec) <= CH_MAX_2G_CHANNEL ?
+			WF_CHAN_FACTOR_2_4_G : WF_CHAN_FACTOR_5_G);
+		iwe.u.freq.e = 6;
+		event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_FREQ_LEN);
+
+		
+		iwe.cmd = IWEVQUAL;
+		iwe.u.qual.qual = rssi_to_qual(dtoh16(bi->RSSI));
+		iwe.u.qual.level = 0x100 + dtoh16(bi->RSSI);
+		iwe.u.qual.noise = 0x100 + bi->phy_noise;
+		event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_QUAL_LEN);
+
+		
+		 wl_iw_handle_scanresults_ies(&event, end, info, bi);
+
+		
+		iwe.cmd = SIOCGIWENCODE;
+		if (dtoh16(bi->capability) & DOT11_CAP_PRIVACY)
+			iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+		else
+			iwe.u.data.flags = IW_ENCODE_DISABLED;
+		iwe.u.data.length = 0;
+		event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)event);
+
+		
+		if (bi->rateset.count) {
+			if (((event -extra) + IW_EV_LCP_LEN) <= (uintptr)end) {
+				value = event + IW_EV_LCP_LEN;
+				iwe.cmd = SIOCGIWRATE;
+				
+				iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
+				for (j = 0; j < bi->rateset.count && j < IW_MAX_BITRATES; j++) {
+					iwe.u.bitrate.value =
+						(bi->rateset.rates[j] & 0x7f) * 500000;
+					value = IWE_STREAM_ADD_VALUE(info, event, value, end, &iwe,
+						IW_EV_PARAM_LEN);
+				}
+				event = value;
+			}
+		}
+	}
+
+	if ((ret = (event - extra)) < 0) {
+		WL_ERROR(("==> Wrong size\n"));
+		ret = 0;
+	}
+
+	WL_TRACE(("%s: size=%d bytes prepared \n", __FUNCTION__, (unsigned int)(event - extra)));
+	return (uint)ret;
+}
+
+static int
+wl_iw_get_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	channel_info_t ci;
+	wl_scan_results_t *list_merge;
+	wl_scan_results_t *list = (wl_scan_results_t *) g_scan;
+	int error;
+	uint buflen_from_user = dwrq->length;
+	uint len =  G_SCAN_RESULTS;
+	__u16 len_ret = 0;
+#if  !defined(CSCAN)
+	__u16 merged_len = 0;
+#endif
+#if defined(WL_IW_USE_ISCAN)
+	iscan_info_t *iscan = g_iscan;
+	iscan_buf_t * p_buf;
+#if  !defined(CSCAN)
+	uint32 counter = 0;
+#endif 
+#endif 
+
+	WL_TRACE(("%s: buflen_from_user %d: \n", dev->name, buflen_from_user));
+
+	if (!extra) {
+		WL_TRACE(("%s: wl_iw_get_scan return -EINVAL\n", dev->name));
+		return -EINVAL;
+	}
+
+	
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_CHANNEL, &ci, sizeof(ci))))
+		return error;
+	ci.scan_channel = dtoh32(ci.scan_channel);
+	if (ci.scan_channel)
+		return -EAGAIN;
+
+#if  !defined(CSCAN)
+	if (g_ss_cache_ctrl.m_timer_expired) {
+		wl_iw_free_ss_cache();
+		g_ss_cache_ctrl.m_timer_expired ^= 1;
+	}
+	if ((!g_scan_specified_ssid && g_ss_cache_ctrl.m_prev_scan_mode) ||
+		g_ss_cache_ctrl.m_cons_br_scan_cnt > 4) {
+		g_ss_cache_ctrl.m_cons_br_scan_cnt = 0;
+		
+		wl_iw_reset_ss_cache();
+	}
+	g_ss_cache_ctrl.m_prev_scan_mode = g_scan_specified_ssid;
+	if (g_scan_specified_ssid) {
+		g_ss_cache_ctrl.m_cons_br_scan_cnt = 0;
+	}
+	else {
+		g_ss_cache_ctrl.m_cons_br_scan_cnt++;
+	}
+#endif 
+
+
+	
+	if (g_scan_specified_ssid) {
+		
+		list = kmalloc(len, GFP_KERNEL);
+		if (!list) {
+			WL_TRACE(("%s: wl_iw_get_scan return -ENOMEM\n", dev->name));
+			g_scan_specified_ssid = 0;
+			return -ENOMEM;
+		}
+	}
+
+	memset(list, 0, len);
+	list->buflen = htod32(len);
+	if ((error = dev_wlc_ioctl(dev, WLC_SCAN_RESULTS, list, len))) {
+		WL_ERROR(("%s: %s : Scan_results ERROR %d\n", dev->name, __FUNCTION__, error));
+		dwrq->length = len;
+		if (g_scan_specified_ssid) {
+			g_scan_specified_ssid = 0;
+			kfree(list);
+		}
+		return 0;
+	}
+	list->buflen = dtoh32(list->buflen);
+	list->version = dtoh32(list->version);
+	list->count = dtoh32(list->count);
+
+	
+	if (list->version != WL_BSS_INFO_VERSION) {
+		WL_ERROR(("%s : list->version %d != WL_BSS_INFO_VERSION\n",
+		          __FUNCTION__, list->version));
+		if (g_scan_specified_ssid) {
+			g_scan_specified_ssid = 0;
+			kfree(list);
+		}
+		return -EINVAL;
+	}
+
+#if  !defined(CSCAN)
+	if (g_scan_specified_ssid) {
+		
+		wl_iw_add_bss_to_ss_cache(list);
+		kfree(list);
+	}
+#endif
+
+#if  !defined(CSCAN)
+	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
+#if defined(WL_IW_USE_ISCAN)
+	if (g_scan_specified_ssid)
+		WL_TRACE(("%s: Specified scan APs from scan=%d\n", __FUNCTION__, list->count));
+	p_buf = iscan->list_hdr;
+	
+	while (p_buf != iscan->list_cur) {
+		list_merge = &((wl_iscan_results_t*)p_buf->iscan_buf)->results;
+		WL_TRACE(("%s: Bcast APs list=%d\n", __FUNCTION__, list_merge->count));
+		counter += list_merge->count;
+		if (list_merge->count > 0)
+			len_ret += (__u16) wl_iw_get_scan_prep(list_merge, info,
+			    extra+len_ret, buflen_from_user -len_ret);
+		p_buf = p_buf->next;
+	}
+	WL_TRACE(("%s merged with total Bcast APs=%d\n", __FUNCTION__, counter));
+#else
+	list_merge = (wl_scan_results_t *) g_scan;
+	len_ret = (__u16) wl_iw_get_scan_prep(list_merge, info, extra, buflen_from_user);
+#endif 
+	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
+	if (g_ss_cache_ctrl.m_link_down) {
+		
+		wl_iw_delete_bss_from_ss_cache(g_ss_cache_ctrl.m_active_bssid);
+	}
+	
+	wl_iw_merge_scan_cache(info, extra+len_ret, buflen_from_user-len_ret, &merged_len);
+	len_ret += merged_len;
+	wl_iw_run_ss_cache_timer(0);
+	wl_iw_run_ss_cache_timer(1);
+#else	
+
+	
+	if (g_scan_specified_ssid) {
+		WL_TRACE(("%s: Specified scan APs in the list =%d\n", __FUNCTION__, list->count));
+		len_ret = (__u16) wl_iw_get_scan_prep(list, info, extra, buflen_from_user);
+		kfree(list);
+
+#if defined(WL_IW_USE_ISCAN)
+		p_buf = iscan->list_hdr;
+		
+		while (p_buf != iscan->list_cur) {
+			list_merge = &((wl_iscan_results_t*)p_buf->iscan_buf)->results;
+			WL_TRACE(("%s: Bcast APs list=%d\n", __FUNCTION__, list_merge->count));
+			if (list_merge->count > 0)
+				len_ret += (__u16) wl_iw_get_scan_prep(list_merge, info,
+				    extra+len_ret, buflen_from_user -len_ret);
+			p_buf = p_buf->next;
+		}
+#else
+		list_merge = (wl_scan_results_t *) g_scan;
+		WL_TRACE(("%s: Bcast APs list=%d\n", __FUNCTION__, list_merge->count));
+		if (list_merge->count > 0)
+			len_ret += (__u16) wl_iw_get_scan_prep(list_merge, info, extra+len_ret,
+				buflen_from_user -len_ret);
+#endif 
+	}
+	else {
+		list = (wl_scan_results_t *) g_scan;
+		len_ret = (__u16) wl_iw_get_scan_prep(list, info, extra, buflen_from_user);
+	}
+#endif	
+
+#if defined(WL_IW_USE_ISCAN)
+	
+	g_scan_specified_ssid = 0;
+#endif 
+	
+	if ((len_ret + WE_ADD_EVENT_FIX) < buflen_from_user)
+		len = len_ret;
+
+	dwrq->length = len;
+	dwrq->flags = 0;	
+
+	WL_TRACE(("%s return to WE %d bytes APs=%d\n", __FUNCTION__, dwrq->length, list->count));
+	return 0;
+}
+#endif 
+
+#if defined(WL_IW_USE_ISCAN)
+static int
+wl_iw_iscan_get_scan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_scan_results_t *list;
+	struct iw_event	iwe;
+	wl_bss_info_t *bi = NULL;
+	int ii, j;
+	int apcnt;
+	char *event = extra, *end = extra + dwrq->length, *value;
+	iscan_info_t *iscan = g_iscan;
+	iscan_buf_t * p_buf;
+	uint32  counter = 0;
+	uint8   channel;
+#if !defined(CSCAN)
+	__u16 merged_len = 0;
+	uint buflen_from_user = dwrq->length;
+#endif
+
+	WL_TRACE(("%s %s buflen_from_user %d:\n", dev->name, __FUNCTION__, dwrq->length));
+
+#if defined(SOFTAP)
+	if (ap_cfg_running) {
+		WL_TRACE(("%s: Not executed, reason -'SOFTAP is active'\n", __FUNCTION__));
+		return -EINVAL;
+	}
+#endif
+
+	if (!extra) {
+		WL_TRACE(("%s: INVALID SIOCGIWSCAN GET bad parameter\n", dev->name));
+		return -EINVAL;
+	}
+
+#if defined(CONFIG_FIRST_SCAN)
+	if (g_first_broadcast_scan < BROADCAST_SCAN_FIRST_RESULT_READY) {
+		WL_TRACE(("%s %s: first ISCAN results are NOT ready yet \n",
+		          dev->name, __FUNCTION__));
+		return -EAGAIN;
+	}
+#endif	
+	
+	if ((!iscan) || (iscan->tsk_ctl.thr_pid < 0)) {
+		WL_ERROR(("%ssysioc_pid\n", __FUNCTION__));
+		return EAGAIN;
+	}
+
+	
+
+#if !defined(CSCAN)
+	if (g_ss_cache_ctrl.m_timer_expired) {
+		wl_iw_free_ss_cache();
+		g_ss_cache_ctrl.m_timer_expired ^= 1;
+	}
+	if (g_scan_specified_ssid) {
+		return wl_iw_get_scan(dev, info, dwrq, extra);
+	}
+	else {
+		if (g_ss_cache_ctrl.m_link_down) {
+			
+			wl_iw_delete_bss_from_ss_cache(g_ss_cache_ctrl.m_active_bssid);
+		}
+		if (g_ss_cache_ctrl.m_prev_scan_mode || g_ss_cache_ctrl.m_cons_br_scan_cnt > 4) {
+			g_ss_cache_ctrl.m_cons_br_scan_cnt = 0;
+			
+			wl_iw_reset_ss_cache();
+		}
+		g_ss_cache_ctrl.m_prev_scan_mode = g_scan_specified_ssid;
+		g_ss_cache_ctrl.m_cons_br_scan_cnt++;
+	}
+#endif 
+
+	WL_TRACE(("%s: SIOCGIWSCAN GET broadcast results\n", dev->name));
+	apcnt = 0;
+	p_buf = iscan->list_hdr;
+	
+	while (p_buf != iscan->list_cur) {
+		list = &((wl_iscan_results_t*)p_buf->iscan_buf)->results;
+
+		counter += list->count;
+
+		if (list->version != WL_BSS_INFO_VERSION) {
+			WL_ERROR(("%s : list->version %d != WL_BSS_INFO_VERSION\n",
+			          __FUNCTION__, list->version));
+			return -EINVAL;
+		}
+
+		bi = NULL;
+		for (ii = 0; ii < list->count && apcnt < IW_MAX_AP; apcnt++, ii++) {
+			bi = (bi ?
+			      (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length)) :
+			      list->bss_info);
+			ASSERT(((uintptr)bi + dtoh32(bi->length)) <= ((uintptr)list +
+			                                              WLC_IW_ISCAN_MAXLEN));
+
+			
+			if (event + ETHER_ADDR_LEN + bi->SSID_len +
+			    IW_EV_UINT_LEN + IW_EV_FREQ_LEN + IW_EV_QUAL_LEN >= end)
+				return -E2BIG;
+			
+			iwe.cmd = SIOCGIWAP;
+			iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+			memcpy(iwe.u.ap_addr.sa_data, &bi->BSSID, ETHER_ADDR_LEN);
+			event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_ADDR_LEN);
+
+			
+			iwe.u.data.length = dtoh32(bi->SSID_len);
+			iwe.cmd = SIOCGIWESSID;
+			iwe.u.data.flags = 1;
+			event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, bi->SSID);
+
+			
+			if (dtoh16(bi->capability) & (DOT11_CAP_ESS | DOT11_CAP_IBSS)) {
+				iwe.cmd = SIOCGIWMODE;
+				if (dtoh16(bi->capability) & DOT11_CAP_ESS)
+					iwe.u.mode = IW_MODE_INFRA;
+				else
+					iwe.u.mode = IW_MODE_ADHOC;
+				event = IWE_STREAM_ADD_EVENT(info, event, end,
+				                             &iwe, IW_EV_UINT_LEN);
+			}
+
+			
+			iwe.cmd = SIOCGIWFREQ;
+			channel = (bi->ctl_ch == 0) ? CHSPEC_CHANNEL(bi->chanspec) : bi->ctl_ch;
+			iwe.u.freq.m = wf_channel2mhz(channel,
+			                              channel <= CH_MAX_2G_CHANNEL ?
+			                              WF_CHAN_FACTOR_2_4_G : WF_CHAN_FACTOR_5_G);
+			iwe.u.freq.e = 6;
+			event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_FREQ_LEN);
+
+			
+			iwe.cmd = IWEVQUAL;
+			iwe.u.qual.qual = rssi_to_qual(dtoh16(bi->RSSI));
+			iwe.u.qual.level = 0x100 + dtoh16(bi->RSSI);
+			iwe.u.qual.noise = 0x100 + bi->phy_noise;
+			event = IWE_STREAM_ADD_EVENT(info, event, end, &iwe, IW_EV_QUAL_LEN);
+
+			
+			wl_iw_handle_scanresults_ies(&event, end, info, bi);
+
+			
+			iwe.cmd = SIOCGIWENCODE;
+			if (dtoh16(bi->capability) & DOT11_CAP_PRIVACY)
+				iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+			else
+				iwe.u.data.flags = IW_ENCODE_DISABLED;
+			iwe.u.data.length = 0;
+			event = IWE_STREAM_ADD_POINT(info, event, end, &iwe, (char *)event);
+
+			
+			if (bi->rateset.count) {
+				if (event + IW_MAX_BITRATES*IW_EV_PARAM_LEN >= end)
+					return -E2BIG;
+
+				value = event + IW_EV_LCP_LEN;
+				iwe.cmd = SIOCGIWRATE;
+				
+				iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
+				for (j = 0; j < bi->rateset.count && j < IW_MAX_BITRATES; j++) {
+					iwe.u.bitrate.value =
+					        (bi->rateset.rates[j] & 0x7f) * 500000;
+					value = IWE_STREAM_ADD_VALUE(info, event, value, end, &iwe,
+					                             IW_EV_PARAM_LEN);
+				}
+				event = value;
+			}
+		}
+		p_buf = p_buf->next;
+	} 
+
+	dwrq->length = event - extra;
+	dwrq->flags = 0;	
+
+#if !defined(CSCAN)
+	
+	wl_iw_merge_scan_cache(info, event, buflen_from_user - dwrq->length, &merged_len);
+	dwrq->length += merged_len;
+	wl_iw_run_ss_cache_timer(0);
+	wl_iw_run_ss_cache_timer(1);
+#endif 
+	
+#if defined(CONFIG_FIRST_SCAN)
+	g_first_broadcast_scan = BROADCAST_SCAN_FIRST_RESULT_CONSUMED;
+#endif 
+
+	WL_TRACE(("%s return to WE %d bytes APs=%d\n", __FUNCTION__, dwrq->length, counter));
+
+	return 0;
+}
+#endif 
+
+#define WL_JOIN_PARAMS_MAX 1600
+#ifdef CONFIG_PRESCANNED
+static int
+check_prescan(wl_join_params_t *join_params, int *join_params_size)
+{
+	int cnt = 0;
+	int indx = 0;
+	wl_iw_ss_cache_t *node = NULL;
+	wl_bss_info_t *bi = NULL;
+	iscan_info_t *iscan = g_iscan;
+	iscan_buf_t * buf;
+	wl_scan_results_t *list;
+	char *destbuf;
+
+	buf = iscan->list_hdr;
+
+	while (buf) {
+		list = &((wl_iscan_results_t*)buf->iscan_buf)->results;
+		bi = NULL;
+		for (indx = 0;  indx < list->count; indx++) {
+			bi = bi ? (wl_bss_info_t *)((uintptr)bi + dtoh32(bi->length))
+				: list->bss_info;
+			if (!(dtoh16(bi->capability) & DOT11_CAP_ESS))
+				continue;
+			if ((dtoh32(bi->SSID_len) != join_params->ssid.SSID_len) ||
+				memcmp(bi->SSID, join_params->ssid.SSID,
+				join_params->ssid.SSID_len))
+				continue;
+			memcpy(&join_params->params.chanspec_list[cnt],
+				&bi->chanspec, sizeof(chanspec_t));
+			WL_ERROR(("iscan : chanspec :%d, count %d \n", bi->chanspec, cnt));
+			cnt++;
+		}
+		buf = buf->next;
+	}
+
+	if (!cnt) {
+		MUTEX_LOCK_WL_SCAN_SET();
+		node = g_ss_cache_ctrl.m_cache_head;
+		for (; node; ) {
+			if (!memcmp(&node->bss_info->SSID, join_params->ssid.SSID,
+				join_params->ssid.SSID_len)) {
+				memcpy(&join_params->params.chanspec_list[cnt],
+					&node->bss_info->chanspec, sizeof(chanspec_t));
+				WL_ERROR(("cache_scan : chanspec :%d, count %d \n",
+				(int)node->bss_info->chanspec, cnt));
+				cnt++;
+			}
+			node = node->next;
+		}
+		MUTEX_UNLOCK_WL_SCAN_SET();
+	}
+
+	if (!cnt) {
+		return 0;
+	}
+
+	destbuf = (char *)&join_params->params.chanspec_list[cnt];
+	*join_params_size = destbuf - (char*)join_params;
+	join_params->ssid.SSID_len = htod32(g_ssid.SSID_len);
+	memcpy(&(join_params->params.bssid), &ether_bcast, ETHER_ADDR_LEN);
+	join_params->params.chanspec_num = htod32(cnt);
+
+	if ((*join_params_size) > WL_JOIN_PARAMS_MAX) {
+		WL_ERROR(("can't fit bssids for all %d APs found\n", cnt));
+			kfree(join_params);
+		return 0;
+	}
+
+	WL_ERROR(("Passing %d channel/bssid pairs.\n", cnt));
+	return cnt;
+}
+#endif 
+
+static int
+wl_iw_set_essid(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	int error;
+	wl_join_params_t *join_params;
+	int join_params_size;
+
+	WL_TRACE(("%s: SIOCSIWESSID\n", dev->name));
+
+	RETURN_IF_EXTRA_NULL(extra);
+
+#ifdef OEM_CHROMIUMOS
+	if (g_set_essid_before_scan)
+		return -EAGAIN;
+#endif
+	if (!(join_params = kmalloc(WL_JOIN_PARAMS_MAX, GFP_KERNEL))) {
+		WL_ERROR(("allocation failed for join_params size is %d\n", WL_JOIN_PARAMS_MAX));
+		return -ENOMEM;
+	}
+
+	memset(join_params, 0, WL_JOIN_PARAMS_MAX);
+
+	
+	memset(&g_ssid, 0, sizeof(g_ssid));
+
+	if (dwrq->length && extra) {
+#if WIRELESS_EXT > 20
+		g_ssid.SSID_len = MIN(sizeof(g_ssid.SSID), dwrq->length);
+#else
+		g_ssid.SSID_len = MIN(sizeof(g_ssid.SSID), dwrq->length-1);
+#endif
+		memcpy(g_ssid.SSID, extra, g_ssid.SSID_len);
+
+#ifdef CONFIG_PRESCANNED
+		memcpy(join_params->ssid.SSID, g_ssid.SSID, g_ssid.SSID_len);
+		join_params->ssid.SSID_len = g_ssid.SSID_len;
+
+		if (check_prescan(join_params, &join_params_size)) {
+			if ((error = dev_wlc_ioctl(dev, WLC_SET_SSID,
+				join_params, join_params_size))) {
+				WL_ERROR(("Invalid ioctl data=%d\n", error));
+				kfree(join_params);
+				return error;
+			}
+			kfree(join_params);
+			return 0;
+		} else {
+			WL_ERROR(("No matched found\n Trying to join to specific channel\n"));
+		}
+#endif 
+	} else {
+		
+		g_ssid.SSID_len = 0;
+	}
+	g_ssid.SSID_len = htod32(g_ssid.SSID_len);
+
+	
+	memset(join_params, 0, sizeof(*join_params));
+	join_params_size = sizeof(join_params->ssid);
+
+	memcpy(join_params->ssid.SSID, g_ssid.SSID, g_ssid.SSID_len);
+	join_params->ssid.SSID_len = htod32(g_ssid.SSID_len);
+	memcpy(&(join_params->params.bssid), &ether_bcast, ETHER_ADDR_LEN);
+
+	
+	
+	wl_iw_ch_to_chanspec(g_wl_iw_params.target_channel, join_params, &join_params_size);
+
+	if ((error = dev_wlc_ioctl(dev, WLC_SET_SSID, join_params, join_params_size))) {
+		WL_ERROR(("Invalid ioctl data=%d\n", error));
+		return error;
+	}
+
+	if (g_ssid.SSID_len) {
+		WL_ERROR(("%s: join SSID=%s ch=%d\n", __FUNCTION__,
+			g_ssid.SSID,  g_wl_iw_params.target_channel));
+	}
+	kfree(join_params);
+	return 0;
+}
+
+static int
+wl_iw_get_essid(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wlc_ssid_t ssid;
+	int error;
+
+	WL_TRACE(("%s: SIOCGIWESSID\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_SSID, &ssid, sizeof(ssid)))) {
+		WL_ERROR(("Error getting the SSID\n"));
+		return error;
+	}
+
+	ssid.SSID_len = dtoh32(ssid.SSID_len);
+
+	
+	memcpy(extra, ssid.SSID, ssid.SSID_len);
+
+	dwrq->length = ssid.SSID_len;
+
+	dwrq->flags = 1; 
+
+	return 0;
+}
+
+static int
+wl_iw_set_nick(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_iw_t *iw = NETDEV_PRIV(dev);
+
+	WL_TRACE(("%s: SIOCSIWNICKN\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	
+	if (dwrq->length > sizeof(iw->nickname))
+		return -E2BIG;
+
+	memcpy(iw->nickname, extra, dwrq->length);
+	iw->nickname[dwrq->length - 1] = '\0';
+
+	return 0;
+}
+
+static int
+wl_iw_get_nick(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_iw_t *iw = NETDEV_PRIV(dev);
+
+	WL_TRACE(("%s: SIOCGIWNICKN\n", dev->name));
+
+	if (!extra)
+		return -EINVAL;
+
+	strcpy(extra, iw->nickname);
+	dwrq->length = strlen(extra) + 1;
+
+	return 0;
+}
+
+static int
+wl_iw_set_rate(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	wl_rateset_t rateset;
+	int error, rate, i, error_bg, error_a;
+
+	WL_TRACE(("%s: SIOCSIWRATE\n", dev->name));
+
+	
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_CURR_RATESET, &rateset, sizeof(rateset))))
+		return error;
+
+	rateset.count = dtoh32(rateset.count);
+
+	if (vwrq->value < 0) {
+		
+		rate = rateset.rates[rateset.count - 1] & 0x7f;
+	} else if (vwrq->value < rateset.count) {
+		
+		rate = rateset.rates[vwrq->value] & 0x7f;
+	} else {
+		
+		rate = vwrq->value / 500000;
+	}
+
+	if (vwrq->fixed) {
+		
+		error_bg = dev_wlc_intvar_set(dev, "bg_rate", rate);
+		error_a = dev_wlc_intvar_set(dev, "a_rate", rate);
+
+		if (error_bg && error_a)
+			return (error_bg | error_a);
+	} else {
+		
+		
+		error_bg = dev_wlc_intvar_set(dev, "bg_rate", 0);
+		
+		error_a = dev_wlc_intvar_set(dev, "a_rate", 0);
+
+		if (error_bg && error_a)
+			return (error_bg | error_a);
+
+		
+		for (i = 0; i < rateset.count; i++)
+			if ((rateset.rates[i] & 0x7f) > rate)
+				break;
+		rateset.count = htod32(i);
+
+		
+		if ((error = dev_wlc_ioctl(dev, WLC_SET_RATESET, &rateset, sizeof(rateset))))
+			return error;
+	}
+
+	return 0;
+}
+
+static int
+wl_iw_get_rate(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, rate;
+
+	WL_TRACE(("%s: SIOCGIWRATE\n", dev->name));
+
+	
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_RATE, &rate, sizeof(rate))))
+		return error;
+	rate = dtoh32(rate);
+	vwrq->value = rate * 500000;
+
+	return 0;
+}
+
+static int
+wl_iw_set_rts(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, rts;
+
+	WL_TRACE(("%s: SIOCSIWRTS\n", dev->name));
+
+	if (vwrq->disabled)
+		rts = DOT11_DEFAULT_RTS_LEN;
+	else if (vwrq->value < 0 || vwrq->value > DOT11_DEFAULT_RTS_LEN)
+		return -EINVAL;
+	else
+		rts = vwrq->value;
+
+	if ((error = dev_wlc_intvar_set(dev, "rtsthresh", rts)))
+		return error;
+
+	return 0;
+}
+
+static int
+wl_iw_get_rts(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, rts;
+
+	WL_TRACE(("%s: SIOCGIWRTS\n", dev->name));
+
+	if ((error = dev_wlc_intvar_get(dev, "rtsthresh", &rts)))
+		return error;
+
+	vwrq->value = rts;
+	vwrq->disabled = (rts >= DOT11_DEFAULT_RTS_LEN);
+	vwrq->fixed = 1;
+
+	return 0;
+}
+
+static int
+wl_iw_set_frag(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, frag;
+
+	WL_TRACE(("%s: SIOCSIWFRAG\n", dev->name));
+
+	if (vwrq->disabled)
+		frag = DOT11_DEFAULT_FRAG_LEN;
+	else if (vwrq->value < 0 || vwrq->value > DOT11_DEFAULT_FRAG_LEN)
+		return -EINVAL;
+	else
+		frag = vwrq->value;
+
+	if ((error = dev_wlc_intvar_set(dev, "fragthresh", frag)))
+		return error;
+
+	return 0;
+}
+
+static int
+wl_iw_get_frag(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, fragthreshold;
+
+	WL_TRACE(("%s: SIOCGIWFRAG\n", dev->name));
+
+	if ((error = dev_wlc_intvar_get(dev, "fragthresh", &fragthreshold)))
+		return error;
+
+	vwrq->value = fragthreshold;
+	vwrq->disabled = (fragthreshold >= DOT11_DEFAULT_FRAG_LEN);
+	vwrq->fixed = 1;
+
+	return 0;
+}
+
+static int
+wl_iw_set_txpow(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, disable;
+	uint16 txpwrmw;
+	WL_TRACE(("%s: SIOCSIWTXPOW\n", dev->name));
+
+	
+	disable = vwrq->disabled ? WL_RADIO_SW_DISABLE : 0;
+	disable += WL_RADIO_SW_DISABLE << 16;
+
+	disable = htod32(disable);
+	if ((error = dev_wlc_ioctl(dev, WLC_SET_RADIO, &disable, sizeof(disable))))
+		return error;
+
+	
+	if (disable & WL_RADIO_SW_DISABLE)
+		return 0;
+
+	
+	if (!(vwrq->flags & IW_TXPOW_MWATT))
+		return -EINVAL;
+
+	
+	if (vwrq->value < 0)
+		return 0;
+
+	if (vwrq->value > 0xffff) txpwrmw = 0xffff;
+	else txpwrmw = (uint16)vwrq->value;
+
+
+	error = dev_wlc_intvar_set(dev, "qtxpower", (int)(bcm_mw_to_qdbm(txpwrmw)));
+	return error;
+}
+
+static int
+wl_iw_get_txpow(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, disable, txpwrdbm;
+	uint8 result;
+
+	WL_TRACE(("%s: SIOCGIWTXPOW\n", dev->name));
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_RADIO, &disable, sizeof(disable))) ||
+	    (error = dev_wlc_intvar_get(dev, "qtxpower", &txpwrdbm)))
+		return error;
+
+	disable = dtoh32(disable);
+	result = (uint8)(txpwrdbm & ~WL_TXPWR_OVERRIDE);
+	vwrq->value = (int32)bcm_qdbm_to_mw(result);
+	vwrq->fixed = 0;
+	vwrq->disabled = (disable & (WL_RADIO_SW_DISABLE | WL_RADIO_HW_DISABLE)) ? 1 : 0;
+	vwrq->flags = IW_TXPOW_MWATT;
+
+	return 0;
+}
+
+#if WIRELESS_EXT > 10
+static int
+wl_iw_set_retry(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, lrl, srl;
+
+	WL_TRACE(("%s: SIOCSIWRETRY\n", dev->name));
+
+	
+	if (vwrq->disabled || (vwrq->flags & IW_RETRY_LIFETIME))
+		return -EINVAL;
+
+	
+	if (vwrq->flags & IW_RETRY_LIMIT) {
+
+		
+#if WIRELESS_EXT > 20
+	if ((vwrq->flags & IW_RETRY_LONG) ||(vwrq->flags & IW_RETRY_MAX) ||
+		!((vwrq->flags & IW_RETRY_SHORT) || (vwrq->flags & IW_RETRY_MIN))) {
+#else
+	if ((vwrq->flags & IW_RETRY_MAX) || !(vwrq->flags & IW_RETRY_MIN)) {
+#endif 
+			lrl = htod32(vwrq->value);
+			if ((error = dev_wlc_ioctl(dev, WLC_SET_LRL, &lrl, sizeof(lrl))))
+				return error;
+		}
+
+		
+#if WIRELESS_EXT > 20
+	if ((vwrq->flags & IW_RETRY_SHORT) ||(vwrq->flags & IW_RETRY_MIN) ||
+		!((vwrq->flags & IW_RETRY_LONG) || (vwrq->flags & IW_RETRY_MAX))) {
+#else
+		if ((vwrq->flags & IW_RETRY_MIN) || !(vwrq->flags & IW_RETRY_MAX)) {
+#endif 
+			srl = htod32(vwrq->value);
+			if ((error = dev_wlc_ioctl(dev, WLC_SET_SRL, &srl, sizeof(srl))))
+				return error;
+		}
+	}
+	return 0;
+}
+
+static int
+wl_iw_get_retry(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, lrl, srl;
+
+	WL_TRACE(("%s: SIOCGIWRETRY\n", dev->name));
+
+	vwrq->disabled = 0;      
+
+	
+	if ((vwrq->flags & IW_RETRY_TYPE) == IW_RETRY_LIFETIME)
+		return -EINVAL;
+
+	
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_LRL, &lrl, sizeof(lrl))) ||
+	    (error = dev_wlc_ioctl(dev, WLC_GET_SRL, &srl, sizeof(srl))))
+		return error;
+
+	lrl = dtoh32(lrl);
+	srl = dtoh32(srl);
+
+	
+	if (vwrq->flags & IW_RETRY_MAX) {
+		vwrq->flags = IW_RETRY_LIMIT | IW_RETRY_MAX;
+		vwrq->value = lrl;
+	} else {
+		vwrq->flags = IW_RETRY_LIMIT;
+		vwrq->value = srl;
+		if (srl != lrl)
+			vwrq->flags |= IW_RETRY_MIN;
+	}
+
+	return 0;
+}
+#endif 
+
+static int
+wl_iw_set_encode(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_wsec_key_t key;
+	int error, val, wsec;
+
+	WL_TRACE(("%s: SIOCSIWENCODE index %d, len %d, flags %04x (%s%s%s%s%s)\n",
+		dev->name, dwrq->flags & IW_ENCODE_INDEX, dwrq->length, dwrq->flags,
+		dwrq->flags & IW_ENCODE_NOKEY ? "NOKEY" : "",
+		dwrq->flags & IW_ENCODE_DISABLED ? " DISABLED" : "",
+		dwrq->flags & IW_ENCODE_RESTRICTED ? " RESTRICTED" : "",
+		dwrq->flags & IW_ENCODE_OPEN ? " OPEN" : "",
+		dwrq->flags & IW_ENCODE_TEMP ? " TEMP" : ""));
+
+	memset(&key, 0, sizeof(key));
+
+	if ((dwrq->flags & IW_ENCODE_INDEX) == 0) {
+		
+		for (key.index = 0; key.index < DOT11_MAX_DEFAULT_KEYS; key.index++) {
+			val = htod32(key.index);
+			if ((error = dev_wlc_ioctl(dev, WLC_GET_KEY_PRIMARY, &val, sizeof(val))))
+				return error;
+			val = dtoh32(val);
+			if (val)
+				break;
+		}
+		
+		if (key.index == DOT11_MAX_DEFAULT_KEYS)
+			key.index = 0;
+	} else {
+		key.index = (dwrq->flags & IW_ENCODE_INDEX) - 1;
+		if (key.index >= DOT11_MAX_DEFAULT_KEYS)
+			return -EINVAL;
+	}
+
+	
+	if (!extra || !dwrq->length || (dwrq->flags & IW_ENCODE_NOKEY)) {
+		
+		val = htod32(key.index);
+		if ((error = dev_wlc_ioctl(dev, WLC_SET_KEY_PRIMARY, &val, sizeof(val))))
+			return error;
+	} else {
+		key.len = dwrq->length;
+
+		if (dwrq->length > sizeof(key.data))
+			return -EINVAL;
+
+		memcpy(key.data, extra, dwrq->length);
+
+		key.flags = WL_PRIMARY_KEY;
+		switch (key.len) {
+		case WEP1_KEY_SIZE:
+			key.algo = CRYPTO_ALGO_WEP1;
+			break;
+		case WEP128_KEY_SIZE:
+			key.algo = CRYPTO_ALGO_WEP128;
+			break;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 14)
+		case TKIP_KEY_SIZE:
+			key.algo = CRYPTO_ALGO_TKIP;
+			break;
+#endif
+		case AES_KEY_SIZE:
+			key.algo = CRYPTO_ALGO_AES_CCM;
+			break;
+		default:
+			return -EINVAL;
+		}
+
+		
+		swap_key_from_BE(&key);
+		if ((error = dev_wlc_ioctl(dev, WLC_SET_KEY, &key, sizeof(key))))
+			return error;
+	}
+
+	
+	val = (dwrq->flags & IW_ENCODE_DISABLED) ? 0 : WEP_ENABLED;
+
+	if ((error = dev_wlc_intvar_get(dev, "wsec", &wsec)))
+		return error;
+
+	wsec  &= ~(WEP_ENABLED);
+	wsec |= val;
+
+	if ((error = dev_wlc_intvar_set(dev, "wsec", wsec)))
+		return error;
+
+	
+	val = (dwrq->flags & IW_ENCODE_RESTRICTED) ? 1 : 0;
+	val = htod32(val);
+	if ((error = dev_wlc_ioctl(dev, WLC_SET_AUTH, &val, sizeof(val))))
+		return error;
+
+	return 0;
+}
+
+static int
+wl_iw_get_encode(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_wsec_key_t key;
+	int error, val, wsec, auth;
+
+	WL_TRACE(("%s: SIOCGIWENCODE\n", dev->name));
+
+	
+	bzero(&key, sizeof(wl_wsec_key_t));
+
+	if ((dwrq->flags & IW_ENCODE_INDEX) == 0) {
+		
+		for (key.index = 0; key.index < DOT11_MAX_DEFAULT_KEYS; key.index++) {
+			val = key.index;
+			if ((error = dev_wlc_ioctl(dev, WLC_GET_KEY_PRIMARY, &val, sizeof(val))))
+				return error;
+			val = dtoh32(val);
+			if (val)
+				break;
+		}
+	} else
+		key.index = (dwrq->flags & IW_ENCODE_INDEX) - 1;
+
+	if (key.index >= DOT11_MAX_DEFAULT_KEYS)
+		key.index = 0;
+
+	
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_WSEC, &wsec, sizeof(wsec))) ||
+	    (error = dev_wlc_ioctl(dev, WLC_GET_AUTH, &auth, sizeof(auth))))
+		return error;
+
+	swap_key_to_BE(&key);
+
+	wsec = dtoh32(wsec);
+	auth = dtoh32(auth);
+	
+	dwrq->length = MIN(DOT11_MAX_KEY_SIZE, key.len);
+
+	
+	dwrq->flags = key.index + 1;
+	if (!(wsec & (WEP_ENABLED | TKIP_ENABLED | AES_ENABLED))) {
+		
+		dwrq->flags |= IW_ENCODE_DISABLED;
+	}
+	if (auth) {
+		
+		dwrq->flags |= IW_ENCODE_RESTRICTED;
+	}
+
+	
+	if (dwrq->length && extra)
+		memcpy(extra, key.data, dwrq->length);
+
+	return 0;
+}
+
+static int
+wl_iw_set_power(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, pm;
+
+	WL_TRACE(("%s: SIOCSIWPOWER\n", dev->name));
+
+	pm = vwrq->disabled ? PM_OFF : PM_MAX;
+
+	pm = htod32(pm);
+	if ((error = dev_wlc_ioctl(dev, WLC_SET_PM, &pm, sizeof(pm))))
+		return error;
+
+	return 0;
+}
+
+static int
+wl_iw_get_power(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error, pm;
+
+	WL_TRACE(("%s: SIOCGIWPOWER\n", dev->name));
+
+	if ((error = dev_wlc_ioctl(dev, WLC_GET_PM, &pm, sizeof(pm))))
+		return error;
+
+	pm = dtoh32(pm);
+	vwrq->disabled = pm ? 0 : 1;
+	vwrq->flags = IW_POWER_ALL_R;
+
+	return 0;
+}
+
+#if WIRELESS_EXT > 17
+static int
+wl_iw_set_wpaie(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *iwp,
+	char *extra
+)
+{
+
+	WL_TRACE(("%s: SIOCSIWGENIE\n", dev->name));
+
+	RETURN_IF_EXTRA_NULL(extra);
+
+#ifdef DHD_DEBUG
+	{
+		int i;
+
+		for (i = 0; i < iwp->length; i++)
+			WL_TRACE(("%02X ", extra[i]));
+		WL_TRACE(("\n"));
+	}
+#endif
+
+		dev_wlc_bufvar_set(dev, "wpaie", extra, iwp->length);
+
+	return 0;
+}
+
+static int
+wl_iw_get_wpaie(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *iwp,
+	char *extra
+)
+{
+	WL_TRACE(("%s: SIOCGIWGENIE\n", dev->name));
+	iwp->length = 64;
+	dev_wlc_bufvar_get(dev, "wpaie", extra, iwp->length);
+	return 0;
+}
+
+static int
+wl_iw_set_encodeext(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *extra
+)
+{
+	wl_wsec_key_t key;
+	int error;
+	struct iw_encode_ext *iwe;
+
+	WL_TRACE(("%s: SIOCSIWENCODEEXT\n", dev->name));
+
+	RETURN_IF_EXTRA_NULL(extra);
+
+	memset(&key, 0, sizeof(key));
+	iwe = (struct iw_encode_ext *)extra;
+
+	
+	if (dwrq->flags & IW_ENCODE_DISABLED) {
+
+	}
+
+	
+	key.index = 0;
+	if (dwrq->flags & IW_ENCODE_INDEX)
+		key.index = (dwrq->flags & IW_ENCODE_INDEX) - 1;
+
+	key.len = iwe->key_len;
+
+	
+	if (!ETHER_ISMULTI(iwe->addr.sa_data))
+		bcopy((void *)&iwe->addr.sa_data, (char *)&key.ea, ETHER_ADDR_LEN);
+
+	
+	if (key.len == 0) {
+		if (iwe->ext_flags & IW_ENCODE_EXT_SET_TX_KEY) {
+			WL_WSEC(("Changing the the primary Key to %d\n", key.index));
+			
+			key.index = htod32(key.index);
+			error = dev_wlc_ioctl(dev, WLC_SET_KEY_PRIMARY,
+				&key.index, sizeof(key.index));
+			if (error)
+				return error;
+		}
+		
+		else {
+			swap_key_from_BE(&key);
+			dev_wlc_ioctl(dev, WLC_SET_KEY, &key, sizeof(key));
+		}
+	}
+	else {
+		if (iwe->key_len > sizeof(key.data))
+			return -EINVAL;
+
+		WL_WSEC(("Setting the key index %d\n", key.index));
+		if (iwe->ext_flags & IW_ENCODE_EXT_SET_TX_KEY) {
+			WL_WSEC(("key is a Primary Key\n"));
+			key.flags = WL_PRIMARY_KEY;
+		}
+
+		bcopy((void *)iwe->key, key.data, iwe->key_len);
+
+		if (iwe->alg == IW_ENCODE_ALG_TKIP) {
+			uint8 keybuf[8];
+			bcopy(&key.data[24], keybuf, sizeof(keybuf));
+			bcopy(&key.data[16], &key.data[24], sizeof(keybuf));
+			bcopy(keybuf, &key.data[16], sizeof(keybuf));
+		}
+
+		
+		if (iwe->ext_flags & IW_ENCODE_EXT_RX_SEQ_VALID) {
+			uchar *ivptr;
+			ivptr = (uchar *)iwe->rx_seq;
+			key.rxiv.hi = (ivptr[5] << 24) | (ivptr[4] << 16) |
+				(ivptr[3] << 8) | ivptr[2];
+			key.rxiv.lo = (ivptr[1] << 8) | ivptr[0];
+			key.iv_initialized = TRUE;
+		}
+
+		switch (iwe->alg) {
+			case IW_ENCODE_ALG_NONE:
+				key.algo = CRYPTO_ALGO_OFF;
+				break;
+			case IW_ENCODE_ALG_WEP:
+				if (iwe->key_len == WEP1_KEY_SIZE)
+					key.algo = CRYPTO_ALGO_WEP1;
+				else
+					key.algo = CRYPTO_ALGO_WEP128;
+				break;
+			case IW_ENCODE_ALG_TKIP:
+				key.algo = CRYPTO_ALGO_TKIP;
+				break;
+			case IW_ENCODE_ALG_CCMP:
+				key.algo = CRYPTO_ALGO_AES_CCM;
+				break;
+			default:
+				break;
+		}
+		swap_key_from_BE(&key);
+
+		dhd_wait_pend8021x(dev);
+
+		error = dev_wlc_ioctl(dev, WLC_SET_KEY, &key, sizeof(key));
+		if (error)
+			return error;
+	}
+	return 0;
+}
+
+#if WIRELESS_EXT > 17
+struct {
+	pmkid_list_t pmkids;
+	pmkid_t foo[MAXPMKID-1];
+} pmkid_list;
+
+static int
+wl_iw_set_pmksa(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	struct iw_pmksa *iwpmksa;
+	uint i;
+	int ret = 0;
+	char eabuf[ETHER_ADDR_STR_LEN];
+	pmkid_t * pmkid_array = pmkid_list.pmkids.pmkid;
+
+	WL_WSEC(("%s: SIOCSIWPMKSA\n", dev->name));
+
+	RETURN_IF_EXTRA_NULL(extra);
+
+	iwpmksa = (struct iw_pmksa *)extra;
+	bzero((char *)eabuf, ETHER_ADDR_STR_LEN);
+
+	if (iwpmksa->cmd == IW_PMKSA_FLUSH) {
+		WL_WSEC(("wl_iw_set_pmksa - IW_PMKSA_FLUSH\n"));
+		bzero((char *)&pmkid_list, sizeof(pmkid_list));
+	}
+
+	else if (iwpmksa->cmd == IW_PMKSA_REMOVE) {
+		{
+			pmkid_list_t pmkid, *pmkidptr;
+			uint j;
+			pmkidptr = &pmkid;
+
+			bcopy(&iwpmksa->bssid.sa_data[0], &pmkidptr->pmkid[0].BSSID,
+				ETHER_ADDR_LEN);
+			bcopy(&iwpmksa->pmkid[0], &pmkidptr->pmkid[0].PMKID, WPA2_PMKID_LEN);
+
+			WL_WSEC(("wl_iw_set_pmksa,IW_PMKSA_REMOVE - PMKID: %s = ",
+				bcm_ether_ntoa(&pmkidptr->pmkid[0].BSSID,
+				eabuf)));
+			for (j = 0; j < WPA2_PMKID_LEN; j++)
+				WL_WSEC(("%02x ", pmkidptr->pmkid[0].PMKID[j]));
+			WL_WSEC(("\n"));
+		}
+
+		for (i = 0; i < pmkid_list.pmkids.npmkid; i++)
+			if (!bcmp(&iwpmksa->bssid.sa_data[0], &pmkid_array[i].BSSID,
+				ETHER_ADDR_LEN))
+				break;
+
+		if ((pmkid_list.pmkids.npmkid > 0) && (i < pmkid_list.pmkids.npmkid)) {
+			bzero(&pmkid_array[i], sizeof(pmkid_t));
+			for (; i < (pmkid_list.pmkids.npmkid - 1); i++) {
+				bcopy(&pmkid_array[i+1].BSSID,
+					&pmkid_array[i].BSSID,
+					ETHER_ADDR_LEN);
+				bcopy(&pmkid_array[i+1].PMKID,
+					&pmkid_array[i].PMKID,
+					WPA2_PMKID_LEN);
+			}
+			pmkid_list.pmkids.npmkid--;
+		}
+		else
+			ret = -EINVAL;
+	}
+
+	else if (iwpmksa->cmd == IW_PMKSA_ADD) {
+		for (i = 0; i < pmkid_list.pmkids.npmkid; i++)
+			if (!bcmp(&iwpmksa->bssid.sa_data[0], &pmkid_array[i].BSSID,
+				ETHER_ADDR_LEN))
+				break;
+		if (i < MAXPMKID) {
+			bcopy(&iwpmksa->bssid.sa_data[0],
+				&pmkid_array[i].BSSID,
+				ETHER_ADDR_LEN);
+			bcopy(&iwpmksa->pmkid[0], &pmkid_array[i].PMKID,
+				WPA2_PMKID_LEN);
+			if (i == pmkid_list.pmkids.npmkid)
+				pmkid_list.pmkids.npmkid++;
+		}
+		else
+			ret = -EINVAL;
+
+		{
+			uint j;
+			uint k;
+			k = pmkid_list.pmkids.npmkid;
+			WL_WSEC(("wl_iw_set_pmksa,IW_PMKSA_ADD - PMKID: %s = ",
+				bcm_ether_ntoa(&pmkid_array[k].BSSID,
+				eabuf)));
+			for (j = 0; j < WPA2_PMKID_LEN; j++)
+				WL_WSEC(("%02x ", pmkid_array[k].PMKID[j]));
+			WL_WSEC(("\n"));
+		}
+	}
+	WL_WSEC(("PRINTING pmkid LIST - No of elements %d", pmkid_list.pmkids.npmkid));
+	for (i = 0; i < pmkid_list.pmkids.npmkid; i++) {
+		uint j;
+		WL_WSEC(("\nPMKID[%d]: %s = ", i,
+			bcm_ether_ntoa(&pmkid_array[i].BSSID,
+			eabuf)));
+		for (j = 0; j < WPA2_PMKID_LEN; j++)
+			WL_WSEC(("%02x ", pmkid_array[i].PMKID[j]));
+	}
+	WL_WSEC(("\n"));
+
+	if (!ret)
+		ret = dev_wlc_bufvar_set(dev, "pmkid_info", (char *)&pmkid_list,
+			sizeof(pmkid_list));
+	return ret;
+}
+#endif 
+
+static int
+wl_iw_get_encodeext(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	WL_TRACE(("%s: SIOCGIWENCODEEXT\n", dev->name));
+	return 0;
+}
+
+
+static uint32
+wl_iw_create_wpaauth_wsec(struct net_device *dev)
+{
+	wl_iw_t *iw = NETDEV_PRIV(dev);
+	uint32 wsec;
+
+	
+	if (iw->pcipher & (IW_AUTH_CIPHER_WEP40 | IW_AUTH_CIPHER_WEP104))
+		wsec = WEP_ENABLED;
+	else if (iw->pcipher & IW_AUTH_CIPHER_TKIP)
+		wsec = TKIP_ENABLED;
+	else if (iw->pcipher & IW_AUTH_CIPHER_CCMP)
+		wsec = AES_ENABLED;
+	else
+		wsec = 0;
+
+	
+	if (iw->gcipher & (IW_AUTH_CIPHER_WEP40 | IW_AUTH_CIPHER_WEP104))
+		wsec |= WEP_ENABLED;
+	else if (iw->gcipher & IW_AUTH_CIPHER_TKIP)
+		wsec |= TKIP_ENABLED;
+	else if (iw->gcipher & IW_AUTH_CIPHER_CCMP)
+		wsec |= AES_ENABLED;
+
+	
+	if (wsec == 0 && iw->privacy_invoked)
+		wsec = WEP_ENABLED;
+
+	WL_INFORM(("%s: returning wsec of %d\n", __FUNCTION__, wsec));
+
+	return wsec;
+}
+
+static int
+wl_iw_set_wpaauth(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error = 0;
+	int paramid;
+	int paramval;
+	int val = 0;
+	wl_iw_t *iw = NETDEV_PRIV(dev);
+
+	paramid = vwrq->flags & IW_AUTH_INDEX;
+	paramval = vwrq->value;
+
+	WL_TRACE(("%s: SIOCSIWAUTH, %s(%d), paramval = 0x%0x\n",
+		dev->name,
+		paramid == IW_AUTH_WPA_VERSION ? "IW_AUTH_WPA_VERSION" :
+		paramid == IW_AUTH_CIPHER_PAIRWISE ? "IW_AUTH_CIPHER_PAIRWISE" :
+		paramid == IW_AUTH_CIPHER_GROUP ? "IW_AUTH_CIPHER_GROUP" :
+		paramid == IW_AUTH_KEY_MGMT ? "IW_AUTH_KEY_MGMT" :
+		paramid == IW_AUTH_TKIP_COUNTERMEASURES ? "IW_AUTH_TKIP_COUNTERMEASURES" :
+		paramid == IW_AUTH_DROP_UNENCRYPTED ? "IW_AUTH_DROP_UNENCRYPTED" :
+		paramid == IW_AUTH_80211_AUTH_ALG ? "IW_AUTH_80211_AUTH_ALG" :
+		paramid == IW_AUTH_WPA_ENABLED ? "IW_AUTH_WPA_ENABLED" :
+		paramid == IW_AUTH_RX_UNENCRYPTED_EAPOL ? "IW_AUTH_RX_UNENCRYPTED_EAPOL" :
+		paramid == IW_AUTH_ROAMING_CONTROL ? "IW_AUTH_ROAMING_CONTROL" :
+		paramid == IW_AUTH_PRIVACY_INVOKED ? "IW_AUTH_PRIVACY_INVOKED" :
+		"UNKNOWN",
+		paramid, paramval));
+
+#if defined(SOFTAP)
+	if (ap_cfg_running) {
+		WL_TRACE(("%s: Not executed, reason -'SOFTAP is active'\n", __FUNCTION__));
+		return 0;
+	}
+#endif
+
+	switch (paramid) {
+	case IW_AUTH_WPA_VERSION:
+		
+		if (paramval & IW_AUTH_WPA_VERSION_DISABLED)
+			val = WPA_AUTH_DISABLED;
+		else if (paramval & (IW_AUTH_WPA_VERSION_WPA))
+			val = WPA_AUTH_PSK | WPA_AUTH_UNSPECIFIED;
+		else if (paramval & IW_AUTH_WPA_VERSION_WPA2)
+			val = WPA2_AUTH_PSK | WPA2_AUTH_UNSPECIFIED;
+		WL_ERROR(("%s: %d: setting wpa_auth to 0x%0x\n", __FUNCTION__, __LINE__, val));
+		if ((error = dev_wlc_intvar_set(dev, "wpa_auth", val)))
+			return error;
+		break;
+
+	case IW_AUTH_CIPHER_PAIRWISE:
+		iw->pcipher = paramval;
+		val = wl_iw_create_wpaauth_wsec(dev);
+		if ((error = dev_wlc_intvar_set(dev, "wsec", val)))
+			return error;
+		break;
+
+	case IW_AUTH_CIPHER_GROUP:
+		iw->gcipher = paramval;
+		val = wl_iw_create_wpaauth_wsec(dev);
+		if ((error = dev_wlc_intvar_set(dev, "wsec", val)))
+			return error;
+		break;
+
+	case IW_AUTH_KEY_MGMT:
+		if ((error = dev_wlc_intvar_get(dev, "wpa_auth", &val)))
+			return error;
+
+		if (val & (WPA_AUTH_PSK | WPA_AUTH_UNSPECIFIED)) {
+			if (paramval & IW_AUTH_KEY_MGMT_PSK)
+				val = WPA_AUTH_PSK;
+			else
+				val = WPA_AUTH_UNSPECIFIED;
+			if (paramval & 0x04)
+				val |= WPA2_AUTH_FT;
+		}
+		else if (val & (WPA2_AUTH_PSK | WPA2_AUTH_UNSPECIFIED)) {
+			if (paramval & IW_AUTH_KEY_MGMT_PSK)
+				val = WPA2_AUTH_PSK;
+			else
+				val = WPA2_AUTH_UNSPECIFIED;
+			if (paramval & 0x04)
+				val |= WPA2_AUTH_FT;
+		}
+
+		else if (paramval & IW_AUTH_KEY_MGMT_PSK) {
+			if (iw->wpaversion == IW_AUTH_WPA_VERSION_WPA)
+				val = WPA_AUTH_PSK;
+			else if (iw->wpaversion == IW_AUTH_WPA_VERSION_WPA2)
+				val = WPA2_AUTH_PSK;
+			else 
+				val = WPA_AUTH_DISABLED;
+		} else if (paramval & IW_AUTH_KEY_MGMT_802_1X) {
+			if (iw->wpaversion == IW_AUTH_WPA_VERSION_WPA)
+				val = WPA_AUTH_UNSPECIFIED;
+			else if (iw->wpaversion == IW_AUTH_WPA_VERSION_WPA2)
+				val = WPA2_AUTH_UNSPECIFIED;
+			else 
+				val = WPA_AUTH_DISABLED;
+		}
+		else
+			val = WPA_AUTH_DISABLED;
+
+		WL_INFORM(("%s: %d: setting wpa_auth to %d\n", __FUNCTION__, __LINE__, val));
+		if ((error = dev_wlc_intvar_set(dev, "wpa_auth", val)))
+			return error;
+		break;
+
+	case IW_AUTH_TKIP_COUNTERMEASURES:
+		dev_wlc_bufvar_set(dev, "tkip_countermeasures", (char *)&paramval, 1);
+		break;
+
+	case IW_AUTH_80211_AUTH_ALG:
+		
+		WL_INFORM(("Setting the D11auth %d\n", paramval));
+		if (paramval == IW_AUTH_ALG_OPEN_SYSTEM)
+			val = 0;
+		else if (paramval == IW_AUTH_ALG_SHARED_KEY)
+			val = 1;
+		else if (paramval == (IW_AUTH_ALG_OPEN_SYSTEM | IW_AUTH_ALG_SHARED_KEY))
+			val = 2;
+		else
+			error = 1;
+		if (!error && (error = dev_wlc_intvar_set(dev, "auth", val)))
+			return error;
+		break;
+
+	case IW_AUTH_WPA_ENABLED:
+		if (paramval == 0) {
+			iw->privacy_invoked = 0; 
+			iw->pcipher = 0;
+			iw->gcipher = 0;
+			val = wl_iw_create_wpaauth_wsec(dev);
+			if ((error = dev_wlc_intvar_set(dev, "wsec", val)))
+				return error;
+			WL_INFORM(("%s: %d: setting wpa_auth to %d, wsec to %d\n",
+				__FUNCTION__, __LINE__, paramval, val));
+			dev_wlc_intvar_set(dev, "wpa_auth", paramval);
+			return error;
+		}
+
+		
+		break;
+
+	case IW_AUTH_DROP_UNENCRYPTED:
+		if ((error = dev_wlc_intvar_set(dev, "wsec_restrict", paramval)))
+			return error;
+		break;
+
+	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+		dev_wlc_bufvar_set(dev, "rx_unencrypted_eapol", (char *)&paramval, 1);
+		break;
+
+#if WIRELESS_EXT > 17
+	case IW_AUTH_ROAMING_CONTROL:
+		WL_INFORM(("%s: IW_AUTH_ROAMING_CONTROL\n", __FUNCTION__));
+		
+		break;
+
+	case IW_AUTH_PRIVACY_INVOKED:
+		iw->privacy_invoked = paramval;
+		val = wl_iw_create_wpaauth_wsec(dev);
+		if ((error = dev_wlc_intvar_set(dev, "wsec", val)))
+			return error;
+		break;
+
+#endif 
+	default:
+		break;
+	}
+	return 0;
+}
+#define VAL_PSK(_val) (((_val) & WPA_AUTH_PSK) || ((_val) & WPA2_AUTH_PSK))
+
+static int
+wl_iw_get_wpaauth(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_param *vwrq,
+	char *extra
+)
+{
+	int error;
+	int paramid;
+	int paramval = 0;
+	int val;
+	wl_iw_t *iw = NETDEV_PRIV(dev);
+
+	WL_TRACE(("%s: SIOCGIWAUTH\n", dev->name));
+
+	paramid = vwrq->flags & IW_AUTH_INDEX;
+
+	switch (paramid) {
+	case IW_AUTH_WPA_VERSION:
+		paramval = iw->wpaversion;
+		break;
+
+	case IW_AUTH_CIPHER_PAIRWISE:
+		paramval = iw->pcipher;
+		break;
+
+	case IW_AUTH_CIPHER_GROUP:
+		paramval = iw->gcipher;
+		break;
+
+	case IW_AUTH_KEY_MGMT:
+		
+		if ((error = dev_wlc_intvar_get(dev, "wpa_auth", &val)))
+			return error;
+		if (VAL_PSK(val))
+			paramval = IW_AUTH_KEY_MGMT_PSK;
+		else
+			paramval = IW_AUTH_KEY_MGMT_802_1X;
+
+		break;
+
+	case IW_AUTH_TKIP_COUNTERMEASURES:
+		dev_wlc_bufvar_get(dev, "tkip_countermeasures", (char *)&paramval, 1);
+		break;
+
+	case IW_AUTH_DROP_UNENCRYPTED:
+		dev_wlc_intvar_get(dev, "wsec_restrict", &paramval);
+		break;
+
+	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+		dev_wlc_bufvar_get(dev, "rx_unencrypted_eapol", (char *)&paramval, 1);
+		break;
+
+	case IW_AUTH_80211_AUTH_ALG:
+		
+		if ((error = dev_wlc_intvar_get(dev, "auth", &val)))
+			return error;
+		if (!val)
+			paramval = IW_AUTH_ALG_OPEN_SYSTEM;
+		else
+			paramval = IW_AUTH_ALG_SHARED_KEY;
+		break;
+	case IW_AUTH_WPA_ENABLED:
+		if ((error = dev_wlc_intvar_get(dev, "wpa_auth", &val)))
+			return error;
+		if (val)
+			paramval = TRUE;
+		else
+			paramval = FALSE;
+		break;
+#if WIRELESS_EXT > 17
+	case IW_AUTH_ROAMING_CONTROL:
+		WL_ERROR(("%s: IW_AUTH_ROAMING_CONTROL\n", __FUNCTION__));
+		
+		break;
+	case IW_AUTH_PRIVACY_INVOKED:
+		paramval = iw->privacy_invoked;
+		break;
+
+#endif 
+	}
+	vwrq->value = paramval;
+	return 0;
+}
+#endif 
+
+
+#ifdef SOFTAP
+
+static int ap_macmode = MACLIST_MODE_DISABLED;
+static struct mflist ap_black_list;
+
+static int
+wl_iw_parse_wep(char *keystr, wl_wsec_key_t *key)
+{
+	char hex[] = "XX";
+	unsigned char *data = key->data;
+
+	switch (strlen(keystr)) {
+	case 5:
+	case 13:
+	case 16:
+		key->len = strlen(keystr);
+		memcpy(data, keystr, key->len + 1);
+		break;
+	case 12:
+	case 28:
+	case 34:
+	case 66:
+		
+		if (!strnicmp(keystr, "0x", 2))
+			keystr += 2;
+		else
+			return -1;
+		
+	case 10:
+	case 26:
+	case 32:
+	case 64:
+		key->len = strlen(keystr) / 2;
+		while (*keystr) {
+			strncpy(hex, keystr, 2);
+			*data++ = (char) bcm_strtoul(hex, NULL, 16);
+			keystr += 2;
+		}
+		break;
+	default:
+		return -1;
+	}
+
+	switch (key->len) {
+	case 5:
+		key->algo = CRYPTO_ALGO_WEP1;
+		break;
+	case 13:
+		key->algo = CRYPTO_ALGO_WEP128;
+		break;
+	case 16:
+		
+		key->algo = CRYPTO_ALGO_AES_CCM;
+		break;
+	case 32:
+		key->algo = CRYPTO_ALGO_TKIP;
+		break;
+	default:
+		return -1;
+	}
+
+	
+	key->flags |= WL_PRIMARY_KEY;
+
+	return 0;
+}
+
+#ifdef EXT_WPA_CRYPTO
+#define SHA1HashSize 20
+extern void pbkdf2_sha1(const char *passphrase, const char *ssid, size_t ssid_len,
+                        int iterations, u8 *buf, size_t buflen);
+
+#else
+
+#define SHA1HashSize 20
+static int
+pbkdf2_sha1(const char *passphrase, const char *ssid, size_t ssid_len,
+            int iterations, u8 *buf, size_t buflen)
+{
+	WL_ERROR(("WARNING: %s is not implemented !!!\n", __FUNCTION__));
+	return -1;
+}
+
+#endif 
+
+
+static int
+dev_iw_write_cfg1_bss_var(struct net_device *dev, int val)
+{
+	struct {
+		int cfg;
+		int val;
+	} bss_setbuf;
+
+	int bss_set_res;
+	char smbuf[WLC_IOCTL_SMLEN];
+	memset(smbuf, 0, sizeof(smbuf));
+
+	bss_setbuf.cfg = 1;
+	bss_setbuf.val = val;
+
+	bss_set_res = dev_iw_iovar_setbuf(dev, "bss",
+		&bss_setbuf, sizeof(bss_setbuf), smbuf, sizeof(smbuf));
+	WL_TRACE(("%s: bss_set_result:%d set with %d\n", __FUNCTION__, bss_set_res, val));
+
+	return bss_set_res;
+}
+
+
+
+#ifndef AP_ONLY
+static int
+wl_bssiovar_mkbuf(
+		const char *iovar,
+		int bssidx,
+		void *param,
+		int paramlen,
+		void *bufptr,
+		int buflen,
+		int *perr)
+{
+	const char *prefix = "bsscfg:";
+	int8* p;
+	uint prefixlen;
+	uint namelen;
+	uint iolen;
+
+	prefixlen = strlen(prefix);	
+	namelen = strlen(iovar) + 1;	
+	iolen = prefixlen + namelen + sizeof(int) + paramlen;
+
+	
+	if (buflen < 0 || iolen > (uint)buflen) {
+		*perr = BCME_BUFTOOSHORT;
+		return 0;
+	}
+
+	p = (int8*)bufptr;
+
+	
+	memcpy(p, prefix, prefixlen);
+	p += prefixlen;
+
+	
+	memcpy(p, iovar, namelen);
+	p += namelen;
+
+	
+	bssidx = htod32(bssidx);
+	memcpy(p, &bssidx, sizeof(int32));
+	p += sizeof(int32);
+
+	
+	if (paramlen)
+		memcpy(p, param, paramlen);
+
+	*perr = 0;
+	return iolen;
+}
+#endif 
+
+
+
+
+#define strtoul(nptr, endptr, base) bcm_strtoul((nptr), (endptr), (base))
+
+
+#if defined(CSCAN)
+
+
+
+static int
+wl_iw_combined_scan_set(struct net_device *dev, wlc_ssid_t* ssids_local, int nssid, int nchan)
+{
+	int params_size = WL_SCAN_PARAMS_FIXED_SIZE + WL_NUMCHANNELS * sizeof(uint16);
+	int err = 0;
+	char *p;
+	int i;
+	iscan_info_t *iscan = g_iscan;
+
+	WL_TRACE(("%s nssid=%d nchan=%d\n", __FUNCTION__, nssid, nchan));
+
+	if ((!dev) && (!g_iscan) && (!iscan->iscan_ex_params_p)) {
+		WL_ERROR(("%s error exit\n", __FUNCTION__));
+		err = -1;
+		goto exit;
+	}
+
+#ifdef PNO_SUPPORT
+	
+	if  (dhd_dev_get_pno_status(dev)) {
+		WL_ERROR(("%s: Scan called when PNO is active\n", __FUNCTION__));
+	}
+#endif 
+
+	params_size += WL_SCAN_PARAMS_SSID_MAX * sizeof(wlc_ssid_t);
+
+	
+	if (nssid > 0) {
+		i = OFFSETOF(wl_scan_params_t, channel_list) + nchan * sizeof(uint16);
+		i = ROUNDUP(i, sizeof(uint32));
+		if (i + nssid * sizeof(wlc_ssid_t) > params_size) {
+			printf("additional ssids exceed params_size\n");
+			err = -1;
+			goto exit;
+		}
+
+		p = ((char*)&iscan->iscan_ex_params_p->params) + i;
+		memcpy(p, ssids_local, nssid * sizeof(wlc_ssid_t));
+		p += nssid * sizeof(wlc_ssid_t);
+	} else {
+		p = (char*)iscan->iscan_ex_params_p->params.channel_list + nchan * sizeof(uint16);
+	}
+
+	
+	iscan->iscan_ex_params_p->params.channel_num =
+	        htod32((nssid << WL_SCAN_PARAMS_NSSID_SHIFT) |
+	               (nchan & WL_SCAN_PARAMS_COUNT_MASK));
+
+	nssid = (uint)
+	        ((iscan->iscan_ex_params_p->params.channel_num >> WL_SCAN_PARAMS_NSSID_SHIFT) &
+	         WL_SCAN_PARAMS_COUNT_MASK);
+
+	
+	params_size = (int) (p - (char*)iscan->iscan_ex_params_p + nssid * sizeof(wlc_ssid_t));
+	iscan->iscan_ex_param_size = params_size;
+
+	iscan->list_cur = iscan->list_hdr;
+	iscan->iscan_state = ISCAN_STATE_SCANING;
+	wl_iw_set_event_mask(dev);
+	mod_timer(&iscan->timer, jiffies + iscan->timer_ms*HZ/1000);
+
+	iscan->timer_on = 1;
+
+#ifdef SCAN_DUMP
+	{
+		int i;
+		WL_SCAN(("\n### List of SSIDs to scan ###\n"));
+		for (i = 0; i < nssid; i++) {
+			if (!ssids_local[i].SSID_len)
+				WL_SCAN(("%d: Broadcast scan\n", i));
+			else
+			WL_SCAN(("%d: scan  for  %s size =%d\n", i,
+				ssids_local[i].SSID, ssids_local[i].SSID_len));
+		}
+		WL_SCAN(("### List of channels to scan ###\n"));
+		for (i = 0; i < nchan; i++)
+		{
+			WL_SCAN(("%d ", iscan->iscan_ex_params_p->params.channel_list[i]));
+		}
+		WL_SCAN(("\nnprobes=%d\n", iscan->iscan_ex_params_p->params.nprobes));
+		WL_SCAN(("active_time=%d\n", iscan->iscan_ex_params_p->params.active_time));
+		WL_SCAN(("passive_time=%d\n", iscan->iscan_ex_params_p->params.passive_time));
+		WL_SCAN(("home_time=%d\n", iscan->iscan_ex_params_p->params.home_time));
+		WL_SCAN(("scan_type=%d\n", iscan->iscan_ex_params_p->params.scan_type));
+		WL_SCAN(("\n###################\n"));
+	}
+#endif 
+
+	if (params_size > WLC_IOCTL_MEDLEN) {
+			WL_ERROR(("Set ISCAN for %s due to params_size=%d  \n",
+				__FUNCTION__, params_size));
+			err = -1;
+	}
+
+	if ((err = dev_iw_iovar_setbuf(dev, "iscan", iscan->iscan_ex_params_p,
+	                               iscan->iscan_ex_param_size,
+	                               iscan->ioctlbuf, sizeof(iscan->ioctlbuf)))) {
+		WL_TRACE(("Set ISCAN for %s failed with %d\n", __FUNCTION__, err));
+		err = -1;
+	}
+
+exit:
+	return err;
+}
+
+
+static int
+iwpriv_set_cscan(struct net_device *dev, struct iw_request_info *info,
+                 union iwreq_data *wrqu, char *ext)
+{
+	int res;
+	char  *extra = NULL;
+	iscan_info_t *iscan = g_iscan;
+	wlc_ssid_t ssids_local[WL_SCAN_PARAMS_SSID_MAX];
+	int nssid = 0;
+	int nchan = 0;
+	char *str_ptr;
+
+	WL_TRACE(("%s: info->cmd:%x, info->flags:%x, u.data=0x%p, u.len=%d\n",
+		__FUNCTION__, info->cmd, info->flags,
+		wrqu->data.pointer, wrqu->data.length));
+
+	if (g_onoff == G_WLAN_SET_OFF) {
+		WL_TRACE(("%s: driver is not up yet after START\n", __FUNCTION__));
+		return -ENODEV;
+	}
+
+	if (wrqu->data.length == 0) {
+		WL_ERROR(("IWPRIV argument len = 0\n"));
+		return -EINVAL;
+	}
+
+	if (!iscan->iscan_ex_params_p) {
+		return -EFAULT;
+	}
+
+	if (!(extra = kmalloc(wrqu->data.length+1, GFP_KERNEL)))
+		return -ENOMEM;
+
+	if (copy_from_user(extra, wrqu->data.pointer, wrqu->data.length)) {
+		res = -EFAULT;
+		goto exit_proc;
+	}
+
+	extra[wrqu->data.length] = 0;
+	WL_ERROR(("Got str param in iw_point:\n %s\n", extra));
+
+	str_ptr = extra;
+
+	
+	if (strncmp(str_ptr, GET_SSID, strlen(GET_SSID))) {
+		WL_ERROR(("%s Error: extracting SSID='' string\n", __FUNCTION__));
+		res = -EINVAL;
+		goto exit_proc;
+	}
+
+	str_ptr += strlen(GET_SSID);
+	nssid = wl_iw_parse_ssid_list(&str_ptr, ssids_local, nssid,
+	                              WL_SCAN_PARAMS_SSID_MAX);
+	if (nssid == -1) {
+		WL_ERROR(("%s wrong ssid list", __FUNCTION__));
+		res = -EINVAL;
+		goto exit_proc;
+	}
+
+	memset(iscan->iscan_ex_params_p, 0, iscan->iscan_ex_param_size);
+	ASSERT(iscan->iscan_ex_param_size < WLC_IOCTL_MAXLEN);
+
+	
+	wl_iw_iscan_prep(&iscan->iscan_ex_params_p->params, NULL);
+	iscan->iscan_ex_params_p->version = htod32(ISCAN_REQ_VERSION);
+	iscan->iscan_ex_params_p->action = htod16(WL_SCAN_ACTION_START);
+	iscan->iscan_ex_params_p->scan_duration = htod16(0);
+
+	
+	if ((nchan = wl_iw_parse_channel_list(&str_ptr,
+	                                      &iscan->iscan_ex_params_p->params.channel_list[0],
+	                                      WL_NUMCHANNELS)) == -1) {
+		WL_ERROR(("%s missing channel list\n", __FUNCTION__));
+		res = -EINVAL;
+		goto exit_proc;
+	}
+
+	
+	get_parameter_from_string(&str_ptr,
+	                          GET_NPROBE, PTYPE_INTDEC,
+	                          &iscan->iscan_ex_params_p->params.nprobes, 2);
+
+	get_parameter_from_string(&str_ptr, GET_ACTIVE_ASSOC_DWELL, PTYPE_INTDEC,
+	                          &iscan->iscan_ex_params_p->params.active_time, 4);
+
+	get_parameter_from_string(&str_ptr, GET_PASSIVE_ASSOC_DWELL, PTYPE_INTDEC,
+	                          &iscan->iscan_ex_params_p->params.passive_time, 4);
+
+	get_parameter_from_string(&str_ptr, GET_HOME_DWELL, PTYPE_INTDEC,
+	                          &iscan->iscan_ex_params_p->params.home_time, 4);
+
+	get_parameter_from_string(&str_ptr, GET_SCAN_TYPE, PTYPE_INTDEC,
+	                          &iscan->iscan_ex_params_p->params.scan_type, 1);
+
+	
+	res = wl_iw_combined_scan_set(dev, ssids_local, nssid, nchan);
+
+exit_proc:
+	kfree(extra);
+
+	return res;
+}
+
+
+static int
+wl_iw_set_cscan(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int res = -1;
+	iscan_info_t *iscan = g_iscan;
+	wlc_ssid_t ssids_local[WL_SCAN_PARAMS_SSID_MAX];
+	int nssid = 0;
+	int nchan = 0;
+	cscan_tlv_t *cscan_tlv_temp;
+	char type;
+	char *str_ptr;
+	int tlv_size_left;
+#ifdef TLV_DEBUG
+	int i;
+	char tlv_in_example[] = {
+		'C', 'S', 'C', 'A', 'N', ' ',
+		0x53, 0x01, 0x00, 0x00,
+		'S',	  
+		0x00, 
+		'S',    
+		0x04, 
+		'B', 'R', 'C', 'M',
+		'C',
+		0x06, 
+		'P', 
+		0x94,
+		0x11,
+		'T',     
+		0x01  
+	};
+#endif 
+
+	WL_TRACE(("\n### %s: info->cmd:%x, info->flags:%x, u.data=0x%p, u.len=%d\n",
+		__FUNCTION__, info->cmd, info->flags,
+		wrqu->data.pointer, wrqu->data.length));
+
+	net_os_wake_lock(dev);
+
+	if (g_onoff == G_WLAN_SET_OFF) {
+		WL_TRACE(("%s: driver is not up yet after START\n", __FUNCTION__));
+		goto exit_proc;
+	}
+
+	if (wrqu->data.length < (strlen(CSCAN_COMMAND) + sizeof(cscan_tlv_t))) {
+		WL_ERROR(("%s argument=%d  less %d\n", __FUNCTION__,
+		          wrqu->data.length, (int)(strlen(CSCAN_COMMAND) + sizeof(cscan_tlv_t))));
+		goto exit_proc;
+	}
+
+#ifdef TLV_DEBUG
+	memcpy(extra, tlv_in_example, sizeof(tlv_in_example));
+	wrqu->data.length = sizeof(tlv_in_example);
+	for (i = 0; i < wrqu->data.length; i++)
+		printf("%02X ", extra[i]);
+	printf("\n");
+#endif 
+
+	str_ptr = extra;
+	str_ptr +=  strlen(CSCAN_COMMAND);
+	tlv_size_left = wrqu->data.length - strlen(CSCAN_COMMAND);
+
+	cscan_tlv_temp = (cscan_tlv_t *)str_ptr;
+	memset(ssids_local, 0, sizeof(ssids_local));
+	
+	if ((cscan_tlv_temp->prefix == CSCAN_TLV_PREFIX) &&
+		(cscan_tlv_temp->version == CSCAN_TLV_VERSION) &&
+		(cscan_tlv_temp->subver == CSCAN_TLV_SUBVERSION))
+	{
+		str_ptr += sizeof(cscan_tlv_t);
+		tlv_size_left  -= sizeof(cscan_tlv_t);
+
+		
+		if ((nssid = wl_iw_parse_ssid_list_tlv(&str_ptr, ssids_local,
+			WL_SCAN_PARAMS_SSID_MAX, &tlv_size_left)) <= 0) {
+			WL_ERROR(("SSID is not presented or corrupted ret=%d\n", nssid));
+			goto exit_proc;
+		}
+		else {
+			
+			memset(iscan->iscan_ex_params_p, 0, iscan->iscan_ex_param_size);
+
+			
+			wl_iw_iscan_prep(&iscan->iscan_ex_params_p->params, NULL);
+			iscan->iscan_ex_params_p->version = htod32(ISCAN_REQ_VERSION);
+			iscan->iscan_ex_params_p->action = htod16(WL_SCAN_ACTION_START);
+			iscan->iscan_ex_params_p->scan_duration = htod16(0);
+
+			
+			while (tlv_size_left > 0)
+			{
+			type = str_ptr[0];
+			switch (type) {
+				case CSCAN_TLV_TYPE_CHANNEL_IE:
+					
+					if ((nchan = wl_iw_parse_channel_list_tlv(&str_ptr,
+					&iscan->iscan_ex_params_p->params.channel_list[0],
+					WL_NUMCHANNELS, &tlv_size_left)) == -1) {
+					WL_ERROR(("%s missing channel list\n",
+						__FUNCTION__));
+						goto exit_proc;
+					}
+				break;
+				case CSCAN_TLV_TYPE_NPROBE_IE:
+					if ((res = wl_iw_parse_data_tlv(&str_ptr,
+						&iscan->iscan_ex_params_p->params.nprobes,
+						sizeof(iscan->iscan_ex_params_p->params.nprobes),
+						type, sizeof(char), &tlv_size_left)) == -1) {
+						WL_ERROR(("%s return %d\n",
+							__FUNCTION__, res));
+							goto exit_proc;
+					}
+				break;
+				case CSCAN_TLV_TYPE_ACTIVE_IE:
+					if ((res = wl_iw_parse_data_tlv(&str_ptr,
+					&iscan->iscan_ex_params_p->params.active_time,
+					sizeof(iscan->iscan_ex_params_p->params.active_time),
+					type, sizeof(short), &tlv_size_left)) == -1) {
+						WL_ERROR(("%s return %d\n",
+						__FUNCTION__, res));
+						goto exit_proc;
+					}
+				break;
+				case CSCAN_TLV_TYPE_PASSIVE_IE:
+					if ((res = wl_iw_parse_data_tlv(&str_ptr,
+					&iscan->iscan_ex_params_p->params.passive_time,
+					sizeof(iscan->iscan_ex_params_p->params.passive_time),
+					type, sizeof(short), &tlv_size_left)) == -1) {
+						WL_ERROR(("%s return %d\n",
+						__FUNCTION__, res));
+						goto exit_proc;
+					}
+				break;
+				case CSCAN_TLV_TYPE_HOME_IE:
+					if ((res = wl_iw_parse_data_tlv(&str_ptr,
+					&iscan->iscan_ex_params_p->params.home_time,
+					sizeof(iscan->iscan_ex_params_p->params.home_time),
+					type, sizeof(short), &tlv_size_left)) == -1) {
+						WL_ERROR(("%s return %d\n",
+						__FUNCTION__, res));
+						goto exit_proc;
+					}
+				break;
+				case CSCAN_TLV_TYPE_STYPE_IE:
+					if ((res = wl_iw_parse_data_tlv(&str_ptr,
+					&iscan->iscan_ex_params_p->params.scan_type,
+					sizeof(iscan->iscan_ex_params_p->params.scan_type),
+					type, sizeof(char), &tlv_size_left)) == -1) {
+					WL_ERROR(("%s return %d\n",
+						__FUNCTION__, res));
+						goto exit_proc;
+					}
+				break;
+
+				default :
+					WL_ERROR(("%s get unkwown type %X\n",
+					__FUNCTION__, type));
+					goto exit_proc;
+				break;
+				}
+			} 
+			}
+		}
+		else {
+			WL_ERROR(("%s get wrong TLV command\n", __FUNCTION__));
+			goto exit_proc;
+		}
+
+#if defined(CONFIG_FIRST_SCAN)
+		if (g_first_broadcast_scan < BROADCAST_SCAN_FIRST_RESULT_CONSUMED) {
+			if (++g_first_counter_scans == MAX_ALLOWED_BLOCK_SCAN_FROM_FIRST_SCAN) {
+				
+				WL_ERROR(("%s Clean up First scan flag which is %d\n",
+					__FUNCTION__, g_first_broadcast_scan));
+				g_first_broadcast_scan = BROADCAST_SCAN_FIRST_RESULT_CONSUMED;
+			}
+			else {
+				WL_ERROR(("%s Ignoring CSCAN : First Scan is not done yet %d\n",
+					__FUNCTION__, g_first_counter_scans));
+				res = -EBUSY;
+				goto exit_proc;
+			}
+		}
+#endif 
+
+		
+		res = wl_iw_combined_scan_set(dev, ssids_local, nssid, nchan);
+
+exit_proc:
+	net_os_wake_unlock(dev);
+	return res;
+}
+
+#endif 
+
+#ifdef CONFIG_WPS2
+static int
+wl_iw_del_wps_probe_req_ie(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	int ret;
+	vndr_ie_setbuf_t *ie_delbuf;
+
+	if (g_wps_probe_req_ie) {
+		ie_delbuf = (vndr_ie_setbuf_t *)(g_wps_probe_req_ie + strlen("vndr_ie "));
+		strncpy(ie_delbuf->cmd, "del", 3);
+		ie_delbuf->cmd[3] = '\0';
+
+		ret = dev_wlc_ioctl(dev, WLC_SET_VAR, g_wps_probe_req_ie, g_wps_probe_req_ie_len);
+		if (ret) {
+			WL_ERROR(("ioctl failed %d \n", ret));
+		}
+
+		kfree(g_wps_probe_req_ie);
+		g_wps_probe_req_ie = NULL;
+		g_wps_probe_req_ie_len = 0;
+	}
+
+	return 0;
+}
+
+static int
+wl_iw_add_wps_probe_req_ie(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *extra
+)
+{
+	char *str_ptr = NULL;
+	char *bufptr = NULL;
+	uint buflen, datalen, iecount, pktflag, iolen, total_len;
+	int ret = 0;
+	vndr_ie_setbuf_t *ie_setbuf = NULL;
+
+	if (!g_wps_probe_req_ie) {
+		ret = -1;
+		str_ptr = extra;
+		str_ptr += WPS_PROBE_REQ_IE_CMD_LENGTH;
+		datalen = wrqu->data.length - WPS_PROBE_REQ_IE_CMD_LENGTH;
+
+		
+		
+		buflen = sizeof(vndr_ie_setbuf_t) + datalen - sizeof(vndr_ie_t);
+		ie_setbuf = (vndr_ie_setbuf_t *)kmalloc(buflen, GFP_KERNEL);
+		if (!ie_setbuf) {
+			WL_ERROR(("memory alloc failure ie_setbuf\n"));
+			return ret;
+		}
+
+		memset(ie_setbuf, 0x00, buflen);
+
+		
+		strncpy(ie_setbuf->cmd, "add", VNDR_IE_CMD_LEN - 1);
+		ie_setbuf->cmd[VNDR_IE_CMD_LEN - 1] = '\0';
+
+		
+		iecount = htod32(1);
+		memcpy((void *)&ie_setbuf->vndr_ie_buffer.iecount, &iecount, sizeof(int));
+
+		
+		pktflag = 0x10;
+		memcpy((void *)&ie_setbuf->vndr_ie_buffer.vndr_ie_list[0].pktflag,
+			&pktflag, sizeof(uint32));
+
+		memcpy((void *)&ie_setbuf->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data,
+			str_ptr, datalen);
+
+		total_len = strlen("vndr_ie ") + buflen;
+		bufptr = (char *)kmalloc(total_len, GFP_KERNEL);
+		if (!bufptr) {
+			WL_ERROR(("memory alloc failure bufptr\n"));
+			goto fail;
+		}
+
+		iolen = bcm_mkiovar("vndr_ie", (char *)ie_setbuf, buflen, bufptr, total_len);
+		if (iolen == 0) {
+			WL_ERROR(("Buffer length is illegal\n"));
+			goto fail2;
+		}
+
+		ret = dev_wlc_ioctl(dev, WLC_SET_VAR, bufptr, iolen);
+		if (ret) {
+			WL_ERROR(("ioctl failed\n"));
+			goto fail2;
+		}
+
+		g_wps_probe_req_ie = (char *)kmalloc(iolen, GFP_KERNEL);
+		if (!g_wps_probe_req_ie) {
+			WL_ERROR(("memory alloc failure g_wps_probe_req_ie\n"));
+			goto fail2;
+		}
+
+		memcpy(g_wps_probe_req_ie, bufptr, iolen);
+		g_wps_probe_req_ie_len = iolen;
+	}
+
+fail2:
+	if (bufptr) {
+		kfree(bufptr);
+		bufptr = NULL;
+	}
+fail:
+	if (ie_setbuf) {
+		kfree(ie_setbuf);
+		ie_setbuf = NULL;
+	}
+	return ret;
+}
+#endif
+
+
+#ifdef SOFTAP
+#ifndef AP_ONLY
+
+
+static int
+thr_wait_for_2nd_eth_dev(void *data)
+{
+	wl_iw_t *iw;
+	int ret = 0;
+	unsigned long flags = 0;
+
+	tsk_ctl_t *tsk_ctl = (tsk_ctl_t *)data;
+	struct net_device *dev = (struct net_device *)tsk_ctl->parent;
+	iw = *(wl_iw_t **)netdev_priv(dev);
+
+	DAEMONIZE("wl0_eth_wthread");
+
+
+	WL_SOFTAP(("\n>%s threda started:, PID:%x\n", __FUNCTION__, current->pid));
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	if (!iw) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		tsk_ctl->thr_pid = -1;
+		complete(&tsk_ctl->completed);
+		return -1;
+	}
+	DHD_OS_WAKE_LOCK(iw->pub);
+	complete(&tsk_ctl->completed);
+	if (down_timeout(&tsk_ctl->sema, msecs_to_jiffies(1000)) != 0) {
+#else
+	if (down_interruptible(&tsk_ctl->sema) != 0) {
+#endif 
+		WL_ERROR(("\n%s: sap_eth_sema timeout \n", __FUNCTION__));
+		ret = -1;
+		goto fail;
+	}
+
+	SMP_RD_BARRIER_DEPENDS();
+	if (tsk_ctl->terminated) {
+			ret = -1;
+			goto fail;
+	}
+
+	flags = dhd_os_spin_lock(iw->pub);
+	if (!ap_net_dev) {
+		WL_ERROR((" ap_net_dev is null !!!"));
+		ret = -1;
+		dhd_os_spin_unlock(iw->pub, flags);
+		goto fail;
+	}
+
+	WL_SOFTAP(("\n>%s: Thread:'softap ethdev IF:%s is detected!'\n\n",
+		__FUNCTION__, ap_net_dev->name));
+
+	ap_cfg_running = TRUE;
+
+	dhd_os_spin_unlock(iw->pub, flags);
+	bcm_mdelay(500); 
+
+	
+	wl_iw_send_priv_event(priv_dev, "AP_SET_CFG_OK");
+
+fail:
+
+	DHD_OS_WAKE_UNLOCK(iw->pub);
+
+	WL_SOFTAP(("\n>%s, thread completed\n", __FUNCTION__));
+
+	complete_and_exit(&tsk_ctl->completed, 0);
+	return ret;
+}
+#endif 
+#ifndef AP_ONLY
+static int last_auto_channel = 6;
+#endif
+
+static int
+get_softap_auto_channel(struct net_device *dev, struct ap_profile *ap)
+{
+	int chosen = 0;
+	wl_uint32_list_t request;
+	int retry = 0;
+	int updown = 0;
+	int ret = 0;
+	wlc_ssid_t null_ssid;
+	int res = 0;
+#ifndef AP_ONLY
+	int iolen = 0;
+	int mkvar_err = 0;
+	int bsscfg_index = 1;
+	char buf[WLC_IOCTL_SMLEN];
+#endif
+	WL_SOFTAP(("Enter %s\n", __FUNCTION__));
+
+#ifndef AP_ONLY
+	if (ap_cfg_running) {
+		ap->channel = last_auto_channel;
+		return res;
+	}
+#endif
+
+	memset(&null_ssid, 0, sizeof(wlc_ssid_t));
+	res |= dev_wlc_ioctl(dev, WLC_UP, &updown, sizeof(updown));
+
+#ifdef AP_ONLY
+	res |= dev_wlc_ioctl(dev, WLC_SET_SSID, &null_ssid, sizeof(null_ssid));
+#else
+
+	iolen = wl_bssiovar_mkbuf("ssid", bsscfg_index, (char *)(&null_ssid),
+		null_ssid.SSID_len+4, buf, sizeof(buf), &mkvar_err);
+	ASSERT(iolen);
+	res |= dev_wlc_ioctl(dev, WLC_SET_VAR, buf, iolen);
+
+#endif
+
+	request.count = htod32(0);
+	ret = dev_wlc_ioctl(dev, WLC_START_CHANNEL_SEL, &request, sizeof(request));
+	if (ret < 0) {
+		WL_ERROR(("can't start auto channel scan\n"));
+		goto fail;
+	}
+
+	get_channel_retry:
+		bcm_mdelay(350);
+
+	ret = dev_wlc_ioctl(dev, WLC_GET_CHANNEL_SEL, &chosen, sizeof(chosen));
+		if (ret < 0 || dtoh32(chosen) == 0) {
+			if (retry++ < 15) {
+				goto get_channel_retry;
+			} else {
+				if (ret < 0) {
+					WL_ERROR(("can't get auto channel sel, err = %d, "
+					          "chosen = 0x%04X\n", ret, (uint16)chosen));
+					goto fail;
+				} else {
+					ap->channel = (uint16)last_auto_channel;
+					WL_ERROR(("auto channel sel timed out. we get channel %d\n",
+						ap->channel));
+				}
+			}
+		}
+
+		if (chosen) {
+			ap->channel = (uint16)chosen & 0x00FF;
+			WL_SOFTAP(("%s: Got auto channel = %d, attempt:%d\n",
+				__FUNCTION__, ap->channel, retry));
+		}
+
+		if ((res = dev_wlc_ioctl(dev, WLC_DOWN, &updown, sizeof(updown))) < 0) {
+			WL_ERROR(("%s fail to set up err =%d\n", __FUNCTION__, res));
+			goto fail;
+		}
+
+#ifndef AP_ONLY
+	if (!res || !ret)
+		last_auto_channel = ap->channel;
+#endif
+
+fail :
+	if (ret < 0) {
+		WL_TRACE(("%s: return value %d\n", __FUNCTION__, ret));
+		return ret;
+	}
+	return res;
+} 
+
+
+static int
+set_ap_cfg(struct net_device *dev, struct ap_profile *ap)
+{
+	int updown = 0;
+	int channel = 0;
+
+	wlc_ssid_t ap_ssid;
+	int max_assoc = 8;
+
+	int res = 0;
+	int apsta_var = 0;
+#ifndef AP_ONLY
+	int mpc = 0;
+	int iolen = 0;
+	int mkvar_err = 0;
+	int bsscfg_index = 1;
+	char buf[WLC_IOCTL_SMLEN];
+#endif
+
+	if (!dev) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return -1;
+	}
+
+	net_os_wake_lock(dev);
+	DHD_OS_MUTEX_LOCK(&wl_softap_lock);
+
+	WL_SOFTAP(("wl_iw: set ap profile:\n"));
+	WL_SOFTAP(("	ssid = '%s'\n", ap->ssid));
+	WL_SOFTAP(("	security = '%s'\n", ap->sec));
+	if (ap->key[0] != '\0')
+		WL_SOFTAP(("	key = '%s'\n", ap->key));
+	WL_SOFTAP(("	channel = %d\n", ap->channel));
+	WL_SOFTAP(("	max scb = %d\n", ap->max_scb));
+
+#ifdef AP_ONLY
+	if (ap_cfg_running) {
+		wl_iw_softap_deassoc_stations(dev, NULL); 
+		ap_cfg_running = FALSE;
+	}
+#endif	
+
+	
+	if (ap_cfg_running == FALSE) {
+
+#ifndef AP_ONLY
+
+		
+		sema_init(&ap_eth_ctl.sema, 0);
+
+		mpc = 0;
+		if ((res = dev_wlc_intvar_set(dev, "mpc", mpc))) {
+			WL_ERROR(("%s fail to set mpc\n", __FUNCTION__));
+			goto fail;
+		}
+#endif
+
+		updown = 0;
+		if ((res = dev_wlc_ioctl(dev, WLC_DOWN, &updown, sizeof(updown)))) {
+			WL_ERROR(("%s fail to set updown\n", __FUNCTION__));
+			goto fail;
+		}
+
+#ifdef AP_ONLY
+		
+		apsta_var = 0;
+		if ((res = dev_wlc_ioctl(dev, WLC_SET_AP, &apsta_var, sizeof(apsta_var)))) {
+			WL_ERROR(("%s fail to set apsta_var 0\n", __FUNCTION__));
+			goto fail;
+		}
+		apsta_var = 1;
+		if ((res = dev_wlc_ioctl(dev, WLC_SET_AP, &apsta_var, sizeof(apsta_var)))) {
+			WL_ERROR(("%s fail to set apsta_var 1\n", __FUNCTION__));
+			goto fail;
+		}
+		res = dev_wlc_ioctl(dev, WLC_GET_AP, &apsta_var, sizeof(apsta_var));
+#else
+		
+		apsta_var = 1;
+		iolen = wl_bssiovar_mkbuf("apsta",
+			bsscfg_index,  &apsta_var, sizeof(apsta_var)+4,
+			buf, sizeof(buf), &mkvar_err);
+		ASSERT(iolen);
+		if ((res = dev_wlc_ioctl(dev, WLC_SET_VAR, buf, iolen)) < 0) {
+			WL_ERROR(("%s fail to set apsta \n", __FUNCTION__));
+			goto fail;
+		}
+		WL_TRACE(("\n>in %s: apsta set result: %d \n", __FUNCTION__, res));
+
+
+		mpc = 0;
+		if ((res = dev_wlc_intvar_set(dev, "mpc", mpc))) {
+			WL_ERROR(("%s fail to set mpc\n", __FUNCTION__));
+			goto fail;
+		}
+
+
+#endif 
+
+		updown = 1;
+		if ((res = dev_wlc_ioctl(dev, WLC_UP, &updown, sizeof(updown))) < 0) {
+			WL_ERROR(("%s fail to set apsta \n", __FUNCTION__));
+			goto fail;
+		}
+
+	} else {
+		
+		if (!ap_net_dev) {
+			WL_ERROR(("%s: ap_net_dev is null\n", __FUNCTION__));
+			goto fail;
+		}
+
+		res = wl_iw_softap_deassoc_stations(ap_net_dev, NULL);
+
+		
+		if ((res = dev_iw_write_cfg1_bss_var(dev, 0)) < 0) {
+			WL_ERROR(("%s fail to set bss down\n", __FUNCTION__));
+			goto fail;
+		}
+	}
+
+
+	if (strlen(ap->country_code)) {
+		WL_ERROR(("%s: Igonored: Country MUST be specified"
+			"COUNTRY command with \n",	__FUNCTION__));
+	} else {
+		WL_SOFTAP(("%s: Country code is not specified,"
+			" will use Radio's default\n",
+			__FUNCTION__));
+
+	}
+	iolen = wl_bssiovar_mkbuf("closednet",
+		bsscfg_index,  &ap->closednet, sizeof(ap->closednet)+4,
+		buf, sizeof(buf), &mkvar_err);
+	ASSERT(iolen);
+	if ((res = dev_wlc_ioctl(dev, WLC_SET_VAR, buf, iolen)) < 0) {
+		WL_ERROR(("%s failed to set 'closednet'for apsta \n", __FUNCTION__));
+		goto fail;
+	}
+
+	
+	if ((ap->channel == 0) && (get_softap_auto_channel(dev, ap) < 0)) {
+		ap->channel = 1;
+		WL_ERROR(("%s auto channel failed, use channel=%d\n",
+		          __FUNCTION__, ap->channel));
+	}
+
+	channel = ap->channel;
+	if ((res = dev_wlc_ioctl(dev, WLC_SET_CHANNEL, &channel, sizeof(channel)))) {
+		WL_ERROR(("%s fail to set channel\n", __FUNCTION__));
+	}
+
+
+	if (ap_cfg_running == FALSE) {
+		updown = 0;
+		if ((res = dev_wlc_ioctl(dev, WLC_UP, &updown, sizeof(updown)))) {
+			WL_ERROR(("%s fail to set up\n", __FUNCTION__));
+			goto fail;
+		}
+	}
+
+	max_assoc = ap->max_scb;
+	if ((res = dev_wlc_intvar_set(dev, "maxassoc", max_assoc))) {
+		WL_ERROR(("%s fail to set maxassoc\n", __FUNCTION__));
+		goto fail;
+	}
+
+	ap_ssid.SSID_len = strlen(ap->ssid);
+	strncpy(ap_ssid.SSID, ap->ssid, ap_ssid.SSID_len);
+
+	
+#ifdef AP_ONLY
+	if ((res = wl_iw_set_ap_security(dev, &my_ap)) != 0) {
+		WL_ERROR(("ERROR:%d in:%s, wl_iw_set_ap_security is skipped\n",
+		          res, __FUNCTION__));
+		goto fail;
+	}
+	wl_iw_send_priv_event(dev, "ASCII_CMD=AP_BSS_START");
+	ap_cfg_running = TRUE;
+#else
+
+	iolen = wl_bssiovar_mkbuf("ssid", bsscfg_index, (char *)(&ap_ssid),
+		ap_ssid.SSID_len+4, buf, sizeof(buf), &mkvar_err);
+	ASSERT(iolen);
+	if ((res = dev_wlc_ioctl(dev, WLC_SET_VAR, buf, iolen)) != 0) {
+		WL_ERROR(("ERROR:%d in:%s, Security & BSS reconfiguration is skipped\n",
+		          res, __FUNCTION__));
+		goto fail;
+	}
+	if (ap_cfg_running == FALSE) {
+		
+		PROC_START(thr_wait_for_2nd_eth_dev, dev, &ap_eth_ctl, 0);
+	} else {
+		ap_eth_ctl.thr_pid = -1;
+		
+		if (ap_net_dev == NULL) {
+			WL_ERROR(("%s ERROR: ap_net_dev is NULL !!!\n", __FUNCTION__));
+			goto fail;
+		}
+
+		WL_ERROR(("%s: %s Configure security & restart AP bss \n",
+		          __FUNCTION__, ap_net_dev->name));
+
+		
+		if ((res = wl_iw_set_ap_security(ap_net_dev, &my_ap)) < 0) {
+			WL_ERROR(("%s fail to set security : %d\n", __FUNCTION__, res));
+			goto fail;
+		}
+
+		
+		if ((res = dev_iw_write_cfg1_bss_var(dev, 1)) < 0) {
+			WL_ERROR(("%s fail to set bss up\n", __FUNCTION__));
+			goto fail;
+		}
+	}
+#endif 
+fail:
+	WL_SOFTAP(("%s exit with %d\n", __FUNCTION__, res));
+
+	DHD_OS_MUTEX_UNLOCK(&wl_softap_lock);
+	net_os_wake_unlock(dev);
+
+	return res;
+}
+#endif 
+
+
+
+static int
+wl_iw_set_ap_security(struct net_device *dev, struct ap_profile *ap)
+{
+	int wsec = 0;
+	int wpa_auth = 0;
+	int res = 0;
+	int i;
+	char *ptr;
+#ifdef AP_ONLY
+	int mpc = 0;
+	wlc_ssid_t ap_ssid;
+#endif
+	wl_wsec_key_t key;
+
+	WL_SOFTAP(("\nsetting SOFTAP security mode:\n"));
+	WL_SOFTAP(("wl_iw: set ap profile:\n"));
+	WL_SOFTAP(("	ssid = '%s'\n", ap->ssid));
+	WL_SOFTAP(("	security = '%s'\n", ap->sec));
+	if (ap->key[0] != '\0')
+		WL_SOFTAP(("	key = '%s'\n", ap->key));
+	WL_SOFTAP(("	channel = %d\n", ap->channel));
+	WL_SOFTAP(("	max scb = %d\n", ap->max_scb));
+
+
+	if (strnicmp(ap->sec, "open", strlen("open")) == 0) {
+
+	   
+		wsec = 0;
+		res = dev_wlc_intvar_set(dev, "wsec", wsec);
+		wpa_auth = WPA_AUTH_DISABLED;
+		res |= dev_wlc_intvar_set(dev, "wpa_auth", wpa_auth);
+
+		WL_SOFTAP(("=====================\n"));
+		WL_SOFTAP((" wsec & wpa_auth set 'OPEN', result:&d %d\n", res));
+		WL_SOFTAP(("=====================\n"));
+
+	} else if (strnicmp(ap->sec, "wep", strlen("wep")) == 0) {
+
+	   
+		memset(&key, 0, sizeof(key));
+
+		wsec = WEP_ENABLED;
+		res = dev_wlc_intvar_set(dev, "wsec", wsec);
+
+		key.index = 0;
+		if (wl_iw_parse_wep(ap->key, &key)) {
+			WL_SOFTAP(("wep key parse err!\n"));
+			return -1;
+		}
+
+		key.index = htod32(key.index);
+		key.len = htod32(key.len);
+		key.algo = htod32(key.algo);
+		key.flags = htod32(key.flags);
+
+		res |= dev_wlc_ioctl(dev, WLC_SET_KEY, &key, sizeof(key));
+
+		wpa_auth = WPA_AUTH_DISABLED;
+		res |= dev_wlc_intvar_set(dev, "wpa_auth", wpa_auth);
+
+		WL_SOFTAP(("=====================\n"));
+		WL_SOFTAP((" wsec & auth set 'WEP', result:&d %d\n", res));
+		WL_SOFTAP(("=====================\n"));
+
+	} else if (strnicmp(ap->sec, "wpa2-psk", strlen("wpa2-psk")) == 0) {
+
+	   
+
+		wsec_pmk_t psk;
+		size_t key_len;
+
+		wsec = AES_ENABLED;
+		dev_wlc_intvar_set(dev, "wsec", wsec);
+
+		key_len = strlen(ap->key);
+		if (key_len < WSEC_MIN_PSK_LEN || key_len > WSEC_MAX_PSK_LEN) {
+			WL_SOFTAP(("passphrase must be between %d and %d characters long\n",
+			WSEC_MIN_PSK_LEN, WSEC_MAX_PSK_LEN));
+			return -1;
+		}
+
+		
+		if (key_len < WSEC_MAX_PSK_LEN) {
+			unsigned char output[2*SHA1HashSize];
+			char key_str_buf[WSEC_MAX_PSK_LEN+1];
+
+			
+			memset(output, 0, sizeof(output));
+			pbkdf2_sha1(ap->key, ap->ssid, strlen(ap->ssid), 4096, output, 32);
+			
+			ptr = key_str_buf;
+			for (i = 0; i < (WSEC_MAX_PSK_LEN/8); i++) {
+				
+				sprintf(ptr, "%02x%02x%02x%02x", (uint)output[i*4],
+				        (uint)output[i*4+1], (uint)output[i*4+2],
+				        (uint)output[i*4+3]);
+				ptr += 8;
+			}
+			WL_SOFTAP(("%s: passphase = %s\n", __FUNCTION__, key_str_buf));
+
+			psk.key_len = htod16((ushort)WSEC_MAX_PSK_LEN);
+			memcpy(psk.key, key_str_buf, psk.key_len);
+		} else {
+			psk.key_len = htod16((ushort) key_len);
+			memcpy(psk.key, ap->key, key_len);
+		}
+		psk.flags = htod16(WSEC_PASSPHRASE);
+		dev_wlc_ioctl(dev, WLC_SET_WSEC_PMK, &psk, sizeof(psk));
+
+		wpa_auth = WPA2_AUTH_PSK;
+		dev_wlc_intvar_set(dev, "wpa_auth", wpa_auth);
+
+	} else if (strnicmp(ap->sec, "wpa-psk", strlen("wpa-psk")) == 0) {
+
+		
+		wsec_pmk_t psk;
+		size_t key_len;
+
+		wsec = TKIP_ENABLED;
+		res = dev_wlc_intvar_set(dev, "wsec", wsec);
+
+		key_len = strlen(ap->key);
+		if (key_len < WSEC_MIN_PSK_LEN || key_len > WSEC_MAX_PSK_LEN) {
+			WL_SOFTAP(("passphrase must be between %d and %d characters long\n",
+			WSEC_MIN_PSK_LEN, WSEC_MAX_PSK_LEN));
+			return -1;
+		}
+
+		
+		if (key_len < WSEC_MAX_PSK_LEN) {
+			unsigned char output[2*SHA1HashSize];
+			char key_str_buf[WSEC_MAX_PSK_LEN+1];
+			bzero(output, 2*SHA1HashSize);
+
+			WL_SOFTAP(("%s: do passhash...\n", __FUNCTION__));
+			
+			pbkdf2_sha1(ap->key, ap->ssid, strlen(ap->ssid), 4096, output, 32);
+			
+			ptr = key_str_buf;
+			for (i = 0; i < (WSEC_MAX_PSK_LEN/8); i++) {
+				WL_SOFTAP(("[%02d]: %08x\n", i, *((unsigned int*)&output[i*4])));
+				
+				sprintf(ptr, "%02x%02x%02x%02x", (uint)output[i*4],
+					(uint)output[i*4+1], (uint)output[i*4+2],
+				        (uint)output[i*4+3]);
+				ptr += 8;
+			}
+			printk("%s: passphase = %s\n", __FUNCTION__, key_str_buf);
+
+			psk.key_len = htod16((ushort)WSEC_MAX_PSK_LEN);
+			memcpy(psk.key, key_str_buf, psk.key_len);
+		} else {
+			psk.key_len = htod16((ushort) key_len);
+			memcpy(psk.key, ap->key, key_len);
+		}
+
+		psk.flags = htod16(WSEC_PASSPHRASE);
+		res |= dev_wlc_ioctl(dev, WLC_SET_WSEC_PMK, &psk, sizeof(psk));
+
+		wpa_auth = WPA_AUTH_PSK;
+		res |= dev_wlc_intvar_set(dev, "wpa_auth", wpa_auth);
+
+		WL_SOFTAP((" wsec & auth set 'wpa-psk' (TKIP), result:&d %d\n", res));
+	}
+
+#ifdef AP_ONLY
+		ap_ssid.SSID_len = strlen(ap->ssid);
+		strncpy(ap_ssid.SSID, ap->ssid, ap_ssid.SSID_len);
+		res |= dev_wlc_ioctl(dev, WLC_SET_SSID, &ap_ssid, sizeof(ap_ssid));
+		mpc = 0;
+		res |= dev_wlc_intvar_set(dev, "mpc", mpc);
+		if (strnicmp(ap->sec, "wep", strlen("wep")) == 0) {
+			res |= dev_wlc_ioctl(dev, WLC_SET_KEY, &key, sizeof(key));
+		}
+#endif
+	return res;
+}
+
+
+
+static int
+get_parameter_from_string(
+			char **str_ptr, const char *token,
+			int param_type, void  *dst, int param_max_len)
+{
+	char int_str[7] = "0";
+	int parm_str_len;
+	char  *param_str_begin;
+	char  *param_str_end;
+	char  *orig_str = *str_ptr;
+
+	if ((*str_ptr) && !strncmp(*str_ptr, token, strlen(token))) {
+
+		strsep(str_ptr, "=,"); 
+		param_str_begin = *str_ptr;
+		strsep(str_ptr, "=,"); 
+
+		if (*str_ptr == NULL) {
+			
+			parm_str_len = strlen(param_str_begin);
+		} else {
+			param_str_end = *str_ptr-1;  
+			parm_str_len = param_str_end - param_str_begin;
+		}
+
+		WL_TRACE((" 'token:%s', len:%d, ", token, parm_str_len));
+
+		if (parm_str_len > param_max_len) {
+			WL_ERROR((" WARNING: extracted param len:%d is > MAX:%d\n",
+				parm_str_len, param_max_len));
+
+			parm_str_len = param_max_len;
+		}
+
+		switch (param_type) {
+
+			case PTYPE_INTDEC: {
+			
+				int *pdst_int = dst;
+				char *eptr;
+
+				if (parm_str_len > sizeof(int_str))
+					 parm_str_len = sizeof(int_str);
+
+				memcpy(int_str, param_str_begin, parm_str_len);
+
+				*pdst_int = simple_strtoul(int_str, &eptr, 10);
+
+				WL_TRACE((" written as integer:%d\n",  *pdst_int));
+			}
+			break;
+			case PTYPE_STR_HEX: {
+				u8 *buf = dst;
+				
+				param_max_len = param_max_len >> 1;  
+				hstr_2_buf(param_str_begin, buf, param_max_len);
+				dhd_print_buf(buf, param_max_len, 0);
+			}
+			break;
+			default:
+				
+				memcpy(dst, param_str_begin, parm_str_len);
+				*((char *)dst + parm_str_len) = 0; 
+				WL_ERROR((" written as a string:%s\n", (char *)dst));
+			break;
+
+		}
+
+		return 0;
+	} else {
+		WL_ERROR(("\n %s: ERROR: can't find token:%s in str:%s \n",
+			__FUNCTION__, token, orig_str));
+
+	 return -1;
+	}
+}
+
+static int wl_iw_softap_deassoc_stations(struct net_device *dev, u8 *mac)
+{
+	int i;
+	int res = 0;
+	char mac_buf[128] = {0};
+	char z_mac[6] = {0, 0, 0, 0, 0, 0};
+	char *sta_mac;
+	struct maclist *assoc_maclist = (struct maclist *) mac_buf;
+	bool deauth_all = FALSE;
+
+	
+	if (mac == NULL) {
+		deauth_all = TRUE;
+		sta_mac = z_mac;  
+	} else {
+		sta_mac = mac;  
+	}
+
+	memset(assoc_maclist, 0, sizeof(mac_buf));
+	assoc_maclist->count = 8; 
+
+	res = dev_wlc_ioctl(dev, WLC_GET_ASSOCLIST, assoc_maclist, 128);
+	if (res != 0) {
+		WL_SOFTAP(("%s: Error:%d Couldn't get ASSOC List\n", __FUNCTION__, res));
+		return res;
+	}
+
+	if (assoc_maclist->count)
+		for (i = 0; i < assoc_maclist->count; i++) {
+		scb_val_t scbval;
+		scbval.val = htod32(1);
+		
+		bcopy(&assoc_maclist->ea[i], &scbval.ea, ETHER_ADDR_LEN);
+
+		if (deauth_all || (memcmp(&scbval.ea, sta_mac, ETHER_ADDR_LEN) == 0))  {
+			
+			WL_SOFTAP(("%s, deauth STA:%d \n", __FUNCTION__, i));
+			res |= dev_wlc_ioctl(dev, WLC_SCB_DEAUTHENTICATE_FOR_REASON,
+				&scbval, sizeof(scb_val_t));
+		}
+	} else WL_SOFTAP(("%s: No Stations \n", __FUNCTION__));
+
+	if (res != 0) {
+		WL_ERROR(("%s: Error:%d\n", __FUNCTION__, res));
+	} else if (assoc_maclist->count) {
+		
+		bcm_mdelay(200);
+	}
+	return res;
+}
+
+
+
+static int
+iwpriv_softap_stop(struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *ext)
+{
+	int res = 0;
+
+	WL_SOFTAP(("got iwpriv AP_BSS_STOP \n"));
+
+	if ((!dev) && (!ap_net_dev)) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return res;
+	}
+
+	net_os_wake_lock(dev);
+	DHD_OS_MUTEX_LOCK(&wl_softap_lock);
+
+	if  ((ap_cfg_running == TRUE)) {
+#ifdef AP_ONLY
+		 wl_iw_softap_deassoc_stations(dev, NULL);
+#else
+		 wl_iw_softap_deassoc_stations(ap_net_dev, NULL);
+		if ((res = dev_iw_write_cfg1_bss_var(dev, 2)) < 0)
+			WL_ERROR(("%s failed to del BSS err = %d", __FUNCTION__, res));
+#endif
+
+		
+		bcm_mdelay(100);
+
+		wrqu->data.length = 0;
+		ap_cfg_running = FALSE;
+	} else
+		WL_ERROR(("%s: was called when SoftAP is OFF : move on\n", __FUNCTION__));
+
+	WL_SOFTAP(("%s Done with %d\n", __FUNCTION__, res));
+	DHD_OS_MUTEX_UNLOCK(&wl_softap_lock);
+	net_os_wake_unlock(dev);
+
+	return res;
+}
+
+
+
+static int
+iwpriv_fw_reload(struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu,
+	char *ext)
+{
+	int ret = -1;
+	char extra[256];
+	char *fwstr = fw_path ; 
+
+	WL_SOFTAP(("current firmware_path[]=%s\n", fwstr));
+
+	WL_TRACE((">Got FW_RELOAD cmd:"
+	          "info->cmd:%x, info->flags:%x, u.data:%p, u.len:%d, "
+	          "fw_path:%p, len:%d \n",
+	          info->cmd, info->flags,
+	          wrqu->data.pointer, wrqu->data.length, fwstr, strlen(fwstr)));
+
+	if ((wrqu->data.length > 4) && (wrqu->data.length < sizeof(extra))) {
+		char *str_ptr;
+
+		if (copy_from_user(extra, wrqu->data.pointer, wrqu->data.length)) {
+			ret = -EFAULT;
+			goto exit_proc;
+		}
+
+		
+		extra[wrqu->data.length] = 8;
+		str_ptr = extra;
+
+		if (get_parameter_from_string(&str_ptr,
+		                              "FW_PATH=", PTYPE_STRING, fwstr, 255) != 0) {
+			WL_ERROR(("Error: extracting FW_PATH='' string\n"));
+			goto exit_proc;
+		}
+
+		if  (strstr(fwstr, "apsta") != NULL) {
+			  WL_SOFTAP(("GOT APSTA FIRMWARE\n"));
+			  ap_fw_loaded = TRUE;
+		} else {
+			WL_SOFTAP(("GOT STA FIRMWARE\n"));
+			ap_fw_loaded = FALSE;
+		}
+
+		WL_SOFTAP(("SET firmware_path[]=%s , str_p:%p\n", fwstr, fwstr));
+		ret = 0;
+	} else {
+		WL_ERROR(("Error: ivalid param len:%d\n", wrqu->data.length));
+	}
+
+exit_proc:
+	return ret;
+}
+
+#ifdef SOFTAP
+
+static int
+iwpriv_wpasupp_loop_tst(struct net_device *dev,
+            struct iw_request_info *info,
+            union iwreq_data *wrqu,
+            char *ext)
+{
+	int res = 0;
+	char *params = NULL;
+
+	WL_TRACE((">Got IWPRIV  wp_supp loopback cmd test:"
+	          "info->cmd:%x, info->flags:%x, u.data:%p, u.len:%d\n",
+	          info->cmd, info->flags,
+	          wrqu->data.pointer, wrqu->data.length));
+
+	if (wrqu->data.length != 0) {
+
+		if (!(params = kmalloc(wrqu->data.length+1, GFP_KERNEL)))
+			return -ENOMEM;
+
+
+		if (copy_from_user(params, wrqu->data.pointer, wrqu->data.length)) {
+			kfree(params);
+			return -EFAULT;
+		}
+
+		params[wrqu->data.length] = 0;
+		WL_SOFTAP(("\n>> copied from user:\n %s\n", params));
+	} else {
+		WL_ERROR(("ERROR param length is 0\n"));
+		return -EFAULT;
+	}
+
+	
+	res = wl_iw_send_priv_event(dev, params);
+	kfree(params);
+
+	return res;
+}
+#endif 
+
+
+static int
+iwpriv_en_ap_bss(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	void *wrqu,
+	char *extra)
+{
+	int res = 0;
+
+	if (!dev) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return -1;
+	}
+
+	net_os_wake_lock(dev);
+	DHD_OS_MUTEX_LOCK(&wl_softap_lock);
+
+	WL_TRACE(("%s: rcvd IWPRIV IOCTL:  for dev:%s\n", __FUNCTION__, dev->name));
+
+	
+#ifndef AP_ONLY
+	if ((res = wl_iw_set_ap_security(dev, &my_ap)) != 0) {
+		WL_ERROR((" %s ERROR setting SOFTAP security in :%d\n", __FUNCTION__, res));
+	}
+	else {
+		
+		if ((res = dev_iw_write_cfg1_bss_var(dev, 1)) < 0)
+			WL_ERROR(("%s fail to set bss up err=%d\n", __FUNCTION__, res));
+		else
+			
+			bcm_mdelay(100);
+	}
+
+#endif 
+	WL_SOFTAP(("%s done with res %d \n", __FUNCTION__, res));
+
+	DHD_OS_MUTEX_UNLOCK(&wl_softap_lock);
+	net_os_wake_unlock(dev);
+
+	return res;
+}
+
+static int
+get_assoc_sta_list(struct net_device *dev, char *buf, int len)
+{
+	
+	WL_TRACE(("%s: dev_wlc_ioctl(dev:%p, cmd:%d, buf:%p, len:%d)\n",
+		__FUNCTION__, dev, WLC_GET_ASSOCLIST, buf, len));
+
+	return dev_wlc_ioctl(dev, WLC_GET_ASSOCLIST, buf, len);
+
+}
+
+
+void check_error(int res, const char *msg, const char *func, int line)
+{
+	if (res != 0)
+		WL_ERROR(("%s, %d function:%s, line:%d\n", msg, res, func, line));
+}
+
+static int
+set_ap_mac_list(struct net_device *dev, void *buf)
+{
+	struct mac_list_set *mac_list_set = (struct mac_list_set *)buf;
+	struct maclist *maclist = (struct maclist *)&mac_list_set->mac_list;
+	int length;
+	int i;
+	int mac_mode = mac_list_set->mode;
+	int ioc_res = 0;
+	ap_macmode = mac_list_set->mode;  
+
+	
+	bzero(&ap_black_list, sizeof(struct mflist));
+
+	if (mac_mode == MACLIST_MODE_DISABLED) {
+		
+		ioc_res = dev_wlc_ioctl(dev, WLC_SET_MACMODE, &mac_mode, sizeof(mac_mode));
+		check_error(ioc_res, "ioctl ERROR:", __FUNCTION__, __LINE__);
+		WL_SOFTAP(("%s: MAC filtering disabled\n", __FUNCTION__));
+	} else {
+		
+		scb_val_t scbval;
+		char mac_buf[256] = {0};
+		struct maclist *assoc_maclist = (struct maclist *) mac_buf;
+
+		
+		bcopy(maclist, &ap_black_list, sizeof(ap_black_list));
+
+		
+		ioc_res = dev_wlc_ioctl(dev, WLC_SET_MACMODE, &mac_mode, sizeof(mac_mode));
+		check_error(ioc_res, "ioctl ERROR:", __FUNCTION__, __LINE__);
+
+		
+		length = sizeof(maclist->count) + maclist->count*ETHER_ADDR_LEN;
+		dev_wlc_ioctl(dev, WLC_SET_MACLIST, maclist, length);
+
+		WL_SOFTAP(("%s: applied MAC List, mode:%d, length %d:\n",
+			__FUNCTION__, mac_mode, length));
+
+		for (i = 0; i < maclist->count; i++)
+			WL_SOFTAP(("mac %d: %02X:%02X:%02X:%02X:%02X:%02X\n",
+				i, maclist->ea[i].octet[0], maclist->ea[i].octet[1],
+				maclist->ea[i].octet[2],
+				maclist->ea[i].octet[3], maclist->ea[i].octet[4],
+				maclist->ea[i].octet[5]));
+
+		
+		assoc_maclist->count = 8;
+		ioc_res = dev_wlc_ioctl(dev, WLC_GET_ASSOCLIST, assoc_maclist, 256);
+		check_error(ioc_res, "ioctl ERROR:", __FUNCTION__, __LINE__);
+		WL_SOFTAP((" Cur assoc clients:%d\n", assoc_maclist->count));
+
+		
+		if (assoc_maclist->count)
+			for (i = 0; i < assoc_maclist->count; i++) {
+				int j;
+				bool assoc_mac_matched = FALSE;
+				
+				WL_SOFTAP(("\n Cheking assoc STA: "));
+				dhd_print_buf(&assoc_maclist->ea[i], 6, 7);
+				WL_SOFTAP(("with the b/w list:"));
+
+				for (j = 0; j < maclist->count; j++)
+					if (!bcmp(&assoc_maclist->ea[i], &maclist->ea[j],
+						ETHER_ADDR_LEN)) {
+						
+						assoc_mac_matched = TRUE;
+						break;
+					}
+
+				
+				if (((mac_mode == MACLIST_MODE_ALLOW) && !assoc_mac_matched) ||
+					((mac_mode == MACLIST_MODE_DENY) && assoc_mac_matched)) {
+
+					WL_SOFTAP(("b-match or w-mismatch,"
+								" do deauth/disassoc \n"));
+							scbval.val = htod32(1);
+							bcopy(&assoc_maclist->ea[i], &scbval.ea,
+							ETHER_ADDR_LEN);
+							ioc_res = dev_wlc_ioctl(dev,
+								WLC_SCB_DEAUTHENTICATE_FOR_REASON,
+								&scbval, sizeof(scb_val_t));
+							check_error(ioc_res,
+								"ioctl ERROR:",
+								__FUNCTION__, __LINE__);
+
+				} else {
+					WL_SOFTAP((" no b/w list hits, let it be\n"));
+				}
+		} else {
+			WL_SOFTAP(("No ASSOC CLIENTS\n"));
+		} 
+
+	} 
+
+	WL_SOFTAP(("%s iocres:%d\n", __FUNCTION__, ioc_res));
+	return ioc_res;
+}
+#endif 
+
+
+
+#ifdef SOFTAP
+#define PARAM_OFFSET PROFILE_OFFSET
+
+static int
+wl_iw_process_private_ascii_cmd(
+			struct net_device *dev,
+			struct iw_request_info *info,
+			union iwreq_data *dwrq,
+			char *cmd_str)
+{
+	int ret = 0;
+	char *sub_cmd = cmd_str + PROFILE_OFFSET + strlen("ASCII_CMD=");
+
+	WL_SOFTAP(("\n %s: ASCII_CMD: offs_0:%s, offset_32:\n'%s'\n",
+		__FUNCTION__, cmd_str, cmd_str + PROFILE_OFFSET));
+
+	if (strnicmp(sub_cmd, "AP_CFG", strlen("AP_CFG")) == 0) {
+
+		WL_SOFTAP((" AP_CFG \n"));
+
+		
+		if (init_ap_profile_from_string(cmd_str+PROFILE_OFFSET, &my_ap) != 0) {
+				WL_ERROR(("ERROR: SoftAP CFG prams !\n"));
+				ret = -1;
+		} else {
+			ret = set_ap_cfg(dev, &my_ap);
+		}
+
+	} else if (strnicmp(sub_cmd, "AP_BSS_START", strlen("AP_BSS_START")) == 0) {
+
+		WL_SOFTAP(("\n SOFTAP - ENABLE BSS \n"));
+
+		
+		WL_SOFTAP(("\n!!! got 'WL_AP_EN_BSS' from WPA supplicant, dev:%s\n", dev->name));
+
+#ifndef AP_ONLY
+		if (ap_net_dev == NULL) {
+				 printf("\n ERROR: SOFTAP net_dev* is NULL !!!\n");
+		} else {
+			  
+			if ((ret = iwpriv_en_ap_bss(ap_net_dev, info, dwrq, cmd_str)) < 0)
+				WL_ERROR(("%s line %d fail to set bss up\n",
+					__FUNCTION__, __LINE__));
+		}
+#else
+		if ((ret = iwpriv_en_ap_bss(dev, info, dwrq, cmd_str)) < 0)
+				WL_ERROR(("%s line %d fail to set bss up\n",
+					__FUNCTION__, __LINE__));
+#endif
+	} else if (strnicmp(sub_cmd, "ASSOC_LST", strlen("ASSOC_LST")) == 0) {
+
+		
+
+	} else if (strnicmp(sub_cmd, "AP_BSS_STOP", strlen("AP_BSS_STOP")) == 0) {
+
+		WL_SOFTAP((" \n temp DOWN SOFTAP\n"));
+#ifndef AP_ONLY
+		if ((ret = dev_iw_write_cfg1_bss_var(dev, 0)) < 0) {
+				WL_ERROR(("%s line %d fail to set bss down\n",
+					__FUNCTION__, __LINE__));
+		}
+#endif
+	}
+
+	return ret;
+
+}
+#endif 
+
+
+static int
+wl_iw_set_priv(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	struct iw_point *dwrq,
+	char *ext
+)
+{
+	int ret = 0;
+	char * extra;
+
+	if (!(extra = kmalloc(dwrq->length, GFP_KERNEL)))
+	    return -ENOMEM;
+
+	if (copy_from_user(extra, dwrq->pointer, dwrq->length)) {
+	    kfree(extra);
+	    return -EFAULT;
+	}
+
+	WL_TRACE(("%s: SIOCSIWPRIV request %s, info->cmd:%x, info->flags:%d\n dwrq->length:%d\n",
+		dev->name, extra, info->cmd, info->flags, dwrq->length));
+
+	
+
+	net_os_wake_lock(dev);
+
+	if (dwrq->length && extra) {
+		if (strnicmp(extra, "START", strlen("START")) == 0) {
+			wl_iw_control_wl_on(dev, info);
+			WL_TRACE(("%s, Received regular START command\n", __FUNCTION__));
+		}
+
+		if (g_onoff == G_WLAN_SET_OFF) {
+			WL_TRACE(("%s, missing START, Fail\n", __FUNCTION__));
+			kfree(extra);
+			net_os_wake_unlock(dev);
+			return -EFAULT;
+		}
+
+		if (strnicmp(extra, "SCAN-ACTIVE", strlen("SCAN-ACTIVE")) == 0) {
+#ifdef ENABLE_ACTIVE_PASSIVE_SCAN_SUPPRESS
+			WL_TRACE(("%s: active scan setting suppressed\n", dev->name));
+#else
+			ret = wl_iw_set_active_scan(dev, info, (union iwreq_data *)dwrq, extra);
+#endif 
+		}
+		else if (strnicmp(extra, "SCAN-PASSIVE", strlen("SCAN-PASSIVE")) == 0)
+#ifdef ENABLE_ACTIVE_PASSIVE_SCAN_SUPPRESS
+			WL_TRACE(("%s: passive scan setting suppressed\n", dev->name));
+#else
+			ret = wl_iw_set_passive_scan(dev, info, (union iwreq_data *)dwrq, extra);
+#endif 
+		else if (strnicmp(extra, "RSSI", strlen("RSSI")) == 0)
+			ret = wl_iw_get_rssi(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, "LINKSPEED", strlen("LINKSPEED")) == 0)
+			ret = wl_iw_get_link_speed(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, "MACADDR", strlen("MACADDR")) == 0)
+			ret = wl_iw_get_macaddr(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, "COUNTRY", strlen("COUNTRY")) == 0)
+			ret = wl_iw_set_country(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, "STOP", strlen("STOP")) == 0)
+			ret = wl_iw_control_wl_off(dev, info);
+		else if (strnicmp(extra, BAND_GET_CMD, strlen(BAND_GET_CMD)) == 0)
+			ret = wl_iw_get_band(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, BAND_SET_CMD, strlen(BAND_SET_CMD)) == 0)
+			ret = wl_iw_set_band(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, DTIM_SKIP_GET_CMD, strlen(DTIM_SKIP_GET_CMD)) == 0)
+			ret = wl_iw_get_dtim_skip(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, DTIM_SKIP_SET_CMD, strlen(DTIM_SKIP_SET_CMD)) == 0)
+			ret = wl_iw_set_dtim_skip(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, SETSUSPENDOPT_CMD, strlen(SETSUSPENDOPT_CMD)) == 0)
+			ret = wl_iw_set_suspend_opt(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, SETSUSPENDMODE_CMD, strlen(SETSUSPENDMODE_CMD)) == 0)
+			ret = wl_iw_set_suspend_mode(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, TXPOWER_SET_CMD, strlen(TXPOWER_SET_CMD)) == 0)
+			ret = wl_iw_set_txpower(dev, info, (union iwreq_data *)dwrq, extra);
+#if defined(PNO_SUPPORT)
+		else if (strnicmp(extra, PNOSSIDCLR_SET_CMD, strlen(PNOSSIDCLR_SET_CMD)) == 0)
+			ret = wl_iw_set_pno_reset(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, PNOSETUP_SET_CMD, strlen(PNOSETUP_SET_CMD)) == 0)
+			ret = wl_iw_set_pno_set(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, PNOSETADD_SET_CMD, strlen(PNOSETADD_SET_CMD)) == 0)
+			ret = wl_iw_set_pno_setadd(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, PNOENABLE_SET_CMD, strlen(PNOENABLE_SET_CMD)) == 0)
+			ret = wl_iw_set_pno_enable(dev, info, (union iwreq_data *)dwrq, extra);
+#endif 
+#if defined(CSCAN)
+	    
+		else if (strnicmp(extra, CSCAN_COMMAND, strlen(CSCAN_COMMAND)) == 0)
+			ret = wl_iw_set_cscan(dev, info, (union iwreq_data *)dwrq, extra);
+#endif 
+#ifdef CONFIG_WPS2
+		else if (strnicmp(extra, WPS_ADD_PROBE_REQ_IE_CMD,
+			strlen(WPS_ADD_PROBE_REQ_IE_CMD)) == 0)
+			ret = wl_iw_add_wps_probe_req_ie(dev, info,
+				(union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, WPS_DEL_PROBE_REQ_IE_CMD,
+			strlen(WPS_DEL_PROBE_REQ_IE_CMD)) == 0)
+			ret = wl_iw_del_wps_probe_req_ie(dev, info,
+				(union iwreq_data *)dwrq, extra);
+#endif 
+		else if (strnicmp(extra, "POWERMODE", strlen("POWERMODE")) == 0)
+			ret = wl_iw_set_power_mode(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, "BTCOEXMODE", strlen("BTCOEXMODE")) == 0)
+			ret = wl_iw_set_btcoex_dhcp(dev, info, (union iwreq_data *)dwrq, extra);
+		else if (strnicmp(extra, "GETPOWER", strlen("GETPOWER")) == 0)
+			ret = wl_iw_get_power_mode(dev, info, (union iwreq_data *)dwrq, extra);
+#ifdef SOFTAP
+		else if (strnicmp(extra, "ASCII_CMD", strlen("ASCII_CMD")) == 0) {
+			wl_iw_process_private_ascii_cmd(dev, info, (union iwreq_data *)dwrq, extra);
+		}
+		else if (strnicmp(extra, "AP_MAC_LIST_SET", strlen("AP_MAC_LIST_SET")) == 0) {
+			WL_SOFTAP(("penguin, set AP_MAC_LIST_SET\n"));
+			set_ap_mac_list(dev, (extra + PROFILE_OFFSET));
+		}
+#endif
+	    else {
+			WL_ERROR(("Unknown PRIVATE command %s - ignored\n", extra));
+			snprintf(extra, MAX_WX_STRING, "OK");
+			dwrq->length = strlen("OK") + 1;
+		}
+	}
+
+	net_os_wake_unlock(dev);
+
+	if (extra) {
+	    if (copy_to_user(dwrq->pointer, extra, dwrq->length)) {
+			kfree(extra);
+			return -EFAULT;
+	    }
+
+	    kfree(extra);
+	}
+
+	return ret;
+}
+
+static const iw_handler wl_iw_handler[] =
+{
+	(iw_handler) wl_iw_config_commit,	
+	(iw_handler) wl_iw_get_name,		
+	(iw_handler) NULL,			
+	(iw_handler) NULL,			
+	(iw_handler) wl_iw_set_freq,		
+	(iw_handler) wl_iw_get_freq,		
+	(iw_handler) wl_iw_set_mode,		
+	(iw_handler) wl_iw_get_mode,		
+	(iw_handler) NULL,			
+	(iw_handler) NULL,			
+	(iw_handler) NULL,			
+	(iw_handler) wl_iw_get_range,		
+	(iw_handler) wl_iw_set_priv,		
+	(iw_handler) NULL,			
+	(iw_handler) NULL,			
+	(iw_handler) NULL,			
+	(iw_handler) wl_iw_set_spy,		
+	(iw_handler) wl_iw_get_spy,		
+	(iw_handler) NULL,			
+	(iw_handler) NULL,			
+	(iw_handler) wl_iw_set_wap,		
+	(iw_handler) wl_iw_get_wap,		
+#if WIRELESS_EXT > 17
+	(iw_handler) wl_iw_mlme,		
+#else
+	(iw_handler) NULL,			
+#endif
+#if defined(WL_IW_USE_ISCAN)
+	(iw_handler) wl_iw_iscan_get_aplist,	
+#else
+	(iw_handler) wl_iw_get_aplist,		
+#endif 
+#if WIRELESS_EXT > 13
+#if defined(WL_IW_USE_ISCAN)
+	(iw_handler) wl_iw_iscan_set_scan,	
+	(iw_handler) wl_iw_iscan_get_scan,	
+#else
+	(iw_handler) wl_iw_set_scan,		
+	(iw_handler) wl_iw_get_scan,		
+#endif
+#else	
+	(iw_handler) NULL,			
+	(iw_handler) NULL,			
+#endif	
+	(iw_handler) wl_iw_set_essid,		
+	(iw_handler) wl_iw_get_essid,		
+	(iw_handler) wl_iw_set_nick,		
+	(iw_handler) wl_iw_get_nick,		
+	(iw_handler) NULL,			
+	(iw_handler) NULL,			
+	(iw_handler) wl_iw_set_rate,		
+	(iw_handler) wl_iw_get_rate,		
+	(iw_handler) wl_iw_set_rts,		
+	(iw_handler) wl_iw_get_rts,		
+	(iw_handler) wl_iw_set_frag,		
+	(iw_handler) wl_iw_get_frag,		
+	(iw_handler) wl_iw_set_txpow,		
+	(iw_handler) wl_iw_get_txpow,		
+#if WIRELESS_EXT > 10
+	(iw_handler) wl_iw_set_retry,		
+	(iw_handler) wl_iw_get_retry,		
+#endif 
+	(iw_handler) wl_iw_set_encode,		
+	(iw_handler) wl_iw_get_encode,		
+	(iw_handler) wl_iw_set_power,		
+	(iw_handler) wl_iw_get_power,		
+#if WIRELESS_EXT > 17
+	(iw_handler) NULL,			
+	(iw_handler) NULL,			
+	(iw_handler) wl_iw_set_wpaie,		
+	(iw_handler) wl_iw_get_wpaie,		
+	(iw_handler) wl_iw_set_wpaauth,		
+	(iw_handler) wl_iw_get_wpaauth,		
+	(iw_handler) wl_iw_set_encodeext,	
+	(iw_handler) wl_iw_get_encodeext,	
+	(iw_handler) wl_iw_set_pmksa,			
+#endif 
+};
+
+#if WIRELESS_EXT > 12
+static const iw_handler wl_iw_priv_handler[] = {
+	NULL,
+	(iw_handler)wl_iw_set_active_scan,
+	NULL,
+	(iw_handler)wl_iw_get_rssi,
+	NULL,
+	(iw_handler)wl_iw_set_passive_scan,
+	NULL,
+	(iw_handler)wl_iw_get_link_speed,
+	NULL,
+	(iw_handler)wl_iw_get_macaddr,
+	NULL,
+	(iw_handler)wl_iw_control_wl_off,
+	NULL,
+	(iw_handler)wl_iw_control_wl_on,
+#ifdef SOFTAP       
+
+	
+	NULL,
+	(iw_handler)iwpriv_set_ap_config,
+
+	
+	
+	NULL,
+	(iw_handler)iwpriv_get_assoc_list,
+
+	
+	NULL,
+	(iw_handler)iwpriv_set_mac_filters,
+
+	
+	NULL,
+	(iw_handler)iwpriv_en_ap_bss,
+
+	
+	NULL,
+	(iw_handler)iwpriv_wpasupp_loop_tst,
+	
+	NULL,
+	(iw_handler)iwpriv_softap_stop,
+	
+	NULL,
+	(iw_handler)iwpriv_fw_reload,
+	NULL, 
+	(iw_handler)iwpriv_set_ap_sta_disassoc,
+#endif 
+#if defined(CSCAN)
+	
+	NULL,
+	(iw_handler)iwpriv_set_cscan
+#endif 	
+};
+
+static const struct iw_priv_args wl_iw_priv_args[] =
+{
+	{	
+		WL_IW_SET_ACTIVE_SCAN,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"SCAN-ACTIVE"
+	},
+	{
+		WL_IW_GET_RSSI,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"RSSI"
+	},
+	{
+		WL_IW_SET_PASSIVE_SCAN,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"SCAN-PASSIVE"
+	},
+	{
+		WL_IW_GET_LINK_SPEED,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"LINKSPEED"
+	},
+	{
+		WL_IW_GET_CURR_MACADDR,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"Macaddr"
+	},
+	{
+		WL_IW_SET_STOP,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"STOP"
+	},
+	{
+		WL_IW_SET_START,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"START"
+	},
+
+#ifdef SOFTAP
+	
+	
+	{
+		WL_SET_AP_CFG,
+		IW_PRIV_TYPE_CHAR |  256,      
+		0,
+		"AP_SET_CFG"
+	},
+
+	{
+		WL_AP_STA_LIST,
+		IW_PRIV_TYPE_CHAR | 0,	
+		IW_PRIV_TYPE_CHAR | 1024,  
+		"AP_GET_STA_LIST"
+	},
+
+	{
+		WL_AP_MAC_FLTR,
+		IW_PRIV_TYPE_CHAR | 256,                      
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 0,    
+		"AP_SET_MAC_FLTR"
+	},
+
+	{ 
+		WL_AP_BSS_START,
+		0,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | MAX_WX_STRING,
+		"AP_BSS_START"
+	},
+
+	{
+		AP_LPB_CMD,
+		IW_PRIV_TYPE_CHAR | 256,   
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 0,    
+		"AP_LPB_CMD"
+	},
+
+	{ 
+		WL_AP_STOP,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 0,   
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 0,   
+		"AP_BSS_STOP"
+	},
+	{ 
+		WL_FW_RELOAD,
+		IW_PRIV_TYPE_CHAR | 256,
+		IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 0,
+		"WL_FW_RELOAD"
+	},
+#endif 
+#if defined(CSCAN)
+	{ 
+		WL_COMBO_SCAN,
+		IW_PRIV_TYPE_CHAR | 1024,  
+		0,
+		"CSCAN"
+	},
+#endif 
+	};
+
+const struct iw_handler_def wl_iw_handler_def =
+{
+	.num_standard = ARRAYSIZE(wl_iw_handler),
+	.standard = (iw_handler *) wl_iw_handler,
+	.num_private = ARRAYSIZE(wl_iw_priv_handler),
+	.num_private_args = ARRAY_SIZE(wl_iw_priv_args),
+	.private = (iw_handler *)wl_iw_priv_handler,
+	.private_args = (void *) wl_iw_priv_args,
+
+#if WIRELESS_EXT >= 19
+	get_wireless_stats: dhd_get_wireless_stats,
+#endif 
+	};
+#endif 
+
+
+
+int
+wl_iw_ioctl(
+	struct net_device *dev,
+	struct ifreq *rq,
+	int cmd
+)
+{
+	struct iwreq *wrq = (struct iwreq *) rq;
+	struct iw_request_info info;
+	iw_handler handler;
+	char *extra = NULL;
+	size_t token_size = 1;
+	int max_tokens = 0, ret = 0;
+
+	net_os_wake_lock(dev);
+
+	WL_TRACE(("\n%s, cmd:%x called via dhd->do_ioctl()entry point\n", __FUNCTION__, cmd));
+	if (cmd < SIOCIWFIRST ||
+		IW_IOCTL_IDX(cmd) >= ARRAYSIZE(wl_iw_handler) ||
+		!(handler = wl_iw_handler[IW_IOCTL_IDX(cmd)])) {
+			WL_ERROR(("%s: error in cmd=%x : not supported\n", __FUNCTION__, cmd));
+			net_os_wake_unlock(dev);
+			return -EOPNOTSUPP;
+	}
+
+	switch (cmd) {
+
+	case SIOCSIWESSID:
+	case SIOCGIWESSID:
+	case SIOCSIWNICKN:
+	case SIOCGIWNICKN:
+		max_tokens = IW_ESSID_MAX_SIZE + 1;
+		break;
+
+	case SIOCSIWENCODE:
+	case SIOCGIWENCODE:
+#if WIRELESS_EXT > 17
+	case SIOCSIWENCODEEXT:
+	case SIOCGIWENCODEEXT:
+#endif
+		max_tokens = wrq->u.data.length;
+		break;
+
+	case SIOCGIWRANGE:
+		
+		max_tokens = sizeof(struct iw_range) + 500;
+		break;
+
+	case SIOCGIWAPLIST:
+		token_size = sizeof(struct sockaddr) + sizeof(struct iw_quality);
+		max_tokens = IW_MAX_AP;
+		break;
+
+#if WIRELESS_EXT > 13
+	case SIOCGIWSCAN:
+#if defined(WL_IW_USE_ISCAN)
+	if (g_iscan)
+		max_tokens = wrq->u.data.length;
+	else
+#endif
+		max_tokens = IW_SCAN_MAX_DATA;
+		break;
+#endif 
+
+	case SIOCSIWSPY:
+		token_size = sizeof(struct sockaddr);
+		max_tokens = IW_MAX_SPY;
+		break;
+
+	case SIOCGIWSPY:
+		token_size = sizeof(struct sockaddr) + sizeof(struct iw_quality);
+		max_tokens = IW_MAX_SPY;
+		break;
+
+#if WIRELESS_EXT > 17
+	case SIOCSIWPMKSA:
+	case SIOCSIWGENIE:
+#endif 
+	case SIOCSIWPRIV:
+		max_tokens = wrq->u.data.length;
+		break;
+	}
+
+	if (max_tokens && wrq->u.data.pointer) {
+		if (wrq->u.data.length > max_tokens) {
+			WL_ERROR(("%s: error in cmd=%x wrq->u.data.length=%d  > max_tokens=%d\n",
+				__FUNCTION__, cmd, wrq->u.data.length, max_tokens));
+			ret = -E2BIG;
+			goto wl_iw_ioctl_done;
+		}
+		if (!(extra = kmalloc(max_tokens * token_size, GFP_KERNEL))) {
+			ret = -ENOMEM;
+			goto wl_iw_ioctl_done;
+		}
+
+		if (copy_from_user(extra, wrq->u.data.pointer, wrq->u.data.length * token_size)) {
+			kfree(extra);
+			ret = -EFAULT;
+			goto wl_iw_ioctl_done;
+		}
+	}
+
+	info.cmd = cmd;
+	info.flags = 0;
+
+	ret = handler(dev, &info, &wrq->u, extra);
+
+	if (extra) {
+		if (copy_to_user(wrq->u.data.pointer, extra, wrq->u.data.length * token_size)) {
+			kfree(extra);
+			ret = -EFAULT;
+			goto wl_iw_ioctl_done;
+		}
+
+		kfree(extra);
+	}
+
+wl_iw_ioctl_done:
+
+	net_os_wake_unlock(dev);
+
+	return ret;
+}
+
+
+static bool
+wl_iw_conn_status_str(uint32 event_type, uint32 status, uint32 reason,
+	char* stringBuf, uint buflen)
+{
+	typedef struct conn_fail_event_map_t {
+		uint32 inEvent;			
+		uint32 inStatus;		
+		uint32 inReason;		
+		const char* outName;	
+		const char* outCause;	
+	} conn_fail_event_map_t;
+
+	
+#define WL_IW_DONT_CARE	9999
+	const conn_fail_event_map_t event_map [] = {
+		
+		
+		{WLC_E_SET_SSID,     WLC_E_STATUS_SUCCESS,   WL_IW_DONT_CARE,
+		"Conn", "Success"},
+		{WLC_E_SET_SSID,     WLC_E_STATUS_NO_NETWORKS, WL_IW_DONT_CARE,
+		"Conn", "NoNetworks"},
+		{WLC_E_SET_SSID,     WLC_E_STATUS_FAIL,      WL_IW_DONT_CARE,
+		"Conn", "ConfigMismatch"},
+		{WLC_E_PRUNE,        WL_IW_DONT_CARE,        WLC_E_PRUNE_ENCR_MISMATCH,
+		"Conn", "EncrypMismatch"},
+		{WLC_E_PRUNE,        WL_IW_DONT_CARE,        WLC_E_RSN_MISMATCH,
+		"Conn", "RsnMismatch"},
+		{WLC_E_AUTH,         WLC_E_STATUS_TIMEOUT,   WL_IW_DONT_CARE,
+		"Conn", "AuthTimeout"},
+		{WLC_E_AUTH,         WLC_E_STATUS_FAIL,      WL_IW_DONT_CARE,
+		"Conn", "AuthFail"},
+		{WLC_E_AUTH,         WLC_E_STATUS_NO_ACK,    WL_IW_DONT_CARE,
+		"Conn", "AuthNoAck"},
+		{WLC_E_REASSOC,      WLC_E_STATUS_FAIL,      WL_IW_DONT_CARE,
+		"Conn", "ReassocFail"},
+		{WLC_E_REASSOC,      WLC_E_STATUS_TIMEOUT,   WL_IW_DONT_CARE,
+		"Conn", "ReassocTimeout"},
+		{WLC_E_REASSOC,      WLC_E_STATUS_ABORT,     WL_IW_DONT_CARE,
+		"Conn", "ReassocAbort"},
+		{WLC_E_PSK_SUP,      WLC_SUP_KEYED,          WL_IW_DONT_CARE,
+		"Sup", "ConnSuccess"},
+		{WLC_E_PSK_SUP,      WL_IW_DONT_CARE,        WL_IW_DONT_CARE,
+		"Sup", "WpaHandshakeFail"},
+		{WLC_E_DEAUTH_IND,   WL_IW_DONT_CARE,        WL_IW_DONT_CARE,
+		"Conn", "Deauth"},
+		{WLC_E_DISASSOC_IND, WL_IW_DONT_CARE,        WL_IW_DONT_CARE,
+		"Conn", "DisassocInd"},
+		{WLC_E_DISASSOC,     WL_IW_DONT_CARE,        WL_IW_DONT_CARE,
+		"Conn", "Disassoc"}
+	};
+
+	const char* name = "";
+	const char* cause = NULL;
+	int i;
+
+	
+	for (i = 0;  i < sizeof(event_map)/sizeof(event_map[0]);  i++) {
+		const conn_fail_event_map_t* row = &event_map[i];
+		if (row->inEvent == event_type &&
+		    (row->inStatus == status || row->inStatus == WL_IW_DONT_CARE) &&
+		    (row->inReason == reason || row->inReason == WL_IW_DONT_CARE)) {
+			name = row->outName;
+			cause = row->outCause;
+			break;
+		}
+	}
+
+	
+	if (cause) {
+		memset(stringBuf, 0, buflen);
+		snprintf(stringBuf, buflen, "%s %s %02d %02d",
+			name, cause, status, reason);
+		WL_INFORM(("Connection status: %s\n", stringBuf));
+		return TRUE;
+	} else {
+		return FALSE;
+	}
+}
+
+#if WIRELESS_EXT > 14
+
+static bool
+wl_iw_check_conn_fail(wl_event_msg_t *e, char* stringBuf, uint buflen)
+{
+	uint32 event = ntoh32(e->event_type);
+	uint32 status =  ntoh32(e->status);
+	uint32 reason =  ntoh32(e->reason);
+
+	if (wl_iw_conn_status_str(event, status, reason, stringBuf, buflen)) {
+		return TRUE;
+	}
+	else
+		return FALSE;
+}
+#endif 
+
+#ifndef IW_CUSTOM_MAX
+#define IW_CUSTOM_MAX 256 
+#endif 
+
+void
+wl_iw_event(struct net_device *dev, wl_event_msg_t *e, void* data)
+{
+#if WIRELESS_EXT > 13
+	union iwreq_data wrqu;
+	char extra[IW_CUSTOM_MAX + 1];
+	int cmd = 0;
+	uint32 event_type = ntoh32(e->event_type);
+	uint16 flags =  ntoh16(e->flags);
+	uint32 datalen = ntoh32(e->datalen);
+	uint32 status =  ntoh32(e->status);
+	uint32 toto;
+	memset(&wrqu, 0, sizeof(wrqu));
+	memset(extra, 0, sizeof(extra));
+
+	if (!dev) {
+		WL_ERROR(("%s: dev is null\n", __FUNCTION__));
+		return;
+	}
+
+	net_os_wake_lock(dev);
+
+	WL_TRACE(("%s: dev=%s event=%d \n", __FUNCTION__, dev->name, event_type));
+
+	
+	switch (event_type) {
+#if defined(SOFTAP)
+	case WLC_E_PRUNE:
+		if (ap_cfg_running) {
+			char *macaddr = (char *)&e->addr;
+			WL_SOFTAP(("PRUNE received, %02X:%02X:%02X:%02X:%02X:%02X!\n",
+				macaddr[0], macaddr[1], macaddr[2], macaddr[3],
+				macaddr[4], macaddr[5]));
+
+			
+			if (ap_macmode)
+			{
+				int i;
+				for (i = 0; i < ap_black_list.count; i++) {
+					if (!bcmp(macaddr, &ap_black_list.ea[i],
+						sizeof(struct ether_addr))) {
+						WL_SOFTAP(("mac in black list, ignore it\n"));
+						break;
+					}
+				}
+
+				if (i == ap_black_list.count) {
+					
+					char mac_buf[32] = {0};
+					sprintf(mac_buf, "STA_BLOCK %02X:%02X:%02X:%02X:%02X:%02X",
+						macaddr[0], macaddr[1], macaddr[2],
+						macaddr[3], macaddr[4], macaddr[5]);
+					wl_iw_send_priv_event(priv_dev, mac_buf);
+				}
+			}
+		}
+		break;
+#endif 
+	case WLC_E_TXFAIL:
+		cmd = IWEVTXDROP;
+		memcpy(wrqu.addr.sa_data, &e->addr, ETHER_ADDR_LEN);
+		wrqu.addr.sa_family = ARPHRD_ETHER;
+		break;
+#if WIRELESS_EXT > 14
+	case WLC_E_JOIN:
+	case WLC_E_ASSOC_IND:
+	case WLC_E_REASSOC_IND:
+#if defined(SOFTAP)
+		WL_SOFTAP(("STA connect received %d\n", event_type));
+		if (ap_cfg_running) {
+			wl_iw_send_priv_event(priv_dev, "STA_JOIN");
+			goto wl_iw_event_end;
+		}
+#endif 
+		memcpy(wrqu.addr.sa_data, &e->addr, ETHER_ADDR_LEN);
+		wrqu.addr.sa_family = ARPHRD_ETHER;
+		cmd = IWEVREGISTERED;
+		break;
+	case WLC_E_ROAM:
+		if (status == WLC_E_STATUS_SUCCESS) {
+			WL_ASSOC((" WLC_E_ROAM : success \n"));
+			goto wl_iw_event_end;
+		}
+	break;
+
+	case WLC_E_DEAUTH_IND:
+	case WLC_E_DISASSOC_IND:
+#if defined(SOFTAP)
+		WL_SOFTAP(("STA disconnect received %d\n", event_type));
+		if (ap_cfg_running) {
+			wl_iw_send_priv_event(priv_dev, "STA_LEAVE");
+			goto wl_iw_event_end;
+		}
+#endif 
+		cmd = SIOCGIWAP;
+		bzero(wrqu.addr.sa_data, ETHER_ADDR_LEN);
+		wrqu.addr.sa_family = ARPHRD_ETHER;
+		bzero(&extra, ETHER_ADDR_LEN);
+		break;
+	case WLC_E_LINK:
+	case WLC_E_NDIS_LINK:
+		cmd = SIOCGIWAP;
+		if (!(flags & WLC_EVENT_MSG_LINK)) {
+			
+			
+#ifdef SOFTAP
+#ifdef AP_ONLY
+		if (ap_cfg_running) {
+#else
+		if (ap_cfg_running && !strncmp(dev->name, "wl0.1", 5)) {
+#endif	
+			
+			WL_SOFTAP(("AP DOWN %d\n", event_type));
+			wl_iw_send_priv_event(priv_dev, "AP_DOWN");
+		} else {
+			WL_TRACE(("STA_Link Down\n"));
+			g_ss_cache_ctrl.m_link_down = 1;
+		}
+#else		
+		g_ss_cache_ctrl.m_link_down = 1;
+#endif 
+			WL_TRACE(("Link Down\n"));
+
+			bzero(wrqu.addr.sa_data, ETHER_ADDR_LEN);
+			bzero(&extra, ETHER_ADDR_LEN);
+		}
+		else {
+			
+			memcpy(wrqu.addr.sa_data, &e->addr, ETHER_ADDR_LEN);
+			g_ss_cache_ctrl.m_link_down = 0;
+			
+			memcpy(g_ss_cache_ctrl.m_active_bssid, &e->addr, ETHER_ADDR_LEN);
+#ifdef SOFTAP
+
+#ifdef AP_ONLY
+			if (ap_cfg_running) {
+#else
+			if (ap_cfg_running && !strncmp(dev->name, "wl0.1", 5)) {
+#endif
+			
+				WL_SOFTAP(("AP UP %d\n", event_type));
+				wl_iw_send_priv_event(priv_dev, "AP_UP");
+			} else {
+				WL_TRACE(("STA_LINK_UP\n"));
+			}
+#else
+#endif 
+			WL_TRACE(("Link UP\n"));
+
+		}
+		wrqu.addr.sa_family = ARPHRD_ETHER;
+		break;
+	case WLC_E_ACTION_FRAME:
+		cmd = IWEVCUSTOM;
+		if (datalen + 1 <= sizeof(extra)) {
+			wrqu.data.length = datalen + 1;
+			extra[0] = WLC_E_ACTION_FRAME;
+			memcpy(&extra[1], data, datalen);
+			WL_TRACE(("WLC_E_ACTION_FRAME len %d \n", wrqu.data.length));
+		}
+		break;
+
+	case WLC_E_ACTION_FRAME_COMPLETE:
+		cmd = IWEVCUSTOM;
+		memcpy(&toto, data, 4);
+		if (sizeof(status) + 1 <= sizeof(extra)) {
+			wrqu.data.length = sizeof(status) + 1;
+			extra[0] = WLC_E_ACTION_FRAME_COMPLETE;
+			memcpy(&extra[1], &status, sizeof(status));
+			printf("wl_iw_event status %d PacketId %d \n", status, toto);
+			printf("WLC_E_ACTION_FRAME_COMPLETE len %d \n", wrqu.data.length);
+		}
+		break;
+#endif 
+#if WIRELESS_EXT > 17
+	case WLC_E_MIC_ERROR: {
+		struct	iw_michaelmicfailure  *micerrevt = (struct  iw_michaelmicfailure  *)&extra;
+		cmd = IWEVMICHAELMICFAILURE;
+		wrqu.data.length = sizeof(struct iw_michaelmicfailure);
+		if (flags & WLC_EVENT_MSG_GROUP)
+			micerrevt->flags |= IW_MICFAILURE_GROUP;
+		else
+			micerrevt->flags |= IW_MICFAILURE_PAIRWISE;
+		memcpy(micerrevt->src_addr.sa_data, &e->addr, ETHER_ADDR_LEN);
+		micerrevt->src_addr.sa_family = ARPHRD_ETHER;
+
+		break;
+	}
+
+	case WLC_E_ASSOC_REQ_IE:
+		cmd = IWEVASSOCREQIE;
+		wrqu.data.length = datalen;
+		if (datalen < sizeof(extra))
+			memcpy(extra, data, datalen);
+		break;
+
+	case WLC_E_ASSOC_RESP_IE:
+		cmd = IWEVASSOCRESPIE;
+		wrqu.data.length = datalen;
+		if (datalen < sizeof(extra))
+			memcpy(extra, data, datalen);
+		break;
+
+	case WLC_E_PMKID_CACHE: {
+		if (data)
+		{
+			struct iw_pmkid_cand *iwpmkidcand = (struct iw_pmkid_cand *)&extra;
+			pmkid_cand_list_t *pmkcandlist;
+			pmkid_cand_t	*pmkidcand;
+			int count;
+
+			cmd = IWEVPMKIDCAND;
+			pmkcandlist = data;
+			count = ntoh32_ua((uint8 *)&pmkcandlist->npmkid_cand);
+			ASSERT(count >= 0);
+			wrqu.data.length = sizeof(struct iw_pmkid_cand);
+			pmkidcand = pmkcandlist->pmkid_cand;
+			while (count) {
+				bzero(iwpmkidcand, sizeof(struct iw_pmkid_cand));
+				if (pmkidcand->preauth)
+					iwpmkidcand->flags |= IW_PMKID_CAND_PREAUTH;
+				bcopy(&pmkidcand->BSSID, &iwpmkidcand->bssid.sa_data,
+					ETHER_ADDR_LEN);
+				wireless_send_event(dev, cmd, &wrqu, extra);
+				pmkidcand++;
+				count--;
+			}
+		}
+		goto wl_iw_event_end;
+	}
+#endif 
+
+	case WLC_E_SCAN_COMPLETE:
+#if defined(WL_IW_USE_ISCAN)
+		if (!g_iscan) {
+			WL_ERROR(("Event WLC_E_SCAN_COMPLETE on g_iscan NULL!"));
+			goto wl_iw_event_end;
+		}
+
+		if ((g_iscan) && (g_iscan->tsk_ctl.thr_pid >= 0) &&
+			(g_iscan->iscan_state != ISCAN_STATE_IDLE))
+		{
+			up(&g_iscan->tsk_ctl.sema);
+		} else {
+			cmd = SIOCGIWSCAN;
+			wrqu.data.length = strlen(extra);
+			WL_TRACE(("Event WLC_E_SCAN_COMPLETE from specific scan %d\n",
+				g_iscan->iscan_state));
+		}
+#else
+		cmd = SIOCGIWSCAN;
+		wrqu.data.length = strlen(extra);
+		WL_TRACE(("Event WLC_E_SCAN_COMPLETE\n"));
+#endif 
+	break;
+
+	
+	case WLC_E_PFN_NET_FOUND:
+	{
+		wl_pfn_net_info_t *netinfo;
+		netinfo = (wl_pfn_net_info_t *)(data + sizeof(wl_pfn_scanresults_t) -
+		            sizeof(wl_pfn_net_info_t));
+		WL_ERROR(("%s Event WLC_E_PFN_NET_FOUND, send %s up : find %s len=%d\n",
+		   __FUNCTION__, PNO_EVENT_UP, netinfo->pfnsubnet.SSID,
+		   netinfo->pfnsubnet.SSID_len));
+		cmd = IWEVCUSTOM;
+		memset(&wrqu, 0, sizeof(wrqu));
+		strcpy(extra, PNO_EVENT_UP);
+		wrqu.data.length = strlen(extra);
+	}
+	break;
+
+	default:
+		
+		WL_TRACE(("Unknown Event %d: ignoring\n", event_type));
+		break;
+	}
+		if (cmd) {
+			if (cmd == SIOCGIWSCAN)
+				wireless_send_event(dev, cmd, &wrqu, NULL);
+			else
+				wireless_send_event(dev, cmd, &wrqu, extra);
+		}
+
+#if WIRELESS_EXT > 14
+	
+	memset(extra, 0, sizeof(extra));
+	if (wl_iw_check_conn_fail(e, extra, sizeof(extra))) {
+		cmd = IWEVCUSTOM;
+		wrqu.data.length = strlen(extra);
+		wireless_send_event(dev, cmd, &wrqu, extra);
+	}
+#endif 
+
+	goto wl_iw_event_end;	
+wl_iw_event_end:
+
+	net_os_wake_unlock(dev);
+#endif 
+}
+
+int
+wl_iw_get_wireless_stats(struct net_device *dev, struct iw_statistics *wstats)
+{
+	int res = 0;
+	wl_cnt_t cnt;
+	int phy_noise;
+	int rssi;
+	scb_val_t scb_val;
+
+	phy_noise = 0;
+	if ((res = dev_wlc_ioctl(dev, WLC_GET_PHY_NOISE, &phy_noise, sizeof(phy_noise))))
+		goto done;
+
+	phy_noise = dtoh32(phy_noise);
+	WL_TRACE(("wl_iw_get_wireless_stats phy noise=%d\n", phy_noise));
+
+	bzero(&scb_val, sizeof(scb_val_t));
+	if ((res = dev_wlc_ioctl(dev, WLC_GET_RSSI, &scb_val, sizeof(scb_val_t))))
+		goto done;
+
+	rssi = dtoh32(scb_val.val);
+	WL_TRACE(("wl_iw_get_wireless_stats rssi=%d\n", rssi));
+	if (rssi <= WL_IW_RSSI_NO_SIGNAL)
+		wstats->qual.qual = 0;
+	else if (rssi <= WL_IW_RSSI_VERY_LOW)
+		wstats->qual.qual = 1;
+	else if (rssi <= WL_IW_RSSI_LOW)
+		wstats->qual.qual = 2;
+	else if (rssi <= WL_IW_RSSI_GOOD)
+		wstats->qual.qual = 3;
+	else if (rssi <= WL_IW_RSSI_VERY_GOOD)
+		wstats->qual.qual = 4;
+	else
+		wstats->qual.qual = 5;
+
+	
+	wstats->qual.level = 0x100 + rssi;
+	wstats->qual.noise = 0x100 + phy_noise;
+#if WIRELESS_EXT > 18
+	wstats->qual.updated |= (IW_QUAL_ALL_UPDATED | IW_QUAL_DBM);
+#else
+	wstats->qual.updated |= 7;
+#endif 
+
+#if WIRELESS_EXT > 11
+	WL_TRACE(("wl_iw_get_wireless_stats counters=%d\n", (int)sizeof(wl_cnt_t)));
+
+	memset(&cnt, 0, sizeof(wl_cnt_t));
+	res = dev_wlc_bufvar_get(dev, "counters", (char *)&cnt, sizeof(wl_cnt_t));
+	if (res)
+	{
+		WL_ERROR(("wl_iw_get_wireless_stats counters failed error=%d\n", res));
+		goto done;
+	}
+
+	cnt.version = dtoh16(cnt.version);
+	if (cnt.version != WL_CNT_T_VERSION) {
+		WL_TRACE(("\tIncorrect version of counters struct: expected %d; got %d\n",
+			WL_CNT_T_VERSION, cnt.version));
+		goto done;
+	}
+
+	wstats->discard.nwid = 0;
+	wstats->discard.code = dtoh32(cnt.rxundec);
+	wstats->discard.fragment = dtoh32(cnt.rxfragerr);
+	wstats->discard.retries = dtoh32(cnt.txfail);
+	wstats->discard.misc = dtoh32(cnt.rxrunt) + dtoh32(cnt.rxgiant);
+	wstats->miss.beacon = 0;
+
+	WL_TRACE(("wl_iw_get_wireless_stats counters txframe=%d txbyte=%d\n",
+		dtoh32(cnt.txframe), dtoh32(cnt.txbyte)));
+	WL_TRACE(("wl_iw_get_wireless_stats counters rxfrmtoolong=%d\n", dtoh32(cnt.rxfrmtoolong)));
+	WL_TRACE(("wl_iw_get_wireless_stats counters rxbadplcp=%d\n", dtoh32(cnt.rxbadplcp)));
+	WL_TRACE(("wl_iw_get_wireless_stats counters rxundec=%d\n", dtoh32(cnt.rxundec)));
+	WL_TRACE(("wl_iw_get_wireless_stats counters rxfragerr=%d\n", dtoh32(cnt.rxfragerr)));
+	WL_TRACE(("wl_iw_get_wireless_stats counters txfail=%d\n", dtoh32(cnt.txfail)));
+	WL_TRACE(("wl_iw_get_wireless_stats counters rxrunt=%d\n", dtoh32(cnt.rxrunt)));
+	WL_TRACE(("wl_iw_get_wireless_stats counters rxgiant=%d\n", dtoh32(cnt.rxgiant)));
+
+#endif 
+
+done:
+	return res;
+}
+#if defined(COEX_DHCP)
+static void
+wl_iw_bt_flag_set(
+	struct net_device *dev,
+	bool set)
+{
+#if defined(BT_DHCP_USE_FLAGS)
+	char buf_flag7_dhcp_on[8] = { 7, 00, 00, 00, 0x1, 0x0, 0x00, 0x00 };
+	char buf_flag7_default[8]   = { 7, 00, 00, 00, 0x0, 0x00, 0x00, 0x00};
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	rtnl_lock();
+#endif
+
+
+#if defined(BT_DHCP_eSCO_FIX)
+	
+	set_btc_esco_params(dev, set);
+#endif
+
+
+#if defined(BT_DHCP_USE_FLAGS)
+	WL_TRACE_COEX(("WI-FI priority boost via bt flags, set:%d\n", set));
+	if (set == TRUE) {
+		
+		dev_wlc_bufvar_set(dev, "btc_flags",
+			(char *)&buf_flag7_dhcp_on[0], sizeof(buf_flag7_dhcp_on));
+	}
+	else  {
+		
+		dev_wlc_bufvar_set(dev, "btc_flags",
+			(char *)&buf_flag7_default[0], sizeof(buf_flag7_default));
+	}
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27))
+	rtnl_unlock();
+#endif
+}
+
+static void
+wl_iw_bt_timerfunc(ulong data)
+{
+	bt_info_t  *bt_local = (bt_info_t *)data;
+	bt_local->timer_on = 0;
+	WL_TRACE(("%s\n", __FUNCTION__));
+	
+	up(&bt_local->tsk_ctl.sema);
+}
+
+static int
+_bt_dhcp_sysioc_thread(void *data)
+{
+	tsk_ctl_t *tsk_ctl =  (tsk_ctl_t *)data;
+
+	DAEMONIZE("dhcp_sysioc");
+
+	complete(&tsk_ctl->completed);
+
+	while (down_interruptible(&tsk_ctl->sema) == 0) {
+
+		SMP_RD_BARRIER_DEPENDS();
+		if (tsk_ctl->terminated) {
+			break;
+		}
+
+		if (g_bt->timer_on) {
+			g_bt->timer_on = 0;
+			del_timer_sync(&g_bt->timer);
+		}
+
+		switch (g_bt->bt_state) {
+			case BT_DHCP_START:
+				
+				WL_TRACE_COEX(("%s bt_dhcp stm: started \n", __FUNCTION__));
+				g_bt->bt_state = BT_DHCP_OPPORTUNITY_WINDOW;
+				mod_timer(&g_bt->timer,
+				          jiffies + BT_DHCP_OPPORTUNITY_WINDOW_TIME*HZ/1000);
+				g_bt->timer_on = 1;
+				break;
+
+			case BT_DHCP_OPPORTUNITY_WINDOW:
+				if 	(g_bt->dhcp_done) {
+					WL_TRACE_COEX(("%s DHCP Done before T1 expiration\n",
+						__FUNCTION__));
+					goto btc_coex_idle;
+				}
+
+				
+				WL_TRACE_COEX(("%s DHCP T1:%d expired\n",
+					__FUNCTION__, BT_DHCP_OPPORTUNITY_WINDOW_TIME));
+				
+				if (g_bt->dev) wl_iw_bt_flag_set(g_bt->dev, TRUE);
+				g_bt->bt_state = BT_DHCP_FLAG_FORCE_TIMEOUT;
+				mod_timer(&g_bt->timer, jiffies + BT_DHCP_FLAG_FORCE_TIME*HZ/1000);
+				g_bt->timer_on = 1;
+				break;
+
+			case BT_DHCP_FLAG_FORCE_TIMEOUT:
+				if 	(g_bt->dhcp_done) {
+					WL_TRACE_COEX(("%s DHCP Done before T2 expiration\n",
+						__FUNCTION__));
+				} else  {
+					
+					WL_TRACE_COEX(("%s DHCP wait interval T2:%d msec expired\n",
+						__FUNCTION__, BT_DHCP_FLAG_FORCE_TIME));
+				}
+
+				
+				if (g_bt->dev)  wl_iw_bt_flag_set(g_bt->dev, FALSE);
+			btc_coex_idle:
+				g_bt->bt_state = BT_DHCP_IDLE;
+				g_bt->timer_on = 0;
+				break;
+
+			default:
+				WL_ERROR(("%s error g_status=%d !!!\n", __FUNCTION__,
+				          g_bt->bt_state));
+				if (g_bt->dev) wl_iw_bt_flag_set(g_bt->dev, FALSE);
+				g_bt->bt_state = BT_DHCP_IDLE;
+				g_bt->timer_on = 0;
+				break;
+		 }
+
+		net_os_wake_unlock(g_bt->dev);
+	}
+
+	if (g_bt->timer_on) {
+		g_bt->timer_on = 0;
+		del_timer_sync(&g_bt->timer);
+	}
+	complete_and_exit(&tsk_ctl->completed, 0);
+}
+
+static void
+wl_iw_bt_release(void)
+{
+	bt_info_t *bt_local = g_bt;
+
+	if (!bt_local) {
+		return;
+	}
+
+	if (bt_local->tsk_ctl.thr_pid >= 0) {
+		PROC_STOP(&bt_local->tsk_ctl);
+	}
+	kfree(bt_local);
+	g_bt = NULL;
+}
+
+static int
+wl_iw_bt_init(struct net_device *dev)
+{
+	bt_info_t *bt_dhcp = NULL;
+
+	bt_dhcp = kmalloc(sizeof(bt_info_t), GFP_KERNEL);
+	if (!bt_dhcp)
+		return -ENOMEM;
+
+	memset(bt_dhcp, 0, sizeof(bt_info_t));
+
+	g_bt = bt_dhcp;
+	bt_dhcp->dev = dev;
+	bt_dhcp->bt_state = BT_DHCP_IDLE;
+
+	
+	bt_dhcp->timer_ms    = 10;
+	init_timer(&bt_dhcp->timer);
+	bt_dhcp->timer.data = (ulong)bt_dhcp;
+	bt_dhcp->timer.function = wl_iw_bt_timerfunc;
+	bt_dhcp->ts_dhcp_start = 0;
+	bt_dhcp->ts_dhcp_ok = 0;
+
+	PROC_START(_bt_dhcp_sysioc_thread, bt_dhcp, &bt_dhcp->tsk_ctl, 0);
+	if (bt_dhcp->tsk_ctl.thr_pid < 0) {
+		WL_ERROR(("Failed in %s\n", __FUNCTION__));
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+#endif 
+
+int
+wl_iw_attach(struct net_device *dev, void * dhdp)
+{
+#if defined(WL_IW_USE_ISCAN)
+	int params_size = 0;
+#endif 
+	wl_iw_t *iw;
+#if defined(WL_IW_USE_ISCAN)
+	iscan_info_t *iscan = NULL;
+#endif
+
+	DHD_OS_MUTEX_INIT(&wl_cache_lock);
+	DHD_OS_MUTEX_INIT(&wl_softap_lock);
+
+#if defined(WL_IW_USE_ISCAN)
+	if (!dev)
+		return 0;
+
+	
+	memset(&g_wl_iw_params, 0, sizeof(wl_iw_extra_params_t));
+
+	
+#ifdef CSCAN
+	params_size = (WL_SCAN_PARAMS_FIXED_SIZE + OFFSETOF(wl_iscan_params_t, params)) +
+	    (WL_NUMCHANNELS * sizeof(uint16)) + WL_SCAN_PARAMS_SSID_MAX * sizeof(wlc_ssid_t);
+#else
+	params_size = (WL_SCAN_PARAMS_FIXED_SIZE + OFFSETOF(wl_iscan_params_t, params));
+#endif 
+	iscan = kmalloc(sizeof(iscan_info_t), GFP_KERNEL);
+	if (!iscan)
+		return -ENOMEM;
+	memset(iscan, 0, sizeof(iscan_info_t));
+
+	
+	iscan->iscan_ex_params_p = (wl_iscan_params_t*)kmalloc(params_size, GFP_KERNEL);
+	if (!iscan->iscan_ex_params_p) {
+		kfree(iscan);
+		return -ENOMEM;
+	}
+	iscan->iscan_ex_param_size = params_size;
+
+	
+	g_iscan = iscan;
+	iscan->dev = dev;
+	iscan->iscan_state = ISCAN_STATE_IDLE;
+
+#if defined(CONFIG_FIRST_SCAN)
+	g_first_broadcast_scan = BROADCAST_SCAN_FIRST_IDLE;
+	g_first_counter_scans = 0;
+	g_iscan->scan_flag = 0;
+#endif 
+
+#ifdef CONFIG_WPS2
+	g_wps_probe_req_ie = NULL;
+	g_wps_probe_req_ie_len = 0;
+#endif
+	
+	iscan->timer_ms    = 8000;
+	init_timer(&iscan->timer);
+	iscan->timer.data = (ulong)iscan;
+	iscan->timer.function = wl_iw_timerfunc;
+
+	PROC_START(_iscan_sysioc_thread, iscan, &iscan->tsk_ctl, 0);
+	if (iscan->tsk_ctl.thr_pid < 0)
+		return -ENOMEM;
+#endif 
+
+	iw = *(wl_iw_t **)netdev_priv(dev);
+	iw->pub = (dhd_pub_t *)dhdp;
+#ifdef SOFTAP
+	priv_dev = dev;
+#endif 
+	g_scan = NULL;
+
+	
+	g_scan = (void *)kmalloc(G_SCAN_RESULTS, GFP_KERNEL);
+	if (!g_scan)
+		return -ENOMEM;
+
+	memset(g_scan, 0, G_SCAN_RESULTS);
+	g_scan_specified_ssid = 0;
+
+#if !defined(CSCAN)
+	
+	wl_iw_init_ss_cache_ctrl();
+#endif 
+#ifdef COEX_DHCP
+	
+	wl_iw_bt_init(dev);
+#endif 
+
+
+	return 0;
+}
+
+void
+wl_iw_detach(void)
+{
+#if defined(WL_IW_USE_ISCAN)
+	iscan_buf_t  *buf;
+	iscan_info_t *iscan = g_iscan;
+
+	if (!iscan)
+		return;
+	if (iscan->tsk_ctl.thr_pid >= 0) {
+		PROC_STOP(&iscan->tsk_ctl);
+	}
+	DHD_OS_MUTEX_LOCK(&wl_cache_lock);
+	while (iscan->list_hdr) {
+		buf = iscan->list_hdr->next;
+		kfree(iscan->list_hdr);
+		iscan->list_hdr = buf;
+	}
+	kfree(iscan->iscan_ex_params_p);
+	kfree(iscan);
+	g_iscan = NULL;
+	DHD_OS_MUTEX_UNLOCK(&wl_cache_lock);
+#endif 
+
+	if (g_scan)
+		kfree(g_scan);
+
+	g_scan = NULL;
+#ifdef CONFIG_WPS2
+
+	if (g_wps_probe_req_ie) {
+		kfree(g_wps_probe_req_ie);
+		g_wps_probe_req_ie = NULL;
+		g_wps_probe_req_ie_len = 0;
+	}
+#endif
+#if !defined(CSCAN)
+	wl_iw_release_ss_cache_ctrl();
+#endif 
+#ifdef COEX_DHCP
+	wl_iw_bt_release();
+#endif 
+
+#ifdef SOFTAP
+	if (ap_cfg_running) {
+		WL_TRACE(("\n%s AP is going down\n", __FUNCTION__));
+		
+		wl_iw_send_priv_event(priv_dev, "AP_DOWN");
+	}
+#endif
+
+}
diff --git a/drivers/net/wireless/bcmdhd/wl_iw.h b/drivers/net/wireless/bcmdhd/wl_iw.h
new file mode 100644
index 000000000000..9cdb53dfef8c
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/wl_iw.h
@@ -0,0 +1,319 @@
+/*
+ * Linux Wireless Extensions support
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wl_iw.h,v 1.15.80.6 2010-12-23 01:13:23 $
+ */
+
+
+#ifndef _wl_iw_h_
+#define _wl_iw_h_
+
+#include <linux/wireless.h>
+
+#include <typedefs.h>
+#include <proto/ethernet.h>
+#include <wlioctl.h>
+
+#define WL_SCAN_PARAMS_SSID_MAX 	10
+#define GET_SSID			"SSID="
+#define GET_CHANNEL			"CH="
+#define GET_NPROBE 			"NPROBE="
+#define GET_ACTIVE_ASSOC_DWELL  	"ACTIVE="
+#define GET_PASSIVE_ASSOC_DWELL  	"PASSIVE="
+#define GET_HOME_DWELL  		"HOME="
+#define GET_SCAN_TYPE			"TYPE="
+
+#define BAND_GET_CMD				"GETBAND"
+#define BAND_SET_CMD				"SETBAND"
+#define DTIM_SKIP_GET_CMD			"DTIMSKIPGET"
+#define DTIM_SKIP_SET_CMD			"DTIMSKIPSET"
+#define SETSUSPENDOPT_CMD			"SETSUSPENDOPT"
+#define SETSUSPENDMODE_CMD			"SETSUSPENDMODE"
+#define PNOSSIDCLR_SET_CMD			"PNOSSIDCLR"
+
+#define PNOSETUP_SET_CMD			"PNOSETUP " 
+#define PNOSETADD_SET_CMD			"PNOSETADD"
+#define PNOENABLE_SET_CMD			"PNOFORCE"
+#define PNODEBUG_SET_CMD			"PNODEBUG"
+#define TXPOWER_SET_CMD				"TXPOWER"
+
+#define MAC2STR(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5]
+#define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x"
+
+
+typedef struct wl_iw_extra_params {
+	int 	target_channel; 
+} wl_iw_extra_params_t;
+
+struct cntry_locales_custom {
+	char iso_abbrev[WLC_CNTRY_BUF_SZ];	
+	char custom_locale[WLC_CNTRY_BUF_SZ];	
+	int32 custom_locale_rev;		
+};
+
+
+#define	WL_IW_RSSI_MINVAL		-200	
+#define	WL_IW_RSSI_NO_SIGNAL	-91	
+#define	WL_IW_RSSI_VERY_LOW	-80	
+#define	WL_IW_RSSI_LOW		-70	
+#define	WL_IW_RSSI_GOOD		-68	
+#define	WL_IW_RSSI_VERY_GOOD	-58	
+#define	WL_IW_RSSI_EXCELLENT	-57	
+#define	WL_IW_RSSI_INVALID	 0	
+#define MAX_WX_STRING 80
+#define isprint(c) bcm_isprint(c)
+#define WL_IW_SET_ACTIVE_SCAN	(SIOCIWFIRSTPRIV+1)
+#define WL_IW_GET_RSSI			(SIOCIWFIRSTPRIV+3)
+#define WL_IW_SET_PASSIVE_SCAN	(SIOCIWFIRSTPRIV+5)
+#define WL_IW_GET_LINK_SPEED	(SIOCIWFIRSTPRIV+7)
+#define WL_IW_GET_CURR_MACADDR	(SIOCIWFIRSTPRIV+9)
+#define WL_IW_SET_STOP				(SIOCIWFIRSTPRIV+11)
+#define WL_IW_SET_START			(SIOCIWFIRSTPRIV+13)
+
+
+#define WL_SET_AP_CFG           (SIOCIWFIRSTPRIV+15)
+#define WL_AP_STA_LIST          (SIOCIWFIRSTPRIV+17)
+#define WL_AP_MAC_FLTR	        (SIOCIWFIRSTPRIV+19)
+#define WL_AP_BSS_START         (SIOCIWFIRSTPRIV+21)
+#define AP_LPB_CMD              (SIOCIWFIRSTPRIV+23)
+#define WL_AP_STOP              (SIOCIWFIRSTPRIV+25)
+#define WL_FW_RELOAD            (SIOCIWFIRSTPRIV+27)
+#define WL_AP_STA_DISASSOC		(SIOCIWFIRSTPRIV+29)
+#define WL_COMBO_SCAN           (SIOCIWFIRSTPRIV+31)
+
+
+#define			G_SCAN_RESULTS 8*1024
+#define 		WE_ADD_EVENT_FIX	0x80
+#define          G_WLAN_SET_ON	0
+#define          G_WLAN_SET_OFF	1
+
+#define CHECK_EXTRA_FOR_NULL(extra) \
+if (!extra) { \
+	WL_ERROR(("%s: error : extra is null pointer\n", __FUNCTION__)); \
+	return -EINVAL; \
+}
+
+typedef struct wl_iw {
+	char nickname[IW_ESSID_MAX_SIZE];
+
+	struct iw_statistics wstats;
+
+	int spy_num;
+	int wpaversion;			
+	int pcipher;			
+	int gcipher;			
+	int privacy_invoked; 		
+
+	struct ether_addr spy_addr[IW_MAX_SPY];
+	struct iw_quality spy_qual[IW_MAX_SPY];
+	void  *wlinfo;
+	dhd_pub_t * pub;
+} wl_iw_t;
+
+int	 wl_control_wl_start(struct net_device *dev);
+#define WLC_IW_SS_CACHE_MAXLEN				2048
+#define WLC_IW_SS_CACHE_CTRL_FIELD_MAXLEN	32
+#define WLC_IW_BSS_INFO_MAXLEN 				\
+	(WLC_IW_SS_CACHE_MAXLEN - WLC_IW_SS_CACHE_CTRL_FIELD_MAXLEN)
+
+typedef struct wl_iw_ss_cache {
+	struct wl_iw_ss_cache *next;
+	int dirty;
+	uint32 buflen;
+	uint32 version;
+	uint32 count;
+	wl_bss_info_t bss_info[1];
+} wl_iw_ss_cache_t;
+
+typedef struct wl_iw_ss_cache_ctrl {
+	wl_iw_ss_cache_t *m_cache_head;	
+	int m_link_down;		
+	int m_timer_expired;		
+	char m_active_bssid[ETHER_ADDR_LEN];	
+	uint m_prev_scan_mode;	
+	uint m_cons_br_scan_cnt;	
+	struct timer_list *m_timer;	
+} wl_iw_ss_cache_ctrl_t;
+
+typedef enum broadcast_first_scan {
+	BROADCAST_SCAN_FIRST_IDLE = 0,
+	BROADCAST_SCAN_FIRST_STARTED,
+	BROADCAST_SCAN_FIRST_RESULT_READY,
+	BROADCAST_SCAN_FIRST_RESULT_CONSUMED
+} broadcast_first_scan_t;
+#ifdef SOFTAP
+#define SSID_LEN	33
+#define SEC_LEN		16
+#define KEY_LEN		65
+#define PROFILE_OFFSET	32
+struct ap_profile {
+	uint8	ssid[SSID_LEN];
+	uint8	sec[SEC_LEN];
+	uint8	key[KEY_LEN];
+	uint32	channel; 
+	uint32	preamble;
+	uint32	max_scb;	
+	uint32  closednet;  
+	char country_code[WLC_CNTRY_BUF_SZ];
+};
+
+
+#define MACLIST_MODE_DISABLED	0
+#define MACLIST_MODE_DENY		1
+#define MACLIST_MODE_ALLOW		2
+struct mflist {
+	uint count;
+	struct ether_addr ea[16];
+};
+struct mac_list_set {
+	uint32	mode;
+	struct mflist mac_list;
+};
+#endif   
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+extern const struct iw_handler_def wl_iw_handler_def;
+#endif 
+
+extern int wl_iw_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
+extern void wl_iw_event(struct net_device *dev, wl_event_msg_t *e, void* data);
+extern int wl_iw_get_wireless_stats(struct net_device *dev, struct iw_statistics *wstats);
+int wl_iw_attach(struct net_device *dev, void * dhdp);
+void wl_iw_detach(void);
+
+extern int net_os_wake_lock(struct net_device *dev);
+extern int net_os_wake_unlock(struct net_device *dev);
+extern int net_os_wake_lock_timeout(struct net_device *dev);
+extern int  net_os_wake_lock_ctrl_timeout_enable(struct net_device *dev, int val);
+extern int net_os_set_suspend_disable(struct net_device *dev, int val);
+extern int net_os_set_suspend(struct net_device *dev, int val, int force);
+extern int net_os_set_dtim_skip(struct net_device *dev, int val);
+extern void get_customized_country_code(char *country_iso_code, wl_country_t *cspec);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)
+#define IWE_STREAM_ADD_EVENT(info, stream, ends, iwe, extra) \
+	iwe_stream_add_event(info, stream, ends, iwe, extra)
+#define IWE_STREAM_ADD_VALUE(info, event, value, ends, iwe, event_len) \
+	iwe_stream_add_value(info, event, value, ends, iwe, event_len)
+#define IWE_STREAM_ADD_POINT(info, stream, ends, iwe, extra) \
+	iwe_stream_add_point(info, stream, ends, iwe, extra)
+#else
+#define IWE_STREAM_ADD_EVENT(info, stream, ends, iwe, extra) \
+	iwe_stream_add_event(stream, ends, iwe, extra)
+#define IWE_STREAM_ADD_VALUE(info, event, value, ends, iwe, event_len) \
+	iwe_stream_add_value(event, value, ends, iwe, event_len)
+#define IWE_STREAM_ADD_POINT(info, stream, ends, iwe, extra) \
+	iwe_stream_add_point(stream, ends, iwe, extra)
+#endif
+
+extern int dhd_pno_enable(dhd_pub_t *dhd, int pfn_enabled);
+extern int dhd_pno_clean(dhd_pub_t *dhd);
+extern int dhd_pno_set(dhd_pub_t *dhd, wlc_ssid_t* ssids_local, int nssid,
+                       ushort  scan_fr, int pno_repeat, int pno_freq_expo_max);
+extern int dhd_pno_get_status(dhd_pub_t *dhd);
+extern int dhd_dev_pno_reset(struct net_device *dev);
+extern int dhd_dev_pno_set(struct net_device *dev, wlc_ssid_t* ssids_local,
+                           int nssid, ushort  scan_fr, int pno_repeat, int pno_freq_expo_max);
+extern int dhd_dev_pno_enable(struct net_device *dev,  int pfn_enabled);
+extern int dhd_dev_get_pno_status(struct net_device *dev);
+extern int dhd_get_dtim_skip(dhd_pub_t *dhd);
+
+void	dhd_bus_country_set(struct net_device *dev, wl_country_t *cspec);
+
+#define PNO_TLV_PREFIX			'S'
+#define PNO_TLV_VERSION			'1'
+#define PNO_TLV_SUBVERSION 		'2'
+#define PNO_TLV_RESERVED		'0'
+#define PNO_TLV_TYPE_SSID_IE		'S'
+#define PNO_TLV_TYPE_TIME		'T'
+#define PNO_TLV_FREQ_REPEAT		'R'
+#define PNO_TLV_FREQ_EXPO_MAX	'M'
+#define  PNO_EVENT_UP			"PNO_EVENT"
+
+typedef struct cmd_tlv {
+	char prefix;
+	char version;
+	char subver;
+	char reserved;
+} cmd_tlv_t;
+
+
+
+
+typedef struct cscan_tlv {
+	char prefix;
+	char version;
+	char subver;
+	char reserved;
+} cscan_tlv_t;
+
+#define CSCAN_COMMAND				"CSCAN "
+#define CSCAN_TLV_PREFIX 			'S'
+#define CSCAN_TLV_VERSION			1
+#define CSCAN_TLV_SUBVERSION			0
+#define CSCAN_TLV_TYPE_SSID_IE          'S'
+#define CSCAN_TLV_TYPE_CHANNEL_IE   'C'
+#define CSCAN_TLV_TYPE_NPROBE_IE     'N'
+#define CSCAN_TLV_TYPE_ACTIVE_IE      'A'
+#define CSCAN_TLV_TYPE_PASSIVE_IE    'P'
+#define CSCAN_TLV_TYPE_HOME_IE         'H'
+#define CSCAN_TLV_TYPE_STYPE_IE        'T'
+
+#ifdef SOFTAP_TLV_CFG
+
+#define SOFTAP_SET_CMD				"SOFTAPSET "
+#define SOFTAP_TLV_PREFIX			'A'
+#define SOFTAP_TLV_VERSION			'1'
+#define SOFTAP_TLV_SUBVERSION		'0'
+#define SOFTAP_TLV_RESERVED		'0'
+
+#define TLV_TYPE_SSID				'S'
+#define TLV_TYPE_SECUR				'E'
+#define TLV_TYPE_KEY				'K'
+#define TLV_TYPE_CHANNEL			'C'
+#endif 
+
+extern int wl_iw_parse_channel_list_tlv(char** list_str, uint16* channel_list,
+	int channel_num, int *bytes_left);
+
+extern int wl_iw_parse_data_tlv(char** list_str, void  *dst, int dst_size,
+                                const char token, int input_size, int *bytes_left);
+
+extern int wl_iw_parse_ssid_list_tlv(char** list_str, wlc_ssid_t* ssid,
+                                     int max, int *bytes_left);
+
+extern int wl_iw_parse_ssid_list(char** list_str, wlc_ssid_t* ssid, int idx, int max);
+
+extern int wl_iw_parse_channel_list(char** list_str, uint16* channel_list, int channel_num);
+
+
+#define NETDEV_PRIV(dev)	(*(wl_iw_t **)netdev_priv(dev))
+
+#ifdef CONFIG_WPS2
+#define WPS_ADD_PROBE_REQ_IE_CMD "ADD_WPS_PROBE_REQ_IE "
+#define WPS_DEL_PROBE_REQ_IE_CMD "DEL_WPS_PROBE_REQ_IE "
+#define WPS_PROBE_REQ_IE_CMD_LENGTH 21
+#endif
+
+#endif 
diff --git a/drivers/net/wireless/bcmdhd/wl_linux_mon.c b/drivers/net/wireless/bcmdhd/wl_linux_mon.c
new file mode 100644
index 000000000000..f44b4b04bb96
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/wl_linux_mon.c
@@ -0,0 +1,409 @@
+/*
+ * Broadcom Dongle Host Driver (DHD), Linux monitor network interface
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wl_linux_mon.c 303266 2011-12-16 00:15:23Z $
+ */
+
+#include <linux/string.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/if_arp.h>
+#include <linux/ieee80211.h>
+#include <linux/rtnetlink.h>
+#include <net/ieee80211_radiotap.h>
+
+#include <wlioctl.h>
+#include <bcmutils.h>
+#include <linux_osl.h>
+#include <dhd_dbg.h>
+#include <dngl_stats.h>
+#include <dhd.h>
+
+typedef enum monitor_states
+{
+	MONITOR_STATE_DEINIT = 0x0,
+	MONITOR_STATE_INIT = 0x1,
+	MONITOR_STATE_INTERFACE_ADDED = 0x2,
+	MONITOR_STATE_INTERFACE_DELETED = 0x4
+} monitor_states_t;
+extern int dhd_start_xmit(struct sk_buff *skb, struct net_device *net);
+
+/**
+ * Local declarations and defintions (not exposed)
+ */
+#define MON_PRINT(format, ...) printk("DHD-MON: %s " format, __func__, ##__VA_ARGS__)
+#define MON_TRACE MON_PRINT
+
+typedef struct monitor_interface {
+	int radiotap_enabled;
+	struct net_device* real_ndev;	/* The real interface that the monitor is on */
+	struct net_device* mon_ndev;
+} monitor_interface;
+
+typedef struct dhd_linux_monitor {
+	void *dhd_pub;
+	monitor_states_t monitor_state;
+	monitor_interface mon_if[DHD_MAX_IFS];
+	struct mutex lock;		/* lock to protect mon_if */
+} dhd_linux_monitor_t;
+
+static dhd_linux_monitor_t g_monitor;
+
+static struct net_device* lookup_real_netdev(char *name);
+static monitor_interface* ndev_to_monif(struct net_device *ndev);
+static int dhd_mon_if_open(struct net_device *ndev);
+static int dhd_mon_if_stop(struct net_device *ndev);
+static int dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev);
+static void dhd_mon_if_set_multicast_list(struct net_device *ndev);
+static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr);
+
+static const struct net_device_ops dhd_mon_if_ops = {
+	.ndo_open		= dhd_mon_if_open,
+	.ndo_stop		= dhd_mon_if_stop,
+	.ndo_start_xmit		= dhd_mon_if_subif_start_xmit,
+	.ndo_set_multicast_list = dhd_mon_if_set_multicast_list,
+	.ndo_set_mac_address 	= dhd_mon_if_change_mac,
+};
+
+/**
+ * Local static function defintions
+ */
+
+/* Look up dhd's net device table to find a match (e.g. interface "eth0" is a match for "mon.eth0"
+ * "p2p-eth0-0" is a match for "mon.p2p-eth0-0")
+ */
+static struct net_device* lookup_real_netdev(char *name)
+{
+	int i;
+	int len = 0;
+	int last_name_len = 0;
+	struct net_device *ndev;
+	struct net_device *ndev_found = NULL;
+
+	/* We need to find interface "p2p-p2p-0" corresponding to monitor interface "mon-p2p-0",
+	 * Once mon iface name reaches IFNAMSIZ, it is reset to p2p0-0 and corresponding mon
+	 * iface would be mon-p2p0-0.
+	 */
+	for (i = 0; i < DHD_MAX_IFS; i++) {
+		ndev = dhd_idx2net(g_monitor.dhd_pub, i);
+
+		/* Skip "p2p" and look for "-p2p0-x" in monitor interface name. If it
+		 * it matches, then this netdev is the corresponding real_netdev.
+		 */
+		if (ndev && strstr(ndev->name, "p2p-p2p0")) {
+			len = strlen("p2p");
+		} else {
+		/* if p2p- is not present, then the IFNAMSIZ have reached and name
+		 * would have got reset. In this casse,look for p2p0-x in mon-p2p0-x
+		 */
+			len = 0;
+		}
+		if (ndev && strstr(name, (ndev->name + len))) {
+			if (strlen(ndev->name) > last_name_len) {
+				ndev_found = ndev;
+				last_name_len = strlen(ndev->name);
+			}
+		}
+	}
+
+	return ndev_found;
+}
+
+static monitor_interface* ndev_to_monif(struct net_device *ndev)
+{
+	int i;
+
+	for (i = 0; i < DHD_MAX_IFS; i++) {
+		if (g_monitor.mon_if[i].mon_ndev == ndev)
+			return &g_monitor.mon_if[i];
+	}
+
+	return NULL;
+}
+
+static int dhd_mon_if_open(struct net_device *ndev)
+{
+	int ret = 0;
+
+	MON_PRINT("enter\n");
+	return ret;
+}
+
+static int dhd_mon_if_stop(struct net_device *ndev)
+{
+	int ret = 0;
+
+	MON_PRINT("enter\n");
+	return ret;
+}
+
+static int dhd_mon_if_subif_start_xmit(struct sk_buff *skb, struct net_device *ndev)
+{
+	int ret = 0;
+	int rtap_len;
+	int qos_len = 0;
+	int dot11_hdr_len = 24;
+	int snap_len = 6;
+	unsigned char *pdata;
+	unsigned short frame_ctl;
+	unsigned char src_mac_addr[6];
+	unsigned char dst_mac_addr[6];
+	struct ieee80211_hdr *dot11_hdr;
+	struct ieee80211_radiotap_header *rtap_hdr;
+	monitor_interface* mon_if;
+
+	MON_PRINT("enter\n");
+
+	mon_if = ndev_to_monif(ndev);
+	if (mon_if == NULL || mon_if->real_ndev == NULL) {
+		MON_PRINT(" cannot find matched net dev, skip the packet\n");
+		goto fail;
+	}
+
+	if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header)))
+		goto fail;
+
+	rtap_hdr = (struct ieee80211_radiotap_header *)skb->data;
+	if (unlikely(rtap_hdr->it_version))
+		goto fail;
+
+	rtap_len = ieee80211_get_radiotap_len(skb->data);
+	if (unlikely(skb->len < rtap_len))
+		goto fail;
+
+	MON_PRINT("radiotap len (should be 14): %d\n", rtap_len);
+
+	/* Skip the ratio tap header */
+	skb_pull(skb, rtap_len);
+
+	dot11_hdr = (struct ieee80211_hdr *)skb->data;
+	frame_ctl = le16_to_cpu(dot11_hdr->frame_control);
+	/* Check if the QoS bit is set */
+	if ((frame_ctl & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) {
+		/* Check if this ia a Wireless Distribution System (WDS) frame
+		 * which has 4 MAC addresses
+		 */
+		if (dot11_hdr->frame_control & 0x0080)
+			qos_len = 2;
+		if ((dot11_hdr->frame_control & 0x0300) == 0x0300)
+			dot11_hdr_len += 6;
+
+		memcpy(dst_mac_addr, dot11_hdr->addr1, sizeof(dst_mac_addr));
+		memcpy(src_mac_addr, dot11_hdr->addr2, sizeof(src_mac_addr));
+
+		/* Skip the 802.11 header, QoS (if any) and SNAP, but leave spaces for
+		 * for two MAC addresses
+		 */
+		skb_pull(skb, dot11_hdr_len + qos_len + snap_len - sizeof(src_mac_addr) * 2);
+		pdata = (unsigned char*)skb->data;
+		memcpy(pdata, dst_mac_addr, sizeof(dst_mac_addr));
+		memcpy(pdata + sizeof(dst_mac_addr), src_mac_addr, sizeof(src_mac_addr));
+
+		MON_PRINT("if name: %s, matched if name %s\n", ndev->name, mon_if->real_ndev->name);
+
+		/* Use the real net device to transmit the packet */
+		ret = dhd_start_xmit(skb, mon_if->real_ndev);
+
+		return ret;
+	}
+fail:
+	dev_kfree_skb(skb);
+	return 0;
+}
+
+static void dhd_mon_if_set_multicast_list(struct net_device *ndev)
+{
+	monitor_interface* mon_if;
+
+	mon_if = ndev_to_monif(ndev);
+	if (mon_if == NULL || mon_if->real_ndev == NULL) {
+		MON_PRINT(" cannot find matched net dev, skip the packet\n");
+	} else {
+		MON_PRINT("enter, if name: %s, matched if name %s\n",
+		ndev->name, mon_if->real_ndev->name);
+	}
+}
+
+static int dhd_mon_if_change_mac(struct net_device *ndev, void *addr)
+{
+	int ret = 0;
+	monitor_interface* mon_if;
+
+	mon_if = ndev_to_monif(ndev);
+	if (mon_if == NULL || mon_if->real_ndev == NULL) {
+		MON_PRINT(" cannot find matched net dev, skip the packet\n");
+	} else {
+		MON_PRINT("enter, if name: %s, matched if name %s\n",
+		ndev->name, mon_if->real_ndev->name);
+	}
+	return ret;
+}
+
+/**
+ * Global function definitions (declared in dhd_linux_mon.h)
+ */
+
+int dhd_add_monitor(char *name, struct net_device **new_ndev)
+{
+	int i;
+	int idx = -1;
+	int ret = 0;
+	struct net_device* ndev = NULL;
+	dhd_linux_monitor_t **dhd_mon;
+
+	mutex_lock(&g_monitor.lock);
+
+	MON_TRACE("enter, if name: %s\n", name);
+	if (!name || !new_ndev) {
+		MON_PRINT("invalid parameters\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/*
+	 * Find a vacancy
+	 */
+	for (i = 0; i < DHD_MAX_IFS; i++)
+		if (g_monitor.mon_if[i].mon_ndev == NULL) {
+			idx = i;
+			break;
+		}
+	if (idx == -1) {
+		MON_PRINT("exceeds maximum interfaces\n");
+		ret = -EFAULT;
+		goto out;
+	}
+
+	ndev = alloc_etherdev(sizeof(dhd_linux_monitor_t*));
+	if (!ndev) {
+		MON_PRINT("failed to allocate memory\n");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ndev->type = ARPHRD_IEEE80211_RADIOTAP;
+	strncpy(ndev->name, name, IFNAMSIZ);
+	ndev->name[IFNAMSIZ - 1] = 0;
+	ndev->netdev_ops = &dhd_mon_if_ops;
+
+	ret = register_netdevice(ndev);
+	if (ret) {
+		MON_PRINT(" register_netdevice failed (%d)\n", ret);
+		goto out;
+	}
+
+	*new_ndev = ndev;
+	g_monitor.mon_if[idx].radiotap_enabled = TRUE;
+	g_monitor.mon_if[idx].mon_ndev = ndev;
+	g_monitor.mon_if[idx].real_ndev = lookup_real_netdev(name);
+	dhd_mon = (dhd_linux_monitor_t **)netdev_priv(ndev);
+	*dhd_mon = &g_monitor;
+	g_monitor.monitor_state = MONITOR_STATE_INTERFACE_ADDED;
+	MON_PRINT("net device returned: 0x%p\n", ndev);
+	MON_PRINT("found a matched net device, name %s\n", g_monitor.mon_if[idx].real_ndev->name);
+
+out:
+	if (ret && ndev)
+		free_netdev(ndev);
+
+	mutex_unlock(&g_monitor.lock);
+	return ret;
+
+}
+
+int dhd_del_monitor(struct net_device *ndev)
+{
+	int i;
+	bool rollback_lock = false;
+	if (!ndev)
+		return -EINVAL;
+	mutex_lock(&g_monitor.lock);
+	for (i = 0; i < DHD_MAX_IFS; i++) {
+		if (g_monitor.mon_if[i].mon_ndev == ndev ||
+			g_monitor.mon_if[i].real_ndev == ndev) {
+			g_monitor.mon_if[i].real_ndev = NULL;
+			if (rtnl_is_locked()) {
+				rtnl_unlock();
+				rollback_lock = true;
+			}
+			unregister_netdev(g_monitor.mon_if[i].mon_ndev);
+			free_netdev(g_monitor.mon_if[i].mon_ndev);
+			g_monitor.mon_if[i].mon_ndev = NULL;
+			g_monitor.monitor_state = MONITOR_STATE_INTERFACE_DELETED;
+			break;
+		}
+	}
+	if (rollback_lock) {
+		rtnl_lock();
+		rollback_lock = false;
+	}
+
+	if (g_monitor.monitor_state !=
+	MONITOR_STATE_INTERFACE_DELETED)
+		MON_PRINT("interface not found in monitor IF array, is this a monitor IF? 0x%p\n",
+			ndev);
+	mutex_unlock(&g_monitor.lock);
+
+	return 0;
+}
+
+int dhd_monitor_init(void *dhd_pub)
+{
+	if (g_monitor.monitor_state == MONITOR_STATE_DEINIT) {
+		g_monitor.dhd_pub = dhd_pub;
+		mutex_init(&g_monitor.lock);
+		g_monitor.monitor_state = MONITOR_STATE_INIT;
+	}
+	return 0;
+}
+
+int dhd_monitor_uninit(void)
+{
+	int i;
+	struct net_device *ndev;
+	bool rollback_lock = false;
+	mutex_lock(&g_monitor.lock);
+	if (g_monitor.monitor_state != MONITOR_STATE_DEINIT) {
+		for (i = 0; i < DHD_MAX_IFS; i++) {
+			ndev = g_monitor.mon_if[i].mon_ndev;
+			if (ndev) {
+				if (rtnl_is_locked()) {
+					rtnl_unlock();
+					rollback_lock = true;
+				}
+				unregister_netdev(ndev);
+				free_netdev(ndev);
+				g_monitor.mon_if[i].real_ndev = NULL;
+				g_monitor.mon_if[i].mon_ndev = NULL;
+				if (rollback_lock) {
+					rtnl_lock();
+					rollback_lock = false;
+				}
+			}
+		}
+		g_monitor.monitor_state = MONITOR_STATE_DEINIT;
+	}
+	mutex_unlock(&g_monitor.lock);
+	return 0;
+}
diff --git a/drivers/net/wireless/bcmdhd/wldev_common.c b/drivers/net/wireless/bcmdhd/wldev_common.c
new file mode 100644
index 000000000000..827033185035
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/wldev_common.c
@@ -0,0 +1,418 @@
+/*
+ * Common function shared by Linux WEXT, cfg80211 and p2p drivers
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wldev_common.c,v 1.1.4.1.2.14 2011-02-09 01:40:07 $
+ */
+
+#include <osl.h>
+#include <linux/kernel.h>
+#include <linux/kthread.h>
+#include <linux/netdevice.h>
+
+#include <wldev_common.h>
+#include <bcmutils.h>
+
+#define htod32(i) i
+#define htod16(i) i
+#define dtoh32(i) i
+#define dtoh16(i) i
+#define htodchanspec(i) i
+#define dtohchanspec(i) i
+
+#define	WLDEV_ERROR(args)						\
+	do {										\
+		printk(KERN_ERR "WLDEV-ERROR) %s : ", __func__);	\
+		printk args;							\
+	} while (0)
+
+extern int dhd_ioctl_entry_local(struct net_device *net, wl_ioctl_t *ioc, int cmd);
+
+s32 wldev_ioctl(
+	struct net_device *dev, u32 cmd, void *arg, u32 len, u32 set)
+{
+	s32 ret = 0;
+	struct wl_ioctl ioc;
+
+	memset(&ioc, 0, sizeof(ioc));
+	ioc.cmd = cmd;
+	ioc.buf = arg;
+	ioc.len = len;
+	ioc.set = set;
+
+	ret = dhd_ioctl_entry_local(dev, &ioc, cmd);
+	return ret;
+}
+
+/* Format a iovar buffer, not bsscfg indexed. The bsscfg index will be
+ * taken care of in dhd_ioctl_entry. Internal use only, not exposed to
+ * wl_iw, wl_cfg80211 and wl_cfgp2p
+ */
+static s32 wldev_mkiovar(
+	s8 *iovar_name, s8 *param, s32 paramlen,
+	s8 *iovar_buf, u32 buflen)
+{
+	s32 iolen = 0;
+
+	iolen = bcm_mkiovar(iovar_name, param, paramlen, iovar_buf, buflen);
+	return iolen;
+}
+
+s32 wldev_iovar_getbuf(
+	struct net_device *dev, s8 *iovar_name,
+	void *param, s32 paramlen, void *buf, s32 buflen, struct mutex* buf_sync)
+{
+	s32 ret = 0;
+	s32 iovar_len = 0;
+	if (buf_sync) {
+		mutex_lock(buf_sync);
+	}
+	iovar_len = wldev_mkiovar(iovar_name, param, paramlen, buf, buflen);
+	ret = wldev_ioctl(dev, WLC_GET_VAR, buf, buflen, FALSE);
+	if (buf_sync)
+		mutex_unlock(buf_sync);
+	return ret;
+}
+
+
+s32 wldev_iovar_setbuf(
+	struct net_device *dev, s8 *iovar_name,
+	void *param, s32 paramlen, void *buf, s32 buflen, struct mutex* buf_sync)
+{
+	s32 ret = 0;
+	s32 iovar_len;
+	if (buf_sync) {
+		mutex_lock(buf_sync);
+	}
+	iovar_len = wldev_mkiovar(iovar_name, param, paramlen, buf, buflen);
+	ret = wldev_ioctl(dev, WLC_SET_VAR, buf, iovar_len, TRUE);
+	if (buf_sync)
+		mutex_unlock(buf_sync);
+	return ret;
+}
+
+s32 wldev_iovar_setint(
+	struct net_device *dev, s8 *iovar, s32 val)
+{
+	s8 iovar_buf[WLC_IOCTL_SMLEN];
+
+	val = htod32(val);
+	memset(iovar_buf, 0, sizeof(iovar_buf));
+	return wldev_iovar_setbuf(dev, iovar, &val, sizeof(val), iovar_buf,
+		sizeof(iovar_buf), NULL);
+}
+
+
+s32 wldev_iovar_getint(
+	struct net_device *dev, s8 *iovar, s32 *pval)
+{
+	s8 iovar_buf[WLC_IOCTL_SMLEN];
+	s32 err;
+
+	memset(iovar_buf, 0, sizeof(iovar_buf));
+	err = wldev_iovar_getbuf(dev, iovar, pval, sizeof(*pval), iovar_buf,
+		sizeof(iovar_buf), NULL);
+	if (err == 0)
+	{
+		memcpy(pval, iovar_buf, sizeof(*pval));
+		*pval = dtoh32(*pval);
+	}
+	return err;
+}
+
+/** Format a bsscfg indexed iovar buffer. The bsscfg index will be
+ *  taken care of in dhd_ioctl_entry. Internal use only, not exposed to
+ *  wl_iw, wl_cfg80211 and wl_cfgp2p
+ */
+s32 wldev_mkiovar_bsscfg(
+	const s8 *iovar_name, s8 *param, s32 paramlen,
+	s8 *iovar_buf, s32 buflen, s32 bssidx)
+{
+	const s8 *prefix = "bsscfg:";
+	s8 *p;
+	u32 prefixlen;
+	u32 namelen;
+	u32 iolen;
+
+	if (bssidx == 0) {
+		return wldev_mkiovar((s8*)iovar_name, (s8 *)param, paramlen,
+			(s8 *) iovar_buf, buflen);
+	}
+
+	prefixlen = (u32) strlen(prefix); /* lengh of bsscfg prefix */
+	namelen = (u32) strlen(iovar_name) + 1; /* lengh of iovar  name + null */
+	iolen = prefixlen + namelen + sizeof(u32) + paramlen;
+
+	if (buflen < 0 || iolen > (u32)buflen)
+	{
+		WLDEV_ERROR(("%s: buffer is too short\n", __FUNCTION__));
+		return BCME_BUFTOOSHORT;
+	}
+
+	p = (s8 *)iovar_buf;
+
+	/* copy prefix, no null */
+	memcpy(p, prefix, prefixlen);
+	p += prefixlen;
+
+	/* copy iovar name including null */
+	memcpy(p, iovar_name, namelen);
+	p += namelen;
+
+	/* bss config index as first param */
+	bssidx = htod32(bssidx);
+	memcpy(p, &bssidx, sizeof(u32));
+	p += sizeof(u32);
+
+	/* parameter buffer follows */
+	if (paramlen)
+		memcpy(p, param, paramlen);
+
+	return iolen;
+
+}
+
+s32 wldev_iovar_getbuf_bsscfg(
+	struct net_device *dev, s8 *iovar_name,
+	void *param, s32 paramlen, void *buf, s32 buflen, s32 bsscfg_idx, struct mutex* buf_sync)
+{
+	s32 ret = 0;
+	s32 iovar_len = 0;
+	if (buf_sync) {
+		mutex_lock(buf_sync);
+	}
+	iovar_len = wldev_mkiovar_bsscfg(iovar_name, param, paramlen, buf, buflen, bsscfg_idx);
+	ret = wldev_ioctl(dev, WLC_GET_VAR, buf, buflen, FALSE);
+	if (buf_sync) {
+		mutex_unlock(buf_sync);
+	}
+	return ret;
+
+}
+
+s32 wldev_iovar_setbuf_bsscfg(
+	struct net_device *dev, s8 *iovar_name,
+	void *param, s32 paramlen, void *buf, s32 buflen, s32 bsscfg_idx, struct mutex* buf_sync)
+{
+	s32 ret = 0;
+	s32 iovar_len;
+	if (buf_sync) {
+		mutex_lock(buf_sync);
+	}
+	iovar_len = wldev_mkiovar_bsscfg(iovar_name, param, paramlen, buf, buflen, bsscfg_idx);
+
+	ret = wldev_ioctl(dev, WLC_SET_VAR, buf, iovar_len, TRUE);
+	if (buf_sync) {
+		mutex_unlock(buf_sync);
+	}
+	return ret;
+}
+
+s32 wldev_iovar_setint_bsscfg(
+	struct net_device *dev, s8 *iovar, s32 val, s32 bssidx)
+{
+	s8 iovar_buf[WLC_IOCTL_SMLEN];
+
+	val = htod32(val);
+	memset(iovar_buf, 0, sizeof(iovar_buf));
+	return wldev_iovar_setbuf_bsscfg(dev, iovar, &val, sizeof(val), iovar_buf,
+		sizeof(iovar_buf), bssidx, NULL);
+}
+
+
+s32 wldev_iovar_getint_bsscfg(
+	struct net_device *dev, s8 *iovar, s32 *pval, s32 bssidx)
+{
+	s8 iovar_buf[WLC_IOCTL_SMLEN];
+	s32 err;
+
+	memset(iovar_buf, 0, sizeof(iovar_buf));
+	err = wldev_iovar_getbuf_bsscfg(dev, iovar, pval, sizeof(*pval), iovar_buf,
+		sizeof(iovar_buf), bssidx, NULL);
+	if (err == 0)
+	{
+		memcpy(pval, iovar_buf, sizeof(*pval));
+		*pval = dtoh32(*pval);
+	}
+	return err;
+}
+
+int wldev_get_link_speed(
+	struct net_device *dev, int *plink_speed)
+{
+	int error;
+
+	if (!plink_speed)
+		return -ENOMEM;
+	error = wldev_ioctl(dev, WLC_GET_RATE, plink_speed, sizeof(int), 0);
+	if (unlikely(error))
+		return error;
+
+	/* Convert internal 500Kbps to Kbps */
+	*plink_speed *= 500;
+	return error;
+}
+
+int wldev_get_rssi(
+	struct net_device *dev, int *prssi)
+{
+	scb_val_t scb_val;
+	int error;
+
+	if (!prssi)
+		return -ENOMEM;
+	bzero(&scb_val, sizeof(scb_val_t));
+
+	error = wldev_ioctl(dev, WLC_GET_RSSI, &scb_val, sizeof(scb_val_t), 0);
+	if (unlikely(error))
+		return error;
+
+	*prssi = dtoh32(scb_val.val);
+	return error;
+}
+
+int wldev_get_ssid(
+	struct net_device *dev, wlc_ssid_t *pssid)
+{
+	int error;
+
+	if (!pssid)
+		return -ENOMEM;
+	error = wldev_ioctl(dev, WLC_GET_SSID, pssid, sizeof(wlc_ssid_t), 0);
+	if (unlikely(error))
+		return error;
+	pssid->SSID_len = dtoh32(pssid->SSID_len);
+	return error;
+}
+
+int wldev_get_band(
+	struct net_device *dev, uint *pband)
+{
+	int error;
+
+	error = wldev_ioctl(dev, WLC_GET_BAND, pband, sizeof(uint), 0);
+	return error;
+}
+
+int wldev_set_band(
+	struct net_device *dev, uint band)
+{
+	int error = -1;
+
+	if ((band == WLC_BAND_AUTO) || (band == WLC_BAND_5G) || (band == WLC_BAND_2G)) {
+		error = wldev_ioctl(dev, WLC_SET_BAND, &band, sizeof(band), 1);
+	}
+	return error;
+}
+
+int wldev_set_country(
+	struct net_device *dev, char *country_code)
+{
+	int error = -1;
+	wl_country_t cspec = {{0}, 0, {0}};
+	scb_val_t scbval;
+	char smbuf[WLC_IOCTL_SMLEN];
+
+	if (!country_code)
+		return error;
+
+	error = wldev_iovar_getbuf(dev, "country", &cspec, sizeof(cspec),
+		smbuf, sizeof(smbuf), NULL);
+	if (error < 0)
+		WLDEV_ERROR(("%s: get country failed = %d\n", __FUNCTION__, error));
+
+	if ((error < 0) ||
+	    (strncmp(country_code, smbuf, WLC_CNTRY_BUF_SZ) != 0)) {
+		bzero(&scbval, sizeof(scb_val_t));
+		error = wldev_ioctl(dev, WLC_DISASSOC, &scbval, sizeof(scb_val_t), 1);
+		if (error < 0) {
+			WLDEV_ERROR(("%s: set country failed due to Disassoc error %d\n",
+				__FUNCTION__, error));
+			return error;
+		}
+	}
+	cspec.rev = -1;
+	memcpy(cspec.country_abbrev, country_code, WLC_CNTRY_BUF_SZ);
+	memcpy(cspec.ccode, country_code, WLC_CNTRY_BUF_SZ);
+	get_customized_country_code((char *)&cspec.country_abbrev, &cspec);
+	error = wldev_iovar_setbuf(dev, "country", &cspec, sizeof(cspec),
+		smbuf, sizeof(smbuf), NULL);
+	if (error < 0) {
+		WLDEV_ERROR(("%s: set country for %s as %s rev %d failed\n",
+			__FUNCTION__, country_code, cspec.ccode, cspec.rev));
+		return error;
+	}
+	dhd_bus_country_set(dev, &cspec);
+	WLDEV_ERROR(("%s: set country for %s as %s rev %d\n",
+		__FUNCTION__, country_code, cspec.ccode, cspec.rev));
+	return 0;
+}
+
+/*
+ *  softap channel autoselect
+ */
+int wldev_get_auto_channel(struct net_device *dev, int *chan)
+{
+	int chosen = 0;
+	wl_uint32_list_t request;
+	int retry = 0;
+	int updown = 0;
+	int ret = 0;
+	wlc_ssid_t null_ssid;
+
+	memset(&null_ssid, 0, sizeof(wlc_ssid_t));
+	ret |= wldev_ioctl(dev, WLC_UP, &updown, sizeof(updown), true);
+
+	ret |= wldev_ioctl(dev, WLC_SET_SSID, &null_ssid, sizeof(null_ssid), true);
+
+	request.count = htod32(0);
+	ret = wldev_ioctl(dev, WLC_START_CHANNEL_SEL, &request, sizeof(request), true);
+	if (ret < 0) {
+		WLDEV_ERROR(("can't start auto channel scan:%d\n", ret));
+		goto fail;
+	}
+
+	while  (retry++ < 15) {
+
+		bcm_mdelay(350);
+
+		ret = wldev_ioctl(dev, WLC_GET_CHANNEL_SEL, &chosen, sizeof(chosen), false);
+
+		if ((ret == 0) && (dtoh32(chosen) != 0)) {
+			*chan = (uint16)chosen & 0x00FF;  /* covert chanspec --> chan number  */
+			printf("%s: Got channel = %d, attempt:%d\n",
+				__FUNCTION__, *chan, retry);
+			break;
+		}
+	}
+
+	if ((ret = wldev_ioctl(dev, WLC_DOWN, &updown, sizeof(updown), true)) < 0) {
+		WLDEV_ERROR(("%s fail to WLC_DOWN ioctl err =%d\n", __FUNCTION__, ret));
+		goto fail;
+	}
+
+fail :
+	if (ret < 0) {
+		WLDEV_ERROR(("%s: return value %d\n", __FUNCTION__, ret));
+	}
+	return ret;
+}
diff --git a/drivers/net/wireless/bcmdhd/wldev_common.h b/drivers/net/wireless/bcmdhd/wldev_common.h
new file mode 100644
index 000000000000..f58660944420
--- /dev/null
+++ b/drivers/net/wireless/bcmdhd/wldev_common.h
@@ -0,0 +1,112 @@
+/*
+ * Common function shared by Linux WEXT, cfg80211 and p2p drivers
+ *
+ * Copyright (C) 1999-2011, Broadcom Corporation
+ * 
+ *         Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2 (the "GPL"),
+ * available at http://www.broadcom.com/licenses/GPLv2.php, with the
+ * following added to such license:
+ * 
+ *      As a special exception, the copyright holders of this software give you
+ * permission to link this software with independent modules, and to copy and
+ * distribute the resulting executable under terms of your choice, provided that
+ * you also meet, for each linked independent module, the terms and conditions of
+ * the license of that module.  An independent module is a module which is not
+ * derived from this software.  The special exception does not apply to any
+ * modifications of the software.
+ * 
+ *      Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a license
+ * other than the GPL, without Broadcom's express prior written consent.
+ *
+ * $Id: wldev_common.h,v 1.1.4.1.2.14 2011-02-09 01:40:07 $
+ */
+#ifndef __WLDEV_COMMON_H__
+#define __WLDEV_COMMON_H__
+
+#include <wlioctl.h>
+
+/* wl_dev_ioctl - get/set IOCTLs, will call net_device's do_ioctl (or
+ *  netdev_ops->ndo_do_ioctl in new kernels)
+ *  @dev: the net_device handle
+ */
+s32 wldev_ioctl(
+	struct net_device *dev, u32 cmd, void *arg, u32 len, u32 set);
+
+/** Retrieve named IOVARs, this function calls wl_dev_ioctl with
+ *  WLC_GET_VAR IOCTL code
+ */
+s32 wldev_iovar_getbuf(
+	struct net_device *dev, s8 *iovar_name,
+	void *param, s32 paramlen, void *buf, s32 buflen, struct mutex* buf_sync);
+
+/** Set named IOVARs, this function calls wl_dev_ioctl with
+ *  WLC_SET_VAR IOCTL code
+ */
+s32 wldev_iovar_setbuf(
+	struct net_device *dev, s8 *iovar_name,
+	void *param, s32 paramlen, void *buf, s32 buflen, struct mutex* buf_sync);
+
+s32 wldev_iovar_setint(
+	struct net_device *dev, s8 *iovar, s32 val);
+
+s32 wldev_iovar_getint(
+	struct net_device *dev, s8 *iovar, s32 *pval);
+
+/** The following function can be implemented if there is a need for bsscfg
+ *  indexed IOVARs
+ */
+
+s32 wldev_mkiovar_bsscfg(
+	const s8 *iovar_name, s8 *param, s32 paramlen,
+	s8 *iovar_buf, s32 buflen, s32 bssidx);
+
+/** Retrieve named and bsscfg indexed IOVARs, this function calls wl_dev_ioctl with
+ *  WLC_GET_VAR IOCTL code
+ */
+s32 wldev_iovar_getbuf_bsscfg(
+	struct net_device *dev, s8 *iovar_name, void *param, s32 paramlen,
+	void *buf, s32 buflen, s32 bsscfg_idx, struct mutex* buf_sync);
+
+/** Set named and bsscfg indexed IOVARs, this function calls wl_dev_ioctl with
+ *  WLC_SET_VAR IOCTL code
+ */
+s32 wldev_iovar_setbuf_bsscfg(
+	struct net_device *dev, s8 *iovar_name, void *param, s32 paramlen,
+	void *buf, s32 buflen, s32 bsscfg_idx, struct mutex* buf_sync);
+
+s32 wldev_iovar_getint_bsscfg(
+	struct net_device *dev, s8 *iovar, s32 *pval, s32 bssidx);
+
+s32 wldev_iovar_setint_bsscfg(
+	struct net_device *dev, s8 *iovar, s32 val, s32 bssidx);
+
+extern void get_customized_country_code(char *country_iso_code, wl_country_t *cspec);
+extern void dhd_bus_country_set(struct net_device *dev, wl_country_t *cspec);
+extern int wldev_set_country(struct net_device *dev, char *country_code);
+extern int net_os_wake_lock(struct net_device *dev);
+extern int net_os_wake_unlock(struct net_device *dev);
+extern int net_os_wake_lock_timeout(struct net_device *dev);
+extern int net_os_wake_lock_timeout_enable(struct net_device *dev, int val);
+extern int net_os_set_dtim_skip(struct net_device *dev, int val);
+extern int net_os_set_suspend_disable(struct net_device *dev, int val);
+extern int net_os_set_suspend(struct net_device *dev, int val, int force);
+extern int wl_iw_parse_ssid_list_tlv(char** list_str, wlc_ssid_t* ssid,
+	int max, int *bytes_left);
+
+/* Get the link speed from dongle, speed is in kpbs */
+int wldev_get_link_speed(struct net_device *dev, int *plink_speed);
+
+int wldev_get_rssi(struct net_device *dev, int *prssi);
+
+int wldev_get_ssid(struct net_device *dev, wlc_ssid_t *pssid);
+
+int wldev_get_band(struct net_device *dev, uint *pband);
+
+int wldev_set_band(struct net_device *dev, uint band);
+
+int wldev_get_auto_channel(struct net_device *dev, int *chan);
+
+#endif /* __WLDEV_COMMON_H__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c b/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c
index d42ef1763a71..5493f94d23cc 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c
@@ -295,8 +295,8 @@ static int iwlagn_rxon_connect(struct iwl_priv *priv,
 		return ret;
 	}
 
-	if ((ctx->vif && ctx->vif->type == NL80211_IFTYPE_STATION) &&
-	    priv->cfg->ht_params->smps_mode)
+	if (ctx->vif && ctx->vif->type == NL80211_IFTYPE_STATION &&
+	    priv->cfg->ht_params && priv->cfg->ht_params->smps_mode)
 		ieee80211_request_smps(ctx->vif,
 				       priv->cfg->ht_params->smps_mode);
 
@@ -542,6 +542,24 @@ int iwlagn_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	return 0;
 }
 
+void iwlagn_config_ht40(struct ieee80211_conf *conf,
+	struct iwl_rxon_context *ctx)
+{
+	if (conf_is_ht40_minus(conf)) {
+		ctx->ht.extension_chan_offset =
+			IEEE80211_HT_PARAM_CHA_SEC_BELOW;
+		ctx->ht.is_40mhz = true;
+	} else if (conf_is_ht40_plus(conf)) {
+		ctx->ht.extension_chan_offset =
+			IEEE80211_HT_PARAM_CHA_SEC_ABOVE;
+		ctx->ht.is_40mhz = true;
+	} else {
+		ctx->ht.extension_chan_offset =
+			IEEE80211_HT_PARAM_CHA_SEC_NONE;
+		ctx->ht.is_40mhz = false;
+	}
+}
+
 int iwlagn_mac_config(struct ieee80211_hw *hw, u32 changed)
 {
 	struct iwl_priv *priv = hw->priv;
@@ -600,19 +618,11 @@ int iwlagn_mac_config(struct ieee80211_hw *hw, u32 changed)
 				ctx->ht.enabled = conf_is_ht(conf);
 
 			if (ctx->ht.enabled) {
-				if (conf_is_ht40_minus(conf)) {
-					ctx->ht.extension_chan_offset =
-						IEEE80211_HT_PARAM_CHA_SEC_BELOW;
-					ctx->ht.is_40mhz = true;
-				} else if (conf_is_ht40_plus(conf)) {
-					ctx->ht.extension_chan_offset =
-						IEEE80211_HT_PARAM_CHA_SEC_ABOVE;
-					ctx->ht.is_40mhz = true;
-				} else {
-					ctx->ht.extension_chan_offset =
-						IEEE80211_HT_PARAM_CHA_SEC_NONE;
-					ctx->ht.is_40mhz = false;
-				}
+				/* if HT40 is used, it should not change
+				 * after associated except channel switch */
+				if (!ctx->ht.is_40mhz ||
+						!iwl_is_associated_ctx(ctx))
+					iwlagn_config_ht40(conf, ctx);
 			} else
 				ctx->ht.is_40mhz = false;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-sta.c b/drivers/net/wireless/iwlwifi/iwl-agn-sta.c
index 37e624095e40..211a5ad6a4f4 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-sta.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-sta.c
@@ -440,9 +440,6 @@ int iwl_set_dynamic_key(struct iwl_priv *priv,
 
 	switch (keyconf->cipher) {
 	case WLAN_CIPHER_SUITE_TKIP:
-		keyconf->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
-		keyconf->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
-
 		if (sta)
 			addr = sta->addr;
 		else /* station mode case only */
@@ -455,8 +452,6 @@ int iwl_set_dynamic_key(struct iwl_priv *priv,
 					  seq.tkip.iv32, p1k, CMD_SYNC);
 		break;
 	case WLAN_CIPHER_SUITE_CCMP:
-		keyconf->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
-		/* fall through */
 	case WLAN_CIPHER_SUITE_WEP40:
 	case WLAN_CIPHER_SUITE_WEP104:
 		ret = iwlagn_send_sta_key(priv, keyconf, sta_id,
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c b/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
index 53bb59ee719d..475f9d4f56e2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
@@ -166,7 +166,10 @@ static void iwlagn_tx_cmd_build_basic(struct iwl_priv *priv,
 		tx_cmd->tid_tspec = qc[0] & 0xf;
 		tx_flags &= ~TX_CMD_FLG_SEQ_CTL_MSK;
 	} else {
-		tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+		if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ)
+			tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+		else
+			tx_flags &= ~TX_CMD_FLG_SEQ_CTL_MSK;
 	}
 
 	iwlagn_tx_cmd_protection(priv, info, fc, &tx_flags);
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-ucode.c b/drivers/net/wireless/iwlwifi/iwl-agn-ucode.c
index 56211006a182..a5c5a0accd5e 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-ucode.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-ucode.c
@@ -113,13 +113,8 @@ static int iwlagn_load_section(struct iwl_priv *priv, const char *name,
 		FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_ENDTFD);
 
 	IWL_DEBUG_FW(priv, "%s uCode section being loaded...\n", name);
-	ret = wait_event_interruptible_timeout(priv->wait_command_queue,
-					priv->ucode_write_complete, 5 * HZ);
-	if (ret == -ERESTARTSYS) {
-		IWL_ERR(priv, "Could not load the %s uCode section due "
-			"to interrupt\n", name);
-		return ret;
-	}
+	ret = wait_event_timeout(priv->wait_command_queue,
+				 priv->ucode_write_complete, 5 * HZ);
 	if (!ret) {
 		IWL_ERR(priv, "Could not load the %s uCode section\n",
 			name);
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn.c b/drivers/net/wireless/iwlwifi/iwl-agn.c
index f9c3cd95d614..f473c019c64a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn.c
@@ -2793,6 +2793,17 @@ static int iwlagn_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		return -EOPNOTSUPP;
 	}
 
+	switch (key->cipher) {
+	case WLAN_CIPHER_SUITE_TKIP:
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
+		/* fall through */
+	case WLAN_CIPHER_SUITE_CCMP:
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+		break;
+	default:
+		break;
+	}
+
 	/*
 	 * We could program these keys into the hardware as well, but we
 	 * don't expect much multicast traffic in IBSS and having keys
@@ -3075,21 +3086,9 @@ static void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
 
 	/* Configure HT40 channels */
 	ctx->ht.enabled = conf_is_ht(conf);
-	if (ctx->ht.enabled) {
-		if (conf_is_ht40_minus(conf)) {
-			ctx->ht.extension_chan_offset =
-				IEEE80211_HT_PARAM_CHA_SEC_BELOW;
-			ctx->ht.is_40mhz = true;
-		} else if (conf_is_ht40_plus(conf)) {
-			ctx->ht.extension_chan_offset =
-				IEEE80211_HT_PARAM_CHA_SEC_ABOVE;
-			ctx->ht.is_40mhz = true;
-		} else {
-			ctx->ht.extension_chan_offset =
-				IEEE80211_HT_PARAM_CHA_SEC_NONE;
-			ctx->ht.is_40mhz = false;
-		}
-	} else
+	if (ctx->ht.enabled)
+		iwlagn_config_ht40(conf, ctx);
+	else
 		ctx->ht.is_40mhz = false;
 
 	if ((le16_to_cpu(ctx->staging.channel) != ch))
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn.h b/drivers/net/wireless/iwlwifi/iwl-agn.h
index d941c4c98e4b..e172f6baad35 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn.h
+++ b/drivers/net/wireless/iwlwifi/iwl-agn.h
@@ -135,6 +135,8 @@ void iwlagn_bss_info_changed(struct ieee80211_hw *hw,
 			     struct ieee80211_vif *vif,
 			     struct ieee80211_bss_conf *bss_conf,
 			     u32 changes);
+void iwlagn_config_ht40(struct ieee80211_conf *conf,
+			struct iwl_rxon_context *ctx);
 
 /* uCode */
 void iwlagn_rx_calib_result(struct iwl_priv *priv,
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.c b/drivers/net/wireless/iwlwifi/iwl-core.c
index cf376f62b2f6..d652778253a4 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.c
+++ b/drivers/net/wireless/iwlwifi/iwl-core.c
@@ -867,7 +867,7 @@ void iwlagn_fw_error(struct iwl_priv *priv, bool ondemand)
 	 * commands by clearing the ready bit */
 	clear_bit(STATUS_READY, &priv->status);
 
-	wake_up_interruptible(&priv->wait_command_queue);
+	wake_up(&priv->wait_command_queue);
 
 	if (!ondemand) {
 		/*
@@ -918,7 +918,7 @@ void iwl_irq_handle_error(struct iwl_priv *priv)
 		 */
 		clear_bit(STATUS_READY, &priv->status);
 		clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
-		wake_up_interruptible(&priv->wait_command_queue);
+		wake_up(&priv->wait_command_queue);
 		IWL_ERR(priv, "RF is used by WiMAX\n");
 		return;
 	}
diff --git a/drivers/net/wireless/iwlwifi/iwl-pci.c b/drivers/net/wireless/iwlwifi/iwl-pci.c
index 2fdbffa079c1..32d64e71861c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-pci.c
+++ b/drivers/net/wireless/iwlwifi/iwl-pci.c
@@ -442,10 +442,9 @@ static int iwl_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	pci_write_config_byte(pdev, PCI_CFG_RETRY_TIMEOUT, 0x00);
 
 	err = pci_enable_msi(pdev);
-	if (err) {
-		dev_printk(KERN_ERR, &pdev->dev, "pci_enable_msi failed");
-		goto out_iounmap;
-	}
+	if (err)
+		dev_printk(KERN_ERR, &pdev->dev,
+			"pci_enable_msi failed(0X%x)", err);
 
 	/* TODO: Move this away, not needed if not MSI */
 	/* enable rfkill interrupt: hw bug w/a */
@@ -466,7 +465,6 @@ static int iwl_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 out_disable_msi:
 	pci_disable_msi(pdev);
-out_iounmap:
 	pci_iounmap(pdev, pci_bus->hw_base);
 out_pci_release_regions:
 	pci_set_drvdata(pdev, NULL);
diff --git a/drivers/net/wireless/iwlwifi/iwl-rx.c b/drivers/net/wireless/iwlwifi/iwl-rx.c
index 8e314003b63a..732f01b565de 100644
--- a/drivers/net/wireless/iwlwifi/iwl-rx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-rx.c
@@ -561,7 +561,7 @@ static void iwl_rx_card_state_notif(struct iwl_priv *priv,
 		wiphy_rfkill_set_hw_state(priv->hw->wiphy,
 			test_bit(STATUS_RF_KILL_HW, &priv->status));
 	else
-		wake_up_interruptible(&priv->wait_command_queue);
+		wake_up(&priv->wait_command_queue);
 }
 
 static void iwl_rx_missed_beacon_notif(struct iwl_priv *priv,
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans-rx-pcie.c b/drivers/net/wireless/iwlwifi/iwl-trans-rx-pcie.c
index 474860290404..f9f0df0cecbf 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans-rx-pcie.c
+++ b/drivers/net/wireless/iwlwifi/iwl-trans-rx-pcie.c
@@ -671,7 +671,7 @@ void iwl_irq_tasklet(struct iwl_priv *priv)
 		handled |= CSR_INT_BIT_FH_TX;
 		/* Wake up uCode load routine, now that load is complete */
 		priv->ucode_write_complete = 1;
-		wake_up_interruptible(&priv->wait_command_queue);
+		wake_up(&priv->wait_command_queue);
 	}
 
 	if (inta & ~handled) {
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans-tx-pcie.c b/drivers/net/wireless/iwlwifi/iwl-trans-tx-pcie.c
index 222d410c586e..2bf3107be930 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans-tx-pcie.c
+++ b/drivers/net/wireless/iwlwifi/iwl-trans-tx-pcie.c
@@ -790,7 +790,7 @@ void iwl_tx_cmd_complete(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb)
 		clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
 		IWL_DEBUG_INFO(priv, "Clearing HCMD_ACTIVE for command %s\n",
 			       get_cmd_string(cmd->hdr.cmd));
-		wake_up_interruptible(&priv->wait_command_queue);
+		wake_up(&priv->wait_command_queue);
 	}
 
 	meta->flags = 0;
@@ -957,7 +957,7 @@ static int iwl_send_cmd_sync(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
 		return ret;
 	}
 
-	ret = wait_event_interruptible_timeout(priv->wait_command_queue,
+	ret = wait_event_timeout(priv->wait_command_queue,
 			!test_bit(STATUS_HCMD_ACTIVE, &priv->status),
 			HOST_COMPLETE_TIMEOUT);
 	if (!ret) {
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans.c b/drivers/net/wireless/iwlwifi/iwl-trans.c
index 41f0de914008..32eb4fe04328 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans.c
+++ b/drivers/net/wireless/iwlwifi/iwl-trans.c
@@ -1068,9 +1068,7 @@ static int iwl_trans_tx(struct iwl_priv *priv, struct sk_buff *skb,
 	iwl_print_hex_dump(priv, IWL_DL_TX, (u8 *)tx_cmd->hdr, hdr_len);
 
 	/* Set up entry for this TFD in Tx byte-count array */
-	if (ampdu)
-		iwl_trans_txq_update_byte_cnt_tbl(priv, txq,
-					       le16_to_cpu(tx_cmd->len));
+	iwl_trans_txq_update_byte_cnt_tbl(priv, txq, le16_to_cpu(tx_cmd->len));
 
 	dma_sync_single_for_device(priv->bus->dev, txcmd_phys, firstlen,
 			DMA_BIDIRECTIONAL);
diff --git a/drivers/net/wireless/libertas/if_spi.c b/drivers/net/wireless/libertas/if_spi.c
index e0286cfbc91d..7be8b051aabc 100644
--- a/drivers/net/wireless/libertas/if_spi.c
+++ b/drivers/net/wireless/libertas/if_spi.c
@@ -999,6 +999,7 @@ static int if_spi_host_to_card(struct lbs_private *priv,
 		spin_unlock_irqrestore(&card->buffer_lock, flags);
 		break;
 	default:
+		kfree(packet);
 		netdev_err(priv->dev, "can't transfer buffer of type %d\n",
 			   type);
 		err = -EINVAL;
diff --git a/drivers/net/wireless/p54/p54spi.c b/drivers/net/wireless/p54/p54spi.c
index 6d9204fef90b..b33ceb1c066a 100644
--- a/drivers/net/wireless/p54/p54spi.c
+++ b/drivers/net/wireless/p54/p54spi.c
@@ -589,8 +589,6 @@ static void p54spi_op_stop(struct ieee80211_hw *dev)
 
 	WARN_ON(priv->fw_state != FW_STATE_READY);
 
-	cancel_work_sync(&priv->work);
-
 	p54spi_power_off(priv);
 	spin_lock_irqsave(&priv->tx_lock, flags);
 	INIT_LIST_HEAD(&priv->tx_pending);
@@ -598,6 +596,8 @@ static void p54spi_op_stop(struct ieee80211_hw *dev)
 
 	priv->fw_state = FW_STATE_OFF;
 	mutex_unlock(&priv->mutex);
+
+	cancel_work_sync(&priv->work);
 }
 
 static int __devinit p54spi_probe(struct spi_device *spi)
@@ -657,6 +657,7 @@ static int __devinit p54spi_probe(struct spi_device *spi)
 	init_completion(&priv->fw_comp);
 	INIT_LIST_HEAD(&priv->tx_pending);
 	mutex_init(&priv->mutex);
+	spin_lock_init(&priv->tx_lock);
 	SET_IEEE80211_DEV(hw, &spi->dev);
 	priv->common.open = p54spi_op_start;
 	priv->common.stop = p54spi_op_stop;
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index 0019dfd8fb01..c6ad97f6daa0 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -3699,7 +3699,7 @@ static void rt2800_efuse_read(struct rt2x00_dev *rt2x00dev, unsigned int i)
 	/* Apparently the data is read from end to start */
 	rt2800_register_read_lock(rt2x00dev, EFUSE_DATA3, &reg);
 	/* The returned value is in CPU order, but eeprom is le */
-	rt2x00dev->eeprom[i] = cpu_to_le32(reg);
+	*(u32 *)&rt2x00dev->eeprom[i] = cpu_to_le32(reg);
 	rt2800_register_read_lock(rt2x00dev, EFUSE_DATA2, &reg);
 	*(u32 *)&rt2x00dev->eeprom[i + 2] = cpu_to_le32(reg);
 	rt2800_register_read_lock(rt2x00dev, EFUSE_DATA1, &reg);
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index dbf501ca317f..05f97695838e 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -970,6 +970,7 @@ static struct usb_device_id rt2800usb_device_table[] = {
 	{ USB_DEVICE(0x13b1, 0x0031) },
 	{ USB_DEVICE(0x1737, 0x0070) },
 	{ USB_DEVICE(0x1737, 0x0071) },
+	{ USB_DEVICE(0x1737, 0x0077) },
 	/* Logitec */
 	{ USB_DEVICE(0x0789, 0x0162) },
 	{ USB_DEVICE(0x0789, 0x0163) },
@@ -1165,7 +1166,6 @@ static struct usb_device_id rt2800usb_device_table[] = {
 	{ USB_DEVICE(0x1740, 0x0605) },
 	{ USB_DEVICE(0x1740, 0x0615) },
 	/* Linksys */
-	{ USB_DEVICE(0x1737, 0x0077) },
 	{ USB_DEVICE(0x1737, 0x0078) },
 	/* Logitec */
 	{ USB_DEVICE(0x0789, 0x0168) },
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index f82bfeb79ebb..0415e470ddd5 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -923,6 +923,7 @@ struct rt2x00_dev {
 	 * Powersaving work
 	 */
 	struct delayed_work autowakeup_work;
+	struct work_struct sleep_work;
 
 	/*
 	 * Data queue arrays for RX, TX, Beacon and ATIM.
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index 0955c941317f..9d05f871a987 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -449,6 +449,23 @@ static u8 *rt2x00lib_find_ie(u8 *data, unsigned int len, u8 ie)
 	return NULL;
 }
 
+static void rt2x00lib_sleep(struct work_struct *work)
+{
+	struct rt2x00_dev *rt2x00dev =
+	    container_of(work, struct rt2x00_dev, sleep_work);
+
+	if (!test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags))
+		return;
+
+	/*
+	 * Check again is powersaving is enabled, to prevent races from delayed
+	 * work execution.
+	 */
+	if (!test_bit(CONFIG_POWERSAVING, &rt2x00dev->flags))
+		rt2x00lib_config(rt2x00dev, &rt2x00dev->hw->conf,
+				 IEEE80211_CONF_CHANGE_PS);
+}
+
 static void rt2x00lib_rxdone_check_ps(struct rt2x00_dev *rt2x00dev,
 				      struct sk_buff *skb,
 				      struct rxdone_entry_desc *rxdesc)
@@ -496,8 +513,7 @@ static void rt2x00lib_rxdone_check_ps(struct rt2x00_dev *rt2x00dev,
 	cam |= (tim_ie->bitmap_ctrl & 0x01);
 
 	if (!cam && !test_bit(CONFIG_POWERSAVING, &rt2x00dev->flags))
-		rt2x00lib_config(rt2x00dev, &rt2x00dev->hw->conf,
-				 IEEE80211_CONF_CHANGE_PS);
+		queue_work(rt2x00dev->workqueue, &rt2x00dev->sleep_work);
 }
 
 static int rt2x00lib_rxdone_read_signal(struct rt2x00_dev *rt2x00dev,
@@ -1121,6 +1137,7 @@ int rt2x00lib_probe_dev(struct rt2x00_dev *rt2x00dev)
 
 	INIT_WORK(&rt2x00dev->intf_work, rt2x00lib_intf_scheduled);
 	INIT_DELAYED_WORK(&rt2x00dev->autowakeup_work, rt2x00lib_autowakeup);
+	INIT_WORK(&rt2x00dev->sleep_work, rt2x00lib_sleep);
 
 	/*
 	 * Let the driver probe the device to detect the capabilities.
@@ -1177,6 +1194,7 @@ void rt2x00lib_remove_dev(struct rt2x00_dev *rt2x00dev)
 	 */
 	cancel_work_sync(&rt2x00dev->intf_work);
 	cancel_delayed_work_sync(&rt2x00dev->autowakeup_work);
+	cancel_work_sync(&rt2x00dev->sleep_work);
 	if (rt2x00_is_usb(rt2x00dev)) {
 		del_timer_sync(&rt2x00dev->txstatus_timer);
 		cancel_work_sync(&rt2x00dev->rxdone_work);
diff --git a/drivers/net/wireless/rtlwifi/ps.c b/drivers/net/wireless/rtlwifi/ps.c
index a693feffbe72..0b04b2e7b597 100644
--- a/drivers/net/wireless/rtlwifi/ps.c
+++ b/drivers/net/wireless/rtlwifi/ps.c
@@ -394,7 +394,7 @@ void rtl_lps_enter(struct ieee80211_hw *hw)
 	if (mac->link_state != MAC80211_LINKED)
 		return;
 
-	spin_lock(&rtlpriv->locks.lps_lock);
+	spin_lock_irq(&rtlpriv->locks.lps_lock);
 
 	/* Idle for a while if we connect to AP a while ago. */
 	if (mac->cnt_after_linked >= 2) {
@@ -406,7 +406,7 @@ void rtl_lps_enter(struct ieee80211_hw *hw)
 		}
 	}
 
-	spin_unlock(&rtlpriv->locks.lps_lock);
+	spin_unlock_irq(&rtlpriv->locks.lps_lock);
 }
 
 /*Leave the leisure power save mode.*/
@@ -415,8 +415,9 @@ void rtl_lps_leave(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	unsigned long flags;
 
-	spin_lock(&rtlpriv->locks.lps_lock);
+	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flags);
 
 	if (ppsc->fwctrl_lps) {
 		if (ppsc->dot11_psmode != EACTIVE) {
@@ -437,7 +438,7 @@ void rtl_lps_leave(struct ieee80211_hw *hw)
 			rtl_lps_set_psmode(hw, EACTIVE);
 		}
 	}
-	spin_unlock(&rtlpriv->locks.lps_lock);
+	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flags);
 }
 
 /* For sw LPS*/
@@ -538,9 +539,9 @@ void rtl_swlps_rf_awake(struct ieee80211_hw *hw)
 		RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
 	}
 
-	spin_lock(&rtlpriv->locks.lps_lock);
+	spin_lock_irq(&rtlpriv->locks.lps_lock);
 	rtl_ps_set_rf_state(hw, ERFON, RF_CHANGE_BY_PS);
-	spin_unlock(&rtlpriv->locks.lps_lock);
+	spin_unlock_irq(&rtlpriv->locks.lps_lock);
 }
 
 void rtl_swlps_rfon_wq_callback(void *data)
@@ -573,9 +574,9 @@ void rtl_swlps_rf_sleep(struct ieee80211_hw *hw)
 	if (rtlpriv->link_info.busytraffic)
 		return;
 
-	spin_lock(&rtlpriv->locks.lps_lock);
+	spin_lock_irq(&rtlpriv->locks.lps_lock);
 	rtl_ps_set_rf_state(hw, ERFSLEEP, RF_CHANGE_BY_PS);
-	spin_unlock(&rtlpriv->locks.lps_lock);
+	spin_unlock_irq(&rtlpriv->locks.lps_lock);
 
 	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
 		!RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c b/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c
index 592a10ac5929..3b585aadabfc 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c
@@ -569,7 +569,7 @@ static bool _rtl92ce_phy_set_rf_power_state(struct ieee80211_hw *hw,
 		}
 	case ERFSLEEP:{
 			if (ppsc->rfpwr_state == ERFOFF)
-				break;
+				return false;
 			for (queue_id = 0, i = 0;
 			     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
 				ring = &pcipriv->dev.tx_ring[queue_id];
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c b/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c
index 72852900df84..e49cf2244c75 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c
@@ -548,7 +548,7 @@ static bool _rtl92cu_phy_set_rf_power_state(struct ieee80211_hw *hw,
 		break;
 	case ERFSLEEP:
 		if (ppsc->rfpwr_state == ERFOFF)
-			break;
+			return false;
 		for (queue_id = 0, i = 0;
 		     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
 			ring = &pcipriv->dev.tx_ring[queue_id];
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/phy.c b/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
index 3ac7af1c5509..0883349e1c83 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
@@ -3374,7 +3374,7 @@ bool rtl92d_phy_set_rf_power_state(struct ieee80211_hw *hw,
 		break;
 	case ERFSLEEP:
 		if (ppsc->rfpwr_state == ERFOFF)
-			break;
+			return false;
 
 		for (queue_id = 0, i = 0;
 		     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/fw.c b/drivers/net/wireless/rtlwifi/rtl8192se/fw.c
index 6f91a148c222..3fda6b1dcf46 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/fw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/fw.c
@@ -196,6 +196,8 @@ static bool _rtl92s_firmware_downloadcode(struct ieee80211_hw *hw,
 		/* Allocate skb buffer to contain firmware */
 		/* info and tx descriptor info. */
 		skb = dev_alloc_skb(frag_length);
+		if (!skb)
+			return false;
 		skb_reserve(skb, extra_descoffset);
 		seg_ptr = (u8 *)skb_put(skb, (u32)(frag_length -
 					extra_descoffset));
@@ -573,6 +575,8 @@ static bool _rtl92s_firmware_set_h2c_cmd(struct ieee80211_hw *hw, u8 h2c_cmd,
 
 	len = _rtl92s_get_h2c_cmdlen(MAX_TRANSMIT_BUFFER_SIZE, 1, &cmd_len);
 	skb = dev_alloc_skb(len);
+	if (!skb)
+		return false;
 	cb_desc = (struct rtl_tcb_desc *)(skb->cb);
 	cb_desc->queue_index = TXCMD_QUEUE;
 	cb_desc->cmd_or_init = DESC_PACKET_TYPE_NORMAL;
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/phy.c b/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
index f27171af979c..f10ac1ad9087 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
@@ -602,7 +602,7 @@ bool rtl92s_phy_set_rf_power_state(struct ieee80211_hw *hw,
 		}
 	case ERFSLEEP:
 			if (ppsc->rfpwr_state == ERFOFF)
-				break;
+				return false;
 
 			for (queue_id = 0, i = 0;
 			     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
diff --git a/drivers/net/wireless/sd8797/Kconfig b/drivers/net/wireless/sd8797/Kconfig
new file mode 100644
index 000000000000..d823c85166c8
--- /dev/null
+++ b/drivers/net/wireless/sd8797/Kconfig
@@ -0,0 +1,39 @@
+config SD8797
+	tristate "Marvel 8797 wireless cards support"
+	depends on MMC
+	default n
+	---help---
+	  This module adds support for wireless adapters based on
+	  Marvel 8797 chipset.
+
+config STA_SUPPORT
+	depends on SD8797
+	bool "WLAN station support"
+	default y
+
+config UAP_SUPPORT
+	depends on SD8797
+	bool "Soft AP Support"
+	default y
+
+config WIFI_DIRECT_SUPPORT
+	depends on SD8797
+	bool "WiFi Direct Support"
+	default n
+
+config WIFI_DISPLAY_SUPPORT
+	depends on SD8797
+	bool "WiFi Display Support"
+	default n
+
+config STA_WEXT
+	depends on SD8797
+	depends on !CFG80211
+	bool "station wext support"
+	default n
+
+config UAP_WEXT
+	depends on SD8797
+	depends on !CFG80211
+	bool "UAP wext support"
+	default n
diff --git a/drivers/net/wireless/sd8797/Makefile b/drivers/net/wireless/sd8797/Makefile
new file mode 100644
index 000000000000..3ef70380dd82
--- /dev/null
+++ b/drivers/net/wireless/sd8797/Makefile
@@ -0,0 +1,176 @@
+# File: Makefile
+#
+# Copyright (C) 2008-2012, Marvell International Ltd. 
+#
+# This software file (the "File") is distributed by Marvell International 
+# Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+# (the "License").  You may use, redistribute and/or modify this File in 
+# accordance with the terms and conditions of the License, a copy of which 
+# is available by writing to the Free Software Foundation, Inc.,
+# 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+# worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+#
+# A copy of the GPL is available in file gpl-2.0.txt accompanying in this 
+# deliverables.
+#
+# THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+# IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+# ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+# this warranty disclaimer.
+
+# Enable CFG80211 for STA
+ifeq ($(CONFIG_CFG80211),y)
+CONFIG_STA_CFG80211=y
+else
+ifeq ($(CONFIG_CFG80211),m)
+CONFIG_STA_CFG80211=y
+else
+CONFIG_STA_CFG80211=n
+endif
+endif
+
+# Enable CFG80211 for uAP
+ifeq ($(CONFIG_CFG80211),y)
+CONFIG_UAP_CFG80211=y
+else
+ifeq ($(CONFIG_CFG80211),m)
+CONFIG_UAP_CFG80211=y
+else
+CONFIG_UAP_CFG80211=n
+endif
+endif
+
+#############################################################################
+# Compiler Flags
+#############################################################################
+
+EXTRA_CFLAGS += -Idrivers/net/wireless/sd8797/mlan
+EXTRA_CFLAGS += -DLINUX
+EXTRA_CFLAGS += -DFPNUM='"69"'
+EXTRA_CFLAGS += -DDEBUG_LEVEL1
+EXTRA_CFLAGS += -DPROC_DEBUG
+EXTRA_CFLAGS += -DSDIO_MULTI_PORT_TX_AGGR
+EXTRA_CFLAGS += -DSDIO_MULTI_PORT_RX_AGGR
+EXTRA_CFLAGS += -DSDIO_SUSPEND_RESUME
+EXTRA_CFLAGS += -DMMC_PM_KEEP_POWER
+EXTRA_CFLAGS += -DDFS_TESTING_SUPPORT
+EXTRA_CFLAGS += -DMFG_CMD_SUPPORT
+
+ifeq ($(CONFIG_64BIT), y)
+	EXTRA_CFLAGS += -DMLAN_64BIT
+endif
+
+ifeq ($(CONFIG_STA_SUPPORT),y)
+	EXTRA_CFLAGS += -DSTA_SUPPORT
+	EXTRA_CFLAGS += -DREASSOCIATION
+ifeq ($(CONFIG_STA_WEXT),y)
+	EXTRA_CFLAGS += -DSTA_WEXT
+endif
+ifeq ($(CONFIG_STA_CFG80211),y)
+	EXTRA_CFLAGS += -DSTA_CFG80211
+endif
+else
+CONFIG_WIFI_DIRECT_SUPPORT=n
+CONFIG_WIFI_DISPLAY_SUPPORT=n
+CONFIG_STA_WEXT=n
+CONFIG_STA_CFG80211=n
+endif
+
+ifeq ($(CONFIG_UAP_SUPPORT),y)
+	EXTRA_CFLAGS += -DUAP_SUPPORT
+ifeq ($(CONFIG_UAP_WEXT),y)
+	EXTRA_CFLAGS += -DUAP_WEXT
+endif
+ifeq ($(CONFIG_UAP_CFG80211),y)
+	EXTRA_CFLAGS += -DUAP_CFG80211
+endif
+else
+CONFIG_WIFI_DIRECT_SUPPORT=n
+CONFIG_WIFI_DISPLAY_SUPPORT=n
+CONFIG_UAP_WEXT=n
+CONFIG_UAP_CFG80211=n
+endif
+
+ifeq ($(CONFIG_WIFI_DIRECT_SUPPORT),y)
+	EXTRA_CFLAGS += -DWIFI_DIRECT_SUPPORT
+endif
+ifeq ($(CONFIG_WIFI_DISPLAY_SUPPORT),y)
+	EXTRA_CFLAGS += -DWIFI_DISPLAY_SUPPORT
+endif
+
+ifeq ($(CONFIG_BIG_ENDIAN),y)
+	EXTRA_CFLAGS += -DBIG_ENDIAN_SUPPORT
+endif
+
+#############################################################################
+# Make Targets
+#############################################################################
+
+MOALOBJS =	mlinux/moal_main.o \
+		mlinux/moal_ioctl.o \
+		mlinux/moal_shim.o \
+		mlinux/moal_eth_ioctl.o
+
+MLANOBJS =	mlan/mlan_shim.o mlan/mlan_init.o \
+		mlan/mlan_txrx.o \
+		mlan/mlan_cmdevt.o mlan/mlan_misc.o \
+		mlan/mlan_cfp.o \
+		mlan/mlan_module.o
+
+MLANOBJS += mlan/mlan_wmm.o
+MLANOBJS += mlan/mlan_sdio.o
+MLANOBJS += mlan/mlan_11n_aggr.o
+MLANOBJS += mlan/mlan_11n_rxreorder.o
+MLANOBJS += mlan/mlan_11n.o
+MLANOBJS += mlan/mlan_11d.o
+MLANOBJS += mlan/mlan_11h.o
+ifeq ($(CONFIG_STA_SUPPORT),y)
+MLANOBJS += mlan/mlan_meas.o
+MLANOBJS += mlan/mlan_scan.o \
+			mlan/mlan_sta_ioctl.o \
+			mlan/mlan_sta_rx.o \
+			mlan/mlan_sta_tx.o \
+			mlan/mlan_sta_event.o \
+			mlan/mlan_sta_cmd.o \
+			mlan/mlan_sta_cmdresp.o \
+			mlan/mlan_join.o
+ifeq ($(CONFIG_STA_WEXT),y)
+MOALOBJS += mlinux/moal_priv.o \
+            mlinux/moal_wext.o
+endif
+endif
+ifeq ($(CONFIG_UAP_SUPPORT),y)
+MLANOBJS += mlan/mlan_uap_ioctl.o
+MLANOBJS += mlan/mlan_uap_cmdevent.o
+MLANOBJS += mlan/mlan_uap_txrx.o
+MOALOBJS += mlinux/moal_uap.o
+ifeq ($(CONFIG_UAP_WEXT),y)
+MOALOBJS += mlinux/moal_uap_priv.o
+MOALOBJS += mlinux/moal_uap_wext.o
+endif
+ifeq ($(CONFIG_STA_WEXT),y)
+MOALOBJS += mlinux/moal_wext.o
+endif
+endif
+ifeq ($(CONFIG_STA_CFG80211),y)
+MOALOBJS += mlinux/moal_cfg80211.o
+MOALOBJS += mlinux/moal_sta_cfg80211.o
+endif
+ifeq ($(CONFIG_UAP_CFG80211),y)
+MOALOBJS += mlinux/moal_cfg80211.o
+MOALOBJS += mlinux/moal_uap_cfg80211.o
+endif
+
+ifdef CONFIG_PROC_FS
+MOALOBJS += mlinux/moal_proc.o
+MOALOBJS += mlinux/moal_debug.o
+endif
+
+obj-m := mlan.o
+mlan-objs := $(MLANOBJS)
+MOALOBJS += mlinux/moal_sdio_mmc.o
+obj-m += sd8xxx.o
+sd8xxx-objs := $(MOALOBJS)
+
+###############################################################
+# End of file
diff --git a/drivers/net/wireless/sd8797/mlan/mlan.h b/drivers/net/wireless/sd8797/mlan/mlan.h
new file mode 100644
index 000000000000..a341e1547099
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan.h
@@ -0,0 +1,35 @@
+/** @file mlan.h
+ *
+ *  @brief This file declares all APIs that will be called from MOAL module.
+ *  It also defines the data structures used for APIs between MLAN and MOAL.
+ *
+ * Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available by writing to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    10/13/2008: initial version
+    11/07/2008: split mlan.h into mlan_decl.h & mlan_ioctl.h
+******************************************************/
+
+#ifndef _MLAN_H_
+#define _MLAN_H_
+
+#include "mlan_decl.h"
+#include "mlan_ioctl.h"
+#include "mlan_ieee.h"
+
+#endif /* !_MLAN_H_ */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_11d.c b/drivers/net/wireless/sd8797/mlan/mlan_11d.c
new file mode 100644
index 000000000000..fdc0d993e381
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_11d.c
@@ -0,0 +1,1515 @@
+/** @file mlan_11d.c
+ *
+ *  @brief This file contains functions for 802.11D.
+ *
+ *  Copyright (C) 2008-2012, Marvell International Ltd.
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ *
+ */
+/********************************************************
+Change log:
+    10/21/2008: initial version
+********************************************************/
+
+#include "mlan.h"
+#include "mlan_join.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_11h.h"
+
+/********************************************************
+                Local Variables
+********************************************************/
+
+/** Region code mapping */
+typedef struct _region_code_mapping
+{
+    /** Region */
+    t_u8 region[COUNTRY_CODE_LEN];
+    /** Code */
+    t_u8 code;
+} region_code_mapping_t;
+
+/** Region code mapping table */
+static region_code_mapping_t region_code_mapping[] = {
+    {"US ", 0x10},              /* US FCC */
+    {"CA ", 0x20},              /* IC Canada */
+    {"SG ", 0x10},              /* Singapore */
+    {"EU ", 0x30},              /* ETSI */
+    {"AU ", 0x30},              /* Australia */
+    {"KR ", 0x30},              /* Republic Of Korea */
+    {"FR ", 0x32},              /* France */
+    {"JP ", 0x40},              /* Japan */
+    {"JP ", 0x41},              /* Japan */
+    {"CN ", 0x50},              /* China */
+    {"JP ", 0xFE},              /* Japan */
+    {"JP ", 0xFF},              /* Japan special */
+};
+
+#ifdef STA_SUPPORT
+/** Default Tx power */
+#define TX_PWR_DEFAULT  10
+
+/** Universal region code */
+#define UNIVERSAL_REGION_CODE   0xff
+
+/* Following two structures define the supported channels */
+/** Channels for 802.11b/g */
+static chan_freq_power_t channel_freq_power_UN_BG[] = {
+    {1, 2412, TX_PWR_DEFAULT},
+    {2, 2417, TX_PWR_DEFAULT},
+    {3, 2422, TX_PWR_DEFAULT},
+    {4, 2427, TX_PWR_DEFAULT},
+    {5, 2432, TX_PWR_DEFAULT},
+    {6, 2437, TX_PWR_DEFAULT},
+    {7, 2442, TX_PWR_DEFAULT},
+    {8, 2447, TX_PWR_DEFAULT},
+    {9, 2452, TX_PWR_DEFAULT},
+    {10, 2457, TX_PWR_DEFAULT},
+    {11, 2462, TX_PWR_DEFAULT},
+    {12, 2467, TX_PWR_DEFAULT},
+    {13, 2472, TX_PWR_DEFAULT},
+    {14, 2484, TX_PWR_DEFAULT}
+};
+
+/** Channels for 802.11a/j */
+static chan_freq_power_t channel_freq_power_UN_AJ[] = {
+    {8, 5040, TX_PWR_DEFAULT},
+    {12, 5060, TX_PWR_DEFAULT},
+    {16, 5080, TX_PWR_DEFAULT},
+    {34, 5170, TX_PWR_DEFAULT},
+    {38, 5190, TX_PWR_DEFAULT},
+    {42, 5210, TX_PWR_DEFAULT},
+    {46, 5230, TX_PWR_DEFAULT},
+    {36, 5180, TX_PWR_DEFAULT},
+    {40, 5200, TX_PWR_DEFAULT},
+    {44, 5220, TX_PWR_DEFAULT},
+    {48, 5240, TX_PWR_DEFAULT},
+    {52, 5260, TX_PWR_DEFAULT},
+    {56, 5280, TX_PWR_DEFAULT},
+    {60, 5300, TX_PWR_DEFAULT},
+    {64, 5320, TX_PWR_DEFAULT},
+    {100, 5500, TX_PWR_DEFAULT},
+    {104, 5520, TX_PWR_DEFAULT},
+    {108, 5540, TX_PWR_DEFAULT},
+    {112, 5560, TX_PWR_DEFAULT},
+    {116, 5580, TX_PWR_DEFAULT},
+    {120, 5600, TX_PWR_DEFAULT},
+    {124, 5620, TX_PWR_DEFAULT},
+    {128, 5640, TX_PWR_DEFAULT},
+    {132, 5660, TX_PWR_DEFAULT},
+    {136, 5680, TX_PWR_DEFAULT},
+    {140, 5700, TX_PWR_DEFAULT},
+    {149, 5745, TX_PWR_DEFAULT},
+    {153, 5765, TX_PWR_DEFAULT},
+    {157, 5785, TX_PWR_DEFAULT},
+    {161, 5805, TX_PWR_DEFAULT},
+    {165, 5825, TX_PWR_DEFAULT},
+/*  {240, 4920, TX_PWR_DEFAULT},
+    {244, 4940, TX_PWR_DEFAULT},
+    {248, 4960, TX_PWR_DEFAULT},
+    {252, 4980, TX_PWR_DEFAULT},
+channels for 11J JP 10M channel gap */
+};
+#endif /* STA_SUPPORT */
+
+/********************************************************
+                Global Variables
+********************************************************/
+
+/********************************************************
+                Local Functions
+********************************************************/
+
+/**
+ *  @brief This function converts region string to integer code
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param region       Region string
+ *  @param code         [output] Region code
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_11d_region_2_code(pmlan_adapter pmadapter, t_u8 * region, OUT t_u8 * code)
+{
+    t_u8 i;
+    t_u8 size = sizeof(region_code_mapping) / sizeof(region_code_mapping_t);
+
+    ENTER();
+
+    /* Look for code in mapping table */
+    for (i = 0; i < size; i++) {
+        if (!memcmp(pmadapter, region_code_mapping[i].region,
+                    region, COUNTRY_CODE_LEN)) {
+            *code = region_code_mapping[i].code;
+            LEAVE();
+            return MLAN_STATUS_SUCCESS;
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_FAILURE;
+}
+
+#ifdef STA_SUPPORT
+/**
+ *  @brief This function converts integer code to region string
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param code         Region code
+ *
+ *  @return             Region string
+ */
+static t_u8 *
+wlan_11d_code_2_region(pmlan_adapter pmadapter, t_u8 code)
+{
+    t_u8 i;
+    t_u8 size = sizeof(region_code_mapping) / sizeof(region_code_mapping_t);
+
+    ENTER();
+
+    /* Look for code in mapping table */
+    for (i = 0; i < size; i++) {
+        if (region_code_mapping[i].code == code) {
+            LEAVE();
+            return (region_code_mapping[i].region);
+        }
+    }
+
+    LEAVE();
+    /* Default is US */
+    return (region_code_mapping[0].region);
+}
+
+/**
+ *  @brief This function Checks if channel txpwr is learned from AP/IBSS
+ *
+ *  @param pmadapter            A pointer to mlan_adapter structure
+ *  @param band                 Band number
+ *  @param chan                 Channel number
+ *  @param parsed_region_chan   Pointer to parsed_region_chan_11d_t
+ *
+ *  @return                     MTRUE or MFALSE
+ */
+static t_u8
+wlan_11d_channel_known(pmlan_adapter pmadapter,
+                       t_u8 band,
+                       t_u8 chan, parsed_region_chan_11d_t * parsed_region_chan)
+{
+    chan_power_11d_t *pchan_pwr = parsed_region_chan->chan_pwr;
+    t_u8 no_of_chan = parsed_region_chan->no_of_chan;
+    t_u8 i = 0;
+    t_u8 ret = MFALSE;
+    mlan_private *pmpriv;
+
+    ENTER();
+
+    HEXDUMP("11D: parsed_region_chan", (t_u8 *) pchan_pwr,
+            sizeof(chan_power_11d_t) * no_of_chan);
+
+    /* Search channel */
+    for (i = 0; i < no_of_chan; i++) {
+        if (chan == pchan_pwr[i].chan && band == pchan_pwr[i].band) {
+            PRINTM(MINFO, "11D: Found channel:%d (band:%d)\n", chan, band);
+            ret = MTRUE;
+
+            if (band & BAND_A) {
+                /* If chan is a DFS channel, we need to see an AP on it */
+                if ((pmpriv = wlan_get_priv(pmadapter, MLAN_BSS_ROLE_STA))
+                    && wlan_11h_radar_detect_required(pmpriv, chan)) {
+                    PRINTM(MINFO, "11H: DFS channel %d, and ap_seen=%d\n",
+                           chan, pchan_pwr[i].ap_seen);
+                    ret = pchan_pwr[i].ap_seen;
+                }
+            }
+
+            LEAVE();
+            return ret;
+        }
+    }
+
+    PRINTM(MINFO, "11D: Could not find channel:%d (band:%d)\n", chan, band);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function generates parsed_region_chan from Domain Info
+ *           learned from AP/IBSS
+ *
+ *  @param pmadapter            Pointer to mlan_adapter structure
+ *  @param region_chan          Pointer to region_chan_t
+ *  @param parsed_region_chan   Pointer to parsed_region_chan_11d_t
+ *
+ *  @return                     N/A
+ */
+static t_void
+wlan_11d_generate_parsed_region_chan(pmlan_adapter pmadapter,
+                                     region_chan_t * region_chan,
+                                     parsed_region_chan_11d_t *
+                                     parsed_region_chan)
+{
+    chan_freq_power_t *cfp;
+    t_u8 i;
+
+    ENTER();
+
+    /* Region channel must be provided */
+    if (!region_chan) {
+        PRINTM(MWARN, "11D: region_chan is MNULL\n");
+        LEAVE();
+        return;
+    }
+
+    /* Get channel-frequency-power trio */
+    cfp = region_chan->pcfp;
+    if (!cfp) {
+        PRINTM(MWARN, "11D: cfp equal MNULL\n");
+        LEAVE();
+        return;
+    }
+
+    /* Set channel, band and power */
+    for (i = 0; i < region_chan->num_cfp; i++, cfp++) {
+        parsed_region_chan->chan_pwr[i].chan = (t_u8) cfp->channel;
+        parsed_region_chan->chan_pwr[i].band = region_chan->band;
+        parsed_region_chan->chan_pwr[i].pwr = (t_u8) cfp->max_tx_power;
+        PRINTM(MINFO, "11D: Chan[%d] Band[%d] Pwr[%d]\n",
+               parsed_region_chan->chan_pwr[i].chan,
+               parsed_region_chan->chan_pwr[i].band,
+               parsed_region_chan->chan_pwr[i].pwr);
+    }
+    parsed_region_chan->no_of_chan = region_chan->num_cfp;
+
+    PRINTM(MINFO, "11D: no_of_chan[%d]\n", parsed_region_chan->no_of_chan);
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function generates domain_info from parsed_region_chan
+ *
+ *  @param pmadapter            Pointer to mlan_adapter structure
+ *  @param parsed_region_chan   Pointer to parsed_region_chan_11d_t
+ *
+ *  @return                     MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_11d_generate_domain_info(pmlan_adapter pmadapter,
+                              parsed_region_chan_11d_t * parsed_region_chan)
+{
+    t_u8 no_of_sub_band = 0;
+    t_u8 no_of_chan = parsed_region_chan->no_of_chan;
+    t_u8 no_of_parsed_chan = 0;
+    t_u8 first_chan = 0, next_chan = 0, max_pwr = 0;
+    t_u8 i, flag = MFALSE;
+    wlan_802_11d_domain_reg_t *domain_info = &pmadapter->domain_reg;
+
+    ENTER();
+
+    /* Should be only place that clear domain_reg (besides init) */
+    memset(pmadapter, domain_info, 0, sizeof(wlan_802_11d_domain_reg_t));
+
+    /* Set country code */
+    memcpy(pmadapter, domain_info->country_code,
+           wlan_11d_code_2_region(pmadapter, (t_u8) pmadapter->region_code),
+           COUNTRY_CODE_LEN);
+
+    PRINTM(MINFO, "11D: Number of channel = %d\n", no_of_chan);
+    HEXDUMP("11D: parsed_region_chan", (t_u8 *) parsed_region_chan,
+            sizeof(parsed_region_chan_11d_t));
+
+    /* Set channel and power */
+    for (i = 0; i < no_of_chan; i++) {
+        if (!flag) {
+            flag = MTRUE;
+            next_chan = first_chan = parsed_region_chan->chan_pwr[i].chan;
+            max_pwr = parsed_region_chan->chan_pwr[i].pwr;
+            no_of_parsed_chan = 1;
+            continue;
+        }
+
+        if (parsed_region_chan->chan_pwr[i].chan == next_chan + 1 &&
+            parsed_region_chan->chan_pwr[i].pwr == max_pwr) {
+            next_chan++;
+            no_of_parsed_chan++;
+        } else {
+            domain_info->sub_band[no_of_sub_band].first_chan = first_chan;
+            domain_info->sub_band[no_of_sub_band].no_of_chan =
+                no_of_parsed_chan;
+            domain_info->sub_band[no_of_sub_band].max_tx_pwr = max_pwr;
+            no_of_sub_band++;
+            no_of_parsed_chan = 1;
+            next_chan = first_chan = parsed_region_chan->chan_pwr[i].chan;
+            max_pwr = parsed_region_chan->chan_pwr[i].pwr;
+        }
+    }
+
+    if (flag) {
+        domain_info->sub_band[no_of_sub_band].first_chan = first_chan;
+        domain_info->sub_band[no_of_sub_band].no_of_chan = no_of_parsed_chan;
+        domain_info->sub_band[no_of_sub_band].max_tx_pwr = max_pwr;
+        no_of_sub_band++;
+    }
+    domain_info->no_of_sub_band = no_of_sub_band;
+
+    PRINTM(MINFO, "11D: Number of sub-band =0x%x\n",
+           domain_info->no_of_sub_band);
+    HEXDUMP("11D: domain_info", (t_u8 *) domain_info,
+            COUNTRY_CODE_LEN + 1 +
+            sizeof(IEEEtypes_SubbandSet_t) * no_of_sub_band);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function updates the channel power table with the channel
+ *            present in BSSDescriptor.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pbss_desc    A pointer to BSSDescriptor_t
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_11d_update_chan_pwr_table(mlan_private * pmpriv,
+                               BSSDescriptor_t * pbss_desc)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    parsed_region_chan_11d_t *parsed_region_chan =
+        &pmadapter->parsed_region_chan;
+    t_u16 i;
+    t_u8 tx_power = 0;
+    t_u8 chan;
+
+    ENTER();
+
+    chan = pbss_desc->phy_param_set.ds_param_set.current_chan;
+
+    tx_power = wlan_get_txpwr_of_chan_from_cfp(pmpriv, chan);
+
+    if (!tx_power) {
+        PRINTM(MMSG, "11D: Invalid channel\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    /* Check whether the channel already exists in channel power table of
+       parsed region */
+    for (i = 0; ((i < parsed_region_chan->no_of_chan) &&
+                 (i < MAX_NO_OF_CHAN)); i++) {
+        if (parsed_region_chan->chan_pwr[i].chan == chan
+            && parsed_region_chan->chan_pwr[i].band == pbss_desc->bss_band) {
+            /* Channel already exists, use minimum of existing and tx_power */
+            parsed_region_chan->chan_pwr[i].pwr =
+                MIN(parsed_region_chan->chan_pwr[i].pwr, tx_power);
+            parsed_region_chan->chan_pwr[i].ap_seen = MTRUE;
+            break;
+        }
+    }
+
+    if (i == parsed_region_chan->no_of_chan && i < MAX_NO_OF_CHAN) {
+        /* Channel not found. Update the channel in the channel-power table */
+        parsed_region_chan->chan_pwr[i].chan = chan;
+        parsed_region_chan->chan_pwr[i].band = (t_u8) pbss_desc->bss_band;
+        parsed_region_chan->chan_pwr[i].pwr = tx_power;
+        parsed_region_chan->chan_pwr[i].ap_seen = MTRUE;
+        parsed_region_chan->no_of_chan++;
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function finds the no_of_chan-th chan after the first_chan
+ *
+ *  @param pmadapter  A pointer to mlan_adapter structure
+ *  @param band       Band
+ *  @param first_chan First channel number
+ *  @param no_of_chan Number of channels
+ *  @param chan       Pointer to the returned no_of_chan-th chan number
+ *
+ *  @return           MTRUE or MFALSE
+ */
+static t_u8
+wlan_11d_get_chan(pmlan_adapter pmadapter, t_u8 band, t_u8 first_chan,
+                  t_u8 no_of_chan, t_u8 * chan)
+{
+    chan_freq_power_t *cfp = MNULL;
+    t_u8 i;
+    t_u8 cfp_no = 0;
+
+    ENTER();
+    if (band & (BAND_B | BAND_G | BAND_GN)) {
+        cfp = channel_freq_power_UN_BG;
+        cfp_no = sizeof(channel_freq_power_UN_BG) / sizeof(chan_freq_power_t);
+    } else if (band & (BAND_A | BAND_AN)) {
+        cfp = channel_freq_power_UN_AJ;
+        cfp_no = sizeof(channel_freq_power_UN_AJ) / sizeof(chan_freq_power_t);
+    } else {
+        PRINTM(MERROR, "11D: Wrong Band[%d]\n", band);
+        LEAVE();
+        return MFALSE;
+    }
+    /* Locate the first_chan */
+    for (i = 0; i < cfp_no; i++) {
+        if (cfp && ((cfp + i)->channel == first_chan)) {
+            PRINTM(MINFO, "11D: first_chan found\n");
+            break;
+        }
+    }
+
+    if (i < cfp_no) {
+        /* Check if beyond the boundary */
+        if (i + no_of_chan < cfp_no) {
+            /* Get first_chan + no_of_chan */
+            *chan = (t_u8) (cfp + i + no_of_chan)->channel;
+            LEAVE();
+            return MTRUE;
+        }
+    }
+
+    LEAVE();
+    return MFALSE;
+}
+
+/**
+ *  @brief This function processes the country info present in BSSDescriptor.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pbss_desc     A pointer to BSSDescriptor_t
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_11d_process_country_info(mlan_private * pmpriv,
+                              BSSDescriptor_t * pbss_desc)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    parsed_region_chan_11d_t region_chan;
+    parsed_region_chan_11d_t *parsed_region_chan =
+        &pmadapter->parsed_region_chan;
+    t_u16 i, j, num_chan_added = 0;
+
+    ENTER();
+
+    memset(pmadapter, &region_chan, 0, sizeof(parsed_region_chan_11d_t));
+
+    /* Parse 11D country info */
+    if (wlan_11d_parse_domain_info(pmadapter, &pbss_desc->country_info,
+                                   (t_u8) pbss_desc->bss_band,
+                                   &region_chan) != MLAN_STATUS_SUCCESS) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    if (parsed_region_chan->no_of_chan != 0) {
+        /*
+         * Check if the channel number already exists in the
+         * chan-power table of parsed_region_chan
+         */
+        for (i = 0; (i < region_chan.no_of_chan && i < MAX_NO_OF_CHAN); i++) {
+            for (j = 0; (j < parsed_region_chan->no_of_chan &&
+                         j < MAX_NO_OF_CHAN); j++) {
+                /*
+                 * Channel already exists, update the tx power with new tx
+                 * power, since country IE is valid here.
+                 */
+                if (region_chan.chan_pwr[i].chan ==
+                    parsed_region_chan->chan_pwr[j].chan &&
+                    region_chan.chan_pwr[i].band ==
+                    parsed_region_chan->chan_pwr[j].band) {
+                    parsed_region_chan->chan_pwr[j].pwr =
+                        region_chan.chan_pwr[i].pwr;
+                    break;
+                }
+            }
+
+            if (j == parsed_region_chan->no_of_chan && j < MAX_NO_OF_CHAN) {
+                /*
+                 * Channel does not exist in the channel power table,
+                 * update this new chan and tx_power to the channel power table
+                 */
+                parsed_region_chan->chan_pwr[parsed_region_chan->no_of_chan +
+                                             num_chan_added].chan =
+                    region_chan.chan_pwr[i].chan;
+                parsed_region_chan->chan_pwr[parsed_region_chan->no_of_chan +
+                                             num_chan_added].band =
+                    region_chan.chan_pwr[i].band;
+                parsed_region_chan->chan_pwr[parsed_region_chan->no_of_chan +
+                                             num_chan_added].pwr =
+                    region_chan.chan_pwr[i].pwr;
+                parsed_region_chan->chan_pwr[parsed_region_chan->no_of_chan +
+                                             num_chan_added].ap_seen = MFALSE;
+                num_chan_added++;
+            }
+        }
+        parsed_region_chan->no_of_chan += num_chan_added;
+    } else {
+        /* Parsed region is empty, copy the first one */
+        memcpy(pmadapter, parsed_region_chan,
+               &region_chan, sizeof(parsed_region_chan_11d_t));
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This helper function copies chan_power_11d_t element
+ *
+ *  @param chan_dst   Pointer to destination of chan_power
+ *  @param chan_src   Pointer to source of chan_power
+ *
+ *  @return           N/A
+ */
+static t_void
+wlan_11d_copy_chan_power(chan_power_11d_t * chan_dst,
+                         chan_power_11d_t * chan_src)
+{
+    ENTER();
+
+    chan_dst->chan = chan_src->chan;
+    chan_dst->band = chan_src->band;
+    chan_dst->pwr = chan_src->pwr;
+    chan_dst->ap_seen = chan_src->ap_seen;
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function sorts parsed_region_chan in ascending
+ *  channel number.
+ *
+ *  @param parsed_region_chan   Pointer to parsed_region_chan_11d_t
+ *
+ *  @return                     N/A
+ */
+static t_void
+wlan_11d_sort_parsed_region_chan(parsed_region_chan_11d_t * parsed_region_chan)
+{
+    int i, j;
+    chan_power_11d_t temp;
+    chan_power_11d_t *pchan_power = parsed_region_chan->chan_pwr;
+
+    ENTER();
+
+    PRINTM(MINFO, "11D: Number of channel = %d\n",
+           parsed_region_chan->no_of_chan);
+
+    // Use insertion sort method
+    for (i = 1; i < parsed_region_chan->no_of_chan; i++) {
+        wlan_11d_copy_chan_power(&temp, pchan_power + i);
+        for (j = i; j > 0 && (pchan_power + j - 1)->chan > temp.chan; j--)
+            wlan_11d_copy_chan_power(pchan_power + j, pchan_power + j - 1);
+        wlan_11d_copy_chan_power(pchan_power + j, &temp);
+    }
+
+    HEXDUMP("11D: parsed_region_chan", (t_u8 *) parsed_region_chan,
+            sizeof(parsed_region_chan_11d_t));
+
+    LEAVE();
+    return;
+}
+#endif /* STA_SUPPORT */
+
+/**
+ *  @brief This function sends domain info to FW
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pioctl_buf   A pointer to MLAN IOCTL Request buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_11d_send_domain_info(mlan_private * pmpriv, t_void * pioctl_buf)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    /* Send cmd to FW to set domain info */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11D_DOMAIN_INFO,
+                           HostCmd_ACT_GEN_SET,
+                           0, (t_void *) pioctl_buf, MNULL);
+    if (ret)
+        PRINTM(MERROR, "11D: Failed to download domain Info\n");
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function overwrites domain_info
+ *
+ *  @param pmadapter        Pointer to mlan_adapter structure
+ *  @param band             Intended operating band
+ *  @param country_code     Intended country code
+ *  @param num_sub_band     Count of tuples in list below
+ *  @param sub_band_list    List of sub_band tuples
+ *
+ *  @return                 MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_11d_set_domain_info(mlan_private * pmpriv,
+                         t_u8 band,
+                         t_u8 country_code[COUNTRY_CODE_LEN],
+                         t_u8 num_sub_band,
+                         IEEEtypes_SubbandSet_t * sub_band_list)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    wlan_802_11d_domain_reg_t *pdomain = &pmadapter->domain_reg;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    memset(pmadapter, pdomain, 0, sizeof(wlan_802_11d_domain_reg_t));
+    memcpy(pmadapter, pdomain->country_code, country_code, COUNTRY_CODE_LEN);
+    pdomain->band = band;
+    pdomain->no_of_sub_band = num_sub_band;
+    memcpy(pmadapter, pdomain->sub_band, sub_band_list,
+           MIN(MRVDRV_MAX_SUBBAND_802_11D,
+               num_sub_band) * sizeof(IEEEtypes_SubbandSet_t));
+
+    LEAVE();
+    return ret;
+}
+
+/********************************************************
+                Global functions
+********************************************************/
+
+/**
+ *  @brief This function gets if priv is a station (STA)
+ *
+ *  @param pmpriv       Pointer to mlan_private structure
+ *
+ *  @return             MTRUE or MFALSE
+ */
+t_bool
+wlan_is_station(mlan_private * pmpriv)
+{
+    ENTER();
+    LEAVE();
+    return (GET_BSS_ROLE(pmpriv) == MLAN_BSS_ROLE_STA) ? MTRUE : MFALSE;
+}
+
+/**
+ *  @brief This function gets if 11D is enabled
+ *
+ *  @param pmpriv       Pointer to mlan_private structure
+ *
+ *  @return             MTRUE or MFALSE
+ */
+t_bool
+wlan_11d_is_enabled(mlan_private * pmpriv)
+{
+    ENTER();
+    LEAVE();
+    return (pmpriv->state_11d.enable_11d == ENABLE_11D) ? MTRUE : MFALSE;
+}
+
+/**
+ *  @brief Initialize interface variable for 11D
+ *
+ *  @param pmpriv       Pointer to mlan_private structure
+ *
+ *  @return             N/A
+ */
+t_void
+wlan_11d_priv_init(mlan_private * pmpriv)
+{
+    wlan_802_11d_state_t *state = &pmpriv->state_11d;
+
+    ENTER();
+
+    /* Start in disabled mode */
+    state->enable_11d = DISABLE_11D;
+    if (!pmpriv->adapter->init_para.cfg_11d)
+        state->user_enable_11d = DEFAULT_11D_STATE;
+    else
+        state->user_enable_11d =
+            (pmpriv->adapter->init_para.cfg_11d == MLAN_INIT_PARA_DISABLED) ?
+            DISABLE_11D : ENABLE_11D;
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief Initialize device variable for 11D
+ *
+ *  @param pmadapter    Pointer to mlan_adapter structure
+ *
+ *  @return             N/A
+ */
+t_void
+wlan_11d_init(mlan_adapter * pmadapter)
+{
+    ENTER();
+
+#ifdef STA_SUPPORT
+    memset(pmadapter, &(pmadapter->parsed_region_chan), 0,
+           sizeof(parsed_region_chan_11d_t));
+    memset(pmadapter, &(pmadapter->universal_channel), 0,
+           sizeof(region_chan_t));
+#endif
+    memset(pmadapter, &(pmadapter->domain_reg), 0,
+           sizeof(wlan_802_11d_domain_reg_t));
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function enable/disable 11D
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pioctl_buf   A pointer to MLAN IOCTL Request buffer
+ *  @param flag         11D status
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_11d_enable(mlan_private * pmpriv, t_void * pioctl_buf, state_11d_t flag)
+{
+#ifdef STA_SUPPORT
+    mlan_adapter *pmadapter = pmpriv->adapter;
+#endif
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    state_11d_t enable = flag;
+
+    ENTER();
+
+    /* Send cmd to FW to enable/disable 11D function */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_SNMP_MIB,
+                           HostCmd_ACT_GEN_SET,
+                           Dot11D_i, (t_void *) pioctl_buf, &enable);
+
+    if (ret) {
+        PRINTM(MERROR, "11D: Failed to %s 11D\n",
+               (flag) ? "enable" : "disable");
+    }
+#ifdef STA_SUPPORT
+    else {
+        /* clear parsed table regardless of flag */
+        memset(pmadapter, &(pmadapter->parsed_region_chan), 0,
+               sizeof(parsed_region_chan_11d_t));
+    }
+#endif
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function implements command CMD_802_11D_DOMAIN_INFO
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pcmd         A pointer to HostCmd_DS_COMMAND structure of
+ *                        command buffer
+ *  @param cmd_action   Command action
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_802_11d_domain_info(mlan_private * pmpriv,
+                             HostCmd_DS_COMMAND * pcmd, t_u16 cmd_action)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    HostCmd_DS_802_11D_DOMAIN_INFO *pdomain_info = &pcmd->params.domain_info;
+    MrvlIEtypes_DomainParamSet_t *domain = &pdomain_info->domain;
+    t_u8 no_of_sub_band = pmadapter->domain_reg.no_of_sub_band;
+
+    ENTER();
+
+    PRINTM(MINFO, "11D: number of sub-band=0x%x\n", no_of_sub_band);
+
+    pcmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11D_DOMAIN_INFO);
+    pdomain_info->action = wlan_cpu_to_le16(cmd_action);
+    if (cmd_action == HostCmd_ACT_GEN_GET) {
+        /* Dump domain info */
+        pcmd->size = wlan_cpu_to_le16(sizeof(pdomain_info->action) + S_DS_GEN);
+        HEXDUMP("11D: 802_11D_DOMAIN_INFO", (t_u8 *) pcmd,
+                wlan_le16_to_cpu(pcmd->size));
+        LEAVE();
+        return MLAN_STATUS_SUCCESS;
+    }
+
+    /* Set domain info fields */
+    domain->header.type = wlan_cpu_to_le16(TLV_TYPE_DOMAIN);
+    memcpy(pmadapter, domain->country_code,
+           pmadapter->domain_reg.country_code, sizeof(domain->country_code));
+
+    domain->header.len = ((no_of_sub_band * sizeof(IEEEtypes_SubbandSet_t)) +
+                          sizeof(domain->country_code));
+
+    if (no_of_sub_band) {
+        memcpy(pmadapter, domain->sub_band,
+               pmadapter->domain_reg.sub_band,
+               MIN(MRVDRV_MAX_SUBBAND_802_11D,
+                   no_of_sub_band) * sizeof(IEEEtypes_SubbandSet_t));
+
+        pcmd->size = wlan_cpu_to_le16(sizeof(pdomain_info->action) +
+                                      domain->header.len +
+                                      sizeof(MrvlIEtypesHeader_t) + S_DS_GEN);
+    } else {
+        pcmd->size = wlan_cpu_to_le16(sizeof(pdomain_info->action) + S_DS_GEN);
+    }
+    domain->header.len = wlan_cpu_to_le16(domain->header.len);
+
+    HEXDUMP("11D: 802_11D_DOMAIN_INFO", (t_u8 *) pcmd,
+            wlan_le16_to_cpu(pcmd->size));
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handle response of CMD_802_11D_DOMAIN_INFO
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         Pointer to command response buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ret_802_11d_domain_info(mlan_private * pmpriv, HostCmd_DS_COMMAND * resp)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    HostCmd_DS_802_11D_DOMAIN_INFO_RSP *domain_info =
+        &resp->params.domain_info_resp;
+    MrvlIEtypes_DomainParamSet_t *domain = &domain_info->domain;
+    t_u16 action = wlan_le16_to_cpu(domain_info->action);
+    t_u8 no_of_sub_band = 0;
+
+    ENTER();
+
+    /* Dump domain info response data */
+    HEXDUMP("11D: DOMAIN Info Rsp Data", (t_u8 *) resp, resp->size);
+
+    no_of_sub_band =
+        (t_u8) ((wlan_le16_to_cpu(domain->header.len) - COUNTRY_CODE_LEN)
+                / sizeof(IEEEtypes_SubbandSet_t));
+
+    PRINTM(MINFO, "11D Domain Info Resp: number of sub-band=%d\n",
+           no_of_sub_band);
+
+    if (no_of_sub_band > MRVDRV_MAX_SUBBAND_802_11D) {
+        PRINTM(MWARN, "11D: Invalid number of subbands %d returned!!\n",
+               no_of_sub_band);
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    switch (action) {
+    case HostCmd_ACT_GEN_SET:  /* Proc Set Action */
+        break;
+    case HostCmd_ACT_GEN_GET:
+        break;
+    default:
+        PRINTM(MERROR, "11D: Invalid Action:%d\n", domain_info->action);
+        ret = MLAN_STATUS_FAILURE;
+        break;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+#ifdef STA_SUPPORT
+/**
+ *  @brief This function parses country information for region channel
+ *
+ *  @param pmadapter            Pointer to mlan_adapter structure
+ *  @param country_info         Country information
+ *  @param band                 Chan band
+ *  @param parsed_region_chan   Pointer to parsed_region_chan_11d_t
+ *
+ *  @return                     MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_11d_parse_domain_info(pmlan_adapter pmadapter,
+                           IEEEtypes_CountryInfoFullSet_t * country_info,
+                           t_u8 band,
+                           parsed_region_chan_11d_t * parsed_region_chan)
+{
+    t_u8 no_of_sub_band, no_of_chan;
+    t_u8 last_chan, first_chan, cur_chan = 0;
+    t_u8 idx = 0;
+    t_u8 j, i;
+
+    ENTER();
+
+    /*
+     * Validation Rules:
+     *    1. Valid Region Code
+     *    2. First Chan increment
+     *    3. Channel range no overlap
+     *    4. Channel is valid?
+     *    5. Channel is supported by Region?
+     *    6. Others
+     */
+
+    HEXDUMP("country_info", (t_u8 *) country_info, 30);
+
+    /* Step 1: Check region_code */
+    if (!(*(country_info->country_code)) ||
+        (country_info->len <= COUNTRY_CODE_LEN)) {
+        /* No region info or wrong region info: treat as no 11D info */
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    no_of_sub_band = (country_info->len - COUNTRY_CODE_LEN) /
+        sizeof(IEEEtypes_SubbandSet_t);
+
+    for (j = 0, last_chan = 0; j < no_of_sub_band; j++) {
+
+        if (country_info->sub_band[j].first_chan <= last_chan) {
+            /* Step2&3: Check First Chan Num increment and no overlap */
+            PRINTM(MINFO, "11D: Chan[%d>%d] Overlap\n",
+                   country_info->sub_band[j].first_chan, last_chan);
+            continue;
+        }
+
+        first_chan = country_info->sub_band[j].first_chan;
+        no_of_chan = country_info->sub_band[j].no_of_chan;
+
+        for (i = 0; idx < MAX_NO_OF_CHAN && i < no_of_chan; i++) {
+            /* Step 4 : Channel is supported? */
+            if (wlan_11d_get_chan(pmadapter, band, first_chan, i, &cur_chan) ==
+                MFALSE) {
+                /* Chan is not found in UN table */
+                PRINTM(MWARN, "11D: channel is not supported: %d\n", i);
+                break;
+            }
+
+            last_chan = cur_chan;
+
+            /* Step 5: We don't need to check if cur_chan is supported by mrvl
+               in region */
+            parsed_region_chan->chan_pwr[idx].chan = cur_chan;
+            parsed_region_chan->chan_pwr[idx].band = band;
+            parsed_region_chan->chan_pwr[idx].pwr =
+                country_info->sub_band[j].max_tx_pwr;
+            idx++;
+        }
+
+        /* Step 6: Add other checking if any */
+    }
+
+    parsed_region_chan->no_of_chan = idx;
+
+    PRINTM(MINFO, "11D: number of channel=0x%x\n",
+           parsed_region_chan->no_of_chan);
+    HEXDUMP("11D: parsed_region_chan", (t_u8 *) parsed_region_chan->chan_pwr,
+            sizeof(chan_power_11d_t) * idx);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function converts channel to frequency
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param chan         Channel number
+ *  @param band         Band
+ *
+ *  @return             Channel frequency
+ */
+t_u32
+wlan_11d_chan_2_freq(pmlan_adapter pmadapter, t_u8 chan, t_u8 band)
+{
+    chan_freq_power_t *cf;
+    t_u16 cnt;
+    t_u16 i;
+    t_u32 freq = 0;
+
+    ENTER();
+
+    /* Get channel-frequency-power trios */
+    if (band & (BAND_A | BAND_AN)) {
+        cf = channel_freq_power_UN_AJ;
+        cnt = sizeof(channel_freq_power_UN_AJ) / sizeof(chan_freq_power_t);
+    } else {
+        cf = channel_freq_power_UN_BG;
+        cnt = sizeof(channel_freq_power_UN_BG) / sizeof(chan_freq_power_t);
+    }
+
+    /* Locate channel and return corresponding frequency */
+    for (i = 0; i < cnt; i++) {
+        if (chan == cf[i].channel)
+            freq = cf[i].freq;
+    }
+
+    LEAVE();
+    return freq;
+}
+
+/**
+ *  @brief This function setups scan channels
+ *
+ *  @param pmpriv       Pointer to mlan_private structure
+ *  @param band         Band
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_11d_set_universaltable(mlan_private * pmpriv, t_u8 band)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    t_u16 size = sizeof(chan_freq_power_t);
+    t_u16 i = 0;
+
+    ENTER();
+
+    memset(pmadapter, pmadapter->universal_channel, 0,
+           sizeof(pmadapter->universal_channel));
+
+    if (band & (BAND_B | BAND_G | BAND_GN))
+        /* If band B, G or N */
+    {
+        /* Set channel-frequency-power */
+        pmadapter->universal_channel[i].num_cfp =
+            (t_u8) (sizeof(channel_freq_power_UN_BG) / size);
+        PRINTM(MINFO, "11D: BG-band num_cfp=%d\n",
+               pmadapter->universal_channel[i].num_cfp);
+
+        pmadapter->universal_channel[i].pcfp = channel_freq_power_UN_BG;
+        pmadapter->universal_channel[i].valid = MTRUE;
+
+        /* Set region code */
+        pmadapter->universal_channel[i].region = UNIVERSAL_REGION_CODE;
+
+        /* Set band */
+        if (band & BAND_GN)
+            pmadapter->universal_channel[i].band = BAND_G;
+        else
+            pmadapter->universal_channel[i].band =
+                (band & BAND_G) ? BAND_G : BAND_B;
+        i++;
+    }
+
+    if (band & (BAND_A | BAND_AN)) {
+        /* If band A */
+
+        /* Set channel-frequency-power */
+        pmadapter->universal_channel[i].num_cfp =
+            sizeof(channel_freq_power_UN_AJ) / size;
+        PRINTM(MINFO, "11D: AJ-band num_cfp=%d\n",
+               pmadapter->universal_channel[i].num_cfp);
+
+        pmadapter->universal_channel[i].pcfp = channel_freq_power_UN_AJ;
+
+        pmadapter->universal_channel[i].valid = MTRUE;
+
+        /* Set region code */
+        pmadapter->universal_channel[i].region = UNIVERSAL_REGION_CODE;
+
+        /* Set band */
+        pmadapter->universal_channel[i].band = BAND_A;
+        i++;
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function calculates the scan type for channels
+ *
+ *  @param pmadapter            A pointer to mlan_adapter structure
+ *  @param band                 Band number
+ *  @param chan                 Chan number
+ *  @param parsed_region_chan   Pointer to parsed_region_chan_11d_t
+ *
+ *  @return                     PASSIVE if chan is unknown; ACTIVE if chan is known
+ */
+t_u8
+wlan_11d_get_scan_type(pmlan_adapter pmadapter,
+                       t_u8 band,
+                       t_u8 chan, parsed_region_chan_11d_t * parsed_region_chan)
+{
+    t_u8 scan_type = MLAN_SCAN_TYPE_PASSIVE;
+
+    ENTER();
+
+    if (wlan_11d_channel_known(pmadapter, band, chan, parsed_region_chan)) {
+        /* Channel found */
+        PRINTM(MINFO, "11D: Channel found and doing Active Scan\n");
+        scan_type = MLAN_SCAN_TYPE_ACTIVE;
+    } else
+        PRINTM(MINFO, "11D: Channel not found and doing Passive Scan\n");
+
+    LEAVE();
+    return scan_type;
+}
+
+/**
+ *  @brief This function clears the parsed region table, if 11D is enabled
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_11d_clear_parsedtable(mlan_private * pmpriv)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    if (wlan_11d_is_enabled(pmpriv))
+        memset(pmadapter, &(pmadapter->parsed_region_chan), 0,
+               sizeof(parsed_region_chan_11d_t));
+    else
+        ret = MLAN_STATUS_FAILURE;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function generates 11D info from user specified regioncode
+ *         and download to FW
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param band         Band to create
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_11d_create_dnld_countryinfo(mlan_private * pmpriv, t_u8 band)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    region_chan_t *region_chan;
+    parsed_region_chan_11d_t parsed_region_chan;
+    t_u8 j;
+
+    ENTER();
+
+    /* Only valid if 11D is enabled */
+    if (wlan_11d_is_enabled(pmpriv)) {
+
+        PRINTM(MINFO, "11D: Band[%d]\n", band);
+
+        /* Update parsed_region_chan; download domain info to FW */
+
+        /* Find region channel */
+        for (j = 0; j < MAX_REGION_CHANNEL_NUM; j++) {
+            region_chan = &pmadapter->region_channel[j];
+
+            PRINTM(MINFO, "11D: [%d] region_chan->Band[%d]\n", j,
+                   region_chan->band);
+
+            if (!region_chan || !region_chan->valid || !region_chan->pcfp)
+                continue;
+            switch (region_chan->band) {
+            case BAND_A:
+                switch (band) {
+                case BAND_A:
+                case BAND_AN:
+                case BAND_A | BAND_AN:
+                    break;
+                default:
+                    continue;
+                }
+                break;
+            case BAND_B:
+            case BAND_G:
+                switch (band) {
+                case BAND_B:
+                case BAND_G:
+                case BAND_G | BAND_B:
+                case BAND_GN:
+                case BAND_G | BAND_GN:
+                case BAND_B | BAND_G | BAND_GN:
+                    break;
+                default:
+                    continue;
+                }
+                break;
+            default:
+                continue;
+            }
+            break;
+        }
+
+        /* Check if region channel found */
+        if (j >= MAX_REGION_CHANNEL_NUM) {
+            PRINTM(MERROR, "11D: region_chan not found. Band[%d]\n", band);
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+
+        /* Generate parsed region channel info from region channel */
+        memset(pmadapter, &parsed_region_chan, 0,
+               sizeof(parsed_region_chan_11d_t));
+        wlan_11d_generate_parsed_region_chan(pmadapter, region_chan,
+                                             &parsed_region_chan);
+
+        /* Generate domain info from parsed region channel info */
+        wlan_11d_generate_domain_info(pmadapter, &parsed_region_chan);
+
+        /* Set domain info */
+        ret = wlan_11d_send_domain_info(pmpriv, MNULL);
+        if (ret) {
+            PRINTM(MERROR, "11D: Error sending domain info to FW\n");
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function parses country info from AP and
+ *           download country info to FW
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pbss_desc     A pointer to BSS descriptor
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_11d_parse_dnld_countryinfo(mlan_private * pmpriv,
+                                BSSDescriptor_t * pbss_desc)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    parsed_region_chan_11d_t region_chan;
+    parsed_region_chan_11d_t bssdesc_region_chan;
+    t_u32 i, j;
+
+    ENTER();
+
+    /* Only valid if 11D is enabled */
+    if (wlan_11d_is_enabled(pmpriv)) {
+
+        memset(pmadapter, &region_chan, 0, sizeof(parsed_region_chan_11d_t));
+        memset(pmadapter, &bssdesc_region_chan, 0,
+               sizeof(parsed_region_chan_11d_t));
+
+        memcpy(pmadapter, &region_chan,
+               &pmadapter->parsed_region_chan,
+               sizeof(parsed_region_chan_11d_t));
+
+        if (pbss_desc) {
+            /* Parse domain info if available */
+            ret =
+                wlan_11d_parse_domain_info(pmadapter, &pbss_desc->country_info,
+                                           (t_u8) pbss_desc->bss_band,
+                                           &bssdesc_region_chan);
+
+            if (ret == MLAN_STATUS_SUCCESS) {
+                /* Update the channel-power table */
+                for (i = 0; ((i < bssdesc_region_chan.no_of_chan)
+                             && (i < MAX_NO_OF_CHAN)); i++) {
+
+                    for (j = 0; ((j < region_chan.no_of_chan)
+                                 && (j < MAX_NO_OF_CHAN)); j++) {
+                        /*
+                         * Channel already exists, use minimum of existing
+                         * tx power and tx_power received from
+                         * country info of the current AP
+                         */
+                        if (region_chan.chan_pwr[i].chan ==
+                            bssdesc_region_chan.chan_pwr[j].chan &&
+                            region_chan.chan_pwr[i].band ==
+                            bssdesc_region_chan.chan_pwr[j].band) {
+                            region_chan.chan_pwr[j].pwr =
+                                MIN(region_chan.chan_pwr[j].pwr,
+                                    bssdesc_region_chan.chan_pwr[i].pwr);
+                            break;
+                        }
+                    }
+                }
+            }
+        }
+
+        /* Generate domain info */
+        wlan_11d_generate_domain_info(pmadapter, &region_chan);
+
+        /* Set domain info */
+        ret = wlan_11d_send_domain_info(pmpriv, MNULL);
+        if (ret) {
+            PRINTM(MERROR, "11D: Error sending domain info to FW\n");
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function prepares domain info from scan table and
+ *         downloads the domain info command to the FW.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_11d_prepare_dnld_domain_info_cmd(mlan_private * pmpriv)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    IEEEtypes_CountryInfoFullSet_t *pcountry_full = MNULL;
+    t_u32 idx;
+
+    ENTER();
+
+    /* Only valid if 11D is enabled */
+    if (wlan_11d_is_enabled(pmpriv) && pmadapter->num_in_scan_table != 0) {
+        for (idx = 0; idx < pmadapter->num_in_scan_table; idx++) {
+            pcountry_full = &pmadapter->pscan_table[idx].country_info;
+
+            ret =
+                wlan_11d_update_chan_pwr_table(pmpriv,
+                                               &pmadapter->pscan_table[idx]);
+
+            if (*(pcountry_full->country_code) != 0 &&
+                (pcountry_full->len > COUNTRY_CODE_LEN)) {
+                /* Country info found in the BSS Descriptor */
+                ret =
+                    wlan_11d_process_country_info(pmpriv,
+                                                  &pmadapter->pscan_table[idx]);
+            }
+        }
+
+        /* Sort parsed_region_chan in ascending channel number */
+        wlan_11d_sort_parsed_region_chan(&pmadapter->parsed_region_chan);
+
+        /* Check if connected */
+        if (pmpriv->media_connected == MTRUE) {
+            ret =
+                wlan_11d_parse_dnld_countryinfo(pmpriv,
+                                                &pmpriv->curr_bss_params.
+                                                bss_descriptor);
+        } else {
+            ret = wlan_11d_parse_dnld_countryinfo(pmpriv, MNULL);
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function sets up domain_reg and downloads CMD to FW
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   Pointer to the IOCTL request buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_11d_cfg_domain_info(IN pmlan_adapter pmadapter,
+                         IN mlan_ioctl_req * pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_11d_domain_info *domain_info = MNULL;
+    mlan_ds_11d_cfg *cfg_11d = MNULL;
+    t_u8 region_code = 0;
+
+    ENTER();
+
+    cfg_11d = (mlan_ds_11d_cfg *) pioctl_req->pbuf;
+    domain_info = &cfg_11d->param.domain_info;
+
+    /* Update region code and table based on country code */
+    if (wlan_11d_region_2_code(pmadapter, domain_info->country_code,
+                               &region_code) == MLAN_STATUS_SUCCESS) {
+        pmadapter->region_code = region_code;
+        ret = wlan_set_regiontable(pmpriv, region_code, pmadapter->fw_bands);
+        if (ret != MLAN_STATUS_SUCCESS)
+            goto done;
+    }
+
+    wlan_11d_set_domain_info(pmpriv, domain_info->band,
+                             domain_info->country_code,
+                             domain_info->no_of_sub_band,
+                             (IEEEtypes_SubbandSet_t *) domain_info->sub_band);
+    ret = wlan_11d_send_domain_info(pmpriv, pioctl_req);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  done:
+    LEAVE();
+    return ret;
+}
+#endif /* STA_SUPPORT */
+
+#if defined(UAP_SUPPORT)
+/**
+ *  @brief This function handles domain info data from UAP interface.
+ *         Checks conditions, sets up domain_reg, then downloads CMD.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param band         Band interface is operating on
+ *  @param domain_tlv   Pointer to domain_info tlv
+ *  @param pioctl_buf   Pointer to the IOCTL buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_11d_handle_uap_domain_info(mlan_private * pmpriv,
+                                t_u8 band,
+                                t_u8 * domain_tlv, t_void * pioctl_buf)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    MrvlIEtypes_DomainParamSet_t *pdomain_tlv;
+    t_u8 num_sub_band = 0;
+    t_u8 region_code = 0;
+
+    ENTER();
+
+    pdomain_tlv = (MrvlIEtypes_DomainParamSet_t *) domain_tlv;
+
+    // update region code & table based on country string
+    if (wlan_11d_region_2_code(pmadapter, pdomain_tlv->country_code,
+                               &region_code) == MLAN_STATUS_SUCCESS) {
+        pmadapter->region_code = region_code;
+        ret = wlan_set_regiontable(pmpriv, region_code, pmadapter->fw_bands);
+    }
+
+    num_sub_band = ((pdomain_tlv->header.len - COUNTRY_CODE_LEN) /
+                    sizeof(IEEEtypes_SubbandSet_t));
+
+    // TODO: don't just clobber pmadapter->domain_reg.
+    // Add some checking or merging between STA & UAP domain_info
+
+    wlan_11d_set_domain_info(pmpriv, band, pdomain_tlv->country_code,
+                             num_sub_band, pdomain_tlv->sub_band);
+    ret = wlan_11d_send_domain_info(pmpriv, pioctl_buf);
+
+    LEAVE();
+    return ret;
+}
+#endif
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_11h.c b/drivers/net/wireless/sd8797/mlan/mlan_11h.c
new file mode 100644
index 000000000000..3a38979a8dd2
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_11h.c
@@ -0,0 +1,3208 @@
+/** @file mlan_11h.c
+ *
+ *  @brief This file contains functions for 802.11H.
+ *
+ *  Copyright (C) 2008-2012, Marvell International Ltd.
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ *
+ */
+
+/*************************************************************
+Change Log:
+    03/26/2009: initial version
+************************************************************/
+
+#include "mlan.h"
+#include "mlan_join.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_ioctl.h"
+#include "mlan_11h.h"
+#ifdef UAP_SUPPORT
+#include "mlan_uap.h"
+#endif
+
+/********************************************************
+                Local Variables
+********************************************************/
+
+/** Default IBSS DFS recovery interval (in TBTTs); used for adhoc start */
+#define WLAN_11H_DEFAULT_DFS_RECOVERY_INTERVAL   100
+
+/** Default 11h power constraint used to offset the maximum transmit power */
+#define WLAN_11H_TPC_POWERCONSTRAINT  0
+
+/** 11h TPC Power capability minimum setting, sent in TPC_INFO command to fw */
+#define WLAN_11H_TPC_POWERCAPABILITY_MIN     5
+
+/** 11h TPC Power capability maximum setting, sent in TPC_INFO command to fw */
+#define WLAN_11H_TPC_POWERCAPABILITY_MAX     20
+
+/** Regulatory requirement for the duration of a channel availability check */
+#define WLAN_11H_CHANNEL_AVAIL_CHECK_DURATION    60000  /* in ms */
+
+/** Starting Frequency for 11A band */
+#define START_FREQ_11A_BAND     5000    /* in MHz */
+
+/** Regulatory requirement for the duration of a non-occupancy period */
+#define WLAN_11H_NON_OCCUPANCY_PERIOD    1800   /* in sec (30mins) */
+
+/** Maximum allowable age (seconds) on DFS report data */
+#define MAX_DFS_REPORT_USABLE_AGE_SEC  (120)    // 2 minutes
+
+/** Minimum delay for CHAN_SW IE to broadcast by FW */
+#define MIN_RDH_CHAN_SW_IE_PERIOD_MSEC  (500)   // 5 beacons @ 100ms
+
+/** Maximum delay for CHAN_SW IE to broadcast by FW */
+#define MAX_RDH_CHAN_SW_IE_PERIOD_MSEC  (3000)  // 5 beacons @ 600ms
+
+/** Maximum retries on selecting new random channel */
+#define MAX_RANDOM_CHANNEL_RETRIES  (20)
+
+/** Maximum retries on selecting new random non-dfs channel */
+#define MAX_SWITCH_CHANNEL_RETRIES  (30)
+
+/** Value for undetermined priv_curr_idx on first entry to new RDH stage */
+#define RDH_STAGE_FIRST_ENTRY_PRIV_IDX  (0xff)
+
+/** Region codes 0x10, 0x20:  channels 1 thru 11 supported */
+static const
+    IEEEtypes_SupportChan_Subband_t wlan_11h_2_4G_region_FCC = { 1, 11 };
+
+/** Region codes 0x30, 0x32, 0x41, 0x50:  channels 1 thru 13 supported */
+static const
+    IEEEtypes_SupportChan_Subband_t wlan_11h_2_4G_region_EU = { 1, 13 };
+
+/** Region code 0x40:  only channel 14 supported */
+static const
+    IEEEtypes_SupportChan_Subband_t wlan_11h_2_4G_region_JPN40 = { 14, 1 };
+
+/** JPN sub-band config : Start Channel = 8, NumChans = 3 */
+static const
+    IEEEtypes_SupportChan_Subband_t wlan_11h_JPN_bottom_band = { 8, 3 };
+
+/** U-NII sub-band config : Start Channel = 36, NumChans = 4 */
+static const
+    IEEEtypes_SupportChan_Subband_t wlan_11h_unii_lower_band = { 36, 4 };
+
+/** U-NII sub-band config : Start Channel = 52, NumChans = 4 */
+static const
+    IEEEtypes_SupportChan_Subband_t wlan_11h_unii_middle_band = { 52, 4 };
+
+/** U-NII sub-band config : Start Channel = 100, NumChans = 11 */
+static const
+    IEEEtypes_SupportChan_Subband_t wlan_11h_unii_mid_upper_band = { 100, 11 };
+
+/** U-NII sub-band config : Start Channel = 149, NumChans = 5 */
+static const
+    IEEEtypes_SupportChan_Subband_t wlan_11h_unii_upper_band = { 149, 5 };
+
+/** Internally passed structure used to send a CMD_802_11_TPC_INFO command */
+typedef struct
+{
+    t_u8 chan;                  /**< Channel to which the power constraint applies */
+    t_u8 power_constraint;      /**< Local power constraint to send to firmware */
+} wlan_11h_tpc_info_param_t;
+
+/********************************************************
+                Global Variables
+********************************************************/
+
+/********************************************************
+                Local Functions
+********************************************************/
+
+/**
+ *  @brief Utility function to get a random number based on the underlying OS
+ *
+ *  @param pmadapter Pointer to mlan_adapter
+ *  @return random integer
+ */
+static t_u32
+wlan_11h_get_random_num(pmlan_adapter pmadapter)
+{
+    t_u32 sec, usec;
+
+    ENTER();
+    pmadapter->callbacks.moal_get_system_time(pmadapter->pmoal_handle, &sec,
+                                              &usec);
+    sec = (sec & 0xFFFF) + (sec >> 16);
+    usec = (usec & 0xFFFF) + (usec >> 16);
+
+    LEAVE();
+    return ((usec << 16) | sec);
+}
+
+/**
+ *  @brief Convert an IEEE formatted IE to 16-bit ID/Len Marvell
+ *         proprietary format
+ *
+ *  @param pmadapter Pointer to mlan_adapter
+ *  @param pout_buf Output parameter: Buffer to output Marvell formatted IE
+ *  @param pin_ie   Pointer to IEEE IE to be converted to Marvell format
+ *
+ *  @return         Number of bytes output to pout_buf parameter return
+ */
+static t_u32
+wlan_11h_convert_ieee_to_mrvl_ie(mlan_adapter * pmadapter,
+                                 t_u8 * pout_buf, const t_u8 * pin_ie)
+{
+    MrvlIEtypesHeader_t mrvl_ie_hdr;
+    t_u8 *ptmp_buf = pout_buf;
+
+    ENTER();
+    /* Assign the Element Id and Len to the Marvell struct attributes */
+    mrvl_ie_hdr.type = wlan_cpu_to_le16(pin_ie[0]);
+    mrvl_ie_hdr.len = wlan_cpu_to_le16(pin_ie[1]);
+
+    /* If the element ID is zero, return without doing any copying */
+    if (!mrvl_ie_hdr.type) {
+        LEAVE();
+        return 0;
+    }
+
+    /* Copy the header to the buffer pointer */
+    memcpy(pmadapter, ptmp_buf, &mrvl_ie_hdr, sizeof(mrvl_ie_hdr));
+
+    /* Increment the temp buffer pointer by the size appended */
+    ptmp_buf += sizeof(mrvl_ie_hdr);
+
+    /* Append the data section of the IE; length given by the IEEE IE length */
+    memcpy(pmadapter, ptmp_buf, pin_ie + 2, pin_ie[1]);
+
+    LEAVE();
+    /* Return the number of bytes appended to pout_buf */
+    return (sizeof(mrvl_ie_hdr) + pin_ie[1]);
+}
+
+#ifdef STA_SUPPORT
+/**
+ *  @brief Setup the IBSS DFS element passed to the firmware in adhoc start
+ *         and join commands
+ *
+ *  The DFS Owner and recovery fields are set to be our MAC address and
+ *    a predetermined constant recovery value.  If we are joining an adhoc
+ *    network, these values are replaced with the existing IBSS values.
+ *    They are valid only when starting a new IBSS.
+ *
+ *  The IBSS DFS Element is variable in size based on the number of
+ *    channels supported in our current region.
+ *
+ *  @param priv Private driver information structure
+ *  @param pdfs Output parameter: Pointer to the IBSS DFS element setup by
+ *              this function.
+ *
+ *  @return
+ *    - Length of the returned element in pdfs output parameter
+ *    - 0 if returned element is not setup
+ */
+static t_u32
+wlan_11h_set_ibss_dfs_ie(mlan_private * priv, IEEEtypes_IBSS_DFS_t * pdfs)
+{
+    t_u8 num_chans = 0;
+    MeasRptBasicMap_t initial_map;
+    mlan_adapter *adapter = priv->adapter;
+
+    ENTER();
+
+    memset(adapter, pdfs, 0x00, sizeof(IEEEtypes_IBSS_DFS_t));
+
+    /*
+     * A basic measurement report is included with each channel in the
+     *   map field.  Initial value for the map for each supported channel
+     *   is with only the unmeasured bit set.
+     */
+    memset(adapter, &initial_map, 0x00, sizeof(initial_map));
+    initial_map.unmeasured = 1;
+
+    /* Set the DFS Owner and recovery interval fields */
+    memcpy(adapter, pdfs->dfs_owner, priv->curr_addr, sizeof(pdfs->dfs_owner));
+    pdfs->dfs_recovery_interval = WLAN_11H_DEFAULT_DFS_RECOVERY_INTERVAL;
+
+    for (; (num_chans < adapter->parsed_region_chan.no_of_chan)
+         && (num_chans < WLAN_11H_MAX_IBSS_DFS_CHANNELS); num_chans++) {
+        pdfs->channel_map[num_chans].channel_number =
+            adapter->parsed_region_chan.chan_pwr[num_chans].chan;
+
+        /*
+         * Set the initial map field with a basic measurement
+         */
+        pdfs->channel_map[num_chans].rpt_map = initial_map;
+    }
+
+    /*
+     * If we have an established channel map, include it and return
+     *   a valid DFS element
+     */
+    if (num_chans) {
+        PRINTM(MINFO, "11h: Added %d channels to IBSS DFS Map\n", num_chans);
+
+        pdfs->element_id = IBSS_DFS;
+        pdfs->len =
+            (sizeof(pdfs->dfs_owner) + sizeof(pdfs->dfs_recovery_interval)
+             + num_chans * sizeof(IEEEtypes_ChannelMap_t));
+
+        LEAVE();
+        return (pdfs->len + sizeof(pdfs->len) + sizeof(pdfs->element_id));
+    }
+
+    /* Ensure the element is zeroed out for an invalid return */
+    memset(adapter, pdfs, 0x00, sizeof(IEEEtypes_IBSS_DFS_t));
+
+    LEAVE();
+    return 0;
+}
+#endif
+
+/**
+ *  @brief Setup the Supported Channel IE sent in association requests
+ *
+ *  The Supported Channels IE is required to be sent when the spectrum
+ *    management capability (11h) is enabled.  The element contains a
+ *    starting channel and number of channels tuple for each sub-band
+ *    the STA supports.  This information is based on the operating region.
+ *
+ *  @param priv      Private driver information structure
+ *  @param band      Band in use
+ *  @param psup_chan Output parameter: Pointer to the Supported Chan element
+ *                   setup by this function.
+ *
+ *  @return
+ *    - Length of the returned element in psup_chan output parameter
+ *    - 0 if returned element is not setup
+ */
+static t_u16
+wlan_11h_set_supp_channels_ie(mlan_private * priv,
+                              t_u8 band,
+                              IEEEtypes_SupportedChannels_t * psup_chan)
+{
+    t_u16 num_subbands = 0;
+    t_u16 ret_len = 0;
+    t_u8 cfp_bg, cfp_a;
+
+    ENTER();
+    memset(priv->adapter, psup_chan, 0x00,
+           sizeof(IEEEtypes_SupportedChannels_t));
+
+    cfp_bg = cfp_a = priv->adapter->region_code;
+    if (!priv->adapter->region_code) {
+        /* Invalid region code, use CFP code */
+        cfp_bg = priv->adapter->cfp_code_bg;
+        cfp_a = priv->adapter->cfp_code_a;
+    }
+
+    if ((band & BAND_B) || (band & BAND_G)) {
+        /*
+         * Channels are contiguous in 2.4GHz, usually only one subband.
+         */
+        switch (cfp_bg) {
+        case 0x10:             /* USA FCC */
+        case 0x20:             /* Canada IC */
+        default:
+            psup_chan->subband[num_subbands++] = wlan_11h_2_4G_region_FCC;
+            break;
+        case 0x30:             /* Europe ETSI */
+        case 0x32:             /* France */
+        case 0x41:             /* Japan */
+        case 0x50:             /* China */
+            psup_chan->subband[num_subbands++] = wlan_11h_2_4G_region_EU;
+            break;
+        case 0x40:             /* Japan */
+            psup_chan->subband[num_subbands++] = wlan_11h_2_4G_region_JPN40;
+            break;
+        case 0xff:             /* Japan special */
+            psup_chan->subband[num_subbands++] = wlan_11h_2_4G_region_EU;
+            psup_chan->subband[num_subbands++] = wlan_11h_2_4G_region_JPN40;
+            break;
+        }
+    } else if (band & BAND_A) {
+        /*
+         * Set the supported channel elements based on the region code,
+         *   incrementing num_subbands for each sub-band we append to the
+         *   element.
+         */
+        switch (cfp_a) {
+        case 0x10:             /* USA FCC */
+        case 0x20:             /* Canada IC */
+        case 0x32:             /* France */
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_lower_band;
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_middle_band;
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_mid_upper_band;
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_upper_band;
+            break;
+        case 0x30:             /* Europe ETSI */
+        default:
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_lower_band;
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_middle_band;
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_mid_upper_band;
+            break;
+        case 0x50:             /* China */
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_upper_band;
+            break;
+        case 0x40:             /* Japan */
+        case 0x41:             /* Japan */
+        case 0xff:             /* Japan special */
+            psup_chan->subband[num_subbands++] = wlan_11h_JPN_bottom_band;
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_lower_band;
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_middle_band;
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_mid_upper_band;
+            break;
+        case 0x1:              /* Low band (5150-5250 MHz) channels */
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_lower_band;
+            break;
+        case 0x2:              /* Lower middle band (5250-5350 MHz) channels */
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_middle_band;
+            break;
+        case 0x3:              /* Upper middle band (5470-5725 MHz) channels */
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_mid_upper_band;
+            break;
+        case 0x4:              /* High band (5725-5850 MHz) channels */
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_upper_band;
+            break;
+        case 0x5:              /* Low band (5150-5250 MHz) and High band
+                                   (5725-5850 MHz) channels */
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_lower_band;
+            psup_chan->subband[num_subbands++] = wlan_11h_unii_upper_band;
+            break;
+        }
+    }
+
+    /*
+     * If we have setup any supported subbands in the element, return a
+     *    valid IE along with its size, else return 0.
+     */
+    if (num_subbands) {
+        psup_chan->element_id = SUPPORTED_CHANNELS;
+        psup_chan->len = num_subbands * sizeof(IEEEtypes_SupportChan_Subband_t);
+
+        ret_len = (t_u16) (psup_chan->len
+                           + sizeof(psup_chan->len) +
+                           sizeof(psup_chan->element_id));
+
+        HEXDUMP("11h: SupChan", (t_u8 *) psup_chan, ret_len);
+    }
+
+    LEAVE();
+    return ret_len;
+}
+
+/**
+ *  @brief Prepare CMD_802_11_TPC_ADAPT_REQ firmware command
+ *
+ *  @param priv      Private driver information structure
+ *  @param pcmd_ptr  Output parameter: Pointer to the command being prepared
+ *                   for the firmware
+ *  @param pinfo_buf HostCmd_DS_802_11_TPC_ADAPT_REQ passed as void data block
+ *
+ *  @return          MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_11h_cmd_tpc_request(mlan_private * priv,
+                         HostCmd_DS_COMMAND * pcmd_ptr,
+                         const t_void * pinfo_buf)
+{
+    ENTER();
+
+    memcpy(priv->adapter, &pcmd_ptr->params.tpc_req, pinfo_buf,
+           sizeof(HostCmd_DS_802_11_TPC_ADAPT_REQ));
+
+    pcmd_ptr->params.tpc_req.req.timeout =
+        wlan_cpu_to_le16(pcmd_ptr->params.tpc_req.req.timeout);
+
+    /* Converted to little endian in wlan_11h_cmd_process */
+    pcmd_ptr->size = sizeof(HostCmd_DS_802_11_TPC_ADAPT_REQ) + S_DS_GEN;
+
+    HEXDUMP("11h: 11_TPC_ADAPT_REQ:", (t_u8 *) pcmd_ptr,
+            (t_u32) pcmd_ptr->size);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Prepare CMD_802_11_TPC_INFO firmware command
+ *
+ *  @param priv      Private driver information structure
+ *  @param pcmd_ptr  Output parameter: Pointer to the command being prepared
+ *                   for the firmware
+ *  @param pinfo_buf wlan_11h_tpc_info_param_t passed as void data block
+ *
+ *  @return          MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_11h_cmd_tpc_info(mlan_private * priv,
+                      HostCmd_DS_COMMAND * pcmd_ptr, const t_void * pinfo_buf)
+{
+    HostCmd_DS_802_11_TPC_INFO *ptpc_info = &pcmd_ptr->params.tpc_info;
+    MrvlIEtypes_LocalPowerConstraint_t *pconstraint =
+        &ptpc_info->local_constraint;
+    MrvlIEtypes_PowerCapability_t *pcap = &ptpc_info->power_cap;
+
+    wlan_11h_device_state_t *pstate = &priv->adapter->state_11h;
+    const wlan_11h_tpc_info_param_t *ptpc_info_param =
+        (wlan_11h_tpc_info_param_t *) pinfo_buf;
+
+    ENTER();
+
+    pcap->min_power = pstate->min_tx_power_capability;
+    pcap->max_power = pstate->max_tx_power_capability;
+    pcap->header.len = wlan_cpu_to_le16(2);
+    pcap->header.type = wlan_cpu_to_le16(TLV_TYPE_POWER_CAPABILITY);
+
+    pconstraint->chan = ptpc_info_param->chan;
+    pconstraint->constraint = ptpc_info_param->power_constraint;
+    pconstraint->header.type = wlan_cpu_to_le16(TLV_TYPE_POWER_CONSTRAINT);
+    pconstraint->header.len = wlan_cpu_to_le16(2);
+
+    /* Converted to little endian in wlan_11h_cmd_process */
+    pcmd_ptr->size = sizeof(HostCmd_DS_802_11_TPC_INFO) + S_DS_GEN;
+
+    HEXDUMP("11h: TPC INFO", (t_u8 *) pcmd_ptr, (t_u32) pcmd_ptr->size);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief  Prepare CMD_802_11_CHAN_SW_ANN firmware command
+ *
+ *  @param priv      Private driver information structure
+ *  @param pcmd_ptr  Output parameter: Pointer to the command being
+ *                   prepared to for firmware
+ *  @param pinfo_buf HostCmd_DS_802_11_CHAN_SW_ANN passed as void data block
+ *
+ *  @return          MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_11h_cmd_chan_sw_ann(mlan_private * priv,
+                         HostCmd_DS_COMMAND * pcmd_ptr,
+                         const t_void * pinfo_buf)
+{
+    const HostCmd_DS_802_11_CHAN_SW_ANN *pch_sw_ann =
+        (HostCmd_DS_802_11_CHAN_SW_ANN *) pinfo_buf;
+
+    ENTER();
+
+    /* Converted to little endian in wlan_11h_cmd_process */
+    pcmd_ptr->size = sizeof(HostCmd_DS_802_11_CHAN_SW_ANN) + S_DS_GEN;
+
+    memcpy(priv->adapter, &pcmd_ptr->params.chan_sw_ann, pch_sw_ann,
+           sizeof(HostCmd_DS_802_11_CHAN_SW_ANN));
+
+    PRINTM(MINFO, "11h: ChSwAnn: %#x-%u, Seq=%u, Ret=%u\n",
+           pcmd_ptr->command, pcmd_ptr->size, pcmd_ptr->seq_num,
+           pcmd_ptr->result);
+    PRINTM(MINFO, "11h: ChSwAnn: Ch=%d, Cnt=%d, Mode=%d\n",
+           pch_sw_ann->new_chan, pch_sw_ann->switch_count,
+           pch_sw_ann->switch_mode);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief  Prepare CMD_CHAN_REPORT_REQUEST firmware command
+ *
+ *  @param priv      Private driver information structure
+ *  @param pcmd_ptr  Output parameter: Pointer to the command being
+ *                   prepared to for firmware
+ *  @param pinfo_buf HostCmd_DS_CHAN_RPT_REQ passed as void data block
+ *
+ *  @return          MLAN_STATUS_SUCCESS or MLAN_STATUS_PENDING
+ */
+static mlan_status
+wlan_11h_cmd_chan_rpt_req(mlan_private * priv,
+                          HostCmd_DS_COMMAND * pcmd_ptr,
+                          const t_void * pinfo_buf)
+{
+    const HostCmd_DS_CHAN_RPT_REQ *pchan_rpt_req =
+        (HostCmd_DS_CHAN_RPT_REQ *) pinfo_buf;
+    wlan_dfs_device_state_t *pstate_dfs = &priv->adapter->state_dfs;
+    MrvlIEtypes_ChanRpt11hBasic_t *ptlv_basic;
+
+    ENTER();
+
+    if (pstate_dfs->dfs_check_pending) {
+        PRINTM(MERROR, "11h: ChanRptReq - previous CMD_CHAN_REPORT_REQUEST has"
+               " not returned its result yet (as EVENT_CHANNEL_READY)."
+               "  This command will be dropped.\n");
+        LEAVE();
+        return MLAN_STATUS_PENDING;
+    }
+
+    /* Converted to little endian in wlan_11h_cmd_process */
+    pcmd_ptr->size = sizeof(HostCmd_DS_CHAN_RPT_REQ) + S_DS_GEN;
+
+    memcpy(priv->adapter, &pcmd_ptr->params.chan_rpt_req, pchan_rpt_req,
+           sizeof(HostCmd_DS_CHAN_RPT_REQ));
+
+    /* if DFS channel, add BASIC report TLV, and set radar bit */
+    if (wlan_11h_radar_detect_required(priv, pchan_rpt_req->chan_desc.chanNum)) {
+        ptlv_basic =
+            (MrvlIEtypes_ChanRpt11hBasic_t *) (((t_u8 *) (pcmd_ptr)) +
+                                               pcmd_ptr->size);
+        ptlv_basic->Header.type = wlan_cpu_to_le16(TLV_TYPE_CHANRPT_11H_BASIC);
+        ptlv_basic->Header.len = wlan_cpu_to_le16(sizeof(MeasRptBasicMap_t));
+        memset(priv->adapter, &ptlv_basic->map, 0, sizeof(MeasRptBasicMap_t));
+        ptlv_basic->map.radar = 1;
+        pcmd_ptr->size += sizeof(MrvlIEtypes_ChanRpt11hBasic_t);
+    }
+
+    /* update dfs sturcture. dfs_check_pending is set when we receive CMD_RESP
+       == SUCCESS */
+    pstate_dfs->dfs_check_pending = MFALSE;
+    pstate_dfs->dfs_radar_found = MFALSE;
+    pstate_dfs->dfs_check_channel = pchan_rpt_req->chan_desc.chanNum;
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Set the local power capability and constraint TLV
+ *
+ *  @param ppbuffer                The buffer to add these two TLVs
+ *  @param channel                 Channel to which the power constraint applies
+ *  @param power_constraint        Power constraint to be applied on the channel
+ *  @param min_tx_power_capability Min. Tx Power in Power Capability IE
+ *  @param max_tx_power_capability Max. Tx Power in Power Capability IE
+ *
+ *  @return                        The len increased
+ */
+static t_u32
+wlan_11h_set_local_power_constraint_tlv(t_u8 ** ppbuffer,
+                                        t_u8 channel,
+                                        t_u8 power_constraint,
+                                        t_u8 min_tx_power_capability,
+                                        t_u8 max_tx_power_capability)
+{
+    MrvlIEtypes_PowerCapability_t *pcap;
+    MrvlIEtypes_LocalPowerConstraint_t *pconstraint;
+    t_u8 *startPtr = MNULL;
+
+    ENTER();
+
+    /* Null Checks */
+    if ((ppbuffer == MNULL) || (((t_u8 *) (*ppbuffer)) == MNULL)) {
+        LEAVE();
+        return 0;
+    }
+
+    startPtr = (t_u8 *) (*ppbuffer);
+
+    PRINTM(MINFO,
+           "11h: Set local power constraint = %d channel=%d min_tx_pwr=%d max_tx_pwr=%d\n",
+           power_constraint, channel, min_tx_power_capability,
+           max_tx_power_capability);
+
+    pcap = (MrvlIEtypes_PowerCapability_t *) * ppbuffer;
+    pcap->header.type = wlan_cpu_to_le16(TLV_TYPE_POWER_CAPABILITY);
+    pcap->header.len = wlan_cpu_to_le16(2);
+    pcap->min_power = min_tx_power_capability;
+    pcap->max_power = max_tx_power_capability;
+    *ppbuffer += sizeof(MrvlIEtypesHeader_t) + 2;
+
+    pconstraint = (MrvlIEtypes_LocalPowerConstraint_t *) * ppbuffer;
+    pconstraint->header.type = wlan_cpu_to_le16(TLV_TYPE_POWER_CONSTRAINT);
+    pconstraint->header.len = wlan_cpu_to_le16(2);
+    pconstraint->chan = channel;
+    pconstraint->constraint = power_constraint;
+    *ppbuffer += sizeof(MrvlIEtypesHeader_t) + 2;
+
+    LEAVE();
+    return (t_u32) (*ppbuffer - startPtr);
+}
+
+/**
+ *  @brief  Utility function to process a join to an infrastructure BSS
+ *
+ *  @param priv          Private driver information structure
+ *  @param ppbuffer      Output parameter: Pointer to the TLV output buffer,
+ *                       modified on return to point after the appended 11h TLVs
+ *  @param band          Band on which we are joining the BSS
+ *  @param channel       Channel on which we are joining the BSS
+ *  @param p11h_bss_info Pointer to the 11h BSS information for this network
+ *                       that was parsed out of the scan response.
+ *
+ *  @return              Integer number of bytes appended to the TLV output
+ *                       buffer (ppbuffer)
+ */
+static t_u32
+wlan_11h_process_infra_join(mlan_private * priv,
+                            t_u8 ** ppbuffer,
+                            t_u8 band,
+                            t_u32 channel, wlan_11h_bss_info_t * p11h_bss_info)
+{
+    MrvlIEtypesHeader_t ie_header;
+    IEEEtypes_SupportedChannels_t sup_chan_ie;
+    t_u32 ret_len = 0;
+    t_u16 sup_chan_len = 0;
+
+    ENTER();
+
+    /* Null Checks */
+    if ((ppbuffer == MNULL) || (((t_u8 *) (*ppbuffer)) == MNULL)) {
+        LEAVE();
+        return 0;
+    }
+
+    ret_len += wlan_11h_set_local_power_constraint_tlv(ppbuffer, (t_u8) channel,
+                                                       (t_u8) p11h_bss_info->
+                                                       power_constraint.
+                                                       local_constraint,
+                                                       (t_u8) priv->adapter->
+                                                       state_11h.
+                                                       min_tx_power_capability,
+                                                       (t_u8) priv->adapter->
+                                                       state_11h.
+                                                       max_tx_power_capability);
+
+    /* Setup the Supported Channels IE */
+    sup_chan_len = wlan_11h_set_supp_channels_ie(priv, band, &sup_chan_ie);
+
+    /*
+     * If we returned a valid Supported Channels IE, wrap and append it
+     */
+    if (sup_chan_len) {
+        /* Wrap the supported channels IE with a passthrough TLV type */
+        ie_header.type = wlan_cpu_to_le16(TLV_TYPE_PASSTHROUGH);
+        ie_header.len = sup_chan_len;
+        memcpy(priv->adapter, *ppbuffer, &ie_header, sizeof(ie_header));
+
+        /* Increment the return size and the return buffer pointer param */
+        *ppbuffer += sizeof(ie_header);
+        ret_len += sizeof(ie_header);
+
+        /* Copy the supported channels IE to the output buf, advance pointer */
+        memcpy(priv->adapter, *ppbuffer, &sup_chan_ie, sup_chan_len);
+        *ppbuffer += sup_chan_len;
+        ret_len += sup_chan_len;
+    }
+
+    LEAVE();
+    return ret_len;
+}
+
+/**
+ *  @brief Utility function to process a start or join to an adhoc network
+ *
+ *  Add the elements to the TLV buffer needed in the start/join adhoc commands:
+ *       - IBSS DFS IE
+ *       - Quiet IE
+ *
+ *  Also send the local constraint to the firmware in a TPC_INFO command.
+ *
+ *  @param priv          Private driver information structure
+ *  @param ppbuffer      Output parameter: Pointer to the TLV output buffer,
+ *                       modified on return to point after the appended 11h TLVs
+ *  @param channel       Channel on which we are starting/joining the IBSS
+ *  @param p11h_bss_info Pointer to the 11h BSS information for this network
+ *                       that was parsed out of the scan response.  NULL
+ *                       indicates we are starting the adhoc network
+ *
+ *  @return              Integer number of bytes appended to the TLV output
+ *                       buffer (ppbuffer)
+ */
+static t_u32
+wlan_11h_process_adhoc(mlan_private * priv,
+                       t_u8 ** ppbuffer,
+                       t_u32 channel, wlan_11h_bss_info_t * p11h_bss_info)
+{
+    IEEEtypes_IBSS_DFS_t dfs_elem;
+    t_u32 size_appended;
+    t_u32 ret_len = 0;
+    t_s8 local_constraint = 0;
+    mlan_adapter *adapter = priv->adapter;
+
+    ENTER();
+
+#ifdef STA_SUPPORT
+    /* Format our own IBSS DFS Element.  Include our channel map fields */
+    wlan_11h_set_ibss_dfs_ie(priv, &dfs_elem);
+#endif
+
+    if (p11h_bss_info) {
+        /*
+         * Copy the DFS Owner/Recovery Interval from the BSS we are joining
+         */
+        memcpy(adapter, dfs_elem.dfs_owner,
+               p11h_bss_info->ibss_dfs.dfs_owner, sizeof(dfs_elem.dfs_owner));
+        dfs_elem.dfs_recovery_interval =
+            p11h_bss_info->ibss_dfs.dfs_recovery_interval;
+    }
+
+    /* Append the dfs element to the TLV buffer */
+    size_appended =
+        wlan_11h_convert_ieee_to_mrvl_ie(adapter, (t_u8 *) * ppbuffer,
+                                         (t_u8 *) & dfs_elem);
+
+    HEXDUMP("11h: IBSS-DFS", (t_u8 *) * ppbuffer, size_appended);
+    *ppbuffer += size_appended;
+    ret_len += size_appended;
+
+    /*
+     * Check to see if we are joining a network.  Join is indicated by the
+     *   BSS Info pointer being valid (not NULL)
+     */
+    if (p11h_bss_info) {
+        /*
+         * If there was a quiet element, include it in adhoc join command
+         */
+        if (p11h_bss_info->quiet.element_id == QUIET) {
+            size_appended
+                = wlan_11h_convert_ieee_to_mrvl_ie(adapter, (t_u8 *) * ppbuffer,
+                                                   (t_u8 *) & p11h_bss_info->
+                                                   quiet);
+            HEXDUMP("11h: Quiet", (t_u8 *) * ppbuffer, size_appended);
+            *ppbuffer += size_appended;
+            ret_len += size_appended;
+        }
+
+        /* Copy the local constraint from the network */
+        local_constraint = p11h_bss_info->power_constraint.local_constraint;
+    } else {
+        /*
+         * If we are the adhoc starter, we can add a quiet element
+         */
+        if (adapter->state_11h.quiet_ie.quiet_period) {
+            size_appended =
+                wlan_11h_convert_ieee_to_mrvl_ie(adapter, (t_u8 *) * ppbuffer,
+                                                 (t_u8 *) & adapter->state_11h.
+                                                 quiet_ie);
+            HEXDUMP("11h: Quiet", (t_u8 *) * ppbuffer, size_appended);
+            *ppbuffer += size_appended;
+            ret_len += size_appended;
+        }
+        /* Use the local_constraint configured in the driver state */
+        local_constraint = adapter->state_11h.usr_def_power_constraint;
+    }
+
+    PRINTM(MINFO, "WEILIE 1: ppbuffer = %p\n", *ppbuffer);
+
+    ret_len +=
+        wlan_11h_set_local_power_constraint_tlv(ppbuffer, (t_u8) channel,
+                                                (t_u8) local_constraint,
+                                                (t_u8) priv->adapter->state_11h.
+                                                min_tx_power_capability,
+                                                (t_u8) priv->adapter->state_11h.
+                                                max_tx_power_capability);
+    PRINTM(MINFO, "WEILIE 2: ppbuffer = %p\n", *ppbuffer);
+
+    LEAVE();
+    return ret_len;
+}
+
+/**
+ *  @brief Return whether the driver has enabled 11h for the interface
+ *
+ *  Association/Join commands are dynamic in that they enable 11h in the
+ *    driver/firmware when they are detected in the existing BSS.
+ *
+ *  @param priv  Private driver information structure
+ *
+ *  @return
+ *    - MTRUE if 11h is enabled
+ *    - MFALSE otherwise
+ */
+static t_bool
+wlan_11h_is_enabled(mlan_private * priv)
+{
+    ENTER();
+    LEAVE();
+    return (priv->intf_state_11h.is_11h_enabled);
+}
+
+/**
+ *  @brief Return whether the device has activated slave radar detection.
+ *
+ *  @param priv  Private driver information structure
+ *
+ *  @return
+ *    - MTRUE if slave radar detection is enabled in firmware
+ *    - MFALSE otherwise
+ */
+static t_bool
+wlan_11h_is_slave_radar_det_active(mlan_private * priv)
+{
+    ENTER();
+    LEAVE();
+    return (priv->adapter->state_11h.is_slave_radar_det_active);
+}
+
+/**
+ *  @brief Return whether the slave interface is active, and on DFS channel.
+ *  priv is assumed to already be a dfs slave interface, doesn't check this.
+ *
+ *  @param priv  Private driver information structure
+ *
+ *  @return
+ *    - MTRUE if priv is slave, and meets both conditions
+ *    - MFALSE otherwise
+ */
+static t_bool
+wlan_11h_is_slave_active_on_dfs_chan(mlan_private * priv)
+{
+    t_bool ret = MFALSE;
+
+    ENTER();
+    if ((priv->media_connected == MTRUE) &&
+        (priv->curr_bss_params.band & BAND_A) &&
+        wlan_11h_radar_detect_required(priv,
+                                       priv->curr_bss_params.bss_descriptor.
+                                       channel))
+        ret = MTRUE;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Return whether the master interface is active, and on DFS channel.
+ *  priv is assumed to already be a dfs master interface, doesn't check this.
+ *
+ *  @param priv  Private driver information structure
+ *
+ *  @return
+ *    - MTRUE if priv is master, and meets both conditions
+ *    - MFALSE otherwise
+ */
+static t_bool
+wlan_11h_is_master_active_on_dfs_chan(mlan_private * priv)
+{
+    t_bool ret = MFALSE;
+
+    ENTER();
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) {
+        /* Ad-hoc creator */
+        if (((priv->media_connected == MTRUE)
+             || (priv->adhoc_state == ADHOC_STARTING)) &&
+            (priv->adapter->adhoc_start_band & BAND_A) &&
+            wlan_11h_radar_detect_required(priv, priv->adhoc_channel))
+            ret = MTRUE;
+    } else if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP) {
+        /* UAP */
+#ifdef UAP_SUPPORT
+        if ((priv->uap_bss_started == MTRUE) &&
+            (priv->uap_state_chan_cb.band_config & BAND_CONFIG_5GHZ) &&
+            wlan_11h_radar_detect_required(priv,
+                                           priv->uap_state_chan_cb.channel))
+            ret = MTRUE;
+#endif
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Determine if priv is DFS Master interface
+ *
+ *  @param priv Pointer to mlan_private
+ *
+ *  @return MTRUE or MFALSE
+ */
+static t_bool
+wlan_11h_is_dfs_master(mlan_private * priv)
+{
+    t_bool ret = MFALSE;
+
+    ENTER();
+    /* UAP: all are master */
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP) {
+        ret = MTRUE;
+    }
+    /* STA: only ad-hoc creator is master */
+    else if ((GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) &&
+             (priv->bss_mode == MLAN_BSS_MODE_IBSS) &&
+             (priv->adhoc_state == ADHOC_STARTED ||
+              priv->adhoc_state == ADHOC_STARTING)) {
+        ret = MTRUE;
+    }
+    /* all other cases = slave interface */
+    LEAVE();
+    return ret;
+}
+
+/* Need this as function to pass to wlan_count_priv_cond() */
+/**
+ *  @brief Determine if priv is DFS Slave interface
+ *
+ *  @param priv Pointer to mlan_private
+ *
+ *  @return MTRUE or MFALSE
+ */
+
+static t_bool
+wlan_11h_is_dfs_slave(mlan_private * priv)
+{
+    t_bool ret = MFALSE;
+    ENTER();
+    ret = !wlan_11h_is_dfs_master(priv);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function checks if interface is active.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *
+ *  @return             MTRUE or MFALSE
+ */
+static t_bool
+wlan_is_intf_active(mlan_private * pmpriv)
+{
+    t_bool ret = MFALSE;
+    ENTER();
+
+#ifdef UAP_SUPPORT
+    if (GET_BSS_ROLE(pmpriv) == MLAN_BSS_ROLE_UAP)
+        /* NOTE: UAP's media_connected == true only after first STA associated.
+           Need different variable to tell if UAP has been started. */
+        ret = pmpriv->uap_bss_started;
+    else
+#endif
+    if (GET_BSS_ROLE(pmpriv) == MLAN_BSS_ROLE_STA)
+        ret = pmpriv->media_connected;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function gets current radar detect flags
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *
+ *  @return             11H MIB setting for radar detect
+ */
+static t_u32
+wlan_11h_get_current_radar_detect_flags(mlan_adapter * pmadapter)
+{
+    t_u32 radar_det_flags = 0;
+
+    ENTER();
+    if (pmadapter->state_11h.is_master_radar_det_active)
+        radar_det_flags |= MASTER_RADAR_DET_MASK;
+    if (pmadapter->state_11h.is_slave_radar_det_active)
+        radar_det_flags |= SLAVE_RADAR_DET_MASK;
+
+    PRINTM(MINFO, "%s: radar_det_state_curr=0x%x\n",
+           __FUNCTION__, radar_det_flags);
+
+    LEAVE();
+    return radar_det_flags;
+}
+
+/**
+ *  @brief This function checks if radar detect flags have/should be changed.
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pnew_state   Output param with new state, if return MTRUE.
+ *
+ *  @return             MTRUE (need update) or MFALSE (no change in flags)
+ */
+static t_bool
+wlan_11h_check_radar_det_state(mlan_adapter * pmadapter, OUT t_u32 * pnew_state)
+{
+    t_u32 radar_det_state_new = 0;
+    t_bool ret;
+
+    ENTER();
+    PRINTM(MINFO, "%s: master_radar_det_pending=%d, "
+           " slave_radar_det_pending=%d\n", __FUNCTION__,
+           pmadapter->state_11h.master_radar_det_enable_pending,
+           pmadapter->state_11h.slave_radar_det_enable_pending);
+
+    /* new state comes from evaluating interface states & pending starts */
+    if (pmadapter->state_11h.master_radar_det_enable_pending ||
+        (wlan_count_priv_cond(pmadapter,
+                              wlan_11h_is_master_active_on_dfs_chan,
+                              wlan_11h_is_dfs_master) > 0))
+        radar_det_state_new |= MASTER_RADAR_DET_MASK;
+    if (pmadapter->state_11h.slave_radar_det_enable_pending ||
+        (wlan_count_priv_cond(pmadapter,
+                              wlan_11h_is_slave_active_on_dfs_chan,
+                              wlan_11h_is_dfs_slave) > 0))
+        radar_det_state_new |= SLAVE_RADAR_DET_MASK;
+
+    PRINTM(MINFO, "%s: radar_det_state_new=0x%x\n",
+           __FUNCTION__, radar_det_state_new);
+
+    /* now compare flags with current state */
+    ret = (wlan_11h_get_current_radar_detect_flags(pmadapter)
+           != radar_det_state_new) ? MTRUE : MFALSE;
+    if (ret)
+        *pnew_state = radar_det_state_new;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Determine if mlan_private list only contains UAP interface(s)
+ *
+ *  @param priv_list        List of mlan_private pointers
+ *  @param priv_list_count  Number of mlan_privates in above list
+ *
+ *  @return MTRUE or MFALSE
+ */
+static t_bool
+wlan_only_uap_priv_in_list(mlan_private ** priv_list, t_u8 priv_list_count)
+{
+#if defined(STA_SUPPORT) && !defined(UAP_SUPPORT)
+    return MFALSE;
+#else
+    t_u8 uap_count = 0;
+    t_u8 sta_count = 0;
+    t_u8 i;
+
+    ENTER();
+    for (i = 0; i < priv_list_count; i++) {
+        if (GET_BSS_ROLE(priv_list[i]) == MLAN_BSS_ROLE_UAP)
+            uap_count++;
+        else
+            sta_count++;
+    }
+
+    LEAVE();
+    return ((uap_count > 0) && (sta_count == 0)) ? MTRUE : MFALSE;
+#endif
+}
+
+/**
+ *  @brief Prepare ioctl for add/remove CHAN_SW IE - RADAR_DETECTED event handling
+ *
+ *  @param pmadapter        Pointer to mlan_adapter
+ *  @param pioctl_req       Pointer to completed mlan_ioctl_req (allocated inside)
+ *  @param is_adding_ie     CHAN_SW IE is to be added (MTRUE), or removed (MFALSE)
+ *
+ *  @return                 MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_11h_prepare_custom_ie_chansw(IN mlan_adapter * pmadapter,
+                                  OUT mlan_ioctl_req ** ppioctl_req,
+                                  IN t_bool is_adding_ie)
+{
+    mlan_ioctl_req *pioctl_req = MNULL;
+    mlan_ds_misc_cfg *pds_misc_cfg = MNULL;
+    custom_ie *pcust_chansw_ie = MNULL;
+    IEEEtypes_ChanSwitchAnn_t *pchansw_ie = MNULL;
+    mlan_status ret;
+
+    ENTER();
+
+    if (pmadapter == MNULL || ppioctl_req == MNULL) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    /* allocate buffer for mlan_ioctl_req and mlan_ds_misc_cfg */
+    /* FYI - will be freed as part of cmd_response handler */
+    ret = pmadapter->callbacks.moal_malloc(pmadapter->pmoal_handle,
+                                           sizeof(mlan_ioctl_req) +
+                                           sizeof(mlan_ds_misc_cfg),
+                                           MLAN_MEM_DEF,
+                                           (t_u8 **) & pioctl_req);
+    if ((ret != MLAN_STATUS_SUCCESS) || !pioctl_req) {
+        PRINTM(MERROR, "%s(): Could not allocate ioctl req\n", __FUNCTION__);
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    pds_misc_cfg = (mlan_ds_misc_cfg *) ((t_u8 *) pioctl_req +
+                                         sizeof(mlan_ioctl_req));
+
+    /* prepare mlan_ioctl_req */
+    memset(pmadapter, pioctl_req, 0x00, sizeof(mlan_ioctl_req));
+    pioctl_req->req_id = MLAN_IOCTL_MISC_CFG;
+    pioctl_req->action = MLAN_ACT_SET;
+    pioctl_req->pbuf = (t_u8 *) pds_misc_cfg;
+    pioctl_req->buf_len = sizeof(mlan_ds_misc_cfg);
+
+    /* prepare mlan_ds_misc_cfg */
+    memset(pmadapter, pds_misc_cfg, 0x00, sizeof(mlan_ds_misc_cfg));
+    pds_misc_cfg->sub_command = MLAN_OID_MISC_CUSTOM_IE;
+    pds_misc_cfg->param.cust_ie.type = TLV_TYPE_MGMT_IE;
+    pds_misc_cfg->param.cust_ie.len = (sizeof(custom_ie) - MAX_IE_SIZE);
+
+    /* configure custom_ie api settings */
+    pcust_chansw_ie =
+        (custom_ie *) & pds_misc_cfg->param.cust_ie.ie_data_list[0];
+    pcust_chansw_ie->ie_index = 0xffff; /* Auto index */
+    pcust_chansw_ie->ie_length = sizeof(IEEEtypes_ChanSwitchAnn_t);
+    pcust_chansw_ie->mgmt_subtype_mask = (is_adding_ie)
+        ? MBIT(8) | MBIT(5)     /* add IE for BEACON | PROBE_RSP */
+        : 0;                    /* remove IE */
+
+    /* prepare CHAN_SW IE inside ioctl */
+    pchansw_ie = (IEEEtypes_ChanSwitchAnn_t *) pcust_chansw_ie->ie_buffer;
+    pchansw_ie->element_id = CHANNEL_SWITCH_ANN;
+    pchansw_ie->len =
+        sizeof(IEEEtypes_ChanSwitchAnn_t) - sizeof(IEEEtypes_Header_t);
+    pchansw_ie->chan_switch_mode = 1;   /* STA should not transmit */
+    pchansw_ie->new_channel_num = pmadapter->state_rdh.new_channel;
+    pchansw_ie->chan_switch_count = 0;  /* simplification */
+
+    pds_misc_cfg->param.cust_ie.len += pcust_chansw_ie->ie_length;
+    DBG_HEXDUMP(MCMD_D, "11h: custom_ie containing CHAN_SW IE",
+                (t_u8 *) pcust_chansw_ie, pds_misc_cfg->param.cust_ie.len);
+
+    /* assign output pointer before returning */
+    *ppioctl_req = pioctl_req;
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+#ifdef UAP_SUPPORT
+/**
+ *  @brief Retrieve a randomly selected starting channel if needed for 11h
+ *
+ *  If 11h is enabled and 5GHz band is selected in band_config
+ *    return a random channel in A band, else one from BG band.
+ *
+ *  @param priv          Private driver information structure
+ *  @param uap_band_cfg  Private driver information structure
+ *
+ *  @return      Starting channel
+ */
+static t_u8
+wlan_11h_get_uap_start_channel(mlan_private * priv, t_u8 uap_band_cfg)
+{
+    t_u8 start_chn;
+    mlan_adapter *adapter = priv->adapter;
+    t_u32 region;
+    t_u32 rand_entry;
+    region_chan_t *chn_tbl;
+    t_u8 rand_tries = 0;
+
+    // TODO: right now mostly a copy of wlan_11h_get_adhoc_start_channel.
+    // Improve to be more specfic to UAP, e.g.
+    // 1. take into account COUNTRY_CODE -> region_code
+    // 2. check domain_info for value channels
+
+    ENTER();
+
+    /*
+     * Set start_chn to the Default.  Used if 11h is disabled or the band
+     *   does not require 11h support.
+     */
+    start_chn = DEFAULT_AD_HOC_CHANNEL;
+
+    /*
+     * Check that we are looking for a channel in the A Band
+     */
+    if (uap_band_cfg & UAP_BAND_CONFIG_5GHZ) {
+        /*
+         * Set default to the A Band default. Used if random selection fails
+         *   or if 11h is not enabled
+         */
+        start_chn = DEFAULT_AD_HOC_CHANNEL_A;
+
+        /*
+         * Check that 11h is enabled in the driver
+         */
+        if (wlan_11h_is_enabled(priv)) {
+            /*
+             * Search the region_channel tables for a channel table
+             *   that is marked for the A Band.
+             */
+            for (region = 0; (region < MAX_REGION_CHANNEL_NUM); region++) {
+                chn_tbl = &adapter->region_channel[region];
+
+                /* Check if table is valid and marked for A Band */
+                if (chn_tbl->valid
+                    && chn_tbl->region == adapter->region_code
+                    && chn_tbl->band & BAND_A) {
+                    /*
+                     * Set the start channel.  Get a random number and
+                     *   use it to pick an entry in the table between 0
+                     *   and the number of channels in the table (NumCFP).
+                     */
+                    do {
+                        rand_entry =
+                            wlan_11h_get_random_num(adapter) % chn_tbl->num_cfp;
+                        start_chn = (t_u8) chn_tbl->pcfp[rand_entry].channel;
+                    } while (wlan_11h_is_channel_under_nop(adapter, start_chn)
+                             && (++rand_tries < MAX_RANDOM_CHANNEL_RETRIES));
+                }
+            }
+        }
+    }
+
+    PRINTM(MCMD_D, "11h: UAP Get Start Channel %d\n", start_chn);
+    LEAVE();
+    return start_chn;
+}
+#endif /* UAP_SUPPORT */
+
+#ifdef DEBUG_LEVEL1
+static const char *DFS_TS_REPR_STRINGS[] = { "",
+    "NOP_start",
+    "CAC_completed"
+};
+#endif
+
+/**
+ *  @brief Search for a dfs timestamp in the list with desired channel.
+ *
+ *  Assumes there will only be one timestamp per channel in the list.
+ *
+ *  @param pmadapter  Pointer to mlan_adapter
+ *  @param channel    Channel number
+ *
+ *  @return           Pointer to timestamp if found, or MNULL
+ */
+static wlan_dfs_timestamp_t *
+wlan_11h_find_dfs_timestamp(mlan_adapter * pmadapter, t_u8 channel)
+{
+    wlan_dfs_timestamp_t *pts = MNULL, *pts_found = MNULL;
+
+    ENTER();
+    pts = (wlan_dfs_timestamp_t *) util_peek_list(pmadapter->pmoal_handle,
+                                                  &pmadapter->state_dfs.
+                                                  dfs_ts_head, MNULL, MNULL);
+
+    while (pts &&
+           pts != (wlan_dfs_timestamp_t *) & pmadapter->state_dfs.dfs_ts_head) {
+        PRINTM(MINFO,
+               "dfs_timestamp(@ %p) - chan=%d, repr=%d(%s),"
+               " time(sec.usec)=%lu.%06lu\n", pts, pts->channel,
+               pts->represents, DFS_TS_REPR_STRINGS[pts->represents],
+               pts->ts_sec, pts->ts_usec);
+
+        if (pts->channel == channel) {
+            pts_found = pts;
+            break;
+        }
+        pts = pts->pnext;
+    }
+
+    LEAVE();
+    return pts_found;
+}
+
+/**
+ *  @brief Removes dfs timestamp from list.
+ *
+ *  @param pmadapter  Pointer to mlan_adapter
+ *  @param pdfs_ts    Pointer to dfs_timestamp to remove
+ */
+static t_void
+wlan_11h_remove_dfs_timestamp(mlan_adapter * pmadapter,
+                              wlan_dfs_timestamp_t * pdfs_ts)
+{
+    ENTER();
+    // dequeue and delete timestamp
+    util_unlink_list(pmadapter->pmoal_handle,
+                     &pmadapter->state_dfs.dfs_ts_head,
+                     (pmlan_linked_list) pdfs_ts, MNULL, MNULL);
+    pmadapter->callbacks.moal_mfree(pmadapter->pmoal_handle, (t_u8 *) pdfs_ts);
+    LEAVE();
+}
+
+/**
+ *  @brief Add a dfs timestamp to the list
+ *
+ *  Assumes there will only be one timestamp per channel in the list,
+ *  and that timestamp modes (represents) are mutually exclusive.
+ *
+ *  @param pmadapter  Pointer to mlan_adapter
+ *  @param repr       Timestamp 'represents' value (see _dfs_timestamp_repr_e)
+ *  @param channel    Channel number
+ *
+ *  @return           Pointer to timestamp if found, or MNULL
+ */
+static mlan_status
+wlan_11h_add_dfs_timestamp(mlan_adapter * pmadapter, t_u8 repr, t_u8 channel)
+{
+    wlan_dfs_timestamp_t *pdfs_ts = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    pdfs_ts = wlan_11h_find_dfs_timestamp(pmadapter, channel);
+
+    if (!pdfs_ts) {
+        // need to allocate new timestamp
+        ret = pmadapter->callbacks.moal_malloc(pmadapter->pmoal_handle,
+                                               sizeof(wlan_dfs_timestamp_t),
+                                               MLAN_MEM_DEF,
+                                               (t_u8 **) & pdfs_ts);
+        if ((ret != MLAN_STATUS_SUCCESS) || !pdfs_ts) {
+            PRINTM(MERROR, "%s(): Could not allocate dfs_ts\n", __FUNCTION__);
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+
+        memset(pmadapter, (t_u8 *) pdfs_ts, 0, sizeof(wlan_dfs_timestamp_t));
+
+        util_enqueue_list_tail(pmadapter->pmoal_handle,
+                               &pmadapter->state_dfs.dfs_ts_head,
+                               (pmlan_linked_list) pdfs_ts, MNULL, MNULL);
+        pdfs_ts->channel = channel;
+    }
+    // (else, use existing timestamp for channel; see assumptions above)
+
+    // update params
+    pmadapter->callbacks.moal_get_system_time(pmadapter->pmoal_handle,
+                                              &pdfs_ts->ts_sec,
+                                              &pdfs_ts->ts_usec);
+    pdfs_ts->represents = repr;
+
+    PRINTM(MCMD_D, "11h: add/update dfs_timestamp - chan=%d, repr=%d(%s),"
+           " time(sec.usec)=%lu.%06lu\n", pdfs_ts->channel,
+           pdfs_ts->represents, DFS_TS_REPR_STRINGS[pdfs_ts->represents],
+           pdfs_ts->ts_sec, pdfs_ts->ts_usec);
+
+    LEAVE();
+    return ret;
+}
+
+/********************************************************
+                Global functions
+********************************************************/
+
+/**
+ *  @brief Return whether the device has activated master radar detection.
+ *
+ *  @param priv  Private driver information structure
+ *
+ *  @return
+ *    - MTRUE if master radar detection is enabled in firmware
+ *    - MFALSE otherwise
+ */
+t_bool
+wlan_11h_is_master_radar_det_active(mlan_private * priv)
+{
+    ENTER();
+    LEAVE();
+    return (priv->adapter->state_11h.is_master_radar_det_active);
+}
+
+/**
+ *  @brief Configure master radar detection.
+ *  Call wlan_11h_check_update_radar_det_state() afterwards
+ *    to push this to firmware.
+ *
+ *  @param priv  Private driver information structure
+ *  @param enable Whether to enable or disable master radar detection
+ *
+ *  @return  MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ *
+ *  @sa wlan_11h_check_update_radar_det_state
+ */
+mlan_status
+wlan_11h_config_master_radar_det(mlan_private * priv, t_bool enable)
+{
+    mlan_status ret = MLAN_STATUS_FAILURE;
+
+    ENTER();
+    if (wlan_11h_is_dfs_master(priv) &&
+        priv->adapter->init_para.dfs_master_radar_det_en) {
+        priv->adapter->state_11h.master_radar_det_enable_pending = enable;
+        ret = MLAN_STATUS_SUCCESS;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Configure slave radar detection.
+ *  Call wlan_11h_check_update_radar_det_state() afterwards
+ *    to push this to firmware.
+ *
+ *  @param priv  Private driver information structure
+ *  @param enable Whether to enable or disable slave radar detection
+ *
+ *  @return  MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ *
+ *  @sa wlan_11h_check_update_radar_det_state
+ */
+mlan_status
+wlan_11h_config_slave_radar_det(mlan_private * priv, t_bool enable)
+{
+    mlan_status ret = MLAN_STATUS_FAILURE;
+
+    ENTER();
+    if (wlan_11h_is_dfs_slave(priv) &&
+        priv->adapter->init_para.dfs_slave_radar_det_en) {
+        priv->adapter->state_11h.slave_radar_det_enable_pending = enable;
+        ret = MLAN_STATUS_SUCCESS;
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Checks all interfaces and determines if radar_detect flag states
+ *         have/should be changed.  If so, sends SNMP_MIB 11H command to FW.
+ *         Call this function on any interface enable/disable/channel change.
+ *
+ *  @param pmpriv  Pointer to mlan_private structure
+ *
+ *  @return        MLAN_STATUS_SUCCESS (update or not)
+ *              or MLAN_STATUS_FAILURE (cmd failure)
+ *
+ *  @sa    wlan_11h_check_radar_det_state
+ */
+mlan_status
+wlan_11h_check_update_radar_det_state(mlan_private * pmpriv)
+{
+    t_u32 new_radar_det_state = 0;
+    t_u32 mib_11h = 0;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    if (wlan_11h_check_radar_det_state(pmpriv->adapter, &new_radar_det_state)) {
+        PRINTM(MCMD_D, "%s: radar_det_state being updated.\n", __FUNCTION__);
+
+        mib_11h |= new_radar_det_state;
+        /* keep priv's existing 11h state */
+        if (pmpriv->intf_state_11h.is_11h_active)
+            mib_11h |= ENABLE_11H_MASK;
+
+        /* Send cmd to FW to enable/disable 11h function in firmware */
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_802_11_SNMP_MIB,
+                               HostCmd_ACT_GEN_SET, Dot11H_i, MNULL, &mib_11h);
+        if (ret)
+            ret = MLAN_STATUS_FAILURE;
+    }
+
+    /* updated state sent OR no change, thus no longer pending */
+    pmpriv->adapter->state_11h.master_radar_det_enable_pending = MFALSE;
+    pmpriv->adapter->state_11h.slave_radar_det_enable_pending = MFALSE;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Query 11h firmware enabled state.
+ *
+ *  Return whether the firmware currently has 11h extensions enabled
+ *
+ *  @param priv  Private driver information structure
+ *
+ *  @return
+ *    - MTRUE if 11h has been activated in the firmware
+ *    - MFALSE otherwise
+ *
+ *  @sa wlan_11h_activate
+ */
+t_bool
+wlan_11h_is_active(mlan_private * priv)
+{
+    ENTER();
+    LEAVE();
+    return (priv->intf_state_11h.is_11h_active);
+}
+
+/**
+ *  @brief Enable the transmit interface and record the state.
+ *
+ *  @param priv  Private driver information structure
+ *
+ *  @return      N/A
+ */
+t_void
+wlan_11h_tx_enable(mlan_private * priv)
+{
+    ENTER();
+    if (priv->intf_state_11h.tx_disabled) {
+        wlan_recv_event(priv, MLAN_EVENT_ID_FW_START_TX, MNULL);
+        priv->intf_state_11h.tx_disabled = MFALSE;
+    }
+    LEAVE();
+}
+
+/**
+ *  @brief Disable the transmit interface and record the state.
+ *
+ *  @param priv  Private driver information structure
+ *
+ *  @return      N/A
+ */
+t_void
+wlan_11h_tx_disable(mlan_private * priv)
+{
+    ENTER();
+    if (!priv->intf_state_11h.tx_disabled) {
+        priv->intf_state_11h.tx_disabled = MTRUE;
+        wlan_recv_event(priv, MLAN_EVENT_ID_FW_STOP_TX, MNULL);
+    }
+    LEAVE();
+}
+
+/**
+ *  @brief Enable or Disable the 11h extensions in the firmware
+ *
+ *  @param priv         Private driver information structure
+ *  @param pioctl_buf   A pointer to MLAN IOCTL Request buffer
+ *  @param flag         Enable 11h if MTRUE, disable otherwise
+ *
+ *  @return      MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_11h_activate(mlan_private * priv, t_void * pioctl_buf, t_bool flag)
+{
+    t_u32 enable = flag & ENABLE_11H_MASK;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    /* add bits for master/slave radar detect into enable. */
+    enable |= wlan_11h_get_current_radar_detect_flags(priv->adapter);
+
+    /*
+     * Send cmd to FW to enable/disable 11h function in firmware
+     */
+    ret = wlan_prepare_cmd(priv,
+                           HostCmd_CMD_802_11_SNMP_MIB,
+                           HostCmd_ACT_GEN_SET,
+                           Dot11H_i, (t_void *) pioctl_buf, &enable);
+    if (ret)
+        ret = MLAN_STATUS_FAILURE;
+    else
+        /* Set boolean flag in driver 11h state */
+        priv->intf_state_11h.is_11h_active = flag;
+
+    PRINTM(MINFO, "11h: %s\n", flag ? "Activate" : "Deactivate");
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Initialize the 11h parameters and enable 11h when starting an IBSS
+ *
+ *  @param adapter mlan_adapter structure
+ *
+ *  @return      N/A
+ */
+t_void
+wlan_11h_init(mlan_adapter * adapter)
+{
+    wlan_11h_device_state_t *pstate_11h = &adapter->state_11h;
+    IEEEtypes_Quiet_t *pquiet = &adapter->state_11h.quiet_ie;
+    wlan_dfs_device_state_t *pstate_dfs = &adapter->state_dfs;
+    wlan_radar_det_hndlg_state_t *pstate_rdh = &adapter->state_rdh;
+#ifdef DFS_TESTING_SUPPORT
+    wlan_dfs_testing_settings_t *pdfs_test = &adapter->dfs_test_params;
+#endif
+
+    ENTER();
+
+    /* Initialize 11H struct */
+    pstate_11h->usr_def_power_constraint = WLAN_11H_TPC_POWERCONSTRAINT;
+    pstate_11h->min_tx_power_capability = WLAN_11H_TPC_POWERCAPABILITY_MIN;
+    pstate_11h->max_tx_power_capability = WLAN_11H_TPC_POWERCAPABILITY_MAX;
+
+    pstate_11h->recvd_chanswann_event = MFALSE;
+    pstate_11h->master_radar_det_enable_pending = MFALSE;
+    pstate_11h->slave_radar_det_enable_pending = MFALSE;
+    pstate_11h->is_master_radar_det_active = MFALSE;
+    pstate_11h->is_slave_radar_det_active = MFALSE;
+
+    /* Initialize quiet_ie */
+    memset(adapter, pquiet, 0, sizeof(IEEEtypes_Quiet_t));
+    pquiet->element_id = QUIET;
+    pquiet->len = (sizeof(pquiet->quiet_count) + sizeof(pquiet->quiet_period)
+                   + sizeof(pquiet->quiet_duration)
+                   + sizeof(pquiet->quiet_offset));
+
+    /* Initialize DFS struct */
+    pstate_dfs->dfs_check_pending = MFALSE;
+    pstate_dfs->dfs_radar_found = MFALSE;
+    pstate_dfs->dfs_check_channel = 0;
+    pstate_dfs->dfs_report_time_sec = 0;
+    util_init_list((pmlan_linked_list) & pstate_dfs->dfs_ts_head);
+
+    /* Initialize RDH struct */
+    pstate_rdh->stage = RDH_OFF;
+    pstate_rdh->priv_list_count = 0;
+    pstate_rdh->priv_curr_idx = 0;
+    pstate_rdh->curr_channel = 0;
+    pstate_rdh->new_channel = 0;
+    pstate_rdh->uap_band_cfg = 0;
+    pstate_rdh->max_bcn_dtim_ms = 0;
+    memset(adapter, pstate_rdh->priv_list, 0, sizeof(pstate_rdh->priv_list));
+
+#ifdef DFS_TESTING_SUPPORT
+    /* Initialize DFS testing struct */
+    pdfs_test->user_cac_period_msec = 0;
+    pdfs_test->user_nop_period_sec = 0;
+    pdfs_test->no_channel_change_on_radar = MFALSE;
+    pdfs_test->fixed_new_channel_on_radar = 0;
+#endif
+
+    LEAVE();
+}
+
+/**
+ *  @brief Cleanup for the 11h parameters that allocated memory, etc.
+ *
+ *  @param adapter mlan_adapter structure
+ *
+ *  @return      N/A
+ */
+t_void
+wlan_11h_cleanup(mlan_adapter * adapter)
+{
+    wlan_dfs_device_state_t *pstate_dfs = &adapter->state_dfs;
+    wlan_dfs_timestamp_t *pdfs_ts;
+
+    ENTER();
+
+    /* cleanup dfs_timestamp list */
+    pdfs_ts = (wlan_dfs_timestamp_t *) util_peek_list(adapter->pmoal_handle,
+                                                      &pstate_dfs->dfs_ts_head,
+                                                      MNULL, MNULL);
+    while (pdfs_ts) {
+        util_unlink_list(adapter->pmoal_handle, &pstate_dfs->dfs_ts_head,
+                         (pmlan_linked_list) pdfs_ts, MNULL, MNULL);
+        adapter->callbacks.moal_mfree(adapter->pmoal_handle, (t_u8 *) pdfs_ts);
+
+        pdfs_ts = (wlan_dfs_timestamp_t *) util_peek_list(adapter->pmoal_handle,
+                                                          &pstate_dfs->
+                                                          dfs_ts_head, MNULL,
+                                                          MNULL);
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief Initialize the 11h parameters and enable 11h when starting an IBSS
+ *
+ *  @param pmpriv Pointer to mlan_private structure
+ *
+ *  @return      N/A
+ */
+t_void
+wlan_11h_priv_init(mlan_private * pmpriv)
+{
+    wlan_11h_interface_state_t *pistate_11h = &pmpriv->intf_state_11h;
+
+    ENTER();
+
+    pistate_11h->is_11h_enabled = MTRUE;
+    pistate_11h->is_11h_active = MFALSE;
+    pistate_11h->adhoc_auto_sel_chan = MTRUE;
+    pistate_11h->tx_disabled = MFALSE;
+
+    LEAVE();
+}
+
+/**
+ *  @brief Retrieve a randomly selected starting channel if needed for 11h
+ *
+ *  If 11h is enabled and an A-Band channel start band preference
+ *    configured in the driver, the start channel must be random in order
+ *    to meet with
+ *
+ *  @param priv  Private driver information structure
+ *
+ *  @return      Starting channel
+ */
+t_u8
+wlan_11h_get_adhoc_start_channel(mlan_private * priv)
+{
+    t_u8 start_chn;
+    mlan_adapter *adapter = priv->adapter;
+    t_u32 region;
+    t_u32 rand_entry;
+    region_chan_t *chn_tbl;
+    t_u8 rand_tries = 0;
+
+    ENTER();
+
+    /*
+     * Set start_chn to the Default.  Used if 11h is disabled or the band
+     *   does not require 11h support.
+     */
+    start_chn = DEFAULT_AD_HOC_CHANNEL;
+
+    /*
+     * Check that we are looking for a channel in the A Band
+     */
+    if ((adapter->adhoc_start_band & BAND_A)
+        || (adapter->adhoc_start_band & BAND_AN)
+        ) {
+        /*
+         * Set default to the A Band default. Used if random selection fails
+         *   or if 11h is not enabled
+         */
+        start_chn = DEFAULT_AD_HOC_CHANNEL_A;
+
+        /*
+         * Check that 11h is enabled in the driver
+         */
+        if (wlan_11h_is_enabled(priv)) {
+            /*
+             * Search the region_channel tables for a channel table
+             *   that is marked for the A Band.
+             */
+            for (region = 0; (region < MAX_REGION_CHANNEL_NUM); region++) {
+                chn_tbl = &adapter->region_channel[region];
+
+                /* Check if table is valid and marked for A Band */
+                if (chn_tbl->valid
+                    && chn_tbl->region == adapter->region_code
+                    && chn_tbl->band & BAND_A) {
+                    /*
+                     * Set the start channel.  Get a random number and
+                     *   use it to pick an entry in the table between 0
+                     *   and the number of channels in the table (NumCFP).
+                     */
+                    do {
+                        rand_entry =
+                            wlan_11h_get_random_num(adapter) % chn_tbl->num_cfp;
+                        start_chn = (t_u8) chn_tbl->pcfp[rand_entry].channel;
+                    } while ((wlan_11h_is_channel_under_nop(adapter, start_chn)
+                              ||
+                              ((adapter->state_rdh.stage ==
+                                RDH_GET_INFO_CHANNEL) &&
+                               wlan_11h_radar_detect_required(priv, start_chn)))
+                             && (++rand_tries < MAX_RANDOM_CHANNEL_RETRIES));
+                }
+            }
+        }
+    }
+
+    PRINTM(MINFO, "11h: %s: AdHoc Channel set to %u\n",
+           wlan_11h_is_enabled(priv) ? "Enabled" : "Disabled", start_chn);
+
+    LEAVE();
+    return start_chn;
+}
+
+/**
+ *  @brief Check if the current region's regulations require the input channel
+ *         to be scanned for radar.
+ *
+ *  Based on statically defined requirements for sub-bands per regulatory
+ *    agency requirements.
+ *
+ *  Used in adhoc start to determine if channel availability check is required
+ *
+ *  @param priv    Private driver information structure
+ *  @param channel Channel to determine radar detection requirements
+ *
+ *  @return
+ *    - MTRUE if radar detection is required
+ *    - MFALSE otherwise
+ */
+/**  @sa wlan_11h_issue_radar_detect
+ */
+t_bool
+wlan_11h_radar_detect_required(mlan_private * priv, t_u8 channel)
+{
+    t_bool required = MFALSE;
+
+    ENTER();
+
+    /*
+     *   No checks for 11h or measurement code being enabled is placed here
+     *   since regulatory requirements exist whether we support them or not.
+     */
+
+    required = wlan_get_cfp_radar_detect(priv, channel);
+
+    if (!priv->adapter->region_code)
+        PRINTM(MINFO, "11h: Radar detection in CFP code[BG:%#x, A:%#x] "
+               "is %srequired for channel %d\n",
+               priv->adapter->cfp_code_bg, priv->adapter->cfp_code_a,
+               (required ? "" : "not "), channel);
+    else
+        PRINTM(MINFO, "11h: Radar detection in region %#02x "
+               "is %srequired for channel %d\n",
+               priv->adapter->region_code, (required ? "" : "not "), channel);
+
+    if (required == MTRUE && priv->media_connected == MTRUE
+        && priv->curr_bss_params.bss_descriptor.channel == channel) {
+        required = MFALSE;
+
+        PRINTM(MINFO, "11h: Radar detection not required. "
+               "Already operating on the channel\n");
+    }
+
+    LEAVE();
+    return required;
+}
+
+/**
+ *  @brief Perform a radar measurement if required on given channel
+ *
+ *  Check to see if the provided channel requires a channel availability
+ *    check (60 second radar detection measurement).  If required, perform
+ *    measurement, stalling calling thread until the measurement completes
+ *    and then report result.
+ *
+ *  Used when starting an adhoc or AP network.
+ *
+ *  @param priv         Private driver information structure
+ *  @param pioctl_req   Pointer to IOCTL request buffer
+ *  @param channel      Channel on which to perform radar measurement
+ *
+ *  @return
+ *    - MTRUE  if radar measurement request was successfully issued
+ *    - MFALSE if radar detection is not required
+ *    - < 0 for error during radar detection (if performed)
+ *
+ *  @sa wlan_11h_radar_detect_required
+ */
+t_s32
+wlan_11h_issue_radar_detect(mlan_private * priv,
+                            pmlan_ioctl_req pioctl_req, t_u8 channel)
+{
+    t_s32 ret;
+    HostCmd_DS_CHAN_RPT_REQ chan_rpt_req;
+
+    ENTER();
+
+    ret = wlan_11h_radar_detect_required(priv, channel);
+    if (ret) {
+        /* Prepare and issue CMD_CHAN_RPT_REQ. */
+        memset(priv->adapter, &chan_rpt_req, 0x00, sizeof(chan_rpt_req));
+
+        chan_rpt_req.chan_desc.startFreq = START_FREQ_11A_BAND;
+        chan_rpt_req.chan_desc.chanWidth = 0;   // 1 for 40Mhz
+        chan_rpt_req.chan_desc.chanNum = channel;
+        chan_rpt_req.millisec_dwell_time =
+            WLAN_11H_CHANNEL_AVAIL_CHECK_DURATION;
+#ifdef DFS_TESTING_SUPPORT
+        if (priv->adapter->dfs_test_params.user_cac_period_msec) {
+            PRINTM(MCMD_D, "dfs_testing - user CAC period=%d (msec)\n",
+                   priv->adapter->dfs_test_params.user_cac_period_msec);
+            chan_rpt_req.millisec_dwell_time =
+                priv->adapter->dfs_test_params.user_cac_period_msec;
+        }
+#endif
+
+        PRINTM(MMSG, "11h: issuing DFS Radar check for channel=%d."
+               "  Please wait for response...\n", channel);
+
+        ret = wlan_prepare_cmd(priv, HostCmd_CMD_CHAN_REPORT_REQUEST,
+                               HostCmd_ACT_GEN_SET, 0,
+                               (t_void *) pioctl_req,
+                               (t_void *) & chan_rpt_req);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Checks if a radar measurement was performed on channel,
+ *         and if so, whether radar was detected on it.
+ *
+ *  Used when starting an adhoc network.
+ *
+ *  @param priv         Private driver information structure
+ *  @param chan         Channel to check upon
+ *
+ *  @return
+ *    - MLAN_STATUS_SUCCESS if no radar on channel
+ *    - MLAN_STATUS_FAILURE if radar was found on channel
+ *    - (TBD??) MLAN_STATUS_PENDING if radar report NEEDS TO BE REISSUED
+ *
+ *  @sa wlan_11h_issue_radar_detect
+ *  @sa wlan_11h_process_start
+ */
+mlan_status
+wlan_11h_check_chan_report(mlan_private * priv, t_u8 chan)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    wlan_dfs_device_state_t *pstate_dfs = &priv->adapter->state_dfs;
+    t_u32 sec, usec;
+
+    ENTER();
+
+    /* check report we hold is valid or not */
+    priv->adapter->callbacks.moal_get_system_time(priv->adapter->pmoal_handle,
+                                                  &sec, &usec);
+
+    PRINTM(MINFO, "11h: %s()\n", __FUNCTION__);
+    PRINTM(MINFO, "- sec_now=%d, sec_report=%d.\n",
+           sec, pstate_dfs->dfs_report_time_sec);
+    PRINTM(MINFO, "- rpt_channel=%d, rpt_radar=%d.\n",
+           pstate_dfs->dfs_check_channel, pstate_dfs->dfs_radar_found);
+
+    if ((!pstate_dfs->dfs_check_pending) &&
+        (chan == pstate_dfs->dfs_check_channel) &&
+        ((sec - pstate_dfs->dfs_report_time_sec) <
+         MAX_DFS_REPORT_USABLE_AGE_SEC)) {
+        /* valid and not out-dated, check if radar */
+        if (pstate_dfs->dfs_radar_found) {
+            PRINTM(MMSG, "Radar was detected on channel %d.\n", chan);
+            ret = MLAN_STATUS_FAILURE;
+        }
+    } else {
+        // TODO: reissue report request if not pending.
+        // BUT HOW to make the code wait for it???
+        /* For now, just fail since we don't have the info. */
+        ret = MLAN_STATUS_PENDING;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Process an TLV buffer for a pending BSS Adhoc start command.
+ *
+ *  Activate 11h functionality in the firmware if driver has is enabled
+ *    for 11h (configured by the application via IOCTL).
+ *
+ *  @param priv          Private driver information structure
+ *  @param ppbuffer      Output parameter: Pointer to the TLV output buffer,
+ *                       modified on return to point after the appended 11h TLVs
+ *  @param pcap_info     Pointer to the capability info for the BSS to join
+ *  @param channel       Channel on which we are starting the IBSS
+ *  @param p11h_bss_info Input/Output parameter: Pointer to the 11h BSS
+ *                       information for this network that we are establishing.
+ *                       11h sensed flag set on output if warranted.
+ *
+ *  @return
+ *      - MLAN_STATUS_SUCCESS if 11h is disabled
+ *      - Integer number of bytes appended to the TLV output buffer (ppbuffer)
+ *      - < 0 for error (e.g. radar detected on channel)
+ */
+t_s32
+wlan_11h_process_start(mlan_private * priv,
+                       t_u8 ** ppbuffer,
+                       IEEEtypes_CapInfo_t * pcap_info,
+                       t_u32 channel, wlan_11h_bss_info_t * p11h_bss_info)
+{
+    mlan_adapter *adapter = priv->adapter;
+    t_s32 ret = MLAN_STATUS_SUCCESS;
+    t_bool is_dfs_chan = MFALSE;
+
+    ENTER();
+    if (wlan_11h_is_enabled(priv)
+        && ((adapter->adhoc_start_band & BAND_A)
+            || (adapter->adhoc_start_band & BAND_AN)
+        )
+        ) {
+        if (!wlan_11d_is_enabled(priv)) {
+            /* No use having 11h enabled without 11d enabled */
+            wlan_11d_enable(priv, MNULL, ENABLE_11D);
+#ifdef STA_SUPPORT
+            wlan_11d_create_dnld_countryinfo(priv, adapter->adhoc_start_band);
+#endif
+        }
+
+        /* Activate 11h functions in firmware, turns on capability bit */
+        wlan_11h_activate(priv, MNULL, MTRUE);
+        pcap_info->spectrum_mgmt = MTRUE;
+
+        /* If using a DFS channel, enable radar detection. */
+        is_dfs_chan = wlan_11h_radar_detect_required(priv, channel);
+        if (is_dfs_chan) {
+            if (!wlan_11h_is_master_radar_det_active(priv))
+                wlan_11h_config_master_radar_det(priv, MTRUE);
+        }
+        wlan_11h_check_update_radar_det_state(priv);
+
+        /* Set flag indicating this BSS we are starting is using 11h */
+        p11h_bss_info->sensed_11h = MTRUE;
+
+        if (is_dfs_chan) {
+            /* check if this channel is under NOP */
+            if (wlan_11h_is_channel_under_nop(adapter, channel))
+                ret = MLAN_STATUS_FAILURE;
+            /* check last channel report, if this channel is free of radar */
+            if (ret == MLAN_STATUS_SUCCESS)
+                ret = wlan_11h_check_chan_report(priv, channel);
+        }
+        if (ret == MLAN_STATUS_SUCCESS)
+            ret = wlan_11h_process_adhoc(priv, ppbuffer, channel, MNULL);
+        else
+            ret = MLAN_STATUS_FAILURE;
+    } else {
+        /* Deactivate 11h functions in the firmware */
+        wlan_11h_activate(priv, MNULL, MFALSE);
+        pcap_info->spectrum_mgmt = MFALSE;
+        wlan_11h_check_update_radar_det_state(priv);
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Process an TLV buffer for a pending BSS Join command for
+ *         both adhoc and infra networks
+ *
+ *  The TLV command processing for a BSS join for either adhoc or
+ *    infrastructure network is performed with this function.  The
+ *    capability bits are inspected for the IBSS flag and the appropriate
+ *    local routines are called to setup the necessary TLVs.
+ *
+ *  Activate 11h functionality in the firmware if the spectrum management
+ *    capability bit is found in the network information for the BSS we are
+ *    joining.
+ *
+ *  @param priv          Private driver information structure
+ *  @param ppbuffer      Output parameter: Pointer to the TLV output buffer,
+ *                       modified on return to point after the appended 11h TLVs
+ *  @param pcap_info     Pointer to the capability info for the BSS to join
+ *  @param band          Band on which we are joining the BSS
+ *  @param channel       Channel on which we are joining the BSS
+ *  @param p11h_bss_info Pointer to the 11h BSS information for this
+ *                       network that was parsed out of the scan response.
+ *
+ *  @return              Integer number of bytes appended to the TLV output
+ *                       buffer (ppbuffer), MLAN_STATUS_FAILURE (-1),
+ *                       or MLAN_STATUS_SUCCESS (0)
+ */
+t_s32
+wlan_11h_process_join(mlan_private * priv,
+                      t_u8 ** ppbuffer,
+                      IEEEtypes_CapInfo_t * pcap_info,
+                      t_u8 band,
+                      t_u32 channel, wlan_11h_bss_info_t * p11h_bss_info)
+{
+    t_s32 ret = 0;
+
+    ENTER();
+
+    if (priv->media_connected == MTRUE) {
+        if (wlan_11h_is_active(priv) == p11h_bss_info->sensed_11h) {
+            /* Assume DFS parameters are the same for roaming as long as the
+               current & next APs have the same spectrum mgmt capability bit
+               setting */
+            ret = MLAN_STATUS_SUCCESS;
+
+        } else {
+            /* No support for roaming between DFS/non-DFS yet */
+            ret = MLAN_STATUS_FAILURE;
+        }
+
+        LEAVE();
+        return ret;
+    }
+
+    if (p11h_bss_info->sensed_11h) {
+        if (!wlan_11d_is_enabled(priv)) {
+            /* No use having 11h enabled without 11d enabled */
+            wlan_11d_enable(priv, MNULL, ENABLE_11D);
+#ifdef STA_SUPPORT
+            wlan_11d_parse_dnld_countryinfo(priv, priv->pattempted_bss_desc);
+#endif
+        }
+        /* Activate 11h functions in firmware, turns on capability bit */
+        wlan_11h_activate(priv, MNULL, MTRUE);
+        pcap_info->spectrum_mgmt = MTRUE;
+
+        /* If using a DFS channel, enable radar detection. */
+        if ((band & BAND_A) && wlan_11h_radar_detect_required(priv, channel)) {
+            if (!wlan_11h_is_slave_radar_det_active(priv))
+                wlan_11h_config_slave_radar_det(priv, MTRUE);
+        }
+        wlan_11h_check_update_radar_det_state(priv);
+
+        if (pcap_info->ibss) {
+            PRINTM(MINFO, "11h: Adhoc join: Sensed\n");
+            ret = wlan_11h_process_adhoc(priv, ppbuffer, channel,
+                                         p11h_bss_info);
+        } else {
+            PRINTM(MINFO, "11h: Infra join: Sensed\n");
+            ret = wlan_11h_process_infra_join(priv, ppbuffer, band,
+                                              channel, p11h_bss_info);
+        }
+    } else {
+        /* Deactivate 11h functions in the firmware */
+        wlan_11h_activate(priv, MNULL, MFALSE);
+        pcap_info->spectrum_mgmt = MFALSE;
+        wlan_11h_check_update_radar_det_state(priv);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *
+ *  @brief  Prepare the HostCmd_DS_Command structure for an 11h command.
+ *
+ *  Use the Command field to determine if the command being set up is for
+ *     11h and call one of the local command handlers accordingly for:
+ *
+ *        - HostCmd_CMD_802_11_TPC_ADAPT_REQ
+ *        - HostCmd_CMD_802_11_TPC_INFO
+ *        - HostCmd_CMD_802_11_CHAN_SW_ANN
+ */
+/**       - HostCmd_CMD_CHAN_REPORT_REQUEST
+ */
+/**
+ *  @param priv      Private driver information structure
+ *  @param pcmd_ptr  Output parameter: Pointer to the command being prepared
+ *                   for the firmware
+ *  @param pinfo_buf Void buffer pass through with data necessary for a
+ *                   specific command type
+ */
+/**  @return          MLAN_STATUS_SUCCESS, MLAN_STATUS_FAILURE or MLAN_STATUS_PENDING
+ */
+/**  @sa wlan_11h_cmd_tpc_request
+ *  @sa wlan_11h_cmd_tpc_info
+ *  @sa wlan_11h_cmd_chan_sw_ann
+ */
+/** @sa wlan_11h_cmd_chan_report_req
+ */
+mlan_status
+wlan_11h_cmd_process(mlan_private * priv,
+                     HostCmd_DS_COMMAND * pcmd_ptr, const t_void * pinfo_buf)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    switch (pcmd_ptr->command) {
+    case HostCmd_CMD_802_11_TPC_ADAPT_REQ:
+        ret = wlan_11h_cmd_tpc_request(priv, pcmd_ptr, pinfo_buf);
+        break;
+    case HostCmd_CMD_802_11_TPC_INFO:
+        ret = wlan_11h_cmd_tpc_info(priv, pcmd_ptr, pinfo_buf);
+        break;
+    case HostCmd_CMD_802_11_CHAN_SW_ANN:
+        ret = wlan_11h_cmd_chan_sw_ann(priv, pcmd_ptr, pinfo_buf);
+        break;
+    case HostCmd_CMD_CHAN_REPORT_REQUEST:
+        ret = wlan_11h_cmd_chan_rpt_req(priv, pcmd_ptr, pinfo_buf);
+        break;
+    default:
+        ret = MLAN_STATUS_FAILURE;
+    }
+
+    pcmd_ptr->command = wlan_cpu_to_le16(pcmd_ptr->command);
+    pcmd_ptr->size = wlan_cpu_to_le16(pcmd_ptr->size);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Handle the command response from the firmware if from an 11h command
+ *
+ *  Use the Command field to determine if the command response being
+ *    is for 11h.  Call the local command response handler accordingly for:
+ *
+ *        - HostCmd_CMD_802_11_TPC_ADAPT_REQ
+ *        - HostCmd_CMD_802_11_TPC_INFO
+ *        - HostCmd_CMD_802_11_CHAN_SW_ANN
+ */
+/**       - HostCmd_CMD_CHAN_REPORT_REQUEST
+ */
+/**
+ *  @param priv  Private driver information structure
+ *  @param resp  HostCmd_DS_COMMAND struct returned from the firmware
+ *               command
+ *
+ *  @return      MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_11h_cmdresp_process(mlan_private * priv, const HostCmd_DS_COMMAND * resp)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    switch (resp->command) {
+    case HostCmd_CMD_802_11_TPC_ADAPT_REQ:
+        HEXDUMP("11h: TPC REQUEST Rsp:", (t_u8 *) resp, (t_u32) resp->size);
+        memcpy(priv->adapter, priv->adapter->curr_cmd->pdata_buf,
+               &resp->params.tpc_req, sizeof(HostCmd_DS_802_11_TPC_ADAPT_REQ));
+        break;
+
+    case HostCmd_CMD_802_11_TPC_INFO:
+        HEXDUMP("11h: TPC INFO Rsp Data:", (t_u8 *) resp, (t_u32) resp->size);
+        break;
+
+    case HostCmd_CMD_802_11_CHAN_SW_ANN:
+        PRINTM(MINFO, "11h: Ret ChSwAnn: Sz=%u, Seq=%u, Ret=%u\n",
+               resp->size, resp->seq_num, resp->result);
+        break;
+
+    case HostCmd_CMD_CHAN_REPORT_REQUEST:
+        PRINTM(MINFO, "11h: Ret ChanRptReq.  Set dfs_check_pending and wait"
+               " for EVENT_CHANNEL_REPORT.\n");
+        priv->adapter->state_dfs.dfs_check_pending = MTRUE;
+        break;
+
+    default:
+        ret = MLAN_STATUS_FAILURE;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Process an element from a scan response, copy relevant info for 11h
+ *
+ *  @param pmadapter Pointer to mlan_adapter
+ *  @param p11h_bss_info Output parameter: Pointer to the 11h BSS information
+ *                       for the network that is being processed
+ *  @param pelement      Pointer to the current IE we are inspecting for 11h
+ *                       relevance
+ *
+ *  @return              MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_11h_process_bss_elem(mlan_adapter * pmadapter,
+                          wlan_11h_bss_info_t * p11h_bss_info,
+                          const t_u8 * pelement)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u8 element_len = *((t_u8 *) pelement + 1);
+
+    ENTER();
+    switch (*pelement) {
+    case POWER_CONSTRAINT:
+        PRINTM(MINFO, "11h: Power Constraint IE Found\n");
+        p11h_bss_info->sensed_11h = MTRUE;
+        memcpy(pmadapter, &p11h_bss_info->power_constraint, pelement,
+               MIN((element_len + sizeof(IEEEtypes_Header_t)),
+                   sizeof(IEEEtypes_PowerConstraint_t)));
+        p11h_bss_info->power_constraint.len =
+            MIN(element_len, (sizeof(IEEEtypes_PowerConstraint_t)
+                              - sizeof(IEEEtypes_Header_t)));
+        break;
+
+    case POWER_CAPABILITY:
+        PRINTM(MINFO, "11h: Power Capability IE Found\n");
+        p11h_bss_info->sensed_11h = MTRUE;
+        memcpy(pmadapter, &p11h_bss_info->power_capability, pelement,
+               MIN((element_len + sizeof(IEEEtypes_Header_t)),
+                   sizeof(IEEEtypes_PowerCapability_t)));
+        p11h_bss_info->power_capability.len =
+            MIN(element_len, (sizeof(IEEEtypes_PowerCapability_t)
+                              - sizeof(IEEEtypes_Header_t)));
+        break;
+
+    case TPC_REPORT:
+        PRINTM(MINFO, "11h: Tpc Report IE Found\n");
+        p11h_bss_info->sensed_11h = MTRUE;
+        memcpy(pmadapter, &p11h_bss_info->tpc_report, pelement,
+               MIN((element_len + sizeof(IEEEtypes_Header_t)),
+                   sizeof(IEEEtypes_TPCReport_t)));
+        p11h_bss_info->tpc_report.len =
+            MIN(element_len, (sizeof(IEEEtypes_TPCReport_t)
+                              - sizeof(IEEEtypes_Header_t)));
+        break;
+
+    case CHANNEL_SWITCH_ANN:
+        PRINTM(MINFO, "11h: Channel Switch Ann IE Found\n");
+        p11h_bss_info->sensed_11h = MTRUE;
+        memcpy(pmadapter, &p11h_bss_info->chan_switch_ann, pelement,
+               MIN((element_len + sizeof(IEEEtypes_Header_t)),
+                   sizeof(IEEEtypes_ChanSwitchAnn_t)));
+        p11h_bss_info->chan_switch_ann.len =
+            MIN(element_len, (sizeof(IEEEtypes_ChanSwitchAnn_t)
+                              - sizeof(IEEEtypes_Header_t)));
+        break;
+
+    case QUIET:
+        PRINTM(MINFO, "11h: Quiet IE Found\n");
+        p11h_bss_info->sensed_11h = MTRUE;
+        memcpy(pmadapter, &p11h_bss_info->quiet, pelement,
+               MIN((element_len + sizeof(IEEEtypes_Header_t)),
+                   sizeof(IEEEtypes_Quiet_t)));
+        p11h_bss_info->quiet.len = MIN(element_len, (sizeof(IEEEtypes_Quiet_t)
+                                                     -
+                                                     sizeof
+                                                     (IEEEtypes_Header_t)));
+        break;
+
+    case IBSS_DFS:
+        PRINTM(MINFO, "11h: Ibss Dfs IE Found\n");
+        p11h_bss_info->sensed_11h = MTRUE;
+        memcpy(pmadapter, &p11h_bss_info->ibss_dfs, pelement,
+               MIN((element_len + sizeof(IEEEtypes_Header_t)),
+                   sizeof(IEEEtypes_IBSS_DFS_t)));
+        p11h_bss_info->ibss_dfs.len =
+            MIN(element_len, (sizeof(IEEEtypes_IBSS_DFS_t)
+                              - sizeof(IEEEtypes_Header_t)));
+        break;
+
+    case SUPPORTED_CHANNELS:
+    case TPC_REQUEST:
+        /*
+         * These elements are not in beacons/probe responses.  Included here
+         *   to cover set of enumerated 11h elements.
+         */
+        break;
+
+    default:
+        ret = MLAN_STATUS_FAILURE;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Driver handling for CHANNEL_SWITCH_ANN event
+ *
+ *  @param priv Pointer to mlan_private
+ *
+ *  @return MLAN_STATUS_SUCCESS, MLAN_STATUS_FAILURE or MLAN_STATUS_PENDING
+ */
+mlan_status
+wlan_11h_handle_event_chanswann(mlan_private * priv)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    ENTER();
+    priv->adapter->state_11h.recvd_chanswann_event = MTRUE;
+
+#ifdef STA_SUPPORT
+    /* do directed deauth.  priv flag above will cause different reason code */
+    PRINTM(MINFO, "11h: handle_event_chanswann() - sending deauth\n");
+    ret = wlan_disconnect(priv, MNULL,
+                          &priv->curr_bss_params.bss_descriptor.mac_address);
+
+    /* clear region table so next scan will be all passive */
+    PRINTM(MINFO, "11h: handle_event_chanswann() - clear region table\n");
+    wlan_11d_clear_parsedtable(priv);
+#endif
+
+    priv->adapter->state_11h.recvd_chanswann_event = MFALSE;
+    LEAVE();
+    return ret;
+}
+
+#ifdef DFS_TESTING_SUPPORT
+/**
+ *  @brief 802.11h DFS Testing configuration
+ *
+ *  @param pmadapter    Pointer to mlan_adapter
+ *  @param pioctl_req   Pointer to mlan_ioctl_req
+ *
+ *  @return MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_11h_ioctl_dfs_testing(pmlan_adapter pmadapter, pmlan_ioctl_req pioctl_req)
+{
+    mlan_ds_11h_cfg *ds_11hcfg = MNULL;
+    mlan_ds_11h_dfs_testing *dfs_test = MNULL;
+    wlan_dfs_testing_settings_t *pdfs_test_params = MNULL;
+
+    ENTER();
+
+    ds_11hcfg = (mlan_ds_11h_cfg *) pioctl_req->pbuf;
+    dfs_test = &ds_11hcfg->param.dfs_testing;
+    pdfs_test_params = &pmadapter->dfs_test_params;
+
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        dfs_test->usr_cac_period_msec = pdfs_test_params->user_cac_period_msec;
+        dfs_test->usr_nop_period_sec = pdfs_test_params->user_nop_period_sec;
+        dfs_test->usr_no_chan_change =
+            pdfs_test_params->no_channel_change_on_radar;
+        dfs_test->usr_fixed_new_chan =
+            pdfs_test_params->fixed_new_channel_on_radar;
+    } else {
+        pdfs_test_params->user_cac_period_msec = dfs_test->usr_cac_period_msec;
+        pdfs_test_params->user_nop_period_sec = dfs_test->usr_nop_period_sec;
+        pdfs_test_params->no_channel_change_on_radar =
+            dfs_test->usr_no_chan_change;
+        pdfs_test_params->fixed_new_channel_on_radar =
+            dfs_test->usr_fixed_new_chan;
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+#endif // DFS_TESTING_SUPPORT
+
+/**
+ *  @brief Check if channel is under NOP (Non-Occupancy Period)
+ *  If so, the channel should not be used until the period expires.
+ *
+ *  @param pmadapter  Pointer to mlan_adapter
+ *  @param channel    Channel number
+ *
+ *  @return MTRUE or MFALSE
+ */
+t_bool
+wlan_11h_is_channel_under_nop(mlan_adapter * pmadapter, t_u8 channel)
+{
+    wlan_dfs_timestamp_t *pdfs_ts = MNULL;
+    t_u32 now_sec, now_usec;
+    t_bool ret = MFALSE;
+
+    ENTER();
+    pdfs_ts = wlan_11h_find_dfs_timestamp(pmadapter, channel);
+
+    if (pdfs_ts && (pdfs_ts->channel == channel)
+        && (pdfs_ts->represents == DFS_TS_REPR_NOP_START)) {
+        /* found NOP_start timestamp entry on channel */
+        pmadapter->callbacks.moal_get_system_time(pmadapter->pmoal_handle,
+                                                  &now_sec, &now_usec);
+#ifdef DFS_TESTING_SUPPORT
+        if (pmadapter->dfs_test_params.user_nop_period_sec) {
+            PRINTM(MCMD_D, "dfs_testing - user NOP period=%d (sec)\n",
+                   pmadapter->dfs_test_params.user_nop_period_sec);
+            if ((now_sec - pdfs_ts->ts_sec) <=
+                pmadapter->dfs_test_params.user_nop_period_sec) {
+                ret = MTRUE;
+            }
+        } else
+#endif
+        {
+            if ((now_sec - pdfs_ts->ts_sec) <= WLAN_11H_NON_OCCUPANCY_PERIOD)
+                ret = MTRUE;
+        }
+
+        /* if entry is expired, remove it */
+        if (!ret)
+            wlan_11h_remove_dfs_timestamp(pmadapter, pdfs_ts);
+        else
+            PRINTM(MMSG, "11h: channel %d is under NOP - can't use.\n",
+                   channel);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Driver handling for CHANNEL_REPORT_RDY event
+ *  This event will have the channel report data appended.
+ *
+ *  @param priv     Pointer to mlan_private
+ *  @param pevent   Pointer to mlan_event
+ *
+ *  @return MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_11h_handle_event_chanrpt_ready(mlan_private * priv, mlan_event * pevent)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    HostCmd_DS_CHAN_RPT_RSP *pChanRptRsp;
+    MrvlIEtypes_Data_t *pTlv;
+    MeasRptBasicMap_t *pMeasRptBasic;
+    t_u8 *pBuffer;
+    t_s32 evtLen;
+    t_u16 tlvLen;
+    t_u32 sec, uSec;
+    wlan_dfs_device_state_t *pstate_dfs = &priv->adapter->state_dfs;
+
+    ENTER();
+    pChanRptRsp = (HostCmd_DS_CHAN_RPT_RSP *) & pevent->event_buf;
+    DBG_HEXDUMP(MCMD_D, "11h: Event ChanRptReady (HostCmd_DS_CHAN_RPT_RSP)",
+                (t_u8 *) pChanRptRsp, wlan_le32_to_cpu(pevent->event_len));
+
+    if (wlan_le32_to_cpu(pChanRptRsp->cmd_result) == MLAN_CMD_RESULT_SUCCESS) {
+        pBuffer = (t_u8 *) & pChanRptRsp->tlv_buffer;
+        evtLen = wlan_le32_to_cpu(pevent->event_len);
+        evtLen -=
+            sizeof(HostCmd_DS_CHAN_RPT_RSP) - sizeof(pChanRptRsp->tlv_buffer);
+
+        while (evtLen >= sizeof(MrvlIEtypesHeader_t)) {
+            pTlv = (MrvlIEtypes_Data_t *) pBuffer;
+            tlvLen = wlan_le16_to_cpu(pTlv->header.len);
+
+            switch (wlan_le16_to_cpu(pTlv->header.type)) {
+            case TLV_TYPE_CHANRPT_11H_BASIC:
+                pMeasRptBasic = (MeasRptBasicMap_t *) & pTlv->data;
+                if (pMeasRptBasic->radar) {
+                    pstate_dfs->dfs_radar_found = MTRUE;
+                    PRINTM(MMSG, "RADAR Detected on channel %d!\n",
+                           pstate_dfs->dfs_check_channel);
+                    /* add channel to NOP list */
+                    wlan_11h_add_dfs_timestamp(priv->adapter,
+                                               DFS_TS_REPR_NOP_START,
+                                               pstate_dfs->dfs_check_channel);
+                }
+                break;
+
+            default:
+                break;
+            }
+
+            pBuffer += (tlvLen + sizeof(pTlv->header));
+            evtLen -= (tlvLen + sizeof(pTlv->header));
+            evtLen = (evtLen > 0) ? evtLen : 0;
+        }
+    } else {
+        ret = MLAN_STATUS_FAILURE;
+    }
+
+    /* Update DFS structure. */
+    priv->adapter->callbacks.moal_get_system_time(priv->adapter->pmoal_handle,
+                                                  &sec, &uSec);
+    pstate_dfs->dfs_report_time_sec = sec;
+    pstate_dfs->dfs_check_pending = MFALSE;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Check if RADAR_DETECTED handling is blocking data tx
+ *
+ *  @param pmadapter    Pointer to mlan_adapter
+ *
+ *  @return MTRUE or MFALSE
+ */
+t_bool
+wlan_11h_radar_detected_tx_blocked(mlan_adapter * pmadapter)
+{
+    switch (pmadapter->state_rdh.stage) {
+    case RDH_OFF:
+    case RDH_CHK_INTFS:
+    case RDH_STOP_TRAFFIC:
+        return MFALSE;
+    }
+    return MTRUE;
+}
+
+/**
+ *  @brief Callback for RADAR_DETECTED event driver handling
+ *
+ *  @param priv    Void pointer to mlan_private
+ *
+ *  @return MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_11h_radar_detected_callback(t_void * priv)
+{
+    mlan_status ret;
+    ENTER();
+    ret = wlan_11h_radar_detected_handling(((mlan_private *) (priv))->adapter);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Driver handling for RADAR_DETECTED event
+ *
+ *  @param pmadapter    Pointer to mlan_adapter
+ *
+ *  @return MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE or MLAN_STATUS_PENDING
+ */
+mlan_status
+wlan_11h_radar_detected_handling(mlan_adapter * pmadapter)
+{
+#ifdef DEBUG_LEVEL1
+    const char *RDH_stage_str[] = {
+        "RDH_OFF",
+        "RDH_CHK_INTFS",
+        "RDH_STOP_TRAFFIC",
+        "RDH_GET_INFO_CHANNEL",
+        "RDH_GET_INFO_BEACON_DTIM",
+        "RDH_SET_CUSTOM_IE",
+        "RDH_REM_CUSTOM_IE",
+        "RDH_STOP_INTFS",
+        "RDH_SET_NEW_CHANNEL",
+        "RDH_RESTART_INTFS",
+        "RDH_RESTART_TRAFFIC"
+    };
+#endif
+
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = MNULL;
+    t_u32 i;
+    wlan_radar_det_hndlg_state_t *pstate_rdh = &pmadapter->state_rdh;
+
+    ENTER();
+
+    switch (pstate_rdh->stage) {
+    case RDH_CHK_INTFS:
+        PRINTM(MCMD_D, "%s(): stage(%d)=%s\n",
+               __FUNCTION__, pstate_rdh->stage,
+               RDH_stage_str[pstate_rdh->stage]);
+
+        /* get active interfaces */
+        memset(pmadapter, pstate_rdh->priv_list, 0x00,
+               sizeof(pstate_rdh->priv_list));
+        pstate_rdh->priv_list_count = wlan_get_privs_by_cond(pmadapter,
+                                                             wlan_is_intf_active,
+                                                             pstate_rdh->
+                                                             priv_list);
+        PRINTM(MCMD_D, "%s():  priv_list_count = %d\n", __FUNCTION__,
+               pstate_rdh->priv_list_count);
+        for (i = 0; i < pstate_rdh->priv_list_count; i++)
+            PRINTM(MINFO, "%s():  priv_list[%d] = %p\n",
+                   __FUNCTION__, i, pstate_rdh->priv_list[i]);
+
+        if (pstate_rdh->priv_list_count == 0) {
+            /* no interfaces active... nothing to do */
+            PRINTM(MMSG, "11h: Radar Detected - no active priv's,"
+                   " skip event handling.\n");
+            pstate_rdh->stage = RDH_OFF;
+            PRINTM(MCMD_D, "%s(): finished - stage(%d)=%s\n",
+                   __FUNCTION__, pstate_rdh->stage,
+                   RDH_stage_str[pstate_rdh->stage]);
+            break;              // EXIT CASE
+        }
+        // else: start handling
+        pstate_rdh->curr_channel = 0;
+        pstate_rdh->new_channel = 0;
+        pstate_rdh->uap_band_cfg = 0;
+        pstate_rdh->max_bcn_dtim_ms = 0;
+        pstate_rdh->priv_curr_idx = RDH_STAGE_FIRST_ENTRY_PRIV_IDX;
+        pstate_rdh->stage = RDH_STOP_TRAFFIC;
+        // FALL THROUGH TO NEXT STAGE
+
+    case RDH_STOP_TRAFFIC:
+        PRINTM(MCMD_D, "%s(): stage(%d)=%s\n",
+               __FUNCTION__, pstate_rdh->stage,
+               RDH_stage_str[pstate_rdh->stage]);
+
+        PRINTM(MMSG, "11h: Radar Detected - stopping host tx traffic.\n");
+        for (i = 0; i < pstate_rdh->priv_list_count; i++) {
+            wlan_11h_tx_disable(pstate_rdh->priv_list[i]);
+        }
+
+        pstate_rdh->priv_curr_idx = RDH_STAGE_FIRST_ENTRY_PRIV_IDX;
+        pstate_rdh->stage = RDH_GET_INFO_CHANNEL;
+        // FALL THROUGH TO NEXT STAGE
+
+    case RDH_GET_INFO_CHANNEL:
+        PRINTM(MCMD_D, "%s(): stage(%d)=%s, priv_idx=%d\n",
+               __FUNCTION__, pstate_rdh->stage,
+               RDH_stage_str[pstate_rdh->stage], pstate_rdh->priv_curr_idx);
+
+        /* here, prefer STA info over UAP info - one less CMD to send */
+        if (pstate_rdh->priv_curr_idx == RDH_STAGE_FIRST_ENTRY_PRIV_IDX) {
+            if (wlan_only_uap_priv_in_list(pstate_rdh->priv_list,
+                                           pstate_rdh->priv_list_count)) {
+#ifdef UAP_SUPPORT
+                /* Assume all UAPs on same channel, use first UAP */
+                pmpriv = pstate_rdh->priv_list[0];
+                pstate_rdh->priv_curr_idx = 0;
+                /* send cmd to get first UAP's info */
+                pmpriv->uap_state_chan_cb.pioctl_req_curr = MNULL;
+                pmpriv->uap_state_chan_cb.get_chan_callback
+                    = wlan_11h_radar_detected_callback;
+                ret = wlan_uap_get_channel(pmpriv);
+                break;          // EXIT CASE
+#endif
+            } else {
+                /* Assume all STAs on same channel, find first STA */
+                MASSERT(pstate_rdh->priv_list_count > 0);
+                for (i = 0; i < pstate_rdh->priv_list_count; i++) {
+                    pmpriv = pstate_rdh->priv_list[i];
+                    if (GET_BSS_ROLE(pmpriv) == MLAN_BSS_ROLE_STA)
+                        break;
+                }
+                /* STA info kept in driver, just copy */
+                pstate_rdh->curr_channel =
+                    pmpriv->curr_bss_params.bss_descriptor.channel;
+            }
+        }
+#ifdef UAP_SUPPORT
+        else if (pstate_rdh->priv_curr_idx < pstate_rdh->priv_list_count) {
+            /* repeat entry: UAP return with info */
+            pmpriv = pstate_rdh->priv_list[pstate_rdh->priv_curr_idx];
+            pstate_rdh->curr_channel = pmpriv->uap_state_chan_cb.channel;
+            pstate_rdh->uap_band_cfg = pmpriv->uap_state_chan_cb.band_config;
+            PRINTM(MCMD_D, "%s():  uap_band_cfg=0x%02x\n",
+                   __FUNCTION__, pstate_rdh->uap_band_cfg);
+        }
+#endif
+
+        /* add channel to NOP list */
+        wlan_11h_add_dfs_timestamp(pmadapter, DFS_TS_REPR_NOP_START,
+                                   pstate_rdh->curr_channel);
+
+        /* choose new channel (!= curr channel) and move on */
+        i = 0;
+        do {
+#ifdef UAP_SUPPORT
+            if (GET_BSS_ROLE(pmpriv) == MLAN_BSS_ROLE_UAP)
+                pstate_rdh->new_channel = wlan_11h_get_uap_start_channel(pmpriv,
+                                                                         pmpriv->
+                                                                         uap_state_chan_cb.
+                                                                         band_config);
+            else
+#endif
+                pstate_rdh->new_channel =
+                    wlan_11h_get_adhoc_start_channel(pmpriv);
+        } while ((pstate_rdh->new_channel == pstate_rdh->curr_channel) && (++i < MAX_RANDOM_CHANNEL_RETRIES));  /* avoid
+                                                                                                                   deadloop
+                                                                                                                 */
+        if (i >= MAX_RANDOM_CHANNEL_RETRIES)    /* report error */
+            PRINTM(MERROR, "%s():  ERROR - could not choose new_chan"
+                   " (!= curr_chan) !!\n", __FUNCTION__);
+
+#ifdef DFS_TESTING_SUPPORT
+        if (pmadapter->dfs_test_params.fixed_new_channel_on_radar) {
+            PRINTM(MCMD_D, "dfs_testing - user fixed new_chan=%d\n",
+                   pmadapter->dfs_test_params.fixed_new_channel_on_radar);
+            pstate_rdh->new_channel =
+                pmadapter->dfs_test_params.fixed_new_channel_on_radar;
+        }
+#endif
+        PRINTM(MCMD_D, "%s():  curr_chan=%d, new_chan=%d\n",
+               __FUNCTION__, pstate_rdh->curr_channel, pstate_rdh->new_channel);
+
+        pstate_rdh->priv_curr_idx = RDH_STAGE_FIRST_ENTRY_PRIV_IDX;
+        pstate_rdh->stage = RDH_GET_INFO_BEACON_DTIM;
+        // FALL THROUGH TO NEXT STAGE
+
+    case RDH_GET_INFO_BEACON_DTIM:
+        PRINTM(MCMD_D, "%s(): stage(%d)=%s, priv_idx=%d\n",
+               __FUNCTION__, pstate_rdh->stage,
+               RDH_stage_str[pstate_rdh->stage], pstate_rdh->priv_curr_idx);
+
+#ifdef UAP_SUPPORT
+        /* check all intfs in this stage to find longest period */
+        /* UAP intf callback returning with info */
+        if (pstate_rdh->priv_curr_idx < pstate_rdh->priv_list_count) {
+            t_u16 bcn_dtim_msec;
+            pmpriv = pstate_rdh->priv_list[pstate_rdh->priv_curr_idx];
+            PRINTM(MCMD_D, "%s():  uap.bcn_pd=%d, uap.dtim_pd=%d\n",
+                   __FUNCTION__, pmpriv->uap_state_chan_cb.beacon_period,
+                   pmpriv->uap_state_chan_cb.dtim_period);
+            bcn_dtim_msec = (pmpriv->uap_state_chan_cb.beacon_period
+                             * pmpriv->uap_state_chan_cb.dtim_period);
+            if (bcn_dtim_msec > pstate_rdh->max_bcn_dtim_ms)
+                pstate_rdh->max_bcn_dtim_ms = bcn_dtim_msec;
+        }
+#endif
+
+        /* check next intf */
+        while ((++pstate_rdh->priv_curr_idx) < pstate_rdh->priv_list_count) {
+            pmpriv = pstate_rdh->priv_list[pstate_rdh->priv_curr_idx];
+
+#ifdef UAP_SUPPORT
+            if (GET_BSS_ROLE(pmpriv) == MLAN_BSS_ROLE_UAP) {
+                pmpriv->uap_state_chan_cb.pioctl_req_curr = MNULL;
+                pmpriv->uap_state_chan_cb.get_chan_callback
+                    = wlan_11h_radar_detected_callback;
+                ret = wlan_uap_get_beacon_dtim(pmpriv);
+                break;          // leads to exit case
+            } else
+#endif
+            {                   /* get STA info from driver and compare here */
+                t_u16 bcn_pd_msec = 100;
+                t_u16 dtim_pd_msec = 1;
+                t_u16 bcn_dtim_msec;
+
+                if (wlan_11h_is_dfs_master(pmpriv)) {   /* adhoc creator */
+                    bcn_pd_msec = pmpriv->beacon_period;
+                } else {
+                    bcn_pd_msec =
+                        pmpriv->curr_bss_params.bss_descriptor.beacon_period;
+                    // if (priv->bss_mode != MLAN_BSS_MODE_IBSS)
+                    /* TODO: mlan_scan.c needs to parse TLV 0x05 (TIM) for
+                       dtim_period */
+                }
+                PRINTM(MCMD_D, "%s():  sta.bcn_pd=%d, sta.dtim_pd=%d\n",
+                       __FUNCTION__, bcn_pd_msec, dtim_pd_msec);
+                bcn_dtim_msec = (bcn_pd_msec * dtim_pd_msec);
+                if (bcn_dtim_msec > pstate_rdh->max_bcn_dtim_ms)
+                    pstate_rdh->max_bcn_dtim_ms = bcn_dtim_msec;
+            }
+        }
+
+        if (pstate_rdh->priv_curr_idx < pstate_rdh->priv_list_count)
+            break;              // EXIT CASE (for UAP)
+        // else
+        pstate_rdh->priv_curr_idx = RDH_STAGE_FIRST_ENTRY_PRIV_IDX;
+        pstate_rdh->stage = RDH_SET_CUSTOM_IE;
+        // FALL THROUGH TO NEXT STAGE
+
+    case RDH_SET_CUSTOM_IE:
+        PRINTM(MCMD_D, "%s(): stage(%d)=%s, priv_idx=%d\n",
+               __FUNCTION__, pstate_rdh->stage,
+               RDH_stage_str[pstate_rdh->stage], pstate_rdh->priv_curr_idx);
+
+        /* add CHAN_SW IE - firmware will accept on any interface, and apply to
+           all */
+        if (pstate_rdh->priv_curr_idx == RDH_STAGE_FIRST_ENTRY_PRIV_IDX) {
+            mlan_ioctl_req *pioctl_req = MNULL;
+
+            ret =
+                wlan_11h_prepare_custom_ie_chansw(pmadapter, &pioctl_req,
+                                                  MTRUE);
+            if ((ret != MLAN_STATUS_SUCCESS) || !pioctl_req) {
+                PRINTM(MERROR, "%s(): Error in preparng CHAN_SW IE.\n",
+                       __FUNCTION__);
+                break;          // EXIT CASE
+            }
+
+            PRINTM(MMSG,
+                   "11h: Radar Detected - adding CHAN_SW IE to interfaces.\n");
+            pmpriv = pstate_rdh->priv_list[0];
+            pstate_rdh->priv_curr_idx = 0;
+            pioctl_req->bss_index = pmpriv->bss_index;
+            ret = wlan_misc_ioctl_custom_ie_list(pmadapter, pioctl_req, MFALSE);
+            if (ret != MLAN_STATUS_SUCCESS && ret != MLAN_STATUS_PENDING) {
+                PRINTM(MERROR,
+                       "%s(): Could not set IE for priv=%p [priv_bss_idx=%d]!\n",
+                       __FUNCTION__, pmpriv, pmpriv->bss_index);
+                // TODO: how to handle this error case?? ignore & continue?
+            }
+            /* free ioctl buffer memory before we leave */
+            pmadapter->callbacks.moal_mfree(pmadapter->pmoal_handle,
+                                            (t_u8 *) pioctl_req);
+            break;              // EXIT CASE
+        }
+        // else
+        pstate_rdh->priv_curr_idx = RDH_STAGE_FIRST_ENTRY_PRIV_IDX;
+        pstate_rdh->stage = RDH_REM_CUSTOM_IE;
+        // FALL THROUGH TO NEXT STAGE
+
+    case RDH_REM_CUSTOM_IE:
+        PRINTM(MCMD_D, "%s(): stage(%d)=%s, priv_idx=%d\n",
+               __FUNCTION__, pstate_rdh->stage,
+               RDH_stage_str[pstate_rdh->stage], pstate_rdh->priv_curr_idx);
+
+        /* remove CHAN_SW IE - firmware will accept on any interface, and apply
+           to all */
+        if (pstate_rdh->priv_curr_idx == RDH_STAGE_FIRST_ENTRY_PRIV_IDX) {
+            mlan_ioctl_req *pioctl_req = MNULL;
+
+            /* first entry to this stage, do delay DFS requires a minimum of 5
+               chances for clients to hear this IE. Use delay: 5 beacons <=
+               (BCN_DTIM_MSEC*5) <= 3 seconds). */
+            t_u16 delay_ms = MAX(MIN_RDH_CHAN_SW_IE_PERIOD_MSEC,
+                                 MIN((5 * pstate_rdh->max_bcn_dtim_ms),
+                                     MAX_RDH_CHAN_SW_IE_PERIOD_MSEC));
+            PRINTM(MMSG, "11h: Radar Detected - delay %d ms for FW to"
+                   " broadcast CHAN_SW IE.\n", delay_ms);
+            wlan_mdelay(pmadapter, delay_ms);
+            PRINTM(MMSG, "11h: Radar Detected - delay over, removing"
+                   " CHAN_SW IE from interfaces.\n");
+
+            ret =
+                wlan_11h_prepare_custom_ie_chansw(pmadapter, &pioctl_req,
+                                                  MFALSE);
+            if ((ret != MLAN_STATUS_SUCCESS) || !pioctl_req) {
+                PRINTM(MERROR, "%s(): Error in preparng CHAN_SW IE.\n",
+                       __FUNCTION__);
+                break;          // EXIT CASE
+            }
+
+            pmpriv = pstate_rdh->priv_list[0];
+            pstate_rdh->priv_curr_idx = 0;
+            pioctl_req->bss_index = pmpriv->bss_index;
+            ret = wlan_misc_ioctl_custom_ie_list(pmadapter, pioctl_req, MFALSE);
+            if (ret != MLAN_STATUS_SUCCESS && ret != MLAN_STATUS_PENDING) {
+                PRINTM(MERROR,
+                       "%s(): Could not set IE for priv=%p [priv_bss_idx=%d]!\n",
+                       __FUNCTION__, pmpriv, pmpriv->bss_index);
+                // TODO: how to handle this error case?? ignore & continue?
+            }
+            /* free ioctl buffer memory before we leave */
+            pmadapter->callbacks.moal_mfree(pmadapter->pmoal_handle,
+                                            (t_u8 *) pioctl_req);
+            break;              // EXIT CASE
+        }
+        // else
+        pstate_rdh->priv_curr_idx = RDH_STAGE_FIRST_ENTRY_PRIV_IDX;
+        pstate_rdh->stage = RDH_STOP_INTFS;
+#ifdef DFS_TESTING_SUPPORT
+        if (pmadapter->dfs_test_params.no_channel_change_on_radar) {
+            PRINTM(MCMD_D, "dfs_testing - no channel change on radar."
+                   "  Also skip stop/restart interface stages.\n",
+                   pmadapter->dfs_test_params.no_channel_change_on_radar);
+            pstate_rdh->priv_curr_idx = RDH_STAGE_FIRST_ENTRY_PRIV_IDX;
+            pstate_rdh->stage = RDH_RESTART_TRAFFIC;
+            goto rdh_restart_traffic;   // skip several stages
+        }
+#endif
+        // FALL THROUGH TO NEXT STAGE
+
+    case RDH_STOP_INTFS:
+        PRINTM(MCMD_D, "%s(): stage(%d)=%s, priv_idx=%d\n",
+               __FUNCTION__, pstate_rdh->stage,
+               RDH_stage_str[pstate_rdh->stage], pstate_rdh->priv_curr_idx);
+
+        /* issues one cmd (DEAUTH/ADHOC_STOP/BSS_STOP) to each intf */
+        while ((++pstate_rdh->priv_curr_idx) < pstate_rdh->priv_list_count) {
+            pmpriv = pstate_rdh->priv_list[pstate_rdh->priv_curr_idx];
+#ifdef UAP_SUPPORT
+            if (GET_BSS_ROLE(pmpriv) == MLAN_BSS_ROLE_UAP) {
+                ret = wlan_prepare_cmd(pmpriv, HOST_CMD_APCMD_BSS_STOP,
+                                       HostCmd_ACT_GEN_SET, 0, MNULL, MNULL);
+                break;          // leads to exit case
+            }
+#endif
+#ifdef STA_SUPPORT
+            if (GET_BSS_ROLE(pmpriv) == MLAN_BSS_ROLE_STA) {
+                if (wlan_11h_is_dfs_master(pmpriv)) {
+                    /* Save ad-hoc creator state before stop clears it */
+                    pmpriv->adhoc_state_prev = pmpriv->adhoc_state;
+                }
+                if (pmpriv->media_connected == MTRUE) {
+                    wlan_disconnect(pmpriv, MNULL, MNULL);
+                    break;      // leads to exit case
+                }
+            }
+#endif
+        }
+
+        if (pstate_rdh->priv_curr_idx < pstate_rdh->priv_list_count)
+            break;              // EXIT CASE
+        // else
+        pstate_rdh->priv_curr_idx = RDH_STAGE_FIRST_ENTRY_PRIV_IDX;
+        pstate_rdh->stage = RDH_SET_NEW_CHANNEL;
+        // FALL THROUGH TO NEXT STAGE
+
+    case RDH_SET_NEW_CHANNEL:
+        PRINTM(MCMD_D, "%s(): stage(%d)=%s, priv_idx=%d\n",
+               __FUNCTION__, pstate_rdh->stage,
+               RDH_stage_str[pstate_rdh->stage], pstate_rdh->priv_curr_idx);
+
+        /* only set new channel for UAP intfs */
+        while ((++pstate_rdh->priv_curr_idx) < pstate_rdh->priv_list_count) {
+            pmpriv = pstate_rdh->priv_list[pstate_rdh->priv_curr_idx];
+#ifdef UAP_SUPPORT
+            if (GET_BSS_ROLE(pmpriv) == MLAN_BSS_ROLE_UAP) {
+                pmpriv->uap_state_chan_cb.pioctl_req_curr = MNULL;
+                pmpriv->uap_state_chan_cb.get_chan_callback
+                    = wlan_11h_radar_detected_callback;
+                pstate_rdh->uap_band_cfg |= UAP_BAND_CONFIG_5GHZ;       /* DFS
+                                                                           only
+                                                                           in
+                                                                           5GHz
+                                                                         */
+                ret = wlan_uap_set_channel(pmpriv, pstate_rdh->uap_band_cfg,
+                                           pstate_rdh->new_channel);
+                break;          // leads to exit case
+            }
+#endif
+        }
+
+        if (pstate_rdh->priv_curr_idx < pstate_rdh->priv_list_count)
+            break;              // EXIT CASE (for UAP)
+        // else
+        pstate_rdh->priv_curr_idx = RDH_STAGE_FIRST_ENTRY_PRIV_IDX;
+        pstate_rdh->stage = RDH_RESTART_INTFS;
+        // FALL THROUGH TO NEXT STAGE
+
+    case RDH_RESTART_INTFS:
+        PRINTM(MCMD_D, "%s(): stage(%d)=%s, priv_idx=%d\n",
+               __FUNCTION__, pstate_rdh->stage,
+               RDH_stage_str[pstate_rdh->stage], pstate_rdh->priv_curr_idx);
+
+        /* can only restart master intfs */
+        while ((++pstate_rdh->priv_curr_idx) < pstate_rdh->priv_list_count) {
+            pmpriv = pstate_rdh->priv_list[pstate_rdh->priv_curr_idx];
+#ifdef UAP_SUPPORT
+            if (GET_BSS_ROLE(pmpriv) == MLAN_BSS_ROLE_UAP) {
+                if (wlan_11h_radar_detect_required(pmpriv,
+                                                   pstate_rdh->new_channel)) {
+                    /* Radar detection is required for this channel, make sure
+                       11h is activated in the firmware */
+                    ret = wlan_11h_activate(pmpriv, MNULL, MTRUE);
+                    ret = wlan_11h_config_master_radar_det(pmpriv, MTRUE);
+                    ret = wlan_11h_check_update_radar_det_state(pmpriv);
+                }
+                ret = wlan_prepare_cmd(pmpriv, HOST_CMD_APCMD_BSS_START,
+                                       HostCmd_ACT_GEN_SET, 0, MNULL, MNULL);
+                break;          // leads to exit case
+            }
+#endif
+#ifdef STA_SUPPORT
+            if (GET_BSS_ROLE(pmpriv) == MLAN_BSS_ROLE_STA) {
+                /* Check previous state to find former Ad-hoc creator
+                   interface. Set new state to Starting, so it'll be seen as a
+                   DFS master. */
+                if (pmpriv->adhoc_state_prev == ADHOC_STARTED) {
+                    pmpriv->adhoc_state = ADHOC_STARTING;
+                    pmpriv->adhoc_state_prev = ADHOC_IDLE;
+                }
+                if (wlan_11h_is_dfs_master(pmpriv)) {
+                    /* set new adhoc channel here */
+                    pmpriv->adhoc_channel = pstate_rdh->new_channel;
+                    if (wlan_11h_radar_detect_required(pmpriv,
+                                                       pstate_rdh->
+                                                       new_channel)) {
+                        /* Radar detection is required for this channel, make
+                           sure 11h is activated in the firmware */
+                        ret = wlan_11h_activate(pmpriv, MNULL, MTRUE);
+                        ret = wlan_11h_config_master_radar_det(pmpriv, MTRUE);
+                        ret = wlan_11h_check_update_radar_det_state(pmpriv);
+                    }
+                    ret =
+                        wlan_prepare_cmd(pmpriv,
+                                         HostCmd_CMD_802_11_AD_HOC_START,
+                                         HostCmd_ACT_GEN_SET, 0, MNULL,
+                                         &pmpriv->adhoc_last_start_ssid);
+                    break;      // leads to exit case
+                }
+
+                /* NOTE: DON'T reconnect slave STA intfs - infra/adhoc_joiner
+                   Do we want to return to same AP/network (on radar channel)?
+                   If want to connect back, depend on either: 1. driver's
+                   reassoc thread 2. wpa_supplicant, or other user-space app */
+            }
+#endif
+        }
+
+        if (pstate_rdh->priv_curr_idx < pstate_rdh->priv_list_count)
+            break;              // EXIT CASE (for UAP)
+        // else
+        pstate_rdh->priv_curr_idx = RDH_STAGE_FIRST_ENTRY_PRIV_IDX;
+        pstate_rdh->stage = RDH_RESTART_TRAFFIC;
+        // FALL THROUGH TO NEXT STAGE
+
+    case RDH_RESTART_TRAFFIC:
+#ifdef DFS_TESTING_SUPPORT
+      rdh_restart_traffic:
+#endif
+        PRINTM(MCMD_D, "%s(): stage(%d)=%s\n",
+               __FUNCTION__, pstate_rdh->stage,
+               RDH_stage_str[pstate_rdh->stage]);
+
+        /* continue traffic for reactivated interfaces */
+        PRINTM(MMSG, "11h: Radar Detected - restarting host tx traffic.\n");
+        for (i = 0; i < pstate_rdh->priv_list_count; i++) {
+            wlan_11h_tx_enable(pstate_rdh->priv_list[i]);
+        }
+
+        pstate_rdh->stage = RDH_OFF;    /* DONE! */
+        PRINTM(MCMD_D, "%s(): finished - stage(%d)=%s\n",
+               __FUNCTION__, pstate_rdh->stage,
+               RDH_stage_str[pstate_rdh->stage]);
+        break;
+
+    default:
+        pstate_rdh->stage = RDH_OFF;    /* cancel RDH to unblock Tx packets */
+        break;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief DFS Event Preprocessing.
+ *  Operates directly on pmadapter variables.
+ *
+ *  1. EVENT_RADAR_DETECTED comes from firmware without specific
+ *     bss_num/bss_type.  Find it an appropriate interface and
+ *     update event_cause field in event_buf.
+ *
+ *  @param pmadapter    Pointer to mlan_adapter
+ *
+ *  @return    MLAN_STATUS_SUCCESS (update successful)
+ *          or MLAN_STATUS_FAILURE (no change)
+ */
+mlan_status
+wlan_11h_dfs_event_preprocessing(mlan_adapter * pmadapter)
+{
+    mlan_status ret = MLAN_STATUS_FAILURE;
+    mlan_private *pmpriv = MNULL;
+    mlan_private *priv_list[MLAN_MAX_BSS_NUM];
+
+    ENTER();
+    switch (pmadapter->event_cause & EVENT_ID_MASK) {
+    case EVENT_RADAR_DETECTED:
+        /* find active intf: prefer dfs_master over dfs_slave */
+        if (wlan_get_privs_by_two_cond(pmadapter,
+                                       wlan_11h_is_master_active_on_dfs_chan,
+                                       wlan_11h_is_dfs_master,
+                                       MTRUE, priv_list)) {
+            pmpriv = priv_list[0];
+            PRINTM(MINFO, "%s: found dfs_master priv=%p\n",
+                   __FUNCTION__, pmpriv);
+        } else if (wlan_get_privs_by_two_cond(pmadapter,
+                                              wlan_11h_is_slave_active_on_dfs_chan,
+                                              wlan_11h_is_dfs_slave,
+                                              MTRUE, priv_list)) {
+            pmpriv = priv_list[0];
+            PRINTM(MINFO, "%s: found dfs_slave priv=%p\n",
+                   __FUNCTION__, pmpriv);
+        }
+
+        /* update event_cause if we found an appropriate priv */
+        if (pmpriv) {
+            pmlan_buffer pmevbuf = pmadapter->pmlan_buffer_event;
+            t_u32 new_event_cause = pmadapter->event_cause & EVENT_ID_MASK;
+            new_event_cause |= ((GET_BSS_NUM(pmpriv) & 0xff) << 16) |
+                ((pmpriv->bss_type & 0xff) << 24);
+            PRINTM(MINFO, "%s: priv - bss_num=%d, bss_type=%d\n",
+                   __FUNCTION__, GET_BSS_NUM(pmpriv), pmpriv->bss_type);
+            memcpy(pmadapter, pmevbuf->pbuf + pmevbuf->data_offset,
+                   &new_event_cause, sizeof(new_event_cause));
+            ret = MLAN_STATUS_SUCCESS;
+        }
+        break;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief try to switch to a non-dfs channel
+ *
+ *  @param priv    Void pointer to mlan_private
+ *
+ *  @param chan    pointer to channel
+ *
+ *  @return MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE or MLAN_STATUS_PENDING
+ */
+mlan_status
+wlan_11h_switch_non_dfs_chan(mlan_private * priv, t_u8 * chan)
+{
+    mlan_status ret = MLAN_STATUS_FAILURE;
+    t_u32 i;
+    t_u32 rand_entry;
+    t_u8 def_chan;
+    t_u8 rand_tries = 0;
+    region_chan_t *chn_tbl = MNULL;
+    pmlan_adapter pmadapter = priv->adapter;
+
+    ENTER();
+
+    /* get the channel table first */
+    for (i = 0; i < MAX_REGION_CHANNEL_NUM; i++) {
+        if (pmadapter->region_channel[i].band == BAND_A
+            && pmadapter->region_channel[i].valid) {
+            chn_tbl = &pmadapter->region_channel[i];
+            break;
+        }
+    }
+
+    if (!chn_tbl || !chn_tbl->pcfp) {
+        goto done;
+    }
+
+    do {
+        rand_entry = wlan_11h_get_random_num(pmadapter) % chn_tbl->num_cfp;
+        def_chan = (t_u8) chn_tbl->pcfp[rand_entry].channel;
+        rand_tries++;
+    } while ((wlan_11h_is_channel_under_nop(pmadapter, def_chan) ||
+              chn_tbl->pcfp[rand_entry].passive_scan_or_radar_detect == MTRUE)
+             && (rand_tries < MAX_SWITCH_CHANNEL_RETRIES));
+
+    /* meet max retries, use the lowest non-dfs channel */
+    if (rand_tries == MAX_SWITCH_CHANNEL_RETRIES) {
+        for (i = 0; i < chn_tbl->num_cfp; i++) {
+            if (chn_tbl->pcfp[i].passive_scan_or_radar_detect == MFALSE &&
+                !wlan_11h_is_channel_under_nop(pmadapter,
+                                               (t_u8) chn_tbl->pcfp[i].
+                                               channel)) {
+                def_chan = (t_u8) chn_tbl->pcfp[i].channel;
+                break;
+            }
+        }
+        if (i == chn_tbl->num_cfp) {
+            goto done;
+        }
+    }
+
+    *chan = def_chan;
+    ret = MLAN_STATUS_SUCCESS;
+  done:
+    LEAVE();
+    return ret;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_11h.h b/drivers/net/wireless/sd8797/mlan/mlan_11h.h
new file mode 100644
index 000000000000..cfa7fb915b39
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_11h.h
@@ -0,0 +1,158 @@
+/** @file mlan_11h.h
+ *
+ *  @brief This header file contains data structures and
+ *  function declarations of 802.11h
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ *
+ */
+
+/*************************************************************
+Change Log:
+    03/26/2009: initial creation
+*************************************************************/
+
+#ifndef _MLAN_11H_
+#define _MLAN_11H_
+
+/** 11H OID bitmasks */
+#define ENABLE_11H_MASK         MBIT(0)
+#define MASTER_RADAR_DET_MASK   MBIT(1)
+#define SLAVE_RADAR_DET_MASK    MBIT(2)
+
+/** DFS Master Radar Detect global enable */
+#define DFS_MASTER_RADAR_DETECT_EN  (MTRUE)
+/** DFS Slave Radar Detect global enable */
+#define DFS_SLAVE_RADAR_DETECT_EN   (MFALSE)
+
+/**
+ *  11H APIs
+ */
+
+/* Is master radar detection enabled in firmware? */
+extern t_bool wlan_11h_is_master_radar_det_active(mlan_private * priv);
+
+/** Configure master radar detection.  Need call wlan_11h_check_update_radar_det_state() after. */
+extern mlan_status wlan_11h_config_master_radar_det(mlan_private * priv,
+                                                    t_bool enable);
+
+/** Configure slave radar detection.  Need call wlan_11h_check_update_radar_det_state() after. */
+extern mlan_status wlan_11h_config_slave_radar_det(mlan_private * priv,
+                                                   t_bool enable);
+
+/** Checks all interfaces and updates radar detect flags if necessary */
+extern mlan_status wlan_11h_check_update_radar_det_state(mlan_private * pmpriv);
+
+/** Return 1 if 11h is active in the firmware, 0 if it is inactive */
+extern t_bool wlan_11h_is_active(mlan_private * priv);
+
+/** Enable the tx interface and record the new transmit state */
+extern void wlan_11h_tx_enable(mlan_private * priv);
+
+/** Disable the tx interface and record the new transmit state */
+extern void wlan_11h_tx_disable(mlan_private * priv);
+
+/** Activate 11h extensions in the firmware */
+extern mlan_status wlan_11h_activate(mlan_private * priv, t_void * pioctl_buf,
+                                     t_bool flag);
+
+/** Initialize the 11h device structure */
+extern void wlan_11h_init(mlan_adapter * pmadapter);
+
+/** Cleanup for the 11h device structure */
+extern void wlan_11h_cleanup(mlan_adapter * pmadapter);
+
+/** Initialize the 11h interface structure */
+extern void wlan_11h_priv_init(mlan_private * pmpriv);
+
+/** Get an initial random channel to start an adhoc network on */
+extern t_u8 wlan_11h_get_adhoc_start_channel(mlan_private * priv);
+
+/** Check if radar detection is required on the specified channel */
+extern t_bool wlan_11h_radar_detect_required(mlan_private * priv, t_u8 channel);
+
+/** Perform a standard availibility check on the specified channel */
+extern t_s32 wlan_11h_issue_radar_detect(mlan_private * priv,
+                                         pmlan_ioctl_req pioctl_req,
+                                         t_u8 channel);
+
+/** Check previously issued radar report for a channel */
+extern mlan_status wlan_11h_check_chan_report(mlan_private * priv, t_u8 chan);
+
+/** Add any 11h TLVs necessary to complete an adhoc start command */
+extern t_s32 wlan_11h_process_start(mlan_private * priv,
+                                    t_u8 ** ppbuffer,
+                                    IEEEtypes_CapInfo_t * pcap_info,
+                                    t_u32 channel,
+                                    wlan_11h_bss_info_t * p11h_bss_info);
+
+/** Add any 11h TLVs necessary to complete a join command (adhoc or infra) */
+extern t_s32 wlan_11h_process_join(mlan_private * priv,
+                                   t_u8 ** ppbuffer,
+                                   IEEEtypes_CapInfo_t * pcap_info,
+                                   t_u8 band,
+                                   t_u32 channel,
+                                   wlan_11h_bss_info_t * p11h_bss_info);
+
+/** Complete the firmware command preparation for an 11h command function */
+extern mlan_status wlan_11h_cmd_process(mlan_private * priv,
+                                        HostCmd_DS_COMMAND * pcmd_ptr,
+                                        const t_void * pinfo_buf);
+
+/** Process the response of an 11h firmware command */
+extern mlan_status wlan_11h_cmdresp_process(mlan_private * priv,
+                                            const HostCmd_DS_COMMAND * resp);
+
+/** Receive IEs from scan processing and record any needed info for 11h */
+extern mlan_status wlan_11h_process_bss_elem(mlan_adapter * pmadapter,
+                                             wlan_11h_bss_info_t *
+                                             p11h_bss_info,
+                                             const t_u8 * pelement);
+
+/** Handler for EVENT_CHANNEL_SWITCH_ANN */
+extern mlan_status wlan_11h_handle_event_chanswann(mlan_private * priv);
+
+/** Handler for EVENT_CHANNEL_REPORT_RDY */
+extern mlan_status wlan_11h_handle_event_chanrpt_ready(mlan_private * priv,
+                                                       mlan_event * pevent);
+
+#ifdef DFS_TESTING_SUPPORT
+/** Handler for DFS_TESTING IOCTL */
+extern mlan_status wlan_11h_ioctl_dfs_testing(pmlan_adapter pmadapter,
+                                              pmlan_ioctl_req pioctl_req);
+#endif
+
+/** Check if channel is under a NOP duration (should not be used) */
+extern t_bool wlan_11h_is_channel_under_nop(mlan_adapter * pmadapter,
+                                            t_u8 channel);
+
+/** Check if RADAR_DETECTED handling is blocking data tx */
+extern t_bool wlan_11h_radar_detected_tx_blocked(mlan_adapter * pmadapter);
+
+/** Callback for RADAR_DETECTED (for UAP cmdresp) */
+extern mlan_status wlan_11h_radar_detected_callback(t_void * priv);
+
+/** Handler for RADAR_DETECTED */
+extern mlan_status wlan_11h_radar_detected_handling(mlan_adapter * pmadapter);
+
+/** DFS Event pre-processing */
+extern mlan_status wlan_11h_dfs_event_preprocessing(mlan_adapter * pmadapter);
+
+/** DFS switch to non-DFS channel */
+extern mlan_status wlan_11h_switch_non_dfs_chan(mlan_private * priv,
+                                                t_u8 * chan);
+
+#endif /*_MLAN_11H_ */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_11n.c b/drivers/net/wireless/sd8797/mlan/mlan_11n.c
new file mode 100644
index 000000000000..5e78748e4a3b
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_11n.c
@@ -0,0 +1,1936 @@
+/** @file mlan_11n.c
+ *
+ *  @brief This file contains functions for 11n handling.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ *
+ */
+
+/********************************************************
+Change log:
+    11/10/2008: initial version
+********************************************************/
+
+#include "mlan.h"
+#include "mlan_join.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_wmm.h"
+#include "mlan_11n.h"
+
+/********************************************************
+    Local Variables
+********************************************************/
+
+/********************************************************
+    Global Variables
+********************************************************/
+
+/********************************************************
+    Local Functions
+********************************************************/
+
+/**
+ *
+ *  @brief set/get max tx buf size
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return				MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_11n_ioctl_max_tx_buf_size(IN pmlan_adapter pmadapter,
+                               IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_11n_cfg *cfg = MNULL;
+
+    ENTER();
+
+    cfg = (mlan_ds_11n_cfg *) pioctl_req->pbuf;
+    cfg->param.tx_buf_size = (t_u32) pmadapter->max_tx_buf_size;
+    pioctl_req->data_read_written = sizeof(t_u32) + MLAN_SUB_COMMAND_SIZE;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/get htcapinfo configuration
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return				MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_11n_ioctl_htusrcfg(IN pmlan_adapter pmadapter,
+                        IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_11n_cfg *cfg = MNULL;
+
+    ENTER();
+
+    cfg = (mlan_ds_11n_cfg *) pioctl_req->pbuf;
+
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        if (((cfg->param.htcap_cfg.htcap & ~IGN_HW_DEV_CAP) &
+             pmpriv->adapter->hw_dot_11n_dev_cap)
+            != (cfg->param.htcap_cfg.htcap & ~IGN_HW_DEV_CAP)) {
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+        } else {
+            if (cfg->param.htcap_cfg.misc_cfg == BAND_SELECT_BG) {
+                pmadapter->usr_dot_11n_dev_cap_bg = cfg->param.htcap_cfg.htcap;
+                PRINTM(MINFO, "Set: UsrDot11nCap for 2.4GHz 0x%x\n",
+                       pmadapter->usr_dot_11n_dev_cap_bg);
+            }
+            if (cfg->param.htcap_cfg.misc_cfg == BAND_SELECT_A) {
+                pmadapter->usr_dot_11n_dev_cap_a = cfg->param.htcap_cfg.htcap;
+                PRINTM(MINFO, "Set: UsrDot11nCap for 5GHz 0x%x\n",
+                       pmadapter->usr_dot_11n_dev_cap_a);
+            }
+            if (cfg->param.htcap_cfg.misc_cfg == BAND_SELECT_BOTH) {
+                pmadapter->usr_dot_11n_dev_cap_bg = cfg->param.htcap_cfg.htcap;
+                pmadapter->usr_dot_11n_dev_cap_a = cfg->param.htcap_cfg.htcap;
+                PRINTM(MINFO, "Set: UsrDot11nCap for 2.4GHz and 5GHz 0x%x\n",
+                       cfg->param.htcap_cfg.htcap);
+            }
+        }
+    } else {
+        /* Hardware 11N device capability required */
+        if (cfg->param.htcap_cfg.hw_cap_req)
+            cfg->param.htcap_cfg.htcap = pmadapter->hw_dot_11n_dev_cap;
+        else {
+            if (cfg->param.htcap_cfg.misc_cfg == BAND_SELECT_BG) {
+                cfg->param.htcap_cfg.htcap = pmadapter->usr_dot_11n_dev_cap_bg;
+                PRINTM(MINFO, "Get: UsrDot11nCap for 2.4GHz 0x%x\n",
+                       cfg->param.htcap_cfg.htcap);
+            }
+            if (cfg->param.htcap_cfg.misc_cfg == BAND_SELECT_A) {
+                cfg->param.htcap_cfg.htcap = pmadapter->usr_dot_11n_dev_cap_a;
+                PRINTM(MINFO, "Get: UsrDot11nCap for 5GHz 0x%x\n",
+                       cfg->param.htcap_cfg.htcap);
+            }
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Enable/Disable AMSDU AGGR CTRL
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_11n_ioctl_amsdu_aggr_ctrl(IN pmlan_adapter pmadapter,
+                               IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_11n_cfg *cfg = MNULL;
+    t_u16 cmd_action = 0;
+
+    ENTER();
+
+    cfg = (mlan_ds_11n_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET)
+        cmd_action = HostCmd_ACT_GEN_SET;
+    else
+        cmd_action = HostCmd_ACT_GEN_GET;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_AMSDU_AGGR_CTRL,
+                           cmd_action,
+                           0,
+                           (t_void *) pioctl_req,
+                           (t_void *) & cfg->param.amsdu_aggr_ctrl);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/get 11n configuration
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_11n_ioctl_httxcfg(IN pmlan_adapter pmadapter,
+                       IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_11n_cfg *cfg = MNULL;
+    t_u16 cmd_action = 0;
+
+    ENTER();
+
+    cfg = (mlan_ds_11n_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET)
+        cmd_action = HostCmd_ACT_GEN_SET;
+    else
+        cmd_action = HostCmd_ACT_GEN_GET;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_11N_CFG,
+                           cmd_action,
+                           0,
+                           (t_void *) pioctl_req,
+                           (t_void *) & cfg->param.tx_cfg);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/get TX beamforming capabilities
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_11n_ioctl_tx_bf_cap(IN pmlan_adapter pmadapter,
+                         IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_11n_cfg *cfg = MNULL;
+
+    ENTER();
+
+    cfg = (mlan_ds_11n_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET)
+        pmpriv->tx_bf_cap = cfg->param.tx_bf_cap;
+    else
+        cfg->param.tx_bf_cap = pmpriv->tx_bf_cap;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/get TX beamforming configurations
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_11n_ioctl_tx_bf_cfg(IN pmlan_adapter pmadapter,
+                         IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_11n_cfg *cfg = MNULL;
+    t_u16 cmd_action = 0;
+
+    ENTER();
+
+    cfg = (mlan_ds_11n_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET)
+        cmd_action = HostCmd_ACT_GEN_SET;
+    else
+        cmd_action = HostCmd_ACT_GEN_GET;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_TX_BF_CFG,
+                           cmd_action,
+                           0,
+                           (t_void *) pioctl_req,
+                           (t_void *) & cfg->param.tx_bf);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/get HT stream configurations
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_11n_ioctl_stream_cfg(IN pmlan_adapter pmadapter,
+                          IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_11n_cfg *cfg = MNULL;
+
+    ENTER();
+
+    cfg = (mlan_ds_11n_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        cfg->param.stream_cfg = pmadapter->usr_dev_mcs_support;
+    } else if (pioctl_req->action == MLAN_ACT_SET) {
+        switch (cfg->param.stream_cfg) {
+        case HT_STREAM_MODE_2X2:
+            if (pmadapter->hw_dev_mcs_support == HT_STREAM_MODE_1X1) {
+                PRINTM(MERROR, "HW does not support this mode\n");
+                ret = MLAN_STATUS_FAILURE;
+            } else
+                pmadapter->usr_dev_mcs_support = cfg->param.stream_cfg;
+            break;
+        case HT_STREAM_MODE_1X1:
+            pmadapter->usr_dev_mcs_support = cfg->param.stream_cfg;
+            break;
+        default:
+            PRINTM(MERROR, "Invalid stream mode\n");
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+            break;
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function will resend addba request to all
+ *          the peer in the TxBAStreamTbl
+ *
+ *  @param priv     A pointer to mlan_private
+ *
+ *  @return         N/A
+ */
+static void
+wlan_11n_update_addba_request(mlan_private * priv)
+{
+
+    TxBAStreamTbl *ptx_tbl;
+
+    ENTER();
+
+    if (!
+        (ptx_tbl =
+         (TxBAStreamTbl *) util_peek_list(priv->adapter->pmoal_handle,
+                                          &priv->tx_ba_stream_tbl_ptr,
+                                          priv->adapter->callbacks.
+                                          moal_spin_lock,
+                                          priv->adapter->callbacks.
+                                          moal_spin_unlock))) {
+        LEAVE();
+        return;
+    }
+
+    while (ptx_tbl != (TxBAStreamTbl *) & priv->tx_ba_stream_tbl_ptr) {
+        wlan_send_addba(priv, ptx_tbl->tid, ptx_tbl->ra);
+        ptx_tbl = ptx_tbl->pnext;
+    }
+    /* Signal MOAL to trigger mlan_main_process */
+    wlan_recv_event(priv, MLAN_EVENT_ID_DRV_DEFER_HANDLING, MNULL);
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief Set/get addba parameter
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_11n_ioctl_addba_param(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_11n_cfg *cfg = MNULL;
+    t_u32 timeout;
+
+    ENTER();
+
+    cfg = (mlan_ds_11n_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        cfg->param.addba_param.timeout = pmpriv->add_ba_param.timeout;
+        cfg->param.addba_param.txwinsize = pmpriv->add_ba_param.tx_win_size;
+        cfg->param.addba_param.rxwinsize = pmpriv->add_ba_param.rx_win_size;
+        cfg->param.addba_param.txamsdu = pmpriv->add_ba_param.tx_amsdu;
+        cfg->param.addba_param.rxamsdu = pmpriv->add_ba_param.rx_amsdu;
+    } else {
+        timeout = pmpriv->add_ba_param.timeout;
+        pmpriv->add_ba_param.timeout = cfg->param.addba_param.timeout;
+        pmpriv->add_ba_param.tx_win_size = cfg->param.addba_param.txwinsize;
+        pmpriv->add_ba_param.rx_win_size = cfg->param.addba_param.rxwinsize;
+        pmpriv->add_ba_param.tx_amsdu = cfg->param.addba_param.txamsdu;
+        pmpriv->add_ba_param.rx_amsdu = cfg->param.addba_param.rxamsdu;
+        if (timeout != pmpriv->add_ba_param.timeout) {
+            wlan_11n_update_addba_request(pmpriv);
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/get addba reject set
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_11n_ioctl_addba_reject(IN pmlan_adapter pmadapter,
+                            IN pmlan_ioctl_req pioctl_req)
+{
+    int i = 0;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_11n_cfg *cfg = MNULL;
+
+    ENTER();
+
+    cfg = (mlan_ds_11n_cfg *) pioctl_req->pbuf;
+
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        PRINTM(MINFO, "Get Addba reject\n");
+        memcpy(pmadapter, cfg->param.addba_reject, pmpriv->addba_reject,
+               MAX_NUM_TID);
+    } else {
+        if (pmpriv->media_connected == MTRUE) {
+            PRINTM(MERROR, "Can not set aggr priority table in connected"
+                   " state\n");
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+
+        for (i = 0; i < MAX_NUM_TID; i++) {
+            /* For AMPDU */
+            if (cfg->param.addba_reject[i] > ADDBA_RSP_STATUS_REJECT) {
+                pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+                ret = MLAN_STATUS_FAILURE;
+                break;
+            }
+
+            pmpriv->addba_reject[i] = cfg->param.addba_reject[i];
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/get aggr_prio_tbl
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_11n_ioctl_aggr_prio_tbl(IN pmlan_adapter pmadapter,
+                             IN pmlan_ioctl_req pioctl_req)
+{
+    int i = 0;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_11n_cfg *cfg = MNULL;
+
+    ENTER();
+
+    cfg = (mlan_ds_11n_cfg *) pioctl_req->pbuf;
+
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        for (i = 0; i < MAX_NUM_TID; i++) {
+            cfg->param.aggr_prio_tbl.ampdu[i] =
+                pmpriv->aggr_prio_tbl[i].ampdu_user;
+            cfg->param.aggr_prio_tbl.amsdu[i] = pmpriv->aggr_prio_tbl[i].amsdu;
+        }
+    } else {
+        if (pmpriv->media_connected == MTRUE) {
+            PRINTM(MERROR, "Can not set aggr priority table in connected"
+                   " state\n");
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+
+        for (i = 0; i < MAX_NUM_TID; i++) {
+            /* For AMPDU */
+            if ((cfg->param.aggr_prio_tbl.ampdu[i] > HIGH_PRIO_TID) &&
+                (cfg->param.aggr_prio_tbl.ampdu[i] != BA_STREAM_NOT_ALLOWED)) {
+                pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+                ret = MLAN_STATUS_FAILURE;
+                break;
+            }
+
+            pmpriv->aggr_prio_tbl[i].ampdu_ap =
+                pmpriv->aggr_prio_tbl[i].ampdu_user =
+                cfg->param.aggr_prio_tbl.ampdu[i];
+
+            /* For AMSDU */
+            if ((cfg->param.aggr_prio_tbl.amsdu[i] > HIGH_PRIO_TID &&
+                 cfg->param.aggr_prio_tbl.amsdu[i] != BA_STREAM_NOT_ALLOWED)) {
+                pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+                ret = MLAN_STATUS_FAILURE;
+                break;
+            } else {
+                pmpriv->aggr_prio_tbl[i].amsdu =
+                    cfg->param.aggr_prio_tbl.amsdu[i];
+            }
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get supported MCS set
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_11n_ioctl_supported_mcs_set(IN pmlan_adapter pmadapter,
+                                 IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_ds_11n_cfg *cfg = MNULL;
+    int rx_mcs_supp;
+    t_u8 mcs_set[NUM_MCS_FIELD];
+
+    ENTER();
+
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        PRINTM(MERROR, "Set operation is not supported\n");
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    rx_mcs_supp = GET_RXMCSSUPP(pmadapter->usr_dev_mcs_support);
+    /* Set MCS for 1x1/2x2 */
+    memset(pmadapter, (t_u8 *) mcs_set, 0xff, rx_mcs_supp);
+    /* Clear all the other values */
+    memset(pmadapter, (t_u8 *) & mcs_set[rx_mcs_supp], 0,
+           NUM_MCS_FIELD - rx_mcs_supp);
+    /* Set MCS32 with 40MHz support */
+    if (ISSUPP_CHANWIDTH40(pmadapter->usr_dot_11n_dev_cap_bg)
+        || ISSUPP_CHANWIDTH40(pmadapter->usr_dot_11n_dev_cap_a)
+        )
+        SETHT_MCS32(mcs_set);
+
+    cfg = (mlan_ds_11n_cfg *) pioctl_req->pbuf;
+    memcpy(pmadapter, cfg->param.supported_mcs_set, mcs_set, NUM_MCS_FIELD);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function checks if the given pointer is valid entry of
+ *         Tx BA Stream table
+ *
+ *  @param priv         Pointer to mlan_private
+ *  @param ptxtblptr    Pointer to tx ba stream entry
+ *
+ *  @return             MTRUE or MFALSE
+ */
+static int
+wlan_is_txbastreamptr_valid(mlan_private * priv, TxBAStreamTbl * ptxtblptr)
+{
+    TxBAStreamTbl *ptx_tbl;
+
+    ENTER();
+
+    if (!
+        (ptx_tbl =
+         (TxBAStreamTbl *) util_peek_list(priv->adapter->pmoal_handle,
+                                          &priv->tx_ba_stream_tbl_ptr, MNULL,
+                                          MNULL))) {
+        LEAVE();
+        return MFALSE;
+    }
+
+    while (ptx_tbl != (TxBAStreamTbl *) & priv->tx_ba_stream_tbl_ptr) {
+        if (ptx_tbl == ptxtblptr) {
+            LEAVE();
+            return MTRUE;
+        }
+
+        ptx_tbl = ptx_tbl->pnext;
+    }
+
+    LEAVE();
+    return MFALSE;
+}
+
+/**
+ *  @brief This function will return the pointer to a entry in BA Stream
+ *  		table which matches the ba_status requested
+ *
+ *  @param priv    	    A pointer to mlan_private
+ *  @param ba_status	Current status of the BA stream
+ *
+ *  @return 	        A pointer to first entry matching status in BA stream
+ *                      NULL if not found
+ */
+static TxBAStreamTbl *
+wlan_11n_get_txbastream_status(mlan_private * priv, baStatus_e ba_status)
+{
+    TxBAStreamTbl *ptx_tbl;
+
+    ENTER();
+
+    if (!
+        (ptx_tbl =
+         (TxBAStreamTbl *) util_peek_list(priv->adapter->pmoal_handle,
+                                          &priv->tx_ba_stream_tbl_ptr,
+                                          priv->adapter->callbacks.
+                                          moal_spin_lock,
+                                          priv->adapter->callbacks.
+                                          moal_spin_unlock))) {
+        LEAVE();
+        return MNULL;
+    }
+
+    while (ptx_tbl != (TxBAStreamTbl *) & priv->tx_ba_stream_tbl_ptr) {
+        if (ptx_tbl->ba_status == ba_status) {
+            LEAVE();
+            return ptx_tbl;
+        }
+
+        ptx_tbl = ptx_tbl->pnext;
+    }
+
+    LEAVE();
+    return MNULL;
+}
+
+/********************************************************
+    Global Functions
+********************************************************/
+
+#ifdef STA_SUPPORT
+/**
+ *  @brief This function fills the cap info
+ *
+ *  @param priv         A pointer to mlan_private structure
+ *  @param pht_cap      A pointer to MrvlIETypes_HTCap_t structure
+ *  @param bands        Band configuration
+ *
+ *  @return             N/A
+ */
+static void
+wlan_fill_cap_info(mlan_private * priv,
+                   MrvlIETypes_HTCap_t * pht_cap, t_u8 bands)
+{
+    mlan_adapter *pmadapter = priv->adapter;
+    t_u32 usr_dot_11n_dev_cap;
+
+    ENTER();
+
+    if (bands & BAND_A)
+        usr_dot_11n_dev_cap = pmadapter->usr_dot_11n_dev_cap_a;
+    else
+        usr_dot_11n_dev_cap = pmadapter->usr_dot_11n_dev_cap_bg;
+
+    if (ISSUPP_CHANWIDTH40(usr_dot_11n_dev_cap))
+        SETHT_SUPPCHANWIDTH(pht_cap->ht_cap.ht_cap_info);
+    else
+        RESETHT_SUPPCHANWIDTH(pht_cap->ht_cap.ht_cap_info);
+
+    if (ISSUPP_GREENFIELD(usr_dot_11n_dev_cap))
+        SETHT_GREENFIELD(pht_cap->ht_cap.ht_cap_info);
+    else
+        RESETHT_GREENFIELD(pht_cap->ht_cap.ht_cap_info);
+
+    if (ISSUPP_SHORTGI20(usr_dot_11n_dev_cap))
+        SETHT_SHORTGI20(pht_cap->ht_cap.ht_cap_info);
+    else
+        RESETHT_SHORTGI20(pht_cap->ht_cap.ht_cap_info);
+
+    if (ISSUPP_SHORTGI40(usr_dot_11n_dev_cap))
+        SETHT_SHORTGI40(pht_cap->ht_cap.ht_cap_info);
+    else
+        RESETHT_SHORTGI40(pht_cap->ht_cap.ht_cap_info);
+
+    if (ISSUPP_RXSTBC(usr_dot_11n_dev_cap))
+        SETHT_RXSTBC(pht_cap->ht_cap.ht_cap_info, 1);
+    else
+        RESETHT_RXSTBC(pht_cap->ht_cap.ht_cap_info);
+
+    if (ISENABLED_40MHZ_INTOLARENT(usr_dot_11n_dev_cap))
+        SETHT_40MHZ_INTOLARANT(pht_cap->ht_cap.ht_cap_info);
+    else
+        RESETHT_40MHZ_INTOLARANT(pht_cap->ht_cap.ht_cap_info);
+
+    /* No user config for LDPC coding capability yet */
+    if (ISSUPP_RXLDPC(pmadapter->hw_dot_11n_dev_cap))
+        SETHT_LDPCCODINGCAP(pht_cap->ht_cap.ht_cap_info);
+    else
+        RESETHT_LDPCCODINGCAP(pht_cap->ht_cap.ht_cap_info);
+
+    /* No user config for TX STBC yet */
+    if (ISSUPP_TXSTBC(pmadapter->hw_dot_11n_dev_cap))
+        SETHT_TXSTBC(pht_cap->ht_cap.ht_cap_info);
+    else
+        RESETHT_TXSTBC(pht_cap->ht_cap.ht_cap_info);
+
+    /* No user config for Delayed BACK yet */
+    if (GET_DELAYEDBACK(pmadapter->hw_dot_11n_dev_cap))
+        SETHT_DELAYEDBACK(pht_cap->ht_cap.ht_cap_info);
+    else
+        RESETHT_DELAYEDBACK(pht_cap->ht_cap.ht_cap_info);
+
+    /* Need change to support 8k AMSDU receive */
+    RESETHT_MAXAMSDU(pht_cap->ht_cap.ht_cap_info);
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function fills the HT cap tlv
+ *
+ *  @param priv         A pointer to mlan_private structure
+ *  @param pht_cap      A pointer to MrvlIETypes_HTCap_t structure
+ *  @param bands        Band configuration
+ *
+ *  @return             N/A
+ */
+void
+wlan_fill_ht_cap_tlv(mlan_private * priv,
+                     MrvlIETypes_HTCap_t * pht_cap, t_u8 bands)
+{
+    mlan_adapter *pmadapter = priv->adapter;
+    int rx_mcs_supp;
+    t_u32 usr_dot_11n_dev_cap;
+
+    ENTER();
+
+    if (bands & BAND_A)
+        usr_dot_11n_dev_cap = pmadapter->usr_dot_11n_dev_cap_a;
+    else
+        usr_dot_11n_dev_cap = pmadapter->usr_dot_11n_dev_cap_bg;
+
+    /* Fill HT cap info */
+    wlan_fill_cap_info(priv, pht_cap, bands);
+    pht_cap->ht_cap.ht_cap_info = wlan_cpu_to_le16(pht_cap->ht_cap.ht_cap_info);
+
+    /* Set ampdu param */
+    SETAMPDU_SIZE(pht_cap->ht_cap.ampdu_param, AMPDU_FACTOR_64K);
+    SETAMPDU_SPACING(pht_cap->ht_cap.ampdu_param, 0);
+
+    rx_mcs_supp = GET_RXMCSSUPP(pmadapter->usr_dev_mcs_support);
+    /* Set MCS for 1x1/2x2 */
+    memset(pmadapter, (t_u8 *) pht_cap->ht_cap.supported_mcs_set, 0xff,
+           rx_mcs_supp);
+    /* Clear all the other values */
+    memset(pmadapter, (t_u8 *) & pht_cap->ht_cap.supported_mcs_set[rx_mcs_supp],
+           0, NUM_MCS_FIELD - rx_mcs_supp);
+    /* Set MCS32 with 40MHz support */
+    if (ISSUPP_CHANWIDTH40(usr_dot_11n_dev_cap))
+        SETHT_MCS32(pht_cap->ht_cap.supported_mcs_set);
+
+    /* Clear RD responder bit */
+    RESETHT_EXTCAP_RDG(pht_cap->ht_cap.ht_ext_cap);
+    pht_cap->ht_cap.ht_ext_cap = wlan_cpu_to_le16(pht_cap->ht_cap.ht_ext_cap);
+
+    /* Set Tx BF cap */
+    pht_cap->ht_cap.tx_bf_cap = wlan_cpu_to_le32(priv->tx_bf_cap);
+
+    LEAVE();
+    return;
+}
+#endif /* STA_SUPPORT */
+
+/**
+ *  @brief This function prints the 802.11n device capability
+ *
+ *  @param pmadapter     A pointer to mlan_adapter structure
+ *  @param cap           Capability value
+ *
+ *  @return        N/A
+ */
+void
+wlan_show_dot11ndevcap(pmlan_adapter pmadapter, t_u32 cap)
+{
+    ENTER();
+
+    PRINTM(MINFO, "GET_HW_SPEC: Maximum MSDU length = %s octets\n",
+           (ISSUPP_MAXAMSDU(cap) ? "7935" : "3839"));
+    PRINTM(MINFO, "GET_HW_SPEC: Beam forming %s\n",
+           (ISSUPP_BEAMFORMING(cap) ? "supported" : "not supported"));
+    PRINTM(MINFO, "GET_HW_SPEC: Greenfield preamble %s\n",
+           (ISSUPP_GREENFIELD(cap) ? "supported" : "not supported"));
+    PRINTM(MINFO, "GET_HW_SPEC: AMPDU %s\n",
+           (ISSUPP_AMPDU(cap) ? "supported" : "not supported"));
+    PRINTM(MINFO, "GET_HW_SPEC: MIMO Power Save %s\n",
+           (ISSUPP_MIMOPS(cap) ? "supported" : "not supported"));
+    PRINTM(MINFO, "GET_HW_SPEC: Rx STBC %s\n",
+           (ISSUPP_RXSTBC(cap) ? "supported" : "not supported"));
+    PRINTM(MINFO, "GET_HW_SPEC: Tx STBC %s\n",
+           (ISSUPP_TXSTBC(cap) ? "supported" : "not supported"));
+    PRINTM(MINFO, "GET_HW_SPEC: Short GI for 40 Mhz %s\n",
+           (ISSUPP_SHORTGI40(cap) ? "supported" : "not supported"));
+    PRINTM(MINFO, "GET_HW_SPEC: Short GI for 20 Mhz %s\n",
+           (ISSUPP_SHORTGI20(cap) ? "supported" : "not supported"));
+    PRINTM(MINFO, "GET_HW_SPEC: LDPC coded packet receive %s\n",
+           (ISSUPP_RXLDPC(cap) ? "supported" : "not supported"));
+    PRINTM(MINFO, "GET_HW_SPEC: Number of Delayed Block Ack streams = %d\n",
+           GET_DELAYEDBACK(cap));
+    PRINTM(MINFO, "GET_HW_SPEC: Number of Immediate Block Ack streams = %d\n",
+           GET_IMMEDIATEBACK(cap));
+    PRINTM(MINFO, "GET_HW_SPEC: 40 Mhz channel width %s\n",
+           (ISSUPP_CHANWIDTH40(cap) ? "supported" : "not supported"));
+    PRINTM(MINFO, "GET_HW_SPEC: 20 Mhz channel width %s\n",
+           (ISSUPP_CHANWIDTH20(cap) ? "supported" : "not supported"));
+    PRINTM(MINFO, "GET_HW_SPEC: 10 Mhz channel width %s\n",
+           (ISSUPP_CHANWIDTH10(cap) ? "supported" : "not supported"));
+
+    if (ISSUPP_RXANTENNAA(cap)) {
+        PRINTM(MINFO, "GET_HW_SPEC: Presence of Rx antenna A\n");
+    }
+    if (ISSUPP_RXANTENNAB(cap)) {
+        PRINTM(MINFO, "GET_HW_SPEC: Presence of Rx antenna B\n");
+    }
+    if (ISSUPP_RXANTENNAC(cap)) {
+        PRINTM(MINFO, "GET_HW_SPEC: Presence of Rx antenna C\n");
+    }
+    if (ISSUPP_RXANTENNAD(cap)) {
+        PRINTM(MINFO, "GET_HW_SPEC: Presence of Rx antenna D\n");
+    }
+    if (ISSUPP_TXANTENNAA(cap)) {
+        PRINTM(MINFO, "GET_HW_SPEC: Presence of Tx antenna A\n");
+    }
+    if (ISSUPP_TXANTENNAB(cap)) {
+        PRINTM(MINFO, "GET_HW_SPEC: Presence of Tx antenna B\n");
+    }
+    if (ISSUPP_TXANTENNAC(cap)) {
+        PRINTM(MINFO, "GET_HW_SPEC: Presence of Tx antenna C\n");
+    }
+    if (ISSUPP_TXANTENNAD(cap)) {
+        PRINTM(MINFO, "GET_HW_SPEC: Presence of Tx antenna D\n");
+    }
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function prints the 802.11n device MCS
+ *
+ *  @param pmadapter A pointer to mlan_adapter structure
+ *  @param support   Support value
+ *
+ *  @return        N/A
+ */
+void
+wlan_show_devmcssupport(pmlan_adapter pmadapter, t_u8 support)
+{
+    ENTER();
+
+    PRINTM(MINFO, "GET_HW_SPEC: MCSs for %dx%d MIMO\n", GET_RXMCSSUPP(support),
+           GET_TXMCSSUPP(support));
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function handles the command response of
+ *              delete a block ack request
+ *
+ *  @param priv    A pointer to mlan_private structure
+ *  @param resp    A pointer to HostCmd_DS_COMMAND
+ *
+ *  @return        MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_11n_delba(mlan_private * priv, HostCmd_DS_COMMAND * resp)
+{
+    int tid;
+    TxBAStreamTbl *ptx_ba_tbl;
+    HostCmd_DS_11N_DELBA *pdel_ba =
+        (HostCmd_DS_11N_DELBA *) & resp->params.del_ba;
+
+    ENTER();
+
+    pdel_ba->del_ba_param_set = wlan_le16_to_cpu(pdel_ba->del_ba_param_set);
+    pdel_ba->reason_code = wlan_le16_to_cpu(pdel_ba->reason_code);
+
+    tid = pdel_ba->del_ba_param_set >> DELBA_TID_POS;
+    if (pdel_ba->del_result == BA_RESULT_SUCCESS) {
+        mlan_11n_delete_bastream_tbl(priv, tid, pdel_ba->peer_mac_addr,
+                                     TYPE_DELBA_SENT,
+                                     INITIATOR_BIT(pdel_ba->del_ba_param_set));
+
+        if ((ptx_ba_tbl = wlan_11n_get_txbastream_status(priv,
+                                                         BA_STREAM_SETUP_INPROGRESS)))
+        {
+            wlan_send_addba(priv, ptx_ba_tbl->tid, ptx_ba_tbl->ra);
+        }
+    } else {                    /*
+                                 * In case of failure, recreate the deleted stream in
+                                 * case we initiated the ADDBA
+                                 */
+        if (INITIATOR_BIT(pdel_ba->del_ba_param_set)) {
+            wlan_11n_create_txbastream_tbl(priv, pdel_ba->peer_mac_addr, tid,
+                                           BA_STREAM_SETUP_INPROGRESS);
+            if ((ptx_ba_tbl = wlan_11n_get_txbastream_status(priv,
+                                                             BA_STREAM_SETUP_INPROGRESS)))
+            {
+                mlan_11n_delete_bastream_tbl(priv, ptx_ba_tbl->tid,
+                                             ptx_ba_tbl->ra, TYPE_DELBA_SENT,
+                                             MTRUE);
+            }
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of
+ *              add a block ack request
+ *
+ *  @param priv    A pointer to mlan_private structure
+ *  @param resp    A pointer to HostCmd_DS_COMMAND
+ *
+ *  @return        MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_11n_addba_req(mlan_private * priv, HostCmd_DS_COMMAND * resp)
+{
+    t_u8 tid;
+    HostCmd_DS_11N_ADDBA_RSP *padd_ba_rsp =
+        (HostCmd_DS_11N_ADDBA_RSP *) & resp->params.add_ba_rsp;
+    TxBAStreamTbl *ptx_ba_tbl;
+    raListTbl *ra_list = MNULL;
+
+    ENTER();
+
+    padd_ba_rsp->block_ack_param_set =
+        wlan_le16_to_cpu(padd_ba_rsp->block_ack_param_set);
+    padd_ba_rsp->block_ack_tmo = wlan_le16_to_cpu(padd_ba_rsp->block_ack_tmo);
+    padd_ba_rsp->ssn = (wlan_le16_to_cpu(padd_ba_rsp->ssn)) & SSN_MASK;
+    padd_ba_rsp->status_code = wlan_le16_to_cpu(padd_ba_rsp->status_code);
+
+    tid = (padd_ba_rsp->block_ack_param_set & BLOCKACKPARAM_TID_MASK)
+        >> BLOCKACKPARAM_TID_POS;
+    if (padd_ba_rsp->status_code == BA_RESULT_SUCCESS) {
+        if ((ptx_ba_tbl = wlan_11n_get_txbastream_tbl(priv, tid,
+                                                      padd_ba_rsp->
+                                                      peer_mac_addr))) {
+            PRINTM(MCMND,
+                   "ADDBA REQ: %02x:%02x:%02x:%02x:%02x:%02x tid=%d ssn=%d win_size=%d,amsdu=%d\n",
+                   padd_ba_rsp->peer_mac_addr[0], padd_ba_rsp->peer_mac_addr[1],
+                   padd_ba_rsp->peer_mac_addr[2], padd_ba_rsp->peer_mac_addr[3],
+                   padd_ba_rsp->peer_mac_addr[4], padd_ba_rsp->peer_mac_addr[5],
+                   tid, padd_ba_rsp->ssn,
+                   ((padd_ba_rsp->
+                     block_ack_param_set & BLOCKACKPARAM_WINSIZE_MASK) >>
+                    BLOCKACKPARAM_WINSIZE_POS),
+                   padd_ba_rsp->
+                   block_ack_param_set & BLOCKACKPARAM_AMSDU_SUPP_MASK);
+            ptx_ba_tbl->ba_status = BA_STREAM_SETUP_COMPLETE;
+            if ((padd_ba_rsp->
+                 block_ack_param_set & BLOCKACKPARAM_AMSDU_SUPP_MASK) &&
+                priv->add_ba_param.tx_amsdu &&
+                (priv->aggr_prio_tbl[tid].amsdu != BA_STREAM_NOT_ALLOWED))
+                ptx_ba_tbl->amsdu = MTRUE;
+            else
+                ptx_ba_tbl->amsdu = MFALSE;
+        } else {
+            PRINTM(MERROR, "BA stream not created\n");
+        }
+    } else {
+        mlan_11n_delete_bastream_tbl(priv, tid, padd_ba_rsp->peer_mac_addr,
+                                     TYPE_DELBA_SENT, MTRUE);
+        if (padd_ba_rsp->add_rsp_result != BA_RESULT_TIMEOUT) {
+#ifdef UAP_SUPPORT
+            if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP)
+                disable_station_ampdu(priv, tid, padd_ba_rsp->peer_mac_addr);
+#endif /* UAP_SUPPORT */
+            priv->aggr_prio_tbl[tid].ampdu_ap = BA_STREAM_NOT_ALLOWED;
+
+        } else {
+            /* reset packet threshold */
+            ra_list =
+                wlan_wmm_get_ralist_node(priv, tid, padd_ba_rsp->peer_mac_addr);
+            if (ra_list) {
+                ra_list->packet_count = 0;
+                ra_list->ba_packet_threshold =
+                    wlan_get_random_ba_threshold(priv->adapter);
+            }
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of reconfigure tx buf
+ *
+ *  @param priv         A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_recfg_tx_buf(mlan_private * priv,
+                      HostCmd_DS_COMMAND * cmd, int cmd_action, void *pdata_buf)
+{
+    HostCmd_DS_TXBUF_CFG *ptx_buf = &cmd->params.tx_buf;
+    t_u16 action = (t_u16) cmd_action;
+    t_u16 buf_size = *((t_u16 *) pdata_buf);
+
+    ENTER();
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_RECONFIGURE_TX_BUFF);
+    cmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_TXBUF_CFG)
+                                 + S_DS_GEN);
+    ptx_buf->action = wlan_cpu_to_le16(action);
+    switch (action) {
+    case HostCmd_ACT_GEN_SET:
+        PRINTM(MCMND, "set tx_buf = %d\n", buf_size);
+        ptx_buf->buff_size = wlan_cpu_to_le16(buf_size);
+        break;
+    case HostCmd_ACT_GEN_GET:
+    default:
+        ptx_buf->buff_size = 0;
+        break;
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of amsdu aggr control
+ *
+ *  @param priv         A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_amsdu_aggr_ctrl(mlan_private * priv,
+                         HostCmd_DS_COMMAND * cmd,
+                         int cmd_action, void *pdata_buf)
+{
+    HostCmd_DS_AMSDU_AGGR_CTRL *pamsdu_ctrl = &cmd->params.amsdu_aggr_ctrl;
+    t_u16 action = (t_u16) cmd_action;
+    mlan_ds_11n_amsdu_aggr_ctrl *aa_ctrl = (mlan_ds_11n_amsdu_aggr_ctrl *)
+        pdata_buf;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_AMSDU_AGGR_CTRL);
+    cmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_AMSDU_AGGR_CTRL)
+                                 + S_DS_GEN);
+    pamsdu_ctrl->action = wlan_cpu_to_le16(action);
+    switch (action) {
+    case HostCmd_ACT_GEN_SET:
+        pamsdu_ctrl->enable = wlan_cpu_to_le16(aa_ctrl->enable);
+        pamsdu_ctrl->curr_buf_size = 0;
+        break;
+    case HostCmd_ACT_GEN_GET:
+    default:
+        pamsdu_ctrl->curr_buf_size = 0;
+        break;
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of amsdu aggr ctrl
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return        MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_amsdu_aggr_ctrl(IN pmlan_private pmpriv,
+                         IN HostCmd_DS_COMMAND * resp,
+                         IN mlan_ioctl_req * pioctl_buf)
+{
+    mlan_ds_11n_cfg *cfg = MNULL;
+    HostCmd_DS_AMSDU_AGGR_CTRL *amsdu_ctrl = &resp->params.amsdu_aggr_ctrl;
+
+    ENTER();
+
+    if (pioctl_buf) {
+        cfg = (mlan_ds_11n_cfg *) pioctl_buf->pbuf;
+        cfg->param.amsdu_aggr_ctrl.enable =
+            wlan_le16_to_cpu(amsdu_ctrl->enable);
+        cfg->param.amsdu_aggr_ctrl.curr_buf_size =
+            wlan_le16_to_cpu(amsdu_ctrl->curr_buf_size);
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares 11n cfg command
+ *
+ *  @param pmpriv    	A pointer to mlan_private structure
+ *  @param cmd	   	A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action 	the action: GET or SET
+ *  @param pdata_buf 	A pointer to data buffer
+ *  @return 	   	MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_11n_cfg(IN pmlan_private pmpriv,
+                 IN HostCmd_DS_COMMAND * cmd,
+                 IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    HostCmd_DS_11N_CFG *htcfg = &cmd->params.htcfg;
+    mlan_ds_11n_tx_cfg *txcfg = (mlan_ds_11n_tx_cfg *) pdata_buf;
+
+    ENTER();
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_11N_CFG);
+    cmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_11N_CFG) + S_DS_GEN);
+    htcfg->action = wlan_cpu_to_le16(cmd_action);
+    htcfg->ht_tx_cap = wlan_cpu_to_le16(txcfg->httxcap);
+    htcfg->ht_tx_info = wlan_cpu_to_le16(txcfg->httxinfo);
+    htcfg->misc_config = wlan_cpu_to_le16(txcfg->misc_cfg);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of 11ncfg
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return        MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_11n_cfg(IN pmlan_private pmpriv,
+                 IN HostCmd_DS_COMMAND * resp, IN mlan_ioctl_req * pioctl_buf)
+{
+    mlan_ds_11n_cfg *cfg = MNULL;
+    HostCmd_DS_11N_CFG *htcfg = &resp->params.htcfg;
+
+    ENTER();
+    if (pioctl_buf && (wlan_le16_to_cpu(htcfg->action) == HostCmd_ACT_GEN_GET)) {
+        cfg = (mlan_ds_11n_cfg *) pioctl_buf->pbuf;
+        cfg->param.tx_cfg.httxcap = wlan_le16_to_cpu(htcfg->ht_tx_cap);
+        cfg->param.tx_cfg.httxinfo = wlan_le16_to_cpu(htcfg->ht_tx_info);
+        cfg->param.tx_cfg.misc_cfg = wlan_le16_to_cpu(htcfg->misc_config);
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares TX BF configuration command
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_cmd_tx_bf_cfg(IN pmlan_private pmpriv,
+                   IN HostCmd_DS_COMMAND * cmd,
+                   IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    pmlan_adapter pmadapter = pmpriv->adapter;
+    HostCmd_DS_TX_BF_CFG *txbfcfg = &cmd->params.tx_bf_cfg;
+    mlan_ds_11n_tx_bf_cfg *txbf = (mlan_ds_11n_tx_bf_cfg *) pdata_buf;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_TX_BF_CFG);
+    cmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_TX_BF_CFG) + S_DS_GEN);
+
+    if (txbf->bf_action == SET_GET_BF_PERIODICITY) {
+        memcpy(pmadapter, txbfcfg->body.bf_periodicity.peer_mac,
+               txbf->body.bf_periodicity[0].peer_mac, MLAN_MAC_ADDR_LENGTH);
+    }
+    txbfcfg->action = wlan_cpu_to_le16(txbf->action);
+    txbfcfg->bf_action = wlan_cpu_to_le16(txbf->bf_action);
+    if (cmd_action == HostCmd_ACT_GEN_SET) {
+        switch (txbf->bf_action) {
+        case BF_GLOBAL_CONFIGURATION:
+            txbfcfg->body.bf_global_cfg.bf_enbl =
+                txbf->body.bf_global_cfg.bf_enbl;
+            txbfcfg->body.bf_global_cfg.sounding_enbl =
+                txbf->body.bf_global_cfg.sounding_enbl;
+            txbfcfg->body.bf_global_cfg.fb_type =
+                txbf->body.bf_global_cfg.fb_type;
+            txbfcfg->body.bf_global_cfg.snr_threshold =
+                txbf->body.bf_global_cfg.snr_threshold;
+            txbfcfg->body.bf_global_cfg.sounding_interval =
+                wlan_cpu_to_le16(txbf->body.bf_global_cfg.sounding_interval);
+            txbfcfg->body.bf_global_cfg.bf_mode =
+                txbf->body.bf_global_cfg.bf_mode;
+            break;
+        case TRIGGER_SOUNDING_FOR_PEER:
+            memcpy(pmadapter, txbfcfg->body.bf_sound_args.peer_mac,
+                   txbf->body.bf_sound[0].peer_mac, MLAN_MAC_ADDR_LENGTH);
+            break;
+        case SET_GET_BF_PERIODICITY:
+            txbfcfg->body.bf_periodicity.interval =
+                wlan_cpu_to_le16(txbf->body.bf_periodicity->interval);
+            break;
+        case TX_BF_FOR_PEER_ENBL:
+            memcpy(pmadapter, txbfcfg->body.tx_bf_peer.peer_mac,
+                   txbf->body.tx_bf_peer[0].peer_mac, MLAN_MAC_ADDR_LENGTH);
+            txbfcfg->body.tx_bf_peer.bf_enbl = txbf->body.tx_bf_peer[0].bf_enbl;
+            txbfcfg->body.tx_bf_peer.sounding_enbl =
+                txbf->body.tx_bf_peer[0].sounding_enbl;
+            txbfcfg->body.tx_bf_peer.fb_type = txbf->body.tx_bf_peer[0].fb_type;
+            break;
+        case SET_SNR_THR_PEER:
+            memcpy(pmadapter, txbfcfg->body.bf_snr.peer_mac,
+                   txbf->body.bf_snr[0].peer_mac, MLAN_MAC_ADDR_LENGTH);
+            txbfcfg->body.bf_snr.snr = txbf->body.bf_snr[0].snr;
+            break;
+        default:
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response
+ *  of TX BF configuration
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ret_tx_bf_cfg(IN pmlan_private pmpriv,
+                   IN HostCmd_DS_COMMAND * resp, IN mlan_ioctl_req * pioctl_buf)
+{
+    pmlan_adapter pmadapter = pmpriv->adapter;
+    HostCmd_DS_TX_BF_CFG *txbfcfg = &resp->params.tx_bf_cfg;
+    mlan_ds_11n_cfg *cfg_11n = MNULL;
+    mlan_ds_11n_tx_bf_cfg *txbf = MNULL;
+    bf_peer_args *tx_bf_peer;
+    bf_snr_thr_t *bf_snr;
+    int i;
+
+    ENTER();
+
+    if (pioctl_buf) {
+        cfg_11n = (mlan_ds_11n_cfg *) pioctl_buf->pbuf;
+        txbf = (mlan_ds_11n_tx_bf_cfg *) & cfg_11n->param.tx_bf;
+        txbf->bf_action = wlan_le16_to_cpu(txbfcfg->bf_action);
+        switch (txbf->bf_action) {
+        case BF_GLOBAL_CONFIGURATION:
+            txbf->body.bf_global_cfg.bf_enbl =
+                txbfcfg->body.bf_global_cfg.bf_enbl;
+            txbf->body.bf_global_cfg.sounding_enbl =
+                txbfcfg->body.bf_global_cfg.sounding_enbl;
+            txbf->body.bf_global_cfg.fb_type =
+                txbfcfg->body.bf_global_cfg.fb_type;
+            txbf->body.bf_global_cfg.snr_threshold =
+                txbfcfg->body.bf_global_cfg.snr_threshold;
+            txbf->body.bf_global_cfg.sounding_interval =
+                wlan_le16_to_cpu(txbfcfg->body.bf_global_cfg.sounding_interval);
+            txbf->body.bf_global_cfg.bf_mode =
+                txbfcfg->body.bf_global_cfg.bf_mode;
+            break;
+        case TRIGGER_SOUNDING_FOR_PEER:
+            memcpy(pmadapter, txbf->body.bf_sound[0].peer_mac,
+                   txbfcfg->body.bf_sound_args.peer_mac, MLAN_MAC_ADDR_LENGTH);
+            txbf->body.bf_sound[0].status = txbfcfg->body.bf_sound_args.status;
+            break;
+        case SET_GET_BF_PERIODICITY:
+            memcpy(pmadapter, txbf->body.bf_periodicity->peer_mac,
+                   txbfcfg->body.bf_periodicity.peer_mac, MLAN_MAC_ADDR_LENGTH);
+            txbf->body.bf_periodicity->interval =
+                wlan_le16_to_cpu(txbfcfg->body.bf_periodicity.interval);
+            break;
+        case TX_BF_FOR_PEER_ENBL:
+            txbf->no_of_peers = *(t_u8 *) & txbfcfg->body;
+            tx_bf_peer =
+                (bf_peer_args *) ((t_u8 *) & txbfcfg->body + sizeof(t_u8));
+            for (i = 0; i < txbf->no_of_peers; i++) {
+                memcpy(pmadapter, txbf->body.tx_bf_peer[i].peer_mac,
+                       (t_u8 *) tx_bf_peer->peer_mac, MLAN_MAC_ADDR_LENGTH);
+                txbf->body.tx_bf_peer[i].bf_enbl = tx_bf_peer->bf_enbl;
+                txbf->body.tx_bf_peer[i].sounding_enbl =
+                    tx_bf_peer->sounding_enbl;
+                txbf->body.tx_bf_peer[i].fb_type = tx_bf_peer->fb_type;
+                tx_bf_peer++;
+            }
+            break;
+        case SET_SNR_THR_PEER:
+            txbf->no_of_peers = *(t_u8 *) & txbfcfg->body;
+            bf_snr = (bf_snr_thr_t *) ((t_u8 *) & txbfcfg->body + sizeof(t_u8));
+            for (i = 0; i < txbf->no_of_peers; i++) {
+                memcpy(pmadapter, txbf->body.bf_snr[i].peer_mac,
+                       (t_u8 *) bf_snr->peer_mac, MLAN_MAC_ADDR_LENGTH);
+                txbf->body.bf_snr[i].snr = bf_snr->snr;
+                bf_snr++;
+            }
+            break;
+        default:
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+#ifdef STA_SUPPORT
+
+/**
+ *  @brief This function append the 802_11N tlv
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pbss_desc    A pointer to BSSDescriptor_t structure
+ *  @param ppbuffer     A Pointer to command buffer pointer
+ *
+ *  @return bytes added to the buffer
+ */
+int
+wlan_cmd_append_11n_tlv(IN mlan_private * pmpriv,
+                        IN BSSDescriptor_t * pbss_desc, OUT t_u8 ** ppbuffer)
+{
+    pmlan_adapter pmadapter = pmpriv->adapter;
+    MrvlIETypes_HTCap_t *pht_cap;
+    MrvlIETypes_HTInfo_t *pht_info;
+    MrvlIEtypes_ChanListParamSet_t *pchan_list;
+    MrvlIETypes_2040BSSCo_t *p2040_bss_co;
+    MrvlIETypes_ExtCap_t *pext_cap;
+    t_u32 usr_dot_11n_dev_cap;
+    int ret_len = 0;
+
+    ENTER();
+
+    /* Null Checks */
+    if (ppbuffer == MNULL) {
+        LEAVE();
+        return 0;
+    }
+    if (*ppbuffer == MNULL) {
+        LEAVE();
+        return 0;
+    }
+
+    if (pbss_desc->bss_band & BAND_A)
+        usr_dot_11n_dev_cap = pmadapter->usr_dot_11n_dev_cap_a;
+    else
+        usr_dot_11n_dev_cap = pmadapter->usr_dot_11n_dev_cap_bg;
+
+    if (pbss_desc->pht_cap) {
+        pht_cap = (MrvlIETypes_HTCap_t *) * ppbuffer;
+        memset(pmadapter, pht_cap, 0, sizeof(MrvlIETypes_HTCap_t));
+        pht_cap->header.type = wlan_cpu_to_le16(HT_CAPABILITY);
+        pht_cap->header.len = sizeof(HTCap_t);
+        memcpy(pmadapter, (t_u8 *) pht_cap + sizeof(MrvlIEtypesHeader_t),
+               (t_u8 *) pbss_desc->pht_cap + sizeof(IEEEtypes_Header_t),
+               pht_cap->header.len);
+
+        pht_cap->ht_cap.ht_cap_info =
+            wlan_le16_to_cpu(pht_cap->ht_cap.ht_cap_info);
+        pht_cap->ht_cap.ht_ext_cap =
+            wlan_le16_to_cpu(pht_cap->ht_cap.ht_ext_cap);
+        wlan_fill_ht_cap_tlv(pmpriv, pht_cap, pbss_desc->bss_band);
+
+        HEXDUMP("HT_CAPABILITIES IE", (t_u8 *) pht_cap,
+                sizeof(MrvlIETypes_HTCap_t));
+        *ppbuffer += sizeof(MrvlIETypes_HTCap_t);
+        ret_len += sizeof(MrvlIETypes_HTCap_t);
+        pht_cap->header.len = wlan_cpu_to_le16(pht_cap->header.len);
+    }
+
+    if (pbss_desc->pht_info) {
+        if (pmpriv->bss_mode == MLAN_BSS_MODE_IBSS) {
+            pht_info = (MrvlIETypes_HTInfo_t *) * ppbuffer;
+            memset(pmadapter, pht_info, 0, sizeof(MrvlIETypes_HTInfo_t));
+            pht_info->header.type = wlan_cpu_to_le16(HT_OPERATION);
+            pht_info->header.len = sizeof(HTInfo_t);
+
+            memcpy(pmadapter, (t_u8 *) pht_info + sizeof(MrvlIEtypesHeader_t),
+                   (t_u8 *) pbss_desc->pht_info + sizeof(IEEEtypes_Header_t),
+                   pht_info->header.len);
+
+            if (!ISSUPP_CHANWIDTH40(usr_dot_11n_dev_cap)) {
+                RESET_CHANWIDTH40(pht_info->ht_info.field2);
+            }
+
+            *ppbuffer += sizeof(MrvlIETypes_HTInfo_t);
+            ret_len += sizeof(MrvlIETypes_HTInfo_t);
+            pht_info->header.len = wlan_cpu_to_le16(pht_info->header.len);
+        }
+
+        pchan_list = (MrvlIEtypes_ChanListParamSet_t *) * ppbuffer;
+        memset(pmadapter, pchan_list, 0,
+               sizeof(MrvlIEtypes_ChanListParamSet_t));
+        pchan_list->header.type = wlan_cpu_to_le16(TLV_TYPE_CHANLIST);
+        pchan_list->header.len = sizeof(MrvlIEtypes_ChanListParamSet_t) -
+            sizeof(MrvlIEtypesHeader_t);
+        pchan_list->chan_scan_param[0].chan_number =
+            pbss_desc->pht_info->ht_info.pri_chan;
+        pchan_list->chan_scan_param[0].radio_type =
+            wlan_band_to_radio_type((t_u8) pbss_desc->bss_band);
+        if (ISSUPP_CHANWIDTH40(usr_dot_11n_dev_cap) &&
+            ISALLOWED_CHANWIDTH40(pbss_desc->pht_info->ht_info.field2)) {
+            SET_SECONDARYCHAN(pchan_list->chan_scan_param[0].radio_type,
+                              GET_SECONDARYCHAN(pbss_desc->pht_info->ht_info.
+                                                field2));
+        }
+
+        HEXDUMP("ChanList", (t_u8 *) pchan_list,
+                sizeof(MrvlIEtypes_ChanListParamSet_t));
+        HEXDUMP("pht_info", (t_u8 *) pbss_desc->pht_info,
+                sizeof(MrvlIETypes_HTInfo_t) - 2);
+        *ppbuffer += sizeof(MrvlIEtypes_ChanListParamSet_t);
+        ret_len += sizeof(MrvlIEtypes_ChanListParamSet_t);
+        pchan_list->header.len = wlan_cpu_to_le16(pchan_list->header.len);
+    }
+
+    if (pbss_desc->pbss_co_2040) {
+        p2040_bss_co = (MrvlIETypes_2040BSSCo_t *) * ppbuffer;
+        memset(pmadapter, p2040_bss_co, 0, sizeof(MrvlIETypes_2040BSSCo_t));
+        p2040_bss_co->header.type = wlan_cpu_to_le16(BSSCO_2040);
+        p2040_bss_co->header.len = sizeof(BSSCo2040_t);
+
+        memcpy(pmadapter, (t_u8 *) p2040_bss_co + sizeof(MrvlIEtypesHeader_t),
+               (t_u8 *) pbss_desc->pbss_co_2040 + sizeof(IEEEtypes_Header_t),
+               p2040_bss_co->header.len);
+
+        HEXDUMP("20/40 BSS Coexistence IE", (t_u8 *) p2040_bss_co,
+                sizeof(MrvlIETypes_2040BSSCo_t));
+        *ppbuffer += sizeof(MrvlIETypes_2040BSSCo_t);
+        ret_len += sizeof(MrvlIETypes_2040BSSCo_t);
+        p2040_bss_co->header.len = wlan_cpu_to_le16(p2040_bss_co->header.len);
+    }
+
+    if (pbss_desc->pext_cap) {
+        pext_cap = (MrvlIETypes_ExtCap_t *) * ppbuffer;
+        memset(pmadapter, pext_cap, 0, sizeof(MrvlIETypes_ExtCap_t));
+        pext_cap->header.type = wlan_cpu_to_le16(EXT_CAPABILITY);
+        pext_cap->header.len = sizeof(ExtCap_t);
+
+        memcpy(pmadapter, (t_u8 *) pext_cap + sizeof(MrvlIEtypesHeader_t),
+               (t_u8 *) pbss_desc->pext_cap + sizeof(IEEEtypes_Header_t),
+               pbss_desc->pext_cap->ieee_hdr.len);
+        HEXDUMP("Extended Capabilities IE", (t_u8 *) pext_cap,
+                sizeof(MrvlIETypes_ExtCap_t));
+        *ppbuffer += sizeof(MrvlIETypes_ExtCap_t);
+        ret_len += sizeof(MrvlIETypes_ExtCap_t);
+        pext_cap->header.len = wlan_cpu_to_le16(pext_cap->header.len);
+    }
+    LEAVE();
+    return ret_len;
+}
+
+#endif /* STA_SUPPORT */
+
+/**
+ *  @brief 11n configuration handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+mlan_status
+wlan_11n_cfg_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_11n_cfg *cfg = MNULL;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_11n_cfg)) {
+        PRINTM(MINFO, "MLAN bss IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_11n_cfg);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+    cfg = (mlan_ds_11n_cfg *) pioctl_req->pbuf;
+    switch (cfg->sub_command) {
+    case MLAN_OID_11N_CFG_TX:
+        status = wlan_11n_ioctl_httxcfg(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_11N_HTCAP_CFG:
+        status = wlan_11n_ioctl_htusrcfg(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_11N_CFG_AGGR_PRIO_TBL:
+        status = wlan_11n_ioctl_aggr_prio_tbl(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_11N_CFG_ADDBA_REJECT:
+        status = wlan_11n_ioctl_addba_reject(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_11N_CFG_ADDBA_PARAM:
+        status = wlan_11n_ioctl_addba_param(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_11N_CFG_MAX_TX_BUF_SIZE:
+        status = wlan_11n_ioctl_max_tx_buf_size(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_11N_CFG_AMSDU_AGGR_CTRL:
+        status = wlan_11n_ioctl_amsdu_aggr_ctrl(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_11N_CFG_SUPPORTED_MCS_SET:
+        status = wlan_11n_ioctl_supported_mcs_set(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_11N_CFG_TX_BF_CAP:
+        status = wlan_11n_ioctl_tx_bf_cap(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_11N_CFG_TX_BF_CFG:
+        status = wlan_11n_ioctl_tx_bf_cfg(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_11N_CFG_STREAM_CFG:
+        status = wlan_11n_ioctl_stream_cfg(pmadapter, pioctl_req);
+        break;
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief This function will delete the given entry in Tx BA Stream table
+ *
+ *  @param priv    	Pointer to mlan_private
+ *  @param ptx_tbl	Pointer to tx ba stream entry to delete
+ *
+ *  @return 	        N/A
+ */
+void
+wlan_11n_delete_txbastream_tbl_entry(mlan_private * priv,
+                                     TxBAStreamTbl * ptx_tbl)
+{
+    pmlan_adapter pmadapter = priv->adapter;
+
+    ENTER();
+
+    pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                        priv->tx_ba_stream_tbl_ptr.plock);
+
+    if (!ptx_tbl || !wlan_is_txbastreamptr_valid(priv, ptx_tbl)) {
+        goto exit;
+    }
+
+    PRINTM(MINFO, "Delete BA stream table entry: %p\n", ptx_tbl);
+
+    util_unlink_list(pmadapter->pmoal_handle, &priv->tx_ba_stream_tbl_ptr,
+                     (pmlan_linked_list) ptx_tbl, MNULL, MNULL);
+
+    pmadapter->callbacks.moal_mfree(pmadapter->pmoal_handle, (t_u8 *) ptx_tbl);
+
+  exit:
+    pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                          priv->tx_ba_stream_tbl_ptr.plock);
+    LEAVE();
+}
+
+/**
+ *  @brief This function will delete all the entries in Tx BA Stream table
+ *
+ *  @param priv         A pointer to mlan_private
+ *
+ *  @return             N/A
+ */
+void
+wlan_11n_deleteall_txbastream_tbl(mlan_private * priv)
+{
+    int i;
+    TxBAStreamTbl *del_tbl_ptr = MNULL;
+
+    ENTER();
+
+    while ((del_tbl_ptr = (TxBAStreamTbl *)
+            util_peek_list(priv->adapter->pmoal_handle,
+                           &priv->tx_ba_stream_tbl_ptr,
+                           priv->adapter->callbacks.moal_spin_lock,
+                           priv->adapter->callbacks.moal_spin_unlock))) {
+        wlan_11n_delete_txbastream_tbl_entry(priv, del_tbl_ptr);
+    }
+
+    util_init_list((pmlan_linked_list) & priv->tx_ba_stream_tbl_ptr);
+
+    for (i = 0; i < MAX_NUM_TID; ++i) {
+        priv->aggr_prio_tbl[i].ampdu_ap = priv->aggr_prio_tbl[i].ampdu_user;
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function will return the pointer to an entry in BA Stream
+ *  		table which matches the give RA/TID pair
+ *
+ *  @param priv    A pointer to mlan_private
+ *  @param tid	   TID to find in reordering table
+ *  @param ra      RA to find in reordering table
+ *
+ *  @return 	   A pointer to first entry matching RA/TID in BA stream
+ *                 NULL if not found
+ */
+TxBAStreamTbl *
+wlan_11n_get_txbastream_tbl(mlan_private * priv, int tid, t_u8 * ra)
+{
+    TxBAStreamTbl *ptx_tbl;
+    pmlan_adapter pmadapter = priv->adapter;
+
+    ENTER();
+
+    if (!(ptx_tbl = (TxBAStreamTbl *) util_peek_list(pmadapter->pmoal_handle,
+                                                     &priv->
+                                                     tx_ba_stream_tbl_ptr,
+                                                     pmadapter->callbacks.
+                                                     moal_spin_lock,
+                                                     pmadapter->callbacks.
+                                                     moal_spin_unlock))) {
+        LEAVE();
+        return MNULL;
+    }
+
+    while (ptx_tbl != (TxBAStreamTbl *) & priv->tx_ba_stream_tbl_ptr) {
+
+        PRINTM(MDAT_D, "get_txbastream_tbl TID %d\n", ptx_tbl->tid);
+        DBG_HEXDUMP(MDAT_D, "RA", ptx_tbl->ra, MLAN_MAC_ADDR_LENGTH);
+
+        if ((!memcmp(pmadapter, ptx_tbl->ra, ra, MLAN_MAC_ADDR_LENGTH))
+            && (ptx_tbl->tid == tid)) {
+            LEAVE();
+            return ptx_tbl;
+        }
+
+        ptx_tbl = ptx_tbl->pnext;
+    }
+
+    LEAVE();
+    return MNULL;
+}
+
+/**
+ *  @brief This function will create a entry in tx ba stream table for the
+ *  		given RA/TID.
+ *
+ *  @param priv      A pointer to mlan_private
+ *  @param ra        RA to find in reordering table
+ *  @param tid	     TID to find in reordering table
+ *  @param ba_status BA stream status to create the stream with
+ *
+ *  @return 	    N/A
+ */
+void
+wlan_11n_create_txbastream_tbl(mlan_private * priv,
+                               t_u8 * ra, int tid, baStatus_e ba_status)
+{
+    TxBAStreamTbl *newNode = MNULL;
+    pmlan_adapter pmadapter = priv->adapter;
+
+    ENTER();
+
+    if (!wlan_11n_get_txbastream_tbl(priv, tid, ra)) {
+        PRINTM(MDAT_D, "get_txbastream_tbl TID %d\n", tid);
+        DBG_HEXDUMP(MDAT_D, "RA", ra, MLAN_MAC_ADDR_LENGTH);
+
+        pmadapter->callbacks.moal_malloc(pmadapter->pmoal_handle,
+                                         sizeof(TxBAStreamTbl), MLAN_MEM_DEF,
+                                         (t_u8 **) & newNode);
+        util_init_list((pmlan_linked_list) newNode);
+
+        newNode->tid = tid;
+        newNode->ba_status = ba_status;
+        memcpy(pmadapter, newNode->ra, ra, MLAN_MAC_ADDR_LENGTH);
+
+        util_enqueue_list_tail(pmadapter->pmoal_handle,
+                               &priv->tx_ba_stream_tbl_ptr,
+                               (pmlan_linked_list) newNode,
+                               pmadapter->callbacks.moal_spin_lock,
+                               pmadapter->callbacks.moal_spin_unlock);
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function will send a block ack to given tid/ra
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param tid	    TID to send the ADDBA
+ *  @param peer_mac MAC address to send the ADDBA
+ *
+ *  @return 	    MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+int
+wlan_send_addba(mlan_private * priv, int tid, t_u8 * peer_mac)
+{
+    HostCmd_DS_11N_ADDBA_REQ add_ba_req;
+    static t_u8 dialog_tok;
+    mlan_status ret;
+
+    ENTER();
+
+    PRINTM(MCMND, "Send addba: TID %d\n", tid);
+    DBG_HEXDUMP(MCMD_D, "Send addba RA", peer_mac, MLAN_MAC_ADDR_LENGTH);
+
+    add_ba_req.block_ack_param_set = (t_u16) ((tid << BLOCKACKPARAM_TID_POS) |
+                                              (priv->add_ba_param.tx_win_size
+                                               << BLOCKACKPARAM_WINSIZE_POS) |
+                                              IMMEDIATE_BLOCK_ACK);
+    /** enable AMSDU inside AMPDU */
+    if (priv->add_ba_param.tx_amsdu &&
+        (priv->aggr_prio_tbl[tid].amsdu != BA_STREAM_NOT_ALLOWED))
+        add_ba_req.block_ack_param_set |= BLOCKACKPARAM_AMSDU_SUPP_MASK;
+    add_ba_req.block_ack_tmo = (t_u16) priv->add_ba_param.timeout;
+
+    ++dialog_tok;
+
+    if (dialog_tok == 0)
+        dialog_tok = 1;
+
+    add_ba_req.dialog_token = dialog_tok;
+    memcpy(priv->adapter, &add_ba_req.peer_mac_addr, peer_mac,
+           MLAN_MAC_ADDR_LENGTH);
+
+    /* We don't wait for the response of this command */
+    ret = wlan_prepare_cmd(priv, HostCmd_CMD_11N_ADDBA_REQ,
+                           0, 0, MNULL, &add_ba_req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function will delete a block ack to given tid/ra
+ *
+ *  @param priv    		A pointer to mlan_private
+ *  @param tid	   		TID to send the ADDBA
+ *  @param peer_mac 	MAC address to send the ADDBA
+ *  @param initiator 	MTRUE if we have initiated ADDBA, MFALSE otherwise
+ *
+ *  @return 	        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+int
+wlan_send_delba(mlan_private * priv, int tid, t_u8 * peer_mac, int initiator)
+{
+    HostCmd_DS_11N_DELBA delba;
+    mlan_status ret;
+
+    ENTER();
+
+    memset(priv->adapter, &delba, 0, sizeof(delba));
+    delba.del_ba_param_set = (tid << DELBA_TID_POS);
+
+    if (initiator)
+        DELBA_INITIATOR(delba.del_ba_param_set);
+    else
+        DELBA_RECIPIENT(delba.del_ba_param_set);
+
+    memcpy(priv->adapter, &delba.peer_mac_addr, peer_mac, MLAN_MAC_ADDR_LENGTH);
+
+    /* We don't wait for the response of this command */
+    ret = wlan_prepare_cmd(priv, HostCmd_CMD_11N_DELBA,
+                           HostCmd_ACT_GEN_SET, 0, MNULL, &delba);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function handles the command response of
+ *  		delete a block ack request
+ *
+ *  @param priv		A pointer to mlan_private structure
+ *  @param del_ba	A pointer to command response buffer
+ *
+ *  @return        N/A
+ */
+void
+wlan_11n_delete_bastream(mlan_private * priv, t_u8 * del_ba)
+{
+    HostCmd_DS_11N_DELBA *pdel_ba = (HostCmd_DS_11N_DELBA *) del_ba;
+    int tid;
+
+    ENTER();
+
+    DBG_HEXDUMP(MCMD_D, "Delba:", (t_u8 *) pdel_ba, 20);
+    pdel_ba->del_ba_param_set = wlan_le16_to_cpu(pdel_ba->del_ba_param_set);
+    pdel_ba->reason_code = wlan_le16_to_cpu(pdel_ba->reason_code);
+
+    tid = pdel_ba->del_ba_param_set >> DELBA_TID_POS;
+
+    mlan_11n_delete_bastream_tbl(priv, tid, pdel_ba->peer_mac_addr,
+                                 TYPE_DELBA_RECEIVE,
+                                 INITIATOR_BIT(pdel_ba->del_ba_param_set));
+
+    LEAVE();
+}
+
+/**
+ *  @brief Get Rx reordering table
+ *
+ *  @param priv         A pointer to mlan_private structure
+ *  @param buf          A pointer to rx_reorder_tbl structure
+ *  @return             number of rx reorder table entry
+ */
+int
+wlan_get_rxreorder_tbl(mlan_private * priv, rx_reorder_tbl * buf)
+{
+    int i;
+    rx_reorder_tbl *ptbl = buf;
+    RxReorderTbl *rxReorderTblPtr;
+    int count = 0;
+    ENTER();
+    if (!
+        (rxReorderTblPtr =
+         (RxReorderTbl *) util_peek_list(priv->adapter->pmoal_handle,
+                                         &priv->rx_reorder_tbl_ptr,
+                                         priv->adapter->callbacks.
+                                         moal_spin_lock,
+                                         priv->adapter->callbacks.
+                                         moal_spin_unlock))) {
+        LEAVE();
+        return count;
+    }
+    while (rxReorderTblPtr != (RxReorderTbl *) & priv->rx_reorder_tbl_ptr) {
+        ptbl->tid = (t_u16) rxReorderTblPtr->tid;
+        memcpy(priv->adapter, ptbl->ta, rxReorderTblPtr->ta,
+               MLAN_MAC_ADDR_LENGTH);
+        ptbl->start_win = rxReorderTblPtr->start_win;
+        ptbl->win_size = rxReorderTblPtr->win_size;
+        ptbl->amsdu = rxReorderTblPtr->amsdu;
+        for (i = 0; i < rxReorderTblPtr->win_size; ++i) {
+            if (rxReorderTblPtr->rx_reorder_ptr[i])
+                ptbl->buffer[i] = MTRUE;
+            else
+                ptbl->buffer[i] = MFALSE;
+        }
+        rxReorderTblPtr = rxReorderTblPtr->pnext;
+        ptbl++;
+        count++;
+        if (count >= MLAN_MAX_RX_BASTREAM_SUPPORTED)
+            break;
+    }
+    LEAVE();
+    return count;
+}
+
+/**
+ *  @brief Get transmit BA stream table
+ *
+ *  @param priv         A pointer to mlan_private structure
+ *  @param buf          A pointer to tx_ba_stream_tbl structure
+ *  @return             number of ba stream table entry
+ */
+int
+wlan_get_txbastream_tbl(mlan_private * priv, tx_ba_stream_tbl * buf)
+{
+    TxBAStreamTbl *ptxtbl;
+    tx_ba_stream_tbl *ptbl = buf;
+    int count = 0;
+
+    ENTER();
+
+    if (!(ptxtbl = (TxBAStreamTbl *) util_peek_list(priv->adapter->pmoal_handle,
+                                                    &priv->tx_ba_stream_tbl_ptr,
+                                                    priv->adapter->callbacks.
+                                                    moal_spin_lock,
+                                                    priv->adapter->callbacks.
+                                                    moal_spin_unlock))) {
+        LEAVE();
+        return count;
+    }
+
+    while (ptxtbl != (TxBAStreamTbl *) & priv->tx_ba_stream_tbl_ptr) {
+        ptbl->tid = (t_u16) ptxtbl->tid;
+        PRINTM(MINFO, "tid=%d\n", ptbl->tid);
+        memcpy(priv->adapter, ptbl->ra, ptxtbl->ra, MLAN_MAC_ADDR_LENGTH);
+        ptbl->amsdu = ptxtbl->amsdu;
+        ptxtbl = ptxtbl->pnext;
+        ptbl++;
+        count++;
+        if (count >= MLAN_MAX_TX_BASTREAM_SUPPORTED)
+            break;
+    }
+
+    LEAVE();
+    return count;
+}
+
+/**
+ *  @brief This function cleans up txbastream_tbl for specific station
+ *
+ *  @param priv    	A pointer to mlan_private
+ *  @param ra       RA to find in txbastream_tbl
+ *  @return 	   	N/A
+ */
+void
+wlan_11n_cleanup_txbastream_tbl(mlan_private * priv, t_u8 * ra)
+{
+    TxBAStreamTbl *ptx_tbl = MNULL;
+    t_u8 i;
+    ENTER();
+    for (i = 0; i < MAX_NUM_TID; ++i) {
+        if ((ptx_tbl = wlan_11n_get_txbastream_tbl(priv, i, ra))) {
+            wlan_11n_delete_txbastream_tbl_entry(priv, ptx_tbl);
+        }
+    }
+    LEAVE();
+    return;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_11n.h b/drivers/net/wireless/sd8797/mlan/mlan_11n.h
new file mode 100644
index 000000000000..3b83ad75ebcb
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_11n.h
@@ -0,0 +1,420 @@
+/** @file mlan_11n.h
+ *
+ *  @brief Interface for the 802.11n mlan_11n module implemented in mlan_11n.c
+ *
+ *  Driver interface functions and type declarations for the 11n module
+ *    implemented in mlan_11n.c.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd.
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ *
+ */
+
+/********************************************************
+Change log:
+    12/01/2008: initial version
+********************************************************/
+
+#ifndef _MLAN_11N_H_
+#define _MLAN_11N_H_
+
+#include "mlan_11n_aggr.h"
+#include "mlan_11n_rxreorder.h"
+#include "mlan_wmm.h"
+
+/** Print the 802.11n device capability */
+void wlan_show_dot11ndevcap(pmlan_adapter pmadapter, t_u32 cap);
+/** Print the 802.11n device MCS */
+void wlan_show_devmcssupport(pmlan_adapter pmadapter, t_u8 support);
+/** Handle the command response of a delete block ack request */
+mlan_status wlan_ret_11n_delba(mlan_private * priv, HostCmd_DS_COMMAND * resp);
+/** Handle the command response of an add block ack request */
+mlan_status wlan_ret_11n_addba_req(mlan_private * priv,
+                                   HostCmd_DS_COMMAND * resp);
+/** Handle the command response of 11ncfg command */
+mlan_status wlan_ret_11n_cfg(IN pmlan_private pmpriv,
+                             IN HostCmd_DS_COMMAND * resp,
+                             IN mlan_ioctl_req * pioctl_buf);
+/** Prepare 11ncfg command */
+mlan_status wlan_cmd_11n_cfg(IN pmlan_private pmpriv,
+                             IN HostCmd_DS_COMMAND * cmd, IN t_u16 cmd_action,
+                             IN t_void * pdata_buf);
+/** Prepare TX BF configuration command */
+mlan_status wlan_cmd_tx_bf_cfg(IN pmlan_private pmpriv,
+                               IN HostCmd_DS_COMMAND * cmd,
+                               IN t_u16 cmd_action, IN t_void * pdata_buf);
+/** Handle the command response TX BF configuration */
+mlan_status wlan_ret_tx_bf_cfg(IN pmlan_private pmpriv,
+                               IN HostCmd_DS_COMMAND * resp,
+                               IN mlan_ioctl_req * pioctl_buf);
+#ifdef STA_SUPPORT
+/** Append the 802_11N tlv */
+int wlan_cmd_append_11n_tlv(IN mlan_private * pmpriv,
+                            IN BSSDescriptor_t * pbss_desc,
+                            OUT t_u8 ** ppbuffer);
+/** wlan fill HT cap tlv */
+void wlan_fill_ht_cap_tlv(mlan_private * priv, MrvlIETypes_HTCap_t * pht_cap,
+                          t_u8 band);
+#endif /* STA_SUPPORT */
+/** Miscellaneous configuration handler */
+mlan_status wlan_11n_cfg_ioctl(IN pmlan_adapter pmadapter,
+                               IN pmlan_ioctl_req pioctl_req);
+/** Delete Tx BA stream table entry */
+void wlan_11n_delete_txbastream_tbl_entry(mlan_private * priv,
+                                          TxBAStreamTbl * ptx_tbl);
+/** Delete all Tx BA stream table entries */
+void wlan_11n_deleteall_txbastream_tbl(mlan_private * priv);
+/** Get Tx BA stream table */
+TxBAStreamTbl *wlan_11n_get_txbastream_tbl(mlan_private * priv, int tid,
+                                           t_u8 * ra);
+/** Create Tx BA stream table */
+void wlan_11n_create_txbastream_tbl(mlan_private * priv, t_u8 * ra, int tid,
+                                    baStatus_e ba_status);
+/** Send ADD BA request */
+int wlan_send_addba(mlan_private * priv, int tid, t_u8 * peer_mac);
+/** Send DEL BA request */
+int wlan_send_delba(mlan_private * priv, int tid, t_u8 * peer_mac,
+                    int initiator);
+/** This function handles the command response of delete a block ack request*/
+void wlan_11n_delete_bastream(mlan_private * priv, t_u8 * del_ba);
+/** get rx reorder table */
+int wlan_get_rxreorder_tbl(mlan_private * priv, rx_reorder_tbl * buf);
+/** get tx ba stream table */
+int wlan_get_txbastream_tbl(mlan_private * priv, tx_ba_stream_tbl * buf);
+/** Minimum number of AMSDU */
+#define MIN_NUM_AMSDU 2
+/** AMSDU Aggr control cmd resp */
+mlan_status wlan_ret_amsdu_aggr_ctrl(pmlan_private pmpriv,
+                                     HostCmd_DS_COMMAND * resp,
+                                     mlan_ioctl_req * pioctl_buf);
+/** reconfigure tx buf size */
+mlan_status wlan_cmd_recfg_tx_buf(mlan_private * priv,
+                                  HostCmd_DS_COMMAND * cmd,
+                                  int cmd_action, void *pdata_buf);
+/** AMSDU aggr control cmd */
+mlan_status wlan_cmd_amsdu_aggr_ctrl(mlan_private * priv,
+                                     HostCmd_DS_COMMAND * cmd,
+                                     int cmd_action, void *pdata_buf);
+
+/** clean up txbastream_tbl */
+void wlan_11n_cleanup_txbastream_tbl(mlan_private * priv, t_u8 * ra);
+/**
+ *  @brief This function checks whether a station has 11N enabled or not
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param mac      station mac address
+ *  @return 	    MTRUE or MFALSE
+ */
+static INLINE t_u8
+is_station_11n_enabled(mlan_private * priv, t_u8 * mac)
+{
+    sta_node *sta_ptr = MNULL;
+    if ((sta_ptr = wlan_get_station_entry(priv, mac))) {
+        return (sta_ptr->is_11n_enabled) ? MTRUE : MFALSE;
+    }
+    return MFALSE;
+}
+
+/**
+ *  @brief This function get station max amsdu size
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param mac      station mac address
+ *  @return 	    max amsdu size statio supported
+ */
+static INLINE t_u16
+get_station_max_amsdu_size(mlan_private * priv, t_u8 * mac)
+{
+    sta_node *sta_ptr = MNULL;
+    if ((sta_ptr = wlan_get_station_entry(priv, mac))) {
+        return sta_ptr->max_amsdu;
+    }
+    return 0;
+}
+
+/**
+ *  @brief This function checks whether a station allows AMPDU or not
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param ptr      A pointer to RA list table
+ *  @param tid      TID value for ptr
+ *  @return 	    MTRUE or MFALSE
+ */
+static INLINE t_u8
+is_station_ampdu_allowed(mlan_private * priv, raListTbl * ptr, int tid)
+{
+    sta_node *sta_ptr = MNULL;
+    if ((sta_ptr = wlan_get_station_entry(priv, ptr->ra))) {
+        if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP) {
+            if (priv->sec_info.wapi_enabled && !sta_ptr->wapi_key_on)
+                return MFALSE;
+        }
+        return ((sta_ptr->ampdu_sta[tid] != BA_STREAM_NOT_ALLOWED)
+                ? MTRUE : MFALSE);
+    }
+    return MFALSE;
+}
+
+/**
+ *  @brief This function disable station ampdu for specific tid
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param tid     tid index
+ *  @param ra      station mac address
+ *  @return 	   N/A
+ */
+static INLINE void
+disable_station_ampdu(mlan_private * priv, t_u8 tid, t_u8 * ra)
+{
+    sta_node *sta_ptr = MNULL;
+    if ((sta_ptr = wlan_get_station_entry(priv, ra))) {
+        sta_ptr->ampdu_sta[tid] = BA_STREAM_NOT_ALLOWED;
+    }
+    return;
+}
+
+/**
+ *  @brief This function checks whether current BA stream is high priority or not
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param tid	    TID
+ *
+ *  @return 	    MTRUE or MFALSE
+ */
+static INLINE t_u8
+wlan_is_cur_bastream_high_prio(mlan_private * priv, int tid)
+{
+    TxBAStreamTbl *ptx_tbl;
+
+    ENTER();
+
+    if (!
+        (ptx_tbl =
+         (TxBAStreamTbl *) util_peek_list(priv->adapter->pmoal_handle,
+                                          &priv->tx_ba_stream_tbl_ptr,
+                                          priv->adapter->callbacks.
+                                          moal_spin_lock,
+                                          priv->adapter->callbacks.
+                                          moal_spin_unlock)))
+        return MFALSE;
+
+    while (ptx_tbl != (TxBAStreamTbl *) & priv->tx_ba_stream_tbl_ptr) {
+        if (priv->aggr_prio_tbl[tid].ampdu_user >
+            priv->aggr_prio_tbl[ptx_tbl->tid].ampdu_user) {
+            LEAVE();
+            return MTRUE;
+        }
+
+        ptx_tbl = ptx_tbl->pnext;
+    }
+
+    LEAVE();
+    return MFALSE;
+}
+
+/**
+ *  @brief This function checks whether AMPDU is allowed or not
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param ptr      A pointer to RA list table
+ *  @param tid      TID value for ptr
+ *
+ *  @return 	    MTRUE or MFALSE
+ */
+static INLINE t_u8
+wlan_is_ampdu_allowed(mlan_private * priv, raListTbl * ptr, int tid)
+{
+#ifdef UAP_SUPPORT
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP)
+        return is_station_ampdu_allowed(priv, ptr, tid);
+#endif /* UAP_SUPPORT */
+    if (priv->sec_info.wapi_enabled && !priv->sec_info.wapi_key_on)
+        return MFALSE;
+
+    return ((priv->aggr_prio_tbl[tid].ampdu_ap != BA_STREAM_NOT_ALLOWED)
+            ? MTRUE : MFALSE);
+}
+
+/**
+ *  @brief This function checks whether AMSDU is allowed for BA stream
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param ptr      A pointer to RA list table
+ *  @param tid	    TID value for ptr
+ *
+ *  @return 	    MTRUE or MFALSE
+ */
+static int INLINE
+wlan_is_amsdu_in_ampdu_allowed(mlan_private * priv, raListTbl * ptr, int tid)
+{
+    TxBAStreamTbl *ptx_tbl;
+    ENTER();
+    if ((ptx_tbl = wlan_11n_get_txbastream_tbl(priv, tid, ptr->ra))) {
+        LEAVE();
+        return ptx_tbl->amsdu;
+    }
+    LEAVE();
+    return MFALSE;
+}
+
+/**
+ *  @brief This function checks whether AMSDU is allowed or not
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param ptr      A pointer to RA list table
+ *  @param tid      TID value for ptr
+ *
+ *  @return 	    MTRUE or MFALSE
+ */
+static INLINE t_u8
+wlan_is_amsdu_allowed(mlan_private * priv, raListTbl * ptr, int tid)
+{
+#ifdef UAP_SUPPORT
+    sta_node *sta_ptr = MNULL;
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP) {
+        if ((sta_ptr = wlan_get_station_entry(priv, ptr->ra))) {
+            if (priv->sec_info.wapi_enabled && !sta_ptr->wapi_key_on)
+                return MFALSE;
+        }
+    }
+#endif /* UAP_SUPPORT */
+#define TXRATE_BITMAP_INDEX_MCS0_7 2
+    return (((priv->aggr_prio_tbl[tid].amsdu != BA_STREAM_NOT_ALLOWED)
+             && ((priv->is_data_rate_auto)
+                 || !((priv->bitmap_rates[TXRATE_BITMAP_INDEX_MCS0_7]) & 0x03)))
+            ? MTRUE : MFALSE);
+}
+
+/**
+ *  @brief This function checks whether a BA stream is available or not
+ *
+ *  @param priv     A pointer to mlan_private
+ *
+ *  @return 	    MTRUE or MFALSE
+ */
+static INLINE t_u8
+wlan_is_bastream_avail(mlan_private * priv)
+{
+    mlan_private *pmpriv = MNULL;
+    t_u8 i = 0;
+    t_u32 bastream_num = 0;
+    for (i = 0; i < priv->adapter->priv_num; i++) {
+        if ((pmpriv = priv->adapter->priv[i]))
+            bastream_num +=
+                wlan_wmm_list_len(priv->adapter,
+                                  (pmlan_list_head) & pmpriv->
+                                  tx_ba_stream_tbl_ptr);
+    }
+    return ((bastream_num < MLAN_MAX_TX_BASTREAM_SUPPORTED) ? MTRUE : MFALSE);
+}
+
+/**
+ *  @brief This function finds the stream to delete
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param ptr      A pointer to RA list table
+ *  @param ptr_tid  TID value of ptr
+ *  @param ptid     A pointer to TID of stream to delete, if return MTRUE
+ *  @param ra       RA of stream to delete, if return MTRUE
+ *
+ *  @return 	    MTRUE or MFALSE
+ */
+static INLINE t_u8
+wlan_find_stream_to_delete(mlan_private * priv,
+                           raListTbl * ptr, int ptr_tid, int *ptid, t_u8 * ra)
+{
+    int tid;
+    t_u8 ret = MFALSE;
+    TxBAStreamTbl *ptx_tbl;
+
+    ENTER();
+
+    if (!
+        (ptx_tbl =
+         (TxBAStreamTbl *) util_peek_list(priv->adapter->pmoal_handle,
+                                          &priv->tx_ba_stream_tbl_ptr,
+                                          priv->adapter->callbacks.
+                                          moal_spin_lock,
+                                          priv->adapter->callbacks.
+                                          moal_spin_unlock))) {
+        LEAVE();
+        return ret;
+    }
+
+    tid = priv->aggr_prio_tbl[ptr_tid].ampdu_user;
+
+    while (ptx_tbl != (TxBAStreamTbl *) & priv->tx_ba_stream_tbl_ptr) {
+        if (tid > priv->aggr_prio_tbl[ptx_tbl->tid].ampdu_user) {
+            tid = priv->aggr_prio_tbl[ptx_tbl->tid].ampdu_user;
+            *ptid = ptx_tbl->tid;
+            memcpy(priv->adapter, ra, ptx_tbl->ra, MLAN_MAC_ADDR_LENGTH);
+            ret = MTRUE;
+        }
+
+        ptx_tbl = ptx_tbl->pnext;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function checks whether BA stream is setup
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param ptr      A pointer to RA list table
+ *  @param tid	    TID value for ptr
+ *
+ *  @return 	    MTRUE or MFALSE
+ */
+static int INLINE
+wlan_is_bastream_setup(mlan_private * priv, raListTbl * ptr, int tid)
+{
+    TxBAStreamTbl *ptx_tbl;
+
+    ENTER();
+
+    if ((ptx_tbl = wlan_11n_get_txbastream_tbl(priv, tid, ptr->ra))) {
+        LEAVE();
+        return IS_BASTREAM_SETUP(ptx_tbl) ? MTRUE : MFALSE;
+    }
+
+    LEAVE();
+    return MFALSE;
+}
+
+/**
+ *  @brief This function checks whether 11n is supported
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param ra       Address of the receiver STA
+ *
+ *  @return 	    MTRUE or MFALSE
+ */
+static int INLINE
+wlan_is_11n_enabled(mlan_private * priv, t_u8 * ra)
+{
+    int ret = MFALSE;
+    ENTER();
+#ifdef UAP_SUPPORT
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP) {
+        if ((!(ra[0] & 0x01)) && (priv->is_11n_enabled))
+            ret = is_station_11n_enabled(priv, ra);
+    }
+#endif /* UAP_SUPPORT */
+    LEAVE();
+    return ret;
+}
+#endif /* !_MLAN_11N_H_ */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_11n_aggr.c b/drivers/net/wireless/sd8797/mlan/mlan_11n_aggr.c
new file mode 100644
index 000000000000..6125262d5b7a
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_11n_aggr.c
@@ -0,0 +1,509 @@
+/** @file mlan_11n_aggr.c
+ *
+ *  @brief This file contains functions for 11n Aggregation.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ *
+ */
+
+/********************************************************
+Change log:
+    11/10/2008: initial version
+********************************************************/
+
+#include "mlan.h"
+#include "mlan_join.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_wmm.h"
+#include "mlan_11n.h"
+#include "mlan_11n_aggr.h"
+#include "mlan_sdio.h"
+
+/********************************************************
+    Local Variables
+********************************************************/
+
+/********************************************************
+    Global Variables
+********************************************************/
+
+/********************************************************
+    Local Functions
+********************************************************/
+
+/**
+ *  @brief Aggregate individual packets into one AMSDU packet
+ *
+ *  @param pmadapter A pointer to mlan_adapter structure
+ *  @param amsdu_buf A pointer to packet buffer
+ *  @param data      A pointer to aggregated data packet being formed
+ *  @param pkt_len   Length of current packet to aggregate
+ *  @param pad       Pad
+ *
+ *  @return         Final packet size
+ */
+static int
+wlan_11n_form_amsdu_pkt(pmlan_adapter pmadapter, t_u8 * amsdu_buf, t_u8 * data,
+                        int pkt_len, int *pad)
+{
+    int dt_offset, amsdu_buf_offset;
+    Rfc1042Hdr_t snap = {
+        0xaa,                   /* LLC DSAP */
+        0xaa,                   /* LLC SSAP */
+        0x03,                   /* LLC CTRL */
+        {0x00, 0x00, 0x00},     /* SNAP OUI */
+        0x0000                  /* SNAP type */
+            /*
+             * This field will be overwritten
+             * later with ethertype
+             */
+    };
+
+    ENTER();
+
+    memcpy(pmadapter, amsdu_buf, data, (MLAN_MAC_ADDR_LENGTH) * 2);
+    dt_offset = amsdu_buf_offset = (MLAN_MAC_ADDR_LENGTH) * 2;
+
+    snap.snap_type = *(t_u16 *) (data + dt_offset);
+    dt_offset += sizeof(t_u16);
+    *(t_u16 *) (amsdu_buf + amsdu_buf_offset) = mlan_htons(pkt_len +
+                                                           LLC_SNAP_LEN -
+                                                           ((2 *
+                                                             MLAN_MAC_ADDR_LENGTH)
+                                                            + sizeof(t_u16)));
+    amsdu_buf_offset += sizeof(t_u16);
+    memcpy(pmadapter, amsdu_buf + amsdu_buf_offset, &snap, LLC_SNAP_LEN);
+    amsdu_buf_offset += LLC_SNAP_LEN;
+
+    memcpy(pmadapter, amsdu_buf + amsdu_buf_offset, data + dt_offset,
+           pkt_len - dt_offset);
+    *pad =
+        (((pkt_len + LLC_SNAP_LEN) & 3)) ? (4 -
+                                            (((pkt_len +
+                                               LLC_SNAP_LEN)) & 3)) : 0;
+
+    LEAVE();
+    return (pkt_len + LLC_SNAP_LEN + *pad);
+}
+
+/**
+ *  @brief Add TxPD to AMSDU header
+ *
+ *  @param priv     A pointer to mlan_private structure
+ *  @param mbuf		Pointer to buffer where the TxPD will be formed
+ *
+ *  @return		N/A
+ */
+static void
+wlan_11n_form_amsdu_txpd(mlan_private * priv, mlan_buffer * mbuf)
+{
+    TxPD *ptx_pd;
+    mlan_adapter *pmadapter = priv->adapter;
+
+    ENTER();
+
+    ptx_pd = (TxPD *) mbuf->pbuf;
+    memset(pmadapter, ptx_pd, 0, sizeof(TxPD));
+
+    /*
+     * Original priority has been overwritten
+     */
+    ptx_pd->priority = (t_u8) mbuf->priority;
+    ptx_pd->pkt_delay_2ms = wlan_wmm_compute_driver_packet_delay(priv, mbuf);
+    ptx_pd->bss_num = GET_BSS_NUM(priv);
+    ptx_pd->bss_type = priv->bss_type;
+    /* Always zero as the data is followed by TxPD */
+    ptx_pd->tx_pkt_offset = sizeof(TxPD);
+    ptx_pd->tx_pkt_type = PKT_TYPE_AMSDU;
+
+    if (ptx_pd->tx_control == 0)
+        /* TxCtrl set by user or default */
+        ptx_pd->tx_control = priv->pkt_tx_ctrl;
+
+    endian_convert_TxPD(ptx_pd);
+
+    LEAVE();
+}
+
+/**
+ *  @brief Update the TxPktLength field in TxPD after the complete AMSDU
+ *  packet is formed
+ *
+ *  @param priv     A pointer to mlan_private structure
+ *  @param mbuf     	TxPD buffer
+ *
+ *  @return		N/A
+ */
+static INLINE void
+wlan_11n_update_pktlen_amsdu_txpd(mlan_private * priv, pmlan_buffer mbuf)
+{
+    TxPD *ptx_pd;
+    ENTER();
+
+    ptx_pd = (TxPD *) mbuf->pbuf;
+    ptx_pd->tx_pkt_length =
+        (t_u16) wlan_cpu_to_le16(mbuf->data_len - sizeof(TxPD));
+#ifdef STA_SUPPORT
+    if ((GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) &&
+        (priv->adapter->pps_uapsd_mode)) {
+        if (MTRUE == wlan_check_last_packet_indication(priv)) {
+            priv->adapter->tx_lock_flag = MTRUE;
+            ptx_pd->flags = MRVDRV_TxPD_POWER_MGMT_LAST_PACKET;
+        }
+    }
+#endif /* STA_SUPPORT */
+    LEAVE();
+}
+
+/**
+ *  @brief Get number of aggregated packets
+ *
+ *  @param data			A pointer to packet data
+ *  @param total_pkt_len	Total packet length
+ *
+ *  @return			Number of packets
+ */
+static int
+wlan_11n_get_num_aggrpkts(t_u8 * data, int total_pkt_len)
+{
+    int pkt_count = 0, pkt_len, pad;
+
+    ENTER();
+    while (total_pkt_len > 0) {
+        /* Length will be in network format, change it to host */
+        pkt_len = mlan_ntohs((*(t_u16 *) (data + (2 * MLAN_MAC_ADDR_LENGTH))));
+        pad = (((pkt_len + sizeof(Eth803Hdr_t)) & 3)) ?
+            (4 - ((pkt_len + sizeof(Eth803Hdr_t)) & 3)) : 0;
+        data += pkt_len + pad + sizeof(Eth803Hdr_t);
+        total_pkt_len -= pkt_len + pad + sizeof(Eth803Hdr_t);
+        ++pkt_count;
+    }
+    LEAVE();
+    return pkt_count;
+}
+
+/********************************************************
+    Global Functions
+********************************************************/
+
+/**
+ *  @brief Deaggregate the received AMSDU packet
+ *
+ *  @param priv		A pointer to mlan_private structure
+ *  @param pmbuf	A pointer to aggregated data packet
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+mlan_status
+wlan_11n_deaggregate_pkt(mlan_private * priv, pmlan_buffer pmbuf)
+{
+    t_u16 pkt_len;
+    int total_pkt_len;
+    t_u8 *data;
+    int pad;
+    mlan_status ret = MLAN_STATUS_FAILURE;
+    RxPacketHdr_t *prx_pkt;
+    mlan_buffer *daggr_mbuf = MNULL;
+    mlan_adapter *pmadapter = priv->adapter;
+    t_u8 rfc1042_eth_hdr[MLAN_MAC_ADDR_LENGTH] = { 0xaa, 0xaa, 0x03,
+        0x00, 0x00, 0x00
+    };
+
+    ENTER();
+
+    data = (t_u8 *) (pmbuf->pbuf + pmbuf->data_offset);
+    total_pkt_len = pmbuf->data_len;
+
+    /* Sanity test */
+    if (total_pkt_len > MLAN_RX_DATA_BUF_SIZE) {
+        PRINTM(MERROR, "Total packet length greater than tx buffer"
+               " size %d\n", total_pkt_len);
+        goto done;
+    }
+
+    pmbuf->use_count = wlan_11n_get_num_aggrpkts(data, total_pkt_len);
+
+    while (total_pkt_len > 0) {
+        prx_pkt = (RxPacketHdr_t *) data;
+        /* Length will be in network format, change it to host */
+        pkt_len = mlan_ntohs((*(t_u16 *) (data + (2 * MLAN_MAC_ADDR_LENGTH))));
+        if (pkt_len > total_pkt_len) {
+            PRINTM(MERROR,
+                   "Error in packet length: total_pkt_len = %d, pkt_len = %d\n",
+                   total_pkt_len, pkt_len);
+            break;
+        }
+
+        pad = (((pkt_len + sizeof(Eth803Hdr_t)) & 3)) ?
+            (4 - ((pkt_len + sizeof(Eth803Hdr_t)) & 3)) : 0;
+
+        total_pkt_len -= pkt_len + pad + sizeof(Eth803Hdr_t);
+
+        if (memcmp(pmadapter, &prx_pkt->rfc1042_hdr,
+                   rfc1042_eth_hdr, sizeof(rfc1042_eth_hdr)) == 0) {
+            memmove(pmadapter, data + LLC_SNAP_LEN, data, (2 *
+                                                           MLAN_MAC_ADDR_LENGTH));
+            data += LLC_SNAP_LEN;
+            pkt_len += sizeof(Eth803Hdr_t) - LLC_SNAP_LEN;
+        } else {
+            *(t_u16 *) (data + (2 * MLAN_MAC_ADDR_LENGTH))
+                = (t_u16) 0;
+            pkt_len += sizeof(Eth803Hdr_t);
+        }
+        daggr_mbuf = wlan_alloc_mlan_buffer(pmadapter, pkt_len, 0, MFALSE);
+        if (daggr_mbuf == MNULL) {
+            PRINTM(MERROR, "Error allocating daggr mlan_buffer\n");
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+        daggr_mbuf->bss_index = pmbuf->bss_index;
+        daggr_mbuf->buf_type = pmbuf->buf_type;
+        daggr_mbuf->data_len = pkt_len;
+        daggr_mbuf->in_ts_sec = pmbuf->in_ts_sec;
+        daggr_mbuf->in_ts_usec = pmbuf->in_ts_usec;
+        daggr_mbuf->pparent = pmbuf;
+        daggr_mbuf->priority = pmbuf->priority;
+        memcpy(pmadapter, daggr_mbuf->pbuf + daggr_mbuf->data_offset, data,
+               pkt_len);
+
+#ifdef UAP_SUPPORT
+        if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP)
+            ret = wlan_uap_recv_packet(priv, daggr_mbuf);
+        else
+#endif /* UAP_SUPPORT */
+            ret =
+                pmadapter->callbacks.moal_recv_packet(pmadapter->pmoal_handle,
+                                                      daggr_mbuf);
+
+        switch (ret) {
+        case MLAN_STATUS_PENDING:
+            break;
+        case MLAN_STATUS_FAILURE:
+            PRINTM(MERROR, "Deaggr, send to moal failed\n");
+            daggr_mbuf->status_code = MLAN_ERROR_PKT_INVALID;
+        case MLAN_STATUS_SUCCESS:
+            wlan_recv_packet_complete(pmadapter, daggr_mbuf, ret);
+            break;
+        default:
+            break;
+        }
+
+        data += pkt_len + pad;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Aggregate multiple packets into one single AMSDU packet
+ *
+ *  @param priv 	A pointer to mlan_private structure
+ *  @param pra_list	Pointer to the RA List table containing the pointers
+ *  			    to packets.
+ *  @param headroom	Any interface specific headroom that may be need. TxPD
+ *  				will be formed leaving this headroom.
+ *  @param ptrindex	Pointer index
+ *
+ *  @return		Final packet size or MLAN_STATUS_FAILURE
+ */
+int
+wlan_11n_aggregate_pkt(mlan_private * priv, raListTbl * pra_list,
+                       int headroom, int ptrindex)
+{
+    int pkt_size = 0;
+    pmlan_adapter pmadapter = priv->adapter;
+    mlan_buffer *pmbuf_aggr, *pmbuf_src;
+    t_u8 *data;
+    int pad = 0;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+#ifdef DEBUG_LEVEL1
+    t_u32 sec, usec;
+#endif
+    mlan_tx_param tx_param;
+#ifdef STA_SUPPORT
+    TxPD *ptx_pd = MNULL;
+#endif
+    t_u32 max_amsdu_size = MIN(pra_list->max_amsdu, pmadapter->tx_buf_size);
+    ENTER();
+
+    PRINTM(MDAT_D, "Handling Aggr packet\n");
+
+    if ((pmbuf_src =
+         (pmlan_buffer) util_peek_list(pmadapter->pmoal_handle,
+                                       &pra_list->buf_head, MNULL, MNULL))) {
+
+        if (!(pmbuf_aggr = wlan_alloc_mlan_buffer(pmadapter,
+                                                  pmadapter->tx_buf_size, 0,
+                                                  MTRUE))) {
+            PRINTM(MERROR, "Error allocating mlan_buffer\n");
+            pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                                  priv->wmm.ra_list_spinlock);
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+
+        data = pmbuf_aggr->pbuf + headroom;
+        pmbuf_aggr->bss_index = pmbuf_src->bss_index;
+        pmbuf_aggr->buf_type = pmbuf_src->buf_type;
+        pmbuf_aggr->priority = pmbuf_src->priority;
+        pmbuf_aggr->pbuf = data;
+        pmbuf_aggr->data_offset = 0;
+
+        /* Form AMSDU */
+        wlan_11n_form_amsdu_txpd(priv, pmbuf_aggr);
+        pkt_size = sizeof(TxPD);
+#ifdef STA_SUPPORT
+        if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA)
+            ptx_pd = (TxPD *) pmbuf_aggr->pbuf;
+#endif
+    } else {
+        pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                              priv->wmm.ra_list_spinlock);
+        goto exit;
+    }
+
+    while (pmbuf_src && ((pkt_size + (pmbuf_src->data_len + LLC_SNAP_LEN)
+                          + headroom) <= max_amsdu_size)) {
+
+        pmbuf_src = (pmlan_buffer)
+            util_dequeue_list(pmadapter->pmoal_handle, &pra_list->buf_head,
+                              MNULL, MNULL);
+
+        pra_list->total_pkts--;
+
+        /* decrement for every PDU taken from the list */
+        priv->wmm.pkts_queued[ptrindex]--;
+        util_scalar_decrement(pmadapter->pmoal_handle,
+                              &priv->wmm.tx_pkts_queued, MNULL, MNULL);
+
+        pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                              priv->wmm.ra_list_spinlock);
+
+        pkt_size += wlan_11n_form_amsdu_pkt(pmadapter,
+                                            (data + pkt_size),
+                                            pmbuf_src->pbuf +
+                                            pmbuf_src->data_offset,
+                                            pmbuf_src->data_len, &pad);
+
+        DBG_HEXDUMP(MDAT_D, "pmbuf_src", pmbuf_src, sizeof(mlan_buffer));
+        wlan_write_data_complete(pmadapter, pmbuf_src, MLAN_STATUS_SUCCESS);
+
+        pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                            priv->wmm.ra_list_spinlock);
+
+        if (!wlan_is_ralist_valid(priv, pra_list, ptrindex)) {
+            pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                                  priv->wmm.ra_list_spinlock);
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+
+        pmbuf_src =
+            (pmlan_buffer) util_peek_list(pmadapter->pmoal_handle,
+                                          &pra_list->buf_head, MNULL, MNULL);
+    }
+
+    pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                          priv->wmm.ra_list_spinlock);
+
+    /* Last AMSDU packet does not need padding */
+    pkt_size -= pad;
+    pmbuf_aggr->data_len = pkt_size;
+    wlan_11n_update_pktlen_amsdu_txpd(priv, pmbuf_aggr);
+    pmbuf_aggr->data_len += headroom;
+    pmbuf_aggr->pbuf = data - headroom;
+    tx_param.next_pkt_len = ((pmbuf_src) ?
+                             pmbuf_src->data_len + sizeof(TxPD) : 0);
+
+    ret = wlan_sdio_host_to_card(pmadapter, MLAN_TYPE_DATA,
+                                 pmbuf_aggr, &tx_param);
+    switch (ret) {
+    case MLAN_STATUS_RESOURCE:
+        pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                            priv->wmm.ra_list_spinlock);
+
+        if (!wlan_is_ralist_valid(priv, pra_list, ptrindex)) {
+            pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                                  priv->wmm.ra_list_spinlock);
+            pmbuf_aggr->status_code = MLAN_ERROR_PKT_INVALID;
+            wlan_write_data_complete(pmadapter, pmbuf_aggr,
+                                     MLAN_STATUS_FAILURE);
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+#ifdef STA_SUPPORT
+        /* reset tx_lock_flag */
+        if ((GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) &&
+            pmadapter->pps_uapsd_mode && (pmadapter->tx_lock_flag == MTRUE)) {
+            pmadapter->tx_lock_flag = MFALSE;
+            ptx_pd->flags = 0;
+        }
+#endif
+        util_enqueue_list_head(pmadapter->pmoal_handle, &pra_list->buf_head,
+                               (pmlan_linked_list) pmbuf_aggr, MNULL, MNULL);
+
+        pra_list->total_pkts++;
+
+        /* add back only one: aggregated packet is requeued as one */
+        priv->wmm.pkts_queued[ptrindex]++;
+        util_scalar_increment(pmadapter->pmoal_handle,
+                              &priv->wmm.tx_pkts_queued, MNULL, MNULL);
+        pmbuf_aggr->flags |= MLAN_BUF_FLAG_REQUEUED_PKT;
+        pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                              priv->wmm.ra_list_spinlock);
+        PRINTM(MINFO, "MLAN_STATUS_RESOURCE is returned\n");
+        pmbuf_aggr->status_code = MLAN_ERROR_PKT_INVALID;
+        break;
+    case MLAN_STATUS_FAILURE:
+        pmadapter->data_sent = MFALSE;
+        PRINTM(MERROR, "Error: host_to_card failed: 0x%X\n", ret);
+        pmbuf_aggr->status_code = MLAN_ERROR_DATA_TX_FAIL;
+        pmadapter->dbg.num_tx_host_to_card_failure++;
+        wlan_write_data_complete(pmadapter, pmbuf_aggr, ret);
+        goto exit;
+    case MLAN_STATUS_PENDING:
+        pmadapter->data_sent = MFALSE;
+        break;
+    case MLAN_STATUS_SUCCESS:
+        wlan_write_data_complete(pmadapter, pmbuf_aggr, ret);
+        break;
+    default:
+        break;
+    }
+    if (ret != MLAN_STATUS_RESOURCE) {
+        pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                            priv->wmm.ra_list_spinlock);
+        if (wlan_is_ralist_valid(priv, pra_list, ptrindex)) {
+            priv->wmm.packets_out[ptrindex]++;
+            priv->wmm.tid_tbl_ptr[ptrindex].ra_list_curr = pra_list;
+        }
+        pmadapter->bssprio_tbl[priv->bss_priority].bssprio_cur =
+            pmadapter->bssprio_tbl[priv->bss_priority].bssprio_cur->pnext;
+        pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                              priv->wmm.ra_list_spinlock);
+    }
+    PRINTM_GET_SYS_TIME(MDATA, &sec, &usec);
+    PRINTM_NETINTF(MDATA, priv);
+    PRINTM(MDATA, "%lu.%06lu : Data => FW\n", sec, usec);
+
+  exit:
+    LEAVE();
+    return (pkt_size + headroom);
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_11n_aggr.h b/drivers/net/wireless/sd8797/mlan/mlan_11n_aggr.h
new file mode 100644
index 000000000000..ef8108a764c0
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_11n_aggr.h
@@ -0,0 +1,37 @@
+/** @file mlan_11n_aggr.h
+ *
+ *  @brief This file contains related macros, enum, and struct
+ *  of 11n aggregation functionalities
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd.
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ *
+ */
+
+/********************************************************
+Change log:
+    11/10/2008: initial version
+********************************************************/
+
+#ifndef _MLAN_11N_AGGR_H_
+#define _MLAN_11N_AGGR_H_
+
+/** Aggregate 11N packets */
+mlan_status wlan_11n_deaggregate_pkt(pmlan_private priv, pmlan_buffer pmbuf);
+/** Deaggregate 11N packets */
+int wlan_11n_aggregate_pkt(mlan_private * priv, raListTbl * ptr,
+                           int headroom, int ptrindex);
+
+#endif /* !_MLAN_11N_AGGR_H_ */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_11n_rxreorder.c b/drivers/net/wireless/sd8797/mlan/mlan_11n_rxreorder.c
new file mode 100644
index 000000000000..af689b7bab49
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_11n_rxreorder.c
@@ -0,0 +1,1138 @@
+/** @file mlan_11n_rxreorder.c
+ *
+ *  @brief This file contains the handling of RxReordering in wlan
+ *  driver.
+ * 
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *   
+ *  This software file (the "File") is distributed by Marvell International 
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+ *  (the "License").  You may use, redistribute and/or modify this File in 
+ *  accordance with the terms and conditions of the License, a copy of which 
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+ *  this warranty disclaimer.
+ */
+
+/********************************************************
+Change log:
+    11/10/2008: initial version
+********************************************************/
+
+#include "mlan.h"
+#include "mlan_join.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_wmm.h"
+#include "mlan_11n.h"
+#include "mlan_11n_rxreorder.h"
+
+/********************************************************
+    Local Variables
+********************************************************/
+
+/********************************************************
+    Global Variables
+********************************************************/
+
+/********************************************************
+    Local Functions
+********************************************************/
+/**
+ *  @brief This function will dispatch amsdu packet and
+ *  		forward it to kernel/upper layer
+ *
+ *  @param priv    	A pointer to mlan_private
+ *  @param pmbuf    A pointer to the received buffer
+ *
+ *  @return 	   	MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_11n_dispatch_amsdu_pkt(mlan_private * priv, pmlan_buffer pmbuf)
+{
+    RxPD *prx_pd;
+    prx_pd = (RxPD *) (pmbuf->pbuf + pmbuf->data_offset);
+
+    ENTER();
+    if (prx_pd->rx_pkt_type == PKT_TYPE_AMSDU) {
+        pmbuf->data_len = prx_pd->rx_pkt_length;
+        pmbuf->data_offset += prx_pd->rx_pkt_offset;
+        wlan_11n_deaggregate_pkt(priv, pmbuf);
+        LEAVE();
+        return MLAN_STATUS_SUCCESS;
+    }
+    LEAVE();
+    return MLAN_STATUS_FAILURE;
+}
+
+/**
+ *  @brief This function will process the rx packet and
+ *  		forward it to kernel/upper layer
+ *
+ *  @param priv    	A pointer to mlan_private
+ *  @param payload  A pointer to rx packet payload
+ *
+ *  @return 	   	MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_11n_dispatch_pkt(t_void * priv, t_void * payload)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+#ifdef STA_SUPPORT
+    pmlan_adapter pmadapter = ((pmlan_private) priv)->adapter;
+#endif
+    ENTER();
+    if (payload == (t_void *) RX_PKT_DROPPED_IN_FW) {
+        LEAVE();
+        return ret;
+    }
+#ifdef UAP_SUPPORT
+    if (GET_BSS_ROLE((mlan_private *) priv) == MLAN_BSS_ROLE_UAP) {
+        if (MLAN_STATUS_SUCCESS ==
+            wlan_11n_dispatch_amsdu_pkt((mlan_private *) priv,
+                                        (pmlan_buffer) payload)) {
+            LEAVE();
+            return ret;
+        }
+        ret = wlan_process_uap_rx_packet(priv, (pmlan_buffer) payload);
+        LEAVE();
+        return ret;
+    }
+#endif /* UAP_SUPPORT */
+
+#ifdef STA_SUPPORT
+    if (MLAN_STATUS_SUCCESS ==
+        wlan_11n_dispatch_amsdu_pkt((mlan_private *) priv,
+                                    (pmlan_buffer) payload)) {
+        LEAVE();
+        return ret;
+    }
+    ret = wlan_process_rx_packet(pmadapter, (pmlan_buffer) payload);
+#endif /* STA_SUPPORT */
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function restarts the reordering timeout timer
+ *
+ *  @param pmadapter    	A pointer to mlan_adapter
+ *  @param rx_reor_tbl_ptr  A pointer to structure RxReorderTbl
+ *
+ *  @return                 N/A
+ */
+static void
+mlan_11n_rxreorder_timer_restart(pmlan_adapter pmadapter,
+                                 RxReorderTbl * rx_reor_tbl_ptr)
+{
+    ENTER();
+    if (rx_reor_tbl_ptr->timer_context.timer_is_set)
+        pmadapter->callbacks.moal_stop_timer(pmadapter->pmoal_handle,
+                                             rx_reor_tbl_ptr->timer_context.
+                                             timer);
+
+    pmadapter->callbacks.moal_start_timer(pmadapter->pmoal_handle,
+                                          rx_reor_tbl_ptr->timer_context.timer,
+                                          MFALSE,
+                                          (rx_reor_tbl_ptr->win_size
+                                           * MIN_FLUSH_TIMER_MS));
+
+    rx_reor_tbl_ptr->timer_context.timer_is_set = MTRUE;
+    LEAVE();
+}
+
+/**
+ *  @brief This function dispatches all the packets in the buffer.
+ *  		There could be holes in the buffer.
+ *
+ *  @param priv    	        A pointer to mlan_private
+ *  @param rx_reor_tbl_ptr  A pointer to structure RxReorderTbl
+ *  @param start_win        Start window
+ *
+ *  @return 	   	        MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_11n_dispatch_pkt_until_start_win(t_void * priv,
+                                      RxReorderTbl * rx_reor_tbl_ptr,
+                                      int start_win)
+{
+    int no_pkt_to_send, i, xchg;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    void *rx_tmp_ptr = MNULL;
+    mlan_private *pmpriv = (mlan_private *) priv;
+
+    ENTER();
+
+    no_pkt_to_send = (start_win > rx_reor_tbl_ptr->start_win) ?
+        MIN((start_win - rx_reor_tbl_ptr->start_win),
+            rx_reor_tbl_ptr->win_size) : rx_reor_tbl_ptr->win_size;
+
+    for (i = 0; i < no_pkt_to_send; ++i) {
+        pmpriv->adapter->callbacks.moal_spin_lock(pmpriv->adapter->pmoal_handle,
+                                                  pmpriv->rx_pkt_lock);
+        rx_tmp_ptr = MNULL;
+        if (rx_reor_tbl_ptr->rx_reorder_ptr[i]) {
+            rx_tmp_ptr = rx_reor_tbl_ptr->rx_reorder_ptr[i];
+            rx_reor_tbl_ptr->rx_reorder_ptr[i] = MNULL;
+        }
+        pmpriv->adapter->callbacks.moal_spin_unlock(pmpriv->adapter->
+                                                    pmoal_handle,
+                                                    pmpriv->rx_pkt_lock);
+        if (rx_tmp_ptr)
+            wlan_11n_dispatch_pkt(priv, rx_tmp_ptr);
+    }
+
+    pmpriv->adapter->callbacks.moal_spin_lock(pmpriv->adapter->pmoal_handle,
+                                              pmpriv->rx_pkt_lock);
+    /*
+     * We don't have a circular buffer, hence use rotation to simulate
+     * circular buffer
+     */
+    xchg = rx_reor_tbl_ptr->win_size - no_pkt_to_send;
+    for (i = 0; i < xchg; ++i) {
+        rx_reor_tbl_ptr->rx_reorder_ptr[i] =
+            rx_reor_tbl_ptr->rx_reorder_ptr[no_pkt_to_send + i];
+        rx_reor_tbl_ptr->rx_reorder_ptr[no_pkt_to_send + i] = MNULL;
+    }
+
+    rx_reor_tbl_ptr->start_win = start_win;
+    pmpriv->adapter->callbacks.moal_spin_unlock(pmpriv->adapter->pmoal_handle,
+                                                pmpriv->rx_pkt_lock);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function will display the rxReorder table
+ *
+ *  @param pmadapter          A pointer to mlan_adapter structure
+ *  @param rx_reor_tbl_ptr    A pointer to structure RxReorderTbl
+ *
+ *  @return 	   	N/A
+ */
+static t_void
+wlan_11n_display_tbl_ptr(pmlan_adapter pmadapter,
+                         RxReorderTbl * rx_reor_tbl_ptr)
+{
+    ENTER();
+
+    DBG_HEXDUMP(MDAT_D, "Reorder ptr", rx_reor_tbl_ptr->rx_reorder_ptr,
+                sizeof(t_void *) * rx_reor_tbl_ptr->win_size);
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function will dispatch all packets sequentially
+ *  		from start_win until a hole is found and adjust the
+ *  		start_win appropriately
+ *
+ *  @param priv    	        A pointer to mlan_private
+ *  @param rx_reor_tbl_ptr  A pointer to structure RxReorderTbl
+ *
+ *  @return 	   	        MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_11n_scan_and_dispatch(t_void * priv, RxReorderTbl * rx_reor_tbl_ptr)
+{
+    int i, j, xchg;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    void *rx_tmp_ptr = MNULL;
+    mlan_private *pmpriv = (mlan_private *) priv;
+
+    ENTER();
+
+    for (i = 0; i < rx_reor_tbl_ptr->win_size; ++i) {
+        pmpriv->adapter->callbacks.moal_spin_lock(pmpriv->adapter->pmoal_handle,
+                                                  pmpriv->rx_pkt_lock);
+        if (!rx_reor_tbl_ptr->rx_reorder_ptr[i]) {
+            pmpriv->adapter->callbacks.moal_spin_unlock(pmpriv->adapter->
+                                                        pmoal_handle,
+                                                        pmpriv->rx_pkt_lock);
+            break;
+        }
+        rx_tmp_ptr = rx_reor_tbl_ptr->rx_reorder_ptr[i];
+        rx_reor_tbl_ptr->rx_reorder_ptr[i] = MNULL;
+        pmpriv->adapter->callbacks.moal_spin_unlock(pmpriv->adapter->
+                                                    pmoal_handle,
+                                                    pmpriv->rx_pkt_lock);
+        wlan_11n_dispatch_pkt(priv, rx_tmp_ptr);
+    }
+
+    pmpriv->adapter->callbacks.moal_spin_lock(pmpriv->adapter->pmoal_handle,
+                                              pmpriv->rx_pkt_lock);
+    /*
+     * We don't have a circular buffer, hence use rotation to simulate
+     * circular buffer
+     */
+    if (i > 0) {
+        xchg = rx_reor_tbl_ptr->win_size - i;
+        for (j = 0; j < xchg; ++j) {
+            rx_reor_tbl_ptr->rx_reorder_ptr[j] =
+                rx_reor_tbl_ptr->rx_reorder_ptr[i + j];
+            rx_reor_tbl_ptr->rx_reorder_ptr[i + j] = MNULL;
+        }
+    }
+
+    rx_reor_tbl_ptr->start_win = (rx_reor_tbl_ptr->start_win + i)
+        & (MAX_TID_VALUE - 1);
+
+    pmpriv->adapter->callbacks.moal_spin_unlock(pmpriv->adapter->pmoal_handle,
+                                                pmpriv->rx_pkt_lock);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function delete rxreorder table's entry
+ *         	and free the memory
+ *
+ *  @param priv    	        A pointer to mlan_private
+ *  @param rx_reor_tbl_ptr  A pointer to structure RxReorderTbl
+ *
+ *  @return 	   	        N/A
+ */
+static t_void
+wlan_11n_delete_rxreorder_tbl_entry(mlan_private * priv,
+                                    RxReorderTbl * rx_reor_tbl_ptr)
+{
+    pmlan_adapter pmadapter = priv->adapter;
+
+    ENTER();
+
+    if (!rx_reor_tbl_ptr) {
+        LEAVE();
+        return;
+    }
+
+    wlan_11n_dispatch_pkt_until_start_win(priv, rx_reor_tbl_ptr,
+                                          (rx_reor_tbl_ptr->start_win +
+                                           rx_reor_tbl_ptr->win_size)
+                                          & (MAX_TID_VALUE - 1));
+
+    if (rx_reor_tbl_ptr->timer_context.timer) {
+        if (rx_reor_tbl_ptr->timer_context.timer_is_set)
+            priv->adapter->callbacks.moal_stop_timer(pmadapter->pmoal_handle,
+                                                     rx_reor_tbl_ptr->timer_context.
+                                                     timer);
+        priv->adapter->callbacks.moal_free_timer(pmadapter->pmoal_handle,
+                                                 rx_reor_tbl_ptr->timer_context.
+                                                 timer);
+    }
+
+    PRINTM(MDAT_D, "Delete rx_reor_tbl_ptr: %p\n", rx_reor_tbl_ptr);
+    util_unlink_list(pmadapter->pmoal_handle,
+                     &priv->rx_reorder_tbl_ptr,
+                     (pmlan_linked_list) rx_reor_tbl_ptr,
+                     pmadapter->callbacks.moal_spin_lock,
+                     pmadapter->callbacks.moal_spin_unlock);
+
+    pmadapter->callbacks.moal_mfree(pmadapter->pmoal_handle,
+                                    (t_u8 *) rx_reor_tbl_ptr->rx_reorder_ptr);
+    pmadapter->callbacks.moal_mfree(pmadapter->pmoal_handle,
+                                    (t_u8 *) rx_reor_tbl_ptr);
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function returns the last used sequence number
+ *
+ *  @param rx_reorder_tbl_ptr   A pointer to structure RxReorderTbl
+ *
+ *  @return 	   	            Last used sequence number
+ */
+static int
+wlan_11n_find_last_seqnum(RxReorderTbl * rx_reorder_tbl_ptr)
+{
+    int i;
+
+    ENTER();
+    for (i = (rx_reorder_tbl_ptr->win_size - 1); i >= 0; --i) {
+        if (rx_reorder_tbl_ptr->rx_reorder_ptr[i]) {
+            LEAVE();
+            return i;
+        }
+    }
+    LEAVE();
+    return -1;
+}
+
+/**
+ *  @brief This function flushes all data
+ *
+ *  @param context      Reorder context pointer
+ *
+ *  @return 	   	    N/A
+ */
+static t_void
+wlan_flush_data(t_void * context)
+{
+    reorder_tmr_cnxt_t *reorder_cnxt = (reorder_tmr_cnxt_t *) context;
+    int startWin;
+
+    ENTER();
+    reorder_cnxt->timer_is_set = MFALSE;
+    wlan_11n_display_tbl_ptr(reorder_cnxt->priv->adapter, reorder_cnxt->ptr);
+
+    if ((startWin = wlan_11n_find_last_seqnum(reorder_cnxt->ptr)) >= 0) {
+        PRINTM(MINFO, "Flush data %d\n", startWin);
+        wlan_11n_dispatch_pkt_until_start_win(reorder_cnxt->priv,
+                                              reorder_cnxt->ptr,
+                                              ((reorder_cnxt->ptr->start_win +
+                                                startWin + 1) & (MAX_TID_VALUE -
+                                                                 1)));
+    }
+
+    wlan_11n_display_tbl_ptr(reorder_cnxt->priv->adapter, reorder_cnxt->ptr);
+    LEAVE();
+}
+
+/**
+ *  @brief This function will create a entry in rx reordering table for the
+ *  		given ta/tid and will initialize it with seq_num, win_size
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param ta       ta to find in reordering table
+ *  @param tid	    tid to find in reordering table
+ *  @param win_size win_size for the give ta/tid pair.
+ *  @param seq_num  Starting sequence number for current entry.
+ *
+ *  @return 	    N/A
+ */
+static t_void
+wlan_11n_create_rxreorder_tbl(mlan_private * priv, t_u8 * ta, int tid,
+                              int win_size, int seq_num)
+{
+    int i;
+    pmlan_adapter pmadapter = priv->adapter;
+    RxReorderTbl *rx_reor_tbl_ptr, *new_node;
+    sta_node *sta_ptr = MNULL;
+    t_u16 last_seq = 0;
+
+    ENTER();
+
+    /*
+     * If we get a TID, ta pair which is already present dispatch all the
+     * the packets and move the window size until the ssn
+     */
+    if ((rx_reor_tbl_ptr = wlan_11n_get_rxreorder_tbl(priv, tid, ta))) {
+        wlan_11n_dispatch_pkt_until_start_win(priv, rx_reor_tbl_ptr, seq_num);
+    } else {
+        PRINTM(MDAT_D, "%s: seq_num %d, tid %d, ta %02x:%02x:%02x:%02x:"
+               "%02x:%02x, win_size %d\n", __FUNCTION__,
+               seq_num, tid, ta[0], ta[1], ta[2], ta[3],
+               ta[4], ta[5], win_size);
+        if (pmadapter->callbacks.
+            moal_malloc(pmadapter->pmoal_handle, sizeof(RxReorderTbl),
+                        MLAN_MEM_DEF, (t_u8 **) & new_node)) {
+            PRINTM(MERROR, "Rx reorder memory allocation failed\n");
+            LEAVE();
+            return;
+        }
+
+        util_init_list((pmlan_linked_list) new_node);
+        new_node->tid = tid;
+        memcpy(pmadapter, new_node->ta, ta, MLAN_MAC_ADDR_LENGTH);
+        new_node->start_win = seq_num;
+        if (queuing_ra_based(priv)) {
+            // TODO for adhoc
+            if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP) {
+                if ((sta_ptr = wlan_get_station_entry(priv, ta)))
+                    last_seq = sta_ptr->rx_seq[tid];
+            }
+            PRINTM(MINFO, "UAP/ADHOC:last_seq=%d start_win=%d\n", last_seq,
+                   new_node->start_win);
+        } else {
+            last_seq = priv->rx_seq[tid];
+        }
+        if ((last_seq != DEFAULT_SEQ_NUM) && (last_seq >= new_node->start_win)) {
+            PRINTM(MDAT_D, "Update start_win: last_seq=%d, start_win=%d\n",
+                   last_seq, new_node->start_win);
+            new_node->start_win = last_seq + 1;
+        }
+        new_node->win_size = win_size;
+
+        if (pmadapter->callbacks.
+            moal_malloc(pmadapter->pmoal_handle, sizeof(t_void *) * win_size,
+                        MLAN_MEM_DEF, (t_u8 **) & new_node->rx_reorder_ptr)) {
+            PRINTM(MERROR, "Rx reorder table memory allocation" "failed\n");
+            pmadapter->callbacks.moal_mfree(pmadapter->pmoal_handle,
+                                            (t_u8 *) new_node);
+            LEAVE();
+            return;
+        }
+
+        PRINTM(MDAT_D, "Create ReorderPtr: %p\n", new_node);
+        new_node->timer_context.ptr = new_node;
+        new_node->timer_context.priv = priv;
+        new_node->timer_context.timer_is_set = MFALSE;
+        new_node->ba_status = BA_STREAM_SETUP_INPROGRESS;
+
+        pmadapter->callbacks.moal_init_timer(pmadapter->pmoal_handle,
+                                             &new_node->timer_context.timer,
+                                             wlan_flush_data,
+                                             &new_node->timer_context);
+
+        for (i = 0; i < win_size; ++i)
+            new_node->rx_reorder_ptr[i] = MNULL;
+
+        util_enqueue_list_tail(pmadapter->pmoal_handle,
+                               &priv->rx_reorder_tbl_ptr,
+                               (pmlan_linked_list) new_node,
+                               pmadapter->callbacks.moal_spin_lock,
+                               pmadapter->callbacks.moal_spin_unlock);
+    }
+
+    LEAVE();
+}
+
+/********************************************************
+    Global Functions
+********************************************************/
+
+/**
+ *  @brief This function will return the pointer to a entry in rx reordering
+ *  		table which matches the give TA/TID pair
+ *
+ *  @param priv    A pointer to mlan_private
+ *  @param ta      ta to find in reordering table
+ *  @param tid	   tid to find in reordering table
+ *
+ *  @return	   A pointer to structure RxReorderTbl
+ */
+RxReorderTbl *
+wlan_11n_get_rxreorder_tbl(mlan_private * priv, int tid, t_u8 * ta)
+{
+    RxReorderTbl *rx_reor_tbl_ptr;
+
+    ENTER();
+
+    if (!
+        (rx_reor_tbl_ptr =
+         (RxReorderTbl *) util_peek_list(priv->adapter->pmoal_handle,
+                                         &priv->rx_reorder_tbl_ptr,
+                                         priv->adapter->callbacks.
+                                         moal_spin_lock,
+                                         priv->adapter->callbacks.
+                                         moal_spin_unlock))) {
+        LEAVE();
+        return MNULL;
+    }
+
+    while (rx_reor_tbl_ptr != (RxReorderTbl *) & priv->rx_reorder_tbl_ptr) {
+        if ((!memcmp
+             (priv->adapter, rx_reor_tbl_ptr->ta, ta, MLAN_MAC_ADDR_LENGTH)) &&
+            (rx_reor_tbl_ptr->tid == tid)) {
+            LEAVE();
+            return rx_reor_tbl_ptr;
+        }
+
+        rx_reor_tbl_ptr = rx_reor_tbl_ptr->pnext;
+    }
+
+    LEAVE();
+    return MNULL;
+}
+
+/**
+  *  @brief This function prepares command for adding a block ack
+  *  		request.
+  *
+  *  @param priv        A pointer to mlan_private structure
+  *  @param cmd         A pointer to HostCmd_DS_COMMAND structure
+  *  @param pdata_buf   A pointer to data buffer
+  *
+  *  @return            MLAN_STATUS_SUCCESS
+  */
+mlan_status
+wlan_cmd_11n_addba_req(mlan_private * priv,
+                       HostCmd_DS_COMMAND * cmd, t_void * pdata_buf)
+{
+    HostCmd_DS_11N_ADDBA_REQ *padd_ba_req = (HostCmd_DS_11N_ADDBA_REQ *)
+        & cmd->params.add_ba_req;
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_11N_ADDBA_REQ);
+    cmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_11N_ADDBA_REQ) + S_DS_GEN);
+
+    memcpy(priv->adapter, padd_ba_req, pdata_buf,
+           sizeof(HostCmd_DS_11N_ADDBA_REQ));
+    padd_ba_req->block_ack_param_set =
+        wlan_cpu_to_le16(padd_ba_req->block_ack_param_set);
+    padd_ba_req->block_ack_tmo = wlan_cpu_to_le16(padd_ba_req->block_ack_tmo);
+    padd_ba_req->ssn = wlan_cpu_to_le16(padd_ba_req->ssn);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+  *  @brief This function prepares command for adding a block ack
+  *  		response.
+  *
+  *  @param priv        A pointer to mlan_private structure
+  *  @param cmd         A pointer to HostCmd_DS_COMMAND structure
+  *  @param pdata_buf   A pointer to data buffer
+  *
+  *  @return            MLAN_STATUS_SUCCESS
+  */
+mlan_status
+wlan_cmd_11n_addba_rspgen(mlan_private * priv,
+                          HostCmd_DS_COMMAND * cmd, void *pdata_buf)
+{
+    HostCmd_DS_11N_ADDBA_RSP *padd_ba_rsp = (HostCmd_DS_11N_ADDBA_RSP *)
+        & cmd->params.add_ba_rsp;
+    HostCmd_DS_11N_ADDBA_REQ *pevt_addba_req =
+        (HostCmd_DS_11N_ADDBA_REQ *) pdata_buf;
+    t_u8 tid = 0;
+    int win_size = 0;
+
+    ENTER();
+
+    pevt_addba_req->block_ack_param_set =
+        wlan_le16_to_cpu(pevt_addba_req->block_ack_param_set);
+    pevt_addba_req->block_ack_tmo =
+        wlan_le16_to_cpu(pevt_addba_req->block_ack_tmo);
+    pevt_addba_req->ssn = wlan_le16_to_cpu(pevt_addba_req->ssn);
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_11N_ADDBA_RSP);
+    cmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_11N_ADDBA_RSP) + S_DS_GEN);
+
+    memcpy(priv->adapter, padd_ba_rsp->peer_mac_addr,
+           pevt_addba_req->peer_mac_addr, MLAN_MAC_ADDR_LENGTH);
+    padd_ba_rsp->dialog_token = pevt_addba_req->dialog_token;
+    padd_ba_rsp->block_ack_tmo =
+        wlan_cpu_to_le16(pevt_addba_req->block_ack_tmo);
+    padd_ba_rsp->ssn = wlan_cpu_to_le16(pevt_addba_req->ssn);
+
+    padd_ba_rsp->block_ack_param_set = pevt_addba_req->block_ack_param_set;
+    tid = (padd_ba_rsp->block_ack_param_set & BLOCKACKPARAM_TID_MASK)
+        >> BLOCKACKPARAM_TID_POS;
+    if (priv->addba_reject[tid]
+#ifdef STA_SUPPORT
+        || ((GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA)
+            && priv->wps.session_enable)
+#endif
+#ifdef UAP_SUPPORT
+        || ((GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP)
+            && (priv->adapter->pending_bridge_pkts > RX_LOW_THRESHOLD))
+#endif
+        )
+        padd_ba_rsp->status_code = wlan_cpu_to_le16(ADDBA_RSP_STATUS_DECLINED);
+    else
+        padd_ba_rsp->status_code = wlan_cpu_to_le16(ADDBA_RSP_STATUS_ACCEPT);
+    padd_ba_rsp->block_ack_param_set &= ~BLOCKACKPARAM_WINSIZE_MASK;
+    if (!priv->add_ba_param.rx_amsdu ||
+        (priv->aggr_prio_tbl[tid].amsdu == BA_STREAM_NOT_ALLOWED))
+        /* We do not support AMSDU inside AMPDU, hence reset the bit */
+        padd_ba_rsp->block_ack_param_set &= ~BLOCKACKPARAM_AMSDU_SUPP_MASK;
+
+    padd_ba_rsp->block_ack_param_set |= (priv->add_ba_param.rx_win_size <<
+                                         BLOCKACKPARAM_WINSIZE_POS);
+    win_size = (padd_ba_rsp->block_ack_param_set & BLOCKACKPARAM_WINSIZE_MASK)
+        >> BLOCKACKPARAM_WINSIZE_POS;
+    if (win_size == 0)
+        padd_ba_rsp->status_code = wlan_cpu_to_le16(ADDBA_RSP_STATUS_DECLINED);
+
+    padd_ba_rsp->block_ack_param_set =
+        wlan_cpu_to_le16(padd_ba_rsp->block_ack_param_set);
+
+    if (padd_ba_rsp->status_code == wlan_cpu_to_le16(ADDBA_RSP_STATUS_ACCEPT))
+        wlan_11n_create_rxreorder_tbl(priv, pevt_addba_req->peer_mac_addr, tid,
+                                      win_size, pevt_addba_req->ssn);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+  *  @brief This function prepares command for deleting a block ack
+  *  		request.
+  *
+  *  @param priv       A pointer to mlan_private structure
+  *  @param cmd        A pointer to HostCmd_DS_COMMAND structure
+  *  @param pdata_buf  A pointer to data buffer
+  *
+  *  @return           MLAN_STATUS_SUCCESS
+  */
+mlan_status
+wlan_cmd_11n_delba(mlan_private * priv,
+                   HostCmd_DS_COMMAND * cmd, void *pdata_buf)
+{
+    HostCmd_DS_11N_DELBA *pdel_ba = (HostCmd_DS_11N_DELBA *)
+        & cmd->params.del_ba;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_11N_DELBA);
+    cmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_11N_DELBA) + S_DS_GEN);
+
+    memcpy(priv->adapter, pdel_ba, pdata_buf, sizeof(HostCmd_DS_11N_DELBA));
+    pdel_ba->del_ba_param_set = wlan_cpu_to_le16(pdel_ba->del_ba_param_set);
+    pdel_ba->reason_code = wlan_cpu_to_le16(pdel_ba->reason_code);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function will identify if RxReodering is needed for the packet
+ *  		and will do the reordering if required before sending it to kernel
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param seq_num  Seqence number of the current packet
+ *  @param tid	    Tid of the current packet
+ *  @param ta	    Transmiter address of the current packet
+ *  @param pkt_type Packetype for the current packet (to identify if its a BAR)
+ *  @param payload  Pointer to the payload
+ *
+ *  @return 	    MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+mlan_11n_rxreorder_pkt(void *priv, t_u16 seq_num, t_u16 tid,
+                       t_u8 * ta, t_u8 pkt_type, void *payload)
+{
+    RxReorderTbl *rx_reor_tbl_ptr;
+    int prev_start_win, start_win, end_win, win_size;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_adapter pmadapter = ((mlan_private *) priv)->adapter;
+
+    ENTER();
+
+    if (!
+        (rx_reor_tbl_ptr =
+         wlan_11n_get_rxreorder_tbl((mlan_private *) priv, tid, ta))) {
+        if (pkt_type != PKT_TYPE_BAR)
+            wlan_11n_dispatch_pkt(priv, payload);
+
+        LEAVE();
+        return ret;
+
+    } else {
+        if ((pkt_type == PKT_TYPE_AMSDU) && !rx_reor_tbl_ptr->amsdu) {
+            wlan_11n_dispatch_pkt(priv, payload);
+            LEAVE();
+            return ret;
+        }
+        if (pkt_type == PKT_TYPE_BAR)
+            PRINTM(MDAT_D, "BAR ");
+        if (pkt_type == PKT_TYPE_AMSDU)
+            PRINTM(MDAT_D, "AMSDU ");
+
+        prev_start_win = start_win = rx_reor_tbl_ptr->start_win;
+        win_size = rx_reor_tbl_ptr->win_size;
+        end_win = ((start_win + win_size) - 1) & (MAX_TID_VALUE - 1);
+
+        PRINTM(MDAT_D,
+               "TID %d, TA %02x:%02x:%02x:%02x:%02x:%02x\n",
+               tid, ta[0], ta[1], ta[2], ta[3], ta[4], ta[5]);
+        PRINTM(MDAT_D,
+               "1:seq_num %d start_win %d win_size %d end_win %d\n",
+               seq_num, start_win, win_size, end_win);
+        /*
+         * If seq_num is less then starting win then ignore and drop
+         * the packet
+         */
+        if ((start_win + TWOPOW11) > (MAX_TID_VALUE - 1)) {     /* Wrap */
+            if (seq_num >= ((start_win + (TWOPOW11)) &
+                            (MAX_TID_VALUE - 1)) && (seq_num < start_win)) {
+                ret = MLAN_STATUS_FAILURE;
+                goto done;
+            }
+        } else if ((seq_num < start_win) ||
+                   (seq_num > (start_win + (TWOPOW11)))) {
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+
+        /*
+         * If this packet is a BAR we adjust seq_num as
+         * WinStart = seq_num
+         */
+        if (pkt_type == PKT_TYPE_BAR)
+            seq_num = ((seq_num + win_size) - 1) & (MAX_TID_VALUE - 1);
+
+        PRINTM(MDAT_D,
+               "2:seq_num %d start_win %d win_size %d end_win %d\n",
+               seq_num, start_win, win_size, end_win);
+
+        if (((end_win < start_win) &&
+             (seq_num < (TWOPOW11 - (MAX_TID_VALUE - start_win))) &&
+             (seq_num > end_win))
+            || ((end_win > start_win) &&
+                ((seq_num > end_win) || (seq_num < start_win)))) {
+
+            end_win = seq_num;
+            if (((seq_num - win_size) + 1) >= 0)
+                start_win = (end_win - win_size) + 1;
+            else
+                start_win = (MAX_TID_VALUE - (win_size - seq_num)) + 1;
+
+            if ((ret = wlan_11n_dispatch_pkt_until_start_win(priv,
+                                                             rx_reor_tbl_ptr,
+                                                             start_win))) {
+                goto done;
+            }
+        }
+
+        PRINTM(MDAT_D, "3:seq_num %d start_win %d win_size %d"
+               " end_win %d\n", seq_num, start_win, win_size, end_win);
+        if (pkt_type != PKT_TYPE_BAR) {
+            if (seq_num >= start_win) {
+                if (rx_reor_tbl_ptr->rx_reorder_ptr[seq_num - start_win]) {
+                    PRINTM(MDAT_D, "Drop Duplicate Pkt\n");
+                    ret = MLAN_STATUS_FAILURE;
+                    goto done;
+                }
+                rx_reor_tbl_ptr->rx_reorder_ptr[seq_num - start_win] = payload;
+            } else {            /* Wrap condition */
+                if (rx_reor_tbl_ptr->rx_reorder_ptr[(seq_num
+                                                     + (MAX_TID_VALUE)) -
+                                                    start_win]) {
+                    PRINTM(MDAT_D, "Drop Duplicate Pkt\n");
+                    ret = MLAN_STATUS_FAILURE;
+                    goto done;
+                }
+                rx_reor_tbl_ptr->rx_reorder_ptr[(seq_num
+                                                 + (MAX_TID_VALUE)) -
+                                                start_win] = payload;
+            }
+        }
+
+        wlan_11n_display_tbl_ptr(pmadapter, rx_reor_tbl_ptr);
+
+        /*
+         * Dispatch all packets sequentially from start_win until a
+         * hole is found and adjust the start_win appropriately
+         */
+        ret = wlan_11n_scan_and_dispatch(priv, rx_reor_tbl_ptr);
+
+        wlan_11n_display_tbl_ptr(pmadapter, rx_reor_tbl_ptr);
+    }
+
+  done:
+    if (!rx_reor_tbl_ptr->timer_context.timer_is_set ||
+        (prev_start_win != rx_reor_tbl_ptr->start_win)) {
+
+        mlan_11n_rxreorder_timer_restart(pmadapter, rx_reor_tbl_ptr);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function will delete an entry for a given tid/ta pair. tid/ta
+ *  		are taken from delba_event body
+ *
+ *  @param priv    	    A pointer to mlan_private
+ *  @param tid		    tid to send delba
+ *  @param peer_mac	    MAC address to send delba
+ *  @param type 	    TYPE_DELBA_SENT	or TYPE_DELBA_RECEIVE
+ *  @param initiator    MTRUE if we are initiator of ADDBA, MFALSE otherwise
+ *
+ *  @return 	   	    N/A
+ */
+void
+mlan_11n_delete_bastream_tbl(mlan_private * priv, int tid,
+                             t_u8 * peer_mac, t_u8 type, int initiator)
+{
+    RxReorderTbl *rx_reor_tbl_ptr;
+    TxBAStreamTbl *ptxtbl;
+    t_u8 cleanup_rx_reorder_tbl;
+
+    ENTER();
+
+    if (type == TYPE_DELBA_RECEIVE)
+        cleanup_rx_reorder_tbl = (initiator) ? MTRUE : MFALSE;
+    else
+        cleanup_rx_reorder_tbl = (initiator) ? MFALSE : MTRUE;
+
+    PRINTM(MEVENT, "DELBA: %02x:%02x:%02x:%02x:%02x:%02x tid=%d,"
+           "initiator=%d\n", peer_mac[0],
+           peer_mac[1], peer_mac[2],
+           peer_mac[3], peer_mac[4], peer_mac[5], tid, initiator);
+
+    if (cleanup_rx_reorder_tbl) {
+        if (!(rx_reor_tbl_ptr = wlan_11n_get_rxreorder_tbl(priv, tid,
+                                                           peer_mac))) {
+            PRINTM(MWARN, "TID, TA not found in table!\n");
+            LEAVE();
+            return;
+        }
+        wlan_11n_delete_rxreorder_tbl_entry(priv, rx_reor_tbl_ptr);
+    } else {
+        if (!(ptxtbl = wlan_11n_get_txbastream_tbl(priv, tid, peer_mac))) {
+            PRINTM(MWARN, "TID, RA not found in table!\n");
+            LEAVE();
+            return;
+        }
+
+        wlan_11n_delete_txbastream_tbl_entry(priv, ptxtbl);
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function handles the command response of
+ *  		a block ack response
+ *
+ *  @param priv    A pointer to mlan_private structure
+ *  @param resp    A pointer to HostCmd_DS_COMMAND
+ *
+ *  @return        MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_11n_addba_resp(mlan_private * priv, HostCmd_DS_COMMAND * resp)
+{
+    HostCmd_DS_11N_ADDBA_RSP *padd_ba_rsp = (HostCmd_DS_11N_ADDBA_RSP *)
+        & resp->params.add_ba_rsp;
+    int tid;
+    RxReorderTbl *rx_reor_tbl_ptr = MNULL;
+
+    ENTER();
+
+    padd_ba_rsp->status_code = wlan_le16_to_cpu(padd_ba_rsp->status_code);
+    padd_ba_rsp->block_ack_param_set =
+        wlan_le16_to_cpu(padd_ba_rsp->block_ack_param_set);
+    padd_ba_rsp->block_ack_tmo = wlan_le16_to_cpu(padd_ba_rsp->block_ack_tmo);
+    padd_ba_rsp->ssn = wlan_le16_to_cpu(padd_ba_rsp->ssn);
+
+    tid = (padd_ba_rsp->block_ack_param_set & BLOCKACKPARAM_TID_MASK)
+        >> BLOCKACKPARAM_TID_POS;
+    /* Check if we had rejected the ADDBA, if yes then do not create the stream */
+    if (padd_ba_rsp->status_code == BA_RESULT_SUCCESS) {
+        PRINTM(MCMND,
+               "ADDBA RSP: %02x:%02x:%02x:%02x:%02x:%02x tid=%d ssn=%d win_size=%d,amsdu=%d\n",
+               padd_ba_rsp->peer_mac_addr[0], padd_ba_rsp->peer_mac_addr[1],
+               padd_ba_rsp->peer_mac_addr[2], padd_ba_rsp->peer_mac_addr[3],
+               padd_ba_rsp->peer_mac_addr[4], padd_ba_rsp->peer_mac_addr[5],
+               tid, padd_ba_rsp->ssn,
+               ((padd_ba_rsp->block_ack_param_set & BLOCKACKPARAM_WINSIZE_MASK)
+                >> BLOCKACKPARAM_WINSIZE_POS),
+               padd_ba_rsp->
+               block_ack_param_set & BLOCKACKPARAM_AMSDU_SUPP_MASK);
+
+        if ((rx_reor_tbl_ptr =
+             wlan_11n_get_rxreorder_tbl(priv, tid,
+                                        padd_ba_rsp->peer_mac_addr))) {
+            rx_reor_tbl_ptr->ba_status = BA_STREAM_SETUP_COMPLETE;
+            if ((padd_ba_rsp->
+                 block_ack_param_set & BLOCKACKPARAM_AMSDU_SUPP_MASK) &&
+                priv->add_ba_param.rx_amsdu &&
+                (priv->aggr_prio_tbl[tid].amsdu != BA_STREAM_NOT_ALLOWED))
+                rx_reor_tbl_ptr->amsdu = MTRUE;
+            else
+                rx_reor_tbl_ptr->amsdu = MFALSE;
+        }
+    } else {
+        PRINTM(MERROR,
+               "ADDBA RSP: Failed(%02x:%02x:%02x:%02x:%02x:%02x tid=%d)\n",
+               padd_ba_rsp->peer_mac_addr[0], padd_ba_rsp->peer_mac_addr[1],
+               padd_ba_rsp->peer_mac_addr[2], padd_ba_rsp->peer_mac_addr[3],
+               padd_ba_rsp->peer_mac_addr[4], padd_ba_rsp->peer_mac_addr[5],
+               tid);
+        if ((rx_reor_tbl_ptr =
+             wlan_11n_get_rxreorder_tbl(priv, tid,
+                                        padd_ba_rsp->peer_mac_addr))) {
+            wlan_11n_delete_rxreorder_tbl_entry(priv, rx_reor_tbl_ptr);
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles ba_stream_timeout event
+ *
+ *  @param priv    	A pointer to mlan_private
+ *  @param event    A pointer to structure HostCmd_DS_11N_BATIMEOUT
+ *
+ *  @return 	   	N/A
+ */
+void
+wlan_11n_ba_stream_timeout(mlan_private * priv,
+                           HostCmd_DS_11N_BATIMEOUT * event)
+{
+    HostCmd_DS_11N_DELBA delba;
+
+    ENTER();
+
+    DBG_HEXDUMP(MCMD_D, "Event:", (t_u8 *) event, 20);
+
+    memset(priv->adapter, &delba, 0, sizeof(HostCmd_DS_11N_DELBA));
+    memcpy(priv->adapter, delba.peer_mac_addr, event->peer_mac_addr,
+           MLAN_MAC_ADDR_LENGTH);
+
+    delba.del_ba_param_set |= (t_u16) event->tid << DELBA_TID_POS;
+    delba.del_ba_param_set |= (t_u16) event->origninator << DELBA_INITIATOR_POS;
+    delba.reason_code = REASON_CODE_STA_TIMEOUT;
+    wlan_prepare_cmd(priv, HostCmd_CMD_11N_DELBA, 0, 0, MNULL, &delba);
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function cleans up reorder tbl
+ *
+ *  @param priv    	A pointer to mlan_private
+ *
+ *  @return 	   	N/A
+ */
+void
+wlan_11n_cleanup_reorder_tbl(mlan_private * priv)
+{
+    RxReorderTbl *del_tbl_ptr;
+
+    ENTER();
+
+    while ((del_tbl_ptr = (RxReorderTbl *)
+            util_peek_list(priv->adapter->pmoal_handle,
+                           &priv->rx_reorder_tbl_ptr,
+                           priv->adapter->callbacks.moal_spin_lock,
+                           priv->adapter->callbacks.moal_spin_unlock))) {
+        wlan_11n_delete_rxreorder_tbl_entry(priv, del_tbl_ptr);
+    }
+
+    util_init_list((pmlan_linked_list) & priv->rx_reorder_tbl_ptr);
+
+    memset(priv->adapter, priv->rx_seq, 0xff, sizeof(priv->rx_seq));
+    LEAVE();
+}
+
+/**
+ *  @brief This function handle the rxba_sync event
+ *
+ *  @param priv    	  A pointer to mlan_private
+ *  @param event_buf  A pointer to event buf
+ *  @param len        event_buf length
+ *  @return 	   	N/A
+ */
+void
+wlan_11n_rxba_sync_event(mlan_private * priv, t_u8 * event_buf, t_u16 len)
+{
+    MrvlIEtypes_RxBaSync_t *tlv_rxba = (MrvlIEtypes_RxBaSync_t *) event_buf;
+    t_u16 tlv_type, tlv_len;
+    RxReorderTbl *rx_reor_tbl_ptr = MNULL;
+    t_u8 i, j;
+    t_u16 seq_num = 0;
+    int tlv_buf_left = len;
+    ENTER();
+    DBG_HEXDUMP(MEVT_D, "RXBA_SYNC_EVT", event_buf, len);
+    while (tlv_buf_left >= sizeof(MrvlIEtypes_RxBaSync_t)) {
+        tlv_type = wlan_le16_to_cpu(tlv_rxba->header.type);
+        tlv_len = wlan_le16_to_cpu(tlv_rxba->header.len);
+        if (tlv_type != TLV_TYPE_RXBA_SYNC) {
+            PRINTM(MERROR, "Wrong TLV id=0x%x\n", tlv_type);
+            goto done;
+        }
+        tlv_rxba->seq_num = wlan_le16_to_cpu(tlv_rxba->seq_num);
+        tlv_rxba->bitmap_len = wlan_le16_to_cpu(tlv_rxba->bitmap_len);
+        PRINTM(MEVENT,
+               "%02x:%02x:%02x:%02x:%02x:%02x tid=%d seq_num=%d bitmap_len=%d\n",
+               tlv_rxba->mac[0], tlv_rxba->mac[1], tlv_rxba->mac[2],
+               tlv_rxba->mac[3], tlv_rxba->mac[4], tlv_rxba->mac[5],
+               tlv_rxba->tid, tlv_rxba->seq_num, tlv_rxba->bitmap_len);
+        rx_reor_tbl_ptr =
+            wlan_11n_get_rxreorder_tbl(priv, tlv_rxba->tid, tlv_rxba->mac);
+        if (!rx_reor_tbl_ptr) {
+            PRINTM(MEVENT, "Can not find rx_reorder_tbl\n");
+            goto done;
+        }
+        for (i = 0; i < tlv_rxba->bitmap_len; i++) {
+            for (j = 0; j < 8; j++) {
+                if (tlv_rxba->bitmap[i] & (1 << j)) {
+                    seq_num =
+                        (tlv_rxba->seq_num + i * 8 + j) & (MAX_TID_VALUE - 1);
+                    PRINTM(MEVENT,
+                           "Fw dropped packet, seq=%d start_win=%d, win_size=%d\n",
+                           seq_num, rx_reor_tbl_ptr->start_win,
+                           rx_reor_tbl_ptr->win_size);
+                    if (MLAN_STATUS_SUCCESS !=
+                        mlan_11n_rxreorder_pkt(priv, seq_num, tlv_rxba->tid,
+                                               tlv_rxba->mac, 0,
+                                               (t_void *) RX_PKT_DROPPED_IN_FW))
+                    {
+                        PRINTM(MERROR,
+                               "Fail to handle dropped packet, seq=%d\n",
+                               seq_num);
+                    }
+                }
+            }
+        }
+        tlv_buf_left -= (sizeof(MrvlIEtypesHeader_t) + tlv_len);
+        tlv_rxba =
+            (MrvlIEtypes_RxBaSync_t *) ((t_u8 *) tlv_rxba + tlv_len +
+                                        sizeof(MrvlIEtypesHeader_t));
+    }
+  done:
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function will send a DELBA for each entry in the priv's
+ *          rx reordering table
+ *
+ *  @param priv    A pointer to mlan_private
+ */
+t_void
+wlan_send_delba_to_all_in_reorder_tbl(pmlan_private priv)
+{
+    RxReorderTbl *rx_reor_tbl_ptr;
+
+    ENTER();
+
+    if (!
+        (rx_reor_tbl_ptr =
+         (RxReorderTbl *) util_peek_list(priv->adapter->pmoal_handle,
+                                         &priv->rx_reorder_tbl_ptr,
+                                         priv->adapter->callbacks.
+                                         moal_spin_lock,
+                                         priv->adapter->callbacks.
+                                         moal_spin_unlock))) {
+        LEAVE();
+        return;
+    }
+
+    while (rx_reor_tbl_ptr != (RxReorderTbl *) & priv->rx_reorder_tbl_ptr) {
+        if (rx_reor_tbl_ptr->ba_status == BA_STREAM_SETUP_COMPLETE) {
+            rx_reor_tbl_ptr->ba_status = BA_STREAM_SETUP_INPROGRESS;
+            wlan_send_delba(priv, rx_reor_tbl_ptr->tid, rx_reor_tbl_ptr->ta, 0);
+        }
+        rx_reor_tbl_ptr = rx_reor_tbl_ptr->pnext;
+    }
+    LEAVE();
+}
+
+/**
+ *  @brief This function cleans up reorder tbl for specific station
+ *
+ *  @param priv    	A pointer to mlan_private
+ *  @param ta      ta to find in reordering table
+ *  @return 	   	N/A
+ */
+void
+wlan_cleanup_reorder_tbl(mlan_private * priv, t_u8 * ta)
+{
+    RxReorderTbl *rx_reor_tbl_ptr = MNULL;
+    t_u8 i;
+    ENTER();
+    for (i = 0; i < MAX_NUM_TID; ++i) {
+        if ((rx_reor_tbl_ptr = wlan_11n_get_rxreorder_tbl(priv, i, ta))) {
+            wlan_11n_delete_rxreorder_tbl_entry(priv, rx_reor_tbl_ptr);
+        }
+    }
+    LEAVE();
+    return;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_11n_rxreorder.h b/drivers/net/wireless/sd8797/mlan/mlan_11n_rxreorder.h
new file mode 100644
index 000000000000..4ff15834c8a3
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_11n_rxreorder.h
@@ -0,0 +1,102 @@
+/** @file mlan_11n_rxreorder.h
+ *
+ *  @brief This file contains related macros, enum, and struct
+ *  of 11n RxReordering functionalities
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International 
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+ *  (the "License").  You may use, redistribute and/or modify this File in 
+ *  accordance with the terms and conditions of the License, a copy of which 
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+ *  this warranty disclaimer.
+ */
+
+/********************************************************
+Change log:
+    11/10/2008: initial version
+********************************************************/
+
+#ifndef _MLAN_11N_RXREORDER_H_
+#define _MLAN_11N_RXREORDER_H_
+
+/** Max value a TID can take = 2^12 = 4096 */
+#define MAX_TID_VALUE			(2 << 11)
+/** 2^11 = 2048 */
+#define TWOPOW11			(2 << 10)
+
+/** Tid Mask used for extracting TID from BlockAckParamSet */
+#define BLOCKACKPARAM_TID_MASK		0x3C
+/** Tid position in BlockAckParamSet */
+#define BLOCKACKPARAM_TID_POS		2
+/** WinSize Mask used for extracting WinSize from BlockAckParamSet */
+#define BLOCKACKPARAM_WINSIZE_MASK	0xffc0
+/** WinSize Mask used for extracting WinSize from BlockAckParamSet */
+#define BLOCKACKPARAM_AMSDU_SUPP_MASK	0x1
+/** WinSize position in BlockAckParamSet */
+#define BLOCKACKPARAM_WINSIZE_POS	6
+/** Position of TID in DelBA Param set */
+#define DELBA_TID_POS			12
+/** Position of INITIATOR in DelBA Param set */
+#define DELBA_INITIATOR_POS		11
+/** Reason code: Requested from peer STA as it does not want to use the mechanism */
+#define REASON_CODE_STA_DONT_WANT	37
+/** Reason code: Requested from peer STA due to timeout*/
+#define REASON_CODE_STA_TIMEOUT		39
+/** Type: send delba command */
+#define TYPE_DELBA_SENT			1
+/** Type: recieve delba command */
+#define TYPE_DELBA_RECEIVE		2
+/** Set Initiator Bit */
+#define DELBA_INITIATOR(paramset)	(paramset = (paramset | (1 << 11)))
+/** Reset Initiator Bit for recipient */
+#define DELBA_RECIPIENT(paramset)	(paramset = (paramset & ~(1 << 11)))
+/** Immediate block ack */
+#define IMMEDIATE_BLOCK_ACK		0x2
+
+/** The request has been declined */
+#define ADDBA_RSP_STATUS_DECLINED  37
+/** ADDBA response status : Reject */
+#define ADDBA_RSP_STATUS_REJECT 1
+/** ADDBA response status : Accept */
+#define ADDBA_RSP_STATUS_ACCEPT 0
+
+/** DEFAULT SEQ NUM */
+#define DEFAULT_SEQ_NUM            0xffff
+
+/** Indicate packet has been dropped in FW */
+#define RX_PKT_DROPPED_IN_FW             0xffffffff
+
+mlan_status mlan_11n_rxreorder_pkt(void *priv, t_u16 seqNum, t_u16 tid,
+                                   t_u8 * ta, t_u8 pkttype, void *payload);
+void mlan_11n_delete_bastream_tbl(mlan_private * priv, int Tid,
+                                  t_u8 * PeerMACAddr, t_u8 type, int initiator);
+void wlan_11n_ba_stream_timeout(mlan_private * priv,
+                                HostCmd_DS_11N_BATIMEOUT * event);
+mlan_status wlan_ret_11n_addba_resp(mlan_private * priv,
+                                    HostCmd_DS_COMMAND * resp);
+mlan_status wlan_cmd_11n_delba(mlan_private * priv, HostCmd_DS_COMMAND * cmd,
+                               void *pdata_buf);
+mlan_status wlan_cmd_11n_addba_rspgen(mlan_private * priv,
+                                      HostCmd_DS_COMMAND * cmd,
+                                      void *pdata_buf);
+mlan_status wlan_cmd_11n_addba_req(mlan_private * priv,
+                                   HostCmd_DS_COMMAND * cmd, void *pdata_buf);
+void wlan_11n_cleanup_reorder_tbl(mlan_private * priv);
+RxReorderTbl *wlan_11n_get_rxreorder_tbl(mlan_private * priv, int tid,
+                                         t_u8 * ta);
+void wlan_11n_rxba_sync_event(mlan_private * priv, t_u8 * event_buf, t_u16 len);
+
+/** send delba for all entries in reorder_tbl */
+t_void wlan_send_delba_to_all_in_reorder_tbl(pmlan_private priv);
+
+/** clean up reorder_tbl */
+void wlan_cleanup_reorder_tbl(mlan_private * priv, t_u8 * ta);
+#endif /* _MLAN_11N_RXREORDER_H_ */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_cfp.c b/drivers/net/wireless/sd8797/mlan/mlan_cfp.c
new file mode 100644
index 000000000000..ccbb5aa1f666
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_cfp.c
@@ -0,0 +1,1378 @@
+/**
+ * @file mlan_cfp.c
+ *
+ *  @brief This file contains WLAN client mode channel, frequency and power
+ *  related code
+ *
+ *  Copyright (C) 2009-2012, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *  
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/*************************************************************
+Change Log:
+    04/16/2009: initial version
+************************************************************/
+
+#include "mlan.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_join.h"
+#include "mlan_main.h"
+
+/********************************************************
+    Local Variables
+********************************************************/
+
+/** 100mW */
+#define WLAN_TX_PWR_DEFAULT     20
+/** 100mW */
+#define WLAN_TX_PWR_US_DEFAULT      20
+/** 100mW */
+#define WLAN_TX_PWR_JP_BG_DEFAULT   20
+/** 200mW */
+#define WLAN_TX_PWR_JP_A_DEFAULT    23
+/** 100mW */
+#define WLAN_TX_PWR_FR_100MW        20
+/** 10mW */
+#define WLAN_TX_PWR_FR_10MW         10
+/** 100mW */
+#define WLAN_TX_PWR_EMEA_DEFAULT    20
+/** 2000mW */
+#define WLAN_TX_PWR_CN_2000MW       33
+
+/** Region code mapping */
+typedef struct _country_code_mapping
+{
+    /** Region */
+    t_u8 country_code[COUNTRY_CODE_LEN];
+    /** Code for B/G CFP table */
+    t_u8 cfp_code_bg;
+    /** Code for A CFP table */
+    t_u8 cfp_code_a;
+} country_code_mapping_t;
+
+/** Region code mapping table */
+static country_code_mapping_t country_code_mapping[] = {
+    {"US", 0x10, 0x10},         /* US FCC */
+    {"CA", 0x10, 0x20},         /* IC Canada */
+    {"SG", 0x10, 0x10},         /* Singapore */
+    {"EU", 0x30, 0x30},         /* ETSI */
+    {"AU", 0x30, 0x30},         /* Australia */
+    {"KR", 0x30, 0x30},         /* Republic Of Korea */
+    {"FR", 0x32, 0x32},         /* France */
+    {"JP", 0xFF, 0x40},         /* Japan */
+    {"CN", 0x30, 0x50},         /* China */
+};
+
+/**
+ * The structure for Channel-Frequency-Power table
+ */
+typedef struct _cfp_table
+{
+    /** Region or Code */
+    t_u8 code;
+    /** Frequency/Power */
+    chan_freq_power_t *cfp;
+    /** No of CFP flag */
+    int cfp_no;
+} cfp_table_t;
+
+/* Format { Channel, Frequency (MHz), MaxTxPower } */
+/** Band: 'B/G', Region: USA FCC/Canada IC */
+static chan_freq_power_t channel_freq_power_US_BG[] = {
+    {1, 2412, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {2, 2417, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {3, 2422, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {4, 2427, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {5, 2432, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {6, 2437, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {7, 2442, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {8, 2447, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {9, 2452, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {10, 2457, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {11, 2462, WLAN_TX_PWR_US_DEFAULT, MFALSE}
+};
+
+/** Band: 'B/G', Region: Europe ETSI/China */
+static chan_freq_power_t channel_freq_power_EU_BG[] = {
+    {1, 2412, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {2, 2417, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {3, 2422, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {4, 2427, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {5, 2432, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {6, 2437, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {7, 2442, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {8, 2447, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {9, 2452, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {10, 2457, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {11, 2462, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {12, 2467, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {13, 2472, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE}
+};
+
+/** Band: 'B/G', Region: France */
+static chan_freq_power_t channel_freq_power_FR_BG[] = {
+    {1, 2412, WLAN_TX_PWR_FR_100MW, MFALSE},
+    {2, 2417, WLAN_TX_PWR_FR_100MW, MFALSE},
+    {3, 2422, WLAN_TX_PWR_FR_100MW, MFALSE},
+    {4, 2427, WLAN_TX_PWR_FR_100MW, MFALSE},
+    {5, 2432, WLAN_TX_PWR_FR_100MW, MFALSE},
+    {6, 2437, WLAN_TX_PWR_FR_100MW, MFALSE},
+    {7, 2442, WLAN_TX_PWR_FR_100MW, MFALSE},
+    {8, 2447, WLAN_TX_PWR_FR_100MW, MFALSE},
+    {9, 2452, WLAN_TX_PWR_FR_100MW, MFALSE},
+    {10, 2457, WLAN_TX_PWR_FR_10MW, MFALSE},
+    {11, 2462, WLAN_TX_PWR_FR_10MW, MFALSE},
+    {12, 2467, WLAN_TX_PWR_FR_10MW, MFALSE},
+    {13, 2472, WLAN_TX_PWR_FR_10MW, MFALSE}
+};
+
+/** Band: 'B/G', Region: Japan */
+static chan_freq_power_t channel_freq_power_JPN41_BG[] = {
+    {1, 2412, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {2, 2417, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {3, 2422, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {4, 2427, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {5, 2432, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {6, 2437, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {7, 2442, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {8, 2447, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {9, 2452, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {10, 2457, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {11, 2462, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {12, 2467, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {13, 2472, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE}
+};
+
+/** Band: 'B/G', Region: Japan */
+static chan_freq_power_t channel_freq_power_JPN40_BG[] = {
+    {14, 2484, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE}
+};
+
+/** Band: 'B/G', Region: Japan */
+static chan_freq_power_t channel_freq_power_JPNFE_BG[] = {
+    {1, 2412, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {2, 2417, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {3, 2422, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {4, 2427, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {5, 2432, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {6, 2437, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {7, 2442, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {8, 2447, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {9, 2452, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {10, 2457, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {11, 2462, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {12, 2467, WLAN_TX_PWR_JP_BG_DEFAULT, MTRUE},
+    {13, 2472, WLAN_TX_PWR_JP_BG_DEFAULT, MTRUE}
+};
+
+/** Band : 'B/G', Region: Special */
+static chan_freq_power_t channel_freq_power_SPECIAL_BG[] = {
+    {1, 2412, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {2, 2417, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {3, 2422, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {4, 2427, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {5, 2432, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {6, 2437, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {7, 2442, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {8, 2447, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {9, 2452, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {10, 2457, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {11, 2462, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {12, 2467, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {13, 2472, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE},
+    {14, 2484, WLAN_TX_PWR_JP_BG_DEFAULT, MFALSE}
+};
+
+/**
+ * The 2.4GHz CFP tables
+ */
+static cfp_table_t cfp_table_BG[] = {
+    {0x10,                      /* US FCC */
+     channel_freq_power_US_BG,
+     sizeof(channel_freq_power_US_BG) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0x20,                      /* CANADA IC */
+     channel_freq_power_US_BG,
+     sizeof(channel_freq_power_US_BG) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0x30,                      /* EU */
+     channel_freq_power_EU_BG,
+     sizeof(channel_freq_power_EU_BG) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0x32,                      /* FRANCE */
+     channel_freq_power_FR_BG,
+     sizeof(channel_freq_power_FR_BG) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0x40,                      /* JAPAN */
+     channel_freq_power_JPN40_BG,
+     sizeof(channel_freq_power_JPN40_BG) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0x41,                      /* JAPAN */
+     channel_freq_power_JPN41_BG,
+     sizeof(channel_freq_power_JPN41_BG) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0x50,                      /* China */
+     channel_freq_power_EU_BG,
+     sizeof(channel_freq_power_EU_BG) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {
+     0xfe,                      /* JAPAN */
+     channel_freq_power_JPNFE_BG,
+     sizeof(channel_freq_power_JPNFE_BG) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0xff,                      /* Special */
+     channel_freq_power_SPECIAL_BG,
+     sizeof(channel_freq_power_SPECIAL_BG) / sizeof(chan_freq_power_t),
+     }
+    ,
+/* Add new region here */
+};
+
+/** Number of the CFP tables for 2.4GHz */
+#define MLAN_CFP_TABLE_SIZE_BG  (sizeof(cfp_table_BG)/sizeof(cfp_table_t))
+
+/* Format { Channel, Frequency (MHz), MaxTxPower, DFS } */
+/** Band: 'A', Region: USA FCC, Spain, France */
+static chan_freq_power_t channel_freq_power_A[] = {
+    {36, 5180, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {40, 5200, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {44, 5220, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {48, 5240, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {52, 5260, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {56, 5280, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {60, 5300, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {64, 5320, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {100, 5500, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {104, 5520, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {108, 5540, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {112, 5560, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {116, 5580, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {120, 5600, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {124, 5620, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {128, 5640, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {132, 5660, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {136, 5680, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {140, 5700, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {149, 5745, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {153, 5765, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {157, 5785, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {161, 5805, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {165, 5825, WLAN_TX_PWR_US_DEFAULT, MFALSE}
+};
+
+/** Band: 'A', Region: Canada IC */
+static chan_freq_power_t channel_freq_power_CAN_A[] = {
+    {36, 5180, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {40, 5200, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {44, 5220, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {48, 5240, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {52, 5260, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {56, 5280, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {60, 5300, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {64, 5320, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {100, 5500, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {104, 5520, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {108, 5540, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {112, 5560, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {116, 5580, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {132, 5660, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {136, 5680, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {140, 5700, WLAN_TX_PWR_US_DEFAULT, MTRUE},
+    {149, 5745, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {153, 5765, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {157, 5785, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {161, 5805, WLAN_TX_PWR_US_DEFAULT, MFALSE},
+    {165, 5825, WLAN_TX_PWR_US_DEFAULT, MFALSE}
+};
+
+/** Band: 'A', Region: Europe ETSI */
+static chan_freq_power_t channel_freq_power_EU_A[] = {
+    {36, 5180, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {40, 5200, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {44, 5220, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {48, 5240, WLAN_TX_PWR_EMEA_DEFAULT, MFALSE},
+    {52, 5260, WLAN_TX_PWR_EMEA_DEFAULT, MTRUE},
+    {56, 5280, WLAN_TX_PWR_EMEA_DEFAULT, MTRUE},
+    {60, 5300, WLAN_TX_PWR_EMEA_DEFAULT, MTRUE},
+    {64, 5320, WLAN_TX_PWR_EMEA_DEFAULT, MTRUE},
+    {100, 5500, WLAN_TX_PWR_EMEA_DEFAULT, MTRUE},
+    {104, 5520, WLAN_TX_PWR_EMEA_DEFAULT, MTRUE},
+    {108, 5540, WLAN_TX_PWR_EMEA_DEFAULT, MTRUE},
+    {112, 5560, WLAN_TX_PWR_EMEA_DEFAULT, MTRUE},
+    {116, 5580, WLAN_TX_PWR_EMEA_DEFAULT, MTRUE},
+    {120, 5600, WLAN_TX_PWR_EMEA_DEFAULT, MTRUE},
+    {124, 5620, WLAN_TX_PWR_EMEA_DEFAULT, MTRUE},
+    {128, 5640, WLAN_TX_PWR_EMEA_DEFAULT, MTRUE},
+    {132, 5660, WLAN_TX_PWR_EMEA_DEFAULT, MTRUE},
+    {136, 5680, WLAN_TX_PWR_EMEA_DEFAULT, MTRUE},
+    {140, 5700, WLAN_TX_PWR_EMEA_DEFAULT, MTRUE}
+};
+
+/** Band: 'A', Region: Japan */
+static chan_freq_power_t channel_freq_power_JPN_A[] = {
+    {8, 5040, WLAN_TX_PWR_JP_A_DEFAULT, MFALSE},
+    {12, 5060, WLAN_TX_PWR_JP_A_DEFAULT, MFALSE},
+    {16, 5080, WLAN_TX_PWR_JP_A_DEFAULT, MFALSE},
+    {36, 5180, WLAN_TX_PWR_JP_A_DEFAULT, MFALSE},
+    {40, 5200, WLAN_TX_PWR_JP_A_DEFAULT, MFALSE},
+    {44, 5220, WLAN_TX_PWR_JP_A_DEFAULT, MFALSE},
+    {48, 5240, WLAN_TX_PWR_JP_A_DEFAULT, MFALSE},
+    {52, 5260, WLAN_TX_PWR_JP_A_DEFAULT, MTRUE},
+    {56, 5280, WLAN_TX_PWR_JP_A_DEFAULT, MTRUE},
+    {60, 5300, WLAN_TX_PWR_JP_A_DEFAULT, MTRUE},
+    {64, 5320, WLAN_TX_PWR_JP_A_DEFAULT, MTRUE},
+    {100, 5500, WLAN_TX_PWR_JP_A_DEFAULT, MTRUE},
+    {104, 5520, WLAN_TX_PWR_JP_A_DEFAULT, MTRUE},
+    {108, 5540, WLAN_TX_PWR_JP_A_DEFAULT, MTRUE},
+    {112, 5560, WLAN_TX_PWR_JP_A_DEFAULT, MTRUE},
+    {116, 5580, WLAN_TX_PWR_JP_A_DEFAULT, MTRUE},
+    {120, 5600, WLAN_TX_PWR_JP_A_DEFAULT, MTRUE},
+    {124, 5620, WLAN_TX_PWR_JP_A_DEFAULT, MTRUE},
+    {128, 5640, WLAN_TX_PWR_JP_A_DEFAULT, MTRUE},
+    {132, 5660, WLAN_TX_PWR_JP_A_DEFAULT, MTRUE},
+    {136, 5680, WLAN_TX_PWR_JP_A_DEFAULT, MTRUE},
+    {140, 5700, WLAN_TX_PWR_JP_A_DEFAULT, MTRUE}
+};
+
+/** Band: 'A', Region: China */
+static chan_freq_power_t channel_freq_power_CN_A[] = {
+    {149, 5745, WLAN_TX_PWR_CN_2000MW, MFALSE},
+    {153, 5765, WLAN_TX_PWR_CN_2000MW, MFALSE},
+    {157, 5785, WLAN_TX_PWR_CN_2000MW, MFALSE},
+    {161, 5805, WLAN_TX_PWR_CN_2000MW, MFALSE},
+    {165, 5825, WLAN_TX_PWR_CN_2000MW, MFALSE}
+};
+
+/** Band: 'A', NULL */
+static chan_freq_power_t channel_freq_power_NULL_A[] = {
+};
+
+/** Band: 'A', Code: 1, Low band (5150-5250 MHz) channels */
+static chan_freq_power_t channel_freq_power_low_band[] = {
+    {36, 5180, WLAN_TX_PWR_DEFAULT, MFALSE},
+    {40, 5200, WLAN_TX_PWR_DEFAULT, MFALSE},
+    {44, 5220, WLAN_TX_PWR_DEFAULT, MFALSE},
+    {48, 5240, WLAN_TX_PWR_DEFAULT, MFALSE},
+};
+
+/** Band: 'A', Code: 2, Lower middle band (5250-5350 MHz) channels */
+static chan_freq_power_t channel_freq_power_lower_middle_band[] = {
+    {52, 5260, WLAN_TX_PWR_DEFAULT, MTRUE},
+    {56, 5280, WLAN_TX_PWR_DEFAULT, MTRUE},
+    {60, 5300, WLAN_TX_PWR_DEFAULT, MTRUE},
+    {64, 5320, WLAN_TX_PWR_DEFAULT, MTRUE},
+};
+
+/** Band: 'A', Code: 3, Upper middle band (5470-5725 MHz) channels */
+static chan_freq_power_t channel_freq_power_upper_middle_band[] = {
+    {100, 5500, WLAN_TX_PWR_DEFAULT, MTRUE},
+    {104, 5520, WLAN_TX_PWR_DEFAULT, MTRUE},
+    {108, 5540, WLAN_TX_PWR_DEFAULT, MTRUE},
+    {112, 5560, WLAN_TX_PWR_DEFAULT, MTRUE},
+    {116, 5580, WLAN_TX_PWR_DEFAULT, MTRUE},
+    {120, 5600, WLAN_TX_PWR_DEFAULT, MTRUE},
+    {124, 5620, WLAN_TX_PWR_DEFAULT, MTRUE},
+    {128, 5640, WLAN_TX_PWR_DEFAULT, MTRUE},
+    {132, 5660, WLAN_TX_PWR_DEFAULT, MTRUE},
+    {136, 5680, WLAN_TX_PWR_DEFAULT, MTRUE},
+    {140, 5700, WLAN_TX_PWR_DEFAULT, MTRUE},
+};
+
+/** Band: 'A', Code: 4, High band (5725-5850 MHz) channels */
+static chan_freq_power_t channel_freq_power_high_band[] = {
+    {149, 5745, WLAN_TX_PWR_DEFAULT, MFALSE},
+    {153, 5765, WLAN_TX_PWR_DEFAULT, MFALSE},
+    {157, 5785, WLAN_TX_PWR_DEFAULT, MFALSE},
+    {161, 5805, WLAN_TX_PWR_DEFAULT, MFALSE},
+    {165, 5825, WLAN_TX_PWR_DEFAULT, MFALSE}
+};
+
+/** Band: 'A', Code: 5, Low band (5150-5250 MHz) and
+ * High band (5725-5850 MHz) channels */
+static chan_freq_power_t channel_freq_power_low_high_band[] = {
+    {36, 5180, WLAN_TX_PWR_DEFAULT, MFALSE},
+    {40, 5200, WLAN_TX_PWR_DEFAULT, MFALSE},
+    {44, 5220, WLAN_TX_PWR_DEFAULT, MFALSE},
+    {48, 5240, WLAN_TX_PWR_DEFAULT, MFALSE},
+    {149, 5745, WLAN_TX_PWR_DEFAULT, MFALSE},
+    {153, 5765, WLAN_TX_PWR_DEFAULT, MFALSE},
+    {157, 5785, WLAN_TX_PWR_DEFAULT, MFALSE},
+    {161, 5805, WLAN_TX_PWR_DEFAULT, MFALSE},
+    {165, 5825, WLAN_TX_PWR_DEFAULT, MFALSE}
+};
+
+/**
+ * The 5GHz CFP tables
+ */
+static cfp_table_t cfp_table_A[] = {
+    {0x10,                      /* US FCC */
+     channel_freq_power_A,
+     sizeof(channel_freq_power_A) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0x20,                      /* CANADA IC */
+     channel_freq_power_CAN_A,
+     sizeof(channel_freq_power_CAN_A) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0x30,                      /* EU */
+     channel_freq_power_EU_A,
+     sizeof(channel_freq_power_EU_A) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0x32,                      /* FRANCE */
+     channel_freq_power_A,
+     sizeof(channel_freq_power_A) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0x40,                      /* JAPAN */
+     channel_freq_power_JPN_A,
+     sizeof(channel_freq_power_JPN_A) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0x41,                      /* JAPAN */
+     channel_freq_power_JPN_A,
+     sizeof(channel_freq_power_JPN_A) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0x50,                      /* China */
+     channel_freq_power_CN_A,
+     sizeof(channel_freq_power_CN_A) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0xfe,                      /* JAPAN */
+     channel_freq_power_NULL_A,
+     sizeof(channel_freq_power_NULL_A) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0xff,                      /* Special */
+     channel_freq_power_JPN_A,
+     sizeof(channel_freq_power_JPN_A) / sizeof(chan_freq_power_t),
+     }
+    ,
+    {0x1,                       /* Low band (5150-5250 MHz) channels */
+     channel_freq_power_low_band,
+     sizeof(channel_freq_power_low_band) / sizeof(chan_freq_power_t)
+     }
+    ,
+    {0x2,                       /* Lower middle band (5250-5350 MHz) channels */
+     channel_freq_power_lower_middle_band,
+     sizeof(channel_freq_power_lower_middle_band) / sizeof(chan_freq_power_t)
+     }
+    ,
+    {0x3,                       /* Upper middle band (5470-5725 MHz) channels */
+     channel_freq_power_upper_middle_band,
+     sizeof(channel_freq_power_upper_middle_band) / sizeof(chan_freq_power_t)
+     }
+    ,
+    {0x4,                       /* High band (5725-5850 MHz) channels */
+     channel_freq_power_high_band,
+     sizeof(channel_freq_power_high_band) / sizeof(chan_freq_power_t)
+     }
+    ,
+    {0x5,                       /* Low band (5150-5250 MHz) and High band
+                                   (5725-5850 MHz) channels */
+     channel_freq_power_low_high_band,
+     sizeof(channel_freq_power_low_high_band) / sizeof(chan_freq_power_t)
+     }
+    ,
+/* Add new region here */
+};
+
+/** Number of the CFP tables for 5GHz */
+#define MLAN_CFP_TABLE_SIZE_A   (sizeof(cfp_table_A)/sizeof(cfp_table_t))
+
+/********************************************************
+    Global Variables
+********************************************************/
+/**
+ * The table to keep region code
+ */
+t_u16 region_code_index[MRVDRV_MAX_REGION_CODE] =
+    { 0x10, 0x20, 0x30, 0x32, 0x40, 0x41, 0x50, 0xfe, 0xff };
+
+/** The table to keep CFP code for BG */
+t_u16 cfp_code_index_bg[MRVDRV_MAX_CFP_CODE_BG] = { };
+
+/** The table to keep CFP code for A */
+t_u16 cfp_code_index_a[MRVDRV_MAX_CFP_CODE_A] = { 0x1, 0x2, 0x3, 0x4, 0x5 };
+
+/**
+ * The rates supported for ad-hoc B mode
+ */
+t_u8 AdhocRates_B[B_SUPPORTED_RATES] = { 0x82, 0x84, 0x8b, 0x96, 0 };
+
+/**
+ * The rates supported for ad-hoc G mode
+ */
+t_u8 AdhocRates_G[G_SUPPORTED_RATES] =
+    { 0x8c, 0x12, 0x98, 0x24, 0xb0, 0x48, 0x60, 0x6c, 0 };
+
+/**
+ * The rates supported for ad-hoc BG mode
+ */
+t_u8 AdhocRates_BG[BG_SUPPORTED_RATES] =
+    { 0x82, 0x84, 0x8b, 0x96, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48,
+    0x60, 0x6c, 0
+};
+
+/**
+ * The rates supported in A mode for ad-hoc
+ */
+t_u8 AdhocRates_A[A_SUPPORTED_RATES] =
+    { 0x8c, 0x12, 0x98, 0x24, 0xb0, 0x48, 0x60, 0x6c, 0 };
+
+/**
+ * The rates supported in A mode (used for BAND_A)
+ */
+t_u8 SupportedRates_A[A_SUPPORTED_RATES] =
+    { 0x0c, 0x12, 0x18, 0x24, 0xb0, 0x48, 0x60, 0x6c, 0 };
+
+/**
+ * The rates supported by the card
+ */
+t_u16 WlanDataRates[WLAN_SUPPORTED_RATES_EXT] =
+    { 0x02, 0x04, 0x0B, 0x16, 0x00, 0x0C, 0x12,
+    0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90,
+    0x0D, 0x1A, 0x27, 0x34, 0x4E, 0x68, 0x75,
+    0x82, 0x0C, 0x1B, 0x36, 0x51, 0x6C, 0xA2,
+    0xD8, 0xF3, 0x10E, 0x00
+};
+
+/**
+ * The rates supported in B mode
+ */
+t_u8 SupportedRates_B[B_SUPPORTED_RATES] = { 0x02, 0x04, 0x0b, 0x16, 0 };
+
+/**
+ * The rates supported in G mode (BAND_G, BAND_G|BAND_GN)
+ */
+t_u8 SupportedRates_G[G_SUPPORTED_RATES] =
+    { 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6c, 0 };
+
+/**
+ * The rates supported in BG mode (BAND_B|BAND_G, BAND_B|BAND_G|BAND_GN)
+ */
+t_u8 SupportedRates_BG[BG_SUPPORTED_RATES] =
+    { 0x02, 0x04, 0x0b, 0x0c, 0x12, 0x16, 0x18, 0x24, 0x30, 0x48,
+    0x60, 0x6c, 0
+};
+
+/**
+ * The rates supported in N mode
+ */
+t_u8 SupportedRates_N[N_SUPPORTED_RATES] = { 0x02, 0x04, 0 };
+
+/********************************************************
+    Local Functions
+********************************************************/
+/**
+ *  @brief Find a character in a string.
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param s            A pointer to string
+ *  @param c            Character to be located
+ *  @param n            The length of string
+ *
+ *  @return        A pointer to the first occurrence of c in string, or MNULL if c is not found.
+ */
+static void *
+wlan_memchr(pmlan_adapter pmadapter, void *s, int c, int n)
+{
+    const t_u8 *p = (t_u8 *) s;
+
+    ENTER();
+
+    while (n--) {
+        if ((t_u8) c == *p++) {
+            LEAVE();
+            return (void *) (p - 1);
+        }
+    }
+
+    LEAVE();
+    return MNULL;
+}
+
+/**
+ *  @brief This function finds the CFP in
+ *  		cfp_table_BG/A based on region/code and band parameter.
+ *
+ *  @param pmadapter  A pointer to mlan_adapter structure
+ *  @param region     The region code
+ *  @param band       The band
+ *  @param cfp_no     A pointer to CFP number
+ *
+ *  @return           A pointer to CFP
+ */
+static chan_freq_power_t *
+wlan_get_region_cfp_table(pmlan_adapter pmadapter, t_u8 region, t_u8 band,
+                          int *cfp_no)
+{
+    t_u32 i;
+    t_u8 cfp_bg, cfp_a;
+
+    ENTER();
+
+    cfp_bg = cfp_a = region;
+    if (!region) {
+        /* Invalid region code, use CFP code */
+        cfp_bg = pmadapter->cfp_code_bg;
+        cfp_a = pmadapter->cfp_code_a;
+    }
+
+    if (band & (BAND_B | BAND_G | BAND_GN)) {
+        for (i = 0; i < MLAN_CFP_TABLE_SIZE_BG; i++) {
+            PRINTM(MINFO, "cfp_table_BG[%d].code=%d\n", i,
+                   cfp_table_BG[i].code);
+            /* Check if region/code matches for BG bands */
+            if (cfp_table_BG[i].code == cfp_bg) {
+                /* Select by band */
+                *cfp_no = cfp_table_BG[i].cfp_no;
+                LEAVE();
+                return cfp_table_BG[i].cfp;
+            }
+        }
+    }
+    if (band & (BAND_A | BAND_AN)) {
+        for (i = 0; i < MLAN_CFP_TABLE_SIZE_A; i++) {
+            PRINTM(MINFO, "cfp_table_A[%d].code=%d\n", i, cfp_table_A[i].code);
+            /* Check if region/code matches for A bands */
+            if (cfp_table_A[i].code == cfp_a) {
+                /* Select by band */
+                *cfp_no = cfp_table_A[i].cfp_no;
+                LEAVE();
+                return cfp_table_A[i].cfp;
+            }
+        }
+    }
+
+    if (!region)
+        PRINTM(MERROR, "Error Band[0x%x] or code[BG:%#x, A:%#x]\n",
+               band, cfp_bg, cfp_a);
+    else
+        PRINTM(MERROR, "Error Band[0x%x] or region[%#x]\n", band, region);
+
+    LEAVE();
+    return MNULL;
+}
+
+/********************************************************
+    Global Functions
+********************************************************/
+/**
+ *  @brief This function converts region string to integer code
+ *
+ *  @param pmadapter        A pointer to mlan_adapter structure
+ *  @param country_code     Country string
+ *  @param cfp_bg           Pointer to buffer
+ *  @param cfp_a            Pointer to buffer
+ *
+ *  @return                 MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_misc_country_2_cfp_table_code(pmlan_adapter pmadapter, t_u8 * country_code,
+                                   t_u8 * cfp_bg, t_u8 * cfp_a)
+{
+    t_u8 i;
+
+    ENTER();
+
+    /* Look for code in mapping table */
+    for (i = 0; i < NELEMENTS(country_code_mapping); i++) {
+        if (!memcmp(pmadapter, country_code_mapping[i].country_code,
+                    country_code, COUNTRY_CODE_LEN - 1)) {
+            *cfp_bg = country_code_mapping[i].cfp_code_bg;
+            *cfp_a = country_code_mapping[i].cfp_code_a;
+            LEAVE();
+            return MLAN_STATUS_SUCCESS;
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_FAILURE;
+}
+
+#ifdef STA_SUPPORT
+#endif /* STA_SUPPORT */
+
+/**
+ *  @brief Use index to get the data rate
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param index        The index of data rate
+ *  @param ht_info      ht info
+ *
+ *  @return                     Data rate or 0
+ */
+
+t_u32
+wlan_index_to_data_rate(pmlan_adapter pmadapter, t_u8 index, t_u8 ht_info)
+{
+#define MCS_NUM_SUPP    16
+    t_u16 mcs_rate[4][MCS_NUM_SUPP] =
+        { {0x1b, 0x36, 0x51, 0x6c, 0xa2, 0xd8, 0xf3, 0x10e, 0x36, 0x6c, 0xa2,
+           0xd8, 0x144, 0x1b0, 0x1e6, 0x21c}
+    ,                           /* LG 40M */
+    {0x1e, 0x3c, 0x5a, 0x78, 0xb4, 0xf0, 0x10e, 0x12c, 0x3c, 0x78, 0xb4, 0xf0,
+     0x168, 0x1e0, 0x21c, 0x258}
+    ,                           /* SG 40M */
+    {0x0d, 0x1a, 0x27, 0x34, 0x4e, 0x68, 0x75, 0x82, 0x1a, 0x34, 0x4e, 0x68,
+     0x9c, 0xd0, 0xea, 0x104}
+    ,                           /* LG 20M */
+    {0x0e, 0x1c, 0x2b, 0x39, 0x56, 0x73, 0x82, 0x90, 0x1c, 0x39, 0x56, 0x73,
+     0xad, 0xe7, 0x104, 0x120}
+    };                          /* SG 20M */
+
+    t_u32 rate = 0;
+    ENTER();
+
+    if (ht_info & MBIT(0)) {
+        if (index == MLAN_RATE_BITMAP_MCS0) {
+            if (ht_info & MBIT(2))
+                rate = 0x0D;    /* MCS 32 SGI rate */
+            else
+                rate = 0x0C;    /* MCS 32 LGI rate */
+        } else if (index < MCS_NUM_SUPP) {
+            if (ht_info & MBIT(1)) {
+                if (ht_info & MBIT(2))
+                    rate = mcs_rate[1][index];  /* SGI, 40M */
+                else
+                    rate = mcs_rate[0][index];  /* LGI, 40M */
+            } else {
+                if (ht_info & MBIT(2))
+                    rate = mcs_rate[3][index];  /* SGI, 20M */
+                else
+                    rate = mcs_rate[2][index];  /* LGI, 20M */
+            }
+        } else
+            rate = WlanDataRates[0];
+    } else {
+        /* 11n non HT rates */
+        if (index >= WLAN_SUPPORTED_RATES_EXT)
+            index = 0;
+        rate = WlanDataRates[index];
+    }
+    LEAVE();
+    return rate;
+}
+
+/**
+ *  @brief Use rate to get the index
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param rate         Data rate
+ *
+ *  @return                     Index or 0
+ */
+t_u8
+wlan_data_rate_to_index(pmlan_adapter pmadapter, t_u32 rate)
+{
+    t_u16 *ptr;
+
+    ENTER();
+    if (rate)
+        if ((ptr = wlan_memchr(pmadapter, WlanDataRates, (t_u8) rate,
+                               sizeof(WlanDataRates)))) {
+            LEAVE();
+            return (t_u8) (ptr - WlanDataRates);
+        }
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Get active data rates
+ *
+ *  @param pmpriv           A pointer to mlan_private structure
+ *  @param bss_mode         The specified BSS mode (Infra/IBSS)
+ *  @param config_bands     The specified band configuration
+ *  @param rates            The buf to return the active rates
+ *
+ *  @return                 The number of Rates
+ */
+t_u32
+wlan_get_active_data_rates(mlan_private * pmpriv, t_u32 bss_mode,
+                           t_u8 config_bands, WLAN_802_11_RATES rates)
+{
+    t_u32 k;
+
+    ENTER();
+
+    if (pmpriv->media_connected != MTRUE) {
+        k = wlan_get_supported_rates(pmpriv, bss_mode, config_bands, rates);
+    } else {
+        k = wlan_copy_rates(rates, 0, pmpriv->curr_bss_params.data_rates,
+                            pmpriv->curr_bss_params.num_of_rates);
+    }
+
+    LEAVE();
+    return k;
+}
+
+#ifdef STA_SUPPORT
+/**
+ *  @brief This function search through all the regions cfp table to find the channel,
+ *            if the channel is found then gets the MIN txpower of the channel
+ *            present in all the regions.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param channel      Channel number.
+ *
+ *  @return             The Tx power
+ */
+t_u8
+wlan_get_txpwr_of_chan_from_cfp(mlan_private * pmpriv, t_u8 channel)
+{
+    t_u8 i = 0;
+    t_u8 j = 0;
+    t_u8 tx_power = 0;
+    t_u32 cfp_no;
+    chan_freq_power_t *cfp = MNULL;
+    chan_freq_power_t *cfp_a = MNULL;
+    t_u32 cfp_no_a;
+
+    ENTER();
+
+    for (i = 0; i < MLAN_CFP_TABLE_SIZE_BG; i++) {
+        /* Get CFP */
+        cfp = cfp_table_BG[i].cfp;
+        cfp_no = cfp_table_BG[i].cfp_no;
+        /* Find matching channel and get Tx power */
+        for (j = 0; j < cfp_no; j++) {
+            if ((cfp + j)->channel == channel) {
+                if (tx_power != 0)
+                    tx_power = MIN(tx_power, (cfp + j)->max_tx_power);
+                else
+                    tx_power = (t_u8) (cfp + j)->max_tx_power;
+                break;
+            }
+        }
+    }
+
+    for (i = 0; i < MLAN_CFP_TABLE_SIZE_A; i++) {
+        /* Get CFP */
+        cfp_a = cfp_table_A[i].cfp;
+        cfp_no_a = cfp_table_A[i].cfp_no;
+        for (j = 0; j < cfp_no_a; j++) {
+            if ((cfp_a + j)->channel == channel) {
+                if (tx_power != 0)
+                    tx_power = MIN(tx_power, (cfp_a + j)->max_tx_power);
+                else
+                    tx_power = (t_u8) ((cfp_a + j)->max_tx_power);
+                break;
+            }
+        }
+    }
+
+    LEAVE();
+    return tx_power;
+}
+
+/**
+ *  @brief Get the channel frequency power info for a specific channel
+ *
+ *  @param pmadapter            A pointer to mlan_adapter structure
+ *  @param band                 It can be BAND_A, BAND_G or BAND_B
+ *  @param channel              The channel to search for
+ *  @param region_channel       A pointer to region_chan_t structure
+ *
+ *  @return                     A pointer to chan_freq_power_t structure or MNULL if not found.
+ */
+
+chan_freq_power_t *
+wlan_get_cfp_by_band_and_channel(pmlan_adapter pmadapter,
+                                 t_u8 band,
+                                 t_u16 channel, region_chan_t * region_channel)
+{
+    region_chan_t *rc;
+    chan_freq_power_t *cfp = MNULL;
+    int i, j;
+
+    ENTER();
+
+    for (j = 0; !cfp && (j < MAX_REGION_CHANNEL_NUM); j++) {
+        rc = &region_channel[j];
+
+        if (!rc->valid || !rc->pcfp)
+            continue;
+        switch (rc->band) {
+        case BAND_A:
+            switch (band) {
+            case BAND_AN:
+            case BAND_A | BAND_AN:
+            case BAND_A:       /* Matching BAND_A */
+                break;
+
+            default:
+                continue;
+            }
+            break;
+        case BAND_B:
+        case BAND_G:
+            switch (band) {
+            case BAND_GN:
+            case BAND_B | BAND_G | BAND_GN:
+            case BAND_G | BAND_GN:
+            case BAND_B | BAND_G:
+            case BAND_B:       /* Matching BAND_B/G */
+            case BAND_G:
+            case 0:
+                break;
+            default:
+                continue;
+            }
+            break;
+        default:
+            continue;
+        }
+        if (channel == FIRST_VALID_CHANNEL)
+            cfp = &rc->pcfp[0];
+        else {
+            for (i = 0; i < rc->num_cfp; i++) {
+                if (rc->pcfp[i].channel == channel) {
+                    cfp = &rc->pcfp[i];
+                    break;
+                }
+            }
+        }
+    }
+
+    if (!cfp && channel)
+        PRINTM(MERROR, "wlan_get_cfp_by_band_and_channel(): cannot find "
+               "cfp by band %d & channel %d\n", band, channel);
+
+    LEAVE();
+    return cfp;
+}
+
+/**
+ *  @brief Find the channel frequency power info for a specific channel
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param band         It can be BAND_A, BAND_G or BAND_B
+ *  @param channel      The channel to search for
+ *
+ *  @return             A pointer to chan_freq_power_t structure or MNULL if not found.
+ */
+chan_freq_power_t *
+wlan_find_cfp_by_band_and_channel(mlan_adapter * pmadapter,
+                                  t_u8 band, t_u16 channel)
+{
+    chan_freq_power_t *cfp = MNULL;
+
+    ENTER();
+
+    /* Any station(s) with 11D enabled */
+    if (wlan_count_priv_cond(pmadapter, wlan_11d_is_enabled,
+                             wlan_is_station) > 0)
+        cfp = wlan_get_cfp_by_band_and_channel(pmadapter, band, channel,
+                                               pmadapter->universal_channel);
+    else
+        cfp = wlan_get_cfp_by_band_and_channel(pmadapter, band, channel,
+                                               pmadapter->region_channel);
+
+    LEAVE();
+    return cfp;
+}
+
+/**
+ *  @brief Find the channel frequency power info for a specific frequency
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param band         It can be BAND_A, BAND_G or BAND_B
+ *  @param freq         The frequency to search for
+ *
+ *  @return         Pointer to chan_freq_power_t structure; MNULL if not found
+ */
+chan_freq_power_t *
+wlan_find_cfp_by_band_and_freq(mlan_adapter * pmadapter, t_u8 band, t_u32 freq)
+{
+    chan_freq_power_t *cfp = MNULL;
+    region_chan_t *rc;
+    int i, j;
+
+    ENTER();
+
+    for (j = 0; !cfp && (j < MAX_REGION_CHANNEL_NUM); j++) {
+        rc = &pmadapter->region_channel[j];
+
+        /* Any station(s) with 11D enabled */
+        if (wlan_count_priv_cond(pmadapter, wlan_11d_is_enabled,
+                                 wlan_is_station) > 0)
+            rc = &pmadapter->universal_channel[j];
+
+        if (!rc->valid || !rc->pcfp)
+            continue;
+        switch (rc->band) {
+        case BAND_A:
+            switch (band) {
+            case BAND_AN:
+            case BAND_A | BAND_AN:
+            case BAND_A:       /* Matching BAND_A */
+                break;
+            default:
+                continue;
+            }
+            break;
+        case BAND_B:
+        case BAND_G:
+            switch (band) {
+            case BAND_GN:
+            case BAND_B | BAND_G | BAND_GN:
+            case BAND_G | BAND_GN:
+            case BAND_B | BAND_G:
+            case BAND_B:
+            case BAND_G:
+            case 0:
+                break;
+            default:
+                continue;
+            }
+            break;
+        default:
+            continue;
+        }
+        for (i = 0; i < rc->num_cfp; i++) {
+            if (rc->pcfp[i].freq == freq) {
+                cfp = &rc->pcfp[i];
+                break;
+            }
+        }
+    }
+
+    if (!cfp && freq)
+        PRINTM(MERROR, "wlan_find_cfp_by_band_and_freq(): cannot find cfp by "
+               "band %d & freq %d\n", band, freq);
+
+    LEAVE();
+    return cfp;
+}
+#endif /* STA_SUPPORT */
+
+/**
+ *  @brief Check if Rate Auto
+ *
+ *  @param pmpriv               A pointer to mlan_private structure
+ *
+ *  @return                     MTRUE or MFALSE
+ */
+t_u8
+wlan_is_rate_auto(mlan_private * pmpriv)
+{
+    t_u32 i;
+    int rate_num = 0;
+
+    ENTER();
+
+    for (i = 0; i < NELEMENTS(pmpriv->bitmap_rates); i++)
+        if (pmpriv->bitmap_rates[i])
+            rate_num++;
+
+    LEAVE();
+    if (rate_num > 1)
+        return MTRUE;
+    else
+        return MFALSE;
+}
+
+/**
+ *  @brief Covert Rate Bitmap to Rate index
+ *
+ *  @param pmadapter    Pointer to mlan_adapter structure
+ *  @param rate_bitmap  Pointer to rate bitmap
+ *  @param size         Size of the bitmap array
+ *
+ *  @return             Rate index
+ */
+int
+wlan_get_rate_index(pmlan_adapter pmadapter, t_u16 * rate_bitmap, int size)
+{
+    int i;
+
+    ENTER();
+
+    for (i = 0; i < size * 8; i++) {
+        if (rate_bitmap[i / 16] & (1 << (i % 16))) {
+            LEAVE();
+            return i;
+        }
+    }
+
+    LEAVE();
+    return -1;
+}
+
+/**
+ *  @brief Get supported data rates
+ *
+ *  @param pmpriv           A pointer to mlan_private structure
+ *  @param bss_mode         The specified BSS mode (Infra/IBSS)
+ *  @param config_bands     The specified band configuration
+ *  @param rates            The buf to return the supported rates
+ *
+ *  @return                 The number of Rates
+ */
+t_u32
+wlan_get_supported_rates(mlan_private * pmpriv, t_u32 bss_mode,
+                         t_u8 config_bands, WLAN_802_11_RATES rates)
+{
+    t_u32 k = 0;
+
+    ENTER();
+
+    if (bss_mode == MLAN_BSS_MODE_INFRA) {
+        /* Infra. mode */
+        switch (config_bands) {
+        case BAND_B:
+            PRINTM(MINFO, "Infra Band=%d SupportedRates_B\n", config_bands);
+            k = wlan_copy_rates(rates, k, SupportedRates_B,
+                                sizeof(SupportedRates_B));
+            break;
+        case BAND_G:
+        case BAND_G | BAND_GN:
+            PRINTM(MINFO, "Infra band=%d SupportedRates_G\n", config_bands);
+            k = wlan_copy_rates(rates, k, SupportedRates_G,
+                                sizeof(SupportedRates_G));
+            break;
+        case BAND_B | BAND_G:
+        case BAND_A | BAND_B | BAND_G:
+        case BAND_A | BAND_B:
+        case BAND_A | BAND_B | BAND_G | BAND_GN | BAND_AN:
+        case BAND_B | BAND_G | BAND_GN:
+            PRINTM(MINFO, "Infra band=%d SupportedRates_BG\n", config_bands);
+#ifdef WIFI_DIRECT_SUPPORT
+            if (pmpriv->bss_type == MLAN_BSS_TYPE_WIFIDIRECT)
+                k = wlan_copy_rates(rates, k, SupportedRates_G,
+                                    sizeof(SupportedRates_G));
+            else
+                k = wlan_copy_rates(rates, k, SupportedRates_BG,
+                                    sizeof(SupportedRates_BG));
+#else
+            k = wlan_copy_rates(rates, k, SupportedRates_BG,
+                                sizeof(SupportedRates_BG));
+#endif
+            break;
+        case BAND_A:
+        case BAND_A | BAND_G:
+            PRINTM(MINFO, "Infra band=%d SupportedRates_A\n", config_bands);
+            k = wlan_copy_rates(rates, k, SupportedRates_A,
+                                sizeof(SupportedRates_A));
+            break;
+        case BAND_AN:
+        case BAND_A | BAND_AN:
+        case BAND_A | BAND_G | BAND_AN | BAND_GN:
+            PRINTM(MINFO, "Infra band=%d SupportedRates_A\n", config_bands);
+            k = wlan_copy_rates(rates, k, SupportedRates_A,
+                                sizeof(SupportedRates_A));
+            break;
+        case BAND_GN:
+            PRINTM(MINFO, "Infra band=%d SupportedRates_N\n", config_bands);
+            k = wlan_copy_rates(rates, k, SupportedRates_N,
+                                sizeof(SupportedRates_N));
+            break;
+        }
+    } else {
+        /* Ad-hoc mode */
+        switch (config_bands) {
+        case BAND_B:
+            PRINTM(MINFO, "Band: Adhoc B\n");
+            k = wlan_copy_rates(rates, k, AdhocRates_B, sizeof(AdhocRates_B));
+            break;
+        case BAND_G:
+        case BAND_G | BAND_GN:
+            PRINTM(MINFO, "Band: Adhoc G only\n");
+            k = wlan_copy_rates(rates, k, AdhocRates_G, sizeof(AdhocRates_G));
+            break;
+        case BAND_B | BAND_G:
+        case BAND_B | BAND_G | BAND_GN:
+            PRINTM(MINFO, "Band: Adhoc BG\n");
+            k = wlan_copy_rates(rates, k, AdhocRates_BG, sizeof(AdhocRates_BG));
+            break;
+        case BAND_A:
+        case BAND_AN:
+        case BAND_A | BAND_AN:
+            PRINTM(MINFO, "Band: Adhoc A\n");
+            k = wlan_copy_rates(rates, k, AdhocRates_A, sizeof(AdhocRates_A));
+            break;
+        }
+    }
+
+    LEAVE();
+    return k;
+}
+
+/**
+ *  @brief This function sets region table.
+ *
+ *  @param pmpriv  A pointer to mlan_private structure
+ *  @param region  The region code
+ *  @param band    The band
+ *
+ *  @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_set_regiontable(mlan_private * pmpriv, t_u8 region, t_u8 band)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    int i = 0;
+    chan_freq_power_t *cfp;
+    int cfp_no;
+
+    ENTER();
+
+    memset(pmadapter, pmadapter->region_channel, 0,
+           sizeof(pmadapter->region_channel));
+
+    if (band & (BAND_B | BAND_G | BAND_GN)) {
+        cfp =
+            wlan_get_region_cfp_table(pmadapter, region,
+                                      BAND_G | BAND_B | BAND_GN, &cfp_no);
+        if (cfp) {
+            pmadapter->region_channel[i].num_cfp = (t_u8) cfp_no;
+            pmadapter->region_channel[i].pcfp = cfp;
+        } else {
+            PRINTM(MERROR, "wrong region code %#x in Band B-G\n", region);
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+        pmadapter->region_channel[i].valid = MTRUE;
+        pmadapter->region_channel[i].region = region;
+        if (band & BAND_GN)
+            pmadapter->region_channel[i].band = BAND_G;
+        else
+            pmadapter->region_channel[i].band =
+                (band & BAND_G) ? BAND_G : BAND_B;
+        i++;
+    }
+    if (band & (BAND_A | BAND_AN)) {
+        cfp = wlan_get_region_cfp_table(pmadapter, region, BAND_A, &cfp_no);
+        if (cfp) {
+            pmadapter->region_channel[i].num_cfp = (t_u8) cfp_no;
+            pmadapter->region_channel[i].pcfp = cfp;
+        } else {
+            PRINTM(MERROR, "wrong region code %#x in Band A\n", region);
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+        pmadapter->region_channel[i].valid = MTRUE;
+        pmadapter->region_channel[i].region = region;
+        pmadapter->region_channel[i].band = BAND_A;
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Get if radar detection is enabled or not on a certain channel
+ *
+ *  @param priv    Private driver information structure
+ *  @param chnl Channel to determine radar detection requirements
+ *
+ *  @return
+ *    - MTRUE if radar detection is required
+ *    - MFALSE otherwise
+ */
+t_bool
+wlan_get_cfp_radar_detect(mlan_private * priv, t_u8 chnl)
+{
+    int i, j;
+    t_bool required = MFALSE;
+    chan_freq_power_t *pcfp = MNULL;
+
+    ENTER();
+
+    /* get the cfp table first */
+    for (i = 0; i < MAX_REGION_CHANNEL_NUM; i++) {
+        if (priv->adapter->region_channel[i].band == BAND_A) {
+            pcfp = priv->adapter->region_channel[i].pcfp;
+            break;
+        }
+    }
+
+    if (!pcfp) {
+        /* This means operation in BAND-A is not support, we can just return
+           false here, it's harmless */
+        goto done;
+    }
+
+    /* get the radar detection requirements according to chan num */
+    for (j = 0; j < priv->adapter->region_channel[i].num_cfp; j++) {
+        if (pcfp[j].channel == chnl) {
+            required = pcfp[j].passive_scan_or_radar_detect;
+            break;
+        }
+    }
+
+  done:
+    LEAVE();
+    return required;
+}
+
+/**
+ *  @brief Get if scan type is passive or not on a certain channel for b/g band
+ *
+ *  @param priv    Private driver information structure
+ *  @param chnl Channel to determine scan type
+ *
+ *  @return
+ *    - MTRUE if scan type is passive
+ *    - MFALSE otherwise
+ */
+
+t_bool
+wlan_bg_scan_type_is_passive(mlan_private * priv, t_u8 chnl)
+{
+    int i, j;
+    t_bool passive = MFALSE;
+    chan_freq_power_t *pcfp = MNULL;
+
+    ENTER();
+
+    /* get the cfp table first */
+    for (i = 0; i < MAX_REGION_CHANNEL_NUM; i++) {
+        if (priv->adapter->region_channel[i].band & (BAND_B | BAND_G)) {
+            pcfp = priv->adapter->region_channel[i].pcfp;
+            break;
+        }
+    }
+
+    if (!pcfp) {
+        /* This means operation in BAND-B or BAND_G is not support, we can
+           just return false here */
+        goto done;
+    }
+
+    /* get the bg scan type according to chan num */
+    for (j = 0; j < priv->adapter->region_channel[i].num_cfp; j++) {
+        if (pcfp[j].channel == chnl) {
+            passive = pcfp[j].passive_scan_or_radar_detect;
+            break;
+        }
+    }
+
+  done:
+    LEAVE();
+    return passive;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_cmdevt.c b/drivers/net/wireless/sd8797/mlan/mlan_cmdevt.c
new file mode 100644
index 000000000000..a3e5ef0517f0
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_cmdevt.c
@@ -0,0 +1,2934 @@
+/**
+ * @file mlan_cmdevt.c
+ *
+ *  @brief This file contains the handling of CMD/EVENT in MLAN
+ *
+ *  Copyright (C) 2009-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *  
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/*************************************************************
+Change Log:
+    05/12/2009: initial version
+************************************************************/
+#include "mlan.h"
+#ifdef STA_SUPPORT
+#include "mlan_join.h"
+#endif
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_wmm.h"
+#include "mlan_11n.h"
+#include "mlan_11h.h"
+#include "mlan_sdio.h"
+/********************************************************
+                Local Variables
+********************************************************/
+
+/*******************************************************
+                Global Variables
+********************************************************/
+
+/********************************************************
+                Local Functions
+********************************************************/
+
+/**
+ *  @brief This function convert a given character to hex
+ *
+ *  @param chr        Character to be converted
+ *
+ *  @return           The converted hex if chr is a valid hex, else 0
+ */
+static t_u32
+wlan_hexval(t_u8 chr)
+{
+    if (chr >= '0' && chr <= '9')
+        return chr - '0';
+    if (chr >= 'A' && chr <= 'F')
+        return chr - 'A' + 10;
+    if (chr >= 'a' && chr <= 'f')
+        return chr - 'a' + 10;
+
+    return 0;
+}
+
+/**
+ *  @brief This function convert a given string to hex
+ *
+ *  @param a            A pointer to string to be converted
+ *
+ *  @return             The converted hex value if param a is a valid hex, else 0
+ */
+int
+wlan_atox(t_u8 * a)
+{
+    int i = 0;
+
+    ENTER();
+
+    while (wlan_isxdigit(*a))
+        i = i * 16 + wlan_hexval(*a++);
+
+    LEAVE();
+    return i;
+}
+
+/**
+ *  @brief This function parse cal data from ASCII to hex
+ *
+ *  @param src          A pointer to source data
+ *  @param len          Source dara length
+ *  @param dst          A pointer to a buf to store the parsed data
+ *
+ *  @return             The parsed hex data length
+ */
+static t_u32
+wlan_parse_cal_cfg(t_u8 * src, t_size len, t_u8 * dst)
+{
+    t_u8 *ptr;
+    t_u8 *dptr;
+
+    ENTER();
+    ptr = src;
+    dptr = dst;
+
+    while (ptr - src < len) {
+        while (*ptr && (wlan_isspace(*ptr) || *ptr == '\t')) {
+            ptr++;
+        }
+
+        if (wlan_isxdigit(*ptr)) {
+            *dptr++ = wlan_atox(ptr);
+            ptr += 2;
+        } else {
+            ptr++;
+        }
+    }
+    LEAVE();
+    return (dptr - dst);
+}
+
+/**
+ *  @brief This function initializes the command node.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pcmd_node    A pointer to cmd_ctrl_node structure
+ *  @param cmd_oid      Cmd oid: treated as sub command
+ *  @param pioctl_buf   A pointer to MLAN IOCTL Request buffer
+ *  @param pdata_buf    A pointer to information buffer
+ *
+ *  @return             N/A
+ */
+static void
+wlan_init_cmd_node(IN pmlan_private pmpriv,
+                   IN cmd_ctrl_node * pcmd_node,
+                   IN t_u32 cmd_oid,
+                   IN t_void * pioctl_buf, IN t_void * pdata_buf)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+
+    ENTER();
+
+    if (pcmd_node == MNULL) {
+        LEAVE();
+        return;
+    }
+    pcmd_node->priv = pmpriv;
+    pcmd_node->cmd_oid = cmd_oid;
+    pcmd_node->pioctl_buf = pioctl_buf;
+    pcmd_node->pdata_buf = pdata_buf;
+
+    pcmd_node->cmdbuf = pcmd_node->pmbuf;
+
+    /* Make sure head_ptr for cmd buf is Align */
+    pcmd_node->cmdbuf->data_offset = 0;
+    memset(pmadapter, pcmd_node->cmdbuf->pbuf, 0, MRVDRV_SIZE_OF_CMD_BUFFER);
+
+    /* Prepare mlan_buffer for command sending */
+    pcmd_node->cmdbuf->buf_type = MLAN_BUF_TYPE_CMD;
+    pcmd_node->cmdbuf->data_offset += INTF_HEADER_LEN;
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function gets a free command node if available in
+ *              command free queue.
+ *
+ *  @param pmadapter        A pointer to mlan_adapter structure
+ *
+ *  @return cmd_ctrl_node   A pointer to cmd_ctrl_node structure or MNULL
+ */
+static cmd_ctrl_node *
+wlan_get_cmd_node(mlan_adapter * pmadapter)
+{
+    cmd_ctrl_node *pcmd_node;
+
+    ENTER();
+
+    if (pmadapter == MNULL) {
+        LEAVE();
+        return MNULL;
+    }
+
+    if (util_peek_list(pmadapter->pmoal_handle, &pmadapter->cmd_free_q,
+                       pmadapter->callbacks.moal_spin_lock,
+                       pmadapter->callbacks.moal_spin_unlock)) {
+        pcmd_node = (cmd_ctrl_node *) util_dequeue_list(pmadapter->pmoal_handle,
+                                                        &pmadapter->cmd_free_q,
+                                                        pmadapter->callbacks.
+                                                        moal_spin_lock,
+                                                        pmadapter->callbacks.
+                                                        moal_spin_unlock);
+    } else {
+        PRINTM(MERROR, "GET_CMD_NODE: cmd_ctrl_node is not available\n");
+        pcmd_node = MNULL;
+    }
+
+    LEAVE();
+    return pcmd_node;
+}
+
+/**
+ *  @brief This function cleans command node.
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pcmd_node    A pointer to cmd_ctrl_node structure
+ *
+ *  @return             N/A
+ */
+static t_void
+wlan_clean_cmd_node(pmlan_adapter pmadapter, cmd_ctrl_node * pcmd_node)
+{
+    ENTER();
+
+    if (pcmd_node == MNULL) {
+        LEAVE();
+        return;
+    }
+    pcmd_node->cmd_oid = 0;
+    pcmd_node->cmd_flag = 0;
+    pcmd_node->pioctl_buf = MNULL;
+    pcmd_node->pdata_buf = MNULL;
+
+    if (pcmd_node->respbuf) {
+        wlan_free_mlan_buffer(pmadapter, pcmd_node->respbuf);
+        pcmd_node->respbuf = MNULL;
+    }
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function will return the pointer to the first entry in
+ *  		pending cmd which matches the given pioctl_req
+ *
+ *  @param pmadapter    A pointer to mlan_adapter
+ *  @param pioctl_req   A pointer to mlan_ioctl_req buf
+ *
+ *  @return 	   A pointer to first entry match pioctl_req
+ */
+static cmd_ctrl_node *
+wlan_get_pending_ioctl_cmd(pmlan_adapter pmadapter, pmlan_ioctl_req pioctl_req)
+{
+    cmd_ctrl_node *pcmd_node = MNULL;
+
+    ENTER();
+
+    if (!
+        (pcmd_node =
+         (cmd_ctrl_node *) util_peek_list(pmadapter->pmoal_handle,
+                                          &pmadapter->cmd_pending_q,
+                                          pmadapter->callbacks.moal_spin_lock,
+                                          pmadapter->callbacks.
+                                          moal_spin_unlock))) {
+        LEAVE();
+        return MNULL;
+    }
+    while (pcmd_node != (cmd_ctrl_node *) & pmadapter->cmd_pending_q) {
+        if (pcmd_node->pioctl_buf == pioctl_req) {
+            LEAVE();
+            return pcmd_node;
+        }
+        pcmd_node = pcmd_node->pnext;
+    }
+    LEAVE();
+    return MNULL;
+}
+
+/**
+ *  @brief This function handles the command response of host_cmd
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return        MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_host_cmd(IN pmlan_private pmpriv,
+                  IN HostCmd_DS_COMMAND * resp, IN mlan_ioctl_req * pioctl_buf)
+{
+    mlan_ds_misc_cfg *misc;
+    t_u16 size = wlan_le16_to_cpu(resp->size);
+
+    ENTER();
+
+    PRINTM(MINFO, "host command response size = %d\n", size);
+    size = MIN(size, MRVDRV_SIZE_OF_CMD_BUFFER);
+    if (pioctl_buf) {
+        misc = (mlan_ds_misc_cfg *) pioctl_buf->pbuf;
+        misc->param.hostcmd.len = size;
+        memcpy(pmpriv->adapter, misc->param.hostcmd.cmd, (void *) resp, size);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function sends host command to firmware.
+ *
+ *  @param pmpriv     	A pointer to mlan_private structure
+ *  @param cmd      	A pointer to HostCmd_DS_COMMAND structure
+ *  @param pdata_buf	A pointer to data buffer
+ *  @return         	MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_host_cmd(IN pmlan_private pmpriv,
+                  IN HostCmd_DS_COMMAND * cmd, IN t_void * pdata_buf)
+{
+    mlan_ds_misc_cmd *pcmd_ptr = (mlan_ds_misc_cmd *) pdata_buf;
+
+    ENTER();
+
+    /* Copy the HOST command to command buffer */
+    memcpy(pmpriv->adapter, (void *) cmd, pcmd_ptr->cmd,
+           MIN(MRVDRV_SIZE_OF_CMD_BUFFER, pcmd_ptr->len));
+    PRINTM(MINFO, "Host command size = %d\n", pcmd_ptr->len);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function downloads a command to firmware.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pcmd_node    A pointer to cmd_ctrl_node structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_dnld_cmd_to_fw(IN mlan_private * pmpriv, IN cmd_ctrl_node * pcmd_node)
+{
+
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    mlan_callbacks *pcb = (mlan_callbacks *) & pmadapter->callbacks;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    HostCmd_DS_COMMAND *pcmd;
+    mlan_ioctl_req *pioctl_buf = MNULL;
+    t_u16 cmd_code;
+    t_u16 cmd_size;
+#ifdef DEBUG_LEVEL1
+    t_u32 sec, usec;
+#endif
+
+    ENTER();
+
+    if (pcmd_node)
+        if (pcmd_node->pioctl_buf != MNULL)
+            pioctl_buf = (mlan_ioctl_req *) pcmd_node->pioctl_buf;
+    if (!pmadapter || !pcmd_node) {
+        if (pioctl_buf)
+            pioctl_buf->status_code = MLAN_ERROR_CMD_DNLD_FAIL;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    pcmd =
+        (HostCmd_DS_COMMAND *) (pcmd_node->cmdbuf->pbuf +
+                                pcmd_node->cmdbuf->data_offset);
+
+    /* Sanity test */
+    if (pcmd == MNULL || pcmd->size == 0) {
+        PRINTM(MERROR, "DNLD_CMD: pcmd is null or command size is zero, "
+               "Not sending\n");
+        if (pioctl_buf)
+            pioctl_buf->status_code = MLAN_ERROR_CMD_DNLD_FAIL;
+        wlan_insert_cmd_to_free_q(pmadapter, pcmd_node);
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Set command sequence number */
+    pmadapter->seq_num++;
+    pcmd->seq_num =
+        wlan_cpu_to_le16(HostCmd_SET_SEQ_NO_BSS_INFO
+                         (pmadapter->seq_num, pcmd_node->priv->bss_num,
+                          pcmd_node->priv->bss_type));
+
+    wlan_request_cmd_lock(pmadapter);
+    pmadapter->curr_cmd = pcmd_node;
+    wlan_release_cmd_lock(pmadapter);
+
+    cmd_code = wlan_le16_to_cpu(pcmd->command);
+    cmd_size = wlan_le16_to_cpu(pcmd->size);
+
+    pcmd_node->cmdbuf->data_len = cmd_size;
+
+    PRINTM_GET_SYS_TIME(MCMND, &sec, &usec);
+    PRINTM_NETINTF(MCMND, pmpriv);
+    PRINTM(MCMND, "DNLD_CMD (%lu.%06lu): 0x%x, act 0x%x, len %d, seqno 0x%x\n",
+           sec, usec, cmd_code,
+           wlan_le16_to_cpu(*(t_u16 *) ((t_u8 *) pcmd + S_DS_GEN)), cmd_size,
+           wlan_le16_to_cpu(pcmd->seq_num));
+    DBG_HEXDUMP(MCMD_D, "DNLD_CMD", (t_u8 *) pcmd, cmd_size);
+
+    /* Send the command to lower layer */
+
+    pcmd_node->cmdbuf->data_offset -= INTF_HEADER_LEN;
+    pcmd_node->cmdbuf->data_len += INTF_HEADER_LEN;
+    /* Extra header for SDIO is added here */
+    ret =
+        wlan_sdio_host_to_card(pmadapter, MLAN_TYPE_CMD, pcmd_node->cmdbuf,
+                               MNULL);
+
+    if (ret == MLAN_STATUS_FAILURE) {
+        PRINTM(MERROR, "DNLD_CMD: Host to Card Failed\n");
+        if (pioctl_buf)
+            pioctl_buf->status_code = MLAN_ERROR_CMD_DNLD_FAIL;
+        wlan_insert_cmd_to_free_q(pmadapter, pmadapter->curr_cmd);
+
+        wlan_request_cmd_lock(pmadapter);
+        pmadapter->curr_cmd = MNULL;
+        wlan_release_cmd_lock(pmadapter);
+
+        pmadapter->dbg.num_cmd_host_to_card_failure++;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Save the last command id and action to debug log */
+    pmadapter->dbg.last_cmd_index =
+        (pmadapter->dbg.last_cmd_index + 1) % DBG_CMD_NUM;
+    pmadapter->dbg.last_cmd_id[pmadapter->dbg.last_cmd_index] = cmd_code;
+    pmadapter->dbg.last_cmd_act[pmadapter->dbg.last_cmd_index] =
+        wlan_le16_to_cpu(*(t_u16 *) ((t_u8 *) pcmd + S_DS_GEN));
+
+    /* Clear BSS_NO_BITS from HostCmd */
+    cmd_code &= HostCmd_CMD_ID_MASK;
+    /* soft_reset command has no command response, we should return here */
+    if (cmd_code == HostCmd_CMD_SOFT_RESET) {
+        PRINTM(MCMND, "DNLD_CMD: SoftReset\n");
+        if (pioctl_buf) {
+            wlan_request_cmd_lock(pmadapter);
+            pmadapter->curr_cmd->pioctl_buf = MNULL;
+            wlan_release_cmd_lock(pmadapter);
+            pcb->moal_ioctl_complete(pmadapter->pmoal_handle, pioctl_buf,
+                                     MLAN_STATUS_SUCCESS);
+        }
+        goto done;
+    }
+
+    /* Setup the timer after transmit command */
+    pcb->moal_start_timer(pmadapter->pmoal_handle, pmadapter->pmlan_cmd_timer,
+                          MFALSE, MRVDRV_TIMER_60S);
+
+    pmadapter->cmd_timer_is_set = MTRUE;
+
+    ret = MLAN_STATUS_SUCCESS;
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function sends sleep confirm command to firmware.
+ *
+ *  @param pmadapter  A pointer to mlan_adapter structure
+ *
+ *  @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_dnld_sleep_confirm_cmd(mlan_adapter * pmadapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    static t_u32 i = 0;
+    t_u16 cmd_len = 0;
+    opt_sleep_confirm_buffer *sleep_cfm_buf =
+        (opt_sleep_confirm_buffer *) (pmadapter->psleep_cfm->pbuf +
+                                      pmadapter->psleep_cfm->data_offset);
+
+    ENTER();
+
+    cmd_len = sizeof(OPT_Confirm_Sleep);
+    pmadapter->seq_num++;
+    sleep_cfm_buf->ps_cfm_sleep.seq_num =
+        wlan_cpu_to_le16(HostCmd_SET_SEQ_NO_BSS_INFO
+                         (pmadapter->seq_num,
+                          (wlan_get_priv(pmadapter, MLAN_BSS_ROLE_ANY))->
+                          bss_num,
+                          (wlan_get_priv(pmadapter, MLAN_BSS_ROLE_ANY))->
+                          bss_type));
+    DBG_HEXDUMP(MCMD_D, "SLEEP_CFM", &sleep_cfm_buf->ps_cfm_sleep,
+                sizeof(OPT_Confirm_Sleep));
+
+    /* Send sleep confirm command to firmware */
+
+    pmadapter->psleep_cfm->data_len = cmd_len + INTF_HEADER_LEN;
+    ret = wlan_sdio_host_to_card(pmadapter, MLAN_TYPE_CMD,
+                                 pmadapter->psleep_cfm, MNULL);
+
+    if (ret == MLAN_STATUS_FAILURE) {
+        PRINTM(MERROR, "SLEEP_CFM: failed\n");
+        pmadapter->dbg.num_cmd_sleep_cfm_host_to_card_failure++;
+        goto done;
+    } else {
+        if (GET_BSS_ROLE(wlan_get_priv(pmadapter, MLAN_BSS_ROLE_ANY)) ==
+            MLAN_BSS_ROLE_UAP)
+            pmadapter->ps_state = PS_STATE_SLEEP_CFM;
+#ifdef STA_SUPPORT
+        if (GET_BSS_ROLE(wlan_get_priv(pmadapter, MLAN_BSS_ROLE_ANY)) ==
+            MLAN_BSS_ROLE_STA) {
+            if (!sleep_cfm_buf->ps_cfm_sleep.sleep_cfm.resp_ctrl) {
+                /* Response is not needed for sleep confirm command */
+                pmadapter->ps_state = PS_STATE_SLEEP;
+            } else {
+                pmadapter->ps_state = PS_STATE_SLEEP_CFM;
+            }
+
+            if (!sleep_cfm_buf->ps_cfm_sleep.sleep_cfm.resp_ctrl
+                && (pmadapter->is_hs_configured &&
+                    !pmadapter->sleep_period.period)) {
+                pmadapter->pm_wakeup_card_req = MTRUE;
+                wlan_host_sleep_activated_event(wlan_get_priv
+                                                (pmadapter, MLAN_BSS_ROLE_STA),
+                                                MTRUE);
+            }
+        }
+#endif /* STA_SUPPORT */
+
+#define NUM_SC_PER_LINE         16
+        if (++i % NUM_SC_PER_LINE == 0)
+            PRINTM(MEVENT, "+\n");
+        else
+            PRINTM(MEVENT, "+");
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/********************************************************
+                Global Functions
+********************************************************/
+
+/**
+ *  @brief Event handler
+ *
+ *  @param priv		A pointer to mlan_private structure
+ *  @param event_id	Event ID
+ *  @param pmevent	Event buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_recv_event(pmlan_private priv, mlan_event_id event_id, t_void * pmevent)
+{
+    pmlan_callbacks pcb = &priv->adapter->callbacks;
+
+    ENTER();
+
+    if (pmevent)
+        /* The caller has provided the event. */
+        pcb->moal_recv_event(priv->adapter->pmoal_handle,
+                             (pmlan_event) pmevent);
+    else {
+        mlan_event mevent;
+
+        memset(priv->adapter, &mevent, 0, sizeof(mlan_event));
+        mevent.bss_index = priv->bss_index;
+        mevent.event_id = event_id;
+        mevent.event_len = 0;
+
+        pcb->moal_recv_event(priv->adapter->pmoal_handle, &mevent);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function allocates the command buffer and links
+ *              it to command free queue.
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_alloc_cmd_buffer(IN mlan_adapter * pmadapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_callbacks *pcb = (mlan_callbacks *) & pmadapter->callbacks;
+    cmd_ctrl_node *pcmd_array = MNULL;
+    t_u32 buf_size;
+    t_u32 i;
+
+    ENTER();
+
+    /* Allocate and initialize cmd_ctrl_node */
+    buf_size = sizeof(cmd_ctrl_node) * MRVDRV_NUM_OF_CMD_BUFFER;
+    ret =
+        pcb->moal_malloc(pmadapter->pmoal_handle, buf_size,
+                         MLAN_MEM_DEF | MLAN_MEM_DMA, (t_u8 **) & pcmd_array);
+    if (ret != MLAN_STATUS_SUCCESS || !pcmd_array) {
+        PRINTM(MERROR, "ALLOC_CMD_BUF: Failed to allocate pcmd_array\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    pmadapter->cmd_pool = pcmd_array;
+    memset(pmadapter, pmadapter->cmd_pool, 0, buf_size);
+
+    /* Allocate and initialize command buffers */
+    for (i = 0; i < MRVDRV_NUM_OF_CMD_BUFFER; i++) {
+        if (!(pcmd_array[i].pmbuf = wlan_alloc_mlan_buffer(pmadapter,
+                                                           MRVDRV_SIZE_OF_CMD_BUFFER,
+                                                           0, MTRUE))) {
+            PRINTM(MERROR,
+                   "ALLOC_CMD_BUF: Failed to allocate command buffer\n");
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+    }
+
+    for (i = 0; i < MRVDRV_NUM_OF_CMD_BUFFER; i++) {
+        wlan_insert_cmd_to_free_q(pmadapter, &pcmd_array[i]);
+    }
+    ret = MLAN_STATUS_SUCCESS;
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function frees the command buffer.
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_free_cmd_buffer(IN mlan_adapter * pmadapter)
+{
+    mlan_callbacks *pcb = (mlan_callbacks *) & pmadapter->callbacks;
+    cmd_ctrl_node *pcmd_array;
+    t_u32 i;
+
+    ENTER();
+
+    /* Need to check if cmd pool is allocated or not */
+    if (pmadapter->cmd_pool == MNULL) {
+        PRINTM(MINFO, "FREE_CMD_BUF: cmd_pool is Null\n");
+        goto done;
+    }
+
+    pcmd_array = pmadapter->cmd_pool;
+
+    /* Release shared memory buffers */
+    for (i = 0; i < MRVDRV_NUM_OF_CMD_BUFFER; i++) {
+        if (pcmd_array[i].pmbuf) {
+            PRINTM(MINFO, "Free all the command buffer.\n");
+            wlan_free_mlan_buffer(pmadapter, pcmd_array[i].pmbuf);
+            pcmd_array[i].pmbuf = MNULL;
+        }
+        if (pcmd_array[i].respbuf) {
+            wlan_free_mlan_buffer(pmadapter, pcmd_array[i].respbuf);
+            pcmd_array[i].respbuf = MNULL;
+        }
+    }
+    /* Release cmd_ctrl_node */
+    if (pmadapter->cmd_pool) {
+        PRINTM(MINFO, "Free command pool.\n");
+        pcb->moal_mfree(pmadapter->pmoal_handle, (t_u8 *) pmadapter->cmd_pool);
+        pmadapter->cmd_pool = MNULL;
+    }
+
+  done:
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles events generated by firmware
+ *
+ *  @param pmadapter		A pointer to mlan_adapter structure
+ *
+ *  @return		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_process_event(pmlan_adapter pmadapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private priv = wlan_get_priv(pmadapter, MLAN_BSS_ROLE_ANY);
+    pmlan_buffer pmbuf = pmadapter->pmlan_buffer_event;
+    t_u32 eventcause = pmadapter->event_cause;
+#ifdef DEBUG_LEVEL1
+    t_u32 in_ts_sec, in_ts_usec;
+#endif
+    ENTER();
+
+    /* Save the last event to debug log */
+    pmadapter->dbg.last_event_index =
+        (pmadapter->dbg.last_event_index + 1) % DBG_CMD_NUM;
+    pmadapter->dbg.last_event[pmadapter->dbg.last_event_index] =
+        (t_u16) eventcause;
+
+    if ((eventcause & EVENT_ID_MASK) == EVENT_RADAR_DETECTED) {
+        if (wlan_11h_dfs_event_preprocessing(pmadapter) == MLAN_STATUS_SUCCESS) {
+            memcpy(pmadapter, (t_u8 *) & eventcause,
+                   pmbuf->pbuf + pmbuf->data_offset, sizeof(eventcause));
+        }
+    }
+    /* Get BSS number and corresponding priv */
+    priv =
+        wlan_get_priv_by_id(pmadapter, EVENT_GET_BSS_NUM(eventcause),
+                            EVENT_GET_BSS_TYPE(eventcause));
+    if (!priv)
+        priv = wlan_get_priv(pmadapter, MLAN_BSS_ROLE_ANY);
+
+    /* Clear BSS_NO_BITS from event */
+    eventcause &= EVENT_ID_MASK;
+    pmadapter->event_cause = eventcause;
+
+    if (pmbuf) {
+        pmbuf->bss_index = priv->bss_index;
+        memcpy(pmadapter,
+               pmbuf->pbuf + pmbuf->data_offset,
+               (t_u8 *) & eventcause, sizeof(eventcause));
+    }
+
+    if (MTRUE && (eventcause != EVENT_PS_SLEEP && eventcause != EVENT_PS_AWAKE)
+        ) {
+        PRINTM_GET_SYS_TIME(MEVENT, &in_ts_sec, &in_ts_usec);
+        PRINTM_NETINTF(MEVENT, priv);
+        PRINTM(MEVENT, "%lu.%06lu : Event: 0x%x\n", in_ts_sec, in_ts_usec,
+               eventcause);
+    }
+
+    ret = priv->ops.process_event(priv);
+
+    pmadapter->event_cause = 0;
+    pmadapter->pmlan_buffer_event = MNULL;
+    if (pmbuf) {
+        wlan_free_mlan_buffer(pmadapter, pmbuf);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function requests a lock on command queue.
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *
+ *  @return             N/A
+ */
+t_void
+wlan_request_cmd_lock(IN mlan_adapter * pmadapter)
+{
+    mlan_callbacks *pcb = (mlan_callbacks *) & pmadapter->callbacks;
+
+    ENTER();
+
+    /* Call MOAL spin lock callback function */
+    pcb->moal_spin_lock(pmadapter->pmoal_handle, pmadapter->pmlan_cmd_lock);
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function releases a lock on command queue.
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *
+ *  @return             N/A
+ */
+t_void
+wlan_release_cmd_lock(IN mlan_adapter * pmadapter)
+{
+    mlan_callbacks *pcb = (mlan_callbacks *) & pmadapter->callbacks;
+
+    ENTER();
+
+    /* Call MOAL spin unlock callback function */
+    pcb->moal_spin_unlock(pmadapter->pmoal_handle, pmadapter->pmlan_cmd_lock);
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function prepare the command before sending to firmware.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd_no       Command number
+ *  @param cmd_action   Command action: GET or SET
+ *  @param cmd_oid      Cmd oid: treated as sub command
+ *  @param pioctl_buf   A pointer to MLAN IOCTL Request buffer
+ *  @param pdata_buf    A pointer to information buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_prepare_cmd(IN mlan_private * pmpriv,
+                 IN t_u16 cmd_no,
+                 IN t_u16 cmd_action,
+                 IN t_u32 cmd_oid,
+                 IN t_void * pioctl_buf, IN t_void * pdata_buf)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    cmd_ctrl_node *pcmd_node = MNULL;
+    HostCmd_DS_COMMAND *cmd_ptr = MNULL;
+    pmlan_ioctl_req pioctl_req = (mlan_ioctl_req *) pioctl_buf;
+
+    ENTER();
+
+    /* Sanity test */
+    if (!pmadapter || pmadapter->surprise_removed) {
+        PRINTM(MERROR, "PREP_CMD: Card is Removed\n");
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_FW_NOT_READY;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    if (pmadapter->hw_status == WlanHardwareStatusReset) {
+        if ((cmd_no != HostCmd_CMD_FUNC_INIT)
+            ) {
+            PRINTM(MERROR, "PREP_CMD: FW is in reset state\n");
+            if (pioctl_req)
+                pioctl_req->status_code = MLAN_ERROR_FW_NOT_READY;
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+    }
+
+    /* Get a new command node */
+    pcmd_node = wlan_get_cmd_node(pmadapter);
+
+    if (pcmd_node == MNULL) {
+        PRINTM(MERROR, "PREP_CMD: No free cmd node\n");
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_NO_MEM;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Initialize the command node */
+    wlan_init_cmd_node(pmpriv, pcmd_node, cmd_oid, pioctl_buf, pdata_buf);
+
+    if (pcmd_node->cmdbuf == MNULL) {
+        PRINTM(MERROR, "PREP_CMD: No free cmd buf\n");
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_NO_MEM;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    cmd_ptr =
+        (HostCmd_DS_COMMAND *) (pcmd_node->cmdbuf->pbuf +
+                                pcmd_node->cmdbuf->data_offset);
+    cmd_ptr->command = cmd_no;
+    cmd_ptr->result = 0;
+
+    /* Prepare command */
+    if (cmd_no)
+        ret =
+            pmpriv->ops.prepare_cmd(pmpriv, cmd_no, cmd_action, cmd_oid,
+                                    pioctl_buf, pdata_buf, cmd_ptr);
+    else {
+        ret = wlan_cmd_host_cmd(pmpriv, cmd_ptr, pdata_buf);
+        pcmd_node->cmd_flag |= CMD_F_HOSTCMD;
+    }
+
+    /* Return error, since the command preparation failed */
+    if (ret != MLAN_STATUS_SUCCESS) {
+        PRINTM(MERROR, "PREP_CMD: Command 0x%x preparation failed\n", cmd_no);
+        pcmd_node->pioctl_buf = MNULL;
+        wlan_insert_cmd_to_free_q(pmadapter, pcmd_node);
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_CMD_DNLD_FAIL;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Send command */
+#ifdef STA_SUPPORT
+    if (cmd_no == HostCmd_CMD_802_11_SCAN
+        || cmd_no == HostCmd_CMD_802_11_SCAN_EXT)
+        wlan_queue_scan_cmd(pmpriv, pcmd_node);
+    else {
+#endif
+        if ((cmd_no == HostCmd_CMD_802_11_HS_CFG_ENH) &&
+            (cmd_action == HostCmd_ACT_GEN_SET) &&
+            (pmadapter->hs_cfg.conditions == HOST_SLEEP_CFG_CANCEL))
+            wlan_insert_cmd_to_pending_q(pmadapter, pcmd_node, MFALSE);
+        else
+            wlan_insert_cmd_to_pending_q(pmadapter, pcmd_node, MTRUE);
+#ifdef STA_SUPPORT
+    }
+#endif
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function inserts command node to cmd_free_q
+ *              after cleaning it.
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pcmd_node    A pointer to cmd_ctrl_node structure
+ *
+ *  @return             N/A
+ */
+t_void
+wlan_insert_cmd_to_free_q(IN mlan_adapter * pmadapter,
+                          IN cmd_ctrl_node * pcmd_node)
+{
+    mlan_callbacks *pcb = (mlan_callbacks *) & pmadapter->callbacks;
+    mlan_ioctl_req *pioctl_req = MNULL;
+    ENTER();
+
+    if (pcmd_node == MNULL)
+        goto done;
+    if (pcmd_node->pioctl_buf) {
+        pioctl_req = (mlan_ioctl_req *) pcmd_node->pioctl_buf;
+        if (pioctl_req->status_code != MLAN_ERROR_NO_ERROR)
+            pcb->moal_ioctl_complete(pmadapter->pmoal_handle,
+                                     pioctl_req, MLAN_STATUS_FAILURE);
+        else
+            pcb->moal_ioctl_complete(pmadapter->pmoal_handle,
+                                     pioctl_req, MLAN_STATUS_SUCCESS);
+    }
+    /* Clean the node */
+    wlan_clean_cmd_node(pmadapter, pcmd_node);
+
+    /* Insert node into cmd_free_q */
+    util_enqueue_list_tail(pmadapter->pmoal_handle, &pmadapter->cmd_free_q,
+                           (pmlan_linked_list) pcmd_node,
+                           pmadapter->callbacks.moal_spin_lock,
+                           pmadapter->callbacks.moal_spin_unlock);
+  done:
+    LEAVE();
+}
+
+/**
+ *  @brief This function queues the command to cmd list.
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pcmd_node    A pointer to cmd_ctrl_node structure
+ *  @param add_tail      Specify if the cmd needs to be queued in the header or tail
+ *
+ *  @return             N/A
+ */
+t_void
+wlan_insert_cmd_to_pending_q(IN mlan_adapter * pmadapter,
+                             IN cmd_ctrl_node * pcmd_node, IN t_u32 add_tail)
+{
+    HostCmd_DS_COMMAND *pcmd = MNULL;
+    t_u16 command;
+
+    ENTER();
+
+    if (pcmd_node == MNULL) {
+        PRINTM(MERROR, "QUEUE_CMD: pcmd_node is MNULL\n");
+        goto done;
+    }
+
+    pcmd =
+        (HostCmd_DS_COMMAND *) (pcmd_node->cmdbuf->pbuf +
+                                pcmd_node->cmdbuf->data_offset);
+    if (pcmd == MNULL) {
+        PRINTM(MERROR, "QUEUE_CMD: pcmd is MNULL\n");
+        goto done;
+    }
+
+    command = wlan_le16_to_cpu(pcmd->command);
+
+    /* Exit_PS command needs to be queued in the header always. */
+    if (command == HostCmd_CMD_802_11_PS_MODE_ENH) {
+        HostCmd_DS_802_11_PS_MODE_ENH *pm = &pcmd->params.psmode_enh;
+        if (wlan_le16_to_cpu(pm->action) == DIS_AUTO_PS) {
+            if (pmadapter->ps_state != PS_STATE_AWAKE)
+                add_tail = MFALSE;
+        }
+    }
+
+    if (add_tail) {
+        util_enqueue_list_tail(pmadapter->pmoal_handle,
+                               &pmadapter->cmd_pending_q,
+                               (pmlan_linked_list) pcmd_node,
+                               pmadapter->callbacks.moal_spin_lock,
+                               pmadapter->callbacks.moal_spin_unlock);
+    } else {
+        util_enqueue_list_head(pmadapter->pmoal_handle,
+                               &pmadapter->cmd_pending_q,
+                               (pmlan_linked_list) pcmd_node,
+                               pmadapter->callbacks.moal_spin_lock,
+                               pmadapter->callbacks.moal_spin_unlock);
+    }
+
+    PRINTM_NETINTF(MCMND, pcmd_node->priv);
+    PRINTM(MCMND, "QUEUE_CMD: cmd=0x%x is queued\n", command);
+
+  done:
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function executes next command in command
+ *      pending queue. It will put firmware back to PS mode
+ *      if applicable.
+ *
+ *  @param pmadapter     A pointer to mlan_adapter structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_exec_next_cmd(mlan_adapter * pmadapter)
+{
+    mlan_private *priv = MNULL;
+    cmd_ctrl_node *pcmd_node = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    HostCmd_DS_COMMAND *pcmd;
+
+    ENTER();
+
+    /* Sanity test */
+    if (pmadapter == MNULL) {
+        PRINTM(MERROR, "EXEC_NEXT_CMD: pmadapter is MNULL\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    /* Check if already in processing */
+    if (pmadapter->curr_cmd) {
+        PRINTM(MERROR, "EXEC_NEXT_CMD: there is command in processing!\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    wlan_request_cmd_lock(pmadapter);
+    /* Check if any command is pending */
+    pcmd_node =
+        (cmd_ctrl_node *) util_peek_list(pmadapter->pmoal_handle,
+                                         &pmadapter->cmd_pending_q,
+                                         pmadapter->callbacks.moal_spin_lock,
+                                         pmadapter->callbacks.moal_spin_unlock);
+
+    if (pcmd_node) {
+        pcmd =
+            (HostCmd_DS_COMMAND *) (pcmd_node->cmdbuf->pbuf +
+                                    pcmd_node->cmdbuf->data_offset);
+        priv = pcmd_node->priv;
+
+        if (pmadapter->ps_state != PS_STATE_AWAKE) {
+            PRINTM(MERROR,
+                   "Cannot send command in sleep state, this should not happen\n");
+            wlan_release_cmd_lock(pmadapter);
+            goto done;
+        }
+
+        util_unlink_list(pmadapter->pmoal_handle,
+                         &pmadapter->cmd_pending_q,
+                         (pmlan_linked_list) pcmd_node,
+                         pmadapter->callbacks.moal_spin_lock,
+                         pmadapter->callbacks.moal_spin_unlock);
+        wlan_release_cmd_lock(pmadapter);
+        ret = wlan_dnld_cmd_to_fw(priv, pcmd_node);
+        priv = wlan_get_priv(pmadapter, MLAN_BSS_ROLE_ANY);
+        /* Any command sent to the firmware when host is in sleep mode, should
+           de-configure host sleep */
+        /* We should skip the host sleep configuration command itself though */
+        if (priv &&
+            (pcmd->command !=
+             wlan_cpu_to_le16(HostCmd_CMD_802_11_HS_CFG_ENH))) {
+            if (pmadapter->hs_activated == MTRUE) {
+                pmadapter->is_hs_configured = MFALSE;
+                wlan_host_sleep_activated_event(priv, MFALSE);
+            }
+        }
+        goto done;
+    } else {
+        wlan_release_cmd_lock(pmadapter);
+    }
+    ret = MLAN_STATUS_SUCCESS;
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function handles the command response
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_process_cmdresp(mlan_adapter * pmadapter)
+{
+    HostCmd_DS_COMMAND *resp = MNULL;
+    mlan_private *pmpriv = wlan_get_priv(pmadapter, MLAN_BSS_ROLE_ANY);
+    mlan_private *pmpriv_next = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 orig_cmdresp_no;
+    t_u16 cmdresp_no;
+    t_u16 cmdresp_result;
+    mlan_ioctl_req *pioctl_buf = MNULL;
+    mlan_callbacks *pcb = (mlan_callbacks *) & pmadapter->callbacks;
+#ifdef DEBUG_LEVEL1
+    t_u32 sec, usec;
+#endif
+    t_u32 i;
+
+    ENTER();
+
+    /* Now we got response from FW, cancel the command timer */
+    if (pmadapter->cmd_timer_is_set) {
+        /* Cancel command timeout timer */
+        pcb->moal_stop_timer(pmadapter->pmoal_handle,
+                             pmadapter->pmlan_cmd_timer);
+        /* Cancel command timeout timer */
+        pmadapter->cmd_timer_is_set = MFALSE;
+    }
+
+    if (pmadapter->curr_cmd)
+        if (pmadapter->curr_cmd->pioctl_buf != MNULL) {
+            pioctl_buf = (mlan_ioctl_req *) pmadapter->curr_cmd->pioctl_buf;
+        }
+
+    if (!pmadapter->curr_cmd || !pmadapter->curr_cmd->respbuf) {
+        resp = (HostCmd_DS_COMMAND *) pmadapter->upld_buf;
+        resp->command = wlan_le16_to_cpu(resp->command);
+        PRINTM(MERROR, "CMD_RESP: No curr_cmd, 0x%x\n", resp->command);
+        if (pioctl_buf)
+            pioctl_buf->status_code = MLAN_ERROR_CMD_RESP_FAIL;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    pmadapter->num_cmd_timeout = 0;
+
+    DBG_HEXDUMP(MCMD_D, "CMD_RESP",
+                pmadapter->curr_cmd->respbuf->pbuf +
+                pmadapter->curr_cmd->respbuf->data_offset,
+                pmadapter->curr_cmd->respbuf->data_len);
+
+    resp =
+        (HostCmd_DS_COMMAND *) (pmadapter->curr_cmd->respbuf->pbuf +
+                                pmadapter->curr_cmd->respbuf->data_offset);
+    wlan_request_cmd_lock(pmadapter);
+    if (pmadapter->curr_cmd->cmd_flag & CMD_F_CANCELED) {
+        cmd_ctrl_node *free_cmd = pmadapter->curr_cmd;
+        pmadapter->curr_cmd = MNULL;
+        wlan_release_cmd_lock(pmadapter);
+        PRINTM(MERROR, "CMD_RESP: 0x%x been canceled!\n",
+               wlan_le16_to_cpu(resp->command));
+        wlan_insert_cmd_to_free_q(pmadapter, free_cmd);
+        if (pioctl_buf)
+            pioctl_buf->status_code = MLAN_ERROR_CMD_CANCEL;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    } else {
+        wlan_release_cmd_lock(pmadapter);
+    }
+    if (pmadapter->curr_cmd->cmd_flag & CMD_F_HOSTCMD) {
+        /* Copy original response back to response buffer */
+        wlan_ret_host_cmd(pmpriv, resp, pioctl_buf);
+    }
+    orig_cmdresp_no = wlan_le16_to_cpu(resp->command);
+    resp->size = wlan_le16_to_cpu(resp->size);
+    resp->seq_num = wlan_le16_to_cpu(resp->seq_num);
+    resp->result = wlan_le16_to_cpu(resp->result);
+
+    /* Get BSS number and corresponding priv */
+    pmpriv =
+        wlan_get_priv_by_id(pmadapter, HostCmd_GET_BSS_NO(resp->seq_num),
+                            HostCmd_GET_BSS_TYPE(resp->seq_num));
+    if (!pmpriv)
+        pmpriv = wlan_get_priv(pmadapter, MLAN_BSS_ROLE_ANY);
+    /* Clear RET_BIT from HostCmd */
+    resp->command = (orig_cmdresp_no & HostCmd_CMD_ID_MASK);
+    cmdresp_no = resp->command;
+
+    cmdresp_result = resp->result;
+
+    /* Save the last command response to debug log */
+    pmadapter->dbg.last_cmd_resp_index =
+        (pmadapter->dbg.last_cmd_resp_index + 1) % DBG_CMD_NUM;
+    pmadapter->dbg.last_cmd_resp_id[pmadapter->dbg.last_cmd_resp_index] =
+        orig_cmdresp_no;
+
+    PRINTM_GET_SYS_TIME(MCMND, &sec, &usec);
+    PRINTM_NETINTF(MCMND, pmadapter->curr_cmd->priv);
+    PRINTM(MCMND, "CMD_RESP (%lu.%06lu): 0x%x, result %d, len %d, seqno 0x%x\n",
+           sec, usec, orig_cmdresp_no, cmdresp_result, resp->size,
+           resp->seq_num);
+
+    if (!(orig_cmdresp_no & HostCmd_RET_BIT)) {
+        PRINTM(MERROR, "CMD_RESP: Invalid response to command!\n");
+        if (pioctl_buf) {
+            pioctl_buf->status_code = MLAN_ERROR_FW_CMDRESP;
+        }
+        wlan_insert_cmd_to_free_q(pmadapter, pmadapter->curr_cmd);
+        wlan_request_cmd_lock(pmadapter);
+        pmadapter->curr_cmd = MNULL;
+        wlan_release_cmd_lock(pmadapter);
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    if (pmadapter->curr_cmd->cmd_flag & CMD_F_HOSTCMD) {
+        pmadapter->curr_cmd->cmd_flag &= ~CMD_F_HOSTCMD;
+        if ((cmdresp_result == HostCmd_RESULT_OK)
+            && (cmdresp_no == HostCmd_CMD_802_11_HS_CFG_ENH))
+            ret = wlan_ret_802_11_hs_cfg(pmpriv, resp, pioctl_buf);
+    } else {
+        /* handle response */
+        ret = pmpriv->ops.process_cmdresp(pmpriv, cmdresp_no, resp, pioctl_buf);
+    }
+
+    /* Check init command response */
+    if (pmadapter->hw_status == WlanHardwareStatusInitializing) {
+        if (ret == MLAN_STATUS_FAILURE) {
+#if defined(STA_SUPPORT)
+            if (pmadapter->pwarm_reset_ioctl_req) {
+                /* warm reset failure */
+                pmadapter->pwarm_reset_ioctl_req->status_code =
+                    MLAN_ERROR_CMD_RESP_FAIL;
+                pcb->moal_ioctl_complete(pmadapter->pmoal_handle,
+                                         pmadapter->pwarm_reset_ioctl_req,
+                                         MLAN_STATUS_FAILURE);
+                pmadapter->pwarm_reset_ioctl_req = MNULL;
+                goto done;
+            }
+#endif
+            PRINTM(MERROR, "cmd 0x%02x failed during initialization\n",
+                   cmdresp_no);
+            wlan_init_fw_complete(pmadapter);
+            goto done;
+        }
+    }
+
+    wlan_request_cmd_lock(pmadapter);
+    if (pmadapter->curr_cmd) {
+        cmd_ctrl_node *free_cmd = pmadapter->curr_cmd;
+        pioctl_buf = (mlan_ioctl_req *) pmadapter->curr_cmd->pioctl_buf;
+        pmadapter->curr_cmd = MNULL;
+        wlan_release_cmd_lock(pmadapter);
+
+        if (pioctl_buf && (ret == MLAN_STATUS_SUCCESS))
+            pioctl_buf->status_code = MLAN_ERROR_NO_ERROR;
+        else if (pioctl_buf && (ret == MLAN_STATUS_FAILURE))
+            pioctl_buf->status_code = MLAN_ERROR_CMD_RESP_FAIL;
+
+        /* Clean up and put current command back to cmd_free_q */
+        wlan_insert_cmd_to_free_q(pmadapter, free_cmd);
+    } else {
+        wlan_release_cmd_lock(pmadapter);
+    }
+
+    if ((pmadapter->hw_status == WlanHardwareStatusInitializing) &&
+        (pmadapter->last_init_cmd == cmdresp_no)) {
+        i = pmpriv->bss_index + 1;
+        while (!(pmpriv_next = pmadapter->priv[i]) && i < pmadapter->priv_num)
+            i++;
+        if (!pmpriv_next || i >= pmadapter->priv_num) {
+#if defined(STA_SUPPORT)
+            if (pmadapter->pwarm_reset_ioctl_req) {
+                /* warm reset complete */
+                pmadapter->hw_status = WlanHardwareStatusReady;
+                pcb->moal_ioctl_complete(pmadapter->pmoal_handle,
+                                         pmadapter->pwarm_reset_ioctl_req,
+                                         MLAN_STATUS_SUCCESS);
+                pmadapter->pwarm_reset_ioctl_req = MNULL;
+                goto done;
+            }
+#endif
+            pmadapter->hw_status = WlanHardwareStatusInitdone;
+        } else {
+            /* Issue init commands for the next interface */
+            ret = pmpriv_next->ops.init_cmd(pmpriv_next, MFALSE);
+        }
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function handles the timeout of command sending.
+ *  It will re-send the same command again.
+ *
+ *  @param function_context   A pointer to function_context
+ *  @return 	   N/A
+ */
+t_void
+wlan_cmd_timeout_func(t_void * function_context)
+{
+    mlan_adapter *pmadapter = (mlan_adapter *) function_context;
+    cmd_ctrl_node *pcmd_node = MNULL;
+    mlan_ioctl_req *pioctl_buf = MNULL;
+#ifdef DEBUG_LEVEL1
+    t_u32 sec, usec;
+#endif
+    t_u8 i;
+    mlan_private *pmpriv = MNULL;
+
+    ENTER();
+
+    pmadapter->cmd_timer_is_set = MFALSE;
+    pmadapter->num_cmd_timeout++;
+    pmadapter->dbg.num_cmd_timeout++;
+    if (!pmadapter->curr_cmd) {
+        PRINTM(MWARN, "CurCmd Empty\n");
+        goto exit;
+    }
+    pcmd_node = pmadapter->curr_cmd;
+    if (pcmd_node->pioctl_buf != MNULL) {
+        pioctl_buf = (mlan_ioctl_req *) pcmd_node->pioctl_buf;
+        pioctl_buf->status_code = MLAN_ERROR_CMD_TIMEOUT;
+    }
+
+    if (pcmd_node) {
+        pmadapter->dbg.timeout_cmd_id =
+            pmadapter->dbg.last_cmd_id[pmadapter->dbg.last_cmd_index];
+        pmadapter->dbg.timeout_cmd_act =
+            pmadapter->dbg.last_cmd_act[pmadapter->dbg.last_cmd_index];
+        PRINTM_GET_SYS_TIME(MERROR, &sec, &usec);
+        PRINTM(MERROR, "Timeout cmd id (%lu.%06lu) = 0x%x, act = 0x%x \n", sec,
+               usec, pmadapter->dbg.timeout_cmd_id,
+               pmadapter->dbg.timeout_cmd_act);
+        if (pcmd_node->cmdbuf) {
+            t_u8 *pcmd_buf;
+            pcmd_buf =
+                pcmd_node->cmdbuf->pbuf + pcmd_node->cmdbuf->data_offset +
+                INTF_HEADER_LEN;
+            for (i = 0; i < 16; i++) {
+                PRINTM(MERROR, "%02x ", *pcmd_buf++);
+            }
+            PRINTM(MERROR, "\n");
+        }
+
+        pmpriv = pcmd_node->priv;
+        if (pmpriv) {
+            PRINTM(MERROR, "BSS type = %d BSS role= %d\n", pmpriv->bss_type,
+                   pmpriv->bss_role);
+        }
+
+        PRINTM(MERROR, "num_cmd_timeout = %d\n",
+               pmadapter->dbg.num_cmd_timeout);
+        PRINTM(MERROR, "last_cmd_index = %d\n", pmadapter->dbg.last_cmd_index);
+        PRINTM(MERROR, "last_cmd_id = ");
+        for (i = 0; i < DBG_CMD_NUM; i++) {
+            PRINTM(MERROR, "0x%x ", pmadapter->dbg.last_cmd_id[i]);
+        }
+        PRINTM(MERROR, "\n");
+        PRINTM(MERROR, "last_cmd_act = ");
+        for (i = 0; i < DBG_CMD_NUM; i++) {
+            PRINTM(MERROR, "0x%x ", pmadapter->dbg.last_cmd_act[i]);
+        }
+        PRINTM(MERROR, "\n");
+        PRINTM(MERROR, "last_cmd_resp_index = %d\n",
+               pmadapter->dbg.last_cmd_resp_index);
+        PRINTM(MERROR, "last_cmd_resp_id = ");
+        for (i = 0; i < DBG_CMD_NUM; i++) {
+            PRINTM(MERROR, "0x%x ", pmadapter->dbg.last_cmd_resp_id[i]);
+        }
+        PRINTM(MERROR, "\n");
+        PRINTM(MERROR, "last_event_index = %d\n",
+               pmadapter->dbg.last_event_index);
+        PRINTM(MERROR, "last_event = ");
+        for (i = 0; i < DBG_CMD_NUM; i++) {
+            PRINTM(MERROR, "0x%x ", pmadapter->dbg.last_event[i]);
+        }
+        PRINTM(MERROR, "\n");
+
+        PRINTM(MERROR, "num_data_h2c_failure = %d\n",
+               pmadapter->dbg.num_tx_host_to_card_failure);
+        PRINTM(MERROR, "num_cmd_h2c_failure = %d\n",
+               pmadapter->dbg.num_cmd_host_to_card_failure);
+        PRINTM(MERROR, "num_data_c2h_failure = %d\n",
+               pmadapter->dbg.num_rx_card_to_host_failure);
+        PRINTM(MERROR, "num_cmdevt_c2h_failure = %d\n",
+               pmadapter->dbg.num_cmdevt_card_to_host_failure);
+        PRINTM(MERROR, "num_int_read_failure = %d\n",
+               pmadapter->dbg.num_int_read_failure);
+        PRINTM(MERROR, "last_int_status = %d\n",
+               pmadapter->dbg.last_int_status);
+
+        PRINTM(MERROR, "num_event_deauth = %d\n",
+               pmadapter->dbg.num_event_deauth);
+        PRINTM(MERROR, "num_event_disassoc = %d\n",
+               pmadapter->dbg.num_event_disassoc);
+        PRINTM(MERROR, "num_event_link_lost = %d\n",
+               pmadapter->dbg.num_event_link_lost);
+        PRINTM(MERROR, "num_cmd_deauth = %d\n", pmadapter->dbg.num_cmd_deauth);
+        PRINTM(MERROR, "num_cmd_assoc_success = %d\n",
+               pmadapter->dbg.num_cmd_assoc_success);
+        PRINTM(MERROR, "num_cmd_assoc_failure = %d\n",
+               pmadapter->dbg.num_cmd_assoc_failure);
+        PRINTM(MERROR, "cmd_resp_received=%d\n", pmadapter->cmd_resp_received);
+        PRINTM(MERROR, "event_received=%d\n", pmadapter->event_received);
+
+        PRINTM(MERROR, "max_tx_buf_size=%d\n", pmadapter->max_tx_buf_size);
+        PRINTM(MERROR, "tx_buf_size=%d\n", pmadapter->tx_buf_size);
+        PRINTM(MERROR, "curr_tx_buf_size=%d\n", pmadapter->curr_tx_buf_size);
+
+        PRINTM(MERROR, "data_sent=%d cmd_sent=%d\n", pmadapter->data_sent,
+               pmadapter->cmd_sent);
+
+        PRINTM(MERROR, "ps_mode=%d ps_state=%d\n", pmadapter->ps_mode,
+               pmadapter->ps_state);
+        PRINTM(MERROR, "wakeup_dev_req=%d wakeup_tries=%d\n",
+               pmadapter->pm_wakeup_card_req, pmadapter->pm_wakeup_fw_try);
+        PRINTM(MERROR, "hs_configured=%d hs_activated=%d\n",
+               pmadapter->is_hs_configured, pmadapter->hs_activated);
+        PRINTM(MERROR, "pps_uapsd_mode=%d sleep_pd=%d\n",
+               pmadapter->pps_uapsd_mode, pmadapter->sleep_period.period);
+        PRINTM(MERROR, "tx_lock_flag = %d\n", pmadapter->tx_lock_flag);
+        PRINTM(MERROR, "scan_processing = %d\n", pmadapter->scan_processing);
+
+        PRINTM(MERROR, "mp_rd_bitmap=0x%x curr_rd_port=0x%x\n",
+               pmadapter->mp_rd_bitmap, pmadapter->curr_rd_port);
+        PRINTM(MERROR, "mp_wr_bitmap=0x%x curr_wr_port=0x%x\n",
+               pmadapter->mp_wr_bitmap, pmadapter->curr_wr_port);
+    }
+    if (pmadapter->hw_status == WlanHardwareStatusInitializing)
+        wlan_init_fw_complete(pmadapter);
+    else
+        /* Signal MOAL to perform extra handling for debugging */
+    if (pmpriv) {
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_DRV_DBG_DUMP, MNULL);
+    } else {
+        wlan_recv_event(wlan_get_priv(pmadapter, MLAN_BSS_ROLE_ANY),
+                        MLAN_EVENT_ID_DRV_DBG_DUMP, MNULL);
+    }
+
+  exit:
+    LEAVE();
+    return;
+}
+
+#ifdef STA_SUPPORT
+/**
+ *  @brief Internal function used to flush the scan pending queue
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *
+ *  @return             N/A
+ */
+t_void
+wlan_flush_scan_queue(IN pmlan_adapter pmadapter)
+{
+    mlan_callbacks *pcb = (pmlan_callbacks) & pmadapter->callbacks;
+    cmd_ctrl_node *pcmd_node = MNULL;
+
+    ENTER();
+
+    while ((pcmd_node =
+            (cmd_ctrl_node *) util_peek_list(pmadapter->pmoal_handle,
+                                             &pmadapter->scan_pending_q,
+                                             pcb->moal_spin_lock,
+                                             pcb->moal_spin_unlock))) {
+        util_unlink_list(pmadapter->pmoal_handle, &pmadapter->scan_pending_q,
+                         (pmlan_linked_list) pcmd_node, pcb->moal_spin_lock,
+                         pcb->moal_spin_unlock);
+        pcmd_node->pioctl_buf = MNULL;
+        wlan_insert_cmd_to_free_q(pmadapter, pcmd_node);
+    }
+    wlan_request_cmd_lock(pmadapter);
+    pmadapter->scan_processing = MFALSE;
+    wlan_release_cmd_lock(pmadapter);
+
+    LEAVE();
+}
+#endif
+
+/**
+ *  @brief Cancel all pending cmd.
+ *
+ *  @param pmadapter	A pointer to mlan_adapter
+ *
+ *  @return		N/A
+ */
+t_void
+wlan_cancel_all_pending_cmd(pmlan_adapter pmadapter)
+{
+    cmd_ctrl_node *pcmd_node = MNULL;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    mlan_ioctl_req *pioctl_buf = MNULL;
+
+    ENTER();
+    /* Cancel current cmd */
+    wlan_request_cmd_lock(pmadapter);
+    if ((pmadapter->curr_cmd) && (pmadapter->curr_cmd->pioctl_buf)) {
+        pioctl_buf = (mlan_ioctl_req *) pmadapter->curr_cmd->pioctl_buf;
+        pmadapter->curr_cmd->pioctl_buf = MNULL;
+        wlan_release_cmd_lock(pmadapter);
+        pioctl_buf->status_code = MLAN_ERROR_CMD_CANCEL;
+        pcb->moal_ioctl_complete(pmadapter->pmoal_handle, pioctl_buf,
+                                 MLAN_STATUS_FAILURE);
+    } else {
+        wlan_release_cmd_lock(pmadapter);
+    }
+    /* Cancel all pending command */
+    while ((pcmd_node =
+            (cmd_ctrl_node *) util_peek_list(pmadapter->pmoal_handle,
+                                             &pmadapter->cmd_pending_q,
+                                             pcb->moal_spin_lock,
+                                             pcb->moal_spin_unlock))) {
+        util_unlink_list(pmadapter->pmoal_handle, &pmadapter->cmd_pending_q,
+                         (pmlan_linked_list) pcmd_node, pcb->moal_spin_lock,
+                         pcb->moal_spin_unlock);
+        if (pcmd_node->pioctl_buf) {
+            pioctl_buf = (mlan_ioctl_req *) pcmd_node->pioctl_buf;
+            pioctl_buf->status_code = MLAN_ERROR_CMD_CANCEL;
+            pcb->moal_ioctl_complete(pmadapter->pmoal_handle, pioctl_buf,
+                                     MLAN_STATUS_FAILURE);
+            pcmd_node->pioctl_buf = MNULL;
+        }
+        wlan_insert_cmd_to_free_q(pmadapter, pcmd_node);
+    }
+#ifdef STA_SUPPORT
+    /* Cancel all pending scan command */
+    wlan_flush_scan_queue(pmadapter);
+#endif
+    LEAVE();
+}
+
+/**
+ *  @brief Cancel pending ioctl cmd.
+ *
+ *  @param pmadapter	A pointer to mlan_adapter
+ *  @param pioctl_req	A pointer to mlan_ioctl_req buf
+ *
+ *  @return		N/A
+ */
+t_void
+wlan_cancel_pending_ioctl(pmlan_adapter pmadapter, pmlan_ioctl_req pioctl_req)
+{
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    cmd_ctrl_node *pcmd_node = MNULL;
+
+    ENTER();
+
+    PRINTM(MIOCTL, "MOAL Cancel IOCTL: 0x%x sub_id=0x%x action=%d\n",
+           pioctl_req->req_id, *((t_u32 *) pioctl_req->pbuf),
+           (int) pioctl_req->action);
+
+    wlan_request_cmd_lock(pmadapter);
+    if ((pmadapter->curr_cmd) &&
+        (pmadapter->curr_cmd->pioctl_buf == pioctl_req)) {
+        pcmd_node = pmadapter->curr_cmd;
+        pcmd_node->pioctl_buf = MNULL;
+        pcmd_node->cmd_flag |= CMD_F_CANCELED;
+    }
+
+    while ((pcmd_node =
+            wlan_get_pending_ioctl_cmd(pmadapter, pioctl_req)) != MNULL) {
+        util_unlink_list(pmadapter->pmoal_handle, &pmadapter->cmd_pending_q,
+                         (pmlan_linked_list) pcmd_node,
+                         pmadapter->callbacks.moal_spin_lock,
+                         pmadapter->callbacks.moal_spin_unlock);
+        pcmd_node->pioctl_buf = MNULL;
+        wlan_release_cmd_lock(pmadapter);
+        wlan_insert_cmd_to_free_q(pmadapter, pcmd_node);
+        wlan_request_cmd_lock(pmadapter);
+    }
+    wlan_release_cmd_lock(pmadapter);
+#ifdef STA_SUPPORT
+    if (pioctl_req->req_id == MLAN_IOCTL_SCAN) {
+        /* IOCTL will be completed, avoid calling IOCTL complete again from
+           EVENT */
+        if (pmadapter->pext_scan_ioctl_req == pioctl_req)
+            pmadapter->pext_scan_ioctl_req = MNULL;
+        /* Cancel all pending scan command */
+        wlan_flush_scan_queue(pmadapter);
+    }
+#endif
+    pioctl_req->status_code = MLAN_ERROR_CMD_CANCEL;
+    pcb->moal_ioctl_complete(pmadapter->pmoal_handle, pioctl_req,
+                             MLAN_STATUS_FAILURE);
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief Handle the version_ext resp
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_ver_ext(IN pmlan_private pmpriv,
+                 IN HostCmd_DS_COMMAND * resp, IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_VERSION_EXT *ver_ext = &resp->params.verext;
+    mlan_ds_get_info *info;
+    ENTER();
+    if (pioctl_buf) {
+        info = (mlan_ds_get_info *) pioctl_buf->pbuf;
+        info->param.ver_ext.version_str_sel = ver_ext->version_str_sel;
+        memcpy(pmpriv->adapter, info->param.ver_ext.version_str,
+               ver_ext->version_str, sizeof(char) * 128);
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Handle the rx mgmt forward registration resp
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_rx_mgmt_ind(IN pmlan_private pmpriv,
+                     IN HostCmd_DS_COMMAND * resp,
+                     IN mlan_ioctl_req * pioctl_buf)
+{
+    mlan_ds_misc_cfg *misc = MNULL;
+    ENTER();
+
+    if (pioctl_buf) {
+        misc = (mlan_ds_misc_cfg *) pioctl_buf->pbuf;
+        misc->param.mgmt_subtype_mask =
+            wlan_le32_to_cpu(resp->params.rx_mgmt_ind.mgmt_subtype_mask);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function checks conditions and prepares to
+ *              send sleep confirm command to firmware if OK.
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *
+ *  @return             N/A
+ */
+t_void
+wlan_check_ps_cond(mlan_adapter * pmadapter)
+{
+    ENTER();
+
+    if (!pmadapter->cmd_sent &&
+        !pmadapter->curr_cmd && !IS_CARD_RX_RCVD(pmadapter)) {
+        wlan_dnld_sleep_confirm_cmd(pmadapter);
+    } else {
+        PRINTM(MCMND, "Delay Sleep Confirm (%s%s%s)\n",
+               (pmadapter->cmd_sent) ? "D" : "",
+               (pmadapter->curr_cmd) ? "C" : "",
+               (IS_CARD_RX_RCVD(pmadapter)) ? "R" : "");
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function sends the HS_ACTIVATED event to the application
+ *
+ *  @param priv         A pointer to mlan_private structure
+ *  @param activated    MTRUE if activated, MFALSE if de-activated
+ *
+ *  @return             N/A
+ */
+t_void
+wlan_host_sleep_activated_event(pmlan_private priv, t_u8 activated)
+{
+    ENTER();
+
+    if (activated) {
+        if (priv->adapter->is_hs_configured) {
+            priv->adapter->hs_activated = MTRUE;
+            PRINTM(MEVENT, "hs_activated\n");
+            wlan_recv_event(priv, MLAN_EVENT_ID_DRV_HS_ACTIVATED, MNULL);
+        } else
+            PRINTM(MWARN, "hs_activated: HS not configured !!!\n");
+    } else {
+        PRINTM(MEVENT, "hs_deactived\n");
+        priv->adapter->hs_activated = MFALSE;
+        wlan_recv_event(priv, MLAN_EVENT_ID_DRV_HS_DEACTIVATED, MNULL);
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function sends the HS_WAKEUP event to the application
+ *
+ *  @param priv         A pointer to mlan_private structure
+ *
+ *  @return             N/A
+ */
+t_void
+wlan_host_sleep_wakeup_event(pmlan_private priv)
+{
+    ENTER();
+
+    if (priv->adapter->is_hs_configured) {
+        wlan_recv_event(priv, MLAN_EVENT_ID_FW_HS_WAKEUP, MNULL);
+    } else {
+        PRINTM(MWARN, "hs_wakeup: Host Sleep not configured !!!\n");
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function handles the command response of hs_cfg
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_802_11_hs_cfg(IN pmlan_private pmpriv,
+                       IN HostCmd_DS_COMMAND * resp,
+                       IN mlan_ioctl_req * pioctl_buf)
+{
+    pmlan_adapter pmadapter = pmpriv->adapter;
+    HostCmd_DS_802_11_HS_CFG_ENH *phs_cfg = &resp->params.opt_hs_cfg;
+
+    ENTER();
+
+    if (wlan_le16_to_cpu(phs_cfg->action) == HS_ACTIVATE) {
+        /* clean up curr_cmd to allow suspend */
+        if (pioctl_buf)
+            pioctl_buf->status_code = MLAN_ERROR_NO_ERROR;
+        /* Clean up and put current command back to cmd_free_q */
+        wlan_insert_cmd_to_free_q(pmadapter, pmadapter->curr_cmd);
+        wlan_request_cmd_lock(pmadapter);
+        pmadapter->curr_cmd = MNULL;
+        wlan_release_cmd_lock(pmadapter);
+        wlan_host_sleep_activated_event(pmpriv, MTRUE);
+        goto done;
+    } else {
+        phs_cfg->params.hs_config.conditions =
+            wlan_le32_to_cpu(phs_cfg->params.hs_config.conditions);
+        PRINTM(MCMND,
+               "CMD_RESP: HS_CFG cmd reply result=%#x,"
+               " conditions=0x%x gpio=0x%x gap=0x%x\n", resp->result,
+               phs_cfg->params.hs_config.conditions,
+               phs_cfg->params.hs_config.gpio, phs_cfg->params.hs_config.gap);
+    }
+    if (phs_cfg->params.hs_config.conditions != HOST_SLEEP_CFG_CANCEL) {
+        pmadapter->is_hs_configured = MTRUE;
+    } else {
+        pmadapter->is_hs_configured = MFALSE;
+        if (pmadapter->hs_activated)
+            wlan_host_sleep_activated_event(pmpriv, MFALSE);
+    }
+
+  done:
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Perform hs related activities on receiving the power up interrupt
+ *
+ *  @param pmadapter  A pointer to the adapter structure
+ *  @return           N/A
+ */
+t_void
+wlan_process_hs_config(pmlan_adapter pmadapter)
+{
+    ENTER();
+    PRINTM(MINFO, "Recevie interrupt/data in HS mode\n");
+    if (pmadapter->hs_cfg.gap == HOST_SLEEP_CFG_GAP_FF)
+        wlan_pm_wakeup_card(pmadapter);
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief Check sleep confirm command response and set the state to ASLEEP
+ *
+ *  @param pmadapter  A pointer to the adapter structure
+ *  @param pbuf       A pointer to the command response buffer
+ *  @param upld_len   Command response buffer length
+ *  @return           N/A
+ */
+void
+wlan_process_sleep_confirm_resp(pmlan_adapter pmadapter, t_u8 * pbuf,
+                                t_u32 upld_len)
+{
+    HostCmd_DS_COMMAND *cmd;
+
+    ENTER();
+
+    if (!upld_len) {
+        PRINTM(MERROR, "Command size is 0\n");
+        LEAVE();
+        return;
+    }
+    cmd = (HostCmd_DS_COMMAND *) pbuf;
+    cmd->result = wlan_le16_to_cpu(cmd->result);
+    cmd->command = wlan_le16_to_cpu(cmd->command);
+    cmd->seq_num = wlan_le16_to_cpu(cmd->seq_num);
+
+    /* Update sequence number */
+    cmd->seq_num = HostCmd_GET_SEQ_NO(cmd->seq_num);
+    /* Clear RET_BIT from HostCmd */
+    cmd->command &= HostCmd_CMD_ID_MASK;
+
+    if (cmd->command != HostCmd_CMD_802_11_PS_MODE_ENH) {
+        PRINTM(MERROR,
+               "Received unexpected response for command %x, result = %x\n",
+               cmd->command, cmd->result);
+        LEAVE();
+        return;
+    }
+    PRINTM(MEVENT, "#\n");
+    if (cmd->result != MLAN_STATUS_SUCCESS) {
+        PRINTM(MERROR, "Sleep confirm command failed\n");
+        pmadapter->pm_wakeup_card_req = MFALSE;
+        pmadapter->ps_state = PS_STATE_AWAKE;
+        LEAVE();
+        return;
+    }
+    pmadapter->pm_wakeup_card_req = MTRUE;
+
+    if (pmadapter->is_hs_configured) {
+        wlan_host_sleep_activated_event(wlan_get_priv
+                                        (pmadapter, MLAN_BSS_ROLE_ANY), MTRUE);
+    }
+    pmadapter->ps_state = PS_STATE_SLEEP;
+    LEAVE();
+}
+
+/**
+ *  @brief This function prepares command of power mode
+ *
+ *  @param pmpriv		A pointer to mlan_private structure
+ *  @param cmd	   		A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   the action: GET or SET
+ *  @param ps_bitmap    PS bitmap
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return         MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_enh_power_mode(pmlan_private pmpriv,
+                        IN HostCmd_DS_COMMAND * cmd,
+                        IN t_u16 cmd_action,
+                        IN t_u16 ps_bitmap, IN t_void * pdata_buf)
+{
+    HostCmd_DS_802_11_PS_MODE_ENH *psmode_enh = &cmd->params.psmode_enh;
+    t_u8 *tlv = MNULL;
+    t_u16 cmd_size = 0;
+
+    ENTER();
+
+    PRINTM(MCMND, "PS Command: action = 0x%x, bitmap = 0x%x\n", cmd_action,
+           ps_bitmap);
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_PS_MODE_ENH);
+    if (cmd_action == DIS_AUTO_PS) {
+        psmode_enh->action = wlan_cpu_to_le16(DIS_AUTO_PS);
+        psmode_enh->params.ps_bitmap = wlan_cpu_to_le16(ps_bitmap);
+        cmd->size = wlan_cpu_to_le16(S_DS_GEN + AUTO_PS_FIX_SIZE);
+    } else if (cmd_action == GET_PS) {
+        psmode_enh->action = wlan_cpu_to_le16(GET_PS);
+        psmode_enh->params.ps_bitmap = wlan_cpu_to_le16(ps_bitmap);
+        cmd->size = wlan_cpu_to_le16(S_DS_GEN + AUTO_PS_FIX_SIZE);
+    } else if (cmd_action == EN_AUTO_PS) {
+        psmode_enh->action = wlan_cpu_to_le16(EN_AUTO_PS);
+        psmode_enh->params.auto_ps.ps_bitmap = wlan_cpu_to_le16(ps_bitmap);
+        cmd_size = S_DS_GEN + AUTO_PS_FIX_SIZE;
+        tlv = (t_u8 *) cmd + cmd_size;
+        if (ps_bitmap & BITMAP_STA_PS) {
+            pmlan_adapter pmadapter = pmpriv->adapter;
+            MrvlIEtypes_ps_param_t *ps_tlv = (MrvlIEtypes_ps_param_t *) tlv;
+            ps_param *ps_mode = &ps_tlv->param;
+            ps_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_PS_PARAM);
+            ps_tlv->header.len =
+                wlan_cpu_to_le16(sizeof(MrvlIEtypes_ps_param_t) -
+                                 sizeof(MrvlIEtypesHeader_t));
+            cmd_size += sizeof(MrvlIEtypes_ps_param_t);
+            tlv += sizeof(MrvlIEtypes_ps_param_t);
+            ps_mode->null_pkt_interval =
+                wlan_cpu_to_le16(pmadapter->null_pkt_interval);
+            ps_mode->multiple_dtims =
+                wlan_cpu_to_le16(pmadapter->multiple_dtim);
+            ps_mode->bcn_miss_timeout =
+                wlan_cpu_to_le16(pmadapter->bcn_miss_time_out);
+            ps_mode->local_listen_interval =
+                wlan_cpu_to_le16(pmadapter->local_listen_interval);
+            ps_mode->adhoc_wake_period =
+                wlan_cpu_to_le16(pmadapter->adhoc_awake_period);
+            ps_mode->delay_to_ps = wlan_cpu_to_le16(pmadapter->delay_to_ps);
+            ps_mode->mode = wlan_cpu_to_le16(pmadapter->enhanced_ps_mode);
+        }
+        if (ps_bitmap & BITMAP_AUTO_DS) {
+            MrvlIEtypes_auto_ds_param_t *auto_ps_tlv =
+                (MrvlIEtypes_auto_ds_param_t *) tlv;
+            auto_ds_param *auto_ds = &auto_ps_tlv->param;
+            t_u16 idletime = 0;
+            auto_ps_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_AUTO_DS_PARAM);
+            auto_ps_tlv->header.len =
+                wlan_cpu_to_le16(sizeof(MrvlIEtypes_auto_ds_param_t) -
+                                 sizeof(MrvlIEtypesHeader_t));
+            cmd_size += sizeof(MrvlIEtypes_auto_ds_param_t);
+            tlv += sizeof(MrvlIEtypes_auto_ds_param_t);
+            if (pdata_buf)
+                idletime = ((mlan_ds_auto_ds *) pdata_buf)->idletime;
+            auto_ds->deep_sleep_timeout = wlan_cpu_to_le16(idletime);
+        }
+#if defined(UAP_SUPPORT)
+        if (pdata_buf &&
+            (ps_bitmap & (BITMAP_UAP_INACT_PS | BITMAP_UAP_DTIM_PS))) {
+            mlan_ds_ps_mgmt *ps_mgmt = (mlan_ds_ps_mgmt *) pdata_buf;
+            MrvlIEtypes_sleep_param_t *sleep_tlv = MNULL;
+            MrvlIEtypes_inact_sleep_param_t *inact_tlv = MNULL;
+            if (ps_mgmt->flags & PS_FLAG_SLEEP_PARAM) {
+                sleep_tlv = (MrvlIEtypes_sleep_param_t *) tlv;
+                sleep_tlv->header.type =
+                    wlan_cpu_to_le16(TLV_TYPE_AP_SLEEP_PARAM);
+                sleep_tlv->header.len =
+                    wlan_cpu_to_le16(sizeof(MrvlIEtypes_sleep_param_t) -
+                                     sizeof(MrvlIEtypesHeader_t));
+                sleep_tlv->ctrl_bitmap =
+                    wlan_cpu_to_le32(ps_mgmt->sleep_param.ctrl_bitmap);
+                sleep_tlv->min_sleep =
+                    wlan_cpu_to_le32(ps_mgmt->sleep_param.min_sleep);
+                sleep_tlv->max_sleep =
+                    wlan_cpu_to_le32(ps_mgmt->sleep_param.max_sleep);
+                cmd_size += sizeof(MrvlIEtypes_sleep_param_t);
+                tlv += sizeof(MrvlIEtypes_sleep_param_t);
+            }
+            if (ps_mgmt->flags & PS_FLAG_INACT_SLEEP_PARAM) {
+                inact_tlv = (MrvlIEtypes_inact_sleep_param_t *) tlv;
+                inact_tlv->header.type =
+                    wlan_cpu_to_le16(TLV_TYPE_AP_INACT_SLEEP_PARAM);
+                inact_tlv->header.len =
+                    wlan_cpu_to_le16(sizeof(MrvlIEtypes_inact_sleep_param_t)
+                                     - sizeof(MrvlIEtypesHeader_t));
+                inact_tlv->inactivity_to =
+                    wlan_cpu_to_le32(ps_mgmt->inact_param.inactivity_to);
+                inact_tlv->min_awake =
+                    wlan_cpu_to_le32(ps_mgmt->inact_param.min_awake);
+                inact_tlv->max_awake =
+                    wlan_cpu_to_le32(ps_mgmt->inact_param.max_awake);
+                cmd_size += sizeof(MrvlIEtypes_inact_sleep_param_t);
+                tlv += sizeof(MrvlIEtypes_inact_sleep_param_t);
+            }
+        }
+#endif
+        cmd->size = wlan_cpu_to_le16(cmd_size);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of ps_mode_enh
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_enh_power_mode(IN pmlan_private pmpriv,
+                        IN HostCmd_DS_COMMAND * resp,
+                        IN mlan_ioctl_req * pioctl_buf)
+{
+    pmlan_adapter pmadapter = pmpriv->adapter;
+    MrvlIEtypesHeader_t *mrvl_tlv = MNULL;
+    MrvlIEtypes_auto_ds_param_t *auto_ds_tlv = MNULL;
+    HostCmd_DS_802_11_PS_MODE_ENH *ps_mode = &resp->params.psmode_enh;
+
+    ENTER();
+
+    ps_mode->action = wlan_le16_to_cpu(ps_mode->action);
+    PRINTM(MINFO, "CMD_RESP: PS_MODE cmd reply result=%#x action=0x%X\n",
+           resp->result, ps_mode->action);
+    if (ps_mode->action == EN_AUTO_PS) {
+        ps_mode->params.auto_ps.ps_bitmap =
+            wlan_le16_to_cpu(ps_mode->params.auto_ps.ps_bitmap);
+        if (ps_mode->params.auto_ps.ps_bitmap & BITMAP_AUTO_DS) {
+            PRINTM(MCMND, "Enabled auto deep sleep\n");
+            pmpriv->adapter->is_deep_sleep = MTRUE;
+            mrvl_tlv =
+                (MrvlIEtypesHeader_t *) ((t_u8 *) ps_mode + AUTO_PS_FIX_SIZE);
+            while (wlan_le16_to_cpu(mrvl_tlv->type) != TLV_TYPE_AUTO_DS_PARAM) {
+                mrvl_tlv =
+                    (MrvlIEtypesHeader_t *) ((t_u8 *) mrvl_tlv +
+                                             wlan_le16_to_cpu(mrvl_tlv->len)
+                                             + sizeof(MrvlIEtypesHeader_t));
+            }
+            auto_ds_tlv = (MrvlIEtypes_auto_ds_param_t *) mrvl_tlv;
+            pmpriv->adapter->idle_time =
+                wlan_le16_to_cpu(auto_ds_tlv->param.deep_sleep_timeout);
+        }
+        if (ps_mode->params.auto_ps.ps_bitmap & BITMAP_STA_PS) {
+            PRINTM(MCMND, "Enabled STA power save\n");
+            if (pmadapter->sleep_period.period) {
+                PRINTM(MCMND, "Setting uapsd/pps mode to TRUE\n");
+            }
+        }
+#if defined(UAP_SUPPORT)
+        if (ps_mode->params.auto_ps.
+            ps_bitmap & (BITMAP_UAP_INACT_PS | BITMAP_UAP_DTIM_PS)) {
+            pmadapter->ps_mode = Wlan802_11PowerModePSP;
+            PRINTM(MCMND, "Enabled uAP power save\n");
+        }
+#endif
+    } else if (ps_mode->action == DIS_AUTO_PS) {
+        ps_mode->params.ps_bitmap = wlan_cpu_to_le16(ps_mode->params.ps_bitmap);
+        if (ps_mode->params.ps_bitmap & BITMAP_AUTO_DS) {
+            pmpriv->adapter->is_deep_sleep = MFALSE;
+            PRINTM(MCMND, "Disabled auto deep sleep\n");
+        }
+        if (ps_mode->params.ps_bitmap & BITMAP_STA_PS) {
+            PRINTM(MCMND, "Disabled STA power save\n");
+            if (pmadapter->sleep_period.period) {
+                pmadapter->delay_null_pkt = MFALSE;
+                pmadapter->tx_lock_flag = MFALSE;
+                pmadapter->pps_uapsd_mode = MFALSE;
+            }
+        }
+#if defined(UAP_SUPPORT)
+        if (ps_mode->params.
+            ps_bitmap & (BITMAP_UAP_INACT_PS | BITMAP_UAP_DTIM_PS)) {
+            pmadapter->ps_mode = Wlan802_11PowerModeCAM;
+            PRINTM(MCMND, "Disabled uAP power save\n");
+        }
+#endif
+    } else if (ps_mode->action == GET_PS) {
+        ps_mode->params.ps_bitmap = wlan_le16_to_cpu(ps_mode->params.ps_bitmap);
+        if (ps_mode->params.auto_ps.
+            ps_bitmap & (BITMAP_STA_PS | BITMAP_UAP_INACT_PS |
+                         BITMAP_UAP_DTIM_PS))
+            pmadapter->ps_mode = Wlan802_11PowerModePSP;
+        else
+            pmadapter->ps_mode = Wlan802_11PowerModeCAM;
+        PRINTM(MCMND, "ps_bitmap=0x%x\n", ps_mode->params.ps_bitmap);
+        if (pioctl_buf) {
+            mlan_ds_pm_cfg *pm_cfg = (mlan_ds_pm_cfg *) pioctl_buf->pbuf;
+            if (pm_cfg->sub_command == MLAN_OID_PM_CFG_IEEE_PS) {
+                if (ps_mode->params.auto_ps.ps_bitmap & BITMAP_STA_PS)
+                    pm_cfg->param.ps_mode = 1;
+                else
+                    pm_cfg->param.ps_mode = 0;
+            }
+#if defined(UAP_SUPPORT)
+            if (pm_cfg->sub_command == MLAN_OID_PM_CFG_PS_MODE) {
+                MrvlIEtypes_sleep_param_t *sleep_tlv = MNULL;
+                MrvlIEtypes_inact_sleep_param_t *inact_tlv = MNULL;
+                MrvlIEtypesHeader_t *tlv = MNULL;
+                t_u16 tlv_type = 0;
+                t_u16 tlv_len = 0;
+                t_u16 tlv_buf_left = 0;
+                pm_cfg->param.ps_mgmt.flags = PS_FLAG_PS_MODE;
+                if (ps_mode->params.ps_bitmap & BITMAP_UAP_INACT_PS)
+                    pm_cfg->param.ps_mgmt.ps_mode = PS_MODE_INACTIVITY;
+                else if (ps_mode->params.ps_bitmap & BITMAP_UAP_DTIM_PS)
+                    pm_cfg->param.ps_mgmt.ps_mode = PS_MODE_PERIODIC_DTIM;
+                else
+                    pm_cfg->param.ps_mgmt.ps_mode = PS_MODE_DISABLE;
+                tlv_buf_left = resp->size - (S_DS_GEN + AUTO_PS_FIX_SIZE);
+                tlv =
+                    (MrvlIEtypesHeader_t *) ((t_u8 *) ps_mode +
+                                             AUTO_PS_FIX_SIZE);
+                while (tlv_buf_left >= sizeof(MrvlIEtypesHeader_t)) {
+                    tlv_type = wlan_le16_to_cpu(tlv->type);
+                    tlv_len = wlan_le16_to_cpu(tlv->len);
+                    switch (tlv_type) {
+                    case TLV_TYPE_AP_SLEEP_PARAM:
+                        sleep_tlv = (MrvlIEtypes_sleep_param_t *) tlv;
+                        pm_cfg->param.ps_mgmt.flags |= PS_FLAG_SLEEP_PARAM;
+                        pm_cfg->param.ps_mgmt.sleep_param.ctrl_bitmap =
+                            wlan_le32_to_cpu(sleep_tlv->ctrl_bitmap);
+                        pm_cfg->param.ps_mgmt.sleep_param.min_sleep =
+                            wlan_le32_to_cpu(sleep_tlv->min_sleep);
+                        pm_cfg->param.ps_mgmt.sleep_param.max_sleep =
+                            wlan_le32_to_cpu(sleep_tlv->max_sleep);
+                        break;
+                    case TLV_TYPE_AP_INACT_SLEEP_PARAM:
+                        inact_tlv = (MrvlIEtypes_inact_sleep_param_t *) tlv;
+                        pm_cfg->param.ps_mgmt.flags |=
+                            PS_FLAG_INACT_SLEEP_PARAM;
+                        pm_cfg->param.ps_mgmt.inact_param.inactivity_to =
+                            wlan_le32_to_cpu(inact_tlv->inactivity_to);
+                        pm_cfg->param.ps_mgmt.inact_param.min_awake =
+                            wlan_le32_to_cpu(inact_tlv->min_awake);
+                        pm_cfg->param.ps_mgmt.inact_param.max_awake =
+                            wlan_le32_to_cpu(inact_tlv->max_awake);
+                        break;
+                    }
+                    tlv_buf_left -= tlv_len + sizeof(MrvlIEtypesHeader_t);
+                    tlv =
+                        (MrvlIEtypesHeader_t *) ((t_u8 *) tlv + tlv_len +
+                                                 sizeof(MrvlIEtypesHeader_t));
+                }
+            }
+#endif
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of tx rate query
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_802_11_tx_rate_query(IN pmlan_private pmpriv,
+                              IN HostCmd_DS_COMMAND * resp,
+                              IN mlan_ioctl_req * pioctl_buf)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    mlan_ds_rate *rate = MNULL;
+    ENTER();
+
+    pmpriv->tx_rate = resp->params.tx_rate.tx_rate;
+    pmpriv->tx_htinfo = resp->params.tx_rate.ht_info;
+    if (!pmpriv->is_data_rate_auto) {
+        pmpriv->data_rate = wlan_index_to_data_rate(pmadapter,
+                                                    pmpriv->tx_rate,
+                                                    pmpriv->tx_htinfo);
+    }
+
+    if (pioctl_buf) {
+        rate = (mlan_ds_rate *) pioctl_buf->pbuf;
+        if (rate->sub_command == MLAN_OID_RATE_CFG) {
+            if (rate->param.rate_cfg.rate_type == MLAN_RATE_INDEX) {
+                if (pmpriv->tx_htinfo & MBIT(0))
+                    rate->param.rate_cfg.rate =
+                        pmpriv->tx_rate + MLAN_RATE_INDEX_MCS0;
+                else
+                    /* For HostCmd_CMD_802_11_TX_RATE_QUERY, there is a hole in
+                       rate table between HR/DSSS and OFDM rates, so minus 1
+                       for OFDM rate index */
+                    rate->param.rate_cfg.rate =
+                        (pmpriv->tx_rate >
+                         MLAN_RATE_INDEX_OFDM0) ? pmpriv->tx_rate -
+                        1 : pmpriv->tx_rate;
+            } else {
+                rate->param.rate_cfg.rate =
+                    wlan_index_to_data_rate(pmadapter, pmpriv->tx_rate,
+                                            pmpriv->tx_htinfo);
+            }
+        } else if (rate->sub_command == MLAN_OID_GET_DATA_RATE) {
+            if (pmpriv->tx_htinfo & MBIT(0)) {
+                rate->param.data_rate.tx_data_rate =
+                    pmpriv->tx_rate + MLAN_RATE_INDEX_MCS0;
+                if (pmpriv->tx_htinfo & MBIT(1))
+                    rate->param.data_rate.tx_ht_bw = MLAN_HT_BW40;
+                else
+                    rate->param.data_rate.tx_ht_bw = MLAN_HT_BW20;
+                if (pmpriv->tx_htinfo & MBIT(2))
+                    rate->param.data_rate.tx_ht_gi = MLAN_HT_SGI;
+                else
+                    rate->param.data_rate.tx_ht_gi = MLAN_HT_LGI;
+            } else
+                /* For HostCmd_CMD_802_11_TX_RATE_QUERY, there is a hole in
+                   rate table between HR/DSSS and OFDM rates, so minus 1 for
+                   OFDM rate index */
+                rate->param.data_rate.tx_data_rate =
+                    (pmpriv->tx_rate >
+                     MLAN_RATE_INDEX_OFDM0) ? pmpriv->tx_rate -
+                    1 : pmpriv->tx_rate;
+            if (pmpriv->rxpd_htinfo & MBIT(0)) {
+                rate->param.data_rate.rx_data_rate =
+                    pmpriv->rxpd_rate + MLAN_RATE_INDEX_MCS0;
+                if (pmpriv->rxpd_htinfo & MBIT(1))
+                    rate->param.data_rate.rx_ht_bw = MLAN_HT_BW40;
+                else
+                    rate->param.data_rate.rx_ht_bw = MLAN_HT_BW20;
+                if (pmpriv->tx_htinfo & MBIT(2))
+                    rate->param.data_rate.rx_ht_gi = MLAN_HT_SGI;
+                else
+                    rate->param.data_rate.rx_ht_gi = MLAN_HT_LGI;
+            } else
+                /* For rate index in RxPD, there is a hole in rate table
+                   between HR/DSSS and OFDM rates, so minus 1 for OFDM rate
+                   index */
+                rate->param.data_rate.rx_data_rate =
+                    (pmpriv->rxpd_rate >
+                     MLAN_RATE_INDEX_OFDM0) ? pmpriv->rxpd_rate -
+                    1 : pmpriv->rxpd_rate;
+        }
+        pioctl_buf->data_read_written =
+            sizeof(mlan_multicast_list) + MLAN_SUB_COMMAND_SIZE;
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of tx_rate_cfg.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_tx_rate_cfg(IN pmlan_private pmpriv,
+                     IN HostCmd_DS_COMMAND * cmd,
+                     IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    HostCmd_DS_TX_RATE_CFG *rate_cfg = &cmd->params.tx_rate_cfg;
+    MrvlRateScope_t *rate_scope;
+    MrvlRateDropPattern_t *rate_drop;
+    t_u16 *pbitmap_rates = (t_u16 *) pdata_buf;
+
+    t_u32 i;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_TX_RATE_CFG);
+
+    rate_cfg->action = wlan_cpu_to_le16(cmd_action);
+    rate_cfg->cfg_index = 0;
+
+    rate_scope = (MrvlRateScope_t *) ((t_u8 *) rate_cfg +
+                                      sizeof(HostCmd_DS_TX_RATE_CFG));
+    rate_scope->type = wlan_cpu_to_le16(TLV_TYPE_RATE_SCOPE);
+    rate_scope->length = wlan_cpu_to_le16(sizeof(MrvlRateScope_t) -
+                                          sizeof(MrvlIEtypesHeader_t));
+    if (pbitmap_rates != MNULL) {
+        rate_scope->hr_dsss_rate_bitmap = wlan_cpu_to_le16(pbitmap_rates[0]);
+        rate_scope->ofdm_rate_bitmap = wlan_cpu_to_le16(pbitmap_rates[1]);
+        for (i = 0; i < sizeof(rate_scope->ht_mcs_rate_bitmap) / sizeof(t_u16);
+             i++)
+            rate_scope->ht_mcs_rate_bitmap[i] =
+                wlan_cpu_to_le16(pbitmap_rates[2 + i]);
+    } else {
+        rate_scope->hr_dsss_rate_bitmap =
+            wlan_cpu_to_le16(pmpriv->bitmap_rates[0]);
+        rate_scope->ofdm_rate_bitmap =
+            wlan_cpu_to_le16(pmpriv->bitmap_rates[1]);
+        for (i = 0; i < sizeof(rate_scope->ht_mcs_rate_bitmap) / sizeof(t_u16);
+             i++)
+            rate_scope->ht_mcs_rate_bitmap[i] =
+                wlan_cpu_to_le16(pmpriv->bitmap_rates[2 + i]);
+    }
+
+    rate_drop = (MrvlRateDropPattern_t *) ((t_u8 *) rate_scope +
+                                           sizeof(MrvlRateScope_t));
+    rate_drop->type = wlan_cpu_to_le16(TLV_TYPE_RATE_DROP_PATTERN);
+    rate_drop->length = wlan_cpu_to_le16(sizeof(rate_drop->rate_drop_mode));
+    rate_drop->rate_drop_mode = 0;
+
+    cmd->size = wlan_cpu_to_le16(S_DS_GEN + sizeof(HostCmd_DS_TX_RATE_CFG) +
+                                 sizeof(MrvlRateScope_t) +
+                                 sizeof(MrvlRateDropPattern_t));
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of tx_rate_cfg
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ret_tx_rate_cfg(IN pmlan_private pmpriv,
+                     IN HostCmd_DS_COMMAND * resp,
+                     IN mlan_ioctl_req * pioctl_buf)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    mlan_ds_rate *ds_rate = MNULL;
+    HostCmd_DS_TX_RATE_CFG *prate_cfg = MNULL;
+    MrvlRateScope_t *prate_scope;
+    MrvlIEtypesHeader_t *head = MNULL;
+    t_u16 tlv, tlv_buf_len;
+    t_u8 *tlv_buf;
+    t_u32 i;
+    t_s32 index;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    if (resp == MNULL) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    prate_cfg = &resp->params.tx_rate_cfg;
+
+    tlv_buf = (t_u8 *) ((t_u8 *) prate_cfg) + sizeof(HostCmd_DS_TX_RATE_CFG);
+    tlv_buf_len = *(t_u16 *) (tlv_buf + sizeof(t_u16));
+    tlv_buf_len = wlan_le16_to_cpu(tlv_buf_len);
+
+    while (tlv_buf && tlv_buf_len > 0) {
+        tlv = (*tlv_buf);
+        tlv = tlv | (*(tlv_buf + 1) << 8);
+
+        switch (tlv) {
+        case TLV_TYPE_RATE_SCOPE:
+            prate_scope = (MrvlRateScope_t *) tlv_buf;
+            pmpriv->bitmap_rates[0] =
+                wlan_le16_to_cpu(prate_scope->hr_dsss_rate_bitmap);
+            pmpriv->bitmap_rates[1] =
+                wlan_le16_to_cpu(prate_scope->ofdm_rate_bitmap);
+            for (i = 0;
+                 i < sizeof(prate_scope->ht_mcs_rate_bitmap) / sizeof(t_u16);
+                 i++)
+                pmpriv->bitmap_rates[2 + i] =
+                    wlan_le16_to_cpu(prate_scope->ht_mcs_rate_bitmap[i]);
+            break;
+            /* Add RATE_DROP tlv here */
+        }
+
+        head = (MrvlIEtypesHeader_t *) tlv_buf;
+        head->len = wlan_le16_to_cpu(head->len);
+        tlv_buf += head->len + sizeof(MrvlIEtypesHeader_t);
+        tlv_buf_len -= head->len;
+    }
+
+    pmpriv->is_data_rate_auto = wlan_is_rate_auto(pmpriv);
+
+    if (pmpriv->is_data_rate_auto) {
+        pmpriv->data_rate = 0;
+    } else {
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_802_11_TX_RATE_QUERY,
+                               HostCmd_ACT_GEN_GET, 0, MNULL, MNULL);
+
+    }
+
+    if (pioctl_buf) {
+        ds_rate = (mlan_ds_rate *) pioctl_buf->pbuf;
+        if (ds_rate == MNULL) {
+            PRINTM(MERROR, "Request buffer not found!\n");
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+        if (pmpriv->is_data_rate_auto) {
+            ds_rate->param.rate_cfg.is_rate_auto = MTRUE;
+        } else {
+            ds_rate->param.rate_cfg.is_rate_auto = MFALSE;
+            /* check the LG rate */
+            index = wlan_get_rate_index(pmadapter, &pmpriv->bitmap_rates[0], 4);
+            if (index != -1) {
+                if ((index >= MLAN_RATE_BITMAP_OFDM0) &&
+                    (index <= MLAN_RATE_BITMAP_OFDM7))
+                    index -= (MLAN_RATE_BITMAP_OFDM0 - MLAN_RATE_INDEX_OFDM0);
+                ds_rate->param.rate_cfg.rate = index;
+            }
+            /* check the HT rate */
+            index = wlan_get_rate_index(pmadapter,
+                                        &pmpriv->bitmap_rates[2], 16);
+            if (index != -1) {
+                ds_rate->param.rate_cfg.rate = index + MLAN_RATE_INDEX_MCS0;
+            }
+            PRINTM(MINFO, "Rate index is %d\n", ds_rate->param.rate_cfg.rate);
+        }
+        for (i = 0; i < MAX_BITMAP_RATES_SIZE; i++) {
+            ds_rate->param.rate_cfg.bitmap_rates[i] = pmpriv->bitmap_rates[i];
+        }
+
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief  This function issues adapter specific commands
+ *  		to initialize firmware
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *
+ *  @return             MLAN_STATUS_PENDING or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_adapter_init_cmd(IN pmlan_adapter pmadapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = MNULL;
+#ifdef STA_SUPPORT
+    pmlan_private pmpriv_sta = MNULL;
+#endif
+
+    ENTER();
+
+    pmpriv = wlan_get_priv(pmadapter, MLAN_BSS_ROLE_ANY);
+#ifdef STA_SUPPORT
+    pmpriv_sta = wlan_get_priv(pmadapter, MLAN_BSS_ROLE_STA);
+#endif
+
+    /*
+     * This should be issued in the very first to config
+     *   SDIO_GPIO interrupt mode.
+     */
+    if (wlan_set_sdio_gpio_int(pmpriv) != MLAN_STATUS_SUCCESS) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    ret = wlan_prepare_cmd(pmpriv, HostCmd_CMD_FUNC_INIT,
+                           HostCmd_ACT_GEN_SET, 0, MNULL, MNULL);
+    if (ret) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /** Cal data dnld cmd prepare */
+    if ((pmadapter->pcal_data) && (pmadapter->cal_data_len > 0)) {
+        ret =
+            wlan_prepare_cmd(pmpriv, HostCmd_CMD_CFG_DATA, HostCmd_ACT_GEN_SET,
+                             0, MNULL, MNULL);
+        if (ret) {
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+        pmadapter->pcal_data = MNULL;
+        pmadapter->cal_data_len = 0;
+    }
+
+    /*
+     * Get HW spec
+     */
+    ret = wlan_prepare_cmd(pmpriv, HostCmd_CMD_GET_HW_SPEC,
+                           HostCmd_ACT_GEN_GET, 0, MNULL, MNULL);
+    if (ret) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Reconfigure tx buf size */
+    ret = wlan_prepare_cmd(pmpriv, HostCmd_CMD_RECONFIGURE_TX_BUFF,
+                           HostCmd_ACT_GEN_SET, 0, MNULL,
+                           &pmadapter->max_tx_buf_size);
+    if (ret) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+#if defined(STA_SUPPORT)
+    if (pmpriv_sta && (pmpriv_sta->state_11d.user_enable_11d == ENABLE_11D)) {
+        /* Send command to FW to enable 11d */
+        ret = wlan_prepare_cmd(pmpriv_sta,
+                               HostCmd_CMD_802_11_SNMP_MIB,
+                               HostCmd_ACT_GEN_SET,
+                               Dot11D_i,
+                               MNULL, &pmpriv_sta->state_11d.user_enable_11d);
+        if (ret) {
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+    }
+#endif
+
+#if defined(STA_SUPPORT)
+    if (pmpriv_sta && (pmadapter->ps_mode == Wlan802_11PowerModePSP)) {
+        ret = wlan_prepare_cmd(pmpriv_sta, HostCmd_CMD_802_11_PS_MODE_ENH,
+                               EN_AUTO_PS, BITMAP_STA_PS, MNULL, MNULL);
+        if (ret) {
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+    }
+#endif
+
+    if (pmadapter->init_auto_ds) {
+        mlan_ds_auto_ds auto_ds;
+        /* Enable auto deep sleep */
+        auto_ds.idletime = pmadapter->idle_time;
+        ret = wlan_prepare_cmd(pmpriv, HostCmd_CMD_802_11_PS_MODE_ENH,
+                               EN_AUTO_PS, BITMAP_AUTO_DS, MNULL, &auto_ds);
+        if (ret) {
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+    }
+
+    ret = MLAN_STATUS_PENDING;
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function prepares command of get_hw_spec.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pcmd         A pointer to HostCmd_DS_COMMAND structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_get_hw_spec(IN pmlan_private pmpriv, IN HostCmd_DS_COMMAND * pcmd)
+{
+    HostCmd_DS_GET_HW_SPEC *hw_spec = &pcmd->params.hw_spec;
+
+    ENTER();
+
+    pcmd->command = wlan_cpu_to_le16(HostCmd_CMD_GET_HW_SPEC);
+    pcmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_GET_HW_SPEC) + S_DS_GEN);
+    memcpy(pmpriv->adapter, hw_spec->permanent_addr, pmpriv->curr_addr,
+           MLAN_MAC_ADDR_LENGTH);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of set_cfg_data.
+ *
+ *  @param pmpriv       A pointer to mlan_private strcture
+ *  @param pcmd         A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   Command action: GET or SET
+ *  @param pdata_buf    A pointer to cal_data buf
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_cfg_data(IN pmlan_private pmpriv,
+                  IN HostCmd_DS_COMMAND * pcmd,
+                  IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    HostCmd_DS_802_11_CFG_DATA *pcfg_data = &(pcmd->params.cfg_data);
+    pmlan_adapter pmadapter = pmpriv->adapter;
+    t_u32 len;
+    t_u32 cal_data_offset;
+    t_u8 *temp_pcmd = (t_u8 *) pcmd;
+
+    ENTER();
+
+    cal_data_offset = S_DS_GEN + sizeof(HostCmd_DS_802_11_CFG_DATA);
+    if ((pmadapter->pcal_data) && (pmadapter->cal_data_len > 0)) {
+        len = wlan_parse_cal_cfg((t_u8 *) pmadapter->pcal_data,
+                                 pmadapter->cal_data_len,
+                                 (t_u8 *) (temp_pcmd + cal_data_offset));
+    } else {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    pcfg_data->action = cmd_action;
+    pcfg_data->type = 2;        /* cal data type */
+    pcfg_data->data_len = len;
+
+    pcmd->command = HostCmd_CMD_CFG_DATA;
+    pcmd->size = pcfg_data->data_len + cal_data_offset;
+
+    pcmd->command = wlan_cpu_to_le16(pcmd->command);
+    pcmd->size = wlan_cpu_to_le16(pcmd->size);
+
+    pcfg_data->action = wlan_cpu_to_le16(pcfg_data->action);
+    pcfg_data->type = wlan_cpu_to_le16(pcfg_data->type);
+    pcfg_data->data_len = wlan_cpu_to_le16(pcfg_data->data_len);
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function handles the command response of set_cfg_data
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to A pointer to mlan_ioctl_req
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ret_cfg_data(IN pmlan_private pmpriv,
+                  IN HostCmd_DS_COMMAND * resp, IN t_void * pioctl_buf)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    if (resp->result != HostCmd_RESULT_OK) {
+        PRINTM(MERROR, "Cal data cmd resp failed\n");
+        ret = MLAN_STATUS_FAILURE;
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function handles the command response of get_hw_spec
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to command buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ret_get_hw_spec(IN pmlan_private pmpriv,
+                     IN HostCmd_DS_COMMAND * resp, IN t_void * pioctl_buf)
+{
+    HostCmd_DS_GET_HW_SPEC *hw_spec = &resp->params.hw_spec;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u32 i;
+    pmlan_ioctl_req pioctl_req = (mlan_ioctl_req *) pioctl_buf;
+
+    ENTER();
+
+    pmadapter->fw_cap_info = wlan_le32_to_cpu(hw_spec->fw_cap_info);
+#ifdef STA_SUPPORT
+    if (IS_SUPPORT_MULTI_BANDS(pmadapter)) {
+        pmadapter->fw_bands = (t_u8) GET_FW_DEFAULT_BANDS(pmadapter);
+    } else {
+        pmadapter->fw_bands = BAND_B;
+    }
+
+    pmadapter->config_bands = pmadapter->fw_bands;
+    for (i = 0; i < pmadapter->priv_num; i++) {
+        if (pmadapter->priv[i])
+            pmadapter->priv[i]->config_bands = pmadapter->fw_bands;
+    }
+
+    if (pmadapter->fw_bands & BAND_A) {
+        if (pmadapter->fw_bands & BAND_GN) {
+            pmadapter->config_bands |= BAND_AN;
+            for (i = 0; i < pmadapter->priv_num; i++) {
+                if (pmadapter->priv[i])
+                    pmadapter->priv[i]->config_bands |= BAND_AN;
+            }
+
+            pmadapter->fw_bands |= BAND_AN;
+        }
+        if ((pmadapter->fw_bands & BAND_AN)
+            ) {
+            pmadapter->adhoc_start_band = BAND_A | BAND_AN;
+            pmadapter->adhoc_11n_enabled = MTRUE;
+        } else
+            pmadapter->adhoc_start_band = BAND_A;
+        pmpriv->adhoc_channel = DEFAULT_AD_HOC_CHANNEL_A;
+    } else if ((pmadapter->fw_bands & BAND_GN)
+        ) {
+        pmadapter->adhoc_start_band = BAND_G | BAND_B | BAND_GN;
+        pmpriv->adhoc_channel = DEFAULT_AD_HOC_CHANNEL;
+        pmadapter->adhoc_11n_enabled = MTRUE;
+    } else if (pmadapter->fw_bands & BAND_G) {
+        pmadapter->adhoc_start_band = BAND_G | BAND_B;
+        pmpriv->adhoc_channel = DEFAULT_AD_HOC_CHANNEL;
+    } else if (pmadapter->fw_bands & BAND_B) {
+        pmadapter->adhoc_start_band = BAND_B;
+        pmpriv->adhoc_channel = DEFAULT_AD_HOC_CHANNEL;
+    }
+#endif /* STA_SUPPORT */
+
+    pmadapter->fw_release_number = hw_spec->fw_release_number;
+    pmadapter->number_of_antenna = wlan_le16_to_cpu(hw_spec->number_of_antenna);
+
+    PRINTM(MINFO, "GET_HW_SPEC: fw_release_number- 0x%X\n",
+           wlan_le32_to_cpu(pmadapter->fw_release_number));
+    PRINTM(MINFO, "GET_HW_SPEC: Permanent addr- %2x:%2x:%2x:%2x:%2x:%2x\n",
+           hw_spec->permanent_addr[0], hw_spec->permanent_addr[1],
+           hw_spec->permanent_addr[2], hw_spec->permanent_addr[3],
+           hw_spec->permanent_addr[4], hw_spec->permanent_addr[5]);
+    PRINTM(MINFO, "GET_HW_SPEC: hw_if_version=0x%X  version=0x%X\n",
+           wlan_le16_to_cpu(hw_spec->hw_if_version),
+           wlan_le16_to_cpu(hw_spec->version));
+
+    if (pmpriv->curr_addr[0] == 0xff)
+        memmove(pmadapter, pmpriv->curr_addr, hw_spec->permanent_addr,
+                MLAN_MAC_ADDR_LENGTH);
+
+    pmadapter->hw_dot_11n_dev_cap = wlan_le32_to_cpu(hw_spec->dot_11n_dev_cap);
+    pmadapter->usr_dot_11n_dev_cap_bg = pmadapter->hw_dot_11n_dev_cap &
+        DEFAULT_11N_CAP_MASK_BG;
+    pmadapter->usr_dot_11n_dev_cap_a = pmadapter->hw_dot_11n_dev_cap &
+        DEFAULT_11N_CAP_MASK_A;
+    pmadapter->usr_dev_mcs_support = pmadapter->hw_dev_mcs_support =
+        hw_spec->dev_mcs_support;
+    wlan_show_dot11ndevcap(pmadapter, pmadapter->hw_dot_11n_dev_cap);
+    wlan_show_devmcssupport(pmadapter, pmadapter->hw_dev_mcs_support);
+    pmadapter->mp_end_port = wlan_le16_to_cpu(hw_spec->mp_end_port);
+
+    for (i = 1; i <= (unsigned) (MAX_PORT - pmadapter->mp_end_port); i++) {
+        pmadapter->mp_data_port_mask &= ~(1 << (MAX_PORT - i));
+    }
+
+    pmadapter->max_mgmt_ie_index = wlan_le16_to_cpu(hw_spec->mgmt_buf_count);
+    PRINTM(MINFO, "GET_HW_SPEC: mgmt IE count=%d\n",
+           pmadapter->max_mgmt_ie_index);
+    if (!pmadapter->max_mgmt_ie_index)
+        pmadapter->max_mgmt_ie_index = MAX_MGMT_IE_INDEX;
+
+    pmadapter->region_code = wlan_le16_to_cpu(hw_spec->region_code);
+    for (i = 0; i < MRVDRV_MAX_REGION_CODE; i++) {
+        /* Use the region code to search for the index */
+        if (pmadapter->region_code == region_code_index[i])
+            break;
+    }
+    /* If it's unidentified region code, use the default */
+    if (i >= MRVDRV_MAX_REGION_CODE) {
+        pmadapter->region_code = MRVDRV_DEFAULT_REGION_CODE;
+        PRINTM(MWARN, "unidentified region code, use the default (0x%02x)\n",
+               MRVDRV_DEFAULT_REGION_CODE);
+    }
+    /* Synchronize CFP code with region code */
+    pmadapter->cfp_code_bg = pmadapter->region_code;
+    pmadapter->cfp_code_a = pmadapter->region_code;
+
+    if (wlan_set_regiontable(pmpriv, (t_u8) pmadapter->region_code,
+                             pmadapter->fw_bands)) {
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_CMD_SCAN_FAIL;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+#ifdef STA_SUPPORT
+    if (wlan_11d_set_universaltable(pmpriv, pmadapter->fw_bands)) {
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_CMD_SCAN_FAIL;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+#endif /* STA_SUPPORT */
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function prepares command of radio_control.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_802_11_radio_control(IN pmlan_private pmpriv,
+                              IN HostCmd_DS_COMMAND * cmd,
+                              IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    HostCmd_DS_802_11_RADIO_CONTROL *pradio_control = &cmd->params.radio;
+    t_u32 radio_ctl;
+    ENTER();
+    cmd->size = wlan_cpu_to_le16((sizeof(HostCmd_DS_802_11_RADIO_CONTROL))
+                                 + S_DS_GEN);
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_RADIO_CONTROL);
+    pradio_control->action = wlan_cpu_to_le16(cmd_action);
+    memcpy(pmpriv->adapter, &radio_ctl, pdata_buf, sizeof(t_u32));
+    pradio_control->control = wlan_cpu_to_le16((t_u16) radio_ctl);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of radio_control
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_802_11_radio_control(IN pmlan_private pmpriv,
+                              IN HostCmd_DS_COMMAND * resp,
+                              IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_802_11_RADIO_CONTROL *pradio_ctrl =
+        (HostCmd_DS_802_11_RADIO_CONTROL *) & resp->params.radio;
+    mlan_ds_radio_cfg *radio_cfg = MNULL;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+
+    ENTER();
+    pmadapter->radio_on = wlan_le16_to_cpu(pradio_ctrl->control);
+    if (pioctl_buf) {
+        radio_cfg = (mlan_ds_radio_cfg *) pioctl_buf->pbuf;
+        radio_cfg->param.radio_on_off = (t_u32) pmadapter->radio_on;
+        pioctl_buf->data_read_written = sizeof(mlan_ds_radio_cfg);
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+#ifdef WIFI_DIRECT_SUPPORT
+/**
+ *  @brief This function prepares command of remain_on_channel.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_remain_on_channel(IN pmlan_private pmpriv,
+                           IN HostCmd_DS_COMMAND * cmd,
+                           IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    HostCmd_DS_REMAIN_ON_CHANNEL *remain_channel = &cmd->params.remain_on_chan;
+    mlan_ds_remain_chan *cfg = (mlan_ds_remain_chan *) pdata_buf;
+    ENTER();
+    cmd->size = wlan_cpu_to_le16((sizeof(HostCmd_DS_REMAIN_ON_CHANNEL))
+                                 + S_DS_GEN);
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_REMAIN_ON_CHANNEL);
+    remain_channel->action = cmd_action;
+    if (cmd_action == HostCmd_ACT_GEN_SET) {
+        if (cfg->remove) {
+            remain_channel->action = HostCmd_ACT_GEN_REMOVE;
+        } else {
+            remain_channel->bandcfg = cfg->bandcfg;
+            remain_channel->channel = cfg->channel;
+            remain_channel->remain_period =
+                wlan_cpu_to_le32(cfg->remain_period);
+        }
+    }
+    remain_channel->action = wlan_cpu_to_le16(remain_channel->action);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of remain_on_channel
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_remain_on_channel(IN pmlan_private pmpriv,
+                           IN HostCmd_DS_COMMAND * resp,
+                           IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_REMAIN_ON_CHANNEL *remain_channel = &resp->params.remain_on_chan;
+    mlan_ds_radio_cfg *radio_cfg = MNULL;
+
+    ENTER();
+    if (pioctl_buf) {
+        radio_cfg = (mlan_ds_radio_cfg *) pioctl_buf->pbuf;
+        radio_cfg->param.remain_chan.status = remain_channel->status;
+        radio_cfg->param.remain_chan.bandcfg = remain_channel->bandcfg;
+        radio_cfg->param.remain_chan.channel = remain_channel->channel;
+        radio_cfg->param.remain_chan.remain_period =
+            wlan_le32_to_cpu(remain_channel->remain_period);
+        pioctl_buf->data_read_written = sizeof(mlan_ds_radio_cfg);
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of wifi direct mode.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_wifi_direct_mode(IN pmlan_private pmpriv,
+                          IN HostCmd_DS_COMMAND * cmd,
+                          IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    HostCmd_DS_WIFI_DIRECT_MODE *wfd_mode = &cmd->params.wifi_direct_mode;
+    t_u16 mode = *((t_u16 *) pdata_buf);
+    ENTER();
+    cmd->size = wlan_cpu_to_le16((sizeof(HostCmd_DS_WIFI_DIRECT_MODE))
+                                 + S_DS_GEN);
+    cmd->command = wlan_cpu_to_le16(HOST_CMD_WIFI_DIRECT_MODE_CONFIG);
+    wfd_mode->action = wlan_cpu_to_le16(cmd_action);
+    if (cmd_action == HostCmd_ACT_GEN_SET)
+        wfd_mode->mode = wlan_cpu_to_le16(mode);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of wifi direct mode
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_wifi_direct_mode(IN pmlan_private pmpriv,
+                          IN HostCmd_DS_COMMAND * resp,
+                          IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_WIFI_DIRECT_MODE *wfd_mode = &resp->params.wifi_direct_mode;
+    mlan_ds_bss *bss = MNULL;
+
+    ENTER();
+    if (pioctl_buf) {
+        bss = (mlan_ds_bss *) pioctl_buf->pbuf;
+        bss->param.wfd_mode = wlan_le16_to_cpu(wfd_mode->mode);
+        pioctl_buf->data_read_written = sizeof(mlan_ds_bss);
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+#endif
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_decl.h b/drivers/net/wireless/sd8797/mlan/mlan_decl.h
new file mode 100644
index 000000000000..41cf2bf5f2f1
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_decl.h
@@ -0,0 +1,893 @@
+/** @file mlan_decl.h
+ *
+ *  @brief This file declares the generic data structures and APIs.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    11/07/2008: initial version
+******************************************************/
+
+#ifndef _MLAN_DECL_H_
+#define _MLAN_DECL_H_
+
+/** MLAN release version */
+#define MLAN_RELEASE_VERSION		"311"
+
+/** Re-define generic data types for MLAN/MOAL */
+/** Signed char (1-byte) */
+typedef char t_s8;
+/** Unsigned char (1-byte) */
+typedef unsigned char t_u8;
+/** Signed short (2-bytes) */
+typedef short t_s16;
+/** Unsigned short (2-bytes) */
+typedef unsigned short t_u16;
+/** Signed long (4-bytes) */
+typedef int t_s32;
+/** Unsigned long (4-bytes) */
+typedef unsigned int t_u32;
+/** Signed long long 8-bytes) */
+typedef long long t_s64;
+/** Unsigned long long 8-bytes) */
+typedef unsigned long long t_u64;
+/** Void pointer (4-bytes) */
+typedef void t_void;
+/** Size type */
+typedef t_u32 t_size;
+/** Boolean type */
+typedef t_u8 t_bool;
+
+#ifdef MLAN_64BIT
+/** Pointer type (64-bit) */
+typedef t_u64 t_ptr;
+/** Signed value (64-bit) */
+typedef t_s64 t_sval;
+#else
+/** Pointer type (32-bit) */
+typedef t_u32 t_ptr;
+/** Signed value (32-bit) */
+typedef t_s32 t_sval;
+#endif
+
+/** Constants below */
+
+#ifdef __GNUC__
+/** Structure packing begins */
+#define MLAN_PACK_START
+/** Structure packeing end */
+#define MLAN_PACK_END  __attribute__ ((packed))
+#else /* !__GNUC__ */
+#ifdef PRAGMA_PACK
+/** Structure packing begins */
+#define MLAN_PACK_START
+/** Structure packeing end */
+#define MLAN_PACK_END
+#else /* !PRAGMA_PACK */
+/** Structure packing begins */
+#define MLAN_PACK_START   __packed
+/** Structure packing end */
+#define MLAN_PACK_END
+#endif /* PRAGMA_PACK */
+#endif /* __GNUC__ */
+
+#ifndef INLINE
+#ifdef __GNUC__
+/** inline directive */
+#define	INLINE	inline
+#else
+/** inline directive */
+#define	INLINE	__inline
+#endif
+#endif
+
+/** MLAN TRUE */
+#define MTRUE                    (1)
+/** MLAN FALSE */
+#define MFALSE                   (0)
+
+/** Macros for Data Alignment : size */
+#define ALIGN_SZ(p, a)	\
+	(((p) + ((a) - 1)) & ~((a) - 1))
+
+/** Macros for Data Alignment : address */
+#define ALIGN_ADDR(p, a)    \
+    ((((t_ptr)(p)) + (((t_ptr)(a)) - 1)) & ~(((t_ptr)(a)) - 1))
+
+/** Return the byte offset of a field in the given structure */
+#define MLAN_FIELD_OFFSET(type, field) ((t_u32)(t_ptr)&(((type *)0)->field))
+/** Return aligned offset */
+#define OFFSET_ALIGN_ADDR(p, a) (t_u32)(ALIGN_ADDR(p, a) - (t_ptr)p)
+
+/** Maximum BSS numbers */
+#define MLAN_MAX_BSS_NUM         (16)
+
+/** NET IP alignment */
+#define MLAN_NET_IP_ALIGN        0
+
+/** DMA alignment */
+#define DMA_ALIGNMENT            64
+/** max size of TxPD */
+#define MAX_TXPD_SIZE            32
+
+/** Minimum data header length */
+#define MLAN_MIN_DATA_HEADER_LEN (DMA_ALIGNMENT+MAX_TXPD_SIZE)
+
+/** rx data header length */
+#define MLAN_RX_HEADER_LEN       MLAN_MIN_DATA_HEADER_LEN
+
+/** This is current limit on Maximum Tx AMPDU allowed */
+#define MLAN_MAX_TX_BASTREAM_SUPPORTED     2
+/** This is current limit on Maximum Rx AMPDU allowed */
+#define MLAN_MAX_RX_BASTREAM_SUPPORTED     16
+
+#ifdef STA_SUPPORT
+/** Default Win size attached during ADDBA request */
+#define MLAN_STA_AMPDU_DEF_TXWINSIZE       16
+/** Default Win size attached during ADDBA response */
+#define MLAN_STA_AMPDU_DEF_RXWINSIZE       32
+#endif /* STA_SUPPORT */
+#ifdef UAP_SUPPORT
+/** Default Win size attached during ADDBA request */
+#define MLAN_UAP_AMPDU_DEF_TXWINSIZE       32
+/** Default Win size attached during ADDBA response */
+#define MLAN_UAP_AMPDU_DEF_RXWINSIZE       16
+#endif /* UAP_SUPPORT */
+/** Block ack timeout value */
+#define MLAN_DEFAULT_BLOCK_ACK_TIMEOUT  0xffff
+/** Maximum Tx Win size configured for ADDBA request [10 bits] */
+#define MLAN_AMPDU_MAX_TXWINSIZE        0x3ff
+/** Maximum Rx Win size configured for ADDBA request [10 bits] */
+#define MLAN_AMPDU_MAX_RXWINSIZE        0x3ff
+
+/** Rate index for HR/DSSS 0 */
+#define MLAN_RATE_INDEX_HRDSSS0 0
+/** Rate index for HR/DSSS 3 */
+#define MLAN_RATE_INDEX_HRDSSS3 3
+/** Rate index for OFDM 0 */
+#define MLAN_RATE_INDEX_OFDM0   4
+/** Rate index for OFDM 7 */
+#define MLAN_RATE_INDEX_OFDM7   11
+/** Rate index for MCS 0 */
+#define MLAN_RATE_INDEX_MCS0    12
+/** Rate index for MCS 7 */
+#define MLAN_RATE_INDEX_MCS7    19
+/** Rate index for MCS 9 */
+#define MLAN_RATE_INDEX_MCS9    21
+/** Rate index for MCS15 */
+#define MLAN_RATE_INDEX_MCS15   27
+/** Rate index for MCS 32 */
+#define MLAN_RATE_INDEX_MCS32   44
+/** Rate index for MCS 127 */
+#define MLAN_RATE_INDEX_MCS127  139
+
+/** Rate bitmap for OFDM 0 */
+#define MLAN_RATE_BITMAP_OFDM0  16
+/** Rate bitmap for OFDM 7 */
+#define MLAN_RATE_BITMAP_OFDM7  23
+/** Rate bitmap for MCS 0 */
+#define MLAN_RATE_BITMAP_MCS0   32
+/** Rate bitmap for MCS 127 */
+#define MLAN_RATE_BITMAP_MCS127 159
+
+/** Size of rx data buffer */
+#define MLAN_RX_DATA_BUF_SIZE     (4 * 1024)
+/** Size of rx command buffer */
+#define MLAN_RX_CMD_BUF_SIZE      (2 * 1024)
+
+/** MLAN MAC Address Length */
+#define MLAN_MAC_ADDR_LENGTH     (6)
+/** MLAN 802.11 MAC Address */
+typedef t_u8 mlan_802_11_mac_addr[MLAN_MAC_ADDR_LENGTH];
+
+/** MLAN Maximum SSID Length */
+#define MLAN_MAX_SSID_LENGTH     (32)
+
+/** RTS/FRAG related defines */
+/** Minimum RTS value */
+#define MLAN_RTS_MIN_VALUE              (0)
+/** Maximum RTS value */
+#define MLAN_RTS_MAX_VALUE              (2347)
+/** Minimum FRAG value */
+#define MLAN_FRAG_MIN_VALUE             (256)
+/** Maximum FRAG value */
+#define MLAN_FRAG_MAX_VALUE             (2346)
+
+/** Minimum tx retry count */
+#define MLAN_TX_RETRY_MIN 		(0)
+/** Maximum tx retry count */
+#define MLAN_TX_RETRY_MAX 		(14)
+
+/** define SDIO block size for data Tx/Rx */
+/* We support up to 480-byte block size due to FW buffer limitation. */
+#define MLAN_SDIO_BLOCK_SIZE		256
+
+/** define SDIO block size for firmware download */
+#define MLAN_SDIO_BLOCK_SIZE_FW_DNLD	MLAN_SDIO_BLOCK_SIZE
+
+/** define allocated buffer size */
+#define ALLOC_BUF_SIZE           (4 * 1024)
+
+/** SDIO IO Port mask */
+#define MLAN_SDIO_IO_PORT_MASK		0xfffff
+/** SDIO Block/Byte mode mask */
+#define MLAN_SDIO_BYTE_MODE_MASK	0x80000000
+
+/** Max retry number of IO write */
+#define MAX_WRITE_IOMEM_RETRY		2
+
+/** IN parameter */
+#define IN
+/** OUT parameter */
+#define OUT
+
+/** BIT value */
+#define MBIT(x)    (((t_u32)1) << (x))
+
+/** Buffer flag for requeued packet */
+#define MLAN_BUF_FLAG_REQUEUED_PKT      MBIT(0)
+/** Buffer flag for transmit buf from moal */
+#define MLAN_BUF_FLAG_MOAL_TX_BUF        MBIT(1)
+/** Buffer flag for malloc mlan_buffer */
+#define MLAN_BUF_FLAG_MALLOC_BUF        MBIT(2)
+
+/** Buffer flag for bridge packet */
+#define MLAN_BUF_FLAG_BRIDGE_BUF        MBIT(3)
+
+#ifdef DEBUG_LEVEL1
+/** Debug level bit definition */
+#define	MMSG        MBIT(0)
+#define MFATAL      MBIT(1)
+#define MERROR      MBIT(2)
+#define MDATA       MBIT(3)
+#define MCMND       MBIT(4)
+#define MEVENT      MBIT(5)
+#define MINTR       MBIT(6)
+#define MIOCTL      MBIT(7)
+
+#define MDAT_D      MBIT(16)
+#define MCMD_D      MBIT(17)
+#define MEVT_D      MBIT(18)
+#define MFW_D       MBIT(19)
+#define MIF_D       MBIT(20)
+
+#define MENTRY      MBIT(28)
+#define MWARN       MBIT(29)
+#define MINFO       MBIT(30)
+#define MHEX_DUMP   MBIT(31)
+#endif /* DEBUG_LEVEL1 */
+
+/** Memory allocation type: DMA */
+#define	MLAN_MEM_DMA    MBIT(0)
+
+/** Default memory allocation flag */
+#define MLAN_MEM_DEF    0
+
+/** mlan_status */
+typedef enum _mlan_status
+{
+    MLAN_STATUS_FAILURE = 0xffffffff,
+    MLAN_STATUS_SUCCESS = 0,
+    MLAN_STATUS_PENDING,
+    MLAN_STATUS_RESOURCE,
+} mlan_status;
+
+/** mlan_error_code */
+typedef enum _mlan_error_code
+{
+    /** No error */
+    MLAN_ERROR_NO_ERROR = 0,
+    /** Firmware/device errors below (MSB=0) */
+    MLAN_ERROR_FW_NOT_READY = 0x00000001,
+    MLAN_ERROR_FW_BUSY,
+    MLAN_ERROR_FW_CMDRESP,
+    MLAN_ERROR_DATA_TX_FAIL,
+    MLAN_ERROR_DATA_RX_FAIL,
+    /** Driver errors below (MSB=1) */
+    MLAN_ERROR_PKT_SIZE_INVALID = 0x80000001,
+    MLAN_ERROR_PKT_TIMEOUT,
+    MLAN_ERROR_PKT_INVALID,
+    MLAN_ERROR_CMD_INVALID,
+    MLAN_ERROR_CMD_TIMEOUT,
+    MLAN_ERROR_CMD_DNLD_FAIL,
+    MLAN_ERROR_CMD_CANCEL,
+    MLAN_ERROR_CMD_RESP_FAIL,
+    MLAN_ERROR_CMD_ASSOC_FAIL,
+    MLAN_ERROR_CMD_SCAN_FAIL,
+    MLAN_ERROR_IOCTL_INVALID,
+    MLAN_ERROR_IOCTL_FAIL,
+    MLAN_ERROR_EVENT_UNKNOWN,
+    MLAN_ERROR_INVALID_PARAMETER,
+    MLAN_ERROR_NO_MEM,
+    /** More to add */
+} mlan_error_code;
+
+/** mlan_buf_type */
+typedef enum _mlan_buf_type
+{
+    MLAN_BUF_TYPE_CMD = 1,
+    MLAN_BUF_TYPE_DATA,
+    MLAN_BUF_TYPE_EVENT,
+    MLAN_BUF_TYPE_RAW_DATA,
+} mlan_buf_type;
+
+/** MLAN BSS type */
+typedef enum _mlan_bss_type
+{
+    MLAN_BSS_TYPE_STA = 0,
+    MLAN_BSS_TYPE_UAP = 1,
+#ifdef WIFI_DIRECT_SUPPORT
+    MLAN_BSS_TYPE_WIFIDIRECT = 2,
+#endif
+    MLAN_BSS_TYPE_ANY = 0xff,
+} mlan_bss_type;
+
+/** MLAN BSS role */
+typedef enum _mlan_bss_role
+{
+    MLAN_BSS_ROLE_STA = 0,
+    MLAN_BSS_ROLE_UAP = 1,
+    MLAN_BSS_ROLE_ANY = 0xff,
+} mlan_bss_role;
+
+/** BSS role bit mask */
+#define BSS_ROLE_BIT_MASK    MBIT(0)
+
+/** Get BSS role */
+#define GET_BSS_ROLE(priv)   ((priv)->bss_role & BSS_ROLE_BIT_MASK)
+
+/** mlan_data_frame_type */
+typedef enum _mlan_data_frame_type
+{
+    MLAN_DATA_FRAME_TYPE_ETH_II = 0,
+    MLAN_DATA_FRAME_TYPE_802_11,
+} mlan_data_frame_type;
+
+/** mlan_event_id */
+typedef enum _mlan_event_id
+{
+    /* Event generated by firmware (MSB=0) */
+    MLAN_EVENT_ID_FW_UNKNOWN = 0x00000001,
+    MLAN_EVENT_ID_FW_ADHOC_LINK_SENSED,
+    MLAN_EVENT_ID_FW_ADHOC_LINK_LOST,
+    MLAN_EVENT_ID_FW_DISCONNECTED,
+    MLAN_EVENT_ID_FW_MIC_ERR_UNI,
+    MLAN_EVENT_ID_FW_MIC_ERR_MUL,
+    MLAN_EVENT_ID_FW_BCN_RSSI_LOW,
+    MLAN_EVENT_ID_FW_BCN_RSSI_HIGH,
+    MLAN_EVENT_ID_FW_BCN_SNR_LOW,
+    MLAN_EVENT_ID_FW_BCN_SNR_HIGH,
+    MLAN_EVENT_ID_FW_MAX_FAIL,
+    MLAN_EVENT_ID_FW_DATA_RSSI_LOW,
+    MLAN_EVENT_ID_FW_DATA_RSSI_HIGH,
+    MLAN_EVENT_ID_FW_DATA_SNR_LOW,
+    MLAN_EVENT_ID_FW_DATA_SNR_HIGH,
+    MLAN_EVENT_ID_FW_LINK_QUALITY,
+    MLAN_EVENT_ID_FW_PORT_RELEASE,
+    MLAN_EVENT_ID_FW_PRE_BCN_LOST,
+    MLAN_EVENT_ID_FW_WMM_CONFIG_CHANGE,
+    MLAN_EVENT_ID_FW_HS_WAKEUP,
+    MLAN_EVENT_ID_FW_BG_SCAN,
+    MLAN_EVENT_ID_FW_WEP_ICV_ERR,
+    MLAN_EVENT_ID_FW_STOP_TX,
+    MLAN_EVENT_ID_FW_START_TX,
+    MLAN_EVENT_ID_FW_CHANNEL_SWITCH_ANN,
+    MLAN_EVENT_ID_FW_RADAR_DETECTED,
+    MLAN_EVENT_ID_FW_CHANNEL_REPORT_RDY,
+    MLAN_EVENT_ID_FW_BW_CHANGED,
+#ifdef WIFI_DIRECT_SUPPORT
+    MLAN_EVENT_ID_FW_REMAIN_ON_CHAN_EXPIRED,
+#endif
+#ifdef UAP_SUPPORT
+    MLAN_EVENT_ID_UAP_FW_BSS_START,
+    MLAN_EVENT_ID_UAP_FW_BSS_ACTIVE,
+    MLAN_EVENT_ID_UAP_FW_BSS_IDLE,
+    MLAN_EVENT_ID_UAP_FW_STA_CONNECT,
+    MLAN_EVENT_ID_UAP_FW_STA_DISCONNECT,
+#endif
+
+    /* Event generated by MLAN driver (MSB=1) */
+    MLAN_EVENT_ID_DRV_CONNECTED = 0x80000001,
+    MLAN_EVENT_ID_DRV_DEFER_HANDLING,
+    MLAN_EVENT_ID_DRV_HS_ACTIVATED,
+    MLAN_EVENT_ID_DRV_HS_DEACTIVATED,
+    MLAN_EVENT_ID_DRV_MGMT_FRAME,
+    MLAN_EVENT_ID_DRV_OBSS_SCAN_PARAM,
+    MLAN_EVENT_ID_DRV_PASSTHRU,
+    MLAN_EVENT_ID_DRV_SCAN_REPORT,
+    MLAN_EVENT_ID_DRV_MEAS_REPORT,
+    MLAN_EVENT_ID_DRV_REPORT_STRING,
+    MLAN_EVENT_ID_DRV_DBG_DUMP,
+} mlan_event_id;
+
+/** Data Structures */
+/** mlan_image data structure */
+typedef struct _mlan_fw_image
+{
+    /** Helper image buffer pointer */
+    t_u8 *phelper_buf;
+    /** Helper image length */
+    t_u32 helper_len;
+    /** Firmware image buffer pointer */
+    t_u8 *pfw_buf;
+    /** Firmware image length */
+    t_u32 fw_len;
+} mlan_fw_image, *pmlan_fw_image;
+
+/** Custom data structure */
+typedef struct _mlan_init_param
+{
+    /** Cal data buffer pointer */
+    t_u8 *pcal_data_buf;
+    /** Cal data length */
+    t_u32 cal_data_len;
+    /** Other custom data */
+} mlan_init_param, *pmlan_init_param;
+
+/** mlan_event data structure */
+typedef struct _mlan_event
+{
+    /** BSS index number for multiple BSS support */
+    t_u32 bss_index;
+    /** Event ID */
+    mlan_event_id event_id;
+    /** Event length */
+    t_u32 event_len;
+    /** Event buffer */
+    t_u8 event_buf[1];
+} mlan_event, *pmlan_event;
+
+/** mlan_event_scan_result data structure */
+typedef MLAN_PACK_START struct _mlan_event_scan_result
+{
+    /** Event ID */
+    t_u16 event_id;
+    /** BSS index number for multiple BSS support */
+    t_u8 bss_index;
+    /** BSS type */
+    t_u8 bss_type;
+    /** More event available or not */
+    t_u8 more_event;
+    /** Reserved */
+    t_u8 reserved[3];
+    /** Size of the response buffer */
+    t_u16 buf_size;
+    /** Number of BSS in scan response */
+    t_u8 num_of_set;
+} MLAN_PACK_END mlan_event_scan_result, *pmlan_event_scan_result;
+
+/** mlan_ioctl_req data structure */
+typedef struct _mlan_ioctl_req
+{
+    /** Status code from firmware/driver */
+    t_u32 status_code;
+    /** BSS index number for multiple BSS support */
+    t_u32 bss_index;
+    /** Request id */
+    t_u32 req_id;
+    /** Action: set or get */
+    t_u32 action;
+    /** Pointer to buffer */
+    t_u8 *pbuf;
+    /** Length of buffer */
+    t_u32 buf_len;
+    /** Length of the data read/written in buffer */
+    t_u32 data_read_written;
+    /** Length of buffer needed */
+    t_u32 buf_len_needed;
+    /** Reserved for MOAL module */
+    t_ptr reserved_1;
+} mlan_ioctl_req, *pmlan_ioctl_req;
+
+/** mlan_buffer data structure */
+typedef struct _mlan_buffer
+{
+    /** Pointer to previous mlan_buffer */
+    struct _mlan_buffer *pprev;
+    /** Pointer to next mlan_buffer */
+    struct _mlan_buffer *pnext;
+    /** Status code from firmware/driver */
+    t_u32 status_code;
+    /** Flags for this buffer */
+    t_u32 flags;
+    /** BSS index number for multiple BSS support */
+    t_u32 bss_index;
+    /** Buffer descriptor, e.g. skb in Linux */
+    t_void *pdesc;
+    /** Pointer to buffer */
+    t_u8 *pbuf;
+    /** Offset to data */
+    t_u32 data_offset;
+    /** Data length */
+    t_u32 data_len;
+    /** Buffer type: data, cmd, event etc. */
+    mlan_buf_type buf_type;
+
+    /** Fields below are valid for data packet only */
+    /** QoS priority */
+    t_u32 priority;
+    /** Time stamp when packet is received (seconds) */
+    t_u32 in_ts_sec;
+    /** Time stamp when packet is received (micro seconds) */
+    t_u32 in_ts_usec;
+    /** Time stamp when packet is processed (seconds) */
+    t_u32 out_ts_sec;
+    /** Time stamp when packet is processed (micro seconds) */
+    t_u32 out_ts_usec;
+
+    /** Fields below are valid for MLAN module only */
+    /** Pointer to parent mlan_buffer */
+    struct _mlan_buffer *pparent;
+    /** Use count for this buffer */
+    t_u32 use_count;
+} mlan_buffer, *pmlan_buffer;
+
+/** mlan_bss_attr data structure */
+typedef struct _mlan_bss_attr
+{
+    /** BSS type */
+    t_u32 bss_type;
+    /** Data frame type: Ethernet II, 802.11, etc. */
+    t_u32 frame_type;
+    /** The BSS is active (non-0) or not (0). */
+    t_u32 active;
+    /** BSS Priority */
+    t_u32 bss_priority;
+    /** BSS number */
+    t_u32 bss_num;
+} mlan_bss_attr, *pmlan_bss_attr;
+
+#ifdef PRAGMA_PACK
+#pragma pack(push, 1)
+#endif
+
+/** Type enumeration for the command result */
+typedef MLAN_PACK_START enum _mlan_cmd_result_e
+{
+    MLAN_CMD_RESULT_SUCCESS = 0,
+    MLAN_CMD_RESULT_FAILURE = 1,
+    MLAN_CMD_RESULT_TIMEOUT = 2,
+    MLAN_CMD_RESULT_INVALID_DATA = 3
+} MLAN_PACK_END mlan_cmd_result_e;
+
+/** Type enumeration of WMM AC_QUEUES */
+typedef MLAN_PACK_START enum _mlan_wmm_ac_e
+{
+    WMM_AC_BK,
+    WMM_AC_BE,
+    WMM_AC_VI,
+    WMM_AC_VO
+} MLAN_PACK_END mlan_wmm_ac_e;
+
+/** Type enumeration for the action field in the Queue Config command */
+typedef MLAN_PACK_START enum _mlan_wmm_queue_config_action_e
+{
+    MLAN_WMM_QUEUE_CONFIG_ACTION_GET = 0,
+    MLAN_WMM_QUEUE_CONFIG_ACTION_SET = 1,
+    MLAN_WMM_QUEUE_CONFIG_ACTION_DEFAULT = 2,
+    MLAN_WMM_QUEUE_CONFIG_ACTION_MAX
+} MLAN_PACK_END mlan_wmm_queue_config_action_e;
+
+/** Type enumeration for the action field in the queue stats command */
+typedef MLAN_PACK_START enum _mlan_wmm_queue_stats_action_e
+{
+    MLAN_WMM_STATS_ACTION_START = 0,
+    MLAN_WMM_STATS_ACTION_STOP = 1,
+    MLAN_WMM_STATS_ACTION_GET_CLR = 2,
+    MLAN_WMM_STATS_ACTION_SET_CFG = 3,  /* Not currently used */
+    MLAN_WMM_STATS_ACTION_GET_CFG = 4,  /* Not currently used */
+    MLAN_WMM_STATS_ACTION_MAX
+} MLAN_PACK_END mlan_wmm_queue_stats_action_e;
+
+/**
+ *  @brief IOCTL structure for a Traffic stream status.
+ *
+ */
+typedef MLAN_PACK_START struct
+{
+    /** TSID: Range: 0->7 */
+    t_u8 tid;
+    /** TSID specified is valid */
+    t_u8 valid;
+    /** AC TSID is active on */
+    t_u8 access_category;
+    /** UP specified for the TSID */
+    t_u8 user_priority;
+    /** Power save mode for TSID: 0 (legacy), 1 (UAPSD) */
+    t_u8 psb;
+    /** Upstream(0), Downlink(1), Bidirectional(3) */
+    t_u8 flow_dir;
+    /** Medium time granted for the TSID */
+    t_u16 medium_time;
+} MLAN_PACK_END wlan_ioctl_wmm_ts_status_t,
+/** Type definition of mlan_ds_wmm_ts_status for MLAN_OID_WMM_CFG_TS_STATUS */
+  mlan_ds_wmm_ts_status, *pmlan_ds_wmm_ts_status;
+
+/** Max Ie length */
+#define MAX_IE_SIZE             256
+
+/** custom IE */
+typedef MLAN_PACK_START struct _custom_ie
+{
+    /** IE Index */
+    t_u16 ie_index;
+    /** Mgmt Subtype Mask */
+    t_u16 mgmt_subtype_mask;
+    /** IE Length */
+    t_u16 ie_length;
+    /** IE buffer */
+    t_u8 ie_buffer[MAX_IE_SIZE];
+} MLAN_PACK_END custom_ie;
+
+/** Max IE index to FW */
+#define MAX_MGMT_IE_INDEX_TO_FW         4
+/** Max IE index per BSS */
+#define MAX_MGMT_IE_INDEX               16
+
+/** custom IE info */
+typedef MLAN_PACK_START struct _custom_ie_info
+{
+    /** size of buffer */
+    t_u16 buf_size;
+    /** no of buffers of buf_size */
+    t_u16 buf_count;
+} MLAN_PACK_END custom_ie_info;
+
+/** TLV buffer : Max Mgmt IE */
+typedef MLAN_PACK_START struct _tlvbuf_max_mgmt_ie
+{
+    /** Type */
+    t_u16 type;
+    /** Length */
+    t_u16 len;
+    /** No of tuples */
+    t_u16 count;
+    /** custom IE info tuples */
+    custom_ie_info info[MAX_MGMT_IE_INDEX];
+} MLAN_PACK_END tlvbuf_max_mgmt_ie;
+
+/** TLV buffer : custom IE */
+typedef MLAN_PACK_START struct _tlvbuf_custom_ie
+{
+    /** Type */
+    t_u16 type;
+    /** Length */
+    t_u16 len;
+    /** IE data */
+    custom_ie ie_data_list[MAX_MGMT_IE_INDEX_TO_FW];
+    /** Max mgmt IE TLV */
+    tlvbuf_max_mgmt_ie max_mgmt_ie;
+} MLAN_PACK_END mlan_ds_misc_custom_ie;
+
+#ifdef PRAGMA_PACK
+#pragma pack(pop)
+#endif
+
+/** mlan_callbacks data structure */
+typedef struct _mlan_callbacks
+{
+    /** moal_get_fw_data */
+    mlan_status(*moal_get_fw_data) (IN t_void * pmoal_handle,
+                                    IN t_u32 offset,
+                                    IN t_u32 len, OUT t_u8 * pbuf);
+    /** moal_init_fw_complete */
+    mlan_status(*moal_init_fw_complete) (IN t_void * pmoal_handle,
+                                         IN mlan_status status);
+    /** moal_shutdown_fw_complete */
+    mlan_status(*moal_shutdown_fw_complete) (IN t_void * pmoal_handle,
+                                             IN mlan_status status);
+    /** moal_send_packet_complete */
+    mlan_status(*moal_send_packet_complete) (IN t_void * pmoal_handle,
+                                             IN pmlan_buffer pmbuf,
+                                             IN mlan_status status);
+    /** moal_recv_complete */
+    mlan_status(*moal_recv_complete) (IN t_void * pmoal_handle,
+                                      IN pmlan_buffer pmbuf,
+                                      IN t_u32 port, IN mlan_status status);
+    /** moal_recv_packet */
+    mlan_status(*moal_recv_packet) (IN t_void * pmoal_handle,
+                                    IN pmlan_buffer pmbuf);
+    /** moal_recv_event */
+    mlan_status(*moal_recv_event) (IN t_void * pmoal_handle,
+                                   IN pmlan_event pmevent);
+    /** moal_ioctl_complete */
+    mlan_status(*moal_ioctl_complete) (IN t_void * pmoal_handle,
+                                       IN pmlan_ioctl_req pioctl_req,
+                                       IN mlan_status status);
+    /** moal_alloc_mlan_buffer */
+    mlan_status(*moal_alloc_mlan_buffer) (IN t_void * pmoal_handle,
+                                          IN t_u32 size,
+                                          OUT pmlan_buffer * pmbuf);
+    /** moal_free_mlan_buffer */
+    mlan_status(*moal_free_mlan_buffer) (IN t_void * pmoal_handle,
+                                         IN pmlan_buffer pmbuf);
+    /** moal_write_reg */
+    mlan_status(*moal_write_reg) (IN t_void * pmoal_handle,
+                                  IN t_u32 reg, IN t_u32 data);
+    /** moal_read_reg */
+    mlan_status(*moal_read_reg) (IN t_void * pmoal_handle,
+                                 IN t_u32 reg, OUT t_u32 * data);
+    /** moal_write_data_sync */
+    mlan_status(*moal_write_data_sync) (IN t_void * pmoal_handle,
+                                        IN pmlan_buffer pmbuf,
+                                        IN t_u32 port, IN t_u32 timeout);
+    /** moal_read_data_sync */
+    mlan_status(*moal_read_data_sync) (IN t_void * pmoal_handle,
+                                       IN OUT pmlan_buffer pmbuf,
+                                       IN t_u32 port, IN t_u32 timeout);
+    /** moal_malloc */
+    mlan_status(*moal_malloc) (IN t_void * pmoal_handle,
+                               IN t_u32 size, IN t_u32 flag, OUT t_u8 ** ppbuf);
+    /** moal_mfree */
+    mlan_status(*moal_mfree) (IN t_void * pmoal_handle, IN t_u8 * pbuf);
+    /** moal_memset */
+    t_void *(*moal_memset) (IN t_void * pmoal_handle,
+                            IN t_void * pmem, IN t_u8 byte, IN t_u32 num);
+    /** moal_memcpy */
+    t_void *(*moal_memcpy) (IN t_void * pmoal_handle,
+                            IN t_void * pdest,
+                            IN const t_void * psrc, IN t_u32 num);
+    /** moal_memmove */
+    t_void *(*moal_memmove) (IN t_void * pmoal_handle,
+                             IN t_void * pdest,
+                             IN const t_void * psrc, IN t_u32 num);
+    /** moal_memcmp */
+      t_s32(*moal_memcmp) (IN t_void * pmoal_handle,
+                           IN const t_void * pmem1,
+                           IN const t_void * pmem2, IN t_u32 num);
+    /** moal_udelay */
+      t_void(*moal_udelay) (IN t_void * pmoal_handle, IN t_u32 udelay);
+    /** moal_get_system_time */
+      mlan_status(*moal_get_system_time) (IN t_void * pmoal_handle,
+                                          OUT t_u32 * psec, OUT t_u32 * pusec);
+    /** moal_init_timer*/
+      mlan_status(*moal_init_timer) (IN t_void * pmoal_handle,
+                                     OUT t_void ** pptimer,
+                                     IN t_void(*callback) (t_void * pcontext),
+                                     IN t_void * pcontext);
+    /** moal_free_timer */
+      mlan_status(*moal_free_timer) (IN t_void * pmoal_handle,
+                                     IN t_void * ptimer);
+    /** moal_start_timer*/
+      mlan_status(*moal_start_timer) (IN t_void * pmoal_handle,
+                                      IN t_void * ptimer,
+                                      IN t_u8 periodic, IN t_u32 msec);
+    /** moal_stop_timer*/
+      mlan_status(*moal_stop_timer) (IN t_void * pmoal_handle,
+                                     IN t_void * ptimer);
+    /** moal_init_lock */
+      mlan_status(*moal_init_lock) (IN t_void * pmoal_handle,
+                                    OUT t_void ** pplock);
+    /** moal_free_lock */
+      mlan_status(*moal_free_lock) (IN t_void * pmoal_handle,
+                                    IN t_void * plock);
+    /** moal_spin_lock */
+      mlan_status(*moal_spin_lock) (IN t_void * pmoal_handle,
+                                    IN t_void * plock);
+    /** moal_spin_unlock */
+      mlan_status(*moal_spin_unlock) (IN t_void * pmoal_handle,
+                                      IN t_void * plock);
+    /** moal_print */
+      t_void(*moal_print) (IN t_void * pmoal_handle,
+                           IN t_u32 level, IN t_s8 * pformat, IN ...);
+    /** moal_print_netintf */
+      t_void(*moal_print_netintf) (IN t_void * pmoal_handle,
+                                   IN t_u32 bss_index, IN t_u32 level);
+    /** moal_assert */
+      t_void(*moal_assert) (IN t_void * pmoal_handle, IN t_u32 cond);
+} mlan_callbacks, *pmlan_callbacks;
+
+/** Interrupt Mode SDIO */
+#define INT_MODE_SDIO       0
+/** Interrupt Mode GPIO */
+#define INT_MODE_GPIO       1
+
+/** Parameter unchanged, use MLAN default setting */
+#define MLAN_INIT_PARA_UNCHANGED     0
+/** Parameter enabled, override MLAN default setting */
+#define MLAN_INIT_PARA_ENABLED       1
+/** Parameter disabled, override MLAN default setting */
+#define MLAN_INIT_PARA_DISABLED      2
+
+/** mlan_device data structure */
+typedef struct _mlan_device
+{
+    /** MOAL Handle */
+    t_void *pmoal_handle;
+    /** BSS Attributes */
+    mlan_bss_attr bss_attr[MLAN_MAX_BSS_NUM];
+    /** Callbacks */
+    mlan_callbacks callbacks;
+#ifdef MFG_CMD_SUPPORT
+    /** MFG mode */
+    t_u32 mfg_mode;
+#endif
+    /** SDIO interrupt mode (0: INT_MODE_SDIO, 1: INT_MODE_GPIO) */
+    t_u32 int_mode;
+    /** GPIO interrupt pin number */
+    t_u32 gpio_pin;
+#ifdef DEBUG_LEVEL1
+    /** Driver debug bit masks */
+    t_u32 drvdbg;
+#endif
+#ifdef SDIO_MULTI_PORT_TX_AGGR
+    /** SDIO MPA Tx */
+    t_u32 mpa_tx_cfg;
+#endif
+#ifdef SDIO_MULTI_PORT_RX_AGGR
+    /** SDIO MPA Rx */
+    t_u32 mpa_rx_cfg;
+#endif
+    /** Auto deep sleep */
+    t_u32 auto_ds;
+    /** IEEE PS mode */
+    t_u32 ps_mode;
+    /** Max Tx buffer size */
+    t_u32 max_tx_buf;
+#if defined(STA_SUPPORT)
+    /** 802.11d configuration */
+    t_u32 cfg_11d;
+#endif
+} mlan_device, *pmlan_device;
+
+/** MLAN API function prototype */
+#define MLAN_API
+
+/** Registration */
+MLAN_API mlan_status mlan_register(IN pmlan_device pmdevice,
+                                   OUT t_void ** ppmlan_adapter);
+
+/** Un-registration */
+MLAN_API mlan_status mlan_unregister(IN t_void * pmlan_adapter);
+
+/** Firmware Downloading */
+MLAN_API mlan_status mlan_dnld_fw(IN t_void * pmlan_adapter,
+                                  IN pmlan_fw_image pmfw);
+
+/** Custom data pass API */
+MLAN_API mlan_status mlan_set_init_param(IN t_void * pmlan_adapter,
+                                         IN pmlan_init_param pparam);
+
+/** Firmware Initialization */
+MLAN_API mlan_status mlan_init_fw(IN t_void * pmlan_adapter);
+
+/** Firmware Shutdown */
+MLAN_API mlan_status mlan_shutdown_fw(IN t_void * pmlan_adapter);
+
+/** Main Process */
+MLAN_API mlan_status mlan_main_process(IN t_void * pmlan_adapter);
+
+/** Packet Transmission */
+MLAN_API mlan_status mlan_send_packet(IN t_void * pmlan_adapter,
+                                      IN pmlan_buffer pmbuf);
+
+/** Packet Reception complete callback */
+MLAN_API mlan_status mlan_recv_packet_complete(IN t_void * pmlan_adapter,
+                                               IN pmlan_buffer pmbuf,
+                                               IN mlan_status status);
+
+/** interrupt handler */
+MLAN_API t_void mlan_interrupt(IN t_void * pmlan_adapter);
+
+/** mlan ioctl */
+MLAN_API mlan_status mlan_ioctl(IN t_void * pmlan_adapter,
+                                IN pmlan_ioctl_req pioctl_req);
+/** mlan select wmm queue */
+MLAN_API t_u8 mlan_select_wmm_queue(IN t_void * pmlan_adapter,
+                                    IN t_u8 bss_num, IN t_u8 tid);
+#endif /* !_MLAN_DECL_H_ */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_fw.h b/drivers/net/wireless/sd8797/mlan/mlan_fw.h
new file mode 100644
index 000000000000..fda2e7285dfd
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_fw.h
@@ -0,0 +1,4672 @@
+/** @file mlan_fw.h
+ *
+ *  @brief This file contains firmware specific defines.
+ *  structures and declares global function prototypes used
+ *  in MLAN module.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    10/27/2008: initial version
+******************************************************/
+
+#ifndef _MLAN_FW_H_
+#define _MLAN_FW_H_
+
+/** Interface header length */
+#define INTF_HEADER_LEN     4
+
+/** Ethernet header */
+typedef struct
+{
+    /** Ethernet header destination address */
+    t_u8 dest_addr[MLAN_MAC_ADDR_LENGTH];
+    /** Ethernet header source address */
+    t_u8 src_addr[MLAN_MAC_ADDR_LENGTH];
+    /** Ethernet header length */
+    t_u16 h803_len;
+
+} Eth803Hdr_t;
+
+/** RFC 1042 header */
+typedef struct
+{
+    /** LLC DSAP */
+    t_u8 llc_dsap;
+    /** LLC SSAP */
+    t_u8 llc_ssap;
+    /** LLC CTRL */
+    t_u8 llc_ctrl;
+    /** SNAP OUI */
+    t_u8 snap_oui[3];
+    /** SNAP type */
+    t_u16 snap_type;
+
+} Rfc1042Hdr_t;
+
+#ifdef PRAGMA_PACK
+#pragma pack(push, 1)
+#endif
+
+/** Rx packet header */
+typedef MLAN_PACK_START struct
+{
+    /** Etherner header */
+    Eth803Hdr_t eth803_hdr;
+    /** RFC 1042 header */
+    Rfc1042Hdr_t rfc1042_hdr;
+
+} MLAN_PACK_END RxPacketHdr_t;
+
+/** Rates supported in band B */
+#define B_SUPPORTED_RATES               5
+/** Rates supported in band G */
+#define G_SUPPORTED_RATES               9
+/** Rates supported in band BG */
+#define BG_SUPPORTED_RATES              13
+
+/** Setup the number of rates passed in the driver/firmware API */
+#define A_SUPPORTED_RATES               9
+
+/** CapInfo Short Slot Time Disabled */
+//#define SHORT_SLOT_TIME_DISABLED(CapInfo) ((IEEEtypes_CapInfo_t)(CapInfo).short_slot_time = 0)
+#define SHORT_SLOT_TIME_DISABLED(CapInfo) (CapInfo &= ~MBIT(10))
+/** CapInfo Short Slot Time Enabled */
+#define SHORT_SLOT_TIME_ENABLED(CapInfo)  (CapInfo |= MBIT(10))
+
+/** Setup the number of rates passed in the driver/firmware API */
+#define HOSTCMD_SUPPORTED_RATES         14
+
+/** Rates supported in band N */
+#define N_SUPPORTED_RATES               3
+#ifdef STA_SUPPORT
+/** All bands (B, G, N) */
+#define ALL_802_11_BANDS           (BAND_A | BAND_B | BAND_G | BAND_GN)
+#else
+/** All bands (B, G, A) */
+#define ALL_802_11_BANDS        (BAND_B | BAND_G | BAND_A)
+#endif /* STA_SUPPORT */
+
+#ifdef STA_SUPPORT
+/** Firmware multiple bands support */
+#define FW_MULTI_BANDS_SUPPORT  (MBIT(8) | MBIT(9) | MBIT(10) | MBIT(11))
+#else
+/** Firmware multiple bands support */
+#define FW_MULTI_BANDS_SUPPORT  (MBIT(8) | MBIT(9) | MBIT(10))
+#endif /* STA_SUPPORT */
+/** Check if multiple bands support is enabled in firmware */
+#define IS_SUPPORT_MULTI_BANDS(_adapter)        \
+        (_adapter->fw_cap_info & FW_MULTI_BANDS_SUPPORT)
+/** Get default bands of the firmware */
+#define GET_FW_DEFAULT_BANDS(_adapter)  \
+        ((_adapter->fw_cap_info >> 8) & ALL_802_11_BANDS)
+
+extern t_u8 SupportedRates_B[B_SUPPORTED_RATES];
+extern t_u8 SupportedRates_G[G_SUPPORTED_RATES];
+extern t_u8 SupportedRates_BG[BG_SUPPORTED_RATES];
+extern t_u8 SupportedRates_A[A_SUPPORTED_RATES];
+extern t_u8 SupportedRates_N[N_SUPPORTED_RATES];
+extern t_u8 AdhocRates_G[G_SUPPORTED_RATES];
+extern t_u8 AdhocRates_B[B_SUPPORTED_RATES];
+extern t_u8 AdhocRates_BG[BG_SUPPORTED_RATES];
+extern t_u8 AdhocRates_A[A_SUPPORTED_RATES];
+
+/** Default auto deep sleep mode */
+#define DEFAULT_AUTO_DS_MODE                  MTRUE
+/** Default power save mode */
+#define DEFAULT_PS_MODE                 Wlan802_11PowerModePSP
+
+/** WEP Key index mask */
+#define HostCmd_WEP_KEY_INDEX_MASK              0x3fff
+
+/** Key information enabled */
+#define KEY_INFO_ENABLED        0x01
+/** KEY_TYPE_ID */
+typedef enum _KEY_TYPE_ID
+{
+    /** Key type : WEP */
+    KEY_TYPE_ID_WEP = 0,
+    /** Key type : TKIP */
+    KEY_TYPE_ID_TKIP,
+    /** Key type : AES */
+    KEY_TYPE_ID_AES,
+    KEY_TYPE_ID_WAPI,
+} KEY_TYPE_ID;
+
+/** Key Info flag for multicast key */
+#define KEY_INFO_MCAST_KEY      0x01
+/** Key Info flag for unicast key */
+#define KEY_INFO_UCAST_KEY      0x02
+
+/** KEY_INFO_WEP*/
+typedef enum _KEY_INFO_WEP
+{
+    KEY_INFO_WEP_MCAST = 0x01,
+    KEY_INFO_WEP_UNICAST = 0x02,
+    KEY_INFO_WEP_ENABLED = 0x04
+} KEY_INFO_WEP;
+
+/** KEY_INFO_TKIP */
+typedef enum _KEY_INFO_TKIP
+{
+    KEY_INFO_TKIP_MCAST = 0x01,
+    KEY_INFO_TKIP_UNICAST = 0x02,
+    KEY_INFO_TKIP_ENABLED = 0x04
+} KEY_INFO_TKIP;
+
+/** KEY_INFO_AES*/
+typedef enum _KEY_INFO_AES
+{
+    KEY_INFO_AES_MCAST = 0x01,
+    KEY_INFO_AES_UNICAST = 0x02,
+    KEY_INFO_AES_ENABLED = 0x04
+} KEY_INFO_AES;
+
+/** WPA AES key length */
+#define WPA_AES_KEY_LEN                 16
+/** WPA TKIP key length */
+#define WPA_TKIP_KEY_LEN                32
+
+/** WAPI key length */
+#define WAPI_KEY_LEN            50
+/** KEY_INFO_WAPI*/
+typedef enum _KEY_INFO_WAPI
+{
+    KEY_INFO_WAPI_MCAST = 0x01,
+    KEY_INFO_WAPI_UNICAST = 0x02,
+    KEY_INFO_WAPI_ENABLED = 0x04
+} KEY_INFO_WAPI;
+
+/** Maximum ethernet frame length sans FCS */
+#define MV_ETH_FRAME_LEN   1514
+
+/** Length of SNAP header */
+#define MRVDRV_SNAP_HEADER_LEN          8
+
+/** The number of times to try when polling for status bits */
+#define MAX_POLL_TRIES			100
+
+/** The number of times to try when waiting for downloaded firmware to
+     become active when multiple interface is present */
+#define MAX_MULTI_INTERFACE_POLL_TRIES  1000
+
+/** The number of times to try when waiting for downloaded firmware to
+     become active. (polling the scratch register). */
+#define MAX_FIRMWARE_POLL_TRIES		100
+
+/** This is for firmware specific length */
+#define EXTRA_LEN	36
+
+/** Buffer size for ethernet Tx packets */
+#define MRVDRV_ETH_TX_PACKET_BUFFER_SIZE \
+	(MV_ETH_FRAME_LEN + sizeof(TxPD) + EXTRA_LEN)
+
+/** Buffer size for ethernet Rx packets */
+#define MRVDRV_ETH_RX_PACKET_BUFFER_SIZE \
+	(MV_ETH_FRAME_LEN + sizeof(RxPD) \
+	 + MRVDRV_SNAP_HEADER_LEN + EXTRA_LEN)
+
+/* Macros in interface module */
+/** Firmware ready */
+#define FIRMWARE_READY          0xfedc
+
+/** Number of firmware blocks to transfer */
+#define FIRMWARE_TRANSFER_NBLOCK	2
+
+/** Enumeration definition*/
+/** WLAN_802_11_PRIVACY_FILTER */
+typedef enum _WLAN_802_11_PRIVACY_FILTER
+{
+    Wlan802_11PrivFilterAcceptAll,
+    Wlan802_11PrivFilter8021xWEP
+} WLAN_802_11_PRIVACY_FILTER;
+
+/** WLAN_802_11_WEP_STATUS */
+typedef enum _WLAN_802_11_WEP_STATUS
+{
+    Wlan802_11WEPEnabled,
+    Wlan802_11WEPDisabled,
+    Wlan802_11WEPKeyAbsent,
+    Wlan802_11WEPNotSupported
+} WLAN_802_11_WEP_STATUS;
+
+/** SNR calculation */
+#define CAL_SNR(RSSI, NF)		((t_s16)((t_s16)(RSSI)-(t_s16)(NF)))
+
+/** 2K buf size */
+#define MLAN_TX_DATA_BUF_SIZE_2K        2048
+
+/** TLV  type ID definition */
+#define PROPRIETARY_TLV_BASE_ID                 0x0100
+
+/** Terminating TLV Type */
+#define MRVL_TERMINATE_TLV_ID                   0xffff
+
+/** TLV type : SSID */
+#define TLV_TYPE_SSID                           0x0000
+/** TLV type : Rates */
+#define TLV_TYPE_RATES                          0x0001
+/** TLV type : PHY FH */
+#define TLV_TYPE_PHY_FH                         0x0002
+/** TLV type : PHY DS */
+#define TLV_TYPE_PHY_DS                         0x0003
+/** TLV type : CF */
+#define TLV_TYPE_CF                             0x0004
+/** TLV type : IBSS */
+#define TLV_TYPE_IBSS                           0x0006
+
+/** TLV type : Domain */
+#define TLV_TYPE_DOMAIN                         0x0007
+
+/** TLV type : Power constraint */
+#define TLV_TYPE_POWER_CONSTRAINT               0x0020
+
+/** TLV type : Power capability */
+#define TLV_TYPE_POWER_CAPABILITY               0x0021
+
+/** TLV type : Vendor Specific IE */
+#define TLV_TYPE_VENDOR_SPECIFIC_IE             0xdd
+
+/** TLV type : Key material */
+#define TLV_TYPE_KEY_MATERIAL       (PROPRIETARY_TLV_BASE_ID + 0x00)    // 0x0100
+/** TLV type : Channel list */
+#define TLV_TYPE_CHANLIST           (PROPRIETARY_TLV_BASE_ID + 0x01)    // 0x0101
+/** TLV type : Number of probes */
+#define TLV_TYPE_NUMPROBES          (PROPRIETARY_TLV_BASE_ID + 0x02)    // 0x0102
+/** TLV type : Beacon RSSI low */
+#define TLV_TYPE_RSSI_LOW           (PROPRIETARY_TLV_BASE_ID + 0x04)    // 0x0104
+/** TLV type : Beacon SNR low */
+#define TLV_TYPE_SNR_LOW            (PROPRIETARY_TLV_BASE_ID + 0x05)    // 0x0105
+/** TLV type : Fail count */
+#define TLV_TYPE_FAILCOUNT          (PROPRIETARY_TLV_BASE_ID + 0x06)    // 0x0106
+/** TLV type : BCN miss */
+#define TLV_TYPE_BCNMISS            (PROPRIETARY_TLV_BASE_ID + 0x07)    // 0x0107
+/** TLV type : LED behavior */
+#define TLV_TYPE_LEDBEHAVIOR        (PROPRIETARY_TLV_BASE_ID + 0x09)    // 0x0109
+/** TLV type : Passthrough */
+#define TLV_TYPE_PASSTHROUGH        (PROPRIETARY_TLV_BASE_ID + 0x0a)    // 0x010a
+/** TLV type : Power TBL 2.4 Ghz */
+#define TLV_TYPE_POWER_TBL_2_4GHZ   (PROPRIETARY_TLV_BASE_ID + 0x0c)    // 0x010c
+/** TLV type : Power TBL 5 GHz */
+#define TLV_TYPE_POWER_TBL_5GHZ     (PROPRIETARY_TLV_BASE_ID + 0x0d)    // 0x010d
+/** TLV type : WMM queue status */
+#define TLV_TYPE_WMMQSTATUS         (PROPRIETARY_TLV_BASE_ID + 0x10)    // 0x0110
+/** TLV type : Wildcard SSID */
+#define TLV_TYPE_WILDCARDSSID       (PROPRIETARY_TLV_BASE_ID + 0x12)    // 0x0112
+/** TLV type : TSF timestamp */
+#define TLV_TYPE_TSFTIMESTAMP       (PROPRIETARY_TLV_BASE_ID + 0x13)    // 0x0113
+/** TLV type : ARP filter */
+#define TLV_TYPE_ARP_FILTER         (PROPRIETARY_TLV_BASE_ID + 0x15)    // 0x0115
+/** TLV type : Beacon RSSI high */
+#define TLV_TYPE_RSSI_HIGH          (PROPRIETARY_TLV_BASE_ID + 0x16)    // 0x0116
+/** TLV type : Beacon SNR high */
+#define TLV_TYPE_SNR_HIGH           (PROPRIETARY_TLV_BASE_ID + 0x17)    // 0x0117
+/** TLV type : Start BG scan later */
+#define TLV_TYPE_STARTBGSCANLATER   (PROPRIETARY_TLV_BASE_ID + 0x1e)    // 0x011e
+/** TLV type: BG scan repeat count */
+#define TLV_TYPE_REPEAT_COUNT       (PROPRIETARY_TLV_BASE_ID + 0xb0)    // 0x01b0
+/** TLV type : Authentication type */
+#define TLV_TYPE_AUTH_TYPE          (PROPRIETARY_TLV_BASE_ID + 0x1f)    // 0x011f
+/** TLV type : BSSID */
+#define TLV_TYPE_BSSID              (PROPRIETARY_TLV_BASE_ID + 0x23)    // 0x0123
+
+/** TLV type : Link Quality */
+#define TLV_TYPE_LINK_QUALITY       (PROPRIETARY_TLV_BASE_ID + 0x24)    // 0x0124
+
+/** TLV type : Data RSSI low */
+#define TLV_TYPE_RSSI_LOW_DATA      (PROPRIETARY_TLV_BASE_ID + 0x26)    // 0x0126
+/** TLV type : Data SNR low */
+#define TLV_TYPE_SNR_LOW_DATA       (PROPRIETARY_TLV_BASE_ID + 0x27)    // 0x0127
+/** TLV type : Data RSSI high */
+#define TLV_TYPE_RSSI_HIGH_DATA     (PROPRIETARY_TLV_BASE_ID + 0x28)    // 0x0128
+/** TLV type : Data SNR high */
+#define TLV_TYPE_SNR_HIGH_DATA      (PROPRIETARY_TLV_BASE_ID + 0x29)    // 0x0129
+
+/** TLV type : Channel band list */
+#define TLV_TYPE_CHANNELBANDLIST    (PROPRIETARY_TLV_BASE_ID + 0x2a)    // 0x012a
+
+/** TLV type : Passphrase */
+#define TLV_TYPE_PASSPHRASE         (PROPRIETARY_TLV_BASE_ID + 0x3c)    // 0x013c
+
+/** TLV type : Encryption Protocol TLV */
+#define TLV_TYPE_ENCRYPTION_PROTO   (PROPRIETARY_TLV_BASE_ID + 0x40)    // 0x0140
+/** TLV type : Cipher TLV */
+#define TLV_TYPE_CIPHER             (PROPRIETARY_TLV_BASE_ID + 0x42)    // 0x0142
+/** TLV type : PMK */
+#define TLV_TYPE_PMK                (PROPRIETARY_TLV_BASE_ID + 0x44)    // 0x0144
+
+/** TLV type : BCN miss */
+#define TLV_TYPE_PRE_BCNMISS         (PROPRIETARY_TLV_BASE_ID + 0x49)   // 0x0149
+
+/** TLV type: WAPI IE */
+#define TLV_TYPE_WAPI_IE             (PROPRIETARY_TLV_BASE_ID + 0x5e)   // 0x015e
+
+/** TLV type: MGMT IE */
+#define TLV_TYPE_MGMT_IE             (PROPRIETARY_TLV_BASE_ID + 0x69)   // 0x0169
+/** TLV type: MAX_MGMT_IE */
+#define TLV_TYPE_MAX_MGMT_IE         (PROPRIETARY_TLV_BASE_ID + 0xaa)   // 0x01aa
+
+/** TLV type : HT Capabilities */
+#define TLV_TYPE_HT_CAP                  (PROPRIETARY_TLV_BASE_ID + 0x4a)       // 0x014a
+/** TLV type : HT Information */
+#define TLV_TYPE_HT_INFO                 (PROPRIETARY_TLV_BASE_ID + 0x4b)       // 0x014b
+/** TLV type : Secondary Channel Offset */
+#define TLV_SECONDARY_CHANNEL_OFFSET     (PROPRIETARY_TLV_BASE_ID + 0x4c)       // 0x014c
+/** TLV type : 20/40 BSS Coexistence */
+#define TLV_TYPE_2040BSS_COEXISTENCE     (PROPRIETARY_TLV_BASE_ID + 0x4d)       // 0x014d
+/** TLV type : Overlapping BSS Scan Parameters */
+#define TLV_TYPE_OVERLAP_BSS_SCAN_PARAM  (PROPRIETARY_TLV_BASE_ID + 0x4e)       // 0x014e
+/** TLV type : Extended capabilities */
+#define TLV_TYPE_EXTCAP                  (PROPRIETARY_TLV_BASE_ID + 0x4f)       // 0x014f
+/** TLV type : Set of MCS values that STA desires to use within the BSS */
+#define TLV_TYPE_HT_OPERATIONAL_MCS_SET  (PROPRIETARY_TLV_BASE_ID + 0x50)       // 0x0150
+/** TLV type : RXBA_SYNC */
+#define TLV_TYPE_RXBA_SYNC               (PROPRIETARY_TLV_BASE_ID + 0x99)       // 0x0199
+/** ADDBA TID mask */
+#define ADDBA_TID_MASK   (MBIT(2) | MBIT(3) | MBIT(4) | MBIT(5))
+/** DELBA TID mask */
+#define DELBA_TID_MASK   (MBIT(12) | MBIT(13) | MBIT(14) | MBIT(15))
+/** ADDBA Starting Sequence Number Mask */
+#define SSN_MASK         0xfff0
+
+/** Block Ack result status */
+/** Block Ack Result : Success */
+#define BA_RESULT_SUCCESS        0x0
+/** Block Ack Result : Execution failure */
+#define BA_RESULT_FAILURE        0x1
+/** Block Ack Result : Timeout */
+#define BA_RESULT_TIMEOUT        0x2
+/** Block Ack Result : Data invalid */
+#define BA_RESULT_DATA_INVALID   0x3
+
+/** Get the baStatus (NOT_SETUP, COMPLETE, IN_PROGRESS)
+ *  in Tx BA stream table */
+#define IS_BASTREAM_SETUP(ptr)  (ptr->ba_status)
+
+/** An AMPDU/AMSDU could be disallowed for certain TID. 0xff means
+ *  no aggregation is enabled for the assigned TID */
+#define BA_STREAM_NOT_ALLOWED   0xff
+
+/** Test if 11n is enabled by checking the HTCap IE */
+#define IS_11N_ENABLED(priv) ((priv->config_bands & BAND_GN ||priv->config_bands & BAND_AN) \
+        && priv->curr_bss_params.bss_descriptor.pht_cap)
+/** Find out if we are the initiator or not */
+#define INITIATOR_BIT(DelBAParamSet) (((DelBAParamSet) & \
+                        MBIT(DELBA_INITIATOR_POS)) >> DELBA_INITIATOR_POS)
+
+/** 4K buf size */
+#define MLAN_TX_DATA_BUF_SIZE_4K        4096
+/** 8K buf size */
+#define MLAN_TX_DATA_BUF_SIZE_8K        8192
+/** Max Rx AMPDU Size */
+#define MAX_RX_AMPDU_SIZE_64K   0x03
+/** Non green field station */
+#define NON_GREENFIELD_STAS     0x04
+
+/** Greenfield support */
+#define HWSPEC_GREENFIELD_SUPP	 MBIT(29)
+/** RX STBC support */
+#define HWSPEC_RXSTBC_SUPP	 MBIT(26)
+/** ShortGI @ 40Mhz support */
+#define HWSPEC_SHORTGI40_SUPP	 MBIT(24)
+/** ShortGI @ 20Mhz support */
+#define HWSPEC_SHORTGI20_SUPP	 MBIT(23)
+/** Channel width 40Mhz support */
+#define HWSPEC_CHANBW40_SUPP	 MBIT(17)
+/** 40Mhz intolarent enable */
+#define CAPINFO_40MHZ_INTOLARENT MBIT(8)
+
+/** Default 11n capability mask for 2.4GHz */
+#define DEFAULT_11N_CAP_MASK_BG (HWSPEC_SHORTGI20_SUPP | HWSPEC_RXSTBC_SUPP)
+/** Default 11n capability mask for 5GHz */
+#define DEFAULT_11N_CAP_MASK_A  (HWSPEC_CHANBW40_SUPP | HWSPEC_SHORTGI20_SUPP | \
+                                 HWSPEC_SHORTGI40_SUPP | HWSPEC_RXSTBC_SUPP)
+/** Bits to ignore in hw_dev_cap as these bits are set in get_hw_spec */
+#define IGN_HW_DEV_CAP		(CAPINFO_40MHZ_INTOLARENT)
+
+/** HW_SPEC FwCapInfo */
+#define ISSUPP_11NENABLED(FwCapInfo) (FwCapInfo & MBIT(11))
+
+/** HW_SPEC Dot11nDevCap : MAX AMSDU supported */
+#define ISSUPP_MAXAMSDU(Dot11nDevCap) (Dot11nDevCap & MBIT(31))
+/** HW_SPEC Dot11nDevCap : Beamforming support */
+#define ISSUPP_BEAMFORMING(Dot11nDevCap) (Dot11nDevCap & MBIT(30))
+/** HW_SPEC Dot11nDevCap : Green field support */
+#define ISSUPP_GREENFIELD(Dot11nDevCap) (Dot11nDevCap & MBIT(29))
+/** HW_SPEC Dot11nDevCap : AMPDU support */
+#define ISSUPP_AMPDU(Dot11nDevCap) (Dot11nDevCap & MBIT(28))
+/** HW_SPEC Dot11nDevCap : MIMO PS support  */
+#define ISSUPP_MIMOPS(Dot11nDevCap) (Dot11nDevCap & MBIT(27))
+/** HW_SPEC Dot11nDevCap : Rx STBC support */
+#define ISSUPP_RXSTBC(Dot11nDevCap) (Dot11nDevCap & MBIT(26))
+/** HW_SPEC Dot11nDevCap : Tx STBC support */
+#define ISSUPP_TXSTBC(Dot11nDevCap) (Dot11nDevCap & MBIT(25))
+/** HW_SPEC Dot11nDevCap : Short GI @ 40Mhz support */
+#define ISSUPP_SHORTGI40(Dot11nDevCap) (Dot11nDevCap & MBIT(24))
+/** HW_SPEC Dot11nDevCap : Short GI @ 20Mhz support */
+#define ISSUPP_SHORTGI20(Dot11nDevCap) (Dot11nDevCap & MBIT(23))
+/** HW_SPEC Dot11nDevCap : Rx LDPC support */
+#define ISSUPP_RXLDPC(Dot11nDevCap) (Dot11nDevCap & MBIT(22))
+/** HW_SPEC Dot11nDevCap : Delayed ACK */
+#define GET_DELAYEDBACK(Dot11nDevCap) (((Dot11nDevCap >> 20) & 0x03))
+/** HW_SPEC Dot11nDevCap : Immediate ACK */
+#define GET_IMMEDIATEBACK(Dot11nDevCap) (((Dot11nDevCap >> 18) & 0x03))
+/** HW_SPEC Dot11nDevCap : Channel BW support @ 40Mhz  support */
+#define ISSUPP_CHANWIDTH40(Dot11nDevCap) (Dot11nDevCap & MBIT(17))
+/** HW_SPEC Dot11nDevCap : Channel BW support @ 20Mhz  support */
+#define ISSUPP_CHANWIDTH20(Dot11nDevCap) (Dot11nDevCap & MBIT(16))
+/** HW_SPEC Dot11nDevCap : Channel BW support @ 10Mhz  support */
+#define ISSUPP_CHANWIDTH10(Dot11nDevCap) (Dot11nDevCap & MBIT(15))
+/** Dot11nUsrCap : 40Mhz intolarance enabled */
+#define ISENABLED_40MHZ_INTOLARENT(Dot11nDevCap) (Dot11nDevCap & MBIT(8))
+/** HW_SPEC Dot11nDevCap : Rx AntennaD support */
+#define ISSUPP_RXANTENNAD(Dot11nDevCap) (Dot11nDevCap & MBIT(7))
+/** HW_SPEC Dot11nDevCap : Rx AntennaC support */
+#define ISSUPP_RXANTENNAC(Dot11nDevCap) (Dot11nDevCap & MBIT(6))
+/** HW_SPEC Dot11nDevCap : Rx AntennaB support */
+#define ISSUPP_RXANTENNAB(Dot11nDevCap) (Dot11nDevCap & MBIT(5))
+/** HW_SPEC Dot11nDevCap : Rx AntennaA support */
+#define ISSUPP_RXANTENNAA(Dot11nDevCap) (Dot11nDevCap & MBIT(4))
+/** HW_SPEC Dot11nDevCap : Tx AntennaD support */
+#define ISSUPP_TXANTENNAD(Dot11nDevCap) (Dot11nDevCap & MBIT(3))
+/** HW_SPEC Dot11nDevCap : Tx AntennaC support */
+#define ISSUPP_TXANTENNAC(Dot11nDevCap) (Dot11nDevCap & MBIT(2))
+/** HW_SPEC Dot11nDevCap : Tx AntennaB support */
+#define ISSUPP_TXANTENNAB(Dot11nDevCap) (Dot11nDevCap & MBIT(1))
+/** HW_SPEC Dot11nDevCap : Tx AntennaA support */
+#define ISSUPP_TXANTENNAA(Dot11nDevCap) (Dot11nDevCap & MBIT(0))
+
+/** HW_SPEC Dot11nDevCap : Set support of channel bw @ 40Mhz */
+#define SETSUPP_CHANWIDTH40(Dot11nDevCap) (Dot11nDevCap |= MBIT(17))
+/** HW_SPEC Dot11nDevCap : Reset support of channel bw @ 40Mhz */
+#define RESETSUPP_CHANWIDTH40(Dot11nDevCap) (Dot11nDevCap &= ~MBIT(17))
+
+/** DevMCSSupported : Tx MCS supported */
+#define GET_TXMCSSUPP(DevMCSSupported) (DevMCSSupported >> 4)
+/** DevMCSSupported : Rx MCS supported */
+#define GET_RXMCSSUPP(DevMCSSupported) (DevMCSSupported & 0x0f)
+
+/** GET HTCapInfo : Supported Channel BW */
+#define GETHT_SUPPCHANWIDTH(HTCapInfo) (HTCapInfo & MBIT(1))
+/** GET HTCapInfo : Support for Greenfield */
+#define GETHT_GREENFIELD(HTCapInfo) (HTCapInfo & MBIT(4))
+/** GET HTCapInfo : Support for Short GI @ 20Mhz */
+#define GETHT_SHORTGI20(HTCapInfo) (HTCapInfo & MBIT(5))
+/** GET HTCapInfo : Support for Short GI @ 40Mhz */
+#define GETHT_SHORTGI40(HTCapInfo) (HTCapInfo & MBIT(6))
+/** GET HTCapInfo : Support for Tx STBC */
+#define GETHT_TXSTBC(HTCapInfo) (HTCapInfo & MBIT(7))
+
+/** GET HTCapInfo : Support for Rx STBC */
+#define GETHT_RXSTBC(HTCapInfo) ((HTCapInfo >> 8) & 0x03)
+/** GET HTCapInfo : Support for Delayed ACK */
+#define GETHT_DELAYEDBACK(HTCapInfo) (HTCapInfo & MBIT(10))
+/** GET HTCapInfo : Support for Max AMSDU */
+#define GETHT_MAXAMSDU(HTCapInfo) (HTCapInfo & MBIT(11))
+
+/** SET HTCapInfo : Set support for LDPC coding capability */
+#define SETHT_LDPCCODINGCAP(HTCapInfo) (HTCapInfo |= MBIT(0))
+/** SET HTCapInfo : Set support for Channel BW */
+#define SETHT_SUPPCHANWIDTH(HTCapInfo) (HTCapInfo |= MBIT(1))
+/** SET HTCapInfo : Set support for Greenfield */
+#define SETHT_GREENFIELD(HTCapInfo) (HTCapInfo |= MBIT(4))
+/** SET HTCapInfo : Set support for Short GI @ 20Mhz */
+#define SETHT_SHORTGI20(HTCapInfo) (HTCapInfo |= MBIT(5))
+/** SET HTCapInfo : Set support for Short GI @ 40Mhz */
+#define SETHT_SHORTGI40(HTCapInfo) (HTCapInfo |= MBIT(6))
+/** SET HTCapInfo : Set support for Tx STBC */
+#define SETHT_TXSTBC(HTCapInfo) (HTCapInfo |= MBIT(7))
+/** SET HTCapInfo : Set support for Rx STBC */
+#define SETHT_RXSTBC(HTCapInfo, value) (HTCapInfo |= (value << 8))
+/** SET HTCapInfo : Set support for delayed block ack */
+#define SETHT_DELAYEDBACK(HTCapInfo) (HTCapInfo |= MBIT(10))
+/** SET HTCapInfo : Set support for Max size AMSDU */
+#define SETHT_MAXAMSDU(HTCapInfo) (HTCapInfo |= MBIT(11))
+/** SET HTCapInfo : Set support for DSSS/CCK Rates @ 40Mhz */
+#define SETHT_DSSSCCK40(HTCapInfo) (HTCapInfo |= MBIT(12))
+/** SET HTCapInfo : Enable 40Mhz Intolarence */
+#define SETHT_40MHZ_INTOLARANT(HTCapInfo) (HTCapInfo |= MBIT(14))
+
+/** RESET HTCapInfo : Set support for LDPC coding capability */
+#define RESETHT_LDPCCODINGCAP(HTCapInfo) (HTCapInfo &= ~MBIT(0))
+/** RESET HTCapInfo : Set support for Channel BW */
+#define RESETHT_SUPPCHANWIDTH(HTCapInfo) (HTCapInfo &= ~MBIT(1))
+/** RESET HTCapInfo : Set support for Greenfield */
+#define RESETHT_GREENFIELD(HTCapInfo) (HTCapInfo &= ~MBIT(4))
+/** RESET HTCapInfo : Set support for Short GI @ 20Mhz */
+#define RESETHT_SHORTGI20(HTCapInfo) (HTCapInfo &= ~MBIT(5))
+/** RESET HTCapInfo : Set support for Short GI @ 40Mhz */
+#define RESETHT_SHORTGI40(HTCapInfo) (HTCapInfo &= ~MBIT(6))
+/** RESET HTCapInfo : Set support for Tx STBC */
+#define RESETHT_TXSTBC(HTCapInfo) (HTCapInfo &= ~MBIT(7))
+/** RESET HTCapInfo : Set support for Rx STBC */
+#define RESETHT_RXSTBC(HTCapInfo) (HTCapInfo &= ~(0x03 << 8))
+/** RESET HTCapInfo : Set support for delayed block ack */
+#define RESETHT_DELAYEDBACK(HTCapInfo) (HTCapInfo &= ~MBIT(10))
+/** RESET HTCapInfo : Set support for Max size AMSDU */
+#define RESETHT_MAXAMSDU(HTCapInfo) (HTCapInfo &= ~MBIT(11))
+/** RESET HTCapInfo : Disable 40Mhz Intolarence */
+#define RESETHT_40MHZ_INTOLARANT(HTCapInfo) (HTCapInfo &= ~MBIT(14))
+/** RESET HTExtCap : Clear RD Responder bit */
+#define RESETHT_EXTCAP_RDG(HTExtCap) (HTExtCap &= ~MBIT(11))
+/** SET MCS32 */
+#define SETHT_MCS32(x) (x[4] |= 1)
+/** Set mcs set defined bit */
+#define SETHT_MCS_SET_DEFINED(x) (x[12] |= 1)
+/** Set the highest Rx data rate */
+#define SETHT_RX_HIGHEST_DT_SUPP(x, y) ((*(t_u16 *) (x + 10)) = y)
+/** AMPDU factor size */
+#define AMPDU_FACTOR_64K	0x03
+/** Set AMPDU size in A-MPDU paramter field */
+#define SETAMPDU_SIZE(x, y) do { \
+	x = x & ~0x03; \
+	x |= y & 0x03; \
+} while (0) \
+/** Set AMPDU spacing in A-MPDU paramter field */
+#define SETAMPDU_SPACING(x, y) do { \
+	x = x & ~0x1c; \
+	x |= (y & 0x07) << 2; \
+} while (0) \
+
+/** RadioType : Support for Band A */
+#define ISSUPP_BANDA(FwCapInfo) (FwCapInfo & MBIT(10))
+/** RadioType : Support for 40Mhz channel BW */
+#define ISALLOWED_CHANWIDTH40(Field2) (Field2 & MBIT(2))
+/** RadioType : Set support 40Mhz channel */
+#define SET_CHANWIDTH40(Field2) (Field2 |= MBIT(2))
+/** RadioType : Reset support 40Mhz channel */
+#define RESET_CHANWIDTH40(Field2) (Field2 &= ~(MBIT(0) | MBIT(1) | MBIT(2)))
+/** RadioType : Get secondary channel */
+#define GET_SECONDARYCHAN(Field2) (Field2 & (MBIT(0) | MBIT(1)))
+/** RadioType : Set secondary channel */
+#define SET_SECONDARYCHAN(RadioType, SECCHAN) (RadioType |= (SECCHAN << 4))
+
+/** LLC/SNAP header len   */
+#define LLC_SNAP_LEN    8
+
+/** TLV type : Rate scope */
+#define TLV_TYPE_RATE_DROP_PATTERN  (PROPRIETARY_TLV_BASE_ID + 0x51)    // 0x0151
+/** TLV type : Rate drop pattern */
+#define TLV_TYPE_RATE_DROP_CONTROL  (PROPRIETARY_TLV_BASE_ID + 0x52)    // 0x0152
+/** TLV type : Rate scope */
+#define TLV_TYPE_RATE_SCOPE         (PROPRIETARY_TLV_BASE_ID + 0x53)    // 0x0153
+
+/** TLV type : Power group */
+#define TLV_TYPE_POWER_GROUP        (PROPRIETARY_TLV_BASE_ID + 0x54)    // 0x0154
+
+/** Modulation class for DSSS Rates */
+#define MOD_CLASS_HR_DSSS       0x03
+/** Modulation class for OFDM Rates */
+#define MOD_CLASS_OFDM          0x07
+/** Modulation class for HT Rates */
+#define MOD_CLASS_HT            0x08
+/** HT bandwidth 20 MHz */
+#define HT_BW_20    0
+/** HT bandwidth 40 MHz */
+#define HT_BW_40    1
+
+/** TLV type : Scan Response */
+#define TLV_TYPE_BSS_SCAN_RSP        (PROPRIETARY_TLV_BASE_ID + 0x56)   // 0x0156
+/** TLV type : Scan Response Stats */
+#define TLV_TYPE_BSS_SCAN_INFO       (PROPRIETARY_TLV_BASE_ID + 0x57)   // 0x0157
+
+/** TLV type : 11h Basic Rpt */
+#define TLV_TYPE_CHANRPT_11H_BASIC   (PROPRIETARY_TLV_BASE_ID + 0x5b)   // 0x015b
+
+/** TLV type : Action frame */
+#define TLV_TYPE_IEEE_ACTION_FRAME   (PROPRIETARY_TLV_BASE_ID + 0x8c)   // 0x018c
+
+/** Firmware Host Command ID Constants */
+/** Host Command ID : Get hardware specifications */
+#define HostCmd_CMD_GET_HW_SPEC               0x0003
+/** Host Command ID : 802.11 scan */
+#define HostCmd_CMD_802_11_SCAN               0x0006
+/** Host Command ID : 802.11 get log */
+#define HostCmd_CMD_802_11_GET_LOG            0x000b
+/** Host Command ID : MAC multicast address */
+#define HostCmd_CMD_MAC_MULTICAST_ADR         0x0010
+/** Host Command ID : 802.11 EEPROM access */
+#define HostCmd_CMD_802_11_EEPROM_ACCESS      0x0059
+/** Host Command ID : 802.11 associate */
+#define HostCmd_CMD_802_11_ASSOCIATE          0x0012
+
+/** Host Command ID : 802.11 SNMP MIB */
+#define HostCmd_CMD_802_11_SNMP_MIB           0x0016
+/** Host Command ID : MAC register access */
+#define HostCmd_CMD_MAC_REG_ACCESS            0x0019
+/** Host Command ID : BBP register access */
+#define HostCmd_CMD_BBP_REG_ACCESS            0x001a
+/** Host Command ID : RF register access */
+#define HostCmd_CMD_RF_REG_ACCESS             0x001b
+
+/** Host Command ID : 802.11 radio control */
+#define HostCmd_CMD_802_11_RADIO_CONTROL      0x001c
+/** Host Command ID : 802.11 RF channel */
+#define HostCmd_CMD_802_11_RF_CHANNEL         0x001d
+/** Host Command ID : 802.11 RF Tx power */
+#define HostCmd_CMD_802_11_RF_TX_POWER        0x001e
+
+/** Host Command ID : 802.11 RF antenna */
+#define HostCmd_CMD_802_11_RF_ANTENNA         0x0020
+
+/** Host Command ID : 802.11 deauthenticate */
+#define HostCmd_CMD_802_11_DEAUTHENTICATE     0x0024
+/** Host Command ID : MAC control */
+#define HostCmd_CMD_MAC_CONTROL               0x0028
+/** Host Command ID : 802.11 Ad-Hoc start */
+#define HostCmd_CMD_802_11_AD_HOC_START       0x002b
+/** Host Command ID : 802.11 Ad-Hoc join */
+#define HostCmd_CMD_802_11_AD_HOC_JOIN        0x002c
+
+/** Host Command ID : 802.11 key material */
+#define HostCmd_CMD_802_11_KEY_MATERIAL       0x005e
+
+/** Host Command ID : 802.11 Ad-Hoc stop */
+#define HostCmd_CMD_802_11_AD_HOC_STOP        0x0040
+
+/** Host Command ID : 802.22 MAC address */
+#define HostCmd_CMD_802_11_MAC_ADDRESS        0x004D
+
+/** Host Command ID : WMM Traffic Stream Status */
+#define HostCmd_CMD_WMM_TS_STATUS             0x005d
+
+/** Host Command ID : 802.11 D domain information */
+#define HostCmd_CMD_802_11D_DOMAIN_INFO       0x005b
+
+/** Host Command ID : 802.11 TPC information */
+#define HostCmd_CMD_802_11_TPC_INFO           0x005f
+/** Host Command ID : 802.11 TPC adapt req */
+#define HostCmd_CMD_802_11_TPC_ADAPT_REQ      0x0060
+/** Host Command ID : 802.11 channel SW ann */
+#define HostCmd_CMD_802_11_CHAN_SW_ANN        0x0061
+
+/** Host Command ID : Measurement request */
+#define HostCmd_CMD_MEASUREMENT_REQUEST       0x0062
+/** Host Command ID : Measurement report */
+#define HostCmd_CMD_MEASUREMENT_REPORT        0x0063
+
+/** Host Command ID : 802.11 sleep parameters */
+#define HostCmd_CMD_802_11_SLEEP_PARAMS       0x0066
+
+/** Host Command ID : 802.11 sleep period */
+#define HostCmd_CMD_802_11_SLEEP_PERIOD       0x0068
+
+/** Host Command ID: 802.11 BG scan config */
+#define HostCmd_CMD_802_11_BG_SCAN_CONFIG  	  0x006b
+/** Host Command ID : 802.11 BG scan query */
+#define HostCmd_CMD_802_11_BG_SCAN_QUERY      0x006c
+
+/** Host Command ID : WMM ADDTS req */
+#define HostCmd_CMD_WMM_ADDTS_REQ             0x006E
+/** Host Command ID : WMM DELTS req */
+#define HostCmd_CMD_WMM_DELTS_REQ             0x006F
+/** Host Command ID : WMM queue configuration */
+#define HostCmd_CMD_WMM_QUEUE_CONFIG          0x0070
+/** Host Command ID : 802.11 get status */
+#define HostCmd_CMD_WMM_GET_STATUS            0x0071
+
+/** Host Command ID : 802.11 subscribe event */
+#define HostCmd_CMD_802_11_SUBSCRIBE_EVENT    0x0075
+
+/** Host Command ID : 802.11 Tx rate query */
+#define HostCmd_CMD_802_11_TX_RATE_QUERY      0x007f
+
+/** Host Command ID : WMM queue stats */
+#define HostCmd_CMD_WMM_QUEUE_STATS           0x0081
+
+/** Host Command ID : 802.11 IBSS coalescing status */
+#define HostCmd_CMD_802_11_IBSS_COALESCING_STATUS 0x0083
+
+/** Host Command ID : Memory access */
+#define HostCmd_CMD_MEM_ACCESS                0x0086
+
+/** Host Command ID : SDIO GPIO interrupt configuration */
+#define HostCmd_CMD_SDIO_GPIO_INT_CONFIG      0x0088
+
+#ifdef MFG_CMD_SUPPORT
+/** Host Command ID : Mfg command */
+#define HostCmd_CMD_MFG_COMMAND               0x0089
+#endif
+/** Host Command ID : Inactivity timeout ext */
+#define HostCmd_CMD_INACTIVITY_TIMEOUT_EXT    0x008a
+
+/** Host Command ID : DBGS configuration */
+#define HostCmd_CMD_DBGS_CFG                  0x008b
+/** Host Command ID : Get memory */
+#define HostCmd_CMD_GET_MEM                   0x008c
+
+/** Host Command ID : Cal data dnld */
+#define HostCmd_CMD_CFG_DATA                  0x008f
+
+/** Host Command ID : SDIO pull control */
+#define HostCmd_CMD_SDIO_PULL_CTRL            0x0093
+
+/** Host Command ID : ECL system clock configuration */
+#define HostCmd_CMD_ECL_SYSTEM_CLOCK_CONFIG   0x0094
+
+/** Host Command ID : Extended version */
+#define HostCmd_CMD_VERSION_EXT               0x0097
+
+/** Host Command ID : MEF configuration */
+#define HostCmd_CMD_MEF_CFG                   0x009a
+
+/** Host Command ID : 802.11 RSSI INFO*/
+#define HostCmd_CMD_RSSI_INFO                 0x00a4
+
+/** Host Command ID : Function initialization */
+#define HostCmd_CMD_FUNC_INIT                 0x00a9
+/** Host Command ID : Function shutdown */
+#define HostCmd_CMD_FUNC_SHUTDOWN             0x00aa
+
+/** Host Command ID : Channel report request */
+#define HostCmd_CMD_CHAN_REPORT_REQUEST              0x00dd
+
+/** Host Command ID: SUPPLICANT_PMK */
+#define HostCmd_CMD_SUPPLICANT_PMK            0x00c4
+/** Host Command ID: SUPPLICANT_PROFILE */
+#define HostCmd_CMD_SUPPLICANT_PROFILE        0x00c5
+
+/** Host Command ID : Add Block Ack Request */
+#define HostCmd_CMD_11N_ADDBA_REQ             0x00ce
+/** Host Command ID : Delete a Block Ack Request */
+#define HostCmd_CMD_11N_CFG                   0x00cd
+/** Host Command ID : Add Block Ack Response */
+#define HostCmd_CMD_11N_ADDBA_RSP             0x00cf
+/** Host Command ID : Delete a Block Ack Request */
+#define HostCmd_CMD_11N_DELBA                 0x00d0
+/** Host Command ID: Configure Tx Buf size */
+#define HostCmd_CMD_RECONFIGURE_TX_BUFF       0x00d9
+/** Host Command ID: AMSDU Aggr Ctrl */
+#define HostCmd_CMD_AMSDU_AGGR_CTRL	      	  0x00df
+/** Host Command ID: Configure TX Beamforming capability */
+#define HostCmd_CMD_TX_BF_CFG                 0x0104
+
+/** Host Command ID : 802.11 TX power configuration */
+#define HostCmd_CMD_TXPWR_CFG                 0x00d1
+
+/** Host Command ID : Soft Reset */
+#define  HostCmd_CMD_SOFT_RESET		          0x00d5
+
+/** Host Command ID : 802.11 b/g/n rate configration */
+#define HostCmd_CMD_TX_RATE_CFG               0x00d6
+
+/** Host Command ID : Enhanced PS mode */
+#define HostCmd_CMD_802_11_PS_MODE_ENH        0x00e4
+
+/** Host command action : Host sleep configuration */
+#define HostCmd_CMD_802_11_HS_CFG_ENH         0x00e5
+
+/** Host Command ID : CAU register access */
+#define HostCmd_CMD_CAU_REG_ACCESS            0x00ed
+
+/** Host Command ID : mgmt IE list */
+#define  HostCmd_CMD_MGMT_IE_LIST             0x00f2
+
+/** Host Command ID : Extended scan support */
+#define  HostCmd_CMD_802_11_SCAN_EXT          0x0107
+
+/** Host Command ID : Forward mgmt frame */
+#define HostCmd_CMD_RX_MGMT_IND               0x010c
+
+/** Host Command ID : Set BSS_MODE */
+#define HostCmd_CMD_SET_BSS_MODE               0x00f7
+
+#ifdef UAP_SUPPORT
+/**  Host Command id: SYS_INFO */
+#define HOST_CMD_APCMD_SYS_INFO                0x00ae
+/** Host Command id: sys_reset */
+#define HOST_CMD_APCMD_SYS_RESET               0x00af
+/** Host Command id: SYS_CONFIGURE  */
+#define HOST_CMD_APCMD_SYS_CONFIGURE           0x00b0
+/** Host Command id: BSS_START */
+#define HOST_CMD_APCMD_BSS_START               0x00b1
+/** Host Command id: BSS_STOP  */
+#define HOST_CMD_APCMD_BSS_STOP                0x00b2
+/** Host Command id: sta_list */
+#define HOST_CMD_APCMD_STA_LIST                0x00b3
+/** Host Command id: STA_DEAUTH */
+#define HOST_CMD_APCMD_STA_DEAUTH              0x00b5
+
+#endif /* UAP_SUPPORT */
+
+/** Host Command ID: Tx data pause */
+#define HostCmd_CMD_CFG_TX_DATA_PAUSE           0x0103
+
+#ifdef WIFI_DIRECT_SUPPORT
+/** Host Command ID: WIFI_DIRECT_MODE_CONFIG */
+#define HOST_CMD_WIFI_DIRECT_MODE_CONFIG	0x00eb
+/** Host Command ID: Remain On Channel */
+#define HostCmd_CMD_802_11_REMAIN_ON_CHANNEL     0x010d
+#endif
+
+/** Host Command ID : OTP user data */
+#define HostCmd_CMD_OTP_READ_USER_DATA          0x0114
+
+/** Enhanced PS modes */
+typedef enum _ENH_PS_MODES
+{
+    GET_PS = 0,
+    SLEEP_CONFIRM = 5,
+    DIS_AUTO_PS = 0xfe,
+    EN_AUTO_PS = 0xff,
+} ENH_PS_MODES;
+
+/** Command RET code, MSB is set to 1 */
+#define HostCmd_RET_BIT                       0x8000
+
+/** General purpose action : Get */
+#define HostCmd_ACT_GEN_GET                   0x0000
+/** General purpose action : Set */
+#define HostCmd_ACT_GEN_SET                   0x0001
+/** General purpose action : Get_Current */
+#define HostCmd_ACT_GEN_GET_CURRENT           0x0003
+/** General purpose action : Remove */
+#define HostCmd_ACT_GEN_REMOVE                0x0004
+
+/** Host command action : Set Rx */
+#define HostCmd_ACT_SET_RX                    0x0001
+/** Host command action : Set Tx */
+#define HostCmd_ACT_SET_TX                    0x0002
+/** Host command action : Set both Rx and Tx */
+#define HostCmd_ACT_SET_BOTH                  0x0003
+/** Host command action : Get Rx */
+#define HostCmd_ACT_GET_RX                    0x0004
+/** Host command action : Get Tx */
+#define HostCmd_ACT_GET_TX                    0x0008
+/** Host command action : Get both Rx and Tx */
+#define HostCmd_ACT_GET_BOTH                  0x000c
+
+/** General Result Code*/
+/** General result code OK */
+#define HostCmd_RESULT_OK                     0x0000
+/** Genenral error */
+#define HostCmd_RESULT_ERROR                  0x0001
+/** Command is not valid */
+#define HostCmd_RESULT_NOT_SUPPORT            0x0002
+/** Command is pending */
+#define HostCmd_RESULT_PENDING                0x0003
+/** System is busy (command ignored) */
+#define HostCmd_RESULT_BUSY                   0x0004
+/** Data buffer is not big enough */
+#define HostCmd_RESULT_PARTIAL_DATA           0x0005
+
+/* Define action or option for HostCmd_CMD_MAC_CONTROL */
+/** MAC action : Rx on */
+#define HostCmd_ACT_MAC_RX_ON                 0x0001
+/** MAC action : Tx on */
+#define HostCmd_ACT_MAC_TX_ON                 0x0002
+/** MAC action : WEP enable */
+#define HostCmd_ACT_MAC_WEP_ENABLE            0x0008
+/** MAC action : EthernetII enable */
+#define HostCmd_ACT_MAC_ETHERNETII_ENABLE     0x0010
+/** MAC action : Promiscous mode enable */
+#define HostCmd_ACT_MAC_PROMISCUOUS_ENABLE    0x0080
+/** MAC action : All multicast enable */
+#define HostCmd_ACT_MAC_ALL_MULTICAST_ENABLE  0x0100
+/** MAC action : RTS/CTS enable */
+#define HostCmd_ACT_MAC_RTS_CTS_ENABLE        0x0200
+/** MAC action : Strict protection enable */
+#define HostCmd_ACT_MAC_STRICT_PROTECTION_ENABLE  0x0400
+/** MAC action : Force 11n protection disable */
+#define HostCmd_ACT_MAC_FORCE_11N_PROTECTION_OFF  0x0800
+/** MAC action : Ad-Hoc G protection on */
+#define HostCmd_ACT_MAC_ADHOC_G_PROTECTION_ON     0x2000
+
+/* Define action or option for HostCmd_CMD_802_11_SCAN */
+/** Scan type : BSS */
+#define HostCmd_BSS_MODE_BSS                0x0001
+/** Scan type : IBSS */
+#define HostCmd_BSS_MODE_IBSS               0x0002
+/** Scan type : Any */
+#define HostCmd_BSS_MODE_ANY                0x0003
+
+/* Radio type definitions for the channel TLV */
+/** Radio type BG */
+#define HostCmd_SCAN_RADIO_TYPE_BG          0
+/** Radio type A */
+#define HostCmd_SCAN_RADIO_TYPE_A           1
+
+/** Define bitmap conditions for HOST_SLEEP_CFG : GPIO FF */
+#define HOST_SLEEP_CFG_GPIO_FF              0xff
+/** Define bitmap conditions for HOST_SLEEP_CFG : GAP FF */
+#define HOST_SLEEP_CFG_GAP_FF               0xff
+
+/** Buffer Constants */
+/** Number of command buffers */
+#define MRVDRV_NUM_OF_CMD_BUFFER        20
+/** Size of command buffer */
+#define MRVDRV_SIZE_OF_CMD_BUFFER       (2 * 1024)
+
+/** Maximum number of BSS Descriptors */
+#define MRVDRV_MAX_BSSID_LIST           64
+
+/** Host command flag in command */
+#define CMD_F_HOSTCMD           (1 << 0)
+/** command cancel flag in command */
+#define CMD_F_CANCELED          (1 << 1)
+
+/** Host Command ID bit mask (bit 11:0) */
+#define HostCmd_CMD_ID_MASK             0x0fff
+
+/** Host Command Sequence number mask (bit 7:0) */
+#define HostCmd_SEQ_NUM_MASK            0x00ff
+
+/** Host Command BSS number mask (bit 11:8) */
+#define HostCmd_BSS_NUM_MASK            0x0f00
+
+/** Host Command BSS type mask (bit 15:12) */
+#define HostCmd_BSS_TYPE_MASK           0xf000
+
+/** Set BSS information to Host Command */
+#define HostCmd_SET_SEQ_NO_BSS_INFO(seq, num, type)    \
+    (((seq) & 0x00ff) |         \
+     (((num) & 0x000f) << 8)) | \
+     (((type) & 0x000f) << 12)
+
+/** Get Sequence Number from Host Command (bit 7:0) */
+#define HostCmd_GET_SEQ_NO(seq)       \
+        ((seq) & HostCmd_SEQ_NUM_MASK)
+
+/** Get BSS number from Host Command (bit 11:8) */
+#define HostCmd_GET_BSS_NO(seq)         \
+        (((seq) & HostCmd_BSS_NUM_MASK) >> 8)
+
+/** Get BSS type from Host Command (bit 15:12) */
+#define HostCmd_GET_BSS_TYPE(seq)       \
+        (((seq) & HostCmd_BSS_TYPE_MASK) >> 12)
+
+/** Card Event definition : Dummy host wakeup signal */
+#define EVENT_DUMMY_HOST_WAKEUP_SIGNAL  0x00000001
+/** Card Event definition : Link lost */
+#define EVENT_LINK_LOST                 0x00000003
+/** Card Event definition : Link sensed */
+#define EVENT_LINK_SENSED               0x00000004
+/** Card Event definition : MIB changed */
+#define EVENT_MIB_CHANGED               0x00000006
+/** Card Event definition : Init done */
+#define EVENT_INIT_DONE                 0x00000007
+/** Card Event definition : Deauthenticated */
+#define EVENT_DEAUTHENTICATED           0x00000008
+/** Card Event definition : Disassociated */
+#define EVENT_DISASSOCIATED             0x00000009
+/** Card Event definition : Power save awake */
+#define EVENT_PS_AWAKE                  0x0000000a
+/** Card Event definition : Power save sleep */
+#define EVENT_PS_SLEEP                  0x0000000b
+/** Card Event definition : MIC error multicast */
+#define EVENT_MIC_ERR_MULTICAST         0x0000000d
+/** Card Event definition : MIC error unicast */
+#define EVENT_MIC_ERR_UNICAST           0x0000000e
+
+/** Card Event definition : Ad-Hoc BCN lost */
+#define EVENT_ADHOC_BCN_LOST            0x00000011
+
+/** Card Event definition : Stop Tx */
+#define EVENT_STOP_TX                   0x00000013
+/** Card Event definition : Start Tx */
+#define EVENT_START_TX                  0x00000014
+/** Card Event definition : Channel switch */
+#define EVENT_CHANNEL_SWITCH            0x00000015
+
+/** Card Event definition : MEAS report ready */
+#define EVENT_MEAS_REPORT_RDY           0x00000016
+
+/** Card Event definition : WMM status change */
+#define EVENT_WMM_STATUS_CHANGE         0x00000017
+
+/** Card Event definition : BG scan report */
+#define EVENT_BG_SCAN_REPORT            0x00000018
+
+/** Card Event definition : Beacon RSSI low */
+#define EVENT_RSSI_LOW                  0x00000019
+/** Card Event definition : Beacon SNR low */
+#define EVENT_SNR_LOW                   0x0000001a
+/** Card Event definition : Maximum fail */
+#define EVENT_MAX_FAIL                  0x0000001b
+/** Card Event definition : Beacon RSSI high */
+#define EVENT_RSSI_HIGH                 0x0000001c
+/** Card Event definition : Beacon SNR high */
+#define EVENT_SNR_HIGH                  0x0000001d
+
+/** Card Event definition : IBSS coalsced */
+#define EVENT_IBSS_COALESCED            0x0000001e
+
+/** Card Event definition : Data RSSI low */
+#define EVENT_DATA_RSSI_LOW             0x00000024
+/** Card Event definition : Data SNR low */
+#define EVENT_DATA_SNR_LOW              0x00000025
+/** Card Event definition : Data RSSI high */
+#define EVENT_DATA_RSSI_HIGH            0x00000026
+/** Card Event definition : Data SNR high */
+#define EVENT_DATA_SNR_HIGH             0x00000027
+
+/** Card Event definition : Link Quality */
+#define EVENT_LINK_QUALITY              0x00000028
+
+/** Card Event definition : Port release event */
+#define EVENT_PORT_RELEASE              0x0000002b
+
+/** Card Event definition : Pre-Beacon Lost */
+#define EVENT_PRE_BEACON_LOST           0x00000031
+
+/** Card Event definition : Add BA event */
+#define EVENT_ADDBA                     0x00000033
+/** Card Event definition : Del BA event */
+#define EVENT_DELBA                     0x00000034
+/** Card Event definition: BA stream timeout*/
+#define EVENT_BA_STREAM_TIMEOUT         0x00000037
+
+/** Card Event definition : AMSDU aggr control */
+#define EVENT_AMSDU_AGGR_CTRL           0x00000042
+
+/** Card Event definition: WEP ICV error */
+#define EVENT_WEP_ICV_ERR               0x00000046
+
+/** Card Event definition : Host sleep enable */
+#define EVENT_HS_ACT_REQ                0x00000047
+
+/** Card Event definition : BW changed */
+#define EVENT_BW_CHANGE                 0x00000048
+
+#ifdef WIFI_DIRECT_SUPPORT
+/** WIFIDIRECT generic event */
+#define EVENT_WIFIDIRECT_GENERIC_EVENT         0x00000049
+/** WIFIDIRECT service discovery event */
+#define EVENT_WIFIDIRECT_SERVICE_DISCOVERY     0x0000004a
+/** Remain on Channel expired event */
+#define EVENT_REMAIN_ON_CHANNEL_EXPIRED        0x0000005f
+#endif
+
+/** Card Event definition: Channel switch pending announcment */
+#define EVENT_CHANNEL_SWITCH_ANN        0x00000050
+
+/** Event definition:  Radar Detected by card */
+#define EVENT_RADAR_DETECTED            0x00000053
+
+/** Event definition:  Radar Detected by card */
+#define EVENT_CHANNEL_REPORT_RDY        0x00000054
+
+/** Event definition:  Scan results through event */
+#define EVENT_EXT_SCAN_REPORT           0x00000058
+
+/** Event definition: RXBA_SYNC */
+#define EVENT_RXBA_SYNC                 0x00000059
+
+#ifdef UAP_SUPPORT
+/** Event ID: STA deauth */
+#define EVENT_MICRO_AP_STA_DEAUTH       0x0000002c
+/** Event ID: STA assoicated */
+#define EVENT_MICRO_AP_STA_ASSOC        0x0000002d
+/** Event ID: BSS started */
+#define EVENT_MICRO_AP_BSS_START        0x0000002e
+/** Event ID: BSS idle event */
+#define EVENT_MICRO_AP_BSS_IDLE         0x00000043
+/** Event ID: BSS active event */
+#define EVENT_MICRO_AP_BSS_ACTIVE       0x00000044
+
+#endif /* UAP_SUPPORT */
+
+/** Event ID: TX data pause event */
+#define EVENT_TX_DATA_PAUSE                  0x00000055
+
+/** Event ID mask */
+#define EVENT_ID_MASK                   0xffff
+
+/** BSS number mask */
+#define BSS_NUM_MASK                    0xf
+
+/** Get BSS number from event cause (bit 23:16) */
+#define EVENT_GET_BSS_NUM(event_cause)          \
+    (((event_cause) >> 16) & BSS_NUM_MASK)
+
+/** Get BSS type from event cause (bit 31:24) */
+#define EVENT_GET_BSS_TYPE(event_cause)         \
+    (((event_cause) >> 24) & 0x00ff)
+
+/** Event_WEP_ICV_ERR structure */
+typedef MLAN_PACK_START struct _Event_WEP_ICV_ERR
+{
+    /** Reason code */
+    t_u16 reason_code;
+    /** Source MAC address */
+    t_u8 src_mac_addr[MLAN_MAC_ADDR_LENGTH];
+    /** WEP decryption used key */
+    t_u8 wep_key_index;
+    /** WEP key length */
+    t_u8 wep_key_length;
+    /** WEP key */
+    t_u8 key[MAX_WEP_KEY_SIZE];
+} MLAN_PACK_END Event_WEP_ICV_ERR;
+
+/** WLAN_802_11_FIXED_IEs */
+typedef MLAN_PACK_START struct _WLAN_802_11_FIXED_IEs
+{
+    /** Timestamp */
+    t_u8 time_stamp[8];
+    /** Beacon interval */
+    t_u16 beacon_interval;
+    /** Capabilities*/
+    t_u16 capabilities;
+} MLAN_PACK_END WLAN_802_11_FIXED_IEs;
+
+/** WLAN_802_11_VARIABLE_IEs */
+typedef MLAN_PACK_START struct _WLAN_802_11_VARIABLE_IEs
+{
+    /** Element ID */
+    t_u8 element_id;
+    /** Length */
+    t_u8 length;
+    /** IE data */
+    t_u8 data[1];
+} MLAN_PACK_END WLAN_802_11_VARIABLE_IEs;
+
+/** TLV related data structures*/
+/** MrvlIEtypesHeader_t */
+typedef MLAN_PACK_START struct _MrvlIEtypesHeader
+{
+    /** Header type */
+    t_u16 type;
+    /** Header length */
+    t_u16 len;
+} MLAN_PACK_END MrvlIEtypesHeader_t;
+
+/** MrvlIEtypes_Data_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_Data_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Data */
+    t_u8 data[1];
+} MLAN_PACK_END MrvlIEtypes_Data_t;
+
+/** Bit mask for TxPD status field for null packet */
+#define MRVDRV_TxPD_POWER_MGMT_NULL_PACKET 0x01
+/** Bit mask for TxPD status field for last packet */
+#define MRVDRV_TxPD_POWER_MGMT_LAST_PACKET 0x08
+
+/** Packet type: 802.11 */
+#define PKT_TYPE_802DOT11   0x05
+#define PKT_TYPE_MGMT_FRAME 0xE5
+/** Packet type: AMSDU */
+#define PKT_TYPE_AMSDU      0xE6
+/** Packet type: BAR */
+#define PKT_TYPE_BAR        0xE7
+/** Packet type: debugging */
+#define PKT_TYPE_DEBUG      0xEF
+
+/** TxPD descriptor */
+typedef MLAN_PACK_START struct _TxPD
+{
+    /** BSS type */
+    t_u8 bss_type;
+    /** BSS number */
+    t_u8 bss_num;
+    /** Tx packet length */
+    t_u16 tx_pkt_length;
+    /** Tx packet offset */
+    t_u16 tx_pkt_offset;
+    /** Tx packet type */
+    t_u16 tx_pkt_type;
+    /** Tx Control */
+    t_u32 tx_control;
+    /** Pkt Priority */
+    t_u8 priority;
+    /** Transmit Pkt Flags*/
+    t_u8 flags;
+    /** Amount of time the packet has been queued in the driver (units = 2ms)*/
+    t_u8 pkt_delay_2ms;
+    /** Reserved */
+    t_u8 reserved1;
+} MLAN_PACK_END TxPD, *PTxPD;
+
+/** RxPD Descriptor */
+typedef MLAN_PACK_START struct _RxPD
+{
+    /** BSS type */
+    t_u8 bss_type;
+    /** BSS number */
+    t_u8 bss_num;
+    /** Rx Packet Length */
+    t_u16 rx_pkt_length;
+    /** Rx Pkt offset */
+    t_u16 rx_pkt_offset;
+    /** Rx packet type */
+    t_u16 rx_pkt_type;
+    /** Sequence number */
+    t_u16 seq_num;
+    /** Packet Priority */
+    t_u8 priority;
+    /** Rx Packet Rate */
+    t_u8 rx_rate;
+    /** SNR */
+    t_s8 snr;
+    /** Noise Floor */
+    t_s8 nf;
+    /** Ht Info [Bit 0] RxRate format: LG=0, HT=1
+     * [Bit 1]  HT Bandwidth: BW20 = 0, BW40 = 1
+     * [Bit 2]  HT Guard Interval: LGI = 0, SGI = 1 */
+    t_u8 ht_info;
+    /** Reserved */
+    t_u8 reserved;
+} MLAN_PACK_END RxPD, *PRxPD;
+
+#ifdef UAP_SUPPORT
+/** TxPD descriptor */
+typedef MLAN_PACK_START struct _UapTxPD
+{
+        /** BSS type */
+    t_u8 bss_type;
+        /** BSS number */
+    t_u8 bss_num;
+        /** Tx packet length */
+    t_u16 tx_pkt_length;
+        /** Tx packet offset */
+    t_u16 tx_pkt_offset;
+        /** Tx packet type */
+    t_u16 tx_pkt_type;
+        /** Tx Control */
+    t_u32 tx_control;
+        /** Pkt Priority */
+    t_u8 priority;
+        /** Transmit Pkt Flags*/
+    t_u8 flags;
+        /** Amount of time the packet has been queued in the driver (units = 2ms)*/
+    t_u8 pkt_delay_2ms;
+        /** Reserved */
+    t_u8 reserved1;
+        /** Reserved */
+    t_u32 reserved;
+} MLAN_PACK_END UapTxPD, *PUapTxPD;
+
+/** RxPD Descriptor */
+typedef MLAN_PACK_START struct _UapRxPD
+{
+        /** BSS Type */
+    t_u8 bss_type;
+        /** BSS number*/
+    t_u8 bss_num;
+        /** Rx packet length */
+    t_u16 rx_pkt_length;
+        /** Rx packet offset */
+    t_u16 rx_pkt_offset;
+        /** Rx packet type */
+    t_u16 rx_pkt_type;
+    /** Sequence number */
+    t_u16 seq_num;
+    /** Packet Priority */
+    t_u8 priority;
+    /** reserved */
+    t_u8 reserved1;
+} MLAN_PACK_END UapRxPD, *PUapRxPD;
+
+/** Fixed size of station association event */
+#define ASSOC_EVENT_FIX_SIZE    12
+
+/** IEEEtypes_FrameCtl_t*/
+#ifdef BIG_ENDIAN_SUPPORT
+typedef MLAN_PACK_START struct _IEEEtypes_FrameCtl_t
+{
+    /** Order */
+    t_u8 order:1;
+    /** Wep */
+    t_u8 wep:1;
+    /** More Data */
+    t_u8 more_data:1;
+    /** Power Mgmt */
+    t_u8 pwr_mgmt:1;
+    /** Retry */
+    t_u8 retry:1;
+    /** More Frag */
+    t_u8 more_frag:1;
+    /** From DS */
+    t_u8 from_ds:1;
+    /** To DS */
+    t_u8 to_ds:1;
+    /** Sub Type */
+    t_u8 sub_type:4;
+    /** Type */
+    t_u8 type:2;
+    /** Protocol Version */
+    t_u8 protocol_version:2;
+} MLAN_PACK_END IEEEtypes_FrameCtl_t;
+#else
+typedef MLAN_PACK_START struct _IEEEtypes_FrameCtl_t
+{
+    /** Protocol Version */
+    t_u8 protocol_version:2;
+    /** Type */
+    t_u8 type:2;
+    /** Sub Type */
+    t_u8 sub_type:4;
+    /** To DS */
+    t_u8 to_ds:1;
+    /** From DS */
+    t_u8 from_ds:1;
+    /** More Frag */
+    t_u8 more_frag:1;
+    /** Retry */
+    t_u8 retry:1;
+    /** Power Mgmt */
+    t_u8 pwr_mgmt:1;
+    /** More Data */
+    t_u8 more_data:1;
+    /** Wep */
+    t_u8 wep:1;
+    /** Order */
+    t_u8 order:1;
+} MLAN_PACK_END IEEEtypes_FrameCtl_t;
+#endif
+
+/** MrvlIETypes_MgmtFrameSet_t */
+typedef MLAN_PACK_START struct _MrvlIETypes_MgmtFrameSet_t
+{
+    /** Type */
+    t_u16 type;
+    /** Length */
+    t_u16 len;
+    /** Frame Control */
+    IEEEtypes_FrameCtl_t frame_control;
+    /* t_u8 frame_contents[0]; */
+} MLAN_PACK_END MrvlIETypes_MgmtFrameSet_t;
+
+/** IEEEtypes_AssocRqst_t */
+typedef MLAN_PACK_START struct _IEEEtypes_AssocRqst_t
+{
+    /** Capability Info */
+    t_u16 cap_info;
+    /** Listen Interval */
+    t_u16 listen_interval;
+    /* t_u8 ie_buffer[0]; */
+} MLAN_PACK_END IEEEtypes_AssocRqst_t;
+
+/** IEEEtypes_ReAssocRqst_t */
+typedef MLAN_PACK_START struct _IEEEtypes_ReAssocRqst_t
+{
+    /** Capability Info */
+    t_u16 cap_info;
+    /** Listen Interval */
+    t_u16 listen_interval;
+    /** Current AP Address */
+    t_u8 current_ap_addr[MLAN_MAC_ADDR_LENGTH];
+    /* t_u8 ie_buffer[0]; */
+} MLAN_PACK_END IEEEtypes_ReAssocRqst_t;
+#endif /* UAP_SUPPORT */
+
+/** wlan_802_11_header */
+typedef MLAN_PACK_START struct _wlan_802_11_header
+{
+    /** Frame Control */
+    t_u16 frm_ctl;
+    /** Duration ID */
+    t_u16 duration_id;
+    /** Address1 */
+    mlan_802_11_mac_addr addr1;
+    /** Address2 */
+    mlan_802_11_mac_addr addr2;
+    /** Address3 */
+    mlan_802_11_mac_addr addr3;
+    /** Sequence Control */
+    t_u16 seq_ctl;
+    /** Address4 */
+    mlan_802_11_mac_addr addr4;
+} MLAN_PACK_END wlan_802_11_header;
+
+/** wlan_802_11_header packet from FW with length */
+typedef MLAN_PACK_START struct _wlan_mgmt_pkt
+{
+    /** Packet Length */
+    t_u16 frm_len;
+    /** wlan_802_11_header */
+    wlan_802_11_header wlan_header;
+} MLAN_PACK_END wlan_mgmt_pkt;
+
+#ifdef STA_SUPPORT
+/** (Beaconsize(256)-5(IEId,len,contrystr(3))/3(FirstChan,NoOfChan,MaxPwr) */
+#define MAX_NO_OF_CHAN          40
+
+/** Channel-power table entries */
+typedef MLAN_PACK_START struct _chan_power_11d
+{
+    /** 11D channel */
+    t_u8 chan;
+    /** Band for channel */
+    t_u8 band;
+    /** 11D channel power */
+    t_u8 pwr;
+    /** AP seen on channel */
+    t_u8 ap_seen;
+} MLAN_PACK_END chan_power_11d_t;
+
+/** Region channel info */
+typedef MLAN_PACK_START struct _parsed_region_chan_11d
+{
+    /** 11D channel power per channel */
+    chan_power_11d_t chan_pwr[MAX_NO_OF_CHAN];
+    /** 11D number of channels */
+    t_u8 no_of_chan;
+} MLAN_PACK_END parsed_region_chan_11d_t;
+#endif /* STA_SUPPORT */
+
+/** ChanScanMode_t */
+typedef MLAN_PACK_START struct _ChanScanMode_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** Reserved */
+    t_u8 reserved_2_7:6;
+    /** Disble channel filtering flag */
+    t_u8 disable_chan_filt:1;
+    /** Channel scan mode passive flag */
+    t_u8 passive_scan:1;
+#else
+    /** Channel scan mode passive flag */
+    t_u8 passive_scan:1;
+    /** Disble channel filtering flag */
+    t_u8 disable_chan_filt:1;
+    /** Reserved */
+    t_u8 reserved_2_7:6;
+#endif
+} MLAN_PACK_END ChanScanMode_t;
+
+/** secondary channel is below */
+#define SECOND_CHANNEL_BELOW    0x30
+/** secondary channel is above */
+#define SECOND_CHANNEL_ABOVE    0x10
+/** channel offset */
+enum
+{
+    SEC_CHAN_NONE = 0,
+    SEC_CHAN_ABOVE = 1,
+    SEC_CHAN_BELOW = 3
+};
+/** channel bandwidth */
+enum
+{
+    CHAN_BW_20MHZ = 0,
+    CHAN_BW_10MHZ,
+    CHAN_BW_40MHZ,
+};
+/** ChanScanParamSet_t */
+typedef MLAN_PACK_START struct _ChanScanParamSet_t
+{
+    /** Channel scan parameter : Radio type */
+    t_u8 radio_type;
+    /** Channel scan parameter : Channel number */
+    t_u8 chan_number;
+    /** Channel scan parameter : Channel scan mode */
+    ChanScanMode_t chan_scan_mode;
+    /** Channel scan parameter : Minimum scan time */
+    t_u16 min_scan_time;
+    /** Channel scan parameter : Maximum scan time */
+    t_u16 max_scan_time;
+} MLAN_PACK_END ChanScanParamSet_t;
+
+/** MrvlIEtypes_ChanListParamSet_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_ChanListParamSet_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Channel scan parameters */
+    ChanScanParamSet_t chan_scan_param[1];
+} MLAN_PACK_END MrvlIEtypes_ChanListParamSet_t;
+
+/** ChanBandParamSet_t */
+typedef struct _ChanBandParamSet_t
+{
+    /** Channel scan parameter : Radio type */
+    t_u8 radio_type;
+    /** Channel number */
+    t_u8 chan_number;
+} ChanBandParamSet_t;
+
+/** MrvlIEtypes_ChanBandListParamSet_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_ChanBandListParamSet_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Channel Band parameters */
+    ChanBandParamSet_t chan_band_param[1];
+} MLAN_PACK_END MrvlIEtypes_ChanBandListParamSet_t;
+
+/** MrvlIEtypes_RatesParamSet_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_RatesParamSet_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Rates */
+    t_u8 rates[1];
+} MLAN_PACK_END MrvlIEtypes_RatesParamSet_t;
+
+/** MrvlIEtypes_SsIdParamSet_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_SsIdParamSet_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** SSID */
+    t_u8 ssid[1];
+} MLAN_PACK_END MrvlIEtypes_SsIdParamSet_t;
+
+/** MrvlIEtypes_NumProbes_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_NumProbes_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Number of probes */
+    t_u16 num_probes;
+} MLAN_PACK_END MrvlIEtypes_NumProbes_t;
+
+/** MrvlIEtypes_WildCardSsIdParamSet_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_WildCardSsIdParamSet_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Maximum SSID length */
+    t_u8 max_ssid_length;
+    /** SSID */
+    t_u8 ssid[1];
+} MLAN_PACK_END MrvlIEtypes_WildCardSsIdParamSet_t;
+
+/**TSF data size */
+#define TSF_DATA_SIZE            8
+/** Table of TSF values returned in the scan result */
+typedef MLAN_PACK_START struct _MrvlIEtypes_TsfTimestamp_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** the length of each TSF data is 8 bytes, could be multiple TSF here */
+    t_u8 tsf_data[1];
+} MLAN_PACK_END MrvlIEtypes_TsfTimestamp_t;
+
+/** CfParamSet_t */
+typedef MLAN_PACK_START struct _CfParamSet_t
+{
+    /** CF parameter : Count */
+    t_u8 cfp_cnt;
+    /** CF parameter : Period */
+    t_u8 cfp_period;
+    /** CF parameter : Duration */
+    t_u16 cfp_max_duration;
+    /** CF parameter : Duration remaining */
+    t_u16 cfp_duration_remaining;
+} MLAN_PACK_END CfParamSet_t;
+
+/** IbssParamSet_t */
+typedef MLAN_PACK_START struct _IbssParamSet_t
+{
+    /** ATIM window value */
+    t_u16 atim_window;
+} MLAN_PACK_END IbssParamSet_t;
+
+/** MrvlIEtypes_SsParamSet_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_SsParamSet_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** CF/IBSS parameters sets */
+    union
+    {
+        /** CF parameter set */
+        CfParamSet_t cf_param_set[1];
+        /** IBSS parameter set */
+        IbssParamSet_t ibss_param_set[1];
+    } cf_ibss;
+} MLAN_PACK_END MrvlIEtypes_SsParamSet_t;
+
+/** FhParamSet_t */
+typedef MLAN_PACK_START struct _FhParamSet_t
+{
+    /** FH parameter : Dwell time */
+    t_u16 dwell_time;
+    /** FH parameter : Hop set */
+    t_u8 hop_set;
+    /** FH parameter : Hop pattern */
+    t_u8 hop_pattern;
+    /** FH parameter : Hop index */
+    t_u8 hop_index;
+} MLAN_PACK_END FhParamSet_t;
+
+/** DsParamSet_t */
+typedef MLAN_PACK_START struct _DsParamSet_t
+{
+    /** Current channel number */
+    t_u8 current_chan;
+} MLAN_PACK_END DsParamSet_t;
+
+/** MrvlIEtypes_PhyParamSet_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_PhyParamSet_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** FH/DS parameters */
+    union
+    {
+        /** FH parameter set */
+        FhParamSet_t fh_param_set[1];
+        /** DS parameter set */
+        DsParamSet_t ds_param_set[1];
+    } fh_ds;
+} MLAN_PACK_END MrvlIEtypes_PhyParamSet_t;
+
+/* Auth type to be used in the Authentication portion of an Assoc seq */
+/** MrvlIEtypes_AuthType_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_AuthType_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Authentication type */
+    t_u16 auth_type;
+} MLAN_PACK_END MrvlIEtypes_AuthType_t;
+
+/** MrvlIETypes_ActionFrame_t */
+typedef MLAN_PACK_START struct
+{
+    MrvlIEtypesHeader_t header;   /**< Header */
+
+    t_u8 srcAddr[MLAN_MAC_ADDR_LENGTH];
+    t_u8 dstAddr[MLAN_MAC_ADDR_LENGTH];
+
+    IEEEtypes_ActionFrame_t actionFrame;
+
+} MLAN_PACK_END MrvlIETypes_ActionFrame_t;
+
+/** MrvlIEtypes_RxBaSync_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_RxBaSync_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** mac address */
+    t_u8 mac[MLAN_MAC_ADDR_LENGTH];
+    /** tid */
+    t_u8 tid;
+    /** reserved field */
+    t_u8 reserved;
+    /** start seq num */
+    t_u16 seq_num;
+    /** bitmap len */
+    t_u16 bitmap_len;
+    /** bitmap */
+    t_u8 bitmap[1];
+} MLAN_PACK_END MrvlIEtypes_RxBaSync_t;
+
+/** MrvlIEtypes_RsnParamSet_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_RsnParamSet_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** RSN IE */
+    t_u8 rsn_ie[1];
+} MLAN_PACK_END MrvlIEtypes_RsnParamSet_t;
+
+/** Key_param_set fixed length */
+#define KEYPARAMSET_FIXED_LEN 6
+
+/** MrvlIEtype_KeyParamSet_t */
+typedef MLAN_PACK_START struct _MrvlIEtype_KeyParamSet_t
+{
+    /** Type ID */
+    t_u16 type;
+    /** Length of Payload */
+    t_u16 length;
+    /** Type of Key: WEP=0, TKIP=1, AES=2 */
+    t_u16 key_type_id;
+    /** Key Control Info specific to a key_type_id */
+    t_u16 key_info;
+    /** Length of key */
+    t_u16 key_len;
+    /** Key material of size key_len */
+    t_u8 key[50];
+} MLAN_PACK_END MrvlIEtype_KeyParamSet_t;
+
+/** HostCmd_DS_802_11_KEY_MATERIAL */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_KEY_MATERIAL
+{
+    /** Action */
+    t_u16 action;
+    /** Key parameter set */
+    MrvlIEtype_KeyParamSet_t key_param_set;
+} MLAN_PACK_END HostCmd_DS_802_11_KEY_MATERIAL;
+
+/** Data structure of WMM QoS information */
+typedef MLAN_PACK_START struct _WmmQosInfo_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** QoS UAPSD */
+    t_u8 qos_uapsd:1;
+    /** Reserved */
+    t_u8 reserved:3;
+    /** Parameter set count */
+    t_u8 para_set_count:4;
+#else
+    /** Parameter set count */
+    t_u8 para_set_count:4;
+    /** Reserved */
+    t_u8 reserved:3;
+    /** QoS UAPSD */
+    t_u8 qos_uapsd:1;
+#endif                          /* BIG_ENDIAN_SUPPORT */
+} MLAN_PACK_END WmmQosInfo_t, *pWmmQosInfo_t;
+
+/** Data structure of WMM ECW */
+typedef MLAN_PACK_START struct _WmmEcw_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** Maximum Ecw */
+    t_u8 ecw_max:4;
+    /** Minimum Ecw */
+    t_u8 ecw_min:4;
+#else
+    /** Minimum Ecw */
+    t_u8 ecw_min:4;
+    /** Maximum Ecw */
+    t_u8 ecw_max:4;
+#endif                          /* BIG_ENDIAN_SUPPORT */
+} MLAN_PACK_END WmmEcw_t, *pWmmEcw_t;
+
+/** Data structure of WMM Aci/Aifsn */
+typedef MLAN_PACK_START struct _WmmAciAifsn_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** Reserved */
+    t_u8 reserved:1;
+    /** Aci */
+    t_u8 aci:2;
+    /** Acm */
+    t_u8 acm:1;
+    /** Aifsn */
+    t_u8 aifsn:4;
+#else
+    /** Aifsn */
+    t_u8 aifsn:4;
+    /** Acm */
+    t_u8 acm:1;
+    /** Aci */
+    t_u8 aci:2;
+    /** Reserved */
+    t_u8 reserved:1;
+#endif                          /* BIG_ENDIAN_SUPPORT */
+} MLAN_PACK_END WmmAciAifsn_t, *pWmmAciAifsn_t;
+
+/** Data structure of WMM AC parameters  */
+typedef MLAN_PACK_START struct _WmmAcParameters_t
+{
+    WmmAciAifsn_t aci_aifsn;       /**< AciAifSn */
+    WmmEcw_t ecw;                 /**< Ecw */
+    t_u16 tx_op_limit;                        /**< Tx op limit */
+} MLAN_PACK_END WmmAcParameters_t, *pWmmAcParameters_t;
+
+/** Data structure of WMM parameter  */
+typedef MLAN_PACK_START struct _WmmParameter_t
+{
+    /** OuiType:  00:50:f2:02 */
+    t_u8 ouitype[4];
+    /** Oui subtype: 01 */
+    t_u8 ouisubtype;
+    /** version: 01 */
+    t_u8 version;
+    /** QoS information */
+    t_u8 qos_info;
+    /** Reserved */
+    t_u8 reserved;
+    /** AC Parameters Record WMM_AC_BE, WMM_AC_BK, WMM_AC_VI, WMM_AC_VO */
+    WmmAcParameters_t ac_params[MAX_AC_QUEUES];
+} MLAN_PACK_END WmmParameter_t, *pWmmParameter_t;
+
+/* Definition of firmware host command */
+/** HostCmd_DS_GEN */
+typedef MLAN_PACK_START struct _HostCmd_DS_GEN
+{
+    /** Command */
+    t_u16 command;
+    /** Size */
+    t_u16 size;
+    /** Sequence number */
+    t_u16 seq_num;
+    /** Result */
+    t_u16 result;
+} MLAN_PACK_END HostCmd_DS_GEN;
+
+/** Size of HostCmd_DS_GEN */
+#define S_DS_GEN        sizeof(HostCmd_DS_GEN)
+
+/** Address type: broadcast */
+#define ADDR_TYPE_BROADCAST		1
+/* Address type: unicast */
+#define ADDR_TYPE_UNICAST		2
+/* Address type: multicast */
+#define ADDR_TYPE_MULTICAST		3
+
+/** Ether type: any */
+#define ETHER_TYPE_ANY			0xffff
+/** Ether type: ARP */
+#define ETHER_TYPE_ARP			0x0608
+
+/** IPv4 address any */
+#define IPV4_ADDR_ANY			0xffffffff
+
+/** Header structure for ARP filter */
+typedef MLAN_PACK_START struct _arpfilter_header
+{
+        /** Type */
+    t_u16 type;
+        /** TLV length */
+    t_u16 len;
+} MLAN_PACK_END arpfilter_header;
+
+/** Filter entry structure */
+typedef MLAN_PACK_START struct _filter_entry
+{
+        /** Address type */
+    t_u16 addr_type;
+        /** Ether type */
+    t_u16 eth_type;
+        /** IPv4 address */
+    t_u32 ipv4_addr;
+} MLAN_PACK_END filter_entry;
+
+typedef MLAN_PACK_START struct _HostCmd_DS_MEF_CFG
+{
+    /** Criteria */
+    t_u32 criteria;
+    /** Number of entries */
+    t_u16 nentries;
+} MLAN_PACK_END HostCmd_DS_MEF_CFG;
+
+/* HostCmd_DS_802_11_SLEEP_PERIOD */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_SLEEP_PERIOD
+{
+    /** ACT_GET/ACT_SET */
+    t_u16 action;
+
+    /** Sleep Period in msec */
+    t_u16 sleep_pd;
+} MLAN_PACK_END HostCmd_DS_802_11_SLEEP_PERIOD;
+
+/* HostCmd_DS_802_11_SLEEP_PARAMS */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_SLEEP_PARAMS
+{
+    /** ACT_GET/ACT_SET */
+    t_u16 action;
+    /** Sleep clock error in ppm */
+    t_u16 error;
+    /** Wakeup offset in usec */
+    t_u16 offset;
+    /** Clock stabilization time in usec */
+    t_u16 stable_time;
+    /** Control periodic calibration */
+    t_u8 cal_control;
+    /** Control the use of external sleep clock */
+    t_u8 external_sleep_clk;
+    /** Reserved field, should be set to zero */
+    t_u16 reserved;
+} MLAN_PACK_END HostCmd_DS_802_11_SLEEP_PARAMS;
+
+/** Sleep response control */
+typedef enum _sleep_resp_ctrl
+{
+    RESP_NOT_NEEDED = 0,
+    RESP_NEEDED,
+} sleep_resp_ctrl;
+
+/** Structure definition for the new ieee power save parameters*/
+typedef struct __ps_param
+{
+      /** Null packet interval */
+    t_u16 null_pkt_interval;
+      /** Num dtims */
+    t_u16 multiple_dtims;
+      /** becaon miss interval */
+    t_u16 bcn_miss_timeout;
+      /** local listen interval */
+    t_u16 local_listen_interval;
+     /** Adhoc awake period */
+    t_u16 adhoc_wake_period;
+     /** mode - (0x01 - firmware to automatically choose PS_POLL or NULL mode, 0x02 - PS_POLL, 0x03 - NULL mode ) */
+    t_u16 mode;
+     /** Delay to PS in milliseconds */
+    t_u16 delay_to_ps;
+} ps_param;
+
+/** Structure definition for the new auto deep sleep command */
+typedef struct __auto_ds_param
+{
+     /** Deep sleep inactivity timeout */
+    t_u16 deep_sleep_timeout;
+} auto_ds_param;
+
+/** Structure definition for sleep confirmation in the new ps command */
+typedef struct __sleep_confirm_param
+{
+     /** response control 0x00 - response not needed, 0x01 - response needed */
+    t_u16 resp_ctrl;
+} sleep_confirm_param;
+
+/** bitmap for get auto deepsleep */
+#define BITMAP_AUTO_DS         0x01
+/** bitmap for sta power save */
+#define BITMAP_STA_PS          0x10
+/** bitmap for uap inactivity based PS */
+#define BITMAP_UAP_INACT_PS    0x100
+/** bitmap for uap DTIM PS */
+#define BITMAP_UAP_DTIM_PS     0x200
+/** Structure definition for the new ieee power save parameters*/
+typedef struct _auto_ps_param
+{
+    /** bitmap for enable power save mode */
+    t_u16 ps_bitmap;
+    /* auto deep sleep parameter, sta power save parameter uap inactivity
+       parameter uap DTIM parameter */
+} auto_ps_param;
+
+/** fix size for auto ps */
+#define AUTO_PS_FIX_SIZE    4
+
+/** TLV type : auto ds param */
+#define TLV_TYPE_AUTO_DS_PARAM        (PROPRIETARY_TLV_BASE_ID + 0x71)  // 0x0171
+/** TLV type : ps param */
+#define TLV_TYPE_PS_PARAM             (PROPRIETARY_TLV_BASE_ID + 0x72)  // 0x0172
+
+/** MrvlIEtypes_auto_ds_param_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_auto_ds_param_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** auto ds param */
+    auto_ds_param param;
+} MLAN_PACK_END MrvlIEtypes_auto_ds_param_t;
+
+/** MrvlIEtypes_ps_param_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_ps_param_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** ps param */
+    ps_param param;
+} MLAN_PACK_END MrvlIEtypes_ps_param_t;
+
+/** Structure definition for new power save command */
+typedef MLAN_PACK_START struct _HostCmd_DS_PS_MODE_ENH
+{
+    /** Action */
+    t_u16 action;
+    /** Data speciifc to action */
+    /* For IEEE power save data will be as UINT16 mode (0x01 - firmware to
+       automatically choose PS_POLL or NULL mode, 0x02 - PS_POLL, 0x03 - NULL
+       mode ) UINT16 NullpacketInterval UINT16 NumDtims UINT16
+       BeaconMissInterval UINT16 locallisteninterval UINT16 adhocawakeperiod */
+
+    /* For auto deep sleep */
+    /* UINT16 Deep sleep inactivity timeout */
+
+    /* For PS sleep confirm UINT16 responeCtrl - 0x00 - reponse from fw not
+       needed, 0x01 - response from fw is needed */
+
+    union
+    {
+    /** PS param definition */
+        ps_param opt_ps;
+    /** Auto ds param definition */
+        auto_ds_param auto_ds;
+    /** Sleep comfirm param definition */
+        sleep_confirm_param sleep_cfm;
+    /** bitmap for get PS info and Disable PS mode */
+        t_u16 ps_bitmap;
+    /** auto ps param */
+        auto_ps_param auto_ps;
+    } params;
+} MLAN_PACK_END HostCmd_DS_802_11_PS_MODE_ENH;
+
+/** HostCmd_DS_GET_HW_SPEC */
+typedef MLAN_PACK_START struct _HostCmd_DS_GET_HW_SPEC
+{
+    /** HW Interface version number */
+    t_u16 hw_if_version;
+    /** HW version number */
+    t_u16 version;
+    /** Reserved field */
+    t_u16 reserved;
+    /** Max no of Multicast address  */
+    t_u16 num_of_mcast_adr;
+    /** MAC address */
+    t_u8 permanent_addr[MLAN_MAC_ADDR_LENGTH];
+    /** Region Code */
+    t_u16 region_code;
+    /** Number of antenna used */
+    t_u16 number_of_antenna;
+    /** FW release number, example 0x1234=1.2.3.4 */
+    t_u32 fw_release_number;
+    /** Reserved field */
+    t_u32 reserved_1;
+    /** Reserved field */
+    t_u32 reserved_2;
+    /** Reserved field */
+    t_u32 reserved_3;
+    /** FW/HW Capability */
+    t_u32 fw_cap_info;
+    /** 802.11n Device Capabilities */
+    t_u32 dot_11n_dev_cap;
+    /** MIMO abstraction of MCSs supported by device */
+    t_u8 dev_mcs_support;
+        /** Valid end port at init */
+    t_u16 mp_end_port;
+        /** mgmt IE buffer count */
+    t_u16 mgmt_buf_count;
+} MLAN_PACK_END HostCmd_DS_GET_HW_SPEC;
+
+/**  HostCmd_DS_802_11_CFG_DATA */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_CFG_DATA
+{
+    /** Action */
+    t_u16 action;
+    /** Type */
+    t_u16 type;
+    /** Data length */
+    t_u16 data_len;
+    /** Data */
+} MLAN_PACK_END HostCmd_DS_802_11_CFG_DATA;
+
+/**  HostCmd_DS_CMD_802_11_RSSI_INFO */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_RSSI_INFO
+{
+    /** Action */
+    t_u16 action;
+    /** Parameter used for exponential averaging for Data */
+    t_u16 ndata;
+    /** Parameter used for exponential averaging for Beacon */
+    t_u16 nbcn;
+    /** Reserved field 0 */
+    t_u16 reserved[9];
+    /** Reserved field 1 */
+    t_u64 reserved_1;
+} MLAN_PACK_END HostCmd_DS_802_11_RSSI_INFO;
+
+/** HostCmd_DS_802_11_RSSI_INFO_RSP */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_RSSI_INFO_RSP
+{
+    /** Action */
+    t_u16 action;
+    /** Parameter used for exponential averaging for Data */
+    t_u16 ndata;
+    /** Parameter used for exponential averaging for beacon */
+    t_u16 nbcn;
+    /** Last Data RSSI in dBm */
+    t_s16 data_rssi_last;
+    /** Last Data NF in dBm */
+    t_s16 data_nf_last;
+    /** AVG DATA RSSI in dBm */
+    t_s16 data_rssi_avg;
+    /** AVG DATA NF in dBm */
+    t_s16 data_nf_avg;
+    /** Last BEACON RSSI in dBm */
+    t_s16 bcn_rssi_last;
+    /** Last BEACON NF in dBm */
+    t_s16 bcn_nf_last;
+    /** AVG BEACON RSSI in dBm */
+    t_s16 bcn_rssi_avg;
+    /** AVG BEACON NF in dBm */
+    t_s16 bcn_nf_avg;
+    /** Last RSSI Beacon TSF */
+    t_u64 tsf_bcn;
+} MLAN_PACK_END HostCmd_DS_802_11_RSSI_INFO_RSP;
+
+/** HostCmd_DS_802_11_MAC_ADDRESS */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_MAC_ADDRESS
+{
+    /** Action */
+    t_u16 action;
+    /** MAC address */
+    t_u8 mac_addr[MLAN_MAC_ADDR_LENGTH];
+} MLAN_PACK_END HostCmd_DS_802_11_MAC_ADDRESS;
+
+/** HostCmd_DS_MAC_CONTROL */
+typedef MLAN_PACK_START struct _HostCmd_DS_MAC_CONTROL
+{
+    /** Action */
+    t_u16 action;
+    /** Reserved field */
+    t_u16 reserved;
+} MLAN_PACK_END HostCmd_DS_MAC_CONTROL;
+
+/** HostCmd_DS_CMD_TX_DATA_PAUSE */
+typedef MLAN_PACK_START struct _HostCmd_DS_CMD_TX_DATA_PAUSE
+{
+    /** Action */
+    t_u16 action;
+    /** Enable/disable Tx data pause */
+    t_u8 enable_tx_pause;
+    /** Max number of TX buffers allowed for all PS clients*/
+    t_u8 pause_tx_count;
+} MLAN_PACK_END HostCmd_DS_CMD_TX_DATA_PAUSE;
+
+/** TLV type : TX pause TLV */
+#define TLV_TYPE_TX_PAUSE      (PROPRIETARY_TLV_BASE_ID + 0x94) // 0x0194
+/** MrvlIEtypes_SsIdParamSet_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_tx_pause_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** peer mac address */
+    t_u8 peermac[MLAN_MAC_ADDR_LENGTH];
+    /** Tx pause state, 1--pause, 0--free flowing */
+    t_u8 tx_pause;
+    /** total packets queued for the client */
+    t_u8 pkt_cnt;
+} MLAN_PACK_END MrvlIEtypes_tx_pause_t;
+
+/**  HostCmd_CMD_MAC_MULTICAST_ADR */
+typedef MLAN_PACK_START struct _HostCmd_DS_MAC_MULTICAST_ADR
+{
+    /** Action */
+    t_u16 action;
+    /** Number of addresses */
+    t_u16 num_of_adrs;
+    /** List of MAC */
+    t_u8 mac_list[MLAN_MAC_ADDR_LENGTH * MLAN_MAX_MULTICAST_LIST_SIZE];
+} MLAN_PACK_END HostCmd_DS_MAC_MULTICAST_ADR;
+
+/**  HostCmd_CMD_802_11_DEAUTHENTICATE */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_DEAUTHENTICATE
+{
+    /** MAC address */
+    t_u8 mac_addr[MLAN_MAC_ADDR_LENGTH];
+    /** Deauthentication resaon code */
+    t_u16 reason_code;
+} MLAN_PACK_END HostCmd_DS_802_11_DEAUTHENTICATE;
+
+/** HostCmd_DS_802_11_ASSOCIATE */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_ASSOCIATE
+{
+    /** Peer STA address */
+    t_u8 peer_sta_addr[MLAN_MAC_ADDR_LENGTH];
+    /** Capability information */
+    IEEEtypes_CapInfo_t cap_info;
+    /** Listen interval */
+    t_u16 listen_interval;
+    /** Beacon period */
+    t_u16 beacon_period;
+    /** DTIM period */
+    t_u8 dtim_period;
+
+    /**
+     *  MrvlIEtypes_SsIdParamSet_t  SsIdParamSet;
+     *  MrvlIEtypes_PhyParamSet_t   PhyParamSet;
+     *  MrvlIEtypes_SsParamSet_t    SsParamSet;
+     *  MrvlIEtypes_RatesParamSet_t RatesParamSet;
+     */
+} MLAN_PACK_END HostCmd_DS_802_11_ASSOCIATE;
+
+/** HostCmd_CMD_802_11_ASSOCIATE response */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_ASSOCIATE_RSP
+{
+    /** Association response structure */
+    IEEEtypes_AssocRsp_t assoc_rsp;
+} MLAN_PACK_END HostCmd_DS_802_11_ASSOCIATE_RSP;
+
+/** HostCmd_DS_802_11_AD_HOC_START*/
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_AD_HOC_START
+{
+    /** AdHoc SSID */
+    t_u8 ssid[MLAN_MAX_SSID_LENGTH];
+    /** BSS mode */
+    t_u8 bss_mode;
+    /** Beacon period */
+    t_u16 beacon_period;
+    /** DTIM period */
+    t_u8 dtim_period;
+    /** SS parameter set */
+    IEEEtypes_SsParamSet_t ss_param_set;
+    /** PHY parameter set */
+    IEEEtypes_PhyParamSet_t phy_param_set;
+    /** Reserved field */
+    t_u16 reserved1;
+    /** Capability information */
+    IEEEtypes_CapInfo_t cap;
+    /** Supported data rates */
+    t_u8 DataRate[HOSTCMD_SUPPORTED_RATES];
+} MLAN_PACK_END HostCmd_DS_802_11_AD_HOC_START;
+
+/**  HostCmd_CMD_802_11_AD_HOC_START response */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_AD_HOC_START_RESULT
+{
+    /** Padding */
+    t_u8 pad[3];
+    /** AdHoc BSSID */
+    t_u8 bssid[MLAN_MAC_ADDR_LENGTH];
+    /** Padding to sync with FW structure*/
+    t_u8 pad2[2];
+    /** Result */
+    t_u8 result;
+} MLAN_PACK_END HostCmd_DS_802_11_AD_HOC_START_RESULT;
+
+/**  HostCmd_CMD_802_11_AD_HOC_START response */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_AD_HOC_JOIN_RESULT
+{
+    /** Result */
+    t_u8 result;
+} MLAN_PACK_END HostCmd_DS_802_11_AD_HOC_JOIN_RESULT;
+
+/** AdHoc_BssDesc_t */
+typedef MLAN_PACK_START struct _AdHoc_BssDesc_t
+{
+    /** BSSID */
+    t_u8 bssid[MLAN_MAC_ADDR_LENGTH];
+    /** SSID */
+    t_u8 ssid[MLAN_MAX_SSID_LENGTH];
+    /** BSS mode */
+    t_u8 bss_mode;
+    /** Beacon period */
+    t_u16 beacon_period;
+    /** DTIM period */
+    t_u8 dtim_period;
+    /** Timestamp */
+    t_u8 time_stamp[8];
+    /** Local time */
+    t_u8 local_time[8];
+    /** PHY parameter set */
+    IEEEtypes_PhyParamSet_t phy_param_set;
+    /** SS parameter set */
+    IEEEtypes_SsParamSet_t ss_param_set;
+    /** Capability information */
+    IEEEtypes_CapInfo_t cap;
+    /** Supported data rates */
+    t_u8 data_rates[HOSTCMD_SUPPORTED_RATES];
+
+    /*
+     *  DO NOT ADD ANY FIELDS TO THIS STRUCTURE.
+     *  It is used in the Adhoc join command and will cause a
+     *  binary layout mismatch with the firmware
+     */
+} MLAN_PACK_END AdHoc_BssDesc_t;
+
+/** HostCmd_DS_802_11_AD_HOC_JOIN */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_AD_HOC_JOIN
+{
+    /** AdHoc BSS descriptor */
+    AdHoc_BssDesc_t bss_descriptor;
+    /** Reserved field */
+    t_u16 reserved1;
+    /** Reserved field */
+    t_u16 reserved2;
+} MLAN_PACK_END HostCmd_DS_802_11_AD_HOC_JOIN;
+
+/** Interrupt Raising Edge */
+#define INT_RASING_EDGE     0
+/** Interrupt Falling Edge */
+#define INT_FALLING_EDGE    1
+
+/** Delay 1 usec */
+#define DELAY_1_US          1
+
+typedef MLAN_PACK_START struct _HostCmd_DS_SDIO_GPIO_INT_CONFIG
+{
+    /** Action */
+    t_u16 action;
+    /** GPIO interrupt pin */
+    t_u16 gpio_pin;
+    /** GPIO interrupt edge, 1: failing edge; 0: raising edge */
+    t_u16 gpio_int_edge;
+    /** GPIO interrupt pulse widthin usec units */
+    t_u16 gpio_pulse_width;
+} MLAN_PACK_END HostCmd_DS_SDIO_GPIO_INT_CONFIG;
+
+typedef MLAN_PACK_START struct _HostCmd_DS_SDIO_PULL_CTRL
+{
+    /** Action */
+    t_u16 action;
+    /** The delay of pulling up in us */
+    t_u16 pull_up;
+    /** The delay of pulling down in us */
+    t_u16 pull_down;
+} MLAN_PACK_END HostCmd_DS_SDIO_PULL_CTRL;
+
+/** HostCmd_DS_802_11_GET_LOG */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_GET_LOG
+{
+    /** Number of multicast transmitted frames */
+    t_u32 mcast_tx_frame;
+    /** Number of failures */
+    t_u32 failed;
+    /** Number of retries */
+    t_u32 retry;
+    /** Number of multiretries */
+    t_u32 multiretry;
+    /** Number of duplicate frames */
+    t_u32 frame_dup;
+    /** Number of RTS success */
+    t_u32 rts_success;
+    /** Number of RTS failure */
+    t_u32 rts_failure;
+    /** Number of acknowledgement failure */
+    t_u32 ack_failure;
+    /** Number of fragmented packets received */
+    t_u32 rx_frag;
+    /** Number of multicast frames received */
+    t_u32 mcast_rx_frame;
+    /** FCS error */
+    t_u32 fcs_error;
+    /** Number of transmitted frames */
+    t_u32 tx_frame;
+    /** Reserved field */
+    t_u32 reserved;
+    /** Number of WEP icv error for each key */
+    t_u32 wep_icv_err_cnt[4];
+} MLAN_PACK_END HostCmd_DS_802_11_GET_LOG;
+
+/**_HostCmd_TX_RATE_QUERY */
+typedef MLAN_PACK_START struct _HostCmd_TX_RATE_QUERY
+{
+    /** Tx rate */
+    t_u8 tx_rate;
+    /** Ht Info [Bit 0] RxRate format: LG=0, HT=1
+     * [Bit 1]  HT Bandwidth: BW20 = 0, BW40 = 1
+     * [Bit 2]  HT Guard Interval: LGI = 0, SGI = 1 */
+    t_u8 ht_info;
+} MLAN_PACK_END HostCmd_TX_RATE_QUERY;
+
+typedef MLAN_PACK_START struct _hs_config_param
+{
+    /** bit0=1: broadcast data
+      * bit1=1: unicast data
+      * bit2=1: mac events
+      * bit3=1: multicast data
+      */
+    t_u32 conditions;
+    /** GPIO pin or 0xff for interface */
+    t_u8 gpio;
+    /** gap in milliseconds or or 0xff for special setting when GPIO is used to wakeup host */
+    t_u8 gap;
+} MLAN_PACK_END hs_config_param;
+
+/** HS Action 0x0001 - Configure enhanced host sleep mode, 0x0002 - Activate enhanced host sleep mode */
+typedef enum _Host_Sleep_Action
+{
+    HS_CONFIGURE = 0x0001,
+    HS_ACTIVATE = 0x0002,
+} Host_Sleep_Action;
+
+/** Structure definition for activating enhanced hs */
+typedef MLAN_PACK_START struct __hs_activate_param
+{
+     /** response control 0x00 - response not needed, 0x01 - response needed */
+    t_u16 resp_ctrl;
+} MLAN_PACK_END hs_activate_param;
+
+/** HostCmd_DS_802_11_HS_CFG_ENH */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_HS_CFG_ENH
+{
+    /** Action 0x0001 - Configure enhanced host sleep mode, 0x0002 - Activate enhanced host sleep mode */
+    t_u16 action;
+
+    union
+    {
+    /** Configure enhanced hs */
+        hs_config_param hs_config;
+    /** Activate enhanced hs */
+        hs_activate_param hs_activate;
+    } params;
+} MLAN_PACK_END HostCmd_DS_802_11_HS_CFG_ENH;
+
+/** SNMP_MIB_INDEX */
+typedef enum _SNMP_MIB_INDEX
+{
+    OpRateSet_i = 1,
+    DtimPeriod_i = 3,
+    RtsThresh_i = 5,
+    ShortRetryLim_i = 6,
+    LongRetryLim_i = 7,
+    FragThresh_i = 8,
+    Dot11D_i = 9,
+    Dot11H_i = 10,
+    WwsMode_i = 17,
+    Thermal_i = 34,
+} SNMP_MIB_INDEX;
+
+/** max SNMP buf size */
+#define MAX_SNMP_BUF_SIZE   128
+
+/**  HostCmd_CMD_802_11_SNMP_MIB */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_SNMP_MIB
+{
+    /** SNMP query type */
+    t_u16 query_type;
+    /** SNMP object ID */
+    t_u16 oid;
+    /** SNMP buffer size */
+    t_u16 buf_size;
+    /** Value */
+    t_u8 value[1];
+} MLAN_PACK_END HostCmd_DS_802_11_SNMP_MIB;
+
+/** Radio on */
+#define RADIO_ON                                0x01
+/** Radio off */
+#define RADIO_OFF                               0x00
+
+/** HostCmd_CMD_802_11_RADIO_CONTROL */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_RADIO_CONTROL
+{
+    /** Action */
+    t_u16 action;
+    /** Control */
+    t_u16 control;
+} MLAN_PACK_END HostCmd_DS_802_11_RADIO_CONTROL;
+
+/** MrvlRateScope_t */
+typedef MLAN_PACK_START struct _MrvlRateScope_t
+{
+    /** Header Type */
+    t_u16 type;
+    /** Header Length */
+    t_u16 length;
+    /** Bitmap of HR/DSSS rates */
+    t_u16 hr_dsss_rate_bitmap;
+    /** Bitmap of OFDM rates */
+    t_u16 ofdm_rate_bitmap;
+    /** Bitmap of HT-MCSs allowed for initial rate */
+    t_u16 ht_mcs_rate_bitmap[8];
+} MLAN_PACK_END MrvlRateScope_t;
+
+/** MrvlRateDropControl_t */
+typedef MLAN_PACK_START struct _MrvlRateDropControl_t
+{
+    /** Header Length */
+    t_u16 length;
+    /** Rate Information */
+    t_u32 rate_info[1];
+} MLAN_PACK_END MrvlRateDropControl_t;
+
+/** MrvlRateDropPattern_t */
+typedef MLAN_PACK_START struct _MrvlRateDropPattern_t
+{
+    /** Header Type */
+    t_u16 type;
+    /** Header Length */
+    t_u16 length;
+    /** Rate Drop Mode */
+    t_u32 rate_drop_mode;
+    /* MrvlRateDropControl_t RateDropControl[0]; */
+} MLAN_PACK_END MrvlRateDropPattern_t;
+
+/** HostCmd_DS_TX_RATE_CFG */
+typedef MLAN_PACK_START struct _HostCmd_DS_TX_RATE_CFG
+{
+    /** Action */
+    t_u16 action;
+    /** Tx Rate Configuration Index */
+    t_u16 cfg_index;
+    /* MrvlRateScope_t RateScope; MrvlRateDropPattern_t RateDrop; */
+} MLAN_PACK_END HostCmd_DS_TX_RATE_CFG;
+
+/** Power_Group_t */
+typedef MLAN_PACK_START struct _Power_Group_t
+{
+    /** Modulation Class */
+    t_u8 modulation_class;
+    /** MCS Code or Legacy RateID */
+    t_u8 first_rate_code;
+    /** MCS Code or Legacy RateID */
+    t_u8 last_rate_code;
+    /** Power Adjustment Step */
+    t_s8 power_step;
+    /** Minimal Tx Power Level [dBm] */
+    t_s8 power_min;
+    /** Maximal Tx Power Level [dBm] */
+    t_s8 power_max;
+    /** 0: HTBW20, 1: HTBW40 */
+    t_u8 ht_bandwidth;
+    /** Reserved */
+    t_u8 reserved;
+} MLAN_PACK_END Power_Group_t;
+
+/** MrvlTypes_Power_Group_t */
+typedef MLAN_PACK_START struct _MrvlTypes_Power_Group_t
+{
+    /** Header Type */
+    t_u16 type;
+    /** Header Length */
+    t_u16 length;
+    /* Power_Group_t PowerGroups */
+} MLAN_PACK_END MrvlTypes_Power_Group_t;
+
+/** HostCmd_CMD_TXPWR_CFG */
+typedef MLAN_PACK_START struct _HostCmd_DS_TXPWR_CFG
+{
+    /** Action */
+    t_u16 action;
+    /** Power group configuration index */
+    t_u16 cfg_index;
+    /** Power group configuration mode */
+    t_u32 mode;
+    /* MrvlTypes_Power_Group_t PowerGrpCfg[0] */
+} MLAN_PACK_END HostCmd_DS_TXPWR_CFG;
+
+/** HostCmd_CMD_802_11_RF_TX_POWER */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_RF_TX_POWER
+{
+    /** Action */
+    t_u16 action;
+    /** Current power level */
+    t_u16 current_level;
+    /** Maximum power */
+    t_u8 max_power;
+    /** Minimum power */
+    t_u8 min_power;
+} MLAN_PACK_END HostCmd_DS_802_11_RF_TX_POWER;
+
+/** Connection type infra */
+#define CONNECTION_TYPE_INFRA 	0
+/** Connection type adhoc */
+#define CONNECTION_TYPE_ADHOC	1
+#ifdef WIFI_DIRECT_SUPPORT
+/** BSS Mode: WIFIDIRECT Client */
+#define BSS_MODE_WIFIDIRECT_CLIENT 	0
+/** BSS Mode: WIFIDIRECT GO */
+#define BSS_MODE_WIFIDIRECT_GO     	2
+#endif
+/** HostCmd_DS_SET_BSS_MODE */
+typedef MLAN_PACK_START struct _HostCmd_DS_SET_BSS_MODE
+{
+    /** connection type */
+    t_u8 con_type;
+} MLAN_PACK_END HostCmd_DS_SET_BSS_MODE;
+
+#ifdef WIFI_DIRECT_SUPPORT
+/** HostCmd_DS_REMAIN_ON_CHANNEL */
+typedef MLAN_PACK_START struct _HostCmd_DS_REMAIN_ON_CHANNEL
+{
+    /** Action 0-GET, 1-SET, 4 CLEAR*/
+    t_u16 action;
+    /** Not used set to zero */
+    t_u8 status;
+    /** Not used set to zero */
+    t_u8 reserved;
+    /** Band cfg */
+    t_u8 bandcfg;
+    /** channel */
+    t_u8 channel;
+    /** remain time: Unit ms*/
+    t_u32 remain_period;
+} MLAN_PACK_END HostCmd_DS_REMAIN_ON_CHANNEL;
+
+/** HostCmd_DS_WIFI_DIRECT_MODE */
+typedef MLAN_PACK_START struct _HostCmd_DS_WIFI_DIRECT_MODE
+{
+    /** Action 0-GET, 1-SET*/
+    t_u16 action;
+    /**0:disable 1:listen 2:GO 3:p2p client 4:find 5:stop find*/
+    t_u16 mode;
+} MLAN_PACK_END HostCmd_DS_WIFI_DIRECT_MODE;
+#endif
+
+#ifdef STA_SUPPORT
+
+/**
+ * @brief Structure used internally in the wlan driver to configure a scan.
+ *
+ * Sent to the command process module to configure the firmware
+ *   scan command prepared by wlan_cmd_802_11_scan.
+ *
+ * @sa wlan_scan_networks
+ *
+ */
+typedef MLAN_PACK_START struct _wlan_scan_cmd_config
+{
+    /**
+     *  BSS Type to be sent in the firmware command
+     *
+     *  Field can be used to restrict the types of networks returned in the
+     *    scan.  Valid settings are:
+     *
+     *   - MLAN_SCAN_MODE_BSS  (infrastructure)
+     *   - MLAN_SCAN_MODE_IBSS (adhoc)
+     *   - MLAN_SCAN_MODE_ANY  (unrestricted, adhoc and infrastructure)
+     */
+    t_u8 bss_mode;
+
+    /**
+     *  Specific BSSID used to filter scan results in the firmware
+     */
+    t_u8 specific_bssid[MLAN_MAC_ADDR_LENGTH];
+
+    /**
+     *  Length of TLVs sent in command starting at tlvBuffer
+     */
+    t_u32 tlv_buf_len;
+
+    /**
+     *  SSID TLV(s) and ChanList TLVs to be sent in the firmware command
+     *
+     *  TLV_TYPE_CHANLIST, MrvlIEtypes_ChanListParamSet_t
+     *  TLV_TYPE_SSID, MrvlIEtypes_SsIdParamSet_t
+     */
+    t_u8 tlv_buf[1];            /* SSID TLV(s) and ChanList TLVs are stored
+                                   here */
+} MLAN_PACK_END wlan_scan_cmd_config;
+
+/**
+ *  Sructure to retrieve the scan table
+ */
+typedef MLAN_PACK_START struct
+{
+    /**
+     *  - Zero based scan entry to start retrieval in command request
+     *  - Number of scans entries returned in command response
+     */
+    t_u32 scan_number;
+    /**
+     * Buffer marker for multiple wlan_ioctl_get_scan_table_entry structures.
+     *   Each struct is padded to the nearest 32 bit boundary.
+     */
+    t_u8 scan_table_entry_buf[1];
+} MLAN_PACK_END wlan_get_scan_table_info;
+
+/** Generic structure defined for parsing WPA/RSN IEs for GTK/PTK OUIs */
+typedef MLAN_PACK_START struct
+{
+        /** Group key oui */
+    t_u8 GrpKeyOui[4];
+        /** Number of PTKs */
+    t_u8 PtkCnt[2];
+        /** Ptk body starts here */
+    t_u8 PtkBody[4];
+} MLAN_PACK_END IEBody;
+#endif /* STA_SUPPORT */
+
+/*
+ * This scan handle Country Information IE(802.11d compliant)
+ * Define data structure for HostCmd_CMD_802_11_SCAN
+ */
+/** HostCmd_DS_802_11_SCAN */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_SCAN
+{
+    /** BSS mode */
+    t_u8 bss_mode;
+    /** BSSID */
+    t_u8 bssid[MLAN_MAC_ADDR_LENGTH];
+    /** TLV buffer */
+    t_u8 tlv_buffer[1];
+    /** MrvlIEtypes_SsIdParamSet_t      SsIdParamSet;
+     *  MrvlIEtypes_ChanListParamSet_t  ChanListParamSet;
+     *  MrvlIEtypes_RatesParamSet_t     OpRateSet;
+     */
+} MLAN_PACK_END HostCmd_DS_802_11_SCAN;
+
+/*
+ * This scan handle Country Information IE(802.11d compliant)
+ * Define data structure for HostCmd_CMD_802_11_SCAN_EXT
+ */
+/** HostCmd_DS_802_11_SCAN_EXT */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_SCAN_EXT
+{
+    /** Reserved */
+    t_u32 reserved;
+    /** TLV buffer */
+    t_u8 tlv_buffer[1];
+    /** MrvlIEtypes_Bssid_List_t            BssIdList;
+     *  MrvlIEtypes_SsIdParamSet_t          SSIDParamSet;
+     *  MrvlIEtypes_ChanListParamSet_t      ChanListParamSet;
+     *  MrvlIEtypes_RatesParamSet_t         OpRateSet;
+     *  MrvlIEtypes_NumProbes_t             NumProbes;
+     *  MrvlIEtypes_WildCardSsIdParamSet_t  WildCardSSIDParamSet;
+     */
+} MLAN_PACK_END HostCmd_DS_802_11_SCAN_EXT;
+
+typedef MLAN_PACK_START struct _MrvlIEtypes_Bss_Scan_Rsp_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** BSSID of the BSS descriptor */
+    t_u8 bssid[MLAN_MAC_ADDR_LENGTH];
+    /** Beacon/Probe response buffer */
+    t_u8 frame_body[1];
+} MLAN_PACK_END MrvlIEtypes_Bss_Scan_Rsp_t;
+
+typedef MLAN_PACK_START struct _MrvlIEtypes_Bss_Scan_Info_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** RSSI for scan entry */
+    t_s16 rssi;
+    /** Channel ANPI */
+    t_s16 anpi;
+    /** Channel load (parts per 255) */
+    t_u8 cca_busy_fraction;
+    /** Band */
+    t_u8 band;
+    /** Channel */
+    t_u8 channel;
+    /** Reserved */
+    t_u8 reserved;
+    /** TSF data */
+    t_u64 tsf;
+} MLAN_PACK_END MrvlIEtypes_Bss_Scan_Info_t;
+
+/** HostCmd_DS_RX_MGMT_IND */
+typedef MLAN_PACK_START struct _HostCmd_DS_RX_MGMT_IND
+{
+    /** Action */
+    t_u16 action;
+    /** Mgmt frame subtype mask */
+    t_u32 mgmt_subtype_mask;
+} MLAN_PACK_END HostCmd_DS_RX_MGMT_IND;
+
+/** HostCmd_DS_802_11_SCAN_RSP */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_SCAN_RSP
+{
+    /** Size of BSS descriptor */
+    t_u16 bss_descript_size;
+    /** Numner of sets */
+    t_u8 number_of_sets;
+    /** BSS descriptor and TLV buffer */
+    t_u8 bss_desc_and_tlv_buffer[1];
+} MLAN_PACK_END HostCmd_DS_802_11_SCAN_RSP;
+
+/** HostCmd_DS_802_11_BG_SCAN_CONFIG */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_BG_SCAN_CONFIG
+{
+    /** action */
+    t_u16 action;
+    /** 0: disable, 1: enable */
+    t_u8 enable;
+    /** bss type */
+    t_u8 bss_type;
+    /** num of channel per scan */
+    t_u8 chan_per_scan;
+    /** reserved field */
+    t_u8 reserved;
+    /** reserved field */
+    t_u16 reserved1;
+    /** interval between consecutive scans */
+    t_u32 scan_interval;
+    /** reserved field */
+    t_u32 reserved2;
+    /** condition to trigger report to host */
+    t_u32 report_condition;
+    /** reserved field */
+    t_u16 reserved3;
+} MLAN_PACK_END HostCmd_DS_802_11_BG_SCAN_CONFIG;
+
+/** HostCmd_DS_802_11_BG_SCAN_QUERY */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_BG_SCAN_QUERY
+{
+    /** Flush */
+    t_u8 flush;
+} MLAN_PACK_END HostCmd_DS_802_11_BG_SCAN_QUERY;
+
+/** HostCmd_DS_802_11_BG_SCAN_QUERY_RSP */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_BG_SCAN_QUERY_RSP
+{
+    /** Report condition */
+    t_u32 report_condition;
+    /** Scan response */
+    HostCmd_DS_802_11_SCAN_RSP scan_resp;
+} MLAN_PACK_END HostCmd_DS_802_11_BG_SCAN_QUERY_RSP;
+
+/** MrvlIEtypes_StartLater_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_StartLater_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /* 0 - BGScan start immediately, 1 - BGScan will start later after "Scan
+       Interval" */
+    t_u16 value;
+} MLAN_PACK_END MrvlIEtypes_StartLater_t;
+
+/** MrvlIEtypes_RepeatCount_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_RepeatCount_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /* Repeat count */
+    t_u16 repeat_count;
+} MLAN_PACK_END MrvlIEtypes_RepeatCount_t;
+
+/** MrvlIEtypes_DomainParamSet_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_DomainParamSet
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Country code */
+    t_u8 country_code[COUNTRY_CODE_LEN];
+    /** Set of subbands */
+    IEEEtypes_SubbandSet_t sub_band[1];
+} MLAN_PACK_END MrvlIEtypes_DomainParamSet_t;
+
+/** HostCmd_DS_802_11D_DOMAIN_INFO */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11D_DOMAIN_INFO
+{
+    /** Action */
+    t_u16 action;
+    /** Domain parameter set */
+    MrvlIEtypes_DomainParamSet_t domain;
+} MLAN_PACK_END HostCmd_DS_802_11D_DOMAIN_INFO;
+
+/** HostCmd_DS_802_11D_DOMAIN_INFO_RSP */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11D_DOMAIN_INFO_RSP
+{
+    /** Action */
+    t_u16 action;
+    /** Domain parameter set */
+    MrvlIEtypes_DomainParamSet_t domain;
+} MLAN_PACK_END HostCmd_DS_802_11D_DOMAIN_INFO_RSP;
+
+/** HostCmd_DS_11N_ADDBA_REQ */
+typedef MLAN_PACK_START struct _HostCmd_DS_11N_ADDBA_REQ
+{
+    /** Result of the ADDBA Request Operation */
+    t_u8 add_req_result;
+    /** Peer MAC address */
+    t_u8 peer_mac_addr[MLAN_MAC_ADDR_LENGTH];
+    /** Dialog Token */
+    t_u8 dialog_token;
+    /** Block Ack Parameter Set */
+    t_u16 block_ack_param_set;
+    /** Block Act Timeout Value */
+    t_u16 block_ack_tmo;
+    /** Starting Sequence Number */
+    t_u16 ssn;
+} MLAN_PACK_END HostCmd_DS_11N_ADDBA_REQ;
+
+/** HostCmd_DS_11N_ADDBA_RSP */
+typedef MLAN_PACK_START struct _HostCmd_DS_11N_ADDBA_RSP
+{
+    /** Result of the ADDBA Response Operation */
+    t_u8 add_rsp_result;
+    /** Peer MAC address */
+    t_u8 peer_mac_addr[MLAN_MAC_ADDR_LENGTH];
+    /** Dialog Token */
+    t_u8 dialog_token;
+    /** Status Code */
+    t_u16 status_code;
+    /** Block Ack Parameter Set */
+    t_u16 block_ack_param_set;
+    /** Block Act Timeout Value */
+    t_u16 block_ack_tmo;
+    /** Starting Sequence Number */
+    t_u16 ssn;
+} MLAN_PACK_END HostCmd_DS_11N_ADDBA_RSP;
+
+/** HostCmd_DS_11N_DELBA */
+typedef MLAN_PACK_START struct _HostCmd_DS_11N_DELBA
+{
+    /** Result of the ADDBA Request Operation */
+    t_u8 del_result;
+    /** Peer MAC address */
+    t_u8 peer_mac_addr[MLAN_MAC_ADDR_LENGTH];
+    /** Delete Block Ack Parameter Set */
+    t_u16 del_ba_param_set;
+    /** Reason Code sent for DELBA */
+    t_u16 reason_code;
+    /** Reserved */
+    t_u8 reserved;
+} MLAN_PACK_END HostCmd_DS_11N_DELBA;
+
+/** HostCmd_DS_11N_BATIMEOUT */
+typedef MLAN_PACK_START struct _HostCmd_DS_11N_BATIMEOUT
+{
+    /** TID */
+    t_u8 tid;
+    /** Peer MAC address */
+    t_u8 peer_mac_addr[MLAN_MAC_ADDR_LENGTH];
+    /** Delete Block Ack Parameter Set */
+    t_u8 origninator;
+} MLAN_PACK_END HostCmd_DS_11N_BATIMEOUT;
+
+/** HostCmd_DS_11N_CFG */
+typedef MLAN_PACK_START struct _HostCmd_DS_11N_CFG
+{
+    /** Action */
+    t_u16 action;
+    /** HTTxCap */
+    t_u16 ht_tx_cap;
+    /** HTTxInfo */
+    t_u16 ht_tx_info;
+    /** Misc configuration */
+    t_u16 misc_config;
+} MLAN_PACK_END HostCmd_DS_11N_CFG;
+
+/** HostCmd_DS_TXBUF_CFG*/
+typedef MLAN_PACK_START struct _HostCmd_DS_TXBUF_CFG
+{
+    /** Action */
+    t_u16 action;
+    /** Buffer Size */
+    t_u16 buff_size;
+    /** End Port_for Multiport */
+    t_u16 mp_end_port;
+    /** Reserved */
+    t_u16 reserved3;
+} MLAN_PACK_END HostCmd_DS_TXBUF_CFG;
+
+/** HostCmd_DS_AMSDU_AGGR_CTRL */
+typedef MLAN_PACK_START struct _HostCmd_DS_AMSDU_AGGR_CTRL
+{
+    /** Action */
+    t_u16 action;
+    /** Enable */
+    t_u16 enable;
+    /** Get the current Buffer Size valid */
+    t_u16 curr_buf_size;
+} MLAN_PACK_END HostCmd_DS_AMSDU_AGGR_CTRL;
+
+/** HostCmd_DS_ECL_SYSTEM_CLOCK_CONFIG */
+typedef MLAN_PACK_START struct _HostCmd_DS_ECL_SYSTEM_CLOCK_CONFIG
+{
+    /** Action */
+    t_u16 action;
+    /** Current system clock */
+    t_u16 cur_sys_clk;
+    /** Clock type */
+    t_u16 sys_clk_type;
+    /** Length of clocks */
+    t_u16 sys_clk_len;
+    /** System clocks */
+    t_u16 sys_clk[16];
+} MLAN_PACK_END HostCmd_DS_ECL_SYSTEM_CLOCK_CONFIG;
+
+/** MrvlIEtypes_WmmParamSet_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_WmmParamSet_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** WMM IE */
+    t_u8 wmm_ie[1];
+} MLAN_PACK_END MrvlIEtypes_WmmParamSet_t;
+
+/** MrvlIEtypes_WmmQueueStatus_t */
+typedef MLAN_PACK_START struct
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Queue index */
+    t_u8 queue_index;
+    /** Disabled flag */
+    t_u8 disabled;
+    /** Medium time allocation in 32us units*/
+    t_u16 medium_time;
+    /** Flow required flag */
+    t_u8 flow_required;
+    /** Flow created flag */
+    t_u8 flow_created;
+    /** Reserved */
+    t_u32 reserved;
+} MLAN_PACK_END MrvlIEtypes_WmmQueueStatus_t;
+
+/** Size of a TSPEC.  Used to allocate necessary buffer space in commands */
+#define WMM_TSPEC_SIZE              63
+
+/** Maximum number of AC QOS queues available in the driver/firmware */
+#define MAX_AC_QUEUES               4
+
+/** Extra IE bytes allocated in messages for appended IEs after a TSPEC */
+#define WMM_ADDTS_EXTRA_IE_BYTES    256
+
+/** Extra TLV bytes allocated in messages for configuring WMM Queues */
+#define WMM_QUEUE_CONFIG_EXTRA_TLV_BYTES 64
+
+/** Number of bins in the histogram for the HostCmd_DS_WMM_QUEUE_STATS */
+#define WMM_STATS_PKTS_HIST_BINS  7
+
+/**
+ *  @brief Firmware command structure to retrieve the firmware WMM status.
+ *
+ *  Used to retrieve the status of each WMM AC Queue in TLV
+ *    format (MrvlIEtypes_WmmQueueStatus_t) as well as the current WMM
+ *    parameter IE advertised by the AP.
+ *
+ *  Used in response to a EVENT_WMM_STATUS_CHANGE event signaling
+ *    a QOS change on one of the ACs or a change in the WMM Parameter in
+ *    the Beacon.
+ *
+ *  TLV based command, byte arrays used for max sizing purpose. There are no
+ *    arguments sent in the command, the TLVs are returned by the firmware.
+ */
+typedef MLAN_PACK_START struct
+{
+    /** Queue status TLV */
+    t_u8 queue_status_tlv[sizeof(MrvlIEtypes_WmmQueueStatus_t)
+                          * MAX_AC_QUEUES];
+    /** WMM parameter TLV */
+    t_u8 wmm_param_tlv[sizeof(IEEEtypes_WmmParameter_t) + 2];
+}
+MLAN_PACK_END HostCmd_DS_WMM_GET_STATUS;
+
+/**
+ *  @brief Command structure for the HostCmd_CMD_WMM_ADDTS_REQ firmware command
+ */
+typedef MLAN_PACK_START struct
+{
+    mlan_cmd_result_e command_result;    /**< Command result */
+    t_u32 timeout_ms;                    /**< Timeout value in milliseconds */
+    t_u8 dialog_token;                   /**< Dialog token */
+    t_u8 ieee_status_code;               /**< IEEE status code */
+    t_u8 tspec_data[WMM_TSPEC_SIZE];     /**< TSPEC data */
+    t_u8 addts_extra_ie_buf[WMM_ADDTS_EXTRA_IE_BYTES];  /**< Extra IE buffer */
+} MLAN_PACK_END HostCmd_DS_WMM_ADDTS_REQ;
+
+/**
+ *  @brief Command structure for the HostCmd_CMD_WMM_DELTS_REQ firmware command
+ */
+typedef MLAN_PACK_START struct
+{
+    mlan_cmd_result_e command_result;   /**< Command result */
+    t_u8 dialog_token;                  /**< Dialog token */
+    t_u8 ieee_reason_code;              /**< IEEE reason code */
+    t_u8 tspec_data[WMM_TSPEC_SIZE];    /**< TSPEC data */
+} MLAN_PACK_END HostCmd_DS_WMM_DELTS_REQ;
+
+/**
+ *  @brief Command structure for the HostCmd_CMD_WMM_QUEUE_CONFIG firmware cmd
+ *
+ *  Set/Get/Default the Queue parameters for a specific AC in the firmware.
+ */
+typedef MLAN_PACK_START struct
+{
+    mlan_wmm_queue_config_action_e action; /**< Set, Get, or Default */
+    mlan_wmm_ac_e access_category;         /**< WMM_AC_BK(0) to WMM_AC_VO(3) */
+    /** @brief MSDU lifetime expiry per 802.11e
+     *
+     *   - Ignored if 0 on a set command
+     *   - Set to the 802.11e specified 500 TUs when defaulted
+     */
+    t_u16 msdu_lifetime_expiry;
+    t_u8 tlv_buffer[WMM_QUEUE_CONFIG_EXTRA_TLV_BYTES];  /**< Not supported */
+} MLAN_PACK_END HostCmd_DS_WMM_QUEUE_CONFIG;
+
+/**
+ *  @brief Command structure for the HostCmd_CMD_WMM_QUEUE_STATS firmware cmd
+ *
+ *  Turn statistical collection on/off for a given AC or retrieve the
+ *    accumulated stats for an AC and clear them in the firmware.
+ */
+typedef MLAN_PACK_START struct
+{
+    mlan_wmm_queue_stats_action_e action;  /**< Start, Stop, or Get */
+#ifdef BIG_ENDIAN_SUPPORT
+    t_u8 select_bin:7;          /**< WMM_AC_BK(0) to WMM_AC_VO(3), or TID */
+    t_u8 select_is_userpri:1;   /**< Set if select_bin is UP, Clear for AC */
+#else
+    t_u8 select_is_userpri:1;   /**< Set if select_bin is UP, Clear for AC */
+    t_u8 select_bin:7;          /**< WMM_AC_BK(0) to WMM_AC_VO(3), or TID */
+#endif
+    t_u16 pkt_count;        /**< Number of successful packets transmitted */
+    t_u16 pkt_loss;         /**< Packets lost; not included in pktCount */
+    t_u32 avg_queue_delay;  /**< Average Queue delay in microsec */
+    t_u32 avg_tx_delay;     /**< Average Transmission delay in microsec */
+    t_u16 used_time;        /**< Calc used time - units of 32 microsec */
+    t_u16 policed_time;     /**< Calc policed time - units of 32 microsec */
+    /** @brief Queue Delay Histogram; number of packets per queue delay range
+     *
+     *  [0] -  0ms <= delay < 5ms
+     *  [1] -  5ms <= delay < 10ms
+     *  [2] - 10ms <= delay < 20ms
+     *  [3] - 20ms <= delay < 30ms
+     *  [4] - 30ms <= delay < 40ms
+     *  [5] - 40ms <= delay < 50ms
+     *  [6] - 50ms <= delay < msduLifetime (TUs)
+     */
+    t_u16 delay_histogram[WMM_STATS_PKTS_HIST_BINS];
+    /** Reserved */
+    t_u16 reserved_1;
+} MLAN_PACK_END HostCmd_DS_WMM_QUEUE_STATS;
+
+/**
+ *  @brief Command structure for the HostCmd_CMD_WMM_TS_STATUS firmware cmd
+ *
+ *  Query the firmware to get the status of the WMM Traffic Streams
+ */
+typedef MLAN_PACK_START struct
+{
+    /** TSID: Range: 0->7 */
+    t_u8 tid;
+    /** TSID specified is valid */
+    t_u8 valid;
+    /** AC TSID is active on */
+    t_u8 access_category;
+    /** UP specified for the TSID */
+    t_u8 user_priority;
+    /** Power save mode for TSID: 0 (legacy), 1 (UAPSD) */
+    t_u8 psb;
+    /** Uplink(1), Downlink(2), Bidirectional(3) */
+    t_u8 flow_dir;
+    /** Medium time granted for the TSID */
+    t_u16 medium_time;
+} MLAN_PACK_END HostCmd_DS_WMM_TS_STATUS;
+
+/** Firmware status for a specific AC */
+typedef MLAN_PACK_START struct
+{
+    /** Disabled flag */
+    t_u8 disabled;
+    /** Flow required flag */
+    t_u8 flow_required;
+    /** Flow created flag */
+    t_u8 flow_created;
+} MLAN_PACK_END WmmAcStatus_t;
+
+/**  Local Power Capability */
+typedef MLAN_PACK_START struct _MrvlIEtypes_PowerCapability_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Minmum power */
+    t_s8 min_power;
+    /** Maximum power */
+    t_s8 max_power;
+} MLAN_PACK_END MrvlIEtypes_PowerCapability_t;
+
+/** HT Capabilities element */
+typedef MLAN_PACK_START struct _MrvlIETypes_HTCap_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+
+    /** HTCap struct */
+    HTCap_t ht_cap;
+} MLAN_PACK_END MrvlIETypes_HTCap_t;
+
+/** HT Information element */
+typedef MLAN_PACK_START struct _MrvlIETypes_HTInfo_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+
+    /** HTInfo struct */
+    HTInfo_t ht_info;
+} MLAN_PACK_END MrvlIETypes_HTInfo_t;
+
+/** 20/40 BSS Coexistence element */
+typedef MLAN_PACK_START struct _MrvlIETypes_2040BSSCo_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+
+    /** BSSCo2040_t struct */
+    BSSCo2040_t bss_co_2040;
+} MLAN_PACK_END MrvlIETypes_2040BSSCo_t;
+
+/** Extended Capabilities element */
+typedef MLAN_PACK_START struct _MrvlIETypes_ExtCap_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+
+    /** ExtCap_t struct */
+    ExtCap_t ext_cap;
+} MLAN_PACK_END MrvlIETypes_ExtCap_t;
+
+/** Overlapping BSS Scan Parameters element */
+typedef MLAN_PACK_START struct _MrvlIETypes_OverlapBSSScanParam_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+
+    /** OBSSScanParam_t struct */
+    OBSSScanParam_t obss_scan_param;
+} MLAN_PACK_END MrvlIETypes_OverlapBSSScanParam_t;
+
+/** Set of MCS values that STA desires to use within the BSS */
+typedef MLAN_PACK_START struct _MrvlIETypes_HTOperationalMCSSet_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+
+    /** Bitmap indicating MCSs that STA desires to use within the BSS */
+    t_u8 ht_operational_mcs_bitmap[16];
+} MLAN_PACK_END MrvlIETypes_HTOperationalMCSSet_t;
+
+/** bf global args */
+typedef struct MLAN_PACK_START _bf_global_cfg_args
+{
+    /** Global enable/disable bf */
+    t_u8 bf_enbl;
+    /** Global enable/disable sounding */
+    t_u8 sounding_enbl;
+    /** FB Type */
+    t_u8 fb_type;
+    /** SNR Threshold */
+    t_u8 snr_threshold;
+    /** Sounding interval */
+    t_u16 sounding_interval;
+    /** BF mode */
+    t_u8 bf_mode;
+    /** Reserved */
+    t_u8 reserved;
+} MLAN_PACK_END bf_global_cfg_args;
+
+/** bf_trigger_sound_args_t */
+typedef MLAN_PACK_START struct _bf_trigger_sound_args_t
+{
+    /** Peer MAC address */
+    t_u8 peer_mac[MLAN_MAC_ADDR_LENGTH];
+    /** Status */
+    t_u8 status;
+} MLAN_PACK_END bf_trigger_sound_args_t;
+
+/** bf periodicity args */
+typedef MLAN_PACK_START struct _bf_periodicity_args
+{
+    /** Peer MAC address */
+    t_u8 peer_mac[MLAN_MAC_ADDR_LENGTH];
+    /** Current Tx BF Interval */
+    t_u16 interval;
+    /** Status */
+    t_u8 status;
+} MLAN_PACK_END bf_periodicity_args;
+
+/** bf peer configuration args */
+typedef struct MLAN_PACK_START _bf_peer_args
+{
+    /** Peer MAC address */
+    t_u8 peer_mac[MLAN_MAC_ADDR_LENGTH];
+    /** Reserved */
+    t_u16 reserved;
+    /** Enable/Disable Beamforming */
+    t_u8 bf_enbl;
+    /** Enable/Disable sounding */
+    t_u8 sounding_enbl;
+    /** FB Type */
+    t_u8 fb_type;
+} MLAN_PACK_END bf_peer_args;
+
+/** bf_snr_thr_t */
+typedef MLAN_PACK_START struct _bf_snr_thr_t
+{
+    /** Peer MAC address */
+    t_u8 peer_mac[MLAN_MAC_ADDR_LENGTH];
+    /** SNR */
+    t_u8 snr;
+} MLAN_PACK_END bf_snr_thr_t;
+
+/** HostCmd_DS_TX_BF_CFG */
+typedef MLAN_PACK_START struct _HostCmd_DS_TX_BF_CFG
+{
+    /* Beamforming action */
+    t_u16 bf_action;
+    /* action - SET/GET */
+    t_u16 action;
+
+    MLAN_PACK_START union
+    {
+        bf_global_cfg_args bf_global_cfg;
+        bf_trigger_sound_args_t bf_sound_args;
+        bf_periodicity_args bf_periodicity;
+        bf_peer_args tx_bf_peer;
+        bf_snr_thr_t bf_snr;
+    } MLAN_PACK_END body;
+} MLAN_PACK_END HostCmd_DS_TX_BF_CFG;
+
+#ifdef WIFI_DIRECT_SUPPORT
+/** MrvlIEtypes_psk_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_psk_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** PSK */
+    t_u8 psk[MLAN_MAX_KEY_LENGTH];
+} MLAN_PACK_END MrvlIEtypes_psk_t;
+#endif /* WIFI_DIRECT_SUPPORT */
+
+/** MrvlIEtypes_PMK_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_PMK_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** PMK */
+    t_u8 pmk[1];
+} MLAN_PACK_END MrvlIEtypes_PMK_t;
+
+/** MrvlIEtypes_Passphrase_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_Passphrase_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Passphrase */
+    char passphrase[1];
+} MLAN_PACK_END MrvlIEtypes_Passphrase_t;
+
+/* unicastCipher -
+ *      Bit 0   : RFU
+ *      Bit 1   : RFU
+ *      Bit 2   : TKIP
+ *      Bit 3   : AES CCKM
+ *      Bit 2-7 : RFU
+ * multicastCipher -
+ *      Bit 0   : WEP40
+ *      Bit 1   : WEP104
+ *      Bit 2   : TKIP
+ *      Bit 3   : AES
+ *      Bit 4-7 : Reserved for now
+ */
+/** MrvlIEtypes_Cipher_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_Cipher_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** PairCipher */
+    t_u8 pair_cipher;
+    /** GroupCipher */
+    t_u8 group_cipher;
+} MLAN_PACK_END MrvlIEtypes_Cipher_t;
+
+/* rsnMode -
+ *      Bit 0    : No RSN
+ *      Bit 1-2  : RFU
+ *      Bit 3    : WPA
+ *      Bit 4    : WPA-NONE
+ *      Bit 5    : WPA2
+ *      Bit 6    : AES CCKM
+ *      Bit 7-15 : RFU
+ */
+/** MrvlIEtypes_EncrProto_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_EncrProto_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** EncrProto */
+    t_u16 rsn_mode;
+} MLAN_PACK_END MrvlIEtypes_EncrProto_t;
+
+/** MrvlIEtypes_Bssid_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_Bssid_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Bssid */
+    t_u8 bssid[MLAN_MAC_ADDR_LENGTH];
+} MLAN_PACK_END MrvlIEtypes_Bssid_t;
+
+/*
+ * This struct will handle GET,SET,CLEAR function for embedded
+ * supplicant.
+ * Define data structure for HostCmd_CMD_802_11_SUPPLICANT_PMK
+ */
+/** HostCmd_DS_802_11_SUPPLICANT_PMK */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_SUPPLICANT_PMK
+{
+    /** CMD Action GET/SET/CLEAR */
+    t_u16 action;
+    /** CacheResult initialized to 0 */
+    t_u16 cache_result;
+    /** TLV Buffer */
+    t_u8 tlv_buffer[1];
+    /** MrvlIEtypes_SsidParamSet_t  SsidParamSet;
+      * MrvlIEtypes_PMK_t           Pmk;
+      * MrvlIEtypes_Passphrase_t    Passphrase;
+      * MrvlIEtypes_Bssid_t         Bssid;
+      **/
+} MLAN_PACK_END HostCmd_DS_802_11_SUPPLICANT_PMK;
+
+/*
+ * This struct will GET the Supplicant supported bitmaps
+ * The GET_CURRENT action will get the network profile used
+ * for the current assocation.
+ * Define data structure for HostCmd_CMD_802_11_SUPPLICANT_PROFILE
+ */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_SUPPLICANT_PROFILE
+{
+    /** GET/SET/GET_CURRENT */
+    t_u16 action;
+    /** Reserved */
+    t_u16 reserved;
+    /** TLVBuffer */
+    t_u8 tlv_buf[1];
+    /* MrvlIEtypes_EncrProto_t */
+} MLAN_PACK_END HostCmd_DS_802_11_SUPPLICANT_PROFILE;
+
+/** HostCmd_CMD_802_11_RF_CHANNEL */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_RF_CHANNEL
+{
+    /** Action */
+    t_u16 action;
+    /** Current channel */
+    t_u16 current_channel;
+    /** RF type */
+    t_u16 rf_type;
+    /** Reserved field */
+    t_u16 reserved;
+#ifdef STA_SUPPORT
+    /** Reserved */
+    t_u8 reserved_1[32];
+#else                           /* STA_SUPPORT */
+    /** List of channels */
+    t_u8 channel_list[32];
+#endif                          /* !STA_SUPPORT */
+} MLAN_PACK_END HostCmd_DS_802_11_RF_CHANNEL;
+
+/** HostCmd_DS_VERSION_EXT */
+typedef MLAN_PACK_START struct _HostCmd_DS_VERSION_EXT
+{
+    /** Selected version string */
+    t_u8 version_str_sel;
+    /** Version string */
+    char version_str[128];
+} MLAN_PACK_END HostCmd_DS_VERSION_EXT;
+
+/** HostCmd_CMD_802_11_RF_ANTENNA */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_RF_ANTENNA
+{
+    /** Action for Tx antenna */
+    t_u16 action_tx;
+    /** Tx antenna mode Bit0:1, Bit1:2, Bit0-1:1+2, 0xffff: diversity */
+    t_u16 tx_antenna_mode;
+    /** Action for Rx antenna */
+    t_u16 action_rx;
+    /** Rx antenna mode Bit0:1, Bit1:2, Bit0-1:1+2, 0xffff: diversity */
+    t_u16 rx_antenna_mode;
+} MLAN_PACK_END HostCmd_DS_802_11_RF_ANTENNA;
+
+/** HostCmd_DS_802_11_IBSS_STATUS */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_IBSS_STATUS
+{
+    /** Action */
+    t_u16 action;
+    /** Enable */
+    t_u16 enable;
+    /** BSSID */
+    t_u8 bssid[MLAN_MAC_ADDR_LENGTH];
+    /** Beacon interval */
+    t_u16 beacon_interval;
+    /** ATIM window interval */
+    t_u16 atim_window;
+    /** User G rate protection */
+    t_u16 use_g_rate_protect;
+} MLAN_PACK_END HostCmd_DS_802_11_IBSS_STATUS;
+
+/** HostCmd_DS_MGMT_IE_LIST_CFG */
+typedef MLAN_PACK_START struct _HostCmd_DS_MGMT_IE_LIST
+{
+    /** Action */
+    t_u16 action;
+    /** Get/Set mgmt IE */
+    mlan_ds_misc_custom_ie ds_mgmt_ie;
+} MLAN_PACK_END HostCmd_DS_MGMT_IE_LIST_CFG;
+
+/** HostCmd_CMD_MAC_REG_ACCESS */
+typedef MLAN_PACK_START struct _HostCmd_DS_MAC_REG_ACCESS
+{
+    /** Action */
+    t_u16 action;
+    /** MAC register offset */
+    t_u16 offset;
+    /** MAC register value */
+    t_u32 value;
+} MLAN_PACK_END HostCmd_DS_MAC_REG_ACCESS;
+
+/** HostCmd_CMD_BBP_REG_ACCESS */
+typedef MLAN_PACK_START struct _HostCmd_DS_BBP_REG_ACCESS
+{
+    /** Acion */
+    t_u16 action;
+    /** BBP register offset */
+    t_u16 offset;
+    /** BBP register value */
+    t_u8 value;
+    /** Reserved field */
+    t_u8 reserved[3];
+} MLAN_PACK_END HostCmd_DS_BBP_REG_ACCESS;
+
+/**  HostCmd_CMD_RF_REG_ACCESS */
+typedef MLAN_PACK_START struct _HostCmd_DS_RF_REG_ACCESS
+{
+    /** Action */
+    t_u16 action;
+    /** RF register offset */
+    t_u16 offset;
+    /** RF register value */
+    t_u8 value;
+    /** Reserved field */
+    t_u8 reserved[3];
+} MLAN_PACK_END HostCmd_DS_RF_REG_ACCESS;
+
+/** HostCmd_DS_802_11_EEPROM_ACCESS */
+typedef MLAN_PACK_START struct _HostCmd_DS_802_11_EEPROM_ACCESS
+{
+        /** Action */
+    t_u16 action;
+
+        /** multiple 4 */
+    t_u16 offset;
+        /** Number of bytes */
+    t_u16 byte_count;
+        /** Value */
+    t_u8 value;
+} MLAN_PACK_END HostCmd_DS_802_11_EEPROM_ACCESS;
+
+/** HostCmd_DS_MEM_ACCESS */
+typedef MLAN_PACK_START struct _HostCmd_DS_MEM_ACCESS
+{
+        /** Action */
+    t_u16 action;
+        /** Reserved field */
+    t_u16 reserved;
+        /** Address */
+    t_u32 addr;
+        /** Value */
+    t_u32 value;
+} MLAN_PACK_END HostCmd_DS_MEM_ACCESS;
+
+/** HostCmd_DS_SUBSCRIBE_EVENT */
+typedef MLAN_PACK_START struct _HostCmd_DS_SUBSCRIBE_EVENT
+{
+    /** Action */
+    t_u16 action;
+    /** Bitmap of subscribed events */
+    t_u16 event_bitmap;
+} MLAN_PACK_END HostCmd_DS_SUBSCRIBE_EVENT;
+
+/** HostCmd_DS_OTP_USER_DATA */
+typedef MLAN_PACK_START struct _HostCmd_DS_OTP_USER_DATA
+{
+        /** Action */
+    t_u16 action;
+        /** Reserved field */
+    t_u16 reserved;
+        /** User data length */
+    t_u16 user_data_length;
+        /** User data */
+    t_u8 user_data[1];
+} MLAN_PACK_END HostCmd_DS_OTP_USER_DATA;
+
+/** HostCmd_DS_INACTIVITY_TIMEOUT_EXT */
+typedef MLAN_PACK_START struct _HostCmd_DS_INACTIVITY_TIMEOUT_EXT
+{
+    /** ACT_GET/ACT_SET */
+    t_u16 action;
+    /** uS, 0 means 1000uS(1ms) */
+    t_u16 timeout_unit;
+    /** Inactivity timeout for unicast data */
+    t_u16 unicast_timeout;
+    /** Inactivity timeout for multicast data */
+    t_u16 mcast_timeout;
+    /** Timeout for additional RX traffic after Null PM1 packet exchange */
+    t_u16 ps_entry_timeout;
+    /** Reserved to further expansion */
+    t_u16 reserved;
+} MLAN_PACK_END HostCmd_DS_INACTIVITY_TIMEOUT_EXT;
+
+/** TLV type : STA Mac address */
+#define TLV_TYPE_STA_MAC_ADDRESS      (PROPRIETARY_TLV_BASE_ID + 0x20)  // 0x0120
+
+/** MrvlIEtypes_MacAddr_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_MacAddr_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** mac address */
+    t_u8 mac[MLAN_MAC_ADDR_LENGTH];
+} MLAN_PACK_END MrvlIEtypes_MacAddr_t;
+
+/** Assoc Request */
+#define SUBTYPE_ASSOC_REQUEST        0
+/** ReAssoc Request */
+#define SUBTYPE_REASSOC_REQUEST      2
+/** Probe Resp */
+#define SUBTYPE_PROBE_RESP			5
+/** Disassoc Request */
+#define SUBTYPE_DISASSOC			10
+/** Auth Request */
+#define SUBTYPE_AUTH				11
+/** Deauth Request */
+#define SUBTYPE_DEAUTH              12
+/** Action frame */
+#define SUBTYPE_ACTION				13
+
+#ifdef UAP_SUPPORT
+/** TLV type : AP Channel band Config */
+#define TLV_TYPE_UAP_CHAN_BAND_CONFIG (PROPRIETARY_TLV_BASE_ID + 0x2a)  // 0x012a
+/** TLV type : AP Mac address */
+#define TLV_TYPE_UAP_MAC_ADDRESS      (PROPRIETARY_TLV_BASE_ID + 0x2b)  // 0x012b
+/** TLV type : AP Beacon period */
+#define TLV_TYPE_UAP_BEACON_PERIOD    (PROPRIETARY_TLV_BASE_ID + 0x2c)  // 0x012c
+/** TLV type : AP DTIM period */
+#define TLV_TYPE_UAP_DTIM_PERIOD      (PROPRIETARY_TLV_BASE_ID + 0x2d)  // 0x012d
+/** TLV type : AP Tx power */
+#define TLV_TYPE_UAP_TX_POWER         (PROPRIETARY_TLV_BASE_ID + 0x2f)  // 0x012f
+/** TLV type : AP SSID broadcast control */
+#define TLV_TYPE_UAP_BCAST_SSID_CTL   (PROPRIETARY_TLV_BASE_ID + 0x30)  // 0x0130
+/** TLV type : AP Preamble control */
+#define TLV_TYPE_UAP_PREAMBLE_CTL     (PROPRIETARY_TLV_BASE_ID + 0x31)  // 0x0131
+/** TLV type : AP Antenna control */
+#define TLV_TYPE_UAP_ANTENNA_CTL      (PROPRIETARY_TLV_BASE_ID + 0x32)  // 0x0132
+/** TLV type : AP RTS threshold */
+#define TLV_TYPE_UAP_RTS_THRESHOLD    (PROPRIETARY_TLV_BASE_ID + 0x33)  // 0x0133
+/** TLV type : AP Tx data rate */
+#define TLV_TYPE_UAP_TX_DATA_RATE     (PROPRIETARY_TLV_BASE_ID + 0x35)  // 0x0135
+/** TLV type: AP Packet forwarding control */
+#define TLV_TYPE_UAP_PKT_FWD_CTL      (PROPRIETARY_TLV_BASE_ID + 0x36)  // 0x0136
+/** TLV type: STA information */
+#define TLV_TYPE_UAP_STA_INFO         (PROPRIETARY_TLV_BASE_ID + 0x37)  // 0x0137
+/** TLV type: AP STA MAC address filter */
+#define TLV_TYPE_UAP_STA_MAC_ADDR_FILTER (PROPRIETARY_TLV_BASE_ID + 0x38)       // 0x0138
+/** TLV type: AP STA ageout timer */
+#define TLV_TYPE_UAP_STA_AGEOUT_TIMER (PROPRIETARY_TLV_BASE_ID + 0x39)  // 0x0139
+/** TLV type: AP WEP keys */
+#define TLV_TYPE_UAP_WEP_KEY          (PROPRIETARY_TLV_BASE_ID + 0x3b)  // 0x013b
+/** TLV type: AP WPA passphrase */
+#define TLV_TYPE_UAP_WPA_PASSPHRASE   (PROPRIETARY_TLV_BASE_ID + 0x3c)  // 0x013c
+/** TLV type: AP protocol */
+#define TLV_TYPE_UAP_ENCRYPT_PROTOCOL (PROPRIETARY_TLV_BASE_ID + 0x40)  // 0x0140
+/** TLV type: AP AKMP */
+#define TLV_TYPE_UAP_AKMP             (PROPRIETARY_TLV_BASE_ID + 0x41)  // 0x0141
+/** TLV type: AP Fragment threshold */
+#define TLV_TYPE_UAP_FRAG_THRESHOLD   (PROPRIETARY_TLV_BASE_ID + 0x46)  // 0x0146
+/** TLV type: AP Group rekey timer */
+#define TLV_TYPE_UAP_GRP_REKEY_TIME   (PROPRIETARY_TLV_BASE_ID + 0x47)  // 0x0147
+/**TLV type : AP Max Station number */
+#define TLV_TYPE_UAP_MAX_STA_CNT      (PROPRIETARY_TLV_BASE_ID + 0x55)  // 0x0155
+/**TLV type : AP Retry limit */
+#define TLV_TYPE_UAP_RETRY_LIMIT      (PROPRIETARY_TLV_BASE_ID + 0x5d)  // 0x015d
+/** TLV type : AP MCBC data rate */
+#define TLV_TYPE_UAP_MCBC_DATA_RATE   (PROPRIETARY_TLV_BASE_ID + 0x62)  // 0x0162
+/**TLV type: AP RSN replay protection */
+#define TLV_TYPE_UAP_RSN_REPLAY_PROTECT  (PROPRIETARY_TLV_BASE_ID + 0x64)       // 0x0164
+/** TLV ID : Management Frame */
+#define TLV_TYPE_UAP_MGMT_FRAME          (PROPRIETARY_TLV_BASE_ID + 0x68)       // 0x0168
+#ifdef UAP_SUPPORT
+/**TLV type: AP mgmt IE passthru mask */
+#define TLV_TYPE_UAP_MGMT_IE_PASSTHRU_MASK  (PROPRIETARY_TLV_BASE_ID + 0x70)    // 0x0170
+#endif
+/** TLV : 20/40 coex config */
+#define TLV_TYPE_2040_BSS_COEX_CONTROL      (PROPRIETARY_TLV_BASE_ID + 0x98)    // 0x0198
+/**TLV type: AP pairwise handshake timeout */
+#define TLV_TYPE_UAP_EAPOL_PWK_HSK_TIMEOUT  (PROPRIETARY_TLV_BASE_ID + 0x75)    // 0x0175
+/**TLV type: AP pairwise handshake retries */
+#define TLV_TYPE_UAP_EAPOL_PWK_HSK_RETRIES  (PROPRIETARY_TLV_BASE_ID + 0x76)    // 0x0176
+/**TLV type: AP groupwise handshake timeout */
+#define TLV_TYPE_UAP_EAPOL_GWK_HSK_TIMEOUT  (PROPRIETARY_TLV_BASE_ID + 0x77)    // 0x0177
+/**TLV type: AP groupwise handshake retries */
+#define TLV_TYPE_UAP_EAPOL_GWK_HSK_RETRIES  (PROPRIETARY_TLV_BASE_ID + 0x78)    // 0x0178
+/** TLV type: AP PS STA ageout timer */
+#define TLV_TYPE_UAP_PS_STA_AGEOUT_TIMER (PROPRIETARY_TLV_BASE_ID + 0x7b)       // 0x017b
+/** TLV type : Pairwise Cipher */
+#define TLV_TYPE_PWK_CIPHER              (PROPRIETARY_TLV_BASE_ID + 0x91)       // 0x0191
+/** TLV type : Group Cipher */
+#define TLV_TYPE_GWK_CIPHER              (PROPRIETARY_TLV_BASE_ID + 0x92)       // 0x0192
+/** TLV type : BSS Status */
+#define TLV_TYPE_BSS_STATUS              (PROPRIETARY_TLV_BASE_ID + 0x93)       // 0x0193
+
+#ifdef WIFI_DIRECT_SUPPORT
+/** TLV type : AP PSK */
+#define TLV_TYPE_UAP_PSK   (PROPRIETARY_TLV_BASE_ID + 0xa8)     // 0x01a8
+#endif /* WIFI_DIRECT_SUPPORT */
+
+/** MrvlIEtypes_beacon_period_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_beacon_period_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** beacon period */
+    t_u16 beacon_period;
+} MLAN_PACK_END MrvlIEtypes_beacon_period_t;
+
+/** MrvlIEtypes_dtim_period_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_dtim_period_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** DTIM period */
+    t_u8 dtim_period;
+} MLAN_PACK_END MrvlIEtypes_dtim_period_t;
+
+/** MrvlIEtypes_tx_rate_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_tx_rate_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** tx data rate */
+    t_u16 tx_data_rate;
+} MLAN_PACK_END MrvlIEtypes_tx_rate_t;
+
+/** MrvlIEtypes_mcbc_rate_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_mcbc_rate_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** mcbc data rate */
+    t_u16 mcbc_data_rate;
+} MLAN_PACK_END MrvlIEtypes_mcbc_rate_t;
+
+/** MrvlIEtypes_tx_power_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_tx_power_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** tx power */
+    t_u8 tx_power;
+} MLAN_PACK_END MrvlIEtypes_tx_power_t;
+
+/** MrvlIEtypes_bcast_ssid_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_bcast_ssid_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** bcast ssid control*/
+    t_u8 bcast_ssid_ctl;
+} MLAN_PACK_END MrvlIEtypes_bcast_ssid_t;
+
+/** MrvlIEtypes_antenna_mode_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_antenna_mode_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** which antenna */
+    t_u8 which_antenna;
+    /** antenna mode*/
+    t_u8 antenna_mode;
+} MLAN_PACK_END MrvlIEtypes_antenna_mode_t;
+
+/** MrvlIEtypes_pkt_forward_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_pkt_forward_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** pkt foward control */
+    t_u8 pkt_forward_ctl;
+} MLAN_PACK_END MrvlIEtypes_pkt_forward_t;
+
+/** MrvlIEtypes_max_sta_count_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_max_sta_count_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** max station count */
+    t_u16 max_sta_count;
+} MLAN_PACK_END MrvlIEtypes_max_sta_count_t;
+
+/** MrvlIEtypes_sta_ageout_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_sta_ageout_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** station age out timer */
+    t_u32 sta_ageout_timer;
+} MLAN_PACK_END MrvlIEtypes_sta_ageout_t;
+
+/** MrvlIEtypes_rts_threshold_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_rts_threshold_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** rts threshold */
+    t_u16 rts_threshold;
+} MLAN_PACK_END MrvlIEtypes_rts_threshold_t;
+
+/** MrvlIEtypes_frag_threshold_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_frag_threshold_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** frag threshold */
+    t_u16 frag_threshold;
+} MLAN_PACK_END MrvlIEtypes_frag_threshold_t;
+
+/** MrvlIEtypes_retry_limit_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_retry_limit_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** retry limit */
+    t_u8 retry_limit;
+} MLAN_PACK_END MrvlIEtypes_retry_limit_t;
+
+/** MrvlIEtypes_eapol_pwk_hsk_timeout_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_eapol_pwk_hsk_timeout_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** pairwise update timeout in milliseconds */
+    t_u32 pairwise_update_timeout;
+} MLAN_PACK_END MrvlIEtypes_eapol_pwk_hsk_timeout_t;
+
+/** MrvlIEtypes_eapol_pwk_hsk_retries_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_eapol_pwk_hsk_retries_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** pairwise handshake retries */
+    t_u32 pwk_retries;
+} MLAN_PACK_END MrvlIEtypes_eapol_pwk_hsk_retries_t;
+
+/** MrvlIEtypes_eapol_gwk_hsk_timeout_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_eapol_gwk_hsk_timeout_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** groupwise update timeout in milliseconds */
+    t_u32 groupwise_update_timeout;
+} MLAN_PACK_END MrvlIEtypes_eapol_gwk_hsk_timeout_t;
+
+/** MrvlIEtypes_eapol_gwk_hsk_retries_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_eapol_gwk_hsk_retries_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** groupwise handshake retries */
+    t_u32 gwk_retries;
+} MLAN_PACK_END MrvlIEtypes_eapol_gwk_hsk_retries_t;
+
+#ifdef UAP_SUPPORT
+/** MrvlIEtypes_mgmt_ie_passthru_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_mgmt_ie_passthru_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** mgmt IE mask value */
+    t_u32 mgmt_ie_mask;
+} MLAN_PACK_END MrvlIEtypes_mgmt_ie_passthru_t;
+#endif
+
+/** TLV buffer : 2040 coex config */
+typedef MLAN_PACK_START struct _MrvlIEtypes_2040_coex_enable_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Enable */
+    t_u8 enable_2040coex;
+} MLAN_PACK_END MrvlIEtypes_2040_coex_enable_t;
+
+/** MrvlIEtypes_mac_filter_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_mac_filter_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Filter mode */
+    t_u8 filter_mode;
+    /** Number of STA MACs */
+    t_u8 count;
+    /** STA MAC addresses buffer */
+    t_u8 mac_address[1];
+} MLAN_PACK_END MrvlIEtypes_mac_filter_t;
+
+/** setting for band_config - band=5GHZ */
+#define BAND_CONFIG_5GHZ        0x01
+
+/** MrvlIEtypes_retry_limit_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_channel_band_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Band Configuration
+     *
+     * [7-6] Channel Selection Mode; 00 manual, 01 ACS
+     * [3-2] Channel Width; 00 20 MHz
+     * [1-0] Band Info; 00 2.4 GHz
+     */
+    t_u8 band_config;
+    /** channel */
+    t_u8 channel;
+} MLAN_PACK_END MrvlIEtypes_channel_band_t;
+
+/** MrvlIEtypes_auth_type_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_auth_type_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Authentication type */
+    t_u8 auth_type;
+} MLAN_PACK_END MrvlIEtypes_auth_type_t;
+
+/** MrvlIEtypes_encrypt_protocol_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_encrypt_protocol_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** encryption protocol */
+    t_u16 protocol;
+} MLAN_PACK_END MrvlIEtypes_encrypt_protocol_t;
+
+/** MrvlIEtypes_pwk_cipher_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_pwk_cipher_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** protocol */
+    t_u16 protocol;
+    /** pairwise cipher */
+    t_u8 pairwise_cipher;
+    /** reserved */
+    t_u8 reserved;
+} MLAN_PACK_END MrvlIEtypes_pwk_cipher_t;
+
+/** MrvlIEtypes_gwk_cipher_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_gwk_cipher_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** group cipher */
+    t_u8 group_cipher;
+    /** reserved */
+    t_u8 reserved;
+} MLAN_PACK_END MrvlIEtypes_gwk_cipher_t;
+
+/** MrvlIEtypes_akmp_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_akmp_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** key management */
+    t_u16 key_mgmt;
+    /** key management operation */
+    t_u16 key_mgmt_operation;
+} MLAN_PACK_END MrvlIEtypes_akmp_t;
+
+/** MrvlIEtypes_passphrase_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_passphrase_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** passphrase */
+    t_u8 passphrase[1];
+} MLAN_PACK_END MrvlIEtypes_passphrase_t;
+
+/** MrvlIEtypes_rsn_replay_prot_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_rsn_replay_prot_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** rsn replay proection */
+    t_u8 rsn_replay_prot;
+} MLAN_PACK_END MrvlIEtypes_rsn_replay_prot_t;
+
+/** MrvlIEtypes_group_rekey_time_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_group_rekey_time_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** group key rekey time */
+    t_u32 gk_rekey_time;
+} MLAN_PACK_END MrvlIEtypes_group_rekey_time_t;
+
+/** MrvlIEtypes_wep_key_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_wep_key_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** key index */
+    t_u8 key_index;
+    /** is default */
+    t_u8 is_default;
+    /** key data */
+    t_u8 key[1];
+} MLAN_PACK_END MrvlIEtypes_wep_key_t;
+
+/** MrvlIEtypes_bss_status_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_bss_status_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** BSS status, READ only */
+    t_u16 bss_status;
+} MLAN_PACK_END MrvlIEtypes_bss_status_t;
+
+/** MrvlIEtypes_preamble_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_preamble_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** preamble type, READ only */
+    t_u8 preamble_type;
+} MLAN_PACK_END MrvlIEtypes_preamble_t;
+
+/** MrvlIEtypes_wmm_parameter_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_wmm_parameter_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** WMM parameter */
+    WmmParameter_t wmm_para;
+} MLAN_PACK_END MrvlIEtypes_wmm_parameter_t;
+
+/** SNMP_MIB_UAP_INDEX */
+typedef enum _SNMP_MIB_UAP_INDEX
+{
+    tkip_mic_failures = 0x0b,
+    ccmp_decrypt_errors = 0x0c,
+    wep_undecryptable_count = 0x0d,
+    wep_icv_error_count = 0x0e,
+    decrypt_failure_count = 0xf,
+    dot11_failed_count = 0x12,
+    dot11_retry_count = 0x13,
+    dot11_multi_retry_count = 0x14,
+    dot11_frame_dup_count = 0x15,
+    dot11_rts_success_count = 0x16,
+    dot11_rts_failure_count = 0x17,
+    dot11_ack_failure_count = 0x18,
+    dot11_rx_fragment_count = 0x19,
+    dot11_mcast_rx_frame_count = 0x1a,
+    dot11_fcs_error_count = 0x1b,
+    dot11_tx_frame_count = 0x1c,
+    dot11_rsna_tkip_cm_invoked = 0x1d,
+    dot11_rsna_4way_hshk_failures = 0x1e,
+    dot11_mcast_tx_count = 0x1f,
+} SNMP_MIB_UAP_INDEX;
+
+/** MrvlIEtypes_snmp_oid_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_snmp_oid_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** data */
+    t_u32 data;
+} MLAN_PACK_END MrvlIEtypes_snmp_oid_t;
+
+/** HostCmd_SYS_CONFIG */
+typedef MLAN_PACK_START struct _HostCmd_DS_SYS_CONFIG
+{
+        /** CMD Action GET/SET*/
+    t_u16 action;
+        /** Tlv buffer */
+    t_u8 tlv_buffer[1];
+} MLAN_PACK_END HostCmd_DS_SYS_CONFIG;
+
+/** HostCmd_SYS_CONFIG */
+typedef MLAN_PACK_START struct _HostCmd_DS_SYS_INFO
+{
+    /** sys info */
+    t_u8 sys_info[64];
+} MLAN_PACK_END HostCmd_DS_SYS_INFO;
+
+/** HostCmd_DS_STA_DEAUTH */
+typedef MLAN_PACK_START struct _HostCmd_DS_STA_DEAUTH
+{
+    /** mac address */
+    t_u8 mac[MLAN_MAC_ADDR_LENGTH];
+    /** reason code */
+    t_u16 reason;
+} MLAN_PACK_END HostCmd_DS_STA_DEAUTH;
+
+/** Host Command id: POWER_MGMT  */
+#define HOST_CMD_POWER_MGMT_EXT                   0x00ef
+/** TLV type: AP Sleep param */
+#define TLV_TYPE_AP_SLEEP_PARAM         (PROPRIETARY_TLV_BASE_ID + 0x6a)        // 0x016a
+/** TLV type: AP Inactivity Sleep param */
+#define TLV_TYPE_AP_INACT_SLEEP_PARAM   (PROPRIETARY_TLV_BASE_ID + 0x6b)        // 0x016b
+
+/** MrvlIEtypes_sleep_param_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_sleep_param_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** control bitmap */
+    t_u32 ctrl_bitmap;
+    /** min_sleep */
+    t_u32 min_sleep;
+    /** max_sleep */
+    t_u32 max_sleep;
+} MLAN_PACK_END MrvlIEtypes_sleep_param_t;
+
+/** MrvlIEtypes_inact_sleep_param_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_inact_sleep_param_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** inactivity timeout */
+    t_u32 inactivity_to;
+
+    /** min_awake */
+    t_u32 min_awake;
+    /** max_awake */
+    t_u32 max_awake;
+} MLAN_PACK_END MrvlIEtypes_inact_sleep_param_t;
+
+/** HostCmd_DS_POWER_MGMT */
+typedef MLAN_PACK_START struct _HostCmd_DS_POWER_MGMT_EXT
+{
+    /** CMD Action Get/Set*/
+    t_u16 action;
+    /** power mode */
+    t_u16 power_mode;
+} MLAN_PACK_END HostCmd_DS_POWER_MGMT_EXT;
+
+/** MrvlIEtypes_ps_sta_ageout_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_ps_sta_ageout_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** station age out timer */
+    t_u32 ps_sta_ageout_timer;
+} MLAN_PACK_END MrvlIEtypes_ps_sta_ageout_t;
+
+/** MrvlIEtypes_sta_info_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_sta_info_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** STA MAC address */
+    t_u8 mac_address[MLAN_MAC_ADDR_LENGTH];
+    /** Power mfg status */
+    t_u8 power_mfg_status;
+    /** RSSI */
+    t_s8 rssi;
+} MLAN_PACK_END MrvlIEtypes_sta_info_t;
+
+/** HostCmd_DS_STA_LIST */
+typedef MLAN_PACK_START struct _HostCmd_DS_STA_LIST
+{
+    /** Number of STAs */
+    t_u16 sta_count;
+    /* MrvlIEtypes_sta_info_t sta_info[0]; */
+} MLAN_PACK_END HostCmd_DS_STA_LIST;
+
+/** TLV ID : WAPI Information */
+#define TLV_TYPE_AP_WAPI_INFO         (PROPRIETARY_TLV_BASE_ID + 0x67)  // 0x0167
+
+/** MrvlIEtypes_sta_info_t */
+typedef MLAN_PACK_START struct _MrvlIEtypes_wapi_info_t
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Multicast PN */
+    t_u8 multicast_PN[16];
+} MLAN_PACK_END MrvlIEtypes_wapi_info_t;
+#endif /* UAP_SUPPORT */
+
+/**
+ * @brief 802.11h Local Power Constraint Marvell extended TLV
+ */
+typedef MLAN_PACK_START struct
+{
+    MrvlIEtypesHeader_t header; /**< Marvell TLV header: ID/Len */
+    t_u8 chan;                  /**< Channel local constraint applies to */
+
+    /** Power constraint included in beacons and used by fw to offset 11d info */
+    t_u8 constraint;
+
+} MLAN_PACK_END MrvlIEtypes_LocalPowerConstraint_t;
+
+/*
+ *
+ * Data structures for driver/firmware command processing
+ *
+ */
+
+/**  TPC Info structure sent in CMD_802_11_TPC_INFO command to firmware */
+typedef MLAN_PACK_START struct
+{
+    MrvlIEtypes_LocalPowerConstraint_t local_constraint;  /**< Local constraint */
+    MrvlIEtypes_PowerCapability_t power_cap;              /**< Power Capability */
+
+} MLAN_PACK_END HostCmd_DS_802_11_TPC_INFO;
+
+/**  TPC Request structure sent in CMD_802_11_TPC_ADAPT_REQ command to firmware */
+typedef MLAN_PACK_START struct
+{
+    t_u8 dest_mac[MLAN_MAC_ADDR_LENGTH];        /**< Destination STA address  */
+    t_u16 timeout;                              /**< Response timeout in ms */
+    t_u8 rate_index;                            /**< IEEE Rate index to send request */
+
+} MLAN_PACK_END HostCmd_TpcRequest;
+
+/**  TPC Response structure received from the CMD_802_11_TPC_ADAPT_REQ command */
+typedef MLAN_PACK_START struct
+{
+    t_u8 tpc_ret_code;      /**< Firmware command result status code */
+    t_s8 tx_power;          /**< Reported TX Power from the TPC Report element */
+    t_s8 link_margin;       /**< Reported link margin from the TPC Report element */
+    t_s8 rssi;              /**< RSSI of the received TPC Report frame */
+
+} MLAN_PACK_END HostCmd_TpcResponse;
+
+/**  CMD_802_11_TPC_ADAPT_REQ substruct. Union of the TPC request and response */
+typedef MLAN_PACK_START union
+{
+    HostCmd_TpcRequest req;   /**< Request struct sent to firmware */
+    HostCmd_TpcResponse resp; /**< Response struct received from firmware */
+
+} MLAN_PACK_END HostCmd_DS_802_11_TPC_ADAPT_REQ;
+
+/**  CMD_802_11_CHAN_SW_ANN firmware command substructure */
+typedef MLAN_PACK_START struct
+{
+    t_u8 switch_mode;   /**< Set to 1 for a quiet switch request, no STA tx */
+    t_u8 new_chan;      /**< Requested new channel */
+    t_u8 switch_count;  /**< Number of TBTTs until the switch is to occur */
+} MLAN_PACK_END HostCmd_DS_802_11_CHAN_SW_ANN;
+
+/**
+ * @brief Enumeration of measurement types, including max supported
+ *        enum for 11h/11k
+ */
+typedef MLAN_PACK_START enum _MeasType_t
+{
+    WLAN_MEAS_BASIC = 0,              /**< 11h: Basic */
+    WLAN_MEAS_NUM_TYPES,              /**< Number of enumerated measurements */
+    WLAN_MEAS_11H_MAX_TYPE = WLAN_MEAS_BASIC,          /**< Max 11h measurement */
+
+} MLAN_PACK_END MeasType_t;
+
+/**
+ * @brief Mode octet of the measurement request element (7.3.2.21)
+ */
+typedef MLAN_PACK_START struct
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    t_u8 rsvd5_7:3;               /**< Reserved */
+    t_u8 duration_mandatory:1;    /**< 11k: duration spec. for meas. is mandatory */
+    t_u8 report:1;                /**< 11h: en/disable report rcpt. of spec. type */
+    t_u8 request:1;               /**< 11h: en/disable requests of specified type */
+    t_u8 enable:1;                /**< 11h: enable report/request bits */
+    t_u8 parallel:1;              /**< 11k: series or parallel with previous meas */
+#else
+    t_u8 parallel:1;              /**< 11k: series or parallel with previous meas */
+    t_u8 enable:1;                /**< 11h: enable report/request bits */
+    t_u8 request:1;               /**< 11h: en/disable requests of specified type */
+    t_u8 report:1;                /**< 11h: en/disable report rcpt. of spec. type */
+    t_u8 duration_mandatory:1;    /**< 11k: duration spec. for meas. is mandatory */
+    t_u8 rsvd5_7:3;               /**< Reserved */
+#endif                          /* BIG_ENDIAN_SUPPORT */
+
+} MLAN_PACK_END MeasReqMode_t;
+
+/**
+ * @brief Common measurement request structure (7.3.2.21.1 to 7.3.2.21.3)
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 channel;      /**< Channel to measure */
+    t_u64 start_time;  /**< TSF Start time of measurement (0 for immediate) */
+    t_u16 duration;    /**< TU duration of the measurement */
+
+} MLAN_PACK_END MeasReqCommonFormat_t;
+
+/**
+ * @brief Basic measurement request structure (7.3.2.21.1)
+ */
+typedef MeasReqCommonFormat_t MeasReqBasic_t;
+
+/**
+ * @brief CCA measurement request structure (7.3.2.21.2)
+ */
+typedef MeasReqCommonFormat_t MeasReqCCA_t;
+
+/**
+ * @brief RPI measurement request structure (7.3.2.21.3)
+ */
+typedef MeasReqCommonFormat_t MeasReqRPI_t;
+
+/**
+ * @brief Union of the availble measurement request types.  Passed in the
+ *        driver/firmware interface.
+ */
+typedef union
+{
+    MeasReqBasic_t basic; /**< Basic measurement request */
+    MeasReqCCA_t cca;     /**< CCA measurement request */
+    MeasReqRPI_t rpi;     /**< RPI measurement request */
+
+} MeasRequest_t;
+
+/**
+ * @brief Mode octet of the measurement report element (7.3.2.22)
+ */
+typedef MLAN_PACK_START struct
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    t_u8 rsvd3_7:5;        /**< Reserved */
+    t_u8 refused:1;        /**< Measurement refused */
+    t_u8 incapable:1;      /**< Incapable of performing measurement */
+    t_u8 late:1;           /**< Start TSF time missed for measurement */
+#else
+    t_u8 late:1;           /**< Start TSF time missed for measurement */
+    t_u8 incapable:1;      /**< Incapable of performing measurement */
+    t_u8 refused:1;        /**< Measurement refused */
+    t_u8 rsvd3_7:5;        /**< Reserved */
+#endif                          /* BIG_ENDIAN_SUPPORT */
+
+} MLAN_PACK_END MeasRptMode_t;
+
+/**
+ * @brief Basic measurement report (7.3.2.22.1)
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 channel;              /**< Channel to measured */
+    t_u64 start_time;          /**< Start time (TSF) of measurement */
+    t_u16 duration;            /**< Duration of measurement in TUs */
+    MeasRptBasicMap_t map;     /**< Basic measurement report */
+
+} MLAN_PACK_END MeasRptBasic_t;
+
+/**
+ * @brief CCA measurement report (7.3.2.22.2)
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 channel;                /**< Channel to measured */
+    t_u64 start_time;            /**< Start time (TSF) of measurement */
+    t_u16 duration;              /**< Duration of measurement in TUs  */
+    t_u8 busy_fraction;          /**< Fractional duration CCA indicated chan busy */
+
+} MLAN_PACK_END MeasRptCCA_t;
+
+/**
+ * @brief RPI measurement report (7.3.2.22.3)
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 channel;              /**< Channel to measured  */
+    t_u64 start_time;          /**< Start time (TSF) of measurement */
+    t_u16 duration;            /**< Duration of measurement in TUs  */
+    t_u8 density[8];           /**< RPI Density histogram report */
+
+} MLAN_PACK_END MeasRptRPI_t;
+
+/**
+ * @brief Union of the availble measurement report types.  Passed in the
+ *        driver/firmware interface.
+ */
+typedef union
+{
+    MeasRptBasic_t basic;    /**< Basic measurement report */
+    MeasRptCCA_t cca;        /**< CCA measurement report */
+    MeasRptRPI_t rpi;        /**< RPI measurement report */
+
+} MeasReport_t;
+
+/**
+ * @brief Structure passed to firmware to perform a measurement
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 mac_addr[MLAN_MAC_ADDR_LENGTH];          /**< Reporting STA address */
+    t_u8 dialog_token;                   /**< Measurement dialog toke */
+    MeasReqMode_t req_mode;              /**< Report mode  */
+    MeasType_t meas_type;                /**< Measurement type */
+    MeasRequest_t req;                   /**< Measurement request data */
+
+} MLAN_PACK_END HostCmd_DS_MEASUREMENT_REQUEST;
+
+/**
+ * @brief Structure passed back from firmware with a measurement report,
+ *        also can be to send a measurement report to another STA
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 mac_addr[MLAN_MAC_ADDR_LENGTH];          /**< Reporting STA address */
+    t_u8 dialog_token;                   /**< Measurement dialog token */
+    MeasRptMode_t rpt_mode;              /**< Report mode */
+    MeasType_t meas_type;                /**< Measurement type */
+    MeasReport_t rpt;                    /**< Measurement report data */
+
+} MLAN_PACK_END HostCmd_DS_MEASUREMENT_REPORT;
+
+typedef MLAN_PACK_START struct
+{
+    t_u16 startFreq;
+    t_u8 chanWidth;
+    t_u8 chanNum;
+
+} MLAN_PACK_END MrvlChannelDesc_t;
+
+typedef MLAN_PACK_START struct
+{
+    MrvlIEtypesHeader_t Header;      /**< Header */
+
+    MeasRptBasicMap_t map;           /**< IEEE 802.11h basic meas report */
+} MLAN_PACK_END MrvlIEtypes_ChanRpt11hBasic_t;
+
+typedef MLAN_PACK_START struct
+{
+    MrvlChannelDesc_t chan_desc;  /**< Channel band, number */
+    t_u32 millisec_dwell_time; /**< Channel dwell time in milliseconds */
+} MLAN_PACK_END HostCmd_DS_CHAN_RPT_REQ;
+
+typedef MLAN_PACK_START struct
+{
+    t_u32 cmd_result;     /**< Rpt request command result (0 == SUCCESS) */
+    t_u64 start_tsf;      /**< TSF Measurement started */
+    t_u32 duration;       /**< Duration of measurement in microsecs */
+    t_u8 tlv_buffer[1];   /**< TLV Buffer */
+} MLAN_PACK_END HostCmd_DS_CHAN_RPT_RSP;
+
+/** statistics threshold */
+typedef MLAN_PACK_START struct
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** value */
+    t_u8 value;
+    /** reporting frequency */
+    t_u8 frequency;
+} MLAN_PACK_END MrvlIEtypes_BeaconHighRssiThreshold_t,
+    MrvlIEtypes_BeaconLowRssiThreshold_t,
+    MrvlIEtypes_BeaconHighSnrThreshold_t,
+    MrvlIEtypes_BeaconLowSnrThreshold_t,
+    MrvlIEtypes_FailureCount_t,
+    MrvlIEtypes_DataLowRssiThreshold_t,
+    MrvlIEtypes_DataHighRssiThreshold_t,
+    MrvlIEtypes_DataLowSnrThreshold_t,
+    MrvlIEtypes_DataHighSnrThreshold_t,
+    MrvlIETypes_PreBeaconMissed_t, MrvlIEtypes_BeaconsMissed_t;
+
+/** statistics threshold for LinkQuality */
+typedef MLAN_PACK_START struct
+{
+    /** Header */
+    MrvlIEtypesHeader_t header;
+    /** Link SNR threshold (dB) */
+    t_u16 link_snr;
+    /** Link SNR frequency */
+    t_u16 link_snr_freq;
+    /* Second minimum rate value as per the rate table below */
+    t_u16 link_rate;
+    /* Second minimum rate frequency */
+    t_u16 link_rate_freq;
+    /* Tx latency value (us) */
+    t_u16 link_tx_latency;
+    /* Tx latency frequency */
+    t_u16 link_tx_lantency_freq;
+} MLAN_PACK_END MrvlIEtypes_LinkQualityThreshold_t;
+
+/** HostCmd_DS_COMMAND */
+typedef struct MLAN_PACK_START _HostCmd_DS_COMMAND
+{
+    /** Command Header : Command */
+    t_u16 command;
+    /** Command Header : Size */
+    t_u16 size;
+    /** Command Header : Sequence number */
+    t_u16 seq_num;
+    /** Command Header : Result */
+    t_u16 result;
+    /** Command Body */
+    union
+    {
+        /** Hardware specifications */
+        HostCmd_DS_GET_HW_SPEC hw_spec;
+        /** Cfg data */
+        HostCmd_DS_802_11_CFG_DATA cfg_data;
+        /** MAC control */
+        HostCmd_DS_MAC_CONTROL mac_ctrl;
+        /** MAC address */
+        HostCmd_DS_802_11_MAC_ADDRESS mac_addr;
+        /** MAC muticast address */
+        HostCmd_DS_MAC_MULTICAST_ADR mc_addr;
+        /** Get log */
+        HostCmd_DS_802_11_GET_LOG get_log;
+        /** RSSI information */
+        HostCmd_DS_802_11_RSSI_INFO rssi_info;
+        /** RSSI information response */
+        HostCmd_DS_802_11_RSSI_INFO_RSP rssi_info_rsp;
+        /** SNMP MIB */
+        HostCmd_DS_802_11_SNMP_MIB smib;
+        /** Radio control */
+        HostCmd_DS_802_11_RADIO_CONTROL radio;
+        /** RF channel */
+        HostCmd_DS_802_11_RF_CHANNEL rf_channel;
+        /** Tx rate query */
+        HostCmd_TX_RATE_QUERY tx_rate;
+        /** Tx rate configuration */
+        HostCmd_DS_TX_RATE_CFG tx_rate_cfg;
+        /** Tx power configuration */
+        HostCmd_DS_TXPWR_CFG txp_cfg;
+        /** RF Tx power configuration */
+        HostCmd_DS_802_11_RF_TX_POWER txp;
+
+        /** RF antenna */
+        HostCmd_DS_802_11_RF_ANTENNA antenna;
+        /** Enhanced power save command */
+        HostCmd_DS_802_11_PS_MODE_ENH psmode_enh;
+        HostCmd_DS_802_11_HS_CFG_ENH opt_hs_cfg;
+        /** Scan */
+        HostCmd_DS_802_11_SCAN scan;
+        /** Extended Scan */
+        HostCmd_DS_802_11_SCAN_EXT ext_scan;
+
+        /** Mgmt frame subtype mask */
+        HostCmd_DS_RX_MGMT_IND rx_mgmt_ind;
+        /** Scan response */
+        HostCmd_DS_802_11_SCAN_RSP scan_resp;
+
+        HostCmd_DS_802_11_BG_SCAN_CONFIG bg_scan_config;
+        HostCmd_DS_802_11_BG_SCAN_QUERY bg_scan_query;
+        HostCmd_DS_802_11_BG_SCAN_QUERY_RSP bg_scan_query_resp;
+        HostCmd_DS_SUBSCRIBE_EVENT subscribe_event;
+        HostCmd_DS_OTP_USER_DATA otp_user_data;
+        /** Associate */
+        HostCmd_DS_802_11_ASSOCIATE associate;
+
+        /** Associate response */
+        HostCmd_DS_802_11_ASSOCIATE_RSP associate_rsp;
+        /** Deauthenticate */
+        HostCmd_DS_802_11_DEAUTHENTICATE deauth;
+        /** Ad-Hoc start */
+        HostCmd_DS_802_11_AD_HOC_START adhoc_start;
+        /** Ad-Hoc start result */
+        HostCmd_DS_802_11_AD_HOC_START_RESULT adhoc_start_result;
+        /** Ad-Hoc join result */
+        HostCmd_DS_802_11_AD_HOC_JOIN_RESULT adhoc_join_result;
+        /** Ad-Hoc join */
+        HostCmd_DS_802_11_AD_HOC_JOIN adhoc_join;
+        /** Domain information */
+        HostCmd_DS_802_11D_DOMAIN_INFO domain_info;
+        /** Domain information response */
+        HostCmd_DS_802_11D_DOMAIN_INFO_RSP domain_info_resp;
+        HostCmd_DS_802_11_TPC_ADAPT_REQ tpc_req;
+        HostCmd_DS_802_11_TPC_INFO tpc_info;
+        HostCmd_DS_802_11_CHAN_SW_ANN chan_sw_ann;
+        HostCmd_DS_CHAN_RPT_REQ chan_rpt_req;
+        HostCmd_DS_MEASUREMENT_REQUEST meas_req;
+        HostCmd_DS_MEASUREMENT_REPORT meas_rpt;
+        /** Add BA request */
+        HostCmd_DS_11N_ADDBA_REQ add_ba_req;
+        /** Add BA response */
+        HostCmd_DS_11N_ADDBA_RSP add_ba_rsp;
+        /** Delete BA entry */
+        HostCmd_DS_11N_DELBA del_ba;
+        /** Tx buffer configuration */
+        HostCmd_DS_TXBUF_CFG tx_buf;
+        /** AMSDU Aggr Ctrl configuration */
+        HostCmd_DS_AMSDU_AGGR_CTRL amsdu_aggr_ctrl;
+        /** 11n configuration */
+        HostCmd_DS_11N_CFG htcfg;
+        /** 11n configuration */
+        HostCmd_DS_TX_BF_CFG tx_bf_cfg;
+        /** WMM status get */
+        HostCmd_DS_WMM_GET_STATUS get_wmm_status;
+        /** WMM ADDTS */
+        HostCmd_DS_WMM_ADDTS_REQ add_ts;
+        /** WMM DELTS */
+        HostCmd_DS_WMM_DELTS_REQ del_ts;
+        /** WMM set/get queue config */
+        HostCmd_DS_WMM_QUEUE_CONFIG queue_config;
+        /** WMM on/of/get queue statistics */
+        HostCmd_DS_WMM_QUEUE_STATS queue_stats;
+        /** WMM get traffic stream status */
+        HostCmd_DS_WMM_TS_STATUS ts_status;
+        /** Key material */
+        HostCmd_DS_802_11_KEY_MATERIAL key_material;
+        /** E-Supplicant PSK */
+        HostCmd_DS_802_11_SUPPLICANT_PMK esupplicant_psk;
+        /** E-Supplicant profile */
+        HostCmd_DS_802_11_SUPPLICANT_PROFILE esupplicant_profile;
+        /** Extended version */
+        HostCmd_DS_VERSION_EXT verext;
+        /** Adhoc Coalescing */
+        HostCmd_DS_802_11_IBSS_STATUS ibss_coalescing;
+        /** Mgmt IE list configuration */
+        HostCmd_DS_MGMT_IE_LIST_CFG mgmt_ie_list;
+        /** System clock configuration */
+        HostCmd_DS_ECL_SYSTEM_CLOCK_CONFIG sys_clock_cfg;
+        /** MAC register access */
+        HostCmd_DS_MAC_REG_ACCESS mac_reg;
+        /** BBP register access */
+        HostCmd_DS_BBP_REG_ACCESS bbp_reg;
+        /** RF register access */
+        HostCmd_DS_RF_REG_ACCESS rf_reg;
+        /** EEPROM register access */
+        HostCmd_DS_802_11_EEPROM_ACCESS eeprom;
+        /** Memory access */
+        HostCmd_DS_MEM_ACCESS mem;
+
+        /** Inactivity timeout extend */
+        HostCmd_DS_INACTIVITY_TIMEOUT_EXT inactivity_to;
+#ifdef UAP_SUPPORT
+        HostCmd_DS_SYS_CONFIG sys_config;
+        HostCmd_DS_SYS_INFO sys_info;
+        HostCmd_DS_STA_DEAUTH sta_deauth;
+        HostCmd_DS_STA_LIST sta_list;
+        HostCmd_DS_POWER_MGMT_EXT pm_cfg;
+#endif                          /* UAP_SUPPORT */
+
+       /** Sleep period command */
+        HostCmd_DS_802_11_SLEEP_PERIOD sleep_pd;
+       /** Sleep params command */
+        HostCmd_DS_802_11_SLEEP_PARAMS sleep_param;
+
+       /** SDIO GPIO interrupt config command */
+        HostCmd_DS_SDIO_GPIO_INT_CONFIG sdio_gpio_int;
+        HostCmd_DS_SDIO_PULL_CTRL sdio_pull_ctl;
+        HostCmd_DS_SET_BSS_MODE bss_mode;
+        HostCmd_DS_CMD_TX_DATA_PAUSE tx_data_pause;
+#ifdef WIFI_DIRECT_SUPPORT
+        HostCmd_DS_REMAIN_ON_CHANNEL remain_on_chan;
+        HostCmd_DS_WIFI_DIRECT_MODE wifi_direct_mode;
+#endif
+    } params;
+} MLAN_PACK_END HostCmd_DS_COMMAND;
+
+/** PS_CMD_ConfirmSleep */
+typedef MLAN_PACK_START struct _OPT_Confirm_Sleep
+{
+    /** Command */
+    t_u16 command;
+    /** Size */
+    t_u16 size;
+    /** Sequence number */
+    t_u16 seq_num;
+    /** Result */
+    t_u16 result;
+    /** Action */
+    t_u16 action;
+    /** Sleep comfirm param definition */
+    sleep_confirm_param sleep_cfm;
+} MLAN_PACK_END OPT_Confirm_Sleep;
+
+typedef struct MLAN_PACK_START _opt_sleep_confirm_buffer
+{
+    /** Header for interface */
+    t_u8 hdr[4];
+    /** New power save command used to send sleep confirmation to the firmware */
+    OPT_Confirm_Sleep ps_cfm_sleep;
+} MLAN_PACK_END opt_sleep_confirm_buffer;
+
+#ifdef PRAGMA_PACK
+#pragma pack(pop)
+#endif
+
+#endif /* !_MLAN_FW_H_ */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_ieee.h b/drivers/net/wireless/sd8797/mlan/mlan_ieee.h
new file mode 100644
index 000000000000..2431e06f48fa
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_ieee.h
@@ -0,0 +1,1322 @@
+/** @file mlan_ieee.h
+ *
+ *  @brief This file contains IEEE information element related 
+ *  definitions used in MLAN and MOAL module.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    11/03/2008: initial version
+******************************************************/
+
+#ifndef _MLAN_IEEE_H_
+#define _MLAN_IEEE_H_
+
+/** FIX IES size in beacon buffer */
+#define WLAN_802_11_FIXED_IE_SIZE 			12
+/** WLAN supported rates */
+#define WLAN_SUPPORTED_RATES                14
+
+/** WLAN supported rates extension*/
+#define WLAN_SUPPORTED_RATES_EXT            32
+
+/** Enumeration definition*/
+/** WLAN_802_11_NETWORK_TYPE */
+typedef enum _WLAN_802_11_NETWORK_TYPE
+{
+    Wlan802_11FH,
+    Wlan802_11DS,
+    /* Defined as upper bound */
+    Wlan802_11NetworkTypeMax
+} WLAN_802_11_NETWORK_TYPE;
+
+/** Maximum size of IEEE Information Elements */
+#define IEEE_MAX_IE_SIZE      256
+
+#ifdef BIG_ENDIAN_SUPPORT
+/** Frame control: Type Mgmt frame */
+#define IEEE80211_FC_MGMT_FRAME_TYPE_MASK    0x3000
+/** Frame control: SubType Mgmt frame */
+#define IEEE80211_GET_FC_MGMT_FRAME_SUBTYPE(fc) (((fc) & 0xF000) >> 12)
+#else
+/** Frame control: Type Mgmt frame */
+#define IEEE80211_FC_MGMT_FRAME_TYPE_MASK    0x000C
+/** Frame control: SubType Mgmt frame */
+#define IEEE80211_GET_FC_MGMT_FRAME_SUBTYPE(fc) (((fc) & 0x00F0) >> 4)
+#endif
+
+#ifdef PRAGMA_PACK
+#pragma pack(push, 1)
+#endif
+
+/** IEEE Type definitions  */
+typedef MLAN_PACK_START enum _IEEEtypes_ElementId_e
+{
+    SSID = 0,
+    SUPPORTED_RATES = 1,
+
+    FH_PARAM_SET = 2,
+    DS_PARAM_SET = 3,
+    CF_PARAM_SET = 4,
+
+    IBSS_PARAM_SET = 6,
+
+#ifdef STA_SUPPORT
+    COUNTRY_INFO = 7,
+#endif /* STA_SUPPORT */
+
+    POWER_CONSTRAINT = 32,
+    POWER_CAPABILITY = 33,
+    TPC_REQUEST = 34,
+    TPC_REPORT = 35,
+    SUPPORTED_CHANNELS = 36,
+    CHANNEL_SWITCH_ANN = 37,
+    QUIET = 40,
+    IBSS_DFS = 41,
+    HT_CAPABILITY = 45,
+    HT_OPERATION = 61,
+    BSSCO_2040 = 72,
+    OVERLAPBSSSCANPARAM = 74,
+    EXT_CAPABILITY = 127,
+
+    ERP_INFO = 42,
+
+    EXTENDED_SUPPORTED_RATES = 50,
+
+    VENDOR_SPECIFIC_221 = 221,
+    WMM_IE = VENDOR_SPECIFIC_221,
+
+    WPS_IE = VENDOR_SPECIFIC_221,
+
+    WPA_IE = VENDOR_SPECIFIC_221,
+    RSN_IE = 48,
+    VS_IE = VENDOR_SPECIFIC_221,
+    WAPI_IE = 68,
+} MLAN_PACK_END IEEEtypes_ElementId_e;
+
+/** IEEE IE header */
+typedef MLAN_PACK_START struct _IEEEtypes_Header_t
+{
+    /** Element ID */
+    t_u8 element_id;
+    /** Length */
+    t_u8 len;
+} MLAN_PACK_END IEEEtypes_Header_t, *pIEEEtypes_Header_t;
+
+/** Vendor specific IE header */
+typedef MLAN_PACK_START struct _IEEEtypes_VendorHeader_t
+{
+    /** Element ID */
+    t_u8 element_id;
+    /** Length */
+    t_u8 len;
+    /** OUI */
+    t_u8 oui[3];
+    /** OUI type */
+    t_u8 oui_type;
+    /** OUI subtype */
+    t_u8 oui_subtype;
+    /** Version */
+    t_u8 version;
+} MLAN_PACK_END IEEEtypes_VendorHeader_t, *pIEEEtypes_VendorHeader_t;
+
+/** Vendor specific IE */
+typedef MLAN_PACK_START struct _IEEEtypes_VendorSpecific_t
+{
+    /** Vendor specific IE header */
+    IEEEtypes_VendorHeader_t vend_hdr;
+    /** IE Max - size of previous fields */
+    t_u8 data[IEEE_MAX_IE_SIZE - sizeof(IEEEtypes_VendorHeader_t)];
+}
+MLAN_PACK_END IEEEtypes_VendorSpecific_t, *pIEEEtypes_VendorSpecific_t;
+
+/** IEEE IE */
+typedef MLAN_PACK_START struct _IEEEtypes_Generic_t
+{
+    /** Generic IE header */
+    IEEEtypes_Header_t ieee_hdr;
+    /** IE Max - size of previous fields */
+    t_u8 data[IEEE_MAX_IE_SIZE - sizeof(IEEEtypes_Header_t)];
+}
+MLAN_PACK_END IEEEtypes_Generic_t, *pIEEEtypes_Generic_t;
+
+/** Capability information mask */
+#define CAPINFO_MASK    (~(MBIT(15) | MBIT(14) |            \
+                           MBIT(12) | MBIT(11) | MBIT(9)))
+
+/** Capability Bit Map*/
+#ifdef BIG_ENDIAN_SUPPORT
+typedef MLAN_PACK_START struct _IEEEtypes_CapInfo_t
+{
+    t_u8 rsrvd1:2;
+    t_u8 dsss_ofdm:1;
+    t_u8 rsvrd2:2;
+    t_u8 short_slot_time:1;
+    t_u8 rsrvd3:1;
+    t_u8 spectrum_mgmt:1;
+    t_u8 chan_agility:1;
+    t_u8 pbcc:1;
+    t_u8 short_preamble:1;
+    t_u8 privacy:1;
+    t_u8 cf_poll_rqst:1;
+    t_u8 cf_pollable:1;
+    t_u8 ibss:1;
+    t_u8 ess:1;
+} MLAN_PACK_END IEEEtypes_CapInfo_t, *pIEEEtypes_CapInfo_t;
+#else
+typedef MLAN_PACK_START struct _IEEEtypes_CapInfo_t
+{
+    /** Capability Bit Map : ESS */
+    t_u8 ess:1;
+    /** Capability Bit Map : IBSS */
+    t_u8 ibss:1;
+    /** Capability Bit Map : CF pollable */
+    t_u8 cf_pollable:1;
+    /** Capability Bit Map : CF poll request */
+    t_u8 cf_poll_rqst:1;
+    /** Capability Bit Map : privacy */
+    t_u8 privacy:1;
+    /** Capability Bit Map : Short preamble */
+    t_u8 short_preamble:1;
+    /** Capability Bit Map : PBCC */
+    t_u8 pbcc:1;
+    /** Capability Bit Map : Channel agility */
+    t_u8 chan_agility:1;
+    /** Capability Bit Map : Spectrum management */
+    t_u8 spectrum_mgmt:1;
+    /** Capability Bit Map : Reserved */
+    t_u8 rsrvd3:1;
+    /** Capability Bit Map : Short slot time */
+    t_u8 short_slot_time:1;
+    /** Capability Bit Map : APSD */
+    t_u8 Apsd:1;
+    /** Capability Bit Map : Reserved */
+    t_u8 rsvrd2:1;
+    /** Capability Bit Map : DSS OFDM */
+    t_u8 dsss_ofdm:1;
+    /** Capability Bit Map : Reserved */
+    t_u8 rsrvd1:2;
+} MLAN_PACK_END IEEEtypes_CapInfo_t, *pIEEEtypes_CapInfo_t;
+#endif /* BIG_ENDIAN_SUPPORT */
+
+/** IEEEtypes_CfParamSet_t */
+typedef MLAN_PACK_START struct _IEEEtypes_CfParamSet_t
+{
+    /** CF peremeter : Element ID */
+    t_u8 element_id;
+    /** CF peremeter : Length */
+    t_u8 len;
+    /** CF peremeter : Count */
+    t_u8 cfp_cnt;
+    /** CF peremeter : Period */
+    t_u8 cfp_period;
+    /** CF peremeter : Maximum duration */
+    t_u16 cfp_max_duration;
+    /** CF peremeter : Remaining duration */
+    t_u16 cfp_duration_remaining;
+} MLAN_PACK_END IEEEtypes_CfParamSet_t, *pIEEEtypes_CfParamSet_t;
+
+/** IEEEtypes_IbssParamSet_t */
+typedef MLAN_PACK_START struct _IEEEtypes_IbssParamSet_t
+{
+    /** Element ID */
+    t_u8 element_id;
+    /** Length */
+    t_u8 len;
+    /** ATIM window value in milliseconds */
+    t_u16 atim_window;
+} MLAN_PACK_END IEEEtypes_IbssParamSet_t, *pIEEEtypes_IbssParamSet_t;
+
+/** IEEEtypes_SsParamSet_t */
+typedef MLAN_PACK_START union _IEEEtypes_SsParamSet_t
+{
+    /** SS parameter : CF parameter set */
+    IEEEtypes_CfParamSet_t cf_param_set;
+    /** SS parameter : IBSS parameter set */
+    IEEEtypes_IbssParamSet_t ibss_param_set;
+} MLAN_PACK_END IEEEtypes_SsParamSet_t, *pIEEEtypes_SsParamSet_t;
+
+/** IEEEtypes_FhParamSet_t */
+typedef MLAN_PACK_START struct _IEEEtypes_FhParamSet_t
+{
+    /** FH parameter : Element ID */
+    t_u8 element_id;
+    /** FH parameter : Length */
+    t_u8 len;
+    /** FH parameter : Dwell time in milliseconds */
+    t_u16 dwell_time;
+    /** FH parameter : Hop set */
+    t_u8 hop_set;
+    /** FH parameter : Hop pattern */
+    t_u8 hop_pattern;
+    /** FH parameter : Hop index */
+    t_u8 hop_index;
+} MLAN_PACK_END IEEEtypes_FhParamSet_t, *pIEEEtypes_FhParamSet_t;
+
+/** IEEEtypes_DsParamSet_t */
+typedef MLAN_PACK_START struct _IEEEtypes_DsParamSet_t
+{
+    /** DS parameter : Element ID */
+    t_u8 element_id;
+    /** DS parameter : Length */
+    t_u8 len;
+    /** DS parameter : Current channel */
+    t_u8 current_chan;
+} MLAN_PACK_END IEEEtypes_DsParamSet_t, *pIEEEtypes_DsParamSet_t;
+
+/** IEEEtypes_PhyParamSet_t */
+typedef MLAN_PACK_START union _IEEEtypes_PhyParamSet_t
+{
+    /** FH parameter set */
+    IEEEtypes_FhParamSet_t fh_param_set;
+    /** DS parameter set */
+    IEEEtypes_DsParamSet_t ds_param_set;
+} MLAN_PACK_END IEEEtypes_PhyParamSet_t, *pIEEEtypes_PhyParamSet_t;
+
+/** IEEEtypes_ERPInfo_t */
+typedef MLAN_PACK_START struct _IEEEtypes_ERPInfo_t
+{
+    /** Element ID */
+    t_u8 element_id;
+    /** Length */
+    t_u8 len;
+    /** ERP flags */
+    t_u8 erp_flags;
+} MLAN_PACK_END IEEEtypes_ERPInfo_t, *pIEEEtypes_ERPInfo_t;
+
+/** IEEEtypes_AId_t */
+typedef t_u16 IEEEtypes_AId_t;
+
+/** IEEEtypes_StatusCode_t */
+typedef t_u16 IEEEtypes_StatusCode_t;
+
+/** IEEEtypes_AssocRsp_t */
+typedef MLAN_PACK_START struct _IEEEtypes_AssocRsp_t
+{
+    /** Capability information */
+    IEEEtypes_CapInfo_t capability;
+    /** Association response status code */
+    IEEEtypes_StatusCode_t status_code;
+    /** Association ID */
+    IEEEtypes_AId_t a_id;
+    /** IE data buffer */
+    t_u8 ie_buffer[1];
+} MLAN_PACK_END IEEEtypes_AssocRsp_t, *pIEEEtypes_AssocRsp_t;
+
+/** 802.11 supported rates */
+typedef t_u8 WLAN_802_11_RATES[WLAN_SUPPORTED_RATES];
+
+/** cipher TKIP */
+#define WPA_CIPHER_TKIP		2
+/** cipher AES */
+#define WPA_CIPHER_AES_CCM	4
+/** AKM: 8021x */
+#define RSN_AKM_8021X		1
+/** AKM: PSK */
+#define RSN_AKM_PSK     	2
+
+/** wpa_suite_t */
+typedef MLAN_PACK_START struct _wpa_suite_t
+{
+    /** OUI */
+    t_u8 oui[3];
+    /** tyep */
+    t_u8 type;
+} MLAN_PACK_END wpa_suite, wpa_suite_mcast_t;
+
+/** wpa_suite_ucast_t */
+typedef MLAN_PACK_START struct
+{
+    /* count */
+    t_u16 count;
+    /** wpa_suite list */
+    wpa_suite list[1];
+} MLAN_PACK_END wpa_suite_ucast_t, wpa_suite_auth_key_mgmt_t;
+
+/** IEEEtypes_Rsn_t */
+typedef MLAN_PACK_START struct _IEEEtypes_Rsn_t
+{
+    /** Rsn : Element ID */
+    t_u8 element_id;
+    /** Rsn : Length */
+    t_u8 len;
+    /** Rsn : version */
+    t_u16 version;
+    /** Rsn : group cipher */
+    wpa_suite_mcast_t group_cipher;
+    /** Rsn : pairwise cipher */
+    wpa_suite_ucast_t pairwise_cipher;
+} MLAN_PACK_END IEEEtypes_Rsn_t, *pIEEEtypes_Rsn_t;
+
+/** IEEEtypes_Wpa_t */
+typedef MLAN_PACK_START struct _IEEEtypes_Wpa_t
+{
+    /** Wpa : Element ID */
+    t_u8 element_id;
+    /** Wpa : Length */
+    t_u8 len;
+    /** Wpa : oui */
+    t_u8 oui[4];
+    /** version */
+    t_u16 version;
+    /** Wpa : group cipher */
+    wpa_suite_mcast_t group_cipher;
+    /** Wpa : pairwise cipher */
+    wpa_suite_ucast_t pairwise_cipher;
+} MLAN_PACK_END IEEEtypes_Wpa_t, *pIEEEtypes_Wpa_t;
+
+/** Maximum number of AC QOS queues available in the driver/firmware */
+#define MAX_AC_QUEUES 4
+
+/** Data structure of WMM QoS information */
+typedef MLAN_PACK_START struct _IEEEtypes_WmmQosInfo_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** QoS UAPSD */
+    t_u8 qos_uapsd:1;
+    /** Reserved */
+    t_u8 reserved:3;
+    /** Parameter set count */
+    t_u8 para_set_count:4;
+#else
+    /** Parameter set count */
+    t_u8 para_set_count:4;
+    /** Reserved */
+    t_u8 reserved:3;
+    /** QoS UAPSD */
+    t_u8 qos_uapsd:1;
+#endif                          /* BIG_ENDIAN_SUPPORT */
+} MLAN_PACK_END IEEEtypes_WmmQosInfo_t, *pIEEEtypes_WmmQosInfo_t;
+
+/** Data structure of WMM Aci/Aifsn */
+typedef MLAN_PACK_START struct _IEEEtypes_WmmAciAifsn_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** Reserved */
+    t_u8 reserved:1;
+    /** Aci */
+    t_u8 aci:2;
+    /** Acm */
+    t_u8 acm:1;
+    /** Aifsn */
+    t_u8 aifsn:4;
+#else
+    /** Aifsn */
+    t_u8 aifsn:4;
+    /** Acm */
+    t_u8 acm:1;
+    /** Aci */
+    t_u8 aci:2;
+    /** Reserved */
+    t_u8 reserved:1;
+#endif                          /* BIG_ENDIAN_SUPPORT */
+} MLAN_PACK_END IEEEtypes_WmmAciAifsn_t, *pIEEEtypes_WmmAciAifsn_t;
+
+/** Data structure of WMM ECW */
+typedef MLAN_PACK_START struct _IEEEtypes_WmmEcw_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** Maximum Ecw */
+    t_u8 ecw_max:4;
+    /** Minimum Ecw */
+    t_u8 ecw_min:4;
+#else
+    /** Minimum Ecw */
+    t_u8 ecw_min:4;
+    /** Maximum Ecw */
+    t_u8 ecw_max:4;
+#endif                          /* BIG_ENDIAN_SUPPORT */
+} MLAN_PACK_END IEEEtypes_WmmEcw_t, *pIEEEtypes_WmmEcw_t;
+
+/** Data structure of WMM AC parameters  */
+typedef MLAN_PACK_START struct _IEEEtypes_WmmAcParameters_t
+{
+    IEEEtypes_WmmAciAifsn_t aci_aifsn;       /**< AciAifSn */
+    IEEEtypes_WmmEcw_t ecw;                 /**< Ecw */
+    t_u16 tx_op_limit;                        /**< Tx op limit */
+} MLAN_PACK_END IEEEtypes_WmmAcParameters_t, *pIEEEtypes_WmmAcParameters_t;
+
+/** Data structure of WMM Info IE  */
+typedef MLAN_PACK_START struct _IEEEtypes_WmmInfo_t
+{
+
+    /**
+     * WMM Info IE - Vendor Specific Header:
+     *   element_id  [221/0xdd]
+     *   Len         [7]
+     *   Oui         [00:50:f2]
+     *   OuiType     [2]
+     *   OuiSubType  [0]
+     *   Version     [1]
+     */
+    IEEEtypes_VendorHeader_t vend_hdr;
+
+    /** QoS information */
+    IEEEtypes_WmmQosInfo_t qos_info;
+
+} MLAN_PACK_END IEEEtypes_WmmInfo_t, *pIEEEtypes_WmmInfo_t;
+
+/** Data structure of WMM parameter IE  */
+typedef MLAN_PACK_START struct _IEEEtypes_WmmParameter_t
+{
+    /**
+     * WMM Parameter IE - Vendor Specific Header:
+     *   element_id  [221/0xdd]
+     *   Len         [24]
+     *   Oui         [00:50:f2]
+     *   OuiType     [2]
+     *   OuiSubType  [1]
+     *   Version     [1]
+     */
+    IEEEtypes_VendorHeader_t vend_hdr;
+
+    /** QoS information */
+    IEEEtypes_WmmQosInfo_t qos_info;
+    /** Reserved */
+    t_u8 reserved;
+
+    /** AC Parameters Record WMM_AC_BE, WMM_AC_BK, WMM_AC_VI, WMM_AC_VO */
+    IEEEtypes_WmmAcParameters_t ac_params[MAX_AC_QUEUES];
+} MLAN_PACK_END IEEEtypes_WmmParameter_t, *pIEEEtypes_WmmParameter_t;
+
+/** Enumerator for TSPEC direction */
+typedef MLAN_PACK_START enum _IEEEtypes_WMM_TSPEC_TS_Info_Direction_e
+{
+
+    TSPEC_DIR_UPLINK = 0,
+    TSPEC_DIR_DOWNLINK = 1,
+    /* 2 is a reserved value */
+    TSPEC_DIR_BIDIRECT = 3,
+
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_TS_Info_Direction_e;
+
+/** Enumerator for TSPEC PSB */
+typedef MLAN_PACK_START enum _IEEEtypes_WMM_TSPEC_TS_Info_PSB_e
+{
+
+    TSPEC_PSB_LEGACY = 0,
+    TSPEC_PSB_TRIG = 1,
+
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_TS_Info_PSB_e;
+
+/** Enumerator for TSPEC Ack Policy */
+typedef MLAN_PACK_START enum _IEEEtypes_WMM_TSPEC_TS_Info_AckPolicy_e
+{
+
+    TSPEC_ACKPOLICY_NORMAL = 0,
+    TSPEC_ACKPOLICY_NOACK = 1,
+    /* 2 is reserved */
+    TSPEC_ACKPOLICY_BLOCKACK = 3,
+
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_TS_Info_AckPolicy_e;
+
+/** Enumerator for TSPEC Trafffice type */
+typedef MLAN_PACK_START enum _IEEEtypes_WMM_TSPEC_TS_TRAFFIC_TYPE_e
+{
+
+    TSPEC_TRAFFIC_APERIODIC = 0,
+    TSPEC_TRAFFIC_PERIODIC = 1,
+
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_TS_TRAFFIC_TYPE_e;
+
+/** Data structure of WMM TSPEC information */
+typedef MLAN_PACK_START struct
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    t_u8 Reserved17_23:7;       // ! Reserved
+    t_u8 Schedule:1;
+    IEEEtypes_WMM_TSPEC_TS_Info_AckPolicy_e AckPolicy:2;
+    t_u8 UserPri:3;             // ! 802.1d User Priority
+    IEEEtypes_WMM_TSPEC_TS_Info_PSB_e PowerSaveBehavior:1;      // !
+                                                                // Legacy/Trigg
+    t_u8 Aggregation:1;         // ! Reserved
+    t_u8 AccessPolicy2:1;       // !
+    t_u8 AccessPolicy1:1;       // !
+    IEEEtypes_WMM_TSPEC_TS_Info_Direction_e Direction:2;
+    t_u8 TID:4;                 // ! Unique identifier
+    IEEEtypes_WMM_TSPEC_TS_TRAFFIC_TYPE_e TrafficType:1;
+#else
+    IEEEtypes_WMM_TSPEC_TS_TRAFFIC_TYPE_e TrafficType:1;
+    t_u8 TID:4;                 // ! Unique identifier
+    IEEEtypes_WMM_TSPEC_TS_Info_Direction_e Direction:2;
+    t_u8 AccessPolicy1:1;       // !
+    t_u8 AccessPolicy2:1;       // !
+    t_u8 Aggregation:1;         // ! Reserved
+    IEEEtypes_WMM_TSPEC_TS_Info_PSB_e PowerSaveBehavior:1;      // !
+                                                                // Legacy/Trigg
+    t_u8 UserPri:3;             // ! 802.1d User Priority
+    IEEEtypes_WMM_TSPEC_TS_Info_AckPolicy_e AckPolicy:2;
+    t_u8 Schedule:1;
+    t_u8 Reserved17_23:7;       // ! Reserved
+#endif
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_TS_Info_t;
+
+/** Data structure of WMM TSPEC Nominal Size */
+typedef MLAN_PACK_START struct
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    t_u16 Fixed:1;              // ! 1: Fixed size given in Size, 0: Var, size
+                                // is nominal
+    t_u16 Size:15;              // ! Nominal size in octets
+#else
+    t_u16 Size:15;              // ! Nominal size in octets
+    t_u16 Fixed:1;              // ! 1: Fixed size given in Size, 0: Var, size
+                                // is nominal
+#endif
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_NomMSDUSize_t;
+
+/** Data structure of WMM TSPEC SBWA */
+typedef MLAN_PACK_START struct
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    t_u16 Whole:3;              // ! Whole portion
+    t_u16 Fractional:13;        // ! Fractional portion
+#else
+    t_u16 Fractional:13;        // ! Fractional portion
+    t_u16 Whole:3;              // ! Whole portion
+#endif
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_SBWA;
+
+/** Data structure of WMM TSPEC Body */
+typedef MLAN_PACK_START struct
+{
+
+    IEEEtypes_WMM_TSPEC_TS_Info_t TSInfo;
+    IEEEtypes_WMM_TSPEC_NomMSDUSize_t NomMSDUSize;
+    t_u16 MaximumMSDUSize;
+    t_u32 MinServiceInterval;
+    t_u32 MaxServiceInterval;
+    t_u32 InactivityInterval;
+    t_u32 SuspensionInterval;
+    t_u32 ServiceStartTime;
+    t_u32 MinimumDataRate;
+    t_u32 MeanDataRate;
+    t_u32 PeakDataRate;
+    t_u32 MaxBurstSize;
+    t_u32 DelayBound;
+    t_u32 MinPHYRate;
+    IEEEtypes_WMM_TSPEC_SBWA SurplusBWAllowance;
+    t_u16 MediumTime;
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_Body_t;
+
+/** Data structure of WMM TSPEC all elements */
+typedef MLAN_PACK_START struct
+{
+    t_u8 ElementId;
+    t_u8 Len;
+    t_u8 OuiType[4];            /* 00:50:f2:02 */
+    t_u8 OuiSubType;            /* 01 */
+    t_u8 Version;
+
+    IEEEtypes_WMM_TSPEC_Body_t TspecBody;
+
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_t;
+
+/** WMM Action Category values */
+typedef MLAN_PACK_START enum _IEEEtypes_ActionCategory_e
+{
+
+    IEEE_MGMT_ACTION_CATEGORY_SPECTRUM_MGMT = 0,
+    IEEE_MGMT_ACTION_CATEGORY_QOS = 1,
+    IEEE_MGMT_ACTION_CATEGORY_DLS = 2,
+    IEEE_MGMT_ACTION_CATEGORY_BLOCK_ACK = 3,
+    IEEE_MGMT_ACTION_CATEGORY_PUBLIC = 4,
+    IEEE_MGMT_ACTION_CATEGORY_RADIO_RSRC = 5,
+    IEEE_MGMT_ACTION_CATEGORY_FAST_BSS_TRANS = 6,
+    IEEE_MGMT_ACTION_CATEGORY_HT = 7,
+
+    IEEE_MGMT_ACTION_CATEGORY_WNM = 10,
+    IEEE_MGMT_ACTION_CATEGORY_UNPROTECT_WNM = 11,
+
+    IEEE_MGMT_ACTION_CATEGORY_WMM_TSPEC = 17
+} MLAN_PACK_END IEEEtypes_ActionCategory_e;
+
+/** WMM TSPEC operations */
+typedef MLAN_PACK_START enum _IEEEtypes_WMM_Tspec_Action_e
+{
+
+    TSPEC_ACTION_CODE_ADDTS_REQ = 0,
+    TSPEC_ACTION_CODE_ADDTS_RSP = 1,
+    TSPEC_ACTION_CODE_DELTS = 2,
+
+} MLAN_PACK_END IEEEtypes_WMM_Tspec_Action_e;
+
+/** WMM TSPEC Category Action Base */
+typedef MLAN_PACK_START struct
+{
+
+    IEEEtypes_ActionCategory_e category;
+    IEEEtypes_WMM_Tspec_Action_e action;
+    t_u8 dialogToken;
+
+} MLAN_PACK_END IEEEtypes_WMM_Tspec_Action_Base_Tspec_t;
+
+/** WMM TSPEC AddTS request structure */
+typedef MLAN_PACK_START struct
+{
+
+    IEEEtypes_WMM_Tspec_Action_Base_Tspec_t tspecAct;
+    t_u8 statusCode;
+    IEEEtypes_WMM_TSPEC_t tspecIE;
+
+    /* Place holder for additional elements after the TSPEC */
+    t_u8 subElem[256];
+
+} MLAN_PACK_END IEEEtypes_Action_WMM_AddTsReq_t;
+
+/** WMM TSPEC AddTS response structure */
+typedef MLAN_PACK_START struct
+{
+    IEEEtypes_WMM_Tspec_Action_Base_Tspec_t tspecAct;
+    t_u8 statusCode;
+    IEEEtypes_WMM_TSPEC_t tspecIE;
+
+    /* Place holder for additional elements after the TSPEC */
+    t_u8 subElem[256];
+
+} MLAN_PACK_END IEEEtypes_Action_WMM_AddTsRsp_t;
+
+/** WMM TSPEC DelTS structure */
+typedef MLAN_PACK_START struct
+{
+    IEEEtypes_WMM_Tspec_Action_Base_Tspec_t tspecAct;
+    t_u8 reasonCode;
+    IEEEtypes_WMM_TSPEC_t tspecIE;
+
+} MLAN_PACK_END IEEEtypes_Action_WMM_DelTs_t;
+
+/** union of WMM TSPEC structures */
+typedef MLAN_PACK_START union
+{
+    IEEEtypes_WMM_Tspec_Action_Base_Tspec_t tspecAct;
+
+    IEEEtypes_Action_WMM_AddTsReq_t addTsReq;
+    IEEEtypes_Action_WMM_AddTsRsp_t addTsRsp;
+    IEEEtypes_Action_WMM_DelTs_t delTs;
+
+} MLAN_PACK_END IEEEtypes_Action_WMMAC_t;
+
+/** union of WMM TSPEC & Action category */
+typedef MLAN_PACK_START union
+{
+    IEEEtypes_ActionCategory_e category;
+
+    IEEEtypes_Action_WMMAC_t wmmAc;
+
+} MLAN_PACK_END IEEEtypes_ActionFrame_t;
+
+/** Data structure for subband set */
+typedef MLAN_PACK_START struct _IEEEtypes_SubbandSet_t
+{
+    /** First channel */
+    t_u8 first_chan;
+    /** Number of channels */
+    t_u8 no_of_chan;
+    /** Maximum Tx power in dBm */
+    t_u8 max_tx_pwr;
+} MLAN_PACK_END IEEEtypes_SubbandSet_t, *pIEEEtypes_SubbandSet_t;
+
+#ifdef STA_SUPPORT
+/** Data structure for Country IE */
+typedef MLAN_PACK_START struct _IEEEtypes_CountryInfoSet_t
+{
+    /** Element ID */
+    t_u8 element_id;
+    /** Length */
+    t_u8 len;
+    /** Country code */
+    t_u8 country_code[COUNTRY_CODE_LEN];
+    /** Set of subbands */
+    IEEEtypes_SubbandSet_t sub_band[1];
+} MLAN_PACK_END IEEEtypes_CountryInfoSet_t, *pIEEEtypes_CountryInfoSet_t;
+
+/** Data structure for Country IE full set */
+typedef MLAN_PACK_START struct _IEEEtypes_CountryInfoFullSet_t
+{
+    /** Element ID */
+    t_u8 element_id;
+    /** Length */
+    t_u8 len;
+    /** Country code */
+    t_u8 country_code[COUNTRY_CODE_LEN];
+    /** Set of subbands */
+    IEEEtypes_SubbandSet_t sub_band[MRVDRV_MAX_SUBBAND_802_11D];
+} MLAN_PACK_END IEEEtypes_CountryInfoFullSet_t,
+    *pIEEEtypes_CountryInfoFullSet_t;
+
+#endif /* STA_SUPPORT */
+
+/** HT Capabilities Data */
+typedef struct MLAN_PACK_START _HTCap_t
+{
+    /** HT Capabilities Info field */
+    t_u16 ht_cap_info;
+    /** A-MPDU Parameters field */
+    t_u8 ampdu_param;
+    /** Supported MCS Set field */
+    t_u8 supported_mcs_set[16];
+    /** HT Extended Capabilities field */
+    t_u16 ht_ext_cap;
+    /** Transmit Beamforming Capabilities field */
+    t_u32 tx_bf_cap;
+    /** Antenna Selection Capability field */
+    t_u8 asel;
+} MLAN_PACK_END HTCap_t, *pHTCap_t;
+
+/** HT Information Data */
+typedef struct MLAN_PACK_START _HTInfo_t
+{
+    /** Primary channel */
+    t_u8 pri_chan;
+    /** Field 2 */
+    t_u8 field2;
+    /** Field 3 */
+    t_u16 field3;
+    /** Field 4 */
+    t_u16 field4;
+    /** Bitmap indicating MCSs supported by all HT STAs in the BSS */
+    t_u8 basic_mcs_set[16];
+} MLAN_PACK_END HTInfo_t, *pHTInfo_t;
+
+/** 20/40 BSS Coexistence Data */
+typedef struct MLAN_PACK_START _BSSCo2040_t
+{
+    /** 20/40 BSS Coexistence value */
+    t_u8 bss_co_2040_value;
+} MLAN_PACK_END BSSCo2040_t, *pBSSCo2040_t;
+
+/** Extended Capabilities Data */
+typedef struct MLAN_PACK_START _ExtCap_t
+{
+    /** Extended Capabilities value */
+    t_u8 ext_cap_value;
+} MLAN_PACK_END ExtCap_t, *pExtCap_t;
+
+/** Overlapping BSS Scan Parameters Data */
+typedef struct MLAN_PACK_START _OverlapBSSScanParam_t
+{
+    /** OBSS Scan Passive Dwell in milliseconds */
+    t_u16 obss_scan_passive_dwell;
+    /** OBSS Scan Active Dwell in milliseconds */
+    t_u16 obss_scan_active_dwell;
+    /** BSS Channel Width Trigger Scan Interval in seconds */
+    t_u16 bss_chan_width_trigger_scan_int;
+    /** OBSS Scan Passive Total Per Channel */
+    t_u16 obss_scan_passive_total;
+    /** OBSS Scan Active Total Per Channel */
+    t_u16 obss_scan_active_total;
+    /** BSS Width Channel Transition Delay Factor */
+    t_u16 bss_width_chan_trans_delay;
+    /** OBSS Scan Activity Threshold */
+    t_u16 obss_scan_active_threshold;
+} MLAN_PACK_END OBSSScanParam_t, *pOBSSScanParam_t;
+
+/** HT Capabilities IE */
+typedef MLAN_PACK_START struct _IEEEtypes_HTCap_t
+{
+    /** Generic IE header */
+    IEEEtypes_Header_t ieee_hdr;
+    /** HTCap struct */
+    HTCap_t ht_cap;
+} MLAN_PACK_END IEEEtypes_HTCap_t, *pIEEEtypes_HTCap_t;
+
+/** HT Information IE */
+typedef MLAN_PACK_START struct _IEEEtypes_HTInfo_t
+{
+    /** Generic IE header */
+    IEEEtypes_Header_t ieee_hdr;
+    /** HTInfo struct */
+    HTInfo_t ht_info;
+} MLAN_PACK_END IEEEtypes_HTInfo_t, *pIEEEtypes_HTInfo_t;
+
+/** 20/40 BSS Coexistence IE */
+typedef MLAN_PACK_START struct _IEEEtypes_2040BSSCo_t
+{
+    /** Generic IE header */
+    IEEEtypes_Header_t ieee_hdr;
+    /** BSSCo2040_t struct */
+    BSSCo2040_t bss_co_2040;
+} MLAN_PACK_END IEEEtypes_2040BSSCo_t, *pIEEEtypes_2040BSSCo_t;
+
+/** Extended Capabilities IE */
+typedef MLAN_PACK_START struct _IEEEtypes_ExtCap_t
+{
+    /** Generic IE header */
+    IEEEtypes_Header_t ieee_hdr;
+    /** ExtCap_t struct */
+    ExtCap_t ext_cap;
+} MLAN_PACK_END IEEEtypes_ExtCap_t, *pIEEEtypes_ExtCap_t;
+
+/** Overlapping BSS Scan Parameters IE */
+typedef MLAN_PACK_START struct _IEEEtypes_OverlapBSSScanParam_t
+{
+    /** Generic IE header */
+    IEEEtypes_Header_t ieee_hdr;
+    /** OBSSScanParam_t struct */
+    OBSSScanParam_t obss_scan_param;
+} MLAN_PACK_END IEEEtypes_OverlapBSSScanParam_t,
+    *pIEEEtypes_OverlapBSSScanParam_t;
+
+/** Maximum number of subbands in the IEEEtypes_SupportedChannels_t structure */
+#define WLAN_11H_MAX_SUBBANDS  5
+
+/** Maximum number of DFS channels configured in IEEEtypes_IBSS_DFS_t */
+#define WLAN_11H_MAX_IBSS_DFS_CHANNELS 25
+
+/**  IEEE Power Constraint element (7.3.2.15) */
+typedef MLAN_PACK_START struct
+{
+    t_u8 element_id;        /**< IEEE Element ID = 32 */
+    t_u8 len;               /**< Element length after id and len */
+    t_u8 local_constraint;  /**< Local power constraint applied to 11d chan info */
+} MLAN_PACK_END IEEEtypes_PowerConstraint_t;
+
+/**  IEEE Power Capability element (7.3.2.16) */
+typedef MLAN_PACK_START struct
+{
+    t_u8 element_id;                /**< IEEE Element ID = 33 */
+    t_u8 len;                       /**< Element length after id and len */
+    t_s8 min_tx_power_capability;   /**< Minimum Transmit power (dBm) */
+    t_s8 max_tx_power_capability;   /**< Maximum Transmit power (dBm) */
+} MLAN_PACK_END IEEEtypes_PowerCapability_t;
+
+/**  IEEE TPC Report element (7.3.2.18) */
+typedef MLAN_PACK_START struct
+{
+    t_u8 element_id;    /**< IEEE Element ID = 35 */
+    t_u8 len;           /**< Element length after id and len */
+    t_s8 tx_power;      /**< Max power used to transmit the TPC Report frame (dBm) */
+    t_s8 link_margin;   /**< Link margin when TPC Request received (dB) */
+} MLAN_PACK_END IEEEtypes_TPCReport_t;
+
+/*  IEEE Supported Channel sub-band description (7.3.2.19) */
+/**
+ *  Sub-band description used in the supported channels element.
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 start_chan;    /**< Starting channel in the subband */
+    t_u8 num_chans;     /**< Number of channels in the subband */
+
+} MLAN_PACK_END IEEEtypes_SupportChan_Subband_t;
+
+/*  IEEE Supported Channel element (7.3.2.19) */
+/**
+ *  Sent in association requests. Details the sub-bands and number
+ *    of channels supported in each subband
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 element_id;    /**< IEEE Element ID = 36 */
+    t_u8 len;           /**< Element length after id and len */
+
+    /** Configured sub-bands information in the element */
+    IEEEtypes_SupportChan_Subband_t subband[WLAN_11H_MAX_SUBBANDS];
+
+} MLAN_PACK_END IEEEtypes_SupportedChannels_t;
+
+/*  IEEE Channel Switch Announcement Element (7.3.2.20) */
+/**
+ *  Provided in beacons and probe responses.  Used to advertise when
+ *    and to which channel it is changing to.  Only starting STAs in
+ *    an IBSS and APs are allowed to originate a chan switch element.
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 element_id;            /**< IEEE Element ID = 37 */
+    t_u8 len;                   /**< Element length after id and len */
+    t_u8 chan_switch_mode;      /**< STA should not transmit any frames if 1 */
+    t_u8 new_channel_num;       /**< Channel # that AP/IBSS is moving to */
+    t_u8 chan_switch_count;     /**< # of TBTTs before channel switch */
+
+} MLAN_PACK_END IEEEtypes_ChanSwitchAnn_t;
+
+/*  IEEE Quiet Period Element (7.3.2.23) */
+/**
+ *  Provided in beacons and probe responses.  Indicates times during
+ *    which the STA should not be transmitting data.  Only starting STAs in
+ *    an IBSS and APs are allowed to originate a quiet element.
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 element_id;        /**< IEEE Element ID = 40 */
+    t_u8 len;               /**< Element length after id and len */
+    t_u8 quiet_count;       /**< Number of TBTTs until beacon with the quiet period */
+    t_u8 quiet_period;      /**< Regular quiet period, # of TBTTS between periods */
+    t_u16 quiet_duration;   /**< Duration of the quiet period in TUs */
+    t_u16 quiet_offset;     /**< Offset in TUs from the TBTT for the quiet period */
+
+} MLAN_PACK_END IEEEtypes_Quiet_t;
+
+/**
+***  @brief Map octet of the basic measurement report (7.3.2.22.1)
+**/
+typedef MLAN_PACK_START struct
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    t_u8 rsvd5_7:3;                /**< Reserved */
+    t_u8 unmeasured:1;             /**< Channel is unmeasured */
+    t_u8 radar:1;                  /**< Radar detected on channel */
+    t_u8 unidentified_sig:1;       /**< Unidentified signal found on channel */
+    t_u8 ofdm_preamble:1;          /**< OFDM preamble detected on channel */
+    t_u8 bss:1;                    /**< At least one valid MPDU received on channel */
+#else
+    t_u8 bss:1;                    /**< At least one valid MPDU received on channel */
+    t_u8 ofdm_preamble:1;          /**< OFDM preamble detected on channel */
+    t_u8 unidentified_sig:1;       /**< Unidentified signal found on channel */
+    t_u8 radar:1;                  /**< Radar detected on channel */
+    t_u8 unmeasured:1;             /**< Channel is unmeasured */
+    t_u8 rsvd5_7:3;                /**< Reserved */
+#endif                          /* BIG_ENDIAN_SUPPORT */
+
+} MLAN_PACK_END MeasRptBasicMap_t;
+
+/*  IEEE DFS Channel Map field (7.3.2.24) */
+/**
+ *  Used to list supported channels and provide a octet "map" field which
+ *    contains a basic measurement report for that channel in the
+ *    IEEEtypes_IBSS_DFS_t element
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 channel_number;        /**< Channel number */
+    MeasRptBasicMap_t rpt_map;  /**< Basic measurement report for the channel */
+
+} MLAN_PACK_END IEEEtypes_ChannelMap_t;
+
+/*  IEEE IBSS DFS Element (7.3.2.24) */
+/**
+ *  IBSS DFS element included in ad hoc beacons and probe responses.
+ *    Provides information regarding the IBSS DFS Owner as well as the
+ *    originating STAs supported channels and basic measurement results.
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 element_id;                        /**< IEEE Element ID = 41 */
+    t_u8 len;                               /**< Element length after id and len */
+    t_u8 dfs_owner[MLAN_MAC_ADDR_LENGTH];   /**< DFS Owner STA Address */
+    t_u8 dfs_recovery_interval;             /**< DFS Recovery time in TBTTs */
+
+    /** Variable length map field, one Map entry for each supported channel */
+    IEEEtypes_ChannelMap_t channel_map[WLAN_11H_MAX_IBSS_DFS_CHANNELS];
+
+} MLAN_PACK_END IEEEtypes_IBSS_DFS_t;
+
+/* 802.11h BSS information kept for each BSSID received in scan results */
+/**
+ * IEEE BSS information needed from scan results for later processing in
+ *    join commands
+ */
+typedef struct
+{
+    t_u8 sensed_11h;  /**< Capability bit set or 11h IE found in this BSS */
+
+    IEEEtypes_PowerConstraint_t power_constraint; /**< Power Constraint IE */
+    IEEEtypes_PowerCapability_t power_capability; /**< Power Capability IE */
+    IEEEtypes_TPCReport_t tpc_report;             /**< TPC Report IE */
+    IEEEtypes_ChanSwitchAnn_t chan_switch_ann;    /**< Channel Switch Announcement IE */
+    IEEEtypes_Quiet_t quiet;                      /**< Quiet IE */
+    IEEEtypes_IBSS_DFS_t ibss_dfs;                /**< IBSS DFS Element IE */
+
+} wlan_11h_bss_info_t;
+
+#ifdef STA_SUPPORT
+/** Macro for maximum size of scan response buffer */
+#define MAX_SCAN_RSP_BUF (16 * 1024)
+
+/** Maximum number of channels that can be sent in user scan config */
+#define WLAN_USER_SCAN_CHAN_MAX             50
+
+/** Maximum length of SSID list */
+#define MRVDRV_MAX_SSID_LIST_LENGTH         10
+
+/** Scan all the channels in specified band */
+#define BAND_SPECIFIED    0x80
+
+/**
+ *  IOCTL SSID List sub-structure sent in wlan_ioctl_user_scan_cfg
+ *
+ *  Used to specify SSID specific filters as well as SSID pattern matching
+ *    filters for scan result processing in firmware.
+ */
+typedef MLAN_PACK_START struct _wlan_user_scan_ssid
+{
+    /** SSID */
+    t_u8 ssid[MLAN_MAX_SSID_LENGTH + 1];
+    /** Maximum length of SSID */
+    t_u8 max_len;
+} MLAN_PACK_END wlan_user_scan_ssid;
+
+/**
+ *  @brief IOCTL channel sub-structure sent in wlan_ioctl_user_scan_cfg
+ *
+ *  Multiple instances of this structure are included in the IOCTL command
+ *   to configure a instance of a scan on the specific channel.
+ */
+typedef MLAN_PACK_START struct _wlan_user_scan_chan
+{
+    /** Channel Number to scan */
+    t_u8 chan_number;
+    /** Radio type: 'B/G' Band = 0, 'A' Band = 1 */
+    t_u8 radio_type;
+    /** Scan type: Active = 1, Passive = 2 */
+    t_u8 scan_type;
+    /** Reserved */
+    t_u8 reserved;
+    /** Scan duration in milliseconds; if 0 default used */
+    t_u32 scan_time;
+} MLAN_PACK_END wlan_user_scan_chan;
+
+/**
+ *  Input structure to configure an immediate scan cmd to firmware
+ *
+ *  Specifies a number of parameters to be used in general for the scan
+ *    as well as a channel list (wlan_user_scan_chan) for each scan period
+ *    desired.
+ */
+typedef MLAN_PACK_START struct
+{
+    /**
+     *  Flag set to keep the previous scan table intact
+     *
+     *  If set, the scan results will accumulate, replacing any previous
+     *   matched entries for a BSS with the new scan data
+     */
+    t_u8 keep_previous_scan;
+    /**
+     *  BSS mode to be sent in the firmware command
+     *
+     *  Field can be used to restrict the types of networks returned in the
+     *    scan.  Valid settings are:
+     *
+     *   - MLAN_SCAN_MODE_BSS  (infrastructure)
+     *   - MLAN_SCAN_MODE_IBSS (adhoc)
+     *   - MLAN_SCAN_MODE_ANY  (unrestricted, adhoc and infrastructure)
+     */
+    t_u8 bss_mode;
+    /**
+     *  Configure the number of probe requests for active chan scans
+     */
+    t_u8 num_probes;
+    /**
+     *  @brief Reserved
+     */
+    t_u8 reserved;
+    /**
+     *  @brief BSSID filter sent in the firmware command to limit the results
+     */
+    t_u8 specific_bssid[MLAN_MAC_ADDR_LENGTH];
+    /**
+     *  SSID filter list used in the to limit the scan results
+     */
+    wlan_user_scan_ssid ssid_list[MRVDRV_MAX_SSID_LIST_LENGTH];
+    /**
+     *  Variable number (fixed maximum) of channels to scan up
+     */
+    wlan_user_scan_chan chan_list[WLAN_USER_SCAN_CHAN_MAX];
+} MLAN_PACK_END wlan_user_scan_cfg;
+
+/** Default scan interval in millisecond*/
+#define DEFAULT_BGSCAN_INTERVAL 30000
+
+/** action get all, except pps/uapsd config */
+#define BG_SCAN_ACT_GET		0x0000
+/** action set all, except pps/uapsd config */
+#define BG_SCAN_ACT_SET             0x0001
+/** action get pps/uapsd config */
+#define BG_SCAN_ACT_GET_PPS_UAPSD   0x0100
+/** action set pps/uapsd config */
+#define BG_SCAN_ACT_SET_PPS_UAPSD   0x0101
+/** action set all */
+#define BG_SCAN_ACT_SET_ALL         0xff01
+/** ssid match */
+#define BG_SCAN_SSID_MATCH			0x0001
+/** ssid match and RSSI exceeded */
+#define BG_SCAN_SSID_RSSI_MATCH		0x0004
+/** Maximum number of channels that can be sent in bg scan config */
+#define WLAN_BG_SCAN_CHAN_MAX       32
+
+/**
+ *  Input structure to configure bs scan cmd to firmware
+ */
+typedef MLAN_PACK_START struct
+{
+    /** action */
+    t_u16 action;
+    /** enable/disable */
+    t_u8 enable;
+    /**  BSS type:
+      *   MLAN_SCAN_MODE_BSS  (infrastructure)
+      *   MLAN_SCAN_MODE_IBSS (adhoc)
+      *   MLAN_SCAN_MODE_ANY  (unrestricted, adhoc and infrastructure)
+      */
+    t_u8 bss_type;
+    /** number of channel scanned during each scan */
+    t_u8 chan_per_scan;
+    /** interval between consecutive scan */
+    t_u32 scan_interval;
+    /** bit 0: ssid match bit 1: ssid match and SNR exceeded
+      *  bit 2: ssid match and RSSI exceeded
+      *  bit 31: wait for all channel scan to complete to report scan result
+      */
+    t_u32 report_condition;
+    /* Configure the number of probe requests for active chan scans */
+    t_u8 num_probes;
+    /** RSSI threshold */
+    t_u8 rssi_threshold;
+    /** SNR threshold */
+    t_u8 snr_threshold;
+    /** repeat count */
+    t_u16 repeat_count;
+    /** SSID filter list used in the to limit the scan results */
+    wlan_user_scan_ssid ssid_list[MRVDRV_MAX_SSID_LIST_LENGTH];
+    /** Variable number (fixed maximum) of channels to scan up */
+    wlan_user_scan_chan chan_list[WLAN_BG_SCAN_CHAN_MAX];
+} MLAN_PACK_END wlan_bgscan_cfg;
+#endif /* STA_SUPPORT */
+
+#ifdef PRAGMA_PACK
+#pragma pack(pop)
+#endif
+
+/** BSSDescriptor_t
+ *    Structure used to store information for beacon/probe response
+ */
+typedef struct _BSSDescriptor_t
+{
+    /** MAC address */
+    mlan_802_11_mac_addr mac_address;
+
+    /** SSID */
+    mlan_802_11_ssid ssid;
+
+    /** WEP encryption requirement */
+    t_u32 privacy;
+
+    /** Receive signal strength in dBm */
+    t_s32 rssi;
+
+    /** Channel */
+    t_u32 channel;
+
+    /** Freq */
+    t_u32 freq;
+
+    /** Beacon period */
+    t_u16 beacon_period;
+
+    /** ATIM window */
+    t_u32 atim_window;
+
+    /** ERP flags */
+    t_u8 erp_flags;
+
+    /** Type of network in use */
+    WLAN_802_11_NETWORK_TYPE network_type_use;
+
+    /** Network infrastructure mode */
+    t_u32 bss_mode;
+
+    /** Network supported rates */
+    WLAN_802_11_RATES supported_rates;
+
+    /** Supported data rates */
+    t_u8 data_rates[WLAN_SUPPORTED_RATES];
+
+    /** Network band.
+     * BAND_B(0x01): 'b' band
+     * BAND_G(0x02): 'g' band
+     * BAND_A(0X04): 'a' band
+     */
+    t_u16 bss_band;
+
+    /** TSF timestamp from the current firmware TSF */
+    t_u64 network_tsf;
+
+    /** TSF value included in the beacon/probe response */
+    t_u8 time_stamp[8];
+
+    /** PHY parameter set */
+    IEEEtypes_PhyParamSet_t phy_param_set;
+
+    /** SS parameter set */
+    IEEEtypes_SsParamSet_t ss_param_set;
+
+    /** Capability information */
+    IEEEtypes_CapInfo_t cap_info;
+
+    /** WMM IE */
+    IEEEtypes_WmmParameter_t wmm_ie;
+
+    /** 802.11h BSS information */
+    wlan_11h_bss_info_t wlan_11h_bss_info;
+
+    /** Indicate disabling 11n when associate with AP */
+    t_u8 disable_11n;
+    /** 802.11n BSS information */
+    /** HT Capabilities IE */
+    IEEEtypes_HTCap_t *pht_cap;
+    /** HT Capabilities Offset */
+    t_u16 ht_cap_offset;
+    /** HT Information IE */
+    IEEEtypes_HTInfo_t *pht_info;
+    /** HT Information Offset */
+    t_u16 ht_info_offset;
+    /** 20/40 BSS Coexistence IE */
+    IEEEtypes_2040BSSCo_t *pbss_co_2040;
+    /** 20/40 BSS Coexistence Offset */
+    t_u16 bss_co_2040_offset;
+    /** Extended Capabilities IE */
+    IEEEtypes_ExtCap_t *pext_cap;
+    /** Extended Capabilities Offset */
+    t_u16 ext_cap_offset;
+    /** Overlapping BSS Scan Parameters IE */
+    IEEEtypes_OverlapBSSScanParam_t *poverlap_bss_scan_param;
+    /** Overlapping BSS Scan Parameters Offset */
+    t_u16 overlap_bss_offset;
+
+#ifdef STA_SUPPORT
+    /** Country information set */
+    IEEEtypes_CountryInfoFullSet_t country_info;
+#endif                          /* STA_SUPPORT */
+
+    /** WPA IE */
+    IEEEtypes_VendorSpecific_t *pwpa_ie;
+    /** WPA IE offset in the beacon buffer */
+    t_u16 wpa_offset;
+    /** RSN IE */
+    IEEEtypes_Generic_t *prsn_ie;
+    /** RSN IE offset in the beacon buffer */
+    t_u16 rsn_offset;
+#ifdef STA_SUPPORT
+    /** WAPI IE */
+    IEEEtypes_Generic_t *pwapi_ie;
+    /** WAPI IE offset in the beacon buffer */
+    t_u16 wapi_offset;
+#endif
+
+    /** Pointer to the returned scan response */
+    t_u8 *pbeacon_buf;
+    /** Length of the stored scan response */
+    t_u32 beacon_buf_size;
+    /** Max allocated size for updated scan response */
+    t_u32 beacon_buf_size_max;
+
+} BSSDescriptor_t, *pBSSDescriptor_t;
+
+#endif /* !_MLAN_IEEE_H_ */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_init.c b/drivers/net/wireless/sd8797/mlan/mlan_init.c
new file mode 100644
index 000000000000..48681b3e3ed9
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_init.c
@@ -0,0 +1,1043 @@
+/** @file mlan_init.c
+ *  
+ *  @brief This file contains the initialization for FW
+ *  and HW. 
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/********************************************************
+Change log:
+    10/13/2008: initial version
+********************************************************/
+
+#include "mlan.h"
+#ifdef STA_SUPPORT
+#include "mlan_join.h"
+#endif
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_init.h"
+#include "mlan_wmm.h"
+#include "mlan_11n.h"
+#include "mlan_11h.h"
+#include "mlan_meas.h"
+#include "mlan_sdio.h"
+
+/********************************************************
+        Global Variables
+********************************************************/
+
+/********************************************************
+        Local Functions
+********************************************************/
+
+/**
+ *  @brief This function adds a BSS priority table
+ *
+ *  @param priv		A pointer to mlan_private structure
+ *
+ *  @return		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_add_bsspriotbl(pmlan_private priv)
+{
+    pmlan_adapter pmadapter = priv->adapter;
+    mlan_bssprio_node *pbssprio = MNULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    if ((status =
+         pmadapter->callbacks.moal_malloc(pmadapter->pmoal_handle,
+                                          sizeof(mlan_bssprio_node),
+                                          MLAN_MEM_DEF,
+                                          (t_u8 **) & pbssprio))) {
+        PRINTM(MERROR, "Failed to allocate bsspriotbl\n");
+        LEAVE();
+        return status;
+    }
+
+    pbssprio->priv = priv;
+
+    util_init_list((pmlan_linked_list) pbssprio);
+
+    if (!pmadapter->bssprio_tbl[priv->bss_priority].bssprio_cur)
+        pmadapter->bssprio_tbl[priv->bss_priority].bssprio_cur = pbssprio;
+
+    util_enqueue_list_tail(pmadapter->pmoal_handle,
+                           &pmadapter->bssprio_tbl[priv->
+                                                   bss_priority].bssprio_head,
+                           (pmlan_linked_list) pbssprio,
+                           pmadapter->callbacks.moal_spin_lock,
+                           pmadapter->callbacks.moal_spin_unlock);
+
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief This function deletes the BSS priority table
+ *
+ *  @param priv		A pointer to mlan_private structure
+ *
+ *  @return		N/A
+ */
+static t_void
+wlan_delete_bsspriotbl(pmlan_private priv)
+{
+    int i;
+    pmlan_adapter pmadapter = priv->adapter;
+    mlan_bssprio_node *pbssprio_node = MNULL, *ptmp_node = MNULL, **ppcur =
+        MNULL;
+    pmlan_list_head phead;
+
+    ENTER();
+
+    for (i = 0; i < pmadapter->priv_num; ++i) {
+        phead = &pmadapter->bssprio_tbl[i].bssprio_head;
+        ppcur = &pmadapter->bssprio_tbl[i].bssprio_cur;
+        PRINTM(MINFO,
+               "Delete BSS priority table, index = %d, i = %d, phead = %p, pcur = %p\n",
+               priv->bss_index, i, phead, *ppcur);
+        if (*ppcur) {
+            pbssprio_node =
+                (mlan_bssprio_node *) util_peek_list(pmadapter->pmoal_handle,
+                                                     phead,
+                                                     pmadapter->callbacks.
+                                                     moal_spin_lock,
+                                                     pmadapter->callbacks.
+                                                     moal_spin_unlock);
+            while (pbssprio_node && ((pmlan_list_head) pbssprio_node != phead)) {
+                ptmp_node = pbssprio_node->pnext;
+                if (pbssprio_node->priv == priv) {
+                    PRINTM(MINFO, "Delete node, pnode = %p, pnext = %p\n",
+                           pbssprio_node, ptmp_node);
+                    util_unlink_list(pmadapter->pmoal_handle, phead,
+                                     (pmlan_linked_list) pbssprio_node,
+                                     pmadapter->callbacks.moal_spin_lock,
+                                     pmadapter->callbacks.moal_spin_unlock);
+                    pmadapter->callbacks.moal_mfree(pmadapter->pmoal_handle,
+                                                    (t_u8 *) pbssprio_node);
+                }
+                pbssprio_node = ptmp_node;
+            }
+            *ppcur = (mlan_bssprio_node *) phead;
+        }
+    }
+
+    LEAVE();
+}
+
+/********************************************************
+        Global Functions
+********************************************************/
+
+/**
+ *  @brief This function allocates buffer for the members of adapter
+ *  		structure like command buffer and BSSID list.
+ *
+ *  @param pmadapter A pointer to mlan_adapter structure
+ *
+ *  @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_allocate_adapter(pmlan_adapter pmadapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+#ifdef STA_SUPPORT
+    t_u32 buf_size;
+    BSSDescriptor_t *ptemp_scan_table = MNULL;
+#endif
+
+    ENTER();
+
+#ifdef STA_SUPPORT
+    /* Allocate buffer to store the BSSID list */
+    buf_size = sizeof(BSSDescriptor_t) * MRVDRV_MAX_BSSID_LIST;
+    ret =
+        pmadapter->callbacks.moal_malloc(pmadapter->pmoal_handle, buf_size,
+                                         MLAN_MEM_DEF,
+                                         (t_u8 **) & ptemp_scan_table);
+    if (ret != MLAN_STATUS_SUCCESS || !ptemp_scan_table) {
+        PRINTM(MERROR, "Failed to allocate scan table\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    pmadapter->pscan_table = ptemp_scan_table;
+    ret =
+        pmadapter->callbacks.moal_malloc(pmadapter->pmoal_handle,
+                                         DEFAULT_SCAN_BEACON_BUFFER,
+                                         MLAN_MEM_DEF,
+                                         (t_u8 **) & pmadapter->bcn_buf);
+    if (ret != MLAN_STATUS_SUCCESS || !pmadapter->bcn_buf) {
+        PRINTM(MERROR, "Failed to allocate bcn buf\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    pmadapter->bcn_buf_size = DEFAULT_SCAN_BEACON_BUFFER;
+#endif
+
+    /* Allocate command buffer */
+    ret = wlan_alloc_cmd_buffer(pmadapter);
+    if (ret != MLAN_STATUS_SUCCESS) {
+        PRINTM(MERROR, "Failed to allocate command buffer\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    ret =
+        pmadapter->callbacks.moal_malloc(pmadapter->pmoal_handle,
+                                         MAX_MP_REGS + DMA_ALIGNMENT,
+                                         MLAN_MEM_DEF | MLAN_MEM_DMA,
+                                         (t_u8 **) & pmadapter->mp_regs_buf);
+    if (ret != MLAN_STATUS_SUCCESS || !pmadapter->mp_regs_buf) {
+        PRINTM(MERROR, "Failed to allocate mp_regs_buf\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    pmadapter->mp_regs =
+        (t_u8 *) ALIGN_ADDR(pmadapter->mp_regs_buf, DMA_ALIGNMENT);
+
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+    ret = wlan_alloc_sdio_mpa_buffers(pmadapter, SDIO_MP_TX_AGGR_DEF_BUF_SIZE,
+                                      SDIO_MP_RX_AGGR_DEF_BUF_SIZE);
+    if (ret != MLAN_STATUS_SUCCESS) {
+        PRINTM(MERROR, "Failed to allocate sdio mp-a buffers\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+#endif
+
+    pmadapter->psleep_cfm =
+        wlan_alloc_mlan_buffer(pmadapter, sizeof(opt_sleep_confirm_buffer), 0,
+                               MTRUE);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function initializes the private structure
+ *  		and sets default values to the members of mlan_private.
+ *
+ *  @param priv    A pointer to mlan_private structure
+ *
+ *  @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_init_priv(pmlan_private priv)
+{
+    t_u32 i;
+    pmlan_adapter pmadapter = priv->adapter;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    priv->media_connected = MFALSE;
+    memset(pmadapter, priv->curr_addr, 0xff, MLAN_MAC_ADDR_LENGTH);
+
+#ifdef STA_SUPPORT
+    priv->pkt_tx_ctrl = 0;
+    priv->bss_mode = MLAN_BSS_MODE_INFRA;
+    priv->data_rate = 0;        /* Initially indicate the rate as auto */
+    priv->is_data_rate_auto = MTRUE;
+    priv->bcn_avg_factor = DEFAULT_BCN_AVG_FACTOR;
+    priv->data_avg_factor = DEFAULT_DATA_AVG_FACTOR;
+
+    priv->sec_info.wep_status = Wlan802_11WEPDisabled;
+    priv->sec_info.authentication_mode = MLAN_AUTH_MODE_AUTO;
+    priv->sec_info.encryption_mode = MLAN_ENCRYPTION_MODE_NONE;
+    for (i = 0; i < sizeof(priv->wep_key) / sizeof(priv->wep_key[0]); i++)
+        memset(pmadapter, &priv->wep_key[i], 0, sizeof(mrvl_wep_key_t));
+    priv->wep_key_curr_index = 0;
+    priv->ewpa_query = MFALSE;
+    priv->adhoc_aes_enabled = MFALSE;
+    priv->curr_pkt_filter =
+        HostCmd_ACT_MAC_RX_ON | HostCmd_ACT_MAC_TX_ON |
+        HostCmd_ACT_MAC_ETHERNETII_ENABLE;
+
+    priv->beacon_period = MLAN_BEACON_INTERVAL;
+    priv->pattempted_bss_desc = MNULL;
+    memset(pmadapter, &priv->curr_bss_params, 0, sizeof(priv->curr_bss_params));
+    priv->listen_interval = MLAN_DEFAULT_LISTEN_INTERVAL;
+
+    memset(pmadapter, &priv->assoc_rsp_buf, 0, sizeof(priv->assoc_rsp_buf));
+    priv->assoc_rsp_size = 0;
+
+    wlan_11d_priv_init(priv);
+    wlan_11h_priv_init(priv);
+#if defined(UAP_SUPPORT)
+    priv->uap_bss_started = MFALSE;
+    memset(pmadapter, &priv->uap_state_chan_cb, 0,
+           sizeof(priv->uap_state_chan_cb));
+#endif
+#if defined(UAP_SUPPORT)
+    priv->num_drop_pkts = 0;
+#endif
+#if defined(STA_SUPPORT)
+    priv->adhoc_state_prev = ADHOC_IDLE;
+    memset(pmadapter, &priv->adhoc_last_start_ssid, 0,
+           sizeof(priv->adhoc_last_start_ssid));
+#endif
+    priv->adhoc_channel = DEFAULT_AD_HOC_CHANNEL;
+    priv->atim_window = 0;
+    priv->adhoc_state = ADHOC_IDLE;
+    priv->tx_power_level = 0;
+    priv->max_tx_power_level = 0;
+    priv->min_tx_power_level = 0;
+    priv->tx_rate = 0;
+    priv->rxpd_htinfo = 0;
+    priv->rxpd_rate = 0;
+    priv->rate_bitmap = 0;
+    priv->data_rssi_last = 0;
+    priv->data_rssi_avg = 0;
+    priv->data_nf_avg = 0;
+    priv->data_nf_last = 0;
+    priv->bcn_rssi_last = 0;
+    priv->bcn_rssi_avg = 0;
+    priv->bcn_nf_avg = 0;
+    priv->bcn_nf_last = 0;
+
+    priv->sec_info.ewpa_enabled = MFALSE;
+    priv->sec_info.wpa_enabled = MFALSE;
+    priv->sec_info.wpa2_enabled = MFALSE;
+    memset(pmadapter, &priv->wpa_ie, 0, sizeof(priv->wpa_ie));
+    memset(pmadapter, &priv->aes_key, 0, sizeof(priv->aes_key));
+    priv->wpa_ie_len = 0;
+    priv->wpa_is_gtk_set = MFALSE;
+    priv->sec_info.wapi_enabled = MFALSE;
+    priv->wapi_ie_len = 0;
+    priv->sec_info.wapi_key_on = MFALSE;
+
+    memset(pmadapter, &priv->wps, 0, sizeof(priv->wps));
+    memset(pmadapter, &priv->gen_ie_buf, 0, sizeof(priv->gen_ie_buf));
+    priv->gen_ie_buf_len = 0;
+#endif /* STA_SUPPORT */
+
+    priv->tx_bf_cap = 0;
+    priv->wmm_required = MTRUE;
+    priv->wmm_enabled = MFALSE;
+    priv->wmm_qosinfo = 0;
+#ifdef STA_SUPPORT
+    priv->pcurr_bcn_buf = MNULL;
+    priv->curr_bcn_size = 0;
+#endif /* STA_SUPPORT */
+
+    for (i = 0; i < MAX_NUM_TID; i++)
+        priv->addba_reject[i] = ADDBA_RSP_STATUS_ACCEPT;
+    priv->max_amsdu = 0;
+
+    priv->scan_block = MFALSE;
+
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) {
+        priv->port_ctrl_mode = MTRUE;
+    } else {
+        priv->port_ctrl_mode = MFALSE;
+    }
+    priv->port_open = MFALSE;
+
+    if (!ret)
+        ret = wlan_add_bsspriotbl(priv);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function initializes the adapter structure
+ *  		and sets default values to the members of adapter.
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *
+ *  @return		N/A
+ */
+t_void
+wlan_init_adapter(pmlan_adapter pmadapter)
+{
+    opt_sleep_confirm_buffer *sleep_cfm_buf = MNULL;
+
+    ENTER();
+
+    sleep_cfm_buf = (opt_sleep_confirm_buffer *) (pmadapter->psleep_cfm->pbuf +
+                                                  pmadapter->psleep_cfm->
+                                                  data_offset);
+
+#ifdef MFG_CMD_SUPPORT
+    if (pmadapter->init_para.mfg_mode == MLAN_INIT_PARA_DISABLED) {
+        pmadapter->mfg_mode = MFALSE;
+    } else {
+        pmadapter->mfg_mode = pmadapter->init_para.mfg_mode;
+    }
+#endif
+
+    pmadapter->int_mode = pmadapter->init_para.int_mode;
+    pmadapter->gpio_pin = pmadapter->init_para.gpio_pin;
+
+#if defined(STA_SUPPORT)
+    pmadapter->pwarm_reset_ioctl_req = MNULL;
+#endif
+    pmadapter->cmd_sent = MFALSE;
+    pmadapter->data_sent = MTRUE;
+    pmadapter->mp_rd_bitmap = 0;
+    pmadapter->mp_wr_bitmap = 0;
+    pmadapter->curr_rd_port = 1;
+    pmadapter->curr_wr_port = 1;
+    pmadapter->mp_data_port_mask = DATA_PORT_MASK;
+
+#ifdef SDIO_MULTI_PORT_TX_AGGR
+    pmadapter->mpa_tx.buf_len = 0;
+    pmadapter->mpa_tx.pkt_cnt = 0;
+    pmadapter->mpa_tx.start_port = 0;
+
+    if (!pmadapter->init_para.mpa_tx_cfg) {
+        pmadapter->mpa_tx.enabled = MFALSE;
+    } else if (pmadapter->init_para.mpa_tx_cfg == MLAN_INIT_PARA_DISABLED) {
+        pmadapter->mpa_tx.enabled = MFALSE;
+    } else {
+        pmadapter->mpa_tx.enabled = MTRUE;
+    }
+    pmadapter->mpa_tx.pkt_aggr_limit = SDIO_MP_AGGR_DEF_PKT_LIMIT;
+#endif /* SDIO_MULTI_PORT_TX_AGGR */
+
+#ifdef SDIO_MULTI_PORT_RX_AGGR
+    pmadapter->mpa_rx.buf_len = 0;
+    pmadapter->mpa_rx.pkt_cnt = 0;
+    pmadapter->mpa_rx.start_port = 0;
+
+    if (!pmadapter->init_para.mpa_rx_cfg) {
+        pmadapter->mpa_rx.enabled = MFALSE;
+    } else if (pmadapter->init_para.mpa_rx_cfg == MLAN_INIT_PARA_DISABLED) {
+        pmadapter->mpa_rx.enabled = MFALSE;
+    } else {
+        pmadapter->mpa_rx.enabled = MTRUE;
+    }
+    pmadapter->mpa_rx.pkt_aggr_limit = SDIO_MP_AGGR_DEF_PKT_LIMIT;
+#endif /* SDIO_MULTI_PORT_RX_AGGR */
+
+    pmadapter->cmd_resp_received = MFALSE;
+    pmadapter->event_received = MFALSE;
+    pmadapter->data_received = MFALSE;
+
+    pmadapter->cmd_timer_is_set = MFALSE;
+
+    /* PnP and power profile */
+    pmadapter->surprise_removed = MFALSE;
+
+    /* Status variables */
+    pmadapter->hw_status = WlanHardwareStatusInitializing;
+
+    if (!pmadapter->init_para.ps_mode) {
+        pmadapter->ps_mode = DEFAULT_PS_MODE;
+    } else if (pmadapter->init_para.ps_mode == MLAN_INIT_PARA_DISABLED) {
+        pmadapter->ps_mode = Wlan802_11PowerModeCAM;
+    } else {
+        pmadapter->ps_mode = Wlan802_11PowerModePSP;
+    }
+    pmadapter->ps_state = PS_STATE_AWAKE;
+    pmadapter->need_to_wakeup = MFALSE;
+
+#ifdef STA_SUPPORT
+    /* Scan type */
+    pmadapter->scan_type = MLAN_SCAN_TYPE_ACTIVE;
+    /* Scan mode */
+    pmadapter->scan_mode = HostCmd_BSS_MODE_ANY;
+    /* Scan time */
+    pmadapter->specific_scan_time = MRVDRV_SPECIFIC_SCAN_CHAN_TIME;
+    pmadapter->active_scan_time = MRVDRV_ACTIVE_SCAN_CHAN_TIME;
+    pmadapter->passive_scan_time = MRVDRV_PASSIVE_SCAN_CHAN_TIME;
+
+    pmadapter->num_in_scan_table = 0;
+    memset(pmadapter, pmadapter->pscan_table, 0,
+           (sizeof(BSSDescriptor_t) * MRVDRV_MAX_BSSID_LIST));
+    pmadapter->ext_scan = 0;
+    pmadapter->scan_probes = DEFAULT_PROBES;
+
+    memset(pmadapter, pmadapter->bcn_buf, 0, pmadapter->bcn_buf_size);
+    pmadapter->pbcn_buf_end = pmadapter->bcn_buf;
+
+    pmadapter->radio_on = RADIO_ON;
+    pmadapter->multiple_dtim = MRVDRV_DEFAULT_MULTIPLE_DTIM;
+
+    pmadapter->local_listen_interval = 0;       /* default value in firmware
+                                                   will be used */
+#endif /* STA_SUPPORT */
+
+    pmadapter->is_deep_sleep = MFALSE;
+    pmadapter->idle_time = DEEP_SLEEP_IDLE_TIME;
+    if (!pmadapter->init_para.auto_ds) {
+        pmadapter->init_auto_ds = DEFAULT_AUTO_DS_MODE;
+    } else if (pmadapter->init_para.auto_ds == MLAN_INIT_PARA_DISABLED) {
+        pmadapter->init_auto_ds = MFALSE;
+    } else {
+        pmadapter->init_auto_ds = MTRUE;
+    }
+
+    pmadapter->delay_null_pkt = MFALSE;
+    pmadapter->delay_to_ps = DELAY_TO_PS_DEFAULT;
+    pmadapter->enhanced_ps_mode = PS_MODE_AUTO;
+
+    pmadapter->gen_null_pkt = MFALSE;   /* Disable NULL Pkt generation-default */
+    pmadapter->pps_uapsd_mode = MFALSE; /* Disable pps/uapsd mode -default */
+
+    pmadapter->pm_wakeup_card_req = MFALSE;
+
+    pmadapter->pm_wakeup_fw_try = MFALSE;
+
+    if (!pmadapter->init_para.max_tx_buf)
+        pmadapter->max_tx_buf_size = MLAN_TX_DATA_BUF_SIZE_2K;
+    else
+        pmadapter->max_tx_buf_size = (t_u16) pmadapter->init_para.max_tx_buf;
+    pmadapter->tx_buf_size = MLAN_TX_DATA_BUF_SIZE_2K;
+    pmadapter->curr_tx_buf_size = MLAN_TX_DATA_BUF_SIZE_2K;
+
+    pmadapter->is_hs_configured = MFALSE;
+    pmadapter->hs_cfg.conditions = HOST_SLEEP_DEF_COND;
+    pmadapter->hs_cfg.gpio = HOST_SLEEP_DEF_GPIO;
+    pmadapter->hs_cfg.gap = HOST_SLEEP_DEF_GAP;
+    pmadapter->hs_activated = MFALSE;
+
+    memset(pmadapter, pmadapter->event_body, 0, sizeof(pmadapter->event_body));
+    pmadapter->hw_dot_11n_dev_cap = 0;
+    pmadapter->hw_dev_mcs_support = 0;
+    pmadapter->usr_dot_11n_dev_cap_bg = 0;
+    pmadapter->usr_dot_11n_dev_cap_a = 0;
+    pmadapter->usr_dev_mcs_support = 0;
+#ifdef STA_SUPPORT
+    pmadapter->chan_bandwidth = 0;
+    pmadapter->adhoc_11n_enabled = MFALSE;
+#endif /* STA_SUPPORT */
+
+    /* Initialize 802.11d */
+    wlan_11d_init(pmadapter);
+
+    wlan_11h_init(pmadapter);
+
+    wlan_wmm_init(pmadapter);
+
+    if (pmadapter->psleep_cfm) {
+        pmadapter->psleep_cfm->buf_type = MLAN_BUF_TYPE_CMD;
+        pmadapter->psleep_cfm->data_len = sizeof(OPT_Confirm_Sleep);
+        memset(pmadapter, &sleep_cfm_buf->ps_cfm_sleep, 0,
+               sizeof(OPT_Confirm_Sleep));
+        sleep_cfm_buf->ps_cfm_sleep.command =
+            wlan_cpu_to_le16(HostCmd_CMD_802_11_PS_MODE_ENH);
+        sleep_cfm_buf->ps_cfm_sleep.size =
+            wlan_cpu_to_le16(sizeof(OPT_Confirm_Sleep));
+        sleep_cfm_buf->ps_cfm_sleep.result = 0;
+        sleep_cfm_buf->ps_cfm_sleep.action = wlan_cpu_to_le16(SLEEP_CONFIRM);
+        sleep_cfm_buf->ps_cfm_sleep.sleep_cfm.resp_ctrl =
+            wlan_cpu_to_le16(RESP_NEEDED);
+    }
+    memset(pmadapter, &pmadapter->sleep_params, 0,
+           sizeof(pmadapter->sleep_params));
+    memset(pmadapter, &pmadapter->sleep_period, 0,
+           sizeof(pmadapter->sleep_period));
+    pmadapter->tx_lock_flag = MFALSE;
+    pmadapter->null_pkt_interval = 0;
+    pmadapter->fw_bands = 0;
+    pmadapter->config_bands = 0;
+    pmadapter->adhoc_start_band = 0;
+    pmadapter->pscan_channels = MNULL;
+    pmadapter->fw_release_number = 0;
+    pmadapter->fw_cap_info = 0;
+    memset(pmadapter, &pmadapter->upld_buf, 0, sizeof(pmadapter->upld_buf));
+    pmadapter->upld_len = 0;
+    pmadapter->event_cause = 0;
+    pmadapter->pmlan_buffer_event = MNULL;
+    memset(pmadapter, &pmadapter->region_channel, 0,
+           sizeof(pmadapter->region_channel));
+    pmadapter->region_code = 0;
+    memcpy(pmadapter, pmadapter->country_code, MRVDRV_DEFAULT_COUNTRY_CODE,
+           COUNTRY_CODE_LEN);
+    pmadapter->bcn_miss_time_out = DEFAULT_BCN_MISS_TIMEOUT;
+    pmadapter->adhoc_awake_period = 0;
+#ifdef STA_SUPPORT
+    memset(pmadapter, &pmadapter->arp_filter, 0, sizeof(pmadapter->arp_filter));
+    pmadapter->arp_filter_size = 0;
+#endif /* STA_SUPPORT */
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function intializes the lock variables and
+ *  the list heads.
+ *
+ *  @param pmadapter  A pointer to a mlan_adapter structure
+ *
+ *  @return           MLAN_STATUS_SUCCESS -- on success,
+ *                    otherwise MLAN_STATUS_FAILURE
+ *
+ */
+mlan_status
+wlan_init_lock_list(IN pmlan_adapter pmadapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private priv = MNULL;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    t_s32 i = 0;
+    t_u32 j = 0;
+
+    ENTER();
+
+    if (pcb->moal_init_lock(pmadapter->pmoal_handle, &pmadapter->pmlan_lock)
+        != MLAN_STATUS_SUCCESS) {
+        ret = MLAN_STATUS_FAILURE;
+        goto error;
+    }
+    if (pcb->moal_init_lock(pmadapter->pmoal_handle, &pmadapter->pint_lock)
+        != MLAN_STATUS_SUCCESS) {
+        ret = MLAN_STATUS_FAILURE;
+        goto error;
+    }
+    if (pcb->
+        moal_init_lock(pmadapter->pmoal_handle, &pmadapter->pmain_proc_lock)
+        != MLAN_STATUS_SUCCESS) {
+        ret = MLAN_STATUS_FAILURE;
+        goto error;
+    }
+    if (pcb->moal_init_lock(pmadapter->pmoal_handle, &pmadapter->pmlan_cmd_lock)
+        != MLAN_STATUS_SUCCESS) {
+        ret = MLAN_STATUS_FAILURE;
+        goto error;
+    }
+    for (i = 0; i < pmadapter->priv_num; i++) {
+        if (pmadapter->priv[i]) {
+            priv = pmadapter->priv[i];
+            if (pcb->moal_init_lock(pmadapter->pmoal_handle, &priv->rx_pkt_lock)
+                != MLAN_STATUS_SUCCESS) {
+                ret = MLAN_STATUS_FAILURE;
+                goto error;
+            }
+            if (pcb->
+                moal_init_lock(pmadapter->pmoal_handle,
+                               &priv->wmm.ra_list_spinlock)
+                != MLAN_STATUS_SUCCESS) {
+                ret = MLAN_STATUS_FAILURE;
+                goto error;
+            }
+#ifdef STA_SUPPORT
+            if (pcb->
+                moal_init_lock(pmadapter->pmoal_handle,
+                               &priv->curr_bcn_buf_lock)
+                != MLAN_STATUS_SUCCESS) {
+                ret = MLAN_STATUS_FAILURE;
+                goto error;
+            }
+#endif
+        }
+    }
+
+    /* Initialize bypass_txq */
+    util_init_list_head((t_void *) pmadapter->pmoal_handle,
+                        &pmadapter->bypass_txq, MTRUE,
+                        pmadapter->callbacks.moal_init_lock);
+    /* Initialize cmd_free_q */
+    util_init_list_head((t_void *) pmadapter->pmoal_handle,
+                        &pmadapter->cmd_free_q, MTRUE,
+                        pmadapter->callbacks.moal_init_lock);
+    /* Initialize cmd_pending_q */
+    util_init_list_head((t_void *) pmadapter->pmoal_handle,
+                        &pmadapter->cmd_pending_q, MTRUE,
+                        pmadapter->callbacks.moal_init_lock);
+    /* Initialize scan_pending_q */
+    util_init_list_head((t_void *) pmadapter->pmoal_handle,
+                        &pmadapter->scan_pending_q, MTRUE,
+                        pmadapter->callbacks.moal_init_lock);
+
+    for (i = 0; i < pmadapter->priv_num; ++i) {
+        util_init_list_head((t_void *) pmadapter->pmoal_handle,
+                            &pmadapter->bssprio_tbl[i].bssprio_head,
+                            MTRUE, pmadapter->callbacks.moal_init_lock);
+        pmadapter->bssprio_tbl[i].bssprio_cur = MNULL;
+    }
+
+    for (i = 0; i < pmadapter->priv_num; i++) {
+        if (pmadapter->priv[i]) {
+            priv = pmadapter->priv[i];
+            for (j = 0; j < MAX_NUM_TID; ++j) {
+                util_init_list_head((t_void *) pmadapter->pmoal_handle,
+                                    &priv->wmm.tid_tbl_ptr[j].ra_list, MTRUE,
+                                    priv->adapter->callbacks.moal_init_lock);
+            }
+            util_init_list_head((t_void *) pmadapter->pmoal_handle,
+                                &priv->tx_ba_stream_tbl_ptr, MTRUE,
+                                pmadapter->callbacks.moal_init_lock);
+            util_init_list_head((t_void *) pmadapter->pmoal_handle,
+                                &priv->rx_reorder_tbl_ptr, MTRUE,
+                                pmadapter->callbacks.moal_init_lock);
+            util_scalar_init((t_void *) pmadapter->pmoal_handle,
+                             &priv->wmm.tx_pkts_queued, 0,
+                             priv->wmm.ra_list_spinlock,
+                             pmadapter->callbacks.moal_init_lock);
+            util_scalar_init((t_void *) pmadapter->pmoal_handle,
+                             &priv->wmm.highest_queued_prio, HIGH_PRIO_TID,
+                             priv->wmm.ra_list_spinlock,
+                             pmadapter->callbacks.moal_init_lock);
+            util_init_list_head((t_void *) pmadapter->pmoal_handle,
+                                &priv->sta_list, MTRUE,
+                                pmadapter->callbacks.moal_init_lock);
+        }
+    }
+
+  error:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function releases the lock variables
+ *
+ *  @param pmadapter  A pointer to a mlan_adapter structure
+ *
+ *  @return           None
+ *
+ */
+t_void
+wlan_free_lock_list(IN pmlan_adapter pmadapter)
+{
+    pmlan_private priv = MNULL;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    t_s32 i = 0;
+    t_s32 j = 0;
+
+    ENTER();
+
+    if (pmadapter->pmlan_lock)
+        pcb->moal_free_lock(pmadapter->pmoal_handle, pmadapter->pmlan_lock);
+    if (pmadapter->pint_lock)
+        pcb->moal_free_lock(pmadapter->pmoal_handle, pmadapter->pint_lock);
+    if (pmadapter->pmain_proc_lock)
+        pcb->moal_free_lock(pmadapter->pmoal_handle,
+                            pmadapter->pmain_proc_lock);
+    if (pmadapter->pmlan_cmd_lock)
+        pcb->moal_free_lock(pmadapter->pmoal_handle, pmadapter->pmlan_cmd_lock);
+
+    for (i = 0; i < pmadapter->priv_num; i++) {
+        if (pmadapter->priv[i]) {
+            priv = pmadapter->priv[i];
+            if (priv->rx_pkt_lock)
+                pcb->moal_free_lock(pmadapter->pmoal_handle, priv->rx_pkt_lock);
+            if (priv->wmm.ra_list_spinlock)
+                pcb->moal_free_lock(pmadapter->pmoal_handle,
+                                    priv->wmm.ra_list_spinlock);
+#ifdef STA_SUPPORT
+            if (priv->curr_bcn_buf_lock)
+                pcb->moal_free_lock(pmadapter->pmoal_handle,
+                                    priv->curr_bcn_buf_lock);
+#endif
+        }
+    }
+
+    /* Free lists */
+    util_free_list_head((t_void *) pmadapter->pmoal_handle,
+                        &pmadapter->bypass_txq,
+                        pmadapter->callbacks.moal_free_lock);
+    util_free_list_head((t_void *) pmadapter->pmoal_handle,
+                        &pmadapter->cmd_free_q,
+                        pmadapter->callbacks.moal_free_lock);
+
+    util_free_list_head((t_void *) pmadapter->pmoal_handle,
+                        &pmadapter->cmd_pending_q,
+                        pmadapter->callbacks.moal_free_lock);
+
+    util_free_list_head((t_void *) pmadapter->pmoal_handle,
+                        &pmadapter->scan_pending_q,
+                        pmadapter->callbacks.moal_free_lock);
+
+    for (i = 0; i < pmadapter->priv_num; i++)
+        util_free_list_head((t_void *) pmadapter->pmoal_handle,
+                            &pmadapter->bssprio_tbl[i].bssprio_head,
+                            pcb->moal_free_lock);
+
+    for (i = 0; i < pmadapter->priv_num; i++) {
+        if (pmadapter->priv[i]) {
+            priv = pmadapter->priv[i];
+            util_free_list_head((t_void *) pmadapter->pmoal_handle,
+                                &priv->sta_list,
+                                priv->adapter->callbacks.moal_free_lock);
+
+            for (j = 0; j < MAX_NUM_TID; ++j)
+                util_free_list_head((t_void *) priv->adapter->pmoal_handle,
+                                    &priv->wmm.tid_tbl_ptr[j].ra_list,
+                                    priv->adapter->callbacks.moal_free_lock);
+            util_free_list_head((t_void *) priv->adapter->pmoal_handle,
+                                &priv->tx_ba_stream_tbl_ptr,
+                                priv->adapter->callbacks.moal_free_lock);
+            util_free_list_head((t_void *) priv->adapter->pmoal_handle,
+                                &priv->rx_reorder_tbl_ptr,
+                                priv->adapter->callbacks.moal_free_lock);
+            util_scalar_free((t_void *) priv->adapter->pmoal_handle,
+                             &priv->wmm.tx_pkts_queued,
+                             priv->adapter->callbacks.moal_free_lock);
+            util_scalar_free((t_void *) priv->adapter->pmoal_handle,
+                             &priv->wmm.highest_queued_prio,
+                             priv->adapter->callbacks.moal_free_lock);
+        }
+    }
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function intializes the timers
+ *
+ *  @param pmadapter  A pointer to a mlan_adapter structure
+ *
+ *  @return           MLAN_STATUS_SUCCESS -- on success,
+ *                    otherwise MLAN_STATUS_FAILURE
+ *
+ */
+mlan_status
+wlan_init_timer(IN pmlan_adapter pmadapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+
+    ENTER();
+
+    if (pcb->
+        moal_init_timer(pmadapter->pmoal_handle, &pmadapter->pmlan_cmd_timer,
+                        wlan_cmd_timeout_func, pmadapter)
+        != MLAN_STATUS_SUCCESS) {
+        ret = MLAN_STATUS_FAILURE;
+        goto error;
+    }
+  error:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function releases the timers
+ *
+ *  @param pmadapter  A pointer to a mlan_adapter structure
+ *
+ *  @return           None
+ *
+ */
+t_void
+wlan_free_timer(IN pmlan_adapter pmadapter)
+{
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+
+    ENTER();
+
+    if (pmadapter->pmlan_cmd_timer)
+        pcb->moal_free_timer(pmadapter->pmoal_handle,
+                             pmadapter->pmlan_cmd_timer);
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief  This function initializes firmware
+ *
+ *  @param pmadapter		A pointer to mlan_adapter
+ *
+ *  @return		MLAN_STATUS_SUCCESS, MLAN_STATUS_PENDING or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_init_fw(IN pmlan_adapter pmadapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private priv = MNULL;
+    t_u8 i = 0;
+
+    ENTER();
+
+    /* Initialize adapter structure */
+    wlan_init_adapter(pmadapter);
+
+    for (i = 0; i < pmadapter->priv_num; i++) {
+        if (pmadapter->priv[i]) {
+            priv = pmadapter->priv[i];
+
+            /* Initialize private structure */
+            if ((ret = wlan_init_priv(priv))) {
+                ret = MLAN_STATUS_FAILURE;
+                goto done;
+            }
+        }
+    }
+#ifdef MFG_CMD_SUPPORT
+    if (pmadapter->mfg_mode != MTRUE) {
+#endif
+        /* Issue firmware initialize commands for first BSS, for other
+           interfaces it will be called after getting the last init command
+           response of previous interface */
+        priv = wlan_get_priv(pmadapter, MLAN_BSS_ROLE_ANY);
+        ret = priv->ops.init_cmd(priv, MTRUE);
+        if (ret == MLAN_STATUS_FAILURE)
+            goto done;
+#ifdef MFG_CMD_SUPPORT
+    }
+#endif
+
+    if (util_peek_list(pmadapter->pmoal_handle, &pmadapter->cmd_pending_q,
+                       pmadapter->callbacks.moal_spin_lock,
+                       pmadapter->callbacks.moal_spin_unlock)) {
+        /* Send the first command in queue and return */
+        if (mlan_main_process(pmadapter) == MLAN_STATUS_FAILURE)
+            ret = MLAN_STATUS_FAILURE;
+        else
+            ret = MLAN_STATUS_PENDING;
+    } else {
+        pmadapter->hw_status = WlanHardwareStatusReady;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function frees the structure of adapter
+ *
+ *  @param pmadapter      A pointer to mlan_adapter structure
+ *
+ *  @return             N/A
+ */
+t_void
+wlan_free_adapter(pmlan_adapter pmadapter)
+{
+    mlan_callbacks *pcb = (mlan_callbacks *) & pmadapter->callbacks;
+
+    ENTER();
+
+    if (!pmadapter) {
+        PRINTM(MERROR, "The adapter is NULL\n");
+        LEAVE();
+        return;
+    }
+
+    wlan_cancel_all_pending_cmd(pmadapter);
+    /* Free command buffer */
+    PRINTM(MINFO, "Free Command buffer\n");
+    wlan_free_cmd_buffer(pmadapter);
+
+    if (pmadapter->cmd_timer_is_set) {
+        /* Cancel command timeout timer */
+        pcb->moal_stop_timer(pmadapter->pmoal_handle,
+                             pmadapter->pmlan_cmd_timer);
+        pmadapter->cmd_timer_is_set = MFALSE;
+    }
+#ifdef STA_SUPPORT
+    PRINTM(MINFO, "Free ScanTable\n");
+    if (pmadapter->pscan_table) {
+        pcb->moal_mfree(pmadapter->pmoal_handle,
+                        (t_u8 *) pmadapter->pscan_table);
+        pmadapter->pscan_table = MNULL;
+    }
+    if (pmadapter->bcn_buf) {
+        pcb->moal_mfree(pmadapter->pmoal_handle, (t_u8 *) pmadapter->bcn_buf);
+        pmadapter->bcn_buf = MNULL;
+    }
+#endif
+
+    wlan_11h_cleanup(pmadapter);
+
+    if (pmadapter->mp_regs_buf) {
+        pcb->moal_mfree(pmadapter->pmoal_handle,
+                        (t_u8 *) pmadapter->mp_regs_buf);
+        pmadapter->mp_regs_buf = MNULL;
+        pmadapter->mp_regs = MNULL;
+    }
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+    wlan_free_sdio_mpa_buffers(pmadapter);
+#endif
+    wlan_free_mlan_buffer(pmadapter, pmadapter->psleep_cfm);
+    pmadapter->psleep_cfm = MNULL;
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function frees the structure of priv
+ *
+ *  @param pmpriv  A pointer to mlan_private structure
+ *
+ *  @return        N/A
+ */
+t_void
+wlan_free_priv(mlan_private * pmpriv)
+{
+    ENTER();
+    wlan_clean_txrx(pmpriv);
+    wlan_delete_bsspriotbl(pmpriv);
+
+#ifdef STA_SUPPORT
+    wlan_free_curr_bcn(pmpriv);
+#endif /* STA_SUPPORT */
+
+    wlan_delete_station_list(pmpriv);
+    LEAVE();
+}
+
+/**
+ *  @brief The cmdresp handler calls this function for init_fw_complete callback
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *
+ *  @return		MLAN_STATUS_SUCCESS
+ *                      	The firmware initialization callback succeeded.
+ */
+mlan_status
+wlan_init_fw_complete(IN pmlan_adapter pmadapter)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+
+    ENTER();
+
+    /* Check if hardware is ready */
+    if (pmadapter->hw_status != WlanHardwareStatusReady)
+        status = MLAN_STATUS_FAILURE;
+
+    /* Invoke callback */
+    ret = pcb->moal_init_fw_complete(pmadapter->pmoal_handle, status);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief The cmdresp handler calls this function for shutdown_fw_complete callback
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *
+ *  @return		MLAN_STATUS_SUCCESS
+ *                      	The firmware shutdown callback succeeded.
+ */
+mlan_status
+wlan_shutdown_fw_complete(IN pmlan_adapter pmadapter)
+{
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    pmadapter->hw_status = WlanHardwareStatusNotReady;
+    /* Invoke callback */
+    ret = pcb->moal_shutdown_fw_complete(pmadapter->pmoal_handle, status);
+    LEAVE();
+    return ret;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_init.h b/drivers/net/wireless/sd8797/mlan/mlan_init.h
new file mode 100644
index 000000000000..4496741a3cfb
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_init.h
@@ -0,0 +1,88 @@
+/** @file mlan_init.h
+ *
+ *  @brief This file defines the FW initialization data
+ *  structures.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    10/13/2008: initial version
+******************************************************/
+
+#ifndef _MLAN_INIT_H_
+#define _MLAN_INIT_H_
+
+/** Tx buffer size for firmware download*/
+#define FW_DNLD_TX_BUF_SIZE	620
+/** Rx buffer size for firmware download*/
+#define FW_DNLD_RX_BUF_SIZE       2048
+/** Max firmware retry */
+#define MAX_FW_RETRY        	3
+
+/** Firmware has last block */
+#define FW_HAS_LAST_BLOCK		0x00000004
+
+/** Firmware data transmit size */
+#define FW_DATA_XMIT_SIZE \
+	sizeof(FWHeader) + DataLength + sizeof(t_u32)
+
+/** FWHeader */
+typedef struct _FWHeader
+{
+    /** FW download command */
+    t_u32 dnld_cmd;
+    /** FW base address */
+    t_u32 base_addr;
+    /** FW data length */
+    t_u32 data_length;
+    /** FW CRC */
+    t_u32 crc;
+} FWHeader;
+
+/** FWData */
+typedef struct _FWData
+{
+    /** FW data header */
+    FWHeader fw_header;
+    /** FW data sequence number */
+    t_u32 seq_num;
+    /** FW data buffer */
+    t_u8 data[1];
+} FWData;
+
+/** FWSyncHeader */
+typedef struct _FWSyncHeader
+{
+    /** FW sync header command */
+    t_u32 cmd;
+    /** FW sync header sequence number */
+    t_u32 seq_num;
+} FWSyncHeader;
+
+#ifdef BIG_ENDIAN_SUPPORT
+/** Convert sequence number and command fields of fwheader to correct endian format */
+#define endian_convert_syncfwheader(x)  { \
+		(x)->cmd = wlan_le32_to_cpu((x)->cmd); \
+		(x)->seq_num = wlan_le32_to_cpu((x)->seq_num); \
+	}
+#else
+/** Convert sequence number and command fields of fwheader to correct endian format */
+#define endian_convert_syncfwheader(x)
+#endif /* BIG_ENDIAN_SUPPORT */
+
+#endif /* _MLAN_INIT_H_ */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_ioctl.h b/drivers/net/wireless/sd8797/mlan/mlan_ioctl.h
new file mode 100644
index 000000000000..964de9197ffe
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_ioctl.h
@@ -0,0 +1,2981 @@
+/** @file mlan_ioctl.h
+ *
+ *  @brief This file declares the IOCTL data structures and APIs.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    11/07/2008: initial version
+******************************************************/
+
+#ifndef _MLAN_IOCTL_H_
+#define _MLAN_IOCTL_H_
+
+/** Enumeration for IOCTL request ID */
+enum _mlan_ioctl_req_id
+{
+    /* Scan Group */
+    MLAN_IOCTL_SCAN = 0x00010000,
+    MLAN_OID_SCAN_NORMAL,
+    MLAN_OID_SCAN_SPECIFIC_SSID,
+    MLAN_OID_SCAN_USER_CONFIG,
+    MLAN_OID_SCAN_CONFIG,
+    MLAN_OID_SCAN_GET_CURRENT_BSS,
+    MLAN_OID_SCAN_CANCEL,
+    MLAN_OID_SCAN_TABLE_FLUSH,
+    MLAN_OID_SCAN_BGSCAN_CONFIG,
+    /* BSS Configuration Group */
+    MLAN_IOCTL_BSS = 0x00020000,
+    MLAN_OID_BSS_START,
+    MLAN_OID_BSS_STOP,
+    MLAN_OID_BSS_MODE,
+    MLAN_OID_BSS_CHANNEL,
+    MLAN_OID_BSS_CHANNEL_LIST,
+    MLAN_OID_BSS_MAC_ADDR,
+    MLAN_OID_BSS_MULTICAST_LIST,
+    MLAN_OID_BSS_FIND_BSS,
+    MLAN_OID_IBSS_BCN_INTERVAL,
+    MLAN_OID_IBSS_ATIM_WINDOW,
+    MLAN_OID_IBSS_CHANNEL,
+#ifdef UAP_SUPPORT
+    MLAN_OID_UAP_BSS_CONFIG,
+    MLAN_OID_UAP_DEAUTH_STA,
+    MLAN_OID_UAP_BSS_RESET,
+#endif
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+    MLAN_OID_BSS_ROLE,
+#endif
+#ifdef WIFI_DIRECT_SUPPORT
+    MLAN_OID_WIFI_DIRECT_MODE,
+#endif
+
+    /* Radio Configuration Group */
+    MLAN_IOCTL_RADIO_CFG = 0x00030000,
+    MLAN_OID_RADIO_CTRL,
+    MLAN_OID_BAND_CFG,
+    MLAN_OID_ANT_CFG,
+#ifdef WIFI_DIRECT_SUPPORT
+    MLAN_OID_REMAIN_CHAN_CFG,
+#endif
+
+    /* SNMP MIB Group */
+    MLAN_IOCTL_SNMP_MIB = 0x00040000,
+    MLAN_OID_SNMP_MIB_RTS_THRESHOLD,
+    MLAN_OID_SNMP_MIB_FRAG_THRESHOLD,
+    MLAN_OID_SNMP_MIB_RETRY_COUNT,
+#if defined(UAP_SUPPORT)
+    MLAN_OID_SNMP_MIB_DOT11D,
+    MLAN_OID_SNMP_MIB_DOT11H,
+#endif
+    MLAN_OID_SNMP_MIB_DTIM_PERIOD,
+
+    /* Status Information Group */
+    MLAN_IOCTL_GET_INFO = 0x00050000,
+    MLAN_OID_GET_STATS,
+    MLAN_OID_GET_SIGNAL,
+    MLAN_OID_GET_FW_INFO,
+    MLAN_OID_GET_VER_EXT,
+    MLAN_OID_GET_BSS_INFO,
+    MLAN_OID_GET_DEBUG_INFO,
+#ifdef UAP_SUPPORT
+    MLAN_OID_UAP_STA_LIST,
+#endif
+
+    /* Security Configuration Group */
+    MLAN_IOCTL_SEC_CFG = 0x00060000,
+    MLAN_OID_SEC_CFG_AUTH_MODE,
+    MLAN_OID_SEC_CFG_ENCRYPT_MODE,
+    MLAN_OID_SEC_CFG_WPA_ENABLED,
+    MLAN_OID_SEC_CFG_ENCRYPT_KEY,
+    MLAN_OID_SEC_CFG_PASSPHRASE,
+    MLAN_OID_SEC_CFG_EWPA_ENABLED,
+    MLAN_OID_SEC_CFG_ESUPP_MODE,
+    MLAN_OID_SEC_CFG_WAPI_ENABLED,
+    MLAN_OID_SEC_CFG_PORT_CTRL_ENABLED,
+
+    /* Rate Group */
+    MLAN_IOCTL_RATE = 0x00070000,
+    MLAN_OID_RATE_CFG,
+    MLAN_OID_GET_DATA_RATE,
+    MLAN_OID_SUPPORTED_RATES,
+
+    /* Power Configuration Group */
+    MLAN_IOCTL_POWER_CFG = 0x00080000,
+    MLAN_OID_POWER_CFG,
+    MLAN_OID_POWER_CFG_EXT,
+
+    /* Power Management Configuration Group */
+    MLAN_IOCTL_PM_CFG = 0x00090000,
+    MLAN_OID_PM_CFG_IEEE_PS,
+    MLAN_OID_PM_CFG_HS_CFG,
+    MLAN_OID_PM_CFG_INACTIVITY_TO,
+    MLAN_OID_PM_CFG_DEEP_SLEEP,
+    MLAN_OID_PM_CFG_SLEEP_PD,
+    MLAN_OID_PM_CFG_PS_CFG,
+    MLAN_OID_PM_CFG_SLEEP_PARAMS,
+#ifdef UAP_SUPPORT
+    MLAN_OID_PM_CFG_PS_MODE,
+#endif /* UAP_SUPPORT */
+    MLAN_OID_PM_INFO,
+
+    /* WMM Configuration Group */
+    MLAN_IOCTL_WMM_CFG = 0x000A0000,
+    MLAN_OID_WMM_CFG_ENABLE,
+    MLAN_OID_WMM_CFG_QOS,
+    MLAN_OID_WMM_CFG_ADDTS,
+    MLAN_OID_WMM_CFG_DELTS,
+    MLAN_OID_WMM_CFG_QUEUE_CONFIG,
+    MLAN_OID_WMM_CFG_QUEUE_STATS,
+    MLAN_OID_WMM_CFG_QUEUE_STATUS,
+    MLAN_OID_WMM_CFG_TS_STATUS,
+
+    /* WPS Configuration Group */
+    MLAN_IOCTL_WPS_CFG = 0x000B0000,
+    MLAN_OID_WPS_CFG_SESSION,
+
+    /* 802.11n Configuration Group */
+    MLAN_IOCTL_11N_CFG = 0x000C0000,
+    MLAN_OID_11N_CFG_TX,
+    MLAN_OID_11N_HTCAP_CFG,
+    MLAN_OID_11N_CFG_ADDBA_REJECT,
+    MLAN_OID_11N_CFG_AGGR_PRIO_TBL,
+    MLAN_OID_11N_CFG_ADDBA_PARAM,
+    MLAN_OID_11N_CFG_MAX_TX_BUF_SIZE,
+    MLAN_OID_11N_CFG_AMSDU_AGGR_CTRL,
+    MLAN_OID_11N_CFG_SUPPORTED_MCS_SET,
+    MLAN_OID_11N_CFG_TX_BF_CAP,
+    MLAN_OID_11N_CFG_TX_BF_CFG,
+    MLAN_OID_11N_CFG_STREAM_CFG,
+
+    /* 802.11d Configuration Group */
+    MLAN_IOCTL_11D_CFG = 0x000D0000,
+#ifdef STA_SUPPORT
+    MLAN_OID_11D_CFG_ENABLE,
+    MLAN_OID_11D_CLR_CHAN_TABLE,
+#endif /* STA_SUPPORT */
+    MLAN_OID_11D_DOMAIN_INFO,
+
+    /* Register Memory Access Group */
+    MLAN_IOCTL_REG_MEM = 0x000E0000,
+    MLAN_OID_REG_RW,
+    MLAN_OID_EEPROM_RD,
+    MLAN_OID_MEM_RW,
+
+    /* Multi-Radio Configuration Group */
+    MLAN_IOCTL_MFR_CFG = 0x00100000,
+
+    /* 802.11h Configuration Group */
+    MLAN_IOCTL_11H_CFG = 0x00110000,
+    MLAN_OID_11H_CHANNEL_CHECK,
+    MLAN_OID_11H_LOCAL_POWER_CONSTRAINT,
+#if defined(DFS_TESTING_SUPPORT)
+    MLAN_OID_11H_DFS_TESTING,
+#endif
+
+    /* Miscellaneous Configuration Group */
+    MLAN_IOCTL_MISC_CFG = 0x00200000,
+    MLAN_OID_MISC_GEN_IE,
+    MLAN_OID_MISC_REGION,
+    MLAN_OID_MISC_WARM_RESET,
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+    MLAN_OID_MISC_SDIO_MPA_CTRL,
+#endif
+    MLAN_OID_MISC_HOST_CMD,
+    MLAN_OID_MISC_SYS_CLOCK,
+    MLAN_OID_MISC_SOFT_RESET,
+    MLAN_OID_MISC_WWS,
+    MLAN_OID_MISC_INIT_SHUTDOWN,
+    MLAN_OID_MISC_CUSTOM_IE,
+    MLAN_OID_MISC_TX_DATAPAUSE,
+    MLAN_OID_MISC_IP_ADDR,
+    MLAN_OID_MISC_MAC_CONTROL,
+    MLAN_OID_MISC_MEF_CFG,
+    MLAN_OID_MISC_CFP_CODE,
+    MLAN_OID_MISC_COUNTRY_CODE,
+    MLAN_OID_MISC_THERMAL,
+    MLAN_OID_MISC_RX_MGMT_IND,
+    MLAN_OID_MISC_SUBSCRIBE_EVENT,
+#ifdef DEBUG_LEVEL1
+    MLAN_OID_MISC_DRVDBG,
+#endif
+    MLAN_OID_MISC_OTP_USER_DATA,
+};
+
+/** Sub command size */
+#define MLAN_SUB_COMMAND_SIZE	4
+
+/** Enumeration for the action of IOCTL request */
+enum _mlan_act_ioctl
+{
+    MLAN_ACT_SET = 1,
+    MLAN_ACT_GET,
+    MLAN_ACT_CANCEL
+};
+
+/** Enumeration for generic enable/disable */
+enum _mlan_act_generic
+{
+    MLAN_ACT_DISABLE = 0,
+    MLAN_ACT_ENABLE = 1
+};
+
+/** Enumeration for scan mode */
+enum _mlan_scan_mode
+{
+    MLAN_SCAN_MODE_UNCHANGED = 0,
+    MLAN_SCAN_MODE_BSS,
+    MLAN_SCAN_MODE_IBSS,
+    MLAN_SCAN_MODE_ANY
+};
+
+/** Enumeration for scan type */
+enum _mlan_scan_type
+{
+    MLAN_SCAN_TYPE_UNCHANGED = 0,
+    MLAN_SCAN_TYPE_ACTIVE,
+    MLAN_SCAN_TYPE_PASSIVE
+};
+
+/** Max number of supported rates */
+#define MLAN_SUPPORTED_RATES	32
+
+/** RSSI scan */
+#define SCAN_RSSI(RSSI)			(0x100 - ((t_u8)(RSSI)))
+
+/** Max passive scan time for each channel in milliseconds */
+#define MRVDRV_MAX_PASSIVE_SCAN_CHAN_TIME   2000
+
+/** Max active scan time for each channel in milliseconds  */
+#define MRVDRV_MAX_ACTIVE_SCAN_CHAN_TIME    500
+
+/** Maximum number of probes to send on each channel */
+#define MAX_PROBES      4
+
+/** Default number of probes to send on each channel */
+#define DEFAULT_PROBES  4
+
+/**
+ *  @brief Sub-structure passed in wlan_ioctl_get_scan_table_entry for each BSS
+ *
+ *  Fixed field information returned for the scan response in the IOCTL
+ *    response.
+ */
+typedef struct _wlan_get_scan_table_fixed
+{
+    /** BSSID of this network */
+    t_u8 bssid[MLAN_MAC_ADDR_LENGTH];
+    /** Channel this beacon/probe response was detected */
+    t_u8 channel;
+    /** RSSI for the received packet */
+    t_u8 rssi;
+    /** TSF value in microseconds from the firmware at packet reception */
+    t_u64 network_tsf;
+} wlan_get_scan_table_fixed;
+
+/** mlan_802_11_ssid data structure */
+typedef struct _mlan_802_11_ssid
+{
+    /** SSID Length */
+    t_u32 ssid_len;
+    /** SSID information field */
+    t_u8 ssid[MLAN_MAX_SSID_LENGTH];
+} mlan_802_11_ssid, *pmlan_802_11_ssid;
+
+/**
+ *  Sructure to retrieve the scan table
+ */
+typedef struct
+{
+    /**
+     *  - Zero based scan entry to start retrieval in command request
+     *  - Number of scans entries returned in command response
+     */
+    t_u32 scan_number;
+    /**
+     * Buffer marker for multiple wlan_ioctl_get_scan_table_entry structures.
+     *   Each struct is padded to the nearest 32 bit boundary.
+     */
+    t_u8 scan_table_entry_buf[1];
+} wlan_ioctl_get_scan_table_info;
+
+/**
+ *  Structure passed in the wlan_ioctl_get_scan_table_info for each
+ *    BSS returned in the WLAN_GET_SCAN_RESP IOCTL
+ */
+typedef struct _wlan_ioctl_get_scan_table_entry
+{
+    /**
+     *  Fixed field length included in the response.
+     *
+     *  Length value is included so future fixed fields can be added to the
+     *   response without breaking backwards compatibility.  Use the length
+     *   to find the offset for the bssInfoLength field, not a sizeof() calc.
+     */
+    t_u32 fixed_field_length;
+
+    /**
+     *  Length of the BSS Information (probe resp or beacon) that
+     *    follows after the fixed_field_length
+     */
+    t_u32 bss_info_length;
+
+    /**
+     *  Always present, fixed length data fields for the BSS
+     */
+    wlan_get_scan_table_fixed fixed_fields;
+
+    /*
+     *  Probe response or beacon scanned for the BSS.
+     *
+     *  Field layout:
+     *   - TSF              8 octets
+     *   - Beacon Interval  2 octets
+     *   - Capability Info  2 octets
+     *
+     *   - IEEE Infomation Elements; variable number & length per 802.11 spec
+     */
+    /* t_u8 bss_info_buffer[0]; */
+} wlan_ioctl_get_scan_table_entry;
+
+/** Type definition of mlan_scan_time_params */
+typedef struct _mlan_scan_time_params
+{
+    /** Scan channel time for specific scan in milliseconds */
+    t_u32 specific_scan_time;
+    /** Scan channel time for active scan in milliseconds */
+    t_u32 active_scan_time;
+    /** Scan channel time for passive scan in milliseconds */
+    t_u32 passive_scan_time;
+} mlan_scan_time_params, *pmlan_scan_time_params;
+
+/** Type definition of mlan_user_scan */
+typedef struct _mlan_user_scan
+{
+    /** Length of scan_cfg_buf */
+    t_u32 scan_cfg_len;
+    /** Buffer of scan config */
+    t_u8 scan_cfg_buf[1];
+} mlan_user_scan, *pmlan_user_scan;
+
+/** Type definition of mlan_scan_req */
+typedef struct _mlan_scan_req
+{
+    /** BSS mode for scanning */
+    t_u32 scan_mode;
+    /** Scan type */
+    t_u32 scan_type;
+    /** SSID */
+    mlan_802_11_ssid scan_ssid;
+    /** Scan time parameters */
+    mlan_scan_time_params scan_time;
+    /** Scan config parameters in user scan */
+    mlan_user_scan user_scan;
+} mlan_scan_req, *pmlan_scan_req;
+
+/** Type defnition of mlan_scan_resp */
+typedef struct _mlan_scan_resp
+{
+    /** Number of scan result */
+    t_u32 num_in_scan_table;
+    /** Scan table */
+    t_u8 *pscan_table;
+    /* Age in seconds */
+    t_u32 age_in_secs;
+} mlan_scan_resp, *pmlan_scan_resp;
+
+/** Type definition of mlan_scan_cfg */
+typedef struct _mlan_scan_cfg
+{
+    /** Scan type */
+    t_u32 scan_type;
+    /** BSS mode for scanning */
+    t_u32 scan_mode;
+    /** Scan probe */
+    t_u32 scan_probe;
+    /** Scan time parameters */
+    mlan_scan_time_params scan_time;
+    /** Extended Scan */
+    t_u32 ext_scan;
+} mlan_scan_cfg, *pmlan_scan_cfg;
+
+/** Type defnition of mlan_ds_scan for MLAN_IOCTL_SCAN */
+typedef struct _mlan_ds_scan
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Scan request/response */
+    union
+    {
+        /** Scan request */
+        mlan_scan_req scan_req;
+        /** Scan response */
+        mlan_scan_resp scan_resp;
+        /** Scan config parameters in user scan */
+        mlan_user_scan user_scan;
+        /** Scan config parameters */
+        mlan_scan_cfg scan_cfg;
+    } param;
+} mlan_ds_scan, *pmlan_ds_scan;
+
+/*-----------------------------------------------------------------*/
+/** BSS Configuration Group */
+/*-----------------------------------------------------------------*/
+/** Enumeration for BSS mode */
+enum _mlan_bss_mode
+{
+    MLAN_BSS_MODE_INFRA = 1,
+    MLAN_BSS_MODE_IBSS,
+    MLAN_BSS_MODE_AUTO
+};
+
+/** Maximum key length */
+#define MLAN_MAX_KEY_LENGTH             32
+
+/** max Wmm AC queues */
+#define MAX_AC_QUEUES                   4
+
+/** Maximum atim window in milliseconds */
+#define MLAN_MAX_ATIM_WINDOW		50
+
+/** Minimum beacon interval */
+#define MLAN_MIN_BEACON_INTERVAL        20
+/** Maximum beacon interval */
+#define MLAN_MAX_BEACON_INTERVAL        1000
+/** Default beacon interval */
+#define MLAN_BEACON_INTERVAL            100
+
+/** Receive all packets */
+#define MLAN_PROMISC_MODE     	1
+/** Receive multicast packets in multicast list */
+#define MLAN_MULTICAST_MODE		2
+/** Receive all multicast packets */
+#define	MLAN_ALL_MULTI_MODE		4
+
+/** Maximum size of multicast list */
+#define MLAN_MAX_MULTICAST_LIST_SIZE	32
+
+/** mlan_multicast_list data structure for MLAN_OID_BSS_MULTICAST_LIST */
+typedef struct _mlan_multicast_list
+{
+    /** Multicast mode */
+    t_u32 mode;
+    /** Number of multicast addresses in the list */
+    t_u32 num_multicast_addr;
+    /** Multicast address list */
+    mlan_802_11_mac_addr mac_list[MLAN_MAX_MULTICAST_LIST_SIZE];
+} mlan_multicast_list, *pmlan_multicast_list;
+
+/** Max channel */
+#define MLAN_MAX_CHANNEL    165
+
+/** Maximum number of channels in table */
+#define MLAN_MAX_CHANNEL_NUM	128
+
+/** Channel/frequence for MLAN_OID_BSS_CHANNEL */
+typedef struct _chan_freq
+{
+    /** Channel Number */
+    t_u32 channel;
+    /** Frequency of this Channel */
+    t_u32 freq;
+} chan_freq;
+
+/** mlan_chan_list data structure for MLAN_OID_BSS_CHANNEL_LIST */
+typedef struct _mlan_chan_list
+{
+    /** Number of channel */
+    t_u32 num_of_chan;
+    /** Channel-Frequency table */
+    chan_freq cf[MLAN_MAX_CHANNEL_NUM];
+} mlan_chan_list;
+
+/** mlan_ssid_bssid  data structure for MLAN_OID_BSS_START and MLAN_OID_BSS_FIND_BSS */
+typedef struct _mlan_ssid_bssid
+{
+    /** SSID */
+    mlan_802_11_ssid ssid;
+    /** BSSID */
+    mlan_802_11_mac_addr bssid;
+    /** index in BSSID list, start from 1 */
+    t_u32 idx;
+} mlan_ssid_bssid;
+
+#ifdef UAP_SUPPORT
+/** Maximum packet forward control value */
+#define MAX_PKT_FWD_CTRL 15
+/** Maximum BEACON period */
+#define MAX_BEACON_PERIOD 4000
+/** Minimum BEACON period */
+#define MIN_BEACON_PERIOD 50
+/** Maximum DTIM period */
+#define MAX_DTIM_PERIOD 100
+/** Minimum DTIM period */
+#define MIN_DTIM_PERIOD 1
+/** Maximum TX Power Limit */
+#define MAX_TX_POWER    20
+/** Minimum TX Power Limit */
+#define MIN_TX_POWER    0
+/** MAX station count */
+#define MAX_STA_COUNT   10
+/** Maximum RTS threshold */
+#define MAX_RTS_THRESHOLD   2347
+/** Maximum fragmentation threshold */
+#define MAX_FRAG_THRESHOLD 2346
+/** Minimum fragmentation threshold */
+#define MIN_FRAG_THRESHOLD 256
+/** data rate 54 M */
+#define DATA_RATE_54M   108
+/** antenna A */
+#define ANTENNA_MODE_A      0
+/** antenna B */
+#define ANTENNA_MODE_B      1
+/** transmit antenna */
+#define TX_ANTENNA          1
+/** receive antenna */
+#define RX_ANTENNA          0
+/** Maximum stage out time */
+#define MAX_STAGE_OUT_TIME  864000
+/** Minimum stage out time */
+#define MIN_STAGE_OUT_TIME  300
+/** Maximum Retry Limit */
+#define MAX_RETRY_LIMIT         14
+
+/** Maximum group key timer in seconds */
+#define MAX_GRP_TIMER           86400
+
+/** Maximum value of 4 byte configuration */
+#define MAX_VALID_DWORD         0x7FFFFFFF      /* (1 << 31) - 1 */
+
+/** Band config ACS mode */
+#define BAND_CONFIG_ACS_MODE    0x40
+/** Band config manual */
+#define BAND_CONFIG_MANUAL      0x00
+
+/** Maximum data rates */
+#define MAX_DATA_RATES          14
+
+/** auto data rate */
+#define DATA_RATE_AUTO       0
+
+/**filter mode: disable */
+#define MAC_FILTER_MODE_DISABLE         0
+/**filter mode: block mac address */
+#define MAC_FILTER_MODE_ALLOW_MAC       1
+/**filter mode: block mac address */
+#define MAC_FILTER_MODE_BLOCK_MAC       2
+/** Maximum mac filter num */
+#define MAX_MAC_FILTER_NUM           16
+
+/* Bitmap for protocol to use */
+/** No security */
+#define PROTOCOL_NO_SECURITY        0x01
+/** Static WEP */
+#define PROTOCOL_STATIC_WEP         0x02
+/** WPA */
+#define PROTOCOL_WPA                0x08
+/** WPA2 */
+#define PROTOCOL_WPA2               0x20
+/** WP2 Mixed */
+#define PROTOCOL_WPA2_MIXED         0x28
+/** EAP */
+#define PROTOCOL_EAP                0x40
+/** WAPI */
+#define PROTOCOL_WAPI               0x80
+
+/** Key_mgmt_psk */
+#define KEY_MGMT_NONE   0x04
+/** Key_mgmt_none */
+#define KEY_MGMT_PSK    0x02
+/** Key_mgmt_eap  */
+#define KEY_MGMT_EAP    0x01
+
+/** TKIP */
+#define CIPHER_TKIP                 0x04
+/** AES CCMP */
+#define CIPHER_AES_CCMP             0x08
+
+/** Valid cipher bitmap */
+#define VALID_CIPHER_BITMAP         0x0c
+
+/** Channel List Entry */
+typedef struct _channel_list
+{
+    /** Channel Number */
+    t_u8 chan_number;
+    /** Band Config */
+    t_u8 band_config_type;
+} scan_chan_list;
+
+/** mac_filter data structure */
+typedef struct _mac_filter
+{
+    /** mac filter mode */
+    t_u16 filter_mode;
+    /** mac adress count */
+    t_u16 mac_count;
+    /** mac address list */
+    mlan_802_11_mac_addr mac_list[MAX_MAC_FILTER_NUM];
+} mac_filter;
+
+/** wpa parameter */
+typedef struct _wpa_param
+{
+    /** Pairwise cipher WPA */
+    t_u8 pairwise_cipher_wpa;
+    /** Pairwise cipher WPA2 */
+    t_u8 pairwise_cipher_wpa2;
+    /** group cipher */
+    t_u8 group_cipher;
+    /** RSN replay protection */
+    t_u8 rsn_protection;
+    /** passphrase length */
+    t_u32 length;
+    /** passphrase */
+    t_u8 passphrase[64];
+    /**group key rekey time in seconds */
+    t_u32 gk_rekey_time;
+} wpa_param;
+
+/** wep key */
+typedef struct _wep_key
+{
+    /** key index 0-3 */
+    t_u8 key_index;
+    /** is default */
+    t_u8 is_default;
+    /** length */
+    t_u16 length;
+    /** key data */
+    t_u8 key[26];
+} wep_key;
+
+/** wep param */
+typedef struct _wep_param
+{
+    /** key 0 */
+    wep_key key0;
+    /** key 1 */
+    wep_key key1;
+    /** key 2 */
+    wep_key key2;
+    /** key 3 */
+    wep_key key3;
+} wep_param;
+
+/** Data structure of WMM QoS information */
+typedef struct _wmm_qos_info_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** QoS UAPSD */
+    t_u8 qos_uapsd:1;
+    /** Reserved */
+    t_u8 reserved:3;
+    /** Parameter set count */
+    t_u8 para_set_count:4;
+#else
+    /** Parameter set count */
+    t_u8 para_set_count:4;
+    /** Reserved */
+    t_u8 reserved:3;
+    /** QoS UAPSD */
+    t_u8 qos_uapsd:1;
+#endif                          /* BIG_ENDIAN_SUPPORT */
+} wmm_qos_info_t, *pwmm_qos_info_t;
+
+/** Data structure of WMM ECW */
+typedef struct _wmm_ecw_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** Maximum Ecw */
+    t_u8 ecw_max:4;
+    /** Minimum Ecw */
+    t_u8 ecw_min:4;
+#else
+    /** Minimum Ecw */
+    t_u8 ecw_min:4;
+    /** Maximum Ecw */
+    t_u8 ecw_max:4;
+#endif                          /* BIG_ENDIAN_SUPPORT */
+} wmm_ecw_t, *pwmm_ecw_t;
+
+/** Data structure of WMM Aci/Aifsn */
+typedef struct _wmm_aci_aifsn_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** Reserved */
+    t_u8 reserved:1;
+    /** Aci */
+    t_u8 aci:2;
+    /** Acm */
+    t_u8 acm:1;
+    /** Aifsn */
+    t_u8 aifsn:4;
+#else
+    /** Aifsn */
+    t_u8 aifsn:4;
+    /** Acm */
+    t_u8 acm:1;
+    /** Aci */
+    t_u8 aci:2;
+    /** Reserved */
+    t_u8 reserved:1;
+#endif                          /* BIG_ENDIAN_SUPPORT */
+} wmm_aci_aifsn_t, *pwmm_aci_aifsn_t;
+
+/** Data structure of WMM AC parameters  */
+typedef struct _wmm_ac_parameters_t
+{
+    wmm_aci_aifsn_t aci_aifsn;       /**< AciAifSn */
+    wmm_ecw_t ecw;                 /**< Ecw */
+    t_u16 tx_op_limit;                        /**< Tx op limit */
+} wmm_ac_parameters_t, *pwmm_ac_parameters_t;
+
+/** Data structure of WMM parameter IE  */
+typedef struct _wmm_parameter_t
+{
+    /** OuiType:  00:50:f2:02 */
+    t_u8 ouitype[4];
+    /** Oui subtype: 01 */
+    t_u8 ouisubtype;
+    /** version: 01 */
+    t_u8 version;
+    /** QoS information */
+    t_u8 qos_info;
+    /** Reserved */
+    t_u8 reserved;
+    /** AC Parameters Record WMM_AC_BE, WMM_AC_BK, WMM_AC_VI, WMM_AC_VO */
+    wmm_ac_parameters_t ac_params[MAX_AC_QUEUES];
+} wmm_parameter_t, *pwmm_parameter_t;
+
+/** mlan_bss_param
+ * Note: For each entry you must enter an invalid value
+ * in the MOAL function woal_set_sys_config_invalid_data().
+ * Otherwise for a valid data an unwanted TLV will be
+ * added to that command.
+ */
+typedef struct _mlan_uap_bss_param
+{
+    /** AP mac addr */
+    mlan_802_11_mac_addr mac_addr;
+    /** SSID */
+    mlan_802_11_ssid ssid;
+    /** Broadcast ssid control */
+    t_u8 bcast_ssid_ctl;
+    /** Radio control: on/off */
+    t_u8 radio_ctl;
+    /** dtim period */
+    t_u8 dtim_period;
+    /** beacon period */
+    t_u16 beacon_period;
+    /** rates */
+    t_u8 rates[MAX_DATA_RATES];
+    /** Tx data rate */
+    t_u16 tx_data_rate;
+    /** multicast/broadcast data rate */
+    t_u16 mcbc_data_rate;
+    /** Tx power level in dBm */
+    t_u8 tx_power_level;
+    /** Tx antenna */
+    t_u8 tx_antenna;
+    /** Rx antenna */
+    t_u8 rx_antenna;
+    /** packet forward control */
+    t_u8 pkt_forward_ctl;
+    /** max station count */
+    t_u16 max_sta_count;
+    /** mac filter */
+    mac_filter filter;
+    /** station ageout timer in unit of 100ms  */
+    t_u32 sta_ageout_timer;
+    /** PS station ageout timer in unit of 100ms  */
+    t_u32 ps_sta_ageout_timer;
+    /** RTS threshold */
+    t_u16 rts_threshold;
+    /** fragmentation threshold */
+    t_u16 frag_threshold;
+    /**  retry_limit */
+    t_u16 retry_limit;
+    /**  pairwise update timeout in milliseconds */
+    t_u32 pairwise_update_timeout;
+    /** pairwise handshake retries */
+    t_u32 pwk_retries;
+    /**  groupwise update timeout in milliseconds */
+    t_u32 groupwise_update_timeout;
+    /** groupwise handshake retries */
+    t_u32 gwk_retries;
+    /** preamble type */
+    t_u8 preamble_type;
+    /** band cfg */
+    t_u8 band_cfg;
+    /** channel */
+    t_u8 channel;
+    /** auth mode */
+    t_u16 auth_mode;
+    /** encryption protocol */
+    t_u16 protocol;
+    /** key managment type */
+    t_u16 key_mgmt;
+    /** wep param */
+    wep_param wep_cfg;
+    /** wpa param */
+    wpa_param wpa_cfg;
+    /** Mgmt IE passthru mask */
+    t_u32 mgmt_ie_passthru_mask;
+    /*
+     * 11n HT Cap  HTCap_t  ht_cap
+     */
+    /** HT Capabilities Info field */
+    t_u16 ht_cap_info;
+    /** A-MPDU Parameters field */
+    t_u8 ampdu_param;
+    /** Supported MCS Set field */
+    t_u8 supported_mcs_set[16];
+    /** HT Extended Capabilities field */
+    t_u16 ht_ext_cap;
+    /** Transmit Beamforming Capabilities field */
+    t_u32 tx_bf_cap;
+    /** Antenna Selection Capability field */
+    t_u8 asel;
+    /** Enable 2040 Coex */
+    t_u8 enable_2040coex;
+    /** key management operation */
+    t_u16 key_mgmt_operation;
+    /** BSS status */
+    t_u16 bss_status;
+#ifdef WIFI_DIRECT_SUPPORT
+    /* pre shared key */
+    t_u8 psk[MLAN_MAX_KEY_LENGTH];
+#endif                          /* WIFI_DIRECT_SUPPORT */
+    /** Number of channels in scan_channel_list */
+    t_u32 num_of_chan;
+    /** scan channel list in ACS mode */
+    scan_chan_list chan_list[MLAN_MAX_CHANNEL];
+    /** Wmm parameters */
+    wmm_parameter_t wmm_para;
+} mlan_uap_bss_param;
+
+/** mlan_deauth_param */
+typedef struct _mlan_deauth_param
+{
+    /** STA mac addr */
+    t_u8 mac_addr[MLAN_MAC_ADDR_LENGTH];
+    /** deauth reason */
+    t_u16 reason_code;
+} mlan_deauth_param;
+#endif
+
+#ifdef WIFI_DIRECT_SUPPORT
+/** mode: disable wifi direct */
+#define WIFI_DIRECT_MODE_DISABLE		0
+/** mode: listen */
+#define WIFI_DIRECT_MODE_LISTEN			1
+/** mode: GO */
+#define WIFI_DIRECT_MODE_GO		        2
+/** mode: client */
+#define WIFI_DIRECT_MODE_CLIENT			3
+/** mode: find */
+#define WIFI_DIRECT_MODE_FIND			4
+/** mode: stop find */
+#define WIFI_DIRECT_MODE_STOP_FIND		5
+#endif
+
+/** Type definition of mlan_ds_bss for MLAN_IOCTL_BSS */
+typedef struct _mlan_ds_bss
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** BSS parameter */
+    union
+    {
+        /** SSID-BSSID for MLAN_OID_BSS_START */
+        mlan_ssid_bssid ssid_bssid;
+        /** BSSID for MLAN_OID_BSS_STOP */
+        mlan_802_11_mac_addr bssid;
+        /** BSS mode for MLAN_OID_BSS_MODE */
+        t_u32 bss_mode;
+        /** BSS channel/frequency for MLAN_OID_BSS_CHANNEL */
+        chan_freq bss_chan;
+        /** BSS channel list for MLAN_OID_BSS_CHANNEL_LIST */
+        mlan_chan_list chanlist;
+        /** MAC address for MLAN_OID_BSS_MAC_ADDR */
+        mlan_802_11_mac_addr mac_addr;
+        /** Multicast list for MLAN_OID_BSS_MULTICAST_LIST */
+        mlan_multicast_list multicast_list;
+        /** Beacon interval for MLAN_OID_IBSS_BCN_INTERVAL */
+        t_u32 bcn_interval;
+        /** ATIM window for MLAN_OID_IBSS_ATIM_WINDOW */
+        t_u32 atim_window;
+#ifdef UAP_SUPPORT
+        /** BSS param for AP mode */
+        mlan_uap_bss_param bss_config;
+        /** deauth param for MLAN_OID_UAP_DEAUTH_STA */
+        mlan_deauth_param deauth_param;
+#endif
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+        /** BSS role */
+        t_u8 bss_role;
+#endif
+#ifdef WIFI_DIRECT_SUPPORT
+        t_u16 wfd_mode;
+#endif
+    } param;
+} mlan_ds_bss, *pmlan_ds_bss;
+
+/*-----------------------------------------------------------------*/
+/** Radio Control Group */
+/*-----------------------------------------------------------------*/
+/** Enumeration for band */
+enum _mlan_band_def
+{
+    BAND_B = 1,
+    BAND_G = 2,
+    BAND_A = 4,
+    BAND_GN = 8,
+    BAND_AN = 16,
+};
+
+/** NO secondary channel */
+#define NO_SEC_CHANNEL               0
+/** secondary channel is above primary channel */
+#define SEC_CHANNEL_ABOVE            1
+/** secondary channel is below primary channel */
+#define SEC_CHANNEL_BELOW            3
+/** Channel bandwidth */
+#define CHANNEL_BW_20MHZ             0
+#define CHANNEL_BW_40MHZ_ABOVE       1
+#define CHANNEL_BW_40MHZ_BELOW       3
+
+/** Type definition of mlan_ds_band_cfg for MLAN_OID_BAND_CFG */
+typedef struct _mlan_ds_band_cfg
+{
+    /** Infra band */
+    t_u32 config_bands;
+    /** Ad-hoc start band */
+    t_u32 adhoc_start_band;
+    /** Ad-hoc start channel */
+    t_u32 adhoc_channel;
+    /** Ad-hoc channel bandwidth */
+    t_u32 sec_chan_offset;
+    /** fw supported band */
+    t_u32 fw_bands;
+} mlan_ds_band_cfg;
+
+/** Type definition of mlan_ds_ant_cfg for MLAN_OID_ANT_CFG */
+typedef struct _mlan_ds_ant_cfg
+{
+    /** Tx antenna mode */
+    t_u32 tx_antenna;
+    /** Rx antenna mode */
+    t_u32 rx_antenna;
+} mlan_ds_ant_cfg, *pmlan_ds_ant_cfg;
+
+#ifdef WIFI_DIRECT_SUPPORT
+/** Type definition of mlan_ds_remain_chan for MLAN_OID_REMAIN_CHAN_CFG */
+typedef struct _mlan_ds_remain_chan
+{
+    /** remove flag */
+    t_u16 remove;
+    /** status */
+    t_u8 status;
+    /** Band cfg */
+    t_u8 bandcfg;
+    /** channel */
+    t_u8 channel;
+    /** remain time: Unit ms*/
+    t_u32 remain_period;
+} mlan_ds_remain_chan, *pmlan_ds_remain_chan;
+#endif
+
+/** Type definition of mlan_ds_radio_cfg for MLAN_IOCTL_RADIO_CFG */
+typedef struct _mlan_ds_radio_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Radio control parameter */
+    union
+    {
+        /** Radio on/off for MLAN_OID_RADIO_CTRL */
+        t_u32 radio_on_off;
+        /** Band info for MLAN_OID_BAND_CFG */
+        mlan_ds_band_cfg band_cfg;
+        /** Antenna info for MLAN_OID_ANT_CFG */
+        mlan_ds_ant_cfg ant_cfg;
+        /** Antenna info for MLAN_OID_ANT_CFG */
+        t_u32 antenna;
+#ifdef WIFI_DIRECT_SUPPORT
+        /** remain on channel for MLAN_OID_REMAIN_CHAN_CFG */
+        mlan_ds_remain_chan remain_chan;
+#endif
+    } param;
+} mlan_ds_radio_cfg, *pmlan_ds_radio_cfg;
+
+/*-----------------------------------------------------------------*/
+/** SNMP MIB Group */
+/*-----------------------------------------------------------------*/
+/** Type definition of mlan_ds_snmp_mib for MLAN_IOCTL_SNMP_MIB */
+typedef struct _mlan_ds_snmp_mib
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** SNMP MIB parameter */
+    union
+    {
+        /** RTS threshold for MLAN_OID_SNMP_MIB_RTS_THRESHOLD */
+        t_u32 rts_threshold;
+        /** Fragment threshold for MLAN_OID_SNMP_MIB_FRAG_THRESHOLD */
+        t_u32 frag_threshold;
+        /** Retry count for MLAN_OID_SNMP_MIB_RETRY_COUNT */
+        t_u32 retry_count;
+#if defined(UAP_SUPPORT)
+        /** OID value for MLAN_OID_SNMP_MIB_DOT11D/H */
+        t_u32 oid_value;
+#endif
+        /** DTIM period for MLAN_OID_SNMP_MIB_DTIM_PERIOD */
+        t_u32 dtim_period;
+    } param;
+} mlan_ds_snmp_mib, *pmlan_ds_snmp_mib;
+
+/*-----------------------------------------------------------------*/
+/** Status Information Group */
+/*-----------------------------------------------------------------*/
+/** Enumeration for ad-hoc status */
+enum _mlan_adhoc_status
+{
+    ADHOC_IDLE,
+    ADHOC_STARTED,
+    ADHOC_JOINED,
+    ADHOC_COALESCED, ADHOC_STARTING
+};
+
+/** Type definition of mlan_ds_get_stats for MLAN_OID_GET_STATS */
+typedef struct _mlan_ds_get_stats
+{
+    /** Statistics counter */
+    /** Multicast transmitted frame count */
+    t_u32 mcast_tx_frame;
+    /** Failure count */
+    t_u32 failed;
+    /** Retry count */
+    t_u32 retry;
+    /** Multi entry count */
+    t_u32 multi_retry;
+    /** Duplicate frame count */
+    t_u32 frame_dup;
+    /** RTS success count */
+    t_u32 rts_success;
+    /** RTS failure count */
+    t_u32 rts_failure;
+    /** Ack failure count */
+    t_u32 ack_failure;
+    /** Rx fragmentation count */
+    t_u32 rx_frag;
+    /** Multicast Tx frame count */
+    t_u32 mcast_rx_frame;
+    /** FCS error count */
+    t_u32 fcs_error;
+    /** Tx frame count */
+    t_u32 tx_frame;
+    /** WEP ICV error count */
+    t_u32 wep_icv_error[4];
+} mlan_ds_get_stats, *pmlan_ds_get_stats;
+
+/** Type definition of mlan_ds_uap_stats for MLAN_OID_GET_STATS */
+typedef struct _mlan_ds_uap_stats
+{
+    /** tkip mic failures */
+    t_u32 tkip_mic_failures;
+    /** ccmp decrypt errors */
+    t_u32 ccmp_decrypt_errors;
+    /** wep undecryptable count */
+    t_u32 wep_undecryptable_count;
+    /** wep icv error count */
+    t_u32 wep_icv_error_count;
+    /** decrypt failure count */
+    t_u32 decrypt_failure_count;
+    /** dot11 multicast tx count */
+    t_u32 mcast_tx_count;
+    /** dot11 failed count */
+    t_u32 failed_count;
+    /** dot11 retry count */
+    t_u32 retry_count;
+    /** dot11 multi retry count */
+    t_u32 multi_retry_count;
+    /** dot11 frame duplicate count */
+    t_u32 frame_dup_count;
+    /** dot11 rts success count */
+    t_u32 rts_success_count;
+    /** dot11 rts failure count */
+    t_u32 rts_failure_count;
+    /** dot11 ack failure count */
+    t_u32 ack_failure_count;
+    /** dot11 rx ragment count */
+    t_u32 rx_fragment_count;
+    /** dot11 mcast rx frame count */
+    t_u32 mcast_rx_frame_count;
+    /** dot11 fcs error count */
+    t_u32 fcs_error_count;
+    /** dot11 tx frame count */
+    t_u32 tx_frame_count;
+    /** dot11 rsna tkip cm invoked */
+    t_u32 rsna_tkip_cm_invoked;
+    /** dot11 rsna 4way handshake failures */
+    t_u32 rsna_4way_hshk_failures;
+} mlan_ds_uap_stats, *pmlan_ds_uap_stats;
+
+/** Mask of last beacon RSSI */
+#define BCN_RSSI_LAST_MASK              0x00000001
+/** Mask of average beacon RSSI */
+#define BCN_RSSI_AVG_MASK               0x00000002
+/** Mask of last data RSSI */
+#define DATA_RSSI_LAST_MASK             0x00000004
+/** Mask of average data RSSI */
+#define DATA_RSSI_AVG_MASK              0x00000008
+/** Mask of last beacon SNR */
+#define BCN_SNR_LAST_MASK               0x00000010
+/** Mask of average beacon SNR */
+#define BCN_SNR_AVG_MASK                0x00000020
+/** Mask of last data SNR */
+#define DATA_SNR_LAST_MASK              0x00000040
+/** Mask of average data SNR */
+#define DATA_SNR_AVG_MASK               0x00000080
+/** Mask of last beacon NF */
+#define BCN_NF_LAST_MASK                0x00000100
+/** Mask of average beacon NF */
+#define BCN_NF_AVG_MASK                 0x00000200
+/** Mask of last data NF */
+#define DATA_NF_LAST_MASK               0x00000400
+/** Mask of average data NF */
+#define DATA_NF_AVG_MASK                0x00000800
+/** Mask of all RSSI_INFO */
+#define ALL_RSSI_INFO_MASK              0x00000fff
+
+/** Type definition of mlan_ds_get_signal for MLAN_OID_GET_SIGNAL */
+typedef struct _mlan_ds_get_signal
+{
+    /** Selector of get operation */
+    /*
+     * Bit0:  Last Beacon RSSI,  Bit1:  Average Beacon RSSI,
+     * Bit2:  Last Data RSSI,    Bit3:  Average Data RSSI,
+     * Bit4:  Last Beacon SNR,   Bit5:  Average Beacon SNR,
+     * Bit6:  Last Data SNR,     Bit7:  Average Data SNR,
+     * Bit8:  Last Beacon NF,    Bit9:  Average Beacon NF,
+     * Bit10: Last Data NF,      Bit11: Average Data NF
+     */
+    t_u16 selector;
+
+    /** RSSI */
+    /** RSSI of last beacon */
+    t_s16 bcn_rssi_last;
+    /** RSSI of beacon average */
+    t_s16 bcn_rssi_avg;
+    /** RSSI of last data packet */
+    t_s16 data_rssi_last;
+    /** RSSI of data packet average */
+    t_s16 data_rssi_avg;
+
+    /** SNR */
+    /** SNR of last beacon */
+    t_s16 bcn_snr_last;
+    /** SNR of beacon average */
+    t_s16 bcn_snr_avg;
+    /** SNR of last data packet */
+    t_s16 data_snr_last;
+    /** SNR of data packet average */
+    t_s16 data_snr_avg;
+
+    /** NF */
+    /** NF of last beacon */
+    t_s16 bcn_nf_last;
+    /** NF of beacon average */
+    t_s16 bcn_nf_avg;
+    /** NF of last data packet */
+    t_s16 data_nf_last;
+    /** NF of data packet average */
+    t_s16 data_nf_avg;
+} mlan_ds_get_signal, *pmlan_ds_get_signal;
+
+/** mlan_fw_info data structure for MLAN_OID_GET_FW_INFO */
+typedef struct _mlan_fw_info
+{
+    /** Firmware version */
+    t_u32 fw_ver;
+    /** MAC address */
+    mlan_802_11_mac_addr mac_addr;
+    /** Device support for MIMO abstraction of MCSs */
+    t_u8 hw_dev_mcs_support;
+        /** fw supported band */
+    t_u8 fw_bands;
+} mlan_fw_info, *pmlan_fw_info;
+
+/** Version string buffer length */
+#define MLAN_MAX_VER_STR_LEN    128
+
+/** mlan_ver_ext data structure for MLAN_OID_GET_VER_EXT */
+typedef struct _mlan_ver_ext
+{
+    /** Selected version string */
+    t_u32 version_str_sel;
+    /** Version string */
+    char version_str[MLAN_MAX_VER_STR_LEN];
+} mlan_ver_ext, *pmlan_ver_ext;
+
+/** mlan_bss_info data structure for MLAN_OID_GET_BSS_INFO */
+typedef struct _mlan_bss_info
+{
+    /** BSS mode */
+    t_u32 bss_mode;
+    /** SSID */
+    mlan_802_11_ssid ssid;
+    /** Table index */
+    t_u32 scan_table_idx;
+    /** Channel */
+    t_u32 bss_chan;
+    /** Band */
+    t_u8 bss_band;
+    /** Region code */
+    t_u32 region_code;
+    /** Connection status */
+    t_u32 media_connected;
+    /** Radio on */
+    t_u32 radio_on;
+    /** Max power level in dBm */
+    t_u32 max_power_level;
+    /** Min power level in dBm */
+    t_u32 min_power_level;
+    /** Adhoc state */
+    t_u32 adhoc_state;
+    /** NF of last beacon */
+    t_s32 bcn_nf_last;
+    /** wep status */
+    t_u32 wep_status;
+     /** Host Sleep configured flag */
+    t_u32 is_hs_configured;
+    /** Deep Sleep flag */
+    t_u32 is_deep_sleep;
+    /** BSSID */
+    mlan_802_11_mac_addr bssid;
+#ifdef STA_SUPPORT
+    /** Capability Info */
+    t_u16 capability_info;
+    /** Beacon Interval */
+    t_u16 beacon_interval;
+    /** Listen Interval */
+    t_u16 listen_interval;
+    /** Association Id  */
+    t_u16 assoc_id;
+    /** AP/Peer supported rates */
+    t_u8 peer_supp_rates[MLAN_SUPPORTED_RATES];
+#endif                          /* STA_SUPPORT */
+} mlan_bss_info, *pmlan_bss_info;
+
+/** MAXIMUM number of TID */
+#define MAX_NUM_TID     8
+
+/** Max RX Win size */
+#define MAX_RX_WINSIZE 		64
+
+/** rx_reorder_tbl */
+typedef struct
+{
+    /** TID */
+    t_u16 tid;
+    /** TA */
+    t_u8 ta[MLAN_MAC_ADDR_LENGTH];
+    /** Start window */
+    t_u32 start_win;
+    /** Window size */
+    t_u32 win_size;
+    /** amsdu flag */
+    t_u8 amsdu;
+    /** buffer status */
+    t_u32 buffer[MAX_RX_WINSIZE];
+} rx_reorder_tbl;
+
+/** tx_ba_stream_tbl */
+typedef struct
+{
+    /** TID */
+    t_u16 tid;
+    /** RA */
+    t_u8 ra[MLAN_MAC_ADDR_LENGTH];
+    /** amsdu flag */
+    t_u8 amsdu;
+} tx_ba_stream_tbl;
+
+/** Debug command number */
+#define DBG_CMD_NUM	5
+
+/** mlan_debug_info data structure for MLAN_OID_GET_DEBUG_INFO */
+typedef struct _mlan_debug_info
+{
+    /* WMM AC_BK count */
+    t_u32 wmm_ac_bk;
+    /* WMM AC_BE count */
+    t_u32 wmm_ac_be;
+    /* WMM AC_VI count */
+    t_u32 wmm_ac_vi;
+    /* WMM AC_VO count */
+    t_u32 wmm_ac_vo;
+    /** Corresponds to max_tx_buf_size member of mlan_adapter*/
+    t_u32 max_tx_buf_size;
+     /** Corresponds to tx_buf_size member of mlan_adapter*/
+    t_u32 tx_buf_size;
+    /** Corresponds to curr_tx_buf_size member of mlan_adapter*/
+    t_u32 curr_tx_buf_size;
+    /** Tx table num */
+    t_u32 tx_tbl_num;
+    /** Tx ba stream table */
+    tx_ba_stream_tbl tx_tbl[MLAN_MAX_TX_BASTREAM_SUPPORTED];
+    /** Rx table num */
+    t_u32 rx_tbl_num;
+    /** Rx reorder table*/
+    rx_reorder_tbl rx_tbl[MLAN_MAX_RX_BASTREAM_SUPPORTED];
+    /** Corresponds to ps_mode member of mlan_adapter */
+    t_u16 ps_mode;
+    /** Corresponds to ps_state member of mlan_adapter */
+    t_u32 ps_state;
+#ifdef STA_SUPPORT
+    /** Corresponds to is_deep_sleep member of mlan_adapter */
+    t_u8 is_deep_sleep;
+#endif /** STA_SUPPORT */
+    /** Corresponds to pm_wakeup_card_req member of mlan_adapter */
+    t_u8 pm_wakeup_card_req;
+    /** Corresponds to pm_wakeup_fw_try member of mlan_adapter */
+    t_u32 pm_wakeup_fw_try;
+    /** Corresponds to is_hs_configured member of mlan_adapter */
+    t_u8 is_hs_configured;
+    /** Corresponds to hs_activated member of mlan_adapter */
+    t_u8 hs_activated;
+    /** Corresponds to pps_uapsd_mode member of mlan_adapter */
+    t_u16 pps_uapsd_mode;
+    /** Corresponds to sleep_period.period member of mlan_adapter */
+    t_u16 sleep_pd;
+    /** Corresponds to wmm_qosinfo member of mlan_private */
+    t_u8 qos_cfg;
+    /** Corresponds to tx_lock_flag member of mlan_adapter */
+    t_u8 tx_lock_flag;
+    /** Corresponds to port_open member of mlan_private */
+    t_u8 port_open;
+    /** Corresponds to scan_processing member of mlan_adapter */
+    t_u32 scan_processing;
+    /** Number of host to card command failures */
+    t_u32 num_cmd_host_to_card_failure;
+    /** Number of host to card sleep confirm failures */
+    t_u32 num_cmd_sleep_cfm_host_to_card_failure;
+    /** Number of host to card Tx failures */
+    t_u32 num_tx_host_to_card_failure;
+    /** Number of card to host command/event failures */
+    t_u32 num_cmdevt_card_to_host_failure;
+    /** Number of card to host Rx failures */
+    t_u32 num_rx_card_to_host_failure;
+    /** Number of interrupt read failures */
+    t_u32 num_int_read_failure;
+    /** Last interrupt status */
+    t_u32 last_int_status;
+    /** Number of deauthentication events */
+    t_u32 num_event_deauth;
+    /** Number of disassosiation events */
+    t_u32 num_event_disassoc;
+    /** Number of link lost events */
+    t_u32 num_event_link_lost;
+    /** Number of deauthentication commands */
+    t_u32 num_cmd_deauth;
+    /** Number of association comamnd successes */
+    t_u32 num_cmd_assoc_success;
+    /** Number of association command failures */
+    t_u32 num_cmd_assoc_failure;
+    /** Number of Tx timeouts */
+    t_u32 num_tx_timeout;
+    /** Number of command timeouts */
+    t_u32 num_cmd_timeout;
+    /** Timeout command ID */
+    t_u16 timeout_cmd_id;
+    /** Timeout command action */
+    t_u16 timeout_cmd_act;
+    /** List of last command IDs */
+    t_u16 last_cmd_id[DBG_CMD_NUM];
+    /** List of last command actions */
+    t_u16 last_cmd_act[DBG_CMD_NUM];
+    /** Last command index */
+    t_u16 last_cmd_index;
+    /** List of last command response IDs */
+    t_u16 last_cmd_resp_id[DBG_CMD_NUM];
+    /** Last command response index */
+    t_u16 last_cmd_resp_index;
+    /** List of last events */
+    t_u16 last_event[DBG_CMD_NUM];
+    /** Last event index */
+    t_u16 last_event_index;
+
+    /** Corresponds to data_sent member of mlan_adapter */
+    t_u8 data_sent;
+    /** Corresponds to cmd_sent member of mlan_adapter */
+    t_u8 cmd_sent;
+    /** SDIO multiple port read bitmap */
+    t_u32 mp_rd_bitmap;
+    /** SDIO multiple port write bitmap */
+    t_u32 mp_wr_bitmap;
+    /** Current available port for read */
+    t_u8 curr_rd_port;
+    /** Current available port for write */
+    t_u8 curr_wr_port;
+    /** Corresponds to cmdresp_received member of mlan_adapter */
+    t_u8 cmd_resp_received;
+    /** Corresponds to event_received member of mlan_adapter */
+    t_u8 event_received;
+    /**  pendig tx pkts */
+    t_u32 tx_pkts_queued;
+#ifdef UAP_SUPPORT
+    /**  pending bridge pkts */
+    t_u16 num_bridge_pkts;
+    /**  dropped pkts */
+    t_u32 num_drop_pkts;
+#endif
+} mlan_debug_info, *pmlan_debug_info;
+
+#ifdef UAP_SUPPORT
+/** Maximum number of clients supported by AP */
+#define MAX_NUM_CLIENTS         MAX_STA_COUNT
+
+/** station info */
+typedef struct _sta_info
+{
+    /** STA MAC address */
+    t_u8 mac_address[MLAN_MAC_ADDR_LENGTH];
+    /** Power mfg status */
+    t_u8 power_mfg_status;
+    /** RSSI */
+    t_s8 rssi;
+} sta_info;
+
+/** mlan_ds_sta_list structure for MLAN_OID_UAP_STA_LIST */
+typedef struct _mlan_ds_sta_list
+{
+    /** station count */
+    t_u16 sta_count;
+    /** station list */
+    sta_info info[MAX_NUM_CLIENTS];
+} mlan_ds_sta_list, *pmlan_ds_sta_list;
+#endif
+
+/** Type definition of mlan_ds_get_info for MLAN_IOCTL_GET_INFO */
+typedef struct _mlan_ds_get_info
+{
+    /** Sub-command */
+    t_u32 sub_command;
+
+    /** Status information parameter */
+    union
+    {
+        /** Signal information for MLAN_OID_GET_SIGNAL */
+        mlan_ds_get_signal signal;
+        /** Statistics information for MLAN_OID_GET_STATS */
+        mlan_ds_get_stats stats;
+        /** Firmware information for MLAN_OID_GET_FW_INFO */
+        mlan_fw_info fw_info;
+        /** Extended version information for MLAN_OID_GET_VER_EXT */
+        mlan_ver_ext ver_ext;
+        /** BSS information for MLAN_OID_GET_BSS_INFO */
+        mlan_bss_info bss_info;
+        /** Debug information for MLAN_OID_GET_DEBUG_INFO */
+        mlan_debug_info debug_info;
+#ifdef UAP_SUPPORT
+        /** UAP Statistics information for MLAN_OID_GET_STATS */
+        mlan_ds_uap_stats ustats;
+        /** UAP station list for MLAN_OID_UAP_STA_LIST */
+        mlan_ds_sta_list sta_list;
+#endif
+    } param;
+} mlan_ds_get_info, *pmlan_ds_get_info;
+
+/*-----------------------------------------------------------------*/
+/** Security Configuration Group */
+/*-----------------------------------------------------------------*/
+/** Enumeration for authentication mode */
+enum _mlan_auth_mode
+{
+    MLAN_AUTH_MODE_OPEN = 0x00,
+    MLAN_AUTH_MODE_SHARED = 0x01,
+    MLAN_AUTH_MODE_NETWORKEAP = 0x80,
+    MLAN_AUTH_MODE_AUTO = 0xFF,
+};
+
+/** Enumeration for encryption mode */
+enum _mlan_encryption_mode
+{
+    MLAN_ENCRYPTION_MODE_NONE = 0,
+    MLAN_ENCRYPTION_MODE_WEP40 = 1,
+    MLAN_ENCRYPTION_MODE_TKIP = 2,
+    MLAN_ENCRYPTION_MODE_CCMP = 3,
+    MLAN_ENCRYPTION_MODE_WEP104 = 4,
+};
+
+/** Enumeration for PSK */
+enum _mlan_psk_type
+{
+    MLAN_PSK_PASSPHRASE = 1,
+    MLAN_PSK_PMK,
+    MLAN_PSK_CLEAR,
+    MLAN_PSK_QUERY,
+};
+
+/** The bit to indicate the key is for unicast */
+#define MLAN_KEY_INDEX_UNICAST        0x40000000
+/** The key index to indicate default key */
+#define MLAN_KEY_INDEX_DEFAULT        0x000000ff
+/** Maximum key length */
+// #define MLAN_MAX_KEY_LENGTH        32
+/** Minimum passphrase length */
+#define MLAN_MIN_PASSPHRASE_LENGTH    8
+/** Maximum passphrase length */
+#define MLAN_MAX_PASSPHRASE_LENGTH    63
+/** PMK length */
+#define MLAN_PMK_HEXSTR_LENGTH        64
+/* A few details needed for WEP (Wireless Equivalent Privacy) */
+/** 104 bits */
+#define MAX_WEP_KEY_SIZE	13
+/** 40 bits RC4 - WEP */
+#define MIN_WEP_KEY_SIZE	5
+/** packet number size */
+#define PN_SIZE			16
+/** max seq size of wpa/wpa2 key */
+#define SEQ_MAX_SIZE        8
+
+/** key flag for tx_seq */
+#define KEY_FLAG_TX_SEQ_VALID	0x00000001
+/** key flag for rx_seq */
+#define KEY_FLAG_RX_SEQ_VALID	0x00000002
+/** key flag for group key */
+#define KEY_FLAG_GROUP_KEY      0x00000004
+/** key flag for tx and rx */
+#define KEY_FLAG_SET_TX_KEY	    0x00000008
+/** key flag for remove key */
+#define KEY_FLAG_REMOVE_KEY	    0x80000000
+
+/** Type definition of mlan_ds_encrypt_key for MLAN_OID_SEC_CFG_ENCRYPT_KEY */
+typedef struct _mlan_ds_encrypt_key
+{
+    /** Key disabled, all other fields will be ignore when this flag set to MTRUE */
+    t_u32 key_disable;
+    /** key removed flag, when this flag is set to MTRUE, only key_index will be check */
+    t_u32 key_remove;
+    /** Key index, used as current tx key index when is_current_wep_key is set to MTRUE */
+    t_u32 key_index;
+    /** Current Tx key flag */
+    t_u32 is_current_wep_key;
+    /** Key length */
+    t_u32 key_len;
+    /** Key */
+    t_u8 key_material[MLAN_MAX_KEY_LENGTH];
+    /** mac address */
+    t_u8 mac_addr[MLAN_MAC_ADDR_LENGTH];
+    /** wapi key flag */
+    t_u32 is_wapi_key;
+    /** Initial packet number */
+    t_u8 pn[PN_SIZE];
+    /** key flags */
+    t_u32 key_flags;
+} mlan_ds_encrypt_key, *pmlan_ds_encrypt_key;
+
+/** Type definition of mlan_passphrase_t */
+typedef struct _mlan_passphrase_t
+{
+    /** Length of passphrase */
+    t_u32 passphrase_len;
+    /** Passphrase */
+    t_u8 passphrase[MLAN_MAX_PASSPHRASE_LENGTH];
+} mlan_passphrase_t;
+
+/** Type defnition of mlan_pmk_t */
+typedef struct _mlan_pmk_t
+{
+    /** PMK */
+    t_u8 pmk[MLAN_MAX_KEY_LENGTH];
+} mlan_pmk_t;
+
+/** Embedded supplicant RSN type: No RSN */
+#define RSN_TYPE_NO_RSN     MBIT(0)
+/** Embedded supplicant RSN type: WPA */
+#define RSN_TYPE_WPA        MBIT(3)
+/** Embedded supplicant RSN type: WPA-NONE */
+#define RSN_TYPE_WPANONE    MBIT(4)
+/** Embedded supplicant RSN type: WPA2 */
+#define RSN_TYPE_WPA2       MBIT(5)
+/** Embedded supplicant RSN type: RFU */
+#define RSN_TYPE_VALID_BITS (RSN_TYPE_NO_RSN | RSN_TYPE_WPA | RSN_TYPE_WPANONE | RSN_TYPE_WPA2)
+
+/** Embedded supplicant cipher type: TKIP */
+#define EMBED_CIPHER_TKIP       MBIT(2)
+/** Embedded supplicant cipher type: AES */
+#define EMBED_CIPHER_AES        MBIT(3)
+/** Embedded supplicant cipher type: RFU */
+#define EMBED_CIPHER_VALID_BITS (EMBED_CIPHER_TKIP | EMBED_CIPHER_AES)
+
+/** Type definition of mlan_ds_passphrase for MLAN_OID_SEC_CFG_PASSPHRASE */
+typedef struct _mlan_ds_passphrase
+{
+    /** SSID may be used */
+    mlan_802_11_ssid ssid;
+    /** BSSID may be used */
+    mlan_802_11_mac_addr bssid;
+    /** Flag for passphrase or pmk used */
+    t_u16 psk_type;
+    /** Passphrase or PMK */
+    union
+    {
+        /** Passphrase */
+        mlan_passphrase_t passphrase;
+        /** PMK */
+        mlan_pmk_t pmk;
+    } psk;
+} mlan_ds_passphrase, *pmlan_ds_passphrase;
+
+/** Type definition of mlan_ds_esupp_mode for MLAN_OID_SEC_CFG_ESUPP_MODE */
+typedef struct _mlan_ds_ewpa_mode
+{
+    /** RSN mode */
+    t_u32 rsn_mode;
+    /** Active pairwise cipher */
+    t_u32 act_paircipher;
+    /** Active pairwise cipher */
+    t_u32 act_groupcipher;
+} mlan_ds_esupp_mode, *pmlan_ds_esupp_mode;
+
+/** Type definition of mlan_ds_sec_cfg for MLAN_IOCTL_SEC_CFG */
+typedef struct _mlan_ds_sec_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Security configuration parameter */
+    union
+    {
+        /** Authentication mode for MLAN_OID_SEC_CFG_AUTH_MODE */
+        t_u32 auth_mode;
+        /** Encryption mode for MLAN_OID_SEC_CFG_ENCRYPT_MODE */
+        t_u32 encrypt_mode;
+        /** WPA enabled flag for MLAN_OID_SEC_CFG_WPA_ENABLED */
+        t_u32 wpa_enabled;
+        /** WAPI enabled flag for MLAN_OID_SEC_CFG_WAPI_ENABLED */
+        t_u32 wapi_enabled;
+        /** Port Control enabled flag for MLAN_OID_SEC_CFG_PORT_CTRL */
+        t_u32 port_ctrl_enabled;
+        /** Encryption key for MLAN_OID_SEC_CFG_ENCRYPT_KEY */
+        mlan_ds_encrypt_key encrypt_key;
+        /** Passphrase for MLAN_OID_SEC_CFG_PASSPHRASE */
+        mlan_ds_passphrase passphrase;
+        /** Embedded supplicant WPA enabled flag for MLAN_OID_SEC_CFG_EWPA_ENABLED */
+        t_u32 ewpa_enabled;
+        /** Embedded supplicant mode for MLAN_OID_SEC_CFG_ESUPP_MODE */
+        mlan_ds_esupp_mode esupp_mode;
+    } param;
+} mlan_ds_sec_cfg, *pmlan_ds_sec_cfg;
+
+/*-----------------------------------------------------------------*/
+/** Rate Configuration Group */
+/*-----------------------------------------------------------------*/
+/** Enumeration for rate type */
+enum _mlan_rate_type
+{
+    MLAN_RATE_INDEX,
+    MLAN_RATE_VALUE
+};
+
+/** Enumeration for rate format */
+enum _mlan_rate_format
+{
+    MLAN_RATE_FORMAT_LG = 0,
+    MLAN_RATE_FORMAT_HT,
+    MLAN_RATE_FORMAT_AUTO = 0xFF,
+};
+/** Max bitmap rates size */
+#define MAX_BITMAP_RATES_SIZE   10
+
+/** Type definition of mlan_rate_cfg_t for MLAN_OID_RATE_CFG */
+typedef struct _mlan_rate_cfg_t
+{
+    /** Fixed rate: 0, auto rate: 1 */
+    t_u32 is_rate_auto;
+    /** Rate type. 0: index; 1: valude  */
+    t_u32 rate_type;
+    /** Rate/MCS index or rate value if fixed rate */
+    t_u32 rate;
+    /** Rate Bitmap */
+    t_u16 bitmap_rates[MAX_BITMAP_RATES_SIZE];
+} mlan_rate_cfg_t;
+
+/** HT channel bandwidth */
+typedef enum _mlan_ht_bw
+{
+    MLAN_HT_BW20,
+    MLAN_HT_BW40,
+} mlan_ht_bw;
+
+/** HT guard interval */
+typedef enum _mlan_ht_gi
+{
+    MLAN_HT_LGI,
+    MLAN_HT_SGI,
+} mlan_ht_gi;
+
+/** Band and BSS mode */
+typedef struct _mlan_band_data_rate
+{
+    /** Band configuration */
+    t_u8 config_bands;
+    /** BSS mode (Infra or IBSS) */
+    t_u8 bss_mode;
+} mlan_band_data_rate;
+
+/** Type definition of mlan_data_rate for MLAN_OID_GET_DATA_RATE */
+typedef struct _mlan_data_rate
+{
+    /** Tx data rate */
+    t_u32 tx_data_rate;
+    /** Rx data rate */
+    t_u32 rx_data_rate;
+
+    /** Tx channel bandwidth */
+    t_u32 tx_ht_bw;
+    /** Tx guard interval */
+    t_u32 tx_ht_gi;
+    /** Rx channel bandwidth */
+    t_u32 rx_ht_bw;
+    /** Rx guard interval */
+    t_u32 rx_ht_gi;
+} mlan_data_rate;
+
+/** Type definition of mlan_ds_rate for MLAN_IOCTL_RATE */
+typedef struct _mlan_ds_rate
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Rate configuration parameter */
+    union
+    {
+        /** Rate configuration for MLAN_OID_RATE_CFG */
+        mlan_rate_cfg_t rate_cfg;
+        /** Data rate for MLAN_OID_GET_DATA_RATE */
+        mlan_data_rate data_rate;
+        /** Supported rates for MLAN_OID_SUPPORTED_RATES */
+        t_u8 rates[MLAN_SUPPORTED_RATES];
+        /** Band/BSS mode for getting supported rates */
+        mlan_band_data_rate rate_band_cfg;
+    } param;
+} mlan_ds_rate, *pmlan_ds_rate;
+
+/*-----------------------------------------------------------------*/
+/** Power Configuration Group */
+/*-----------------------------------------------------------------*/
+
+/** Type definition of mlan_power_cfg_t for MLAN_OID_POWER_CFG */
+typedef struct _mlan_power_cfg_t
+{
+    /** Is power auto */
+    t_u32 is_power_auto;
+    /** Power level in dBm */
+    t_u32 power_level;
+} mlan_power_cfg_t;
+
+/** max power table size */
+#define MAX_POWER_TABLE_SIZE 	128
+
+/** The HT BW40 bit in Tx rate index */
+#define TX_RATE_HT_BW40_BIT 	MBIT(7)
+
+/** Type definition of mlan_power_cfg_ext for MLAN_OID_POWER_CFG_EXT */
+typedef struct _mlan_power_cfg_ext
+{
+    /** Length of power_data */
+    t_u32 len;
+    /** Buffer of power configuration data */
+    t_u32 power_data[MAX_POWER_TABLE_SIZE];
+} mlan_power_cfg_ext;
+
+/** Type definition of mlan_ds_power_cfg for MLAN_IOCTL_POWER_CFG */
+typedef struct _mlan_ds_power_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Power configuration parameter */
+    union
+    {
+        /** Power configuration for MLAN_OID_POWER_CFG */
+        mlan_power_cfg_t power_cfg;
+        /** Extended power configuration for MLAN_OID_POWER_CFG_EXT */
+        mlan_power_cfg_ext power_ext;
+    } param;
+} mlan_ds_power_cfg, *pmlan_ds_power_cfg;
+
+/*-----------------------------------------------------------------*/
+/** Power Management Configuration Group */
+/*-----------------------------------------------------------------*/
+/** Host sleep config conditions : Cancel */
+#define HOST_SLEEP_CFG_CANCEL   0xffffffff
+
+/** Host sleep config condition: broadcast data */
+#define HOST_SLEEP_COND_BROADCAST_DATA  MBIT(0)
+/** Host sleep config condition: unicast data */
+#define HOST_SLEEP_COND_UNICAST_DATA    MBIT(1)
+/** Host sleep config condition: mac event */
+#define HOST_SLEEP_COND_MAC_EVENT       MBIT(2)
+/** Host sleep config condition: multicast data */
+#define HOST_SLEEP_COND_MULTICAST_DATA  MBIT(3)
+
+/** Host sleep config conditions: Default */
+#define HOST_SLEEP_DEF_COND     (HOST_SLEEP_COND_BROADCAST_DATA | HOST_SLEEP_COND_UNICAST_DATA | HOST_SLEEP_COND_MAC_EVENT)
+/** Host sleep config GPIO : Default */
+#define HOST_SLEEP_DEF_GPIO     0xff
+/** Host sleep config gap : Default */
+#define HOST_SLEEP_DEF_GAP      200
+
+/** Type definition of mlan_ds_hs_cfg for MLAN_OID_PM_CFG_HS_CFG */
+typedef struct _mlan_ds_hs_cfg
+{
+    /** MTRUE to invoke the HostCmd, MFALSE otherwise */
+    t_u32 is_invoke_hostcmd;
+    /** Host sleep config condition */
+    /** Bit0: broadcast data
+     *  Bit1: unicast data
+     *  Bit2: mac event
+     *  Bit3: multicast data
+     */
+    t_u32 conditions;
+    /** GPIO pin or 0xff for interface */
+    t_u32 gpio;
+    /** Gap in milliseconds or or 0xff for special setting when GPIO is used to wakeup host */
+    t_u32 gap;
+} mlan_ds_hs_cfg, *pmlan_ds_hs_cfg;
+
+/** Enable deep sleep mode */
+#define DEEP_SLEEP_ON  1
+/** Disable deep sleep mode */
+#define DEEP_SLEEP_OFF 0
+
+/** Default idle time in milliseconds for auto deep sleep */
+#define DEEP_SLEEP_IDLE_TIME	100
+
+typedef struct _mlan_ds_auto_ds
+{
+    /** auto ds mode, 0 - disable, 1 - enable */
+    t_u16 auto_ds;
+    /** auto ds idle time in milliseconds */
+    t_u16 idletime;
+} mlan_ds_auto_ds;
+
+/** Type definition of mlan_ds_inactivity_to for MLAN_OID_PM_CFG_INACTIVITY_TO */
+typedef struct _mlan_ds_inactivity_to
+{
+    /** Timeout unit in microsecond, 0 means 1000us (1ms) */
+    t_u32 timeout_unit;
+    /** Inactivity timeout for unicast data */
+    t_u32 unicast_timeout;
+    /** Inactivity timeout for multicast data */
+    t_u32 mcast_timeout;
+    /** Timeout for additional Rx traffic after Null PM1 packet exchange */
+    t_u32 ps_entry_timeout;
+} mlan_ds_inactivity_to, *pmlan_ds_inactivity_to;
+
+/** Minimum sleep period in milliseconds */
+#define MIN_SLEEP_PERIOD    10
+/** Maximum sleep period in milliseconds */
+#define MAX_SLEEP_PERIOD    60
+/** Special setting for UPSD certification tests */
+#define SLEEP_PERIOD_RESERVED_FF    0xFF
+
+/** PS null interval disable */
+#define PS_NULL_DISABLE         (-1)
+
+/** Local listen interval disable */
+#define MRVDRV_LISTEN_INTERVAL_DISABLE   (-1)
+/** Minimum listen interval */
+#define MRVDRV_MIN_LISTEN_INTERVAL       0
+
+/** Minimum multiple DTIM */
+#define MRVDRV_MIN_MULTIPLE_DTIM                0
+/** Maximum multiple DTIM */
+#define MRVDRV_MAX_MULTIPLE_DTIM                5
+/** Ignore multiple DTIM */
+#define MRVDRV_IGNORE_MULTIPLE_DTIM             0xfffe
+/** Match listen interval to closest DTIM */
+#define MRVDRV_MATCH_CLOSEST_DTIM               0xfffd
+
+/** Minimum adhoc awake period */
+#define MIN_ADHOC_AWAKE_PD      0
+/** Maximum adhoc awake period */
+#define MAX_ADHOC_AWAKE_PD      31
+/** Special adhoc awake period */
+#define SPECIAL_ADHOC_AWAKE_PD  255
+
+/** Minimum beacon miss timeout in milliseconds */
+#define MIN_BCN_MISS_TO         0
+/** Maximum beacon miss timeout in milliseconds */
+#define MAX_BCN_MISS_TO         50
+/** Disable beacon miss timeout */
+#define DISABLE_BCN_MISS_TO     65535
+
+/** Minimum delay to PS in milliseconds */
+#define MIN_DELAY_TO_PS         0
+/** Maximum delay to PS in milliseconds */
+#define MAX_DELAY_TO_PS         65535
+/** Delay to PS unchanged */
+#define DELAY_TO_PS_UNCHANGED   (-1)
+/** Default delay to PS in milliseconds */
+#define DELAY_TO_PS_DEFAULT     1000
+
+/** PS mode: Unchanged */
+#define PS_MODE_UNCHANGED       0
+/** PS mode: Auto */
+#define PS_MODE_AUTO            1
+/** PS mode: Poll */
+#define PS_MODE_POLL            2
+/** PS mode: Null */
+#define PS_MODE_NULL            3
+
+/** Type definition of mlan_ds_ps_cfg for MLAN_OID_PM_CFG_PS_CFG */
+typedef struct _mlan_ds_ps_cfg
+{
+    /** PS null interval in seconds */
+    t_u32 ps_null_interval;
+    /** Multiple DTIM interval */
+    t_u32 multiple_dtim_interval;
+    /** Listen interval */
+    t_u32 listen_interval;
+    /** Adhoc awake period */
+    t_u32 adhoc_awake_period;
+    /** Beacon miss timeout in milliseconds */
+    t_u32 bcn_miss_timeout;
+    /** Delay to PS in milliseconds */
+    t_s32 delay_to_ps;
+    /** PS mode */
+    t_u32 ps_mode;
+} mlan_ds_ps_cfg, *pmlan_ds_ps_cfg;
+
+/** Type definition of mlan_ds_sleep_params for MLAN_OID_PM_CFG_SLEEP_PARAMS */
+typedef struct _mlan_ds_sleep_params
+{
+    /** Error */
+    t_u32 error;
+    /** Offset in microseconds */
+    t_u32 offset;
+    /** Stable time in microseconds */
+    t_u32 stable_time;
+    /** Calibration control */
+    t_u32 cal_control;
+    /** External sleep clock */
+    t_u32 ext_sleep_clk;
+    /** Reserved */
+    t_u32 reserved;
+} mlan_ds_sleep_params, *pmlan_ds_sleep_params;
+
+/** sleep_param */
+typedef struct _ps_sleep_param
+{
+    /** control bitmap */
+    t_u32 ctrl_bitmap;
+    /** minimum sleep period (micro second) */
+    t_u32 min_sleep;
+    /** maximum sleep period (micro second) */
+    t_u32 max_sleep;
+} ps_sleep_param;
+
+/** inactivity sleep_param */
+typedef struct _inact_sleep_param
+{
+    /** inactivity timeout (micro second) */
+    t_u32 inactivity_to;
+    /** miniumu awake period (micro second) */
+    t_u32 min_awake;
+    /** maximum awake period (micro second) */
+    t_u32 max_awake;
+} inact_sleep_param;
+
+/** flag for ps mode */
+#define PS_FLAG_PS_MODE                 1
+/** flag for sleep param */
+#define PS_FLAG_SLEEP_PARAM             2
+/** flag for inactivity sleep param */
+#define PS_FLAG_INACT_SLEEP_PARAM       4
+
+/** Disable power mode */
+#define PS_MODE_DISABLE                      0
+/** Enable periodic dtim ps */
+#define PS_MODE_PERIODIC_DTIM                1
+/** Enable inactivity ps */
+#define PS_MODE_INACTIVITY                   2
+
+/** mlan_ds_ps_mgmt */
+typedef struct _mlan_ds_ps_mgmt
+{
+    /** flags for valid field */
+    t_u16 flags;
+    /** power mode */
+    t_u16 ps_mode;
+    /** sleep param */
+    ps_sleep_param sleep_param;
+    /** inactivity sleep param */
+    inact_sleep_param inact_param;
+} mlan_ds_ps_mgmt;
+
+/** mlan_ds_ps_info */
+typedef struct _mlan_ds_ps_info
+{
+    /** suspend allowed flag */
+    t_u32 is_suspend_allowed;
+} mlan_ds_ps_info;
+
+/** Type definition of mlan_ds_pm_cfg for MLAN_IOCTL_PM_CFG */
+typedef struct _mlan_ds_pm_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Power management parameter */
+    union
+    {
+        /** Power saving mode for MLAN_OID_PM_CFG_IEEE_PS */
+        t_u32 ps_mode;
+        /** Host Sleep configuration for MLAN_OID_PM_CFG_HS_CFG */
+        mlan_ds_hs_cfg hs_cfg;
+        /** Deep sleep mode for MLAN_OID_PM_CFG_DEEP_SLEEP */
+        mlan_ds_auto_ds auto_deep_sleep;
+        /** Inactivity timeout for MLAN_OID_PM_CFG_INACTIVITY_TO */
+        mlan_ds_inactivity_to inactivity_to;
+        /** Sleep period for MLAN_OID_PM_CFG_SLEEP_PD */
+        t_u32 sleep_period;
+        /** PS configuration parameters for MLAN_OID_PM_CFG_PS_CFG */
+        mlan_ds_ps_cfg ps_cfg;
+        /** PS configuration parameters for MLAN_OID_PM_CFG_SLEEP_PARAMS */
+        mlan_ds_sleep_params sleep_params;
+        /** PS configuration parameters for MLAN_OID_PM_CFG_PS_MODE */
+        mlan_ds_ps_mgmt ps_mgmt;
+        /** power info for MLAN_OID_PM_INFO */
+        mlan_ds_ps_info ps_info;
+    } param;
+} mlan_ds_pm_cfg, *pmlan_ds_pm_cfg;
+
+/*-----------------------------------------------------------------*/
+/** WMM Configuration Group */
+/*-----------------------------------------------------------------*/
+
+/** WMM TSpec size */
+#define MLAN_WMM_TSPEC_SIZE             63
+/** WMM Add TS extra IE bytes */
+#define MLAN_WMM_ADDTS_EXTRA_IE_BYTES   256
+/** WMM statistics for packets hist bins */
+#define MLAN_WMM_STATS_PKTS_HIST_BINS   7
+/** Maximum number of AC QOS queues available */
+#define MLAN_WMM_MAX_AC_QUEUES          4
+
+/**
+ *  @brief IOCTL structure to send an ADDTS request and retrieve the response.
+ *
+ *  IOCTL structure from the application layer relayed to firmware to
+ *    instigate an ADDTS management frame with an appropriate TSPEC IE as well
+ *    as any additional IEs appended in the ADDTS Action frame.
+ *
+ *  @sa woal_wmm_addts_req_ioctl
+ */
+typedef struct
+{
+    mlan_cmd_result_e cmd_result;     /**< Firmware execution result */
+
+    t_u32 timeout_ms;                 /**< Timeout value in milliseconds */
+    t_u8 ieee_status_code;            /**< IEEE status code */
+
+    t_u32 ie_data_len;                /**< Length of ie block in ie_data */
+    t_u8 ie_data[MLAN_WMM_TSPEC_SIZE  /**< TSPEC to send in the ADDTS */
+                 + MLAN_WMM_ADDTS_EXTRA_IE_BYTES];  /**< Extra IE buf*/
+} wlan_ioctl_wmm_addts_req_t;
+
+/**
+ *  @brief IOCTL structure to send a DELTS request.
+ *
+ *  IOCTL structure from the application layer relayed to firmware to
+ *    instigate an DELTS management frame with an appropriate TSPEC IE.
+ *
+ *  @sa woal_wmm_delts_req_ioctl
+ */
+typedef struct
+{
+    mlan_cmd_result_e cmd_result; /**< Firmware execution result */
+    t_u8 ieee_reason_code;        /**< IEEE reason code sent, unused for WMM */
+    t_u32 ie_data_len;            /**< Length of ie block in ie_data */
+    t_u8 ie_data[MLAN_WMM_TSPEC_SIZE]; /**< TSPEC to send in the DELTS */
+} wlan_ioctl_wmm_delts_req_t;
+
+/**
+ *  @brief IOCTL structure to configure a specific AC Queue's parameters
+ *
+ *  IOCTL structure from the application layer relayed to firmware to
+ *    get, set, or default the WMM AC queue parameters.
+ *
+ *  - msdu_lifetime_expiry is ignored if set to 0 on a set command
+ *
+ *  @sa woal_wmm_queue_config_ioctl
+ */
+typedef struct
+{
+    mlan_wmm_queue_config_action_e action;    /**< Set, Get, or Default */
+    mlan_wmm_ac_e access_category;            /**< WMM_AC_BK(0) to WMM_AC_VO(3) */
+    t_u16 msdu_lifetime_expiry;               /**< lifetime expiry in TUs */
+    t_u8 supported_rates[10];                 /**< Not supported yet */
+} wlan_ioctl_wmm_queue_config_t;
+
+/**
+ *  @brief IOCTL structure to start, stop, and get statistics for a WMM AC
+ *
+ *  IOCTL structure from the application layer relayed to firmware to
+ *    start or stop statistical collection for a given AC.  Also used to
+ *    retrieve and clear the collected stats on a given AC.
+ *
+ *  @sa woal_wmm_queue_stats_ioctl
+ */
+typedef struct
+{
+    /** Action of Queue Config : Start, Stop, or Get */
+    mlan_wmm_queue_stats_action_e action;
+    /** User Priority */
+    t_u8 user_priority;
+    /** Number of successful packets transmitted */
+    t_u16 pkt_count;
+    /** Packets lost; not included in pkt_count */
+    t_u16 pkt_loss;
+    /** Average Queue delay in microseconds */
+    t_u32 avg_queue_delay;
+    /** Average Transmission delay in microseconds */
+    t_u32 avg_tx_delay;
+    /** Calculated used time in units of 32 microseconds */
+    t_u16 used_time;
+    /** Calculated policed time in units of 32 microseconds */
+    t_u16 policed_time;
+    /** Queue Delay Histogram; number of packets per queue delay range
+     *
+     *  [0] -  0ms <= delay < 5ms
+     *  [1] -  5ms <= delay < 10ms
+     *  [2] - 10ms <= delay < 20ms
+     *  [3] - 20ms <= delay < 30ms
+     *  [4] - 30ms <= delay < 40ms
+     *  [5] - 40ms <= delay < 50ms
+     *  [6] - 50ms <= delay < msduLifetime (TUs)
+     */
+    t_u16 delay_histogram[MLAN_WMM_STATS_PKTS_HIST_BINS];
+} wlan_ioctl_wmm_queue_stats_t,
+/** Type definition of mlan_ds_wmm_queue_stats for MLAN_OID_WMM_CFG_QUEUE_STATS */
+  mlan_ds_wmm_queue_stats, *pmlan_ds_wmm_queue_stats;
+
+/**
+ *  @brief IOCTL sub structure for a specific WMM AC Status
+ */
+typedef struct
+{
+    /** WMM Acm */
+    t_u8 wmm_acm;
+    /** Flow required flag */
+    t_u8 flow_required;
+    /** Flow created flag */
+    t_u8 flow_created;
+    /** Disabled flag */
+    t_u8 disabled;
+} wlan_ioctl_wmm_queue_status_ac_t;
+
+/**
+ *  @brief IOCTL structure to retrieve the WMM AC Queue status
+ *
+ *  IOCTL structure from the application layer to retrieve:
+ *     - ACM bit setting for the AC
+ *     - Firmware status (flow required, flow created, flow disabled)
+ *
+ *  @sa woal_wmm_queue_status_ioctl
+ */
+typedef struct
+{
+    /** WMM AC queue status */
+    wlan_ioctl_wmm_queue_status_ac_t ac_status[MLAN_WMM_MAX_AC_QUEUES];
+} wlan_ioctl_wmm_queue_status_t,
+/** Type definition of mlan_ds_wmm_queue_status for MLAN_OID_WMM_CFG_QUEUE_STATUS */
+  mlan_ds_wmm_queue_status, *pmlan_ds_wmm_queue_status;
+
+/** Type definition of mlan_ds_wmm_addts for MLAN_OID_WMM_CFG_ADDTS */
+typedef struct _mlan_ds_wmm_addts
+{
+    /** Result of ADDTS request */
+    mlan_cmd_result_e result;
+    /** Timeout value in milliseconds */
+    t_u32 timeout;
+    /** IEEE status code */
+    t_u32 status_code;
+    /** Dialog token */
+    t_u8 dialog_tok;
+    /** TSPEC data length */
+    t_u8 ie_data_len;
+    /** TSPEC to send in the ADDTS + buffering for any extra IEs */
+    t_u8 ie_data[MLAN_WMM_TSPEC_SIZE + MLAN_WMM_ADDTS_EXTRA_IE_BYTES];
+} mlan_ds_wmm_addts, *pmlan_ds_wmm_addts;
+
+/** Type definition of mlan_ds_wmm_delts for MLAN_OID_WMM_CFG_DELTS */
+typedef struct _mlan_ds_wmm_delts
+{
+    /** Result of DELTS request */
+    mlan_cmd_result_e result;
+    /** IEEE status code */
+    t_u32 status_code;
+    /** TSPEC data length */
+    t_u8 ie_data_len;
+    /** TSPEC to send in the DELTS */
+    t_u8 ie_data[MLAN_WMM_TSPEC_SIZE];
+} mlan_ds_wmm_delts, *pmlan_ds_wmm_delts;
+
+/** Type definition of mlan_ds_wmm_queue_config for MLAN_OID_WMM_CFG_QUEUE_CONFIG */
+typedef struct _mlan_ds_wmm_queue_config
+{
+    /** Action of Queue Config : Set, Get, or Default */
+    mlan_wmm_queue_config_action_e action;
+    /** WMM Access Category: WMM_AC_BK(0) to WMM_AC_VO(3) */
+    mlan_wmm_ac_e access_category;
+    /** Lifetime expiry in TUs */
+    t_u16 msdu_lifetime_expiry;
+    /** Reserve for future use */
+    t_u8 reserved[10];
+} mlan_ds_wmm_queue_config, *pmlan_ds_wmm_queue_config;
+
+/** Type definition of mlan_ds_wmm_cfg for MLAN_IOCTL_WMM_CFG */
+typedef struct _mlan_ds_wmm_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** WMM configuration parameter */
+    union
+    {
+        /** WMM enable for MLAN_OID_WMM_CFG_ENABLE */
+        t_u32 wmm_enable;
+        /** QoS configuration for MLAN_OID_WMM_CFG_QOS */
+        t_u8 qos_cfg;
+        /** WMM add TS for MLAN_OID_WMM_CFG_ADDTS */
+        mlan_ds_wmm_addts addts;
+        /** WMM delete TS for MLAN_OID_WMM_CFG_DELTS */
+        mlan_ds_wmm_delts delts;
+        /** WMM queue configuration for MLAN_OID_WMM_CFG_QUEUE_CONFIG */
+        mlan_ds_wmm_queue_config q_cfg;
+        /** WMM queue status for MLAN_OID_WMM_CFG_QUEUE_STATS */
+        mlan_ds_wmm_queue_stats q_stats;
+        /** WMM queue status for MLAN_OID_WMM_CFG_QUEUE_STATUS */
+        mlan_ds_wmm_queue_status q_status;
+        /** WMM TS status for MLAN_OID_WMM_CFG_TS_STATUS */
+        mlan_ds_wmm_ts_status ts_status;
+    } param;
+} mlan_ds_wmm_cfg, *pmlan_ds_wmm_cfg;
+
+/*-----------------------------------------------------------------*/
+/** WPS Configuration Group */
+/*-----------------------------------------------------------------*/
+/** Enumeration for WPS session */
+enum _mlan_wps_status
+{
+    MLAN_WPS_CFG_SESSION_START = 1,
+    MLAN_WPS_CFG_SESSION_END = 0
+};
+
+/** Type definition of mlan_ds_wps_cfg for MLAN_IOCTL_WPS_CFG */
+typedef struct _mlan_ds_wps_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** WPS configuration parameter */
+    union
+    {
+        /** WPS session for MLAN_OID_WPS_CFG_SESSION */
+        t_u32 wps_session;
+    } param;
+} mlan_ds_wps_cfg, *pmlan_ds_wps_cfg;
+
+/*-----------------------------------------------------------------*/
+/** 802.11n Configuration Group */
+/*-----------------------------------------------------------------*/
+/** Maximum MCS */
+#define NUM_MCS_FIELD      16
+
+/** Supported stream modes */
+#define HT_STREAM_MODE_1X1   0x11
+#define HT_STREAM_MODE_2X2   0x22
+
+/* Both 2.4G and 5G band selected */
+#define BAND_SELECT_BOTH    0
+/* Band 2.4G selected */
+#define BAND_SELECT_BG      1
+/* Band 5G selected */
+#define BAND_SELECT_A       2
+
+/** Type definition of mlan_ds_11n_htcap_cfg for MLAN_OID_11N_HTCAP_CFG */
+typedef struct _mlan_ds_11n_htcap_cfg
+{
+    /** HT Capability information */
+    t_u32 htcap;
+    /** Band selection */
+    t_u32 misc_cfg;
+    /** Hardware HT cap information required */
+    t_u32 hw_cap_req;
+} mlan_ds_11n_htcap_cfg, *pmlan_ds_11n_htcap_cfg;
+
+/** Type definition of mlan_ds_11n_addba_param for MLAN_OID_11N_CFG_ADDBA_PARAM */
+typedef struct _mlan_ds_11n_addba_param
+{
+    /** Timeout */
+    t_u32 timeout;
+    /** Buffer size for ADDBA request */
+    t_u32 txwinsize;
+    /** Buffer size for ADDBA response */
+    t_u32 rxwinsize;
+    /** amsdu for ADDBA request */
+    t_u8 txamsdu;
+    /** amsdu for ADDBA response */
+    t_u8 rxamsdu;
+} mlan_ds_11n_addba_param, *pmlan_ds_11n_addba_param;
+
+/** Type definition of mlan_ds_11n_tx_cfg for MLAN_OID_11N_CFG_TX */
+typedef struct _mlan_ds_11n_tx_cfg
+{
+    /** HTTxCap */
+    t_u16 httxcap;
+    /** HTTxInfo */
+    t_u16 httxinfo;
+    /** Band selection */
+    t_u32 misc_cfg;
+} mlan_ds_11n_tx_cfg, *pmlan_ds_11n_tx_cfg;
+
+/** BF Global Configuration */
+#define BF_GLOBAL_CONFIGURATION     0x00
+/** Performs NDP sounding for PEER specified */
+#define TRIGGER_SOUNDING_FOR_PEER   0x01
+/** TX BF interval for channel sounding */
+#define SET_GET_BF_PERIODICITY      0x02
+/** Tell FW not to perform any sounding for peer */
+#define TX_BF_FOR_PEER_ENBL         0x03
+/** TX BF SNR threshold for peer */
+#define SET_SNR_THR_PEER            0x04
+
+/* Maximum number of peer MAC and status/SNR tuples */
+#define MAX_PEER_MAC_TUPLES         10
+
+/** Any new subcommand structure should be declare here */
+
+/** bf global cfg args */
+typedef struct _mlan_bf_global_cfg_args
+{
+    /** Global enable/disable bf */
+    t_u8 bf_enbl;
+    /** Global enable/disable sounding */
+    t_u8 sounding_enbl;
+    /** FB Type */
+    t_u8 fb_type;
+    /** SNR Threshold */
+    t_u8 snr_threshold;
+    /** Sounding interval in milliseconds */
+    t_u16 sounding_interval;
+    /** BF mode */
+    t_u8 bf_mode;
+    /** Reserved */
+    t_u8 reserved;
+} mlan_bf_global_cfg_args;
+
+/** trigger sounding args */
+typedef struct _mlan_trigger_sound_args
+{
+    /** Peer MAC address */
+    t_u8 peer_mac[MLAN_MAC_ADDR_LENGTH];
+    /** Status */
+    t_u8 status;
+} mlan_trigger_sound_args;
+
+/** bf periodicity args */
+typedef struct _mlan_bf_periodicity_args
+{
+    /** Peer MAC address */
+    t_u8 peer_mac[MLAN_MAC_ADDR_LENGTH];
+    /** Current Tx BF Interval in milliseconds */
+    t_u16 interval;
+    /** Status */
+    t_u8 status;
+} mlan_bf_periodicity_args;
+
+/** tx bf peer args */
+typedef struct _mlan_tx_bf_peer_args
+{
+    /** Peer MAC address */
+    t_u8 peer_mac[MLAN_MAC_ADDR_LENGTH];
+    /** Reserved */
+    t_u16 reserved;
+    /** Enable/Disable Beamforming */
+    t_u8 bf_enbl;
+    /** Enable/Disable sounding */
+    t_u8 sounding_enbl;
+    /** FB Type */
+    t_u8 fb_type;
+} mlan_tx_bf_peer_args;
+
+/** SNR threshold args */
+typedef struct _mlan_snr_thr_args
+{
+    /** Peer MAC address */
+    t_u8 peer_mac[MLAN_MAC_ADDR_LENGTH];
+    /** SNR for peer */
+    t_u8 snr;
+} mlan_snr_thr_args;
+
+/** Type definition of mlan_ds_11n_tx_bf_cfg for MLAN_OID_11N_CFG_TX_BF_CFG */
+typedef struct _mlan_ds_11n_tx_bf_cfg
+{
+    /** BF Action */
+    t_u16 bf_action;
+    /** Action */
+    t_u16 action;
+    /** Number of peers */
+    t_u32 no_of_peers;
+    union
+    {
+        mlan_bf_global_cfg_args bf_global_cfg;
+        mlan_trigger_sound_args bf_sound[MAX_PEER_MAC_TUPLES];
+        mlan_bf_periodicity_args bf_periodicity[MAX_PEER_MAC_TUPLES];
+        mlan_tx_bf_peer_args tx_bf_peer[MAX_PEER_MAC_TUPLES];
+        mlan_snr_thr_args bf_snr[MAX_PEER_MAC_TUPLES];
+    } body;
+} mlan_ds_11n_tx_bf_cfg, *pmlan_ds_11n_tx_bf_cfg;
+
+/** Type definition of mlan_ds_11n_amsdu_aggr_ctrl for
+ * MLAN_OID_11N_AMSDU_AGGR_CTRL*/
+typedef struct _mlan_ds_11n_amsdu_aggr_ctrl
+{
+    /** Enable/Disable */
+    t_u16 enable;
+    /** Current AMSDU size valid */
+    t_u16 curr_buf_size;
+} mlan_ds_11n_amsdu_aggr_ctrl, *pmlan_ds_11n_amsdu_aggr_ctrl;
+
+/** Type definition of mlan_ds_11n_aggr_prio_tbl for MLAN_OID_11N_CFG_AGGR_PRIO_TBL */
+typedef struct _mlan_ds_11n_aggr_prio_tbl
+{
+    /** ampdu priority table */
+    t_u8 ampdu[MAX_NUM_TID];
+    /** amsdu priority table */
+    t_u8 amsdu[MAX_NUM_TID];
+} mlan_ds_11n_aggr_prio_tbl, *pmlan_ds_11n_aggr_prio_tbl;
+
+/** Type definition of mlan_ds_11n_cfg for MLAN_IOCTL_11N_CFG */
+typedef struct _mlan_ds_11n_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** 802.11n configuration parameter */
+    union
+    {
+        /** Tx param for 11n for MLAN_OID_11N_CFG_TX */
+        mlan_ds_11n_tx_cfg tx_cfg;
+        /** Aggr priority table for MLAN_OID_11N_CFG_AGGR_PRIO_TBL */
+        mlan_ds_11n_aggr_prio_tbl aggr_prio_tbl;
+        /** Add BA param for MLAN_OID_11N_CFG_ADDBA_PARAM */
+        mlan_ds_11n_addba_param addba_param;
+        /** Add BA Reject paramters for MLAN_OID_11N_CFG_ADDBA_REJECT */
+        t_u8 addba_reject[MAX_NUM_TID];
+        /** Tx buf size for MLAN_OID_11N_CFG_MAX_TX_BUF_SIZE */
+        t_u32 tx_buf_size;
+        /** HT cap info configuration for MLAN_OID_11N_HTCAP_CFG */
+        mlan_ds_11n_htcap_cfg htcap_cfg;
+        /** Tx param for 11n for MLAN_OID_11N_AMSDU_AGGR_CTRL */
+        mlan_ds_11n_amsdu_aggr_ctrl amsdu_aggr_ctrl;
+        /** Supported MCS Set field */
+        t_u8 supported_mcs_set[NUM_MCS_FIELD];
+        /** Transmit Beamforming Capabilities field */
+        t_u32 tx_bf_cap;
+        /** Transmit Beamforming configuration */
+        mlan_ds_11n_tx_bf_cfg tx_bf;
+        /** HT stream configuration */
+        t_u32 stream_cfg;
+    } param;
+} mlan_ds_11n_cfg, *pmlan_ds_11n_cfg;
+
+/** Country code length */
+#define COUNTRY_CODE_LEN                        3
+
+/*-----------------------------------------------------------------*/
+/** 802.11d Configuration Group */
+/*-----------------------------------------------------------------*/
+/** Maximum subbands for 11d */
+#define MRVDRV_MAX_SUBBAND_802_11D              83
+
+#ifdef STA_SUPPORT
+/** Data structure for subband set */
+typedef struct _mlan_ds_subband_set_t
+{
+    /** First channel */
+    t_u8 first_chan;
+    /** Number of channels */
+    t_u8 no_of_chan;
+    /** Maximum Tx power in dBm */
+    t_u8 max_tx_pwr;
+} mlan_ds_subband_set_t;
+
+/** Domain regulatory information */
+typedef struct _mlan_ds_11d_domain_info
+{
+    /** Country Code */
+    t_u8 country_code[COUNTRY_CODE_LEN];
+    /** Band that channels in sub_band belong to */
+    t_u8 band;
+    /** No. of subband in below */
+    t_u8 no_of_sub_band;
+    /** Subband data to send/last sent */
+    mlan_ds_subband_set_t sub_band[MRVDRV_MAX_SUBBAND_802_11D];
+} mlan_ds_11d_domain_info;
+#endif
+
+/** Type definition of mlan_ds_11d_cfg for MLAN_IOCTL_11D_CFG */
+typedef struct _mlan_ds_11d_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** 802.11d configuration parameter */
+    union
+    {
+#ifdef STA_SUPPORT
+        /** Enable for MLAN_OID_11D_CFG_ENABLE */
+        t_u32 enable_11d;
+        /** Domain info for MLAN_OID_11D_DOMAIN_INFO */
+        mlan_ds_11d_domain_info domain_info;
+#endif                          /* STA_SUPPORT */
+#ifdef UAP_SUPPORT
+        /** tlv data for MLAN_OID_11D_DOMAIN_INFO */
+        t_u8 domain_tlv[MAX_IE_SIZE];
+#endif                          /* UAP_SUPPORT */
+    } param;
+} mlan_ds_11d_cfg, *pmlan_ds_11d_cfg;
+
+/*-----------------------------------------------------------------*/
+/** Register Memory Access Group */
+/*-----------------------------------------------------------------*/
+/** Enumeration for register type */
+enum _mlan_reg_type
+{
+    MLAN_REG_MAC = 1,
+    MLAN_REG_BBP,
+    MLAN_REG_RF,
+    MLAN_REG_CAU = 5,
+};
+
+/** Type definition of mlan_ds_reg_rw for MLAN_OID_REG_RW */
+typedef struct _mlan_ds_reg_rw
+{
+    /** Register type */
+    t_u32 type;
+    /** Offset */
+    t_u32 offset;
+    /** Value */
+    t_u32 value;
+} mlan_ds_reg_rw;
+
+/** Maximum EEPROM data */
+#define MAX_EEPROM_DATA 256
+
+/** Type definition of mlan_ds_read_eeprom for MLAN_OID_EEPROM_RD */
+typedef struct _mlan_ds_read_eeprom
+{
+    /** Multiples of 4 */
+    t_u16 offset;
+    /** Number of bytes */
+    t_u16 byte_count;
+    /** Value */
+    t_u8 value[MAX_EEPROM_DATA];
+} mlan_ds_read_eeprom;
+
+/** Type definition of mlan_ds_mem_rw for MLAN_OID_MEM_RW */
+typedef struct _mlan_ds_mem_rw
+{
+    /** Address */
+    t_u32 addr;
+    /** Value */
+    t_u32 value;
+} mlan_ds_mem_rw;
+
+/** Type definition of mlan_ds_reg_mem for MLAN_IOCTL_REG_MEM */
+typedef struct _mlan_ds_reg_mem
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Register memory access parameter */
+    union
+    {
+        /** Register access for MLAN_OID_REG_RW */
+        mlan_ds_reg_rw reg_rw;
+        /** EEPROM access for MLAN_OID_EEPROM_RD */
+        mlan_ds_read_eeprom rd_eeprom;
+        /** Memory access for MLAN_OID_MEM_RW */
+        mlan_ds_mem_rw mem_rw;
+    } param;
+} mlan_ds_reg_mem, *pmlan_ds_reg_mem;
+
+/*-----------------------------------------------------------------*/
+/** Multi-Radio Configuration Group */
+/*-----------------------------------------------------------------*/
+
+/*-----------------------------------------------------------------*/
+/** 802.11h Configuration Group */
+/*-----------------------------------------------------------------*/
+#if defined(DFS_TESTING_SUPPORT)
+/** Type definition of mlan_ds_11h_dfs_testing for MLAN_OID_11H_DFS_TESTING */
+typedef struct _mlan_ds_11h_dfs_testing
+{
+    /** User-configured CAC period in milliseconds, 0 to use default */
+    t_u16 usr_cac_period_msec;
+    /** User-configured NOP period in seconds, 0 to use default */
+    t_u16 usr_nop_period_sec;
+    /** User-configured skip channel change, 0 to disable */
+    t_u8 usr_no_chan_change;
+    /** User-configured fixed channel to change to, 0 to use random channel */
+    t_u8 usr_fixed_new_chan;
+} mlan_ds_11h_dfs_testing, *pmlan_ds_11h_dfs_testing;
+#endif
+
+/** Type definition of mlan_ds_11h_cfg for MLAN_IOCTL_11H_CFG */
+typedef struct _mlan_ds_11h_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    union
+    {
+        /** Local power constraint for MLAN_OID_11H_LOCAL_POWER_CONSTRAINT */
+        t_s8 usr_local_power_constraint;
+#if defined(DFS_TESTING_SUPPORT)
+        /** User-configuation for MLAN_OID_11H_DFS_TESTING */
+        mlan_ds_11h_dfs_testing dfs_testing;
+#endif
+    } param;
+} mlan_ds_11h_cfg, *pmlan_ds_11h_cfg;
+
+/*-----------------------------------------------------------------*/
+/** Miscellaneous Configuration Group */
+/*-----------------------------------------------------------------*/
+
+/** CMD buffer size */
+#define MLAN_SIZE_OF_CMD_BUFFER 2048
+
+/** LDO Internal */
+#define LDO_INTERNAL            0
+/** LDO External */
+#define LDO_EXTERNAL            1
+
+/** Enumeration for IE type */
+enum _mlan_ie_type
+{
+    MLAN_IE_TYPE_GEN_IE = 0,
+#ifdef STA_SUPPORT
+    MLAN_IE_TYPE_ARP_FILTER,
+#endif /* STA_SUPPORT */
+};
+
+/** Type definition of mlan_ds_misc_gen_ie for MLAN_OID_MISC_GEN_IE */
+typedef struct _mlan_ds_misc_gen_ie
+{
+    /** IE type */
+    t_u32 type;
+    /** IE length */
+    t_u32 len;
+    /** IE buffer */
+    t_u8 ie_data[MAX_IE_SIZE];
+} mlan_ds_misc_gen_ie;
+
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+/** Type definition of mlan_ds_misc_sdio_mpa_ctrl for MLAN_OID_MISC_SDIO_MPA_CTRL */
+typedef struct _mlan_ds_misc_sdio_mpa_ctrl
+{
+    /** SDIO MP-A TX enable/disable */
+    t_u16 tx_enable;
+    /** SDIO MP-A RX enable/disable */
+    t_u16 rx_enable;
+    /** SDIO MP-A TX buf size */
+    t_u16 tx_buf_size;
+    /** SDIO MP-A RX buf size */
+    t_u16 rx_buf_size;
+    /** SDIO MP-A TX Max Ports */
+    t_u16 tx_max_ports;
+    /** SDIO MP-A RX Max Ports */
+    t_u16 rx_max_ports;
+} mlan_ds_misc_sdio_mpa_ctrl;
+#endif
+
+/** Type definition of mlan_ds_misc_cmd for MLAN_OID_MISC_HOST_CMD */
+typedef struct _mlan_ds_misc_cmd
+{
+    /** Command length */
+    t_u32 len;
+    /** Command buffer */
+    t_u8 cmd[MLAN_SIZE_OF_CMD_BUFFER];
+} mlan_ds_misc_cmd;
+
+/** Maximum number of system clocks */
+#define MLAN_MAX_CLK_NUM     	16
+
+/** Clock type : Configurable */
+#define MLAN_CLK_CONFIGURABLE	0
+/** Clock type : Supported */
+#define MLAN_CLK_SUPPORTED   	1
+
+/** Type definition of mlan_ds_misc_sys_clock for MLAN_OID_MISC_SYS_CLOCK */
+typedef struct _mlan_ds_misc_sys_clock
+{
+    /** Current system clock */
+    t_u16 cur_sys_clk;
+    /** Clock type */
+    t_u16 sys_clk_type;
+    /** Number of clocks */
+    t_u16 sys_clk_num;
+    /** System clocks */
+    t_u16 sys_clk[MLAN_MAX_CLK_NUM];
+} mlan_ds_misc_sys_clock;
+
+/** Enumeration for function init/shutdown */
+enum _mlan_func_cmd
+{
+    MLAN_FUNC_INIT = 1,
+    MLAN_FUNC_SHUTDOWN,
+};
+
+/** Type definition of mlan_ds_misc_tx_datapause for MLAN_OID_MISC_TX_DATAPAUSE */
+typedef struct _mlan_ds_misc_tx_datapause
+{
+    /** Tx data pause flag */
+    t_u16 tx_pause;
+    /** Max number of Tx buffers for all PS clients */
+    t_u16 tx_buf_cnt;
+} mlan_ds_misc_tx_datapause;
+
+/** IP address length */
+#define IPADDR_LEN                  (16)
+/** Max number of ip */
+#define MAX_IPADDR                  (4)
+/** IP address type - IPv4*/
+#define IPADDR_TYPE_IPV4            (1)
+/** IP operation remove */
+#define MLAN_IPADDR_OP_IP_REMOVE    (0)
+/** IP operation ARP filter */
+#define MLAN_IPADDR_OP_ARP_FILTER   MBIT(0)
+/** IP operation ARP response */
+#define MLAN_IPADDR_OP_AUTO_ARP_RESP    MBIT(1)
+
+/** Type definition of mlan_ds_misc_ipaddr_cfg for MLAN_OID_MISC_IP_ADDR */
+typedef struct _mlan_ds_misc_ipaddr_cfg
+{
+    /** Operation code */
+    t_u32 op_code;
+    /** IP address type */
+    t_u32 ip_addr_type;
+    /** Number of IP */
+    t_u32 ip_addr_num;
+    /** IP address */
+    t_u8 ip_addr[MAX_IPADDR][IPADDR_LEN];
+} mlan_ds_misc_ipaddr_cfg;
+
+/* MEF configuration disable */
+#define MEF_CFG_DISABLE             0
+/* MEF configuration Rx filter enable */
+#define MEF_CFG_RX_FILTER_ENABLE    1
+/* MEF configuration auto ARP response */
+#define MEF_CFG_AUTO_ARP_RESP       2
+/* MEF configuration host command */
+#define MEF_CFG_HOSTCMD             0xFFFF
+
+/** Type definition of mlan_ds_misc_mef_cfg for MLAN_OID_MISC_MEF_CFG */
+typedef struct _mlan_ds_misc_mef_cfg
+{
+    /** Sub-ID for operation */
+    t_u32 sub_id;
+    /** Parameter according to sub-ID */
+    union
+    {
+        /** MEF command buffer for MEF_CFG_HOSTCMD */
+        mlan_ds_misc_cmd cmd_buf;
+    } param;
+} mlan_ds_misc_mef_cfg;
+
+/** Type definition of mlan_ds_misc_cfp_code for MLAN_OID_MISC_CFP_CODE */
+typedef struct _mlan_ds_misc_cfp_code
+{
+    /** CFP table code for 2.4GHz */
+    t_u32 cfp_code_bg;
+    /** CFP table code for 5GHz */
+    t_u32 cfp_code_a;
+} mlan_ds_misc_cfp_code;
+
+/** Type definition of mlan_ds_misc_country_code for MLAN_OID_MISC_COUNTRY_CODE */
+typedef struct _mlan_ds_misc_country_code
+{
+    /** Country Code */
+    t_u8 country_code[COUNTRY_CODE_LEN];
+} mlan_ds_misc_country_code;
+
+/** BITMAP for subscribe event rssi low */
+#define SUBSCRIBE_EVT_RSSI_LOW  		MBIT(0)
+/** BITMAP for subscribe event snr low */
+#define SUBSCRIBE_EVT_SNR_LOW			MBIT(1)
+/** BITMAP for subscribe event max fail */
+#define SUBSCRIBE_EVT_MAX_FAIL			MBIT(2)
+/** BITMAP for subscribe event beacon missed */
+#define SUBSCRIBE_EVT_BEACON_MISSED 	MBIT(3)
+/** BITMAP for subscribe event rssi high */
+#define SUBSCRIBE_EVT_RSSI_HIGH			MBIT(4)
+/** BITMAP for subscribe event snr high */
+#define SUBSCRIBE_EVT_SNR_HIGH			MBIT(5)
+/** BITMAP for subscribe event data rssi low */
+#define SUBSCRIBE_EVT_DATA_RSSI_LOW		MBIT(6)
+/** BITMAP for subscribe event data snr low */
+#define SUBSCRIBE_EVT_DATA_SNR_LOW		MBIT(7)
+/** BITMAP for subscribe event data rssi high */
+#define SUBSCRIBE_EVT_DATA_RSSI_HIGH	MBIT(8)
+/** BITMAP for subscribe event data snr high */
+#define SUBSCRIBE_EVT_DATA_SNR_HIGH		MBIT(9)
+/** BITMAP for subscribe event link quality */
+#define SUBSCRIBE_EVT_LINK_QUALITY		MBIT(10)
+/** BITMAP for subscribe event pre_beacon_lost */
+#define SUBSCRIBE_EVT_PRE_BEACON_LOST	MBIT(11)
+/** default PRE_BEACON_MISS_COUNT */
+#define DEFAULT_PRE_BEACON_MISS			30
+
+/** Type definition of mlan_ds_subscribe_evt for MLAN_OID_MISC_CFP_CODE */
+typedef struct _mlan_ds_subscribe_evt
+{
+    /** bitmap for subscribe event */
+    t_u16 evt_bitmap;
+    /** Absolute value of RSSI threshold value (dBm) */
+    t_u8 low_rssi;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 low_rssi_freq;
+    /** SNR threshold value (dB) */
+    t_u8 low_snr;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 low_snr_freq;
+    /** Failure count threshold */
+    t_u8 failure_count;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 failure_count_freq;
+    /** num of missed beacons */
+    t_u8 beacon_miss;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 beacon_miss_freq;
+    /** Absolute value of RSSI threshold value (dBm) */
+    t_u8 high_rssi;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 high_rssi_freq;
+    /** SNR threshold value (dB) */
+    t_u8 high_snr;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 high_snr_freq;
+    /** Absolute value of data RSSI threshold value (dBm) */
+    t_u8 data_low_rssi;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 data_low_rssi_freq;
+    /** Absolute value of data SNR threshold value (dBm) */
+    t_u8 data_low_snr;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 data_low_snr_freq;
+    /** Absolute value of data RSSI threshold value (dBm) */
+    t_u8 data_high_rssi;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 data_high_rssi_freq;
+    /** Absolute value of data SNR threshold value (dBm) */
+    t_u8 data_high_snr;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 data_high_snr_freq;
+    /* Link SNR threshold (dB) */
+    t_u16 link_snr;
+    /* Link SNR frequency */
+    t_u16 link_snr_freq;
+    /* Second minimum rate value as per the rate table below */
+    t_u16 link_rate;
+    /* Second minimum rate frequency */
+    t_u16 link_rate_freq;
+    /* Tx latency value (us) */
+    t_u16 link_tx_latency;
+    /* Tx latency frequency */
+    t_u16 link_tx_lantency_freq;
+    /* Number of pre missed beacons */
+    t_u8 pre_beacon_miss;
+} mlan_ds_subscribe_evt;
+
+/** Max OTP user data length */
+#define MAX_OTP_USER_DATA_LEN	252
+
+/** Type definition of mlan_ds_misc_otp_user_data for MLAN_OID_MISC_OTP_USER_DATA */
+typedef struct _mlan_ds_misc_otp_user_data
+{
+    /** Reserved */
+    t_u16 reserved;
+    /** OTP user data length */
+    t_u16 user_data_length;
+    /** User data buffer */
+    t_u8 user_data[MAX_OTP_USER_DATA_LEN];
+} mlan_ds_misc_otp_user_data;
+
+/** Type definition of mlan_ds_misc_cfg for MLAN_IOCTL_MISC_CFG */
+typedef struct _mlan_ds_misc_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Miscellaneous configuration parameter */
+    union
+    {
+        /** Generic IE for MLAN_OID_MISC_GEN_IE */
+        mlan_ds_misc_gen_ie gen_ie;
+        /** Region code for MLAN_OID_MISC_REGION */
+        t_u32 region_code;
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+        /** SDIO MP-A Ctrl command for MLAN_OID_MISC_SDIO_MPA_CTRL */
+        mlan_ds_misc_sdio_mpa_ctrl mpa_ctrl;
+#endif
+        /** Hostcmd for MLAN_OID_MISC_HOST_CMD */
+        mlan_ds_misc_cmd hostcmd;
+        /** System clock for MLAN_OID_MISC_SYS_CLOCK */
+        mlan_ds_misc_sys_clock sys_clock;
+        /** WWS set/get for MLAN_OID_MISC_WWS */
+        t_u32 wws_cfg;
+        /** Function init/shutdown for MLAN_OID_MISC_INIT_SHUTDOWN */
+        t_u32 func_init_shutdown;
+        /** Custom IE for MLAN_OID_MISC_CUSTOM_IE */
+        mlan_ds_misc_custom_ie cust_ie;
+        /** Tx data pause for MLAN_OID_MISC_TX_DATAPAUSE */
+        mlan_ds_misc_tx_datapause tx_datapause;
+        /** IP address configuration */
+        mlan_ds_misc_ipaddr_cfg ipaddr_cfg;
+        /** MAC control for MLAN_OID_MISC_MAC_CONTROL */
+        t_u32 mac_ctrl;
+        /** MEF configuration for MLAN_OID_MISC_MEF_CFG */
+        mlan_ds_misc_mef_cfg mef_cfg;
+        /** CFP code for MLAN_OID_MISC_CFP_CODE */
+        mlan_ds_misc_cfp_code cfp_code;
+        /** Country code for MLAN_OID_MISC_COUNTRY_CODE */
+        mlan_ds_misc_country_code country_code;
+        /** Thermal reading for MLAN_OID_MISC_THERMAL */
+        t_u32 thermal;
+        /** Mgmt subtype mask for MLAN_OID_MISC_RX_MGMT_IND */
+        t_u32 mgmt_subtype_mask;
+        /** subscribe event for MLAN_OID_MISC_SUBSCRIBE_EVENT */
+        mlan_ds_subscribe_evt subscribe_event;
+#ifdef DEBUG_LEVEL1
+        /** Driver debug bit masks */
+        t_u32 drvdbg;
+#endif
+        mlan_ds_misc_otp_user_data otp_user_data;
+    } param;
+} mlan_ds_misc_cfg, *pmlan_ds_misc_cfg;
+
+#endif /* !_MLAN_IOCTL_H_ */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_join.c b/drivers/net/wireless/sd8797/mlan/mlan_join.c
new file mode 100644
index 000000000000..1399ae8b3c16
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_join.c
@@ -0,0 +1,1853 @@
+/** @file mlan_join.c
+ *
+ *  @brief Functions implementing wlan infrastructure and adhoc join routines
+ *
+ *  IOCTL handlers as well as command preparation and response routines
+ *  for sending adhoc start, adhoc join, and association commands
+ *  to the firmware.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    10/30/2008: initial version
+******************************************************/
+
+#include "mlan.h"
+#include "mlan_join.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_wmm.h"
+#include "mlan_11n.h"
+#include "mlan_11h.h"
+
+/********************************************************
+                Local Constants
+********************************************************/
+
+/********************************************************
+                Local Variables
+********************************************************/
+
+/********************************************************
+                Global Variables
+********************************************************/
+
+/********************************************************
+                Local Functions
+********************************************************/
+/**
+ *  @brief Append a generic IE as a pass through TLV to a TLV buffer.
+ *
+ *  This function is called from the network join command prep. routine.
+ *    If the IE buffer has been setup by the application, this routine appends
+ *    the buffer as a pass through TLV type to the request.
+ *
+ *  @param priv     A pointer to mlan_private structure
+ *  @param ppbuffer pointer to command buffer pointer
+ *
+ *  @return         bytes added to the buffer
+ */
+static int
+wlan_cmd_append_generic_ie(mlan_private * priv, t_u8 ** ppbuffer)
+{
+    int ret_len = 0;
+    MrvlIEtypesHeader_t ie_header;
+
+    ENTER();
+
+    /* Null Checks */
+    if (ppbuffer == MNULL) {
+        LEAVE();
+        return 0;
+    }
+    if (*ppbuffer == MNULL) {
+        LEAVE();
+        return 0;
+    }
+
+    /*
+     * If there is a generic ie buffer setup, append it to the return
+     *   parameter buffer pointer.
+     */
+    if (priv->gen_ie_buf_len) {
+        PRINTM(MINFO, "append generic IE %d to %p\n", priv->gen_ie_buf_len,
+               *ppbuffer);
+
+        /* Wrap the generic IE buffer with a pass through TLV type */
+        ie_header.type = wlan_cpu_to_le16(TLV_TYPE_PASSTHROUGH);
+        ie_header.len = wlan_cpu_to_le16(priv->gen_ie_buf_len);
+        memcpy(priv->adapter, *ppbuffer, &ie_header, sizeof(ie_header));
+
+        /* Increment the return size and the return buffer pointer param */
+        *ppbuffer += sizeof(ie_header);
+        ret_len += sizeof(ie_header);
+
+        /* Copy the generic IE buffer to the output buffer, advance pointer */
+        memcpy(priv->adapter, *ppbuffer, priv->gen_ie_buf,
+               priv->gen_ie_buf_len);
+
+        /* Increment the return size and the return buffer pointer param */
+        *ppbuffer += priv->gen_ie_buf_len;
+        ret_len += priv->gen_ie_buf_len;
+
+        /* Reset the generic IE buffer */
+        priv->gen_ie_buf_len = 0;
+    }
+
+    /* return the length appended to the buffer */
+    LEAVE();
+    return ret_len;
+}
+
+/**
+  *  @brief Append TSF tracking info from the scan table for the target AP
+  *
+  *  This function is called from the network join command prep. routine.
+  *     The TSF table TSF sent to the firmware contains two TSF values:
+  *        - the TSF of the target AP from its previous beacon/probe response
+  *        - the TSF timestamp of our local MAC at the time we observed the
+  *          beacon/probe response.
+  *
+  *     The firmware uses the timestamp values to set an initial TSF value
+  *        in the MAC for the new association after a reassociation attempt.
+  *
+  *    @param pmpriv     A pointer to mlan_private structure
+  *    @param ppbuffer   A pointer to command buffer pointer
+  *    @param pbss_desc  A pointer to the BSS Descriptor from the scan table of
+  *                      the AP we are trying to join
+  *
+  *    @return         bytes added to the buffer
+  */
+static int
+wlan_cmd_append_tsf_tlv(mlan_private * pmriv, t_u8 ** ppbuffer,
+                        BSSDescriptor_t * pbss_desc)
+{
+    MrvlIEtypes_TsfTimestamp_t tsf_tlv;
+    t_u64 tsf_val;
+
+    ENTER();
+
+    /* Null Checks */
+    if (ppbuffer == MNULL) {
+        LEAVE();
+        return 0;
+    }
+    if (*ppbuffer == MNULL) {
+        LEAVE();
+        return 0;
+    }
+
+    memset(pmriv->adapter, &tsf_tlv, 0x00, sizeof(MrvlIEtypes_TsfTimestamp_t));
+
+    tsf_tlv.header.type = wlan_cpu_to_le16(TLV_TYPE_TSFTIMESTAMP);
+    tsf_tlv.header.len = wlan_cpu_to_le16(2 * sizeof(tsf_val));
+
+    memcpy(pmriv->adapter, *ppbuffer, &tsf_tlv, sizeof(tsf_tlv.header));
+    *ppbuffer += sizeof(tsf_tlv.header);
+
+    /* TSF timestamp from the firmware TSF when the bcn/prb rsp was received */
+    tsf_val = wlan_cpu_to_le64(pbss_desc->network_tsf);
+    memcpy(pmriv->adapter, *ppbuffer, &tsf_val, sizeof(tsf_val));
+    *ppbuffer += sizeof(tsf_val);
+
+    memcpy(pmriv->adapter, &tsf_val, pbss_desc->time_stamp, sizeof(tsf_val));
+
+    PRINTM(MINFO, "ASSOC: TSF offset calc: %016llx - %016llx\n",
+           tsf_val, pbss_desc->network_tsf);
+
+    memcpy(pmriv->adapter, *ppbuffer, &tsf_val, sizeof(tsf_val));
+    *ppbuffer += sizeof(tsf_val);
+
+    LEAVE();
+    return (sizeof(tsf_tlv.header) + (2 * sizeof(tsf_val)));
+}
+
+/**
+ *  @brief This function finds out the common rates between rate1 and rate2.
+ *
+ *  It will fill common rates in rate1 as output if found.
+ *
+ *  NOTE: Setting the MSB of the basic rates needs to be taken
+ *   care of, either before or after calling this function
+ *
+ *  @param pmpriv      A pointer to mlan_private structure
+ *  @param rate1       the buffer which keeps input and output
+ *  @param rate1_size  the size of rate1 buffer
+ *  @param rate2       the buffer which keeps rate2
+ *  @param rate2_size  the size of rate2 buffer.
+ *
+ *  @return            MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_get_common_rates(IN mlan_private * pmpriv,
+                      IN t_u8 * rate1,
+                      IN t_u32 rate1_size, IN t_u8 * rate2, IN t_u32 rate2_size)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_callbacks *pcb = (mlan_callbacks *) & pmpriv->adapter->callbacks;
+    t_u8 *ptr = rate1;
+    t_u8 *tmp = MNULL;
+    t_u32 i, j;
+
+    ENTER();
+
+    ret =
+        pcb->moal_malloc(pmpriv->adapter->pmoal_handle, rate1_size,
+                         MLAN_MEM_DEF, &tmp);
+    if (ret != MLAN_STATUS_SUCCESS || !tmp) {
+        PRINTM(MERROR, "Failed to allocate buffer\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    memcpy(pmpriv->adapter, tmp, rate1, rate1_size);
+    memset(pmpriv->adapter, rate1, 0, rate1_size);
+
+    for (i = 0; rate2[i] && i < rate2_size; i++) {
+        for (j = 0; tmp[j] && j < rate1_size; j++) {
+            /* Check common rate, excluding the bit for basic rate */
+            if ((rate2[i] & 0x7F) == (tmp[j] & 0x7F)) {
+                *rate1++ = tmp[j];
+                break;
+            }
+        }
+    }
+
+    HEXDUMP("rate1 (AP) Rates", tmp, rate1_size);
+    HEXDUMP("rate2 (Card) Rates", rate2, rate2_size);
+    HEXDUMP("Common Rates", ptr, rate1 - ptr);
+    PRINTM(MINFO, "Tx DataRate is set to 0x%X\n", pmpriv->data_rate);
+
+    if (!pmpriv->is_data_rate_auto) {
+        while (*ptr) {
+            if ((*ptr & 0x7f) == pmpriv->data_rate) {
+                ret = MLAN_STATUS_SUCCESS;
+                goto done;
+            }
+            ptr++;
+        }
+        PRINTM(MMSG, "Previously set fixed data rate %#x is not "
+               "compatible with the network\n", pmpriv->data_rate);
+
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    ret = MLAN_STATUS_SUCCESS;
+  done:
+    if (tmp)
+        pcb->moal_mfree(pmpriv->adapter->pmoal_handle, tmp);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Create the intersection of the rates supported by a target BSS and
+ *         our pmadapter settings for use in an assoc/join command.
+ *
+ *  @param pmpriv           A pointer to mlan_private structure
+ *  @param pbss_desc        BSS Descriptor whose rates are used in the setup
+ *  @param pout_rates       Output: Octet array of rates common between the BSS
+ *                          and the pmadapter supported rates settings
+ *  @param pout_rates_size  Output: Number of rates/octets set in pout_rates
+ *
+ *  @return                 MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_setup_rates_from_bssdesc(IN mlan_private * pmpriv,
+                              IN BSSDescriptor_t * pbss_desc,
+                              OUT t_u8 * pout_rates,
+                              OUT t_u32 * pout_rates_size)
+{
+    t_u8 card_rates[WLAN_SUPPORTED_RATES];
+    t_u32 card_rates_size = 0;
+    ENTER();
+    /* Copy AP supported rates */
+    memcpy(pmpriv->adapter, pout_rates, pbss_desc->supported_rates,
+           WLAN_SUPPORTED_RATES);
+    /* Get the STA supported rates */
+    card_rates_size = wlan_get_active_data_rates(pmpriv, pmpriv->bss_mode,
+                                                 pmpriv->config_bands,
+                                                 card_rates);
+    /* Get the common rates between AP and STA supported rates */
+    if (wlan_get_common_rates(pmpriv, pout_rates, WLAN_SUPPORTED_RATES,
+                              card_rates, card_rates_size)) {
+        *pout_rates_size = 0;
+        PRINTM(MERROR, "wlan_get_common_rates failed\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    *pout_rates_size =
+        MIN(wlan_strlen((char *) pout_rates), WLAN_SUPPORTED_RATES);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Update the scan entry TSF timestamps to reflect a new association
+ *
+ *  @param pmpriv        A pointer to mlan_private structure
+ *  @param pnew_bss_desc A pointer to the newly associated AP's scan table entry
+ *
+ *  @return              N/A
+ */
+static t_void
+wlan_update_tsf_timestamps(IN mlan_private * pmpriv,
+                           IN BSSDescriptor_t * pnew_bss_desc)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    t_u32 table_idx;
+    t_u64 new_tsf_base;
+    t_s64 tsf_delta;
+
+    ENTER();
+
+    memcpy(pmpriv->adapter, &new_tsf_base, pnew_bss_desc->time_stamp,
+           sizeof(new_tsf_base));
+
+    tsf_delta = new_tsf_base - pnew_bss_desc->network_tsf;
+
+    PRINTM(MINFO, "TSF: Update TSF timestamps, 0x%016llx -> 0x%016llx\n",
+           pnew_bss_desc->network_tsf, new_tsf_base);
+
+    for (table_idx = 0; table_idx < pmadapter->num_in_scan_table; table_idx++) {
+        pmadapter->pscan_table[table_idx].network_tsf += tsf_delta;
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief Append a wapi IE
+ *
+ *  This function is called from the network join command prep. routine.
+ *    If the IE buffer has been setup by the application, this routine appends
+ *    the buffer as a wapi TLV type to the request.
+ *
+ *  @param priv     A pointer to mlan_private structure
+ *  @param ppBuffer pointer to command buffer pointer
+ *
+ *  @return         bytes added to the buffer
+ */
+static int
+wlan_cmd_append_wapi_ie(mlan_private * priv, t_u8 ** ppBuffer)
+{
+    int retLen = 0;
+    MrvlIEtypesHeader_t ie_header;
+
+    ENTER();
+
+    /* Null Checks */
+    if (ppBuffer == MNULL) {
+        LEAVE();
+        return 0;
+    }
+    if (*ppBuffer == MNULL) {
+        LEAVE();
+        return 0;
+    }
+
+    /*
+     * If there is a wapi ie buffer setup, append it to the return
+     *   parameter buffer pointer.
+     */
+    if (priv->wapi_ie_len) {
+        PRINTM(MCMND, "append wapi ie %d to %p\n", priv->wapi_ie_len,
+               *ppBuffer);
+
+        /* Wrap the generic IE buffer with a pass through TLV type */
+        ie_header.type = wlan_cpu_to_le16(TLV_TYPE_WAPI_IE);
+        ie_header.len = wlan_cpu_to_le16(priv->wapi_ie_len);
+        memcpy(priv->adapter, *ppBuffer, &ie_header, sizeof(ie_header));
+
+        /* Increment the return size and the return buffer pointer param */
+        *ppBuffer += sizeof(ie_header);
+        retLen += sizeof(ie_header);
+
+        /* Copy the wapi IE buffer to the output buffer, advance pointer */
+        memcpy(priv->adapter, *ppBuffer, priv->wapi_ie, priv->wapi_ie_len);
+
+        /* Increment the return size and the return buffer pointer param */
+        *ppBuffer += priv->wapi_ie_len;
+        retLen += priv->wapi_ie_len;
+
+    }
+    /* return the length appended to the buffer */
+    LEAVE();
+    return retLen;
+}
+
+/********************************************************
+                Global Functions
+********************************************************/
+/**
+ *  @brief This function prepares command of association.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param pdata_buf    A pointer cast of BSSDescriptor_t from the
+ *                        scan table to assoc
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_cmd_802_11_associate(IN mlan_private * pmpriv,
+                          IN HostCmd_DS_COMMAND * cmd, IN t_void * pdata_buf)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    HostCmd_DS_802_11_ASSOCIATE *passo = &cmd->params.associate;
+    BSSDescriptor_t *pbss_desc;
+    MrvlIEtypes_SsIdParamSet_t *pssid_tlv;
+    MrvlIEtypes_PhyParamSet_t *pphy_tlv;
+    MrvlIEtypes_SsParamSet_t *pss_tlv;
+    MrvlIEtypes_RatesParamSet_t *prates_tlv;
+    MrvlIEtypes_AuthType_t *pauth_tlv;
+    MrvlIEtypes_RsnParamSet_t *prsn_ie_tlv;
+    MrvlIEtypes_ChanListParamSet_t *pchan_tlv;
+    WLAN_802_11_RATES rates;
+    t_u32 rates_size;
+    t_u16 tmp_cap;
+    t_u8 *pos;
+
+    ENTER();
+
+    pbss_desc = (BSSDescriptor_t *) pdata_buf;
+    pos = (t_u8 *) passo;
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_ASSOCIATE);
+
+    /* Save so we know which BSS Desc to use in the response handler */
+    pmpriv->pattempted_bss_desc = pbss_desc;
+
+    memcpy(pmadapter, passo->peer_sta_addr,
+           pbss_desc->mac_address, sizeof(passo->peer_sta_addr));
+    pos += sizeof(passo->peer_sta_addr);
+
+    /* Set the listen interval */
+    passo->listen_interval = wlan_cpu_to_le16(pmpriv->listen_interval);
+    /* Set the beacon period */
+    passo->beacon_period = wlan_cpu_to_le16(pbss_desc->beacon_period);
+
+    pos += sizeof(passo->cap_info);
+    pos += sizeof(passo->listen_interval);
+    pos += sizeof(passo->beacon_period);
+    pos += sizeof(passo->dtim_period);
+
+    pssid_tlv = (MrvlIEtypes_SsIdParamSet_t *) pos;
+    pssid_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_SSID);
+    pssid_tlv->header.len = (t_u16) pbss_desc->ssid.ssid_len;
+    memcpy(pmadapter, pssid_tlv->ssid, pbss_desc->ssid.ssid,
+           pssid_tlv->header.len);
+    pos += sizeof(pssid_tlv->header) + pssid_tlv->header.len;
+    pssid_tlv->header.len = wlan_cpu_to_le16(pssid_tlv->header.len);
+
+    pphy_tlv = (MrvlIEtypes_PhyParamSet_t *) pos;
+    pphy_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_PHY_DS);
+    pphy_tlv->header.len = sizeof(pphy_tlv->fh_ds.ds_param_set);
+    memcpy(pmadapter, &pphy_tlv->fh_ds.ds_param_set,
+           &pbss_desc->phy_param_set.ds_param_set.current_chan,
+           sizeof(pphy_tlv->fh_ds.ds_param_set));
+    pos += sizeof(pphy_tlv->header) + pphy_tlv->header.len;
+    pphy_tlv->header.len = wlan_cpu_to_le16(pphy_tlv->header.len);
+
+    pss_tlv = (MrvlIEtypes_SsParamSet_t *) pos;
+    pss_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_CF);
+    pss_tlv->header.len = sizeof(pss_tlv->cf_ibss.cf_param_set);
+    pos += sizeof(pss_tlv->header) + pss_tlv->header.len;
+    pss_tlv->header.len = wlan_cpu_to_le16(pss_tlv->header.len);
+
+    /* Get the common rates supported between the driver and the BSS Desc */
+    if (wlan_setup_rates_from_bssdesc(pmpriv, pbss_desc, rates, &rates_size)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Save the data rates into Current BSS state structure */
+    pmpriv->curr_bss_params.num_of_rates = rates_size;
+    memcpy(pmadapter, &pmpriv->curr_bss_params.data_rates, rates, rates_size);
+
+    /* Setup the Rates TLV in the association command */
+    prates_tlv = (MrvlIEtypes_RatesParamSet_t *) pos;
+    prates_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_RATES);
+    prates_tlv->header.len = wlan_cpu_to_le16((t_u16) rates_size);
+    memcpy(pmadapter, prates_tlv->rates, rates, rates_size);
+    pos += sizeof(prates_tlv->header) + rates_size;
+    PRINTM(MINFO, "ASSOC_CMD: Rates size = %d\n", rates_size);
+
+    /* Add the Authentication type to be used for Auth frames if needed */
+    if (pmpriv->sec_info.authentication_mode != MLAN_AUTH_MODE_AUTO) {
+        pauth_tlv = (MrvlIEtypes_AuthType_t *) pos;
+        pauth_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_AUTH_TYPE);
+        pauth_tlv->header.len = sizeof(pauth_tlv->auth_type);
+        if (pmpriv->sec_info.wep_status == Wlan802_11WEPEnabled)
+            pauth_tlv->auth_type =
+                wlan_cpu_to_le16((t_u16) pmpriv->sec_info.authentication_mode);
+        else
+            pauth_tlv->auth_type = wlan_cpu_to_le16(MLAN_AUTH_MODE_OPEN);
+        pos += sizeof(pauth_tlv->header) + pauth_tlv->header.len;
+        pauth_tlv->header.len = wlan_cpu_to_le16(pauth_tlv->header.len);
+    }
+
+    if (IS_SUPPORT_MULTI_BANDS(pmadapter)
+        && (pbss_desc->bss_band & pmpriv->config_bands)
+        && !(ISSUPP_11NENABLED(pmadapter->fw_cap_info)
+             && (!pbss_desc->disable_11n)
+             && (pmpriv->config_bands & BAND_GN
+                 || pmpriv->config_bands & BAND_AN)
+             && (pbss_desc->pht_cap)
+        )
+        ) {
+        /* Append a channel TLV for the channel the attempted AP was found on */
+        pchan_tlv = (MrvlIEtypes_ChanListParamSet_t *) pos;
+        pchan_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_CHANLIST);
+        pchan_tlv->header.len = wlan_cpu_to_le16(sizeof(ChanScanParamSet_t));
+
+        memset(pmadapter, pchan_tlv->chan_scan_param, 0x00,
+               sizeof(ChanScanParamSet_t));
+        pchan_tlv->chan_scan_param[0].chan_number =
+            (pbss_desc->phy_param_set.ds_param_set.current_chan);
+        PRINTM(MINFO, "Assoc: TLV Chan = %d\n",
+               pchan_tlv->chan_scan_param[0].chan_number);
+
+        pchan_tlv->chan_scan_param[0].radio_type =
+            wlan_band_to_radio_type((t_u8) pbss_desc->bss_band);
+
+        PRINTM(MINFO, "Assoc: TLV Band = %d\n",
+               pchan_tlv->chan_scan_param[0].radio_type);
+        pos += sizeof(pchan_tlv->header) + sizeof(ChanScanParamSet_t);
+    }
+    if (!pmpriv->wps.session_enable) {
+        if ((pmpriv->sec_info.wpa_enabled || pmpriv->sec_info.wpa2_enabled)) {
+            prsn_ie_tlv = (MrvlIEtypes_RsnParamSet_t *) pos;
+            prsn_ie_tlv->header.type = (t_u16) pmpriv->wpa_ie[0];       /* WPA_IE
+                                                                           or
+                                                                           RSN_IE
+                                                                         */
+            prsn_ie_tlv->header.type = prsn_ie_tlv->header.type & 0x00FF;
+            prsn_ie_tlv->header.type =
+                wlan_cpu_to_le16(prsn_ie_tlv->header.type);
+            prsn_ie_tlv->header.len = (t_u16) pmpriv->wpa_ie[1];
+            prsn_ie_tlv->header.len = prsn_ie_tlv->header.len & 0x00FF;
+            if (prsn_ie_tlv->header.len <= (sizeof(pmpriv->wpa_ie) - 2))
+                memcpy(pmadapter, prsn_ie_tlv->rsn_ie, &pmpriv->wpa_ie[2],
+                       prsn_ie_tlv->header.len);
+            else {
+                ret = MLAN_STATUS_FAILURE;
+                goto done;
+            }
+            HEXDUMP("ASSOC_CMD: RSN IE", (t_u8 *) prsn_ie_tlv,
+                    sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len);
+            pos += sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len;
+            prsn_ie_tlv->header.len = wlan_cpu_to_le16(prsn_ie_tlv->header.len);
+        } else if (pmpriv->sec_info.ewpa_enabled) {
+            prsn_ie_tlv = (MrvlIEtypes_RsnParamSet_t *) pos;
+            if (pbss_desc->pwpa_ie) {
+                prsn_ie_tlv->header.type =
+                    (t_u16) (*(pbss_desc->pwpa_ie)).vend_hdr.element_id;
+                prsn_ie_tlv->header.type = prsn_ie_tlv->header.type & 0x00FF;
+                prsn_ie_tlv->header.type =
+                    wlan_cpu_to_le16(prsn_ie_tlv->header.type);
+                prsn_ie_tlv->header.len =
+                    (t_u16) (*(pbss_desc->pwpa_ie)).vend_hdr.len;
+                prsn_ie_tlv->header.len = prsn_ie_tlv->header.len & 0x00FF;
+                if (prsn_ie_tlv->header.len <= (sizeof(pmpriv->wpa_ie))) {
+                    memcpy(pmadapter, prsn_ie_tlv->rsn_ie,
+                           &((*(pbss_desc->pwpa_ie)).vend_hdr.oui[0]),
+                           prsn_ie_tlv->header.len);
+                } else {
+                    ret = MLAN_STATUS_FAILURE;
+                    goto done;
+                }
+
+                HEXDUMP("ASSOC_CMD: RSN IE", (t_u8 *) prsn_ie_tlv,
+                        sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len);
+                pos += sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len;
+                prsn_ie_tlv->header.len =
+                    wlan_cpu_to_le16(prsn_ie_tlv->header.len);
+            }
+            if (pbss_desc->prsn_ie) {
+                prsn_ie_tlv = (MrvlIEtypes_RsnParamSet_t *) pos;
+                prsn_ie_tlv->header.type =
+                    (t_u16) (*(pbss_desc->prsn_ie)).ieee_hdr.element_id;
+                prsn_ie_tlv->header.type = prsn_ie_tlv->header.type & 0x00FF;
+                prsn_ie_tlv->header.type =
+                    wlan_cpu_to_le16(prsn_ie_tlv->header.type);
+                prsn_ie_tlv->header.len =
+                    (t_u16) (*(pbss_desc->prsn_ie)).ieee_hdr.len;
+                prsn_ie_tlv->header.len = prsn_ie_tlv->header.len & 0x00FF;
+                if (prsn_ie_tlv->header.len <= (sizeof(pmpriv->wpa_ie))) {
+                    memcpy(pmadapter, prsn_ie_tlv->rsn_ie,
+                           &((*(pbss_desc->prsn_ie)).data[0])
+                           , prsn_ie_tlv->header.len);
+                } else {
+                    ret = MLAN_STATUS_FAILURE;
+                    goto done;
+                }
+
+                HEXDUMP("ASSOC_CMD: RSN IE", (t_u8 *) prsn_ie_tlv,
+                        sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len);
+                pos += sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len;
+                prsn_ie_tlv->header.len =
+                    wlan_cpu_to_le16(prsn_ie_tlv->header.len);
+            }
+        }
+    }
+
+    if (ISSUPP_11NENABLED(pmadapter->fw_cap_info)
+        && (!pbss_desc->disable_11n)
+        && (pmpriv->config_bands & BAND_GN || pmpriv->config_bands & BAND_AN))
+        wlan_cmd_append_11n_tlv(pmpriv, pbss_desc, &pos);
+
+    wlan_wmm_process_association_req(pmpriv, &pos, &pbss_desc->wmm_ie,
+                                     pbss_desc->pht_cap);
+    if (pmpriv->sec_info.wapi_enabled && pmpriv->wapi_ie_len) {
+        wlan_cmd_append_wapi_ie(pmpriv, &pos);
+    }
+
+    wlan_cmd_append_generic_ie(pmpriv, &pos);
+
+    wlan_cmd_append_tsf_tlv(pmpriv, &pos, pbss_desc);
+
+    if (wlan_11d_create_dnld_countryinfo(pmpriv, (t_u8) pbss_desc->bss_band)) {
+        PRINTM(MERROR, "Dnld_countryinfo_11d failed\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    if (wlan_11d_parse_dnld_countryinfo(pmpriv, pmpriv->pattempted_bss_desc)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /*
+     * Call 11h join API after capability bits are set so adhoc/infra 11h
+     * behavior can be properly triggered.  pos modified if data is appended
+     */
+    wlan_11h_process_join(pmpriv, &pos, &passo->cap_info,
+                          (t_u8) pbss_desc->bss_band,
+                          pbss_desc->phy_param_set.ds_param_set.current_chan,
+                          &pbss_desc->wlan_11h_bss_info);
+
+    cmd->size = wlan_cpu_to_le16((t_u16) (pos - (t_u8 *) passo) + S_DS_GEN);
+
+    /* Set the Capability info at last */
+    memcpy(pmadapter, &tmp_cap, &pbss_desc->cap_info, sizeof(passo->cap_info));
+
+    if (pmpriv->config_bands == BAND_B) {
+        SHORT_SLOT_TIME_DISABLED(tmp_cap);
+    }
+
+    tmp_cap &= CAPINFO_MASK;
+    PRINTM(MINFO, "ASSOC_CMD: tmp_cap=%4X CAPINFO_MASK=%4lX\n",
+           tmp_cap, CAPINFO_MASK);
+    tmp_cap = wlan_cpu_to_le16(tmp_cap);
+    memcpy(pmadapter, &passo->cap_info, &tmp_cap, sizeof(passo->cap_info));
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Association firmware command response handler
+ *
+ *   The response buffer for the association command has the following
+ *      memory layout.
+ *
+ *   For cases where an association response was not received (indicated
+ *      by the CapInfo and AId field):
+ *
+ *     .------------------------------------------------------------.
+ *     |  Header(4 * sizeof(t_u16)):  Standard command response hdr |
+ *     .------------------------------------------------------------.
+ *     |  cap_info/Error Return(t_u16):                             |
+ *     |           0xFFFF(-1): Internal error for association       |
+ *     |           0xFFFE(-2): Authentication unhandled message     |
+ *     |           0xFFFD(-3): Authentication refused               |
+ *     |           0xFFFC(-4): Timeout waiting for AP response      |
+ *     |           0xFFFB(-5): Internal error for authentication    |
+ *     .------------------------------------------------------------.
+ *     |  status_code(t_u16):                                       |
+ *     |        If cap_info is -1:                                  |
+ *     |           An internal firmware failure prevented the       |
+ *     |           command from being processed. The status code    |
+ *     |           is 6 if associate response parameter invlaid,    |
+ *     |           1 otherwise.                                     |
+ *     |                                                            |
+ *     |        If cap_info is -2:                                  |
+ *     |           An authentication frame was received but was     |
+ *     |           not handled by the firmware. IEEE Status code    |
+ *     |           for the failure is returned.                     |
+ *     |                                                            |
+ *     |        If cap_info is -3:                                  |
+ *     |           An authentication frame was received and the     |
+ *     |           status_code is the IEEE Status reported in the   |
+ *     |           response.                                        |
+ *     |                                                            |
+ *     |        If cap_info is -4:                                  |
+ *     |           (1) Association response timeout                 |
+ *     |           (2) Authentication response timeout              |
+ *     |                                                            |
+ *     |        If cap_info is -5:                                  |
+ *     |           An internal firmware failure prevented the       |
+ *     |           command from being processed. The status code    |
+ *     |           is 6 if authentication parameter invlaid,        |
+ *     |           1 otherwise.                                     |
+ *     .------------------------------------------------------------.
+ *     |  a_id(t_u16): 0xFFFF                                       |
+ *     .------------------------------------------------------------.
+ *
+ *
+ *   For cases where an association response was received, the IEEE
+ *     standard association response frame is returned:
+ *
+ *     .------------------------------------------------------------.
+ *     |  Header(4 * sizeof(t_u16)):  Standard command response hdr |
+ *     .------------------------------------------------------------.
+ *     |  cap_info(t_u16): IEEE Capability                          |
+ *     .------------------------------------------------------------.
+ *     |  status_code(t_u16): IEEE Status Code                      |
+ *     .------------------------------------------------------------.
+ *     |  a_id(t_u16): IEEE Association ID                          |
+ *     .------------------------------------------------------------.
+ *     |  IEEE IEs(variable): Any received IEs comprising the       |
+ *     |                      remaining portion of a received       |
+ *     |                      association response frame.           |
+ *     .------------------------------------------------------------.
+ *
+ *  For simplistic handling, the status_code field can be used to determine
+ *    an association success (0) or failure (non-zero).
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ret_802_11_associate(IN mlan_private * pmpriv,
+                          IN HostCmd_DS_COMMAND * resp, IN t_void * pioctl_buf)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ioctl_req *pioctl_req = (mlan_ioctl_req *) pioctl_buf;
+    IEEEtypes_AssocRsp_t *passoc_rsp;
+    BSSDescriptor_t *pbss_desc;
+    t_u8 enable_data = MTRUE;
+    t_u8 event_buf[100];
+    mlan_event *pevent = (mlan_event *) event_buf;
+
+    ENTER();
+
+    passoc_rsp = (IEEEtypes_AssocRsp_t *) & resp->params;
+    passoc_rsp->status_code = wlan_le16_to_cpu(passoc_rsp->status_code);
+
+    HEXDUMP("ASSOC_RESP:", (t_u8 *) & resp->params, (resp->size - S_DS_GEN));
+
+    pmpriv->assoc_rsp_size = MIN(resp->size - S_DS_GEN,
+                                 sizeof(pmpriv->assoc_rsp_buf));
+
+    memcpy(pmpriv->adapter, pmpriv->assoc_rsp_buf, &resp->params,
+           pmpriv->assoc_rsp_size);
+
+    if (passoc_rsp->status_code) {
+        pmpriv->adapter->dbg.num_cmd_assoc_failure++;
+        PRINTM(MERROR, "ASSOC_RESP: Association Failed, "
+               "status code = %d, error = 0x%x, a_id = 0x%x\n",
+               wlan_le16_to_cpu(passoc_rsp->status_code),
+               wlan_le16_to_cpu(*(t_u16 *) & passoc_rsp->capability),
+               wlan_le16_to_cpu(passoc_rsp->a_id));
+
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Send a Media Connected event, according to the Spec */
+    pmpriv->media_connected = MTRUE;
+
+    pmpriv->adapter->pps_uapsd_mode = MFALSE;
+    pmpriv->adapter->tx_lock_flag = MFALSE;
+    pmpriv->adapter->delay_null_pkt = MFALSE;
+
+    /* Set the attempted BSSID Index to current */
+    pbss_desc = pmpriv->pattempted_bss_desc;
+
+    PRINTM(MINFO, "ASSOC_RESP: %s\n", pbss_desc->ssid.ssid);
+
+    /* Make a copy of current BSSID descriptor */
+    memcpy(pmpriv->adapter, &pmpriv->curr_bss_params.bss_descriptor,
+           pbss_desc, sizeof(BSSDescriptor_t));
+
+    /* Update curr_bss_params */
+    pmpriv->curr_bss_params.bss_descriptor.channel
+        = pbss_desc->phy_param_set.ds_param_set.current_chan;
+
+    pmpriv->curr_bss_params.band = (t_u8) pbss_desc->bss_band;
+
+    /*
+     * Adjust the timestamps in the scan table to be relative to the newly
+     * associated AP's TSF
+     */
+    wlan_update_tsf_timestamps(pmpriv, pbss_desc);
+
+    if (pbss_desc->wmm_ie.vend_hdr.element_id == WMM_IE)
+        pmpriv->curr_bss_params.wmm_enabled = MTRUE;
+    else
+        pmpriv->curr_bss_params.wmm_enabled = MFALSE;
+
+    if ((pmpriv->wmm_required
+         || (pbss_desc->pht_cap &&
+             (pbss_desc->pht_cap->ieee_hdr.element_id == HT_CAPABILITY))
+        ) && pmpriv->curr_bss_params.wmm_enabled)
+        pmpriv->wmm_enabled = MTRUE;
+    else
+        pmpriv->wmm_enabled = MFALSE;
+
+    pmpriv->curr_bss_params.wmm_uapsd_enabled = MFALSE;
+
+    if (pmpriv->wmm_enabled == MTRUE)
+        pmpriv->curr_bss_params.wmm_uapsd_enabled
+            = pbss_desc->wmm_ie.qos_info.qos_uapsd;
+
+    PRINTM(MINFO, "ASSOC_RESP: curr_pkt_filter is 0x%x\n",
+           pmpriv->curr_pkt_filter);
+    if (pmpriv->sec_info.wpa_enabled || pmpriv->sec_info.wpa2_enabled)
+        pmpriv->wpa_is_gtk_set = MFALSE;
+
+    if (pmpriv->wmm_enabled)
+        /* Don't re-enable carrier until we get the WMM_GET_STATUS event */
+        enable_data = MFALSE;
+    else
+        /* Since WMM is not enabled, setup the queues with the defaults */
+        wlan_wmm_setup_queues(pmpriv);
+
+    if (enable_data) {
+        PRINTM(MINFO, "Post association, re-enabling data flow\n");
+    }
+
+    /* Reset SNR/NF/RSSI values */
+    pmpriv->data_rssi_last = 0;
+    pmpriv->data_nf_last = 0;
+    pmpriv->data_rssi_avg = 0;
+    pmpriv->data_nf_avg = 0;
+    pmpriv->bcn_rssi_last = 0;
+    pmpriv->bcn_nf_last = 0;
+    pmpriv->bcn_rssi_avg = 0;
+    pmpriv->bcn_nf_avg = 0;
+    pmpriv->rxpd_rate = 0;
+    pmpriv->rxpd_htinfo = 0;
+    if (pbss_desc->pht_cap) {
+        if (GETHT_MAXAMSDU(pbss_desc->pht_cap->ht_cap.ht_cap_info))
+            pmpriv->max_amsdu = MLAN_TX_DATA_BUF_SIZE_8K;
+        else
+            pmpriv->max_amsdu = MLAN_TX_DATA_BUF_SIZE_4K;
+    }
+
+    wlan_save_curr_bcn(pmpriv);
+
+    pmpriv->adapter->dbg.num_cmd_assoc_success++;
+
+    PRINTM(MINFO, "ASSOC_RESP: Associated\n");
+    pevent->bss_index = pmpriv->bss_index;
+    pevent->event_id = MLAN_EVENT_ID_DRV_CONNECTED;
+    pevent->event_len = MLAN_MAC_ADDR_LENGTH;
+    memcpy(pmpriv->adapter, (t_u8 *) pevent->event_buf,
+           (t_u8 *) pmpriv->curr_bss_params.bss_descriptor.mac_address,
+           MLAN_MAC_ADDR_LENGTH);
+
+    /* Add the ra_list here for infra mode as there will be only 1 ra always */
+    wlan_ralist_add(pmpriv, pmpriv->curr_bss_params.bss_descriptor.mac_address);
+
+    wlan_recv_event(pmpriv, MLAN_EVENT_ID_DRV_CONNECTED, pevent);
+
+    /* Send OBSS scan param to the application if available */
+    wlan_2040_coex_event(pmpriv);
+
+    if (!pmpriv->sec_info.wpa_enabled
+        && !pmpriv->sec_info.wpa2_enabled
+        && !pmpriv->sec_info.ewpa_enabled && !pmpriv->sec_info.wapi_enabled) {
+        /* We are in Open/WEP mode, open port immediately */
+        if (pmpriv->port_ctrl_mode == MTRUE) {
+            pmpriv->port_open = MTRUE;
+            PRINTM(MINFO, "ASSOC_RESP: port_status = OPEN\n");
+        }
+    }
+
+    if (pmpriv->sec_info.wpa_enabled
+        || pmpriv->sec_info.wpa2_enabled
+        || pmpriv->sec_info.ewpa_enabled || pmpriv->sec_info.wapi_enabled)
+        pmpriv->scan_block = MTRUE;
+
+  done:
+    /* Need to indicate IOCTL complete */
+    if (pioctl_req != MNULL) {
+        if (ret != MLAN_STATUS_SUCCESS) {
+            if (passoc_rsp->status_code)
+                pioctl_req->status_code =
+                    wlan_le16_to_cpu(passoc_rsp->status_code);
+            else
+                pioctl_req->status_code = MLAN_ERROR_CMD_ASSOC_FAIL;
+        } else {
+            pioctl_req->status_code = MLAN_ERROR_NO_ERROR;
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function prepares command of ad_hoc_start.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param pdata_buf    A pointer cast of mlan_802_11_ssid structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_cmd_802_11_ad_hoc_start(IN mlan_private * pmpriv,
+                             IN HostCmd_DS_COMMAND * cmd, IN t_void * pdata_buf)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    HostCmd_DS_802_11_AD_HOC_START *padhoc_start = &cmd->params.adhoc_start;
+    BSSDescriptor_t *pbss_desc;
+    t_u32 cmd_append_size = 0;
+    t_u32 i;
+    t_u16 tmp_cap;
+    MrvlIEtypes_ChanListParamSet_t *pchan_tlv;
+
+    MrvlIEtypes_RsnParamSet_t *prsn_ie_tlv;
+    MrvlIETypes_HTCap_t *pht_cap;
+    MrvlIETypes_HTInfo_t *pht_info;
+    /* wpa ie for WPA_NONE AES */
+    const t_u8 wpa_ie[24] = { 0xdd, 0x16, 0x00, 0x50, 0xf2, 0x01, 0x01, 0x00,
+        0x00, 0x50, 0xf2, 0x04, 0x01, 0x00, 0x00, 0x50,
+        0xf2, 0x00, 0x01, 0x00, 0x00, 0x50, 0xf2, 0x00
+    };
+    t_s32 append_size_11h = 0;
+    t_u8 *pos = (t_u8 *) padhoc_start + sizeof(HostCmd_DS_802_11_AD_HOC_START);
+
+    ENTER();
+
+    if (!pmadapter) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_AD_HOC_START);
+
+    pbss_desc = &pmpriv->curr_bss_params.bss_descriptor;
+    pmpriv->pattempted_bss_desc = pbss_desc;
+
+    /*
+     * Fill in the parameters for 2 data structures:
+     *   1. HostCmd_DS_802_11_AD_HOC_START command
+     *   2. pbss_desc
+     * Driver will fill up SSID, bss_mode,IBSS param, Physical Param,
+     * probe delay, and Cap info.
+     * Firmware will fill up beacon period, Basic rates
+     * and operational rates.
+     */
+
+    memset(pmadapter, padhoc_start->ssid, 0, MLAN_MAX_SSID_LENGTH);
+
+    memcpy(pmadapter, padhoc_start->ssid,
+           ((mlan_802_11_ssid *) pdata_buf)->ssid,
+           MIN(MLAN_MAX_SSID_LENGTH,
+               ((mlan_802_11_ssid *) pdata_buf)->ssid_len));
+
+    PRINTM(MINFO, "ADHOC_S_CMD: SSID = %s\n", padhoc_start->ssid);
+
+    memset(pmadapter, pbss_desc->ssid.ssid, 0, MLAN_MAX_SSID_LENGTH);
+    memcpy(pmadapter, pbss_desc->ssid.ssid,
+           ((mlan_802_11_ssid *) pdata_buf)->ssid,
+           MIN(MLAN_MAX_SSID_LENGTH,
+               ((mlan_802_11_ssid *) pdata_buf)->ssid_len));
+
+    pbss_desc->ssid.ssid_len =
+        MIN(MLAN_MAX_SSID_LENGTH, ((mlan_802_11_ssid *) pdata_buf)->ssid_len);
+
+    /* Set the BSS mode */
+    padhoc_start->bss_mode = HostCmd_BSS_MODE_IBSS;
+    pbss_desc->bss_mode = MLAN_BSS_MODE_IBSS;
+    padhoc_start->beacon_period = wlan_cpu_to_le16(pmpriv->beacon_period);
+    pbss_desc->beacon_period = pmpriv->beacon_period;
+
+    /* Set Physical param set */
+/** Parameter IE Id */
+#define DS_PARA_IE_ID   3
+/** Parameter IE length */
+#define DS_PARA_IE_LEN  1
+
+    padhoc_start->phy_param_set.ds_param_set.element_id = DS_PARA_IE_ID;
+    padhoc_start->phy_param_set.ds_param_set.len = DS_PARA_IE_LEN;
+
+    if (!wlan_get_cfp_by_band_and_channel
+        (pmadapter, pmadapter->adhoc_start_band, (t_u16) pmpriv->adhoc_channel,
+         pmadapter->region_channel)) {
+
+        chan_freq_power_t *cfp;
+        cfp =
+            wlan_get_cfp_by_band_and_channel(pmadapter,
+                                             pmadapter->adhoc_start_band,
+                                             FIRST_VALID_CHANNEL,
+                                             pmadapter->region_channel);
+        if (cfp)
+            pmpriv->adhoc_channel = (t_u8) cfp->channel;
+    }
+
+    MASSERT(pmpriv->adhoc_channel);
+
+    PRINTM(MINFO, "ADHOC_S_CMD: Creating ADHOC on Channel %d\n",
+           pmpriv->adhoc_channel);
+
+    pmpriv->curr_bss_params.bss_descriptor.channel = pmpriv->adhoc_channel;
+    pmpriv->curr_bss_params.band = pmadapter->adhoc_start_band;
+
+    pbss_desc->channel = pmpriv->adhoc_channel;
+    padhoc_start->phy_param_set.ds_param_set.current_chan =
+        pmpriv->adhoc_channel;
+
+    memcpy(pmadapter, &pbss_desc->phy_param_set,
+           &padhoc_start->phy_param_set, sizeof(IEEEtypes_PhyParamSet_t));
+
+    pbss_desc->network_type_use = Wlan802_11DS;
+
+    /* Set IBSS param set */
+/** IBSS parameter IE Id */
+#define IBSS_PARA_IE_ID   6
+/** IBSS parameter IE length */
+#define IBSS_PARA_IE_LEN  2
+
+    padhoc_start->ss_param_set.ibss_param_set.element_id = IBSS_PARA_IE_ID;
+    padhoc_start->ss_param_set.ibss_param_set.len = IBSS_PARA_IE_LEN;
+    padhoc_start->ss_param_set.ibss_param_set.atim_window
+        = wlan_cpu_to_le16(pmpriv->atim_window);
+    pbss_desc->atim_window = pmpriv->atim_window;
+    memcpy(pmadapter, &pbss_desc->ss_param_set,
+           &padhoc_start->ss_param_set, sizeof(IEEEtypes_SsParamSet_t));
+
+    /* Set Capability info */
+    padhoc_start->cap.ess = 0;
+    padhoc_start->cap.ibss = 1;
+    pbss_desc->cap_info.ibss = 1;
+
+    /* Set up privacy in pbss_desc */
+    if (pmpriv->sec_info.wep_status == Wlan802_11WEPEnabled
+        || pmpriv->adhoc_aes_enabled
+        || pmpriv->sec_info.wpa_enabled || pmpriv->sec_info.ewpa_enabled) {
+/** Ad-Hoc capability privacy on */
+#define AD_HOC_CAP_PRIVACY_ON   1
+        PRINTM(MINFO, "ADHOC_S_CMD: wep_status set, Privacy to WEP\n");
+        pbss_desc->privacy = Wlan802_11PrivFilter8021xWEP;
+        padhoc_start->cap.privacy = AD_HOC_CAP_PRIVACY_ON;
+    } else {
+        PRINTM(MWARN, "ADHOC_S_CMD: wep_status NOT set, Setting "
+               "Privacy to ACCEPT ALL\n");
+        pbss_desc->privacy = Wlan802_11PrivFilterAcceptAll;
+    }
+
+    memset(pmadapter, padhoc_start->DataRate, 0,
+           sizeof(padhoc_start->DataRate));
+    wlan_get_active_data_rates(pmpriv, pmpriv->bss_mode,
+                               pmadapter->adhoc_start_band,
+                               padhoc_start->DataRate);
+    if ((pmadapter->adhoc_start_band & BAND_G) &&
+        (pmpriv->curr_pkt_filter & HostCmd_ACT_MAC_ADHOC_G_PROTECTION_ON)) {
+        ret =
+            wlan_prepare_cmd(pmpriv, HostCmd_CMD_MAC_CONTROL,
+                             HostCmd_ACT_GEN_SET, 0, MNULL,
+                             &pmpriv->curr_pkt_filter);
+
+        if (ret) {
+            PRINTM(MERROR, "ADHOC_S_CMD: G Protection config failed\n");
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+    }
+    /* Find the last non zero */
+    for (i = 0; i < sizeof(padhoc_start->DataRate) && padhoc_start->DataRate[i];
+         i++);
+
+    pmpriv->curr_bss_params.num_of_rates = i;
+
+    /* Copy the ad-hoc creating rates into Current BSS rate structure */
+    memcpy(pmadapter, &pmpriv->curr_bss_params.data_rates,
+           &padhoc_start->DataRate, pmpriv->curr_bss_params.num_of_rates);
+
+    PRINTM(MINFO, "ADHOC_S_CMD: Rates=%02x %02x %02x %02x \n",
+           padhoc_start->DataRate[0], padhoc_start->DataRate[1],
+           padhoc_start->DataRate[2], padhoc_start->DataRate[3]);
+
+    PRINTM(MINFO, "ADHOC_S_CMD: AD HOC Start command is ready\n");
+
+    if (IS_SUPPORT_MULTI_BANDS(pmadapter)) {
+        /* Append a channel TLV */
+        pchan_tlv = (MrvlIEtypes_ChanListParamSet_t *) pos;
+        pchan_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_CHANLIST);
+        pchan_tlv->header.len = wlan_cpu_to_le16(sizeof(ChanScanParamSet_t));
+
+        memset(pmadapter, pchan_tlv->chan_scan_param, 0x00,
+               sizeof(ChanScanParamSet_t));
+        pchan_tlv->chan_scan_param[0].chan_number =
+            (t_u8) pmpriv->curr_bss_params.bss_descriptor.channel;
+
+        PRINTM(MINFO, "ADHOC_S_CMD: TLV Chan = %d\n",
+               pchan_tlv->chan_scan_param[0].chan_number);
+
+        pchan_tlv->chan_scan_param[0].radio_type
+            = wlan_band_to_radio_type(pmpriv->curr_bss_params.band);
+        if (pmadapter->adhoc_start_band & BAND_GN
+            || pmadapter->adhoc_start_band & BAND_AN) {
+            if (pmadapter->chan_bandwidth == CHANNEL_BW_40MHZ_ABOVE) {
+                pchan_tlv->chan_scan_param[0].radio_type |=
+                    SECOND_CHANNEL_ABOVE;
+                pchan_tlv->chan_scan_param[0].radio_type |= CHAN_BW_40MHZ << 2;
+            } else if (pmadapter->chan_bandwidth == CHANNEL_BW_40MHZ_BELOW) {
+                pchan_tlv->chan_scan_param[0].radio_type |=
+                    SECOND_CHANNEL_BELOW;
+                pchan_tlv->chan_scan_param[0].radio_type |= CHAN_BW_40MHZ << 2;
+            }
+        }
+        PRINTM(MINFO, "ADHOC_S_CMD: TLV Band = %d\n",
+               pchan_tlv->chan_scan_param[0].radio_type);
+        pos += sizeof(pchan_tlv->header) + sizeof(ChanScanParamSet_t);
+        cmd_append_size +=
+            sizeof(pchan_tlv->header) + sizeof(ChanScanParamSet_t);
+    }
+
+    if (wlan_11d_create_dnld_countryinfo(pmpriv, pmpriv->curr_bss_params.band)) {
+        PRINTM(MERROR, "ADHOC_S_CMD: dnld_countryinfo_11d failed\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /*
+     * Call 11h start API to add any 11h flags/elements as TLV parameters
+     */
+    append_size_11h = wlan_11h_process_start(pmpriv, &pos, &padhoc_start->cap,
+                                             pmpriv->adhoc_channel,
+                                             &pbss_desc->wlan_11h_bss_info);
+    if (append_size_11h >= 0)
+        cmd_append_size += append_size_11h;
+    else {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    if (pmpriv->sec_info.ewpa_enabled) {
+        memcpy(pmadapter, pmpriv->wpa_ie, wpa_ie, sizeof(wpa_ie));
+        pmpriv->wpa_ie_len = sizeof(wpa_ie);
+    }
+
+    if (pmpriv->sec_info.wpa_enabled || pmpriv->sec_info.ewpa_enabled) {
+        prsn_ie_tlv = (MrvlIEtypes_RsnParamSet_t *) pos;
+        prsn_ie_tlv->header.type = (t_u16) pmpriv->wpa_ie[0];   /* WPA_IE or
+                                                                   RSN_IE */
+        prsn_ie_tlv->header.type = prsn_ie_tlv->header.type & 0x00FF;
+        prsn_ie_tlv->header.type = wlan_cpu_to_le16(prsn_ie_tlv->header.type);
+        prsn_ie_tlv->header.len = (t_u16) pmpriv->wpa_ie[1];
+        prsn_ie_tlv->header.len = prsn_ie_tlv->header.len & 0x00FF;
+        if (prsn_ie_tlv->header.len <= (sizeof(pmpriv->wpa_ie) - 2))
+            memcpy(pmadapter, prsn_ie_tlv->rsn_ie, &pmpriv->wpa_ie[2],
+                   prsn_ie_tlv->header.len);
+        else {
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+
+        DBG_HEXDUMP(MCMD_D, "ADHOC_S_CMD: RSN IE", (t_u8 *) prsn_ie_tlv,
+                    sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len);
+        pos += sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len;
+        cmd_append_size +=
+            sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len;
+        prsn_ie_tlv->header.len = wlan_cpu_to_le16(prsn_ie_tlv->header.len);
+    }
+
+    if (pmadapter->adhoc_11n_enabled == MTRUE) {
+        {
+            pht_cap = (MrvlIETypes_HTCap_t *) pos;
+            memset(pmadapter, pht_cap, 0, sizeof(MrvlIETypes_HTCap_t));
+            pht_cap->header.type = wlan_cpu_to_le16(HT_CAPABILITY);
+            pht_cap->header.len = sizeof(HTCap_t);
+            wlan_fill_ht_cap_tlv(pmpriv, pht_cap, pmpriv->curr_bss_params.band);
+            HEXDUMP("ADHOC_START: HT_CAPABILITIES IE", (t_u8 *) pht_cap,
+                    sizeof(MrvlIETypes_HTCap_t));
+            pos += sizeof(MrvlIETypes_HTCap_t);
+            cmd_append_size += sizeof(MrvlIETypes_HTCap_t);
+            pht_cap->header.len = wlan_cpu_to_le16(pht_cap->header.len);
+        }
+        {
+            pht_info = (MrvlIETypes_HTInfo_t *) pos;
+            memset(pmadapter, pht_info, 0, sizeof(MrvlIETypes_HTInfo_t));
+            pht_info->header.type = wlan_cpu_to_le16(HT_OPERATION);
+            pht_info->header.len = sizeof(HTInfo_t);
+            pht_info->ht_info.pri_chan =
+                (t_u8) pmpriv->curr_bss_params.bss_descriptor.channel;
+            if (pmadapter->chan_bandwidth) {
+                pht_info->ht_info.field2 = pmadapter->chan_bandwidth;
+                SET_CHANWIDTH40(pht_info->ht_info.field2);
+            }
+            pht_info->ht_info.field3 = NON_GREENFIELD_STAS;
+            pht_info->ht_info.basic_mcs_set[0] = 0xff;
+            HEXDUMP("ADHOC_START: HT_INFORMATION IE", (t_u8 *) pht_info,
+                    sizeof(MrvlIETypes_HTInfo_t));
+            pos += sizeof(MrvlIETypes_HTInfo_t);
+            cmd_append_size += sizeof(MrvlIETypes_HTInfo_t);
+            pht_info->header.len = wlan_cpu_to_le16(pht_info->header.len);
+        }
+    }
+
+    cmd->size =
+        (t_u16) wlan_cpu_to_le16((t_u16) (sizeof(HostCmd_DS_802_11_AD_HOC_START)
+                                          + S_DS_GEN + cmd_append_size));
+
+    memcpy(pmadapter, &tmp_cap, &padhoc_start->cap, sizeof(t_u16));
+
+    if (pmadapter->adhoc_start_band == BAND_B) {
+        SHORT_SLOT_TIME_DISABLED(tmp_cap);
+    } else {
+        SHORT_SLOT_TIME_ENABLED(tmp_cap);
+    }
+
+    tmp_cap = wlan_cpu_to_le16(tmp_cap);
+    memcpy(pmadapter, &padhoc_start->cap, &tmp_cap, sizeof(t_u16));
+
+    ret = MLAN_STATUS_SUCCESS;
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function prepares command of ad_hoc_join.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param pdata_buf    Void cast of BSSDescriptor_t from the
+ *                        scan table to join
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_cmd_802_11_ad_hoc_join(IN mlan_private * pmpriv,
+                            IN HostCmd_DS_COMMAND * cmd, IN t_void * pdata_buf)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    HostCmd_DS_802_11_AD_HOC_JOIN *padhoc_join = &cmd->params.adhoc_join;
+    BSSDescriptor_t *pbss_desc = (BSSDescriptor_t *) pdata_buf;
+    MrvlIEtypes_ChanListParamSet_t *pchan_tlv;
+    MrvlIEtypes_RsnParamSet_t *prsn_ie_tlv;
+    t_u32 cmd_append_size = 0;
+    t_u16 tmp_cap;
+    t_u32 i, rates_size = 0;
+    t_u16 curr_pkt_filter;
+    t_u8 *pos = (t_u8 *) padhoc_join + sizeof(HostCmd_DS_802_11_AD_HOC_JOIN);
+
+    ENTER();
+
+/** Use G protection */
+#define USE_G_PROTECTION        0x02
+    if (pbss_desc->erp_flags & USE_G_PROTECTION) {
+        curr_pkt_filter =
+            pmpriv->curr_pkt_filter | HostCmd_ACT_MAC_ADHOC_G_PROTECTION_ON;
+
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_MAC_CONTROL,
+                               HostCmd_ACT_GEN_SET, 0, MNULL, &curr_pkt_filter);
+        if (ret) {
+            PRINTM(MERROR, "ADHOC_J_CMD: G Protection config failed\n");
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+    }
+
+    pmpriv->pattempted_bss_desc = pbss_desc;
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_AD_HOC_JOIN);
+
+    padhoc_join->bss_descriptor.bss_mode = HostCmd_BSS_MODE_IBSS;
+
+    padhoc_join->bss_descriptor.beacon_period
+        = wlan_cpu_to_le16(pbss_desc->beacon_period);
+
+    memcpy(pmadapter, &padhoc_join->bss_descriptor.bssid,
+           &pbss_desc->mac_address, MLAN_MAC_ADDR_LENGTH);
+
+    memcpy(pmadapter, &padhoc_join->bss_descriptor.ssid,
+           &pbss_desc->ssid.ssid,
+           MIN(MLAN_MAX_SSID_LENGTH, pbss_desc->ssid.ssid_len));
+
+    memcpy(pmadapter, &padhoc_join->bss_descriptor.phy_param_set,
+           &pbss_desc->phy_param_set, sizeof(IEEEtypes_PhyParamSet_t));
+
+    memcpy(pmadapter, &padhoc_join->bss_descriptor.ss_param_set,
+           &pbss_desc->ss_param_set, sizeof(IEEEtypes_SsParamSet_t));
+
+    memcpy(pmadapter, &tmp_cap, &pbss_desc->cap_info,
+           sizeof(IEEEtypes_CapInfo_t));
+
+    tmp_cap &= CAPINFO_MASK;
+
+    PRINTM(MINFO, "ADHOC_J_CMD: tmp_cap=%4X CAPINFO_MASK=%4lX\n",
+           tmp_cap, CAPINFO_MASK);
+    memcpy(pmadapter, &padhoc_join->bss_descriptor.cap, &tmp_cap,
+           sizeof(IEEEtypes_CapInfo_t));
+
+    /* Information on BSSID descriptor passed to FW */
+    PRINTM(MINFO,
+           "ADHOC_J_CMD: BSSID = %02x-%02x-%02x-%02x-%02x-%02x, SSID = %s\n",
+           padhoc_join->bss_descriptor.bssid[0],
+           padhoc_join->bss_descriptor.bssid[1],
+           padhoc_join->bss_descriptor.bssid[2],
+           padhoc_join->bss_descriptor.bssid[3],
+           padhoc_join->bss_descriptor.bssid[4],
+           padhoc_join->bss_descriptor.bssid[5],
+           padhoc_join->bss_descriptor.ssid);
+
+    for (i = 0; i < WLAN_SUPPORTED_RATES && pbss_desc->supported_rates[i]; i++);
+    rates_size = i;
+
+    /* Copy Data Rates from the Rates recorded in scan response */
+    memset(pmadapter, padhoc_join->bss_descriptor.data_rates, 0,
+           sizeof(padhoc_join->bss_descriptor.data_rates));
+    memcpy(pmadapter, padhoc_join->bss_descriptor.data_rates,
+           pbss_desc->supported_rates, rates_size);
+
+    HEXDUMP("Adapted Rates:", padhoc_join->bss_descriptor.data_rates,
+            rates_size);
+
+    /* Copy the adhoc join rates into Current BSS state structure */
+    pmpriv->curr_bss_params.num_of_rates = rates_size;
+    memcpy(pmadapter, &pmpriv->curr_bss_params.data_rates,
+           pbss_desc->supported_rates, rates_size);
+
+    /* Copy the channel information */
+    pmpriv->curr_bss_params.bss_descriptor.channel = pbss_desc->channel;
+    pmpriv->curr_bss_params.band = (t_u8) pbss_desc->bss_band;
+
+    if (pmpriv->sec_info.wep_status == Wlan802_11WEPEnabled
+        || pmpriv->adhoc_aes_enabled
+        || pmpriv->sec_info.wpa_enabled || pmpriv->sec_info.ewpa_enabled)
+        padhoc_join->bss_descriptor.cap.privacy = AD_HOC_CAP_PRIVACY_ON;
+
+    if (IS_SUPPORT_MULTI_BANDS(pmadapter)) {
+        /* Append a channel TLV */
+        pchan_tlv = (MrvlIEtypes_ChanListParamSet_t *) pos;
+        pchan_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_CHANLIST);
+        pchan_tlv->header.len = wlan_cpu_to_le16(sizeof(ChanScanParamSet_t));
+
+        memset(pmadapter, pchan_tlv->chan_scan_param, 0x00,
+               sizeof(ChanScanParamSet_t));
+        pchan_tlv->chan_scan_param[0].chan_number =
+            (pbss_desc->phy_param_set.ds_param_set.current_chan);
+        PRINTM(MINFO, "ADHOC_J_CMD: TLV Chan = %d\n",
+               pchan_tlv->chan_scan_param[0].chan_number);
+
+        pchan_tlv->chan_scan_param[0].radio_type
+            = wlan_band_to_radio_type((t_u8) pbss_desc->bss_band);
+
+        PRINTM(MINFO, "ADHOC_J_CMD: TLV Band = %d\n",
+               pchan_tlv->chan_scan_param[0].radio_type);
+        pos += sizeof(pchan_tlv->header) + sizeof(ChanScanParamSet_t);
+        cmd_append_size +=
+            sizeof(pchan_tlv->header) + sizeof(ChanScanParamSet_t);
+    }
+
+    if (wlan_11d_create_dnld_countryinfo(pmpriv, (t_u8) pbss_desc->bss_band)) {
+        PRINTM(MERROR, "Dnld_countryinfo_11d failed\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    if (wlan_11d_parse_dnld_countryinfo(pmpriv, pmpriv->pattempted_bss_desc)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /*
+     * Call 11h join API after capability bits are set so
+     *   adhoc/infra 11h behavior can be properly triggered.
+     *   pos modified if data is appended
+     */
+    cmd_append_size += wlan_11h_process_join(pmpriv, &pos,
+                                             &padhoc_join->bss_descriptor.cap,
+                                             (t_u8) pbss_desc->bss_band,
+                                             pbss_desc->channel,
+                                             &pbss_desc->wlan_11h_bss_info);
+
+    if (pmpriv->sec_info.wpa_enabled) {
+        prsn_ie_tlv = (MrvlIEtypes_RsnParamSet_t *) pos;
+        prsn_ie_tlv->header.type = (t_u16) pmpriv->wpa_ie[0];   /* WPA_IE or
+                                                                   RSN_IE */
+        prsn_ie_tlv->header.type = prsn_ie_tlv->header.type & 0x00FF;
+        prsn_ie_tlv->header.type = wlan_cpu_to_le16(prsn_ie_tlv->header.type);
+        prsn_ie_tlv->header.len = (t_u16) pmpriv->wpa_ie[1];
+        prsn_ie_tlv->header.len = prsn_ie_tlv->header.len & 0x00FF;
+        if (prsn_ie_tlv->header.len <= (sizeof(pmpriv->wpa_ie) - 2))
+            memcpy(pmadapter, prsn_ie_tlv->rsn_ie, &pmpriv->wpa_ie[2],
+                   prsn_ie_tlv->header.len);
+        else {
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+
+        HEXDUMP("ADHOC_JOIN: RSN IE", (t_u8 *) prsn_ie_tlv,
+                sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len);
+        pos += sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len;
+        cmd_append_size +=
+            sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len;
+        prsn_ie_tlv->header.len = wlan_cpu_to_le16(prsn_ie_tlv->header.len);
+    } else if (pmpriv->sec_info.ewpa_enabled) {
+        prsn_ie_tlv = (MrvlIEtypes_RsnParamSet_t *) pos;
+        if (pbss_desc->pwpa_ie) {
+            prsn_ie_tlv->header.type =
+                (t_u16) (*(pbss_desc->pwpa_ie)).vend_hdr.element_id;
+            prsn_ie_tlv->header.type = prsn_ie_tlv->header.type & 0x00FF;
+            prsn_ie_tlv->header.type =
+                wlan_cpu_to_le16(prsn_ie_tlv->header.type);
+            prsn_ie_tlv->header.len =
+                (t_u16) (*(pbss_desc->pwpa_ie)).vend_hdr.len;
+            prsn_ie_tlv->header.len = prsn_ie_tlv->header.len & 0x00FF;
+            if (prsn_ie_tlv->header.len <= (sizeof(pmpriv->wpa_ie))) {
+                memcpy(pmadapter, prsn_ie_tlv->rsn_ie,
+                       &((*(pbss_desc->pwpa_ie)).vend_hdr.oui[0]),
+                       prsn_ie_tlv->header.len);
+            } else {
+                ret = MLAN_STATUS_FAILURE;
+                goto done;
+            }
+
+            HEXDUMP("ADHOC_JOIN: RSN IE", (t_u8 *) prsn_ie_tlv,
+                    sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len);
+            pos += sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len;
+            cmd_append_size +=
+                sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len;
+            prsn_ie_tlv->header.len = wlan_cpu_to_le16(prsn_ie_tlv->header.len);
+        }
+        if (pbss_desc->prsn_ie) {
+            prsn_ie_tlv = (MrvlIEtypes_RsnParamSet_t *) pos;
+            prsn_ie_tlv->header.type =
+                (t_u16) (*(pbss_desc->prsn_ie)).ieee_hdr.element_id;
+            prsn_ie_tlv->header.type = prsn_ie_tlv->header.type & 0x00FF;
+            prsn_ie_tlv->header.type =
+                wlan_cpu_to_le16(prsn_ie_tlv->header.type);
+            prsn_ie_tlv->header.len =
+                (t_u16) (*(pbss_desc->prsn_ie)).ieee_hdr.len;
+            prsn_ie_tlv->header.len = prsn_ie_tlv->header.len & 0x00FF;
+            if (prsn_ie_tlv->header.len <= (sizeof(pmpriv->wpa_ie))) {
+                memcpy(pmadapter, prsn_ie_tlv->rsn_ie,
+                       &((*(pbss_desc->prsn_ie)).data[0])
+                       , prsn_ie_tlv->header.len);
+            } else {
+                ret = MLAN_STATUS_FAILURE;
+                goto done;
+            }
+
+            HEXDUMP("ADHOC_JOIN: RSN IE", (t_u8 *) prsn_ie_tlv,
+                    sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len);
+            pos += sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len;
+            cmd_append_size +=
+                sizeof(prsn_ie_tlv->header) + prsn_ie_tlv->header.len;
+            prsn_ie_tlv->header.len = wlan_cpu_to_le16(prsn_ie_tlv->header.len);
+        }
+    }
+
+    if (ISSUPP_11NENABLED(pmadapter->fw_cap_info)
+        )
+        cmd_append_size += wlan_cmd_append_11n_tlv(pmpriv, pbss_desc, &pos);
+
+    cmd->size =
+        (t_u16) wlan_cpu_to_le16((t_u16) (sizeof(HostCmd_DS_802_11_AD_HOC_JOIN)
+                                          + S_DS_GEN + cmd_append_size));
+
+    memcpy(pmadapter, &tmp_cap, &padhoc_join->bss_descriptor.cap,
+           sizeof(IEEEtypes_CapInfo_t));
+    tmp_cap = wlan_cpu_to_le16(tmp_cap);
+
+    memcpy(pmadapter, &padhoc_join->bss_descriptor.cap,
+           &tmp_cap, sizeof(IEEEtypes_CapInfo_t));
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function handles the command response of ad_hoc_start and
+ *  		ad_hoc_join
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ret_802_11_ad_hoc(IN mlan_private * pmpriv,
+                       IN HostCmd_DS_COMMAND * resp, IN t_void * pioctl_buf)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ioctl_req *pioctl_req = (mlan_ioctl_req *) pioctl_buf;
+    HostCmd_DS_802_11_AD_HOC_START_RESULT *padhoc_start_result =
+        &resp->params.adhoc_start_result;
+    HostCmd_DS_802_11_AD_HOC_JOIN_RESULT *padhoc_join_result =
+        &resp->params.adhoc_join_result;
+    BSSDescriptor_t *pbss_desc;
+    t_u16 command = resp->command;
+    t_u8 result = 0;
+    t_u8 event_buf[100];
+    mlan_event *pevent = (mlan_event *) event_buf;
+
+    ENTER();
+
+    if (command == HostCmd_CMD_802_11_AD_HOC_START)
+        result = padhoc_start_result->result;
+    else
+        result = padhoc_join_result->result;
+
+    pbss_desc = pmpriv->pattempted_bss_desc;
+
+    /*
+     * Join result code 0 --> SUCCESS
+     */
+    if (result) {
+        PRINTM(MERROR, "ADHOC_RESP Failed 0x%x\n", result);
+        if (pmpriv->media_connected == MTRUE)
+            wlan_reset_connect_state(pmpriv, MTRUE);
+        if (pmpriv->adhoc_state == ADHOC_STARTING)
+            pmpriv->adhoc_state = ADHOC_IDLE;
+
+        memset(pmpriv->adapter, &pmpriv->curr_bss_params.bss_descriptor,
+               0x00, sizeof(BSSDescriptor_t));
+
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Send a Media Connected event, according to the Spec */
+    pmpriv->media_connected = MTRUE;
+
+    if (command == HostCmd_CMD_802_11_AD_HOC_START) {
+        PRINTM(MINFO, "ADHOC_S_RESP  %s\n", pbss_desc->ssid.ssid);
+
+        /* Update the created network descriptor with the new BSSID */
+        memcpy(pmpriv->adapter, pbss_desc->mac_address,
+               padhoc_start_result->bssid, MLAN_MAC_ADDR_LENGTH);
+
+        pmpriv->adhoc_state = ADHOC_STARTED;
+        if (pmpriv->adapter->state_rdh.stage == RDH_RESTART_INTFS)
+            wlan_11h_radar_detected_callback((t_void *) pmpriv);
+    } else {
+        /*
+         * Now the join cmd should be successful.
+         * If BSSID has changed use SSID to compare instead of BSSID
+         */
+        PRINTM(MINFO, "ADHOC_J_RESP  %s\n", pbss_desc->ssid.ssid);
+
+        /*
+         * Make a copy of current BSSID descriptor, only needed for join since
+         * the current descriptor is already being used for adhoc start
+         */
+        memcpy(pmpriv->adapter, &pmpriv->curr_bss_params.bss_descriptor,
+               pbss_desc, sizeof(BSSDescriptor_t));
+
+        pmpriv->adhoc_state = ADHOC_JOINED;
+    }
+
+    PRINTM(MINFO, "ADHOC_RESP: Channel = %d\n", pmpriv->adhoc_channel);
+    PRINTM(MINFO, "ADHOC_RESP: BSSID = %02x:%02x:%02x:%02x:%02x:%02x\n",
+           pmpriv->curr_bss_params.bss_descriptor.mac_address[0],
+           pmpriv->curr_bss_params.bss_descriptor.mac_address[1],
+           pmpriv->curr_bss_params.bss_descriptor.mac_address[2],
+           pmpriv->curr_bss_params.bss_descriptor.mac_address[3],
+           pmpriv->curr_bss_params.bss_descriptor.mac_address[4],
+           pmpriv->curr_bss_params.bss_descriptor.mac_address[5]);
+
+    pevent->bss_index = pmpriv->bss_index;
+    pevent->event_id = MLAN_EVENT_ID_DRV_CONNECTED;
+    pevent->event_len = MLAN_MAC_ADDR_LENGTH;
+    memcpy(pmpriv->adapter, (t_u8 *) pevent->event_buf,
+           (t_u8 *) pmpriv->curr_bss_params.bss_descriptor.mac_address,
+           MLAN_MAC_ADDR_LENGTH);
+    wlan_recv_event(pmpriv, MLAN_EVENT_ID_DRV_CONNECTED, pevent);
+    wlan_save_curr_bcn(pmpriv);
+
+  done:
+    /* Need to indicate IOCTL complete */
+    if (pioctl_req != MNULL) {
+        if (ret != MLAN_STATUS_SUCCESS) {
+            pioctl_req->status_code = MLAN_ERROR_CMD_ASSOC_FAIL;
+        } else {
+            pioctl_req->status_code = MLAN_ERROR_NO_ERROR;
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Associated to a specific BSS discovered in a scan
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pioctl_buf   A pointer to MLAN IOCTL Request buffer
+ *  @param pbss_desc     A pointer to the BSS descriptor to associate with.
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_associate(IN mlan_private * pmpriv,
+               IN t_void * pioctl_buf, IN BSSDescriptor_t * pbss_desc)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u8 current_bssid[MLAN_MAC_ADDR_LENGTH];
+    pmlan_ioctl_req pioctl_req = (mlan_ioctl_req *) pioctl_buf;
+
+    ENTER();
+
+    /* Return error if the pmadapter or table entry is not marked as infra */
+    if ((pmpriv->bss_mode != MLAN_BSS_MODE_INFRA) ||
+        (pbss_desc->bss_mode != MLAN_BSS_MODE_INFRA)) {
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    memcpy(pmpriv->adapter, &current_bssid,
+           &pmpriv->curr_bss_params.bss_descriptor.mac_address,
+           sizeof(current_bssid));
+
+    /* Clear any past association response stored for application retrieval */
+    pmpriv->assoc_rsp_size = 0;
+
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_ASSOCIATE,
+                           HostCmd_ACT_GEN_SET, 0, pioctl_buf, pbss_desc);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Start an Adhoc Network
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pioctl_buf   A pointer to MLAN IOCTL Request buffer
+ *  @param padhoc_ssid  The ssid of the Adhoc Network
+ *
+ *  @return             MLAN_STATUS_SUCCESS--success, MLAN_STATUS_FAILURE--fail
+ */
+mlan_status
+wlan_adhoc_start(IN mlan_private * pmpriv,
+                 IN t_void * pioctl_buf, IN mlan_802_11_ssid * padhoc_ssid)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    wlan_meas_state_t *pmeas_state = &pmpriv->adapter->state_meas;
+    t_u8 radar = MFALSE;
+    pmlan_ioctl_req pioctl_req = (mlan_ioctl_req *) pioctl_buf;
+
+    ENTER();
+
+    /*
+     * If the report indicates no measurement was done, leave the default
+     * return value alone.
+     */
+    if (!pmeas_state->meas_rpt_returned.rpt.basic.map.unmeasured) {
+        radar =
+            pmeas_state->meas_rpt_returned.rpt.basic.map.radar ? MTRUE : MFALSE;
+    }
+
+    if (radar) {
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        ret = MLAN_STATUS_FAILURE;
+        LEAVE();
+        return ret;
+    }
+
+    PRINTM(MINFO, "Adhoc Channel = %d\n", pmpriv->adhoc_channel);
+    PRINTM(MINFO, "curr_bss_params.channel = %d\n",
+           pmpriv->curr_bss_params.bss_descriptor.channel);
+    PRINTM(MINFO, "curr_bss_params.band = %d\n", pmpriv->curr_bss_params.band);
+
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_AD_HOC_START,
+                           HostCmd_ACT_GEN_SET, 0, pioctl_buf, padhoc_ssid);
+#if defined(STA_SUPPORT)
+    if (ret == MLAN_STATUS_SUCCESS)
+        memcpy(pmpriv->adapter, &pmpriv->adhoc_last_start_ssid,
+               padhoc_ssid, sizeof(mlan_802_11_ssid));
+#endif
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Join an adhoc network found in a previous scan
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pioctl_buf   A pointer to MLAN IOCTL Request buffer
+ *  @param pbss_desc     A pointer to the BSS descriptor found in a previous scan
+ *                      to attempt to join
+ *
+ *  @return             MLAN_STATUS_SUCCESS--success, MLAN_STATUS_FAILURE--fail
+ */
+mlan_status
+wlan_adhoc_join(IN mlan_private * pmpriv,
+                IN t_void * pioctl_buf, IN BSSDescriptor_t * pbss_desc)
+{
+    pmlan_adapter pmadapter = pmpriv->adapter;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_ioctl_req pioctl_req = (mlan_ioctl_req *) pioctl_buf;
+
+    ENTER();
+
+    PRINTM(MINFO, "wlan_adhoc_join: CurBss.ssid =%s\n",
+           pmpriv->curr_bss_params.bss_descriptor.ssid.ssid);
+    PRINTM(MINFO, "wlan_adhoc_join: CurBss.ssid_len =%u\n",
+           pmpriv->curr_bss_params.bss_descriptor.ssid.ssid_len);
+    PRINTM(MINFO, "wlan_adhoc_join: ssid =%s\n", pbss_desc->ssid.ssid);
+    PRINTM(MINFO, "wlan_adhoc_join: ssid len =%u\n", pbss_desc->ssid.ssid_len);
+
+    /* Check if the requested SSID is already joined */
+    if (pmpriv->curr_bss_params.bss_descriptor.ssid.ssid_len &&
+        !wlan_ssid_cmp(pmadapter, &pbss_desc->ssid,
+                       &pmpriv->curr_bss_params.bss_descriptor.ssid) &&
+        (pmpriv->curr_bss_params.bss_descriptor.bss_mode ==
+         MLAN_BSS_MODE_IBSS)) {
+
+        PRINTM(MINFO,
+               "ADHOC_J_CMD: New ad-hoc SSID is the same as current, "
+               "not attempting to re-join\n");
+
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    PRINTM(MINFO, "curr_bss_params.channel = %d\n",
+           pmpriv->curr_bss_params.bss_descriptor.channel);
+    PRINTM(MINFO, "curr_bss_params.band = %d\n", pmpriv->curr_bss_params.band);
+
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_AD_HOC_JOIN,
+                           HostCmd_ACT_GEN_SET, 0, pioctl_buf, pbss_desc);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Send Deauthentication Request or Stop the AdHoc network depending on mode
+ *
+ *  @param pmpriv    A pointer to mlan_private structure
+ *  @param pioctl_req A pointer to mlan_ioctl_req structure
+ *  @param mac       A pointer to mlan_802_11_mac_addr structure
+ *
+ *  @return          MLAN_STATUS_SUCCESS--success, MLAN_STATUS_FAILURE--fail, MLAN_STATUS_PENDING--pending
+ */
+mlan_status
+wlan_disconnect(IN mlan_private * pmpriv,
+                IN mlan_ioctl_req * pioctl_req, IN mlan_802_11_mac_addr * mac)
+{
+    mlan_802_11_mac_addr mac_address;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u8 zero_mac[] = { 0, 0, 0, 0, 0, 0 };
+
+    ENTER();
+
+    if (pmpriv->media_connected == MTRUE) {
+        if (pmpriv->bss_mode == MLAN_BSS_MODE_INFRA) {
+            if (mac) {
+                if (!memcmp(pmpriv->adapter, mac, zero_mac, sizeof(zero_mac)))
+                    memcpy(pmpriv->adapter, (t_u8 *) & mac_address,
+                           (t_u8 *) & pmpriv->curr_bss_params.bss_descriptor.
+                           mac_address, MLAN_MAC_ADDR_LENGTH);
+                else {
+                    memcpy(pmpriv->adapter, (t_u8 *) & mac_address,
+                           (t_u8 *) mac, MLAN_MAC_ADDR_LENGTH);
+                }
+            } else {
+                memcpy(pmpriv->adapter, (t_u8 *) & mac_address,
+                       (t_u8 *) & pmpriv->curr_bss_params.bss_descriptor.
+                       mac_address, MLAN_MAC_ADDR_LENGTH);
+            }
+
+            ret = wlan_prepare_cmd(pmpriv,
+                                   HostCmd_CMD_802_11_DEAUTHENTICATE,
+                                   HostCmd_ACT_GEN_SET,
+                                   0, (t_void *) pioctl_req, &mac_address);
+
+            if (ret == MLAN_STATUS_SUCCESS && pioctl_req)
+                ret = MLAN_STATUS_PENDING;
+
+        } else if (pmpriv->bss_mode == MLAN_BSS_MODE_IBSS) {
+            ret = wlan_prepare_cmd(pmpriv,
+                                   HostCmd_CMD_802_11_AD_HOC_STOP,
+                                   HostCmd_ACT_GEN_SET,
+                                   0, (t_void *) pioctl_req, MNULL);
+
+            if (ret == MLAN_STATUS_SUCCESS && pioctl_req)
+                ret = MLAN_STATUS_PENDING;
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Convert band to radio type used in channel TLV
+ *
+ *  @param band		Band enumeration to convert to a channel TLV radio type
+ *
+ *  @return		Radio type designator for use in a channel TLV
+ */
+t_u8
+wlan_band_to_radio_type(IN t_u8 band)
+{
+    t_u8 ret_radio_type;
+
+    ENTER();
+
+    switch (band) {
+    case BAND_A:
+    case BAND_AN:
+    case BAND_A | BAND_AN:
+        ret_radio_type = HostCmd_SCAN_RADIO_TYPE_A;
+        break;
+    case BAND_B:
+    case BAND_G:
+    case BAND_B | BAND_G:
+    default:
+        ret_radio_type = HostCmd_SCAN_RADIO_TYPE_BG;
+        break;
+    }
+
+    LEAVE();
+    return ret_radio_type;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_join.h b/drivers/net/wireless/sd8797/mlan/mlan_join.h
new file mode 100644
index 000000000000..d7091f3a4b28
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_join.h
@@ -0,0 +1,40 @@
+/** @file mlan_join.h
+ *
+ *  @brief This file defines the interface for the WLAN infrastructure
+ *  and adhoc join routines.
+ *
+ *  Driver interface functions and type declarations for the join module
+ *  implemented in mlan_join.c.  Process all start/join requests for
+ *  both adhoc and infrastructure networks
+ *    
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    10/13/2008: initial version
+******************************************************/
+
+#ifndef _MLAN_JOIN_H_
+#define _MLAN_JOIN_H_
+
+/** Size of buffer allocated to store the association response from firmware */
+#define MRVDRV_ASSOC_RSP_BUF_SIZE  500
+
+/** Size of buffer allocated to store IEs passed to firmware in the assoc req */
+#define MRVDRV_GENIE_BUF_SIZE      256
+
+#endif /* _MLAN_JOIN_H_ */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_main.h b/drivers/net/wireless/sd8797/mlan/mlan_main.h
new file mode 100644
index 000000000000..44e4f8001201
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_main.h
@@ -0,0 +1,2742 @@
+/** @file mlan_main.h
+ *
+ *  @brief This file defines the private and adapter data
+ *  structures and declares global function prototypes used
+ *  in MLAN module.
+ *
+ *  Copyright (C) 2008-2012, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    10/13/2008: initial version
+******************************************************/
+
+#ifndef _MLAN_MAIN_H_
+#define _MLAN_MAIN_H_
+
+#ifdef DEBUG_LEVEL1
+extern t_void(*print_callback) (IN t_void * pmoal_handle,
+                                IN t_u32 level, IN t_s8 * pformat, IN ...);
+
+extern mlan_status(*get_sys_time_callback) (IN t_void * pmoal_handle,
+                                            OUT t_u32 * psec,
+                                            OUT t_u32 * pusec);
+
+extern t_u32 drvdbg;
+
+#ifdef	DEBUG_LEVEL2
+#define	PRINTM_MINFO(msg...)  do {if ((drvdbg & MINFO) && (print_callback)) \
+                                    print_callback(MNULL, MINFO, msg);} while(0)
+#define	PRINTM_MWARN(msg...)  do {if ((drvdbg & MWARN) && (print_callback)) \
+                                    print_callback(MNULL, MWARN, msg);} while(0)
+#define	PRINTM_MENTRY(msg...) do {if ((drvdbg & MENTRY) && (print_callback)) \
+                                    print_callback(MNULL, MENTRY, msg);} while(0)
+#define PRINTM_GET_SYS_TIME(level, psec, pusec)             \
+do {                                                        \
+    if ((level & drvdbg) && (get_sys_time_callback))        \
+        get_sys_time_callback(MNULL, psec, pusec);          \
+} while (0)
+
+/** Hexdump for level-2 debugging */
+#define HEXDUMP(x,y,z)   \
+do {                \
+    if ((drvdbg & (MHEX_DUMP | MINFO)) && (print_callback))  \
+        print_callback(MNULL, MHEX_DUMP | MINFO, x, y, z); \
+} while (0)
+
+#else
+
+#define	PRINTM_MINFO(msg...)  do {} while (0)
+#define	PRINTM_MWARN(msg...)  do {} while (0)
+#define	PRINTM_MENTRY(msg...) do {} while (0)
+
+#define PRINTM_GET_SYS_TIME(level, psec, pusec)         \
+do {                                                    \
+    if ((level & drvdbg) && (get_sys_time_callback)     \
+            && (level != MINFO) && (level != MWARN))    \
+        get_sys_time_callback(MNULL, psec, pusec);      \
+} while (0)
+
+/** Hexdump for debugging */
+#define HEXDUMP(x,y,z) do {} while (0)
+
+#endif /* DEBUG_LEVEL2 */
+
+#define	PRINTM_MFW_D(msg...)  do {if ((drvdbg & MFW_D) && (print_callback)) \
+                                    print_callback(MNULL, MFW_D, msg);} while(0)
+#define	PRINTM_MCMD_D(msg...) do {if ((drvdbg & MCMD_D) && (print_callback)) \
+                                    print_callback(MNULL, MCMD_D, msg);} while(0)
+#define	PRINTM_MDAT_D(msg...) do {if ((drvdbg & MDAT_D) && (print_callback)) \
+                                    print_callback(MNULL, MDAT_D, msg);} while(0)
+#define	PRINTM_MIF_D(msg...) do {if ((drvdbg & MIF_D) && (print_callback)) \
+                                    print_callback(MNULL, MIF_D, msg);} while(0)
+
+#define	PRINTM_MIOCTL(msg...) do {if ((drvdbg & MIOCTL) && (print_callback)) \
+                                    print_callback(MNULL, MIOCTL, msg);} while(0)
+#define	PRINTM_MINTR(msg...)  do {if ((drvdbg & MINTR) && (print_callback)) \
+                                    print_callback(MNULL, MINTR, msg);} while(0)
+#define	PRINTM_MEVENT(msg...) do {if ((drvdbg & MEVENT) && (print_callback)) \
+                                    print_callback(MNULL, MEVENT, msg);} while(0)
+#define	PRINTM_MCMND(msg...)  do {if ((drvdbg & MCMND) && (print_callback)) \
+                                    print_callback(MNULL, MCMND, msg);} while(0)
+#define	PRINTM_MDATA(msg...)  do {if ((drvdbg & MDATA) && (print_callback)) \
+                                    print_callback(MNULL, MDATA, msg);} while(0)
+#define	PRINTM_MERROR(msg...) do {if ((drvdbg & MERROR) && (print_callback)) \
+                                    print_callback(MNULL, MERROR, msg);} while(0)
+#define	PRINTM_MFATAL(msg...) do {if ((drvdbg & MFATAL) && (print_callback)) \
+                                    print_callback(MNULL, MFATAL, msg);} while(0)
+#define	PRINTM_MMSG(msg...)   do {if ((drvdbg & MMSG) && (print_callback)) \
+                                    print_callback(MNULL, MMSG, msg);} while(0)
+
+#define	PRINTM(level,msg...) PRINTM_##level(msg)
+
+/** Log debug message */
+#ifdef __GNUC__
+#define PRINTM_NETINTF(level, pmpriv)   \
+do {                                    \
+    if ((drvdbg & level) && pmpriv      \
+            && pmpriv->adapter->callbacks.moal_print_netintf) \
+        pmpriv->adapter->callbacks.moal_print_netintf( \
+            pmpriv->adapter->pmoal_handle, \
+            pmpriv->bss_index, level); \
+} while (0)
+#endif /* __GNUC__ */
+
+/** Max hex dump data length */
+#define MAX_DATA_DUMP_LEN	64
+
+/** Debug hexdump for level-1 debugging */
+#define DBG_HEXDUMP(level,x,y,z)   \
+do {                \
+    if ((drvdbg & level) && print_callback)  \
+        print_callback(MNULL, MHEX_DUMP | level, x, y, z); \
+} while (0)
+
+#else /* DEBUG_LEVEL1 */
+
+#define	PRINTM(level,msg...) do {} while (0)
+
+#define PRINTM_NETINTF(level, pmpriv) do {} while (0)
+
+/** Debug hexdump for level-1 debugging */
+#define DBG_HEXDUMP(level,x,y,z) do {} while (0)
+
+/** Hexdump for debugging */
+#define HEXDUMP(x,y,z) do {} while (0)
+
+#define PRINTM_GET_SYS_TIME(level, psec, pusec) do { } while(0)
+
+#endif /* DEBUG_LEVEL1 */
+
+/** Log entry point for debugging */
+#define ENTER()     \
+do {                \
+        PRINTM(MENTRY, "Enter: %s\n", __FUNCTION__);   \
+} while (0)
+
+/** Log exit point for debugging */
+#define LEAVE()     \
+do {                \
+        PRINTM(MENTRY, "Leave: %s\n", __FUNCTION__);   \
+} while (0)
+
+/** Find minimum */
+#ifndef MIN
+#define MIN(a,b)		((a) < (b) ? (a) : (b))
+#endif
+
+/** Find maximum */
+#ifndef MAX
+#define MAX(a,b)		((a) > (b) ? (a) : (b))
+#endif
+
+#ifdef memset
+#undef memset
+#endif
+/** Memset routine */
+#define memset(adapter, s, c, len) \
+  adapter->callbacks.moal_memset(adapter->pmoal_handle, s, c, len)
+
+#ifdef memmove
+#undef memmove
+#endif
+/** Memmove routine */
+#define memmove(adapter, dest, src, len) \
+  adapter->callbacks.moal_memmove(adapter->pmoal_handle, dest, src, len)
+
+#ifdef memcpy
+#undef memcpy
+#endif
+/** Memcpy routine */
+#define memcpy(adapter, to, from, len) \
+  adapter->callbacks.moal_memcpy(adapter->pmoal_handle, to, from, len)
+
+#ifdef memcmp
+#undef memcmp
+#endif
+/** Memcmp routine */
+#define memcmp(adapter, s1, s2, len) \
+  adapter->callbacks.moal_memcmp(adapter->pmoal_handle, s1, s2, len)
+
+/** Find number of elements */
+#ifndef NELEMENTS
+#define NELEMENTS(x)    (sizeof(x)/sizeof(x[0]))
+#endif
+
+/** SWAP: swap t_u8 */
+#define SWAP_U8(a,b)	{t_u8 t; t=a; a=b; b=t;}
+
+/** SWAP: swap t_u8 */
+#define SWAP_U16(a,b)	{t_u16 t; t=a; a=b; b=t;}
+
+/** MLAN MNULL pointer */
+#define MNULL                           (0)
+
+/** 16 bits byte swap */
+#define swap_byte_16(x) \
+((t_u16)((((t_u16)(x) & 0x00ffU) << 8) | \
+         (((t_u16)(x) & 0xff00U) >> 8)))
+
+/** 32 bits byte swap */
+#define swap_byte_32(x) \
+((t_u32)((((t_u32)(x) & 0x000000ffUL) << 24) | \
+         (((t_u32)(x) & 0x0000ff00UL) <<  8) | \
+         (((t_u32)(x) & 0x00ff0000UL) >>  8) | \
+         (((t_u32)(x) & 0xff000000UL) >> 24)))
+
+/** 64 bits byte swap */
+#define swap_byte_64(x) \
+((t_u64)((t_u64)(((t_u64)(x) & 0x00000000000000ffULL) << 56) | \
+         (t_u64)(((t_u64)(x) & 0x000000000000ff00ULL) << 40) | \
+         (t_u64)(((t_u64)(x) & 0x0000000000ff0000ULL) << 24) | \
+         (t_u64)(((t_u64)(x) & 0x00000000ff000000ULL) <<  8) | \
+         (t_u64)(((t_u64)(x) & 0x000000ff00000000ULL) >>  8) | \
+         (t_u64)(((t_u64)(x) & 0x0000ff0000000000ULL) >> 24) | \
+         (t_u64)(((t_u64)(x) & 0x00ff000000000000ULL) >> 40) | \
+         (t_u64)(((t_u64)(x) & 0xff00000000000000ULL) >> 56) ))
+
+#ifdef BIG_ENDIAN_SUPPORT
+/** Convert ulong n/w to host */
+#define mlan_ntohl(x) x
+/** Convert host ulong to n/w */
+#define mlan_htonl(x) x
+/** Convert n/w to host */
+#define mlan_ntohs(x)  x
+/** Convert host to n/w */
+#define mlan_htons(x)  x
+/** Convert from 16 bit little endian format to CPU format */
+#define wlan_le16_to_cpu(x) swap_byte_16(x)
+/** Convert from 32 bit little endian format to CPU format */
+#define wlan_le32_to_cpu(x) swap_byte_32(x)
+/** Convert from 64 bit little endian format to CPU format */
+#define wlan_le64_to_cpu(x) swap_byte_64(x)
+/** Convert to 16 bit little endian format from CPU format */
+#define wlan_cpu_to_le16(x) swap_byte_16(x)
+/** Convert to 32 bit little endian format from CPU format */
+#define wlan_cpu_to_le32(x) swap_byte_32(x)
+/** Convert to 64 bit little endian format from CPU format */
+#define wlan_cpu_to_le64(x) swap_byte_64(x)
+
+/** Convert TxPD to little endian format from CPU format */
+#define endian_convert_TxPD(x)                                          \
+    {                                                                   \
+        (x)->tx_pkt_length = wlan_cpu_to_le16((x)->tx_pkt_length);      \
+        (x)->tx_pkt_offset = wlan_cpu_to_le16((x)->tx_pkt_offset);      \
+        (x)->tx_pkt_type   = wlan_cpu_to_le16((x)->tx_pkt_type);        \
+        (x)->tx_control    = wlan_cpu_to_le32((x)->tx_control);         \
+    }
+
+/** Convert RxPD from little endian format to CPU format */
+#define endian_convert_RxPD(x)                                          \
+    {                                                                   \
+        (x)->rx_pkt_length = wlan_le16_to_cpu((x)->rx_pkt_length);      \
+        (x)->rx_pkt_offset = wlan_le16_to_cpu((x)->rx_pkt_offset);      \
+        (x)->rx_pkt_type   = wlan_le16_to_cpu((x)->rx_pkt_type);        \
+        (x)->seq_num       = wlan_le16_to_cpu((x)->seq_num);            \
+    }
+#else
+/** Convert ulong n/w to host */
+#define mlan_ntohl(x) swap_byte_32(x)
+/** Convert host ulong to n/w */
+#define mlan_htonl(x) swap_byte_32(x)
+/** Convert n/w to host */
+#define mlan_ntohs(x) swap_byte_16(x)
+/** Convert host to n/w */
+#define mlan_htons(x) swap_byte_16(x)
+/** Do nothing */
+#define wlan_le16_to_cpu(x) x
+/** Do nothing */
+#define wlan_le32_to_cpu(x) x
+/** Do nothing */
+#define wlan_le64_to_cpu(x) x
+/** Do nothing */
+#define wlan_cpu_to_le16(x) x
+/** Do nothing */
+#define wlan_cpu_to_le32(x) x
+/** Do nothing */
+#define wlan_cpu_to_le64(x) x
+
+/** Convert TxPD to little endian format from CPU format */
+#define endian_convert_TxPD(x)  do {} while (0)
+/** Convert RxPD from little endian format to CPU format */
+#define endian_convert_RxPD(x)  do {} while (0)
+#endif /* BIG_ENDIAN_SUPPORT */
+
+/** Global moal_assert_callback */
+extern t_void(*assert_callback) (IN t_void * pmoal_handle, IN t_u32 cond);
+
+/** Assertion */
+#define MASSERT(cond)                   \
+do {                                    \
+    if (!(cond)) {                      \
+        PRINTM(MFATAL, "ASSERT: %s: %i\n", __FUNCTION__, __LINE__); \
+        if (assert_callback) {          \
+            assert_callback(MNULL, (t_ptr)(cond)); \
+        } else {                        \
+            do {} while(1);             \
+        }                               \
+    }                                   \
+} while(0)
+
+/** Upload size */
+#define WLAN_UPLD_SIZE                  (2312)
+
+/** Maximum event buffer size */
+#define MAX_EVENT_SIZE                  1024
+
+#ifdef STA_SUPPORT
+/** Maximum buffer size for ARP filter */
+#define ARP_FILTER_MAX_BUF_SIZE     	68
+#endif /* STA_SUPPORT */
+
+/** 60 seconds */
+#define MRVDRV_TIMER_60S                60000
+/** 10 seconds */
+#define MRVDRV_TIMER_10S                10000
+/** 5 seconds */
+#define MRVDRV_TIMER_5S                 5000
+/** 1 second */
+#define MRVDRV_TIMER_1S                 1000
+
+/** Maximum size of multicast list */
+#define MRVDRV_MAX_MULTICAST_LIST_SIZE  32
+/** Maximum size of channel */
+#define MRVDRV_MAX_CHANNEL_SIZE         14
+/** Maximum length of SSID */
+#define MRVDRV_MAX_SSID_LENGTH          32
+/** WEP list macros & data structures */
+/** Size of key buffer in bytes */
+#define MRVL_KEY_BUFFER_SIZE_IN_BYTE    16
+/** Maximum length of WPA key */
+#define MRVL_MAX_KEY_WPA_KEY_LENGTH     32
+
+/** Default listen interval */
+#define MLAN_DEFAULT_LISTEN_INTERVAL    10
+
+/** Maximum number of region codes */
+#define MRVDRV_MAX_REGION_CODE          9
+
+/** Maximum number of CFP codes for BG */
+#define MRVDRV_MAX_CFP_CODE_BG          0
+/** Maximum number of CFP codes for A */
+#define MRVDRV_MAX_CFP_CODE_A           5
+
+/** Default region code */
+#define MRVDRV_DEFAULT_REGION_CODE      0x10
+
+/** Default country code */
+#define MRVDRV_DEFAULT_COUNTRY_CODE      "US"
+
+/** Default factor for calculating beacon average */
+#define DEFAULT_BCN_AVG_FACTOR          8
+/** Default factor for calculating data average */
+#define DEFAULT_DATA_AVG_FACTOR         8
+
+/** The first valid channel for use */
+#define FIRST_VALID_CHANNEL          0xff
+/** Default Ad-Hoc channel */
+#define DEFAULT_AD_HOC_CHANNEL       6
+/** Default Ad-Hoc channel A */
+#define DEFAULT_AD_HOC_CHANNEL_A     36
+
+/** Number of WEP keys */
+#define MRVL_NUM_WEP_KEY                (4)
+
+/** Default multiple DTIM */
+#define MRVDRV_DEFAULT_MULTIPLE_DTIM        1
+
+/** Default beacon missing timeout */
+#define DEFAULT_BCN_MISS_TIMEOUT            10
+
+/** Maximum buffer space for beacons retrieved from scan responses */
+#define MAX_SCAN_BEACON_BUFFER          16384
+/** Default buffer space for beacons retrieved from scan responses */
+#define DEFAULT_SCAN_BEACON_BUFFER      4096
+
+/**
+ * @brief Buffer pad space for newly allocated beacons/probe responses
+ *
+ * Beacons are typically 6 bytes longer than an equivalent probe response.
+ *  For each scan response stored, allocate an extra byte pad at the end to
+ *  allow easy expansion to store a beacon in the same memory a probe response
+ *  previously contained
+ */
+#define SCAN_BEACON_ENTRY_PAD          6
+
+/** Scan time specified in the channel TLV for each channel for passive scans */
+#define MRVDRV_PASSIVE_SCAN_CHAN_TIME       200
+
+/** Scan time specified in the channel TLV for each channel for active scans */
+#define MRVDRV_ACTIVE_SCAN_CHAN_TIME        200
+
+/** Scan time specified in the channel TLV for each channel for specific scans */
+#define MRVDRV_SPECIFIC_SCAN_CHAN_TIME      110
+
+/**
+ * Max total scan time in milliseconds
+ * The total scan time should be less than scan command timeout value (10s)
+ */
+#define MRVDRV_MAX_TOTAL_SCAN_TIME     (MRVDRV_TIMER_10S - MRVDRV_TIMER_1S)
+
+/** Offset for GTK as it has version to skip past for GTK */
+#define RSN_GTK_OUI_OFFSET 2
+
+/** If OUI is not found */
+#define MLAN_OUI_NOT_PRESENT 0
+/** If OUI is found */
+#define MLAN_OUI_PRESENT 1
+
+/** RF antenna selection */
+#define RF_ANTENNA_MASK(n)	((1<<(n))-1)
+/** RF antenna auto select */
+#define RF_ANTENNA_AUTO		0xFFFF
+
+/** Is cmd_resp, event or data packet received? */
+#define IS_CARD_RX_RCVD(adapter) (adapter->cmd_resp_received || \
+                                  adapter->event_received || \
+                                  adapter->data_received)
+
+/** Type command */
+#define MLAN_TYPE_CMD			1
+/** Type data */
+#define MLAN_TYPE_DATA			0
+/** Type event */
+#define MLAN_TYPE_EVENT			3
+
+/** Maximum numbfer of registers to read for multiple port */
+#define MAX_MP_REGS			64
+/** Maximum port */
+#define MAX_PORT			16
+/** Multi port aggregation packet limit */
+#define SDIO_MP_AGGR_DEF_PKT_LIMIT       (8)
+
+#ifdef SDIO_MULTI_PORT_TX_AGGR
+/** Multi port TX aggregation buffer size */
+#define SDIO_MP_TX_AGGR_DEF_BUF_SIZE        (16384)     /* 16K */
+#endif /* SDIO_MULTI_PORT_TX_AGGR */
+
+#ifdef SDIO_MULTI_PORT_RX_AGGR
+/** Multi port RX aggregation buffer size */
+#define SDIO_MP_RX_AGGR_DEF_BUF_SIZE        (16384)     /* 16K */
+#endif /* SDIO_MULTI_PORT_RX_AGGR */
+
+/** High threshold at which to start drop packets */
+#define  RX_HIGH_THRESHOLD           1024
+/** Medium threshold at which to disable Rx BA */
+#define  RX_MED_THRESHOLD            256
+/** Low threshold to allow Rx BA */
+#define  RX_LOW_THRESHOLD            128
+
+/** Debug command number */
+#define DBG_CMD_NUM	5
+
+/** Info for debug purpose */
+typedef struct _wlan_dbg
+{
+    /** Number of host to card command failures */
+    t_u32 num_cmd_host_to_card_failure;
+    /** Number of host to card sleep confirm failures */
+    t_u32 num_cmd_sleep_cfm_host_to_card_failure;
+    /** Number of host to card Tx failures */
+    t_u32 num_tx_host_to_card_failure;
+    /** Number of card to host command/event failures */
+    t_u32 num_cmdevt_card_to_host_failure;
+    /** Number of card to host Rx failures */
+    t_u32 num_rx_card_to_host_failure;
+    /** Number of interrupt read failures */
+    t_u32 num_int_read_failure;
+    /** Last interrupt status */
+    t_u32 last_int_status;
+    /** Number of deauthentication events */
+    t_u32 num_event_deauth;
+    /** Number of disassosiation events */
+    t_u32 num_event_disassoc;
+    /** Number of link lost events */
+    t_u32 num_event_link_lost;
+    /** Number of deauthentication commands */
+    t_u32 num_cmd_deauth;
+    /** Number of association comamnd successes */
+    t_u32 num_cmd_assoc_success;
+    /** Number of association command failures */
+    t_u32 num_cmd_assoc_failure;
+    /** Number of Tx timeouts */
+    t_u32 num_tx_timeout;
+    /** Number of command timeouts */
+    t_u32 num_cmd_timeout;
+    /** Timeout command ID */
+    t_u16 timeout_cmd_id;
+    /** Timeout command action */
+    t_u16 timeout_cmd_act;
+    /** List of last command IDs */
+    t_u16 last_cmd_id[DBG_CMD_NUM];
+    /** List of last command actions */
+    t_u16 last_cmd_act[DBG_CMD_NUM];
+    /** Last command index */
+    t_u16 last_cmd_index;
+    /** List of last command response IDs */
+    t_u16 last_cmd_resp_id[DBG_CMD_NUM];
+    /** Last command response index */
+    t_u16 last_cmd_resp_index;
+    /** List of last events */
+    t_u16 last_event[DBG_CMD_NUM];
+    /** Last event index */
+    t_u16 last_event_index;
+} wlan_dbg;
+
+/** Hardware status codes */
+typedef enum _WLAN_HARDWARE_STATUS
+{
+    WlanHardwareStatusReady,
+    WlanHardwareStatusInitializing,
+    WlanHardwareStatusInitdone,
+    WlanHardwareStatusReset,
+    WlanHardwareStatusClosing,
+    WlanHardwareStatusNotReady
+} WLAN_HARDWARE_STATUS;
+
+/** WLAN_802_11_POWER_MODE */
+typedef enum _WLAN_802_11_POWER_MODE
+{
+    Wlan802_11PowerModeCAM,
+    Wlan802_11PowerModePSP
+} WLAN_802_11_POWER_MODE;
+
+/** tx param */
+typedef struct _mlan_tx_param
+{
+    /** next packet length */
+    t_u32 next_pkt_len;
+} mlan_tx_param;
+
+/** PS_STATE */
+typedef enum _PS_STATE
+{
+    PS_STATE_AWAKE,
+    PS_STATE_PRE_SLEEP,
+    PS_STATE_SLEEP_CFM,
+    PS_STATE_SLEEP
+} PS_STATE;
+
+/** Minimum flush timer for win size of 1 is 50 ms */
+#define MIN_FLUSH_TIMER_MS 50
+/** Tx BA stream table */
+typedef struct _TxBAStreamTbl TxBAStreamTbl;
+
+/** Add BA parameter data structure */
+typedef struct
+{
+    /** Window size for initiator */
+    t_u32 tx_win_size;
+    /** Window size for receiver */
+    t_u32 rx_win_size;
+    /** Block ack timeout */
+    t_u32 timeout;
+    /** amsdu support for ADDBA request */
+    t_u8 tx_amsdu;
+    /** amsdu support for ADDBA response */
+    t_u8 rx_amsdu;
+} add_ba_param_t;
+
+/** Tx aggregation data structure */
+typedef struct _txAggr_t
+{
+    /** AMPDU user */
+    t_u8 ampdu_user;
+    /** AMPDU AP */
+    t_u8 ampdu_ap;
+    /** AMSDU */
+    t_u8 amsdu;
+} tx_aggr_t;
+
+/** RA list table */
+typedef struct _raListTbl raListTbl;
+
+/** RA list table */
+struct _raListTbl
+{
+    /** Pointer to previous node */
+    raListTbl *pprev;
+    /** Pointer to next node */
+    raListTbl *pnext;
+    /** Buffer list head */
+    mlan_list_head buf_head;
+    /** RA list buffer */
+    t_u8 ra[MLAN_MAC_ADDR_LENGTH];
+    /** total packets in RA list */
+    t_u16 total_pkts;
+        /** packets received */
+    t_u16 packet_count;
+        /** packet count threshold to setup BA */
+    t_u8 ba_packet_threshold;
+        /** is 11n enabled */
+    t_u8 is_11n_enabled;
+        /** max amsdu size */
+    t_u16 max_amsdu;
+        /** tx_pause flag */
+    t_u8 tx_pause;
+};
+
+/** TID table */
+typedef struct _tidTbl
+{
+    /** RA list head */
+    mlan_list_head ra_list;
+    /** Current RA list */
+    raListTbl *ra_list_curr;
+} tid_tbl_t;
+
+/** Highest priority setting for a packet (uses voice AC) */
+#define WMM_HIGHEST_PRIORITY  7
+/** Highest priority TID  */
+#define HIGH_PRIO_TID   7
+/** Lowest priority TID  */
+#define LOW_PRIO_TID    0
+/** No packet priority (< lowest) */
+#define NO_PKT_PRIO_TID    -1
+
+/** Struct of WMM DESC */
+typedef struct _wmm_desc
+{
+    /** TID table */
+    tid_tbl_t tid_tbl_ptr[MAX_NUM_TID];
+    /** Packets out */
+    t_u32 packets_out[MAX_NUM_TID];
+    /** Packets queued */
+    t_u32 pkts_queued[MAX_NUM_TID];
+    /** Spin lock to protect ra_list */
+    t_void *ra_list_spinlock;
+
+    /** AC status */
+    WmmAcStatus_t ac_status[MAX_AC_QUEUES];
+    /** AC downgraded values */
+    mlan_wmm_ac_e ac_down_graded_vals[MAX_AC_QUEUES];
+
+    /** Max driver packet delay sent to the firmware for expiry eval */
+    t_u32 drv_pkt_delay_max;
+
+    /** WMM queue priority table */
+    t_u8 queue_priority[MAX_AC_QUEUES];
+    /** User priority packet transmission control */
+    t_u32 user_pri_pkt_tx_ctrl[WMM_HIGHEST_PRIORITY + 1];       /* UP: 0 to 7 */
+
+    /** Number of transmit packets queued */
+    mlan_scalar tx_pkts_queued;
+    /** Tracks highest priority with a packet queued */
+    mlan_scalar highest_queued_prio;
+} wmm_desc_t;
+
+/** Security structure */
+typedef struct _wlan_802_11_security_t
+{
+    /** WPA enabled flag */
+    t_u8 wpa_enabled;
+    /** E-Supplicant enabled flag */
+    t_u8 ewpa_enabled;
+    /** WPA2 enabled flag */
+    t_u8 wpa2_enabled;
+    /** WAPI enabled flag */
+    t_u8 wapi_enabled;
+    /** WAPI key on flag */
+    t_u8 wapi_key_on;
+    /** WEP status */
+    WLAN_802_11_WEP_STATUS wep_status;
+    /** Authentication mode */
+    t_u32 authentication_mode;
+    /** Encryption mode */
+    t_u32 encryption_mode;
+} wlan_802_11_security_t;
+
+/** Current Basic Service Set State Structure */
+typedef struct
+{
+    /** BSS descriptor */
+    BSSDescriptor_t bss_descriptor;
+    /** WMM enable? */
+    t_u8 wmm_enabled;
+    /** Uapsd enable?*/
+    t_u8 wmm_uapsd_enabled;
+    /** Band */
+    t_u8 band;
+    /** Number of rates supported */
+    t_u32 num_of_rates;
+    /** Supported rates*/
+    t_u8 data_rates[WLAN_SUPPORTED_RATES];
+} current_bss_params_t;
+
+/** Sleep_params */
+typedef struct _sleep_params_t
+{
+    /** Sleep parameter error */
+    t_u16 sp_error;
+    /** Sleep parameter offset */
+    t_u16 sp_offset;
+    /** Sleep parameter stable time */
+    t_u16 sp_stable_time;
+    /** Sleep parameter calibration control */
+    t_u8 sp_cal_control;
+    /** Sleep parameter external sleep clock */
+    t_u8 sp_ext_sleep_clk;
+    /** Sleep parameter reserved */
+    t_u16 sp_reserved;
+} sleep_params_t;
+
+/** Sleep_period */
+typedef struct sleep_period_t
+{
+    /** Sleep period */
+    t_u16 period;
+    /** Reserved */
+    t_u16 reserved;
+} sleep_period_t;
+
+/** mrvl_wep_key_t */
+typedef struct _mrvl_wep_key_t
+{
+    /** Length */
+    t_u32 length;
+    /** WEP key index */
+    t_u32 key_index;
+    /** WEP key length */
+    t_u32 key_length;
+    /** WEP keys */
+    t_u8 key_material[MRVL_KEY_BUFFER_SIZE_IN_BYTE];
+} mrvl_wep_key_t;
+
+/** Maximum number of region channel */
+#define MAX_REGION_CHANNEL_NUM  2
+
+/** Chan-Freq-TxPower mapping table*/
+typedef struct _chan_freq_power_t
+{
+    /** Channel Number */
+    t_u16 channel;
+    /** Frequency of this Channel */
+    t_u32 freq;
+    /** Max allowed Tx power level */
+    t_u16 max_tx_power;
+    /** TRUE:radar detect required for BAND A or passive scan for BAND B/G;
+      * FALSE:radar detect not required for BAND A or active scan for BAND B/G*/
+    t_bool passive_scan_or_radar_detect;
+    /** TRUE:channel unsupported;  FALSE:supported */
+    t_u8 unsupported;
+} chan_freq_power_t;
+
+/** Region-band mapping table */
+typedef struct _region_chan_t
+{
+    /** TRUE if this entry is valid */
+    t_u8 valid;
+    /** Region code for US, Japan ... */
+    t_u8 region;
+    /** Band B/G/A, used for BAND_CONFIG cmd */
+    t_u8 band;
+    /** Actual No. of elements in the array below */
+    t_u8 num_cfp;
+    /** chan-freq-txpower mapping table */
+    chan_freq_power_t *pcfp;
+} region_chan_t;
+
+/** State of 11d */
+typedef enum _state_11d_t
+{
+    DISABLE_11D = 0,
+    ENABLE_11D = 1,
+} state_11d_t;
+
+#define DEFAULT_11D_STATE   DISABLE_11D
+
+/** Domain regulatory information */
+typedef struct _wlan_802_11d_domain_reg
+{
+    /** Country Code */
+    t_u8 country_code[COUNTRY_CODE_LEN];
+    /** band that channels in sub_band belong to */
+    t_u8 band;
+    /** No. of subband in below */
+    t_u8 no_of_sub_band;
+    /** Subband data to send/last sent */
+    IEEEtypes_SubbandSet_t sub_band[MRVDRV_MAX_SUBBAND_802_11D];
+} wlan_802_11d_domain_reg_t;
+
+/** Data for state machine */
+typedef struct _wlan_802_11d_state
+{
+    /** True for enabling 11D */
+    state_11d_t enable_11d;
+    /** True for user enabling 11D */
+    state_11d_t user_enable_11d;
+} wlan_802_11d_state_t;
+
+/** 802.11h State information kept in the 'mlan_private' driver structure */
+typedef struct
+{
+    /** Indicates whether 11h is enabled in the driver */
+    t_bool is_11h_enabled;
+    /** Indicates whether 11h is active in the firmware */
+    t_bool is_11h_active;
+    /** Master device using automatic channel select */
+    t_bool adhoc_auto_sel_chan;
+    /** Set when driver receives a STOP TX event from fw */
+    t_bool tx_disabled;
+} wlan_11h_interface_state_t;
+
+#if defined(UAP_SUPPORT)
+/** UAP get info callback state kept in the 'mlan_private' driver structure */
+typedef struct
+{
+    /** UAP internal callback after wlan_uap_get_channel */
+    /**  (parameter is really pointer to mlan_private)   */
+    mlan_status(*get_chan_callback) (t_void *);
+    /** current ioctl_req (to be completed in callback) */
+    pmlan_ioctl_req pioctl_req_curr;
+    /** band_cfg from MrvlIEtypes_channel_band_t */
+    t_u8 band_config;
+    /** channel from MrvlIEtypes_channel_band_t */
+    t_u8 channel;
+    /** beacon period (in msec) from MrvlIEtypes_beacon_period_t */
+    t_u16 beacon_period;
+    /** dtim period (no unit) from MrvlIEtypes_dtim_period_t */
+    t_u8 dtim_period;
+} wlan_uap_get_info_cb_t;
+#endif
+
+/** Data structure for WPS information */
+typedef struct
+{
+    /** WPS IE */
+    IEEEtypes_VendorSpecific_t wps_ie;
+    /** Session enable flag */
+    t_u8 session_enable;
+} wps_t;
+
+/** mlan_operations data structure */
+typedef struct _mlan_operations
+{
+    /** cmd init handler */
+    mlan_status(*init_cmd) (IN t_void * priv, IN t_u8 first_bss);
+    /** ioctl handler */
+    mlan_status(*ioctl) (t_void * adapter, pmlan_ioctl_req pioctl_req);
+    /** cmd handler */
+    mlan_status(*prepare_cmd) (IN t_void * priv,
+                               IN t_u16 cmd_no,
+                               IN t_u16 cmd_action,
+                               IN t_u32 cmd_oid,
+                               IN t_void * pioctl_buf,
+                               IN t_void * pdata_buf, IN t_void * pcmd_buf);
+    /** cmdresp handler */
+    mlan_status(*process_cmdresp) (IN t_void * priv,
+                                   IN t_u16 cmdresp_no,
+                                   IN t_void * pcmd_buf, IN t_void * pioctl);
+    /** rx handler */
+    mlan_status(*process_rx_packet) (IN t_void * adapter,
+                                     IN pmlan_buffer pmbuf);
+    /** event handler */
+    mlan_status(*process_event) (IN t_void * priv);
+    /** txpd handler */
+    t_void *(*process_txpd) (IN t_void * priv, IN pmlan_buffer pmbuf);
+    /** BSS role */
+    mlan_bss_role bss_role;
+} mlan_operations;
+
+/** Private structure for MLAN */
+typedef struct _mlan_private
+{
+    /** Pointer to mlan_adapter */
+    struct _mlan_adapter *adapter;
+    /** BSS index */
+    t_u8 bss_index;
+    /** BSS type */
+    t_u8 bss_type;
+    /** BSS role */
+    t_u8 bss_role;
+    /** BSS Priority */
+    t_u8 bss_priority;
+    /** BSS number */
+    t_u8 bss_num;
+    /** Frame type */
+    t_u8 frame_type;
+    /** MAC address information */
+    t_u8 curr_addr[MLAN_MAC_ADDR_LENGTH];
+    /** Media connection status */
+    t_bool media_connected;
+
+    /** Current packet filter */
+    t_u16 curr_pkt_filter;
+    /** Infrastructure mode */
+    t_u32 bss_mode;
+
+    /** Tx packet control */
+    t_u32 pkt_tx_ctrl;
+
+    /** Tx power level */
+    t_u16 tx_power_level;
+    /** Maximum Tx power level */
+    t_u8 max_tx_power_level;
+    /** Minimum Tx power level */
+    t_u8 min_tx_power_level;
+    /** Tx rate */
+    t_u8 tx_rate;
+    /** tx ht_info */
+    t_u8 tx_htinfo;
+    /** rxpd_htinfo */
+    t_u8 rxpd_htinfo;
+    /** max amsdu size */
+    t_u16 max_amsdu;
+#ifdef UAP_SUPPORT
+    /** UAP 11n flag */
+    t_u8 is_11n_enabled;
+#endif                          /* UAP_SUPPORT */
+#ifdef UAP_SUPPORT
+#endif                          /* UAP_SUPPORT */
+#ifdef UAP_SUPPORT
+    /** packet forward control */
+    t_u8 pkt_fwd;
+    /**  dropped pkts */
+    t_u32 num_drop_pkts;
+#endif
+    /** TX beamforming capability */
+    t_u32 tx_bf_cap;
+    /** Rx PD rate */
+    t_u8 rxpd_rate;
+    /** Rate bitmap */
+    t_u16 rate_bitmap;
+    /** Bitmap rates */
+    t_u16 bitmap_rates[MAX_BITMAP_RATES_SIZE];
+    /** Data rate */
+    t_u32 data_rate;
+    /** Automatic data rate flag */
+    t_u8 is_data_rate_auto;
+    /** Factor for calculating beacon average */
+    t_u16 bcn_avg_factor;
+    /** Factor for calculating data average */
+    t_u16 data_avg_factor;
+    /** Last data RSSI */
+    t_s16 data_rssi_last;
+    /** Last data Noise Floor */
+    t_s16 data_nf_last;
+    /** Average data RSSI */
+    t_s16 data_rssi_avg;
+    /** Averag data Noise Floor */
+    t_s16 data_nf_avg;
+    /** Last beacon RSSI */
+    t_s16 bcn_rssi_last;
+    /** Last beacon Noise Floor */
+    t_s16 bcn_nf_last;
+    /** Average beacon RSSI */
+    t_s16 bcn_rssi_avg;
+    /** Average beacon Noise Floor */
+    t_s16 bcn_nf_avg;
+
+    /** Attempted BSS descriptor */
+    BSSDescriptor_t *pattempted_bss_desc;
+
+    /** Current SSID/BSSID related parameters*/
+    current_bss_params_t curr_bss_params;
+
+    /** User selected bands */
+    t_u8 config_bands;
+
+    /** Beacon period */
+    t_u16 beacon_period;
+    /** Listen interval */
+    t_u16 listen_interval;
+    /** ATIM window */
+    t_u16 atim_window;
+
+    /** AdHoc channel */
+    t_u8 adhoc_channel;
+    /** AdHoc link sensed flag */
+    t_u8 adhoc_is_link_sensed;
+    /** AdHoc operating state */
+    t_u8 adhoc_state;
+#if defined(STA_SUPPORT)
+    /** AdHoc operating state backup */
+    t_u8 adhoc_state_prev;
+    /** AdHoc previous ssid used for Start */
+    mlan_802_11_ssid adhoc_last_start_ssid;
+#endif
+    /** FSM variable for 11d support */
+    wlan_802_11d_state_t state_11d;
+    /** FSM variable for 11h support */
+    wlan_11h_interface_state_t intf_state_11h;
+#if defined(UAP_SUPPORT)
+    /** Whether UAP interface has started */
+    t_bool uap_bss_started;
+    /** state variable for UAP Get Info callback */
+    wlan_uap_get_info_cb_t uap_state_chan_cb;
+#endif
+
+    /** Security related */
+    /** Encryption parameter */
+    wlan_802_11_security_t sec_info;
+    /** WEP keys */
+    mrvl_wep_key_t wep_key[MRVL_NUM_WEP_KEY];
+    /** Current WEP key index */
+    t_u16 wep_key_curr_index;
+    /** EWPA query 0: disable, 1: enable */
+    t_u8 ewpa_query;
+    /** Encryption Key*/
+    t_u8 wpa_ie[256];
+    /** WPA IE length */
+    t_u8 wpa_ie_len;
+    /** GTK set flag */
+    t_u8 wpa_is_gtk_set;
+    /** AES key material */
+    HostCmd_DS_802_11_KEY_MATERIAL aes_key;
+    /** WAPI IE */
+    t_u8 wapi_ie[256];
+    /** WAPI IE length */
+    t_u8 wapi_ie_len;
+    /** Pointer to the station table */
+    mlan_list_head sta_list;
+
+    /** MGMT IE */
+    custom_ie mgmt_ie[MAX_MGMT_IE_INDEX];
+    /** mgmt frame passthru mask */
+    t_u32 mgmt_frame_passthru_mask;
+    /** Advanced Encryption Standard */
+    t_u8 adhoc_aes_enabled;
+    /** WMM required */
+    t_u8 wmm_required;
+    /** WMM enabled */
+    t_u8 wmm_enabled;
+    /** WMM qos info */
+    t_u8 wmm_qosinfo;
+    /** WMM related variable*/
+    wmm_desc_t wmm;
+
+    /** Pointer to the Transmit BA stream table*/
+    mlan_list_head tx_ba_stream_tbl_ptr;
+    /** Pointer to the priorities for AMSDU/AMPDU table*/
+    tx_aggr_t aggr_prio_tbl[MAX_NUM_TID];
+    /** Pointer to the priorities for AMSDU/AMPDU table*/
+    t_u8 addba_reject[MAX_NUM_TID];
+    /** Struct to store ADDBA parameters */
+    add_ba_param_t add_ba_param;
+    /** last rx_seq */
+    t_u16 rx_seq[MAX_NUM_TID];
+    /** Pointer to the Receive Reordering table*/
+    mlan_list_head rx_reorder_tbl_ptr;
+    /** Lock for Rx packets */
+    t_void *rx_pkt_lock;
+
+#ifdef STA_SUPPORT
+    /** Buffer to store the association response for application retrieval */
+    t_u8 assoc_rsp_buf[MRVDRV_ASSOC_RSP_BUF_SIZE];
+    /** Length of the data stored in assoc_rsp_buf */
+    t_u32 assoc_rsp_size;
+
+    /** Generic IEEE IEs passed from the application to be inserted into the
+     *    association request to firmware
+     */
+    t_u8 gen_ie_buf[MRVDRV_GENIE_BUF_SIZE];
+    /** Length of the data stored in gen_ie_buf */
+    t_u8 gen_ie_buf_len;
+
+    t_u8 *pcurr_bcn_buf;
+    t_u32 curr_bcn_size;
+    t_void *curr_bcn_buf_lock;
+
+    /** WPS */
+    wps_t wps;
+#endif                          /* STA_SUPPORT */
+
+    /** function table */
+    mlan_operations ops;
+
+    /** Port Control mode */
+    t_u8 port_ctrl_mode;
+
+    /** Port open flag */
+    t_u8 port_open;
+
+    /** Port open flag state at time of association attempt */
+    t_u8 prior_port_status;
+
+    /** Scan block flag */
+    t_u8 scan_block;
+    /** IP address operation */
+    t_u32 op_code;
+    /** IP address */
+    t_u8 ip_addr[IPADDR_LEN];
+} mlan_private, *pmlan_private;
+
+/** BA stream status */
+typedef enum _baStatus_e
+{
+    BA_STREAM_NOT_SETUP = 0,
+    BA_STREAM_SETUP_INPROGRESS,
+    BA_STREAM_SETUP_COMPLETE
+} baStatus_e;
+
+/** Tx BA stream table */
+struct _TxBAStreamTbl
+{
+    /** TxBAStreamTbl previous node */
+    TxBAStreamTbl *pprev;
+    /** TxBAStreamTbl next node */
+    TxBAStreamTbl *pnext;
+    /** TID */
+    int tid;
+    /** RA */
+    t_u8 ra[MLAN_MAC_ADDR_LENGTH];
+    /** BA stream status */
+    baStatus_e ba_status;
+    t_u8 amsdu;
+};
+
+/** RX reorder table */
+typedef struct _RxReorderTbl RxReorderTbl;
+
+typedef struct
+{
+    /** Timer for flushing */
+    t_void *timer;
+    /** Timer set flag */
+    t_u8 timer_is_set;
+    /** RxReorderTbl ptr */
+    RxReorderTbl *ptr;
+    /** Priv pointer */
+    mlan_private *priv;
+} reorder_tmr_cnxt_t;
+
+/** RX reorder table */
+struct _RxReorderTbl
+{
+    /** RxReorderTbl previous node */
+    RxReorderTbl *pprev;
+    /** RxReorderTbl next node */
+    RxReorderTbl *pnext;
+    /** TID */
+    int tid;
+    /** TA */
+    t_u8 ta[MLAN_MAC_ADDR_LENGTH];
+    /** Start window */
+    int start_win;
+    /** Window size */
+    int win_size;
+    /** Pointer to pointer to RxReorderTbl */
+    t_void **rx_reorder_ptr;
+    /** Timer context */
+    reorder_tmr_cnxt_t timer_context;
+    /** BA stream status */
+    baStatus_e ba_status;
+    t_u8 amsdu;
+};
+
+/** BSS priority node */
+typedef struct _mlan_bssprio_node mlan_bssprio_node;
+
+/** BSS priority node */
+struct _mlan_bssprio_node
+{
+    /** Pointer to previous node */
+    mlan_bssprio_node *pprev;
+    /** Pointer to next node */
+    mlan_bssprio_node *pnext;
+    /** Pointer to priv */
+    pmlan_private priv;
+};
+
+/** BSS priority table */
+typedef struct _mlan_bssprio_tbl mlan_bssprio_tbl;
+
+/** BSS priority table */
+struct _mlan_bssprio_tbl
+{
+    /** BSS priority list head */
+    mlan_list_head bssprio_head;
+    /** Current priority node */
+    mlan_bssprio_node *bssprio_cur;
+};
+
+/** cmd_ctrl_node */
+typedef struct _cmd_ctrl_node cmd_ctrl_node;
+
+/** _cmd_ctrl_node */
+struct _cmd_ctrl_node
+{
+    /** Pointer to previous node */
+    cmd_ctrl_node *pprev;
+    /** Pointer to next node */
+    cmd_ctrl_node *pnext;
+    /** Pointer to priv */
+    pmlan_private priv;
+    /** Command OID for sub-command use */
+    t_u32 cmd_oid;
+    /** Command flag */
+    t_u32 cmd_flag;
+    /** Pointer to mlan_buffer */
+    mlan_buffer *cmdbuf;
+    /** Pointer to mlan_buffer */
+    mlan_buffer *respbuf;
+    /** Command parameter */
+    t_void *pdata_buf;
+    /** Pointer to mlan_ioctl_req if command is from IOCTL */
+    t_void *pioctl_buf;
+    /** pre_allocated mlan_buffer for cmd */
+    mlan_buffer *pmbuf;
+};
+
+/** station node */
+typedef struct _sta_node sta_node;
+
+/** station node*/
+struct _sta_node
+{
+    /** previous node */
+    sta_node *pprev;
+    /** next node */
+    sta_node *pnext;
+    /** station mac address */
+    t_u8 mac_addr[MLAN_MAC_ADDR_LENGTH];
+    /** 11n flag */
+    t_u8 is_11n_enabled;
+    /** AMPDU STA */
+    t_u8 ampdu_sta[MAX_NUM_TID];
+    /** last rx_seq */
+    t_u16 rx_seq[MAX_NUM_TID];
+    /** max amsdu size */
+    t_u16 max_amsdu;
+    /** wapi key on off flag */
+    t_u8 wapi_key_on;
+    /** tx pause status */
+    t_u8 tx_pause;
+};
+
+/** 802.11h State information kept in the 'mlan_adapter' driver structure */
+typedef struct
+{
+    /** Min TX Power capability sent to FW for 11h use and fw power control */
+    t_s8 min_tx_power_capability;
+    /** Max TX Power capability sent to FW for 11h use and fw power control */
+    t_s8 max_tx_power_capability;
+    /** User provisioned local power constraint sent in association requests */
+    t_s8 usr_def_power_constraint;
+    /** Received CHANNEL_SWITCH_ANN event */
+    t_bool recvd_chanswann_event;
+    /** Indicates an interface wants to enable master radar detection */
+    t_bool master_radar_det_enable_pending;
+    /** Indicates an interface wants to enable slave radar detection */
+    t_bool slave_radar_det_enable_pending;
+    /** Indicates whether master radar detection active in the firmware */
+    t_bool is_master_radar_det_active;
+    /** Indicates whether slave radar detection active in the firmware */
+    t_bool is_slave_radar_det_active;
+    /** Quiet IE */
+    IEEEtypes_Quiet_t quiet_ie;
+} wlan_11h_device_state_t;
+
+/** Enumeration for DFS Timestamp represents field */
+enum _dfs_timestamp_repr_e
+{
+    /** Ignore entry */
+    DFS_TS_REPR_NOT_IN_USE = 0,
+    /** NOP (Non-Occupancy Period) start time */
+    DFS_TS_REPR_NOP_START = 1,
+    /** CAC (Channel Availability Check) completion time */
+    DFS_TS_REPR_CAC_COMPLETION
+};
+
+/** DFS Timestamp type used for marking NOP/CAC events */
+typedef struct _wlan_dfs_timestamp_t wlan_dfs_timestamp_t;
+
+/** DFS Timestamp type used for marking NOP/CAC events */
+struct _wlan_dfs_timestamp_t
+{
+    /** Pointer to previous node */
+    wlan_dfs_timestamp_t *pprev;
+    /** Pointer to next node */
+    wlan_dfs_timestamp_t *pnext;
+    /** WLAN Channel number */
+    t_u8 channel;
+    /** What this timestamp represents */
+    t_u8 represents;
+    /** reserved field */
+    t_u16 reserved;
+    /** timestamp - seconds */
+    t_u32 ts_sec;
+    /** timestamp - microseconds */
+    t_u32 ts_usec;
+};
+
+/** DFS State information kept in the 'mlan_adapter' driver structure */
+typedef struct
+{
+    /** Indicates whether DFS channel check is occurring in firmware */
+    t_bool dfs_check_pending;
+    /** Indicates whether DFS channel check found radar */
+    t_bool dfs_radar_found;
+    /** Channel radar is being checked on.  BAND_A is assumed. */
+    t_u8 dfs_check_channel;
+    /** Timestamp when we got last report, to determine if data is old or not. */
+    t_u32 dfs_report_time_sec;
+    /** List for holding dfs_timestamps for NOP/CAC events */
+    mlan_list_head dfs_ts_head;
+} wlan_dfs_device_state_t;
+
+/** Enumeration for mlan_ds_11h_radar_det_hndlg stages */
+enum _mlan_ds_11h_rdh_stages
+{
+    RDH_OFF = 0,
+    RDH_CHK_INTFS = 1,
+    RDH_STOP_TRAFFIC,
+    RDH_GET_INFO_CHANNEL,
+    RDH_GET_INFO_BEACON_DTIM,
+    RDH_SET_CUSTOM_IE,
+    RDH_REM_CUSTOM_IE,
+    RDH_STOP_INTFS,
+    RDH_SET_NEW_CHANNEL,
+    RDH_RESTART_INTFS,
+    RDH_RESTART_TRAFFIC
+};
+
+/** State info for Radar Detected Handling kept in 'mlan_adapter' */
+typedef struct
+{
+    /** Stage (of Operation) */
+    t_u8 stage;
+    /** Number of interfaces to handle */
+    t_u8 priv_list_count;
+    /** Index of interface in process (used by some stages) */
+    t_u8 priv_curr_idx;
+    /** Current Channel (to leave) */
+    t_u8 curr_channel;
+    /** New Channel (to switch to) */
+    t_u8 new_channel;
+    /** UAP band_config */
+    t_u8 uap_band_cfg;
+    /** BEACON*DTIM period (in msec; max of STA/UAP) */
+    t_u16 max_bcn_dtim_ms;
+    /** List of interfaces to handle */
+    mlan_private *priv_list[MLAN_MAX_BSS_NUM];
+} wlan_radar_det_hndlg_state_t;
+
+#ifdef DFS_TESTING_SUPPORT
+/** DFS/RDH testing exception settings kept in 'mlan_adapter' */
+typedef struct
+{
+    /** user-configured CAC period (in msec) */
+    t_u16 user_cac_period_msec;
+    /** user-configured NOP period (in sec) */
+    t_u16 user_nop_period_sec;
+    /** user-configured skip channel change on radar */
+    t_bool no_channel_change_on_radar;
+    /** user-configured new channel to change to on radar */
+    t_u8 fixed_new_channel_on_radar;
+} wlan_dfs_testing_settings_t;
+#endif /* DFS_SUPPORT_TESTING */
+
+/**
+ * @brief Driver measurement state held in 'mlan_adapter' structure
+ *
+ *  Used to record a measurement request that the driver is pending on
+ *    the result (received measurement report).
+ */
+typedef struct
+{
+    /**
+     * Dialog token of a pending measurement request/report.  Used to
+     *   block execution while waiting for the specific dialog token
+     */
+    t_u8 meas_rpt_pend_on;
+
+    /**
+     * Measurement report received from the firmware that we were pending on
+     */
+    HostCmd_DS_MEASUREMENT_REPORT meas_rpt_returned;
+
+} wlan_meas_state_t;
+
+#ifdef SDIO_MULTI_PORT_TX_AGGR
+/** data structure for SDIO MPA TX */
+typedef struct _sdio_mpa_tx
+{
+        /** allocated buf for tx aggreation */
+    t_u8 *head_ptr;
+        /** multiport tx aggregation buffer pointer */
+    t_u8 *buf;
+        /** multiport tx aggregation buffer length */
+    t_u32 buf_len;
+        /** multiport tx aggregation packet count */
+    t_u32 pkt_cnt;
+        /** multiport tx aggregation ports */
+    t_u32 ports;
+        /** multiport tx aggregation starting port */
+    t_u16 start_port;
+        /** multiport tx aggregation enable/disable flag */
+    t_u8 enabled;
+        /** multiport tx aggregation buffer size */
+    t_u32 buf_size;
+        /** multiport tx aggregation pkt aggr limit */
+    t_u32 pkt_aggr_limit;
+} sdio_mpa_tx;
+#endif
+
+#ifdef SDIO_MULTI_PORT_RX_AGGR
+/** data structure for SDIO MPA RX */
+typedef struct _sdio_mpa_rx
+{
+        /** allocated buf for rx aggreation */
+    t_u8 *head_ptr;
+        /** multiport rx aggregation buffer pointer */
+    t_u8 *buf;
+        /** multiport rx aggregation buffer length */
+    t_u32 buf_len;
+        /** multiport rx aggregation packet count */
+    t_u32 pkt_cnt;
+        /** multiport rx aggregation ports */
+    t_u32 ports;
+        /** multiport rx aggregation starting port */
+    t_u16 start_port;
+
+        /** multiport rx aggregation mbuf array */
+    pmlan_buffer mbuf_arr[SDIO_MP_AGGR_DEF_PKT_LIMIT];
+        /** multiport rx aggregation pkt len array */
+    t_u32 len_arr[SDIO_MP_AGGR_DEF_PKT_LIMIT];
+
+        /** multiport rx aggregation enable/disable flag */
+    t_u8 enabled;
+        /** multiport rx aggregation buffer size */
+    t_u32 buf_size;
+        /** multiport rx aggregation pkt aggr limit */
+    t_u32 pkt_aggr_limit;
+} sdio_mpa_rx;
+#endif /* SDIO_MULTI_PORT_RX_AGGR */
+
+/** mlan_init_para structure */
+typedef struct _mlan_init_para
+{
+#ifdef MFG_CMD_SUPPORT
+    /** MFG mode */
+    t_u32 mfg_mode;
+#endif
+    /** SDIO interrupt mode (0: INT_MODE_SDIO, 1: INT_MODE_GPIO) */
+    t_u32 int_mode;
+    /** GPIO interrupt pin number */
+    t_u32 gpio_pin;
+#ifdef SDIO_MULTI_PORT_TX_AGGR
+    /** SDIO MPA Tx */
+    t_u32 mpa_tx_cfg;
+#endif
+#ifdef SDIO_MULTI_PORT_RX_AGGR
+    /** SDIO MPA Rx */
+    t_u32 mpa_rx_cfg;
+#endif
+    /** Auto deep sleep */
+    t_u32 auto_ds;
+    /** IEEE PS mode */
+    t_u32 ps_mode;
+    /** Max Tx buffer size */
+    t_u32 max_tx_buf;
+    /** 802.11d configuration */
+    t_u32 cfg_11d;
+    /** 802.11H DFS Master Radar Detect */
+    t_u32 dfs_master_radar_det_en;
+    /** 802.11H DFS Slave Radar Detect */
+    t_u32 dfs_slave_radar_det_en;
+} mlan_init_para, *pmlan_init_para;
+
+/** Adapter data structure for MLAN */
+typedef struct _mlan_adapter
+{
+    /** MOAL handle structure */
+    t_void *pmoal_handle;
+    /** Private pointer */
+    pmlan_private priv[MLAN_MAX_BSS_NUM];
+    /** Total number of Priv number */
+    t_u8 priv_num;
+    /** Priority table for bss */
+    mlan_bssprio_tbl bssprio_tbl[MLAN_MAX_BSS_NUM];
+    /** Callback table */
+    mlan_callbacks callbacks;
+    /** Init parameters */
+    mlan_init_para init_para;
+
+    /** mlan_lock for init/shutdown */
+    t_void *pmlan_lock;
+    /** main_proc_lock for main_process */
+    t_void *pmain_proc_lock;
+    /** mlan_processing */
+    t_u32 mlan_processing;
+    /** Max tx buf size */
+    t_u16 max_tx_buf_size;
+    /** Tx buf size */
+    t_u16 tx_buf_size;
+    /** current tx buf size in fw */
+    t_u16 curr_tx_buf_size;
+    /** IO port */
+    t_u32 ioport;
+
+    /** STATUS variables */
+    WLAN_HARDWARE_STATUS hw_status;
+    /** PnP SUPPORT */
+    t_u8 surprise_removed;
+
+    /** Radio on flag */
+    t_u16 radio_on;
+
+    /** Firmware release number */
+    t_u32 fw_release_number;
+
+    /** Number of antenna used */
+    t_u16 number_of_antenna;
+
+    /** Firmware capability information */
+    t_u32 fw_cap_info;
+    /** pint_lock for interrupt handling */
+    t_void *pint_lock;
+    /** Interrupt status */
+    t_u8 sdio_ireg;
+    /** SDIO multiple port read bitmap */
+    t_u32 mp_rd_bitmap;
+    /** SDIO multiple port write bitmap */
+    t_u32 mp_wr_bitmap;
+    /** SDIO end port from txbufcfg */
+    t_u16 mp_end_port;
+    /** SDIO port mask calculated based on txbufcfg end port */
+    t_u32 mp_data_port_mask;
+    /** Current available port for read */
+    t_u8 curr_rd_port;
+    /** Current available port for write */
+    t_u8 curr_wr_port;
+    /** Array to store values of SDIO multiple port group registers */
+    t_u8 *mp_regs;
+    /** allocated buf to read SDIO multiple port group registers */
+    t_u8 *mp_regs_buf;
+
+#ifdef SDIO_MULTI_PORT_TX_AGGR
+        /** data structure for SDIO MPA TX */
+    sdio_mpa_tx mpa_tx;
+#endif                          /* SDIO_MULTI_PORT_TX_AGGR */
+
+#ifdef SDIO_MULTI_PORT_RX_AGGR
+        /** data structure for SDIO MPA RX */
+    sdio_mpa_rx mpa_rx;
+#endif                          /* SDIO_MULTI_PORT_RX_AGGR */
+
+    /** SDIO interrupt mode (0: INT_MODE_SDIO, 1: INT_MODE_GPIO) */
+    t_u32 int_mode;
+    /** GPIO interrupt pin number */
+    t_u32 gpio_pin;
+
+    /** Event cause */
+    t_u32 event_cause;
+    /** Event buffer */
+    pmlan_buffer pmlan_buffer_event;
+    /** Upload length */
+    t_u32 upld_len;
+    /** Upload buffer*/
+    t_u8 upld_buf[WLAN_UPLD_SIZE];
+    /** Data sent:
+     *       TRUE - Data is sent to fw, no Tx Done received
+     *       FALSE - Tx done received for previous Tx
+     */
+    t_u8 data_sent;
+    /** CMD sent:
+     *       TRUE - CMD is sent to fw, no CMD Done received
+     *       FALSE - CMD done received for previous CMD
+     */
+    t_u8 cmd_sent;
+    /** CMD Response received:
+     *       TRUE - CMD is response is received from fw, and yet to process
+     *       FALSE - No cmd response to process
+     */
+    t_u8 cmd_resp_received;
+    /** Event received:
+     *       TRUE - Event received from fw, and yet to process
+     *       FALSE - No events to process
+     */
+    t_u8 event_received;
+
+    /** Data received:
+     *       TRUE - Data received from fw
+     *       FALSE - No Data received
+     */
+    t_u8 data_received;
+
+    /** Command-related variables */
+    /** Command sequence number */
+    t_u16 seq_num;
+    /** Command controller nodes */
+    cmd_ctrl_node *cmd_pool;
+    /** Current Command */
+    cmd_ctrl_node *curr_cmd;
+    /** mlan_lock for command */
+    t_void *pmlan_cmd_lock;
+    /** Number of command timeouts */
+    t_u32 num_cmd_timeout;
+    /** Last init fw command id */
+    t_u16 last_init_cmd;
+    /** Command timer */
+    t_void *pmlan_cmd_timer;
+    /** Command timer set flag */
+    t_u8 cmd_timer_is_set;
+
+    /** Command Queues */
+    /** Free command buffers */
+    mlan_list_head cmd_free_q;
+    /** Pending command buffers */
+    mlan_list_head cmd_pending_q;
+    /** Command queue for scanning */
+    mlan_list_head scan_pending_q;
+    /** mlan_processing */
+    t_u32 scan_processing;
+
+    /** Region code */
+    t_u16 region_code;
+    /** Region Channel data */
+    region_chan_t region_channel[MAX_REGION_CHANNEL_NUM];
+    /** CFP table code for 2.4GHz */
+    t_u8 cfp_code_bg;
+    /** CFP table code for 5GHz */
+    t_u8 cfp_code_a;
+#ifdef STA_SUPPORT
+    /** Universal Channel data */
+    region_chan_t universal_channel[MAX_REGION_CHANNEL_NUM];
+    /** Parsed region channel */
+    parsed_region_chan_11d_t parsed_region_chan;
+#endif                          /* STA_SUPPORT */
+    /** 11D and Domain Regulatory Data */
+    wlan_802_11d_domain_reg_t domain_reg;
+    /** Country Code */
+    t_u8 country_code[COUNTRY_CODE_LEN];
+    /** FSM variable for 11h support */
+    wlan_11h_device_state_t state_11h;
+    /** FSM variable for DFS support */
+    wlan_dfs_device_state_t state_dfs;
+    /** FSM variable for RDH support */
+    wlan_radar_det_hndlg_state_t state_rdh;
+#ifdef DFS_TESTING_SUPPORT
+    /** User configured settings for DFS testing */
+    wlan_dfs_testing_settings_t dfs_test_params;
+#endif
+    /** FSM variable for MEAS support */
+    wlan_meas_state_t state_meas;
+    /** Scan table */
+    BSSDescriptor_t *pscan_table;
+    /** scan age in secs */
+    t_u32 age_in_secs;
+    t_u8 bgscan_reported;
+
+    /** Number of records in the scan table */
+    t_u32 num_in_scan_table;
+    /** Scan probes */
+    t_u16 scan_probes;
+
+    /** Scan type */
+    t_u8 scan_type;
+    /** Scan mode */
+    t_u32 scan_mode;
+    /** Specific scan time */
+    t_u16 specific_scan_time;
+    /** Active scan time */
+    t_u16 active_scan_time;
+    /** Passive scan time */
+    t_u16 passive_scan_time;
+    /** Extended scan or legacy scan */
+    t_u8 ext_scan;
+    t_u16 bcn_buf_size;
+    /** Beacon buffer */
+    t_u8 *bcn_buf;
+    /** Pointer to valid beacon buffer end */
+    t_u8 *pbcn_buf_end;
+
+    /** F/W supported bands */
+    t_u8 fw_bands;
+    /** User selected band to start adhoc network */
+    t_u8 adhoc_start_band;
+    /** User selected bands */
+    t_u8 config_bands;
+    /** Pointer to channel list last sent to the firmware for scanning */
+    ChanScanParamSet_t *pscan_channels;
+
+    /** Tx lock flag */
+    t_u8 tx_lock_flag;
+
+    /** sleep_params_t */
+    sleep_params_t sleep_params;
+    /** sleep_period_t (Enhanced Power Save) */
+    sleep_period_t sleep_period;
+
+    /** Power Save mode */
+    /**
+      * Wlan802_11PowerModeCAM = disable
+      * Wlan802_11PowerModePSP = enable
+      */
+    t_u16 ps_mode;
+    /** Power Save state */
+    t_u32 ps_state;
+    /** Need to wakeup flag */
+    t_u8 need_to_wakeup;
+
+    /** Multiple DTIM */
+    t_u16 multiple_dtim;
+    /** Local listen interval */
+    t_u16 local_listen_interval;
+    /** Null packet interval */
+    t_u16 null_pkt_interval;
+
+    /** Power save confirm sleep command buffer */
+    pmlan_buffer psleep_cfm;
+    /** Beacon miss timeout */
+    t_u16 bcn_miss_time_out;
+
+    /** AdHoc awake period */
+    t_u16 adhoc_awake_period;
+
+    /** Deep Sleep flag */
+    t_u8 is_deep_sleep;
+    /** Idle time */
+    t_u16 idle_time;
+    /** Auto Deep Sleep enabled at init time */
+    t_u8 init_auto_ds;
+
+    /** delay null pkt flag */
+    t_u8 delay_null_pkt;
+    /** Delay to PS in milliseconds */
+    t_u16 delay_to_ps;
+    /** Enhanced PS mode */
+    t_u16 enhanced_ps_mode;
+    /** Device wakeup required flag */
+    t_u8 pm_wakeup_card_req;
+
+    /** Gen NULL pkg */
+    t_u16 gen_null_pkt;
+
+    /** PPS/UAPSD mode flag */
+    t_u16 pps_uapsd_mode;
+    /** Number of wakeup tries */
+    t_u32 pm_wakeup_fw_try;
+
+    /** Host Sleep configured flag */
+    t_u8 is_hs_configured;
+    /** Host Sleep configuration */
+    hs_config_param hs_cfg;
+    /** Host Sleep activated flag */
+    t_u8 hs_activated;
+    /** Event body */
+    t_u8 event_body[MAX_EVENT_SIZE];
+    /** 802.11n device capabilities */
+    t_u32 hw_dot_11n_dev_cap;
+    /** Device support for MIMO abstraction of MCSs */
+    t_u8 hw_dev_mcs_support;
+    /** 802.11n Device Capabilities for 2.4GHz */
+    t_u32 usr_dot_11n_dev_cap_bg;
+    /** 802.11n Device Capabilities for 5GHz */
+    t_u32 usr_dot_11n_dev_cap_a;
+    /** MIMO abstraction of MCSs supported by device */
+    t_u8 usr_dev_mcs_support;
+#ifdef STA_SUPPORT
+    /** Enable 11n support for adhoc start */
+    t_u8 adhoc_11n_enabled;
+    /** Adhoc Secondary Channel Bandwidth */
+    t_u8 chan_bandwidth;
+#endif                          /* STA_SUPPORT */
+
+    /** max mgmt IE index in device */
+    t_u16 max_mgmt_ie_index;
+
+#ifdef MFG_CMD_SUPPORT
+    t_u32 mfg_mode;
+#endif
+    /** Debug */
+    wlan_dbg dbg;
+
+    /** RX pending for forwarding packets */
+    t_u16 pending_bridge_pkts;
+
+#ifdef STA_SUPPORT
+    /** ARP filter buffer */
+    t_u8 arp_filter[ARP_FILTER_MAX_BUF_SIZE];
+    /** ARP filter buffer size */
+    t_u32 arp_filter_size;
+#endif                          /* STA_SUPPORT */
+
+    /** Bypass TX queue */
+    mlan_list_head bypass_txq;
+#if defined(STA_SUPPORT)
+    /** warm-reset IOCTL request buffer pointer */
+    pmlan_ioctl_req pwarm_reset_ioctl_req;
+#endif
+    /** Extended SCAN IOCTL request buffer pointer */
+    pmlan_ioctl_req pext_scan_ioctl_req;
+    /** Cal data pointer */
+    t_u8 *pcal_data;
+    /** Cal data length  */
+    t_u32 cal_data_len;
+
+} mlan_adapter, *pmlan_adapter;
+
+/** Ethernet packet type for EAPOL */
+#define MLAN_ETHER_PKT_TYPE_EAPOL	(0x888E)
+/** Ethernet packet type for WAPI */
+#define MLAN_ETHER_PKT_TYPE_WAPI 	(0x88B4)
+/** Ethernet packet type offset */
+#define MLAN_ETHER_PKT_TYPE_OFFSET  (12)
+
+mlan_status wlan_init_lock_list(IN pmlan_adapter pmadapter);
+t_void wlan_free_lock_list(IN pmlan_adapter pmadapter);
+mlan_status wlan_init_timer(IN pmlan_adapter pmadapter);
+t_void wlan_free_timer(IN pmlan_adapter pmadapter);
+
+/* Function prototype */
+/** Download firmware */
+mlan_status wlan_dnld_fw(IN pmlan_adapter pmadapter, IN pmlan_fw_image pmfw);
+
+/** Initialize firmware */
+mlan_status wlan_init_fw(IN pmlan_adapter pmadapter);
+
+/** Initialize firmware complete */
+mlan_status wlan_init_fw_complete(IN pmlan_adapter pmadapter);
+
+/** Shutdown firmware complete */
+mlan_status wlan_shutdown_fw_complete(IN pmlan_adapter pmadapter);
+
+/** Receive event */
+mlan_status wlan_recv_event(pmlan_private priv,
+                            mlan_event_id event_id, t_void * pmevent);
+
+/** Initialize mlan_adapter structure */
+t_void wlan_init_adapter(IN pmlan_adapter pmadapter);
+
+/** Initialize mlan_private structure */
+mlan_status wlan_init_priv(IN pmlan_private priv);
+
+/** Process event */
+mlan_status wlan_process_event(pmlan_adapter pmadapter);
+
+/** Prepare command */
+mlan_status wlan_prepare_cmd(IN pmlan_private priv,
+                             IN t_u16 cmd_no,
+                             IN t_u16 cmd_action,
+                             IN t_u32 cmd_oid,
+                             IN t_void * pioctl_buf, IN t_void * pdata_buf);
+
+/** cmd timeout handler */
+t_void wlan_cmd_timeout_func(t_void * FunctionContext);
+/** process host cmd */
+mlan_status wlan_misc_ioctl_host_cmd(IN pmlan_adapter pmadapter,
+                                     IN pmlan_ioctl_req pioctl_req);
+/** process init/shutdown cmd*/
+mlan_status wlan_misc_ioctl_init_shutdown(IN pmlan_adapter pmadapter,
+                                          IN pmlan_ioctl_req pioctl_req);
+/** process debug info */
+mlan_status wlan_get_info_debug_info(IN pmlan_adapter pmadapter,
+                                     IN pmlan_ioctl_req pioctl_req);
+
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+/** Set/Get BSS role */
+mlan_status wlan_bss_ioctl_bss_role(IN pmlan_adapter pmadapter,
+                                    IN pmlan_ioctl_req pioctl_req);
+#endif
+
+mlan_status wlan_set_ewpa_mode(mlan_private * priv,
+                               mlan_ds_passphrase * psec_pp);
+mlan_status wlan_find_bss(mlan_private * pmpriv, pmlan_ioctl_req pioctl_req);
+
+/** Allocate memory for adapter structure members */
+mlan_status wlan_allocate_adapter(pmlan_adapter pmadapter);
+/** Free adapter */
+t_void wlan_free_adapter(pmlan_adapter pmadapter);
+/** Free priv */
+t_void wlan_free_priv(mlan_private * pmpriv);
+/** Allocate command buffer */
+mlan_status wlan_alloc_cmd_buffer(IN mlan_adapter * pmadapter);
+/** Free command buffer */
+mlan_status wlan_free_cmd_buffer(IN mlan_adapter * pmadapter);
+/** Request command lock */
+t_void wlan_request_cmd_lock(mlan_adapter * pmadapter);
+/** Release command lock */
+t_void wlan_release_cmd_lock(mlan_adapter * pmadapter);
+#ifdef STA_SUPPORT
+/** Flush the scan pending queue */
+t_void wlan_flush_scan_queue(pmlan_adapter pmadapter);
+#endif
+/**Cancel pending command */
+t_void wlan_cancel_all_pending_cmd(pmlan_adapter pmadapter);
+/**Cancel pending ioctl */
+t_void wlan_cancel_pending_ioctl(pmlan_adapter pmadapter,
+                                 pmlan_ioctl_req pioctl_req);
+
+/** Insert command to free queue */
+t_void wlan_insert_cmd_to_free_q(IN mlan_adapter * pmadapter,
+                                 IN cmd_ctrl_node * pcmd_node);
+
+/** Insert command to pending queue */
+t_void wlan_insert_cmd_to_pending_q(IN mlan_adapter * pmadapter,
+                                    IN cmd_ctrl_node * pcmd_node,
+                                    IN t_u32 addtail);
+
+/** Execute next command */
+mlan_status wlan_exec_next_cmd(mlan_adapter * pmadapter);
+/** Proecess command response */
+mlan_status wlan_process_cmdresp(mlan_adapter * pmadapter);
+/** Handle received packet, has extra handling for aggregate packets */
+mlan_status wlan_handle_rx_packet(pmlan_adapter pmadapter, pmlan_buffer pmbuf);
+/** Process transmission */
+mlan_status wlan_process_tx(pmlan_private priv, pmlan_buffer pmbuf,
+                            mlan_tx_param * tx_param);
+/** Transmit a null data packet */
+mlan_status wlan_send_null_packet(pmlan_private priv, t_u8 flags);
+
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+mlan_status wlan_alloc_sdio_mpa_buffers(IN mlan_adapter * pmadapter,
+                                        t_u32 mpa_tx_buf_size,
+                                        t_u32 mpa_rx_buf_size);
+
+mlan_status wlan_free_sdio_mpa_buffers(IN mlan_adapter * pmadapter);
+#endif
+
+/** Process write data complete */
+mlan_status wlan_write_data_complete(pmlan_adapter pmlan_adapter,
+                                     pmlan_buffer pmbuf, mlan_status status);
+/** Process receive packet complete */
+mlan_status wlan_recv_packet_complete(pmlan_adapter pmadapter,
+                                      pmlan_buffer pmbuf, mlan_status status);
+/** Clean Tx Rx queues */
+t_void wlan_clean_txrx(pmlan_private priv);
+
+t_void wlan_add_buf_bypass_txqueue(mlan_adapter * pmadapter,
+                                   pmlan_buffer pmbuf);
+t_void wlan_process_bypass_tx(mlan_adapter * pmadapter);
+t_void wlan_cleanup_bypass_txq(mlan_adapter * pmadapter);
+t_u8 wlan_bypass_tx_list_empty(mlan_adapter * pmadapter);
+
+/** Check if this is the last packet */
+t_u8 wlan_check_last_packet_indication(pmlan_private priv);
+
+/** function to allocate a mlan_buffer */
+pmlan_buffer wlan_alloc_mlan_buffer(mlan_adapter * pmadapter, t_u32 data_len,
+                                    t_u32 head_room, t_u32 malloc_flag);
+/** function to free a mlan_buffer */
+t_void wlan_free_mlan_buffer(mlan_adapter * pmadapter, pmlan_buffer pmbuf);
+
+/** command resp handler for version ext */
+mlan_status wlan_ret_ver_ext(pmlan_private pmpriv, HostCmd_DS_COMMAND * resp,
+                             mlan_ioctl_req * pioctl_buf);
+
+/** command resp handler for rx mgmt forward registration */
+mlan_status wlan_ret_rx_mgmt_ind(pmlan_private pmpriv,
+                                 HostCmd_DS_COMMAND * resp,
+                                 mlan_ioctl_req * pioctl_buf);
+
+/** Check Power Save condition */
+t_void wlan_check_ps_cond(mlan_adapter * pmadapter);
+
+/** handle command for enhanced power save mode */
+mlan_status wlan_cmd_enh_power_mode(pmlan_private pmpriv,
+                                    IN HostCmd_DS_COMMAND * cmd,
+                                    IN t_u16 cmd_action,
+                                    IN t_u16 ps_bitmap, IN t_void * pdata_buf);
+/** handle command resp for enhanced power save mode */
+mlan_status wlan_ret_enh_power_mode(IN pmlan_private pmpriv,
+                                    IN HostCmd_DS_COMMAND * resp,
+                                    IN mlan_ioctl_req * pioctl_buf);
+
+/** handle commnand for cfg data */
+mlan_status wlan_cmd_cfg_data(IN pmlan_private pmpriv,
+                              IN HostCmd_DS_COMMAND * pcmd,
+                              IN t_u16 cmd_action, IN t_void * pdata_buf);
+/** handle command resp for cfg data */
+mlan_status wlan_ret_cfg_data(IN pmlan_private pmpriv,
+                              IN HostCmd_DS_COMMAND * resp,
+                              IN t_void * pioctl_buf);
+
+/** Process sleep confirm command response */
+void wlan_process_sleep_confirm_resp(pmlan_adapter pmadapter, t_u8 * pbuf,
+                                     t_u32 len);
+
+/** Perform hs related activities on receving the power up interrupt */
+void wlan_process_hs_config(pmlan_adapter pmadapter);
+
+mlan_status wlan_pm_reset_card(pmlan_adapter adapter);
+mlan_status wlan_pm_wakeup_card(pmlan_adapter pmadapter);
+
+mlan_status wlan_process_802dot11_mgmt_pkt(mlan_private * priv, t_u8 * payload,
+                                           t_u32 payload_len);
+
+mlan_status wlan_pm_ioctl_hscfg(pmlan_adapter pmadapter,
+                                pmlan_ioctl_req pioctl_req);
+
+#ifdef WIFI_DIRECT_SUPPORT
+mlan_status wlan_bss_ioctl_wifi_direct_mode(IN pmlan_adapter pmadapter,
+                                            IN pmlan_ioctl_req pioctl_req);
+
+mlan_status wlan_cmd_wifi_direct_mode(IN pmlan_private pmpriv,
+                                      IN HostCmd_DS_COMMAND * cmd,
+                                      IN t_u16 cmd_action,
+                                      IN t_void * pdata_buf);
+mlan_status wlan_ret_wifi_direct_mode(IN pmlan_private pmpriv,
+                                      IN HostCmd_DS_COMMAND * resp,
+                                      IN mlan_ioctl_req * pioctl_buf);
+
+mlan_status wlan_radio_ioctl_remain_chan_cfg(IN pmlan_adapter pmadapter,
+                                             IN pmlan_ioctl_req pioctl_req);
+mlan_status wlan_cmd_remain_on_channel(IN pmlan_private pmpriv,
+                                       IN HostCmd_DS_COMMAND * cmd,
+                                       IN t_u16 cmd_action,
+                                       IN t_void * pdata_buf);
+mlan_status wlan_ret_remain_on_channel(IN pmlan_private pmpriv,
+                                       IN HostCmd_DS_COMMAND * resp,
+                                       IN mlan_ioctl_req * pioctl_buf);
+#endif
+
+/** get pm info */
+mlan_status wlan_get_pm_info(IN pmlan_adapter pmadapter,
+                             IN pmlan_ioctl_req pioctl_req);
+
+mlan_status wlan_radio_ioctl_radio_ctl(IN pmlan_adapter pmadapter,
+                                       IN pmlan_ioctl_req pioctl_req);
+
+mlan_status wlan_radio_ioctl_ant_cfg(IN pmlan_adapter pmadapter,
+                                     IN pmlan_ioctl_req pioctl_req);
+
+mlan_status wlan_cmd_tx_rate_cfg(IN pmlan_private pmpriv,
+                                 IN HostCmd_DS_COMMAND * cmd,
+                                 IN t_u16 cmd_action, IN t_void * pdata_buf);
+mlan_status wlan_ret_tx_rate_cfg(IN pmlan_private pmpriv,
+                                 IN HostCmd_DS_COMMAND * resp,
+                                 IN mlan_ioctl_req * pioctl_buf);
+
+mlan_status wlan_rate_ioctl_cfg(IN pmlan_adapter pmadapter,
+                                IN pmlan_ioctl_req pioctl_req);
+mlan_status wlan_ret_802_11_tx_rate_query(IN pmlan_private pmpriv,
+                                          IN HostCmd_DS_COMMAND * resp,
+                                          IN mlan_ioctl_req * pioctl_buf);
+
+t_void wlan_host_sleep_activated_event(pmlan_private priv, t_u8 activated);
+/** Handles the command response of hs_cfg */
+mlan_status wlan_ret_802_11_hs_cfg(IN pmlan_private pmpriv,
+                                   IN HostCmd_DS_COMMAND * resp,
+                                   IN mlan_ioctl_req * pioctl_buf);
+/** Sends HS_WAKEUP event to applications */
+t_void wlan_host_sleep_wakeup_event(pmlan_private priv);
+
+/** send adapter specific init cmd to firmware */
+mlan_status wlan_adapter_init_cmd(IN pmlan_adapter pmadapter);
+
+#ifdef STA_SUPPORT
+/** Process received packet */
+mlan_status wlan_process_rx_packet(pmlan_adapter pmadapter, pmlan_buffer pmbuf);
+/** ioctl handler for station mode */
+mlan_status wlan_ops_sta_ioctl(t_void * adapter, pmlan_ioctl_req pioctl_req);
+
+/** cmd handler for station mode */
+mlan_status wlan_ops_sta_prepare_cmd(IN t_void * priv,
+                                     IN t_u16 cmd_no,
+                                     IN t_u16 cmd_action,
+                                     IN t_u32 cmd_oid,
+                                     IN t_void * pioctl_buf,
+                                     IN t_void * pdata_buf,
+                                     IN t_void * pcmd_buf);
+
+/** cmdresp handler for station mode */
+mlan_status wlan_ops_sta_process_cmdresp(IN t_void * priv,
+                                         IN t_u16 cmdresp_no,
+                                         IN t_void * pcmd_buf,
+                                         IN t_void * pioctl);
+
+/** rx handler for station mode */
+mlan_status wlan_ops_sta_process_rx_packet(IN t_void * adapter,
+                                           IN pmlan_buffer pmbuf);
+
+/** event handler for station mode */
+mlan_status wlan_ops_sta_process_event(IN t_void * priv);
+
+/** fill txpd for station mode */
+t_void *wlan_ops_sta_process_txpd(IN t_void * priv, IN pmlan_buffer pmbuf);
+
+/** send init cmd to firmware for station mode */
+mlan_status wlan_ops_sta_init_cmd(IN t_void * priv, IN t_u8 first_bss);
+
+/** Flush the scan table */
+mlan_status wlan_flush_scan_table(IN pmlan_adapter pmadapter);
+
+/** Scan for networks */
+mlan_status wlan_scan_networks(IN mlan_private * pmpriv,
+                               IN t_void * pioctl_buf,
+                               IN const wlan_user_scan_cfg * puser_scan_in);
+
+/** Scan for specific SSID */
+mlan_status wlan_scan_specific_ssid(IN mlan_private * pmpriv,
+                                    IN t_void * pioctl_buf,
+                                    IN mlan_802_11_ssid * preq_ssid);
+
+/** Scan command handler */
+mlan_status wlan_cmd_802_11_scan(IN pmlan_private pmpriv,
+                                 IN HostCmd_DS_COMMAND * pcmd,
+                                 IN t_void * pdata_buf);
+
+/** Queue scan command handler */
+t_void wlan_queue_scan_cmd(IN mlan_private * pmpriv,
+                           IN cmd_ctrl_node * pcmd_node);
+
+/** Handler for scan command response */
+mlan_status wlan_ret_802_11_scan(IN pmlan_private pmpriv,
+                                 IN HostCmd_DS_COMMAND * resp,
+                                 IN t_void * pioctl_buf);
+
+/** Extended scan command handler */
+mlan_status wlan_cmd_802_11_scan_ext(IN pmlan_private pmpriv,
+                                     IN HostCmd_DS_COMMAND * pcmd,
+                                     IN t_void * pdata_buf);
+/** Handler for extended scan command response */
+mlan_status wlan_ret_802_11_scan_ext(IN pmlan_private pmpriv,
+                                     IN HostCmd_DS_COMMAND * resp,
+                                     IN t_void * pioctl_buf);
+/** Handler event for extended scan report */
+mlan_status wlan_handle_event_ext_scan_report(IN mlan_private * pmpriv,
+                                              IN mlan_buffer * pmbuf);
+
+/** check network compatibility */
+t_s32 wlan_is_network_compatible(IN mlan_private * pmpriv,
+                                 IN t_u32 index, IN t_u32 mode);
+
+/** Find an SSID in a list */
+t_s32 wlan_find_ssid_in_list(IN pmlan_private pmpriv,
+                             IN mlan_802_11_ssid * ssid,
+                             IN t_u8 * bssid, IN t_u32 mode);
+
+/** Find a BSSID in a list */
+t_s32 wlan_find_bssid_in_list(IN mlan_private * pmpriv,
+                              IN t_u8 * bssid, IN t_u32 mode);
+
+/** Find best network */
+mlan_status wlan_find_best_network(IN mlan_private * pmpriv,
+                                   OUT mlan_ssid_bssid * preq_ssid_bssid);
+
+/** Compare two SSIDs */
+t_s32 wlan_ssid_cmp(IN pmlan_adapter pmadapter,
+                    IN mlan_802_11_ssid * ssid1, IN mlan_802_11_ssid * ssid2);
+
+/** Associate */
+mlan_status wlan_associate(IN mlan_private * pmpriv,
+                           IN t_void * pioctl_buf,
+                           IN BSSDescriptor_t * pBSSDesc);
+
+/** Associate command handler */
+mlan_status wlan_cmd_802_11_associate(IN mlan_private * pmpriv,
+                                      IN HostCmd_DS_COMMAND * cmd,
+                                      IN t_void * pdata_buf);
+
+/** Handler for association command response */
+mlan_status wlan_ret_802_11_associate(IN mlan_private * pmpriv,
+                                      IN HostCmd_DS_COMMAND * resp,
+                                      IN t_void * pioctl_buf);
+
+/** Reset connected state */
+t_void wlan_reset_connect_state(IN pmlan_private priv, IN t_u8 drv_disconnect);
+
+t_void wlan_2040_coex_event(pmlan_private pmpriv);
+
+/** convert band to radio type */
+t_u8 wlan_band_to_radio_type(IN t_u8 band);
+
+/** Disconnect */
+mlan_status wlan_disconnect(IN mlan_private * pmpriv,
+                            IN mlan_ioctl_req * pioctl_req,
+                            IN mlan_802_11_mac_addr * mac);
+
+/** Ad-Hoc start */
+mlan_status wlan_adhoc_start(IN mlan_private * pmpriv,
+                             IN t_void * pioctl_buf,
+                             IN mlan_802_11_ssid * padhoc_ssid);
+
+/** Ad-Hoc join */
+mlan_status wlan_adhoc_join(IN mlan_private * pmpriv,
+                            IN t_void * pioctl_buf,
+                            IN BSSDescriptor_t * pBSSDesc);
+
+/** Ad-Hoc start command handler */
+mlan_status wlan_cmd_802_11_ad_hoc_start(IN mlan_private * pmpriv,
+                                         IN HostCmd_DS_COMMAND * cmd,
+                                         IN t_void * pdata_buf);
+
+/** Ad-Hoc command handler */
+mlan_status wlan_cmd_802_11_ad_hoc_join(IN mlan_private * pmpriv,
+                                        IN HostCmd_DS_COMMAND * cmd,
+                                        IN t_void * pdata_buf);
+
+/** Handler for Ad-Hoc commands */
+mlan_status wlan_ret_802_11_ad_hoc(IN mlan_private * pmpriv,
+                                   IN HostCmd_DS_COMMAND * resp,
+                                   IN t_void * pioctl_buf);
+
+/** Handler for bgscan query commands */
+mlan_status wlan_cmd_802_11_bg_scan_query(IN mlan_private * pmpriv,
+                                          IN HostCmd_DS_COMMAND * pcmd,
+                                          IN t_void * pdata_buf);
+/** Handler for bgscan config command */
+mlan_status wlan_cmd_bgscan_config(IN mlan_private * pmpriv,
+                                   IN HostCmd_DS_COMMAND * pcmd,
+                                   IN t_void * pdata_buf);
+/** Hander for bgscan config command response */
+mlan_status wlan_ret_bgscan_config(IN mlan_private * pmpriv,
+                                   IN HostCmd_DS_COMMAND * resp,
+                                   IN mlan_ioctl_req * pioctl_buf);
+mlan_status wlan_ret_802_11_bgscan_query(IN mlan_private * pmpriv,
+                                         IN HostCmd_DS_COMMAND * resp,
+                                         IN mlan_ioctl_req * pioctl_buf);
+
+/** Get Channel-Frequency-Power by band and channel */
+chan_freq_power_t *wlan_get_cfp_by_band_and_channel(pmlan_adapter pmadapter,
+                                                    t_u8 band, t_u16 channel,
+                                                    region_chan_t *
+                                                    region_channel);
+/** Find Channel-Frequency-Power by band and channel */
+chan_freq_power_t *wlan_find_cfp_by_band_and_channel(mlan_adapter * pmadapter,
+                                                     t_u8 band, t_u16 channel);
+/** Find Channel-Frequency-Power by band and frequency */
+chan_freq_power_t *wlan_find_cfp_by_band_and_freq(mlan_adapter * pmadapter,
+                                                  t_u8 band, t_u32 freq);
+/** Get Tx power of channel from Channel-Frequency-Power */
+t_u8 wlan_get_txpwr_of_chan_from_cfp(mlan_private * pmpriv, t_u8 channel);
+/** find frequency from band and channel */
+t_u32 wlan_find_freq_from_band_chan(t_u8, t_u8);
+
+/*  Save a beacon buffer of the current bss descriptor */
+t_void wlan_save_curr_bcn(IN mlan_private * pmpriv);
+/*  Free a beacon buffer of the current bss descriptor */
+t_void wlan_free_curr_bcn(IN mlan_private * pmpriv);
+
+#endif /* STA_SUPPORT */
+
+/* Rate related functions */
+/** Convert index into data rate */
+t_u32 wlan_index_to_data_rate(pmlan_adapter pmadapter, t_u8 index,
+                              t_u8 ht_info);
+/** Get active data rates */
+t_u32 wlan_get_active_data_rates(mlan_private * pmpriv, t_u32 bss_mode,
+                                 t_u8 config_bands, WLAN_802_11_RATES rates);
+/** Get supported data rates */
+t_u32 wlan_get_supported_rates(mlan_private * pmpriv, t_u32 bss_mode,
+                               t_u8 config_bands, WLAN_802_11_RATES rates);
+/** Convert data rate to index */
+t_u8 wlan_data_rate_to_index(pmlan_adapter pmadapter, t_u32 rate);
+/** Check if rate is auto */
+t_u8 wlan_is_rate_auto(mlan_private * pmpriv);
+/** Get rate index */
+int wlan_get_rate_index(pmlan_adapter pmadapter, t_u16 * rateBitmap, int size);
+
+/* CFP related functions */
+/** Region code index table */
+extern t_u16 region_code_index[MRVDRV_MAX_REGION_CODE];
+/** The table to keep CFP code for BG */
+extern t_u16 cfp_code_index_bg[MRVDRV_MAX_CFP_CODE_BG];
+/** The table to keep CFP code for A */
+extern t_u16 cfp_code_index_a[MRVDRV_MAX_CFP_CODE_A];
+/** Set region table */
+mlan_status wlan_set_regiontable(mlan_private * pmpriv, t_u8 region, t_u8 band);
+/** Get radar detection requirements*/
+t_bool wlan_get_cfp_radar_detect(mlan_private * priv, t_u8 chnl);
+/** check if scan type is passive for b/g band*/
+t_bool wlan_bg_scan_type_is_passive(mlan_private * priv, t_u8 chnl);
+
+/* 802.11D related functions */
+/** Initialize 11D */
+t_void wlan_11d_priv_init(mlan_private * pmpriv);
+/** Initialize 11D */
+t_void wlan_11d_init(mlan_adapter * pmadapter);
+/** Enable 11D */
+mlan_status wlan_11d_enable(mlan_private * pmpriv, t_void * pioctl_buf,
+                            state_11d_t flag);
+/** Get if 11D is enabled */
+t_bool wlan_11d_is_enabled(mlan_private * pmpriv);
+/** Get if priv is station */
+t_bool wlan_is_station(mlan_private * pmpriv);
+/** Command handler for 11D country info */
+mlan_status wlan_cmd_802_11d_domain_info(mlan_private * pmpriv,
+                                         HostCmd_DS_COMMAND * pcmd,
+                                         t_u16 cmd_action);
+/** Handler for 11D country info command response */
+mlan_status wlan_ret_802_11d_domain_info(mlan_private * pmpriv,
+                                         HostCmd_DS_COMMAND * resp);
+#ifdef STA_SUPPORT
+/** Convert channel to frequency */
+t_u32 wlan_11d_chan_2_freq(pmlan_adapter pmadapter, t_u8 chan, t_u8 band);
+/** Set 11D universal table */
+mlan_status wlan_11d_set_universaltable(mlan_private * pmpriv, t_u8 band);
+/** Clear 11D region table */
+mlan_status wlan_11d_clear_parsedtable(mlan_private * pmpriv);
+/** Create 11D country information for downloading */
+mlan_status wlan_11d_create_dnld_countryinfo(mlan_private * pmpriv, t_u8 band);
+/** Get scan type from 11D info */
+t_u8 wlan_11d_get_scan_type(pmlan_adapter pmadapter, t_u8 band, t_u8 chan,
+                            parsed_region_chan_11d_t * parsed_region_chan);
+/** Parse 11D country info */
+mlan_status wlan_11d_parse_dnld_countryinfo(mlan_private * pmpriv,
+                                            BSSDescriptor_t * pBSSDesc);
+/** Prepare 11D domain information for download */
+mlan_status wlan_11d_prepare_dnld_domain_info_cmd(mlan_private * pmpriv);
+/** Parse 11D country information into domain info */
+mlan_status wlan_11d_parse_domain_info(pmlan_adapter pmadapter,
+                                       IEEEtypes_CountryInfoFullSet_t *
+                                       country_info, t_u8 band,
+                                       parsed_region_chan_11d_t *
+                                       parsed_region_chan);
+/** Configure 11D domain info command */
+mlan_status wlan_11d_cfg_domain_info(IN pmlan_adapter pmadapter,
+                                     IN mlan_ioctl_req * pioctl_req);
+#endif /* STA_SUPPORT */
+#ifdef UAP_SUPPORT
+/** Handle 11D domain information from UAP */
+mlan_status wlan_11d_handle_uap_domain_info(mlan_private * pmpriv,
+                                            t_u8 band,
+                                            t_u8 * domain_tlv,
+                                            t_void * pioctl_buf);
+#endif
+
+/** This function converts region string to CFP table code */
+mlan_status wlan_misc_country_2_cfp_table_code(IN pmlan_adapter pmadapter,
+                                               IN t_u8 * country_code,
+                                               OUT t_u8 * cfp_bg,
+                                               OUT t_u8 * cfp_a);
+
+/** check if station list is empty */
+t_u8 wlan_is_station_list_empty(mlan_private * priv);
+/** get station node */
+sta_node *wlan_get_station_entry(mlan_private * priv, t_u8 * mac);
+/** delete station list */
+t_void wlan_delete_station_list(pmlan_private priv);
+/** delete station entry */
+t_void wlan_delete_station_entry(mlan_private * priv, t_u8 * mac);
+/** add station entry */
+sta_node *wlan_add_station_entry(mlan_private * priv, t_u8 * mac);
+/** process uap rx packet */
+
+/** find specific ie */
+t_u8 *wlan_get_specific_ie(pmlan_private priv, t_u8 * ie_buf, t_u8 ie_len,
+                           IEEEtypes_ElementId_e id);
+
+/**
+ *  @brief This function checks tx_pause flag for peer
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param ra       Address of the receiver STA
+ *
+ *  @return 	    MTRUE or MFALSE
+ */
+static int INLINE
+wlan_is_tx_pause(mlan_private * priv, t_u8 * ra)
+{
+    sta_node *sta_ptr = MNULL;
+    if ((sta_ptr = wlan_get_station_entry(priv, ra))) {
+        return sta_ptr->tx_pause;
+    }
+    return MFALSE;
+}
+
+t_void wlan_updata_ralist_tx_pause(pmlan_private priv, t_u8 * mac,
+                                   t_u8 tx_pause);
+
+#ifdef UAP_SUPPORT
+mlan_status wlan_process_uap_rx_packet(IN mlan_private * priv,
+                                       IN pmlan_buffer pmbuf);
+t_void wlan_drop_tx_pkts(pmlan_private priv);
+#endif /* UAP_SUPPORT */
+
+#ifdef UAP_SUPPORT
+/* process the recevied packet and bridge the packet */
+mlan_status wlan_uap_recv_packet(IN mlan_private * priv, IN pmlan_buffer pmbuf);
+#endif /* UAP_SUPPORT */
+
+mlan_status wlan_misc_ioctl_custom_ie_list(IN pmlan_adapter pmadapter,
+                                           IN pmlan_ioctl_req pioctl_req,
+                                           IN t_bool send_ioctl);
+
+mlan_status wlan_cmd_get_hw_spec(IN pmlan_private pmpriv,
+                                 IN HostCmd_DS_COMMAND * pcmd);
+mlan_status wlan_ret_get_hw_spec(IN pmlan_private pmpriv,
+                                 IN HostCmd_DS_COMMAND * resp,
+                                 IN t_void * pioctl_buf);
+
+mlan_status wlan_cmd_802_11_radio_control(IN pmlan_private pmpriv,
+                                          IN HostCmd_DS_COMMAND * cmd,
+                                          IN t_u16 cmd_action,
+                                          IN t_void * pdata_buf);
+mlan_status wlan_ret_802_11_radio_control(IN pmlan_private pmpriv,
+                                          IN HostCmd_DS_COMMAND * resp,
+                                          IN mlan_ioctl_req * pioctl_buf);
+
+mlan_status wlan_cmd_802_11_rf_antenna(IN pmlan_private pmpriv,
+                                       IN HostCmd_DS_COMMAND * cmd,
+                                       IN t_u16 cmd_action,
+                                       IN t_void * pdata_buf);
+mlan_status wlan_ret_802_11_rf_antenna(IN pmlan_private pmpriv,
+                                       IN HostCmd_DS_COMMAND * resp,
+                                       IN mlan_ioctl_req * pioctl_buf);
+
+mlan_status wlan_get_info_ver_ext(IN pmlan_adapter pmadapter,
+                                  IN pmlan_ioctl_req pioctl_req);
+
+mlan_status wlan_reg_rx_mgmt_ind(IN pmlan_adapter pmadapter,
+                                 IN pmlan_ioctl_req pioctl_req);
+
+#ifdef DEBUG_LEVEL1
+mlan_status wlan_set_drvdbg(IN pmlan_adapter pmadapter,
+                            IN pmlan_ioctl_req pioctl_req);
+#endif
+
+mlan_status wlan_misc_otp_user_data(IN pmlan_adapter pmadapter,
+                                    IN pmlan_ioctl_req pioctl_req);
+
+/**
+ *  @brief RA based queueing
+ *
+ *  @param priv                 A pointer to mlan_private structure
+ *
+ *  @return 	   	        MTRUE or MFALSE
+ */
+static INLINE t_u8
+queuing_ra_based(pmlan_private priv)
+{
+    /*
+     * Currently we assume if we are in Infra, then DA=RA. This might not be
+     * true in the future
+     */
+    if ((priv->bss_mode == MLAN_BSS_MODE_INFRA) &&
+        (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA))
+        return MFALSE;
+
+    return MTRUE;
+}
+
+/**
+ *  @brief Copy Rates
+ *
+ *  @param dest                 A pointer to Dest Buf
+ *  @param pos		        The position for copy
+ *  @param src		        A pointer to Src Buf
+ *  @param len                  The len of Src Buf
+ *
+ *  @return 	   	        Number of Rates copied
+ */
+static INLINE t_u32
+wlan_copy_rates(t_u8 * dest, t_u32 pos, t_u8 * src, int len)
+{
+    int i;
+
+    for (i = 0; i < len && src[i]; i++, pos++) {
+        if (pos >= sizeof(WLAN_802_11_RATES))
+            break;
+        dest[pos] = src[i];
+    }
+
+    return pos;
+}
+
+/**
+ *  @brief strlen
+ *
+ *  @param str		        A pointer to string
+ *
+ *  @return 	   	        Length of string
+ */
+static INLINE t_u32
+wlan_strlen(const t_s8 * str)
+{
+    t_u32 i;
+
+    for (i = 0; str[i] != 0; i++) {
+    }
+    return i;
+}
+
+/**
+ *  @brief iscdigit
+ *
+ *  @param chr                  A char
+ *
+ *  @return                     Non zero if chr is a hex, else 0
+ */
+static INLINE t_u32
+wlan_isxdigit(t_u8 chr)
+{
+    return ((chr <= 'f' && chr >= 'a') || (chr <= 'F' && chr >= 'A') ||
+            (chr <= '9' && chr >= '0'));
+}
+
+/**
+ *  @brief isspace
+ *
+ *  @param                       A chr
+ *
+ *  @return                      Non zero if chr is space etc, else 0
+ */
+static INLINE t_u32
+wlan_isspace(t_u8 chr)
+{
+    return (chr <= ' ' && (chr == ' ' || (chr <= 13 && chr >= 9)));
+}
+
+/** delay unit */
+typedef enum _delay_unit
+{
+    USEC,
+    MSEC,
+    SEC,
+} t_delay_unit;
+
+/** delay function */
+t_void wlan_delay_func(mlan_adapter * pmadapter, t_u32 delay, t_delay_unit u);
+
+/** delay function wrapper */
+#define wlan_delay(p, n)   wlan_delay_func(p, n, SEC)
+/** delay function wrapper */
+#define wlan_mdelay(p, n)  wlan_delay_func(p, n, MSEC)
+/** delay function wrapper */
+#define wlan_udelay(p, n)  wlan_delay_func(p, n, USEC)
+
+/** Function to check if any command is pending in the queue */
+#define IS_COMMAND_PENDING(pmadapter) ((cmd_ctrl_node *)util_peek_list(pmadapter->pmoal_handle, \
+                                       &pmadapter->cmd_pending_q,\
+                                       pmadapter->callbacks.moal_spin_lock,\
+                                       pmadapter->callbacks.moal_spin_unlock))
+
+/** Get BSS number from priv */
+#define GET_BSS_NUM(priv)   (priv)->bss_num
+/**
+ *  @brief This function returns priv based on the BSS num and BSS type
+ *
+ *  @param pmadapter A pointer to mlan_adapter
+ *  @param bss_num   BSS number
+ *  @param bss_type  BSS type
+ *
+ *  @return          Pointer to mlan_private
+ */
+static INLINE mlan_private *
+wlan_get_priv_by_id(mlan_adapter * pmadapter, t_u32 bss_num, t_u32 bss_type)
+{
+    int i;
+
+    for (i = 0; i < MIN(pmadapter->priv_num, MLAN_MAX_BSS_NUM); i++) {
+        if (pmadapter->priv[i]) {
+            if ((pmadapter->priv[i]->bss_num == bss_num) &&
+                (pmadapter->priv[i]->bss_type == bss_type))
+                return (pmadapter->priv[i]);
+        }
+    }
+    return MNULL;
+}
+
+/**
+ *  @brief This function returns first available priv
+ *  based on the BSS role
+ *
+ *  @param pmadapter A pointer to mlan_adapter
+ *  @param bss_role  BSS role or MLAN_BSS_ROLE_ANY
+ *
+ *  @return          Pointer to mlan_private
+ */
+static INLINE mlan_private *
+wlan_get_priv(mlan_adapter * pmadapter, mlan_bss_role bss_role)
+{
+    int i;
+
+    for (i = 0; i < MIN(pmadapter->priv_num, MLAN_MAX_BSS_NUM); i++) {
+        if (pmadapter->priv[i]) {
+            if (bss_role == MLAN_BSS_ROLE_ANY ||
+                GET_BSS_ROLE(pmadapter->priv[i]) == bss_role)
+                return (pmadapter->priv[i]);
+        }
+    }
+    return MNULL;
+}
+
+/**
+ *  @brief This function counts the number of occurences for a certain
+ *  condition among privs.  Which privs are checked can be configured
+ *  via a second condition.
+ *
+ *  @param pmadapter  A pointer to mlan_adapter
+ *  @param count_cond Function pointer to condition to count on privs
+ *  @param check_cond Function pointer to condition to decide whether priv
+ *                    should be counted or not.  Use MNULL to check all privs.
+ *
+ *  @return          Count of privs where count_cond returned MTRUE.
+ */
+static int INLINE
+wlan_count_priv_cond(mlan_adapter * pmadapter,
+                     t_bool(*count_cond) (IN pmlan_private pmpriv),
+                     t_bool(*check_cond) (IN pmlan_private pmpriv))
+{
+    pmlan_private pmpriv;
+    int count = 0;
+    int i;
+
+    if (pmadapter == MNULL || count_cond == MNULL)
+        return 0;
+
+    for (i = 0; i < pmadapter->priv_num; i++) {
+        if ((pmpriv = pmadapter->priv[i])) {
+            if ((check_cond == MNULL) || (check_cond && check_cond(pmpriv))) {
+                if (count_cond(pmpriv))
+                    count++;
+            }
+        }
+    }
+
+    return count;
+}
+
+/**
+ *  @brief This function runs a procedure on each priv.
+ *  Which privs it is run on can be configured via a condition.
+ *
+ *  @param pmadapter  A pointer to mlan_adapter
+ *  @param operation  Function pointer to produedure to operate on priv
+ *  @param check_cond Function pointer to condition to decide whether priv
+ *                    operated on or not.  Use MNULL to run on all privs.
+ *
+ *  @return           Number of privs that operation was run on.
+ */
+static int INLINE
+wlan_do_task_on_privs(mlan_adapter * pmadapter,
+                      t_void(*operation) (IN pmlan_private pmpriv),
+                      t_bool(*check_cond) (IN pmlan_private pmpriv))
+{
+    pmlan_private pmpriv;
+    int count = 0;
+    int i;
+
+    if (pmadapter == MNULL || operation == MNULL)
+        return 0;
+
+    for (i = 0; i < pmadapter->priv_num; i++) {
+        if ((pmpriv = pmadapter->priv[i])) {
+            if ((check_cond == MNULL) || (check_cond && check_cond(pmpriv))) {
+                operation(pmpriv);
+                count++;
+            }
+        }
+    }
+
+    return count;
+}
+
+/**
+ *  @brief This function builds a list of privs that test for a condition
+ *  This is useful if you need to do a number of operations on the same set
+ *  of privs.  For one-off tasks, the above two functions might be better.
+ *
+ *  @param pmadapter  A pointer to mlan_adapter
+ *  @param check_cond Function pointer to condition to decide whether priv
+ *                    should be placed in the list.
+ *  @param ppriv_list Output param.  Externally supplied array of mlan_private*
+ *                    to hold priv's that test positive with check_cond.
+ *                    Array size should be at least pmadapter->priv_num.
+ *
+ *  @return          Number of privs in ppriv_list
+ *
+ *  @sa              wlan_count_priv_cond
+ */
+static int INLINE
+wlan_get_privs_by_cond(mlan_adapter * pmadapter,
+                       t_bool(*check_cond) (IN pmlan_private pmpriv),
+                       mlan_private ** ppriv_list)
+{
+    pmlan_private pmpriv;
+    int count = 0;
+    int i;
+
+    if (pmadapter == MNULL || check_cond == MNULL || ppriv_list == MNULL)
+        return 0;
+
+    for (i = 0; i < pmadapter->priv_num; i++) {
+        if ((pmpriv = pmadapter->priv[i])) {
+            if (check_cond(pmpriv)) {
+                ppriv_list[count++] = pmpriv;
+            }
+        }
+    }
+
+    return count;
+}
+
+/**
+ *  @brief This function builds a list of privs that test against two conditions
+ *  This is useful if you need to do a number of operations on the same set
+ *  of privs.  Can choose whether both conditions (AND) or either condition (OR)
+ *  is required.
+ *
+ *  @param pmadapter      A pointer to mlan_adapter
+ *  @param check_cond     Function pointer to condition to decide whether priv
+ *                        should be placed in the list.
+ *  @param check_cond_2   Function pointer to second condition to check.
+ *  @param and_conditions If MTRUE, both conditions must be met (AND),
+ *                        else either condition can be met (OR).
+ *  @param ppriv_list     Output param.  Externally supplied array of mlan_private*
+ *                        to hold priv's that test positive with check_cond.
+ *                        Array size should be at least pmadapter->priv_num.
+ *
+ *  @return          Number of privs in ppriv_list
+ *
+ *  @sa              wlan_count_priv_cond, wlan_get_privs_by_cond
+ */
+static int INLINE
+wlan_get_privs_by_two_cond(mlan_adapter * pmadapter,
+                           t_bool(*check_cond) (IN pmlan_private pmpriv),
+                           t_bool(*check_cond_2) (IN pmlan_private pmpriv),
+                           t_bool and_conditions, mlan_private ** ppriv_list)
+{
+    pmlan_private pmpriv;
+    int count = 0;
+    int i;
+
+    if (pmadapter == MNULL || check_cond == MNULL ||
+        check_cond_2 == MNULL || ppriv_list == MNULL)
+        return 0;
+
+    for (i = 0; i < pmadapter->priv_num; i++) {
+        if ((pmpriv = pmadapter->priv[i])) {
+            if (and_conditions) {
+                if (check_cond(pmpriv) && check_cond_2(pmpriv)) {
+                    ppriv_list[count++] = pmpriv;
+                }
+            } else {
+                if (check_cond(pmpriv) || check_cond_2(pmpriv)) {
+                    ppriv_list[count++] = pmpriv;
+                }
+            }
+        }
+    }
+
+    return count;
+}
+
+#endif /* !_MLAN_MAIN_H_ */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_meas.c b/drivers/net/wireless/sd8797/mlan/mlan_meas.c
new file mode 100644
index 000000000000..a2a6f2f5b8a1
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_meas.c
@@ -0,0 +1,464 @@
+/**
+ * @file mlan_meas.c
+ *
+ *  @brief Implementation of measurement interface code with the app/firmware
+ *
+ *  Driver implementation for sending and retrieving measurement requests
+ *    and responses. 
+ *  
+ *  Current use is limited to 802.11h.
+ *
+ *  Requires use of the following preprocessor define:
+ *    - ENABLE_MEAS
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *  
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/*************************************************************
+Change Log:
+    03/24/2009: initial version
+************************************************************/
+
+#include "mlan.h"
+#include "mlan_join.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_ioctl.h"
+#include "mlan_meas.h"
+
+/** Default measurement duration when not provided by the application */
+#define WLAN_MEAS_DEFAULT_MEAS_DURATION    1000U        /* TUs */
+
+#ifdef DEBUG_LEVEL2
+/** String descriptions of the different measurement enums.  Debug display */
+static const char *meas_type_str[WLAN_MEAS_NUM_TYPES] = {
+    "basic",
+};
+
+/********************************************************
+                Local Functions
+********************************************************/
+
+/**
+ *  @brief Retrieve the measurement string representation of a meas_type enum
+ *  Used for debug display only
+ *
+ *  @param meas_type Measurement type enumeration input for string lookup
+ *
+ *  @return         Constant string representing measurement type
+ */
+static const char *
+wlan_meas_get_meas_type_str(MeasType_t meas_type)
+{
+    if (meas_type <= WLAN_MEAS_11H_MAX_TYPE)
+        return meas_type_str[meas_type];
+
+    return "Invld";
+}
+#endif
+
+/**
+ *  @brief Debug print display of the input measurement request
+ *
+ *  @param pmeas_req  Pointer to the measurement request to display
+ *
+ *  @return          N/A
+ */
+static void
+wlan_meas_dump_meas_req(const HostCmd_DS_MEASUREMENT_REQUEST * pmeas_req)
+{
+    ENTER();
+
+    PRINTM(MINFO, "Meas: Req: ------------------------------\n");
+
+    PRINTM(MINFO, "Meas: Req: mac_addr: %02x:%02x:%02x:%02x:%02x:%02x\n",
+           pmeas_req->mac_addr[0],
+           pmeas_req->mac_addr[1],
+           pmeas_req->mac_addr[2],
+           pmeas_req->mac_addr[3],
+           pmeas_req->mac_addr[4], pmeas_req->mac_addr[5]);
+
+    PRINTM(MINFO, "Meas: Req:  dlgTkn: %d\n", pmeas_req->dialog_token);
+    PRINTM(MINFO, "Meas: Req:    mode: dm[%c] rpt[%c] req[%c]\n",
+           pmeas_req->req_mode.duration_mandatory ? 'X' : ' ',
+           pmeas_req->req_mode.report ? 'X' : ' ',
+           pmeas_req->req_mode.request ? 'X' : ' ');
+    PRINTM(MINFO, "Meas: Req:        : en[%c] par[%c]\n",
+           pmeas_req->req_mode.enable ? 'X' : ' ',
+           pmeas_req->req_mode.parallel ? 'X' : ' ');
+#ifdef DEBUG_LEVEL2
+    PRINTM(MINFO, "Meas: Req: measTyp: %s\n",
+           wlan_meas_get_meas_type_str(pmeas_req->meas_type));
+#endif
+
+    switch (pmeas_req->meas_type) {
+    case WLAN_MEAS_BASIC:
+        /* Lazy cheat, fields of bas, cca, rpi union match on the request */
+        PRINTM(MINFO, "Meas: Req: chan: %u\n", pmeas_req->req.basic.channel);
+        PRINTM(MINFO, "Meas: Req: strt: %llu\n",
+               wlan_le64_to_cpu(pmeas_req->req.basic.start_time));
+        PRINTM(MINFO, "Meas: Req:  dur: %u\n",
+               wlan_le16_to_cpu(pmeas_req->req.basic.duration));
+        break;
+    default:
+        PRINTM(MINFO, "Meas: Req: <unhandled>\n");
+        break;
+    }
+
+    PRINTM(MINFO, "Meas: Req: ------------------------------\n");
+    LEAVE();
+}
+
+/**
+ *  @brief Debug print display of the input measurement report
+ *
+ *  @param pmeas_rpt  Pointer to measurement report to display
+ *
+ *  @return          N/A
+ */
+static void
+wlan_meas_dump_meas_rpt(const HostCmd_DS_MEASUREMENT_REPORT * pmeas_rpt)
+{
+    ENTER();
+
+    PRINTM(MINFO, "Meas: Rpt: ------------------------------\n");
+    PRINTM(MINFO, "Meas: Rpt: mac_addr: %02x:%02x:%02x:%02x:%02x:%02x\n",
+           pmeas_rpt->mac_addr[0],
+           pmeas_rpt->mac_addr[1],
+           pmeas_rpt->mac_addr[2],
+           pmeas_rpt->mac_addr[3],
+           pmeas_rpt->mac_addr[4], pmeas_rpt->mac_addr[5]);
+
+    PRINTM(MINFO, "Meas: Rpt:  dlgTkn: %d\n", pmeas_rpt->dialog_token);
+
+    PRINTM(MINFO, "Meas: Rpt: rptMode: (%x): Rfs[%c] ICp[%c] Lt[%c]\n",
+           *(t_u8 *) & pmeas_rpt->rpt_mode,
+           pmeas_rpt->rpt_mode.refused ? 'X' : ' ',
+           pmeas_rpt->rpt_mode.incapable ? 'X' : ' ',
+           pmeas_rpt->rpt_mode.late ? 'X' : ' ');
+#ifdef DEBUG_LEVEL2
+    PRINTM(MINFO, "Meas: Rpt: measTyp: %s\n",
+           wlan_meas_get_meas_type_str(pmeas_rpt->meas_type));
+#endif
+
+    switch (pmeas_rpt->meas_type) {
+    case WLAN_MEAS_BASIC:
+        PRINTM(MINFO, "Meas: Rpt: chan: %u\n", pmeas_rpt->rpt.basic.channel);
+        PRINTM(MINFO, "Meas: Rpt: strt: %llu\n",
+               wlan_le64_to_cpu(pmeas_rpt->rpt.basic.start_time));
+        PRINTM(MINFO, "Meas: Rpt:  dur: %u\n",
+               wlan_le16_to_cpu(pmeas_rpt->rpt.basic.duration));
+        PRINTM(MINFO, "Meas: Rpt:  bas: (%x): unmsd[%c], radar[%c]\n",
+               *(t_u8 *) & (pmeas_rpt->rpt.basic.map),
+               pmeas_rpt->rpt.basic.map.unmeasured ? 'X' : ' ',
+               pmeas_rpt->rpt.basic.map.radar ? 'X' : ' ');
+        PRINTM(MINFO, "Meas: Rpt:  bas: unidSig[%c] ofdm[%c] bss[%c]\n",
+               pmeas_rpt->rpt.basic.map.unidentified_sig ? 'X' : ' ',
+               pmeas_rpt->rpt.basic.map.ofdm_preamble ? 'X' : ' ',
+               pmeas_rpt->rpt.basic.map.bss ? 'X' : ' ');
+        break;
+    default:
+        PRINTM(MINFO, "Meas: Rpt: <unhandled>\n");
+        break;
+    }
+
+    PRINTM(MINFO, "Meas: Rpt: ------------------------------\n");
+    LEAVE();
+}
+
+/**
+ *  @brief Retrieve a measurement report from the firmware
+ *
+ *  Callback from command processing when a measurement report is received
+ *    from the firmware.  Perform the following when a report is received:
+ *
+ *   -# Debug displays the report if compiled with the appropriate flags
+ *   -# If we are pending on a specific measurement report token, and it
+ *      matches the received report's token, store the report and wake up
+ *      any pending threads
+ *
+ *  @param pmpriv Private driver information structure
+ *  @param resp HostCmd_DS_COMMAND struct returned from the firmware command
+ *              passing a HostCmd_DS_MEASUREMENT_REPORT structure.
+ *
+ *  @return     MLAN_STATUS_SUCCESS
+ */
+static int
+wlan_meas_cmdresp_get_report(mlan_private * pmpriv,
+                             const HostCmd_DS_COMMAND * resp)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    const HostCmd_DS_MEASUREMENT_REPORT *pmeas_rpt = &resp->params.meas_rpt;
+
+    ENTER();
+
+    PRINTM(MINFO, "Meas: Rpt: %#x-%u, Seq=%u, Ret=%u\n",
+           resp->command, resp->size, resp->seq_num, resp->result);
+
+    /* Debug displays the measurement report */
+    wlan_meas_dump_meas_rpt(pmeas_rpt);
+
+    /*
+     * Check if we are pending on a measurement report and it matches
+     *  the dialog token of the received report:
+     */
+    if (pmadapter->state_meas.meas_rpt_pend_on
+        && pmadapter->state_meas.meas_rpt_pend_on == pmeas_rpt->dialog_token) {
+        PRINTM(MINFO, "Meas: Rpt: RCV'd Pend on meas #%d\n",
+               pmadapter->state_meas.meas_rpt_pend_on);
+
+        /* Clear the pending report indicator */
+        pmadapter->state_meas.meas_rpt_pend_on = 0;
+
+        /* Copy the received report into the measurement state for retrieval */
+        memcpy(pmadapter, &pmadapter->state_meas.meas_rpt_returned, pmeas_rpt,
+               sizeof(pmadapter->state_meas.meas_rpt_returned));
+
+        /*
+         * Wake up any threads pending on the wait queue
+         */
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_DRV_MEAS_REPORT, MNULL);
+    }
+
+    LEAVE();
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Prepare CMD_MEASURMENT_REPORT firmware command
+ *
+ *  @param pmpriv     Private driver information structure
+ *  @param pcmd_ptr   Output parameter: Pointer to the command being prepared
+ *                    for the firmware
+ *  @param pinfo_buf  HostCmd_DS_MEASUREMENT_REQUEST passed as void data block
+ *
+ *  @return          MLAN_STATUS_SUCCESS
+ */
+static int
+wlan_meas_cmd_request(mlan_private * pmpriv,
+                      HostCmd_DS_COMMAND * pcmd_ptr, const void *pinfo_buf)
+{
+    const HostCmd_DS_MEASUREMENT_REQUEST *pmeas_req =
+        (HostCmd_DS_MEASUREMENT_REQUEST *) pinfo_buf;
+
+    ENTER();
+
+    pcmd_ptr->command = HostCmd_CMD_MEASUREMENT_REQUEST;
+    pcmd_ptr->size = sizeof(HostCmd_DS_MEASUREMENT_REQUEST) + S_DS_GEN;
+
+    memcpy(pmpriv->adapter, &pcmd_ptr->params.meas_req, pmeas_req,
+           sizeof(pcmd_ptr->params.meas_req));
+
+    PRINTM(MINFO, "Meas: Req: %#x-%u, Seq=%u, Ret=%u\n",
+           pcmd_ptr->command, pcmd_ptr->size, pcmd_ptr->seq_num,
+           pcmd_ptr->result);
+
+    wlan_meas_dump_meas_req(pmeas_req);
+
+    LEAVE();
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief  Retrieve a measurement report from the firmware
+ *
+ *  The firmware will send a EVENT_MEAS_REPORT_RDY event when it
+ *    completes or receives a measurement report.  The event response
+ *    handler will then start a HostCmd_CMD_MEASUREMENT_REPORT firmware command
+ *    which gets completed for transmission to the firmware in this routine.
+ *
+ *  @param pmpriv    Private driver information structure
+ *  @param pcmd_ptr  Output parameter: Pointer to the command being prepared
+ *                   for the firmware
+ *
+ *  @return        MLAN_STATUS_SUCCESS
+ */
+static int
+wlan_meas_cmd_get_report(mlan_private * pmpriv, HostCmd_DS_COMMAND * pcmd_ptr)
+{
+    ENTER();
+
+    pcmd_ptr->command = HostCmd_CMD_MEASUREMENT_REPORT;
+    pcmd_ptr->size = sizeof(HostCmd_DS_MEASUREMENT_REPORT) + S_DS_GEN;
+
+    memset(pmpriv->adapter, &pcmd_ptr->params.meas_rpt, 0x00,
+           sizeof(pcmd_ptr->params.meas_rpt));
+
+    /*
+     * Set the meas_rpt.mac_addr to our mac address to get a meas report,
+     *   setting the mac to another STA address instructs the firmware
+     *   to transmit this measurement report frame instead
+     */
+    memcpy(pmpriv->adapter, pcmd_ptr->params.meas_rpt.mac_addr,
+           pmpriv->curr_addr, sizeof(pcmd_ptr->params.meas_rpt.mac_addr));
+
+    LEAVE();
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/********************************************************
+                Global functions
+********************************************************/
+
+/**
+ *  @brief Send the input measurement request to the firmware.
+ *
+ *  If the dialog token in the measurement request is set to 0, the function
+ *    will use an local static auto-incremented token in the measurement
+ *    request.  This ensures the dialog token is always set.
+ *
+ *  If wait_for_resp_timeout is set, the function will block its return on
+ *     a timeout or returned measurement report that matches the requests
+ *     dialog token.
+ *
+ *  @param pmpriv                  Private driver information structure
+ *  @param pmeas_req               Pointer to the measurement request to send
+ *  @param wait_for_resp_timeout   Timeout value of the measurement request
+ *                                 in ms.
+ *  @param pioctl_req              Pointer to IOCTL request buffer
+ *  @param pmeas_rpt               Output parameter: Pointer for the resulting
+ *                                 measurement report
+ *
+ *  @return
+ *    - 0 for success
+ *    - -ETIMEDOUT if the measurement report does not return before
+ *      the timeout expires
+ *    - Error return from wlan_prepare_cmd routine otherwise
+ */
+int
+wlan_meas_util_send_req(mlan_private * pmpriv,
+                        HostCmd_DS_MEASUREMENT_REQUEST * pmeas_req,
+                        t_u32 wait_for_resp_timeout, pmlan_ioctl_req pioctl_req,
+                        HostCmd_DS_MEASUREMENT_REPORT * pmeas_rpt)
+{
+    static t_u8 auto_dialog_tok = 0;
+    wlan_meas_state_t *pmeas_state = &pmpriv->adapter->state_meas;
+    int ret;
+
+    ENTER();
+
+    /* If dialogTok was set to 0 or not provided, autoset */
+    pmeas_req->dialog_token = (pmeas_req->dialog_token ?
+                               pmeas_req->dialog_token : ++auto_dialog_tok);
+
+    /* Check for rollover of the dialog token.  Avoid using 0 as a token */
+    pmeas_req->dialog_token = (pmeas_req->dialog_token ?
+                               pmeas_req->dialog_token : 1);
+
+    /*
+     * If the request is to pend waiting for the result, set the dialog token
+     * of this measurement request in the state structure.  The measurement
+     * report handling routines can then check the incoming measurement
+     * reports for a match with this dialog token.
+     */
+    if (wait_for_resp_timeout) {
+        pmeas_state->meas_rpt_pend_on = pmeas_req->dialog_token;
+        PRINTM(MINFO, "Meas: Req: START Pend on meas #%d\n",
+               pmeas_req->dialog_token);
+    }
+
+    /* Send the measurement request to the firmware */
+    ret = wlan_prepare_cmd(pmpriv, HostCmd_CMD_MEASUREMENT_REQUEST,
+                           HostCmd_ACT_GEN_SET,
+                           0, (t_void *) pioctl_req, (void *) pmeas_req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief  Prepare the HostCmd_DS_Command structure for a measurement command.
+ *
+ *  Use the Command field to determine if the command being set up is for
+ *     11h and call one of the local command handlers accordingly for:
+ *
+ *        - HostCmd_CMD_MEASUREMENT_REQUEST
+ *        - HostCmd_CMD_MEASUREMENT_REPORT
+ *
+ *  @param pmpriv     Private driver information structure
+ *  @param pcmd_ptr   Output parameter: Pointer to the command being prepared
+ *                    for the firmware
+ *  @param pinfo_buf  Void buffer passthrough with data necessary for a
+ *                    specific command type
+ *
+ *  @return         MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ *
+ */
+int
+wlan_meas_cmd_process(mlan_private * pmpriv,
+                      HostCmd_DS_COMMAND * pcmd_ptr, const void *pinfo_buf)
+{
+    int ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    switch (pcmd_ptr->command) {
+    case HostCmd_CMD_MEASUREMENT_REQUEST:
+        ret = wlan_meas_cmd_request(pmpriv, pcmd_ptr, pinfo_buf);
+        break;
+    case HostCmd_CMD_MEASUREMENT_REPORT:
+        ret = wlan_meas_cmd_get_report(pmpriv, pcmd_ptr);
+        break;
+    default:
+        ret = MLAN_STATUS_FAILURE;
+    }
+
+    pcmd_ptr->command = wlan_cpu_to_le16(pcmd_ptr->command);
+    pcmd_ptr->size = wlan_cpu_to_le16(pcmd_ptr->size);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Handle the command response from the firmware for a measurement
+ *         command
+ *
+ *  Use the Command field to determine if the command response being
+ *    is for meas.  Call the local command response handler accordingly for:
+ *
+ *        - HostCmd_CMD_802_MEASUREMENT_REQUEST
+ *        - HostCmd_CMD_802_MEASUREMENT_REPORT
+ *
+ *  @param pmpriv Private driver information structure
+ *  @param resp   HostCmd_DS_COMMAND struct returned from the firmware command
+ *
+ *  @return     MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+int
+wlan_meas_cmdresp_process(mlan_private * pmpriv,
+                          const HostCmd_DS_COMMAND * resp)
+{
+    int ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    switch (resp->command) {
+    case HostCmd_CMD_MEASUREMENT_REQUEST:
+        PRINTM(MINFO, "Meas: Req Resp: Sz=%u, Seq=%u, Ret=%u\n",
+               resp->size, resp->seq_num, resp->result);
+        break;
+    case HostCmd_CMD_MEASUREMENT_REPORT:
+        ret = wlan_meas_cmdresp_get_report(pmpriv, resp);
+        break;
+    default:
+        ret = MLAN_STATUS_FAILURE;
+    }
+
+    LEAVE();
+    return ret;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_meas.h b/drivers/net/wireless/sd8797/mlan/mlan_meas.h
new file mode 100644
index 000000000000..0eb51d3a3bb2
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_meas.h
@@ -0,0 +1,54 @@
+/**
+ *  @file mlan_meas.h
+ *
+ *  @brief Interface for the measurement module implemented in mlan_meas.c
+ *
+ *  Driver interface functions and type declarations for the measurement module
+ *    implemented in mlan_meas.c
+ *  
+ *  @sa mlan_meas.c
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *  
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *  
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ *
+ */
+
+/*************************************************************
+Change Log:
+    03/25/2009: initial version
+************************************************************/
+
+#ifndef _MLAN_MEAS_H_
+#define _MLAN_MEAS_H_
+
+#include  "mlan_fw.h"
+
+/* Send a given measurement request to the firmware, report back the result */
+extern int
+wlan_meas_util_send_req(mlan_private * pmpriv,
+                        HostCmd_DS_MEASUREMENT_REQUEST * pmeas_req,
+                        t_u32 wait_for_resp_timeout, pmlan_ioctl_req pioctl_req,
+                        HostCmd_DS_MEASUREMENT_REPORT * pmeas_rpt);
+
+/* Setup a measurement command before it is sent to the firmware */
+extern int wlan_meas_cmd_process(mlan_private * pmpriv,
+                                 HostCmd_DS_COMMAND * pcmd_ptr,
+                                 const t_void * pinfo_buf);
+
+/* Handle a given measurement command response from the firmware */
+extern int wlan_meas_cmdresp_process(mlan_private * pmpriv,
+                                     const HostCmd_DS_COMMAND * resp);
+
+#endif /* _MLAN_MEAS_H_ */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_misc.c b/drivers/net/wireless/sd8797/mlan/mlan_misc.c
new file mode 100644
index 000000000000..f41dcea3d206
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_misc.c
@@ -0,0 +1,2156 @@
+/**
+ * @file mlan_misc.c
+ *
+ *  @brief This file include miscellaneous functions for MLAN module
+ *
+ *  Copyright (C) 2009-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *  
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/*************************************************************
+Change Log:
+    05/11/2009: initial version
+************************************************************/
+#include "mlan.h"
+#ifdef STA_SUPPORT
+#include "mlan_join.h"
+#endif /* STA_SUPPORT */
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_wmm.h"
+#include "mlan_11n.h"
+#include "mlan_sdio.h"
+#ifdef UAP_SUPPORT
+#include "mlan_uap.h"
+#endif
+
+/********************************************************
+                Local Variables
+********************************************************/
+
+/********************************************************
+                Global Variables
+********************************************************/
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+extern mlan_operations *mlan_ops[];
+#endif
+extern t_u8 ac_to_tid[4][2];
+
+/********************************************************
+                Local Functions
+********************************************************/
+
+/** Custom IE auto index and mask */
+#define	MLAN_CUSTOM_IE_AUTO_IDX_MASK	0xffff
+/** Custom IE mask for delete operation */
+#define	MLAN_CUSTOM_IE_DELETE_MASK      0
+/** Custom IE header size */
+#define	MLAN_CUSTOM_IE_HDR_SIZE         (sizeof(custom_ie)-MAX_IE_SIZE)
+
+/**
+ *  @brief Check if current custom IE index is used on other interfaces.
+ *
+ *  @param pmpriv   A pointer to mlan_private structure
+ *  @param idx		index to check for in use
+ *
+ *  @return		MLAN_STATUS_SUCCESS --unused, otherwise used.
+ */
+static mlan_status
+wlan_is_custom_ie_index_unused(IN pmlan_private pmpriv, IN t_u16 idx)
+{
+    t_u8 i = 0;
+    pmlan_adapter pmadapter = pmpriv->adapter;
+    pmlan_private priv;
+    ENTER();
+
+    for (i = 0; i < pmadapter->priv_num; i++) {
+        priv = pmadapter->priv[i];
+        /* Check for other interfaces only */
+        if (priv && priv->bss_index != pmpriv->bss_index) {
+
+            if (priv->mgmt_ie[idx].mgmt_subtype_mask &&
+                priv->mgmt_ie[idx].ie_length) {
+                /* used entry found */
+                LEAVE();
+                return MLAN_STATUS_FAILURE;
+            }
+        }
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Get the custom IE index
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *  @param mask	mask value for which the index to be returned
+ *  @param ie_data	a pointer to custom_ie structure
+ *  @param idx		will hold the computed index
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_custom_ioctl_get_autoidx(IN pmlan_private pmpriv,
+                              IN pmlan_ioctl_req pioctl_req,
+                              IN t_u16 mask,
+                              IN custom_ie * ie_data, OUT t_u16 * idx)
+{
+    t_u16 index = 0, insert = MFALSE;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    /* Determine the index where the IE needs to be inserted */
+    while (!insert) {
+        while (index < pmpriv->adapter->max_mgmt_ie_index) {
+            if (pmpriv->mgmt_ie[index].mgmt_subtype_mask ==
+                MLAN_CUSTOM_IE_AUTO_IDX_MASK) {
+                index++;
+                continue;
+            }
+            if (pmpriv->mgmt_ie[index].mgmt_subtype_mask == mask) {
+                /* Duplicate IE should be avoided */
+                if (pmpriv->mgmt_ie[index].ie_length) {
+                    if (!memcmp
+                        (pmpriv->adapter, pmpriv->mgmt_ie[index].ie_buffer,
+                         ie_data->ie_buffer,
+                         pmpriv->mgmt_ie[index].ie_length)) {
+                        PRINTM(MERROR,
+                               "IE with the same mask exists at index %d\n",
+                               index);
+                        *idx = MLAN_CUSTOM_IE_AUTO_IDX_MASK;
+                        goto done;
+                    }
+                }
+                /* Check if enough space is available */
+                if (pmpriv->mgmt_ie[index].ie_length + ie_data->ie_length >
+                    MAX_IE_SIZE) {
+                    index++;
+                    continue;
+                }
+                insert = MTRUE;
+                break;
+            }
+            index++;
+        }
+        if (!insert) {
+            for (index = 0; index < pmpriv->adapter->max_mgmt_ie_index; index++) {
+                if (pmpriv->mgmt_ie[index].ie_length == 0) {
+                    /*
+                     * Check if this index is in use by other interface
+                     * If yes, move ahead to next index
+                     */
+                    if (MLAN_STATUS_SUCCESS ==
+                        wlan_is_custom_ie_index_unused(pmpriv, index)) {
+                        insert = MTRUE;
+                        break;
+                    } else {
+                        PRINTM(MINFO, "Skipping IE index %d in use.\n", index);
+                    }
+                }
+            }
+        }
+        if (index == pmpriv->adapter->max_mgmt_ie_index && !insert) {
+            PRINTM(MERROR, "Failed to Set the IE buffer\n");
+            if (pioctl_req)
+                pioctl_req->status_code = MLAN_ERROR_IOCTL_FAIL;
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+    }
+
+    *idx = index;
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Delete custom IE
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *  @param ie_data	a pointer to custom_ie structure
+ *  @param idx		index supplied
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+
+static mlan_status
+wlan_custom_ioctl_auto_delete(IN pmlan_private pmpriv,
+                              IN pmlan_ioctl_req pioctl_req,
+                              IN custom_ie * ie_data, IN t_u16 idx)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_adapter pmadapter = pmpriv->adapter;
+    t_u16 index = 0, insert = MFALSE, del_len;
+    t_u8 del_ie[MAX_IE_SIZE], ie[MAX_IE_SIZE];
+    t_s32 cnt, tmp_len = 0;
+    t_u8 *tmp_ie;
+
+    ENTER();
+    memset(pmpriv->adapter, del_ie, 0, MAX_IE_SIZE);
+    memcpy(pmpriv->adapter, del_ie, ie_data->ie_buffer,
+           MIN(MAX_IE_SIZE, ie_data->ie_length));
+    del_len = MIN(MAX_IE_SIZE, ie_data->ie_length);
+
+    for (index = 0; index < pmadapter->max_mgmt_ie_index; index++) {
+        if (MLAN_CUSTOM_IE_AUTO_IDX_MASK != idx)
+            index = idx;
+        tmp_ie = pmpriv->mgmt_ie[index].ie_buffer;
+        tmp_len = pmpriv->mgmt_ie[index].ie_length;
+        cnt = 0;
+        while (tmp_len) {
+            if (!memcmp(pmpriv->adapter, tmp_ie, del_ie, del_len)) {
+                memcpy(pmpriv->adapter, ie, pmpriv->mgmt_ie[index].ie_buffer,
+                       cnt);
+                if (pmpriv->mgmt_ie[index].ie_length > (cnt + del_len))
+                    memcpy(pmpriv->adapter, &ie[cnt],
+                           &pmpriv->mgmt_ie[index].ie_buffer[cnt + del_len],
+                           (pmpriv->mgmt_ie[index].ie_length -
+                            (cnt + del_len)));
+                memset(pmpriv->adapter, &pmpriv->mgmt_ie[index].ie_buffer, 0,
+                       sizeof(pmpriv->mgmt_ie[index].ie_buffer));
+                memcpy(pmpriv->adapter, &pmpriv->mgmt_ie[index].ie_buffer, ie,
+                       pmpriv->mgmt_ie[index].ie_length - del_len);
+                pmpriv->mgmt_ie[index].ie_length -= del_len;
+                insert = MTRUE;
+                tmp_ie = pmpriv->mgmt_ie[index].ie_buffer;
+                tmp_len = pmpriv->mgmt_ie[index].ie_length;
+                cnt = 0;
+                continue;
+            }
+            tmp_ie++;
+            tmp_len--;
+            cnt++;
+        }
+        if (MLAN_CUSTOM_IE_AUTO_IDX_MASK != idx)
+            break;
+    }
+    if (index == pmadapter->max_mgmt_ie_index && !insert) {
+        PRINTM(MERROR, "Failed to Clear IE buffer\n");
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_FAIL;
+        ret = MLAN_STATUS_FAILURE;
+    }
+    LEAVE();
+    return ret;
+}
+
+/********************************************************
+                Global Functions
+********************************************************/
+
+/**
+ *  @brief send host cmd
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_misc_ioctl_host_cmd(IN pmlan_adapter pmadapter,
+                         IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_misc_cfg *misc = MNULL;
+
+    ENTER();
+
+    misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           0,
+                           0,
+                           0,
+                           (t_void *) pioctl_req,
+                           (t_void *) & misc->param.hostcmd);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Send function init/shutdown command to firmware
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_misc_ioctl_init_shutdown(IN pmlan_adapter pmadapter,
+                              IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_misc_cfg *misc_cfg = MNULL;
+    t_u16 cmd;
+
+    ENTER();
+
+    misc_cfg = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    if (misc_cfg->param.func_init_shutdown == MLAN_FUNC_INIT)
+        cmd = HostCmd_CMD_FUNC_INIT;
+    else if (misc_cfg->param.func_init_shutdown == MLAN_FUNC_SHUTDOWN)
+        cmd = HostCmd_CMD_FUNC_SHUTDOWN;
+    else {
+        PRINTM(MERROR, "Unsupported parameter\n");
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        ret = MLAN_STATUS_FAILURE;
+        goto exit;
+    }
+
+    /* Send command to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           cmd,
+                           HostCmd_ACT_GEN_SET,
+                           0, (t_void *) pioctl_req, MNULL);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get debug information
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+mlan_status
+wlan_get_info_debug_info(IN pmlan_adapter pmadapter,
+                         IN pmlan_ioctl_req pioctl_req)
+{
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_get_info *info;
+    t_u8 *ptid;
+
+    ENTER();
+
+    info = (mlan_ds_get_info *) pioctl_req->pbuf;
+
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        pmadapter->max_tx_buf_size =
+            (t_u16) info->param.debug_info.max_tx_buf_size;
+        pmadapter->tx_buf_size = (t_u16) info->param.debug_info.tx_buf_size;
+        pmadapter->curr_tx_buf_size =
+            (t_u16) info->param.debug_info.curr_tx_buf_size;
+        pmadapter->ps_mode = info->param.debug_info.ps_mode;
+        pmadapter->ps_state = info->param.debug_info.ps_state;
+#ifdef STA_SUPPORT
+        pmadapter->is_deep_sleep = info->param.debug_info.is_deep_sleep;
+#endif /* STA_SUPPORT */
+        pmadapter->pm_wakeup_card_req =
+            info->param.debug_info.pm_wakeup_card_req;
+        pmadapter->pm_wakeup_fw_try = info->param.debug_info.pm_wakeup_fw_try;
+        pmadapter->is_hs_configured = info->param.debug_info.is_hs_configured;
+        pmadapter->hs_activated = info->param.debug_info.hs_activated;
+        pmadapter->pps_uapsd_mode = info->param.debug_info.pps_uapsd_mode;
+        pmadapter->sleep_period.period = info->param.debug_info.sleep_pd;
+        pmpriv->wmm_qosinfo = info->param.debug_info.qos_cfg;
+        pmadapter->tx_lock_flag = info->param.debug_info.tx_lock_flag;
+        pmpriv->port_open = info->param.debug_info.port_open;
+        pmadapter->scan_processing = info->param.debug_info.scan_processing;
+
+        pmadapter->dbg.num_cmd_host_to_card_failure =
+            info->param.debug_info.num_cmd_host_to_card_failure;
+        pmadapter->dbg.num_cmd_sleep_cfm_host_to_card_failure =
+            info->param.debug_info.num_cmd_sleep_cfm_host_to_card_failure;
+        pmadapter->dbg.num_tx_host_to_card_failure =
+            info->param.debug_info.num_tx_host_to_card_failure;
+        pmadapter->dbg.num_cmdevt_card_to_host_failure =
+            info->param.debug_info.num_cmdevt_card_to_host_failure;
+        pmadapter->dbg.num_rx_card_to_host_failure =
+            info->param.debug_info.num_rx_card_to_host_failure;
+        pmadapter->dbg.num_int_read_failure =
+            info->param.debug_info.num_int_read_failure;
+        pmadapter->dbg.last_int_status = info->param.debug_info.last_int_status;
+        pmadapter->dbg.num_event_deauth =
+            info->param.debug_info.num_event_deauth;
+        pmadapter->dbg.num_event_disassoc =
+            info->param.debug_info.num_event_disassoc;
+        pmadapter->dbg.num_event_link_lost =
+            info->param.debug_info.num_event_link_lost;
+        pmadapter->dbg.num_cmd_deauth = info->param.debug_info.num_cmd_deauth;
+        pmadapter->dbg.num_cmd_assoc_success =
+            info->param.debug_info.num_cmd_assoc_success;
+        pmadapter->dbg.num_cmd_assoc_failure =
+            info->param.debug_info.num_cmd_assoc_failure;
+        pmadapter->dbg.num_tx_timeout = info->param.debug_info.num_tx_timeout;
+        pmadapter->dbg.num_cmd_timeout = info->param.debug_info.num_cmd_timeout;
+        pmadapter->dbg.timeout_cmd_id = info->param.debug_info.timeout_cmd_id;
+        pmadapter->dbg.timeout_cmd_act = info->param.debug_info.timeout_cmd_act;
+        memcpy(pmadapter, pmadapter->dbg.last_cmd_id,
+               info->param.debug_info.last_cmd_id,
+               sizeof(pmadapter->dbg.last_cmd_id));
+        memcpy(pmadapter, pmadapter->dbg.last_cmd_act,
+               info->param.debug_info.last_cmd_act,
+               sizeof(pmadapter->dbg.last_cmd_act));
+        pmadapter->dbg.last_cmd_index = info->param.debug_info.last_cmd_index;
+        memcpy(pmadapter, pmadapter->dbg.last_cmd_resp_id,
+               info->param.debug_info.last_cmd_resp_id,
+               sizeof(pmadapter->dbg.last_cmd_resp_id));
+        pmadapter->dbg.last_cmd_resp_index =
+            info->param.debug_info.last_cmd_resp_index;
+        memcpy(pmadapter, pmadapter->dbg.last_event,
+               info->param.debug_info.last_event,
+               sizeof(pmadapter->dbg.last_event));
+        pmadapter->dbg.last_event_index =
+            info->param.debug_info.last_event_index;
+
+        pmadapter->data_sent = info->param.debug_info.data_sent;
+        pmadapter->cmd_sent = info->param.debug_info.cmd_sent;
+        pmadapter->mp_rd_bitmap = info->param.debug_info.mp_rd_bitmap;
+        pmadapter->mp_wr_bitmap = info->param.debug_info.mp_wr_bitmap;
+        pmadapter->curr_rd_port = info->param.debug_info.curr_rd_port;
+        pmadapter->curr_wr_port = info->param.debug_info.curr_wr_port;
+        pmadapter->cmd_resp_received = info->param.debug_info.cmd_resp_received;
+#ifdef UAP_SUPPORT
+        pmadapter->pending_bridge_pkts = info->param.debug_info.num_bridge_pkts;
+        pmpriv->num_drop_pkts = info->param.debug_info.num_drop_pkts;
+#endif
+    } else {                    /* MLAN_ACT_GET */
+        ptid = ac_to_tid[WMM_AC_BK];
+        info->param.debug_info.wmm_ac_bk =
+            pmpriv->wmm.packets_out[ptid[0]] + pmpriv->wmm.packets_out[ptid[1]];
+        ptid = ac_to_tid[WMM_AC_BE];
+        info->param.debug_info.wmm_ac_be =
+            pmpriv->wmm.packets_out[ptid[0]] + pmpriv->wmm.packets_out[ptid[1]];
+        ptid = ac_to_tid[WMM_AC_VI];
+        info->param.debug_info.wmm_ac_vi =
+            pmpriv->wmm.packets_out[ptid[0]] + pmpriv->wmm.packets_out[ptid[1]];
+        ptid = ac_to_tid[WMM_AC_VO];
+        info->param.debug_info.wmm_ac_vo =
+            pmpriv->wmm.packets_out[ptid[0]] + pmpriv->wmm.packets_out[ptid[1]];
+        info->param.debug_info.max_tx_buf_size =
+            (t_u32) pmadapter->max_tx_buf_size;
+        info->param.debug_info.tx_buf_size = (t_u32) pmadapter->tx_buf_size;
+        info->param.debug_info.curr_tx_buf_size =
+            (t_u32) pmadapter->curr_tx_buf_size;
+        info->param.debug_info.rx_tbl_num =
+            wlan_get_rxreorder_tbl(pmpriv, info->param.debug_info.rx_tbl);
+        info->param.debug_info.tx_tbl_num =
+            wlan_get_txbastream_tbl(pmpriv, info->param.debug_info.tx_tbl);
+        info->param.debug_info.ps_mode = pmadapter->ps_mode;
+        info->param.debug_info.ps_state = pmadapter->ps_state;
+#ifdef STA_SUPPORT
+        info->param.debug_info.is_deep_sleep = pmadapter->is_deep_sleep;
+#endif /* STA_SUPPORT */
+        info->param.debug_info.pm_wakeup_card_req =
+            pmadapter->pm_wakeup_card_req;
+        info->param.debug_info.pm_wakeup_fw_try = pmadapter->pm_wakeup_fw_try;
+        info->param.debug_info.is_hs_configured = pmadapter->is_hs_configured;
+        info->param.debug_info.hs_activated = pmadapter->hs_activated;
+        info->param.debug_info.pps_uapsd_mode = pmadapter->pps_uapsd_mode;
+        info->param.debug_info.sleep_pd = pmadapter->sleep_period.period;
+        info->param.debug_info.qos_cfg = pmpriv->wmm_qosinfo;
+        info->param.debug_info.tx_lock_flag = pmadapter->tx_lock_flag;
+        info->param.debug_info.port_open = pmpriv->port_open;
+        info->param.debug_info.scan_processing = pmadapter->scan_processing;
+
+        info->param.debug_info.num_cmd_host_to_card_failure
+            = pmadapter->dbg.num_cmd_host_to_card_failure;
+        info->param.debug_info.num_cmd_sleep_cfm_host_to_card_failure
+            = pmadapter->dbg.num_cmd_sleep_cfm_host_to_card_failure;
+        info->param.debug_info.num_tx_host_to_card_failure
+            = pmadapter->dbg.num_tx_host_to_card_failure;
+        info->param.debug_info.num_cmdevt_card_to_host_failure
+            = pmadapter->dbg.num_cmdevt_card_to_host_failure;
+        info->param.debug_info.num_rx_card_to_host_failure
+            = pmadapter->dbg.num_rx_card_to_host_failure;
+        info->param.debug_info.num_int_read_failure =
+            pmadapter->dbg.num_int_read_failure;
+        info->param.debug_info.last_int_status = pmadapter->dbg.last_int_status;
+        info->param.debug_info.num_event_deauth =
+            pmadapter->dbg.num_event_deauth;
+        info->param.debug_info.num_event_disassoc =
+            pmadapter->dbg.num_event_disassoc;
+        info->param.debug_info.num_event_link_lost =
+            pmadapter->dbg.num_event_link_lost;
+        info->param.debug_info.num_cmd_deauth = pmadapter->dbg.num_cmd_deauth;
+        info->param.debug_info.num_cmd_assoc_success =
+            pmadapter->dbg.num_cmd_assoc_success;
+        info->param.debug_info.num_cmd_assoc_failure =
+            pmadapter->dbg.num_cmd_assoc_failure;
+        info->param.debug_info.num_tx_timeout = pmadapter->dbg.num_tx_timeout;
+        info->param.debug_info.num_cmd_timeout = pmadapter->dbg.num_cmd_timeout;
+        info->param.debug_info.timeout_cmd_id = pmadapter->dbg.timeout_cmd_id;
+        info->param.debug_info.timeout_cmd_act = pmadapter->dbg.timeout_cmd_act;
+        memcpy(pmadapter, info->param.debug_info.last_cmd_id,
+               pmadapter->dbg.last_cmd_id, sizeof(pmadapter->dbg.last_cmd_id));
+        memcpy(pmadapter, info->param.debug_info.last_cmd_act,
+               pmadapter->dbg.last_cmd_act,
+               sizeof(pmadapter->dbg.last_cmd_act));
+        info->param.debug_info.last_cmd_index = pmadapter->dbg.last_cmd_index;
+        memcpy(pmadapter, info->param.debug_info.last_cmd_resp_id,
+               pmadapter->dbg.last_cmd_resp_id,
+               sizeof(pmadapter->dbg.last_cmd_resp_id));
+        info->param.debug_info.last_cmd_resp_index =
+            pmadapter->dbg.last_cmd_resp_index;
+        memcpy(pmadapter, info->param.debug_info.last_event,
+               pmadapter->dbg.last_event, sizeof(pmadapter->dbg.last_event));
+        info->param.debug_info.last_event_index =
+            pmadapter->dbg.last_event_index;
+
+        info->param.debug_info.mp_rd_bitmap = pmadapter->mp_rd_bitmap;
+        info->param.debug_info.mp_wr_bitmap = pmadapter->mp_wr_bitmap;
+        info->param.debug_info.curr_rd_port = pmadapter->curr_rd_port;
+        info->param.debug_info.curr_wr_port = pmadapter->curr_wr_port;
+        info->param.debug_info.data_sent = pmadapter->data_sent;
+        info->param.debug_info.cmd_sent = pmadapter->cmd_sent;
+        info->param.debug_info.cmd_resp_received = pmadapter->cmd_resp_received;
+        info->param.debug_info.tx_pkts_queued =
+            util_scalar_read(pmadapter->pmoal_handle,
+                             &pmpriv->wmm.tx_pkts_queued, MNULL, MNULL);
+#ifdef UAP_SUPPORT
+        info->param.debug_info.num_bridge_pkts = pmadapter->pending_bridge_pkts;
+        info->param.debug_info.num_drop_pkts = pmpriv->num_drop_pkts;
+#endif
+    }
+
+    pioctl_req->data_read_written =
+        sizeof(mlan_debug_info) + MLAN_SUB_COMMAND_SIZE;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function wakes up the card.
+ *
+ *  @param pmadapter		A pointer to mlan_adapter structure
+ *
+ *  @return			MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_pm_wakeup_card(IN pmlan_adapter pmadapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+
+    ENTER();
+    PRINTM(MEVENT, "Wakeup device...\n");
+    ret =
+        pcb->moal_write_reg(pmadapter->pmoal_handle, HOST_TO_CARD_EVENT_REG,
+                            HOST_POWER_UP);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function resets the PM setting of the card.
+ *
+ *  @param pmadapter		A pointer to mlan_adapter structure
+ *
+ *  @return			MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_pm_reset_card(IN pmlan_adapter pmadapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+
+    ENTER();
+
+    ret =
+        pcb->moal_write_reg(pmadapter->pmoal_handle, HOST_TO_CARD_EVENT_REG, 0);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get HS configuration
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_pm_ioctl_hscfg(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_pm_cfg *pm = MNULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    t_u32 prev_cond = 0;
+
+    ENTER();
+
+    pm = (mlan_ds_pm_cfg *) pioctl_req->pbuf;
+
+    switch (pioctl_req->action) {
+    case MLAN_ACT_SET:
+#ifdef STA_SUPPORT
+        if (pmadapter->pps_uapsd_mode) {
+            PRINTM(MINFO, "Host Sleep IOCTL is blocked in UAPSD/PPS mode\n");
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+            status = MLAN_STATUS_FAILURE;
+            break;
+        }
+#endif /* STA_SUPPORT */
+        if (pm->param.hs_cfg.is_invoke_hostcmd == MTRUE) {
+            if (pm->param.hs_cfg.conditions == HOST_SLEEP_CFG_CANCEL) {
+                if (pmadapter->is_hs_configured == MFALSE) {
+                    /* Already cancelled */
+                    break;
+                }
+                /* Save previous condition */
+                prev_cond = pmadapter->hs_cfg.conditions;
+                pmadapter->hs_cfg.conditions = pm->param.hs_cfg.conditions;
+            } else if (pmadapter->hs_cfg.conditions == HOST_SLEEP_CFG_CANCEL) {
+                /* Return failure if no parameters for HS enable */
+                pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+                status = MLAN_STATUS_FAILURE;
+                break;
+            }
+            status = wlan_prepare_cmd(pmpriv,
+                                      HostCmd_CMD_802_11_HS_CFG_ENH,
+                                      HostCmd_ACT_GEN_SET,
+                                      0, (t_void *) pioctl_req,
+                                      (t_void *) (&pmadapter->hs_cfg));
+            if (status == MLAN_STATUS_SUCCESS)
+                status = MLAN_STATUS_PENDING;
+            if (pm->param.hs_cfg.conditions == HOST_SLEEP_CFG_CANCEL) {
+                /* Restore previous condition */
+                pmadapter->hs_cfg.conditions = prev_cond;
+            }
+        } else {
+            pmadapter->hs_cfg.conditions = pm->param.hs_cfg.conditions;
+            pmadapter->hs_cfg.gpio = (t_u8) pm->param.hs_cfg.gpio;
+            pmadapter->hs_cfg.gap = (t_u8) pm->param.hs_cfg.gap;
+        }
+        break;
+    case MLAN_ACT_GET:
+        pm->param.hs_cfg.conditions = pmadapter->hs_cfg.conditions;
+        pm->param.hs_cfg.gpio = pmadapter->hs_cfg.gpio;
+        pm->param.hs_cfg.gap = pmadapter->hs_cfg.gap;
+        break;
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief This function allocates a mlan_buffer.
+ *
+ *  @param pmadapter Pointer to mlan_adapter
+ *  @param data_len   Data length
+ *  @param head_room  head_room reserved in mlan_buffer
+ *  @param malloc_flag  flag to user moal_malloc
+ *  @return           mlan_buffer pointer or MNULL
+ */
+pmlan_buffer
+wlan_alloc_mlan_buffer(mlan_adapter * pmadapter, t_u32 data_len,
+                       t_u32 head_room, t_u32 malloc_flag)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_buffer pmbuf = MNULL;
+    t_u32 buf_size = 0;
+    t_u8 *tmp_buf = MNULL;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+
+    ENTER();
+
+    /* make sure that the data length is at least SDIO block size */
+    data_len = ALIGN_SZ(data_len, MLAN_SDIO_BLOCK_SIZE);
+
+    /* head_room is not implemented for malloc mlan buffer */
+    if (malloc_flag == MTRUE) {
+        buf_size = sizeof(mlan_buffer) + data_len + DMA_ALIGNMENT;
+        ret =
+            pcb->moal_malloc(pmadapter->pmoal_handle, buf_size,
+                             MLAN_MEM_DEF | MLAN_MEM_DMA, (t_u8 **) & pmbuf);
+        if ((ret != MLAN_STATUS_SUCCESS) || !pmbuf) {
+            pmbuf = MNULL;
+            goto exit;
+        }
+        memset(pmadapter, pmbuf, 0, sizeof(mlan_buffer));
+
+        pmbuf->pdesc = MNULL;
+        /* Align address */
+        pmbuf->pbuf =
+            (t_u8 *) ALIGN_ADDR((t_u8 *) pmbuf + sizeof(mlan_buffer),
+                                DMA_ALIGNMENT);
+        pmbuf->data_offset = 0;
+        pmbuf->data_len = data_len;
+        pmbuf->flags |= MLAN_BUF_FLAG_MALLOC_BUF;
+    } else {
+        /* use moal_alloc_mlan_buffer, head_room supported */
+        ret = pcb->moal_alloc_mlan_buffer(pmadapter->pmoal_handle,
+                                          data_len + DMA_ALIGNMENT + head_room,
+                                          &pmbuf);
+        if ((ret != MLAN_STATUS_SUCCESS) || !pmbuf) {
+            PRINTM(MERROR, "Failed to allocate 'mlan_buffer'\n");
+            goto exit;
+        }
+        pmbuf->data_offset = head_room;
+        tmp_buf =
+            (t_u8 *) ALIGN_ADDR(pmbuf->pbuf + pmbuf->data_offset,
+                                DMA_ALIGNMENT);
+        pmbuf->data_offset +=
+            (t_u32) (tmp_buf - (pmbuf->pbuf + pmbuf->data_offset));
+        pmbuf->data_len = data_len;
+        pmbuf->flags = 0;
+    }
+  exit:
+    LEAVE();
+    return pmbuf;
+}
+
+/**
+ *  @brief This function frees a mlan_buffer.
+ *
+ *  @param pmadapter  Pointer to mlan_adapter
+ *  @param pmbuf      Pointer to mlan_buffer
+ *
+ *  @return           N/A
+ */
+t_void
+wlan_free_mlan_buffer(mlan_adapter * pmadapter, pmlan_buffer pmbuf)
+{
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    ENTER();
+
+    if (pcb && pmbuf) {
+        if (pmbuf->flags & MLAN_BUF_FLAG_MALLOC_BUF)
+            pcb->moal_mfree(pmadapter->pmoal_handle, (t_u8 *) pmbuf);
+        else
+            pcb->moal_free_mlan_buffer(pmadapter->pmoal_handle, pmbuf);
+    }
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief Delay function implementation
+ *
+ *  @param pmadapter        A pointer to mlan_adapter structure
+ *  @param delay            Delay value
+ *  @param u                Units of delay (sec, msec or usec)
+ *
+ *  @return                 N/A
+ */
+t_void
+wlan_delay_func(mlan_adapter * pmadapter, t_u32 delay, t_delay_unit u)
+{
+    t_u32 now_tv_sec, now_tv_usec;
+    t_u32 upto_tv_sec, upto_tv_usec;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+
+    ENTER();
+
+    if (pcb->moal_udelay) {
+        if (u == SEC) {
+            delay *= 1000000;
+        } else if (u == MSEC) {
+            delay *= 1000;
+        }
+        pcb->moal_udelay(pmadapter->pmoal_handle, delay);
+    } else {
+
+        pcb->moal_get_system_time(pmadapter->pmoal_handle, &upto_tv_sec,
+                                  &upto_tv_usec);
+
+        switch (u) {
+        case SEC:
+            upto_tv_sec += delay;
+            break;
+        case MSEC:
+            delay *= 1000;
+        case USEC:
+            upto_tv_sec += (delay / 1000000);
+            upto_tv_usec += (delay % 1000000);
+            break;
+        }
+
+        do {
+            pcb->moal_get_system_time(pmadapter->pmoal_handle, &now_tv_sec,
+                                      &now_tv_usec);
+            if (now_tv_sec > upto_tv_sec) {
+                LEAVE();
+                return;
+            }
+
+            if ((now_tv_sec == upto_tv_sec) && (now_tv_usec >= upto_tv_usec)) {
+                LEAVE();
+                return;
+            }
+        } while (MTRUE);
+    }
+
+    LEAVE();
+    return;
+}
+
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+/**
+ *  @brief Set/Get BSS role
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, MLAN_STATUS_PENDING --pending
+ */
+mlan_status
+wlan_bss_ioctl_bss_role(IN pmlan_adapter pmadapter,
+                        IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_bss *bss = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    HostCmd_DS_VERSION_EXT dummy;
+#if defined(WIFI_DIRECT_SUPPORT)
+    t_u8 bss_mode;
+#endif
+    t_u8 i, global_band = 0;
+    int j;
+
+    ENTER();
+
+    bss = (mlan_ds_bss *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        bss->param.bss_role = GET_BSS_ROLE(pmpriv);
+    } else {
+                /** Switch BSS role */
+        wlan_free_priv(pmpriv);
+
+        pmpriv->bss_role = bss->param.bss_role;
+        if (pmpriv->bss_type == MLAN_BSS_TYPE_UAP)
+            pmpriv->bss_type = MLAN_BSS_TYPE_STA;
+        else if (pmpriv->bss_type == MLAN_BSS_TYPE_STA)
+            pmpriv->bss_type = MLAN_BSS_TYPE_UAP;
+
+        /* Initialize private structures */
+        wlan_init_priv(pmpriv);
+
+        /* Initialize function table */
+        for (j = 0; mlan_ops[j]; j++) {
+            if (mlan_ops[j]->bss_role == GET_BSS_ROLE(pmpriv)) {
+                memcpy(pmadapter, &pmpriv->ops, mlan_ops[j],
+                       sizeof(mlan_operations));
+            }
+        }
+
+        for (i = 0; i < pmadapter->priv_num; i++) {
+            if (pmadapter->priv[i] &&
+                GET_BSS_ROLE(pmadapter->priv[i]) == MLAN_BSS_ROLE_STA)
+                global_band |= pmadapter->priv[i]->config_bands;
+        }
+
+        if (global_band != pmadapter->config_bands) {
+            if (wlan_set_regiontable(pmpriv, (t_u8) pmadapter->region_code,
+                                     global_band | pmadapter->
+                                     adhoc_start_band)) {
+                pioctl_req->status_code = MLAN_ERROR_IOCTL_FAIL;
+                LEAVE();
+                return MLAN_STATUS_FAILURE;
+            }
+
+            if (wlan_11d_set_universaltable
+                (pmpriv, global_band | pmadapter->adhoc_start_band)) {
+                pioctl_req->status_code = MLAN_ERROR_IOCTL_FAIL;
+                LEAVE();
+                return MLAN_STATUS_FAILURE;
+            }
+            pmadapter->config_bands = global_band;
+        }
+
+        /* Issue commands to initialize firmware */
+#if defined(WIFI_DIRECT_SUPPORT)
+        if (GET_BSS_ROLE(pmpriv) == MLAN_BSS_ROLE_STA)
+            bss_mode = BSS_MODE_WIFIDIRECT_CLIENT;
+        else
+            bss_mode = BSS_MODE_WIFIDIRECT_GO;
+        wlan_prepare_cmd(pmpriv, HostCmd_CMD_SET_BSS_MODE,
+                         HostCmd_ACT_GEN_SET, 0, MNULL, &bss_mode);
+#endif
+        pmpriv->ops.init_cmd(pmpriv, MFALSE);
+
+        /* Issue dummy Get command to complete the ioctl */
+        memset(pmadapter, &dummy, 0, sizeof(HostCmd_DS_VERSION_EXT));
+        wlan_prepare_cmd(pmpriv, HostCmd_CMD_VERSION_EXT, HostCmd_ACT_GEN_GET,
+                         0, (t_void *) pioctl_req, (t_void *) & dummy);
+        ret = MLAN_STATUS_PENDING;
+    }
+
+    LEAVE();
+    return ret;
+}
+#endif
+
+/**
+ *  @brief Set the custom IE
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *  @param send_ioctl	Flag to indicate if ioctl should be sent with cmd
+ *                      (MTRUE if from moal/user, MFALSE if internal)
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_misc_ioctl_custom_ie_list(IN pmlan_adapter pmadapter,
+                               IN pmlan_ioctl_req pioctl_req,
+                               IN t_bool send_ioctl)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_misc_cfg *misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    custom_ie *ie_data = MNULL;
+    t_u16 cmd_action = 0, index, mask, i, len, app_data_len, ioctl_len;
+    t_u8 *tmp_ie;
+
+    ENTER();
+
+    if ((misc->param.cust_ie.len == 0) ||
+        (misc->param.cust_ie.len == sizeof(t_u16))) {
+        pioctl_req->action = MLAN_ACT_GET;
+        /* Get the IE */
+        cmd_action = HostCmd_ACT_GEN_GET;
+    } else {
+        /* ioctl_len : ioctl length from application, start with
+           misc->param.cust_ie.len and reach upto 0 */
+        ioctl_len = misc->param.cust_ie.len;
+
+        /* app_data_len : length from application, start with 0 and reach upto
+           ioctl_len */
+        app_data_len = sizeof(MrvlIEtypesHeader_t);
+        misc->param.cust_ie.len = 0;
+
+        while (ioctl_len > 0) {
+            ie_data = (custom_ie *) (((t_u8 *) & misc->param.cust_ie)
+                                     + app_data_len);
+            ioctl_len -= (ie_data->ie_length + MLAN_CUSTOM_IE_HDR_SIZE);
+            app_data_len += (ie_data->ie_length + MLAN_CUSTOM_IE_HDR_SIZE);
+
+            index = ie_data->ie_index;
+            mask = ie_data->mgmt_subtype_mask;
+            if (MLAN_CUSTOM_IE_AUTO_IDX_MASK == index) {        /* Need to be
+                                                                   Autohandled */
+                if (mask == MLAN_CUSTOM_IE_DELETE_MASK) {       /* Automatic
+                                                                   Deletion */
+                    ret =
+                        wlan_custom_ioctl_auto_delete(pmpriv, pioctl_req,
+                                                      ie_data, index);
+                    /* if IE to delete is not found, return error */
+                    if (ret == MLAN_STATUS_FAILURE) {
+                        goto done;
+                    }
+                    memset(pmadapter, ie_data, 0,
+                           sizeof(custom_ie) * MAX_MGMT_IE_INDEX_TO_FW);
+                    len = 0;
+                    for (i = 0; i < pmadapter->max_mgmt_ie_index; i++) {
+                        memcpy(pmadapter, (t_u8 *) ie_data + len, &i,
+                               sizeof(ie_data->ie_index));
+                        len += sizeof(ie_data->ie_index);
+                        memcpy(pmadapter, (t_u8 *) ie_data + len,
+                               &pmpriv->mgmt_ie[i].mgmt_subtype_mask,
+                               sizeof(ie_data->mgmt_subtype_mask));
+                        len += sizeof(ie_data->mgmt_subtype_mask);
+                        memcpy(pmadapter, (t_u8 *) ie_data + len,
+                               &pmpriv->mgmt_ie[i].ie_length,
+                               sizeof(ie_data->ie_length));
+                        len += sizeof(ie_data->ie_length);
+                        if (pmpriv->mgmt_ie[i].ie_length) {
+                            memcpy(pmadapter, (t_u8 *) ie_data + len,
+                                   &pmpriv->mgmt_ie[i].ie_buffer,
+                                   pmpriv->mgmt_ie[i].ie_length);
+                            len += pmpriv->mgmt_ie[i].ie_length;
+                        }
+                    }
+                    misc->param.cust_ie.len += len;
+                    pioctl_req->action = MLAN_ACT_SET;
+                    cmd_action = HostCmd_ACT_GEN_SET;
+                } else {        /* Automatic Addition */
+                    if (MLAN_STATUS_FAILURE ==
+                        wlan_custom_ioctl_get_autoidx(pmpriv, pioctl_req, mask,
+                                                      ie_data, &index)) {
+                        PRINTM(MERROR, "Failed to Set the IE buffer\n");
+                        ret = MLAN_STATUS_FAILURE;
+                        goto done;
+                    }
+                    if (MLAN_CUSTOM_IE_AUTO_IDX_MASK == index) {
+                        ret = MLAN_STATUS_SUCCESS;
+                        goto done;
+                    }
+                    tmp_ie = (t_u8 *) & pmpriv->mgmt_ie[index].ie_buffer;
+                    memcpy(pmadapter, tmp_ie + pmpriv->mgmt_ie[index].ie_length,
+                           &ie_data->ie_buffer, ie_data->ie_length);
+                    pmpriv->mgmt_ie[index].ie_length += ie_data->ie_length;
+                    pmpriv->mgmt_ie[index].ie_index = index;
+                    pmpriv->mgmt_ie[index].mgmt_subtype_mask = mask;
+
+                    pioctl_req->action = MLAN_ACT_SET;
+                    cmd_action = HostCmd_ACT_GEN_SET;
+                    ie_data->ie_index = index;
+                    ie_data->ie_length = pmpriv->mgmt_ie[index].ie_length;
+                    memcpy(pmadapter, &ie_data->ie_buffer,
+                           &pmpriv->mgmt_ie[index].ie_buffer,
+                           pmpriv->mgmt_ie[index].ie_length);
+                    misc->param.cust_ie.len +=
+                        pmpriv->mgmt_ie[index].ie_length +
+                        MLAN_CUSTOM_IE_HDR_SIZE;
+                }
+            } else {
+                if (index >= pmadapter->max_mgmt_ie_index) {
+                    PRINTM(MERROR, "Invalid custom IE index %d\n", index);
+                    ret = MLAN_STATUS_FAILURE;
+                    goto done;
+                }
+                /* Set/Clear the IE and save it */
+                if (ie_data->mgmt_subtype_mask == MLAN_CUSTOM_IE_DELETE_MASK &&
+                    ie_data->ie_length) {
+                    PRINTM(MINFO, "Clear the IE buffer\n");
+                    ret =
+                        wlan_custom_ioctl_auto_delete(pmpriv, pioctl_req,
+                                                      ie_data, index);
+                    /* if IE to delete is not found, return error */
+                    if (ret == MLAN_STATUS_FAILURE) {
+                        goto done;
+                    }
+                    memset(pmadapter, ie_data, 0,
+                           sizeof(custom_ie) * MAX_MGMT_IE_INDEX_TO_FW);
+                    memcpy(pmadapter, (t_u8 *) ie_data, &pmpriv->mgmt_ie[index],
+                           pmpriv->mgmt_ie[index].ie_length +
+                           MLAN_CUSTOM_IE_HDR_SIZE);
+                } else {
+                    /*
+                     * Check if this index is being used on any other
+                     * interfaces. If yes, then the request needs to be rejected.
+                     */
+                    ret = wlan_is_custom_ie_index_unused(pmpriv, index);
+                    if (ret == MLAN_STATUS_FAILURE) {
+                        PRINTM(MERROR,
+                               "IE index is used by other interface.\n");
+                        PRINTM(MERROR,
+                               "Set or delete on index %d is not allowed.\n",
+                               index);
+                        pioctl_req->status_code = MLAN_ERROR_IOCTL_FAIL;
+                        goto done;
+                    }
+                    PRINTM(MINFO, "Set the IE buffer\n");
+                    if (ie_data->mgmt_subtype_mask ==
+                        MLAN_CUSTOM_IE_DELETE_MASK)
+                        ie_data->ie_length = 0;
+                    else {
+                        if ((pmpriv->mgmt_ie[index].ie_length ==
+                             ie_data->ie_length) &&
+                            !memcmp(pmpriv->adapter,
+                                    pmpriv->mgmt_ie[index].ie_buffer,
+                                    ie_data->ie_buffer,
+                                    pmpriv->mgmt_ie[index].ie_length)) {
+                            PRINTM(MIOCTL,
+                                   "same customer ie already configured!\n");
+                            goto done;
+                        }
+                    }
+                    memset(pmadapter, &pmpriv->mgmt_ie[index], 0,
+                           sizeof(custom_ie));
+                    memcpy(pmadapter, &pmpriv->mgmt_ie[index], ie_data,
+                           sizeof(custom_ie));
+                }
+
+                misc->param.cust_ie.len += pmpriv->mgmt_ie[index].ie_length +
+                    MLAN_CUSTOM_IE_HDR_SIZE;
+                pioctl_req->action = MLAN_ACT_SET;
+                cmd_action = HostCmd_ACT_GEN_SET;
+            }
+        }
+    }
+
+    /* Send command to firmware */
+    if (GET_BSS_ROLE(pmpriv) == MLAN_BSS_ROLE_STA) {
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_MGMT_IE_LIST,
+                               cmd_action,
+                               0,
+                               (send_ioctl) ? (t_void *) pioctl_req : MNULL,
+                               &misc->param.cust_ie);
+    }
+#ifdef UAP_SUPPORT
+    else if (GET_BSS_ROLE(pmpriv) == MLAN_BSS_ROLE_UAP) {
+        ret = wlan_prepare_cmd(pmpriv,
+                               HOST_CMD_APCMD_SYS_CONFIGURE,
+                               cmd_action,
+                               0,
+                               (send_ioctl) ? (t_void *) pioctl_req : MNULL,
+                               (send_ioctl) ? MNULL : &misc->param.cust_ie);
+    }
+#endif
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function will check if station list is empty
+ *
+ *  @param priv    A pointer to mlan_private
+ *
+ *  @return	   MFALSE/MTRUE
+ */
+t_u8
+wlan_is_station_list_empty(mlan_private * priv)
+{
+    ENTER();
+    if (!(util_peek_list(priv->adapter->pmoal_handle,
+                         &priv->sta_list,
+                         priv->adapter->callbacks.moal_spin_lock,
+                         priv->adapter->callbacks.moal_spin_unlock))) {
+        LEAVE();
+        return MTRUE;
+    }
+    LEAVE();
+    return MFALSE;
+}
+
+/**
+ *  @brief This function will return the pointer to station entry in station list
+ *  		table which matches the give mac address
+ *
+ *  @param priv    A pointer to mlan_private
+ *  @param mac     mac address to find in station list table
+ *
+ *  @return	   A pointer to structure sta_node
+ */
+sta_node *
+wlan_get_station_entry(mlan_private * priv, t_u8 * mac)
+{
+    sta_node *sta_ptr;
+
+    ENTER();
+
+    if (!mac) {
+        LEAVE();
+        return MNULL;
+    }
+    if (!(sta_ptr = (sta_node *) util_peek_list(priv->adapter->pmoal_handle,
+                                                &priv->sta_list,
+                                                priv->adapter->callbacks.
+                                                moal_spin_lock,
+                                                priv->adapter->callbacks.
+                                                moal_spin_unlock))) {
+        LEAVE();
+        return MNULL;
+    }
+    while (sta_ptr != (sta_node *) & priv->sta_list) {
+        if (!memcmp
+            (priv->adapter, sta_ptr->mac_addr, mac, MLAN_MAC_ADDR_LENGTH)) {
+            LEAVE();
+            return sta_ptr;
+        }
+        sta_ptr = sta_ptr->pnext;
+    }
+    LEAVE();
+    return MNULL;
+}
+
+/**
+ *  @brief This function will add a pointer to station entry in station list
+ *  		table with the give mac address, if it does not exist already
+ *
+ *  @param priv    A pointer to mlan_private
+ *  @param mac     mac address to find in station list table
+ *
+ *  @return	   A pointer to structure sta_node
+ */
+sta_node *
+wlan_add_station_entry(mlan_private * priv, t_u8 * mac)
+{
+    sta_node *sta_ptr = MNULL;
+    mlan_adapter *pmadapter = priv->adapter;
+
+    ENTER();
+    pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                        priv->wmm.ra_list_spinlock);
+
+    sta_ptr = wlan_get_station_entry(priv, mac);
+    if (sta_ptr)
+        goto done;
+    if (priv->adapter->callbacks.
+        moal_malloc(priv->adapter->pmoal_handle, sizeof(sta_node), MLAN_MEM_DEF,
+                    (t_u8 **) & sta_ptr)) {
+        PRINTM(MERROR, "Failed to allocate memory for station node\n");
+        LEAVE();
+        return MNULL;
+    }
+    memset(priv->adapter, sta_ptr, 0, sizeof(sta_node));
+    memcpy(priv->adapter, sta_ptr->mac_addr, mac, MLAN_MAC_ADDR_LENGTH);
+    util_enqueue_list_tail(priv->adapter->pmoal_handle, &priv->sta_list,
+                           (pmlan_linked_list) sta_ptr,
+                           priv->adapter->callbacks.moal_spin_lock,
+                           priv->adapter->callbacks.moal_spin_unlock);
+  done:
+    pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                          priv->wmm.ra_list_spinlock);
+    LEAVE();
+    return sta_ptr;
+}
+
+/**
+ *  @brief This function will delete a station entry from station list
+ *
+ *
+ *  @param priv    A pointer to mlan_private
+ *  @param mac     station's mac address
+ *
+ *  @return	   N/A
+ */
+t_void
+wlan_delete_station_entry(mlan_private * priv, t_u8 * mac)
+{
+    sta_node *sta_ptr = MNULL;
+    mlan_adapter *pmadapter = priv->adapter;
+    ENTER();
+    pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                        priv->wmm.ra_list_spinlock);
+    if ((sta_ptr = wlan_get_station_entry(priv, mac))) {
+        util_unlink_list(priv->adapter->pmoal_handle, &priv->sta_list,
+                         (pmlan_linked_list) sta_ptr,
+                         priv->adapter->callbacks.moal_spin_lock,
+                         priv->adapter->callbacks.moal_spin_unlock);
+        priv->adapter->callbacks.moal_mfree(priv->adapter->pmoal_handle,
+                                            (t_u8 *) sta_ptr);
+    }
+    pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                          priv->wmm.ra_list_spinlock);
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief Clean up wapi station list
+ *
+ *  @param priv  Pointer to the mlan_private driver data struct
+ *
+ *  @return      N/A
+ */
+t_void
+wlan_delete_station_list(pmlan_private priv)
+{
+    sta_node *sta_ptr;
+
+    ENTER();
+    while ((sta_ptr =
+            (sta_node *) util_dequeue_list(priv->adapter->pmoal_handle,
+                                           &priv->sta_list,
+                                           priv->adapter->callbacks.
+                                           moal_spin_lock,
+                                           priv->adapter->callbacks.
+                                           moal_spin_unlock))) {
+        priv->adapter->callbacks.moal_mfree(priv->adapter->pmoal_handle,
+                                            (t_u8 *) sta_ptr);
+    }
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief Get extended version information
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_get_info_ver_ext(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_get_info *pinfo = (mlan_ds_get_info *) pioctl_req->pbuf;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_VERSION_EXT,
+                           HostCmd_ACT_GEN_GET,
+                           0,
+                           (t_void *) pioctl_req,
+                           &pinfo->param.ver_ext.version_str_sel);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+#ifdef DEBUG_LEVEL1
+/**
+ *  @brief Set driver debug bit masks in order to enhance performance
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_set_drvdbg(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_ds_misc_cfg *misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    /* Set driver debug bit masks */
+    drvdbg = misc->param.drvdbg;
+
+    LEAVE();
+    return ret;
+}
+#endif
+
+/**
+ *  @brief Rx mgmt frame forward register
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_reg_rx_mgmt_ind(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_misc_cfg *misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    /* Set passthru mask for mgmt frame */
+    pmpriv->mgmt_frame_passthru_mask = misc->param.mgmt_subtype_mask;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_RX_MGMT_IND,
+                           pioctl_req->action,
+                           0,
+                           (t_void *) pioctl_req,
+                           &misc->param.mgmt_subtype_mask);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *   @brief This function processes the 802.11 mgmt Frame
+ *
+ *   @param priv      A pointer to mlan_private
+ *   @param payload   A pointer to the received buffer
+ *   @param payload_len Length of the received buffer
+ *
+ *   @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_process_802dot11_mgmt_pkt(IN mlan_private * priv,
+                               IN t_u8 * payload, IN t_u32 payload_len)
+{
+    pmlan_adapter pmadapter = priv->adapter;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    wlan_802_11_header *pieee_pkt_hdr = MNULL;
+    t_u16 sub_type = 0;
+    t_u8 *event_buf = MNULL;
+    mlan_event *pevent = MNULL;
+    t_u8 unicast = 0;
+
+    ENTER();
+    if (payload_len > (MAX_EVENT_SIZE - sizeof(mlan_event))) {
+        PRINTM(MERROR, "Dropping large mgmt frame,len =%d\n", payload_len);
+        LEAVE();
+        return ret;
+    }
+    /* Check packet type-subtype and compare with mgmt_passthru_mask If event
+       is needed to host, just eventify it */
+    pieee_pkt_hdr = (wlan_802_11_header *) payload;
+    sub_type = IEEE80211_GET_FC_MGMT_FRAME_SUBTYPE(pieee_pkt_hdr->frm_ctl);
+    if (((1 << sub_type) & priv->mgmt_frame_passthru_mask) == 0) {
+        PRINTM(MINFO, "Dropping mgmt frame for subtype %d.\n", sub_type);
+        LEAVE();
+        return ret;
+    }
+    switch (sub_type) {
+    case SUBTYPE_ASSOC_REQUEST:
+    case SUBTYPE_REASSOC_REQUEST:
+    case SUBTYPE_DISASSOC:
+    case SUBTYPE_DEAUTH:
+    case SUBTYPE_ACTION:
+    case SUBTYPE_AUTH:
+    case SUBTYPE_PROBE_RESP:
+        unicast = MTRUE;
+        break;
+    default:
+        break;
+    }
+    if (unicast == MTRUE) {
+        if (memcmp
+            (pmadapter, pieee_pkt_hdr->addr1, priv->curr_addr,
+             MLAN_MAC_ADDR_LENGTH)) {
+            PRINTM(MINFO,
+                   "Dropping mgmt frame for others: type=%d %02x:%02x:%02x:%02x:%02x:%02x\n",
+                   sub_type, pieee_pkt_hdr->addr1[0], pieee_pkt_hdr->addr1[1],
+                   pieee_pkt_hdr->addr1[2], pieee_pkt_hdr->addr1[3],
+                   pieee_pkt_hdr->addr1[4], pieee_pkt_hdr->addr1[5]);
+            LEAVE();
+            return ret;
+        }
+    }
+    /* Allocate memory for event buffer */
+    ret =
+        pcb->moal_malloc(pmadapter->pmoal_handle, MAX_EVENT_SIZE, MLAN_MEM_DEF,
+                         &event_buf);
+    if ((ret != MLAN_STATUS_SUCCESS) || !event_buf) {
+        PRINTM(MERROR, "Could not allocate buffer for event buf\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    pevent = (pmlan_event) event_buf;
+    pevent->bss_index = priv->bss_index;
+    pevent->event_id = MLAN_EVENT_ID_DRV_MGMT_FRAME;
+    pevent->event_len = payload_len + sizeof(pevent->event_id);
+    memcpy(pmadapter, (t_u8 *) pevent->event_buf,
+           (t_u8 *) & pevent->event_id, sizeof(pevent->event_id));
+    memcpy(pmadapter, (t_u8 *) (pevent->event_buf + sizeof(pevent->event_id)),
+           payload, payload_len);
+    wlan_recv_event(priv, MLAN_EVENT_ID_DRV_MGMT_FRAME, pevent);
+
+    if (event_buf)
+        pcb->moal_mfree(pmadapter->pmoal_handle, event_buf);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Get OTP user data
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_misc_otp_user_data(IN pmlan_adapter pmadapter,
+                        IN pmlan_ioctl_req pioctl_req)
+{
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_misc_cfg *misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    mlan_status ret = MLAN_STATUS_FAILURE;
+
+    ENTER();
+
+    if (misc->param.otp_user_data.user_data_length > MAX_OTP_USER_DATA_LEN) {
+        PRINTM(MERROR, "Invalid OTP user data length\n");
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return ret;
+    }
+
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_OTP_READ_USER_DATA,
+                           HostCmd_ACT_GEN_GET,
+                           0,
+                           (t_void *) pioctl_req, &misc->param.otp_user_data);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function will search for the specific ie
+ *
+ *
+ *  @param priv    A pointer to mlan_private
+ *  @param ie_buf  A pointer to ie_buf
+ *  @param ie_len  total ie length
+ *  @param id      ie's id
+ *
+ *  @return	       ie's poiner or MNULL
+ */
+t_u8 *
+wlan_get_specific_ie(pmlan_private priv, t_u8 * ie_buf, t_u8 ie_len,
+                     IEEEtypes_ElementId_e id)
+{
+    t_u32 bytes_left = ie_len;
+    t_u8 *pcurrent_ptr = ie_buf;
+    t_u16 total_ie_len;
+    t_u8 *ie_ptr = MNULL;
+    IEEEtypes_ElementId_e element_id;
+    t_u8 element_len;
+
+    ENTER();
+
+    DBG_HEXDUMP(MCMD_D, "ie", ie_buf, ie_len);
+    while (bytes_left >= 2) {
+        element_id = (IEEEtypes_ElementId_e) (*((t_u8 *) pcurrent_ptr));
+        element_len = *((t_u8 *) pcurrent_ptr + 1);
+        total_ie_len = element_len + sizeof(IEEEtypes_Header_t);
+        if (bytes_left < total_ie_len) {
+            PRINTM(MERROR, "InterpretIE: Error in processing IE, "
+                   "bytes left < IE length\n");
+            break;
+        }
+        if (element_id == id) {
+            PRINTM(MCMND, "Find IE: id=%d\n", id);
+            DBG_HEXDUMP(MCMND, "IE", pcurrent_ptr, total_ie_len);
+            ie_ptr = pcurrent_ptr;
+            break;
+        }
+        pcurrent_ptr += element_len + 2;
+        /* Need to account for IE ID and IE Len */
+        bytes_left -= (element_len + 2);
+    }
+
+    LEAVE();
+
+    return ie_ptr;
+}
+
+/**
+ *  @brief Get pm info
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		        MLAN_STATUS_SUCCESS --success
+ */
+mlan_status
+wlan_get_pm_info(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_pm_cfg *pm_cfg = MNULL;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+
+    ENTER();
+
+    pm_cfg = (mlan_ds_pm_cfg *) pioctl_req->pbuf;
+    pm_cfg->param.ps_info.is_suspend_allowed = MTRUE;
+    if (util_peek_list(pmadapter->pmoal_handle, &pmadapter->cmd_pending_q,
+                       pcb->moal_spin_lock, pcb->moal_spin_unlock)
+        || pmadapter->curr_cmd || !wlan_bypass_tx_list_empty(pmadapter)
+        || !wlan_wmm_lists_empty(pmadapter)
+        || pmadapter->sdio_ireg) {
+        pm_cfg->param.ps_info.is_suspend_allowed = MFALSE;
+        PRINTM(MIOCTL,
+               "PM: cmd_pending_q=%p,curr_cmd=%p,wmm_list_empty=%d, by_pass=%d sdio_ireg=0x%x\n",
+               util_peek_list(pmadapter->pmoal_handle,
+                              &pmadapter->cmd_pending_q, pcb->moal_spin_lock,
+                              pcb->moal_spin_unlock), pmadapter->curr_cmd,
+               wlan_wmm_lists_empty(pmadapter),
+               wlan_bypass_tx_list_empty(pmadapter), pmadapter->sdio_ireg);
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get radio status
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+mlan_status
+wlan_radio_ioctl_radio_ctl(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_radio_cfg *radio_cfg = MNULL;
+    t_u16 cmd_action = 0;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    radio_cfg = (mlan_ds_radio_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        if (pmadapter->radio_on == radio_cfg->param.radio_on_off) {
+            ret = MLAN_STATUS_SUCCESS;
+            goto exit;
+        } else {
+            if (pmpriv->media_connected == MTRUE) {
+                ret = MLAN_STATUS_FAILURE;
+                goto exit;
+            }
+            cmd_action = HostCmd_ACT_GEN_SET;
+        }
+    } else
+        cmd_action = HostCmd_ACT_GEN_GET;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_RADIO_CONTROL,
+                           cmd_action,
+                           0,
+                           (t_void *) pioctl_req,
+                           &radio_cfg->param.radio_on_off);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get antenna configuration
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return     MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_radio_ioctl_ant_cfg(IN pmlan_adapter pmadapter,
+                         IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_radio_cfg *radio_cfg = MNULL;
+    t_u16 cmd_action = 0;
+    mlan_ds_ant_cfg *ant_cfg;
+
+    ENTER();
+
+    radio_cfg = (mlan_ds_radio_cfg *) pioctl_req->pbuf;
+    ant_cfg = &radio_cfg->param.ant_cfg;
+
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        /* User input validation */
+        if (!ant_cfg->tx_antenna ||
+            ant_cfg->tx_antenna & ~RF_ANTENNA_MASK(pmadapter->
+                                                   number_of_antenna)) {
+            PRINTM(MERROR, "Invalid antenna setting\n");
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+            goto exit;
+        }
+        if (ant_cfg->rx_antenna) {
+            if (ant_cfg->
+                rx_antenna & ~RF_ANTENNA_MASK(pmadapter->number_of_antenna)) {
+                PRINTM(MERROR, "Invalid antenna setting\n");
+                pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+                ret = MLAN_STATUS_FAILURE;
+                goto exit;
+            }
+        } else
+            ant_cfg->rx_antenna = ant_cfg->tx_antenna;
+        cmd_action = HostCmd_ACT_GEN_SET;
+    } else
+        cmd_action = HostCmd_ACT_GEN_GET;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_RF_ANTENNA,
+                           cmd_action,
+                           0, (t_void *) pioctl_req, (t_void *) ant_cfg);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get rate value
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_rate_ioctl_get_rate_value(IN pmlan_adapter pmadapter,
+                               IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_ds_rate *rate = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    rate = (mlan_ds_rate *) pioctl_req->pbuf;
+    rate->param.rate_cfg.is_rate_auto = pmpriv->is_data_rate_auto;
+    pioctl_req->data_read_written =
+        sizeof(mlan_rate_cfg_t) + MLAN_SUB_COMMAND_SIZE;
+
+    /* If not connected, set rate to the lowest in each band */
+    if (pmpriv->media_connected != MTRUE) {
+        if (pmpriv->config_bands & (BAND_B | BAND_G)) {
+            /* Return the lowest supported rate for BG band */
+            rate->param.rate_cfg.rate = SupportedRates_BG[0] & 0x7f;
+        } else if (pmpriv->config_bands & (BAND_A | BAND_B)) {
+            /* Return the lowest supported rate for A band */
+            rate->param.rate_cfg.rate = SupportedRates_BG[0] & 0x7f;
+        } else if (pmpriv->config_bands & BAND_A) {
+            /* Return the lowest supported rate for A band */
+            rate->param.rate_cfg.rate = SupportedRates_A[0] & 0x7f;
+        } else if (pmpriv->config_bands & BAND_G) {
+            /* Return the lowest supported rate for G band */
+            rate->param.rate_cfg.rate = SupportedRates_G[0] & 0x7f;
+        } else if (pmpriv->config_bands & BAND_B) {
+            /* Return the lowest supported rate for B band */
+            rate->param.rate_cfg.rate = SupportedRates_B[0] & 0x7f;
+        } else if (pmpriv->config_bands & BAND_GN) {
+            /* Return the lowest supported rate for N band */
+            rate->param.rate_cfg.rate = SupportedRates_N[0] & 0x7f;
+        } else {
+            PRINTM(MMSG, "Invalid Band 0x%x\n", pmpriv->config_bands);
+        }
+
+    } else {
+        /* Send request to firmware */
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_802_11_TX_RATE_QUERY,
+                               HostCmd_ACT_GEN_GET,
+                               0, (t_void *) pioctl_req, MNULL);
+        if (ret == MLAN_STATUS_SUCCESS)
+            ret = MLAN_STATUS_PENDING;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set rate value
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_rate_ioctl_set_rate_value(IN pmlan_adapter pmadapter,
+                               IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_ds_rate *ds_rate = MNULL;
+    WLAN_802_11_RATES rates;
+    t_u8 *rate = MNULL;
+    int rate_index = 0;
+    t_u16 bitmap_rates[MAX_BITMAP_RATES_SIZE];
+    t_u32 i = 0;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    ds_rate = (mlan_ds_rate *) pioctl_req->pbuf;
+
+    if (ds_rate->param.rate_cfg.is_rate_auto) {
+        memset(pmadapter, bitmap_rates, 0, sizeof(bitmap_rates));
+        /* Support all HR/DSSS rates */
+        bitmap_rates[0] = 0x000F;
+        /* Support all OFDM rates */
+        bitmap_rates[1] = 0x00FF;
+        /* Support all HT-MCSs rate */
+        for (i = 0; i < NELEMENTS(pmpriv->bitmap_rates) - 3; i++)
+            bitmap_rates[i + 2] = 0xFFFF;
+        bitmap_rates[9] = 0x3FFF;
+    } else {
+        memset(pmadapter, rates, 0, sizeof(rates));
+        wlan_get_active_data_rates(pmpriv, pmpriv->bss_mode,
+                                   (pmpriv->bss_mode == MLAN_BSS_MODE_INFRA) ?
+                                   pmpriv->config_bands : pmadapter->
+                                   adhoc_start_band, rates);
+        rate = rates;
+        for (i = 0; (rate[i] && i < WLAN_SUPPORTED_RATES); i++) {
+            PRINTM(MINFO, "Rate=0x%X  Wanted=0x%X\n", rate[i],
+                   ds_rate->param.rate_cfg.rate);
+            if ((rate[i] & 0x7f) == (ds_rate->param.rate_cfg.rate & 0x7f))
+                break;
+        }
+        if (!rate[i] || (i == WLAN_SUPPORTED_RATES)) {
+            PRINTM(MERROR, "The fixed data rate 0x%X is out "
+                   "of range\n", ds_rate->param.rate_cfg.rate);
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+            goto exit;
+        }
+        memset(pmadapter, bitmap_rates, 0, sizeof(bitmap_rates));
+
+        rate_index =
+            wlan_data_rate_to_index(pmadapter, ds_rate->param.rate_cfg.rate);
+
+        /* Only allow b/g rates to be set */
+        if (rate_index >= MLAN_RATE_INDEX_HRDSSS0 &&
+            rate_index <= MLAN_RATE_INDEX_HRDSSS3)
+            bitmap_rates[0] = 1 << rate_index;
+        else {
+            rate_index -= 1;    /* There is a 0x00 in the table */
+            if (rate_index >= MLAN_RATE_INDEX_OFDM0 &&
+                rate_index <= MLAN_RATE_INDEX_OFDM7)
+                bitmap_rates[1] = 1 << (rate_index - MLAN_RATE_INDEX_OFDM0);
+        }
+    }
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_TX_RATE_CFG,
+                           HostCmd_ACT_GEN_SET,
+                           0, (t_void *) pioctl_req, bitmap_rates);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get rate index
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_rate_ioctl_get_rate_index(IN pmlan_adapter pmadapter,
+                               IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_TX_RATE_CFG,
+                           HostCmd_ACT_GEN_GET,
+                           0, (t_void *) pioctl_req, MNULL);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set rate index
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_rate_ioctl_set_rate_index(IN pmlan_adapter pmadapter,
+                               IN pmlan_ioctl_req pioctl_req)
+{
+    t_u32 rate_index;
+    t_u32 i;
+    mlan_ds_rate *ds_rate = MNULL;
+    mlan_status ret = MLAN_STATUS_FAILURE;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    t_u16 bitmap_rates[MAX_BITMAP_RATES_SIZE];
+    int tx_mcs_supp = GET_TXMCSSUPP(pmadapter->usr_dev_mcs_support);
+
+    ENTER();
+
+    ds_rate = (mlan_ds_rate *) pioctl_req->pbuf;
+    rate_index = ds_rate->param.rate_cfg.rate;
+
+    if (ds_rate->param.rate_cfg.is_rate_auto) {
+        memset(pmadapter, bitmap_rates, 0, sizeof(bitmap_rates));
+        /* Rates talbe [0]: HR/DSSS;[1]: OFDM; [2..9] HT; */
+        /* Support all HR/DSSS rates */
+        bitmap_rates[0] = 0x000F;
+        /* Support all OFDM rates */
+        bitmap_rates[1] = 0x00FF;
+        /* Support all HT-MCSs rate */
+        for (i = 2; i < 9; i++)
+            bitmap_rates[i] = 0xFFFF;
+        bitmap_rates[9] = 0x3FFF;
+    } else {
+        PRINTM(MINFO, "Rate index is %d\n", rate_index);
+        if ((rate_index > MLAN_RATE_INDEX_MCS7 &&
+             rate_index <= MLAN_RATE_INDEX_MCS15) && (tx_mcs_supp < 2)) {
+            PRINTM(MERROR,
+                   "HW don't support 2x2, rate_index=%d hw_mcs_supp=0x%x\n",
+                   rate_index, pmadapter->usr_dev_mcs_support);
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+        memset(pmadapter, bitmap_rates, 0, sizeof(bitmap_rates));
+        /* Bitmap of HR/DSSS rates */
+        if ((rate_index >= MLAN_RATE_INDEX_HRDSSS0) &&
+            (rate_index <= MLAN_RATE_INDEX_HRDSSS3)) {
+            bitmap_rates[0] = 1 << rate_index;
+            ret = MLAN_STATUS_SUCCESS;
+            /* Bitmap of OFDM rates */
+        } else if ((rate_index >= MLAN_RATE_INDEX_OFDM0) &&
+                   (rate_index <= MLAN_RATE_INDEX_OFDM7)) {
+            bitmap_rates[1] = 1 << (rate_index - MLAN_RATE_INDEX_OFDM0);
+            ret = MLAN_STATUS_SUCCESS;
+        }
+        if ((rate_index >= MLAN_RATE_INDEX_MCS0) &&
+            (rate_index <= MLAN_RATE_INDEX_MCS32)) {
+            rate_index -= MLAN_RATE_INDEX_MCS0;
+            bitmap_rates[2 + (rate_index / 16)] = 1 << (rate_index % 16);
+            ret = MLAN_STATUS_SUCCESS;
+        }
+
+        if (ret == MLAN_STATUS_FAILURE) {
+            PRINTM(MERROR, "Invalid MCS index=%d. \n", rate_index);
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+    }
+
+    PRINTM(MINFO, "RateBitmap=%04x%04x%04x%04x%04x%04x%04x%04x%04x%04x, "
+           "IsRateAuto=%d, DataRate=%d\n",
+           bitmap_rates[9], bitmap_rates[8],
+           bitmap_rates[7], bitmap_rates[6],
+           bitmap_rates[5], bitmap_rates[4],
+           bitmap_rates[3], bitmap_rates[2],
+           bitmap_rates[1], bitmap_rates[0],
+           pmpriv->is_data_rate_auto, pmpriv->data_rate);
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_TX_RATE_CFG,
+                           HostCmd_ACT_GEN_SET,
+                           0, (t_void *) pioctl_req, (t_void *) bitmap_rates);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Rate configuration command handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_rate_ioctl_cfg(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_ds_rate *rate = MNULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    rate = (mlan_ds_rate *) pioctl_req->pbuf;
+    if (rate->param.rate_cfg.rate_type == MLAN_RATE_VALUE) {
+        if (pioctl_req->action == MLAN_ACT_GET)
+            status = wlan_rate_ioctl_get_rate_value(pmadapter, pioctl_req);
+        else
+            status = wlan_rate_ioctl_set_rate_value(pmadapter, pioctl_req);
+    } else {
+        if (pioctl_req->action == MLAN_ACT_GET)
+            status = wlan_rate_ioctl_get_rate_index(pmadapter, pioctl_req);
+        else
+            status = wlan_rate_ioctl_set_rate_index(pmadapter, pioctl_req);
+    }
+
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief This function prepares command of rf_antenna.
+ *
+ *  @param pmpriv   A pointer to mlan_private structure
+ *  @param cmd      A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *
+ *  @return         MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_802_11_rf_antenna(IN pmlan_private pmpriv,
+                           IN HostCmd_DS_COMMAND * cmd,
+                           IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    HostCmd_DS_802_11_RF_ANTENNA *pantenna = &cmd->params.antenna;
+    mlan_ds_ant_cfg *ant_cfg = (mlan_ds_ant_cfg *) pdata_buf;
+
+    ENTER();
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_RF_ANTENNA);
+    cmd->size =
+        wlan_cpu_to_le16(sizeof(HostCmd_DS_802_11_RF_ANTENNA) + S_DS_GEN);
+
+    if (cmd_action == HostCmd_ACT_GEN_SET) {
+        pantenna->action_tx = wlan_cpu_to_le16(HostCmd_ACT_SET_TX);
+        pantenna->tx_antenna_mode =
+            wlan_cpu_to_le16((t_u16) ant_cfg->tx_antenna);
+        pantenna->action_rx = wlan_cpu_to_le16(HostCmd_ACT_SET_RX);
+        pantenna->rx_antenna_mode =
+            wlan_cpu_to_le16((t_u16) ant_cfg->rx_antenna);
+    } else {
+        pantenna->action_tx = wlan_cpu_to_le16(HostCmd_ACT_GET_TX);
+        pantenna->action_rx = wlan_cpu_to_le16(HostCmd_ACT_GET_RX);
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of rf_antenna
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_802_11_rf_antenna(IN pmlan_private pmpriv,
+                           IN HostCmd_DS_COMMAND * resp,
+                           IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_802_11_RF_ANTENNA *pantenna = &resp->params.antenna;
+    t_u16 tx_ant_mode = wlan_le16_to_cpu(pantenna->tx_antenna_mode);
+    t_u16 rx_ant_mode = wlan_le16_to_cpu(pantenna->rx_antenna_mode);
+    mlan_ds_radio_cfg *radio = MNULL;
+
+    ENTER();
+
+    PRINTM(MINFO, "RF_ANT_RESP: Tx action = 0x%x, Tx Mode = 0x%04x"
+           " Rx action = 0x%x, Rx Mode = 0x%04x\n",
+           wlan_le16_to_cpu(pantenna->action_tx), tx_ant_mode,
+           wlan_le16_to_cpu(pantenna->action_rx), rx_ant_mode);
+
+    if (pioctl_buf) {
+        radio = (mlan_ds_radio_cfg *) pioctl_buf->pbuf;
+        radio->param.ant_cfg.tx_antenna = tx_ant_mode;
+        radio->param.ant_cfg.rx_antenna = rx_ant_mode;
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+#ifdef WIFI_DIRECT_SUPPORT
+/**
+ *  @brief Set/Get wifi_direct_mode
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+mlan_status
+wlan_bss_ioctl_wifi_direct_mode(IN pmlan_adapter pmadapter,
+                                IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_bss *bss = MNULL;
+
+    t_u16 cmd_action = 0;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    bss = (mlan_ds_bss *) pioctl_req->pbuf;
+
+    if (pioctl_req->action == MLAN_ACT_SET)
+        cmd_action = HostCmd_ACT_GEN_SET;
+    else
+        cmd_action = HostCmd_ACT_GEN_GET;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HOST_CMD_WIFI_DIRECT_MODE_CONFIG,
+                           cmd_action,
+                           0, (t_void *) pioctl_req, &bss->param.wfd_mode);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get remain on channel setting
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+mlan_status
+wlan_radio_ioctl_remain_chan_cfg(IN pmlan_adapter pmadapter,
+                                 IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_radio_cfg *radio_cfg = MNULL;
+    t_u16 cmd_action = 0;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    radio_cfg = (mlan_ds_radio_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET)
+        cmd_action = HostCmd_ACT_GEN_SET;
+    else
+        cmd_action = HostCmd_ACT_GEN_GET;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_REMAIN_ON_CHANNEL,
+                           cmd_action,
+                           0,
+                           (t_void *) pioctl_req,
+                           &radio_cfg->param.remain_chan);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+#endif
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_module.c b/drivers/net/wireless/sd8797/mlan/mlan_module.c
new file mode 100644
index 000000000000..cd0bd0c5fb78
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_module.c
@@ -0,0 +1,47 @@
+/** @file mlan_module.c
+ *
+ *  @brief This file declares the exported symbols from MLAN.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    12/08/2008: initial version
+******************************************************/
+
+#ifdef LINUX
+#include <linux/module.h>
+#include "mlan_decl.h"
+#include "mlan_ioctl.h"
+
+EXPORT_SYMBOL(mlan_register);
+EXPORT_SYMBOL(mlan_unregister);
+EXPORT_SYMBOL(mlan_init_fw);
+EXPORT_SYMBOL(mlan_set_init_param);
+EXPORT_SYMBOL(mlan_dnld_fw);
+EXPORT_SYMBOL(mlan_shutdown_fw);
+EXPORT_SYMBOL(mlan_send_packet);
+EXPORT_SYMBOL(mlan_ioctl);
+EXPORT_SYMBOL(mlan_main_process);
+EXPORT_SYMBOL(mlan_select_wmm_queue);
+EXPORT_SYMBOL(mlan_interrupt);
+
+MODULE_DESCRIPTION("M-WLAN MLAN Driver");
+MODULE_AUTHOR("Marvell International Ltd.");
+MODULE_VERSION(MLAN_RELEASE_VERSION);
+MODULE_LICENSE("GPL");
+#endif /* LINUX */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_scan.c b/drivers/net/wireless/sd8797/mlan/mlan_scan.c
new file mode 100644
index 000000000000..895fb38a5786
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_scan.c
@@ -0,0 +1,4223 @@
+/** @file mlan_scan.c
+ *
+ *  @brief Functions implementing wlan scan IOCTL and firmware command APIs
+ *
+ *  IOCTL handlers as well as command preparation and response routines
+ *  for sending scan commands to the firmware.
+ *
+ *  Copyright (C) 2008-2012, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    10/28/2008: initial version
+******************************************************/
+
+#include "mlan.h"
+#include "mlan_join.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_11n.h"
+#include "mlan_11h.h"
+
+/********************************************************
+                Local Constants
+********************************************************/
+
+/** The maximum number of channels the firmware can scan per command */
+#define MRVDRV_MAX_CHANNELS_PER_SPECIFIC_SCAN   14
+
+/**
+ * Number of channels to scan per firmware scan command issuance.
+ *
+ * Number restricted to prevent hitting the limit on the amount of scan data
+ * returned in a single firmware scan command.
+ */
+#define MRVDRV_CHANNELS_PER_SCAN_CMD            4
+
+/** Memory needed to store a max sized Channel List TLV for a firmware scan */
+#define CHAN_TLV_MAX_SIZE  (sizeof(MrvlIEtypesHeader_t)                  \
+                            + (MRVDRV_MAX_CHANNELS_PER_SPECIFIC_SCAN     \
+                               * sizeof(ChanScanParamSet_t)))
+
+/** Memory needed to store supported rate */
+#define RATE_TLV_MAX_SIZE   (sizeof(MrvlIEtypes_RatesParamSet_t) + HOSTCMD_SUPPORTED_RATES)
+
+/** Memory needed to store a max number/size WildCard SSID TLV for a firmware scan */
+#define WILDCARD_SSID_TLV_MAX_SIZE  \
+            (MRVDRV_MAX_SSID_LIST_LENGTH  * (sizeof(MrvlIEtypes_WildCardSsIdParamSet_t) + MRVDRV_MAX_SSID_LENGTH))
+
+/** WPS TLV MAX size is MAX IE size plus 2 bytes for t_u16 MRVL TLV extension */
+#define WPS_TLV_MAX_SIZE   (sizeof(IEEEtypes_VendorSpecific_t) + 2)
+/** Maximum memory needed for a wlan_scan_cmd_config with all TLVs at max */
+#define MAX_SCAN_CFG_ALLOC (sizeof(wlan_scan_cmd_config)        \
+                            + sizeof(MrvlIEtypes_NumProbes_t)   \
+                            + sizeof(MrvlIETypes_HTCap_t)       \
+                            + CHAN_TLV_MAX_SIZE                 \
+                            + RATE_TLV_MAX_SIZE                 \
+                            + WILDCARD_SSID_TLV_MAX_SIZE        \
+                            + WPS_TLV_MAX_SIZE)
+
+/********************************************************
+                Local Variables
+********************************************************/
+
+/**
+ * Interally used to send a configured scan cmd between driver routines
+ */
+typedef union
+{
+    /** Scan configuration (variable length) */
+    wlan_scan_cmd_config config;
+    /** Max allocated block */
+    t_u8 config_alloc_buf[MAX_SCAN_CFG_ALLOC];
+} wlan_scan_cmd_config_tlv;
+
+/********************************************************
+                Global Variables
+********************************************************/
+
+/********************************************************
+                Local Functions
+********************************************************/
+/** Cipher suite definition */
+enum cipher_suite
+{
+    CIPHER_SUITE_TKIP,
+    CIPHER_SUITE_CCMP,
+    CIPHER_SUITE_MAX
+};
+
+static t_u8 wpa_oui[CIPHER_SUITE_MAX][4] = {
+    {0x00, 0x50, 0xf2, 0x02},   /* TKIP */
+    {0x00, 0x50, 0xf2, 0x04},   /* AES */
+};
+
+static t_u8 rsn_oui[CIPHER_SUITE_MAX][4] = {
+    {0x00, 0x0f, 0xac, 0x02},   /* TKIP */
+    {0x00, 0x0f, 0xac, 0x04},   /* AES */
+};
+
+/**
+ *  @brief This function will parse a given IE for a given OUI
+ *
+ *  Parse a given WPA/RSN IE to find if it has a given oui in PTK,
+ *  if no OUI found for PTK it returns 0.
+ *
+ *  @param pbss_desc       A pointer to current BSS descriptor
+ *  @return                0 on failure to find OUI, 1 on success.
+ */
+static t_u8
+search_oui_in_ie(mlan_adapter * pmadapter, IEBody * ie_body, t_u8 * oui)
+{
+    t_u8 count;
+
+    count = ie_body->PtkCnt[0];
+
+    ENTER();
+    /* There could be multiple OUIs for PTK hence 1) Take the length. 2) Check
+       all the OUIs for AES. 3) If one of them is AES then pass success. */
+    while (count) {
+        if (!memcmp(pmadapter, ie_body->PtkBody, oui, sizeof(ie_body->PtkBody))) {
+            LEAVE();
+            return MLAN_OUI_PRESENT;
+        }
+
+        --count;
+        if (count) {
+            ie_body = (IEBody *) ((t_u8 *) ie_body + sizeof(ie_body->PtkBody));
+        }
+    }
+
+    PRINTM(MINFO, "The OUI %x:%x:%x:%x is not found in PTK \n", oui[0], oui[1],
+           oui[2], oui[3]);
+    LEAVE();
+    return MLAN_OUI_NOT_PRESENT;
+}
+
+/**
+ *  @brief This function will pass the correct ie and oui to search_oui_in_ie
+ *
+ *  Check the pbss_desc for appropriate IE and then check if RSN IE has AES
+ *  OUI in it. If RSN IE does not have AES in PTK then return 0;
+ *
+ *  @param pbss_desc       A pointer to current BSS descriptor
+ *  @return                0 on failure to find AES OUI, 1 on success.
+ */
+static t_u8
+is_rsn_oui_present(mlan_adapter * pmadapter, BSSDescriptor_t * pbss_desc,
+                   t_u32 cipher_suite)
+{
+    t_u8 *oui = MNULL;
+    IEBody *ie_body = MNULL;
+    t_u8 ret = MLAN_OUI_NOT_PRESENT;
+
+    ENTER();
+    if (((pbss_desc->prsn_ie) &&
+         ((*(pbss_desc->prsn_ie)).ieee_hdr.element_id == RSN_IE))) {
+        ie_body =
+            (IEBody *) (((t_u8 *) pbss_desc->prsn_ie->data) +
+                        RSN_GTK_OUI_OFFSET);
+        oui = &rsn_oui[cipher_suite][0];
+        if ((ret = search_oui_in_ie(pmadapter, ie_body, oui))) {
+            LEAVE();
+            return ret;
+        }
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function will pass the correct ie and oui to search_oui_in_ie
+ *
+ *  Check the pbss_desc for appropriate IE and then check if WPA IE has AES
+ *  OUI in it. If WPA IE does not have AES in PTK then return 0;
+ *
+ *  @param pbss_desc       A pointer to current BSS descriptor
+ *  @return                0 on failure to find AES OUI, 1 on success.
+ */
+static t_u8
+is_wpa_oui_present(mlan_adapter * pmadapter, BSSDescriptor_t * pbss_desc,
+                   t_u32 cipher_suite)
+{
+    t_u8 *oui = MNULL;
+    IEBody *ie_body = MNULL;
+    t_u8 ret = MLAN_OUI_NOT_PRESENT;
+
+    ENTER();
+    if (((pbss_desc->pwpa_ie) &&
+         ((*(pbss_desc->pwpa_ie)).vend_hdr.element_id == WPA_IE))) {
+        ie_body = (IEBody *) pbss_desc->pwpa_ie->data;
+        oui = &wpa_oui[cipher_suite][0];
+        if ((ret = search_oui_in_ie(pmadapter, ie_body, oui))) {
+            LEAVE();
+            return ret;
+        }
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief compare config band and a band from the scan result,
+ *  which is defined by functiion radio_type_to_band(t_u8 radio_type) above
+ *
+ *  @param cfg_band:  band configured
+ *         scan_band: band from scan result
+ *
+ *  @return  matched: non-zero. unmatched: 0
+ *
+ */
+static t_u8
+wlan_is_band_compatible(t_u8 cfg_band, t_u8 scan_band)
+{
+    t_u8 band;
+    switch (scan_band) {
+    case BAND_A:
+        band = BAND_A | BAND_AN;
+        break;
+    case BAND_G:
+    default:
+        band = BAND_B | BAND_G | BAND_GN;
+    }
+    return cfg_band & band;
+}
+
+/**
+ *  @brief This function finds the best SSID in the Scan List
+ *
+ *  Search the scan table for the best SSID that also matches the current
+ *   adapter network preference (infrastructure or adhoc)
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @return             index in BSSID list
+ */
+static t_s32
+wlan_find_best_network_in_list(IN mlan_private * pmpriv)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    t_u32 mode = pmpriv->bss_mode;
+    t_s32 best_net = -1;
+    t_s32 best_rssi = 0;
+    t_u32 i;
+
+    ENTER();
+
+    PRINTM(MINFO, "Num of BSSIDs = %d\n", pmadapter->num_in_scan_table);
+
+    for (i = 0; i < pmadapter->num_in_scan_table; i++) {
+        switch (mode) {
+        case MLAN_BSS_MODE_INFRA:
+        case MLAN_BSS_MODE_IBSS:
+            if (wlan_is_network_compatible(pmpriv, i, mode) >= 0) {
+                if (SCAN_RSSI(pmadapter->pscan_table[i].rssi) > best_rssi) {
+                    best_rssi = SCAN_RSSI(pmadapter->pscan_table[i].rssi);
+                    best_net = i;
+                }
+            }
+            break;
+        case MLAN_BSS_MODE_AUTO:
+        default:
+            if (SCAN_RSSI(pmadapter->pscan_table[i].rssi) > best_rssi) {
+                best_rssi = SCAN_RSSI(pmadapter->pscan_table[i].rssi);
+                best_net = i;
+            }
+            break;
+        }
+    }
+
+    LEAVE();
+    return best_net;
+}
+
+/**
+ *  @brief Create a channel list for the driver to scan based on region info
+ *
+ *  Use the driver region/band information to construct a comprehensive list
+ *    of channels to scan.  This routine is used for any scan that is not
+ *    provided a specific channel list to scan.
+ *
+ *  @param pmpriv           A pointer to mlan_private structure
+ *  @param puser_scan_in    MNULL or pointer to scan configuration parameters
+ *  @param pscan_chan_list  Output parameter: Resulting channel list to scan
+ *  @param filtered_scan    Flag indicating whether or not a BSSID or SSID filter
+ *                          is being sent in the command to firmware.  Used to
+ *                          increase the number of channels sent in a scan
+ *                          command and to disable the firmware channel scan
+ *                          filter.
+ *
+ *  @return                 N/A
+ */
+static t_void
+wlan_scan_create_channel_list(IN mlan_private * pmpriv,
+                              IN const wlan_user_scan_cfg * puser_scan_in,
+                              OUT ChanScanParamSet_t * pscan_chan_list,
+                              IN t_u8 filtered_scan)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    region_chan_t *pscan_region;
+    chan_freq_power_t *cfp;
+    t_u32 region_idx;
+    t_u32 chan_idx = 0;
+    t_u32 next_chan;
+    t_u8 scan_type;
+    t_u8 radio_type;
+
+    ENTER();
+
+    for (region_idx = 0;
+         region_idx < NELEMENTS(pmadapter->region_channel); region_idx++) {
+
+        if (wlan_11d_is_enabled(pmpriv) && pmpriv->media_connected != MTRUE) {
+            /* Scan all the supported chan for the first scan */
+            if (!pmadapter->universal_channel[region_idx].valid)
+                continue;
+            pscan_region = &pmadapter->universal_channel[region_idx];
+        } else {
+            if (!pmadapter->region_channel[region_idx].valid)
+                continue;
+            pscan_region = &pmadapter->region_channel[region_idx];
+        }
+
+        if (puser_scan_in && !puser_scan_in->chan_list[0].chan_number &&
+            puser_scan_in->chan_list[0].radio_type & BAND_SPECIFIED) {
+            radio_type =
+                puser_scan_in->chan_list[0].radio_type & ~BAND_SPECIFIED;
+            if (!radio_type && (pscan_region->band != BAND_B) &&
+                (pscan_region->band != BAND_G))
+                continue;
+            if (radio_type && (pscan_region->band != BAND_A))
+                continue;
+        }
+        if (!wlan_is_band_compatible
+            (pmpriv->config_bands | pmadapter->adhoc_start_band,
+             pscan_region->band))
+            continue;
+        for (next_chan = 0;
+             next_chan < pscan_region->num_cfp; next_chan++, chan_idx++) {
+            /* Set the default scan type to the user specified type, will later
+               be changed to passive on a per channel basis if restricted by
+               regulatory requirements (11d or 11h) */
+            scan_type = pmadapter->scan_type;
+            cfp = pscan_region->pcfp + next_chan;
+
+            if (scan_type == MLAN_SCAN_TYPE_ACTIVE
+                && wlan_11d_is_enabled(pmpriv)) {
+                scan_type = wlan_11d_get_scan_type(pmadapter,
+                                                   pscan_region->band,
+                                                   (t_u8) cfp->channel,
+                                                   &pmadapter->
+                                                   parsed_region_chan);
+            }
+
+            switch (pscan_region->band) {
+            case BAND_A:
+                pscan_chan_list[chan_idx].radio_type =
+                    HostCmd_SCAN_RADIO_TYPE_A;
+                if (!wlan_11d_is_enabled(pmpriv)) {
+                    /* 11D not available... play it safe on DFS channels */
+                    if (wlan_11h_radar_detect_required
+                        (pmpriv, (t_u8) cfp->channel))
+                        scan_type = MLAN_SCAN_TYPE_PASSIVE;
+                }
+                break;
+            case BAND_B:
+            case BAND_G:
+                if (wlan_bg_scan_type_is_passive(pmpriv, (t_u8) cfp->channel)) {
+                    scan_type = MLAN_SCAN_TYPE_PASSIVE;
+                }
+            default:
+                pscan_chan_list[chan_idx].radio_type =
+                    HostCmd_SCAN_RADIO_TYPE_BG;
+                break;
+            }
+
+            if (puser_scan_in && puser_scan_in->chan_list[0].scan_time) {
+                pscan_chan_list[chan_idx].max_scan_time =
+                    wlan_cpu_to_le16((t_u16) puser_scan_in->chan_list[0].
+                                     scan_time);
+            } else if (scan_type == MLAN_SCAN_TYPE_PASSIVE) {
+                pscan_chan_list[chan_idx].max_scan_time =
+                    wlan_cpu_to_le16(pmadapter->passive_scan_time);
+            } else if (filtered_scan) {
+                pscan_chan_list[chan_idx].max_scan_time =
+                    wlan_cpu_to_le16(pmadapter->specific_scan_time);
+            } else {
+                pscan_chan_list[chan_idx].max_scan_time =
+                    wlan_cpu_to_le16(pmadapter->active_scan_time);
+            }
+
+            if (scan_type == MLAN_SCAN_TYPE_PASSIVE) {
+                pscan_chan_list[chan_idx].chan_scan_mode.passive_scan = MTRUE;
+            } else {
+                pscan_chan_list[chan_idx].chan_scan_mode.passive_scan = MFALSE;
+            }
+
+            pscan_chan_list[chan_idx].chan_number = (t_u8) cfp->channel;
+
+            if (filtered_scan) {
+                pscan_chan_list[chan_idx].chan_scan_mode.disable_chan_filt =
+                    MTRUE;
+            }
+        }
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief Add WPS IE to probe request frame
+ *
+ *  @param pmpriv             A pointer to mlan_private structure
+ *  @param pptlv_out          A pointer to TLV to fill in
+ *
+ *  @return                   N/A
+ */
+static void
+wlan_add_wps_probe_request_ie(IN mlan_private * pmpriv, OUT t_u8 ** pptlv_out)
+{
+    MrvlIEtypesHeader_t *tlv;
+
+    ENTER();
+
+    if (pmpriv->wps.wps_ie.vend_hdr.len) {
+        tlv = (MrvlIEtypesHeader_t *) * pptlv_out;
+        tlv->type = wlan_cpu_to_le16(VENDOR_SPECIFIC_221);
+        tlv->len = wlan_cpu_to_le16(pmpriv->wps.wps_ie.vend_hdr.len);
+        *pptlv_out += sizeof(MrvlIEtypesHeader_t);
+        memcpy(pmpriv->adapter, *pptlv_out,
+               pmpriv->wps.wps_ie.vend_hdr.oui,
+               pmpriv->wps.wps_ie.vend_hdr.len);
+        *pptlv_out += (pmpriv->wps.wps_ie.vend_hdr.len
+                       + sizeof(MrvlIEtypesHeader_t));
+    }
+    LEAVE();
+}
+
+/**
+ *  @brief Construct and send multiple scan config commands to the firmware
+ *
+ *  Previous routines have created a wlan_scan_cmd_config with any requested
+ *   TLVs.  This function splits the channel TLV into max_chan_per_scan lists
+ *   and sends the portion of the channel TLV along with the other TLVs
+ *   to the wlan_cmd routines for execution in the firmware.
+ *
+ *  @param pmpriv             A pointer to mlan_private structure
+ *  @param pioctl_buf         A pointer to MLAN IOCTL Request buffer
+ *  @param max_chan_per_scan  Maximum number channels to be included in each
+ *                            scan command sent to firmware
+ *  @param filtered_scan      Flag indicating whether or not a BSSID or SSID
+ *                            filter is being used for the firmware command
+ *                            scan command sent to firmware
+ *  @param pscan_cfg_out      Scan configuration used for this scan.
+ *  @param pchan_tlv_out      Pointer in the pscan_cfg_out where the channel TLV
+ *                            should start.  This is past any other TLVs that
+ *                            must be sent down in each firmware command.
+ *  @param pscan_chan_list    List of channels to scan in max_chan_per_scan segments
+ *
+ *  @return                   MLAN_STATUS_SUCCESS or error return otherwise
+ */
+static mlan_status
+wlan_scan_channel_list(IN mlan_private * pmpriv,
+                       IN t_void * pioctl_buf,
+                       IN t_u32 max_chan_per_scan,
+                       IN t_u8 filtered_scan,
+                       OUT wlan_scan_cmd_config * pscan_cfg_out,
+                       OUT MrvlIEtypes_ChanListParamSet_t * pchan_tlv_out,
+                       IN ChanScanParamSet_t * pscan_chan_list)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    ChanScanParamSet_t *ptmp_chan_list;
+    ChanScanParamSet_t *pstart_chan;
+    pmlan_ioctl_req pioctl_req = (mlan_ioctl_req *) pioctl_buf;
+
+    t_u32 tlv_idx;
+    t_u32 total_scan_time;
+    t_u32 done_early;
+    t_u32 cmd_no;
+
+    ENTER();
+
+    if (!pscan_cfg_out || !pchan_tlv_out || !pscan_chan_list) {
+        PRINTM(MINFO, "Scan: Null detect: %p, %p, %p\n",
+               pscan_cfg_out, pchan_tlv_out, pscan_chan_list);
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_CMD_SCAN_FAIL;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    if (!pscan_chan_list->chan_number) {
+        PRINTM(MERROR, "Scan: No channel configured\n");
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_CMD_SCAN_FAIL;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    pchan_tlv_out->header.type = wlan_cpu_to_le16(TLV_TYPE_CHANLIST);
+
+    /* Set the temp channel struct pointer to the start of the desired list */
+    ptmp_chan_list = pscan_chan_list;
+
+    /* Loop through the desired channel list, sending a new firmware scan
+       commands for each max_chan_per_scan channels (or for 1,6,11 individually
+       if configured accordingly) */
+    while (ptmp_chan_list->chan_number) {
+
+        tlv_idx = 0;
+        total_scan_time = 0;
+        pchan_tlv_out->header.len = 0;
+        pstart_chan = ptmp_chan_list;
+        done_early = MFALSE;
+
+        /*
+         * Construct the Channel TLV for the scan command.  Continue to
+         * insert channel TLVs until:
+         *   - the tlv_idx hits the maximum configured per scan command
+         *   - the next channel to insert is 0 (end of desired channel list)
+         *   - done_early is set (controlling individual scanning of 1,6,11)
+         */
+        while (tlv_idx < max_chan_per_scan && ptmp_chan_list->chan_number &&
+               !done_early) {
+
+            PRINTM(MINFO, "Scan: Chan(%3d), Radio(%d), Mode(%d,%d), Dur(%d)\n",
+                   ptmp_chan_list->chan_number,
+                   ptmp_chan_list->radio_type,
+                   ptmp_chan_list->chan_scan_mode.passive_scan,
+                   ptmp_chan_list->chan_scan_mode.disable_chan_filt,
+                   wlan_le16_to_cpu(ptmp_chan_list->max_scan_time));
+
+            /* Copy the current channel TLV to the command being prepared */
+            memcpy(pmadapter, pchan_tlv_out->chan_scan_param + tlv_idx,
+                   ptmp_chan_list, sizeof(pchan_tlv_out->chan_scan_param));
+
+            /* Increment the TLV header length by the size appended */
+            pchan_tlv_out->header.len += sizeof(pchan_tlv_out->chan_scan_param);
+
+            /*
+             * The tlv buffer length is set to the number of bytes of the
+             *   between the channel tlv pointer and the start of the
+             *   tlv buffer.  This compensates for any TLVs that were appended
+             *   before the channel list.
+             */
+            pscan_cfg_out->tlv_buf_len = (t_u32) ((t_u8 *) pchan_tlv_out
+                                                  - pscan_cfg_out->tlv_buf);
+
+            /* Add the size of the channel tlv header and the data length */
+            pscan_cfg_out->tlv_buf_len += (sizeof(pchan_tlv_out->header)
+                                           + pchan_tlv_out->header.len);
+
+            /* Increment the index to the channel tlv we are constructing */
+            tlv_idx++;
+
+            /* Count the total scan time per command */
+            total_scan_time += wlan_le16_to_cpu(ptmp_chan_list->max_scan_time);
+
+            done_early = MFALSE;
+
+            /* Stop the loop if the *current* channel is in the 1,6,11 set and
+               we are not filtering on a BSSID or SSID. */
+            if (!filtered_scan && (ptmp_chan_list->chan_number == 1 ||
+                                   ptmp_chan_list->chan_number == 6 ||
+                                   ptmp_chan_list->chan_number == 11)) {
+                done_early = MTRUE;
+            }
+
+            /* Increment the tmp pointer to the next channel to be scanned */
+            ptmp_chan_list++;
+
+            /* Stop the loop if the *next* channel is in the 1,6,11 set.  This
+               will cause it to be the only channel scanned on the next
+               interation */
+            if (!filtered_scan && (ptmp_chan_list->chan_number == 1 ||
+                                   ptmp_chan_list->chan_number == 6 ||
+                                   ptmp_chan_list->chan_number == 11)) {
+                done_early = MTRUE;
+            }
+        }
+
+        /* The total scan time should be less than scan command timeout value */
+        if (total_scan_time > MRVDRV_MAX_TOTAL_SCAN_TIME) {
+            PRINTM(MMSG,
+                   "Total scan time %d ms is over limit (%d ms), scan skipped\n",
+                   total_scan_time, MRVDRV_MAX_TOTAL_SCAN_TIME);
+            if (pioctl_req)
+                pioctl_req->status_code = MLAN_ERROR_CMD_SCAN_FAIL;
+            ret = MLAN_STATUS_FAILURE;
+            break;
+        }
+
+        pchan_tlv_out->header.len = wlan_cpu_to_le16(pchan_tlv_out->header.len);
+
+        pmadapter->pscan_channels = pstart_chan;
+
+        /* Send the scan command to the firmware with the specified cfg */
+        if (pmadapter->ext_scan)
+            cmd_no = HostCmd_CMD_802_11_SCAN_EXT;
+        else
+            cmd_no = HostCmd_CMD_802_11_SCAN;
+        ret = wlan_prepare_cmd(pmpriv,
+                               cmd_no,
+                               HostCmd_ACT_GEN_SET,
+                               0, pioctl_buf, pscan_cfg_out);
+        if (ret)
+            break;
+    }
+
+    LEAVE();
+
+    if (ret) {
+        return MLAN_STATUS_FAILURE;
+    }
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Construct a wlan_scan_cmd_config structure to use in scan commands
+ *
+ *  Application layer or other functions can invoke wlan_scan_networks
+ *    with a scan configuration supplied in a wlan_ioctl_user_scan_cfg struct.
+ *    This structure is used as the basis of one or many wlan_scan_cmd_config
+ *    commands that are sent to the command processing module and sent to
+ *    firmware.
+ *
+ *  Create a wlan_scan_cmd_config based on the following user supplied
+ *    parameters (if present):
+ *             - SSID filter
+ *             - BSSID filter
+ *             - Number of Probes to be sent
+ *             - Channel list
+ *
+ *  If the SSID or BSSID filter is not present, disable/clear the filter.
+ *  If the number of probes is not set, use the adapter default setting
+ *  Qualify the channel
+ *
+ *  @param pmpriv              A pointer to mlan_private structure
+ *  @param puser_scan_in       MNULL or pointer to scan config parameters
+ *  @param pscan_cfg_out       Output parameter: Resulting scan configuration
+ *  @param ppchan_list_out     Output parameter: Pointer to the start of the
+ *                             channel TLV portion of the output scan config
+ *  @param pscan_chan_list     Output parameter: Pointer to the resulting
+ *                             channel list to scan
+ *  @param pmax_chan_per_scan  Output parameter: Number of channels to scan for
+ *                             each issuance of the firmware scan command
+ *  @param pfiltered_scan      Output parameter: Flag indicating whether or not
+ *                             a BSSID or SSID filter is being sent in the
+ *                             command to firmware. Used to increase the number
+ *                             of channels sent in a scan command and to
+ *                             disable the firmware channel scan filter.
+ *  @param pscan_current_only  Output parameter: Flag indicating whether or not
+ *                             we are only scanning our current active channel
+ *
+ *  @return                 MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_scan_setup_scan_config(IN mlan_private * pmpriv,
+                            IN const wlan_user_scan_cfg * puser_scan_in,
+                            OUT wlan_scan_cmd_config * pscan_cfg_out,
+                            OUT MrvlIEtypes_ChanListParamSet_t **
+                            ppchan_list_out,
+                            OUT ChanScanParamSet_t * pscan_chan_list,
+                            OUT t_u8 * pmax_chan_per_scan,
+                            OUT t_u8 * pfiltered_scan,
+                            OUT t_u8 * pscan_current_only)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    MrvlIEtypes_NumProbes_t *pnum_probes_tlv;
+    MrvlIEtypes_WildCardSsIdParamSet_t *pwildcard_ssid_tlv;
+    MrvlIEtypes_RatesParamSet_t *prates_tlv;
+    const t_u8 zero_mac[MLAN_MAC_ADDR_LENGTH] = { 0, 0, 0, 0, 0, 0 };
+    t_u8 *ptlv_pos;
+    t_u32 num_probes;
+    t_u32 ssid_len;
+    t_u32 chan_idx;
+    t_u32 scan_type;
+    t_u16 scan_dur;
+    t_u8 channel;
+    t_u8 radio_type;
+    t_u32 ssid_idx;
+    t_u8 ssid_filter;
+    WLAN_802_11_RATES rates;
+    t_u32 rates_size;
+    MrvlIETypes_HTCap_t *pht_cap;
+
+    ENTER();
+
+    /* The tlv_buf_len is calculated for each scan command.  The TLVs added in
+       this routine will be preserved since the routine that sends the command
+       will append channelTLVs at *ppchan_list_out.  The difference between the
+       *ppchan_list_out and the tlv_buf start will be used to calculate the
+       size of anything we add in this routine. */
+    pscan_cfg_out->tlv_buf_len = 0;
+
+    /* Running tlv pointer.  Assigned to ppchan_list_out at end of function so
+       later routines know where channels can be added to the command buf */
+    ptlv_pos = pscan_cfg_out->tlv_buf;
+
+    /* Initialize the scan as un-filtered; the flag is later set to TRUE below
+       if a SSID or BSSID filter is sent in the command */
+    *pfiltered_scan = MFALSE;
+
+    /* Initialize the scan as not being only on the current channel.  If the
+       channel list is customized, only contains one channel, and is the active
+       channel, this is set true and data flow is not halted. */
+    *pscan_current_only = MFALSE;
+
+    if (puser_scan_in) {
+
+        ssid_filter = MFALSE;
+
+        /* Set the bss type scan filter, use Adapter setting if unset */
+        pscan_cfg_out->bss_mode = (puser_scan_in->bss_mode
+                                   ? (t_u8) puser_scan_in->bss_mode :
+                                   (t_u8) pmadapter->scan_mode);
+
+        /* Set the number of probes to send, use Adapter setting if unset */
+        num_probes = (puser_scan_in->num_probes ? puser_scan_in->num_probes :
+                      pmadapter->scan_probes);
+
+        /*
+         * Set the BSSID filter to the incoming configuration,
+         *   if non-zero.  If not set, it will remain disabled (all zeros).
+         */
+        memcpy(pmadapter, pscan_cfg_out->specific_bssid,
+               puser_scan_in->specific_bssid,
+               sizeof(pscan_cfg_out->specific_bssid));
+
+        for (ssid_idx = 0; ((ssid_idx < NELEMENTS(puser_scan_in->ssid_list))
+                            && (*puser_scan_in->ssid_list[ssid_idx].ssid ||
+                                puser_scan_in->ssid_list[ssid_idx].max_len));
+             ssid_idx++) {
+
+            ssid_len =
+                wlan_strlen((t_s8 *) puser_scan_in->ssid_list[ssid_idx].ssid);
+
+            pwildcard_ssid_tlv
+                = (MrvlIEtypes_WildCardSsIdParamSet_t *) ptlv_pos;
+            pwildcard_ssid_tlv->header.type
+                = wlan_cpu_to_le16(TLV_TYPE_WILDCARDSSID);
+            pwildcard_ssid_tlv->header.len
+                = (t_u16) (ssid_len
+                           + sizeof(pwildcard_ssid_tlv->max_ssid_length));
+            pwildcard_ssid_tlv->max_ssid_length
+                = puser_scan_in->ssid_list[ssid_idx].max_len;
+
+            memcpy(pmadapter, pwildcard_ssid_tlv->ssid,
+                   puser_scan_in->ssid_list[ssid_idx].ssid,
+                   MIN(MLAN_MAX_SSID_LENGTH, ssid_len));
+
+            ptlv_pos += (sizeof(pwildcard_ssid_tlv->header)
+                         + pwildcard_ssid_tlv->header.len);
+
+            pwildcard_ssid_tlv->header.len
+                = wlan_cpu_to_le16(pwildcard_ssid_tlv->header.len);
+
+            PRINTM(MINFO, "Scan: ssid_list[%d]: %s, %d\n",
+                   ssid_idx,
+                   pwildcard_ssid_tlv->ssid,
+                   pwildcard_ssid_tlv->max_ssid_length);
+
+            if (ssid_len) {
+                ssid_filter = MTRUE;
+            }
+        }
+
+        /*
+         *  The default number of channels sent in the command is low to
+         *    ensure the response buffer from the firmware does not truncate
+         *    scan results.  That is not an issue with an SSID or BSSID
+         *    filter applied to the scan results in the firmware.
+         */
+        if ((ssid_idx && ssid_filter) ||
+            memcmp(pmadapter, pscan_cfg_out->specific_bssid, &zero_mac,
+                   sizeof(zero_mac))) {
+            *pfiltered_scan = MTRUE;
+        }
+
+    } else {
+        pscan_cfg_out->bss_mode = (t_u8) pmadapter->scan_mode;
+        num_probes = pmadapter->scan_probes;
+    }
+
+    /*
+     *  If a specific BSSID or SSID is used, the number of channels in the
+     *  scan command will be increased to the absolute maximum.
+     */
+    if (*pfiltered_scan)
+        *pmax_chan_per_scan = MRVDRV_MAX_CHANNELS_PER_SPECIFIC_SCAN;
+    else
+        *pmax_chan_per_scan = MRVDRV_CHANNELS_PER_SCAN_CMD;
+
+    /* If the input config or adapter has the number of Probes set, add tlv */
+    if (num_probes) {
+
+        PRINTM(MINFO, "Scan: num_probes = %d\n", num_probes);
+
+        pnum_probes_tlv = (MrvlIEtypes_NumProbes_t *) ptlv_pos;
+        pnum_probes_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_NUMPROBES);
+        pnum_probes_tlv->header.len = sizeof(pnum_probes_tlv->num_probes);
+        pnum_probes_tlv->num_probes = wlan_cpu_to_le16((t_u16) num_probes);
+
+        ptlv_pos +=
+            sizeof(pnum_probes_tlv->header) + pnum_probes_tlv->header.len;
+
+        pnum_probes_tlv->header.len =
+            wlan_cpu_to_le16(pnum_probes_tlv->header.len);
+    }
+
+    /* Append rates tlv */
+    memset(pmadapter, rates, 0, sizeof(rates));
+
+    rates_size = wlan_get_supported_rates(pmpriv, pmpriv->bss_mode,
+                                          (pmpriv->bss_mode ==
+                                           MLAN_BSS_MODE_INFRA) ? pmpriv->
+                                          config_bands : pmadapter->
+                                          adhoc_start_band, rates);
+
+    prates_tlv = (MrvlIEtypes_RatesParamSet_t *) ptlv_pos;
+    prates_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_RATES);
+    prates_tlv->header.len = wlan_cpu_to_le16((t_u16) rates_size);
+    memcpy(pmadapter, prates_tlv->rates, rates, rates_size);
+    ptlv_pos += sizeof(prates_tlv->header) + rates_size;
+
+    PRINTM(MINFO, "SCAN_CMD: Rates size = %d\n", rates_size);
+
+    if (ISSUPP_11NENABLED(pmpriv->adapter->fw_cap_info)
+        && (pmpriv->config_bands & BAND_GN || pmpriv->config_bands & BAND_AN)) {
+        pht_cap = (MrvlIETypes_HTCap_t *) ptlv_pos;
+        memset(pmadapter, pht_cap, 0, sizeof(MrvlIETypes_HTCap_t));
+        pht_cap->header.type = wlan_cpu_to_le16(HT_CAPABILITY);
+        pht_cap->header.len = sizeof(HTCap_t);
+        wlan_fill_ht_cap_tlv(pmpriv, pht_cap, pmpriv->config_bands);
+        HEXDUMP("SCAN: HT_CAPABILITIES IE", (t_u8 *) pht_cap,
+                sizeof(MrvlIETypes_HTCap_t));
+        ptlv_pos += sizeof(MrvlIETypes_HTCap_t);
+        pht_cap->header.len = wlan_cpu_to_le16(pht_cap->header.len);
+    }
+
+    wlan_add_wps_probe_request_ie(pmpriv, &ptlv_pos);
+
+    /*
+     * Set the output for the channel TLV to the address in the tlv buffer
+     *   past any TLVs that were added in this function (SSID, num_probes).
+     *   Channel TLVs will be added past this for each scan command, preserving
+     *   the TLVs that were previously added.
+     */
+    *ppchan_list_out = (MrvlIEtypes_ChanListParamSet_t *) ptlv_pos;
+
+    if (puser_scan_in && puser_scan_in->chan_list[0].chan_number) {
+
+        PRINTM(MINFO, "Scan: Using supplied channel list\n");
+
+        for (chan_idx = 0;
+             chan_idx < WLAN_USER_SCAN_CHAN_MAX
+             && puser_scan_in->chan_list[chan_idx].chan_number; chan_idx++) {
+
+            channel = puser_scan_in->chan_list[chan_idx].chan_number;
+            (pscan_chan_list + chan_idx)->chan_number = channel;
+
+            radio_type = puser_scan_in->chan_list[chan_idx].radio_type;
+            (pscan_chan_list + chan_idx)->radio_type = radio_type;
+
+            scan_type = puser_scan_in->chan_list[chan_idx].scan_type;
+            if (scan_type == MLAN_SCAN_TYPE_UNCHANGED)
+                scan_type = pmadapter->scan_type;
+
+            if (radio_type == HostCmd_SCAN_RADIO_TYPE_A) {
+                if (pmadapter->fw_bands & BAND_A)
+                    PRINTM(MINFO, "UserScan request for A Band channel %d!!\n",
+                           channel);
+                else {
+                    PRINTM(MERROR, "Scan in A band is not allowed!!\n");
+                    ret = MLAN_STATUS_FAILURE;
+                    LEAVE();
+                    return ret;
+
+                }
+            }
+
+            /* Prevent active scanning on a radar controlled channel */
+            if (radio_type == HostCmd_SCAN_RADIO_TYPE_A) {
+                if (wlan_11h_radar_detect_required(pmpriv, channel)) {
+                    scan_type = MLAN_SCAN_TYPE_PASSIVE;
+                }
+            }
+            if (radio_type == HostCmd_SCAN_RADIO_TYPE_BG) {
+                if (wlan_bg_scan_type_is_passive(pmpriv, channel)) {
+                    scan_type = MLAN_SCAN_TYPE_PASSIVE;
+                }
+            }
+            if (scan_type == MLAN_SCAN_TYPE_PASSIVE) {
+                (pscan_chan_list + chan_idx)->chan_scan_mode.passive_scan =
+                    MTRUE;
+            } else {
+                (pscan_chan_list + chan_idx)->chan_scan_mode.passive_scan =
+                    MFALSE;
+            }
+
+            if (puser_scan_in->chan_list[chan_idx].scan_time) {
+                scan_dur = (t_u16) puser_scan_in->chan_list[chan_idx].scan_time;
+            } else {
+                if (scan_type == MLAN_SCAN_TYPE_PASSIVE) {
+                    scan_dur = pmadapter->passive_scan_time;
+                } else if (*pfiltered_scan) {
+                    scan_dur = pmadapter->specific_scan_time;
+                } else {
+                    scan_dur = pmadapter->active_scan_time;
+                }
+            }
+
+            (pscan_chan_list + chan_idx)->min_scan_time =
+                wlan_cpu_to_le16(scan_dur);
+            (pscan_chan_list + chan_idx)->max_scan_time =
+                wlan_cpu_to_le16(scan_dur);
+        }
+
+        /* Check if we are only scanning the current channel */
+        if ((chan_idx == 1)
+            && (puser_scan_in->chan_list[0].chan_number
+                == pmpriv->curr_bss_params.bss_descriptor.channel)) {
+            *pscan_current_only = MTRUE;
+            PRINTM(MINFO, "Scan: Scanning current channel only\n");
+        }
+
+    } else {
+        PRINTM(MINFO, "Scan: Creating full region channel list\n");
+        wlan_scan_create_channel_list(pmpriv, puser_scan_in, pscan_chan_list,
+                                      *pfiltered_scan);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Inspect the scan response buffer for pointers to expected TLVs
+ *
+ *  TLVs can be included at the end of the scan response BSS information.
+ *    Parse the data in the buffer for pointers to TLVs that can potentially
+ *    be passed back in the response
+ *
+ *  @param pmadapter        Pointer to the mlan_adapter structure
+ *  @param ptlv             Pointer to the start of the TLV buffer to parse
+ *  @param tlv_buf_size     Size of the TLV buffer
+ *  @param req_tlv_type     Request TLV's type
+ *  @param pptlv            Output parameter: Pointer to the request TLV if found
+ *
+ *  @return                 N/A
+ */
+static t_void
+wlan_ret_802_11_scan_get_tlv_ptrs(IN pmlan_adapter pmadapter,
+                                  IN MrvlIEtypes_Data_t * ptlv,
+                                  IN t_u32 tlv_buf_size,
+                                  IN t_u32 req_tlv_type,
+                                  OUT MrvlIEtypes_Data_t ** pptlv)
+{
+    MrvlIEtypes_Data_t *pcurrent_tlv;
+    t_u32 tlv_buf_left;
+    t_u32 tlv_type;
+    t_u32 tlv_len;
+
+    ENTER();
+
+    pcurrent_tlv = ptlv;
+    tlv_buf_left = tlv_buf_size;
+    *pptlv = MNULL;
+
+    PRINTM(MINFO, "SCAN_RESP: tlv_buf_size = %d\n", tlv_buf_size);
+
+    while (tlv_buf_left >= sizeof(MrvlIEtypesHeader_t)) {
+
+        tlv_type = wlan_le16_to_cpu(pcurrent_tlv->header.type);
+        tlv_len = wlan_le16_to_cpu(pcurrent_tlv->header.len);
+
+        if (sizeof(ptlv->header) + tlv_len > tlv_buf_left) {
+            PRINTM(MERROR, "SCAN_RESP: TLV buffer corrupt\n");
+            break;
+        }
+
+        if (req_tlv_type == tlv_type) {
+            switch (tlv_type) {
+            case TLV_TYPE_TSFTIMESTAMP:
+                PRINTM(MINFO, "SCAN_RESP: TSF Timestamp TLV, len = %d\n",
+                       tlv_len);
+                *pptlv = (MrvlIEtypes_Data_t *) pcurrent_tlv;
+                break;
+            case TLV_TYPE_CHANNELBANDLIST:
+                PRINTM(MINFO, "SCAN_RESP: CHANNEL BAND LIST TLV, len = %d\n",
+                       tlv_len);
+                *pptlv = (MrvlIEtypes_Data_t *) pcurrent_tlv;
+                break;
+            default:
+                PRINTM(MERROR, "SCAN_RESP: Unhandled TLV = %d\n", tlv_type);
+                /* Give up, this seems corrupted */
+                LEAVE();
+                return;
+            }
+        }
+
+        if (*pptlv) {
+            // HEXDUMP("SCAN_RESP: TLV Buf", (t_u8 *)*pptlv+4, tlv_len);
+            break;
+        }
+
+        tlv_buf_left -= (sizeof(ptlv->header) + tlv_len);
+        pcurrent_tlv = (MrvlIEtypes_Data_t *) (pcurrent_tlv->data + tlv_len);
+
+    }                           /* while */
+
+    LEAVE();
+}
+
+/**
+ *  @brief Interpret a BSS scan response returned from the firmware
+ *
+ *  Parse the various fixed fields and IEs passed back for a BSS probe
+ *   response or beacon from the scan command.  Record information as needed
+ *   in the scan table BSSDescriptor_t for that entry.
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pbss_entry   Output parameter: Pointer to the BSS Entry
+ *  @param pbeacon_info Pointer to the Beacon information
+ *  @param bytes_left   Number of bytes left to parse
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_interpret_bss_desc_with_ie(IN pmlan_adapter pmadapter,
+                                OUT BSSDescriptor_t * pbss_entry,
+                                IN t_u8 ** pbeacon_info, IN t_u32 * bytes_left)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    IEEEtypes_ElementId_e element_id;
+    IEEEtypes_FhParamSet_t *pfh_param_set;
+    IEEEtypes_DsParamSet_t *pds_param_set;
+    IEEEtypes_CfParamSet_t *pcf_param_set;
+    IEEEtypes_IbssParamSet_t *pibss_param_set;
+    IEEEtypes_CapInfo_t *pcap_info;
+    WLAN_802_11_FIXED_IEs fixed_ie;
+    t_u8 *pcurrent_ptr;
+    t_u8 *prate;
+    t_u8 element_len;
+    t_u16 total_ie_len;
+    t_u8 bytes_to_copy;
+    t_u8 rate_size;
+    t_u16 beacon_size;
+    t_u8 found_data_rate_ie;
+    t_u32 bytes_left_for_current_beacon;
+    IEEEtypes_ERPInfo_t *perp_info;
+
+    IEEEtypes_VendorSpecific_t *pvendor_ie;
+    const t_u8 wpa_oui[4] = { 0x00, 0x50, 0xf2, 0x01 };
+    const t_u8 wmm_oui[4] = { 0x00, 0x50, 0xf2, 0x02 };
+
+    IEEEtypes_CountryInfoSet_t *pcountry_info;
+
+    ENTER();
+
+    found_data_rate_ie = MFALSE;
+    rate_size = 0;
+    beacon_size = 0;
+
+    if (*bytes_left >= sizeof(beacon_size)) {
+        /* Extract & convert beacon size from the command buffer */
+        memcpy(pmadapter, &beacon_size, *pbeacon_info, sizeof(beacon_size));
+        beacon_size = wlan_le16_to_cpu(beacon_size);
+        *bytes_left -= sizeof(beacon_size);
+        *pbeacon_info += sizeof(beacon_size);
+    }
+
+    if (!beacon_size || beacon_size > *bytes_left) {
+
+        *pbeacon_info += *bytes_left;
+        *bytes_left = 0;
+
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    /* Initialize the current working beacon pointer for this BSS iteration */
+    pcurrent_ptr = *pbeacon_info;
+
+    /* Advance the return beacon pointer past the current beacon */
+    *pbeacon_info += beacon_size;
+    *bytes_left -= beacon_size;
+
+    bytes_left_for_current_beacon = beacon_size;
+
+    if (bytes_left_for_current_beacon <
+        (MLAN_MAC_ADDR_LENGTH + sizeof(t_u8) + sizeof(WLAN_802_11_FIXED_IEs))) {
+        PRINTM(MERROR, "InterpretIE: Not enough bytes left\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    memcpy(pmadapter, pbss_entry->mac_address, pcurrent_ptr,
+           MLAN_MAC_ADDR_LENGTH);
+    PRINTM(MINFO, "InterpretIE: AP MAC Addr-%02x:%02x:%02x:%02x:%02x:%02x\n",
+           pbss_entry->mac_address[0], pbss_entry->mac_address[1],
+           pbss_entry->mac_address[2], pbss_entry->mac_address[3],
+           pbss_entry->mac_address[4], pbss_entry->mac_address[5]);
+
+    pcurrent_ptr += MLAN_MAC_ADDR_LENGTH;
+    bytes_left_for_current_beacon -= MLAN_MAC_ADDR_LENGTH;
+
+    /*
+     * Next 4 fields are RSSI (for legacy scan only), time stamp,
+     *   beacon interval, and capability information
+     */
+    if (!pmadapter->ext_scan) {
+        /* RSSI is 1 byte long */
+        pbss_entry->rssi = (t_s32) (*pcurrent_ptr);
+        PRINTM(MINFO, "InterpretIE: RSSI=%02X\n", *pcurrent_ptr);
+        pcurrent_ptr += 1;
+        bytes_left_for_current_beacon -= 1;
+    }
+
+    /*
+     *  The RSSI is not part of the beacon/probe response.  After we have
+     *    advanced pcurrent_ptr past the RSSI field, save the remaining
+     *    data for use at the application layer
+     */
+    pbss_entry->pbeacon_buf = pcurrent_ptr;
+    pbss_entry->beacon_buf_size = bytes_left_for_current_beacon;
+
+    /* Time stamp is 8 bytes long */
+    memcpy(pmadapter, fixed_ie.time_stamp, pcurrent_ptr, 8);
+    memcpy(pmadapter, pbss_entry->time_stamp, pcurrent_ptr, 8);
+    pcurrent_ptr += 8;
+    bytes_left_for_current_beacon -= 8;
+
+    /* Beacon interval is 2 bytes long */
+    memcpy(pmadapter, &fixed_ie.beacon_interval, pcurrent_ptr, 2);
+    pbss_entry->beacon_period = wlan_le16_to_cpu(fixed_ie.beacon_interval);
+    pcurrent_ptr += 2;
+    bytes_left_for_current_beacon -= 2;
+
+    /* Capability information is 2 bytes long */
+    memcpy(pmadapter, &fixed_ie.capabilities, pcurrent_ptr, 2);
+    PRINTM(MINFO, "InterpretIE: fixed_ie.capabilities=0x%X\n",
+           fixed_ie.capabilities);
+    fixed_ie.capabilities = wlan_le16_to_cpu(fixed_ie.capabilities);
+    pcap_info = (IEEEtypes_CapInfo_t *) & fixed_ie.capabilities;
+    memcpy(pmadapter, &pbss_entry->cap_info, pcap_info,
+           sizeof(IEEEtypes_CapInfo_t));
+    pcurrent_ptr += 2;
+    bytes_left_for_current_beacon -= 2;
+
+    /* Rest of the current buffer are IE's */
+    PRINTM(MINFO, "InterpretIE: IELength for this AP = %d\n",
+           bytes_left_for_current_beacon);
+
+    HEXDUMP("InterpretIE: IE info", (t_u8 *) pcurrent_ptr,
+            bytes_left_for_current_beacon);
+
+    if (pcap_info->privacy) {
+        PRINTM(MINFO, "InterpretIE: AP WEP enabled\n");
+        pbss_entry->privacy = Wlan802_11PrivFilter8021xWEP;
+    } else {
+        pbss_entry->privacy = Wlan802_11PrivFilterAcceptAll;
+    }
+
+    if (pcap_info->ibss == 1) {
+        pbss_entry->bss_mode = MLAN_BSS_MODE_IBSS;
+    } else {
+        pbss_entry->bss_mode = MLAN_BSS_MODE_INFRA;
+    }
+
+    if (pcap_info->spectrum_mgmt == 1) {
+        PRINTM(MINFO, "InterpretIE: 11h- Spectrum Management "
+               "capability bit found\n");
+        pbss_entry->wlan_11h_bss_info.sensed_11h = 1;
+    }
+
+    /* Process variable IE */
+    while (bytes_left_for_current_beacon >= 2) {
+        element_id = (IEEEtypes_ElementId_e) (*((t_u8 *) pcurrent_ptr));
+        element_len = *((t_u8 *) pcurrent_ptr + 1);
+        total_ie_len = element_len + sizeof(IEEEtypes_Header_t);
+
+        if (bytes_left_for_current_beacon < total_ie_len) {
+            PRINTM(MERROR, "InterpretIE: Error in processing IE, "
+                   "bytes left < IE length\n");
+            bytes_left_for_current_beacon = 0;
+            continue;
+        }
+
+        switch (element_id) {
+
+        case SSID:
+            if (element_len > MRVDRV_MAX_SSID_LENGTH) {
+                bytes_left_for_current_beacon = 0;
+                continue;
+            }
+            pbss_entry->ssid.ssid_len = element_len;
+            memcpy(pmadapter, pbss_entry->ssid.ssid, (pcurrent_ptr + 2),
+                   element_len);
+            PRINTM(MINFO, "InterpretIE: ssid: %-32s\n", pbss_entry->ssid.ssid);
+            break;
+
+        case SUPPORTED_RATES:
+            if (element_len > WLAN_SUPPORTED_RATES) {
+                bytes_left_for_current_beacon = 0;
+                continue;
+            }
+            memcpy(pmadapter, pbss_entry->data_rates, pcurrent_ptr + 2,
+                   element_len);
+            memcpy(pmadapter, pbss_entry->supported_rates, pcurrent_ptr + 2,
+                   element_len);
+            HEXDUMP("InterpretIE: SupportedRates:", pbss_entry->supported_rates,
+                    element_len);
+            rate_size = element_len;
+            found_data_rate_ie = MTRUE;
+            break;
+
+        case FH_PARAM_SET:
+            pfh_param_set = (IEEEtypes_FhParamSet_t *) pcurrent_ptr;
+            pbss_entry->network_type_use = Wlan802_11FH;
+            memcpy(pmadapter, &pbss_entry->phy_param_set.fh_param_set,
+                   pfh_param_set, MIN(total_ie_len,
+                                      sizeof(IEEEtypes_FhParamSet_t)));
+            pbss_entry->phy_param_set.fh_param_set.len =
+                MIN(element_len, (sizeof(IEEEtypes_FhParamSet_t)
+                                  - sizeof(IEEEtypes_Header_t)));
+            pbss_entry->phy_param_set.fh_param_set.dwell_time
+                =
+                wlan_le16_to_cpu(pbss_entry->phy_param_set.fh_param_set.
+                                 dwell_time);
+            break;
+
+        case DS_PARAM_SET:
+            pds_param_set = (IEEEtypes_DsParamSet_t *) pcurrent_ptr;
+
+            pbss_entry->network_type_use = Wlan802_11DS;
+            pbss_entry->channel = pds_param_set->current_chan;
+
+            memcpy(pmadapter, &pbss_entry->phy_param_set.ds_param_set,
+                   pds_param_set, MIN(total_ie_len,
+                                      sizeof(IEEEtypes_DsParamSet_t)));
+            pbss_entry->phy_param_set.ds_param_set.len =
+                MIN(element_len, (sizeof(IEEEtypes_DsParamSet_t)
+                                  - sizeof(IEEEtypes_Header_t)));
+            break;
+
+        case CF_PARAM_SET:
+            pcf_param_set = (IEEEtypes_CfParamSet_t *) pcurrent_ptr;
+            memcpy(pmadapter, &pbss_entry->ss_param_set.cf_param_set,
+                   pcf_param_set, MIN(total_ie_len,
+                                      sizeof(IEEEtypes_CfParamSet_t)));
+            pbss_entry->ss_param_set.cf_param_set.len =
+                MIN(element_len, (sizeof(IEEEtypes_CfParamSet_t)
+                                  - sizeof(IEEEtypes_Header_t)));
+            break;
+
+        case IBSS_PARAM_SET:
+            pibss_param_set = (IEEEtypes_IbssParamSet_t *) pcurrent_ptr;
+            pbss_entry->atim_window =
+                wlan_le16_to_cpu(pibss_param_set->atim_window);
+            memcpy(pmadapter, &pbss_entry->ss_param_set.ibss_param_set,
+                   pibss_param_set, MIN(total_ie_len,
+                                        sizeof(IEEEtypes_IbssParamSet_t)));
+            pbss_entry->ss_param_set.ibss_param_set.len =
+                MIN(element_len, (sizeof(IEEEtypes_IbssParamSet_t)
+                                  - sizeof(IEEEtypes_Header_t)));
+            break;
+
+            /* Handle Country Info IE */
+        case COUNTRY_INFO:
+            pcountry_info = (IEEEtypes_CountryInfoSet_t *) pcurrent_ptr;
+
+            if (pcountry_info->len < sizeof(pcountry_info->country_code) ||
+                (unsigned) (pcountry_info->len + 2) >
+                sizeof(IEEEtypes_CountryInfoFullSet_t)) {
+                PRINTM(MERROR,
+                       "InterpretIE: 11D- Err "
+                       "country_info len =%d min=%d max=%d\n",
+                       pcountry_info->len, sizeof(pcountry_info->country_code),
+                       sizeof(IEEEtypes_CountryInfoFullSet_t));
+                LEAVE();
+                return MLAN_STATUS_FAILURE;
+            }
+
+            memcpy(pmadapter, &pbss_entry->country_info,
+                   pcountry_info, pcountry_info->len + 2);
+            HEXDUMP("InterpretIE: 11D- country_info:",
+                    (t_u8 *) pcountry_info, (t_u32) (pcountry_info->len + 2));
+            break;
+
+        case ERP_INFO:
+            perp_info = (IEEEtypes_ERPInfo_t *) pcurrent_ptr;
+            pbss_entry->erp_flags = perp_info->erp_flags;
+            break;
+
+        case POWER_CONSTRAINT:
+        case POWER_CAPABILITY:
+        case TPC_REPORT:
+        case CHANNEL_SWITCH_ANN:
+        case QUIET:
+        case IBSS_DFS:
+        case SUPPORTED_CHANNELS:
+        case TPC_REQUEST:
+            wlan_11h_process_bss_elem(pmadapter, &pbss_entry->wlan_11h_bss_info,
+                                      pcurrent_ptr);
+            break;
+        case EXTENDED_SUPPORTED_RATES:
+            /*
+             * Only process extended supported rate
+             * if data rate is already found.
+             * Data rate IE should come before
+             * extended supported rate IE
+             */
+            if (found_data_rate_ie) {
+                if ((element_len + rate_size) > WLAN_SUPPORTED_RATES) {
+                    bytes_to_copy = (WLAN_SUPPORTED_RATES - rate_size);
+                } else {
+                    bytes_to_copy = element_len;
+                }
+
+                prate = (t_u8 *) pbss_entry->data_rates;
+                prate += rate_size;
+                memcpy(pmadapter, prate, pcurrent_ptr + 2, bytes_to_copy);
+
+                prate = (t_u8 *) pbss_entry->supported_rates;
+                prate += rate_size;
+                memcpy(pmadapter, prate, pcurrent_ptr + 2, bytes_to_copy);
+            }
+            HEXDUMP("InterpretIE: ExtSupportedRates:",
+                    pbss_entry->supported_rates, element_len + rate_size);
+            break;
+
+        case VENDOR_SPECIFIC_221:
+            pvendor_ie = (IEEEtypes_VendorSpecific_t *) pcurrent_ptr;
+
+            if (!memcmp
+                (pmadapter, pvendor_ie->vend_hdr.oui, wpa_oui,
+                 sizeof(wpa_oui))) {
+                pbss_entry->pwpa_ie =
+                    (IEEEtypes_VendorSpecific_t *) pcurrent_ptr;
+                pbss_entry->wpa_offset =
+                    (t_u16) (pcurrent_ptr - pbss_entry->pbeacon_buf);
+                HEXDUMP("InterpretIE: Resp WPA_IE",
+                        (t_u8 *) pbss_entry->pwpa_ie,
+                        ((*(pbss_entry->pwpa_ie)).vend_hdr.len +
+                         sizeof(IEEEtypes_Header_t)));
+            } else
+                if (!memcmp
+                    (pmadapter, pvendor_ie->vend_hdr.oui, wmm_oui,
+                     sizeof(wmm_oui))) {
+                if (total_ie_len == sizeof(IEEEtypes_WmmParameter_t)
+                    || total_ie_len == sizeof(IEEEtypes_WmmInfo_t)) {
+
+                    /*
+                     * Only accept and copy the WMM IE if it matches
+                     * the size expected for the WMM Info IE or the
+                     * WMM Parameter IE.
+                     */
+                    memcpy(pmadapter, (t_u8 *) & pbss_entry->wmm_ie,
+                           pcurrent_ptr, total_ie_len);
+                    HEXDUMP("InterpretIE: Resp WMM_IE",
+                            (t_u8 *) & pbss_entry->wmm_ie, total_ie_len);
+                }
+            }
+            break;
+        case RSN_IE:
+            pbss_entry->prsn_ie = (IEEEtypes_Generic_t *) pcurrent_ptr;
+            pbss_entry->rsn_offset =
+                (t_u16) (pcurrent_ptr - pbss_entry->pbeacon_buf);
+            HEXDUMP("InterpretIE: Resp RSN_IE", (t_u8 *) pbss_entry->prsn_ie,
+                    (*(pbss_entry->prsn_ie)).ieee_hdr.len +
+                    sizeof(IEEEtypes_Header_t));
+            break;
+        case WAPI_IE:
+            pbss_entry->pwapi_ie = (IEEEtypes_Generic_t *) pcurrent_ptr;
+            pbss_entry->wapi_offset =
+                (t_u16) (pcurrent_ptr - pbss_entry->pbeacon_buf);
+            HEXDUMP("InterpretIE: Resp WAPI_IE", (t_u8 *) pbss_entry->pwapi_ie,
+                    (*(pbss_entry->pwapi_ie)).ieee_hdr.len +
+                    sizeof(IEEEtypes_Header_t));
+            break;
+        case HT_CAPABILITY:
+            pbss_entry->pht_cap = (IEEEtypes_HTCap_t *) pcurrent_ptr;
+            pbss_entry->ht_cap_offset =
+                (t_u16) (pcurrent_ptr - pbss_entry->pbeacon_buf);
+            HEXDUMP("InterpretIE: Resp HTCAP_IE", (t_u8 *) pbss_entry->pht_cap,
+                    (*(pbss_entry->pht_cap)).ieee_hdr.len +
+                    sizeof(IEEEtypes_Header_t));
+            break;
+        case HT_OPERATION:
+            pbss_entry->pht_info = (IEEEtypes_HTInfo_t *) pcurrent_ptr;
+            pbss_entry->ht_info_offset =
+                (t_u16) (pcurrent_ptr - pbss_entry->pbeacon_buf);
+            HEXDUMP("InterpretIE: Resp HTINFO_IE",
+                    (t_u8 *) pbss_entry->pht_info,
+                    (*(pbss_entry->pht_info)).ieee_hdr.len +
+                    sizeof(IEEEtypes_Header_t));
+            break;
+        case BSSCO_2040:
+            pbss_entry->pbss_co_2040 = (IEEEtypes_2040BSSCo_t *) pcurrent_ptr;
+            pbss_entry->bss_co_2040_offset =
+                (t_u16) (pcurrent_ptr - pbss_entry->pbeacon_buf);
+            HEXDUMP("InterpretIE: Resp 2040BSSCOEXISTANCE_IE",
+                    (t_u8 *) pbss_entry->pbss_co_2040,
+                    (*(pbss_entry->pbss_co_2040)).ieee_hdr.len +
+                    sizeof(IEEEtypes_Header_t));
+            break;
+        case EXT_CAPABILITY:
+            pbss_entry->pext_cap = (IEEEtypes_ExtCap_t *) pcurrent_ptr;
+            pbss_entry->ext_cap_offset =
+                (t_u16) (pcurrent_ptr - pbss_entry->pbeacon_buf);
+            HEXDUMP("InterpretIE: Resp EXTCAP_IE",
+                    (t_u8 *) pbss_entry->pext_cap,
+                    (*(pbss_entry->pext_cap)).ieee_hdr.len +
+                    sizeof(IEEEtypes_Header_t));
+            break;
+        case OVERLAPBSSSCANPARAM:
+            pbss_entry->poverlap_bss_scan_param =
+                (IEEEtypes_OverlapBSSScanParam_t *) pcurrent_ptr;
+            pbss_entry->overlap_bss_offset =
+                (t_u16) (pcurrent_ptr - pbss_entry->pbeacon_buf);
+            HEXDUMP("InterpretIE: Resp HTCAP_IE",
+                    (t_u8 *) pbss_entry->poverlap_bss_scan_param,
+                    (*(pbss_entry->poverlap_bss_scan_param)).ieee_hdr.len +
+                    sizeof(IEEEtypes_Header_t));
+            break;
+        }
+
+        pcurrent_ptr += element_len + 2;
+
+        /* Need to account for IE ID and IE Len */
+        bytes_left_for_current_beacon -= (element_len + 2);
+
+    }                           /* while (bytes_left_for_current_beacon > 2) */
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Adjust ie's position in BSSDescriptor_t
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pbss_entry   A pointer to BSSDescriptor_t structure
+ *
+ *  @return           N/A
+ */
+static t_void
+wlan_adjust_ie_in_bss_entry(IN mlan_private * pmpriv,
+                            IN BSSDescriptor_t * pbss_entry)
+{
+    ENTER();
+    if (pbss_entry->pbeacon_buf) {
+        if (pbss_entry->pwpa_ie) {
+            pbss_entry->pwpa_ie = (IEEEtypes_VendorSpecific_t *)
+                (pbss_entry->pbeacon_buf + pbss_entry->wpa_offset);
+        }
+        if (pbss_entry->prsn_ie) {
+            pbss_entry->prsn_ie = (IEEEtypes_Generic_t *)
+                (pbss_entry->pbeacon_buf + pbss_entry->rsn_offset);
+        }
+        if (pbss_entry->pwapi_ie) {
+            pbss_entry->pwapi_ie = (IEEEtypes_Generic_t *)
+                (pbss_entry->pbeacon_buf + pbss_entry->wapi_offset);
+        }
+        if (pbss_entry->pht_cap) {
+            pbss_entry->pht_cap = (IEEEtypes_HTCap_t *)
+                (pbss_entry->pbeacon_buf + pbss_entry->ht_cap_offset);
+        }
+        if (pbss_entry->pht_info) {
+            pbss_entry->pht_info = (IEEEtypes_HTInfo_t *)
+                (pbss_entry->pbeacon_buf + pbss_entry->ht_info_offset);
+        }
+        if (pbss_entry->pbss_co_2040) {
+            pbss_entry->pbss_co_2040 = (IEEEtypes_2040BSSCo_t *)
+                (pbss_entry->pbeacon_buf + pbss_entry->bss_co_2040_offset);
+        }
+        if (pbss_entry->pext_cap) {
+            pbss_entry->pext_cap = (IEEEtypes_ExtCap_t *)
+                (pbss_entry->pbeacon_buf + pbss_entry->ext_cap_offset);
+        }
+        if (pbss_entry->poverlap_bss_scan_param) {
+            pbss_entry->poverlap_bss_scan_param =
+                (IEEEtypes_OverlapBSSScanParam_t *)
+                (pbss_entry->pbeacon_buf + pbss_entry->overlap_bss_offset);
+        }
+    } else {
+        pbss_entry->pwpa_ie = MNULL;
+        pbss_entry->wpa_offset = 0;
+        pbss_entry->prsn_ie = MNULL;
+        pbss_entry->rsn_offset = 0;
+        pbss_entry->pwapi_ie = MNULL;
+        pbss_entry->wapi_offset = 0;
+        pbss_entry->pht_cap = MNULL;
+        pbss_entry->ht_cap_offset = 0;
+        pbss_entry->pht_info = MNULL;
+        pbss_entry->ht_info_offset = 0;
+        pbss_entry->pbss_co_2040 = MNULL;
+        pbss_entry->bss_co_2040_offset = 0;
+        pbss_entry->pext_cap = MNULL;
+        pbss_entry->ext_cap_offset = 0;
+        pbss_entry->poverlap_bss_scan_param = MNULL;
+        pbss_entry->overlap_bss_offset = 0;
+    }
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief Store a beacon or probe response for a BSS returned in the scan
+ *
+ *  Store a new scan response or an update for a previous scan response.  New
+ *    entries need to verify that they do not exceed the total amount of
+ *    memory allocated for the table.
+
+ *  Replacement entries need to take into consideration the amount of space
+ *    currently allocated for the beacon/probe response and adjust the entry
+ *    as needed.
+ *
+ *  A small amount of extra pad (SCAN_BEACON_ENTRY_PAD) is generally reserved
+ *    for an entry in case it is a beacon since a probe response for the
+ *    network will by larger per the standard.  This helps to reduce the
+ *    amount of memory copying to fit a new probe response into an entry
+ *    already occupied by a network's previously stored beacon.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param beacon_idx   Index in the scan table to store this entry; may be
+ *                      replacing an older duplicate entry for this BSS
+ *  @param num_of_ent   Number of entries currently in the table
+ *  @param pnew_beacon  Pointer to the new beacon/probe response to save
+ *
+ *  @return           N/A
+ */
+static t_void
+wlan_ret_802_11_scan_store_beacon(IN mlan_private * pmpriv,
+                                  IN t_u32 beacon_idx,
+                                  IN t_u32 num_of_ent,
+                                  IN BSSDescriptor_t * pnew_beacon)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    t_u8 *pbcn_store;
+    t_u32 new_bcn_size;
+    t_u32 old_bcn_size;
+    t_u32 bcn_space;
+    t_u32 adj_idx;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u8 *tmp_buf;
+    t_u16 bcn_size = 0;
+    t_u32 bcn_offset = 0;
+
+    ENTER();
+
+    if (pmadapter->pscan_table[beacon_idx].pbeacon_buf) {
+
+        new_bcn_size = pnew_beacon->beacon_buf_size;
+        old_bcn_size = pmadapter->pscan_table[beacon_idx].beacon_buf_size;
+        bcn_space = pmadapter->pscan_table[beacon_idx].beacon_buf_size_max;
+        pbcn_store = pmadapter->pscan_table[beacon_idx].pbeacon_buf;
+
+        /* Set the max to be the same as current entry unless changed below */
+        pnew_beacon->beacon_buf_size_max = bcn_space;
+
+        if (new_bcn_size == old_bcn_size) {
+            /*
+             * Beacon is the same size as the previous entry.
+             *   Replace the previous contents with the scan result
+             */
+            memcpy(pmadapter, pbcn_store,
+                   pnew_beacon->pbeacon_buf, pnew_beacon->beacon_buf_size);
+
+        } else if (new_bcn_size <= bcn_space) {
+            /*
+             * New beacon size will fit in the amount of space
+             *   we have previously allocated for it
+             */
+
+            /* Copy the new beacon buffer entry over the old one */
+            memcpy(pmadapter, pbcn_store, pnew_beacon->pbeacon_buf,
+                   new_bcn_size);
+
+            /*
+             *  If the old beacon size was less than the maximum
+             *  we had allotted for the entry, and the new entry
+             *  is even smaller, reset the max size to the old beacon
+             *  entry and compress the storage space (leaving a new
+             *  pad space of (old_bcn_size - new_bcn_size).
+             */
+            if (old_bcn_size < bcn_space && new_bcn_size <= old_bcn_size) {
+                /*
+                 * Old Beacon size is smaller than the allotted storage size.
+                 *   Shrink the allotted storage space.
+                 */
+                PRINTM(MINFO, "AppControl: Smaller Duplicate Beacon (%d), "
+                       "old = %d, new = %d, space = %d, left = %d\n",
+                       beacon_idx, old_bcn_size, new_bcn_size, bcn_space,
+                       (pmadapter->bcn_buf_size -
+                        (pmadapter->pbcn_buf_end - pmadapter->bcn_buf)));
+
+                /*
+                 *  memmove (since the memory overlaps) the data
+                 *  after the beacon we just stored to the end of
+                 *  the current beacon.  This cleans up any unused
+                 *  space the old larger beacon was using in the buffer
+                 */
+                memmove(pmadapter,
+                        (void *) ((t_ptr) pbcn_store + (t_ptr) old_bcn_size),
+                        (void *) ((t_ptr) pbcn_store + (t_ptr) bcn_space),
+                        (t_u32) ((t_ptr) pmadapter->pbcn_buf_end -
+                                 ((t_ptr) pbcn_store + (t_ptr) bcn_space)));
+
+                /*
+                 * Decrement the end pointer by the difference between
+                 *  the old larger size and the new smaller size since
+                 *  we are using less space due to the new beacon being
+                 *  smaller
+                 */
+                pmadapter->pbcn_buf_end -= (bcn_space - old_bcn_size);
+
+                /* Set the maximum storage size to the old beacon size */
+                pnew_beacon->beacon_buf_size_max = old_bcn_size;
+
+                /* Adjust beacon buffer pointers that are past the current */
+                for (adj_idx = 0; adj_idx < num_of_ent; adj_idx++) {
+                    if (pmadapter->pscan_table[adj_idx].pbeacon_buf >
+                        pbcn_store) {
+                        pmadapter->pscan_table[adj_idx].pbeacon_buf -=
+                            (bcn_space - old_bcn_size);
+                        wlan_adjust_ie_in_bss_entry(pmpriv,
+                                                    &pmadapter->
+                                                    pscan_table[adj_idx]);
+                    }
+                }
+            }
+        } else if (pmadapter->pbcn_buf_end + (new_bcn_size - bcn_space)
+                   < (pmadapter->bcn_buf + pmadapter->bcn_buf_size)) {
+            /*
+             * Beacon is larger than space previously allocated (bcn_space)
+             *   and there is enough space left in the beaconBuffer to store
+             *   the additional data
+             */
+            PRINTM(MINFO, "AppControl: Larger Duplicate Beacon (%d), "
+                   "old = %d, new = %d, space = %d, left = %d\n",
+                   beacon_idx, old_bcn_size, new_bcn_size, bcn_space,
+                   (pmadapter->bcn_buf_size -
+                    (pmadapter->pbcn_buf_end - pmadapter->bcn_buf)));
+
+            /*
+             * memmove (since the memory overlaps) the data
+             *  after the beacon we just stored to the end of
+             *  the current beacon.  This moves the data for
+             *  the beacons after this further in memory to
+             *  make space for the new larger beacon we are
+             *  about to copy in.
+             */
+            memmove(pmadapter,
+                    (void *) ((t_ptr) pbcn_store + (t_ptr) new_bcn_size),
+                    (void *) ((t_ptr) pbcn_store + (t_ptr) bcn_space),
+                    (t_u32) ((t_ptr) pmadapter->pbcn_buf_end -
+                             ((t_ptr) pbcn_store + (t_ptr) bcn_space)));
+
+            /* Copy the new beacon buffer entry over the old one */
+            memcpy(pmadapter, pbcn_store, pnew_beacon->pbeacon_buf,
+                   new_bcn_size);
+
+            /* Move the beacon end pointer by the amount of new beacon data we
+               are adding */
+            pmadapter->pbcn_buf_end += (new_bcn_size - bcn_space);
+
+            /*
+             * This entry is bigger than the allotted max space
+             *  previously reserved.  Increase the max space to
+             *  be equal to the new beacon size
+             */
+            pnew_beacon->beacon_buf_size_max = new_bcn_size;
+
+            /* Adjust beacon buffer pointers that are past the current */
+            for (adj_idx = 0; adj_idx < num_of_ent; adj_idx++) {
+                if (pmadapter->pscan_table[adj_idx].pbeacon_buf > pbcn_store) {
+                    pmadapter->pscan_table[adj_idx].pbeacon_buf
+                        += (new_bcn_size - bcn_space);
+                    wlan_adjust_ie_in_bss_entry(pmpriv,
+                                                &pmadapter->
+                                                pscan_table[adj_idx]);
+                }
+            }
+        } else {
+            /*
+             * Beacon is larger than the previously allocated space, but
+             *   there is not enough free space to store the additional data
+             */
+            PRINTM(MERROR,
+                   "AppControl: Failed: Larger Duplicate Beacon (%d),"
+                   " old = %d, new = %d, space = %d, left = %d\n",
+                   beacon_idx, old_bcn_size, new_bcn_size, bcn_space,
+                   (pmadapter->bcn_buf_size -
+                    (pmadapter->pbcn_buf_end - pmadapter->bcn_buf)));
+
+            /* Storage failure, keep old beacon intact */
+            pnew_beacon->beacon_buf_size = old_bcn_size;
+            if (pnew_beacon->pwpa_ie)
+                pnew_beacon->wpa_offset =
+                    pmadapter->pscan_table[beacon_idx].wpa_offset;
+            if (pnew_beacon->prsn_ie)
+                pnew_beacon->rsn_offset =
+                    pmadapter->pscan_table[beacon_idx].rsn_offset;
+            if (pnew_beacon->pwapi_ie)
+                pnew_beacon->wapi_offset =
+                    pmadapter->pscan_table[beacon_idx].wapi_offset;
+            if (pnew_beacon->pht_cap)
+                pnew_beacon->ht_cap_offset =
+                    pmadapter->pscan_table[beacon_idx].ht_cap_offset;
+            if (pnew_beacon->pht_info)
+                pnew_beacon->ht_info_offset =
+                    pmadapter->pscan_table[beacon_idx].ht_info_offset;
+            if (pnew_beacon->pbss_co_2040)
+                pnew_beacon->bss_co_2040_offset =
+                    pmadapter->pscan_table[beacon_idx].bss_co_2040_offset;
+            if (pnew_beacon->pext_cap)
+                pnew_beacon->ext_cap_offset =
+                    pmadapter->pscan_table[beacon_idx].ext_cap_offset;
+            if (pnew_beacon->poverlap_bss_scan_param)
+                pnew_beacon->overlap_bss_offset =
+                    pmadapter->pscan_table[beacon_idx].overlap_bss_offset;
+        }
+        /* Point the new entry to its permanent storage space */
+        pnew_beacon->pbeacon_buf = pbcn_store;
+        wlan_adjust_ie_in_bss_entry(pmpriv, pnew_beacon);
+    } else {
+        if ((pmadapter->pbcn_buf_end + pnew_beacon->beacon_buf_size +
+             SCAN_BEACON_ENTRY_PAD > (pmadapter->bcn_buf +
+                                      pmadapter->bcn_buf_size)) &&
+            (pmadapter->bcn_buf_size < MAX_SCAN_BEACON_BUFFER)) {
+            /* no space for this entry, realloc bcn buffer */
+            ret = pmadapter->callbacks.moal_malloc(pmadapter->pmoal_handle,
+                                                   pmadapter->bcn_buf_size +
+                                                   DEFAULT_SCAN_BEACON_BUFFER,
+                                                   MLAN_MEM_DEF,
+                                                   (t_u8 **) & tmp_buf);
+            if ((ret == MLAN_STATUS_SUCCESS) && (tmp_buf)) {
+                PRINTM(MCMND,
+                       "Realloc Beacon buffer, old size=%d, new_size=%d\n",
+                       pmadapter->bcn_buf_size,
+                       pmadapter->bcn_buf_size + DEFAULT_SCAN_BEACON_BUFFER);
+                bcn_size = pmadapter->pbcn_buf_end - pmadapter->bcn_buf;
+                memcpy(pmadapter, tmp_buf, pmadapter->bcn_buf, bcn_size);
+                /* Adjust beacon buffer pointers that are past the current */
+                for (adj_idx = 0; adj_idx < num_of_ent; adj_idx++) {
+                    bcn_offset =
+                        pmadapter->pscan_table[adj_idx].pbeacon_buf -
+                        pmadapter->bcn_buf;
+                    pmadapter->pscan_table[adj_idx].pbeacon_buf =
+                        tmp_buf + bcn_offset;
+                    wlan_adjust_ie_in_bss_entry(pmpriv,
+                                                &pmadapter->
+                                                pscan_table[adj_idx]);
+                }
+                pmadapter->pbcn_buf_end = tmp_buf + bcn_size;
+                pmadapter->callbacks.moal_mfree(pmadapter->pmoal_handle,
+                                                (t_u8 *) pmadapter->bcn_buf);
+                pmadapter->bcn_buf = tmp_buf;
+                pmadapter->bcn_buf_size += DEFAULT_SCAN_BEACON_BUFFER;
+            }
+        }
+        /*
+         * No existing beacon data exists for this entry, check to see
+         *   if we can fit it in the remaining space
+         */
+        if (pmadapter->pbcn_buf_end + pnew_beacon->beacon_buf_size +
+            SCAN_BEACON_ENTRY_PAD < (pmadapter->bcn_buf +
+                                     pmadapter->bcn_buf_size)) {
+
+            /*
+             * Copy the beacon buffer data from the local entry to the
+             *   adapter dev struct buffer space used to store the raw
+             *   beacon data for each entry in the scan table
+             */
+            memcpy(pmadapter, pmadapter->pbcn_buf_end, pnew_beacon->pbeacon_buf,
+                   pnew_beacon->beacon_buf_size);
+
+            /* Update the beacon ptr to point to the table save area */
+            pnew_beacon->pbeacon_buf = pmadapter->pbcn_buf_end;
+            pnew_beacon->beacon_buf_size_max = (pnew_beacon->beacon_buf_size
+                                                + SCAN_BEACON_ENTRY_PAD);
+            wlan_adjust_ie_in_bss_entry(pmpriv, pnew_beacon);
+
+            /* Increment the end pointer by the size reserved */
+            pmadapter->pbcn_buf_end += pnew_beacon->beacon_buf_size_max;
+
+            PRINTM(MINFO, "AppControl: Beacon[%02d] sz=%03d,"
+                   " used = %04d, left = %04d\n",
+                   beacon_idx,
+                   pnew_beacon->beacon_buf_size,
+                   (pmadapter->pbcn_buf_end - pmadapter->bcn_buf),
+                   (pmadapter->bcn_buf_size -
+                    (pmadapter->pbcn_buf_end - pmadapter->bcn_buf)));
+        } else {
+            /*
+             * No space for new beacon
+             */
+            PRINTM(MCMND, "AppControl: No space beacon (%d): "
+                   "%02x:%02x:%02x:%02x:%02x:%02x; sz=%03d, left=%03d\n",
+                   beacon_idx,
+                   pnew_beacon->mac_address[0], pnew_beacon->mac_address[1],
+                   pnew_beacon->mac_address[2], pnew_beacon->mac_address[3],
+                   pnew_beacon->mac_address[4], pnew_beacon->mac_address[5],
+                   pnew_beacon->beacon_buf_size,
+                   (pmadapter->bcn_buf_size -
+                    (pmadapter->pbcn_buf_end - pmadapter->bcn_buf)));
+
+            /* Storage failure; clear storage records for this bcn */
+            pnew_beacon->pbeacon_buf = MNULL;
+            pnew_beacon->beacon_buf_size = 0;
+            pnew_beacon->beacon_buf_size_max = 0;
+            wlan_adjust_ie_in_bss_entry(pmpriv, pnew_beacon);
+        }
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief Restore a beacon buffer of the current bss descriptor
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *
+ *  @return             N/A
+ */
+static t_void
+wlan_restore_curr_bcn(IN mlan_private * pmpriv)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    mlan_callbacks *pcb = (pmlan_callbacks) & pmadapter->callbacks;
+    BSSDescriptor_t *pcurr_bss = &pmpriv->curr_bss_params.bss_descriptor;
+
+    ENTER();
+
+    if (pmpriv->pcurr_bcn_buf &&
+        ((pmadapter->pbcn_buf_end + pmpriv->curr_bcn_size) <
+         (pmadapter->bcn_buf + pmadapter->bcn_buf_size))) {
+
+        pcb->moal_spin_lock(pmadapter->pmoal_handle, pmpriv->curr_bcn_buf_lock);
+
+        /* restore the current beacon buffer */
+        memcpy(pmadapter, pmadapter->pbcn_buf_end, pmpriv->pcurr_bcn_buf,
+               pmpriv->curr_bcn_size);
+        pcurr_bss->pbeacon_buf = pmadapter->pbcn_buf_end;
+        pcurr_bss->beacon_buf_size = pmpriv->curr_bcn_size;
+        pmadapter->pbcn_buf_end += pmpriv->curr_bcn_size;
+
+        /* adjust the pointers in the current bss descriptor */
+        if (pcurr_bss->pwpa_ie) {
+            pcurr_bss->pwpa_ie = (IEEEtypes_VendorSpecific_t *)
+                (pcurr_bss->pbeacon_buf + pcurr_bss->wpa_offset);
+        }
+        if (pcurr_bss->prsn_ie) {
+            pcurr_bss->prsn_ie = (IEEEtypes_Generic_t *)
+                (pcurr_bss->pbeacon_buf + pcurr_bss->rsn_offset);
+        }
+        if (pcurr_bss->pht_cap) {
+            pcurr_bss->pht_cap = (IEEEtypes_HTCap_t *)
+                (pcurr_bss->pbeacon_buf + pcurr_bss->ht_cap_offset);
+        }
+
+        if (pcurr_bss->pht_info) {
+            pcurr_bss->pht_info = (IEEEtypes_HTInfo_t *)
+                (pcurr_bss->pbeacon_buf + pcurr_bss->ht_info_offset);
+        }
+
+        if (pcurr_bss->pbss_co_2040) {
+            pcurr_bss->pbss_co_2040 = (IEEEtypes_2040BSSCo_t *)
+                (pcurr_bss->pbeacon_buf + pcurr_bss->bss_co_2040_offset);
+        }
+
+        if (pcurr_bss->pext_cap) {
+            pcurr_bss->pext_cap = (IEEEtypes_ExtCap_t *)
+                (pcurr_bss->pbeacon_buf + pcurr_bss->ext_cap_offset);
+        }
+
+        if (pcurr_bss->poverlap_bss_scan_param) {
+            pcurr_bss->poverlap_bss_scan_param =
+                (IEEEtypes_OverlapBSSScanParam_t *)
+                (pcurr_bss->pbeacon_buf + pcurr_bss->overlap_bss_offset);
+        }
+
+        pcb->moal_spin_unlock(pmadapter->pmoal_handle,
+                              pmpriv->curr_bcn_buf_lock);
+
+        PRINTM(MINFO, "current beacon restored %d\n", pmpriv->curr_bcn_size);
+    } else {
+        PRINTM(MWARN, "curr_bcn_buf not saved or bcn_buf has no space\n");
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief Post process the scan table after a new scan command has completed
+ *
+ *  Inspect each entry of the scan table and try to find an entry that
+ *    matches our current associated/joined network from the scan.  If
+ *    one is found, update the stored copy of the BSSDescriptor for our
+ *    current network.
+ *
+ *  Debug dump the current scan table contents if compiled accordingly.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *
+ *  @return             N/A
+ */
+static t_void
+wlan_scan_process_results(IN mlan_private * pmpriv)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    t_s32 j;
+    t_u32 i;
+
+    ENTER();
+
+    if (pmpriv->media_connected == MTRUE) {
+
+        j = wlan_find_ssid_in_list(pmpriv,
+                                   &pmpriv->curr_bss_params.bss_descriptor.ssid,
+                                   pmpriv->curr_bss_params.bss_descriptor.
+                                   mac_address, pmpriv->bss_mode);
+
+        if (j >= 0) {
+            pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                                pmpriv->curr_bcn_buf_lock);
+            pmpriv->curr_bss_params.bss_descriptor.pwpa_ie = MNULL;
+            pmpriv->curr_bss_params.bss_descriptor.wpa_offset = 0;
+            pmpriv->curr_bss_params.bss_descriptor.prsn_ie = MNULL;
+            pmpriv->curr_bss_params.bss_descriptor.rsn_offset = 0;
+            pmpriv->curr_bss_params.bss_descriptor.pwapi_ie = MNULL;
+            pmpriv->curr_bss_params.bss_descriptor.wapi_offset = 0;
+            pmpriv->curr_bss_params.bss_descriptor.pht_cap = MNULL;
+            pmpriv->curr_bss_params.bss_descriptor.ht_cap_offset = 0;
+            pmpriv->curr_bss_params.bss_descriptor.pht_info = MNULL;
+            pmpriv->curr_bss_params.bss_descriptor.ht_info_offset = 0;
+            pmpriv->curr_bss_params.bss_descriptor.pbss_co_2040 = MNULL;
+            pmpriv->curr_bss_params.bss_descriptor.bss_co_2040_offset = 0;
+            pmpriv->curr_bss_params.bss_descriptor.pext_cap = MNULL;
+            pmpriv->curr_bss_params.bss_descriptor.ext_cap_offset = 0;
+            pmpriv->curr_bss_params.bss_descriptor.poverlap_bss_scan_param =
+                MNULL;
+            pmpriv->curr_bss_params.bss_descriptor.overlap_bss_offset = 0;
+            pmpriv->curr_bss_params.bss_descriptor.pbeacon_buf = MNULL;
+            pmpriv->curr_bss_params.bss_descriptor.beacon_buf_size = 0;
+            pmpriv->curr_bss_params.bss_descriptor.beacon_buf_size_max = 0;
+
+            PRINTM(MINFO, "Found current ssid/bssid in list @ index #%d\n", j);
+            /* Make a copy of current BSSID descriptor */
+            memcpy(pmadapter, &pmpriv->curr_bss_params.bss_descriptor,
+                   &pmadapter->pscan_table[j],
+                   sizeof(pmpriv->curr_bss_params.bss_descriptor));
+
+            wlan_save_curr_bcn(pmpriv);
+            pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                                  pmpriv->curr_bcn_buf_lock);
+        } else {
+            wlan_restore_curr_bcn(pmpriv);
+        }
+    }
+
+    for (i = 0; i < pmadapter->num_in_scan_table; i++)
+        PRINTM(MINFO, "Scan:(%02d) %02x:%02x:%02x:%02x:%02x:%02x, "
+               "RSSI[%03d], SSID[%s]\n",
+               i,
+               pmadapter->pscan_table[i].mac_address[0],
+               pmadapter->pscan_table[i].mac_address[1],
+               pmadapter->pscan_table[i].mac_address[2],
+               pmadapter->pscan_table[i].mac_address[3],
+               pmadapter->pscan_table[i].mac_address[4],
+               pmadapter->pscan_table[i].mac_address[5],
+               (t_s32) pmadapter->pscan_table[i].rssi,
+               pmadapter->pscan_table[i].ssid.ssid);
+
+    /*
+     * Prepares domain info from scan table and downloads the
+     *   domain info command to the FW.
+     */
+    wlan_11d_prepare_dnld_domain_info_cmd(pmpriv);
+
+    LEAVE();
+}
+
+/**
+ *  @brief Convert radio type scan parameter to a band config used in join cmd
+ *
+ *  @param radio_type Scan parameter indicating the radio used for a channel
+ *                    in a scan command.
+ *
+ *  @return          Band type conversion of scanBand used in join/assoc cmds
+ *
+ */
+static t_u8
+radio_type_to_band(t_u8 radio_type)
+{
+    t_u8 ret_band;
+
+    switch (radio_type) {
+    case HostCmd_SCAN_RADIO_TYPE_A:
+        ret_band = BAND_A;
+        break;
+    case HostCmd_SCAN_RADIO_TYPE_BG:
+    default:
+        ret_band = BAND_G;
+        break;
+    }
+
+    return ret_band;
+}
+
+/**
+ *  @brief Delete a specific indexed entry from the scan table.
+ *
+ *  Delete the scan table entry indexed by table_idx.  Compact the remaining
+ *    entries and adjust any buffering of beacon/probe response data
+ *    if needed.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param table_idx    Scan table entry index to delete from the table
+ *
+ *  @return             N/A
+ *
+ *  @pre                table_idx must be an index to a valid entry
+ */
+static t_void
+wlan_scan_delete_table_entry(IN mlan_private * pmpriv, IN t_s32 table_idx)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    t_u32 del_idx;
+    t_u32 beacon_buf_adj;
+    t_u8 *pbeacon_buf;
+
+    ENTER();
+
+    /*
+     * Shift the saved beacon buffer data for the scan table back over the
+     *   entry being removed.  Update the end of buffer pointer.  Save the
+     *   deleted buffer allocation size for pointer adjustments for entries
+     *   compacted after the deleted index.
+     */
+    beacon_buf_adj = pmadapter->pscan_table[table_idx].beacon_buf_size_max;
+
+    PRINTM(MINFO, "Scan: Delete Entry %d, beacon buffer removal = %d bytes\n",
+           table_idx, beacon_buf_adj);
+
+    /* Check if the table entry had storage allocated for its beacon */
+    if (beacon_buf_adj) {
+        pbeacon_buf = pmadapter->pscan_table[table_idx].pbeacon_buf;
+
+        /*
+         * Remove the entry's buffer space, decrement the table end pointer
+         *   by the amount we are removing
+         */
+        pmadapter->pbcn_buf_end -= beacon_buf_adj;
+
+        PRINTM(MINFO,
+               "Scan: Delete Entry %d, compact data: %p <- %p (sz = %d)\n",
+               table_idx,
+               pbeacon_buf,
+               pbeacon_buf + beacon_buf_adj,
+               pmadapter->pbcn_buf_end - pbeacon_buf);
+
+        /*
+         * Compact data storage.  Copy all data after the deleted entry's
+         *   end address (pbeacon_buf + beacon_buf_adj) back to the original
+         *   start address (pbeacon_buf).
+         *
+         * Scan table entries affected by the move will have their entry
+         *   pointer adjusted below.
+         *
+         * Use memmove since the dest/src memory regions overlap.
+         */
+        memmove(pmadapter, pbeacon_buf,
+                (void *) ((t_ptr) pbeacon_buf + (t_ptr) beacon_buf_adj),
+                (t_u32) ((t_ptr) pmadapter->pbcn_buf_end -
+                         (t_ptr) pbeacon_buf));
+    }
+
+    PRINTM(MINFO, "Scan: Delete Entry %d, num_in_scan_table = %d\n",
+           table_idx, pmadapter->num_in_scan_table);
+
+    /* Shift all of the entries after the table_idx back by one, compacting the
+       table and removing the requested entry */
+    for (del_idx = table_idx; (del_idx + 1) < pmadapter->num_in_scan_table;
+         del_idx++) {
+        /* Copy the next entry over this one */
+        memcpy(pmadapter, pmadapter->pscan_table + del_idx,
+               pmadapter->pscan_table + del_idx + 1, sizeof(BSSDescriptor_t));
+
+        /*
+         * Adjust this entry's pointer to its beacon buffer based on the
+         *   removed/compacted entry from the deleted index.  Don't decrement
+         *   if the buffer pointer is MNULL (no data stored for this entry).
+         */
+        if (pmadapter->pscan_table[del_idx].pbeacon_buf) {
+            pmadapter->pscan_table[del_idx].pbeacon_buf -= beacon_buf_adj;
+            if (pmadapter->pscan_table[del_idx].pwpa_ie) {
+                pmadapter->pscan_table[del_idx].pwpa_ie =
+                    (IEEEtypes_VendorSpecific_t *)
+                    (pmadapter->pscan_table[del_idx].pbeacon_buf +
+                     pmadapter->pscan_table[del_idx].wpa_offset);
+            }
+            if (pmadapter->pscan_table[del_idx].prsn_ie) {
+                pmadapter->pscan_table[del_idx].prsn_ie =
+                    (IEEEtypes_Generic_t *)
+                    (pmadapter->pscan_table[del_idx].pbeacon_buf +
+                     pmadapter->pscan_table[del_idx].rsn_offset);
+            }
+            if (pmadapter->pscan_table[del_idx].pwapi_ie) {
+                pmadapter->pscan_table[del_idx].pwapi_ie =
+                    (IEEEtypes_Generic_t *)
+                    (pmadapter->pscan_table[del_idx].pbeacon_buf +
+                     pmadapter->pscan_table[del_idx].wapi_offset);
+            }
+            if (pmadapter->pscan_table[del_idx].pht_cap) {
+                pmadapter->pscan_table[del_idx].pht_cap =
+                    (IEEEtypes_HTCap_t *) (pmadapter->pscan_table[del_idx].
+                                           pbeacon_buf +
+                                           pmadapter->pscan_table[del_idx].
+                                           ht_cap_offset);
+            }
+
+            if (pmadapter->pscan_table[del_idx].pht_info) {
+                pmadapter->pscan_table[del_idx].pht_info =
+                    (IEEEtypes_HTInfo_t *) (pmadapter->pscan_table[del_idx].
+                                            pbeacon_buf +
+                                            pmadapter->pscan_table[del_idx].
+                                            ht_info_offset);
+            }
+            if (pmadapter->pscan_table[del_idx].pbss_co_2040) {
+                pmadapter->pscan_table[del_idx].pbss_co_2040 =
+                    (IEEEtypes_2040BSSCo_t *) (pmadapter->pscan_table[del_idx].
+                                               pbeacon_buf +
+                                               pmadapter->pscan_table[del_idx].
+                                               bss_co_2040_offset);
+            }
+            if (pmadapter->pscan_table[del_idx].pext_cap) {
+                pmadapter->pscan_table[del_idx].pext_cap =
+                    (IEEEtypes_ExtCap_t *) (pmadapter->pscan_table[del_idx].
+                                            pbeacon_buf +
+                                            pmadapter->pscan_table[del_idx].
+                                            ext_cap_offset);
+            }
+            if (pmadapter->pscan_table[del_idx].poverlap_bss_scan_param) {
+                pmadapter->pscan_table[del_idx].poverlap_bss_scan_param =
+                    (IEEEtypes_OverlapBSSScanParam_t *) (pmadapter->
+                                                         pscan_table[del_idx].
+                                                         pbeacon_buf +
+                                                         pmadapter->
+                                                         pscan_table[del_idx].
+                                                         overlap_bss_offset);
+            }
+
+        }
+    }
+
+    /* The last entry is invalid now that it has been deleted or moved back */
+    memset(pmadapter, pmadapter->pscan_table + pmadapter->num_in_scan_table - 1,
+           0x00, sizeof(BSSDescriptor_t));
+
+    pmadapter->num_in_scan_table--;
+
+    LEAVE();
+}
+
+/**
+ *  @brief Delete all occurrences of a given SSID from the scan table
+ *
+ *  Iterate through the scan table and delete all entries that match a given
+ *    SSID.  Compact the remaining scan table entries.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pdel_ssid    Pointer to an SSID to be used in deleting all
+ *                        matching SSIDs from the scan table
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_scan_delete_ssid_table_entry(IN mlan_private * pmpriv,
+                                  IN mlan_802_11_ssid * pdel_ssid)
+{
+    mlan_status ret = MLAN_STATUS_FAILURE;
+    t_s32 table_idx;
+
+    ENTER();
+
+    PRINTM(MINFO, "Scan: Delete Ssid Entry: %-32s\n", pdel_ssid->ssid);
+
+    /* If the requested SSID is found in the table, delete it.  Then keep
+       searching the table for multiple entries for the SSID until no more are
+       found */
+    while ((table_idx = wlan_find_ssid_in_list(pmpriv,
+                                               pdel_ssid,
+                                               MNULL,
+                                               MLAN_BSS_MODE_AUTO)) >= 0) {
+        PRINTM(MINFO, "Scan: Delete SSID Entry: Found Idx = %d\n", table_idx);
+        ret = MLAN_STATUS_SUCCESS;
+        wlan_scan_delete_table_entry(pmpriv, table_idx);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/********************************************************
+                Global Functions
+********************************************************/
+
+/**
+ *  @brief Check if a scanned network compatible with the driver settings
+ *
+ *   WEP     WPA     WPA2    ad-hoc  encrypt                      Network
+ * enabled enabled  enabled   AES     mode   Privacy  WPA  WPA2  Compatible
+ *    0       0        0       0      NONE      0      0    0   yes No security
+ *    0       1        0       0       x        1x     1    x   yes WPA (disable HT if no AES)
+ *    0       0        1       0       x        1x     x    1   yes WPA2 (disable HT if no AES)
+ *    0       0        0       1      NONE      1      0    0   yes Ad-hoc AES
+ *    1       0        0       0      NONE      1      0    0   yes Static WEP (disable HT)
+ *    0       0        0       0     !=NONE     1      0    0   yes Dynamic WEP
+ *
+ *  @param pmpriv  A pointer to mlan_private
+ *  @param index   Index in scan table to check against current driver settings
+ *  @param mode    Network mode: Infrastructure or IBSS
+ *
+ *  @return        Index in ScanTable, or negative value if error
+ */
+t_s32
+wlan_is_network_compatible(IN mlan_private * pmpriv,
+                           IN t_u32 index, IN t_u32 mode)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    BSSDescriptor_t *pbss_desc;
+
+    ENTER();
+
+    pbss_desc = &pmadapter->pscan_table[index];
+    pbss_desc->disable_11n = MFALSE;
+
+    /* Don't check for compatibility if roaming */
+    if ((pmpriv->media_connected == MTRUE)
+        && (pmpriv->bss_mode == MLAN_BSS_MODE_INFRA)
+        && (pbss_desc->bss_mode == MLAN_BSS_MODE_INFRA)) {
+        LEAVE();
+        return index;
+    }
+
+    if (pbss_desc->wlan_11h_bss_info.chan_switch_ann.element_id ==
+        CHANNEL_SWITCH_ANN) {
+        PRINTM(MINFO, "Don't connect to AP with CHANNEL_SWITCH_ANN IE.\n");
+        LEAVE();
+        return -1;
+    }
+
+    if (pmpriv->wps.session_enable == MTRUE) {
+        PRINTM(MINFO, "Return success directly in WPS period\n");
+        LEAVE();
+        return index;
+    }
+
+    if ((pbss_desc->bss_mode == mode) &&
+        (pmpriv->sec_info.ewpa_enabled == MTRUE)) {
+        if (((pbss_desc->pwpa_ie) &&
+             ((*(pbss_desc->pwpa_ie)).vend_hdr.element_id == WPA_IE)) ||
+            ((pbss_desc->prsn_ie) &&
+             ((*(pbss_desc->prsn_ie)).ieee_hdr.element_id == RSN_IE))) {
+            if (((pmpriv->adapter->config_bands & BAND_GN ||
+                  pmpriv->adapter->config_bands & BAND_AN) &&
+                 pbss_desc->pht_cap)
+                && (pmpriv->bss_mode == MLAN_BSS_MODE_INFRA)
+                && !is_wpa_oui_present(pmpriv->adapter, pbss_desc,
+                                       CIPHER_SUITE_CCMP)
+                && !is_rsn_oui_present(pmpriv->adapter, pbss_desc,
+                                       CIPHER_SUITE_CCMP)) {
+
+                if (is_wpa_oui_present
+                    (pmpriv->adapter, pbss_desc, CIPHER_SUITE_TKIP)
+                    || is_rsn_oui_present(pmpriv->adapter, pbss_desc,
+                                          CIPHER_SUITE_TKIP)) {
+                    PRINTM(MINFO,
+                           "Disable 11n if AES is not supported by AP\n");
+                    pbss_desc->disable_11n = MTRUE;
+                } else {
+                    LEAVE();
+                    return -1;
+                }
+            }
+            LEAVE();
+            return index;
+        } else {
+            PRINTM(MINFO, "ewpa_enabled: Ignore none WPA/WPA2 AP\n");
+            LEAVE();
+            return -1;
+        }
+    }
+
+    if (pmpriv->sec_info.wapi_enabled &&
+        (pbss_desc->pwapi_ie &&
+         ((*(pbss_desc->pwapi_ie)).ieee_hdr.element_id == WAPI_IE))) {
+        PRINTM(MINFO, "Return success for WAPI AP\n");
+        LEAVE();
+        return index;
+    }
+
+    if (pbss_desc->bss_mode == mode) {
+        if (pmpriv->sec_info.wep_status == Wlan802_11WEPDisabled
+            && !pmpriv->sec_info.wpa_enabled
+            && !pmpriv->sec_info.wpa2_enabled
+            && ((!pbss_desc->pwpa_ie) ||
+                ((*(pbss_desc->pwpa_ie)).vend_hdr.element_id != WPA_IE))
+            && ((!pbss_desc->prsn_ie) ||
+                ((*(pbss_desc->prsn_ie)).ieee_hdr.element_id != RSN_IE))
+            && !pmpriv->adhoc_aes_enabled &&
+            pmpriv->sec_info.encryption_mode == MLAN_ENCRYPTION_MODE_NONE &&
+            !pbss_desc->privacy) {
+            /* No security */
+            LEAVE();
+            return index;
+        } else if (pmpriv->sec_info.wep_status == Wlan802_11WEPEnabled
+                   && !pmpriv->sec_info.wpa_enabled
+                   && !pmpriv->sec_info.wpa2_enabled
+                   && !pmpriv->adhoc_aes_enabled && pbss_desc->privacy) {
+            /* Static WEP enabled */
+            PRINTM(MINFO, "Disable 11n in WEP mode\n");
+            pbss_desc->disable_11n = MTRUE;
+            LEAVE();
+            return index;
+        } else if (pmpriv->sec_info.wep_status == Wlan802_11WEPDisabled
+                   && pmpriv->sec_info.wpa_enabled
+                   && !pmpriv->sec_info.wpa2_enabled
+                   && ((pbss_desc->pwpa_ie) &&
+                       ((*(pbss_desc->pwpa_ie)).vend_hdr.element_id == WPA_IE))
+                   && !pmpriv->adhoc_aes_enabled
+                   /*
+                    * Privacy bit may NOT be set in some APs like LinkSys WRT54G
+                    * && pbss_desc->privacy
+                    */
+            ) {
+            /* WPA enabled */
+            PRINTM(MINFO,
+                   "wlan_is_network_compatible() WPA: index=%d wpa_ie=%#x "
+                   "rsn_ie=%#x WEP=%s WPA=%s WPA2=%s EncMode=%#x "
+                   "privacy=%#x\n", index,
+                   (pbss_desc->pwpa_ie) ? (*(pbss_desc->pwpa_ie)).vend_hdr.
+                   element_id : 0,
+                   (pbss_desc->prsn_ie) ? (*(pbss_desc->prsn_ie)).ieee_hdr.
+                   element_id : 0,
+                   (pmpriv->sec_info.wep_status ==
+                    Wlan802_11WEPEnabled) ? "e" : "d",
+                   (pmpriv->sec_info.wpa_enabled) ? "e" : "d",
+                   (pmpriv->sec_info.wpa2_enabled) ? "e" : "d",
+                   pmpriv->sec_info.encryption_mode, pbss_desc->privacy);
+            if (((pmpriv->adapter->config_bands & BAND_GN ||
+                  pmpriv->adapter->config_bands & BAND_AN) &&
+                 pbss_desc->pht_cap)
+                && (pmpriv->bss_mode == MLAN_BSS_MODE_INFRA)
+                && !is_wpa_oui_present(pmpriv->adapter, pbss_desc,
+                                       CIPHER_SUITE_CCMP)) {
+                if (is_wpa_oui_present
+                    (pmpriv->adapter, pbss_desc, CIPHER_SUITE_TKIP)) {
+                    PRINTM(MINFO,
+                           "Disable 11n if AES is not supported by AP\n");
+                    pbss_desc->disable_11n = MTRUE;
+                } else {
+                    LEAVE();
+                    return -1;
+                }
+            }
+            LEAVE();
+            return index;
+        } else if (pmpriv->sec_info.wep_status == Wlan802_11WEPDisabled
+                   && !pmpriv->sec_info.wpa_enabled
+                   && pmpriv->sec_info.wpa2_enabled
+                   && ((pbss_desc->prsn_ie) &&
+                       ((*(pbss_desc->prsn_ie)).ieee_hdr.element_id == RSN_IE))
+                   && !pmpriv->adhoc_aes_enabled
+                   /*
+                    * Privacy bit may NOT be set in some APs like LinkSys WRT54G
+                    * && pbss_desc->privacy
+                    */
+            ) {
+            /* WPA2 enabled */
+            PRINTM(MINFO,
+                   "wlan_is_network_compatible() WPA2: index=%d wpa_ie=%#x "
+                   "rsn_ie=%#x WEP=%s WPA=%s WPA2=%s EncMode=%#x "
+                   "privacy=%#x\n", index,
+                   (pbss_desc->pwpa_ie) ? (*(pbss_desc->pwpa_ie)).vend_hdr.
+                   element_id : 0,
+                   (pbss_desc->prsn_ie) ? (*(pbss_desc->prsn_ie)).ieee_hdr.
+                   element_id : 0,
+                   (pmpriv->sec_info.wep_status ==
+                    Wlan802_11WEPEnabled) ? "e" : "d",
+                   (pmpriv->sec_info.wpa_enabled) ? "e" : "d",
+                   (pmpriv->sec_info.wpa2_enabled) ? "e" : "d",
+                   pmpriv->sec_info.encryption_mode, pbss_desc->privacy);
+            if (((pmpriv->adapter->config_bands & BAND_GN ||
+                  pmpriv->adapter->config_bands & BAND_AN) &&
+                 pbss_desc->pht_cap)
+                && (pmpriv->bss_mode == MLAN_BSS_MODE_INFRA)
+                && !is_rsn_oui_present(pmpriv->adapter, pbss_desc,
+                                       CIPHER_SUITE_CCMP)) {
+                if (is_rsn_oui_present
+                    (pmpriv->adapter, pbss_desc, CIPHER_SUITE_TKIP)) {
+                    PRINTM(MINFO,
+                           "Disable 11n if AES is not supported by AP\n");
+                    pbss_desc->disable_11n = MTRUE;
+                } else {
+                    LEAVE();
+                    return -1;
+                }
+            }
+            LEAVE();
+            return index;
+        } else if (pmpriv->sec_info.wep_status == Wlan802_11WEPDisabled
+                   && !pmpriv->sec_info.wpa_enabled
+                   && !pmpriv->sec_info.wpa2_enabled
+                   && ((!pbss_desc->pwpa_ie) ||
+                       ((*(pbss_desc->pwpa_ie)).vend_hdr.element_id != WPA_IE))
+                   && ((!pbss_desc->prsn_ie) ||
+                       ((*(pbss_desc->prsn_ie)).ieee_hdr.element_id != RSN_IE))
+                   && pmpriv->adhoc_aes_enabled &&
+                   pmpriv->sec_info.encryption_mode == MLAN_ENCRYPTION_MODE_NONE
+                   && pbss_desc->privacy) {
+            /* Ad-hoc AES enabled */
+            LEAVE();
+            return index;
+        } else if (pmpriv->sec_info.wep_status == Wlan802_11WEPDisabled
+                   && !pmpriv->sec_info.wpa_enabled
+                   && !pmpriv->sec_info.wpa2_enabled
+                   && ((!pbss_desc->pwpa_ie) ||
+                       ((*(pbss_desc->pwpa_ie)).vend_hdr.element_id != WPA_IE))
+                   && ((!pbss_desc->prsn_ie) ||
+                       ((*(pbss_desc->prsn_ie)).ieee_hdr.element_id != RSN_IE))
+                   && !pmpriv->adhoc_aes_enabled &&
+                   pmpriv->sec_info.encryption_mode != MLAN_ENCRYPTION_MODE_NONE
+                   && pbss_desc->privacy) {
+            /* Dynamic WEP enabled */
+            PRINTM(MINFO, "wlan_is_network_compatible() dynamic WEP: index=%d "
+                   "wpa_ie=%#x rsn_ie=%#x EncMode=%#x privacy=%#x\n",
+                   index,
+                   (pbss_desc->pwpa_ie) ? (*(pbss_desc->pwpa_ie)).vend_hdr.
+                   element_id : 0,
+                   (pbss_desc->prsn_ie) ? (*(pbss_desc->prsn_ie)).ieee_hdr.
+                   element_id : 0, pmpriv->sec_info.encryption_mode,
+                   pbss_desc->privacy);
+            LEAVE();
+            return index;
+        }
+
+        /* Security doesn't match */
+        PRINTM(MINFO,
+               "wlan_is_network_compatible() FAILED: index=%d wpa_ie=%#x "
+               "rsn_ie=%#x WEP=%s WPA=%s WPA2=%s EncMode=%#x privacy=%#x\n",
+               index,
+               (pbss_desc->pwpa_ie) ? (*(pbss_desc->pwpa_ie)).vend_hdr.
+               element_id : 0,
+               (pbss_desc->prsn_ie) ? (*(pbss_desc->prsn_ie)).ieee_hdr.
+               element_id : 0,
+               (pmpriv->sec_info.wep_status ==
+                Wlan802_11WEPEnabled) ? "e" : "d",
+               (pmpriv->sec_info.wpa_enabled) ? "e" : "d",
+               (pmpriv->sec_info.wpa2_enabled) ? "e" : "d",
+               pmpriv->sec_info.encryption_mode, pbss_desc->privacy);
+        LEAVE();
+        return -1;
+    }
+
+    /* Mode doesn't match */
+    LEAVE();
+    return -1;
+}
+
+/**
+ *  @brief Internal function used to flush the scan list
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_flush_scan_table(IN pmlan_adapter pmadapter)
+{
+    ENTER();
+
+    PRINTM(MINFO, "Flushing scan table\n");
+
+    memset(pmadapter, pmadapter->pscan_table, 0,
+           (sizeof(BSSDescriptor_t) * MRVDRV_MAX_BSSID_LIST));
+    pmadapter->num_in_scan_table = 0;
+
+    memset(pmadapter, pmadapter->bcn_buf, 0, pmadapter->bcn_buf_size);
+    pmadapter->pbcn_buf_end = pmadapter->bcn_buf;
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Internal function used to start a scan based on an input config
+ *
+ *  Use the input user scan configuration information when provided in
+ *    order to send the appropriate scan commands to firmware to populate or
+ *    update the internal driver scan table
+ *
+ *  @param pmpriv          A pointer to mlan_private structure
+ *  @param pioctl_buf      A pointer to MLAN IOCTL Request buffer
+ *  @param puser_scan_in   Pointer to the input configuration for the requested
+ *                         scan.
+ *
+ *  @return              MLAN_STATUS_SUCCESS or < 0 if error
+ */
+mlan_status
+wlan_scan_networks(IN mlan_private * pmpriv,
+                   IN t_void * pioctl_buf,
+                   IN const wlan_user_scan_cfg * puser_scan_in)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    mlan_callbacks *pcb = (mlan_callbacks *) & pmadapter->callbacks;
+    cmd_ctrl_node *pcmd_node = MNULL;
+    pmlan_ioctl_req pioctl_req = (mlan_ioctl_req *) pioctl_buf;
+
+    wlan_scan_cmd_config_tlv *pscan_cfg_out = MNULL;
+    MrvlIEtypes_ChanListParamSet_t *pchan_list_out;
+    t_u32 buf_size;
+    ChanScanParamSet_t *pscan_chan_list;
+
+    t_u8 keep_previous_scan;
+    t_u8 filtered_scan;
+    t_u8 scan_current_chan_only;
+    t_u8 max_chan_per_scan;
+
+    ENTER();
+
+    ret =
+        pcb->moal_malloc(pmadapter->pmoal_handle,
+                         sizeof(wlan_scan_cmd_config_tlv), MLAN_MEM_DEF,
+                         (t_u8 **) & pscan_cfg_out);
+    if (ret != MLAN_STATUS_SUCCESS || !pscan_cfg_out) {
+        PRINTM(MERROR, "Memory allocation for pscan_cfg_out failed!\n");
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_NO_MEM;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    buf_size = sizeof(ChanScanParamSet_t) * WLAN_USER_SCAN_CHAN_MAX;
+    ret =
+        pcb->moal_malloc(pmadapter->pmoal_handle, buf_size, MLAN_MEM_DEF,
+                         (t_u8 **) & pscan_chan_list);
+    if (ret != MLAN_STATUS_SUCCESS || !pscan_chan_list) {
+        PRINTM(MERROR, "Failed to allocate scan_chan_list\n");
+        if (pscan_cfg_out)
+            pcb->moal_mfree(pmadapter->pmoal_handle, (t_u8 *) pscan_cfg_out);
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_NO_MEM;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    memset(pmadapter, pscan_chan_list, 0x00, buf_size);
+    memset(pmadapter, pscan_cfg_out, 0x00, sizeof(wlan_scan_cmd_config_tlv));
+
+    keep_previous_scan = MFALSE;
+
+    ret = wlan_scan_setup_scan_config(pmpriv,
+                                      puser_scan_in,
+                                      &pscan_cfg_out->config,
+                                      &pchan_list_out,
+                                      pscan_chan_list,
+                                      &max_chan_per_scan,
+                                      &filtered_scan, &scan_current_chan_only);
+    if (ret != MLAN_STATUS_SUCCESS) {
+
+        PRINTM(MERROR, "Failed to setup scan config\n");
+        if (pscan_cfg_out)
+            pcb->moal_mfree(pmadapter->pmoal_handle, (t_u8 *) pscan_cfg_out);
+        if (pscan_chan_list)
+            pcb->moal_mfree(pmadapter->pmoal_handle, (t_u8 *) pscan_chan_list);
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    if (puser_scan_in) {
+        keep_previous_scan = puser_scan_in->keep_previous_scan;
+    }
+
+    if (keep_previous_scan == MFALSE) {
+        memset(pmadapter, pmadapter->pscan_table, 0x00,
+               sizeof(BSSDescriptor_t) * MRVDRV_MAX_BSSID_LIST);
+        pmadapter->num_in_scan_table = 0;
+        pmadapter->pbcn_buf_end = pmadapter->bcn_buf;
+    }
+
+    ret = wlan_scan_channel_list(pmpriv,
+                                 pioctl_buf,
+                                 max_chan_per_scan,
+                                 filtered_scan,
+                                 &pscan_cfg_out->config,
+                                 pchan_list_out, pscan_chan_list);
+
+    /* Get scan command from scan_pending_q and put to cmd_pending_q */
+    if (ret == MLAN_STATUS_SUCCESS) {
+        if (util_peek_list
+            (pmadapter->pmoal_handle, &pmadapter->scan_pending_q,
+             pcb->moal_spin_lock, pcb->moal_spin_unlock)) {
+            pcmd_node =
+                (cmd_ctrl_node *) util_dequeue_list(pmadapter->pmoal_handle,
+                                                    &pmadapter->scan_pending_q,
+                                                    pcb->moal_spin_lock,
+                                                    pcb->moal_spin_unlock);
+            pmadapter->pext_scan_ioctl_req = pioctl_req;
+            wlan_request_cmd_lock(pmadapter);
+            pmadapter->scan_processing = MTRUE;
+            wlan_release_cmd_lock(pmadapter);
+            wlan_insert_cmd_to_pending_q(pmadapter, pcmd_node, MTRUE);
+        }
+    }
+    if (pscan_cfg_out)
+        pcb->moal_mfree(pmadapter->pmoal_handle, (t_u8 *) pscan_cfg_out);
+
+    if (pscan_chan_list)
+        pcb->moal_mfree(pmadapter->pmoal_handle, (t_u8 *) pscan_chan_list);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Prepare a scan command to be sent to the firmware
+ *
+ *  Use the wlan_scan_cmd_config sent to the command processing module in
+ *   the wlan_prepare_cmd to configure a HostCmd_DS_802_11_SCAN command
+ *   struct to send to firmware.
+ *
+ *  The fixed fields specifying the BSS type and BSSID filters as well as a
+ *   variable number/length of TLVs are sent in the command to firmware.
+ *
+ *  @param pmpriv     A pointer to mlan_private structure
+ *  @param pcmd       A pointer to HostCmd_DS_COMMAND structure to be sent to
+ *                    firmware with the HostCmd_DS_801_11_SCAN structure
+ *  @param pdata_buf  Void pointer cast of a wlan_scan_cmd_config struct used
+ *                    to set the fields/TLVs for the command sent to firmware
+ *
+ *  @return           MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_802_11_scan(IN mlan_private * pmpriv,
+                     IN HostCmd_DS_COMMAND * pcmd, IN t_void * pdata_buf)
+{
+    HostCmd_DS_802_11_SCAN *pscan_cmd = &pcmd->params.scan;
+    wlan_scan_cmd_config *pscan_cfg;
+
+    ENTER();
+
+    pscan_cfg = (wlan_scan_cmd_config *) pdata_buf;
+
+    /* Set fixed field variables in scan command */
+    pscan_cmd->bss_mode = pscan_cfg->bss_mode;
+    memcpy(pmpriv->adapter, pscan_cmd->bssid, pscan_cfg->specific_bssid,
+           sizeof(pscan_cmd->bssid));
+    memcpy(pmpriv->adapter, pscan_cmd->tlv_buffer, pscan_cfg->tlv_buf,
+           pscan_cfg->tlv_buf_len);
+
+    pcmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_SCAN);
+
+    /* Size is equal to the sizeof(fixed portions) + the TLV len + header */
+    pcmd->size = (t_u16) wlan_cpu_to_le16((t_u16) (sizeof(pscan_cmd->bss_mode)
+                                                   + sizeof(pscan_cmd->bssid)
+                                                   + pscan_cfg->tlv_buf_len
+                                                   + S_DS_GEN));
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of scan
+ *
+ *   The response buffer for the scan command has the following
+ *      memory layout:
+ *
+ *     .-------------------------------------------------------------.
+ *     |  Header (4 * sizeof(t_u16)):  Standard command response hdr |
+ *     .-------------------------------------------------------------.
+ *     |  BufSize (t_u16) : sizeof the BSS Description data          |
+ *     .-------------------------------------------------------------.
+ *     |  NumOfSet (t_u8) : Number of BSS Descs returned             |
+ *     .-------------------------------------------------------------.
+ *     |  BSSDescription data (variable, size given in BufSize)      |
+ *     .-------------------------------------------------------------.
+ *     |  TLV data (variable, size calculated using Header->Size,    |
+ *     |            BufSize and sizeof the fixed fields above)       |
+ *     .-------------------------------------------------------------.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ret_802_11_scan(IN mlan_private * pmpriv,
+                     IN HostCmd_DS_COMMAND * resp, IN t_void * pioctl_buf)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    mlan_callbacks *pcb = MNULL;
+    mlan_ioctl_req *pioctl_req = (mlan_ioctl_req *) pioctl_buf;
+    cmd_ctrl_node *pcmd_node = MNULL;
+    HostCmd_DS_802_11_SCAN_RSP *pscan_rsp = MNULL;
+    BSSDescriptor_t *bss_new_entry = MNULL;
+    MrvlIEtypes_Data_t *ptlv;
+    MrvlIEtypes_TsfTimestamp_t *ptsf_tlv = MNULL;
+    t_u8 *pbss_info;
+    t_u32 scan_resp_size;
+    t_u32 bytes_left;
+    t_u32 num_in_table;
+    t_u32 bss_idx;
+    t_u32 idx;
+    t_u32 tlv_buf_size;
+    t_u64 tsf_val;
+    chan_freq_power_t *cfp;
+    MrvlIEtypes_ChanBandListParamSet_t *pchan_band_tlv = MNULL;
+    ChanBandParamSet_t *pchan_band;
+    t_u8 band;
+    t_u8 is_bgscan_resp;
+    t_u32 age_ts_usec;
+
+    ENTER();
+    pcb = (pmlan_callbacks) & pmadapter->callbacks;
+
+    is_bgscan_resp = (resp->command == HostCmd_CMD_802_11_BG_SCAN_QUERY);
+    if (is_bgscan_resp) {
+        pscan_rsp = &resp->params.bg_scan_query_resp.scan_resp;
+    } else {
+        pscan_rsp = &resp->params.scan_resp;
+    }
+
+    if (pscan_rsp->number_of_sets > MRVDRV_MAX_BSSID_LIST) {
+        PRINTM(MERROR, "SCAN_RESP: Invalid number of AP returned (%d)!!\n",
+               pscan_rsp->number_of_sets);
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_CMD_SCAN_FAIL;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    bytes_left = wlan_le16_to_cpu(pscan_rsp->bss_descript_size);
+    PRINTM(MINFO, "SCAN_RESP: bss_descript_size %d\n", bytes_left);
+
+    scan_resp_size = resp->size;
+
+    PRINTM(MINFO, "SCAN_RESP: returned %d APs before parsing\n",
+           pscan_rsp->number_of_sets);
+
+    num_in_table = pmadapter->num_in_scan_table;
+    pbss_info = pscan_rsp->bss_desc_and_tlv_buffer;
+
+    /*
+     * The size of the TLV buffer is equal to the entire command response
+     *   size (scan_resp_size) minus the fixed fields (sizeof()'s), the
+     *   BSS Descriptions (bss_descript_size as bytesLef) and the command
+     *   response header (S_DS_GEN)
+     */
+    tlv_buf_size = scan_resp_size - (bytes_left
+                                     + sizeof(pscan_rsp->bss_descript_size)
+                                     + sizeof(pscan_rsp->number_of_sets)
+                                     + S_DS_GEN);
+
+    ptlv =
+        (MrvlIEtypes_Data_t *) (pscan_rsp->bss_desc_and_tlv_buffer +
+                                bytes_left);
+
+    /* Search the TLV buffer space in the scan response for any valid TLVs */
+    wlan_ret_802_11_scan_get_tlv_ptrs(pmadapter,
+                                      ptlv,
+                                      tlv_buf_size,
+                                      TLV_TYPE_TSFTIMESTAMP,
+                                      (MrvlIEtypes_Data_t **) & ptsf_tlv);
+
+    /* Search the TLV buffer space in the scan response for any valid TLVs */
+    wlan_ret_802_11_scan_get_tlv_ptrs(pmadapter,
+                                      ptlv,
+                                      tlv_buf_size,
+                                      TLV_TYPE_CHANNELBANDLIST,
+                                      (MrvlIEtypes_Data_t **) & pchan_band_tlv);
+
+    /*
+     *  Process each scan response returned (pscan_rsp->number_of_sets).  Save
+     *    the information in the bss_new_entry and then insert into the
+     *    driver scan table either as an update to an existing entry
+     *    or as an addition at the end of the table
+     */
+    ret =
+        pcb->moal_malloc(pmadapter->pmoal_handle, sizeof(BSSDescriptor_t),
+                         MLAN_MEM_DEF, (t_u8 **) & bss_new_entry);
+
+    if (ret != MLAN_STATUS_SUCCESS || !bss_new_entry) {
+        PRINTM(MERROR, "Memory allocation for bss_new_entry failed!\n");
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_NO_MEM;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    for (idx = 0; idx < pscan_rsp->number_of_sets && bytes_left; idx++) {
+        /* Zero out the bss_new_entry we are about to store info in */
+        memset(pmadapter, bss_new_entry, 0x00, sizeof(BSSDescriptor_t));
+
+        /* Process the data fields and IEs returned for this BSS */
+        if (wlan_interpret_bss_desc_with_ie(pmadapter,
+                                            bss_new_entry,
+                                            &pbss_info,
+                                            &bytes_left) ==
+            MLAN_STATUS_SUCCESS) {
+            PRINTM(MINFO, "SCAN_RESP: BSSID = %02x:%02x:%02x:%02x:%02x:%02x\n",
+                   bss_new_entry->mac_address[0], bss_new_entry->mac_address[1],
+                   bss_new_entry->mac_address[2], bss_new_entry->mac_address[3],
+                   bss_new_entry->mac_address[4],
+                   bss_new_entry->mac_address[5]);
+            /*
+             * Search the scan table for the same bssid
+             */
+            for (bss_idx = 0; bss_idx < num_in_table; bss_idx++) {
+                if (!memcmp(pmadapter, bss_new_entry->mac_address,
+                            pmadapter->pscan_table[bss_idx].mac_address,
+                            sizeof(bss_new_entry->mac_address))) {
+                    /*
+                     * If the SSID matches as well, it is a duplicate of
+                     *   this entry.  Keep the bss_idx set to this
+                     *   entry so we replace the old contents in the table
+                     */
+                    if ((bss_new_entry->ssid.ssid_len ==
+                         pmadapter->pscan_table[bss_idx].ssid.ssid_len)
+                        && (!memcmp(pmadapter, bss_new_entry->ssid.ssid,
+                                    pmadapter->pscan_table[bss_idx].ssid.ssid,
+                                    bss_new_entry->ssid.ssid_len))) {
+                        PRINTM(MINFO, "SCAN_RESP: Duplicate of index: %d\n",
+                               bss_idx);
+                        break;
+                    }
+                }
+            }
+            /*
+             * If the bss_idx is equal to the number of entries in the table,
+             *   the new entry was not a duplicate; append it to the scan
+             *   table
+             */
+            if (bss_idx == num_in_table) {
+                /* Range check the bss_idx, keep it limited to the last entry */
+                if (bss_idx == MRVDRV_MAX_BSSID_LIST) {
+                    bss_idx--;
+                } else {
+                    num_in_table++;
+                }
+            }
+
+            /*
+             * Save the beacon/probe response returned for later application
+             *   retrieval.  Duplicate beacon/probe responses are updated if
+             *   possible
+             */
+            wlan_ret_802_11_scan_store_beacon(pmpriv,
+                                              bss_idx,
+                                              num_in_table, bss_new_entry);
+            if (bss_new_entry->pbeacon_buf == MNULL) {
+                PRINTM(MCMND, "No space for beacon, drop this entry\n");
+                num_in_table--;
+                continue;
+            }
+            /*
+             * If the TSF TLV was appended to the scan results, save
+             *   this entry's TSF value in the networkTSF field.  The
+             *   networkTSF is the firmware's TSF value at the time the
+             *   beacon or probe response was received.
+             */
+            if (ptsf_tlv) {
+                memcpy(pmpriv->adapter, &tsf_val,
+                       &ptsf_tlv->tsf_data[idx * TSF_DATA_SIZE],
+                       sizeof(tsf_val));
+                tsf_val = wlan_le64_to_cpu(tsf_val);
+                memcpy(pmpriv->adapter, &bss_new_entry->network_tsf,
+                       &tsf_val, sizeof(bss_new_entry->network_tsf));
+            }
+            band = BAND_G;
+            if (pchan_band_tlv) {
+                pchan_band = &pchan_band_tlv->chan_band_param[idx];
+                band =
+                    radio_type_to_band(pchan_band->
+                                       radio_type & (MBIT(0) | MBIT(1)));
+            }
+
+            /* Save the band designation for this entry for use in join */
+            bss_new_entry->bss_band = band;
+            cfp =
+                wlan_find_cfp_by_band_and_channel(pmadapter,
+                                                  (t_u8) bss_new_entry->
+                                                  bss_band,
+                                                  (t_u16) bss_new_entry->
+                                                  channel);
+
+            if (cfp)
+                bss_new_entry->freq = cfp->freq;
+            else
+                bss_new_entry->freq = 0;
+
+            /* Copy the locally created bss_new_entry to the scan table */
+            memcpy(pmadapter, &pmadapter->pscan_table[bss_idx],
+                   bss_new_entry, sizeof(pmadapter->pscan_table[bss_idx]));
+
+        } else {
+
+            /* Error parsing/interpreting the scan response, skipped */
+            PRINTM(MERROR,
+                   "SCAN_RESP: wlan_interpret_bss_desc_with_ie returned error\n");
+        }
+    }
+
+    PRINTM(MINFO, "SCAN_RESP: Scanned %2d APs, %d valid, %d total\n",
+           pscan_rsp->number_of_sets,
+           num_in_table - pmadapter->num_in_scan_table, num_in_table);
+
+    /* Update the total number of BSSIDs in the scan table */
+    pmadapter->num_in_scan_table = num_in_table;
+    /* Update the age_in_second */
+    pmadapter->callbacks.moal_get_system_time(pmadapter->pmoal_handle,
+                                              &pmadapter->age_in_secs,
+                                              &age_ts_usec);
+    if (is_bgscan_resp)
+        goto done;
+    if (!util_peek_list
+        (pmadapter->pmoal_handle, &pmadapter->scan_pending_q,
+         pcb->moal_spin_lock, pcb->moal_spin_unlock)) {
+        wlan_request_cmd_lock(pmadapter);
+        pmadapter->scan_processing = MFALSE;
+        wlan_release_cmd_lock(pmadapter);
+        /*
+         * Process the resulting scan table:
+         *   - Remove any bad ssids
+         *   - Update our current BSS information from scan data
+         */
+        wlan_scan_process_results(pmpriv);
+
+        /* Need to indicate IOCTL complete */
+        if (pioctl_req != MNULL) {
+            pioctl_req->status_code = MLAN_ERROR_NO_ERROR;
+
+            /* Indicate ioctl complete */
+            pcb->moal_ioctl_complete(pmadapter->pmoal_handle,
+                                     (pmlan_ioctl_req) pioctl_buf,
+                                     MLAN_STATUS_SUCCESS);
+        }
+        pmadapter->bgscan_reported = MFALSE;
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_DRV_SCAN_REPORT, MNULL);
+    } else {
+        /* If firmware not ready, do not issue any more scan commands */
+        if (pmadapter->hw_status != WlanHardwareStatusReady) {
+            /* Flush all pending scan commands */
+            wlan_flush_scan_queue(pmadapter);
+            /* Indicate IOCTL complete */
+            if (pioctl_req != MNULL) {
+                pioctl_req->status_code = MLAN_ERROR_FW_NOT_READY;
+
+                /* Indicate ioctl complete */
+                pcb->moal_ioctl_complete(pmadapter->pmoal_handle,
+                                         (pmlan_ioctl_req) pioctl_buf,
+                                         MLAN_STATUS_FAILURE);
+            }
+        } else {
+            /* Get scan command from scan_pending_q and put to cmd_pending_q */
+            pcmd_node =
+                (cmd_ctrl_node *) util_dequeue_list(pmadapter->pmoal_handle,
+                                                    &pmadapter->scan_pending_q,
+                                                    pcb->moal_spin_lock,
+                                                    pcb->moal_spin_unlock);
+
+            wlan_insert_cmd_to_pending_q(pmadapter, pcmd_node, MTRUE);
+        }
+    }
+
+  done:
+    if (bss_new_entry)
+        pcb->moal_mfree(pmadapter->pmoal_handle, (t_u8 *) bss_new_entry);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Prepare an extended scan command to be sent to the firmware
+ *
+ *  Use the wlan_scan_cmd_config sent to the command processing module in
+ *   the wlan_prepare_cmd to configure a HostCmd_DS_802_11_SCAN_EXT command
+ *   struct to send to firmware.
+ *
+ *  @param pmpriv     A pointer to mlan_private structure
+ *  @param pcmd       A pointer to HostCmd_DS_COMMAND structure to be sent to
+ *                    firmware with the HostCmd_DS_802_11_SCAN_EXT structure
+ *  @param pdata_buf  Void pointer cast of a wlan_scan_cmd_config struct used
+ *                    to set the fields/TLVs for the command sent to firmware
+ *
+ *  @return           MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_802_11_scan_ext(IN mlan_private * pmpriv,
+                         IN HostCmd_DS_COMMAND * pcmd, IN t_void * pdata_buf)
+{
+    HostCmd_DS_802_11_SCAN_EXT *pext_scan_cmd = &pcmd->params.ext_scan;
+    wlan_scan_cmd_config *pscan_cfg = MNULL;
+
+    ENTER();
+
+    pscan_cfg = (wlan_scan_cmd_config *) pdata_buf;
+
+    memcpy(pmpriv->adapter, pext_scan_cmd->tlv_buffer,
+           pscan_cfg->tlv_buf, pscan_cfg->tlv_buf_len);
+
+    pcmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_SCAN_EXT);
+
+    /* Size is equal to the sizeof(fixed portions) + the TLV len + header */
+    pcmd->size = wlan_cpu_to_le16((t_u16) (sizeof(pext_scan_cmd->reserved)
+                                           + pscan_cfg->tlv_buf_len
+                                           + S_DS_GEN));
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of extended scan
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ret_802_11_scan_ext(IN mlan_private * pmpriv,
+                         IN HostCmd_DS_COMMAND * resp, IN t_void * pioctl_buf)
+{
+    ENTER();
+
+    PRINTM(MINFO, "EXT scan returns successfully\n");
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function parse and store the extended scan results
+ *
+ *  @param pmpriv           A pointer to mlan_private structure
+ *  @param number_of_sets   Number of BSS
+ *  @param pscan_resp       A pointer to scan response buffer
+ *  @param scan_resp_size   Size of scan response buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_parse_ext_scan_result(IN mlan_private * pmpriv,
+                           IN t_u8 number_of_sets,
+                           IN t_u8 * pscan_resp, IN t_u16 scan_resp_size)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    mlan_callbacks *pcb = MNULL;
+    BSSDescriptor_t *bss_new_entry = MNULL;
+    t_u8 *pbss_info;
+    t_u32 bytes_left;
+    t_u32 bytes_left_for_tlv;
+    t_u32 num_in_table;
+    t_u32 bss_idx;
+    t_u32 idx;
+    t_u64 tsf_val;
+    chan_freq_power_t *cfp;
+    t_u16 tlv_type, tlv_len;
+    MrvlIEtypes_Data_t *ptlv = MNULL;
+    MrvlIEtypes_Bss_Scan_Rsp_t *pscan_rsp_tlv = MNULL;
+    MrvlIEtypes_Bss_Scan_Info_t *pscan_info_tlv = MNULL;
+    t_u8 band;
+    t_u32 age_ts_usec;
+
+    ENTER();
+    pcb = (pmlan_callbacks) & pmadapter->callbacks;
+
+    if (number_of_sets > MRVDRV_MAX_BSSID_LIST) {
+        PRINTM(MERROR, "EXT_SCAN: Invalid number of AP returned (%d)!!\n",
+               number_of_sets);
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    bytes_left = scan_resp_size;
+    PRINTM(MINFO, "EXT_SCAN: bss_descript_size %d\n", scan_resp_size);
+    PRINTM(MINFO, "EXT_SCAN: returned %d APs before parsing\n", number_of_sets);
+
+    num_in_table = pmadapter->num_in_scan_table;
+    ptlv = (MrvlIEtypes_Data_t *) pscan_resp;
+
+    /*
+     *  Process each scan response returned number_of_sets. Save
+     *    the information in the bss_new_entry and then insert into the
+     *    driver scan table either as an update to an existing entry
+     *    or as an addition at the end of the table
+     */
+    ret =
+        pcb->moal_malloc(pmadapter->pmoal_handle, sizeof(BSSDescriptor_t),
+                         MLAN_MEM_DEF, (t_u8 **) & bss_new_entry);
+
+    if (ret != MLAN_STATUS_SUCCESS || !bss_new_entry) {
+        PRINTM(MERROR, "Memory allocation for bss_new_entry failed!\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    for (idx = 0; idx < number_of_sets && bytes_left >
+         sizeof(MrvlIEtypesHeader_t); idx++) {
+        tlv_type = wlan_le16_to_cpu(ptlv->header.type);
+        tlv_len = wlan_le16_to_cpu(ptlv->header.len);
+        if (bytes_left < sizeof(MrvlIEtypesHeader_t) + tlv_len) {
+            PRINTM(MERROR, "EXT_SCAN: Error bytes left < TLV length\n");
+            break;
+        }
+        pscan_rsp_tlv = MNULL;
+        pscan_info_tlv = MNULL;
+        bytes_left_for_tlv = bytes_left;
+        /* BSS response TLV with beacon or probe response buffer at the initial
+           position of each descriptor */
+        if (tlv_type == TLV_TYPE_BSS_SCAN_RSP) {
+            pbss_info = (t_u8 *) ptlv;
+            pscan_rsp_tlv = (MrvlIEtypes_Bss_Scan_Rsp_t *) ptlv;
+            ptlv = (MrvlIEtypes_Data_t *) (ptlv->data + tlv_len);
+            bytes_left_for_tlv -= (tlv_len + sizeof(MrvlIEtypesHeader_t));
+        } else
+            break;
+
+        /* Process variable TLV */
+        while (bytes_left_for_tlv >= sizeof(MrvlIEtypesHeader_t) &&
+               wlan_le16_to_cpu(ptlv->header.type) != TLV_TYPE_BSS_SCAN_RSP) {
+            tlv_type = wlan_le16_to_cpu(ptlv->header.type);
+            tlv_len = wlan_le16_to_cpu(ptlv->header.len);
+            if (bytes_left_for_tlv < sizeof(MrvlIEtypesHeader_t) + tlv_len) {
+                PRINTM(MERROR, "EXT_SCAN: Error in processing TLV, "
+                       "bytes left < TLV length\n");
+                pscan_rsp_tlv = MNULL;
+                bytes_left_for_tlv = 0;
+                continue;
+            }
+            switch (tlv_type) {
+            case TLV_TYPE_BSS_SCAN_INFO:
+                pscan_info_tlv = (MrvlIEtypes_Bss_Scan_Info_t *) ptlv;
+                if (tlv_len != sizeof(MrvlIEtypes_Bss_Scan_Info_t) -
+                    sizeof(MrvlIEtypesHeader_t)) {
+                    bytes_left_for_tlv = 0;
+                    continue;
+                }
+                break;
+            default:
+                break;
+            }
+            ptlv = (MrvlIEtypes_Data_t *) (ptlv->data + tlv_len);
+            bytes_left -= (tlv_len + sizeof(MrvlIEtypesHeader_t));
+            bytes_left_for_tlv -= (tlv_len + sizeof(MrvlIEtypesHeader_t));
+        }
+        /* No BSS response TLV */
+        if (pscan_rsp_tlv == MNULL)
+            break;
+
+        /* Advance pointer to the beacon buffer length and update the bytes
+           count so that the function wlan_interpret_bss_desc_with_ie() can
+           handle the scan buffer withut any change */
+        pbss_info += sizeof(t_u16);
+        bytes_left -= sizeof(t_u16);
+
+        /* Zero out the bss_new_entry we are about to store info in */
+        memset(pmadapter, bss_new_entry, 0x00, sizeof(BSSDescriptor_t));
+
+        /* Process the data fields and IEs returned for this BSS */
+        if (wlan_interpret_bss_desc_with_ie(pmadapter,
+                                            bss_new_entry,
+                                            &pbss_info,
+                                            &bytes_left) ==
+            MLAN_STATUS_SUCCESS) {
+            PRINTM(MINFO, "EXT_SCAN: BSSID = %02x:%02x:%02x:%02x:%02x:%02x\n",
+                   bss_new_entry->mac_address[0], bss_new_entry->mac_address[1],
+                   bss_new_entry->mac_address[2], bss_new_entry->mac_address[3],
+                   bss_new_entry->mac_address[4],
+                   bss_new_entry->mac_address[5]);
+            /*
+             * Search the scan table for the same bssid
+             */
+            for (bss_idx = 0; bss_idx < num_in_table; bss_idx++) {
+                if (!memcmp(pmadapter, bss_new_entry->mac_address,
+                            pmadapter->pscan_table[bss_idx].mac_address,
+                            sizeof(bss_new_entry->mac_address))) {
+                    /*
+                     * If the SSID matches as well, it is a duplicate of
+                     *   this entry.  Keep the bss_idx set to this
+                     *   entry so we replace the old contents in the table
+                     */
+                    if ((bss_new_entry->ssid.ssid_len ==
+                         pmadapter->pscan_table[bss_idx].ssid.ssid_len)
+                        && (!memcmp(pmadapter, bss_new_entry->ssid.ssid,
+                                    pmadapter->pscan_table[bss_idx].ssid.ssid,
+                                    bss_new_entry->ssid.ssid_len))) {
+                        PRINTM(MINFO, "EXT_SCAN: Duplicate of index: %d\n",
+                               bss_idx);
+                        break;
+                    }
+                }
+            }
+            /*
+             * If the bss_idx is equal to the number of entries in the table,
+             *   the new entry was not a duplicate; append it to the scan
+             *   table
+             */
+            if (bss_idx == num_in_table) {
+                /* Range check the bss_idx, keep it limited to the last entry */
+                if (bss_idx == MRVDRV_MAX_BSSID_LIST) {
+                    bss_idx--;
+                } else {
+                    num_in_table++;
+                }
+            }
+
+            band = BAND_G;
+            /*
+             * If the BSS info TLV was appended to the scan results, save
+             *   this entry's TSF value in the networkTSF field. The
+             *   networkTSF is the firmware's TSF value at the time the
+             *   beacon or probe response was received.
+             */
+            if (pscan_info_tlv) {
+                /* RSSI is 2 byte long */
+                bss_new_entry->rssi =
+                    -(t_s32) (wlan_le16_to_cpu(pscan_info_tlv->rssi));
+                PRINTM(MINFO, "EXT_SCAN: RSSI=%d\n", bss_new_entry->rssi);
+                memcpy(pmpriv->adapter, &tsf_val, &pscan_info_tlv->tsf,
+                       sizeof(tsf_val));
+                tsf_val = wlan_le64_to_cpu(tsf_val);
+                memcpy(pmpriv->adapter, &bss_new_entry->network_tsf,
+                       &tsf_val, sizeof(bss_new_entry->network_tsf));
+                band = radio_type_to_band(pscan_info_tlv->band);
+            }
+            /*
+             * Save the beacon/probe response returned for later application
+             *   retrieval. Duplicate beacon/probe responses are updated if
+             *   possible
+             */
+            wlan_ret_802_11_scan_store_beacon(pmpriv,
+                                              bss_idx,
+                                              num_in_table, bss_new_entry);
+            if (bss_new_entry->pbeacon_buf == MNULL) {
+                PRINTM(MCMND, "No space for beacon, drop this entry\n");
+                num_in_table--;
+                continue;
+            }
+            /* Save the band designation for this entry for use in join */
+            bss_new_entry->bss_band = band;
+            cfp = wlan_find_cfp_by_band_and_channel(pmadapter,
+                                                    (t_u8) bss_new_entry->
+                                                    bss_band,
+                                                    (t_u16) bss_new_entry->
+                                                    channel);
+
+            if (cfp)
+                bss_new_entry->freq = cfp->freq;
+            else
+                bss_new_entry->freq = 0;
+
+            /* Copy the locally created bss_new_entry to the scan table */
+            memcpy(pmadapter, &pmadapter->pscan_table[bss_idx],
+                   bss_new_entry, sizeof(pmadapter->pscan_table[bss_idx]));
+        } else {
+            /* Error parsing/interpreting the scan response, skipped */
+            PRINTM(MERROR,
+                   "EXT_SCAN: wlan_interpret_bss_desc_with_ie returned error\n");
+        }
+    }
+
+    PRINTM(MINFO, "EXT_SCAN: Scanned %2d APs, %d valid, %d total\n",
+           number_of_sets, num_in_table - pmadapter->num_in_scan_table,
+           num_in_table);
+
+    /* Update the total number of BSSIDs in the scan table */
+    pmadapter->num_in_scan_table = num_in_table;
+    /* Update the age_in_second */
+    pmadapter->callbacks.moal_get_system_time(pmadapter->pmoal_handle,
+                                              &pmadapter->age_in_secs,
+                                              &age_ts_usec);
+
+  done:
+    if (bss_new_entry)
+        pcb->moal_mfree(pmadapter->pmoal_handle, (t_u8 *) bss_new_entry);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function handles the event extended scan report
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pmbuf        A pointer to mlan_buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_handle_event_ext_scan_report(IN mlan_private * pmpriv,
+                                  IN mlan_buffer * pmbuf)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    mlan_callbacks *pcb = &pmadapter->callbacks;
+    mlan_ioctl_req *pioctl_req = MNULL;
+    cmd_ctrl_node *pcmd_node = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    mlan_event_scan_result *pevent_scan = (pmlan_event_scan_result)
+        (pmbuf->pbuf + pmbuf->data_offset);
+    t_u8 *ptlv = (pmbuf->pbuf + pmbuf->data_offset
+                  + sizeof(mlan_event_scan_result));
+    t_u16 tlv_buf_left = wlan_cpu_to_le16(pevent_scan->buf_size);
+
+    DBG_HEXDUMP(MCMD_D, "EVENT EXT_SCAN", pmbuf->pbuf +
+                pmbuf->data_offset, pmbuf->data_len);
+    wlan_parse_ext_scan_result(pmpriv, pevent_scan->num_of_set,
+                               ptlv, tlv_buf_left);
+    if (!pevent_scan->more_event) {
+        pioctl_req = pmadapter->pext_scan_ioctl_req;
+        if (!util_peek_list(pmadapter->pmoal_handle,
+                            &pmadapter->scan_pending_q, pcb->moal_spin_lock,
+                            pcb->moal_spin_unlock)) {
+            pmadapter->pext_scan_ioctl_req = MNULL;
+            wlan_request_cmd_lock(pmadapter);
+            pmadapter->scan_processing = MFALSE;
+            wlan_release_cmd_lock(pmadapter);
+            /*
+             * Process the resulting scan table:
+             *   - Remove any bad ssids
+             *   - Update our current BSS information from scan data
+             */
+            wlan_scan_process_results(pmpriv);
+
+            /* Need to indicate IOCTL complete */
+            if (pioctl_req != MNULL) {
+                pioctl_req->status_code = MLAN_ERROR_NO_ERROR;
+                /* Indicate ioctl complete */
+                pcb->moal_ioctl_complete(pmadapter->pmoal_handle,
+                                         (pmlan_ioctl_req) pioctl_req,
+                                         MLAN_STATUS_SUCCESS);
+            }
+            pmadapter->bgscan_reported = MFALSE;
+            wlan_recv_event(pmpriv, MLAN_EVENT_ID_DRV_SCAN_REPORT, MNULL);
+        } else {
+            /* If firmware not ready, do not issue any more scan commands */
+            if (pmadapter->hw_status != WlanHardwareStatusReady) {
+                /* Flush all pending scan commands */
+                wlan_flush_scan_queue(pmadapter);
+                /* Indicate IOCTL complete */
+                if (pioctl_req != MNULL) {
+                    pioctl_req->status_code = MLAN_ERROR_FW_NOT_READY;
+
+                    /* Indicate ioctl complete */
+                    pcb->moal_ioctl_complete(pmadapter->pmoal_handle,
+                                             (pmlan_ioctl_req) pioctl_req,
+                                             MLAN_STATUS_FAILURE);
+                }
+            } else {
+                /* Get scan command from scan_pending_q and put to
+                   cmd_pending_q */
+                pcmd_node =
+                    (cmd_ctrl_node *) util_dequeue_list(pmadapter->pmoal_handle,
+                                                        &pmadapter->
+                                                        scan_pending_q,
+                                                        pcb->moal_spin_lock,
+                                                        pcb->moal_spin_unlock);
+                wlan_insert_cmd_to_pending_q(pmadapter, pcmd_node, MTRUE);
+            }
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function prepares command of bg_scan_query.
+ *
+ *  @param pmpriv     A pointer to mlan_private structure
+ *  @param pcmd       A pointer to HostCmd_DS_COMMAND structure
+ *  @param pdata_buf  Void pointer cast of a wlan_scan_cmd_config struct used
+ *                    to set the fields/TLVs for the command sent to firmware
+ *
+ *  @return           MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_802_11_bg_scan_query(IN mlan_private * pmpriv,
+                              IN HostCmd_DS_COMMAND * pcmd,
+                              IN t_void * pdata_buf)
+{
+    HostCmd_DS_802_11_BG_SCAN_QUERY *bg_query = &pcmd->params.bg_scan_query;
+
+    ENTER();
+
+    pcmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_BG_SCAN_QUERY);
+    pcmd->size =
+        wlan_cpu_to_le16(sizeof(HostCmd_DS_802_11_BG_SCAN_QUERY) + S_DS_GEN);
+
+    bg_query->flush = MTRUE;
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Create a channel list for the driver to scan based on region info
+ *
+ *  Use the driver region/band information to construct a comprehensive list
+ *    of channels to scan.  This routine is used for any scan that is not
+ *    provided a specific channel list to scan.
+ *
+ *  @param pmpriv           A pointer to mlan_private structure
+ *  @param pbg_scan_in      pointer to scan configuration parameters
+ *  @param tlv_chan_list    A pointer to structure MrvlIEtypes_ChanListParamSet_t
+ *
+ *  @return                 channel number
+ */
+static t_u8
+wlan_bgscan_create_channel_list(IN mlan_private * pmpriv,
+                                IN const wlan_bgscan_cfg * pbg_scan_in,
+                                MrvlIEtypes_ChanListParamSet_t * tlv_chan_list)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    region_chan_t *pscan_region;
+    chan_freq_power_t *cfp;
+    t_u32 region_idx;
+    t_u32 chan_idx = 0;
+    t_u32 next_chan;
+    t_u8 scan_type;
+    t_u8 radio_type;
+
+    ENTER();
+
+    for (region_idx = 0;
+         region_idx < NELEMENTS(pmadapter->region_channel); region_idx++) {
+
+        if (wlan_11d_is_enabled(pmpriv) && pmpriv->media_connected != MTRUE) {
+            /* Scan all the supported chan for the first scan */
+            if (!pmadapter->universal_channel[region_idx].valid)
+                continue;
+            pscan_region = &pmadapter->universal_channel[region_idx];
+        } else {
+            if (!pmadapter->region_channel[region_idx].valid)
+                continue;
+            pscan_region = &pmadapter->region_channel[region_idx];
+        }
+
+        if (pbg_scan_in && !pbg_scan_in->chan_list[0].chan_number &&
+            pbg_scan_in->chan_list[0].radio_type & BAND_SPECIFIED) {
+            radio_type = pbg_scan_in->chan_list[0].radio_type & ~BAND_SPECIFIED;
+            if (!radio_type && (pscan_region->band != BAND_B) &&
+                (pscan_region->band != BAND_G))
+                continue;
+            if (radio_type && (pscan_region->band != BAND_A))
+                continue;
+        }
+        if (!wlan_is_band_compatible
+            (pmpriv->config_bands | pmadapter->adhoc_start_band,
+             pscan_region->band))
+            continue;
+        for (next_chan = 0;
+             next_chan < pscan_region->num_cfp; next_chan++, chan_idx++) {
+            if (chan_idx >= WLAN_BG_SCAN_CHAN_MAX)
+                break;
+            /* Set the default scan type to ACTIVE SCAN type, will later be
+               changed to passive on a per channel basis if restricted by
+               regulatory requirements (11d or 11h) */
+            scan_type = MLAN_SCAN_TYPE_ACTIVE;
+            cfp = pscan_region->pcfp + next_chan;
+            if (scan_type == MLAN_SCAN_TYPE_ACTIVE
+                && wlan_11d_is_enabled(pmpriv)) {
+                scan_type = wlan_11d_get_scan_type(pmadapter,
+                                                   pscan_region->band,
+                                                   (t_u8) cfp->channel,
+                                                   &pmadapter->
+                                                   parsed_region_chan);
+            }
+            switch (pscan_region->band) {
+            case BAND_A:
+                tlv_chan_list->chan_scan_param[chan_idx].radio_type =
+                    HostCmd_SCAN_RADIO_TYPE_A;
+                if (!wlan_11d_is_enabled(pmpriv)) {
+                    /* 11D not available... play it safe on DFS channels */
+                    if (wlan_11h_radar_detect_required
+                        (pmpriv, (t_u8) cfp->channel))
+                        scan_type = MLAN_SCAN_TYPE_PASSIVE;
+                }
+                break;
+            case BAND_B:
+            case BAND_G:
+                if (wlan_bg_scan_type_is_passive(pmpriv, (t_u8) cfp->channel))
+                    scan_type = MLAN_SCAN_TYPE_PASSIVE;
+            default:
+                tlv_chan_list->chan_scan_param[chan_idx].radio_type =
+                    HostCmd_SCAN_RADIO_TYPE_BG;
+                break;
+            }
+
+            if (pbg_scan_in && pbg_scan_in->chan_list[0].scan_time) {
+                tlv_chan_list->chan_scan_param[chan_idx].max_scan_time =
+                    wlan_cpu_to_le16((t_u16) pbg_scan_in->chan_list[0].
+                                     scan_time);
+                tlv_chan_list->chan_scan_param[chan_idx].min_scan_time =
+                    wlan_cpu_to_le16((t_u16) pbg_scan_in->chan_list[0].
+                                     scan_time);
+            } else if (scan_type == MLAN_SCAN_TYPE_PASSIVE) {
+                tlv_chan_list->chan_scan_param[chan_idx].max_scan_time =
+                    wlan_cpu_to_le16(pmadapter->passive_scan_time);
+                tlv_chan_list->chan_scan_param[chan_idx].min_scan_time =
+                    wlan_cpu_to_le16(pmadapter->passive_scan_time);
+            } else {
+                tlv_chan_list->chan_scan_param[chan_idx].max_scan_time =
+                    wlan_cpu_to_le16(pmadapter->specific_scan_time);
+                tlv_chan_list->chan_scan_param[chan_idx].min_scan_time =
+                    wlan_cpu_to_le16(pmadapter->specific_scan_time);
+            }
+
+            if (scan_type == MLAN_SCAN_TYPE_PASSIVE) {
+                tlv_chan_list->chan_scan_param[chan_idx].chan_scan_mode.
+                    passive_scan = MTRUE;
+            } else {
+                tlv_chan_list->chan_scan_param[chan_idx].chan_scan_mode.
+                    passive_scan = MFALSE;
+            }
+
+            tlv_chan_list->chan_scan_param[chan_idx].chan_number =
+                (t_u8) cfp->channel;
+            tlv_chan_list->chan_scan_param[chan_idx].chan_scan_mode.
+                disable_chan_filt = MTRUE;
+        }
+    }
+
+    LEAVE();
+    return chan_idx;
+}
+
+/**
+ *  @brief This function prepares command of bg_scan_config
+ *
+ *  @param pmpriv     A pointer to mlan_private structure
+ *  @param pcmd       A pointer to HostCmd_DS_COMMAND structure
+ *  @param pdata_buf  Void pointer cast of a wlan_scan_cmd_config struct used
+ *                    to set the fields/TLVs for the command sent to firmware
+ *
+ *  @return           MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_bgscan_config(IN mlan_private * pmpriv,
+                       IN HostCmd_DS_COMMAND * pcmd, IN t_void * pdata_buf)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    HostCmd_DS_802_11_BG_SCAN_CONFIG *bg_scan = &pcmd->params.bg_scan_config;
+    wlan_bgscan_cfg *bg_scan_in = (wlan_bgscan_cfg *) pdata_buf;
+    t_u16 cmd_size = 0;
+    MrvlIEtypes_NumProbes_t *pnum_probes_tlv = MNULL;
+    MrvlIEtypes_BeaconLowRssiThreshold_t *rssi_tlv = MNULL;
+    MrvlIEtypes_BeaconLowSnrThreshold_t *snr_tlv = MNULL;
+    MrvlIEtypes_WildCardSsIdParamSet_t *pwildcard_ssid_tlv = MNULL;
+    MrvlIEtypes_ChanListParamSet_t *tlv_chan_list = MNULL;
+    MrvlIEtypes_StartLater_t *tlv_start_later = MNULL;
+    MrvlIEtypes_RepeatCount_t *tlv_repeat = MNULL;
+    t_u8 *tlv = MNULL;
+    t_u16 num_probes = 0;
+    t_u32 ssid_idx;
+    t_u32 ssid_len = 0;
+    t_u32 chan_idx;
+    t_u32 chan_num;
+    t_u8 radio_type;
+    t_u16 scan_dur;
+    t_u8 scan_type;
+
+    ENTER();
+
+    pcmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_BG_SCAN_CONFIG);
+    bg_scan->action = wlan_cpu_to_le16(bg_scan_in->action);
+    bg_scan->enable = bg_scan_in->enable;
+    bg_scan->bss_type = bg_scan_in->bss_type;
+    cmd_size = sizeof(HostCmd_DS_802_11_BG_SCAN_CONFIG) + S_DS_GEN;
+    if (bg_scan_in->chan_per_scan)
+        bg_scan->chan_per_scan = bg_scan_in->chan_per_scan;
+    else
+        bg_scan->chan_per_scan = MRVDRV_MAX_CHANNELS_PER_SPECIFIC_SCAN;
+    if (bg_scan_in->scan_interval)
+        bg_scan->scan_interval = wlan_cpu_to_le32(bg_scan_in->scan_interval);
+    else
+        bg_scan->scan_interval = wlan_cpu_to_le32(DEFAULT_BGSCAN_INTERVAL);
+    bg_scan->report_condition = wlan_cpu_to_le32(bg_scan_in->report_condition);
+
+    if ((bg_scan_in->action == BG_SCAN_ACT_GET) ||
+        (bg_scan_in->action == BG_SCAN_ACT_GET_PPS_UAPSD) || (!bg_scan->enable))
+        goto done;
+
+    tlv = (t_u8 *) bg_scan + sizeof(HostCmd_DS_802_11_BG_SCAN_CONFIG);
+    num_probes = (bg_scan_in->num_probes ? bg_scan_in->num_probes :
+                  pmadapter->scan_probes);
+    if (num_probes) {
+        pnum_probes_tlv = (MrvlIEtypes_NumProbes_t *) tlv;
+        pnum_probes_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_NUMPROBES);
+        pnum_probes_tlv->header.len =
+            wlan_cpu_to_le16(sizeof(pnum_probes_tlv->num_probes));
+        pnum_probes_tlv->num_probes = wlan_cpu_to_le16((t_u16) num_probes);
+        tlv += sizeof(MrvlIEtypes_NumProbes_t);
+        cmd_size += sizeof(MrvlIEtypes_NumProbes_t);
+    }
+    if (bg_scan_in->rssi_threshold) {
+        rssi_tlv = (MrvlIEtypes_BeaconLowRssiThreshold_t *) tlv;
+        rssi_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_RSSI_LOW);
+        rssi_tlv->header.len =
+            wlan_cpu_to_le16(sizeof(MrvlIEtypes_BeaconLowRssiThreshold_t) -
+                             sizeof(MrvlIEtypesHeader_t));
+        rssi_tlv->value = bg_scan_in->rssi_threshold;
+        rssi_tlv->frequency = 0;
+        tlv += sizeof(MrvlIEtypes_BeaconLowRssiThreshold_t);
+        cmd_size += sizeof(MrvlIEtypes_BeaconLowRssiThreshold_t);
+    }
+    if (bg_scan_in->snr_threshold) {
+        snr_tlv = (MrvlIEtypes_BeaconLowSnrThreshold_t *) tlv;
+        snr_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_SNR_LOW);
+        snr_tlv->header.len =
+            wlan_cpu_to_le16(sizeof(MrvlIEtypes_BeaconLowSnrThreshold_t) -
+                             sizeof(MrvlIEtypesHeader_t));
+        snr_tlv->value = bg_scan_in->snr_threshold;
+        snr_tlv->frequency = 0;
+        tlv += sizeof(MrvlIEtypes_BeaconLowRssiThreshold_t);
+        cmd_size += sizeof(MrvlIEtypes_BeaconLowRssiThreshold_t);
+    }
+    if (bg_scan_in->repeat_count) {
+        tlv_repeat = (MrvlIEtypes_RepeatCount_t *) tlv;
+        tlv_repeat->header.type = wlan_cpu_to_le16(TLV_TYPE_REPEAT_COUNT);
+        tlv_repeat->header.len =
+            wlan_cpu_to_le16(sizeof(MrvlIEtypes_RepeatCount_t) -
+                             sizeof(MrvlIEtypesHeader_t));
+        tlv_repeat->repeat_count = wlan_cpu_to_le16(bg_scan_in->repeat_count);
+        tlv += sizeof(MrvlIEtypes_RepeatCount_t);
+        cmd_size += sizeof(MrvlIEtypes_RepeatCount_t);
+    }
+    for (ssid_idx = 0; ((ssid_idx < NELEMENTS(bg_scan_in->ssid_list))
+                        && (*bg_scan_in->ssid_list[ssid_idx].ssid ||
+                            bg_scan_in->ssid_list[ssid_idx].max_len));
+         ssid_idx++) {
+        ssid_len = wlan_strlen((t_s8 *) bg_scan_in->ssid_list[ssid_idx].ssid);
+        pwildcard_ssid_tlv = (MrvlIEtypes_WildCardSsIdParamSet_t *) tlv;
+        pwildcard_ssid_tlv->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_WILDCARDSSID);
+        pwildcard_ssid_tlv->header.len =
+            (t_u16) (ssid_len + sizeof(pwildcard_ssid_tlv->max_ssid_length));
+        pwildcard_ssid_tlv->max_ssid_length =
+            bg_scan_in->ssid_list[ssid_idx].max_len;
+        memcpy(pmadapter, pwildcard_ssid_tlv->ssid,
+               bg_scan_in->ssid_list[ssid_idx].ssid, MIN(MLAN_MAX_SSID_LENGTH,
+                                                         ssid_len));
+        tlv +=
+            sizeof(pwildcard_ssid_tlv->header) + pwildcard_ssid_tlv->header.len;
+        cmd_size +=
+            sizeof(pwildcard_ssid_tlv->header) + pwildcard_ssid_tlv->header.len;
+        pwildcard_ssid_tlv->header.len =
+            wlan_cpu_to_le16(pwildcard_ssid_tlv->header.len);
+        PRINTM(MINFO, "Scan: ssid_list[%d]: %s, %d\n", ssid_idx,
+               pwildcard_ssid_tlv->ssid, pwildcard_ssid_tlv->max_ssid_length);
+    }
+    if (bg_scan_in->chan_list[0].chan_number) {
+        tlv_chan_list = (MrvlIEtypes_ChanListParamSet_t *) tlv;
+        PRINTM(MINFO, "Scan: Using supplied channel list\n");
+        chan_num = 0;
+        for (chan_idx = 0; chan_idx < WLAN_BG_SCAN_CHAN_MAX
+             && bg_scan_in->chan_list[chan_idx].chan_number; chan_idx++) {
+            radio_type = bg_scan_in->chan_list[chan_idx].radio_type;
+            if (!wlan_is_band_compatible
+                (pmpriv->config_bands | pmadapter->adhoc_start_band,
+                 radio_type_to_band(radio_type)))
+                continue;
+            scan_type = bg_scan_in->chan_list[chan_idx].scan_type;
+            /* Prevent active scanning on a radar controlled channel */
+            if (radio_type == HostCmd_SCAN_RADIO_TYPE_A) {
+                if (wlan_11h_radar_detect_required
+                    (pmpriv, bg_scan_in->chan_list[chan_idx].chan_number)) {
+                    scan_type = MLAN_SCAN_TYPE_PASSIVE;
+                }
+            }
+            if (radio_type == HostCmd_SCAN_RADIO_TYPE_BG) {
+                if (wlan_bg_scan_type_is_passive
+                    (pmpriv, bg_scan_in->chan_list[chan_idx].chan_number)) {
+                    scan_type = MLAN_SCAN_TYPE_PASSIVE;
+                }
+            }
+            tlv_chan_list->chan_scan_param[chan_num].chan_number =
+                bg_scan_in->chan_list[chan_idx].chan_number;
+            tlv_chan_list->chan_scan_param[chan_num].radio_type =
+                bg_scan_in->chan_list[chan_idx].radio_type;
+
+            if (scan_type == MLAN_SCAN_TYPE_PASSIVE) {
+                tlv_chan_list->chan_scan_param[chan_num].chan_scan_mode.
+                    passive_scan = MTRUE;
+            } else {
+                tlv_chan_list->chan_scan_param[chan_num].chan_scan_mode.
+                    passive_scan = MFALSE;
+            }
+            if (bg_scan_in->chan_list[chan_idx].scan_time) {
+                scan_dur = (t_u16) bg_scan_in->chan_list[chan_idx].scan_time;
+            } else {
+                if (scan_type == MLAN_SCAN_TYPE_PASSIVE) {
+                    scan_dur = pmadapter->passive_scan_time;
+                } else {
+                    scan_dur = pmadapter->specific_scan_time;
+                }
+            }
+            tlv_chan_list->chan_scan_param[chan_num].min_scan_time =
+                wlan_cpu_to_le16(scan_dur);
+            tlv_chan_list->chan_scan_param[chan_num].max_scan_time =
+                wlan_cpu_to_le16(scan_dur);
+            chan_num++;
+        }
+        tlv_chan_list->header.type = wlan_cpu_to_le16(TLV_TYPE_CHANLIST);
+        tlv_chan_list->header.len =
+            wlan_cpu_to_le16(sizeof(ChanScanParamSet_t) * chan_num);
+        tlv +=
+            sizeof(MrvlIEtypesHeader_t) + sizeof(ChanScanParamSet_t) * chan_num;
+        cmd_size +=
+            sizeof(MrvlIEtypesHeader_t) + sizeof(ChanScanParamSet_t) * chan_num;
+    } else {
+        tlv_chan_list = (MrvlIEtypes_ChanListParamSet_t *) tlv;
+        chan_num =
+            wlan_bgscan_create_channel_list(pmpriv, bg_scan_in, tlv_chan_list);
+        tlv_chan_list->header.type = wlan_cpu_to_le16(TLV_TYPE_CHANLIST);
+        tlv_chan_list->header.len =
+            wlan_cpu_to_le16(sizeof(ChanScanParamSet_t) * chan_num);
+        tlv +=
+            sizeof(MrvlIEtypesHeader_t) + sizeof(ChanScanParamSet_t) * chan_num;
+        cmd_size +=
+            sizeof(MrvlIEtypesHeader_t) + sizeof(ChanScanParamSet_t) * chan_num;
+    }
+    tlv_start_later = (MrvlIEtypes_StartLater_t *) tlv;
+    tlv_start_later->header.type = wlan_cpu_to_le16(TLV_TYPE_STARTBGSCANLATER);
+    tlv_start_later->header.len =
+        wlan_cpu_to_le16(sizeof(MrvlIEtypes_StartLater_t) -
+                         sizeof(MrvlIEtypesHeader_t));
+    tlv_start_later->value = 0;
+    tlv += sizeof(MrvlIEtypes_StartLater_t);
+    cmd_size += sizeof(MrvlIEtypes_StartLater_t);
+  done:
+    pcmd->size = wlan_cpu_to_le16(cmd_size);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of extended scan
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ret_bgscan_config(IN mlan_private * pmpriv,
+                       IN HostCmd_DS_COMMAND * resp,
+                       IN mlan_ioctl_req * pioctl_buf)
+{
+    mlan_ds_scan *pscan = MNULL;
+    HostCmd_DS_802_11_BG_SCAN_CONFIG *bg_scan = &resp->params.bg_scan_config;
+    wlan_bgscan_cfg *bg_scan_out = MNULL;
+
+    ENTER();
+    if (pioctl_buf) {
+        pscan = (mlan_ds_scan *) pioctl_buf->pbuf;
+        bg_scan_out = (wlan_bgscan_cfg *) pscan->param.user_scan.scan_cfg_buf;
+        bg_scan_out->action = wlan_le16_to_cpu(bg_scan->action);
+        if ((bg_scan_out->action == BG_SCAN_ACT_GET) &&
+            (bg_scan_out->action == BG_SCAN_ACT_GET_PPS_UAPSD)) {
+            bg_scan_out->enable = bg_scan->enable;
+            bg_scan_out->bss_type = bg_scan->bss_type;
+            bg_scan_out->chan_per_scan = bg_scan->chan_per_scan;
+            bg_scan_out->scan_interval =
+                wlan_le32_to_cpu(bg_scan->scan_interval);
+            bg_scan_out->report_condition =
+                wlan_le32_to_cpu(bg_scan->report_condition);
+            pioctl_buf->data_read_written =
+                sizeof(mlan_ds_scan) + MLAN_SUB_COMMAND_SIZE;
+        }
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of bgscan_query
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ret_802_11_bgscan_query(IN mlan_private * pmpriv,
+                             IN HostCmd_DS_COMMAND * resp,
+                             IN mlan_ioctl_req * pioctl_buf)
+{
+    mlan_ds_scan *pscan = MNULL;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    ENTER();
+    wlan_ret_802_11_scan(pmpriv, resp, MNULL);
+    if (pioctl_buf) {
+        pscan = (mlan_ds_scan *) pioctl_buf->pbuf;
+        pscan->param.scan_resp.pscan_table = (t_u8 *) pmadapter->pscan_table;
+        pscan->param.scan_resp.num_in_scan_table = pmadapter->num_in_scan_table;
+        pscan->param.scan_resp.age_in_secs = pmadapter->age_in_secs;
+        pioctl_buf->data_read_written = sizeof(mlan_scan_resp) +
+            MLAN_SUB_COMMAND_SIZE;
+
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function finds ssid in ssid list.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param ssid         SSID to find in the list
+ *  @param bssid        BSSID to qualify the SSID selection (if provided)
+ *  @param mode         Network mode: Infrastructure or IBSS
+ *
+ *  @return             index in BSSID list or < 0 if error
+ */
+t_s32
+wlan_find_ssid_in_list(IN mlan_private * pmpriv,
+                       IN mlan_802_11_ssid * ssid,
+                       IN t_u8 * bssid, IN t_u32 mode)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    t_s32 net = -1, j;
+    t_u8 best_rssi = 0;
+    t_u32 i;
+
+    ENTER();
+    PRINTM(MINFO, "Num of entries in scan table = %d\n",
+           pmadapter->num_in_scan_table);
+
+    /*
+     * Loop through the table until the maximum is reached or until a match
+     *   is found based on the bssid field comparison
+     */
+    for (i = 0;
+         i < pmadapter->num_in_scan_table && (!bssid || (bssid && net < 0));
+         i++) {
+        if (!wlan_ssid_cmp(pmadapter, &pmadapter->pscan_table[i].ssid, ssid) &&
+            (!bssid
+             || !memcmp(pmadapter, pmadapter->pscan_table[i].mac_address, bssid,
+                        MLAN_MAC_ADDR_LENGTH))) {
+
+            if (((mode == MLAN_BSS_MODE_INFRA) &&
+                 !wlan_is_band_compatible(pmpriv->config_bands,
+                                          pmadapter->pscan_table[i].bss_band))
+                ||
+                (wlan_find_cfp_by_band_and_channel
+                 (pmadapter, (t_u8) pmadapter->pscan_table[i].bss_band,
+                  (t_u16) pmadapter->pscan_table[i].channel) == MNULL)) {
+                continue;
+            }
+
+            switch (mode) {
+            case MLAN_BSS_MODE_INFRA:
+            case MLAN_BSS_MODE_IBSS:
+                j = wlan_is_network_compatible(pmpriv, i, mode);
+
+                if (j >= 0) {
+                    if (SCAN_RSSI(pmadapter->pscan_table[i].rssi) > best_rssi) {
+                        best_rssi = SCAN_RSSI(pmadapter->pscan_table[i].rssi);
+                        net = i;
+                    }
+                } else {
+                    if (net == -1) {
+                        net = j;
+                    }
+                }
+                break;
+            case MLAN_BSS_MODE_AUTO:
+            default:
+                /*
+                 * Do not check compatibility if the mode requested is
+                 *   Auto/Unknown.  Allows generic find to work without
+                 *   verifying against the Adapter security settings
+                 */
+                if (SCAN_RSSI(pmadapter->pscan_table[i].rssi) > best_rssi) {
+                    best_rssi = SCAN_RSSI(pmadapter->pscan_table[i].rssi);
+                    net = i;
+                }
+                break;
+            }
+        }
+    }
+
+    LEAVE();
+    return net;
+}
+
+/**
+ *  @brief This function finds a specific compatible BSSID in the scan list
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param bssid        BSSID to find in the scan list
+ *  @param mode         Network mode: Infrastructure or IBSS
+ *
+ *  @return             index in BSSID list or < 0 if error
+ */
+t_s32
+wlan_find_bssid_in_list(IN mlan_private * pmpriv,
+                        IN t_u8 * bssid, IN t_u32 mode)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    t_s32 net = -1;
+    t_u32 i;
+
+    ENTER();
+
+    if (!bssid) {
+        LEAVE();
+        return -1;
+    }
+
+    PRINTM(MINFO, "FindBSSID: Num of BSSIDs = %d\n",
+           pmadapter->num_in_scan_table);
+
+    /*
+     * Look through the scan table for a compatible match. The ret return
+     *   variable will be equal to the index in the scan table (greater
+     *   than zero) if the network is compatible.  The loop will continue
+     *   past a matched bssid that is not compatible in case there is an
+     *   AP with multiple SSIDs assigned to the same BSSID
+     */
+    for (i = 0; net < 0 && i < pmadapter->num_in_scan_table; i++) {
+        if (!memcmp
+            (pmadapter, pmadapter->pscan_table[i].mac_address, bssid,
+             MLAN_MAC_ADDR_LENGTH)) {
+            if (((mode == MLAN_BSS_MODE_INFRA) &&
+                 !wlan_is_band_compatible(pmpriv->config_bands,
+                                          pmadapter->pscan_table[i].bss_band))
+                ||
+                (wlan_find_cfp_by_band_and_channel
+                 (pmadapter, (t_u8) pmadapter->pscan_table[i].bss_band,
+                  (t_u16) pmadapter->pscan_table[i].channel) == MNULL)) {
+                continue;
+            }
+            switch (mode) {
+            case MLAN_BSS_MODE_INFRA:
+            case MLAN_BSS_MODE_IBSS:
+                net = wlan_is_network_compatible(pmpriv, i, mode);
+                break;
+            default:
+                net = i;
+                break;
+            }
+        }
+    }
+
+    LEAVE();
+    return net;
+}
+
+/**
+ *  @brief Compare two SSIDs
+ *
+ *  @param pmadapter A pointer to mlan_adapter structure
+ *  @param ssid1     A pointer to ssid to compare
+ *  @param ssid2     A pointer to ssid to compare
+ *
+ *  @return         0--ssid is same, otherwise is different
+ */
+t_s32
+wlan_ssid_cmp(IN pmlan_adapter pmadapter,
+              IN mlan_802_11_ssid * ssid1, IN mlan_802_11_ssid * ssid2)
+{
+    ENTER();
+
+    if (!ssid1 || !ssid2) {
+        LEAVE();
+        return -1;
+    }
+
+    if (ssid1->ssid_len != ssid2->ssid_len) {
+        LEAVE();
+        return -1;
+    }
+
+    LEAVE();
+    return memcmp(pmadapter, ssid1->ssid, ssid2->ssid, ssid1->ssid_len);
+}
+
+/**
+ *  @brief This function inserts scan command node to scan_pending_q.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pcmd_node    A pointer to cmd_ctrl_node structure
+ *  @return             N/A
+ */
+t_void
+wlan_queue_scan_cmd(IN mlan_private * pmpriv, IN cmd_ctrl_node * pcmd_node)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+
+    ENTER();
+
+    if (pcmd_node == MNULL)
+        goto done;
+    util_enqueue_list_tail(pmadapter->pmoal_handle, &pmadapter->scan_pending_q,
+                           (pmlan_linked_list) pcmd_node,
+                           pmadapter->callbacks.moal_spin_lock,
+                           pmadapter->callbacks.moal_spin_unlock);
+
+  done:
+    LEAVE();
+}
+
+/**
+ *  @brief Find the AP with specific ssid in the scan list
+ *
+ *  @param pmpriv               A pointer to mlan_private structure
+ *  @param preq_ssid_bssid      A pointer to AP's ssid returned
+ *
+ *  @return                     MLAN_STATUS_SUCCESS--success, otherwise--fail
+ */
+mlan_status
+wlan_find_best_network(IN mlan_private * pmpriv,
+                       OUT mlan_ssid_bssid * preq_ssid_bssid)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    BSSDescriptor_t *preq_bss;
+    t_s32 i;
+
+    ENTER();
+
+    memset(pmadapter, preq_ssid_bssid, 0, sizeof(mlan_ssid_bssid));
+
+    i = wlan_find_best_network_in_list(pmpriv);
+
+    if (i >= 0) {
+        preq_bss = &pmadapter->pscan_table[i];
+        memcpy(pmadapter, &preq_ssid_bssid->ssid, &preq_bss->ssid,
+               sizeof(mlan_802_11_ssid));
+        memcpy(pmadapter, (t_u8 *) & preq_ssid_bssid->bssid,
+               (t_u8 *) & preq_bss->mac_address, MLAN_MAC_ADDR_LENGTH);
+
+        /* Make sure we are in the right mode */
+        if (pmpriv->bss_mode == MLAN_BSS_MODE_AUTO)
+            pmpriv->bss_mode = preq_bss->bss_mode;
+    }
+
+    if (!preq_ssid_bssid->ssid.ssid_len) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    PRINTM(MINFO, "Best network found = [%s], "
+           "[%02x:%02x:%02x:%02x:%02x:%02x]\n",
+           preq_ssid_bssid->ssid.ssid,
+           preq_ssid_bssid->bssid[0], preq_ssid_bssid->bssid[1],
+           preq_ssid_bssid->bssid[2], preq_ssid_bssid->bssid[3],
+           preq_ssid_bssid->bssid[4], preq_ssid_bssid->bssid[5]);
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Send a scan command for all available channels filtered on a spec
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pioctl_buf   A pointer to MLAN IOCTL Request buffer
+ *  @param preq_ssid    A pointer to AP's ssid returned
+ *
+ *  @return             MLAN_STATUS_SUCCESS--success, otherwise--fail
+ */
+mlan_status
+wlan_scan_specific_ssid(IN mlan_private * pmpriv,
+                        IN t_void * pioctl_buf, IN mlan_802_11_ssid * preq_ssid)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_callbacks *pcb = (mlan_callbacks *) & pmpriv->adapter->callbacks;
+    wlan_user_scan_cfg *pscan_cfg;
+    pmlan_ioctl_req pioctl_req = (mlan_ioctl_req *) pioctl_buf;
+
+    ENTER();
+
+    if (!preq_ssid) {
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_CMD_SCAN_FAIL;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    wlan_scan_delete_ssid_table_entry(pmpriv, preq_ssid);
+
+    ret =
+        pcb->moal_malloc(pmpriv->adapter->pmoal_handle,
+                         sizeof(wlan_user_scan_cfg), MLAN_MEM_DEF,
+                         (t_u8 **) & pscan_cfg);
+
+    if (ret != MLAN_STATUS_SUCCESS || !pscan_cfg) {
+        PRINTM(MERROR, "Memory allocation for pscan_cfg failed!\n");
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_NO_MEM;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    memset(pmpriv->adapter, pscan_cfg, 0x00, sizeof(wlan_user_scan_cfg));
+
+    memcpy(pmpriv->adapter, pscan_cfg->ssid_list[0].ssid,
+           preq_ssid->ssid, preq_ssid->ssid_len);
+    pscan_cfg->keep_previous_scan = MTRUE;
+
+    ret = wlan_scan_networks(pmpriv, pioctl_buf, pscan_cfg);
+
+    if (pscan_cfg)
+        pcb->moal_mfree(pmpriv->adapter->pmoal_handle, (t_u8 *) pscan_cfg);
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Save a beacon buffer of the current bss descriptor
+ *  Save the current beacon buffer to restore in the following cases that
+ *  makes the bcn_buf not to contain the current ssid's beacon buffer.
+ *    - the current ssid was not found somehow in the last scan.
+ *    - the current ssid was the last entry of the scan table and overloaded.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *
+ *  @return             N/A
+ */
+t_void
+wlan_save_curr_bcn(IN mlan_private * pmpriv)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    mlan_callbacks *pcb = (pmlan_callbacks) & pmadapter->callbacks;
+    BSSDescriptor_t *pcurr_bss = &pmpriv->curr_bss_params.bss_descriptor;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    /* save the beacon buffer if it is not saved or updated */
+    if ((pmpriv->pcurr_bcn_buf == MNULL) ||
+        (pmpriv->curr_bcn_size != pcurr_bss->beacon_buf_size) ||
+        (memcmp(pmpriv->adapter, pmpriv->pcurr_bcn_buf, pcurr_bss->pbeacon_buf,
+                pcurr_bss->beacon_buf_size))) {
+
+        if (pmpriv->pcurr_bcn_buf) {
+            pcb->moal_mfree(pmadapter->pmoal_handle, pmpriv->pcurr_bcn_buf);
+            pmpriv->pcurr_bcn_buf = MNULL;
+        }
+        pmpriv->curr_bcn_size = pcurr_bss->beacon_buf_size;
+
+        if (pmpriv->curr_bcn_size) {
+            ret =
+                pcb->moal_malloc(pmadapter->pmoal_handle,
+                                 pcurr_bss->beacon_buf_size, MLAN_MEM_DEF,
+                                 &pmpriv->pcurr_bcn_buf);
+
+            if ((ret == MLAN_STATUS_SUCCESS) && pmpriv->pcurr_bcn_buf) {
+                memcpy(pmpriv->adapter, pmpriv->pcurr_bcn_buf,
+                       pcurr_bss->pbeacon_buf, pcurr_bss->beacon_buf_size);
+                PRINTM(MINFO, "current beacon saved %d\n",
+                       pmpriv->curr_bcn_size);
+            }
+        }
+    }
+    LEAVE();
+}
+
+/**
+ *  @brief Free a beacon buffer of the current bss descriptor
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *
+ *  @return             N/A
+ */
+t_void
+wlan_free_curr_bcn(IN mlan_private * pmpriv)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    mlan_callbacks *pcb = (pmlan_callbacks) & pmadapter->callbacks;
+
+    ENTER();
+    if (pmpriv->pcurr_bcn_buf) {
+        pcb->moal_mfree(pmadapter->pmoal_handle, pmpriv->pcurr_bcn_buf);
+        pmpriv->pcurr_bcn_buf = MNULL;
+    }
+    LEAVE();
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_sdio.c b/drivers/net/wireless/sd8797/mlan/mlan_sdio.c
new file mode 100644
index 000000000000..02e73477b619
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_sdio.c
@@ -0,0 +1,1661 @@
+/** @file mlan_sdio.c
+ *
+ *  @brief This file contains SDIO specific code
+ * 
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/********************************************************
+Change log:
+    10/27/2008: initial version
+********************************************************/
+
+#include "mlan.h"
+#ifdef STA_SUPPORT
+#include "mlan_join.h"
+#endif
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_init.h"
+#include "mlan_wmm.h"
+#include "mlan_11n.h"
+#include "mlan_sdio.h"
+
+/********************************************************
+		Local Variables
+********************************************************/
+
+/********************************************************
+		Global Variables
+********************************************************/
+
+/********************************************************
+		Local Functions
+********************************************************/
+
+/**
+ *  @brief This function initialize the SDIO port
+ *
+ *  @param pmadapter  A pointer to mlan_adapter structure
+ *  @return 	   	  MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_sdio_init_ioport(mlan_adapter * pmadapter)
+{
+    t_u32 reg;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+
+    ENTER();
+    pmadapter->ioport = 0;
+
+    /* Read the IO port */
+    if (MLAN_STATUS_SUCCESS ==
+        pcb->moal_read_reg(pmadapter->pmoal_handle, IO_PORT_0_REG, &reg))
+        pmadapter->ioport |= (reg & 0xff);
+    else {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    if (MLAN_STATUS_SUCCESS ==
+        pcb->moal_read_reg(pmadapter->pmoal_handle, IO_PORT_1_REG, &reg))
+        pmadapter->ioport |= ((reg & 0xff) << 8);
+    else {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    if (MLAN_STATUS_SUCCESS ==
+        pcb->moal_read_reg(pmadapter->pmoal_handle, IO_PORT_2_REG, &reg))
+        pmadapter->ioport |= ((reg & 0xff) << 16);
+    else {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    PRINTM(MINFO, "SDIO FUNC1 IO port: 0x%x\n", pmadapter->ioport);
+
+    /* Set Host interrupt reset to read to clear */
+    if (MLAN_STATUS_SUCCESS ==
+        pcb->moal_read_reg(pmadapter->pmoal_handle, HOST_INT_RSR_REG, &reg)) {
+        pcb->moal_write_reg(pmadapter->pmoal_handle, HOST_INT_RSR_REG,
+                            reg | HOST_INT_RSR_MASK);
+    } else {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    /* Dnld/Upld ready set to auto reset */
+    if (MLAN_STATUS_SUCCESS ==
+        pcb->moal_read_reg(pmadapter->pmoal_handle, CARD_MISC_CFG_REG, &reg)) {
+        pcb->moal_write_reg(pmadapter->pmoal_handle, CARD_MISC_CFG_REG,
+                            reg | AUTO_RE_ENABLE_INT);
+    } else {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function sends data to the card.
+ *
+ *  @param pmadapter A pointer to mlan_adapter structure
+ *  @param pmbuf     A pointer to mlan_buffer (pmbuf->data_len should include SDIO header)
+ *  @param port      Port
+ *  @return 	     MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_write_data_sync(mlan_adapter * pmadapter, mlan_buffer * pmbuf, t_u32 port)
+{
+    t_u32 i = 0;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    do {
+        ret =
+            pcb->moal_write_data_sync(pmadapter->pmoal_handle, pmbuf, port, 0);
+        if (ret != MLAN_STATUS_SUCCESS) {
+            i++;
+            PRINTM(MERROR, "host_to_card, write iomem (%d) failed: %d\n", i,
+                   ret);
+            if (MLAN_STATUS_SUCCESS !=
+                pcb->moal_write_reg(pmadapter->pmoal_handle,
+                                    HOST_TO_CARD_EVENT_REG, 0x04)) {
+                PRINTM(MERROR, "write CFG reg failed\n");
+            }
+            ret = MLAN_STATUS_FAILURE;
+            if (i > MAX_WRITE_IOMEM_RETRY) {
+                pmbuf->status_code = MLAN_ERROR_DATA_TX_FAIL;
+                goto exit;
+            }
+        }
+    } while (ret == MLAN_STATUS_FAILURE);
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function gets available SDIO port for reading cmd/data
+ *
+ *  @param pmadapter  A pointer to mlan_adapter structure
+ *  @param pport      A pointer to port number
+ *  @return 	   	  MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_get_rd_port(mlan_adapter * pmadapter, t_u8 * pport)
+{
+    t_u32 rd_bitmap = pmadapter->mp_rd_bitmap;
+
+    ENTER();
+
+    PRINTM(MIF_D, "wlan_get_rd_port: mp_rd_bitmap=0x%08x\n", rd_bitmap);
+
+    if (!(rd_bitmap & (CTRL_PORT_MASK | DATA_PORT_MASK))) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    if (pmadapter->mp_rd_bitmap & CTRL_PORT_MASK) {
+        pmadapter->mp_rd_bitmap &= (t_u32) (~CTRL_PORT_MASK);
+        *pport = CTRL_PORT;
+        PRINTM(MIF_D, "wlan_get_rd_port: port=%d mp_rd_bitmap=0x%08x\n", *pport,
+               pmadapter->mp_rd_bitmap);
+    } else {
+        if (pmadapter->mp_rd_bitmap & (1 << pmadapter->curr_rd_port)) {
+            pmadapter->mp_rd_bitmap &=
+                (t_u32) (~(1 << pmadapter->curr_rd_port));
+            *pport = pmadapter->curr_rd_port;
+
+            /* hw rx wraps round only after port (MAX_PORT-1) */
+            if (++pmadapter->curr_rd_port == MAX_PORT)
+                /* port 0 is reserved for cmd port */
+                pmadapter->curr_rd_port = 1;
+        } else {
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+
+        PRINTM(MIF_D, "port=%d mp_rd_bitmap=0x%08x -> 0x%08x\n",
+               *pport, rd_bitmap, pmadapter->mp_rd_bitmap);
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function gets available SDIO port for writing data
+ *
+ *  @param pmadapter  A pointer to mlan_adapter structure
+ *  @param pport      A pointer to port number
+ *  @return 	   	  MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_get_wr_port_data(mlan_adapter * pmadapter, t_u8 * pport)
+{
+    t_u32 wr_bitmap = pmadapter->mp_wr_bitmap;
+
+    ENTER();
+
+    PRINTM(MIF_D, "wlan_get_wr_port_data: mp_wr_bitmap=0x%08x\n", wr_bitmap);
+
+    if (!(wr_bitmap & pmadapter->mp_data_port_mask)) {
+        pmadapter->data_sent = MTRUE;
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+
+    if (pmadapter->mp_wr_bitmap & (1 << pmadapter->curr_wr_port)) {
+        pmadapter->mp_wr_bitmap &= (t_u32) (~(1 << pmadapter->curr_wr_port));
+        *pport = pmadapter->curr_wr_port;
+        if (++pmadapter->curr_wr_port == pmadapter->mp_end_port)
+            pmadapter->curr_wr_port = 1;
+    } else {
+        pmadapter->data_sent = MTRUE;
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+
+    if (*pport == CTRL_PORT) {
+        PRINTM(MERROR,
+               "Invalid data port=%d cur port=%d mp_wr_bitmap=0x%08x -> 0x%08x\n",
+               *pport, pmadapter->curr_wr_port, wr_bitmap,
+               pmadapter->mp_wr_bitmap);
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    PRINTM(MIF_D, "port=%d mp_wr_bitmap=0x%08x -> 0x%08x\n",
+           *pport, wr_bitmap, pmadapter->mp_wr_bitmap);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function polls the card status register.
+ *
+ *  @param pmadapter  A pointer to mlan_adapter structure
+ *  @param bits    	  the bit mask
+ *  @return 	   	  MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_sdio_poll_card_status(mlan_adapter * pmadapter, t_u8 bits)
+{
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    t_u32 tries;
+    t_u32 cs = 0;
+
+    ENTER();
+
+    for (tries = 0; tries < MAX_POLL_TRIES; tries++) {
+        if (pcb->
+            moal_read_reg(pmadapter->pmoal_handle, CARD_TO_HOST_EVENT_REG,
+                          &cs) != MLAN_STATUS_SUCCESS)
+            break;
+        else if ((cs & bits) == bits) {
+            LEAVE();
+            return MLAN_STATUS_SUCCESS;
+        }
+        wlan_udelay(pmadapter, 10);
+    }
+
+    PRINTM(MERROR, "wlan_sdio_poll_card_status failed, tries = %d, cs = 0x%x\n",
+           tries, cs);
+    LEAVE();
+    return MLAN_STATUS_FAILURE;
+}
+
+/**
+ *  @brief This function reads firmware status registers
+ *
+ *  @param pmadapter A pointer to mlan_adapter structure
+ *  @param dat	   A pointer to keep returned data
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_sdio_read_fw_status(mlan_adapter * pmadapter, t_u16 * dat)
+{
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    t_u32 fws0 = 0, fws1 = 0;
+
+    ENTER();
+    if (MLAN_STATUS_SUCCESS !=
+        pcb->moal_read_reg(pmadapter->pmoal_handle, CARD_FW_STATUS0_REG,
+                           &fws0)) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        pcb->moal_read_reg(pmadapter->pmoal_handle, CARD_FW_STATUS1_REG,
+                           &fws1)) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    *dat = (t_u16) ((fws1 << 8) | fws0);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**  @brief This function disables the host interrupts mask.
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param mask	   the interrupt mask
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_sdio_disable_host_int_mask(pmlan_adapter pmadapter, t_u8 mask)
+{
+    t_u32 host_int_mask = 0;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+
+    ENTER();
+
+    /* Read back the host_int_mask register */
+    if (MLAN_STATUS_SUCCESS !=
+        pcb->moal_read_reg(pmadapter->pmoal_handle, HOST_INT_MASK_REG,
+                           &host_int_mask)) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    /* Update with the mask and write back to the register */
+    host_int_mask &= ~mask;
+
+    if (MLAN_STATUS_SUCCESS !=
+        pcb->moal_write_reg(pmadapter->pmoal_handle, HOST_INT_MASK_REG,
+                            host_int_mask)) {
+        PRINTM(MWARN, "Disable host interrupt failed\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function enables the host interrupts mask
+ *
+ *  @param pmadapter A pointer to mlan_adapter structure
+ *  @param mask	   the interrupt mask
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_sdio_enable_host_int_mask(pmlan_adapter pmadapter, t_u8 mask)
+{
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+
+    ENTER();
+
+    /* Simply write the mask to the register */
+    if (MLAN_STATUS_SUCCESS !=
+        pcb->moal_write_reg(pmadapter->pmoal_handle, HOST_INT_MASK_REG, mask)) {
+        PRINTM(MWARN, "Enable host interrupt failed\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function reads data from the card.
+ *
+ *  @param pmadapter 	A pointer to mlan_adapter structure
+ *  @param type	   	A pointer to keep type as data or command
+ *  @param nb		A pointer to keep the data/cmd length returned in buffer
+ *  @param pmbuf 	A pointer to the SDIO data/cmd buffer
+ *  @param npayload	the length of data/cmd buffer
+ *  @param ioport	the SDIO ioport
+ *  @return 	   	MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_sdio_card_to_host(mlan_adapter * pmadapter,
+                       t_u32 * type, t_u32 * nb, pmlan_buffer pmbuf,
+                       t_u32 npayload, t_u32 ioport)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+
+    ENTER();
+
+    if (!pmbuf) {
+        PRINTM(MWARN, "pmbuf is NULL!\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto exit;
+    }
+
+    ret = pcb->moal_read_data_sync(pmadapter->pmoal_handle, pmbuf, ioport, 0);
+
+    if (ret != MLAN_STATUS_SUCCESS) {
+        PRINTM(MERROR, "card_to_host, read iomem failed: %d\n", ret);
+        pmbuf->status_code = MLAN_ERROR_DATA_RX_FAIL;
+        ret = MLAN_STATUS_FAILURE;
+        goto exit;
+    }
+    *nb = wlan_le16_to_cpu(*(t_u16 *) (pmbuf->pbuf + pmbuf->data_offset));
+    if (*nb > npayload) {
+        PRINTM(MERROR, "invalid packet, *nb=%d, npayload=%d\n", *nb, npayload);
+        pmbuf->status_code = MLAN_ERROR_PKT_SIZE_INVALID;
+        ret = MLAN_STATUS_FAILURE;
+        goto exit;
+    }
+
+    DBG_HEXDUMP(MIF_D, "SDIO Blk Rd", pmbuf->pbuf + pmbuf->data_offset,
+                MIN(*nb, MAX_DATA_DUMP_LEN));
+
+    *type = wlan_le16_to_cpu(*(t_u16 *) (pmbuf->pbuf + pmbuf->data_offset + 2));
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief  This function downloads FW blocks to device
+ *
+ *  @param pmadapter	A pointer to mlan_adapter
+ *  @param pmfw			A pointer to firmware image
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_prog_fw_w_helper(IN pmlan_adapter pmadapter, IN pmlan_fw_image pmfw)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    t_u8 *firmware = pmfw->pfw_buf;
+    t_u32 firmwarelen = pmfw->fw_len;
+    t_u32 offset = 0;
+    t_u32 base0, base1;
+    t_void *tmpfwbuf = MNULL;
+    t_u32 tmpfwbufsz;
+    t_u8 *fwbuf;
+    mlan_buffer mbuf;
+    t_u16 len = 0;
+    t_u32 txlen = 0, tx_blocks = 0, tries = 0;
+    t_u32 i = 0;
+
+    ENTER();
+
+    if (!firmware && !pcb->moal_get_fw_data) {
+        PRINTM(MMSG, "No firmware image found! Terminating download\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    PRINTM(MINFO, "WLAN: Downloading FW image (%d bytes)\n", firmwarelen);
+
+    tmpfwbufsz = ALIGN_SZ(WLAN_UPLD_SIZE, DMA_ALIGNMENT);
+    ret =
+        pcb->moal_malloc(pmadapter->pmoal_handle, tmpfwbufsz,
+                         MLAN_MEM_DEF | MLAN_MEM_DMA, (t_u8 **) & tmpfwbuf);
+    if ((ret != MLAN_STATUS_SUCCESS) || !tmpfwbuf) {
+        PRINTM(MERROR,
+               "Unable to allocate buffer for firmware. Terminating download\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    memset(pmadapter, tmpfwbuf, 0, tmpfwbufsz);
+    /* Ensure 8-byte aligned firmware buffer */
+    fwbuf = (t_u8 *) ALIGN_ADDR(tmpfwbuf, DMA_ALIGNMENT);
+
+    /* Perform firmware data transfer */
+    do {
+        /* The host polls for the DN_LD_CARD_RDY and CARD_IO_READY bits */
+        ret =
+            wlan_sdio_poll_card_status(pmadapter,
+                                       CARD_IO_READY | DN_LD_CARD_RDY);
+        if (ret != MLAN_STATUS_SUCCESS) {
+            PRINTM(MFATAL,
+                   "WLAN: FW download with helper poll status timeout @ %d\n",
+                   offset);
+            goto done;
+        }
+
+        /* More data? */
+        if (firmwarelen && offset >= firmwarelen)
+            break;
+
+        for (tries = 0; tries < MAX_POLL_TRIES; tries++) {
+            if ((ret = pcb->moal_read_reg(pmadapter->pmoal_handle,
+                                          READ_BASE_0_REG,
+                                          &base0)) != MLAN_STATUS_SUCCESS) {
+                PRINTM(MERROR,
+                       "Dev BASE0 register read failed:"
+                       " base0=0x%04X(%d). Terminating download\n", base0,
+                       base0);
+                goto done;
+            }
+            if ((ret = pcb->moal_read_reg(pmadapter->pmoal_handle,
+                                          READ_BASE_1_REG,
+                                          &base1)) != MLAN_STATUS_SUCCESS) {
+                PRINTM(MERROR,
+                       "Dev BASE1 register read failed:"
+                       " base1=0x%04X(%d). Terminating download\n", base1,
+                       base1);
+                goto done;
+            }
+            len = (t_u16) (((base1 & 0xff) << 8) | (base0 & 0xff));
+
+            if (len)
+                break;
+            wlan_udelay(pmadapter, 10);
+        }
+
+        if (!len)
+            break;
+        else if (len > WLAN_UPLD_SIZE) {
+            PRINTM(MFATAL,
+                   "WLAN: FW download failure @ %d, invalid length %d\n",
+                   offset, len);
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+
+        txlen = len;
+
+        if (len & MBIT(0)) {
+            i++;
+            if (i > MAX_WRITE_IOMEM_RETRY) {
+                PRINTM(MFATAL,
+                       "WLAN: FW download failure @ %d, over max retry count\n",
+                       offset);
+                ret = MLAN_STATUS_FAILURE;
+                goto done;
+            }
+            PRINTM(MERROR, "WLAN: FW CRC error indicated by the helper:"
+                   " len = 0x%04X, txlen = %d\n", len, txlen);
+            len &= ~MBIT(0);
+
+            PRINTM(MERROR, "WLAN: retry: %d, offset %d\n", i, offset);
+            DBG_HEXDUMP(MERROR, "WLAN: FW block:", mbuf.pbuf, len);
+
+            /* Setting this to 0 to resend from same offset */
+            txlen = 0;
+        } else {
+            i = 0;
+
+            /* Set blocksize to transfer - checking for last block */
+            if (firmwarelen && firmwarelen - offset < txlen) {
+                txlen = firmwarelen - offset;
+            }
+            PRINTM(MINFO, ".");
+
+            tx_blocks =
+                (txlen + MLAN_SDIO_BLOCK_SIZE_FW_DNLD -
+                 1) / MLAN_SDIO_BLOCK_SIZE_FW_DNLD;
+
+            /* Copy payload to buffer */
+            if (firmware)
+                memmove(pmadapter, fwbuf, &firmware[offset], txlen);
+            else
+                pcb->moal_get_fw_data(pmadapter->pmoal_handle, offset, txlen,
+                                      fwbuf);
+        }
+
+        /* Send data */
+        memset(pmadapter, &mbuf, 0, sizeof(mlan_buffer));
+        mbuf.pbuf = (t_u8 *) fwbuf;
+        mbuf.data_len = tx_blocks * MLAN_SDIO_BLOCK_SIZE_FW_DNLD;
+
+        ret =
+            pcb->moal_write_data_sync(pmadapter->pmoal_handle, &mbuf,
+                                      pmadapter->ioport, 0);
+        if (ret != MLAN_STATUS_SUCCESS) {
+            PRINTM(MERROR, "WLAN: FW download, write iomem (%d) failed @ %d\n",
+                   i, offset);
+            if (pcb->
+                moal_write_reg(pmadapter->pmoal_handle, HOST_TO_CARD_EVENT_REG,
+                               0x04) != MLAN_STATUS_SUCCESS) {
+                PRINTM(MERROR, "write CFG reg failed\n");
+            }
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+
+        offset += txlen;
+    } while (MTRUE);
+
+    PRINTM(MINFO, "\nFW download over, size %d bytes\n", offset);
+
+    ret = MLAN_STATUS_SUCCESS;
+  done:
+    if (tmpfwbuf)
+        pcb->moal_mfree(pmadapter->pmoal_handle, (t_u8 *) tmpfwbuf);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function disables the host interrupts.
+ *
+ *  @param pmadapter A pointer to mlan_adapter structure
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_disable_host_int(pmlan_adapter pmadapter)
+{
+    mlan_status ret;
+
+    ENTER();
+    ret = wlan_sdio_disable_host_int_mask(pmadapter, HIM_DISABLE);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function decodes the rx packet &
+ *  calls corresponding handlers according to the packet type
+ *
+ *  @param pmadapter A pointer to mlan_adapter structure
+ *  @param pmbuf      A pointer to the SDIO data/cmd buffer
+ *  @param upld_typ  Type of rx packet
+ *  @return 	   MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_decode_rx_packet(mlan_adapter * pmadapter, mlan_buffer * pmbuf,
+                      t_u32 upld_typ)
+{
+    t_u8 *cmdBuf;
+    t_u32 event;
+
+    ENTER();
+
+    switch (upld_typ) {
+    case MLAN_TYPE_DATA:
+        PRINTM(MINFO, "--- Rx: Data packet ---\n");
+        pmbuf->data_len = (pmadapter->upld_len - INTF_HEADER_LEN);
+        pmbuf->data_offset += INTF_HEADER_LEN;
+        wlan_handle_rx_packet(pmadapter, pmbuf);
+        pmadapter->data_received = MTRUE;
+        break;
+
+    case MLAN_TYPE_CMD:
+        PRINTM(MINFO, "--- Rx: Cmd Response ---\n");
+        /* take care of curr_cmd = NULL case */
+        if (!pmadapter->curr_cmd) {
+            cmdBuf = pmadapter->upld_buf;
+            if (pmadapter->ps_state == PS_STATE_SLEEP_CFM) {
+                wlan_process_sleep_confirm_resp(pmadapter,
+                                                pmbuf->pbuf +
+                                                pmbuf->data_offset +
+                                                INTF_HEADER_LEN,
+                                                pmadapter->upld_len -
+                                                INTF_HEADER_LEN);
+            }
+            pmadapter->upld_len -= INTF_HEADER_LEN;
+            memcpy(pmadapter, cmdBuf,
+                   pmbuf->pbuf + pmbuf->data_offset + INTF_HEADER_LEN,
+                   MIN(MRVDRV_SIZE_OF_CMD_BUFFER,
+                       pmadapter->upld_len - INTF_HEADER_LEN));
+            wlan_free_mlan_buffer(pmadapter, pmbuf);
+        } else {
+            pmadapter->cmd_resp_received = MTRUE;
+            pmadapter->upld_len -= INTF_HEADER_LEN;
+            pmbuf->data_len = pmadapter->upld_len;
+            pmbuf->data_offset += INTF_HEADER_LEN;
+            pmadapter->curr_cmd->respbuf = pmbuf;
+        }
+        break;
+
+    case MLAN_TYPE_EVENT:
+        PRINTM(MINFO, "--- Rx: Event ---\n");
+        event = *(t_u32 *) & pmbuf->pbuf[pmbuf->data_offset + INTF_HEADER_LEN];
+        pmadapter->event_cause = wlan_le32_to_cpu(event);
+        if ((pmadapter->upld_len > MLAN_EVENT_HEADER_LEN) &&
+            ((pmadapter->upld_len - MLAN_EVENT_HEADER_LEN) < MAX_EVENT_SIZE)) {
+            memcpy(pmadapter, pmadapter->event_body,
+                   pmbuf->pbuf + pmbuf->data_offset + MLAN_EVENT_HEADER_LEN,
+                   pmadapter->upld_len - MLAN_EVENT_HEADER_LEN);
+        }
+
+        /* event cause has been saved to adapter->event_cause */
+        pmadapter->event_received = MTRUE;
+        pmbuf->data_len = pmadapter->upld_len;
+        pmadapter->pmlan_buffer_event = pmbuf;
+
+        /* remove SDIO header */
+        pmbuf->data_offset += INTF_HEADER_LEN;
+        pmbuf->data_len -= INTF_HEADER_LEN;
+        break;
+
+    default:
+        PRINTM(MERROR, "SDIO unknown upload type = 0x%x\n", upld_typ);
+        wlan_free_mlan_buffer(pmadapter, pmbuf);
+        break;
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+#ifdef SDIO_MULTI_PORT_RX_AGGR
+/**
+ *  @brief This function receives data from the card in aggregate mode.
+ *
+ *  @param pmadapter A pointer to mlan_adapter structure
+ *  @param pmbuf      A pointer to the SDIO data/cmd buffer
+ *  @param port      Current port on which packet needs to be rxed
+ *  @param rx_len    Length of received packet
+ *  @return 	     MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_sdio_card_to_host_mp_aggr(mlan_adapter * pmadapter, mlan_buffer
+                               * pmbuf, t_u8 port, t_u16 rx_len)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    t_s32 f_do_rx_aggr = 0;
+    t_s32 f_do_rx_cur = 0;
+    t_s32 f_aggr_cur = 0;
+    mlan_buffer mbuf_aggr;
+    mlan_buffer *mbuf_deaggr;
+    t_u32 pind = 0;
+    t_u32 pkt_len, pkt_type = 0;
+    t_u8 *curr_ptr;
+    t_u32 cmd53_port = 0;
+
+    ENTER();
+
+    if (port == CTRL_PORT) {
+        /* Read the command response or event without aggr */
+        PRINTM(MINFO, "card_2_host_mp_aggr: No aggr for control port\n");
+
+        f_do_rx_cur = 1;
+        goto rx_curr_single;
+    }
+
+    if (!pmadapter->mpa_rx.enabled) {
+        PRINTM(MINFO, "card_2_host_mp_aggr: rx aggregation disabled !\n");
+
+        f_do_rx_cur = 1;
+        goto rx_curr_single;
+    }
+
+    if (pmadapter->mp_rd_bitmap & (~((t_u32) CTRL_PORT_MASK))) {
+        /* Some more data RX pending */
+        PRINTM(MINFO, "card_2_host_mp_aggr: Not last packet\n");
+
+        if (MP_RX_AGGR_IN_PROGRESS(pmadapter)) {
+            if (MP_RX_AGGR_BUF_HAS_ROOM(pmadapter, rx_len)) {
+                f_aggr_cur = 1;
+            } else {
+                /* No room in Aggr buf, do rx aggr now */
+                f_do_rx_aggr = 1;
+                f_do_rx_cur = 1;
+            }
+        } else {
+            /* Rx aggr not in progress */
+            f_aggr_cur = 1;
+        }
+
+    } else {
+        /* No more data RX pending */
+        PRINTM(MINFO, "card_2_host_mp_aggr: Last packet\n");
+
+        if (MP_RX_AGGR_IN_PROGRESS(pmadapter)) {
+            f_do_rx_aggr = 1;
+            if (MP_RX_AGGR_BUF_HAS_ROOM(pmadapter, rx_len)) {
+                f_aggr_cur = 1;
+            } else {
+                /* No room in Aggr buf, do rx aggr now */
+                f_do_rx_cur = 1;
+            }
+        } else {
+            f_do_rx_cur = 1;
+        }
+
+    }
+
+    if (f_aggr_cur) {
+        PRINTM(MINFO, "Current packet aggregation.\n");
+        /* Curr pkt can be aggregated */
+        MP_RX_AGGR_SETUP(pmadapter, pmbuf, port, rx_len);
+
+        if (MP_RX_AGGR_PKT_LIMIT_REACHED(pmadapter) ||
+            MP_RX_AGGR_PORT_LIMIT_REACHED(pmadapter)) {
+            PRINTM(MINFO,
+                   "card_2_host_mp_aggr: Aggregation Packet limit reached\n");
+            /* No more pkts allowed in Aggr buf, rx it */
+            f_do_rx_aggr = 1;
+        }
+    }
+
+    if (f_do_rx_aggr) {
+        /* do aggr RX now */
+        PRINTM(MINFO, "do_rx_aggr: num of packets: %d\n",
+               pmadapter->mpa_rx.pkt_cnt);
+
+        memset(pmadapter, &mbuf_aggr, 0, sizeof(mlan_buffer));
+
+        mbuf_aggr.pbuf = (t_u8 *) pmadapter->mpa_rx.buf;
+        mbuf_aggr.data_len = pmadapter->mpa_rx.buf_len;
+        cmd53_port = (pmadapter->ioport | SDIO_MPA_ADDR_BASE |
+                      (pmadapter->mpa_rx.ports << 4)) +
+            pmadapter->mpa_rx.start_port;
+        if (MLAN_STATUS_SUCCESS !=
+            pcb->moal_read_data_sync(pmadapter->pmoal_handle, &mbuf_aggr,
+                                     cmd53_port, 0)) {
+            pmbuf->status_code = MLAN_ERROR_DATA_RX_FAIL;
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+
+        DBG_HEXDUMP(MIF_D, "SDIO MP-A Blk Rd", pmadapter->mpa_rx.buf,
+                    MIN(pmadapter->mpa_rx.buf_len, MAX_DATA_DUMP_LEN));
+
+        curr_ptr = pmadapter->mpa_rx.buf;
+
+        for (pind = 0; pind < pmadapter->mpa_rx.pkt_cnt; pind++) {
+
+            /* get curr PKT len & type */
+            pkt_len = wlan_le16_to_cpu(*(t_u16 *) & curr_ptr[0]);
+            pkt_type = wlan_le16_to_cpu(*(t_u16 *) & curr_ptr[2]);
+
+            PRINTM(MINFO, "RX: [%d] pktlen: %d pkt_type: 0x%x\n", pind,
+                   pkt_len, pkt_type);
+
+            /* copy pkt to deaggr buf */
+            mbuf_deaggr = pmadapter->mpa_rx.mbuf_arr[pind];
+            if ((pkt_type == MLAN_TYPE_DATA) &&
+                (pkt_len <= pmadapter->mpa_rx.len_arr[pind])) {
+                memcpy(pmadapter, mbuf_deaggr->pbuf + mbuf_deaggr->data_offset,
+                       curr_ptr, pkt_len);
+                pmadapter->upld_len = pkt_len;
+                /* Process de-aggr packet */
+                wlan_decode_rx_packet(pmadapter, mbuf_deaggr, pkt_type);
+            } else {
+                PRINTM(MERROR,
+                       "Wrong aggr packet: type=%d, len=%d, max_len=%d\n",
+                       pkt_type, pkt_len, pmadapter->mpa_rx.len_arr[pind]);
+                wlan_free_mlan_buffer(pmadapter, mbuf_deaggr);
+            }
+            curr_ptr += pmadapter->mpa_rx.len_arr[pind];
+        }
+        MP_RX_AGGR_BUF_RESET(pmadapter);
+    }
+
+  rx_curr_single:
+    if (f_do_rx_cur) {
+        PRINTM(MINFO, "RX: f_do_rx_cur: port: %d rx_len: %d\n", port, rx_len);
+
+        if (MLAN_STATUS_SUCCESS != wlan_sdio_card_to_host(pmadapter, &pkt_type,
+                                                          (t_u32 *) &
+                                                          pmadapter->upld_len,
+                                                          pmbuf, rx_len,
+                                                          pmadapter->ioport +
+                                                          port)) {
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+        if ((port == CTRL_PORT) && ((pkt_type != MLAN_TYPE_EVENT) &&
+                                    (pkt_type != MLAN_TYPE_CMD))) {
+            PRINTM(MERROR, "Wrong pkt from CTRL PORT: type=%d, len=%dd\n",
+                   pkt_type, pmbuf->data_len);
+            pmbuf->status_code = MLAN_ERROR_DATA_RX_FAIL;
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+        wlan_decode_rx_packet(pmadapter, pmbuf, pkt_type);
+    }
+
+  done:
+    if (ret == MLAN_STATUS_FAILURE) {
+        if (MP_RX_AGGR_IN_PROGRESS(pmadapter)) {
+            /* MP-A transfer failed - cleanup */
+            for (pind = 0; pind < pmadapter->mpa_rx.pkt_cnt; pind++) {
+                wlan_free_mlan_buffer(pmadapter,
+                                      pmadapter->mpa_rx.mbuf_arr[pind]);
+            }
+            MP_RX_AGGR_BUF_RESET(pmadapter);
+        }
+
+        if (f_do_rx_cur) {
+            /* Single Transfer pending */
+            /* Free curr buff also */
+            wlan_free_mlan_buffer(pmadapter, pmbuf);
+        }
+    }
+
+    LEAVE();
+    return ret;
+
+}
+#endif
+
+#ifdef SDIO_MULTI_PORT_TX_AGGR
+/**
+ *  @brief This function sends data to the card in SDIO aggregated mode.
+ *
+ *  @param pmadapter A pointer to mlan_adapter structure
+ *  @param mbuf      A pointer to the SDIO data/cmd buffer
+ *  @param port	     current port for aggregation
+ *  @param next_pkt_len Length of next packet used for multiport aggregation
+ *  @return 	     MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_host_to_card_mp_aggr(mlan_adapter * pmadapter, mlan_buffer * mbuf,
+                          t_u8 port, t_u32 next_pkt_len)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_s32 f_send_aggr_buf = 0;
+    t_s32 f_send_cur_buf = 0;
+    t_s32 f_precopy_cur_buf = 0;
+    t_s32 f_postcopy_cur_buf = 0;
+    t_u32 cmd53_port = 0;
+    mlan_buffer mbuf_aggr;
+
+    ENTER();
+
+    PRINTM(MIF_D, "host_2_card_mp_aggr: next_pkt_len: %d curr_port:%d\n",
+           next_pkt_len, port);
+
+    if (!pmadapter->mpa_tx.enabled) {
+        PRINTM(MINFO, "host_2_card_mp_aggr: tx aggregation disabled !\n");
+
+        f_send_cur_buf = 1;
+        goto tx_curr_single;
+    }
+
+    if (next_pkt_len) {
+        /* More pkt in TX queue */
+        PRINTM(MINFO, "host_2_card_mp_aggr: More packets in Queue.\n");
+
+        if (MP_TX_AGGR_IN_PROGRESS(pmadapter)) {
+            if (!MP_TX_AGGR_PORT_LIMIT_REACHED(pmadapter) &&
+                MP_TX_AGGR_BUF_HAS_ROOM(pmadapter, mbuf, mbuf->data_len)) {
+                f_precopy_cur_buf = 1;
+
+                if (!(pmadapter->mp_wr_bitmap & (1 << pmadapter->curr_wr_port))
+                    || !MP_TX_AGGR_BUF_HAS_ROOM(pmadapter, mbuf,
+                                                mbuf->data_len +
+                                                next_pkt_len)) {
+                    f_send_aggr_buf = 1;
+                }
+            } else {
+                /* No room in Aggr buf, send it */
+                f_send_aggr_buf = 1;
+
+                if (MP_TX_AGGR_PORT_LIMIT_REACHED(pmadapter) ||
+                    !(pmadapter->
+                      mp_wr_bitmap & (1 << pmadapter->curr_wr_port))) {
+                    f_send_cur_buf = 1;
+                } else {
+                    f_postcopy_cur_buf = 1;
+                }
+            }
+        } else {
+            if (MP_TX_AGGR_BUF_HAS_ROOM(pmadapter, mbuf, mbuf->data_len) &&
+                (pmadapter->mp_wr_bitmap & (1 << pmadapter->curr_wr_port)))
+                f_precopy_cur_buf = 1;
+            else
+                f_send_cur_buf = 1;
+        }
+    } else {
+        /* Last pkt in TX queue */
+        PRINTM(MINFO, "host_2_card_mp_aggr: Last packet in Tx Queue.\n");
+
+        if (MP_TX_AGGR_IN_PROGRESS(pmadapter)) {
+            /* some packs in Aggr buf already */
+            f_send_aggr_buf = 1;
+
+            if (MP_TX_AGGR_BUF_HAS_ROOM(pmadapter, mbuf, mbuf->data_len)) {
+                f_precopy_cur_buf = 1;
+            } else {
+                /* No room in Aggr buf, send it */
+                f_send_cur_buf = 1;
+            }
+        } else {
+            f_send_cur_buf = 1;
+        }
+    }
+
+    if (f_precopy_cur_buf) {
+        PRINTM(MINFO, "host_2_card_mp_aggr: Precopy current buffer\n");
+        MP_TX_AGGR_BUF_PUT(pmadapter, mbuf, port);
+
+        if (MP_TX_AGGR_PKT_LIMIT_REACHED(pmadapter) ||
+            MP_TX_AGGR_PORT_LIMIT_REACHED(pmadapter)) {
+            PRINTM(MIF_D,
+                   "host_2_card_mp_aggr: Aggregation Pkt limit reached\n");
+            /* No more pkts allowed in Aggr buf, send it */
+            f_send_aggr_buf = 1;
+        }
+    }
+
+    if (f_send_aggr_buf) {
+        PRINTM(MINFO, "host_2_card_mp_aggr: Send aggregation buffer."
+               "%d %d\n", pmadapter->mpa_tx.start_port,
+               pmadapter->mpa_tx.ports);
+
+        memset(pmadapter, &mbuf_aggr, 0, sizeof(mlan_buffer));
+
+        mbuf_aggr.pbuf = (t_u8 *) pmadapter->mpa_tx.buf;
+        mbuf_aggr.data_len = pmadapter->mpa_tx.buf_len;
+        cmd53_port = (pmadapter->ioport | SDIO_MPA_ADDR_BASE |
+                      (pmadapter->mpa_tx.ports << 4)) +
+            pmadapter->mpa_tx.start_port;
+        ret = wlan_write_data_sync(pmadapter, &mbuf_aggr, cmd53_port);
+        MP_TX_AGGR_BUF_RESET(pmadapter);
+    }
+
+  tx_curr_single:
+    if (f_send_cur_buf) {
+        PRINTM(MINFO, "host_2_card_mp_aggr: writing to port #%d\n", port);
+        ret = wlan_write_data_sync(pmadapter, mbuf, pmadapter->ioport + port);
+    }
+    if (f_postcopy_cur_buf) {
+        PRINTM(MINFO, "host_2_card_mp_aggr: Postcopy current buffer\n");
+        MP_TX_AGGR_BUF_PUT(pmadapter, mbuf, port);
+    }
+
+    LEAVE();
+    return ret;
+}
+#endif /* SDIO_MULTI_PORT_TX_AGGR */
+
+/********************************************************
+		Global functions
+********************************************************/
+
+/**
+ *  @brief This function checks if the interface is ready to download
+ *  or not while other download interface is present
+ *
+ *  @param pmadapter  A pointer to mlan_adapter structure
+ *  @param val        Winner status (0: winner)
+ *  @return           MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ *
+ */
+mlan_status
+wlan_check_winner_status(mlan_adapter * pmadapter, t_u32 * val)
+{
+    t_u32 winner = 0;
+    pmlan_callbacks pcb;
+
+    ENTER();
+
+    pcb = &pmadapter->callbacks;
+
+    if (MLAN_STATUS_SUCCESS !=
+        pcb->moal_read_reg(pmadapter->pmoal_handle, CARD_FW_STATUS0_REG,
+                           &winner)) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    *val = winner;
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function checks if the firmware is ready to accept
+ *  command or not.
+ *
+ *  @param pmadapter  A pointer to mlan_adapter structure
+ *  @param pollnum    Maximum polling number
+ *  @return           MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_check_fw_status(mlan_adapter * pmadapter, t_u32 pollnum)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 firmwarestat;
+    t_u32 tries;
+
+    ENTER();
+
+    /* Wait for firmware initialization event */
+    for (tries = 0; tries < pollnum; tries++) {
+        if (MLAN_STATUS_SUCCESS !=
+            (ret = wlan_sdio_read_fw_status(pmadapter, &firmwarestat)))
+            continue;
+        if (firmwarestat == FIRMWARE_READY) {
+            ret = MLAN_STATUS_SUCCESS;
+            break;
+        } else {
+            wlan_mdelay(pmadapter, 100);
+            ret = MLAN_STATUS_FAILURE;
+        }
+    }
+
+    if (ret != MLAN_STATUS_SUCCESS)
+        goto done;
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief  This function downloads firmware to card
+ *
+ *  @param pmadapter	A pointer to mlan_adapter
+ *  @param pmfw			A pointer to firmware image
+ *
+ *  @return		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_dnld_fw(IN pmlan_adapter pmadapter, IN pmlan_fw_image pmfw)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    /* Download the firmware image via helper */
+    ret = wlan_prog_fw_w_helper(pmadapter, pmfw);
+    if (ret != MLAN_STATUS_SUCCESS) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function probes the driver
+ *
+ *  @param pmadapter  A pointer to mlan_adapter structure
+ *  @return 	      MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_sdio_probe(pmlan_adapter pmadapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u32 sdio_ireg = 0;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+
+    ENTER();
+    /*
+     * Read the HOST_INT_STATUS_REG for ACK the first interrupt got
+     * from the bootloader. If we don't do this we get a interrupt
+     * as soon as we register the irq.
+     */
+    pcb->moal_read_reg(pmadapter->pmoal_handle, HOST_INT_STATUS_REG,
+                       &sdio_ireg);
+
+    /* Disable host interrupt mask register for SDIO */
+    ret = wlan_disable_host_int(pmadapter);
+    if (ret != MLAN_STATUS_SUCCESS) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    /* Get SDIO ioport */
+    ret = wlan_sdio_init_ioport(pmadapter);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function gets interrupt status.
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @return             N/A
+ */
+t_void
+wlan_interrupt(pmlan_adapter pmadapter)
+{
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    mlan_buffer mbuf;
+    t_u32 sdio_ireg = 0;
+
+    ENTER();
+
+    memset(pmadapter, &mbuf, 0, sizeof(mlan_buffer));
+    mbuf.pbuf = pmadapter->mp_regs;
+    mbuf.data_len = MAX_MP_REGS;
+
+    if (MLAN_STATUS_SUCCESS !=
+        pcb->moal_read_data_sync(pmadapter->pmoal_handle, &mbuf,
+                                 REG_PORT | MLAN_SDIO_BYTE_MODE_MASK, 0)) {
+        PRINTM(MERROR, "moal_read_data_sync: read registers failed\n");
+        pmadapter->dbg.num_int_read_failure++;
+        goto done;
+    }
+
+    DBG_HEXDUMP(MIF_D, "SDIO MP Registers", pmadapter->mp_regs, MAX_MP_REGS);
+    sdio_ireg = pmadapter->mp_regs[HOST_INT_STATUS_REG];
+    pmadapter->dbg.last_int_status = pmadapter->sdio_ireg | sdio_ireg;
+    if (sdio_ireg) {
+        /*
+         * DN_LD_HOST_INT_STATUS and/or UP_LD_HOST_INT_STATUS
+         * DN_LD_CMD_PORT_HOST_INT_STATUS and/or
+         * UP_LD_CMD_PORT_HOST_INT_STATUS
+         * Clear the interrupt status register
+         */
+        PRINTM(MINTR, "wlan_interrupt: sdio_ireg = 0x%x\n", sdio_ireg);
+        pcb->moal_spin_lock(pmadapter->pmoal_handle, pmadapter->pint_lock);
+        pmadapter->sdio_ireg |= sdio_ireg;
+        pcb->moal_spin_unlock(pmadapter->pmoal_handle, pmadapter->pint_lock);
+    } else {
+        PRINTM(MMSG, "wlan_interrupt: sdio_ireg = 0x%x\n", sdio_ireg);
+    }
+  done:
+    LEAVE();
+}
+
+/**
+ *  @brief This function enables the host interrupts.
+ *
+ *  @param pmadapter A pointer to mlan_adapter structure
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_enable_host_int(pmlan_adapter pmadapter)
+{
+    mlan_status ret;
+
+    ENTER();
+    ret = wlan_sdio_enable_host_int_mask(pmadapter, HIM_ENABLE);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function checks the interrupt status and handle it accordingly.
+ *
+ *  @param pmadapter A pointer to mlan_adapter structure
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_process_int_status(mlan_adapter * pmadapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    t_u8 sdio_ireg;
+    mlan_buffer *pmbuf = MNULL;
+    t_u8 port = 0;
+    t_u32 len_reg_l, len_reg_u;
+    t_u32 rx_blocks;
+    t_u32 ps_state = pmadapter->ps_state;
+    t_u16 rx_len;
+#if !defined(SDIO_MULTI_PORT_RX_AGGR)
+    t_u32 upld_typ = 0;
+#endif
+    t_u32 cr = 0;
+
+    ENTER();
+
+    pcb->moal_spin_lock(pmadapter->pmoal_handle, pmadapter->pint_lock);
+    sdio_ireg = pmadapter->sdio_ireg;
+    pmadapter->sdio_ireg = 0;
+    pcb->moal_spin_unlock(pmadapter->pmoal_handle, pmadapter->pint_lock);
+
+    if (!sdio_ireg)
+        goto done;
+
+    if (sdio_ireg & DN_LD_HOST_INT_STATUS) {
+        pmadapter->mp_wr_bitmap = (t_u32) pmadapter->mp_regs[WR_BITMAP_L];
+        pmadapter->mp_wr_bitmap |=
+            ((t_u32) pmadapter->mp_regs[WR_BITMAP_U]) << 8;
+        PRINTM(MINTR, "DNLD: wr_bitmap=0x%08x\n", pmadapter->mp_wr_bitmap);
+        if (pmadapter->data_sent &&
+            (pmadapter->mp_wr_bitmap & pmadapter->mp_data_port_mask)) {
+            PRINTM(MINFO, " <--- Tx DONE Interrupt --->\n");
+            pmadapter->data_sent = MFALSE;
+        }
+    }
+
+    /* As firmware will not generate download ready interrupt if the port
+       updated is command port only, cmd_sent should be done for any SDIO
+       interrupt. */
+    if (pmadapter->cmd_sent == MTRUE) {
+        /* Check if firmware has attach buffer at command port and update just
+           that in wr_bit_map. */
+        pmadapter->mp_wr_bitmap |=
+            (t_u32) pmadapter->mp_regs[WR_BITMAP_L] & CTRL_PORT_MASK;
+        if (pmadapter->mp_wr_bitmap & CTRL_PORT_MASK)
+            pmadapter->cmd_sent = MFALSE;
+    }
+
+    PRINTM(MINFO, "cmd_sent=%d, data_sent=%d\n", pmadapter->cmd_sent,
+           pmadapter->data_sent);
+
+    if (sdio_ireg & UP_LD_HOST_INT_STATUS) {
+        pmadapter->mp_rd_bitmap = (t_u32) pmadapter->mp_regs[RD_BITMAP_L];
+        pmadapter->mp_rd_bitmap |=
+            ((t_u32) pmadapter->mp_regs[RD_BITMAP_U]) << 8;
+        PRINTM(MINTR, "UPLD: rd_bitmap=0x%08x\n", pmadapter->mp_rd_bitmap);
+
+        while (MTRUE) {
+            ret = wlan_get_rd_port(pmadapter, &port);
+            if (ret != MLAN_STATUS_SUCCESS) {
+                PRINTM(MINFO, "no more rd_port to be handled\n");
+                break;
+            }
+            len_reg_l = RD_LEN_P0_L + (port << 1);
+            len_reg_u = RD_LEN_P0_U + (port << 1);
+            rx_len = ((t_u16) pmadapter->mp_regs[len_reg_u]) << 8;
+            rx_len |= (t_u16) pmadapter->mp_regs[len_reg_l];
+            PRINTM(MINFO, "RX: port=%d rx_len=%u\n", port, rx_len);
+            rx_blocks =
+                (rx_len + MLAN_SDIO_BLOCK_SIZE - 1) / MLAN_SDIO_BLOCK_SIZE;
+            if (rx_len <= INTF_HEADER_LEN ||
+                (rx_blocks * MLAN_SDIO_BLOCK_SIZE) > ALLOC_BUF_SIZE) {
+                PRINTM(MERROR, "invalid rx_len=%d\n", rx_len);
+                ret = MLAN_STATUS_FAILURE;
+                goto done;
+            }
+            rx_len = (t_u16) (rx_blocks * MLAN_SDIO_BLOCK_SIZE);
+            if (port == CTRL_PORT)
+                pmbuf = wlan_alloc_mlan_buffer(pmadapter, rx_len, 0, MTRUE);
+            else
+                pmbuf =
+                    wlan_alloc_mlan_buffer(pmadapter, rx_len,
+                                           MLAN_RX_HEADER_LEN, MFALSE);
+            if (pmbuf == MNULL) {
+                PRINTM(MERROR, "Failed to allocate 'mlan_buffer'\n");
+                ret = MLAN_STATUS_FAILURE;
+                goto done;
+            }
+            PRINTM(MINFO, "rx_len = %d\n", rx_len);
+#ifdef SDIO_MULTI_PORT_RX_AGGR
+            if (MLAN_STATUS_SUCCESS !=
+                wlan_sdio_card_to_host_mp_aggr(pmadapter, pmbuf, port,
+                                               rx_len)) {
+#else
+            /* Transfer data from card */
+            if (MLAN_STATUS_SUCCESS !=
+                wlan_sdio_card_to_host(pmadapter, &upld_typ,
+                                       (t_u32 *) & pmadapter->upld_len, pmbuf,
+                                       rx_len, pmadapter->ioport + port)) {
+#endif /* SDIO_MULTI_PORT_RX_AGGR */
+
+                if (port == CTRL_PORT)
+                    pmadapter->dbg.num_cmdevt_card_to_host_failure++;
+                else
+                    pmadapter->dbg.num_rx_card_to_host_failure++;
+
+                PRINTM(MERROR, "Card to host failed: int status=0x%x\n",
+                       sdio_ireg);
+#ifndef SDIO_MULTI_PORT_RX_AGGR
+                wlan_free_mlan_buffer(pmadapter, pmbuf);
+#endif
+                ret = MLAN_STATUS_FAILURE;
+                goto term_cmd53;
+            }
+#ifndef SDIO_MULTI_PORT_RX_AGGR
+            wlan_decode_rx_packet(pmadapter, pmbuf, upld_typ);
+#endif
+        }
+        /* We might receive data/sleep_cfm at the same time */
+        /* reset data_receive flag to avoid ps_state change */
+        if ((ps_state == PS_STATE_SLEEP_CFM) &&
+            (pmadapter->ps_state == PS_STATE_SLEEP))
+            pmadapter->data_received = MFALSE;
+    }
+
+    ret = MLAN_STATUS_SUCCESS;
+    goto done;
+
+  term_cmd53:
+    /* terminate cmd53 */
+    if (MLAN_STATUS_SUCCESS != pcb->moal_read_reg(pmadapter->pmoal_handle,
+                                                  HOST_TO_CARD_EVENT_REG, &cr))
+        PRINTM(MERROR, "read CFG reg failed\n");
+    PRINTM(MINFO, "Config Reg val = %d\n", cr);
+    if (MLAN_STATUS_SUCCESS != pcb->moal_write_reg(pmadapter->pmoal_handle,
+                                                   HOST_TO_CARD_EVENT_REG,
+                                                   (cr | HOST_TERM_CMD53)))
+        PRINTM(MERROR, "write CFG reg failed\n");
+    PRINTM(MINFO, "write success\n");
+    if (MLAN_STATUS_SUCCESS != pcb->moal_read_reg(pmadapter->pmoal_handle,
+                                                  HOST_TO_CARD_EVENT_REG, &cr))
+        PRINTM(MERROR, "read CFG reg failed\n");
+    PRINTM(MINFO, "Config reg val =%x\n", cr);
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function sends data to the card.
+ *
+ *  @param pmadapter A pointer to mlan_adapter structure
+ *  @param type	     data or command
+ *  @param pmbuf     A pointer to mlan_buffer (pmbuf->data_len should include SDIO header)
+ *  @param tx_param  A pointer to mlan_tx_param
+ *  @return 	     MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_sdio_host_to_card(mlan_adapter * pmadapter, t_u8 type, mlan_buffer * pmbuf,
+                       mlan_tx_param * tx_param)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u32 buf_block_len;
+    t_u32 blksz;
+    t_u8 port = 0;
+    t_u32 cmd53_port = 0;
+    t_u8 *payload = pmbuf->pbuf + pmbuf->data_offset;
+
+    ENTER();
+
+    /* Allocate buffer and copy payload */
+    blksz = MLAN_SDIO_BLOCK_SIZE;
+    buf_block_len = (pmbuf->data_len + blksz - 1) / blksz;
+    *(t_u16 *) & payload[0] = wlan_cpu_to_le16((t_u16) pmbuf->data_len);
+    *(t_u16 *) & payload[2] = wlan_cpu_to_le16(type);
+
+    /*
+     * This is SDIO specific header
+     *  t_u16 length,
+     *  t_u16 type (MLAN_TYPE_DATA = 0, MLAN_TYPE_CMD = 1, MLAN_TYPE_EVENT = 3)
+     */
+    if (type == MLAN_TYPE_DATA) {
+        ret = wlan_get_wr_port_data(pmadapter, &port);
+        if (ret != MLAN_STATUS_SUCCESS) {
+            PRINTM(MERROR, "no wr_port available: %d\n", ret);
+            goto exit;
+        }
+        /* Transfer data to card */
+        pmbuf->data_len = buf_block_len * blksz;
+
+#ifdef SDIO_MULTI_PORT_TX_AGGR
+        if (tx_param)
+            ret =
+                wlan_host_to_card_mp_aggr(pmadapter, pmbuf, port,
+                                          tx_param->next_pkt_len);
+        else
+            ret = wlan_host_to_card_mp_aggr(pmadapter, pmbuf, port, 0);
+#else
+        ret = wlan_write_data_sync(pmadapter, pmbuf, pmadapter->ioport + port);
+#endif /* SDIO_MULTI_PORT_TX_AGGR */
+
+    } else {
+        /* Type must be MLAN_TYPE_CMD */
+        pmadapter->cmd_sent = MTRUE;
+        /* clear CTRL PORT */
+        pmadapter->mp_wr_bitmap &= (t_u32) (~(1 << CTRL_PORT));
+        if (pmbuf->data_len <= INTF_HEADER_LEN ||
+            pmbuf->data_len > WLAN_UPLD_SIZE)
+            PRINTM(MWARN,
+                   "wlan_sdio_host_to_card(): Error: payload=%p, nb=%d\n",
+                   payload, pmbuf->data_len);
+        /* Transfer data to card */
+        pmbuf->data_len = buf_block_len * blksz;
+        cmd53_port = pmadapter->ioport + CTRL_PORT;
+        ret = wlan_write_data_sync(pmadapter, pmbuf, cmd53_port);
+    }
+
+    if (ret != MLAN_STATUS_SUCCESS) {
+        if (type == MLAN_TYPE_CMD)
+            pmadapter->cmd_sent = MFALSE;
+        if (type == MLAN_TYPE_DATA)
+            pmadapter->data_sent = MFALSE;
+    } else {
+        if (type == MLAN_TYPE_DATA) {
+            if (!(pmadapter->mp_wr_bitmap & (1 << pmadapter->curr_wr_port)))
+                pmadapter->data_sent = MTRUE;
+            else
+                pmadapter->data_sent = MFALSE;
+        }
+        DBG_HEXDUMP(MIF_D, "SDIO Blk Wr", pmbuf->pbuf + pmbuf->data_offset,
+                    MIN(pmbuf->data_len, MAX_DATA_DUMP_LEN));
+    }
+  exit:
+    LEAVE();
+    return ret;
+}
+
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+/**
+ *  @brief This function allocates buffer for the SDIO aggregation buffer
+ *  		related members of adapter structure
+ *
+ *  @param pmadapter       A pointer to mlan_adapter structure
+ *  @param mpa_tx_buf_size Tx buffer size to allocate
+ *  @param mpa_rx_buf_size Rx buffer size to allocate
+ *
+ *  @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_alloc_sdio_mpa_buffers(IN mlan_adapter * pmadapter,
+                            t_u32 mpa_tx_buf_size, t_u32 mpa_rx_buf_size)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+
+    ENTER();
+
+#ifdef SDIO_MULTI_PORT_TX_AGGR
+    ret =
+        pcb->moal_malloc(pmadapter->pmoal_handle,
+                         mpa_tx_buf_size + DMA_ALIGNMENT,
+                         MLAN_MEM_DEF | MLAN_MEM_DMA,
+                         (t_u8 **) & pmadapter->mpa_tx.head_ptr);
+    if (ret != MLAN_STATUS_SUCCESS || !pmadapter->mpa_tx.head_ptr) {
+        PRINTM(MERROR, "Could not allocate buffer for SDIO MP TX aggr\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto error;
+    }
+    pmadapter->mpa_tx.buf =
+        (t_u8 *) ALIGN_ADDR(pmadapter->mpa_tx.head_ptr, DMA_ALIGNMENT);
+    pmadapter->mpa_tx.buf_size = mpa_tx_buf_size;
+#endif /* SDIO_MULTI_PORT_TX_AGGR */
+
+#ifdef SDIO_MULTI_PORT_RX_AGGR
+    ret =
+        pcb->moal_malloc(pmadapter->pmoal_handle,
+                         mpa_rx_buf_size + DMA_ALIGNMENT,
+                         MLAN_MEM_DEF | MLAN_MEM_DMA,
+                         (t_u8 **) & pmadapter->mpa_rx.head_ptr);
+    if (ret != MLAN_STATUS_SUCCESS || !pmadapter->mpa_rx.head_ptr) {
+        PRINTM(MERROR, "Could not allocate buffer for SDIO MP RX aggr\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto error;
+    }
+    pmadapter->mpa_rx.buf =
+        (t_u8 *) ALIGN_ADDR(pmadapter->mpa_rx.head_ptr, DMA_ALIGNMENT);
+    pmadapter->mpa_rx.buf_size = mpa_rx_buf_size;
+#endif /* SDIO_MULTI_PORT_RX_AGGR */
+  error:
+    if (ret != MLAN_STATUS_SUCCESS) {
+        wlan_free_sdio_mpa_buffers(pmadapter);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function frees buffers for the SDIO aggregation
+ *
+ *  @param pmadapter       A pointer to mlan_adapter structure
+ *
+ *  @return        MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_free_sdio_mpa_buffers(IN mlan_adapter * pmadapter)
+{
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+
+    ENTER();
+
+#ifdef SDIO_MULTI_PORT_TX_AGGR
+    if (pmadapter->mpa_tx.buf) {
+        pcb->moal_mfree(pmadapter->pmoal_handle,
+                        (t_u8 *) pmadapter->mpa_tx.head_ptr);
+        pmadapter->mpa_tx.head_ptr = MNULL;
+        pmadapter->mpa_tx.buf = MNULL;
+        pmadapter->mpa_tx.buf_size = 0;
+    }
+#endif /* SDIO_MULTI_PORT_TX_AGGR */
+
+#ifdef SDIO_MULTI_PORT_RX_AGGR
+    if (pmadapter->mpa_rx.buf) {
+        pcb->moal_mfree(pmadapter->pmoal_handle,
+                        (t_u8 *) pmadapter->mpa_rx.head_ptr);
+        pmadapter->mpa_rx.head_ptr = MNULL;
+        pmadapter->mpa_rx.buf = MNULL;
+        pmadapter->mpa_rx.buf_size = 0;
+    }
+#endif /* SDIO_MULTI_PORT_RX_AGGR */
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+#endif /* SDIO_MULTI_PORT_TX_AGGR || SDIO_MULTI_PORT_RX_AGGR */
+
+/**
+ *  @brief  This function issues commands to initialize firmware
+ *
+ *  @param priv     	A pointer to mlan_private structure
+ *
+ *  @return		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_set_sdio_gpio_int(IN pmlan_private priv)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_adapter pmadapter = priv->adapter;
+    HostCmd_DS_SDIO_GPIO_INT_CONFIG sdio_int_cfg;
+
+    ENTER();
+
+    if (pmadapter->int_mode == INT_MODE_GPIO) {
+        PRINTM(MINFO, "SDIO_GPIO_INT_CONFIG: interrupt mode is GPIO\n");
+        sdio_int_cfg.action = HostCmd_ACT_GEN_SET;
+        sdio_int_cfg.gpio_pin = pmadapter->gpio_pin;
+        sdio_int_cfg.gpio_int_edge = INT_FALLING_EDGE;
+        sdio_int_cfg.gpio_pulse_width = DELAY_1_US;
+        ret = wlan_prepare_cmd(priv, HostCmd_CMD_SDIO_GPIO_INT_CONFIG,
+                               HostCmd_ACT_GEN_SET, 0, MNULL, &sdio_int_cfg);
+
+        if (ret) {
+            PRINTM(MERROR, "SDIO_GPIO_INT_CONFIG: send command fail\n");
+            ret = MLAN_STATUS_FAILURE;
+        }
+    } else {
+        PRINTM(MINFO, "SDIO_GPIO_INT_CONFIG: interrupt mode is SDIO\n");
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function prepares command of SDIO GPIO interrupt
+ *
+ *  @param pmpriv	A pointer to mlan_private structure
+ *  @param cmd	   	A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_sdio_gpio_int(pmlan_private pmpriv,
+                       IN HostCmd_DS_COMMAND * cmd,
+                       IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    HostCmd_DS_SDIO_GPIO_INT_CONFIG *psdio_gpio_int =
+        &cmd->params.sdio_gpio_int;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_SDIO_GPIO_INT_CONFIG);
+    cmd->size =
+        wlan_cpu_to_le16((sizeof(HostCmd_DS_SDIO_GPIO_INT_CONFIG)) + S_DS_GEN);
+
+    memset(pmpriv->adapter, psdio_gpio_int, 0,
+           sizeof(HostCmd_DS_SDIO_GPIO_INT_CONFIG));
+    if (cmd_action == HostCmd_ACT_GEN_SET) {
+        memcpy(pmpriv->adapter, psdio_gpio_int, pdata_buf,
+               sizeof(HostCmd_DS_SDIO_GPIO_INT_CONFIG));
+        psdio_gpio_int->action = wlan_cpu_to_le16(psdio_gpio_int->action);
+        psdio_gpio_int->gpio_pin = wlan_cpu_to_le16(psdio_gpio_int->gpio_pin);
+        psdio_gpio_int->gpio_int_edge =
+            wlan_cpu_to_le16(psdio_gpio_int->gpio_int_edge);
+        psdio_gpio_int->gpio_pulse_width =
+            wlan_cpu_to_le16(psdio_gpio_int->gpio_pulse_width);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_sdio.h b/drivers/net/wireless/sd8797/mlan/mlan_sdio.h
new file mode 100644
index 000000000000..f77397d66eb4
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_sdio.h
@@ -0,0 +1,307 @@
+/** @file mlan_sdio.h
+  * 
+  * @brief This file contains definitions for SDIO interface.
+  * driver. 
+  *
+  * Copyright (C) 2008-2011, Marvell International Ltd. 
+  * 
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+/****************************************************
+Change log:
+****************************************************/
+
+#ifndef	_MLAN_SDIO_H
+#define	_MLAN_SDIO_H
+
+/** Block mode */
+#define BLOCK_MODE	1
+/** Fixed address mode */
+#define FIXED_ADDRESS	0
+
+/* Host Control Registers */
+/** Host Control Registers : Host to Card Event */
+#define HOST_TO_CARD_EVENT_REG		0x00
+/** Host Control Registers : Host terminates Command 53 */
+#define HOST_TERM_CMD53			(0x1U << 2)
+/** Host Control Registers : Host without Command 53 finish host */
+#define HOST_WO_CMD53_FINISH_HOST	(0x1U << 2)
+/** Host Control Registers : Host power up */
+#define HOST_POWER_UP			(0x1U << 1)
+/** Host Control Registers : Host power down */
+#define HOST_POWER_DOWN			(0x1U << 0)
+
+/** Host Control Registers : Host interrupt RSR */
+#define HOST_INT_RSR_REG		0x01
+/** Host Control Registers : Upload host interrupt RSR */
+#define UP_LD_HOST_INT_RSR		(0x1U)
+#define HOST_INT_RSR_MASK		0x3F
+
+/** Host Control Registers : Host interrupt mask */
+#define HOST_INT_MASK_REG		0x02
+/** Host Control Registers : Upload host interrupt mask */
+#define UP_LD_HOST_INT_MASK		(0x1U)
+/** Host Control Registers : Download host interrupt mask */
+#define DN_LD_HOST_INT_MASK		(0x2U)
+/** Enable Host interrupt mask */
+#define HIM_ENABLE			(UP_LD_HOST_INT_MASK | DN_LD_HOST_INT_MASK)
+/** Disable Host interrupt mask */
+#define	HIM_DISABLE			0xff
+
+/** Host Control Registers : Host interrupt status */
+#define HOST_INT_STATUS_REG		0x03
+/** Host Control Registers : Upload host interrupt status */
+#define UP_LD_HOST_INT_STATUS		(0x1U)
+/** Host Control Registers : Download host interrupt status */
+#define DN_LD_HOST_INT_STATUS		(0x2U)
+
+/** Port for registers */
+#define REG_PORT			0
+/** LSB of read bitmap */
+#define RD_BITMAP_L			0x04
+/** MSB of read bitmap */
+#define RD_BITMAP_U			0x05
+/** LSB of write bitmap */
+#define WR_BITMAP_L			0x06
+/** MSB of write bitmap */
+#define WR_BITMAP_U			0x07
+/** LSB of read length for port 0 */
+#define RD_LEN_P0_L			0x08
+/** MSB of read length for port 0 */
+#define RD_LEN_P0_U			0x09
+/** Ctrl port */
+#define CTRL_PORT			0
+/** Ctrl port mask */
+#define CTRL_PORT_MASK			0x0001
+/** Data port mask */
+#define DATA_PORT_MASK			0xfffe
+/** Misc. Config Register : Auto Re-enable interrupts */
+#define AUTO_RE_ENABLE_INT             	MBIT(4)
+
+/** Host Control Registers : Host transfer status */
+#define HOST_RESTART_REG		0x28
+/** Host Control Registers : Download CRC error */
+#define DN_LD_CRC_ERR              	(0x1U << 2)
+/** Host Control Registers : Upload restart */
+#define UP_LD_RESTART              	(0x1U << 1)
+/** Host Control Registers : Download restart */
+#define DN_LD_RESTART              	(0x1U << 0)
+
+/* Card Control Registers */
+/** Card Control Registers : Card to host event */
+#define CARD_TO_HOST_EVENT_REG         	0x30
+/** Card Control Registers : Card I/O ready */
+#define CARD_IO_READY              	(0x1U << 3)
+/** Card Control Registers : CIS card ready */
+#define CIS_CARD_RDY                 	(0x1U << 2)
+/** Card Control Registers : Upload card ready */
+#define UP_LD_CARD_RDY               	(0x1U << 1)
+/** Card Control Registers : Download card ready */
+#define DN_LD_CARD_RDY               	(0x1U << 0)
+
+/** Card Control Registers : Host interrupt mask register */
+#define HOST_INTERRUPT_MASK_REG      	0x34
+/** Card Control Registers : Host power interrupt mask */
+#define HOST_POWER_INT_MASK          	(0x1U << 3)
+/** Card Control Registers : Abort card interrupt mask */
+#define ABORT_CARD_INT_MASK          	(0x1U << 2)
+/** Card Control Registers : Upload card interrupt mask */
+#define UP_LD_CARD_INT_MASK          	(0x1U << 1)
+/** Card Control Registers : Download card interrupt mask */
+#define DN_LD_CARD_INT_MASK          	(0x1U << 0)
+
+/** Card Control Registers : Card interrupt status register */
+#define CARD_INTERRUPT_STATUS_REG    	0x38
+/** Card Control Registers : Power up interrupt */
+#define POWER_UP_INT                 	(0x1U << 4)
+/** Card Control Registers : Power down interrupt */
+#define POWER_DOWN_INT               	(0x1U << 3)
+
+/** Card Control Registers : Card interrupt RSR register */
+#define CARD_INTERRUPT_RSR_REG       	0x3c
+/** Card Control Registers : Power up RSR */
+#define POWER_UP_RSR                 	(0x1U << 4)
+/** Card Control Registers : Power down RSR */
+#define POWER_DOWN_RSR               	(0x1U << 3)
+
+/** Card Control Registers : SQ Read base address 0 register */
+#define READ_BASE_0_REG  		0x40
+/** Card Control Registers : SQ Read base address 1 register */
+#define READ_BASE_1_REG  		0x41
+
+/** Card Control Registers : Card revision register */
+#define CARD_REVISION_REG            	0x5c
+
+/** Firmware status 0 register (SCRATCH0_0) */
+#define CARD_FW_STATUS0_REG		0x60
+/** Firmware status 1 register (SCRATCH0_1) */
+#define CARD_FW_STATUS1_REG		0x61
+/** Rx length register (SCRATCH0_2) */
+#define CARD_RX_LEN_REG			0x62
+/** Rx unit register (SCRATCH0_3) */
+#define CARD_RX_UNIT_REG		0x63
+
+/** Card Control Registers : Card OCR 0 register */
+#define CARD_OCR_0_REG               	0x68
+/** Card Control Registers : Card OCR 1 register */
+#define CARD_OCR_1_REG               	0x69
+/** Card Control Registers : Card OCR 3 register */
+#define CARD_OCR_3_REG               	0x6A
+/** Card Control Registers : Card config register */
+#define CARD_CONFIG_REG              	0x6B
+/** Card Control Registers : Miscellaneous Configuration Register */
+#define CARD_MISC_CFG_REG              	0x6C
+
+/** Card Control Registers : Debug 0 register */
+#define DEBUG_0_REG                  	0x70
+/** Card Control Registers : SD test BUS 0 */
+#define SD_TESTBUS0                  	(0x1U)
+/** Card Control Registers : Debug 1 register */
+#define DEBUG_1_REG                  	0x71
+/** Card Control Registers : SD test BUS 1 */
+#define SD_TESTBUS1                  	(0x1U)
+/** Card Control Registers : Debug 2 register */
+#define DEBUG_2_REG                  	0x72
+/** Card Control Registers : SD test BUS 2 */
+#define SD_TESTBUS2                  	(0x1U)
+/** Card Control Registers : Debug 3 register */
+#define DEBUG_3_REG                  	0x73
+/** Card Control Registers : SD test BUS 3 */
+#define SD_TESTBUS3                  	(0x1U)
+
+/** Host Control Registers : I/O port 0 */
+#define IO_PORT_0_REG			0x78
+/** Host Control Registers : I/O port 1 */
+#define IO_PORT_1_REG			0x79
+/** Host Control Registers : I/O port 2 */
+#define IO_PORT_2_REG			0x7A
+
+/** Event header Len*/
+#define MLAN_EVENT_HEADER_LEN           8
+
+/** SDIO byte mode size */
+#define MAX_BYTE_MODE_SIZE             512
+
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+/** The base address for packet with multiple ports aggregation */
+#define SDIO_MPA_ADDR_BASE             0x1000
+#endif
+
+#ifdef SDIO_MULTI_PORT_TX_AGGR
+
+/** SDIO Tx aggregation in progress ? */
+#define MP_TX_AGGR_IN_PROGRESS(a) (a->mpa_tx.pkt_cnt>0)
+
+/** SDIO Tx aggregation buffer room for next packet ? */
+#define MP_TX_AGGR_BUF_HAS_ROOM(a,mbuf, len) ((a->mpa_tx.buf_len+len)<= a->mpa_tx.buf_size)
+
+/** Copy current packet (SDIO Tx aggregation buffer) to SDIO buffer */
+#define MP_TX_AGGR_BUF_PUT(a, mbuf, port) do{                   \
+    pmadapter->callbacks.moal_memmove(a->pmoal_handle, &a->mpa_tx.buf[a->mpa_tx.buf_len],mbuf->pbuf+mbuf->data_offset,mbuf->data_len);\
+    a->mpa_tx.buf_len += mbuf->data_len;                        \
+    if(!a->mpa_tx.pkt_cnt){                                     \
+        a->mpa_tx.start_port = port;                            \
+    }                                                           \
+    if(a->mpa_tx.start_port<=port){                             \
+        a->mpa_tx.ports |= (1<<(a->mpa_tx.pkt_cnt)); 			\
+    }else{                                                      \
+          a->mpa_tx.ports |= (1<<(a->mpa_tx.pkt_cnt+1+(MAX_PORT - a->mp_end_port)));           \
+    }                                                           \
+    a->mpa_tx.pkt_cnt++;                                        \
+}while(0);
+
+/** SDIO Tx aggregation limit ? */
+#define MP_TX_AGGR_PKT_LIMIT_REACHED(a) (a->mpa_tx.pkt_cnt==a->mpa_tx.pkt_aggr_limit)
+
+/** SDIO Tx aggregation port limit ? */
+#define MP_TX_AGGR_PORT_LIMIT_REACHED(a) ((a->curr_wr_port < \
+			a->mpa_tx.start_port) && (((MAX_PORT - \
+			a->mpa_tx.start_port) + a->curr_wr_port) >= \
+				SDIO_MP_AGGR_DEF_PKT_LIMIT))
+
+/** Reset SDIO Tx aggregation buffer parameters */
+#define MP_TX_AGGR_BUF_RESET(a) do{         \
+   a->mpa_tx.pkt_cnt = 0;                   \
+   a->mpa_tx.buf_len = 0;                   \
+   a->mpa_tx.ports = 0;                     \
+   a->mpa_tx.start_port = 0;                \
+} while(0);
+
+#endif /* SDIO_MULTI_PORT_TX_AGGR */
+
+#ifdef SDIO_MULTI_PORT_RX_AGGR
+
+/** SDIO Rx aggregation limit ? */
+#define MP_RX_AGGR_PKT_LIMIT_REACHED(a) (a->mpa_rx.pkt_cnt== a->mpa_rx.pkt_aggr_limit)
+
+/** SDIO Rx aggregation port limit ? */
+#define MP_RX_AGGR_PORT_LIMIT_REACHED(a) ((a->curr_rd_port < \
+			a->mpa_rx.start_port) && (((MAX_PORT - \
+			a->mpa_rx.start_port) + a->curr_rd_port) >= \
+			SDIO_MP_AGGR_DEF_PKT_LIMIT))
+
+/** SDIO Rx aggregation in progress ? */
+#define MP_RX_AGGR_IN_PROGRESS(a) (a->mpa_rx.pkt_cnt>0)
+
+/** SDIO Rx aggregation buffer room for next packet ? */
+#define MP_RX_AGGR_BUF_HAS_ROOM(a,rx_len)   ((a->mpa_rx.buf_len+rx_len)<=a->mpa_rx.buf_size)
+
+/** Prepare to copy current packet from card to SDIO Rx aggregation buffer */
+#define MP_RX_AGGR_SETUP(a, mbuf, port, rx_len) do{    \
+    a->mpa_rx.buf_len += rx_len;                       \
+    if(!a->mpa_rx.pkt_cnt){                            \
+        a->mpa_rx.start_port = port;                   \
+    }                                                  \
+    if(a->mpa_rx.start_port<=port){                    \
+        a->mpa_rx.ports |= (1<<(a->mpa_rx.pkt_cnt));   \
+    }else{                                             \
+        a->mpa_rx.ports |= (1<<(a->mpa_rx.pkt_cnt+1)); \
+    }                                                  \
+    a->mpa_rx.mbuf_arr[a->mpa_rx.pkt_cnt] = mbuf;      \
+    a->mpa_rx.len_arr[a->mpa_rx.pkt_cnt] = rx_len;     \
+    a->mpa_rx.pkt_cnt++;                               \
+}while(0);
+
+/** Reset SDIO Rx aggregation buffer parameters */
+#define MP_RX_AGGR_BUF_RESET(a) do{         \
+   a->mpa_rx.pkt_cnt = 0;                   \
+   a->mpa_rx.buf_len = 0;                   \
+   a->mpa_rx.ports = 0;                     \
+   a->mpa_rx.start_port = 0;                \
+} while(0);
+
+#endif /* SDIO_MULTI_PORT_RX_AGGR */
+
+/** Enable host interrupt */
+mlan_status wlan_enable_host_int(pmlan_adapter pmadapter);
+/** Probe and initialization function */
+mlan_status wlan_sdio_probe(pmlan_adapter pmadapter);
+/** multi interface download check */
+mlan_status wlan_check_winner_status(mlan_adapter * pmadapter, t_u32 * val);
+/** Firmware status check */
+mlan_status wlan_check_fw_status(mlan_adapter * pmadapter, t_u32 pollnum);
+/** Read interrupt status */
+t_void wlan_interrupt(pmlan_adapter pmadapter);
+/** Process Interrupt Status */
+mlan_status wlan_process_int_status(mlan_adapter * pmadapter);
+/** Transfer data to card */
+mlan_status wlan_sdio_host_to_card(mlan_adapter * pmadapter, t_u8 type,
+                                   mlan_buffer * mbuf,
+                                   mlan_tx_param * tx_param);
+mlan_status wlan_set_sdio_gpio_int(IN pmlan_private priv);
+mlan_status wlan_cmd_sdio_gpio_int(pmlan_private pmpriv,
+                                   IN HostCmd_DS_COMMAND * cmd,
+                                   IN t_u16 cmd_action, IN t_void * pdata_buf);
+#endif /* _MLAN_SDIO_H */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_shim.c b/drivers/net/wireless/sd8797/mlan/mlan_shim.c
new file mode 100644
index 000000000000..07f7db5c7227
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_shim.c
@@ -0,0 +1,946 @@
+/** @file mlan_shim.c
+ *  
+ *  @brief This file contains APIs to MOAL module.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/********************************************************
+Change log:
+    10/13/2008: initial version
+********************************************************/
+
+#include "mlan.h"
+#ifdef STA_SUPPORT
+#include "mlan_join.h"
+#endif
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_wmm.h"
+#include "mlan_sdio.h"
+#ifdef UAP_SUPPORT
+#include "mlan_uap.h"
+#endif
+#include "mlan_11h.h"
+
+/********************************************************
+                Local Variables
+********************************************************/
+
+/********************************************************
+        Global Variables
+********************************************************/
+#ifdef STA_SUPPORT
+mlan_operations mlan_sta_ops = {
+    /* init cmd handler */
+    wlan_ops_sta_init_cmd,
+    /* ioctl handler */
+    wlan_ops_sta_ioctl,
+    /* cmd handler */
+    wlan_ops_sta_prepare_cmd,
+    /* cmdresp handler */
+    wlan_ops_sta_process_cmdresp,
+    /* rx handler */
+    wlan_ops_sta_process_rx_packet,
+    /* Event handler */
+    wlan_ops_sta_process_event,
+    /* txpd handler */
+    wlan_ops_sta_process_txpd,
+    /* BSS role: STA */
+    MLAN_BSS_ROLE_STA,
+};
+#endif
+#ifdef UAP_SUPPORT
+mlan_operations mlan_uap_ops = {
+    /* init cmd handler */
+    wlan_ops_uap_init_cmd,
+    /* ioctl handler */
+    wlan_ops_uap_ioctl,
+    /* cmd handler */
+    wlan_ops_uap_prepare_cmd,
+    /* cmdresp handler */
+    wlan_ops_uap_process_cmdresp,
+    /* rx handler */
+    wlan_ops_uap_process_rx_packet,
+    /* Event handler */
+    wlan_ops_uap_process_event,
+    /* txpd handler */
+    wlan_ops_uap_process_txpd,
+    /* BSS role: uAP */
+    MLAN_BSS_ROLE_UAP,
+};
+#endif
+
+/** mlan function table */
+mlan_operations *mlan_ops[] = {
+#ifdef STA_SUPPORT
+    &mlan_sta_ops,
+#endif
+#ifdef UAP_SUPPORT
+    &mlan_uap_ops,
+#endif
+    MNULL,
+};
+
+/** Global moal_assert callback */
+t_void(*assert_callback) (IN t_void * pmoal_handle, IN t_u32 cond) = MNULL;
+#ifdef DEBUG_LEVEL1
+#ifdef DEBUG_LEVEL2
+#define DEFAULT_DEBUG_MASK	(0xffffffff)
+#else
+#define DEFAULT_DEBUG_MASK	(MMSG | MFATAL | MERROR)
+#endif
+
+/** Global moal_print callback */
+t_void(*print_callback) (IN t_void * pmoal_handle,
+                         IN t_u32 level, IN t_s8 * pformat, IN ...) = MNULL;
+
+/** Global moal_get_system_time callback */
+mlan_status(*get_sys_time_callback) (IN t_void * pmoal_handle,
+                                     OUT t_u32 * psec,
+                                     OUT t_u32 * pusec) = MNULL;
+
+/** Global driver debug mit masks */
+t_u32 drvdbg = DEFAULT_DEBUG_MASK;
+#endif
+
+/********************************************************
+        Local Functions
+*******************************************************/
+
+/********************************************************
+        Global Functions
+********************************************************/
+
+/**
+ *  @brief This function registers MOAL to MLAN module.
+ *
+ *  @param pmdevice        A pointer to a mlan_device structure
+ *                         allocated in MOAL
+ *  @param ppmlan_adapter  A pointer to a t_void pointer to store
+ *                         mlan_adapter structure pointer as the context
+ *
+ *  @return                MLAN_STATUS_SUCCESS
+ *                             The registration succeeded.
+ *                         MLAN_STATUS_FAILURE
+ *                             The registration failed.
+ *
+ * mlan_status mlan_register (
+ *   IN pmlan_device     pmdevice,
+ *   OUT t_void          **ppmlan_adapter
+ * );
+ *
+ * Comments
+ *   MOAL constructs mlan_device data structure to pass moal_handle and
+ *   mlan_callback table to MLAN. MLAN returns mlan_adapter pointer to
+ *   the ppmlan_adapter buffer provided by MOAL.
+ * Headers:
+ *   declared in mlan_decl.h
+ * See Also
+ *   mlan_unregister
+ */
+mlan_status
+mlan_register(IN pmlan_device pmdevice, OUT t_void ** ppmlan_adapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_adapter pmadapter = MNULL;
+    pmlan_callbacks pcb = MNULL;
+    t_u8 i = 0;
+    t_u32 j = 0;
+
+    MASSERT(pmdevice);
+    MASSERT(ppmlan_adapter);
+    MASSERT(pmdevice->callbacks.moal_print);
+#ifdef DEBUG_LEVEL1
+    print_callback = pmdevice->callbacks.moal_print;
+    get_sys_time_callback = pmdevice->callbacks.moal_get_system_time;
+#endif
+    assert_callback = pmdevice->callbacks.moal_assert;
+
+    ENTER();
+
+    MASSERT(pmdevice->callbacks.moal_malloc);
+    MASSERT(pmdevice->callbacks.moal_memset);
+    MASSERT(pmdevice->callbacks.moal_memmove);
+
+    /* Allocate memory for adapter structure */
+    if ((pmdevice->callbacks.
+         moal_malloc(pmdevice->pmoal_handle, sizeof(mlan_adapter), MLAN_MEM_DEF,
+                     (t_u8 **) & pmadapter) != MLAN_STATUS_SUCCESS)
+        || !pmadapter) {
+        ret = MLAN_STATUS_FAILURE;
+        goto exit_register;
+    }
+
+    pmdevice->callbacks.moal_memset(pmadapter, pmadapter,
+                                    0, sizeof(mlan_adapter));
+
+    pcb = &pmadapter->callbacks;
+
+    /* Save callback functions */
+    pmdevice->callbacks.moal_memmove(pmadapter->pmoal_handle, pcb,
+                                     &pmdevice->callbacks,
+                                     sizeof(mlan_callbacks));
+
+    /* Assertion for all callback functions */
+    MASSERT(pcb->moal_init_fw_complete);
+    MASSERT(pcb->moal_shutdown_fw_complete);
+    MASSERT(pcb->moal_send_packet_complete);
+    MASSERT(pcb->moal_recv_packet);
+    MASSERT(pcb->moal_recv_event);
+    MASSERT(pcb->moal_ioctl_complete);
+    MASSERT(pcb->moal_write_reg);
+    MASSERT(pcb->moal_read_reg);
+    MASSERT(pcb->moal_alloc_mlan_buffer);
+    MASSERT(pcb->moal_free_mlan_buffer);
+    MASSERT(pcb->moal_write_data_sync);
+    MASSERT(pcb->moal_read_data_sync);
+    MASSERT(pcb->moal_mfree);
+    MASSERT(pcb->moal_memcpy);
+    MASSERT(pcb->moal_memcmp);
+    MASSERT(pcb->moal_get_system_time);
+    MASSERT(pcb->moal_init_timer);
+    MASSERT(pcb->moal_free_timer);
+    MASSERT(pcb->moal_start_timer);
+    MASSERT(pcb->moal_stop_timer);
+    MASSERT(pcb->moal_init_lock);
+    MASSERT(pcb->moal_free_lock);
+    MASSERT(pcb->moal_spin_lock);
+    MASSERT(pcb->moal_spin_unlock);
+
+    /* Save pmoal_handle */
+    pmadapter->pmoal_handle = pmdevice->pmoal_handle;
+
+    if ((pmdevice->int_mode == INT_MODE_GPIO) && (pmdevice->gpio_pin == 0)) {
+        PRINTM(MERROR, "SDIO_GPIO_INT_CONFIG: Invalid GPIO Pin\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto error;
+    }
+    pmadapter->init_para.int_mode = pmdevice->int_mode;
+    pmadapter->init_para.gpio_pin = pmdevice->gpio_pin;
+    /* card specific probing has been deferred until now .. */
+    if (MLAN_STATUS_SUCCESS != (ret = wlan_sdio_probe(pmadapter))) {
+        ret = MLAN_STATUS_FAILURE;
+        goto error;
+    }
+#ifdef DEBUG_LEVEL1
+    drvdbg = pmdevice->drvdbg;
+#endif
+
+#ifdef MFG_CMD_SUPPORT
+    pmadapter->init_para.mfg_mode = pmdevice->mfg_mode;
+#endif
+#ifdef SDIO_MULTI_PORT_TX_AGGR
+    pmadapter->init_para.mpa_tx_cfg = pmdevice->mpa_tx_cfg;
+#endif
+#ifdef SDIO_MULTI_PORT_RX_AGGR
+    pmadapter->init_para.mpa_rx_cfg = pmdevice->mpa_rx_cfg;
+#endif
+    pmadapter->init_para.auto_ds = pmdevice->auto_ds;
+    pmadapter->init_para.ps_mode = pmdevice->ps_mode;
+    if (pmdevice->max_tx_buf == MLAN_TX_DATA_BUF_SIZE_2K ||
+        pmdevice->max_tx_buf == MLAN_TX_DATA_BUF_SIZE_4K ||
+        pmdevice->max_tx_buf == MLAN_TX_DATA_BUF_SIZE_8K)
+        pmadapter->init_para.max_tx_buf = pmdevice->max_tx_buf;
+#ifdef STA_SUPPORT
+    pmadapter->init_para.cfg_11d = pmdevice->cfg_11d;
+#else
+    pmadapter->init_para.cfg_11d = 0;
+#endif
+    pmadapter->init_para.dfs_master_radar_det_en = DFS_MASTER_RADAR_DETECT_EN;
+    pmadapter->init_para.dfs_slave_radar_det_en = DFS_SLAVE_RADAR_DETECT_EN;
+
+    pmadapter->priv_num = 0;
+    for (i = 0; i < MLAN_MAX_BSS_NUM; i++) {
+        pmadapter->priv[i] = MNULL;
+        if (pmdevice->bss_attr[i].active == MTRUE) {
+            /* For valid bss_attr, allocate memory for private structure */
+            if ((pcb->
+                 moal_malloc(pmadapter->pmoal_handle, sizeof(mlan_private),
+                             MLAN_MEM_DEF,
+                             (t_u8 **) & pmadapter->priv[i]) !=
+                 MLAN_STATUS_SUCCESS)
+                || !pmadapter->priv[i]) {
+                ret = MLAN_STATUS_FAILURE;
+                goto error;
+            }
+
+            pmadapter->priv_num++;
+            memset(pmadapter, pmadapter->priv[i], 0, sizeof(mlan_private));
+
+            pmadapter->priv[i]->adapter = pmadapter;
+
+            /* Save bss_type, frame_type & bss_priority */
+            pmadapter->priv[i]->bss_type =
+                (t_u8) pmdevice->bss_attr[i].bss_type;
+            pmadapter->priv[i]->frame_type =
+                (t_u8) pmdevice->bss_attr[i].frame_type;
+            pmadapter->priv[i]->bss_priority =
+                (t_u8) pmdevice->bss_attr[i].bss_priority;
+            if (pmdevice->bss_attr[i].bss_type == MLAN_BSS_TYPE_STA)
+                pmadapter->priv[i]->bss_role = MLAN_BSS_ROLE_STA;
+            else if (pmdevice->bss_attr[i].bss_type == MLAN_BSS_TYPE_UAP)
+                pmadapter->priv[i]->bss_role = MLAN_BSS_ROLE_UAP;
+#ifdef WIFI_DIRECT_SUPPORT
+            else if (pmdevice->bss_attr[i].bss_type == MLAN_BSS_TYPE_WIFIDIRECT)
+                pmadapter->priv[i]->bss_role = MLAN_BSS_ROLE_STA;
+#endif
+            /* Save bss_index and bss_num */
+            pmadapter->priv[i]->bss_index = i;
+            pmadapter->priv[i]->bss_num = (t_u8) pmdevice->bss_attr[i].bss_num;
+
+            /* init function table */
+            for (j = 0; mlan_ops[j]; j++) {
+                if (mlan_ops[j]->bss_role == GET_BSS_ROLE(pmadapter->priv[i])) {
+                    memcpy(pmadapter, &pmadapter->priv[i]->ops, mlan_ops[j],
+                           sizeof(mlan_operations));
+                }
+            }
+        }
+    }
+
+    /* Initialize lock variables */
+    if (wlan_init_lock_list(pmadapter) != MLAN_STATUS_SUCCESS) {
+        ret = MLAN_STATUS_FAILURE;
+        goto error;
+    }
+
+    /* Allocate memory for member of adapter structure */
+    if (wlan_allocate_adapter(pmadapter)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto error;
+    }
+
+    /* Initialize timers */
+    if (wlan_init_timer(pmadapter) != MLAN_STATUS_SUCCESS) {
+        ret = MLAN_STATUS_FAILURE;
+        goto error;
+    }
+    /* Return pointer of mlan_adapter to MOAL */
+    *ppmlan_adapter = pmadapter;
+
+    goto exit_register;
+
+  error:
+    PRINTM(MINFO, "Leave mlan_register with error\n");
+    /* Free timers */
+    wlan_free_timer(pmadapter);
+    /* Free adapter structure */
+    wlan_free_adapter(pmadapter);
+    /* Free lock variables */
+    wlan_free_lock_list(pmadapter);
+    for (i = 0; i < MLAN_MAX_BSS_NUM; i++) {
+        if (pmadapter->priv[i])
+            pcb->moal_mfree(pmadapter->pmoal_handle,
+                            (t_u8 *) pmadapter->priv[i]);
+    }
+    pcb->moal_mfree(pmadapter->pmoal_handle, (t_u8 *) pmadapter);
+
+  exit_register:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function unregisters MOAL from MLAN module.
+ *
+ *  @param pmlan_adapter   A pointer to a mlan_device structure
+ *                         allocated in MOAL
+ *
+ *  @return                MLAN_STATUS_SUCCESS
+ *                             The deregistration succeeded.
+ */
+mlan_status
+mlan_unregister(IN t_void * pmlan_adapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = (mlan_adapter *) pmlan_adapter;
+    pmlan_callbacks pcb;
+    t_s32 i = 0;
+
+    MASSERT(pmlan_adapter);
+
+    ENTER();
+
+    pcb = &pmadapter->callbacks;
+
+    /* Free adapter structure */
+    wlan_free_adapter(pmadapter);
+
+    /* Free timers */
+    wlan_free_timer(pmadapter);
+
+    /* Free lock variables */
+    wlan_free_lock_list(pmadapter);
+
+    /* Free private structures */
+    for (i = 0; i < pmadapter->priv_num; i++) {
+        if (pmadapter->priv[i]) {
+            pcb->moal_mfree(pmadapter->pmoal_handle,
+                            (t_u8 *) pmadapter->priv[i]);
+        }
+    }
+
+    /* Free mlan_adapter */
+    pcb->moal_mfree(pmadapter->pmoal_handle, (t_u8 *) pmadapter);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function downloads the firmware
+ *
+ *  @param pmlan_adapter   A pointer to a t_void pointer to store
+ *                         mlan_adapter structure pointer
+ *  @param pmfw            A pointer to firmware image
+ *
+ *  @return                MLAN_STATUS_SUCCESS
+ *                             The firmware download succeeded.
+ *                         MLAN_STATUS_FAILURE
+ *                             The firmware download failed.
+ */
+mlan_status
+mlan_dnld_fw(IN t_void * pmlan_adapter, IN pmlan_fw_image pmfw)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = (mlan_adapter *) pmlan_adapter;
+    t_u32 poll_num = 1;
+    t_u32 winner = 0;
+
+    ENTER();
+    MASSERT(pmlan_adapter);
+
+    /* Card specific probing */
+    ret = wlan_sdio_probe(pmadapter);
+    if (ret == MLAN_STATUS_FAILURE) {
+        PRINTM(MERROR, "WLAN SDIO probe failed\n", ret);
+        LEAVE();
+        return ret;
+    }
+
+    /* Check if firmware is already running */
+    ret = wlan_check_fw_status(pmadapter, poll_num);
+    if (ret == MLAN_STATUS_SUCCESS) {
+        PRINTM(MMSG, "WLAN FW already running! Skip FW download\n");
+        goto done;
+    }
+    poll_num = MAX_FIRMWARE_POLL_TRIES;
+
+    /* Check if other interface is downloading */
+    ret = wlan_check_winner_status(pmadapter, &winner);
+    if (ret == MLAN_STATUS_FAILURE) {
+        PRINTM(MFATAL, "WLAN read winner status failed!\n");
+        goto done;
+    }
+    if (winner) {
+        PRINTM(MMSG, "WLAN is not the winner (0x%x). Skip FW download\n",
+               winner);
+        poll_num = MAX_MULTI_INTERFACE_POLL_TRIES;
+        goto poll_fw;
+    }
+
+    if (pmfw) {
+        /* Download helper/firmware */
+        ret = wlan_dnld_fw(pmadapter, pmfw);
+        if (ret != MLAN_STATUS_SUCCESS) {
+            PRINTM(MERROR, "wlan_dnld_fw fail ret=0x%x\n", ret);
+            LEAVE();
+            return ret;
+        }
+    }
+
+  poll_fw:
+    /* Check if the firmware is downloaded successfully or not */
+    ret = wlan_check_fw_status(pmadapter, poll_num);
+    if (ret != MLAN_STATUS_SUCCESS) {
+        PRINTM(MFATAL, "FW failed to be active in time!\n");
+        ret = MLAN_STATUS_FAILURE;
+        LEAVE();
+        return ret;
+    }
+  done:
+
+    /* re-enable host interrupt for mlan after fw dnld is successful */
+    wlan_enable_host_int(pmadapter);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function pass init param to MLAN
+ *
+ *  @param pmlan_adapter  A pointer to a t_void pointer to store
+ *                        mlan_adapter structure pointer
+ *  @param pparam         A pointer to mlan_init_param structure
+ *
+ *  @return               MLAN_STATUS_SUCCESS
+ *
+ */
+mlan_status
+mlan_set_init_param(IN t_void * pmlan_adapter, IN pmlan_init_param pparam)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = (mlan_adapter *) pmlan_adapter;
+
+    ENTER();
+    MASSERT(pmlan_adapter);
+
+    /** Save cal data in MLAN */
+    if ((pparam->pcal_data_buf) && (pparam->cal_data_len > 0)) {
+        pmadapter->pcal_data = pparam->pcal_data_buf;
+        pmadapter->cal_data_len = pparam->cal_data_len;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function initializes the firmware
+ *
+ *  @param pmlan_adapter   A pointer to a t_void pointer to store
+ *                         mlan_adapter structure pointer
+ *
+ *  @return                MLAN_STATUS_SUCCESS
+ *                             The firmware initialization succeeded.
+ *                         MLAN_STATUS_PENDING
+ *                             The firmware initialization is pending.
+ *                         MLAN_STATUS_FAILURE
+ *                             The firmware initialization failed.
+ */
+mlan_status
+mlan_init_fw(IN t_void * pmlan_adapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = (mlan_adapter *) pmlan_adapter;
+
+    ENTER();
+    MASSERT(pmlan_adapter);
+
+    pmadapter->hw_status = WlanHardwareStatusInitializing;
+
+    /* Initialize firmware, may return PENDING */
+    ret = wlan_init_fw(pmadapter);
+    PRINTM(MINFO, "wlan_init_fw returned ret=0x%x\n", ret);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Shutdown firmware
+ *
+ *  @param pmlan_adapter	A pointer to mlan_adapter structure
+ *
+ *  @return			MLAN_STATUS_SUCCESS
+ *                      		The firmware shutdown call succeeded.
+ *				MLAN_STATUS_PENDING
+ *      	                	The firmware shutdown call is pending.
+ *				MLAN_STATUS_FAILURE
+ *      	                	The firmware shutdown call failed.
+ */
+mlan_status
+mlan_shutdown_fw(IN t_void * pmlan_adapter)
+{
+    mlan_status ret = MLAN_STATUS_PENDING;
+    mlan_adapter *pmadapter = (mlan_adapter *) pmlan_adapter;
+    pmlan_callbacks pcb;
+    t_s32 i = 0;
+
+    ENTER();
+    MASSERT(pmlan_adapter);
+    /* mlan already shutdown */
+    if (pmadapter->hw_status == WlanHardwareStatusNotReady) {
+        LEAVE();
+        return MLAN_STATUS_SUCCESS;
+    }
+
+    pmadapter->hw_status = WlanHardwareStatusClosing;
+    /* wait for mlan_process to complete */
+    if (pmadapter->mlan_processing) {
+        PRINTM(MWARN, "mlan main processing is still running\n");
+        LEAVE();
+        return ret;
+    }
+
+    /* shut down mlan */
+    PRINTM(MINFO, "Shutdown mlan...\n");
+
+    /* Clean up priv structures */
+    for (i = 0; i < pmadapter->priv_num; i++) {
+        if (pmadapter->priv[i]) {
+            wlan_free_priv(pmadapter->priv[i]);
+        }
+    }
+
+    pcb = &pmadapter->callbacks;
+
+    if (pcb->moal_spin_lock(pmadapter->pmoal_handle, pmadapter->pmlan_lock)
+        != MLAN_STATUS_SUCCESS) {
+        ret = MLAN_STATUS_FAILURE;
+        goto exit_shutdown_fw;
+    }
+
+    if (pcb->moal_spin_unlock(pmadapter->pmoal_handle, pmadapter->pmlan_lock)
+        != MLAN_STATUS_SUCCESS) {
+        ret = MLAN_STATUS_FAILURE;
+        goto exit_shutdown_fw;
+    }
+
+    /* Notify completion */
+    ret = wlan_shutdown_fw_complete(pmadapter);
+
+  exit_shutdown_fw:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief The main process
+ *
+ *  @param pmlan_adapter	A pointer to mlan_adapter structure
+ *
+ *  @return			MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+mlan_main_process(IN t_void * pmlan_adapter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_adapter *pmadapter = (mlan_adapter *) pmlan_adapter;
+    pmlan_callbacks pcb;
+
+    ENTER();
+
+    MASSERT(pmlan_adapter);
+
+    pcb = &pmadapter->callbacks;
+
+    pcb->moal_spin_lock(pmadapter->pmoal_handle, pmadapter->pmain_proc_lock);
+
+    /* Check if already processing */
+    if (pmadapter->mlan_processing) {
+        pcb->moal_spin_unlock(pmadapter->pmoal_handle,
+                              pmadapter->pmain_proc_lock);
+        goto exit_main_proc;
+    } else {
+        pmadapter->mlan_processing = MTRUE;
+        pcb->moal_spin_unlock(pmadapter->pmoal_handle,
+                              pmadapter->pmain_proc_lock);
+    }
+  process_start:
+    do {
+        /* Is MLAN shutting down or not ready? */
+        if ((pmadapter->hw_status == WlanHardwareStatusClosing) ||
+            (pmadapter->hw_status == WlanHardwareStatusNotReady))
+            break;
+
+        /* Handle pending SDIO interrupts if any */
+        if (pmadapter->sdio_ireg) {
+            if (pmadapter->hs_activated == MTRUE)
+                wlan_process_hs_config(pmadapter);
+            wlan_process_int_status(pmadapter);
+        }
+
+        /* Need to wake up the card ? */
+        if ((pmadapter->ps_state == PS_STATE_SLEEP) &&
+            (pmadapter->pm_wakeup_card_req &&
+             !pmadapter->pm_wakeup_fw_try) &&
+            (util_peek_list
+             (pmadapter->pmoal_handle, &pmadapter->cmd_pending_q,
+              pcb->moal_spin_lock, pcb->moal_spin_unlock)
+             || !wlan_bypass_tx_list_empty(pmadapter)
+             || !wlan_wmm_lists_empty(pmadapter)
+            )) {
+            pmadapter->pm_wakeup_fw_try = MTRUE;
+            wlan_pm_wakeup_card(pmadapter);
+            continue;
+        }
+        if (IS_CARD_RX_RCVD(pmadapter)) {
+            pmadapter->data_received = MFALSE;
+            if (pmadapter->hs_activated == MTRUE) {
+                pmadapter->is_hs_configured = MFALSE;
+                wlan_host_sleep_activated_event(wlan_get_priv
+                                                (pmadapter, MLAN_BSS_ROLE_ANY),
+                                                MFALSE);
+            }
+            pmadapter->pm_wakeup_fw_try = MFALSE;
+            if (pmadapter->ps_state == PS_STATE_SLEEP)
+                pmadapter->ps_state = PS_STATE_AWAKE;
+        } else {
+            /* We have tried to wakeup the card already */
+            if (pmadapter->pm_wakeup_fw_try)
+                break;
+            if (pmadapter->ps_state != PS_STATE_AWAKE ||
+                (pmadapter->tx_lock_flag == MTRUE))
+                break;
+
+            if (pmadapter->scan_processing || pmadapter->data_sent
+                || (wlan_bypass_tx_list_empty(pmadapter) &&
+                    wlan_wmm_lists_empty(pmadapter))
+                || wlan_11h_radar_detected_tx_blocked(pmadapter)
+                ) {
+                if (pmadapter->cmd_sent || pmadapter->curr_cmd ||
+                    (!util_peek_list
+                     (pmadapter->pmoal_handle, &pmadapter->cmd_pending_q,
+                      pcb->moal_spin_lock, pcb->moal_spin_unlock))) {
+                    break;
+                }
+            }
+        }
+
+        /* Check for Cmd Resp */
+        if (pmadapter->cmd_resp_received) {
+            pmadapter->cmd_resp_received = MFALSE;
+            wlan_process_cmdresp(pmadapter);
+
+            /* call moal back when init_fw is done */
+            if (pmadapter->hw_status == WlanHardwareStatusInitdone) {
+                pmadapter->hw_status = WlanHardwareStatusReady;
+                wlan_init_fw_complete(pmadapter);
+            }
+        }
+
+        /* Check for event */
+        if (pmadapter->event_received) {
+            pmadapter->event_received = MFALSE;
+            wlan_process_event(pmadapter);
+        }
+
+        /* Check if we need to confirm Sleep Request received previously */
+        if (pmadapter->ps_state == PS_STATE_PRE_SLEEP) {
+            if (!pmadapter->cmd_sent && !pmadapter->curr_cmd) {
+                wlan_check_ps_cond(pmadapter);
+            }
+        }
+
+        /*
+         * The ps_state may have been changed during processing of
+         * Sleep Request event.
+         */
+        if ((pmadapter->ps_state == PS_STATE_SLEEP)
+            || (pmadapter->ps_state == PS_STATE_PRE_SLEEP)
+            || (pmadapter->ps_state == PS_STATE_SLEEP_CFM)
+            || (pmadapter->tx_lock_flag == MTRUE)
+            )
+            continue;
+
+        if (!pmadapter->cmd_sent && !pmadapter->curr_cmd) {
+            if (wlan_exec_next_cmd(pmadapter) == MLAN_STATUS_FAILURE) {
+                ret = MLAN_STATUS_FAILURE;
+                break;
+            }
+        }
+
+        if (!pmadapter->scan_processing && !pmadapter->data_sent &&
+            !wlan_11h_radar_detected_tx_blocked(pmadapter) &&
+            !wlan_bypass_tx_list_empty(pmadapter)) {
+            PRINTM(MINFO, "mlan_send_pkt(): deq(bybass_txq)\n");
+            wlan_process_bypass_tx(pmadapter);
+            if (pmadapter->hs_activated == MTRUE) {
+                pmadapter->is_hs_configured = MFALSE;
+                wlan_host_sleep_activated_event(wlan_get_priv
+                                                (pmadapter, MLAN_BSS_ROLE_ANY),
+                                                MFALSE);
+            }
+        }
+
+        if (!pmadapter->scan_processing && !pmadapter->data_sent &&
+            !wlan_wmm_lists_empty(pmadapter)
+            && !wlan_11h_radar_detected_tx_blocked(pmadapter)
+            ) {
+            wlan_wmm_process_tx(pmadapter);
+            if (pmadapter->hs_activated == MTRUE) {
+                pmadapter->is_hs_configured = MFALSE;
+                wlan_host_sleep_activated_event(wlan_get_priv
+                                                (pmadapter, MLAN_BSS_ROLE_ANY),
+                                                MFALSE);
+            }
+        }
+
+#ifdef STA_SUPPORT
+        if (pmadapter->delay_null_pkt && !pmadapter->cmd_sent &&
+            !pmadapter->curr_cmd && !IS_COMMAND_PENDING(pmadapter) &&
+            wlan_bypass_tx_list_empty(pmadapter) &&
+            wlan_wmm_lists_empty(pmadapter)) {
+            if (wlan_send_null_packet
+                (wlan_get_priv(pmadapter, MLAN_BSS_ROLE_STA),
+                 MRVDRV_TxPD_POWER_MGMT_NULL_PACKET |
+                 MRVDRV_TxPD_POWER_MGMT_LAST_PACKET)
+                == MLAN_STATUS_SUCCESS) {
+                pmadapter->delay_null_pkt = MFALSE;
+            }
+            break;
+        }
+#endif
+
+    } while (MTRUE);
+
+    if ((pmadapter->sdio_ireg) || IS_CARD_RX_RCVD(pmadapter)) {
+        goto process_start;
+    }
+    pcb->moal_spin_lock(pmadapter->pmoal_handle, pmadapter->pmain_proc_lock);
+    pmadapter->mlan_processing = MFALSE;
+    pcb->moal_spin_unlock(pmadapter->pmoal_handle, pmadapter->pmain_proc_lock);
+
+  exit_main_proc:
+    if (pmadapter->hw_status == WlanHardwareStatusClosing)
+        mlan_shutdown_fw(pmadapter);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Function to send packet
+ *
+ *  @param pmlan_adapter	A pointer to mlan_adapter structure
+ *  @param pmbuf		A pointer to mlan_buffer structure
+ *
+ *  @return			MLAN_STATUS_PENDING
+ */
+mlan_status
+mlan_send_packet(IN t_void * pmlan_adapter, IN pmlan_buffer pmbuf)
+{
+    mlan_status ret = MLAN_STATUS_PENDING;
+    mlan_adapter *pmadapter = (mlan_adapter *) pmlan_adapter;
+
+    ENTER();
+    MASSERT(pmlan_adapter && pmbuf);
+
+    MASSERT(pmbuf->bss_index < pmadapter->priv_num);
+    pmbuf->flags = MLAN_BUF_FLAG_MOAL_TX_BUF;
+
+    if (((pmadapter->priv[pmbuf->bss_index]->port_ctrl_mode == MTRUE) &&
+         ((mlan_ntohs(*(t_u16 *) & pmbuf->pbuf[pmbuf->data_offset +
+                                               MLAN_ETHER_PKT_TYPE_OFFSET]) ==
+           MLAN_ETHER_PKT_TYPE_EAPOL)
+          ||
+          (mlan_ntohs
+           (*(t_u16 *) & pmbuf->
+            pbuf[pmbuf->data_offset + MLAN_ETHER_PKT_TYPE_OFFSET]) ==
+           MLAN_ETHER_PKT_TYPE_WAPI)
+         ))
+        || (pmbuf->buf_type == MLAN_BUF_TYPE_RAW_DATA)
+
+        ) {
+        PRINTM(MINFO, "mlan_send_pkt(): enq(bybass_txq)\n");
+        wlan_add_buf_bypass_txqueue(pmadapter, pmbuf);
+    } else {
+        /* Transmit the packet */
+        wlan_wmm_add_buf_txqueue(pmadapter, pmbuf);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief MLAN ioctl handler
+ *
+ *  @param adapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+mlan_ioctl(IN t_void * adapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_adapter pmadapter = (pmlan_adapter) adapter;
+    pmlan_private pmpriv = MNULL;
+
+    ENTER();
+
+    if (pioctl_req == MNULL) {
+        PRINTM(MERROR, "MLAN IOCTL information buffer is NULL\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto exit;
+    }
+    if (pioctl_req->action == MLAN_ACT_CANCEL) {
+        wlan_cancel_pending_ioctl(pmadapter, pioctl_req);
+        ret = MLAN_STATUS_SUCCESS;
+        goto exit;
+    }
+    pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    ret = pmpriv->ops.ioctl(adapter, pioctl_req);
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Packet receive completion callback handler
+ *
+ *  @param pmlan_adapter	A pointer to mlan_adapter structure
+ *  @param pmbuf		A pointer to mlan_buffer structure
+ *  @param status		Callback status
+ *
+ *  @return			MLAN_STATUS_SUCCESS
+ */
+mlan_status
+mlan_recv_packet_complete(IN t_void * pmlan_adapter,
+                          IN pmlan_buffer pmbuf, IN mlan_status status)
+{
+    mlan_adapter *pmadapter = (mlan_adapter *) pmlan_adapter;
+
+    ENTER();
+    wlan_recv_packet_complete(pmadapter, pmbuf, status);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief select wmm queue
+ *
+ *  @param pmlan_adapter	A pointer to mlan_adapter structure
+ *  @param bss_num		BSS number
+ *  @param tid			TID
+ *
+ *  @return			wmm queue priority (0 - 3)
+ */
+t_u8
+mlan_select_wmm_queue(IN t_void * pmlan_adapter, IN t_u8 bss_num, IN t_u8 tid)
+{
+    mlan_adapter *pmadapter = (mlan_adapter *) pmlan_adapter;
+    pmlan_private pmpriv = pmadapter->priv[bss_num];
+    t_u8 ret;
+    ENTER();
+    ret = wlan_wmm_select_queue(pmpriv, tid);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function gets interrupt status.
+ *
+ *  @param adapter  A pointer to mlan_adapter structure
+ *  @return         N/A
+ */
+t_void
+mlan_interrupt(IN t_void * adapter)
+{
+    mlan_adapter *pmadapter = (mlan_adapter *) adapter;
+
+    ENTER();
+    if (!pmadapter->pps_uapsd_mode && pmadapter->ps_state == PS_STATE_SLEEP) {
+        pmadapter->pm_wakeup_fw_try = MFALSE;
+        pmadapter->ps_state = PS_STATE_AWAKE;
+    }
+    wlan_interrupt(pmadapter);
+    LEAVE();
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_sta_cmd.c b/drivers/net/wireless/sd8797/mlan/mlan_sta_cmd.c
new file mode 100644
index 000000000000..7c426595117e
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_sta_cmd.c
@@ -0,0 +1,1996 @@
+/** @file mlan_sta_cmd.c
+ *
+ *  @brief This file contains the handling of command.
+ *  it prepares command and sends it to firmware when
+ *  it is ready.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ *
+ */
+
+/******************************************************
+Change log:
+    10/21/2008: initial version
+******************************************************/
+
+#include "mlan.h"
+#include "mlan_join.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_wmm.h"
+#include "mlan_11n.h"
+#include "mlan_11h.h"
+#include "mlan_sdio.h"
+#include "mlan_meas.h"
+
+/********************************************************
+                Local Variables
+********************************************************/
+
+/********************************************************
+                Global Variables
+********************************************************/
+
+/********************************************************
+                Local Functions
+********************************************************/
+
+/**
+ *  @brief This function prepares command of RSSI info.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pcmd         A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   Command action
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_802_11_rssi_info(IN pmlan_private pmpriv,
+                          IN HostCmd_DS_COMMAND * pcmd, IN t_u16 cmd_action)
+{
+    ENTER();
+
+    pcmd->command = wlan_cpu_to_le16(HostCmd_CMD_RSSI_INFO);
+    pcmd->size =
+        wlan_cpu_to_le16(sizeof(HostCmd_DS_802_11_RSSI_INFO) + S_DS_GEN);
+    pcmd->params.rssi_info.action = wlan_cpu_to_le16(cmd_action);
+    pcmd->params.rssi_info.ndata = wlan_cpu_to_le16(pmpriv->data_avg_factor);
+    pcmd->params.rssi_info.nbcn = wlan_cpu_to_le16(pmpriv->bcn_avg_factor);
+
+    /* Reset SNR/NF/RSSI values in private structure */
+    pmpriv->data_rssi_last = 0;
+    pmpriv->data_nf_last = 0;
+    pmpriv->data_rssi_avg = 0;
+    pmpriv->data_nf_avg = 0;
+    pmpriv->bcn_rssi_last = 0;
+    pmpriv->bcn_nf_last = 0;
+    pmpriv->bcn_rssi_avg = 0;
+    pmpriv->bcn_nf_avg = 0;
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of mac_control.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pcmd         A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   Command action
+ *  @param pdata_buf    A pointer to command information buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_cmd_mac_control(IN pmlan_private pmpriv,
+                     IN HostCmd_DS_COMMAND * pcmd,
+                     IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    HostCmd_DS_MAC_CONTROL *pmac = &pcmd->params.mac_ctrl;
+    t_u16 action = *((t_u16 *) pdata_buf);
+
+    ENTER();
+
+    if (cmd_action != HostCmd_ACT_GEN_SET) {
+        PRINTM(MERROR, "wlan_cmd_mac_control(): support SET only.\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    pcmd->command = wlan_cpu_to_le16(HostCmd_CMD_MAC_CONTROL);
+    pcmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_MAC_CONTROL) + S_DS_GEN);
+    pmac->action = wlan_cpu_to_le16(action);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of snmp_mib.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param cmd_oid      OID: ENABLE or DISABLE
+ *  @param pdata_buf    A pointer to command information buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_802_11_snmp_mib(IN pmlan_private pmpriv,
+                         IN HostCmd_DS_COMMAND * cmd,
+                         IN t_u16 cmd_action,
+                         IN t_u32 cmd_oid, IN t_void * pdata_buf)
+{
+    HostCmd_DS_802_11_SNMP_MIB *psnmp_mib = &cmd->params.smib;
+    t_u32 ul_temp;
+
+    ENTER();
+    PRINTM(MINFO, "SNMP_CMD: cmd_oid = 0x%x\n", cmd_oid);
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_SNMP_MIB);
+    cmd->size = sizeof(HostCmd_DS_802_11_SNMP_MIB) - 1 + S_DS_GEN;
+
+    if (cmd_action == HostCmd_ACT_GEN_GET) {
+        psnmp_mib->query_type = wlan_cpu_to_le16(HostCmd_ACT_GEN_GET);
+        psnmp_mib->buf_size = wlan_cpu_to_le16(MAX_SNMP_BUF_SIZE);
+        cmd->size += MAX_SNMP_BUF_SIZE;
+    }
+
+    switch (cmd_oid) {
+    case DtimPeriod_i:
+        psnmp_mib->oid = wlan_cpu_to_le16((t_u16) DtimPeriod_i);
+        if (cmd_action == HostCmd_ACT_GEN_SET) {
+            psnmp_mib->query_type = wlan_cpu_to_le16(HostCmd_ACT_GEN_SET);
+            psnmp_mib->buf_size = wlan_cpu_to_le16(sizeof(t_u8));
+            ul_temp = *((t_u32 *) pdata_buf);
+            psnmp_mib->value[0] = (t_u8) ul_temp;
+            cmd->size += sizeof(t_u8);
+        }
+        break;
+    case FragThresh_i:
+        psnmp_mib->oid = wlan_cpu_to_le16((t_u16) FragThresh_i);
+        if (cmd_action == HostCmd_ACT_GEN_SET) {
+            psnmp_mib->query_type = wlan_cpu_to_le16(HostCmd_ACT_GEN_SET);
+            psnmp_mib->buf_size = wlan_cpu_to_le16(sizeof(t_u16));
+            ul_temp = *((t_u32 *) pdata_buf);
+            *((t_u16 *) (psnmp_mib->value)) = wlan_cpu_to_le16((t_u16) ul_temp);
+            cmd->size += sizeof(t_u16);
+        }
+        break;
+    case RtsThresh_i:
+        psnmp_mib->oid = wlan_cpu_to_le16((t_u16) RtsThresh_i);
+        if (cmd_action == HostCmd_ACT_GEN_SET) {
+            psnmp_mib->query_type = wlan_cpu_to_le16(HostCmd_ACT_GEN_SET);
+            psnmp_mib->buf_size = wlan_cpu_to_le16(sizeof(t_u16));
+            ul_temp = *((t_u32 *) pdata_buf);
+            *(t_u16 *) (psnmp_mib->value) = wlan_cpu_to_le16((t_u16) ul_temp);
+            cmd->size += sizeof(t_u16);
+        }
+        break;
+
+    case ShortRetryLim_i:
+        psnmp_mib->oid = wlan_cpu_to_le16((t_u16) ShortRetryLim_i);
+        if (cmd_action == HostCmd_ACT_GEN_SET) {
+            psnmp_mib->query_type = wlan_cpu_to_le16(HostCmd_ACT_GEN_SET);
+            psnmp_mib->buf_size = wlan_cpu_to_le16(sizeof(t_u16));
+            ul_temp = (*(t_u32 *) pdata_buf);
+            *((t_u16 *) (psnmp_mib->value)) = wlan_cpu_to_le16((t_u16) ul_temp);
+            cmd->size += sizeof(t_u16);
+        }
+        break;
+    case Dot11D_i:
+        psnmp_mib->oid = wlan_cpu_to_le16((t_u16) Dot11D_i);
+        if (cmd_action == HostCmd_ACT_GEN_SET) {
+            psnmp_mib->query_type = wlan_cpu_to_le16(HostCmd_ACT_GEN_SET);
+            psnmp_mib->buf_size = wlan_cpu_to_le16(sizeof(t_u16));
+            ul_temp = *(t_u32 *) pdata_buf;
+            *((t_u16 *) (psnmp_mib->value)) = wlan_cpu_to_le16((t_u16) ul_temp);
+            cmd->size += sizeof(t_u16);
+        }
+        break;
+    case Dot11H_i:
+        psnmp_mib->oid = wlan_cpu_to_le16((t_u16) Dot11H_i);
+        if (cmd_action == HostCmd_ACT_GEN_SET) {
+            psnmp_mib->query_type = wlan_cpu_to_le16(HostCmd_ACT_GEN_SET);
+            psnmp_mib->buf_size = wlan_cpu_to_le16(sizeof(t_u16));
+            ul_temp = *(t_u32 *) pdata_buf;
+            *((t_u16 *) (psnmp_mib->value)) = wlan_cpu_to_le16((t_u16) ul_temp);
+            cmd->size += sizeof(t_u16);
+        }
+        break;
+    case WwsMode_i:
+        psnmp_mib->oid = wlan_cpu_to_le16((t_u16) WwsMode_i);
+        if (cmd_action == HostCmd_ACT_GEN_SET) {
+            psnmp_mib->query_type = wlan_cpu_to_le16(HostCmd_ACT_GEN_SET);
+            psnmp_mib->buf_size = wlan_cpu_to_le16(sizeof(t_u16));
+            ul_temp = *((t_u32 *) pdata_buf);
+            *((t_u16 *) (psnmp_mib->value)) = wlan_cpu_to_le16((t_u16) ul_temp);
+            cmd->size += sizeof(t_u16);
+        }
+        break;
+    case Thermal_i:
+        psnmp_mib->oid = wlan_cpu_to_le16((t_u16) Thermal_i);
+        break;
+    default:
+        break;
+    }
+    cmd->size = wlan_cpu_to_le16(cmd->size);
+    PRINTM(MINFO, "SNMP_CMD: Action=0x%x, OID=0x%x, OIDSize=0x%x, Value=0x%x\n",
+           cmd_action, cmd_oid, wlan_le16_to_cpu(psnmp_mib->buf_size),
+           wlan_le16_to_cpu(*(t_u16 *) psnmp_mib->value));
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of get_log.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_802_11_get_log(IN pmlan_private pmpriv, IN HostCmd_DS_COMMAND * cmd)
+{
+    ENTER();
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_GET_LOG);
+    cmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_802_11_GET_LOG) + S_DS_GEN);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of tx_power_cfg.
+ *
+ *  @param pmpriv      A pointer to mlan_private structure
+ *  @param cmd         A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action  The action: GET or SET
+ *  @param pdata_buf   A pointer to data buffer
+ *
+ *  @return            MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_tx_power_cfg(IN pmlan_private pmpriv,
+                      IN HostCmd_DS_COMMAND * cmd,
+                      IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    MrvlTypes_Power_Group_t *ppg_tlv = MNULL;
+    HostCmd_DS_TXPWR_CFG *ptxp = MNULL;
+    HostCmd_DS_TXPWR_CFG *ptxp_cfg = &cmd->params.txp_cfg;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_TXPWR_CFG);
+    cmd->size = wlan_cpu_to_le16(S_DS_GEN + sizeof(HostCmd_DS_TXPWR_CFG));
+    switch (cmd_action) {
+    case HostCmd_ACT_GEN_SET:
+        ptxp = (HostCmd_DS_TXPWR_CFG *) pdata_buf;
+        if (ptxp->mode) {
+            ppg_tlv =
+                (MrvlTypes_Power_Group_t *) ((t_u8 *) pdata_buf +
+                                             sizeof(HostCmd_DS_TXPWR_CFG));
+            memmove(pmpriv->adapter, ptxp_cfg, pdata_buf,
+                    sizeof(HostCmd_DS_TXPWR_CFG) +
+                    sizeof(MrvlTypes_Power_Group_t) + ppg_tlv->length);
+
+            ppg_tlv = (MrvlTypes_Power_Group_t *) ((t_u8 *) ptxp_cfg +
+                                                   sizeof
+                                                   (HostCmd_DS_TXPWR_CFG));
+            cmd->size +=
+                wlan_cpu_to_le16(sizeof(MrvlTypes_Power_Group_t) +
+                                 ppg_tlv->length);
+            ppg_tlv->type = wlan_cpu_to_le16(ppg_tlv->type);
+            ppg_tlv->length = wlan_cpu_to_le16(ppg_tlv->length);
+        } else {
+            memmove(pmpriv->adapter, ptxp_cfg, pdata_buf,
+                    sizeof(HostCmd_DS_TXPWR_CFG));
+        }
+        ptxp_cfg->action = wlan_cpu_to_le16(cmd_action);
+        ptxp_cfg->cfg_index = wlan_cpu_to_le16(ptxp_cfg->cfg_index);
+        ptxp_cfg->mode = wlan_cpu_to_le32(ptxp_cfg->mode);
+        break;
+    case HostCmd_ACT_GEN_GET:
+        ptxp_cfg->action = wlan_cpu_to_le16(cmd_action);
+        break;
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of rf_tx_power.
+ *
+ *  @param pmpriv     A pointer to wlan_private structure
+ *  @param cmd        A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action the action: GET or SET
+ *  @param pdata_buf  A pointer to data buffer
+ *  @return           MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_802_11_rf_tx_power(IN pmlan_private pmpriv,
+                            IN HostCmd_DS_COMMAND * cmd,
+                            IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+
+    HostCmd_DS_802_11_RF_TX_POWER *prtp = &cmd->params.txp;
+
+    ENTER();
+
+    cmd->size = wlan_cpu_to_le16((sizeof(HostCmd_DS_802_11_RF_TX_POWER))
+                                 + S_DS_GEN);
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_RF_TX_POWER);
+    prtp->action = cmd_action;
+
+    PRINTM(MINFO, "RF_TX_POWER_CMD: Size:%d Cmd:0x%x Act:%d\n",
+           cmd->size, cmd->command, prtp->action);
+
+    switch (cmd_action) {
+    case HostCmd_ACT_GEN_GET:
+        prtp->action = wlan_cpu_to_le16(HostCmd_ACT_GEN_GET);
+        prtp->current_level = 0;
+        break;
+
+    case HostCmd_ACT_GEN_SET:
+        prtp->action = wlan_cpu_to_le16(HostCmd_ACT_GEN_SET);
+        prtp->current_level = wlan_cpu_to_le16(*((t_u16 *) pdata_buf));
+        break;
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ * @brief This function prepares command of hs_cfg.
+ *
+ * @param pmpriv       A pointer to mlan_private structure
+ * @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ * @param cmd_action   The action: GET or SET
+ * @param pdata_buf    A pointer to data buffer
+ *
+ * @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_802_11_hs_cfg(IN pmlan_private pmpriv,
+                       IN HostCmd_DS_COMMAND * cmd,
+                       IN t_u16 cmd_action, IN hs_config_param * pdata_buf)
+{
+    pmlan_adapter pmadapter = pmpriv->adapter;
+    HostCmd_DS_802_11_HS_CFG_ENH *phs_cfg = &cmd->params.opt_hs_cfg;
+    t_u16 hs_activate = MFALSE;
+
+    ENTER();
+
+    if (pdata_buf == MNULL) {
+        /* New Activate command */
+        hs_activate = MTRUE;
+    }
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_HS_CFG_ENH);
+
+    if (!hs_activate && (pdata_buf->conditions != HOST_SLEEP_CFG_CANCEL)
+        && ((pmadapter->arp_filter_size > 0)
+            && (pmadapter->arp_filter_size <= ARP_FILTER_MAX_BUF_SIZE))) {
+        PRINTM(MINFO, "Attach %d bytes ArpFilter to HSCfg cmd\n",
+               pmadapter->arp_filter_size);
+        memcpy(pmpriv->adapter,
+               ((t_u8 *) phs_cfg) + sizeof(HostCmd_DS_802_11_HS_CFG_ENH),
+               pmadapter->arp_filter, pmadapter->arp_filter_size);
+        cmd->size =
+            (t_u16) wlan_cpu_to_le16(pmadapter->arp_filter_size +
+                                     sizeof(HostCmd_DS_802_11_HS_CFG_ENH) +
+                                     S_DS_GEN);
+    } else
+        cmd->size =
+            wlan_cpu_to_le16(S_DS_GEN + sizeof(HostCmd_DS_802_11_HS_CFG_ENH));
+
+    if (hs_activate) {
+        phs_cfg->action = wlan_cpu_to_le16(HS_ACTIVATE);
+        phs_cfg->params.hs_activate.resp_ctrl = wlan_cpu_to_le16(RESP_NEEDED);
+    } else {
+        phs_cfg->action = wlan_cpu_to_le16(HS_CONFIGURE);
+        phs_cfg->params.hs_config.conditions =
+            wlan_cpu_to_le32(pdata_buf->conditions);
+        phs_cfg->params.hs_config.gpio = pdata_buf->gpio;
+        phs_cfg->params.hs_config.gap = pdata_buf->gap;
+        PRINTM(MCMND, "HS_CFG_CMD: condition:0x%x gpio:0x%x gap:0x%x\n",
+               phs_cfg->params.hs_config.conditions,
+               phs_cfg->params.hs_config.gpio, phs_cfg->params.hs_config.gap);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of mac_address.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_802_11_mac_address(IN pmlan_private pmpriv,
+                            IN HostCmd_DS_COMMAND * cmd, IN t_u16 cmd_action)
+{
+    ENTER();
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_MAC_ADDRESS);
+    cmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_802_11_MAC_ADDRESS) +
+                                 S_DS_GEN);
+    cmd->result = 0;
+
+    cmd->params.mac_addr.action = wlan_cpu_to_le16(cmd_action);
+
+    if (cmd_action == HostCmd_ACT_GEN_SET) {
+        memcpy(pmpriv->adapter, cmd->params.mac_addr.mac_addr,
+               pmpriv->curr_addr, MLAN_MAC_ADDR_LENGTH);
+        // HEXDUMP("SET_CMD: MAC ADDRESS-", priv->CurrentAddr, 6);
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ * @brief This function prepares command of sleep_period.
+ *
+ * @param pmpriv       A pointer to mlan_private structure
+ * @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ * @param cmd_action   The action: GET or SET
+ * @param pdata_buf    A pointer to data buffer
+ *
+ * @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_802_11_sleep_period(IN pmlan_private pmpriv,
+                             IN HostCmd_DS_COMMAND * cmd,
+                             IN t_u16 cmd_action, IN t_u16 * pdata_buf)
+{
+    HostCmd_DS_802_11_SLEEP_PERIOD *pcmd_sleep_pd = &cmd->params.sleep_pd;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_SLEEP_PERIOD);
+    cmd->size =
+        wlan_cpu_to_le16(sizeof(HostCmd_DS_802_11_SLEEP_PERIOD) + S_DS_GEN);
+    if (cmd_action == HostCmd_ACT_GEN_SET) {
+        pcmd_sleep_pd->sleep_pd = wlan_cpu_to_le16(*(t_u16 *) pdata_buf);
+    }
+    pcmd_sleep_pd->action = wlan_cpu_to_le16(cmd_action);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ * @brief This function prepares command of sleep_params.
+ *
+ * @param pmpriv       A pointer to mlan_private structure
+ * @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ * @param cmd_action   The action: GET or SET
+ * @param pdata_buf    A pointer to data buffer
+ *
+ * @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_802_11_sleep_params(IN pmlan_private pmpriv,
+                             IN HostCmd_DS_COMMAND * cmd,
+                             IN t_u16 cmd_action, IN t_u16 * pdata_buf)
+{
+    HostCmd_DS_802_11_SLEEP_PARAMS *pcmd_sp = &cmd->params.sleep_param;
+    mlan_ds_sleep_params *psp = (mlan_ds_sleep_params *) pdata_buf;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_SLEEP_PARAMS);
+    cmd->size =
+        wlan_cpu_to_le16(sizeof(HostCmd_DS_802_11_SLEEP_PARAMS) + S_DS_GEN);
+    if (cmd_action == HostCmd_ACT_GEN_SET) {
+        pcmd_sp->reserved = (t_u16) psp->reserved;
+        pcmd_sp->error = (t_u16) psp->error;
+        pcmd_sp->offset = (t_u16) psp->offset;
+        pcmd_sp->stable_time = (t_u16) psp->stable_time;
+        pcmd_sp->cal_control = (t_u8) psp->cal_control;
+        pcmd_sp->external_sleep_clk = (t_u8) psp->ext_sleep_clk;
+
+        pcmd_sp->reserved = wlan_cpu_to_le16(pcmd_sp->reserved);
+        pcmd_sp->error = wlan_cpu_to_le16(pcmd_sp->error);
+        pcmd_sp->offset = wlan_cpu_to_le16(pcmd_sp->offset);
+        pcmd_sp->stable_time = wlan_cpu_to_le16(pcmd_sp->stable_time);
+    }
+    pcmd_sp->action = wlan_cpu_to_le16(cmd_action);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of mac_multicast_adr.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_mac_multicast_adr(IN pmlan_private pmpriv,
+                           IN HostCmd_DS_COMMAND * cmd,
+                           IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    mlan_multicast_list *pmcast_list = (mlan_multicast_list *) pdata_buf;
+    HostCmd_DS_MAC_MULTICAST_ADR *pmc_addr = &cmd->params.mc_addr;
+
+    ENTER();
+    cmd->size =
+        wlan_cpu_to_le16(sizeof(HostCmd_DS_MAC_MULTICAST_ADR) + S_DS_GEN);
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_MAC_MULTICAST_ADR);
+
+    pmc_addr->action = wlan_cpu_to_le16(cmd_action);
+    pmc_addr->num_of_adrs =
+        wlan_cpu_to_le16((t_u16) pmcast_list->num_multicast_addr);
+    memcpy(pmpriv->adapter, pmc_addr->mac_list, pmcast_list->mac_list,
+           pmcast_list->num_multicast_addr * MLAN_MAC_ADDR_LENGTH);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of deauthenticate.
+ *
+ * @param pmpriv       A pointer to mlan_private structure
+ * @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ * @param pdata_buf    A pointer to data buffer
+ *
+ * @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_802_11_deauthenticate(IN pmlan_private pmpriv,
+                               IN HostCmd_DS_COMMAND * cmd,
+                               IN t_void * pdata_buf)
+{
+    HostCmd_DS_802_11_DEAUTHENTICATE *pdeauth = &cmd->params.deauth;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_DEAUTHENTICATE);
+    cmd->size =
+        wlan_cpu_to_le16(sizeof(HostCmd_DS_802_11_DEAUTHENTICATE) + S_DS_GEN);
+
+    /* Set AP MAC address */
+    memcpy(pmpriv->adapter, pdeauth->mac_addr, (t_u8 *) pdata_buf,
+           MLAN_MAC_ADDR_LENGTH);
+
+    PRINTM(MCMND, "Deauth: %02x:%02x:%02x:%02x:%02x:%02x\n",
+           pdeauth->mac_addr[0], pdeauth->mac_addr[1], pdeauth->mac_addr[2],
+           pdeauth->mac_addr[3], pdeauth->mac_addr[4], pdeauth->mac_addr[5]);
+
+    if (pmpriv->adapter->state_11h.recvd_chanswann_event) {
+/** Reason code 36 = Requested from peer station as it is leaving the BSS */
+#define REASON_CODE_PEER_STA_LEAVING  36
+        pdeauth->reason_code = wlan_cpu_to_le16(REASON_CODE_PEER_STA_LEAVING);
+    } else {
+/** Reason code 3 = Station is leaving */
+#define REASON_CODE_STA_LEAVING 3
+        pdeauth->reason_code = wlan_cpu_to_le16(REASON_CODE_STA_LEAVING);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of ad_hoc_stop.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_802_11_ad_hoc_stop(IN pmlan_private pmpriv,
+                            IN HostCmd_DS_COMMAND * cmd)
+{
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_AD_HOC_STOP);
+    cmd->size = wlan_cpu_to_le16(S_DS_GEN);
+
+    if (wlan_11h_is_active(pmpriv))
+        wlan_11h_activate(pmpriv, MNULL, MFALSE);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/** Length of WEP 40 bit key */
+#define WEP_40_BIT_LEN  5
+/** Length of WEP 104 bit key */
+#define WEP_104_BIT_LEN 13
+
+/**
+ *  @brief This function sets WEP key(s) to key_param_set TLV(s).
+ *
+ *  @param priv           	A pointer to mlan_private structure
+ *  @param key_param_set    A pointer to MrvlIEtype_KeyParamSet_t structure
+ *  @param key_param_len    A pointer to the length variable
+ *
+ *  @return                 MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_set_keyparamset_wep(mlan_private * priv,
+                         MrvlIEtype_KeyParamSet_t * key_param_set,
+                         t_u16 * key_param_len)
+{
+    int cur_key_param_len = 0;
+    t_u8 i;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    /* Multi-key_param_set TLV is supported */
+    for (i = 0; i < MRVL_NUM_WEP_KEY; i++) {
+        if ((priv->wep_key[i].key_length == WEP_40_BIT_LEN) ||
+            (priv->wep_key[i].key_length == WEP_104_BIT_LEN)) {
+            key_param_set->type = wlan_cpu_to_le16(TLV_TYPE_KEY_MATERIAL);
+/** Key_param_set WEP fixed length */
+#define KEYPARAMSET_WEP_FIXED_LEN 8
+            key_param_set->length =
+                wlan_cpu_to_le16((t_u16)
+                                 (priv->wep_key[i].key_length +
+                                  KEYPARAMSET_WEP_FIXED_LEN));
+            key_param_set->key_type_id = wlan_cpu_to_le16(KEY_TYPE_ID_WEP);
+            key_param_set->key_info = wlan_cpu_to_le16
+                (KEY_INFO_WEP_ENABLED | KEY_INFO_WEP_UNICAST |
+                 KEY_INFO_WEP_MCAST);
+            key_param_set->key_len =
+                (t_u16) wlan_cpu_to_le16(priv->wep_key[i].key_length);
+            /* Set WEP key index */
+            key_param_set->key[0] = i;
+            /* Set default Tx key flag */
+            if (i == (priv->wep_key_curr_index & HostCmd_WEP_KEY_INDEX_MASK))
+                key_param_set->key[1] = 1;
+            else
+                key_param_set->key[1] = 0;
+            memmove(priv->adapter, &key_param_set->key[2],
+                    priv->wep_key[i].key_material, priv->wep_key[i].key_length);
+
+            cur_key_param_len = priv->wep_key[i].key_length +
+                KEYPARAMSET_WEP_FIXED_LEN + sizeof(MrvlIEtypesHeader_t);
+            *key_param_len += (t_u16) cur_key_param_len;
+            key_param_set =
+                (MrvlIEtype_KeyParamSet_t *) ((t_u8 *) key_param_set +
+                                              cur_key_param_len);
+        } else if (!priv->wep_key[i].key_length) {
+            continue;
+        } else {
+            PRINTM(MERROR, "key%d Length = %d is incorrect\n", (i + 1),
+                   priv->wep_key[i].key_length);
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function prepares command of key_material.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param cmd_oid      OID: ENABLE or DISABLE
+ *  @param pdata_buf    A pointer to data buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_cmd_802_11_key_material(IN pmlan_private pmpriv,
+                             IN HostCmd_DS_COMMAND * cmd,
+                             IN t_u16 cmd_action,
+                             IN t_u32 cmd_oid, IN t_void * pdata_buf)
+{
+    HostCmd_DS_802_11_KEY_MATERIAL *pkey_material = &cmd->params.key_material;
+    mlan_ds_encrypt_key *pkey = (mlan_ds_encrypt_key *) pdata_buf;
+    t_u16 key_param_len = 0;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    const t_u8 bc_mac[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_KEY_MATERIAL);
+    pkey_material->action = wlan_cpu_to_le16(cmd_action);
+
+    if (cmd_action == HostCmd_ACT_GEN_GET) {
+        cmd->size =
+            wlan_cpu_to_le16(sizeof(pkey_material->action) + S_DS_GEN +
+                             KEYPARAMSET_FIXED_LEN +
+                             sizeof(MrvlIEtypesHeader_t));
+        memset(pmpriv->adapter, &pkey_material->key_param_set, 0,
+               sizeof(MrvlIEtype_KeyParamSet_t));
+        pkey_material->key_param_set.type =
+            wlan_cpu_to_le16(TLV_TYPE_KEY_MATERIAL);
+        pkey_material->key_param_set.length =
+            wlan_cpu_to_le16(KEYPARAMSET_FIXED_LEN);
+        if (pkey->key_flags & KEY_FLAG_GROUP_KEY)
+            pkey_material->key_param_set.key_info |= KEY_INFO_MCAST_KEY;
+        else
+            pkey_material->key_param_set.key_info |= KEY_INFO_UCAST_KEY;
+        pkey_material->key_param_set.key_info =
+            wlan_cpu_to_le16(pkey_material->key_param_set.key_info);
+        goto done;
+    }
+
+    if (!pkey) {
+        memset(pmpriv->adapter, &pkey_material->key_param_set, 0,
+               (MRVL_NUM_WEP_KEY * sizeof(MrvlIEtype_KeyParamSet_t)));
+        ret =
+            wlan_set_keyparamset_wep(pmpriv, &pkey_material->key_param_set,
+                                     &key_param_len);
+        cmd->size =
+            wlan_cpu_to_le16(key_param_len + sizeof(pkey_material->action) +
+                             S_DS_GEN);
+        goto done;
+    } else
+        memset(pmpriv->adapter, &pkey_material->key_param_set, 0,
+               sizeof(MrvlIEtype_KeyParamSet_t));
+    if (pkey->is_wapi_key) {
+        PRINTM(MINFO, "Set WAPI Key\n");
+        pkey_material->key_param_set.key_type_id =
+            wlan_cpu_to_le16(KEY_TYPE_ID_WAPI);
+        if (cmd_oid == KEY_INFO_ENABLED)
+            pkey_material->key_param_set.key_info =
+                wlan_cpu_to_le16(KEY_INFO_WAPI_ENABLED);
+        else
+            pkey_material->key_param_set.key_info =
+                !(wlan_cpu_to_le16(KEY_INFO_WAPI_ENABLED));
+
+        pkey_material->key_param_set.key[0] = (t_u8) pkey->key_index;
+        if (!pmpriv->sec_info.wapi_key_on)
+            pkey_material->key_param_set.key[1] = 1;
+        else
+            pkey_material->key_param_set.key[1] = 0;    /* set 0 when re-key */
+
+        if (0 != memcmp(pmpriv->adapter, pkey->mac_addr, bc_mac, sizeof(bc_mac)))       /* WAPI
+                                                                                           pairwise
+                                                                                           key:
+                                                                                           unicast
+                                                                                         */
+            pkey_material->key_param_set.key_info |=
+                wlan_cpu_to_le16(KEY_INFO_WAPI_UNICAST);
+        else {                  /* WAPI group key: multicast */
+            pkey_material->key_param_set.key_info |=
+                wlan_cpu_to_le16(KEY_INFO_WAPI_MCAST);
+            pmpriv->sec_info.wapi_key_on = MTRUE;
+        }
+
+        pkey_material->key_param_set.type =
+            wlan_cpu_to_le16(TLV_TYPE_KEY_MATERIAL);
+        pkey_material->key_param_set.key_len = wlan_cpu_to_le16(WAPI_KEY_LEN);
+        memcpy(pmpriv->adapter, &pkey_material->key_param_set.key[2],
+               pkey->key_material, pkey->key_len);
+        memcpy(pmpriv->adapter,
+               &pkey_material->key_param_set.key[2 + pkey->key_len], pkey->pn,
+               PN_SIZE);
+        pkey_material->key_param_set.length =
+            wlan_cpu_to_le16(WAPI_KEY_LEN + KEYPARAMSET_FIXED_LEN);
+
+        key_param_len =
+            (WAPI_KEY_LEN + KEYPARAMSET_FIXED_LEN) +
+            sizeof(MrvlIEtypesHeader_t);
+        cmd->size =
+            wlan_cpu_to_le16(key_param_len + sizeof(pkey_material->action) +
+                             S_DS_GEN);
+        goto done;
+    }
+    if (pkey->key_len == WPA_AES_KEY_LEN) {
+        PRINTM(MCMND, "WPA_AES\n");
+        pkey_material->key_param_set.key_type_id =
+            wlan_cpu_to_le16(KEY_TYPE_ID_AES);
+        if (cmd_oid == KEY_INFO_ENABLED)
+            pkey_material->key_param_set.key_info =
+                wlan_cpu_to_le16(KEY_INFO_AES_ENABLED);
+        else
+            pkey_material->key_param_set.key_info =
+                !(wlan_cpu_to_le16(KEY_INFO_AES_ENABLED));
+
+        if (pkey->key_index & MLAN_KEY_INDEX_UNICAST)   /* AES pairwise key:
+                                                           unicast */
+            pkey_material->key_param_set.key_info |=
+                wlan_cpu_to_le16(KEY_INFO_AES_UNICAST);
+        else                    /* AES group key: multicast */
+            pkey_material->key_param_set.key_info |=
+                wlan_cpu_to_le16(KEY_INFO_AES_MCAST);
+    } else if (pkey->key_len == WPA_TKIP_KEY_LEN) {
+        PRINTM(MCMND, "WPA_TKIP\n");
+        pkey_material->key_param_set.key_type_id =
+            wlan_cpu_to_le16(KEY_TYPE_ID_TKIP);
+        pkey_material->key_param_set.key_info =
+            wlan_cpu_to_le16(KEY_INFO_TKIP_ENABLED);
+
+        if (pkey->key_index & MLAN_KEY_INDEX_UNICAST)   /* TKIP pairwise key:
+                                                           unicast */
+            pkey_material->key_param_set.key_info |=
+                wlan_cpu_to_le16(KEY_INFO_TKIP_UNICAST);
+        else                    /* TKIP group key: multicast */
+            pkey_material->key_param_set.key_info |=
+                wlan_cpu_to_le16(KEY_INFO_TKIP_MCAST);
+    }
+
+    if (pkey_material->key_param_set.key_type_id) {
+        pkey_material->key_param_set.type =
+            wlan_cpu_to_le16(TLV_TYPE_KEY_MATERIAL);
+        pkey_material->key_param_set.key_len =
+            wlan_cpu_to_le16((t_u16) pkey->key_len);
+        memcpy(pmpriv->adapter, pkey_material->key_param_set.key,
+               pkey->key_material, pkey->key_len);
+        pkey_material->key_param_set.length =
+            wlan_cpu_to_le16((t_u16) pkey->key_len + KEYPARAMSET_FIXED_LEN);
+
+        key_param_len =
+            (t_u16) (pkey->key_len + KEYPARAMSET_FIXED_LEN) +
+            sizeof(MrvlIEtypesHeader_t);
+
+        cmd->size =
+            wlan_cpu_to_le16(key_param_len + sizeof(pkey_material->action) +
+                             S_DS_GEN);
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function prepares command of supplicant pmk
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_cmd_802_11_supplicant_pmk(IN pmlan_private pmpriv,
+                               IN HostCmd_DS_COMMAND * cmd,
+                               IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    MrvlIEtypes_PMK_t *ppmk_tlv = MNULL;
+    MrvlIEtypes_Passphrase_t *ppassphrase_tlv = MNULL;
+    MrvlIEtypes_SsIdParamSet_t *pssid_tlv = MNULL;
+    MrvlIEtypes_Bssid_t *pbssid_tlv = MNULL;
+    HostCmd_DS_802_11_SUPPLICANT_PMK *pesupplicant_psk =
+        &cmd->params.esupplicant_psk;
+    t_u8 *ptlv_buffer = (t_u8 *) pesupplicant_psk->tlv_buffer;
+    mlan_ds_passphrase *psk = (mlan_ds_passphrase *) pdata_buf;
+    t_u8 zero_mac[] = { 0, 0, 0, 0, 0, 0 };
+
+    ENTER();
+    /*
+     * Parse the rest of the buf here
+     *      1)  <ssid="valid ssid"> - This will get the passphrase, AKMP
+     *          for specified ssid, if none specified then it will get all.
+     *          Eg: iwpriv <mlanX> passphrase 0:ssid=marvell
+     *      2)  <psk="psk">:<passphrase="passphare">:<bssid="00:50:43:ef:23:f3">
+     *          <ssid="valid ssid"> - passphrase and psk cannot be provided to
+     *          the same SSID, Takes one SSID at a time, If ssid= is present
+     *          the it should contain a passphrase or psk. If no arguments are
+     *          provided then AKMP=802.1x, and passphrase should be provided
+     *          after association.
+     *          End of each parameter should be followed by a ':'(except for the
+     *          last parameter) as the delimiter. If ':' has to be used in
+     *          an SSID then a '/' should be preceded to ':' as a escape.
+     *          Eg:iwpriv <mlanX> passphrase
+     *                    "1:ssid=mrvl AP:psk=abcdefgh:bssid=00:50:43:ef:23:f3"
+     *          iwpriv <mlanX> passphrase
+     *                 "1:ssid=mrvl/: AP:psk=abcdefgd:bssid=00:50:43:ef:23:f3"
+     *          iwpriv <mlanX> passphrase "1:ssid=mrvlAP:psk=abcdefgd"
+     *      3)  <ssid="valid ssid"> - This will clear the passphrase
+     *          for specified ssid, if none specified then it will clear all.
+     *          Eg: iwpriv <mlanX> passphrase 2:ssid=marvell
+     */
+
+    /* -1 is for t_u8 TlvBuffer[1] as this should not be included */
+    cmd->size = sizeof(HostCmd_DS_802_11_SUPPLICANT_PMK) + S_DS_GEN - 1;
+    if (psk->ssid.ssid_len) {
+        pssid_tlv = (MrvlIEtypes_SsIdParamSet_t *) ptlv_buffer;
+        pssid_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_SSID);
+        pssid_tlv->header.len =
+            (t_u16) MIN(MLAN_MAX_SSID_LENGTH, psk->ssid.ssid_len);
+        memcpy(pmpriv->adapter, (char *) pssid_tlv->ssid, psk->ssid.ssid,
+               MIN(MLAN_MAX_SSID_LENGTH, psk->ssid.ssid_len));
+        ptlv_buffer += (pssid_tlv->header.len + sizeof(MrvlIEtypesHeader_t));
+        cmd->size += (pssid_tlv->header.len + sizeof(MrvlIEtypesHeader_t));
+        pssid_tlv->header.len = wlan_cpu_to_le16(pssid_tlv->header.len);
+    }
+    if (memcmp
+        (pmpriv->adapter, (t_u8 *) & psk->bssid, zero_mac, sizeof(zero_mac))) {
+        pbssid_tlv = (MrvlIEtypes_Bssid_t *) ptlv_buffer;
+        pbssid_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_BSSID);
+        pbssid_tlv->header.len = MLAN_MAC_ADDR_LENGTH;
+        memcpy(pmpriv->adapter, pbssid_tlv->bssid, (t_u8 *) & psk->bssid,
+               MLAN_MAC_ADDR_LENGTH);
+        ptlv_buffer += (pbssid_tlv->header.len + sizeof(MrvlIEtypesHeader_t));
+        cmd->size += (pbssid_tlv->header.len + sizeof(MrvlIEtypesHeader_t));
+        pbssid_tlv->header.len = wlan_cpu_to_le16(pbssid_tlv->header.len);
+    }
+    if (psk->psk_type == MLAN_PSK_PASSPHRASE) {
+        ppassphrase_tlv = (MrvlIEtypes_Passphrase_t *) ptlv_buffer;
+        ppassphrase_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_PASSPHRASE);
+        ppassphrase_tlv->header.len =
+            (t_u16) MIN(MLAN_MAX_PASSPHRASE_LENGTH,
+                        psk->psk.passphrase.passphrase_len);
+        memcpy(pmpriv->adapter, ppassphrase_tlv->passphrase,
+               psk->psk.passphrase.passphrase, MIN(MLAN_MAX_PASSPHRASE_LENGTH,
+                                                   psk->psk.passphrase.
+                                                   passphrase_len));
+        ptlv_buffer +=
+            (ppassphrase_tlv->header.len + sizeof(MrvlIEtypesHeader_t));
+        cmd->size +=
+            (ppassphrase_tlv->header.len + sizeof(MrvlIEtypesHeader_t));
+        ppassphrase_tlv->header.len =
+            wlan_cpu_to_le16(ppassphrase_tlv->header.len);
+    }
+    if (psk->psk_type == MLAN_PSK_PMK) {
+        ppmk_tlv = (MrvlIEtypes_PMK_t *) ptlv_buffer;
+        ppmk_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_PMK);
+        ppmk_tlv->header.len = MLAN_MAX_KEY_LENGTH;
+        memcpy(pmpriv->adapter, ppmk_tlv->pmk, psk->psk.pmk.pmk,
+               MLAN_MAX_KEY_LENGTH);
+        ptlv_buffer += (ppmk_tlv->header.len + sizeof(MrvlIEtypesHeader_t));
+        cmd->size += (ppmk_tlv->header.len + sizeof(MrvlIEtypesHeader_t));
+        ppmk_tlv->header.len = wlan_cpu_to_le16(ppmk_tlv->header.len);
+    }
+    if ((cmd_action == HostCmd_ACT_GEN_SET) &&
+        ((pssid_tlv || pbssid_tlv) && (!ppmk_tlv && !ppassphrase_tlv))) {
+        PRINTM(MERROR,
+               "Invalid case,ssid/bssid present without pmk or passphrase\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_SUPPLICANT_PMK);
+    pesupplicant_psk->action = wlan_cpu_to_le16(cmd_action);
+    pesupplicant_psk->cache_result = 0;
+    cmd->size = wlan_cpu_to_le16(cmd->size);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Handle the supplicant profile command
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_802_11_supplicant_profile(IN pmlan_private pmpriv,
+                                   IN HostCmd_DS_COMMAND * cmd,
+                                   IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    HostCmd_DS_802_11_SUPPLICANT_PROFILE *sup_profile =
+        &cmd->params.esupplicant_profile;
+    MrvlIEtypes_EncrProto_t *encr_proto_tlv = MNULL;
+    MrvlIEtypes_Cipher_t *pcipher_tlv = MNULL;
+    t_u8 *ptlv_buffer = (t_u8 *) sup_profile->tlv_buf;
+    mlan_ds_esupp_mode *esupp = MNULL;
+
+    ENTER();
+
+    cmd->size =
+        wlan_cpu_to_le16(sizeof(HostCmd_DS_802_11_SUPPLICANT_PROFILE) +
+                         S_DS_GEN - 1);
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_SUPPLICANT_PROFILE);
+    sup_profile->action = wlan_cpu_to_le16(cmd_action);
+    if ((cmd_action == HostCmd_ACT_GEN_SET) && pdata_buf) {
+        esupp = (mlan_ds_esupp_mode *) pdata_buf;
+        if (esupp->rsn_mode) {
+            encr_proto_tlv = (MrvlIEtypes_EncrProto_t *) ptlv_buffer;
+            encr_proto_tlv->header.type =
+                wlan_cpu_to_le16(TLV_TYPE_ENCRYPTION_PROTO);
+            encr_proto_tlv->header.len =
+                (t_u16) sizeof(encr_proto_tlv->rsn_mode);
+            encr_proto_tlv->rsn_mode = wlan_cpu_to_le16(esupp->rsn_mode);
+            ptlv_buffer +=
+                (encr_proto_tlv->header.len + sizeof(MrvlIEtypesHeader_t));
+            cmd->size +=
+                (encr_proto_tlv->header.len + sizeof(MrvlIEtypesHeader_t));
+            encr_proto_tlv->header.len =
+                wlan_cpu_to_le16(encr_proto_tlv->header.len);
+        }
+        if (esupp->act_paircipher || esupp->act_groupcipher) {
+            pcipher_tlv = (MrvlIEtypes_Cipher_t *) ptlv_buffer;
+            pcipher_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_CIPHER);
+            pcipher_tlv->header.len =
+                (t_u16) (sizeof(pcipher_tlv->pair_cipher) +
+                         sizeof(pcipher_tlv->group_cipher));
+            if (esupp->act_paircipher) {
+                pcipher_tlv->pair_cipher =
+                    wlan_cpu_to_le16(esupp->act_paircipher);
+            }
+            if (esupp->act_groupcipher) {
+                pcipher_tlv->group_cipher =
+                    wlan_cpu_to_le16(esupp->act_groupcipher);
+            }
+            ptlv_buffer +=
+                (pcipher_tlv->header.len + sizeof(MrvlIEtypesHeader_t));
+            cmd->size +=
+                (pcipher_tlv->header.len + sizeof(MrvlIEtypesHeader_t));
+            pcipher_tlv->header.len = wlan_cpu_to_le16(pcipher_tlv->header.len);
+        }
+    }
+    cmd->size = wlan_cpu_to_le16(cmd->size);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of rf_channel.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_802_11_rf_channel(IN pmlan_private pmpriv,
+                           IN HostCmd_DS_COMMAND * cmd,
+                           IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    HostCmd_DS_802_11_RF_CHANNEL *prf_chan = &cmd->params.rf_channel;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_RF_CHANNEL);
+    cmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_802_11_RF_CHANNEL)
+                                 + S_DS_GEN);
+
+    if (cmd_action == HostCmd_ACT_GEN_SET) {
+        if ((pmpriv->adapter->adhoc_start_band & BAND_A)
+            || (pmpriv->adapter->adhoc_start_band & BAND_AN)
+            )
+            prf_chan->rf_type = HostCmd_SCAN_RADIO_TYPE_A;
+        SET_SECONDARYCHAN(prf_chan->rf_type, pmpriv->adapter->chan_bandwidth);
+        prf_chan->rf_type = wlan_cpu_to_le16(prf_chan->rf_type);
+        prf_chan->current_channel = wlan_cpu_to_le16(*((t_u16 *) pdata_buf));
+    }
+    prf_chan->action = wlan_cpu_to_le16(cmd_action);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of ibss_coalescing_status.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer or MNULL
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_ibss_coalescing_status(IN pmlan_private pmpriv,
+                                IN HostCmd_DS_COMMAND * cmd,
+                                IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    HostCmd_DS_802_11_IBSS_STATUS *pibss_coal = &(cmd->params.ibss_coalescing);
+    t_u16 enable = 0;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_IBSS_COALESCING_STATUS);
+    cmd->size =
+        wlan_cpu_to_le16(sizeof(HostCmd_DS_802_11_IBSS_STATUS) + S_DS_GEN);
+    cmd->result = 0;
+    pibss_coal->action = wlan_cpu_to_le16(cmd_action);
+
+    switch (cmd_action) {
+    case HostCmd_ACT_GEN_SET:
+        if (pdata_buf != MNULL)
+            enable = *(t_u16 *) pdata_buf;
+        pibss_coal->enable = wlan_cpu_to_le16(enable);
+        break;
+
+        /* In other case.. Nothing to do */
+    case HostCmd_ACT_GEN_GET:
+    default:
+        break;
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of mgmt IE list.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param pdata_buf	A pointer to data buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_mgmt_ie_list(IN pmlan_private pmpriv,
+                      IN HostCmd_DS_COMMAND * cmd,
+                      IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    t_u16 req_len = 0, travel_len = 0;
+    custom_ie *cptr = MNULL;
+    mlan_ds_misc_custom_ie *cust_ie = MNULL;
+    HostCmd_DS_MGMT_IE_LIST_CFG *pmgmt_ie_list = &(cmd->params.mgmt_ie_list);
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_MGMT_IE_LIST);
+    cmd->size = sizeof(HostCmd_DS_MGMT_IE_LIST_CFG) + S_DS_GEN;
+    cmd->result = 0;
+    pmgmt_ie_list->action = wlan_cpu_to_le16(cmd_action);
+
+    cust_ie = (mlan_ds_misc_custom_ie *) pdata_buf;
+    pmgmt_ie_list->ds_mgmt_ie.type = wlan_cpu_to_le16(cust_ie->type);
+    pmgmt_ie_list->ds_mgmt_ie.len = wlan_cpu_to_le16(cust_ie->len);
+
+    if (pmgmt_ie_list->ds_mgmt_ie.ie_data_list && cust_ie->ie_data_list) {
+        req_len = cust_ie->len;
+        travel_len = 0;
+        /* conversion for index, mask, len */
+        if (req_len == sizeof(t_u16))
+            cust_ie->ie_data_list[0].ie_index =
+                wlan_cpu_to_le16(cust_ie->ie_data_list[0].ie_index);
+
+        while (req_len > sizeof(t_u16)) {
+            cptr =
+                (custom_ie *) (((t_u8 *) cust_ie->ie_data_list) + travel_len);
+            travel_len += cptr->ie_length + sizeof(custom_ie) - MAX_IE_SIZE;
+            req_len -= cptr->ie_length + sizeof(custom_ie) - MAX_IE_SIZE;
+            cptr->ie_index = wlan_cpu_to_le16(cptr->ie_index);
+            cptr->mgmt_subtype_mask = wlan_cpu_to_le16(cptr->mgmt_subtype_mask);
+            cptr->ie_length = wlan_cpu_to_le16(cptr->ie_length);
+        }
+        if (cust_ie->len)
+            memcpy(pmpriv->adapter, pmgmt_ie_list->ds_mgmt_ie.ie_data_list,
+                   cust_ie->ie_data_list, cust_ie->len);
+    }
+
+    cmd->size -=
+        (MAX_MGMT_IE_INDEX_TO_FW * sizeof(custom_ie)) +
+        sizeof(tlvbuf_max_mgmt_ie);
+    cmd->size += cust_ie->len;
+    cmd->size = wlan_cpu_to_le16(cmd->size);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares system clock cfg command
+ *
+ *  @param pmpriv    	A pointer to mlan_private structure
+ *  @param cmd	   	A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action 	The action: GET or SET
+ *  @param pdata_buf 	A pointer to data buffer
+ *  @return 	   	MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_sysclock_cfg(IN pmlan_private pmpriv,
+                      IN HostCmd_DS_COMMAND * cmd,
+                      IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    HostCmd_DS_ECL_SYSTEM_CLOCK_CONFIG *cfg = &cmd->params.sys_clock_cfg;
+    mlan_ds_misc_sys_clock *clk_cfg = (mlan_ds_misc_sys_clock *) pdata_buf;
+    int i = 0;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_ECL_SYSTEM_CLOCK_CONFIG);
+    cmd->size =
+        wlan_cpu_to_le16(sizeof(HostCmd_DS_ECL_SYSTEM_CLOCK_CONFIG) + S_DS_GEN);
+
+    cfg->action = wlan_cpu_to_le16(cmd_action);
+    cfg->cur_sys_clk = wlan_cpu_to_le16(clk_cfg->cur_sys_clk);
+    cfg->sys_clk_type = wlan_cpu_to_le16(clk_cfg->sys_clk_type);
+    cfg->sys_clk_len = wlan_cpu_to_le16(clk_cfg->sys_clk_num) * sizeof(t_u16);
+    for (i = 0; i < clk_cfg->sys_clk_num; i++)
+        cfg->sys_clk[i] = wlan_cpu_to_le16(clk_cfg->sys_clk[i]);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of reg_access.
+ *
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   the action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_cmd_reg_access(IN HostCmd_DS_COMMAND * cmd,
+                    IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    mlan_ds_reg_rw *reg_rw;
+
+    ENTER();
+
+    reg_rw = (mlan_ds_reg_rw *) pdata_buf;
+    switch (cmd->command) {
+    case HostCmd_CMD_MAC_REG_ACCESS:
+        {
+            HostCmd_DS_MAC_REG_ACCESS *mac_reg;
+            cmd->size =
+                wlan_cpu_to_le16(sizeof(HostCmd_DS_MAC_REG_ACCESS) + S_DS_GEN);
+            mac_reg = (HostCmd_DS_MAC_REG_ACCESS *) & cmd->params.mac_reg;
+            mac_reg->action = wlan_cpu_to_le16(cmd_action);
+            mac_reg->offset = wlan_cpu_to_le16((t_u16) reg_rw->offset);
+            mac_reg->value = wlan_cpu_to_le32(reg_rw->value);
+            break;
+        }
+    case HostCmd_CMD_BBP_REG_ACCESS:
+        {
+            HostCmd_DS_BBP_REG_ACCESS *bbp_reg;
+            cmd->size =
+                wlan_cpu_to_le16(sizeof(HostCmd_DS_BBP_REG_ACCESS) + S_DS_GEN);
+            bbp_reg = (HostCmd_DS_BBP_REG_ACCESS *) & cmd->params.bbp_reg;
+            bbp_reg->action = wlan_cpu_to_le16(cmd_action);
+            bbp_reg->offset = wlan_cpu_to_le16((t_u16) reg_rw->offset);
+            bbp_reg->value = (t_u8) reg_rw->value;
+            break;
+        }
+    case HostCmd_CMD_RF_REG_ACCESS:
+        {
+            HostCmd_DS_RF_REG_ACCESS *rf_reg;
+            cmd->size =
+                wlan_cpu_to_le16(sizeof(HostCmd_DS_RF_REG_ACCESS) + S_DS_GEN);
+            rf_reg = (HostCmd_DS_RF_REG_ACCESS *) & cmd->params.rf_reg;
+            rf_reg->action = wlan_cpu_to_le16(cmd_action);
+            rf_reg->offset = wlan_cpu_to_le16((t_u16) reg_rw->offset);
+            rf_reg->value = (t_u8) reg_rw->value;
+            break;
+        }
+    case HostCmd_CMD_CAU_REG_ACCESS:
+        {
+            HostCmd_DS_RF_REG_ACCESS *cau_reg;
+            cmd->size =
+                wlan_cpu_to_le16(sizeof(HostCmd_DS_RF_REG_ACCESS) + S_DS_GEN);
+            cau_reg = (HostCmd_DS_RF_REG_ACCESS *) & cmd->params.rf_reg;
+            cau_reg->action = wlan_cpu_to_le16(cmd_action);
+            cau_reg->offset = wlan_cpu_to_le16((t_u16) reg_rw->offset);
+            cau_reg->value = (t_u8) reg_rw->value;
+            break;
+        }
+    case HostCmd_CMD_802_11_EEPROM_ACCESS:
+        {
+            mlan_ds_read_eeprom *rd_eeprom = (mlan_ds_read_eeprom *) pdata_buf;
+            HostCmd_DS_802_11_EEPROM_ACCESS *cmd_eeprom =
+                (HostCmd_DS_802_11_EEPROM_ACCESS *) & cmd->params.eeprom;
+            cmd->size =
+                wlan_cpu_to_le16(sizeof(HostCmd_DS_802_11_EEPROM_ACCESS) +
+                                 S_DS_GEN);
+            cmd_eeprom->action = wlan_cpu_to_le16(cmd_action);
+            cmd_eeprom->offset = wlan_cpu_to_le16(rd_eeprom->offset);
+            cmd_eeprom->byte_count = wlan_cpu_to_le16(rd_eeprom->byte_count);
+            cmd_eeprom->value = 0;
+            break;
+        }
+    default:
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    cmd->command = wlan_cpu_to_le16(cmd->command);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of mem_access.
+ *
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   the action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_mem_access(IN HostCmd_DS_COMMAND * cmd,
+                    IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    mlan_ds_mem_rw *mem_rw = (mlan_ds_mem_rw *) pdata_buf;
+    HostCmd_DS_MEM_ACCESS *mem_access =
+        (HostCmd_DS_MEM_ACCESS *) & cmd->params.mem;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_MEM_ACCESS);
+    cmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_MEM_ACCESS) + S_DS_GEN);
+
+    mem_access->action = wlan_cpu_to_le16(cmd_action);
+    mem_access->addr = wlan_cpu_to_le32(mem_rw->addr);
+    mem_access->value = wlan_cpu_to_le32(mem_rw->value);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of subscribe event.
+ *
+ *  @param pmpriv    	A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   the action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_subscribe_event(IN pmlan_private pmpriv,
+                         IN HostCmd_DS_COMMAND * cmd,
+                         IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    mlan_ds_subscribe_evt *sub_evt = (mlan_ds_subscribe_evt *) pdata_buf;
+    HostCmd_DS_SUBSCRIBE_EVENT *evt =
+        (HostCmd_DS_SUBSCRIBE_EVENT *) & cmd->params.subscribe_event;
+    t_u16 cmd_size = 0;
+    t_u8 *tlv = MNULL;
+    MrvlIEtypes_BeaconLowRssiThreshold_t *rssi_low = MNULL;
+    MrvlIEtypes_BeaconLowSnrThreshold_t *snr_low = MNULL;
+    MrvlIEtypes_FailureCount_t *fail_count = MNULL;
+    MrvlIEtypes_BeaconsMissed_t *beacon_missed = MNULL;
+    MrvlIEtypes_BeaconHighRssiThreshold_t *rssi_high = MNULL;
+    MrvlIEtypes_BeaconHighSnrThreshold_t *snr_high = MNULL;
+    MrvlIEtypes_DataLowRssiThreshold_t *data_rssi_low = MNULL;
+    MrvlIEtypes_DataLowSnrThreshold_t *data_snr_low = MNULL;
+    MrvlIEtypes_DataHighRssiThreshold_t *data_rssi_high = MNULL;
+    MrvlIEtypes_DataHighSnrThreshold_t *data_snr_high = MNULL;
+    MrvlIEtypes_LinkQualityThreshold_t *link_quality = MNULL;
+    MrvlIETypes_PreBeaconMissed_t *pre_bcn_missed = MNULL;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_SUBSCRIBE_EVENT);
+    evt->action = wlan_cpu_to_le16(cmd_action);
+    cmd_size = sizeof(HostCmd_DS_SUBSCRIBE_EVENT) + S_DS_GEN;
+    if (cmd_action == HostCmd_ACT_GEN_GET)
+        goto done;
+#define HostCmd_ACT_BITWISE_SET	0x02
+    evt->action = wlan_cpu_to_le16(HostCmd_ACT_BITWISE_SET);
+    evt->event_bitmap = wlan_cpu_to_le16(sub_evt->evt_bitmap);
+    tlv = (t_u8 *) cmd + cmd_size;
+    if (sub_evt->evt_bitmap & SUBSCRIBE_EVT_RSSI_LOW) {
+        rssi_low = (MrvlIEtypes_BeaconLowRssiThreshold_t *) tlv;
+        rssi_low->header.type = wlan_cpu_to_le16(TLV_TYPE_RSSI_LOW);
+        rssi_low->header.len =
+            wlan_cpu_to_le16(sizeof(MrvlIEtypes_BeaconLowRssiThreshold_t) -
+                             sizeof(MrvlIEtypesHeader_t));
+        rssi_low->value = sub_evt->low_rssi;
+        rssi_low->frequency = sub_evt->low_rssi_freq;
+        tlv += sizeof(MrvlIEtypes_BeaconLowRssiThreshold_t);
+        cmd_size += sizeof(MrvlIEtypes_BeaconLowRssiThreshold_t);
+    }
+    if (sub_evt->evt_bitmap & SUBSCRIBE_EVT_SNR_LOW) {
+        snr_low = (MrvlIEtypes_BeaconLowSnrThreshold_t *) tlv;
+        snr_low->header.type = wlan_cpu_to_le16(TLV_TYPE_SNR_LOW);
+        snr_low->header.len =
+            wlan_cpu_to_le16(sizeof(MrvlIEtypes_BeaconLowSnrThreshold_t) -
+                             sizeof(MrvlIEtypesHeader_t));
+        snr_low->value = sub_evt->low_snr;
+        snr_low->frequency = sub_evt->low_snr_freq;
+        tlv += sizeof(MrvlIEtypes_BeaconLowSnrThreshold_t);
+        cmd_size += sizeof(MrvlIEtypes_BeaconLowSnrThreshold_t);
+    }
+    if (sub_evt->evt_bitmap & SUBSCRIBE_EVT_MAX_FAIL) {
+        fail_count = (MrvlIEtypes_FailureCount_t *) tlv;
+        fail_count->header.type = wlan_cpu_to_le16(TLV_TYPE_FAILCOUNT);
+        fail_count->header.len =
+            wlan_cpu_to_le16(sizeof(MrvlIEtypes_FailureCount_t) -
+                             sizeof(MrvlIEtypesHeader_t));
+        fail_count->value = sub_evt->failure_count;
+        fail_count->frequency = sub_evt->failure_count_freq;
+        tlv += sizeof(MrvlIEtypes_FailureCount_t);
+        cmd_size += sizeof(MrvlIEtypes_FailureCount_t);
+    }
+    if (sub_evt->evt_bitmap & SUBSCRIBE_EVT_BEACON_MISSED) {
+        beacon_missed = (MrvlIEtypes_BeaconsMissed_t *) tlv;
+        beacon_missed->header.type = wlan_cpu_to_le16(TLV_TYPE_BCNMISS);
+        beacon_missed->header.len =
+            wlan_cpu_to_le16(sizeof(MrvlIEtypes_BeaconsMissed_t) -
+                             sizeof(MrvlIEtypesHeader_t));
+        beacon_missed->value = sub_evt->beacon_miss;
+        beacon_missed->frequency = sub_evt->beacon_miss_freq;
+        tlv += sizeof(MrvlIEtypes_BeaconsMissed_t);
+        cmd_size += sizeof(MrvlIEtypes_BeaconsMissed_t);
+    }
+    if (sub_evt->evt_bitmap & SUBSCRIBE_EVT_RSSI_HIGH) {
+        rssi_high = (MrvlIEtypes_BeaconHighRssiThreshold_t *) tlv;
+        rssi_high->header.type = wlan_cpu_to_le16(TLV_TYPE_RSSI_HIGH);
+        rssi_high->header.len =
+            wlan_cpu_to_le16(sizeof(MrvlIEtypes_BeaconHighRssiThreshold_t) -
+                             sizeof(MrvlIEtypesHeader_t));
+        rssi_high->value = sub_evt->high_rssi;
+        rssi_high->frequency = sub_evt->high_rssi_freq;
+        tlv += sizeof(MrvlIEtypes_BeaconHighRssiThreshold_t);
+        cmd_size += sizeof(MrvlIEtypes_BeaconHighRssiThreshold_t);
+    }
+    if (sub_evt->evt_bitmap & SUBSCRIBE_EVT_SNR_HIGH) {
+        snr_high = (MrvlIEtypes_BeaconHighSnrThreshold_t *) tlv;
+        snr_high->header.type = wlan_cpu_to_le16(TLV_TYPE_SNR_HIGH);
+        snr_high->header.len =
+            wlan_cpu_to_le16(sizeof(MrvlIEtypes_BeaconHighSnrThreshold_t) -
+                             sizeof(MrvlIEtypesHeader_t));
+        snr_high->value = sub_evt->high_snr;
+        snr_high->frequency = sub_evt->high_snr_freq;
+        tlv += sizeof(MrvlIEtypes_BeaconHighSnrThreshold_t);
+        cmd_size += sizeof(MrvlIEtypes_BeaconHighSnrThreshold_t);
+    }
+    if (sub_evt->evt_bitmap & SUBSCRIBE_EVT_DATA_RSSI_LOW) {
+        data_rssi_low = (MrvlIEtypes_DataLowRssiThreshold_t *) tlv;
+        data_rssi_low->header.type = wlan_cpu_to_le16(TLV_TYPE_RSSI_LOW_DATA);
+        data_rssi_low->header.len =
+            wlan_cpu_to_le16(sizeof(MrvlIEtypes_DataLowRssiThreshold_t) -
+                             sizeof(MrvlIEtypesHeader_t));
+        data_rssi_low->value = sub_evt->data_low_rssi;
+        data_rssi_low->frequency = sub_evt->data_low_rssi_freq;
+        tlv += sizeof(MrvlIEtypes_DataLowRssiThreshold_t);
+        cmd_size += sizeof(MrvlIEtypes_DataLowRssiThreshold_t);
+    }
+    if (sub_evt->evt_bitmap & SUBSCRIBE_EVT_DATA_SNR_LOW) {
+        data_snr_low = (MrvlIEtypes_DataLowSnrThreshold_t *) tlv;
+        data_snr_low->header.type = wlan_cpu_to_le16(TLV_TYPE_SNR_LOW_DATA);
+        data_snr_low->header.len =
+            wlan_cpu_to_le16(sizeof(MrvlIEtypes_DataLowSnrThreshold_t) -
+                             sizeof(MrvlIEtypesHeader_t));
+        data_snr_low->value = sub_evt->data_low_snr;
+        data_snr_low->frequency = sub_evt->data_low_snr_freq;
+        tlv += sizeof(MrvlIEtypes_DataLowSnrThreshold_t);
+        cmd_size += sizeof(MrvlIEtypes_DataLowSnrThreshold_t);
+    }
+    if (sub_evt->evt_bitmap & SUBSCRIBE_EVT_DATA_RSSI_HIGH) {
+        data_rssi_high = (MrvlIEtypes_DataHighRssiThreshold_t *) tlv;
+        data_rssi_high->header.type = wlan_cpu_to_le16(TLV_TYPE_RSSI_HIGH_DATA);
+        data_rssi_high->header.len =
+            wlan_cpu_to_le16(sizeof(MrvlIEtypes_DataHighRssiThreshold_t) -
+                             sizeof(MrvlIEtypesHeader_t));
+        data_rssi_high->value = sub_evt->data_high_rssi;
+        data_rssi_high->frequency = sub_evt->data_high_rssi_freq;
+        tlv += sizeof(MrvlIEtypes_DataHighRssiThreshold_t);
+        cmd_size += sizeof(MrvlIEtypes_DataHighRssiThreshold_t);
+    }
+    if (sub_evt->evt_bitmap & SUBSCRIBE_EVT_DATA_SNR_HIGH) {
+        data_snr_high = (MrvlIEtypes_DataHighSnrThreshold_t *) tlv;
+        data_snr_high->header.type = wlan_cpu_to_le16(TLV_TYPE_SNR_HIGH_DATA);
+        data_snr_high->header.len =
+            wlan_cpu_to_le16(sizeof(MrvlIEtypes_DataHighSnrThreshold_t) -
+                             sizeof(MrvlIEtypesHeader_t));
+        data_snr_high->value = sub_evt->data_high_snr;
+        data_snr_high->frequency = sub_evt->data_high_snr_freq;
+        tlv += sizeof(MrvlIEtypes_DataHighSnrThreshold_t);
+        cmd_size += sizeof(MrvlIEtypes_DataHighSnrThreshold_t);
+    }
+    if (sub_evt->evt_bitmap & SUBSCRIBE_EVT_LINK_QUALITY) {
+        link_quality = (MrvlIEtypes_LinkQualityThreshold_t *) tlv;
+        link_quality->header.type = wlan_cpu_to_le16(TLV_TYPE_LINK_QUALITY);
+        link_quality->header.len =
+            wlan_cpu_to_le16(sizeof(MrvlIEtypes_LinkQualityThreshold_t) -
+                             sizeof(MrvlIEtypesHeader_t));
+        link_quality->link_snr = wlan_cpu_to_le16(sub_evt->link_snr);
+        link_quality->link_snr_freq = wlan_cpu_to_le16(sub_evt->link_snr_freq);
+        link_quality->link_rate = wlan_cpu_to_le16(sub_evt->link_rate);
+        link_quality->link_rate_freq =
+            wlan_cpu_to_le16(sub_evt->link_rate_freq);
+        link_quality->link_tx_latency =
+            wlan_cpu_to_le16(sub_evt->link_tx_latency);
+        link_quality->link_tx_lantency_freq =
+            wlan_cpu_to_le16(sub_evt->link_tx_lantency_freq);
+        tlv += sizeof(MrvlIEtypes_LinkQualityThreshold_t);
+        cmd_size += sizeof(MrvlIEtypes_LinkQualityThreshold_t);
+    }
+    if (sub_evt->evt_bitmap & SUBSCRIBE_EVT_PRE_BEACON_LOST) {
+        pre_bcn_missed = (MrvlIETypes_PreBeaconMissed_t *) tlv;
+        pre_bcn_missed->header.type = wlan_cpu_to_le16(TLV_TYPE_PRE_BCNMISS);
+        pre_bcn_missed->header.len =
+            wlan_cpu_to_le16(sizeof(MrvlIETypes_PreBeaconMissed_t) -
+                             sizeof(MrvlIEtypesHeader_t));
+        pre_bcn_missed->value = sub_evt->pre_beacon_miss;
+        pre_bcn_missed->frequency = 0;
+        tlv += sizeof(MrvlIETypes_PreBeaconMissed_t);
+        cmd_size += sizeof(MrvlIETypes_PreBeaconMissed_t);
+    }
+  done:
+    cmd->size = wlan_cpu_to_le16(cmd_size);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of OTP user data.
+ *
+ *  @param pmpriv    	A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   the action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_otp_user_data(IN pmlan_private pmpriv,
+                       IN HostCmd_DS_COMMAND * cmd,
+                       IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    mlan_ds_misc_otp_user_data *user_data =
+        (mlan_ds_misc_otp_user_data *) pdata_buf;
+    HostCmd_DS_OTP_USER_DATA *cmd_user_data =
+        (HostCmd_DS_OTP_USER_DATA *) & cmd->params.otp_user_data;
+    t_u16 cmd_size = 0;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_OTP_READ_USER_DATA);
+    cmd_size = sizeof(HostCmd_DS_OTP_USER_DATA) + S_DS_GEN - 1;
+
+    cmd_user_data->action = wlan_cpu_to_le16(cmd_action);
+    cmd_user_data->reserved = 0;
+    cmd_user_data->user_data_length =
+        wlan_cpu_to_le16(user_data->user_data_length);
+    cmd_size += user_data->user_data_length;
+    cmd->size = wlan_cpu_to_le16(cmd_size);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares inactivity timeout command
+ *
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   the action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_cmd_inactivity_timeout(IN HostCmd_DS_COMMAND * cmd,
+                            IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    pmlan_ds_inactivity_to inac_to;
+    HostCmd_DS_INACTIVITY_TIMEOUT_EXT *cmd_inac_to = &cmd->params.inactivity_to;
+
+    ENTER();
+
+    inac_to = (mlan_ds_inactivity_to *) pdata_buf;
+
+    cmd->size =
+        wlan_cpu_to_le16(sizeof(HostCmd_DS_INACTIVITY_TIMEOUT_EXT) + S_DS_GEN);
+    cmd->command = wlan_cpu_to_le16(cmd->command);
+    cmd_inac_to->action = wlan_cpu_to_le16(cmd_action);
+    if (cmd_action == HostCmd_ACT_GEN_SET) {
+        cmd_inac_to->timeout_unit =
+            wlan_cpu_to_le16((t_u16) inac_to->timeout_unit);
+        cmd_inac_to->unicast_timeout =
+            wlan_cpu_to_le16((t_u16) inac_to->unicast_timeout);
+        cmd_inac_to->mcast_timeout =
+            wlan_cpu_to_le16((t_u16) inac_to->mcast_timeout);
+        cmd_inac_to->ps_entry_timeout =
+            wlan_cpu_to_le16((t_u16) inac_to->ps_entry_timeout);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/********************************************************
+                Global Functions
+********************************************************/
+
+/**
+ *  @brief This function prepare the command before sending to firmware.
+ *
+ *  @param priv       A pointer to mlan_private structure
+ *  @param cmd_no       Command number
+ *  @param cmd_action   Command action: GET or SET
+ *  @param cmd_oid      Cmd oid: treated as sub command
+ *  @param pioctl_buf   A pointer to MLAN IOCTL Request buffer
+ *  @param pdata_buf    A pointer to information buffer
+ *  @param pcmd_buf      A pointer to cmd buf
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ops_sta_prepare_cmd(IN t_void * priv,
+                         IN t_u16 cmd_no,
+                         IN t_u16 cmd_action,
+                         IN t_u32 cmd_oid,
+                         IN t_void * pioctl_buf,
+                         IN t_void * pdata_buf, IN t_void * pcmd_buf)
+{
+    HostCmd_DS_COMMAND *cmd_ptr = (HostCmd_DS_COMMAND *) pcmd_buf;
+    mlan_private *pmpriv = (mlan_private *) priv;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    /* Prepare command */
+    switch (cmd_no) {
+    case HostCmd_CMD_GET_HW_SPEC:
+        ret = wlan_cmd_get_hw_spec(pmpriv, cmd_ptr);
+        break;
+    case HostCmd_CMD_CFG_DATA:
+        ret = wlan_cmd_cfg_data(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_MAC_CONTROL:
+        ret = wlan_cmd_mac_control(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_MAC_ADDRESS:
+        ret = wlan_cmd_802_11_mac_address(pmpriv, cmd_ptr, cmd_action);
+        break;
+    case HostCmd_CMD_MAC_MULTICAST_ADR:
+        ret =
+            wlan_cmd_mac_multicast_adr(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_TX_RATE_CFG:
+        ret = wlan_cmd_tx_rate_cfg(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_RF_ANTENNA:
+        ret =
+            wlan_cmd_802_11_rf_antenna(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_TXPWR_CFG:
+        ret = wlan_cmd_tx_power_cfg(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_RF_TX_POWER:
+        ret = wlan_cmd_802_11_rf_tx_power(pmpriv, cmd_ptr,
+                                          cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_PS_MODE_ENH:
+        ret =
+            wlan_cmd_enh_power_mode(pmpriv, cmd_ptr, cmd_action,
+                                    (t_u16) cmd_oid, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_HS_CFG_ENH:
+        ret =
+            wlan_cmd_802_11_hs_cfg(pmpriv, cmd_ptr, cmd_action,
+                                   (hs_config_param *) pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_SLEEP_PERIOD:
+        ret = wlan_cmd_802_11_sleep_period(pmpriv, cmd_ptr,
+                                           cmd_action, (t_u16 *) pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_SLEEP_PARAMS:
+        ret = wlan_cmd_802_11_sleep_params(pmpriv, cmd_ptr,
+                                           cmd_action, (t_u16 *) pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_SCAN:
+        ret = wlan_cmd_802_11_scan(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_BG_SCAN_CONFIG:
+        ret = wlan_cmd_bgscan_config(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_BG_SCAN_QUERY:
+        ret = wlan_cmd_802_11_bg_scan_query(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_ASSOCIATE:
+        ret = wlan_cmd_802_11_associate(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_DEAUTHENTICATE:
+        ret = wlan_cmd_802_11_deauthenticate(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_AD_HOC_START:
+        ret = wlan_cmd_802_11_ad_hoc_start(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_AD_HOC_JOIN:
+        ret = wlan_cmd_802_11_ad_hoc_join(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_AD_HOC_STOP:
+        ret = wlan_cmd_802_11_ad_hoc_stop(pmpriv, cmd_ptr);
+        break;
+    case HostCmd_CMD_802_11_GET_LOG:
+        ret = wlan_cmd_802_11_get_log(pmpriv, cmd_ptr);
+        break;
+    case HostCmd_CMD_RSSI_INFO:
+        ret = wlan_cmd_802_11_rssi_info(pmpriv, cmd_ptr, cmd_action);
+        break;
+    case HostCmd_CMD_802_11_SNMP_MIB:
+        ret =
+            wlan_cmd_802_11_snmp_mib(pmpriv, cmd_ptr, cmd_action, cmd_oid,
+                                     pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_RADIO_CONTROL:
+        ret =
+            wlan_cmd_802_11_radio_control(pmpriv, cmd_ptr, cmd_action,
+                                          pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_TX_RATE_QUERY:
+        cmd_ptr->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_TX_RATE_QUERY);
+        cmd_ptr->size =
+            wlan_cpu_to_le16(sizeof(HostCmd_TX_RATE_QUERY) + S_DS_GEN);
+        pmpriv->tx_rate = 0;
+        ret = MLAN_STATUS_SUCCESS;
+        break;
+    case HostCmd_CMD_VERSION_EXT:
+        cmd_ptr->command = wlan_cpu_to_le16(cmd_no);
+        cmd_ptr->params.verext.version_str_sel =
+            (t_u8) (*((t_u32 *) pdata_buf));
+        cmd_ptr->size =
+            wlan_cpu_to_le16(sizeof(HostCmd_DS_VERSION_EXT) + S_DS_GEN);
+        ret = MLAN_STATUS_SUCCESS;
+        break;
+    case HostCmd_CMD_RX_MGMT_IND:
+        cmd_ptr->command = wlan_cpu_to_le16(cmd_no);
+        cmd_ptr->params.rx_mgmt_ind.action = wlan_cpu_to_le16(cmd_action);
+        cmd_ptr->params.rx_mgmt_ind.mgmt_subtype_mask =
+            wlan_cpu_to_le32((t_u32) (*((t_u32 *) pdata_buf)));
+        cmd_ptr->size = wlan_cpu_to_le16(sizeof(t_u32) + S_DS_GEN);
+        break;
+    case HostCmd_CMD_802_11_RF_CHANNEL:
+        ret =
+            wlan_cmd_802_11_rf_channel(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_FUNC_INIT:
+        if (pmpriv->adapter->hw_status == WlanHardwareStatusReset)
+            pmpriv->adapter->hw_status = WlanHardwareStatusInitializing;
+        cmd_ptr->command = wlan_cpu_to_le16(cmd_no);
+        cmd_ptr->size = wlan_cpu_to_le16(S_DS_GEN);
+        break;
+    case HostCmd_CMD_FUNC_SHUTDOWN:
+        pmpriv->adapter->hw_status = WlanHardwareStatusReset;
+        cmd_ptr->command = wlan_cpu_to_le16(cmd_no);
+        cmd_ptr->size = wlan_cpu_to_le16(S_DS_GEN);
+        break;
+    case HostCmd_CMD_SOFT_RESET:
+        cmd_ptr->command = wlan_cpu_to_le16(cmd_no);
+        cmd_ptr->size = wlan_cpu_to_le16(S_DS_GEN);
+        break;
+    case HostCmd_CMD_11N_ADDBA_REQ:
+        ret = wlan_cmd_11n_addba_req(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_11N_DELBA:
+        ret = wlan_cmd_11n_delba(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_11N_ADDBA_RSP:
+        ret = wlan_cmd_11n_addba_rspgen(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_KEY_MATERIAL:
+        ret =
+            wlan_cmd_802_11_key_material(pmpriv, cmd_ptr, cmd_action, cmd_oid,
+                                         pdata_buf);
+        break;
+
+    case HostCmd_CMD_SUPPLICANT_PMK:
+        ret = wlan_cmd_802_11_supplicant_pmk(pmpriv, cmd_ptr, cmd_action,
+                                             pdata_buf);
+        break;
+    case HostCmd_CMD_SUPPLICANT_PROFILE:
+        ret = wlan_cmd_802_11_supplicant_profile(pmpriv, cmd_ptr, cmd_action,
+                                                 pdata_buf);
+        break;
+
+    case HostCmd_CMD_802_11D_DOMAIN_INFO:
+        ret = wlan_cmd_802_11d_domain_info(pmpriv, cmd_ptr, cmd_action);
+        break;
+    case HostCmd_CMD_802_11_TPC_ADAPT_REQ:
+    case HostCmd_CMD_802_11_TPC_INFO:
+    case HostCmd_CMD_802_11_CHAN_SW_ANN:
+    case HostCmd_CMD_CHAN_REPORT_REQUEST:
+        ret = wlan_11h_cmd_process(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_RECONFIGURE_TX_BUFF:
+        ret = wlan_cmd_recfg_tx_buf(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_AMSDU_AGGR_CTRL:
+        ret = wlan_cmd_amsdu_aggr_ctrl(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_11N_CFG:
+        ret = wlan_cmd_11n_cfg(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_TX_BF_CFG:
+        ret = wlan_cmd_tx_bf_cfg(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_WMM_GET_STATUS:
+        PRINTM(MINFO, "WMM: WMM_GET_STATUS cmd sent\n");
+        cmd_ptr->command = wlan_cpu_to_le16(HostCmd_CMD_WMM_GET_STATUS);
+        cmd_ptr->size =
+            wlan_cpu_to_le16(sizeof(HostCmd_DS_WMM_GET_STATUS) + S_DS_GEN);
+        ret = MLAN_STATUS_SUCCESS;
+        break;
+    case HostCmd_CMD_WMM_ADDTS_REQ:
+        ret = wlan_cmd_wmm_addts_req(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_WMM_DELTS_REQ:
+        ret = wlan_cmd_wmm_delts_req(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_WMM_QUEUE_CONFIG:
+        ret = wlan_cmd_wmm_queue_config(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_WMM_QUEUE_STATS:
+        ret = wlan_cmd_wmm_queue_stats(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_WMM_TS_STATUS:
+        ret = wlan_cmd_wmm_ts_status(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_IBSS_COALESCING_STATUS:
+        ret =
+            wlan_cmd_ibss_coalescing_status(pmpriv, cmd_ptr, cmd_action,
+                                            pdata_buf);
+        break;
+    case HostCmd_CMD_MGMT_IE_LIST:
+        ret = wlan_cmd_mgmt_ie_list(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_SCAN_EXT:
+        ret = wlan_cmd_802_11_scan_ext(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_ECL_SYSTEM_CLOCK_CONFIG:
+        ret = wlan_cmd_sysclock_cfg(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_MAC_REG_ACCESS:
+    case HostCmd_CMD_BBP_REG_ACCESS:
+    case HostCmd_CMD_RF_REG_ACCESS:
+    case HostCmd_CMD_CAU_REG_ACCESS:
+    case HostCmd_CMD_802_11_EEPROM_ACCESS:
+        ret = wlan_cmd_reg_access(cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_MEM_ACCESS:
+        ret = wlan_cmd_mem_access(cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_INACTIVITY_TIMEOUT_EXT:
+        ret = wlan_cmd_inactivity_timeout(cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_SDIO_GPIO_INT_CONFIG:
+        ret = wlan_cmd_sdio_gpio_int(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_SET_BSS_MODE:
+        cmd_ptr->command = wlan_cpu_to_le16(cmd_no);
+#ifdef WIFI_DIRECT_SUPPORT
+        if (pdata_buf) {
+            cmd_ptr->params.bss_mode.con_type = *(t_u8 *) pdata_buf;
+        } else
+#endif
+        if (pmpriv->bss_mode == MLAN_BSS_MODE_IBSS)
+            cmd_ptr->params.bss_mode.con_type = CONNECTION_TYPE_ADHOC;
+        else if (pmpriv->bss_mode == MLAN_BSS_MODE_INFRA)
+            cmd_ptr->params.bss_mode.con_type = CONNECTION_TYPE_INFRA;
+        cmd_ptr->size =
+            wlan_cpu_to_le16(sizeof(HostCmd_DS_SET_BSS_MODE) + S_DS_GEN);
+        ret = MLAN_STATUS_SUCCESS;
+        break;
+    case HostCmd_CMD_MEASUREMENT_REQUEST:
+    case HostCmd_CMD_MEASUREMENT_REPORT:
+        ret = wlan_meas_cmd_process(pmpriv, cmd_ptr, pdata_buf);
+        break;
+#ifdef WIFI_DIRECT_SUPPORT
+    case HostCmd_CMD_802_11_REMAIN_ON_CHANNEL:
+        ret =
+            wlan_cmd_remain_on_channel(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HOST_CMD_WIFI_DIRECT_MODE_CONFIG:
+        ret = wlan_cmd_wifi_direct_mode(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+#endif
+    case HostCmd_CMD_802_11_SUBSCRIBE_EVENT:
+        ret = wlan_cmd_subscribe_event(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_OTP_READ_USER_DATA:
+        ret = wlan_cmd_otp_user_data(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    default:
+        PRINTM(MERROR, "PREP_CMD: unknown command- %#x\n", cmd_no);
+        ret = MLAN_STATUS_FAILURE;
+        break;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief  This function issues commands to initialize firmware
+ *
+ *  @param priv     	A pointer to mlan_private structure
+ *  @param first_bss	flag for first BSS
+ *
+ *  @return		MLAN_STATUS_PENDING or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ops_sta_init_cmd(IN t_void * priv, IN t_u8 first_bss)
+{
+    pmlan_private pmpriv = (pmlan_private) priv;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 enable = MTRUE;
+    mlan_ds_11n_amsdu_aggr_ctrl amsdu_aggr_ctrl;
+
+    ENTER();
+
+    if (first_bss == MTRUE) {
+        ret = wlan_adapter_init_cmd(pmpriv->adapter);
+        if (ret == MLAN_STATUS_FAILURE)
+            goto done;
+    }
+
+    /* get tx rate */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_TX_RATE_CFG,
+                           HostCmd_ACT_GEN_GET, 0, MNULL, MNULL);
+    if (ret) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    pmpriv->data_rate = 0;
+
+    /* get tx power */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_RF_TX_POWER,
+                           HostCmd_ACT_GEN_GET, 0, MNULL, MNULL);
+    if (ret) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* set ibss coalescing_status */
+    ret = wlan_prepare_cmd(pmpriv, HostCmd_CMD_802_11_IBSS_COALESCING_STATUS,
+                           HostCmd_ACT_GEN_SET, 0, MNULL, &enable);
+    if (ret) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    memset(pmpriv->adapter, &amsdu_aggr_ctrl, 0, sizeof(amsdu_aggr_ctrl));
+    amsdu_aggr_ctrl.enable = MLAN_ACT_ENABLE;
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv, HostCmd_CMD_AMSDU_AGGR_CTRL,
+                           HostCmd_ACT_GEN_SET, 0, MNULL,
+                           (t_void *) & amsdu_aggr_ctrl);
+    if (ret) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    /* MAC Control must be the last command in init_fw */
+    /* set MAC Control */
+    ret = wlan_prepare_cmd(pmpriv, HostCmd_CMD_MAC_CONTROL,
+                           HostCmd_ACT_GEN_SET, 0, MNULL,
+                           &pmpriv->curr_pkt_filter);
+    if (ret) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    /** set last_init_cmd */
+    pmpriv->adapter->last_init_cmd = HostCmd_CMD_MAC_CONTROL;
+
+    if (first_bss == MFALSE) {
+        /* Get MAC address */
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_802_11_MAC_ADDRESS,
+                               HostCmd_ACT_GEN_GET, 0, MNULL, MNULL);
+        if (ret) {
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+        pmpriv->adapter->last_init_cmd = HostCmd_CMD_802_11_MAC_ADDRESS;
+    }
+
+    ret = MLAN_STATUS_PENDING;
+  done:
+    LEAVE();
+    return ret;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_sta_cmdresp.c b/drivers/net/wireless/sd8797/mlan/mlan_sta_cmdresp.c
new file mode 100644
index 000000000000..086d16ae5a62
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_sta_cmdresp.c
@@ -0,0 +1,1726 @@
+/** @file mlan_sta_cmdresp.c
+ *
+ *  @brief This file contains the handling of command
+ *  responses generated by firmware.
+ *
+ *  Copyright (C) 2008-2012, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    10/21/2008: initial version
+******************************************************/
+
+#include "mlan.h"
+#include "mlan_join.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_wmm.h"
+#include "mlan_11n.h"
+#include "mlan_11h.h"
+
+#include "mlan_sdio.h"
+#include "mlan_meas.h"
+
+/********************************************************
+                Local Variables
+********************************************************/
+
+/********************************************************
+                Global Variables
+********************************************************/
+
+/********************************************************
+                Local Functions
+********************************************************/
+/**
+ *  @brief This function handles the command response error
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to command buffer
+ *
+ *  @return             N/A
+ */
+static void
+wlan_process_cmdresp_error(mlan_private * pmpriv, HostCmd_DS_COMMAND * resp,
+                           mlan_ioctl_req * pioctl_buf)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+
+    ENTER();
+
+    PRINTM(MERROR, "CMD_RESP: cmd %#x error, result=%#x\n",
+           resp->command, resp->result);
+    if (pioctl_buf)
+        pioctl_buf->status_code = MLAN_ERROR_FW_CMDRESP;
+
+    switch (resp->command) {
+    case HostCmd_CMD_802_11_PS_MODE_ENH:
+        {
+            HostCmd_DS_802_11_PS_MODE_ENH *pm = &resp->params.psmode_enh;
+            PRINTM(MERROR,
+                   "PS_MODE_ENH command failed: result=0x%x action=0x%X\n",
+                   resp->result, wlan_le16_to_cpu(pm->action));
+            /*
+             * We do not re-try enter-ps command in ad-hoc mode.
+             */
+            if (wlan_le16_to_cpu(pm->action) == EN_AUTO_PS &&
+                (wlan_le16_to_cpu(pm->params.auto_ps.ps_bitmap) & BITMAP_STA_PS)
+                && pmpriv->bss_mode == MLAN_BSS_MODE_IBSS)
+                pmadapter->ps_mode = Wlan802_11PowerModeCAM;
+        }
+        break;
+    case HostCmd_CMD_802_11_SCAN_EXT:
+    case HostCmd_CMD_802_11_SCAN:
+        /* Cancel all pending scan command */
+        wlan_flush_scan_queue(pmadapter);
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_DRV_SCAN_REPORT, MNULL);
+        break;
+
+    case HostCmd_CMD_MAC_CONTROL:
+        break;
+
+    default:
+        break;
+    }
+    /*
+     * Handling errors here
+     */
+    wlan_insert_cmd_to_free_q(pmadapter, pmadapter->curr_cmd);
+
+    wlan_request_cmd_lock(pmadapter);
+    pmadapter->curr_cmd = MNULL;
+    wlan_release_cmd_lock(pmadapter);
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function handles the command response of RSSI info
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_802_11_rssi_info(IN pmlan_private pmpriv,
+                          IN HostCmd_DS_COMMAND * resp,
+                          IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_802_11_RSSI_INFO_RSP *prssi_info_rsp =
+        &resp->params.rssi_info_rsp;
+    mlan_ds_get_info *pget_info = MNULL;
+
+    ENTER();
+
+    pmpriv->data_rssi_last = wlan_le16_to_cpu(prssi_info_rsp->data_rssi_last);
+    pmpriv->data_nf_last = wlan_le16_to_cpu(prssi_info_rsp->data_nf_last);
+
+    pmpriv->data_rssi_avg = wlan_le16_to_cpu(prssi_info_rsp->data_rssi_avg);
+    pmpriv->data_nf_avg = wlan_le16_to_cpu(prssi_info_rsp->data_nf_avg);
+
+    pmpriv->bcn_rssi_last = wlan_le16_to_cpu(prssi_info_rsp->bcn_rssi_last);
+    pmpriv->bcn_nf_last = wlan_le16_to_cpu(prssi_info_rsp->bcn_nf_last);
+
+    pmpriv->bcn_rssi_avg = wlan_le16_to_cpu(prssi_info_rsp->bcn_rssi_avg);
+    pmpriv->bcn_nf_avg = wlan_le16_to_cpu(prssi_info_rsp->bcn_nf_avg);
+
+    /* Need to indicate IOCTL complete */
+    if (pioctl_buf != MNULL) {
+        pget_info = (mlan_ds_get_info *) pioctl_buf->pbuf;
+
+        memset(pmpriv->adapter, &pget_info->param.signal, 0,
+               sizeof(mlan_ds_get_signal));
+
+        pget_info->param.signal.selector = ALL_RSSI_INFO_MASK;
+
+        /* RSSI */
+        pget_info->param.signal.bcn_rssi_last = pmpriv->bcn_rssi_last;
+        pget_info->param.signal.bcn_rssi_avg = pmpriv->bcn_rssi_avg;
+        pget_info->param.signal.data_rssi_last = pmpriv->data_rssi_last;
+        pget_info->param.signal.data_rssi_avg = pmpriv->data_rssi_avg;
+
+        /* SNR */
+        pget_info->param.signal.bcn_snr_last =
+            CAL_SNR(pmpriv->bcn_rssi_last, pmpriv->bcn_nf_last);
+        pget_info->param.signal.bcn_snr_avg =
+            CAL_SNR(pmpriv->bcn_rssi_avg, pmpriv->bcn_nf_avg);
+        pget_info->param.signal.data_snr_last =
+            CAL_SNR(pmpriv->data_rssi_last, pmpriv->data_nf_last);
+        pget_info->param.signal.data_snr_avg =
+            CAL_SNR(pmpriv->data_rssi_avg, pmpriv->data_nf_avg);
+
+        /* NF */
+        pget_info->param.signal.bcn_nf_last = pmpriv->bcn_nf_last;
+        pget_info->param.signal.bcn_nf_avg = pmpriv->bcn_nf_avg;
+        pget_info->param.signal.data_nf_last = pmpriv->data_nf_last;
+        pget_info->param.signal.data_nf_avg = pmpriv->data_nf_avg;
+
+        pioctl_buf->data_read_written = sizeof(mlan_ds_get_info);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of mac_control
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_mac_control(IN pmlan_private pmpriv,
+                     IN HostCmd_DS_COMMAND * resp,
+                     IN mlan_ioctl_req * pioctl_buf)
+{
+    ENTER();
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of snmp_mib
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_802_11_snmp_mib(IN pmlan_private pmpriv,
+                         IN HostCmd_DS_COMMAND * resp,
+                         IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_802_11_SNMP_MIB *psmib = &resp->params.smib;
+    t_u16 oid = wlan_le16_to_cpu(psmib->oid);
+    t_u16 query_type = wlan_le16_to_cpu(psmib->query_type);
+    t_u32 ul_temp;
+    mlan_ds_snmp_mib *mib = MNULL;
+
+    ENTER();
+
+    if (pioctl_buf)
+        mib = (mlan_ds_snmp_mib *) pioctl_buf->pbuf;
+
+    PRINTM(MINFO, "SNMP_RESP: value of the oid = 0x%x, query_type=0x%x\n", oid,
+           query_type);
+    PRINTM(MINFO, "SNMP_RESP: Buf size  = 0x%x\n",
+           wlan_le16_to_cpu(psmib->buf_size));
+    if (query_type == HostCmd_ACT_GEN_GET) {
+        switch (oid) {
+        case DtimPeriod_i:
+            ul_temp = psmib->value[0];
+            PRINTM(MINFO, "SNMP_RESP: DTIM Period =%u\n", ul_temp);
+            if (mib)
+                mib->param.dtim_period = ul_temp;
+            break;
+        case FragThresh_i:
+            ul_temp = wlan_le16_to_cpu(*((t_u16 *) (psmib->value)));
+            PRINTM(MINFO, "SNMP_RESP: FragThsd =%u\n", ul_temp);
+            if (mib)
+                mib->param.frag_threshold = ul_temp;
+            break;
+
+        case RtsThresh_i:
+            ul_temp = wlan_le16_to_cpu(*((t_u16 *) (psmib->value)));
+            PRINTM(MINFO, "SNMP_RESP: RTSThsd =%u\n", ul_temp);
+            if (mib)
+                mib->param.rts_threshold = ul_temp;
+            break;
+
+        case ShortRetryLim_i:
+            ul_temp = wlan_le16_to_cpu(*((t_u16 *) (psmib->value)));
+            PRINTM(MINFO, "SNMP_RESP: TxRetryCount=%u\n", ul_temp);
+            if (mib)
+                mib->param.retry_count = ul_temp;
+            break;
+        case WwsMode_i:
+            ul_temp = wlan_le16_to_cpu(*((t_u16 *) (psmib->value)));
+            PRINTM(MINFO, "SNMP_RESP: WWSCfg =%u\n", ul_temp);
+            if (pioctl_buf)
+                ((mlan_ds_misc_cfg *) pioctl_buf->pbuf)->param.wws_cfg =
+                    ul_temp;
+            break;
+        case Thermal_i:
+            ul_temp = wlan_le32_to_cpu(*((t_u32 *) (psmib->value)));
+            PRINTM(MINFO, "SNMP_RESP: Thermal =%u\n", ul_temp);
+            if (pioctl_buf)
+                ((mlan_ds_misc_cfg *) pioctl_buf->pbuf)->param.thermal =
+                    ul_temp;
+            break;
+
+        default:
+            break;
+        }
+    } else {                    /* (query_type == HostCmd_ACT_GEN_SET) */
+        /* Update state for 11d */
+        if (oid == Dot11D_i) {
+            ul_temp = wlan_le16_to_cpu(*((t_u16 *) (psmib->value)));
+            /* Set 11d state to private */
+            pmpriv->state_11d.enable_11d = ul_temp;
+            /* Set user enable flag if called from ioctl */
+            if (pioctl_buf)
+                pmpriv->state_11d.user_enable_11d = ul_temp;
+        }
+        /* Update state for 11h */
+        if (oid == Dot11H_i) {
+            ul_temp = wlan_le16_to_cpu(*((t_u16 *) (psmib->value)));
+            /* Set 11h state to priv */
+            pmpriv->intf_state_11h.is_11h_active = (ul_temp & ENABLE_11H_MASK);
+            /* Set radar_det state to adapter */
+            pmpriv->adapter->state_11h.is_master_radar_det_active
+                = (ul_temp & MASTER_RADAR_DET_MASK) ? MTRUE : MFALSE;
+            pmpriv->adapter->state_11h.is_slave_radar_det_active
+                = (ul_temp & SLAVE_RADAR_DET_MASK) ? MTRUE : MFALSE;
+        }
+    }
+
+    if (pioctl_buf) {
+        /* Indicate ioctl complete */
+        pioctl_buf->data_read_written = sizeof(mlan_ds_snmp_mib);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of get_log
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_get_log(IN pmlan_private pmpriv,
+                 IN HostCmd_DS_COMMAND * resp, IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_802_11_GET_LOG *pget_log =
+        (HostCmd_DS_802_11_GET_LOG *) & resp->params.get_log;
+    mlan_ds_get_info *pget_info = MNULL;
+
+    ENTER();
+    if (pioctl_buf) {
+        pget_info = (mlan_ds_get_info *) pioctl_buf->pbuf;
+        pget_info->param.stats.mcast_tx_frame =
+            wlan_le32_to_cpu(pget_log->mcast_tx_frame);
+        pget_info->param.stats.failed = wlan_le32_to_cpu(pget_log->failed);
+        pget_info->param.stats.retry = wlan_le32_to_cpu(pget_log->retry);
+        pget_info->param.stats.multi_retry =
+            wlan_le32_to_cpu(pget_log->multiretry);
+        pget_info->param.stats.frame_dup =
+            wlan_le32_to_cpu(pget_log->frame_dup);
+        pget_info->param.stats.rts_success =
+            wlan_le32_to_cpu(pget_log->rts_success);
+        pget_info->param.stats.rts_failure =
+            wlan_le32_to_cpu(pget_log->rts_failure);
+        pget_info->param.stats.ack_failure =
+            wlan_le32_to_cpu(pget_log->ack_failure);
+        pget_info->param.stats.rx_frag = wlan_le32_to_cpu(pget_log->rx_frag);
+        pget_info->param.stats.mcast_rx_frame =
+            wlan_le32_to_cpu(pget_log->mcast_rx_frame);
+        pget_info->param.stats.fcs_error =
+            wlan_le32_to_cpu(pget_log->fcs_error);
+        pget_info->param.stats.tx_frame = wlan_le32_to_cpu(pget_log->tx_frame);
+        pget_info->param.stats.wep_icv_error[0] =
+            wlan_le32_to_cpu(pget_log->wep_icv_err_cnt[0]);
+        pget_info->param.stats.wep_icv_error[1] =
+            wlan_le32_to_cpu(pget_log->wep_icv_err_cnt[1]);
+        pget_info->param.stats.wep_icv_error[2] =
+            wlan_le32_to_cpu(pget_log->wep_icv_err_cnt[2]);
+        pget_info->param.stats.wep_icv_error[3] =
+            wlan_le32_to_cpu(pget_log->wep_icv_err_cnt[3]);
+        /* Indicate ioctl complete */
+        pioctl_buf->data_read_written = sizeof(mlan_ds_get_info);
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Get power level and rate index
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param pdata_buf    Pointer to the data buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_get_power_level(pmlan_private pmpriv, void *pdata_buf)
+{
+    int length = -1, max_power = -1, min_power = -1;
+    MrvlTypes_Power_Group_t *ppg_tlv = MNULL;
+    Power_Group_t *pg = MNULL;
+
+    ENTER();
+
+    if (pdata_buf) {
+        ppg_tlv =
+            (MrvlTypes_Power_Group_t *) ((t_u8 *) pdata_buf +
+                                         sizeof(HostCmd_DS_TXPWR_CFG));
+        pg = (Power_Group_t *) ((t_u8 *) ppg_tlv +
+                                sizeof(MrvlTypes_Power_Group_t));
+        length = ppg_tlv->length;
+        if (length > 0) {
+            max_power = pg->power_max;
+            min_power = pg->power_min;
+            length -= sizeof(Power_Group_t);
+        }
+        while (length) {
+            pg++;
+            if (max_power < pg->power_max) {
+                max_power = pg->power_max;
+            }
+            if (min_power > pg->power_min) {
+                min_power = pg->power_min;
+            }
+            length -= sizeof(Power_Group_t);
+        }
+        if (ppg_tlv->length > 0) {
+            pmpriv->min_tx_power_level = (t_u8) min_power;
+            pmpriv->max_tx_power_level = (t_u8) max_power;
+        }
+    } else {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of tx_power_cfg
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_tx_power_cfg(IN pmlan_private pmpriv,
+                      IN HostCmd_DS_COMMAND * resp,
+                      IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_TXPWR_CFG *ptxp_cfg = &resp->params.txp_cfg;
+    MrvlTypes_Power_Group_t *ppg_tlv = MNULL;
+    Power_Group_t *pg = MNULL;
+    t_u16 action = wlan_le16_to_cpu(ptxp_cfg->action);
+    mlan_ds_power_cfg *power = MNULL;
+    t_u32 data[5];
+
+    ENTER();
+
+    ppg_tlv = (MrvlTypes_Power_Group_t *) ((t_u8 *) ptxp_cfg
+                                           + sizeof(HostCmd_DS_TXPWR_CFG));
+    pg = (Power_Group_t *) ((t_u8 *) ppg_tlv + sizeof(MrvlTypes_Power_Group_t));
+
+    switch (action) {
+    case HostCmd_ACT_GEN_GET:
+        ppg_tlv->length = wlan_le16_to_cpu(ppg_tlv->length);
+        if (pmpriv->adapter->hw_status == WlanHardwareStatusInitializing)
+            wlan_get_power_level(pmpriv, ptxp_cfg);
+        pmpriv->tx_power_level = (t_u16) pg->power_min;
+        break;
+
+    case HostCmd_ACT_GEN_SET:
+        if (wlan_le32_to_cpu(ptxp_cfg->mode)) {
+            if (pg->power_max == pg->power_min)
+                pmpriv->tx_power_level = (t_u16) pg->power_min;
+        }
+        break;
+
+    default:
+        PRINTM(MERROR, "CMD_RESP: unknown command action %d\n", action);
+        LEAVE();
+        return MLAN_STATUS_SUCCESS;
+    }
+
+    PRINTM(MINFO, "Current TxPower Level = %d,Max Power=%d, Min Power=%d\n",
+           pmpriv->tx_power_level,
+           pmpriv->max_tx_power_level, pmpriv->min_tx_power_level);
+
+    if (pioctl_buf) {
+        power = (mlan_ds_power_cfg *) pioctl_buf->pbuf;
+        if (action == HostCmd_ACT_GEN_GET) {
+            if (power->sub_command == MLAN_OID_POWER_CFG) {
+                pioctl_buf->data_read_written
+                    = sizeof(mlan_power_cfg_t) + MLAN_SUB_COMMAND_SIZE;
+                power->param.power_cfg.power_level = pmpriv->tx_power_level;
+                if (wlan_le32_to_cpu(ptxp_cfg->mode))
+                    power->param.power_cfg.is_power_auto = 0;
+                else
+                    power->param.power_cfg.is_power_auto = 1;
+            } else {
+                power->param.power_ext.len = 0;
+                while (ppg_tlv->length) {
+                    data[0] = pg->first_rate_code;
+                    data[1] = pg->last_rate_code;
+                    if (pg->modulation_class == MOD_CLASS_OFDM) {
+                        data[0] += MLAN_RATE_INDEX_OFDM0;
+                        data[1] += MLAN_RATE_INDEX_OFDM0;
+                    } else if (pg->modulation_class == MOD_CLASS_HT) {
+                        data[0] += MLAN_RATE_INDEX_MCS0;
+                        data[1] += MLAN_RATE_INDEX_MCS0;
+                        if (pg->ht_bandwidth == HT_BW_40) {
+                            data[0] |= TX_RATE_HT_BW40_BIT;
+                            data[1] |= TX_RATE_HT_BW40_BIT;
+                        }
+                    }
+                    data[2] = pg->power_min;
+                    data[3] = pg->power_max;
+                    data[4] = pg->power_step;
+                    memcpy(pmpriv->adapter,
+                           (t_u8 *) (&power->param.power_ext.
+                                     power_data[power->param.power_ext.len]),
+                           (t_u8 *) data, sizeof(data));
+                    power->param.power_ext.len += 5;
+                    pg++;
+                    ppg_tlv->length -= sizeof(Power_Group_t);
+                }
+                pioctl_buf->data_read_written
+                    = sizeof(mlan_power_cfg_ext) + MLAN_SUB_COMMAND_SIZE;
+            }
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of rf_tx_power
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_802_11_rf_tx_power(IN pmlan_private pmpriv,
+                            IN HostCmd_DS_COMMAND * resp,
+                            IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_802_11_RF_TX_POWER *rtp = &resp->params.txp;
+    t_u16 action = wlan_le16_to_cpu(rtp->action);
+    mlan_ds_power_cfg *power = MNULL;
+
+    ENTER();
+
+    pmpriv->tx_power_level = wlan_le16_to_cpu(rtp->current_level);
+
+    if (action == HostCmd_ACT_GEN_GET) {
+        pmpriv->max_tx_power_level = rtp->max_power;
+        pmpriv->min_tx_power_level = rtp->min_power;
+        if (pioctl_buf) {
+            power = (mlan_ds_power_cfg *) pioctl_buf->pbuf;
+            if (power->sub_command == MLAN_OID_POWER_CFG) {
+                pioctl_buf->data_read_written
+                    = sizeof(mlan_power_cfg_t) + MLAN_SUB_COMMAND_SIZE;
+                power->param.power_cfg.power_level = pmpriv->tx_power_level;
+            }
+        }
+    }
+
+    PRINTM(MINFO, "Current TxPower Level = %d,Max Power=%d, Min Power=%d\n",
+           pmpriv->tx_power_level,
+           pmpriv->max_tx_power_level, pmpriv->min_tx_power_level);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of sleep_period
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_802_11_sleep_period(IN pmlan_private pmpriv,
+                             IN HostCmd_DS_COMMAND * resp,
+                             IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_802_11_SLEEP_PERIOD *pcmd_sleep_pd = &resp->params.sleep_pd;
+    mlan_ds_pm_cfg *pm_cfg = MNULL;
+    t_u16 sleep_pd = 0;
+
+    ENTER();
+
+    sleep_pd = wlan_le16_to_cpu(pcmd_sleep_pd->sleep_pd);
+    if (pioctl_buf) {
+        pm_cfg = (mlan_ds_pm_cfg *) pioctl_buf->pbuf;
+        pm_cfg->param.sleep_period = (t_u32) sleep_pd;
+        pioctl_buf->data_read_written = sizeof(pm_cfg->param.sleep_period)
+            + MLAN_SUB_COMMAND_SIZE;
+    }
+    pmpriv->adapter->sleep_period.period = sleep_pd;
+
+    pmpriv->adapter->pps_uapsd_mode = MFALSE;
+    if ((pmpriv->adapter->sleep_period.period != 0) &&
+        (pmpriv->adapter->sleep_period.period != SLEEP_PERIOD_RESERVED_FF)) {
+        pmpriv->adapter->gen_null_pkt = MTRUE;
+    } else {
+        pmpriv->adapter->delay_null_pkt = MFALSE;
+        pmpriv->adapter->gen_null_pkt = MFALSE;
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of sleep_params
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_802_11_sleep_params(IN pmlan_private pmpriv,
+                             IN HostCmd_DS_COMMAND * resp,
+                             IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_802_11_SLEEP_PARAMS *presp_sp = &resp->params.sleep_param;
+    mlan_ds_pm_cfg *pm_cfg = MNULL;
+    mlan_ds_sleep_params *psp = MNULL;
+    sleep_params_t *psleep_params = &pmpriv->adapter->sleep_params;
+
+    ENTER();
+
+    psleep_params->sp_reserved = wlan_le16_to_cpu(presp_sp->reserved);
+    psleep_params->sp_error = wlan_le16_to_cpu(presp_sp->error);
+    psleep_params->sp_offset = wlan_le16_to_cpu(presp_sp->offset);
+    psleep_params->sp_stable_time = wlan_le16_to_cpu(presp_sp->stable_time);
+    psleep_params->sp_cal_control = presp_sp->cal_control;
+    psleep_params->sp_ext_sleep_clk = presp_sp->external_sleep_clk;
+
+    if (pioctl_buf) {
+        pm_cfg = (mlan_ds_pm_cfg *) pioctl_buf->pbuf;
+        psp = (mlan_ds_sleep_params *) & pm_cfg->param.sleep_params;
+
+        psp->error = (t_u32) psleep_params->sp_error;
+        psp->offset = (t_u32) psleep_params->sp_offset;
+        psp->stable_time = (t_u32) psleep_params->sp_stable_time;
+        psp->cal_control = (t_u32) psleep_params->sp_cal_control;
+        psp->ext_sleep_clk = (t_u32) psleep_params->sp_ext_sleep_clk;
+        psp->reserved = (t_u32) psleep_params->sp_reserved;
+
+        pioctl_buf->data_read_written = sizeof(pm_cfg->param.sleep_params)
+            + MLAN_SUB_COMMAND_SIZE;
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of mac_address
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_802_11_mac_address(IN pmlan_private pmpriv,
+                            IN HostCmd_DS_COMMAND * resp,
+                            IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_802_11_MAC_ADDRESS *pmac_addr = &resp->params.mac_addr;
+    mlan_ds_bss *bss = MNULL;
+
+    ENTER();
+
+    memcpy(pmpriv->adapter, pmpriv->curr_addr, pmac_addr->mac_addr,
+           MLAN_MAC_ADDR_LENGTH);
+
+    PRINTM(MINFO, "MAC address: %02x:%02x:%02x:%02x:%02x:%02x\n",
+           pmpriv->curr_addr[0], pmpriv->curr_addr[1], pmpriv->curr_addr[2],
+           pmpriv->curr_addr[3], pmpriv->curr_addr[4], pmpriv->curr_addr[5]);
+    if (pioctl_buf) {
+        bss = (mlan_ds_bss *) pioctl_buf->pbuf;
+        memcpy(pmpriv->adapter, &bss->param.mac_addr, pmpriv->curr_addr,
+               MLAN_MAC_ADDR_LENGTH);
+        pioctl_buf->data_read_written =
+            MLAN_MAC_ADDR_LENGTH + MLAN_SUB_COMMAND_SIZE;
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of multicast_address
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_mac_multicast_adr(IN pmlan_private pmpriv,
+                           IN HostCmd_DS_COMMAND * resp,
+                           IN mlan_ioctl_req * pioctl_buf)
+{
+    ENTER();
+    if (pioctl_buf) {
+        pioctl_buf->data_read_written =
+            sizeof(mlan_multicast_list) + MLAN_SUB_COMMAND_SIZE;
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of deauthenticate
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_802_11_deauthenticate(IN pmlan_private pmpriv,
+                               IN HostCmd_DS_COMMAND * resp,
+                               IN mlan_ioctl_req * pioctl_buf)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    ENTER();
+
+    pmadapter->dbg.num_cmd_deauth++;
+
+    if (!memcmp(pmadapter, resp->params.deauth.mac_addr,
+                &pmpriv->curr_bss_params.bss_descriptor.mac_address,
+                sizeof(resp->params.deauth.mac_addr))) {
+        wlan_reset_connect_state(pmpriv, MTRUE);
+
+    }
+    if (pmpriv->adapter->state_rdh.stage == RDH_STOP_INTFS)
+        wlan_11h_radar_detected_callback((t_void *) pmpriv);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of ad_hoc_stop
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return        MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_802_11_ad_hoc_stop(IN pmlan_private pmpriv,
+                            IN HostCmd_DS_COMMAND * resp,
+                            IN mlan_ioctl_req * pioctl_buf)
+{
+    ENTER();
+
+    wlan_reset_connect_state(pmpriv, MTRUE);
+    if (pmpriv->adapter->state_rdh.stage == RDH_STOP_INTFS)
+        wlan_11h_radar_detected_callback((t_void *) pmpriv);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of key_material
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return        MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_802_11_key_material(IN pmlan_private pmpriv,
+                             IN HostCmd_DS_COMMAND * resp,
+                             IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_802_11_KEY_MATERIAL *pkey = &resp->params.key_material;
+    mlan_ds_sec_cfg *sec = MNULL;
+
+    ENTER();
+
+    if (wlan_le16_to_cpu(pkey->action) == HostCmd_ACT_GEN_SET) {
+        if ((wlan_le16_to_cpu(pkey->key_param_set.key_info) &
+             KEY_INFO_TKIP_MCAST)) {
+            PRINTM(MINFO, "key: GTK is set\n");
+            pmpriv->wpa_is_gtk_set = MTRUE;
+            if (pmpriv->port_ctrl_mode == MTRUE) {
+                /* GTK is set, open the port */
+                PRINTM(MINFO, "GTK_SET: Open port for WPA/WPA2 h-supp mode\n");
+                pmpriv->port_open = MTRUE;
+            }
+            pmpriv->scan_block = MFALSE;
+        }
+    } else {
+        if (pioctl_buf &&
+            (wlan_le16_to_cpu(pkey->key_param_set.type) ==
+             TLV_TYPE_KEY_MATERIAL)) {
+            PRINTM(MIOCTL, "key_type_id=%d, key_len=%d, key_info=0x%x\n",
+                   wlan_le16_to_cpu(pkey->key_param_set.key_type_id),
+                   wlan_le16_to_cpu(pkey->key_param_set.key_len),
+                   wlan_le16_to_cpu(pkey->key_param_set.key_info));
+            sec = (mlan_ds_sec_cfg *) pioctl_buf->pbuf;
+#define WAPI_KEY_SIZE 32
+            switch (wlan_le16_to_cpu(pkey->key_param_set.key_type_id)) {
+            case KEY_TYPE_ID_WEP:
+                sec->param.encrypt_key.key_index = pkey->key_param_set.key[0];
+                sec->param.encrypt_key.key_len =
+                    wlan_le16_to_cpu(pkey->key_param_set.key_len);
+                memcpy(pmpriv->adapter, sec->param.encrypt_key.key_material,
+                       &pkey->key_param_set.key[2],
+                       sec->param.encrypt_key.key_len);
+                break;
+            case KEY_TYPE_ID_TKIP:
+                sec->param.encrypt_key.key_len =
+                    wlan_le16_to_cpu(pkey->key_param_set.key_len);
+                memcpy(pmpriv->adapter, sec->param.encrypt_key.key_material,
+                       pkey->key_param_set.key, sec->param.encrypt_key.key_len);
+                break;
+            case KEY_TYPE_ID_AES:
+                sec->param.encrypt_key.key_len =
+                    wlan_le16_to_cpu(pkey->key_param_set.key_len);
+                memcpy(pmpriv->adapter, sec->param.encrypt_key.key_material,
+                       pkey->key_param_set.key, sec->param.encrypt_key.key_len);
+                break;
+            case KEY_TYPE_ID_WAPI:
+                sec->param.encrypt_key.is_wapi_key = MTRUE;
+                sec->param.encrypt_key.key_index = pkey->key_param_set.key[0];
+                sec->param.encrypt_key.key_len = WAPI_KEY_SIZE;
+                memcpy(pmpriv->adapter, sec->param.encrypt_key.key_material,
+                       &pkey->key_param_set.key[2],
+                       sec->param.encrypt_key.key_len);
+                memcpy(pmpriv->adapter, sec->param.encrypt_key.pn,
+                       &pkey->key_param_set.key[2 + WAPI_KEY_SIZE], PN_SIZE);
+                break;
+            }
+        }
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Handle the supplicant pmk response
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return        MLAN_STATUS_SUCCESS MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_ret_802_11_supplicant_pmk(IN pmlan_private pmpriv,
+                               IN HostCmd_DS_COMMAND * resp,
+                               IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_802_11_SUPPLICANT_PMK *supplicant_pmk_resp =
+        &resp->params.esupplicant_psk;
+    mlan_ds_sec_cfg sec_buf;
+    mlan_ds_sec_cfg *sec = MNULL;
+    MrvlIEtypes_PMK_t *ppmk_tlv = MNULL;
+    MrvlIEtypes_Passphrase_t *passphrase_tlv = MNULL;
+    MrvlIEtypes_SsIdParamSet_t *pssid_tlv = MNULL;
+    MrvlIEtypes_Bssid_t *pbssid_tlv = MNULL;
+    t_u8 *tlv_buf = (t_u8 *) supplicant_pmk_resp->tlv_buffer;
+    t_u16 action = wlan_le16_to_cpu(supplicant_pmk_resp->action);
+    int tlv_buf_len = 0;
+    t_u16 tlv;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    tlv_buf_len = resp->size - (sizeof(HostCmd_DS_802_11_SUPPLICANT_PMK) +
+                                S_DS_GEN - 1);
+    if (pioctl_buf) {
+        if (((mlan_ds_bss *) pioctl_buf->pbuf)->sub_command ==
+            MLAN_OID_BSS_FIND_BSS)
+            sec = &sec_buf;
+        else
+            sec = (mlan_ds_sec_cfg *) pioctl_buf->pbuf;
+        if (action == HostCmd_ACT_GEN_GET) {
+            while (tlv_buf_len > 0) {
+                tlv = (*tlv_buf) | (*(tlv_buf + 1) << 8);
+                if ((tlv != TLV_TYPE_SSID) && (tlv != TLV_TYPE_BSSID) &&
+                    (tlv != TLV_TYPE_PASSPHRASE)
+                    && (tlv != TLV_TYPE_PMK))
+                    break;
+                switch (tlv) {
+                case TLV_TYPE_SSID:
+                    pssid_tlv = (MrvlIEtypes_SsIdParamSet_t *) tlv_buf;
+                    pssid_tlv->header.len =
+                        wlan_le16_to_cpu(pssid_tlv->header.len);
+                    memcpy(pmpriv->adapter, sec->param.passphrase.ssid.ssid,
+                           pssid_tlv->ssid, MIN(MLAN_MAX_SSID_LENGTH,
+                                                pssid_tlv->header.len));
+                    sec->param.passphrase.ssid.ssid_len =
+                        MIN(MLAN_MAX_SSID_LENGTH, pssid_tlv->header.len);
+                    tlv_buf +=
+                        pssid_tlv->header.len + sizeof(MrvlIEtypesHeader_t);
+                    tlv_buf_len -=
+                        (pssid_tlv->header.len + sizeof(MrvlIEtypesHeader_t));
+                    break;
+                case TLV_TYPE_BSSID:
+                    pbssid_tlv = (MrvlIEtypes_Bssid_t *) tlv_buf;
+                    pbssid_tlv->header.len =
+                        wlan_le16_to_cpu(pbssid_tlv->header.len);
+                    memcpy(pmpriv->adapter, &sec->param.passphrase.bssid,
+                           pbssid_tlv->bssid, MLAN_MAC_ADDR_LENGTH);
+                    tlv_buf +=
+                        pbssid_tlv->header.len + sizeof(MrvlIEtypesHeader_t);
+                    tlv_buf_len -=
+                        (pbssid_tlv->header.len + sizeof(MrvlIEtypesHeader_t));
+                    break;
+                case TLV_TYPE_PASSPHRASE:
+                    passphrase_tlv = (MrvlIEtypes_Passphrase_t *) tlv_buf;
+                    passphrase_tlv->header.len =
+                        wlan_le16_to_cpu(passphrase_tlv->header.len);
+                    sec->param.passphrase.psk_type = MLAN_PSK_PASSPHRASE;
+                    sec->param.passphrase.psk.passphrase.passphrase_len =
+                        passphrase_tlv->header.len;
+                    memcpy(pmpriv->adapter,
+                           sec->param.passphrase.psk.passphrase.passphrase,
+                           passphrase_tlv->passphrase,
+                           MIN(MLAN_MAX_PASSPHRASE_LENGTH,
+                               passphrase_tlv->header.len));
+                    tlv_buf +=
+                        passphrase_tlv->header.len +
+                        sizeof(MrvlIEtypesHeader_t);
+                    tlv_buf_len -=
+                        (passphrase_tlv->header.len +
+                         sizeof(MrvlIEtypesHeader_t));
+                    break;
+                case TLV_TYPE_PMK:
+                    ppmk_tlv = (MrvlIEtypes_PMK_t *) tlv_buf;
+                    ppmk_tlv->header.len =
+                        wlan_le16_to_cpu(ppmk_tlv->header.len);
+                    sec->param.passphrase.psk_type = MLAN_PSK_PMK;
+                    memcpy(pmpriv->adapter, sec->param.passphrase.psk.pmk.pmk,
+                           ppmk_tlv->pmk, MIN(MLAN_MAX_KEY_LENGTH,
+                                              ppmk_tlv->header.len));
+                    tlv_buf +=
+                        ppmk_tlv->header.len + sizeof(MrvlIEtypesHeader_t);
+                    tlv_buf_len -=
+                        (ppmk_tlv->header.len + sizeof(MrvlIEtypesHeader_t));
+                    break;
+
+                }
+            }
+            if (((mlan_ds_bss *) pioctl_buf->pbuf)->sub_command ==
+                MLAN_OID_BSS_FIND_BSS) {
+                wlan_set_ewpa_mode(pmpriv, &sec->param.passphrase);
+                ret = wlan_find_bss(pmpriv, pioctl_buf);
+            }
+        } else if (action == HostCmd_ACT_GEN_SET) {
+            PRINTM(MINFO, "Esupp PMK set: enable ewpa query\n");
+            pmpriv->ewpa_query = MTRUE;
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Handle the supplicant profile response
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return        MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_802_11_supplicant_profile(IN pmlan_private pmpriv,
+                                   IN HostCmd_DS_COMMAND * resp,
+                                   IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_802_11_SUPPLICANT_PROFILE *psup_profile =
+        &resp->params.esupplicant_profile;
+    MrvlIEtypesHeader_t *head;
+    MrvlIEtypes_EncrProto_t *encr_proto_tlv = MNULL;
+    MrvlIEtypes_Cipher_t *pcipher_tlv = MNULL;
+    mlan_ds_sec_cfg *sec = MNULL;
+    t_u8 *tlv;
+    int len;
+
+    ENTER();
+
+    len = resp->size - S_DS_GEN - sizeof(t_u16);
+    tlv = psup_profile->tlv_buf;
+    if (pioctl_buf) {
+        sec = (mlan_ds_sec_cfg *) pioctl_buf->pbuf;
+        while (len > 0) {
+            head = (MrvlIEtypesHeader_t *) tlv;
+            head->type = wlan_le16_to_cpu(head->type);
+            head->len = wlan_le16_to_cpu(head->len);
+            switch (head->type) {
+            case TLV_TYPE_ENCRYPTION_PROTO:
+                encr_proto_tlv = (MrvlIEtypes_EncrProto_t *) head;
+                sec->param.esupp_mode.rsn_mode =
+                    wlan_le16_to_cpu(encr_proto_tlv->rsn_mode);
+                PRINTM(MINFO, "rsn_mode=0x%x\n",
+                       sec->param.esupp_mode.rsn_mode);
+                break;
+            case TLV_TYPE_CIPHER:
+                pcipher_tlv = (MrvlIEtypes_Cipher_t *) head;
+                sec->param.esupp_mode.act_paircipher = pcipher_tlv->pair_cipher;
+                sec->param.esupp_mode.act_groupcipher =
+                    pcipher_tlv->group_cipher;
+                PRINTM(MINFO, "paircipher=0x%x, groupcipher=0x%x\n",
+                       sec->param.esupp_mode.act_paircipher,
+                       sec->param.esupp_mode.act_groupcipher);
+                break;
+            }
+            len -= (head->len - sizeof(MrvlIEtypesHeader_t));
+            tlv = tlv + (head->len + sizeof(MrvlIEtypesHeader_t));
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of rf_channel
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_802_11_rf_channel(IN pmlan_private pmpriv,
+                           IN HostCmd_DS_COMMAND * resp,
+                           IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_802_11_RF_CHANNEL *prf_channel = &resp->params.rf_channel;
+    t_u16 new_channel = wlan_le16_to_cpu(prf_channel->current_channel);
+    mlan_ds_bss *bss = MNULL;
+    ENTER();
+    if (pmpriv->curr_bss_params.bss_descriptor.channel != new_channel) {
+        PRINTM(MINFO, "Channel Switch: %d to %d\n",
+               pmpriv->curr_bss_params.bss_descriptor.channel, new_channel);
+        /* Update the channel again */
+        pmpriv->curr_bss_params.bss_descriptor.channel = new_channel;
+    }
+    if (pioctl_buf) {
+        bss = (mlan_ds_bss *) pioctl_buf->pbuf;
+        bss->param.bss_chan.channel = new_channel;
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Handle the ibss_coalescing_status resp
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_ibss_coalescing_status(IN pmlan_private pmpriv,
+                                IN HostCmd_DS_COMMAND * resp)
+{
+    HostCmd_DS_802_11_IBSS_STATUS *pibss_coal_resp =
+        &(resp->params.ibss_coalescing);
+    t_u8 zero_mac[MLAN_MAC_ADDR_LENGTH] = { 0, 0, 0, 0, 0, 0 };
+
+    ENTER();
+
+    if (wlan_le16_to_cpu(pibss_coal_resp->action) == HostCmd_ACT_GEN_SET) {
+        LEAVE();
+        return MLAN_STATUS_SUCCESS;
+    }
+
+    PRINTM(MINFO, "New BSSID %02x:%02x:%02x:%02x:%02x:%02x\n",
+           pibss_coal_resp->bssid[0], pibss_coal_resp->bssid[1],
+           pibss_coal_resp->bssid[2], pibss_coal_resp->bssid[3],
+           pibss_coal_resp->bssid[4], pibss_coal_resp->bssid[5]);
+
+    /* If rsp has MNULL BSSID, Just return..... No Action */
+    if (!memcmp
+        (pmpriv->adapter, pibss_coal_resp->bssid, zero_mac,
+         MLAN_MAC_ADDR_LENGTH)) {
+        PRINTM(MMSG, "New BSSID is MNULL\n");
+        LEAVE();
+        return MLAN_STATUS_SUCCESS;
+    }
+
+    /* If BSSID is diff, modify current BSS parameters */
+    if (memcmp
+        (pmpriv->adapter, pmpriv->curr_bss_params.bss_descriptor.mac_address,
+         pibss_coal_resp->bssid, MLAN_MAC_ADDR_LENGTH)) {
+        /* BSSID */
+        memcpy(pmpriv->adapter,
+               pmpriv->curr_bss_params.bss_descriptor.mac_address,
+               pibss_coal_resp->bssid, MLAN_MAC_ADDR_LENGTH);
+
+        /* Beacon Interval and ATIM window */
+        pmpriv->curr_bss_params.bss_descriptor.beacon_period
+            = wlan_le16_to_cpu(pibss_coal_resp->beacon_interval);
+        pmpriv->curr_bss_params.bss_descriptor.atim_window
+            = wlan_le16_to_cpu(pibss_coal_resp->atim_window);
+
+        /* ERP Information */
+        pmpriv->curr_bss_params.bss_descriptor.erp_flags
+            = (t_u8) wlan_le16_to_cpu(pibss_coal_resp->use_g_rate_protect);
+
+        pmpriv->adhoc_state = ADHOC_COALESCED;
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of MGMT_IE_LIST
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_mgmt_ie_list(IN pmlan_private pmpriv,
+                      IN HostCmd_DS_COMMAND * resp,
+                      OUT mlan_ioctl_req * pioctl_buf)
+{
+    t_u16 resp_len = 0, travel_len = 0;
+    int i = 0;
+    mlan_ds_misc_cfg *misc = MNULL;
+    mlan_ds_misc_custom_ie *cust_ie = MNULL;
+    custom_ie *cptr;
+    tlvbuf_max_mgmt_ie *max_mgmt_ie = MNULL;
+    HostCmd_DS_MGMT_IE_LIST_CFG *pmgmt_ie_list = &(resp->params.mgmt_ie_list);
+
+    ENTER();
+
+    if (wlan_le16_to_cpu(pmgmt_ie_list->action) == HostCmd_ACT_GEN_SET) {
+        if ((pmpriv->adapter->state_rdh.stage == RDH_SET_CUSTOM_IE) ||
+            (pmpriv->adapter->state_rdh.stage == RDH_REM_CUSTOM_IE))
+            wlan_11h_radar_detected_callback((t_void *) pmpriv);
+        LEAVE();
+        return MLAN_STATUS_SUCCESS;
+    }
+
+    misc = (mlan_ds_misc_cfg *) pioctl_buf->pbuf;
+    cust_ie = (mlan_ds_misc_custom_ie *) & pmgmt_ie_list->ds_mgmt_ie;
+    max_mgmt_ie =
+        (tlvbuf_max_mgmt_ie *) ((t_u8 *) cust_ie + cust_ie->len +
+                                sizeof(MrvlIEtypesHeader_t));
+    if (cust_ie) {
+        cust_ie->type = wlan_le16_to_cpu(cust_ie->type);
+        resp_len = cust_ie->len = wlan_le16_to_cpu(cust_ie->len);
+        travel_len = 0;
+        /* conversion for index, mask, len */
+        if (resp_len == sizeof(t_u16))
+            cust_ie->ie_data_list[0].ie_index =
+                wlan_cpu_to_le16(cust_ie->ie_data_list[0].ie_index);
+
+        while (resp_len > sizeof(t_u16)) {
+            cptr =
+                (custom_ie *) (((t_u8 *) cust_ie->ie_data_list) + travel_len);
+            cptr->ie_index = wlan_le16_to_cpu(cptr->ie_index);
+            cptr->mgmt_subtype_mask = wlan_le16_to_cpu(cptr->mgmt_subtype_mask);
+            cptr->ie_length = wlan_le16_to_cpu(cptr->ie_length);
+            travel_len += cptr->ie_length + sizeof(custom_ie) - MAX_IE_SIZE;
+            resp_len -= cptr->ie_length + sizeof(custom_ie) - MAX_IE_SIZE;
+        }
+        memcpy(pmpriv->adapter, &misc->param.cust_ie, cust_ie,
+               (cust_ie->len + sizeof(MrvlIEtypesHeader_t)));
+        if (max_mgmt_ie) {
+            max_mgmt_ie->type = wlan_le16_to_cpu(max_mgmt_ie->type);
+            if (max_mgmt_ie->type == TLV_TYPE_MAX_MGMT_IE) {
+                max_mgmt_ie->len = wlan_le16_to_cpu(max_mgmt_ie->len);
+                max_mgmt_ie->count = wlan_le16_to_cpu(max_mgmt_ie->count);
+                for (i = 0; i < max_mgmt_ie->count; i++) {
+                    max_mgmt_ie->info[i].buf_size =
+                        wlan_le16_to_cpu(max_mgmt_ie->info[i].buf_size);
+                    max_mgmt_ie->info[i].buf_count =
+                        wlan_le16_to_cpu(max_mgmt_ie->info[i].buf_count);
+                }
+                /* Append max_mgmt_ie TLV after custom_ie */
+                memcpy(pmpriv->adapter,
+                       (t_u8 *) & misc->param.cust_ie + (cust_ie->len +
+                                                         sizeof
+                                                         (MrvlIEtypesHeader_t)),
+                       max_mgmt_ie,
+                       max_mgmt_ie->len + sizeof(MrvlIEtypesHeader_t));
+            }
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of sysclock
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return        MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_sysclock_cfg(IN pmlan_private pmpriv,
+                      IN HostCmd_DS_COMMAND * resp,
+                      IN mlan_ioctl_req * pioctl_buf)
+{
+    mlan_ds_misc_cfg *mis_ccfg = MNULL;
+    HostCmd_DS_ECL_SYSTEM_CLOCK_CONFIG *clk_cfg = &resp->params.sys_clock_cfg;
+    int i = 0;
+
+    ENTER();
+
+    if (pioctl_buf) {
+        mis_ccfg = (mlan_ds_misc_cfg *) pioctl_buf->pbuf;
+        mis_ccfg->param.sys_clock.cur_sys_clk =
+            wlan_le16_to_cpu(clk_cfg->cur_sys_clk);
+        mis_ccfg->param.sys_clock.sys_clk_type =
+            wlan_le16_to_cpu(clk_cfg->sys_clk_type);
+        mis_ccfg->param.sys_clock.sys_clk_num =
+            wlan_le16_to_cpu(clk_cfg->sys_clk_len) / sizeof(t_u16);
+        for (i = 0; i < mis_ccfg->param.sys_clock.sys_clk_num; i++)
+            mis_ccfg->param.sys_clock.sys_clk[i] =
+                wlan_le16_to_cpu(clk_cfg->sys_clk[i]);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of reg_access
+ *
+ *  @param type         The type of reg access (MAC, BBP or RF)
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to command buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_ret_reg_access(mlan_adapter * pmadapter,
+                    t_u16 type,
+                    IN HostCmd_DS_COMMAND * resp,
+                    IN mlan_ioctl_req * pioctl_buf)
+{
+    mlan_ds_reg_mem *reg_mem = MNULL;
+    mlan_ds_reg_rw *reg_rw = MNULL;
+
+    ENTER();
+
+    if (pioctl_buf) {
+        reg_mem = (mlan_ds_reg_mem *) pioctl_buf->pbuf;
+        reg_rw = &reg_mem->param.reg_rw;
+        switch (type) {
+        case HostCmd_CMD_MAC_REG_ACCESS:
+            {
+                HostCmd_DS_MAC_REG_ACCESS *reg;
+                reg = (HostCmd_DS_MAC_REG_ACCESS *) & resp->params.mac_reg;
+                reg_rw->offset = (t_u32) wlan_le16_to_cpu(reg->offset);
+                reg_rw->value = wlan_le32_to_cpu(reg->value);
+                break;
+            }
+        case HostCmd_CMD_BBP_REG_ACCESS:
+            {
+                HostCmd_DS_BBP_REG_ACCESS *reg;
+                reg = (HostCmd_DS_BBP_REG_ACCESS *) & resp->params.bbp_reg;
+                reg_rw->offset = (t_u32) wlan_le16_to_cpu(reg->offset);
+                reg_rw->value = (t_u32) reg->value;
+                break;
+            }
+
+        case HostCmd_CMD_RF_REG_ACCESS:
+            {
+                HostCmd_DS_RF_REG_ACCESS *reg;
+                reg = (HostCmd_DS_RF_REG_ACCESS *) & resp->params.rf_reg;
+                reg_rw->offset = (t_u32) wlan_le16_to_cpu(reg->offset);
+                reg_rw->value = (t_u32) reg->value;
+                break;
+            }
+        case HostCmd_CMD_CAU_REG_ACCESS:
+            {
+                HostCmd_DS_RF_REG_ACCESS *reg;
+                reg = (HostCmd_DS_RF_REG_ACCESS *) & resp->params.rf_reg;
+                reg_rw->offset = (t_u32) wlan_le16_to_cpu(reg->offset);
+                reg_rw->value = (t_u32) reg->value;
+                break;
+            }
+        case HostCmd_CMD_802_11_EEPROM_ACCESS:
+            {
+                mlan_ds_read_eeprom *eeprom = &reg_mem->param.rd_eeprom;
+                HostCmd_DS_802_11_EEPROM_ACCESS *cmd_eeprom =
+                    (HostCmd_DS_802_11_EEPROM_ACCESS *) & resp->params.eeprom;
+                cmd_eeprom->byte_count =
+                    wlan_le16_to_cpu(cmd_eeprom->byte_count);
+                PRINTM(MINFO, "EEPROM read len=%x\n", cmd_eeprom->byte_count);
+                if (eeprom->byte_count < cmd_eeprom->byte_count) {
+                    eeprom->byte_count = 0;
+                    PRINTM(MINFO, "EEPROM read return length is too big\n");
+                    pioctl_buf->status_code = MLAN_ERROR_CMD_RESP_FAIL;
+                    LEAVE();
+                    return MLAN_STATUS_FAILURE;
+                }
+                eeprom->offset = wlan_le16_to_cpu(cmd_eeprom->offset);
+                eeprom->byte_count = cmd_eeprom->byte_count;
+                if (eeprom->byte_count > 0) {
+                    memcpy(pmadapter, &eeprom->value, &cmd_eeprom->value,
+                           MIN(MAX_EEPROM_DATA, eeprom->byte_count));
+                    HEXDUMP("EEPROM", (char *) &eeprom->value,
+                            MIN(MAX_EEPROM_DATA, eeprom->byte_count));
+                }
+                break;
+            }
+        default:
+            pioctl_buf->status_code = MLAN_ERROR_CMD_RESP_FAIL;
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of mem_access
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to command buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_mem_access(IN pmlan_private pmpriv,
+                    IN HostCmd_DS_COMMAND * resp,
+                    IN mlan_ioctl_req * pioctl_buf)
+{
+    mlan_ds_reg_mem *reg_mem = MNULL;
+    mlan_ds_mem_rw *mem_rw = MNULL;
+    HostCmd_DS_MEM_ACCESS *mem = (HostCmd_DS_MEM_ACCESS *) & resp->params.mem;
+
+    ENTER();
+
+    if (pioctl_buf) {
+        reg_mem = (mlan_ds_reg_mem *) pioctl_buf->pbuf;
+        mem_rw = &reg_mem->param.mem_rw;
+
+        mem_rw->addr = wlan_le32_to_cpu(mem->addr);
+        mem_rw->value = wlan_le32_to_cpu(mem->value);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of inactivity timeout
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp	        A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to command buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_inactivity_timeout(IN pmlan_private pmpriv,
+                            IN HostCmd_DS_COMMAND * resp,
+                            IN mlan_ioctl_req * pioctl_buf)
+{
+    mlan_ds_pm_cfg *pmcfg = MNULL;
+    mlan_ds_inactivity_to *inac_to = MNULL;
+    HostCmd_DS_INACTIVITY_TIMEOUT_EXT *cmd_inac_to =
+        (HostCmd_DS_INACTIVITY_TIMEOUT_EXT *) & resp->params.inactivity_to;
+
+    ENTER();
+
+    if (pioctl_buf) {
+        pmcfg = (mlan_ds_pm_cfg *) pioctl_buf->pbuf;
+        inac_to = &pmcfg->param.inactivity_to;
+        inac_to->timeout_unit = wlan_le16_to_cpu(cmd_inac_to->timeout_unit);
+        inac_to->unicast_timeout =
+            wlan_le16_to_cpu(cmd_inac_to->unicast_timeout);
+        inac_to->mcast_timeout = wlan_le16_to_cpu(cmd_inac_to->mcast_timeout);
+        inac_to->ps_entry_timeout =
+            wlan_le16_to_cpu(cmd_inac_to->ps_entry_timeout);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of
+ *  subscribe event
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to command buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_subscribe_event(IN pmlan_private pmpriv,
+                         IN HostCmd_DS_COMMAND * resp,
+                         IN mlan_ioctl_req * pioctl_buf)
+{
+
+    HostCmd_DS_SUBSCRIBE_EVENT *evt =
+        (HostCmd_DS_SUBSCRIBE_EVENT *) & resp->params.subscribe_event;
+    mlan_ds_subscribe_evt *sub_evt = MNULL;
+    mlan_ds_misc_cfg *misc = MNULL;
+
+    ENTER();
+    if (pioctl_buf && (pioctl_buf->action == MLAN_ACT_GET)) {
+        misc = (mlan_ds_misc_cfg *) pioctl_buf->pbuf;
+        sub_evt = &misc->param.subscribe_event;
+        sub_evt->evt_bitmap = wlan_le16_to_cpu(evt->event_bitmap);
+        pioctl_buf->data_read_written = sizeof(mlan_ds_misc_cfg);
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of
+ *  OTP user data
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to command buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_ret_otp_user_data(IN pmlan_private pmpriv,
+                       IN HostCmd_DS_COMMAND * resp,
+                       IN mlan_ioctl_req * pioctl_buf)
+{
+
+    HostCmd_DS_OTP_USER_DATA *cmd_user_data =
+        (HostCmd_DS_OTP_USER_DATA *) & resp->params.otp_user_data;
+    mlan_ds_misc_otp_user_data *user_data = MNULL;
+
+    ENTER();
+    if (pioctl_buf && (pioctl_buf->action == MLAN_ACT_GET)) {
+        user_data = (mlan_ds_misc_otp_user_data *) pioctl_buf->pbuf;
+        user_data->user_data_length = MIN(MAX_OTP_USER_DATA_LEN,
+                                          wlan_le16_to_cpu(cmd_user_data->
+                                                           user_data_length));
+        memcpy(pmpriv->adapter, user_data->user_data, cmd_user_data->user_data,
+               user_data->user_data_length);
+        pioctl_buf->data_read_written =
+            sizeof(mlan_ds_misc_otp_user_data) + user_data->user_data_length;
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/********************************************************
+                Global Functions
+********************************************************/
+/**
+ *  @brief This function handles the station command response
+ *
+ *  @param priv             A pointer to mlan_private structure
+ *  @param cmdresp_no       cmd no
+ *  @param pcmd_buf         cmdresp buf
+ *  @param pioctl           A pointer to ioctl buf
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ops_sta_process_cmdresp(IN t_void * priv,
+                             IN t_u16 cmdresp_no,
+                             IN t_void * pcmd_buf, IN t_void * pioctl)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = (mlan_private *) priv;
+    HostCmd_DS_COMMAND *resp = (HostCmd_DS_COMMAND *) pcmd_buf;
+    mlan_ioctl_req *pioctl_buf = (mlan_ioctl_req *) pioctl;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    int ctr;
+
+    ENTER();
+
+    /* If the command is not successful, cleanup and return failure */
+    if ((resp->result != HostCmd_RESULT_OK)
+        ) {
+        wlan_process_cmdresp_error(pmpriv, resp, pioctl_buf);
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    /* Command successful, handle response */
+    switch (cmdresp_no) {
+    case HostCmd_CMD_GET_HW_SPEC:
+        ret = wlan_ret_get_hw_spec(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_CFG_DATA:
+        ret = wlan_ret_cfg_data(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_MAC_CONTROL:
+        ret = wlan_ret_mac_control(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_MAC_ADDRESS:
+        ret = wlan_ret_802_11_mac_address(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_MAC_MULTICAST_ADR:
+        ret = wlan_ret_mac_multicast_adr(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_TX_RATE_CFG:
+        ret = wlan_ret_tx_rate_cfg(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_SCAN:
+        ret = wlan_ret_802_11_scan(pmpriv, resp, pioctl_buf);
+        pioctl_buf = MNULL;
+        pmadapter->curr_cmd->pioctl_buf = MNULL;
+        break;
+    case HostCmd_CMD_802_11_SCAN_EXT:
+        ret = wlan_ret_802_11_scan_ext(pmpriv, resp, pioctl_buf);
+        pioctl_buf = MNULL;
+        pmadapter->curr_cmd->pioctl_buf = MNULL;
+        break;
+    case HostCmd_CMD_802_11_BG_SCAN_CONFIG:
+        ret = wlan_ret_bgscan_config(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_BG_SCAN_QUERY:
+        ret = wlan_ret_802_11_bgscan_query(pmpriv, resp, pioctl_buf);
+        PRINTM(MINFO, "CMD_RESP: BG_SCAN result is ready!\n");
+        break;
+    case HostCmd_CMD_TXPWR_CFG:
+        ret = wlan_ret_tx_power_cfg(pmpriv, resp, pioctl_buf);
+        break;
+
+    case HostCmd_CMD_802_11_RF_TX_POWER:
+        ret = wlan_ret_802_11_rf_tx_power(pmpriv, resp, pioctl_buf);
+        break;
+
+    case HostCmd_CMD_802_11_PS_MODE_ENH:
+        ret = wlan_ret_enh_power_mode(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_HS_CFG_ENH:
+        ret = wlan_ret_802_11_hs_cfg(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_SLEEP_PERIOD:
+        ret = wlan_ret_802_11_sleep_period(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_SLEEP_PARAMS:
+        ret = wlan_ret_802_11_sleep_params(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_ASSOCIATE:
+        ret = wlan_ret_802_11_associate(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_DEAUTHENTICATE:
+        ret = wlan_ret_802_11_deauthenticate(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_AD_HOC_START:
+    case HostCmd_CMD_802_11_AD_HOC_JOIN:
+        ret = wlan_ret_802_11_ad_hoc(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_AD_HOC_STOP:
+        ret = wlan_ret_802_11_ad_hoc_stop(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_GET_LOG:
+        ret = wlan_ret_get_log(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_RSSI_INFO:
+        ret = wlan_ret_802_11_rssi_info(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_SNMP_MIB:
+        ret = wlan_ret_802_11_snmp_mib(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_RADIO_CONTROL:
+        ret = wlan_ret_802_11_radio_control(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_TX_RATE_QUERY:
+        ret = wlan_ret_802_11_tx_rate_query(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_RF_CHANNEL:
+        ret = wlan_ret_802_11_rf_channel(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_RF_ANTENNA:
+        ret = wlan_ret_802_11_rf_antenna(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_VERSION_EXT:
+        ret = wlan_ret_ver_ext(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_RX_MGMT_IND:
+        ret = wlan_ret_rx_mgmt_ind(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_FUNC_INIT:
+    case HostCmd_CMD_FUNC_SHUTDOWN:
+        break;
+    case HostCmd_CMD_802_11_KEY_MATERIAL:
+        ret = wlan_ret_802_11_key_material(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_SUPPLICANT_PMK:
+        ret = wlan_ret_802_11_supplicant_pmk(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_SUPPLICANT_PROFILE:
+        ret = wlan_ret_802_11_supplicant_profile(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11D_DOMAIN_INFO:
+        ret = wlan_ret_802_11d_domain_info(pmpriv, resp);
+        break;
+    case HostCmd_CMD_802_11_TPC_ADAPT_REQ:
+    case HostCmd_CMD_802_11_TPC_INFO:
+    case HostCmd_CMD_802_11_CHAN_SW_ANN:
+    case HostCmd_CMD_CHAN_REPORT_REQUEST:
+        ret = wlan_11h_cmdresp_process(pmpriv, resp);
+        break;
+    case HostCmd_CMD_11N_ADDBA_REQ:
+        ret = wlan_ret_11n_addba_req(pmpriv, resp);
+        break;
+    case HostCmd_CMD_11N_DELBA:
+        ret = wlan_ret_11n_delba(pmpriv, resp);
+        break;
+    case HostCmd_CMD_11N_ADDBA_RSP:
+        ret = wlan_ret_11n_addba_resp(pmpriv, resp);
+        break;
+    case HostCmd_CMD_RECONFIGURE_TX_BUFF:
+        pmadapter->tx_buf_size =
+            (t_u16) wlan_le16_to_cpu(resp->params.tx_buf.buff_size);
+        pmadapter->tx_buf_size =
+            (pmadapter->tx_buf_size / MLAN_SDIO_BLOCK_SIZE) *
+            MLAN_SDIO_BLOCK_SIZE;
+        pmadapter->curr_tx_buf_size = pmadapter->tx_buf_size;
+        pmadapter->mp_end_port =
+            wlan_le16_to_cpu(resp->params.tx_buf.mp_end_port);
+        pmadapter->mp_data_port_mask = DATA_PORT_MASK;
+
+        for (ctr = 1; ctr <= MAX_PORT - pmadapter->mp_end_port; ctr++) {
+            pmadapter->mp_data_port_mask &= ~(1 << (MAX_PORT - ctr));
+        }
+
+        pmadapter->curr_wr_port = 1;
+        PRINTM(MCMND, "end port %d, data port mask %x\n",
+               wlan_le16_to_cpu(resp->params.tx_buf.mp_end_port),
+               pmadapter->mp_data_port_mask);
+        PRINTM(MCMND, "max_tx_buf_size=%d, tx_buf_size=%d\n",
+               pmadapter->max_tx_buf_size, pmadapter->tx_buf_size);
+        break;
+    case HostCmd_CMD_AMSDU_AGGR_CTRL:
+        ret = wlan_ret_amsdu_aggr_ctrl(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_WMM_GET_STATUS:
+        ret = wlan_ret_wmm_get_status(pmpriv,
+                                      resp->params.get_wmm_status.
+                                      queue_status_tlv, resp->size - S_DS_GEN);
+        break;
+    case HostCmd_CMD_WMM_ADDTS_REQ:
+        ret = wlan_ret_wmm_addts_req(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_WMM_DELTS_REQ:
+        ret = wlan_ret_wmm_delts_req(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_WMM_QUEUE_CONFIG:
+        ret = wlan_ret_wmm_queue_config(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_WMM_QUEUE_STATS:
+        ret = wlan_ret_wmm_queue_stats(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_WMM_TS_STATUS:
+        ret = wlan_ret_wmm_ts_status(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_IBSS_COALESCING_STATUS:
+        ret = wlan_ret_ibss_coalescing_status(pmpriv, resp);
+        break;
+    case HostCmd_CMD_MGMT_IE_LIST:
+        ret = wlan_ret_mgmt_ie_list(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_11N_CFG:
+        ret = wlan_ret_11n_cfg(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_TX_BF_CFG:
+        ret = wlan_ret_tx_bf_cfg(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_ECL_SYSTEM_CLOCK_CONFIG:
+        ret = wlan_ret_sysclock_cfg(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_MAC_REG_ACCESS:
+    case HostCmd_CMD_BBP_REG_ACCESS:
+    case HostCmd_CMD_RF_REG_ACCESS:
+    case HostCmd_CMD_CAU_REG_ACCESS:
+    case HostCmd_CMD_802_11_EEPROM_ACCESS:
+        ret =
+            wlan_ret_reg_access(pmpriv->adapter, cmdresp_no, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_MEM_ACCESS:
+        ret = wlan_ret_mem_access(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_INACTIVITY_TIMEOUT_EXT:
+        ret = wlan_ret_inactivity_timeout(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_SDIO_GPIO_INT_CONFIG:
+        break;
+    case HostCmd_CMD_SET_BSS_MODE:
+        break;
+    case HostCmd_CMD_MEASUREMENT_REQUEST:
+    case HostCmd_CMD_MEASUREMENT_REPORT:
+        ret = wlan_meas_cmdresp_process(pmpriv, resp);
+        break;
+#ifdef WIFI_DIRECT_SUPPORT
+    case HostCmd_CMD_802_11_REMAIN_ON_CHANNEL:
+        ret = wlan_ret_remain_on_channel(pmpriv, resp, pioctl_buf);
+        break;
+    case HOST_CMD_WIFI_DIRECT_MODE_CONFIG:
+        ret = wlan_ret_wifi_direct_mode(pmpriv, resp, pioctl_buf);
+        break;
+#endif
+    case HostCmd_CMD_802_11_SUBSCRIBE_EVENT:
+        ret = wlan_ret_subscribe_event(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_OTP_READ_USER_DATA:
+        ret = wlan_ret_otp_user_data(pmpriv, resp, pioctl_buf);
+        break;
+    default:
+        PRINTM(MERROR, "CMD_RESP: Unknown command response %#x\n",
+               resp->command);
+        break;
+    }
+
+    LEAVE();
+    return ret;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_sta_event.c b/drivers/net/wireless/sd8797/mlan/mlan_sta_event.c
new file mode 100644
index 000000000000..1ad77818995f
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_sta_event.c
@@ -0,0 +1,589 @@
+/** @file mlan_sta_event.c
+ *
+ *  @brief This file contains MLAN event handling.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/********************************************************
+Change log:
+    10/13/2008: initial version
+********************************************************/
+
+#include "mlan.h"
+#include "mlan_join.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_wmm.h"
+#include "mlan_11n.h"
+#include "mlan_11h.h"
+
+/********************************************************
+        Global Variables
+********************************************************/
+
+/********************************************************
+        Local Functions
+********************************************************/
+
+/**
+ *  @brief This function handles link lost, deauth and
+ *  disassoc events.
+ *
+ *  @param priv    A pointer to mlan_private structure
+ *  @return        N/A
+ */
+static t_void
+wlan_handle_disconnect_event(pmlan_private pmpriv)
+{
+    ENTER();
+
+    if (pmpriv->media_connected == MTRUE) {
+        wlan_reset_connect_state(pmpriv, MTRUE);
+    }
+
+    LEAVE();
+}
+
+/********************************************************
+        Global Functions
+********************************************************/
+/**
+ *  @brief This function handles disconnect event, reports disconnect
+ *  to upper layer, cleans tx/rx packets,
+ *  resets link state etc.
+ *
+ *  @param priv            A pointer to mlan_private structure
+ *  @param drv_disconnect  Flag indicating the driver should disconnect
+ *                         and flush pending packets.
+ *
+ *  @return        N/A
+ */
+t_void
+wlan_reset_connect_state(pmlan_private priv, t_u8 drv_disconnect)
+{
+    mlan_adapter *pmadapter = priv->adapter;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    state_11d_t enable;
+
+    ENTER();
+
+    if (priv->media_connected != MTRUE) {
+        LEAVE();
+        return;
+    }
+
+    PRINTM(MINFO, "Handles disconnect event.\n");
+
+    if (drv_disconnect) {
+        priv->media_connected = MFALSE;
+        wlan_11h_check_update_radar_det_state(priv);
+    }
+
+    if (priv->port_ctrl_mode == MTRUE) {
+        /* Close the port on Disconnect */
+        PRINTM(MINFO, "DISC: port_status = CLOSED\n");
+        priv->port_open = MFALSE;
+    }
+    priv->scan_block = MFALSE;
+
+    /* Reset SNR/NF/RSSI values */
+    priv->data_rssi_last = 0;
+    priv->data_nf_last = 0;
+    priv->data_rssi_avg = 0;
+    priv->data_nf_avg = 0;
+    priv->bcn_rssi_last = 0;
+    priv->bcn_nf_last = 0;
+    priv->bcn_rssi_avg = 0;
+    priv->bcn_nf_avg = 0;
+    priv->rxpd_rate = 0;
+    priv->rxpd_htinfo = 0;
+    priv->max_amsdu = 0;
+
+    priv->sec_info.ewpa_enabled = MFALSE;
+    priv->sec_info.wpa_enabled = MFALSE;
+    priv->sec_info.wpa2_enabled = MFALSE;
+    priv->wpa_ie_len = 0;
+
+    priv->sec_info.wapi_enabled = MFALSE;
+    priv->wapi_ie_len = 0;
+    priv->sec_info.wapi_key_on = MFALSE;
+
+    priv->wps.session_enable = MFALSE;
+    memset(priv->adapter, (t_u8 *) & priv->wps.wps_ie, 0x00,
+           sizeof(priv->wps.wps_ie));
+
+    priv->sec_info.encryption_mode = MLAN_ENCRYPTION_MODE_NONE;
+
+    /* Enable auto data rate */
+    priv->is_data_rate_auto = MTRUE;
+    priv->data_rate = 0;
+
+    if (priv->bss_mode == MLAN_BSS_MODE_IBSS) {
+        priv->adhoc_state = ADHOC_IDLE;
+        priv->adhoc_is_link_sensed = MFALSE;
+        priv->intf_state_11h.adhoc_auto_sel_chan = MTRUE;
+    }
+
+    if (drv_disconnect) {
+        /* Free Tx and Rx packets, report disconnect to upper layer */
+        wlan_clean_txrx(priv);
+
+        /* Need to erase the current SSID and BSSID info */
+        memset(pmadapter,
+               &priv->curr_bss_params, 0x00, sizeof(priv->curr_bss_params));
+    }
+    pmadapter->tx_lock_flag = MFALSE;
+    pmadapter->pps_uapsd_mode = MFALSE;
+    pmadapter->delay_null_pkt = MFALSE;
+
+    if ((wlan_11d_is_enabled(priv)) &&
+        (priv->state_11d.user_enable_11d == DISABLE_11D)) {
+
+        priv->state_11d.enable_11d = DISABLE_11D;
+        enable = DISABLE_11D;
+
+        /* Send cmd to FW to enable/disable 11D function */
+        ret = wlan_prepare_cmd(priv,
+                               HostCmd_CMD_802_11_SNMP_MIB,
+                               HostCmd_ACT_GEN_SET, Dot11D_i, MNULL, &enable);
+        if (ret)
+            PRINTM(MERROR, "11D: Failed to enable 11D\n");
+    }
+    if (pmadapter->num_cmd_timeout && pmadapter->curr_cmd &&
+        (pmadapter->cmd_timer_is_set == MFALSE)) {
+        LEAVE();
+        return;
+    }
+
+    wlan_recv_event(priv, MLAN_EVENT_ID_FW_DISCONNECTED, MNULL);
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function sends the OBSS scan parameters to the application
+ *
+ *  @param pmpriv     A pointer to mlan_private structure
+ *
+ *  @return           N/A
+ */
+t_void
+wlan_2040_coex_event(pmlan_private pmpriv)
+{
+    t_u8 event_buf[100];
+    mlan_event *pevent = (mlan_event *) event_buf;
+    t_u8 ele_len;
+
+    ENTER();
+
+    if (pmpriv->curr_bss_params.bss_descriptor.poverlap_bss_scan_param &&
+        pmpriv->curr_bss_params.bss_descriptor.poverlap_bss_scan_param->
+        ieee_hdr.element_id == OVERLAPBSSSCANPARAM) {
+        ele_len =
+            pmpriv->curr_bss_params.bss_descriptor.poverlap_bss_scan_param->
+            ieee_hdr.len;
+        pevent->bss_index = pmpriv->bss_index;
+        pevent->event_id = MLAN_EVENT_ID_DRV_OBSS_SCAN_PARAM;
+        /* Copy OBSS scan parameters */
+        memcpy(pmpriv->adapter, (t_u8 *) pevent->event_buf,
+               (t_u8 *) & pmpriv->curr_bss_params.bss_descriptor.
+               poverlap_bss_scan_param->obss_scan_param, ele_len);
+        pevent->event_len = ele_len;
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_DRV_OBSS_SCAN_PARAM, pevent);
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function handles events generated by firmware
+ *
+ *  @param priv	A pointer to mlan_private structure
+ *
+ *  @return		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ops_sta_process_event(IN t_void * priv)
+{
+    pmlan_private pmpriv = (pmlan_private) priv;
+    pmlan_adapter pmadapter = pmpriv->adapter;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u32 eventcause = pmadapter->event_cause;
+    t_u8 event_buf[100];
+    t_u8 *evt_buf = MNULL;
+    pmlan_buffer pmbuf = pmadapter->pmlan_buffer_event;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    mlan_event *pevent = (mlan_event *) event_buf;
+
+    ENTER();
+
+    if (eventcause != EVENT_PS_SLEEP && eventcause != EVENT_PS_AWAKE &&
+        pmbuf->data_len > sizeof(eventcause))
+        DBG_HEXDUMP(MEVT_D, "EVENT", pmbuf->pbuf + pmbuf->data_offset,
+                    pmbuf->data_len);
+
+    switch (eventcause) {
+    case EVENT_DUMMY_HOST_WAKEUP_SIGNAL:
+        PRINTM(MERROR,
+               "Invalid EVENT: DUMMY_HOST_WAKEUP_SIGNAL, ignoring it\n");
+        break;
+    case EVENT_LINK_SENSED:
+        PRINTM(MEVENT, "EVENT: LINK_SENSED\n");
+        pmpriv->adhoc_is_link_sensed = MTRUE;
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_ADHOC_LINK_SENSED, MNULL);
+        break;
+
+    case EVENT_DEAUTHENTICATED:
+        PRINTM(MEVENT, "EVENT: Deauthenticated\n");
+        pmadapter->dbg.num_event_deauth++;
+        wlan_handle_disconnect_event(pmpriv);
+
+        break;
+
+    case EVENT_DISASSOCIATED:
+        PRINTM(MEVENT, "EVENT: Disassociated\n");
+        pmadapter->dbg.num_event_disassoc++;
+        wlan_handle_disconnect_event(pmpriv);
+        break;
+
+    case EVENT_LINK_LOST:
+        PRINTM(MEVENT, "EVENT: Link lost\n");
+        pmadapter->dbg.num_event_link_lost++;
+        wlan_handle_disconnect_event(pmpriv);
+        break;
+
+    case EVENT_PS_SLEEP:
+        PRINTM(MINFO, "EVENT: SLEEP\n");
+        PRINTM(MEVENT, "_");
+
+        /* Handle unexpected PS SLEEP event */
+        if (pmadapter->ps_state == PS_STATE_SLEEP_CFM)
+            break;
+        pmadapter->ps_state = PS_STATE_PRE_SLEEP;
+
+        wlan_check_ps_cond(pmadapter);
+        break;
+
+    case EVENT_PS_AWAKE:
+        PRINTM(MINFO, "EVENT: AWAKE \n");
+        PRINTM(MEVENT, "|");
+        if (!pmadapter->pps_uapsd_mode &&
+            pmpriv->media_connected &&
+            (pmpriv->port_open || !pmpriv->port_ctrl_mode) &&
+            pmadapter->sleep_period.period) {
+            pmadapter->pps_uapsd_mode = MTRUE;
+            PRINTM(MEVENT, "PPS/UAPSD mode activated\n");
+        }
+        /* Handle unexpected PS AWAKE event */
+        if (pmadapter->ps_state == PS_STATE_SLEEP_CFM)
+            break;
+        pmadapter->tx_lock_flag = MFALSE;
+        if (pmadapter->pps_uapsd_mode && pmadapter->gen_null_pkt) {
+            if (MTRUE == wlan_check_last_packet_indication(pmpriv)) {
+                if (!pmadapter->data_sent) {
+                    if (wlan_send_null_packet(pmpriv,
+                                              MRVDRV_TxPD_POWER_MGMT_NULL_PACKET
+                                              |
+                                              MRVDRV_TxPD_POWER_MGMT_LAST_PACKET)
+                        == MLAN_STATUS_SUCCESS) {
+                        LEAVE();
+                        return MLAN_STATUS_SUCCESS;
+                    }
+                }
+            }
+        }
+        pmadapter->ps_state = PS_STATE_AWAKE;
+        pmadapter->pm_wakeup_card_req = MFALSE;
+        pmadapter->pm_wakeup_fw_try = MFALSE;
+        break;
+
+    case EVENT_HS_ACT_REQ:
+        PRINTM(MEVENT, "EVENT: HS_ACT_REQ\n");
+        ret = wlan_prepare_cmd(priv,
+                               HostCmd_CMD_802_11_HS_CFG_ENH,
+                               0, 0, MNULL, MNULL);
+        break;
+
+    case EVENT_MIC_ERR_UNICAST:
+        PRINTM(MEVENT, "EVENT: UNICAST MIC ERROR\n");
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_MIC_ERR_UNI, MNULL);
+        break;
+
+    case EVENT_MIC_ERR_MULTICAST:
+        PRINTM(MEVENT, "EVENT: MULTICAST MIC ERROR\n");
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_MIC_ERR_MUL, MNULL);
+        break;
+    case EVENT_MIB_CHANGED:
+    case EVENT_INIT_DONE:
+        break;
+
+    case EVENT_ADHOC_BCN_LOST:
+        PRINTM(MEVENT, "EVENT: ADHOC_BCN_LOST\n");
+        pmpriv->adhoc_is_link_sensed = MFALSE;
+        wlan_clean_txrx(pmpriv);
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_ADHOC_LINK_LOST, MNULL);
+        break;
+
+    case EVENT_BG_SCAN_REPORT:
+        PRINTM(MEVENT, "EVENT: BGS_REPORT\n");
+        pmadapter->bgscan_reported = MTRUE;
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_BG_SCAN, MNULL);
+        break;
+
+    case EVENT_PORT_RELEASE:
+        PRINTM(MEVENT, "EVENT: PORT RELEASE\n");
+        /* Open the port for e-supp mode */
+        if (pmpriv->port_ctrl_mode == MTRUE) {
+            PRINTM(MINFO, "PORT_REL: port_status = OPEN\n");
+            pmpriv->port_open = MTRUE;
+        }
+        pmpriv->scan_block = MFALSE;
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_PORT_RELEASE, MNULL);
+        break;
+
+    case EVENT_STOP_TX:
+        PRINTM(MEVENT, "EVENT: Stop Tx (%#x)\n", eventcause);
+        wlan_11h_tx_disable(pmpriv);    // this fn will send event up to MOAL
+        break;
+    case EVENT_START_TX:
+        PRINTM(MEVENT, "EVENT: Start Tx (%#x)\n", eventcause);
+        wlan_11h_tx_enable(pmpriv);     // this fn will send event up to MOAL
+        break;
+    case EVENT_CHANNEL_SWITCH:
+        PRINTM(MEVENT, "EVENT: Channel Switch (%#x)\n", eventcause);
+        /* To be handled for 'chanswann' private command */
+        break;
+    case EVENT_CHANNEL_SWITCH_ANN:
+        PRINTM(MEVENT, "EVENT: Channel Switch Announcement\n");
+        /* Here, pass up event first, as handling will send deauth */
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_CHANNEL_SWITCH_ANN, MNULL);
+        wlan_11h_handle_event_chanswann(pmpriv);
+        break;
+    case EVENT_RADAR_DETECTED:
+        PRINTM(MEVENT, "EVENT: Radar Detected\n");
+
+        /* Send as passthru first, this event can cause other events */
+        memset(pmadapter, pevent, 0x00, sizeof(event_buf));
+        pevent->bss_index = pmpriv->bss_index;
+        pevent->event_id = MLAN_EVENT_ID_DRV_PASSTHRU;
+        pevent->event_len = pmbuf->data_len;
+        memcpy(pmadapter, (t_u8 *) pevent->event_buf,
+               pmbuf->pbuf + pmbuf->data_offset, pevent->event_len);
+        wlan_recv_event(pmpriv, pevent->event_id, pevent);
+
+        if (pmadapter->state_rdh.stage == RDH_OFF) {
+            pmadapter->state_rdh.stage = RDH_CHK_INTFS;
+            wlan_11h_radar_detected_handling(pmadapter);
+        } else {
+            PRINTM(MEVENT, "Ignore Event Radar Detected - handling"
+                   " already in progress.\n");
+        }
+
+        break;
+
+    case EVENT_CHANNEL_REPORT_RDY:
+        PRINTM(MEVENT, "EVENT: Channel Report Ready\n");
+        /* Allocate memory for event buffer */
+        ret = pcb->moal_malloc(pmadapter->pmoal_handle,
+                               MAX_EVENT_SIZE, MLAN_MEM_DEF, &evt_buf);
+        if ((ret == MLAN_STATUS_SUCCESS) && evt_buf) {
+            memset(pmadapter, evt_buf, 0x00, MAX_EVENT_SIZE);
+            /* Setup event buffer */
+            pevent = (pmlan_event) evt_buf;
+            pevent->bss_index = pmpriv->bss_index;
+            pevent->event_id = MLAN_EVENT_ID_FW_CHANNEL_REPORT_RDY;
+            pevent->event_len = pmbuf->data_len - sizeof(eventcause);
+            /* Copy event data */
+            memcpy(pmadapter, (t_u8 *) pevent->event_buf,
+                   pmbuf->pbuf + pmbuf->data_offset + sizeof(eventcause),
+                   pevent->event_len);
+            /* Handle / pass event data */
+            ret = wlan_11h_handle_event_chanrpt_ready(pmpriv, pevent);
+
+            /* Also send this event as passthru */
+            pevent->event_id = MLAN_EVENT_ID_DRV_PASSTHRU;
+            pevent->event_len = pmbuf->data_len;
+            memcpy(pmadapter, (t_u8 *) pevent->event_buf,
+                   pmbuf->pbuf + pmbuf->data_offset, pevent->event_len);
+            wlan_recv_event(pmpriv, pevent->event_id, pevent);
+            /* Now done with buffer */
+            pcb->moal_mfree(pmadapter->pmoal_handle, evt_buf);
+        }
+        /* Send up this Event to unblock MOAL waitqueue */
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_DRV_MEAS_REPORT, MNULL);
+        break;
+    case EVENT_EXT_SCAN_REPORT:
+        PRINTM(MEVENT, "EVENT: EXT_SCAN Report (%#x)\n", eventcause);
+        ret = wlan_handle_event_ext_scan_report(priv, pmbuf);
+        break;
+    case EVENT_MEAS_REPORT_RDY:
+        PRINTM(MEVENT, "EVENT: Measurement Report Ready (%#x)\n", eventcause);
+        wlan_prepare_cmd(priv, HostCmd_CMD_MEASUREMENT_REPORT,
+                         HostCmd_ACT_GEN_SET, 0, 0, MNULL);
+        break;
+    case EVENT_WMM_STATUS_CHANGE:
+        if (pmbuf && pmbuf->data_len
+            > sizeof(eventcause) + sizeof(MrvlIEtypesHeader_t)) {
+            PRINTM(MEVENT, "EVENT: WMM status changed: %d\n", pmbuf->data_len);
+
+            evt_buf = (pmbuf->pbuf + pmbuf->data_offset + sizeof(eventcause));
+
+            wlan_ret_wmm_get_status(pmpriv,
+                                    evt_buf,
+                                    pmbuf->data_len - sizeof(eventcause));
+        } else {
+            PRINTM(MEVENT, "EVENT: WMM status changed\n");
+            ret = wlan_cmd_wmm_status_change(pmpriv);
+        }
+        break;
+
+    case EVENT_RSSI_LOW:
+        PRINTM(MEVENT, "EVENT: Beacon RSSI_LOW\n");
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_BCN_RSSI_LOW, MNULL);
+        break;
+    case EVENT_SNR_LOW:
+        PRINTM(MEVENT, "EVENT: Beacon SNR_LOW\n");
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_BCN_SNR_LOW, MNULL);
+        break;
+    case EVENT_MAX_FAIL:
+        PRINTM(MEVENT, "EVENT: MAX_FAIL\n");
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_MAX_FAIL, MNULL);
+        break;
+    case EVENT_RSSI_HIGH:
+        PRINTM(MEVENT, "EVENT: Beacon RSSI_HIGH\n");
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_BCN_RSSI_HIGH, MNULL);
+        break;
+    case EVENT_SNR_HIGH:
+        PRINTM(MEVENT, "EVENT: Beacon SNR_HIGH\n");
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_BCN_SNR_HIGH, MNULL);
+        break;
+    case EVENT_DATA_RSSI_LOW:
+        PRINTM(MEVENT, "EVENT: Data RSSI_LOW\n");
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_DATA_RSSI_LOW, MNULL);
+        break;
+    case EVENT_DATA_SNR_LOW:
+        PRINTM(MEVENT, "EVENT: Data SNR_LOW\n");
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_DATA_SNR_LOW, MNULL);
+        break;
+    case EVENT_DATA_RSSI_HIGH:
+        PRINTM(MEVENT, "EVENT: Data RSSI_HIGH\n");
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_DATA_RSSI_HIGH, MNULL);
+        break;
+    case EVENT_DATA_SNR_HIGH:
+        PRINTM(MEVENT, "EVENT: Data SNR_HIGH\n");
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_DATA_SNR_HIGH, MNULL);
+        break;
+    case EVENT_LINK_QUALITY:
+        PRINTM(MEVENT, "EVENT: Link Quality\n");
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_LINK_QUALITY, MNULL);
+        break;
+    case EVENT_PRE_BEACON_LOST:
+        PRINTM(MEVENT, "EVENT: Pre-Beacon Lost\n");
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_PRE_BCN_LOST, MNULL);
+        break;
+    case EVENT_IBSS_COALESCED:
+        PRINTM(MEVENT, "EVENT: IBSS_COALESCED\n");
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_802_11_IBSS_COALESCING_STATUS,
+                               HostCmd_ACT_GEN_GET, 0, MNULL, MNULL);
+        break;
+    case EVENT_ADDBA:
+        PRINTM(MEVENT, "EVENT: ADDBA Request\n");
+        wlan_prepare_cmd(pmpriv, HostCmd_CMD_11N_ADDBA_RSP,
+                         HostCmd_ACT_GEN_SET, 0, MNULL, pmadapter->event_body);
+        break;
+    case EVENT_DELBA:
+        PRINTM(MEVENT, "EVENT: DELBA Request\n");
+        wlan_11n_delete_bastream(pmpriv, pmadapter->event_body);
+        break;
+    case EVENT_BA_STREAM_TIMEOUT:
+        PRINTM(MEVENT, "EVENT:  BA Stream timeout\n");
+        wlan_11n_ba_stream_timeout(pmpriv,
+                                   (HostCmd_DS_11N_BATIMEOUT *) pmadapter->
+                                   event_body);
+        break;
+    case EVENT_RXBA_SYNC:
+        PRINTM(MEVENT, "EVENT:  RXBA_SYNC\n");
+        wlan_11n_rxba_sync_event(pmpriv, pmadapter->event_body,
+                                 pmbuf->data_len - sizeof(eventcause));
+        break;
+    case EVENT_AMSDU_AGGR_CTRL:
+        PRINTM(MEVENT, "EVENT:  AMSDU_AGGR_CTRL %d\n",
+               *(t_u16 *) pmadapter->event_body);
+        pmadapter->tx_buf_size =
+            MIN(pmadapter->curr_tx_buf_size,
+                wlan_le16_to_cpu(*(t_u16 *) pmadapter->event_body));
+        PRINTM(MEVENT, "tx_buf_size %d\n", pmadapter->tx_buf_size);
+        break;
+
+    case EVENT_WEP_ICV_ERR:
+        PRINTM(MEVENT, "EVENT: WEP ICV error\n");
+        pevent->bss_index = pmpriv->bss_index;
+        pevent->event_id = MLAN_EVENT_ID_FW_WEP_ICV_ERR;
+        pevent->event_len = sizeof(Event_WEP_ICV_ERR);
+        memcpy(pmadapter, (t_u8 *) pevent->event_buf, pmadapter->event_body,
+               pevent->event_len);
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_WEP_ICV_ERR, pevent);
+        break;
+
+    case EVENT_BW_CHANGE:
+        PRINTM(MEVENT, "EVENT: BW Change\n");
+        pevent->bss_index = pmpriv->bss_index;
+        pevent->event_id = MLAN_EVENT_ID_FW_BW_CHANGED;
+        pevent->event_len = sizeof(t_u8);
+        /* Copy event body from the event buffer */
+        memcpy(pmadapter, (t_u8 *) pevent->event_buf, pmadapter->event_body,
+               pevent->event_len);
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_BW_CHANGED, pevent);
+        break;
+
+#ifdef WIFI_DIRECT_SUPPORT
+    case EVENT_WIFIDIRECT_GENERIC_EVENT:
+    case EVENT_WIFIDIRECT_SERVICE_DISCOVERY:
+        PRINTM(MEVENT, "EVENT: WIFIDIRECT event %d\n", eventcause);
+        /* Allocate memory for event buffer */
+        ret =
+            pcb->moal_malloc(pmadapter->pmoal_handle, MAX_EVENT_SIZE,
+                             MLAN_MEM_DEF, &evt_buf);
+        if ((ret == MLAN_STATUS_SUCCESS) && evt_buf) {
+            pevent = (pmlan_event) evt_buf;
+            pevent->bss_index = pmpriv->bss_index;
+            pevent->event_id = MLAN_EVENT_ID_DRV_PASSTHRU;
+            pevent->event_len = pmbuf->data_len;
+            memcpy(pmadapter, (t_u8 *) pevent->event_buf,
+                   pmbuf->pbuf + pmbuf->data_offset, pevent->event_len);
+            wlan_recv_event(pmpriv, pevent->event_id, pevent);
+            pcb->moal_mfree(pmadapter->pmoal_handle, evt_buf);
+        }
+        break;
+    case EVENT_REMAIN_ON_CHANNEL_EXPIRED:
+        PRINTM(MEVENT, "EVENT: REMAIN_ON_CHANNEL_EXPIRED reason=%d\n",
+               *(t_u16 *) pmadapter->event_body);
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_REMAIN_ON_CHAN_EXPIRED, MNULL);
+        break;
+#endif /* WIFI_DIRECT_SUPPORT */
+
+    default:
+        PRINTM(MEVENT, "EVENT: unknown event id: %#x\n", eventcause);
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_FW_UNKNOWN, MNULL);
+        break;
+    }
+
+    LEAVE();
+    return ret;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_sta_ioctl.c b/drivers/net/wireless/sd8797/mlan/mlan_sta_ioctl.c
new file mode 100644
index 000000000000..eb715cb804c8
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_sta_ioctl.c
@@ -0,0 +1,5489 @@
+/** @file mlan_sta_ioctl.c
+ *  
+ *  @brief This file contains the functions for station ioctl.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    10/21/2008: initial version
+******************************************************/
+
+#include "mlan.h"
+#include "mlan_join.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_wmm.h"
+#include "mlan_11n.h"
+#include "mlan_sdio.h"
+#include "mlan_11h.h"
+
+/********************************************************
+                Local Variables
+********************************************************/
+
+/********************************************************
+                Global Variables
+********************************************************/
+
+/********************************************************
+                Local Functions
+********************************************************/
+/**
+ *  @brief enable adhoc aes key
+ *
+ *  @param pmpriv	A pointer to mlan_private structure
+ *
+ *  @return		N/A
+ */
+static void
+wlan_enable_aes_key(pmlan_private pmpriv)
+{
+    mlan_ds_encrypt_key encrypt_key;
+
+    ENTER();
+
+    if (pmpriv->aes_key.key_param_set.key_len != WPA_AES_KEY_LEN) {
+        LEAVE();
+        return;
+    }
+
+    memset(pmpriv->adapter, &encrypt_key, 0, sizeof(mlan_ds_encrypt_key));
+    encrypt_key.key_len = WPA_AES_KEY_LEN;
+    encrypt_key.key_index = MLAN_KEY_INDEX_UNICAST;
+    memcpy(pmpriv->adapter, encrypt_key.key_material,
+           pmpriv->aes_key.key_param_set.key, encrypt_key.key_len);
+    wlan_prepare_cmd(pmpriv, HostCmd_CMD_802_11_KEY_MATERIAL,
+                     HostCmd_ACT_GEN_SET, KEY_INFO_ENABLED, MNULL,
+                     &encrypt_key);
+    encrypt_key.key_index &= ~MLAN_KEY_INDEX_UNICAST;
+    wlan_prepare_cmd(pmpriv, HostCmd_CMD_802_11_KEY_MATERIAL,
+                     HostCmd_ACT_GEN_SET,
+                     KEY_INFO_ENABLED, MNULL, &encrypt_key);
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief Get signal information
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_get_info_signal(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    pmlan_private pmpriv = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    if (pioctl_req != MNULL) {
+        pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    } else {
+        PRINTM(MERROR, "MLAN IOCTL information is not present\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto exit;
+    }
+
+    /* Check information buffer length of MLAN IOCTL */
+    if (pioctl_req->buf_len < sizeof(mlan_ds_get_signal)) {
+        PRINTM(MWARN, "MLAN IOCTL information buffer length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_get_signal);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        ret = MLAN_STATUS_RESOURCE;
+        goto exit;
+    }
+
+    /* Signal info can be obtained only if connected */
+    if (pmpriv->media_connected == MFALSE) {
+        PRINTM(MINFO, "Can not get signal in disconnected state\n");
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        ret = MLAN_STATUS_FAILURE;
+        goto exit;
+    }
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_RSSI_INFO,
+                           HostCmd_ACT_GEN_GET,
+                           0, (t_void *) pioctl_req, MNULL);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get statistics information
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_get_info_stats(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    pmlan_private pmpriv = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    if (pioctl_req != MNULL) {
+        pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    } else {
+        PRINTM(MERROR, "MLAN IOCTL information is not present\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto exit;
+    }
+
+    /* Check information buffer length of MLAN IOCTL */
+    if (pioctl_req->buf_len < sizeof(mlan_ds_get_stats)) {
+        PRINTM(MWARN, "MLAN IOCTL information buffer length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_get_stats);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        ret = MLAN_STATUS_RESOURCE;
+        goto exit;
+    }
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_GET_LOG,
+                           HostCmd_ACT_GEN_GET,
+                           0, (t_void *) pioctl_req, MNULL);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get BSS information
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_get_info_bss_info(IN pmlan_adapter pmadapter,
+                       IN pmlan_ioctl_req pioctl_req)
+{
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_get_info *info;
+    BSSDescriptor_t *pbss_desc;
+    t_s32 tbl_idx = 0;
+
+    ENTER();
+
+    /* Get current BSS info */
+    pbss_desc = &pmpriv->curr_bss_params.bss_descriptor;
+    info = (mlan_ds_get_info *) pioctl_req->pbuf;
+
+    /* BSS mode */
+    info->param.bss_info.bss_mode = pmpriv->bss_mode;
+
+    /* SSID */
+    memcpy(pmadapter, &info->param.bss_info.ssid, &pbss_desc->ssid,
+           sizeof(mlan_802_11_ssid));
+
+    /* BSSID */
+    memcpy(pmadapter, &info->param.bss_info.bssid, &pbss_desc->mac_address,
+           MLAN_MAC_ADDR_LENGTH);
+
+    /* Channel */
+    info->param.bss_info.bss_chan = pbss_desc->channel;
+
+    /* Beacon interval */
+    info->param.bss_info.beacon_interval = pbss_desc->beacon_period;
+
+    /* Band */
+    info->param.bss_info.bss_band = (t_u8) pbss_desc->bss_band;
+
+    /* Region code */
+    info->param.bss_info.region_code = pmadapter->region_code;
+
+    /* Scan table index if connected */
+    info->param.bss_info.scan_table_idx = 0;
+    if (pmpriv->media_connected == MTRUE) {
+        tbl_idx =
+            wlan_find_ssid_in_list(pmpriv, &pbss_desc->ssid,
+                                   pbss_desc->mac_address, pmpriv->bss_mode);
+        if (tbl_idx >= 0)
+            info->param.bss_info.scan_table_idx = tbl_idx;
+    }
+
+    /* Connection status */
+    info->param.bss_info.media_connected = pmpriv->media_connected;
+
+    /* Radio status */
+    info->param.bss_info.radio_on = pmadapter->radio_on;
+
+    /* Tx power information */
+    info->param.bss_info.max_power_level = pmpriv->max_tx_power_level;
+    info->param.bss_info.min_power_level = pmpriv->min_tx_power_level;
+
+    /* AdHoc state */
+    info->param.bss_info.adhoc_state = pmpriv->adhoc_state;
+
+    /* Last beacon NF */
+    info->param.bss_info.bcn_nf_last = pmpriv->bcn_nf_last;
+
+    /* wep status */
+    if (pmpriv->sec_info.wep_status == Wlan802_11WEPEnabled)
+        info->param.bss_info.wep_status = MTRUE;
+    else
+        info->param.bss_info.wep_status = MFALSE;
+
+    info->param.bss_info.is_hs_configured = pmadapter->is_hs_configured;
+    info->param.bss_info.is_deep_sleep = pmadapter->is_deep_sleep;
+
+    /* Capability Info */
+    info->param.bss_info.capability_info = 0;
+    memcpy(pmadapter, &info->param.bss_info.capability_info,
+           &pbss_desc->cap_info, sizeof(info->param.bss_info.capability_info));
+
+    /* Listen Interval */
+    info->param.bss_info.listen_interval = pmpriv->listen_interval;
+
+    /* Association ID */
+    if (pmpriv->assoc_rsp_buf)
+        info->param.bss_info.assoc_id =
+            (t_u16) ((IEEEtypes_AssocRsp_t *) pmpriv->assoc_rsp_buf)->a_id;
+    else
+        info->param.bss_info.assoc_id = 0;
+
+    /* AP/Peer supported rates */
+    memset(pmadapter, info->param.bss_info.peer_supp_rates, 0,
+           sizeof(info->param.bss_info.peer_supp_rates));
+    memcpy(pmadapter, info->param.bss_info.peer_supp_rates,
+           pbss_desc->supported_rates,
+           MIN(sizeof(info->param.bss_info.peer_supp_rates),
+               sizeof(pbss_desc->supported_rates)));
+
+    pioctl_req->data_read_written =
+        sizeof(mlan_bss_info) + MLAN_SUB_COMMAND_SIZE;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get information handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_get_info_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_get_info *pget_info = MNULL;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    pget_info = (mlan_ds_get_info *) pioctl_req->pbuf;
+
+    switch (pget_info->sub_command) {
+    case MLAN_OID_GET_STATS:
+        status = wlan_get_info_stats(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_GET_SIGNAL:
+        status = wlan_get_info_signal(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_GET_FW_INFO:
+        pioctl_req->data_read_written =
+            sizeof(mlan_fw_info) + MLAN_SUB_COMMAND_SIZE;
+        pget_info->param.fw_info.fw_ver = pmadapter->fw_release_number;
+        memcpy(pmadapter, &pget_info->param.fw_info.mac_addr, pmpriv->curr_addr,
+               MLAN_MAC_ADDR_LENGTH);
+        pget_info->param.fw_info.fw_bands = pmadapter->fw_bands;
+        pget_info->param.fw_info.hw_dev_mcs_support =
+            pmadapter->hw_dev_mcs_support;
+        break;
+    case MLAN_OID_GET_BSS_INFO:
+        status = wlan_get_info_bss_info(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_GET_DEBUG_INFO:
+        status = wlan_get_info_debug_info(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_GET_VER_EXT:
+        status = wlan_get_info_ver_ext(pmadapter, pioctl_req);
+        break;
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Set/Get SNMP MIB handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_snmp_mib_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 cmd_action = 0;
+    t_u16 cmd_oid = 0;
+    mlan_ds_snmp_mib *mib = MNULL;
+    t_u32 value = 0;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_snmp_mib)) {
+        PRINTM(MWARN, "MLAN IOCTL information buffer length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_snmp_mib);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        ret = MLAN_STATUS_RESOURCE;
+        goto exit;
+    }
+
+    mib = (mlan_ds_snmp_mib *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET)
+        cmd_action = HostCmd_ACT_GEN_SET;
+    else
+        cmd_action = HostCmd_ACT_GEN_GET;
+
+    switch (mib->sub_command) {
+    case MLAN_OID_SNMP_MIB_RTS_THRESHOLD:
+        value = mib->param.rts_threshold;
+        cmd_oid = RtsThresh_i;
+        break;
+    case MLAN_OID_SNMP_MIB_FRAG_THRESHOLD:
+        value = mib->param.frag_threshold;
+        cmd_oid = FragThresh_i;
+        break;
+    case MLAN_OID_SNMP_MIB_RETRY_COUNT:
+        value = mib->param.retry_count;
+        cmd_oid = ShortRetryLim_i;
+        break;
+    case MLAN_OID_SNMP_MIB_DTIM_PERIOD:
+        value = mib->param.dtim_period;
+        cmd_oid = DtimPeriod_i;
+        break;
+    }
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_SNMP_MIB,
+                           cmd_action, cmd_oid, (t_void *) pioctl_req, &value);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get Infra/Ad-hoc band configuration
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_radio_ioctl_band_cfg(IN pmlan_adapter pmadapter,
+                          IN pmlan_ioctl_req pioctl_req)
+{
+    t_u8 i, global_band = 0;
+    t_u8 infra_band = 0;
+    t_u8 adhoc_band = 0;
+    t_u32 adhoc_channel = 0;
+    mlan_ds_radio_cfg *radio_cfg = MNULL;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    radio_cfg = (mlan_ds_radio_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        infra_band = (t_u8) radio_cfg->param.band_cfg.config_bands;
+        adhoc_band = (t_u8) radio_cfg->param.band_cfg.adhoc_start_band;
+        adhoc_channel = radio_cfg->param.band_cfg.adhoc_channel;
+
+        /* SET Infra band */
+        if ((infra_band | pmadapter->fw_bands) & ~pmadapter->fw_bands) {
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+
+        /* SET Ad-hoc Band */
+        if ((adhoc_band | pmadapter->fw_bands) & ~pmadapter->fw_bands) {
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+        if (!adhoc_band)
+            adhoc_band = pmadapter->adhoc_start_band;
+
+        for (i = 0; i < pmadapter->priv_num; i++) {
+            if (pmadapter->priv[i] && pmadapter->priv[i] != pmpriv &&
+                GET_BSS_ROLE(pmadapter->priv[i]) == MLAN_BSS_ROLE_STA)
+                global_band |= pmadapter->priv[i]->config_bands;
+        }
+        global_band |= infra_band;
+
+        if (wlan_set_regiontable
+            (pmpriv, (t_u8) pmadapter->region_code, global_band | adhoc_band)) {
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_FAIL;
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+
+        if (wlan_11d_set_universaltable(pmpriv, global_band | adhoc_band)) {
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_FAIL;
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+        pmpriv->config_bands = infra_band;
+        pmadapter->config_bands = global_band;
+
+        pmadapter->adhoc_start_band = adhoc_band;
+        pmpriv->intf_state_11h.adhoc_auto_sel_chan = MFALSE;
+        pmadapter->chan_bandwidth =
+            (t_u8) radio_cfg->param.band_cfg.sec_chan_offset;
+        /*
+         * If no adhoc_channel is supplied verify if the existing adhoc channel
+         * compiles with new adhoc_band
+         */
+        if (!adhoc_channel) {
+            if (!wlan_find_cfp_by_band_and_channel
+                (pmadapter, pmadapter->adhoc_start_band,
+                 pmpriv->adhoc_channel)) {
+                /* Pass back the default channel */
+                radio_cfg->param.band_cfg.adhoc_channel =
+                    DEFAULT_AD_HOC_CHANNEL;
+                if ((pmadapter->adhoc_start_band & BAND_A)
+                    || (pmadapter->adhoc_start_band & BAND_AN)
+                    ) {
+                    radio_cfg->param.band_cfg.adhoc_channel =
+                        DEFAULT_AD_HOC_CHANNEL_A;
+                }
+            }
+        } else {                /* Return error if adhoc_band and adhoc_channel
+                                   combination is invalid */
+            if (!wlan_find_cfp_by_band_and_channel
+                (pmadapter, pmadapter->adhoc_start_band,
+                 (t_u16) adhoc_channel)) {
+                pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+                LEAVE();
+                return MLAN_STATUS_FAILURE;
+            }
+            pmpriv->adhoc_channel = (t_u8) adhoc_channel;
+        }
+
+        if ((adhoc_band & BAND_GN)
+            || (adhoc_band & BAND_AN)
+            ) {
+            pmadapter->adhoc_11n_enabled = MTRUE;
+        } else {
+            pmadapter->adhoc_11n_enabled = MFALSE;
+        }
+    } else {
+        radio_cfg->param.band_cfg.config_bands = pmpriv->config_bands;  /* Infra
+                                                                           Bands
+                                                                         */
+        radio_cfg->param.band_cfg.adhoc_start_band = pmadapter->adhoc_start_band;       /* Adhoc
+                                                                                           Band
+                                                                                         */
+        radio_cfg->param.band_cfg.adhoc_channel = pmpriv->adhoc_channel;        /* Adhoc
+                                                                                   Channel
+                                                                                 */
+        radio_cfg->param.band_cfg.fw_bands = pmadapter->fw_bands;       /* FW
+                                                                           support
+                                                                           Bands
+                                                                         */
+        PRINTM(MINFO, "Global config band = %d\n", pmadapter->config_bands);
+        radio_cfg->param.band_cfg.sec_chan_offset = pmadapter->chan_bandwidth;  /* adhoc
+                                                                                   channel
+                                                                                   bandwidth
+                                                                                 */
+
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Radio command handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_radio_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_radio_cfg *radio_cfg = MNULL;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_radio_cfg)) {
+        PRINTM(MWARN, "MLAN IOCTL information buffer length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_radio_cfg);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+    radio_cfg = (mlan_ds_radio_cfg *) pioctl_req->pbuf;
+    switch (radio_cfg->sub_command) {
+    case MLAN_OID_RADIO_CTRL:
+        status = wlan_radio_ioctl_radio_ctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_BAND_CFG:
+        status = wlan_radio_ioctl_band_cfg(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_ANT_CFG:
+        status = wlan_radio_ioctl_ant_cfg(pmadapter, pioctl_req);
+        break;
+#ifdef WIFI_DIRECT_SUPPORT
+    case MLAN_OID_REMAIN_CHAN_CFG:
+        status = wlan_radio_ioctl_remain_chan_cfg(pmadapter, pioctl_req);
+        break;
+#endif
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Set/Get MAC address
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_bss_ioctl_mac_address(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_bss *bss = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    ENTER();
+    bss = (mlan_ds_bss *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        pioctl_req->data_read_written =
+            MLAN_MAC_ADDR_LENGTH + MLAN_SUB_COMMAND_SIZE;
+        memcpy(pmadapter, &bss->param.mac_addr, pmpriv->curr_addr,
+               MLAN_MAC_ADDR_LENGTH);
+        ret = MLAN_STATUS_SUCCESS;
+        goto exit;
+    }
+
+    memcpy(pmadapter, pmpriv->curr_addr, &bss->param.mac_addr,
+           MLAN_MAC_ADDR_LENGTH);
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_MAC_ADDRESS,
+                           HostCmd_ACT_GEN_SET,
+                           0, (t_void *) pioctl_req, MNULL);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set multicast list
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_bss_ioctl_set_multicast_list(IN pmlan_adapter pmadapter,
+                                  IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_bss *bss = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 old_pkt_filter;
+
+    ENTER();
+
+    old_pkt_filter = pmpriv->curr_pkt_filter;
+    bss = (mlan_ds_bss *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        ret = MLAN_STATUS_FAILURE;
+        goto exit;
+    }
+    pioctl_req->data_read_written =
+        sizeof(mlan_multicast_list) + MLAN_SUB_COMMAND_SIZE;
+    if (bss->param.multicast_list.mode == MLAN_PROMISC_MODE) {
+        PRINTM(MINFO, "Enable Promiscuous mode\n");
+        pmpriv->curr_pkt_filter |= HostCmd_ACT_MAC_PROMISCUOUS_ENABLE;
+        pmpriv->curr_pkt_filter &= ~HostCmd_ACT_MAC_ALL_MULTICAST_ENABLE;
+    } else {
+        /* Multicast */
+        pmpriv->curr_pkt_filter &= ~HostCmd_ACT_MAC_PROMISCUOUS_ENABLE;
+        if (bss->param.multicast_list.mode == MLAN_ALL_MULTI_MODE) {
+            PRINTM(MINFO, "Enabling All Multicast!\n");
+            pmpriv->curr_pkt_filter |= HostCmd_ACT_MAC_ALL_MULTICAST_ENABLE;
+        } else {
+            pmpriv->curr_pkt_filter &= ~HostCmd_ACT_MAC_ALL_MULTICAST_ENABLE;
+            if (bss->param.multicast_list.num_multicast_addr) {
+                PRINTM(MINFO, "Set multicast list=%d\n",
+                       bss->param.multicast_list.num_multicast_addr);
+                /* Set multicast addresses to firmware */
+                if (old_pkt_filter == pmpriv->curr_pkt_filter) {
+                    /* Send request to firmware */
+                    ret =
+                        wlan_prepare_cmd(pmpriv, HostCmd_CMD_MAC_MULTICAST_ADR,
+                                         HostCmd_ACT_GEN_SET, 0,
+                                         (t_void *) pioctl_req,
+                                         &bss->param.multicast_list);
+                    if (ret == MLAN_STATUS_SUCCESS)
+                        ret = MLAN_STATUS_PENDING;
+                } else {
+                    /* Send request to firmware */
+                    ret =
+                        wlan_prepare_cmd(pmpriv, HostCmd_CMD_MAC_MULTICAST_ADR,
+                                         HostCmd_ACT_GEN_SET, 0, MNULL,
+                                         &bss->param.multicast_list);
+                }
+            }
+        }
+    }
+    PRINTM(MINFO, "old_pkt_filter=0x%x, curr_pkt_filter=0x%x\n", old_pkt_filter,
+           pmpriv->curr_pkt_filter);
+    if (old_pkt_filter != pmpriv->curr_pkt_filter) {
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_MAC_CONTROL,
+                               HostCmd_ACT_GEN_SET,
+                               0,
+                               (t_void *) pioctl_req, &pmpriv->curr_pkt_filter);
+        if (ret == MLAN_STATUS_SUCCESS)
+            ret = MLAN_STATUS_PENDING;
+    }
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get channel list
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_bss_ioctl_get_channel_list(IN pmlan_adapter pmadapter,
+                                IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_bss *bss = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    chan_freq_power_t *cfp;
+    t_u32 i, j;
+
+    ENTER();
+
+    bss = (mlan_ds_bss *) pioctl_req->pbuf;
+    if (pioctl_req->action != MLAN_ACT_GET) {
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    if ((wlan_11d_is_enabled(pmpriv) &&
+         pmpriv->media_connected == MTRUE) &&
+        ((pmpriv->bss_mode == MLAN_BSS_MODE_INFRA) ||
+         (pmpriv->bss_mode == MLAN_BSS_MODE_IBSS &&
+          pmpriv->adhoc_state != ADHOC_STARTED))
+        ) {
+        t_u8 chan_no;
+        t_u8 band;
+
+        parsed_region_chan_11d_t *parsed_region_chan = MNULL;
+        parsed_region_chan_11d_t region_chan;
+
+        BSSDescriptor_t *pbss_desc = &pmpriv->curr_bss_params.bss_descriptor;
+
+        memset(pmadapter, &region_chan, 0, sizeof(parsed_region_chan_11d_t));
+
+        /* If country IE is present in the associated AP then return the
+           channel list from country IE else return it from the learning table */
+
+        if (wlan_11d_parse_domain_info(pmadapter, &pbss_desc->country_info,
+                                       (t_u8) pbss_desc->bss_band,
+                                       &region_chan) == MLAN_STATUS_SUCCESS) {
+
+            parsed_region_chan = &region_chan;
+        } else {
+            parsed_region_chan = &pmadapter->parsed_region_chan;
+        }
+
+        PRINTM(MINFO, "no_of_chan=%d\n", parsed_region_chan->no_of_chan);
+
+        for (i = 0; (bss->param.chanlist.num_of_chan < MLAN_MAX_CHANNEL_NUM)
+             && (i < parsed_region_chan->no_of_chan); i++) {
+            chan_no = parsed_region_chan->chan_pwr[i].chan;
+            band = parsed_region_chan->chan_pwr[i].band;
+            PRINTM(MINFO, "band=%d, chan_no=%d\n", band, chan_no);
+            bss->param.chanlist.cf[bss->param.chanlist.num_of_chan].channel =
+                (t_u32) chan_no;
+            bss->param.chanlist.cf[bss->param.chanlist.num_of_chan].freq =
+                (t_u32) wlan_11d_chan_2_freq(pmadapter, chan_no, band);
+            bss->param.chanlist.num_of_chan++;
+        }
+    } else {
+        for (j = 0; (bss->param.chanlist.num_of_chan < MLAN_MAX_CHANNEL_NUM)
+             && (j < MAX_REGION_CHANNEL_NUM); j++) {
+            cfp = pmadapter->region_channel[j].pcfp;
+            for (i = 0; (bss->param.chanlist.num_of_chan < MLAN_MAX_CHANNEL_NUM)
+                 && pmadapter->region_channel[j].valid
+                 && cfp && (i < pmadapter->region_channel[j].num_cfp); i++) {
+                bss->param.chanlist.cf[bss->param.chanlist.num_of_chan].
+                    channel = (t_u32) cfp->channel;
+                bss->param.chanlist.cf[bss->param.chanlist.num_of_chan].freq =
+                    (t_u32) cfp->freq;
+                bss->param.chanlist.num_of_chan++;
+                cfp++;
+            }
+        }
+    }
+
+    PRINTM(MINFO, "num of channel=%d\n", bss->param.chanlist.num_of_chan);
+
+    LEAVE();
+    return ret;
+}
+
+/** Highest channel used in 2.4GHz band */
+#define MAX_CHANNEL_BAND_B    (14)
+
+/** Highest frequency used in 2.4GHz band */
+#define MAX_FREQUENCY_BAND_B  (2484)
+
+/**
+ *  @brief Set/Get BSS channel
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_bss_ioctl_channel(IN pmlan_adapter pmadapter,
+                       IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_bss *bss = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    chan_freq_power_t *cfp = MNULL;
+    ENTER();
+
+    if ((pioctl_req == MNULL) || (pioctl_req->pbuf == MNULL)) {
+        PRINTM(MERROR, "Request buffer not found!\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    bss = (mlan_ds_bss *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        cfp = wlan_find_cfp_by_band_and_channel(pmadapter,
+                                                pmpriv->curr_bss_params.band,
+                                                (t_u16) pmpriv->curr_bss_params.
+                                                bss_descriptor.channel);
+        if (cfp) {
+            bss->param.bss_chan.channel = cfp->channel;
+            bss->param.bss_chan.freq = cfp->freq;
+        } else {
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+        }
+        pioctl_req->data_read_written =
+            sizeof(chan_freq) + MLAN_SUB_COMMAND_SIZE;
+        LEAVE();
+        return ret;
+    }
+    if (!bss->param.bss_chan.channel && !bss->param.bss_chan.freq) {
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    if (pmadapter->adhoc_start_band & BAND_AN)
+        pmadapter->adhoc_start_band = BAND_G | BAND_B | BAND_GN;
+    else if (pmadapter->adhoc_start_band & BAND_A)
+        pmadapter->adhoc_start_band = BAND_G | BAND_B;
+    if (bss->param.bss_chan.channel) {
+        if (bss->param.bss_chan.channel <= MAX_CHANNEL_BAND_B)
+            cfp =
+                wlan_find_cfp_by_band_and_channel(pmadapter, BAND_B,
+                                                  (t_u16) bss->param.bss_chan.
+                                                  channel);
+        if (!cfp) {
+            cfp =
+                wlan_find_cfp_by_band_and_channel(pmadapter, BAND_A,
+                                                  (t_u16) bss->param.bss_chan.
+                                                  channel);
+            if (cfp) {
+                if (pmadapter->adhoc_11n_enabled)
+                    pmadapter->adhoc_start_band = BAND_A | BAND_AN;
+                else
+                    pmadapter->adhoc_start_band = BAND_A;
+            }
+        }
+    } else {
+        if (bss->param.bss_chan.freq <= MAX_FREQUENCY_BAND_B)
+            cfp =
+                wlan_find_cfp_by_band_and_freq(pmadapter, BAND_B,
+                                               bss->param.bss_chan.freq);
+        if (!cfp) {
+            cfp =
+                wlan_find_cfp_by_band_and_freq(pmadapter, BAND_A,
+                                               bss->param.bss_chan.freq);
+            if (cfp) {
+                if (pmadapter->adhoc_11n_enabled)
+                    pmadapter->adhoc_start_band = BAND_A | BAND_AN;
+                else
+                    pmadapter->adhoc_start_band = BAND_A;
+            }
+        }
+    }
+    if (!cfp || !cfp->channel) {
+        PRINTM(MERROR, "Invalid channel/freq\n");
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+
+    }
+    pmpriv->adhoc_channel = (t_u8) cfp->channel;
+    pmpriv->intf_state_11h.adhoc_auto_sel_chan = MFALSE;
+    bss->param.bss_chan.channel = cfp->channel;
+    bss->param.bss_chan.freq = cfp->freq;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get BSS mode
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_bss_ioctl_mode(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_bss *bss = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    bss = (mlan_ds_bss *) pioctl_req->pbuf;
+
+    ENTER();
+
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        bss->param.bss_mode = pmpriv->bss_mode;
+        pioctl_req->data_read_written = sizeof(t_u32) + MLAN_SUB_COMMAND_SIZE;
+        goto exit;
+    }
+
+    if ((pmpriv->bss_mode == bss->param.bss_mode) ||
+        (bss->param.bss_mode == MLAN_BSS_MODE_AUTO)) {
+        PRINTM(MINFO, "Already set to required mode! No change!\n");
+        pmpriv->bss_mode = bss->param.bss_mode;
+        goto exit;
+    }
+
+    if (pmpriv->bss_mode != MLAN_BSS_MODE_AUTO)
+        ret = wlan_disconnect(pmpriv, MNULL, MNULL);
+    else
+        ret = wlan_disconnect(pmpriv, pioctl_req, MNULL);
+
+    if (pmpriv->sec_info.authentication_mode != MLAN_AUTH_MODE_AUTO)
+        pmpriv->sec_info.authentication_mode = MLAN_AUTH_MODE_OPEN;
+    pmpriv->bss_mode = bss->param.bss_mode;
+
+    if (pmpriv->bss_mode == MLAN_BSS_MODE_INFRA)
+        pmpriv->port_ctrl_mode = MTRUE;
+    else
+        pmpriv->port_ctrl_mode = MFALSE;
+
+    if ((pmpriv->bss_mode != MLAN_BSS_MODE_AUTO)
+        ) {
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_SET_BSS_MODE,
+                               HostCmd_ACT_GEN_SET,
+                               0, (t_void *) pioctl_req, MNULL);
+        if (ret == MLAN_STATUS_SUCCESS)
+            ret = MLAN_STATUS_PENDING;
+    }
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Start BSS
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_bss_ioctl_start(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_bss *bss = (mlan_ds_bss *) pioctl_req->pbuf;
+    t_s32 i = -1;
+    t_u8 zero_mac[] = { 0, 0, 0, 0, 0, 0 };
+
+    ENTER();
+
+    /* Before ASSOC REQ, If "port ctrl" mode is enabled, move the port to
+       CLOSED state */
+    if (pmpriv->port_ctrl_mode == MTRUE) {
+        PRINTM(MINFO, "bss_ioctl_start(): port_state=CLOSED\n");
+        pmpriv->prior_port_status = pmpriv->port_open;
+        pmpriv->port_open = MFALSE;
+    }
+    pmpriv->scan_block = MFALSE;
+
+    if (pmpriv->bss_mode == MLAN_BSS_MODE_INFRA) {
+        if (!bss->param.ssid_bssid.idx ||
+            bss->param.ssid_bssid.idx > pmadapter->num_in_scan_table) {
+            /* Search for the requested SSID in the scan table */
+            if (bss->param.ssid_bssid.ssid.ssid_len) {
+                if (memcmp
+                    (pmadapter, &bss->param.ssid_bssid.bssid, zero_mac,
+                     sizeof(zero_mac)))
+                    i = wlan_find_ssid_in_list(pmpriv,
+                                               &bss->param.ssid_bssid.ssid,
+                                               (t_u8 *) & bss->param.ssid_bssid.
+                                               bssid, MLAN_BSS_MODE_INFRA);
+                else
+                    i = wlan_find_ssid_in_list(pmpriv,
+                                               &bss->param.ssid_bssid.ssid,
+                                               MNULL, MLAN_BSS_MODE_INFRA);
+            } else {
+                i = wlan_find_bssid_in_list(pmpriv,
+                                            (t_u8 *) & bss->param.ssid_bssid.
+                                            bssid, MLAN_BSS_MODE_INFRA);
+            }
+        } else {
+            /* use bsslist index number to assoicate */
+            i = wlan_is_network_compatible(pmpriv,
+                                           bss->param.ssid_bssid.idx - 1,
+                                           pmpriv->bss_mode);
+        }
+        if (i >= 0) {
+            PRINTM(MINFO, "SSID found in scan list ... associating...\n");
+
+            /* Clear any past association response stored for application
+               retrieval */
+            pmpriv->assoc_rsp_size = 0;
+            ret = wlan_associate(pmpriv, pioctl_req,
+                                 &pmadapter->pscan_table[i]);
+            if (ret)
+                goto start_ssid_done;
+        } else {                /* i >= 0 */
+            PRINTM(MERROR,
+                   "SSID not found in scan list: ssid=%s, %02x:%02x:%02x:%02x:%02x:%02x, idx=%d\n",
+                   bss->param.ssid_bssid.ssid.ssid,
+                   bss->param.ssid_bssid.bssid[0],
+                   bss->param.ssid_bssid.bssid[1],
+                   bss->param.ssid_bssid.bssid[2],
+                   bss->param.ssid_bssid.bssid[3],
+                   bss->param.ssid_bssid.bssid[4],
+                   bss->param.ssid_bssid.bssid[5],
+                   (int) bss->param.ssid_bssid.idx);
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+            goto start_ssid_done;
+        }
+    } else {
+        /* Adhoc mode */
+        /* If the requested SSID matches current SSID, return */
+        if (bss->param.ssid_bssid.ssid.ssid_len &&
+            (!wlan_ssid_cmp
+             (pmadapter, &pmpriv->curr_bss_params.bss_descriptor.ssid,
+              &bss->param.ssid_bssid.ssid))) {
+            ret = MLAN_STATUS_SUCCESS;
+            goto start_ssid_done;
+        }
+
+        /* Exit Adhoc mode first */
+        PRINTM(MINFO, "Sending Adhoc Stop\n");
+        ret = wlan_disconnect(pmpriv, MNULL, MNULL);
+        if (ret)
+            goto start_ssid_done;
+
+        pmpriv->adhoc_is_link_sensed = MFALSE;
+
+        if (!bss->param.ssid_bssid.idx ||
+            bss->param.ssid_bssid.idx > pmadapter->num_in_scan_table) {
+            /* Search for the requested network in the scan table */
+            if (bss->param.ssid_bssid.ssid.ssid_len) {
+                i = wlan_find_ssid_in_list(pmpriv,
+                                           &bss->param.ssid_bssid.ssid,
+                                           MNULL, MLAN_BSS_MODE_IBSS);
+            } else {
+                i = wlan_find_bssid_in_list(pmpriv,
+                                            (t_u8 *) & bss->param.ssid_bssid.
+                                            bssid, MLAN_BSS_MODE_IBSS);
+            }
+        } else {
+            /* use bsslist index number to assoicate */
+            i = wlan_is_network_compatible(pmpriv,
+                                           bss->param.ssid_bssid.idx - 1,
+                                           pmpriv->bss_mode);
+        }
+
+        if (i >= 0) {
+            PRINTM(MINFO, "Network found in scan list ... joining ...\n");
+            ret =
+                wlan_adhoc_join(pmpriv, pioctl_req, &pmadapter->pscan_table[i]);
+            if (ret)
+                goto start_ssid_done;
+            if (pmpriv->adhoc_aes_enabled)
+                wlan_enable_aes_key(pmpriv);
+        } else {                /* i >= 0 */
+            PRINTM(MINFO, "Network not found in the list, "
+                   "creating adhoc with ssid = %s\n",
+                   bss->param.ssid_bssid.ssid.ssid);
+            ret =
+                wlan_adhoc_start(pmpriv, pioctl_req,
+                                 &bss->param.ssid_bssid.ssid);
+            if (ret)
+                goto start_ssid_done;
+            if (pmpriv->adhoc_aes_enabled)
+                wlan_enable_aes_key(pmpriv);
+        }
+    }
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  start_ssid_done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Stop BSS
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_bss_ioctl_stop(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_bss *bss = (mlan_ds_bss *) pioctl_req->pbuf;
+
+    ENTER();
+
+    ret = wlan_disconnect(pmpriv, pioctl_req, &bss->param.bssid);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get IBSS channel
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_bss_ioctl_ibss_channel(IN pmlan_adapter pmadapter,
+                            IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_bss *bss = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 cmd_action;
+
+    ENTER();
+
+    bss = (mlan_ds_bss *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        if (pmpriv->media_connected == MFALSE) {
+            bss->param.bss_chan.channel = pmpriv->adhoc_channel;
+            goto exit;
+        }
+        cmd_action = HostCmd_ACT_GEN_GET;
+    } else {
+        cmd_action = HostCmd_ACT_GEN_SET;
+        pmpriv->adhoc_channel = (t_u8) bss->param.bss_chan.channel;
+        pmpriv->intf_state_11h.adhoc_auto_sel_chan = MFALSE;
+    }
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_RF_CHANNEL,
+                           cmd_action,
+                           0,
+                           (t_void *) pioctl_req, &bss->param.bss_chan.channel);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get beacon interval
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_bss_ioctl_beacon_interval(IN pmlan_adapter pmadapter,
+                               IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_bss *bss = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    ENTER();
+    bss = (mlan_ds_bss *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        bss->param.bcn_interval = pmpriv->beacon_period;
+        if (pmpriv->media_connected == MTRUE)
+            bss->param.bcn_interval =
+                pmpriv->curr_bss_params.bss_descriptor.beacon_period;
+    } else
+        pmpriv->beacon_period = (t_u16) bss->param.bcn_interval;
+    pioctl_req->data_read_written = sizeof(t_u32) + MLAN_SUB_COMMAND_SIZE;
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get ATIM window
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_bss_ioctl_atim_window(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_bss *bss = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    ENTER();
+    bss = (mlan_ds_bss *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        bss->param.atim_window = pmpriv->atim_window;
+        if (pmpriv->media_connected == MTRUE)
+            bss->param.atim_window =
+                pmpriv->curr_bss_params.bss_descriptor.atim_window;
+    } else
+        pmpriv->atim_window = (t_u16) bss->param.atim_window;
+
+    pioctl_req->data_read_written = sizeof(t_u32) + MLAN_SUB_COMMAND_SIZE;
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Query embe
+ *
+ *  @param priv                 A pointer to mlan_private structure
+ *  @param pioctl_req           A pointer to ioctl request buffer
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+static mlan_status
+wlan_query_passphrase(mlan_private * priv, pmlan_ioctl_req pioctl_req)
+{
+    mlan_adapter *pmadapter = priv->adapter;
+    mlan_ds_bss *bss = MNULL;
+    mlan_ssid_bssid *ssid_bssid = MNULL;
+    mlan_ds_passphrase sec_pp;
+    int i = 0;
+    BSSDescriptor_t *pbss_desc;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    bss = (mlan_ds_bss *) pioctl_req->pbuf;
+    ssid_bssid = &bss->param.ssid_bssid;
+
+    memset(pmadapter, &sec_pp, 0, sizeof(mlan_ds_passphrase));
+    sec_pp.psk_type = MLAN_PSK_QUERY;
+    if (ssid_bssid->ssid.ssid_len == 0) {
+        i = wlan_find_bssid_in_list(priv, (t_u8 *) & ssid_bssid->bssid,
+                                    MLAN_BSS_MODE_AUTO);
+        if (i >= 0) {
+            pbss_desc = &pmadapter->pscan_table[i];
+            memcpy(pmadapter, &sec_pp.ssid, &pbss_desc->ssid,
+                   sizeof(mlan_802_11_ssid));
+        } else
+            memcpy(pmadapter, &sec_pp.bssid, &ssid_bssid->bssid,
+                   MLAN_MAC_ADDR_LENGTH);
+    } else {
+        memcpy(pmadapter, &sec_pp.ssid, &ssid_bssid->ssid,
+               sizeof(mlan_802_11_ssid));
+    }
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(priv,
+                           HostCmd_CMD_SUPPLICANT_PMK,
+                           HostCmd_ACT_GEN_GET,
+                           0, (t_void *) pioctl_req, &sec_pp);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Search for a BSS
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_bss_ioctl_find_bss(IN pmlan_adapter pmadapter,
+                        IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    if (pmpriv->ewpa_query) {
+        if (wlan_query_passphrase(pmpriv, pioctl_req) == MLAN_STATUS_SUCCESS) {
+            PRINTM(MINFO, "Find BSS ioctl: query passphrase\n");
+            LEAVE();
+            return MLAN_STATUS_PENDING;
+        }
+    }
+    ret = wlan_find_bss(pmpriv, pioctl_req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief BSS command handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_bss_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_bss *bss = MNULL;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_bss)) {
+        PRINTM(MWARN, "MLAN bss IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_bss);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+
+    bss = (mlan_ds_bss *) pioctl_req->pbuf;
+
+    switch (bss->sub_command) {
+    case MLAN_OID_BSS_START:
+        status = wlan_bss_ioctl_start(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_BSS_STOP:
+        status = wlan_bss_ioctl_stop(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_BSS_MODE:
+        status = wlan_bss_ioctl_mode(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_BSS_CHANNEL:
+        status = wlan_bss_ioctl_channel(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_BSS_CHANNEL_LIST:
+        status = wlan_bss_ioctl_get_channel_list(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_BSS_MAC_ADDR:
+        status = wlan_bss_ioctl_mac_address(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_BSS_MULTICAST_LIST:
+        status = wlan_bss_ioctl_set_multicast_list(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_BSS_FIND_BSS:
+        status = wlan_bss_ioctl_find_bss(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_IBSS_BCN_INTERVAL:
+        status = wlan_bss_ioctl_beacon_interval(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_IBSS_ATIM_WINDOW:
+        status = wlan_bss_ioctl_atim_window(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_IBSS_CHANNEL:
+        status = wlan_bss_ioctl_ibss_channel(pmadapter, pioctl_req);
+        break;
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+    case MLAN_OID_BSS_ROLE:
+        status = wlan_bss_ioctl_bss_role(pmadapter, pioctl_req);
+        break;
+#endif
+#ifdef WIFI_DIRECT_SUPPORT
+    case MLAN_OID_WIFI_DIRECT_MODE:
+        status = wlan_bss_ioctl_wifi_direct_mode(pmadapter, pioctl_req);
+        break;
+#endif
+    default:
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Get supported rates
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_rate_ioctl_get_supported_rate(IN pmlan_adapter pmadapter,
+                                   IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_rate *rate = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    if (pioctl_req->action != MLAN_ACT_GET) {
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    rate = (mlan_ds_rate *) pioctl_req->pbuf;
+    if (rate->param.rate_band_cfg.config_bands &&
+        rate->param.rate_band_cfg.bss_mode)
+        wlan_get_active_data_rates(pmpriv, rate->param.rate_band_cfg.bss_mode,
+                                   rate->param.rate_band_cfg.config_bands,
+                                   rate->param.rates);
+    else
+        wlan_get_active_data_rates(pmpriv, pmpriv->bss_mode,
+                                   (pmpriv->bss_mode == MLAN_BSS_MODE_INFRA) ?
+                                   pmpriv->config_bands : pmadapter->
+                                   adhoc_start_band, rate->param.rates);
+    pioctl_req->data_read_written =
+        MLAN_SUPPORTED_RATES + MLAN_SUB_COMMAND_SIZE;
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get data rates
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_rate_ioctl_get_data_rate(IN pmlan_adapter pmadapter,
+                              IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    if (pioctl_req->action != MLAN_ACT_GET) {
+        ret = MLAN_STATUS_FAILURE;
+        goto exit;
+    }
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_TX_RATE_QUERY,
+                           HostCmd_ACT_GEN_GET,
+                           0, (t_void *) pioctl_req, MNULL);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Rate command handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_rate_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_rate *rate = MNULL;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_rate)) {
+        PRINTM(MWARN, "MLAN bss IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_rate);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+    rate = (mlan_ds_rate *) pioctl_req->pbuf;
+    switch (rate->sub_command) {
+    case MLAN_OID_RATE_CFG:
+        status = wlan_rate_ioctl_cfg(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_GET_DATA_RATE:
+        status = wlan_rate_ioctl_get_data_rate(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_SUPPORTED_RATES:
+        status = wlan_rate_ioctl_get_supported_rate(pmadapter, pioctl_req);
+        break;
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Get Tx power configuration
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param cmd_no	Firmware command number used to retrieve power values
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_power_ioctl_get_power(IN pmlan_adapter pmadapter,
+                           IN t_u16 cmd_no, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           cmd_no,
+                           HostCmd_ACT_GEN_GET,
+                           0, (t_void *) pioctl_req, MNULL);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set Tx power configuration
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_power_ioctl_set_power(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_ds_power_cfg *power = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    HostCmd_DS_TXPWR_CFG *txp_cfg = MNULL;
+    MrvlTypes_Power_Group_t *pg_tlv = MNULL;
+    Power_Group_t *pg = MNULL;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    t_u8 *buf = MNULL;
+    t_u16 dbm = 0;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    power = (mlan_ds_power_cfg *) pioctl_req->pbuf;
+    if (!power->param.power_cfg.is_power_auto) {
+        dbm = (t_u16) power->param.power_cfg.power_level;
+        if ((dbm < pmpriv->min_tx_power_level) ||
+            (dbm > pmpriv->max_tx_power_level)) {
+            PRINTM(MERROR,
+                   "The set txpower value %d dBm is out of range (%d dBm-%d dBm)!\n",
+                   dbm, pmpriv->min_tx_power_level, pmpriv->max_tx_power_level);
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+            goto exit;
+        }
+    }
+
+    ret = pcb->moal_malloc(pmadapter->pmoal_handle,
+                           MRVDRV_SIZE_OF_CMD_BUFFER, MLAN_MEM_DEF, &buf);
+    if (ret != MLAN_STATUS_SUCCESS || buf == MNULL) {
+        PRINTM(MERROR, "ALLOC_CMD_BUF: Failed to allocate command buffer\n");
+        pioctl_req->status_code = MLAN_ERROR_NO_MEM;
+        ret = MLAN_STATUS_FAILURE;
+        goto exit;
+    }
+    memset(pmadapter, buf, 0, MRVDRV_SIZE_OF_CMD_BUFFER);
+    txp_cfg = (HostCmd_DS_TXPWR_CFG *) buf;
+    txp_cfg->action = HostCmd_ACT_GEN_SET;
+    if (!power->param.power_cfg.is_power_auto) {
+        txp_cfg->mode = 1;
+        pg_tlv = (MrvlTypes_Power_Group_t *) (buf +
+                                              sizeof(HostCmd_DS_TXPWR_CFG));
+        pg_tlv->type = TLV_TYPE_POWER_GROUP;
+        pg_tlv->length = 4 * sizeof(Power_Group_t);
+        pg = (Power_Group_t *) (buf + sizeof(HostCmd_DS_TXPWR_CFG) +
+                                sizeof(MrvlTypes_Power_Group_t));
+        /* Power group for modulation class HR/DSSS */
+        pg->first_rate_code = 0x00;
+        pg->last_rate_code = 0x03;
+        pg->modulation_class = MOD_CLASS_HR_DSSS;
+        pg->power_step = 0;
+        pg->power_min = (t_s8) dbm;
+        pg->power_max = (t_s8) dbm;
+        pg++;
+        /* Power group for modulation class OFDM */
+        pg->first_rate_code = 0x00;
+        pg->last_rate_code = 0x07;
+        pg->modulation_class = MOD_CLASS_OFDM;
+        pg->power_step = 0;
+        pg->power_min = (t_s8) dbm;
+        pg->power_max = (t_s8) dbm;
+        pg++;
+        /* Power group for modulation class HTBW20 */
+        pg->first_rate_code = 0x00;
+        pg->last_rate_code = 0x20;
+        pg->modulation_class = MOD_CLASS_HT;
+        pg->power_step = 0;
+        pg->power_min = (t_s8) dbm;
+        pg->power_max = (t_s8) dbm;
+        pg->ht_bandwidth = HT_BW_20;
+        pg++;
+        /* Power group for modulation class HTBW40 */
+        pg->first_rate_code = 0x00;
+        pg->last_rate_code = 0x20;
+        pg->modulation_class = MOD_CLASS_HT;
+        pg->power_step = 0;
+        pg->power_min = (t_s8) dbm;
+        pg->power_max = (t_s8) dbm;
+        pg->ht_bandwidth = HT_BW_40;
+    }
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_TXPWR_CFG,
+                           HostCmd_ACT_GEN_SET, 0, (t_void *) pioctl_req, buf);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    if (buf)
+        pcb->moal_mfree(pmadapter->pmoal_handle, buf);
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get modulation class from rate index
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param rate_index	Rate index
+ *
+ *  @return		0 fail, otherwise return modulation class
+ */
+static int
+wlan_get_modulation_class(pmlan_adapter pmadapter, int rate_index)
+{
+    int tx_mcs_supp = GET_TXMCSSUPP(pmadapter->usr_dev_mcs_support);
+    ENTER();
+    if (rate_index >= MLAN_RATE_INDEX_HRDSSS0 &&
+        rate_index <= MLAN_RATE_INDEX_HRDSSS3) {
+        LEAVE();
+        return MOD_CLASS_HR_DSSS;
+    } else if (rate_index >= MLAN_RATE_INDEX_OFDM0 &&
+               rate_index <= MLAN_RATE_INDEX_OFDM7) {
+        LEAVE();
+        return MOD_CLASS_OFDM;
+    } else if (rate_index >= MLAN_RATE_INDEX_MCS0 &&
+               rate_index <= MLAN_RATE_INDEX_MCS127) {
+        if ((rate_index > MLAN_RATE_INDEX_MCS7 &&
+             rate_index <= MLAN_RATE_INDEX_MCS15) && (tx_mcs_supp < 2)) {
+            PRINTM(MERROR,
+                   "HW don't support 2x2, rate_index=%d hw_mcs_supp=0x%x\n",
+                   rate_index, pmadapter->usr_dev_mcs_support);
+            LEAVE();
+            return 0;
+        }
+        LEAVE();
+        return MOD_CLASS_HT;
+    }
+    PRINTM(MERROR, "Invalid rate index = %d supplied!\n", rate_index);
+
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Set extended power configuration
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_power_ioctl_set_power_ext(IN pmlan_adapter pmadapter,
+                               IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_power_cfg *power = MNULL;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    t_u8 *buf = MNULL;
+    HostCmd_DS_TXPWR_CFG *txp_cfg = MNULL;
+    MrvlTypes_Power_Group_t *pg_tlv = MNULL;
+    Power_Group_t *pg = MNULL;
+    int mod_class;
+    t_u32 data[4];
+    t_u8 ht_bw;
+
+    ENTER();
+
+    power = (mlan_ds_power_cfg *) pioctl_req->pbuf;
+    ret =
+        pcb->moal_malloc(pmadapter->pmoal_handle, MRVDRV_SIZE_OF_CMD_BUFFER,
+                         MLAN_MEM_DEF, &buf);
+    if (ret != MLAN_STATUS_SUCCESS || buf == MNULL) {
+        PRINTM(MERROR, "ALLOC_CMD_BUF: Failed to allocate command buffer\n");
+        pioctl_req->status_code = MLAN_ERROR_NO_MEM;
+        ret = MLAN_STATUS_FAILURE;
+        goto exit;
+    }
+    memset(pmadapter, buf, 0, MRVDRV_SIZE_OF_CMD_BUFFER);
+    txp_cfg = (HostCmd_DS_TXPWR_CFG *) buf;
+    txp_cfg->action = HostCmd_ACT_GEN_SET;
+    memcpy(pmadapter, (t_u8 *) & data,
+           (t_u8 *) power->param.power_ext.power_data, sizeof(data));
+    switch (power->param.power_ext.len) {
+    case 1:
+        if (data[0] == 0xFF)
+            txp_cfg->mode = 0;
+        else {
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+        }
+        break;
+    case 2:
+    case 4:
+        ht_bw = (data[0] & TX_RATE_HT_BW40_BIT) ? HT_BW_40 : HT_BW_20;
+        data[0] &= ~TX_RATE_HT_BW40_BIT;
+        if (!(mod_class = wlan_get_modulation_class(pmadapter, data[0]))) {
+            pioctl_req->status_code = MLAN_ERROR_CMD_RESP_FAIL;
+            ret = MLAN_STATUS_FAILURE;
+            break;
+        }
+        if (ht_bw && mod_class != MOD_CLASS_HT) {
+            pioctl_req->status_code = MLAN_ERROR_CMD_RESP_FAIL;
+            ret = MLAN_STATUS_FAILURE;
+            break;
+        }
+        txp_cfg->mode = 1;
+        pg_tlv =
+            (MrvlTypes_Power_Group_t *) (buf + sizeof(HostCmd_DS_TXPWR_CFG));
+        pg_tlv->type = TLV_TYPE_POWER_GROUP;
+        pg_tlv->length = sizeof(Power_Group_t);
+        pg = (Power_Group_t *) (buf + sizeof(HostCmd_DS_TXPWR_CFG) +
+                                sizeof(MrvlTypes_Power_Group_t));
+        pg->modulation_class = (t_u8) mod_class;
+        pg->first_rate_code = (t_u8) data[0];
+        pg->last_rate_code = (t_u8) data[0];
+        if (mod_class == MOD_CLASS_OFDM) {
+            pg->first_rate_code = (t_u8) (data[0] - MLAN_RATE_INDEX_OFDM0);
+            pg->last_rate_code = (t_u8) (data[0] - MLAN_RATE_INDEX_OFDM0);
+        } else if (mod_class == MOD_CLASS_HT) {
+            pg->first_rate_code = (t_u8) (data[0] - MLAN_RATE_INDEX_MCS0);
+            pg->last_rate_code = (t_u8) (data[0] - MLAN_RATE_INDEX_MCS0);
+            pg->ht_bandwidth = ht_bw;
+        }
+        pg->power_min = (t_s8) data[1];
+        pg->power_max = (t_s8) data[1];
+        if (power->param.power_ext.len == 4) {
+            pg->power_max = (t_s8) data[2];
+            pg->power_step = (t_s8) data[3];
+        }
+        break;
+    default:
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        ret = MLAN_STATUS_FAILURE;
+        break;
+    }
+    if (ret == MLAN_STATUS_FAILURE) {
+        if (buf)
+            pcb->moal_mfree(pmadapter->pmoal_handle, buf);
+        goto exit;
+    }
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_TXPWR_CFG,
+                           HostCmd_ACT_GEN_SET, 0, (t_void *) pioctl_req, buf);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+    if (buf)
+        pcb->moal_mfree(pmadapter->pmoal_handle, buf);
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Power configuration command handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_power_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_power_cfg *power = MNULL;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_power_cfg)) {
+        PRINTM(MWARN, "MLAN bss IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_power_cfg);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+    power = (mlan_ds_power_cfg *) pioctl_req->pbuf;
+    switch (power->sub_command) {
+    case MLAN_OID_POWER_CFG:
+        if (pioctl_req->action == MLAN_ACT_GET)
+            status = wlan_power_ioctl_get_power(pmadapter,
+                                                HostCmd_CMD_TXPWR_CFG,
+                                                pioctl_req);
+        else
+            status = wlan_power_ioctl_set_power(pmadapter, pioctl_req);
+        break;
+
+    case MLAN_OID_POWER_CFG_EXT:
+        if (pioctl_req->action == MLAN_ACT_GET)
+            status = wlan_power_ioctl_get_power(pmadapter,
+                                                HostCmd_CMD_TXPWR_CFG,
+                                                pioctl_req);
+        else
+            status = wlan_power_ioctl_set_power_ext(pmadapter, pioctl_req);
+        break;
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Set power save configurations
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *  @param ps_mode	Power save mode
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_pm_ioctl_ps_mode(IN pmlan_adapter pmadapter,
+                      IN pmlan_ioctl_req pioctl_req, IN t_u16 ps_mode)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    t_u16 sub_cmd;
+
+    ENTER();
+
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        sub_cmd = (pmadapter->ps_mode == Wlan802_11PowerModePSP) ?
+            EN_AUTO_PS : DIS_AUTO_PS;
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_802_11_PS_MODE_ENH, sub_cmd,
+                               BITMAP_STA_PS, (t_void *) pioctl_req, MNULL);
+        if ((ret == MLAN_STATUS_SUCCESS) && (sub_cmd == DIS_AUTO_PS)) {
+            ret = wlan_prepare_cmd(pmpriv,
+                                   HostCmd_CMD_802_11_PS_MODE_ENH, GET_PS,
+                                   0, MNULL, MNULL);
+        }
+    } else {
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_802_11_PS_MODE_ENH, GET_PS,
+                               0, (t_void *) pioctl_req, MNULL);
+    }
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get Inactivity timeout extend
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_pm_ioctl_inactivity_timeout(IN pmlan_adapter pmadapter,
+                                 IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_pm_cfg *pmcfg = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 cmd_action = 0;
+
+    ENTER();
+
+    pmcfg = (mlan_ds_pm_cfg *) pioctl_req->pbuf;
+    cmd_action = HostCmd_ACT_GEN_GET;
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        cmd_action = HostCmd_ACT_GEN_SET;
+    }
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_INACTIVITY_TIMEOUT_EXT,
+                           cmd_action, 0, (t_void *) pioctl_req,
+                           (t_void *) & pmcfg->param.inactivity_to);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Enable/Disable Auto Deep Sleep
+ *
+ *  @param pmadapter 	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_set_auto_deep_sleep(IN pmlan_adapter pmadapter,
+                         IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv =
+        (pmlan_private) pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_auto_ds auto_ds;
+    t_u32 mode;
+
+    ENTER();
+
+    if (((mlan_ds_pm_cfg *) pioctl_req->pbuf)->param.auto_deep_sleep.auto_ds ==
+        DEEP_SLEEP_ON) {
+        auto_ds.auto_ds = DEEP_SLEEP_ON;
+        PRINTM(MINFO, "Auto Deep Sleep: on\n");
+        mode = EN_AUTO_PS;
+    } else {
+        auto_ds.auto_ds = DEEP_SLEEP_OFF;
+        PRINTM(MINFO, "Auto Deep Sleep: off\n");
+        mode = DIS_AUTO_PS;
+    }
+    if (((mlan_ds_pm_cfg *) pioctl_req->pbuf)->param.auto_deep_sleep.idletime)
+        auto_ds.idletime =
+            ((mlan_ds_pm_cfg *) pioctl_req->pbuf)->param.auto_deep_sleep.
+            idletime;
+    else
+        auto_ds.idletime = pmadapter->idle_time;
+    /* note: the command could be queued and executed later if there is
+       command in progress. */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_PS_MODE_ENH,
+                           (t_u16) mode,
+                           BITMAP_AUTO_DS, (t_void *) pioctl_req, &auto_ds);
+    if (ret) {
+        LEAVE();
+        return ret;
+    }
+    ret = MLAN_STATUS_PENDING;
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get sleep period
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_set_get_sleep_pd(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_pm_cfg *pm_cfg = MNULL;
+    t_u16 cmd_action = 0, sleep_pd = 0;
+
+    ENTER();
+
+    pm_cfg = (mlan_ds_pm_cfg *) pioctl_req->pbuf;
+    cmd_action = HostCmd_ACT_GEN_GET;
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        cmd_action = HostCmd_ACT_GEN_SET;
+        sleep_pd = (t_u16) pm_cfg->param.sleep_period;
+    }
+
+    ret = wlan_prepare_cmd(pmpriv, HostCmd_CMD_802_11_SLEEP_PERIOD,
+                           cmd_action, 0, (t_void *) pioctl_req, &sleep_pd);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get PS configuration parameter
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_set_get_ps_cfg(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_ds_pm_cfg *pm_cfg = MNULL;
+
+    ENTER();
+
+    pm_cfg = (mlan_ds_pm_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        pm_cfg->param.ps_cfg.ps_null_interval =
+            (t_u32) pmadapter->null_pkt_interval;
+        pm_cfg->param.ps_cfg.multiple_dtim_interval =
+            (t_u32) pmadapter->multiple_dtim;
+        pm_cfg->param.ps_cfg.listen_interval =
+            (t_u32) pmadapter->local_listen_interval;
+        pm_cfg->param.ps_cfg.adhoc_awake_period =
+            (t_u32) pmadapter->adhoc_awake_period;
+        pm_cfg->param.ps_cfg.bcn_miss_timeout =
+            (t_u32) pmadapter->bcn_miss_time_out;
+        pm_cfg->param.ps_cfg.delay_to_ps = (t_u32) pmadapter->delay_to_ps;
+        pm_cfg->param.ps_cfg.ps_mode = (t_u32) pmadapter->enhanced_ps_mode;
+    } else {
+        if (pm_cfg->param.ps_cfg.ps_null_interval)
+            pmadapter->null_pkt_interval =
+                (t_u16) pm_cfg->param.ps_cfg.ps_null_interval;
+        else
+            pm_cfg->param.ps_cfg.ps_null_interval =
+                (t_u32) pmadapter->null_pkt_interval;
+        if (pm_cfg->param.ps_cfg.multiple_dtim_interval)
+            pmadapter->multiple_dtim =
+                (t_u16) pm_cfg->param.ps_cfg.multiple_dtim_interval;
+        else
+            pm_cfg->param.ps_cfg.multiple_dtim_interval =
+                (t_u32) pmadapter->multiple_dtim;
+        if (((t_s32) pm_cfg->param.ps_cfg.listen_interval) ==
+            MRVDRV_LISTEN_INTERVAL_DISABLE)
+            pmadapter->local_listen_interval = 0;
+        else if (pm_cfg->param.ps_cfg.listen_interval)
+            pmadapter->local_listen_interval =
+                (t_u16) pm_cfg->param.ps_cfg.listen_interval;
+        else
+            pm_cfg->param.ps_cfg.listen_interval =
+                (t_u32) pmadapter->local_listen_interval;
+        if (pm_cfg->param.ps_cfg.adhoc_awake_period)
+            pmadapter->adhoc_awake_period =
+                (t_u16) pm_cfg->param.ps_cfg.adhoc_awake_period;
+        else
+            pm_cfg->param.ps_cfg.adhoc_awake_period =
+                (t_u32) pmadapter->adhoc_awake_period;
+        if (pm_cfg->param.ps_cfg.bcn_miss_timeout)
+            pmadapter->bcn_miss_time_out =
+                (t_u16) pm_cfg->param.ps_cfg.bcn_miss_timeout;
+        else
+            pm_cfg->param.ps_cfg.bcn_miss_timeout =
+                (t_u32) pmadapter->bcn_miss_time_out;
+        if (pm_cfg->param.ps_cfg.delay_to_ps != DELAY_TO_PS_UNCHANGED)
+            pmadapter->delay_to_ps = (t_u16) pm_cfg->param.ps_cfg.delay_to_ps;
+        else
+            pm_cfg->param.ps_cfg.delay_to_ps = (t_u32) pmadapter->delay_to_ps;
+        if (pm_cfg->param.ps_cfg.ps_mode)
+            pmadapter->enhanced_ps_mode = (t_u16) pm_cfg->param.ps_cfg.ps_mode;
+        else
+            pm_cfg->param.ps_cfg.ps_mode = (t_u32) pmadapter->enhanced_ps_mode;
+    }
+    pioctl_req->data_read_written = sizeof(t_u32) + MLAN_SUB_COMMAND_SIZE;
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Get/Set the sleep parameters
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		        MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_set_get_sleep_params(IN pmlan_adapter pmadapter,
+                          IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_pm_cfg *pm_cfg = MNULL;
+    t_u16 cmd_action = 0;
+
+    ENTER();
+
+    pm_cfg = (mlan_ds_pm_cfg *) pioctl_req->pbuf;
+    cmd_action = HostCmd_ACT_GEN_GET;
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        cmd_action = HostCmd_ACT_GEN_SET;
+    }
+
+    /* Send command to firmware */
+    ret = wlan_prepare_cmd(pmpriv, HostCmd_CMD_802_11_SLEEP_PARAMS,
+                           cmd_action, 0, (t_void *) pioctl_req,
+                           &pm_cfg->param.sleep_params);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Power save command handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_pm_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_pm_cfg *pm = MNULL;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_pm_cfg)) {
+        PRINTM(MWARN, "MLAN bss IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_pm_cfg);
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+    pm = (mlan_ds_pm_cfg *) pioctl_req->pbuf;
+    switch (pm->sub_command) {
+    case MLAN_OID_PM_CFG_IEEE_PS:
+        switch (pioctl_req->action) {
+        case MLAN_ACT_SET:
+            if (pm->param.ps_mode)
+                pmadapter->ps_mode = Wlan802_11PowerModePSP;
+            else
+                pmadapter->ps_mode = Wlan802_11PowerModeCAM;
+            status =
+                wlan_pm_ioctl_ps_mode(pmadapter, pioctl_req,
+                                      pmadapter->ps_mode);
+            break;
+        case MLAN_ACT_GET:
+            status =
+                wlan_pm_ioctl_ps_mode(pmadapter, pioctl_req,
+                                      pmadapter->ps_mode);
+            break;
+        default:
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+            status = MLAN_STATUS_FAILURE;
+            break;
+        }
+        break;
+    case MLAN_OID_PM_CFG_HS_CFG:
+        status = wlan_pm_ioctl_hscfg(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_PM_CFG_INACTIVITY_TO:
+        status = wlan_pm_ioctl_inactivity_timeout(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_PM_CFG_DEEP_SLEEP:
+        switch (pioctl_req->action) {
+        case MLAN_ACT_SET:
+            if (pmadapter->is_deep_sleep &&
+                pm->param.auto_deep_sleep.auto_ds == DEEP_SLEEP_ON) {
+                PRINTM(MMSG, "Station already in enhanced deep sleep mode\n");
+                status = MLAN_STATUS_FAILURE;
+                break;
+            } else if (!pmadapter->is_deep_sleep &&
+                       pm->param.auto_deep_sleep.auto_ds == DEEP_SLEEP_OFF) {
+                PRINTM(MMSG,
+                       "Station already not in enhanced deep sleep mode\n");
+                status = MLAN_STATUS_FAILURE;
+                break;
+            }
+            status = wlan_set_auto_deep_sleep(pmadapter, pioctl_req);
+            break;
+        case MLAN_ACT_GET:
+            if (pmadapter->is_deep_sleep) {
+                pm->param.auto_deep_sleep.auto_ds = DEEP_SLEEP_ON;
+                pm->param.auto_deep_sleep.idletime = pmadapter->idle_time;
+            } else
+                pm->param.auto_deep_sleep.auto_ds = DEEP_SLEEP_OFF;
+            break;
+        default:
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+            status = MLAN_STATUS_FAILURE;
+            break;
+        }
+        break;
+    case MLAN_OID_PM_CFG_PS_CFG:
+        status = wlan_set_get_ps_cfg(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_PM_CFG_SLEEP_PD:
+        status = wlan_set_get_sleep_pd(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_PM_CFG_SLEEP_PARAMS:
+        status = wlan_set_get_sleep_params(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_PM_INFO:
+        status = wlan_get_pm_info(pmadapter, pioctl_req);
+        break;
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Set/Get WMM status
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_wmm_ioctl_enable(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_wmm_cfg *wmm = MNULL;
+    ENTER();
+    wmm = (mlan_ds_wmm_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET)
+        wmm->param.wmm_enable = (t_u32) pmpriv->wmm_required;
+    else
+        pmpriv->wmm_required = (t_u8) wmm->param.wmm_enable;
+    pioctl_req->data_read_written = sizeof(t_u32) + MLAN_SUB_COMMAND_SIZE;
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get WMM QoS configuration
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_wmm_ioctl_qos(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_wmm_cfg *wmm = MNULL;
+
+    ENTER();
+
+    wmm = (mlan_ds_wmm_cfg *) pioctl_req->pbuf;
+
+    if (pioctl_req->action == MLAN_ACT_GET)
+        wmm->param.qos_cfg = pmpriv->wmm_qosinfo;
+    else {
+        pmpriv->wmm_qosinfo = wmm->param.qos_cfg;
+    }
+
+    pioctl_req->data_read_written = sizeof(t_u8) + MLAN_SUB_COMMAND_SIZE;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Request for add a TSPEC
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_wmm_ioctl_addts_req(IN pmlan_adapter pmadapter,
+                         IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_wmm_cfg *cfg = MNULL;
+
+    ENTER();
+    cfg = (mlan_ds_wmm_cfg *) pioctl_req->pbuf;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv, HostCmd_CMD_WMM_ADDTS_REQ,
+                           0, 0, (t_void *) pioctl_req,
+                           (t_void *) & cfg->param.addts);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Request for delete a TSPEC
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_wmm_ioctl_delts_req(IN pmlan_adapter pmadapter,
+                         IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_wmm_cfg *cfg = MNULL;
+
+    ENTER();
+    cfg = (mlan_ds_wmm_cfg *) pioctl_req->pbuf;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv, HostCmd_CMD_WMM_DELTS_REQ,
+                           0, 0, (t_void *) pioctl_req,
+                           (t_void *) & cfg->param.delts);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get a specified AC Queue's parameters
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_wmm_ioctl_queue_config(IN pmlan_adapter pmadapter,
+                            IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_wmm_cfg *cfg = MNULL;
+
+    ENTER();
+    cfg = (mlan_ds_wmm_cfg *) pioctl_req->pbuf;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv, HostCmd_CMD_WMM_QUEUE_CONFIG,
+                           0, 0, (t_void *) pioctl_req,
+                           (t_void *) & cfg->param.q_cfg);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief To get and start/stop queue stats on a WMM AC
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_wmm_ioctl_queue_stats(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_wmm_cfg *cfg = MNULL;
+
+    ENTER();
+    cfg = (mlan_ds_wmm_cfg *) pioctl_req->pbuf;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv, HostCmd_CMD_WMM_QUEUE_STATS,
+                           0, 0, (t_void *) pioctl_req,
+                           (t_void *) & cfg->param.q_stats);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get the status of the WMM AC queues
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_wmm_ioctl_queue_status(IN pmlan_adapter pmadapter,
+                            IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_wmm_cfg *cfg = MNULL;
+    mlan_ds_wmm_queue_status *pqstatus = MNULL;
+    WmmAcStatus_t *pac_status = MNULL;
+    mlan_wmm_ac_e ac_idx;
+
+    ENTER();
+
+    cfg = (mlan_ds_wmm_cfg *) pioctl_req->pbuf;
+    pqstatus = (mlan_ds_wmm_queue_status *) & cfg->param.q_status;
+
+    for (ac_idx = WMM_AC_BK; ac_idx <= WMM_AC_VO; ac_idx++) {
+        pac_status = &pmpriv->wmm.ac_status[ac_idx];
+
+        /* Firmware status */
+        pqstatus->ac_status[ac_idx].flow_required = pac_status->flow_required;
+        pqstatus->ac_status[ac_idx].flow_created = pac_status->flow_created;
+        pqstatus->ac_status[ac_idx].disabled = pac_status->disabled;
+
+        /* ACM bit reflected in firmware status (redundant) */
+        pqstatus->ac_status[ac_idx].wmm_acm = pac_status->flow_required;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get the status of the WMM Traffic Streams
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_wmm_ioctl_ts_status(IN pmlan_adapter pmadapter,
+                         IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_wmm_cfg *cfg = MNULL;
+
+    ENTER();
+
+    cfg = (mlan_ds_wmm_cfg *) pioctl_req->pbuf;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv, HostCmd_CMD_WMM_TS_STATUS,
+                           0, 0, (t_void *) pioctl_req,
+                           (t_void *) & cfg->param.ts_status);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief WMM configuration handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_wmm_cfg_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_wmm_cfg *wmm = MNULL;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_wmm_cfg)) {
+        PRINTM(MWARN, "MLAN bss IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_wmm_cfg);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+    wmm = (mlan_ds_wmm_cfg *) pioctl_req->pbuf;
+    switch (wmm->sub_command) {
+    case MLAN_OID_WMM_CFG_ENABLE:
+        status = wlan_wmm_ioctl_enable(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_WMM_CFG_QOS:
+        status = wlan_wmm_ioctl_qos(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_WMM_CFG_ADDTS:
+        status = wlan_wmm_ioctl_addts_req(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_WMM_CFG_DELTS:
+        status = wlan_wmm_ioctl_delts_req(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_WMM_CFG_QUEUE_CONFIG:
+        status = wlan_wmm_ioctl_queue_config(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_WMM_CFG_QUEUE_STATS:
+        status = wlan_wmm_ioctl_queue_stats(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_WMM_CFG_QUEUE_STATUS:
+        status = wlan_wmm_ioctl_queue_status(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_WMM_CFG_TS_STATUS:
+        status = wlan_wmm_ioctl_ts_status(pmadapter, pioctl_req);
+        break;
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Set/Get WPA IE
+ *
+ *  @param priv         A pointer to mlan_private structure
+ *  @param ie_data_ptr  A pointer to IE
+ *  @param ie_len       Length of the IE
+ *
+ *  @return             MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_set_wpa_ie_helper(mlan_private * priv, t_u8 * ie_data_ptr, t_u16 ie_len)
+{
+    ENTER();
+
+    if (ie_len) {
+        if (ie_len > sizeof(priv->wpa_ie)) {
+            PRINTM(MERROR, "failed to copy, WPA IE is too big \n");
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+        memcpy(priv->adapter, priv->wpa_ie, ie_data_ptr, ie_len);
+        priv->wpa_ie_len = (t_u8) ie_len;
+        PRINTM(MIOCTL, "Set Wpa_ie_len=%d IE=%#x\n", priv->wpa_ie_len,
+               priv->wpa_ie[0]);
+        DBG_HEXDUMP(MCMD_D, "Wpa_ie", priv->wpa_ie, priv->wpa_ie_len);
+        if (priv->wpa_ie[0] == WPA_IE) {
+            priv->sec_info.wpa_enabled = MTRUE;
+        } else if (priv->wpa_ie[0] == RSN_IE) {
+            priv->sec_info.wpa2_enabled = MTRUE;
+        } else {
+            priv->sec_info.wpa_enabled = MFALSE;
+            priv->sec_info.wpa2_enabled = MFALSE;
+        }
+    } else {
+        memset(priv->adapter, priv->wpa_ie, 0, sizeof(priv->wpa_ie));
+        priv->wpa_ie_len = 0;
+        PRINTM(MINFO, "Reset Wpa_ie_len=%d IE=%#x\n", priv->wpa_ie_len,
+               priv->wpa_ie[0]);
+        priv->sec_info.wpa_enabled = MFALSE;
+        priv->sec_info.wpa2_enabled = MFALSE;
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Set WAPI IE
+ *
+ *  @param priv         A pointer to mlan_private structure
+ *  @param ie_data_ptr  A pointer to IE
+ *  @param ie_len       Length of the IE
+ *
+ *  @return             MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_set_wapi_ie(mlan_private * priv, t_u8 * ie_data_ptr, t_u16 ie_len)
+{
+    ENTER();
+    if (ie_len) {
+        if (ie_len > sizeof(priv->wapi_ie)) {
+            PRINTM(MWARN, "failed to copy, WAPI IE is too big \n");
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+        memcpy(priv->adapter, priv->wapi_ie, ie_data_ptr, ie_len);
+        priv->wapi_ie_len = (t_u8) ie_len;
+        PRINTM(MIOCTL, "Set wapi_ie_len=%d IE=%#x\n", priv->wapi_ie_len,
+               priv->wapi_ie[0]);
+        DBG_HEXDUMP(MCMD_D, "wapi_ie", priv->wapi_ie, priv->wapi_ie_len);
+        if (priv->wapi_ie[0] == WAPI_IE)
+            priv->sec_info.wapi_enabled = MTRUE;
+    } else {
+        memset(priv->adapter, priv->wapi_ie, 0, sizeof(priv->wapi_ie));
+        priv->wapi_ie_len = (t_u8) ie_len;
+        PRINTM(MINFO, "Reset wapi_ie_len=%d IE=%#x\n", priv->wapi_ie_len,
+               priv->wapi_ie[0]);
+        priv->sec_info.wapi_enabled = MFALSE;
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Set/Get WAPI status
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_sec_ioctl_wapi_enable(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_sec_cfg *sec = MNULL;
+    ENTER();
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        if (pmpriv->wapi_ie_len)
+            sec->param.wapi_enabled = MTRUE;
+        else
+            sec->param.wapi_enabled = MFALSE;
+    } else {
+        if (sec->param.wapi_enabled == MFALSE) {
+            wlan_set_wapi_ie(pmpriv, MNULL, 0);
+        }
+    }
+    pioctl_req->data_read_written = sizeof(t_u32) + MLAN_SUB_COMMAND_SIZE;
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set WAPI key
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_sec_ioctl_set_wapi_key(IN pmlan_adapter pmadapter,
+                            IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_sec_cfg *sec = MNULL;
+    ENTER();
+
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_KEY_MATERIAL,
+                           HostCmd_ACT_GEN_SET,
+                           KEY_INFO_ENABLED,
+                           (t_void *) pioctl_req, &sec->param.encrypt_key);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get Port Control status
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_sec_ioctl_port_ctrl_enable(IN pmlan_adapter pmadapter,
+                                IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_sec_cfg *sec = MNULL;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        if (pmpriv->port_ctrl_mode)
+            sec->param.port_ctrl_enabled = MTRUE;
+        else
+            sec->param.port_ctrl_enabled = MFALSE;
+    } else {
+        if (sec->param.port_ctrl_enabled) {
+            pmpriv->port_ctrl_mode = MTRUE;
+            pmpriv->port_open = MFALSE;
+        } else {
+            if (pmpriv->port_ctrl_mode == MTRUE) {
+                pmpriv->port_ctrl_mode = MFALSE;
+                /* Cleanup the bypass TX queue */
+                wlan_cleanup_bypass_txq(pmadapter);
+            }
+        }
+    }
+    PRINTM(MINFO, "port_ctrl: port_ctrl_mode=%d port_open=%d\n",
+           pmpriv->port_ctrl_mode, pmpriv->port_open);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get authentication mode
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_sec_ioctl_auth_mode(IN pmlan_adapter pmadapter,
+                         IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_sec_cfg *sec = MNULL;
+    ENTER();
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET)
+        sec->param.auth_mode = pmpriv->sec_info.authentication_mode;
+    else {
+        pmpriv->sec_info.authentication_mode = sec->param.auth_mode;
+        if (pmpriv->sec_info.authentication_mode == MLAN_AUTH_MODE_NETWORKEAP)
+            wlan_set_wpa_ie_helper(pmpriv, MNULL, 0);
+    }
+    pioctl_req->data_read_written = sizeof(t_u32) + MLAN_SUB_COMMAND_SIZE;
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get encryption mode
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_sec_ioctl_encrypt_mode(IN pmlan_adapter pmadapter,
+                            IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_sec_cfg *sec = MNULL;
+    ENTER();
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET)
+        sec->param.encrypt_mode = pmpriv->sec_info.encryption_mode;
+    else {
+        pmpriv->sec_info.encryption_mode = sec->param.encrypt_mode;
+    }
+    pioctl_req->data_read_written = sizeof(t_u32) + MLAN_SUB_COMMAND_SIZE;
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get WPA status
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_sec_ioctl_wpa_enable(IN pmlan_adapter pmadapter,
+                          IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_sec_cfg *sec = MNULL;
+    ENTER();
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        if (pmpriv->wpa_ie_len)
+            sec->param.wpa_enabled = MTRUE;
+        else
+            sec->param.wpa_enabled = MFALSE;
+    } else {
+        if (sec->param.wpa_enabled == MFALSE) {
+            wlan_set_wpa_ie_helper(pmpriv, MNULL, 0);
+        }
+                /** clear adhoc aes flag, when WPA enabled */
+        pmpriv->adhoc_aes_enabled = MFALSE;
+        pmpriv->aes_key.key_param_set.key_len = 0;
+    }
+    pioctl_req->data_read_written = sizeof(t_u32) + MLAN_SUB_COMMAND_SIZE;
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set WEP keys
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_sec_ioctl_set_wep_key(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_sec_cfg *sec = MNULL;
+    mrvl_wep_key_t *pwep_key = MNULL;
+    int index;
+
+    ENTER();
+
+    if (pmpriv->wep_key_curr_index >= MRVL_NUM_WEP_KEY)
+        pmpriv->wep_key_curr_index = 0;
+    pwep_key = &pmpriv->wep_key[pmpriv->wep_key_curr_index];
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+    if (sec->param.encrypt_key.key_index == MLAN_KEY_INDEX_DEFAULT) {
+        index = pmpriv->wep_key_curr_index;
+    } else
+        index = sec->param.encrypt_key.key_index;
+    if ((sec->param.encrypt_key.key_disable == MTRUE) ||
+        (sec->param.encrypt_key.key_remove == MTRUE)) {
+        pmpriv->sec_info.wep_status = Wlan802_11WEPDisabled;
+        /* remove key */
+        if (sec->param.encrypt_key.key_remove == MTRUE) {
+            memset(pmadapter, &pmpriv->wep_key[index], 0,
+                   sizeof(mrvl_wep_key_t));
+        }
+    } else {
+        if (sec->param.encrypt_key.key_len) {
+            pwep_key = &pmpriv->wep_key[index];
+            /* Cleanup */
+            memset(pmadapter, pwep_key, 0, sizeof(mrvl_wep_key_t));
+            /* Copy the key in the driver */
+
+            memcpy(pmadapter, pwep_key->key_material,
+                   sec->param.encrypt_key.key_material,
+                   sec->param.encrypt_key.key_len);
+            pwep_key->key_index = index;
+            pwep_key->key_length = sec->param.encrypt_key.key_len;
+            if (pmpriv->sec_info.wep_status != Wlan802_11WEPEnabled) {
+                /*
+                 * The status is set as Key Absent
+                 * so as to make sure we display the
+                 * keys when iwlist mlanX key is used
+                 */
+                pmpriv->sec_info.wep_status = Wlan802_11WEPKeyAbsent;
+            }
+        }
+        if (sec->param.encrypt_key.is_current_wep_key == MTRUE) {
+            /* Copy the required key as the current key */
+            pwep_key = &pmpriv->wep_key[index];
+            if (!pwep_key->key_length) {
+                PRINTM(MERROR, "Key %d not set,so cannot enable it\n", index);
+                pioctl_req->status_code = MLAN_ERROR_CMD_RESP_FAIL;
+                ret = MLAN_STATUS_FAILURE;
+                goto exit;
+            }
+            pmpriv->wep_key_curr_index = (t_u16) index;
+            pmpriv->sec_info.wep_status = Wlan802_11WEPEnabled;
+        }
+    }
+    if (pmpriv->sec_info.wep_status == Wlan802_11WEPEnabled)
+        pmpriv->curr_pkt_filter |= HostCmd_ACT_MAC_WEP_ENABLE;
+    else
+        pmpriv->curr_pkt_filter &= ~HostCmd_ACT_MAC_WEP_ENABLE;
+
+    /* Send request to firmware */
+    if (pmpriv->sec_info.wep_status == Wlan802_11WEPEnabled
+        && pwep_key->key_length) {
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_MAC_CONTROL,
+                               HostCmd_ACT_GEN_SET,
+                               0, MNULL, &pmpriv->curr_pkt_filter);
+        if (ret)
+            goto exit;
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_802_11_KEY_MATERIAL,
+                               HostCmd_ACT_GEN_SET,
+                               0, (t_void *) pioctl_req, MNULL);
+    } else {
+        if (pwep_key->key_length) {
+            ret = wlan_prepare_cmd(pmpriv,
+                                   HostCmd_CMD_802_11_KEY_MATERIAL,
+                                   HostCmd_ACT_GEN_SET, 0, MNULL, MNULL);
+            if (ret)
+                goto exit;
+        }
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_MAC_CONTROL,
+                               HostCmd_ACT_GEN_SET,
+                               0,
+                               (t_void *) pioctl_req, &pmpriv->curr_pkt_filter);
+    }
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set WPA key
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_sec_ioctl_set_wpa_key(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_sec_cfg *sec = MNULL;
+    t_u8 broadcast_mac_addr[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+    t_u8 remove_key = MFALSE;
+
+    ENTER();
+
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+    /* Current driver only supports key length of up to 32 bytes */
+    if (sec->param.encrypt_key.key_len > MLAN_MAX_KEY_LENGTH) {
+        PRINTM(MERROR, "Key length is incorrect\n");
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        ret = MLAN_STATUS_FAILURE;
+        goto exit;
+    }
+    if ((pmpriv->bss_mode == MLAN_BSS_MODE_IBSS) &&
+        pmpriv->sec_info.wpa_enabled) {
+        /*
+         * IBSS/WPA-None uses only one key (Group) for both receiving and
+         *  sending unicast and multicast packets.
+         */
+        /* Send the key as PTK to firmware */
+        sec->param.encrypt_key.key_index = MLAN_KEY_INDEX_UNICAST;
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_802_11_KEY_MATERIAL,
+                               HostCmd_ACT_GEN_SET,
+                               KEY_INFO_ENABLED,
+                               MNULL, &sec->param.encrypt_key);
+        if (ret)
+            goto exit;
+
+        /* Send the key as GTK to firmware */
+        sec->param.encrypt_key.key_index = ~MLAN_KEY_INDEX_UNICAST;
+    }
+
+    if (sec->param.encrypt_key.key_len == WPA_AES_KEY_LEN) {
+        /** back up adhoc AES key */
+        memset(pmpriv->adapter, pmpriv->aes_key.key_param_set.key, 0,
+               sizeof(pmpriv->aes_key.key_param_set.key));
+        pmpriv->aes_key.key_param_set.key_len = sec->param.encrypt_key.key_len;
+        memcpy(pmpriv->adapter, pmpriv->aes_key.key_param_set.key,
+               sec->param.encrypt_key.key_material,
+               pmpriv->aes_key.key_param_set.key_len);
+    }
+
+    /** only adhoc aes key_index = MLAN_KEY_INDEX_UNICAST */
+    if (pmpriv->bss_mode == MLAN_BSS_MODE_IBSS &&
+        sec->param.encrypt_key.key_len == WPA_AES_KEY_LEN
+        && sec->param.encrypt_key.key_index & MLAN_KEY_INDEX_UNICAST) {
+        t_u8 zero_key_material[WPA_AES_KEY_LEN];
+        memset(pmadapter, zero_key_material, 0, sizeof(zero_key_material));
+        if (memcmp
+            (pmadapter, sec->param.encrypt_key.key_material, zero_key_material,
+             WPA_AES_KEY_LEN)) {
+            PRINTM(MINFO, "Adhoc AES Enabled.\n");
+            pmpriv->adhoc_aes_enabled = MTRUE;
+            remove_key = MFALSE;
+        } else {
+            PRINTM(MINFO, "Adhoc AES Disabled.\n");
+            pmpriv->adhoc_aes_enabled = MFALSE;
+            /** clear adhoc AES key */
+            remove_key = MTRUE;
+            pmpriv->aes_key.key_param_set.key_len = 0;
+        }
+    }
+
+    if (memcmp
+        (pmadapter, sec->param.encrypt_key.mac_addr, broadcast_mac_addr,
+         MLAN_MAC_ADDR_LENGTH))
+        sec->param.encrypt_key.key_index = MLAN_KEY_INDEX_UNICAST;
+
+    if (remove_key == MTRUE) {
+        /* Send request to firmware */
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_802_11_KEY_MATERIAL,
+                               HostCmd_ACT_GEN_SET,
+                               !(KEY_INFO_ENABLED),
+                               (t_void *) pioctl_req, &sec->param.encrypt_key);
+    } else {
+        /* Send request to firmware */
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_802_11_KEY_MATERIAL,
+                               HostCmd_ACT_GEN_SET,
+                               KEY_INFO_ENABLED,
+                               (t_void *) pioctl_req, &sec->param.encrypt_key);
+    }
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get security keys
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_sec_ioctl_get_key(IN pmlan_adapter pmadapter,
+                       IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_sec_cfg *sec = MNULL;
+    int index;
+    ENTER();
+
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+
+    if ((sec->param.encrypt_key.key_index == MLAN_KEY_INDEX_UNICAST) &&
+        (sec->param.encrypt_key.key_len == WPA_AES_KEY_LEN)) {
+        if (pmpriv->adhoc_aes_enabled == MTRUE &&
+            (pmpriv->aes_key.key_param_set.key_len == WPA_AES_KEY_LEN)) {
+            HEXDUMP("Get ADHOCAES Key", pmpriv->aes_key.key_param_set.key,
+                    WPA_AES_KEY_LEN);
+            memcpy(pmadapter, sec->param.encrypt_key.key_material,
+                   pmpriv->aes_key.key_param_set.key, WPA_AES_KEY_LEN);
+            LEAVE();
+            return ret;
+        } else {
+            PRINTM(MERROR, " ADHOCAES key is not set yet!\n");
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+            LEAVE();
+            return ret;
+        }
+    }
+    if (pmpriv->wep_key_curr_index >= MRVL_NUM_WEP_KEY)
+        pmpriv->wep_key_curr_index = 0;
+
+    if ((pmpriv->sec_info.wep_status == Wlan802_11WEPEnabled)
+        || (pmpriv->sec_info.wep_status == Wlan802_11WEPKeyAbsent)
+        || pmpriv->sec_info.ewpa_enabled
+        || pmpriv->sec_info.wpa_enabled
+        || pmpriv->sec_info.wpa2_enabled || pmpriv->adhoc_aes_enabled) {
+        sec->param.encrypt_key.key_disable = MFALSE;
+    } else {
+        sec->param.encrypt_key.key_disable = MTRUE;
+    }
+    if (sec->param.encrypt_key.key_index == MLAN_KEY_INDEX_DEFAULT) {
+        if ((pmpriv->wep_key[pmpriv->wep_key_curr_index].key_length) &&
+            (pmpriv->sec_info.wep_status == Wlan802_11WEPEnabled)) {
+            index = pmpriv->wep_key_curr_index;
+            sec->param.encrypt_key.key_index = pmpriv->wep_key[index].key_index;
+            memcpy(pmadapter, sec->param.encrypt_key.key_material,
+                   pmpriv->wep_key[index].key_material, MIN(MLAN_MAX_KEY_LENGTH,
+                                                            pmpriv->
+                                                            wep_key[index].
+                                                            key_length));
+            sec->param.encrypt_key.key_len =
+                MIN(MLAN_MAX_KEY_LENGTH, pmpriv->wep_key[index].key_length);
+        } else if ((pmpriv->sec_info.wpa_enabled)
+                   || (pmpriv->sec_info.ewpa_enabled)
+                   || (pmpriv->sec_info.wpa2_enabled)
+                   || (pmpriv->sec_info.wapi_enabled)
+                   || (pmpriv->adhoc_aes_enabled)
+            ) {
+            /* Return WPA enabled */
+            sec->param.encrypt_key.key_disable = MFALSE;
+            memcpy(pmadapter, sec->param.encrypt_key.key_material,
+                   pmpriv->aes_key.key_param_set.key, MIN(MLAN_MAX_KEY_LENGTH,
+                                                          pmpriv->aes_key.
+                                                          key_param_set.
+                                                          key_len));
+            sec->param.encrypt_key.key_len =
+                MIN(MLAN_MAX_KEY_LENGTH, pmpriv->aes_key.key_param_set.key_len);
+        } else {
+            sec->param.encrypt_key.key_disable = MTRUE;
+        }
+    } else {
+        index = sec->param.encrypt_key.key_index;
+        if (pmpriv->wep_key[index].key_length) {
+            sec->param.encrypt_key.key_index = pmpriv->wep_key[index].key_index;
+            memcpy(pmadapter, sec->param.encrypt_key.key_material,
+                   pmpriv->wep_key[index].key_material, MIN(MLAN_MAX_KEY_LENGTH,
+                                                            pmpriv->
+                                                            wep_key[index].
+                                                            key_length));
+            sec->param.encrypt_key.key_len =
+                MIN(MLAN_MAX_KEY_LENGTH, pmpriv->wep_key[index].key_length);
+        } else if ((pmpriv->sec_info.wpa_enabled)
+                   || (pmpriv->sec_info.ewpa_enabled)
+                   || (pmpriv->sec_info.wpa2_enabled)
+                   || (pmpriv->sec_info.wapi_enabled)
+                   || (pmpriv->adhoc_aes_enabled)
+            ) {
+            /* Return WPA enabled */
+            sec->param.encrypt_key.key_disable = MFALSE;
+        } else {
+            sec->param.encrypt_key.key_disable = MTRUE;
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set security key(s)
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_sec_ioctl_encrypt_key(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_sec_cfg *sec = MNULL;
+    ENTER();
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        if (sec->param.encrypt_key.is_wapi_key)
+            status = wlan_sec_ioctl_set_wapi_key(pmadapter, pioctl_req);
+        else if (sec->param.encrypt_key.key_len > MAX_WEP_KEY_SIZE)
+            status = wlan_sec_ioctl_set_wpa_key(pmadapter, pioctl_req);
+        else
+            status = wlan_sec_ioctl_set_wep_key(pmadapter, pioctl_req);
+    } else {
+        status = wlan_sec_ioctl_get_key(pmadapter, pioctl_req);
+    }
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Set/Get WPA passphrase for esupplicant
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_sec_ioctl_passphrase(IN pmlan_adapter pmadapter,
+                          IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_sec_cfg *sec = MNULL;
+    t_u16 cmd_action = 0;
+    BSSDescriptor_t *pbss_desc;
+    int i = 0;
+
+    ENTER();
+
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        if (sec->param.passphrase.psk_type == MLAN_PSK_CLEAR)
+            cmd_action = HostCmd_ACT_GEN_REMOVE;
+        else
+            cmd_action = HostCmd_ACT_GEN_SET;
+    } else {
+        if (sec->param.passphrase.psk_type == MLAN_PSK_QUERY) {
+            if (sec->param.passphrase.ssid.ssid_len == 0) {
+                i = wlan_find_bssid_in_list(pmpriv,
+                                            (t_u8 *) & sec->param.passphrase.
+                                            bssid, MLAN_BSS_MODE_AUTO);
+                if (i >= 0) {
+                    pbss_desc = &pmadapter->pscan_table[i];
+                    memcpy(pmadapter, &sec->param.passphrase.ssid,
+                           &pbss_desc->ssid, sizeof(mlan_802_11_ssid));
+                    memset(pmadapter, &sec->param.passphrase.bssid, 0,
+                           MLAN_MAC_ADDR_LENGTH);
+                    PRINTM(MINFO, "PSK_QUERY: found ssid=%s\n",
+                           sec->param.passphrase.ssid.ssid);
+                }
+            } else
+                memset(pmadapter, &sec->param.passphrase.bssid, 0,
+                       MLAN_MAC_ADDR_LENGTH);
+        }
+        cmd_action = HostCmd_ACT_GEN_GET;
+    }
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_SUPPLICANT_PMK,
+                           cmd_action,
+                           0, (t_void *) pioctl_req, &sec->param.passphrase);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get esupplicant status
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_sec_ioctl_ewpa_enable(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_sec_cfg *sec = MNULL;
+    ENTER();
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        sec->param.ewpa_enabled = pmpriv->sec_info.ewpa_enabled;
+    } else {
+        pmpriv->sec_info.ewpa_enabled = (t_u8) sec->param.ewpa_enabled;
+        PRINTM(MINFO, "Set: ewpa_enabled = %d\n",
+               (int) pmpriv->sec_info.ewpa_enabled);
+    }
+    pioctl_req->data_read_written = sizeof(t_u32) + MLAN_SUB_COMMAND_SIZE;
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get esupplicant mode
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_sec_ioctl_esupp_mode(IN pmlan_adapter pmadapter,
+                          IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    t_u16 cmd_action = 0;
+    mlan_ds_sec_cfg *sec = MNULL;
+
+    ENTER();
+
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        cmd_action = HostCmd_ACT_GEN_SET;
+        if (pmpriv->media_connected == MTRUE) {
+            PRINTM(MERROR,
+                   "Cannot set esupplicant mode configuration while connected.\n");
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+            ret = MLAN_STATUS_FAILURE;
+            goto exit;
+        }
+        if (!sec->param.esupp_mode.rsn_mode ||
+            (sec->param.esupp_mode.rsn_mode & RSN_TYPE_VALID_BITS)
+            != sec->param.esupp_mode.rsn_mode) {
+            PRINTM(MERROR, "Invalid RSN mode\n");
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+            ret = MLAN_STATUS_FAILURE;
+            goto exit;
+        }
+        if (!sec->param.esupp_mode.act_paircipher ||
+            (sec->param.esupp_mode.act_paircipher & EMBED_CIPHER_VALID_BITS)
+            != sec->param.esupp_mode.act_paircipher) {
+            PRINTM(MERROR, "Invalid pairwise cipher\n");
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+            ret = MLAN_STATUS_FAILURE;
+            goto exit;
+        }
+        if (!sec->param.esupp_mode.act_groupcipher ||
+            (sec->param.esupp_mode.act_groupcipher & EMBED_CIPHER_VALID_BITS)
+            != sec->param.esupp_mode.act_groupcipher) {
+            PRINTM(MERROR, "Invalid group cipher\n");
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+            ret = MLAN_STATUS_FAILURE;
+            goto exit;
+        }
+    } else {
+        cmd_action = HostCmd_ACT_GEN_GET_CURRENT;
+    }
+
+    /* Send request to firmware */
+    if (sec) {
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_SUPPLICANT_PROFILE,
+                               cmd_action,
+                               0,
+                               (t_void *) pioctl_req, &sec->param.esupp_mode);
+    } else {
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_SUPPLICANT_PROFILE,
+                               cmd_action, 0, (t_void *) pioctl_req, MNULL);
+    }
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Security configuration handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_sec_cfg_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_sec_cfg *sec = MNULL;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_sec_cfg)) {
+        PRINTM(MWARN, "MLAN bss IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_sec_cfg);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+    switch (sec->sub_command) {
+    case MLAN_OID_SEC_CFG_AUTH_MODE:
+        status = wlan_sec_ioctl_auth_mode(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_SEC_CFG_ENCRYPT_MODE:
+        status = wlan_sec_ioctl_encrypt_mode(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_SEC_CFG_WPA_ENABLED:
+        status = wlan_sec_ioctl_wpa_enable(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_SEC_CFG_WAPI_ENABLED:
+        status = wlan_sec_ioctl_wapi_enable(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_SEC_CFG_PORT_CTRL_ENABLED:
+        status = wlan_sec_ioctl_port_ctrl_enable(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_SEC_CFG_ENCRYPT_KEY:
+        status = wlan_sec_ioctl_encrypt_key(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_SEC_CFG_PASSPHRASE:
+        status = wlan_sec_ioctl_passphrase(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_SEC_CFG_EWPA_ENABLED:
+        status = wlan_sec_ioctl_ewpa_enable(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_SEC_CFG_ESUPP_MODE:
+        status = wlan_sec_ioctl_esupp_mode(pmadapter, pioctl_req);
+        break;
+
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief  Append/Reset IE buffer.
+ *
+ *  Pass an opaque block of data, expected to be IEEE IEs, to the driver
+ *    for eventual passthrough to the firmware in an associate/join
+ *    (and potentially start) command.  This function is the main body
+ *    for both wlan_set_gen_ie_ioctl and wlan_set_gen_ie
+ *
+ *  Data is appended to an existing buffer and then wrapped in a passthrough
+ *    TLV in the command API to the firmware.  The firmware treats the data
+ *    as a transparent passthrough to the transmitted management frame.
+ *
+ *  @param priv         A pointer to mlan_private structure
+ *  @param ie_data_ptr  A pointer to iwreq structure
+ *  @param ie_len       Length of the IE or IE block passed in ie_data_ptr
+ *
+ *  @return             MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static int
+wlan_set_gen_ie_helper(mlan_private * priv, t_u8 * ie_data_ptr, t_u16 ie_len)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    IEEEtypes_VendorHeader_t *pvendor_ie;
+    const t_u8 wpa_oui[] = { 0x00, 0x50, 0xf2, 0x01 };
+    const t_u8 wps_oui[] = { 0x00, 0x50, 0xf2, 0x04 };
+
+    ENTER();
+
+    /* If the passed length is zero, reset the buffer */
+    if (!ie_len) {
+        priv->gen_ie_buf_len = 0;
+        priv->wps.session_enable = MFALSE;
+        wlan_set_wpa_ie_helper(priv, MNULL, 0);
+    } else if (!ie_data_ptr) {
+        /* MNULL check */
+        ret = MLAN_STATUS_FAILURE;
+    } else {
+
+        pvendor_ie = (IEEEtypes_VendorHeader_t *) ie_data_ptr;
+        /* Test to see if it is a WPA IE, if not, then it is a gen IE */
+        if (((pvendor_ie->element_id == WPA_IE)
+             &&
+             (!memcmp
+              (priv->adapter, pvendor_ie->oui, wpa_oui, sizeof(wpa_oui))))
+            || (pvendor_ie->element_id == RSN_IE)
+            ) {
+
+            /* IE is a WPA/WPA2 IE so call set_wpa function */
+            ret = wlan_set_wpa_ie_helper(priv, ie_data_ptr, ie_len);
+            priv->wps.session_enable = MFALSE;
+        } else if (pvendor_ie->element_id == WAPI_IE) {
+            /* IE is a WAPI IE so call set_wapi function */
+            ret = wlan_set_wapi_ie(priv, ie_data_ptr, ie_len);
+        } else
+            if ((pvendor_ie->element_id == WPS_IE) &&
+                (priv->wps.session_enable == MFALSE) &&
+                (!memcmp
+                 (priv->adapter, pvendor_ie->oui, wps_oui, sizeof(wps_oui)))) {
+            /*
+             * Discard first two byte (Element ID and Length)
+             * because they are not needed in the case of setting WPS_IE
+             */
+            if (pvendor_ie->len > 4) {
+                memcpy(priv->adapter, (t_u8 *) & priv->wps.wps_ie, ie_data_ptr,
+                       ie_len);
+                HEXDUMP("wps_ie", (t_u8 *) & priv->wps.wps_ie,
+                        priv->wps.wps_ie.vend_hdr.len + 2);
+            } else {
+                /* Only wps oui exist, reset driver wps buffer */
+                memset(priv->adapter, (t_u8 *) & priv->wps.wps_ie, 0x00,
+                       sizeof(priv->wps.wps_ie));
+                PRINTM(MINFO, "wps_ie cleared\n");
+            }
+        } else {
+            /*
+             * Verify that the passed length is not larger than the available
+             * space remaining in the buffer
+             */
+            if (ie_len < (sizeof(priv->gen_ie_buf) - priv->gen_ie_buf_len)) {
+
+                /* Test to see if it is a WPS IE, if so, enable wps session
+                   flag */
+                pvendor_ie = (IEEEtypes_VendorHeader_t *) ie_data_ptr;
+                if ((pvendor_ie->element_id == WPS_IE)
+                    &&
+                    (!memcmp
+                     (priv->adapter, pvendor_ie->oui, wps_oui,
+                      sizeof(wps_oui)))) {
+                    priv->wps.session_enable = MTRUE;
+                    PRINTM(MINFO, "WPS Session Enabled.\n");
+                }
+
+                /* Append the passed data to the end of the genIeBuffer */
+                memcpy(priv->adapter, priv->gen_ie_buf + priv->gen_ie_buf_len,
+                       ie_data_ptr, ie_len);
+                /* Increment the stored buffer length by the size passed */
+                priv->gen_ie_buf_len += ie_len;
+            } else {
+                /* Passed data does not fit in the remaining buffer space */
+                ret = MLAN_STATUS_FAILURE;
+            }
+        }
+    }
+
+    /* Return MLAN_STATUS_SUCCESS, or MLAN_STATUS_FAILURE for error case */
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get WWS mode
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_misc_ioctl_wws_cfg(IN pmlan_adapter pmadapter,
+                        IN pmlan_ioctl_req pioctl_req)
+{
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_misc_cfg *misc_cfg = MNULL;
+    t_u16 cmd_action = 0;
+    t_u32 enable = 0;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_misc_cfg)) {
+        PRINTM(MWARN, "MLAN IOCTL information buffer length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_misc_cfg);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        ret = MLAN_STATUS_RESOURCE;
+        goto exit;
+    }
+
+    misc_cfg = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET)
+        cmd_action = HostCmd_ACT_GEN_SET;
+    else
+        cmd_action = HostCmd_ACT_GEN_GET;
+
+    enable = misc_cfg->param.wws_cfg;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_SNMP_MIB,
+                           cmd_action,
+                           WwsMode_i, (t_void *) pioctl_req, &enable);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  exit:
+    LEAVE();
+    return ret;
+
+}
+
+/**
+ *  @brief Set/Get 11D status
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_11d_cfg_ioctl_enable(IN pmlan_adapter pmadapter,
+                          IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_11d_cfg *pcfg_11d = MNULL;
+
+    ENTER();
+
+    pcfg_11d = (mlan_ds_11d_cfg *) pioctl_req->pbuf;
+
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        if (pmpriv->media_connected == MTRUE) {
+            PRINTM(MIOCTL,
+                   "11D setting cannot be changed while interface is active.\n");
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+
+        PRINTM(MINFO, "11D: 11dcfg SET=%d\n", pcfg_11d->param.enable_11d);
+
+        /* Compare with current settings */
+        if (pmpriv->state_11d.user_enable_11d != pcfg_11d->param.enable_11d) {
+            ret =
+                wlan_11d_enable(pmpriv, pioctl_req,
+                                (state_11d_t) pcfg_11d->param.enable_11d);
+            if (ret == MLAN_STATUS_SUCCESS)
+                ret = MLAN_STATUS_PENDING;
+        } else {
+            PRINTM(MINFO, "11D: same as current setting, do nothing\n");
+        }
+    } else {
+        pcfg_11d->param.enable_11d = (t_u32) pmpriv->state_11d.user_enable_11d;
+        pioctl_req->data_read_written = sizeof(t_u32) + MLAN_SUB_COMMAND_SIZE;
+        PRINTM(MINFO, "11D: 11dcfg GET=%d\n", pcfg_11d->param.enable_11d);
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Clear 11D chan table
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_11d_clr_chan_table(IN pmlan_adapter pmadapter,
+                        IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        PRINTM(MINFO, "11D: 11dclrtbl SET\n");
+
+        if (wlan_11d_clear_parsedtable(pmpriv) == MLAN_STATUS_SUCCESS)
+            PRINTM(MINFO,
+                   "11D: cleared parsed_region_chan (now no_of_chan=%d)\n",
+                   pmadapter->parsed_region_chan.no_of_chan);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief 11D configuration handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_11d_cfg_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_11d_cfg *pcfg_11d = MNULL;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_11d_cfg)) {
+        PRINTM(MWARN, "MLAN bss IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_11d_cfg);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        status = MLAN_STATUS_RESOURCE;
+        goto exit;
+    }
+
+    pcfg_11d = (mlan_ds_11d_cfg *) pioctl_req->pbuf;
+    switch (pcfg_11d->sub_command) {
+    case MLAN_OID_11D_CFG_ENABLE:
+        status = wlan_11d_cfg_ioctl_enable(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_11D_CLR_CHAN_TABLE:
+        status = wlan_11d_clr_chan_table(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_11D_DOMAIN_INFO:
+        status = wlan_11d_cfg_domain_info(pmadapter, pioctl_req);
+        break;
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+
+  exit:
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief WPS configuration handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_wps_cfg_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_wps_cfg *pwps = MNULL;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_wps_cfg)) {
+        PRINTM(MWARN, "MLAN bss IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_wps_cfg);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+
+    pwps = (mlan_ds_wps_cfg *) pioctl_req->pbuf;
+    switch (pwps->sub_command) {
+    case MLAN_OID_WPS_CFG_SESSION:
+        if (pioctl_req->action == MLAN_ACT_SET) {
+            if (pwps->param.wps_session == MLAN_WPS_CFG_SESSION_START)
+                pmpriv->wps.session_enable = MTRUE;
+        } else {
+            pwps->param.wps_session = (t_u32) pmpriv->wps.session_enable;
+            pioctl_req->data_read_written = sizeof(t_u32);
+            PRINTM(MINFO, "wpscfg GET=%d\n", pwps->param.wps_session);
+        }
+        break;
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Read/write adapter register
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_reg_mem_ioctl_reg_rw(IN pmlan_adapter pmadapter,
+                          IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_reg_mem *reg_mem = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 cmd_action = 0, cmd_no;
+
+    ENTER();
+
+    reg_mem = (mlan_ds_reg_mem *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET)
+        cmd_action = HostCmd_ACT_GEN_GET;
+    else
+        cmd_action = HostCmd_ACT_GEN_SET;
+
+    switch (reg_mem->param.reg_rw.type) {
+    case MLAN_REG_MAC:
+        cmd_no = HostCmd_CMD_MAC_REG_ACCESS;
+        break;
+    case MLAN_REG_BBP:
+        cmd_no = HostCmd_CMD_BBP_REG_ACCESS;
+        break;
+    case MLAN_REG_RF:
+        cmd_no = HostCmd_CMD_RF_REG_ACCESS;
+        break;
+    case MLAN_REG_CAU:
+        cmd_no = HostCmd_CMD_CAU_REG_ACCESS;
+        break;
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        ret = MLAN_STATUS_FAILURE;
+        goto exit;
+    }
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv, cmd_no, cmd_action,
+                           0, (t_void *) pioctl_req,
+                           (t_void *) & reg_mem->param.reg_rw);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  exit:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Read the EEPROM contents of the card
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_reg_mem_ioctl_read_eeprom(IN pmlan_adapter pmadapter,
+                               IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_reg_mem *reg_mem = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 cmd_action = 0;
+
+    ENTER();
+
+    reg_mem = (mlan_ds_reg_mem *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET)
+        cmd_action = HostCmd_ACT_GEN_GET;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_EEPROM_ACCESS,
+                           cmd_action, 0, (t_void *) pioctl_req,
+                           (t_void *) & reg_mem->param.rd_eeprom);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Read/write memory of device
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_reg_mem_ioctl_mem_rw(IN pmlan_adapter pmadapter,
+                          IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_reg_mem *reg_mem = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 cmd_action = 0;
+
+    ENTER();
+
+    reg_mem = (mlan_ds_reg_mem *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET)
+        cmd_action = HostCmd_ACT_GEN_GET;
+    else
+        cmd_action = HostCmd_ACT_GEN_SET;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_MEM_ACCESS,
+                           cmd_action, 0,
+                           (t_void *) pioctl_req,
+                           (t_void *) & reg_mem->param.mem_rw);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief register memory access handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_reg_mem_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_reg_mem *reg_mem = MNULL;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_reg_mem)) {
+        PRINTM(MWARN, "MLAN REG_MEM IOCTL length is too short\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_reg_mem);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+    reg_mem = (mlan_ds_reg_mem *) pioctl_req->pbuf;
+    switch (reg_mem->sub_command) {
+    case MLAN_OID_REG_RW:
+        status = wlan_reg_mem_ioctl_reg_rw(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_EEPROM_RD:
+        status = wlan_reg_mem_ioctl_read_eeprom(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_MEM_RW:
+        status = wlan_reg_mem_ioctl_mem_rw(pmadapter, pioctl_req);
+        break;
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief 802.11h ad-hoc start channel check
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_11h_channel_check_req(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    pmlan_private pmpriv = MNULL;
+    mlan_status ret = MLAN_STATUS_FAILURE;
+
+    ENTER();
+
+    if (pioctl_req != MNULL) {
+        pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    } else {
+        PRINTM(MERROR, "MLAN IOCTL information is not present\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    pmpriv->adhoc_state = ADHOC_STARTING;
+
+    if ((pmadapter->adhoc_start_band & BAND_A)
+        || (pmadapter->adhoc_start_band & BAND_AN)
+        ) {
+        if (pmpriv->intf_state_11h.adhoc_auto_sel_chan)
+            pmpriv->adhoc_channel = wlan_11h_get_adhoc_start_channel(pmpriv);
+
+        /*
+         * Check if the region and channel requires a channel availability
+         * check.
+         */
+        if (wlan_11h_radar_detect_required(pmpriv, pmpriv->adhoc_channel)
+            && !wlan_11h_is_channel_under_nop(pmadapter, pmpriv->adhoc_channel)
+            ) {
+            /*
+             * Radar detection is required for this channel, make sure
+             * 11h is activated in the firmware
+             */
+            ret = wlan_11h_activate(pmpriv, MNULL, MTRUE);
+            ret = wlan_11h_config_master_radar_det(pmpriv, MTRUE);
+            ret = wlan_11h_check_update_radar_det_state(pmpriv);
+
+            /* Check for radar on the channel */
+            ret = wlan_11h_issue_radar_detect(pmpriv, pioctl_req,
+                                              pmpriv->adhoc_channel);
+            if (ret == MLAN_STATUS_SUCCESS)
+                ret = MLAN_STATUS_PENDING;
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief 802.11h set/get local power constraint
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_11h_ioctl_local_power_constraint(IN pmlan_adapter pmadapter,
+                                      IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_ds_11h_cfg *ds_11hcfg = MNULL;
+    t_s8 *plocalpower = &pmadapter->state_11h.usr_def_power_constraint;
+
+    ENTER();
+
+    ds_11hcfg = (mlan_ds_11h_cfg *) pioctl_req->pbuf;
+
+    if (pioctl_req->action == MLAN_ACT_GET)
+        ds_11hcfg->param.usr_local_power_constraint = *plocalpower;
+    else
+        *plocalpower = ds_11hcfg->param.usr_local_power_constraint;
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief 11h configuration handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_11h_cfg_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_11h_cfg *ds_11hcfg = MNULL;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_11h_cfg)) {
+        PRINTM(MWARN, "MLAN 11H IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_11h_cfg);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+    ds_11hcfg = (mlan_ds_11h_cfg *) pioctl_req->pbuf;
+
+    switch (ds_11hcfg->sub_command) {
+    case MLAN_OID_11H_CHANNEL_CHECK:
+        status = wlan_11h_channel_check_req(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_11H_LOCAL_POWER_CONSTRAINT:
+        status = wlan_11h_ioctl_local_power_constraint(pmadapter, pioctl_req);
+        break;
+#if defined(DFS_TESTING_SUPPORT)
+    case MLAN_OID_11H_DFS_TESTING:
+        status = wlan_11h_ioctl_dfs_testing(pmadapter, pioctl_req);
+        break;
+#endif
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Set/Get generic IE
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_misc_ioctl_gen_ie(IN pmlan_adapter pmadapter,
+                       IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_misc_cfg *misc = MNULL;
+
+    ENTER();
+
+    misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    switch (misc->param.gen_ie.type) {
+    case MLAN_IE_TYPE_GEN_IE:
+        if (pioctl_req->action == MLAN_ACT_GET) {
+            misc->param.gen_ie.len = pmpriv->wpa_ie_len;
+            memcpy(pmadapter, misc->param.gen_ie.ie_data, pmpriv->wpa_ie,
+                   misc->param.gen_ie.len);
+        } else {
+            wlan_set_gen_ie_helper(pmpriv, misc->param.gen_ie.ie_data,
+                                   (t_u16) misc->param.gen_ie.len);
+        }
+        break;
+    case MLAN_IE_TYPE_ARP_FILTER:
+        memset(pmadapter, pmadapter->arp_filter, 0,
+               sizeof(pmadapter->arp_filter));
+        if (misc->param.gen_ie.len > ARP_FILTER_MAX_BUF_SIZE) {
+            pmadapter->arp_filter_size = 0;
+            PRINTM(MERROR, "Invalid ARP Filter Size\n");
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+        } else if (misc->param.gen_ie.len <= sizeof(MrvlIEtypesHeader_t)) {
+            pmadapter->arp_filter_size = 0;
+            PRINTM(MINFO, "Clear ARP filter\n");
+        } else {
+            memcpy(pmadapter, pmadapter->arp_filter, misc->param.gen_ie.ie_data,
+                   misc->param.gen_ie.len);
+            pmadapter->arp_filter_size = misc->param.gen_ie.len;
+            HEXDUMP("ArpFilter", pmadapter->arp_filter,
+                    pmadapter->arp_filter_size);
+        }
+        break;
+    default:
+        PRINTM(MERROR, "Invalid IE type\n");
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        ret = MLAN_STATUS_FAILURE;
+    }
+    pioctl_req->data_read_written =
+        sizeof(mlan_ds_misc_gen_ie) + MLAN_SUB_COMMAND_SIZE;
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get region code
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_misc_ioctl_region(IN pmlan_adapter pmadapter,
+                       IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_misc_cfg *misc = MNULL;
+    int i;
+
+    ENTER();
+
+    misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        misc->param.region_code = pmadapter->region_code;
+    } else {
+        for (i = 0; i < MRVDRV_MAX_REGION_CODE; i++) {
+            /* Use the region code to search for the index */
+            if (misc->param.region_code == region_code_index[i]) {
+                pmadapter->region_code = (t_u16) misc->param.region_code;
+                break;
+            }
+        }
+        /* It's unidentified region code */
+        if (i >= MRVDRV_MAX_REGION_CODE) {
+            PRINTM(MERROR, "Region Code not identified\n");
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+        pmadapter->cfp_code_bg = misc->param.region_code;
+        pmadapter->cfp_code_a = misc->param.region_code;
+        if (wlan_set_regiontable(pmpriv, (t_u8) pmadapter->region_code,
+                                 pmadapter->config_bands | pmadapter->
+                                 adhoc_start_band)) {
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_FAIL;
+            ret = MLAN_STATUS_FAILURE;
+        }
+    }
+    pioctl_req->data_read_written = sizeof(t_u32) + MLAN_SUB_COMMAND_SIZE;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Perform warm reset
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING
+ */
+static mlan_status
+wlan_misc_ioctl_warm_reset(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    t_s32 i = 0;
+
+    ENTER();
+
+    /** Init all the head nodes and free all the locks here */
+    for (i = 0; i < pmadapter->priv_num; i++) {
+        wlan_free_priv(pmadapter->priv[i]);
+    }
+
+    /* Initialize adapter structure */
+    wlan_init_adapter(pmadapter);
+
+    /* Initialize private structures */
+    for (i = 0; i < pmadapter->priv_num; i++) {
+        if (pmadapter->priv[i]) {
+            wlan_init_priv(pmadapter->priv[i]);
+        }
+    }
+
+    /* Restart the firmware */
+    wlan_prepare_cmd(pmpriv, HostCmd_CMD_FUNC_SHUTDOWN,
+                     HostCmd_ACT_GEN_SET, 0, MNULL, MNULL);
+
+    /* Issue firmware initialize commands for first BSS, for other interfaces
+       it will be called after getting the last init command response of
+       previous interface */
+    pmpriv = wlan_get_priv(pmadapter, MLAN_BSS_ROLE_ANY);
+    pmadapter->pwarm_reset_ioctl_req = pioctl_req;
+    pmpriv->ops.init_cmd(pmpriv, MTRUE);
+
+    LEAVE();
+    return MLAN_STATUS_PENDING;
+}
+
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+/**
+ *  @brief Reconfigure SDIO multiport aggregation parameters
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static mlan_status
+wlan_misc_ioctl_sdio_mpa_ctrl(IN pmlan_adapter pmadapter,
+                              IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_misc_cfg *misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    mlan_ds_misc_sdio_mpa_ctrl *mpa_ctrl = MNULL;
+
+    ENTER();
+
+    mpa_ctrl = &misc->param.mpa_ctrl;
+
+    if (pioctl_req->action == MLAN_ACT_SET) {
+
+        if (pmpriv->media_connected == MTRUE) {
+            PRINTM(MMSG, "SDIO MPA CTRL: not allowed in connected state\n");
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+            ret = MLAN_STATUS_FAILURE;
+            goto exit;
+        }
+
+        if (mpa_ctrl->tx_enable > 1) {
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+            goto exit;
+        }
+
+        if (mpa_ctrl->rx_enable > 1) {
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+            goto exit;
+        }
+
+        if (mpa_ctrl->tx_max_ports > SDIO_MP_AGGR_DEF_PKT_LIMIT) {
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+            goto exit;
+        }
+
+        if (mpa_ctrl->rx_max_ports > SDIO_MP_AGGR_DEF_PKT_LIMIT) {
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+            goto exit;
+        }
+
+        if (mpa_ctrl->tx_buf_size || mpa_ctrl->rx_buf_size) {
+
+            wlan_free_sdio_mpa_buffers(pmadapter);
+
+            if (mpa_ctrl->tx_buf_size > 0)
+                pmadapter->mpa_tx.buf_size = mpa_ctrl->tx_buf_size;
+
+            if (mpa_ctrl->rx_buf_size > 0)
+                pmadapter->mpa_rx.buf_size = mpa_ctrl->rx_buf_size;
+
+            if (wlan_alloc_sdio_mpa_buffers(pmadapter,
+                                            pmadapter->mpa_tx.buf_size,
+                                            pmadapter->mpa_rx.buf_size) !=
+                MLAN_STATUS_SUCCESS) {
+                PRINTM(MERROR, "Failed to allocate sdio mp-a buffers\n");
+                pioctl_req->status_code = MLAN_ERROR_NO_MEM;
+                ret = MLAN_STATUS_FAILURE;
+                goto exit;
+            }
+        }
+
+        if (mpa_ctrl->tx_max_ports > 0)
+            pmadapter->mpa_tx.pkt_aggr_limit = mpa_ctrl->tx_max_ports;
+        if (mpa_ctrl->rx_max_ports > 0)
+            pmadapter->mpa_rx.pkt_aggr_limit = mpa_ctrl->rx_max_ports;
+
+        pmadapter->mpa_tx.enabled = (t_u8) mpa_ctrl->tx_enable;
+        pmadapter->mpa_rx.enabled = (t_u8) mpa_ctrl->rx_enable;
+
+    } else {
+        mpa_ctrl->tx_enable = (t_u16) pmadapter->mpa_tx.enabled;
+        mpa_ctrl->rx_enable = (t_u16) pmadapter->mpa_rx.enabled;
+        mpa_ctrl->tx_buf_size = (t_u16) pmadapter->mpa_tx.buf_size;
+        mpa_ctrl->rx_buf_size = (t_u16) pmadapter->mpa_rx.buf_size;
+        mpa_ctrl->tx_max_ports = (t_u16) pmadapter->mpa_tx.pkt_aggr_limit;
+        mpa_ctrl->rx_max_ports = (t_u16) pmadapter->mpa_rx.pkt_aggr_limit;
+    }
+
+  exit:
+    LEAVE();
+    return ret;
+}
+#endif /* SDIO_MULTI_PORT_TX_AGGR || SDIO_MULTI_PORT_RX_AGGR */
+
+/**
+ *  @brief Set/Get system clock configuration
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_misc_ioctl_sysclock(IN pmlan_adapter pmadapter,
+                         IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_misc_cfg *misc = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 cmd_action = 0;
+
+    ENTER();
+
+    misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET)
+        cmd_action = HostCmd_ACT_GEN_GET;
+    else
+        cmd_action = HostCmd_ACT_GEN_SET;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_ECL_SYSTEM_CLOCK_CONFIG,
+                           cmd_action,
+                           0,
+                           (t_void *) pioctl_req,
+                           (t_void *) & misc->param.sys_clock);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Send function softreset command to firmware
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_misc_ioctl_soft_reset(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    /* Send command to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_SOFT_RESET,
+                           HostCmd_ACT_GEN_SET,
+                           0, (t_void *) pioctl_req, MNULL);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get the MAC control configuration.
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+wlan_misc_ioctl_mac_control(IN pmlan_adapter pmadapter,
+                            IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_misc_cfg *misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 cmd_action = 0;
+
+    ENTER();
+
+    misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        misc->param.mac_ctrl = pmpriv->curr_pkt_filter;
+    } else {
+        pmpriv->curr_pkt_filter = misc->param.mac_ctrl;
+        cmd_action = HostCmd_ACT_GEN_SET;
+
+        /* Send command to firmware */
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_MAC_CONTROL,
+                               cmd_action, 0,
+                               (t_void *) pioctl_req, &misc->param.mac_ctrl);
+
+        if (ret == MLAN_STATUS_SUCCESS)
+            ret = MLAN_STATUS_PENDING;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get the thermal reading
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+wlan_misc_ioctl_thermal(IN pmlan_adapter pmadapter,
+                        IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_misc_cfg *misc = MNULL;
+    t_u16 cmd_action = 0;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_misc_cfg)) {
+        PRINTM(MWARN, "MLAN IOCTL information buffer length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_misc_cfg);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+
+    misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        PRINTM(MERROR, "Thermal reading setting is not allowed!\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    } else {
+        cmd_action = HostCmd_ACT_GEN_GET;
+    }
+
+    /* Send command to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_SNMP_MIB,
+                           cmd_action, Thermal_i, (t_void *) pioctl_req, MNULL);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get/Set subscribe event
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+wlan_misc_ioctl_subscribe_evt(IN pmlan_adapter pmadapter,
+                              IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_misc_cfg *misc = MNULL;
+    t_u16 cmd_action = 0;
+
+    ENTER();
+
+    misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        cmd_action = HostCmd_ACT_GEN_SET;
+    } else {
+        cmd_action = HostCmd_ACT_GEN_GET;
+    }
+
+    /* Send command to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_SUBSCRIBE_EVENT,
+                           cmd_action,
+                           0,
+                           (t_void *) pioctl_req, &misc->param.subscribe_event);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set ARP filter based on IP address
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *  @param ipv4_addr    ipv4 Address
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_ipaddr_arp_filter(IN pmlan_adapter pmadapter,
+                       IN pmlan_ioctl_req pioctl_req, IN t_u32 ipv4_addr)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    t_u8 *buf;
+    arpfilter_header *arpfilter = MNULL;
+    filter_entry *entry = MNULL;
+    t_u32 len;
+
+    ENTER();
+
+    pcb->moal_malloc(pmadapter->pmoal_handle, MRVDRV_SIZE_OF_CMD_BUFFER,
+                     MLAN_MEM_DEF, &buf);
+    if (!buf) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Construct the ARP filter TLV */
+    arpfilter = (arpfilter_header *) buf;
+    arpfilter->type = wlan_cpu_to_le16(TLV_TYPE_ARP_FILTER);
+
+    if (ipv4_addr) {
+        arpfilter->len = wlan_cpu_to_le16(sizeof(filter_entry) * 3);
+        entry = (filter_entry *) (buf + sizeof(arpfilter_header));
+        entry->addr_type = wlan_cpu_to_le16(ADDR_TYPE_BROADCAST);
+        entry->eth_type = wlan_cpu_to_le16(ETHER_TYPE_ARP);
+        entry->ipv4_addr = wlan_cpu_to_le32(ipv4_addr);
+        entry++;
+        entry->addr_type = wlan_cpu_to_le16(ADDR_TYPE_UNICAST);
+        entry->eth_type = wlan_cpu_to_le16(ETHER_TYPE_ANY);
+        entry->ipv4_addr = wlan_cpu_to_le32(IPV4_ADDR_ANY);
+        entry++;
+        entry->addr_type = wlan_cpu_to_le16(ADDR_TYPE_MULTICAST);
+        entry->eth_type = wlan_cpu_to_le16(ETHER_TYPE_ANY);
+        entry->ipv4_addr = wlan_cpu_to_le32(IPV4_ADDR_ANY);
+    } else
+        arpfilter->len = 0;
+
+    /* Update the total length */
+    len = sizeof(arpfilter_header) + wlan_le16_to_cpu(arpfilter->len);
+
+    memset(pmadapter, pmadapter->arp_filter, 0, sizeof(pmadapter->arp_filter));
+    if (len > ARP_FILTER_MAX_BUF_SIZE) {
+        pmadapter->arp_filter_size = 0;
+        PRINTM(MERROR, "Invalid ARP Filter Size\n");
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        ret = MLAN_STATUS_FAILURE;
+    } else if (len <= sizeof(MrvlIEtypesHeader_t)) {
+        pmadapter->arp_filter_size = 0;
+        PRINTM(MINFO, "Clear ARP filter\n");
+    } else {
+        memcpy(pmadapter, pmadapter->arp_filter, buf, len);
+        pmadapter->arp_filter_size = len;
+        HEXDUMP("ArpFilter", pmadapter->arp_filter, pmadapter->arp_filter_size);
+    }
+
+  done:
+    if (buf)
+        pcb->moal_mfree(pmadapter->pmoal_handle, buf);
+    LEAVE();
+    return ret;
+}
+
+#define FLTR_BUF_IP_OFFSET      39
+/**
+ *  @brief Enable/Disable Auto ARP resonse
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *  @param ipv4_addr    ipv4 Address
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_ipaddr_auto_arp_resp(IN pmlan_adapter pmadapter,
+                          IN pmlan_ioctl_req pioctl_req, IN t_u32 ipv4_addr)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    HostCmd_DS_GEN *hostcmd_hdr;
+    HostCmd_DS_MEF_CFG *mefcmd;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    mlan_ds_misc_cmd *hostcmd;
+    t_u32 buf_len = 0;
+    t_u8 *buf, *filter;
+    t_u8 fltr_buf[] = { 0x01, 0x10, 0x30, 0x00, 0x06, 0x00, 0x00, 0x00,
+        0x01, 0xff, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00,
+        0x01, 0x41, 0x01, 0x00, 0x00, 0x00, 0x01, 0x08,
+        0x06, 0x02, 0x02, 0x14, 0x00, 0x00, 0x00, 0x01,
+        0x41, 0x44, 0x01, 0x00, 0x00, 0x00, 0x01, 0xc0,
+        0xa8, 0x00, 0x68, 0x04, 0x02, 0x2e, 0x00, 0x00,
+        0x00, 0x01, 0x41, 0x44
+    };
+
+    ENTER();
+
+    ret = pcb->moal_malloc(pmadapter->pmoal_handle,
+                           sizeof(mlan_ds_misc_cmd),
+                           MLAN_MEM_DEF, (t_u8 **) & hostcmd);
+
+    if (ret != MLAN_STATUS_SUCCESS || hostcmd == MNULL) {
+        PRINTM(MERROR, "Failed to allocate hostcmd  buffer\n");
+        pioctl_req->status_code = MLAN_ERROR_NO_MEM;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    memset(pmpriv->adapter, hostcmd, 0, sizeof(mlan_ds_misc_cmd));
+    buf = hostcmd->cmd;
+
+    /* Prepare hostcmd buffer */
+    hostcmd_hdr = (HostCmd_DS_GEN *) (buf);
+    hostcmd_hdr->command = wlan_cpu_to_le16(HostCmd_CMD_MEF_CFG);
+    mefcmd = (HostCmd_DS_MEF_CFG *) (buf + S_DS_GEN);
+    buf_len = S_DS_GEN;
+
+    if (!ipv4_addr) {
+        PRINTM(MINFO, "Disable Auto ARP Response\n");
+        mefcmd->criteria = wlan_cpu_to_le32(0);
+        mefcmd->nentries = wlan_cpu_to_le16(0);
+        buf_len += sizeof(HostCmd_DS_MEF_CFG);
+    } else {
+        /* Event bit (bit2) of HS conditions should be masked out */
+        mefcmd->criteria =
+            wlan_cpu_to_le32(pmpriv->adapter->hs_cfg.conditions & ~MBIT(2));
+        mefcmd->nentries = wlan_cpu_to_le16(1);
+        buf_len += sizeof(HostCmd_DS_MEF_CFG);
+        filter = buf + buf_len;
+        memcpy(pmpriv->adapter, filter, fltr_buf, sizeof(fltr_buf));
+        memcpy(pmpriv->adapter, &filter[FLTR_BUF_IP_OFFSET], &ipv4_addr,
+               sizeof(ipv4_addr));
+        buf_len += sizeof(fltr_buf);
+    }
+    hostcmd_hdr->size = wlan_cpu_to_le16(buf_len);
+    hostcmd->len = buf_len;
+
+    /* Send command to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           0, 0, 0, (t_void *) pioctl_req, (t_void *) hostcmd);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    pcb->moal_mfree(pmadapter->pmoal_handle, (t_u8 *) hostcmd);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief MEF configuration
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_misc_ioctl_mef_cfg(IN pmlan_adapter pmadapter,
+                        IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_misc_mef_cfg *mef_cfg =
+        &((mlan_ds_misc_cfg *) pioctl_req->pbuf)->param.mef_cfg;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    HostCmd_DS_GEN *hostcmd_hdr;
+    HostCmd_DS_MEF_CFG *mefcmd;
+    mlan_ds_misc_cmd *hostcmd = MNULL;
+    t_u32 buf_len = 0;
+    t_u8 *buf, *filter;
+    t_u8 fltr_buf[] = { 0x02, 0x00, 0x2f, 0x00, 0x01, 0x00, 0x00, 0x00,
+        0x01, 0x01, 0x00, 0x5e, 0x03, 0x02, 0x00, 0x00,
+        0x00, 0x00, 0x01, 0x41, 0x06, 0x00, 0x00, 0x00,
+        0x01, 0xff, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00,
+        0x01, 0x41, 0x45, 0x01, 0x00, 0x00, 0x00, 0x01,
+        0x33, 0x33, 0x02, 0x02, 0x00, 0x00, 0x00, 0x00,
+        0x01, 0x41, 0x45
+    };
+
+    ENTER();
+
+    /* GET operation */
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        /* TODO: need to store for get operation */
+        goto done;
+    }
+
+    ret = pcb->moal_malloc(pmadapter->pmoal_handle,
+                           sizeof(mlan_ds_misc_cmd),
+                           MLAN_MEM_DEF, (t_u8 **) & hostcmd);
+
+    if (ret != MLAN_STATUS_SUCCESS || hostcmd == MNULL) {
+        PRINTM(MERROR, "Failed to allocate hostcmd  buffer\n");
+        pioctl_req->status_code = MLAN_ERROR_NO_MEM;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    memset(pmpriv->adapter, hostcmd, 0, sizeof(mlan_ds_misc_cmd));
+    buf = hostcmd->cmd;
+
+    /* Prepare hostcmd buffer */
+    hostcmd_hdr = (HostCmd_DS_GEN *) (buf);
+    hostcmd_hdr->command = wlan_cpu_to_le16(HostCmd_CMD_MEF_CFG);
+    mefcmd = (HostCmd_DS_MEF_CFG *) (buf + S_DS_GEN);
+    buf_len = S_DS_GEN;
+
+    switch (mef_cfg->sub_id) {
+    case MEF_CFG_DISABLE:
+        PRINTM(MINFO, "Disable MEF\n");
+        mefcmd->criteria = wlan_cpu_to_le32(0);
+        mefcmd->nentries = wlan_cpu_to_le16(0);
+        buf_len += sizeof(HostCmd_DS_MEF_CFG);
+        break;
+    case MEF_CFG_RX_FILTER_ENABLE:
+        PRINTM(MINFO, "Enable Rx filter\n");
+        mefcmd->criteria = wlan_cpu_to_le32((MBIT(3) | MBIT(0)));
+        mefcmd->nentries = wlan_cpu_to_le16(1);
+        buf_len += sizeof(HostCmd_DS_MEF_CFG);
+        filter = buf + buf_len;
+        memcpy(pmpriv->adapter, filter, fltr_buf, sizeof(fltr_buf));
+        buf_len += sizeof(fltr_buf);
+        break;
+    case MEF_CFG_AUTO_ARP_RESP:
+        PRINTM(MINFO, "Enable auto ARP response\n");
+        // TODO
+        break;
+    case MEF_CFG_HOSTCMD:
+        PRINTM(MINFO, "MEF hostcmd from MOAL\n");
+        filter = buf + buf_len;
+        memcpy(pmpriv->adapter, filter, mef_cfg->param.cmd_buf.cmd,
+               mef_cfg->param.cmd_buf.len);
+        buf_len += mef_cfg->param.cmd_buf.len;
+        break;
+    default:
+        PRINTM(MERROR, "Invalid sub ID parameter\n");
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+        break;
+    }
+    hostcmd_hdr->size = wlan_cpu_to_le16(buf_len);
+    hostcmd->len = buf_len;
+
+    /* Send command to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           0, 0, 0, (t_void *) pioctl_req, (t_void *) hostcmd);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  done:
+    if (hostcmd)
+        pcb->moal_mfree(pmadapter->pmoal_handle, (t_u8 *) hostcmd);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief ipaddr configuration
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_misc_ioctl_ipaddr_cfg(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_misc_cfg *misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u32 ipv4_addr;
+
+    ENTER();
+
+    /* GET operation */
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        memcpy(pmadapter, misc->param.ipaddr_cfg.ip_addr,
+               pmpriv->ip_addr, IPADDR_LEN);
+        misc->param.ipaddr_cfg.op_code = pmpriv->op_code;
+        goto done;
+    }
+    /* only one IP is supported in current firmware */
+    memcpy(pmadapter, &ipv4_addr, misc->param.ipaddr_cfg.ip_addr[0],
+           sizeof(t_u32));
+
+    if (misc->param.ipaddr_cfg.op_code != MLAN_IPADDR_OP_IP_REMOVE &&
+        !ipv4_addr) {
+        PRINTM(MERROR, "Invalid IPv4 address\n");
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    if (misc->param.ipaddr_cfg.op_code & MLAN_IPADDR_OP_ARP_FILTER)
+        ret = wlan_ipaddr_arp_filter(pmadapter, pioctl_req, ipv4_addr);
+    else if (pmpriv->op_code & MLAN_IPADDR_OP_ARP_FILTER)
+        ret = wlan_ipaddr_arp_filter(pmadapter, pioctl_req, 0);
+    if (ret == MLAN_STATUS_FAILURE)
+        goto done;
+    if (misc->param.ipaddr_cfg.op_code & MLAN_IPADDR_OP_AUTO_ARP_RESP)
+        ret = wlan_ipaddr_auto_arp_resp(pmadapter, pioctl_req, ipv4_addr);
+    else if (pmpriv->op_code & MLAN_IPADDR_OP_AUTO_ARP_RESP)
+        ret = wlan_ipaddr_auto_arp_resp(pmadapter, pioctl_req, 0);
+    if (ret == MLAN_STATUS_FAILURE)
+        goto done;
+
+    /* Save the values in MLAN */
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        pmpriv->op_code = misc->param.ipaddr_cfg.op_code;
+        memcpy(pmadapter, pmpriv->ip_addr,
+               misc->param.ipaddr_cfg.ip_addr, IPADDR_LEN);
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief CFP code configuration
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_misc_ioctl_cfp_code_cfg(IN pmlan_adapter pmadapter,
+                             IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_misc_cfg *misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    mlan_ds_misc_cfp_code *cfp_code = MNULL;
+    t_u32 region_bg = 0;
+    t_u32 region_a = 0;
+    int i;
+
+    ENTER();
+
+    cfp_code = &misc->param.cfp_code;
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        /* Save the values in MLAN */
+        if (!cfp_code->cfp_code_bg)
+            cfp_code->cfp_code_bg = pmadapter->cfp_code_bg;
+        for (i = 0; i < MRVDRV_MAX_REGION_CODE; i++) {
+            /* Use the region code to search for the index */
+            if (cfp_code->cfp_code_bg == region_code_index[i]) {
+                region_bg = cfp_code->cfp_code_bg;
+                break;
+            }
+        }
+        if (!region_bg) {
+            for (i = 0; i < MRVDRV_MAX_CFP_CODE_BG; i++) {
+                /* Use the CFP code to search for the index */
+                if (cfp_code->cfp_code_bg == cfp_code_index_bg[i])
+                    break;
+            }
+            if (i >= MRVDRV_MAX_CFP_CODE_BG) {
+                PRINTM(MERROR, "CFP Code not identified for BG\n");
+                pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+                ret = MLAN_STATUS_FAILURE;
+                goto done;
+            }
+        }
+        if (!cfp_code->cfp_code_a)
+            cfp_code->cfp_code_a = pmadapter->cfp_code_a;
+        for (i = 0; i < MRVDRV_MAX_REGION_CODE; i++) {
+            /* Use the region code to search for the index */
+            if (cfp_code->cfp_code_a == region_code_index[i]) {
+                region_a = cfp_code->cfp_code_a;
+                break;
+            }
+        }
+        if (!region_a) {
+            for (i = 0; i < MRVDRV_MAX_CFP_CODE_A; i++) {
+                /* Use the CFP code to search for the index */
+                if (cfp_code->cfp_code_a == cfp_code_index_a[i])
+                    break;
+            }
+            if (i >= MRVDRV_MAX_CFP_CODE_A) {
+                PRINTM(MERROR, "CFP Code not identified for A\n");
+                pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+                ret = MLAN_STATUS_FAILURE;
+                goto done;
+            }
+        }
+        pmadapter->cfp_code_bg = (t_u8) cfp_code->cfp_code_bg;
+        pmadapter->cfp_code_a = (t_u8) cfp_code->cfp_code_a;
+        if (region_bg && region_a && (region_bg == region_a))
+            pmadapter->region_code = pmadapter->cfp_code_a;
+        else
+            pmadapter->region_code = 0;
+        if (wlan_set_regiontable(pmpriv, (t_u8) pmadapter->region_code,
+                                 pmadapter->config_bands | pmadapter->
+                                 adhoc_start_band)) {
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+    } else {
+        /* GET operation */
+        cfp_code->cfp_code_bg = pmadapter->cfp_code_bg;
+        cfp_code->cfp_code_a = pmadapter->cfp_code_a;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function sets up country code and downloads CMD to FW
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   Pointer to the IOCTL request buffer
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_misc_ioctl_country_code(IN pmlan_adapter pmadapter,
+                             IN mlan_ioctl_req * pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_misc_country_code *country_code = MNULL;
+    mlan_ds_misc_cfg *cfg_misc = MNULL;
+    t_u8 cfp_bg = 0, cfp_a = 0;
+
+    ENTER();
+
+    cfg_misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    country_code = &cfg_misc->param.country_code;
+
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        /* Update region code and table based on country code */
+        if (wlan_misc_country_2_cfp_table_code(pmadapter,
+                                               country_code->country_code,
+                                               &cfp_bg, &cfp_a)) {
+            PRINTM(MERROR, "Country code not found!\n");
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+        pmadapter->cfp_code_bg = cfp_bg;
+        pmadapter->cfp_code_a = cfp_a;
+        if (cfp_bg && cfp_a && (cfp_bg == cfp_a))
+            pmadapter->region_code = cfp_a;
+        else
+            pmadapter->region_code = 0;
+        if (wlan_set_regiontable(pmpriv, pmadapter->region_code,
+                                 pmadapter->config_bands | pmadapter->
+                                 adhoc_start_band)) {
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+        memcpy(pmadapter, pmadapter->country_code,
+               country_code->country_code, COUNTRY_CODE_LEN);
+    } else {
+        /* GET operation */
+        memcpy(pmadapter, country_code->country_code,
+               pmadapter->country_code, COUNTRY_CODE_LEN);
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Miscellaneous configuration handler
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_misc_cfg_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_misc_cfg *misc = MNULL;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_misc_cfg)) {
+        PRINTM(MWARN, "MLAN bss IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_misc_cfg);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+    if ((pioctl_req == MNULL) || (pioctl_req->pbuf == MNULL)) {
+        PRINTM(MERROR, "Request buffer not found!\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    switch (misc->sub_command) {
+    case MLAN_OID_MISC_GEN_IE:
+        status = wlan_misc_ioctl_gen_ie(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_MISC_REGION:
+        status = wlan_misc_ioctl_region(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_MISC_WARM_RESET:
+        status = wlan_misc_ioctl_warm_reset(pmadapter, pioctl_req);
+        break;
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+    case MLAN_OID_MISC_SDIO_MPA_CTRL:
+        status = wlan_misc_ioctl_sdio_mpa_ctrl(pmadapter, pioctl_req);
+        break;
+#endif
+    case MLAN_OID_MISC_HOST_CMD:
+        status = wlan_misc_ioctl_host_cmd(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_MISC_SYS_CLOCK:
+        status = wlan_misc_ioctl_sysclock(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_MISC_WWS:
+        status = wlan_misc_ioctl_wws_cfg(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_MISC_INIT_SHUTDOWN:
+        status = wlan_misc_ioctl_init_shutdown(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_MISC_SOFT_RESET:
+        status = wlan_misc_ioctl_soft_reset(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_MISC_CUSTOM_IE:
+        status = wlan_misc_ioctl_custom_ie_list(pmadapter, pioctl_req, MTRUE);
+        break;
+    case MLAN_OID_MISC_MAC_CONTROL:
+        status = wlan_misc_ioctl_mac_control(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_MISC_MEF_CFG:
+        status = wlan_misc_ioctl_mef_cfg(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_MISC_RX_MGMT_IND:
+        status = wlan_reg_rx_mgmt_ind(pmadapter, pioctl_req);
+        break;
+#ifdef DEBUG_LEVEL1
+    case MLAN_OID_MISC_DRVDBG:
+        status = wlan_set_drvdbg(pmadapter, pioctl_req);
+        break;
+#endif
+    case MLAN_OID_MISC_IP_ADDR:
+        status = wlan_misc_ioctl_ipaddr_cfg(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_MISC_CFP_CODE:
+        status = wlan_misc_ioctl_cfp_code_cfg(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_MISC_COUNTRY_CODE:
+        status = wlan_misc_ioctl_country_code(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_MISC_THERMAL:
+        status = wlan_misc_ioctl_thermal(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_MISC_SUBSCRIBE_EVENT:
+        status = wlan_misc_ioctl_subscribe_evt(pmadapter, pioctl_req);
+        break;
+    case MLAN_OID_MISC_OTP_USER_DATA:
+        status = wlan_misc_otp_user_data(pmadapter, pioctl_req);
+        break;
+    default:
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Set/Get scan configuration parameter
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *  @param action	    Set/Get
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_set_get_scan_cfg(IN pmlan_adapter pmadapter,
+                      IN pmlan_ioctl_req pioctl_req, IN t_u32 action)
+{
+    mlan_ds_scan *scan = MNULL;
+
+    ENTER();
+
+    scan = (mlan_ds_scan *) pioctl_req->pbuf;
+    if (action == MLAN_ACT_GET) {
+        scan->param.scan_cfg.scan_type = (t_u32) pmadapter->scan_type;
+        scan->param.scan_cfg.scan_mode = pmadapter->scan_mode;
+        scan->param.scan_cfg.scan_probe = (t_u32) pmadapter->scan_probes;
+        scan->param.scan_cfg.scan_time.specific_scan_time =
+            (t_u32) pmadapter->specific_scan_time;
+        scan->param.scan_cfg.scan_time.active_scan_time =
+            (t_u32) pmadapter->active_scan_time;
+        scan->param.scan_cfg.scan_time.passive_scan_time =
+            (t_u32) pmadapter->passive_scan_time;
+        scan->param.scan_cfg.ext_scan = pmadapter->ext_scan;
+    } else {
+        if (scan->param.scan_cfg.scan_type)
+            pmadapter->scan_type = (t_u8) scan->param.scan_cfg.scan_type;
+        if (scan->param.scan_cfg.scan_mode)
+            pmadapter->scan_mode = scan->param.scan_cfg.scan_mode;
+        if (scan->param.scan_cfg.scan_probe)
+            pmadapter->scan_probes = (t_u16) scan->param.scan_cfg.scan_probe;
+        if (scan->param.scan_cfg.scan_time.specific_scan_time)
+            pmadapter->specific_scan_time =
+                (t_u16) scan->param.scan_cfg.scan_time.specific_scan_time;
+        if (scan->param.scan_cfg.scan_time.active_scan_time)
+            pmadapter->active_scan_time =
+                (t_u16) scan->param.scan_cfg.scan_time.active_scan_time;
+        if (scan->param.scan_cfg.scan_time.passive_scan_time)
+            pmadapter->passive_scan_time =
+                (t_u16) scan->param.scan_cfg.scan_time.passive_scan_time;
+        pmadapter->ext_scan = scan->param.scan_cfg.ext_scan;
+    }
+    pioctl_req->data_read_written = sizeof(t_u32) + MLAN_SUB_COMMAND_SIZE;
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Set/Get scan
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_scan_ioctl(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_scan *pscan;
+
+    ENTER();
+
+    pscan = (mlan_ds_scan *) pioctl_req->pbuf;
+    if (pscan->sub_command == MLAN_OID_SCAN_CONFIG)
+        goto start_config;
+    if (pmadapter->scan_processing && pioctl_req->action == MLAN_ACT_SET &&
+        pscan->sub_command != MLAN_OID_SCAN_CANCEL) {
+        PRINTM(MINFO, "Scan already in process...\n");
+        LEAVE();
+        return status;
+    }
+
+    if (pmpriv->scan_block && pioctl_req->action == MLAN_ACT_SET) {
+        PRINTM(MINFO, "Scan is blocked during association...\n");
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_DRV_SCAN_REPORT, MNULL);
+        LEAVE();
+        return status;
+    }
+  start_config:
+    /* Set scan */
+    if (pioctl_req->action == MLAN_ACT_SET) {
+
+        switch (pscan->sub_command) {
+        case MLAN_OID_SCAN_NORMAL:
+            status = wlan_scan_networks(pmpriv, pioctl_req, MNULL);
+            break;
+        case MLAN_OID_SCAN_SPECIFIC_SSID:
+            status =
+                wlan_scan_specific_ssid(pmpriv, pioctl_req,
+                                        &pscan->param.scan_req.scan_ssid);
+            break;
+        case MLAN_OID_SCAN_USER_CONFIG:
+            status = wlan_scan_networks(pmpriv, pioctl_req,
+                                        (wlan_user_scan_cfg *) pscan->param.
+                                        user_scan.scan_cfg_buf);
+            break;
+        case MLAN_OID_SCAN_CONFIG:
+            status = wlan_set_get_scan_cfg(pmadapter, pioctl_req, MLAN_ACT_SET);
+            break;
+        case MLAN_OID_SCAN_CANCEL:
+            wlan_flush_scan_queue(pmadapter);
+            break;
+        case MLAN_OID_SCAN_TABLE_FLUSH:
+            status = wlan_flush_scan_table(pmadapter);
+            break;
+        case MLAN_OID_SCAN_BGSCAN_CONFIG:
+            /* Send request to firmware */
+            status = wlan_prepare_cmd(pmpriv,
+                                      HostCmd_CMD_802_11_BG_SCAN_CONFIG,
+                                      HostCmd_ACT_GEN_SET,
+                                      0,
+                                      (t_void *) pioctl_req,
+                                      pscan->param.user_scan.scan_cfg_buf);
+            break;
+        default:
+            pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+            status = MLAN_STATUS_FAILURE;
+            break;
+        }
+
+        if ((status == MLAN_STATUS_SUCCESS) &&
+            (pscan->sub_command != MLAN_OID_SCAN_TABLE_FLUSH) &&
+            (pscan->sub_command != MLAN_OID_SCAN_CANCEL) &&
+            (pscan->sub_command != MLAN_OID_SCAN_CONFIG)) {
+            PRINTM(MINFO, "wlan_scan_ioctl: return MLAN_STATUS_PENDING\n");
+            status = MLAN_STATUS_PENDING;
+        }
+    }
+    /* Get scan */
+    else {
+        if (pscan->sub_command == MLAN_OID_SCAN_CONFIG) {
+            status = wlan_set_get_scan_cfg(pmadapter, pioctl_req, MLAN_ACT_GET);
+        } else if (pscan->sub_command == MLAN_OID_SCAN_GET_CURRENT_BSS) {
+            pscan->param.scan_resp.num_in_scan_table =
+                pmadapter->num_in_scan_table;
+            pscan->param.scan_resp.pscan_table =
+                (t_u8 *) & pmpriv->curr_bss_params.bss_descriptor;
+            pioctl_req->data_read_written =
+                sizeof(mlan_scan_resp) + MLAN_SUB_COMMAND_SIZE;
+        } else {
+            if (pmadapter->bgscan_reported) {
+                pmadapter->bgscan_reported = MFALSE;
+                /* Clear the previous scan result */
+                memset(pmadapter, pmadapter->pscan_table, 0x00,
+                       sizeof(BSSDescriptor_t) * MRVDRV_MAX_BSSID_LIST);
+                pmadapter->num_in_scan_table = 0;
+                pmadapter->pbcn_buf_end = pmadapter->bcn_buf;
+                status = wlan_prepare_cmd(pmpriv,
+                                          HostCmd_CMD_802_11_BG_SCAN_QUERY,
+                                          HostCmd_ACT_GEN_GET,
+                                          0, (t_void *) pioctl_req, MNULL);
+                if (status == MLAN_STATUS_SUCCESS) {
+                    PRINTM(MINFO,
+                           "wlan_scan_ioctl: return MLAN_STATUS_PENDING\n");
+                    status = MLAN_STATUS_PENDING;
+                }
+            } else {
+                pscan->param.scan_resp.pscan_table =
+                    (t_u8 *) pmadapter->pscan_table;
+                pscan->param.scan_resp.num_in_scan_table =
+                    pmadapter->num_in_scan_table;
+                pscan->param.scan_resp.age_in_secs = pmadapter->age_in_secs;
+                pioctl_req->data_read_written = sizeof(mlan_scan_resp) +
+                    MLAN_SUB_COMMAND_SIZE;
+            }
+        }
+    }
+
+    LEAVE();
+    return status;
+}
+
+/********************************************************
+                Global Functions
+********************************************************/
+
+/**
+ *  @brief Set ewpa mode
+ *
+ *  @param priv                 A pointer to mlan_private structure
+ *  @param psec_pp              A pointer to mlan_ds_passphrase structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_set_ewpa_mode(mlan_private * priv, mlan_ds_passphrase * psec_pp)
+{
+    ENTER();
+
+    if ((psec_pp->psk_type == MLAN_PSK_PASSPHRASE &&
+         psec_pp->psk.passphrase.passphrase_len > 0) ||
+        (psec_pp->psk_type == MLAN_PSK_PMK))
+        priv->sec_info.ewpa_enabled = MTRUE;
+    else
+        priv->sec_info.ewpa_enabled = MFALSE;
+
+    PRINTM(MINFO, "Set ewpa mode = %d\n", priv->sec_info.ewpa_enabled);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Search for a BSS
+ *
+ *  @param pmpriv           A pointer to mlan_private structure
+ *  @param pioctl_req       A pointer to ioctl request buffer
+ *
+ *  @return		            MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+mlan_status
+wlan_find_bss(mlan_private * pmpriv, pmlan_ioctl_req pioctl_req)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    mlan_ds_bss *bss = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u8 zero_mac[] = { 0, 0, 0, 0, 0, 0 };
+    t_u8 mac[MLAN_MAC_ADDR_LENGTH];
+    int i = 0;
+    BSSDescriptor_t *pbss_desc;
+
+    ENTER();
+
+    bss = (mlan_ds_bss *) pioctl_req->pbuf;
+
+    if (memcmp
+        (pmadapter, &bss->param.ssid_bssid.bssid, zero_mac, sizeof(zero_mac))) {
+        if (bss->param.ssid_bssid.ssid.ssid_len)        /* ssid & bssid */
+            i = wlan_find_ssid_in_list(pmpriv, &bss->param.ssid_bssid.ssid,
+                                       (t_u8 *) & bss->param.ssid_bssid.bssid,
+                                       pmpriv->bss_mode);
+        else
+            i = wlan_find_bssid_in_list(pmpriv,
+                                        (t_u8 *) & bss->param.ssid_bssid.bssid,
+                                        pmpriv->bss_mode);
+        if (i < 0) {
+            memcpy(pmadapter, mac, &bss->param.ssid_bssid.bssid, sizeof(mac));
+            PRINTM(MERROR, "Can not find bssid %02x:%02x:%02x:%02x:%02x:%02x\n",
+                   mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+        pbss_desc = &pmadapter->pscan_table[i];
+        memcpy(pmadapter, &bss->param.ssid_bssid.ssid, &pbss_desc->ssid,
+               sizeof(mlan_802_11_ssid));
+        /* index in bss list,start from 1 */
+        bss->param.ssid_bssid.idx = i + 1;
+    } else if (bss->param.ssid_bssid.ssid.ssid_len) {
+        i = wlan_find_ssid_in_list(pmpriv, &bss->param.ssid_bssid.ssid, MNULL,
+                                   pmpriv->bss_mode);
+        if (i < 0) {
+            PRINTM(MERROR, "Can not find ssid %s\n",
+                   bss->param.ssid_bssid.ssid.ssid);
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+        pbss_desc = &pmadapter->pscan_table[i];
+        memcpy(pmadapter, (t_u8 *) & bss->param.ssid_bssid.bssid,
+               (t_u8 *) & pbss_desc->mac_address, MLAN_MAC_ADDR_LENGTH);
+        /* index in bss list, start from 1 */
+        bss->param.ssid_bssid.idx = i + 1;
+    } else {
+        ret = wlan_find_best_network(pmpriv, &bss->param.ssid_bssid);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief MLAN station ioctl handler
+ *
+ *  @param adapter 	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+wlan_ops_sta_ioctl(t_void * adapter, pmlan_ioctl_req pioctl_req)
+{
+    pmlan_adapter pmadapter = (pmlan_adapter) adapter;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    switch (pioctl_req->req_id) {
+    case MLAN_IOCTL_SCAN:
+        status = wlan_scan_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_BSS:
+        status = wlan_bss_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_RADIO_CFG:
+        status = wlan_radio_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_SNMP_MIB:
+        status = wlan_snmp_mib_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_GET_INFO:
+        status = wlan_get_info_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_SEC_CFG:
+        status = wlan_sec_cfg_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_RATE:
+        status = wlan_rate_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_POWER_CFG:
+        status = wlan_power_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_PM_CFG:
+        status = wlan_pm_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_WMM_CFG:
+        status = wlan_wmm_cfg_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_WPS_CFG:
+        status = wlan_wps_cfg_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_11N_CFG:
+        status = wlan_11n_cfg_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_11D_CFG:
+        status = wlan_11d_cfg_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_REG_MEM:
+        status = wlan_reg_mem_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_MISC_CFG:
+        status = wlan_misc_cfg_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_11H_CFG:
+        status = wlan_11h_cfg_ioctl(pmadapter, pioctl_req);
+        break;
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        status = MLAN_STATUS_FAILURE;
+        break;
+    }
+    LEAVE();
+    return status;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_sta_rx.c b/drivers/net/wireless/sd8797/mlan/mlan_sta_rx.c
new file mode 100644
index 000000000000..5e5c68e2a108
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_sta_rx.c
@@ -0,0 +1,289 @@
+/** @file mlan_sta_rx.c
+ *
+ *  @brief This file contains the handling of RX in MLAN
+ *  module.
+ * 
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/********************************************************
+Change log:
+    10/27/2008: initial version
+********************************************************/
+
+#include "mlan.h"
+#include "mlan_join.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_11n_aggr.h"
+#include "mlan_11n_rxreorder.h"
+
+/********************************************************
+		Local Variables
+********************************************************/
+
+/** Ethernet II header */
+typedef struct
+{
+    /** Ethernet II header destination address */
+    t_u8 dest_addr[MLAN_MAC_ADDR_LENGTH];
+    /** Ethernet II header source address */
+    t_u8 src_addr[MLAN_MAC_ADDR_LENGTH];
+    /** Ethernet II header length */
+    t_u16 ethertype;
+
+} EthII_Hdr_t;
+
+/********************************************************
+		Global Variables
+********************************************************/
+
+/********************************************************
+		Local Functions
+********************************************************/
+
+/********************************************************
+		Global functions
+********************************************************/
+
+/**
+ *  @brief This function processes received packet and forwards it
+ *  		to kernel/upper layer
+ *
+ *  @param pmadapter A pointer to mlan_adapter
+ *  @param pmbuf     A pointer to mlan_buffer which includes the received packet
+ *
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_process_rx_packet(pmlan_adapter pmadapter, pmlan_buffer pmbuf)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private priv = pmadapter->priv[pmbuf->bss_index];
+    RxPacketHdr_t *prx_pkt;
+    RxPD *prx_pd;
+    int hdr_chop;
+    EthII_Hdr_t *peth_hdr;
+    t_u8 rfc1042_eth_hdr[MLAN_MAC_ADDR_LENGTH] =
+        { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00 };
+    t_u8 snap_oui_802_h[MLAN_MAC_ADDR_LENGTH] =
+        { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8 };
+    t_u8 appletalk_aarp_type[2] = { 0x80, 0xf3 };
+    t_u8 ipx_snap_type[2] = { 0x81, 0x37 };
+
+    ENTER();
+
+    prx_pd = (RxPD *) (pmbuf->pbuf + pmbuf->data_offset);
+    prx_pkt = (RxPacketHdr_t *) ((t_u8 *) prx_pd + prx_pd->rx_pkt_offset);
+
+/** Small debug type */
+#define DBG_TYPE_SMALL  2
+/** Size of debugging structure */
+#define SIZE_OF_DBG_STRUCT 4
+    if (prx_pd->rx_pkt_type == PKT_TYPE_DEBUG) {
+        t_u8 dbgType;
+        dbgType = *(t_u8 *) & prx_pkt->eth803_hdr;
+        if (dbgType == DBG_TYPE_SMALL) {
+            PRINTM(MFW_D, "\n");
+            DBG_HEXDUMP(MFW_D, "FWDBG",
+                        (t_s8 *) ((t_u8 *) & prx_pkt->eth803_hdr +
+                                  SIZE_OF_DBG_STRUCT), prx_pd->rx_pkt_length);
+            PRINTM(MFW_D, "FWDBG::\n");
+        }
+        goto done;
+    }
+
+    PRINTM(MINFO, "RX Data: data_len - prx_pd->rx_pkt_offset = %d - %d = %d\n",
+           pmbuf->data_len, prx_pd->rx_pkt_offset,
+           pmbuf->data_len - prx_pd->rx_pkt_offset);
+
+    HEXDUMP("RX Data: Dest", prx_pkt->eth803_hdr.dest_addr,
+            sizeof(prx_pkt->eth803_hdr.dest_addr));
+    HEXDUMP("RX Data: Src", prx_pkt->eth803_hdr.src_addr,
+            sizeof(prx_pkt->eth803_hdr.src_addr));
+
+    if ((memcmp(pmadapter, &prx_pkt->rfc1042_hdr,
+                snap_oui_802_h, sizeof(snap_oui_802_h)) == 0) ||
+        ((memcmp(pmadapter, &prx_pkt->rfc1042_hdr,
+                 rfc1042_eth_hdr, sizeof(rfc1042_eth_hdr)) == 0) &&
+         memcmp(pmadapter, &prx_pkt->rfc1042_hdr.snap_type,
+                appletalk_aarp_type, sizeof(appletalk_aarp_type)) &&
+         memcmp(pmadapter, &prx_pkt->rfc1042_hdr.snap_type,
+                ipx_snap_type, sizeof(ipx_snap_type)))) {
+        /*
+         *  Replace the 803 header and rfc1042 header (llc/snap) with an
+         *    EthernetII header, keep the src/dst and snap_type (ethertype).
+         *  The firmware only passes up SNAP frames converting
+         *    all RX Data from 802.11 to 802.2/LLC/SNAP frames.
+         *  To create the Ethernet II, just move the src, dst address right
+         *    before the snap_type.
+         */
+        peth_hdr = (EthII_Hdr_t *)
+            ((t_u8 *) & prx_pkt->eth803_hdr
+             + sizeof(prx_pkt->eth803_hdr) + sizeof(prx_pkt->rfc1042_hdr)
+             - sizeof(prx_pkt->eth803_hdr.dest_addr)
+             - sizeof(prx_pkt->eth803_hdr.src_addr)
+             - sizeof(prx_pkt->rfc1042_hdr.snap_type));
+
+        memcpy(pmadapter, peth_hdr->src_addr, prx_pkt->eth803_hdr.src_addr,
+               sizeof(peth_hdr->src_addr));
+        memcpy(pmadapter, peth_hdr->dest_addr, prx_pkt->eth803_hdr.dest_addr,
+               sizeof(peth_hdr->dest_addr));
+
+        /* Chop off the RxPD + the excess memory from the 802.2/llc/snap header
+           that was removed. */
+        hdr_chop = (t_u32) ((t_ptr) peth_hdr - (t_ptr) prx_pd);
+    } else {
+        HEXDUMP("RX Data: LLC/SNAP",
+                (t_u8 *) & prx_pkt->rfc1042_hdr, sizeof(prx_pkt->rfc1042_hdr));
+
+        /* Chop off the RxPD */
+        hdr_chop = (t_u32) ((t_ptr) & prx_pkt->eth803_hdr - (t_ptr) prx_pd);
+    }
+
+    /* Chop off the leading header bytes so the it points to the start of
+       either the reconstructed EthII frame or the 802.2/llc/snap frame */
+    pmbuf->data_len -= hdr_chop;
+    pmbuf->data_offset += hdr_chop;
+    pmbuf->pparent = MNULL;
+
+    DBG_HEXDUMP(MDAT_D, "RxPD", (t_u8 *) prx_pd,
+                MIN(sizeof(RxPD), MAX_DATA_DUMP_LEN));
+    DBG_HEXDUMP(MDAT_D, "Rx Payload", ((t_u8 *) prx_pd + prx_pd->rx_pkt_offset),
+                MIN(prx_pd->rx_pkt_length, MAX_DATA_DUMP_LEN));
+
+    priv->rxpd_rate = prx_pd->rx_rate;
+
+    priv->rxpd_htinfo = prx_pd->ht_info;
+    pmadapter->callbacks.moal_get_system_time(pmadapter->pmoal_handle,
+                                              &pmbuf->out_ts_sec,
+                                              &pmbuf->out_ts_usec);
+    PRINTM(MDATA, "%lu.%06lu : Data => kernel seq_num=%d tid=%d\n",
+           pmbuf->out_ts_sec, pmbuf->out_ts_usec, prx_pd->seq_num,
+           prx_pd->priority);
+
+    ret = pmadapter->callbacks.moal_recv_packet(pmadapter->pmoal_handle, pmbuf);
+    if (ret == MLAN_STATUS_FAILURE) {
+        pmbuf->status_code = MLAN_ERROR_PKT_INVALID;
+        PRINTM(MERROR, "STA Rx Error: moal_recv_packet returned error\n");
+    }
+  done:
+    if (ret != MLAN_STATUS_PENDING) {
+        wlan_free_mlan_buffer(pmadapter, pmbuf);
+    }
+    LEAVE();
+
+    return ret;
+}
+
+/**
+ *   @brief This function processes the received buffer
+ *
+ *   @param adapter A pointer to mlan_adapter
+ *   @param pmbuf     A pointer to the received buffer
+ *
+ *   @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ops_sta_process_rx_packet(IN t_void * adapter, IN pmlan_buffer pmbuf)
+{
+    pmlan_adapter pmadapter = (pmlan_adapter) adapter;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    RxPD *prx_pd;
+    RxPacketHdr_t *prx_pkt;
+    pmlan_private priv = pmadapter->priv[pmbuf->bss_index];
+    t_u8 ta[MLAN_MAC_ADDR_LENGTH];
+    t_u16 rx_pkt_type = 0;
+    wlan_mgmt_pkt *pmgmt_pkt_hdr = MNULL;
+    ENTER();
+
+    prx_pd = (RxPD *) (pmbuf->pbuf + pmbuf->data_offset);
+    /* Endian conversion */
+    endian_convert_RxPD(prx_pd);
+    rx_pkt_type = prx_pd->rx_pkt_type;
+    prx_pkt = (RxPacketHdr_t *) ((t_u8 *) prx_pd + prx_pd->rx_pkt_offset);
+
+    if ((prx_pd->rx_pkt_offset + prx_pd->rx_pkt_length) >
+        (t_u16) pmbuf->data_len) {
+        PRINTM(MERROR,
+               "Wrong rx packet: len=%d,rx_pkt_offset=%d,"
+               " rx_pkt_length=%d\n", pmbuf->data_len, prx_pd->rx_pkt_offset,
+               prx_pd->rx_pkt_length);
+        pmbuf->status_code = MLAN_ERROR_PKT_SIZE_INVALID;
+        ret = MLAN_STATUS_FAILURE;
+        wlan_free_mlan_buffer(pmadapter, pmbuf);
+        goto done;
+    }
+    pmbuf->data_len = prx_pd->rx_pkt_offset + prx_pd->rx_pkt_length;
+
+    if (pmadapter->priv[pmbuf->bss_index]->mgmt_frame_passthru_mask &&
+        prx_pd->rx_pkt_type == PKT_TYPE_MGMT_FRAME) {
+        /* Check if this is mgmt packet and needs to forwarded to app as an
+           event */
+        pmgmt_pkt_hdr =
+            (wlan_mgmt_pkt *) ((t_u8 *) prx_pd + prx_pd->rx_pkt_offset);
+        pmgmt_pkt_hdr->frm_len = wlan_le16_to_cpu(pmgmt_pkt_hdr->frm_len);
+
+        if ((pmgmt_pkt_hdr->wlan_header.frm_ctl
+             & IEEE80211_FC_MGMT_FRAME_TYPE_MASK) == 0)
+            wlan_process_802dot11_mgmt_pkt(pmadapter->priv[pmbuf->bss_index],
+                                           (t_u8 *) & pmgmt_pkt_hdr->
+                                           wlan_header,
+                                           pmgmt_pkt_hdr->frm_len +
+                                           sizeof(wlan_mgmt_pkt)
+                                           - sizeof(pmgmt_pkt_hdr->frm_len));
+        wlan_free_mlan_buffer(pmadapter, pmbuf);
+        goto done;
+    }
+
+    /*
+     * If the packet is not an unicast packet then send the packet
+     * directly to os. Don't pass thru rx reordering
+     */
+    if (!IS_11N_ENABLED(priv) ||
+        memcmp(priv->adapter, priv->curr_addr, prx_pkt->eth803_hdr.dest_addr,
+               MLAN_MAC_ADDR_LENGTH)) {
+        wlan_process_rx_packet(pmadapter, pmbuf);
+        goto done;
+    }
+
+    if (queuing_ra_based(priv)) {
+        memcpy(pmadapter, ta, prx_pkt->eth803_hdr.src_addr,
+               MLAN_MAC_ADDR_LENGTH);
+    } else {
+        if ((rx_pkt_type != PKT_TYPE_BAR) && (prx_pd->priority < MAX_NUM_TID))
+            priv->rx_seq[prx_pd->priority] = prx_pd->seq_num;
+        memcpy(pmadapter, ta,
+               priv->curr_bss_params.bss_descriptor.mac_address,
+               MLAN_MAC_ADDR_LENGTH);
+    }
+
+    /* Reorder and send to OS */
+    if ((ret = mlan_11n_rxreorder_pkt(priv, prx_pd->seq_num,
+                                      prx_pd->priority, ta,
+                                      (t_u8) prx_pd->rx_pkt_type,
+                                      (void *) pmbuf)) ||
+        (rx_pkt_type == PKT_TYPE_BAR)
+        ) {
+        wlan_free_mlan_buffer(pmadapter, pmbuf);
+    }
+
+  done:
+
+    LEAVE();
+    return (ret);
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_sta_tx.c b/drivers/net/wireless/sd8797/mlan/mlan_sta_tx.c
new file mode 100644
index 000000000000..2258853a4089
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_sta_tx.c
@@ -0,0 +1,277 @@
+/** @file mlan_sta_tx.c
+ *
+ *  @brief This file contains the handling of data packet
+ *  transmission in MLAN module.
+ * 
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/********************************************************
+Change log:
+    10/21/2008: initial version
+********************************************************/
+
+#include "mlan.h"
+#include "mlan_join.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_wmm.h"
+#include "mlan_sdio.h"
+
+/********************************************************
+		Local Variables
+********************************************************/
+
+/********************************************************
+		Global Variables
+********************************************************/
+
+/********************************************************
+		Local Functions
+********************************************************/
+
+/********************************************************
+		Global functions
+********************************************************/
+/**
+ *  @brief This function fill the txpd for tx packet
+ *
+ *  @param priv	   A pointer to mlan_private structure
+ *  @param pmbuf   A pointer to the mlan_buffer for process
+ *
+ *  @return 	   headptr or MNULL
+ */
+t_void *
+wlan_ops_sta_process_txpd(IN t_void * priv, IN pmlan_buffer pmbuf)
+{
+    mlan_private *pmpriv = (mlan_private *) priv;
+    pmlan_adapter pmadapter = pmpriv->adapter;
+    TxPD *plocal_tx_pd;
+    t_u8 *head_ptr = MNULL;
+    t_u32 pkt_type;
+    t_u32 tx_control;
+
+    ENTER();
+
+    if (!pmbuf->data_len) {
+        PRINTM(MERROR, "STA Tx Error: Invalid packet length: %d\n",
+               pmbuf->data_len);
+        pmbuf->status_code = MLAN_ERROR_PKT_SIZE_INVALID;
+        goto done;
+    }
+    if (pmbuf->buf_type == MLAN_BUF_TYPE_RAW_DATA) {
+        memcpy(pmpriv->adapter, &pkt_type, pmbuf->pbuf + pmbuf->data_offset,
+               sizeof(pkt_type));
+        memcpy(pmpriv->adapter, &tx_control,
+               pmbuf->pbuf + pmbuf->data_offset + sizeof(pkt_type),
+               sizeof(tx_control));
+        pmbuf->data_offset += sizeof(pkt_type) + sizeof(tx_control);
+        pmbuf->data_len -= sizeof(pkt_type) + sizeof(tx_control);
+    }
+
+    if (pmbuf->data_offset < (sizeof(TxPD) + INTF_HEADER_LEN + DMA_ALIGNMENT)) {
+        PRINTM(MERROR, "not enough space for TxPD: %d\n", pmbuf->data_len);
+        pmbuf->status_code = MLAN_ERROR_PKT_SIZE_INVALID;
+        goto done;
+    }
+
+    /* head_ptr should be aligned */
+    head_ptr =
+        pmbuf->pbuf + pmbuf->data_offset - sizeof(TxPD) - INTF_HEADER_LEN;
+    head_ptr = (t_u8 *) ((t_ptr) head_ptr & ~((t_ptr) (DMA_ALIGNMENT - 1)));
+
+    plocal_tx_pd = (TxPD *) (head_ptr + INTF_HEADER_LEN);
+    memset(pmadapter, plocal_tx_pd, 0, sizeof(TxPD));
+    /* Set the BSS number to TxPD */
+    plocal_tx_pd->bss_num = GET_BSS_NUM(pmpriv);
+    plocal_tx_pd->bss_type = pmpriv->bss_type;
+
+    plocal_tx_pd->tx_pkt_length = (t_u16) pmbuf->data_len;
+
+    plocal_tx_pd->priority = (t_u8) pmbuf->priority;
+    plocal_tx_pd->pkt_delay_2ms =
+        wlan_wmm_compute_driver_packet_delay(pmpriv, pmbuf);
+
+    if (plocal_tx_pd->priority < NELEMENTS(pmpriv->wmm.user_pri_pkt_tx_ctrl))
+        /*
+         * Set the priority specific tx_control field, setting of 0 will
+         *   cause the default value to be used later in this function
+         */
+        plocal_tx_pd->tx_control
+            = pmpriv->wmm.user_pri_pkt_tx_ctrl[plocal_tx_pd->priority];
+    if (pmadapter->pps_uapsd_mode) {
+        if (MTRUE == wlan_check_last_packet_indication(pmpriv)) {
+            pmadapter->tx_lock_flag = MTRUE;
+            plocal_tx_pd->flags = MRVDRV_TxPD_POWER_MGMT_LAST_PACKET;
+        }
+    }
+
+    /* Offset of actual data */
+    plocal_tx_pd->tx_pkt_offset =
+        (t_u16) ((t_ptr) pmbuf->pbuf + pmbuf->data_offset -
+                 (t_ptr) plocal_tx_pd);
+
+    if (!plocal_tx_pd->tx_control) {
+        /* TxCtrl set by user or default */
+        plocal_tx_pd->tx_control = pmpriv->pkt_tx_ctrl;
+    }
+
+    if (pmbuf->buf_type == MLAN_BUF_TYPE_RAW_DATA) {
+        plocal_tx_pd->tx_pkt_type = (t_u16) pkt_type;
+        plocal_tx_pd->tx_control = tx_control;
+    }
+
+    endian_convert_TxPD(plocal_tx_pd);
+
+    /* Adjust the data offset and length to include TxPD in pmbuf */
+    pmbuf->data_len += pmbuf->data_offset;
+    pmbuf->data_offset = (t_u32) (head_ptr - pmbuf->pbuf);
+    pmbuf->data_len -= pmbuf->data_offset;
+
+  done:
+    LEAVE();
+    return head_ptr;
+}
+
+/**
+ *  @brief This function tells firmware to send a NULL data packet.
+ *
+ *  @param priv     A pointer to mlan_private structure
+ *  @param flags    Transmit Pkt Flags
+ *
+ *  @return 	    MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise failure
+ */
+mlan_status
+wlan_send_null_packet(pmlan_private priv, t_u8 flags)
+{
+    pmlan_adapter pmadapter = priv->adapter;
+    TxPD *ptx_pd;
+/* sizeof(TxPD) + Interface specific header */
+#define NULL_PACKET_HDR 256
+    t_u32 data_len = NULL_PACKET_HDR;
+    pmlan_buffer pmbuf = MNULL;
+    t_u8 *ptr;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+#ifdef DEBUG_LEVEL1
+    t_u32 sec, usec;
+#endif
+
+    ENTER();
+
+    if (pmadapter->surprise_removed == MTRUE) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    if (priv->media_connected == MFALSE) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    if (pmadapter->data_sent == MTRUE) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    pmbuf = wlan_alloc_mlan_buffer(pmadapter, data_len, 0, MTRUE);
+    if (!pmbuf) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    memset(pmadapter, pmbuf->pbuf, 0, data_len);
+    pmbuf->bss_index = priv->bss_index;
+    pmbuf->buf_type = MLAN_BUF_TYPE_DATA;
+    ptr = pmbuf->pbuf + pmbuf->data_offset;
+    pmbuf->data_len = sizeof(TxPD) + INTF_HEADER_LEN;
+    ptx_pd = (TxPD *) (ptr + INTF_HEADER_LEN);
+    ptx_pd->tx_control = priv->pkt_tx_ctrl;
+    ptx_pd->flags = flags;
+    ptx_pd->priority = WMM_HIGHEST_PRIORITY;
+    ptx_pd->tx_pkt_offset = sizeof(TxPD);
+    /* Set the BSS number to TxPD */
+    ptx_pd->bss_num = GET_BSS_NUM(priv);
+    ptx_pd->bss_type = priv->bss_type;
+
+    endian_convert_TxPD(ptx_pd);
+
+    ret = wlan_sdio_host_to_card(pmadapter, MLAN_TYPE_DATA, pmbuf, MNULL);
+
+    switch (ret) {
+    case MLAN_STATUS_RESOURCE:
+        pmadapter->data_sent = MTRUE;
+        /* Fall through FAILURE handling */
+    case MLAN_STATUS_FAILURE:
+        wlan_free_mlan_buffer(pmadapter, pmbuf);
+        PRINTM(MERROR, "STA Tx Error: Failed to send NULL packet!\n");
+        pmadapter->dbg.num_tx_host_to_card_failure++;
+        goto done;
+    case MLAN_STATUS_SUCCESS:
+        wlan_free_mlan_buffer(pmadapter, pmbuf);
+        PRINTM(MINFO, "STA Tx: Successfully send the NULL packet\n");
+        pmadapter->tx_lock_flag = MTRUE;
+        break;
+    case MLAN_STATUS_PENDING:
+        break;
+    default:
+        break;
+    }
+
+    PRINTM_GET_SYS_TIME(MDATA, &sec, &usec);
+    PRINTM(MDATA, "%lu.%06lu : Null data => FW\n", sec, usec);
+    DBG_HEXDUMP(MDAT_D, "Null data", ptr, sizeof(TxPD) + INTF_HEADER_LEN);
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function checks if we need to send last packet indication.
+ *
+ *  @param priv    A pointer to mlan_private structure
+ *
+ *  @return 	   MTRUE or MFALSE
+ */
+t_u8
+wlan_check_last_packet_indication(pmlan_private priv)
+{
+    pmlan_adapter pmadapter = priv->adapter;
+    t_u8 ret = MFALSE;
+    t_u8 prop_ps = MTRUE;
+
+    ENTER();
+
+    if (!pmadapter->sleep_period.period) {
+        LEAVE();
+        return ret;
+    }
+    if (wlan_bypass_tx_list_empty(pmadapter) && wlan_wmm_lists_empty(pmadapter)) {
+        if (((priv->curr_bss_params.wmm_uapsd_enabled == MTRUE) &&
+             priv->wmm_qosinfo) || prop_ps)
+
+            ret = MTRUE;
+    }
+    if (ret && !pmadapter->cmd_sent && !pmadapter->curr_cmd
+        && !IS_COMMAND_PENDING(pmadapter)) {
+        pmadapter->delay_null_pkt = MFALSE;
+        ret = MTRUE;
+    } else {
+        ret = MFALSE;
+        pmadapter->delay_null_pkt = MTRUE;
+    }
+    LEAVE();
+    return ret;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_txrx.c b/drivers/net/wireless/sd8797/mlan/mlan_txrx.c
new file mode 100644
index 000000000000..93dea9e284f8
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_txrx.c
@@ -0,0 +1,376 @@
+/**
+ * @file mlan_txrx.c
+ *
+ *  @brief This file contains the handling of TX/RX in MLAN
+ *
+ *  Copyright (C) 2009-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *  
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/*************************************************************
+Change Log:
+    05/11/2009: initial version
+************************************************************/
+
+#include "mlan.h"
+#ifdef STA_SUPPORT
+#include "mlan_join.h"
+#endif
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_wmm.h"
+#include "mlan_sdio.h"
+
+/********************************************************
+                Local Variables
+********************************************************/
+
+/********************************************************
+                Global Variables
+********************************************************/
+
+/********************************************************
+                Local Functions
+********************************************************/
+
+/********************************************************
+                Global Functions
+********************************************************/
+/**
+ *   @brief This function processes the received buffer
+ *
+ *   @param pmadapter A pointer to mlan_adapter
+ *   @param pmbuf     A pointer to the received buffer
+ *
+ *   @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_handle_rx_packet(pmlan_adapter pmadapter, pmlan_buffer pmbuf)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private priv = wlan_get_priv(pmadapter, MLAN_BSS_ROLE_ANY);
+    RxPD *prx_pd;
+#ifdef DEBUG_LEVEL1
+    t_u32 sec, usec;
+#endif
+
+    ENTER();
+
+    prx_pd = (RxPD *) (pmbuf->pbuf + pmbuf->data_offset);
+    /* Get the BSS number from RxPD, get corresponding priv */
+    priv =
+        wlan_get_priv_by_id(pmadapter, prx_pd->bss_num & BSS_NUM_MASK,
+                            prx_pd->bss_type);
+    if (!priv)
+        priv = wlan_get_priv(pmadapter, MLAN_BSS_ROLE_ANY);
+    pmbuf->bss_index = priv->bss_index;
+    PRINTM_GET_SYS_TIME(MDATA, &sec, &usec);
+    PRINTM_NETINTF(MDATA, priv);
+    PRINTM(MDATA, "%lu.%06lu : Data <= FW\n", sec, usec);
+    ret = priv->ops.process_rx_packet(pmadapter, pmbuf);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function checks the conditions and sends packet to device
+ *
+ *  @param priv	   A pointer to mlan_private structure
+ *  @param pmbuf   A pointer to the mlan_buffer for process
+ *  @param tx_param A pointer to mlan_tx_param structure
+ *
+ *  @return 	    MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise failure
+ */
+mlan_status
+wlan_process_tx(pmlan_private priv, pmlan_buffer pmbuf,
+                mlan_tx_param * tx_param)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_adapter pmadapter = priv->adapter;
+    t_u8 *head_ptr = MNULL;
+#ifdef DEBUG_LEVEL1
+    t_u32 sec, usec;
+#endif
+#ifdef STA_SUPPORT
+    TxPD *plocal_tx_pd = MNULL;
+#endif
+
+    ENTER();
+
+    head_ptr = (t_u8 *) priv->ops.process_txpd(priv, pmbuf);
+    if (!head_ptr) {
+        pmbuf->status_code = MLAN_ERROR_PKT_INVALID;
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+#ifdef STA_SUPPORT
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA)
+        plocal_tx_pd = (TxPD *) (head_ptr + INTF_HEADER_LEN);
+#endif
+    ret = wlan_sdio_host_to_card(pmadapter, MLAN_TYPE_DATA, pmbuf, tx_param);
+  done:
+    switch (ret) {
+    case MLAN_STATUS_RESOURCE:
+#ifdef STA_SUPPORT
+        if ((GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) &&
+            pmadapter->pps_uapsd_mode && (pmadapter->tx_lock_flag == MTRUE)) {
+            pmadapter->tx_lock_flag = MFALSE;
+            plocal_tx_pd->flags = 0;
+        }
+#endif
+        PRINTM(MINFO, "MLAN_STATUS_RESOURCE is returned\n");
+        break;
+    case MLAN_STATUS_FAILURE:
+        pmadapter->data_sent = MFALSE;
+        PRINTM(MERROR, "Error: host_to_card failed: 0x%X\n", ret);
+        pmadapter->dbg.num_tx_host_to_card_failure++;
+        pmbuf->status_code = MLAN_ERROR_DATA_TX_FAIL;
+        wlan_write_data_complete(pmadapter, pmbuf, ret);
+        break;
+    case MLAN_STATUS_PENDING:
+        pmadapter->data_sent = MFALSE;
+        DBG_HEXDUMP(MDAT_D, "Tx", head_ptr + INTF_HEADER_LEN,
+                    MIN(pmbuf->data_len + sizeof(TxPD), MAX_DATA_DUMP_LEN));
+        break;
+    case MLAN_STATUS_SUCCESS:
+        DBG_HEXDUMP(MDAT_D, "Tx", head_ptr + INTF_HEADER_LEN,
+                    MIN(pmbuf->data_len + sizeof(TxPD), MAX_DATA_DUMP_LEN));
+        wlan_write_data_complete(pmadapter, pmbuf, ret);
+        break;
+    default:
+        break;
+    }
+
+    if ((ret == MLAN_STATUS_SUCCESS) || (ret == MLAN_STATUS_PENDING)) {
+        PRINTM_GET_SYS_TIME(MDATA, &sec, &usec);
+        PRINTM_NETINTF(MDATA, priv);
+        PRINTM(MDATA, "%lu.%06lu : Data => FW\n", sec, usec);
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Packet send completion handling
+ *
+ *  @param pmadapter		A pointer to mlan_adapter structure
+ *  @param pmbuf		A pointer to mlan_buffer structure
+ *  @param status		Callback status
+ *
+ *  @return			MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_write_data_complete(IN pmlan_adapter pmadapter,
+                         IN pmlan_buffer pmbuf, IN mlan_status status)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_callbacks pcb;
+
+    ENTER();
+
+    MASSERT(pmadapter && pmbuf);
+
+    pcb = &pmadapter->callbacks;
+    if ((pmbuf->buf_type == MLAN_BUF_TYPE_DATA) ||
+        (pmbuf->buf_type == MLAN_BUF_TYPE_RAW_DATA)) {
+        PRINTM(MINFO, "wlan_write_data_complete: DATA %p\n", pmbuf);
+        if (pmbuf->flags & MLAN_BUF_FLAG_MOAL_TX_BUF) {
+            /* pmbuf was allocated by MOAL */
+            pcb->moal_send_packet_complete(pmadapter->pmoal_handle,
+                                           pmbuf, status);
+        } else {
+            if (pmbuf->flags & MLAN_BUF_FLAG_BRIDGE_BUF) {
+                pmadapter->pending_bridge_pkts--;
+            }
+            /* pmbuf was allocated by MLAN */
+            wlan_free_mlan_buffer(pmadapter, pmbuf);
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Packet receive completion callback handler
+ *
+ *  @param pmadapter		A pointer to mlan_adapter structure
+ *  @param pmbuf		A pointer to mlan_buffer structure
+ *  @param status		Callback status
+ *
+ *  @return			MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_recv_packet_complete(IN pmlan_adapter pmadapter,
+                          IN pmlan_buffer pmbuf, IN mlan_status status)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmp;
+    pmlan_callbacks pcb;
+    pmlan_buffer pmbuf_parent;
+
+    ENTER();
+
+    MASSERT(pmadapter && pmbuf);
+    pcb = &pmadapter->callbacks;
+    MASSERT(pmbuf->bss_index < pmadapter->priv_num);
+
+    pmp = pmadapter->priv[pmbuf->bss_index];
+
+    if (pmbuf->pparent) {
+        pmbuf_parent = pmbuf->pparent;
+
+        pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                            pmp->rx_pkt_lock);
+        --pmbuf_parent->use_count;
+        if (!pmbuf_parent->use_count) {
+            pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                                  pmp->rx_pkt_lock);
+            wlan_free_mlan_buffer(pmadapter, pmbuf_parent);
+        } else {
+            pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                                  pmp->rx_pkt_lock);
+        }
+        wlan_free_mlan_buffer(pmadapter, pmbuf);
+    } else {
+        wlan_free_mlan_buffer(pmadapter, pmbuf);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Add packet to Bypass TX queue
+ *
+ *  @param pmadapter  Pointer to the mlan_adapter driver data struct
+ *  @param pmbuf      Pointer to the mlan_buffer data struct
+ *
+ *  @return         N/A
+ */
+t_void
+wlan_add_buf_bypass_txqueue(mlan_adapter * pmadapter, pmlan_buffer pmbuf)
+{
+    ENTER();
+
+    if (pmbuf->buf_type != MLAN_BUF_TYPE_RAW_DATA) {
+        pmbuf->buf_type = MLAN_BUF_TYPE_DATA;
+    }
+
+    util_enqueue_list_tail(pmadapter->pmoal_handle, &pmadapter->bypass_txq,
+                           (pmlan_linked_list) pmbuf,
+                           pmadapter->callbacks.moal_spin_lock,
+                           pmadapter->callbacks.moal_spin_unlock);
+    LEAVE();
+}
+
+/**
+ *  @brief Check if packets are available in Bypass TX queue
+ *
+ *  @param pmadapter  Pointer to the mlan_adapter driver data struct
+ *
+ *  @return         MFALSE if not empty; MTRUE if empty
+ */
+INLINE t_u8
+wlan_bypass_tx_list_empty(mlan_adapter * pmadapter)
+{
+    t_u8 q_empty = MTRUE;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    ENTER();
+
+    q_empty = (util_peek_list(pmadapter->pmoal_handle, &pmadapter->bypass_txq,
+                              pcb->moal_spin_lock,
+                              pcb->moal_spin_unlock)) ? MFALSE : MTRUE;
+
+    LEAVE();
+    return q_empty;
+}
+
+/**
+ *  @brief Clean up the By-pass TX queue
+ *
+ *  @param pmadapter Pointer to the mlan_adapter driver data struct
+ *
+ *  @return      N/A
+ */
+t_void
+wlan_cleanup_bypass_txq(mlan_adapter * pmadapter)
+{
+    pmlan_buffer pmbuf;
+    ENTER();
+
+    pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                        pmadapter->bypass_txq.plock);
+
+    while ((pmbuf =
+            (pmlan_buffer) util_peek_list(pmadapter->pmoal_handle,
+                                          &pmadapter->bypass_txq, MNULL,
+                                          MNULL))) {
+        util_unlink_list(pmadapter->pmoal_handle, &pmadapter->bypass_txq,
+                         (pmlan_linked_list) pmbuf, MNULL, MNULL);
+        wlan_write_data_complete(pmadapter, pmbuf, MLAN_STATUS_FAILURE);
+    }
+
+    pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                          pmadapter->bypass_txq.plock);
+
+    LEAVE();
+}
+
+/**
+ *  @brief Transmit the By-passed packet awaiting in by-pass queue
+ *
+ *  @param pmadapter Pointer to the mlan_adapter driver data struct
+ *
+ *  @return        N/A
+ */
+t_void
+wlan_process_bypass_tx(pmlan_adapter pmadapter)
+{
+    pmlan_buffer pmbuf;
+    mlan_tx_param tx_param;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    if ((pmbuf = (pmlan_buffer) util_dequeue_list(pmadapter->pmoal_handle,
+                                                  &pmadapter->bypass_txq,
+                                                  pmadapter->callbacks.
+                                                  moal_spin_lock,
+                                                  pmadapter->callbacks.
+                                                  moal_spin_unlock))) {
+        PRINTM(MINFO, "Dequeuing bypassed packet %p\n", pmbuf);
+        /* XXX: nex_pkt_len ??? */
+        tx_param.next_pkt_len = 0;
+        status =
+            wlan_process_tx(pmadapter->priv[pmbuf->bss_index], pmbuf,
+                            &tx_param);
+
+        if (status == MLAN_STATUS_RESOURCE) {
+            /* Queue the packet again so that it will be TX'ed later */
+            util_enqueue_list_head(pmadapter->pmoal_handle,
+                                   &pmadapter->bypass_txq,
+                                   (pmlan_linked_list) pmbuf,
+                                   pmadapter->callbacks.moal_spin_lock,
+                                   pmadapter->callbacks.moal_spin_unlock);
+        }
+    } else {
+        PRINTM(MINFO, "Nothing to send\n");
+    }
+
+    LEAVE();
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_uap.h b/drivers/net/wireless/sd8797/mlan/mlan_uap.h
new file mode 100644
index 000000000000..c2d2800f1a2d
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_uap.h
@@ -0,0 +1,104 @@
+/** @file mlan_uap.h
+ *
+ *  @brief This file contains related macros, enum, and struct
+ *  of uap functionalities
+ *
+ *  Copyright (C) 2009-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International 
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+ *  (the "License").  You may use, redistribute and/or modify this File in 
+ *  accordance with the terms and conditions of the License, a copy of which 
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+ *  this warranty disclaimer.
+ */
+
+/********************************************************
+Change log:
+    02/05/2009: initial version
+********************************************************/
+
+#ifndef _MLAN_UAP_H_
+#define _MLAN_UAP_H_
+
+#ifdef BIG_ENDIAN_SUPPORT
+/** Convert TxPD to little endian format from CPU format */
+#define uap_endian_convert_TxPD(x)                                          \
+    {                                                                   \
+        (x)->tx_pkt_length = wlan_cpu_to_le16((x)->tx_pkt_length);      \
+        (x)->tx_pkt_offset = wlan_cpu_to_le16((x)->tx_pkt_offset);      \
+        (x)->tx_pkt_type = wlan_cpu_to_le16((x)->tx_pkt_type);      \
+        (x)->tx_control = wlan_cpu_to_le32((x)->tx_control);      \
+    }
+
+/** Convert RxPD from little endian format to CPU format */
+#define uap_endian_convert_RxPD(x)                                          \
+    {                                                                   \
+        (x)->rx_pkt_length = wlan_le16_to_cpu((x)->rx_pkt_length);      \
+        (x)->rx_pkt_offset = wlan_le16_to_cpu((x)->rx_pkt_offset);      \
+        (x)->rx_pkt_type = wlan_le16_to_cpu((x)->rx_pkt_type);      \
+        (x)->seq_num = wlan_le16_to_cpu((x)->seq_num);              \
+    }
+#else
+/** Convert TxPD to little endian format from CPU format */
+#define uap_endian_convert_TxPD(x)  do {} while (0)
+/** Convert RxPD from little endian format to CPU format */
+#define uap_endian_convert_RxPD(x)  do {} while (0)
+#endif /* BIG_ENDIAN_SUPPORT */
+
+/** Band config 5GHz */
+#define UAP_BAND_CONFIG_5GHZ  0x01
+
+/** Packet forwarding to be done by FW or host */
+#define PKT_FWD_FW_BIT  0x01
+/** Intra-BSS broadcast packet forwarding allow bit */
+#define PKT_FWD_INTRA_BCAST 0x02
+/** Intra-BSS unicast packet forwarding allow bit */
+#define PKT_FWD_INTRA_UCAST 0x04
+/** Inter-BSS unicast packet forwarding allow bit */
+#define PKT_FWD_INTER_UCAST 0x08
+/** Intra-BSS unicast packet */
+#define PKT_INTRA_UCAST     0x01
+/** Inter-BSS unicast packet */
+#define PKT_INTER_UCAST     0x02
+/** Enable Host PKT forwarding */
+#define PKT_FWD_ENABLE_BIT  0x01
+
+mlan_status wlan_uap_get_channel(IN pmlan_private pmpriv);
+
+mlan_status wlan_uap_set_channel(IN pmlan_private pmpriv,
+                                 IN t_u8 uap_band_cfg, IN t_u8 channel);
+
+mlan_status wlan_uap_get_beacon_dtim(IN pmlan_private pmpriv);
+
+mlan_status wlan_ops_uap_ioctl(t_void * adapter, pmlan_ioctl_req pioctl_req);
+
+mlan_status wlan_ops_uap_prepare_cmd(IN t_void * priv,
+                                     IN t_u16 cmd_no,
+                                     IN t_u16 cmd_action,
+                                     IN t_u32 cmd_oid,
+                                     IN t_void * pioctl_buf,
+                                     IN t_void * pdata_buf,
+                                     IN t_void * pcmd_buf);
+
+mlan_status wlan_ops_uap_process_cmdresp(IN t_void * priv,
+                                         IN t_u16 cmdresp_no,
+                                         IN t_void * pcmd_buf,
+                                         IN t_void * pioctl);
+
+mlan_status wlan_ops_uap_process_rx_packet(IN t_void * adapter,
+                                           IN pmlan_buffer pmbuf);
+
+mlan_status wlan_ops_uap_process_event(IN t_void * priv);
+
+t_void *wlan_ops_uap_process_txpd(IN t_void * priv, IN pmlan_buffer pmbuf);
+
+mlan_status wlan_ops_uap_init_cmd(IN t_void * priv, IN t_u8 first_bss);
+
+#endif /* _MLAN_UAP_H_ */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_uap_cmdevent.c b/drivers/net/wireless/sd8797/mlan/mlan_uap_cmdevent.c
new file mode 100644
index 000000000000..7d7d5510433c
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_uap_cmdevent.c
@@ -0,0 +1,3021 @@
+/** @file mlan_uap_cmdevent.c
+ *
+ *  @brief This file contains the handling of AP mode command and event
+ * 
+ *  Copyright (C) 2009-2011, Marvell International Ltd. 
+ *   
+ *  This software file (the "File") is distributed by Marvell International 
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+ *  (the "License").  You may use, redistribute and/or modify this File in 
+ *  accordance with the terms and conditions of the License, a copy of which 
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+ *  this warranty disclaimer.
+ */
+
+/********************************************************
+Change log:
+    02/05/2009: initial version
+********************************************************/
+
+#include "mlan.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#ifdef STA_SUPPORT
+#include "mlan_join.h"
+#endif
+#include "mlan_main.h"
+#include "mlan_uap.h"
+#include "mlan_sdio.h"
+#include "mlan_11n.h"
+#include "mlan_11h.h"
+
+/********************************************************
+    Local Functions
+********************************************************/
+/**
+ *  @brief This function handles the command response error
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to command buffer
+ *
+ *  @return             N/A
+ */
+static void
+uap_process_cmdresp_error(mlan_private * pmpriv, HostCmd_DS_COMMAND * resp,
+                          mlan_ioctl_req * pioctl_buf)
+{
+    mlan_adapter *pmadapter = pmpriv->adapter;
+
+    ENTER();
+
+    PRINTM(MERROR, "CMD_RESP: cmd %#x error, result=%#x\n", resp->command,
+           resp->result);
+    if (pioctl_buf)
+        pioctl_buf->status_code = resp->result;
+    /*
+     * Handling errors here
+     */
+    wlan_insert_cmd_to_free_q(pmadapter, pmadapter->curr_cmd);
+
+    wlan_request_cmd_lock(pmadapter);
+    pmadapter->curr_cmd = MNULL;
+    wlan_release_cmd_lock(pmadapter);
+
+    LEAVE();
+    return;
+}
+
+/**
+ * @brief This function prepares command of hs_cfg.
+ *
+ * @param pmpriv       A pointer to mlan_private structure
+ * @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ * @param cmd_action   The action: GET or SET
+ * @param pdata_buf    A pointer to data buffer
+ *
+ * @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_uap_cmd_802_11_hs_cfg(IN pmlan_private pmpriv,
+                           IN HostCmd_DS_COMMAND * cmd,
+                           IN t_u16 cmd_action, IN hs_config_param * pdata_buf)
+{
+    HostCmd_DS_802_11_HS_CFG_ENH *phs_cfg = &cmd->params.opt_hs_cfg;
+
+    ENTER();
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_HS_CFG_ENH);
+    cmd->size =
+        wlan_cpu_to_le16(S_DS_GEN + sizeof(HostCmd_DS_802_11_HS_CFG_ENH));
+
+    if (pdata_buf == MNULL) {
+        phs_cfg->action = wlan_cpu_to_le16(HS_ACTIVATE);
+        phs_cfg->params.hs_activate.resp_ctrl = wlan_cpu_to_le16(RESP_NEEDED);
+    } else {
+        phs_cfg->action = wlan_cpu_to_le16(HS_CONFIGURE);
+        phs_cfg->params.hs_config.conditions =
+            wlan_cpu_to_le32(pdata_buf->conditions);
+        phs_cfg->params.hs_config.gpio = pdata_buf->gpio;
+        phs_cfg->params.hs_config.gap = pdata_buf->gap;
+        PRINTM(MCMND, "HS_CFG_CMD: condition:0x%x gpio:0x%x gap:0x%x\n",
+               phs_cfg->params.hs_config.conditions,
+               phs_cfg->params.hs_config.gpio, phs_cfg->params.hs_config.gap);
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of Tx data pause
+ *
+ *  @param pmpriv		A pointer to mlan_private structure
+ *  @param cmd	   		A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   the action: GET or SET
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return         MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_uap_cmd_txdatapause(pmlan_private pmpriv,
+                         IN HostCmd_DS_COMMAND * cmd,
+                         IN t_u16 cmd_action, IN t_void * pdata_buf)
+{
+    HostCmd_DS_CMD_TX_DATA_PAUSE *pause_cmd =
+        (HostCmd_DS_CMD_TX_DATA_PAUSE *) & cmd->params.tx_data_pause;
+    mlan_ds_misc_tx_datapause *data_pause =
+        (mlan_ds_misc_tx_datapause *) pdata_buf;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_CFG_TX_DATA_PAUSE);
+    cmd->size =
+        wlan_cpu_to_le16(sizeof(HostCmd_DS_CMD_TX_DATA_PAUSE) + S_DS_GEN);
+    pause_cmd->action = wlan_cpu_to_le16(cmd_action);
+
+    if (cmd_action == HostCmd_ACT_GEN_SET) {
+        pause_cmd->enable_tx_pause = (t_u8) data_pause->tx_pause;
+        pause_cmd->pause_tx_count = (t_u8) data_pause->tx_buf_cnt;
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of Tx data pause
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_uap_ret_txdatapause(IN pmlan_private pmpriv,
+                         IN HostCmd_DS_COMMAND * resp,
+                         IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_CMD_TX_DATA_PAUSE *pause_cmd =
+        (HostCmd_DS_CMD_TX_DATA_PAUSE *) & resp->params.tx_data_pause;
+    mlan_ds_misc_cfg *misc_cfg = MNULL;
+
+    ENTER();
+
+    if (pioctl_buf) {
+        misc_cfg = (mlan_ds_misc_cfg *) pioctl_buf->pbuf;
+        misc_cfg->param.tx_datapause.tx_pause = pause_cmd->enable_tx_pause;
+        misc_cfg->param.tx_datapause.tx_buf_cnt = pause_cmd->pause_tx_count;
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function will process tx pause event
+ *
+ *
+ *  @param priv    A pointer to mlan_private
+ *  @param pevent  A pointer to event buf
+ *
+ *  @return	       N/A
+ */
+static void
+wlan_process_tx_pause_event(pmlan_private priv, pmlan_buffer pevent)
+{
+    t_u16 tlv_type, tlv_len;
+    int tlv_buf_left = pevent->data_len - sizeof(t_u32);
+    MrvlIEtypesHeader_t *tlv =
+        (MrvlIEtypesHeader_t *) (pevent->pbuf + pevent->data_offset +
+                                 sizeof(t_u32));
+    MrvlIEtypes_tx_pause_t *tx_pause_tlv;
+    sta_node *sta_ptr = MNULL;
+
+    ENTER();
+
+    while (tlv_buf_left >= (int) sizeof(MrvlIEtypesHeader_t)) {
+        tlv_type = wlan_le16_to_cpu(tlv->type);
+        tlv_len = wlan_le16_to_cpu(tlv->len);
+        if ((sizeof(MrvlIEtypesHeader_t) + tlv_len) >
+            (unsigned int) tlv_buf_left) {
+            PRINTM(MERROR, "wrong tlv: tlvLen=%d, tlvBufLeft=%d\n", tlv_len,
+                   tlv_buf_left);
+            break;
+        }
+        if (tlv_type == TLV_TYPE_TX_PAUSE) {
+            tx_pause_tlv = (MrvlIEtypes_tx_pause_t *) tlv;
+            PRINTM(MCMND,
+                   "TxPause: %02x:%02x:%02x:%02x:%02x:%02x pause=%d, pkts=%d\n",
+                   tx_pause_tlv->peermac[0], tx_pause_tlv->peermac[1],
+                   tx_pause_tlv->peermac[2], tx_pause_tlv->peermac[3],
+                   tx_pause_tlv->peermac[4], tx_pause_tlv->peermac[5],
+                   tx_pause_tlv->tx_pause, tx_pause_tlv->pkt_cnt);
+            if ((sta_ptr = wlan_get_station_entry(priv, tx_pause_tlv->peermac))) {
+                if (sta_ptr->tx_pause != tx_pause_tlv->tx_pause) {
+                    sta_ptr->tx_pause = tx_pause_tlv->tx_pause;
+                    wlan_updata_ralist_tx_pause(priv, tx_pause_tlv->peermac,
+                                                tx_pause_tlv->tx_pause);
+                }
+            }
+        }
+        tlv_buf_left -= (sizeof(MrvlIEtypesHeader_t) + tlv_len);
+        tlv =
+            (MrvlIEtypesHeader_t *) ((t_u8 *) tlv + tlv_len +
+                                     sizeof(MrvlIEtypesHeader_t));
+    }
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This function prepares command for config uap settings
+ *
+ *  @param pmpriv		A pointer to mlan_private structure
+ *  @param cmd	   		A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   the action: GET or SET
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *  @return         MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_uap_cmd_ap_config(pmlan_private pmpriv,
+                       IN HostCmd_DS_COMMAND * cmd,
+                       IN t_u16 cmd_action, IN pmlan_ioctl_req pioctl_buf)
+{
+    mlan_ds_bss *bss = MNULL;
+    HostCmd_DS_SYS_CONFIG *sys_config =
+        (HostCmd_DS_SYS_CONFIG *) & cmd->params.sys_config;
+    t_u8 *tlv = MNULL;
+    MrvlIEtypes_MacAddr_t *tlv_mac = MNULL;
+    MrvlIEtypes_SsIdParamSet_t *tlv_ssid = MNULL;
+    MrvlIEtypes_beacon_period_t *tlv_beacon_period = MNULL;
+    MrvlIEtypes_dtim_period_t *tlv_dtim_period = MNULL;
+    MrvlIEtypes_RatesParamSet_t *tlv_rates = MNULL;
+    MrvlIEtypes_tx_rate_t *tlv_txrate = MNULL;
+    MrvlIEtypes_mcbc_rate_t *tlv_mcbc_rate = MNULL;
+    MrvlIEtypes_tx_power_t *tlv_tx_power = MNULL;
+    MrvlIEtypes_bcast_ssid_t *tlv_bcast_ssid = MNULL;
+    MrvlIEtypes_antenna_mode_t *tlv_antenna = MNULL;
+    MrvlIEtypes_pkt_forward_t *tlv_pkt_forward = MNULL;
+    MrvlIEtypes_max_sta_count_t *tlv_sta_count = MNULL;
+    MrvlIEtypes_sta_ageout_t *tlv_sta_ageout = MNULL;
+    MrvlIEtypes_ps_sta_ageout_t *tlv_ps_sta_ageout = MNULL;
+    MrvlIEtypes_rts_threshold_t *tlv_rts_threshold = MNULL;
+    MrvlIEtypes_frag_threshold_t *tlv_frag_threshold = MNULL;
+    MrvlIEtypes_retry_limit_t *tlv_retry_limit = MNULL;
+    MrvlIEtypes_eapol_pwk_hsk_timeout_t *tlv_pairwise_timeout = MNULL;
+    MrvlIEtypes_eapol_pwk_hsk_retries_t *tlv_pairwise_retries = MNULL;
+    MrvlIEtypes_eapol_gwk_hsk_timeout_t *tlv_groupwise_timeout = MNULL;
+    MrvlIEtypes_eapol_gwk_hsk_retries_t *tlv_groupwise_retries = MNULL;
+    MrvlIEtypes_mgmt_ie_passthru_t *tlv_mgmt_ie_passthru = MNULL;
+    MrvlIEtypes_2040_coex_enable_t *tlv_2040_coex_enable = MNULL;
+    MrvlIEtypes_mac_filter_t *tlv_mac_filter = MNULL;
+    MrvlIEtypes_channel_band_t *tlv_chan_band = MNULL;
+    MrvlIEtypes_ChanListParamSet_t *tlv_chan_list = MNULL;
+    ChanScanParamSet_t *pscan_chan = MNULL;
+    MrvlIEtypes_auth_type_t *tlv_auth_type = MNULL;
+    MrvlIEtypes_encrypt_protocol_t *tlv_encrypt_protocol = MNULL;
+    MrvlIEtypes_akmp_t *tlv_akmp = MNULL;
+    MrvlIEtypes_pwk_cipher_t *tlv_pwk_cipher = MNULL;
+    MrvlIEtypes_gwk_cipher_t *tlv_gwk_cipher = MNULL;
+    MrvlIEtypes_rsn_replay_prot_t *tlv_rsn_prot = MNULL;
+    MrvlIEtypes_passphrase_t *tlv_passphrase = MNULL;
+    MrvlIEtypes_group_rekey_time_t *tlv_rekey_time = MNULL;
+    MrvlIEtypes_wep_key_t *tlv_wep_key = MNULL;
+    MrvlIETypes_HTCap_t *tlv_htcap = MNULL;
+    MrvlIEtypes_wmm_parameter_t *tlv_wmm_parameter = MNULL;
+    t_u32 cmd_size = 0;
+    t_u8 zero_mac[] = { 0, 0, 0, 0, 0, 0 };
+    t_u16 i;
+    t_u16 ac;
+
+    ENTER();
+    if (pioctl_buf == MNULL) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    bss = (mlan_ds_bss *) pioctl_buf->pbuf;
+
+    cmd->command = wlan_cpu_to_le16(HOST_CMD_APCMD_SYS_CONFIGURE);
+    sys_config->action = wlan_cpu_to_le16(cmd_action);
+    cmd_size = sizeof(HostCmd_DS_SYS_CONFIG) - 1 + S_DS_GEN;
+
+    tlv = (t_u8 *) sys_config->tlv_buffer;
+    if (memcmp
+        (pmpriv->adapter, zero_mac, &bss->param.bss_config.mac_addr,
+         MLAN_MAC_ADDR_LENGTH)) {
+        tlv_mac = (MrvlIEtypes_MacAddr_t *) tlv;
+        tlv_mac->header.type = wlan_cpu_to_le16(TLV_TYPE_UAP_MAC_ADDRESS);
+        tlv_mac->header.len = wlan_cpu_to_le16(MLAN_MAC_ADDR_LENGTH);
+        memcpy(pmpriv->adapter, tlv_mac->mac, &bss->param.bss_config.mac_addr,
+               MLAN_MAC_ADDR_LENGTH);
+        cmd_size += sizeof(MrvlIEtypes_MacAddr_t);
+        tlv += sizeof(MrvlIEtypes_MacAddr_t);
+    }
+
+    if (bss->param.bss_config.ssid.ssid_len) {
+        tlv_ssid = (MrvlIEtypes_SsIdParamSet_t *) tlv;
+        tlv_ssid->header.type = wlan_cpu_to_le16(TLV_TYPE_SSID);
+        tlv_ssid->header.len =
+            wlan_cpu_to_le16((t_u16) bss->param.bss_config.ssid.ssid_len);
+        memcpy(pmpriv->adapter, tlv_ssid->ssid, bss->param.bss_config.ssid.ssid,
+               bss->param.bss_config.ssid.ssid_len);
+        cmd_size +=
+            sizeof(MrvlIEtypesHeader_t) + bss->param.bss_config.ssid.ssid_len;
+        tlv +=
+            sizeof(MrvlIEtypesHeader_t) + bss->param.bss_config.ssid.ssid_len;
+    }
+
+    if ((bss->param.bss_config.beacon_period >= MIN_BEACON_PERIOD) &&
+        (bss->param.bss_config.beacon_period <= MAX_BEACON_PERIOD)) {
+        tlv_beacon_period = (MrvlIEtypes_beacon_period_t *) tlv;
+        tlv_beacon_period->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_BEACON_PERIOD);
+        tlv_beacon_period->header.len = wlan_cpu_to_le16(sizeof(t_u16));
+        tlv_beacon_period->beacon_period =
+            wlan_cpu_to_le16(bss->param.bss_config.beacon_period);
+        cmd_size += sizeof(MrvlIEtypes_beacon_period_t);
+        tlv += sizeof(MrvlIEtypes_beacon_period_t);
+    }
+
+    if ((bss->param.bss_config.dtim_period >= MIN_DTIM_PERIOD) &&
+        (bss->param.bss_config.dtim_period <= MAX_DTIM_PERIOD)) {
+        tlv_dtim_period = (MrvlIEtypes_dtim_period_t *) tlv;
+        tlv_dtim_period->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_DTIM_PERIOD);
+        tlv_dtim_period->header.len = wlan_cpu_to_le16(sizeof(t_u8));
+        tlv_dtim_period->dtim_period = bss->param.bss_config.dtim_period;
+        cmd_size += sizeof(MrvlIEtypes_dtim_period_t);
+        tlv += sizeof(MrvlIEtypes_dtim_period_t);
+    }
+
+    if (bss->param.bss_config.rates[0]) {
+        tlv_rates = (MrvlIEtypes_RatesParamSet_t *) tlv;
+        tlv_rates->header.type = wlan_cpu_to_le16(TLV_TYPE_RATES);
+        for (i = 0; i < MAX_DATA_RATES && bss->param.bss_config.rates[i]; i++) {
+            tlv_rates->rates[i] = bss->param.bss_config.rates[i];
+        }
+        tlv_rates->header.len = wlan_cpu_to_le16(i);
+        cmd_size += sizeof(MrvlIEtypesHeader_t) + i;
+        tlv += sizeof(MrvlIEtypesHeader_t) + i;
+    }
+
+    if (bss->param.bss_config.tx_data_rate <= DATA_RATE_54M) {
+        tlv_txrate = (MrvlIEtypes_tx_rate_t *) tlv;
+        tlv_txrate->header.type = wlan_cpu_to_le16(TLV_TYPE_UAP_TX_DATA_RATE);
+        tlv_txrate->header.len = wlan_cpu_to_le16(sizeof(t_u16));
+        tlv_txrate->tx_data_rate =
+            wlan_cpu_to_le16(bss->param.bss_config.tx_data_rate);
+        cmd_size += sizeof(MrvlIEtypes_tx_rate_t);
+        tlv += sizeof(MrvlIEtypes_tx_rate_t);
+    }
+
+    if (bss->param.bss_config.mcbc_data_rate <= DATA_RATE_54M) {
+        tlv_mcbc_rate = (MrvlIEtypes_mcbc_rate_t *) tlv;
+        tlv_mcbc_rate->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_MCBC_DATA_RATE);
+        tlv_mcbc_rate->header.len = wlan_cpu_to_le16(sizeof(t_u16));
+        tlv_mcbc_rate->mcbc_data_rate =
+            wlan_cpu_to_le16(bss->param.bss_config.mcbc_data_rate);
+        cmd_size += sizeof(MrvlIEtypes_mcbc_rate_t);
+        tlv += sizeof(MrvlIEtypes_mcbc_rate_t);
+    }
+
+    if (bss->param.bss_config.tx_power_level <= MAX_TX_POWER) {
+        tlv_tx_power = (MrvlIEtypes_tx_power_t *) tlv;
+        tlv_tx_power->header.type = wlan_cpu_to_le16(TLV_TYPE_UAP_TX_POWER);
+        tlv_tx_power->header.len = wlan_cpu_to_le16(sizeof(t_u8));
+        tlv_tx_power->tx_power = bss->param.bss_config.tx_power_level;
+        cmd_size += sizeof(MrvlIEtypes_tx_power_t);
+        tlv += sizeof(MrvlIEtypes_tx_power_t);
+    }
+
+    if (bss->param.bss_config.bcast_ssid_ctl <= MTRUE) {
+        tlv_bcast_ssid = (MrvlIEtypes_bcast_ssid_t *) tlv;
+        tlv_bcast_ssid->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_BCAST_SSID_CTL);
+        tlv_bcast_ssid->header.len = wlan_cpu_to_le16(sizeof(t_u8));
+        tlv_bcast_ssid->bcast_ssid_ctl = bss->param.bss_config.bcast_ssid_ctl;
+        cmd_size += sizeof(MrvlIEtypes_bcast_ssid_t);
+        tlv += sizeof(MrvlIEtypes_bcast_ssid_t);
+    }
+
+    if ((bss->param.bss_config.tx_antenna == ANTENNA_MODE_A) ||
+        (bss->param.bss_config.tx_antenna == ANTENNA_MODE_B)) {
+        tlv_antenna = (MrvlIEtypes_antenna_mode_t *) tlv;
+        tlv_antenna->header.type = wlan_cpu_to_le16(TLV_TYPE_UAP_ANTENNA_CTL);
+        tlv_antenna->header.len = wlan_cpu_to_le16(sizeof(t_u8) + sizeof(t_u8));
+        tlv_antenna->which_antenna = TX_ANTENNA;
+        tlv_antenna->antenna_mode = bss->param.bss_config.tx_antenna;
+        cmd_size += sizeof(MrvlIEtypes_antenna_mode_t);
+        tlv += sizeof(MrvlIEtypes_antenna_mode_t);
+    }
+
+    if ((bss->param.bss_config.rx_antenna == ANTENNA_MODE_A) ||
+        (bss->param.bss_config.rx_antenna == ANTENNA_MODE_B)) {
+        tlv_antenna = (MrvlIEtypes_antenna_mode_t *) tlv;
+        tlv_antenna->header.type = wlan_cpu_to_le16(TLV_TYPE_UAP_ANTENNA_CTL);
+        tlv_antenna->header.len = wlan_cpu_to_le16(sizeof(t_u8) + sizeof(t_u8));
+        tlv_antenna->which_antenna = RX_ANTENNA;
+        tlv_antenna->antenna_mode = bss->param.bss_config.rx_antenna;
+        cmd_size += sizeof(MrvlIEtypes_antenna_mode_t);
+        tlv += sizeof(MrvlIEtypes_antenna_mode_t);
+    }
+
+    if (bss->param.bss_config.pkt_forward_ctl <= MAX_PKT_FWD_CTRL) {
+        tlv_pkt_forward = (MrvlIEtypes_pkt_forward_t *) tlv;
+        tlv_pkt_forward->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_PKT_FWD_CTL);
+        tlv_pkt_forward->header.len = wlan_cpu_to_le16(sizeof(t_u8));
+        tlv_pkt_forward->pkt_forward_ctl =
+            bss->param.bss_config.pkt_forward_ctl;
+        cmd_size += sizeof(MrvlIEtypes_pkt_forward_t);
+        tlv += sizeof(MrvlIEtypes_pkt_forward_t);
+    }
+
+    if (bss->param.bss_config.max_sta_count <= MAX_STA_COUNT) {
+        tlv_sta_count = (MrvlIEtypes_max_sta_count_t *) tlv;
+        tlv_sta_count->header.type = wlan_cpu_to_le16(TLV_TYPE_UAP_MAX_STA_CNT);
+        tlv_sta_count->header.len = wlan_cpu_to_le16(sizeof(t_u16));
+        tlv_sta_count->max_sta_count =
+            wlan_cpu_to_le16(bss->param.bss_config.max_sta_count);
+        cmd_size += sizeof(MrvlIEtypes_max_sta_count_t);
+        tlv += sizeof(MrvlIEtypes_max_sta_count_t);
+    }
+
+    if (((bss->param.bss_config.sta_ageout_timer >= MIN_STAGE_OUT_TIME) &&
+         (bss->param.bss_config.sta_ageout_timer <= MAX_STAGE_OUT_TIME)) ||
+        (bss->param.bss_config.sta_ageout_timer == 0)) {
+        tlv_sta_ageout = (MrvlIEtypes_sta_ageout_t *) tlv;
+        tlv_sta_ageout->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_STA_AGEOUT_TIMER);
+        tlv_sta_ageout->header.len = wlan_cpu_to_le16(sizeof(t_u32));
+        tlv_sta_ageout->sta_ageout_timer =
+            wlan_cpu_to_le32(bss->param.bss_config.sta_ageout_timer);
+        cmd_size += sizeof(MrvlIEtypes_sta_ageout_t);
+        tlv += sizeof(MrvlIEtypes_sta_ageout_t);
+    }
+
+    if (((bss->param.bss_config.ps_sta_ageout_timer >= MIN_STAGE_OUT_TIME) &&
+         (bss->param.bss_config.ps_sta_ageout_timer <= MAX_STAGE_OUT_TIME)) ||
+        (bss->param.bss_config.ps_sta_ageout_timer == 0)) {
+        tlv_ps_sta_ageout = (MrvlIEtypes_ps_sta_ageout_t *) tlv;
+        tlv_ps_sta_ageout->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_PS_STA_AGEOUT_TIMER);
+        tlv_ps_sta_ageout->header.len = wlan_cpu_to_le16(sizeof(t_u32));
+        tlv_ps_sta_ageout->ps_sta_ageout_timer =
+            wlan_cpu_to_le32(bss->param.bss_config.ps_sta_ageout_timer);
+        cmd_size += sizeof(MrvlIEtypes_ps_sta_ageout_t);
+        tlv += sizeof(MrvlIEtypes_ps_sta_ageout_t);
+    }
+    if (bss->param.bss_config.rts_threshold <= MAX_RTS_THRESHOLD) {
+        tlv_rts_threshold = (MrvlIEtypes_rts_threshold_t *) tlv;
+        tlv_rts_threshold->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_RTS_THRESHOLD);
+        tlv_rts_threshold->header.len = wlan_cpu_to_le16(sizeof(t_u16));
+        tlv_rts_threshold->rts_threshold =
+            wlan_cpu_to_le16(bss->param.bss_config.rts_threshold);
+        cmd_size += sizeof(MrvlIEtypes_rts_threshold_t);
+        tlv += sizeof(MrvlIEtypes_rts_threshold_t);
+    }
+
+    if ((bss->param.bss_config.frag_threshold >= MIN_FRAG_THRESHOLD) &&
+        (bss->param.bss_config.frag_threshold <= MAX_FRAG_THRESHOLD)) {
+        tlv_frag_threshold = (MrvlIEtypes_frag_threshold_t *) tlv;
+        tlv_frag_threshold->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_FRAG_THRESHOLD);
+        tlv_frag_threshold->header.len = wlan_cpu_to_le16(sizeof(t_u16));
+        tlv_frag_threshold->frag_threshold =
+            wlan_cpu_to_le16(bss->param.bss_config.frag_threshold);
+        cmd_size += sizeof(MrvlIEtypes_frag_threshold_t);
+        tlv += sizeof(MrvlIEtypes_frag_threshold_t);
+    }
+
+    if (bss->param.bss_config.retry_limit <= MAX_RETRY_LIMIT) {
+        tlv_retry_limit = (MrvlIEtypes_retry_limit_t *) tlv;
+        tlv_retry_limit->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_RETRY_LIMIT);
+        tlv_retry_limit->header.len = wlan_cpu_to_le16(sizeof(t_u8));
+        tlv_retry_limit->retry_limit = (t_u8) bss->param.bss_config.retry_limit;
+        cmd_size += sizeof(MrvlIEtypes_retry_limit_t);
+        tlv += sizeof(MrvlIEtypes_retry_limit_t);
+    }
+
+    if (bss->param.bss_config.pairwise_update_timeout < (MAX_VALID_DWORD)) {
+        tlv_pairwise_timeout = (MrvlIEtypes_eapol_pwk_hsk_timeout_t *) tlv;
+        tlv_pairwise_timeout->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_EAPOL_PWK_HSK_TIMEOUT);
+        tlv_pairwise_timeout->header.len = wlan_cpu_to_le16(sizeof(t_u32));
+        tlv_pairwise_timeout->pairwise_update_timeout =
+            wlan_cpu_to_le32(bss->param.bss_config.pairwise_update_timeout);
+        cmd_size += sizeof(MrvlIEtypes_eapol_pwk_hsk_timeout_t);
+        tlv += sizeof(MrvlIEtypes_eapol_pwk_hsk_timeout_t);
+    }
+
+    if (bss->param.bss_config.pwk_retries < (MAX_VALID_DWORD)) {
+        tlv_pairwise_retries = (MrvlIEtypes_eapol_pwk_hsk_retries_t *) tlv;
+        tlv_pairwise_retries->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_EAPOL_PWK_HSK_RETRIES);
+        tlv_pairwise_retries->header.len = wlan_cpu_to_le16(sizeof(t_u32));
+        tlv_pairwise_retries->pwk_retries =
+            wlan_cpu_to_le32(bss->param.bss_config.pwk_retries);
+        cmd_size += sizeof(MrvlIEtypes_eapol_pwk_hsk_retries_t);
+        tlv += sizeof(MrvlIEtypes_eapol_pwk_hsk_retries_t);
+    }
+
+    if (bss->param.bss_config.groupwise_update_timeout < (MAX_VALID_DWORD)) {
+        tlv_groupwise_timeout = (MrvlIEtypes_eapol_gwk_hsk_timeout_t *) tlv;
+        tlv_groupwise_timeout->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_EAPOL_GWK_HSK_TIMEOUT);
+        tlv_groupwise_timeout->header.len = wlan_cpu_to_le16(sizeof(t_u32));
+        tlv_groupwise_timeout->groupwise_update_timeout =
+            wlan_cpu_to_le32(bss->param.bss_config.groupwise_update_timeout);
+        cmd_size += sizeof(MrvlIEtypes_eapol_gwk_hsk_timeout_t);
+        tlv += sizeof(MrvlIEtypes_eapol_gwk_hsk_timeout_t);
+    }
+
+    if (bss->param.bss_config.gwk_retries < (MAX_VALID_DWORD)) {
+        tlv_groupwise_retries = (MrvlIEtypes_eapol_gwk_hsk_retries_t *) tlv;
+        tlv_groupwise_retries->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_EAPOL_GWK_HSK_RETRIES);
+        tlv_groupwise_retries->header.len = wlan_cpu_to_le16(sizeof(t_u32));
+        tlv_groupwise_retries->gwk_retries =
+            wlan_cpu_to_le32(bss->param.bss_config.gwk_retries);
+        cmd_size += sizeof(MrvlIEtypes_eapol_gwk_hsk_retries_t);
+        tlv += sizeof(MrvlIEtypes_eapol_gwk_hsk_retries_t);
+    }
+
+    if ((bss->param.bss_config.filter.filter_mode <= MAC_FILTER_MODE_BLOCK_MAC)
+        && (bss->param.bss_config.filter.mac_count <= MAX_MAC_FILTER_NUM)) {
+        tlv_mac_filter = (MrvlIEtypes_mac_filter_t *) tlv;
+        tlv_mac_filter->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_STA_MAC_ADDR_FILTER);
+        tlv_mac_filter->header.len =
+            wlan_cpu_to_le16(2 +
+                             MLAN_MAC_ADDR_LENGTH *
+                             bss->param.bss_config.filter.mac_count);
+        tlv_mac_filter->count = (t_u8) bss->param.bss_config.filter.mac_count;
+        tlv_mac_filter->filter_mode =
+            (t_u8) bss->param.bss_config.filter.filter_mode;
+        memcpy(pmpriv->adapter, tlv_mac_filter->mac_address,
+               (t_u8 *) bss->param.bss_config.filter.mac_list,
+               MLAN_MAC_ADDR_LENGTH * bss->param.bss_config.filter.mac_count);
+        cmd_size +=
+            sizeof(MrvlIEtypesHeader_t) + 2 +
+            MLAN_MAC_ADDR_LENGTH * bss->param.bss_config.filter.mac_count;
+        tlv +=
+            sizeof(MrvlIEtypesHeader_t) + 2 +
+            MLAN_MAC_ADDR_LENGTH * bss->param.bss_config.filter.mac_count;
+    }
+
+    if ((((bss->param.bss_config.band_cfg & BAND_CONFIG_ACS_MODE) ==
+          BAND_CONFIG_MANUAL) && (bss->param.bss_config.channel > 0) &&
+         (bss->param.bss_config.channel <= MLAN_MAX_CHANNEL)) ||
+        (bss->param.bss_config.band_cfg & BAND_CONFIG_ACS_MODE)) {
+        tlv_chan_band = (MrvlIEtypes_channel_band_t *) tlv;
+        tlv_chan_band->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_CHAN_BAND_CONFIG);
+        tlv_chan_band->header.len =
+            wlan_cpu_to_le16(sizeof(t_u8) + sizeof(t_u8));
+        tlv_chan_band->band_config = bss->param.bss_config.band_cfg;
+        tlv_chan_band->channel = bss->param.bss_config.channel;
+        cmd_size += sizeof(MrvlIEtypes_channel_band_t);
+        tlv += sizeof(MrvlIEtypes_channel_band_t);
+    }
+
+    if ((bss->param.bss_config.num_of_chan) &&
+        (bss->param.bss_config.num_of_chan <= MLAN_MAX_CHANNEL)) {
+        tlv_chan_list = (MrvlIEtypes_ChanListParamSet_t *) tlv;
+        tlv_chan_list->header.type = wlan_cpu_to_le16(TLV_TYPE_CHANLIST);
+        tlv_chan_list->header.len =
+            wlan_cpu_to_le16((t_u16)
+                             (sizeof(ChanScanParamSet_t) *
+                              bss->param.bss_config.num_of_chan));
+        pscan_chan = tlv_chan_list->chan_scan_param;
+        for (i = 0; i < bss->param.bss_config.num_of_chan; i++) {
+            pscan_chan->chan_number =
+                bss->param.bss_config.chan_list[i].chan_number;
+            pscan_chan->radio_type =
+                bss->param.bss_config.chan_list[i].band_config_type;
+            pscan_chan++;
+        }
+        cmd_size += sizeof(tlv_chan_list->header) +
+            (sizeof(ChanScanParamSet_t) * bss->param.bss_config.num_of_chan);
+        tlv += sizeof(tlv_chan_list->header) +
+            (sizeof(ChanScanParamSet_t) * bss->param.bss_config.num_of_chan);
+    }
+
+    if ((bss->param.bss_config.auth_mode <= MLAN_AUTH_MODE_SHARED) ||
+        (bss->param.bss_config.auth_mode == MLAN_AUTH_MODE_AUTO)) {
+        tlv_auth_type = (MrvlIEtypes_auth_type_t *) tlv;
+        tlv_auth_type->header.type = wlan_cpu_to_le16(TLV_TYPE_AUTH_TYPE);
+        tlv_auth_type->header.len = wlan_cpu_to_le16(sizeof(t_u8));
+        tlv_auth_type->auth_type = (t_u8) bss->param.bss_config.auth_mode;
+        cmd_size += sizeof(MrvlIEtypes_auth_type_t);
+        tlv += sizeof(MrvlIEtypes_auth_type_t);
+    }
+
+    if (bss->param.bss_config.protocol) {
+        tlv_encrypt_protocol = (MrvlIEtypes_encrypt_protocol_t *) tlv;
+        tlv_encrypt_protocol->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_ENCRYPT_PROTOCOL);
+        tlv_encrypt_protocol->header.len = wlan_cpu_to_le16(sizeof(t_u16));
+        tlv_encrypt_protocol->protocol =
+            wlan_cpu_to_le16(bss->param.bss_config.protocol);
+        cmd_size += sizeof(MrvlIEtypes_encrypt_protocol_t);
+        tlv += sizeof(MrvlIEtypes_encrypt_protocol_t);
+    }
+
+    if ((bss->param.bss_config.protocol & PROTOCOL_WPA) ||
+        (bss->param.bss_config.protocol & PROTOCOL_WPA2) ||
+        (bss->param.bss_config.protocol & PROTOCOL_EAP)) {
+        tlv_akmp = (MrvlIEtypes_akmp_t *) tlv;
+        tlv_akmp->header.type = wlan_cpu_to_le16(TLV_TYPE_UAP_AKMP);
+        tlv_akmp->key_mgmt = wlan_cpu_to_le16(bss->param.bss_config.key_mgmt);
+        tlv_akmp->header.len = sizeof(t_u16);
+        tlv_akmp->key_mgmt_operation =
+            wlan_cpu_to_le16(bss->param.bss_config.key_mgmt_operation);
+        tlv_akmp->header.len += sizeof(t_u16);
+        tlv_akmp->header.len = wlan_cpu_to_le16(tlv_akmp->header.len);
+        cmd_size += sizeof(MrvlIEtypes_akmp_t);
+        tlv += sizeof(MrvlIEtypes_akmp_t);
+
+        if (bss->param.bss_config.wpa_cfg.
+            pairwise_cipher_wpa & VALID_CIPHER_BITMAP) {
+            tlv_pwk_cipher = (MrvlIEtypes_pwk_cipher_t *) tlv;
+            tlv_pwk_cipher->header.type = wlan_cpu_to_le16(TLV_TYPE_PWK_CIPHER);
+            tlv_pwk_cipher->header.len =
+                wlan_cpu_to_le16(sizeof(t_u16) + sizeof(t_u8) + sizeof(t_u8));
+            tlv_pwk_cipher->protocol = wlan_cpu_to_le16(PROTOCOL_WPA);
+            tlv_pwk_cipher->pairwise_cipher =
+                bss->param.bss_config.wpa_cfg.pairwise_cipher_wpa;
+            cmd_size += sizeof(MrvlIEtypes_pwk_cipher_t);
+            tlv += sizeof(MrvlIEtypes_pwk_cipher_t);
+        }
+
+        if (bss->param.bss_config.wpa_cfg.
+            pairwise_cipher_wpa2 & VALID_CIPHER_BITMAP) {
+            tlv_pwk_cipher = (MrvlIEtypes_pwk_cipher_t *) tlv;
+            tlv_pwk_cipher->header.type = wlan_cpu_to_le16(TLV_TYPE_PWK_CIPHER);
+            tlv_pwk_cipher->header.len =
+                wlan_cpu_to_le16(sizeof(t_u16) + sizeof(t_u8) + sizeof(t_u8));
+            tlv_pwk_cipher->protocol = wlan_cpu_to_le16(PROTOCOL_WPA2);
+            tlv_pwk_cipher->pairwise_cipher =
+                bss->param.bss_config.wpa_cfg.pairwise_cipher_wpa2;
+            cmd_size += sizeof(MrvlIEtypes_pwk_cipher_t);
+            tlv += sizeof(MrvlIEtypes_pwk_cipher_t);
+        }
+
+        if (bss->param.bss_config.wpa_cfg.group_cipher & VALID_CIPHER_BITMAP) {
+            tlv_gwk_cipher = (MrvlIEtypes_gwk_cipher_t *) tlv;
+            tlv_gwk_cipher->header.type = wlan_cpu_to_le16(TLV_TYPE_GWK_CIPHER);
+            tlv_gwk_cipher->header.len =
+                wlan_cpu_to_le16(sizeof(t_u8) + sizeof(t_u8));
+            tlv_gwk_cipher->group_cipher =
+                bss->param.bss_config.wpa_cfg.group_cipher;
+            cmd_size += sizeof(MrvlIEtypes_gwk_cipher_t);
+            tlv += sizeof(MrvlIEtypes_gwk_cipher_t);
+        }
+
+        if (bss->param.bss_config.wpa_cfg.rsn_protection <= MTRUE) {
+            tlv_rsn_prot = (MrvlIEtypes_rsn_replay_prot_t *) tlv;
+            tlv_rsn_prot->header.type =
+                wlan_cpu_to_le16(TLV_TYPE_UAP_RSN_REPLAY_PROTECT);
+            tlv_rsn_prot->header.len = wlan_cpu_to_le16(sizeof(t_u8));
+            tlv_rsn_prot->rsn_replay_prot =
+                bss->param.bss_config.wpa_cfg.rsn_protection;
+            cmd_size += sizeof(MrvlIEtypes_rsn_replay_prot_t);
+            tlv += sizeof(MrvlIEtypes_rsn_replay_prot_t);
+        }
+
+        if (bss->param.bss_config.wpa_cfg.length) {
+            tlv_passphrase = (MrvlIEtypes_passphrase_t *) tlv;
+            tlv_passphrase->header.type =
+                wlan_cpu_to_le16(TLV_TYPE_UAP_WPA_PASSPHRASE);
+            tlv_passphrase->header.len =
+                (t_u16) wlan_cpu_to_le16(bss->param.bss_config.wpa_cfg.length);
+            memcpy(pmpriv->adapter, tlv_passphrase->passphrase,
+                   bss->param.bss_config.wpa_cfg.passphrase,
+                   bss->param.bss_config.wpa_cfg.length);
+            cmd_size +=
+                sizeof(MrvlIEtypesHeader_t) +
+                bss->param.bss_config.wpa_cfg.length;
+            tlv +=
+                sizeof(MrvlIEtypesHeader_t) +
+                bss->param.bss_config.wpa_cfg.length;
+        }
+
+        if (bss->param.bss_config.wpa_cfg.gk_rekey_time < MAX_GRP_TIMER) {
+            tlv_rekey_time = (MrvlIEtypes_group_rekey_time_t *) tlv;
+            tlv_rekey_time->header.type =
+                wlan_cpu_to_le16(TLV_TYPE_UAP_GRP_REKEY_TIME);
+            tlv_rekey_time->header.len = wlan_cpu_to_le16(sizeof(t_u32));
+            tlv_rekey_time->gk_rekey_time =
+                wlan_cpu_to_le32(bss->param.bss_config.wpa_cfg.gk_rekey_time);
+            cmd_size += sizeof(MrvlIEtypes_group_rekey_time_t);
+            tlv += sizeof(MrvlIEtypes_group_rekey_time_t);
+        }
+    } else {
+        if ((bss->param.bss_config.wep_cfg.key0.length) &&
+            ((bss->param.bss_config.wep_cfg.key0.length == 5) ||
+             (bss->param.bss_config.wep_cfg.key0.length == 10) ||
+             (bss->param.bss_config.wep_cfg.key0.length == 13) ||
+             (bss->param.bss_config.wep_cfg.key0.length == 26))) {
+            tlv_wep_key = (MrvlIEtypes_wep_key_t *) tlv;
+            tlv_wep_key->header.type = wlan_cpu_to_le16(TLV_TYPE_UAP_WEP_KEY);
+            tlv_wep_key->header.len =
+                wlan_cpu_to_le16(2 + bss->param.bss_config.wep_cfg.key0.length);
+            tlv_wep_key->key_index =
+                bss->param.bss_config.wep_cfg.key0.key_index;
+            tlv_wep_key->is_default =
+                bss->param.bss_config.wep_cfg.key0.is_default;
+            memcpy(pmpriv->adapter, tlv_wep_key->key,
+                   bss->param.bss_config.wep_cfg.key0.key,
+                   bss->param.bss_config.wep_cfg.key0.length);
+            cmd_size +=
+                sizeof(MrvlIEtypesHeader_t) + 2 +
+                bss->param.bss_config.wep_cfg.key0.length;
+            tlv +=
+                sizeof(MrvlIEtypesHeader_t) + 2 +
+                bss->param.bss_config.wep_cfg.key0.length;
+        }
+
+        if ((bss->param.bss_config.wep_cfg.key1.length) &&
+            ((bss->param.bss_config.wep_cfg.key1.length == 5) ||
+             (bss->param.bss_config.wep_cfg.key1.length == 10) ||
+             (bss->param.bss_config.wep_cfg.key1.length == 13) ||
+             (bss->param.bss_config.wep_cfg.key1.length == 26))) {
+            tlv_wep_key = (MrvlIEtypes_wep_key_t *) tlv;
+            tlv_wep_key->header.type = wlan_cpu_to_le16(TLV_TYPE_UAP_WEP_KEY);
+            tlv_wep_key->header.len =
+                wlan_cpu_to_le16(2 + bss->param.bss_config.wep_cfg.key1.length);
+            tlv_wep_key->key_index =
+                bss->param.bss_config.wep_cfg.key1.key_index;
+            tlv_wep_key->is_default =
+                bss->param.bss_config.wep_cfg.key1.is_default;
+            memcpy(pmpriv->adapter, tlv_wep_key->key,
+                   bss->param.bss_config.wep_cfg.key1.key,
+                   bss->param.bss_config.wep_cfg.key1.length);
+            cmd_size +=
+                sizeof(MrvlIEtypesHeader_t) + 2 +
+                bss->param.bss_config.wep_cfg.key1.length;
+            tlv +=
+                sizeof(MrvlIEtypesHeader_t) + 2 +
+                bss->param.bss_config.wep_cfg.key1.length;
+        }
+
+        if ((bss->param.bss_config.wep_cfg.key2.length) &&
+            ((bss->param.bss_config.wep_cfg.key2.length == 5) ||
+             (bss->param.bss_config.wep_cfg.key2.length == 10) ||
+             (bss->param.bss_config.wep_cfg.key2.length == 13) ||
+             (bss->param.bss_config.wep_cfg.key2.length == 26))) {
+            tlv_wep_key = (MrvlIEtypes_wep_key_t *) tlv;
+            tlv_wep_key->header.type = wlan_cpu_to_le16(TLV_TYPE_UAP_WEP_KEY);
+            tlv_wep_key->header.len =
+                wlan_cpu_to_le16(2 + bss->param.bss_config.wep_cfg.key2.length);
+            tlv_wep_key->key_index =
+                bss->param.bss_config.wep_cfg.key2.key_index;
+            tlv_wep_key->is_default =
+                bss->param.bss_config.wep_cfg.key2.is_default;
+            memcpy(pmpriv->adapter, tlv_wep_key->key,
+                   bss->param.bss_config.wep_cfg.key2.key,
+                   bss->param.bss_config.wep_cfg.key2.length);
+            cmd_size +=
+                sizeof(MrvlIEtypesHeader_t) + 2 +
+                bss->param.bss_config.wep_cfg.key2.length;
+            tlv +=
+                sizeof(MrvlIEtypesHeader_t) + 2 +
+                bss->param.bss_config.wep_cfg.key2.length;
+        }
+
+        if ((bss->param.bss_config.wep_cfg.key3.length) &&
+            ((bss->param.bss_config.wep_cfg.key3.length == 5) ||
+             (bss->param.bss_config.wep_cfg.key3.length == 10) ||
+             (bss->param.bss_config.wep_cfg.key3.length == 13) ||
+             (bss->param.bss_config.wep_cfg.key3.length == 26))) {
+            tlv_wep_key = (MrvlIEtypes_wep_key_t *) tlv;
+            tlv_wep_key->header.type = wlan_cpu_to_le16(TLV_TYPE_UAP_WEP_KEY);
+            tlv_wep_key->header.len =
+                wlan_cpu_to_le16(2 + bss->param.bss_config.wep_cfg.key3.length);
+            tlv_wep_key->key_index =
+                bss->param.bss_config.wep_cfg.key3.key_index;
+            tlv_wep_key->is_default =
+                bss->param.bss_config.wep_cfg.key3.is_default;
+            memcpy(pmpriv->adapter, tlv_wep_key->key,
+                   bss->param.bss_config.wep_cfg.key3.key,
+                   bss->param.bss_config.wep_cfg.key3.length);
+            cmd_size +=
+                sizeof(MrvlIEtypesHeader_t) + 2 +
+                bss->param.bss_config.wep_cfg.key3.length;
+            tlv +=
+                sizeof(MrvlIEtypesHeader_t) + 2 +
+                bss->param.bss_config.wep_cfg.key3.length;
+        }
+    }
+    if ((bss->param.bss_config.ht_cap_info)
+        ) {
+        tlv_htcap = (MrvlIETypes_HTCap_t *) tlv;
+        tlv_htcap->header.type = wlan_cpu_to_le16(HT_CAPABILITY);
+        tlv_htcap->header.len = wlan_cpu_to_le16(sizeof(HTCap_t));
+        tlv_htcap->ht_cap.ht_cap_info =
+            wlan_cpu_to_le16(bss->param.bss_config.ht_cap_info);
+        tlv_htcap->ht_cap.ampdu_param = bss->param.bss_config.ampdu_param;
+        memcpy(pmpriv->adapter, tlv_htcap->ht_cap.supported_mcs_set,
+               bss->param.bss_config.supported_mcs_set, 16);
+        tlv_htcap->ht_cap.ht_ext_cap =
+            wlan_cpu_to_le16(bss->param.bss_config.ht_ext_cap);
+        tlv_htcap->ht_cap.tx_bf_cap =
+            wlan_cpu_to_le32(bss->param.bss_config.tx_bf_cap);
+        tlv_htcap->ht_cap.asel = bss->param.bss_config.asel;
+        cmd_size += sizeof(MrvlIETypes_HTCap_t);
+        tlv += sizeof(MrvlIETypes_HTCap_t);
+    }
+    if (bss->param.bss_config.mgmt_ie_passthru_mask < (MAX_VALID_DWORD)) {
+        tlv_mgmt_ie_passthru = (MrvlIEtypes_mgmt_ie_passthru_t *) tlv;
+        tlv_mgmt_ie_passthru->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_UAP_MGMT_IE_PASSTHRU_MASK);
+        tlv_mgmt_ie_passthru->header.len = wlan_cpu_to_le16(sizeof(t_u32));
+        /* keep copy in private data */
+        pmpriv->mgmt_frame_passthru_mask =
+            bss->param.bss_config.mgmt_ie_passthru_mask;
+        tlv_mgmt_ie_passthru->mgmt_ie_mask =
+            wlan_cpu_to_le32(bss->param.bss_config.mgmt_ie_passthru_mask);
+        cmd_size += sizeof(MrvlIEtypes_mgmt_ie_passthru_t);
+        tlv += sizeof(MrvlIEtypes_mgmt_ie_passthru_t);
+    }
+    if (((bss->param.bss_config.enable_2040coex == 0) ||
+         (bss->param.bss_config.enable_2040coex == 1))
+        ) {
+        tlv_2040_coex_enable = (MrvlIEtypes_2040_coex_enable_t *) tlv;
+        tlv_2040_coex_enable->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_2040_BSS_COEX_CONTROL);
+        tlv_2040_coex_enable->header.len = wlan_cpu_to_le16(sizeof(t_u8));
+        tlv_2040_coex_enable->enable_2040coex =
+            bss->param.bss_config.enable_2040coex;
+        cmd_size += sizeof(MrvlIEtypes_2040_coex_enable_t);
+        tlv += sizeof(MrvlIEtypes_2040_coex_enable_t);
+    }
+    if (bss->param.bss_config.wmm_para.qos_info == 0x80 ||
+        bss->param.bss_config.wmm_para.qos_info == 0x00) {
+        tlv_wmm_parameter = (MrvlIEtypes_wmm_parameter_t *) tlv;
+        tlv_wmm_parameter->header.type =
+            wlan_cpu_to_le16(TLV_TYPE_VENDOR_SPECIFIC_IE);
+        tlv_wmm_parameter->header.len =
+            wlan_cpu_to_le16(sizeof(bss->param.bss_config.wmm_para));
+        memcpy(pmpriv->adapter, tlv_wmm_parameter->wmm_para.ouitype,
+               bss->param.bss_config.wmm_para.ouitype,
+               sizeof(tlv_wmm_parameter->wmm_para.ouitype));
+        tlv_wmm_parameter->wmm_para.ouisubtype =
+            bss->param.bss_config.wmm_para.ouisubtype;
+        tlv_wmm_parameter->wmm_para.version =
+            bss->param.bss_config.wmm_para.version;
+        tlv_wmm_parameter->wmm_para.qos_info =
+            bss->param.bss_config.wmm_para.qos_info;
+        for (ac = 0; ac < 4; ac++) {
+            tlv_wmm_parameter->wmm_para.ac_params[ac].aci_aifsn.aifsn =
+                bss->param.bss_config.wmm_para.ac_params[ac].aci_aifsn.aifsn;
+            tlv_wmm_parameter->wmm_para.ac_params[ac].ecw.ecw_max =
+                bss->param.bss_config.wmm_para.ac_params[ac].ecw.ecw_max;
+            tlv_wmm_parameter->wmm_para.ac_params[ac].ecw.ecw_min =
+                bss->param.bss_config.wmm_para.ac_params[ac].ecw.ecw_min;
+            tlv_wmm_parameter->wmm_para.ac_params[ac].tx_op_limit =
+                wlan_cpu_to_le16(bss->param.bss_config.wmm_para.ac_params[ac].
+                                 tx_op_limit);
+        }
+        cmd_size += sizeof(MrvlIEtypes_wmm_parameter_t);
+        tlv += sizeof(MrvlIEtypes_wmm_parameter_t);
+    }
+
+    cmd->size = (t_u16) wlan_cpu_to_le16(cmd_size);
+    PRINTM(MCMND, "AP config: cmd_size=%d\n", cmd_size);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of sys_config
+ *
+ *  @param pmpriv		A pointer to mlan_private structure
+ *  @param cmd	   		A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   the action: GET or SET
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *  @return         MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_uap_cmd_sys_configure(pmlan_private pmpriv,
+                           IN HostCmd_DS_COMMAND * cmd,
+                           IN t_u16 cmd_action,
+                           IN pmlan_ioctl_req pioctl_buf, IN t_void * pdata_buf)
+{
+    mlan_ds_bss *bss = MNULL;
+    HostCmd_DS_SYS_CONFIG *sys_config =
+        (HostCmd_DS_SYS_CONFIG *) & cmd->params.sys_config;
+    MrvlIEtypes_MacAddr_t *mac_tlv = MNULL;
+    MrvlIEtypes_channel_band_t *chan_band_tlv = MNULL, *pdat_tlv_cb = MNULL;
+    MrvlIEtypes_beacon_period_t *bcn_pd_tlv = MNULL, *pdat_tlv_bcnpd = MNULL;
+    MrvlIEtypes_dtim_period_t *dtim_pd_tlv = MNULL, *pdat_tlv_dtimpd = MNULL;
+    mlan_ds_misc_custom_ie *cust_ie = MNULL;
+    mlan_ds_misc_cfg *misc = MNULL;
+    MrvlIEtypesHeader_t *ie_header =
+        (MrvlIEtypesHeader_t *) sys_config->tlv_buffer;
+    t_u8 *ie = (t_u8 *) sys_config->tlv_buffer + sizeof(MrvlIEtypesHeader_t);
+    t_u16 req_len = 0, travel_len = 0;
+    custom_ie *cptr = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HOST_CMD_APCMD_SYS_CONFIGURE);
+    sys_config->action = wlan_cpu_to_le16(cmd_action);
+    cmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_SYS_CONFIG) - 1 + S_DS_GEN);
+    if (pioctl_buf == MNULL) {
+        if (pdata_buf) {
+            switch (*(t_u16 *) pdata_buf) {
+            case TLV_TYPE_UAP_CHAN_BAND_CONFIG:
+                pdat_tlv_cb = (MrvlIEtypes_channel_band_t *) pdata_buf;
+                chan_band_tlv =
+                    (MrvlIEtypes_channel_band_t *) sys_config->tlv_buffer;
+                cmd->size =
+                    wlan_cpu_to_le16(sizeof(HostCmd_DS_SYS_CONFIG) - 1 +
+                                     S_DS_GEN +
+                                     sizeof(MrvlIEtypes_channel_band_t));
+                chan_band_tlv->header.type =
+                    wlan_cpu_to_le16(TLV_TYPE_UAP_CHAN_BAND_CONFIG);
+                chan_band_tlv->header.len =
+                    wlan_cpu_to_le16(sizeof(MrvlIEtypes_channel_band_t)
+                                     - sizeof(MrvlIEtypesHeader_t));
+                if (cmd_action) {
+                    chan_band_tlv->band_config = pdat_tlv_cb->band_config;
+                    chan_band_tlv->channel = pdat_tlv_cb->channel;
+                }
+                ret = MLAN_STATUS_SUCCESS;
+                break;
+            case TLV_TYPE_UAP_BEACON_PERIOD:
+                pdat_tlv_bcnpd = (MrvlIEtypes_beacon_period_t *) pdata_buf;
+                bcn_pd_tlv =
+                    (MrvlIEtypes_beacon_period_t *) sys_config->tlv_buffer;
+                cmd->size =
+                    sizeof(HostCmd_DS_SYS_CONFIG) - 1 + S_DS_GEN +
+                    sizeof(MrvlIEtypes_beacon_period_t);
+                bcn_pd_tlv->header.type =
+                    wlan_cpu_to_le16(TLV_TYPE_UAP_BEACON_PERIOD);
+                bcn_pd_tlv->header.len =
+                    wlan_cpu_to_le16(sizeof(MrvlIEtypes_beacon_period_t)
+                                     - sizeof(MrvlIEtypesHeader_t));
+                if (cmd_action) {
+                    bcn_pd_tlv->beacon_period =
+                        wlan_cpu_to_le16(pdat_tlv_bcnpd->beacon_period);
+                }
+                /* Add TLV_UAP_DTIM_PERIOD if it follws in pdata_buf */
+                pdat_tlv_dtimpd =
+                    (MrvlIEtypes_dtim_period_t *) (((t_u8 *) pdata_buf)
+                                                   +
+                                                   sizeof
+                                                   (MrvlIEtypes_beacon_period_t));
+                if (TLV_TYPE_UAP_DTIM_PERIOD == pdat_tlv_dtimpd->header.type) {
+                    dtim_pd_tlv =
+                        (MrvlIEtypes_dtim_period_t *) (sys_config->tlv_buffer +
+                                                       sizeof
+                                                       (MrvlIEtypes_beacon_period_t));
+                    cmd->size += sizeof(MrvlIEtypes_dtim_period_t);
+                    dtim_pd_tlv->header.type =
+                        wlan_cpu_to_le16(TLV_TYPE_UAP_DTIM_PERIOD);
+                    dtim_pd_tlv->header.len =
+                        wlan_cpu_to_le16(sizeof(MrvlIEtypes_dtim_period_t)
+                                         - sizeof(MrvlIEtypesHeader_t));
+                    if (cmd_action) {
+                        dtim_pd_tlv->dtim_period = pdat_tlv_dtimpd->dtim_period;
+                    }
+                }
+                /* Finalize cmd size */
+                cmd->size = wlan_cpu_to_le16(cmd->size);
+                ret = MLAN_STATUS_SUCCESS;
+                break;
+            case TLV_TYPE_MGMT_IE:
+                cust_ie = (mlan_ds_misc_custom_ie *) pdata_buf;
+                cmd->size =
+                    wlan_cpu_to_le16(sizeof(HostCmd_DS_SYS_CONFIG) - 1 +
+                                     S_DS_GEN + sizeof(MrvlIEtypesHeader_t) +
+                                     cust_ie->len);
+                ie_header->type = wlan_cpu_to_le16(TLV_TYPE_MGMT_IE);
+                ie_header->len = wlan_cpu_to_le16(cust_ie->len);
+
+                if (ie && cust_ie->ie_data_list) {
+                    req_len = cust_ie->len;
+                    travel_len = 0;
+                    /* conversion for index, mask, len */
+                    if (req_len == sizeof(t_u16))
+                        cust_ie->ie_data_list[0].ie_index =
+                            wlan_cpu_to_le16(cust_ie->ie_data_list[0].ie_index);
+                    while (req_len > sizeof(t_u16)) {
+                        cptr =
+                            (custom_ie *) (((t_u8 *) & cust_ie->ie_data_list) +
+                                           travel_len);
+                        travel_len +=
+                            cptr->ie_length + sizeof(custom_ie) - MAX_IE_SIZE;
+                        req_len -=
+                            cptr->ie_length + sizeof(custom_ie) - MAX_IE_SIZE;
+                        cptr->ie_index = wlan_cpu_to_le16(cptr->ie_index);
+                        cptr->mgmt_subtype_mask =
+                            wlan_cpu_to_le16(cptr->mgmt_subtype_mask);
+                        cptr->ie_length = wlan_cpu_to_le16(cptr->ie_length);
+                    }
+                    memcpy(pmpriv->adapter, ie, cust_ie->ie_data_list,
+                           cust_ie->len);
+                }
+                break;
+            default:
+                PRINTM(MERROR,
+                       "Wrong data, or missing TLV_TYPE 0x%04x handler.\n",
+                       *(t_u16 *) pdata_buf);
+                break;
+            }
+            goto done;
+        } else {
+            mac_tlv = (MrvlIEtypes_MacAddr_t *) sys_config->tlv_buffer;
+            cmd->size =
+                wlan_cpu_to_le16(sizeof(HostCmd_DS_SYS_CONFIG) - 1 + S_DS_GEN +
+                                 sizeof(MrvlIEtypes_MacAddr_t));
+            mac_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_UAP_MAC_ADDRESS);
+            mac_tlv->header.len = wlan_cpu_to_le16(MLAN_MAC_ADDR_LENGTH);
+            ret = MLAN_STATUS_SUCCESS;
+            goto done;
+        }
+    }
+    if (pioctl_buf->req_id == MLAN_IOCTL_BSS) {
+        bss = (mlan_ds_bss *) pioctl_buf->pbuf;
+        if (bss->sub_command == MLAN_OID_BSS_MAC_ADDR) {
+            mac_tlv = (MrvlIEtypes_MacAddr_t *) sys_config->tlv_buffer;
+            cmd->size =
+                wlan_cpu_to_le16(sizeof(HostCmd_DS_SYS_CONFIG) - 1 + S_DS_GEN +
+                                 sizeof(MrvlIEtypes_MacAddr_t));
+            mac_tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_UAP_MAC_ADDRESS);
+            mac_tlv->header.len = wlan_cpu_to_le16(MLAN_MAC_ADDR_LENGTH);
+            if (cmd_action == HostCmd_ACT_GEN_SET)
+                memcpy(pmpriv->adapter, mac_tlv->mac, &bss->param.mac_addr,
+                       MLAN_MAC_ADDR_LENGTH);
+        } else if ((bss->sub_command == MLAN_OID_UAP_BSS_CONFIG) &&
+                   (cmd_action == HostCmd_ACT_GEN_SET)) {
+            ret = wlan_uap_cmd_ap_config(pmpriv, cmd, cmd_action, pioctl_buf);
+            goto done;
+        }
+    } else if (pioctl_buf->req_id == MLAN_IOCTL_MISC_CFG) {
+        misc = (mlan_ds_misc_cfg *) pioctl_buf->pbuf;
+        if ((misc->sub_command == MLAN_OID_MISC_GEN_IE) &&
+            (misc->param.gen_ie.type == MLAN_IE_TYPE_GEN_IE)
+            ) {
+            cmd->size =
+                wlan_cpu_to_le16(sizeof(HostCmd_DS_SYS_CONFIG) - 1 + S_DS_GEN +
+                                 sizeof(MrvlIEtypesHeader_t) +
+                                 misc->param.gen_ie.len);
+            ie_header->type = wlan_cpu_to_le16(TLV_TYPE_WAPI_IE);
+            ie_header->len = wlan_cpu_to_le16(misc->param.gen_ie.len);
+            if (cmd_action == HostCmd_ACT_GEN_SET)
+                memcpy(pmpriv->adapter, ie, misc->param.gen_ie.ie_data,
+                       misc->param.gen_ie.len);
+        }
+        if ((misc->sub_command == MLAN_OID_MISC_CUSTOM_IE) &&
+            (misc->param.cust_ie.type == TLV_TYPE_MGMT_IE)) {
+            cmd->size =
+                wlan_cpu_to_le16(sizeof(HostCmd_DS_SYS_CONFIG) - 1 + S_DS_GEN +
+                                 sizeof(MrvlIEtypesHeader_t) +
+                                 misc->param.cust_ie.len);
+            ie_header->type = wlan_cpu_to_le16(TLV_TYPE_MGMT_IE);
+            ie_header->len = wlan_cpu_to_le16(misc->param.cust_ie.len);
+
+            if (ie && misc->param.cust_ie.ie_data_list) {
+                req_len = misc->param.cust_ie.len;
+                travel_len = 0;
+                /* conversion for index, mask, len */
+                if (req_len == sizeof(t_u16))
+                    misc->param.cust_ie.ie_data_list[0].ie_index =
+                        wlan_cpu_to_le16(misc->param.cust_ie.ie_data_list[0].
+                                         ie_index);
+                while (req_len > sizeof(t_u16)) {
+                    cptr =
+                        (custom_ie
+                         *) (((t_u8 *) & misc->param.cust_ie.ie_data_list) +
+                             travel_len);
+                    travel_len +=
+                        cptr->ie_length + sizeof(custom_ie) - MAX_IE_SIZE;
+                    req_len -=
+                        cptr->ie_length + sizeof(custom_ie) - MAX_IE_SIZE;
+                    cptr->ie_index = wlan_cpu_to_le16(cptr->ie_index);
+                    cptr->mgmt_subtype_mask =
+                        wlan_cpu_to_le16(cptr->mgmt_subtype_mask);
+                    cptr->ie_length = wlan_cpu_to_le16(cptr->ie_length);
+                }
+                if (misc->param.cust_ie.len)
+                    memcpy(pmpriv->adapter, ie,
+                           misc->param.cust_ie.ie_data_list,
+                           misc->param.cust_ie.len);
+            }
+        }
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function handles command resp for get uap settings
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_uap_ret_cmd_ap_config(IN pmlan_private pmpriv,
+                           IN HostCmd_DS_COMMAND * resp,
+                           IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_SYS_CONFIG *sys_config =
+        (HostCmd_DS_SYS_CONFIG *) & resp->params.sys_config;
+    mlan_ds_bss *bss = MNULL;
+    MrvlIEtypesHeader_t *tlv = MNULL;
+    t_u16 tlv_buf_left = 0;
+    t_u16 tlv_type = 0;
+    t_u16 tlv_len = 0;
+    MrvlIEtypes_MacAddr_t *tlv_mac = MNULL;
+    MrvlIEtypes_SsIdParamSet_t *tlv_ssid = MNULL;
+    MrvlIEtypes_beacon_period_t *tlv_beacon_period = MNULL;
+    MrvlIEtypes_dtim_period_t *tlv_dtim_period = MNULL;
+    MrvlIEtypes_RatesParamSet_t *tlv_rates = MNULL;
+    MrvlIEtypes_tx_rate_t *tlv_txrate = MNULL;
+    MrvlIEtypes_mcbc_rate_t *tlv_mcbc_rate = MNULL;
+    MrvlIEtypes_tx_power_t *tlv_tx_power = MNULL;
+    MrvlIEtypes_bcast_ssid_t *tlv_bcast_ssid = MNULL;
+    MrvlIEtypes_antenna_mode_t *tlv_antenna = MNULL;
+    MrvlIEtypes_pkt_forward_t *tlv_pkt_forward = MNULL;
+    MrvlIEtypes_max_sta_count_t *tlv_sta_count = MNULL;
+    MrvlIEtypes_sta_ageout_t *tlv_sta_ageout = MNULL;
+    MrvlIEtypes_ps_sta_ageout_t *tlv_ps_sta_ageout = MNULL;
+    MrvlIEtypes_rts_threshold_t *tlv_rts_threshold = MNULL;
+    MrvlIEtypes_frag_threshold_t *tlv_frag_threshold = MNULL;
+    MrvlIEtypes_retry_limit_t *tlv_retry_limit = MNULL;
+    MrvlIEtypes_eapol_pwk_hsk_timeout_t *tlv_pairwise_timeout = MNULL;
+    MrvlIEtypes_eapol_pwk_hsk_retries_t *tlv_pairwise_retries = MNULL;
+    MrvlIEtypes_eapol_gwk_hsk_timeout_t *tlv_groupwise_timeout = MNULL;
+    MrvlIEtypes_eapol_gwk_hsk_retries_t *tlv_groupwise_retries = MNULL;
+    MrvlIEtypes_mgmt_ie_passthru_t *tlv_mgmt_ie_passthru = MNULL;
+    MrvlIEtypes_2040_coex_enable_t *tlv_2040_coex_enable = MNULL;
+    MrvlIEtypes_mac_filter_t *tlv_mac_filter = MNULL;
+    MrvlIEtypes_channel_band_t *tlv_chan_band = MNULL;
+    MrvlIEtypes_ChanListParamSet_t *tlv_chan_list = MNULL;
+    ChanScanParamSet_t *pscan_chan = MNULL;
+    MrvlIEtypes_auth_type_t *tlv_auth_type = MNULL;
+    MrvlIEtypes_encrypt_protocol_t *tlv_encrypt_protocol = MNULL;
+    MrvlIEtypes_akmp_t *tlv_akmp = MNULL;
+    MrvlIEtypes_pwk_cipher_t *tlv_pwk_cipher = MNULL;
+    MrvlIEtypes_gwk_cipher_t *tlv_gwk_cipher = MNULL;
+    MrvlIEtypes_rsn_replay_prot_t *tlv_rsn_prot = MNULL;
+    MrvlIEtypes_passphrase_t *tlv_passphrase = MNULL;
+#ifdef WIFI_DIRECT_SUPPORT
+    MrvlIEtypes_psk_t *tlv_psk = MNULL;
+#endif /* WIFI_DIRECT_SUPPORT */
+    MrvlIEtypes_group_rekey_time_t *tlv_rekey_time = MNULL;
+    MrvlIEtypes_wep_key_t *tlv_wep_key = MNULL;
+    MrvlIEtypes_preamble_t *tlv_preamble = MNULL;
+    MrvlIEtypes_bss_status_t *tlv_bss_status = MNULL;
+    MrvlIETypes_HTCap_t *tlv_htcap = MNULL;
+    MrvlIEtypes_wmm_parameter_t *tlv_wmm_parameter = MNULL;
+
+    wep_key *pkey = MNULL;
+    t_u16 i;
+    t_u16 ac;
+
+    ENTER();
+
+    bss = (mlan_ds_bss *) pioctl_buf->pbuf;
+    tlv = (MrvlIEtypesHeader_t *) sys_config->tlv_buffer;
+    tlv_buf_left = resp->size - (sizeof(HostCmd_DS_SYS_CONFIG) - 1 + S_DS_GEN);
+
+    while (tlv_buf_left >= sizeof(MrvlIEtypesHeader_t)) {
+
+        tlv_type = wlan_le16_to_cpu(tlv->type);
+        tlv_len = wlan_le16_to_cpu(tlv->len);
+
+        if (tlv_buf_left < (tlv_len + sizeof(MrvlIEtypesHeader_t))) {
+            PRINTM(MERROR,
+                   "Error processing uAP sys config TLVs, bytes left < TLV length\n");
+            break;
+        }
+
+        switch (tlv_type) {
+        case TLV_TYPE_UAP_MAC_ADDRESS:
+            tlv_mac = (MrvlIEtypes_MacAddr_t *) tlv;
+            memcpy(pmpriv->adapter, &bss->param.bss_config.mac_addr,
+                   tlv_mac->mac, MLAN_MAC_ADDR_LENGTH);
+            break;
+        case TLV_TYPE_SSID:
+            tlv_ssid = (MrvlIEtypes_SsIdParamSet_t *) tlv;
+            bss->param.bss_config.ssid.ssid_len =
+                MIN(MLAN_MAX_SSID_LENGTH, tlv_len);
+            memcpy(pmpriv->adapter, bss->param.bss_config.ssid.ssid,
+                   tlv_ssid->ssid, MIN(MLAN_MAX_SSID_LENGTH, tlv_len));
+            break;
+        case TLV_TYPE_UAP_BEACON_PERIOD:
+            tlv_beacon_period = (MrvlIEtypes_beacon_period_t *) tlv;
+            bss->param.bss_config.beacon_period =
+                wlan_le16_to_cpu(tlv_beacon_period->beacon_period);
+            pmpriv->uap_state_chan_cb.beacon_period =
+                wlan_le16_to_cpu(tlv_beacon_period->beacon_period);
+            break;
+        case TLV_TYPE_UAP_DTIM_PERIOD:
+            tlv_dtim_period = (MrvlIEtypes_dtim_period_t *) tlv;
+            bss->param.bss_config.dtim_period = tlv_dtim_period->dtim_period;
+            pmpriv->uap_state_chan_cb.dtim_period =
+                tlv_dtim_period->dtim_period;
+            break;
+        case TLV_TYPE_RATES:
+            tlv_rates = (MrvlIEtypes_RatesParamSet_t *) tlv;
+            memcpy(pmpriv->adapter, bss->param.bss_config.rates,
+                   tlv_rates->rates, MIN(MAX_DATA_RATES, tlv_len));
+            break;
+        case TLV_TYPE_UAP_TX_DATA_RATE:
+            tlv_txrate = (MrvlIEtypes_tx_rate_t *) tlv;
+            bss->param.bss_config.tx_data_rate =
+                wlan_le16_to_cpu(tlv_txrate->tx_data_rate);
+            break;
+        case TLV_TYPE_UAP_MCBC_DATA_RATE:
+            tlv_mcbc_rate = (MrvlIEtypes_mcbc_rate_t *) tlv;
+            bss->param.bss_config.mcbc_data_rate =
+                wlan_le16_to_cpu(tlv_mcbc_rate->mcbc_data_rate);
+            break;
+        case TLV_TYPE_UAP_TX_POWER:
+            tlv_tx_power = (MrvlIEtypes_tx_power_t *) tlv;
+            bss->param.bss_config.tx_power_level = tlv_tx_power->tx_power;
+            break;
+        case TLV_TYPE_UAP_BCAST_SSID_CTL:
+            tlv_bcast_ssid = (MrvlIEtypes_bcast_ssid_t *) tlv;
+            bss->param.bss_config.bcast_ssid_ctl =
+                tlv_bcast_ssid->bcast_ssid_ctl;
+            break;
+        case TLV_TYPE_UAP_ANTENNA_CTL:
+            tlv_antenna = (MrvlIEtypes_antenna_mode_t *) tlv;
+            if (tlv_antenna->which_antenna == TX_ANTENNA)
+                bss->param.bss_config.tx_antenna = tlv_antenna->antenna_mode;
+            else if (tlv_antenna->which_antenna == RX_ANTENNA)
+                bss->param.bss_config.rx_antenna = tlv_antenna->antenna_mode;
+            break;
+        case TLV_TYPE_UAP_PKT_FWD_CTL:
+            tlv_pkt_forward = (MrvlIEtypes_pkt_forward_t *) tlv;
+            bss->param.bss_config.pkt_forward_ctl =
+                tlv_pkt_forward->pkt_forward_ctl;
+            break;
+        case TLV_TYPE_UAP_MAX_STA_CNT:
+            tlv_sta_count = (MrvlIEtypes_max_sta_count_t *) tlv;
+            bss->param.bss_config.max_sta_count =
+                wlan_le16_to_cpu(tlv_sta_count->max_sta_count);
+            break;
+        case TLV_TYPE_UAP_STA_AGEOUT_TIMER:
+            tlv_sta_ageout = (MrvlIEtypes_sta_ageout_t *) tlv;
+            bss->param.bss_config.sta_ageout_timer =
+                wlan_le32_to_cpu(tlv_sta_ageout->sta_ageout_timer);
+            break;
+        case TLV_TYPE_UAP_PS_STA_AGEOUT_TIMER:
+            tlv_ps_sta_ageout = (MrvlIEtypes_ps_sta_ageout_t *) tlv;
+            bss->param.bss_config.ps_sta_ageout_timer =
+                wlan_le32_to_cpu(tlv_ps_sta_ageout->ps_sta_ageout_timer);
+            break;
+        case TLV_TYPE_UAP_RTS_THRESHOLD:
+            tlv_rts_threshold = (MrvlIEtypes_rts_threshold_t *) tlv;
+            bss->param.bss_config.rts_threshold =
+                wlan_le16_to_cpu(tlv_rts_threshold->rts_threshold);
+            break;
+        case TLV_TYPE_UAP_FRAG_THRESHOLD:
+            tlv_frag_threshold = (MrvlIEtypes_frag_threshold_t *) tlv;
+            bss->param.bss_config.frag_threshold =
+                wlan_le16_to_cpu(tlv_frag_threshold->frag_threshold);
+            break;
+        case TLV_TYPE_UAP_RETRY_LIMIT:
+            tlv_retry_limit = (MrvlIEtypes_retry_limit_t *) tlv;
+            bss->param.bss_config.retry_limit = tlv_retry_limit->retry_limit;
+            break;
+        case TLV_TYPE_UAP_EAPOL_PWK_HSK_TIMEOUT:
+            tlv_pairwise_timeout = (MrvlIEtypes_eapol_pwk_hsk_timeout_t *) tlv;
+            bss->param.bss_config.pairwise_update_timeout =
+                wlan_le32_to_cpu(tlv_pairwise_timeout->pairwise_update_timeout);
+            break;
+        case TLV_TYPE_UAP_EAPOL_PWK_HSK_RETRIES:
+            tlv_pairwise_retries = (MrvlIEtypes_eapol_pwk_hsk_retries_t *) tlv;
+            bss->param.bss_config.pwk_retries =
+                wlan_le32_to_cpu(tlv_pairwise_retries->pwk_retries);
+            break;
+        case TLV_TYPE_UAP_EAPOL_GWK_HSK_TIMEOUT:
+            tlv_groupwise_timeout = (MrvlIEtypes_eapol_gwk_hsk_timeout_t *) tlv;
+            bss->param.bss_config.groupwise_update_timeout =
+                wlan_le32_to_cpu(tlv_groupwise_timeout->
+                                 groupwise_update_timeout);
+            break;
+        case TLV_TYPE_UAP_EAPOL_GWK_HSK_RETRIES:
+            tlv_groupwise_retries = (MrvlIEtypes_eapol_gwk_hsk_retries_t *) tlv;
+            bss->param.bss_config.gwk_retries =
+                wlan_le32_to_cpu(tlv_groupwise_retries->gwk_retries);
+            break;
+        case TLV_TYPE_UAP_MGMT_IE_PASSTHRU_MASK:
+            tlv_mgmt_ie_passthru = (MrvlIEtypes_mgmt_ie_passthru_t *) tlv;
+            bss->param.bss_config.mgmt_ie_passthru_mask =
+                wlan_le32_to_cpu(tlv_mgmt_ie_passthru->mgmt_ie_mask);
+            break;
+        case TLV_TYPE_2040_BSS_COEX_CONTROL:
+            tlv_2040_coex_enable = (MrvlIEtypes_2040_coex_enable_t *) tlv;
+            bss->param.bss_config.enable_2040coex =
+                tlv_2040_coex_enable->enable_2040coex;
+            break;
+        case TLV_TYPE_UAP_STA_MAC_ADDR_FILTER:
+            tlv_mac_filter = (MrvlIEtypes_mac_filter_t *) tlv;
+            bss->param.bss_config.filter.mac_count =
+                MIN(MAX_MAC_FILTER_NUM, tlv_mac_filter->count);
+            bss->param.bss_config.filter.filter_mode =
+                tlv_mac_filter->filter_mode;
+            memcpy(pmpriv->adapter,
+                   (t_u8 *) bss->param.bss_config.filter.mac_list,
+                   tlv_mac_filter->mac_address,
+                   MLAN_MAC_ADDR_LENGTH *
+                   bss->param.bss_config.filter.mac_count);
+            break;
+        case TLV_TYPE_UAP_CHAN_BAND_CONFIG:
+            tlv_chan_band = (MrvlIEtypes_channel_band_t *) tlv;
+            bss->param.bss_config.band_cfg = tlv_chan_band->band_config;
+            bss->param.bss_config.channel = tlv_chan_band->channel;
+            pmpriv->uap_state_chan_cb.band_config = tlv_chan_band->band_config;
+            pmpriv->uap_state_chan_cb.channel = tlv_chan_band->channel;
+            break;
+        case TLV_TYPE_CHANLIST:
+            tlv_chan_list = (MrvlIEtypes_ChanListParamSet_t *) tlv;
+            bss->param.bss_config.num_of_chan =
+                tlv_len / sizeof(ChanScanParamSet_t);
+            pscan_chan = tlv_chan_list->chan_scan_param;
+            for (i = 0; i < bss->param.bss_config.num_of_chan; i++) {
+                bss->param.bss_config.chan_list[i].chan_number =
+                    pscan_chan->chan_number;
+                bss->param.bss_config.chan_list[i].band_config_type =
+                    pscan_chan->radio_type;
+                pscan_chan++;
+            }
+            break;
+        case TLV_TYPE_AUTH_TYPE:
+            tlv_auth_type = (MrvlIEtypes_auth_type_t *) tlv;
+            bss->param.bss_config.auth_mode = tlv_auth_type->auth_type;
+            break;
+        case TLV_TYPE_UAP_ENCRYPT_PROTOCOL:
+            tlv_encrypt_protocol = (MrvlIEtypes_encrypt_protocol_t *) tlv;
+            bss->param.bss_config.protocol =
+                wlan_le16_to_cpu(tlv_encrypt_protocol->protocol);
+            break;
+        case TLV_TYPE_UAP_AKMP:
+            tlv_akmp = (MrvlIEtypes_akmp_t *) tlv;
+            bss->param.bss_config.key_mgmt =
+                wlan_le16_to_cpu(tlv_akmp->key_mgmt);
+            if (tlv_len > sizeof(t_u16))
+                bss->param.bss_config.key_mgmt_operation =
+                    wlan_le16_to_cpu(tlv_akmp->key_mgmt_operation);
+            break;
+        case TLV_TYPE_PWK_CIPHER:
+            tlv_pwk_cipher = (MrvlIEtypes_pwk_cipher_t *) tlv;
+            if (wlan_le16_to_cpu(tlv_pwk_cipher->protocol) & PROTOCOL_WPA)
+                bss->param.bss_config.wpa_cfg.pairwise_cipher_wpa =
+                    tlv_pwk_cipher->pairwise_cipher;
+            if (wlan_le16_to_cpu(tlv_pwk_cipher->protocol) & PROTOCOL_WPA2)
+                bss->param.bss_config.wpa_cfg.pairwise_cipher_wpa2 =
+                    tlv_pwk_cipher->pairwise_cipher;
+            break;
+        case TLV_TYPE_GWK_CIPHER:
+            tlv_gwk_cipher = (MrvlIEtypes_gwk_cipher_t *) tlv;
+            bss->param.bss_config.wpa_cfg.group_cipher =
+                tlv_gwk_cipher->group_cipher;
+            break;
+        case TLV_TYPE_UAP_RSN_REPLAY_PROTECT:
+            tlv_rsn_prot = (MrvlIEtypes_rsn_replay_prot_t *) tlv;
+            bss->param.bss_config.wpa_cfg.rsn_protection =
+                tlv_rsn_prot->rsn_replay_prot;
+            break;
+        case TLV_TYPE_UAP_WPA_PASSPHRASE:
+            tlv_passphrase = (MrvlIEtypes_passphrase_t *) tlv;
+            bss->param.bss_config.wpa_cfg.length =
+                MIN(MLAN_PMK_HEXSTR_LENGTH, tlv_len);
+            memcpy(pmpriv->adapter, bss->param.bss_config.wpa_cfg.passphrase,
+                   tlv_passphrase->passphrase,
+                   bss->param.bss_config.wpa_cfg.length);
+            break;
+#ifdef WIFI_DIRECT_SUPPORT
+        case TLV_TYPE_UAP_PSK:
+            tlv_psk = (MrvlIEtypes_psk_t *) tlv;
+            memcpy(pmpriv->adapter, bss->param.bss_config.psk, tlv_psk->psk,
+                   MIN(MLAN_MAX_KEY_LENGTH, tlv_len));
+            break;
+#endif /* WIFI_DIRECT_SUPPORT */
+        case TLV_TYPE_UAP_GRP_REKEY_TIME:
+            tlv_rekey_time = (MrvlIEtypes_group_rekey_time_t *) tlv;
+            bss->param.bss_config.wpa_cfg.gk_rekey_time =
+                wlan_le32_to_cpu(tlv_rekey_time->gk_rekey_time);
+            break;
+        case TLV_TYPE_UAP_WEP_KEY:
+            tlv_wep_key = (MrvlIEtypes_wep_key_t *) tlv;
+            pkey = MNULL;
+            if (tlv_wep_key->key_index == 0)
+                pkey = &bss->param.bss_config.wep_cfg.key0;
+            else if (tlv_wep_key->key_index == 1)
+                pkey = &bss->param.bss_config.wep_cfg.key1;
+            else if (tlv_wep_key->key_index == 2)
+                pkey = &bss->param.bss_config.wep_cfg.key2;
+            else if (tlv_wep_key->key_index == 3)
+                pkey = &bss->param.bss_config.wep_cfg.key3;
+            if (pkey) {
+                pkey->key_index = tlv_wep_key->key_index;
+                pkey->is_default = tlv_wep_key->is_default;
+                pkey->length = MIN(MAX_WEP_KEY_SIZE, (tlv_len - 2));
+                memcpy(pmpriv->adapter, pkey->key, tlv_wep_key->key,
+                       pkey->length);
+            }
+            break;
+        case TLV_TYPE_UAP_PREAMBLE_CTL:
+            tlv_preamble = (MrvlIEtypes_preamble_t *) tlv;
+            bss->param.bss_config.preamble_type = tlv_preamble->preamble_type;
+            break;
+        case TLV_TYPE_BSS_STATUS:
+            tlv_bss_status = (MrvlIEtypes_bss_status_t *) tlv;
+            bss->param.bss_config.bss_status =
+                wlan_le16_to_cpu(tlv_bss_status->bss_status);
+            pmpriv->uap_bss_started =
+                (bss->param.bss_config.bss_status) ? MTRUE : MFALSE;
+            break;
+        case HT_CAPABILITY:
+            tlv_htcap = (MrvlIETypes_HTCap_t *) tlv;
+            bss->param.bss_config.ht_cap_info =
+                wlan_le16_to_cpu(tlv_htcap->ht_cap.ht_cap_info);
+            bss->param.bss_config.ampdu_param = tlv_htcap->ht_cap.ampdu_param;
+            memcpy(pmpriv->adapter, bss->param.bss_config.supported_mcs_set,
+                   tlv_htcap->ht_cap.supported_mcs_set, 16);
+            bss->param.bss_config.ht_ext_cap =
+                wlan_le16_to_cpu(tlv_htcap->ht_cap.ht_ext_cap);
+            bss->param.bss_config.tx_bf_cap =
+                wlan_le32_to_cpu(tlv_htcap->ht_cap.tx_bf_cap);
+            bss->param.bss_config.asel = tlv_htcap->ht_cap.asel;
+            break;
+        case TLV_TYPE_VENDOR_SPECIFIC_IE:
+            tlv_wmm_parameter = (MrvlIEtypes_wmm_parameter_t *) tlv;
+            bss->param.bss_config.wmm_para.qos_info =
+                tlv_wmm_parameter->wmm_para.qos_info;
+            for (ac = 0; ac < 4; ac++) {
+                bss->param.bss_config.wmm_para.ac_params[ac].aci_aifsn.aifsn =
+                    tlv_wmm_parameter->wmm_para.ac_params[ac].aci_aifsn.aifsn;
+                bss->param.bss_config.wmm_para.ac_params[ac].ecw.ecw_max =
+                    tlv_wmm_parameter->wmm_para.ac_params[ac].ecw.ecw_max;
+                bss->param.bss_config.wmm_para.ac_params[ac].ecw.ecw_min =
+                    tlv_wmm_parameter->wmm_para.ac_params[ac].ecw.ecw_min;
+                bss->param.bss_config.wmm_para.ac_params[ac].tx_op_limit =
+                    wlan_le16_to_cpu(tlv_wmm_parameter->wmm_para.ac_params[ac].
+                                     tx_op_limit);
+            }
+            break;
+        }
+
+        tlv_buf_left -= tlv_len + sizeof(MrvlIEtypesHeader_t);
+        tlv =
+            (MrvlIEtypesHeader_t *) ((t_u8 *) tlv + tlv_len +
+                                     sizeof(MrvlIEtypesHeader_t));
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of sys_reset
+ *         Clear various private state variables used by DFS.
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_uap_ret_sys_reset(IN pmlan_private pmpriv,
+                       IN HostCmd_DS_COMMAND * resp,
+                       IN mlan_ioctl_req * pioctl_buf)
+{
+    ENTER();
+
+    pmpriv->uap_state_chan_cb.band_config = 0;
+    pmpriv->uap_state_chan_cb.channel = 0;
+    pmpriv->uap_state_chan_cb.beacon_period = 0;
+    pmpriv->uap_state_chan_cb.dtim_period = 0;
+
+    /* assume default 11d/11h states are off, should check with FW */
+    /* currently don't clear domain_info... global, could be from STA */
+    wlan_11d_priv_init(pmpriv);
+    wlan_11h_priv_init(pmpriv);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of sys_config
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_uap_ret_sys_config(IN pmlan_private pmpriv,
+                        IN HostCmd_DS_COMMAND * resp,
+                        IN mlan_ioctl_req * pioctl_buf)
+{
+    int resp_len = 0, travel_len = 0;
+    int i = 0;
+    custom_ie *cptr;
+    HostCmd_DS_SYS_CONFIG *sys_config =
+        (HostCmd_DS_SYS_CONFIG *) & resp->params.sys_config;
+    mlan_ds_bss *bss = MNULL;
+    mlan_ds_misc_cfg *misc = MNULL;
+    MrvlIEtypes_MacAddr_t *tlv =
+        (MrvlIEtypes_MacAddr_t *) sys_config->tlv_buffer;
+    mlan_ds_misc_custom_ie *cust_ie = MNULL;
+    tlvbuf_max_mgmt_ie *max_mgmt_ie = MNULL;
+    MrvlIEtypes_channel_band_t *tlv_cb = MNULL;
+    MrvlIEtypes_beacon_period_t *tlv_bcnpd = MNULL;
+    MrvlIEtypes_dtim_period_t *tlv_dtimpd = MNULL;
+
+    ENTER();
+    if (pioctl_buf) {
+        if (pioctl_buf->req_id == MLAN_IOCTL_BSS) {
+            bss = (mlan_ds_bss *) pioctl_buf->pbuf;
+            if (bss->sub_command == MLAN_OID_BSS_MAC_ADDR) {
+                if (TLV_TYPE_UAP_MAC_ADDRESS ==
+                    wlan_le16_to_cpu(tlv->header.type)) {
+                    memcpy(pmpriv->adapter, &bss->param.mac_addr, tlv->mac,
+                           MLAN_MAC_ADDR_LENGTH);
+                }
+            } else if ((bss->sub_command == MLAN_OID_UAP_BSS_CONFIG) &&
+                       (pioctl_buf->action == MLAN_ACT_GET)) {
+                wlan_uap_ret_cmd_ap_config(pmpriv, resp, pioctl_buf);
+            }
+        }
+        if (pioctl_buf->req_id == MLAN_IOCTL_MISC_CFG) {
+            misc = (mlan_ds_misc_cfg *) pioctl_buf->pbuf;
+            cust_ie = (mlan_ds_misc_custom_ie *) sys_config->tlv_buffer;
+            max_mgmt_ie =
+                (tlvbuf_max_mgmt_ie *) (sys_config->tlv_buffer + cust_ie->len +
+                                        sizeof(MrvlIEtypesHeader_t));
+            if ((pioctl_buf->action == MLAN_ACT_GET) &&
+                (misc->sub_command == MLAN_OID_MISC_CUSTOM_IE)) {
+
+                cust_ie->type = wlan_le16_to_cpu(cust_ie->type);
+                resp_len = cust_ie->len = wlan_le16_to_cpu(cust_ie->len);
+                travel_len = 0;
+                /* conversion for index, mask, len */
+                if (resp_len == sizeof(t_u16))
+                    cust_ie->ie_data_list[0].ie_index =
+                        wlan_cpu_to_le16(cust_ie->ie_data_list[0].ie_index);
+
+                while (resp_len > sizeof(t_u16)) {
+                    cptr =
+                        (custom_ie *) (((t_u8 *) cust_ie->ie_data_list) +
+                                       travel_len);
+                    cptr->ie_index = wlan_le16_to_cpu(cptr->ie_index);
+                    cptr->mgmt_subtype_mask =
+                        wlan_le16_to_cpu(cptr->mgmt_subtype_mask);
+                    cptr->ie_length = wlan_le16_to_cpu(cptr->ie_length);
+                    travel_len +=
+                        cptr->ie_length + sizeof(custom_ie) - MAX_IE_SIZE;
+                    resp_len -=
+                        cptr->ie_length + sizeof(custom_ie) - MAX_IE_SIZE;
+                }
+                memcpy(pmpriv->adapter, &misc->param.cust_ie, cust_ie,
+                       MIN(sizeof(mlan_ds_misc_custom_ie) -
+                           sizeof(tlvbuf_max_mgmt_ie),
+                           (cust_ie->len + sizeof(MrvlIEtypesHeader_t))));
+                if (max_mgmt_ie) {
+                    max_mgmt_ie->type = wlan_le16_to_cpu(max_mgmt_ie->type);
+                    if (max_mgmt_ie->type == TLV_TYPE_MAX_MGMT_IE) {
+                        max_mgmt_ie->len = wlan_le16_to_cpu(max_mgmt_ie->len);
+                        max_mgmt_ie->count =
+                            wlan_le16_to_cpu(max_mgmt_ie->count);
+                        for (i = 0; i < max_mgmt_ie->count; i++) {
+                            max_mgmt_ie->info[i].buf_size =
+                                wlan_le16_to_cpu(max_mgmt_ie->info[i].buf_size);
+                            max_mgmt_ie->info[i].buf_count =
+                                wlan_le16_to_cpu(max_mgmt_ie->info[i].
+                                                 buf_count);
+                        }
+                        /* Append max_mgmt_ie TLV after custom_ie */
+                        memcpy(pmpriv->adapter,
+                               (t_u8 *) & misc->param.cust_ie + (cust_ie->len +
+                                                                 sizeof
+                                                                 (MrvlIEtypesHeader_t)),
+                               max_mgmt_ie, MIN(sizeof(tlvbuf_max_mgmt_ie),
+                                                max_mgmt_ie->len +
+                                                sizeof(MrvlIEtypesHeader_t)));
+                    }
+                }
+            }
+        }
+    } else {                    /* no ioctl: driver generated get/set */
+        switch (wlan_le16_to_cpu(tlv->header.type)) {
+        case TLV_TYPE_UAP_MAC_ADDRESS:
+            memcpy(pmpriv->adapter, pmpriv->curr_addr, tlv->mac,
+                   MLAN_MAC_ADDR_LENGTH);
+            break;
+        case TLV_TYPE_UAP_CHAN_BAND_CONFIG:
+            tlv_cb = (MrvlIEtypes_channel_band_t *) tlv;
+            pmpriv->uap_state_chan_cb.band_config = tlv_cb->band_config;
+            pmpriv->uap_state_chan_cb.channel = tlv_cb->channel;
+            /* call callback waiting for channel info */
+            if (pmpriv->uap_state_chan_cb.get_chan_callback)
+                pmpriv->uap_state_chan_cb.get_chan_callback(pmpriv);
+            break;
+        case TLV_TYPE_UAP_BEACON_PERIOD:
+            tlv_bcnpd = (MrvlIEtypes_beacon_period_t *) tlv;
+            pmpriv->uap_state_chan_cb.beacon_period =
+                wlan_le16_to_cpu(tlv_bcnpd->beacon_period);
+            /* copy dtim_period as well if it follows */
+            tlv_dtimpd =
+                (MrvlIEtypes_dtim_period_t *) (((t_u8 *) tlv) +
+                                               sizeof
+                                               (MrvlIEtypes_beacon_period_t));
+            if (TLV_TYPE_UAP_DTIM_PERIOD ==
+                wlan_le16_to_cpu(tlv_dtimpd->header.type))
+                pmpriv->uap_state_chan_cb.dtim_period = tlv_dtimpd->dtim_period;
+            /* call callback waiting for beacon/dtim info */
+            if (pmpriv->uap_state_chan_cb.get_chan_callback)
+                pmpriv->uap_state_chan_cb.get_chan_callback(pmpriv);
+            break;
+        case TLV_TYPE_MGMT_IE:
+            if ((pmpriv->adapter->state_rdh.stage == RDH_SET_CUSTOM_IE) ||
+                (pmpriv->adapter->state_rdh.stage == RDH_REM_CUSTOM_IE))
+                wlan_11h_radar_detected_callback((t_void *) pmpriv);
+            break;
+        }
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of snmp_mib
+ *
+ *  @param pmpriv		A pointer to mlan_private structure
+ *  @param cmd	   		A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   the action: GET or SET
+ *  @param cmd_oid      Cmd oid: treated as sub command
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *  @param pdata_buf    A pointer to information buffer
+ *  @return         MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_uap_cmd_snmp_mib(pmlan_private pmpriv,
+                      IN HostCmd_DS_COMMAND * cmd,
+                      IN t_u16 cmd_action,
+                      IN t_u32 cmd_oid,
+                      IN pmlan_ioctl_req pioctl_buf, IN t_void * pdata_buf)
+{
+    HostCmd_DS_802_11_SNMP_MIB *psnmp_mib = &cmd->params.smib;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u8 *psnmp_oid = MNULL;
+    t_u32 ul_temp;
+    t_u8 i;
+
+    t_u8 snmp_oids[] = {
+        tkip_mic_failures,
+        ccmp_decrypt_errors,
+        wep_undecryptable_count,
+        wep_icv_error_count,
+        decrypt_failure_count,
+        dot11_mcast_tx_count,
+        dot11_failed_count,
+        dot11_retry_count,
+        dot11_multi_retry_count,
+        dot11_frame_dup_count,
+        dot11_rts_success_count,
+        dot11_rts_failure_count,
+        dot11_ack_failure_count,
+        dot11_rx_fragment_count,
+        dot11_mcast_rx_frame_count,
+        dot11_fcs_error_count,
+        dot11_tx_frame_count,
+        dot11_rsna_tkip_cm_invoked,
+        dot11_rsna_4way_hshk_failures,
+    };
+
+    ENTER();
+
+    if (cmd_action == HostCmd_ACT_GEN_GET) {
+        cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_SNMP_MIB);
+        psnmp_mib->query_type = wlan_cpu_to_le16(HostCmd_ACT_GEN_GET);
+        cmd->size =
+            wlan_cpu_to_le16(sizeof(t_u16) + S_DS_GEN +
+                             sizeof(snmp_oids) *
+                             sizeof(MrvlIEtypes_snmp_oid_t));
+        psnmp_oid = (t_u8 *) & psnmp_mib->oid;
+        for (i = 0; i < sizeof(snmp_oids); i++) {
+            /* SNMP OID header type */
+            *(t_u16 *) psnmp_oid = wlan_cpu_to_le16(snmp_oids[i]);
+            psnmp_oid += sizeof(t_u16);
+            /* SNMP OID header length */
+            *(t_u16 *) psnmp_oid = wlan_cpu_to_le16(sizeof(t_u32));
+            psnmp_oid += sizeof(t_u16) + sizeof(t_u32);
+        }
+    } else {                    /* cmd_action == ACT_SET */
+        cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_SNMP_MIB);
+        cmd->size = sizeof(HostCmd_DS_802_11_SNMP_MIB) - 1 + S_DS_GEN;
+        psnmp_mib->query_type = wlan_cpu_to_le16(HostCmd_ACT_GEN_SET);
+
+        switch (cmd_oid) {
+        case Dot11D_i:
+        case Dot11H_i:
+            psnmp_mib->oid = wlan_cpu_to_le16((t_u16) cmd_oid);
+            psnmp_mib->buf_size = wlan_cpu_to_le16(sizeof(t_u16));
+            ul_temp = *(t_u32 *) pdata_buf;
+            *((t_u16 *) (psnmp_mib->value)) = wlan_cpu_to_le16((t_u16) ul_temp);
+            cmd->size += sizeof(t_u16);
+            break;
+        default:
+            PRINTM(MERROR, "Unsupported OID.\n");
+            ret = MLAN_STATUS_FAILURE;
+            break;
+        }
+        cmd->size = wlan_cpu_to_le16(cmd->size);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function handles the command response of snmp_mib
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_uap_ret_snmp_mib(IN pmlan_private pmpriv,
+                      IN HostCmd_DS_COMMAND * resp,
+                      IN mlan_ioctl_req * pioctl_buf)
+{
+    pmlan_adapter pmadapter = pmpriv->adapter;
+    HostCmd_DS_802_11_SNMP_MIB *psnmp_mib =
+        (HostCmd_DS_802_11_SNMP_MIB *) & resp->params.smib;
+    mlan_ds_get_info *info;
+    t_u8 *psnmp_oid = MNULL;
+    t_u32 data;
+    t_u16 tlv_buf_left = 0;
+    t_u16 tlv_type = 0;
+
+    ENTER();
+    if (psnmp_mib->query_type == HostCmd_ACT_GEN_GET) {
+        if (!pioctl_buf) {
+            LEAVE();
+            return MLAN_STATUS_SUCCESS;
+        }
+        info = (mlan_ds_get_info *) pioctl_buf->pbuf;
+        tlv_buf_left = resp->size - (sizeof(t_u16) + S_DS_GEN);
+        psnmp_oid = (t_u8 *) & psnmp_mib->oid;
+        while (tlv_buf_left >= sizeof(MrvlIEtypes_snmp_oid_t)) {
+            tlv_type = wlan_le16_to_cpu(*(t_u16 *) psnmp_oid);
+            psnmp_oid += sizeof(t_u16) + sizeof(t_u16);
+            memcpy(pmadapter, &data, psnmp_oid, sizeof(t_u32));
+            switch (tlv_type) {
+            case tkip_mic_failures:
+                info->param.ustats.tkip_mic_failures = wlan_le32_to_cpu(data);
+                break;
+            case ccmp_decrypt_errors:
+                info->param.ustats.ccmp_decrypt_errors = wlan_le32_to_cpu(data);
+                break;
+            case wep_undecryptable_count:
+                info->param.ustats.wep_undecryptable_count =
+                    wlan_le32_to_cpu(data);
+                break;
+            case wep_icv_error_count:
+                info->param.ustats.wep_icv_error_count = wlan_le32_to_cpu(data);
+                break;
+            case decrypt_failure_count:
+                info->param.ustats.decrypt_failure_count =
+                    wlan_le32_to_cpu(data);
+                break;
+            case dot11_mcast_tx_count:
+                info->param.ustats.mcast_tx_count = wlan_le32_to_cpu(data);
+                break;
+            case dot11_failed_count:
+                info->param.ustats.failed_count = wlan_le32_to_cpu(data);
+                break;
+            case dot11_retry_count:
+                info->param.ustats.retry_count = wlan_le32_to_cpu(data);
+                break;
+            case dot11_multi_retry_count:
+                info->param.ustats.multi_retry_count = wlan_le32_to_cpu(data);
+                break;
+            case dot11_frame_dup_count:
+                info->param.ustats.frame_dup_count = wlan_le32_to_cpu(data);
+                break;
+            case dot11_rts_success_count:
+                info->param.ustats.rts_success_count = wlan_le32_to_cpu(data);
+                break;
+            case dot11_rts_failure_count:
+                info->param.ustats.rts_failure_count = wlan_le32_to_cpu(data);
+                break;
+            case dot11_ack_failure_count:
+                info->param.ustats.ack_failure_count = wlan_le32_to_cpu(data);
+                break;
+            case dot11_rx_fragment_count:
+                info->param.ustats.rx_fragment_count = wlan_le32_to_cpu(data);
+                break;
+            case dot11_mcast_rx_frame_count:
+                info->param.ustats.mcast_rx_frame_count =
+                    wlan_le32_to_cpu(data);
+                break;
+            case dot11_fcs_error_count:
+                info->param.ustats.fcs_error_count = wlan_le32_to_cpu(data);
+                break;
+            case dot11_tx_frame_count:
+                info->param.ustats.tx_frame_count = wlan_le32_to_cpu(data);
+                break;
+            case dot11_rsna_tkip_cm_invoked:
+                info->param.ustats.rsna_tkip_cm_invoked =
+                    wlan_le32_to_cpu(data);
+                break;
+            case dot11_rsna_4way_hshk_failures:
+                info->param.ustats.rsna_4way_hshk_failures =
+                    wlan_le32_to_cpu(data);
+                break;
+            }
+            tlv_buf_left -= sizeof(MrvlIEtypes_snmp_oid_t);
+            psnmp_oid += sizeof(t_u32);
+        }
+    } else {                    /* ACT_SET */
+        switch (psnmp_mib->oid) {
+        case Dot11D_i:
+            data = wlan_le16_to_cpu(*((t_u16 *) (psnmp_mib->value)));
+            /* Set 11d state to private */
+            pmpriv->state_11d.enable_11d = data;
+            /* Set user enable flag if called from ioctl */
+            if (pioctl_buf)
+                pmpriv->state_11d.user_enable_11d = data;
+            break;
+        case Dot11H_i:
+            data = wlan_le16_to_cpu(*((t_u16 *) (psnmp_mib->value)));
+            /* Set 11h state to priv */
+            pmpriv->intf_state_11h.is_11h_active = (data & ENABLE_11H_MASK);
+            /* Set radar_det state to adapter */
+            pmpriv->adapter->state_11h.is_master_radar_det_active
+                = (data & MASTER_RADAR_DET_MASK) ? MTRUE : MFALSE;
+            pmpriv->adapter->state_11h.is_slave_radar_det_active
+                = (data & SLAVE_RADAR_DET_MASK) ? MTRUE : MFALSE;
+            break;
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of deauth station
+ *
+ *  @param pmpriv		A pointer to mlan_private structure
+ *  @param cmd	   		A pointer to HostCmd_DS_COMMAND structure
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return         MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_uap_cmd_sta_deauth(pmlan_private pmpriv,
+                        IN HostCmd_DS_COMMAND * cmd, IN t_void * pdata_buf)
+{
+    HostCmd_DS_STA_DEAUTH *pcmd_sta_deauth =
+        (HostCmd_DS_STA_DEAUTH *) & cmd->params.sta_deauth;
+    mlan_deauth_param *deauth = (mlan_deauth_param *) pdata_buf;
+
+    ENTER();
+    cmd->command = wlan_cpu_to_le16(HOST_CMD_APCMD_STA_DEAUTH);
+    cmd->size = wlan_cpu_to_le16(S_DS_GEN + sizeof(HostCmd_DS_STA_DEAUTH));
+    memcpy(pmpriv->adapter, pcmd_sta_deauth->mac, deauth->mac_addr,
+           MLAN_MAC_ADDR_LENGTH);
+    pcmd_sta_deauth->reason = wlan_cpu_to_le16(deauth->reason_code);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares command of key material
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param cmd_action   The action: GET or SET
+ *  @param cmd_oid      OID: ENABLE or DISABLE
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_uap_cmd_key_material(IN pmlan_private pmpriv,
+                          IN HostCmd_DS_COMMAND * cmd,
+                          IN t_u16 cmd_action,
+                          IN t_u16 cmd_oid, IN t_void * pdata_buf)
+{
+    HostCmd_DS_802_11_KEY_MATERIAL *pkey_material = &cmd->params.key_material;
+    mlan_ds_encrypt_key *pkey = (mlan_ds_encrypt_key *) pdata_buf;
+    MrvlIEtypes_MacAddr_t *tlv = MNULL;
+    const t_u8 bc_mac[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+    t_u16 key_param_len = 0;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    sta_node *sta_ptr = MNULL;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_KEY_MATERIAL);
+    pkey_material->action = wlan_cpu_to_le16(cmd_action);
+
+    if (cmd_action == HostCmd_ACT_GEN_GET) {
+        cmd->size = wlan_cpu_to_le16(sizeof(pkey_material->action) + S_DS_GEN);
+        goto done;
+    }
+
+    memset(pmpriv->adapter, &pkey_material->key_param_set, 0,
+           sizeof(MrvlIEtype_KeyParamSet_t));
+    if (pkey->is_wapi_key) {
+        PRINTM(MINFO, "Set WAPI Key\n");
+        pkey_material->key_param_set.key_type_id =
+            wlan_cpu_to_le16(KEY_TYPE_ID_WAPI);
+        if (cmd_oid == KEY_INFO_ENABLED)
+            pkey_material->key_param_set.key_info =
+                wlan_cpu_to_le16(KEY_INFO_WAPI_ENABLED);
+        else
+            pkey_material->key_param_set.key_info =
+                !(wlan_cpu_to_le16(KEY_INFO_WAPI_ENABLED));
+
+        pkey_material->key_param_set.key[0] = pkey->key_index;
+        if (!pmpriv->sec_info.wapi_key_on)
+            pkey_material->key_param_set.key[1] = 1;
+        else
+            pkey_material->key_param_set.key[1] = 0;    /* set 0 when re-key */
+
+        if (0 != memcmp(pmpriv->adapter, pkey->mac_addr, bc_mac, sizeof(bc_mac))) {     /* WAPI
+                                                                                           pairwise
+                                                                                           key:
+                                                                                           unicast
+                                                                                         */
+            pkey_material->key_param_set.key_info |=
+                wlan_cpu_to_le16(KEY_INFO_WAPI_UNICAST);
+            if ((sta_ptr = wlan_add_station_entry(pmpriv, pkey->mac_addr))) {
+                PRINTM(MCMND, "station: wapi_key_on\n");
+                sta_ptr->wapi_key_on = MTRUE;
+            }
+        } else {                /* WAPI group key: multicast */
+            pkey_material->key_param_set.key_info |=
+                wlan_cpu_to_le16(KEY_INFO_WAPI_MCAST);
+            pmpriv->sec_info.wapi_key_on = MTRUE;
+        }
+        pkey_material->key_param_set.type =
+            wlan_cpu_to_le16(TLV_TYPE_KEY_MATERIAL);
+        pkey_material->key_param_set.key_len = wlan_cpu_to_le16(WAPI_KEY_LEN);
+        memcpy(pmpriv->adapter, &pkey_material->key_param_set.key[2],
+               pkey->key_material, pkey->key_len);
+        pkey_material->key_param_set.length =
+            wlan_cpu_to_le16(WAPI_KEY_LEN + KEYPARAMSET_FIXED_LEN);
+
+        key_param_len =
+            (WAPI_KEY_LEN + KEYPARAMSET_FIXED_LEN) +
+            sizeof(MrvlIEtypesHeader_t);
+        tlv =
+            (MrvlIEtypes_MacAddr_t *) ((t_u8 *) & pkey_material->key_param_set +
+                                       key_param_len);
+        tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_STA_MAC_ADDRESS);
+        tlv->header.len = wlan_cpu_to_le16(MLAN_MAC_ADDR_LENGTH);
+        memcpy(pmpriv->adapter, tlv->mac, pkey->mac_addr, MLAN_MAC_ADDR_LENGTH);
+        cmd->size =
+            wlan_cpu_to_le16(key_param_len + sizeof(pkey_material->action) +
+                             S_DS_GEN + sizeof(MrvlIEtypes_MacAddr_t));
+        goto done;
+    }
+    if (pkey->key_len == WPA_AES_KEY_LEN) {
+        PRINTM(MCMND, "WPA_AES\n");
+        pkey_material->key_param_set.key_type_id =
+            wlan_cpu_to_le16(KEY_TYPE_ID_AES);
+        if (cmd_oid == KEY_INFO_ENABLED)
+            pkey_material->key_param_set.key_info =
+                wlan_cpu_to_le16(KEY_INFO_AES_ENABLED);
+        else
+            pkey_material->key_param_set.key_info =
+                !(wlan_cpu_to_le16(KEY_INFO_AES_ENABLED));
+
+        if (memcmp(pmpriv->adapter, pkey->mac_addr, bc_mac, sizeof(bc_mac)))    /* AES
+                                                                                   pairwise
+                                                                                   key:
+                                                                                   unicast
+                                                                                 */
+            pkey_material->key_param_set.key_info |=
+                wlan_cpu_to_le16(KEY_INFO_AES_UNICAST);
+        else                    /* AES group key: multicast */
+            pkey_material->key_param_set.key_info |=
+                wlan_cpu_to_le16(KEY_INFO_AES_MCAST);
+    } else if (pkey->key_len == WPA_TKIP_KEY_LEN) {
+        PRINTM(MCMND, "WPA_TKIP\n");
+        pkey_material->key_param_set.key_type_id =
+            wlan_cpu_to_le16(KEY_TYPE_ID_TKIP);
+        pkey_material->key_param_set.key_info =
+            wlan_cpu_to_le16(KEY_INFO_TKIP_ENABLED);
+
+        if (memcmp(pmpriv->adapter, pkey->mac_addr, bc_mac, sizeof(bc_mac)))    /* TKIP
+                                                                                   pairwise
+                                                                                   key:
+                                                                                   unicast
+                                                                                 */
+            pkey_material->key_param_set.key_info |=
+                wlan_cpu_to_le16(KEY_INFO_TKIP_UNICAST);
+        else                    /* TKIP group key: multicast */
+            pkey_material->key_param_set.key_info |=
+                wlan_cpu_to_le16(KEY_INFO_TKIP_MCAST);
+    }
+
+    if (pkey_material->key_param_set.key_type_id) {
+        pkey_material->key_param_set.type =
+            wlan_cpu_to_le16(TLV_TYPE_KEY_MATERIAL);
+        pkey_material->key_param_set.key_len = wlan_cpu_to_le16(pkey->key_len);
+        memcpy(pmpriv->adapter, pkey_material->key_param_set.key,
+               pkey->key_material, pkey->key_len);
+        pkey_material->key_param_set.length =
+            wlan_cpu_to_le16((t_u16) pkey->key_len + KEYPARAMSET_FIXED_LEN);
+        key_param_len =
+            (pkey->key_len + KEYPARAMSET_FIXED_LEN) +
+            sizeof(MrvlIEtypesHeader_t);
+
+        tlv =
+            (MrvlIEtypes_MacAddr_t *) ((t_u8 *) & pkey_material->key_param_set +
+                                       key_param_len);
+        tlv->header.type = wlan_cpu_to_le16(TLV_TYPE_STA_MAC_ADDRESS);
+        tlv->header.len = wlan_cpu_to_le16(MLAN_MAC_ADDR_LENGTH);
+        memcpy(pmpriv->adapter, tlv->mac, pkey->mac_addr, MLAN_MAC_ADDR_LENGTH);
+        cmd->size =
+            wlan_cpu_to_le16(key_param_len + sizeof(pkey_material->action) +
+                             S_DS_GEN + sizeof(MrvlIEtypes_MacAddr_t));
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function handles the command response of sta_list
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+wlan_uap_ret_sta_list(IN pmlan_private pmpriv,
+                      IN HostCmd_DS_COMMAND * resp,
+                      IN mlan_ioctl_req * pioctl_buf)
+{
+    HostCmd_DS_STA_LIST *sta_list =
+        (HostCmd_DS_STA_LIST *) & resp->params.sta_list;
+    mlan_ds_get_info *info;
+    MrvlIEtypes_sta_info_t *tlv = MNULL;
+    t_u8 i = 0;
+
+    ENTER();
+    if (pioctl_buf) {
+        info = (mlan_ds_get_info *) pioctl_buf->pbuf;
+        info->param.sta_list.sta_count = wlan_le16_to_cpu(sta_list->sta_count);
+        tlv =
+            (MrvlIEtypes_sta_info_t *) ((t_u8 *) sta_list +
+                                        sizeof(HostCmd_DS_STA_LIST));
+        info->param.sta_list.sta_count =
+            MIN(info->param.sta_list.sta_count, MAX_NUM_CLIENTS);
+        for (i = 0; i < info->param.sta_list.sta_count; i++) {
+            memcpy(pmpriv->adapter, info->param.sta_list.info[i].mac_address,
+                   tlv->mac_address, MLAN_MAC_ADDR_LENGTH);
+            info->param.sta_list.info[i].power_mfg_status =
+                tlv->power_mfg_status;
+            info->param.sta_list.info[i].rssi = tlv->rssi;
+            tlv++;
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function will search for the specific ie
+ *
+ *
+ *  @param priv    A pointer to mlan_private
+ *  @param pevent  A pointer to event buf
+ *  @param sta_ptr A pointer to sta_node
+ *
+ *  @return	       N/A
+ */
+static void
+wlan_check_sta_capability(pmlan_private priv, pmlan_buffer pevent,
+                          sta_node * sta_ptr)
+{
+    t_u16 tlv_type, tlv_len;
+    t_u16 frame_control, frame_sub_type = 0;
+    t_u8 *assoc_req_ie = MNULL;
+    t_u8 ie_len = 0, assoc_ie_len = 0;
+    IEEEtypes_HTCap_t *pht_cap = MNULL;
+    int tlv_buf_left = pevent->data_len - ASSOC_EVENT_FIX_SIZE;
+    MrvlIEtypesHeader_t *tlv = (MrvlIEtypesHeader_t *)
+        (pevent->pbuf + pevent->data_offset + ASSOC_EVENT_FIX_SIZE);
+    MrvlIETypes_MgmtFrameSet_t *mgmt_tlv = MNULL;
+
+    ENTER();
+    while (tlv_buf_left >= (int) sizeof(MrvlIEtypesHeader_t)) {
+        tlv_type = wlan_le16_to_cpu(tlv->type);
+        tlv_len = wlan_le16_to_cpu(tlv->len);
+        if ((sizeof(MrvlIEtypesHeader_t) + tlv_len) >
+            (unsigned int) tlv_buf_left) {
+            PRINTM(MERROR, "wrong tlv: tlvLen=%d, tlvBufLeft=%d\n", tlv_len,
+                   tlv_buf_left);
+            break;
+        }
+        if (tlv_type == TLV_TYPE_UAP_MGMT_FRAME) {
+            mgmt_tlv = (MrvlIETypes_MgmtFrameSet_t *) tlv;
+            memcpy(priv->adapter, &frame_control,
+                   (t_u8 *) & (mgmt_tlv->frame_control), sizeof(frame_control));
+            frame_sub_type = IEEE80211_GET_FC_MGMT_FRAME_SUBTYPE(frame_control);
+            if ((mgmt_tlv->frame_control.type == 0) &&
+                ((frame_sub_type == SUBTYPE_ASSOC_REQUEST) ||
+                 (frame_sub_type == SUBTYPE_REASSOC_REQUEST))) {
+
+                if (frame_sub_type == SUBTYPE_ASSOC_REQUEST)
+                    assoc_ie_len = sizeof(IEEEtypes_AssocRqst_t);
+                else if (frame_sub_type == SUBTYPE_REASSOC_REQUEST)
+                    assoc_ie_len = sizeof(IEEEtypes_ReAssocRqst_t);
+
+                ie_len = tlv_len - sizeof(IEEEtypes_FrameCtl_t) - assoc_ie_len;
+                assoc_req_ie =
+                    (t_u8 *) tlv + sizeof(MrvlIETypes_MgmtFrameSet_t) +
+                    assoc_ie_len;
+                pht_cap =
+                    (IEEEtypes_HTCap_t *) wlan_get_specific_ie(priv,
+                                                               assoc_req_ie,
+                                                               ie_len,
+                                                               HT_CAPABILITY);
+                if (pht_cap) {
+                    PRINTM(MCMND, "STA supports 11n\n");
+                    sta_ptr->is_11n_enabled = MTRUE;
+                    if (GETHT_MAXAMSDU(pht_cap->ht_cap.ht_cap_info))
+                        sta_ptr->max_amsdu = MLAN_TX_DATA_BUF_SIZE_8K;
+                    else
+                        sta_ptr->max_amsdu = MLAN_TX_DATA_BUF_SIZE_4K;
+                } else {
+                    PRINTM(MCMND, "STA doesn't support 11n\n");
+                }
+                break;
+            }
+        }
+        tlv_buf_left -= (sizeof(MrvlIEtypesHeader_t) + tlv_len);
+        tlv =
+            (MrvlIEtypesHeader_t *) ((t_u8 *) tlv + tlv_len +
+                                     sizeof(MrvlIEtypesHeader_t));
+    }
+    LEAVE();
+
+    return;
+}
+
+/** Fixed size of bss start event */
+#define BSS_START_EVENT_FIX_SIZE    12
+
+/**
+ *  @brief This function will search for the specific ie
+ *
+ *
+ *  @param priv    A pointer to mlan_private
+ *  @param pevent  A pointer to event buf
+ *
+ *  @return	       N/A
+ */
+static void
+wlan_check_uap_capability(pmlan_private priv, pmlan_buffer pevent)
+{
+    t_u16 tlv_type, tlv_len;
+    int tlv_buf_left = pevent->data_len - BSS_START_EVENT_FIX_SIZE;
+    MrvlIEtypesHeader_t *tlv =
+        (MrvlIEtypesHeader_t *) (pevent->pbuf + pevent->data_offset +
+                                 BSS_START_EVENT_FIX_SIZE);
+    const t_u8 wmm_oui[4] = { 0x00, 0x50, 0xf2, 0x02 };
+    IEEEtypes_WmmParameter_t *pWmmParamIe = MNULL;
+    priv->wmm_enabled = MFALSE;
+    priv->pkt_fwd = MFALSE;
+    priv->is_11n_enabled = MFALSE;
+
+    ENTER();
+
+    while (tlv_buf_left >= (int) sizeof(MrvlIEtypesHeader_t)) {
+        tlv_type = wlan_le16_to_cpu(tlv->type);
+        tlv_len = wlan_le16_to_cpu(tlv->len);
+        if ((sizeof(MrvlIEtypesHeader_t) + tlv_len) >
+            (unsigned int) tlv_buf_left) {
+            PRINTM(MERROR, "wrong tlv: tlvLen=%d, tlvBufLeft=%d\n", tlv_len,
+                   tlv_buf_left);
+            break;
+        }
+        if (tlv_type == HT_CAPABILITY) {
+            DBG_HEXDUMP(MCMD_D, "HT_CAP tlv", tlv,
+                        tlv_len + sizeof(MrvlIEtypesHeader_t));
+            priv->is_11n_enabled = MTRUE;
+        }
+        if (tlv_type == VENDOR_SPECIFIC_221) {
+            if (!memcmp
+                (priv->adapter, (t_u8 *) tlv + sizeof(MrvlIEtypesHeader_t),
+                 wmm_oui, sizeof(wmm_oui))) {
+                DBG_HEXDUMP(MCMD_D, "wmm ie tlv", tlv,
+                            tlv_len + sizeof(MrvlIEtypesHeader_t));
+                priv->wmm_enabled = MFALSE;
+                wlan_wmm_setup_ac_downgrade(priv);
+                priv->wmm_enabled = MTRUE;
+                pWmmParamIe = (IEEEtypes_WmmParameter_t *) ((t_u8 *) tlv + 2);
+                pWmmParamIe->vend_hdr.len = (t_u8) tlv_len;
+                pWmmParamIe->vend_hdr.element_id = WMM_IE;
+                wlan_wmm_setup_queue_priorities(priv, pWmmParamIe);
+            }
+        }
+        if (tlv_type == TLV_TYPE_UAP_PKT_FWD_CTL) {
+            DBG_HEXDUMP(MCMD_D, "pkt_fwd tlv", tlv,
+                        tlv_len + sizeof(MrvlIEtypesHeader_t));
+            priv->pkt_fwd = *((t_u8 *) tlv + sizeof(MrvlIEtypesHeader_t));
+            PRINTM(MCMND, "pkt_fwd FW: 0x%x\n", priv->pkt_fwd);
+            if (priv->pkt_fwd & PKT_FWD_FW_BIT)
+                priv->pkt_fwd = MFALSE;
+            else
+                priv->pkt_fwd |= PKT_FWD_ENABLE_BIT;
+            PRINTM(MCMND, "pkt_fwd DRV: 0x%x\n", priv->pkt_fwd);
+        }
+        tlv_buf_left -= (sizeof(MrvlIEtypesHeader_t) + tlv_len);
+        tlv =
+            (MrvlIEtypesHeader_t *) ((t_u8 *) tlv + tlv_len +
+                                     sizeof(MrvlIEtypesHeader_t));
+    }
+    if (priv->wmm_enabled == MFALSE) {
+        /* Since WMM is not enabled, setup the queues with the defaults */
+        wlan_wmm_setup_queues(priv);
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function will update WAPI PN in statation assoc event
+ *
+ *
+ *  @param priv    A pointer to mlan_private
+ *  @param pevent  A pointer to event buf
+ *
+ *  @return	       MFALSE
+ */
+static t_u32
+wlan_update_wapi_info_tlv(pmlan_private priv, pmlan_buffer pevent)
+{
+    t_u32 ret = MFALSE;
+    t_u16 tlv_type, tlv_len;
+    t_u32 tx_pn[4];
+    t_u32 i = 0;
+    int tlv_buf_left = pevent->data_len - ASSOC_EVENT_FIX_SIZE;
+    MrvlIEtypesHeader_t *tlv = (MrvlIEtypesHeader_t *)
+        (pevent->pbuf + pevent->data_offset + ASSOC_EVENT_FIX_SIZE);
+    MrvlIEtypes_wapi_info_t *wapi_tlv = MNULL;
+
+    ENTER();
+    while (tlv_buf_left >= (int) sizeof(MrvlIEtypesHeader_t)) {
+        tlv_type = wlan_le16_to_cpu(tlv->type);
+        tlv_len = wlan_le16_to_cpu(tlv->len);
+        if ((sizeof(MrvlIEtypesHeader_t) + tlv_len) >
+            (unsigned int) tlv_buf_left) {
+            PRINTM(MERROR, "wrong tlv: tlvLen=%d, tlvBufLeft=%d\n", tlv_len,
+                   tlv_buf_left);
+            break;
+        }
+        if (tlv_type == TLV_TYPE_AP_WAPI_INFO) {
+            wapi_tlv = (MrvlIEtypes_wapi_info_t *) tlv;
+            DBG_HEXDUMP(MCMD_D, "Fw:multicast_PN", wapi_tlv->multicast_PN,
+                        PN_SIZE);
+            memcpy(priv->adapter, (t_u8 *) tx_pn, wapi_tlv->multicast_PN,
+                   PN_SIZE);
+            for (i = 0; i < 4; i++)
+                tx_pn[i] = mlan_ntohl(tx_pn[i]);
+            memcpy(priv->adapter, wapi_tlv->multicast_PN, (t_u8 *) tx_pn,
+                   PN_SIZE);
+            DBG_HEXDUMP(MCMD_D, "Host:multicast_PN", wapi_tlv->multicast_PN,
+                        PN_SIZE);
+            break;
+        }
+        tlv_buf_left -= (sizeof(MrvlIEtypesHeader_t) + tlv_len);
+        tlv =
+            (MrvlIEtypesHeader_t *) ((t_u8 *) tlv + tlv_len +
+                                     sizeof(MrvlIEtypesHeader_t));
+    }
+    LEAVE();
+
+    return ret;
+}
+
+/**
+ *  @brief This function send sta_assoc_event to moal
+ *  	   payload with sta mac address and assoc ie.
+ *
+ *  @param priv    A pointer to mlan_private
+ *  @param pevent  A pointer to mlan_event buffer
+ *  @param pbuf    A pointer to mlan_buffer which has event content.
+ *
+ *  @return	       MFALSE
+ */
+static t_u32
+wlan_process_sta_assoc_event(pmlan_private priv, mlan_event * pevent,
+                             pmlan_buffer pmbuf)
+{
+    t_u32 ret = MFALSE;
+    t_u16 tlv_type, tlv_len;
+    t_u16 frame_control, frame_sub_type = 0;
+    t_u8 *assoc_req_ie = MNULL;
+    t_u8 ie_len = 0, assoc_ie_len = 0;
+    int tlv_buf_left = pmbuf->data_len - ASSOC_EVENT_FIX_SIZE;
+    MrvlIEtypesHeader_t *tlv = (MrvlIEtypesHeader_t *)
+        (pmbuf->pbuf + pmbuf->data_offset + ASSOC_EVENT_FIX_SIZE);
+    MrvlIETypes_MgmtFrameSet_t *mgmt_tlv = MNULL;
+
+    ENTER();
+    pevent->event_id = MLAN_EVENT_ID_UAP_FW_STA_CONNECT;
+    pevent->bss_index = priv->bss_index;
+    pevent->event_len = MLAN_MAC_ADDR_LENGTH;
+    memcpy(priv->adapter, pevent->event_buf,
+           pmbuf->pbuf + pmbuf->data_offset + 6, pevent->event_len);
+    while (tlv_buf_left >= (int) sizeof(MrvlIEtypesHeader_t)) {
+        tlv_type = wlan_le16_to_cpu(tlv->type);
+        tlv_len = wlan_le16_to_cpu(tlv->len);
+        if ((sizeof(MrvlIEtypesHeader_t) + tlv_len) >
+            (unsigned int) tlv_buf_left) {
+            PRINTM(MERROR, "wrong tlv: tlvLen=%d, tlvBufLeft=%d\n", tlv_len,
+                   tlv_buf_left);
+            break;
+        }
+        if (tlv_type == TLV_TYPE_UAP_MGMT_FRAME) {
+            mgmt_tlv = (MrvlIETypes_MgmtFrameSet_t *) tlv;
+            memcpy(priv->adapter, &frame_control,
+                   (t_u8 *) & (mgmt_tlv->frame_control), sizeof(frame_control));
+            frame_sub_type = IEEE80211_GET_FC_MGMT_FRAME_SUBTYPE(frame_control);
+            if ((mgmt_tlv->frame_control.type == 0) &&
+                ((frame_sub_type == SUBTYPE_ASSOC_REQUEST) ||
+                 (frame_sub_type == SUBTYPE_REASSOC_REQUEST))) {
+
+                if (frame_sub_type == SUBTYPE_ASSOC_REQUEST)
+                    assoc_ie_len = sizeof(IEEEtypes_AssocRqst_t);
+                else if (frame_sub_type == SUBTYPE_REASSOC_REQUEST)
+                    assoc_ie_len = sizeof(IEEEtypes_ReAssocRqst_t);
+
+                ie_len = tlv_len - sizeof(IEEEtypes_FrameCtl_t) - assoc_ie_len;
+                assoc_req_ie =
+                    (t_u8 *) tlv + sizeof(MrvlIETypes_MgmtFrameSet_t) +
+                    assoc_ie_len;
+                memcpy(priv->adapter, pevent->event_buf + pevent->event_len,
+                       assoc_req_ie, ie_len);
+                pevent->event_len += ie_len;
+                break;
+            }
+        }
+        tlv_buf_left -= (sizeof(MrvlIEtypesHeader_t) + tlv_len);
+        tlv =
+            (MrvlIEtypesHeader_t *) ((t_u8 *) tlv + tlv_len +
+                                     sizeof(MrvlIEtypesHeader_t));
+    }
+    PRINTM(MEVENT, "STA assoc event len=%d\n", pevent->event_len);
+    DBG_HEXDUMP(MCMD_D, "STA assoc event", pevent->event_buf,
+                pevent->event_len);
+    wlan_recv_event(priv, pevent->event_id, pevent);
+    LEAVE();
+    return ret;
+}
+
+/********************************************************
+    Global Functions
+********************************************************/
+/**
+ *  @brief This function prepare the command before sending to firmware.
+ *
+ *  @param priv       A pointer to mlan_private structure
+ *  @param cmd_no       Command number
+ *  @param cmd_action   Command action: GET or SET
+ *  @param cmd_oid      Cmd oid: treated as sub command
+ *  @param pioctl_buf   A pointer to MLAN IOCTL Request buffer
+ *  @param pdata_buf    A pointer to information buffer
+ *  @param pcmd_buf      A pointer to cmd buf
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ops_uap_prepare_cmd(IN t_void * priv,
+                         IN t_u16 cmd_no,
+                         IN t_u16 cmd_action,
+                         IN t_u32 cmd_oid,
+                         IN t_void * pioctl_buf,
+                         IN t_void * pdata_buf, IN t_void * pcmd_buf)
+{
+    HostCmd_DS_COMMAND *cmd_ptr = (HostCmd_DS_COMMAND *) pcmd_buf;
+    mlan_private *pmpriv = (mlan_private *) priv;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_ioctl_req pioctl_req = (mlan_ioctl_req *) pioctl_buf;
+
+    ENTER();
+
+    /* Prepare command */
+    switch (cmd_no) {
+    case HostCmd_CMD_SOFT_RESET:
+    case HOST_CMD_APCMD_BSS_STOP:
+    case HOST_CMD_APCMD_BSS_START:
+    case HOST_CMD_APCMD_SYS_INFO:
+    case HOST_CMD_APCMD_SYS_RESET:
+    case HOST_CMD_APCMD_STA_LIST:
+        cmd_ptr->command = wlan_cpu_to_le16(cmd_no);
+        cmd_ptr->size = wlan_cpu_to_le16(S_DS_GEN);
+        break;
+    case HOST_CMD_APCMD_SYS_CONFIGURE:
+        ret = wlan_uap_cmd_sys_configure(pmpriv, cmd_ptr, cmd_action,
+                                         (pmlan_ioctl_req) pioctl_buf,
+                                         pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_PS_MODE_ENH:
+        ret =
+            wlan_cmd_enh_power_mode(pmpriv, cmd_ptr, cmd_action,
+                                    (t_u16) cmd_oid, pdata_buf);
+        break;
+    case HostCmd_CMD_SDIO_GPIO_INT_CONFIG:
+        ret = wlan_cmd_sdio_gpio_int(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_FUNC_INIT:
+        if (pmpriv->adapter->hw_status == WlanHardwareStatusReset)
+            pmpriv->adapter->hw_status = WlanHardwareStatusInitializing;
+        cmd_ptr->command = wlan_cpu_to_le16(cmd_no);
+        cmd_ptr->size = wlan_cpu_to_le16(S_DS_GEN);
+        break;
+    case HostCmd_CMD_FUNC_SHUTDOWN:
+        pmpriv->adapter->hw_status = WlanHardwareStatusReset;
+        cmd_ptr->command = wlan_cpu_to_le16(cmd_no);
+        cmd_ptr->size = wlan_cpu_to_le16(S_DS_GEN);
+        break;
+    case HostCmd_CMD_CFG_DATA:
+        ret = wlan_cmd_cfg_data(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_SNMP_MIB:
+        ret = wlan_uap_cmd_snmp_mib(pmpriv, cmd_ptr, cmd_action, cmd_oid,
+                                    (pmlan_ioctl_req) pioctl_buf, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11D_DOMAIN_INFO:
+        ret = wlan_cmd_802_11d_domain_info(pmpriv, cmd_ptr, cmd_action);
+        break;
+    case HostCmd_CMD_CHAN_REPORT_REQUEST:
+        ret = wlan_11h_cmd_process(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HOST_CMD_APCMD_STA_DEAUTH:
+        ret = wlan_uap_cmd_sta_deauth(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_KEY_MATERIAL:
+        ret =
+            wlan_uap_cmd_key_material(pmpriv, cmd_ptr, cmd_action, cmd_oid,
+                                      pdata_buf);
+        break;
+    case HostCmd_CMD_GET_HW_SPEC:
+        ret = wlan_cmd_get_hw_spec(pmpriv, cmd_ptr);
+        break;
+    case HostCmd_CMD_802_11_HS_CFG_ENH:
+        ret =
+            wlan_uap_cmd_802_11_hs_cfg(pmpriv, cmd_ptr, cmd_action,
+                                       (hs_config_param *) pdata_buf);
+        break;
+    case HostCmd_CMD_RECONFIGURE_TX_BUFF:
+        ret = wlan_cmd_recfg_tx_buf(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_AMSDU_AGGR_CTRL:
+        ret = wlan_cmd_amsdu_aggr_ctrl(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_11N_ADDBA_REQ:
+        ret = wlan_cmd_11n_addba_req(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_11N_DELBA:
+        ret = wlan_cmd_11n_delba(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_11N_ADDBA_RSP:
+        ret = wlan_cmd_11n_addba_rspgen(pmpriv, cmd_ptr, pdata_buf);
+        break;
+    case HostCmd_CMD_TX_BF_CFG:
+        ret = wlan_cmd_tx_bf_cfg(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+#if defined(WIFI_DIRECT_SUPPORT)
+    case HostCmd_CMD_SET_BSS_MODE:
+        cmd_ptr->command = wlan_cpu_to_le16(cmd_no);
+        if (pdata_buf)
+            cmd_ptr->params.bss_mode.con_type = *(t_u8 *) pdata_buf;
+        else
+            cmd_ptr->params.bss_mode.con_type = BSS_MODE_WIFIDIRECT_GO;
+        cmd_ptr->size =
+            wlan_cpu_to_le16(sizeof(HostCmd_DS_SET_BSS_MODE) + S_DS_GEN);
+        ret = MLAN_STATUS_SUCCESS;
+        break;
+#endif
+    case HostCmd_CMD_VERSION_EXT:
+        cmd_ptr->command = wlan_cpu_to_le16(cmd_no);
+        cmd_ptr->params.verext.version_str_sel =
+            (t_u8) (*((t_u32 *) pdata_buf));
+        cmd_ptr->size =
+            wlan_cpu_to_le16(sizeof(HostCmd_DS_VERSION_EXT) + S_DS_GEN);
+        ret = MLAN_STATUS_SUCCESS;
+        break;
+    case HostCmd_CMD_RX_MGMT_IND:
+        cmd_ptr->command = wlan_cpu_to_le16(cmd_no);
+        cmd_ptr->params.rx_mgmt_ind.action = wlan_cpu_to_le16(cmd_action);
+        cmd_ptr->params.rx_mgmt_ind.mgmt_subtype_mask =
+            (t_u32) (*((t_u32 *) pdata_buf));
+        cmd_ptr->size = wlan_cpu_to_le16(sizeof(t_u32) + S_DS_GEN);
+        break;
+    case HostCmd_CMD_CFG_TX_DATA_PAUSE:
+        ret = wlan_uap_cmd_txdatapause(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_RADIO_CONTROL:
+        ret =
+            wlan_cmd_802_11_radio_control(pmpriv, cmd_ptr, cmd_action,
+                                          pdata_buf);
+        break;
+    case HostCmd_CMD_TX_RATE_CFG:
+        ret = wlan_cmd_tx_rate_cfg(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HostCmd_CMD_802_11_TX_RATE_QUERY:
+        cmd_ptr->command = wlan_cpu_to_le16(HostCmd_CMD_802_11_TX_RATE_QUERY);
+        cmd_ptr->size =
+            wlan_cpu_to_le16(sizeof(HostCmd_TX_RATE_QUERY) + S_DS_GEN);
+        pmpriv->tx_rate = 0;
+        ret = MLAN_STATUS_SUCCESS;
+        break;
+#ifdef WIFI_DIRECT_SUPPORT
+    case HostCmd_CMD_802_11_REMAIN_ON_CHANNEL:
+        ret =
+            wlan_cmd_remain_on_channel(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    case HOST_CMD_WIFI_DIRECT_MODE_CONFIG:
+        ret = wlan_cmd_wifi_direct_mode(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+#endif
+    case HostCmd_CMD_802_11_RF_ANTENNA:
+        ret =
+            wlan_cmd_802_11_rf_antenna(pmpriv, cmd_ptr, cmd_action, pdata_buf);
+        break;
+    default:
+        PRINTM(MERROR, "PREP_CMD: unknown command- %#x\n", cmd_no);
+        if (pioctl_req)
+            pioctl_req->status_code = MLAN_ERROR_CMD_INVALID;
+        ret = MLAN_STATUS_FAILURE;
+        break;
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function handles the AP mode command response
+ *
+ *  @param priv             A pointer to mlan_private structure
+ *  @param cmdresp_no       cmd no
+ *  @param pcmd_buf         cmdresp buf
+ *  @param pioctl           A pointer to ioctl buf
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ops_uap_process_cmdresp(IN t_void * priv,
+                             IN t_u16 cmdresp_no,
+                             IN t_void * pcmd_buf, IN t_void * pioctl)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = (mlan_private *) priv;
+    HostCmd_DS_COMMAND *resp = (HostCmd_DS_COMMAND *) pcmd_buf;
+    mlan_ioctl_req *pioctl_buf = (mlan_ioctl_req *) pioctl;
+    mlan_adapter *pmadapter = pmpriv->adapter;
+    int ctr;
+
+    ENTER();
+
+    /* If the command is not successful, cleanup and return failure */
+    if (resp->result != HostCmd_RESULT_OK) {
+        uap_process_cmdresp_error(pmpriv, resp, pioctl_buf);
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    /* Command successful, handle response */
+    switch (cmdresp_no) {
+    case HOST_CMD_APCMD_BSS_STOP:
+        pmpriv->uap_bss_started = MFALSE;
+        wlan_11h_check_update_radar_det_state(pmpriv);
+
+        if (pmpriv->adapter->state_rdh.stage == RDH_STOP_INTFS)
+            wlan_11h_radar_detected_callback((t_void *) pmpriv);
+        break;
+    case HOST_CMD_APCMD_BSS_START:
+        if (pmpriv->adapter->state_rdh.stage == RDH_RESTART_INTFS)
+            wlan_11h_radar_detected_callback((t_void *) pmpriv);
+        break;
+    case HOST_CMD_APCMD_SYS_RESET:
+        ret = wlan_uap_ret_sys_reset(pmpriv, resp, pioctl_buf);
+
+        pmpriv->uap_bss_started = MFALSE;
+        wlan_11h_check_update_radar_det_state(pmpriv);
+        break;
+    case HOST_CMD_APCMD_SYS_INFO:
+        break;
+    case HOST_CMD_APCMD_SYS_CONFIGURE:
+        ret = wlan_uap_ret_sys_config(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_PS_MODE_ENH:
+        ret = wlan_ret_enh_power_mode(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_SDIO_GPIO_INT_CONFIG:
+        break;
+    case HostCmd_CMD_FUNC_INIT:
+    case HostCmd_CMD_FUNC_SHUTDOWN:
+        break;
+    case HostCmd_CMD_802_11_SNMP_MIB:
+        ret = wlan_uap_ret_snmp_mib(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11D_DOMAIN_INFO:
+        ret = wlan_ret_802_11d_domain_info(pmpriv, resp);
+        break;
+    case HostCmd_CMD_CHAN_REPORT_REQUEST:
+        ret = wlan_11h_cmdresp_process(pmpriv, resp);
+        break;
+    case HOST_CMD_APCMD_STA_DEAUTH:
+        break;
+    case HostCmd_CMD_802_11_KEY_MATERIAL:
+        break;
+    case HOST_CMD_APCMD_STA_LIST:
+        ret = wlan_uap_ret_sta_list(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_GET_HW_SPEC:
+        ret = wlan_ret_get_hw_spec(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_CFG_DATA:
+        ret = wlan_ret_cfg_data(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_HS_CFG_ENH:
+        ret = wlan_ret_802_11_hs_cfg(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_11N_ADDBA_REQ:
+        ret = wlan_ret_11n_addba_req(pmpriv, resp);
+        break;
+    case HostCmd_CMD_11N_DELBA:
+        ret = wlan_ret_11n_delba(pmpriv, resp);
+        break;
+    case HostCmd_CMD_11N_ADDBA_RSP:
+        ret = wlan_ret_11n_addba_resp(pmpriv, resp);
+        break;
+    case HostCmd_CMD_SET_BSS_MODE:
+        break;
+    case HostCmd_CMD_RECONFIGURE_TX_BUFF:
+        pmadapter->tx_buf_size =
+            (t_u16) wlan_le16_to_cpu(resp->params.tx_buf.buff_size);
+        pmadapter->tx_buf_size =
+            (pmadapter->tx_buf_size / MLAN_SDIO_BLOCK_SIZE) *
+            MLAN_SDIO_BLOCK_SIZE;
+        pmadapter->curr_tx_buf_size = pmadapter->tx_buf_size;
+        pmadapter->mp_end_port =
+            wlan_le16_to_cpu(resp->params.tx_buf.mp_end_port);
+        pmadapter->mp_data_port_mask = DATA_PORT_MASK;
+
+        for (ctr = 1; ctr <= MAX_PORT - pmadapter->mp_end_port; ctr++) {
+            pmadapter->mp_data_port_mask &= ~(1 << (MAX_PORT - ctr));
+        }
+        pmadapter->curr_wr_port = 1;
+        PRINTM(MCMND, "end port %d, data port mask %x\n",
+               wlan_le16_to_cpu(resp->params.tx_buf.mp_end_port),
+               pmadapter->mp_data_port_mask);
+        PRINTM(MCMND, "max_tx_buf_size=%d, tx_buf_size=%d\n",
+               pmadapter->max_tx_buf_size, pmadapter->tx_buf_size);
+        break;
+    case HostCmd_CMD_AMSDU_AGGR_CTRL:
+        ret = wlan_ret_amsdu_aggr_ctrl(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_TX_BF_CFG:
+        ret = wlan_ret_tx_bf_cfg(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_VERSION_EXT:
+        ret = wlan_ret_ver_ext(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_RX_MGMT_IND:
+        ret = wlan_ret_rx_mgmt_ind(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_CFG_TX_DATA_PAUSE:
+        ret = wlan_uap_ret_txdatapause(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_RADIO_CONTROL:
+        ret = wlan_ret_802_11_radio_control(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_TX_RATE_CFG:
+        ret = wlan_ret_tx_rate_cfg(pmpriv, resp, pioctl_buf);
+        break;
+    case HostCmd_CMD_802_11_TX_RATE_QUERY:
+        ret = wlan_ret_802_11_tx_rate_query(pmpriv, resp, pioctl_buf);
+        break;
+#ifdef WIFI_DIRECT_SUPPORT
+    case HostCmd_CMD_802_11_REMAIN_ON_CHANNEL:
+        ret = wlan_ret_remain_on_channel(pmpriv, resp, pioctl_buf);
+        break;
+    case HOST_CMD_WIFI_DIRECT_MODE_CONFIG:
+        ret = wlan_ret_wifi_direct_mode(pmpriv, resp, pioctl_buf);
+        break;
+#endif
+    case HostCmd_CMD_802_11_RF_ANTENNA:
+        ret = wlan_ret_802_11_rf_antenna(pmpriv, resp, pioctl_buf);
+        break;
+    default:
+        PRINTM(MERROR, "CMD_RESP: Unknown command response %#x\n",
+               resp->command);
+        if (pioctl_buf)
+            pioctl_buf->status_code = MLAN_ERROR_CMD_RESP_FAIL;
+        break;
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function handles events generated by firmware
+ *
+ *  @param priv		A pointer to mlan_private structure
+ *
+ *  @return		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ops_uap_process_event(IN t_void * priv)
+{
+    pmlan_private pmpriv = (pmlan_private) priv;
+    pmlan_adapter pmadapter = pmpriv->adapter;
+    pmlan_callbacks pcb = &pmadapter->callbacks;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u32 eventcause = pmadapter->event_cause;
+    pmlan_buffer pmbuf = pmadapter->pmlan_buffer_event;
+    t_u8 *event_buf = MNULL;
+    mlan_event *pevent = MNULL;
+    t_u8 sta_addr[MLAN_MAC_ADDR_LENGTH];
+    sta_node *sta_ptr = MNULL;
+    t_u8 i = 0;
+
+    ENTER();
+
+    /* Allocate memory for event buffer */
+    ret =
+        pcb->moal_malloc(pmadapter->pmoal_handle, MAX_EVENT_SIZE, MLAN_MEM_DEF,
+                         &event_buf);
+    if ((ret != MLAN_STATUS_SUCCESS) || !event_buf) {
+        PRINTM(MERROR, "Could not allocate buffer for event buf\n");
+        if (pmbuf)
+            pmbuf->status_code = MLAN_ERROR_NO_MEM;
+        goto done;
+    }
+    pevent = (pmlan_event) event_buf;
+    memset(pmadapter, &pevent->event_id, 0, sizeof(pevent->event_id));
+
+    if (eventcause != EVENT_PS_SLEEP && eventcause != EVENT_PS_AWAKE &&
+        pmbuf->data_len > sizeof(eventcause))
+        DBG_HEXDUMP(MEVT_D, "EVENT", pmbuf->pbuf + pmbuf->data_offset,
+                    pmbuf->data_len);
+
+    switch (eventcause) {
+    case EVENT_MICRO_AP_BSS_START:
+        PRINTM(MEVENT, "EVENT: MICRO_AP_BSS_START\n");
+        pmpriv->uap_bss_started = MTRUE;
+        memcpy(pmadapter, pmpriv->curr_addr, pmadapter->event_body + 2,
+               MLAN_MAC_ADDR_LENGTH);
+        pevent->event_id = MLAN_EVENT_ID_UAP_FW_BSS_START;
+        wlan_check_uap_capability(pmpriv, pmbuf);
+        break;
+    case EVENT_MICRO_AP_BSS_ACTIVE:
+        PRINTM(MEVENT, "EVENT: MICRO_AP_BSS_ACTIVE\n");
+        pmpriv->media_connected = MTRUE;
+        pevent->event_id = MLAN_EVENT_ID_UAP_FW_BSS_ACTIVE;
+        break;
+    case EVENT_MICRO_AP_BSS_IDLE:
+        PRINTM(MEVENT, "EVENT: MICRO_AP_BSS_IDLE\n");
+        pevent->event_id = MLAN_EVENT_ID_UAP_FW_BSS_IDLE;
+        pmpriv->media_connected = MFALSE;
+        wlan_clean_txrx(pmpriv);
+        wlan_delete_station_list(pmpriv);
+        break;
+    case EVENT_PS_AWAKE:
+        PRINTM(MINFO, "EVENT: AWAKE \n");
+        PRINTM(MEVENT, "||");
+        /* Handle unexpected PS AWAKE event */
+        if (pmadapter->ps_state == PS_STATE_SLEEP_CFM)
+            break;
+        pmadapter->pm_wakeup_card_req = MFALSE;
+        pmadapter->pm_wakeup_fw_try = MFALSE;
+        pmadapter->ps_state = PS_STATE_AWAKE;
+        break;
+    case EVENT_PS_SLEEP:
+        PRINTM(MINFO, "EVENT: SLEEP\n");
+        PRINTM(MEVENT, "__");
+        /* Handle unexpected PS SLEEP event */
+        if (pmadapter->ps_state == PS_STATE_SLEEP_CFM)
+            break;
+        pmadapter->ps_state = PS_STATE_PRE_SLEEP;
+        wlan_check_ps_cond(pmadapter);
+        break;
+    case EVENT_MICRO_AP_STA_ASSOC:
+        PRINTM(MEVENT, "EVENT: MICRO_AP_STA_ASSOC\n");
+        wlan_process_sta_assoc_event(pmpriv, pevent, pmbuf);
+        memcpy(pmadapter, sta_addr, pmadapter->event_body + 2,
+               MLAN_MAC_ADDR_LENGTH);
+        sta_ptr = wlan_add_station_entry(pmpriv, sta_addr);
+        if (pmpriv->is_11n_enabled) {
+            wlan_check_sta_capability(pmpriv, pmbuf, sta_ptr);
+            for (i = 0; i < MAX_NUM_TID; i++) {
+                if (sta_ptr->is_11n_enabled)
+                    sta_ptr->ampdu_sta[i] = pmpriv->aggr_prio_tbl[i].ampdu_user;
+                else
+                    sta_ptr->ampdu_sta[i] = BA_STREAM_NOT_ALLOWED;
+            }
+            memset(pmadapter, sta_ptr->rx_seq, 0xff, sizeof(sta_ptr->rx_seq));
+        }
+        if (pmpriv->sec_info.wapi_enabled)
+            wlan_update_wapi_info_tlv(pmpriv, pmbuf);
+        pevent->event_id = MLAN_EVENT_ID_DRV_PASSTHRU;
+        break;
+    case EVENT_MICRO_AP_STA_DEAUTH:
+        PRINTM(MEVENT, "EVENT: MICRO_AP_STA_DEAUTH\n");
+        pevent->event_id = MLAN_EVENT_ID_UAP_FW_STA_DISCONNECT;
+        pevent->bss_index = pmpriv->bss_index;
+        pevent->event_len = pmbuf->data_len - 4;
+        /* skip event length field */
+        memcpy(pmadapter, (t_u8 *) pevent->event_buf,
+               pmbuf->pbuf + pmbuf->data_offset + 4, pevent->event_len);
+        wlan_recv_event(pmpriv, pevent->event_id, pevent);
+        memcpy(pmadapter, sta_addr, pmadapter->event_body + 2,
+               MLAN_MAC_ADDR_LENGTH);
+        if (pmpriv->is_11n_enabled) {
+            wlan_cleanup_reorder_tbl(pmpriv, sta_addr);
+            pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                                pmpriv->wmm.ra_list_spinlock);
+            wlan_11n_cleanup_txbastream_tbl(pmpriv, sta_addr);
+            pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                                  pmpriv->wmm.ra_list_spinlock);
+        }
+        wlan_wmm_delete_peer_ralist(pmpriv, sta_addr);
+        wlan_delete_station_entry(pmpriv, sta_addr);
+        pevent->event_id = MLAN_EVENT_ID_DRV_PASSTHRU;
+        break;
+    case EVENT_HS_ACT_REQ:
+        PRINTM(MEVENT, "EVENT: HS_ACT_REQ\n");
+        ret =
+            wlan_prepare_cmd(pmpriv, HostCmd_CMD_802_11_HS_CFG_ENH, 0, 0, MNULL,
+                             MNULL);
+        break;
+    case EVENT_ADDBA:
+        PRINTM(MEVENT, "EVENT: ADDBA Request\n");
+        wlan_prepare_cmd(pmpriv, HostCmd_CMD_11N_ADDBA_RSP,
+                         HostCmd_ACT_GEN_SET, 0, MNULL, pmadapter->event_body);
+        break;
+    case EVENT_DELBA:
+        PRINTM(MEVENT, "EVENT: DELBA Request\n");
+        wlan_11n_delete_bastream(pmpriv, pmadapter->event_body);
+        break;
+    case EVENT_BA_STREAM_TIMEOUT:
+        PRINTM(MEVENT, "EVENT:  BA Stream timeout\n");
+        wlan_11n_ba_stream_timeout(pmpriv,
+                                   (HostCmd_DS_11N_BATIMEOUT *) pmadapter->
+                                   event_body);
+        break;
+    case EVENT_RXBA_SYNC:
+        PRINTM(MEVENT, "EVENT:  RXBA_SYNC\n");
+        wlan_11n_rxba_sync_event(pmpriv, pmadapter->event_body,
+                                 pmbuf->data_len - sizeof(eventcause));
+        break;
+    case EVENT_AMSDU_AGGR_CTRL:
+        PRINTM(MEVENT, "EVENT:  AMSDU_AGGR_CTRL %d\n",
+               *(t_u16 *) pmadapter->event_body);
+        pmadapter->tx_buf_size =
+            MIN(pmadapter->curr_tx_buf_size,
+                wlan_le16_to_cpu(*(t_u16 *) pmadapter->event_body));
+        PRINTM(MEVENT, "tx_buf_size %d\n", pmadapter->tx_buf_size);
+        break;
+    case EVENT_TX_DATA_PAUSE:
+        PRINTM(MEVENT, "EVENT: TX_DATA_PAUSE\n");
+        wlan_process_tx_pause_event(priv, pmbuf);
+        break;
+    case EVENT_RADAR_DETECTED:
+        PRINTM(MEVENT, "EVENT: Radar Detected\n");
+
+        /* Send as passthru first, this event can cause other events */
+        memset(pmadapter, event_buf, 0x00, MAX_EVENT_SIZE);
+        pevent->bss_index = pmpriv->bss_index;
+        pevent->event_id = MLAN_EVENT_ID_DRV_PASSTHRU;
+        pevent->event_len = pmbuf->data_len;
+        memcpy(pmadapter, (t_u8 *) pevent->event_buf,
+               pmbuf->pbuf + pmbuf->data_offset, pevent->event_len);
+        wlan_recv_event(pmpriv, pevent->event_id, pevent);
+        pevent->event_id = 0;   // clear to avoid resending at end of fcn
+
+        if (pmadapter->state_rdh.stage == RDH_OFF) {
+            pmadapter->state_rdh.stage = RDH_CHK_INTFS;
+            wlan_11h_radar_detected_handling(pmadapter);
+        } else {
+            PRINTM(MEVENT, "Ignore Event Radar Detected - handling"
+                   " already in progress.\n");
+        }
+        break;
+    case EVENT_CHANNEL_REPORT_RDY:
+        PRINTM(MEVENT, "EVENT: Channel Report Ready\n");
+        memset(pmadapter, event_buf, 0x00, MAX_EVENT_SIZE);
+        /* Setup event buffer */
+        pevent->bss_index = pmpriv->bss_index;
+        pevent->event_id = MLAN_EVENT_ID_FW_CHANNEL_REPORT_RDY;
+        pevent->event_len = pmbuf->data_len - sizeof(eventcause);
+        /* Copy event data */
+        memcpy(pmadapter, (t_u8 *) pevent->event_buf,
+               pmbuf->pbuf + pmbuf->data_offset + sizeof(eventcause),
+               pevent->event_len);
+        /* Handle / pass event data, and free buffer */
+        ret = wlan_11h_handle_event_chanrpt_ready(pmpriv, pevent);
+
+        /* Send up this Event to unblock MOAL waitqueue */
+        wlan_recv_event(pmpriv, MLAN_EVENT_ID_DRV_MEAS_REPORT, MNULL);
+        pevent->event_id = MLAN_EVENT_ID_DRV_PASSTHRU;
+        break;
+#ifdef WIFI_DIRECT_SUPPORT
+    case EVENT_REMAIN_ON_CHANNEL_EXPIRED:
+        PRINTM(MEVENT, "EVENT: REMAIN_ON_CHANNEL_EXPIRED reason=%d\n",
+               *(t_u16 *) pmadapter->event_body);
+        pevent->event_id = MLAN_EVENT_ID_FW_REMAIN_ON_CHAN_EXPIRED;
+        break;
+#endif
+    default:
+        pevent->event_id = MLAN_EVENT_ID_DRV_PASSTHRU;
+        break;
+    }
+
+    if (pevent->event_id) {
+        pevent->bss_index = pmpriv->bss_index;
+        pevent->event_len = pmbuf->data_len;
+        memcpy(pmadapter, (t_u8 *) pevent->event_buf,
+               pmbuf->pbuf + pmbuf->data_offset, pevent->event_len);
+        wlan_recv_event(pmpriv, pevent->event_id, pevent);
+    }
+  done:
+    if (event_buf)
+        pcb->moal_mfree(pmadapter->pmoal_handle, event_buf);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief  This function issues commands to initialize firmware
+ *
+ *  @param priv     	A pointer to mlan_private structure
+ *  @param first_bss	flag for first BSS
+ *
+ *  @return		MLAN_STATUS_SUCCESS or MLAN_STATUS_PENDING or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ops_uap_init_cmd(IN t_void * priv, IN t_u8 first_bss)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = (pmlan_private) priv;
+    t_u16 last_cmd = 0;
+
+    ENTER();
+
+    if (first_bss) {
+        if (wlan_adapter_init_cmd(pmpriv->adapter) == MLAN_STATUS_FAILURE) {
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+    }
+
+    ret = wlan_prepare_cmd(pmpriv, HOST_CMD_APCMD_SYS_CONFIGURE,
+                           HostCmd_ACT_GEN_GET, 0, MNULL, MNULL);
+    if (ret) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    last_cmd = HOST_CMD_APCMD_SYS_CONFIGURE;
+    /** set last_init_cmd */
+    if (last_cmd) {
+        pmpriv->adapter->last_init_cmd = last_cmd;
+        ret = MLAN_STATUS_PENDING;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_uap_ioctl.c b/drivers/net/wireless/sd8797/mlan/mlan_uap_ioctl.c
new file mode 100644
index 000000000000..f71a222f4f48
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_uap_ioctl.c
@@ -0,0 +1,1393 @@
+/** @file mlan_uap_ioctl.c
+ *
+ *  @brief This file contains the handling of AP mode ioctls 
+ * 
+ *  Copyright (C) 2009-2011, Marvell International Ltd. 
+ *   
+ *  This software file (the "File") is distributed by Marvell International 
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+ *  (the "License").  You may use, redistribute and/or modify this File in 
+ *  accordance with the terms and conditions of the License, a copy of which 
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+ *  this warranty disclaimer.
+ */
+
+/********************************************************
+Change log:
+    02/05/2009: initial version
+********************************************************/
+
+#include "mlan.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#ifdef STA_SUPPORT
+#include "mlan_join.h"
+#endif
+#include "mlan_main.h"
+#include "mlan_uap.h"
+#include "mlan_sdio.h"
+#include "mlan_11n.h"
+#include "mlan_fw.h"
+#include "mlan_11h.h"
+
+/********************************************************
+                Global Variables
+********************************************************/
+extern t_u8 tos_to_tid_inv[];
+
+/********************************************************
+    Local Functions
+********************************************************/
+/**
+ *  @brief Stop BSS
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_uap_bss_ioctl_stop(IN pmlan_adapter pmadapter,
+                        IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    ret = wlan_prepare_cmd(pmpriv,
+                           HOST_CMD_APCMD_BSS_STOP,
+                           HostCmd_ACT_GEN_SET,
+                           0, (t_void *) pioctl_req, MNULL);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Callback to finish BSS IOCTL START
+ *  Not to be called directly to initiate bss_start
+ *
+ *  @param priv A pointer to mlan_private structure (cast from t_void*)
+ *
+ *  @return     MLAN_STATUS_PENDING --success, otherwise fail
+ *  @sa         wlan_uap_bss_ioctl_start
+ */
+static mlan_status
+wlan_uap_callback_bss_ioctl_start(IN t_void * priv)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = (mlan_private *) priv;
+    mlan_callbacks *pcb = (mlan_callbacks *) & pmpriv->adapter->callbacks;
+    wlan_uap_get_info_cb_t *puap_state_chan_cb = &pmpriv->uap_state_chan_cb;
+    t_u8 old_channel;
+
+    ENTER();
+    /* clear callback now that we're here */
+    puap_state_chan_cb->get_chan_callback = MNULL;
+
+    /*
+     * Check if the region and channel requires we check for radar.
+     */
+    if ((puap_state_chan_cb->band_config & BAND_CONFIG_5GHZ) &&
+        wlan_11h_radar_detect_required(pmpriv, puap_state_chan_cb->channel)) {
+        /* first check if channel is under NOP */
+        if (wlan_11h_is_channel_under_nop(pmpriv->adapter,
+                                          puap_state_chan_cb->channel)) {
+            /* recently we've seen radar on this channel */
+            ret = MLAN_STATUS_FAILURE;
+        }
+
+        /* Check cached radar check on the channel */
+        if (ret == MLAN_STATUS_SUCCESS)
+            ret =
+                wlan_11h_check_chan_report(pmpriv, puap_state_chan_cb->channel);
+
+        /* Found radar: try to switch to a non-dfs channel */
+        if (ret != MLAN_STATUS_SUCCESS) {
+            old_channel = puap_state_chan_cb->channel;
+            ret =
+                wlan_11h_switch_non_dfs_chan(pmpriv,
+                                             &puap_state_chan_cb->channel);
+
+            if (ret == MLAN_STATUS_SUCCESS) {
+                ret = wlan_uap_set_channel(pmpriv,
+                                           pmpriv->uap_state_chan_cb.
+                                           band_config,
+                                           puap_state_chan_cb->channel);
+                if (ret == MLAN_STATUS_SUCCESS) {
+                    PRINTM(MMSG, "Radar found on channel %d,"
+                           "switch to new channel %d.\n",
+                           old_channel, puap_state_chan_cb->channel);
+                } else {
+                    PRINTM(MMSG, "Radar found on channel %d,"
+                           " switch to new channel %d failed.\n",
+                           old_channel, puap_state_chan_cb->channel);
+                    pcb->moal_ioctl_complete(pmpriv->adapter->pmoal_handle,
+                                             puap_state_chan_cb->
+                                             pioctl_req_curr,
+                                             MLAN_STATUS_FAILURE);
+                    goto done;
+                }
+            } else {
+                PRINTM(MMSG,
+                       "Radar found on channel %d, no switch channel available.\n",
+                       old_channel);
+                /* No command sent with the ioctl, need manually signal
+                   completion */
+                pcb->moal_ioctl_complete(pmpriv->adapter->pmoal_handle,
+                                         puap_state_chan_cb->pioctl_req_curr,
+                                         MLAN_STATUS_FAILURE);
+                goto done;
+            }
+        } else {
+            PRINTM(MINFO, "No Radar found on channel %d\n",
+                   puap_state_chan_cb->channel);
+        }
+    }
+
+    /* else okay to send command: not DFS channel or no radar */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HOST_CMD_APCMD_BSS_START,
+                           HostCmd_ACT_GEN_SET,
+                           0,
+                           (t_void *) puap_state_chan_cb->pioctl_req_curr,
+                           MNULL);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+  done:
+    puap_state_chan_cb->pioctl_req_curr = MNULL;        // prevent re-use
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Start BSS
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+/**
+ *  @sa         wlan_uap_callback_bss_ioctl_start
+ */
+static mlan_status
+wlan_uap_bss_ioctl_start(IN pmlan_adapter pmadapter,
+                         IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    /* First check channel report, defer BSS_START CMD to callback. */
+    /* store params, issue command to get UAP channel, whose CMD_RESP will
+       callback remainder of bss_start handling */
+    pmpriv->uap_state_chan_cb.pioctl_req_curr = pioctl_req;
+    pmpriv->uap_state_chan_cb.get_chan_callback =
+        wlan_uap_callback_bss_ioctl_start;
+
+    ret = wlan_uap_get_channel(pmpriv);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief reset BSS
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_uap_bss_ioctl_reset(IN pmlan_adapter pmadapter,
+                         IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    t_u8 i = 0;
+
+    ENTER();
+
+    /*
+     * Reset any uap private parameters here
+     */
+    for (i = 0; i < pmadapter->max_mgmt_ie_index; i++) {
+        memset(pmadapter, &pmpriv->mgmt_ie[i], 0, sizeof(custom_ie));
+    }
+    pmpriv->add_ba_param.timeout = MLAN_DEFAULT_BLOCK_ACK_TIMEOUT;
+    pmpriv->add_ba_param.tx_win_size = MLAN_UAP_AMPDU_DEF_TXWINSIZE;
+    pmpriv->add_ba_param.rx_win_size = MLAN_UAP_AMPDU_DEF_RXWINSIZE;
+    for (i = 0; i < MAX_NUM_TID; i++) {
+        pmpriv->aggr_prio_tbl[i].ampdu_user = tos_to_tid_inv[i];
+        pmpriv->aggr_prio_tbl[i].amsdu = BA_STREAM_NOT_ALLOWED;
+        pmpriv->addba_reject[i] = ADDBA_RSP_STATUS_ACCEPT;
+    }
+
+    /* hs_configured, hs_activated are reset by main loop */
+    pmadapter->hs_cfg.conditions = HOST_SLEEP_DEF_COND;
+    pmadapter->hs_cfg.gpio = HOST_SLEEP_DEF_GPIO;
+    pmadapter->hs_cfg.gap = HOST_SLEEP_DEF_GAP;
+
+    ret = wlan_prepare_cmd(pmpriv,
+                           HOST_CMD_APCMD_SYS_RESET,
+                           HostCmd_ACT_GEN_SET,
+                           0, (t_void *) pioctl_req, MNULL);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get MAC address
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_uap_bss_ioctl_mac_address(IN pmlan_adapter pmadapter,
+                               IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_bss *bss = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 cmd_action = 0;
+
+    ENTER();
+
+    bss = (mlan_ds_bss *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        memcpy(pmadapter, pmpriv->curr_addr, &bss->param.mac_addr,
+               MLAN_MAC_ADDR_LENGTH);
+        cmd_action = HostCmd_ACT_GEN_SET;
+    } else
+        cmd_action = HostCmd_ACT_GEN_GET;
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv, HOST_CMD_APCMD_SYS_CONFIGURE,
+                           cmd_action, 0, (t_void *) pioctl_req, MNULL);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get Uap statistics
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_uap_get_stats(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_SNMP_MIB,
+                           HostCmd_ACT_GEN_GET,
+                           0, (t_void *) pioctl_req, MNULL);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get AP config
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_uap_bss_ioctl_config(IN pmlan_adapter pmadapter,
+                          IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 cmd_action = 0;
+
+    ENTER();
+
+    if (pioctl_req->action == MLAN_ACT_SET)
+        cmd_action = HostCmd_ACT_GEN_SET;
+    else
+        cmd_action = HostCmd_ACT_GEN_GET;
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv, HOST_CMD_APCMD_SYS_CONFIGURE,
+                           cmd_action, 0, (t_void *) pioctl_req, MNULL);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief deauth sta
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_uap_bss_ioctl_deauth_sta(IN pmlan_adapter pmadapter,
+                              IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_bss *bss = MNULL;
+
+    ENTER();
+
+    bss = (mlan_ds_bss *) pioctl_req->pbuf;
+    ret = wlan_prepare_cmd(pmpriv,
+                           HOST_CMD_APCMD_STA_DEAUTH,
+                           HostCmd_ACT_GEN_SET,
+                           0,
+                           (t_void *) pioctl_req,
+                           (t_void *) & bss->param.deauth_param);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get station list
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_uap_get_sta_list(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HOST_CMD_APCMD_STA_LIST,
+                           HostCmd_ACT_GEN_GET,
+                           0, (t_void *) pioctl_req, MNULL);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief soft_reset
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_uap_misc_ioctl_soft_reset(IN pmlan_adapter pmadapter,
+                               IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_SOFT_RESET,
+                           HostCmd_ACT_GEN_SET,
+                           0, (t_void *) pioctl_req, MNULL);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Tx data pause
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_uap_misc_ioctl_txdatapause(IN pmlan_adapter pmadapter,
+                                IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_misc_cfg *pmisc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 cmd_action = 0;
+
+    ENTER();
+
+    if (pioctl_req->action == MLAN_ACT_SET)
+        cmd_action = HostCmd_ACT_GEN_SET;
+    else
+        cmd_action = HostCmd_ACT_GEN_GET;
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_CFG_TX_DATA_PAUSE,
+                           cmd_action,
+                           0,
+                           (t_void *) pioctl_req, &(pmisc->param.tx_datapause));
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get Power mode
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_uap_pm_ioctl_mode(IN pmlan_adapter pmadapter,
+                       IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_pm_cfg *pm = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u16 cmd_action = 0;
+    t_u32 cmd_oid = 0;
+
+    ENTER();
+
+    pm = (mlan_ds_pm_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_SET) {
+        if (pm->param.ps_mgmt.ps_mode == PS_MODE_INACTIVITY) {
+            cmd_action = EN_AUTO_PS;
+            cmd_oid = BITMAP_UAP_INACT_PS;
+        } else if (pm->param.ps_mgmt.ps_mode == PS_MODE_PERIODIC_DTIM) {
+            cmd_action = EN_AUTO_PS;
+            cmd_oid = BITMAP_UAP_DTIM_PS;
+        } else {
+            cmd_action = DIS_AUTO_PS;
+            cmd_oid = BITMAP_UAP_INACT_PS | BITMAP_UAP_DTIM_PS;
+        }
+    } else {
+        cmd_action = GET_PS;
+        cmd_oid = BITMAP_UAP_INACT_PS | BITMAP_UAP_DTIM_PS;
+    }
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_PS_MODE_ENH,
+                           cmd_action, cmd_oid, (t_void *) pioctl_req,
+                           (t_void *) & pm->param.ps_mgmt);
+    if ((ret == MLAN_STATUS_SUCCESS) &&
+        (pioctl_req->action == MLAN_ACT_SET) && (cmd_action == DIS_AUTO_PS)) {
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_802_11_PS_MODE_ENH, GET_PS,
+                               0, MNULL, MNULL);
+    }
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set WAPI IE
+ *
+ *  @param priv         A pointer to mlan_private structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return             MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_uap_set_wapi_ie(mlan_private * priv, pmlan_ioctl_req pioctl_req)
+{
+    mlan_ds_misc_cfg *misc = MNULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+    if (misc->param.gen_ie.len) {
+        if (misc->param.gen_ie.len > sizeof(priv->wapi_ie)) {
+            PRINTM(MWARN, "failed to copy WAPI IE, too big \n");
+            pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+        memcpy(priv->adapter, priv->wapi_ie, misc->param.gen_ie.ie_data,
+               misc->param.gen_ie.len);
+        priv->wapi_ie_len = misc->param.gen_ie.len;
+        PRINTM(MIOCTL, "Set wapi_ie_len=%d IE=%#x\n", priv->wapi_ie_len,
+               priv->wapi_ie[0]);
+        DBG_HEXDUMP(MCMD_D, "wapi_ie", priv->wapi_ie, priv->wapi_ie_len);
+        if (priv->wapi_ie[0] == WAPI_IE)
+            priv->sec_info.wapi_enabled = MTRUE;
+    } else {
+        memset(priv->adapter, priv->wapi_ie, 0, sizeof(priv->wapi_ie));
+        priv->wapi_ie_len = misc->param.gen_ie.len;
+        PRINTM(MINFO, "Reset wapi_ie_len=%d IE=%#x\n", priv->wapi_ie_len,
+               priv->wapi_ie[0]);
+        priv->sec_info.wapi_enabled = MFALSE;
+    }
+
+    /* Send request to firmware */
+    ret = wlan_prepare_cmd(priv, HOST_CMD_APCMD_SYS_CONFIGURE,
+                           HostCmd_ACT_GEN_SET, 0, (t_void *) pioctl_req,
+                           MNULL);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set generic IE
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_uap_misc_ioctl_gen_ie(IN pmlan_adapter pmadapter,
+                           IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_misc_cfg *misc = MNULL;
+    IEEEtypes_VendorHeader_t *pvendor_ie = MNULL;
+
+    ENTER();
+
+    misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+
+    if ((misc->param.gen_ie.type == MLAN_IE_TYPE_GEN_IE) &&
+        (pioctl_req->action == MLAN_ACT_SET)) {
+        if (misc->param.gen_ie.len) {
+            pvendor_ie =
+                (IEEEtypes_VendorHeader_t *) misc->param.gen_ie.ie_data;
+            if (pvendor_ie->element_id == WAPI_IE) {
+                /* IE is a WAPI IE so call set_wapi function */
+                ret = wlan_uap_set_wapi_ie(pmpriv, pioctl_req);
+            }
+        } else {
+            /* clear WAPI IE */
+            ret = wlan_uap_set_wapi_ie(pmpriv, pioctl_req);
+        }
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get WAPI status
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_uap_sec_ioctl_wapi_enable(IN pmlan_adapter pmadapter,
+                               IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_sec_cfg *sec = MNULL;
+    ENTER();
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        if (pmpriv->wapi_ie_len)
+            sec->param.wapi_enabled = MTRUE;
+        else
+            sec->param.wapi_enabled = MFALSE;
+    } else {
+        if (sec->param.wapi_enabled == MFALSE) {
+            memset(pmpriv->adapter, pmpriv->wapi_ie, 0,
+                   sizeof(pmpriv->wapi_ie));
+            pmpriv->wapi_ie_len = 0;
+            PRINTM(MINFO, "Reset wapi_ie_len=%d IE=%#x\n", pmpriv->wapi_ie_len,
+                   pmpriv->wapi_ie[0]);
+            pmpriv->sec_info.wapi_enabled = MFALSE;
+        }
+    }
+    pioctl_req->data_read_written = sizeof(t_u32) + MLAN_SUB_COMMAND_SIZE;
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set encrypt key
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_uap_sec_ioctl_set_encrypt_key(IN pmlan_adapter pmadapter,
+                                   IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_sec_cfg *sec = MNULL;
+
+    ENTER();
+    sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+    if (pioctl_req->action != MLAN_ACT_SET) {
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    if (!sec->param.encrypt_key.key_len) {
+        PRINTM(MCMND, "Skip set key with key_len = 0\n");
+        LEAVE();
+        return ret;
+    }
+    ret = wlan_prepare_cmd(pmpriv,
+                           HostCmd_CMD_802_11_KEY_MATERIAL,
+                           HostCmd_ACT_GEN_SET,
+                           KEY_INFO_ENABLED,
+                           (t_void *) pioctl_req, &sec->param.encrypt_key);
+
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get BSS information
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success
+ */
+static mlan_status
+wlan_uap_get_bss_info(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    pmlan_private pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_get_info *info;
+
+    ENTER();
+
+    info = (mlan_ds_get_info *) pioctl_req->pbuf;
+    /* Connection status */
+    info->param.bss_info.media_connected = pmpriv->media_connected;
+
+    /* Radio status */
+    info->param.bss_info.radio_on = pmadapter->radio_on;
+
+    /* BSSID */
+    memcpy(pmadapter, &info->param.bss_info.bssid, pmpriv->curr_addr,
+           MLAN_MAC_ADDR_LENGTH);
+    info->param.bss_info.is_hs_configured = pmadapter->is_hs_configured;
+    pioctl_req->data_read_written =
+        sizeof(mlan_bss_info) + MLAN_SUB_COMMAND_SIZE;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set Host Sleep configurations
+ *
+ *  @param pmadapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCES/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_uap_pm_ioctl_deepsleep(IN pmlan_adapter pmadapter,
+                            IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_pm_cfg *pm = MNULL;
+    mlan_ds_auto_ds auto_ds;
+    t_u32 mode;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_pm_cfg)) {
+        PRINTM(MWARN, "MLAN bss IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_pm_cfg);
+        pioctl_req->status_code = MLAN_ERROR_INVALID_PARAMETER;
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+    pm = (mlan_ds_pm_cfg *) pioctl_req->pbuf;
+
+    if (pioctl_req->action == MLAN_ACT_GET) {
+        if (pmadapter->is_deep_sleep) {
+            pm->param.auto_deep_sleep.auto_ds = DEEP_SLEEP_ON;
+            pm->param.auto_deep_sleep.idletime = pmadapter->idle_time;
+        } else
+            pm->param.auto_deep_sleep.auto_ds = DEEP_SLEEP_OFF;
+    } else {
+        if (pmadapter->is_deep_sleep &&
+            pm->param.auto_deep_sleep.auto_ds == DEEP_SLEEP_ON) {
+            PRINTM(MMSG, "uAP already in deep sleep mode\n");
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+        if (((mlan_ds_pm_cfg *) pioctl_req->pbuf)->param.auto_deep_sleep.
+            auto_ds == DEEP_SLEEP_ON) {
+            auto_ds.auto_ds = DEEP_SLEEP_ON;
+            mode = EN_AUTO_PS;
+            PRINTM(MINFO, "Auto Deep Sleep: on\n");
+        } else {
+            mode = DIS_AUTO_PS;
+            auto_ds.auto_ds = DEEP_SLEEP_OFF;
+            PRINTM(MINFO, "Auto Deep Sleep: off\n");
+        }
+        if (((mlan_ds_pm_cfg *) pioctl_req->pbuf)->param.auto_deep_sleep.
+            idletime)
+            auto_ds.idletime =
+                ((mlan_ds_pm_cfg *) pioctl_req->pbuf)->param.auto_deep_sleep.
+                idletime;
+        else
+            auto_ds.idletime = pmadapter->idle_time;
+        ret = wlan_prepare_cmd(pmpriv,
+                               HostCmd_CMD_802_11_PS_MODE_ENH,
+                               (t_u16) mode,
+                               BITMAP_AUTO_DS, (t_void *) pioctl_req, &auto_ds);
+        if (ret == MLAN_STATUS_SUCCESS)
+            ret = MLAN_STATUS_PENDING;
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set SNMP MIB for 11D
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return     MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static mlan_status
+wlan_uap_snmp_mib_11d(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_snmp_mib *snmp = MNULL;
+    state_11d_t flag;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_snmp_mib)) {
+        PRINTM(MWARN, "MLAN snmp_mib IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_snmp_mib);
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+
+    if ((pioctl_req->action == MLAN_ACT_SET) && pmpriv->uap_bss_started) {
+        PRINTM(MIOCTL,
+               "11D setting cannot be changed while UAP bss is started.\n");
+        pioctl_req->data_read_written = 0;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    snmp = (mlan_ds_snmp_mib *) pioctl_req->pbuf;
+    flag = (snmp->param.oid_value) ? ENABLE_11D : DISABLE_11D;
+
+    ret = wlan_11d_enable(pmpriv, (t_void *) pioctl_req, flag);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Callback to finish domain_info handling
+ *  Not to be called directly to initiate domain_info setting.
+ *
+ *  @param pmpriv   A pointer to mlan_private structure (cast from t_void*)
+ *
+ *  @return     MLAN_STATUS_PENDING --success, otherwise fail
+ *  @sa         wlan_uap_domain_info
+ */
+static mlan_status
+wlan_uap_callback_domain_info(IN t_void * priv)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = (mlan_private *) priv;
+    wlan_uap_get_info_cb_t *puap_state_chan_cb = &pmpriv->uap_state_chan_cb;
+    mlan_ds_11d_cfg *cfg11d;
+    t_u8 band;
+
+    ENTER();
+    /* clear callback now that we're here */
+    puap_state_chan_cb->get_chan_callback = MNULL;
+
+    cfg11d = (mlan_ds_11d_cfg *) puap_state_chan_cb->pioctl_req_curr->pbuf;
+    band =
+        (puap_state_chan_cb->band_config & BAND_CONFIG_5GHZ) ? BAND_A : BAND_B;
+
+    ret =
+        wlan_11d_handle_uap_domain_info(pmpriv, band, cfg11d->param.domain_tlv,
+                                        puap_state_chan_cb->pioctl_req_curr);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    puap_state_chan_cb->pioctl_req_curr = MNULL;        // prevent re-use
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set Domain Info for 11D
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return     MLAN_STATUS_PENDING --success, otherwise fail
+ *  @sa         wlan_uap_callback_domain_info
+ */
+static mlan_status
+wlan_uap_domain_info(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_11d_cfg)) {
+        PRINTM(MWARN, "MLAN 11d_cfg IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_11d_cfg);
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+
+    if ((pioctl_req->action == MLAN_ACT_SET) && pmpriv->uap_bss_started) {
+        PRINTM(MIOCTL,
+               "Domain_info cannot be changed while UAP bss is started.\n");
+        pioctl_req->data_read_written = 0;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    /* store params, issue command to get UAP channel, whose CMD_RESP will
+       callback remainder of domain_info handling */
+    pmpriv->uap_state_chan_cb.pioctl_req_curr = pioctl_req;
+    pmpriv->uap_state_chan_cb.get_chan_callback = wlan_uap_callback_domain_info;
+
+    ret = wlan_uap_get_channel(pmpriv);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Callback to finish 11H channel check handling.
+ *  Not to be called directly to initiate channel check.
+ *
+ *  @param priv A pointer to mlan_private structure (cast from t_void*)
+ *
+ *  @return     MLAN_STATUS_SUCCESS/PENDING --success, otherwise fail
+ *  @sa         wlan_uap_11h_channel_check_req
+ */
+static mlan_status
+wlan_uap_callback_11h_channel_check_req(IN t_void * priv)
+{
+    mlan_status ret = MLAN_STATUS_FAILURE;
+    mlan_private *pmpriv = (mlan_private *) priv;
+    mlan_callbacks *pcb = (mlan_callbacks *) & pmpriv->adapter->callbacks;
+    wlan_uap_get_info_cb_t *puap_state_chan_cb = &pmpriv->uap_state_chan_cb;
+    /* keep copy as local variable */
+    pmlan_ioctl_req pioctl = puap_state_chan_cb->pioctl_req_curr;
+
+    ENTER();
+    /* clear callback now that we're here */
+    puap_state_chan_cb->get_chan_callback = MNULL;
+    /* clear early to avoid race condition */
+    puap_state_chan_cb->pioctl_req_curr = MNULL;
+
+    /*
+     * Check if the region and channel requires a channel availability
+     * check.
+     */
+    if ((puap_state_chan_cb->band_config & BAND_CONFIG_5GHZ) &&
+        wlan_11h_radar_detect_required(pmpriv, puap_state_chan_cb->channel) &&
+        !wlan_11h_is_channel_under_nop(pmpriv->adapter,
+                                       puap_state_chan_cb->channel)) {
+
+        /*
+         * Radar detection is required for this channel, make sure
+         * 11h is activated in the firmware
+         */
+        ret = wlan_11h_activate(pmpriv, MNULL, MTRUE);
+        ret = wlan_11h_config_master_radar_det(pmpriv, MTRUE);
+        ret = wlan_11h_check_update_radar_det_state(pmpriv);
+
+        /* Check for radar on the channel */
+        ret = wlan_11h_issue_radar_detect(pmpriv,
+                                          pioctl, puap_state_chan_cb->channel);
+        if (ret == MLAN_STATUS_SUCCESS)
+            ret = MLAN_STATUS_PENDING;
+    } else {
+        /* No command sent with the ioctl, need manually signal completion */
+        pcb->moal_ioctl_complete(pmpriv->adapter->pmoal_handle,
+                                 pioctl, MLAN_STATUS_FAILURE);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief 802.11h uap start channel check
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return     MLAN_STATUS_PENDING --success, otherwise fail
+ *  @sa         wlan_uap_callback_11h_channel_check_req
+ */
+static mlan_status
+wlan_uap_11h_channel_check_req(IN pmlan_adapter pmadapter,
+                               IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_11h_cfg)) {
+        PRINTM(MWARN, "MLAN 11h_cfg IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_11h_cfg);
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+
+    /* store params, issue command to get UAP channel, whose CMD_RESP will
+       callback remainder of 11H channel check handling */
+    pmpriv->uap_state_chan_cb.pioctl_req_curr = pioctl_req;
+    pmpriv->uap_state_chan_cb.get_chan_callback =
+        wlan_uap_callback_11h_channel_check_req;
+
+    ret = wlan_uap_get_channel(pmpriv);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Callback to finish 11H handling
+ *  Not to be called directly to initiate 11H setting.
+ *
+ *  @param pmpriv   A pointer to mlan_private structure (cast from t_void*)
+ *
+ *  @return     MLAN_STATUS_PENDING --success, otherwise fail
+ *  @sa         wlan_uap_snmp_mib_11h
+ */
+static mlan_status
+wlan_uap_callback_snmp_mib_11h(IN t_void * priv)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = (mlan_private *) priv;
+    wlan_uap_get_info_cb_t *puap_state_chan_cb = &pmpriv->uap_state_chan_cb;
+    mlan_ds_snmp_mib *snmp;
+    t_bool enable_11h;
+
+    ENTER();
+    /* clear callback now that we're here */
+    puap_state_chan_cb->get_chan_callback = MNULL;
+
+    snmp = (mlan_ds_snmp_mib *) puap_state_chan_cb->pioctl_req_curr->pbuf;
+    enable_11h = (snmp->param.oid_value) ? MTRUE : MFALSE;
+
+    if (enable_11h) {
+        if ((puap_state_chan_cb->band_config & BAND_CONFIG_5GHZ) &&
+            wlan_11h_radar_detect_required(pmpriv,
+                                           puap_state_chan_cb->channel)) {
+            if (!wlan_11h_is_master_radar_det_active(pmpriv))
+                wlan_11h_config_master_radar_det(pmpriv, MTRUE);
+        }
+    }
+
+    ret =
+        wlan_11h_activate(pmpriv,
+                          (t_void *) puap_state_chan_cb->pioctl_req_curr,
+                          enable_11h);
+    wlan_11h_check_update_radar_det_state(pmpriv);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    puap_state_chan_cb->pioctl_req_curr = MNULL;        // prevent re-use
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set SNMP MIB for 11H
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param pioctl_req   A pointer to ioctl request buffer
+ *
+ *  @return     MLAN_STATUS_PENDING --success, otherwise fail
+ *  @sa         wlan_uap_callback_snmp_mib_11h
+ */
+static mlan_status
+wlan_uap_snmp_mib_11h(IN pmlan_adapter pmadapter, IN pmlan_ioctl_req pioctl_req)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+    mlan_ds_snmp_mib *snmp = MNULL;
+    t_bool enable;
+
+    ENTER();
+
+    if (pioctl_req->buf_len < sizeof(mlan_ds_snmp_mib)) {
+        PRINTM(MWARN, "MLAN snmp_mib IOCTL length is too short.\n");
+        pioctl_req->data_read_written = 0;
+        pioctl_req->buf_len_needed = sizeof(mlan_ds_snmp_mib);
+        LEAVE();
+        return MLAN_STATUS_RESOURCE;
+    }
+    snmp = (mlan_ds_snmp_mib *) pioctl_req->pbuf;
+    enable = (snmp->param.oid_value) ? MTRUE : MFALSE;
+
+    if (enable) {
+        /* first enable 11D if it is not enabled */
+        if (!wlan_11d_is_enabled(pmpriv)) {
+            ret = wlan_11d_enable(pmpriv, MNULL, ENABLE_11D);
+            if (ret != MLAN_STATUS_SUCCESS) {
+                PRINTM(MERROR,
+                       "Failed to first enable 11D before enabling 11H.\n");
+                LEAVE();
+                return ret;
+            }
+        }
+    }
+
+    /* store params, issue command to get UAP channel, whose CMD_RESP will
+       callback remainder of 11H handling (and radar detect if DFS chan) */
+    pmpriv->uap_state_chan_cb.pioctl_req_curr = pioctl_req;
+    pmpriv->uap_state_chan_cb.get_chan_callback =
+        wlan_uap_callback_snmp_mib_11h;
+
+    ret = wlan_uap_get_channel(pmpriv);
+    if (ret == MLAN_STATUS_SUCCESS)
+        ret = MLAN_STATUS_PENDING;
+
+    LEAVE();
+    return ret;
+}
+
+/********************************************************
+    Global Functions
+********************************************************/
+
+/**
+ *  @brief Issue CMD to UAP firmware to get current channel
+ *
+ *  @param pmpriv   A pointer to mlan_private structure
+ *
+ *  @return         MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+mlan_status
+wlan_uap_get_channel(IN pmlan_private pmpriv)
+{
+    MrvlIEtypes_channel_band_t tlv_chan_band;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    memset(pmpriv->adapter, &tlv_chan_band, 0, sizeof(tlv_chan_band));
+    tlv_chan_band.header.type = TLV_TYPE_UAP_CHAN_BAND_CONFIG;
+    tlv_chan_band.header.len = sizeof(MrvlIEtypes_channel_band_t)
+        - sizeof(MrvlIEtypesHeader_t);
+
+    ret = wlan_prepare_cmd(pmpriv, HOST_CMD_APCMD_SYS_CONFIGURE,
+                           HostCmd_ACT_GEN_GET, 0, MNULL, &tlv_chan_band);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Issue CMD to UAP firmware to set current channel
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param uap_band_cfg UAP band configuration
+ *  @param channel      New channel
+ *
+ *  @return         MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+mlan_status
+wlan_uap_set_channel(IN pmlan_private pmpriv,
+                     IN t_u8 uap_band_cfg, IN t_u8 channel)
+{
+    MrvlIEtypes_channel_band_t tlv_chan_band;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    memset(pmpriv->adapter, &tlv_chan_band, 0, sizeof(tlv_chan_band));
+    tlv_chan_band.header.type = TLV_TYPE_UAP_CHAN_BAND_CONFIG;
+    tlv_chan_band.header.len = sizeof(MrvlIEtypes_channel_band_t)
+        - sizeof(MrvlIEtypesHeader_t);
+    tlv_chan_band.band_config = uap_band_cfg;
+    tlv_chan_band.channel = channel;
+
+    ret = wlan_prepare_cmd(pmpriv, HOST_CMD_APCMD_SYS_CONFIGURE,
+                           HostCmd_ACT_GEN_SET, 0, MNULL, &tlv_chan_band);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Issue CMD to UAP firmware to get current beacon and dtim periods
+ *
+ *  @param pmpriv   A pointer to mlan_private structure
+ *
+ *  @return         MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+mlan_status
+wlan_uap_get_beacon_dtim(IN pmlan_private pmpriv)
+{
+    t_u8 tlv_buffer[sizeof(MrvlIEtypes_beacon_period_t) +
+                    sizeof(MrvlIEtypes_dtim_period_t)];
+    MrvlIEtypes_beacon_period_t *ptlv_beacon_pd;
+    MrvlIEtypes_dtim_period_t *ptlv_dtim_pd;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    memset(pmpriv->adapter, &tlv_buffer, 0, sizeof(tlv_buffer));
+    ptlv_beacon_pd = (MrvlIEtypes_beacon_period_t *) tlv_buffer;
+    ptlv_beacon_pd->header.type = TLV_TYPE_UAP_BEACON_PERIOD;
+    ptlv_beacon_pd->header.len = sizeof(MrvlIEtypes_beacon_period_t)
+        - sizeof(MrvlIEtypesHeader_t);
+
+    ptlv_dtim_pd = (MrvlIEtypes_dtim_period_t *) (tlv_buffer
+                                                  +
+                                                  sizeof
+                                                  (MrvlIEtypes_beacon_period_t));
+    ptlv_dtim_pd->header.type = TLV_TYPE_UAP_DTIM_PERIOD;
+    ptlv_dtim_pd->header.len = sizeof(MrvlIEtypes_dtim_period_t)
+        - sizeof(MrvlIEtypesHeader_t);
+
+    ret = wlan_prepare_cmd(pmpriv, HOST_CMD_APCMD_SYS_CONFIGURE,
+                           HostCmd_ACT_GEN_GET, 0, MNULL, tlv_buffer);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief MLAN uap ioctl handler
+ *
+ *  @param adapter	A pointer to mlan_adapter structure
+ *  @param pioctl_req	A pointer to ioctl request buffer
+ *
+ *  @return		MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+mlan_status
+wlan_ops_uap_ioctl(t_void * adapter, pmlan_ioctl_req pioctl_req)
+{
+    pmlan_adapter pmadapter = (pmlan_adapter) adapter;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    mlan_ds_bss *bss = MNULL;
+    mlan_ds_get_info *pget_info = MNULL;
+    mlan_ds_misc_cfg *misc = MNULL;
+    mlan_ds_sec_cfg *sec = MNULL;
+    mlan_ds_pm_cfg *pm = MNULL;
+    mlan_ds_snmp_mib *snmp = MNULL;
+    mlan_ds_11d_cfg *cfg11d = MNULL;
+    mlan_ds_11h_cfg *cfg11h = MNULL;
+    mlan_ds_radio_cfg *radiocfg = MNULL;
+    mlan_ds_rate *rate = MNULL;
+    mlan_private *pmpriv = pmadapter->priv[pioctl_req->bss_index];
+
+    ENTER();
+
+    switch (pioctl_req->req_id) {
+    case MLAN_IOCTL_BSS:
+        bss = (mlan_ds_bss *) pioctl_req->pbuf;
+        if (bss->sub_command == MLAN_OID_BSS_MAC_ADDR)
+            status = wlan_uap_bss_ioctl_mac_address(pmadapter, pioctl_req);
+        else if (bss->sub_command == MLAN_OID_BSS_STOP)
+            status = wlan_uap_bss_ioctl_stop(pmadapter, pioctl_req);
+        else if (bss->sub_command == MLAN_OID_BSS_START)
+            status = wlan_uap_bss_ioctl_start(pmadapter, pioctl_req);
+        else if (bss->sub_command == MLAN_OID_UAP_BSS_CONFIG)
+            status = wlan_uap_bss_ioctl_config(pmadapter, pioctl_req);
+        else if (bss->sub_command == MLAN_OID_UAP_DEAUTH_STA)
+            status = wlan_uap_bss_ioctl_deauth_sta(pmadapter, pioctl_req);
+        else if (bss->sub_command == MLAN_OID_UAP_BSS_RESET)
+            status = wlan_uap_bss_ioctl_reset(pmadapter, pioctl_req);
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+        else if (bss->sub_command == MLAN_OID_BSS_ROLE)
+            status = wlan_bss_ioctl_bss_role(pmadapter, pioctl_req);
+#endif
+#ifdef WIFI_DIRECT_SUPPORT
+        else if (bss->sub_command == MLAN_OID_WIFI_DIRECT_MODE)
+            status = wlan_bss_ioctl_wifi_direct_mode(pmadapter, pioctl_req);
+#endif
+        break;
+    case MLAN_IOCTL_GET_INFO:
+        pget_info = (mlan_ds_get_info *) pioctl_req->pbuf;
+        if (pget_info->sub_command == MLAN_OID_GET_VER_EXT)
+            status = wlan_get_info_ver_ext(pmadapter, pioctl_req);
+        else if (pget_info->sub_command == MLAN_OID_GET_DEBUG_INFO)
+            status = wlan_get_info_debug_info(pmadapter, pioctl_req);
+        else if (pget_info->sub_command == MLAN_OID_GET_STATS)
+            status = wlan_uap_get_stats(pmadapter, pioctl_req);
+        else if (pget_info->sub_command == MLAN_OID_UAP_STA_LIST)
+            status = wlan_uap_get_sta_list(pmadapter, pioctl_req);
+        else if (pget_info->sub_command == MLAN_OID_GET_BSS_INFO)
+            status = wlan_uap_get_bss_info(pmadapter, pioctl_req);
+        else if (pget_info->sub_command == MLAN_OID_GET_FW_INFO) {
+            pioctl_req->data_read_written =
+                sizeof(mlan_fw_info) + MLAN_SUB_COMMAND_SIZE;
+            memcpy(pmadapter, &pget_info->param.fw_info.mac_addr,
+                   pmpriv->curr_addr, MLAN_MAC_ADDR_LENGTH);
+            pget_info->param.fw_info.fw_ver = pmadapter->fw_release_number;
+            pget_info->param.fw_info.fw_bands = pmadapter->fw_bands;
+            pget_info->param.fw_info.hw_dev_mcs_support =
+                pmadapter->hw_dev_mcs_support;
+        }
+        break;
+    case MLAN_IOCTL_MISC_CFG:
+        misc = (mlan_ds_misc_cfg *) pioctl_req->pbuf;
+        if (misc->sub_command == MLAN_OID_MISC_INIT_SHUTDOWN)
+            status = wlan_misc_ioctl_init_shutdown(pmadapter, pioctl_req);
+        if (misc->sub_command == MLAN_OID_MISC_SOFT_RESET)
+            status = wlan_uap_misc_ioctl_soft_reset(pmadapter, pioctl_req);
+        if (misc->sub_command == MLAN_OID_MISC_HOST_CMD)
+            status = wlan_misc_ioctl_host_cmd(pmadapter, pioctl_req);
+        if (misc->sub_command == MLAN_OID_MISC_GEN_IE)
+            status = wlan_uap_misc_ioctl_gen_ie(pmadapter, pioctl_req);
+        if (misc->sub_command == MLAN_OID_MISC_CUSTOM_IE)
+            status =
+                wlan_misc_ioctl_custom_ie_list(pmadapter, pioctl_req, MTRUE);
+        if (misc->sub_command == MLAN_OID_MISC_TX_DATAPAUSE)
+            status = wlan_uap_misc_ioctl_txdatapause(pmadapter, pioctl_req);
+        if (misc->sub_command == MLAN_OID_MISC_RX_MGMT_IND)
+            status = wlan_reg_rx_mgmt_ind(pmadapter, pioctl_req);
+#ifdef DEBUG_LEVEL1
+        if (misc->sub_command == MLAN_OID_MISC_DRVDBG)
+            status = wlan_set_drvdbg(pmadapter, pioctl_req);
+#endif
+        break;
+    case MLAN_IOCTL_PM_CFG:
+        pm = (mlan_ds_pm_cfg *) pioctl_req->pbuf;
+        if (pm->sub_command == MLAN_OID_PM_CFG_PS_MODE)
+            status = wlan_uap_pm_ioctl_mode(pmadapter, pioctl_req);
+        if (pm->sub_command == MLAN_OID_PM_CFG_DEEP_SLEEP)
+            status = wlan_uap_pm_ioctl_deepsleep(pmadapter, pioctl_req);
+        if (pm->sub_command == MLAN_OID_PM_CFG_HS_CFG) {
+            status = wlan_pm_ioctl_hscfg(pmadapter, pioctl_req);
+        }
+        if (pm->sub_command == MLAN_OID_PM_INFO) {
+            status = wlan_get_pm_info(pmadapter, pioctl_req);
+        }
+        break;
+    case MLAN_IOCTL_SNMP_MIB:
+        snmp = (mlan_ds_snmp_mib *) pioctl_req->pbuf;
+        if (snmp->sub_command == MLAN_OID_SNMP_MIB_DOT11D)
+            status = wlan_uap_snmp_mib_11d(pmadapter, pioctl_req);
+        if (snmp->sub_command == MLAN_OID_SNMP_MIB_DOT11H)
+            status = wlan_uap_snmp_mib_11h(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_SEC_CFG:
+        sec = (mlan_ds_sec_cfg *) pioctl_req->pbuf;
+        if (sec->sub_command == MLAN_OID_SEC_CFG_ENCRYPT_KEY)
+            status = wlan_uap_sec_ioctl_set_encrypt_key(pmadapter, pioctl_req);
+        if (sec->sub_command == MLAN_OID_SEC_CFG_WAPI_ENABLED)
+            status = wlan_uap_sec_ioctl_wapi_enable(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_11N_CFG:
+        status = wlan_11n_cfg_ioctl(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_11D_CFG:
+        cfg11d = (mlan_ds_11d_cfg *) pioctl_req->pbuf;
+        if (cfg11d->sub_command == MLAN_OID_11D_DOMAIN_INFO)
+            status = wlan_uap_domain_info(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_11H_CFG:
+        cfg11h = (mlan_ds_11h_cfg *) pioctl_req->pbuf;
+        if (cfg11h->sub_command == MLAN_OID_11H_CHANNEL_CHECK)
+            status = wlan_uap_11h_channel_check_req(pmadapter, pioctl_req);
+#if defined(DFS_TESTING_SUPPORT)
+        if (cfg11h->sub_command == MLAN_OID_11H_DFS_TESTING)
+            status = wlan_11h_ioctl_dfs_testing(pmadapter, pioctl_req);
+#endif
+        break;
+    case MLAN_IOCTL_RADIO_CFG:
+        radiocfg = (mlan_ds_radio_cfg *) pioctl_req->pbuf;
+        if (radiocfg->sub_command == MLAN_OID_RADIO_CTRL)
+            status = wlan_radio_ioctl_radio_ctl(pmadapter, pioctl_req);
+#ifdef WIFI_DIRECT_SUPPORT
+        if (radiocfg->sub_command == MLAN_OID_REMAIN_CHAN_CFG)
+            status = wlan_radio_ioctl_remain_chan_cfg(pmadapter, pioctl_req);
+#endif
+        if (radiocfg->sub_command == MLAN_OID_ANT_CFG)
+            status = wlan_radio_ioctl_ant_cfg(pmadapter, pioctl_req);
+        break;
+    case MLAN_IOCTL_RATE:
+        rate = (mlan_ds_rate *) pioctl_req->pbuf;
+        if (rate->sub_command == MLAN_OID_RATE_CFG)
+            status = wlan_rate_ioctl_cfg(pmadapter, pioctl_req);
+        break;
+    default:
+        pioctl_req->status_code = MLAN_ERROR_IOCTL_INVALID;
+        break;
+    }
+    LEAVE();
+    return status;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_uap_txrx.c b/drivers/net/wireless/sd8797/mlan/mlan_uap_txrx.c
new file mode 100644
index 000000000000..590def325f27
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_uap_txrx.c
@@ -0,0 +1,556 @@
+/** @file mlan_uap_txrx.c
+ *
+ *  @brief This file contains AP mode transmit and receive functions
+ * 
+ *  Copyright (C) 2009-2011, Marvell International Ltd. 
+ *   
+ *  This software file (the "File") is distributed by Marvell International 
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+ *  (the "License").  You may use, redistribute and/or modify this File in 
+ *  accordance with the terms and conditions of the License, a copy of which 
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+ *  this warranty disclaimer.
+ */
+
+/********************************************************
+Change log:
+    02/05/2009: initial version
+********************************************************/
+
+#include "mlan.h"
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#ifdef STA_SUPPORT
+#include "mlan_join.h"
+#endif
+#include "mlan_main.h"
+#include "mlan_uap.h"
+#include "mlan_sdio.h"
+#include "mlan_wmm.h"
+#include "mlan_11n_aggr.h"
+#include "mlan_11n_rxreorder.h"
+
+/********************************************************
+                Local Functions
+********************************************************/
+
+/**
+ *  @brief This function processes received packet and forwards it
+ *  		to kernel/upper layer
+ *
+ *  @param pmadapter A pointer to mlan_adapter
+ *  @param pmbuf     A pointer to mlan_buffer which includes the received packet
+ *
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+wlan_upload_uap_rx_packet(pmlan_adapter pmadapter, pmlan_buffer pmbuf)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    UapRxPD *prx_pd;
+    ENTER();
+    prx_pd = (UapRxPD *) (pmbuf->pbuf + pmbuf->data_offset);
+
+    /* Chop off RxPD */
+    pmbuf->data_len -= prx_pd->rx_pkt_offset;
+    pmbuf->data_offset += prx_pd->rx_pkt_offset;
+    pmbuf->pparent = MNULL;
+
+    DBG_HEXDUMP(MDAT_D, "uAP RxPD", (t_u8 *) prx_pd,
+                MIN(sizeof(UapRxPD), MAX_DATA_DUMP_LEN));
+    DBG_HEXDUMP(MDAT_D, "uAP Rx Payload",
+                ((t_u8 *) prx_pd + prx_pd->rx_pkt_offset),
+                MIN(prx_pd->rx_pkt_length, MAX_DATA_DUMP_LEN));
+
+    pmadapter->callbacks.moal_get_system_time(pmadapter->pmoal_handle,
+                                              &pmbuf->out_ts_sec,
+                                              &pmbuf->out_ts_usec);
+    PRINTM(MDATA, "%lu.%06lu : Data => kernel seq_num=%d tid=%d\n",
+           pmbuf->out_ts_sec, pmbuf->out_ts_usec, prx_pd->seq_num,
+           prx_pd->priority);
+    ret = pmadapter->callbacks.moal_recv_packet(pmadapter->pmoal_handle, pmbuf);
+    if (ret == MLAN_STATUS_FAILURE) {
+        PRINTM(MERROR, "uAP Rx Error: moal_recv_packet returned error\n");
+        pmbuf->status_code = MLAN_ERROR_PKT_INVALID;
+    }
+
+    if (ret != MLAN_STATUS_PENDING) {
+        wlan_free_mlan_buffer(pmadapter, pmbuf);
+    }
+    LEAVE();
+
+    return ret;
+
+}
+
+/**
+ *  @brief This function will check if unicast packet need be dropped
+ *
+ *  @param priv    A pointer to mlan_private
+ *  @param mac     mac address to find in station list table
+ *
+ *  @return	       MLAN_STATUS_FAILURE -- drop packet, otherwise forward to network stack
+ */
+static mlan_status
+wlan_check_unicast_packet(mlan_private * priv, t_u8 * mac)
+{
+    int j;
+    sta_node *sta_ptr = MNULL;
+    pmlan_adapter pmadapter = priv->adapter;
+    pmlan_private pmpriv = MNULL;
+    t_u8 pkt_type = 0;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    ENTER();
+    for (j = 0; j < MLAN_MAX_BSS_NUM; ++j) {
+        if ((pmpriv = pmadapter->priv[j])) {
+            if (GET_BSS_ROLE(pmpriv) == MLAN_BSS_ROLE_STA)
+                continue;
+            sta_ptr = wlan_get_station_entry(pmpriv, mac);
+            if (sta_ptr) {
+                if (pmpriv == priv)
+                    pkt_type = PKT_INTRA_UCAST;
+                else
+                    pkt_type = PKT_INTER_UCAST;
+                break;
+            }
+        }
+    }
+    if ((pkt_type == PKT_INTRA_UCAST) && (priv->pkt_fwd & PKT_FWD_INTRA_UCAST)) {
+        PRINTM(MDATA, "Drop INTRA_UCAST packet\n");
+        ret = MLAN_STATUS_FAILURE;
+    } else if ((pkt_type == PKT_INTER_UCAST) &&
+               (priv->pkt_fwd & PKT_FWD_INTER_UCAST)) {
+        PRINTM(MDATA, "Drop INTER_UCAST packet\n");
+        ret = MLAN_STATUS_FAILURE;
+    }
+    LEAVE();
+    return ret;
+}
+
+/********************************************************
+    Global Functions
+********************************************************/
+/**
+ *  @brief This function fill the txpd for tx packet
+ *
+ *  @param priv	   A pointer to mlan_private structure
+ *  @param pmbuf   A pointer to the mlan_buffer for process
+ *
+ *  @return 	   headptr or MNULL
+ */
+t_void *
+wlan_ops_uap_process_txpd(IN t_void * priv, IN pmlan_buffer pmbuf)
+{
+    pmlan_private pmpriv = (pmlan_private) priv;
+    UapTxPD *plocal_tx_pd;
+    t_u8 *head_ptr = MNULL;
+    t_u32 pkt_type;
+    t_u32 tx_control;
+    ENTER();
+
+    if (!pmbuf->data_len) {
+        PRINTM(MERROR, "uAP Tx Error: Invalid packet length: %d\n",
+               pmbuf->data_len);
+        pmbuf->status_code = MLAN_ERROR_PKT_SIZE_INVALID;
+        goto done;
+    }
+    if (pmbuf->buf_type == MLAN_BUF_TYPE_RAW_DATA) {
+        memcpy(pmpriv->adapter, &pkt_type, pmbuf->pbuf + pmbuf->data_offset,
+               sizeof(pkt_type));
+        memcpy(pmpriv->adapter, &tx_control,
+               pmbuf->pbuf + pmbuf->data_offset + sizeof(pkt_type),
+               sizeof(tx_control));
+        pmbuf->data_offset += sizeof(pkt_type) + sizeof(tx_control);
+        pmbuf->data_len -= sizeof(pkt_type) + sizeof(tx_control);
+    }
+    if (pmbuf->data_offset < (sizeof(UapTxPD) + INTF_HEADER_LEN +
+                              DMA_ALIGNMENT)) {
+        PRINTM(MERROR, "not enough space for UapTxPD: len=%d, offset=%d\n",
+               pmbuf->data_len, pmbuf->data_offset);
+        DBG_HEXDUMP(MDAT_D, "drop pkt", pmbuf->pbuf + pmbuf->data_offset,
+                    pmbuf->data_len);
+        pmbuf->status_code = MLAN_ERROR_PKT_SIZE_INVALID;
+        goto done;
+    }
+
+    /* head_ptr should be aligned */
+    head_ptr =
+        pmbuf->pbuf + pmbuf->data_offset - sizeof(UapTxPD) - INTF_HEADER_LEN;
+    head_ptr = (t_u8 *) ((t_ptr) head_ptr & ~((t_ptr) (DMA_ALIGNMENT - 1)));
+
+    plocal_tx_pd = (UapTxPD *) (head_ptr + INTF_HEADER_LEN);
+    memset(pmpriv->adapter, plocal_tx_pd, 0, sizeof(UapTxPD));
+
+    /* Set the BSS number to TxPD */
+    plocal_tx_pd->bss_num = GET_BSS_NUM(pmpriv);
+    plocal_tx_pd->bss_type = pmpriv->bss_type;
+
+    plocal_tx_pd->tx_pkt_length = (t_u16) pmbuf->data_len;
+
+    plocal_tx_pd->priority = (t_u8) pmbuf->priority;
+    plocal_tx_pd->pkt_delay_2ms =
+        wlan_wmm_compute_driver_packet_delay(pmpriv, pmbuf);
+
+    if (plocal_tx_pd->priority < NELEMENTS(pmpriv->wmm.user_pri_pkt_tx_ctrl))
+        /*
+         * Set the priority specific tx_control field, setting of 0 will
+         *   cause the default value to be used later in this function
+         */
+        plocal_tx_pd->tx_control
+            = pmpriv->wmm.user_pri_pkt_tx_ctrl[plocal_tx_pd->priority];
+
+    /* Offset of actual data */
+    plocal_tx_pd->tx_pkt_offset =
+        (t_u16) ((t_ptr) pmbuf->pbuf + pmbuf->data_offset -
+                 (t_ptr) plocal_tx_pd);
+
+    if (pmbuf->buf_type == MLAN_BUF_TYPE_RAW_DATA) {
+        plocal_tx_pd->tx_pkt_type = (t_u16) pkt_type;
+        plocal_tx_pd->tx_control = tx_control;
+    }
+    uap_endian_convert_TxPD(plocal_tx_pd);
+
+    /* Adjust the data offset and length to include TxPD in pmbuf */
+    pmbuf->data_len += pmbuf->data_offset;
+    pmbuf->data_offset = (t_u32) ((t_ptr) head_ptr - (t_ptr) pmbuf->pbuf);
+    pmbuf->data_len -= pmbuf->data_offset;
+
+  done:
+    LEAVE();
+    return head_ptr;
+}
+
+/**
+ *  @brief This function processes received packet and forwards it
+ *  		to kernel/upper layer
+ *
+ *  @param adapter   A pointer to mlan_adapter
+ *  @param pmbuf     A pointer to mlan_buffer which includes the received packet
+ *
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_ops_uap_process_rx_packet(IN t_void * adapter, IN pmlan_buffer pmbuf)
+{
+    pmlan_adapter pmadapter = (pmlan_adapter) adapter;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    UapRxPD *prx_pd;
+    wlan_mgmt_pkt *puap_pkt_hdr = MNULL;
+
+    RxPacketHdr_t *prx_pkt;
+    pmlan_private priv = pmadapter->priv[pmbuf->bss_index];
+    t_u8 ta[MLAN_MAC_ADDR_LENGTH];
+    t_u16 rx_pkt_type = 0;
+    sta_node *sta_ptr = MNULL;
+
+    ENTER();
+
+    prx_pd = (UapRxPD *) (pmbuf->pbuf + pmbuf->data_offset);
+    /* Endian conversion */
+    uap_endian_convert_RxPD(prx_pd);
+    rx_pkt_type = prx_pd->rx_pkt_type;
+    prx_pkt = (RxPacketHdr_t *) ((t_u8 *) prx_pd + prx_pd->rx_pkt_offset);
+
+    PRINTM(MINFO, "RX Data: data_len - prx_pd->rx_pkt_offset = %d - %d = %d\n",
+           pmbuf->data_len, prx_pd->rx_pkt_offset,
+           pmbuf->data_len - prx_pd->rx_pkt_offset);
+
+    if ((prx_pd->rx_pkt_offset + prx_pd->rx_pkt_length) >
+        (t_u16) pmbuf->data_len) {
+        PRINTM(MERROR,
+               "Wrong rx packet: len=%d,rx_pkt_offset=%d,"
+               " rx_pkt_length=%d\n", pmbuf->data_len, prx_pd->rx_pkt_offset,
+               prx_pd->rx_pkt_length);
+        pmbuf->status_code = MLAN_ERROR_PKT_SIZE_INVALID;
+        ret = MLAN_STATUS_FAILURE;
+        wlan_free_mlan_buffer(pmadapter, pmbuf);
+        goto done;
+    }
+    pmbuf->data_len = prx_pd->rx_pkt_offset + prx_pd->rx_pkt_length;
+
+    if (pmadapter->priv[pmbuf->bss_index]->mgmt_frame_passthru_mask &&
+        prx_pd->rx_pkt_type == PKT_TYPE_MGMT_FRAME) {
+        /* Check if this is mgmt packet and needs to forwarded to app as an
+           event */
+        puap_pkt_hdr =
+            (wlan_mgmt_pkt *) ((t_u8 *) prx_pd + prx_pd->rx_pkt_offset);
+        puap_pkt_hdr->frm_len = wlan_le16_to_cpu(puap_pkt_hdr->frm_len);
+        if ((puap_pkt_hdr->wlan_header.
+             frm_ctl & IEEE80211_FC_MGMT_FRAME_TYPE_MASK) == 0)
+            wlan_process_802dot11_mgmt_pkt(pmadapter->priv[pmbuf->bss_index],
+                                           (t_u8 *) & puap_pkt_hdr->wlan_header,
+                                           puap_pkt_hdr->frm_len +
+                                           sizeof(wlan_mgmt_pkt) -
+                                           sizeof(puap_pkt_hdr->frm_len));
+        wlan_free_mlan_buffer(pmadapter, pmbuf);
+        goto done;
+    }
+
+    pmbuf->priority = prx_pd->priority;
+    memcpy(pmadapter, ta, prx_pkt->eth803_hdr.src_addr, MLAN_MAC_ADDR_LENGTH);
+    if ((rx_pkt_type != PKT_TYPE_BAR) && (prx_pd->priority < MAX_NUM_TID)) {
+        if ((sta_ptr = wlan_get_station_entry(priv, ta)))
+            sta_ptr->rx_seq[prx_pd->priority] = prx_pd->seq_num;
+    }
+    /* check if UAP enable 11n */
+    if (!priv->is_11n_enabled ||
+        (!wlan_11n_get_rxreorder_tbl
+         ((mlan_private *) priv, prx_pd->priority, ta)
+         && (prx_pd->rx_pkt_type != PKT_TYPE_AMSDU)
+        )) {
+        if (priv->pkt_fwd)
+            wlan_process_uap_rx_packet(priv, pmbuf);
+        else
+            wlan_upload_uap_rx_packet(pmadapter, pmbuf);
+        goto done;
+    }
+    /* Reorder and send to OS */
+    if ((ret = mlan_11n_rxreorder_pkt(priv, prx_pd->seq_num,
+                                      prx_pd->priority, ta,
+                                      (t_u8) prx_pd->rx_pkt_type,
+                                      (void *) pmbuf))
+        || (rx_pkt_type == PKT_TYPE_BAR)) {
+        wlan_free_mlan_buffer(pmadapter, pmbuf);
+    }
+  done:
+    if (priv->is_11n_enabled &&
+        (pmadapter->pending_bridge_pkts >= RX_MED_THRESHOLD))
+        wlan_send_delba_to_all_in_reorder_tbl(priv);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function processes received packet and forwards it
+ *  		to kernel/upper layer or send back to firmware
+ *
+ *  @param priv      A pointer to mlan_private
+ *  @param pmbuf     A pointer to mlan_buffer which includes the received packet
+ *
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_uap_recv_packet(IN mlan_private * priv, IN pmlan_buffer pmbuf)
+{
+    pmlan_adapter pmadapter = priv->adapter;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    RxPacketHdr_t *prx_pkt;
+    pmlan_buffer newbuf = MNULL;
+
+    ENTER();
+
+    prx_pkt = (RxPacketHdr_t *) ((t_u8 *) pmbuf->pbuf + pmbuf->data_offset);
+
+    DBG_HEXDUMP(MDAT_D, "uap_recv_packet", pmbuf->pbuf + pmbuf->data_offset,
+                MIN(pmbuf->data_len, MAX_DATA_DUMP_LEN));
+
+    PRINTM(MDATA, "AMSDU dest %02x:%02x:%02x:%02x:%02x:%02x\n",
+           prx_pkt->eth803_hdr.dest_addr[0], prx_pkt->eth803_hdr.dest_addr[1],
+           prx_pkt->eth803_hdr.dest_addr[2], prx_pkt->eth803_hdr.dest_addr[3],
+           prx_pkt->eth803_hdr.dest_addr[4], prx_pkt->eth803_hdr.dest_addr[5]);
+
+    /* don't do packet forwarding in disconnected state */
+    if ((priv->media_connected == MFALSE) ||
+        (pmbuf->data_len > MV_ETH_FRAME_LEN))
+        goto upload;
+
+    if (prx_pkt->eth803_hdr.dest_addr[0] & 0x01) {
+        if (!(priv->pkt_fwd & PKT_FWD_INTRA_BCAST)) {
+            /* Multicast pkt */
+            if ((newbuf =
+                 wlan_alloc_mlan_buffer(pmadapter, MLAN_TX_DATA_BUF_SIZE_2K, 0,
+                                        MTRUE))) {
+                newbuf->bss_index = pmbuf->bss_index;
+                newbuf->buf_type = pmbuf->buf_type;
+                newbuf->priority = pmbuf->priority;
+                newbuf->in_ts_sec = pmbuf->in_ts_sec;
+                newbuf->in_ts_usec = pmbuf->in_ts_usec;
+                newbuf->data_offset =
+                    (sizeof(UapTxPD) + INTF_HEADER_LEN + DMA_ALIGNMENT);
+                pmadapter->pending_bridge_pkts++;
+                newbuf->flags |= MLAN_BUF_FLAG_BRIDGE_BUF;
+
+                /* copy the data */
+                memcpy(pmadapter, (t_u8 *) newbuf->pbuf + newbuf->data_offset,
+                       pmbuf->pbuf + pmbuf->data_offset, pmbuf->data_len);
+                newbuf->data_len = pmbuf->data_len;
+                wlan_wmm_add_buf_txqueue(pmadapter, newbuf);
+                if (pmadapter->pending_bridge_pkts > RX_HIGH_THRESHOLD)
+                    wlan_drop_tx_pkts(priv);
+            }
+        }
+    } else {
+        if ((!(priv->pkt_fwd & PKT_FWD_INTRA_UCAST)) &&
+            (wlan_get_station_entry(priv, prx_pkt->eth803_hdr.dest_addr))) {
+            /* Intra BSS packet */
+            if ((newbuf =
+                 wlan_alloc_mlan_buffer(pmadapter, MLAN_TX_DATA_BUF_SIZE_2K, 0,
+                                        MTRUE))) {
+                newbuf->bss_index = pmbuf->bss_index;
+                newbuf->buf_type = pmbuf->buf_type;
+                newbuf->priority = pmbuf->priority;
+                newbuf->in_ts_sec = pmbuf->in_ts_sec;
+                newbuf->in_ts_usec = pmbuf->in_ts_usec;
+                newbuf->data_offset =
+                    (sizeof(UapTxPD) + INTF_HEADER_LEN + DMA_ALIGNMENT);
+                pmadapter->pending_bridge_pkts++;
+                newbuf->flags |= MLAN_BUF_FLAG_BRIDGE_BUF;
+
+                /* copy the data */
+                memcpy(pmadapter, (t_u8 *) newbuf->pbuf + newbuf->data_offset,
+                       pmbuf->pbuf + pmbuf->data_offset, pmbuf->data_len);
+                newbuf->data_len = pmbuf->data_len;
+                wlan_wmm_add_buf_txqueue(pmadapter, newbuf);
+                if (pmadapter->pending_bridge_pkts > RX_HIGH_THRESHOLD)
+                    wlan_drop_tx_pkts(priv);
+            }
+            goto done;
+        } else if (MLAN_STATUS_FAILURE ==
+                   wlan_check_unicast_packet(priv,
+                                             prx_pkt->eth803_hdr.dest_addr)) {
+            /* drop packet */
+            PRINTM(MDATA, "Drop AMSDU dest %02x:%02x:%02x:%02x:%02x:%02x\n",
+                   prx_pkt->eth803_hdr.dest_addr[0],
+                   prx_pkt->eth803_hdr.dest_addr[1],
+                   prx_pkt->eth803_hdr.dest_addr[2],
+                   prx_pkt->eth803_hdr.dest_addr[3],
+                   prx_pkt->eth803_hdr.dest_addr[4],
+                   prx_pkt->eth803_hdr.dest_addr[5]);
+            goto done;
+        }
+    }
+  upload:
+    /** send packet to moal */
+    ret = pmadapter->callbacks.moal_recv_packet(pmadapter->pmoal_handle, pmbuf);
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function processes received packet and forwards it
+ *  		to kernel/upper layer or send back to firmware
+ *
+ *  @param priv      A pointer to mlan_private
+ *  @param pmbuf     A pointer to mlan_buffer which includes the received packet
+ *
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_process_uap_rx_packet(IN mlan_private * priv, IN pmlan_buffer pmbuf)
+{
+    pmlan_adapter pmadapter = priv->adapter;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    UapRxPD *prx_pd;
+    RxPacketHdr_t *prx_pkt;
+    pmlan_buffer newbuf = MNULL;
+
+    ENTER();
+
+    prx_pd = (UapRxPD *) (pmbuf->pbuf + pmbuf->data_offset);
+    prx_pkt = (RxPacketHdr_t *) ((t_u8 *) prx_pd + prx_pd->rx_pkt_offset);
+
+    DBG_HEXDUMP(MDAT_D, "uAP RxPD", prx_pd,
+                MIN(sizeof(UapRxPD), MAX_DATA_DUMP_LEN));
+    DBG_HEXDUMP(MDAT_D, "uAP Rx Payload",
+                ((t_u8 *) prx_pd + prx_pd->rx_pkt_offset),
+                MIN(prx_pd->rx_pkt_length, MAX_DATA_DUMP_LEN));
+
+    PRINTM(MINFO, "RX Data: data_len - prx_pd->rx_pkt_offset = %d - %d = %d\n",
+           pmbuf->data_len, prx_pd->rx_pkt_offset,
+           pmbuf->data_len - prx_pd->rx_pkt_offset);
+    PRINTM(MDATA, "Rx dest %02x:%02x:%02x:%02x:%02x:%02x\n",
+           prx_pkt->eth803_hdr.dest_addr[0], prx_pkt->eth803_hdr.dest_addr[1],
+           prx_pkt->eth803_hdr.dest_addr[2], prx_pkt->eth803_hdr.dest_addr[3],
+           prx_pkt->eth803_hdr.dest_addr[4], prx_pkt->eth803_hdr.dest_addr[5]);
+
+    /* don't do packet forwarding in disconnected state */
+    /* don't do packet forwarding when packet > 1514 */
+    if ((priv->media_connected == MFALSE) ||
+        ((pmbuf->data_len - prx_pd->rx_pkt_offset) > MV_ETH_FRAME_LEN))
+        goto upload;
+
+    if (prx_pkt->eth803_hdr.dest_addr[0] & 0x01) {
+        if (!(priv->pkt_fwd & PKT_FWD_INTRA_BCAST)) {
+            /* Multicast pkt */
+            if ((newbuf =
+                 wlan_alloc_mlan_buffer(pmadapter, MLAN_TX_DATA_BUF_SIZE_2K, 0,
+                                        MTRUE))) {
+                newbuf->bss_index = pmbuf->bss_index;
+                newbuf->buf_type = pmbuf->buf_type;
+                newbuf->priority = pmbuf->priority;
+                newbuf->in_ts_sec = pmbuf->in_ts_sec;
+                newbuf->in_ts_usec = pmbuf->in_ts_usec;
+                newbuf->data_offset =
+                    (sizeof(UapTxPD) + INTF_HEADER_LEN + DMA_ALIGNMENT);
+                pmadapter->pending_bridge_pkts++;
+                newbuf->flags |= MLAN_BUF_FLAG_BRIDGE_BUF;
+
+                /* copy the data, skip rxpd */
+                memcpy(pmadapter, (t_u8 *) newbuf->pbuf + newbuf->data_offset,
+                       pmbuf->pbuf + pmbuf->data_offset + prx_pd->rx_pkt_offset,
+                       pmbuf->data_len - prx_pd->rx_pkt_offset);
+                newbuf->data_len = pmbuf->data_len - prx_pd->rx_pkt_offset;
+                wlan_wmm_add_buf_txqueue(pmadapter, newbuf);
+                if (pmadapter->pending_bridge_pkts > RX_HIGH_THRESHOLD)
+                    wlan_drop_tx_pkts(priv);
+            }
+        }
+    } else {
+        if ((!(priv->pkt_fwd & PKT_FWD_INTRA_UCAST)) &&
+            (wlan_get_station_entry(priv, prx_pkt->eth803_hdr.dest_addr))) {
+            /* Forwarding Intra-BSS packet */
+            pmbuf->data_len -= prx_pd->rx_pkt_offset;
+            pmbuf->data_offset += prx_pd->rx_pkt_offset;
+            pmbuf->flags |= MLAN_BUF_FLAG_BRIDGE_BUF;
+            pmadapter->pending_bridge_pkts++;
+            wlan_wmm_add_buf_txqueue(pmadapter, pmbuf);
+            if (pmadapter->pending_bridge_pkts > RX_HIGH_THRESHOLD)
+                wlan_drop_tx_pkts(priv);
+            goto done;
+        } else if (MLAN_STATUS_FAILURE ==
+                   wlan_check_unicast_packet(priv,
+                                             prx_pkt->eth803_hdr.dest_addr)) {
+            PRINTM(MDATA, "Drop Pkts: Rx dest %02x:%02x:%02x:%02x:%02x:%02x\n",
+                   prx_pkt->eth803_hdr.dest_addr[0],
+                   prx_pkt->eth803_hdr.dest_addr[1],
+                   prx_pkt->eth803_hdr.dest_addr[2],
+                   prx_pkt->eth803_hdr.dest_addr[3],
+                   prx_pkt->eth803_hdr.dest_addr[4],
+                   prx_pkt->eth803_hdr.dest_addr[5]);
+            pmbuf->status_code = MLAN_ERROR_PKT_INVALID;
+            wlan_free_mlan_buffer(pmadapter, pmbuf);
+            goto done;
+        }
+    }
+  upload:
+    /* Chop off RxPD */
+    pmbuf->data_len -= prx_pd->rx_pkt_offset;
+    pmbuf->data_offset += prx_pd->rx_pkt_offset;
+    pmbuf->pparent = MNULL;
+
+    pmadapter->callbacks.moal_get_system_time(pmadapter->pmoal_handle,
+                                              &pmbuf->out_ts_sec,
+                                              &pmbuf->out_ts_usec);
+    PRINTM(MDATA, "%lu.%06lu : Data => kernel seq_num=%d tid=%d\n",
+           pmbuf->out_ts_sec, pmbuf->out_ts_usec, prx_pd->seq_num,
+           prx_pd->priority);
+    ret = pmadapter->callbacks.moal_recv_packet(pmadapter->pmoal_handle, pmbuf);
+    if (ret == MLAN_STATUS_FAILURE) {
+        PRINTM(MERROR, "uAP Rx Error: moal_recv_packet returned error\n");
+        pmbuf->status_code = MLAN_ERROR_PKT_INVALID;
+    }
+
+    if (ret != MLAN_STATUS_PENDING) {
+        wlan_free_mlan_buffer(pmadapter, pmbuf);
+    }
+  done:
+    LEAVE();
+    return ret;
+}
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_util.h b/drivers/net/wireless/sd8797/mlan/mlan_util.h
new file mode 100644
index 000000000000..6274f75054a0
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_util.h
@@ -0,0 +1,526 @@
+/** @file mlan_util.h
+ *
+ *  @brief This file contains wrappers for linked-list,
+ *  spinlock and timer defines.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    10/28/2008: initial version
+******************************************************/
+
+#ifndef _MLAN_UTIL_H_
+#define _MLAN_UTIL_H_
+
+/** Circular doubly linked list */
+typedef struct _mlan_linked_list
+{
+    /** Pointer to previous node */
+    struct _mlan_linked_list *pprev;
+    /** Pointer to next node */
+    struct _mlan_linked_list *pnext;
+} mlan_linked_list, *pmlan_linked_list;
+
+/** List head */
+typedef struct _mlan_list_head
+{
+    /** Pointer to previous node */
+    struct _mlan_linked_list *pprev;
+    /** Pointer to next node */
+    struct _mlan_linked_list *pnext;
+    /** Pointer to lock */
+    t_void *plock;
+} mlan_list_head, *pmlan_list_head;
+
+/**
+ *  @brief This function initializes a list without locking
+ *
+ *  @param phead		List head
+ *
+ *  @return			N/A
+ */
+static INLINE t_void
+util_init_list(pmlan_linked_list phead)
+{
+    /* Both next and prev point to self */
+    phead->pprev = phead->pnext = (pmlan_linked_list) phead;
+}
+
+/**
+ *  @brief This function initializes a list
+ *
+ *  @param phead		List head
+ *  @param lock_required	A flag for spinlock requirement
+ *  @param moal_init_lock	A pointer to init lock handler
+ *
+ *  @return			N/A
+ */
+static INLINE t_void
+util_init_list_head(t_void * pmoal_handle,
+                    pmlan_list_head phead,
+                    t_u8 lock_required,
+                    mlan_status(*moal_init_lock) (t_void * handle,
+                                                  t_void ** pplock))
+{
+    /* Both next and prev point to self */
+    util_init_list((pmlan_linked_list) phead);
+    if (lock_required)
+        moal_init_lock(pmoal_handle, &phead->plock);
+    else
+        phead->plock = 0;
+}
+
+/**
+ *  @brief This function frees a list
+ *
+ *  @param phead		List head
+ *  @param moal_free_lock	A pointer to free lock handler
+ *
+ *  @return			N/A
+ */
+static INLINE t_void
+util_free_list_head(t_void * pmoal_handle,
+                    pmlan_list_head phead,
+                    mlan_status(*moal_free_lock) (t_void * handle,
+                                                  t_void * plock))
+{
+    phead->pprev = phead->pnext = 0;
+    if (phead->plock)
+        moal_free_lock(pmoal_handle, phead->plock);
+}
+
+/**
+ *  @brief This function peeks into a list
+ *
+ *  @param phead		List head
+ *  @param moal_spin_lock	A pointer to spin lock handler
+ *  @param moal_spin_unlock	A pointer to spin unlock handler
+ *
+ *  @return			List node
+ */
+static INLINE pmlan_linked_list
+util_peek_list(t_void * pmoal_handle,
+               pmlan_list_head phead,
+               mlan_status(*moal_spin_lock) (t_void * handle, t_void * plock),
+               mlan_status(*moal_spin_unlock) (t_void * handle, t_void * plock))
+{
+    pmlan_linked_list pnode = 0;
+
+    if (moal_spin_lock)
+        moal_spin_lock(pmoal_handle, phead->plock);
+    if (phead->pnext != (pmlan_linked_list) phead) {
+        pnode = phead->pnext;
+    }
+    if (moal_spin_unlock)
+        moal_spin_unlock(pmoal_handle, phead->plock);
+    return pnode;
+}
+
+/**
+ *  @brief This function queues a node at the list tail
+ *
+ *  @param phead		List head
+ *  @param pnode		List node to queue
+ *  @param moal_spin_lock	A pointer to spin lock handler
+ *  @param moal_spin_unlock	A pointer to spin unlock handler
+ *
+ *  @return			N/A
+ */
+static INLINE t_void
+util_enqueue_list_tail(t_void * pmoal_handle,
+                       pmlan_list_head phead,
+                       pmlan_linked_list pnode,
+                       mlan_status(*moal_spin_lock) (t_void * handle,
+                                                     t_void * plock),
+                       mlan_status(*moal_spin_unlock) (t_void * handle,
+                                                       t_void * plock))
+{
+    pmlan_linked_list pold_last;
+
+    if (moal_spin_lock)
+        moal_spin_lock(pmoal_handle, phead->plock);
+    pold_last = phead->pprev;
+    pnode->pprev = pold_last;
+    pnode->pnext = (pmlan_linked_list) phead;
+
+    phead->pprev = pold_last->pnext = pnode;
+    if (moal_spin_unlock)
+        moal_spin_unlock(pmoal_handle, phead->plock);
+}
+
+/**
+ *  @brief This function adds a node at the list head
+ *
+ *  @param phead		List head
+ *  @param pnode		List node to add
+ *  @param moal_spin_lock	A pointer to spin lock handler
+ *  @param moal_spin_unlock	A pointer to spin unlock handler
+ *
+ *  @return			N/A
+ */
+static INLINE t_void
+util_enqueue_list_head(t_void * pmoal_handle,
+                       pmlan_list_head phead,
+                       pmlan_linked_list pnode,
+                       mlan_status(*moal_spin_lock) (t_void * handle,
+                                                     t_void * plock),
+                       mlan_status(*moal_spin_unlock) (t_void * handle,
+                                                       t_void * plock))
+{
+    pmlan_linked_list pold_first;
+
+    if (moal_spin_lock)
+        moal_spin_lock(pmoal_handle, phead->plock);
+    pold_first = phead->pnext;
+    pnode->pprev = (pmlan_linked_list) phead;
+    pnode->pnext = pold_first;
+
+    phead->pnext = pold_first->pprev = pnode;
+    if (moal_spin_unlock)
+        moal_spin_unlock(pmoal_handle, phead->plock);
+}
+
+/**
+ *  @brief This function removes a node from the list
+ *
+ *  @param phead		List head
+ *  @param pnode		List node to remove
+ *  @param moal_spin_lock	A pointer to spin lock handler
+ *  @param moal_spin_unlock	A pointer to spin unlock handler
+ *
+ *  @return			N/A
+ */
+static INLINE t_void
+util_unlink_list(t_void * pmoal_handle,
+                 pmlan_list_head phead,
+                 pmlan_linked_list pnode,
+                 mlan_status(*moal_spin_lock) (t_void * handle, t_void * plock),
+                 mlan_status(*moal_spin_unlock) (t_void * handle,
+                                                 t_void * plock))
+{
+    pmlan_linked_list pmy_prev;
+    pmlan_linked_list pmy_next;
+
+    if (moal_spin_lock)
+        moal_spin_lock(pmoal_handle, phead->plock);
+    pmy_prev = pnode->pprev;
+    pmy_next = pnode->pnext;
+    pmy_next->pprev = pmy_prev;
+    pmy_prev->pnext = pmy_next;
+
+    pnode->pnext = pnode->pprev = 0;
+    if (moal_spin_unlock)
+        moal_spin_unlock(pmoal_handle, phead->plock);
+}
+
+/**
+ *  @brief This function dequeues a node from the list
+ *
+ *  @param phead		List head
+ *  @param moal_spin_lock	A pointer to spin lock handler
+ *  @param moal_spin_unlock	A pointer to spin unlock handler
+ *
+ *  @return			List node
+ */
+static INLINE pmlan_linked_list
+util_dequeue_list(t_void * pmoal_handle,
+                  pmlan_list_head phead,
+                  mlan_status(*moal_spin_lock) (t_void * handle,
+                                                t_void * plock),
+                  mlan_status(*moal_spin_unlock) (t_void * handle,
+                                                  t_void * plock))
+{
+    pmlan_linked_list pnode;
+
+    if (moal_spin_lock)
+        moal_spin_lock(pmoal_handle, phead->plock);
+    pnode = phead->pnext;
+    if (pnode && (pnode != (pmlan_linked_list) phead)) {
+        util_unlink_list(pmoal_handle, phead, pnode, 0, 0);
+    } else {
+        pnode = 0;
+    }
+    if (moal_spin_unlock)
+        moal_spin_unlock(pmoal_handle, phead->plock);
+    return pnode;
+}
+
+/** Access controlled scalar variable */
+typedef struct _mlan_scalar
+{
+    /** Value */
+    t_s32 value;
+    /** Pointer to lock */
+    t_void *plock;
+    /** Control flags */
+    t_u32 flags;
+} mlan_scalar, *pmlan_scalar;
+
+/** Flag to scalar lock acquired */
+#define MLAN_SCALAR_FLAG_UNIQUE_LOCK	MBIT(16)
+
+/** scalar conditional value list */
+typedef enum _MLAN_SCALAR_CONDITIONAL
+{
+    MLAN_SCALAR_COND_EQUAL,
+    MLAN_SCALAR_COND_NOT_EQUAL,
+    MLAN_SCALAR_COND_GREATER_THAN,
+    MLAN_SCALAR_COND_GREATER_OR_EQUAL,
+    MLAN_SCALAR_COND_LESS_THAN,
+    MLAN_SCALAR_COND_LESS_OR_EQUAL
+} MLAN_SCALAR_CONDITIONAL;
+
+/**
+ *  @brief This function initializes a scalar
+ *
+ *  @param pscalar			Pointer to scalar
+ *  @param val				Initial scalar value
+ *  @param plock_to_use		A new lock is created if NULL, else lock to use
+ *  @param moal_init_lock	A pointer to init lock handler
+ *
+ *  @return					N/A
+ */
+static INLINE t_void
+util_scalar_init(t_void * pmoal_handle,
+                 pmlan_scalar pscalar,
+                 t_s32 val,
+                 t_void * plock_to_use,
+                 mlan_status(*moal_init_lock) (t_void * handle,
+                                               t_void ** pplock))
+{
+    pscalar->value = val;
+    pscalar->flags = 0;
+    if (plock_to_use) {
+        pscalar->flags &= ~MLAN_SCALAR_FLAG_UNIQUE_LOCK;
+        pscalar->plock = plock_to_use;
+    } else {
+        pscalar->flags |= MLAN_SCALAR_FLAG_UNIQUE_LOCK;
+        moal_init_lock(pmoal_handle, &pscalar->plock);
+    }
+}
+
+/**
+ *  @brief This function frees a scalar
+ *
+ *  @param pscalar			Pointer to scalar
+ *  @param moal_free_lock	A pointer to free lock handler
+ *
+ *  @return			N/A
+ */
+static INLINE t_void
+util_scalar_free(t_void * pmoal_handle,
+                 pmlan_scalar pscalar,
+                 mlan_status(*moal_free_lock) (t_void * handle, t_void * plock))
+{
+    if (pscalar->flags & MLAN_SCALAR_FLAG_UNIQUE_LOCK)
+        moal_free_lock(pmoal_handle, &pscalar->plock);
+}
+
+/**
+ *  @brief This function reads value from scalar
+ *
+ *  @param pscalar			Pointer to scalar
+ *  @param moal_spin_lock	A pointer to spin lock handler
+ *  @param moal_spin_unlock	A pointer to spin unlock handler
+ *
+ *  @return					Stored value
+ */
+static INLINE t_s32
+util_scalar_read(t_void * pmoal_handle,
+                 pmlan_scalar pscalar,
+                 mlan_status(*moal_spin_lock) (t_void * handle, t_void * plock),
+                 mlan_status(*moal_spin_unlock) (t_void * handle,
+                                                 t_void * plock))
+{
+    t_s32 val;
+
+    if (moal_spin_lock)
+        moal_spin_lock(pmoal_handle, pscalar->plock);
+    val = pscalar->value;
+    if (moal_spin_unlock)
+        moal_spin_unlock(pmoal_handle, pscalar->plock);
+
+    return val;
+}
+
+/**
+ *  @brief This function writes value to scalar
+ *
+ *  @param pscalar			Pointer to scalar
+ *  @param val				Value to write
+ *  @param moal_spin_lock	A pointer to spin lock handler
+ *  @param moal_spin_unlock	A pointer to spin unlock handler
+ *
+ *  @return					N/A
+ */
+static INLINE t_void
+util_scalar_write(t_void * pmoal_handle,
+                  pmlan_scalar pscalar,
+                  t_s32 val,
+                  mlan_status(*moal_spin_lock) (t_void * handle,
+                                                t_void * plock),
+                  mlan_status(*moal_spin_unlock) (t_void * handle,
+                                                  t_void * plock))
+{
+    if (moal_spin_lock)
+        moal_spin_lock(pmoal_handle, pscalar->plock);
+    pscalar->value = val;
+    if (moal_spin_unlock)
+        moal_spin_unlock(pmoal_handle, pscalar->plock);
+}
+
+/**
+ *  @brief This function increments the value in scalar
+ *
+ *  @param pscalar			Pointer to scalar
+ *  @param moal_spin_lock	A pointer to spin lock handler
+ *  @param moal_spin_unlock	A pointer to spin unlock handler
+ *
+ *  @return					N/A
+ */
+static INLINE t_void
+util_scalar_increment(t_void * pmoal_handle,
+                      pmlan_scalar pscalar,
+                      mlan_status(*moal_spin_lock) (t_void * handle,
+                                                    t_void * plock),
+                      mlan_status(*moal_spin_unlock) (t_void * handle,
+                                                      t_void * plock))
+{
+    if (moal_spin_lock)
+        moal_spin_lock(pmoal_handle, pscalar->plock);
+    pscalar->value++;
+    if (moal_spin_unlock)
+        moal_spin_unlock(pmoal_handle, pscalar->plock);
+}
+
+/**
+ *  @brief This function decrements the value in scalar
+ *
+ *  @param pscalar			Pointer to scalar
+ *  @param moal_spin_lock	A pointer to spin lock handler
+ *  @param moal_spin_unlock	A pointer to spin unlock handler
+ *
+ *  @return					N/A
+ */
+static INLINE t_void
+util_scalar_decrement(t_void * pmoal_handle,
+                      pmlan_scalar pscalar,
+                      mlan_status(*moal_spin_lock) (t_void * handle,
+                                                    t_void * plock),
+                      mlan_status(*moal_spin_unlock) (t_void * handle,
+                                                      t_void * plock))
+{
+    if (moal_spin_lock)
+        moal_spin_lock(pmoal_handle, pscalar->plock);
+    pscalar->value--;
+    if (moal_spin_unlock)
+        moal_spin_unlock(pmoal_handle, pscalar->plock);
+}
+
+/**
+ *  @brief This function adds an offset to the value in scalar,
+ *        and returns the new value
+ *
+ *  @param pscalar			Pointer to scalar
+ *  @param offset			Offset value (can be negative)
+ *  @param moal_spin_lock	A pointer to spin lock handler
+ *  @param moal_spin_unlock	A pointer to spin unlock handler
+ *
+ *  @return					Value after offset
+ */
+static INLINE t_s32
+util_scalar_offset(t_void * pmoal_handle,
+                   pmlan_scalar pscalar,
+                   t_s32 offset,
+                   mlan_status(*moal_spin_lock) (t_void * handle,
+                                                 t_void * plock),
+                   mlan_status(*moal_spin_unlock) (t_void * handle,
+                                                   t_void * plock))
+{
+    t_s32 newval;
+
+    if (moal_spin_lock)
+        moal_spin_lock(pmoal_handle, pscalar->plock);
+    newval = (pscalar->value += offset);
+    if (moal_spin_unlock)
+        moal_spin_unlock(pmoal_handle, pscalar->plock);
+
+    return newval;
+}
+
+/**
+ *  @brief This function writes the value to the scalar
+ *         if existing value compared with other value is true.
+ *
+ *  @param pscalar			Pointer to scalar
+ *  @param condition		Condition to check
+ *  @param val_compare		Value to compare against current value
+ *  						((A X B), where B = val_compare)
+ *  @param val_to_set		Value to set if comparison is true
+ *  @param moal_spin_lock	A pointer to spin lock handler
+ *  @param moal_spin_unlock	A pointer to spin unlock handler
+ *
+ *  @return					Comparison result (MTRUE or MFALSE)
+ */
+static INLINE t_u8
+util_scalar_conditional_write(t_void * pmoal_handle,
+                              pmlan_scalar pscalar,
+                              MLAN_SCALAR_CONDITIONAL condition,
+                              t_s32 val_compare,
+                              t_s32 val_to_set,
+                              mlan_status(*moal_spin_lock) (t_void * handle,
+                                                            t_void * plock),
+                              mlan_status(*moal_spin_unlock) (t_void * handle,
+                                                              t_void * plock))
+{
+    t_u8 update;
+    if (moal_spin_lock)
+        moal_spin_lock(pmoal_handle, pscalar->plock);
+
+    switch (condition) {
+    case MLAN_SCALAR_COND_EQUAL:
+        update = (pscalar->value == val_compare);
+        break;
+    case MLAN_SCALAR_COND_NOT_EQUAL:
+        update = (pscalar->value != val_compare);
+        break;
+    case MLAN_SCALAR_COND_GREATER_THAN:
+        update = (pscalar->value > val_compare);
+        break;
+    case MLAN_SCALAR_COND_GREATER_OR_EQUAL:
+        update = (pscalar->value >= val_compare);
+        break;
+    case MLAN_SCALAR_COND_LESS_THAN:
+        update = (pscalar->value < val_compare);
+        break;
+    case MLAN_SCALAR_COND_LESS_OR_EQUAL:
+        update = (pscalar->value <= val_compare);
+        break;
+    default:
+        update = MFALSE;
+        break;
+    }
+    if (update)
+        pscalar->value = val_to_set;
+
+    if (moal_spin_unlock)
+        moal_spin_unlock(pmoal_handle, pscalar->plock);
+    return (update) ? MTRUE : MFALSE;
+}
+
+#endif /* !_MLAN_UTIL_H_ */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_wmm.c b/drivers/net/wireless/sd8797/mlan/mlan_wmm.c
new file mode 100644
index 000000000000..4b72c9193e27
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_wmm.c
@@ -0,0 +1,2521 @@
+/** @file mlan_wmm.c
+ *
+ *  @brief This file contains functions for WMM.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/********************************************************
+Change log:
+    10/24/2008: initial version
+********************************************************/
+
+#include "mlan.h"
+#ifdef STA_SUPPORT
+#include "mlan_join.h"
+#endif
+#include "mlan_util.h"
+#include "mlan_fw.h"
+#include "mlan_main.h"
+#include "mlan_wmm.h"
+#include "mlan_11n.h"
+#include "mlan_sdio.h"
+
+/********************************************************
+    Local Variables
+********************************************************/
+
+/** Maximum value FW can accept for driver delay in packet transmission */
+#define DRV_PKT_DELAY_TO_FW_MAX   512
+
+/*
+ * Upper and Lower threshold for packet queuing in the driver
+
+ *    - When the number of packets queued reaches the upper limit,
+ *      the driver will stop the net queue in the app/kernel space.
+
+ *    - When the number of packets drops beneath the lower limit after
+ *      having reached the upper limit, the driver will restart the net
+ *      queue.
+ */
+
+/** Lower threshold for packet queuing in the driver.
+  * When the number of packets drops beneath the lower limit after having
+  * reached the upper limit, the driver will restart the net queue.
+  */
+#define WMM_QUEUED_PACKET_LOWER_LIMIT   180
+
+/** Upper threshold for packet queuing in the driver.
+  * When the number of packets queued reaches the upper limit, the driver
+  * will stop the net queue in the app/kernel space.
+  */
+#define WMM_QUEUED_PACKET_UPPER_LIMIT   200
+
+/** Offset for TOS field in the IP header */
+#define IPTOS_OFFSET 5
+
+/** WMM information IE */
+static const t_u8 wmm_info_ie[] = { WMM_IE, 0x07,
+    0x00, 0x50, 0xf2, 0x02,
+    0x00, 0x01, 0x00
+};
+
+/**
+ * AC Priorities go from AC_BK to AC_VO.  The ACI enumeration for AC_BK (1)
+ *   is higher than the enumeration for AC_BE (0); hence the needed
+ *   mapping conversion for wmm AC to priority Queue Index
+ */
+static const t_u8 wmm_aci_to_qidx_map[] = { WMM_AC_BE,
+    WMM_AC_BK,
+    WMM_AC_VI,
+    WMM_AC_VO
+};
+
+/**
+ * This table will be used to store the tid values based on ACs.
+ * It is initialized to default values per TID.
+ */
+t_u8 tos_to_tid[] = {
+    /* TID DSCP_P2 DSCP_P1 DSCP_P0 WMM_AC */
+    0x01,                       /* 0 1 0 AC_BK */
+    0x02,                       /* 0 0 0 AC_BK */
+    0x00,                       /* 0 0 1 AC_BE */
+    0x03,                       /* 0 1 1 AC_BE */
+    0x04,                       /* 1 0 0 AC_VI */
+    0x05,                       /* 1 0 1 AC_VI */
+    0x06,                       /* 1 1 0 AC_VO */
+    0x07                        /* 1 1 1 AC_VO */
+};
+
+/**
+ * This table inverses the tos_to_tid operation to get a priority
+ * which is in sequential order, and can be compared.
+ * Use this to compare the priority of two different TIDs.
+ */
+t_u8 tos_to_tid_inv[] = { 0x02, /* from tos_to_tid[2] = 0 */
+    0x00,                       /* from tos_to_tid[0] = 1 */
+    0x01,                       /* from tos_to_tid[1] = 2 */
+    0x03,
+    0x04,
+    0x05,
+    0x06,
+    0x07
+};
+
+/**
+ * This table will provide the tid value for given ac. This table does not
+ * change and will be used to copy back the default values to tos_to_tid in
+ * case of disconnect.
+ */
+const t_u8 ac_to_tid[4][2] = { {1, 2}, {0, 3}, {4, 5}, {6, 7} };
+
+/* Map of TOS UP values to WMM AC */
+static const mlan_wmm_ac_e tos_to_ac[] = { WMM_AC_BE,
+    WMM_AC_BK,
+    WMM_AC_BK,
+    WMM_AC_BE,
+    WMM_AC_VI,
+    WMM_AC_VI,
+    WMM_AC_VO,
+    WMM_AC_VO
+};
+
+raListTbl *wlan_wmm_get_ralist_node(pmlan_private priv, t_u8 tid,
+                                    t_u8 * ra_addr);
+
+/********************************************************
+    Local Functions
+********************************************************/
+#ifdef DEBUG_LEVEL2
+/**
+ *  @brief Debug print function to display the priority parameters for a WMM AC
+ *
+ *  @param pac_param	Pointer to the AC parameters to display
+ *
+ *  @return		N/A
+ */
+static void
+wlan_wmm_ac_debug_print(const IEEEtypes_WmmAcParameters_t * pac_param)
+{
+    const char *ac_str[] = { "BK", "BE", "VI", "VO" };
+
+    ENTER();
+
+    PRINTM(MINFO, "WMM AC_%s: ACI=%d, ACM=%d, Aifsn=%d, "
+           "EcwMin=%d, EcwMax=%d, TxopLimit=%d\n",
+           ac_str[wmm_aci_to_qidx_map[pac_param->aci_aifsn.aci]],
+           pac_param->aci_aifsn.aci, pac_param->aci_aifsn.acm,
+           pac_param->aci_aifsn.aifsn, pac_param->ecw.ecw_min,
+           pac_param->ecw.ecw_max, wlan_le16_to_cpu(pac_param->tx_op_limit));
+
+    LEAVE();
+}
+
+/** Print the WMM AC for debug purpose */
+#define PRINTM_AC(pac_param) wlan_wmm_ac_debug_print(pac_param)
+#else
+/** Print the WMM AC for debug purpose */
+#define PRINTM_AC(pac_param)
+#endif
+
+/**
+ *  @brief Allocate route address
+ *
+ *  @param pmadapter       Pointer to the mlan_adapter structure
+ *  @param ra              Pointer to the route address
+ *
+ *  @return         ra_list
+ */
+static raListTbl *
+wlan_wmm_allocate_ralist_node(pmlan_adapter pmadapter, t_u8 * ra)
+{
+    raListTbl *ra_list = MNULL;
+
+    ENTER();
+
+    if (pmadapter->callbacks.
+        moal_malloc(pmadapter->pmoal_handle, sizeof(raListTbl), MLAN_MEM_DEF,
+                    (t_u8 **) & ra_list)) {
+        PRINTM(MERROR, "Fail to allocate ra_list\n");
+        goto done;
+    }
+    util_init_list((pmlan_linked_list) ra_list);
+    util_init_list_head((t_void *) pmadapter->pmoal_handle,
+                        &ra_list->buf_head, MFALSE,
+                        pmadapter->callbacks.moal_init_lock);
+
+    memcpy(pmadapter, ra_list->ra, ra, MLAN_MAC_ADDR_LENGTH);
+
+    ra_list->total_pkts = 0;
+    ra_list->tx_pause = 0;
+    PRINTM(MINFO, "RAList: Allocating buffers for TID %p\n", ra_list);
+  done:
+    LEAVE();
+    return ra_list;
+}
+
+/**
+ * @brief Map ACs to TID
+ *
+ * @param priv             Pointer to the mlan_private driver data struct
+ * @param queue_priority   Queue_priority structure
+ *
+ * @return 	   N/A
+ */
+static void
+wlan_wmm_queue_priorities_tid(pmlan_private priv, t_u8 queue_priority[])
+{
+    int i;
+
+    ENTER();
+
+    for (i = 0; i < 4; ++i) {
+        tos_to_tid[7 - (i * 2)] = ac_to_tid[queue_priority[i]][1];
+        tos_to_tid[6 - (i * 2)] = ac_to_tid[queue_priority[i]][0];
+    }
+
+    for (i = 0; i < MAX_NUM_TID; i++) {
+        tos_to_tid_inv[tos_to_tid[i]] = (t_u8) i;
+    }
+
+    /* in case priorities have changed, force highest priority so next packet
+       will check from top to re-establish the highest */
+    util_scalar_write(priv->adapter->pmoal_handle,
+                      &priv->wmm.highest_queued_prio,
+                      HIGH_PRIO_TID,
+                      priv->adapter->callbacks.moal_spin_lock,
+                      priv->adapter->callbacks.moal_spin_unlock);
+
+    LEAVE();
+}
+
+/**
+ *  @brief Evaluate whether or not an AC is to be downgraded
+ *
+ *  @param priv     Pointer to the mlan_private driver data struct
+ *  @param eval_ac  AC to evaluate for downgrading
+ *
+ *  @return WMM AC  The eval_ac traffic is to be sent on.
+ */
+static mlan_wmm_ac_e
+wlan_wmm_eval_downgrade_ac(pmlan_private priv, mlan_wmm_ac_e eval_ac)
+{
+    int down_ac;
+    mlan_wmm_ac_e ret_ac;
+    WmmAcStatus_t *pac_status;
+
+    ENTER();
+
+    pac_status = &priv->wmm.ac_status[eval_ac];
+
+    if (pac_status->disabled == MFALSE) {
+        LEAVE();
+        /* Okay to use this AC, its enabled */
+        return eval_ac;
+    }
+
+    /* Setup a default return value of the lowest priority */
+    ret_ac = WMM_AC_BK;
+
+    /*
+     *  Find the highest AC that is enabled and does not require admission
+     *    control.  The spec disallows downgrading to an AC, which is enabled
+     *    due to a completed admission control.  Unadmitted traffic is not
+     *    to be sent on an AC with admitted traffic.
+     */
+    for (down_ac = WMM_AC_BK; down_ac < eval_ac; down_ac++) {
+        pac_status = &priv->wmm.ac_status[down_ac];
+
+        if ((pac_status->disabled == MFALSE)
+            && (pac_status->flow_required == MFALSE))
+            /* AC is enabled and does not require admission control */
+            ret_ac = (mlan_wmm_ac_e) down_ac;
+    }
+
+    LEAVE();
+    return ret_ac;
+}
+
+/**
+ *  @brief Convert the IP TOS field to an WMM AC Queue assignment
+ *
+ *  @param pmadapter A pointer to mlan_adapter structure
+ *  @param tos       IP TOS field
+ *
+ *  @return     WMM AC Queue mapping of the IP TOS field
+ */
+static mlan_wmm_ac_e INLINE
+wlan_wmm_convert_tos_to_ac(pmlan_adapter pmadapter, t_u32 tos)
+{
+    ENTER();
+
+    if (tos >= NELEMENTS(tos_to_ac)) {
+        LEAVE();
+        return WMM_AC_BE;
+    }
+
+    LEAVE();
+    return tos_to_ac[tos];
+}
+
+/**
+ *  @brief  Evaluate a given TID and downgrade it to a lower TID if the
+ *          WMM Parameter IE received from the AP indicates that the AP
+ *          is disabled (due to call admission control (ACM bit). Mapping
+ * 			of TID to AC is taken care internally
+ *
+ *  @param priv		Pointer to the mlan_private data struct
+ *  @param tid  	tid to evaluate for downgrading
+ *
+ *  @return       Same tid as input if downgrading not required or
+ *                the tid the traffic for the given tid should be downgraded to
+ */
+static t_u8 INLINE
+wlan_wmm_downgrade_tid(pmlan_private priv, t_u32 tid)
+{
+    mlan_wmm_ac_e ac_down;
+    pmlan_adapter pmadapter = priv->adapter;
+
+    ENTER();
+
+    ac_down =
+        priv->wmm.ac_down_graded_vals[wlan_wmm_convert_tos_to_ac(pmadapter,
+                                                                 tid)];
+    LEAVE();
+    /*
+     * Send the index to tid array, picking from the array will be
+     * taken care by dequeuing function
+     */
+    if (tid == 1 || tid == 2)
+        return ac_to_tid[ac_down][(tid + 1) % 2];
+    else if (tid >= MAX_NUM_TID)
+        return ac_to_tid[ac_down][0];
+    else
+        return ac_to_tid[ac_down][tid % 2];
+}
+
+/**
+ *  @brief Delete packets in RA node
+ *
+ *  @param priv			Pointer to the mlan_private driver data struct
+ *  @param ra_list	    	Pointer to raListTbl
+ *
+ *  @return		N/A
+ */
+static INLINE void
+wlan_wmm_del_pkts_in_ralist_node(pmlan_private priv, raListTbl * ra_list)
+{
+    pmlan_buffer pmbuf;
+    pmlan_adapter pmadapter = priv->adapter;
+
+    ENTER();
+    while ((pmbuf =
+            (pmlan_buffer) util_peek_list(pmadapter->pmoal_handle,
+                                          &ra_list->buf_head, MNULL, MNULL))) {
+        util_unlink_list(pmadapter->pmoal_handle, &ra_list->buf_head,
+                         (pmlan_linked_list) pmbuf, MNULL, MNULL);
+        wlan_write_data_complete(pmadapter, pmbuf, MLAN_STATUS_FAILURE);
+    }
+    util_free_list_head((t_void *) pmadapter->pmoal_handle, &ra_list->buf_head,
+                        pmadapter->callbacks.moal_free_lock);
+
+    LEAVE();
+}
+
+/**
+ *  @brief Delete packets in RA list
+ *
+ *  @param priv			Pointer to the mlan_private driver data struct
+ *  @param ra_list_head	ra list header
+ *
+ *  @return		N/A
+ */
+static INLINE void
+wlan_wmm_del_pkts_in_ralist(pmlan_private priv, mlan_list_head * ra_list_head)
+{
+    raListTbl *ra_list;
+
+    ENTER();
+
+    ra_list =
+        (raListTbl *) util_peek_list(priv->adapter->pmoal_handle, ra_list_head,
+                                     MNULL, MNULL);
+
+    while (ra_list && ra_list != (raListTbl *) ra_list_head) {
+        wlan_wmm_del_pkts_in_ralist_node(priv, ra_list);
+
+        ra_list = ra_list->pnext;
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief Clean up the wmm queue
+ *
+ *  @param priv  Pointer to the mlan_private driver data struct
+ *
+ *  @return      N/A
+ */
+static void
+wlan_wmm_cleanup_queues(pmlan_private priv)
+{
+    int i;
+
+    ENTER();
+
+    for (i = 0; i < MAX_NUM_TID; i++) {
+        wlan_wmm_del_pkts_in_ralist(priv, &priv->wmm.tid_tbl_ptr[i].ra_list);
+        priv->wmm.pkts_queued[i] = 0;
+    }
+    util_scalar_write(priv->adapter->pmoal_handle, &priv->wmm.tx_pkts_queued, 0,
+                      MNULL, MNULL);
+    util_scalar_write(priv->adapter->pmoal_handle,
+                      &priv->wmm.highest_queued_prio, HIGH_PRIO_TID, MNULL,
+                      MNULL);
+
+    LEAVE();
+}
+
+/**
+ *  @brief Delete all route address from RA list
+ *
+ *  @param priv     Pointer to the mlan_private driver data struct
+ *
+ *  @return         N/A
+ */
+static void
+wlan_wmm_delete_all_ralist(pmlan_private priv)
+{
+    raListTbl *ra_list;
+    int i;
+    pmlan_adapter pmadapter = priv->adapter;
+
+    ENTER();
+
+    for (i = 0; i < MAX_NUM_TID; ++i) {
+        PRINTM(MINFO, "RAList: Freeing buffers for TID %d\n", i);
+        while ((ra_list = (raListTbl *) util_peek_list(pmadapter->pmoal_handle,
+                                                       &priv->wmm.
+                                                       tid_tbl_ptr[i].ra_list,
+                                                       MNULL, MNULL))) {
+            util_unlink_list(pmadapter->pmoal_handle,
+                             &priv->wmm.tid_tbl_ptr[i].ra_list,
+                             (pmlan_linked_list) ra_list, MNULL, MNULL);
+
+            pmadapter->callbacks.moal_mfree(pmadapter->pmoal_handle,
+                                            (t_u8 *) ra_list);
+        }
+
+        util_init_list((pmlan_linked_list)
+                       & priv->wmm.tid_tbl_ptr[i].ra_list);
+        priv->wmm.tid_tbl_ptr[i].ra_list_curr = MNULL;
+    }
+
+    LEAVE();
+}
+
+/**
+ *   @brief Get queue RA pointer
+ *
+ *   @param priv     Pointer to the mlan_private driver data struct
+ *   @param tid      TID
+ *   @param ra_addr  Pointer to the route address
+ *
+ *   @return         ra_list
+ */
+static raListTbl *
+wlan_wmm_get_queue_raptr(pmlan_private priv, t_u8 tid, t_u8 * ra_addr)
+{
+    raListTbl *ra_list;
+#if defined(UAP_SUPPORT)
+    t_u8 bcast_addr[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+#endif
+
+    ENTER();
+    ra_list = wlan_wmm_get_ralist_node(priv, tid, ra_addr);
+    if (ra_list) {
+        LEAVE();
+        return ra_list;
+    }
+#if defined(UAP_SUPPORT)
+    if ((GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP) &&
+        (0 != memcmp(priv->adapter, ra_addr, bcast_addr, sizeof(bcast_addr)))) {
+        if (MNULL == wlan_get_station_entry(priv, ra_addr)) {
+            PRINTM(MDATA, "Drop packets to unknown station\n");
+            LEAVE();
+            return MNULL;
+        }
+    }
+#endif
+    wlan_ralist_add(priv, ra_addr);
+
+    ra_list = wlan_wmm_get_ralist_node(priv, tid, ra_addr);
+    LEAVE();
+    return ra_list;
+}
+
+#ifdef STA_SUPPORT
+/**
+ * 	@brief Sends wmmac host event
+ *
+ *  @param priv		Pointer to the mlan_private driver data struct
+ *  @param typeStr	Type of host event
+ *  @param srcAddr	Pointer to the source Address
+ *  @param tid    	TID
+ *  @param up		User priority
+ *  @param status	Status code or Reason code
+ *
+ *  @return		N/A
+ */
+static void
+wlan_send_wmmac_host_event(pmlan_private priv,
+                           char *typeStr,
+                           t_u8 * srcAddr, t_u8 tid, t_u8 up, t_u8 status)
+{
+    t_u8 event_buf[100];
+    mlan_event *pevent;
+    t_u8 *pOutBuf;
+
+    ENTER();
+
+    /* Format one of the following two output strings: ** -
+       TSPEC:ADDTS_RSP:[<status code>]:TID=X:UP=Y ** - TSPEC:DELTS_RX:[<reason
+       code>]:TID=X:UP=Y */
+    pevent = (mlan_event *) event_buf;
+    pOutBuf = pevent->event_buf;
+
+    memcpy(priv->adapter, pOutBuf, (t_u8 *) "TSPEC:", 6);
+    pOutBuf += 6;
+
+    memcpy(priv->adapter, pOutBuf, (t_u8 *) typeStr, wlan_strlen(typeStr));
+    pOutBuf += wlan_strlen(typeStr);
+
+    *pOutBuf++ = ':';
+    *pOutBuf++ = '[';
+
+    if (status >= 100) {
+        *pOutBuf++ = (status / 100) + '0';
+        status = (status % 100);
+    }
+
+    if (status >= 10) {
+        *pOutBuf++ = (status / 10) + '0';
+        status = (status % 10);
+    }
+
+    *pOutBuf++ = status + '0';
+
+    memcpy(priv->adapter, pOutBuf, (t_u8 *) "]:TID", 5);
+    pOutBuf += 5;
+    *pOutBuf++ = tid + '0';
+
+    memcpy(priv->adapter, pOutBuf, (t_u8 *) ":UP", 3);
+    pOutBuf += 3;
+    *pOutBuf++ = up + '0';
+
+    *pOutBuf = '\0';
+
+    pevent->bss_index = priv->bss_index;
+    pevent->event_id = MLAN_EVENT_ID_DRV_REPORT_STRING;
+    pevent->event_len = wlan_strlen((const t_s8 *) (pevent->event_buf));
+
+    wlan_recv_event(priv, MLAN_EVENT_ID_DRV_REPORT_STRING, pevent);
+    LEAVE();
+}
+#endif /* STA_SUPPORT */
+
+/**
+ *  @brief This function gets the highest priority list pointer
+ *
+ *  @param pmadapter      A pointer to mlan_adapter
+ *  @param priv           A pointer to mlan_private
+ *  @param tid            A pointer to return tid
+ *
+ *  @return             raListTbl
+ */
+static raListTbl *
+wlan_wmm_get_highest_priolist_ptr(pmlan_adapter pmadapter,
+                                  pmlan_private * priv, int *tid)
+{
+    pmlan_private priv_tmp;
+    raListTbl *ptr, *head;
+    mlan_bssprio_node *bssprio_node, *bssprio_head;
+    tid_tbl_t *tid_ptr;
+    int i, j;
+    int next_prio = 0;
+    int next_tid = 0;
+    ENTER();
+
+    PRINTM(MDAT_D, "POP\n");
+    for (j = pmadapter->priv_num - 1; j >= 0; --j) {
+        if (!(util_peek_list(pmadapter->pmoal_handle,
+                             &pmadapter->bssprio_tbl[j].bssprio_head,
+                             pmadapter->callbacks.moal_spin_lock,
+                             pmadapter->callbacks.moal_spin_unlock)))
+            continue;
+
+        if (pmadapter->bssprio_tbl[j].bssprio_cur == (mlan_bssprio_node *)
+            & pmadapter->bssprio_tbl[j].bssprio_head) {
+            pmadapter->bssprio_tbl[j].bssprio_cur =
+                pmadapter->bssprio_tbl[j].bssprio_cur->pnext;
+        }
+
+        bssprio_head = bssprio_node = pmadapter->bssprio_tbl[j].bssprio_cur;
+
+        do {
+            priv_tmp = bssprio_node->priv;
+
+            if ((priv_tmp->port_ctrl_mode == MTRUE)
+                && (priv_tmp->port_open == MFALSE)) {
+                PRINTM(MINFO, "get_highest_prio_ptr(): "
+                       "PORT_CLOSED Ignore pkts from BSS%d\n",
+                       priv_tmp->bss_index);
+                /* Ignore data pkts from a BSS if port is closed */
+                goto next_intf;
+            }
+
+            pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                                priv_tmp->wmm.ra_list_spinlock);
+
+            for (i = util_scalar_read(pmadapter->pmoal_handle,
+                                      &priv_tmp->wmm.highest_queued_prio,
+                                      MNULL, MNULL); i >= LOW_PRIO_TID; --i) {
+
+                tid_ptr = &(priv_tmp)->wmm.tid_tbl_ptr[tos_to_tid[i]];
+                if (!util_peek_list
+                    (pmadapter->pmoal_handle, &tid_ptr->ra_list, MNULL, MNULL))
+                    continue;
+
+                /*
+                 * Always choose the next ra we transmitted
+                 * last time, this way we pick the ra's in
+                 * round robin fashion.
+                 */
+                head = ptr = tid_ptr->ra_list_curr->pnext;
+                if (ptr == (raListTbl *) & tid_ptr->ra_list)
+                    head = ptr = ptr->pnext;
+
+                do {
+                    if (!ptr->tx_pause &&
+                        util_peek_list(pmadapter->pmoal_handle, &ptr->buf_head,
+                                       MNULL, MNULL)) {
+
+                        /* Because WMM only support BK/BE/VI/VO, we have 8 tid
+                           We should balance the traffic of the same AC */
+                        if (i % 2)
+                            next_prio = i - 1;
+                        else
+                            next_prio = i + 1;
+                        next_tid = tos_to_tid[next_prio];
+                        if (priv_tmp->wmm.pkts_queued[next_tid])
+                            util_scalar_write(pmadapter->pmoal_handle,
+                                              &priv_tmp->wmm.
+                                              highest_queued_prio, next_prio,
+                                              MNULL, MNULL);
+                        else
+                            /* if highest_queued_prio > i, set it to i */
+                            util_scalar_conditional_write(pmadapter->
+                                                          pmoal_handle,
+                                                          &priv_tmp->wmm.
+                                                          highest_queued_prio,
+                                                          MLAN_SCALAR_COND_GREATER_THAN,
+                                                          i, i, MNULL, MNULL);
+                        *priv = priv_tmp;
+                        *tid = tos_to_tid[i];
+                        /* hold priv->ra_list_spinlock to maintain ptr */
+                        PRINTM(MDAT_D, "get highest prio ptr %p, tid %d\n",
+                               ptr, *tid);
+                        LEAVE();
+                        return ptr;
+                    }
+
+                    if ((ptr = ptr->pnext) == (raListTbl *) & tid_ptr->ra_list)
+                        ptr = ptr->pnext;
+                } while (ptr != head);
+            }
+
+            /* No packet at any TID for this priv.  Mark as such to skip
+               checking TIDs for this priv (until pkt is added). */
+            util_scalar_write(pmadapter->pmoal_handle,
+                              &priv_tmp->wmm.highest_queued_prio,
+                              NO_PKT_PRIO_TID, MNULL, MNULL);
+
+            pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                                  priv_tmp->wmm.
+                                                  ra_list_spinlock);
+
+          next_intf:
+            if ((bssprio_node = bssprio_node->pnext) == (mlan_bssprio_node *)
+                & pmadapter->bssprio_tbl[j].bssprio_head)
+                bssprio_node = bssprio_node->pnext;
+        } while (bssprio_node != bssprio_head);
+    }
+
+    LEAVE();
+    return MNULL;
+}
+
+/**
+ *  @brief This function gets the number of packets in the Tx queue
+ *
+ *  @param priv         A pointer to mlan_private
+ *  @param ptr          A pointer to RA list table
+ *  @param maxBufSize   Maximum buffer size
+ *
+ *  @return             Packet count
+ */
+static int
+wlan_num_pkts_in_txq(mlan_private * priv, raListTbl * ptr, int maxBufSize)
+{
+    int count = 0, total_size = 0;
+    pmlan_buffer pmbuf;
+
+    ENTER();
+
+    for (pmbuf = (pmlan_buffer) ptr->buf_head.pnext;
+         pmbuf != (pmlan_buffer) (&ptr->buf_head); pmbuf = pmbuf->pnext) {
+
+        total_size += pmbuf->data_len;
+        if (total_size < maxBufSize)
+            ++count;
+        else
+            break;
+    }
+
+    LEAVE();
+    return count;
+}
+
+/**
+ *  @brief This function sends a single packet
+ *
+ *  @param priv         A pointer to mlan_private
+ *  @param ptr          A pointer to RA list table
+ *  @param ptrindex     ptr's TID index
+ *
+ *  @return             N/A
+ */
+static void INLINE
+wlan_send_single_packet(pmlan_private priv, raListTbl * ptr, int ptrindex)
+{
+    pmlan_buffer pmbuf;
+    pmlan_buffer pmbuf_next;
+    mlan_tx_param tx_param;
+    pmlan_adapter pmadapter = priv->adapter;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    if ((pmbuf = (pmlan_buffer) util_dequeue_list(pmadapter->pmoal_handle,
+                                                  &ptr->buf_head,
+                                                  MNULL, MNULL))) {
+        PRINTM(MINFO, "Dequeuing the packet %p %p\n", ptr, pmbuf);
+        priv->wmm.pkts_queued[ptrindex]--;
+        util_scalar_decrement(pmadapter->pmoal_handle,
+                              &priv->wmm.tx_pkts_queued, MNULL, MNULL);
+        ptr->total_pkts--;
+        pmbuf_next = (pmlan_buffer) util_peek_list(pmadapter->pmoal_handle,
+                                                   &ptr->buf_head,
+                                                   MNULL, MNULL);
+        pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                              priv->wmm.ra_list_spinlock);
+
+        tx_param.next_pkt_len = ((pmbuf_next)
+                                 ? pmbuf_next->data_len + sizeof(TxPD) : 0);
+        status = wlan_process_tx(priv, pmbuf, &tx_param);
+
+        if (status == MLAN_STATUS_RESOURCE) {
+            /** Queue the packet back at the head */
+            PRINTM(MDAT_D, "Queuing pkt back to raList %p %p\n", ptr, pmbuf);
+            pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                                priv->wmm.ra_list_spinlock);
+
+            if (!wlan_is_ralist_valid(priv, ptr, ptrindex)) {
+                pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                                      priv->wmm.
+                                                      ra_list_spinlock);
+                wlan_write_data_complete(pmadapter, pmbuf, MLAN_STATUS_FAILURE);
+                LEAVE();
+                return;
+            }
+            priv->wmm.pkts_queued[ptrindex]++;
+            util_scalar_increment(pmadapter->pmoal_handle,
+                                  &priv->wmm.tx_pkts_queued, MNULL, MNULL);
+            util_enqueue_list_head(pmadapter->pmoal_handle, &ptr->buf_head,
+                                   (pmlan_linked_list) pmbuf, MNULL, MNULL);
+
+            ptr->total_pkts++;
+            pmbuf->flags |= MLAN_BUF_FLAG_REQUEUED_PKT;
+            pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                                  priv->wmm.ra_list_spinlock);
+        } else {
+            pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                                priv->wmm.ra_list_spinlock);
+            if (wlan_is_ralist_valid(priv, ptr, ptrindex)) {
+                priv->wmm.packets_out[ptrindex]++;
+                priv->wmm.tid_tbl_ptr[ptrindex].ra_list_curr = ptr;
+            }
+            pmadapter->bssprio_tbl[priv->bss_priority].bssprio_cur =
+                pmadapter->bssprio_tbl[priv->bss_priority].bssprio_cur->pnext;
+            pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                                  priv->wmm.ra_list_spinlock);
+        }
+    } else {
+        pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                              priv->wmm.ra_list_spinlock);
+        PRINTM(MINFO, "Nothing to send\n");
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function checks if this mlan_buffer is already processed.
+ *
+ *  @param priv     A pointer to mlan_private
+ *  @param ptr      A pointer to RA list table
+ *
+ *  @return 	    MTRUE or MFALSE
+ */
+static int INLINE
+wlan_is_ptr_processed(mlan_private * priv, raListTbl * ptr)
+{
+    pmlan_buffer pmbuf;
+
+    if ((pmbuf = (pmlan_buffer) util_peek_list(priv->adapter->pmoal_handle,
+                                               &ptr->buf_head, MNULL, MNULL))
+        && (pmbuf->flags & MLAN_BUF_FLAG_REQUEUED_PKT))
+        return MTRUE;
+
+    return MFALSE;
+}
+
+/**
+ *  @brief This function sends a single packet that has been processed
+ *
+ *  @param priv         A pointer to mlan_private
+ *  @param ptr          A pointer to RA list table
+ *  @param ptrindex     ptr's TID index
+ *
+ *  @return             N/A
+ */
+static void INLINE
+wlan_send_processed_packet(pmlan_private priv, raListTbl * ptr, int ptrindex)
+{
+    pmlan_buffer pmbuf_next;
+    mlan_tx_param tx_param;
+    pmlan_buffer pmbuf;
+    pmlan_adapter pmadapter = priv->adapter;
+    mlan_status ret = MLAN_STATUS_FAILURE;
+
+    if ((pmbuf = (pmlan_buffer) util_dequeue_list(pmadapter->pmoal_handle,
+                                                  &ptr->buf_head,
+                                                  MNULL, MNULL))) {
+        pmbuf_next =
+            (pmlan_buffer) util_peek_list(pmadapter->pmoal_handle,
+                                          &ptr->buf_head, MNULL, MNULL);
+        pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                              priv->wmm.ra_list_spinlock);
+        tx_param.next_pkt_len =
+            ((pmbuf_next) ? pmbuf_next->data_len + sizeof(TxPD) : 0);
+        ret =
+            wlan_sdio_host_to_card(pmadapter, MLAN_TYPE_DATA, pmbuf, &tx_param);
+        switch (ret) {
+        case MLAN_STATUS_RESOURCE:
+            PRINTM(MINFO, "MLAN_STATUS_RESOURCE is returned\n");
+            pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                                priv->wmm.ra_list_spinlock);
+
+            if (!wlan_is_ralist_valid(priv, ptr, ptrindex)) {
+                pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                                      priv->wmm.
+                                                      ra_list_spinlock);
+                wlan_write_data_complete(pmadapter, pmbuf, MLAN_STATUS_FAILURE);
+                LEAVE();
+                return;
+            }
+            util_enqueue_list_head(pmadapter->pmoal_handle,
+                                   &ptr->buf_head,
+                                   (pmlan_linked_list) pmbuf, MNULL, MNULL);
+
+            pmbuf->flags |= MLAN_BUF_FLAG_REQUEUED_PKT;
+            pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                                  priv->wmm.ra_list_spinlock);
+            break;
+        case MLAN_STATUS_FAILURE:
+            pmadapter->data_sent = MFALSE;
+            PRINTM(MERROR, "Error: Failed to write data\n");
+            pmadapter->dbg.num_tx_host_to_card_failure++;
+            pmbuf->status_code = MLAN_ERROR_DATA_TX_FAIL;
+            wlan_write_data_complete(pmadapter, pmbuf, ret);
+            break;
+        case MLAN_STATUS_PENDING:
+            pmadapter->data_sent = MFALSE;
+        default:
+            break;
+        }
+        if (ret != MLAN_STATUS_RESOURCE) {
+            pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                                priv->wmm.ra_list_spinlock);
+            if (wlan_is_ralist_valid(priv, ptr, ptrindex)) {
+                priv->wmm.packets_out[ptrindex]++;
+                priv->wmm.tid_tbl_ptr[ptrindex].ra_list_curr = ptr;
+                ptr->total_pkts--;
+            }
+            pmadapter->bssprio_tbl[priv->bss_priority].bssprio_cur =
+                pmadapter->bssprio_tbl[priv->bss_priority].bssprio_cur->pnext;
+            priv->wmm.pkts_queued[ptrindex]--;
+            util_scalar_decrement(pmadapter->pmoal_handle,
+                                  &priv->wmm.tx_pkts_queued, MNULL, MNULL);
+            pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                                  priv->wmm.ra_list_spinlock);
+        }
+    } else {
+        pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                              priv->wmm.ra_list_spinlock);
+    }
+}
+
+/**
+ *  @brief This function dequeues a packet
+ *
+ *  @param pmadapter  A pointer to mlan_adapter
+ *
+ *  @return 	    MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static int
+wlan_dequeue_tx_packet(pmlan_adapter pmadapter)
+{
+    raListTbl *ptr;
+    pmlan_private priv = MNULL;
+    int ptrindex = 0;
+    t_u8 ra[MLAN_MAC_ADDR_LENGTH];
+    int tid_del = 0;
+    int tid = 0;
+
+    ENTER();
+
+    if (!(ptr = wlan_wmm_get_highest_priolist_ptr(pmadapter, &priv, &ptrindex))) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    /* Note:- Spinlock is locked in wlan_wmm_get_highest_priolist_ptr when it
+       returns a pointer (for the priv it returns), and is unlocked in
+       wlan_send_processed_packet, wlan_send_single_packet or
+       wlan_11n_aggregate_pkt.  The spinlock would be required for some parts
+       of both of function.  But, the the bulk of these function will execute
+       w/o spinlock.  Unlocking the spinlock inside these function will help
+       us avoid taking the spinlock again, check to see if the ptr is still
+       valid and then proceed. This is done purely to increase execution time.
+     */
+
+    /* Note:- Also, anybody adding code which does not get into
+       wlan_send_processed_packet, wlan_send_single_packet, or
+       wlan_11n_aggregate_pkt should make sure ra_list_spinlock is freed.
+       Otherwise there would be a lock up. */
+
+    tid = wlan_get_tid(priv->adapter, ptr);
+    if (tid >= MAX_NUM_TID)
+        tid = wlan_wmm_downgrade_tid(priv, tid);
+
+    if (wlan_is_ptr_processed(priv, ptr)) {
+        wlan_send_processed_packet(priv, ptr, ptrindex);
+        LEAVE();
+        return MLAN_STATUS_SUCCESS;
+    }
+
+    if (!ptr->is_11n_enabled || wlan_is_bastream_setup(priv, ptr, tid)
+#ifdef STA_SUPPORT
+        || ((priv->sec_info.ewpa_enabled == MFALSE) &&
+            ((priv->sec_info.wpa_enabled
+              || priv->sec_info.wpa2_enabled) &&
+             priv->wpa_is_gtk_set == MFALSE))
+        || priv->wps.session_enable
+#endif /* STA_SUPPORT */
+        ) {
+        if (ptr->is_11n_enabled && wlan_is_bastream_setup(priv, ptr, tid)
+            && wlan_is_amsdu_in_ampdu_allowed(priv, ptr, tid)
+            && wlan_is_amsdu_allowed(priv, ptr, tid)
+            && (wlan_num_pkts_in_txq(priv, ptr, pmadapter->tx_buf_size) >=
+                MIN_NUM_AMSDU)) {
+            wlan_11n_aggregate_pkt(priv, ptr, INTF_HEADER_LEN, ptrindex);
+        } else
+            wlan_send_single_packet(priv, ptr, ptrindex);
+    } else {
+        if (wlan_is_ampdu_allowed(priv, ptr, tid) &&
+            (ptr->packet_count > ptr->ba_packet_threshold)) {
+            if (wlan_is_bastream_avail(priv)) {
+                PRINTM(MINFO, "BA setup threshold %d reached. tid=%d\n",
+                       ptr->packet_count, tid);
+                wlan_11n_create_txbastream_tbl(priv,
+                                               ptr->ra, tid,
+                                               BA_STREAM_SETUP_INPROGRESS);
+                wlan_send_addba(priv, tid, ptr->ra);
+            } else if (wlan_find_stream_to_delete(priv, ptr, tid, &tid_del, ra)) {
+                PRINTM(MDAT_D, "tid_del=%d tid=%d\n", tid_del, tid);
+                wlan_11n_create_txbastream_tbl(priv,
+                                               ptr->ra, tid,
+                                               BA_STREAM_SETUP_INPROGRESS);
+                wlan_send_delba(priv, tid_del, ra, 1);
+            }
+        }
+        if (wlan_is_amsdu_allowed(priv, ptr, tid) &&
+            (wlan_num_pkts_in_txq(priv, ptr,
+                                  pmadapter->tx_buf_size) >= MIN_NUM_AMSDU)) {
+            wlan_11n_aggregate_pkt(priv, ptr, INTF_HEADER_LEN, ptrindex);
+        } else {
+            wlan_send_single_packet(priv, ptr, ptrindex);
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief update tx_pause flag in ra_list
+ *
+ *  @param priv		  A pointer to mlan_private
+ *  @param mac        peer mac address
+ *  @param tx_pause   tx_pause flag (0/1)
+ *
+ *  @return           N/A
+ */
+t_void
+wlan_updata_ralist_tx_pause(pmlan_private priv, t_u8 * mac, t_u8 tx_pause)
+{
+    raListTbl *ra_list;
+    int i;
+    pmlan_adapter pmadapter = priv->adapter;
+    t_u32 pkt_cnt = 0;
+    t_u32 tx_pkts_queued = 0;
+    ENTER();
+
+    pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                        priv->wmm.ra_list_spinlock);
+    for (i = 0; i < MAX_NUM_TID; ++i) {
+        ra_list = wlan_wmm_get_ralist_node(priv, i, mac);
+        if (ra_list) {
+            pkt_cnt += ra_list->total_pkts;
+            ra_list->tx_pause = tx_pause;
+        }
+    }
+    if (pkt_cnt) {
+        tx_pkts_queued = util_scalar_read(pmadapter->pmoal_handle,
+                                          &priv->wmm.tx_pkts_queued, MNULL,
+                                          MNULL);
+        if (tx_pause)
+            tx_pkts_queued -= pkt_cnt;
+        else
+            tx_pkts_queued += pkt_cnt;
+        util_scalar_write(priv->adapter->pmoal_handle,
+                          &priv->wmm.tx_pkts_queued, tx_pkts_queued, MNULL,
+                          MNULL);
+        util_scalar_write(priv->adapter->pmoal_handle,
+                          &priv->wmm.highest_queued_prio, HIGH_PRIO_TID, MNULL,
+                          MNULL);
+    }
+    pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                          priv->wmm.ra_list_spinlock);
+    LEAVE();
+}
+
+#ifdef STA_SUPPORT
+#endif /* STA_SUPPORT */
+/********************************************************
+    Global Functions
+********************************************************/
+
+/**
+ *  @brief Get the threshold value for BA setup using system time.
+ *
+ *  @param pmadapter       Pointer to the mlan_adapter structure
+ *
+ *  @return         threshold value.
+ */
+t_u8
+wlan_get_random_ba_threshold(pmlan_adapter pmadapter)
+{
+    t_u32 sec, usec;
+    t_u8 ba_threshold = 0;
+
+    ENTER();
+
+    /* setup ba_packet_threshold here random number between
+       [BA_SETUP_PACKET_OFFSET,
+       BA_SETUP_PACKET_OFFSET+BA_SETUP_MAX_PACKET_THRESHOLD-1] */
+
+#define BA_SETUP_MAX_PACKET_THRESHOLD   16
+#define BA_SETUP_PACKET_OFFSET          16
+
+    pmadapter->callbacks.moal_get_system_time(pmadapter->pmoal_handle, &sec,
+                                              &usec);
+    sec = (sec & 0xFFFF) + (sec >> 16);
+    usec = (usec & 0xFFFF) + (usec >> 16);
+
+    ba_threshold =
+        (((sec << 16) + usec) % BA_SETUP_MAX_PACKET_THRESHOLD) +
+        BA_SETUP_PACKET_OFFSET;
+    PRINTM(MINFO, "setup BA after %d packets\n", ba_threshold);
+
+    LEAVE();
+    return ba_threshold;
+}
+
+/**
+ *  @brief  This function cleans Tx/Rx queues
+ *
+ *  @param priv		A pointer to mlan_private
+ *
+ *  @return		N/A
+ */
+t_void
+wlan_clean_txrx(pmlan_private priv)
+{
+    mlan_adapter *pmadapter = priv->adapter;
+    t_u8 i = 0;
+
+    ENTER();
+
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) {
+        wlan_cleanup_bypass_txq(pmadapter);
+    }
+
+    wlan_11n_cleanup_reorder_tbl(priv);
+
+    pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                        priv->wmm.ra_list_spinlock);
+
+    wlan_wmm_cleanup_queues(priv);
+    wlan_11n_deleteall_txbastream_tbl(priv);
+#ifdef SDIO_MULTI_PORT_TX_AGGR
+    MP_TX_AGGR_BUF_RESET(priv->adapter);
+#endif
+#ifdef SDIO_MULTI_PORT_RX_AGGR
+    MP_RX_AGGR_BUF_RESET(priv->adapter);
+#endif
+    wlan_wmm_delete_all_ralist(priv);
+    memcpy(pmadapter, tos_to_tid, ac_to_tid, sizeof(tos_to_tid));
+    for (i = 0; i < MAX_NUM_TID; i++) {
+        tos_to_tid_inv[tos_to_tid[i]] = (t_u8) i;
+    }
+#if defined(UAP_SUPPORT)
+    priv->num_drop_pkts = 0;
+#endif
+    pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                          priv->wmm.ra_list_spinlock);
+
+    LEAVE();
+}
+
+/**
+ *  @brief Set the WMM queue priorities to their default values
+ *
+ *  @param priv     Pointer to the mlan_private driver data struct
+ *
+ *  @return         N/A
+ */
+void
+wlan_wmm_default_queue_priorities(pmlan_private priv)
+{
+    ENTER();
+
+    /* Default queue priorities: VO->VI->BE->BK */
+    priv->wmm.queue_priority[0] = WMM_AC_VO;
+    priv->wmm.queue_priority[1] = WMM_AC_VI;
+    priv->wmm.queue_priority[2] = WMM_AC_BE;
+    priv->wmm.queue_priority[3] = WMM_AC_BK;
+
+    LEAVE();
+}
+
+/**
+ *  @brief Initialize WMM priority queues
+ *
+ *  @param priv     Pointer to the mlan_private driver data struct
+ *  @param pwmm_ie  Pointer to the IEEEtypes_WmmParameter_t data struct
+ *
+ *  @return         N/A
+ */
+void
+wlan_wmm_setup_queue_priorities(pmlan_private priv,
+                                IEEEtypes_WmmParameter_t * pwmm_ie)
+{
+    t_u16 cw_min, avg_back_off, tmp[4];
+    t_u32 i, j, num_ac;
+    t_u8 ac_idx;
+
+    ENTER();
+
+    if (!pwmm_ie || priv->wmm_enabled == MFALSE) {
+        /* WMM is not enabled, just set the defaults and return */
+        wlan_wmm_default_queue_priorities(priv);
+        LEAVE();
+        return;
+    }
+
+    HEXDUMP("WMM: setup_queue_priorities: param IE",
+            (t_u8 *) pwmm_ie, sizeof(IEEEtypes_WmmParameter_t));
+
+    PRINTM(MINFO, "WMM Parameter IE: version=%d, "
+           "qos_info Parameter Set Count=%d, Reserved=%#x\n",
+           pwmm_ie->vend_hdr.version, pwmm_ie->qos_info.para_set_count,
+           pwmm_ie->reserved);
+
+    for (num_ac = 0; num_ac < NELEMENTS(pwmm_ie->ac_params); num_ac++) {
+        cw_min = (1 << pwmm_ie->ac_params[num_ac].ecw.ecw_min) - 1;
+        avg_back_off
+            = (cw_min >> 1) + pwmm_ie->ac_params[num_ac].aci_aifsn.aifsn;
+
+        ac_idx = wmm_aci_to_qidx_map[pwmm_ie->ac_params[num_ac].aci_aifsn.aci];
+        priv->wmm.queue_priority[ac_idx] = ac_idx;
+        tmp[ac_idx] = avg_back_off;
+
+        PRINTM(MCMND, "WMM: CWmax=%d CWmin=%d Avg Back-off=%d\n",
+               (1 << pwmm_ie->ac_params[num_ac].ecw.ecw_max) - 1,
+               cw_min, avg_back_off);
+        PRINTM_AC(&pwmm_ie->ac_params[num_ac]);
+    }
+
+    HEXDUMP("WMM: avg_back_off", (t_u8 *) tmp, sizeof(tmp));
+    HEXDUMP("WMM: queue_priority", priv->wmm.queue_priority,
+            sizeof(priv->wmm.queue_priority));
+
+    /* Bubble sort */
+    for (i = 0; i < num_ac; i++) {
+        for (j = 1; j < num_ac - i; j++) {
+            if (tmp[j - 1] > tmp[j]) {
+                SWAP_U16(tmp[j - 1], tmp[j]);
+                SWAP_U8(priv->wmm.queue_priority[j - 1],
+                        priv->wmm.queue_priority[j]);
+            } else if (tmp[j - 1] == tmp[j]) {
+                if (priv->wmm.queue_priority[j - 1]
+                    < priv->wmm.queue_priority[j]) {
+                    SWAP_U8(priv->wmm.queue_priority[j - 1],
+                            priv->wmm.queue_priority[j]);
+                }
+            }
+        }
+    }
+
+    wlan_wmm_queue_priorities_tid(priv, priv->wmm.queue_priority);
+
+    HEXDUMP("WMM: avg_back_off, sort", (t_u8 *) tmp, sizeof(tmp));
+    DBG_HEXDUMP(MCMD_D, "WMM: queue_priority, sort", priv->wmm.queue_priority,
+                sizeof(priv->wmm.queue_priority));
+    LEAVE();
+}
+
+/**
+ *  @brief Downgrade WMM priority queue
+ *
+ *  @param priv     Pointer to the mlan_private driver data struct
+ *
+ *  @return         N/A
+ */
+void
+wlan_wmm_setup_ac_downgrade(pmlan_private priv)
+{
+    int ac_val;
+
+    ENTER();
+
+    PRINTM(MINFO, "WMM: AC Priorities: BK(0), BE(1), VI(2), VO(3)\n");
+
+    if (priv->wmm_enabled == MFALSE) {
+        /* WMM is not enabled, default priorities */
+        for (ac_val = WMM_AC_BK; ac_val <= WMM_AC_VO; ac_val++) {
+            priv->wmm.ac_down_graded_vals[ac_val] = (mlan_wmm_ac_e) ac_val;
+        }
+    } else {
+        for (ac_val = WMM_AC_BK; ac_val <= WMM_AC_VO; ac_val++) {
+            priv->wmm.ac_down_graded_vals[ac_val]
+                = wlan_wmm_eval_downgrade_ac(priv, (mlan_wmm_ac_e) ac_val);
+            PRINTM(MINFO, "WMM: AC PRIO %d maps to %d\n",
+                   ac_val, priv->wmm.ac_down_graded_vals[ac_val]);
+        }
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief  Allocate and add a RA list for all TIDs with the given RA
+ *
+ *  @param priv  Pointer to the mlan_private driver data struct
+ *  @param ra	 Address of the receiver STA (AP in case of infra)
+ *
+ *  @return      N/A
+ */
+void
+wlan_ralist_add(mlan_private * priv, t_u8 * ra)
+{
+    int i;
+    raListTbl *ra_list;
+    pmlan_adapter pmadapter = priv->adapter;
+
+    ENTER();
+
+    for (i = 0; i < MAX_NUM_TID; ++i) {
+        ra_list = wlan_wmm_allocate_ralist_node(pmadapter, ra);
+        PRINTM(MINFO, "Creating RA List %p for tid %d\n", ra_list, i);
+        if (!ra_list)
+            break;
+        ra_list->max_amsdu = 0;
+        if (queuing_ra_based(priv)) {
+            ra_list->is_11n_enabled = wlan_is_11n_enabled(priv, ra);
+            if (ra_list->is_11n_enabled)
+                ra_list->max_amsdu = get_station_max_amsdu_size(priv, ra);
+            ra_list->tx_pause = wlan_is_tx_pause(priv, ra);
+        } else {
+            ra_list->is_11n_enabled = IS_11N_ENABLED(priv);
+            if (ra_list->is_11n_enabled)
+                ra_list->max_amsdu = priv->max_amsdu;
+        }
+
+        PRINTM(MDATA, "ralist %p: is_11n_enabled=%d max_amsdu=%d\n",
+               ra_list, ra_list->is_11n_enabled, ra_list->max_amsdu);
+
+        if (ra_list->is_11n_enabled) {
+            ra_list->packet_count = 0;
+            ra_list->ba_packet_threshold =
+                wlan_get_random_ba_threshold(pmadapter);
+        }
+
+        util_enqueue_list_tail(pmadapter->pmoal_handle,
+                               &priv->wmm.tid_tbl_ptr[i].ra_list,
+                               (pmlan_linked_list) ra_list, MNULL, MNULL);
+
+        if (!priv->wmm.tid_tbl_ptr[i].ra_list_curr)
+            priv->wmm.tid_tbl_ptr[i].ra_list_curr = ra_list;
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief Initialize the WMM state information and the WMM data path queues.
+ *
+ *  @param pmadapter  Pointer to the mlan_adapter data structure
+ *
+ *  @return         N/A
+ */
+t_void
+wlan_wmm_init(pmlan_adapter pmadapter)
+{
+    int i, j;
+    pmlan_private priv;
+
+    ENTER();
+
+    for (j = 0; j < pmadapter->priv_num; ++j) {
+        if ((priv = pmadapter->priv[j])) {
+            for (i = 0; i < MAX_NUM_TID; ++i) {
+                priv->aggr_prio_tbl[i].amsdu = BA_STREAM_NOT_ALLOWED;
+#ifdef WIFI_DIRECT_SUPPORT
+                if (priv->bss_type == MLAN_BSS_TYPE_WIFIDIRECT)
+                    priv->aggr_prio_tbl[i].ampdu_ap =
+                        priv->aggr_prio_tbl[i].ampdu_user =
+                        BA_STREAM_NOT_ALLOWED;
+                else
+#endif
+                    priv->aggr_prio_tbl[i].ampdu_ap =
+                        priv->aggr_prio_tbl[i].ampdu_user = tos_to_tid_inv[i];
+                priv->wmm.pkts_queued[i] = 0;
+                priv->wmm.tid_tbl_ptr[i].ra_list_curr = MNULL;
+            }
+
+            priv->aggr_prio_tbl[6].ampdu_ap
+                = priv->aggr_prio_tbl[6].ampdu_user = BA_STREAM_NOT_ALLOWED;
+
+            priv->aggr_prio_tbl[7].ampdu_ap
+                = priv->aggr_prio_tbl[7].ampdu_user = BA_STREAM_NOT_ALLOWED;
+
+            priv->add_ba_param.timeout = MLAN_DEFAULT_BLOCK_ACK_TIMEOUT;
+#ifdef STA_SUPPORT
+            if (priv->bss_type == MLAN_BSS_TYPE_STA) {
+                priv->add_ba_param.tx_win_size = MLAN_STA_AMPDU_DEF_TXWINSIZE;
+                priv->add_ba_param.rx_win_size = MLAN_STA_AMPDU_DEF_RXWINSIZE;
+            }
+#endif
+#ifdef UAP_SUPPORT
+            if (priv->bss_type == MLAN_BSS_TYPE_UAP
+#ifdef WIFI_DIRECT_SUPPORT
+                || priv->bss_type == MLAN_BSS_TYPE_WIFIDIRECT
+#endif
+                ) {
+                priv->add_ba_param.tx_win_size = MLAN_UAP_AMPDU_DEF_TXWINSIZE;
+                priv->add_ba_param.rx_win_size = MLAN_UAP_AMPDU_DEF_RXWINSIZE;
+            }
+#endif
+            priv->add_ba_param.tx_amsdu = MTRUE;
+            priv->add_ba_param.rx_amsdu = MTRUE;
+            memset(priv->adapter, priv->rx_seq, 0xff, sizeof(priv->rx_seq));
+            wlan_wmm_default_queue_priorities(priv);
+        }
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief Setup the queue priorities and downgrade any queues as required
+ *         by the WMM info.  Setups default values if WMM is not active
+ *         for this association.
+ *
+ *  @param priv     Pointer to the mlan_private driver data struct
+ *
+ *  @return         N/A
+ */
+void
+wlan_wmm_setup_queues(pmlan_private priv)
+{
+    ENTER();
+    wlan_wmm_setup_queue_priorities(priv, MNULL);
+    wlan_wmm_setup_ac_downgrade(priv);
+    LEAVE();
+}
+
+#ifdef STA_SUPPORT
+/**
+ *  @brief  Send a command to firmware to retrieve the current WMM status
+ *
+ *  @param priv     Pointer to the mlan_private driver data struct
+ *
+ *  @return         MLAN_STATUS_SUCCESS; MLAN_STATUS_FAILURE
+ */
+mlan_status
+wlan_cmd_wmm_status_change(pmlan_private priv)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    ret = wlan_prepare_cmd(priv, HostCmd_CMD_WMM_GET_STATUS, 0, 0, 0, MNULL);
+    LEAVE();
+    return ret;
+}
+#endif
+
+/**
+ *  @brief Check if wmm TX queue is empty
+ *
+ *  @param pmadapter  Pointer to the mlan_adapter driver data struct
+ *
+ *  @return         MFALSE if not empty; MTRUE if empty
+ */
+int
+wlan_wmm_lists_empty(pmlan_adapter pmadapter)
+{
+    int j;
+    pmlan_private priv;
+
+    ENTER();
+
+    for (j = 0; j < pmadapter->priv_num; ++j) {
+        if ((priv = pmadapter->priv[j])) {
+            if ((priv->port_ctrl_mode == MTRUE) && (priv->port_open == MFALSE)) {
+                PRINTM(MINFO,
+                       "wmm_lists_empty: PORT_CLOSED Ignore pkts from BSS%d\n",
+                       j);
+                continue;
+            }
+
+            if (util_scalar_read(pmadapter->pmoal_handle,
+                                 &priv->wmm.tx_pkts_queued,
+                                 pmadapter->callbacks.moal_spin_lock,
+                                 pmadapter->callbacks.moal_spin_unlock)) {
+                LEAVE();
+                return MFALSE;
+            }
+        }
+    }
+
+    LEAVE();
+    return MTRUE;
+}
+
+/**
+ *   @brief Get ralist node
+ *
+ *   @param priv     Pointer to the mlan_private driver data struct
+ *   @param tid      TID
+ *   @param ra_addr  Pointer to the route address
+ *
+ *   @return         ra_list or MNULL
+ */
+raListTbl *
+wlan_wmm_get_ralist_node(pmlan_private priv, t_u8 tid, t_u8 * ra_addr)
+{
+    raListTbl *ra_list;
+    ENTER();
+    ra_list =
+        (raListTbl *) util_peek_list(priv->adapter->pmoal_handle,
+                                     &priv->wmm.tid_tbl_ptr[tid].ra_list, MNULL,
+                                     MNULL);
+    while (ra_list && (ra_list != (raListTbl *)
+                       & priv->wmm.tid_tbl_ptr[tid].ra_list)) {
+        if (!memcmp(priv->adapter, ra_list->ra, ra_addr, MLAN_MAC_ADDR_LENGTH)) {
+            LEAVE();
+            return ra_list;
+        }
+        ra_list = ra_list->pnext;
+    }
+    LEAVE();
+    return MNULL;
+}
+
+/**
+ *   @brief Check if RA list is valid or not
+ *
+ *   @param priv     Pointer to the mlan_private driver data struct
+ *   @param ra_list  Pointer to raListTbl
+ *   @param ptrindex TID pointer index
+ *
+ *   @return         MTRUE- valid. MFALSE- invalid.
+ */
+int
+wlan_is_ralist_valid(mlan_private * priv, raListTbl * ra_list, int ptrindex)
+{
+    raListTbl *rlist;
+
+    ENTER();
+
+    rlist =
+        (raListTbl *) util_peek_list(priv->adapter->pmoal_handle,
+                                     &priv->wmm.tid_tbl_ptr[ptrindex].ra_list,
+                                     MNULL, MNULL);
+
+    while (rlist && (rlist != (raListTbl *)
+                     & priv->wmm.tid_tbl_ptr[ptrindex].ra_list)) {
+        if (rlist == ra_list) {
+            LEAVE();
+            return MTRUE;
+        }
+
+        rlist = rlist->pnext;
+    }
+    LEAVE();
+    return MFALSE;
+}
+
+/**
+ *  @brief  Update an existing raList with a new RA and 11n capability
+ *
+ *  @param priv     Pointer to the mlan_private driver data struct
+ *  @param old_ra   Old receiver address
+ *  @param new_ra   New receiver address
+ *
+ *  @return         integer count of updated nodes
+ */
+int
+wlan_ralist_update(mlan_private * priv, t_u8 * old_ra, t_u8 * new_ra)
+{
+    t_u8 tid;
+    int update_count;
+    raListTbl *ra_list;
+
+    ENTER();
+
+    update_count = 0;
+
+    for (tid = 0; tid < MAX_NUM_TID; ++tid) {
+
+        ra_list = wlan_wmm_get_ralist_node(priv, tid, old_ra);
+
+        if (ra_list) {
+            update_count++;
+
+            if (queuing_ra_based(priv))
+                ra_list->is_11n_enabled = wlan_is_11n_enabled(priv, new_ra);
+            else
+                ra_list->is_11n_enabled = IS_11N_ENABLED(priv);
+            ra_list->packet_count = 0;
+            ra_list->ba_packet_threshold =
+                wlan_get_random_ba_threshold(priv->adapter);
+            PRINTM(MINFO,
+                   "ralist_update: %p, %d, %02x:%02x:%02x:%02x:%02x:%02x-->"
+                   "%02x:%02x:%02x:%02x:%02x:%02x\n", ra_list,
+                   ra_list->is_11n_enabled, ra_list->ra[0], ra_list->ra[1],
+                   ra_list->ra[2], ra_list->ra[3], ra_list->ra[4],
+                   ra_list->ra[5], new_ra[0], new_ra[1], new_ra[2], new_ra[3],
+                   new_ra[4], new_ra[5]);
+
+            memcpy(priv->adapter, ra_list->ra, new_ra, MLAN_MAC_ADDR_LENGTH);
+        }
+    }
+
+    LEAVE();
+    return update_count;
+}
+
+/**
+ *  @brief Add packet to WMM queue
+ *
+ *  @param pmadapter  Pointer to the mlan_adapter driver data struct
+ *  @param pmbuf      Pointer to the mlan_buffer data struct
+ *
+ *  @return         N/A
+ */
+t_void
+wlan_wmm_add_buf_txqueue(pmlan_adapter pmadapter, pmlan_buffer pmbuf)
+{
+    pmlan_private priv = pmadapter->priv[pmbuf->bss_index];
+    t_u32 tid;
+    raListTbl *ra_list;
+    t_u8 ra[MLAN_MAC_ADDR_LENGTH], tid_down;
+
+    ENTER();
+
+    pmbuf->buf_type = MLAN_BUF_TYPE_DATA;
+    if (!priv->media_connected) {
+        PRINTM(MWARN, "Drop packet in disconnect state\n");
+        wlan_write_data_complete(pmadapter, pmbuf, MLAN_STATUS_FAILURE);
+        LEAVE();
+        return;
+    }
+    tid = pmbuf->priority;
+    pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                        priv->wmm.ra_list_spinlock);
+    tid_down = wlan_wmm_downgrade_tid(priv, tid);
+
+    /* In case of infra as we have already created the list during association
+       we just don't have to call get_queue_raptr, we will have only 1 raptr
+       for a tid in case of infra */
+    if (!queuing_ra_based(priv)) {
+        ra_list = (raListTbl *) util_peek_list(pmadapter->pmoal_handle,
+                                               &priv->wmm.tid_tbl_ptr[tid_down].
+                                               ra_list, MNULL, MNULL);
+    } else {
+        memcpy(pmadapter, ra, pmbuf->pbuf + pmbuf->data_offset,
+               MLAN_MAC_ADDR_LENGTH);
+        /** put multicast/broadcast packet in the same ralist */
+        if (ra[0] & 0x01)
+            memset(pmadapter, ra, 0xff, sizeof(ra));
+        ra_list = wlan_wmm_get_queue_raptr(priv, tid_down, ra);
+    }
+
+    if (!ra_list) {
+        PRINTM(MWARN, "Drop packet, ra_list=%p, "
+               "media_connected=%d\n", ra_list, priv->media_connected);
+        pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                              priv->wmm.ra_list_spinlock);
+        wlan_write_data_complete(pmadapter, pmbuf, MLAN_STATUS_FAILURE);
+        LEAVE();
+        return;
+    }
+
+    PRINTM(MDATA, "Adding pkt to ra_list %p %p priority=%d, tid_down=%d\n",
+           ra_list, pmbuf, pmbuf->priority, tid_down);
+    util_enqueue_list_tail(pmadapter->pmoal_handle, &ra_list->buf_head,
+                           (pmlan_linked_list) pmbuf, MNULL, MNULL);
+
+    ra_list->total_pkts++;
+    ra_list->packet_count++;
+
+    priv->wmm.pkts_queued[tid_down]++;
+    if (!ra_list->tx_pause) {
+        util_scalar_increment(pmadapter->pmoal_handle,
+                              &priv->wmm.tx_pkts_queued, MNULL, MNULL);
+        /* if highest_queued_prio < prio(tid_down), set it to prio(tid_down) */
+        util_scalar_conditional_write(pmadapter->pmoal_handle,
+                                      &priv->wmm.highest_queued_prio,
+                                      MLAN_SCALAR_COND_LESS_THAN,
+                                      tos_to_tid_inv[tid_down],
+                                      tos_to_tid_inv[tid_down], MNULL, MNULL);
+    }
+    /* Record the current time the packet was queued; used to determine the
+       amount of time the packet was queued in the driver before it was sent to
+       the firmware.  The delay is then sent along with the packet to the
+       firmware for aggregate delay calculation for stats and MSDU lifetime
+       expiry. */
+    pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                          priv->wmm.ra_list_spinlock);
+    pmadapter->callbacks.moal_get_system_time(pmadapter->pmoal_handle,
+                                              &pmbuf->in_ts_sec,
+                                              &pmbuf->in_ts_usec);
+
+    LEAVE();
+}
+
+#ifdef STA_SUPPORT
+/**
+ *  @brief Process the GET_WMM_STATUS command response from firmware
+ *
+ *  The GET_WMM_STATUS response may contain multiple TLVs for:
+ *      - AC Queue status TLVs
+ *      - Current WMM Parameter IE TLV
+ *      - Admission Control action frame TLVs
+ *
+ *  This function parses the TLVs and then calls further functions
+ *   to process any changes in the queue prioritize or state.
+ *
+ *  @param priv      Pointer to the mlan_private driver data struct
+ *  @param ptlv      Pointer to the tlv block returned in the response.
+ *  @param resp_len  Length of TLV block
+ *
+ *  @return MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_wmm_get_status(pmlan_private priv, t_u8 * ptlv, int resp_len)
+{
+    t_u8 *pcurrent = ptlv;
+    t_u32 tlv_len;
+    t_u8 sendWmmEvent;
+    MrvlIEtypes_Data_t *pTlvHdr;
+    MrvlIEtypes_WmmQueueStatus_t *pTlvWmmQStatus;
+    IEEEtypes_WmmParameter_t *pWmmParamIe = MNULL;
+    WmmAcStatus_t *pac_status;
+
+    MrvlIETypes_ActionFrame_t *pTlvAction;
+    IEEEtypes_Action_WMM_AddTsRsp_t *pAddTsRsp;
+    IEEEtypes_Action_WMM_DelTs_t *pDelTs;
+
+    ENTER();
+
+    sendWmmEvent = MFALSE;
+
+    PRINTM(MINFO, "WMM: WMM_GET_STATUS cmdresp received: %d\n", resp_len);
+    HEXDUMP("CMD_RESP: WMM_GET_STATUS", pcurrent, resp_len);
+
+    while (resp_len >= sizeof(pTlvHdr->header)) {
+        pTlvHdr = (MrvlIEtypes_Data_t *) pcurrent;
+        tlv_len = wlan_le16_to_cpu(pTlvHdr->header.len);
+
+        switch (wlan_le16_to_cpu(pTlvHdr->header.type)) {
+        case TLV_TYPE_WMMQSTATUS:
+            pTlvWmmQStatus = (MrvlIEtypes_WmmQueueStatus_t *) pTlvHdr;
+            PRINTM(MEVENT, "WMM_STATUS: QSTATUS TLV: %d\n",
+                   pTlvWmmQStatus->queue_index);
+
+            PRINTM(MINFO,
+                   "CMD_RESP: WMM_GET_STATUS: QSTATUS TLV: %d, %d, %d\n",
+                   pTlvWmmQStatus->queue_index,
+                   pTlvWmmQStatus->flow_required, pTlvWmmQStatus->disabled);
+
+            pac_status = &priv->wmm.ac_status[pTlvWmmQStatus->queue_index];
+            pac_status->disabled = pTlvWmmQStatus->disabled;
+            pac_status->flow_required = pTlvWmmQStatus->flow_required;
+            pac_status->flow_created = pTlvWmmQStatus->flow_created;
+            break;
+
+        case WMM_IE:
+            /*
+             * Point the regular IEEE IE 2 bytes into the Marvell IE
+             *   and setup the IEEE IE type and length byte fields
+             */
+
+            PRINTM(MEVENT, "WMM STATUS: WMM IE\n");
+
+            HEXDUMP("WMM: WMM TLV:", (t_u8 *) pTlvHdr, tlv_len + 4);
+
+            pWmmParamIe = (IEEEtypes_WmmParameter_t *) (pcurrent + 2);
+            pWmmParamIe->vend_hdr.len = (t_u8) tlv_len;
+            pWmmParamIe->vend_hdr.element_id = WMM_IE;
+
+            PRINTM(MINFO, "CMD_RESP: WMM_GET_STATUS: WMM Parameter Set: %d\n",
+                   pWmmParamIe->qos_info.para_set_count);
+
+            memcpy(priv->adapter,
+                   (t_u8 *) & priv->curr_bss_params.bss_descriptor.wmm_ie,
+                   pWmmParamIe, MIN(sizeof(IEEEtypes_WmmParameter_t),
+                                    (pWmmParamIe->vend_hdr.len + 2)));
+            sendWmmEvent = MTRUE;
+            break;
+
+        case TLV_TYPE_IEEE_ACTION_FRAME:
+            PRINTM(MEVENT, "WMM_STATUS: IEEE Action Frame\n");
+            pTlvAction = (MrvlIETypes_ActionFrame_t *) pcurrent;
+
+            if (pTlvAction->actionFrame.wmmAc.tspecAct.category
+                == IEEE_MGMT_ACTION_CATEGORY_WMM_TSPEC) {
+
+                switch (pTlvAction->actionFrame.wmmAc.tspecAct.action) {
+                case TSPEC_ACTION_CODE_ADDTS_RSP:
+                    pAddTsRsp = &pTlvAction->actionFrame.wmmAc.addTsRsp;
+                    wlan_send_wmmac_host_event(priv, "ADDTS_RSP",
+                                               pTlvAction->srcAddr,
+                                               pAddTsRsp->tspecIE.TspecBody.
+                                               TSInfo.TID,
+                                               pAddTsRsp->tspecIE.TspecBody.
+                                               TSInfo.UserPri,
+                                               pAddTsRsp->statusCode);
+                    break;
+
+                case TSPEC_ACTION_CODE_DELTS:
+                    pDelTs = &pTlvAction->actionFrame.wmmAc.delTs;
+                    wlan_send_wmmac_host_event(priv, "DELTS_RX",
+                                               pTlvAction->srcAddr,
+                                               pDelTs->tspecIE.TspecBody.TSInfo.
+                                               TID,
+                                               pDelTs->tspecIE.TspecBody.TSInfo.
+                                               UserPri, pDelTs->reasonCode);
+                    break;
+
+                case TSPEC_ACTION_CODE_ADDTS_REQ:
+                default:
+                    break;
+                }
+            }
+            break;
+
+        default:
+            break;
+        }
+
+        pcurrent += (tlv_len + sizeof(pTlvHdr->header));
+        resp_len -= (tlv_len + sizeof(pTlvHdr->header));
+    }
+
+    wlan_wmm_setup_queue_priorities(priv, pWmmParamIe);
+    wlan_wmm_setup_ac_downgrade(priv);
+
+    if (sendWmmEvent) {
+        wlan_recv_event(priv, MLAN_EVENT_ID_FW_WMM_CONFIG_CHANGE, MNULL);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Call back from the command module to allow insertion of a WMM TLV
+ *
+ *  If the BSS we are associating to supports WMM, add the required WMM
+ *    Information IE to the association request command buffer in the form
+ *    of a Marvell extended IEEE IE.
+ *
+ *  @param priv        Pointer to the mlan_private driver data struct
+ *  @param ppAssocBuf  Output parameter: Pointer to the TLV output buffer,
+ *                     modified on return to point after the appended WMM TLV
+ *  @param pWmmIE      Pointer to the WMM IE for the BSS we are joining
+ *  @param pHTCap      Pointer to the HT IE for the BSS we are joining
+ *
+ *  @return Length of data appended to the association tlv buffer
+ */
+t_u32
+wlan_wmm_process_association_req(pmlan_private priv,
+                                 t_u8 ** ppAssocBuf,
+                                 IEEEtypes_WmmParameter_t * pWmmIE,
+                                 IEEEtypes_HTCap_t * pHTCap)
+{
+    MrvlIEtypes_WmmParamSet_t *pwmm_tlv;
+    t_u32 ret_len = 0;
+
+    ENTER();
+
+    /* Null checks */
+    if (!ppAssocBuf) {
+        LEAVE();
+        return 0;
+    }
+    if (!(*ppAssocBuf)) {
+        LEAVE();
+        return 0;
+    }
+
+    if (!pWmmIE) {
+        LEAVE();
+        return 0;
+    }
+
+    PRINTM(MINFO, "WMM: process assoc req: bss->wmmIe=0x%x\n",
+           pWmmIE->vend_hdr.element_id);
+
+    if ((priv->wmm_required
+         || (pHTCap && (pHTCap->ieee_hdr.element_id == HT_CAPABILITY)
+             && (priv->config_bands & BAND_GN || priv->config_bands & BAND_AN))
+        )
+        && pWmmIE->vend_hdr.element_id == WMM_IE) {
+        pwmm_tlv = (MrvlIEtypes_WmmParamSet_t *) * ppAssocBuf;
+        pwmm_tlv->header.type = (t_u16) wmm_info_ie[0];
+        pwmm_tlv->header.type = wlan_cpu_to_le16(pwmm_tlv->header.type);
+        pwmm_tlv->header.len = (t_u16) wmm_info_ie[1];
+        memcpy(priv->adapter, pwmm_tlv->wmm_ie, &wmm_info_ie[2],
+               pwmm_tlv->header.len);
+        if (pWmmIE->qos_info.qos_uapsd)
+            memcpy(priv->adapter,
+                   (t_u8 *) (pwmm_tlv->wmm_ie + pwmm_tlv->header.len -
+                             sizeof(priv->wmm_qosinfo)), &priv->wmm_qosinfo,
+                   sizeof(priv->wmm_qosinfo));
+
+        ret_len = sizeof(pwmm_tlv->header) + pwmm_tlv->header.len;
+        pwmm_tlv->header.len = wlan_cpu_to_le16(pwmm_tlv->header.len);
+
+        HEXDUMP("ASSOC_CMD: WMM IE", (t_u8 *) pwmm_tlv, ret_len);
+        *ppAssocBuf += ret_len;
+    }
+
+    LEAVE();
+    return ret_len;
+}
+#endif /* STA_SUPPORT */
+
+/**
+ *   @brief Compute the time delay in the driver queues for a given packet.
+ *
+ *   When the packet is received at the OS/Driver interface, the current
+ *     time is set in the packet structure.  The difference between the present
+ *     time and that received time is computed in this function and limited
+ *     based on pre-compiled limits in the driver.
+ *
+ *   @param priv   Ptr to the mlan_private driver data struct
+ *   @param pmbuf  Ptr to the mlan_buffer which has been previously timestamped
+ *
+ *   @return  Time delay of the packet in 2ms units after having limit applied
+ */
+t_u8
+wlan_wmm_compute_driver_packet_delay(pmlan_private priv,
+                                     const pmlan_buffer pmbuf)
+{
+    t_u8 ret_val = 0;
+    t_u32 out_ts_sec, out_ts_usec, queue_delay;
+
+    ENTER();
+
+    priv->adapter->callbacks.moal_get_system_time(priv->adapter->pmoal_handle,
+                                                  &out_ts_sec, &out_ts_usec);
+
+    queue_delay = (out_ts_sec - pmbuf->in_ts_sec) * 1000;
+    queue_delay += (out_ts_usec - pmbuf->in_ts_usec) / 1000;
+
+    /*
+     * Queue delay is passed as a uint8 in units of 2ms (ms shifted
+     *  by 1). Min value (other than 0) is therefore 2ms, max is 510ms.
+     *
+     * Pass max value if queue_delay is beyond the uint8 range
+     */
+    ret_val = (t_u8) (MIN(queue_delay, priv->wmm.drv_pkt_delay_max) >> 1);
+
+    PRINTM(MINFO, "WMM: Pkt Delay: %d ms, %d ms sent to FW\n",
+           queue_delay, ret_val);
+
+    LEAVE();
+    return ret_val;
+}
+
+/**
+ *  @brief Transmit the highest priority packet awaiting in the WMM Queues
+ *
+ *  @param pmadapter Pointer to the mlan_adapter driver data struct
+ *
+ *  @return        N/A
+ */
+void
+wlan_wmm_process_tx(pmlan_adapter pmadapter)
+{
+    ENTER();
+
+    do {
+        if (wlan_dequeue_tx_packet(pmadapter))
+            break;
+        /* Check if busy */
+    } while (!pmadapter->data_sent && !pmadapter->tx_lock_flag
+             && !wlan_wmm_lists_empty(pmadapter));
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief select wmm queue
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param tid          TID 0-7
+ *
+ *  @return             wmm_queue priority (0-3)
+ */
+t_u8
+wlan_wmm_select_queue(mlan_private * pmpriv, t_u8 tid)
+{
+    pmlan_adapter pmadapter = pmpriv->adapter;
+    t_u8 i;
+    mlan_wmm_ac_e ac_down =
+        pmpriv->wmm.
+        ac_down_graded_vals[wlan_wmm_convert_tos_to_ac(pmadapter, tid)];
+
+    ENTER();
+
+    for (i = 0; i < 4; i++) {
+        if (pmpriv->wmm.queue_priority[i] == ac_down) {
+            LEAVE();
+            return i;
+        }
+    }
+    LEAVE();
+    return 0;
+}
+
+#if defined(UAP_SUPPORT)
+/**
+ *  @brief Delete tx packets in RA list
+ *
+ *  @param priv			Pointer to the mlan_private driver data struct
+ *  @param ra_list_head	ra list header
+ *  @param tid          tid
+ *
+ *  @return		N/A
+ */
+static INLINE t_u8
+wlan_del_tx_pkts_in_ralist(pmlan_private priv,
+                           mlan_list_head * ra_list_head, int tid)
+{
+    raListTbl *ra_list = MNULL;
+    pmlan_adapter pmadapter = priv->adapter;
+    pmlan_buffer pmbuf = MNULL;
+    t_u8 ret = MFALSE;
+    ENTER();
+    ra_list =
+        (raListTbl *) util_peek_list(priv->adapter->pmoal_handle, ra_list_head,
+                                     MNULL, MNULL);
+    while (ra_list && ra_list != (raListTbl *) ra_list_head) {
+        if (ra_list->total_pkts && (ra_list->tx_pause ||
+                                    (ra_list->total_pkts > RX_LOW_THRESHOLD))) {
+            if ((pmbuf =
+                 (pmlan_buffer) util_dequeue_list(pmadapter->pmoal_handle,
+                                                  &ra_list->buf_head, MNULL,
+                                                  MNULL))) {
+                PRINTM(MDATA,
+                       "Drop pkts: tid=%d tx_pause=%d pkts=%d brd_pkts=%d %02x:%02x:%02x:%02x:%02x:%02x\n",
+                       tid, ra_list->tx_pause, ra_list->total_pkts,
+                       pmadapter->pending_bridge_pkts, ra_list->ra[0],
+                       ra_list->ra[1], ra_list->ra[2], ra_list->ra[3],
+                       ra_list->ra[4], ra_list->ra[5]);
+                wlan_write_data_complete(pmadapter, pmbuf, MLAN_STATUS_FAILURE);
+                priv->wmm.pkts_queued[tid]--;
+                priv->num_drop_pkts++;
+                ra_list->total_pkts--;
+                if (!ra_list->tx_pause)
+                    util_scalar_decrement(pmadapter->pmoal_handle,
+                                          &priv->wmm.tx_pkts_queued, MNULL,
+                                          MNULL);
+                ret = MTRUE;
+                break;
+            }
+        }
+        ra_list = ra_list->pnext;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Drop tx pkts
+ *
+ *  @param priv			Pointer to the mlan_private driver data struct
+ *
+ *  @return		N/A
+ */
+t_void
+wlan_drop_tx_pkts(pmlan_private priv)
+{
+    int j;
+    static int i = 0;
+    pmlan_adapter pmadapter = priv->adapter;
+    pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                        priv->wmm.ra_list_spinlock);
+    for (j = 0; j < MAX_NUM_TID; j++, i++) {
+        if (i == MAX_NUM_TID)
+            i = 0;
+        if (wlan_del_tx_pkts_in_ralist
+            (priv, &priv->wmm.tid_tbl_ptr[i].ra_list, i)) {
+            i++;
+            break;
+        }
+    }
+    pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                          priv->wmm.ra_list_spinlock);
+    return;
+}
+
+/**
+ *  @brief Remove peer ralist
+ *
+ *  @param priv		  A pointer to mlan_private
+ *  @param mac        peer mac address
+ *
+ *  @return           N/A
+ */
+t_void
+wlan_wmm_delete_peer_ralist(pmlan_private priv, t_u8 * mac)
+{
+    raListTbl *ra_list;
+    int i;
+    pmlan_adapter pmadapter = priv->adapter;
+    t_u32 pkt_cnt = 0;
+    t_u32 tx_pkts_queued = 0;
+
+    ENTER();
+    pmadapter->callbacks.moal_spin_lock(pmadapter->pmoal_handle,
+                                        priv->wmm.ra_list_spinlock);
+    for (i = 0; i < MAX_NUM_TID; ++i) {
+        ra_list = wlan_wmm_get_ralist_node(priv, i, mac);
+        if (ra_list) {
+            PRINTM(MINFO, "delete sta ralist %p\n", ra_list);
+            if (!ra_list->tx_pause)
+                pkt_cnt += ra_list->total_pkts;
+            wlan_wmm_del_pkts_in_ralist_node(priv, ra_list);
+
+            util_unlink_list(pmadapter->pmoal_handle,
+                             &priv->wmm.tid_tbl_ptr[i].ra_list,
+                             (pmlan_linked_list) ra_list, MNULL, MNULL);
+            pmadapter->callbacks.moal_mfree(pmadapter->pmoal_handle,
+                                            (t_u8 *) ra_list);
+            if (priv->wmm.tid_tbl_ptr[i].ra_list_curr == ra_list)
+                priv->wmm.tid_tbl_ptr[i].ra_list_curr =
+                    (raListTbl *) & priv->wmm.tid_tbl_ptr[i].ra_list;
+        }
+    }
+    if (pkt_cnt) {
+        tx_pkts_queued = util_scalar_read(pmadapter->pmoal_handle,
+                                          &priv->wmm.tx_pkts_queued, MNULL,
+                                          MNULL);
+        tx_pkts_queued -= pkt_cnt;
+        util_scalar_write(priv->adapter->pmoal_handle,
+                          &priv->wmm.tx_pkts_queued, tx_pkts_queued, MNULL,
+                          MNULL);
+        util_scalar_write(priv->adapter->pmoal_handle,
+                          &priv->wmm.highest_queued_prio, HIGH_PRIO_TID, MNULL,
+                          MNULL);
+    }
+    pmadapter->callbacks.moal_spin_unlock(pmadapter->pmoal_handle,
+                                          priv->wmm.ra_list_spinlock);
+    LEAVE();
+}
+#endif
+
+#ifdef STA_SUPPORT
+
+/**
+ *  @brief This function prepares the command of ADDTS
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_wmm_addts_req(IN pmlan_private pmpriv,
+                       OUT HostCmd_DS_COMMAND * cmd, IN t_void * pdata_buf)
+{
+    mlan_ds_wmm_addts *paddts = (mlan_ds_wmm_addts *) pdata_buf;
+    HostCmd_DS_WMM_ADDTS_REQ *pcmd_addts = &cmd->params.add_ts;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_WMM_ADDTS_REQ);
+    cmd->size = wlan_cpu_to_le16(sizeof(pcmd_addts->dialog_token)
+                                 + sizeof(pcmd_addts->timeout_ms)
+                                 + sizeof(pcmd_addts->command_result)
+                                 + sizeof(pcmd_addts->ieee_status_code)
+                                 + paddts->ie_data_len + S_DS_GEN);
+    cmd->result = 0;
+
+    pcmd_addts->timeout_ms = wlan_cpu_to_le32(paddts->timeout);
+    pcmd_addts->dialog_token = paddts->dialog_tok;
+    memcpy(pmpriv->adapter,
+           pcmd_addts->tspec_data,
+           paddts->ie_data, MIN(WMM_TSPEC_SIZE, paddts->ie_data_len));
+
+    LEAVE();
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of ADDTS
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_wmm_addts_req(IN pmlan_private pmpriv,
+                       const IN HostCmd_DS_COMMAND * resp,
+                       OUT mlan_ioctl_req * pioctl_buf)
+{
+    mlan_ds_wmm_cfg *pwmm = MNULL;
+    mlan_ds_wmm_addts *paddts = MNULL;
+    const HostCmd_DS_WMM_ADDTS_REQ *presp_addts = &resp->params.add_ts;
+
+    ENTER();
+
+    if (pioctl_buf) {
+        pwmm = (mlan_ds_wmm_cfg *) pioctl_buf->pbuf;
+        paddts = (mlan_ds_wmm_addts *) & pwmm->param.addts;
+        paddts->result = presp_addts->command_result;
+        paddts->dialog_tok = presp_addts->dialog_token;
+        paddts->status_code = (t_u32) presp_addts->ieee_status_code;
+
+        if (presp_addts->command_result == MLAN_CMD_RESULT_SUCCESS) {
+            /* The tspecData field is potentially variable in size due to extra
+               IEs that may have been in the ADDTS response action frame.
+               Calculate the data length from the firmware command response. */
+            paddts->ie_data_len
+                = (t_u8) (resp->size - sizeof(presp_addts->command_result)
+                          - sizeof(presp_addts->timeout_ms)
+                          - sizeof(presp_addts->dialog_token)
+                          - sizeof(presp_addts->ieee_status_code)
+                          - S_DS_GEN);
+
+            /* Copy the TSPEC data include any extra IEs after the TSPEC */
+            memcpy(pmpriv->adapter,
+                   paddts->ie_data,
+                   presp_addts->tspec_data, paddts->ie_data_len);
+        } else {
+            paddts->ie_data_len = 0;
+        }
+        PRINTM(MINFO, "TSPEC: ADDTS ret = %d,%d sz=%d\n",
+               paddts->result, paddts->status_code, paddts->ie_data_len);
+
+        HEXDUMP("TSPEC: ADDTS data", paddts->ie_data, paddts->ie_data_len);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares the command of DELTS
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_wmm_delts_req(IN pmlan_private pmpriv,
+                       OUT HostCmd_DS_COMMAND * cmd, IN t_void * pdata_buf)
+{
+    mlan_ds_wmm_delts *pdelts = (mlan_ds_wmm_delts *) pdata_buf;
+    HostCmd_DS_WMM_DELTS_REQ *pcmd_delts = &cmd->params.del_ts;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_WMM_DELTS_REQ);
+    cmd->size = wlan_cpu_to_le16(sizeof(pcmd_delts->dialog_token)
+                                 + sizeof(pcmd_delts->command_result)
+                                 + sizeof(pcmd_delts->ieee_reason_code)
+                                 + pdelts->ie_data_len + S_DS_GEN);
+    cmd->result = 0;
+    pcmd_delts->ieee_reason_code = (t_u8) pdelts->status_code;
+    memcpy(pmpriv->adapter,
+           pcmd_delts->tspec_data,
+           pdelts->ie_data, MIN(WMM_TSPEC_SIZE, pdelts->ie_data_len));
+
+    LEAVE();
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of DELTS
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_wmm_delts_req(IN pmlan_private pmpriv,
+                       const IN HostCmd_DS_COMMAND * resp,
+                       OUT mlan_ioctl_req * pioctl_buf)
+{
+    mlan_ds_wmm_cfg *pwmm;
+    IEEEtypes_WMM_TSPEC_t *pTspecIE;
+    const HostCmd_DS_WMM_DELTS_REQ *presp_delts = &resp->params.del_ts;
+
+    ENTER();
+
+    if (pioctl_buf) {
+        pwmm = (mlan_ds_wmm_cfg *) pioctl_buf->pbuf;
+        pwmm->param.delts.result = presp_delts->command_result;
+
+        PRINTM(MINFO, "TSPEC: DELTS result = %d\n",
+               presp_delts->command_result);
+
+        if (presp_delts->command_result == 0) {
+            pTspecIE = (IEEEtypes_WMM_TSPEC_t *) presp_delts->tspec_data;
+            wlan_send_wmmac_host_event(pmpriv,
+                                       "DELTS_TX",
+                                       MNULL,
+                                       pTspecIE->TspecBody.TSInfo.TID,
+                                       pTspecIE->TspecBody.TSInfo.UserPri,
+                                       presp_delts->ieee_reason_code);
+
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares the command of WMM_QUEUE_CONFIG
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_wmm_queue_config(IN pmlan_private pmpriv,
+                          OUT HostCmd_DS_COMMAND * cmd, IN t_void * pdata_buf)
+{
+    mlan_ds_wmm_queue_config *pqcfg = (mlan_ds_wmm_queue_config *) pdata_buf;
+    HostCmd_DS_WMM_QUEUE_CONFIG *pcmd_qcfg = &cmd->params.queue_config;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_WMM_QUEUE_CONFIG);
+    cmd->size = wlan_cpu_to_le16(sizeof(pcmd_qcfg->action)
+                                 + sizeof(pcmd_qcfg->access_category)
+                                 + sizeof(pcmd_qcfg->msdu_lifetime_expiry)
+                                 + S_DS_GEN);
+    cmd->result = 0;
+
+    pcmd_qcfg->action = pqcfg->action;
+    pcmd_qcfg->access_category = pqcfg->access_category;
+    pcmd_qcfg->msdu_lifetime_expiry =
+        wlan_cpu_to_le16(pqcfg->msdu_lifetime_expiry);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of WMM_QUEUE_CONFIG
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_wmm_queue_config(IN pmlan_private pmpriv,
+                          const IN HostCmd_DS_COMMAND * resp,
+                          OUT mlan_ioctl_req * pioctl_buf)
+{
+    mlan_ds_wmm_cfg *pwmm = MNULL;
+    const HostCmd_DS_WMM_QUEUE_CONFIG *presp_qcfg = &resp->params.queue_config;
+
+    ENTER();
+
+    if (pioctl_buf) {
+        pwmm = (mlan_ds_wmm_cfg *) pioctl_buf->pbuf;
+        pwmm->param.q_cfg.action = presp_qcfg->action;
+        pwmm->param.q_cfg.access_category = presp_qcfg->access_category;
+        pwmm->param.q_cfg.msdu_lifetime_expiry =
+            wlan_le16_to_cpu(presp_qcfg->msdu_lifetime_expiry);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares the command of WMM_QUEUE_STATS
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_wmm_queue_stats(IN pmlan_private pmpriv,
+                         OUT HostCmd_DS_COMMAND * cmd, IN t_void * pdata_buf)
+{
+    mlan_ds_wmm_queue_stats *pqstats = (mlan_ds_wmm_queue_stats *) pdata_buf;
+    HostCmd_DS_WMM_QUEUE_STATS *pcmd_qstats = &cmd->params.queue_stats;
+    t_u8 id;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_WMM_QUEUE_STATS);
+    cmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_WMM_QUEUE_STATS)
+                                 + S_DS_GEN);
+    cmd->result = 0;
+
+    pcmd_qstats->action = pqstats->action;
+    pcmd_qstats->select_is_userpri = 1;
+    pcmd_qstats->select_bin = pqstats->user_priority;
+    pcmd_qstats->pkt_count = wlan_cpu_to_le16(pqstats->pkt_count);
+    pcmd_qstats->pkt_loss = wlan_cpu_to_le16(pqstats->pkt_loss);
+    pcmd_qstats->avg_queue_delay = wlan_cpu_to_le32(pqstats->avg_queue_delay);
+    pcmd_qstats->avg_tx_delay = wlan_cpu_to_le32(pqstats->avg_tx_delay);
+    pcmd_qstats->used_time = wlan_cpu_to_le16(pqstats->used_time);
+    pcmd_qstats->policed_time = wlan_cpu_to_le16(pqstats->policed_time);
+    for (id = 0; id < MLAN_WMM_STATS_PKTS_HIST_BINS; id++) {
+        pcmd_qstats->delay_histogram[id] =
+            wlan_cpu_to_le16(pqstats->delay_histogram[id]);
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of WMM_QUEUE_STATS
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_wmm_queue_stats(IN pmlan_private pmpriv,
+                         const IN HostCmd_DS_COMMAND * resp,
+                         OUT mlan_ioctl_req * pioctl_buf)
+{
+    mlan_ds_wmm_cfg *pwmm = MNULL;
+    mlan_ds_wmm_queue_stats *pqstats = MNULL;
+    const HostCmd_DS_WMM_QUEUE_STATS *presp_qstats = &resp->params.queue_stats;
+    t_u8 id;
+
+    ENTER();
+
+    if (pioctl_buf) {
+        pwmm = (mlan_ds_wmm_cfg *) pioctl_buf->pbuf;
+        pqstats = (mlan_ds_wmm_queue_stats *) & pwmm->param.q_stats;
+
+        pqstats->action = presp_qstats->action;
+        pqstats->user_priority = presp_qstats->select_bin;
+        pqstats->pkt_count = wlan_le16_to_cpu(presp_qstats->pkt_count);
+        pqstats->pkt_loss = wlan_le16_to_cpu(presp_qstats->pkt_loss);
+        pqstats->avg_queue_delay
+            = wlan_le32_to_cpu(presp_qstats->avg_queue_delay);
+        pqstats->avg_tx_delay = wlan_le32_to_cpu(presp_qstats->avg_tx_delay);
+        pqstats->used_time = wlan_le16_to_cpu(presp_qstats->used_time);
+        pqstats->policed_time = wlan_le16_to_cpu(presp_qstats->policed_time);
+        for (id = 0; id < MLAN_WMM_STATS_PKTS_HIST_BINS; id++) {
+            pqstats->delay_histogram[id]
+                = wlan_le16_to_cpu(presp_qstats->delay_histogram[id]);
+        }
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prepares the command of WMM_TS_STATUS
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param cmd          A pointer to HostCmd_DS_COMMAND structure
+ *  @param pdata_buf    A pointer to data buffer
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_cmd_wmm_ts_status(IN pmlan_private pmpriv,
+                       OUT HostCmd_DS_COMMAND * cmd, IN t_void * pdata_buf)
+{
+    mlan_ds_wmm_ts_status *pts_status = (mlan_ds_wmm_ts_status *) pdata_buf;
+    HostCmd_DS_WMM_TS_STATUS *pcmd_ts_status = &cmd->params.ts_status;
+
+    ENTER();
+
+    cmd->command = wlan_cpu_to_le16(HostCmd_CMD_WMM_TS_STATUS);
+    cmd->size = wlan_cpu_to_le16(sizeof(HostCmd_DS_WMM_TS_STATUS)
+                                 + S_DS_GEN);
+    cmd->result = 0;
+
+    memcpy(pmpriv->adapter, (t_void *) pcmd_ts_status, (t_void *) pts_status,
+           sizeof(HostCmd_DS_WMM_TS_STATUS));
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function handles the command response of WMM_TS_STATUS
+ *
+ *  @param pmpriv       A pointer to mlan_private structure
+ *  @param resp         A pointer to HostCmd_DS_COMMAND
+ *  @param pioctl_buf   A pointer to mlan_ioctl_req structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS
+ */
+mlan_status
+wlan_ret_wmm_ts_status(IN pmlan_private pmpriv,
+                       IN HostCmd_DS_COMMAND * resp,
+                       OUT mlan_ioctl_req * pioctl_buf)
+{
+    mlan_ds_wmm_cfg *pwmm = MNULL;
+    HostCmd_DS_WMM_TS_STATUS *presp_ts_status = &resp->params.ts_status;
+
+    ENTER();
+
+    if (pioctl_buf) {
+        pwmm = (mlan_ds_wmm_cfg *) pioctl_buf->pbuf;
+        presp_ts_status->medium_time
+            = wlan_le16_to_cpu(presp_ts_status->medium_time);
+        memcpy(pmpriv->adapter,
+               (t_void *) & pwmm->param.ts_status,
+               (t_void *) presp_ts_status, sizeof(mlan_ds_wmm_ts_status));
+    }
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+#endif /* STA_SUPPORT */
diff --git a/drivers/net/wireless/sd8797/mlan/mlan_wmm.h b/drivers/net/wireless/sd8797/mlan/mlan_wmm.h
new file mode 100644
index 000000000000..155f99330a05
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlan/mlan_wmm.h
@@ -0,0 +1,182 @@
+/** @file mlan_wmm.h
+ *
+ *  @brief This file contains related macros, enum, and struct
+ *  of wmm functionalities
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/****************************************************
+Change log:
+    10/24/2008: initial version
+****************************************************/
+
+#ifndef _MLAN_WMM_H_
+#define _MLAN_WMM_H_
+
+/**
+ *  @brief This function gets the TID
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param ptr          A pointer to RA list table
+ *
+ *  @return 	    TID
+ */
+static INLINE int
+wlan_get_tid(pmlan_adapter pmadapter, raListTbl * ptr)
+{
+    pmlan_buffer mbuf;
+
+    ENTER();
+    mbuf =
+        (pmlan_buffer) util_peek_list(pmadapter->pmoal_handle, &ptr->buf_head,
+                                      MNULL, MNULL);
+    LEAVE();
+
+    return mbuf->priority;
+}
+
+/**
+ *  @brief This function gets the length of a list
+ *
+ *  @param pmadapter    A pointer to mlan_adapter structure
+ *  @param head         A pointer to mlan_list_head
+ *
+ *  @return 	    Length of list
+ */
+static INLINE int
+wlan_wmm_list_len(pmlan_adapter pmadapter, pmlan_list_head head)
+{
+    pmlan_linked_list pos;
+    int count = 0;
+
+    ENTER();
+
+    pos = head->pnext;
+
+    while (pos != (pmlan_linked_list) head) {
+        ++count;
+        pos = pos->pnext;
+    }
+
+    LEAVE();
+    return count;
+}
+
+/** Add buffer to WMM Tx queue */
+void wlan_wmm_add_buf_txqueue(pmlan_adapter pmadapter, pmlan_buffer pmbuf);
+/** Add to RA list */
+void wlan_ralist_add(mlan_private * priv, t_u8 * ra);
+/** Update the RA list */
+int wlan_ralist_update(mlan_private * priv, t_u8 * old_ra, t_u8 * new_ra);
+
+/** WMM status change command handler */
+mlan_status wlan_cmd_wmm_status_change(pmlan_private priv);
+/** Check if WMM lists are empty */
+int wlan_wmm_lists_empty(pmlan_adapter pmadapter);
+/** Process WMM transmission */
+t_void wlan_wmm_process_tx(pmlan_adapter pmadapter);
+/** Test to see if the ralist ptr is valid */
+int wlan_is_ralist_valid(mlan_private * priv, raListTbl * ra_list, int tid);
+
+raListTbl *wlan_wmm_get_ralist_node(pmlan_private priv, t_u8 tid,
+                                    t_u8 * ra_addr);
+t_u8 wlan_get_random_ba_threshold(pmlan_adapter pmadapter);
+
+/** Compute driver packet delay */
+t_u8 wlan_wmm_compute_driver_packet_delay(pmlan_private priv,
+                                          const pmlan_buffer pmbuf);
+/** Initialize WMM */
+t_void wlan_wmm_init(pmlan_adapter pmadapter);
+/** Setup WMM queues */
+extern void wlan_wmm_setup_queues(pmlan_private priv);
+/* Setup default queues */
+void wlan_wmm_default_queue_priorities(pmlan_private priv);
+
+#ifdef STA_SUPPORT
+/** Process WMM association request */
+extern t_u32 wlan_wmm_process_association_req(pmlan_private priv,
+                                              t_u8 ** ppAssocBuf,
+                                              IEEEtypes_WmmParameter_t * pWmmIE,
+                                              IEEEtypes_HTCap_t * pHTCap);
+#endif /* STA_SUPPORT */
+
+/** setup wmm queue priorities */
+void wlan_wmm_setup_queue_priorities(pmlan_private priv,
+                                     IEEEtypes_WmmParameter_t * wmm_ie);
+
+/** Downgrade WMM priority queue */
+void wlan_wmm_setup_ac_downgrade(pmlan_private priv);
+/** select WMM queue */
+t_u8 wlan_wmm_select_queue(mlan_private * pmpriv, t_u8 tid);
+#ifdef UAP_SUPPORT
+t_void wlan_wmm_delete_peer_ralist(pmlan_private priv, t_u8 * mac);
+#endif
+
+#ifdef STA_SUPPORT
+/*
+ *  Functions used in the cmd handling routine
+ */
+/** WMM ADDTS request command handler */
+extern mlan_status wlan_cmd_wmm_addts_req(IN pmlan_private pmpriv,
+                                          OUT HostCmd_DS_COMMAND * cmd,
+                                          IN t_void * pdata_buf);
+/** WMM DELTS request command handler */
+extern mlan_status wlan_cmd_wmm_delts_req(IN pmlan_private pmpriv,
+                                          OUT HostCmd_DS_COMMAND * cmd,
+                                          IN t_void * pdata_buf);
+/** WMM QUEUE_CONFIG command handler */
+extern mlan_status wlan_cmd_wmm_queue_config(IN pmlan_private pmpriv,
+                                             OUT HostCmd_DS_COMMAND * cmd,
+                                             IN t_void * pdata_buf);
+/** WMM QUEUE_STATS command handler */
+extern mlan_status wlan_cmd_wmm_queue_stats(IN pmlan_private pmpriv,
+                                            OUT HostCmd_DS_COMMAND * cmd,
+                                            IN t_void * pdata_buf);
+/** WMM TS_STATUS command handler */
+extern mlan_status wlan_cmd_wmm_ts_status(IN pmlan_private pmpriv,
+                                          OUT HostCmd_DS_COMMAND * cmd,
+                                          IN t_void * pdata_buf);
+
+/*
+ *  Functions used in the cmdresp handling routine
+ */
+/** WMM get status command response handler */
+extern mlan_status wlan_ret_wmm_get_status(IN pmlan_private priv,
+                                           IN t_u8 * ptlv, IN int resp_len);
+/** WMM ADDTS request command response handler */
+extern mlan_status wlan_ret_wmm_addts_req(IN pmlan_private pmpriv,
+                                          const IN HostCmd_DS_COMMAND * resp,
+                                          OUT mlan_ioctl_req * pioctl_buf);
+/** WMM DELTS request command response handler */
+extern mlan_status wlan_ret_wmm_delts_req(IN pmlan_private pmpriv,
+                                          const IN HostCmd_DS_COMMAND * resp,
+                                          OUT mlan_ioctl_req * pioctl_buf);
+/** WMM QUEUE_CONFIG command response handler */
+extern mlan_status wlan_ret_wmm_queue_config(IN pmlan_private pmpriv,
+                                             const IN HostCmd_DS_COMMAND * resp,
+                                             OUT mlan_ioctl_req * pioctl_buf);
+/** WMM QUEUE_STATS command response handler */
+extern mlan_status wlan_ret_wmm_queue_stats(IN pmlan_private pmpriv,
+                                            const IN HostCmd_DS_COMMAND * resp,
+                                            OUT mlan_ioctl_req * pioctl_buf);
+/** WMM TS_STATUS command response handler */
+extern mlan_status wlan_ret_wmm_ts_status(IN pmlan_private pmpriv,
+                                          IN HostCmd_DS_COMMAND * resp,
+                                          OUT mlan_ioctl_req * pioctl_buf);
+#endif /* STA_SUPPORT */
+
+#endif /* !_MLAN_WMM_H_ */
diff --git a/drivers/net/wireless/sd8797/mlinux/mlan.h b/drivers/net/wireless/sd8797/mlinux/mlan.h
new file mode 100644
index 000000000000..a341e1547099
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/mlan.h
@@ -0,0 +1,35 @@
+/** @file mlan.h
+ *
+ *  @brief This file declares all APIs that will be called from MOAL module.
+ *  It also defines the data structures used for APIs between MLAN and MOAL.
+ *
+ * Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available by writing to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    10/13/2008: initial version
+    11/07/2008: split mlan.h into mlan_decl.h & mlan_ioctl.h
+******************************************************/
+
+#ifndef _MLAN_H_
+#define _MLAN_H_
+
+#include "mlan_decl.h"
+#include "mlan_ioctl.h"
+#include "mlan_ieee.h"
+
+#endif /* !_MLAN_H_ */
diff --git a/drivers/net/wireless/sd8797/mlinux/mlan_decl.h b/drivers/net/wireless/sd8797/mlinux/mlan_decl.h
new file mode 100644
index 000000000000..41cf2bf5f2f1
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/mlan_decl.h
@@ -0,0 +1,893 @@
+/** @file mlan_decl.h
+ *
+ *  @brief This file declares the generic data structures and APIs.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    11/07/2008: initial version
+******************************************************/
+
+#ifndef _MLAN_DECL_H_
+#define _MLAN_DECL_H_
+
+/** MLAN release version */
+#define MLAN_RELEASE_VERSION		"311"
+
+/** Re-define generic data types for MLAN/MOAL */
+/** Signed char (1-byte) */
+typedef char t_s8;
+/** Unsigned char (1-byte) */
+typedef unsigned char t_u8;
+/** Signed short (2-bytes) */
+typedef short t_s16;
+/** Unsigned short (2-bytes) */
+typedef unsigned short t_u16;
+/** Signed long (4-bytes) */
+typedef int t_s32;
+/** Unsigned long (4-bytes) */
+typedef unsigned int t_u32;
+/** Signed long long 8-bytes) */
+typedef long long t_s64;
+/** Unsigned long long 8-bytes) */
+typedef unsigned long long t_u64;
+/** Void pointer (4-bytes) */
+typedef void t_void;
+/** Size type */
+typedef t_u32 t_size;
+/** Boolean type */
+typedef t_u8 t_bool;
+
+#ifdef MLAN_64BIT
+/** Pointer type (64-bit) */
+typedef t_u64 t_ptr;
+/** Signed value (64-bit) */
+typedef t_s64 t_sval;
+#else
+/** Pointer type (32-bit) */
+typedef t_u32 t_ptr;
+/** Signed value (32-bit) */
+typedef t_s32 t_sval;
+#endif
+
+/** Constants below */
+
+#ifdef __GNUC__
+/** Structure packing begins */
+#define MLAN_PACK_START
+/** Structure packeing end */
+#define MLAN_PACK_END  __attribute__ ((packed))
+#else /* !__GNUC__ */
+#ifdef PRAGMA_PACK
+/** Structure packing begins */
+#define MLAN_PACK_START
+/** Structure packeing end */
+#define MLAN_PACK_END
+#else /* !PRAGMA_PACK */
+/** Structure packing begins */
+#define MLAN_PACK_START   __packed
+/** Structure packing end */
+#define MLAN_PACK_END
+#endif /* PRAGMA_PACK */
+#endif /* __GNUC__ */
+
+#ifndef INLINE
+#ifdef __GNUC__
+/** inline directive */
+#define	INLINE	inline
+#else
+/** inline directive */
+#define	INLINE	__inline
+#endif
+#endif
+
+/** MLAN TRUE */
+#define MTRUE                    (1)
+/** MLAN FALSE */
+#define MFALSE                   (0)
+
+/** Macros for Data Alignment : size */
+#define ALIGN_SZ(p, a)	\
+	(((p) + ((a) - 1)) & ~((a) - 1))
+
+/** Macros for Data Alignment : address */
+#define ALIGN_ADDR(p, a)    \
+    ((((t_ptr)(p)) + (((t_ptr)(a)) - 1)) & ~(((t_ptr)(a)) - 1))
+
+/** Return the byte offset of a field in the given structure */
+#define MLAN_FIELD_OFFSET(type, field) ((t_u32)(t_ptr)&(((type *)0)->field))
+/** Return aligned offset */
+#define OFFSET_ALIGN_ADDR(p, a) (t_u32)(ALIGN_ADDR(p, a) - (t_ptr)p)
+
+/** Maximum BSS numbers */
+#define MLAN_MAX_BSS_NUM         (16)
+
+/** NET IP alignment */
+#define MLAN_NET_IP_ALIGN        0
+
+/** DMA alignment */
+#define DMA_ALIGNMENT            64
+/** max size of TxPD */
+#define MAX_TXPD_SIZE            32
+
+/** Minimum data header length */
+#define MLAN_MIN_DATA_HEADER_LEN (DMA_ALIGNMENT+MAX_TXPD_SIZE)
+
+/** rx data header length */
+#define MLAN_RX_HEADER_LEN       MLAN_MIN_DATA_HEADER_LEN
+
+/** This is current limit on Maximum Tx AMPDU allowed */
+#define MLAN_MAX_TX_BASTREAM_SUPPORTED     2
+/** This is current limit on Maximum Rx AMPDU allowed */
+#define MLAN_MAX_RX_BASTREAM_SUPPORTED     16
+
+#ifdef STA_SUPPORT
+/** Default Win size attached during ADDBA request */
+#define MLAN_STA_AMPDU_DEF_TXWINSIZE       16
+/** Default Win size attached during ADDBA response */
+#define MLAN_STA_AMPDU_DEF_RXWINSIZE       32
+#endif /* STA_SUPPORT */
+#ifdef UAP_SUPPORT
+/** Default Win size attached during ADDBA request */
+#define MLAN_UAP_AMPDU_DEF_TXWINSIZE       32
+/** Default Win size attached during ADDBA response */
+#define MLAN_UAP_AMPDU_DEF_RXWINSIZE       16
+#endif /* UAP_SUPPORT */
+/** Block ack timeout value */
+#define MLAN_DEFAULT_BLOCK_ACK_TIMEOUT  0xffff
+/** Maximum Tx Win size configured for ADDBA request [10 bits] */
+#define MLAN_AMPDU_MAX_TXWINSIZE        0x3ff
+/** Maximum Rx Win size configured for ADDBA request [10 bits] */
+#define MLAN_AMPDU_MAX_RXWINSIZE        0x3ff
+
+/** Rate index for HR/DSSS 0 */
+#define MLAN_RATE_INDEX_HRDSSS0 0
+/** Rate index for HR/DSSS 3 */
+#define MLAN_RATE_INDEX_HRDSSS3 3
+/** Rate index for OFDM 0 */
+#define MLAN_RATE_INDEX_OFDM0   4
+/** Rate index for OFDM 7 */
+#define MLAN_RATE_INDEX_OFDM7   11
+/** Rate index for MCS 0 */
+#define MLAN_RATE_INDEX_MCS0    12
+/** Rate index for MCS 7 */
+#define MLAN_RATE_INDEX_MCS7    19
+/** Rate index for MCS 9 */
+#define MLAN_RATE_INDEX_MCS9    21
+/** Rate index for MCS15 */
+#define MLAN_RATE_INDEX_MCS15   27
+/** Rate index for MCS 32 */
+#define MLAN_RATE_INDEX_MCS32   44
+/** Rate index for MCS 127 */
+#define MLAN_RATE_INDEX_MCS127  139
+
+/** Rate bitmap for OFDM 0 */
+#define MLAN_RATE_BITMAP_OFDM0  16
+/** Rate bitmap for OFDM 7 */
+#define MLAN_RATE_BITMAP_OFDM7  23
+/** Rate bitmap for MCS 0 */
+#define MLAN_RATE_BITMAP_MCS0   32
+/** Rate bitmap for MCS 127 */
+#define MLAN_RATE_BITMAP_MCS127 159
+
+/** Size of rx data buffer */
+#define MLAN_RX_DATA_BUF_SIZE     (4 * 1024)
+/** Size of rx command buffer */
+#define MLAN_RX_CMD_BUF_SIZE      (2 * 1024)
+
+/** MLAN MAC Address Length */
+#define MLAN_MAC_ADDR_LENGTH     (6)
+/** MLAN 802.11 MAC Address */
+typedef t_u8 mlan_802_11_mac_addr[MLAN_MAC_ADDR_LENGTH];
+
+/** MLAN Maximum SSID Length */
+#define MLAN_MAX_SSID_LENGTH     (32)
+
+/** RTS/FRAG related defines */
+/** Minimum RTS value */
+#define MLAN_RTS_MIN_VALUE              (0)
+/** Maximum RTS value */
+#define MLAN_RTS_MAX_VALUE              (2347)
+/** Minimum FRAG value */
+#define MLAN_FRAG_MIN_VALUE             (256)
+/** Maximum FRAG value */
+#define MLAN_FRAG_MAX_VALUE             (2346)
+
+/** Minimum tx retry count */
+#define MLAN_TX_RETRY_MIN 		(0)
+/** Maximum tx retry count */
+#define MLAN_TX_RETRY_MAX 		(14)
+
+/** define SDIO block size for data Tx/Rx */
+/* We support up to 480-byte block size due to FW buffer limitation. */
+#define MLAN_SDIO_BLOCK_SIZE		256
+
+/** define SDIO block size for firmware download */
+#define MLAN_SDIO_BLOCK_SIZE_FW_DNLD	MLAN_SDIO_BLOCK_SIZE
+
+/** define allocated buffer size */
+#define ALLOC_BUF_SIZE           (4 * 1024)
+
+/** SDIO IO Port mask */
+#define MLAN_SDIO_IO_PORT_MASK		0xfffff
+/** SDIO Block/Byte mode mask */
+#define MLAN_SDIO_BYTE_MODE_MASK	0x80000000
+
+/** Max retry number of IO write */
+#define MAX_WRITE_IOMEM_RETRY		2
+
+/** IN parameter */
+#define IN
+/** OUT parameter */
+#define OUT
+
+/** BIT value */
+#define MBIT(x)    (((t_u32)1) << (x))
+
+/** Buffer flag for requeued packet */
+#define MLAN_BUF_FLAG_REQUEUED_PKT      MBIT(0)
+/** Buffer flag for transmit buf from moal */
+#define MLAN_BUF_FLAG_MOAL_TX_BUF        MBIT(1)
+/** Buffer flag for malloc mlan_buffer */
+#define MLAN_BUF_FLAG_MALLOC_BUF        MBIT(2)
+
+/** Buffer flag for bridge packet */
+#define MLAN_BUF_FLAG_BRIDGE_BUF        MBIT(3)
+
+#ifdef DEBUG_LEVEL1
+/** Debug level bit definition */
+#define	MMSG        MBIT(0)
+#define MFATAL      MBIT(1)
+#define MERROR      MBIT(2)
+#define MDATA       MBIT(3)
+#define MCMND       MBIT(4)
+#define MEVENT      MBIT(5)
+#define MINTR       MBIT(6)
+#define MIOCTL      MBIT(7)
+
+#define MDAT_D      MBIT(16)
+#define MCMD_D      MBIT(17)
+#define MEVT_D      MBIT(18)
+#define MFW_D       MBIT(19)
+#define MIF_D       MBIT(20)
+
+#define MENTRY      MBIT(28)
+#define MWARN       MBIT(29)
+#define MINFO       MBIT(30)
+#define MHEX_DUMP   MBIT(31)
+#endif /* DEBUG_LEVEL1 */
+
+/** Memory allocation type: DMA */
+#define	MLAN_MEM_DMA    MBIT(0)
+
+/** Default memory allocation flag */
+#define MLAN_MEM_DEF    0
+
+/** mlan_status */
+typedef enum _mlan_status
+{
+    MLAN_STATUS_FAILURE = 0xffffffff,
+    MLAN_STATUS_SUCCESS = 0,
+    MLAN_STATUS_PENDING,
+    MLAN_STATUS_RESOURCE,
+} mlan_status;
+
+/** mlan_error_code */
+typedef enum _mlan_error_code
+{
+    /** No error */
+    MLAN_ERROR_NO_ERROR = 0,
+    /** Firmware/device errors below (MSB=0) */
+    MLAN_ERROR_FW_NOT_READY = 0x00000001,
+    MLAN_ERROR_FW_BUSY,
+    MLAN_ERROR_FW_CMDRESP,
+    MLAN_ERROR_DATA_TX_FAIL,
+    MLAN_ERROR_DATA_RX_FAIL,
+    /** Driver errors below (MSB=1) */
+    MLAN_ERROR_PKT_SIZE_INVALID = 0x80000001,
+    MLAN_ERROR_PKT_TIMEOUT,
+    MLAN_ERROR_PKT_INVALID,
+    MLAN_ERROR_CMD_INVALID,
+    MLAN_ERROR_CMD_TIMEOUT,
+    MLAN_ERROR_CMD_DNLD_FAIL,
+    MLAN_ERROR_CMD_CANCEL,
+    MLAN_ERROR_CMD_RESP_FAIL,
+    MLAN_ERROR_CMD_ASSOC_FAIL,
+    MLAN_ERROR_CMD_SCAN_FAIL,
+    MLAN_ERROR_IOCTL_INVALID,
+    MLAN_ERROR_IOCTL_FAIL,
+    MLAN_ERROR_EVENT_UNKNOWN,
+    MLAN_ERROR_INVALID_PARAMETER,
+    MLAN_ERROR_NO_MEM,
+    /** More to add */
+} mlan_error_code;
+
+/** mlan_buf_type */
+typedef enum _mlan_buf_type
+{
+    MLAN_BUF_TYPE_CMD = 1,
+    MLAN_BUF_TYPE_DATA,
+    MLAN_BUF_TYPE_EVENT,
+    MLAN_BUF_TYPE_RAW_DATA,
+} mlan_buf_type;
+
+/** MLAN BSS type */
+typedef enum _mlan_bss_type
+{
+    MLAN_BSS_TYPE_STA = 0,
+    MLAN_BSS_TYPE_UAP = 1,
+#ifdef WIFI_DIRECT_SUPPORT
+    MLAN_BSS_TYPE_WIFIDIRECT = 2,
+#endif
+    MLAN_BSS_TYPE_ANY = 0xff,
+} mlan_bss_type;
+
+/** MLAN BSS role */
+typedef enum _mlan_bss_role
+{
+    MLAN_BSS_ROLE_STA = 0,
+    MLAN_BSS_ROLE_UAP = 1,
+    MLAN_BSS_ROLE_ANY = 0xff,
+} mlan_bss_role;
+
+/** BSS role bit mask */
+#define BSS_ROLE_BIT_MASK    MBIT(0)
+
+/** Get BSS role */
+#define GET_BSS_ROLE(priv)   ((priv)->bss_role & BSS_ROLE_BIT_MASK)
+
+/** mlan_data_frame_type */
+typedef enum _mlan_data_frame_type
+{
+    MLAN_DATA_FRAME_TYPE_ETH_II = 0,
+    MLAN_DATA_FRAME_TYPE_802_11,
+} mlan_data_frame_type;
+
+/** mlan_event_id */
+typedef enum _mlan_event_id
+{
+    /* Event generated by firmware (MSB=0) */
+    MLAN_EVENT_ID_FW_UNKNOWN = 0x00000001,
+    MLAN_EVENT_ID_FW_ADHOC_LINK_SENSED,
+    MLAN_EVENT_ID_FW_ADHOC_LINK_LOST,
+    MLAN_EVENT_ID_FW_DISCONNECTED,
+    MLAN_EVENT_ID_FW_MIC_ERR_UNI,
+    MLAN_EVENT_ID_FW_MIC_ERR_MUL,
+    MLAN_EVENT_ID_FW_BCN_RSSI_LOW,
+    MLAN_EVENT_ID_FW_BCN_RSSI_HIGH,
+    MLAN_EVENT_ID_FW_BCN_SNR_LOW,
+    MLAN_EVENT_ID_FW_BCN_SNR_HIGH,
+    MLAN_EVENT_ID_FW_MAX_FAIL,
+    MLAN_EVENT_ID_FW_DATA_RSSI_LOW,
+    MLAN_EVENT_ID_FW_DATA_RSSI_HIGH,
+    MLAN_EVENT_ID_FW_DATA_SNR_LOW,
+    MLAN_EVENT_ID_FW_DATA_SNR_HIGH,
+    MLAN_EVENT_ID_FW_LINK_QUALITY,
+    MLAN_EVENT_ID_FW_PORT_RELEASE,
+    MLAN_EVENT_ID_FW_PRE_BCN_LOST,
+    MLAN_EVENT_ID_FW_WMM_CONFIG_CHANGE,
+    MLAN_EVENT_ID_FW_HS_WAKEUP,
+    MLAN_EVENT_ID_FW_BG_SCAN,
+    MLAN_EVENT_ID_FW_WEP_ICV_ERR,
+    MLAN_EVENT_ID_FW_STOP_TX,
+    MLAN_EVENT_ID_FW_START_TX,
+    MLAN_EVENT_ID_FW_CHANNEL_SWITCH_ANN,
+    MLAN_EVENT_ID_FW_RADAR_DETECTED,
+    MLAN_EVENT_ID_FW_CHANNEL_REPORT_RDY,
+    MLAN_EVENT_ID_FW_BW_CHANGED,
+#ifdef WIFI_DIRECT_SUPPORT
+    MLAN_EVENT_ID_FW_REMAIN_ON_CHAN_EXPIRED,
+#endif
+#ifdef UAP_SUPPORT
+    MLAN_EVENT_ID_UAP_FW_BSS_START,
+    MLAN_EVENT_ID_UAP_FW_BSS_ACTIVE,
+    MLAN_EVENT_ID_UAP_FW_BSS_IDLE,
+    MLAN_EVENT_ID_UAP_FW_STA_CONNECT,
+    MLAN_EVENT_ID_UAP_FW_STA_DISCONNECT,
+#endif
+
+    /* Event generated by MLAN driver (MSB=1) */
+    MLAN_EVENT_ID_DRV_CONNECTED = 0x80000001,
+    MLAN_EVENT_ID_DRV_DEFER_HANDLING,
+    MLAN_EVENT_ID_DRV_HS_ACTIVATED,
+    MLAN_EVENT_ID_DRV_HS_DEACTIVATED,
+    MLAN_EVENT_ID_DRV_MGMT_FRAME,
+    MLAN_EVENT_ID_DRV_OBSS_SCAN_PARAM,
+    MLAN_EVENT_ID_DRV_PASSTHRU,
+    MLAN_EVENT_ID_DRV_SCAN_REPORT,
+    MLAN_EVENT_ID_DRV_MEAS_REPORT,
+    MLAN_EVENT_ID_DRV_REPORT_STRING,
+    MLAN_EVENT_ID_DRV_DBG_DUMP,
+} mlan_event_id;
+
+/** Data Structures */
+/** mlan_image data structure */
+typedef struct _mlan_fw_image
+{
+    /** Helper image buffer pointer */
+    t_u8 *phelper_buf;
+    /** Helper image length */
+    t_u32 helper_len;
+    /** Firmware image buffer pointer */
+    t_u8 *pfw_buf;
+    /** Firmware image length */
+    t_u32 fw_len;
+} mlan_fw_image, *pmlan_fw_image;
+
+/** Custom data structure */
+typedef struct _mlan_init_param
+{
+    /** Cal data buffer pointer */
+    t_u8 *pcal_data_buf;
+    /** Cal data length */
+    t_u32 cal_data_len;
+    /** Other custom data */
+} mlan_init_param, *pmlan_init_param;
+
+/** mlan_event data structure */
+typedef struct _mlan_event
+{
+    /** BSS index number for multiple BSS support */
+    t_u32 bss_index;
+    /** Event ID */
+    mlan_event_id event_id;
+    /** Event length */
+    t_u32 event_len;
+    /** Event buffer */
+    t_u8 event_buf[1];
+} mlan_event, *pmlan_event;
+
+/** mlan_event_scan_result data structure */
+typedef MLAN_PACK_START struct _mlan_event_scan_result
+{
+    /** Event ID */
+    t_u16 event_id;
+    /** BSS index number for multiple BSS support */
+    t_u8 bss_index;
+    /** BSS type */
+    t_u8 bss_type;
+    /** More event available or not */
+    t_u8 more_event;
+    /** Reserved */
+    t_u8 reserved[3];
+    /** Size of the response buffer */
+    t_u16 buf_size;
+    /** Number of BSS in scan response */
+    t_u8 num_of_set;
+} MLAN_PACK_END mlan_event_scan_result, *pmlan_event_scan_result;
+
+/** mlan_ioctl_req data structure */
+typedef struct _mlan_ioctl_req
+{
+    /** Status code from firmware/driver */
+    t_u32 status_code;
+    /** BSS index number for multiple BSS support */
+    t_u32 bss_index;
+    /** Request id */
+    t_u32 req_id;
+    /** Action: set or get */
+    t_u32 action;
+    /** Pointer to buffer */
+    t_u8 *pbuf;
+    /** Length of buffer */
+    t_u32 buf_len;
+    /** Length of the data read/written in buffer */
+    t_u32 data_read_written;
+    /** Length of buffer needed */
+    t_u32 buf_len_needed;
+    /** Reserved for MOAL module */
+    t_ptr reserved_1;
+} mlan_ioctl_req, *pmlan_ioctl_req;
+
+/** mlan_buffer data structure */
+typedef struct _mlan_buffer
+{
+    /** Pointer to previous mlan_buffer */
+    struct _mlan_buffer *pprev;
+    /** Pointer to next mlan_buffer */
+    struct _mlan_buffer *pnext;
+    /** Status code from firmware/driver */
+    t_u32 status_code;
+    /** Flags for this buffer */
+    t_u32 flags;
+    /** BSS index number for multiple BSS support */
+    t_u32 bss_index;
+    /** Buffer descriptor, e.g. skb in Linux */
+    t_void *pdesc;
+    /** Pointer to buffer */
+    t_u8 *pbuf;
+    /** Offset to data */
+    t_u32 data_offset;
+    /** Data length */
+    t_u32 data_len;
+    /** Buffer type: data, cmd, event etc. */
+    mlan_buf_type buf_type;
+
+    /** Fields below are valid for data packet only */
+    /** QoS priority */
+    t_u32 priority;
+    /** Time stamp when packet is received (seconds) */
+    t_u32 in_ts_sec;
+    /** Time stamp when packet is received (micro seconds) */
+    t_u32 in_ts_usec;
+    /** Time stamp when packet is processed (seconds) */
+    t_u32 out_ts_sec;
+    /** Time stamp when packet is processed (micro seconds) */
+    t_u32 out_ts_usec;
+
+    /** Fields below are valid for MLAN module only */
+    /** Pointer to parent mlan_buffer */
+    struct _mlan_buffer *pparent;
+    /** Use count for this buffer */
+    t_u32 use_count;
+} mlan_buffer, *pmlan_buffer;
+
+/** mlan_bss_attr data structure */
+typedef struct _mlan_bss_attr
+{
+    /** BSS type */
+    t_u32 bss_type;
+    /** Data frame type: Ethernet II, 802.11, etc. */
+    t_u32 frame_type;
+    /** The BSS is active (non-0) or not (0). */
+    t_u32 active;
+    /** BSS Priority */
+    t_u32 bss_priority;
+    /** BSS number */
+    t_u32 bss_num;
+} mlan_bss_attr, *pmlan_bss_attr;
+
+#ifdef PRAGMA_PACK
+#pragma pack(push, 1)
+#endif
+
+/** Type enumeration for the command result */
+typedef MLAN_PACK_START enum _mlan_cmd_result_e
+{
+    MLAN_CMD_RESULT_SUCCESS = 0,
+    MLAN_CMD_RESULT_FAILURE = 1,
+    MLAN_CMD_RESULT_TIMEOUT = 2,
+    MLAN_CMD_RESULT_INVALID_DATA = 3
+} MLAN_PACK_END mlan_cmd_result_e;
+
+/** Type enumeration of WMM AC_QUEUES */
+typedef MLAN_PACK_START enum _mlan_wmm_ac_e
+{
+    WMM_AC_BK,
+    WMM_AC_BE,
+    WMM_AC_VI,
+    WMM_AC_VO
+} MLAN_PACK_END mlan_wmm_ac_e;
+
+/** Type enumeration for the action field in the Queue Config command */
+typedef MLAN_PACK_START enum _mlan_wmm_queue_config_action_e
+{
+    MLAN_WMM_QUEUE_CONFIG_ACTION_GET = 0,
+    MLAN_WMM_QUEUE_CONFIG_ACTION_SET = 1,
+    MLAN_WMM_QUEUE_CONFIG_ACTION_DEFAULT = 2,
+    MLAN_WMM_QUEUE_CONFIG_ACTION_MAX
+} MLAN_PACK_END mlan_wmm_queue_config_action_e;
+
+/** Type enumeration for the action field in the queue stats command */
+typedef MLAN_PACK_START enum _mlan_wmm_queue_stats_action_e
+{
+    MLAN_WMM_STATS_ACTION_START = 0,
+    MLAN_WMM_STATS_ACTION_STOP = 1,
+    MLAN_WMM_STATS_ACTION_GET_CLR = 2,
+    MLAN_WMM_STATS_ACTION_SET_CFG = 3,  /* Not currently used */
+    MLAN_WMM_STATS_ACTION_GET_CFG = 4,  /* Not currently used */
+    MLAN_WMM_STATS_ACTION_MAX
+} MLAN_PACK_END mlan_wmm_queue_stats_action_e;
+
+/**
+ *  @brief IOCTL structure for a Traffic stream status.
+ *
+ */
+typedef MLAN_PACK_START struct
+{
+    /** TSID: Range: 0->7 */
+    t_u8 tid;
+    /** TSID specified is valid */
+    t_u8 valid;
+    /** AC TSID is active on */
+    t_u8 access_category;
+    /** UP specified for the TSID */
+    t_u8 user_priority;
+    /** Power save mode for TSID: 0 (legacy), 1 (UAPSD) */
+    t_u8 psb;
+    /** Upstream(0), Downlink(1), Bidirectional(3) */
+    t_u8 flow_dir;
+    /** Medium time granted for the TSID */
+    t_u16 medium_time;
+} MLAN_PACK_END wlan_ioctl_wmm_ts_status_t,
+/** Type definition of mlan_ds_wmm_ts_status for MLAN_OID_WMM_CFG_TS_STATUS */
+  mlan_ds_wmm_ts_status, *pmlan_ds_wmm_ts_status;
+
+/** Max Ie length */
+#define MAX_IE_SIZE             256
+
+/** custom IE */
+typedef MLAN_PACK_START struct _custom_ie
+{
+    /** IE Index */
+    t_u16 ie_index;
+    /** Mgmt Subtype Mask */
+    t_u16 mgmt_subtype_mask;
+    /** IE Length */
+    t_u16 ie_length;
+    /** IE buffer */
+    t_u8 ie_buffer[MAX_IE_SIZE];
+} MLAN_PACK_END custom_ie;
+
+/** Max IE index to FW */
+#define MAX_MGMT_IE_INDEX_TO_FW         4
+/** Max IE index per BSS */
+#define MAX_MGMT_IE_INDEX               16
+
+/** custom IE info */
+typedef MLAN_PACK_START struct _custom_ie_info
+{
+    /** size of buffer */
+    t_u16 buf_size;
+    /** no of buffers of buf_size */
+    t_u16 buf_count;
+} MLAN_PACK_END custom_ie_info;
+
+/** TLV buffer : Max Mgmt IE */
+typedef MLAN_PACK_START struct _tlvbuf_max_mgmt_ie
+{
+    /** Type */
+    t_u16 type;
+    /** Length */
+    t_u16 len;
+    /** No of tuples */
+    t_u16 count;
+    /** custom IE info tuples */
+    custom_ie_info info[MAX_MGMT_IE_INDEX];
+} MLAN_PACK_END tlvbuf_max_mgmt_ie;
+
+/** TLV buffer : custom IE */
+typedef MLAN_PACK_START struct _tlvbuf_custom_ie
+{
+    /** Type */
+    t_u16 type;
+    /** Length */
+    t_u16 len;
+    /** IE data */
+    custom_ie ie_data_list[MAX_MGMT_IE_INDEX_TO_FW];
+    /** Max mgmt IE TLV */
+    tlvbuf_max_mgmt_ie max_mgmt_ie;
+} MLAN_PACK_END mlan_ds_misc_custom_ie;
+
+#ifdef PRAGMA_PACK
+#pragma pack(pop)
+#endif
+
+/** mlan_callbacks data structure */
+typedef struct _mlan_callbacks
+{
+    /** moal_get_fw_data */
+    mlan_status(*moal_get_fw_data) (IN t_void * pmoal_handle,
+                                    IN t_u32 offset,
+                                    IN t_u32 len, OUT t_u8 * pbuf);
+    /** moal_init_fw_complete */
+    mlan_status(*moal_init_fw_complete) (IN t_void * pmoal_handle,
+                                         IN mlan_status status);
+    /** moal_shutdown_fw_complete */
+    mlan_status(*moal_shutdown_fw_complete) (IN t_void * pmoal_handle,
+                                             IN mlan_status status);
+    /** moal_send_packet_complete */
+    mlan_status(*moal_send_packet_complete) (IN t_void * pmoal_handle,
+                                             IN pmlan_buffer pmbuf,
+                                             IN mlan_status status);
+    /** moal_recv_complete */
+    mlan_status(*moal_recv_complete) (IN t_void * pmoal_handle,
+                                      IN pmlan_buffer pmbuf,
+                                      IN t_u32 port, IN mlan_status status);
+    /** moal_recv_packet */
+    mlan_status(*moal_recv_packet) (IN t_void * pmoal_handle,
+                                    IN pmlan_buffer pmbuf);
+    /** moal_recv_event */
+    mlan_status(*moal_recv_event) (IN t_void * pmoal_handle,
+                                   IN pmlan_event pmevent);
+    /** moal_ioctl_complete */
+    mlan_status(*moal_ioctl_complete) (IN t_void * pmoal_handle,
+                                       IN pmlan_ioctl_req pioctl_req,
+                                       IN mlan_status status);
+    /** moal_alloc_mlan_buffer */
+    mlan_status(*moal_alloc_mlan_buffer) (IN t_void * pmoal_handle,
+                                          IN t_u32 size,
+                                          OUT pmlan_buffer * pmbuf);
+    /** moal_free_mlan_buffer */
+    mlan_status(*moal_free_mlan_buffer) (IN t_void * pmoal_handle,
+                                         IN pmlan_buffer pmbuf);
+    /** moal_write_reg */
+    mlan_status(*moal_write_reg) (IN t_void * pmoal_handle,
+                                  IN t_u32 reg, IN t_u32 data);
+    /** moal_read_reg */
+    mlan_status(*moal_read_reg) (IN t_void * pmoal_handle,
+                                 IN t_u32 reg, OUT t_u32 * data);
+    /** moal_write_data_sync */
+    mlan_status(*moal_write_data_sync) (IN t_void * pmoal_handle,
+                                        IN pmlan_buffer pmbuf,
+                                        IN t_u32 port, IN t_u32 timeout);
+    /** moal_read_data_sync */
+    mlan_status(*moal_read_data_sync) (IN t_void * pmoal_handle,
+                                       IN OUT pmlan_buffer pmbuf,
+                                       IN t_u32 port, IN t_u32 timeout);
+    /** moal_malloc */
+    mlan_status(*moal_malloc) (IN t_void * pmoal_handle,
+                               IN t_u32 size, IN t_u32 flag, OUT t_u8 ** ppbuf);
+    /** moal_mfree */
+    mlan_status(*moal_mfree) (IN t_void * pmoal_handle, IN t_u8 * pbuf);
+    /** moal_memset */
+    t_void *(*moal_memset) (IN t_void * pmoal_handle,
+                            IN t_void * pmem, IN t_u8 byte, IN t_u32 num);
+    /** moal_memcpy */
+    t_void *(*moal_memcpy) (IN t_void * pmoal_handle,
+                            IN t_void * pdest,
+                            IN const t_void * psrc, IN t_u32 num);
+    /** moal_memmove */
+    t_void *(*moal_memmove) (IN t_void * pmoal_handle,
+                             IN t_void * pdest,
+                             IN const t_void * psrc, IN t_u32 num);
+    /** moal_memcmp */
+      t_s32(*moal_memcmp) (IN t_void * pmoal_handle,
+                           IN const t_void * pmem1,
+                           IN const t_void * pmem2, IN t_u32 num);
+    /** moal_udelay */
+      t_void(*moal_udelay) (IN t_void * pmoal_handle, IN t_u32 udelay);
+    /** moal_get_system_time */
+      mlan_status(*moal_get_system_time) (IN t_void * pmoal_handle,
+                                          OUT t_u32 * psec, OUT t_u32 * pusec);
+    /** moal_init_timer*/
+      mlan_status(*moal_init_timer) (IN t_void * pmoal_handle,
+                                     OUT t_void ** pptimer,
+                                     IN t_void(*callback) (t_void * pcontext),
+                                     IN t_void * pcontext);
+    /** moal_free_timer */
+      mlan_status(*moal_free_timer) (IN t_void * pmoal_handle,
+                                     IN t_void * ptimer);
+    /** moal_start_timer*/
+      mlan_status(*moal_start_timer) (IN t_void * pmoal_handle,
+                                      IN t_void * ptimer,
+                                      IN t_u8 periodic, IN t_u32 msec);
+    /** moal_stop_timer*/
+      mlan_status(*moal_stop_timer) (IN t_void * pmoal_handle,
+                                     IN t_void * ptimer);
+    /** moal_init_lock */
+      mlan_status(*moal_init_lock) (IN t_void * pmoal_handle,
+                                    OUT t_void ** pplock);
+    /** moal_free_lock */
+      mlan_status(*moal_free_lock) (IN t_void * pmoal_handle,
+                                    IN t_void * plock);
+    /** moal_spin_lock */
+      mlan_status(*moal_spin_lock) (IN t_void * pmoal_handle,
+                                    IN t_void * plock);
+    /** moal_spin_unlock */
+      mlan_status(*moal_spin_unlock) (IN t_void * pmoal_handle,
+                                      IN t_void * plock);
+    /** moal_print */
+      t_void(*moal_print) (IN t_void * pmoal_handle,
+                           IN t_u32 level, IN t_s8 * pformat, IN ...);
+    /** moal_print_netintf */
+      t_void(*moal_print_netintf) (IN t_void * pmoal_handle,
+                                   IN t_u32 bss_index, IN t_u32 level);
+    /** moal_assert */
+      t_void(*moal_assert) (IN t_void * pmoal_handle, IN t_u32 cond);
+} mlan_callbacks, *pmlan_callbacks;
+
+/** Interrupt Mode SDIO */
+#define INT_MODE_SDIO       0
+/** Interrupt Mode GPIO */
+#define INT_MODE_GPIO       1
+
+/** Parameter unchanged, use MLAN default setting */
+#define MLAN_INIT_PARA_UNCHANGED     0
+/** Parameter enabled, override MLAN default setting */
+#define MLAN_INIT_PARA_ENABLED       1
+/** Parameter disabled, override MLAN default setting */
+#define MLAN_INIT_PARA_DISABLED      2
+
+/** mlan_device data structure */
+typedef struct _mlan_device
+{
+    /** MOAL Handle */
+    t_void *pmoal_handle;
+    /** BSS Attributes */
+    mlan_bss_attr bss_attr[MLAN_MAX_BSS_NUM];
+    /** Callbacks */
+    mlan_callbacks callbacks;
+#ifdef MFG_CMD_SUPPORT
+    /** MFG mode */
+    t_u32 mfg_mode;
+#endif
+    /** SDIO interrupt mode (0: INT_MODE_SDIO, 1: INT_MODE_GPIO) */
+    t_u32 int_mode;
+    /** GPIO interrupt pin number */
+    t_u32 gpio_pin;
+#ifdef DEBUG_LEVEL1
+    /** Driver debug bit masks */
+    t_u32 drvdbg;
+#endif
+#ifdef SDIO_MULTI_PORT_TX_AGGR
+    /** SDIO MPA Tx */
+    t_u32 mpa_tx_cfg;
+#endif
+#ifdef SDIO_MULTI_PORT_RX_AGGR
+    /** SDIO MPA Rx */
+    t_u32 mpa_rx_cfg;
+#endif
+    /** Auto deep sleep */
+    t_u32 auto_ds;
+    /** IEEE PS mode */
+    t_u32 ps_mode;
+    /** Max Tx buffer size */
+    t_u32 max_tx_buf;
+#if defined(STA_SUPPORT)
+    /** 802.11d configuration */
+    t_u32 cfg_11d;
+#endif
+} mlan_device, *pmlan_device;
+
+/** MLAN API function prototype */
+#define MLAN_API
+
+/** Registration */
+MLAN_API mlan_status mlan_register(IN pmlan_device pmdevice,
+                                   OUT t_void ** ppmlan_adapter);
+
+/** Un-registration */
+MLAN_API mlan_status mlan_unregister(IN t_void * pmlan_adapter);
+
+/** Firmware Downloading */
+MLAN_API mlan_status mlan_dnld_fw(IN t_void * pmlan_adapter,
+                                  IN pmlan_fw_image pmfw);
+
+/** Custom data pass API */
+MLAN_API mlan_status mlan_set_init_param(IN t_void * pmlan_adapter,
+                                         IN pmlan_init_param pparam);
+
+/** Firmware Initialization */
+MLAN_API mlan_status mlan_init_fw(IN t_void * pmlan_adapter);
+
+/** Firmware Shutdown */
+MLAN_API mlan_status mlan_shutdown_fw(IN t_void * pmlan_adapter);
+
+/** Main Process */
+MLAN_API mlan_status mlan_main_process(IN t_void * pmlan_adapter);
+
+/** Packet Transmission */
+MLAN_API mlan_status mlan_send_packet(IN t_void * pmlan_adapter,
+                                      IN pmlan_buffer pmbuf);
+
+/** Packet Reception complete callback */
+MLAN_API mlan_status mlan_recv_packet_complete(IN t_void * pmlan_adapter,
+                                               IN pmlan_buffer pmbuf,
+                                               IN mlan_status status);
+
+/** interrupt handler */
+MLAN_API t_void mlan_interrupt(IN t_void * pmlan_adapter);
+
+/** mlan ioctl */
+MLAN_API mlan_status mlan_ioctl(IN t_void * pmlan_adapter,
+                                IN pmlan_ioctl_req pioctl_req);
+/** mlan select wmm queue */
+MLAN_API t_u8 mlan_select_wmm_queue(IN t_void * pmlan_adapter,
+                                    IN t_u8 bss_num, IN t_u8 tid);
+#endif /* !_MLAN_DECL_H_ */
diff --git a/drivers/net/wireless/sd8797/mlinux/mlan_ieee.h b/drivers/net/wireless/sd8797/mlinux/mlan_ieee.h
new file mode 100644
index 000000000000..2431e06f48fa
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/mlan_ieee.h
@@ -0,0 +1,1322 @@
+/** @file mlan_ieee.h
+ *
+ *  @brief This file contains IEEE information element related 
+ *  definitions used in MLAN and MOAL module.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    11/03/2008: initial version
+******************************************************/
+
+#ifndef _MLAN_IEEE_H_
+#define _MLAN_IEEE_H_
+
+/** FIX IES size in beacon buffer */
+#define WLAN_802_11_FIXED_IE_SIZE 			12
+/** WLAN supported rates */
+#define WLAN_SUPPORTED_RATES                14
+
+/** WLAN supported rates extension*/
+#define WLAN_SUPPORTED_RATES_EXT            32
+
+/** Enumeration definition*/
+/** WLAN_802_11_NETWORK_TYPE */
+typedef enum _WLAN_802_11_NETWORK_TYPE
+{
+    Wlan802_11FH,
+    Wlan802_11DS,
+    /* Defined as upper bound */
+    Wlan802_11NetworkTypeMax
+} WLAN_802_11_NETWORK_TYPE;
+
+/** Maximum size of IEEE Information Elements */
+#define IEEE_MAX_IE_SIZE      256
+
+#ifdef BIG_ENDIAN_SUPPORT
+/** Frame control: Type Mgmt frame */
+#define IEEE80211_FC_MGMT_FRAME_TYPE_MASK    0x3000
+/** Frame control: SubType Mgmt frame */
+#define IEEE80211_GET_FC_MGMT_FRAME_SUBTYPE(fc) (((fc) & 0xF000) >> 12)
+#else
+/** Frame control: Type Mgmt frame */
+#define IEEE80211_FC_MGMT_FRAME_TYPE_MASK    0x000C
+/** Frame control: SubType Mgmt frame */
+#define IEEE80211_GET_FC_MGMT_FRAME_SUBTYPE(fc) (((fc) & 0x00F0) >> 4)
+#endif
+
+#ifdef PRAGMA_PACK
+#pragma pack(push, 1)
+#endif
+
+/** IEEE Type definitions  */
+typedef MLAN_PACK_START enum _IEEEtypes_ElementId_e
+{
+    SSID = 0,
+    SUPPORTED_RATES = 1,
+
+    FH_PARAM_SET = 2,
+    DS_PARAM_SET = 3,
+    CF_PARAM_SET = 4,
+
+    IBSS_PARAM_SET = 6,
+
+#ifdef STA_SUPPORT
+    COUNTRY_INFO = 7,
+#endif /* STA_SUPPORT */
+
+    POWER_CONSTRAINT = 32,
+    POWER_CAPABILITY = 33,
+    TPC_REQUEST = 34,
+    TPC_REPORT = 35,
+    SUPPORTED_CHANNELS = 36,
+    CHANNEL_SWITCH_ANN = 37,
+    QUIET = 40,
+    IBSS_DFS = 41,
+    HT_CAPABILITY = 45,
+    HT_OPERATION = 61,
+    BSSCO_2040 = 72,
+    OVERLAPBSSSCANPARAM = 74,
+    EXT_CAPABILITY = 127,
+
+    ERP_INFO = 42,
+
+    EXTENDED_SUPPORTED_RATES = 50,
+
+    VENDOR_SPECIFIC_221 = 221,
+    WMM_IE = VENDOR_SPECIFIC_221,
+
+    WPS_IE = VENDOR_SPECIFIC_221,
+
+    WPA_IE = VENDOR_SPECIFIC_221,
+    RSN_IE = 48,
+    VS_IE = VENDOR_SPECIFIC_221,
+    WAPI_IE = 68,
+} MLAN_PACK_END IEEEtypes_ElementId_e;
+
+/** IEEE IE header */
+typedef MLAN_PACK_START struct _IEEEtypes_Header_t
+{
+    /** Element ID */
+    t_u8 element_id;
+    /** Length */
+    t_u8 len;
+} MLAN_PACK_END IEEEtypes_Header_t, *pIEEEtypes_Header_t;
+
+/** Vendor specific IE header */
+typedef MLAN_PACK_START struct _IEEEtypes_VendorHeader_t
+{
+    /** Element ID */
+    t_u8 element_id;
+    /** Length */
+    t_u8 len;
+    /** OUI */
+    t_u8 oui[3];
+    /** OUI type */
+    t_u8 oui_type;
+    /** OUI subtype */
+    t_u8 oui_subtype;
+    /** Version */
+    t_u8 version;
+} MLAN_PACK_END IEEEtypes_VendorHeader_t, *pIEEEtypes_VendorHeader_t;
+
+/** Vendor specific IE */
+typedef MLAN_PACK_START struct _IEEEtypes_VendorSpecific_t
+{
+    /** Vendor specific IE header */
+    IEEEtypes_VendorHeader_t vend_hdr;
+    /** IE Max - size of previous fields */
+    t_u8 data[IEEE_MAX_IE_SIZE - sizeof(IEEEtypes_VendorHeader_t)];
+}
+MLAN_PACK_END IEEEtypes_VendorSpecific_t, *pIEEEtypes_VendorSpecific_t;
+
+/** IEEE IE */
+typedef MLAN_PACK_START struct _IEEEtypes_Generic_t
+{
+    /** Generic IE header */
+    IEEEtypes_Header_t ieee_hdr;
+    /** IE Max - size of previous fields */
+    t_u8 data[IEEE_MAX_IE_SIZE - sizeof(IEEEtypes_Header_t)];
+}
+MLAN_PACK_END IEEEtypes_Generic_t, *pIEEEtypes_Generic_t;
+
+/** Capability information mask */
+#define CAPINFO_MASK    (~(MBIT(15) | MBIT(14) |            \
+                           MBIT(12) | MBIT(11) | MBIT(9)))
+
+/** Capability Bit Map*/
+#ifdef BIG_ENDIAN_SUPPORT
+typedef MLAN_PACK_START struct _IEEEtypes_CapInfo_t
+{
+    t_u8 rsrvd1:2;
+    t_u8 dsss_ofdm:1;
+    t_u8 rsvrd2:2;
+    t_u8 short_slot_time:1;
+    t_u8 rsrvd3:1;
+    t_u8 spectrum_mgmt:1;
+    t_u8 chan_agility:1;
+    t_u8 pbcc:1;
+    t_u8 short_preamble:1;
+    t_u8 privacy:1;
+    t_u8 cf_poll_rqst:1;
+    t_u8 cf_pollable:1;
+    t_u8 ibss:1;
+    t_u8 ess:1;
+} MLAN_PACK_END IEEEtypes_CapInfo_t, *pIEEEtypes_CapInfo_t;
+#else
+typedef MLAN_PACK_START struct _IEEEtypes_CapInfo_t
+{
+    /** Capability Bit Map : ESS */
+    t_u8 ess:1;
+    /** Capability Bit Map : IBSS */
+    t_u8 ibss:1;
+    /** Capability Bit Map : CF pollable */
+    t_u8 cf_pollable:1;
+    /** Capability Bit Map : CF poll request */
+    t_u8 cf_poll_rqst:1;
+    /** Capability Bit Map : privacy */
+    t_u8 privacy:1;
+    /** Capability Bit Map : Short preamble */
+    t_u8 short_preamble:1;
+    /** Capability Bit Map : PBCC */
+    t_u8 pbcc:1;
+    /** Capability Bit Map : Channel agility */
+    t_u8 chan_agility:1;
+    /** Capability Bit Map : Spectrum management */
+    t_u8 spectrum_mgmt:1;
+    /** Capability Bit Map : Reserved */
+    t_u8 rsrvd3:1;
+    /** Capability Bit Map : Short slot time */
+    t_u8 short_slot_time:1;
+    /** Capability Bit Map : APSD */
+    t_u8 Apsd:1;
+    /** Capability Bit Map : Reserved */
+    t_u8 rsvrd2:1;
+    /** Capability Bit Map : DSS OFDM */
+    t_u8 dsss_ofdm:1;
+    /** Capability Bit Map : Reserved */
+    t_u8 rsrvd1:2;
+} MLAN_PACK_END IEEEtypes_CapInfo_t, *pIEEEtypes_CapInfo_t;
+#endif /* BIG_ENDIAN_SUPPORT */
+
+/** IEEEtypes_CfParamSet_t */
+typedef MLAN_PACK_START struct _IEEEtypes_CfParamSet_t
+{
+    /** CF peremeter : Element ID */
+    t_u8 element_id;
+    /** CF peremeter : Length */
+    t_u8 len;
+    /** CF peremeter : Count */
+    t_u8 cfp_cnt;
+    /** CF peremeter : Period */
+    t_u8 cfp_period;
+    /** CF peremeter : Maximum duration */
+    t_u16 cfp_max_duration;
+    /** CF peremeter : Remaining duration */
+    t_u16 cfp_duration_remaining;
+} MLAN_PACK_END IEEEtypes_CfParamSet_t, *pIEEEtypes_CfParamSet_t;
+
+/** IEEEtypes_IbssParamSet_t */
+typedef MLAN_PACK_START struct _IEEEtypes_IbssParamSet_t
+{
+    /** Element ID */
+    t_u8 element_id;
+    /** Length */
+    t_u8 len;
+    /** ATIM window value in milliseconds */
+    t_u16 atim_window;
+} MLAN_PACK_END IEEEtypes_IbssParamSet_t, *pIEEEtypes_IbssParamSet_t;
+
+/** IEEEtypes_SsParamSet_t */
+typedef MLAN_PACK_START union _IEEEtypes_SsParamSet_t
+{
+    /** SS parameter : CF parameter set */
+    IEEEtypes_CfParamSet_t cf_param_set;
+    /** SS parameter : IBSS parameter set */
+    IEEEtypes_IbssParamSet_t ibss_param_set;
+} MLAN_PACK_END IEEEtypes_SsParamSet_t, *pIEEEtypes_SsParamSet_t;
+
+/** IEEEtypes_FhParamSet_t */
+typedef MLAN_PACK_START struct _IEEEtypes_FhParamSet_t
+{
+    /** FH parameter : Element ID */
+    t_u8 element_id;
+    /** FH parameter : Length */
+    t_u8 len;
+    /** FH parameter : Dwell time in milliseconds */
+    t_u16 dwell_time;
+    /** FH parameter : Hop set */
+    t_u8 hop_set;
+    /** FH parameter : Hop pattern */
+    t_u8 hop_pattern;
+    /** FH parameter : Hop index */
+    t_u8 hop_index;
+} MLAN_PACK_END IEEEtypes_FhParamSet_t, *pIEEEtypes_FhParamSet_t;
+
+/** IEEEtypes_DsParamSet_t */
+typedef MLAN_PACK_START struct _IEEEtypes_DsParamSet_t
+{
+    /** DS parameter : Element ID */
+    t_u8 element_id;
+    /** DS parameter : Length */
+    t_u8 len;
+    /** DS parameter : Current channel */
+    t_u8 current_chan;
+} MLAN_PACK_END IEEEtypes_DsParamSet_t, *pIEEEtypes_DsParamSet_t;
+
+/** IEEEtypes_PhyParamSet_t */
+typedef MLAN_PACK_START union _IEEEtypes_PhyParamSet_t
+{
+    /** FH parameter set */
+    IEEEtypes_FhParamSet_t fh_param_set;
+    /** DS parameter set */
+    IEEEtypes_DsParamSet_t ds_param_set;
+} MLAN_PACK_END IEEEtypes_PhyParamSet_t, *pIEEEtypes_PhyParamSet_t;
+
+/** IEEEtypes_ERPInfo_t */
+typedef MLAN_PACK_START struct _IEEEtypes_ERPInfo_t
+{
+    /** Element ID */
+    t_u8 element_id;
+    /** Length */
+    t_u8 len;
+    /** ERP flags */
+    t_u8 erp_flags;
+} MLAN_PACK_END IEEEtypes_ERPInfo_t, *pIEEEtypes_ERPInfo_t;
+
+/** IEEEtypes_AId_t */
+typedef t_u16 IEEEtypes_AId_t;
+
+/** IEEEtypes_StatusCode_t */
+typedef t_u16 IEEEtypes_StatusCode_t;
+
+/** IEEEtypes_AssocRsp_t */
+typedef MLAN_PACK_START struct _IEEEtypes_AssocRsp_t
+{
+    /** Capability information */
+    IEEEtypes_CapInfo_t capability;
+    /** Association response status code */
+    IEEEtypes_StatusCode_t status_code;
+    /** Association ID */
+    IEEEtypes_AId_t a_id;
+    /** IE data buffer */
+    t_u8 ie_buffer[1];
+} MLAN_PACK_END IEEEtypes_AssocRsp_t, *pIEEEtypes_AssocRsp_t;
+
+/** 802.11 supported rates */
+typedef t_u8 WLAN_802_11_RATES[WLAN_SUPPORTED_RATES];
+
+/** cipher TKIP */
+#define WPA_CIPHER_TKIP		2
+/** cipher AES */
+#define WPA_CIPHER_AES_CCM	4
+/** AKM: 8021x */
+#define RSN_AKM_8021X		1
+/** AKM: PSK */
+#define RSN_AKM_PSK     	2
+
+/** wpa_suite_t */
+typedef MLAN_PACK_START struct _wpa_suite_t
+{
+    /** OUI */
+    t_u8 oui[3];
+    /** tyep */
+    t_u8 type;
+} MLAN_PACK_END wpa_suite, wpa_suite_mcast_t;
+
+/** wpa_suite_ucast_t */
+typedef MLAN_PACK_START struct
+{
+    /* count */
+    t_u16 count;
+    /** wpa_suite list */
+    wpa_suite list[1];
+} MLAN_PACK_END wpa_suite_ucast_t, wpa_suite_auth_key_mgmt_t;
+
+/** IEEEtypes_Rsn_t */
+typedef MLAN_PACK_START struct _IEEEtypes_Rsn_t
+{
+    /** Rsn : Element ID */
+    t_u8 element_id;
+    /** Rsn : Length */
+    t_u8 len;
+    /** Rsn : version */
+    t_u16 version;
+    /** Rsn : group cipher */
+    wpa_suite_mcast_t group_cipher;
+    /** Rsn : pairwise cipher */
+    wpa_suite_ucast_t pairwise_cipher;
+} MLAN_PACK_END IEEEtypes_Rsn_t, *pIEEEtypes_Rsn_t;
+
+/** IEEEtypes_Wpa_t */
+typedef MLAN_PACK_START struct _IEEEtypes_Wpa_t
+{
+    /** Wpa : Element ID */
+    t_u8 element_id;
+    /** Wpa : Length */
+    t_u8 len;
+    /** Wpa : oui */
+    t_u8 oui[4];
+    /** version */
+    t_u16 version;
+    /** Wpa : group cipher */
+    wpa_suite_mcast_t group_cipher;
+    /** Wpa : pairwise cipher */
+    wpa_suite_ucast_t pairwise_cipher;
+} MLAN_PACK_END IEEEtypes_Wpa_t, *pIEEEtypes_Wpa_t;
+
+/** Maximum number of AC QOS queues available in the driver/firmware */
+#define MAX_AC_QUEUES 4
+
+/** Data structure of WMM QoS information */
+typedef MLAN_PACK_START struct _IEEEtypes_WmmQosInfo_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** QoS UAPSD */
+    t_u8 qos_uapsd:1;
+    /** Reserved */
+    t_u8 reserved:3;
+    /** Parameter set count */
+    t_u8 para_set_count:4;
+#else
+    /** Parameter set count */
+    t_u8 para_set_count:4;
+    /** Reserved */
+    t_u8 reserved:3;
+    /** QoS UAPSD */
+    t_u8 qos_uapsd:1;
+#endif                          /* BIG_ENDIAN_SUPPORT */
+} MLAN_PACK_END IEEEtypes_WmmQosInfo_t, *pIEEEtypes_WmmQosInfo_t;
+
+/** Data structure of WMM Aci/Aifsn */
+typedef MLAN_PACK_START struct _IEEEtypes_WmmAciAifsn_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** Reserved */
+    t_u8 reserved:1;
+    /** Aci */
+    t_u8 aci:2;
+    /** Acm */
+    t_u8 acm:1;
+    /** Aifsn */
+    t_u8 aifsn:4;
+#else
+    /** Aifsn */
+    t_u8 aifsn:4;
+    /** Acm */
+    t_u8 acm:1;
+    /** Aci */
+    t_u8 aci:2;
+    /** Reserved */
+    t_u8 reserved:1;
+#endif                          /* BIG_ENDIAN_SUPPORT */
+} MLAN_PACK_END IEEEtypes_WmmAciAifsn_t, *pIEEEtypes_WmmAciAifsn_t;
+
+/** Data structure of WMM ECW */
+typedef MLAN_PACK_START struct _IEEEtypes_WmmEcw_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** Maximum Ecw */
+    t_u8 ecw_max:4;
+    /** Minimum Ecw */
+    t_u8 ecw_min:4;
+#else
+    /** Minimum Ecw */
+    t_u8 ecw_min:4;
+    /** Maximum Ecw */
+    t_u8 ecw_max:4;
+#endif                          /* BIG_ENDIAN_SUPPORT */
+} MLAN_PACK_END IEEEtypes_WmmEcw_t, *pIEEEtypes_WmmEcw_t;
+
+/** Data structure of WMM AC parameters  */
+typedef MLAN_PACK_START struct _IEEEtypes_WmmAcParameters_t
+{
+    IEEEtypes_WmmAciAifsn_t aci_aifsn;       /**< AciAifSn */
+    IEEEtypes_WmmEcw_t ecw;                 /**< Ecw */
+    t_u16 tx_op_limit;                        /**< Tx op limit */
+} MLAN_PACK_END IEEEtypes_WmmAcParameters_t, *pIEEEtypes_WmmAcParameters_t;
+
+/** Data structure of WMM Info IE  */
+typedef MLAN_PACK_START struct _IEEEtypes_WmmInfo_t
+{
+
+    /**
+     * WMM Info IE - Vendor Specific Header:
+     *   element_id  [221/0xdd]
+     *   Len         [7]
+     *   Oui         [00:50:f2]
+     *   OuiType     [2]
+     *   OuiSubType  [0]
+     *   Version     [1]
+     */
+    IEEEtypes_VendorHeader_t vend_hdr;
+
+    /** QoS information */
+    IEEEtypes_WmmQosInfo_t qos_info;
+
+} MLAN_PACK_END IEEEtypes_WmmInfo_t, *pIEEEtypes_WmmInfo_t;
+
+/** Data structure of WMM parameter IE  */
+typedef MLAN_PACK_START struct _IEEEtypes_WmmParameter_t
+{
+    /**
+     * WMM Parameter IE - Vendor Specific Header:
+     *   element_id  [221/0xdd]
+     *   Len         [24]
+     *   Oui         [00:50:f2]
+     *   OuiType     [2]
+     *   OuiSubType  [1]
+     *   Version     [1]
+     */
+    IEEEtypes_VendorHeader_t vend_hdr;
+
+    /** QoS information */
+    IEEEtypes_WmmQosInfo_t qos_info;
+    /** Reserved */
+    t_u8 reserved;
+
+    /** AC Parameters Record WMM_AC_BE, WMM_AC_BK, WMM_AC_VI, WMM_AC_VO */
+    IEEEtypes_WmmAcParameters_t ac_params[MAX_AC_QUEUES];
+} MLAN_PACK_END IEEEtypes_WmmParameter_t, *pIEEEtypes_WmmParameter_t;
+
+/** Enumerator for TSPEC direction */
+typedef MLAN_PACK_START enum _IEEEtypes_WMM_TSPEC_TS_Info_Direction_e
+{
+
+    TSPEC_DIR_UPLINK = 0,
+    TSPEC_DIR_DOWNLINK = 1,
+    /* 2 is a reserved value */
+    TSPEC_DIR_BIDIRECT = 3,
+
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_TS_Info_Direction_e;
+
+/** Enumerator for TSPEC PSB */
+typedef MLAN_PACK_START enum _IEEEtypes_WMM_TSPEC_TS_Info_PSB_e
+{
+
+    TSPEC_PSB_LEGACY = 0,
+    TSPEC_PSB_TRIG = 1,
+
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_TS_Info_PSB_e;
+
+/** Enumerator for TSPEC Ack Policy */
+typedef MLAN_PACK_START enum _IEEEtypes_WMM_TSPEC_TS_Info_AckPolicy_e
+{
+
+    TSPEC_ACKPOLICY_NORMAL = 0,
+    TSPEC_ACKPOLICY_NOACK = 1,
+    /* 2 is reserved */
+    TSPEC_ACKPOLICY_BLOCKACK = 3,
+
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_TS_Info_AckPolicy_e;
+
+/** Enumerator for TSPEC Trafffice type */
+typedef MLAN_PACK_START enum _IEEEtypes_WMM_TSPEC_TS_TRAFFIC_TYPE_e
+{
+
+    TSPEC_TRAFFIC_APERIODIC = 0,
+    TSPEC_TRAFFIC_PERIODIC = 1,
+
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_TS_TRAFFIC_TYPE_e;
+
+/** Data structure of WMM TSPEC information */
+typedef MLAN_PACK_START struct
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    t_u8 Reserved17_23:7;       // ! Reserved
+    t_u8 Schedule:1;
+    IEEEtypes_WMM_TSPEC_TS_Info_AckPolicy_e AckPolicy:2;
+    t_u8 UserPri:3;             // ! 802.1d User Priority
+    IEEEtypes_WMM_TSPEC_TS_Info_PSB_e PowerSaveBehavior:1;      // !
+                                                                // Legacy/Trigg
+    t_u8 Aggregation:1;         // ! Reserved
+    t_u8 AccessPolicy2:1;       // !
+    t_u8 AccessPolicy1:1;       // !
+    IEEEtypes_WMM_TSPEC_TS_Info_Direction_e Direction:2;
+    t_u8 TID:4;                 // ! Unique identifier
+    IEEEtypes_WMM_TSPEC_TS_TRAFFIC_TYPE_e TrafficType:1;
+#else
+    IEEEtypes_WMM_TSPEC_TS_TRAFFIC_TYPE_e TrafficType:1;
+    t_u8 TID:4;                 // ! Unique identifier
+    IEEEtypes_WMM_TSPEC_TS_Info_Direction_e Direction:2;
+    t_u8 AccessPolicy1:1;       // !
+    t_u8 AccessPolicy2:1;       // !
+    t_u8 Aggregation:1;         // ! Reserved
+    IEEEtypes_WMM_TSPEC_TS_Info_PSB_e PowerSaveBehavior:1;      // !
+                                                                // Legacy/Trigg
+    t_u8 UserPri:3;             // ! 802.1d User Priority
+    IEEEtypes_WMM_TSPEC_TS_Info_AckPolicy_e AckPolicy:2;
+    t_u8 Schedule:1;
+    t_u8 Reserved17_23:7;       // ! Reserved
+#endif
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_TS_Info_t;
+
+/** Data structure of WMM TSPEC Nominal Size */
+typedef MLAN_PACK_START struct
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    t_u16 Fixed:1;              // ! 1: Fixed size given in Size, 0: Var, size
+                                // is nominal
+    t_u16 Size:15;              // ! Nominal size in octets
+#else
+    t_u16 Size:15;              // ! Nominal size in octets
+    t_u16 Fixed:1;              // ! 1: Fixed size given in Size, 0: Var, size
+                                // is nominal
+#endif
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_NomMSDUSize_t;
+
+/** Data structure of WMM TSPEC SBWA */
+typedef MLAN_PACK_START struct
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    t_u16 Whole:3;              // ! Whole portion
+    t_u16 Fractional:13;        // ! Fractional portion
+#else
+    t_u16 Fractional:13;        // ! Fractional portion
+    t_u16 Whole:3;              // ! Whole portion
+#endif
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_SBWA;
+
+/** Data structure of WMM TSPEC Body */
+typedef MLAN_PACK_START struct
+{
+
+    IEEEtypes_WMM_TSPEC_TS_Info_t TSInfo;
+    IEEEtypes_WMM_TSPEC_NomMSDUSize_t NomMSDUSize;
+    t_u16 MaximumMSDUSize;
+    t_u32 MinServiceInterval;
+    t_u32 MaxServiceInterval;
+    t_u32 InactivityInterval;
+    t_u32 SuspensionInterval;
+    t_u32 ServiceStartTime;
+    t_u32 MinimumDataRate;
+    t_u32 MeanDataRate;
+    t_u32 PeakDataRate;
+    t_u32 MaxBurstSize;
+    t_u32 DelayBound;
+    t_u32 MinPHYRate;
+    IEEEtypes_WMM_TSPEC_SBWA SurplusBWAllowance;
+    t_u16 MediumTime;
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_Body_t;
+
+/** Data structure of WMM TSPEC all elements */
+typedef MLAN_PACK_START struct
+{
+    t_u8 ElementId;
+    t_u8 Len;
+    t_u8 OuiType[4];            /* 00:50:f2:02 */
+    t_u8 OuiSubType;            /* 01 */
+    t_u8 Version;
+
+    IEEEtypes_WMM_TSPEC_Body_t TspecBody;
+
+} MLAN_PACK_END IEEEtypes_WMM_TSPEC_t;
+
+/** WMM Action Category values */
+typedef MLAN_PACK_START enum _IEEEtypes_ActionCategory_e
+{
+
+    IEEE_MGMT_ACTION_CATEGORY_SPECTRUM_MGMT = 0,
+    IEEE_MGMT_ACTION_CATEGORY_QOS = 1,
+    IEEE_MGMT_ACTION_CATEGORY_DLS = 2,
+    IEEE_MGMT_ACTION_CATEGORY_BLOCK_ACK = 3,
+    IEEE_MGMT_ACTION_CATEGORY_PUBLIC = 4,
+    IEEE_MGMT_ACTION_CATEGORY_RADIO_RSRC = 5,
+    IEEE_MGMT_ACTION_CATEGORY_FAST_BSS_TRANS = 6,
+    IEEE_MGMT_ACTION_CATEGORY_HT = 7,
+
+    IEEE_MGMT_ACTION_CATEGORY_WNM = 10,
+    IEEE_MGMT_ACTION_CATEGORY_UNPROTECT_WNM = 11,
+
+    IEEE_MGMT_ACTION_CATEGORY_WMM_TSPEC = 17
+} MLAN_PACK_END IEEEtypes_ActionCategory_e;
+
+/** WMM TSPEC operations */
+typedef MLAN_PACK_START enum _IEEEtypes_WMM_Tspec_Action_e
+{
+
+    TSPEC_ACTION_CODE_ADDTS_REQ = 0,
+    TSPEC_ACTION_CODE_ADDTS_RSP = 1,
+    TSPEC_ACTION_CODE_DELTS = 2,
+
+} MLAN_PACK_END IEEEtypes_WMM_Tspec_Action_e;
+
+/** WMM TSPEC Category Action Base */
+typedef MLAN_PACK_START struct
+{
+
+    IEEEtypes_ActionCategory_e category;
+    IEEEtypes_WMM_Tspec_Action_e action;
+    t_u8 dialogToken;
+
+} MLAN_PACK_END IEEEtypes_WMM_Tspec_Action_Base_Tspec_t;
+
+/** WMM TSPEC AddTS request structure */
+typedef MLAN_PACK_START struct
+{
+
+    IEEEtypes_WMM_Tspec_Action_Base_Tspec_t tspecAct;
+    t_u8 statusCode;
+    IEEEtypes_WMM_TSPEC_t tspecIE;
+
+    /* Place holder for additional elements after the TSPEC */
+    t_u8 subElem[256];
+
+} MLAN_PACK_END IEEEtypes_Action_WMM_AddTsReq_t;
+
+/** WMM TSPEC AddTS response structure */
+typedef MLAN_PACK_START struct
+{
+    IEEEtypes_WMM_Tspec_Action_Base_Tspec_t tspecAct;
+    t_u8 statusCode;
+    IEEEtypes_WMM_TSPEC_t tspecIE;
+
+    /* Place holder for additional elements after the TSPEC */
+    t_u8 subElem[256];
+
+} MLAN_PACK_END IEEEtypes_Action_WMM_AddTsRsp_t;
+
+/** WMM TSPEC DelTS structure */
+typedef MLAN_PACK_START struct
+{
+    IEEEtypes_WMM_Tspec_Action_Base_Tspec_t tspecAct;
+    t_u8 reasonCode;
+    IEEEtypes_WMM_TSPEC_t tspecIE;
+
+} MLAN_PACK_END IEEEtypes_Action_WMM_DelTs_t;
+
+/** union of WMM TSPEC structures */
+typedef MLAN_PACK_START union
+{
+    IEEEtypes_WMM_Tspec_Action_Base_Tspec_t tspecAct;
+
+    IEEEtypes_Action_WMM_AddTsReq_t addTsReq;
+    IEEEtypes_Action_WMM_AddTsRsp_t addTsRsp;
+    IEEEtypes_Action_WMM_DelTs_t delTs;
+
+} MLAN_PACK_END IEEEtypes_Action_WMMAC_t;
+
+/** union of WMM TSPEC & Action category */
+typedef MLAN_PACK_START union
+{
+    IEEEtypes_ActionCategory_e category;
+
+    IEEEtypes_Action_WMMAC_t wmmAc;
+
+} MLAN_PACK_END IEEEtypes_ActionFrame_t;
+
+/** Data structure for subband set */
+typedef MLAN_PACK_START struct _IEEEtypes_SubbandSet_t
+{
+    /** First channel */
+    t_u8 first_chan;
+    /** Number of channels */
+    t_u8 no_of_chan;
+    /** Maximum Tx power in dBm */
+    t_u8 max_tx_pwr;
+} MLAN_PACK_END IEEEtypes_SubbandSet_t, *pIEEEtypes_SubbandSet_t;
+
+#ifdef STA_SUPPORT
+/** Data structure for Country IE */
+typedef MLAN_PACK_START struct _IEEEtypes_CountryInfoSet_t
+{
+    /** Element ID */
+    t_u8 element_id;
+    /** Length */
+    t_u8 len;
+    /** Country code */
+    t_u8 country_code[COUNTRY_CODE_LEN];
+    /** Set of subbands */
+    IEEEtypes_SubbandSet_t sub_band[1];
+} MLAN_PACK_END IEEEtypes_CountryInfoSet_t, *pIEEEtypes_CountryInfoSet_t;
+
+/** Data structure for Country IE full set */
+typedef MLAN_PACK_START struct _IEEEtypes_CountryInfoFullSet_t
+{
+    /** Element ID */
+    t_u8 element_id;
+    /** Length */
+    t_u8 len;
+    /** Country code */
+    t_u8 country_code[COUNTRY_CODE_LEN];
+    /** Set of subbands */
+    IEEEtypes_SubbandSet_t sub_band[MRVDRV_MAX_SUBBAND_802_11D];
+} MLAN_PACK_END IEEEtypes_CountryInfoFullSet_t,
+    *pIEEEtypes_CountryInfoFullSet_t;
+
+#endif /* STA_SUPPORT */
+
+/** HT Capabilities Data */
+typedef struct MLAN_PACK_START _HTCap_t
+{
+    /** HT Capabilities Info field */
+    t_u16 ht_cap_info;
+    /** A-MPDU Parameters field */
+    t_u8 ampdu_param;
+    /** Supported MCS Set field */
+    t_u8 supported_mcs_set[16];
+    /** HT Extended Capabilities field */
+    t_u16 ht_ext_cap;
+    /** Transmit Beamforming Capabilities field */
+    t_u32 tx_bf_cap;
+    /** Antenna Selection Capability field */
+    t_u8 asel;
+} MLAN_PACK_END HTCap_t, *pHTCap_t;
+
+/** HT Information Data */
+typedef struct MLAN_PACK_START _HTInfo_t
+{
+    /** Primary channel */
+    t_u8 pri_chan;
+    /** Field 2 */
+    t_u8 field2;
+    /** Field 3 */
+    t_u16 field3;
+    /** Field 4 */
+    t_u16 field4;
+    /** Bitmap indicating MCSs supported by all HT STAs in the BSS */
+    t_u8 basic_mcs_set[16];
+} MLAN_PACK_END HTInfo_t, *pHTInfo_t;
+
+/** 20/40 BSS Coexistence Data */
+typedef struct MLAN_PACK_START _BSSCo2040_t
+{
+    /** 20/40 BSS Coexistence value */
+    t_u8 bss_co_2040_value;
+} MLAN_PACK_END BSSCo2040_t, *pBSSCo2040_t;
+
+/** Extended Capabilities Data */
+typedef struct MLAN_PACK_START _ExtCap_t
+{
+    /** Extended Capabilities value */
+    t_u8 ext_cap_value;
+} MLAN_PACK_END ExtCap_t, *pExtCap_t;
+
+/** Overlapping BSS Scan Parameters Data */
+typedef struct MLAN_PACK_START _OverlapBSSScanParam_t
+{
+    /** OBSS Scan Passive Dwell in milliseconds */
+    t_u16 obss_scan_passive_dwell;
+    /** OBSS Scan Active Dwell in milliseconds */
+    t_u16 obss_scan_active_dwell;
+    /** BSS Channel Width Trigger Scan Interval in seconds */
+    t_u16 bss_chan_width_trigger_scan_int;
+    /** OBSS Scan Passive Total Per Channel */
+    t_u16 obss_scan_passive_total;
+    /** OBSS Scan Active Total Per Channel */
+    t_u16 obss_scan_active_total;
+    /** BSS Width Channel Transition Delay Factor */
+    t_u16 bss_width_chan_trans_delay;
+    /** OBSS Scan Activity Threshold */
+    t_u16 obss_scan_active_threshold;
+} MLAN_PACK_END OBSSScanParam_t, *pOBSSScanParam_t;
+
+/** HT Capabilities IE */
+typedef MLAN_PACK_START struct _IEEEtypes_HTCap_t
+{
+    /** Generic IE header */
+    IEEEtypes_Header_t ieee_hdr;
+    /** HTCap struct */
+    HTCap_t ht_cap;
+} MLAN_PACK_END IEEEtypes_HTCap_t, *pIEEEtypes_HTCap_t;
+
+/** HT Information IE */
+typedef MLAN_PACK_START struct _IEEEtypes_HTInfo_t
+{
+    /** Generic IE header */
+    IEEEtypes_Header_t ieee_hdr;
+    /** HTInfo struct */
+    HTInfo_t ht_info;
+} MLAN_PACK_END IEEEtypes_HTInfo_t, *pIEEEtypes_HTInfo_t;
+
+/** 20/40 BSS Coexistence IE */
+typedef MLAN_PACK_START struct _IEEEtypes_2040BSSCo_t
+{
+    /** Generic IE header */
+    IEEEtypes_Header_t ieee_hdr;
+    /** BSSCo2040_t struct */
+    BSSCo2040_t bss_co_2040;
+} MLAN_PACK_END IEEEtypes_2040BSSCo_t, *pIEEEtypes_2040BSSCo_t;
+
+/** Extended Capabilities IE */
+typedef MLAN_PACK_START struct _IEEEtypes_ExtCap_t
+{
+    /** Generic IE header */
+    IEEEtypes_Header_t ieee_hdr;
+    /** ExtCap_t struct */
+    ExtCap_t ext_cap;
+} MLAN_PACK_END IEEEtypes_ExtCap_t, *pIEEEtypes_ExtCap_t;
+
+/** Overlapping BSS Scan Parameters IE */
+typedef MLAN_PACK_START struct _IEEEtypes_OverlapBSSScanParam_t
+{
+    /** Generic IE header */
+    IEEEtypes_Header_t ieee_hdr;
+    /** OBSSScanParam_t struct */
+    OBSSScanParam_t obss_scan_param;
+} MLAN_PACK_END IEEEtypes_OverlapBSSScanParam_t,
+    *pIEEEtypes_OverlapBSSScanParam_t;
+
+/** Maximum number of subbands in the IEEEtypes_SupportedChannels_t structure */
+#define WLAN_11H_MAX_SUBBANDS  5
+
+/** Maximum number of DFS channels configured in IEEEtypes_IBSS_DFS_t */
+#define WLAN_11H_MAX_IBSS_DFS_CHANNELS 25
+
+/**  IEEE Power Constraint element (7.3.2.15) */
+typedef MLAN_PACK_START struct
+{
+    t_u8 element_id;        /**< IEEE Element ID = 32 */
+    t_u8 len;               /**< Element length after id and len */
+    t_u8 local_constraint;  /**< Local power constraint applied to 11d chan info */
+} MLAN_PACK_END IEEEtypes_PowerConstraint_t;
+
+/**  IEEE Power Capability element (7.3.2.16) */
+typedef MLAN_PACK_START struct
+{
+    t_u8 element_id;                /**< IEEE Element ID = 33 */
+    t_u8 len;                       /**< Element length after id and len */
+    t_s8 min_tx_power_capability;   /**< Minimum Transmit power (dBm) */
+    t_s8 max_tx_power_capability;   /**< Maximum Transmit power (dBm) */
+} MLAN_PACK_END IEEEtypes_PowerCapability_t;
+
+/**  IEEE TPC Report element (7.3.2.18) */
+typedef MLAN_PACK_START struct
+{
+    t_u8 element_id;    /**< IEEE Element ID = 35 */
+    t_u8 len;           /**< Element length after id and len */
+    t_s8 tx_power;      /**< Max power used to transmit the TPC Report frame (dBm) */
+    t_s8 link_margin;   /**< Link margin when TPC Request received (dB) */
+} MLAN_PACK_END IEEEtypes_TPCReport_t;
+
+/*  IEEE Supported Channel sub-band description (7.3.2.19) */
+/**
+ *  Sub-band description used in the supported channels element.
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 start_chan;    /**< Starting channel in the subband */
+    t_u8 num_chans;     /**< Number of channels in the subband */
+
+} MLAN_PACK_END IEEEtypes_SupportChan_Subband_t;
+
+/*  IEEE Supported Channel element (7.3.2.19) */
+/**
+ *  Sent in association requests. Details the sub-bands and number
+ *    of channels supported in each subband
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 element_id;    /**< IEEE Element ID = 36 */
+    t_u8 len;           /**< Element length after id and len */
+
+    /** Configured sub-bands information in the element */
+    IEEEtypes_SupportChan_Subband_t subband[WLAN_11H_MAX_SUBBANDS];
+
+} MLAN_PACK_END IEEEtypes_SupportedChannels_t;
+
+/*  IEEE Channel Switch Announcement Element (7.3.2.20) */
+/**
+ *  Provided in beacons and probe responses.  Used to advertise when
+ *    and to which channel it is changing to.  Only starting STAs in
+ *    an IBSS and APs are allowed to originate a chan switch element.
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 element_id;            /**< IEEE Element ID = 37 */
+    t_u8 len;                   /**< Element length after id and len */
+    t_u8 chan_switch_mode;      /**< STA should not transmit any frames if 1 */
+    t_u8 new_channel_num;       /**< Channel # that AP/IBSS is moving to */
+    t_u8 chan_switch_count;     /**< # of TBTTs before channel switch */
+
+} MLAN_PACK_END IEEEtypes_ChanSwitchAnn_t;
+
+/*  IEEE Quiet Period Element (7.3.2.23) */
+/**
+ *  Provided in beacons and probe responses.  Indicates times during
+ *    which the STA should not be transmitting data.  Only starting STAs in
+ *    an IBSS and APs are allowed to originate a quiet element.
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 element_id;        /**< IEEE Element ID = 40 */
+    t_u8 len;               /**< Element length after id and len */
+    t_u8 quiet_count;       /**< Number of TBTTs until beacon with the quiet period */
+    t_u8 quiet_period;      /**< Regular quiet period, # of TBTTS between periods */
+    t_u16 quiet_duration;   /**< Duration of the quiet period in TUs */
+    t_u16 quiet_offset;     /**< Offset in TUs from the TBTT for the quiet period */
+
+} MLAN_PACK_END IEEEtypes_Quiet_t;
+
+/**
+***  @brief Map octet of the basic measurement report (7.3.2.22.1)
+**/
+typedef MLAN_PACK_START struct
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    t_u8 rsvd5_7:3;                /**< Reserved */
+    t_u8 unmeasured:1;             /**< Channel is unmeasured */
+    t_u8 radar:1;                  /**< Radar detected on channel */
+    t_u8 unidentified_sig:1;       /**< Unidentified signal found on channel */
+    t_u8 ofdm_preamble:1;          /**< OFDM preamble detected on channel */
+    t_u8 bss:1;                    /**< At least one valid MPDU received on channel */
+#else
+    t_u8 bss:1;                    /**< At least one valid MPDU received on channel */
+    t_u8 ofdm_preamble:1;          /**< OFDM preamble detected on channel */
+    t_u8 unidentified_sig:1;       /**< Unidentified signal found on channel */
+    t_u8 radar:1;                  /**< Radar detected on channel */
+    t_u8 unmeasured:1;             /**< Channel is unmeasured */
+    t_u8 rsvd5_7:3;                /**< Reserved */
+#endif                          /* BIG_ENDIAN_SUPPORT */
+
+} MLAN_PACK_END MeasRptBasicMap_t;
+
+/*  IEEE DFS Channel Map field (7.3.2.24) */
+/**
+ *  Used to list supported channels and provide a octet "map" field which
+ *    contains a basic measurement report for that channel in the
+ *    IEEEtypes_IBSS_DFS_t element
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 channel_number;        /**< Channel number */
+    MeasRptBasicMap_t rpt_map;  /**< Basic measurement report for the channel */
+
+} MLAN_PACK_END IEEEtypes_ChannelMap_t;
+
+/*  IEEE IBSS DFS Element (7.3.2.24) */
+/**
+ *  IBSS DFS element included in ad hoc beacons and probe responses.
+ *    Provides information regarding the IBSS DFS Owner as well as the
+ *    originating STAs supported channels and basic measurement results.
+ */
+typedef MLAN_PACK_START struct
+{
+    t_u8 element_id;                        /**< IEEE Element ID = 41 */
+    t_u8 len;                               /**< Element length after id and len */
+    t_u8 dfs_owner[MLAN_MAC_ADDR_LENGTH];   /**< DFS Owner STA Address */
+    t_u8 dfs_recovery_interval;             /**< DFS Recovery time in TBTTs */
+
+    /** Variable length map field, one Map entry for each supported channel */
+    IEEEtypes_ChannelMap_t channel_map[WLAN_11H_MAX_IBSS_DFS_CHANNELS];
+
+} MLAN_PACK_END IEEEtypes_IBSS_DFS_t;
+
+/* 802.11h BSS information kept for each BSSID received in scan results */
+/**
+ * IEEE BSS information needed from scan results for later processing in
+ *    join commands
+ */
+typedef struct
+{
+    t_u8 sensed_11h;  /**< Capability bit set or 11h IE found in this BSS */
+
+    IEEEtypes_PowerConstraint_t power_constraint; /**< Power Constraint IE */
+    IEEEtypes_PowerCapability_t power_capability; /**< Power Capability IE */
+    IEEEtypes_TPCReport_t tpc_report;             /**< TPC Report IE */
+    IEEEtypes_ChanSwitchAnn_t chan_switch_ann;    /**< Channel Switch Announcement IE */
+    IEEEtypes_Quiet_t quiet;                      /**< Quiet IE */
+    IEEEtypes_IBSS_DFS_t ibss_dfs;                /**< IBSS DFS Element IE */
+
+} wlan_11h_bss_info_t;
+
+#ifdef STA_SUPPORT
+/** Macro for maximum size of scan response buffer */
+#define MAX_SCAN_RSP_BUF (16 * 1024)
+
+/** Maximum number of channels that can be sent in user scan config */
+#define WLAN_USER_SCAN_CHAN_MAX             50
+
+/** Maximum length of SSID list */
+#define MRVDRV_MAX_SSID_LIST_LENGTH         10
+
+/** Scan all the channels in specified band */
+#define BAND_SPECIFIED    0x80
+
+/**
+ *  IOCTL SSID List sub-structure sent in wlan_ioctl_user_scan_cfg
+ *
+ *  Used to specify SSID specific filters as well as SSID pattern matching
+ *    filters for scan result processing in firmware.
+ */
+typedef MLAN_PACK_START struct _wlan_user_scan_ssid
+{
+    /** SSID */
+    t_u8 ssid[MLAN_MAX_SSID_LENGTH + 1];
+    /** Maximum length of SSID */
+    t_u8 max_len;
+} MLAN_PACK_END wlan_user_scan_ssid;
+
+/**
+ *  @brief IOCTL channel sub-structure sent in wlan_ioctl_user_scan_cfg
+ *
+ *  Multiple instances of this structure are included in the IOCTL command
+ *   to configure a instance of a scan on the specific channel.
+ */
+typedef MLAN_PACK_START struct _wlan_user_scan_chan
+{
+    /** Channel Number to scan */
+    t_u8 chan_number;
+    /** Radio type: 'B/G' Band = 0, 'A' Band = 1 */
+    t_u8 radio_type;
+    /** Scan type: Active = 1, Passive = 2 */
+    t_u8 scan_type;
+    /** Reserved */
+    t_u8 reserved;
+    /** Scan duration in milliseconds; if 0 default used */
+    t_u32 scan_time;
+} MLAN_PACK_END wlan_user_scan_chan;
+
+/**
+ *  Input structure to configure an immediate scan cmd to firmware
+ *
+ *  Specifies a number of parameters to be used in general for the scan
+ *    as well as a channel list (wlan_user_scan_chan) for each scan period
+ *    desired.
+ */
+typedef MLAN_PACK_START struct
+{
+    /**
+     *  Flag set to keep the previous scan table intact
+     *
+     *  If set, the scan results will accumulate, replacing any previous
+     *   matched entries for a BSS with the new scan data
+     */
+    t_u8 keep_previous_scan;
+    /**
+     *  BSS mode to be sent in the firmware command
+     *
+     *  Field can be used to restrict the types of networks returned in the
+     *    scan.  Valid settings are:
+     *
+     *   - MLAN_SCAN_MODE_BSS  (infrastructure)
+     *   - MLAN_SCAN_MODE_IBSS (adhoc)
+     *   - MLAN_SCAN_MODE_ANY  (unrestricted, adhoc and infrastructure)
+     */
+    t_u8 bss_mode;
+    /**
+     *  Configure the number of probe requests for active chan scans
+     */
+    t_u8 num_probes;
+    /**
+     *  @brief Reserved
+     */
+    t_u8 reserved;
+    /**
+     *  @brief BSSID filter sent in the firmware command to limit the results
+     */
+    t_u8 specific_bssid[MLAN_MAC_ADDR_LENGTH];
+    /**
+     *  SSID filter list used in the to limit the scan results
+     */
+    wlan_user_scan_ssid ssid_list[MRVDRV_MAX_SSID_LIST_LENGTH];
+    /**
+     *  Variable number (fixed maximum) of channels to scan up
+     */
+    wlan_user_scan_chan chan_list[WLAN_USER_SCAN_CHAN_MAX];
+} MLAN_PACK_END wlan_user_scan_cfg;
+
+/** Default scan interval in millisecond*/
+#define DEFAULT_BGSCAN_INTERVAL 30000
+
+/** action get all, except pps/uapsd config */
+#define BG_SCAN_ACT_GET		0x0000
+/** action set all, except pps/uapsd config */
+#define BG_SCAN_ACT_SET             0x0001
+/** action get pps/uapsd config */
+#define BG_SCAN_ACT_GET_PPS_UAPSD   0x0100
+/** action set pps/uapsd config */
+#define BG_SCAN_ACT_SET_PPS_UAPSD   0x0101
+/** action set all */
+#define BG_SCAN_ACT_SET_ALL         0xff01
+/** ssid match */
+#define BG_SCAN_SSID_MATCH			0x0001
+/** ssid match and RSSI exceeded */
+#define BG_SCAN_SSID_RSSI_MATCH		0x0004
+/** Maximum number of channels that can be sent in bg scan config */
+#define WLAN_BG_SCAN_CHAN_MAX       32
+
+/**
+ *  Input structure to configure bs scan cmd to firmware
+ */
+typedef MLAN_PACK_START struct
+{
+    /** action */
+    t_u16 action;
+    /** enable/disable */
+    t_u8 enable;
+    /**  BSS type:
+      *   MLAN_SCAN_MODE_BSS  (infrastructure)
+      *   MLAN_SCAN_MODE_IBSS (adhoc)
+      *   MLAN_SCAN_MODE_ANY  (unrestricted, adhoc and infrastructure)
+      */
+    t_u8 bss_type;
+    /** number of channel scanned during each scan */
+    t_u8 chan_per_scan;
+    /** interval between consecutive scan */
+    t_u32 scan_interval;
+    /** bit 0: ssid match bit 1: ssid match and SNR exceeded
+      *  bit 2: ssid match and RSSI exceeded
+      *  bit 31: wait for all channel scan to complete to report scan result
+      */
+    t_u32 report_condition;
+    /* Configure the number of probe requests for active chan scans */
+    t_u8 num_probes;
+    /** RSSI threshold */
+    t_u8 rssi_threshold;
+    /** SNR threshold */
+    t_u8 snr_threshold;
+    /** repeat count */
+    t_u16 repeat_count;
+    /** SSID filter list used in the to limit the scan results */
+    wlan_user_scan_ssid ssid_list[MRVDRV_MAX_SSID_LIST_LENGTH];
+    /** Variable number (fixed maximum) of channels to scan up */
+    wlan_user_scan_chan chan_list[WLAN_BG_SCAN_CHAN_MAX];
+} MLAN_PACK_END wlan_bgscan_cfg;
+#endif /* STA_SUPPORT */
+
+#ifdef PRAGMA_PACK
+#pragma pack(pop)
+#endif
+
+/** BSSDescriptor_t
+ *    Structure used to store information for beacon/probe response
+ */
+typedef struct _BSSDescriptor_t
+{
+    /** MAC address */
+    mlan_802_11_mac_addr mac_address;
+
+    /** SSID */
+    mlan_802_11_ssid ssid;
+
+    /** WEP encryption requirement */
+    t_u32 privacy;
+
+    /** Receive signal strength in dBm */
+    t_s32 rssi;
+
+    /** Channel */
+    t_u32 channel;
+
+    /** Freq */
+    t_u32 freq;
+
+    /** Beacon period */
+    t_u16 beacon_period;
+
+    /** ATIM window */
+    t_u32 atim_window;
+
+    /** ERP flags */
+    t_u8 erp_flags;
+
+    /** Type of network in use */
+    WLAN_802_11_NETWORK_TYPE network_type_use;
+
+    /** Network infrastructure mode */
+    t_u32 bss_mode;
+
+    /** Network supported rates */
+    WLAN_802_11_RATES supported_rates;
+
+    /** Supported data rates */
+    t_u8 data_rates[WLAN_SUPPORTED_RATES];
+
+    /** Network band.
+     * BAND_B(0x01): 'b' band
+     * BAND_G(0x02): 'g' band
+     * BAND_A(0X04): 'a' band
+     */
+    t_u16 bss_band;
+
+    /** TSF timestamp from the current firmware TSF */
+    t_u64 network_tsf;
+
+    /** TSF value included in the beacon/probe response */
+    t_u8 time_stamp[8];
+
+    /** PHY parameter set */
+    IEEEtypes_PhyParamSet_t phy_param_set;
+
+    /** SS parameter set */
+    IEEEtypes_SsParamSet_t ss_param_set;
+
+    /** Capability information */
+    IEEEtypes_CapInfo_t cap_info;
+
+    /** WMM IE */
+    IEEEtypes_WmmParameter_t wmm_ie;
+
+    /** 802.11h BSS information */
+    wlan_11h_bss_info_t wlan_11h_bss_info;
+
+    /** Indicate disabling 11n when associate with AP */
+    t_u8 disable_11n;
+    /** 802.11n BSS information */
+    /** HT Capabilities IE */
+    IEEEtypes_HTCap_t *pht_cap;
+    /** HT Capabilities Offset */
+    t_u16 ht_cap_offset;
+    /** HT Information IE */
+    IEEEtypes_HTInfo_t *pht_info;
+    /** HT Information Offset */
+    t_u16 ht_info_offset;
+    /** 20/40 BSS Coexistence IE */
+    IEEEtypes_2040BSSCo_t *pbss_co_2040;
+    /** 20/40 BSS Coexistence Offset */
+    t_u16 bss_co_2040_offset;
+    /** Extended Capabilities IE */
+    IEEEtypes_ExtCap_t *pext_cap;
+    /** Extended Capabilities Offset */
+    t_u16 ext_cap_offset;
+    /** Overlapping BSS Scan Parameters IE */
+    IEEEtypes_OverlapBSSScanParam_t *poverlap_bss_scan_param;
+    /** Overlapping BSS Scan Parameters Offset */
+    t_u16 overlap_bss_offset;
+
+#ifdef STA_SUPPORT
+    /** Country information set */
+    IEEEtypes_CountryInfoFullSet_t country_info;
+#endif                          /* STA_SUPPORT */
+
+    /** WPA IE */
+    IEEEtypes_VendorSpecific_t *pwpa_ie;
+    /** WPA IE offset in the beacon buffer */
+    t_u16 wpa_offset;
+    /** RSN IE */
+    IEEEtypes_Generic_t *prsn_ie;
+    /** RSN IE offset in the beacon buffer */
+    t_u16 rsn_offset;
+#ifdef STA_SUPPORT
+    /** WAPI IE */
+    IEEEtypes_Generic_t *pwapi_ie;
+    /** WAPI IE offset in the beacon buffer */
+    t_u16 wapi_offset;
+#endif
+
+    /** Pointer to the returned scan response */
+    t_u8 *pbeacon_buf;
+    /** Length of the stored scan response */
+    t_u32 beacon_buf_size;
+    /** Max allocated size for updated scan response */
+    t_u32 beacon_buf_size_max;
+
+} BSSDescriptor_t, *pBSSDescriptor_t;
+
+#endif /* !_MLAN_IEEE_H_ */
diff --git a/drivers/net/wireless/sd8797/mlinux/mlan_ioctl.h b/drivers/net/wireless/sd8797/mlinux/mlan_ioctl.h
new file mode 100644
index 000000000000..964de9197ffe
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/mlan_ioctl.h
@@ -0,0 +1,2981 @@
+/** @file mlan_ioctl.h
+ *
+ *  @brief This file declares the IOCTL data structures and APIs.
+ *
+ *  Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ *  This software file (the "File") is distributed by Marvell International
+ *  Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ *  (the "License").  You may use, redistribute and/or modify this File in
+ *  accordance with the terms and conditions of the License, a copy of which
+ *  is available by writing to the Free Software Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ *  worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *  THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ *  this warranty disclaimer.
+ */
+
+/******************************************************
+Change log:
+    11/07/2008: initial version
+******************************************************/
+
+#ifndef _MLAN_IOCTL_H_
+#define _MLAN_IOCTL_H_
+
+/** Enumeration for IOCTL request ID */
+enum _mlan_ioctl_req_id
+{
+    /* Scan Group */
+    MLAN_IOCTL_SCAN = 0x00010000,
+    MLAN_OID_SCAN_NORMAL,
+    MLAN_OID_SCAN_SPECIFIC_SSID,
+    MLAN_OID_SCAN_USER_CONFIG,
+    MLAN_OID_SCAN_CONFIG,
+    MLAN_OID_SCAN_GET_CURRENT_BSS,
+    MLAN_OID_SCAN_CANCEL,
+    MLAN_OID_SCAN_TABLE_FLUSH,
+    MLAN_OID_SCAN_BGSCAN_CONFIG,
+    /* BSS Configuration Group */
+    MLAN_IOCTL_BSS = 0x00020000,
+    MLAN_OID_BSS_START,
+    MLAN_OID_BSS_STOP,
+    MLAN_OID_BSS_MODE,
+    MLAN_OID_BSS_CHANNEL,
+    MLAN_OID_BSS_CHANNEL_LIST,
+    MLAN_OID_BSS_MAC_ADDR,
+    MLAN_OID_BSS_MULTICAST_LIST,
+    MLAN_OID_BSS_FIND_BSS,
+    MLAN_OID_IBSS_BCN_INTERVAL,
+    MLAN_OID_IBSS_ATIM_WINDOW,
+    MLAN_OID_IBSS_CHANNEL,
+#ifdef UAP_SUPPORT
+    MLAN_OID_UAP_BSS_CONFIG,
+    MLAN_OID_UAP_DEAUTH_STA,
+    MLAN_OID_UAP_BSS_RESET,
+#endif
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+    MLAN_OID_BSS_ROLE,
+#endif
+#ifdef WIFI_DIRECT_SUPPORT
+    MLAN_OID_WIFI_DIRECT_MODE,
+#endif
+
+    /* Radio Configuration Group */
+    MLAN_IOCTL_RADIO_CFG = 0x00030000,
+    MLAN_OID_RADIO_CTRL,
+    MLAN_OID_BAND_CFG,
+    MLAN_OID_ANT_CFG,
+#ifdef WIFI_DIRECT_SUPPORT
+    MLAN_OID_REMAIN_CHAN_CFG,
+#endif
+
+    /* SNMP MIB Group */
+    MLAN_IOCTL_SNMP_MIB = 0x00040000,
+    MLAN_OID_SNMP_MIB_RTS_THRESHOLD,
+    MLAN_OID_SNMP_MIB_FRAG_THRESHOLD,
+    MLAN_OID_SNMP_MIB_RETRY_COUNT,
+#if defined(UAP_SUPPORT)
+    MLAN_OID_SNMP_MIB_DOT11D,
+    MLAN_OID_SNMP_MIB_DOT11H,
+#endif
+    MLAN_OID_SNMP_MIB_DTIM_PERIOD,
+
+    /* Status Information Group */
+    MLAN_IOCTL_GET_INFO = 0x00050000,
+    MLAN_OID_GET_STATS,
+    MLAN_OID_GET_SIGNAL,
+    MLAN_OID_GET_FW_INFO,
+    MLAN_OID_GET_VER_EXT,
+    MLAN_OID_GET_BSS_INFO,
+    MLAN_OID_GET_DEBUG_INFO,
+#ifdef UAP_SUPPORT
+    MLAN_OID_UAP_STA_LIST,
+#endif
+
+    /* Security Configuration Group */
+    MLAN_IOCTL_SEC_CFG = 0x00060000,
+    MLAN_OID_SEC_CFG_AUTH_MODE,
+    MLAN_OID_SEC_CFG_ENCRYPT_MODE,
+    MLAN_OID_SEC_CFG_WPA_ENABLED,
+    MLAN_OID_SEC_CFG_ENCRYPT_KEY,
+    MLAN_OID_SEC_CFG_PASSPHRASE,
+    MLAN_OID_SEC_CFG_EWPA_ENABLED,
+    MLAN_OID_SEC_CFG_ESUPP_MODE,
+    MLAN_OID_SEC_CFG_WAPI_ENABLED,
+    MLAN_OID_SEC_CFG_PORT_CTRL_ENABLED,
+
+    /* Rate Group */
+    MLAN_IOCTL_RATE = 0x00070000,
+    MLAN_OID_RATE_CFG,
+    MLAN_OID_GET_DATA_RATE,
+    MLAN_OID_SUPPORTED_RATES,
+
+    /* Power Configuration Group */
+    MLAN_IOCTL_POWER_CFG = 0x00080000,
+    MLAN_OID_POWER_CFG,
+    MLAN_OID_POWER_CFG_EXT,
+
+    /* Power Management Configuration Group */
+    MLAN_IOCTL_PM_CFG = 0x00090000,
+    MLAN_OID_PM_CFG_IEEE_PS,
+    MLAN_OID_PM_CFG_HS_CFG,
+    MLAN_OID_PM_CFG_INACTIVITY_TO,
+    MLAN_OID_PM_CFG_DEEP_SLEEP,
+    MLAN_OID_PM_CFG_SLEEP_PD,
+    MLAN_OID_PM_CFG_PS_CFG,
+    MLAN_OID_PM_CFG_SLEEP_PARAMS,
+#ifdef UAP_SUPPORT
+    MLAN_OID_PM_CFG_PS_MODE,
+#endif /* UAP_SUPPORT */
+    MLAN_OID_PM_INFO,
+
+    /* WMM Configuration Group */
+    MLAN_IOCTL_WMM_CFG = 0x000A0000,
+    MLAN_OID_WMM_CFG_ENABLE,
+    MLAN_OID_WMM_CFG_QOS,
+    MLAN_OID_WMM_CFG_ADDTS,
+    MLAN_OID_WMM_CFG_DELTS,
+    MLAN_OID_WMM_CFG_QUEUE_CONFIG,
+    MLAN_OID_WMM_CFG_QUEUE_STATS,
+    MLAN_OID_WMM_CFG_QUEUE_STATUS,
+    MLAN_OID_WMM_CFG_TS_STATUS,
+
+    /* WPS Configuration Group */
+    MLAN_IOCTL_WPS_CFG = 0x000B0000,
+    MLAN_OID_WPS_CFG_SESSION,
+
+    /* 802.11n Configuration Group */
+    MLAN_IOCTL_11N_CFG = 0x000C0000,
+    MLAN_OID_11N_CFG_TX,
+    MLAN_OID_11N_HTCAP_CFG,
+    MLAN_OID_11N_CFG_ADDBA_REJECT,
+    MLAN_OID_11N_CFG_AGGR_PRIO_TBL,
+    MLAN_OID_11N_CFG_ADDBA_PARAM,
+    MLAN_OID_11N_CFG_MAX_TX_BUF_SIZE,
+    MLAN_OID_11N_CFG_AMSDU_AGGR_CTRL,
+    MLAN_OID_11N_CFG_SUPPORTED_MCS_SET,
+    MLAN_OID_11N_CFG_TX_BF_CAP,
+    MLAN_OID_11N_CFG_TX_BF_CFG,
+    MLAN_OID_11N_CFG_STREAM_CFG,
+
+    /* 802.11d Configuration Group */
+    MLAN_IOCTL_11D_CFG = 0x000D0000,
+#ifdef STA_SUPPORT
+    MLAN_OID_11D_CFG_ENABLE,
+    MLAN_OID_11D_CLR_CHAN_TABLE,
+#endif /* STA_SUPPORT */
+    MLAN_OID_11D_DOMAIN_INFO,
+
+    /* Register Memory Access Group */
+    MLAN_IOCTL_REG_MEM = 0x000E0000,
+    MLAN_OID_REG_RW,
+    MLAN_OID_EEPROM_RD,
+    MLAN_OID_MEM_RW,
+
+    /* Multi-Radio Configuration Group */
+    MLAN_IOCTL_MFR_CFG = 0x00100000,
+
+    /* 802.11h Configuration Group */
+    MLAN_IOCTL_11H_CFG = 0x00110000,
+    MLAN_OID_11H_CHANNEL_CHECK,
+    MLAN_OID_11H_LOCAL_POWER_CONSTRAINT,
+#if defined(DFS_TESTING_SUPPORT)
+    MLAN_OID_11H_DFS_TESTING,
+#endif
+
+    /* Miscellaneous Configuration Group */
+    MLAN_IOCTL_MISC_CFG = 0x00200000,
+    MLAN_OID_MISC_GEN_IE,
+    MLAN_OID_MISC_REGION,
+    MLAN_OID_MISC_WARM_RESET,
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+    MLAN_OID_MISC_SDIO_MPA_CTRL,
+#endif
+    MLAN_OID_MISC_HOST_CMD,
+    MLAN_OID_MISC_SYS_CLOCK,
+    MLAN_OID_MISC_SOFT_RESET,
+    MLAN_OID_MISC_WWS,
+    MLAN_OID_MISC_INIT_SHUTDOWN,
+    MLAN_OID_MISC_CUSTOM_IE,
+    MLAN_OID_MISC_TX_DATAPAUSE,
+    MLAN_OID_MISC_IP_ADDR,
+    MLAN_OID_MISC_MAC_CONTROL,
+    MLAN_OID_MISC_MEF_CFG,
+    MLAN_OID_MISC_CFP_CODE,
+    MLAN_OID_MISC_COUNTRY_CODE,
+    MLAN_OID_MISC_THERMAL,
+    MLAN_OID_MISC_RX_MGMT_IND,
+    MLAN_OID_MISC_SUBSCRIBE_EVENT,
+#ifdef DEBUG_LEVEL1
+    MLAN_OID_MISC_DRVDBG,
+#endif
+    MLAN_OID_MISC_OTP_USER_DATA,
+};
+
+/** Sub command size */
+#define MLAN_SUB_COMMAND_SIZE	4
+
+/** Enumeration for the action of IOCTL request */
+enum _mlan_act_ioctl
+{
+    MLAN_ACT_SET = 1,
+    MLAN_ACT_GET,
+    MLAN_ACT_CANCEL
+};
+
+/** Enumeration for generic enable/disable */
+enum _mlan_act_generic
+{
+    MLAN_ACT_DISABLE = 0,
+    MLAN_ACT_ENABLE = 1
+};
+
+/** Enumeration for scan mode */
+enum _mlan_scan_mode
+{
+    MLAN_SCAN_MODE_UNCHANGED = 0,
+    MLAN_SCAN_MODE_BSS,
+    MLAN_SCAN_MODE_IBSS,
+    MLAN_SCAN_MODE_ANY
+};
+
+/** Enumeration for scan type */
+enum _mlan_scan_type
+{
+    MLAN_SCAN_TYPE_UNCHANGED = 0,
+    MLAN_SCAN_TYPE_ACTIVE,
+    MLAN_SCAN_TYPE_PASSIVE
+};
+
+/** Max number of supported rates */
+#define MLAN_SUPPORTED_RATES	32
+
+/** RSSI scan */
+#define SCAN_RSSI(RSSI)			(0x100 - ((t_u8)(RSSI)))
+
+/** Max passive scan time for each channel in milliseconds */
+#define MRVDRV_MAX_PASSIVE_SCAN_CHAN_TIME   2000
+
+/** Max active scan time for each channel in milliseconds  */
+#define MRVDRV_MAX_ACTIVE_SCAN_CHAN_TIME    500
+
+/** Maximum number of probes to send on each channel */
+#define MAX_PROBES      4
+
+/** Default number of probes to send on each channel */
+#define DEFAULT_PROBES  4
+
+/**
+ *  @brief Sub-structure passed in wlan_ioctl_get_scan_table_entry for each BSS
+ *
+ *  Fixed field information returned for the scan response in the IOCTL
+ *    response.
+ */
+typedef struct _wlan_get_scan_table_fixed
+{
+    /** BSSID of this network */
+    t_u8 bssid[MLAN_MAC_ADDR_LENGTH];
+    /** Channel this beacon/probe response was detected */
+    t_u8 channel;
+    /** RSSI for the received packet */
+    t_u8 rssi;
+    /** TSF value in microseconds from the firmware at packet reception */
+    t_u64 network_tsf;
+} wlan_get_scan_table_fixed;
+
+/** mlan_802_11_ssid data structure */
+typedef struct _mlan_802_11_ssid
+{
+    /** SSID Length */
+    t_u32 ssid_len;
+    /** SSID information field */
+    t_u8 ssid[MLAN_MAX_SSID_LENGTH];
+} mlan_802_11_ssid, *pmlan_802_11_ssid;
+
+/**
+ *  Sructure to retrieve the scan table
+ */
+typedef struct
+{
+    /**
+     *  - Zero based scan entry to start retrieval in command request
+     *  - Number of scans entries returned in command response
+     */
+    t_u32 scan_number;
+    /**
+     * Buffer marker for multiple wlan_ioctl_get_scan_table_entry structures.
+     *   Each struct is padded to the nearest 32 bit boundary.
+     */
+    t_u8 scan_table_entry_buf[1];
+} wlan_ioctl_get_scan_table_info;
+
+/**
+ *  Structure passed in the wlan_ioctl_get_scan_table_info for each
+ *    BSS returned in the WLAN_GET_SCAN_RESP IOCTL
+ */
+typedef struct _wlan_ioctl_get_scan_table_entry
+{
+    /**
+     *  Fixed field length included in the response.
+     *
+     *  Length value is included so future fixed fields can be added to the
+     *   response without breaking backwards compatibility.  Use the length
+     *   to find the offset for the bssInfoLength field, not a sizeof() calc.
+     */
+    t_u32 fixed_field_length;
+
+    /**
+     *  Length of the BSS Information (probe resp or beacon) that
+     *    follows after the fixed_field_length
+     */
+    t_u32 bss_info_length;
+
+    /**
+     *  Always present, fixed length data fields for the BSS
+     */
+    wlan_get_scan_table_fixed fixed_fields;
+
+    /*
+     *  Probe response or beacon scanned for the BSS.
+     *
+     *  Field layout:
+     *   - TSF              8 octets
+     *   - Beacon Interval  2 octets
+     *   - Capability Info  2 octets
+     *
+     *   - IEEE Infomation Elements; variable number & length per 802.11 spec
+     */
+    /* t_u8 bss_info_buffer[0]; */
+} wlan_ioctl_get_scan_table_entry;
+
+/** Type definition of mlan_scan_time_params */
+typedef struct _mlan_scan_time_params
+{
+    /** Scan channel time for specific scan in milliseconds */
+    t_u32 specific_scan_time;
+    /** Scan channel time for active scan in milliseconds */
+    t_u32 active_scan_time;
+    /** Scan channel time for passive scan in milliseconds */
+    t_u32 passive_scan_time;
+} mlan_scan_time_params, *pmlan_scan_time_params;
+
+/** Type definition of mlan_user_scan */
+typedef struct _mlan_user_scan
+{
+    /** Length of scan_cfg_buf */
+    t_u32 scan_cfg_len;
+    /** Buffer of scan config */
+    t_u8 scan_cfg_buf[1];
+} mlan_user_scan, *pmlan_user_scan;
+
+/** Type definition of mlan_scan_req */
+typedef struct _mlan_scan_req
+{
+    /** BSS mode for scanning */
+    t_u32 scan_mode;
+    /** Scan type */
+    t_u32 scan_type;
+    /** SSID */
+    mlan_802_11_ssid scan_ssid;
+    /** Scan time parameters */
+    mlan_scan_time_params scan_time;
+    /** Scan config parameters in user scan */
+    mlan_user_scan user_scan;
+} mlan_scan_req, *pmlan_scan_req;
+
+/** Type defnition of mlan_scan_resp */
+typedef struct _mlan_scan_resp
+{
+    /** Number of scan result */
+    t_u32 num_in_scan_table;
+    /** Scan table */
+    t_u8 *pscan_table;
+    /* Age in seconds */
+    t_u32 age_in_secs;
+} mlan_scan_resp, *pmlan_scan_resp;
+
+/** Type definition of mlan_scan_cfg */
+typedef struct _mlan_scan_cfg
+{
+    /** Scan type */
+    t_u32 scan_type;
+    /** BSS mode for scanning */
+    t_u32 scan_mode;
+    /** Scan probe */
+    t_u32 scan_probe;
+    /** Scan time parameters */
+    mlan_scan_time_params scan_time;
+    /** Extended Scan */
+    t_u32 ext_scan;
+} mlan_scan_cfg, *pmlan_scan_cfg;
+
+/** Type defnition of mlan_ds_scan for MLAN_IOCTL_SCAN */
+typedef struct _mlan_ds_scan
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Scan request/response */
+    union
+    {
+        /** Scan request */
+        mlan_scan_req scan_req;
+        /** Scan response */
+        mlan_scan_resp scan_resp;
+        /** Scan config parameters in user scan */
+        mlan_user_scan user_scan;
+        /** Scan config parameters */
+        mlan_scan_cfg scan_cfg;
+    } param;
+} mlan_ds_scan, *pmlan_ds_scan;
+
+/*-----------------------------------------------------------------*/
+/** BSS Configuration Group */
+/*-----------------------------------------------------------------*/
+/** Enumeration for BSS mode */
+enum _mlan_bss_mode
+{
+    MLAN_BSS_MODE_INFRA = 1,
+    MLAN_BSS_MODE_IBSS,
+    MLAN_BSS_MODE_AUTO
+};
+
+/** Maximum key length */
+#define MLAN_MAX_KEY_LENGTH             32
+
+/** max Wmm AC queues */
+#define MAX_AC_QUEUES                   4
+
+/** Maximum atim window in milliseconds */
+#define MLAN_MAX_ATIM_WINDOW		50
+
+/** Minimum beacon interval */
+#define MLAN_MIN_BEACON_INTERVAL        20
+/** Maximum beacon interval */
+#define MLAN_MAX_BEACON_INTERVAL        1000
+/** Default beacon interval */
+#define MLAN_BEACON_INTERVAL            100
+
+/** Receive all packets */
+#define MLAN_PROMISC_MODE     	1
+/** Receive multicast packets in multicast list */
+#define MLAN_MULTICAST_MODE		2
+/** Receive all multicast packets */
+#define	MLAN_ALL_MULTI_MODE		4
+
+/** Maximum size of multicast list */
+#define MLAN_MAX_MULTICAST_LIST_SIZE	32
+
+/** mlan_multicast_list data structure for MLAN_OID_BSS_MULTICAST_LIST */
+typedef struct _mlan_multicast_list
+{
+    /** Multicast mode */
+    t_u32 mode;
+    /** Number of multicast addresses in the list */
+    t_u32 num_multicast_addr;
+    /** Multicast address list */
+    mlan_802_11_mac_addr mac_list[MLAN_MAX_MULTICAST_LIST_SIZE];
+} mlan_multicast_list, *pmlan_multicast_list;
+
+/** Max channel */
+#define MLAN_MAX_CHANNEL    165
+
+/** Maximum number of channels in table */
+#define MLAN_MAX_CHANNEL_NUM	128
+
+/** Channel/frequence for MLAN_OID_BSS_CHANNEL */
+typedef struct _chan_freq
+{
+    /** Channel Number */
+    t_u32 channel;
+    /** Frequency of this Channel */
+    t_u32 freq;
+} chan_freq;
+
+/** mlan_chan_list data structure for MLAN_OID_BSS_CHANNEL_LIST */
+typedef struct _mlan_chan_list
+{
+    /** Number of channel */
+    t_u32 num_of_chan;
+    /** Channel-Frequency table */
+    chan_freq cf[MLAN_MAX_CHANNEL_NUM];
+} mlan_chan_list;
+
+/** mlan_ssid_bssid  data structure for MLAN_OID_BSS_START and MLAN_OID_BSS_FIND_BSS */
+typedef struct _mlan_ssid_bssid
+{
+    /** SSID */
+    mlan_802_11_ssid ssid;
+    /** BSSID */
+    mlan_802_11_mac_addr bssid;
+    /** index in BSSID list, start from 1 */
+    t_u32 idx;
+} mlan_ssid_bssid;
+
+#ifdef UAP_SUPPORT
+/** Maximum packet forward control value */
+#define MAX_PKT_FWD_CTRL 15
+/** Maximum BEACON period */
+#define MAX_BEACON_PERIOD 4000
+/** Minimum BEACON period */
+#define MIN_BEACON_PERIOD 50
+/** Maximum DTIM period */
+#define MAX_DTIM_PERIOD 100
+/** Minimum DTIM period */
+#define MIN_DTIM_PERIOD 1
+/** Maximum TX Power Limit */
+#define MAX_TX_POWER    20
+/** Minimum TX Power Limit */
+#define MIN_TX_POWER    0
+/** MAX station count */
+#define MAX_STA_COUNT   10
+/** Maximum RTS threshold */
+#define MAX_RTS_THRESHOLD   2347
+/** Maximum fragmentation threshold */
+#define MAX_FRAG_THRESHOLD 2346
+/** Minimum fragmentation threshold */
+#define MIN_FRAG_THRESHOLD 256
+/** data rate 54 M */
+#define DATA_RATE_54M   108
+/** antenna A */
+#define ANTENNA_MODE_A      0
+/** antenna B */
+#define ANTENNA_MODE_B      1
+/** transmit antenna */
+#define TX_ANTENNA          1
+/** receive antenna */
+#define RX_ANTENNA          0
+/** Maximum stage out time */
+#define MAX_STAGE_OUT_TIME  864000
+/** Minimum stage out time */
+#define MIN_STAGE_OUT_TIME  300
+/** Maximum Retry Limit */
+#define MAX_RETRY_LIMIT         14
+
+/** Maximum group key timer in seconds */
+#define MAX_GRP_TIMER           86400
+
+/** Maximum value of 4 byte configuration */
+#define MAX_VALID_DWORD         0x7FFFFFFF      /* (1 << 31) - 1 */
+
+/** Band config ACS mode */
+#define BAND_CONFIG_ACS_MODE    0x40
+/** Band config manual */
+#define BAND_CONFIG_MANUAL      0x00
+
+/** Maximum data rates */
+#define MAX_DATA_RATES          14
+
+/** auto data rate */
+#define DATA_RATE_AUTO       0
+
+/**filter mode: disable */
+#define MAC_FILTER_MODE_DISABLE         0
+/**filter mode: block mac address */
+#define MAC_FILTER_MODE_ALLOW_MAC       1
+/**filter mode: block mac address */
+#define MAC_FILTER_MODE_BLOCK_MAC       2
+/** Maximum mac filter num */
+#define MAX_MAC_FILTER_NUM           16
+
+/* Bitmap for protocol to use */
+/** No security */
+#define PROTOCOL_NO_SECURITY        0x01
+/** Static WEP */
+#define PROTOCOL_STATIC_WEP         0x02
+/** WPA */
+#define PROTOCOL_WPA                0x08
+/** WPA2 */
+#define PROTOCOL_WPA2               0x20
+/** WP2 Mixed */
+#define PROTOCOL_WPA2_MIXED         0x28
+/** EAP */
+#define PROTOCOL_EAP                0x40
+/** WAPI */
+#define PROTOCOL_WAPI               0x80
+
+/** Key_mgmt_psk */
+#define KEY_MGMT_NONE   0x04
+/** Key_mgmt_none */
+#define KEY_MGMT_PSK    0x02
+/** Key_mgmt_eap  */
+#define KEY_MGMT_EAP    0x01
+
+/** TKIP */
+#define CIPHER_TKIP                 0x04
+/** AES CCMP */
+#define CIPHER_AES_CCMP             0x08
+
+/** Valid cipher bitmap */
+#define VALID_CIPHER_BITMAP         0x0c
+
+/** Channel List Entry */
+typedef struct _channel_list
+{
+    /** Channel Number */
+    t_u8 chan_number;
+    /** Band Config */
+    t_u8 band_config_type;
+} scan_chan_list;
+
+/** mac_filter data structure */
+typedef struct _mac_filter
+{
+    /** mac filter mode */
+    t_u16 filter_mode;
+    /** mac adress count */
+    t_u16 mac_count;
+    /** mac address list */
+    mlan_802_11_mac_addr mac_list[MAX_MAC_FILTER_NUM];
+} mac_filter;
+
+/** wpa parameter */
+typedef struct _wpa_param
+{
+    /** Pairwise cipher WPA */
+    t_u8 pairwise_cipher_wpa;
+    /** Pairwise cipher WPA2 */
+    t_u8 pairwise_cipher_wpa2;
+    /** group cipher */
+    t_u8 group_cipher;
+    /** RSN replay protection */
+    t_u8 rsn_protection;
+    /** passphrase length */
+    t_u32 length;
+    /** passphrase */
+    t_u8 passphrase[64];
+    /**group key rekey time in seconds */
+    t_u32 gk_rekey_time;
+} wpa_param;
+
+/** wep key */
+typedef struct _wep_key
+{
+    /** key index 0-3 */
+    t_u8 key_index;
+    /** is default */
+    t_u8 is_default;
+    /** length */
+    t_u16 length;
+    /** key data */
+    t_u8 key[26];
+} wep_key;
+
+/** wep param */
+typedef struct _wep_param
+{
+    /** key 0 */
+    wep_key key0;
+    /** key 1 */
+    wep_key key1;
+    /** key 2 */
+    wep_key key2;
+    /** key 3 */
+    wep_key key3;
+} wep_param;
+
+/** Data structure of WMM QoS information */
+typedef struct _wmm_qos_info_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** QoS UAPSD */
+    t_u8 qos_uapsd:1;
+    /** Reserved */
+    t_u8 reserved:3;
+    /** Parameter set count */
+    t_u8 para_set_count:4;
+#else
+    /** Parameter set count */
+    t_u8 para_set_count:4;
+    /** Reserved */
+    t_u8 reserved:3;
+    /** QoS UAPSD */
+    t_u8 qos_uapsd:1;
+#endif                          /* BIG_ENDIAN_SUPPORT */
+} wmm_qos_info_t, *pwmm_qos_info_t;
+
+/** Data structure of WMM ECW */
+typedef struct _wmm_ecw_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** Maximum Ecw */
+    t_u8 ecw_max:4;
+    /** Minimum Ecw */
+    t_u8 ecw_min:4;
+#else
+    /** Minimum Ecw */
+    t_u8 ecw_min:4;
+    /** Maximum Ecw */
+    t_u8 ecw_max:4;
+#endif                          /* BIG_ENDIAN_SUPPORT */
+} wmm_ecw_t, *pwmm_ecw_t;
+
+/** Data structure of WMM Aci/Aifsn */
+typedef struct _wmm_aci_aifsn_t
+{
+#ifdef BIG_ENDIAN_SUPPORT
+    /** Reserved */
+    t_u8 reserved:1;
+    /** Aci */
+    t_u8 aci:2;
+    /** Acm */
+    t_u8 acm:1;
+    /** Aifsn */
+    t_u8 aifsn:4;
+#else
+    /** Aifsn */
+    t_u8 aifsn:4;
+    /** Acm */
+    t_u8 acm:1;
+    /** Aci */
+    t_u8 aci:2;
+    /** Reserved */
+    t_u8 reserved:1;
+#endif                          /* BIG_ENDIAN_SUPPORT */
+} wmm_aci_aifsn_t, *pwmm_aci_aifsn_t;
+
+/** Data structure of WMM AC parameters  */
+typedef struct _wmm_ac_parameters_t
+{
+    wmm_aci_aifsn_t aci_aifsn;       /**< AciAifSn */
+    wmm_ecw_t ecw;                 /**< Ecw */
+    t_u16 tx_op_limit;                        /**< Tx op limit */
+} wmm_ac_parameters_t, *pwmm_ac_parameters_t;
+
+/** Data structure of WMM parameter IE  */
+typedef struct _wmm_parameter_t
+{
+    /** OuiType:  00:50:f2:02 */
+    t_u8 ouitype[4];
+    /** Oui subtype: 01 */
+    t_u8 ouisubtype;
+    /** version: 01 */
+    t_u8 version;
+    /** QoS information */
+    t_u8 qos_info;
+    /** Reserved */
+    t_u8 reserved;
+    /** AC Parameters Record WMM_AC_BE, WMM_AC_BK, WMM_AC_VI, WMM_AC_VO */
+    wmm_ac_parameters_t ac_params[MAX_AC_QUEUES];
+} wmm_parameter_t, *pwmm_parameter_t;
+
+/** mlan_bss_param
+ * Note: For each entry you must enter an invalid value
+ * in the MOAL function woal_set_sys_config_invalid_data().
+ * Otherwise for a valid data an unwanted TLV will be
+ * added to that command.
+ */
+typedef struct _mlan_uap_bss_param
+{
+    /** AP mac addr */
+    mlan_802_11_mac_addr mac_addr;
+    /** SSID */
+    mlan_802_11_ssid ssid;
+    /** Broadcast ssid control */
+    t_u8 bcast_ssid_ctl;
+    /** Radio control: on/off */
+    t_u8 radio_ctl;
+    /** dtim period */
+    t_u8 dtim_period;
+    /** beacon period */
+    t_u16 beacon_period;
+    /** rates */
+    t_u8 rates[MAX_DATA_RATES];
+    /** Tx data rate */
+    t_u16 tx_data_rate;
+    /** multicast/broadcast data rate */
+    t_u16 mcbc_data_rate;
+    /** Tx power level in dBm */
+    t_u8 tx_power_level;
+    /** Tx antenna */
+    t_u8 tx_antenna;
+    /** Rx antenna */
+    t_u8 rx_antenna;
+    /** packet forward control */
+    t_u8 pkt_forward_ctl;
+    /** max station count */
+    t_u16 max_sta_count;
+    /** mac filter */
+    mac_filter filter;
+    /** station ageout timer in unit of 100ms  */
+    t_u32 sta_ageout_timer;
+    /** PS station ageout timer in unit of 100ms  */
+    t_u32 ps_sta_ageout_timer;
+    /** RTS threshold */
+    t_u16 rts_threshold;
+    /** fragmentation threshold */
+    t_u16 frag_threshold;
+    /**  retry_limit */
+    t_u16 retry_limit;
+    /**  pairwise update timeout in milliseconds */
+    t_u32 pairwise_update_timeout;
+    /** pairwise handshake retries */
+    t_u32 pwk_retries;
+    /**  groupwise update timeout in milliseconds */
+    t_u32 groupwise_update_timeout;
+    /** groupwise handshake retries */
+    t_u32 gwk_retries;
+    /** preamble type */
+    t_u8 preamble_type;
+    /** band cfg */
+    t_u8 band_cfg;
+    /** channel */
+    t_u8 channel;
+    /** auth mode */
+    t_u16 auth_mode;
+    /** encryption protocol */
+    t_u16 protocol;
+    /** key managment type */
+    t_u16 key_mgmt;
+    /** wep param */
+    wep_param wep_cfg;
+    /** wpa param */
+    wpa_param wpa_cfg;
+    /** Mgmt IE passthru mask */
+    t_u32 mgmt_ie_passthru_mask;
+    /*
+     * 11n HT Cap  HTCap_t  ht_cap
+     */
+    /** HT Capabilities Info field */
+    t_u16 ht_cap_info;
+    /** A-MPDU Parameters field */
+    t_u8 ampdu_param;
+    /** Supported MCS Set field */
+    t_u8 supported_mcs_set[16];
+    /** HT Extended Capabilities field */
+    t_u16 ht_ext_cap;
+    /** Transmit Beamforming Capabilities field */
+    t_u32 tx_bf_cap;
+    /** Antenna Selection Capability field */
+    t_u8 asel;
+    /** Enable 2040 Coex */
+    t_u8 enable_2040coex;
+    /** key management operation */
+    t_u16 key_mgmt_operation;
+    /** BSS status */
+    t_u16 bss_status;
+#ifdef WIFI_DIRECT_SUPPORT
+    /* pre shared key */
+    t_u8 psk[MLAN_MAX_KEY_LENGTH];
+#endif                          /* WIFI_DIRECT_SUPPORT */
+    /** Number of channels in scan_channel_list */
+    t_u32 num_of_chan;
+    /** scan channel list in ACS mode */
+    scan_chan_list chan_list[MLAN_MAX_CHANNEL];
+    /** Wmm parameters */
+    wmm_parameter_t wmm_para;
+} mlan_uap_bss_param;
+
+/** mlan_deauth_param */
+typedef struct _mlan_deauth_param
+{
+    /** STA mac addr */
+    t_u8 mac_addr[MLAN_MAC_ADDR_LENGTH];
+    /** deauth reason */
+    t_u16 reason_code;
+} mlan_deauth_param;
+#endif
+
+#ifdef WIFI_DIRECT_SUPPORT
+/** mode: disable wifi direct */
+#define WIFI_DIRECT_MODE_DISABLE		0
+/** mode: listen */
+#define WIFI_DIRECT_MODE_LISTEN			1
+/** mode: GO */
+#define WIFI_DIRECT_MODE_GO		        2
+/** mode: client */
+#define WIFI_DIRECT_MODE_CLIENT			3
+/** mode: find */
+#define WIFI_DIRECT_MODE_FIND			4
+/** mode: stop find */
+#define WIFI_DIRECT_MODE_STOP_FIND		5
+#endif
+
+/** Type definition of mlan_ds_bss for MLAN_IOCTL_BSS */
+typedef struct _mlan_ds_bss
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** BSS parameter */
+    union
+    {
+        /** SSID-BSSID for MLAN_OID_BSS_START */
+        mlan_ssid_bssid ssid_bssid;
+        /** BSSID for MLAN_OID_BSS_STOP */
+        mlan_802_11_mac_addr bssid;
+        /** BSS mode for MLAN_OID_BSS_MODE */
+        t_u32 bss_mode;
+        /** BSS channel/frequency for MLAN_OID_BSS_CHANNEL */
+        chan_freq bss_chan;
+        /** BSS channel list for MLAN_OID_BSS_CHANNEL_LIST */
+        mlan_chan_list chanlist;
+        /** MAC address for MLAN_OID_BSS_MAC_ADDR */
+        mlan_802_11_mac_addr mac_addr;
+        /** Multicast list for MLAN_OID_BSS_MULTICAST_LIST */
+        mlan_multicast_list multicast_list;
+        /** Beacon interval for MLAN_OID_IBSS_BCN_INTERVAL */
+        t_u32 bcn_interval;
+        /** ATIM window for MLAN_OID_IBSS_ATIM_WINDOW */
+        t_u32 atim_window;
+#ifdef UAP_SUPPORT
+        /** BSS param for AP mode */
+        mlan_uap_bss_param bss_config;
+        /** deauth param for MLAN_OID_UAP_DEAUTH_STA */
+        mlan_deauth_param deauth_param;
+#endif
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+        /** BSS role */
+        t_u8 bss_role;
+#endif
+#ifdef WIFI_DIRECT_SUPPORT
+        t_u16 wfd_mode;
+#endif
+    } param;
+} mlan_ds_bss, *pmlan_ds_bss;
+
+/*-----------------------------------------------------------------*/
+/** Radio Control Group */
+/*-----------------------------------------------------------------*/
+/** Enumeration for band */
+enum _mlan_band_def
+{
+    BAND_B = 1,
+    BAND_G = 2,
+    BAND_A = 4,
+    BAND_GN = 8,
+    BAND_AN = 16,
+};
+
+/** NO secondary channel */
+#define NO_SEC_CHANNEL               0
+/** secondary channel is above primary channel */
+#define SEC_CHANNEL_ABOVE            1
+/** secondary channel is below primary channel */
+#define SEC_CHANNEL_BELOW            3
+/** Channel bandwidth */
+#define CHANNEL_BW_20MHZ             0
+#define CHANNEL_BW_40MHZ_ABOVE       1
+#define CHANNEL_BW_40MHZ_BELOW       3
+
+/** Type definition of mlan_ds_band_cfg for MLAN_OID_BAND_CFG */
+typedef struct _mlan_ds_band_cfg
+{
+    /** Infra band */
+    t_u32 config_bands;
+    /** Ad-hoc start band */
+    t_u32 adhoc_start_band;
+    /** Ad-hoc start channel */
+    t_u32 adhoc_channel;
+    /** Ad-hoc channel bandwidth */
+    t_u32 sec_chan_offset;
+    /** fw supported band */
+    t_u32 fw_bands;
+} mlan_ds_band_cfg;
+
+/** Type definition of mlan_ds_ant_cfg for MLAN_OID_ANT_CFG */
+typedef struct _mlan_ds_ant_cfg
+{
+    /** Tx antenna mode */
+    t_u32 tx_antenna;
+    /** Rx antenna mode */
+    t_u32 rx_antenna;
+} mlan_ds_ant_cfg, *pmlan_ds_ant_cfg;
+
+#ifdef WIFI_DIRECT_SUPPORT
+/** Type definition of mlan_ds_remain_chan for MLAN_OID_REMAIN_CHAN_CFG */
+typedef struct _mlan_ds_remain_chan
+{
+    /** remove flag */
+    t_u16 remove;
+    /** status */
+    t_u8 status;
+    /** Band cfg */
+    t_u8 bandcfg;
+    /** channel */
+    t_u8 channel;
+    /** remain time: Unit ms*/
+    t_u32 remain_period;
+} mlan_ds_remain_chan, *pmlan_ds_remain_chan;
+#endif
+
+/** Type definition of mlan_ds_radio_cfg for MLAN_IOCTL_RADIO_CFG */
+typedef struct _mlan_ds_radio_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Radio control parameter */
+    union
+    {
+        /** Radio on/off for MLAN_OID_RADIO_CTRL */
+        t_u32 radio_on_off;
+        /** Band info for MLAN_OID_BAND_CFG */
+        mlan_ds_band_cfg band_cfg;
+        /** Antenna info for MLAN_OID_ANT_CFG */
+        mlan_ds_ant_cfg ant_cfg;
+        /** Antenna info for MLAN_OID_ANT_CFG */
+        t_u32 antenna;
+#ifdef WIFI_DIRECT_SUPPORT
+        /** remain on channel for MLAN_OID_REMAIN_CHAN_CFG */
+        mlan_ds_remain_chan remain_chan;
+#endif
+    } param;
+} mlan_ds_radio_cfg, *pmlan_ds_radio_cfg;
+
+/*-----------------------------------------------------------------*/
+/** SNMP MIB Group */
+/*-----------------------------------------------------------------*/
+/** Type definition of mlan_ds_snmp_mib for MLAN_IOCTL_SNMP_MIB */
+typedef struct _mlan_ds_snmp_mib
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** SNMP MIB parameter */
+    union
+    {
+        /** RTS threshold for MLAN_OID_SNMP_MIB_RTS_THRESHOLD */
+        t_u32 rts_threshold;
+        /** Fragment threshold for MLAN_OID_SNMP_MIB_FRAG_THRESHOLD */
+        t_u32 frag_threshold;
+        /** Retry count for MLAN_OID_SNMP_MIB_RETRY_COUNT */
+        t_u32 retry_count;
+#if defined(UAP_SUPPORT)
+        /** OID value for MLAN_OID_SNMP_MIB_DOT11D/H */
+        t_u32 oid_value;
+#endif
+        /** DTIM period for MLAN_OID_SNMP_MIB_DTIM_PERIOD */
+        t_u32 dtim_period;
+    } param;
+} mlan_ds_snmp_mib, *pmlan_ds_snmp_mib;
+
+/*-----------------------------------------------------------------*/
+/** Status Information Group */
+/*-----------------------------------------------------------------*/
+/** Enumeration for ad-hoc status */
+enum _mlan_adhoc_status
+{
+    ADHOC_IDLE,
+    ADHOC_STARTED,
+    ADHOC_JOINED,
+    ADHOC_COALESCED, ADHOC_STARTING
+};
+
+/** Type definition of mlan_ds_get_stats for MLAN_OID_GET_STATS */
+typedef struct _mlan_ds_get_stats
+{
+    /** Statistics counter */
+    /** Multicast transmitted frame count */
+    t_u32 mcast_tx_frame;
+    /** Failure count */
+    t_u32 failed;
+    /** Retry count */
+    t_u32 retry;
+    /** Multi entry count */
+    t_u32 multi_retry;
+    /** Duplicate frame count */
+    t_u32 frame_dup;
+    /** RTS success count */
+    t_u32 rts_success;
+    /** RTS failure count */
+    t_u32 rts_failure;
+    /** Ack failure count */
+    t_u32 ack_failure;
+    /** Rx fragmentation count */
+    t_u32 rx_frag;
+    /** Multicast Tx frame count */
+    t_u32 mcast_rx_frame;
+    /** FCS error count */
+    t_u32 fcs_error;
+    /** Tx frame count */
+    t_u32 tx_frame;
+    /** WEP ICV error count */
+    t_u32 wep_icv_error[4];
+} mlan_ds_get_stats, *pmlan_ds_get_stats;
+
+/** Type definition of mlan_ds_uap_stats for MLAN_OID_GET_STATS */
+typedef struct _mlan_ds_uap_stats
+{
+    /** tkip mic failures */
+    t_u32 tkip_mic_failures;
+    /** ccmp decrypt errors */
+    t_u32 ccmp_decrypt_errors;
+    /** wep undecryptable count */
+    t_u32 wep_undecryptable_count;
+    /** wep icv error count */
+    t_u32 wep_icv_error_count;
+    /** decrypt failure count */
+    t_u32 decrypt_failure_count;
+    /** dot11 multicast tx count */
+    t_u32 mcast_tx_count;
+    /** dot11 failed count */
+    t_u32 failed_count;
+    /** dot11 retry count */
+    t_u32 retry_count;
+    /** dot11 multi retry count */
+    t_u32 multi_retry_count;
+    /** dot11 frame duplicate count */
+    t_u32 frame_dup_count;
+    /** dot11 rts success count */
+    t_u32 rts_success_count;
+    /** dot11 rts failure count */
+    t_u32 rts_failure_count;
+    /** dot11 ack failure count */
+    t_u32 ack_failure_count;
+    /** dot11 rx ragment count */
+    t_u32 rx_fragment_count;
+    /** dot11 mcast rx frame count */
+    t_u32 mcast_rx_frame_count;
+    /** dot11 fcs error count */
+    t_u32 fcs_error_count;
+    /** dot11 tx frame count */
+    t_u32 tx_frame_count;
+    /** dot11 rsna tkip cm invoked */
+    t_u32 rsna_tkip_cm_invoked;
+    /** dot11 rsna 4way handshake failures */
+    t_u32 rsna_4way_hshk_failures;
+} mlan_ds_uap_stats, *pmlan_ds_uap_stats;
+
+/** Mask of last beacon RSSI */
+#define BCN_RSSI_LAST_MASK              0x00000001
+/** Mask of average beacon RSSI */
+#define BCN_RSSI_AVG_MASK               0x00000002
+/** Mask of last data RSSI */
+#define DATA_RSSI_LAST_MASK             0x00000004
+/** Mask of average data RSSI */
+#define DATA_RSSI_AVG_MASK              0x00000008
+/** Mask of last beacon SNR */
+#define BCN_SNR_LAST_MASK               0x00000010
+/** Mask of average beacon SNR */
+#define BCN_SNR_AVG_MASK                0x00000020
+/** Mask of last data SNR */
+#define DATA_SNR_LAST_MASK              0x00000040
+/** Mask of average data SNR */
+#define DATA_SNR_AVG_MASK               0x00000080
+/** Mask of last beacon NF */
+#define BCN_NF_LAST_MASK                0x00000100
+/** Mask of average beacon NF */
+#define BCN_NF_AVG_MASK                 0x00000200
+/** Mask of last data NF */
+#define DATA_NF_LAST_MASK               0x00000400
+/** Mask of average data NF */
+#define DATA_NF_AVG_MASK                0x00000800
+/** Mask of all RSSI_INFO */
+#define ALL_RSSI_INFO_MASK              0x00000fff
+
+/** Type definition of mlan_ds_get_signal for MLAN_OID_GET_SIGNAL */
+typedef struct _mlan_ds_get_signal
+{
+    /** Selector of get operation */
+    /*
+     * Bit0:  Last Beacon RSSI,  Bit1:  Average Beacon RSSI,
+     * Bit2:  Last Data RSSI,    Bit3:  Average Data RSSI,
+     * Bit4:  Last Beacon SNR,   Bit5:  Average Beacon SNR,
+     * Bit6:  Last Data SNR,     Bit7:  Average Data SNR,
+     * Bit8:  Last Beacon NF,    Bit9:  Average Beacon NF,
+     * Bit10: Last Data NF,      Bit11: Average Data NF
+     */
+    t_u16 selector;
+
+    /** RSSI */
+    /** RSSI of last beacon */
+    t_s16 bcn_rssi_last;
+    /** RSSI of beacon average */
+    t_s16 bcn_rssi_avg;
+    /** RSSI of last data packet */
+    t_s16 data_rssi_last;
+    /** RSSI of data packet average */
+    t_s16 data_rssi_avg;
+
+    /** SNR */
+    /** SNR of last beacon */
+    t_s16 bcn_snr_last;
+    /** SNR of beacon average */
+    t_s16 bcn_snr_avg;
+    /** SNR of last data packet */
+    t_s16 data_snr_last;
+    /** SNR of data packet average */
+    t_s16 data_snr_avg;
+
+    /** NF */
+    /** NF of last beacon */
+    t_s16 bcn_nf_last;
+    /** NF of beacon average */
+    t_s16 bcn_nf_avg;
+    /** NF of last data packet */
+    t_s16 data_nf_last;
+    /** NF of data packet average */
+    t_s16 data_nf_avg;
+} mlan_ds_get_signal, *pmlan_ds_get_signal;
+
+/** mlan_fw_info data structure for MLAN_OID_GET_FW_INFO */
+typedef struct _mlan_fw_info
+{
+    /** Firmware version */
+    t_u32 fw_ver;
+    /** MAC address */
+    mlan_802_11_mac_addr mac_addr;
+    /** Device support for MIMO abstraction of MCSs */
+    t_u8 hw_dev_mcs_support;
+        /** fw supported band */
+    t_u8 fw_bands;
+} mlan_fw_info, *pmlan_fw_info;
+
+/** Version string buffer length */
+#define MLAN_MAX_VER_STR_LEN    128
+
+/** mlan_ver_ext data structure for MLAN_OID_GET_VER_EXT */
+typedef struct _mlan_ver_ext
+{
+    /** Selected version string */
+    t_u32 version_str_sel;
+    /** Version string */
+    char version_str[MLAN_MAX_VER_STR_LEN];
+} mlan_ver_ext, *pmlan_ver_ext;
+
+/** mlan_bss_info data structure for MLAN_OID_GET_BSS_INFO */
+typedef struct _mlan_bss_info
+{
+    /** BSS mode */
+    t_u32 bss_mode;
+    /** SSID */
+    mlan_802_11_ssid ssid;
+    /** Table index */
+    t_u32 scan_table_idx;
+    /** Channel */
+    t_u32 bss_chan;
+    /** Band */
+    t_u8 bss_band;
+    /** Region code */
+    t_u32 region_code;
+    /** Connection status */
+    t_u32 media_connected;
+    /** Radio on */
+    t_u32 radio_on;
+    /** Max power level in dBm */
+    t_u32 max_power_level;
+    /** Min power level in dBm */
+    t_u32 min_power_level;
+    /** Adhoc state */
+    t_u32 adhoc_state;
+    /** NF of last beacon */
+    t_s32 bcn_nf_last;
+    /** wep status */
+    t_u32 wep_status;
+     /** Host Sleep configured flag */
+    t_u32 is_hs_configured;
+    /** Deep Sleep flag */
+    t_u32 is_deep_sleep;
+    /** BSSID */
+    mlan_802_11_mac_addr bssid;
+#ifdef STA_SUPPORT
+    /** Capability Info */
+    t_u16 capability_info;
+    /** Beacon Interval */
+    t_u16 beacon_interval;
+    /** Listen Interval */
+    t_u16 listen_interval;
+    /** Association Id  */
+    t_u16 assoc_id;
+    /** AP/Peer supported rates */
+    t_u8 peer_supp_rates[MLAN_SUPPORTED_RATES];
+#endif                          /* STA_SUPPORT */
+} mlan_bss_info, *pmlan_bss_info;
+
+/** MAXIMUM number of TID */
+#define MAX_NUM_TID     8
+
+/** Max RX Win size */
+#define MAX_RX_WINSIZE 		64
+
+/** rx_reorder_tbl */
+typedef struct
+{
+    /** TID */
+    t_u16 tid;
+    /** TA */
+    t_u8 ta[MLAN_MAC_ADDR_LENGTH];
+    /** Start window */
+    t_u32 start_win;
+    /** Window size */
+    t_u32 win_size;
+    /** amsdu flag */
+    t_u8 amsdu;
+    /** buffer status */
+    t_u32 buffer[MAX_RX_WINSIZE];
+} rx_reorder_tbl;
+
+/** tx_ba_stream_tbl */
+typedef struct
+{
+    /** TID */
+    t_u16 tid;
+    /** RA */
+    t_u8 ra[MLAN_MAC_ADDR_LENGTH];
+    /** amsdu flag */
+    t_u8 amsdu;
+} tx_ba_stream_tbl;
+
+/** Debug command number */
+#define DBG_CMD_NUM	5
+
+/** mlan_debug_info data structure for MLAN_OID_GET_DEBUG_INFO */
+typedef struct _mlan_debug_info
+{
+    /* WMM AC_BK count */
+    t_u32 wmm_ac_bk;
+    /* WMM AC_BE count */
+    t_u32 wmm_ac_be;
+    /* WMM AC_VI count */
+    t_u32 wmm_ac_vi;
+    /* WMM AC_VO count */
+    t_u32 wmm_ac_vo;
+    /** Corresponds to max_tx_buf_size member of mlan_adapter*/
+    t_u32 max_tx_buf_size;
+     /** Corresponds to tx_buf_size member of mlan_adapter*/
+    t_u32 tx_buf_size;
+    /** Corresponds to curr_tx_buf_size member of mlan_adapter*/
+    t_u32 curr_tx_buf_size;
+    /** Tx table num */
+    t_u32 tx_tbl_num;
+    /** Tx ba stream table */
+    tx_ba_stream_tbl tx_tbl[MLAN_MAX_TX_BASTREAM_SUPPORTED];
+    /** Rx table num */
+    t_u32 rx_tbl_num;
+    /** Rx reorder table*/
+    rx_reorder_tbl rx_tbl[MLAN_MAX_RX_BASTREAM_SUPPORTED];
+    /** Corresponds to ps_mode member of mlan_adapter */
+    t_u16 ps_mode;
+    /** Corresponds to ps_state member of mlan_adapter */
+    t_u32 ps_state;
+#ifdef STA_SUPPORT
+    /** Corresponds to is_deep_sleep member of mlan_adapter */
+    t_u8 is_deep_sleep;
+#endif /** STA_SUPPORT */
+    /** Corresponds to pm_wakeup_card_req member of mlan_adapter */
+    t_u8 pm_wakeup_card_req;
+    /** Corresponds to pm_wakeup_fw_try member of mlan_adapter */
+    t_u32 pm_wakeup_fw_try;
+    /** Corresponds to is_hs_configured member of mlan_adapter */
+    t_u8 is_hs_configured;
+    /** Corresponds to hs_activated member of mlan_adapter */
+    t_u8 hs_activated;
+    /** Corresponds to pps_uapsd_mode member of mlan_adapter */
+    t_u16 pps_uapsd_mode;
+    /** Corresponds to sleep_period.period member of mlan_adapter */
+    t_u16 sleep_pd;
+    /** Corresponds to wmm_qosinfo member of mlan_private */
+    t_u8 qos_cfg;
+    /** Corresponds to tx_lock_flag member of mlan_adapter */
+    t_u8 tx_lock_flag;
+    /** Corresponds to port_open member of mlan_private */
+    t_u8 port_open;
+    /** Corresponds to scan_processing member of mlan_adapter */
+    t_u32 scan_processing;
+    /** Number of host to card command failures */
+    t_u32 num_cmd_host_to_card_failure;
+    /** Number of host to card sleep confirm failures */
+    t_u32 num_cmd_sleep_cfm_host_to_card_failure;
+    /** Number of host to card Tx failures */
+    t_u32 num_tx_host_to_card_failure;
+    /** Number of card to host command/event failures */
+    t_u32 num_cmdevt_card_to_host_failure;
+    /** Number of card to host Rx failures */
+    t_u32 num_rx_card_to_host_failure;
+    /** Number of interrupt read failures */
+    t_u32 num_int_read_failure;
+    /** Last interrupt status */
+    t_u32 last_int_status;
+    /** Number of deauthentication events */
+    t_u32 num_event_deauth;
+    /** Number of disassosiation events */
+    t_u32 num_event_disassoc;
+    /** Number of link lost events */
+    t_u32 num_event_link_lost;
+    /** Number of deauthentication commands */
+    t_u32 num_cmd_deauth;
+    /** Number of association comamnd successes */
+    t_u32 num_cmd_assoc_success;
+    /** Number of association command failures */
+    t_u32 num_cmd_assoc_failure;
+    /** Number of Tx timeouts */
+    t_u32 num_tx_timeout;
+    /** Number of command timeouts */
+    t_u32 num_cmd_timeout;
+    /** Timeout command ID */
+    t_u16 timeout_cmd_id;
+    /** Timeout command action */
+    t_u16 timeout_cmd_act;
+    /** List of last command IDs */
+    t_u16 last_cmd_id[DBG_CMD_NUM];
+    /** List of last command actions */
+    t_u16 last_cmd_act[DBG_CMD_NUM];
+    /** Last command index */
+    t_u16 last_cmd_index;
+    /** List of last command response IDs */
+    t_u16 last_cmd_resp_id[DBG_CMD_NUM];
+    /** Last command response index */
+    t_u16 last_cmd_resp_index;
+    /** List of last events */
+    t_u16 last_event[DBG_CMD_NUM];
+    /** Last event index */
+    t_u16 last_event_index;
+
+    /** Corresponds to data_sent member of mlan_adapter */
+    t_u8 data_sent;
+    /** Corresponds to cmd_sent member of mlan_adapter */
+    t_u8 cmd_sent;
+    /** SDIO multiple port read bitmap */
+    t_u32 mp_rd_bitmap;
+    /** SDIO multiple port write bitmap */
+    t_u32 mp_wr_bitmap;
+    /** Current available port for read */
+    t_u8 curr_rd_port;
+    /** Current available port for write */
+    t_u8 curr_wr_port;
+    /** Corresponds to cmdresp_received member of mlan_adapter */
+    t_u8 cmd_resp_received;
+    /** Corresponds to event_received member of mlan_adapter */
+    t_u8 event_received;
+    /**  pendig tx pkts */
+    t_u32 tx_pkts_queued;
+#ifdef UAP_SUPPORT
+    /**  pending bridge pkts */
+    t_u16 num_bridge_pkts;
+    /**  dropped pkts */
+    t_u32 num_drop_pkts;
+#endif
+} mlan_debug_info, *pmlan_debug_info;
+
+#ifdef UAP_SUPPORT
+/** Maximum number of clients supported by AP */
+#define MAX_NUM_CLIENTS         MAX_STA_COUNT
+
+/** station info */
+typedef struct _sta_info
+{
+    /** STA MAC address */
+    t_u8 mac_address[MLAN_MAC_ADDR_LENGTH];
+    /** Power mfg status */
+    t_u8 power_mfg_status;
+    /** RSSI */
+    t_s8 rssi;
+} sta_info;
+
+/** mlan_ds_sta_list structure for MLAN_OID_UAP_STA_LIST */
+typedef struct _mlan_ds_sta_list
+{
+    /** station count */
+    t_u16 sta_count;
+    /** station list */
+    sta_info info[MAX_NUM_CLIENTS];
+} mlan_ds_sta_list, *pmlan_ds_sta_list;
+#endif
+
+/** Type definition of mlan_ds_get_info for MLAN_IOCTL_GET_INFO */
+typedef struct _mlan_ds_get_info
+{
+    /** Sub-command */
+    t_u32 sub_command;
+
+    /** Status information parameter */
+    union
+    {
+        /** Signal information for MLAN_OID_GET_SIGNAL */
+        mlan_ds_get_signal signal;
+        /** Statistics information for MLAN_OID_GET_STATS */
+        mlan_ds_get_stats stats;
+        /** Firmware information for MLAN_OID_GET_FW_INFO */
+        mlan_fw_info fw_info;
+        /** Extended version information for MLAN_OID_GET_VER_EXT */
+        mlan_ver_ext ver_ext;
+        /** BSS information for MLAN_OID_GET_BSS_INFO */
+        mlan_bss_info bss_info;
+        /** Debug information for MLAN_OID_GET_DEBUG_INFO */
+        mlan_debug_info debug_info;
+#ifdef UAP_SUPPORT
+        /** UAP Statistics information for MLAN_OID_GET_STATS */
+        mlan_ds_uap_stats ustats;
+        /** UAP station list for MLAN_OID_UAP_STA_LIST */
+        mlan_ds_sta_list sta_list;
+#endif
+    } param;
+} mlan_ds_get_info, *pmlan_ds_get_info;
+
+/*-----------------------------------------------------------------*/
+/** Security Configuration Group */
+/*-----------------------------------------------------------------*/
+/** Enumeration for authentication mode */
+enum _mlan_auth_mode
+{
+    MLAN_AUTH_MODE_OPEN = 0x00,
+    MLAN_AUTH_MODE_SHARED = 0x01,
+    MLAN_AUTH_MODE_NETWORKEAP = 0x80,
+    MLAN_AUTH_MODE_AUTO = 0xFF,
+};
+
+/** Enumeration for encryption mode */
+enum _mlan_encryption_mode
+{
+    MLAN_ENCRYPTION_MODE_NONE = 0,
+    MLAN_ENCRYPTION_MODE_WEP40 = 1,
+    MLAN_ENCRYPTION_MODE_TKIP = 2,
+    MLAN_ENCRYPTION_MODE_CCMP = 3,
+    MLAN_ENCRYPTION_MODE_WEP104 = 4,
+};
+
+/** Enumeration for PSK */
+enum _mlan_psk_type
+{
+    MLAN_PSK_PASSPHRASE = 1,
+    MLAN_PSK_PMK,
+    MLAN_PSK_CLEAR,
+    MLAN_PSK_QUERY,
+};
+
+/** The bit to indicate the key is for unicast */
+#define MLAN_KEY_INDEX_UNICAST        0x40000000
+/** The key index to indicate default key */
+#define MLAN_KEY_INDEX_DEFAULT        0x000000ff
+/** Maximum key length */
+// #define MLAN_MAX_KEY_LENGTH        32
+/** Minimum passphrase length */
+#define MLAN_MIN_PASSPHRASE_LENGTH    8
+/** Maximum passphrase length */
+#define MLAN_MAX_PASSPHRASE_LENGTH    63
+/** PMK length */
+#define MLAN_PMK_HEXSTR_LENGTH        64
+/* A few details needed for WEP (Wireless Equivalent Privacy) */
+/** 104 bits */
+#define MAX_WEP_KEY_SIZE	13
+/** 40 bits RC4 - WEP */
+#define MIN_WEP_KEY_SIZE	5
+/** packet number size */
+#define PN_SIZE			16
+/** max seq size of wpa/wpa2 key */
+#define SEQ_MAX_SIZE        8
+
+/** key flag for tx_seq */
+#define KEY_FLAG_TX_SEQ_VALID	0x00000001
+/** key flag for rx_seq */
+#define KEY_FLAG_RX_SEQ_VALID	0x00000002
+/** key flag for group key */
+#define KEY_FLAG_GROUP_KEY      0x00000004
+/** key flag for tx and rx */
+#define KEY_FLAG_SET_TX_KEY	    0x00000008
+/** key flag for remove key */
+#define KEY_FLAG_REMOVE_KEY	    0x80000000
+
+/** Type definition of mlan_ds_encrypt_key for MLAN_OID_SEC_CFG_ENCRYPT_KEY */
+typedef struct _mlan_ds_encrypt_key
+{
+    /** Key disabled, all other fields will be ignore when this flag set to MTRUE */
+    t_u32 key_disable;
+    /** key removed flag, when this flag is set to MTRUE, only key_index will be check */
+    t_u32 key_remove;
+    /** Key index, used as current tx key index when is_current_wep_key is set to MTRUE */
+    t_u32 key_index;
+    /** Current Tx key flag */
+    t_u32 is_current_wep_key;
+    /** Key length */
+    t_u32 key_len;
+    /** Key */
+    t_u8 key_material[MLAN_MAX_KEY_LENGTH];
+    /** mac address */
+    t_u8 mac_addr[MLAN_MAC_ADDR_LENGTH];
+    /** wapi key flag */
+    t_u32 is_wapi_key;
+    /** Initial packet number */
+    t_u8 pn[PN_SIZE];
+    /** key flags */
+    t_u32 key_flags;
+} mlan_ds_encrypt_key, *pmlan_ds_encrypt_key;
+
+/** Type definition of mlan_passphrase_t */
+typedef struct _mlan_passphrase_t
+{
+    /** Length of passphrase */
+    t_u32 passphrase_len;
+    /** Passphrase */
+    t_u8 passphrase[MLAN_MAX_PASSPHRASE_LENGTH];
+} mlan_passphrase_t;
+
+/** Type defnition of mlan_pmk_t */
+typedef struct _mlan_pmk_t
+{
+    /** PMK */
+    t_u8 pmk[MLAN_MAX_KEY_LENGTH];
+} mlan_pmk_t;
+
+/** Embedded supplicant RSN type: No RSN */
+#define RSN_TYPE_NO_RSN     MBIT(0)
+/** Embedded supplicant RSN type: WPA */
+#define RSN_TYPE_WPA        MBIT(3)
+/** Embedded supplicant RSN type: WPA-NONE */
+#define RSN_TYPE_WPANONE    MBIT(4)
+/** Embedded supplicant RSN type: WPA2 */
+#define RSN_TYPE_WPA2       MBIT(5)
+/** Embedded supplicant RSN type: RFU */
+#define RSN_TYPE_VALID_BITS (RSN_TYPE_NO_RSN | RSN_TYPE_WPA | RSN_TYPE_WPANONE | RSN_TYPE_WPA2)
+
+/** Embedded supplicant cipher type: TKIP */
+#define EMBED_CIPHER_TKIP       MBIT(2)
+/** Embedded supplicant cipher type: AES */
+#define EMBED_CIPHER_AES        MBIT(3)
+/** Embedded supplicant cipher type: RFU */
+#define EMBED_CIPHER_VALID_BITS (EMBED_CIPHER_TKIP | EMBED_CIPHER_AES)
+
+/** Type definition of mlan_ds_passphrase for MLAN_OID_SEC_CFG_PASSPHRASE */
+typedef struct _mlan_ds_passphrase
+{
+    /** SSID may be used */
+    mlan_802_11_ssid ssid;
+    /** BSSID may be used */
+    mlan_802_11_mac_addr bssid;
+    /** Flag for passphrase or pmk used */
+    t_u16 psk_type;
+    /** Passphrase or PMK */
+    union
+    {
+        /** Passphrase */
+        mlan_passphrase_t passphrase;
+        /** PMK */
+        mlan_pmk_t pmk;
+    } psk;
+} mlan_ds_passphrase, *pmlan_ds_passphrase;
+
+/** Type definition of mlan_ds_esupp_mode for MLAN_OID_SEC_CFG_ESUPP_MODE */
+typedef struct _mlan_ds_ewpa_mode
+{
+    /** RSN mode */
+    t_u32 rsn_mode;
+    /** Active pairwise cipher */
+    t_u32 act_paircipher;
+    /** Active pairwise cipher */
+    t_u32 act_groupcipher;
+} mlan_ds_esupp_mode, *pmlan_ds_esupp_mode;
+
+/** Type definition of mlan_ds_sec_cfg for MLAN_IOCTL_SEC_CFG */
+typedef struct _mlan_ds_sec_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Security configuration parameter */
+    union
+    {
+        /** Authentication mode for MLAN_OID_SEC_CFG_AUTH_MODE */
+        t_u32 auth_mode;
+        /** Encryption mode for MLAN_OID_SEC_CFG_ENCRYPT_MODE */
+        t_u32 encrypt_mode;
+        /** WPA enabled flag for MLAN_OID_SEC_CFG_WPA_ENABLED */
+        t_u32 wpa_enabled;
+        /** WAPI enabled flag for MLAN_OID_SEC_CFG_WAPI_ENABLED */
+        t_u32 wapi_enabled;
+        /** Port Control enabled flag for MLAN_OID_SEC_CFG_PORT_CTRL */
+        t_u32 port_ctrl_enabled;
+        /** Encryption key for MLAN_OID_SEC_CFG_ENCRYPT_KEY */
+        mlan_ds_encrypt_key encrypt_key;
+        /** Passphrase for MLAN_OID_SEC_CFG_PASSPHRASE */
+        mlan_ds_passphrase passphrase;
+        /** Embedded supplicant WPA enabled flag for MLAN_OID_SEC_CFG_EWPA_ENABLED */
+        t_u32 ewpa_enabled;
+        /** Embedded supplicant mode for MLAN_OID_SEC_CFG_ESUPP_MODE */
+        mlan_ds_esupp_mode esupp_mode;
+    } param;
+} mlan_ds_sec_cfg, *pmlan_ds_sec_cfg;
+
+/*-----------------------------------------------------------------*/
+/** Rate Configuration Group */
+/*-----------------------------------------------------------------*/
+/** Enumeration for rate type */
+enum _mlan_rate_type
+{
+    MLAN_RATE_INDEX,
+    MLAN_RATE_VALUE
+};
+
+/** Enumeration for rate format */
+enum _mlan_rate_format
+{
+    MLAN_RATE_FORMAT_LG = 0,
+    MLAN_RATE_FORMAT_HT,
+    MLAN_RATE_FORMAT_AUTO = 0xFF,
+};
+/** Max bitmap rates size */
+#define MAX_BITMAP_RATES_SIZE   10
+
+/** Type definition of mlan_rate_cfg_t for MLAN_OID_RATE_CFG */
+typedef struct _mlan_rate_cfg_t
+{
+    /** Fixed rate: 0, auto rate: 1 */
+    t_u32 is_rate_auto;
+    /** Rate type. 0: index; 1: valude  */
+    t_u32 rate_type;
+    /** Rate/MCS index or rate value if fixed rate */
+    t_u32 rate;
+    /** Rate Bitmap */
+    t_u16 bitmap_rates[MAX_BITMAP_RATES_SIZE];
+} mlan_rate_cfg_t;
+
+/** HT channel bandwidth */
+typedef enum _mlan_ht_bw
+{
+    MLAN_HT_BW20,
+    MLAN_HT_BW40,
+} mlan_ht_bw;
+
+/** HT guard interval */
+typedef enum _mlan_ht_gi
+{
+    MLAN_HT_LGI,
+    MLAN_HT_SGI,
+} mlan_ht_gi;
+
+/** Band and BSS mode */
+typedef struct _mlan_band_data_rate
+{
+    /** Band configuration */
+    t_u8 config_bands;
+    /** BSS mode (Infra or IBSS) */
+    t_u8 bss_mode;
+} mlan_band_data_rate;
+
+/** Type definition of mlan_data_rate for MLAN_OID_GET_DATA_RATE */
+typedef struct _mlan_data_rate
+{
+    /** Tx data rate */
+    t_u32 tx_data_rate;
+    /** Rx data rate */
+    t_u32 rx_data_rate;
+
+    /** Tx channel bandwidth */
+    t_u32 tx_ht_bw;
+    /** Tx guard interval */
+    t_u32 tx_ht_gi;
+    /** Rx channel bandwidth */
+    t_u32 rx_ht_bw;
+    /** Rx guard interval */
+    t_u32 rx_ht_gi;
+} mlan_data_rate;
+
+/** Type definition of mlan_ds_rate for MLAN_IOCTL_RATE */
+typedef struct _mlan_ds_rate
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Rate configuration parameter */
+    union
+    {
+        /** Rate configuration for MLAN_OID_RATE_CFG */
+        mlan_rate_cfg_t rate_cfg;
+        /** Data rate for MLAN_OID_GET_DATA_RATE */
+        mlan_data_rate data_rate;
+        /** Supported rates for MLAN_OID_SUPPORTED_RATES */
+        t_u8 rates[MLAN_SUPPORTED_RATES];
+        /** Band/BSS mode for getting supported rates */
+        mlan_band_data_rate rate_band_cfg;
+    } param;
+} mlan_ds_rate, *pmlan_ds_rate;
+
+/*-----------------------------------------------------------------*/
+/** Power Configuration Group */
+/*-----------------------------------------------------------------*/
+
+/** Type definition of mlan_power_cfg_t for MLAN_OID_POWER_CFG */
+typedef struct _mlan_power_cfg_t
+{
+    /** Is power auto */
+    t_u32 is_power_auto;
+    /** Power level in dBm */
+    t_u32 power_level;
+} mlan_power_cfg_t;
+
+/** max power table size */
+#define MAX_POWER_TABLE_SIZE 	128
+
+/** The HT BW40 bit in Tx rate index */
+#define TX_RATE_HT_BW40_BIT 	MBIT(7)
+
+/** Type definition of mlan_power_cfg_ext for MLAN_OID_POWER_CFG_EXT */
+typedef struct _mlan_power_cfg_ext
+{
+    /** Length of power_data */
+    t_u32 len;
+    /** Buffer of power configuration data */
+    t_u32 power_data[MAX_POWER_TABLE_SIZE];
+} mlan_power_cfg_ext;
+
+/** Type definition of mlan_ds_power_cfg for MLAN_IOCTL_POWER_CFG */
+typedef struct _mlan_ds_power_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Power configuration parameter */
+    union
+    {
+        /** Power configuration for MLAN_OID_POWER_CFG */
+        mlan_power_cfg_t power_cfg;
+        /** Extended power configuration for MLAN_OID_POWER_CFG_EXT */
+        mlan_power_cfg_ext power_ext;
+    } param;
+} mlan_ds_power_cfg, *pmlan_ds_power_cfg;
+
+/*-----------------------------------------------------------------*/
+/** Power Management Configuration Group */
+/*-----------------------------------------------------------------*/
+/** Host sleep config conditions : Cancel */
+#define HOST_SLEEP_CFG_CANCEL   0xffffffff
+
+/** Host sleep config condition: broadcast data */
+#define HOST_SLEEP_COND_BROADCAST_DATA  MBIT(0)
+/** Host sleep config condition: unicast data */
+#define HOST_SLEEP_COND_UNICAST_DATA    MBIT(1)
+/** Host sleep config condition: mac event */
+#define HOST_SLEEP_COND_MAC_EVENT       MBIT(2)
+/** Host sleep config condition: multicast data */
+#define HOST_SLEEP_COND_MULTICAST_DATA  MBIT(3)
+
+/** Host sleep config conditions: Default */
+#define HOST_SLEEP_DEF_COND     (HOST_SLEEP_COND_BROADCAST_DATA | HOST_SLEEP_COND_UNICAST_DATA | HOST_SLEEP_COND_MAC_EVENT)
+/** Host sleep config GPIO : Default */
+#define HOST_SLEEP_DEF_GPIO     0xff
+/** Host sleep config gap : Default */
+#define HOST_SLEEP_DEF_GAP      200
+
+/** Type definition of mlan_ds_hs_cfg for MLAN_OID_PM_CFG_HS_CFG */
+typedef struct _mlan_ds_hs_cfg
+{
+    /** MTRUE to invoke the HostCmd, MFALSE otherwise */
+    t_u32 is_invoke_hostcmd;
+    /** Host sleep config condition */
+    /** Bit0: broadcast data
+     *  Bit1: unicast data
+     *  Bit2: mac event
+     *  Bit3: multicast data
+     */
+    t_u32 conditions;
+    /** GPIO pin or 0xff for interface */
+    t_u32 gpio;
+    /** Gap in milliseconds or or 0xff for special setting when GPIO is used to wakeup host */
+    t_u32 gap;
+} mlan_ds_hs_cfg, *pmlan_ds_hs_cfg;
+
+/** Enable deep sleep mode */
+#define DEEP_SLEEP_ON  1
+/** Disable deep sleep mode */
+#define DEEP_SLEEP_OFF 0
+
+/** Default idle time in milliseconds for auto deep sleep */
+#define DEEP_SLEEP_IDLE_TIME	100
+
+typedef struct _mlan_ds_auto_ds
+{
+    /** auto ds mode, 0 - disable, 1 - enable */
+    t_u16 auto_ds;
+    /** auto ds idle time in milliseconds */
+    t_u16 idletime;
+} mlan_ds_auto_ds;
+
+/** Type definition of mlan_ds_inactivity_to for MLAN_OID_PM_CFG_INACTIVITY_TO */
+typedef struct _mlan_ds_inactivity_to
+{
+    /** Timeout unit in microsecond, 0 means 1000us (1ms) */
+    t_u32 timeout_unit;
+    /** Inactivity timeout for unicast data */
+    t_u32 unicast_timeout;
+    /** Inactivity timeout for multicast data */
+    t_u32 mcast_timeout;
+    /** Timeout for additional Rx traffic after Null PM1 packet exchange */
+    t_u32 ps_entry_timeout;
+} mlan_ds_inactivity_to, *pmlan_ds_inactivity_to;
+
+/** Minimum sleep period in milliseconds */
+#define MIN_SLEEP_PERIOD    10
+/** Maximum sleep period in milliseconds */
+#define MAX_SLEEP_PERIOD    60
+/** Special setting for UPSD certification tests */
+#define SLEEP_PERIOD_RESERVED_FF    0xFF
+
+/** PS null interval disable */
+#define PS_NULL_DISABLE         (-1)
+
+/** Local listen interval disable */
+#define MRVDRV_LISTEN_INTERVAL_DISABLE   (-1)
+/** Minimum listen interval */
+#define MRVDRV_MIN_LISTEN_INTERVAL       0
+
+/** Minimum multiple DTIM */
+#define MRVDRV_MIN_MULTIPLE_DTIM                0
+/** Maximum multiple DTIM */
+#define MRVDRV_MAX_MULTIPLE_DTIM                5
+/** Ignore multiple DTIM */
+#define MRVDRV_IGNORE_MULTIPLE_DTIM             0xfffe
+/** Match listen interval to closest DTIM */
+#define MRVDRV_MATCH_CLOSEST_DTIM               0xfffd
+
+/** Minimum adhoc awake period */
+#define MIN_ADHOC_AWAKE_PD      0
+/** Maximum adhoc awake period */
+#define MAX_ADHOC_AWAKE_PD      31
+/** Special adhoc awake period */
+#define SPECIAL_ADHOC_AWAKE_PD  255
+
+/** Minimum beacon miss timeout in milliseconds */
+#define MIN_BCN_MISS_TO         0
+/** Maximum beacon miss timeout in milliseconds */
+#define MAX_BCN_MISS_TO         50
+/** Disable beacon miss timeout */
+#define DISABLE_BCN_MISS_TO     65535
+
+/** Minimum delay to PS in milliseconds */
+#define MIN_DELAY_TO_PS         0
+/** Maximum delay to PS in milliseconds */
+#define MAX_DELAY_TO_PS         65535
+/** Delay to PS unchanged */
+#define DELAY_TO_PS_UNCHANGED   (-1)
+/** Default delay to PS in milliseconds */
+#define DELAY_TO_PS_DEFAULT     1000
+
+/** PS mode: Unchanged */
+#define PS_MODE_UNCHANGED       0
+/** PS mode: Auto */
+#define PS_MODE_AUTO            1
+/** PS mode: Poll */
+#define PS_MODE_POLL            2
+/** PS mode: Null */
+#define PS_MODE_NULL            3
+
+/** Type definition of mlan_ds_ps_cfg for MLAN_OID_PM_CFG_PS_CFG */
+typedef struct _mlan_ds_ps_cfg
+{
+    /** PS null interval in seconds */
+    t_u32 ps_null_interval;
+    /** Multiple DTIM interval */
+    t_u32 multiple_dtim_interval;
+    /** Listen interval */
+    t_u32 listen_interval;
+    /** Adhoc awake period */
+    t_u32 adhoc_awake_period;
+    /** Beacon miss timeout in milliseconds */
+    t_u32 bcn_miss_timeout;
+    /** Delay to PS in milliseconds */
+    t_s32 delay_to_ps;
+    /** PS mode */
+    t_u32 ps_mode;
+} mlan_ds_ps_cfg, *pmlan_ds_ps_cfg;
+
+/** Type definition of mlan_ds_sleep_params for MLAN_OID_PM_CFG_SLEEP_PARAMS */
+typedef struct _mlan_ds_sleep_params
+{
+    /** Error */
+    t_u32 error;
+    /** Offset in microseconds */
+    t_u32 offset;
+    /** Stable time in microseconds */
+    t_u32 stable_time;
+    /** Calibration control */
+    t_u32 cal_control;
+    /** External sleep clock */
+    t_u32 ext_sleep_clk;
+    /** Reserved */
+    t_u32 reserved;
+} mlan_ds_sleep_params, *pmlan_ds_sleep_params;
+
+/** sleep_param */
+typedef struct _ps_sleep_param
+{
+    /** control bitmap */
+    t_u32 ctrl_bitmap;
+    /** minimum sleep period (micro second) */
+    t_u32 min_sleep;
+    /** maximum sleep period (micro second) */
+    t_u32 max_sleep;
+} ps_sleep_param;
+
+/** inactivity sleep_param */
+typedef struct _inact_sleep_param
+{
+    /** inactivity timeout (micro second) */
+    t_u32 inactivity_to;
+    /** miniumu awake period (micro second) */
+    t_u32 min_awake;
+    /** maximum awake period (micro second) */
+    t_u32 max_awake;
+} inact_sleep_param;
+
+/** flag for ps mode */
+#define PS_FLAG_PS_MODE                 1
+/** flag for sleep param */
+#define PS_FLAG_SLEEP_PARAM             2
+/** flag for inactivity sleep param */
+#define PS_FLAG_INACT_SLEEP_PARAM       4
+
+/** Disable power mode */
+#define PS_MODE_DISABLE                      0
+/** Enable periodic dtim ps */
+#define PS_MODE_PERIODIC_DTIM                1
+/** Enable inactivity ps */
+#define PS_MODE_INACTIVITY                   2
+
+/** mlan_ds_ps_mgmt */
+typedef struct _mlan_ds_ps_mgmt
+{
+    /** flags for valid field */
+    t_u16 flags;
+    /** power mode */
+    t_u16 ps_mode;
+    /** sleep param */
+    ps_sleep_param sleep_param;
+    /** inactivity sleep param */
+    inact_sleep_param inact_param;
+} mlan_ds_ps_mgmt;
+
+/** mlan_ds_ps_info */
+typedef struct _mlan_ds_ps_info
+{
+    /** suspend allowed flag */
+    t_u32 is_suspend_allowed;
+} mlan_ds_ps_info;
+
+/** Type definition of mlan_ds_pm_cfg for MLAN_IOCTL_PM_CFG */
+typedef struct _mlan_ds_pm_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Power management parameter */
+    union
+    {
+        /** Power saving mode for MLAN_OID_PM_CFG_IEEE_PS */
+        t_u32 ps_mode;
+        /** Host Sleep configuration for MLAN_OID_PM_CFG_HS_CFG */
+        mlan_ds_hs_cfg hs_cfg;
+        /** Deep sleep mode for MLAN_OID_PM_CFG_DEEP_SLEEP */
+        mlan_ds_auto_ds auto_deep_sleep;
+        /** Inactivity timeout for MLAN_OID_PM_CFG_INACTIVITY_TO */
+        mlan_ds_inactivity_to inactivity_to;
+        /** Sleep period for MLAN_OID_PM_CFG_SLEEP_PD */
+        t_u32 sleep_period;
+        /** PS configuration parameters for MLAN_OID_PM_CFG_PS_CFG */
+        mlan_ds_ps_cfg ps_cfg;
+        /** PS configuration parameters for MLAN_OID_PM_CFG_SLEEP_PARAMS */
+        mlan_ds_sleep_params sleep_params;
+        /** PS configuration parameters for MLAN_OID_PM_CFG_PS_MODE */
+        mlan_ds_ps_mgmt ps_mgmt;
+        /** power info for MLAN_OID_PM_INFO */
+        mlan_ds_ps_info ps_info;
+    } param;
+} mlan_ds_pm_cfg, *pmlan_ds_pm_cfg;
+
+/*-----------------------------------------------------------------*/
+/** WMM Configuration Group */
+/*-----------------------------------------------------------------*/
+
+/** WMM TSpec size */
+#define MLAN_WMM_TSPEC_SIZE             63
+/** WMM Add TS extra IE bytes */
+#define MLAN_WMM_ADDTS_EXTRA_IE_BYTES   256
+/** WMM statistics for packets hist bins */
+#define MLAN_WMM_STATS_PKTS_HIST_BINS   7
+/** Maximum number of AC QOS queues available */
+#define MLAN_WMM_MAX_AC_QUEUES          4
+
+/**
+ *  @brief IOCTL structure to send an ADDTS request and retrieve the response.
+ *
+ *  IOCTL structure from the application layer relayed to firmware to
+ *    instigate an ADDTS management frame with an appropriate TSPEC IE as well
+ *    as any additional IEs appended in the ADDTS Action frame.
+ *
+ *  @sa woal_wmm_addts_req_ioctl
+ */
+typedef struct
+{
+    mlan_cmd_result_e cmd_result;     /**< Firmware execution result */
+
+    t_u32 timeout_ms;                 /**< Timeout value in milliseconds */
+    t_u8 ieee_status_code;            /**< IEEE status code */
+
+    t_u32 ie_data_len;                /**< Length of ie block in ie_data */
+    t_u8 ie_data[MLAN_WMM_TSPEC_SIZE  /**< TSPEC to send in the ADDTS */
+                 + MLAN_WMM_ADDTS_EXTRA_IE_BYTES];  /**< Extra IE buf*/
+} wlan_ioctl_wmm_addts_req_t;
+
+/**
+ *  @brief IOCTL structure to send a DELTS request.
+ *
+ *  IOCTL structure from the application layer relayed to firmware to
+ *    instigate an DELTS management frame with an appropriate TSPEC IE.
+ *
+ *  @sa woal_wmm_delts_req_ioctl
+ */
+typedef struct
+{
+    mlan_cmd_result_e cmd_result; /**< Firmware execution result */
+    t_u8 ieee_reason_code;        /**< IEEE reason code sent, unused for WMM */
+    t_u32 ie_data_len;            /**< Length of ie block in ie_data */
+    t_u8 ie_data[MLAN_WMM_TSPEC_SIZE]; /**< TSPEC to send in the DELTS */
+} wlan_ioctl_wmm_delts_req_t;
+
+/**
+ *  @brief IOCTL structure to configure a specific AC Queue's parameters
+ *
+ *  IOCTL structure from the application layer relayed to firmware to
+ *    get, set, or default the WMM AC queue parameters.
+ *
+ *  - msdu_lifetime_expiry is ignored if set to 0 on a set command
+ *
+ *  @sa woal_wmm_queue_config_ioctl
+ */
+typedef struct
+{
+    mlan_wmm_queue_config_action_e action;    /**< Set, Get, or Default */
+    mlan_wmm_ac_e access_category;            /**< WMM_AC_BK(0) to WMM_AC_VO(3) */
+    t_u16 msdu_lifetime_expiry;               /**< lifetime expiry in TUs */
+    t_u8 supported_rates[10];                 /**< Not supported yet */
+} wlan_ioctl_wmm_queue_config_t;
+
+/**
+ *  @brief IOCTL structure to start, stop, and get statistics for a WMM AC
+ *
+ *  IOCTL structure from the application layer relayed to firmware to
+ *    start or stop statistical collection for a given AC.  Also used to
+ *    retrieve and clear the collected stats on a given AC.
+ *
+ *  @sa woal_wmm_queue_stats_ioctl
+ */
+typedef struct
+{
+    /** Action of Queue Config : Start, Stop, or Get */
+    mlan_wmm_queue_stats_action_e action;
+    /** User Priority */
+    t_u8 user_priority;
+    /** Number of successful packets transmitted */
+    t_u16 pkt_count;
+    /** Packets lost; not included in pkt_count */
+    t_u16 pkt_loss;
+    /** Average Queue delay in microseconds */
+    t_u32 avg_queue_delay;
+    /** Average Transmission delay in microseconds */
+    t_u32 avg_tx_delay;
+    /** Calculated used time in units of 32 microseconds */
+    t_u16 used_time;
+    /** Calculated policed time in units of 32 microseconds */
+    t_u16 policed_time;
+    /** Queue Delay Histogram; number of packets per queue delay range
+     *
+     *  [0] -  0ms <= delay < 5ms
+     *  [1] -  5ms <= delay < 10ms
+     *  [2] - 10ms <= delay < 20ms
+     *  [3] - 20ms <= delay < 30ms
+     *  [4] - 30ms <= delay < 40ms
+     *  [5] - 40ms <= delay < 50ms
+     *  [6] - 50ms <= delay < msduLifetime (TUs)
+     */
+    t_u16 delay_histogram[MLAN_WMM_STATS_PKTS_HIST_BINS];
+} wlan_ioctl_wmm_queue_stats_t,
+/** Type definition of mlan_ds_wmm_queue_stats for MLAN_OID_WMM_CFG_QUEUE_STATS */
+  mlan_ds_wmm_queue_stats, *pmlan_ds_wmm_queue_stats;
+
+/**
+ *  @brief IOCTL sub structure for a specific WMM AC Status
+ */
+typedef struct
+{
+    /** WMM Acm */
+    t_u8 wmm_acm;
+    /** Flow required flag */
+    t_u8 flow_required;
+    /** Flow created flag */
+    t_u8 flow_created;
+    /** Disabled flag */
+    t_u8 disabled;
+} wlan_ioctl_wmm_queue_status_ac_t;
+
+/**
+ *  @brief IOCTL structure to retrieve the WMM AC Queue status
+ *
+ *  IOCTL structure from the application layer to retrieve:
+ *     - ACM bit setting for the AC
+ *     - Firmware status (flow required, flow created, flow disabled)
+ *
+ *  @sa woal_wmm_queue_status_ioctl
+ */
+typedef struct
+{
+    /** WMM AC queue status */
+    wlan_ioctl_wmm_queue_status_ac_t ac_status[MLAN_WMM_MAX_AC_QUEUES];
+} wlan_ioctl_wmm_queue_status_t,
+/** Type definition of mlan_ds_wmm_queue_status for MLAN_OID_WMM_CFG_QUEUE_STATUS */
+  mlan_ds_wmm_queue_status, *pmlan_ds_wmm_queue_status;
+
+/** Type definition of mlan_ds_wmm_addts for MLAN_OID_WMM_CFG_ADDTS */
+typedef struct _mlan_ds_wmm_addts
+{
+    /** Result of ADDTS request */
+    mlan_cmd_result_e result;
+    /** Timeout value in milliseconds */
+    t_u32 timeout;
+    /** IEEE status code */
+    t_u32 status_code;
+    /** Dialog token */
+    t_u8 dialog_tok;
+    /** TSPEC data length */
+    t_u8 ie_data_len;
+    /** TSPEC to send in the ADDTS + buffering for any extra IEs */
+    t_u8 ie_data[MLAN_WMM_TSPEC_SIZE + MLAN_WMM_ADDTS_EXTRA_IE_BYTES];
+} mlan_ds_wmm_addts, *pmlan_ds_wmm_addts;
+
+/** Type definition of mlan_ds_wmm_delts for MLAN_OID_WMM_CFG_DELTS */
+typedef struct _mlan_ds_wmm_delts
+{
+    /** Result of DELTS request */
+    mlan_cmd_result_e result;
+    /** IEEE status code */
+    t_u32 status_code;
+    /** TSPEC data length */
+    t_u8 ie_data_len;
+    /** TSPEC to send in the DELTS */
+    t_u8 ie_data[MLAN_WMM_TSPEC_SIZE];
+} mlan_ds_wmm_delts, *pmlan_ds_wmm_delts;
+
+/** Type definition of mlan_ds_wmm_queue_config for MLAN_OID_WMM_CFG_QUEUE_CONFIG */
+typedef struct _mlan_ds_wmm_queue_config
+{
+    /** Action of Queue Config : Set, Get, or Default */
+    mlan_wmm_queue_config_action_e action;
+    /** WMM Access Category: WMM_AC_BK(0) to WMM_AC_VO(3) */
+    mlan_wmm_ac_e access_category;
+    /** Lifetime expiry in TUs */
+    t_u16 msdu_lifetime_expiry;
+    /** Reserve for future use */
+    t_u8 reserved[10];
+} mlan_ds_wmm_queue_config, *pmlan_ds_wmm_queue_config;
+
+/** Type definition of mlan_ds_wmm_cfg for MLAN_IOCTL_WMM_CFG */
+typedef struct _mlan_ds_wmm_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** WMM configuration parameter */
+    union
+    {
+        /** WMM enable for MLAN_OID_WMM_CFG_ENABLE */
+        t_u32 wmm_enable;
+        /** QoS configuration for MLAN_OID_WMM_CFG_QOS */
+        t_u8 qos_cfg;
+        /** WMM add TS for MLAN_OID_WMM_CFG_ADDTS */
+        mlan_ds_wmm_addts addts;
+        /** WMM delete TS for MLAN_OID_WMM_CFG_DELTS */
+        mlan_ds_wmm_delts delts;
+        /** WMM queue configuration for MLAN_OID_WMM_CFG_QUEUE_CONFIG */
+        mlan_ds_wmm_queue_config q_cfg;
+        /** WMM queue status for MLAN_OID_WMM_CFG_QUEUE_STATS */
+        mlan_ds_wmm_queue_stats q_stats;
+        /** WMM queue status for MLAN_OID_WMM_CFG_QUEUE_STATUS */
+        mlan_ds_wmm_queue_status q_status;
+        /** WMM TS status for MLAN_OID_WMM_CFG_TS_STATUS */
+        mlan_ds_wmm_ts_status ts_status;
+    } param;
+} mlan_ds_wmm_cfg, *pmlan_ds_wmm_cfg;
+
+/*-----------------------------------------------------------------*/
+/** WPS Configuration Group */
+/*-----------------------------------------------------------------*/
+/** Enumeration for WPS session */
+enum _mlan_wps_status
+{
+    MLAN_WPS_CFG_SESSION_START = 1,
+    MLAN_WPS_CFG_SESSION_END = 0
+};
+
+/** Type definition of mlan_ds_wps_cfg for MLAN_IOCTL_WPS_CFG */
+typedef struct _mlan_ds_wps_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** WPS configuration parameter */
+    union
+    {
+        /** WPS session for MLAN_OID_WPS_CFG_SESSION */
+        t_u32 wps_session;
+    } param;
+} mlan_ds_wps_cfg, *pmlan_ds_wps_cfg;
+
+/*-----------------------------------------------------------------*/
+/** 802.11n Configuration Group */
+/*-----------------------------------------------------------------*/
+/** Maximum MCS */
+#define NUM_MCS_FIELD      16
+
+/** Supported stream modes */
+#define HT_STREAM_MODE_1X1   0x11
+#define HT_STREAM_MODE_2X2   0x22
+
+/* Both 2.4G and 5G band selected */
+#define BAND_SELECT_BOTH    0
+/* Band 2.4G selected */
+#define BAND_SELECT_BG      1
+/* Band 5G selected */
+#define BAND_SELECT_A       2
+
+/** Type definition of mlan_ds_11n_htcap_cfg for MLAN_OID_11N_HTCAP_CFG */
+typedef struct _mlan_ds_11n_htcap_cfg
+{
+    /** HT Capability information */
+    t_u32 htcap;
+    /** Band selection */
+    t_u32 misc_cfg;
+    /** Hardware HT cap information required */
+    t_u32 hw_cap_req;
+} mlan_ds_11n_htcap_cfg, *pmlan_ds_11n_htcap_cfg;
+
+/** Type definition of mlan_ds_11n_addba_param for MLAN_OID_11N_CFG_ADDBA_PARAM */
+typedef struct _mlan_ds_11n_addba_param
+{
+    /** Timeout */
+    t_u32 timeout;
+    /** Buffer size for ADDBA request */
+    t_u32 txwinsize;
+    /** Buffer size for ADDBA response */
+    t_u32 rxwinsize;
+    /** amsdu for ADDBA request */
+    t_u8 txamsdu;
+    /** amsdu for ADDBA response */
+    t_u8 rxamsdu;
+} mlan_ds_11n_addba_param, *pmlan_ds_11n_addba_param;
+
+/** Type definition of mlan_ds_11n_tx_cfg for MLAN_OID_11N_CFG_TX */
+typedef struct _mlan_ds_11n_tx_cfg
+{
+    /** HTTxCap */
+    t_u16 httxcap;
+    /** HTTxInfo */
+    t_u16 httxinfo;
+    /** Band selection */
+    t_u32 misc_cfg;
+} mlan_ds_11n_tx_cfg, *pmlan_ds_11n_tx_cfg;
+
+/** BF Global Configuration */
+#define BF_GLOBAL_CONFIGURATION     0x00
+/** Performs NDP sounding for PEER specified */
+#define TRIGGER_SOUNDING_FOR_PEER   0x01
+/** TX BF interval for channel sounding */
+#define SET_GET_BF_PERIODICITY      0x02
+/** Tell FW not to perform any sounding for peer */
+#define TX_BF_FOR_PEER_ENBL         0x03
+/** TX BF SNR threshold for peer */
+#define SET_SNR_THR_PEER            0x04
+
+/* Maximum number of peer MAC and status/SNR tuples */
+#define MAX_PEER_MAC_TUPLES         10
+
+/** Any new subcommand structure should be declare here */
+
+/** bf global cfg args */
+typedef struct _mlan_bf_global_cfg_args
+{
+    /** Global enable/disable bf */
+    t_u8 bf_enbl;
+    /** Global enable/disable sounding */
+    t_u8 sounding_enbl;
+    /** FB Type */
+    t_u8 fb_type;
+    /** SNR Threshold */
+    t_u8 snr_threshold;
+    /** Sounding interval in milliseconds */
+    t_u16 sounding_interval;
+    /** BF mode */
+    t_u8 bf_mode;
+    /** Reserved */
+    t_u8 reserved;
+} mlan_bf_global_cfg_args;
+
+/** trigger sounding args */
+typedef struct _mlan_trigger_sound_args
+{
+    /** Peer MAC address */
+    t_u8 peer_mac[MLAN_MAC_ADDR_LENGTH];
+    /** Status */
+    t_u8 status;
+} mlan_trigger_sound_args;
+
+/** bf periodicity args */
+typedef struct _mlan_bf_periodicity_args
+{
+    /** Peer MAC address */
+    t_u8 peer_mac[MLAN_MAC_ADDR_LENGTH];
+    /** Current Tx BF Interval in milliseconds */
+    t_u16 interval;
+    /** Status */
+    t_u8 status;
+} mlan_bf_periodicity_args;
+
+/** tx bf peer args */
+typedef struct _mlan_tx_bf_peer_args
+{
+    /** Peer MAC address */
+    t_u8 peer_mac[MLAN_MAC_ADDR_LENGTH];
+    /** Reserved */
+    t_u16 reserved;
+    /** Enable/Disable Beamforming */
+    t_u8 bf_enbl;
+    /** Enable/Disable sounding */
+    t_u8 sounding_enbl;
+    /** FB Type */
+    t_u8 fb_type;
+} mlan_tx_bf_peer_args;
+
+/** SNR threshold args */
+typedef struct _mlan_snr_thr_args
+{
+    /** Peer MAC address */
+    t_u8 peer_mac[MLAN_MAC_ADDR_LENGTH];
+    /** SNR for peer */
+    t_u8 snr;
+} mlan_snr_thr_args;
+
+/** Type definition of mlan_ds_11n_tx_bf_cfg for MLAN_OID_11N_CFG_TX_BF_CFG */
+typedef struct _mlan_ds_11n_tx_bf_cfg
+{
+    /** BF Action */
+    t_u16 bf_action;
+    /** Action */
+    t_u16 action;
+    /** Number of peers */
+    t_u32 no_of_peers;
+    union
+    {
+        mlan_bf_global_cfg_args bf_global_cfg;
+        mlan_trigger_sound_args bf_sound[MAX_PEER_MAC_TUPLES];
+        mlan_bf_periodicity_args bf_periodicity[MAX_PEER_MAC_TUPLES];
+        mlan_tx_bf_peer_args tx_bf_peer[MAX_PEER_MAC_TUPLES];
+        mlan_snr_thr_args bf_snr[MAX_PEER_MAC_TUPLES];
+    } body;
+} mlan_ds_11n_tx_bf_cfg, *pmlan_ds_11n_tx_bf_cfg;
+
+/** Type definition of mlan_ds_11n_amsdu_aggr_ctrl for
+ * MLAN_OID_11N_AMSDU_AGGR_CTRL*/
+typedef struct _mlan_ds_11n_amsdu_aggr_ctrl
+{
+    /** Enable/Disable */
+    t_u16 enable;
+    /** Current AMSDU size valid */
+    t_u16 curr_buf_size;
+} mlan_ds_11n_amsdu_aggr_ctrl, *pmlan_ds_11n_amsdu_aggr_ctrl;
+
+/** Type definition of mlan_ds_11n_aggr_prio_tbl for MLAN_OID_11N_CFG_AGGR_PRIO_TBL */
+typedef struct _mlan_ds_11n_aggr_prio_tbl
+{
+    /** ampdu priority table */
+    t_u8 ampdu[MAX_NUM_TID];
+    /** amsdu priority table */
+    t_u8 amsdu[MAX_NUM_TID];
+} mlan_ds_11n_aggr_prio_tbl, *pmlan_ds_11n_aggr_prio_tbl;
+
+/** Type definition of mlan_ds_11n_cfg for MLAN_IOCTL_11N_CFG */
+typedef struct _mlan_ds_11n_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** 802.11n configuration parameter */
+    union
+    {
+        /** Tx param for 11n for MLAN_OID_11N_CFG_TX */
+        mlan_ds_11n_tx_cfg tx_cfg;
+        /** Aggr priority table for MLAN_OID_11N_CFG_AGGR_PRIO_TBL */
+        mlan_ds_11n_aggr_prio_tbl aggr_prio_tbl;
+        /** Add BA param for MLAN_OID_11N_CFG_ADDBA_PARAM */
+        mlan_ds_11n_addba_param addba_param;
+        /** Add BA Reject paramters for MLAN_OID_11N_CFG_ADDBA_REJECT */
+        t_u8 addba_reject[MAX_NUM_TID];
+        /** Tx buf size for MLAN_OID_11N_CFG_MAX_TX_BUF_SIZE */
+        t_u32 tx_buf_size;
+        /** HT cap info configuration for MLAN_OID_11N_HTCAP_CFG */
+        mlan_ds_11n_htcap_cfg htcap_cfg;
+        /** Tx param for 11n for MLAN_OID_11N_AMSDU_AGGR_CTRL */
+        mlan_ds_11n_amsdu_aggr_ctrl amsdu_aggr_ctrl;
+        /** Supported MCS Set field */
+        t_u8 supported_mcs_set[NUM_MCS_FIELD];
+        /** Transmit Beamforming Capabilities field */
+        t_u32 tx_bf_cap;
+        /** Transmit Beamforming configuration */
+        mlan_ds_11n_tx_bf_cfg tx_bf;
+        /** HT stream configuration */
+        t_u32 stream_cfg;
+    } param;
+} mlan_ds_11n_cfg, *pmlan_ds_11n_cfg;
+
+/** Country code length */
+#define COUNTRY_CODE_LEN                        3
+
+/*-----------------------------------------------------------------*/
+/** 802.11d Configuration Group */
+/*-----------------------------------------------------------------*/
+/** Maximum subbands for 11d */
+#define MRVDRV_MAX_SUBBAND_802_11D              83
+
+#ifdef STA_SUPPORT
+/** Data structure for subband set */
+typedef struct _mlan_ds_subband_set_t
+{
+    /** First channel */
+    t_u8 first_chan;
+    /** Number of channels */
+    t_u8 no_of_chan;
+    /** Maximum Tx power in dBm */
+    t_u8 max_tx_pwr;
+} mlan_ds_subband_set_t;
+
+/** Domain regulatory information */
+typedef struct _mlan_ds_11d_domain_info
+{
+    /** Country Code */
+    t_u8 country_code[COUNTRY_CODE_LEN];
+    /** Band that channels in sub_band belong to */
+    t_u8 band;
+    /** No. of subband in below */
+    t_u8 no_of_sub_band;
+    /** Subband data to send/last sent */
+    mlan_ds_subband_set_t sub_band[MRVDRV_MAX_SUBBAND_802_11D];
+} mlan_ds_11d_domain_info;
+#endif
+
+/** Type definition of mlan_ds_11d_cfg for MLAN_IOCTL_11D_CFG */
+typedef struct _mlan_ds_11d_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** 802.11d configuration parameter */
+    union
+    {
+#ifdef STA_SUPPORT
+        /** Enable for MLAN_OID_11D_CFG_ENABLE */
+        t_u32 enable_11d;
+        /** Domain info for MLAN_OID_11D_DOMAIN_INFO */
+        mlan_ds_11d_domain_info domain_info;
+#endif                          /* STA_SUPPORT */
+#ifdef UAP_SUPPORT
+        /** tlv data for MLAN_OID_11D_DOMAIN_INFO */
+        t_u8 domain_tlv[MAX_IE_SIZE];
+#endif                          /* UAP_SUPPORT */
+    } param;
+} mlan_ds_11d_cfg, *pmlan_ds_11d_cfg;
+
+/*-----------------------------------------------------------------*/
+/** Register Memory Access Group */
+/*-----------------------------------------------------------------*/
+/** Enumeration for register type */
+enum _mlan_reg_type
+{
+    MLAN_REG_MAC = 1,
+    MLAN_REG_BBP,
+    MLAN_REG_RF,
+    MLAN_REG_CAU = 5,
+};
+
+/** Type definition of mlan_ds_reg_rw for MLAN_OID_REG_RW */
+typedef struct _mlan_ds_reg_rw
+{
+    /** Register type */
+    t_u32 type;
+    /** Offset */
+    t_u32 offset;
+    /** Value */
+    t_u32 value;
+} mlan_ds_reg_rw;
+
+/** Maximum EEPROM data */
+#define MAX_EEPROM_DATA 256
+
+/** Type definition of mlan_ds_read_eeprom for MLAN_OID_EEPROM_RD */
+typedef struct _mlan_ds_read_eeprom
+{
+    /** Multiples of 4 */
+    t_u16 offset;
+    /** Number of bytes */
+    t_u16 byte_count;
+    /** Value */
+    t_u8 value[MAX_EEPROM_DATA];
+} mlan_ds_read_eeprom;
+
+/** Type definition of mlan_ds_mem_rw for MLAN_OID_MEM_RW */
+typedef struct _mlan_ds_mem_rw
+{
+    /** Address */
+    t_u32 addr;
+    /** Value */
+    t_u32 value;
+} mlan_ds_mem_rw;
+
+/** Type definition of mlan_ds_reg_mem for MLAN_IOCTL_REG_MEM */
+typedef struct _mlan_ds_reg_mem
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Register memory access parameter */
+    union
+    {
+        /** Register access for MLAN_OID_REG_RW */
+        mlan_ds_reg_rw reg_rw;
+        /** EEPROM access for MLAN_OID_EEPROM_RD */
+        mlan_ds_read_eeprom rd_eeprom;
+        /** Memory access for MLAN_OID_MEM_RW */
+        mlan_ds_mem_rw mem_rw;
+    } param;
+} mlan_ds_reg_mem, *pmlan_ds_reg_mem;
+
+/*-----------------------------------------------------------------*/
+/** Multi-Radio Configuration Group */
+/*-----------------------------------------------------------------*/
+
+/*-----------------------------------------------------------------*/
+/** 802.11h Configuration Group */
+/*-----------------------------------------------------------------*/
+#if defined(DFS_TESTING_SUPPORT)
+/** Type definition of mlan_ds_11h_dfs_testing for MLAN_OID_11H_DFS_TESTING */
+typedef struct _mlan_ds_11h_dfs_testing
+{
+    /** User-configured CAC period in milliseconds, 0 to use default */
+    t_u16 usr_cac_period_msec;
+    /** User-configured NOP period in seconds, 0 to use default */
+    t_u16 usr_nop_period_sec;
+    /** User-configured skip channel change, 0 to disable */
+    t_u8 usr_no_chan_change;
+    /** User-configured fixed channel to change to, 0 to use random channel */
+    t_u8 usr_fixed_new_chan;
+} mlan_ds_11h_dfs_testing, *pmlan_ds_11h_dfs_testing;
+#endif
+
+/** Type definition of mlan_ds_11h_cfg for MLAN_IOCTL_11H_CFG */
+typedef struct _mlan_ds_11h_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    union
+    {
+        /** Local power constraint for MLAN_OID_11H_LOCAL_POWER_CONSTRAINT */
+        t_s8 usr_local_power_constraint;
+#if defined(DFS_TESTING_SUPPORT)
+        /** User-configuation for MLAN_OID_11H_DFS_TESTING */
+        mlan_ds_11h_dfs_testing dfs_testing;
+#endif
+    } param;
+} mlan_ds_11h_cfg, *pmlan_ds_11h_cfg;
+
+/*-----------------------------------------------------------------*/
+/** Miscellaneous Configuration Group */
+/*-----------------------------------------------------------------*/
+
+/** CMD buffer size */
+#define MLAN_SIZE_OF_CMD_BUFFER 2048
+
+/** LDO Internal */
+#define LDO_INTERNAL            0
+/** LDO External */
+#define LDO_EXTERNAL            1
+
+/** Enumeration for IE type */
+enum _mlan_ie_type
+{
+    MLAN_IE_TYPE_GEN_IE = 0,
+#ifdef STA_SUPPORT
+    MLAN_IE_TYPE_ARP_FILTER,
+#endif /* STA_SUPPORT */
+};
+
+/** Type definition of mlan_ds_misc_gen_ie for MLAN_OID_MISC_GEN_IE */
+typedef struct _mlan_ds_misc_gen_ie
+{
+    /** IE type */
+    t_u32 type;
+    /** IE length */
+    t_u32 len;
+    /** IE buffer */
+    t_u8 ie_data[MAX_IE_SIZE];
+} mlan_ds_misc_gen_ie;
+
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+/** Type definition of mlan_ds_misc_sdio_mpa_ctrl for MLAN_OID_MISC_SDIO_MPA_CTRL */
+typedef struct _mlan_ds_misc_sdio_mpa_ctrl
+{
+    /** SDIO MP-A TX enable/disable */
+    t_u16 tx_enable;
+    /** SDIO MP-A RX enable/disable */
+    t_u16 rx_enable;
+    /** SDIO MP-A TX buf size */
+    t_u16 tx_buf_size;
+    /** SDIO MP-A RX buf size */
+    t_u16 rx_buf_size;
+    /** SDIO MP-A TX Max Ports */
+    t_u16 tx_max_ports;
+    /** SDIO MP-A RX Max Ports */
+    t_u16 rx_max_ports;
+} mlan_ds_misc_sdio_mpa_ctrl;
+#endif
+
+/** Type definition of mlan_ds_misc_cmd for MLAN_OID_MISC_HOST_CMD */
+typedef struct _mlan_ds_misc_cmd
+{
+    /** Command length */
+    t_u32 len;
+    /** Command buffer */
+    t_u8 cmd[MLAN_SIZE_OF_CMD_BUFFER];
+} mlan_ds_misc_cmd;
+
+/** Maximum number of system clocks */
+#define MLAN_MAX_CLK_NUM     	16
+
+/** Clock type : Configurable */
+#define MLAN_CLK_CONFIGURABLE	0
+/** Clock type : Supported */
+#define MLAN_CLK_SUPPORTED   	1
+
+/** Type definition of mlan_ds_misc_sys_clock for MLAN_OID_MISC_SYS_CLOCK */
+typedef struct _mlan_ds_misc_sys_clock
+{
+    /** Current system clock */
+    t_u16 cur_sys_clk;
+    /** Clock type */
+    t_u16 sys_clk_type;
+    /** Number of clocks */
+    t_u16 sys_clk_num;
+    /** System clocks */
+    t_u16 sys_clk[MLAN_MAX_CLK_NUM];
+} mlan_ds_misc_sys_clock;
+
+/** Enumeration for function init/shutdown */
+enum _mlan_func_cmd
+{
+    MLAN_FUNC_INIT = 1,
+    MLAN_FUNC_SHUTDOWN,
+};
+
+/** Type definition of mlan_ds_misc_tx_datapause for MLAN_OID_MISC_TX_DATAPAUSE */
+typedef struct _mlan_ds_misc_tx_datapause
+{
+    /** Tx data pause flag */
+    t_u16 tx_pause;
+    /** Max number of Tx buffers for all PS clients */
+    t_u16 tx_buf_cnt;
+} mlan_ds_misc_tx_datapause;
+
+/** IP address length */
+#define IPADDR_LEN                  (16)
+/** Max number of ip */
+#define MAX_IPADDR                  (4)
+/** IP address type - IPv4*/
+#define IPADDR_TYPE_IPV4            (1)
+/** IP operation remove */
+#define MLAN_IPADDR_OP_IP_REMOVE    (0)
+/** IP operation ARP filter */
+#define MLAN_IPADDR_OP_ARP_FILTER   MBIT(0)
+/** IP operation ARP response */
+#define MLAN_IPADDR_OP_AUTO_ARP_RESP    MBIT(1)
+
+/** Type definition of mlan_ds_misc_ipaddr_cfg for MLAN_OID_MISC_IP_ADDR */
+typedef struct _mlan_ds_misc_ipaddr_cfg
+{
+    /** Operation code */
+    t_u32 op_code;
+    /** IP address type */
+    t_u32 ip_addr_type;
+    /** Number of IP */
+    t_u32 ip_addr_num;
+    /** IP address */
+    t_u8 ip_addr[MAX_IPADDR][IPADDR_LEN];
+} mlan_ds_misc_ipaddr_cfg;
+
+/* MEF configuration disable */
+#define MEF_CFG_DISABLE             0
+/* MEF configuration Rx filter enable */
+#define MEF_CFG_RX_FILTER_ENABLE    1
+/* MEF configuration auto ARP response */
+#define MEF_CFG_AUTO_ARP_RESP       2
+/* MEF configuration host command */
+#define MEF_CFG_HOSTCMD             0xFFFF
+
+/** Type definition of mlan_ds_misc_mef_cfg for MLAN_OID_MISC_MEF_CFG */
+typedef struct _mlan_ds_misc_mef_cfg
+{
+    /** Sub-ID for operation */
+    t_u32 sub_id;
+    /** Parameter according to sub-ID */
+    union
+    {
+        /** MEF command buffer for MEF_CFG_HOSTCMD */
+        mlan_ds_misc_cmd cmd_buf;
+    } param;
+} mlan_ds_misc_mef_cfg;
+
+/** Type definition of mlan_ds_misc_cfp_code for MLAN_OID_MISC_CFP_CODE */
+typedef struct _mlan_ds_misc_cfp_code
+{
+    /** CFP table code for 2.4GHz */
+    t_u32 cfp_code_bg;
+    /** CFP table code for 5GHz */
+    t_u32 cfp_code_a;
+} mlan_ds_misc_cfp_code;
+
+/** Type definition of mlan_ds_misc_country_code for MLAN_OID_MISC_COUNTRY_CODE */
+typedef struct _mlan_ds_misc_country_code
+{
+    /** Country Code */
+    t_u8 country_code[COUNTRY_CODE_LEN];
+} mlan_ds_misc_country_code;
+
+/** BITMAP for subscribe event rssi low */
+#define SUBSCRIBE_EVT_RSSI_LOW  		MBIT(0)
+/** BITMAP for subscribe event snr low */
+#define SUBSCRIBE_EVT_SNR_LOW			MBIT(1)
+/** BITMAP for subscribe event max fail */
+#define SUBSCRIBE_EVT_MAX_FAIL			MBIT(2)
+/** BITMAP for subscribe event beacon missed */
+#define SUBSCRIBE_EVT_BEACON_MISSED 	MBIT(3)
+/** BITMAP for subscribe event rssi high */
+#define SUBSCRIBE_EVT_RSSI_HIGH			MBIT(4)
+/** BITMAP for subscribe event snr high */
+#define SUBSCRIBE_EVT_SNR_HIGH			MBIT(5)
+/** BITMAP for subscribe event data rssi low */
+#define SUBSCRIBE_EVT_DATA_RSSI_LOW		MBIT(6)
+/** BITMAP for subscribe event data snr low */
+#define SUBSCRIBE_EVT_DATA_SNR_LOW		MBIT(7)
+/** BITMAP for subscribe event data rssi high */
+#define SUBSCRIBE_EVT_DATA_RSSI_HIGH	MBIT(8)
+/** BITMAP for subscribe event data snr high */
+#define SUBSCRIBE_EVT_DATA_SNR_HIGH		MBIT(9)
+/** BITMAP for subscribe event link quality */
+#define SUBSCRIBE_EVT_LINK_QUALITY		MBIT(10)
+/** BITMAP for subscribe event pre_beacon_lost */
+#define SUBSCRIBE_EVT_PRE_BEACON_LOST	MBIT(11)
+/** default PRE_BEACON_MISS_COUNT */
+#define DEFAULT_PRE_BEACON_MISS			30
+
+/** Type definition of mlan_ds_subscribe_evt for MLAN_OID_MISC_CFP_CODE */
+typedef struct _mlan_ds_subscribe_evt
+{
+    /** bitmap for subscribe event */
+    t_u16 evt_bitmap;
+    /** Absolute value of RSSI threshold value (dBm) */
+    t_u8 low_rssi;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 low_rssi_freq;
+    /** SNR threshold value (dB) */
+    t_u8 low_snr;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 low_snr_freq;
+    /** Failure count threshold */
+    t_u8 failure_count;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 failure_count_freq;
+    /** num of missed beacons */
+    t_u8 beacon_miss;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 beacon_miss_freq;
+    /** Absolute value of RSSI threshold value (dBm) */
+    t_u8 high_rssi;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 high_rssi_freq;
+    /** SNR threshold value (dB) */
+    t_u8 high_snr;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 high_snr_freq;
+    /** Absolute value of data RSSI threshold value (dBm) */
+    t_u8 data_low_rssi;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 data_low_rssi_freq;
+    /** Absolute value of data SNR threshold value (dBm) */
+    t_u8 data_low_snr;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 data_low_snr_freq;
+    /** Absolute value of data RSSI threshold value (dBm) */
+    t_u8 data_high_rssi;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 data_high_rssi_freq;
+    /** Absolute value of data SNR threshold value (dBm) */
+    t_u8 data_high_snr;
+    /** 0--report once, 1--report everytime happen, N -- report only happend > N consecutive times */
+    t_u8 data_high_snr_freq;
+    /* Link SNR threshold (dB) */
+    t_u16 link_snr;
+    /* Link SNR frequency */
+    t_u16 link_snr_freq;
+    /* Second minimum rate value as per the rate table below */
+    t_u16 link_rate;
+    /* Second minimum rate frequency */
+    t_u16 link_rate_freq;
+    /* Tx latency value (us) */
+    t_u16 link_tx_latency;
+    /* Tx latency frequency */
+    t_u16 link_tx_lantency_freq;
+    /* Number of pre missed beacons */
+    t_u8 pre_beacon_miss;
+} mlan_ds_subscribe_evt;
+
+/** Max OTP user data length */
+#define MAX_OTP_USER_DATA_LEN	252
+
+/** Type definition of mlan_ds_misc_otp_user_data for MLAN_OID_MISC_OTP_USER_DATA */
+typedef struct _mlan_ds_misc_otp_user_data
+{
+    /** Reserved */
+    t_u16 reserved;
+    /** OTP user data length */
+    t_u16 user_data_length;
+    /** User data buffer */
+    t_u8 user_data[MAX_OTP_USER_DATA_LEN];
+} mlan_ds_misc_otp_user_data;
+
+/** Type definition of mlan_ds_misc_cfg for MLAN_IOCTL_MISC_CFG */
+typedef struct _mlan_ds_misc_cfg
+{
+    /** Sub-command */
+    t_u32 sub_command;
+    /** Miscellaneous configuration parameter */
+    union
+    {
+        /** Generic IE for MLAN_OID_MISC_GEN_IE */
+        mlan_ds_misc_gen_ie gen_ie;
+        /** Region code for MLAN_OID_MISC_REGION */
+        t_u32 region_code;
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+        /** SDIO MP-A Ctrl command for MLAN_OID_MISC_SDIO_MPA_CTRL */
+        mlan_ds_misc_sdio_mpa_ctrl mpa_ctrl;
+#endif
+        /** Hostcmd for MLAN_OID_MISC_HOST_CMD */
+        mlan_ds_misc_cmd hostcmd;
+        /** System clock for MLAN_OID_MISC_SYS_CLOCK */
+        mlan_ds_misc_sys_clock sys_clock;
+        /** WWS set/get for MLAN_OID_MISC_WWS */
+        t_u32 wws_cfg;
+        /** Function init/shutdown for MLAN_OID_MISC_INIT_SHUTDOWN */
+        t_u32 func_init_shutdown;
+        /** Custom IE for MLAN_OID_MISC_CUSTOM_IE */
+        mlan_ds_misc_custom_ie cust_ie;
+        /** Tx data pause for MLAN_OID_MISC_TX_DATAPAUSE */
+        mlan_ds_misc_tx_datapause tx_datapause;
+        /** IP address configuration */
+        mlan_ds_misc_ipaddr_cfg ipaddr_cfg;
+        /** MAC control for MLAN_OID_MISC_MAC_CONTROL */
+        t_u32 mac_ctrl;
+        /** MEF configuration for MLAN_OID_MISC_MEF_CFG */
+        mlan_ds_misc_mef_cfg mef_cfg;
+        /** CFP code for MLAN_OID_MISC_CFP_CODE */
+        mlan_ds_misc_cfp_code cfp_code;
+        /** Country code for MLAN_OID_MISC_COUNTRY_CODE */
+        mlan_ds_misc_country_code country_code;
+        /** Thermal reading for MLAN_OID_MISC_THERMAL */
+        t_u32 thermal;
+        /** Mgmt subtype mask for MLAN_OID_MISC_RX_MGMT_IND */
+        t_u32 mgmt_subtype_mask;
+        /** subscribe event for MLAN_OID_MISC_SUBSCRIBE_EVENT */
+        mlan_ds_subscribe_evt subscribe_event;
+#ifdef DEBUG_LEVEL1
+        /** Driver debug bit masks */
+        t_u32 drvdbg;
+#endif
+        mlan_ds_misc_otp_user_data otp_user_data;
+    } param;
+} mlan_ds_misc_cfg, *pmlan_ds_misc_cfg;
+
+#endif /* !_MLAN_IOCTL_H_ */
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_cfg80211.c b/drivers/net/wireless/sd8797/mlinux/moal_cfg80211.c
new file mode 100644
index 000000000000..6b2a01f5c218
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_cfg80211.c
@@ -0,0 +1,1687 @@
+/** @file moal_cfg80211.c
+  *
+  * @brief This file contains the functions for CFG80211. 
+  *
+  * Copyright (C) 2011-2012, Marvell International Ltd. 
+  * 
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+
+#include    "moal_cfg80211.h"
+#ifdef UAP_CFG80211
+#ifdef UAP_SUPPORT
+#include    "moal_uap.h"
+#endif
+#endif
+
+/********************************************************
+				Local Variables
+********************************************************/
+/** Supported rates to be advertised to the cfg80211 */
+static struct ieee80211_rate cfg80211_rates[] = {
+    {.bitrate = 10,.hw_value = 2,},
+    {.bitrate = 20,.hw_value = 4,},
+    {.bitrate = 55,.hw_value = 11},
+    {.bitrate = 110,.hw_value = 22,},
+    {.bitrate = 220,.hw_value = 44,},
+    {.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,},
+    {.bitrate = 720,.hw_value = 144,},
+};
+
+/** Channel definitions for 2 GHz to be advertised to cfg80211 */
+static struct ieee80211_channel cfg80211_channels_2ghz[] = {
+    {.center_freq = 2412,.hw_value = 1,.max_power = 20},
+    {.center_freq = 2417,.hw_value = 2,.max_power = 20},
+    {.center_freq = 2422,.hw_value = 3,.max_power = 20},
+    {.center_freq = 2427,.hw_value = 4,.max_power = 20},
+    {.center_freq = 2432,.hw_value = 5,.max_power = 20},
+    {.center_freq = 2437,.hw_value = 6,.max_power = 20},
+    {.center_freq = 2442,.hw_value = 7,.max_power = 20},
+    {.center_freq = 2447,.hw_value = 8,.max_power = 20},
+    {.center_freq = 2452,.hw_value = 9,.max_power = 20},
+    {.center_freq = 2457,.hw_value = 10,.max_power = 20},
+    {.center_freq = 2462,.hw_value = 11,.max_power = 20},
+    {.center_freq = 2467,.hw_value = 12,.max_power = 20},
+    {.center_freq = 2472,.hw_value = 13,.max_power = 20},
+    {.center_freq = 2484,.hw_value = 14,.max_power = 20},
+};
+
+/** Channel definitions for 5 GHz to be advertised to cfg80211 */
+static struct ieee80211_channel cfg80211_channels_5ghz[] = {
+    {.center_freq = 5040,.hw_value = 8,.max_power = 20},
+    {.center_freq = 5060,.hw_value = 12,.max_power = 20},
+    {.center_freq = 5080,.hw_value = 16,.max_power = 20},
+    {.center_freq = 5170,.hw_value = 34,.max_power = 20},
+    {.center_freq = 5190,.hw_value = 38,.max_power = 20},
+    {.center_freq = 5210,.hw_value = 42,.max_power = 20},
+    {.center_freq = 5230,.hw_value = 46,.max_power = 20},
+    {.center_freq = 5180,.hw_value = 36,.max_power = 20},
+    {.center_freq = 5200,.hw_value = 40,.max_power = 20},
+    {.center_freq = 5220,.hw_value = 44,.max_power = 20},
+    {.center_freq = 5240,.hw_value = 48,.max_power = 20},
+    {.center_freq = 5260,.hw_value = 52,.max_power = 20},
+    {.center_freq = 5280,.hw_value = 56,.max_power = 20},
+    {.center_freq = 5300,.hw_value = 60,.max_power = 20},
+    {.center_freq = 5320,.hw_value = 64,.max_power = 20},
+    {.center_freq = 5500,.hw_value = 100,.max_power = 20},
+    {.center_freq = 5520,.hw_value = 104,.max_power = 20},
+    {.center_freq = 5540,.hw_value = 108,.max_power = 20},
+    {.center_freq = 5560,.hw_value = 112,.max_power = 20},
+    {.center_freq = 5580,.hw_value = 116,.max_power = 20},
+    {.center_freq = 5600,.hw_value = 120,.max_power = 20},
+    {.center_freq = 5620,.hw_value = 124,.max_power = 20},
+    {.center_freq = 5640,.hw_value = 128,.max_power = 20},
+    {.center_freq = 5660,.hw_value = 132,.max_power = 20},
+    {.center_freq = 5680,.hw_value = 136,.max_power = 20},
+    {.center_freq = 5700,.hw_value = 140,.max_power = 20},
+    {.center_freq = 5745,.hw_value = 149,.max_power = 20},
+    {.center_freq = 5765,.hw_value = 153,.max_power = 20},
+    {.center_freq = 5785,.hw_value = 157,.max_power = 20},
+    {.center_freq = 5805,.hw_value = 161,.max_power = 20},
+    {.center_freq = 5825,.hw_value = 165,.max_power = 20},
+};
+
+/********************************************************
+				Global Variables
+********************************************************/
+extern int cfg80211_wext;
+
+struct ieee80211_supported_band cfg80211_band_2ghz = {
+    .channels = cfg80211_channels_2ghz,
+    .n_channels = ARRAY_SIZE(cfg80211_channels_2ghz),
+    .bitrates = cfg80211_rates,
+    .n_bitrates = ARRAY_SIZE(cfg80211_rates),
+};
+
+struct ieee80211_supported_band cfg80211_band_5ghz = {
+    .channels = cfg80211_channels_5ghz,
+    .n_channels = ARRAY_SIZE(cfg80211_channels_5ghz),
+    .bitrates = cfg80211_rates + 5,
+    .n_bitrates = ARRAY_SIZE(cfg80211_rates) - 5,
+};
+
+#ifndef WLAN_CIPHER_SUITE_SMS4
+#define WLAN_CIPHER_SUITE_SMS4      0x00000020
+#endif
+
+/* Supported crypto cipher suits to be advertised to cfg80211 */
+const u32 cfg80211_cipher_suites[] = {
+    WLAN_CIPHER_SUITE_WEP40,
+    WLAN_CIPHER_SUITE_WEP104,
+    WLAN_CIPHER_SUITE_TKIP,
+    WLAN_CIPHER_SUITE_CCMP,
+    WLAN_CIPHER_SUITE_SMS4,
+};
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+#ifdef UAP_SUPPORT
+/** Network device handlers for uAP */
+extern const struct net_device_ops woal_uap_netdev_ops;
+#endif
+#ifdef STA_SUPPORT
+/** Network device handlers for STA */
+extern const struct net_device_ops woal_netdev_ops;
+#endif
+#endif
+
+/********************************************************
+				Local Functions
+********************************************************/
+
+/********************************************************
+				Global Functions
+********************************************************/
+/**
+ * @brief Get the private structure from wiphy
+ *
+ * @param wiphy     A pointer to wiphy structure
+ *
+ * @return          Pointer to moal_private
+ */
+void *
+woal_get_wiphy_priv(struct wiphy *wiphy)
+{
+    return (void *) (*(unsigned long *) wiphy_priv(wiphy));
+}
+
+/**
+ * @brief Get the private structure from net device
+ *
+ * @param wiphy     A pointer to net_device structure
+ *
+ * @return          Pointer to moal_private
+ */
+void *
+woal_get_netdev_priv(struct net_device *dev)
+{
+    return (void *) netdev_priv(dev);
+}
+
+/**
+ *  @brief Set/Enable encryption key
+ *
+ *  @param priv             A pointer to moal_private structure
+ *  @param is_enable_wep    Enable WEP default key
+ *  @param cipher           Cipher suite selector
+ *  @param key              A pointer to key
+ *  @param key_len          Key length
+ *  @param seq              A pointer to sequence
+ *  @param seq_len          Sequence length
+ *  @param key_index        Key index
+ *  @param addr             Mac for which key is to be set
+ *  @param disable          Key disabled or not
+ *
+ *  @return                 MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_cfg80211_set_key(moal_private * priv, t_u8 is_enable_wep,
+                      t_u32 cipher, const t_u8 * key, int key_len,
+                      const t_u8 * seq, int seq_len, t_u8 key_index,
+                      const t_u8 * addr, int disable)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u8 bcast_addr[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_ENCRYPT_KEY;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    req->action = MLAN_ACT_SET;
+
+    if (is_enable_wep) {
+        sec->param.encrypt_key.key_index = key_index;
+        sec->param.encrypt_key.is_current_wep_key = MTRUE;
+    } else if (!disable) {
+#ifdef UAP_CFG80211
+#ifdef UAP_SUPPORT
+        if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP) {
+            if (key && key_len) {
+                priv->key_len = key_len;
+                memcpy(priv->key_material, key, key_len);
+                priv->cipher = cipher;
+                priv->key_index = key_index;
+            }
+            if ((cipher == WLAN_CIPHER_SUITE_WEP40) ||
+                (cipher == WLAN_CIPHER_SUITE_WEP104)) {
+                PRINTM(MIOCTL, "Set WEP key\n");
+                ret = MLAN_STATUS_SUCCESS;
+                goto done;
+            }
+        }
+#endif
+#endif
+        if (cipher != WLAN_CIPHER_SUITE_WEP40 &&
+            cipher != WLAN_CIPHER_SUITE_WEP104 &&
+            cipher != WLAN_CIPHER_SUITE_TKIP &&
+            cipher != WLAN_CIPHER_SUITE_SMS4 &&
+            cipher != WLAN_CIPHER_SUITE_CCMP) {
+            PRINTM(MERROR, "Invalid cipher suite specified\n");
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+        sec->param.encrypt_key.key_index = key_index;
+        if (key && key_len) {
+            memcpy(sec->param.encrypt_key.key_material, key, key_len);
+            sec->param.encrypt_key.key_len = key_len;
+        }
+        /* Set WAPI key */
+        if (cipher == WLAN_CIPHER_SUITE_SMS4) {
+            sec->param.encrypt_key.is_wapi_key = MTRUE;
+            if (seq_len) {
+                memcpy(sec->param.encrypt_key.pn, seq, PN_SIZE);
+                DBG_HEXDUMP(MCMD_D, "WAPI PN", sec->param.encrypt_key.pn,
+                            seq_len);
+            }
+        }
+        if (cipher != WLAN_CIPHER_SUITE_WEP40 &&
+            cipher != WLAN_CIPHER_SUITE_WEP104) {
+            if (addr) {
+                memcpy(sec->param.encrypt_key.mac_addr, addr, ETH_ALEN);
+                if (0 ==
+                    memcmp(sec->param.encrypt_key.mac_addr, bcast_addr,
+                           ETH_ALEN))
+                    sec->param.encrypt_key.key_flags = KEY_FLAG_GROUP_KEY;
+                else
+                    sec->param.encrypt_key.key_flags = KEY_FLAG_SET_TX_KEY;
+            } else {
+                memcpy(sec->param.encrypt_key.mac_addr, bcast_addr, ETH_ALEN);
+                sec->param.encrypt_key.key_flags = KEY_FLAG_GROUP_KEY;
+            }
+            if (seq && seq_len) {
+                memcpy(sec->param.encrypt_key.pn, seq, seq_len);
+                sec->param.encrypt_key.key_flags |= KEY_FLAG_RX_SEQ_VALID;
+            }
+        }
+    } else {
+        if (key_index == KEY_INDEX_CLEAR_ALL)
+            sec->param.encrypt_key.key_disable = MTRUE;
+        else {
+            sec->param.encrypt_key.key_remove = MTRUE;
+            sec->param.encrypt_key.key_index = key_index;
+        }
+        sec->param.encrypt_key.key_flags = KEY_FLAG_REMOVE_KEY;
+        if (addr)
+            memcpy(sec->param.encrypt_key.mac_addr, addr, ETH_ALEN);
+    }
+
+    /* Send IOCTL request to MLAN */
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+  done:
+    if (req && (ret != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Set/Enable the WEP key to driver
+ *
+ * @param priv      A pointer to moal_private structure
+ * @param key       A pointer to key data
+ * @param key_len   Length of the key data
+ * @param index     Key index
+ *
+ * @return          MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_cfg80211_set_wep_keys(moal_private * priv, const t_u8 * key, int key_len,
+                           t_u8 index)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u32 cipher = 0;
+
+    ENTER();
+
+    if (key_len) {
+        if (key_len == 5)
+            cipher = WLAN_CIPHER_SUITE_WEP40;
+        else
+            cipher = WLAN_CIPHER_SUITE_WEP104;
+        ret =
+            woal_cfg80211_set_key(priv, 0, cipher, key, key_len, NULL, 0, index,
+                                  NULL, 0);
+    } else {
+        /* No key provided so it is enable key. We want to just set the
+           transmit key index */
+        woal_cfg80211_set_key(priv, 1, cipher, key, key_len, NULL, 0, index,
+                              NULL, 0);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+/**
+ * @brief set bss role
+ *
+ * @param priv              A pointer to moal private structure
+ * @param action            Action: set or get
+ * @param role              A pointer to bss role
+ *
+ * @return                  0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_bss_role_cfg(moal_private * priv, t_u16 action, t_u8 * bss_role)
+{
+    int ret = 0;
+
+    ENTER();
+
+    if (action == MLAN_ACT_SET) {
+        /* Reset interface */
+        woal_reset_intf(priv, MOAL_IOCTL_WAIT, MFALSE);
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_bss_role_cfg(priv, action, MOAL_IOCTL_WAIT, bss_role)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (action == MLAN_ACT_SET) {
+        /* Initialize private structures */
+        woal_init_priv(priv, MOAL_IOCTL_WAIT);
+
+        /* Enable interfaces */
+        netif_device_attach(priv->netdev);
+        woal_start_queue(priv->netdev);
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief initialize p2p client for wpa_supplicant
+ *
+ * @param priv			A pointer to moal private structure
+ *
+ * @return              0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_init_p2p_client(moal_private * priv)
+{
+    int ret = MLAN_STATUS_SUCCESS;
+    t_u16 wifi_direct_mode = WIFI_DIRECT_MODE_DISABLE;
+    t_u8 bss_role;
+
+    ENTER();
+
+    /* bss type check */
+    if (priv->bss_type != MLAN_BSS_TYPE_WIFIDIRECT) {
+        PRINTM(MERROR, "Unexpected bss type when init p2p client\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* get the bss role */
+    if (MLAN_STATUS_SUCCESS !=
+        woal_cfg80211_bss_role_cfg(priv, MLAN_ACT_GET, &bss_role)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (bss_role != MLAN_BSS_ROLE_STA) {
+        bss_role = MLAN_BSS_ROLE_STA;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_cfg80211_bss_role_cfg(priv, MLAN_ACT_SET, &bss_role)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+    wifi_direct_mode = WIFI_DIRECT_MODE_DISABLE;
+    if (MLAN_STATUS_SUCCESS !=
+        woal_wifi_direct_mode_cfg(priv, MLAN_ACT_SET, &wifi_direct_mode)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* first, init wifi direct to listen mode */
+    wifi_direct_mode = WIFI_DIRECT_MODE_LISTEN;
+    if (MLAN_STATUS_SUCCESS !=
+        woal_wifi_direct_mode_cfg(priv, MLAN_ACT_SET, &wifi_direct_mode)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* second, init wifi direct client */
+    wifi_direct_mode = WIFI_DIRECT_MODE_CLIENT;
+    if (MLAN_STATUS_SUCCESS !=
+        woal_wifi_direct_mode_cfg(priv, MLAN_ACT_SET, &wifi_direct_mode)) {
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief initialize p2p GO for wpa_supplicant
+ *
+ * @param priv			A pointer to moal private structure
+ *
+ * @return              0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_init_p2p_go(moal_private * priv)
+{
+    int ret = MLAN_STATUS_SUCCESS;
+    t_u16 wifi_direct_mode;
+    t_u8 bss_role;
+
+    ENTER();
+
+    /* bss type check */
+    if (priv->bss_type != MLAN_BSS_TYPE_WIFIDIRECT) {
+        PRINTM(MERROR, "Unexpected bss type when init p2p GO\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    wifi_direct_mode = WIFI_DIRECT_MODE_DISABLE;
+    if (MLAN_STATUS_SUCCESS !=
+        woal_wifi_direct_mode_cfg(priv, MLAN_ACT_SET, &wifi_direct_mode)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* first, init wifi direct to listen mode */
+    wifi_direct_mode = WIFI_DIRECT_MODE_LISTEN;
+    if (MLAN_STATUS_SUCCESS !=
+        woal_wifi_direct_mode_cfg(priv, MLAN_ACT_SET, &wifi_direct_mode)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* second, init wifi direct to GO mode */
+    wifi_direct_mode = WIFI_DIRECT_MODE_GO;
+    if (MLAN_STATUS_SUCCESS !=
+        woal_wifi_direct_mode_cfg(priv, MLAN_ACT_SET, &wifi_direct_mode)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* get the bss role, and set it to uAP */
+    if (MLAN_STATUS_SUCCESS !=
+        woal_cfg80211_bss_role_cfg(priv, MLAN_ACT_GET, &bss_role)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (bss_role != MLAN_BSS_ROLE_UAP) {
+        bss_role = MLAN_BSS_ROLE_UAP;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_cfg80211_bss_role_cfg(priv, MLAN_ACT_SET, &bss_role)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief reset bss role and wifi direct mode for wpa_supplicant
+ *
+ * @param priv			A pointer to moal private structure
+ *
+ * @return              0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_deinit_p2p(moal_private * priv)
+{
+    int ret = MLAN_STATUS_SUCCESS;
+    t_u16 wifi_direct_mode;
+    t_u8 bss_role;
+    t_u8 channel_status;
+
+    ENTER();
+
+    /* bss type check */
+    if (priv->bss_type != MLAN_BSS_TYPE_WIFIDIRECT) {
+        PRINTM(MERROR, "Unexpected bss type when deinit p2p\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* cancel previous remain on channel */
+    if (priv->phandle->remain_on_channel) {
+        if (woal_cfg80211_remain_on_channel_cfg
+            (priv, MOAL_IOCTL_WAIT, MTRUE, &channel_status, NULL, 0, 0)) {
+            PRINTM(MERROR, "Fail to cancel remain on channel\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if (priv->phandle->cookie) {
+            cfg80211_remain_on_channel_expired(priv->netdev,
+                                               priv->phandle->cookie,
+                                               &priv->phandle->chan,
+                                               priv->phandle->channel_type,
+                                               GFP_ATOMIC);
+            priv->phandle->cookie = 0;
+        }
+        priv->phandle->remain_on_channel = MFALSE;
+    }
+
+    /* get the bss role */
+    if (MLAN_STATUS_SUCCESS != woal_cfg80211_bss_role_cfg(priv,
+                                                          MLAN_ACT_GET,
+                                                          &bss_role)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* reset bss role */
+    if (bss_role != MLAN_BSS_ROLE_STA) {
+        bss_role = MLAN_BSS_ROLE_STA;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_cfg80211_bss_role_cfg(priv, MLAN_ACT_SET, &bss_role)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+    wifi_direct_mode = WIFI_DIRECT_MODE_DISABLE;
+    if (MLAN_STATUS_SUCCESS !=
+        woal_wifi_direct_mode_cfg(priv, MLAN_ACT_SET, &wifi_direct_mode)) {
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+#endif /* KERNEL_VERSION */
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+
+/**
+ * @brief Request the driver to change the interface type
+ *
+ * @param wiphy         A pointer to wiphy structure
+ * @param dev           A pointer to net_device structure
+ * @param type          Virtual interface types
+ * @param flags         Flags
+ * @param params        A pointer to vif_params structure
+ *
+ * @return              0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_change_virtual_intf(struct wiphy *wiphy,
+                                  struct net_device *dev,
+                                  enum nl80211_iftype type, u32 * flags,
+                                  struct vif_params *params)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+    mlan_ds_bss *bss = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    if (priv->wdev->iftype == type) {
+        PRINTM(MINFO, "Already set to required type\n");
+        goto done;
+    }
+    PRINTM(MIOCTL, "change virturl intf=%d\n", type);
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+        /** cancel previous remain on channel to avoid firmware hang */
+    if (priv->phandle->remain_on_channel) {
+        t_u8 channel_status;
+        if (woal_cfg80211_remain_on_channel_cfg
+            (priv, MOAL_IOCTL_WAIT, MTRUE, &channel_status, NULL, 0, 0)) {
+            PRINTM(MERROR, "Fail to cancel remain on channel\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if (priv->phandle->cookie) {
+            cfg80211_remain_on_channel_expired(priv->netdev,
+                                               priv->phandle->cookie,
+                                               &priv->phandle->chan,
+                                               priv->phandle->channel_type,
+                                               GFP_ATOMIC);
+            priv->phandle->cookie = 0;
+        }
+        priv->phandle->remain_on_channel = MFALSE;
+    }
+#endif
+#endif
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_BSS_MODE;
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = MLAN_ACT_SET;
+
+    switch (type) {
+    case NL80211_IFTYPE_ADHOC:
+        bss->param.bss_mode = MLAN_BSS_MODE_IBSS;
+        priv->wdev->iftype = NL80211_IFTYPE_ADHOC;
+        PRINTM(MINFO, "Setting interface type to adhoc\n");
+        break;
+    case NL80211_IFTYPE_STATION:
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+        if (priv->bss_type == MLAN_BSS_TYPE_WIFIDIRECT
+            && (priv->wdev->iftype == NL80211_IFTYPE_AP
+                || priv->wdev->iftype == NL80211_IFTYPE_P2P_GO
+                || priv->wdev->iftype == NL80211_IFTYPE_P2P_CLIENT)) {
+            /* if we support wifi direct && priv->bss_type == wifi_direct, and
+               currently the interface type is AP or GO or client, that means
+               wpa_supplicant deinit() wifi direct interface, so we should
+               deinit bss_role and wifi direct mode, for other bss_type, we
+               should not update bss_role and wifi direct mode */
+
+            if (MLAN_STATUS_SUCCESS != woal_cfg80211_deinit_p2p(priv)) {
+                ret = -EFAULT;
+                goto done;
+            }
+        }
+#endif /* KERNEL_VERSION */
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+
+        bss->param.bss_mode = MLAN_BSS_MODE_INFRA;
+        priv->wdev->iftype = NL80211_IFTYPE_STATION;
+        PRINTM(MINFO, "Setting interface type to managed\n");
+        break;
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+    case NL80211_IFTYPE_P2P_CLIENT:
+
+        if (MLAN_STATUS_SUCCESS != woal_cfg80211_init_p2p_client(priv)) {
+            ret = -EFAULT;
+            goto done;
+        }
+
+        bss->param.bss_mode = MLAN_BSS_MODE_INFRA;
+        priv->wdev->iftype = NL80211_IFTYPE_P2P_CLIENT;
+        PRINTM(MINFO, "Setting interface type to P2P client\n");
+
+        break;
+#endif /* KERNEL_VERSION */
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+    case NL80211_IFTYPE_AP:
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+    case NL80211_IFTYPE_P2P_GO:
+        if (priv->bss_type == MLAN_BSS_TYPE_WIFIDIRECT) {
+            if (MLAN_STATUS_SUCCESS != woal_cfg80211_init_p2p_go(priv)) {
+                ret = -EFAULT;
+                goto done;
+            }
+        }
+        if (type == NL80211_IFTYPE_P2P_GO)
+            priv->wdev->iftype = NL80211_IFTYPE_P2P_GO;
+#endif
+#endif
+        if (type == NL80211_IFTYPE_AP)
+            priv->wdev->iftype = NL80211_IFTYPE_AP;
+        PRINTM(MINFO, "Setting interface type to P2P GO\n");
+
+        /* there is no need for P2P GO to set bss_mode */
+        goto done;
+
+        break;
+
+    case NL80211_IFTYPE_UNSPECIFIED:
+        bss->param.bss_mode = MLAN_BSS_MODE_AUTO;
+        priv->wdev->iftype = NL80211_IFTYPE_STATION;
+        PRINTM(MINFO, "Setting interface type to auto\n");
+        break;
+    default:
+        ret = -EINVAL;
+        break;
+    }
+    if (ret)
+        goto done;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Request the driver to change the value of fragment
+ * threshold or rts threshold or retry limit
+ *
+ * @param wiphy         A pointer to wiphy structure
+ * @param changed       Change flags
+ *
+ * @return              0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed)
+{
+    moal_private *priv = (moal_private *) woal_get_wiphy_priv(wiphy);
+#ifdef UAP_CFG80211
+#ifdef UAP_SUPPORT
+    mlan_uap_bss_param sys_cfg;
+#endif
+#endif
+    int frag_thr = wiphy->frag_threshold;
+    int rts_thr = wiphy->frag_threshold;
+    int retry = wiphy->retry_long;
+
+    ENTER();
+
+    if (rts_thr == MLAN_FRAG_RTS_DISABLED)
+        rts_thr = MLAN_RTS_MAX_VALUE;
+    if (frag_thr == MLAN_FRAG_RTS_DISABLED)
+        frag_thr = MLAN_FRAG_MAX_VALUE;
+
+#ifdef UAP_CFG80211
+#ifdef UAP_SUPPORT
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP) {
+        /* Initialize the invalid values so that the correct values below are
+           downloaded to firmware */
+        woal_set_sys_config_invalid_data(&sys_cfg);
+        sys_cfg.frag_threshold = frag_thr;
+        sys_cfg.rts_threshold = rts_thr;
+        sys_cfg.retry_limit = retry;
+
+        if ((changed & WIPHY_PARAM_RTS_THRESHOLD) ||
+            (changed & WIPHY_PARAM_FRAG_THRESHOLD) ||
+            (changed & (WIPHY_PARAM_RETRY_LONG | WIPHY_PARAM_RETRY_SHORT))) {
+            if (woal_set_get_sys_config(priv,
+                                        MLAN_ACT_SET, MOAL_IOCTL_WAIT,
+                                        &sys_cfg))
+                goto fail;
+        }
+    }
+#endif
+#endif
+
+#ifdef STA_CFG80211
+#ifdef STA_SUPPORT
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) {
+        if (changed & WIPHY_PARAM_RTS_THRESHOLD) {
+            if (woal_set_get_rts(priv, MLAN_ACT_SET, MOAL_IOCTL_WAIT, &rts_thr))
+                goto fail;
+        }
+        if (changed & WIPHY_PARAM_FRAG_THRESHOLD) {
+            if (woal_set_get_frag
+                (priv, MLAN_ACT_SET, MOAL_IOCTL_WAIT, &frag_thr))
+                goto fail;
+        }
+        if (changed & (WIPHY_PARAM_RETRY_LONG | WIPHY_PARAM_RETRY_SHORT))
+            if (woal_set_get_retry(priv, MLAN_ACT_SET, MOAL_IOCTL_WAIT, &retry))
+                goto fail;
+    }
+#endif
+#endif
+
+    LEAVE();
+    return 0;
+
+  fail:
+    PRINTM(MERROR, "Failed to change wiphy params %x\n", changed);
+    LEAVE();
+    return -EFAULT;
+}
+
+/**
+ * @brief Request the driver to add a key
+ *
+ * @param wiphy         A pointer to wiphy structure
+ * @param netdev        A pointer to net_device structure
+ * @param key_index     Key index
+ * @param pairwise      Flag to indicate pairwise or group (for kernel > 2.6.36)
+ * @param mac_addr      MAC address (NULL for group key)
+ * @param params        A pointer to key_params structure
+ *
+ * @return              0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_add_key(struct wiphy *wiphy, struct net_device *netdev,
+                      t_u8 key_index,
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,36) || defined(COMPAT_WIRELESS)
+                      bool pairwise,
+#endif
+                      const t_u8 * mac_addr, struct key_params *params)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(netdev);
+
+    ENTER();
+
+    if (woal_cfg80211_set_key(priv, 0, params->cipher, params->key,
+                              params->key_len, params->seq, params->seq_len,
+                              key_index, mac_addr, 0)) {
+        PRINTM(MERROR, "Error adding the crypto keys\n");
+        LEAVE();
+        return -EFAULT;
+    }
+
+    PRINTM(MINFO, "Crypto keys added\n");
+
+    LEAVE();
+    return 0;
+}
+
+/**
+ * @brief Request the driver to delete a key
+ *
+ * @param wiphy         A pointer to wiphy structure
+ * @param netdev        A pointer to net_device structure
+ * @param key_index     Key index
+ * @param pairwise      Flag to indicate pairwise or group (for kernel > 2.6.36)
+ * @param mac_addr      MAC address (NULL for group key)
+ *
+ * @return              0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_del_key(struct wiphy *wiphy, struct net_device *netdev,
+                      t_u8 key_index,
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,36) || defined(COMPAT_WIRELESS)
+                      bool pairwise,
+#endif
+                      const t_u8 * mac_addr)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(netdev);
+
+    ENTER();
+
+    if (woal_cfg80211_set_key(priv, 0, 0, NULL, 0, NULL, 0, key_index,
+                              mac_addr, 1)) {
+        PRINTM(MERROR, "Error deleting the crypto keys\n");
+        LEAVE();
+        return -EFAULT;
+    }
+
+    PRINTM(MINFO, "Crypto keys deleted\n");
+    LEAVE();
+    return 0;
+}
+
+/**
+ * @brief Request to enable WEP key to driver
+ *
+ * @param wiphy         A pointer to wiphy structure
+ * @param netdev        A pointer to net_device structure
+ * @param key_index     Key index
+ * @param ucast         Unicast flag (for kernel > 2.6.37)
+ * @param mcast         Multicast flag (for kernel > 2.6.37)
+ *
+ * @return              0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_set_default_key(struct wiphy *wiphy,
+                              struct net_device *netdev, t_u8 key_index
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,37) || defined(COMPAT_WIRELESS)
+                              , bool ucast, bool mcast
+#endif
+    )
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(netdev);
+    mlan_bss_info bss_info;
+
+    ENTER();
+
+    woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info);
+    if (bss_info.wep_status) {
+        LEAVE();
+        return ret;
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_cfg80211_set_wep_keys(priv, NULL, 0, key_index)) {
+        ret = -EFAULT;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Request the driver to change the channel
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param chan            A pointer to ieee80211_channel structure
+ * @param channel_type    Channel type of nl80211_channel_type
+ *
+ * @return                0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_set_channel(struct wiphy *wiphy,
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,34) || defined(COMPAT_WIRELESS)
+                          struct net_device *dev,
+#endif
+                          struct ieee80211_channel *chan,
+                          enum nl80211_channel_type channel_type)
+{
+    int ret = 0;
+    moal_private *priv = NULL;
+
+    ENTER();
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,34) || defined(COMPAT_WIRELESS)
+    if (dev)
+        priv = woal_get_netdev_priv(dev);
+    else
+#endif
+        priv = (moal_private *) woal_get_wiphy_priv(wiphy);
+#ifdef STA_CFG80211
+#ifdef STA_SUPPORT
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) {
+        if (priv->media_connected == MTRUE) {
+            PRINTM(MERROR, "This configuration is valid only when station "
+                   "is not connected\n");
+            LEAVE();
+            return -EINVAL;
+        }
+        ret = woal_set_rf_channel(priv, chan, channel_type);
+    }
+#endif
+#endif
+    priv->channel = ieee80211_frequency_to_channel(chan->center_freq);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief register/unregister mgmt frame forwarding
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param dev             A pointer to net_device structure
+ * @param frame_type      Bit mask for mgmt frame type
+ * @param reg             Register or unregister
+ *
+ * @return                0 -- success, otherwise fail
+ */
+void
+woal_cfg80211_mgmt_frame_register(struct wiphy *wiphy,
+                                  struct net_device *dev, u16 frame_type,
+                                  bool reg)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    t_u32 mgmt_subtype_mask = 0x0;
+    static t_u32 last_mgmt_subtype_mask = 0x0;
+
+    ENTER();
+#ifdef UAP_SUPPORT
+    if ((priv->bss_type == MLAN_BSS_TYPE_UAP) &&
+        (frame_type == IEEE80211_STYPE_PROBE_REQ)) {
+        LEAVE();
+        return;
+    }
+#endif
+    if (reg == MTRUE) {
+        /* set mgmt_subtype_mask based on origin value */
+        last_mgmt_subtype_mask |= BIT(frame_type >> 4);
+    } else {
+        /* clear mgmt_subtype_mask */
+        last_mgmt_subtype_mask &= ~BIT(frame_type >> 4);
+    }
+    mgmt_subtype_mask = last_mgmt_subtype_mask;
+
+    /* Notify driver that a mgmt frame type was registered. Note that this
+       callback may not sleep, and cannot run concurrently with itself. */
+    status = woal_reg_rx_mgmt_ind(priv, MLAN_ACT_SET,
+                                  &mgmt_subtype_mask, MOAL_NO_WAIT);
+
+    LEAVE();
+}
+
+/**
+ * @brief tx mgmt frame
+ *
+ * @param wiphy                 A pointer to wiphy structure
+ * @param dev                   A pointer to net_device structure
+ * @param chan                  A pointer to ieee80211_channel structure
+ * @param offchan               Off channel or not
+ * @param channel_type          Channel type
+ * @param channel_type_valid    Is channel type valid or not
+ * @param buf                   Frame buffer
+ * @param len                   Frame length
+ * @param cookie                A pointer to frame cookie
+ *
+ * @return                0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_mgmt_tx(struct wiphy *wiphy,
+                      struct net_device *dev,
+                      struct ieee80211_channel *chan, bool offchan,
+                      enum nl80211_channel_type channel_type,
+                      bool channel_type_valid, unsigned int wait,
+                      const u8 * buf, size_t len,
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0) || defined(COMPAT_WIRELESS)
+                      bool no_cck,
+#endif
+                      u64 * cookie)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+    int ret = 0;
+    pmlan_buffer pmbuf = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    t_u16 packet_len = 0;
+    t_u8 addr[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
+    t_u16 framectrl;
+    t_u32 pkt_type;
+    t_u32 tx_control;
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+    t_u8 channel_status;
+    t_u32 duration;
+#endif
+#endif
+
+    ENTER();
+
+    if (buf == NULL || len == 0) {
+        PRINTM(MERROR, "woal_cfg80211_mgmt_tx() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* frame subtype == probe response, that means we are in listen phase, so
+       we should not call remain_on_channel_cfg because remain_on_channl
+       already handled it. frame subtype == action, that means we are in
+       PD/GO negotiation, so we should call remain_on_channel_cfg in order to
+       receive action frame from peer device */
+    framectrl = ((const struct ieee80211_mgmt *) buf)->frame_control;
+    PRINTM(MIOCTL, "Mgmt: framectrl=0x%x\n", framectrl);
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+    if ((priv->bss_type == MLAN_BSS_TYPE_WIFIDIRECT) &&
+        (framectrl == IEEE80211_STYPE_ACTION)) {
+#define MGMT_TX_DEFAULT_WAIT_TIME	   2000
+                /** cancel previous remain on channel */
+        if (priv->phandle->remain_on_channel) {
+            if (woal_cfg80211_remain_on_channel_cfg
+                (priv, MOAL_IOCTL_WAIT, MTRUE, &channel_status, NULL, 0, 0)) {
+                PRINTM(MERROR, "Fail to cancel remain on channel\n");
+                ret = -EFAULT;
+                goto done;
+            }
+            if (priv->phandle->cookie) {
+                cfg80211_remain_on_channel_expired(priv->netdev,
+                                                   priv->phandle->cookie,
+                                                   &priv->phandle->chan,
+                                                   priv->phandle->channel_type,
+                                                   GFP_ATOMIC);
+                priv->phandle->cookie = 0;
+            }
+            priv->phandle->remain_on_channel = MFALSE;
+        }
+        duration = wait;
+        if (!wait)
+            duration = MGMT_TX_DEFAULT_WAIT_TIME;
+        if (channel_type_valid)
+            ret =
+                woal_cfg80211_remain_on_channel_cfg(priv, MOAL_IOCTL_WAIT,
+                                                    MFALSE, &channel_status,
+                                                    chan, channel_type,
+                                                    duration);
+        else
+            ret =
+                woal_cfg80211_remain_on_channel_cfg(priv, MOAL_IOCTL_WAIT,
+                                                    MFALSE, &channel_status,
+                                                    chan, 0, duration);
+        if (ret) {
+            PRINTM(MERROR, "Fail to configure remain on channel\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        priv->phandle->remain_on_channel = MTRUE;
+        priv->phandle->channel_type = channel_type;
+        memcpy(&priv->phandle->chan, chan, sizeof(struct ieee80211_channel));
+        PRINTM(MIOCTL, "Mgmt Tx: Set remain channel=%d\n",
+               ieee80211_frequency_to_channel(chan->center_freq));
+    }
+#endif
+#endif
+#define MRVL_PKT_TYPE_MGMT_FRAME 0xE5
+    /* pkt_type + tx_control */
+#define HEADER_SIZE				8
+    packet_len = (t_u16) len + MLAN_MAC_ADDR_LENGTH;
+    pmbuf = woal_alloc_mlan_buffer(priv->phandle,
+                                   MLAN_MIN_DATA_HEADER_LEN + HEADER_SIZE +
+                                   packet_len + sizeof(packet_len));
+    if (!pmbuf) {
+        PRINTM(MERROR, "Fail to allocate mlan_buffer\n");
+        ret = -ENOMEM;
+        goto done;
+    }
+    *cookie = random32() | 1;
+    pmbuf->data_offset = MLAN_MIN_DATA_HEADER_LEN;
+    pkt_type = MRVL_PKT_TYPE_MGMT_FRAME;
+    tx_control = 0;
+    /* Add pkt_type and tx_control */
+    memcpy(pmbuf->pbuf + pmbuf->data_offset, &pkt_type, sizeof(pkt_type));
+    memcpy(pmbuf->pbuf + pmbuf->data_offset + sizeof(pkt_type), &tx_control,
+           sizeof(tx_control));
+    /* frmctl + durationid + addr1 + addr2 + addr3 + seqctl */
+#define PACKET_ADDR4_POS		(2 + 2 + 6 + 6 + 6 + 2)
+    memcpy(pmbuf->pbuf + pmbuf->data_offset + HEADER_SIZE, &packet_len,
+           sizeof(packet_len));
+    memcpy(pmbuf->pbuf + pmbuf->data_offset + HEADER_SIZE + sizeof(packet_len),
+           buf, PACKET_ADDR4_POS);
+    memcpy(pmbuf->pbuf + pmbuf->data_offset + HEADER_SIZE + sizeof(packet_len)
+           + PACKET_ADDR4_POS, addr, MLAN_MAC_ADDR_LENGTH);
+    memcpy(pmbuf->pbuf + pmbuf->data_offset + HEADER_SIZE + sizeof(packet_len)
+           + PACKET_ADDR4_POS + MLAN_MAC_ADDR_LENGTH,
+           buf + PACKET_ADDR4_POS, len - PACKET_ADDR4_POS);
+
+    pmbuf->data_len = HEADER_SIZE + packet_len + sizeof(packet_len);
+    pmbuf->buf_type = MLAN_BUF_TYPE_RAW_DATA;
+    pmbuf->bss_index = priv->bss_index;
+
+    status = mlan_send_packet(priv->phandle->pmlan_adapter, pmbuf);
+
+    switch (status) {
+    case MLAN_STATUS_PENDING:
+        atomic_inc(&priv->phandle->tx_pending);
+        queue_work(priv->phandle->workqueue, &priv->phandle->main_work);
+
+        /* delay 20ms to guarantee the packet has been already tx'ed becuase if
+           we call cfg80211_mgmt_tx_status() immediately, then wpa_supplicant
+           will call cancel_remain_on_channel(), which may affect the mgmt
+           frame tx */
+        mdelay(20);
+
+        /* Notify the mgmt tx status */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37) || defined(COMPAT_WIRELESS)
+        cfg80211_mgmt_tx_status(dev, *cookie, buf, len, true, GFP_ATOMIC);
+#endif
+        break;
+    case MLAN_STATUS_SUCCESS:
+        woal_free_mlan_buffer(priv->phandle, pmbuf);
+        break;
+    case MLAN_STATUS_FAILURE:
+    default:
+        woal_free_mlan_buffer(priv->phandle, pmbuf);
+        ret = -EFAULT;
+        break;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Look up specific IE in a buf
+ *
+ * @param ie              Pointer to IEs
+ * @param len             Total length of ie
+ * @param id              Element id to lookup
+ *
+ * @return                Pointer of the specific IE -- success, NULL -- fail
+ */
+const t_u8 *
+woal_parse_ie_tlv(const t_u8 * ie, int len, t_u8 id)
+{
+    int left_len = len;
+    const t_u8 *pos = ie;
+    int length;
+
+    /* IE format: | u8 | id | | u8 | len | | var | data | */
+    while (left_len >= 2) {
+        length = *(pos + 1);
+        if ((*pos == id) && (length + 2) <= left_len)
+            return pos;
+        pos += (length + 2);
+        left_len -= (length + 2);
+    }
+
+    return NULL;
+}
+
+/**
+ *  @brief This function returns priv
+ *  based on mgmt ie index
+ *
+ *  @param handle    A pointer to moal_handle
+ *  @param index     mgmt ie index
+ *
+ *  @return          Pointer to moal_private
+ */
+static moal_private *
+woal_get_priv_by_mgmt_index(moal_handle * handle, t_u16 index)
+{
+    int i;
+
+    for (i = 0; i < MIN(handle->priv_num, MLAN_MAX_BSS_NUM); i++) {
+        if (handle->priv[i]) {
+            if (handle->priv[i]->probereq_index == index)
+                return (handle->priv[i]);
+        }
+    }
+    return NULL;
+}
+
+/**
+ * @brief Add custom ie to mgmt frames.
+ *
+ * @param priv                  A pointer to moal private structure
+ * @param beacon_ies_data       Beacon ie
+ * @param beacon_index          The index for beacon when auto index
+ * @param proberesp_ies_data    Probe resp ie
+ * @param proberesp_index       The index for probe resp when auto index
+ * @param assocresp_ies_data    Assoc resp ie
+ * @param assocresp_index       The index for assoc resp when auto index
+ * @param probereq_ies_data     Probe req ie
+ * @param probereq_index        The index for probe req when auto index *
+ *
+ * @return              0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_custom_ie(moal_private * priv,
+                        custom_ie * beacon_ies_data, t_u16 * beacon_index,
+                        custom_ie * proberesp_ies_data, t_u16 * proberesp_index,
+                        custom_ie * assocresp_ies_data, t_u16 * assocresp_index,
+                        custom_ie * probereq_ies_data, t_u16 * probereq_index)
+{
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+    mlan_ds_misc_custom_ie *custom_ie = NULL;
+    t_u8 *pos = NULL;
+    t_u16 len = 0;
+    int ret = 0;
+
+    ENTER();
+
+    if (!(custom_ie = kmalloc(sizeof(mlan_ds_misc_custom_ie), GFP_KERNEL))) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    memset(custom_ie, 0x00, sizeof(mlan_ds_misc_custom_ie));
+    custom_ie->type = TLV_TYPE_MGMT_IE;
+
+    pos = (t_u8 *) custom_ie->ie_data_list;
+    if (beacon_ies_data) {
+        len = sizeof(*beacon_ies_data) - MAX_IE_SIZE
+            + beacon_ies_data->ie_length;
+        memcpy(pos, beacon_ies_data, len);
+        pos += len;
+        custom_ie->len += len;
+    }
+
+    if (proberesp_ies_data) {
+        len = sizeof(*proberesp_ies_data) - MAX_IE_SIZE
+            + proberesp_ies_data->ie_length;
+        memcpy(pos, proberesp_ies_data, len);
+        pos += len;
+        custom_ie->len += len;
+    }
+
+    if (assocresp_ies_data) {
+        len = sizeof(*assocresp_ies_data) - MAX_IE_SIZE
+            + assocresp_ies_data->ie_length;
+        memcpy(pos, assocresp_ies_data, len);
+        custom_ie->len += len;
+    }
+
+    if (probereq_ies_data) {
+        len = sizeof(*probereq_ies_data) - MAX_IE_SIZE
+            + probereq_ies_data->ie_length;
+        memcpy(pos, probereq_ies_data, len);
+        pos += len;
+        custom_ie->len += len;
+    }
+
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (ioctl_req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    misc = (mlan_ds_misc_cfg *) ioctl_req->pbuf;
+    misc->sub_command = MLAN_OID_MISC_CUSTOM_IE;
+    ioctl_req->req_id = MLAN_IOCTL_MISC_CFG;
+    ioctl_req->action = MLAN_ACT_SET;
+
+    memcpy(&misc->param.cust_ie, custom_ie, sizeof(mlan_ds_misc_custom_ie));
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* get the assigned index */
+    pos = (t_u8 *) (&misc->param.cust_ie.ie_data_list[0].ie_index);
+    if (beacon_ies_data && beacon_ies_data->ie_length
+        && beacon_ies_data->ie_index == MLAN_CUSTOM_IE_AUTO_IDX_MASK) {
+        /* save beacon ie index after auto-indexing */
+        *beacon_index = misc->param.cust_ie.ie_data_list[0].ie_index;
+        len = sizeof(*beacon_ies_data) - MAX_IE_SIZE
+            + beacon_ies_data->ie_length;
+        pos += len;
+    }
+
+    if (proberesp_ies_data && proberesp_ies_data->ie_length
+        && proberesp_ies_data->ie_index == MLAN_CUSTOM_IE_AUTO_IDX_MASK) {
+        /* save probe resp ie index after auto-indexing */
+        *proberesp_index = *((t_u16 *) pos);
+        len = sizeof(*proberesp_ies_data) - MAX_IE_SIZE
+            + proberesp_ies_data->ie_length;
+        pos += len;
+    }
+
+    if (assocresp_ies_data && assocresp_ies_data->ie_length
+        && assocresp_ies_data->ie_index == MLAN_CUSTOM_IE_AUTO_IDX_MASK) {
+        /* save assoc resp ie index after auto-indexing */
+        *assocresp_index = *((t_u16 *) pos);
+        len = sizeof(*assocresp_ies_data) - MAX_IE_SIZE
+            + assocresp_ies_data->ie_length;
+        pos += len;
+    }
+    if (probereq_ies_data && probereq_ies_data->ie_length
+        && probereq_ies_data->ie_index == MLAN_CUSTOM_IE_AUTO_IDX_MASK) {
+        /* save probe resp ie index after auto-indexing */
+        *probereq_index = *((t_u16 *) pos);
+        len = sizeof(*probereq_ies_data) - MAX_IE_SIZE
+            + probereq_ies_data->ie_length;
+        pos += len;
+    }
+
+    if (ioctl_req->status_code == MLAN_ERROR_IOCTL_FAIL)
+        ret = -EFAULT;
+
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    if (custom_ie)
+        kfree(custom_ie);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Filter specific IE in ie buf
+ *
+ * @param ie              Pointer to IEs
+ * @param len             Total length of ie
+ * @param ie_out		  Pointer to out IE buf
+ *
+ * @return                out IE length
+ */
+static t_u16
+woal_filter_beacon_ies(const t_u8 * ie, int len, t_u8 * ie_out)
+{
+    int left_len = len;
+    const t_u8 *pos = ie;
+    int length;
+    t_u8 id = 0;
+    t_u16 out_len = 0;
+
+    /* ERP_INFO and RSN IE will be fileter out */
+    while (left_len >= 2) {
+        length = *(pos + 1);
+        id = *pos;
+        if ((length + 2) > left_len)
+            break;
+        switch (id) {
+        case WLAN_EID_ERP_INFO:
+        case RSN_IE:
+            break;
+        default:
+            memcpy(ie_out + out_len, pos, length + 2);
+            out_len += length + 2;
+            break;
+        }
+        pos += (length + 2);
+        left_len -= (length + 2);
+    }
+    return out_len;
+}
+
+/**
+ * @brief config AP or GO for mgmt frame ies.
+ *
+ * @param priv                  A pointer to moal private structure
+ * @param beacon_ies            A pointer to beacon ies
+ * @param beacon_ies_len        Beacon ies length
+ * @param proberesp_ies         A pointer to probe resp ies
+ * @param proberesp_ies_len     Probe resp ies length
+ * @param assocresp_ies         A pointer to probe resp ies
+ * @param assocresp_ies_len     Assoc resp ies length
+ * @param probereq_ies          A pointer to probe req ies
+ * @param probereq_ies_len      Probe req ies length *
+ * @param mask					Mgmt frame mask
+ *
+ * @return                      0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_mgmt_frame_ie(moal_private * priv,
+                            const t_u8 * beacon_ies, size_t beacon_ies_len,
+                            const t_u8 * proberesp_ies,
+                            size_t proberesp_ies_len,
+                            const t_u8 * assocresp_ies,
+                            size_t assocresp_ies_len, const t_u8 * probereq_ies,
+                            size_t probereq_ies_len, t_u16 mask)
+{
+    int ret = 0;
+    t_u8 *pos = NULL;
+    custom_ie *beacon_ies_data = NULL;
+    custom_ie *proberesp_ies_data = NULL;
+    custom_ie *assocresp_ies_data = NULL;
+    custom_ie *probereq_ies_data = NULL;
+
+    /* static variables for mgmt frame ie auto-indexing */
+    static t_u16 beacon_index = MLAN_CUSTOM_IE_AUTO_IDX_MASK;
+    static t_u16 proberesp_index = MLAN_CUSTOM_IE_AUTO_IDX_MASK;
+    static t_u16 assocresp_index = MLAN_CUSTOM_IE_AUTO_IDX_MASK;
+    static t_u16 probereq_index = MLAN_CUSTOM_IE_AUTO_IDX_MASK;
+    moal_private *pmpriv = NULL;
+    static t_u16 rsn_index = MLAN_CUSTOM_IE_AUTO_IDX_MASK;
+    const t_u8 *rsn_ie;
+
+    ENTER();
+
+    if (mask & MGMT_MASK_BEACON) {
+        if (!(beacon_ies_data = kmalloc(sizeof(custom_ie), GFP_KERNEL))) {
+            ret = -ENOMEM;
+            goto done;
+        }
+        if (beacon_ies && beacon_ies_len) {
+            rsn_ie =
+                woal_parse_ie_tlv((t_u8 *) beacon_ies, (int) beacon_ies_len,
+                                  RSN_IE);
+            if (rsn_ie) {
+                beacon_ies_data->ie_index = rsn_index;
+                beacon_ies_data->mgmt_subtype_mask =
+                    MGMT_MASK_BEACON | MGMT_MASK_PROBE_RESP |
+                    MGMT_MASK_ASSOC_RESP;
+                beacon_ies_data->ie_length = rsn_ie[1] + 2;
+                memcpy(beacon_ies_data->ie_buffer, rsn_ie, rsn_ie[1] + 2);
+                if (MLAN_STATUS_SUCCESS !=
+                    woal_cfg80211_custom_ie(priv, beacon_ies_data, &rsn_index,
+                                            NULL, &proberesp_index, NULL,
+                                            &assocresp_index, NULL,
+                                            &probereq_index)) {
+                    ret = -EFAULT;
+                    goto done;
+                }
+            }
+        } else {
+            /* clear rsn_ie */
+            if (rsn_index <= MAX_MGMT_IE_INDEX) {
+                beacon_ies_data->ie_index = rsn_index;
+                beacon_ies_data->mgmt_subtype_mask = MLAN_CUSTOM_IE_DELETE_MASK;
+                beacon_ies_data->ie_length = 0;
+                rsn_index = MLAN_CUSTOM_IE_AUTO_IDX_MASK;
+                if (MLAN_STATUS_SUCCESS !=
+                    woal_cfg80211_custom_ie(priv, beacon_ies_data, &rsn_index,
+                                            NULL, &proberesp_index, NULL,
+                                            &assocresp_index, NULL,
+                                            &probereq_index)) {
+                    ret = -EFAULT;
+                    goto done;
+                }
+            }
+        }
+    }
+
+    if (mask & MGMT_MASK_PROBE_RESP) {
+        if (!(proberesp_ies_data = kmalloc(sizeof(custom_ie), GFP_KERNEL))) {
+            ret = -ENOMEM;
+            goto done;
+        }
+    }
+
+    if (mask & MGMT_MASK_ASSOC_RESP) {
+        if (!(assocresp_ies_data = kmalloc(sizeof(custom_ie), GFP_KERNEL))) {
+            ret = -ENOMEM;
+            goto done;
+        }
+    }
+    if (mask & MGMT_MASK_PROBE_REQ) {
+        if (!(probereq_ies_data = kmalloc(sizeof(custom_ie), GFP_KERNEL))) {
+            ret = -ENOMEM;
+            goto done;
+        }
+    }
+
+    if (beacon_ies_data) {
+        memset(beacon_ies_data, 0x00, sizeof(custom_ie));
+        if (beacon_ies && beacon_ies_len) {
+            /* set the beacon ies */
+            beacon_ies_data->ie_index = beacon_index;
+            beacon_ies_data->mgmt_subtype_mask = MGMT_MASK_BEACON;
+            beacon_ies_data->mgmt_subtype_mask |= MGMT_MASK_ASSOC_RESP;
+            beacon_ies_data->ie_length = woal_filter_beacon_ies(beacon_ies,
+                                                                beacon_ies_len,
+                                                                beacon_ies_data->
+                                                                ie_buffer);
+        } else {
+            /* clear the beacon ies */
+            if (beacon_index > MAX_MGMT_IE_INDEX) {
+                PRINTM(MERROR, "Invalid beacon index for mgmt frame ie.\n");
+                goto done;
+            }
+
+            beacon_ies_data->ie_index = beacon_index;
+            beacon_ies_data->mgmt_subtype_mask = MLAN_CUSTOM_IE_DELETE_MASK;
+            beacon_ies_data->ie_length = 0;
+            beacon_index = MLAN_CUSTOM_IE_AUTO_IDX_MASK;
+        }
+    }
+
+    if (proberesp_ies_data) {
+        memset(proberesp_ies_data, 0x00, sizeof(custom_ie));
+        if (proberesp_ies && proberesp_ies_len) {
+            /* set the probe response ies */
+            // proberesp_ies_data->ie_index = MLAN_CUSTOM_IE_AUTO_IDX_MASK;
+            proberesp_ies_data->ie_index = proberesp_index;
+            proberesp_ies_data->mgmt_subtype_mask = MGMT_MASK_PROBE_RESP;
+            proberesp_ies_data->ie_length = proberesp_ies_len;
+            pos = proberesp_ies_data->ie_buffer;
+            memcpy(pos, proberesp_ies, proberesp_ies_len);
+        } else {
+            /* clear the probe response ies */
+            if (proberesp_index > MAX_MGMT_IE_INDEX) {
+                PRINTM(MERROR, "Invalid probe resp index for mgmt frame ie.\n");
+                goto done;
+            }
+
+            proberesp_ies_data->ie_index = proberesp_index;
+            proberesp_ies_data->mgmt_subtype_mask = MLAN_CUSTOM_IE_DELETE_MASK;
+            proberesp_ies_data->ie_length = 0;
+            proberesp_index = MLAN_CUSTOM_IE_AUTO_IDX_MASK;
+        }
+    }
+    if (assocresp_ies_data) {
+        memset(assocresp_ies_data, 0x00, sizeof(custom_ie));
+        if (assocresp_ies && assocresp_ies_len) {
+            /* set the assoc response ies */
+            assocresp_ies_data->ie_index = assocresp_index;
+            assocresp_ies_data->mgmt_subtype_mask = MGMT_MASK_ASSOC_RESP;
+            assocresp_ies_data->ie_length = assocresp_ies_len;
+            pos = assocresp_ies_data->ie_buffer;
+            memcpy(pos, assocresp_ies, assocresp_ies_len);
+        } else {
+            /* clear the assoc response ies */
+            if (assocresp_index > MAX_MGMT_IE_INDEX) {
+                PRINTM(MERROR, "Invalid assoc resp index for mgmt frame ie.\n");
+                goto done;
+            }
+
+            assocresp_ies_data->ie_index = assocresp_index;
+            assocresp_ies_data->mgmt_subtype_mask = MLAN_CUSTOM_IE_DELETE_MASK;
+            assocresp_ies_data->ie_length = 0;
+            assocresp_index = MLAN_CUSTOM_IE_AUTO_IDX_MASK;
+        }
+    }
+
+    if (probereq_ies_data) {
+        memset(probereq_ies_data, 0x00, sizeof(custom_ie));
+        if ((probereq_index != MLAN_CUSTOM_IE_AUTO_IDX_MASK) &&
+            (priv->probereq_index != probereq_index)) {
+            pmpriv = woal_get_priv_by_mgmt_index(priv->phandle, probereq_index);
+            if (pmpriv) {
+                probereq_ies_data->ie_index = probereq_index;
+                probereq_ies_data->mgmt_subtype_mask =
+                    MLAN_CUSTOM_IE_DELETE_MASK;
+                probereq_ies_data->ie_length = 0;
+                probereq_index = MLAN_CUSTOM_IE_AUTO_IDX_MASK;
+                pmpriv->probereq_index = MLAN_CUSTOM_IE_AUTO_IDX_MASK;
+                if (MLAN_STATUS_SUCCESS !=
+                    woal_cfg80211_custom_ie(pmpriv, NULL, &beacon_index,
+                                            NULL, &proberesp_index,
+                                            NULL, &assocresp_index,
+                                            probereq_ies_data,
+                                            &probereq_index)) {
+                    ret = -EFAULT;
+                    goto done;
+                }
+                memset(probereq_ies_data, 0x00, sizeof(custom_ie));
+            }
+        }
+        if (probereq_ies && probereq_ies_len) {
+            /* set the probe req ies */
+            probereq_ies_data->ie_index = probereq_index;
+            probereq_ies_data->mgmt_subtype_mask = MGMT_MASK_PROBE_REQ;
+            probereq_ies_data->ie_length = probereq_ies_len;
+            pos = probereq_ies_data->ie_buffer;
+            memcpy(pos, probereq_ies, probereq_ies_len);
+        } else {
+            /* clear the probe req ies */
+            if (probereq_index > MAX_MGMT_IE_INDEX) {
+                PRINTM(MERROR, "Invalid probe resp index for mgmt frame ie.\n");
+                goto done;
+            }
+            probereq_ies_data->ie_index = probereq_index;
+            probereq_ies_data->mgmt_subtype_mask = MLAN_CUSTOM_IE_DELETE_MASK;
+            probereq_ies_data->ie_length = 0;
+            probereq_index = MLAN_CUSTOM_IE_AUTO_IDX_MASK;
+        }
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_cfg80211_custom_ie(priv, beacon_ies_data, &beacon_index,
+                                proberesp_ies_data, &proberesp_index,
+                                assocresp_ies_data, &assocresp_index,
+                                probereq_ies_data, &probereq_index)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (probereq_ies_data)
+        priv->probereq_index = probereq_index;
+
+  done:
+    if (beacon_ies_data)
+        kfree(beacon_ies_data);
+    if (proberesp_ies_data)
+        kfree(proberesp_ies_data);
+    if (assocresp_ies_data)
+        kfree(assocresp_ies_data);
+
+    LEAVE();
+
+    return ret;
+}
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_cfg80211.h b/drivers/net/wireless/sd8797/mlinux/moal_cfg80211.h
new file mode 100644
index 000000000000..f58fc18829fa
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_cfg80211.h
@@ -0,0 +1,203 @@
+/** @file moal_cfg80211.h
+  *
+  * @brief This file contains the CFG80211 specific defines.
+  *
+  * Copyright (C) 2011-2012, Marvell International Ltd. 
+  * 
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+
+#ifndef _MOAL_CFG80211_H_
+#define _MOAL_CFG80211_H_
+
+#include    "moal_main.h"
+
+/* Clear all key indexes */
+#define KEY_INDEX_CLEAR_ALL             (0x0000000F)
+
+/** RTS/FRAG disabled value */
+#define MLAN_FRAG_RTS_DISABLED          (0xFFFFFFFF)
+
+#ifndef WLAN_CIPHER_SUITE_WAPI
+#define WLAN_CIPHER_SUITE_WAPI          0x00000020
+#endif
+
+/* define for custom ie operation */
+#define MLAN_CUSTOM_IE_AUTO_IDX_MASK    0xffff
+#define MLAN_CUSTOM_IE_DELETE_MASK      0x0
+#define TLV_TYPE_MGMT_IE                0x0169
+#define MGMT_MASK_ASSOC_REQ             0x01
+#define MGMT_MASK_REASSOC_REQ           0x04
+#define MGMT_MASK_ASSOC_RESP            0x02
+#define MGMT_MASK_REASSOC_RESP          0x08
+#define MGMT_MASK_PROBE_REQ             0x10
+#define MGMT_MASK_PROBE_RESP            0x20
+#define MGMT_MASK_BEACON                0x100
+
+/**
+ * If multiple wiphys are registered e.g. a regular netdev with
+ * assigned ieee80211_ptr and you won't know whether it points
+ * to a wiphy your driver has registered or not. Assign this to
+ * something global to your driver to help determine whether
+ * you own this wiphy or not.
+ */
+static const void *const mrvl_wiphy_privid = &mrvl_wiphy_privid;
+
+/* Get the private structure from wiphy */
+void *woal_get_wiphy_priv(struct wiphy *wiphy);
+
+/* Get the private structure from net device */
+void *woal_get_netdev_priv(struct net_device *dev);
+
+int woal_cfg80211_change_virtual_intf(struct wiphy *wiphy,
+                                      struct net_device *dev,
+                                      enum nl80211_iftype type,
+                                      u32 * flags, struct vif_params *params);
+
+int woal_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed);
+
+int woal_cfg80211_add_key(struct wiphy *wiphy,
+                          struct net_device *dev, t_u8 key_index,
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,36) || defined(COMPAT_WIRELESS)
+                          bool pairwise,
+#endif
+                          const t_u8 * mac_addr, struct key_params *params);
+
+int woal_cfg80211_del_key(struct wiphy *wiphy,
+                          struct net_device *dev, t_u8 key_index,
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,36) || defined(COMPAT_WIRELESS)
+                          bool pairwise,
+#endif
+                          const t_u8 * mac_addr);
+
+#ifdef STA_CFG80211
+#ifdef STA_SUPPORT
+int woal_set_rf_channel(moal_private * priv,
+                        struct ieee80211_channel *chan,
+                        enum nl80211_channel_type channel_type);
+mlan_status woal_inform_bss_from_scan_result(moal_private * priv,
+                                             mlan_802_11_ssid * ssid);
+#endif
+#endif
+
+int woal_cfg80211_set_channel(struct wiphy *wiphy,
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,34) || defined(COMPAT_WIRELESS)
+                              struct net_device *dev,
+#endif
+                              struct ieee80211_channel *chan,
+                              enum nl80211_channel_type channel_type);
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,37) || defined(COMPAT_WIRELESS)
+int woal_cfg80211_set_default_key(struct wiphy *wiphy,
+                                  struct net_device *dev, t_u8 key_index,
+                                  bool ucast, bool mcast);
+#else
+int woal_cfg80211_set_default_key(struct wiphy *wiphy,
+                                  struct net_device *dev, t_u8 key_index);
+#endif
+
+void woal_cfg80211_mgmt_frame_register(struct wiphy *wiphy,
+                                       struct net_device *dev, t_u16 frame_type,
+                                       bool reg);
+
+int woal_cfg80211_mgmt_tx(struct wiphy *wiphy,
+                          struct net_device *dev,
+                          struct ieee80211_channel *chan, bool offchan,
+                          enum nl80211_channel_type channel_type,
+                          bool channel_type_valid, unsigned int wait,
+                          const u8 * buf, size_t len,
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0) || defined(COMPAT_WIRELESS)
+                          bool no_cck,
+#endif
+                          u64 * cookie);
+
+extern struct ieee80211_supported_band cfg80211_band_2ghz;
+extern struct ieee80211_supported_band cfg80211_band_5ghz;
+extern const u32 cfg80211_cipher_suites[10];
+
+typedef struct _monitor_iface
+{
+    /* The priv data of interface on which the monitor iface is based */
+    moal_private *priv;
+    struct wireless_dev wdev;
+    int radiotap_enabled;
+    /* The net_device on which the monitor iface is based. */
+    struct net_device *base_ndev;
+    struct net_device *mon_ndev;
+    char ifname[IFNAMSIZ];
+    int flag;
+} monitor_iface;
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,37) || defined(COMPAT_WIRELESS)
+struct net_device *woal_cfg80211_add_virtual_intf(struct wiphy *wiphy,
+                                                  char *name,
+                                                  enum nl80211_iftype type,
+                                                  u32 * flags,
+                                                  struct vif_params *params);
+#else
+int woal_cfg80211_add_virtual_intf(struct wiphy *wiphy,
+                                   char *name, enum nl80211_iftype type,
+                                   u32 * flags, struct vif_params *params);
+#endif
+int woal_cfg80211_del_virtual_intf(struct wiphy *wiphy, struct net_device *dev);
+
+#if defined(WIFI_DIRECT_SUPPORT)
+/** Define kernel version for wifi direct */
+#if !defined(COMPAT_WIRELESS)
+#define WIFI_DIRECT_KERNEL_VERSION          KERNEL_VERSION(2,6,39)
+#else
+#define WIFI_DIRECT_KERNEL_VERSION          KERNEL_VERSION(2,6,33)
+#endif /* COMPAT_WIRELESS */
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+/** Define for remain on channel duration timer */
+#define MAX_REMAIN_ON_CHANNEL_DURATION      (1000 * 5)
+
+int woal_cfg80211_add_beacon(struct wiphy *wiphy,
+                             struct net_device *dev,
+                             struct beacon_parameters *params);
+
+int woal_cfg80211_set_beacon(struct wiphy *wiphy,
+                             struct net_device *dev,
+                             struct beacon_parameters *params);
+
+int woal_cfg80211_del_beacon(struct wiphy *wiphy, struct net_device *dev);
+
+int woal_cfg80211_init_p2p_client(moal_private * priv);
+
+int woal_cfg80211_init_p2p_go(moal_private * priv);
+
+int woal_cfg80211_deinit_p2p(moal_private * priv);
+
+int woal_cfg80211_remain_on_channel_cfg(moal_private * priv,
+                                        t_u8 wait_option, t_u8 remove,
+                                        t_u8 * status,
+                                        struct ieee80211_channel *chan,
+                                        enum nl80211_channel_type channel_type,
+                                        t_u32 duration);
+int woal_uap_cfg80211_get_station(struct wiphy *wiphy, struct net_device *dev,
+                                  u8 * mac, struct station_info *stainfo);
+#endif /* KERNEL_VERSION */
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+
+const t_u8 *woal_parse_ie_tlv(const t_u8 * ie, int len, t_u8 id);
+
+int woal_cfg80211_mgmt_frame_ie(moal_private * priv,
+                                const t_u8 * beacon_ies, size_t beacon_ies_len,
+                                const t_u8 * proberesp_ies,
+                                size_t proberesp_ies_len,
+                                const t_u8 * assocresp_ies,
+                                size_t assocresp_ies_len,
+                                const t_u8 * probereq_ies,
+                                size_t probereq_ies_len, t_u16 mask);
+#endif /* _MOAL_CFG80211_H_ */
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_debug.c b/drivers/net/wireless/sd8797/mlinux/moal_debug.c
new file mode 100644
index 000000000000..fd74c3b5ae92
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_debug.c
@@ -0,0 +1,674 @@
+/** @file moal_debug.c
+  *
+  * @brief This file contains functions for debug proc file.
+  *
+  * Copyright (C) 2008-2011, Marvell International Ltd.
+  *
+  * This software file (the "File") is distributed by Marvell International
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+  * (the "License").  You may use, redistribute and/or modify this File in
+  * accordance with the terms and conditions of the License, a copy of which
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+  * this warranty disclaimer.
+  *
+  */
+
+/********************************************************
+Change log:
+    11/03/2008: initial version
+********************************************************/
+
+#include	"moal_main.h"
+
+/********************************************************
+		Global Variables
+********************************************************/
+/** MLAN debug info */
+extern mlan_debug_info info;
+
+/********************************************************
+		Local Variables
+********************************************************/
+#ifdef CONFIG_PROC_FS
+
+/** Get info item size */
+#define item_size(n) (sizeof(info.n))
+/** Get info item address */
+#define item_addr(n) ((t_ptr) &(info.n))
+
+/** Get moal_private member size */
+#define item_priv_size(n) (sizeof ((moal_private *)0)->n)
+/** Get moal_private member address */
+#define item_priv_addr(n) ((t_ptr) &((moal_private *)0)->n)
+
+/** Get moal_handle member size */
+#define item_handle_size(n) (sizeof ((moal_handle *)0)->n)
+/** Get moal_handle member address */
+#define item_handle_addr(n) ((t_ptr) &((moal_handle *)0)->n)
+
+#ifdef STA_SUPPORT
+static struct debug_data items[] = {
+#ifdef DEBUG_LEVEL1
+    {"drvdbg", sizeof(drvdbg), (t_ptr) & drvdbg}
+    ,
+#endif
+    {"wmm_ac_vo", item_size(wmm_ac_vo), item_addr(wmm_ac_vo)}
+    ,
+    {"wmm_ac_vi", item_size(wmm_ac_vi), item_addr(wmm_ac_vi)}
+    ,
+    {"wmm_ac_be", item_size(wmm_ac_be), item_addr(wmm_ac_be)}
+    ,
+    {"wmm_ac_bk", item_size(wmm_ac_bk), item_addr(wmm_ac_bk)}
+    ,
+    {"max_tx_buf_size", item_size(max_tx_buf_size), item_addr(max_tx_buf_size)}
+    ,
+    {"tx_buf_size", item_size(tx_buf_size), item_addr(tx_buf_size)}
+    ,
+    {"curr_tx_buf_size", item_size(curr_tx_buf_size),
+     item_addr(curr_tx_buf_size)}
+    ,
+    {"ps_mode", item_size(ps_mode), item_addr(ps_mode)}
+    ,
+    {"ps_state", item_size(ps_state), item_addr(ps_state)}
+    ,
+    {"is_deep_sleep", item_size(is_deep_sleep), item_addr(is_deep_sleep)}
+    ,
+    {"wakeup_dev_req", item_size(pm_wakeup_card_req),
+     item_addr(pm_wakeup_card_req)}
+    ,
+    {"wakeup_tries", item_size(pm_wakeup_fw_try), item_addr(pm_wakeup_fw_try)}
+    ,
+    {"hs_configured", item_size(is_hs_configured), item_addr(is_hs_configured)}
+    ,
+    {"hs_activated", item_size(hs_activated), item_addr(hs_activated)}
+    ,
+    {"tx_pkts_queued", item_size(tx_pkts_queued), item_addr(tx_pkts_queued)}
+    ,
+    {"pps_uapsd_mode", item_size(pps_uapsd_mode), item_addr(pps_uapsd_mode)}
+    ,
+    {"sleep_pd", item_size(sleep_pd), item_addr(sleep_pd)}
+    ,
+    {"qos_cfg", item_size(qos_cfg), item_addr(qos_cfg)}
+    ,
+    {"tx_lock_flag", item_size(tx_lock_flag), item_addr(tx_lock_flag)}
+    ,
+    {"port_open", item_size(port_open), item_addr(port_open)}
+    ,
+    {"scan_processing", item_size(scan_processing), item_addr(scan_processing)}
+    ,
+    {"num_tx_timeout", item_size(num_tx_timeout), item_addr(num_tx_timeout)}
+    ,
+    {"num_cmd_timeout", item_size(num_cmd_timeout), item_addr(num_cmd_timeout)}
+    ,
+    {"timeout_cmd_id", item_size(timeout_cmd_id), item_addr(timeout_cmd_id)}
+    ,
+    {"timeout_cmd_act", item_size(timeout_cmd_act), item_addr(timeout_cmd_act)}
+    ,
+    {"last_cmd_id", item_size(last_cmd_id), item_addr(last_cmd_id)}
+    ,
+    {"last_cmd_act", item_size(last_cmd_act), item_addr(last_cmd_act)}
+    ,
+    {"last_cmd_index", item_size(last_cmd_index), item_addr(last_cmd_index)}
+    ,
+    {"last_cmd_resp_id", item_size(last_cmd_resp_id),
+     item_addr(last_cmd_resp_id)}
+    ,
+    {"last_cmd_resp_index", item_size(last_cmd_resp_index),
+     item_addr(last_cmd_resp_index)}
+    ,
+    {"last_event", item_size(last_event), item_addr(last_event)}
+    ,
+    {"last_event_index", item_size(last_event_index),
+     item_addr(last_event_index)}
+    ,
+    {"num_cmd_h2c_fail", item_size(num_cmd_host_to_card_failure),
+     item_addr(num_cmd_host_to_card_failure)}
+    ,
+    {"num_cmd_sleep_cfm_fail",
+     item_size(num_cmd_sleep_cfm_host_to_card_failure),
+     item_addr(num_cmd_sleep_cfm_host_to_card_failure)}
+    ,
+    {"num_tx_h2c_fail", item_size(num_tx_host_to_card_failure),
+     item_addr(num_tx_host_to_card_failure)}
+    ,
+    {"num_cmdevt_c2h_fail", item_size(num_cmdevt_card_to_host_failure),
+     item_addr(num_cmdevt_card_to_host_failure)}
+    ,
+    {"num_rx_c2h_fail", item_size(num_rx_card_to_host_failure),
+     item_addr(num_rx_card_to_host_failure)}
+    ,
+    {"num_int_read_fail", item_size(num_int_read_failure),
+     item_addr(num_int_read_failure)}
+    ,
+    {"last_int_status", item_size(last_int_status), item_addr(last_int_status)}
+    ,
+    {"num_evt_deauth", item_size(num_event_deauth), item_addr(num_event_deauth)}
+    ,
+    {"num_evt_disassoc", item_size(num_event_disassoc),
+     item_addr(num_event_disassoc)}
+    ,
+    {"num_evt_link_lost", item_size(num_event_link_lost),
+     item_addr(num_event_link_lost)}
+    ,
+    {"num_cmd_deauth", item_size(num_cmd_deauth), item_addr(num_cmd_deauth)}
+    ,
+    {"num_cmd_assoc_ok", item_size(num_cmd_assoc_success),
+     item_addr(num_cmd_assoc_success)}
+    ,
+    {"num_cmd_assoc_fail", item_size(num_cmd_assoc_failure),
+     item_addr(num_cmd_assoc_failure)}
+    ,
+    {"cmd_sent", item_size(cmd_sent), item_addr(cmd_sent)}
+    ,
+    {"data_sent", item_size(data_sent), item_addr(data_sent)}
+    ,
+    {"mp_rd_bitmap", item_size(mp_rd_bitmap), item_addr(mp_rd_bitmap)}
+    ,
+    {"curr_rd_port", item_size(curr_rd_port), item_addr(curr_rd_port)}
+    ,
+    {"mp_wr_bitmap", item_size(mp_wr_bitmap), item_addr(mp_wr_bitmap)}
+    ,
+    {"curr_wr_port", item_size(curr_wr_port), item_addr(curr_wr_port)}
+    ,
+    {"cmd_resp_received", item_size(cmd_resp_received),
+     item_addr(cmd_resp_received)}
+    ,
+    {"event_received", item_size(event_received), item_addr(event_received)}
+    ,
+
+    {"ioctl_pending", item_handle_size(ioctl_pending),
+     item_handle_addr(ioctl_pending)}
+    ,
+    {"tx_pending", item_handle_size(tx_pending), item_handle_addr(tx_pending)}
+    ,
+    {"rx_pending", item_handle_size(rx_pending), item_handle_addr(rx_pending)}
+    ,
+    {"malloc_count", item_handle_size(malloc_count),
+     item_handle_addr(malloc_count)}
+    ,
+    {"lock_count", item_handle_size(lock_count), item_handle_addr(lock_count)}
+    ,
+    {"mbufalloc_count", item_handle_size(mbufalloc_count),
+     item_handle_addr(mbufalloc_count)}
+    ,
+    {"main_state", item_handle_size(main_state), item_handle_addr(main_state)}
+    ,
+#ifdef SDIO_MMC_DEBUG
+    {"sdiocmd53w", item_handle_size(cmd53w), item_handle_addr(cmd53w)}
+    ,
+    {"sdiocmd53r", item_handle_size(cmd53r), item_handle_addr(cmd53r)}
+    ,
+#endif
+#if defined(SDIO_SUSPEND_RESUME)
+    {"hs_skip_count", item_handle_size(hs_skip_count),
+     item_handle_addr(hs_skip_count)}
+    ,
+    {"hs_force_count", item_handle_size(hs_force_count),
+     item_handle_addr(hs_force_count)}
+    ,
+#endif
+};
+
+#endif
+
+#ifdef UAP_SUPPORT
+static struct debug_data uap_items[] = {
+#ifdef DEBUG_LEVEL1
+    {"drvdbg", sizeof(drvdbg), (t_ptr) & drvdbg}
+    ,
+#endif
+    {"wmm_ac_vo", item_size(wmm_ac_vo), item_addr(wmm_ac_vo)}
+    ,
+    {"wmm_ac_vi", item_size(wmm_ac_vi), item_addr(wmm_ac_vi)}
+    ,
+    {"wmm_ac_be", item_size(wmm_ac_be), item_addr(wmm_ac_be)}
+    ,
+    {"wmm_ac_bk", item_size(wmm_ac_bk), item_addr(wmm_ac_bk)}
+    ,
+    {"max_tx_buf_size", item_size(max_tx_buf_size), item_addr(max_tx_buf_size)}
+    ,
+    {"tx_buf_size", item_size(tx_buf_size), item_addr(tx_buf_size)}
+    ,
+    {"curr_tx_buf_size", item_size(curr_tx_buf_size),
+     item_addr(curr_tx_buf_size)}
+    ,
+    {"ps_mode", item_size(ps_mode), item_addr(ps_mode)}
+    ,
+    {"ps_state", item_size(ps_state), item_addr(ps_state)}
+    ,
+    {"wakeup_dev_req", item_size(pm_wakeup_card_req),
+     item_addr(pm_wakeup_card_req)}
+    ,
+    {"wakeup_tries", item_size(pm_wakeup_fw_try), item_addr(pm_wakeup_fw_try)}
+    ,
+    {"hs_configured", item_size(is_hs_configured), item_addr(is_hs_configured)}
+    ,
+    {"hs_activated", item_size(hs_activated), item_addr(hs_activated)}
+    ,
+    {"tx_pkts_queued", item_size(tx_pkts_queued), item_addr(tx_pkts_queued)}
+    ,
+    {"num_bridge_pkts", item_size(num_bridge_pkts), item_addr(num_bridge_pkts)}
+    ,
+    {"num_drop_pkts", item_size(num_drop_pkts), item_addr(num_drop_pkts)}
+    ,
+    {"num_tx_timeout", item_size(num_tx_timeout), item_addr(num_tx_timeout)}
+    ,
+    {"num_cmd_timeout", item_size(num_cmd_timeout), item_addr(num_cmd_timeout)}
+    ,
+    {"timeout_cmd_id", item_size(timeout_cmd_id), item_addr(timeout_cmd_id)}
+    ,
+    {"timeout_cmd_act", item_size(timeout_cmd_act), item_addr(timeout_cmd_act)}
+    ,
+    {"last_cmd_id", item_size(last_cmd_id), item_addr(last_cmd_id)}
+    ,
+    {"last_cmd_act", item_size(last_cmd_act), item_addr(last_cmd_act)}
+    ,
+    {"last_cmd_index", item_size(last_cmd_index), item_addr(last_cmd_index)}
+    ,
+    {"last_cmd_resp_id", item_size(last_cmd_resp_id),
+     item_addr(last_cmd_resp_id)}
+    ,
+    {"last_cmd_resp_index", item_size(last_cmd_resp_index),
+     item_addr(last_cmd_resp_index)}
+    ,
+    {"last_event", item_size(last_event), item_addr(last_event)}
+    ,
+    {"last_event_index", item_size(last_event_index),
+     item_addr(last_event_index)}
+    ,
+    {"num_cmd_h2c_fail", item_size(num_cmd_host_to_card_failure),
+     item_addr(num_cmd_host_to_card_failure)}
+    ,
+    {"num_cmd_sleep_cfm_fail",
+     item_size(num_cmd_sleep_cfm_host_to_card_failure),
+     item_addr(num_cmd_sleep_cfm_host_to_card_failure)}
+    ,
+    {"num_tx_h2c_fail", item_size(num_tx_host_to_card_failure),
+     item_addr(num_tx_host_to_card_failure)}
+    ,
+    {"num_cmdevt_c2h_fail", item_size(num_cmdevt_card_to_host_failure),
+     item_addr(num_cmdevt_card_to_host_failure)}
+    ,
+    {"num_rx_c2h_fail", item_size(num_rx_card_to_host_failure),
+     item_addr(num_rx_card_to_host_failure)}
+    ,
+    {"num_int_read_fail", item_size(num_int_read_failure),
+     item_addr(num_int_read_failure)}
+    ,
+    {"last_int_status", item_size(last_int_status), item_addr(last_int_status)}
+    ,
+    {"cmd_sent", item_size(cmd_sent), item_addr(cmd_sent)}
+    ,
+    {"data_sent", item_size(data_sent), item_addr(data_sent)}
+    ,
+    {"mp_rd_bitmap", item_size(mp_rd_bitmap), item_addr(mp_rd_bitmap)}
+    ,
+    {"curr_rd_port", item_size(curr_rd_port), item_addr(curr_rd_port)}
+    ,
+    {"mp_wr_bitmap", item_size(mp_wr_bitmap), item_addr(mp_wr_bitmap)}
+    ,
+    {"curr_wr_port", item_size(curr_wr_port), item_addr(curr_wr_port)}
+    ,
+    {"cmd_resp_received", item_size(cmd_resp_received),
+     item_addr(cmd_resp_received)}
+    ,
+    {"event_received", item_size(event_received), item_addr(event_received)}
+    ,
+
+    {"ioctl_pending", item_handle_size(ioctl_pending),
+     item_handle_addr(ioctl_pending)}
+    ,
+    {"tx_pending", item_handle_size(tx_pending), item_handle_addr(tx_pending)}
+    ,
+    {"rx_pending", item_handle_size(rx_pending), item_handle_addr(rx_pending)}
+    ,
+    {"malloc_count", item_handle_size(malloc_count),
+     item_handle_addr(malloc_count)}
+    ,
+    {"lock_count", item_handle_size(lock_count), item_handle_addr(lock_count)}
+    ,
+    {"mbufalloc_count", item_handle_size(mbufalloc_count),
+     item_handle_addr(mbufalloc_count)}
+    ,
+    {"main_state", item_handle_size(main_state), item_handle_addr(main_state)}
+    ,
+#ifdef SDIO_MMC_DEBUG
+    {"sdiocmd53w", item_handle_size(cmd53w), item_handle_addr(cmd53w)}
+    ,
+    {"sdiocmd53r", item_handle_size(cmd53r), item_handle_addr(cmd53r)}
+    ,
+#endif
+#if defined(SDIO_SUSPEND_RESUME)
+    {"hs_skip_count", item_handle_size(hs_skip_count),
+     item_handle_addr(hs_skip_count)}
+    ,
+    {"hs_force_count", item_handle_size(hs_force_count),
+     item_handle_addr(hs_force_count)}
+    ,
+#endif
+};
+#endif /* UAP_SUPPORT */
+
+/********************************************************
+		Local Functions
+********************************************************/
+/**
+ *  @brief Proc read function
+ *
+ *  @param page	   Pointer to buffer
+ *  @param s       Read data starting position
+ *  @param off     Offset
+ *  @param cnt     Counter
+ *  @param eof     End of file flag
+ *  @param data    Output data
+ *
+ *  @return 	   Number of output data or MLAN_STATUS_FAILURE
+ */
+static int
+woal_debug_read(char *page, char **s, off_t off, int cnt, int *eof, void *data)
+{
+    int val = 0;
+    unsigned int i;
+    char *p = page;
+    struct debug_data *d = ((struct debug_data_priv *) data)->items;
+    moal_private *priv = ((struct debug_data_priv *) data)->priv;
+
+    ENTER();
+
+    if (MODULE_GET == 0) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    /* Get debug information */
+    if (woal_get_debug_info(priv, MOAL_PROC_WAIT, &info)) {
+        *eof = 1;
+        goto exit;
+    }
+    for (i = 0;
+         i < (unsigned int) ((struct debug_data_priv *) data)->num_of_items;
+         i++) {
+        if (d[i].size == 1)
+            val = *((t_u8 *) d[i].addr);
+        else if (d[i].size == 2)
+            val = *((t_u16 *) d[i].addr);
+        else if (d[i].size == 4)
+            val = *((t_ptr *) d[i].addr);
+        else {
+            unsigned int j;
+            p += sprintf(p, "%s=", d[i].name);
+            for (j = 0; j < d[i].size; j += 2) {
+                val = *(t_u16 *) (d[i].addr + j);
+                p += sprintf(p, "0x%x ", val);
+            }
+            p += sprintf(p, "\n");
+            continue;
+        }
+        if (strstr(d[i].name, "id") || strstr(d[i].name, "bitmap"))
+            p += sprintf(p, "%s=0x%x\n", d[i].name, val);
+        else
+            p += sprintf(p, "%s=%d\n", d[i].name, val);
+    }
+    if (info.tx_tbl_num) {
+        p += sprintf(p, "Tx BA stream table:\n");
+        for (i = 0; i < info.tx_tbl_num; i++) {
+            p += sprintf(p,
+                         "tid = %d, ra = %02x:%02x:%02x:%02x:%02x:%02x amsdu=%d\n",
+                         (int) info.tx_tbl[i].tid, info.tx_tbl[i].ra[0],
+                         info.tx_tbl[i].ra[1], info.tx_tbl[i].ra[2],
+                         info.tx_tbl[i].ra[3], info.tx_tbl[i].ra[4],
+                         info.tx_tbl[i].ra[5], (int) info.tx_tbl[i].amsdu);
+        }
+    }
+    if (info.rx_tbl_num) {
+        p += sprintf(p, "Rx reorder table:\n");
+        for (i = 0; i < info.rx_tbl_num; i++) {
+            unsigned int j;
+
+            p += sprintf(p,
+                         "tid = %d, ta =  %02x:%02x:%02x:%02x:%02x:%02x, start_win = %d, "
+                         "win_size = %d, amsdu=%d\n", (int) info.rx_tbl[i].tid,
+                         info.rx_tbl[i].ta[0], info.rx_tbl[i].ta[1],
+                         info.rx_tbl[i].ta[2], info.rx_tbl[i].ta[3],
+                         info.rx_tbl[i].ta[4], info.rx_tbl[i].ta[5],
+                         (int) info.rx_tbl[i].start_win,
+                         (int) info.rx_tbl[i].win_size,
+                         (int) info.rx_tbl[i].amsdu);
+            p += sprintf(p, "buffer: ");
+            for (j = 0; j < info.rx_tbl[i].win_size; j++) {
+                if (info.rx_tbl[i].buffer[j] == MTRUE)
+                    p += sprintf(p, "1 ");
+                else
+                    p += sprintf(p, "0 ");
+            }
+            p += sprintf(p, "\n");
+        }
+    }
+  exit:
+    MODULE_PUT;
+    LEAVE();
+    return p - page;
+}
+
+/**
+ *  @brief Proc write function
+ *
+ *  @param f	   File pointer
+ *  @param buf     Pointer to data buffer
+ *  @param cnt     Data number to write
+ *  @param data    Data to write
+ *
+ *  @return 	   Number of data or MLAN_STATUS_FAILURE
+ */
+static int
+woal_debug_write(struct file *f, const char *buf, unsigned long cnt, void *data)
+{
+    int r, i;
+    char *pdata;
+    char *p;
+    char *p0;
+    char *p1;
+    char *p2;
+    struct debug_data *d = ((struct debug_data_priv *) data)->items;
+    moal_private *priv = ((struct debug_data_priv *) data)->priv;
+#ifdef DEBUG_LEVEL1
+    t_u32 last_drvdbg = drvdbg;
+#endif
+
+    ENTER();
+
+    if (MODULE_GET == 0) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    pdata = (char *) kmalloc(cnt, GFP_KERNEL);
+    if (pdata == NULL) {
+        MODULE_PUT;
+        LEAVE();
+        return 0;
+    }
+
+    if (copy_from_user(pdata, buf, cnt)) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        kfree(pdata);
+        MODULE_PUT;
+        LEAVE();
+        return 0;
+    }
+
+    if (woal_get_debug_info(priv, MOAL_PROC_WAIT, &info)) {
+        kfree(pdata);
+        MODULE_PUT;
+        LEAVE();
+        return 0;
+    }
+
+    p0 = pdata;
+    for (i = 0; i < ((struct debug_data_priv *) data)->num_of_items; i++) {
+        do {
+            p = strstr(p0, d[i].name);
+            if (p == NULL)
+                break;
+            p1 = strchr(p, '\n');
+            if (p1 == NULL)
+                break;
+            p0 = p1++;
+            p2 = strchr(p, '=');
+            if (!p2)
+                break;
+            p2++;
+            r = woal_string_to_number(p2);
+            if (d[i].size == 1)
+                *((t_u8 *) d[i].addr) = (t_u8) r;
+            else if (d[i].size == 2)
+                *((t_u16 *) d[i].addr) = (t_u16) r;
+            else if (d[i].size == 4)
+                *((t_ptr *) d[i].addr) = (t_ptr) r;
+            break;
+        } while (MTRUE);
+    }
+    kfree(pdata);
+
+#ifdef DEBUG_LEVEL1
+    if (last_drvdbg != drvdbg)
+        woal_set_drvdbg(priv, drvdbg);
+#endif
+
+    /* Set debug information */
+    if (woal_set_debug_info(priv, MOAL_PROC_WAIT, &info)) {
+        MODULE_PUT;
+        LEAVE();
+        return 0;
+    }
+
+    MODULE_PUT;
+    LEAVE();
+    return cnt;
+}
+
+/********************************************************
+		Global Functions
+********************************************************/
+/**
+ *  @brief Create debug proc file
+ *
+ *  @param priv	   A pointer to a moal_private structure
+ *
+ *  @return 	   N/A
+ */
+void
+woal_debug_entry(moal_private * priv)
+{
+    struct proc_dir_entry *r;
+
+    ENTER();
+
+    if (priv->proc_entry == NULL) {
+        LEAVE();
+        return;
+    }
+#ifdef STA_SUPPORT
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) {
+        priv->items_priv.items =
+            (struct debug_data *) kmalloc(sizeof(items), GFP_KERNEL);
+        if (!priv->items_priv.items) {
+            PRINTM(MERROR, "Failed to allocate memory for debug data\n");
+            LEAVE();
+            return;
+        }
+        memcpy(priv->items_priv.items, items, sizeof(items));
+        priv->items_priv.num_of_items = sizeof(items) / sizeof(items[0]);
+    }
+#endif
+#ifdef UAP_SUPPORT
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP) {
+        priv->items_priv.items =
+            (struct debug_data *) kmalloc(sizeof(uap_items), GFP_KERNEL);
+        if (!priv->items_priv.items) {
+            PRINTM(MERROR, "Failed to allocate memory for debug data\n");
+            LEAVE();
+            return;
+        }
+        memcpy(priv->items_priv.items, uap_items, sizeof(uap_items));
+        priv->items_priv.num_of_items =
+            sizeof(uap_items) / sizeof(uap_items[0]);
+    }
+#endif
+    priv->items_priv.priv = priv;
+    priv->items_priv.items[priv->items_priv.num_of_items - 1].addr +=
+        (t_ptr) (priv->phandle);
+    priv->items_priv.items[priv->items_priv.num_of_items - 2].addr +=
+        (t_ptr) (priv->phandle);
+    priv->items_priv.items[priv->items_priv.num_of_items - 3].addr +=
+        (t_ptr) (priv->phandle);
+    priv->items_priv.items[priv->items_priv.num_of_items - 4].addr +=
+        (t_ptr) (priv->phandle);
+    priv->items_priv.items[priv->items_priv.num_of_items - 5].addr +=
+        (t_ptr) (priv->phandle);
+    priv->items_priv.items[priv->items_priv.num_of_items - 6].addr +=
+        (t_ptr) (priv->phandle);
+    priv->items_priv.items[priv->items_priv.num_of_items - 7].addr +=
+        (t_ptr) (priv->phandle);
+#ifdef SDIO_MMC_DEBUG
+    priv->items_priv.items[priv->items_priv.num_of_items - 8].addr +=
+        (t_ptr) (priv->phandle);
+    priv->items_priv.items[priv->items_priv.num_of_items - 9].addr +=
+        (t_ptr) (priv->phandle);
+#ifdef SDIO_SUSPEND_RESUME
+    priv->items_priv.items[priv->items_priv.num_of_items - 10].addr +=
+        (t_ptr) (priv->phandle);
+    priv->items_priv.items[priv->items_priv.num_of_items - 11].addr +=
+        (t_ptr) (priv->phandle);
+#endif
+#else
+#if defined(SDIO_SUSPEND_RESUME)
+    priv->items_priv.items[priv->items_priv.num_of_items - 8].addr +=
+        (t_ptr) (priv->phandle);
+    priv->items_priv.items[priv->items_priv.num_of_items - 9].addr +=
+        (t_ptr) (priv->phandle);
+#endif
+#endif
+
+    /* Create proc entry */
+    r = create_proc_entry("debug", 0644, priv->proc_entry);
+    if (r == NULL) {
+        LEAVE();
+        return;
+    }
+    r->data = &priv->items_priv;
+    r->read_proc = woal_debug_read;
+    r->write_proc = woal_debug_write;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,30)
+    r->owner = THIS_MODULE;
+#endif
+
+    LEAVE();
+}
+
+/**
+ *  @brief Remove proc file
+ *
+ *  @param priv	 A pointer to a moal_private structure
+ *
+ *  @return 	 N/A
+ */
+void
+woal_debug_remove(moal_private * priv)
+{
+    ENTER();
+
+    if (priv->items_priv.items)
+        kfree(priv->items_priv.items);
+    /* Remove proc entry */
+    remove_proc_entry("debug", priv->proc_entry);
+
+    LEAVE();
+}
+#endif
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_eth_ioctl.c b/drivers/net/wireless/sd8797/mlinux/moal_eth_ioctl.c
new file mode 100644
index 000000000000..31040a29ceec
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_eth_ioctl.c
@@ -0,0 +1,2176 @@
+/** @file  moal_eth_ioctl.c
+  *
+  * @brief This file contains private ioctl functions
+  *
+  * Copyright (C) 2012, Marvell International Ltd.
+  *
+  * This software file (the "File") is distributed by Marvell International
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+  * (the "License").  You may use, redistribute and/or modify this File in
+  * accordance with the terms and conditions of the License, a copy of which
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+  * this warranty disclaimer.
+  *
+  */
+
+/************************************************************************
+Change log:
+    01/05/2012: initial version
+************************************************************************/
+
+#include	"moal_main.h"
+#include	"moal_eth_ioctl.h"
+#include	"mlan_ioctl.h"
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+#include	"moal_priv.h"
+#endif
+
+#if defined(STA_CFG80211) && defined(UAP_CFG80211)
+#include	"moal_cfg80211.h"
+#endif
+#ifdef UAP_SUPPORT
+#include    "moal_uap.h"
+#endif
+/********************************************************
+                Local Variables
+********************************************************/
+
+/** Marvell private command identifier string */
+#define CMD_MARVELL     "MRVL_CMD"
+
+/** Private command: Version */
+#define PRIV_CMD_VERSION    "version"
+/** Private command: Band cfg */
+#define PRIV_CMD_BANDCFG    "bandcfg"
+/** Private command: Host cmd */
+#define PRIV_CMD_HOSTCMD    "hostcmd"
+/** Private command: Custom IE config*/
+#define PRIV_CMD_CUSTOMIE   "customie"
+/** Private command: HT Tx Cfg */
+#define PRIV_CMD_HTTXCFG    "httxcfg"
+#define PRIV_CMD_DATARATE   "getdatarate"
+#define PRIV_CMD_TXRATECFG  "txratecfg"
+#define PRIV_CMD_ESUPPMODE  "esuppmode"
+#define PRIV_CMD_PASSPHRASE "passphrase"
+#define PRIV_CMD_DEAUTH     "deauth"
+#if defined(WIFI_DIRECT_SUPPORT)
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+#define PRIV_CMD_BSSROLE    "bssrole"
+#endif
+#endif
+#ifdef STA_SUPPORT
+#define PRIV_CMD_SETUSERSCAN    "setuserscan"
+#endif
+#define PRIV_CMD_DEEPSLEEP      "deepsleep"
+#define PRIV_CMD_IPADDR         "ipaddr"
+#define PRIV_CMD_WPSSESSION     "wpssession"
+#define PRIV_CMD_OTPUSERDATA    "otpuserdata"
+#define PRIV_CMD_COUNTRYCODE    "countrycode"
+#define PRIV_CMD_TCPACKENH      "tcpackenh"
+
+/** Bands supported in Infra mode */
+static t_u8 SupportedInfraBand[] = {
+    BAND_B,
+    BAND_B | BAND_G, BAND_G,
+    BAND_GN, BAND_B | BAND_G | BAND_GN, BAND_G | BAND_GN,
+    BAND_A, BAND_B | BAND_A, BAND_B | BAND_G | BAND_A, BAND_G | BAND_A,
+    BAND_A | BAND_B | BAND_G | BAND_AN | BAND_GN,
+        BAND_A | BAND_G | BAND_AN | BAND_GN, BAND_A | BAND_AN,
+};
+
+/** Bands supported in Ad-Hoc mode */
+static t_u8 SupportedAdhocBand[] = {
+    BAND_B, BAND_B | BAND_G, BAND_G,
+    BAND_GN, BAND_B | BAND_G | BAND_GN, BAND_G | BAND_GN,
+    BAND_A,
+    BAND_AN, BAND_A | BAND_AN,
+};
+
+/********************************************************
+		Global Variables
+********************************************************/
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+#ifdef UAP_SUPPORT
+/** Network device handlers for uAP */
+extern const struct net_device_ops woal_uap_netdev_ops;
+#endif
+#ifdef STA_SUPPORT
+/** Network device handlers for STA */
+extern const struct net_device_ops woal_netdev_ops;
+#endif
+#endif
+extern int cfg80211_wext;
+
+/********************************************************
+		Local Functions
+********************************************************/
+/**
+ * @brief Parse a string to extract arguments
+ *
+ * @param pos           Pointer to the arguments string
+ * @param data          Pointer to the arguments buffer
+ * @param user_data_len Pointer to the number of arguments extracted
+ *
+ * @return              MLAN_STATUS_SUCCESS
+ *
+ * N.B. No boundary check is done on 'data'. The caller must ensure there
+ * are enough space for all extracted arguments.
+ */
+mlan_status
+parse_arguments(t_u8 * pos, int *data, int *user_data_len)
+{
+    unsigned int i, j, k;
+    char cdata[10];
+    int is_hex = 0;
+
+    memset(cdata, 0, sizeof(cdata));
+    for (i = 0, j = 0, k = 0; i <= strlen(pos); i++) {
+        if ((k == 0) && (i <= (strlen(pos) - 2))) {
+            if ((pos[i] == '0') && (pos[i + 1] == 'x')) {
+                is_hex = 1;
+                i = i + 2;
+            }
+        }
+        if (pos[i] == '\0') {
+            if (is_hex) {
+                data[j] = woal_atox(cdata);
+                is_hex = 0;
+            } else {
+                woal_atoi(&data[j], cdata);
+            }
+            j++;
+            (*user_data_len)++;
+            k = 0;
+            memset(cdata, 0, sizeof(char) * 4);
+            break;
+        } else if (pos[i] == ' ') {
+            if (is_hex) {
+                data[j] = woal_atox(cdata);
+                is_hex = 0;
+            } else {
+                woal_atoi(&data[j], cdata);
+            }
+            j++;
+            (*user_data_len)++;
+            k = 0;
+            memset(cdata, 0, sizeof(char) * 4);
+        } else {
+            cdata[k] = pos[i];
+            k++;
+        }
+    }
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+#if defined(STA_CFG80211) && defined(UAP_CFG80211)
+/**
+ *  @brief Set wps & p2p ie in AP mode
+ *
+ *  @param priv         Pointer to priv stucture
+ *  @param ie           Pointer to ies data
+ *  @param len          Length of data
+ *
+ *  @return             MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_set_ap_wps_p2p_ie(moal_private * priv, t_u8 * ie, size_t len)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u8 *pos = ie;
+    t_u32 ie_len;
+
+    ENTER();
+
+    ie_len = len - 2;
+    if (ie_len <= 0 || ie_len > MAX_IE_SIZE) {
+        PRINTM(MERROR, "IE len error: %d\n", ie_len);
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Android cmd format: "SET_AP_WPS_P2P_IE 1" -- beacon IE
+       "SET_AP_WPS_P2P_IE 2" -- proberesp IE "SET_AP_WPS_P2P_IE 4" -- assocresp
+       IE */
+    if (*pos == '1') {
+        PRINTM(MIOCTL, "Ignore set beacon ie\n");
+        goto done;
+    } else if (*pos == '2') {
+        /* set the probe resp ies */
+        pos += 2;
+        if (MLAN_STATUS_SUCCESS != woal_cfg80211_mgmt_frame_ie(priv, NULL,
+                                                               0, pos, ie_len,
+                                                               NULL, 0, NULL, 0,
+                                                               MGMT_MASK_PROBE_RESP))
+        {
+            PRINTM(MERROR, "Failed to set probe resp ie\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    } else if (*pos == '4') {
+        /* set the assoc resp ies */
+        pos += 2;
+        if (MLAN_STATUS_SUCCESS != woal_cfg80211_mgmt_frame_ie(priv, NULL,
+                                                               0, NULL, 0, pos,
+                                                               ie_len, NULL, 0,
+                                                               MGMT_MASK_ASSOC_RESP))
+        {
+            PRINTM(MERROR, "Failed to set assoc resp ie\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+#endif
+
+/**
+ *  @brief Get Driver Version
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_get_priv_driver_version(moal_private * priv, t_u8 * respbuf,
+                             t_u32 respbuflen)
+{
+    int len = 0, ret = -1;
+    char buf[MLAN_MAX_VER_STR_LEN];
+
+    ENTER();
+
+    if (!respbuf) {
+        LEAVE();
+        return 0;
+    }
+
+    memset(buf, 0, sizeof(buf));
+
+    /* Get version string to local buffer */
+    woal_get_version(priv->phandle, buf, sizeof(buf) - 1);
+    len = strlen(buf);
+
+    if (len) {
+        /* Copy back the retrieved version string */
+        PRINTM(MINFO, "MOAL VERSION: %s\n", buf);
+        ret = MIN(len, (respbuflen - 1));
+        memcpy(respbuf, buf, ret);
+    } else {
+        ret = -1;
+        PRINTM(MERROR, "Get version failed!\n");
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Hostcmd interface from application
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_priv_hostcmd(moal_private * priv, t_u8 * respbuf, t_u32 respbuflen)
+{
+    int ret = 0;
+    t_u8 *data_ptr;
+    t_u32 buf_len = 0;
+    HostCmd_Header cmd_header;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *misc_cfg = NULL;
+
+    ENTER();
+
+    data_ptr = respbuf + (strlen(CMD_MARVELL) + strlen(PRIV_CMD_HOSTCMD));
+    buf_len = *((t_u32 *) data_ptr);
+    memcpy(&cmd_header, data_ptr + sizeof(buf_len), sizeof(HostCmd_Header));
+
+    PRINTM(MINFO, "Host command len = %d\n", woal_le16_to_cpu(cmd_header.size));
+    if (woal_le16_to_cpu(cmd_header.size) > MLAN_SIZE_OF_CMD_BUFFER) {
+        LEAVE();
+        return -EINVAL;
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto error;
+    }
+    misc_cfg = (mlan_ds_misc_cfg *) req->pbuf;
+    misc_cfg->sub_command = MLAN_OID_MISC_HOST_CMD;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+    req->action = MLAN_ACT_SET;
+    misc_cfg->param.hostcmd.len = woal_le16_to_cpu(cmd_header.size);
+    /* get the whole command */
+    memcpy(misc_cfg->param.hostcmd.cmd, data_ptr + sizeof(buf_len),
+           misc_cfg->param.hostcmd.len);
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto error;
+    }
+    memcpy(data_ptr + sizeof(buf_len), misc_cfg->param.hostcmd.cmd,
+           misc_cfg->param.hostcmd.len);
+    ret =
+        misc_cfg->param.hostcmd.len + sizeof(buf_len) + strlen(CMD_MARVELL) +
+        strlen(PRIV_CMD_HOSTCMD);
+    memcpy(data_ptr, (t_u8 *) & ret, sizeof(t_u32));
+
+  error:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Custom IE setting
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_priv_customie(moal_private * priv, t_u8 * respbuf, t_u32 respbuflen)
+{
+    int ret = 0;
+    t_u8 *data_ptr;
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+    mlan_ds_misc_custom_ie *custom_ie = NULL;
+
+    ENTER();
+    data_ptr = respbuf + (strlen(CMD_MARVELL) + strlen(PRIV_CMD_CUSTOMIE));
+
+    custom_ie = (mlan_ds_misc_custom_ie *) data_ptr;
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (ioctl_req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    misc = (mlan_ds_misc_cfg *) ioctl_req->pbuf;
+    misc->sub_command = MLAN_OID_MISC_CUSTOM_IE;
+    ioctl_req->req_id = MLAN_IOCTL_MISC_CFG;
+    if ((custom_ie->len == 0) ||
+        (custom_ie->len == sizeof(custom_ie->ie_data_list[0].ie_index)))
+        ioctl_req->action = MLAN_ACT_GET;
+    else
+        ioctl_req->action = MLAN_ACT_SET;
+
+    memcpy(&misc->param.cust_ie, custom_ie, sizeof(mlan_ds_misc_custom_ie));
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    custom_ie = (mlan_ds_misc_custom_ie *) data_ptr;
+    memcpy(custom_ie, &misc->param.cust_ie, sizeof(mlan_ds_misc_custom_ie));
+    ret = sizeof(mlan_ds_misc_custom_ie);
+    if (ioctl_req->status_code == MLAN_ERROR_IOCTL_FAIL) {
+        /* send a separate error code to indicate error from driver */
+        ret = EFAULT;
+    }
+  done:
+    if (ioctl_req) {
+        kfree(ioctl_req);
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get Band and Adhoc-band setting
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_setget_priv_bandcfg(moal_private * priv, t_u8 * respbuf, t_u32 respbuflen)
+{
+    int ret = 0;
+    unsigned int i;
+    int data[4];
+    int user_data_len = 0;
+    t_u32 infra_band = 0;
+    t_u32 adhoc_band = 0;
+    t_u32 adhoc_channel = 0;
+    t_u32 adhoc_chan_bandwidth = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_radio_cfg *radio_cfg = NULL;
+    mlan_ds_band_cfg *band_cfg = NULL;
+
+    ENTER();
+
+    if (strlen(respbuf) == (strlen(CMD_MARVELL) + strlen(PRIV_CMD_BANDCFG))) {
+        /* GET operation */
+        user_data_len = 0;
+    } else {
+        /* SET operation */
+        memset((char *) data, 0, sizeof(data));
+        parse_arguments(respbuf + strlen(CMD_MARVELL) +
+                        strlen(PRIV_CMD_BANDCFG), data, &user_data_len);
+    }
+
+    if (sizeof(int) * user_data_len > sizeof(data)) {
+        PRINTM(MERROR, "Too many arguments\n");
+        LEAVE();
+        return -EINVAL;
+    }
+
+    if (user_data_len > 0) {
+        if (priv->media_connected == MTRUE) {
+            LEAVE();
+            return -EOPNOTSUPP;
+        }
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_radio_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto error;
+    }
+    radio_cfg = (mlan_ds_radio_cfg *) req->pbuf;
+    radio_cfg->sub_command = MLAN_OID_BAND_CFG;
+    req->req_id = MLAN_IOCTL_RADIO_CFG;
+
+    if (user_data_len == 0) {
+        /* Get config_bands, adhoc_start_band and adhoc_channel values from
+           MLAN */
+        req->action = MLAN_ACT_GET;
+    } else {
+        /* To support only <b/bg/bgn/n/aac/gac> */
+        infra_band = data[0];
+        for (i = 0; i < sizeof(SupportedInfraBand); i++)
+            if (infra_band == SupportedInfraBand[i])
+                break;
+        if (i == sizeof(SupportedInfraBand)) {
+            ret = -EINVAL;
+            goto error;
+        }
+
+        /* Set Adhoc band */
+        if (user_data_len >= 2) {
+            adhoc_band = data[1];
+            for (i = 0; i < sizeof(SupportedAdhocBand); i++)
+                if (adhoc_band == SupportedAdhocBand[i])
+                    break;
+            if (i == sizeof(SupportedAdhocBand)) {
+                ret = -EINVAL;
+                goto error;
+            }
+        }
+
+        /* Set Adhoc channel */
+        if (user_data_len >= 3) {
+            adhoc_channel = data[2];
+            if (adhoc_channel == 0) {
+                /* Check if specified adhoc channel is non-zero */
+                ret = -EINVAL;
+                goto error;
+            }
+        }
+        if (user_data_len == 4) {
+            if (!(adhoc_band & (BAND_GN | BAND_AN))) {
+                PRINTM(MERROR,
+                       "11n is not enabled for adhoc, can not set HT/VHT channel bandwidth\n");
+                ret = -EINVAL;
+                goto error;
+            }
+            adhoc_chan_bandwidth = data[3];
+            /* sanity test */
+            if ((adhoc_chan_bandwidth != CHANNEL_BW_20MHZ) &&
+                (adhoc_chan_bandwidth != CHANNEL_BW_40MHZ_ABOVE) &&
+                (adhoc_chan_bandwidth != CHANNEL_BW_40MHZ_BELOW)
+                ) {
+                PRINTM(MERROR,
+                       "Invalid secondary channel bandwidth, only allowed 0, 1, 3 or 4\n");
+                ret = -EINVAL;
+                goto error;
+            }
+
+        }
+        /* Set config_bands and adhoc_start_band values to MLAN */
+        req->action = MLAN_ACT_SET;
+        radio_cfg->param.band_cfg.config_bands = infra_band;
+        radio_cfg->param.band_cfg.adhoc_start_band = adhoc_band;
+        radio_cfg->param.band_cfg.adhoc_channel = adhoc_channel;
+        radio_cfg->param.band_cfg.sec_chan_offset = adhoc_chan_bandwidth;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto error;
+    }
+
+    band_cfg = (mlan_ds_band_cfg *) respbuf;
+
+    memcpy(band_cfg, &radio_cfg->param.band_cfg, sizeof(mlan_ds_band_cfg));
+
+    ret = sizeof(mlan_ds_band_cfg);
+
+  error:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get 11n configurations
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_setget_priv_httxcfg(moal_private * priv, t_u8 * respbuf, t_u32 respbuflen)
+{
+    int data[2];
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_11n_cfg *cfg_11n = NULL;
+    int ret = 0;
+    int user_data_len = 0;
+
+    ENTER();
+
+    if (strlen(respbuf) == (strlen(CMD_MARVELL) + strlen(PRIV_CMD_HTTXCFG))) {
+        /* GET operation */
+        user_data_len = 0;
+    } else {
+        /* SET operation */
+        memset((char *) data, 0, sizeof(data));
+        parse_arguments(respbuf + strlen(CMD_MARVELL) +
+                        strlen(PRIV_CMD_HTTXCFG), data, &user_data_len);
+    }
+
+    if (user_data_len > 2) {
+        PRINTM(MERROR, "Invalid number of arguments\n");
+        ret = -EINVAL;
+        goto done;
+    }
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11n_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    cfg_11n = (mlan_ds_11n_cfg *) req->pbuf;
+    cfg_11n->sub_command = MLAN_OID_11N_CFG_TX;
+    req->req_id = MLAN_IOCTL_11N_CFG;
+
+    if (user_data_len == 0) {
+        /* Get 11n tx parameters from MLAN */
+        req->action = MLAN_ACT_GET;
+        cfg_11n->param.tx_cfg.misc_cfg = BAND_SELECT_BG;
+    } else {
+        cfg_11n->param.tx_cfg.httxcap = data[0];
+        PRINTM(MINFO, "SET: httxcap:0x%x\n", data[0]);
+        cfg_11n->param.tx_cfg.misc_cfg = BAND_SELECT_BOTH;
+        if (user_data_len == 2) {
+            if (data[1] != BAND_SELECT_BG &&
+                data[1] != BAND_SELECT_A && data[1] != BAND_SELECT_BOTH) {
+                PRINTM(MERROR, "Invalid band selection\n");
+                ret = -EINVAL;
+                goto done;
+            }
+            cfg_11n->param.tx_cfg.misc_cfg = data[1];
+            PRINTM(MINFO, "SET: httxcap band:0x%x\n", data[1]);
+        }
+        /* Update 11n tx parameters in MLAN */
+        req->action = MLAN_ACT_SET;
+    }
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    data[0] = cfg_11n->param.tx_cfg.httxcap;
+
+    if (req->action == MLAN_ACT_GET) {
+        user_data_len = 1;
+        cfg_11n->param.tx_cfg.httxcap = 0;
+        cfg_11n->param.tx_cfg.misc_cfg = BAND_SELECT_A;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        if (cfg_11n->param.tx_cfg.httxcap != data[0]) {
+            user_data_len = 2;
+            data[1] = cfg_11n->param.tx_cfg.httxcap;
+            PRINTM(MINFO, "GET: httxcap for 2.4GHz:0x%x\n", data[0]);
+            PRINTM(MINFO, "GET: httxcap for 5GHz:0x%x\n", data[1]);
+        } else
+            PRINTM(MINFO, "GET: httxcap:0x%x\n", data[0]);
+    }
+
+    sprintf(respbuf, "0x%x", data[0]);
+    ret = strlen(respbuf) + 1;
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+
+}
+
+/**
+ *  @brief Set/Get 11AC configurations
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_get_priv_datarate(moal_private * priv, t_u8 * respbuf, t_u32 respbuflen)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_rate *rate = NULL;
+    mlan_data_rate *data_rate = NULL;
+    int ret = 0;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_rate));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    rate = (mlan_ds_rate *) req->pbuf;
+    rate->sub_command = MLAN_OID_GET_DATA_RATE;
+    req->req_id = MLAN_IOCTL_RATE;
+    req->action = MLAN_ACT_GET;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    data_rate = (mlan_data_rate *) respbuf;
+
+    memcpy(data_rate, &rate->param.data_rate, sizeof(mlan_data_rate));
+
+    ret = sizeof(mlan_data_rate);
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+
+}
+
+/**
+ *  @brief Set/Get tx rate configurations
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_setget_priv_txratecfg(moal_private * priv, t_u8 * respbuf,
+                           t_u32 respbuflen)
+{
+    t_u32 data[3];
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_rate *rate = NULL;
+    woal_tx_rate_cfg *ratecfg = NULL;
+    int ret = 0;
+    int user_data_len = 0;
+
+    ENTER();
+
+    if (strlen(respbuf) == (strlen(CMD_MARVELL) + strlen(PRIV_CMD_TXRATECFG))) {
+        /* GET operation */
+        user_data_len = 0;
+    } else {
+        /* SET operation */
+        memset((char *) data, 0, sizeof(data));
+        parse_arguments(respbuf + strlen(CMD_MARVELL) +
+                        strlen(PRIV_CMD_TXRATECFG), data, &user_data_len);
+    }
+
+    if (user_data_len >= 4) {
+        PRINTM(MERROR, "Invalid number of arguments\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_rate));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    req->req_id = MLAN_IOCTL_RATE;
+    rate = (mlan_ds_rate *) req->pbuf;
+    rate->sub_command = MLAN_OID_RATE_CFG;
+    rate->param.rate_cfg.rate_type = MLAN_RATE_INDEX;
+
+    if (user_data_len == 0) {
+        /* Get operation */
+        req->action = MLAN_ACT_GET;
+    } else {
+        /* Set operation */
+        req->action = MLAN_ACT_SET;
+        /* format */
+        if ((data[0] != AUTO_RATE) && (data[0] >= 3)) {
+            PRINTM(MERROR, "Invalid format selection\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        if (data[0] == AUTO_RATE) {
+            /* auto */
+            rate->param.rate_cfg.is_rate_auto = 1;
+        } else {
+            /* fixed rate */
+            PRINTM(MINFO, "SET: txratefg format: 0x%x\n", data[0]);
+            if ((data[0] != AUTO_RATE) && (data[0] > MLAN_RATE_FORMAT_HT)
+                ) {
+                PRINTM(MERROR, "Invalid format selection\n");
+                ret = -EINVAL;
+                goto done;
+            }
+        }
+
+        if ((user_data_len >= 2) && (data[0] != AUTO_RATE)) {
+            PRINTM(MINFO, "SET: txratefg index: 0x%x\n", data[1]);
+            /* sanity check */
+            if (((data[0] == MLAN_RATE_FORMAT_LG) &&
+                 (data[1] > MLAN_RATE_INDEX_OFDM7))
+                || ((data[0] == MLAN_RATE_FORMAT_HT) && (data[1] != 32) &&
+                    (data[1] > 15))
+                ) {
+                PRINTM(MERROR, "Invalid index selection\n");
+                ret = -EINVAL;
+                goto done;
+            }
+
+            PRINTM(MINFO, "SET: txratefg index: 0x%x\n", data[1]);
+            rate->param.rate_cfg.rate = data[1];
+
+            if (data[0] == MLAN_RATE_FORMAT_HT) {
+                rate->param.rate_cfg.rate = data[1] + MLAN_RATE_INDEX_MCS0;
+            }
+        }
+
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    ratecfg = (woal_tx_rate_cfg *) respbuf;
+    if (rate->param.rate_cfg.is_rate_auto == MTRUE) {
+        ratecfg->rate_format = 0xFF;
+    } else {
+        /* fixed rate */
+        if (rate->param.rate_cfg.rate < MLAN_RATE_INDEX_MCS0) {
+            ratecfg->rate_format = MLAN_RATE_FORMAT_LG;
+            ratecfg->rate_index = rate->param.rate_cfg.rate;
+        } else {
+            ratecfg->rate_format = MLAN_RATE_FORMAT_HT;
+            ratecfg->rate_index =
+                rate->param.rate_cfg.rate - MLAN_RATE_INDEX_MCS0;
+        }
+    }
+
+    ret = sizeof(woal_tx_rate_cfg);
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+
+}
+
+/**
+ *  @brief Set/Get esupplicant mode configurations
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_setget_priv_esuppmode(moal_private * priv, t_u8 * respbuf,
+                           t_u32 respbuflen)
+{
+    t_u32 data[3];
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    woal_esuppmode_cfg *esupp_mode = NULL;
+    int ret = 0;
+    int user_data_len = 0;
+
+    ENTER();
+
+    if (strlen(respbuf) == (strlen(CMD_MARVELL) + strlen(PRIV_CMD_ESUPPMODE))) {
+        /* GET operation */
+        user_data_len = 0;
+    } else {
+        /* SET operation */
+        memset((char *) data, 0, sizeof(data));
+        parse_arguments(respbuf + strlen(CMD_MARVELL) +
+                        strlen(PRIV_CMD_ESUPPMODE), data, &user_data_len);
+    }
+
+    if (user_data_len >= 4 || user_data_len == 1 || user_data_len == 2) {
+        PRINTM(MERROR, "Invalid number of arguments\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_ESUPP_MODE;
+
+    if (user_data_len == 0) {
+        /* Get operation */
+        req->action = MLAN_ACT_GET;
+    } else {
+        /* Set operation */
+        req->action = MLAN_ACT_SET;
+        /* RSN mode */
+        sec->param.esupp_mode.rsn_mode = data[0];
+        /* Pairwise cipher */
+        sec->param.esupp_mode.act_paircipher = (data[1] & 0xFF);
+        /* Group cipher */
+        sec->param.esupp_mode.act_groupcipher = (data[2] & 0xFF);
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    esupp_mode = (woal_esuppmode_cfg *) respbuf;
+    esupp_mode->rsn_mode = (t_u16) ((sec->param.esupp_mode.rsn_mode) & 0xFFFF);
+    esupp_mode->pairwise_cipher =
+        (t_u8) ((sec->param.esupp_mode.act_paircipher) & 0xFF);
+    esupp_mode->group_cipher =
+        (t_u8) ((sec->param.esupp_mode.act_groupcipher) & 0xFF);
+
+    ret = sizeof(woal_esuppmode_cfg);
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+
+}
+
+/**
+ *  @brief Set/Get esupplicant passphrase configurations
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_setget_priv_passphrase(moal_private * priv, t_u8 * respbuf,
+                            t_u32 respbuflen)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    int ret = 0, action = -1, i = 0;
+    char *begin, *end, *opt;
+    t_u16 len = 0;
+    t_u8 zero_mac[] = { 0, 0, 0, 0, 0, 0 };
+    t_u8 *mac = NULL;
+
+    ENTER();
+
+    if (strlen(respbuf) == (strlen(CMD_MARVELL) + strlen(PRIV_CMD_PASSPHRASE))) {
+        PRINTM(MERROR, "No arguments provided\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    /* Parse the buf to get the cmd_action */
+    begin = respbuf + strlen(CMD_MARVELL) + strlen(PRIV_CMD_PASSPHRASE);
+    end = woal_strsep(&begin, ';', '/');
+    if (end)
+        action = woal_atox(end);
+    if (action < 0 || action > 2 || end[1] != '\0') {
+        PRINTM(MERROR, "Invalid action argument %s\n", end);
+        ret = -EINVAL;
+        goto done;
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_PASSPHRASE;
+    if (action == 0)
+        req->action = MLAN_ACT_GET;
+    else
+        req->action = MLAN_ACT_SET;
+
+    while (begin) {
+        end = woal_strsep(&begin, ';', '/');
+        opt = woal_strsep(&end, '=', '/');
+        if (!opt || !end || !end[0]) {
+            PRINTM(MERROR, "Invalid option\n");
+            ret = -EINVAL;
+            break;
+        } else if (!strnicmp(opt, "ssid", strlen(opt))) {
+            if (strlen(end) > MLAN_MAX_SSID_LENGTH) {
+                PRINTM(MERROR, "SSID length exceeds max length\n");
+                ret = -EFAULT;
+                break;
+            }
+            sec->param.passphrase.ssid.ssid_len = strlen(end);
+            strncpy((char *) sec->param.passphrase.ssid.ssid, end, strlen(end));
+            PRINTM(MINFO, "ssid=%s, len=%d\n", sec->param.passphrase.ssid.ssid,
+                   (int) sec->param.passphrase.ssid.ssid_len);
+        } else if (!strnicmp(opt, "bssid", strlen(opt))) {
+            woal_mac2u8((t_u8 *) & sec->param.passphrase.bssid, end);
+        } else if (!strnicmp(opt, "psk", strlen(opt)) &&
+                   req->action == MLAN_ACT_SET) {
+            if (strlen(end) != MLAN_PMK_HEXSTR_LENGTH) {
+                PRINTM(MERROR, "Invalid PMK length\n");
+                ret = -EINVAL;
+                break;
+            }
+            woal_ascii2hex((t_u8 *) (sec->param.passphrase.psk.pmk.pmk), end,
+                           MLAN_PMK_HEXSTR_LENGTH / 2);
+            sec->param.passphrase.psk_type = MLAN_PSK_PMK;
+        } else if (!strnicmp(opt, "passphrase", strlen(opt)) &&
+                   req->action == MLAN_ACT_SET) {
+            if (strlen(end) < MLAN_MIN_PASSPHRASE_LENGTH ||
+                strlen(end) > MLAN_MAX_PASSPHRASE_LENGTH) {
+                PRINTM(MERROR, "Invalid length for passphrase\n");
+                ret = -EINVAL;
+                break;
+            }
+            sec->param.passphrase.psk_type = MLAN_PSK_PASSPHRASE;
+            strncpy(sec->param.passphrase.psk.passphrase.passphrase, end,
+                    sizeof(sec->param.passphrase.psk.passphrase.passphrase));
+            sec->param.passphrase.psk.passphrase.passphrase_len = strlen(end);
+            PRINTM(MINFO, "passphrase=%s, len=%d\n",
+                   sec->param.passphrase.psk.passphrase.passphrase,
+                   (int) sec->param.passphrase.psk.passphrase.passphrase_len);
+        } else {
+            PRINTM(MERROR, "Invalid option %s\n", opt);
+            ret = -EINVAL;
+            break;
+        }
+    }
+    if (ret)
+        goto done;
+
+    if (action == 2)
+        sec->param.passphrase.psk_type = MLAN_PSK_CLEAR;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    memset(respbuf, 0, respbuflen);
+    if (sec->param.passphrase.ssid.ssid_len) {
+        len += sprintf(respbuf + len, "ssid:");
+        memcpy(respbuf + len, sec->param.passphrase.ssid.ssid,
+               sec->param.passphrase.ssid.ssid_len);
+        len += sec->param.passphrase.ssid.ssid_len;
+        len += sprintf(respbuf + len, " ");
+    }
+    if (memcmp(&sec->param.passphrase.bssid, zero_mac, sizeof(zero_mac))) {
+        mac = (t_u8 *) & sec->param.passphrase.bssid;
+        len += sprintf(respbuf + len, "bssid:");
+        for (i = 0; i < ETH_ALEN - 1; ++i)
+            len += sprintf(respbuf + len, "%02x:", mac[i]);
+        len += sprintf(respbuf + len, "%02x ", mac[i]);
+    }
+    if (sec->param.passphrase.psk_type == MLAN_PSK_PMK) {
+        len += sprintf(respbuf + len, "psk:");
+        for (i = 0; i < MLAN_MAX_KEY_LENGTH; ++i)
+            len +=
+                sprintf(respbuf + len, "%02x",
+                        sec->param.passphrase.psk.pmk.pmk[i]);
+        len += sprintf(respbuf + len, "\n");
+    }
+    if (sec->param.passphrase.psk_type == MLAN_PSK_PASSPHRASE) {
+        len +=
+            sprintf(respbuf + len, "passphrase:%s \n",
+                    sec->param.passphrase.psk.passphrase.passphrase);
+    }
+
+    ret = len;
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+
+}
+
+/**
+ *  @brief Deauthenticate
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_priv_deauth(moal_private * priv, t_u8 * respbuf, t_u32 respbuflen)
+{
+    mlan_ioctl_req *req = NULL;
+    int ret = 0;
+    t_u8 mac[ETH_ALEN];
+
+    ENTER();
+
+    if (strlen(respbuf) > (strlen(CMD_MARVELL) + strlen(PRIV_CMD_DEAUTH))) {
+        /* Deauth mentioned BSSID */
+        woal_mac2u8(mac,
+                    respbuf + strlen(CMD_MARVELL) + strlen(PRIV_CMD_DEAUTH));
+        if (MLAN_STATUS_SUCCESS != woal_disconnect(priv, MOAL_IOCTL_WAIT, mac)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    } else {
+        if (MLAN_STATUS_SUCCESS != woal_disconnect(priv, MOAL_IOCTL_WAIT, NULL))
+            ret = -EFAULT;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+#if defined(WIFI_DIRECT_SUPPORT)
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+/**
+ *  @brief Set/Get BSS role
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_priv_bssrole(moal_private * priv, t_u8 * respbuf, t_u32 respbuflen)
+{
+    t_u32 data[1];
+    int ret = 0;
+    int user_data_len = 0;
+    t_u8 action = MLAN_ACT_GET;
+
+    ENTER();
+
+    if (strlen(respbuf) == (strlen(CMD_MARVELL) + strlen(PRIV_CMD_BSSROLE))) {
+        /* GET operation */
+        user_data_len = 0;
+    } else {
+        /* SET operation */
+        memset((char *) data, 0, sizeof(data));
+        parse_arguments(respbuf + strlen(CMD_MARVELL) +
+                        strlen(PRIV_CMD_BSSROLE), data, &user_data_len);
+    }
+
+    if (user_data_len >= 2) {
+        PRINTM(MERROR, "Invalid number of arguments\n");
+        ret = -EINVAL;
+        goto error;
+    }
+
+    if (user_data_len == 0) {
+        action = MLAN_ACT_GET;
+    } else {
+        if ((data[0] != MLAN_BSS_ROLE_STA &&
+             data[0] != MLAN_BSS_ROLE_UAP) ||
+            priv->bss_type != MLAN_BSS_TYPE_WIFIDIRECT) {
+            PRINTM(MWARN, "Invalid BSS role\n");
+            ret = -EINVAL;
+            goto error;
+        }
+        if (data[0] == GET_BSS_ROLE(priv)) {
+            PRINTM(MWARN, "Already BSS is in desired role\n");
+            goto done;
+        }
+        action = MLAN_ACT_SET;
+        /* Reset interface */
+        woal_reset_intf(priv, MOAL_IOCTL_WAIT, MFALSE);
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_bss_role_cfg(priv,
+                                                 action, MOAL_IOCTL_WAIT,
+                                                 (t_u8 *) data)) {
+        ret = -EFAULT;
+        goto error;
+    }
+
+    if (user_data_len) {
+        /* Initialize private structures */
+        woal_init_priv(priv, MOAL_IOCTL_WAIT);
+        /* Enable interfaces */
+        netif_device_attach(priv->netdev);
+        woal_start_queue(priv->netdev);
+    }
+
+  done:
+    memset(respbuf, 0, respbuflen);
+    respbuf[0] = (t_u8) data[0];
+    ret = 1;
+
+  error:
+    LEAVE();
+    return ret;
+}
+#endif /* STA_SUPPORT && UAP_SUPPORT */
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+
+#ifdef STA_SUPPORT
+/**
+ *  @brief Set user scan
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_priv_setuserscan(moal_private * priv, t_u8 * respbuf, t_u32 respbuflen)
+{
+    wlan_user_scan_cfg scan_cfg;
+    int ret = 0;
+
+    ENTER();
+
+    /* Create the scan_cfg structure */
+    memset(&scan_cfg, 0, sizeof(scan_cfg));
+
+    /* We expect the scan_cfg structure to be passed in respbuf */
+    memcpy((char *) &scan_cfg,
+           respbuf + strlen(CMD_MARVELL) + strlen(PRIV_CMD_SETUSERSCAN),
+           sizeof(wlan_user_scan_cfg));
+
+    /* Call for scan */
+    if (MLAN_STATUS_FAILURE == woal_do_scan(priv, &scan_cfg))
+        ret = -EFAULT;
+
+    LEAVE();
+    return ret;
+}
+#endif
+
+/**
+ *  @brief Set/Get deep sleep mode configurations
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_priv_setgetdeepsleep(moal_private * priv, t_u8 * respbuf, t_u32 respbuflen)
+{
+    t_u32 data[2];
+    int ret = 0;
+    int user_data_len = 0;
+
+    ENTER();
+
+    if (strlen(respbuf) == (strlen(CMD_MARVELL) + strlen(PRIV_CMD_DEEPSLEEP))) {
+        /* GET operation */
+        user_data_len = 0;
+    } else {
+        /* SET operation */
+        memset((char *) data, 0, sizeof(data));
+        parse_arguments(respbuf + strlen(CMD_MARVELL) +
+                        strlen(PRIV_CMD_DEEPSLEEP), data, &user_data_len);
+    }
+
+    if (user_data_len >= 3) {
+        PRINTM(MERROR, "Invalid number of arguments\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    if (user_data_len == 0) {
+        if (MLAN_STATUS_SUCCESS != woal_get_deep_sleep(priv, data)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        sprintf(respbuf, "%d %d", data[0], data[1]);
+        ret = strlen(respbuf) + 1;
+    } else {
+        if (data[0] == DEEP_SLEEP_OFF) {
+            PRINTM(MINFO, "Exit Deep Sleep Mode\n");
+            ret = woal_set_deep_sleep(priv, MOAL_IOCTL_WAIT, MFALSE, 0);
+            if (ret != MLAN_STATUS_SUCCESS) {
+                ret = -EINVAL;
+                goto done;
+            }
+        } else if (data[0] == DEEP_SLEEP_ON) {
+            PRINTM(MINFO, "Enter Deep Sleep Mode\n");
+            if (user_data_len != 2)
+                data[1] = 0;
+            ret = woal_set_deep_sleep(priv, MOAL_IOCTL_WAIT, MTRUE, data[1]);
+            if (ret != MLAN_STATUS_SUCCESS) {
+                ret = -EINVAL;
+                goto done;
+            }
+        } else {
+            PRINTM(MERROR, "Unknown option = %u\n", data[0]);
+            ret = -EINVAL;
+            goto done;
+        }
+        ret = sprintf(respbuf, "OK\n") + 1;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+
+}
+
+/**
+ *  @brief Set/Get IP address configurations
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_priv_setgetipaddr(moal_private * priv, t_u8 * respbuf, t_u32 respbuflen)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+    int ret = 0, op_code = 0, data_length = 0, header = 0;
+
+    ENTER();
+
+    header = strlen(CMD_MARVELL) + strlen(PRIV_CMD_IPADDR);
+    data_length = strlen(respbuf) - header;
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    misc = (mlan_ds_misc_cfg *) req->pbuf;
+
+    if (data_length < 1) {      /* GET */
+        req->action = MLAN_ACT_GET;
+    } else {
+        /* Make sure we have the operation argument */
+        if (data_length > 2 && respbuf[header + 1] != ';') {
+            PRINTM(MERROR, "No operation argument. Separate with ';'\n");
+            ret = -EINVAL;
+            goto done;
+        } else {
+            respbuf[header + 1] = '\0';
+        }
+        req->action = MLAN_ACT_SET;
+
+        /* Only one IP is supported in current firmware */
+        memset(misc->param.ipaddr_cfg.ip_addr[0], 0, IPADDR_LEN);
+        in4_pton(&respbuf[header + 2],
+                 MIN((IPADDR_MAX_BUF - 3), (data_length - 2)),
+                 misc->param.ipaddr_cfg.ip_addr[0], ' ', NULL);
+        misc->param.ipaddr_cfg.ip_addr_num = 1;
+        misc->param.ipaddr_cfg.ip_addr_type = IPADDR_TYPE_IPV4;
+
+        if (woal_atoi(&op_code, &respbuf[header]) != MLAN_STATUS_SUCCESS) {
+            ret = -EINVAL;
+            goto done;
+        }
+        misc->param.ipaddr_cfg.op_code = (t_u32) op_code;
+    }
+
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+    misc->sub_command = MLAN_OID_MISC_IP_ADDR;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (req->action == MLAN_ACT_GET) {
+        snprintf(respbuf, IPADDR_MAX_BUF, "%d;%d.%d.%d.%d",
+                 misc->param.ipaddr_cfg.op_code,
+                 misc->param.ipaddr_cfg.ip_addr[0][0],
+                 misc->param.ipaddr_cfg.ip_addr[0][1],
+                 misc->param.ipaddr_cfg.ip_addr[0][2],
+                 misc->param.ipaddr_cfg.ip_addr[0][3]);
+        ret = IPADDR_MAX_BUF + 1;
+    } else {
+        ret = sprintf(respbuf, "OK\n") + 1;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get WPS session configurations
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_priv_setwpssession(moal_private * priv, t_u8 * respbuf, t_u32 respbuflen)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_wps_cfg *pwps = NULL;
+    t_u32 data[1];
+    int ret = 0;
+    int user_data_len = 0;
+
+    ENTER();
+
+    if (strlen(respbuf) == (strlen(CMD_MARVELL) + strlen(PRIV_CMD_WPSSESSION))) {
+        /* GET operation */
+        user_data_len = 0;
+    } else {
+        /* SET operation */
+        memset((char *) data, 0, sizeof(data));
+        parse_arguments(respbuf + strlen(CMD_MARVELL) +
+                        strlen(PRIV_CMD_WPSSESSION), data, &user_data_len);
+    }
+
+    if (user_data_len != 1) {
+        PRINTM(MERROR, "Invalid number of arguments\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_wps_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    pwps = (mlan_ds_wps_cfg *) req->pbuf;
+
+    req->req_id = MLAN_IOCTL_WPS_CFG;
+    req->action = MLAN_ACT_SET;
+    pwps->sub_command = MLAN_OID_WPS_CFG_SESSION;
+
+    if (data[0] == 1)
+        pwps->param.wps_session = MLAN_WPS_CFG_SESSION_START;
+    else
+        pwps->param.wps_session = MLAN_WPS_CFG_SESSION_END;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    ret = sprintf(respbuf, "OK\n") + 1;
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get OTP user data
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_priv_otpuserdata(moal_private * priv, t_u8 * respbuf, t_u32 respbuflen)
+{
+    int data[1];
+    int user_data_len = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+    mlan_ds_misc_otp_user_data *otp = NULL;
+    int ret = 0;
+
+    ENTER();
+
+    memset((char *) data, 0, sizeof(data));
+    parse_arguments(respbuf + strlen(CMD_MARVELL) +
+                    strlen(PRIV_CMD_OTPUSERDATA), data, &user_data_len);
+
+    if (user_data_len > 1) {
+        PRINTM(MERROR, "Too many arguments\n");
+        LEAVE();
+        return -EINVAL;
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    req->action = MLAN_ACT_GET;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+
+    misc = (mlan_ds_misc_cfg *) req->pbuf;
+    misc->sub_command = MLAN_OID_MISC_OTP_USER_DATA;
+    misc->param.otp_user_data.user_data_length = data[0];
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    otp = (mlan_ds_misc_otp_user_data *) req->pbuf;
+
+    if (req->action == MLAN_ACT_GET) {
+        ret = MIN(otp->user_data_length, data[0]);
+        memcpy(respbuf, otp->user_data, ret);
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set / Get country code
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_priv_set_get_countrycode(moal_private * priv, t_u8 * respbuf,
+                              t_u32 respbuflen)
+{
+    int ret = 0;
+    // char data[COUNTRY_CODE_LEN] = {0, 0, 0};
+    int header = 0, data_length = 0;    // wrq->u.data.length;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *pcfg_misc = NULL;
+    mlan_ds_misc_country_code *country_code = NULL;
+
+    ENTER();
+
+    header = strlen(CMD_MARVELL) + strlen(PRIV_CMD_COUNTRYCODE);
+    data_length = strlen(respbuf) - header;
+
+    if (data_length > COUNTRY_CODE_LEN) {
+        PRINTM(MERROR, "Invalid argument!\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    pcfg_misc = (mlan_ds_misc_cfg *) req->pbuf;
+    country_code = &pcfg_misc->param.country_code;
+    pcfg_misc->sub_command = MLAN_OID_MISC_COUNTRY_CODE;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+
+    if (data_length <= 1) {
+        req->action = MLAN_ACT_GET;
+    } else {
+        memset(country_code->country_code, 0, COUNTRY_CODE_LEN);
+        memcpy(country_code->country_code, respbuf + header, COUNTRY_CODE_LEN);
+        req->action = MLAN_ACT_SET;
+    }
+
+    /* Send IOCTL request to MLAN */
+    if (woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT) != MLAN_STATUS_SUCCESS) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (req->action == MLAN_ACT_GET) {
+        ret = data_length = COUNTRY_CODE_LEN;
+        memset(respbuf + header, 0, COUNTRY_CODE_LEN);
+        memcpy(respbuf, country_code->country_code, COUNTRY_CODE_LEN);
+    }
+
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get TCP Ack enhancement configurations
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param respbuf      A pointer to response buffer
+ *  @param respbuflen   Available length of response buffer
+ *
+ *  @return             Number of bytes written, negative for failure.
+ */
+int
+woal_priv_setgettcpackenh(moal_private * priv, t_u8 * respbuf, t_u32 respbuflen)
+{
+    t_u32 data[1];
+    int ret = 0;
+    int user_data_len = 0;
+    struct list_head *link = NULL;
+    struct tcp_sess *tcp_sess = NULL;
+
+    ENTER();
+
+    if (strlen(respbuf) == (strlen(CMD_MARVELL) + strlen(PRIV_CMD_TCPACKENH))) {
+        /* GET operation */
+        user_data_len = 0;
+    } else {
+        /* SET operation */
+        memset((char *) data, 0, sizeof(data));
+        parse_arguments(respbuf + strlen(CMD_MARVELL) +
+                        strlen(PRIV_CMD_TCPACKENH), data, &user_data_len);
+    }
+
+    if (user_data_len >= 2) {
+        PRINTM(MERROR, "Invalid number of arguments\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    if (user_data_len == 0) {
+        /* get operation */
+        respbuf[0] = priv->enable_tcp_ack_enh;
+    } else {
+        /* set operation */
+        if (data[0] == MTRUE) {
+            PRINTM(MINFO, "Enabling TCP Ack enhancement\n");
+            priv->enable_tcp_ack_enh = MTRUE;
+        } else if (data[0] == MFALSE) {
+            PRINTM(MINFO, "Disabling TCP Ack enhancement\n");
+            priv->enable_tcp_ack_enh = MFALSE;
+            /* release the tcp sessions if any */
+            while (!list_empty(&priv->tcp_sess_queue)) {
+                link = priv->tcp_sess_queue.next;
+                tcp_sess = list_entry(link, struct tcp_sess, link);
+                PRINTM(MINFO,
+                       "Disable TCP ACK Enh: release a tcp session in dl. (src: ipaddr 0x%08x, port: %d. dst: ipaddr 0x%08x, port: %d\n",
+                       tcp_sess->src_ip_addr, tcp_sess->src_tcp_port,
+                       tcp_sess->dst_ip_addr, tcp_sess->dst_tcp_port);
+                list_del(link);
+                kfree(tcp_sess);
+            }
+        } else {
+            PRINTM(MERROR, "Unknown option = %u\n", data[0]);
+            ret = -EINVAL;
+            goto done;
+        }
+        respbuf[0] = priv->enable_tcp_ack_enh;
+    }
+    ret = 1;
+
+  done:
+    LEAVE();
+    return ret;
+
+}
+
+/**
+ *  @brief Set priv command for Android
+ *  @param dev          A pointer to net_device structure
+ *  @param req          A pointer to ifreq structure
+ *
+ *  @return             0 --success, otherwise fail
+ */
+int
+woal_android_priv_cmd(struct net_device *dev, struct ifreq *req)
+{
+    int ret = 0;
+    android_wifi_priv_cmd priv_cmd;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    char *buf = NULL;
+    char *pdata;
+#ifdef STA_SUPPORT
+    int power_mode = 0;
+    int band = 0;
+    char *pband = NULL;
+    mlan_bss_info bss_info;
+    mlan_ds_get_signal signal;
+    mlan_rate_cfg_t rate;
+    t_u8 country_code[COUNTRY_CODE_LEN];
+#endif
+    int len = 0;
+
+    ENTER();
+    if (copy_from_user(&priv_cmd, req->ifr_data, sizeof(android_wifi_priv_cmd))) {
+        ret = -EFAULT;
+        goto done;
+    }
+    buf = kzalloc(priv_cmd.total_len, GFP_KERNEL);
+    if (!buf) {
+        PRINTM(MERROR, "%s: failed to allocate memory\n", __FUNCTION__);
+        ret = -ENOMEM;
+        goto done;
+    }
+    if (copy_from_user(buf, priv_cmd.buf, priv_cmd.total_len)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    PRINTM(MIOCTL, "Android priv cmd: [%s] on [%s]\n", buf, req->ifr_name);
+
+    if (strncmp(buf, CMD_MARVELL, strlen(CMD_MARVELL)) == 0) {
+        /* This command has come from mlanutl app */
+
+        /* Check command */
+        if (strnicmp
+            (buf + strlen(CMD_MARVELL), PRIV_CMD_VERSION,
+             strlen(PRIV_CMD_VERSION)) == 0) {
+            /* Get version */
+            len = woal_get_priv_driver_version(priv, buf, priv_cmd.total_len);
+            goto handled;
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_BANDCFG,
+                 strlen(PRIV_CMD_BANDCFG)) == 0) {
+            /* Set/Get band configuration */
+            len = woal_setget_priv_bandcfg(priv, buf, priv_cmd.total_len);
+            goto handled;
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_HOSTCMD,
+                 strlen(PRIV_CMD_HOSTCMD)) == 0) {
+            /* hostcmd configuration */
+            len = woal_priv_hostcmd(priv, buf, priv_cmd.total_len);
+            goto handled;
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_HTTXCFG,
+                 strlen(PRIV_CMD_HTTXCFG)) == 0) {
+            /* Set/Get HT Tx configuration */
+            len = woal_setget_priv_httxcfg(priv, buf, priv_cmd.total_len);
+            goto handled;
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_DATARATE,
+                 strlen(PRIV_CMD_DATARATE)) == 0) {
+            /* Get data rate */
+            len = woal_get_priv_datarate(priv, buf, priv_cmd.total_len);
+            goto handled;
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_TXRATECFG,
+                 strlen(PRIV_CMD_TXRATECFG)) == 0) {
+            /* Set/Get tx rate cfg */
+            len = woal_setget_priv_txratecfg(priv, buf, priv_cmd.total_len);
+            goto handled;
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_CUSTOMIE,
+                 strlen(PRIV_CMD_CUSTOMIE)) == 0) {
+            /* Custom IE configuration */
+            len = woal_priv_customie(priv, buf, priv_cmd.total_len);
+            goto handled;
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_ESUPPMODE,
+                 strlen(PRIV_CMD_ESUPPMODE)) == 0) {
+            /* Esupplicant mode configuration */
+            len = woal_setget_priv_esuppmode(priv, buf, priv_cmd.total_len);
+            goto handled;
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_PASSPHRASE,
+                 strlen(PRIV_CMD_PASSPHRASE)) == 0) {
+            /* Esupplicant passphrase configuration */
+            len = woal_setget_priv_passphrase(priv, buf, priv_cmd.total_len);
+            goto handled;
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_DEAUTH,
+                 strlen(PRIV_CMD_DEAUTH)) == 0) {
+            /* Deauth */
+            len = woal_priv_deauth(priv, buf, priv_cmd.total_len);
+            goto handled;
+#if defined(WIFI_DIRECT_SUPPORT)
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_BSSROLE,
+                 strlen(PRIV_CMD_BSSROLE)) == 0) {
+            /* BSS Role */
+            len = woal_priv_bssrole(priv, buf, priv_cmd.total_len);
+            goto handled;
+#endif
+#endif
+#ifdef STA_SUPPORT
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_SETUSERSCAN,
+                 strlen(PRIV_CMD_SETUSERSCAN)) == 0) {
+            /* Set user scan */
+            len = woal_priv_setuserscan(priv, buf, priv_cmd.total_len);
+            goto handled;
+#endif
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_DEEPSLEEP,
+                 strlen(PRIV_CMD_DEEPSLEEP)) == 0) {
+            /* Deep sleep */
+            len = woal_priv_setgetdeepsleep(priv, buf, priv_cmd.total_len);
+            goto handled;
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_IPADDR,
+                 strlen(PRIV_CMD_IPADDR)) == 0) {
+            /* IP address */
+            len = woal_priv_setgetipaddr(priv, buf, priv_cmd.total_len);
+            goto handled;
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_WPSSESSION,
+                 strlen(PRIV_CMD_WPSSESSION)) == 0) {
+            /* WPS Session */
+            len = woal_priv_setwpssession(priv, buf, priv_cmd.total_len);
+            goto handled;
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_OTPUSERDATA,
+                 strlen(PRIV_CMD_OTPUSERDATA)) == 0) {
+            /* OTP user data */
+            len = woal_priv_otpuserdata(priv, buf, priv_cmd.total_len);
+            goto handled;
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_COUNTRYCODE,
+                 strlen(PRIV_CMD_COUNTRYCODE)) == 0) {
+            /* OTP user data */
+            len = woal_priv_set_get_countrycode(priv, buf, priv_cmd.total_len);
+            goto handled;
+        } else
+            if (strnicmp
+                (buf + strlen(CMD_MARVELL), PRIV_CMD_TCPACKENH,
+                 strlen(PRIV_CMD_TCPACKENH)) == 0) {
+            /* OTP user data */
+            len = woal_priv_setgettcpackenh(priv, buf, priv_cmd.total_len);
+            goto handled;
+        } else {
+            /* Fall through, after stripping off the custom header */
+            buf += strlen(CMD_MARVELL);
+        }
+    }
+#ifdef STA_SUPPORT
+    if (strncmp(buf, "RSSILOW-THRESHOLD", strlen("RSSILOW-THRESHOLD")) == 0) {
+        pdata = buf + strlen("RSSILOW-THRESHOLD") + 1;
+        if (MLAN_STATUS_SUCCESS != woal_set_rssi_low_threshold(priv, pdata)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "RSSI", strlen("RSSI")) == 0) {
+        if (MLAN_STATUS_SUCCESS != woal_get_bss_info(priv,
+                                                     MOAL_IOCTL_WAIT,
+                                                     &bss_info)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        if (bss_info.media_connected) {
+            if (MLAN_STATUS_SUCCESS != woal_get_signal_info(priv,
+                                                            MOAL_IOCTL_WAIT,
+                                                            &signal)) {
+                ret = -EFAULT;
+                goto done;
+            }
+            len = sprintf(buf, "%s rssi %d\n", bss_info.ssid.ssid,
+                          signal.bcn_rssi_avg) + 1;
+        } else {
+            len = sprintf(buf, "OK\n") + 1;
+        }
+    } else if (strncmp(buf, "LINKSPEED", strlen("LINKSPEED")) == 0) {
+        if (MLAN_STATUS_SUCCESS != woal_set_get_data_rate(priv, MLAN_ACT_GET,
+                                                          &rate)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        PRINTM(MIOCTL, "tx rate=%d\n", (int) rate.rate);
+        len =
+            sprintf(buf, "LinkSpeed %d\n", (int) (rate.rate * 500000 / 1000000))
+            + 1;
+    } else
+#endif
+    if (strncmp(buf, "MACADDR", strlen("MACADDR")) == 0) {
+        len = sprintf(buf, "Macaddr = %02X:%02X:%02X:%02X:%02X:%02X\n",
+                      priv->current_addr[0], priv->current_addr[1],
+                      priv->current_addr[2], priv->current_addr[3],
+                      priv->current_addr[4], priv->current_addr[5]) + 1;
+    }
+#ifdef STA_SUPPORT
+    else if (strncmp(buf, "GETPOWER", strlen("GETPOWER")) == 0) {
+        if (MLAN_STATUS_SUCCESS != woal_get_powermode(priv, &power_mode)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "powermode = %d\n", power_mode) + 1;
+    } else if (strncmp(buf, "SCAN-ACTIVE", strlen("SCAN-ACTIVE")) == 0) {
+        if (MLAN_STATUS_SUCCESS != woal_set_scan_type(priv,
+                                                      MLAN_SCAN_TYPE_ACTIVE)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        priv->scan_type = MLAN_SCAN_TYPE_ACTIVE;
+        PRINTM(MIOCTL, "Set Active Scan\n");
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "SCAN-PASSIVE", strlen("SCAN-PASSIVE")) == 0) {
+        if (MLAN_STATUS_SUCCESS != woal_set_scan_type(priv,
+                                                      MLAN_SCAN_TYPE_PASSIVE)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        priv->scan_type = MLAN_SCAN_TYPE_PASSIVE;
+        PRINTM(MIOCTL, "Set Passive Scan\n");
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "POWERMODE", strlen("POWERMODE")) == 0) {
+        pdata = buf + strlen("POWERMODE") + 1;
+        if (MLAN_STATUS_SUCCESS != woal_set_powermode(priv, pdata)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "COUNTRY", strlen("COUNTRY")) == 0) {
+        memset(country_code, 0, sizeof(country_code));
+        memcpy(country_code, buf + strlen("COUNTRY") + 1,
+               strlen(buf) - strlen("COUNTRY") - 1);
+        PRINTM(MIOCTL, "Set COUNTRY %s\n", country_code);
+        if (MLAN_STATUS_SUCCESS != woal_set_region_code(priv, country_code)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (memcmp(buf, WEXT_CSCAN_HEADER, WEXT_CSCAN_HEADER_SIZE) == 0) {
+        PRINTM(MIOCTL, "Set Combo Scan\n");
+        if (MLAN_STATUS_SUCCESS != woal_set_combo_scan(priv, buf,
+                                                       priv_cmd.total_len)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "GETBAND", strlen("GETBAND")) == 0) {
+        if (MLAN_STATUS_SUCCESS != woal_get_band(priv, &band)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "Band %d\n", band) + 1;
+    } else if (strncmp(buf, "SETBAND", strlen("SETBAND")) == 0) {
+        pband = buf + strlen("SETBAND") + 1;
+        if (MLAN_STATUS_SUCCESS != woal_set_band(priv, pband)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    }
+#endif
+    else if (strncmp(buf, "START", strlen("START")) == 0) {
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "STOP", strlen("STOP")) == 0) {
+        len = sprintf(buf, "OK\n") + 1;
+    }
+#ifdef UAP_SUPPORT
+    else if (strncmp(buf, "AP_BSS_START", strlen("AP_BSS_START")) == 0) {
+        if ((ret == woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_START)))
+            goto done;
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "AP_BSS_STOP", strlen("AP_BSS_STOP")) == 0) {
+        if ((ret == woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_STOP)))
+            goto done;
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "AP_SET_CFG", strlen("AP_SET_CFG")) == 0) {
+        pdata = buf + strlen("AP_SET_CFG") + 1;
+        if ((ret =
+             woal_uap_set_ap_cfg(priv, pdata,
+                                 priv_cmd.used_len - strlen("AP_SET_CFG") - 1)))
+            goto done;
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "WL_FW_RELOAD", strlen("WL_FW_RELOAD")) == 0) {
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "AP_GET_STA_LIST", strlen("AP_GET_STA_LIST")) == 0) {
+        // TODO Add STA list support
+        len = sprintf(buf, "OK\n") + 1;
+    }
+#endif
+    else if (strncmp(buf, "SETSUSPENDOPT", strlen("SETSUSPENDOPT")) == 0) {
+        /* it will be done by GUI */
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "BTCOEXMODE", strlen("BTCOEXMODE")) == 0) {
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "BTCOEXSCAN-START", strlen("BTCOEXSCAN-START")) ==
+               0) {
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "BTCOEXSCAN-STOP", strlen("BTCOEXSCAN-STOP")) == 0) {
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "BTCOEXSCAN-START", strlen("BTCOEXSCAN-START")) ==
+               0) {
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "BTCOEXSCAN-STOP", strlen("BTCOEXSCAN-STOP")) == 0) {
+        len = sprintf(buf, "OK\n") + 1;
+    }
+#ifdef STA_SUPPORT
+    else if (strncmp(buf, "BGSCAN-START", strlen("BGSCAN-START")) == 0) {
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "BGSCAN-CONFIG", strlen("BGSCAN-CONFIG")) == 0) {
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_bg_scan(priv, buf, priv_cmd.total_len)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        priv->bg_scan_start = MTRUE;
+        priv->bg_scan_reported = MFALSE;
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "BGSCAN-STOP", strlen("BGSCAN-STOP")) == 0) {
+        if (priv->bg_scan_start && !priv->scan_cfg.rssi_threshold) {
+            if (MLAN_STATUS_SUCCESS != woal_stop_bg_scan(priv)) {
+                ret = -EFAULT;
+                goto done;
+            }
+            priv->bg_scan_start = MFALSE;
+            priv->bg_scan_reported = MFALSE;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "RXFILTER-START", strlen("RXFILTER-START")) == 0) {
+#ifdef MEF_CFG_RX_FILTER
+        if ((ret = woal_set_rxfilter(priv, MTRUE))) {
+            goto done;
+        }
+#endif
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "RXFILTER-STOP", strlen("RXFILTER-STOP")) == 0) {
+#ifdef MEF_CFG_RX_FILTER
+        if ((ret = woal_set_rxfilter(priv, MFALSE))) {
+            goto done;
+        }
+#endif
+        len = sprintf(buf, "OK\n") + 1;
+    }
+#ifdef STA_CFG80211
+    else if (strncmp(buf, "GET_RSSI_STATUS", strlen("GET_RSSI_STATUS")) == 0) {
+        if (priv->rssi_status == MLAN_EVENT_ID_FW_BCN_RSSI_LOW)
+            len = sprintf(buf, "EVENT=BEACON_RSSI_LOW\n");
+        else if (priv->rssi_status == MLAN_EVENT_ID_FW_PRE_BCN_LOST)
+            len = sprintf(buf, "EVENT=PRE_BEACON_LOST\n");
+        else
+            len = sprintf(buf, "OK\n") + 1;
+        priv->rssi_status = 0;
+    }
+#endif
+    else if (strncmp(buf, "RXFILTER-ADD", strlen("RXFILTER-ADD")) == 0) {
+        pdata = buf + strlen("RXFILTER-ADD") + 1;
+        if (MLAN_STATUS_SUCCESS != woal_add_rxfilter(priv, pdata)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "RXFILTER-REMOVE", strlen("RXFILTER-REMOVE")) == 0) {
+        pdata = buf + strlen("RXFILTER-REMOVE") + 1;
+        if (MLAN_STATUS_SUCCESS != woal_remove_rxfilter(priv, pdata)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "QOSINFO", strlen("QOSINFO")) == 0) {
+        pdata = buf + strlen("QOSINFO") + 1;
+        if (MLAN_STATUS_SUCCESS != woal_set_qos_cfg(priv, pdata)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "SLEEPPD", strlen("SLEEPPD")) == 0) {
+        pdata = buf + strlen("SLEEPPD") + 1;
+        if (MLAN_STATUS_SUCCESS != woal_set_sleeppd(priv, pdata)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "SET_AP_WPS_P2P_IE",
+                       strlen("SET_AP_WPS_P2P_IE")) == 0) {
+        pdata = buf + strlen("SET_AP_WPS_P2P_IE") + 1;
+        /* Android cmd format: "SET_AP_WPS_P2P_IE 1" -- beacon IE
+           "SET_AP_WPS_P2P_IE 2" -- proberesp IE "SET_AP_WPS_P2P_IE 4" --
+           assocresp IE */
+#if defined(STA_CFG80211) && defined(UAP_CFG80211)
+        if (MLAN_STATUS_SUCCESS != woal_set_ap_wps_p2p_ie(priv, (t_u8 *) pdata,
+                                                          priv_cmd.used_len -
+                                                          strlen
+                                                          ("SET_AP_WPS_P2P_IE")
+                                                          - 1)) {
+            ret = -EFAULT;
+            goto done;
+        }
+#endif
+        len = sprintf(buf, "OK\n") + 1;
+    }
+#endif
+    else if (strncmp(buf, "P2P_DEV_ADDR", strlen("P2P_DEV_ADDR")) == 0) {
+        memset(buf, 0x0, priv_cmd.total_len);
+        memcpy(buf, priv->current_addr, ETH_ALEN);
+        len = ETH_ALEN;
+    } else if (strncmp(buf, ("P2P_GET_NOA"), strlen("P2P_GET_NOA")) == 0) {
+        /* TODO Just return '\0' */
+        memset(buf, 0x0, priv_cmd.total_len);
+        *buf = 0;
+        len = 1;
+    } else {
+        PRINTM(MIOCTL, "Unknow PRIVATE command: %s, ignored\n", buf);
+        ret = -EFAULT;
+        goto done;
+    }
+
+  handled:
+    PRINTM(MIOCTL, "PRIV Command return: %s, length=%d\n", buf, len);
+
+    if (len > 0) {
+        priv_cmd.used_len = len;
+        if (priv_cmd.used_len < priv_cmd.total_len)
+            memset(priv_cmd.buf + priv_cmd.used_len, 0,
+                   priv_cmd.total_len - priv_cmd.used_len);
+        if (copy_to_user(priv_cmd.buf, buf, priv_cmd.used_len)) {
+            PRINTM(MERROR, "%s: failed to copy data to user buffer\n",
+                   __FUNCTION__);
+            ret = -EFAULT;
+            goto done;
+        }
+        if (copy_to_user
+            (req->ifr_data, &priv_cmd, sizeof(android_wifi_priv_cmd))) {
+            PRINTM(MERROR, "%s: failed to copy command header to user buffer\n",
+                   __FUNCTION__);
+            ret = -EFAULT;
+        }
+    } else {
+        ret = len;
+    }
+
+  done:
+    if (buf)
+        kfree(buf);
+    LEAVE();
+    return ret;
+}
+
+/********************************************************
+		Global Functions
+********************************************************/
+/**
+ *  @brief ioctl function - entry point
+ *
+ *  @param dev		A pointer to net_device structure
+ *  @param req	   	A pointer to ifreq structure
+ *  @param cmd 		Command
+ *
+ *  @return          0 --success, otherwise fail
+ */
+int
+woal_do_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
+{
+    int ret = 0;
+
+    ENTER();
+
+    PRINTM(MINFO, "woal_do_ioctl: ioctl cmd = 0x%x\n", cmd);
+    switch (cmd) {
+#ifdef WIFI_DIRECT_SUPPORT
+    case WOAL_WIFIDIRECT_HOST_CMD:
+        ret = woal_hostcmd_ioctl(dev, req);
+        break;
+#endif
+    case WOAL_CUSTOM_IE_CFG:
+        ret = woal_custom_ie_ioctl(dev, req);
+        break;
+    case WOAL_MGMT_FRAME_TX:
+        ret = woal_send_host_packet(dev, req);
+        break;
+    case WOAL_ANDROID_PRIV_CMD:
+        ret = woal_android_priv_cmd(dev, req);
+        break;
+    case WOAL_GET_BSS_TYPE:
+        ret = woal_get_bss_type(dev, req);
+        break;
+    default:
+#if defined(STA_WEXT)
+#ifdef STA_SUPPORT
+        ret = woal_wext_do_ioctl(dev, req, cmd);
+#else
+        ret = -EINVAL;
+#endif
+#else
+	/*
+	 * FIXME Some of the IOCTLs are not supported by Marvel driver
+	 * for CFG80211 and hence it is failing to turn on from UI,
+	 * so returing 0(Sucess) for the time being.
+	 */
+        ret = 0;
+#endif
+        break;
+    }
+
+    LEAVE();
+    return ret;
+}
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_eth_ioctl.h b/drivers/net/wireless/sd8797/mlinux/moal_eth_ioctl.h
new file mode 100644
index 000000000000..a68991694a9d
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_eth_ioctl.h
@@ -0,0 +1,90 @@
+/** @file moal_eth_ioctl.h
+ *
+ * @brief This file contains definition for private IOCTL call.
+ *  
+ * Copyright (C) 2012, Marvell International Ltd.  
+ *
+ * This software file (the "File") is distributed by Marvell International 
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+ * (the "License").  You may use, redistribute and/or modify this File in 
+ * accordance with the terms and conditions of the License, a copy of which 
+ * is available by writing to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+ * this warranty disclaimer.
+ *
+ */
+
+/********************************************************
+Change log:
+    01/05/2012: initial version
+********************************************************/
+
+#ifndef _WOAL_ETH_PRIV_H_
+#define _WOAL_ETH_PRIV_H_
+
+#ifdef WIFI_DIRECT_SUPPORT
+/** Private command ID to Host command */
+#define	WOAL_WIFIDIRECT_HOST_CMD    (SIOCDEVPRIVATE + 1)
+#endif
+
+/** Private command ID to pass mgmt frame */
+#define WOAL_MGMT_FRAME_TX          WOAL_MGMT_FRAME_TX_IOCTL
+
+/** Private command ID to pass custom IE list */
+#define WOAL_CUSTOM_IE_CFG          (SIOCDEVPRIVATE + 13)
+
+/** Private command ID for Android ICS priv CMDs */
+#define	WOAL_ANDROID_PRIV_CMD       (SIOCDEVPRIVATE + 14)
+
+/** Private command ID to get BSS type */
+#define WOAL_GET_BSS_TYPE           (SIOCDEVPRIVATE + 15)
+
+int woal_do_ioctl(struct net_device *dev, struct ifreq *req, int cmd);
+
+/*
+ * For android private commands, fixed value of ioctl is used.
+ * Internally commands are differentiated using strings.
+ *
+ * application needs to specify "total_len" of data for copy_from_user
+ * kernel updates "used_len" during copy_to_user
+ */
+/** Private command structure from app */
+typedef struct _android_wifi_priv_cmd
+{
+    /** Buffer pointer */
+    char *buf;
+    /** buffer updated by driver */
+    int used_len;
+    /** buffer sent by application */
+    int total_len;
+} android_wifi_priv_cmd;
+
+/** data structure for cmd txratecfg */
+typedef struct woal_priv_tx_rate_cfg
+{
+    /* LG rate: 0, HT rate: 1, VHT rate: 2 */
+    t_u32 rate_format;
+    /** Rate/MCS index (0xFF: auto) */
+    t_u32 rate_index;
+} woal_tx_rate_cfg;
+
+typedef struct woal_priv_esuppmode_cfg
+{
+    /* RSN mode */
+    t_u16 rsn_mode;
+    /* Pairwise cipher */
+    t_u8 pairwise_cipher;
+    /* Group cipher */
+    t_u8 group_cipher;
+} woal_esuppmode_cfg;
+
+mlan_status woal_set_ap_wps_p2p_ie(moal_private * priv, t_u8 * ie, size_t len);
+
+int woal_android_priv_cmd(struct net_device *dev, struct ifreq *req);
+
+#endif /* _WOAL_ETH_PRIV_H_ */
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_ioctl.c b/drivers/net/wireless/sd8797/mlinux/moal_ioctl.c
new file mode 100644
index 000000000000..94fe7e11669e
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_ioctl.c
@@ -0,0 +1,4463 @@
+/** @file moal_ioctl.c
+  *
+  * @brief This file contains ioctl function to MLAN
+  * 
+  * Copyright (C) 2008-2012, Marvell International Ltd. 
+  *
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+
+/********************************************************
+Change log:
+    10/21/2008: initial version
+********************************************************/
+
+#include    "moal_main.h"
+#include    "moal_eth_ioctl.h"
+#include    "moal_sdio.h"
+#ifdef UAP_SUPPORT
+#include    "moal_uap.h"
+#endif
+
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+#include "moal_cfg80211.h"
+#endif
+
+/********************************************************
+                Local Variables
+********************************************************/
+/* CAC Measure report default time 60 seconds */
+#define MEAS_REPORT_TIME 60*HZ
+#define MRVL_TLV_HEADER_SIZE            4
+/* Marvell Channel config TLV ID */
+#define MRVL_CHANNELCONFIG_TLV_ID       (0x0100 + 0x2a) // 0x012a
+
+typedef struct _hostcmd_header
+{
+    /** Command Header : Command */
+    t_u16 command;
+    /** Command Header : Size */
+    t_u16 size;
+    /** Command Header : Sequence number */
+    t_u16 seq_num;
+    /** Command Header : Result */
+    t_u16 result;
+    /** Command action */
+    t_u16 action;
+} hostcmd_header, *phostcmd_header;
+
+#ifdef STA_SUPPORT
+/** Region code mapping */
+typedef struct _region_code_mapping_t
+{
+    /** Region */
+    t_u8 region[COUNTRY_CODE_LEN];
+    /** Code */
+    t_u8 code;
+} region_code_mapping_t;
+
+/** Region code mapping table */
+static region_code_mapping_t region_code_mapping[] = {
+    {"US ", 0x10},              /* US FCC */
+    {"CA ", 0x20},              /* IC Canada */
+    {"SG ", 0x10},              /* Singapore */
+    {"EU ", 0x30},              /* ETSI */
+    {"AU ", 0x30},              /* Australia */
+    {"KR ", 0x30},              /* Republic Of Korea */
+    {"FR ", 0x32},              /* France */
+    {"CN ", 0x50},              /* China */
+    {"JP ", 0xFF},              /* Japan special */
+};
+#endif
+
+/********************************************************
+                Global Variables
+********************************************************/
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+#ifdef UAP_SUPPORT
+/** Network device handlers for uAP */
+extern const struct net_device_ops woal_uap_netdev_ops;
+#endif
+#ifdef STA_SUPPORT
+/** Network device handlers for STA */
+extern const struct net_device_ops woal_netdev_ops;
+#endif
+#endif
+extern int cfg80211_wext;
+
+/********************************************************
+                Local Functions
+********************************************************/
+#ifdef STA_SUPPORT
+/**
+ *  @brief This function converts region string to region code
+ *
+ *  @param region_string    Region string
+ *
+ *  @return                 Region code
+ */
+static t_u8
+region_string_2_region_code(char *region_string)
+{
+    t_u8 i;
+    t_u8 size = sizeof(region_code_mapping) / sizeof(region_code_mapping_t);
+
+    ENTER();
+    for (i = 0; i < size; i++) {
+        if (!memcmp(region_string,
+                    region_code_mapping[i].region, strlen(region_string))) {
+            LEAVE();
+            return (region_code_mapping[i].code);
+        }
+    }
+    /* Default is US */
+    LEAVE();
+    return (region_code_mapping[0].code);
+}
+#endif
+
+/**
+ *  @brief Copy multicast table
+ *
+ *  @param mlist    A pointer to mlan_multicast_list structure
+ *  @param dev      A pointer to net_device structure
+ *
+ *  @return         Number of multicast addresses
+ */
+static inline int
+woal_copy_mcast_addr(mlan_multicast_list * mlist, struct net_device *dev)
+{
+    int i = 0;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35)
+    struct dev_mc_list *mcptr = dev->mc_list;
+#else
+    struct netdev_hw_addr *mcptr = NULL;
+#endif /* < 2.6.35 */
+
+    ENTER();
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35)
+    for (i = 0; i < dev->mc_count && mcptr; i++) {
+        memcpy(&mlist->mac_list[i], mcptr->dmi_addr, ETH_ALEN);
+        mcptr = mcptr->next;
+    }
+#else
+    netdev_for_each_mc_addr(mcptr, dev)
+        memcpy(&mlist->mac_list[i++], mcptr->addr, ETH_ALEN);
+#endif /* < 2.6.35 */
+    LEAVE();
+    return i;
+}
+
+/**
+ *  @brief Fill in wait queue
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wait         A pointer to wait_queue structure
+ *  @param wait_option  Wait option
+ *
+ *  @return             N/A
+ */
+static inline void
+woal_fill_wait_queue(moal_private * priv, wait_queue * wait, t_u8 wait_option)
+{
+    ENTER();
+    wait->start_time = jiffies;
+    wait->condition = MFALSE;
+    switch (wait_option) {
+    case MOAL_NO_WAIT:
+        break;
+    case MOAL_IOCTL_WAIT:
+        wait->wait = &priv->ioctl_wait_q;
+        break;
+    case MOAL_CMD_WAIT:
+        wait->wait = &priv->cmd_wait_q;
+        break;
+    case MOAL_PROC_WAIT:
+        wait->wait = &priv->proc_wait_q;
+        break;
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+    case MOAL_WSTATS_WAIT:
+        if (IS_STA_OR_UAP_WEXT(cfg80211_wext))
+            wait->wait = &priv->w_stats_wait_q;
+        break;
+#endif
+    }
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief Wait mlan ioctl complete
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param req          A pointer to mlan_ioctl_req structure
+ *  @param wait_option  Wait option
+ *
+ *  @return             N/A
+ */
+static inline void
+woal_wait_ioctl_complete(moal_private * priv, mlan_ioctl_req * req,
+                         t_u8 wait_option)
+{
+    mlan_status status;
+    wait_queue *wait = (wait_queue *) req->reserved_1;
+
+    ENTER();
+
+    switch (wait_option) {
+    case MOAL_NO_WAIT:
+        break;
+    case MOAL_IOCTL_WAIT:
+        wait_event_interruptible(priv->ioctl_wait_q, wait->condition);
+        break;
+    case MOAL_CMD_WAIT:
+        wait_event_interruptible(priv->cmd_wait_q, wait->condition);
+        break;
+    case MOAL_PROC_WAIT:
+        wait_event_interruptible(priv->proc_wait_q, wait->condition);
+        break;
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+    case MOAL_WSTATS_WAIT:
+        if (IS_STA_OR_UAP_WEXT(cfg80211_wext))
+            wait_event_interruptible(priv->w_stats_wait_q, wait->condition);
+        break;
+#endif
+    }
+    if (wait->condition == MFALSE) {
+        req->action = MLAN_ACT_CANCEL;
+        status = mlan_ioctl(priv->phandle->pmlan_adapter, req);
+        PRINTM(MIOCTL,
+               "IOCTL cancel: id=0x%x, sub_id=0x%x, wait_option=%d, action=%d, status=%d\n",
+               req->req_id, (*(t_u32 *) req->pbuf), wait_option,
+               (int) req->action, status);
+    }
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief CAC period block cmd handler
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param req      A pointer to mlan_ioctl_req buffer
+ *
+ *  @return         MTRUE/MFALSE
+ */
+static inline t_bool
+woal_cac_period_block_cmd(moal_private * priv, pmlan_ioctl_req req)
+{
+    mlan_status ret = MFALSE;
+    t_u32 sub_command;
+
+    ENTER();
+    if (req == NULL || req->pbuf == NULL) {
+        goto done;
+    }
+
+    sub_command = *(t_u32 *) req->pbuf;
+
+    switch (req->req_id) {
+    case MLAN_IOCTL_SCAN:
+        if (sub_command == MLAN_OID_SCAN_NORMAL ||
+            sub_command == MLAN_OID_SCAN_SPECIFIC_SSID ||
+            sub_command == MLAN_OID_SCAN_USER_CONFIG) {
+            ret = MTRUE;
+        }
+        break;
+    case MLAN_IOCTL_BSS:
+        if (sub_command == MLAN_OID_BSS_STOP ||
+#ifdef UAP_SUPPORT
+            sub_command == MLAN_OID_UAP_BSS_CONFIG ||
+#endif
+            sub_command == MLAN_OID_BSS_CHANNEL
+            /* sub_command == MLAN_OID_BSS_ROLE */ ) {
+            ret = MTRUE;
+        }
+        break;
+    case MLAN_IOCTL_RADIO_CFG:
+        if (sub_command == MLAN_OID_BAND_CFG) {
+            ret = MTRUE;
+        }
+        break;
+#if defined(UAP_SUPPORT)
+    case MLAN_IOCTL_SNMP_MIB:
+        if (sub_command == MLAN_OID_SNMP_MIB_DOT11D ||
+            sub_command == MLAN_OID_SNMP_MIB_DOT11H) {
+            ret = MTRUE;
+        }
+        break;
+#endif
+    case MLAN_IOCTL_11D_CFG:
+#ifdef STA_SUPPORT
+        if (sub_command == MLAN_OID_11D_CFG_ENABLE) {
+            ret = MTRUE;
+        }
+#endif
+        if (sub_command == MLAN_OID_11D_DOMAIN_INFO) {
+            ret = MTRUE;
+        }
+        break;
+    case MLAN_IOCTL_MISC_CFG:
+        if (sub_command == MLAN_OID_MISC_REGION) {
+            ret = MTRUE;
+        }
+        if (sub_command == MLAN_OID_MISC_HOST_CMD) {
+            phostcmd_header phostcmd;
+            t_u8 *ptlv_buf;
+            t_u16 tag, length;
+
+            phostcmd =
+                (phostcmd_header) ((pmlan_ds_misc_cfg) req->pbuf)->param.
+                hostcmd.cmd;
+            ptlv_buf = (t_u8 *) phostcmd + sizeof(hostcmd_header);
+            if (phostcmd->action == MLAN_ACT_SET) {
+                while (ptlv_buf < (t_u8 *) phostcmd + phostcmd->size) {
+                    tag = *(t_u16 *) ptlv_buf;
+                    length = *(t_u16 *) (ptlv_buf + 2);
+                    /* Check Blocking TLV here, should add more... */
+                    if (tag == MRVL_CHANNELCONFIG_TLV_ID) {
+                        ret = MTRUE;
+                        break;
+                    }
+                    ptlv_buf += (length + MRVL_TLV_HEADER_SIZE);
+                }
+            }
+        }
+        break;
+    case MLAN_IOCTL_11H_CFG:
+        /* Prevent execute more than once */
+        if (sub_command == MLAN_OID_11H_CHANNEL_CHECK) {
+            ret = MTRUE;
+        }
+        break;
+    default:
+        ret = MFALSE;
+        break;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/********************************************************
+                Global Functions
+********************************************************/
+
+/**
+ *  @brief Send ioctl request to MLAN
+ *
+ *  @param priv          A pointer to moal_private structure
+ *  @param req           A pointer to mlan_ioctl_req buffer
+ *  @param wait_option   Wait option (MOAL_WAIT or MOAL_NO_WAIT)
+ *
+ *  @return              MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_request_ioctl(moal_private * priv, mlan_ioctl_req * req, t_u8 wait_option)
+{
+    wait_queue *wait;
+    mlan_status status;
+
+    ENTER();
+
+    if (priv->phandle->surprise_removed == MTRUE) {
+        PRINTM(MERROR,
+               "IOCTL is not allowed while the device is not present\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+#if defined(SDIO_SUSPEND_RESUME)
+    if (priv->phandle->is_suspended == MTRUE) {
+        PRINTM(MERROR, "IOCTL is not allowed while suspended\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+#endif
+
+    /* For MLAN_OID_MISC_HOST_CMD, action is 0, "action set" is checked later */
+    if ((req->action == MLAN_ACT_SET || req->action == 0) &&
+        priv->phandle->cac_period == MTRUE) {
+        t_u32 sub_command;
+        /* CAC checking period left to complete jiffies */
+        unsigned long cac_left_jiffies;
+
+        sub_command = *(t_u32 *) req->pbuf;
+
+        /* cac_left_jiffies will be negative if and only if * event
+           MLAN_EVENT_ID_DRV_MEAS_REPORT recieved from FW * after CAC measure
+           period ends, * usually this could be considered as a FW bug */
+        cac_left_jiffies = MEAS_REPORT_TIME -
+            (jiffies - priv->phandle->meas_start_jiffies);
+#ifdef DFS_TESTING_SUPPORT
+        if (priv->phandle->cac_period_jiffies) {
+            cac_left_jiffies = priv->phandle->cac_period_jiffies -
+                (jiffies - priv->phandle->meas_start_jiffies);
+        }
+#endif
+        if (cac_left_jiffies < 0) {
+            /* Avoid driver hang in FW died during CAC measure period */
+            priv->phandle->cac_period = MFALSE;
+            PRINTM(MERROR,
+                   "CAC measure period spends longer than scheduled time "
+                   "or meas done event never received\n");
+            status = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+
+        /* Check BSS START first */
+        if (sub_command == MLAN_OID_BSS_START) {
+            mlan_ds_bss *bss;
+            bss = (mlan_ds_bss *) req->pbuf;
+            /*
+             * Bss delay start after channel report received,
+             * not block the driver by delay executing. This is
+             * because a BSS_START cmd is always executed right
+             * after channel check issued.
+             */
+            if (priv->phandle->delay_bss_start == MFALSE) {
+                PRINTM(MMSG, "Received BSS Start command during CAC period, "
+                       "delay executing %ld seconds\n", cac_left_jiffies / HZ);
+                priv->phandle->delay_bss_start = MTRUE;
+                memcpy(&priv->phandle->delay_ssid_bssid,
+                       &bss->param.ssid_bssid, sizeof(mlan_ssid_bssid));
+                /* TODO: return success to allow the half below of routines of
+                   which calling BSS start to execute */
+                status = MLAN_STATUS_SUCCESS;
+                goto done;
+            } else {
+                /* TODO: not blocking it, just return failure */
+                PRINTM(MMSG, "Only one BSS Start command allowed for delay "
+                       "executing!\n");
+                status = MLAN_STATUS_FAILURE;
+                goto done;
+            }
+        }
+        if (woal_cac_period_block_cmd(priv, req)) {
+            priv->phandle->meas_wait_q_woken = MFALSE;
+            PRINTM(MMSG, "CAC check is on going... Blocking Command"
+                   " %ld seconds\n", cac_left_jiffies / HZ);
+            /* blocking timeout set to 1.5 * CAC checking period left time */
+            wait_event_interruptible_timeout(priv->phandle->meas_wait_q,
+                                             priv->phandle->meas_wait_q_woken,
+                                             cac_left_jiffies * 3 / 2);
+        }
+    } else if (priv->phandle->cac_period) {
+        PRINTM(MINFO, "Operation during CAC check period.\n");
+    }
+    wait = (wait_queue *) req->reserved_1;
+    req->bss_index = priv->bss_index;
+    if (wait_option)
+        woal_fill_wait_queue(priv, wait, wait_option);
+    else
+        req->reserved_1 = 0;
+
+    /* Call MLAN ioctl handle */
+    status = mlan_ioctl(priv->phandle->pmlan_adapter, req);
+    switch (status) {
+    case MLAN_STATUS_PENDING:
+        PRINTM(MIOCTL,
+               "IOCTL pending: %p id=0x%x, sub_id=0x%x wait_option=%d, action=%d\n",
+               req, req->req_id, (*(t_u32 *) req->pbuf), wait_option,
+               (int) req->action);
+        atomic_inc(&priv->phandle->ioctl_pending);
+        /* Status pending, wake up main process */
+        queue_work(priv->phandle->workqueue, &priv->phandle->main_work);
+
+        /* Wait for completion */
+        if (wait_option) {
+            woal_wait_ioctl_complete(priv, req, wait_option);
+            status = wait->status;
+        }
+        break;
+    case MLAN_STATUS_SUCCESS:
+    case MLAN_STATUS_FAILURE:
+    case MLAN_STATUS_RESOURCE:
+        PRINTM(MIOCTL,
+               "IOCTL: %p id=0x%x, sub_id=0x%x wait_option=%d, action=%d status=%d\n",
+               req, req->req_id, (*(t_u32 *) req->pbuf), wait_option,
+               (int) req->action, status);
+    default:
+        break;
+    }
+  done:
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Send set MAC address request to MLAN
+ *
+ *  @param priv   A pointer to moal_private structure
+ *
+ *  @return       MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_request_set_mac_address(moal_private * priv)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_bss *bss = NULL;
+    mlan_status status;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = (mlan_ioctl_req *) woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        status = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_BSS_MAC_ADDR;
+    memcpy(&bss->param.mac_addr, priv->current_addr,
+           sizeof(mlan_802_11_mac_addr));
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = MLAN_ACT_SET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, MOAL_CMD_WAIT);
+    if (status == MLAN_STATUS_SUCCESS) {
+        memcpy(priv->netdev->dev_addr, priv->current_addr, ETH_ALEN);
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+        if (IS_STA_OR_UAP_CFG80211(cfg80211_wext) && priv->wdev &&
+            priv->wdev->wiphy)
+            memcpy(priv->wdev->wiphy->perm_addr, priv->current_addr, ETH_ALEN);
+#endif
+        HEXDUMP("priv->MacAddr:", priv->current_addr, ETH_ALEN);
+    } else {
+        PRINTM(MERROR, "set mac address failed! status=%d, error_code=0x%x\n",
+               status, req->status_code);
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Send multicast list request to MLAN
+ *
+ *  @param priv   A pointer to moal_private structure
+ *  @param dev    A pointer to net_device structure
+ *
+ *  @return       N/A
+ */
+void
+woal_request_set_multicast_list(moal_private * priv, struct net_device *dev)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_bss *bss = NULL;
+    mlan_status status;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35)
+    int mc_count = dev->mc_count;
+#else
+    int mc_count = netdev_mc_count(dev);
+#endif
+
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = (mlan_ioctl_req *) woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        PRINTM(MERROR, "%s:Fail to allocate ioctl req buffer\n", __FUNCTION__);
+        goto done;
+    }
+
+    /* Fill request buffer */
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_BSS_MULTICAST_LIST;
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = MLAN_ACT_SET;
+    if (dev->flags & IFF_PROMISC) {
+        bss->param.multicast_list.mode = MLAN_PROMISC_MODE;
+    } else if (dev->flags & IFF_ALLMULTI ||
+               mc_count > MLAN_MAX_MULTICAST_LIST_SIZE) {
+        bss->param.multicast_list.mode = MLAN_ALL_MULTI_MODE;
+    } else {
+        bss->param.multicast_list.mode = MLAN_MULTICAST_MODE;
+        if (mc_count)
+            bss->param.multicast_list.num_multicast_addr =
+                woal_copy_mcast_addr(&bss->param.multicast_list, dev);
+    }
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, MOAL_NO_WAIT);
+    if (status != MLAN_STATUS_PENDING)
+        kfree(req);
+  done:
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief Send deauth command to MLAN
+ *
+ *  @param priv          A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param mac           MAC address to deauthenticate
+ *
+ *  @return              MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_disconnect(moal_private * priv, t_u8 wait_option, t_u8 * mac)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_bss *bss = NULL;
+    mlan_status status;
+
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = (mlan_ioctl_req *) woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        status = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_BSS_STOP;
+    if (mac)
+        memcpy((t_u8 *) & bss->param.bssid, mac, sizeof(mlan_802_11_mac_addr));
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = MLAN_ACT_SET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+
+  done:
+    if (req)
+        kfree(req);
+#ifdef REASSOCIATION
+    priv->reassoc_required = MFALSE;
+#endif /* REASSOCIATION */
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Send bss_start command to MLAN
+ *
+ *  @param priv          A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param ssid_bssid    A point to mlan_ssid_bssid structure
+ *
+ *  @return              MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_bss_start(moal_private * priv, t_u8 wait_option,
+               mlan_ssid_bssid * ssid_bssid)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_bss *bss = NULL;
+    mlan_status status;
+
+    ENTER();
+
+    /* Stop the O.S. TX queue if needed */
+    woal_stop_queue(priv->netdev);
+
+    /* Allocate an IOCTL request buffer */
+    req = (mlan_ioctl_req *) woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        status = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_BSS_START;
+    if (ssid_bssid)
+        memcpy(&bss->param.ssid_bssid, ssid_bssid, sizeof(mlan_ssid_bssid));
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = MLAN_ACT_SET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Get BSS info
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param bss_info             A pointer to mlan_bss_info structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_get_bss_info(moal_private * priv, t_u8 wait_option,
+                  mlan_bss_info * bss_info)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_get_info *info = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_get_info));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    info = (mlan_ds_get_info *) req->pbuf;
+    info->sub_command = MLAN_OID_GET_BSS_INFO;
+    req->req_id = MLAN_IOCTL_GET_INFO;
+    req->action = MLAN_ACT_GET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+    if (status == MLAN_STATUS_SUCCESS) {
+        if (bss_info) {
+            memcpy(bss_info, &info->param.bss_info, sizeof(mlan_bss_info));
+        }
+    }
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+#ifdef STA_SUPPORT
+/**
+ *  @brief Set/Get retry count
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param action               Action set or get
+ *  @param wait_option          Wait option
+ *  @param value                Retry value
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_set_get_retry(moal_private * priv, t_u32 action,
+                   t_u8 wait_option, int *value)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_snmp_mib *mib = NULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_snmp_mib));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    mib = (mlan_ds_snmp_mib *) req->pbuf;
+    mib->sub_command = MLAN_OID_SNMP_MIB_RETRY_COUNT;
+    req->req_id = MLAN_IOCTL_SNMP_MIB;
+    req->action = action;
+
+    if (action == MLAN_ACT_SET) {
+        if (*value < MLAN_TX_RETRY_MIN || *value > MLAN_TX_RETRY_MAX) {
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+        mib->param.retry_count = *value;
+    }
+
+    /* Send IOCTL request to MLAN */
+    ret = woal_request_ioctl(priv, req, wait_option);
+    if (ret == MLAN_STATUS_SUCCESS && action == MLAN_ACT_GET) {
+        *value = mib->param.retry_count;
+    }
+#ifdef STA_CFG80211
+    /* If set is invoked from other than iw i.e iwconfig, wiphy retry count
+       should be updated as well */
+    if (IS_STA_CFG80211(cfg80211_wext) && priv->wdev && priv->wdev->wiphy &&
+        (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) && (action == MLAN_ACT_SET)) {
+        priv->wdev->wiphy->retry_long = (t_u8) * value;
+        priv->wdev->wiphy->retry_short = (t_u8) * value;
+    }
+#endif
+
+  done:
+    if (req && (ret != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get RTS threshold
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param action               Action set or get
+ *  @param wait_option          Wait option
+ *  @param value                RTS threshold value
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_set_get_rts(moal_private * priv, t_u32 action,
+                 t_u8 wait_option, int *value)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_snmp_mib *mib = NULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_snmp_mib));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    mib = (mlan_ds_snmp_mib *) req->pbuf;
+    mib->sub_command = MLAN_OID_SNMP_MIB_RTS_THRESHOLD;
+    req->req_id = MLAN_IOCTL_SNMP_MIB;
+    req->action = action;
+
+    if (action == MLAN_ACT_SET) {
+        if (*value < MLAN_RTS_MIN_VALUE || *value > MLAN_RTS_MAX_VALUE) {
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+        mib->param.rts_threshold = *value;
+    }
+
+    /* Send IOCTL request to MLAN */
+    ret = woal_request_ioctl(priv, req, wait_option);
+    if (ret == MLAN_STATUS_SUCCESS && action == MLAN_ACT_GET) {
+        *value = mib->param.rts_threshold;
+    }
+#ifdef STA_CFG80211
+    /* If set is invoked from other than iw i.e iwconfig, wiphy RTS threshold
+       should be updated as well */
+    if (IS_STA_CFG80211(cfg80211_wext) && priv->wdev && priv->wdev->wiphy &&
+        (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) && (action == MLAN_ACT_SET))
+        priv->wdev->wiphy->rts_threshold = *value;
+#endif
+
+  done:
+    if (req && (ret != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get Fragment threshold
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param action               Action set or get
+ *  @param wait_option          Wait option
+ *  @param value                Fragment threshold value
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_set_get_frag(moal_private * priv, t_u32 action,
+                  t_u8 wait_option, int *value)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_snmp_mib *mib = NULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_snmp_mib));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    mib = (mlan_ds_snmp_mib *) req->pbuf;
+    mib->sub_command = MLAN_OID_SNMP_MIB_FRAG_THRESHOLD;
+    req->req_id = MLAN_IOCTL_SNMP_MIB;
+    req->action = action;
+
+    if (action == MLAN_ACT_SET) {
+        if (*value < MLAN_FRAG_MIN_VALUE || *value > MLAN_FRAG_MAX_VALUE) {
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+        mib->param.frag_threshold = *value;
+    }
+
+    /* Send IOCTL request to MLAN */
+    ret = woal_request_ioctl(priv, req, wait_option);
+    if (ret == MLAN_STATUS_SUCCESS && action == MLAN_ACT_GET) {
+        *value = mib->param.frag_threshold;
+    }
+#ifdef STA_CFG80211
+    /* If set is invoked from other than iw i.e iwconfig, wiphy fragment
+       threshold should be updated as well */
+    if (IS_STA_CFG80211(cfg80211_wext) && priv->wdev && priv->wdev->wiphy &&
+        (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) && (action == MLAN_ACT_SET))
+        priv->wdev->wiphy->frag_threshold = *value;
+#endif
+
+  done:
+    if (req && (ret != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get generic IE
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param action       Action set or get
+ *  @param ie           Information element
+ *  @param ie_len       Length of the IE
+ *
+ *  @return             MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_set_get_gen_ie(moal_private * priv, t_u32 action, t_u8 * ie, int *ie_len)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_misc_cfg *misc = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    if ((action == MLAN_ACT_GET) && (ie == NULL || ie_len == NULL)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    if (action == MLAN_ACT_SET && *ie_len > MAX_IE_SIZE) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    misc = (mlan_ds_misc_cfg *) req->pbuf;
+    misc->sub_command = MLAN_OID_MISC_GEN_IE;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+    req->action = action;
+    misc->param.gen_ie.type = MLAN_IE_TYPE_GEN_IE;
+
+    if (action == MLAN_ACT_SET) {
+        misc->param.gen_ie.len = *ie_len;
+        if (*ie_len)
+            memcpy(misc->param.gen_ie.ie_data, ie, *ie_len);
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    if (action == MLAN_ACT_GET) {
+        *ie_len = misc->param.gen_ie.len;
+        if (*ie_len)
+            memcpy(ie, misc->param.gen_ie.ie_data, *ie_len);
+    }
+
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get TX power
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param action               Action set or get
+ *  @param power_cfg            A pinter to mlan_power_cfg_t structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_set_get_tx_power(moal_private * priv,
+                      t_u32 action, mlan_power_cfg_t * power_cfg)
+{
+    mlan_ds_power_cfg *pcfg = NULL;
+    mlan_ioctl_req *req = NULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_power_cfg));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    pcfg = (mlan_ds_power_cfg *) req->pbuf;
+    pcfg->sub_command = MLAN_OID_POWER_CFG;
+    req->req_id = MLAN_IOCTL_POWER_CFG;
+    req->action = action;
+    if (action == MLAN_ACT_SET && power_cfg)
+        memcpy(&pcfg->param.power_cfg, power_cfg, sizeof(mlan_power_cfg_t));
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT))
+        ret = MLAN_STATUS_FAILURE;
+
+    if (ret == MLAN_STATUS_SUCCESS && power_cfg)
+        memcpy(power_cfg, &pcfg->param.power_cfg, sizeof(mlan_power_cfg_t));
+
+  done:
+    if (req && (ret != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get IEEE power management
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param action               Action set or get
+ *  @param disabled             A pointer to disabled flag
+ *  @param power_type           IEEE power type
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_set_get_power_mgmt(moal_private * priv,
+                        t_u32 action, int *disabled, int power_type)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_pm_cfg *pm_cfg = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_pm_cfg));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    pm_cfg = (mlan_ds_pm_cfg *) req->pbuf;
+    pm_cfg->sub_command = MLAN_OID_PM_CFG_IEEE_PS;
+    req->req_id = MLAN_IOCTL_PM_CFG;
+    req->action = action;
+
+    if (action == MLAN_ACT_SET) {
+        PRINTM(MINFO, "PS_MODE set power disabled=%d power type=%#x\n",
+               *disabled, power_type);
+        if (*disabled)
+            pm_cfg->param.ps_mode = 0;
+        else {
+            /* Check not support case only (vwrq->disabled == FALSE) */
+            if ((power_type & IW_POWER_TYPE) == IW_POWER_TIMEOUT) {
+                PRINTM(MERROR, "Setting power timeout is not supported\n");
+                ret = MLAN_STATUS_FAILURE;
+                goto done;
+            } else if ((power_type & IW_POWER_TYPE) == IW_POWER_PERIOD) {
+                PRINTM(MERROR, "Setting power period is not supported\n");
+                ret = MLAN_STATUS_FAILURE;
+                goto done;
+            }
+            pm_cfg->param.ps_mode = 1;
+        }
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    if (ret == MLAN_STATUS_SUCCESS && action == MLAN_ACT_GET)
+        *disabled = pm_cfg->param.ps_mode;
+
+#ifdef STA_CFG80211
+    /* If set is invoked from other than iw i.e iwconfig, wiphy IEEE power save
+       mode should be updated */
+    if (IS_STA_CFG80211(cfg80211_wext) && priv->wdev &&
+        (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) && (action == MLAN_ACT_SET)) {
+        if (*disabled)
+            priv->wdev->ps = MFALSE;
+        else
+            priv->wdev->ps = MTRUE;
+    }
+#endif
+
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Set region code
+ *
+ * @param priv     A pointer to moal_private structure
+ * @param region   A pointer to region string
+ *
+ * @return         MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+mlan_status
+woal_set_region_code(moal_private * priv, char *region)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_misc_cfg *cfg = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    cfg = (mlan_ds_misc_cfg *) req->pbuf;
+    cfg->sub_command = MLAN_OID_MISC_REGION;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+    req->action = MLAN_ACT_SET;
+    cfg->param.region_code = region_string_2_region_code(region);
+    ret = woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT);
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get data rate
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param action               Action set or get
+ *  @param datarate             A pointer to mlan_rate_cfg_t structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_set_get_data_rate(moal_private * priv,
+                       t_u8 action, mlan_rate_cfg_t * datarate)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_rate *rate = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_rate));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    rate = (mlan_ds_rate *) req->pbuf;
+    rate->param.rate_cfg.rate_type = MLAN_RATE_VALUE;
+    rate->sub_command = MLAN_OID_RATE_CFG;
+    req->req_id = MLAN_IOCTL_RATE;
+    req->action = action;
+
+    if (datarate && (action == MLAN_ACT_SET))
+        memcpy(&rate->param.rate_cfg, datarate, sizeof(mlan_rate_cfg_t));
+
+    if (MLAN_STATUS_SUCCESS == woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        if (datarate && (action == MLAN_ACT_GET))
+            memcpy(datarate, &rate->param.rate_cfg, sizeof(mlan_rate_cfg_t));
+    } else {
+        ret = MLAN_STATUS_FAILURE;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+#endif
+
+/**
+ *  @brief Send get FW info request to MLAN
+ *
+ *  @param priv             A pointer to moal_private structure
+ *  @param wait_option      Wait option
+ *  @param fw_info          FW information
+ *
+ *  @return                 MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_request_get_fw_info(moal_private * priv, t_u8 wait_option,
+                         mlan_fw_info * fw_info)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_get_info *info;
+    mlan_status status;
+    ENTER();
+    memset(priv->current_addr, 0xff, ETH_ALEN);
+
+    /* Allocate an IOCTL request buffer */
+    req = (mlan_ioctl_req *) woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        status = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    info = (mlan_ds_get_info *) req->pbuf;
+    req->req_id = MLAN_IOCTL_GET_INFO;
+    req->action = MLAN_ACT_GET;
+    info->sub_command = MLAN_OID_GET_FW_INFO;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+    if (status == MLAN_STATUS_SUCCESS) {
+        priv->phandle->fw_release_number = info->param.fw_info.fw_ver;
+        if (priv->current_addr[0] == 0xff)
+            memcpy(priv->current_addr, &info->param.fw_info.mac_addr,
+                   sizeof(mlan_802_11_mac_addr));
+        memcpy(priv->netdev->dev_addr, priv->current_addr, ETH_ALEN);
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+        if (IS_STA_OR_UAP_CFG80211(cfg80211_wext) && priv->wdev &&
+            priv->wdev->wiphy)
+            memcpy(priv->wdev->wiphy->perm_addr, priv->current_addr, ETH_ALEN);
+#endif
+        if (fw_info)
+            memcpy(fw_info, &info->param.fw_info, sizeof(mlan_fw_info));
+        DBG_HEXDUMP(MCMD_D, "mac", priv->current_addr, 6);
+    } else
+        PRINTM(MERROR, "get fw info failed! status=%d, error_code=0x%x\n",
+               status, req->status_code);
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+#ifdef PROC_DEBUG
+/**
+ *  @brief Get debug info
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param debug_info           A pointer to mlan_debug_info structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_get_debug_info(moal_private * priv, t_u8 wait_option,
+                    mlan_debug_info * debug_info)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_get_info *info = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_get_info));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    info = (mlan_ds_get_info *) req->pbuf;
+    info->sub_command = MLAN_OID_GET_DEBUG_INFO;
+    req->req_id = MLAN_IOCTL_GET_INFO;
+    req->action = MLAN_ACT_GET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+    if (status == MLAN_STATUS_SUCCESS) {
+        if (debug_info) {
+            memcpy(debug_info, &info->param.debug_info,
+                   sizeof(mlan_debug_info));
+        }
+    }
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Set debug info
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param debug_info           A pointer to mlan_debug_info structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_set_debug_info(moal_private * priv, t_u8 wait_option,
+                    mlan_debug_info * debug_info)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_get_info *info = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    if (!debug_info) {
+        ret = -EINVAL;
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_get_info));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    info = (mlan_ds_get_info *) req->pbuf;
+    info->sub_command = MLAN_OID_GET_DEBUG_INFO;
+    memcpy(&info->param.debug_info, debug_info, sizeof(mlan_debug_info));
+    req->req_id = MLAN_IOCTL_GET_INFO;
+    req->action = MLAN_ACT_SET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+#endif /* PROC_DEBUG */
+
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+/**
+ *  @brief host command ioctl function
+ *
+ *  @param priv		A pointer to moal_private structure
+ *  @param wrq 		A pointer to iwreq structure
+ *  @return    		0 --success, otherwise fail
+ */
+int
+woal_host_command(moal_private * priv, struct iwreq *wrq)
+{
+    HostCmd_Header cmd_header;
+    int ret = 0;
+    mlan_ds_misc_cfg *misc = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    /* Sanity check */
+    if (wrq->u.data.pointer == NULL) {
+        PRINTM(MERROR, "hostcmd IOCTL corrupt data\n");
+        ret = -EINVAL;
+        goto done;
+    }
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    misc = (mlan_ds_misc_cfg *) req->pbuf;
+    memset(&cmd_header, 0, sizeof(cmd_header));
+
+    /* get command header */
+    if (copy_from_user
+        (&cmd_header, wrq->u.data.pointer, sizeof(HostCmd_Header))) {
+        PRINTM(MERROR, "copy from user failed: Host command header\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    misc->param.hostcmd.len = woal_le16_to_cpu(cmd_header.size);
+
+    PRINTM(MINFO, "Host command len = %u\n", misc->param.hostcmd.len);
+
+    if (!misc->param.hostcmd.len ||
+        misc->param.hostcmd.len > MLAN_SIZE_OF_CMD_BUFFER) {
+        PRINTM(MERROR, "Invalid data buffer length\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    /* get the whole command from user */
+    if (copy_from_user
+        (misc->param.hostcmd.cmd, wrq->u.data.pointer,
+         woal_le16_to_cpu(cmd_header.size))) {
+        PRINTM(MERROR, "copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    misc->sub_command = MLAN_OID_MISC_HOST_CMD;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (copy_to_user
+        (wrq->u.data.pointer, (t_u8 *) misc->param.hostcmd.cmd,
+         misc->param.hostcmd.len)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    wrq->u.data.length = misc->param.hostcmd.len;
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+#endif
+
+#if defined(WIFI_DIRECT_SUPPORT) || defined(UAP_SUPPORT)
+/**
+ *  @brief host command ioctl function
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+/*********  format of ifr_data *************/
+/*    buf_len + Hostcmd_body 		   */
+/*    buf_len: 4 bytes                     */
+/*             the length of the buf which */
+/*             can be used to return data  */
+/*             to application		   */
+/*    Hostcmd_body       	           */
+/*******************************************/
+int
+woal_hostcmd_ioctl(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    t_u32 buf_len = 0;
+    HostCmd_Header cmd_header;
+    int ret = 0;
+    mlan_ds_misc_cfg *misc = NULL;
+    mlan_ioctl_req *ioctl_req = NULL;
+
+    ENTER();
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "uap_hostcmd_ioctl() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (copy_from_user(&buf_len, req->ifr_data, sizeof(buf_len))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    memset(&cmd_header, 0, sizeof(cmd_header));
+
+    /* get command header */
+    if (copy_from_user
+        (&cmd_header, req->ifr_data + sizeof(buf_len),
+         sizeof(HostCmd_Header))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    PRINTM(MINFO, "Host command len = %d\n", woal_le16_to_cpu(cmd_header.size));
+
+    if (woal_le16_to_cpu(cmd_header.size) > MLAN_SIZE_OF_CMD_BUFFER) {
+        ret = -EINVAL;
+        goto done;
+    }
+
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (ioctl_req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    misc = (mlan_ds_misc_cfg *) ioctl_req->pbuf;
+
+    misc->param.hostcmd.len = woal_le16_to_cpu(cmd_header.size);
+
+    /* get the whole command from user */
+    if (copy_from_user
+        (misc->param.hostcmd.cmd, req->ifr_data + sizeof(buf_len),
+         misc->param.hostcmd.len)) {
+        PRINTM(MERROR, "copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    misc->sub_command = MLAN_OID_MISC_HOST_CMD;
+    ioctl_req->req_id = MLAN_IOCTL_MISC_CFG;
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (misc->param.hostcmd.len > buf_len) {
+        PRINTM(MERROR, "buf_len is too small, resp_len=%d, buf_len=%d\n",
+               (int) misc->param.hostcmd.len, (int) buf_len);
+        ret = -EFAULT;
+        goto done;
+    }
+    if (copy_to_user
+        (req->ifr_data + sizeof(buf_len), (t_u8 *) misc->param.hostcmd.cmd,
+         misc->param.hostcmd.len)) {
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+#endif
+
+/**
+ *  @brief CUSTOM_IE ioctl handler
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+int
+woal_custom_ie_ioctl(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+    mlan_ds_misc_custom_ie *custom_ie = NULL;
+    int ret = 0;
+
+    ENTER();
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "woal_custom_ie_ioctl() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (!(custom_ie = kmalloc(sizeof(mlan_ds_misc_custom_ie), GFP_KERNEL))) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    memset(custom_ie, 0, sizeof(mlan_ds_misc_custom_ie));
+
+    if (copy_from_user
+        (custom_ie, req->ifr_data, sizeof(mlan_ds_misc_custom_ie))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (ioctl_req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    misc = (mlan_ds_misc_cfg *) ioctl_req->pbuf;
+    misc->sub_command = MLAN_OID_MISC_CUSTOM_IE;
+    ioctl_req->req_id = MLAN_IOCTL_MISC_CFG;
+    if ((custom_ie->len == 0) ||
+        (custom_ie->len == sizeof(custom_ie->ie_data_list[0].ie_index)))
+        ioctl_req->action = MLAN_ACT_GET;
+    else
+        ioctl_req->action = MLAN_ACT_SET;
+
+    memcpy(&misc->param.cust_ie, custom_ie, sizeof(mlan_ds_misc_custom_ie));
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (ioctl_req->action == MLAN_ACT_GET) {
+        if (copy_to_user
+            (req->ifr_data, &misc->param.cust_ie,
+             sizeof(mlan_ds_misc_custom_ie))) {
+            PRINTM(MERROR, "Copy to user failed!\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    } else if (ioctl_req->status_code == MLAN_ERROR_IOCTL_FAIL) {
+        /* send a separate error code to indicate error from driver */
+        ret = EFAULT;
+    }
+
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    if (custom_ie)
+        kfree(custom_ie);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief send raw data packet ioctl function
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+int
+woal_send_host_packet(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    t_u32 packet_len = 0;
+    int ret = 0;
+    pmlan_buffer pmbuf = NULL;
+    mlan_status status;
+
+    ENTER();
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "woal_send_host_packet() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (copy_from_user(&packet_len, req->ifr_data, sizeof(packet_len))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+#define PACKET_HEADER_LEN        8
+    pmbuf =
+        woal_alloc_mlan_buffer(priv->phandle,
+                               MLAN_MIN_DATA_HEADER_LEN + packet_len +
+                               PACKET_HEADER_LEN);
+    if (!pmbuf) {
+        PRINTM(MERROR, "Fail to allocate mlan_buffer\n");
+        ret = -ENOMEM;
+        goto done;
+    }
+    pmbuf->data_offset = MLAN_MIN_DATA_HEADER_LEN;
+
+    /* get whole packet and header */
+    if (copy_from_user
+        (pmbuf->pbuf + pmbuf->data_offset, req->ifr_data + sizeof(packet_len),
+         PACKET_HEADER_LEN + packet_len)) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        woal_free_mlan_buffer(priv->phandle, pmbuf);
+        goto done;
+    }
+    pmbuf->data_len = PACKET_HEADER_LEN + packet_len;
+    pmbuf->buf_type = MLAN_BUF_TYPE_RAW_DATA;
+    pmbuf->bss_index = priv->bss_index;
+    status = mlan_send_packet(priv->phandle->pmlan_adapter, pmbuf);
+    switch (status) {
+    case MLAN_STATUS_PENDING:
+        atomic_inc(&priv->phandle->tx_pending);
+        queue_work(priv->phandle->workqueue, &priv->phandle->main_work);
+        break;
+    case MLAN_STATUS_SUCCESS:
+        woal_free_mlan_buffer(priv->phandle, pmbuf);
+        break;
+    case MLAN_STATUS_FAILURE:
+    default:
+        woal_free_mlan_buffer(priv->phandle, pmbuf);
+        ret = -EFAULT;
+        break;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+#if defined(UAP_WEXT)
+/**
+ *  @brief Set/Get CUSTOM_IE ioctl handler
+ *
+ *  @param mask         Mask to set or clear from caller
+ *  @param ie           IE buffer to set for beacon
+ *  @param ie_len       Length of the IE
+ *
+ *  @return         0 --success, otherwise fail
+ */
+int
+woal_set_get_custom_ie(moal_private * priv, t_u16 mask, t_u8 * ie, int ie_len)
+{
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+    mlan_ds_misc_custom_ie *misc_ie = NULL;
+    int ret = 0;
+    custom_ie *pcust_bcn_ie = NULL;
+
+    ENTER();
+
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (ioctl_req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+
+    misc = (mlan_ds_misc_cfg *) ioctl_req->pbuf;
+    misc->sub_command = MLAN_OID_MISC_CUSTOM_IE;
+    ioctl_req->req_id = MLAN_IOCTL_MISC_CFG;
+    ioctl_req->action = MLAN_ACT_SET;
+    misc_ie = &misc->param.cust_ie;
+
+#ifndef TLV_TYPE_MGMT_IE
+#define TLV_TYPE_MGMT_IE            (0x169)
+#endif
+    misc_ie->type = TLV_TYPE_MGMT_IE;
+    misc_ie->len = (sizeof(custom_ie) - MAX_IE_SIZE) + ie_len;
+    pcust_bcn_ie = misc_ie->ie_data_list;
+    pcust_bcn_ie->ie_index = 0xffff;
+    pcust_bcn_ie->mgmt_subtype_mask = mask;
+    pcust_bcn_ie->ie_length = ie_len;
+    memcpy(pcust_bcn_ie->ie_buffer, ie, ie_len);
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+    }
+
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+#endif /* defined(HOST_TXRX_MGMT_FRAME) && defined(UAP_WEXT) */
+
+/**
+ *  @brief ioctl function get BSS type
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+int
+woal_get_bss_type(struct net_device *dev, struct ifreq *req)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int bss_type;
+
+    ENTER();
+
+    bss_type = (int) priv->bss_type;
+    if (copy_to_user(req->ifr_data, &bss_type, sizeof(int))) {
+        PRINTM(MINFO, "Copy to user failed!\n");
+        ret = -EFAULT;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+#if defined(WIFI_DIRECT_SUPPORT)
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+/**
+ * @brief Swithces BSS role of WFD interface
+ *
+ * @param priv          A pointer to moal_private structure
+ * @param action        Action: set or get
+ * @param wait_option   Wait option (MOAL_WAIT or MOAL_NO_WAIT)
+ * @param bss_role      A pointer to bss role
+ *
+ * @return         0 --success, otherwise fail
+ */
+mlan_status
+woal_bss_role_cfg(moal_private * priv, t_u8 action,
+                  t_u8 wait_option, t_u8 * bss_role)
+{
+    int ret = 0;
+    mlan_ds_bss *bss = NULL;
+    mlan_ioctl_req *req = NULL;
+    struct net_device *dev = priv->netdev;
+
+    ENTER();
+
+    if (priv->bss_type != MLAN_BSS_TYPE_WIFIDIRECT) {
+        PRINTM(MWARN, "Command is not allowed for this interface\n");
+        ret = -EPERM;
+        goto done;
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_BSS_ROLE;
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = action;
+    if (action == MLAN_ACT_SET) {
+        bss->param.bss_role = *bss_role;
+    }
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, wait_option)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (action == MLAN_ACT_GET) {
+        *bss_role = bss->param.bss_role;
+    } else {
+        /* Update moal_private */
+        priv->bss_role = *bss_role;
+        if (priv->bss_type == MLAN_BSS_TYPE_UAP)
+            priv->bss_type = MLAN_BSS_TYPE_STA;
+        else if (priv->bss_type == MLAN_BSS_TYPE_STA)
+            priv->bss_type = MLAN_BSS_TYPE_UAP;
+
+        if (*bss_role == MLAN_BSS_ROLE_UAP) {
+            /* Switch: STA -> uAP */
+            /* Setup the OS Interface to our functions */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,29)
+            dev->do_ioctl = woal_uap_do_ioctl;
+            dev->set_multicast_list = woal_uap_set_multicast_list;
+#else
+            dev->netdev_ops = &woal_uap_netdev_ops;
+#endif
+#ifdef UAP_WEXT
+            if (IS_UAP_WEXT(cfg80211_wext)) {
+#ifdef WIRELESS_EXT
+#if WIRELESS_EXT < 21
+                dev->get_wireless_stats = woal_get_uap_wireless_stats;
+#endif
+                dev->wireless_handlers =
+                    (struct iw_handler_def *) &woal_uap_handler_def;
+#endif /* WIRELESS_EXT */
+                init_waitqueue_head(&priv->w_stats_wait_q);
+            }
+#endif /* UAP_WEXT */
+        } else if (*bss_role == MLAN_BSS_ROLE_STA) {
+            /* Switch: uAP -> STA */
+            /* Setup the OS Interface to our functions */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,29)
+            dev->do_ioctl = woal_do_ioctl;
+            dev->set_multicast_list = woal_set_multicast_list;
+#else
+            dev->netdev_ops = &woal_netdev_ops;
+#endif
+#ifdef STA_WEXT
+            if (IS_STA_WEXT(cfg80211_wext)) {
+#ifdef  WIRELESS_EXT
+#if WIRELESS_EXT < 21
+                dev->get_wireless_stats = woal_get_wireless_stats;
+#endif
+                dev->wireless_handlers =
+                    (struct iw_handler_def *) &woal_handler_def;
+#endif
+                init_waitqueue_head(&priv->w_stats_wait_q);
+            }
+#endif /* STA_WEXT */
+        }
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Set/Get BSS role
+ *
+ * @param priv     A pointer to moal_private structure
+ * @param wrq      A pointer to iwreq structure
+ *
+ * @return         0 --success, otherwise fail
+ */
+int
+woal_set_get_bss_role(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    int bss_role = 0;
+    t_u8 action = MLAN_ACT_GET;
+
+    ENTER();
+
+    if (wrq->u.data.length) {
+        if (copy_from_user(&bss_role, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if ((bss_role != MLAN_BSS_ROLE_STA &&
+             bss_role != MLAN_BSS_ROLE_UAP) ||
+            (priv->bss_type != MLAN_BSS_TYPE_WIFIDIRECT)) {
+            PRINTM(MWARN, "Invalid BSS role\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        if (bss_role == GET_BSS_ROLE(priv)) {
+            PRINTM(MWARN, "Already BSS is in desired role\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        action = MLAN_ACT_SET;
+        /* Reset interface */
+        woal_reset_intf(priv, MOAL_IOCTL_WAIT, MFALSE);
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_bss_role_cfg(priv,
+                                                 action, MOAL_IOCTL_WAIT,
+                                                 (t_u8 *) & bss_role)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (!wrq->u.data.length) {
+        if (copy_to_user(wrq->u.data.pointer, &bss_role, sizeof(int))) {
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = 1;
+    } else {
+        /* Initialize private structures */
+        woal_init_priv(priv, MOAL_IOCTL_WAIT);
+
+        /* Enable interfaces */
+        netif_device_attach(priv->netdev);
+        woal_start_queue(priv->netdev);
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+#endif /* STA_WEXT || UAP_WEXT */
+#endif /* STA_SUPPORT && UAP_SUPPORT */
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+
+/**
+ *  @brief Get Host Sleep parameters
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param action       Action: set or get
+ *  @param wait_option  Wait option (MOAL_WAIT or MOAL_NO_WAIT)
+ *  @param hscfg        A pointer to mlan_ds_hs_cfg structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_set_get_hs_params(moal_private * priv, t_u16 action, t_u8 wait_option,
+                       mlan_ds_hs_cfg * hscfg)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_pm_cfg *pmcfg = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_pm_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    pmcfg = (mlan_ds_pm_cfg *) req->pbuf;
+    pmcfg->sub_command = MLAN_OID_PM_CFG_HS_CFG;
+    req->req_id = MLAN_IOCTL_PM_CFG;
+    req->action = action;
+    if (action == MLAN_ACT_SET)
+        memcpy(&pmcfg->param.hs_cfg, hscfg, sizeof(mlan_ds_hs_cfg));
+
+    /* Send IOCTL request to MLAN */
+    ret = woal_request_ioctl(priv, req, wait_option);
+    if (ret == MLAN_STATUS_SUCCESS) {
+        if (hscfg && action == MLAN_ACT_GET) {
+            memcpy(hscfg, &pmcfg->param.hs_cfg, sizeof(mlan_ds_hs_cfg));
+        }
+    }
+  done:
+    if (req && (ret != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Cancel Host Sleep configuration
+ *
+ *  @param priv             A pointer to moal_private structure
+ *  @param wait_option      wait option
+ *
+ *  @return      MLAN_STATUS_SUCCESS, MLAN_STATUS_PENDING,
+ *                      or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_cancel_hs(moal_private * priv, t_u8 wait_option)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_hs_cfg hscfg;
+
+    ENTER();
+
+    /* Cancel Host Sleep */
+    hscfg.conditions = HOST_SLEEP_CFG_CANCEL;
+    hscfg.is_invoke_hostcmd = MTRUE;
+    ret = woal_set_get_hs_params(priv, MLAN_ACT_SET, wait_option, &hscfg);
+
+    LEAVE();
+    return ret;
+}
+
+#if defined(SDIO_SUSPEND_RESUME)
+/**  @brief This function enables the host sleep
+ *
+ *  @param priv   A Pointer to the moal_private structure
+ *  @return 	  MTRUE or MFALSE
+ */
+int
+woal_enable_hs(moal_private * priv)
+{
+    mlan_ds_hs_cfg hscfg;
+    moal_handle *handle = NULL;
+    int hs_actived = MFALSE;
+    int timeout = 0;
+#ifdef SDIO_SUSPEND_RESUME
+    mlan_ds_ps_info pm_info;
+#endif
+
+    ENTER();
+
+    if (priv == NULL) {
+        PRINTM(MERROR, "Invalid priv\n");
+        goto done;
+    }
+    handle = priv->phandle;
+    if (handle->hs_activated == MTRUE) {
+        PRINTM(MIOCTL, "HS Already actived\n");
+        hs_actived = MTRUE;
+        goto done;
+    }
+#ifdef STA_SUPPORT
+    woal_reconfig_bgscan(priv->phandle);
+#endif
+    /* Enable Host Sleep */
+    handle->hs_activate_wait_q_woken = MFALSE;
+    memset(&hscfg, 0, sizeof(mlan_ds_hs_cfg));
+    hscfg.is_invoke_hostcmd = MTRUE;
+    if (woal_set_get_hs_params(priv, MLAN_ACT_SET, MOAL_NO_WAIT, &hscfg) ==
+        MLAN_STATUS_FAILURE) {
+        PRINTM(MIOCTL, "IOCTL request HS enable failed\n");
+        goto done;
+    }
+    timeout = wait_event_interruptible_timeout(handle->hs_activate_wait_q,
+                                               handle->hs_activate_wait_q_woken,
+                                               HS_ACTIVE_TIMEOUT);
+    sdio_claim_host(((struct sdio_mmc_card *) handle->card)->func);
+    if ((handle->hs_activated == MTRUE) || (handle->is_suspended == MTRUE)) {
+        PRINTM(MCMND, "suspend success! force=%u skip=%u\n",
+               handle->hs_force_count, handle->hs_skip_count);
+        hs_actived = MTRUE;
+    }
+#ifdef SDIO_SUSPEND_RESUME
+    else {
+        handle->suspend_fail = MTRUE;
+        woal_get_pm_info(priv, &pm_info);
+        if (pm_info.is_suspend_allowed == MTRUE) {
+#ifdef MMC_PM_FUNC_SUSPENDED
+            woal_wlan_is_suspended(priv->phandle);
+#endif
+            handle->hs_force_count++;
+            PRINTM(MCMND, "suspend allowed! force=%u skip=%u\n",
+                   handle->hs_force_count, handle->hs_skip_count);
+            hs_actived = MTRUE;
+        }
+    }
+#endif /* SDIO_SUSPEND_RESUME */
+    sdio_release_host(((struct sdio_mmc_card *) handle->card)->func);
+    if (hs_actived != MTRUE) {
+        handle->hs_skip_count++;
+#ifdef SDIO_SUSPEND_RESUME
+        PRINTM(MCMND, "suspend skipped! timeout=%d allow=%d force=%u skip=%u\n",
+               timeout, (int) pm_info.is_suspend_allowed,
+               handle->hs_force_count, handle->hs_skip_count);
+#else
+        PRINTM(MCMND, "suspend skipped! timeout=%d skip=%u\n",
+               timeout, handle->hs_skip_count);
+#endif
+        woal_cancel_hs(priv, MOAL_NO_WAIT);
+    }
+  done:
+    LEAVE();
+    return hs_actived;
+}
+#endif
+
+/**
+ *  @brief This function send soft_reset command to firmware
+ *
+ *  @param handle   A pointer to moal_handle structure
+ *  @return 	    MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING on success, otherwise failure code
+ */
+mlan_status
+woal_request_soft_reset(moal_handle * handle)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+
+    ENTER();
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req) {
+        misc = (mlan_ds_misc_cfg *) req->pbuf;
+        misc->sub_command = MLAN_OID_MISC_SOFT_RESET;
+        req->req_id = MLAN_IOCTL_MISC_CFG;
+        req->action = MLAN_ACT_SET;
+        ret =
+            woal_request_ioctl(woal_get_priv(handle, MLAN_BSS_ROLE_ANY), req,
+                               MOAL_PROC_WAIT);
+    }
+
+    handle->surprise_removed = MTRUE;
+    woal_sched_timeout(5);
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set wapi enable
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param enable               MTRUE or MFALSE
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_set_wapi_enable(moal_private * priv, t_u8 wait_option, t_u32 enable)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_WAPI_ENABLED;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    req->action = MLAN_ACT_SET;
+    sec->param.wapi_enabled = enable;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Get version
+ *
+ *  @param handle 		A pointer to moal_handle structure
+ *  @param version		A pointer to version buffer
+ *  @param max_len		max length of version buffer
+ *
+ *  @return 	   		N/A
+ */
+void
+woal_get_version(moal_handle * handle, char *version, int max_len)
+{
+    union
+    {
+        t_u32 l;
+        t_u8 c[4];
+    } ver;
+    char fw_ver[32];
+
+    ENTER();
+
+    ver.l = handle->fw_release_number;
+    snprintf(fw_ver, sizeof(fw_ver), "%u.%u.%u.p%u",
+             ver.c[2], ver.c[1], ver.c[0], ver.c[3]);
+
+    snprintf(version, max_len, driver_version, fw_ver);
+
+    LEAVE();
+}
+
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+/**
+ *  @brief Get Driver Version
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param req          A pointer to ifreq structure
+ *
+ *  @return             0 --success, otherwise fail
+ */
+int
+woal_get_driver_version(moal_private * priv, struct ifreq *req)
+{
+    struct iwreq *wrq = (struct iwreq *) req;
+    int len;
+    char buf[MLAN_MAX_VER_STR_LEN];
+    ENTER();
+
+    woal_get_version(priv->phandle, buf, sizeof(buf) - 1);
+
+    len = strlen(buf);
+    if (wrq->u.data.pointer) {
+        if (copy_to_user(wrq->u.data.pointer, buf, len)) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            LEAVE();
+            return -EFAULT;
+        }
+        wrq->u.data.length = len;
+    }
+    PRINTM(MINFO, "MOAL VERSION: %s\n", buf);
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Get extended driver version
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param ireq         A pointer to ifreq structure
+ *
+ *  @return             0 --success, otherwise fail
+ */
+int
+woal_get_driver_verext(moal_private * priv, struct ifreq *ireq)
+{
+    struct iwreq *wrq = (struct iwreq *) ireq;
+    mlan_ds_get_info *info = NULL;
+    mlan_ioctl_req *req = NULL;
+    int ret = 0;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_get_info));
+    if (req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+
+    info = (mlan_ds_get_info *) req->pbuf;
+    info->sub_command = MLAN_OID_GET_VER_EXT;
+    req->req_id = MLAN_IOCTL_GET_INFO;
+    req->action = MLAN_ACT_GET;
+
+    if (!wrq->u.data.flags) {
+        info->param.ver_ext.version_str_sel =
+            *((int *) (wrq->u.name + SUBCMD_OFFSET));
+    } else {
+        if (copy_from_user
+            (&info->param.ver_ext.version_str_sel, wrq->u.data.pointer,
+             sizeof(info->param.ver_ext.version_str_sel))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        } else {
+            if (((t_s32) (info->param.ver_ext.version_str_sel)) < 0) {
+                PRINTM(MERROR, "Invalid arguments!\n");
+                ret = -EINVAL;
+                goto done;
+            }
+        }
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (wrq->u.data.pointer) {
+        if (copy_to_user(wrq->u.data.pointer, info->param.ver_ext.version_str,
+                         strlen(info->param.ver_ext.version_str))) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = strlen(info->param.ver_ext.version_str);
+    }
+
+    PRINTM(MINFO, "MOAL EXTENDED VERSION: %s\n",
+           info->param.ver_ext.version_str);
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+#endif
+
+#ifdef DEBUG_LEVEL1
+/**
+ *  @brief Set driver debug bit masks to mlan in order to enhance performance
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param drvdbg		Driver debug level
+ *
+ *  @return             0 --success, otherwise fail
+ */
+int
+woal_set_drvdbg(moal_private * priv, t_u32 drvdbg)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+    int ret = 0;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+
+    misc = (mlan_ds_misc_cfg *) req->pbuf;
+    misc->sub_command = MLAN_OID_MISC_DRVDBG;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+    req->action = MLAN_ACT_SET;
+    misc->param.drvdbg = drvdbg;
+
+    ret = woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT);
+
+    if (req && (ret != MLAN_STATUS_PENDING))
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+#endif
+
+/**
+ *  @brief Mgmt frame forward registration
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param action       Action: set or get
+ *  @param pmgmt_subtype_mask   A Pointer to mgmt frame subtype mask
+ *  @param wait_option  wait option (MOAL_WAIT or MOAL_NO_WAIT)
+ *
+ *  @return             0 --success, otherwise fail
+ */
+int
+woal_reg_rx_mgmt_ind(moal_private * priv, t_u16 action,
+                     t_u32 * pmgmt_subtype_mask, t_u8 wait_option)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+    int ret = 0;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+
+    misc = (mlan_ds_misc_cfg *) req->pbuf;
+    misc->sub_command = MLAN_OID_MISC_RX_MGMT_IND;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+    req->action = action;
+    misc->param.mgmt_subtype_mask = *pmgmt_subtype_mask;
+    if (req->action == MLAN_ACT_SET)
+        memcpy(&misc->param.mgmt_subtype_mask,
+               pmgmt_subtype_mask, sizeof(misc->param.mgmt_subtype_mask));
+
+    ret = woal_request_ioctl(priv, req, wait_option);
+
+    if (req->action == MLAN_ACT_GET)
+        memcpy(pmgmt_subtype_mask, &misc->param.mgmt_subtype_mask,
+               sizeof(misc->param.mgmt_subtype_mask));
+
+    if (req && (ret != MLAN_STATUS_PENDING))
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get Transmit beamforming configuration
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param action       Action: set or get
+ *  @param tx_bf_cfg    A pointer to tx_bf_cfg structure
+ *
+ *  @return         MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_set_get_tx_bf_cfg(moal_private * priv, t_u16 action,
+                       mlan_ds_11n_tx_bf_cfg * tx_bf_cfg)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_11n_cfg *bf_cfg = NULL;
+
+    ENTER();
+
+    /* Sanity test */
+    if (tx_bf_cfg == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11n_cfg));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    bf_cfg = (mlan_ds_11n_cfg *) req->pbuf;
+    req->req_id = MLAN_IOCTL_11N_CFG;
+    bf_cfg->sub_command = MLAN_OID_11N_CFG_TX_BF_CFG;
+
+    req->action = action;
+    memcpy(&bf_cfg->param.tx_bf, tx_bf_cfg, sizeof(mlan_ds_11n_tx_bf_cfg));
+
+    /* Send IOCTL request to MLAN */
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    if (action == MLAN_ACT_GET)
+        memcpy(tx_bf_cfg, &bf_cfg->param.tx_bf, sizeof(mlan_ds_11n_tx_bf_cfg));
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Handle ioctl resp
+ *
+ *  @param priv 	Pointer to moal_private structure
+ *  @param req		Pointer to mlan_ioctl_req structure
+ *
+ *  @return    		N/A
+ */
+void
+woal_process_ioctl_resp(moal_private * priv, mlan_ioctl_req * req)
+{
+    ENTER();
+
+    if (priv == NULL) {
+        LEAVE();
+        return;
+    }
+    switch (req->req_id) {
+    case MLAN_IOCTL_GET_INFO:
+#ifdef STA_WEXT
+#ifdef STA_SUPPORT
+        if (IS_STA_WEXT(cfg80211_wext) &&
+            GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA)
+            woal_ioctl_get_info_resp(priv, (mlan_ds_get_info *) req->pbuf);
+#endif
+#endif
+#ifdef UAP_WEXT
+#ifdef UAP_SUPPORT
+        if (IS_UAP_WEXT(cfg80211_wext) &&
+            GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP)
+            woal_ioctl_get_uap_info_resp(priv, (mlan_ds_get_info *) req->pbuf);
+#endif
+#endif
+        break;
+#ifdef STA_WEXT
+#ifdef STA_SUPPORT
+    case MLAN_IOCTL_BSS:
+        if (IS_STA_WEXT(cfg80211_wext) &&
+            GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA)
+            woal_ioctl_get_bss_resp(priv, (mlan_ds_bss *) req->pbuf);
+        break;
+#endif
+#endif
+    default:
+        break;
+    }
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief Get PM info
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param pm_info              A pointer to mlan_ds_ps_info structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_get_pm_info(moal_private * priv, mlan_ds_ps_info * pm_info)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_pm_cfg *pmcfg = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_pm_cfg));
+    if (req == NULL) {
+        PRINTM(MERROR, "Fail to alloc mlan_ds_pm_cfg buffer\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    pmcfg = (mlan_ds_pm_cfg *) req->pbuf;
+    pmcfg->sub_command = MLAN_OID_PM_INFO;
+    req->req_id = MLAN_IOCTL_PM_CFG;
+    req->action = MLAN_ACT_GET;
+
+    /* Send IOCTL request to MLAN */
+    ret = woal_request_ioctl(priv, req, MOAL_CMD_WAIT);
+    if (ret == MLAN_STATUS_SUCCESS) {
+        if (pm_info) {
+            memcpy(pm_info, &pmcfg->param.ps_info, sizeof(mlan_ds_ps_info));
+        }
+    }
+  done:
+    if (req && (ret != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get Deep Sleep
+ *
+ *  @param priv      Pointer to the moal_private driver data struct
+ *  @param data      Pointer to return deep_sleep setting
+ *
+ *  @return          0 --success, otherwise fail
+ */
+int
+woal_get_deep_sleep(moal_private * priv, t_u32 * data)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_pm_cfg *pm = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_pm_cfg));
+    if (req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+    pm = (mlan_ds_pm_cfg *) req->pbuf;
+    pm->sub_command = MLAN_OID_PM_CFG_DEEP_SLEEP;
+    req->req_id = MLAN_IOCTL_PM_CFG;
+
+    req->action = MLAN_ACT_GET;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    *data = pm->param.auto_deep_sleep.auto_ds;
+    *(data + 1) = pm->param.auto_deep_sleep.idletime;
+
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set Deep Sleep
+ *
+ *  @param priv         Pointer to the moal_private driver data struct
+ *  @param wait_option  wait option
+ *  @param bdeep_sleep  TRUE--enalbe deepsleep, FALSE--disable deepsleep
+ *  @param idletime     Idle time for optimized PS API
+ *
+ *  @return             0 --success, otherwise fail
+ */
+int
+woal_set_deep_sleep(moal_private * priv, t_u8 wait_option, BOOLEAN bdeep_sleep,
+                    t_u16 idletime)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_pm_cfg *pm = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_pm_cfg));
+    if (req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+    pm = (mlan_ds_pm_cfg *) req->pbuf;
+    pm->sub_command = MLAN_OID_PM_CFG_DEEP_SLEEP;
+    req->req_id = MLAN_IOCTL_PM_CFG;
+
+    req->action = MLAN_ACT_SET;
+    if (bdeep_sleep == MTRUE) {
+        PRINTM(MIOCTL, "Deep Sleep: sleep\n");
+        pm->param.auto_deep_sleep.auto_ds = DEEP_SLEEP_ON;
+        if (idletime) {
+            pm->param.auto_deep_sleep.idletime = idletime;
+        }
+        ret = woal_request_ioctl(priv, req, wait_option);
+        if (ret != MLAN_STATUS_SUCCESS && ret != MLAN_STATUS_PENDING) {
+            ret = -EFAULT;
+            goto done;
+        }
+    } else {
+        PRINTM(MIOCTL, "%lu : Deep Sleep: wakeup\n", jiffies);
+        pm->param.auto_deep_sleep.auto_ds = DEEP_SLEEP_OFF;
+        ret = woal_request_ioctl(priv, req, wait_option);
+        if (ret != MLAN_STATUS_SUCCESS && ret != MLAN_STATUS_PENDING) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+  done:
+    if (req && (ret != MLAN_STATUS_PENDING))
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Cancel CAC period block
+ *
+ *  @param priv     A pointer to moal_private structure
+ *
+ *  @return         N/A
+ */
+void
+woal_cancel_cac_block(moal_private * priv)
+{
+    ENTER();
+    /* if during CAC period, wake up wait queue */
+    if (priv->phandle->cac_period == MTRUE) {
+        priv->phandle->cac_period = MFALSE;
+        priv->phandle->meas_start_jiffies = 0;
+        if (priv->phandle->delay_bss_start == MTRUE) {
+            priv->phandle->delay_bss_start = MFALSE;
+        }
+        if (priv->phandle->meas_wait_q_woken == MFALSE) {
+            priv->phandle->meas_wait_q_woken = MTRUE;
+            wake_up_interruptible(&priv->phandle->meas_wait_q);
+        }
+    }
+    LEAVE();
+}
+
+/** MEAS report timeout value in seconds */
+
+/**
+ *  @brief Issue MLAN_OID_11H_CHANNEL_CHECK ioctl
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *
+ *  @return         0 --success, otherwise fail
+ */
+int
+woal_11h_channel_check_ioctl(moal_private * priv)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_11h_cfg *ds_11hcfg = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11h_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    ds_11hcfg = (mlan_ds_11h_cfg *) req->pbuf;
+
+    ds_11hcfg->sub_command = MLAN_OID_11H_CHANNEL_CHECK;
+    req->req_id = MLAN_IOCTL_11H_CFG;
+    req->action = MLAN_ACT_SET;
+    /* Send Channel Check command and wait until the report is ready */
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* set flag from here */
+    priv->phandle->cac_period = MTRUE;
+    priv->phandle->meas_start_jiffies = jiffies;
+
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+#if defined(WIFI_DIRECT_SUPPORT)
+/**
+ *  @brief set/get wifi direct mode
+ *
+ *  @param priv            A pointer to moal_private structure
+ *  @param action     	   set or get
+ *  @param mode            A pointer to wifi direct mode
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_wifi_direct_mode_cfg(moal_private * priv, t_u16 action, t_u16 * mode)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_bss *bss = NULL;
+
+    ENTER();
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_WIFI_DIRECT_MODE;
+    req->req_id = MLAN_IOCTL_BSS;
+
+    req->action = action;
+    if (action == MLAN_ACT_SET)
+        bss->param.wfd_mode = *mode;
+    ret = woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT);
+    if (ret == MLAN_STATUS_SUCCESS) {
+        *mode = bss->param.wfd_mode;
+        PRINTM(MIOCTL, "ACT=%d, wifi_direct_mode=%d\n", action, *mode);
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief set remain channel
+ *
+ *  @param priv            A pointer to moal_private structure
+ *  @param wait_option  Wait option
+ *  @param pchan           A pointer to mlan_ds_remain_chan structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_set_remain_channel_ioctl(moal_private * priv, t_u8 wait_option,
+                              mlan_ds_remain_chan * pchan)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_radio_cfg *radio_cfg = NULL;
+
+    ENTER();
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_radio_cfg));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    radio_cfg = (mlan_ds_radio_cfg *) req->pbuf;
+    radio_cfg->sub_command = MLAN_OID_REMAIN_CHAN_CFG;
+    req->req_id = MLAN_IOCTL_RADIO_CFG;
+
+    req->action = MLAN_ACT_SET;
+    memcpy(&radio_cfg->param.remain_chan, pchan, sizeof(mlan_ds_remain_chan));
+    ret = woal_request_ioctl(priv, req, wait_option);
+    if (ret == MLAN_STATUS_SUCCESS) {
+        memcpy(pchan, &radio_cfg->param.remain_chan,
+               sizeof(mlan_ds_remain_chan));
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+#endif /* WIFI_DIRECT_SUPPORT */
+
+#ifdef STA_SUPPORT
+/**
+ *  @brief Get RSSI info
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wait_option  Wait option
+ *  @param signal       A pointer tp mlan_ds_get_signal structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_get_signal_info(moal_private * priv, t_u8 wait_option,
+                     mlan_ds_get_signal * signal)
+{
+    int ret = 0;
+    mlan_ds_get_info *info = NULL;
+    mlan_ioctl_req *req = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_get_info));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    info = (mlan_ds_get_info *) req->pbuf;
+    info->sub_command = MLAN_OID_GET_SIGNAL;
+    info->param.signal.selector = ALL_RSSI_INFO_MASK;
+    req->req_id = MLAN_IOCTL_GET_INFO;
+    req->action = MLAN_ACT_GET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+    if (status == MLAN_STATUS_SUCCESS) {
+        if (signal)
+            memcpy(signal, &info->param.signal, sizeof(mlan_ds_get_signal));
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext)) {
+            if (info->param.signal.selector & BCN_RSSI_AVG_MASK)
+                priv->w_stats.qual.level = info->param.signal.bcn_rssi_avg;
+            if (info->param.signal.selector & BCN_NF_AVG_MASK)
+                priv->w_stats.qual.noise = info->param.signal.bcn_nf_avg;
+        }
+#endif
+    }
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Get scan table
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wait_option  Wait option
+ *  @param scan_resp    A pointer to mlan_scan_resp structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_get_scan_table(moal_private * priv, t_u8 wait_option,
+                    mlan_scan_resp * scan_resp)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_scan *scan = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_scan));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    scan = (mlan_ds_scan *) req->pbuf;
+    scan->sub_command = MLAN_OID_SCAN_NORMAL;
+    req->req_id = MLAN_IOCTL_SCAN;
+    req->action = MLAN_ACT_GET;
+    memcpy((void *) &scan->param.scan_resp, (void *) scan_resp,
+           sizeof(mlan_scan_resp));
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+    if (status == MLAN_STATUS_SUCCESS) {
+        if (scan_resp) {
+            memcpy(scan_resp, &scan->param.scan_resp, sizeof(mlan_scan_resp));
+        }
+    }
+
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Request a scan
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param req_ssid             A pointer to mlan_802_11_ssid structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_request_scan(moal_private * priv,
+                  t_u8 wait_option, mlan_802_11_ssid * req_ssid)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    moal_handle *handle = priv->phandle;
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_scan *scan = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    if (MOAL_ACQ_SEMAPHORE_BLOCK(&handle->async_sem)) {
+        PRINTM(MERROR, "Acquire semaphore error, request_scan\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    handle->scan_pending_on_block = MTRUE;
+
+    /* Allocate an IOCTL request buffer */
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_scan));
+    if (ioctl_req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    scan = (mlan_ds_scan *) ioctl_req->pbuf;
+
+    if (req_ssid && req_ssid->ssid_len != 0) {
+        /* Specific SSID scan */
+        ioctl_req->req_id = MLAN_IOCTL_SCAN;
+        ioctl_req->action = MLAN_ACT_SET;
+
+        scan->sub_command = MLAN_OID_SCAN_SPECIFIC_SSID;
+
+        memcpy(scan->param.scan_req.scan_ssid.ssid,
+               req_ssid->ssid, MIN(MLAN_MAX_SSID_LENGTH, req_ssid->ssid_len));
+        scan->param.scan_req.scan_ssid.ssid_len =
+            MIN(MLAN_MAX_SSID_LENGTH, req_ssid->ssid_len);
+    } else {
+        /* Normal scan */
+        ioctl_req->req_id = MLAN_IOCTL_SCAN;
+        ioctl_req->action = MLAN_ACT_SET;
+
+        scan->sub_command = MLAN_OID_SCAN_NORMAL;
+    }
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, ioctl_req, wait_option);
+
+    if (status == MLAN_STATUS_FAILURE) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+  done:
+    if ((ioctl_req) && (status != MLAN_STATUS_PENDING))
+        kfree(ioctl_req);
+
+    if (ret == MLAN_STATUS_FAILURE) {
+        handle->scan_pending_on_block = MFALSE;
+        MOAL_REL_SEMAPHORE(&handle->async_sem);
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Change Adhoc Channel
+ *
+ *  @param priv 		A pointer to moal_private structure
+ *  @param channel		The channel to be set.
+ *
+ *  @return 	   		MLAN_STATUS_SUCCESS--success, MLAN_STATUS_FAILURE--fail
+ */
+mlan_status
+woal_change_adhoc_chan(moal_private * priv, int channel)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_bss_info bss_info;
+    mlan_ds_bss *bss = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    memset(&bss_info, 0, sizeof(bss_info));
+
+    /* Get BSS information */
+    if (MLAN_STATUS_SUCCESS !=
+        woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    if (bss_info.bss_mode == MLAN_BSS_MODE_INFRA) {
+        ret = MLAN_STATUS_SUCCESS;
+        goto done;
+    }
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Get current channel */
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_IBSS_CHANNEL;
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = MLAN_ACT_GET;
+
+    /* Send IOCTL request to MLAN */
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    if (bss->param.bss_chan.channel == (unsigned int) channel) {
+        ret = MLAN_STATUS_SUCCESS;
+        goto done;
+    }
+    PRINTM(MINFO, "Updating Channel from %d to %d\n",
+           (int) bss->param.bss_chan.channel, channel);
+
+    if (bss_info.media_connected != MTRUE) {
+        ret = MLAN_STATUS_SUCCESS;
+        goto done;
+    }
+
+    /* Do disonnect */
+    bss->sub_command = MLAN_OID_BSS_STOP;
+    memset((t_u8 *) & bss->param.bssid, 0, ETH_ALEN);
+
+    /* Send IOCTL request to MLAN */
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Do specific SSID scanning */
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_scan(priv, MOAL_IOCTL_WAIT, &bss_info.ssid)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    /* Start/Join Adhoc network */
+    bss->sub_command = MLAN_OID_BSS_START;
+    memset(&bss->param.ssid_bssid, 0, sizeof(mlan_ssid_bssid));
+    memcpy(&bss->param.ssid_bssid.ssid, &bss_info.ssid,
+           sizeof(mlan_802_11_ssid));
+
+    /* Send IOCTL request to MLAN */
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = MLAN_STATUS_FAILURE;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Find the best network to associate
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param ssid_bssid           A pointer to mlan_ssid_bssid structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_find_best_network(moal_private * priv, t_u8 wait_option,
+                       mlan_ssid_bssid * ssid_bssid)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_bss *bss = NULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    t_u8 *mac = 0;
+
+    ENTER();
+
+    if (!ssid_bssid) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    bss = (mlan_ds_bss *) req->pbuf;
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = MLAN_ACT_GET;
+    bss->sub_command = MLAN_OID_BSS_FIND_BSS;
+
+    memcpy(&bss->param.ssid_bssid, ssid_bssid, sizeof(mlan_ssid_bssid));
+
+    /* Send IOCTL request to MLAN */
+    ret = woal_request_ioctl(priv, req, wait_option);
+    if (ret == MLAN_STATUS_SUCCESS) {
+        memcpy(ssid_bssid, &bss->param.ssid_bssid, sizeof(mlan_ssid_bssid));
+        mac = (t_u8 *) & ssid_bssid->bssid;
+        PRINTM(MINFO,
+               "Find network: ssid=%s, %02x:%02x:%02x:%02x:%02x:%02x, idx=%d\n",
+               ssid_bssid->ssid.ssid, mac[0], mac[1], mac[2], mac[3], mac[4],
+               mac[5], (int) ssid_bssid->idx);
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get authentication mode
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param auth_mode            A pointer to authentication mode
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_get_auth_mode(moal_private * priv, t_u8 wait_option, t_u32 * auth_mode)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_AUTH_MODE;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    req->action = MLAN_ACT_GET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+    if (status == MLAN_STATUS_SUCCESS && auth_mode) {
+        *auth_mode = sec->param.auth_mode;
+    }
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Get encrypt mode
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param encrypt_mode         A pointer to encrypt mode
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_get_encrypt_mode(moal_private * priv, t_u8 wait_option,
+                      t_u32 * encrypt_mode)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_ENCRYPT_MODE;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    req->action = MLAN_ACT_GET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+    if (status == MLAN_STATUS_SUCCESS && encrypt_mode) {
+        *encrypt_mode = sec->param.encrypt_mode;
+    }
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Get WPA enable
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param enable               A pointer to wpa enable status
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_get_wpa_enable(moal_private * priv, t_u8 wait_option, t_u32 * enable)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_WPA_ENABLED;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    req->action = MLAN_ACT_GET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+    if (status == MLAN_STATUS_SUCCESS && enable) {
+        *enable = sec->param.wpa_enabled;
+    }
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Set authentication mode
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param auth_mode            Authentication mode
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_set_auth_mode(moal_private * priv, t_u8 wait_option, t_u32 auth_mode)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_AUTH_MODE;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    req->action = MLAN_ACT_SET;
+    sec->param.auth_mode = auth_mode;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Set encrypt mode
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param encrypt_mode         Encryption mode
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_set_encrypt_mode(moal_private * priv, t_u8 wait_option, t_u32 encrypt_mode)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_ENCRYPT_MODE;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    req->action = MLAN_ACT_SET;
+    sec->param.encrypt_mode = encrypt_mode;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Set wpa enable
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param enable               MTRUE or MFALSE
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_set_wpa_enable(moal_private * priv, t_u8 wait_option, t_u32 enable)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_WPA_ENABLED;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    req->action = MLAN_ACT_SET;
+    sec->param.wpa_enabled = enable;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief enable wep key
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_enable_wep_key(moal_private * priv, t_u8 wait_option)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_ENCRYPT_KEY;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    req->action = MLAN_ACT_SET;
+    sec->param.encrypt_key.key_disable = MFALSE;
+    sec->param.encrypt_key.key_index = MLAN_KEY_INDEX_DEFAULT;
+    sec->param.encrypt_key.is_current_wep_key = MTRUE;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Request user scan
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param scan_cfg             A pointer to wlan_user_scan_config structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_request_userscan(moal_private * priv,
+                      t_u8 wait_option, wlan_user_scan_cfg * scan_cfg)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_scan *scan = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    moal_handle *handle = priv->phandle;
+    ENTER();
+
+    if (MOAL_ACQ_SEMAPHORE_BLOCK(&handle->async_sem)) {
+        PRINTM(MERROR, "Acquire semaphore error, request_scan\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    handle->scan_pending_on_block = MTRUE;
+
+    /* Allocate an IOCTL request buffer */
+    ioctl_req =
+        woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_scan) +
+                                  sizeof(wlan_user_scan_cfg));
+    if (ioctl_req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    scan = (mlan_ds_scan *) ioctl_req->pbuf;
+    scan->sub_command = MLAN_OID_SCAN_USER_CONFIG;
+    ioctl_req->req_id = MLAN_IOCTL_SCAN;
+    ioctl_req->action = MLAN_ACT_SET;
+    memcpy(scan->param.user_scan.scan_cfg_buf, scan_cfg,
+           sizeof(wlan_user_scan_cfg));
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, ioctl_req, wait_option);
+    if (status == MLAN_STATUS_FAILURE) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+  done:
+    if ((ioctl_req) && (status != MLAN_STATUS_PENDING))
+        kfree(ioctl_req);
+
+    if (ret == MLAN_STATUS_FAILURE) {
+        handle->scan_pending_on_block = MFALSE;
+        MOAL_REL_SEMAPHORE(&handle->async_sem);
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief set scan time
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param passive_scan_time    passive scan time
+ *  @param specific_scan_time   specific scan time
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+static mlan_status
+woal_set_scan_time(moal_private * priv, t_u16 passive_scan_time,
+                   t_u16 specific_scan_time)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_scan *scan = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_scan));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    scan = (mlan_ds_scan *) req->pbuf;
+    scan->sub_command = MLAN_OID_SCAN_CONFIG;
+    req->req_id = MLAN_IOCTL_SCAN;
+    req->action = MLAN_ACT_SET;
+    memset(&scan->param.scan_cfg, 0, sizeof(mlan_scan_cfg));
+    scan->param.scan_cfg.scan_time.specific_scan_time = specific_scan_time;
+    scan->param.scan_cfg.scan_time.passive_scan_time = passive_scan_time;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT))
+        ret = MLAN_STATUS_FAILURE;
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief request scan
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param scan_cfg             A pointer to wlan_user_scan_cfg structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_do_scan(moal_private * priv, wlan_user_scan_cfg * scan_cfg)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    moal_handle *handle = priv->phandle;
+
+    ENTER();
+    if (handle->scan_pending_on_block == MTRUE) {
+        PRINTM(MINFO, "scan already in processing...\n");
+        LEAVE();
+        return ret;
+    }
+#ifdef REASSOCIATION
+    if (MOAL_ACQ_SEMAPHORE_BLOCK(&handle->reassoc_sem)) {
+        PRINTM(MERROR, "Acquire semaphore error, woal_do_combo_scan\n");
+        LEAVE();
+        return -EBUSY;
+    }
+#endif /* REASSOCIATION */
+    priv->report_scan_result = MTRUE;
+    if (!scan_cfg) {
+        ret = woal_request_scan(priv, MOAL_NO_WAIT, NULL);
+    } else {
+        ret = woal_request_userscan(priv, MOAL_NO_WAIT, scan_cfg);
+    }
+#ifdef REASSOCIATION
+    MOAL_REL_SEMAPHORE(&handle->reassoc_sem);
+#endif
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief find ssid in scan_table
+ *
+ *  @param priv         A pointer to moal_private
+ *  @ssid_bssid         A pointer to mlan_ssid_bssid structure
+ *  @return             MLAN_STATUS_SUCCESS/MLAN_STATUS_FAILURE
+ */
+int
+woal_find_essid(moal_private * priv, mlan_ssid_bssid * ssid_bssid)
+{
+    int ret = 0;
+    mlan_scan_resp scan_resp;
+    struct timeval t;
+    ENTER();
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_get_scan_table(priv, MOAL_IOCTL_WAIT, &scan_resp)) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    do_gettimeofday(&t);
+/** scan result timeout value */
+#define SCAN_RESULT_AGEOUT      10
+    if (t.tv_sec > (scan_resp.age_in_secs + SCAN_RESULT_AGEOUT)) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    ret = woal_find_best_network(priv, MOAL_IOCTL_WAIT, ssid_bssid);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Request user scan
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param scan_cfg             A pointer to wlan_bgscan_cfg structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_request_bgscan(moal_private * priv,
+                    t_u8 wait_option, wlan_bgscan_cfg * scan_cfg)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_scan *scan = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    ioctl_req =
+        woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_scan) +
+                                  sizeof(wlan_bgscan_cfg));
+    if (ioctl_req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    scan = (mlan_ds_scan *) ioctl_req->pbuf;
+    scan->sub_command = MLAN_OID_SCAN_BGSCAN_CONFIG;
+    ioctl_req->req_id = MLAN_IOCTL_SCAN;
+    ioctl_req->action = MLAN_ACT_SET;
+    memcpy(scan->param.user_scan.scan_cfg_buf, scan_cfg,
+           sizeof(wlan_bgscan_cfg));
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, ioctl_req, wait_option);
+    if (status == MLAN_STATUS_FAILURE) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+  done:
+    if ((ioctl_req) && (status != MLAN_STATUS_PENDING))
+        kfree(ioctl_req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief set bgscan config
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param buf                  A pointer to scan command buf
+ *  @param length               buf length
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_set_bg_scan(moal_private * priv, char *buf, int length)
+{
+    t_u8 *ptr = buf + strlen("BGSCAN-CONFIG") + 1;
+    int buf_left = length - (strlen("BGSCAN-CONFIG") + 1);
+    int band = 0;
+    int num_ssid = 0;
+    int ssid_len = 0;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    memset(&priv->scan_cfg, 0, sizeof(priv->scan_cfg));
+    priv->scan_cfg.report_condition = BG_SCAN_SSID_MATCH;
+    while (buf_left >= 2) {
+        switch (*ptr) {
+        case WEXT_CSCAN_SSID_SECTION:
+            ssid_len = *(ptr + 1);
+            if ((buf_left < (ssid_len + 2)) ||
+                (ssid_len > MLAN_MAX_SSID_LENGTH)) {
+                PRINTM(MERROR, "Invalid ssid, buf_left=%d, ssid_len=%d\n",
+                       buf_left, ssid_len);
+                buf_left = 0;
+                break;
+            }
+            if (ssid_len && (num_ssid < (MRVDRV_MAX_SSID_LIST_LENGTH - 1))) {
+                strncpy(priv->scan_cfg.ssid_list[num_ssid].ssid, ptr + 2,
+                        ssid_len);
+                priv->scan_cfg.ssid_list[num_ssid].max_len = 0;
+                PRINTM(MIOCTL, "BG scan: ssid=%s\n",
+                       priv->scan_cfg.ssid_list[num_ssid].ssid);
+                num_ssid++;
+            }
+            buf_left -= ssid_len + 2;
+            ptr += ssid_len + 2;
+            break;
+        case WEXT_BGSCAN_RSSI_SECTION:
+            priv->scan_cfg.report_condition = BG_SCAN_SSID_RSSI_MATCH;
+            priv->scan_cfg.rssi_threshold = ptr[1];
+            PRINTM(MIOCTL, "BG scan: rssi_threshold=%d\n",
+                   (int) priv->scan_cfg.rssi_threshold);
+            ptr += 2;
+            buf_left -= 2;
+            break;
+        case WEXT_BGSCAN_REPEAT_SECTION:
+            priv->scan_cfg.repeat_count = (t_u16) ptr[1];
+            PRINTM(MIOCTL, "BG scan: repeat_count=%d\n",
+                   (int) priv->scan_cfg.repeat_count);
+            ptr += 2;
+            buf_left -= 2;
+            break;
+        case WEXT_BGSCAN_INTERVAL_SECTION:
+            priv->scan_cfg.scan_interval = (ptr[2] << 8 | ptr[1]) * 1000;
+            PRINTM(MIOCTL, "BG scan: scan_interval=%d\n",
+                   (int) priv->scan_cfg.scan_interval);
+            ptr += 3;
+            buf_left -= 3;
+            break;
+        default:
+            buf_left = 0;
+            break;
+        }
+    }
+    /** set bgscan when ssid_num > 0 */
+    if (num_ssid) {
+        if (MLAN_STATUS_SUCCESS != woal_get_band(priv, &band)) {
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+        switch (band) {
+        case WIFI_FREQUENCY_BAND_2GHZ:
+            priv->scan_cfg.chan_list[0].radio_type = 0 | BAND_SPECIFIED;
+            break;
+        case WIFI_FREQUENCY_BAND_5GHZ:
+            priv->scan_cfg.chan_list[0].radio_type = 1 | BAND_SPECIFIED;
+            break;
+        default:
+            break;
+        }
+        priv->scan_cfg.bss_type = MLAN_BSS_MODE_INFRA;
+        priv->scan_cfg.action = BG_SCAN_ACT_SET;
+        priv->scan_cfg.enable = MTRUE;
+        ret = woal_request_bgscan(priv, MOAL_IOCTL_WAIT, &priv->scan_cfg);
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief stop bg scan
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_stop_bg_scan(moal_private * priv)
+{
+    wlan_bgscan_cfg scan_cfg;
+    ENTER();
+
+    memset(&scan_cfg, 0, sizeof(scan_cfg));
+    scan_cfg.action = BG_SCAN_ACT_SET;
+    scan_cfg.enable = MFALSE;
+    return woal_request_bgscan(priv, MOAL_IOCTL_WAIT, &scan_cfg);
+
+    LEAVE();
+}
+
+/**
+ *  @brief set bgscan config
+ *
+ *  @param handle               A pointer to moal_handle structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+void
+woal_reconfig_bgscan(moal_handle * handle)
+{
+    int i;
+    for (i = 0; i < MIN(handle->priv_num, MLAN_MAX_BSS_NUM); i++) {
+        if (handle->priv[i] &&
+            (GET_BSS_ROLE(handle->priv[i]) == MLAN_BSS_ROLE_STA)) {
+            if (handle->priv[i]->bg_scan_start &&
+                handle->priv[i]->bg_scan_reported) {
+                PRINTM(MIOCTL, "Reconfig BGSCAN\n");
+                woal_request_bgscan(handle->priv[i], MOAL_NO_WAIT,
+                                    &handle->priv[i]->scan_cfg);
+                handle->priv[i]->bg_scan_reported = MFALSE;
+            }
+        }
+    }
+}
+
+/**
+ *  @brief set rssi low threshold
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param rssi 	A pointer to low rssi
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_set_rssi_low_threshold(moal_private * priv, char *rssi)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+    int low_rssi = 0;
+
+    ENTER();
+    if (priv->media_connected == MFALSE)
+        goto done;
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    misc = (mlan_ds_misc_cfg *) req->pbuf;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+    misc->sub_command = MLAN_OID_MISC_SUBSCRIBE_EVENT;
+    req->action = MLAN_ACT_SET;
+    misc->param.subscribe_event.evt_bitmap = SUBSCRIBE_EVT_RSSI_LOW;
+    misc->param.subscribe_event.evt_bitmap |= SUBSCRIBE_EVT_PRE_BEACON_LOST;
+    misc->param.subscribe_event.pre_beacon_miss = DEFAULT_PRE_BEACON_MISS;
+
+    if (MLAN_STATUS_SUCCESS != woal_atoi(&low_rssi, rssi)) {
+        ret = -EFAULT;
+        goto done;
+    }
+#ifdef STA_CFG80211
+    priv->mrvl_rssi_low = low_rssi;
+#endif
+    misc->param.subscribe_event.low_rssi = low_rssi;
+    misc->param.subscribe_event.low_rssi_freq = 0;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+#ifdef STA_CFG80211
+/**
+ *  @brief set rssi low threshold
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param event_id				event id.
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_set_rssi_threshold(moal_private * priv, t_u32 event_id)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+
+    ENTER();
+    if (priv->media_connected == MFALSE)
+        goto done;
+    if (priv->mrvl_rssi_low)
+        goto done;
+    if (event_id == MLAN_EVENT_ID_FW_BCN_RSSI_LOW) {
+        if (priv->last_rssi_low < 100)
+            priv->last_rssi_low += priv->cqm_rssi_hyst;
+        priv->last_rssi_high = abs(priv->cqm_rssi_thold);
+    } else if (event_id == MLAN_EVENT_ID_FW_BCN_RSSI_HIGH) {
+        priv->last_rssi_low = abs(priv->cqm_rssi_thold);
+        if (priv->last_rssi_high > priv->cqm_rssi_hyst)
+            priv->last_rssi_high -= priv->cqm_rssi_hyst;
+    } else {
+        priv->last_rssi_low = abs(priv->cqm_rssi_thold);
+        priv->last_rssi_high = abs(priv->cqm_rssi_thold);
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    misc = (mlan_ds_misc_cfg *) req->pbuf;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+    misc->sub_command = MLAN_OID_MISC_SUBSCRIBE_EVENT;
+    req->action = MLAN_ACT_SET;
+    misc->param.subscribe_event.evt_bitmap =
+        SUBSCRIBE_EVT_RSSI_LOW | SUBSCRIBE_EVT_RSSI_HIGH;
+    misc->param.subscribe_event.low_rssi_freq = 0;
+    misc->param.subscribe_event.low_rssi = priv->last_rssi_low;
+    misc->param.subscribe_event.high_rssi_freq = 0;
+    misc->param.subscribe_event.high_rssi = priv->last_rssi_high;
+    PRINTM(MIOCTL, "rssi_low=%d, rssi_high=%d\n", (int) priv->last_rssi_low,
+           (int) priv->last_rssi_high);
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+#endif
+
+/**
+ *  @brief  Get power mode
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param powermode            A pointer to powermode buf
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_get_powermode(moal_private * priv, int *powermode)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    int ps_mode;
+
+    ENTER();
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_power_mgmt(priv, MLAN_ACT_GET, &ps_mode, 0)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    if (ps_mode)
+        *powermode = MFALSE;
+    else
+        *powermode = MTRUE;
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief set scan type
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param scan_type            MLAN_SCAN_TYPE_ACTIVE/MLAN_SCAN_TYPE_PASSIVE
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_set_scan_type(moal_private * priv, t_u32 scan_type)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_scan *scan = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_scan));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    scan = (mlan_ds_scan *) req->pbuf;
+    scan->sub_command = MLAN_OID_SCAN_CONFIG;
+    req->req_id = MLAN_IOCTL_SCAN;
+    req->action = MLAN_ACT_SET;
+    memset(&scan->param.scan_cfg, 0, sizeof(mlan_scan_cfg));
+    scan->param.scan_cfg.scan_type = scan_type;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT))
+        ret = MLAN_STATUS_FAILURE;
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief set power mode
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param powermode            A pointer to powermode string.
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_set_powermode(moal_private * priv, char *powermode)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    int disabled;
+
+    ENTER();
+
+    if (*powermode == '1') {
+        PRINTM(MIOCTL, "Disable power save\n");
+        disabled = 1;
+    } else if (*powermode == '0') {
+        PRINTM(MIOCTL, "Enable power save\n");
+        disabled = 0;
+    } else {
+        PRINTM(MERROR, "unsupported power mode\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_power_mgmt(priv, MLAN_ACT_SET, &disabled, 0))
+        ret = MLAN_STATUS_FAILURE;
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief set combo scan
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param buf                  A pointer to scan command buf
+ *  @param length               buf length
+ *
+ *  @return                     0 -- success, otherwise fail
+ */
+int
+woal_set_combo_scan(moal_private * priv, char *buf, int length)
+{
+    int ret = 0;
+    wlan_user_scan_cfg scan_cfg;
+    t_u8 *ptr = buf + WEXT_CSCAN_HEADER_SIZE;
+    int buf_left = length - WEXT_CSCAN_HEADER_SIZE;
+    int num_ssid = 0;
+    int num_chan = 0;
+    int ssid_len = 0;
+    int i = 0;
+    t_u16 passive_scan_time = 0;
+    t_u16 specific_scan_time = 0;
+
+    ENTER();
+    memset(&scan_cfg, 0, sizeof(scan_cfg));
+    while (buf_left >= 2) {
+        switch (*ptr) {
+        case WEXT_CSCAN_SSID_SECTION:
+            ssid_len = *(ptr + 1);
+            if ((buf_left < (ssid_len + 2)) ||
+                (ssid_len > MLAN_MAX_SSID_LENGTH)) {
+                PRINTM(MERROR, "Invalid ssid, buf_left=%d, ssid_len=%d\n",
+                       buf_left, ssid_len);
+                buf_left = 0;
+                break;
+            }
+            if (ssid_len && (num_ssid < (MRVDRV_MAX_SSID_LIST_LENGTH - 1))) {
+                strncpy(scan_cfg.ssid_list[num_ssid].ssid, ptr + 2, ssid_len);
+                scan_cfg.ssid_list[num_ssid].max_len = 0;
+                PRINTM(MIOCTL, "Combo scan: ssid=%s\n",
+                       scan_cfg.ssid_list[num_ssid].ssid);
+                num_ssid++;
+            }
+            buf_left -= ssid_len + 2;
+            ptr += ssid_len + 2;
+            break;
+        case WEXT_CSCAN_CHANNEL_SECTION:
+            num_chan = ptr[1];
+            if ((buf_left < (num_chan + 2)) ||
+                (num_chan > WLAN_USER_SCAN_CHAN_MAX)) {
+                PRINTM(MERROR,
+                       "Invalid channel list, buf_left=%d, num_chan=%d\n",
+                       buf_left, num_chan);
+                buf_left = 0;
+                break;
+            }
+            for (i = 0; i < num_chan; i++) {
+                scan_cfg.chan_list[i].chan_number = ptr[2 + i];
+                PRINTM(MIOCTL, "Combo scan: chan=%d\n",
+                       scan_cfg.chan_list[i].chan_number);
+            }
+            buf_left -= 2 + num_chan;
+            ptr += 2 + num_chan;
+            break;
+        case WEXT_CSCAN_PASV_DWELL_SECTION:
+            if (buf_left < 3) {
+                PRINTM(MERROR, "Invalid PASV_DWELL_SECTION, buf_left=%d\n",
+                       buf_left);
+                buf_left = 0;
+                break;
+            }
+            passive_scan_time = ptr[2] << 8 | ptr[1];
+            ptr += 3;
+            buf_left -= 3;
+            break;
+        case WEXT_CSCAN_HOME_DWELL_SECTION:
+            if (buf_left < 3) {
+                PRINTM(MERROR, "Invalid HOME_DWELL_SECTION, buf_left=%d\n",
+                       buf_left);
+                buf_left = 0;
+                break;
+            }
+            specific_scan_time = ptr[2] << 8 | ptr[1];
+            ptr += 3;
+            buf_left -= 3;
+            break;
+        default:
+            buf_left = 0;
+            break;
+        }
+    }
+    if (passive_scan_time || specific_scan_time) {
+        PRINTM(MIOCTL, "Set passive_scan_time=%d specific_scan_time=%d\n",
+               passive_scan_time, specific_scan_time);
+        if (MLAN_STATUS_FAILURE ==
+            woal_set_scan_time(priv, passive_scan_time, specific_scan_time)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+    if (num_ssid || num_chan) {
+        if (num_ssid) {
+            /* Add broadcast scan to ssid_list */
+            scan_cfg.ssid_list[num_ssid].max_len = 0xff;
+            if (priv->scan_type == MLAN_SCAN_TYPE_PASSIVE)
+                woal_set_scan_type(priv, MLAN_SCAN_TYPE_ACTIVE);
+        }
+        if (MLAN_STATUS_FAILURE == woal_do_scan(priv, &scan_cfg))
+            ret = -EFAULT;
+        if (num_ssid && (priv->scan_type == MLAN_SCAN_TYPE_PASSIVE))
+            woal_set_scan_type(priv, MLAN_SCAN_TYPE_PASSIVE);
+    } else {
+        /* request broadcast scan */
+        if (MLAN_STATUS_FAILURE == woal_do_scan(priv, NULL))
+            ret = -EFAULT;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief  Get band
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param band                 A pointer to band buf
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_get_band(moal_private * priv, int *band)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_radio_cfg *radio_cfg = NULL;
+    int support_band = 0;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_radio_cfg));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    radio_cfg = (mlan_ds_radio_cfg *) req->pbuf;
+    radio_cfg->sub_command = MLAN_OID_BAND_CFG;
+    req->req_id = MLAN_IOCTL_RADIO_CFG;
+    /* Get config_bands, adhoc_start_band and adhoc_channel values from MLAN */
+    req->action = MLAN_ACT_GET;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    if (radio_cfg->param.band_cfg.config_bands & (BAND_B | BAND_G | BAND_GN))
+        support_band |= WIFI_FREQUENCY_BAND_2GHZ;
+    if (radio_cfg->param.band_cfg.config_bands & (BAND_A | BAND_AN))
+        support_band |= WIFI_FREQUENCY_BAND_5GHZ;
+    *band = support_band;
+    if (support_band == WIFI_FREQUENCY_ALL_BAND)
+        *band = WIFI_FREQUENCY_BAND_AUTO;
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief set band
+ *
+ *  @param priv            A pointer to moal_private structure
+ *  @param pband            A pointer to band string.
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_set_band(moal_private * priv, char *pband)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    int band = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_radio_cfg *radio_cfg = NULL;
+
+    ENTER();
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_radio_cfg));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    radio_cfg = (mlan_ds_radio_cfg *) req->pbuf;
+    radio_cfg->sub_command = MLAN_OID_BAND_CFG;
+    req->req_id = MLAN_IOCTL_RADIO_CFG;
+
+    /* Get fw supported values from MLAN */
+    req->action = MLAN_ACT_GET;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    if (*pband == '0') {
+        PRINTM(MIOCTL, "Set band to AUTO\n");
+        band = radio_cfg->param.band_cfg.fw_bands;
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+        if (IS_STA_OR_UAP_CFG80211(cfg80211_wext) && priv->wdev &&
+            priv->wdev->wiphy) {
+            if (radio_cfg->param.band_cfg.fw_bands & BAND_A)
+                priv->wdev->wiphy->bands[IEEE80211_BAND_5GHZ] =
+                    &cfg80211_band_5ghz;
+            priv->wdev->wiphy->bands[IEEE80211_BAND_2GHZ] = &cfg80211_band_2ghz;
+        }
+#endif
+    } else if (*pband == '1') {
+        PRINTM(MIOCTL, "Set band to 5G\n");
+        if (!(radio_cfg->param.band_cfg.fw_bands & BAND_A)) {
+            PRINTM(MERROR, "Don't support 5G band\n");
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+        band = BAND_A;
+        band |= BAND_AN;
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+        if (IS_STA_OR_UAP_CFG80211(cfg80211_wext) && priv->wdev &&
+            priv->wdev->wiphy) {
+            priv->wdev->wiphy->bands[IEEE80211_BAND_5GHZ] = &cfg80211_band_5ghz;
+            priv->wdev->wiphy->bands[IEEE80211_BAND_2GHZ] = NULL;
+        }
+#endif
+    } else if (*pband == '2') {
+        PRINTM(MIOCTL, "Set band to 2G\n");
+        band = BAND_B | BAND_G;
+        band |= BAND_GN;
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+        if (IS_STA_OR_UAP_CFG80211(cfg80211_wext) && priv->wdev &&
+            priv->wdev->wiphy) {
+            priv->wdev->wiphy->bands[IEEE80211_BAND_5GHZ] = NULL;
+            priv->wdev->wiphy->bands[IEEE80211_BAND_2GHZ] = &cfg80211_band_2ghz;
+        }
+#endif
+    } else {
+        PRINTM(MERROR, "unsupported band\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    /* Set config_bands to MLAN */
+    req->action = MLAN_ACT_SET;
+    memset(&radio_cfg->param.band_cfg, 0, sizeof(mlan_ds_band_cfg));
+    radio_cfg->param.band_cfg.config_bands = band;
+    radio_cfg->param.band_cfg.adhoc_start_band = band;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Add RX Filter
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param rxfilter             A pointer to rxfilter string.
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_add_rxfilter(moal_private * priv, char *rxfilter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    ENTER();
+    /* Android command: "DRIVER RXFILTER-ADD 0" "DRIVER RXFILTER-ADD 1" "DRIVER
+       RXFILTER-ADD 3" */
+    if (*rxfilter == '0') {
+        PRINTM(MIOCTL, "Add IPV4 multicast filter\n");
+        priv->rx_filter |= RX_FILTER_IPV4_MULTICAST;
+    } else if (*rxfilter == '1') {
+        PRINTM(MIOCTL, "Add broadcast filter\n");
+        priv->rx_filter |= RX_FILTER_BROADCAST;
+    } else if (*rxfilter == '2') {
+        PRINTM(MIOCTL, "Add unicast filter\n");
+        priv->rx_filter |= RX_FILTER_UNICAST;
+    } else if (*rxfilter == '3') {
+        PRINTM(MIOCTL, "Add IPV6 multicast fitler\n");
+        priv->rx_filter |= RX_FILTER_IPV6_MULTICAST;
+    } else {
+        PRINTM(MERROR, "unsupported rx fitler\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Remove RX Filter
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param rxfilter             A pointer to rxfilter string.
+ *
+ *  @return                     MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_remove_rxfilter(moal_private * priv, char *rxfilter)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    ENTER();
+    if (*rxfilter == '0') {
+        PRINTM(MIOCTL, "Remove IPV4 multicast filter\n");
+        priv->rx_filter &= ~RX_FILTER_IPV4_MULTICAST;
+    } else if (*rxfilter == '1') {
+        PRINTM(MIOCTL, "Remove broadcast filter\n");
+        priv->rx_filter &= ~RX_FILTER_BROADCAST;
+    } else if (*rxfilter == '2') {
+        PRINTM(MIOCTL, "Remove unicast filter\n");
+        priv->rx_filter &= ~RX_FILTER_UNICAST;
+    } else if (*rxfilter == '3') {
+        PRINTM(MIOCTL, "Remove IPV6 multicast fitler\n");
+        priv->rx_filter &= ~RX_FILTER_IPV6_MULTICAST;
+    } else {
+        PRINTM(MERROR, "unsupported rx fitler\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Set QoS configuration
+ *
+ * @param priv     A pointer to moal_private structure
+ * @param qos_cfg  A pointer to QoS configuration structure
+ *
+ * @return         MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+mlan_status
+woal_set_qos_cfg(moal_private * priv, char *qos_cfg)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_wmm_cfg *cfg = NULL;
+    mlan_ioctl_req *req = NULL;
+    int qosinfo = 0;
+
+    ENTER();
+    if (MLAN_STATUS_SUCCESS != woal_atoi(&qosinfo, qos_cfg)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    PRINTM(MIOCTL, "set qosinfo=%d\n", qosinfo);
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_wmm_cfg));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    cfg = (mlan_ds_wmm_cfg *) req->pbuf;
+    cfg->sub_command = MLAN_OID_WMM_CFG_QOS;
+    req->req_id = MLAN_IOCTL_WMM_CFG;
+    req->action = MLAN_ACT_SET;
+    cfg->param.qos_cfg = (t_u8) qosinfo;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT))
+        ret = MLAN_STATUS_FAILURE;
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Set sleep period
+ *
+ * @param priv     A pointer to moal_private structure
+ * @param psleeppd A pointer to sleep period configuration structure
+ *
+ * @return         MLAN_STATUS_SUCCESS -- success, otherwise fail
+ */
+int
+woal_set_sleeppd(moal_private * priv, char *psleeppd)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_pm_cfg *pm_cfg = NULL;
+    mlan_ioctl_req *req = NULL;
+    int sleeppd = 0;
+
+    ENTER();
+
+    if (MLAN_STATUS_SUCCESS != woal_atoi(&sleeppd, psleeppd)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    PRINTM(MIOCTL, "set sleeppd=%d\n", sleeppd);
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_pm_cfg));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    pm_cfg = (mlan_ds_pm_cfg *) req->pbuf;
+    pm_cfg->sub_command = MLAN_OID_PM_CFG_SLEEP_PD;
+    req->req_id = MLAN_IOCTL_PM_CFG;
+    if ((sleeppd <= MAX_SLEEP_PERIOD && sleeppd >= MIN_SLEEP_PERIOD) ||
+        (sleeppd == 0)
+        || (sleeppd == SLEEP_PERIOD_RESERVED_FF)
+        ) {
+        req->action = MLAN_ACT_SET;
+        pm_cfg->param.sleep_period = sleeppd;
+    } else {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+#endif /* STA_SUPPORT */
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_main.c b/drivers/net/wireless/sd8797/mlinux/moal_main.c
new file mode 100644
index 000000000000..d5bed42fa73d
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_main.c
@@ -0,0 +1,4036 @@
+/** @file moal_main.c
+  *
+  * @brief This file contains the major functions in WLAN
+  * driver. 
+  *
+  * Copyright (C) 2008-2012, Marvell International Ltd. 
+  * 
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+
+/********************************************************
+Change log:
+    10/21/2008: initial version
+********************************************************/
+
+#include	"moal_main.h"
+#include 	"moal_sdio.h"
+#ifdef UAP_SUPPORT
+#include    "moal_uap.h"
+#endif
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+#include    "moal_cfg80211.h"
+#endif
+#ifdef STA_CFG80211
+#ifdef STA_SUPPORT
+#include    "moal_sta_cfg80211.h"
+#endif
+#endif
+#ifdef UAP_CFG80211
+#ifdef UAP_SUPPORT
+#include    "moal_uap_cfg80211.h"
+#endif
+#endif
+#include <linux/platform_device.h>
+#include <linux/wlan_plat.h>
+#include "moal_eth_ioctl.h"
+
+#include <linux/if_ether.h>
+#include <linux/in.h>
+#include <linux/tcp.h>
+#include <net/tcp.h>
+
+/********************************************************
+		Local Variables
+********************************************************/
+
+#define KERN_VERSION    "26"
+
+/** Driver version */
+char driver_version[] =
+    "SD8797-%s-M2614" MLAN_RELEASE_VERSION "-GPL" "-(" "FP" FPNUM ")"
+#ifdef	DEBUG_LEVEL2
+    "-dbg"
+#endif
+    " ";
+
+/** Firmware name */
+char *fw_name = NULL;
+int req_fw_nowait = 0;
+
+/** MAC address */
+char *mac_addr = NULL;
+
+#ifdef MFG_CMD_SUPPORT
+/** Mfg mode */
+int mfg_mode = 0;
+#endif
+
+/** SDIO interrupt mode (0: INT_MODE_SDIO, 1: INT_MODE_GPIO) */
+int intmode = INT_MODE_SDIO;
+/** GPIO interrupt pin number */
+int gpiopin = 0;
+
+/** Auto deep sleep */
+int auto_ds = 0;
+
+/** IEEE PS mode */
+int ps_mode = 0;
+
+/** Max Tx buffer size */
+int max_tx_buf = 0;
+
+#ifdef STA_SUPPORT
+/** Max STA interfaces */
+int max_sta_bss = DEF_STA_BSS;
+#endif
+
+#ifdef UAP_SUPPORT
+/** Max uAP interfaces */
+int max_uap_bss = DEF_UAP_BSS;
+#endif
+
+#if defined(WIFI_DIRECT_SUPPORT)
+/** Max WIFIDIRECT interfaces */
+int max_wfd_bss = DEF_WIFIDIRECT_BSS;
+#endif
+
+#ifdef SDIO_SUSPEND_RESUME
+/** PM keep power */
+int pm_keep_power = 1;
+#endif
+
+#if defined(STA_SUPPORT)
+/** 802.11d configuration */
+int cfg_11d = 0;
+#endif
+
+/** CAL data config file */
+char *cal_data_cfg = NULL;
+/** Init config file (MAC address, register etc.) */
+char *init_cfg = NULL;
+
+/** Enable minicard power-up/down */
+int minicard_pwrup = 1;
+/** Pointer to struct with control hooks */
+struct wifi_platform_data *wifi_control_data = NULL;
+
+int cfg80211_wext = STA_WEXT_MASK | UAP_WEXT_MASK;
+
+/** woal_callbacks */
+static mlan_callbacks woal_callbacks = {
+    .moal_get_fw_data = moal_get_fw_data,
+    .moal_init_fw_complete = moal_init_fw_complete,
+    .moal_shutdown_fw_complete = moal_shutdown_fw_complete,
+    .moal_send_packet_complete = moal_send_packet_complete,
+    .moal_recv_packet = moal_recv_packet,
+    .moal_recv_event = moal_recv_event,
+    .moal_ioctl_complete = moal_ioctl_complete,
+    .moal_alloc_mlan_buffer = moal_alloc_mlan_buffer,
+    .moal_free_mlan_buffer = moal_free_mlan_buffer,
+    .moal_write_reg = moal_write_reg,
+    .moal_read_reg = moal_read_reg,
+    .moal_write_data_sync = moal_write_data_sync,
+    .moal_read_data_sync = moal_read_data_sync,
+    .moal_malloc = moal_malloc,
+    .moal_mfree = moal_mfree,
+    .moal_memset = moal_memset,
+    .moal_memcpy = moal_memcpy,
+    .moal_memmove = moal_memmove,
+    .moal_memcmp = moal_memcmp,
+    .moal_udelay = moal_udelay,
+    .moal_get_system_time = moal_get_system_time,
+    .moal_init_timer = moal_init_timer,
+    .moal_free_timer = moal_free_timer,
+    .moal_start_timer = moal_start_timer,
+    .moal_stop_timer = moal_stop_timer,
+    .moal_init_lock = moal_init_lock,
+    .moal_free_lock = moal_free_lock,
+    .moal_spin_lock = moal_spin_lock,
+    .moal_spin_unlock = moal_spin_unlock,
+    .moal_print = moal_print,
+    .moal_print_netintf = moal_print_netintf,
+    .moal_assert = moal_assert,
+};
+
+/** Default Driver mode */
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+#if defined(WIFI_DIRECT_SUPPORT)
+int drv_mode = (DRV_MODE_STA | DRV_MODE_UAP | DRV_MODE_WIFIDIRECT);
+#else
+int drv_mode = (DRV_MODE_STA | DRV_MODE_UAP);
+#endif
+#else
+#ifdef STA_SUPPORT
+int drv_mode = DRV_MODE_STA;
+#else
+int drv_mode = DRV_MODE_UAP;
+#endif /* STA_SUPPORT */
+#endif /* STA_SUPPORT & UAP_SUPPORT */
+
+/********************************************************
+		Global Variables
+********************************************************/
+
+/** Semaphore for add/remove card */
+struct semaphore AddRemoveCardSem;
+/**
+ * the maximum number of adapter supported
+ **/
+#define MAX_MLAN_ADAPTER    2
+/**
+ * The global variable of a pointer to moal_handle
+ * structure variable
+ **/
+moal_handle *m_handle[MAX_MLAN_ADAPTER];
+
+#ifdef DEBUG_LEVEL1
+#ifdef DEBUG_LEVEL2
+#define	DEFAULT_DEBUG_MASK	(0xffffffff)
+#else
+#define DEFAULT_DEBUG_MASK	(MMSG | MFATAL | MERROR)
+#endif /* DEBUG_LEVEL2 */
+t_u32 drvdbg = DEFAULT_DEBUG_MASK;
+#endif /* DEBUG_LEVEL1 */
+
+int woal_open(struct net_device *dev);
+int woal_close(struct net_device *dev);
+int woal_set_mac_address(struct net_device *dev, void *addr);
+void woal_tx_timeout(struct net_device *dev);
+struct net_device_stats *woal_get_stats(struct net_device *dev);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+u16 woal_select_queue(struct net_device *dev, struct sk_buff *skb);
+#endif
+
+mlan_debug_info info;
+
+/********************************************************
+		Local Functions
+********************************************************/
+
+/**
+ *  @brief This function dynamically populates the driver mode table
+ *
+ *  @param handle           A pointer to moal_handle structure
+ *  @param drv_mode_local   Driver mode
+ *
+ *  @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+woal_update_drv_tbl(moal_handle * handle, int drv_mode_local)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    unsigned int intf_num = 0;
+    int i = 0, j = 0;
+    mlan_bss_attr *bss_tbl = NULL;
+
+    ENTER();
+
+    /* Calculate number of interfaces */
+#ifdef STA_SUPPORT
+    if (drv_mode_local & DRV_MODE_STA) {
+        if ((max_sta_bss < 1) || (max_sta_bss > MAX_STA_BSS)) {
+            PRINTM(MWARN, "Unsupported max_sta_bss (%d), setting to default\n",
+                   max_sta_bss);
+            max_sta_bss = DEF_STA_BSS;
+        }
+        intf_num += max_sta_bss;
+    }
+#endif /* STA_SUPPORT */
+
+#ifdef UAP_SUPPORT
+    if (drv_mode_local & DRV_MODE_UAP) {
+        if ((max_uap_bss < 1) || (max_uap_bss > MAX_UAP_BSS)) {
+            PRINTM(MWARN, "Unsupported max_uap_bss (%d), setting to default\n",
+                   max_uap_bss);
+            max_uap_bss = DEF_UAP_BSS;
+        }
+        intf_num += max_uap_bss;
+    }
+#endif /* UAP_SUPPORT */
+
+#if defined(WIFI_DIRECT_SUPPORT)
+    if (drv_mode_local & DRV_MODE_WIFIDIRECT) {
+        if ((max_wfd_bss < 1) || (max_wfd_bss > MAX_WIFIDIRECT_BSS)) {
+            PRINTM(MWARN, "Unsupported max_wfd_bss (%d), setting to default\n",
+                   max_wfd_bss);
+            max_wfd_bss = DEF_WIFIDIRECT_BSS;
+        }
+        intf_num += max_wfd_bss;
+    }
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+
+    /* Create BSS attribute table */
+    if ((intf_num == 0) || (intf_num > MLAN_MAX_BSS_NUM)) {
+        PRINTM(MERROR, "Unsupported number of BSS %d\n", intf_num);
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    } else {
+        /* Create new table */
+        if (!
+            (bss_tbl =
+             (mlan_bss_attr *) kmalloc(sizeof(mlan_bss_attr) * intf_num,
+                                       GFP_KERNEL))) {
+            PRINTM(MERROR, "Could not create BSS attribute table\n");
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+    }
+
+    /* Populate BSS attribute table */
+#ifdef STA_SUPPORT
+    if (drv_mode_local & DRV_MODE_STA) {
+        for (j = 0; j < max_sta_bss; j++) {
+            if (i >= intf_num)
+                break;
+            bss_tbl[i].bss_type = MLAN_BSS_TYPE_STA;
+            bss_tbl[i].frame_type = MLAN_DATA_FRAME_TYPE_ETH_II;
+            bss_tbl[i].active = MTRUE;
+            bss_tbl[i].bss_priority = 0;
+            bss_tbl[i].bss_num = j;
+            i++;
+        }
+    }
+#endif /* STA_SUPPORT */
+
+#ifdef UAP_SUPPORT
+    if (drv_mode_local & DRV_MODE_UAP) {
+        for (j = 0; j < max_uap_bss; j++) {
+            if (i >= intf_num)
+                break;
+            bss_tbl[i].bss_type = MLAN_BSS_TYPE_UAP;
+            bss_tbl[i].frame_type = MLAN_DATA_FRAME_TYPE_ETH_II;
+            bss_tbl[i].active = MTRUE;
+            bss_tbl[i].bss_priority = 0;
+            bss_tbl[i].bss_num = j;
+            i++;
+        }
+    }
+#endif /* UAP_SUPPORT */
+
+#if defined(WIFI_DIRECT_SUPPORT)
+    if (drv_mode_local & DRV_MODE_WIFIDIRECT) {
+        for (j = 0; j < max_wfd_bss; j++) {
+            if (i >= intf_num)
+                break;
+            bss_tbl[i].bss_type = MLAN_BSS_TYPE_WIFIDIRECT;
+            bss_tbl[i].frame_type = MLAN_DATA_FRAME_TYPE_ETH_II;
+            bss_tbl[i].active = MTRUE;
+            bss_tbl[i].bss_priority = 0;
+            bss_tbl[i].bss_num = j;
+            i++;
+        }
+    }
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+
+    /* Clear existing table, if any */
+    if (handle->drv_mode.bss_attr != NULL) {
+        kfree(handle->drv_mode.bss_attr);
+        handle->drv_mode.bss_attr = NULL;
+    }
+
+    /* Create moal_drv_mode entry */
+    handle->drv_mode.drv_mode = drv_mode;
+    handle->drv_mode.intf_num = intf_num;
+    handle->drv_mode.bss_attr = bss_tbl;
+    if (fw_name) {
+        handle->drv_mode.fw_name = fw_name;
+    } else {
+#if defined(UAP_SUPPORT) && defined(STA_SUPPORT)
+        handle->drv_mode.fw_name = DEFAULT_AP_STA_FW_NAME;
+#else
+#ifdef UAP_SUPPORT
+        handle->drv_mode.fw_name = DEFAULT_AP_FW_NAME;
+#else
+        handle->drv_mode.fw_name = DEFAULT_FW_NAME;
+#endif /* UAP_SUPPORT */
+#endif /* UAP_SUPPORT && STA_SUPPORT */
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function initializes software
+ *
+ *  @param handle A pointer to moal_handle structure
+ *
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+woal_init_sw(moal_handle * handle)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    unsigned int i;
+    mlan_device device;
+    t_void *pmlan;
+
+    ENTER();
+
+    /* Initialize moal_handle structure */
+    handle->hardware_status = HardwareStatusInitializing;
+    handle->main_state = MOAL_STATE_IDLE;
+
+#ifdef STA_SUPPORT
+    if (MTRUE
+#ifdef STA_WEXT
+        && !IS_STA_WEXT(cfg80211_wext)
+#endif
+#ifdef STA_CFG80211
+        && !IS_STA_CFG80211(cfg80211_wext)
+#endif
+        ) {
+        PRINTM(MERROR, "STA without WEXT or CFG80211 bit definition!\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+#endif /* STA_SUPPORT */
+
+    if (woal_update_drv_tbl(handle, drv_mode) != MLAN_STATUS_SUCCESS) {
+        PRINTM(MERROR, "Could not update driver mode table\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    /* PnP and power profile */
+    handle->surprise_removed = MFALSE;
+    init_waitqueue_head(&handle->init_wait_q);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+    spin_lock_init(&handle->queue_lock);
+#endif
+
+#if defined(SDIO_SUSPEND_RESUME)
+    handle->is_suspended = MFALSE;
+    handle->hs_activated = MFALSE;
+    handle->suspend_fail = MFALSE;
+#ifdef SDIO_SUSPEND_RESUME
+    handle->suspend_notify_req = MFALSE;
+#endif
+    handle->hs_skip_count = 0;
+    handle->hs_force_count = 0;
+    handle->cmd52_func = 0;
+    handle->cmd52_reg = 0;
+    handle->cmd52_val = 0;
+    init_waitqueue_head(&handle->hs_activate_wait_q);
+#endif
+
+    /* Initialize measurement wait queue */
+    handle->meas_wait_q_woken = MFALSE;
+    handle->meas_start_jiffies = 0;
+    handle->cac_period = MFALSE;
+    handle->delay_bss_start = MFALSE;
+    init_waitqueue_head(&handle->meas_wait_q);
+#ifdef DFS_TESTING_SUPPORT
+    handle->cac_period_jiffies = 0;
+#endif
+
+#ifdef REASSOCIATION
+    MOAL_INIT_SEMAPHORE(&handle->reassoc_sem);
+    handle->reassoc_on = 0;
+
+    /* Initialize the timer for the reassociation */
+    woal_initialize_timer(&handle->reassoc_timer,
+                          woal_reassoc_timer_func, handle);
+
+    handle->is_reassoc_timer_set = MFALSE;
+#endif /* REASSOCIATION */
+
+    /* Register to MLAN */
+    memset(&device, 0, sizeof(mlan_device));
+    device.pmoal_handle = handle;
+
+#ifdef MFG_CMD_SUPPORT
+    device.mfg_mode = (t_u32) mfg_mode;
+#endif
+    device.int_mode = (t_u32) intmode;
+    device.gpio_pin = (t_u32) gpiopin;
+#ifdef  DEBUG_LEVEL1
+    device.drvdbg = drvdbg;
+#endif
+    device.auto_ds = (t_u32) auto_ds;
+    device.ps_mode = (t_u32) ps_mode;
+    device.max_tx_buf = (t_u32) max_tx_buf;
+#if defined(STA_SUPPORT)
+    device.cfg_11d = (t_u32) cfg_11d;
+#endif
+#ifdef SDIO_MULTI_PORT_TX_AGGR
+#ifdef MMC_QUIRK_BLKSZ_FOR_BYTE_MODE
+    device.mpa_tx_cfg = MLAN_INIT_PARA_ENABLED;
+#else
+    device.mpa_tx_cfg = MLAN_INIT_PARA_DISABLED;
+#endif
+#endif
+#ifdef SDIO_MULTI_PORT_RX_AGGR
+#ifdef MMC_QUIRK_BLKSZ_FOR_BYTE_MODE
+    device.mpa_rx_cfg = MLAN_INIT_PARA_ENABLED;
+#else
+    device.mpa_rx_cfg = MLAN_INIT_PARA_DISABLED;
+#endif
+#endif
+
+    for (i = 0; i < handle->drv_mode.intf_num; i++) {
+        device.bss_attr[i].bss_type = handle->drv_mode.bss_attr[i].bss_type;
+        device.bss_attr[i].frame_type = handle->drv_mode.bss_attr[i].frame_type;
+        device.bss_attr[i].active = handle->drv_mode.bss_attr[i].active;
+        device.bss_attr[i].bss_priority =
+            handle->drv_mode.bss_attr[i].bss_priority;
+        device.bss_attr[i].bss_num = handle->drv_mode.bss_attr[i].bss_num;
+    }
+    memcpy(&device.callbacks, &woal_callbacks, sizeof(mlan_callbacks));
+
+    sdio_claim_host(((struct sdio_mmc_card *) handle->card)->func);
+    if (MLAN_STATUS_SUCCESS == mlan_register(&device, &pmlan))
+        handle->pmlan_adapter = pmlan;
+    else
+        ret = MLAN_STATUS_FAILURE;
+    sdio_release_host(((struct sdio_mmc_card *) handle->card)->func);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function frees the structure of moal_handle
+ *
+ *  @param handle   A pointer to moal_handle structure
+ *
+ *  @return 	    N/A
+ */
+static void
+woal_free_moal_handle(moal_handle * handle)
+{
+    ENTER();
+    if (!handle) {
+        PRINTM(MERROR, "The handle is NULL\n");
+        LEAVE();
+        return;
+    }
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,25)
+    if ((handle->nl_sk) && ((handle->nl_sk)->sk_socket)) {
+        sock_release((handle->nl_sk)->sk_socket);
+        handle->nl_sk = NULL;
+    }
+#else
+    netlink_kernel_release(handle->nl_sk);
+#endif
+
+    if (handle->pmlan_adapter)
+        mlan_unregister(handle->pmlan_adapter);
+
+    /* Free BSS attribute table */
+    if (handle->drv_mode.bss_attr != NULL) {
+        kfree(handle->drv_mode.bss_attr);
+        handle->drv_mode.bss_attr = NULL;
+    }
+    PRINTM(MINFO, "Free Adapter\n");
+    if (handle->malloc_count || handle->lock_count || handle->mbufalloc_count) {
+        PRINTM(MERROR,
+               "mlan has memory leak: malloc_count=%u lock_count=%u mbufalloc_count=%u\n",
+               handle->malloc_count, handle->lock_count,
+               handle->mbufalloc_count);
+    }
+    /* Free the moal handle itself */
+    kfree(handle);
+    LEAVE();
+}
+
+/**
+ *    @brief WOAL get one line data from ASCII format data
+ *
+ *    @param data         Source data
+ *    @param size         Source data length
+ *    @param line_pos     Destination data
+ *    @return             routnine status
+ */
+static t_size
+parse_cfg_get_line(t_u8 * data, t_size size, t_u8 * line_pos)
+{
+    t_u8 *src, *dest;
+    static t_s32 pos = 0;
+
+    ENTER();
+
+    if (pos >= size) {          /* reach the end */
+        pos = 0;                /* Reset position for rfkill */
+        LEAVE();
+        return -1;
+    }
+    memset(line_pos, 0, MAX_LINE_LEN);
+    src = data + pos;
+    dest = line_pos;
+
+    while (*src != '\x0A' && *src != '\0') {
+        if (*src != ' ' && *src != '\t')        /* parse space */
+            *dest++ = *src++;
+        else
+            src++;
+        pos++;
+    }
+    /* parse new line */
+    pos++;
+    *dest = '\0';
+    LEAVE();
+    return strlen(line_pos);
+}
+
+/**
+ *  @brief Process register access request
+ *  @param type_string     String format Register type
+ *  @param offset_string   String format Register offset
+ *  @param value_string    String format Pointer to value
+ *  @return                MLAN_STATUS_SUCCESS--success, otherwise--fail
+ */
+static t_u32
+woal_process_regrdwr(moal_handle * handle, t_u8 * type_string,
+                     t_u8 * offset_string, t_u8 * value_string)
+{
+    mlan_status ret = MLAN_STATUS_FAILURE;
+    int type, offset, value;
+    pmlan_ioctl_req ioctl_req = NULL;
+    mlan_ds_reg_mem *reg = NULL;
+
+    ENTER();
+
+    /* Alloc ioctl_req */
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_reg_mem));
+
+    if (ioctl_req == NULL) {
+        PRINTM(MERROR, "Can't alloc memory\n");
+        goto done;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_atoi(&type, type_string)) {
+        goto done;
+    }
+    if (MLAN_STATUS_SUCCESS != woal_atoi(&offset, offset_string)) {
+        goto done;
+    }
+    if (MLAN_STATUS_SUCCESS != woal_atoi(&value, value_string)) {
+        goto done;
+    }
+
+    ioctl_req->req_id = MLAN_IOCTL_REG_MEM;
+    ioctl_req->action = MLAN_ACT_SET;
+
+    reg = (mlan_ds_reg_mem *) ioctl_req->pbuf;
+    reg->sub_command = MLAN_OID_REG_RW;
+    if (type < 5) {
+        reg->param.reg_rw.type = type;
+    } else {
+        PRINTM(MERROR, "Unsupported Type\n");
+        goto done;
+    }
+    reg->param.reg_rw.offset = offset;
+    reg->param.reg_rw.value = value;
+
+    /* request ioctl for STA */
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(handle->priv[0], ioctl_req, MOAL_IOCTL_WAIT)) {
+        goto done;
+    }
+    PRINTM(MINFO, "Register type: %d, offset: 0x%x, value: 0x%x\n", type,
+           offset, value);
+    ret = MLAN_STATUS_SUCCESS;
+
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *    @brief WOAL parse ASCII format data to MAC address
+ *
+ *    @param handle       MOAL handle
+ *    @param data         Source data
+ *    @param size         data length
+ *    @return             MLAN_STATUS_SUCCESS--success, otherwise--fail
+ */
+static t_u32
+woal_process_init_cfg(moal_handle * handle, t_u8 * data, t_size size)
+{
+    mlan_status ret = MLAN_STATUS_FAILURE;
+    t_u8 *pos;
+    t_u8 *intf_s, *intf_e;
+    t_u8 s[MAX_LINE_LEN];       /* 1 line data */
+    t_size line_len;
+    t_u8 index = 0;
+    t_u32 i;
+    t_u8 bss_mac_addr[MAX_MAC_ADDR_LEN];
+    t_u8 bss_mac_name[MAX_PARAM_LEN];
+    t_u8 type[MAX_PARAM_LEN];
+    t_u8 offset[MAX_PARAM_LEN];
+    t_u8 value[MAX_PARAM_LEN];
+
+    ENTER();
+
+    while ((line_len = parse_cfg_get_line(data, size, s)) != -1) {
+
+        pos = s;
+        while (*pos == ' ' || *pos == '\t')
+            pos++;
+
+        if (*pos == '#' || (*pos == '\r' && *(pos + 1) == '\n') ||
+            *pos == '\n' || *pos == '\0')
+            continue;           /* Needn't process this line */
+
+        /* Process MAC addr */
+        if (strncmp(pos, "mac_addr", 8) == 0) {
+            intf_s = strchr(pos, '=');
+            if (intf_s != NULL)
+                intf_e = strchr(intf_s, ':');
+            else
+                intf_e = NULL;
+            if (intf_s != NULL && intf_e != NULL) {
+                strncpy(bss_mac_addr, intf_e + 1, MAX_MAC_ADDR_LEN - 1);
+                bss_mac_addr[MAX_MAC_ADDR_LEN - 1] = '\0';
+                if ((intf_e - intf_s) > MAX_PARAM_LEN) {
+                    PRINTM(MERROR, "Too long interface name %d\n", __LINE__);
+                    goto done;
+                }
+                strncpy(bss_mac_name, intf_s + 1, intf_e - intf_s - 1);
+                bss_mac_name[intf_e - intf_s - 1] = '\0';
+                for (i = 0; i < handle->priv_num; i++) {
+                    if (strcmp(bss_mac_name, handle->priv[i]->netdev->name) ==
+                        0) {
+                        memset(handle->priv[i]->current_addr, 0, ETH_ALEN);
+                        PRINTM(MINFO, "Interface name: %s mac: %s\n",
+                               bss_mac_name, bss_mac_addr);
+                        woal_mac2u8(handle->priv[i]->current_addr,
+                                    bss_mac_addr);
+                        /* Set WLAN MAC addresses */
+                        if (MLAN_STATUS_SUCCESS !=
+                            woal_request_set_mac_address(handle->priv[i])) {
+                            PRINTM(MERROR, "Set MAC address failed\n");
+                            goto done;
+                        }
+                        memcpy(handle->priv[i]->netdev->dev_addr,
+                               handle->priv[i]->current_addr, ETH_ALEN);
+                        index++;        /* Mark found one interface matching */
+                    }
+                }
+            } else {
+                PRINTM(MERROR, "Wrong config file format %d\n", __LINE__);
+                goto done;
+            }
+        }
+        /* Process REG value */
+        else if (strncmp(pos, "wlan_reg", 8) == 0) {
+            intf_s = strchr(pos, '=');
+            if (intf_s != NULL)
+                intf_e = strchr(intf_s, ',');
+            else
+                intf_e = NULL;
+            if (intf_s != NULL && intf_e != NULL) {
+                /* Copy type */
+                strncpy(type, intf_s + 1, 1);
+                type[1] = '\0';
+            } else {
+                PRINTM(MERROR, "Wrong config file format %d\n", __LINE__);
+                goto done;
+            }
+            intf_s = intf_e + 1;
+            if (intf_s != NULL)
+                intf_e = strchr(intf_s, ',');
+            else
+                intf_e = NULL;
+            if (intf_s != NULL && intf_e != NULL) {
+                if ((intf_e - intf_s) >= MAX_PARAM_LEN) {
+                    PRINTM(MERROR, "Regsier offset is too long %d\n", __LINE__);
+                    goto done;
+                }
+                /* Copy offset */
+                strncpy(offset, intf_s, intf_e - intf_s);
+                offset[intf_e - intf_s] = '\0';
+            } else {
+                PRINTM(MERROR, "Wrong config file format %d\n", __LINE__);
+                goto done;
+            }
+            intf_s = intf_e + 1;
+            if (intf_s != NULL) {
+                if ((strlen(intf_s) >= MAX_PARAM_LEN)) {
+                    PRINTM(MERROR, "Regsier value is too long %d\n", __LINE__);
+                    goto done;
+                }
+                /* Copy value */
+                strncpy(value, intf_s, sizeof(value));
+            } else {
+                PRINTM(MERROR, "Wrong config file format %d\n", __LINE__);
+                goto done;
+            }
+
+            if (MLAN_STATUS_SUCCESS !=
+                woal_process_regrdwr(handle, type, offset, value)) {
+                PRINTM(MERROR, "Access Reg failed\n");
+                goto done;
+            }
+            PRINTM(MINFO, "Reg type: %s, offset: %s, value: %s\n", type, offset,
+                   value);
+        }
+    }
+
+    if (index == 0) {
+        PRINTM(MINFO, "Can't find any matching MAC Address");
+    }
+    ret = MLAN_STATUS_SUCCESS;
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *    @brief WOAL set user defined init data and param
+ *
+ *    @param handle       MOAL handle structure
+ *    @return             MLAN_STATUS_SUCCESS--success, otherwise--fail
+ */
+static t_u32
+woal_set_user_init_data(moal_handle * handle)
+{
+    mlan_status ret = MLAN_STATUS_FAILURE;
+    t_u8 *cfg_data = NULL;
+    t_size len;
+
+    ENTER();
+
+    if ((request_firmware(&handle->user_data, init_cfg, handle->hotplug_device))
+        < 0) {
+        PRINTM(MERROR, "Init config file request_firmware() failed\n");
+        goto done;
+    }
+
+    if (handle->user_data) {
+        cfg_data = (t_u8 *) (handle->user_data)->data;
+        len = (handle->user_data)->size;
+        if (MLAN_STATUS_SUCCESS != woal_process_init_cfg(handle, cfg_data, len)) {
+            PRINTM(MERROR, "Can't process init config file\n");
+            goto done;
+        }
+        ret = MLAN_STATUS_SUCCESS;
+    }
+
+  done:
+    if (handle->user_data)
+        release_firmware(handle->user_data);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Add interfaces DPC
+ *
+ * @param handle    A pointer to moal_handle structure
+ *
+ * @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+woal_add_card_dpc(moal_handle * handle)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    int i;
+
+    ENTER();
+
+#ifdef CONFIG_PROC_FS
+    /* Initialize proc fs */
+    woal_proc_init(handle);
+#endif /* CONFIG_PROC_FS */
+
+    /* Add interfaces */
+    for (i = 0; i < handle->drv_mode.intf_num; i++) {
+        if (!woal_add_interface
+            (handle, handle->priv_num, handle->drv_mode.bss_attr[i].bss_type)) {
+            ret = MLAN_STATUS_FAILURE;
+            goto err;
+        }
+        handle->priv_num++;
+    }
+    if (init_cfg) {
+        if (MLAN_STATUS_SUCCESS != woal_set_user_init_data(handle)) {
+            PRINTM(MFATAL, "Set user init data and param failed\n");
+            ret = MLAN_STATUS_FAILURE;
+            goto err;
+        }
+    }
+
+  err:
+    if (ret != MLAN_STATUS_SUCCESS) {
+        PRINTM(MERROR, "Failed to add interface\n");
+        for (i = 0; i < handle->priv_num; i++)
+            woal_remove_interface(handle, i);
+        handle->priv_num = 0;
+#ifdef CONFIG_PROC_FS
+        woal_proc_exit(handle);
+#endif
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Download and Initialize firmware DPC
+ *
+ * @param handle    A pointer to moal_handle structure
+ *
+ * @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+woal_init_fw_dpc(moal_handle * handle)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_fw_image fw;
+
+    mlan_init_param param;
+
+    ENTER();
+
+    if (handle->firmware) {
+        memset(&fw, 0, sizeof(mlan_fw_image));
+        fw.pfw_buf = (t_u8 *) handle->firmware->data;
+        fw.fw_len = handle->firmware->size;
+        sdio_claim_host(((struct sdio_mmc_card *) handle->card)->func);
+        ret = mlan_dnld_fw(handle->pmlan_adapter, &fw);
+        sdio_release_host(((struct sdio_mmc_card *) handle->card)->func);
+        if (ret == MLAN_STATUS_FAILURE) {
+            PRINTM(MERROR, "WLAN: Download FW with nowwait: %d\n",
+                   req_fw_nowait);
+            goto done;
+        }
+        PRINTM(MMSG, "WLAN FW is active\n");
+    }
+
+        /** Cal data request */
+    memset(&param, 0, sizeof(mlan_init_param));
+    if (cal_data_cfg) {
+        if ((request_firmware
+             (&handle->user_data, cal_data_cfg, handle->hotplug_device)) < 0) {
+            PRINTM(MERROR, "Cal data request_firmware() failed\n");
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+    }
+
+    if (handle->user_data) {
+        param.pcal_data_buf = (t_u8 *) handle->user_data->data;
+        param.cal_data_len = handle->user_data->size;
+    }
+
+    handle->hardware_status = HardwareStatusFwReady;
+    if (ret != MLAN_STATUS_SUCCESS)
+        goto done;
+
+    handle->init_wait_q_woken = MFALSE;
+
+    ret = mlan_set_init_param(handle->pmlan_adapter, &param);
+
+    sdio_claim_host(((struct sdio_mmc_card *) handle->card)->func);
+    ret = mlan_init_fw(handle->pmlan_adapter);
+    sdio_release_host(((struct sdio_mmc_card *) handle->card)->func);
+    if (ret == MLAN_STATUS_FAILURE) {
+        goto done;
+    } else if (ret == MLAN_STATUS_SUCCESS) {
+        handle->hardware_status = HardwareStatusReady;
+        goto done;
+    }
+    /* Wait for mlan_init to complete */
+    wait_event_interruptible(handle->init_wait_q, handle->init_wait_q_woken);
+    if (handle->hardware_status != HardwareStatusReady) {
+        woal_moal_debug_info(woal_get_priv(handle, MLAN_BSS_ROLE_ANY), handle,
+                             MTRUE);
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    ret = MLAN_STATUS_SUCCESS;
+  done:
+    if (handle->user_data)
+        release_firmware(handle->user_data);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Request firmware DPC
+ *
+ * @param handle    A pointer to moal_handle structure
+ * @param firmware  A pointer to firmware image
+ *
+ * @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+woal_request_fw_dpc(moal_handle * handle, const struct firmware *firmware)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    struct timeval tstamp;
+
+    ENTER();
+
+    if (!firmware) {
+        do_gettimeofday(&tstamp);
+        if (tstamp.tv_sec > (handle->req_fw_time.tv_sec + REQUEST_FW_TIMEOUT)) {
+            PRINTM(MERROR, "No firmware image found. Skipping download\n");
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+        PRINTM(MERROR, "request_firmware_nowait failed for %s. Retrying..\n",
+               handle->drv_mode.fw_name);
+        woal_sched_timeout(MOAL_TIMER_1S);
+        woal_request_fw(handle);
+        LEAVE();
+        return ret;
+    }
+    handle->firmware = firmware;
+
+    if ((ret = woal_init_fw_dpc(handle)))
+        goto done;
+    if ((ret = woal_add_card_dpc(handle)))
+        goto done;
+
+  done:
+    /* We should hold the semaphore until callback finishes execution */
+    MOAL_REL_SEMAPHORE(&AddRemoveCardSem);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Request firmware callback
+ *        This function is invoked by request_firmware_nowait system call
+ *
+ * @param firmware  A pointer to firmware image
+ * @param context   A pointer to moal_handle structure
+ *
+ * @return        N/A
+ */
+static void
+woal_request_fw_callback(const struct firmware *firmware, void *context)
+{
+    ENTER();
+    woal_request_fw_dpc((moal_handle *) context, firmware);
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,32)
+    if (firmware)
+        release_firmware(firmware);
+#endif
+    LEAVE();
+    return;
+}
+
+/**
+ * @brief   Download firmware using helper
+ *
+ * @param handle  A pointer to moal_handle structure
+ *
+ * @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_request_fw(moal_handle * handle)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    int err;
+    t_u32 revision_id = 0;
+
+    ENTER();
+
+    if (!fw_name) {
+/** Revision ID register */
+#define REV_ID_REG      0x5c
+        sdio_claim_host(((struct sdio_mmc_card *) handle->card)->func);
+        woal_read_reg(handle, REV_ID_REG, &revision_id);
+        sdio_release_host(((struct sdio_mmc_card *) handle->card)->func);
+        /* Check revision ID */
+        switch (revision_id) {
+        case SD8797_A0:
+            handle->drv_mode.fw_name = SD8797_A0_FW_NAME;
+            break;
+        case SD8797_B0:
+            handle->drv_mode.fw_name = SD8797_B0_FW_NAME;
+            break;
+        default:
+            break;
+        }
+    }
+
+    if (req_fw_nowait) {
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,32)
+        if ((err = request_firmware_nowait(THIS_MODULE, FW_ACTION_HOTPLUG,
+                                           handle->drv_mode.fw_name,
+                                           handle->hotplug_device, GFP_KERNEL,
+                                           handle,
+                                           woal_request_fw_callback)) < 0) {
+#else
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,13)
+        if ((err = request_firmware_nowait(THIS_MODULE, FW_ACTION_HOTPLUG,
+                                           handle->drv_mode.fw_name,
+                                           handle->hotplug_device, handle,
+                                           woal_request_fw_callback)) < 0) {
+#else
+        if ((err = request_firmware_nowait(THIS_MODULE,
+                                           handle->drv_mode.fw_name,
+                                           handle->hotplug_device, handle,
+                                           woal_request_fw_callback)) < 0) {
+#endif
+#endif
+            PRINTM(MFATAL,
+                   "WLAN: request_firmware_nowait() failed, error code = %d\n",
+                   err);
+            ret = MLAN_STATUS_FAILURE;
+        }
+    } else {
+        if ((err =
+             request_firmware(&handle->firmware, handle->drv_mode.fw_name,
+                              handle->hotplug_device)) < 0) {
+            PRINTM(MFATAL, "WLAN: request_firmware() failed, error code = %d\n",
+                   err);
+            ret = MLAN_STATUS_FAILURE;
+        } else {
+            ret = woal_request_fw_dpc(handle, handle->firmware);
+            release_firmware(handle->firmware);
+        }
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function initializes firmware
+ *
+ *  @param handle  A pointer to moal_handle structure
+ *
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+woal_init_fw(moal_handle * handle)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    do_gettimeofday(&handle->req_fw_time);
+    if ((ret = woal_request_fw(handle)) < 0) {
+        PRINTM(MFATAL, "woal_request_fw failed\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function will fill in the mlan_buffer
+ *
+ *  @param pmbuf   A pointer to mlan_buffer
+ *  @param skb     A pointer to struct sk_buff
+ *
+ *  @return        N/A
+ */
+static void
+woal_fill_mlan_buffer(moal_private * priv,
+                      mlan_buffer * pmbuf, struct sk_buff *skb)
+{
+    struct timeval tstamp;
+    struct ethhdr *eth;
+    t_u8 tid;
+
+    ENTER();
+
+    eth = (struct ethhdr *) skb->data;
+
+    switch (eth->h_proto) {
+
+    case __constant_htons(ETH_P_IP):
+        tid = (IPTOS_PREC(SKB_TOS(skb)) >> IPTOS_OFFSET);
+        PRINTM(MDAT_D, "packet type ETH_P_IP: %04x, tid=%#x prio=%#x\n",
+               eth->h_proto, tid, skb->priority);
+        break;
+
+    case __constant_htons(ETH_P_ARP):
+        PRINTM(MDATA, "ARP packet %04x\n", eth->h_proto);
+        tid = 0;
+        break;
+    default:
+        tid = 0;
+        break;
+    }
+
+    skb->priority = tid;
+
+    /* Record the current time the packet was queued; used to determine the
+       amount of time the packet was queued in the driver before it was sent to
+       the firmware.  The delay is then sent along with the packet to the
+       firmware for aggregate delay calculation for stats and MSDU lifetime
+       expiry. */
+    do_gettimeofday(&tstamp);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,22)
+    skb->tstamp = timeval_to_ktime(tstamp);
+#elif LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,14)
+    skb_set_timestamp(skb, &tstamp);
+#else
+    memcpy(&skb->stamp, &tstamp, sizeof(skb->stamp));
+#endif
+
+    pmbuf->pdesc = skb;
+    pmbuf->pbuf = skb->head + sizeof(mlan_buffer);
+    pmbuf->data_offset = skb->data - (skb->head + sizeof(mlan_buffer));
+    pmbuf->data_len = skb->len;
+    pmbuf->priority = skb->priority;
+    pmbuf->buf_type = 0;
+    pmbuf->in_ts_sec = (t_u32) tstamp.tv_sec;
+    pmbuf->in_ts_usec = (t_u32) tstamp.tv_usec;
+
+    LEAVE();
+    return;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,33)
+static struct device_type wlan_type = {.name = "wlan", };
+#endif
+
+#ifdef STA_SUPPORT
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+/** Network device handlers */
+const struct net_device_ops woal_netdev_ops = {
+    .ndo_open = woal_open,
+    .ndo_start_xmit = woal_hard_start_xmit,
+    .ndo_stop = woal_close,
+    .ndo_do_ioctl = woal_do_ioctl,
+    .ndo_set_mac_address = woal_set_mac_address,
+    .ndo_tx_timeout = woal_tx_timeout,
+    .ndo_get_stats = woal_get_stats,
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0)
+    .ndo_set_rx_mode = woal_set_multicast_list,
+#else
+    .ndo_set_multicast_list = woal_set_multicast_list,
+#endif
+    .ndo_select_queue = woal_select_queue,
+};
+#endif
+
+/**
+ *  @brief This function initializes the private structure
+ *  and dev structure for station mode
+ *
+ *  @param dev     A pointer to net_device structure
+ *  @param priv    A pointer to moal_private structure
+ *
+ *  @return 	   MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+woal_init_sta_dev(struct net_device *dev, moal_private * priv)
+{
+    ENTER();
+
+    /* Setup the OS Interface to our functions */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,29)
+    dev->open = woal_open;
+    dev->hard_start_xmit = woal_hard_start_xmit;
+    dev->stop = woal_close;
+    dev->do_ioctl = woal_do_ioctl;
+    dev->set_mac_address = woal_set_mac_address;
+    dev->tx_timeout = woal_tx_timeout;
+    dev->get_stats = woal_get_stats;
+    dev->set_multicast_list = woal_set_multicast_list;
+#else
+    dev->netdev_ops = &woal_netdev_ops;
+#endif
+    dev->watchdog_timeo = MRVDRV_DEFAULT_WATCHDOG_TIMEOUT;
+    dev->hard_header_len += MLAN_MIN_DATA_HEADER_LEN + sizeof(mlan_buffer);
+#ifdef  WIRELESS_EXT
+#ifdef STA_WEXT
+    if (IS_STA_WEXT(cfg80211_wext)) {
+#if WIRELESS_EXT < 21
+        dev->get_wireless_stats = woal_get_wireless_stats;
+#endif
+        dev->wireless_handlers = (struct iw_handler_def *) &woal_handler_def;
+    }
+#endif
+#endif
+    dev->flags |= IFF_BROADCAST | IFF_MULTICAST;
+
+    /* Initialize private structure */
+    init_waitqueue_head(&priv->ioctl_wait_q);
+    init_waitqueue_head(&priv->cmd_wait_q);
+    init_waitqueue_head(&priv->proc_wait_q);
+#ifdef STA_WEXT
+    if (IS_STA_WEXT(cfg80211_wext))
+        init_waitqueue_head(&priv->w_stats_wait_q);
+#endif
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+#endif /* STA_SUPPORT */
+
+#ifdef UAP_SUPPORT
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+/** Network device handlers */
+const struct net_device_ops woal_uap_netdev_ops = {
+    .ndo_open = woal_open,
+    .ndo_start_xmit = woal_hard_start_xmit,
+    .ndo_stop = woal_close,
+    .ndo_do_ioctl = woal_uap_do_ioctl,
+    .ndo_set_mac_address = woal_set_mac_address,
+    .ndo_tx_timeout = woal_tx_timeout,
+    .ndo_get_stats = woal_get_stats,
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0)
+    .ndo_set_rx_mode = woal_uap_set_multicast_list,
+#else
+    .ndo_set_multicast_list = woal_uap_set_multicast_list,
+#endif
+    .ndo_select_queue = woal_select_queue,
+};
+#endif
+
+/**
+ *  @brief This function initializes the private structure
+ *  and dev structure for uap mode
+ *
+ *  @param dev     A pointer to net_device structure
+ *  @param priv    A pointer to moal_private structure
+ *
+ *  @return 	   MLAN_STATUS_SUCCESS
+ */
+static mlan_status
+woal_init_uap_dev(struct net_device *dev, moal_private * priv)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    /* Setup the OS Interface to our functions */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,29)
+    dev->open = woal_open;
+    dev->hard_start_xmit = woal_hard_start_xmit;
+    dev->stop = woal_close;
+    dev->set_mac_address = woal_set_mac_address;
+    dev->tx_timeout = woal_tx_timeout;
+    dev->get_stats = woal_get_stats;
+    dev->do_ioctl = woal_uap_do_ioctl;
+    dev->set_multicast_list = woal_uap_set_multicast_list;
+#else
+    dev->netdev_ops = &woal_uap_netdev_ops;
+#endif
+    dev->watchdog_timeo = MRVDRV_DEFAULT_UAP_WATCHDOG_TIMEOUT;
+    dev->hard_header_len += MLAN_MIN_DATA_HEADER_LEN + sizeof(mlan_buffer);
+#ifdef UAP_WEXT
+#ifdef  WIRELESS_EXT
+    if (IS_UAP_WEXT(cfg80211_wext)) {
+#if WIRELESS_EXT < 21
+        dev->get_wireless_stats = woal_get_uap_wireless_stats;
+#endif
+        dev->wireless_handlers =
+            (struct iw_handler_def *) &woal_uap_handler_def;
+    }
+#endif /* WIRELESS_EXT */
+#endif /* UAP_WEXT */
+    dev->flags |= IFF_BROADCAST | IFF_MULTICAST;
+
+    /* Initialize private structure */
+    init_waitqueue_head(&priv->ioctl_wait_q);
+    init_waitqueue_head(&priv->cmd_wait_q);
+    init_waitqueue_head(&priv->proc_wait_q);
+#ifdef UAP_WEXT
+    if (IS_UAP_WEXT(cfg80211_wext))
+        init_waitqueue_head(&priv->w_stats_wait_q);
+#endif
+
+    LEAVE();
+    return status;
+}
+#endif /* UAP_SUPPORT */
+
+/**
+ * @brief This function adds a new interface. It will
+ * allocate, initialize and register the device.
+ *
+ *  @param handle    A pointer to moal_handle structure
+ *  @param bss_index BSS index number
+ *  @param bss_type  BSS type
+ *
+ *  @return          A pointer to the new priv structure
+ */
+moal_private *
+woal_add_interface(moal_handle * handle, t_u8 bss_index, t_u8 bss_type)
+{
+    struct net_device *dev = NULL;
+    moal_private *priv = NULL;
+
+    ENTER();
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+#define MAX_WMM_QUEUE   4
+    /* Allocate an Ethernet device */
+    if (!(dev = alloc_etherdev_mq(sizeof(moal_private), MAX_WMM_QUEUE))) {
+#else
+    if (!(dev = alloc_etherdev(sizeof(moal_private)))) {
+#endif
+        PRINTM(MFATAL, "Init virtual ethernet device failed\n");
+        goto error;
+    }
+#ifdef STA_SUPPORT
+    /* Allocate device name */
+    if ((bss_type == MLAN_BSS_TYPE_STA) && (dev_alloc_name(dev, "wlan%d") < 0)) {
+        PRINTM(MERROR, "Could not allocate mlan device name\n");
+        goto error;
+    }
+#endif
+#ifdef UAP_SUPPORT
+    if ((bss_type == MLAN_BSS_TYPE_UAP) && (dev_alloc_name(dev, "uap%d") < 0)) {
+        PRINTM(MERROR, "Could not allocate uap device name\n");
+        goto error;
+    }
+#endif
+#if defined(WIFI_DIRECT_SUPPORT)
+    if ((bss_type == MLAN_BSS_TYPE_WIFIDIRECT) &&
+        (dev_alloc_name(dev, "wfd%d") < 0)) {
+        PRINTM(MERROR, "Could not allocate wifidirect device name\n");
+        goto error;
+    }
+#endif
+    priv = (moal_private *) netdev_priv(dev);
+    /* Save the priv to handle */
+    handle->priv[bss_index] = priv;
+
+    /* Use the same handle structure */
+    priv->phandle = handle;
+    priv->netdev = dev;
+    priv->bss_index = bss_index;
+    priv->bss_type = bss_type;
+    if (bss_type == MLAN_BSS_TYPE_STA)
+        priv->bss_role = MLAN_BSS_ROLE_STA;
+    else if (bss_type == MLAN_BSS_TYPE_UAP)
+        priv->bss_role = MLAN_BSS_ROLE_UAP;
+#if defined(WIFI_DIRECT_SUPPORT)
+    else if (bss_type == MLAN_BSS_TYPE_WIFIDIRECT)
+        priv->bss_role = MLAN_BSS_ROLE_STA;
+#endif
+
+    INIT_LIST_HEAD(&priv->tcp_sess_queue);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
+    SET_MODULE_OWNER(dev);
+#endif
+#ifdef STA_SUPPORT
+    if (bss_type == MLAN_BSS_TYPE_STA
+#if defined(WIFI_DIRECT_SUPPORT)
+        || bss_type == MLAN_BSS_TYPE_WIFIDIRECT
+#endif
+        )
+        woal_init_sta_dev(dev, priv);
+#endif
+#ifdef UAP_SUPPORT
+    if (bss_type == MLAN_BSS_TYPE_UAP) {
+        if (MLAN_STATUS_SUCCESS != woal_init_uap_dev(dev, priv))
+            goto error;
+    }
+#endif
+#ifdef STA_CFG80211
+#ifdef STA_SUPPORT
+    if ((priv->bss_role == MLAN_BSS_ROLE_STA) && IS_STA_CFG80211(cfg80211_wext)) {
+        if (bss_type == MLAN_BSS_TYPE_STA
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+            || bss_type == MLAN_BSS_TYPE_WIFIDIRECT
+#endif
+#endif
+            )
+            /* Register cfg80211 for STA or Wifi direct */
+            if (woal_register_sta_cfg80211(dev, bss_type)) {
+                PRINTM(MERROR, "Cannot register STA with cfg80211\n");
+                goto error;
+            }
+    }
+#endif /* STA_SUPPORT */
+#endif /* STA_CFG80211 */
+#ifdef UAP_CFG80211
+#ifdef UAP_SUPPORT
+    if ((priv->bss_role == MLAN_BSS_ROLE_UAP) && IS_UAP_CFG80211(cfg80211_wext)) {
+        /* Register cfg80211 for UAP */
+        if (woal_register_uap_cfg80211(dev, bss_type)) {
+            PRINTM(MERROR, "Cannot register UAP with cfg80211\n");
+            goto error;
+        }
+    }
+#endif
+#endif /* UAP_CFG80211 */
+
+    /* Initialize priv structure */
+    woal_init_priv(priv, MOAL_CMD_WAIT);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+    SET_NETDEV_DEV(dev, handle->hotplug_device);
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,33)
+    SET_NETDEV_DEVTYPE(dev, &wlan_type);
+#endif
+
+    /* Register network device */
+    if (register_netdev(dev)) {
+        PRINTM(MERROR, "Cannot register virtual network device\n");
+        goto error;
+    }
+    netif_carrier_off(dev);
+    woal_stop_queue(dev);
+
+    PRINTM(MINFO, "%s: Marvell 802.11 Adapter\n", dev->name);
+
+    /* Set MAC address from the insmod command line */
+    if (handle->set_mac_addr) {
+        memset(priv->current_addr, 0, ETH_ALEN);
+        memcpy(priv->current_addr, handle->mac_addr, ETH_ALEN);
+        if (MLAN_STATUS_SUCCESS != woal_request_set_mac_address(priv)) {
+            PRINTM(MERROR, "Set MAC address failed\n");
+            goto error;
+        }
+        memcpy(dev->dev_addr, priv->current_addr, ETH_ALEN);
+    }
+#ifdef CONFIG_PROC_FS
+    woal_create_proc_entry(priv);
+#ifdef PROC_DEBUG
+    woal_debug_entry(priv);
+#endif /* PROC_DEBUG */
+#endif /* CONFIG_PROC_FS */
+
+    LEAVE();
+    return priv;
+  error:
+    if (dev && dev->reg_state == NETREG_REGISTERED)
+        unregister_netdev(dev);
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+    /* Unregister wiphy device and free */
+    if (priv->wdev && IS_STA_OR_UAP_CFG80211(cfg80211_wext)) {
+        wiphy_unregister(priv->wdev->wiphy);
+        wiphy_free(priv->wdev->wiphy);
+        /* Free wireless device */
+        kfree(priv->wdev);
+        priv->wdev = NULL;
+    }
+#endif
+    if (dev)
+        free_netdev(dev);
+    LEAVE();
+    return NULL;
+}
+
+/**
+ *  @brief This function removes an interface.
+ *
+ *  @param handle    A pointer to the moal_handle structure
+ *  @param bss_index  BSS index number
+ *
+ *  @return        N/A
+ */
+void
+woal_remove_interface(moal_handle * handle, t_u8 bss_index)
+{
+    struct net_device *dev = NULL;
+    moal_private *priv = handle->priv[bss_index];
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+    union iwreq_data wrqu;
+#endif
+
+    ENTER();
+    if (!priv)
+        goto error;
+    dev = priv->netdev;
+
+    if (priv->media_connected == MTRUE) {
+        priv->media_connected = MFALSE;
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+        if (IS_STA_OR_UAP_WEXT(cfg80211_wext) &&
+            GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) {
+            memset(wrqu.ap_addr.sa_data, 0x00, ETH_ALEN);
+            wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+            wireless_send_event(priv->netdev, SIOCGIWAP, &wrqu, NULL);
+        }
+#endif
+    }
+#ifdef CONFIG_PROC_FS
+#ifdef PROC_DEBUG
+    /* Remove proc debug */
+    woal_debug_remove(priv);
+#endif /* PROC_DEBUG */
+    woal_proc_remove(priv);
+#endif /* CONFIG_PROC_FS */
+    /* Last reference is our one */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,37)
+    PRINTM(MINFO, "refcnt = %d\n", atomic_read(&dev->refcnt));
+#else
+    PRINTM(MINFO, "refcnt = %d\n", netdev_refcnt_read(dev));
+#endif
+
+    PRINTM(MINFO, "netdev_finish_unregister: %s\n", dev->name);
+
+    if (dev->reg_state == NETREG_REGISTERED)
+        unregister_netdev(dev);
+
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+    /* Unregister wiphy device and free */
+    if (priv->wdev && IS_STA_OR_UAP_CFG80211(cfg80211_wext)) {
+        wiphy_unregister(priv->wdev->wiphy);
+        wiphy_free(priv->wdev->wiphy);
+        /* Free wireless device */
+        kfree(priv->wdev);
+        priv->wdev = NULL;
+    }
+#endif
+
+    /* Clear the priv in handle */
+    priv->phandle->priv[priv->bss_index] = NULL;
+    priv->phandle = NULL;
+    priv->netdev = NULL;
+    free_netdev(dev);
+  error:
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief Send FW shutdown command to MLAN
+ *
+ *  @param priv          A pointer to moal_private structure
+ *  @param wait_option   Wait option
+ *
+ *  @return              MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+static mlan_status
+woal_shutdown_fw(moal_private * priv, t_u8 wait_option)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+    mlan_status status;
+
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req =
+        (mlan_ioctl_req *) woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        status = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    misc = (mlan_ds_misc_cfg *) req->pbuf;
+    misc->sub_command = MLAN_OID_MISC_INIT_SHUTDOWN;
+    misc->param.func_init_shutdown = MLAN_FUNC_SHUTDOWN;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+    req->action = MLAN_ACT_SET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Return hex value of a give character
+ *
+ *  @param chr	    Character to be converted
+ *
+ *  @return 	    The converted character if chr is a valid hex, else 0
+ */
+static int
+woal_hexval(char chr)
+{
+    if (chr >= '0' && chr <= '9')
+        return chr - '0';
+    if (chr >= 'A' && chr <= 'F')
+        return chr - 'A' + 10;
+    if (chr >= 'a' && chr <= 'f')
+        return chr - 'a' + 10;
+
+    return 0;
+}
+
+#ifdef STA_WEXT
+#endif
+
+/**
+ *  @brief This function cancel all works in the queue
+ *  and destroy the main workqueue.
+ *
+ *  @param handle    A pointer to moal_handle
+ *
+ *  @return        N/A
+ */
+static void
+woal_terminate_workqueue(moal_handle * handle)
+{
+    ENTER();
+
+    /* Terminate main workqueue */
+    if (handle->workqueue) {
+        flush_workqueue(handle->workqueue);
+        destroy_workqueue(handle->workqueue);
+        handle->workqueue = NULL;
+    }
+
+    LEAVE();
+}
+
+/* Support for Cardhu wifi gpio toggling */
+/**
+ *  @brief Sets the card detect state
+ *
+ *  @param on   0: Card will be made undetected
+ *              1: Card will be detected
+ *
+ *  @return     0
+ */
+static int
+wifi_set_carddetect(int on)
+{
+    ENTER();
+    PRINTM(MMSG, "%s = %d\n", __FUNCTION__, on);
+    if (wifi_control_data && wifi_control_data->set_carddetect) {
+        wifi_control_data->set_carddetect(on);
+    }
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Sets the power state to On or Off after 'msec' millisecond
+ *
+ *  @param on       0: Card will be powered on
+ *                  1: Card will be powered off
+ *  @param msec     Delay in millisecond
+ *
+ *  @return         0
+ */
+static int
+wifi_set_power(int on, unsigned long msec)
+{
+    ENTER();
+    PRINTM(MMSG, "%s = %d\n", __FUNCTION__, on);
+    if (wifi_control_data && wifi_control_data->set_power) {
+        wifi_control_data->set_power(on);
+    }
+    if (msec)
+        mdelay(msec);
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Probes an wifi device
+ *
+ *  @param pdev     A pointer to the platform_device structure
+ *
+ *  @return         0
+ */
+static int
+wifi_probe(struct platform_device *pdev)
+{
+    struct wifi_platform_data *wifi_ctrl =
+        (struct wifi_platform_data *) (pdev->dev.platform_data);
+
+    ENTER();
+
+    wifi_control_data = wifi_ctrl;
+    wifi_set_power(1, 0);       /* Power On */
+    wifi_set_carddetect(1);     /* CardDetect (0->1) */
+
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Removes an wifi device
+ *
+ *  @param pdev     A pointer to the platform_device structure
+ *
+ *  @return         0
+ */
+static int
+wifi_remove(struct platform_device *pdev)
+{
+    struct wifi_platform_data *wifi_ctrl =
+        (struct wifi_platform_data *) (pdev->dev.platform_data);
+
+    ENTER();
+
+    wifi_control_data = wifi_ctrl;
+    wifi_set_power(0, 0);       /* Power Off */
+    wifi_set_carddetect(0);     /* CardDetect (1->0) */
+
+    LEAVE();
+    return 0;
+}
+
+static struct platform_driver wifi_device = {
+    .probe = wifi_probe,
+    .remove = wifi_remove,
+    .driver = {
+               .name = "mrvl8797_wlan",
+               }
+};
+
+/**
+ *  @brief Adds an wifi device
+ *  @return  0 --success, otherwise fail
+ */
+static int
+wifi_add_dev(void)
+{
+    int ret = 0;
+
+    ENTER();
+
+    if (minicard_pwrup)
+        ret = platform_driver_register(&wifi_device);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Removes an wifi device
+ *
+ *  @return     N/A
+ */
+static void
+wifi_del_dev(void)
+{
+    ENTER();
+
+    if (minicard_pwrup)
+        platform_driver_unregister(&wifi_device);
+
+    LEAVE();
+}
+
+/********************************************************
+		Global Functions
+********************************************************/
+
+/**
+ *  @brief This function opens the network device
+ *
+ *  @param dev     A pointer to net_device structure
+ *
+ *  @return        0 --success, otherwise fail
+ */
+int
+woal_open(struct net_device *dev)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    t_u8 carrier_on = MFALSE;
+
+    ENTER();
+
+    if (!MODULE_GET) {
+        LEAVE();
+        return -EFAULT;
+    }
+#ifdef UAP_SUPPORT
+    if ((GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP) && (priv->media_connected))
+        carrier_on = MTRUE;
+#endif
+#ifdef STA_SUPPORT
+    if ((GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) &&
+        (priv->media_connected || priv->is_adhoc_link_sensed))
+        carrier_on = MTRUE;
+#endif
+    if (carrier_on == MTRUE) {
+        if (!netif_carrier_ok(priv->netdev))
+            netif_carrier_on(priv->netdev);
+        woal_wake_queue(priv->netdev);
+    } else {
+        if (netif_carrier_ok(priv->netdev))
+            netif_carrier_off(priv->netdev);
+    }
+
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief This function closes the network device
+ *
+ *  @param dev     A pointer to net_device structure
+ *
+ *  @return        0
+ */
+int
+woal_close(struct net_device *dev)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+
+    ENTER();
+#ifdef STA_SUPPORT
+#ifdef STA_CFG80211
+    if (IS_STA_CFG80211(cfg80211_wext) && priv->scan_request) {
+        cfg80211_scan_done(priv->scan_request, MTRUE);
+        priv->scan_request = NULL;
+    }
+#endif
+#endif
+    woal_stop_queue(priv->netdev);
+
+    MODULE_PUT;
+
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief This function sets the MAC address to firmware.
+ *
+ *  @param dev     A pointer to mlan_private structure
+ *  @param addr    MAC address to set
+ *
+ *  @return        0 --success, otherwise fail
+ */
+int
+woal_set_mac_address(struct net_device *dev, void *addr)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    struct sockaddr *phw_addr = (struct sockaddr *) addr;
+    t_u8 prev_addr[ETH_ALEN];
+
+    ENTER();
+
+    memcpy(prev_addr, priv->current_addr, ETH_ALEN);
+    memset(priv->current_addr, 0, ETH_ALEN);
+    /* dev->dev_addr is 6 bytes */
+    HEXDUMP("dev->dev_addr:", dev->dev_addr, ETH_ALEN);
+
+    HEXDUMP("addr:", (t_u8 *) phw_addr->sa_data, ETH_ALEN);
+    memcpy(priv->current_addr, phw_addr->sa_data, ETH_ALEN);
+    if (MLAN_STATUS_SUCCESS != woal_request_set_mac_address(priv)) {
+        PRINTM(MERROR, "Set MAC address failed\n");
+        /* For failure restore the MAC address */
+        memcpy(priv->current_addr, prev_addr, ETH_ALEN);
+        ret = -EFAULT;
+        goto done;
+    }
+    HEXDUMP("priv->MacAddr:", priv->current_addr, ETH_ALEN);
+    memcpy(dev->dev_addr, priv->current_addr, ETH_ALEN);
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Display MLAN debug information
+ *
+ *  @param priv     A pointer to moal_private
+ *
+ *  @return 	   N/A
+ */
+void
+woal_mlan_debug_info(moal_private * priv)
+{
+    int i;
+
+    ENTER();
+
+    if (!priv || woal_get_debug_info(priv, MOAL_CMD_WAIT, &info)) {
+        PRINTM(MERROR, "Could not retrieve debug information from MLAN\n");
+        LEAVE();
+        return;
+    }
+
+    PRINTM(MERROR, "num_cmd_timeout = %d\n", info.num_cmd_timeout);
+    PRINTM(MERROR, "Timeout cmd id = 0x%x, act = 0x%x \n",
+           info.timeout_cmd_id, info.timeout_cmd_act);
+
+    PRINTM(MERROR, "last_cmd_index = %d\n", info.last_cmd_index);
+    PRINTM(MERROR, "last_cmd_id = ");
+    for (i = 0; i < DBG_CMD_NUM; i++) {
+        PRINTM(MERROR, "0x%x ", info.last_cmd_id[i]);
+    }
+    PRINTM(MERROR, "\n");
+    PRINTM(MERROR, "last_cmd_act = ");
+    for (i = 0; i < DBG_CMD_NUM; i++) {
+        PRINTM(MERROR, "0x%x ", info.last_cmd_act[i]);
+    }
+    PRINTM(MERROR, "\n");
+    PRINTM(MERROR, "last_cmd_resp_index = %d\n", info.last_cmd_resp_index);
+    PRINTM(MERROR, "last_cmd_resp_id = ");
+    for (i = 0; i < DBG_CMD_NUM; i++) {
+        PRINTM(MERROR, "0x%x ", info.last_cmd_resp_id[i]);
+    }
+    PRINTM(MERROR, "\n");
+    PRINTM(MERROR, "last_event_index = %d\n", info.last_event_index);
+    PRINTM(MERROR, "last_event = ");
+    for (i = 0; i < DBG_CMD_NUM; i++) {
+        PRINTM(MERROR, "0x%x ", info.last_event[i]);
+    }
+    PRINTM(MERROR, "\n");
+
+    PRINTM(MERROR, "num_data_h2c_failure = %d\n",
+           info.num_tx_host_to_card_failure);
+    PRINTM(MERROR, "num_cmd_h2c_failure = %d\n",
+           info.num_cmd_host_to_card_failure);
+    PRINTM(MERROR, "num_data_c2h_failure = %d\n",
+           info.num_rx_card_to_host_failure);
+    PRINTM(MERROR, "num_cmdevt_c2h_failure = %d\n",
+           info.num_cmdevt_card_to_host_failure);
+    PRINTM(MERROR, "num_int_read_failure = %d\n", info.num_int_read_failure);
+    PRINTM(MERROR, "last_int_status = %d\n", info.last_int_status);
+
+    PRINTM(MERROR, "num_event_deauth = %d\n", info.num_event_deauth);
+    PRINTM(MERROR, "num_event_disassoc = %d\n", info.num_event_disassoc);
+    PRINTM(MERROR, "num_event_link_lost = %d\n", info.num_event_link_lost);
+    PRINTM(MERROR, "num_cmd_deauth = %d\n", info.num_cmd_deauth);
+    PRINTM(MERROR, "num_cmd_assoc_success = %d\n", info.num_cmd_assoc_success);
+    PRINTM(MERROR, "num_cmd_assoc_failure = %d\n", info.num_cmd_assoc_failure);
+    PRINTM(MERROR, "cmd_resp_received = %d\n", info.cmd_resp_received);
+    PRINTM(MERROR, "event_received = %d\n", info.event_received);
+
+    PRINTM(MERROR, "max_tx_buf_size = %d\n", info.max_tx_buf_size);
+    PRINTM(MERROR, "tx_buf_size = %d\n", info.tx_buf_size);
+    PRINTM(MERROR, "curr_tx_buf_size = %d\n", info.curr_tx_buf_size);
+
+    PRINTM(MERROR, "data_sent=%d cmd_sent=%d\n", info.data_sent, info.cmd_sent);
+
+    PRINTM(MERROR, "ps_mode=%d ps_state=%d\n", info.ps_mode, info.ps_state);
+    PRINTM(MERROR, "wakeup_dev_req=%d wakeup_tries=%d\n",
+           info.pm_wakeup_card_req, info.pm_wakeup_fw_try);
+    PRINTM(MERROR, "hs_configured=%d hs_activated=%d\n",
+           info.is_hs_configured, info.hs_activated);
+    PRINTM(MERROR, "pps_uapsd_mode=%d sleep_pd=%d\n",
+           info.pps_uapsd_mode, info.sleep_pd);
+    PRINTM(MERROR, "tx_lock_flag = %d\n", info.tx_lock_flag);
+    PRINTM(MERROR, "port_open = %d\n", info.port_open);
+    PRINTM(MERROR, "scan_processing = %d\n", info.scan_processing);
+
+    PRINTM(MERROR, "mp_rd_bitmap=0x%x curr_rd_port=0x%x\n",
+           (unsigned int) info.mp_rd_bitmap, info.curr_rd_port);
+    PRINTM(MERROR, "mp_wr_bitmap=0x%x curr_wr_port=0x%x\n",
+           (unsigned int) info.mp_wr_bitmap, info.curr_wr_port);
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function handles the timeout of packet
+ *  		transmission
+ *
+ *  @param dev     A pointer to net_device structure
+ *
+ *  @return        N/A
+ */
+void
+woal_tx_timeout(struct net_device *dev)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+
+    ENTER();
+
+    priv->num_tx_timeout++;
+    PRINTM(MERROR, "%lu : Tx timeout (%d), bss_index=%d\n",
+           jiffies, priv->num_tx_timeout, priv->bss_index);
+    woal_set_trans_start(dev);
+
+    if (priv->num_tx_timeout == NUM_TX_TIMEOUT_THRESHOLD) {
+        woal_mlan_debug_info(priv);
+        woal_moal_debug_info(priv, NULL, MFALSE);
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function returns the network statistics
+ *
+ *  @param dev     A pointer to net_device structure
+ *
+ *  @return        A pointer to net_device_stats structure
+ */
+struct net_device_stats *
+woal_get_stats(struct net_device *dev)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    return &priv->stats;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+/**
+ *  @brief This function handles wmm queue select
+ *
+ *  @param dev     A pointer to net_device structure
+ *  @param skb     A pointer to sk_buff structure
+ *
+ *  @return        tx_queue index (0-3)
+ */
+u16
+woal_select_queue(struct net_device * dev, struct sk_buff * skb)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    struct ethhdr *eth = NULL;
+    struct iphdr *iph;
+    t_u8 tid = 0;
+    t_u8 index = 0;
+
+    ENTER();
+
+    eth = (struct ethhdr *) skb->data;
+    switch (eth->h_proto) {
+    case __constant_htons(ETH_P_IP):
+        iph = ip_hdr(skb);
+        tid = IPTOS_PREC(iph->tos);
+        break;
+    case __constant_htons(ETH_P_ARP):
+    default:
+        break;
+    }
+/** Offset for TOS field in the IP header */
+#define IPTOS_OFFSET 5
+    tid = (tid >> IPTOS_OFFSET);
+    index =
+        mlan_select_wmm_queue(priv->phandle->pmlan_adapter, priv->bss_index,
+                              tid);
+    PRINTM(MDATA, "select queue: tid=%d, index=%d\n", tid, index);
+    LEAVE();
+    return index;
+}
+#endif
+
+/**
+ *  @brief This function gets tcp session from the tcp session queue
+ *
+ *  @param priv      A pointer to moal_private structure
+ *  @param src_ip    IP address of the device
+ *  @param src_port  TCP port of the device
+ *  @param dst_ip    IP address of the client
+ *  @param src_port  TCP port of the client
+ *
+ *  @return          A pointer to the tcp session data structure, if found.
+ *                   Otherwise, null
+ */
+struct tcp_sess *
+woal_get_tcp_sess(moal_private * priv,
+                  t_u32 src_ip, t_u16 src_port, t_u32 dst_ip, t_u16 dst_port)
+{
+    struct tcp_sess *tcp_sess = NULL;
+    ENTER();
+
+    list_for_each_entry(tcp_sess, &priv->tcp_sess_queue, link) {
+        if ((tcp_sess->src_ip_addr == src_ip) &&
+            (tcp_sess->src_tcp_port == src_port) &&
+            (tcp_sess->dst_ip_addr == dst_ip) &&
+            (tcp_sess->dst_tcp_port == dst_port)) {
+            LEAVE();
+            return tcp_sess;
+        }
+    }
+    LEAVE();
+    return NULL;
+}
+
+/**
+ *  @brief This function checks received tcp packet for FIN
+ *          and release the tcp session if received
+ *
+ *  @param priv      A pointer to moal_private structure
+ *  @param skb       A pointer to sk_buff structure
+ *
+ *  @return          None
+ */
+void
+woal_check_tcp_fin(moal_private * priv, struct sk_buff *skb)
+{
+    struct ethhdr *ethh = NULL;
+    struct iphdr *iph = NULL;
+    struct tcphdr *tcph = NULL;
+    struct tcp_sess *tcp_sess = NULL;
+
+    ENTER();
+
+    ethh = eth_hdr(skb);
+    if (ntohs(ethh->h_proto) == ETH_P_IP) {
+        iph = (struct iphdr *) ((t_u8 *) ethh + sizeof(struct ethhdr));
+        if (iph->protocol == IPPROTO_TCP) {
+            tcph = (struct tcphdr *) ((t_u8 *) iph + iph->ihl * 4);
+            if (tcph->fin) {
+                tcp_sess = woal_get_tcp_sess(priv, iph->daddr,
+                                             tcph->dest, iph->saddr,
+                                             tcph->source);
+                if (tcp_sess != NULL) {
+                    PRINTM(MINFO,
+                           "TX: release a tcp session in dl. (src: ipaddr 0x%08x, port: %d. dst: ipaddr 0x%08x, port: %d\n",
+                           iph->daddr, tcph->dest, iph->saddr, tcph->source);
+                    /* remove the tcp session from the queue */
+                    list_del(&tcp_sess->link);
+                    kfree(tcp_sess);
+                } else {
+                    PRINTM(MINFO, "Tx: released TCP session is not found.\n ");
+                }
+            }
+        }
+    }
+    LEAVE();
+}
+
+/**
+ *  @brief This function process tcp ack packets
+ *
+ *  @param priv      A pointer to moal_private structure
+ *  @param pmbuf     A pointer to the mlan buffer associated with the skb
+ *
+ *  @return          1, if a tcp ack packet has been dropped. Otherwise, 0.
+ */
+int
+woal_process_tcp_ack(moal_private * priv, mlan_buffer * pmbuf)
+{
+    int ret = 0;
+    struct ethhdr *ethh = NULL;
+    struct iphdr *iph = NULL;
+    struct tcphdr *tcph = NULL;
+    struct tcp_sess *tcp_sess = NULL;
+    struct sk_buff *skb = NULL;
+    t_u32 ack_seq = 0;
+    t_u32 len = 0;
+    t_u8 opt = 0;
+    t_u8 opt_len = 0;
+    t_u8 *pos = NULL;
+    t_u32 win_size = 0;
+
+#define TCP_ACK_DELAY   3
+#define TCP_ACK_INIT_WARMING_UP  1000   /* initial */
+#define TCP_ACK_RECV_WARMING_UP  1000   /* recovery */
+
+    ENTER();
+
+    /** check the tcp packet */
+    ethh = (struct ethhdr *) (pmbuf->pbuf + pmbuf->data_offset);
+    if (ntohs(ethh->h_proto) != ETH_P_IP) {
+        return 0;
+    }
+    iph = (struct iphdr *) ((t_u8 *) ethh + sizeof(struct ethhdr));
+    if (iph->protocol != IPPROTO_TCP) {
+        return 0;
+    }
+    tcph = (struct tcphdr *) ((t_u8 *) iph + iph->ihl * 4);
+
+    if (tcph->syn & tcph->ack) {
+        /* respond to a TCP request. create a tcp session */
+        if (!(tcp_sess = kmalloc(sizeof(struct tcp_sess), GFP_ATOMIC))) {
+            PRINTM(MERROR, "Fail to allocate tcp_sess.\n");
+            return 0;
+        }
+        memset(tcp_sess, 0, sizeof(struct tcp_sess));
+        tcp_sess->src_ip_addr = iph->saddr;     /* my ip addr */
+        tcp_sess->dst_ip_addr = iph->daddr;
+        tcp_sess->src_tcp_port = tcph->source;
+        tcp_sess->dst_tcp_port = tcph->dest;
+        tcp_sess->start_cnt = TCP_ACK_INIT_WARMING_UP;
+
+        INIT_LIST_HEAD(&tcp_sess->link);
+        list_add_tail(&tcp_sess->link, &priv->tcp_sess_queue);
+        PRINTM(MINFO,
+               "create a tcp session. (src: ipaddr 0x%08x, port: %d. dst: ipaddr 0x%08x, port: %d\n",
+               iph->saddr, tcph->source, iph->daddr, tcph->dest);
+
+        /* parse tcp options for the window scale */
+        len = (tcph->doff * 4) - sizeof(struct tcphdr);
+        pos = (t_u8 *) (tcph + 1);
+        while (len > 0) {
+            opt = *pos++;
+            switch (opt) {
+            case TCPOPT_EOL:
+                len = 0;
+                continue;
+            case TCPOPT_NOP:
+                len--;
+                continue;
+            case TCPOPT_WINDOW:
+                opt_len = *pos++;
+                if (opt_len == TCPOLEN_WINDOW) {
+                    tcp_sess->rx_win_scale = *pos;
+                    tcp_sess->rx_win_opt = 1;
+                    if (tcp_sess->rx_win_scale > 14)
+                        tcp_sess->rx_win_scale = 14;
+                    PRINTM(MINFO, "TCP Window Scale: %d \n",
+                           tcp_sess->rx_win_scale);
+                }
+                break;
+            default:
+                opt_len = *pos++;
+            }
+            len -= opt_len;
+            pos += opt_len - 2;
+        }
+    } else if (tcph->ack && !(tcph->syn) &&
+               (ntohs(iph->tot_len) == (iph->ihl * 4 + tcph->doff * 4))) {
+        /** it is an ack packet, not a piggyback ack */
+        tcp_sess = woal_get_tcp_sess(priv, iph->saddr, tcph->source,
+                                     iph->daddr, tcph->dest);
+        if (tcp_sess == NULL) {
+            return 0;
+        }
+        /** do not drop the ack packets initially */
+        if (tcp_sess->start_cnt) {
+            tcp_sess->start_cnt--;
+            return 0;
+        }
+
+        /* check the window size. */
+        win_size = ntohs(tcph->window);
+        if (tcp_sess->rx_win_opt) {
+            win_size <<= tcp_sess->rx_win_scale;
+            /* Note: it may depend on the rtd */
+            if ((win_size > 1500 * (TCP_ACK_DELAY + 5)) &&
+                (tcp_sess->ack_cnt < TCP_ACK_DELAY)) {
+                /* if windiow is big enough, drop the ack packet */
+                if (tcp_sess->ack_seq != ntohl(tcph->ack_seq)) {
+                    ack_seq = tcp_sess->ack_seq;
+                    tcp_sess->ack_seq = ntohl(tcph->ack_seq);
+                    tcp_sess->ack_cnt++;
+                    skb = (struct sk_buff *) pmbuf->pdesc;
+                    if (skb)
+                        dev_kfree_skb_any(skb);
+                    ret = 1;
+                } else {
+                    /* the ack packet is retransmitted. thus, stop dropping
+                       the ack packets */
+                    tcp_sess->start_cnt = TCP_ACK_RECV_WARMING_UP;
+                    PRINTM(MINFO, "Recover the TCP session.\n ");
+                }
+            } else {
+                /* send the current ack pacekt */
+                ack_seq = tcp_sess->ack_seq;
+                tcp_sess->ack_seq = ntohl(tcph->ack_seq);
+                tcp_sess->ack_cnt = 0;
+                ret = 0;
+            }
+        }
+    } else if (tcph->fin) {
+        tcp_sess = woal_get_tcp_sess(priv, iph->saddr, tcph->source,
+                                     iph->daddr, tcph->dest);
+        if (tcp_sess != NULL) {
+            /* remove the tcp session from the queue */
+            PRINTM(MINFO,
+                   "Rx: release a tcp session in ul. (src: ipaddr 0x%08x, port: %d. dst: ipaddr 0x%08x, port: %d\n",
+                   iph->saddr, tcph->source, iph->daddr, tcph->dest);
+            list_del(&tcp_sess->link);
+            kfree(tcp_sess);
+        } else {
+            PRINTM(MINFO, "released TCP session is not found.\n ");
+        }
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function handles packet transmission
+ *
+ *  @param skb     A pointer to sk_buff structure
+ *  @param dev     A pointer to net_device structure
+ *
+ *  @return        0 --success
+ */
+int
+woal_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_buffer *pmbuf = NULL;
+    mlan_status status;
+    struct sk_buff *new_skb = NULL;
+
+    ENTER();
+
+    PRINTM(MDATA, "%lu BSS(%d): Data <= kernel\n", jiffies, priv->bss_index);
+
+    if (priv->phandle->surprise_removed == MTRUE) {
+        dev_kfree_skb_any(skb);
+        priv->stats.tx_dropped++;
+        goto done;
+    }
+    priv->num_tx_timeout = 0;
+    if (!skb->len || (skb->len > ETH_FRAME_LEN)) {
+        PRINTM(MERROR, "Tx Error: Bad skb length %d : %d\n",
+               skb->len, ETH_FRAME_LEN);
+        dev_kfree_skb_any(skb);
+        priv->stats.tx_dropped++;
+        goto done;
+    }
+    if (skb->cloned ||
+        (skb_headroom(skb) <
+         (MLAN_MIN_DATA_HEADER_LEN + sizeof(mlan_buffer)))) {
+        PRINTM(MWARN, "Tx: Insufficient skb headroom %d\n", skb_headroom(skb));
+        /* Insufficient skb headroom - allocate a new skb */
+        new_skb =
+            skb_realloc_headroom(skb,
+                                 MLAN_MIN_DATA_HEADER_LEN +
+                                 sizeof(mlan_buffer));
+        if (unlikely(!new_skb)) {
+            PRINTM(MERROR, "Tx: Cannot allocate skb\n");
+            dev_kfree_skb_any(skb);
+            priv->stats.tx_dropped++;
+            goto done;
+        }
+        if (new_skb != skb)
+            dev_kfree_skb_any(skb);
+        skb = new_skb;
+        PRINTM(MINFO, "new skb headroom %d\n", skb_headroom(skb));
+    }
+    pmbuf = (mlan_buffer *) skb->head;
+    memset((t_u8 *) pmbuf, 0, sizeof(mlan_buffer));
+    pmbuf->bss_index = priv->bss_index;
+    woal_fill_mlan_buffer(priv, pmbuf, skb);
+    if (priv->enable_tcp_ack_enh == MTRUE) {
+        if (woal_process_tcp_ack(priv, pmbuf)) {
+            /* the ack packet has been dropped */
+            goto done;
+        }
+    }
+
+    status = mlan_send_packet(priv->phandle->pmlan_adapter, pmbuf);
+    switch (status) {
+    case MLAN_STATUS_PENDING:
+        atomic_inc(&priv->phandle->tx_pending);
+        if (atomic_read(&priv->phandle->tx_pending) >= MAX_TX_PENDING)
+            woal_stop_queue(priv->netdev);
+        queue_work(priv->phandle->workqueue, &priv->phandle->main_work);
+        break;
+    case MLAN_STATUS_SUCCESS:
+        priv->stats.tx_packets++;
+        priv->stats.tx_bytes += skb->len;
+        dev_kfree_skb_any(skb);
+        break;
+    case MLAN_STATUS_FAILURE:
+    default:
+        priv->stats.tx_dropped++;
+        dev_kfree_skb_any(skb);
+        break;
+    }
+  done:
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Convert ascii string to Hex integer
+ *
+ *  @param d   		A pointer to integer buf
+ *  @param s   		A pointer to ascii string
+ *  @param dlen		The byte number of ascii string in hex
+ *
+ *  @return    		Number of integer
+ */
+int
+woal_ascii2hex(t_u8 * d, char *s, t_u32 dlen)
+{
+    unsigned int i;
+    t_u8 n;
+
+    ENTER();
+
+    memset(d, 0x00, dlen);
+
+    for (i = 0; i < dlen * 2; i++) {
+        if ((s[i] >= 48) && (s[i] <= 57))
+            n = s[i] - 48;
+        else if ((s[i] >= 65) && (s[i] <= 70))
+            n = s[i] - 55;
+        else if ((s[i] >= 97) && (s[i] <= 102))
+            n = s[i] - 87;
+        else
+            break;
+        if (!(i % 2))
+            n = n * 16;
+        d[i / 2] += n;
+    }
+
+    LEAVE();
+    return i;
+}
+
+/**
+ *  @brief Return integer value of a given ascii string
+ *
+ *  @param data    Converted data to be returned
+ *  @param a       String to be converted
+ *
+ *  @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_atoi(int *data, char *a)
+{
+    int i, val = 0, len;
+
+    ENTER();
+
+    len = strlen(a);
+    if (!strncmp(a, "0x", 2)) {
+        a = a + 2;
+        len -= 2;
+        *data = woal_atox(a);
+        LEAVE();
+        return MLAN_STATUS_SUCCESS;
+    }
+    for (i = 0; i < len; i++) {
+        if (isdigit(a[i])) {
+            val = val * 10 + (a[i] - '0');
+        } else {
+            PRINTM(MERROR, "Invalid char %c in string %s\n", a[i], a);
+            LEAVE();
+            return MLAN_STATUS_FAILURE;
+        }
+    }
+    *data = val;
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Return hex value of a given ascii string
+ *
+ *  @param a	    String to be converted to ascii
+ *
+ *  @return 	    The converted character if a is a valid hex, else 0
+ */
+int
+woal_atox(char *a)
+{
+    int i = 0;
+
+    ENTER();
+
+    while (isxdigit(*a))
+        i = i * 16 + woal_hexval(*a++);
+
+    LEAVE();
+    return i;
+}
+
+/**
+ *  @brief Extension of strsep lib command. This function will also take care
+ *	   escape character
+ *
+ *  @param s         A pointer to array of chars to process
+ *  @param delim     The delimiter character to end the string
+ *  @param esc       The escape character to ignore for delimiter
+ *
+ *  @return          Pointer to the separated string if delim found, else NULL
+ */
+char *
+woal_strsep(char **s, char delim, char esc)
+{
+    char *se = *s, *sb;
+
+    ENTER();
+
+    if (!(*s) || (*se == '\0')) {
+        LEAVE();
+        return NULL;
+    }
+
+    for (sb = *s; *sb != '\0'; ++sb) {
+        if (*sb == esc && *(sb + 1) == esc) {
+            /*
+             * We get a esc + esc seq then keep the one esc
+             * and chop off the other esc character
+             */
+            memmove(sb, sb + 1, strlen(sb));
+            continue;
+        }
+        if (*sb == esc && *(sb + 1) == delim) {
+            /*
+             * We get a delim + esc seq then keep the delim
+             * and chop off the esc character
+             */
+            memmove(sb, sb + 1, strlen(sb));
+            continue;
+        }
+        if (*sb == delim)
+            break;
+    }
+
+    if (*sb == '\0')
+        sb = NULL;
+    else
+        *sb++ = '\0';
+
+    *s = sb;
+
+    LEAVE();
+    return se;
+}
+
+/**
+ *  @brief Convert mac address from string to t_u8 buffer.
+ *
+ *  @param mac_addr The buffer to store the mac address in.
+ *  @param buf      The source of mac address which is a string.
+ *
+ *  @return 	    N/A
+ */
+void
+woal_mac2u8(t_u8 * mac_addr, char *buf)
+{
+    char *begin = buf, *end;
+    int i;
+
+    ENTER();
+
+    for (i = 0; i < ETH_ALEN; ++i) {
+        end = woal_strsep(&begin, ':', '/');
+        if (end)
+            mac_addr[i] = woal_atox(end);
+    }
+
+    LEAVE();
+}
+
+#ifdef STA_SUPPORT
+/**
+ *  @brief This function sets multicast addresses to firmware
+ *
+ *  @param dev     A pointer to net_device structure
+ *
+ *  @return        N/A
+ */
+void
+woal_set_multicast_list(struct net_device *dev)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    ENTER();
+    woal_request_set_multicast_list(priv, dev);
+    LEAVE();
+}
+#endif
+
+/**
+ *  @brief This function initializes the private structure
+ *  		and set default value to the member of moal_private.
+ *
+ *  @param priv             A pointer to moal_private structure
+ *  @param wait_option      Wait option
+ *
+ *  @return 	   N/A
+ */
+void
+woal_init_priv(moal_private * priv, t_u8 wait_option)
+{
+    struct list_head *link = NULL;
+    struct tcp_sess *tcp_sess = NULL;
+    ENTER();
+#ifdef STA_SUPPORT
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) {
+        priv->current_key_index = 0;
+        priv->rate_index = AUTO_RATE;
+        priv->is_adhoc_link_sensed = MFALSE;
+        priv->scan_type = MLAN_SCAN_TYPE_ACTIVE;
+        priv->bg_scan_start = MFALSE;
+        priv->bg_scan_reported = MFALSE;
+        memset(&priv->nick_name, 0, sizeof(priv->nick_name));
+        priv->num_tx_timeout = 0;
+        priv->rx_filter = 0;
+
+#ifdef REASSOCIATION
+        priv->reassoc_on = MFALSE;
+#endif
+#ifdef STA_CFG80211
+        if (IS_STA_CFG80211(cfg80211_wext)) {
+            if (priv->bss_type == MLAN_BSS_TYPE_STA
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+                || priv->bss_type == MLAN_BSS_TYPE_WIFIDIRECT
+#endif
+#endif
+                )
+                woal_cfg80211_sta_init_wiphy(priv, wait_option);
+        }
+#endif
+    }
+#endif /* STA_SUPPORT */
+#ifdef UAP_SUPPORT
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP) {
+        priv->bss_started = MFALSE;
+#ifdef UAP_CFG80211
+        if (IS_UAP_CFG80211(cfg80211_wext))
+            woal_cfg80211_uap_init_wiphy(priv, wait_option);
+#endif
+    }
+#endif
+    priv->media_connected = MFALSE;
+
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+    priv->probereq_index = MLAN_CUSTOM_IE_AUTO_IDX_MASK;
+#endif
+#ifdef STA_SUPPORT
+#endif
+
+    priv->enable_tcp_ack_enh = MTRUE;
+    while (!list_empty(&priv->tcp_sess_queue)) {
+        link = priv->tcp_sess_queue.next;
+        tcp_sess = list_entry(link, struct tcp_sess, link);
+        PRINTM(MINFO,
+               "warm reset: release a tcp session in dl. (src: ipaddr 0x%08x, port: %d. dst: ipaddr 0x%08x, port: %d\n",
+               tcp_sess->src_ip_addr, tcp_sess->src_tcp_port,
+               tcp_sess->dst_ip_addr, tcp_sess->dst_tcp_port);
+        list_del(link);
+        kfree(tcp_sess);
+    }
+
+    woal_request_get_fw_info(priv, wait_option, NULL);
+
+    LEAVE();
+}
+
+/**
+ *  @brief Reset all interfaces if all_intf flag is TRUE,
+ *  	   otherwise specified interface only
+ *
+ *  @param priv          A pointer to moal_private structure
+ *  @param wait_option   Wait option
+ *  @param all_intf      TRUE  : all interfaces
+ *                       FALSE : current interface only
+ *
+ *  @return             MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+int
+woal_reset_intf(moal_private * priv, t_u8 wait_option, int all_intf)
+{
+    int ret = MLAN_STATUS_SUCCESS;
+    int intf_num;
+    moal_handle *handle = priv->phandle;
+    mlan_bss_info bss_info;
+
+    ENTER();
+
+    /* Stop queue and detach device */
+    if (!all_intf) {
+        woal_stop_queue(priv->netdev);
+        netif_device_detach(priv->netdev);
+    } else {
+        for (intf_num = 0; intf_num < handle->priv_num; intf_num++) {
+            woal_stop_queue(handle->priv[intf_num]->netdev);
+            netif_device_detach(handle->priv[intf_num]->netdev);
+        }
+    }
+
+    /* Get BSS info */
+    memset(&bss_info, 0, sizeof(bss_info));
+    woal_get_bss_info(priv, wait_option, &bss_info);
+
+#ifdef STA_SUPPORT
+    /* If scan is in process, cancel the scan command */
+    if (handle->scan_pending_on_block == MTRUE) {
+        mlan_ioctl_req *req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_scan));
+        if (req == NULL) {
+            ret = -EFAULT;
+            goto err_cancel_scan;
+        }
+        req->req_id = MLAN_IOCTL_SCAN;
+        req->action = MLAN_ACT_SET;
+        ((mlan_ds_scan *) req->pbuf)->sub_command = MLAN_OID_SCAN_CANCEL;
+        if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, wait_option)) {
+            ret = -EFAULT;
+            goto err_cancel_scan;
+        }
+      err_cancel_scan:
+        handle->scan_pending_on_block = MFALSE;
+        MOAL_REL_SEMAPHORE(&handle->async_sem);
+        if (req)
+            kfree(req);
+    }
+#endif
+
+    /* Cancel host sleep */
+    if (bss_info.is_hs_configured) {
+        if (MLAN_STATUS_SUCCESS != woal_cancel_hs(priv, wait_option)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+    /* Disconnect from network */
+    if (!all_intf) {
+        /* Disconnect specified interface only */
+        if ((priv->media_connected == MTRUE)
+#ifdef UAP_SUPPORT
+            || (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP)
+#endif
+            ) {
+            woal_disconnect(priv, wait_option, NULL);
+            priv->media_connected = MFALSE;
+        }
+    } else {
+        /* Disconnect all interfaces */
+        for (intf_num = 0; intf_num < handle->priv_num; intf_num++) {
+            if (handle->priv[intf_num]->media_connected == MTRUE
+#ifdef UAP_SUPPORT
+                || (GET_BSS_ROLE(handle->priv[intf_num]) == MLAN_BSS_ROLE_UAP)
+#endif
+                ) {
+                woal_disconnect(handle->priv[intf_num], wait_option, NULL);
+                handle->priv[intf_num]->media_connected = MFALSE;
+            }
+        }
+    }
+
+#ifdef REASSOCIATION
+    /* Reset the reassoc timer and status */
+    if (!all_intf) {
+        handle->reassoc_on &= ~MBIT(priv->bss_index);
+        priv->reassoc_on = MFALSE;
+    } else {
+        handle->reassoc_on = 0;
+        for (intf_num = 0; intf_num < handle->priv_num; intf_num++) {
+            handle->priv[intf_num]->reassoc_on = MFALSE;
+        }
+    }
+    if (!handle->reassoc_on && handle->is_reassoc_timer_set) {
+        woal_cancel_timer(&handle->reassoc_timer);
+        handle->is_reassoc_timer_set = MFALSE;
+    }
+#endif /* REASSOCIATION */
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function return the point to structure moal_private
+ *
+ *  @param handle       Pointer to structure moal_handle
+ *  @param bss_index    BSS index number
+ *
+ *  @return             moal_private pointer or NULL
+ */
+moal_private *
+woal_bss_index_to_priv(moal_handle * handle, t_u8 bss_index)
+{
+    int i;
+
+    ENTER();
+    if (!handle) {
+        LEAVE();
+        return NULL;
+    }
+    for (i = 0; i < MLAN_MAX_BSS_NUM; i++) {
+        if (handle->priv[i] && (handle->priv[i]->bss_index == bss_index)) {
+            LEAVE();
+            return handle->priv[i];
+        }
+    }
+
+    LEAVE();
+    return NULL;
+}
+
+/**
+ *  @brief This function alloc mlan_buffer.
+ *  @param handle  A pointer to moal_handle structure
+ *  @param size	   buffer size to allocate
+ *
+ *  @return        mlan_buffer pointer or NULL
+ */
+pmlan_buffer
+woal_alloc_mlan_buffer(moal_handle * handle, int size)
+{
+    mlan_buffer *pmbuf = NULL;
+    struct sk_buff *skb;
+
+    ENTER();
+    if (!(pmbuf = kmalloc(sizeof(mlan_buffer), GFP_ATOMIC))) {
+        PRINTM(MERROR, "%s: Fail to alloc mlan buffer\n", __FUNCTION__);
+        LEAVE();
+        return NULL;
+    }
+    memset((t_u8 *) pmbuf, 0, sizeof(mlan_buffer));
+    if (!(skb = dev_alloc_skb(size))) {
+        PRINTM(MERROR, "%s: No free skb\n", __FUNCTION__);
+        kfree(pmbuf);
+        LEAVE();
+        return NULL;
+    }
+    pmbuf->pdesc = (t_void *) skb;
+    pmbuf->pbuf = (t_u8 *) skb->data;
+    handle->mbufalloc_count++;
+    LEAVE();
+    return pmbuf;
+}
+
+/**
+ *  @brief This function alloc mlan_ioctl_req.
+ *
+ *  @param size	   buffer size to allocate
+ *
+ *  @return        mlan_ioctl_req pointer or NULL
+ */
+pmlan_ioctl_req
+woal_alloc_mlan_ioctl_req(int size)
+{
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    if (!
+        (req =
+         (mlan_ioctl_req *)
+         kmalloc((sizeof(mlan_ioctl_req) + size + sizeof(int) +
+                  sizeof(wait_queue)), GFP_ATOMIC))) {
+        PRINTM(MERROR, "%s: Fail to alloc ioctl buffer\n", __FUNCTION__);
+        LEAVE();
+        return NULL;
+    }
+    memset((t_u8 *) req, 0, (sizeof(mlan_ioctl_req) + size +
+                             sizeof(int) + sizeof(wait_queue)));
+    req->pbuf = (t_u8 *) req + sizeof(mlan_ioctl_req) + sizeof(wait_queue);
+    req->buf_len = (t_u32) size;
+    req->reserved_1 = (t_ptr) ((t_u8 *) req + sizeof(mlan_ioctl_req));
+
+    LEAVE();
+    return req;
+}
+
+/**
+ *  @brief This function frees mlan_buffer.
+ *  @param handle  A pointer to moal_handle structure
+ *  @param pmbuf   Pointer to mlan_buffer
+ *
+ *  @return        N/A
+ */
+void
+woal_free_mlan_buffer(moal_handle * handle, pmlan_buffer pmbuf)
+{
+    ENTER();
+    if (!pmbuf) {
+        LEAVE();
+        return;
+    }
+    if (pmbuf->pdesc)
+        dev_kfree_skb_any((struct sk_buff *) pmbuf->pdesc);
+    kfree(pmbuf);
+    handle->mbufalloc_count--;
+    LEAVE();
+    return;
+}
+
+#ifdef STA_WEXT
+#endif
+
+/**
+ *  @brief This function handles events generated by firmware
+ *
+ *  @param priv    A pointer to moal_private structure
+ *  @param payload A pointer to payload buffer
+ *  @param len	   Length of the payload
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_broadcast_event(moal_private * priv, t_u8 * payload, t_u32 len)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    struct sk_buff *skb = NULL;
+    struct nlmsghdr *nlh = NULL;
+    moal_handle *handle = priv->phandle;
+    struct sock *sk = handle->nl_sk;
+
+    ENTER();
+    /* interface name to be prepended to event */
+    if ((len + IFNAMSIZ) > NL_MAX_PAYLOAD) {
+        PRINTM(MERROR, "event size is too big, len=%d\n", (int) len);
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    if (sk) {
+        /* Allocate skb */
+        if (!(skb = alloc_skb(NLMSG_SPACE(NL_MAX_PAYLOAD), GFP_ATOMIC))) {
+            PRINTM(MERROR, "Could not allocate skb for netlink\n");
+            ret = MLAN_STATUS_FAILURE;
+            goto done;
+        }
+        nlh = (struct nlmsghdr *) skb->data;
+        nlh->nlmsg_len = NLMSG_SPACE(len + IFNAMSIZ);
+
+        /* From kernel */
+        nlh->nlmsg_pid = 0;
+        nlh->nlmsg_flags = 0;
+
+        /* Data */
+        skb_put(skb, nlh->nlmsg_len);
+        memcpy(NLMSG_DATA(nlh), priv->netdev->name, IFNAMSIZ);
+        memcpy(((t_u8 *) (NLMSG_DATA(nlh))) + IFNAMSIZ, payload, len);
+
+        /* From Kernel */
+        NETLINK_CB(skb).pid = 0;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+        /* Multicast message */
+        NETLINK_CB(skb).dst_pid = 0;
+#endif
+
+        /* Multicast group number */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,14)
+        NETLINK_CB(skb).dst_groups = NL_MULTICAST_GROUP;
+#else
+        NETLINK_CB(skb).dst_group = NL_MULTICAST_GROUP;
+#endif
+
+        /* Send message */
+        netlink_broadcast(sk, skb, 0, NL_MULTICAST_GROUP, GFP_ATOMIC);
+
+        ret = MLAN_STATUS_SUCCESS;
+    } else {
+        PRINTM(MERROR, "Could not send event through NETLINK. Link down.\n");
+        ret = MLAN_STATUS_FAILURE;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+#ifdef REASSOCIATION
+/**
+ *  @brief This function handles re-association. it is triggered
+ *  by re-assoc timer.
+ *
+ *  @param data    A pointer to wlan_thread structure
+ *  @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+int
+woal_reassociation_thread(void *data)
+{
+    moal_thread *pmoal_thread = data;
+    moal_private *priv = NULL;
+    moal_handle *handle = (moal_handle *) pmoal_thread->handle;
+    wait_queue_t wait;
+    int i;
+    BOOLEAN reassoc_timer_req;
+    mlan_802_11_ssid req_ssid;
+    mlan_ssid_bssid ssid_bssid;
+    mlan_status status;
+    mlan_bss_info bss_info;
+    t_u32 timer_val = MOAL_TIMER_10S;
+
+    ENTER();
+
+    woal_activate_thread(pmoal_thread);
+    init_waitqueue_entry(&wait, current);
+
+    current->flags |= PF_NOFREEZE;
+
+    for (;;) {
+        add_wait_queue(&pmoal_thread->wait_q, &wait);
+        set_current_state(TASK_INTERRUPTIBLE);
+
+        schedule();
+
+        set_current_state(TASK_RUNNING);
+        remove_wait_queue(&pmoal_thread->wait_q, &wait);
+
+        /* Cancel re-association timer */
+        if (handle->is_reassoc_timer_set == MTRUE) {
+            woal_cancel_timer(&handle->reassoc_timer);
+            handle->is_reassoc_timer_set = MFALSE;
+        }
+
+        if (handle->surprise_removed)
+            break;
+        if (kthread_should_stop())
+            break;
+
+        if (handle->hardware_status != HardwareStatusReady) {
+            PRINTM(MINFO, "Reassoc: Hardware status is not correct\n");
+            continue;
+        }
+
+        PRINTM(MEVENT, "Reassoc: Thread waking up...\n");
+        reassoc_timer_req = MFALSE;
+
+        for (i = 0; i < handle->priv_num && (priv = handle->priv[i]); i++) {
+
+            if (priv->reassoc_required == MFALSE) {
+                continue;
+            }
+
+            memset(&bss_info, 0x00, sizeof(bss_info));
+
+            if (MLAN_STATUS_SUCCESS != woal_get_bss_info(priv,
+                                                         MOAL_CMD_WAIT,
+                                                         &bss_info)) {
+                PRINTM(MINFO, "Ressoc: Fail to get bss info\n");
+                priv->reassoc_required = MFALSE;
+                continue;
+            }
+
+            if (bss_info.bss_mode != MLAN_BSS_MODE_INFRA ||
+                priv->media_connected != MFALSE) {
+                PRINTM(MINFO, "Reassoc: ad-hoc mode or media connected\n");
+                priv->reassoc_required = MFALSE;
+                continue;
+            }
+
+            /* The semaphore is used to avoid reassociation thread and
+               wlan_set_scan/wlan_set_essid interrupting each other.
+               Reassociation should be disabled completely by application if
+               wlan_set_user_scan_ioctl/wlan_set_wap is used. */
+            if (MOAL_ACQ_SEMAPHORE_BLOCK(&handle->reassoc_sem)) {
+                PRINTM(MERROR,
+                       "Acquire semaphore error, reassociation thread\n");
+                reassoc_timer_req = MTRUE;
+                break;
+            }
+
+            PRINTM(MINFO, "Reassoc: Required ESSID: %s\n",
+                   priv->prev_ssid_bssid.ssid.ssid);
+            PRINTM(MINFO, "Reassoc: Performing Active Scan\n");
+
+            memset(&req_ssid, 0x00, sizeof(mlan_802_11_ssid));
+            memcpy(&req_ssid,
+                   &priv->prev_ssid_bssid.ssid, sizeof(mlan_802_11_ssid));
+
+            /* Do specific SSID scanning */
+            if (MLAN_STATUS_SUCCESS !=
+                woal_request_scan(priv, MOAL_CMD_WAIT, &req_ssid)) {
+                PRINTM(MERROR, "Reassoc: Fail to do specific scan\n");
+                reassoc_timer_req = MTRUE;
+                MOAL_REL_SEMAPHORE(&handle->reassoc_sem);
+                break;
+            }
+
+            if (handle->surprise_removed) {
+                MOAL_REL_SEMAPHORE(&handle->reassoc_sem);
+                break;
+            }
+
+            /* Search AP by BSSID first */
+            PRINTM(MINFO, "Reassoc: Search AP by BSSID first\n");
+            memset(&ssid_bssid, 0, sizeof(mlan_ssid_bssid));
+            memcpy(&ssid_bssid.bssid,
+                   &priv->prev_ssid_bssid.bssid, MLAN_MAC_ADDR_LENGTH);
+            status = woal_find_best_network(priv, MOAL_CMD_WAIT, &ssid_bssid);
+
+            if (MLAN_STATUS_SUCCESS != status) {
+                PRINTM(MINFO, "Reassoc: AP not found in scan list\n");
+                PRINTM(MINFO, "Reassoc: Search AP by SSID\n");
+                /* Search AP by SSID */
+                memset(&ssid_bssid, 0, sizeof(mlan_ssid_bssid));
+                memcpy(&ssid_bssid.ssid,
+                       &priv->prev_ssid_bssid.ssid, sizeof(mlan_802_11_ssid));
+                status = woal_find_best_network(priv,
+                                                MOAL_CMD_WAIT, &ssid_bssid);
+            }
+
+            if (status == MLAN_STATUS_SUCCESS) {
+                /* set the wep key */
+                if (bss_info.wep_status)
+                    woal_enable_wep_key(priv, MOAL_CMD_WAIT);
+                /* Zero SSID implies use BSSID to connect */
+                memset(&ssid_bssid.ssid, 0, sizeof(mlan_802_11_ssid));
+                status = woal_bss_start(priv, MOAL_CMD_WAIT, &ssid_bssid);
+            }
+
+            if (priv->media_connected == MFALSE)
+                reassoc_timer_req = MTRUE;
+            else {
+                mlan_ds_rate *rate = NULL;
+                mlan_ioctl_req *req = NULL;
+
+                reassoc_timer_req = MFALSE;
+                if (priv->rate_index != AUTO_RATE) {
+                    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_rate));
+
+                    if (req == NULL) {
+                        LEAVE();
+                        return MLAN_STATUS_FAILURE;
+                    }
+
+                    rate = (mlan_ds_rate *) req->pbuf;
+                    rate->param.rate_cfg.rate_type = MLAN_RATE_INDEX;
+                    rate->sub_command = MLAN_OID_RATE_CFG;
+                    req->req_id = MLAN_IOCTL_RATE;
+
+                    req->action = MLAN_ACT_SET;
+
+                    rate->param.rate_cfg.rate = priv->rate_index;
+
+                    if (MLAN_STATUS_SUCCESS
+                        != woal_request_ioctl(priv, req, MOAL_CMD_WAIT)) {
+                        kfree(req);
+                        LEAVE();
+                        return MLAN_STATUS_FAILURE;
+                    }
+                    if (req)
+                        kfree(req);
+                }
+            }
+
+            MOAL_REL_SEMAPHORE(&handle->reassoc_sem);
+        }
+
+        if (handle->surprise_removed)
+            break;
+
+        if (reassoc_timer_req == MTRUE) {
+            handle->is_reassoc_timer_set = MTRUE;
+            {
+                PRINTM(MEVENT,
+                       "Reassoc: No AP found or assoc failed. "
+                       "Restarting re-assoc Timer: %d\n", (int) timer_val);
+                woal_mod_timer(&handle->reassoc_timer, timer_val);
+            }
+        }
+    }
+    woal_deactivate_thread(pmoal_thread);
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function triggers re-association by waking up
+ *  re-assoc thread.
+ *
+ *  @param context	A pointer to context
+ *  @return		N/A
+ */
+void
+woal_reassoc_timer_func(void *context)
+{
+    moal_handle *handle = (moal_handle *) context;
+
+    ENTER();
+
+    PRINTM(MINFO, "reassoc_timer fired.\n");
+    handle->is_reassoc_timer_set = MFALSE;
+
+    PRINTM(MINFO, "Waking Up the Reassoc Thread\n");
+    wake_up_interruptible(&handle->reassoc_thread.wait_q);
+
+    LEAVE();
+    return;
+}
+#endif /* REASSOCIATION */
+
+#ifdef STA_SUPPORT
+/**
+ *  @brief Sends disconnect event
+ *
+ *  @param priv A pointer to moal_private struct
+ *  @return		N/A
+ */
+t_void
+woal_send_disconnect_to_system(moal_private * priv)
+{
+#ifdef STA_WEXT
+    union iwreq_data wrqu;
+#endif
+
+    ENTER();
+    priv->media_connected = MFALSE;
+    woal_stop_queue(priv->netdev);
+    if (netif_carrier_ok(priv->netdev))
+        netif_carrier_off(priv->netdev);
+#ifdef STA_WEXT
+    if (IS_STA_WEXT(cfg80211_wext)) {
+        memset(wrqu.ap_addr.sa_data, 0x00, ETH_ALEN);
+        wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+        wireless_send_event(priv->netdev, SIOCGIWAP, &wrqu, NULL);
+    }
+#endif
+    LEAVE();
+}
+#endif /* STA_SUPPORT */
+
+/**
+ *  @brief This function reads and displays SDIO registers for debugging
+ *
+ *  @param phandle  A pointer to moal_handle
+ *
+ *  @return         N/A
+ */
+void
+woal_sdio_reg_dbg(moal_handle * phandle)
+{
+    int ret = 0;
+    t_u8 loop, index = 0, func, data;
+    unsigned int reg, reg_start, reg_end;
+    unsigned int reg_table[] = { 0x28, 0x30, 0x34, 0x38, 0x3c };
+    char buf[128], *ptr;
+
+    sdio_claim_host(((struct sdio_mmc_card *) phandle->card)->func);
+    for (loop = 0; loop < 5; loop++) {
+        memset(buf, 0, sizeof(buf));
+        ptr = buf;
+        if (loop < 2) {
+            /* Read the registers of SDIO function0 and function1 */
+            func = loop;
+            reg_start = 0;
+            reg_end = 9;
+        } else if (loop == 2) {
+            /* Read specific registers of SDIO function1 */
+            index = 0;
+            func = 1;
+            reg_start = reg_table[index++];
+            reg_end = reg_table[sizeof(reg_table) / sizeof(int) - 1];
+        } else {
+            /* Read the scratch registers of SDIO function1 */
+            if (loop == 4)
+                mdelay(100);
+            func = 1;
+#define SDIO_SCRATCH_REG 0x60
+            reg_start = SDIO_SCRATCH_REG;
+            reg_end = SDIO_SCRATCH_REG + 10;
+        }
+        if (loop != 2)
+            ptr +=
+                sprintf(ptr, "SDIO Func%d (%#x-%#x): ", func, reg_start,
+                        reg_end);
+        else
+            ptr += sprintf(ptr, "SDIO Func%d: ", func);
+        for (reg = reg_start; reg <= reg_end;) {
+            if (func == 0)
+                data =
+                    sdio_f0_readb(((struct sdio_mmc_card *) phandle->card)->
+                                  func, reg, &ret);
+            else
+                data =
+                    sdio_readb(((struct sdio_mmc_card *) phandle->card)->func,
+                               reg, &ret);
+            if (loop == 2)
+                ptr += sprintf(ptr, "(%#x) ", reg);
+            if (!ret)
+                ptr += sprintf(ptr, "%02x ", data);
+            else {
+                ptr += sprintf(ptr, "ERR");
+                break;
+            }
+            if (loop == 2 && reg < reg_end)
+                reg = reg_table[index++];
+            else
+                reg++;
+        }
+        PRINTM(MMSG, "%s\n", buf);
+    }
+    sdio_release_host(((struct sdio_mmc_card *) phandle->card)->func);
+}
+
+/**
+ *  @brief This function displays extra MOAL debug information
+ *
+ *  @param priv     A pointer to moal_private
+ *  @param handle   A pointer to moal_handle
+ *  @param flag     Indicates whether register read can be done directly
+ *
+ *  @return         N/A
+ */
+void
+woal_moal_debug_info(moal_private * priv, moal_handle * handle, u8 flag)
+{
+    moal_handle *phandle = NULL;
+    char buf[MLAN_MAX_VER_STR_LEN];
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+    int i = 0;
+#endif
+
+    ENTER();
+
+    if (!priv) {
+        if (handle) {
+            phandle = handle;
+        } else {
+            PRINTM(MERROR, "Could not retrieve debug information from MOAL\n");
+            LEAVE();
+            return;
+        }
+    } else {
+        phandle = priv->phandle;
+    }
+
+    woal_get_version(phandle, buf, sizeof(buf) - 1);
+    PRINTM(MERROR, "Driver version = %s\n", buf);
+    PRINTM(MERROR, "main_state = %d\n", phandle->main_state);
+    PRINTM(MERROR, "ioctl_pending = %d\n",
+           atomic_read(&phandle->ioctl_pending));
+    PRINTM(MERROR, "tx_pending = %d\n", atomic_read(&phandle->tx_pending));
+    PRINTM(MERROR, "rx_pending = %d\n", atomic_read(&phandle->rx_pending));
+    PRINTM(MERROR, "malloc_count = %u\n", phandle->malloc_count);
+    PRINTM(MERROR, "lock_count = %u\n", phandle->lock_count);
+    PRINTM(MERROR, "mbufalloc_count = %u\n", phandle->mbufalloc_count);
+#if defined(SDIO_SUSPEND_RESUME)
+    PRINTM(MERROR, "hs_skip_count = %u\n", phandle->hs_skip_count);
+    PRINTM(MERROR, "hs_force_count = %u\n", phandle->hs_force_count);
+#endif
+
+    if (priv) {
+        PRINTM(MERROR, "Media state = \"%s\"\n",
+               ((priv->media_connected ==
+                 MFALSE) ? "Disconnected" : "Connected"));
+        PRINTM(MERROR, "carrier %s\n",
+               ((netif_carrier_ok(priv->netdev)) ? "on" : "off"));
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+        for (i = 0; i < (priv->netdev->num_tx_queues); i++) {
+            PRINTM(MERROR, "tx queue %d: %s\n", i,
+                   ((netif_tx_queue_stopped
+                     (netdev_get_tx_queue(priv->netdev, 0))) ? "stopped" :
+                    "started"));
+        }
+#else
+        PRINTM(MERROR, "tx queue %s\n",
+               ((netif_queue_stopped(priv->netdev)) ? "stopped" : "started"));
+#endif
+    }
+
+    /* Display SDIO registers */
+    if (flag && phandle->main_state == MOAL_END_MAIN_PROCESS) {
+        woal_sdio_reg_dbg(phandle);
+    } else {
+        phandle->sdio_reg_dbg = MTRUE;
+        queue_work(phandle->workqueue, &phandle->main_work);
+    }
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief This workqueue function handles main_process
+ *
+ *  @param work    A pointer to work_struct
+ *
+ *  @return        N/A
+ */
+t_void
+woal_main_work_queue(struct work_struct * work)
+{
+    moal_handle *handle = container_of(work, moal_handle, main_work);
+
+    ENTER();
+
+    if (handle->surprise_removed == MTRUE) {
+        LEAVE();
+        return;
+    }
+
+    if (handle->sdio_reg_dbg == MTRUE) {
+        handle->sdio_reg_dbg = MFALSE;
+        woal_sdio_reg_dbg(handle);
+        LEAVE();
+        return;
+    }
+
+    handle->main_state = MOAL_ENTER_WORK_QUEUE;
+    sdio_claim_host(((struct sdio_mmc_card *) handle->card)->func);
+    handle->main_state = MOAL_START_MAIN_PROCESS;
+    /* Call MLAN main process */
+    mlan_main_process(handle->pmlan_adapter);
+    handle->main_state = MOAL_END_MAIN_PROCESS;
+    sdio_release_host(((struct sdio_mmc_card *) handle->card)->func);
+
+    LEAVE();
+}
+
+/**
+ * @brief Handles interrupt
+ *
+ * @param handle  A pointer to moal_handle struct
+ *
+ * @return        N/A
+ */
+void
+woal_interrupt(moal_handle * handle)
+{
+    ENTER();
+    handle->main_state = MOAL_RECV_INT;
+    PRINTM(MINTR, "*\n");
+    if (handle->surprise_removed == MTRUE) {
+        LEAVE();
+        return;
+    }
+    /* call mlan_interrupt to read int status */
+    mlan_interrupt(handle->pmlan_adapter);
+    handle->main_state = MOAL_START_MAIN_PROCESS;
+    /* Call MLAN main process */
+    mlan_main_process(handle->pmlan_adapter);
+    handle->main_state = MOAL_END_MAIN_PROCESS;
+    LEAVE();
+}
+
+/**
+ * @brief This function adds the card. it will probe the
+ * 		card, allocate the mlan_private and initialize the device.
+ *
+ *  @param card    A pointer to card
+ *
+ *  @return        A pointer to moal_handle structure
+ */
+moal_handle *
+woal_add_card(void *card)
+{
+    moal_handle *handle = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    int netlink_num = NETLINK_MARVELL;
+    int index = 0;
+
+    ENTER();
+
+    if (MOAL_ACQ_SEMAPHORE_BLOCK(&AddRemoveCardSem))
+        goto exit_sem_err;
+
+    /* Allocate buffer for moal_handle */
+    if (!(handle = kmalloc(sizeof(moal_handle), GFP_KERNEL))) {
+        PRINTM(MERROR, "Allocate buffer for moal_handle failed!\n");
+        goto err_handle;
+    }
+
+    /* Init moal_handle */
+    memset(handle, 0, sizeof(moal_handle));
+    handle->card = card;
+    /* Save the handle */
+    for (index = 0; index < MAX_MLAN_ADAPTER; index++) {
+        if (m_handle[index] == NULL)
+            break;
+    }
+    if (index < MAX_MLAN_ADAPTER) {
+        m_handle[index] = handle;
+        handle->handle_idx = index;
+    } else {
+        PRINTM(MERROR, "Exceeded maximum cards supported!\n");
+        goto err_kmalloc;
+    }
+
+    if (mac_addr) {
+        t_u8 temp[20];
+        t_u8 len = strlen(mac_addr) + 1;
+        if (len < sizeof(temp)) {
+            memcpy(temp, mac_addr, len);
+            handle->set_mac_addr = 1;
+            /* note: the following function overwrites the temp buffer */
+            woal_mac2u8(handle->mac_addr, temp);
+        }
+    }
+
+    ((struct sdio_mmc_card *) card)->handle = handle;
+#ifdef STA_SUPPORT
+    handle->scan_pending_on_block = MFALSE;
+    MOAL_INIT_SEMAPHORE(&handle->async_sem);
+#endif
+
+    /* Init SW */
+    if (MLAN_STATUS_SUCCESS != woal_init_sw(handle)) {
+        PRINTM(MFATAL, "Software Init Failed\n");
+        goto err_kmalloc;
+    }
+
+    do {
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,14)
+        handle->nl_sk = netlink_kernel_create(netlink_num, NULL);
+#else
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22)
+        handle->nl_sk =
+            netlink_kernel_create(netlink_num, NL_MULTICAST_GROUP, NULL,
+                                  THIS_MODULE);
+#else
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
+        handle->nl_sk =
+            netlink_kernel_create(netlink_num, NL_MULTICAST_GROUP, NULL, NULL,
+                                  THIS_MODULE);
+#else
+        handle->nl_sk =
+            netlink_kernel_create(&init_net, netlink_num, NL_MULTICAST_GROUP,
+                                  NULL, NULL, THIS_MODULE);
+#endif
+#endif
+#endif
+        if (handle->nl_sk) {
+            PRINTM(MINFO, "Netlink number = %d\n", netlink_num);
+            handle->netlink_num = netlink_num;
+            break;
+        }
+        netlink_num--;
+    } while (netlink_num > 0);
+
+    if (handle->nl_sk == NULL) {
+        PRINTM(MERROR,
+               "Could not initialize netlink event passing mechanism!\n");
+        goto err_kmalloc;
+    }
+
+    /* Create workqueue for main process */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,14)
+    /* For kernel less than 2.6.14 name can not be greater than 10 characters */
+    handle->workqueue = create_workqueue("MOAL_WORKQ");
+#else
+    handle->workqueue = create_workqueue("MOAL_WORK_QUEUE");
+#endif
+    if (!handle->workqueue)
+        goto err_kmalloc;
+
+    MLAN_INIT_WORK(&handle->main_work, woal_main_work_queue);
+
+#ifdef REASSOCIATION
+    PRINTM(MINFO, "Starting re-association thread...\n");
+    handle->reassoc_thread.handle = handle;
+    woal_create_thread(woal_reassociation_thread,
+                       &handle->reassoc_thread, "woal_reassoc_service");
+
+    while (!handle->reassoc_thread.pid) {
+        woal_sched_timeout(2);
+    }
+#endif /* REASSOCIATION */
+
+    /* Register the device. Fill up the private data structure with relevant
+       information from the card and request for the required IRQ. */
+    if (woal_register_dev(handle) != MLAN_STATUS_SUCCESS) {
+        PRINTM(MFATAL, "Failed to register wlan device!\n");
+        goto err_registerdev;
+    }
+
+    /* Init FW and HW */
+    if (MLAN_STATUS_SUCCESS != woal_init_fw(handle)) {
+        PRINTM(MFATAL, "Firmware Init Failed\n");
+        goto err_init_fw;
+    }
+
+    LEAVE();
+    return handle;
+
+  err_init_fw:
+    /* Unregister device */
+    PRINTM(MINFO, "unregister device\n");
+    woal_unregister_dev(handle);
+  err_registerdev:
+    handle->surprise_removed = MTRUE;
+#ifdef REASSOCIATION
+    if (handle->reassoc_thread.pid) {
+        wake_up_interruptible(&handle->reassoc_thread.wait_q);
+    }
+    /* waiting for main thread quit */
+    while (handle->reassoc_thread.pid) {
+        woal_sched_timeout(2);
+    }
+#endif /* REASSOCIATION */
+    woal_terminate_workqueue(handle);
+  err_kmalloc:
+    if ((handle->hardware_status == HardwareStatusFwReady) ||
+        (handle->hardware_status == HardwareStatusReady)) {
+        PRINTM(MINFO, "shutdown mlan\n");
+        handle->init_wait_q_woken = MFALSE;
+        status = mlan_shutdown_fw(handle->pmlan_adapter);
+        if (status == MLAN_STATUS_PENDING)
+            wait_event_interruptible(handle->init_wait_q,
+                                     handle->init_wait_q_woken);
+    }
+    woal_free_moal_handle(handle);
+    if (index < MAX_MLAN_ADAPTER) {
+        m_handle[index] = NULL;
+    }
+    ((struct sdio_mmc_card *) card)->handle = NULL;
+  err_handle:
+    MOAL_REL_SEMAPHORE(&AddRemoveCardSem);
+  exit_sem_err:
+    LEAVE();
+    return NULL;
+}
+
+/**
+ *  @brief This function removes the card.
+ *
+ *  @param card    A pointer to card
+ *
+ *  @return        MLAN_STATUS_SUCCESS
+ */
+mlan_status
+woal_remove_card(void *card)
+{
+    moal_handle *handle = NULL;
+    moal_private *priv = NULL;
+    mlan_status status;
+    int i;
+    int index = 0;
+
+    ENTER();
+
+    if (MOAL_ACQ_SEMAPHORE_BLOCK(&AddRemoveCardSem))
+        goto exit_sem_err;
+    /* Find the correct handle */
+    for (index = 0; index < MAX_MLAN_ADAPTER; index++) {
+        if (m_handle[index] && (m_handle[index]->card == card)) {
+            handle = m_handle[index];
+            break;
+        }
+    }
+    if (!handle)
+        goto exit_remove;
+    handle->surprise_removed = MTRUE;
+
+    /* Stop data */
+    for (i = 0; i < MIN(handle->priv_num, MLAN_MAX_BSS_NUM); i++) {
+        if ((priv = handle->priv[i])) {
+            woal_stop_queue(priv->netdev);
+            if (netif_carrier_ok(priv->netdev))
+                netif_carrier_off(priv->netdev);
+        }
+    }
+
+    if ((handle->hardware_status == HardwareStatusFwReady) ||
+        (handle->hardware_status == HardwareStatusReady)) {
+        /* Shutdown firmware */
+        PRINTM(MIOCTL, "mlan_shutdown_fw.....\n");
+        handle->init_wait_q_woken = MFALSE;
+
+        status = mlan_shutdown_fw(handle->pmlan_adapter);
+        if (status == MLAN_STATUS_PENDING)
+            wait_event_interruptible(handle->init_wait_q,
+                                     handle->init_wait_q_woken);
+        PRINTM(MIOCTL, "mlan_shutdown_fw done!\n");
+    }
+    if (atomic_read(&handle->rx_pending) || atomic_read(&handle->tx_pending) ||
+        atomic_read(&handle->ioctl_pending)) {
+        PRINTM(MERROR, "ERR: rx_pending=%d,tx_pending=%d,ioctl_pending=%d\n",
+               atomic_read(&handle->rx_pending),
+               atomic_read(&handle->tx_pending),
+               atomic_read(&handle->ioctl_pending));
+    }
+
+    /* Remove interface */
+    for (i = 0; i < handle->priv_num; i++)
+        woal_remove_interface(handle, i);
+
+    woal_terminate_workqueue(handle);
+
+#ifdef REASSOCIATION
+    PRINTM(MINFO, "Free reassoc_timer\n");
+    if (handle->is_reassoc_timer_set) {
+        woal_cancel_timer(&handle->reassoc_timer);
+        handle->is_reassoc_timer_set = MFALSE;
+    }
+    if (handle->reassoc_thread.pid)
+        wake_up_interruptible(&handle->reassoc_thread.wait_q);
+
+    /* waiting for main thread quit */
+    while (handle->reassoc_thread.pid) {
+        woal_sched_timeout(2);
+    }
+#endif /* REASSOCIATION */
+
+#ifdef CONFIG_PROC_FS
+    woal_proc_exit(handle);
+#endif
+    /* Unregister device */
+    PRINTM(MINFO, "unregister device\n");
+    woal_unregister_dev(handle);
+    /* Free adapter structure */
+    PRINTM(MINFO, "Free Adapter\n");
+    woal_free_moal_handle(handle);
+
+    for (index = 0; index < MAX_MLAN_ADAPTER; index++) {
+        if (m_handle[index] == handle) {
+            m_handle[index] = NULL;
+            break;
+        }
+    }
+  exit_remove:
+    MOAL_REL_SEMAPHORE(&AddRemoveCardSem);
+  exit_sem_err:
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function switch the drv_mode
+ *
+ *  @param handle    A pointer to moal_handle structure
+ *  @param mode     new drv_mode to switch.
+ *
+ *  @return        MLAN_STATUS_SUCCESS /MLAN_STATUS_FAILURE /MLAN_STATUS_PENDING
+ */
+mlan_status
+woal_switch_drv_mode(moal_handle * handle, t_u32 mode)
+{
+    unsigned int i;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    moal_private *priv = NULL;
+
+    ENTER();
+
+    if (MOAL_ACQ_SEMAPHORE_BLOCK(&AddRemoveCardSem))
+        goto exit_sem_err;
+
+    if (woal_update_drv_tbl(handle, mode) != MLAN_STATUS_SUCCESS) {
+        PRINTM(MERROR, "Could not update driver mode table!\n");
+        status = MLAN_STATUS_FAILURE;
+        goto exit;
+    }
+
+    /* Reset all interfaces */
+    priv = woal_get_priv(handle, MLAN_BSS_ROLE_ANY);
+    woal_reset_intf(priv, MOAL_PROC_WAIT, MTRUE);
+
+    status = woal_shutdown_fw(priv, MOAL_CMD_WAIT);
+    if (status != MLAN_STATUS_SUCCESS) {
+        PRINTM(MERROR, "func shutdown failed!\n");
+        goto exit;
+    }
+
+    /* Shutdown firmware */
+    PRINTM(MIOCTL, "mlan_shutdown_fw.....\n");
+    handle->init_wait_q_woken = MFALSE;
+    status = mlan_shutdown_fw(handle->pmlan_adapter);
+    if (status == MLAN_STATUS_PENDING)
+        wait_event_interruptible(handle->init_wait_q,
+                                 handle->init_wait_q_woken);
+    PRINTM(MIOCTL, "mlan_shutdown_fw done!\n");
+    if (atomic_read(&handle->rx_pending) || atomic_read(&handle->tx_pending) ||
+        atomic_read(&handle->ioctl_pending)) {
+        PRINTM(MERROR, "ERR: rx_pending=%d,tx_pending=%d,ioctl_pending=%d\n",
+               atomic_read(&handle->rx_pending),
+               atomic_read(&handle->tx_pending),
+               atomic_read(&handle->ioctl_pending));
+    }
+
+    /* Remove interface */
+    for (i = 0; i < handle->priv_num; i++)
+        woal_remove_interface(handle, i);
+
+    /* Unregister mlan */
+    if (handle->pmlan_adapter) {
+        mlan_unregister(handle->pmlan_adapter);
+        if (handle->malloc_count || handle->lock_count) {
+            PRINTM(MERROR,
+                   "mlan has memory leak: malloc_count=%u lock_count=%u\n",
+                   handle->malloc_count, handle->lock_count);
+        }
+        handle->pmlan_adapter = NULL;
+    }
+
+    handle->priv_num = 0;
+    drv_mode = mode;
+    /* Init SW */
+    if (woal_init_sw(handle)) {
+        PRINTM(MFATAL, "Software Init Failed\n");
+        goto exit;
+    }
+    /* Init FW and HW */
+    if (woal_init_fw(handle)) {
+        PRINTM(MFATAL, "Firmware Init Failed\n");
+        goto exit;
+    }
+    LEAVE();
+    return status;
+  exit:
+    MOAL_REL_SEMAPHORE(&AddRemoveCardSem);
+  exit_sem_err:
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief This function initializes module.
+ *
+ *  @return        MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static int
+woal_init_module(void)
+{
+    int ret = (int) MLAN_STATUS_SUCCESS;
+    int index = 0;
+
+    ENTER();
+
+    /* Init the wlan_private pointer array first */
+    for (index = 0; index < MAX_MLAN_ADAPTER; index++) {
+        m_handle[index] = NULL;
+    }
+    /* Init mutex */
+    MOAL_INIT_SEMAPHORE(&AddRemoveCardSem);
+
+    wifi_add_dev();
+
+    /* Register with bus */
+    ret = woal_bus_register();
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function cleans module
+ *
+ *  @return        N/A
+ */
+static void
+woal_cleanup_module(void)
+{
+    moal_handle *handle = NULL;
+    int index = 0;
+    int i;
+
+    ENTER();
+
+    if (MOAL_ACQ_SEMAPHORE_BLOCK(&AddRemoveCardSem))
+        goto exit_sem_err;
+    for (index = 0; index < MAX_MLAN_ADAPTER; index++) {
+        handle = m_handle[index];
+        if (!handle)
+            continue;
+        if (!handle->priv_num)
+            goto exit;
+#ifdef SDIO_SUSPEND_RESUME
+#ifdef MMC_PM_KEEP_POWER
+        if (handle->is_suspended == MTRUE) {
+            woal_sdio_resume(&(((struct sdio_mmc_card *) handle->card)->func)->
+                             dev);
+        }
+#endif
+#endif /* SDIO_SUSPEND_RESUME */
+
+        for (i = 0; i < handle->priv_num; i++) {
+#ifdef STA_SUPPORT
+            if (GET_BSS_ROLE(handle->priv[i]) == MLAN_BSS_ROLE_STA) {
+                if (handle->priv[i]->media_connected == MTRUE)
+                    woal_disconnect(handle->priv[i], MOAL_CMD_WAIT, NULL);
+#ifdef STA_CFG80211
+                if (IS_STA_CFG80211(cfg80211_wext) &&
+                    handle->priv[i]->scan_request) {
+                    cfg80211_scan_done(handle->priv[i]->scan_request, MTRUE);
+                    handle->priv[i]->scan_request = NULL;
+                }
+#endif
+            }
+#endif
+#ifdef UAP_SUPPORT
+            if (GET_BSS_ROLE(handle->priv[i]) == MLAN_BSS_ROLE_UAP) {
+#ifdef MFG_CMD_SUPPORT
+                if (mfg_mode != MLAN_INIT_PARA_ENABLED)
+#endif
+                    woal_disconnect(handle->priv[i], MOAL_CMD_WAIT, NULL);
+            }
+#endif
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+            if (handle->priv[i]->probereq_index != MLAN_CUSTOM_IE_AUTO_IDX_MASK)
+                woal_cfg80211_mgmt_frame_ie(handle->priv[i], NULL, 0, NULL, 0,
+                                            NULL, 0, NULL, 0,
+                                            MGMT_MASK_PROBE_REQ);
+#endif
+        }
+
+#ifdef MFG_CMD_SUPPORT
+        if (mfg_mode != MLAN_INIT_PARA_ENABLED)
+#endif
+            woal_set_deep_sleep(woal_get_priv(handle, MLAN_BSS_ROLE_ANY),
+                                MOAL_CMD_WAIT, MFALSE, 0);
+
+#ifdef MFG_CMD_SUPPORT
+        if (mfg_mode != MLAN_INIT_PARA_ENABLED)
+#endif
+            woal_shutdown_fw(woal_get_priv(handle, MLAN_BSS_ROLE_ANY),
+                             MOAL_CMD_WAIT);
+    }
+
+  exit:
+    MOAL_REL_SEMAPHORE(&AddRemoveCardSem);
+  exit_sem_err:
+    /* Unregister from bus */
+    woal_bus_unregister();
+    wifi_del_dev();
+
+    LEAVE();
+}
+
+module_init(woal_init_module);
+module_exit(woal_cleanup_module);
+
+module_param(fw_name, charp, 0);
+MODULE_PARM_DESC(fw_name, "Firmware name");
+module_param(req_fw_nowait, int, 0);
+MODULE_PARM_DESC(req_fw_nowait,
+                 "0: Use request_firmware API; 1: Use request_firmware_nowait API");
+module_param(mac_addr, charp, 0);
+MODULE_PARM_DESC(mac_addr, "MAC address");
+#ifdef MFG_CMD_SUPPORT
+module_param(mfg_mode, int, 0);
+MODULE_PARM_DESC(mfg_mode,
+                 "0: Download normal firmware; 1: Download MFG firmware");
+#endif /* MFG_CMD_SUPPORT */
+module_param(drv_mode, int, 0);
+#if defined(WIFI_DIRECT_SUPPORT)
+MODULE_PARM_DESC(drv_mode, "Bit 0: STA; Bit 1: uAP; Bit 2: WIFIDIRECT");
+#else
+MODULE_PARM_DESC(drv_mode, "Bit 0: STA; Bit 1: uAP");
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+#ifdef STA_SUPPORT
+module_param(max_sta_bss, int, 0);
+MODULE_PARM_DESC(max_sta_bss, "Number of STA interfaces (1)");
+#endif /* STA_SUPPORT */
+#ifdef UAP_SUPPORT
+module_param(max_uap_bss, int, 0);
+MODULE_PARM_DESC(max_uap_bss, "Number of uAP interfaces (1)");
+#endif /* UAP_SUPPORT */
+#if defined(WIFI_DIRECT_SUPPORT)
+module_param(max_wfd_bss, int, 0);
+MODULE_PARM_DESC(max_wfd_bss, "Number of WIFIDIRECT interfaces (1)");
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+#ifdef DEBUG_LEVEL1
+module_param(drvdbg, uint, 0);
+MODULE_PARM_DESC(drvdbg, "Driver debug");
+#endif /* DEBUG_LEVEL1 */
+module_param(auto_ds, int, 0);
+MODULE_PARM_DESC(auto_ds,
+                 "0: MLAN default; 1: Enable auto deep sleep; 2: Disable auto deep sleep");
+module_param(ps_mode, int, 0);
+MODULE_PARM_DESC(ps_mode,
+                 "0: MLAN default; 1: Enable IEEE PS mode; 2: Disable IEEE PS mode");
+module_param(max_tx_buf, int, 0);
+MODULE_PARM_DESC(max_tx_buf, "Maximum Tx buffer size (2048/4096/8192)");
+#ifdef SDIO_SUSPEND_RESUME
+module_param(pm_keep_power, int, 1);
+MODULE_PARM_DESC(pm_keep_power, "1: PM keep power; 0: PM no power");
+#endif
+#if defined(STA_SUPPORT)
+module_param(cfg_11d, int, 0);
+MODULE_PARM_DESC(cfg_11d,
+                 "0: MLAN default; 1: Enable 802.11d; 2: Disable 802.11d");
+#endif
+module_param(init_cfg, charp, 0);
+MODULE_PARM_DESC(init_cfg, "Init config file name");
+module_param(cal_data_cfg, charp, 0);
+MODULE_PARM_DESC(cal_data_cfg, "Calibration data file name");
+module_param(minicard_pwrup, int, 1);
+MODULE_PARM_DESC(minicard_pwrup,
+                 "1: Driver load clears PDn/Rst, unload sets (default); 0: Don't do this.");
+module_param(cfg80211_wext, int, 0);
+MODULE_PARM_DESC(cfg80211_wext,
+#ifdef STA_WEXT
+                 "Bit 0: STA WEXT; "
+#endif
+#ifdef UAP_WEXT
+                 "Bit 1: UAP WEXT; "
+#endif
+#ifdef STA_CFG80211
+                 "Bit 2: STA CFG80211; "
+#endif
+#ifdef UAP_CFG80211
+                 "Bit 3: UAP CFG80211;"
+#endif
+    );
+MODULE_DESCRIPTION("M-WLAN Driver");
+MODULE_AUTHOR("Marvell International Ltd.");
+MODULE_VERSION(MLAN_RELEASE_VERSION);
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_main.h b/drivers/net/wireless/sd8797/mlinux/moal_main.h
new file mode 100644
index 000000000000..32426a149779
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_main.h
@@ -0,0 +1,1564 @@
+/** @file moal_main.h
+  *
+  * @brief This file contains wlan driver specific defines etc.
+  * 
+  * Copyright (C) 2008-2012, Marvell International Ltd.  
+  *
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+
+/********************************************************
+Change log:
+    10/21/2008: initial version
+********************************************************/
+
+#ifndef _MOAL_MAIN_H
+#define _MOAL_MAIN_H
+
+/* warnfix for FS redefination if any? */
+#ifdef FS
+#undef FS
+#endif
+
+/* Linux header files */
+#include        <linux/kernel.h>
+#include        <linux/module.h>
+#include        <linux/init.h>
+#include        <linux/version.h>
+#include        <linux/param.h>
+#include        <linux/delay.h>
+#include        <linux/slab.h>
+#include        <linux/mm.h>
+#include        <linux/types.h>
+#include        <linux/sched.h>
+#include        <linux/timer.h>
+#include        <linux/ioport.h>
+#include        <linux/pci.h>
+#include        <linux/ctype.h>
+#include        <linux/proc_fs.h>
+#include        <linux/vmalloc.h>
+#include        <linux/ptrace.h>
+#include        <linux/string.h>
+#include        <linux/list.h>
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,18)
+#include       <linux/config.h>
+#endif
+
+/* ASM files */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+#include        <linux/semaphore.h>
+#else
+#include        <asm/semaphore.h>
+#endif
+#include        <asm/byteorder.h>
+#include        <asm/irq.h>
+#include        <asm/uaccess.h>
+#include        <asm/io.h>
+#include        <asm/system.h>
+
+/* Net header files */
+#include        <linux/netdevice.h>
+#include        <linux/net.h>
+#include        <linux/inet.h>
+#include        <linux/ip.h>
+#include        <linux/skbuff.h>
+#include        <linux/if_arp.h>
+#include        <linux/if_ether.h>
+#include        <linux/etherdevice.h>
+#include        <net/sock.h>
+#include        <net/arp.h>
+#include        <linux/rtnetlink.h>
+
+#include	<linux/firmware.h>
+
+#include        "mlan.h"
+#include        "moal_shim.h"
+/* Wireless header */
+#include        <linux/wireless.h>
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+#include        <net/lib80211.h>
+#include        <net/cfg80211.h>
+#include        <net/ieee80211_radiotap.h>
+#endif
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+#include        <net/iw_handler.h>
+#include        "moal_wext.h"
+#endif
+#ifdef STA_WEXT
+#include        "moal_priv.h"
+#endif
+
+/** Define BOOLEAN */
+typedef t_u8 BOOLEAN;
+
+/** Driver version */
+extern char driver_version[];
+
+/** Private structure for MOAL */
+typedef struct _moal_private moal_private;
+/** Handle data structure for MOAL  */
+typedef struct _moal_handle moal_handle;
+
+/** Hardware status codes */
+typedef enum _MOAL_HARDWARE_STATUS
+{
+    HardwareStatusReady,
+    HardwareStatusInitializing,
+    HardwareStatusFwReady,
+    HardwareStatusReset,
+    HardwareStatusClosing,
+    HardwareStatusNotReady
+} MOAL_HARDWARE_STATUS;
+
+/** moal_wait_option */
+enum
+{
+    MOAL_NO_WAIT,
+    MOAL_IOCTL_WAIT,
+    MOAL_CMD_WAIT,
+    MOAL_PROC_WAIT,
+    MOAL_WSTATS_WAIT
+};
+
+/** moal_main_state */
+enum
+{
+    MOAL_STATE_IDLE,
+    MOAL_RECV_INT,
+    MOAL_ENTER_WORK_QUEUE,
+    MOAL_START_MAIN_PROCESS,
+    MOAL_END_MAIN_PROCESS
+};
+
+/** HostCmd_Header */
+typedef struct _HostCmd_Header
+{
+    /** Command */
+    t_u16 command;
+    /** Size */
+    t_u16 size;
+} HostCmd_Header;
+
+#ifndef MIN
+/** Find minimum */
+#define MIN(a,b)		((a) < (b) ? (a) : (b))
+#endif
+
+/*
+ * OS timer specific
+ */
+
+/** Timer structure */
+typedef struct _moal_drv_timer
+{
+        /** Timer list */
+    struct timer_list tl;
+        /** Timer function */
+    void (*timer_function) (void *context);
+        /** Timer function context */
+    void *function_context;
+        /** Time period */
+    t_u32 time_period;
+        /** Is timer periodic ? */
+    t_u32 timer_is_periodic;
+        /** Is timer cancelled ? */
+    t_u32 timer_is_canceled;
+} moal_drv_timer, *pmoal_drv_timer;
+
+/**
+ *  @brief Timer handler
+ *
+ *  @param fcontext	Timer context
+ *
+ *  @return		N/A
+ */
+static inline void
+woal_timer_handler(unsigned long fcontext)
+{
+    pmoal_drv_timer timer = (pmoal_drv_timer) fcontext;
+
+    timer->timer_function(timer->function_context);
+
+    if (timer->timer_is_periodic == MTRUE) {
+        mod_timer(&timer->tl, jiffies + ((timer->time_period * HZ) / 1000));
+    } else {
+        timer->timer_is_canceled = MTRUE;
+        timer->time_period = 0;
+    }
+}
+
+/**
+ *  @brief Initialize timer
+ *
+ *  @param timer		Timer structure
+ *  @param TimerFunction	Timer function
+ *  @param FunctionContext	Timer function context
+ *
+ *  @return			N/A
+ */
+static inline void
+woal_initialize_timer(pmoal_drv_timer timer,
+                      void (*TimerFunction) (void *context),
+                      void *FunctionContext)
+{
+    /* First, setup the timer to trigger the wlan_timer_handler proxy */
+    init_timer(&timer->tl);
+    timer->tl.function = woal_timer_handler;
+    timer->tl.data = (t_ptr) timer;
+
+    /* Then tell the proxy which function to call and what to pass it */
+    timer->timer_function = TimerFunction;
+    timer->function_context = FunctionContext;
+    timer->timer_is_canceled = MTRUE;
+    timer->time_period = 0;
+    timer->timer_is_periodic = MFALSE;
+}
+
+/**
+ *  @brief Modify timer
+ *
+ *  @param timer		Timer structure
+ *  @param MillisecondPeriod	Time period in millisecond
+ *
+ *  @return			N/A
+ */
+static inline void
+woal_mod_timer(pmoal_drv_timer timer, t_u32 MillisecondPeriod)
+{
+    timer->time_period = MillisecondPeriod;
+    mod_timer(&timer->tl, jiffies + (MillisecondPeriod * HZ) / 1000);
+    timer->timer_is_canceled = MFALSE;
+}
+
+/**
+ *  @brief Cancel timer
+ *
+ *  @param timer	Timer structure
+ *
+ *  @return		N/A
+ */
+static inline void
+woal_cancel_timer(moal_drv_timer * timer)
+{
+    del_timer(&timer->tl);
+    timer->timer_is_canceled = MTRUE;
+    timer->time_period = 0;
+}
+
+#ifdef REASSOCIATION
+/*
+ * OS Thread Specific
+ */
+
+#include	<linux/kthread.h>
+
+/** Kernel thread structure */
+typedef struct _moal_thread
+{
+    /** Task control structrue */
+    struct task_struct *task;
+    /** Pointer to wait_queue_head */
+    wait_queue_head_t wait_q;
+    /** PID */
+    pid_t pid;
+    /** Pointer to moal_handle */
+    void *handle;
+} moal_thread;
+
+/**
+ *  @brief Activate thread
+ *
+ *  @param thr			Thread structure
+ *  @return			N/A
+ */
+static inline void
+woal_activate_thread(moal_thread * thr)
+{
+    /** Initialize the wait queue */
+    init_waitqueue_head(&thr->wait_q);
+
+    /** Record the thread pid */
+    thr->pid = current->pid;
+}
+
+/**
+ *  @brief De-activate thread
+ *
+ *  @param thr			Thread structure
+ *  @return			N/A
+ */
+static inline void
+woal_deactivate_thread(moal_thread * thr)
+{
+    /* Reset the pid */
+    thr->pid = 0;
+}
+
+/**
+ *  @brief Create and run the thread
+ *
+ *  @param threadfunc		Thread function
+ *  @param thr			Thread structure
+ *  @param name			Thread name
+ *  @return			N/A
+ */
+static inline void
+woal_create_thread(int (*threadfunc) (void *), moal_thread * thr, char *name)
+{
+    /* Create and run the thread */
+    thr->task = kthread_run(threadfunc, thr, "%s", name);
+}
+#endif /* REASSOCIATION */
+
+/* The following macros are neccessary to retain compatibility
+ * around the workqueue chenges happened in kernels >= 2.6.20:
+ * - INIT_WORK changed to take 2 arguments and let the work function
+ *   get its own data through the container_of macro
+ * - delayed works have been split from normal works to save some
+ *   memory usage in struct work_struct
+ */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+/** Work_queue work initialization */
+#define MLAN_INIT_WORK(_work, _fun)                 INIT_WORK(_work, ((void (*)(void *))_fun), _work)
+/** Work_queue delayed work initialization */
+#define MLAN_INIT_DELAYED_WORK(_work, _fun)         INIT_WORK(_work, ((void (*)(void *))_fun), _work)
+/** Work_queue container parameter */
+#define MLAN_DELAYED_CONTAINER_OF(_ptr, _type, _m)  container_of(_ptr, _type, _m)
+#else /* LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20) */
+/** Work_queue work initialization */
+#define MLAN_INIT_WORK                              INIT_WORK
+/** Work_queue delayed work initialization */
+#define MLAN_INIT_DELAYED_WORK                      INIT_DELAYED_WORK
+/** Work_queue container parameter */
+#define MLAN_DELAYED_CONTAINER_OF(_ptr, _type, _m)  container_of(_ptr, _type, _m.work)
+#endif /* LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20) */
+
+/**
+ *  @brief Schedule timeout
+ *
+ *  @param millisec	Timeout duration in milli second
+ *
+ *  @return		N/A
+ */
+static inline void
+woal_sched_timeout(t_u32 millisec)
+{
+    set_current_state(TASK_INTERRUPTIBLE);
+
+    schedule_timeout((millisec * HZ) / 1000);
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,19)
+#define IN6PTON_XDIGIT		0x00010000
+#define IN6PTON_DIGIT		0x00020000
+#define IN6PTON_COLON_MASK	0x00700000
+#define IN6PTON_COLON_1		0x00100000      /* single : requested */
+#define IN6PTON_COLON_2		0x00200000      /* second : requested */
+#define IN6PTON_COLON_1_2	0x00400000      /* :: requested */
+#define IN6PTON_DOT		0x00800000      /* . */
+#define IN6PTON_DELIM		0x10000000
+#define IN6PTON_NULL		0x20000000      /* first/tail */
+#define IN6PTON_UNKNOWN		0x40000000
+
+static inline int
+xdigit2bin(char c, int delim)
+{
+    if (c == delim || c == '\0')
+        return IN6PTON_DELIM;
+    if (c == ':')
+        return IN6PTON_COLON_MASK;
+    if (c == '.')
+        return IN6PTON_DOT;
+    if (c >= '0' && c <= '9')
+        return (IN6PTON_XDIGIT | IN6PTON_DIGIT | (c - '0'));
+    if (c >= 'a' && c <= 'f')
+        return (IN6PTON_XDIGIT | (c - 'a' + 10));
+    if (c >= 'A' && c <= 'F')
+        return (IN6PTON_XDIGIT | (c - 'A' + 10));
+    if (delim == -1)
+        return IN6PTON_DELIM;
+    return IN6PTON_UNKNOWN;
+}
+
+static inline int
+in4_pton(const char *src, int srclen, u8 * dst, int delim, const char **end)
+{
+    const char *s;
+    u8 *d;
+    u8 dbuf[4];
+    int ret = 0;
+    int i;
+    int w = 0;
+
+    if (srclen < 0)
+        srclen = strlen(src);
+    s = src;
+    d = dbuf;
+    i = 0;
+    while (1) {
+        int c;
+        c = xdigit2bin(srclen > 0 ? *s : '\0', delim);
+        if (!
+            (c &
+             (IN6PTON_DIGIT | IN6PTON_DOT | IN6PTON_DELIM |
+              IN6PTON_COLON_MASK))) {
+            goto out;
+        }
+        if (c & (IN6PTON_DOT | IN6PTON_DELIM | IN6PTON_COLON_MASK)) {
+            if (w == 0)
+                goto out;
+            *d++ = w & 0xff;
+            w = 0;
+            i++;
+            if (c & (IN6PTON_DELIM | IN6PTON_COLON_MASK)) {
+                if (i != 4)
+                    goto out;
+                break;
+            }
+            goto cont;
+        }
+        w = (w * 10) + c;
+        if ((w & 0xffff) > 255) {
+            goto out;
+        }
+      cont:
+        if (i >= 4)
+            goto out;
+        s++;
+        srclen--;
+    }
+    ret = 1;
+    memcpy(dst, dbuf, sizeof(dbuf));
+  out:
+    if (end)
+        *end = s;
+    return ret;
+}
+#endif /* < 2.6.19 */
+
+#ifndef __ATTRIB_ALIGN__
+#define __ATTRIB_ALIGN__ __attribute__((aligned(4)))
+#endif
+
+#ifndef __ATTRIB_PACK__
+#define __ATTRIB_PACK__ __attribute__ ((packed))
+#endif
+
+/** Get module */
+#define MODULE_GET	try_module_get(THIS_MODULE)
+/** Put module */
+#define MODULE_PUT	module_put(THIS_MODULE)
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,37)
+/** Initialize semaphore */
+#define MOAL_INIT_SEMAPHORE(x)    	init_MUTEX(x)
+/** Initialize semaphore */
+#define MOAL_INIT_SEMAPHORE_LOCKED(x) 	init_MUTEX_LOCKED(x)
+#else
+/** Initialize semaphore */
+#define MOAL_INIT_SEMAPHORE(x)    	sema_init(x,1)
+/** Initialize semaphore */
+#define MOAL_INIT_SEMAPHORE_LOCKED(x) 	sema_init(x,0)
+#endif
+
+/** Acquire semaphore and with blocking */
+#define MOAL_ACQ_SEMAPHORE_BLOCK(x)	down_interruptible(x)
+/** Acquire semaphore without blocking */
+#define MOAL_ACQ_SEMAPHORE_NOBLOCK(x)	down_trylock(x)
+/** Release semaphore */
+#define MOAL_REL_SEMAPHORE(x) 		up(x)
+
+/** Request FW timeout in second */
+#define REQUEST_FW_TIMEOUT		30
+
+/** Default watchdog timeout */
+#define MRVDRV_DEFAULT_WATCHDOG_TIMEOUT (5 * HZ)
+
+#ifdef UAP_SUPPORT
+/** Default watchdog timeout
+    Increase the value to avoid kernel Tx timeout message in case
+    station in PS mode or left.
+    The default value of PS station ageout timer is 40 seconds.
+    Hence, the watchdog timer is set to a value higher than it.
+*/
+#define MRVDRV_DEFAULT_UAP_WATCHDOG_TIMEOUT (41 * HZ)
+#endif
+
+/** Threshold value of number of times the Tx timeout happened */
+#define NUM_TX_TIMEOUT_THRESHOLD      5
+
+/** 10 seconds */
+#define MOAL_TIMER_10S                10000
+/** 5 seconds */
+#define MOAL_TIMER_5S                 5000
+/** 1 second */
+#define MOAL_TIMER_1S                 1000
+
+/** Default value of re-assoc timer */
+#define REASSOC_TIMER_DEFAULT         500
+
+/** Netlink protocol number */
+#define NETLINK_MARVELL     (MAX_LINKS - 1)
+/** Netlink maximum payload size */
+#define NL_MAX_PAYLOAD      1024
+/** Netlink multicast group number */
+#define NL_MULTICAST_GROUP  1
+
+/** MAX Tx Pending count */
+#define MAX_TX_PENDING    	100
+
+/** LOW Tx Pending count */
+#define LOW_TX_PENDING      80
+
+/** Offset for subcommand */
+#define SUBCMD_OFFSET       4
+
+/** Macro to extract the TOS field from a skb */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,22)
+#define SKB_TOS(skb) (ip_hdr(skb)->tos)
+#else
+#define SKB_TOS(skb) (skb->nh.iph->tos)
+#endif
+
+/** Offset for TOS field in the IP header */
+#define IPTOS_OFFSET 5
+
+/** Offset for DSCP in the tos field */
+#define DSCP_OFFSET 2
+
+/** wait_queue structure */
+typedef struct _wait_queue
+{
+        /** Pointer to wait_queue_head */
+    wait_queue_head_t *wait;
+        /** Wait condition */
+    BOOLEAN condition;
+        /** Start time */
+    t_u32 start_time;
+        /** Status from MLAN */
+    mlan_status status;
+} wait_queue, *pwait_queue;
+
+/** Auto Rate */
+#define AUTO_RATE 0xFF
+
+#define STA_WEXT_MASK        MBIT(0)
+#define UAP_WEXT_MASK        MBIT(1)
+#define STA_CFG80211_MASK    MBIT(2)
+#define UAP_CFG80211_MASK    MBIT(3)
+#ifdef STA_CFG80211
+#ifdef STA_SUPPORT
+/** Is STA CFG80211 enabled in module param */
+#define IS_STA_CFG80211(x)          (x & STA_CFG80211_MASK)
+#endif
+#endif
+#ifdef UAP_CFG80211
+#ifdef UAP_SUPPORT
+/** Is UAP CFG80211 enabled in module param */
+#define IS_UAP_CFG80211(x)          (x & UAP_CFG80211_MASK)
+#endif
+#endif
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+/** Is UAP or STA CFG80211 enabled in module param */
+#define IS_STA_OR_UAP_CFG80211(x)   (x & (STA_CFG80211_MASK | UAP_CFG80211_MASK))
+#endif
+
+#ifdef STA_WEXT
+/** Is STA WEXT enabled in module param */
+#define IS_STA_WEXT(x)              (x & STA_WEXT_MASK)
+#endif /* STA_WEXT */
+#ifdef UAP_WEXT
+/** Is UAP WEXT enabled in module param */
+#define IS_UAP_WEXT(x)              (x & UAP_WEXT_MASK)
+#endif /* UAP_WEXT */
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+/** Is UAP or STA WEXT enabled in module param */
+#define IS_STA_OR_UAP_WEXT(x)       (x & (STA_WEXT_MASK | UAP_WEXT_MASK))
+#endif
+
+#ifdef STA_SUPPORT
+/** Driver mode STA bit */
+#define DRV_MODE_STA       MBIT(0)
+/** Maximum STA BSS */
+#define MAX_STA_BSS        1
+/** Default STA BSS */
+#define DEF_STA_BSS        1
+#endif
+#ifdef UAP_SUPPORT
+/** Driver mode uAP bit */
+#define DRV_MODE_UAP       MBIT(1)
+/** Maximum uAP BSS */
+#define MAX_UAP_BSS        2
+/** Default uAP BSS */
+#define DEF_UAP_BSS        1
+#endif
+#if defined(WIFI_DIRECT_SUPPORT)
+/** Driver mode WIFIDIRECT bit */
+#define DRV_MODE_WIFIDIRECT       MBIT(2)
+/** Maximum WIFIDIRECT BSS */
+#define MAX_WIFIDIRECT_BSS        1
+/** Default WIFIDIRECT BSS */
+#define DEF_WIFIDIRECT_BSS        1
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+
+typedef struct _moal_drv_mode
+{
+    /** driver mode */
+    t_u16 drv_mode;
+    /** total number of interfaces */
+    t_u16 intf_num;
+    /** attribute of bss */
+    mlan_bss_attr *bss_attr;
+    /** name of firmware image */
+    char *fw_name;
+} moal_drv_mode;
+
+#ifdef PROC_DEBUG
+/** Debug data */
+struct debug_data
+{
+    /** Name */
+    char name[32];
+    /** Size */
+    t_u32 size;
+    /** Address */
+    t_ptr addr;
+};
+
+/** Private debug data */
+struct debug_data_priv
+{
+    /** moal_private handle */
+    moal_private *priv;
+    /** Debug items */
+    struct debug_data *items;
+    /** numbre of item */
+    int num_of_items;
+};
+#endif
+
+/** Maximum IP address buffer length */
+#define IPADDR_MAX_BUF          20
+/** IP address operation: Remove */
+#define IPADDR_OP_REMOVE        0
+
+struct tcp_sess
+{
+    struct list_head link;
+    /** tcp session info */
+    t_u32 src_ip_addr;
+    t_u32 dst_ip_addr;
+    t_u16 src_tcp_port;
+    t_u16 dst_tcp_port;
+    /** tcp window info */
+    t_u8 rx_win_opt;
+    t_u32 rx_win_scale;
+    /** warming up counter */
+    t_u32 start_cnt;
+    /** tx ack packet info */
+    t_u32 ack_seq;
+    t_u32 ack_cnt;
+};
+
+/** Private structure for MOAL */
+struct _moal_private
+{
+        /** Handle structure */
+    moal_handle *phandle;
+        /** Tx timeout count */
+    t_u32 num_tx_timeout;
+        /** BSS index */
+    t_u8 bss_index;
+        /** BSS type */
+    t_u8 bss_type;
+        /** BSS role */
+    t_u8 bss_role;
+        /** MAC address information */
+    t_u8 current_addr[ETH_ALEN];
+        /** Media connection status */
+    BOOLEAN media_connected;
+#ifdef UAP_SUPPORT
+        /** uAP started or not */
+    BOOLEAN bss_started;
+#endif
+#ifdef STA_SUPPORT
+        /** scan type */
+    t_u8 scan_type;
+        /** bg_scan_start */
+    t_u8 bg_scan_start;
+        /** bg_scan reported */
+    t_u8 bg_scan_reported;
+        /** bg_scan config */
+    wlan_bgscan_cfg scan_cfg;
+#endif
+        /** Net device pointer */
+    struct net_device *netdev;
+        /** Net device statistics structure */
+    struct net_device_stats stats;
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+        /** Country code for regulatory domain */
+    t_u8 country_code[COUNTRY_CODE_LEN];
+        /** Wireless device pointer */
+    struct wireless_dev *wdev;
+        /** channel parameter for UAP/GO */
+    t_u16 channel;
+        /** cipher */
+    t_u32 cipher;
+        /** key index */
+    t_u8 key_index;
+        /** key len */
+    t_u16 key_len;
+        /** key data */
+    t_u8 key_material[MLAN_MAX_KEY_LENGTH];
+        /** probereq index for mgmt ie */
+    t_u16 probereq_index;
+#endif
+#ifdef STA_CFG80211
+#ifdef STA_SUPPORT
+        /** CFG80211 scan request description */
+    struct cfg80211_scan_request *scan_request;
+        /** CFG80211 association description */
+    t_u8 cfg_bssid[ETH_ALEN];
+        /** Disconnect request from CFG80211 */
+    bool cfg_disconnect;
+        /** rssi_threshold */
+    s32 cqm_rssi_thold;
+        /** rssi hysteresis */
+    u32 cqm_rssi_hyst;
+        /** last rssi_low */
+    u8 last_rssi_low;
+        /** last rssi_high */
+    u8 last_rssi_high;
+        /** mrvl rssi threshold */
+    u8 mrvl_rssi_low;
+        /** rssi status */
+    u32 rssi_status;
+#endif                          /* STA_SUPPORT */
+#endif                          /* STA_CFG80211 */
+        /** IOCTL wait queue */
+    wait_queue_head_t ioctl_wait_q __ATTRIB_ALIGN__;
+        /** CMD wait queue */
+    wait_queue_head_t cmd_wait_q __ATTRIB_ALIGN__;
+#ifdef CONFIG_PROC_FS
+        /** Proc entry */
+    struct proc_dir_entry *proc_entry;
+        /** Proc entry name */
+    t_s8 proc_entry_name[IFNAMSIZ];
+        /** PROC wait queue */
+    wait_queue_head_t proc_wait_q __ATTRIB_ALIGN__;
+#endif                          /* CONFIG_PROC_FS */
+#ifdef STA_SUPPORT
+        /** Nickname */
+    t_u8 nick_name[16];
+        /** AdHoc link sensed flag */
+    BOOLEAN is_adhoc_link_sensed;
+        /** Current WEP key index */
+    t_u16 current_key_index;
+#ifdef REASSOCIATION
+    mlan_ssid_bssid prev_ssid_bssid;
+        /** Re-association required */
+    BOOLEAN reassoc_required;
+        /** Flag of re-association on/off */
+    BOOLEAN reassoc_on;
+#endif                          /* REASSOCIATION */
+        /** Report scan result */
+    t_u8 report_scan_result;
+        /** wpa_version */
+    t_u8 wpa_version;
+        /** key mgmt */
+    t_u8 key_mgmt;
+        /** rx_filter */
+    t_u8 rx_filter;
+#endif                          /* STA_SUPPORT */
+        /** Rate index */
+    t_u16 rate_index;
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+        /** IW statistics */
+    struct iw_statistics w_stats;
+        /** w_stats wait queue */
+    wait_queue_head_t w_stats_wait_q __ATTRIB_ALIGN__;
+#endif
+#ifdef UAP_WEXT
+    /** Pairwise Cipher used for WPA/WPA2 mode */
+    t_u16 pairwise_cipher;
+    /** Group Cipher */
+    t_u16 group_cipher;
+    /** Protocol stored during uap wext configuratoin */
+    t_u16 uap_protocol;
+    /** Key Mgmt whether PSK or 1x */
+    t_u16 uap_key_mgmt;
+    /** Beacon IE length from hostapd */
+    t_u16 bcn_ie_len;
+    /** Beacon IE buffer from hostapd */
+    t_u8 bcn_ie_buf[MAX_IE_SIZE];
+#endif
+
+#ifdef PROC_DEBUG
+    /** MLAN debug info */
+    struct debug_data_priv items_priv;
+#endif
+
+    /** tcp session queue */
+    struct list_head tcp_sess_queue;
+    /** TCP Ack enhance flag */
+    t_u8 enable_tcp_ack_enh;
+};
+
+/** Handle data structure for MOAL */
+struct _moal_handle
+{
+        /** MLAN adapter structure */
+    t_void *pmlan_adapter;
+        /** Private pointer */
+    moal_private *priv[MLAN_MAX_BSS_NUM];
+        /** Priv number */
+    t_u8 priv_num;
+        /** Bss attr */
+    moal_drv_mode drv_mode;
+        /** set mac address flag */
+    t_u8 set_mac_addr;
+        /** MAC address */
+    t_u8 mac_addr[ETH_ALEN];
+#ifdef CONFIG_PROC_FS
+        /** Proc top level directory entry */
+    struct proc_dir_entry *proc_mwlan;
+#endif
+        /** Firmware */
+    const struct firmware *firmware;
+        /** Firmware request start time */
+    struct timeval req_fw_time;
+        /** Init config file */
+    const struct firmware *user_data;
+        /** Hotplug device */
+    struct device *hotplug_device;
+        /** STATUS variables */
+    MOAL_HARDWARE_STATUS hardware_status;
+        /** POWER MANAGEMENT AND PnP SUPPORT */
+    BOOLEAN surprise_removed;
+        /** Firmware release number */
+    t_u32 fw_release_number;
+        /** Init wait queue token */
+    t_u16 init_wait_q_woken;
+        /** Init wait queue */
+    wait_queue_head_t init_wait_q __ATTRIB_ALIGN__;
+#if defined(SDIO_SUSPEND_RESUME)
+        /** Device suspend flag */
+    BOOLEAN is_suspended;
+#ifdef SDIO_SUSPEND_RESUME
+        /** suspend notify flag */
+    BOOLEAN suspend_notify_req;
+#endif
+        /** Host Sleep activated flag */
+    t_u8 hs_activated;
+        /** Host Sleep activated event wait queue token */
+    t_u16 hs_activate_wait_q_woken;
+        /** Host Sleep activated event wait queue */
+    wait_queue_head_t hs_activate_wait_q __ATTRIB_ALIGN__;
+#endif
+        /** Card pointer */
+    t_void *card;
+        /** Rx pending in MLAN */
+    atomic_t rx_pending;
+        /** Tx packet pending count in mlan */
+    atomic_t tx_pending;
+        /** IOCTL pending count in mlan */
+    atomic_t ioctl_pending;
+        /** Malloc count */
+    t_u32 malloc_count;
+        /** lock count */
+    t_u32 lock_count;
+        /** mlan buffer alloc count */
+    t_u32 mbufalloc_count;
+#if defined(SDIO_SUSPEND_RESUME)
+        /** hs skip count */
+    t_u32 hs_skip_count;
+        /** hs force count */
+    t_u32 hs_force_count;
+        /** suspend_fail flag */
+    BOOLEAN suspend_fail;
+#endif
+#ifdef REASSOCIATION
+        /** Re-association thread */
+    moal_thread reassoc_thread;
+        /** Re-association timer set flag */
+    BOOLEAN is_reassoc_timer_set;
+        /** Re-association timer */
+    moal_drv_timer reassoc_timer __ATTRIB_ALIGN__;
+        /**  */
+    struct semaphore reassoc_sem;
+        /** Bitmap for re-association on/off */
+    t_u8 reassoc_on;
+#endif                          /* REASSOCIATION */
+        /** Driver workqueue */
+    struct workqueue_struct *workqueue;
+        /** main work */
+    struct work_struct main_work;
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+#ifdef WIFI_DIRECT_SUPPORT
+        /** remain on channel flag */
+    t_u8 remain_on_channel;
+        /** ieee802_11_channel */
+    struct ieee80211_channel chan;
+        /** channel type */
+    enum nl80211_channel_type channel_type;
+        /** cookie */
+    t_u64 cookie;
+#endif
+#endif
+        /** Read SDIO registers for debugging */
+    t_u32 sdio_reg_dbg;
+        /** Netlink kernel socket */
+    struct sock *nl_sk;
+        /** Netlink kernel socket number */
+    t_u32 netlink_num;
+    /** w_stats wait queue token */
+    BOOLEAN meas_wait_q_woken;
+    /** w_stats wait queue */
+    wait_queue_head_t meas_wait_q __ATTRIB_ALIGN__;
+    /** Measurement start jiffes */
+    t_u32 meas_start_jiffies;
+    /** CAC checking period flag */
+    BOOLEAN cac_period;
+    /** BSS START command delay executing flag */
+    BOOLEAN delay_bss_start;
+    /** SSID,BSSID parameter of delay executing */
+    mlan_ssid_bssid delay_ssid_bssid;
+#ifdef DFS_TESTING_SUPPORT
+    /** cac period length, valid only when dfs testing is enabled */
+    t_u32 cac_period_jiffies;
+#endif
+    /** handle index - for multiple card supports */
+    t_u8 handle_idx;
+#ifdef SDIO_MMC_DEBUG
+        /** cmd53 write state */
+    u8 cmd53w;
+        /** cmd53 read state */
+    u8 cmd53r;
+#endif
+#ifdef STA_SUPPORT
+        /** Scan pending on blocked flag */
+    t_u8 scan_pending_on_block;
+        /** Async scan semaphore */
+    struct semaphore async_sem;
+
+#endif
+        /** main state */
+    t_u8 main_state;
+        /** cmd52 function */
+    t_u8 cmd52_func;
+        /** cmd52 register */
+    t_u8 cmd52_reg;
+        /** cmd52 value */
+    t_u8 cmd52_val;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+        /** spinlock to stop_queue/wake_queue*/
+    spinlock_t queue_lock;
+#endif
+};
+
+/**
+ *  @brief set trans_start for each TX queue.
+ *
+ *  @param dev		A pointer to net_device structure
+ *
+ *  @return			N/A
+ */
+static inline void
+woal_set_trans_start(struct net_device *dev)
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,31)
+    unsigned int i;
+    for (i = 0; i < dev->num_tx_queues; i++) {
+        netdev_get_tx_queue(dev, i)->trans_start = jiffies;
+    }
+#endif
+    dev->trans_start = jiffies;
+}
+
+/**
+ *  @brief Start queue
+ *
+ *  @param dev		A pointer to net_device structure
+ *
+ *  @return			N/A
+ */
+static inline void
+woal_start_queue(struct net_device *dev)
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,29)
+    netif_start_queue(dev);
+#else
+    netif_tx_start_all_queues(dev);
+#endif
+}
+
+/**
+ *  @brief Stop queue
+ *
+ *  @param dev		A pointer to net_device structure
+ *
+ *  @return			N/A
+ */
+static inline void
+woal_stop_queue(struct net_device *dev)
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+    unsigned long flags;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    spin_lock_irqsave(&priv->phandle->queue_lock, flags);
+    woal_set_trans_start(dev);
+    if (!netif_queue_stopped(dev))
+        netif_tx_stop_all_queues(dev);
+    spin_unlock_irqrestore(&priv->phandle->queue_lock, flags);
+#else
+    woal_set_trans_start(dev);
+    if (!netif_queue_stopped(dev))
+        netif_stop_queue(dev);
+#endif
+}
+
+/**
+ *  @brief wake queue
+ *
+ *  @param dev		A pointer to net_device structure
+ *
+ *  @return			N/A
+ */
+static inline void
+woal_wake_queue(struct net_device *dev)
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+    unsigned long flags;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    spin_lock_irqsave(&priv->phandle->queue_lock, flags);
+    if (netif_queue_stopped(dev))
+        netif_tx_wake_all_queues(dev);
+    spin_unlock_irqrestore(&priv->phandle->queue_lock, flags);
+#else
+    if (netif_queue_stopped(dev))
+        netif_wake_queue(dev);
+#endif
+}
+
+/** Max number of char in custom event - use multiple of them if needed */
+#define IW_CUSTOM_MAX		256     /* In bytes */
+
+/** Debug Macro definition*/
+#ifdef	DEBUG_LEVEL1
+extern t_u32 drvdbg;
+
+#ifdef	DEBUG_LEVEL2
+#define	PRINTM_MINFO(msg...)  do {if (drvdbg & MINFO) printk(KERN_DEBUG msg);} while(0)
+#define	PRINTM_MWARN(msg...)  do {if (drvdbg & MWARN) printk(KERN_DEBUG msg);} while(0)
+#define	PRINTM_MENTRY(msg...) do {if (drvdbg & MENTRY) printk(KERN_DEBUG msg);} while(0)
+#else
+#define	PRINTM_MINFO(msg...)  do {} while (0)
+#define	PRINTM_MWARN(msg...)  do {} while (0)
+#define	PRINTM_MENTRY(msg...) do {} while (0)
+#endif /* DEBUG_LEVEL2 */
+
+#define	PRINTM_MFW_D(msg...)  do {if (drvdbg & MFW_D) printk(KERN_DEBUG msg);} while(0)
+#define	PRINTM_MCMD_D(msg...) do {if (drvdbg & MCMD_D) printk(KERN_DEBUG msg);} while(0)
+#define	PRINTM_MDAT_D(msg...) do {if (drvdbg & MDAT_D) printk(KERN_DEBUG msg);} while(0)
+#define	PRINTM_MIF_D(msg...) do {if (drvdbg & MIF_D) printk(KERN_DEBUG msg);} while(0)
+
+#define	PRINTM_MIOCTL(msg...) do {if (drvdbg & MIOCTL) printk(KERN_DEBUG msg);} while(0)
+#define	PRINTM_MINTR(msg...)  do {if (drvdbg & MINTR) printk(KERN_DEBUG msg);} while(0)
+#define	PRINTM_MEVENT(msg...) do {if (drvdbg & MEVENT) printk(msg);} while(0)
+#define	PRINTM_MCMND(msg...)  do {if (drvdbg & MCMND) printk(KERN_DEBUG msg);} while(0)
+#define	PRINTM_MDATA(msg...)  do {if (drvdbg & MDATA) printk(KERN_DEBUG msg);} while(0)
+#define	PRINTM_MERROR(msg...) do {if (drvdbg & MERROR) printk(KERN_ERR msg);} while(0)
+#define	PRINTM_MFATAL(msg...) do {if (drvdbg & MFATAL) printk(KERN_ERR msg);} while(0)
+#define	PRINTM_MMSG(msg...)   do {if (drvdbg & MMSG) printk(KERN_ALERT msg);} while(0)
+
+#define	PRINTM(level,msg...) PRINTM_##level(msg)
+
+#else
+
+#define	PRINTM(level,msg...) do {} while (0)
+
+#endif /* DEBUG_LEVEL1 */
+
+/** Wait until a condition becomes true */
+#define MASSERT(cond)                   \
+do {                                    \
+    if (!(cond)) {                      \
+        PRINTM(MFATAL, "ASSERT: %s: %i\n", __FUNCTION__, __LINE__); \
+        panic("Assert failed: Panic!"); \
+    }                                   \
+} while(0)
+
+/** Log entry point for debugging */
+#define	ENTER()			PRINTM(MENTRY, "Enter: %s\n", \
+                                    __FUNCTION__)
+/** Log exit point for debugging */
+#define	LEAVE()			PRINTM(MENTRY, "Leave: %s\n", \
+                                    __FUNCTION__)
+
+#ifdef DEBUG_LEVEL1
+#define DBG_DUMP_BUF_LEN 	64
+#define MAX_DUMP_PER_LINE	16
+
+static inline void
+hexdump(char *prompt, t_u8 * buf, int len)
+{
+    int i;
+    char dbgdumpbuf[DBG_DUMP_BUF_LEN];
+    char *ptr = dbgdumpbuf;
+
+    printk(KERN_DEBUG "%s:\n", prompt);
+    for (i = 1; i <= len; i++) {
+        ptr += snprintf(ptr, 4, "%02x ", *buf);
+        buf++;
+        if (i % MAX_DUMP_PER_LINE == 0) {
+            *ptr = 0;
+            printk(KERN_DEBUG "%s\n", dbgdumpbuf);
+            ptr = dbgdumpbuf;
+        }
+    }
+    if (len % MAX_DUMP_PER_LINE) {
+        *ptr = 0;
+        printk(KERN_DEBUG "%s\n", dbgdumpbuf);
+    }
+}
+
+#define DBG_HEXDUMP_MERROR(x,y,z)    do {if (drvdbg & MERROR) hexdump(x,y,z);} while(0)
+#define DBG_HEXDUMP_MCMD_D(x,y,z)    do {if (drvdbg & MCMD_D) hexdump(x,y,z);} while(0)
+#define DBG_HEXDUMP_MDAT_D(x,y,z)    do {if (drvdbg & MDAT_D) hexdump(x,y,z);} while(0)
+#define DBG_HEXDUMP_MIF_D(x,y,z)     do {if (drvdbg & MIF_D) hexdump(x,y,z);} while(0)
+#define DBG_HEXDUMP_MEVT_D(x,y,z)    do {if (drvdbg & MEVT_D) hexdump(x,y,z);} while(0)
+#define DBG_HEXDUMP_MFW_D(x,y,z)     do {if (drvdbg & MFW_D) hexdump(x,y,z);} while(0)
+#define	DBG_HEXDUMP(level,x,y,z)    DBG_HEXDUMP_##level(x,y,z)
+
+#else
+/** Do nothing since debugging is not turned on */
+#define DBG_HEXDUMP(level,x,y,z)    do {} while (0)
+#endif
+
+#ifdef DEBUG_LEVEL2
+#define HEXDUMP(x,y,z)              do {if (drvdbg & MINFO) hexdump(x,y,z);} while(0)
+#else
+/** Do nothing since debugging is not turned on */
+#define HEXDUMP(x,y,z)              do {} while (0)
+#endif
+
+#ifdef BIG_ENDIAN_SUPPORT
+/** Convert from 16 bit little endian format to CPU format */
+#define woal_le16_to_cpu(x) le16_to_cpu(x)
+/** Convert from 32 bit little endian format to CPU format */
+#define woal_le32_to_cpu(x) le32_to_cpu(x)
+/** Convert from 64 bit little endian format to CPU format */
+#define woal_le64_to_cpu(x) le64_to_cpu(x)
+/** Convert to 16 bit little endian format from CPU format */
+#define woal_cpu_to_le16(x) cpu_to_le16(x)
+/** Convert to 32 bit little endian format from CPU format */
+#define woal_cpu_to_le32(x) cpu_to_le32(x)
+/** Convert to 64 bit little endian format from CPU format */
+#define woal_cpu_to_le64(x) cpu_to_le64(x)
+#else
+/** Do nothing */
+#define woal_le16_to_cpu(x) x
+/** Do nothing */
+#define woal_le32_to_cpu(x) x
+/** Do nothing */
+#define woal_le64_to_cpu(x) x
+/** Do nothing */
+#define woal_cpu_to_le16(x) x
+/** Do nothing */
+#define woal_cpu_to_le32(x) x
+/** Do nothing */
+#define woal_cpu_to_le64(x) x
+#endif
+
+/**
+ *  @brief This function returns first available priv
+ *  based on the BSS role
+ *
+ *  @param handle    A pointer to moal_handle
+ *  @param bss_role  BSS role or MLAN_BSS_ROLE_ANY
+ *
+ *  @return          Pointer to moal_private
+ */
+static inline moal_private *
+woal_get_priv(moal_handle * handle, mlan_bss_role bss_role)
+{
+    int i;
+
+    for (i = 0; i < MIN(handle->priv_num, MLAN_MAX_BSS_NUM); i++) {
+        if (handle->priv[i]) {
+            if (bss_role == MLAN_BSS_ROLE_ANY ||
+                GET_BSS_ROLE(handle->priv[i]) == bss_role)
+                return (handle->priv[i]);
+        }
+    }
+    return NULL;
+}
+
+/** Max line length allowed in init config file */
+#define MAX_LINE_LEN        256
+/** Max MAC address string length allowed */
+#define MAX_MAC_ADDR_LEN    18
+/** Max register type/offset/value etc. parameter length allowed */
+#define MAX_PARAM_LEN       12
+
+/** HostCmd_CMD_CFG_DATA for CAL data */
+#define HostCmd_CMD_CFG_DATA 0x008f
+/** HostCmd action set */
+#define HostCmd_ACT_GEN_SET 0x0001
+/** HostCmd CAL data header length */
+#define CFG_DATA_HEADER_LEN	6
+
+typedef struct _HostCmd_DS_GEN
+{
+    t_u16 command;
+    t_u16 size;
+    t_u16 seq_num;
+    t_u16 result;
+} HostCmd_DS_GEN;
+
+typedef struct _HostCmd_DS_802_11_CFG_DATA
+{
+    /** Action */
+    t_u16 action;
+    /** Type */
+    t_u16 type;
+    /** Data length */
+    t_u16 data_len;
+    /** Data */
+    t_u8 data[1];
+} __ATTRIB_PACK__ HostCmd_DS_802_11_CFG_DATA;
+
+/** combo scan header */
+#define WEXT_CSCAN_HEADER		"CSCAN S\x01\x00\x00S\x00"
+/** combo scan header size */
+#define WEXT_CSCAN_HEADER_SIZE		12
+/** combo scan ssid section */
+#define WEXT_CSCAN_SSID_SECTION		'S'
+/** commbo scan channel section */
+#define WEXT_CSCAN_CHANNEL_SECTION	'C'
+/** commbo scan passive dwell section */
+#define WEXT_CSCAN_PASV_DWELL_SECTION	'P'
+/** commbo scan home dwell section */
+#define WEXT_CSCAN_HOME_DWELL_SECTION	'H'
+/** BGSCAN RSSI section */
+#define WEXT_BGSCAN_RSSI_SECTION	 'R'
+/** BGSCAN SCAN INTERVAL SECTION */
+#define WEXT_BGSCAN_INTERVAL_SECTION 'T'
+/** BGSCAN REPEAT SECTION */
+#define WEXT_BGSCAN_REPEAT_SECTION  'E'
+
+/** band AUTO */
+#define	WIFI_FREQUENCY_BAND_AUTO		0
+/** band 5G */
+#define	WIFI_FREQUENCY_BAND_5GHZ        1
+/** band 2G */
+#define	WIFI_FREQUENCY_BAND_2GHZ		2
+/** All band */
+#define WIFI_FREQUENCY_ALL_BAND         3
+
+/** Rx filter: IPV4 multicast */
+#define RX_FILTER_IPV4_MULTICAST        1
+/** Rx filter: broadcast */
+#define RX_FILTER_BROADCAST             2
+/** Rx filter: unicast */
+#define RX_FILTER_UNICAST               4
+/** Rx filter: IPV6 multicast */
+#define RX_FILTER_IPV6_MULTICAST        8
+
+/**  Convert ASCII string to hex value */
+int woal_ascii2hex(t_u8 * d, char *s, t_u32 dlen);
+/**  Convert mac address from string to t_u8 buffer */
+void woal_mac2u8(t_u8 * mac_addr, char *buf);
+/**  Extract token from string */
+char *woal_strsep(char **s, char delim, char esc);
+/** Return int value of a given ASCII string */
+mlan_status woal_atoi(int *data, char *a);
+/** Return hex value of a given ASCII string */
+int woal_atox(char *a);
+/** Allocate buffer */
+pmlan_buffer woal_alloc_mlan_buffer(moal_handle * handle, int size);
+/** Allocate IOCTL request buffer */
+pmlan_ioctl_req woal_alloc_mlan_ioctl_req(int size);
+/** Free buffer */
+void woal_free_mlan_buffer(moal_handle * handle, pmlan_buffer pmbuf);
+/** Get private structure of a BSS by index */
+moal_private *woal_bss_index_to_priv(moal_handle * handle, t_u8 bss_index);
+/* Functions in interface module */
+/** Add card */
+moal_handle *woal_add_card(void *card);
+/** Remove card */
+mlan_status woal_remove_card(void *card);
+/** broadcast event */
+mlan_status woal_broadcast_event(moal_private * priv, t_u8 * payload,
+                                 t_u32 len);
+/** switch driver mode */
+mlan_status woal_switch_drv_mode(moal_handle * handle, t_u32 mode);
+
+/** Interrupt handler */
+void woal_interrupt(moal_handle * handle);
+
+#ifdef STA_WEXT
+#endif
+/** Get version */
+void woal_get_version(moal_handle * handle, char *version, int maxlen);
+/** Get Driver Version */
+int woal_get_driver_version(moal_private * priv, struct ifreq *req);
+/** Get extended driver version */
+int woal_get_driver_verext(moal_private * priv, struct ifreq *ireq);
+/** Mgmt frame forward registration */
+int woal_reg_rx_mgmt_ind(moal_private * priv, t_u16 action,
+                         t_u32 * pmgmt_subtype_mask, t_u8 wait_option);
+#ifdef DEBUG_LEVEL1
+/** Set driver debug bit masks */
+int woal_set_drvdbg(moal_private * priv, t_u32 drvdbg);
+#endif
+/** Set/Get TX beamforming configurations */
+mlan_status woal_set_get_tx_bf_cfg(moal_private * priv, t_u16 action,
+                                   mlan_ds_11n_tx_bf_cfg * bf_cfg);
+/** Request MAC address setting */
+mlan_status woal_request_set_mac_address(moal_private * priv);
+/** Request multicast list setting */
+void woal_request_set_multicast_list(moal_private * priv,
+                                     struct net_device *dev);
+/** Request IOCTL action */
+mlan_status woal_request_ioctl(moal_private * priv, mlan_ioctl_req * req,
+                               t_u8 wait_option);
+mlan_status woal_request_soft_reset(moal_handle * handle);
+#ifdef PROC_DEBUG
+/** Get debug information */
+mlan_status woal_get_debug_info(moal_private * priv, t_u8 wait_option,
+                                mlan_debug_info * debug_info);
+/** Set debug information */
+mlan_status woal_set_debug_info(moal_private * priv, t_u8 wait_option,
+                                mlan_debug_info * debug_info);
+#endif
+/** Disconnect */
+mlan_status woal_disconnect(moal_private * priv, t_u8 wait_option, t_u8 * mac);
+/** associate */
+mlan_status woal_bss_start(moal_private * priv, t_u8 wait_option,
+                           mlan_ssid_bssid * ssid_bssid);
+/** Request firmware information */
+mlan_status woal_request_get_fw_info(moal_private * priv, t_u8 wait_option,
+                                     mlan_fw_info * fw_info);
+/** Set/get Host Sleep parameters */
+mlan_status woal_set_get_hs_params(moal_private * priv, t_u16 action,
+                                   t_u8 wait_option, mlan_ds_hs_cfg * hscfg);
+/** Cancel Host Sleep configuration */
+mlan_status woal_cancel_hs(moal_private * priv, t_u8 wait_option);
+#if defined(SDIO_SUSPEND_RESUME)
+/** Enable Host Sleep configuration */
+int woal_enable_hs(moal_private * priv);
+/** hs active timeout 2 second */
+#define HS_ACTIVE_TIMEOUT  (2 * HZ)
+#endif
+
+/** get deep sleep */
+int woal_get_deep_sleep(moal_private * priv, t_u32 * data);
+/** set deep sleep */
+int woal_set_deep_sleep(moal_private * priv, t_u8 wait_option,
+                        BOOLEAN bdeep_sleep, t_u16 idletime);
+
+/** Get BSS information */
+mlan_status woal_get_bss_info(moal_private * priv, t_u8 wait_option,
+                              mlan_bss_info * bss_info);
+void woal_process_ioctl_resp(moal_private * priv, mlan_ioctl_req * req);
+#ifdef STA_SUPPORT
+void woal_send_disconnect_to_system(moal_private * priv);
+void woal_send_mic_error_event(moal_private * priv, t_u32 event);
+void woal_ioctl_get_bss_resp(moal_private * priv, mlan_ds_bss * bss);
+void woal_ioctl_get_info_resp(moal_private * priv, mlan_ds_get_info * info);
+/** Get signal information */
+mlan_status woal_get_signal_info(moal_private * priv, t_u8 wait_option,
+                                 mlan_ds_get_signal * signal);
+#ifdef STA_WEXT
+/** Get mode */
+t_u32 woal_get_mode(moal_private * priv, t_u8 wait_option);
+/** Get data rates */
+mlan_status woal_get_data_rates(moal_private * priv, t_u8 wait_option,
+                                moal_802_11_rates * m_rates);
+void woal_send_iwevcustom_event(moal_private * priv, t_s8 * str);
+/** Get statistics information */
+mlan_status woal_get_stats_info(moal_private * priv, t_u8 wait_option,
+                                mlan_ds_get_stats * stats);
+/** Get channel list */
+mlan_status woal_get_channel_list(moal_private * priv, t_u8 wait_option,
+                                  mlan_chan_list * chanlist);
+#endif
+/** Set/Get retry count */
+mlan_status woal_set_get_retry(moal_private * priv, t_u32 action,
+                               t_u8 wait_option, int *value);
+/** Set/Get RTS threshold */
+mlan_status woal_set_get_rts(moal_private * priv, t_u32 action,
+                             t_u8 wait_option, int *value);
+/** Set/Get fragment threshold */
+mlan_status woal_set_get_frag(moal_private * priv, t_u32 action,
+                              t_u8 wait_option, int *value);
+/** Set/Get generic element */
+mlan_status woal_set_get_gen_ie(moal_private * priv, t_u32 action, t_u8 * ie,
+                                int *ie_len);
+/** Set/Get TX power */
+mlan_status woal_set_get_tx_power(moal_private * priv, t_u32 action,
+                                  mlan_power_cfg_t * pwr);
+/** Set/Get power IEEE management */
+mlan_status woal_set_get_power_mgmt(moal_private * priv, t_u32 action,
+                                    int *disabled, int type);
+/** Get data rate */
+mlan_status woal_set_get_data_rate(moal_private * priv, t_u8 action,
+                                   mlan_rate_cfg_t * datarate);
+/** Request a network scan */
+mlan_status woal_request_scan(moal_private * priv, t_u8 wait_option,
+                              mlan_802_11_ssid * req_ssid);
+/** Set radio on/off */
+int woal_set_radio(moal_private * priv, t_u8 option);
+/** Set region code */
+mlan_status woal_set_region_code(moal_private * priv, char *region);
+/** Set authentication mode */
+mlan_status woal_set_auth_mode(moal_private * priv, t_u8 wait_option,
+                               t_u32 auth_mode);
+/** Set encryption mode */
+mlan_status woal_set_encrypt_mode(moal_private * priv, t_u8 wait_option,
+                                  t_u32 encrypt_mode);
+/** Enable wep key */
+mlan_status woal_enable_wep_key(moal_private * priv, t_u8 wait_option);
+/** Set WPA enable */
+mlan_status woal_set_wpa_enable(moal_private * priv, t_u8 wait_option,
+                                t_u32 enable);
+
+/** Find best network to connect */
+mlan_status woal_find_best_network(moal_private * priv, t_u8 wait_option,
+                                   mlan_ssid_bssid * ssid_bssid);
+/** Set Ad-Hoc channel */
+mlan_status woal_change_adhoc_chan(moal_private * priv, int channel);
+
+/** Get scan table */
+mlan_status woal_get_scan_table(moal_private * priv, t_u8 wait_option,
+                                mlan_scan_resp * scanresp);
+/** Get authentication mode */
+mlan_status woal_get_auth_mode(moal_private * priv, t_u8 wait_option,
+                               t_u32 * auth_mode);
+/** Get encryption mode */
+mlan_status woal_get_encrypt_mode(moal_private * priv, t_u8 wait_option,
+                                  t_u32 * encrypt_mode);
+/** Get WPA state */
+mlan_status woal_get_wpa_enable(moal_private * priv, t_u8 wait_option,
+                                t_u32 * enable);
+#endif  /**STA_SUPPORT */
+
+mlan_status woal_set_wapi_enable(moal_private * priv, t_u8 wait_option,
+                                 t_u32 enable);
+
+/** Initialize priv */
+void woal_init_priv(moal_private * priv, t_u8 wait_option);
+/** Reset interface(s) */
+int woal_reset_intf(moal_private * priv, t_u8 wait_option, int all_intf);
+/** common ioctl for uap, station */
+int woal_custom_ie_ioctl(struct net_device *dev, struct ifreq *req);
+int woal_send_host_packet(struct net_device *dev, struct ifreq *req);
+/** Private command ID to pass mgmt frame */
+#define WOAL_MGMT_FRAME_TX_IOCTL          (SIOCDEVPRIVATE + 12)
+
+int woal_get_bss_type(struct net_device *dev, struct ifreq *req);
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+int woal_host_command(moal_private * priv, struct iwreq *wrq);
+#endif
+#if defined(WIFI_DIRECT_SUPPORT)
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+mlan_status woal_bss_role_cfg(moal_private * priv, t_u8 action,
+                              t_u8 wait_option, t_u8 * bss_role);
+int woal_set_get_bss_role(moal_private * priv, struct iwreq *wrq);
+#endif
+#endif
+#endif
+#if defined(WIFI_DIRECT_SUPPORT) || defined(UAP_SUPPORT)
+/** hostcmd ioctl for uap, wifidirect */
+int woal_hostcmd_ioctl(struct net_device *dev, struct ifreq *req);
+#endif
+
+#if defined(WIFI_DIRECT_SUPPORT)
+mlan_status woal_set_remain_channel_ioctl(moal_private * priv, t_u8 wait_option,
+                                          mlan_ds_remain_chan * pchan);
+mlan_status woal_wifi_direct_mode_cfg(moal_private * priv, t_u16 action,
+                                      t_u16 * mode);
+#endif /* WIFI_DIRECT_SUPPORT */
+
+#ifdef CONFIG_PROC_FS
+/** Initialize proc fs */
+void woal_proc_init(moal_handle * handle);
+/** Clean up proc fs */
+void woal_proc_exit(moal_handle * handle);
+/** Create proc entry */
+void woal_create_proc_entry(moal_private * priv);
+/** Remove proc entry */
+void woal_proc_remove(moal_private * priv);
+/** string to number */
+int woal_string_to_number(char *s);
+#endif
+
+#ifdef PROC_DEBUG
+/** Create debug proc fs */
+void woal_debug_entry(moal_private * priv);
+/** Remove debug proc fs */
+void woal_debug_remove(moal_private * priv);
+#endif /* PROC_DEBUG */
+
+/** check pm info */
+mlan_status woal_get_pm_info(moal_private * priv, mlan_ds_ps_info * pm_info);
+
+#ifdef REASSOCIATION
+int woal_reassociation_thread(void *data);
+void woal_reassoc_timer_func(void *context);
+#endif /* REASSOCIATION */
+
+t_void woal_main_work_queue(struct work_struct *work);
+
+int woal_hard_start_xmit(struct sk_buff *skb, struct net_device *dev);
+moal_private *woal_add_interface(moal_handle * handle, t_u8 bss_num,
+                                 t_u8 bss_type);
+void woal_remove_interface(moal_handle * handle, t_u8 bss_index);
+void woal_set_multicast_list(struct net_device *dev);
+mlan_status woal_request_fw(moal_handle * handle);
+
+int woal_11h_channel_check_ioctl(moal_private * priv);
+void woal_cancel_cac_block(moal_private * priv);
+void woal_moal_debug_info(moal_private * priv, moal_handle * handle, u8 flag);
+
+#ifdef STA_SUPPORT
+mlan_status woal_get_powermode(moal_private * priv, int *powermode);
+mlan_status woal_set_scan_type(moal_private * priv, t_u32 scan_type);
+mlan_status woal_set_powermode(moal_private * priv, char *powermode);
+int woal_find_essid(moal_private * priv, mlan_ssid_bssid * ssid_bssid);
+mlan_status woal_do_scan(moal_private * priv, wlan_user_scan_cfg * scan_cfg);
+int woal_set_combo_scan(moal_private * priv, char *buf, int length);
+mlan_status woal_get_band(moal_private * priv, int *band);
+mlan_status woal_set_band(moal_private * priv, char *pband);
+mlan_status woal_add_rxfilter(moal_private * priv, char *rxfilter);
+mlan_status woal_remove_rxfilter(moal_private * priv, char *rxfilter);
+mlan_status woal_set_qos_cfg(moal_private * priv, char *qos_cfg);
+int woal_set_sleeppd(moal_private * priv, char *psleeppd);
+mlan_status woal_set_rssi_low_threshold(moal_private * priv, char *rssi);
+mlan_status woal_set_rssi_threshold(moal_private * priv, t_u32 event_id);
+mlan_status woal_set_bg_scan(moal_private * priv, char *buf, int length);
+mlan_status woal_stop_bg_scan(moal_private * priv);
+void woal_reconfig_bgscan(moal_handle * handle);
+#endif
+
+struct tcp_sess *woal_get_tcp_sess(moal_private * priv,
+                                   t_u32 src_ip, t_u16 src_port,
+                                   t_u32 dst_ip, t_u16 dst_port);
+void woal_check_tcp_fin(moal_private * priv, struct sk_buff *skb);
+
+#endif /* _MOAL_MAIN_H */
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_priv.c b/drivers/net/wireless/sd8797/mlinux/moal_priv.c
new file mode 100644
index 000000000000..9a5173ddfb3f
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_priv.c
@@ -0,0 +1,6690 @@
+/** @file  moal_priv.c
+  *
+  * @brief This file contains standard ioctl functions
+  *
+  * Copyright (C) 2008-2011, Marvell International Ltd.
+  *
+  * This software file (the "File") is distributed by Marvell International
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+  * (the "License").  You may use, redistribute and/or modify this File in
+  * accordance with the terms and conditions of the License, a copy of which
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+  * this warranty disclaimer.
+  *
+  */
+
+/************************************************************************
+Change log:
+    10/30/2008: initial version
+************************************************************************/
+
+#include	"moal_main.h"
+#include	"moal_sdio.h"
+
+#if defined(STA_CFG80211) && defined(UAP_CFG80211)
+#include    "moal_cfg80211.h"
+#endif
+#include    "moal_eth_ioctl.h"
+
+/********************************************************
+                Local Variables
+********************************************************/
+/** Bands supported in Infra mode */
+static t_u8 SupportedInfraBand[] = {
+    BAND_B,
+    BAND_B | BAND_G, BAND_G,
+    BAND_GN, BAND_B | BAND_G | BAND_GN, BAND_G | BAND_GN,
+    BAND_A, BAND_B | BAND_A, BAND_B | BAND_G | BAND_A, BAND_G | BAND_A,
+    BAND_A | BAND_B | BAND_G | BAND_AN | BAND_GN,
+        BAND_A | BAND_G | BAND_AN | BAND_GN, BAND_A | BAND_AN,
+};
+
+/** Bands supported in Ad-Hoc mode */
+static t_u8 SupportedAdhocBand[] = {
+    BAND_B, BAND_B | BAND_G, BAND_G,
+    BAND_GN, BAND_B | BAND_G | BAND_GN, BAND_G | BAND_GN,
+    BAND_A,
+    BAND_AN, BAND_A | BAND_AN,
+};
+
+/********************************************************
+		Global Variables
+********************************************************/
+
+extern int cfg80211_wext;
+
+/********************************************************
+		Local Functions
+********************************************************/
+
+/**
+ *  @brief Copy Rates
+ *
+ *  @param dest    A pointer to destination buffer
+ *  @param pos     The position for copy
+ *  @param src     A pointer to source buffer
+ *  @param len     Length of the source buffer
+ *
+ *  @return        Number of rates copied
+ */
+static inline int
+woal_copy_rates(t_u8 * dest, int pos, t_u8 * src, int len)
+{
+    int i;
+
+    for (i = 0; i < len && src[i]; i++, pos++) {
+        if (pos >= MLAN_SUPPORTED_RATES)
+            break;
+        dest[pos] = src[i];
+    }
+    return pos;
+}
+
+/**
+ *  @brief Performs warm reset
+ *
+ *  @param priv         A pointer to moal_private structure
+ *
+ *  @return             0/MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static int
+woal_warm_reset(moal_private * priv)
+{
+    int ret = 0;
+    int intf_num;
+    moal_handle *handle = priv->phandle;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+#if defined(WIFI_DIRECT_SUPPORT)
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+    t_u8 bss_role = MLAN_BSS_ROLE_STA;
+#endif
+#endif
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+
+    ENTER();
+
+    woal_cancel_cac_block(priv);
+
+    /* Reset all interfaces */
+    ret = woal_reset_intf(priv, MOAL_IOCTL_WAIT, MTRUE);
+
+    /* Initialize private structures */
+    for (intf_num = 0; intf_num < handle->priv_num; intf_num++) {
+        woal_init_priv(handle->priv[intf_num], MOAL_IOCTL_WAIT);
+#if defined(WIFI_DIRECT_SUPPORT)
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+        if (handle->priv[intf_num]->bss_type == MLAN_BSS_TYPE_WIFIDIRECT) {
+            if (MLAN_STATUS_SUCCESS != woal_bss_role_cfg(handle->priv[intf_num],
+                                                         MLAN_ACT_SET,
+                                                         MOAL_IOCTL_WAIT,
+                                                         &bss_role)) {
+                ret = -EFAULT;
+                goto done;
+            }
+        }
+#endif /* STA_WEXT || UAP_WEXT */
+#endif /* STA_SUPPORT && UAP_SUPPORT */
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+    }
+
+    /* Restart the firmware */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req) {
+        misc = (mlan_ds_misc_cfg *) req->pbuf;
+        misc->sub_command = MLAN_OID_MISC_WARM_RESET;
+        req->req_id = MLAN_IOCTL_MISC_CFG;
+        req->action = MLAN_ACT_SET;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            kfree(req);
+            goto done;
+        }
+        kfree(req);
+    }
+
+    /* Enable interfaces */
+    for (intf_num = 0; intf_num < handle->priv_num; intf_num++) {
+        netif_device_attach(handle->priv[intf_num]->netdev);
+        woal_start_queue(handle->priv[intf_num]->netdev);
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get signal
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wrq	        A pointer to iwreq structure
+ *
+ *  @return 	  	0 --success, otherwise fail
+ */
+static int
+woal_get_signal(moal_private * priv, struct iwreq *wrq)
+{
+/** Input data size */
+#define IN_DATA_SIZE	2
+/** Output data size */
+#define OUT_DATA_SIZE	12
+    int ret = 0;
+    int in_data[IN_DATA_SIZE];
+    int out_data[OUT_DATA_SIZE];
+    mlan_ds_get_signal signal;
+    int data_length = 0;
+    int buflen = 0;
+
+    ENTER();
+
+    memset(in_data, 0, sizeof(in_data));
+    memset(out_data, 0, sizeof(out_data));
+    buflen = MIN(wrq->u.data.length, IN_DATA_SIZE);
+
+    if (priv->media_connected == MFALSE) {
+        PRINTM(MERROR, "Can not get RSSI in disconnected state\n");
+        ret = -ENOTSUPP;
+        goto done;
+    }
+
+    if (wrq->u.data.length) {
+        if (sizeof(int) * wrq->u.data.length > sizeof(in_data)) {
+            PRINTM(MERROR, "Too many arguments\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        if (copy_from_user(in_data, wrq->u.data.pointer, sizeof(int) * buflen)) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+    switch (wrq->u.data.length) {
+    case 0:                    /* No checking, get everything */
+        break;
+    case 2:                    /* Check subtype range */
+        if (in_data[1] < 1 || in_data[1] > 4) {
+            ret = -EINVAL;
+            goto done;
+        }
+        /* Fall through */
+    case 1:                    /* Check type range */
+        if (in_data[0] < 1 || in_data[0] > 3) {
+            ret = -EINVAL;
+            goto done;
+        }
+        break;
+    default:
+        ret = -EINVAL;
+        goto done;
+    }
+
+    memset(&signal, 0, sizeof(mlan_ds_get_signal));
+    if (MLAN_STATUS_SUCCESS !=
+        woal_get_signal_info(priv, MOAL_IOCTL_WAIT, &signal)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    PRINTM(MINFO, "RSSI Beacon Last   : %d\n", (int) signal.bcn_rssi_last);
+    PRINTM(MINFO, "RSSI Beacon Average: %d\n", (int) signal.bcn_rssi_avg);
+    PRINTM(MINFO, "RSSI Data Last     : %d\n", (int) signal.data_rssi_last);
+    PRINTM(MINFO, "RSSI Data Average  : %d\n", (int) signal.data_rssi_avg);
+    PRINTM(MINFO, "SNR Beacon Last    : %d\n", (int) signal.bcn_snr_last);
+    PRINTM(MINFO, "SNR Beacon Average : %d\n", (int) signal.bcn_snr_avg);
+    PRINTM(MINFO, "SNR Data Last      : %d\n", (int) signal.data_snr_last);
+    PRINTM(MINFO, "SNR Data Average   : %d\n", (int) signal.data_snr_avg);
+    PRINTM(MINFO, "NF Beacon Last     : %d\n", (int) signal.bcn_nf_last);
+    PRINTM(MINFO, "NF Beacon Average  : %d\n", (int) signal.bcn_nf_avg);
+    PRINTM(MINFO, "NF Data Last       : %d\n", (int) signal.data_nf_last);
+    PRINTM(MINFO, "NF Data Average    : %d\n", (int) signal.data_nf_avg);
+
+    /* Check type */
+    switch (in_data[0]) {
+    case 0:                    /* Send everything */
+        out_data[data_length++] = signal.bcn_rssi_last;
+        out_data[data_length++] = signal.bcn_rssi_avg;
+        out_data[data_length++] = signal.data_rssi_last;
+        out_data[data_length++] = signal.data_rssi_avg;
+        out_data[data_length++] = signal.bcn_snr_last;
+        out_data[data_length++] = signal.bcn_snr_avg;
+        out_data[data_length++] = signal.data_snr_last;
+        out_data[data_length++] = signal.data_snr_avg;
+        out_data[data_length++] = signal.bcn_nf_last;
+        out_data[data_length++] = signal.bcn_nf_avg;
+        out_data[data_length++] = signal.data_nf_last;
+        out_data[data_length++] = signal.data_nf_avg;
+        break;
+    case 1:                    /* RSSI */
+        /* Check subtype */
+        switch (in_data[1]) {
+        case 0:                /* Everything */
+            out_data[data_length++] = signal.bcn_rssi_last;
+            out_data[data_length++] = signal.bcn_rssi_avg;
+            out_data[data_length++] = signal.data_rssi_last;
+            out_data[data_length++] = signal.data_rssi_avg;
+            break;
+        case 1:                /* bcn last */
+            out_data[data_length++] = signal.bcn_rssi_last;
+            break;
+        case 2:                /* bcn avg */
+            out_data[data_length++] = signal.bcn_rssi_avg;
+            break;
+        case 3:                /* data last */
+            out_data[data_length++] = signal.data_rssi_last;
+            break;
+        case 4:                /* data avg */
+            out_data[data_length++] = signal.data_rssi_avg;
+            break;
+        default:
+            break;
+        }
+        break;
+    case 2:                    /* SNR */
+        /* Check subtype */
+        switch (in_data[1]) {
+        case 0:                /* Everything */
+            out_data[data_length++] = signal.bcn_snr_last;
+            out_data[data_length++] = signal.bcn_snr_avg;
+            out_data[data_length++] = signal.data_snr_last;
+            out_data[data_length++] = signal.data_snr_avg;
+            break;
+        case 1:                /* bcn last */
+            out_data[data_length++] = signal.bcn_snr_last;
+            break;
+        case 2:                /* bcn avg */
+            out_data[data_length++] = signal.bcn_snr_avg;
+            break;
+        case 3:                /* data last */
+            out_data[data_length++] = signal.data_snr_last;
+            break;
+        case 4:                /* data avg */
+            out_data[data_length++] = signal.data_snr_avg;
+            break;
+        default:
+            break;
+        }
+        break;
+    case 3:                    /* NF */
+        /* Check subtype */
+        switch (in_data[1]) {
+        case 0:                /* Everything */
+            out_data[data_length++] = signal.bcn_nf_last;
+            out_data[data_length++] = signal.bcn_nf_avg;
+            out_data[data_length++] = signal.data_nf_last;
+            out_data[data_length++] = signal.data_nf_avg;
+            break;
+        case 1:                /* bcn last */
+            out_data[data_length++] = signal.bcn_nf_last;
+            break;
+        case 2:                /* bcn avg */
+            out_data[data_length++] = signal.bcn_nf_avg;
+            break;
+        case 3:                /* data last */
+            out_data[data_length++] = signal.data_nf_last;
+            break;
+        case 4:                /* data avg */
+            out_data[data_length++] = signal.data_nf_avg;
+            break;
+        default:
+            break;
+        }
+        break;
+    default:
+        break;
+    }
+
+    wrq->u.data.length = data_length;
+    if (copy_to_user(wrq->u.data.pointer, out_data,
+                     wrq->u.data.length * sizeof(out_data[0]))) {
+        PRINTM(MERROR, "Copy to user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get/Set DeepSleep mode
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wreq	    A pointer to iwreq structure
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_deep_sleep_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    t_u32 deep_sleep = DEEP_SLEEP_OFF;
+    t_u32 data[2];
+    t_u16 idletime = DEEP_SLEEP_IDLE_TIME;
+
+    ENTER();
+
+    if (wrq->u.data.length == 1 || wrq->u.data.length == 2) {
+        if (copy_from_user
+            (&data, wrq->u.data.pointer, wrq->u.data.length * sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            LEAVE();
+            return -EFAULT;
+        }
+        deep_sleep = data[0];
+        if (deep_sleep == DEEP_SLEEP_OFF) {
+            PRINTM(MINFO, "Exit Deep Sleep Mode\n");
+            ret = woal_set_deep_sleep(priv, MOAL_IOCTL_WAIT, MFALSE, 0);
+            if (ret != MLAN_STATUS_SUCCESS) {
+                LEAVE();
+                return -EINVAL;
+            }
+        } else if (deep_sleep == DEEP_SLEEP_ON) {
+            PRINTM(MINFO, "Enter Deep Sleep Mode\n");
+            if (wrq->u.data.length == 2)
+                idletime = data[1];
+            else
+                idletime = 0;
+            ret = woal_set_deep_sleep(priv, MOAL_IOCTL_WAIT, MTRUE, idletime);
+            if (ret != MLAN_STATUS_SUCCESS) {
+                LEAVE();
+                return -EINVAL;
+            }
+        } else {
+            PRINTM(MERROR, "Unknown option = %u\n", deep_sleep);
+            LEAVE();
+            return -EINVAL;
+        }
+    } else if (wrq->u.data.length > 2) {
+        PRINTM(MERROR, "Invalid number of arguments %d\n", wrq->u.data.length);
+        LEAVE();
+        return -EINVAL;
+    } else {                    /* Display Deep Sleep settings */
+        PRINTM(MINFO, "Get Deep Sleep Mode\n");
+        if (MLAN_STATUS_SUCCESS != woal_get_deep_sleep(priv, data)) {
+            LEAVE();
+            return -EFAULT;
+        }
+        if (data[0] == 0)
+            wrq->u.data.length = 1;
+        else
+            wrq->u.data.length = 2;
+    }
+
+    /* Copy the Deep Sleep setting to user */
+    if (copy_to_user
+        (wrq->u.data.pointer, data, wrq->u.data.length * sizeof(int))) {
+        PRINTM(MERROR, "Copy to user failed\n");
+        LEAVE();
+        return -EINVAL;
+    }
+
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Set/Get Usr 11n configuration request
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wrq	    A pointer to iwreq structure
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_11n_htcap_cfg(moal_private * priv, struct iwreq *wrq)
+{
+    int data[2];
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_11n_cfg *cfg_11n = NULL;
+    int ret = 0;
+    int data_length = wrq->u.data.length;
+
+    ENTER();
+
+    if (data_length > 2) {
+        PRINTM(MERROR, "Invalid number of arguments\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    if (((req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11n_cfg))) == NULL)) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    cfg_11n = (mlan_ds_11n_cfg *) req->pbuf;
+    cfg_11n->sub_command = MLAN_OID_11N_HTCAP_CFG;
+    req->req_id = MLAN_IOCTL_11N_CFG;
+
+    if (data_length == 0) {
+        /* Get 11n tx parameters from MLAN */
+        req->action = MLAN_ACT_GET;
+        cfg_11n->param.htcap_cfg.misc_cfg = BAND_SELECT_BG;
+    } else {
+        if (copy_from_user
+            (data, wrq->u.data.pointer, data_length * sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+
+        cfg_11n->param.htcap_cfg.htcap = data[0];
+        PRINTM(MINFO, "SET: htcapinfo:0x%x\n", data[0]);
+        cfg_11n->param.htcap_cfg.misc_cfg = BAND_SELECT_BOTH;
+        if (data_length == 2) {
+            if (data[1] != BAND_SELECT_BG &&
+                data[1] != BAND_SELECT_A && data[1] != BAND_SELECT_BOTH) {
+                PRINTM(MERROR, "Invalid band selection\n");
+                ret = -EINVAL;
+                goto done;
+            }
+            cfg_11n->param.htcap_cfg.misc_cfg = data[1];
+            PRINTM(MINFO, "SET: htcapinfo band:0x%x\n", data[1]);
+        }
+        /* Update 11n tx parameters in MLAN */
+        req->action = MLAN_ACT_SET;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    data[0] = cfg_11n->param.htcap_cfg.htcap;
+
+    if (req->action == MLAN_ACT_GET) {
+        data_length = 1;
+        cfg_11n->param.htcap_cfg.htcap = 0;
+        cfg_11n->param.htcap_cfg.misc_cfg = BAND_SELECT_A;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        if (cfg_11n->param.htcap_cfg.htcap != data[0]) {
+            data_length = 2;
+            data[1] = cfg_11n->param.htcap_cfg.htcap;
+            PRINTM(MINFO, "GET: htcapinfo for 2.4GHz:0x%x\n", data[0]);
+            PRINTM(MINFO, "GET: htcapinfo for 5GHz:0x%x\n", data[1]);
+        } else
+            PRINTM(MINFO, "GET: htcapinfo:0x%x\n", data[0]);
+    }
+
+    if (copy_to_user(wrq->u.data.pointer, data, sizeof(data))) {
+        PRINTM(MERROR, "Copy to user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    wrq->u.data.length = data_length;
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Enable/Disable amsdu_aggr_ctrl
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wrq	    A pointer to iwreq structure
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_11n_amsdu_aggr_ctrl(moal_private * priv, struct iwreq *wrq)
+{
+    int data[2];
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_11n_cfg *cfg_11n = NULL;
+    int ret = 0;
+
+    ENTER();
+
+    if ((wrq->u.data.length != 0) && (wrq->u.data.length != 1)) {
+        PRINTM(MERROR, "Invalid number of arguments\n");
+        ret = -EINVAL;
+        goto done;
+    }
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11n_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    cfg_11n = (mlan_ds_11n_cfg *) req->pbuf;
+    cfg_11n->sub_command = MLAN_OID_11N_CFG_AMSDU_AGGR_CTRL;
+    req->req_id = MLAN_IOCTL_11N_CFG;
+
+    if (wrq->u.data.length == 0) {
+        /* Get 11n tx parameters from MLAN */
+        req->action = MLAN_ACT_GET;
+    } else if (wrq->u.data.length == 1) {
+        if (copy_from_user(data, wrq->u.data.pointer,
+                           wrq->u.data.length * sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        cfg_11n->param.amsdu_aggr_ctrl.enable = data[0];
+        /* Update 11n tx parameters in MLAN */
+        req->action = MLAN_ACT_SET;
+    }
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    data[0] = cfg_11n->param.amsdu_aggr_ctrl.enable;
+    data[1] = cfg_11n->param.amsdu_aggr_ctrl.curr_buf_size;
+
+    if (copy_to_user(wrq->u.data.pointer, data, sizeof(data))) {
+        PRINTM(MERROR, "Copy to user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    wrq->u.data.length = 2;
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get 11n configuration request
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wrq	    A pointer to iwreq structure
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_11n_tx_cfg(moal_private * priv, struct iwreq *wrq)
+{
+    int data[2];
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_11n_cfg *cfg_11n = NULL;
+    int ret = 0;
+    int data_length = wrq->u.data.length;
+
+    ENTER();
+
+    if (data_length > 2) {
+        PRINTM(MERROR, "Invalid number of arguments\n");
+        ret = -EINVAL;
+        goto done;
+    }
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11n_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    cfg_11n = (mlan_ds_11n_cfg *) req->pbuf;
+    cfg_11n->sub_command = MLAN_OID_11N_CFG_TX;
+    req->req_id = MLAN_IOCTL_11N_CFG;
+
+    if (wrq->u.data.length == 0) {
+        /* Get 11n tx parameters from MLAN */
+        req->action = MLAN_ACT_GET;
+        cfg_11n->param.tx_cfg.misc_cfg = BAND_SELECT_BG;
+    } else {
+        if (copy_from_user
+            (data, wrq->u.data.pointer, data_length * sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+
+        cfg_11n->param.tx_cfg.httxcap = data[0];
+        PRINTM(MINFO, "SET: httxcap:0x%x\n", data[0]);
+        cfg_11n->param.tx_cfg.misc_cfg = BAND_SELECT_BOTH;
+        if (data_length == 2) {
+            if (data[1] != BAND_SELECT_BG &&
+                data[1] != BAND_SELECT_A && data[1] != BAND_SELECT_BOTH) {
+                PRINTM(MERROR, "Invalid band selection\n");
+                ret = -EINVAL;
+                goto done;
+            }
+            cfg_11n->param.tx_cfg.misc_cfg = data[1];
+            PRINTM(MINFO, "SET: httxcap band:0x%x\n", data[1]);
+        }
+        /* Update 11n tx parameters in MLAN */
+        req->action = MLAN_ACT_SET;
+    }
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    data[0] = cfg_11n->param.tx_cfg.httxcap;
+
+    if (req->action == MLAN_ACT_GET) {
+        data_length = 1;
+        cfg_11n->param.tx_cfg.httxcap = 0;
+        cfg_11n->param.tx_cfg.misc_cfg = BAND_SELECT_A;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        if (cfg_11n->param.tx_cfg.httxcap != data[0]) {
+            data_length = 2;
+            data[1] = cfg_11n->param.tx_cfg.httxcap;
+            PRINTM(MINFO, "GET: httxcap for 2.4GHz:0x%x\n", data[0]);
+            PRINTM(MINFO, "GET: httxcap for 5GHz:0x%x\n", data[1]);
+        } else
+            PRINTM(MINFO, "GET: httxcap:0x%x\n", data[0]);
+    }
+
+    if (copy_to_user(wrq->u.data.pointer, data, sizeof(data))) {
+        PRINTM(MERROR, "Copy to user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    wrq->u.data.length = data_length;
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Enable/Disable TX Aggregation
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wrq	    A pointer to iwreq structure
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_11n_prio_tbl(moal_private * priv, struct iwreq *wrq)
+{
+    int data[MAX_NUM_TID * 2], i, j;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_11n_cfg *cfg_11n = NULL;
+    int ret = 0;
+
+    ENTER();
+
+    if ((wrq->u.data.pointer == NULL)) {
+        LEAVE();
+        return -EINVAL;
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11n_cfg));
+    if (req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+    cfg_11n = (mlan_ds_11n_cfg *) req->pbuf;
+    cfg_11n->sub_command = MLAN_OID_11N_CFG_AGGR_PRIO_TBL;
+    req->req_id = MLAN_IOCTL_11N_CFG;
+
+    if (wrq->u.data.length == 0) {
+        /* Get aggr priority table from MLAN */
+        req->action = MLAN_ACT_GET;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto error;
+        }
+        wrq->u.data.length = MAX_NUM_TID * 2;
+        for (i = 0, j = 0; i < (wrq->u.data.length); i = i + 2, ++j) {
+            data[i] = cfg_11n->param.aggr_prio_tbl.ampdu[j];
+            data[i + 1] = cfg_11n->param.aggr_prio_tbl.amsdu[j];
+        }
+
+        if (copy_to_user(wrq->u.data.pointer, data,
+                         sizeof(int) * wrq->u.data.length)) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto error;
+        }
+    } else if (wrq->u.data.length == 16) {
+        if (copy_from_user(data, wrq->u.data.pointer,
+                           sizeof(int) * wrq->u.data.length)) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto error;
+        }
+        for (i = 0, j = 0; i < (wrq->u.data.length); i = i + 2, ++j) {
+            if ((data[i] > 7 && data[i] != 0xff) ||
+                (data[i + 1] > 7 && data[i + 1] != 0xff)) {
+                PRINTM(MERROR, "Invalid priority, valid value 0-7 or 0xff.\n");
+                ret = -EFAULT;
+                goto error;
+            }
+            cfg_11n->param.aggr_prio_tbl.ampdu[j] = data[i];
+            cfg_11n->param.aggr_prio_tbl.amsdu[j] = data[i + 1];
+        }
+
+        /* Update aggr priority table in MLAN */
+        req->action = MLAN_ACT_SET;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto error;
+        }
+    } else {
+        PRINTM(MERROR, "Invalid number of arguments\n");
+        ret = -EINVAL;
+        goto error;
+    }
+
+  error:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get add BA Reject parameters
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wrq	    A pointer to iwreq structure
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_addba_reject(moal_private * priv, struct iwreq *wrq)
+{
+    int data[MAX_NUM_TID], ret = 0, i;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_11n_cfg *cfg_11n = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11n_cfg));
+    if (req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+    cfg_11n = (mlan_ds_11n_cfg *) req->pbuf;
+    cfg_11n->sub_command = MLAN_OID_11N_CFG_ADDBA_REJECT;
+    req->req_id = MLAN_IOCTL_11N_CFG;
+
+    if (wrq->u.data.length == 0) {
+        PRINTM(MERROR, "Addba reject moal\n");
+        /* Get aggr priority table from MLAN */
+        req->action = MLAN_ACT_GET;
+        if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req,
+                                                      MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto error;
+        }
+
+        wrq->u.data.length = MAX_NUM_TID;
+        for (i = 0; i < (wrq->u.data.length); ++i) {
+            data[i] = cfg_11n->param.addba_reject[i];
+        }
+
+        if (copy_to_user(wrq->u.data.pointer, data,
+                         sizeof(int) * wrq->u.data.length)) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto error;
+        }
+    } else if (wrq->u.data.length == 8) {
+        if (copy_from_user(data, wrq->u.data.pointer,
+                           sizeof(int) * wrq->u.data.length)) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto error;
+        }
+        for (i = 0; i < (wrq->u.data.length); ++i) {
+            if (data[i] != 0 && data[i] != 1) {
+                PRINTM(MERROR, "addba reject only takes argument as 0 or 1\n");
+                ret = -EFAULT;
+                goto error;
+            }
+            cfg_11n->param.addba_reject[i] = data[i];
+        }
+
+        /* Update aggr priority table in MLAN */
+        req->action = MLAN_ACT_SET;
+        if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req,
+                                                      MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto error;
+        }
+    } else {
+        PRINTM(MERROR, "Invalid number of arguments\n");
+        ret = -EINVAL;
+        goto error;
+    }
+  error:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get add BA parameters
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wrq	    A pointer to iwreq structure
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_addba_para_updt(moal_private * priv, struct iwreq *wrq)
+{
+    int data[5], ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_11n_cfg *cfg_11n = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11n_cfg));
+    if (req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+    cfg_11n = (mlan_ds_11n_cfg *) req->pbuf;
+    cfg_11n->sub_command = MLAN_OID_11N_CFG_ADDBA_PARAM;
+    req->req_id = MLAN_IOCTL_11N_CFG;
+
+    if (wrq->u.data.length == 0) {
+        /* Get Add BA parameters from MLAN */
+        req->action = MLAN_ACT_GET;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto error;
+        }
+        data[0] = cfg_11n->param.addba_param.timeout;
+        data[1] = cfg_11n->param.addba_param.txwinsize;
+        data[2] = cfg_11n->param.addba_param.rxwinsize;
+        data[3] = cfg_11n->param.addba_param.txamsdu;
+        data[4] = cfg_11n->param.addba_param.rxamsdu;
+        PRINTM(MINFO,
+               "GET: timeout:%d txwinsize:%d rxwinsize:%d txamsdu=%d, rxamsdu=%d\n",
+               data[0], data[1], data[2], data[3], data[4]);
+        wrq->u.data.length = 5;
+        if (copy_to_user(wrq->u.data.pointer, data,
+                         wrq->u.data.length * sizeof(int))) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto error;
+        }
+    } else if (wrq->u.data.length == 5) {
+        if (copy_from_user
+            (data, wrq->u.data.pointer, wrq->u.data.length * sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto error;
+        }
+        if (data[0] < 0 || data[0] > MLAN_DEFAULT_BLOCK_ACK_TIMEOUT) {
+            PRINTM(MERROR, "Incorrect addba timeout value.\n");
+            ret = -EFAULT;
+            goto error;
+        }
+        if (data[1] <= 0 || data[1] > MLAN_AMPDU_MAX_TXWINSIZE || data[2] <= 0
+            || data[2] > MLAN_AMPDU_MAX_RXWINSIZE) {
+            PRINTM(MERROR, "Incorrect Tx/Rx window size.\n");
+            ret = -EFAULT;
+            goto error;
+        }
+        cfg_11n->param.addba_param.timeout = data[0];
+        cfg_11n->param.addba_param.txwinsize = data[1];
+        cfg_11n->param.addba_param.rxwinsize = data[2];
+        if (data[3] < 0 || data[3] > 1 || data[4] < 0 || data[4] > 1) {
+            PRINTM(MERROR, "Incorrect Tx/Rx amsdu.\n");
+            ret = -EFAULT;
+            goto error;
+        }
+        cfg_11n->param.addba_param.txamsdu = data[3];
+        cfg_11n->param.addba_param.rxamsdu = data[4];
+        PRINTM(MINFO,
+               "SET: timeout:%d txwinsize:%d rxwinsize:%d txamsdu=%d rxamsdu=%d\n",
+               data[0], data[1], data[2], data[3], data[4]);
+        /* Update Add BA parameters in MLAN */
+        req->action = MLAN_ACT_SET;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = MLAN_STATUS_FAILURE;
+            goto error;
+        }
+    } else {
+        PRINTM(MERROR, "Invalid number of arguments\n");
+        ret = -EINVAL;
+        goto error;
+    }
+
+  error:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get Transmit buffer size
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wrq	    A pointer to iwreq structure
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_txbuf_cfg(moal_private * priv, struct iwreq *wrq)
+{
+    int buf_size;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_11n_cfg *cfg_11n = NULL;
+    int ret = 0;
+
+    ENTER();
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11n_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    cfg_11n = (mlan_ds_11n_cfg *) req->pbuf;
+    cfg_11n->sub_command = MLAN_OID_11N_CFG_MAX_TX_BUF_SIZE;
+    req->req_id = MLAN_IOCTL_11N_CFG;
+
+    if (wrq->u.data.length == 0) {
+        /* Get Tx buffer size from MLAN */
+        req->action = MLAN_ACT_GET;
+    } else {
+        ret = -EINVAL;
+        PRINTM(MERROR,
+               "Don't support set Tx buffer size after driver loaded!\n");
+        goto done;
+    }
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    buf_size = cfg_11n->param.tx_buf_size;
+    if (copy_to_user(wrq->u.data.pointer, &buf_size, sizeof(buf_size))) {
+        PRINTM(MERROR, "Copy to user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    wrq->u.data.length = 1;
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get Host Sleep configuration
+ *
+ *  @param priv             A pointer to moal_private structure
+ *  @param wrq	            A pointer to iwreq structure
+ *  @param invoke_hostcmd	MTRUE --invoke HostCmd, otherwise MFALSE
+ *
+ *  @return             0 --success, otherwise fail
+ */
+static int
+woal_hs_cfg(moal_private * priv, struct iwreq *wrq, BOOLEAN invoke_hostcmd)
+{
+    int data[3];
+    int ret = 0;
+    mlan_ds_hs_cfg hscfg;
+    t_u16 action;
+    mlan_bss_info bss_info;
+    int data_length = wrq->u.data.length;
+
+    ENTER();
+
+    memset(data, 0, sizeof(data));
+    memset(&hscfg, 0, sizeof(mlan_ds_hs_cfg));
+
+    if (data_length == 0) {
+        action = MLAN_ACT_GET;
+    } else {
+        action = MLAN_ACT_SET;
+        if (data_length >= 1 && data_length <= 3) {
+            if (copy_from_user
+                (data, wrq->u.data.pointer, sizeof(int) * data_length)) {
+                PRINTM(MERROR, "Copy from user failed\n");
+                ret = -EFAULT;
+                goto done;
+            }
+        } else {
+            PRINTM(MERROR, "Invalid arguments\n");
+            ret = -EINVAL;
+            goto done;
+        }
+    }
+
+    /* HS config is blocked if HS is already activated */
+    if (data_length &&
+        (data[0] != HOST_SLEEP_CFG_CANCEL || invoke_hostcmd == MFALSE)) {
+        memset(&bss_info, 0, sizeof(bss_info));
+        woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info);
+        if (bss_info.is_hs_configured) {
+            PRINTM(MERROR, "HS already configured\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+    /* Do a GET first if some arguments are not provided */
+    if (data_length >= 1 && data_length < 3) {
+        woal_set_get_hs_params(priv, MLAN_ACT_GET, MOAL_IOCTL_WAIT, &hscfg);
+    }
+
+    if (data_length)
+        hscfg.conditions = data[0];
+    if (data_length >= 2)
+        hscfg.gpio = data[1];
+    if (data_length == 3)
+        hscfg.gap = data[2];
+
+    if ((invoke_hostcmd == MTRUE) && (action == MLAN_ACT_SET)) {
+        /* Need to issue an extra IOCTL first to set up parameters */
+        hscfg.is_invoke_hostcmd = MFALSE;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_get_hs_params(priv, MLAN_ACT_SET, MOAL_IOCTL_WAIT,
+                                   &hscfg)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+    hscfg.is_invoke_hostcmd = invoke_hostcmd;
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_hs_params(priv, action, MOAL_IOCTL_WAIT, &hscfg)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (action == MLAN_ACT_GET) {
+        data[0] = hscfg.conditions;
+        data[1] = hscfg.gpio;
+        data[2] = hscfg.gap;
+        wrq->u.data.length = 3;
+        if (copy_to_user
+            (wrq->u.data.pointer, data, sizeof(int) * wrq->u.data.length)) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set Host Sleep parameters
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wrq	        A pointer to iwreq structure
+ *
+ *  @return             0 --success, otherwise fail
+ */
+static int
+woal_hs_setpara(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    int data_length = wrq->u.data.length;
+
+    ENTER();
+
+    if (data_length >= 1 && data_length <= 3) {
+        ret = woal_hs_cfg(priv, wrq, MFALSE);
+        goto done;
+    } else {
+        PRINTM(MERROR, "Invalid arguments\n");
+        ret = -EINVAL;
+        goto done;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get/Set inactivity timeout extend
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wrq	        A pointer to iwreq structure
+ *
+ *  @return             0 --success, otherwise fail
+ */
+static int
+woal_inactivity_timeout_ext(moal_private * priv, struct iwreq *wrq)
+{
+    int data[4];
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_pm_cfg *pmcfg = NULL;
+    pmlan_ds_inactivity_to inac_to = NULL;
+    int data_length = wrq->u.data.length;
+
+    ENTER();
+
+    memset(data, 0, sizeof(data));
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_pm_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    pmcfg = (mlan_ds_pm_cfg *) req->pbuf;
+    inac_to = &pmcfg->param.inactivity_to;
+    pmcfg->sub_command = MLAN_OID_PM_CFG_INACTIVITY_TO;
+    req->req_id = MLAN_IOCTL_PM_CFG;
+
+    if ((data_length != 0 && data_length != 3 && data_length != 4) ||
+        sizeof(int) * data_length > sizeof(data)) {
+        PRINTM(MERROR, "Invalid number of parameters\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    req->action = MLAN_ACT_GET;
+    if (data_length) {
+        if (copy_from_user
+            (data, wrq->u.data.pointer, sizeof(int) * data_length)) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        req->action = MLAN_ACT_SET;
+        inac_to->timeout_unit = data[0];
+        inac_to->unicast_timeout = data[1];
+        inac_to->mcast_timeout = data[2];
+        inac_to->ps_entry_timeout = data[3];
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Copy back current values regardless of GET/SET */
+    data[0] = inac_to->timeout_unit;
+    data[1] = inac_to->unicast_timeout;
+    data[2] = inac_to->mcast_timeout;
+    data[3] = inac_to->ps_entry_timeout;
+
+    if (req->action == MLAN_ACT_GET) {
+        wrq->u.data.length = 4;
+        if (copy_to_user
+            (wrq->u.data.pointer, data, wrq->u.data.length * sizeof(int))) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get system clock
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wrq	        A pointer to iwreq structure
+ *
+ *  @return             0 --success, otherwise fail
+ */
+static int
+woal_ecl_sys_clock(moal_private * priv, struct iwreq *wrq)
+{
+    int data[64];
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *cfg = NULL;
+    int data_length = wrq->u.data.length;
+    int i = 0;
+
+    ENTER();
+
+    memset(data, 0, sizeof(data));
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    cfg = (mlan_ds_misc_cfg *) req->pbuf;
+    cfg->sub_command = MLAN_OID_MISC_SYS_CLOCK;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+
+    if (!data_length)
+        req->action = MLAN_ACT_GET;
+    else if (data_length <= MLAN_MAX_CLK_NUM) {
+        req->action = MLAN_ACT_SET;
+        if (copy_from_user
+            (data, wrq->u.data.pointer, sizeof(int) * data_length)) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    } else {
+        PRINTM(MERROR, "Invalid arguments\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    if (req->action == MLAN_ACT_GET) {
+        /* Get configurable clocks */
+        cfg->param.sys_clock.sys_clk_type = MLAN_CLK_CONFIGURABLE;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+
+        /* Current system clock */
+        data[0] = (int) cfg->param.sys_clock.cur_sys_clk;
+        wrq->u.data.length = 1;
+
+        data_length = MIN(cfg->param.sys_clock.sys_clk_num, MLAN_MAX_CLK_NUM);
+
+        /* Configurable clocks */
+        for (i = 0; i < data_length; i++) {
+            data[i + wrq->u.data.length] =
+                (int) cfg->param.sys_clock.sys_clk[i];
+        }
+        wrq->u.data.length += data_length;
+
+        /* Get supported clocks */
+        cfg->param.sys_clock.sys_clk_type = MLAN_CLK_SUPPORTED;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+
+        data_length = MIN(cfg->param.sys_clock.sys_clk_num, MLAN_MAX_CLK_NUM);
+
+        /* Supported clocks */
+        for (i = 0; i < data_length; i++) {
+            data[i + wrq->u.data.length] =
+                (int) cfg->param.sys_clock.sys_clk[i];
+        }
+
+        wrq->u.data.length += data_length;
+
+        if (copy_to_user
+            (wrq->u.data.pointer, data, sizeof(int) * wrq->u.data.length)) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    } else {
+        /* Set configurable clocks */
+        cfg->param.sys_clock.sys_clk_type = MLAN_CLK_CONFIGURABLE;
+        cfg->param.sys_clock.sys_clk_num = MIN(MLAN_MAX_CLK_NUM, data_length);
+        for (i = 0; i < cfg->param.sys_clock.sys_clk_num; i++) {
+            cfg->param.sys_clock.sys_clk[i] = (t_u16) data[i];
+        }
+
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get Band and Adhoc-band setting
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wrq	        A pointer to iwreq structure
+ *
+ *  @return             0 --success, otherwise fail
+ */
+static int
+woal_band_cfg(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    unsigned int i;
+    int data[4];
+    int user_data_len = wrq->u.data.length;
+    t_u32 infra_band = 0;
+    t_u32 adhoc_band = 0;
+    t_u32 adhoc_channel = 0;
+    t_u32 adhoc_chan_bandwidth = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_radio_cfg *radio_cfg = NULL;
+
+    ENTER();
+
+    if (sizeof(int) * user_data_len > sizeof(data)) {
+        PRINTM(MERROR, "Too many arguments\n");
+        LEAVE();
+        return -EINVAL;
+    }
+
+    if (user_data_len > 0) {
+        if (priv->media_connected == MTRUE) {
+            LEAVE();
+            return -EOPNOTSUPP;
+        }
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_radio_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto error;
+    }
+    radio_cfg = (mlan_ds_radio_cfg *) req->pbuf;
+    radio_cfg->sub_command = MLAN_OID_BAND_CFG;
+    req->req_id = MLAN_IOCTL_RADIO_CFG;
+
+    if (wrq->u.data.length == 0) {
+        /* Get config_bands, adhoc_start_band and adhoc_channel values from
+           MLAN */
+        req->action = MLAN_ACT_GET;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto error;
+        }
+        data[0] = radio_cfg->param.band_cfg.config_bands;       /* Infra Band */
+        data[1] = radio_cfg->param.band_cfg.adhoc_start_band;   /* Adhoc Band */
+        data[2] = radio_cfg->param.band_cfg.adhoc_channel;      /* Adhoc
+                                                                   Channel */
+        wrq->u.data.length = 3;
+        if (radio_cfg->param.band_cfg.adhoc_start_band & BAND_GN
+            || radio_cfg->param.band_cfg.adhoc_start_band & BAND_AN) {
+            data[3] = radio_cfg->param.band_cfg.sec_chan_offset;
+            wrq->u.data.length = 4;
+        }
+
+        if (copy_to_user
+            (wrq->u.data.pointer, data, sizeof(int) * wrq->u.data.length)) {
+            ret = -EFAULT;
+            goto error;
+        }
+    } else {
+        if (copy_from_user
+            (data, wrq->u.data.pointer, sizeof(int) * user_data_len)) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto error;
+        }
+
+        /* To support only <b/bg/bgn/n> */
+        infra_band = data[0];
+        for (i = 0; i < sizeof(SupportedInfraBand); i++)
+            if (infra_band == SupportedInfraBand[i])
+                break;
+        if (i == sizeof(SupportedInfraBand)) {
+            ret = -EINVAL;
+            goto error;
+        }
+
+        /* Set Adhoc band */
+        if (user_data_len >= 2) {
+            adhoc_band = data[1];
+            for (i = 0; i < sizeof(SupportedAdhocBand); i++)
+                if (adhoc_band == SupportedAdhocBand[i])
+                    break;
+            if (i == sizeof(SupportedAdhocBand)) {
+                ret = -EINVAL;
+                goto error;
+            }
+        }
+
+        /* Set Adhoc channel */
+        if (user_data_len >= 3) {
+            adhoc_channel = data[2];
+            if (adhoc_channel == 0) {
+                /* Check if specified adhoc channel is non-zero */
+                ret = -EINVAL;
+                goto error;
+            }
+        }
+        if (user_data_len == 4) {
+            if (!(adhoc_band & (BAND_GN | BAND_AN))) {
+                PRINTM(MERROR,
+                       "11n is not enabled for adhoc, can not set HT/VHT channel bandwidth\n");
+                ret = -EINVAL;
+                goto error;
+            }
+            adhoc_chan_bandwidth = data[3];
+            if ((adhoc_chan_bandwidth != CHANNEL_BW_20MHZ) &&
+                (adhoc_chan_bandwidth != CHANNEL_BW_40MHZ_ABOVE) &&
+                (adhoc_chan_bandwidth != CHANNEL_BW_40MHZ_BELOW)
+                ) {
+                PRINTM(MERROR,
+                       "Invalid secondary channel bandwidth, only allowed 0, 1, 3 or 4\n");
+                ret = -EINVAL;
+                goto error;
+            }
+        }
+        /* Set config_bands and adhoc_start_band values to MLAN */
+        req->action = MLAN_ACT_SET;
+        radio_cfg->param.band_cfg.config_bands = infra_band;
+        radio_cfg->param.band_cfg.adhoc_start_band = adhoc_band;
+        radio_cfg->param.band_cfg.adhoc_channel = adhoc_channel;
+        radio_cfg->param.band_cfg.sec_chan_offset = adhoc_chan_bandwidth;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto error;
+        }
+    }
+
+  error:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Read/Write adapter registers value
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wrq	        A pointer to iwreq structure
+ *
+ *  @return             0 --success, otherwise fail
+ */
+static int
+woal_reg_read_write(moal_private * priv, struct iwreq *wrq)
+{
+    int data[3];
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_reg_mem *reg = NULL;
+    int data_length = wrq->u.data.length;
+
+    ENTER();
+
+    memset(data, 0, sizeof(data));
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_reg_mem));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    reg = (mlan_ds_reg_mem *) req->pbuf;
+    reg->sub_command = MLAN_OID_REG_RW;
+    req->req_id = MLAN_IOCTL_REG_MEM;
+
+    if (data_length == 2) {
+        req->action = MLAN_ACT_GET;
+    } else if (data_length == 3) {
+        req->action = MLAN_ACT_SET;
+    } else {
+        ret = -EINVAL;
+        goto done;
+    }
+    if (copy_from_user(data, wrq->u.data.pointer, sizeof(int) * data_length)) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    reg->param.reg_rw.type = (t_u32) data[0];
+    reg->param.reg_rw.offset = (t_u32) data[1];
+    if (data_length == 3)
+        reg->param.reg_rw.value = (t_u32) data[2];
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (req->action == MLAN_ACT_GET) {
+        if (copy_to_user
+            (wrq->u.data.pointer, &reg->param.reg_rw.value, sizeof(int))) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = 1;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Read the EEPROM contents of the card
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wrq	        A pointer to iwreq structure
+ *
+ *  @return             0 --success, otherwise fail
+ */
+static int
+woal_read_eeprom(moal_private * priv, struct iwreq *wrq)
+{
+    int data[2];
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_reg_mem *reg = NULL;
+    int data_length = wrq->u.data.length;
+
+    ENTER();
+
+    memset(data, 0, sizeof(data));
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_reg_mem));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    reg = (mlan_ds_reg_mem *) req->pbuf;
+    reg->sub_command = MLAN_OID_EEPROM_RD;
+    req->req_id = MLAN_IOCTL_REG_MEM;
+
+    if (data_length == 2) {
+        req->action = MLAN_ACT_GET;
+    } else {
+        ret = -EINVAL;
+        goto done;
+    }
+    if (copy_from_user(data, wrq->u.data.pointer, sizeof(int) * data_length)) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    reg->param.rd_eeprom.offset = (t_u16) data[0];
+    reg->param.rd_eeprom.byte_count = (t_u16) data[1];
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (req->action == MLAN_ACT_GET) {
+        wrq->u.data.length = reg->param.rd_eeprom.byte_count;
+        if (copy_to_user
+            (wrq->u.data.pointer, reg->param.rd_eeprom.value,
+             MIN(wrq->u.data.length, MAX_EEPROM_DATA))) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Read/Write device memory value
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wrq	        A pointer to iwreq structure
+ *
+ *  @return             0 --success, otherwise fail
+ */
+static int
+woal_mem_read_write(moal_private * priv, struct iwreq *wrq)
+{
+    t_u32 data[2];
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_reg_mem *reg_mem = NULL;
+    int data_length = wrq->u.data.length;
+
+    ENTER();
+
+    memset(data, 0, sizeof(data));
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_reg_mem));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    reg_mem = (mlan_ds_reg_mem *) req->pbuf;
+    reg_mem->sub_command = MLAN_OID_MEM_RW;
+    req->req_id = MLAN_IOCTL_REG_MEM;
+
+    if (data_length == 1) {
+        PRINTM(MINFO, "MEM_RW: GET\n");
+        req->action = MLAN_ACT_GET;
+    } else if (data_length == 2) {
+        PRINTM(MINFO, "MEM_RW: SET\n");
+        req->action = MLAN_ACT_SET;
+    } else {
+        ret = -EINVAL;
+        goto done;
+    }
+    if (copy_from_user(data, wrq->u.data.pointer, sizeof(int) * data_length)) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    reg_mem->param.mem_rw.addr = (t_u32) data[0];
+    if (data_length == 2)
+        reg_mem->param.mem_rw.value = (t_u32) data[1];
+
+    PRINTM(MINFO, "MEM_RW: Addr=0x%x, Value=0x%x\n",
+           (int) reg_mem->param.mem_rw.addr, (int) reg_mem->param.mem_rw.value);
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (req->action == MLAN_ACT_GET) {
+        if (copy_to_user
+            (wrq->u.data.pointer, &reg_mem->param.mem_rw.value, sizeof(int))) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = 1;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get LOG
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wrq			A pointer to iwreq structure
+ *
+ *  @return 	   	 	0 --success, otherwise fail
+ */
+static int
+woal_get_log(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_get_stats stats;
+    char *buf = NULL;
+
+    ENTER();
+
+    PRINTM(MINFO, " GET STATS\n");
+    if (!(buf = kmalloc(GETLOG_BUFSIZE, GFP_KERNEL))) {
+        PRINTM(MERROR, "kmalloc failed!\n");
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    memset(&stats, 0, sizeof(mlan_ds_get_stats));
+    if (MLAN_STATUS_SUCCESS !=
+        woal_get_stats_info(priv, MOAL_IOCTL_WAIT, &stats)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (wrq->u.data.pointer) {
+        sprintf(buf, "\n"
+                "mcasttxframe     %u\n"
+                "failed           %u\n"
+                "retry            %u\n"
+                "multiretry       %u\n"
+                "framedup         %u\n"
+                "rtssuccess       %u\n"
+                "rtsfailure       %u\n"
+                "ackfailure       %u\n"
+                "rxfrag           %u\n"
+                "mcastrxframe     %u\n"
+                "fcserror         %u\n"
+                "txframe          %u\n"
+                "wepicverrcnt-1   %u\n"
+                "wepicverrcnt-2   %u\n"
+                "wepicverrcnt-3   %u\n"
+                "wepicverrcnt-4   %u\n",
+                stats.mcast_tx_frame,
+                stats.failed,
+                stats.retry,
+                stats.multi_retry,
+                stats.frame_dup,
+                stats.rts_success,
+                stats.rts_failure,
+                stats.ack_failure,
+                stats.rx_frag,
+                stats.mcast_rx_frame,
+                stats.fcs_error,
+                stats.tx_frame,
+                stats.wep_icv_error[0],
+                stats.wep_icv_error[1],
+                stats.wep_icv_error[2], stats.wep_icv_error[3]);
+        wrq->u.data.length = strlen(buf) + 1;
+        if (copy_to_user(wrq->u.data.pointer, buf, wrq->u.data.length)) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+        }
+    }
+  done:
+    if (buf)
+        kfree(buf);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Deauthenticate
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wrq			A pointer to iwreq structure
+ *
+ *  @return 	   	 	0 --success, otherwise fail
+ */
+static int
+woal_deauth(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    struct sockaddr saddr;
+
+    ENTER();
+    if (wrq->u.data.length) {
+        /* Deauth mentioned BSSID */
+        if (copy_from_user(&saddr, wrq->u.data.pointer, sizeof(saddr))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if (MLAN_STATUS_SUCCESS !=
+            woal_disconnect(priv, MOAL_IOCTL_WAIT, (t_u8 *) saddr.sa_data)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    } else {
+        if (MLAN_STATUS_SUCCESS != woal_disconnect(priv, MOAL_IOCTL_WAIT, NULL))
+            ret = -EFAULT;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get TX power configurations
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq      A pointer to iwreq structure
+ *
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_tx_power_cfg(moal_private * priv, struct iwreq *wrq)
+{
+    int data[5], user_data_len;
+    int ret = 0;
+    mlan_bss_info bss_info;
+    mlan_ds_power_cfg *pcfg = NULL;
+    mlan_ioctl_req *req = NULL;
+    ENTER();
+
+    memset(&bss_info, 0, sizeof(bss_info));
+    woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info);
+
+    memset(data, 0, sizeof(data));
+    user_data_len = wrq->u.data.length;
+
+    if (user_data_len) {
+        if (sizeof(int) * user_data_len > sizeof(data)) {
+            PRINTM(MERROR, "Too many arguments\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        if (copy_from_user
+            (data, wrq->u.data.pointer, sizeof(int) * user_data_len)) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        switch (user_data_len) {
+        case 1:
+            if (data[0] != 0xFF)
+                ret = -EINVAL;
+            break;
+        case 2:
+        case 4:
+            if (data[0] == 0xFF) {
+                ret = -EINVAL;
+                break;
+            }
+            if ((unsigned int) data[1] < bss_info.min_power_level) {
+                PRINTM(MERROR,
+                       "The set powercfg rate value %d dBm is out of range (%d dBm-%d dBm)!\n",
+                       data[1], (int) bss_info.min_power_level,
+                       (int) bss_info.max_power_level);
+                ret = -EINVAL;
+                break;
+            }
+            if (user_data_len == 4) {
+                if (data[1] > data[2]) {
+                    PRINTM(MERROR, "Min power should be less than maximum!\n");
+                    ret = -EINVAL;
+                    break;
+                }
+                if (data[3] < 0) {
+                    PRINTM(MERROR, "Step should not less than 0!\n");
+                    ret = -EINVAL;
+                    break;
+                }
+                if ((unsigned int) data[2] > bss_info.max_power_level) {
+                    PRINTM(MERROR,
+                           "The set powercfg rate value %d dBm is out of range (%d dBm-%d dBm)!\n",
+                           data[2], (int) bss_info.min_power_level,
+                           (int) bss_info.max_power_level);
+                    ret = -EINVAL;
+                    break;
+                }
+                if (data[3] > data[2] - data[1]) {
+                    PRINTM(MERROR,
+                           "Step should not greater than power difference!\n");
+                    ret = -EINVAL;
+                    break;
+                }
+            }
+            break;
+        default:
+            ret = -EINVAL;
+            break;
+        }
+        if (ret)
+            goto done;
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_power_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    pcfg = (mlan_ds_power_cfg *) req->pbuf;
+    pcfg->sub_command = MLAN_OID_POWER_CFG_EXT;
+    req->req_id = MLAN_IOCTL_POWER_CFG;
+    if (!user_data_len)
+        req->action = MLAN_ACT_GET;
+    else {
+        req->action = MLAN_ACT_SET;
+        pcfg->param.power_ext.len = user_data_len;
+        memcpy((t_u8 *) & pcfg->param.power_ext.power_data, (t_u8 *) data,
+               sizeof(data));
+    }
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (!user_data_len) {
+        if (copy_to_user
+            (wrq->u.data.pointer, (t_u8 *) & pcfg->param.power_ext.power_data,
+             sizeof(int) * pcfg->param.power_ext.len)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = pcfg->param.power_ext.len;
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get Tx/Rx data rates
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq      A pointer to iwreq structure
+ *
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_get_txrx_rate(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_rate *rate = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_rate));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    rate = (mlan_ds_rate *) req->pbuf;
+    rate->sub_command = MLAN_OID_GET_DATA_RATE;
+    req->req_id = MLAN_IOCTL_RATE;
+    req->action = MLAN_ACT_GET;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (copy_to_user
+        (wrq->u.data.pointer, (t_u8 *) & rate->param.data_rate,
+         sizeof(int) * 2)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    wrq->u.data.length = 2;
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Turn on/off the sdio clock
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq      A pointer to iwreq structure
+ *
+ *  @return         0/MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static int
+woal_sdio_clock_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    int data = 2;
+    /* Initialize the clock state as on */
+    static int clock_state = 1;
+
+    ENTER();
+
+    if (wrq->u.data.length) {
+        if (copy_from_user(&data, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    } else {
+        wrq->u.data.length = sizeof(clock_state) / sizeof(int);
+        if (copy_to_user(wrq->u.data.pointer, &clock_state, sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+
+        goto done;
+    }
+    switch (data) {
+    case CMD_DISABLED:
+        PRINTM(MINFO, "SDIO clock is turned off\n");
+        ret = woal_sdio_set_bus_clock(priv->phandle, MFALSE);
+        clock_state = data;
+        break;
+    case CMD_ENABLED:
+        PRINTM(MINFO, "SDIO clock is turned on\n");
+        ret = woal_sdio_set_bus_clock(priv->phandle, MTRUE);
+        clock_state = data;
+        break;
+    default:
+        ret = -EINVAL;
+        PRINTM(MINFO, "sdioclock: wrong parameter\n");
+        break;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get beacon interval
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq      A pointer to iwreq structure
+ *
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_beacon_interval(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_bss *bss = NULL;
+    mlan_ioctl_req *req = NULL;
+    int bcn = 0;
+
+    ENTER();
+
+    if (wrq->u.data.length) {
+        if (copy_from_user(&bcn, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if ((bcn < MLAN_MIN_BEACON_INTERVAL) ||
+            (bcn > MLAN_MAX_BEACON_INTERVAL)) {
+            ret = -EINVAL;
+            goto done;
+        }
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_IBSS_BCN_INTERVAL;
+    req->req_id = MLAN_IOCTL_BSS;
+    if (!wrq->u.data.length)
+        req->action = MLAN_ACT_GET;
+    else {
+        req->action = MLAN_ACT_SET;
+        bss->param.bcn_interval = bcn;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (copy_to_user
+        (wrq->u.data.pointer, (t_u8 *) & bss->param.bcn_interval,
+         sizeof(int))) {
+        ret = -EFAULT;
+        goto done;
+    }
+    wrq->u.data.length = 1;
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get ATIM window
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq      A pointer to iwreq structure
+ *
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_atim_window(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_bss *bss = NULL;
+    mlan_ioctl_req *req = NULL;
+    int atim = 0;
+
+    ENTER();
+
+    if (wrq->u.data.length) {
+        if (copy_from_user(&atim, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if ((atim < 0) || (atim > MLAN_MAX_ATIM_WINDOW)) {
+            ret = -EINVAL;
+            goto done;
+        }
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_IBSS_ATIM_WINDOW;
+    req->req_id = MLAN_IOCTL_BSS;
+    if (!wrq->u.data.length)
+        req->action = MLAN_ACT_GET;
+    else {
+        req->action = MLAN_ACT_SET;
+        bss->param.atim_window = atim;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (copy_to_user
+        (wrq->u.data.pointer, (t_u8 *) & bss->param.atim_window, sizeof(int))) {
+        ret = -EFAULT;
+        goto done;
+    }
+    wrq->u.data.length = 1;
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Set/Get TX data rate
+ *
+ * @param priv     A pointer to moal_private structure
+ * @param wrq      A pointer to iwreq structure
+ *
+ * @return           0 --success, otherwise fail
+ */
+static int
+woal_set_get_txrate(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_rate *rate = NULL;
+    mlan_ioctl_req *req = NULL;
+    int rateindex = 0;
+
+    ENTER();
+    if (wrq->u.data.length) {
+        if (copy_from_user(&rateindex, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_rate));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    rate = (mlan_ds_rate *) req->pbuf;
+    rate->param.rate_cfg.rate_type = MLAN_RATE_INDEX;
+    rate->sub_command = MLAN_OID_RATE_CFG;
+    req->req_id = MLAN_IOCTL_RATE;
+    if (!wrq->u.data.length)
+        req->action = MLAN_ACT_GET;
+    else {
+        req->action = MLAN_ACT_SET;
+        if (rateindex == AUTO_RATE)
+            rate->param.rate_cfg.is_rate_auto = 1;
+        else {
+            if ((rateindex != MLAN_RATE_INDEX_MCS32) &&
+                ((rateindex < 0) || (rateindex > MLAN_RATE_INDEX_MCS15))) {
+                ret = -EINVAL;
+                goto done;
+            }
+        }
+        rate->param.rate_cfg.rate = rateindex;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    } else {
+        if (wrq->u.data.length)
+            priv->rate_index = rateindex;
+    }
+    if (!wrq->u.data.length) {
+        if (rate->param.rate_cfg.is_rate_auto)
+            rateindex = AUTO_RATE;
+        else
+            rateindex = rate->param.rate_cfg.rate;
+        wrq->u.data.length = 1;
+        if (copy_to_user(wrq->u.data.pointer, &rateindex, sizeof(int))) {
+            ret = -EFAULT;
+        }
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Set/Get region code
+ *
+ * @param priv     A pointer to moal_private structure
+ * @param wrq      A pointer to iwreq structure
+ *
+ * @return           0 --success, otherwise fail
+ */
+static int
+woal_set_get_regioncode(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_misc_cfg *cfg = NULL;
+    mlan_ioctl_req *req = NULL;
+    int region = 0;
+
+    ENTER();
+
+    if (wrq->u.data.length) {
+        if (copy_from_user(&region, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    cfg = (mlan_ds_misc_cfg *) req->pbuf;
+    cfg->sub_command = MLAN_OID_MISC_REGION;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+    if (!wrq->u.data.length)
+        req->action = MLAN_ACT_GET;
+    else {
+        req->action = MLAN_ACT_SET;
+        cfg->param.region_code = region;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (!wrq->u.data.length) {
+        wrq->u.data.length = 1;
+        if (copy_to_user
+            (wrq->u.data.pointer, &cfg->param.region_code, sizeof(int)))
+            ret = -EFAULT;
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Set/Get radio
+ *
+ * @param priv     A pointer to moal_private structure
+ * @param wrq      A pointer to iwreq structure
+ *
+ * @return           0 --success, otherwise fail
+ */
+static int
+woal_set_get_radio(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_bss_info bss_info;
+    int option = 0;
+
+    ENTER();
+
+    memset(&bss_info, 0, sizeof(bss_info));
+
+    if (wrq->u.data.length) {
+        /* Set radio */
+        if (copy_from_user(&option, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if (MLAN_STATUS_SUCCESS != woal_set_radio(priv, (t_u8) option))
+            ret = -EFAULT;
+    } else {
+        /* Get radio status */
+        woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info);
+        wrq->u.data.length = 1;
+        if (copy_to_user
+            (wrq->u.data.pointer, &bss_info.radio_on,
+             sizeof(bss_info.radio_on))) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+        }
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+#ifdef DEBUG_LEVEL1
+/**
+ *  @brief Get/Set the bit mask of driver debug message control
+ *
+ *  @param priv			A pointer to moal_private structure
+ *  @param wrq			A pointer to wrq structure
+ *
+ *  @return             0 --success, otherwise fail
+ */
+static int
+woal_drv_dbg(moal_private * priv, struct iwreq *wrq)
+{
+    int data[4];
+    int ret = 0;
+
+    ENTER();
+
+    if (!wrq->u.data.length) {
+        data[0] = drvdbg;
+        /* Return the current driver debug bit masks */
+        if (copy_to_user(wrq->u.data.pointer, data, sizeof(int))) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto drvdbgexit;
+        }
+        wrq->u.data.length = 1;
+    } else if (wrq->u.data.length < 3) {
+        /* Get the driver debug bit masks from user */
+        if (copy_from_user
+            (data, wrq->u.data.pointer, sizeof(int) * wrq->u.data.length)) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto drvdbgexit;
+        }
+        drvdbg = data[0];
+        /* Set the driver debug bit masks into mlan */
+        woal_set_drvdbg(priv, drvdbg);
+    } else {
+        PRINTM(MERROR, "Invalid parameter number\n");
+        goto drvdbgexit;
+    }
+
+    printk(KERN_ALERT "drvdbg = 0x%08x\n", drvdbg);
+#ifdef DEBUG_LEVEL2
+    printk(KERN_ALERT "MINFO  (%08x) %s\n", MINFO, (drvdbg & MINFO) ? "X" : "");
+    printk(KERN_ALERT "MWARN  (%08x) %s\n", MWARN, (drvdbg & MWARN) ? "X" : "");
+    printk(KERN_ALERT "MENTRY (%08x) %s\n", MENTRY,
+           (drvdbg & MENTRY) ? "X" : "");
+#endif
+    printk(KERN_ALERT "MIF_D  (%08x) %s\n", MIF_D, (drvdbg & MIF_D) ? "X" : "");
+    printk(KERN_ALERT "MFW_D  (%08x) %s\n", MFW_D, (drvdbg & MFW_D) ? "X" : "");
+    printk(KERN_ALERT "MEVT_D (%08x) %s\n", MEVT_D,
+           (drvdbg & MEVT_D) ? "X" : "");
+    printk(KERN_ALERT "MCMD_D (%08x) %s\n", MCMD_D,
+           (drvdbg & MCMD_D) ? "X" : "");
+    printk(KERN_ALERT "MDAT_D (%08x) %s\n", MDAT_D,
+           (drvdbg & MDAT_D) ? "X" : "");
+    printk(KERN_ALERT "MIOCTL (%08x) %s\n", MIOCTL,
+           (drvdbg & MIOCTL) ? "X" : "");
+    printk(KERN_ALERT "MINTR  (%08x) %s\n", MINTR, (drvdbg & MINTR) ? "X" : "");
+    printk(KERN_ALERT "MEVENT (%08x) %s\n", MEVENT,
+           (drvdbg & MEVENT) ? "X" : "");
+    printk(KERN_ALERT "MCMND  (%08x) %s\n", MCMND, (drvdbg & MCMND) ? "X" : "");
+    printk(KERN_ALERT "MDATA  (%08x) %s\n", MDATA, (drvdbg & MDATA) ? "X" : "");
+    printk(KERN_ALERT "MERROR (%08x) %s\n", MERROR,
+           (drvdbg & MERROR) ? "X" : "");
+    printk(KERN_ALERT "MFATAL (%08x) %s\n", MFATAL,
+           (drvdbg & MFATAL) ? "X" : "");
+    printk(KERN_ALERT "MMSG   (%08x) %s\n", MMSG, (drvdbg & MMSG) ? "X" : "");
+
+  drvdbgexit:
+    LEAVE();
+    return ret;
+}
+#endif /* DEBUG_LEVEL1 */
+
+/**
+ * @brief Set/Get QoS configuration
+ *
+ * @param priv     A pointer to moal_private structure
+ * @param wrq      A pointer to iwreq structure
+ *
+ * @return         0 --success, otherwise fail
+ */
+static int
+woal_set_get_qos_cfg(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_wmm_cfg *cfg = NULL;
+    mlan_ioctl_req *req = NULL;
+    int data = 0;
+
+    ENTER();
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_wmm_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    cfg = (mlan_ds_wmm_cfg *) req->pbuf;
+    cfg->sub_command = MLAN_OID_WMM_CFG_QOS;
+    req->req_id = MLAN_IOCTL_WMM_CFG;
+    if (wrq->u.data.length) {
+        if (copy_from_user(&data, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        req->action = MLAN_ACT_SET;
+        cfg->param.qos_cfg = (t_u8) data;
+    } else
+        req->action = MLAN_ACT_GET;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (!wrq->u.data.length) {
+        data = (int) cfg->param.qos_cfg;
+        if (copy_to_user(wrq->u.data.pointer, &data, sizeof(int))) {
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = 1;
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Set/Get WWS mode
+ *
+ * @param priv     A pointer to moal_private structure
+ * @param wrq      A pointer to iwreq structure
+ *
+ * @return         0 --success, otherwise fail
+ */
+static int
+woal_wws_cfg(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_misc_cfg *wws = NULL;
+    mlan_ioctl_req *req = NULL;
+    int data = 0;
+
+    ENTER();
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    wws = (mlan_ds_misc_cfg *) req->pbuf;
+    wws->sub_command = MLAN_OID_MISC_WWS;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+    if (wrq->u.data.length) {
+        if (copy_from_user(&data, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if (data != CMD_DISABLED && data != CMD_ENABLED) {
+            ret = -EINVAL;
+            goto done;
+        }
+        req->action = MLAN_ACT_SET;
+        wws->param.wws_cfg = data;
+    } else
+        req->action = MLAN_ACT_GET;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (!wrq->u.data.length) {
+        data = wws->param.wws_cfg;
+        if (copy_to_user(wrq->u.data.pointer, &data, sizeof(int))) {
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = 1;
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Set/Get sleep period
+ *
+ * @param priv     A pointer to moal_private structure
+ * @param wrq      A pointer to iwreq structure
+ *
+ * @return         0 --success, otherwise fail
+ */
+static int
+woal_sleep_pd(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_pm_cfg *pm_cfg = NULL;
+    mlan_ioctl_req *req = NULL;
+    int data = 0;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_pm_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    pm_cfg = (mlan_ds_pm_cfg *) req->pbuf;
+    pm_cfg->sub_command = MLAN_OID_PM_CFG_SLEEP_PD;
+    req->req_id = MLAN_IOCTL_PM_CFG;
+    if (wrq->u.data.length) {
+        if (copy_from_user(&data, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if ((data <= MAX_SLEEP_PERIOD && data >= MIN_SLEEP_PERIOD) ||
+            (data == 0)
+            || (data == SLEEP_PERIOD_RESERVED_FF)
+            ) {
+            req->action = MLAN_ACT_SET;
+            pm_cfg->param.sleep_period = data;
+        } else {
+            ret = -EINVAL;
+            goto done;
+        }
+    } else
+        req->action = MLAN_ACT_GET;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (!wrq->u.data.length) {
+        data = pm_cfg->param.sleep_period;
+        if (copy_to_user(wrq->u.data.pointer, &data, sizeof(int))) {
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = 1;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Configure sleep parameters
+ *
+ * @param priv         A pointer to moal_private structure
+ * @param req          A pointer to ifreq structure
+ *
+ * @return             0 --success, otherwise fail
+ */
+static int
+woal_sleep_params_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_pm_cfg *pm = NULL;
+    mlan_ds_sleep_params *psleep_params = NULL;
+    int data[6] = { 0 }, i;
+#ifdef DEBUG_LEVEL1
+    char err_str[][35] = { {"sleep clock error in ppm"},
+    {"wakeup offset in usec"},
+    {"clock stabilization time in usec"},
+    {"value of reserved for debug"}
+    };
+#endif
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_pm_cfg));
+    if (req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+
+    pm = (mlan_ds_pm_cfg *) req->pbuf;
+    pm->sub_command = MLAN_OID_PM_CFG_SLEEP_PARAMS;
+    req->req_id = MLAN_IOCTL_PM_CFG;
+    psleep_params = (pmlan_ds_sleep_params) & pm->param.sleep_params;
+
+    if (wrq->u.data.length == 0) {
+        req->action = MLAN_ACT_GET;
+    } else if (wrq->u.data.length == 6) {
+        if (copy_from_user(data, wrq->u.data.pointer, sizeof(int) *
+                           wrq->u.data.length)) {
+            /* copy_from_user failed */
+            PRINTM(MERROR, "S_PARAMS: copy from user failed\n");
+            LEAVE();
+            return -EINVAL;
+        }
+#define MIN_VAL 0x0000
+#define MAX_VAL 0xFFFF
+        for (i = 0; i < 6; i++) {
+            if ((i == 3) || (i == 4)) {
+                /* These two cases are handled below the loop */
+                continue;
+            }
+            if (data[i] < MIN_VAL || data[i] > MAX_VAL) {
+                PRINTM(MERROR, "Invalid %s (0-65535)!\n", err_str[i]);
+                ret = -EINVAL;
+                goto done;
+            }
+        }
+        if (data[3] < 0 || data[3] > 2) {
+            PRINTM(MERROR, "Invalid control periodic calibration (0-2)!\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        if (data[4] < 0 || data[4] > 2) {
+            PRINTM(MERROR, "Invalid control of external sleep clock (0-2)!\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        req->action = MLAN_ACT_SET;
+        psleep_params->error = data[0];
+        psleep_params->offset = data[1];
+        psleep_params->stable_time = data[2];
+        psleep_params->cal_control = data[3];
+        psleep_params->ext_sleep_clk = data[4];
+        psleep_params->reserved = data[5];
+    } else {
+        ret = -EINVAL;
+        goto done;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    data[0] = psleep_params->error;
+    data[1] = psleep_params->offset;
+    data[2] = psleep_params->stable_time;
+    data[3] = psleep_params->cal_control;
+    data[4] = psleep_params->ext_sleep_clk;
+    data[5] = psleep_params->reserved;
+    wrq->u.data.length = 6;
+
+    if (copy_to_user(wrq->u.data.pointer, data, sizeof(int) *
+                     wrq->u.data.length)) {
+        PRINTM(MERROR, "QCONFIG: copy to user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/get user provisioned local power constraint
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq      A pointer to iwreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_set_get_11h_local_pwr_constraint(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0, data = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_11h_cfg *ds_11hcfg = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11h_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    ds_11hcfg = (mlan_ds_11h_cfg *) req->pbuf;
+    if (wrq->u.data.length) {
+        if (copy_from_user(&data, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MINFO, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        ds_11hcfg->param.usr_local_power_constraint = (t_s8) data;
+        req->action = MLAN_ACT_SET;
+    } else
+        req->action = MLAN_ACT_GET;
+
+    ds_11hcfg->sub_command = MLAN_OID_11H_LOCAL_POWER_CONSTRAINT;
+    req->req_id = MLAN_IOCTL_11H_CFG;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Copy response to user */
+    if (req->action == MLAN_ACT_GET) {
+        data = (int) ds_11hcfg->param.usr_local_power_constraint;
+        if (copy_to_user(wrq->u.data.pointer, &data, sizeof(int))) {
+            PRINTM(MINFO, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = 1;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/get HT stream configurations
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq      A pointer to iwreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_ht_stream_cfg_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0, data = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_11n_cfg *cfg = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11n_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    cfg = (mlan_ds_11n_cfg *) req->pbuf;
+    if (wrq->u.data.length) {
+        if (copy_from_user(&data, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MINFO, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if (data != HT_STREAM_MODE_1X1 && data != HT_STREAM_MODE_2X2) {
+            PRINTM(MINFO, "Invalid argument\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        cfg->param.stream_cfg = data;
+        req->action = MLAN_ACT_SET;
+    } else
+        req->action = MLAN_ACT_GET;
+
+    cfg->sub_command = MLAN_OID_11N_CFG_STREAM_CFG;
+    req->req_id = MLAN_IOCTL_11N_CFG;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Copy response to user */
+    data = (int) cfg->param.stream_cfg;
+    if (copy_to_user(wrq->u.data.pointer, &data, sizeof(int))) {
+        PRINTM(MINFO, "Copy to user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    wrq->u.data.length = 1;
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/get MAC control configuration
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq      A pointer to iwreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_mac_control_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0, data = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *cfg = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    cfg = (mlan_ds_misc_cfg *) req->pbuf;
+    if (wrq->u.data.length) {
+        if (copy_from_user(&data, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MINFO, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        /* Validation will be done later */
+        cfg->param.mac_ctrl = data;
+        req->action = MLAN_ACT_SET;
+    } else
+        req->action = MLAN_ACT_GET;
+
+    cfg->sub_command = MLAN_OID_MISC_MAC_CONTROL;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Copy response to user */
+    data = (int) cfg->param.mac_ctrl;
+    if (copy_to_user(wrq->u.data.pointer, &data, sizeof(int))) {
+        PRINTM(MINFO, "Copy to user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    wrq->u.data.length = 1;
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get thermal reading
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq      A pointer to iwreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_thermal_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0, data = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *cfg = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    cfg = (mlan_ds_misc_cfg *) req->pbuf;
+    if (wrq->u.data.length) {
+        PRINTM(MERROR, "Set is not supported for this command\n");
+        ret = -EINVAL;
+        goto done;
+    }
+    req->action = MLAN_ACT_GET;
+
+    cfg->sub_command = MLAN_OID_MISC_THERMAL;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Copy response to user */
+    data = (int) cfg->param.thermal;
+    if (copy_to_user(wrq->u.data.pointer, &data, sizeof(int))) {
+        PRINTM(MINFO, "Copy to user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    wrq->u.data.length = 1;
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+#if defined(REASSOCIATION)
+/**
+ * @brief Set/Get reassociation settings
+ *
+ * @param priv     A pointer to moal_private structure
+ * @param wrq      A pointer to iwreq structure
+ *
+ * @return         0 --success, otherwise fail
+ */
+static int
+woal_set_get_reassoc(moal_private * priv, struct iwreq *wrq)
+{
+    moal_handle *handle = priv->phandle;
+    int ret = 0;
+    int data = 0;
+
+    ENTER();
+
+    if (wrq->u.data.length) {
+        if (copy_from_user(&data, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if (data == 0) {
+            handle->reassoc_on &= ~MBIT(priv->bss_index);
+            priv->reassoc_on = MFALSE;
+            priv->reassoc_required = MFALSE;
+            if (!handle->reassoc_on && handle->is_reassoc_timer_set == MTRUE) {
+                woal_cancel_timer(&handle->reassoc_timer);
+                handle->is_reassoc_timer_set = MFALSE;
+            }
+        } else {
+            handle->reassoc_on |= MBIT(priv->bss_index);
+            priv->reassoc_on = MTRUE;
+        }
+    } else {
+        data = (int) (priv->reassoc_on);
+        if (copy_to_user(wrq->u.data.pointer, &data, sizeof(int))) {
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = 1;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+#endif /* REASSOCIATION */
+
+/**
+ *  @brief implement WMM enable command
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wrq      Pointer to user data
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_wmm_enable_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_wmm_cfg *wmm = NULL;
+    mlan_ioctl_req *req = NULL;
+    int data = 0;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_wmm_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    wmm = (mlan_ds_wmm_cfg *) req->pbuf;
+    req->req_id = MLAN_IOCTL_WMM_CFG;
+    wmm->sub_command = MLAN_OID_WMM_CFG_ENABLE;
+
+    if (wrq->u.data.length) {
+        /* Set WMM configuration */
+        if (copy_from_user(&data, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if ((data < CMD_DISABLED) || (data > CMD_ENABLED)) {
+            ret = -EINVAL;
+            goto done;
+        }
+        req->action = MLAN_ACT_SET;
+        if (data == CMD_DISABLED)
+            wmm->param.wmm_enable = MFALSE;
+        else
+            wmm->param.wmm_enable = MTRUE;
+    } else {
+        /* Get WMM status */
+        req->action = MLAN_ACT_GET;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (wrq->u.data.pointer) {
+        if (copy_to_user
+            (wrq->u.data.pointer, &wmm->param.wmm_enable,
+             sizeof(wmm->param.wmm_enable))) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = 1;
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Implement 802.11D enable command
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wrq      Pointer to user data
+ *
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_11d_enable_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_11d_cfg *pcfg_11d = NULL;
+    mlan_ioctl_req *req = NULL;
+    int data = 0;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11d_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    pcfg_11d = (mlan_ds_11d_cfg *) req->pbuf;
+    req->req_id = MLAN_IOCTL_11D_CFG;
+    pcfg_11d->sub_command = MLAN_OID_11D_CFG_ENABLE;
+    if (wrq->u.data.length) {
+        /* Set 11D configuration */
+        if (copy_from_user(&data, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if ((data < CMD_DISABLED) || (data > CMD_ENABLED)) {
+            ret = -EINVAL;
+            goto done;
+        }
+        if (data == CMD_DISABLED)
+            pcfg_11d->param.enable_11d = MFALSE;
+        else
+            pcfg_11d->param.enable_11d = MTRUE;
+        req->action = MLAN_ACT_SET;
+    } else {
+        req->action = MLAN_ACT_GET;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (wrq->u.data.pointer) {
+        if (copy_to_user
+            (wrq->u.data.pointer, &pcfg_11d->param.enable_11d,
+             sizeof(pcfg_11d->param.enable_11d))) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = 1;
+    }
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Implement 802.11D clear chan table command
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wrq      Pointer to user data
+ *
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_11d_clr_chan_table(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_11d_cfg *pcfg_11d = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11d_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    pcfg_11d = (mlan_ds_11d_cfg *) req->pbuf;
+    req->req_id = MLAN_IOCTL_11D_CFG;
+    pcfg_11d->sub_command = MLAN_OID_11D_CLR_CHAN_TABLE;
+    req->action = MLAN_ACT_SET;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Control WPS Session Enable/Disable
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wrq      Pointer to user data
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_wps_cfg_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_wps_cfg *pwps = NULL;
+    mlan_ioctl_req *req = NULL;
+    char buf[8];
+    struct iwreq *wreq = (struct iwreq *) wrq;
+
+    ENTER();
+
+    PRINTM(MINFO, "WOAL_WPS_SESSION\n");
+
+    memset(buf, 0, sizeof(buf));
+    if (copy_from_user(buf, wreq->u.data.pointer,
+                       MIN(sizeof(buf) - 1, wreq->u.data.length))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_wps_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    pwps = (mlan_ds_wps_cfg *) req->pbuf;
+    req->req_id = MLAN_IOCTL_WPS_CFG;
+    req->action = MLAN_ACT_SET;
+    pwps->sub_command = MLAN_OID_WPS_CFG_SESSION;
+    if (buf[0] == 1)
+        pwps->param.wps_session = MLAN_WPS_CFG_SESSION_START;
+    else
+        pwps->param.wps_session = MLAN_WPS_CFG_SESSION_END;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set WPA passphrase and SSID
+ *
+ *  @param priv	    A pointer to moal_private structure
+ *  @param wrq	    A pointer to user data
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_passphrase(moal_private * priv, struct iwreq *wrq)
+{
+    t_u16 len = 0;
+    static char buf[256];
+    char *begin, *end, *opt;
+    int ret = 0, action = -1, i;
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_ioctl_req *req = NULL;
+    t_u8 zero_mac[] = { 0, 0, 0, 0, 0, 0 };
+    t_u8 *mac = NULL;
+
+    ENTER();
+
+    if (!wrq->u.data.length || wrq->u.data.length >= sizeof(buf)) {
+        PRINTM(MERROR, "Argument missing or too long for setpassphrase\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    if (copy_from_user(buf, wrq->u.data.pointer, wrq->u.data.length)) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    buf[wrq->u.data.length] = '\0';
+
+    /* Parse the buf to get the cmd_action */
+    begin = buf;
+    end = woal_strsep(&begin, ';', '/');
+    if (end)
+        action = woal_atox(end);
+    if (action < 0 || action > 2 || end[1] != '\0') {
+        PRINTM(MERROR, "Invalid action argument %s\n", end);
+        ret = -EINVAL;
+        goto done;
+    }
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_PASSPHRASE;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    if (action == 0)
+        req->action = MLAN_ACT_GET;
+    else
+        req->action = MLAN_ACT_SET;
+    while (begin) {
+        end = woal_strsep(&begin, ';', '/');
+        opt = woal_strsep(&end, '=', '/');
+        if (!opt || !end || !end[0]) {
+            PRINTM(MERROR, "Invalid option\n");
+            ret = -EINVAL;
+            break;
+        } else if (!strnicmp(opt, "ssid", strlen(opt))) {
+            if (strlen(end) > MLAN_MAX_SSID_LENGTH) {
+                PRINTM(MERROR, "SSID length exceeds max length\n");
+                ret = -EFAULT;
+                break;
+            }
+            sec->param.passphrase.ssid.ssid_len = strlen(end);
+            strncpy((char *) sec->param.passphrase.ssid.ssid, end, strlen(end));
+            PRINTM(MINFO, "ssid=%s, len=%d\n", sec->param.passphrase.ssid.ssid,
+                   (int) sec->param.passphrase.ssid.ssid_len);
+        } else if (!strnicmp(opt, "bssid", strlen(opt))) {
+            woal_mac2u8((t_u8 *) & sec->param.passphrase.bssid, end);
+        } else if (!strnicmp(opt, "psk", strlen(opt)) &&
+                   req->action == MLAN_ACT_SET) {
+            if (strlen(end) != MLAN_PMK_HEXSTR_LENGTH) {
+                PRINTM(MERROR, "Invalid PMK length\n");
+                ret = -EINVAL;
+                break;
+            }
+            woal_ascii2hex((t_u8 *) (sec->param.passphrase.psk.pmk.pmk), end,
+                           MLAN_PMK_HEXSTR_LENGTH / 2);
+            sec->param.passphrase.psk_type = MLAN_PSK_PMK;
+        } else if (!strnicmp(opt, "passphrase", strlen(opt)) &&
+                   req->action == MLAN_ACT_SET) {
+            if (strlen(end) < MLAN_MIN_PASSPHRASE_LENGTH ||
+                strlen(end) > MLAN_MAX_PASSPHRASE_LENGTH) {
+                PRINTM(MERROR, "Invalid length for passphrase\n");
+                ret = -EINVAL;
+                break;
+            }
+            sec->param.passphrase.psk_type = MLAN_PSK_PASSPHRASE;
+            strncpy(sec->param.passphrase.psk.passphrase.passphrase, end,
+                    sizeof(sec->param.passphrase.psk.passphrase.passphrase));
+            sec->param.passphrase.psk.passphrase.passphrase_len = strlen(end);
+            PRINTM(MINFO, "passphrase=%s, len=%d\n",
+                   sec->param.passphrase.psk.passphrase.passphrase,
+                   (int) sec->param.passphrase.psk.passphrase.passphrase_len);
+        } else {
+            PRINTM(MERROR, "Invalid option %s\n", opt);
+            ret = -EINVAL;
+            break;
+        }
+    }
+    if (ret)
+        goto done;
+
+    if (action == 2)
+        sec->param.passphrase.psk_type = MLAN_PSK_CLEAR;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (action == 0) {
+        memset(buf, 0, sizeof(buf));
+        if (sec->param.passphrase.ssid.ssid_len) {
+            len += sprintf(buf + len, "ssid:");
+            memcpy(buf + len, sec->param.passphrase.ssid.ssid,
+                   sec->param.passphrase.ssid.ssid_len);
+            len += sec->param.passphrase.ssid.ssid_len;
+            len += sprintf(buf + len, " ");
+        }
+        if (memcmp(&sec->param.passphrase.bssid, zero_mac, sizeof(zero_mac))) {
+            mac = (t_u8 *) & sec->param.passphrase.bssid;
+            len += sprintf(buf + len, "bssid:");
+            for (i = 0; i < ETH_ALEN - 1; ++i)
+                len += sprintf(buf + len, "%02x:", mac[i]);
+            len += sprintf(buf + len, "%02x ", mac[i]);
+        }
+        if (sec->param.passphrase.psk_type == MLAN_PSK_PMK) {
+            len += sprintf(buf + len, "psk:");
+            for (i = 0; i < MLAN_MAX_KEY_LENGTH; ++i)
+                len +=
+                    sprintf(buf + len, "%02x",
+                            sec->param.passphrase.psk.pmk.pmk[i]);
+            len += sprintf(buf + len, "\n");
+        }
+        if (sec->param.passphrase.psk_type == MLAN_PSK_PASSPHRASE) {
+            len +=
+                sprintf(buf + len, "passphrase:%s \n",
+                        sec->param.passphrase.psk.passphrase.passphrase);
+        }
+        if (wrq->u.data.pointer) {
+            if (copy_to_user(wrq->u.data.pointer, buf, MIN(len, sizeof(buf)))) {
+                PRINTM(MERROR, "Copy to user failed, len %d\n", len);
+                ret = -EFAULT;
+                goto done;
+            }
+            wrq->u.data.length = len;
+        }
+
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get esupp mode
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq      A pointer to iwreq structure
+ *
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_get_esupp_mode(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_ESUPP_MODE;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    req->action = MLAN_ACT_GET;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (copy_to_user
+        (wrq->u.data.pointer, (t_u8 *) & sec->param.esupp_mode,
+         sizeof(int) * 3)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    wrq->u.data.length = 3;
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/** AES key length */
+#define AES_KEY_LEN 16
+/**
+ *  @brief Adhoc AES control
+ *
+ *  @param priv	    A pointer to moal_private structure
+ *  @param wrq	    A pointer to user data
+ *
+ *  @return 	    0 --success, otherwise fail
+ */
+static int
+woal_adhoc_aes_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    static char buf[256];
+    int ret = 0, action = -1;
+    unsigned int i;
+    t_u8 key_ascii[32];
+    t_u8 key_hex[16];
+    t_u8 *tmp;
+    mlan_bss_info bss_info;
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_ioctl_req *req = NULL;
+    t_u8 bcast_addr[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+
+    ENTER();
+
+    memset(key_ascii, 0x00, sizeof(key_ascii));
+    memset(key_hex, 0x00, sizeof(key_hex));
+
+    /* Get current BSS information */
+    memset(&bss_info, 0, sizeof(bss_info));
+    woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info);
+    if (bss_info.bss_mode != MLAN_BSS_MODE_IBSS ||
+        bss_info.media_connected == MTRUE) {
+        PRINTM(MERROR, "STA is connected or not in IBSS mode.\n");
+        ret = -EOPNOTSUPP;
+        goto done;
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    if (wrq->u.data.length) {
+        if (wrq->u.data.length >= sizeof(buf)) {
+            PRINTM(MERROR, "Too many arguments\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        if (copy_from_user(buf, wrq->u.data.pointer, wrq->u.data.length)) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        buf[wrq->u.data.length] = '\0';
+
+        if (wrq->u.data.length == 1) {
+            /* Get Adhoc AES Key */
+            req->req_id = MLAN_IOCTL_SEC_CFG;
+            req->action = MLAN_ACT_GET;
+            sec = (mlan_ds_sec_cfg *) req->pbuf;
+            sec->sub_command = MLAN_OID_SEC_CFG_ENCRYPT_KEY;
+            sec->param.encrypt_key.key_len = AES_KEY_LEN;
+            sec->param.encrypt_key.key_index = MLAN_KEY_INDEX_UNICAST;
+            if (MLAN_STATUS_SUCCESS !=
+                woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+                ret = -EFAULT;
+                goto done;
+            }
+
+            memcpy(key_hex, sec->param.encrypt_key.key_material,
+                   sizeof(key_hex));
+            HEXDUMP("Adhoc AES Key (HEX)", key_hex, sizeof(key_hex));
+
+            wrq->u.data.length = sizeof(key_ascii) + 1;
+
+            tmp = key_ascii;
+            for (i = 0; i < sizeof(key_hex); i++)
+                tmp += sprintf((char *) tmp, "%02x", key_hex[i]);
+        } else if (wrq->u.data.length >= 2) {
+            /* Parse the buf to get the cmd_action */
+            action = woal_atox(&buf[0]);
+            if (action < 1 || action > 2) {
+                PRINTM(MERROR, "Invalid action argument %d\n", action);
+                ret = -EINVAL;
+                goto done;
+            }
+
+            req->req_id = MLAN_IOCTL_SEC_CFG;
+            req->action = MLAN_ACT_SET;
+            sec = (mlan_ds_sec_cfg *) req->pbuf;
+            sec->sub_command = MLAN_OID_SEC_CFG_ENCRYPT_KEY;
+
+            if (action == 1) {
+                /* Set Adhoc AES Key */
+                memcpy(key_ascii, &buf[2], sizeof(key_ascii));
+                woal_ascii2hex(key_hex, (char *) key_ascii, sizeof(key_hex));
+                HEXDUMP("Adhoc AES Key (HEX)", key_hex, sizeof(key_hex));
+
+                sec->param.encrypt_key.key_len = AES_KEY_LEN;
+                sec->param.encrypt_key.key_index = MLAN_KEY_INDEX_UNICAST;
+                sec->param.encrypt_key.key_flags =
+                    KEY_FLAG_SET_TX_KEY | KEY_FLAG_GROUP_KEY;
+                memcpy(sec->param.encrypt_key.mac_addr, (u8 *) bcast_addr,
+                       ETH_ALEN);
+                memcpy(sec->param.encrypt_key.key_material, key_hex,
+                       sec->param.encrypt_key.key_len);
+
+                if (MLAN_STATUS_SUCCESS !=
+                    woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+                    ret = -EFAULT;
+                    goto done;
+                }
+            } else if (action == 2) {
+                /* Clear Adhoc AES Key */
+                sec->param.encrypt_key.key_len = AES_KEY_LEN;
+                sec->param.encrypt_key.key_index = MLAN_KEY_INDEX_UNICAST;
+                sec->param.encrypt_key.key_flags = KEY_FLAG_REMOVE_KEY;
+                memcpy(sec->param.encrypt_key.mac_addr, (u8 *) bcast_addr,
+                       ETH_ALEN);
+                memset(sec->param.encrypt_key.key_material, 0,
+                       sizeof(sec->param.encrypt_key.key_material));
+
+                if (MLAN_STATUS_SUCCESS !=
+                    woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+                    ret = -EFAULT;
+                    goto done;
+                }
+            } else {
+                PRINTM(MERROR, "Invalid argument\n");
+                ret = -EINVAL;
+                goto done;
+            }
+        }
+
+        HEXDUMP("Adhoc AES Key (ASCII)", key_ascii, sizeof(key_ascii));
+        wrq->u.data.length = sizeof(key_ascii);
+        if (wrq->u.data.pointer) {
+            if (copy_to_user(wrq->u.data.pointer, &key_ascii,
+                             sizeof(key_ascii))) {
+                PRINTM(MERROR, "copy_to_user failed\n");
+                ret = -EFAULT;
+                goto done;
+            }
+        }
+    }
+
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief arpfilter ioctl function
+ *
+ *  @param priv		A pointer to moal_private structure
+ *  @param wrq 		A pointer to iwreq structure
+ *  @return    		0 --success, otherwise fail
+ */
+static int
+woal_arp_filter(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_misc_cfg *misc = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    misc = (mlan_ds_misc_cfg *) req->pbuf;
+    misc->sub_command = MLAN_OID_MISC_GEN_IE;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+    req->action = MLAN_ACT_SET;
+    misc->param.gen_ie.type = MLAN_IE_TYPE_ARP_FILTER;
+    misc->param.gen_ie.len = wrq->u.data.length;
+
+    /* get the whole command from user */
+    if (copy_from_user
+        (misc->param.gen_ie.ie_data, wrq->u.data.pointer, wrq->u.data.length)) {
+        PRINTM(MERROR, "copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/get IP address
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wrq          A pointer to iwreq structure
+ *  @return             0 --success, otherwise fail
+ */
+static int
+woal_set_get_ip_addr(moal_private * priv, struct iwreq *wrq)
+{
+    char buf[IPADDR_MAX_BUF];
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+    int ret = 0, op_code = 0, data_length = wrq->u.data.length;
+
+    ENTER();
+
+    memset(buf, 0, IPADDR_MAX_BUF);
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (ioctl_req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    misc = (mlan_ds_misc_cfg *) ioctl_req->pbuf;
+
+    if (data_length <= 1) {     /* GET */
+        ioctl_req->action = MLAN_ACT_GET;
+    } else {
+        if (copy_from_user(buf, wrq->u.data.pointer,
+                           MIN(IPADDR_MAX_BUF - 1, wrq->u.data.length))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        /* Make sure we have the operation argument */
+        if (data_length > 2 && buf[1] != ';') {
+            PRINTM(MERROR, "No operation argument. Separate with ';'\n");
+            ret = -EINVAL;
+            goto done;
+        } else {
+            buf[1] = '\0';
+        }
+        ioctl_req->action = MLAN_ACT_SET;
+        /* only one IP is supported in current firmware */
+        memset(misc->param.ipaddr_cfg.ip_addr[0], 0, IPADDR_LEN);
+        in4_pton(&buf[2], MIN((IPADDR_MAX_BUF - 3), (wrq->u.data.length - 2)),
+                 misc->param.ipaddr_cfg.ip_addr[0], ' ', NULL);
+        /* only one IP is supported in current firmware */
+        misc->param.ipaddr_cfg.ip_addr_num = 1;
+        misc->param.ipaddr_cfg.ip_addr_type = IPADDR_TYPE_IPV4;
+    }
+    if (woal_atoi(&op_code, &buf[0]) != MLAN_STATUS_SUCCESS) {
+        ret = -EINVAL;
+        goto done;
+    }
+    misc->param.ipaddr_cfg.op_code = (t_u32) op_code;
+    ioctl_req->req_id = MLAN_IOCTL_MISC_CFG;
+    misc->sub_command = MLAN_OID_MISC_IP_ADDR;
+
+    /* Send ioctl to mlan */
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (ioctl_req->action == MLAN_ACT_GET) {
+        snprintf(buf, IPADDR_MAX_BUF, "%d;%d.%d.%d.%d",
+                 misc->param.ipaddr_cfg.op_code,
+                 misc->param.ipaddr_cfg.ip_addr[0][0],
+                 misc->param.ipaddr_cfg.ip_addr[0][1],
+                 misc->param.ipaddr_cfg.ip_addr[0][2],
+                 misc->param.ipaddr_cfg.ip_addr[0][3]);
+        wrq->u.data.length = IPADDR_MAX_BUF;
+        if (copy_to_user(wrq->u.data.pointer, buf, IPADDR_MAX_BUF)) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+        }
+    }
+
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get Transmit beamforming capabilities
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq      A pointer to iwreq structure
+ *
+ *  @return         0 -- success, otherwise fail
+ */
+static int
+woal_tx_bf_cap_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0, data_length = wrq->u.data.length;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_11n_cfg *bf_cfg = NULL;
+    int bf_cap = 0;
+
+    ENTER();
+
+    if (data_length > 1) {
+        PRINTM(MERROR, "Invalid no of arguments!\n");
+        ret = -EINVAL;
+        goto done;
+    }
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11n_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    bf_cfg = (mlan_ds_11n_cfg *) req->pbuf;
+    req->req_id = MLAN_IOCTL_11N_CFG;
+    bf_cfg->sub_command = MLAN_OID_11N_CFG_TX_BF_CAP;
+    req->action = MLAN_ACT_GET;
+    if (data_length) {          /* SET */
+        if (copy_from_user(&bf_cap, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MERROR, "copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        bf_cfg->param.tx_bf_cap = bf_cap;
+        req->action = MLAN_ACT_SET;
+    }
+
+    /* Send IOCTL request to MLAN */
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    bf_cap = bf_cfg->param.tx_bf_cap;
+    if (copy_to_user(wrq->u.data.pointer, &bf_cap, sizeof(int))) {
+        ret = -EFAULT;
+        goto done;
+    }
+    wrq->u.data.length = 1;
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/* Maximum input output characters in group WOAL_SET_GET_256_CHAR */
+#define MAX_IN_OUT_CHAR     256
+/** Tx BF Global conf argument index */
+#define BF_ENABLE_PARAM     1
+#define SOUND_ENABLE_PARAM  2
+#define FB_TYPE_PARAM       3
+#define SNR_THRESHOLD_PARAM 4
+#define SOUND_INTVL_PARAM   5
+#define BF_MODE_PARAM       6
+#define MAX_TX_BF_GLOBAL_ARGS   6
+#define BF_CFG_ACT_GET      0
+#define BF_CFG_ACT_SET      1
+
+/**
+ *  @brief Set/Get Transmit beamforming configuration
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq      A pointer to iwreq structure
+ *
+ *  @return         0 -- success, otherwise fail
+ */
+static int
+woal_tx_bf_cfg_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0, data_length = wrq->u.data.length;
+    int bf_action = 0, interval = 0;
+    int snr = 0, i, tmp_val;
+    t_u8 buf[MAX_IN_OUT_CHAR], char_count = 0;
+    t_u8 *str, *token, *pos;
+    t_u16 action = 0;
+
+    mlan_ds_11n_tx_bf_cfg bf_cfg;
+    mlan_trigger_sound_args *bf_sound = NULL;
+    mlan_tx_bf_peer_args *tx_bf_peer = NULL;
+    mlan_snr_thr_args *bf_snr = NULL;
+    mlan_bf_periodicity_args *bf_periodicity = NULL;
+    mlan_bf_global_cfg_args *bf_global = NULL;
+
+    ENTER();
+
+    memset(&bf_cfg, 0, sizeof(bf_cfg));
+    /* Pointer to corresponding buffer */
+    bf_sound = bf_cfg.body.bf_sound;
+    tx_bf_peer = bf_cfg.body.tx_bf_peer;
+    bf_snr = bf_cfg.body.bf_snr;
+    bf_periodicity = bf_cfg.body.bf_periodicity;
+    bf_global = &bf_cfg.body.bf_global_cfg;
+
+    /* Total characters in buffer */
+    char_count = data_length - 1;
+    memset(buf, 0, sizeof(buf));
+    if (char_count) {
+        if (data_length > sizeof(buf)) {
+            PRINTM(MERROR, "Too many arguments\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        if (copy_from_user(buf, wrq->u.data.pointer, data_length)) {
+            PRINTM(MERROR, "copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+
+        if (char_count > 1 && buf[1] != ';') {
+            PRINTM(MERROR, "No action argument. Separate with ';'\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        /* Replace ';' with NULL in the string to separate args */
+        for (i = 0; i < char_count; i++) {
+            if (buf[i] == ';')
+                buf[i] = '\0';
+        }
+        /* The first byte represents the beamforming action */
+        if (woal_atoi(&bf_action, &buf[0]) != MLAN_STATUS_SUCCESS) {
+            ret = -EINVAL;
+            goto done;
+        }
+        switch (bf_action) {
+        case BF_GLOBAL_CONFIGURATION:
+            if (char_count == 1) {
+                action = MLAN_ACT_GET;
+                bf_cfg.action = BF_CFG_ACT_GET;
+            } else {
+                action = MLAN_ACT_SET;
+                bf_cfg.action = BF_CFG_ACT_SET;
+                /* Eliminate action field */
+                token = &buf[2];
+                for (i = 1, str = &buf[2]; token != NULL; i++) {
+                    token = strstr(str, " ");
+                    pos = str;
+                    if (token != NULL) {
+                        *token = '\0';
+                        str = token + 1;
+                    }
+                    woal_atoi(&tmp_val, pos);
+                    switch (i) {
+                    case BF_ENABLE_PARAM:
+                        bf_global->bf_enbl = (t_u8) tmp_val;
+                        break;
+                    case SOUND_ENABLE_PARAM:
+                        bf_global->sounding_enbl = (t_u8) tmp_val;
+                        break;
+                    case FB_TYPE_PARAM:
+                        bf_global->fb_type = (t_u8) tmp_val;
+                        break;
+                    case SNR_THRESHOLD_PARAM:
+                        bf_global->snr_threshold = (t_u8) tmp_val;
+                        break;
+                    case SOUND_INTVL_PARAM:
+                        bf_global->sounding_interval = (t_u16) tmp_val;
+                        break;
+                    case BF_MODE_PARAM:
+                        bf_global->bf_mode = (t_u8) tmp_val;
+                        break;
+                    default:
+                        PRINTM(MERROR, "Invalid Argument\n");
+                        ret = -EINVAL;
+                        goto done;
+                    }
+                }
+            }
+            break;
+        case TRIGGER_SOUNDING_FOR_PEER:
+            /* First arg = 2 BfAction Second arg = 17 MAC "00:50:43:20:BF:64" */
+            if (char_count != 19) {
+                PRINTM(MERROR, "Invalid argument\n");
+                ret = -EINVAL;
+                goto done;
+            }
+            woal_mac2u8(bf_sound->peer_mac, &buf[2]);
+            action = MLAN_ACT_SET;
+            bf_cfg.action = BF_CFG_ACT_SET;
+            break;
+        case SET_GET_BF_PERIODICITY:
+            /* First arg = 2 BfAction Second arg = 18 MAC "00:50:43:20:BF:64;"
+               Third arg = 1 (min char) TX BF interval 10 (max char) u32
+               maximum value 4294967295 */
+            if (char_count < 19 || char_count > 30) {
+                PRINTM(MERROR, "Invalid argument\n");
+                ret = -EINVAL;
+                goto done;
+            }
+
+            woal_mac2u8(bf_periodicity->peer_mac, &buf[2]);
+            if (char_count == 19) {
+                action = MLAN_ACT_GET;
+                bf_cfg.action = BF_CFG_ACT_GET;
+            } else {
+                action = MLAN_ACT_SET;
+                bf_cfg.action = BF_CFG_ACT_SET;
+                if (woal_atoi(&interval, &buf[20]) != MLAN_STATUS_SUCCESS) {
+                    ret = -EINVAL;
+                    goto done;
+                }
+                bf_periodicity->interval = interval;
+            }
+            break;
+        case TX_BF_FOR_PEER_ENBL:
+            /* Handle only SET operation here First arg = 2 BfAction Second arg
+               = 18 MAC "00:50:43:20:BF:64;" Third arg = 2 enable/disable bf
+               Fourth arg = 2 enable/disable sounding Fifth arg = 1 FB Type */
+            if (char_count != 25 && char_count != 1) {
+                PRINTM(MERROR, "Invalid argument\n");
+                ret = -EINVAL;
+                goto done;
+            }
+            if (char_count == 1) {
+                action = MLAN_ACT_GET;
+                bf_cfg.action = BF_CFG_ACT_GET;
+            } else {
+                woal_mac2u8(tx_bf_peer->peer_mac, &buf[2]);
+                woal_atoi(&tmp_val, &buf[20]);
+                tx_bf_peer->bf_enbl = (t_u8) tmp_val;
+                woal_atoi(&tmp_val, &buf[22]);
+                tx_bf_peer->sounding_enbl = (t_u8) tmp_val;
+                woal_atoi(&tmp_val, &buf[24]);
+                tx_bf_peer->fb_type = (t_u8) tmp_val;
+                action = MLAN_ACT_SET;
+                bf_cfg.action = BF_CFG_ACT_SET;
+            }
+            break;
+        case SET_SNR_THR_PEER:
+            /* First arg = 2 BfAction Second arg = 18 MAC "00:50:43:20:BF:64;"
+               Third arg = 1/2 SNR u8 - can be 1/2 charerters */
+            if (char_count != 1 && !(char_count == 21 || char_count == 22)) {
+                PRINTM(MERROR, "Invalid argument\n");
+                ret = -EINVAL;
+                goto done;
+            }
+            if (char_count == 1) {
+                action = MLAN_ACT_GET;
+                bf_cfg.action = BF_CFG_ACT_GET;
+            } else {
+                woal_mac2u8(bf_snr->peer_mac, &buf[2]);
+                if (woal_atoi(&snr, &buf[20]) != MLAN_STATUS_SUCCESS) {
+                    ret = -EINVAL;
+                    goto done;
+                }
+                bf_snr->snr = snr;
+                action = MLAN_ACT_SET;
+                bf_cfg.action = BF_CFG_ACT_SET;
+            }
+            break;
+        default:
+            ret = -EINVAL;
+            goto done;
+        }
+
+        /* Save the value */
+        bf_cfg.bf_action = bf_action;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_get_tx_bf_cfg(priv, action, &bf_cfg)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    } else {
+        ret = -EINVAL;
+        goto done;
+    }
+
+    if (action == MLAN_ACT_GET) {
+        data_length = 0;
+        memset(buf, 0, sizeof(buf));
+        switch (bf_action) {
+        case BF_GLOBAL_CONFIGURATION:
+            data_length +=
+                sprintf(buf + data_length, "%d ", (int) bf_global->bf_enbl);
+            data_length +=
+                sprintf(buf + data_length, "%d ",
+                        (int) bf_global->sounding_enbl);
+            data_length +=
+                sprintf(buf + data_length, "%d ", (int) bf_global->fb_type);
+            data_length +=
+                sprintf(buf + data_length, "%d ",
+                        (int) bf_global->snr_threshold);
+            data_length +=
+                sprintf(buf + data_length, "%d ",
+                        (int) bf_global->sounding_interval);
+            data_length +=
+                sprintf(buf + data_length, "%d ", (int) bf_global->bf_mode);
+            break;
+        case TRIGGER_SOUNDING_FOR_PEER:
+            data_length += sprintf(buf + data_length,
+                                   "%02x:%02x:%02x:%02x:%02x:%02x",
+                                   bf_sound->peer_mac[0], bf_sound->peer_mac[1],
+                                   bf_sound->peer_mac[2], bf_sound->peer_mac[3],
+                                   bf_sound->peer_mac[4],
+                                   bf_sound->peer_mac[5]);
+            data_length += sprintf(buf + data_length, "%c", ';');
+            data_length += sprintf(buf + data_length, "%d", bf_sound->status);
+            break;
+        case SET_GET_BF_PERIODICITY:
+            data_length += sprintf(buf + data_length,
+                                   "%02x:%02x:%02x:%02x:%02x:%02x",
+                                   bf_periodicity->peer_mac[0],
+                                   bf_periodicity->peer_mac[1],
+                                   bf_periodicity->peer_mac[2],
+                                   bf_periodicity->peer_mac[3],
+                                   bf_periodicity->peer_mac[4],
+                                   bf_periodicity->peer_mac[5]);
+            data_length += sprintf(buf + data_length, "%c", ' ');
+            data_length +=
+                sprintf(buf + data_length, "%d", bf_periodicity->interval);
+            break;
+        case TX_BF_FOR_PEER_ENBL:
+            for (i = 0; i < bf_cfg.no_of_peers; i++) {
+                data_length += sprintf(buf + data_length,
+                                       "%02x:%02x:%02x:%02x:%02x:%02x",
+                                       tx_bf_peer->peer_mac[0],
+                                       tx_bf_peer->peer_mac[1],
+                                       tx_bf_peer->peer_mac[2],
+                                       tx_bf_peer->peer_mac[3],
+                                       tx_bf_peer->peer_mac[4],
+                                       tx_bf_peer->peer_mac[5]);
+                data_length += sprintf(buf + data_length, "%c", ' ');
+                data_length +=
+                    sprintf(buf + data_length, "%d;", tx_bf_peer->bf_enbl);
+                data_length +=
+                    sprintf(buf + data_length, "%d;",
+                            tx_bf_peer->sounding_enbl);
+                data_length +=
+                    sprintf(buf + data_length, "%d ", tx_bf_peer->fb_type);
+                tx_bf_peer++;
+            }
+            break;
+        case SET_SNR_THR_PEER:
+            for (i = 0; i < bf_cfg.no_of_peers; i++) {
+                data_length += sprintf(buf + data_length,
+                                       "%02x:%02x:%02x:%02x:%02x:%02x",
+                                       bf_snr->peer_mac[0], bf_snr->peer_mac[1],
+                                       bf_snr->peer_mac[2], bf_snr->peer_mac[3],
+                                       bf_snr->peer_mac[4],
+                                       bf_snr->peer_mac[5]);
+                data_length += sprintf(buf + data_length, "%c", ';');
+                data_length += sprintf(buf + data_length, "%d", bf_snr->snr);
+                data_length += sprintf(buf + data_length, "%c", ' ');
+                bf_snr++;
+            }
+            break;
+        default:
+            ret = -EINVAL;
+            goto done;
+        }
+        buf[data_length] = '\0';
+    }
+
+    wrq->u.data.length = data_length;
+    if (copy_to_user(wrq->u.data.pointer, buf, wrq->u.data.length)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Create a brief scan resp to relay basic BSS info to the app layer
+ *
+ *  When the beacon/probe response has not been buffered, use the saved BSS
+ *    information available to provide a minimum response for the application
+ *    ioctl retrieval routines.  Include:
+ *        - Timestamp
+ *        - Beacon Period
+ *        - Capabilities (including WMM Element if available)
+ *        - SSID
+ *
+ *  @param ppbuffer  Output parameter: Buffer used to create basic scan rsp
+ *  @param pbss_desc Pointer to a BSS entry in the scan table to create
+ *                   scan response from for delivery to the application layer
+ *
+ *  @return          N/A
+ */
+static void
+wlan_scan_create_brief_table_entry(t_u8 ** ppbuffer,
+                                   BSSDescriptor_t * pbss_desc)
+{
+    t_u8 *ptmp_buf = *ppbuffer;
+    t_u8 tmp_ssid_hdr[2];
+    t_u8 ie_len = 0;
+
+    ENTER();
+
+    if (copy_to_user(ptmp_buf, pbss_desc->time_stamp,
+                     sizeof(pbss_desc->time_stamp))) {
+        PRINTM(MINFO, "Copy to user failed\n");
+        LEAVE();
+        return;
+    }
+    ptmp_buf += sizeof(pbss_desc->time_stamp);
+
+    if (copy_to_user(ptmp_buf, &pbss_desc->beacon_period,
+                     sizeof(pbss_desc->beacon_period))) {
+        PRINTM(MINFO, "Copy to user failed\n");
+        LEAVE();
+        return;
+    }
+    ptmp_buf += sizeof(pbss_desc->beacon_period);
+
+    if (copy_to_user
+        (ptmp_buf, &pbss_desc->cap_info, sizeof(pbss_desc->cap_info))) {
+        PRINTM(MINFO, "Copy to user failed\n");
+        LEAVE();
+        return;
+    }
+    ptmp_buf += sizeof(pbss_desc->cap_info);
+
+    tmp_ssid_hdr[0] = 0;        /* Element ID for SSID is zero */
+    tmp_ssid_hdr[1] = pbss_desc->ssid.ssid_len;
+    if (copy_to_user(ptmp_buf, tmp_ssid_hdr, sizeof(tmp_ssid_hdr))) {
+        PRINTM(MINFO, "Copy to user failed\n");
+        LEAVE();
+        return;
+    }
+    ptmp_buf += sizeof(tmp_ssid_hdr);
+
+    if (copy_to_user(ptmp_buf, pbss_desc->ssid.ssid, pbss_desc->ssid.ssid_len)) {
+        PRINTM(MINFO, "Copy to user failed\n");
+        LEAVE();
+        return;
+    }
+    ptmp_buf += pbss_desc->ssid.ssid_len;
+
+    if (pbss_desc->wmm_ie.vend_hdr.element_id == WMM_IE) {
+        ie_len = sizeof(IEEEtypes_Header_t) + pbss_desc->wmm_ie.vend_hdr.len;
+        if (copy_to_user(ptmp_buf, &pbss_desc->wmm_ie, ie_len)) {
+            PRINTM(MINFO, "Copy to user failed\n");
+            LEAVE();
+            return;
+        }
+
+        ptmp_buf += ie_len;
+    }
+
+    if (pbss_desc->pwpa_ie) {
+        if ((*(pbss_desc->pwpa_ie)).vend_hdr.element_id == WPA_IE) {
+            ie_len =
+                sizeof(IEEEtypes_Header_t) +
+                (*(pbss_desc->pwpa_ie)).vend_hdr.len;
+            if (copy_to_user(ptmp_buf, pbss_desc->pwpa_ie, ie_len)) {
+                PRINTM(MINFO, "Copy to user failed\n");
+                LEAVE();
+                return;
+            }
+        }
+
+        ptmp_buf += ie_len;
+    }
+
+    if (pbss_desc->prsn_ie) {
+        if ((*(pbss_desc->prsn_ie)).ieee_hdr.element_id == RSN_IE) {
+            ie_len =
+                sizeof(IEEEtypes_Header_t) +
+                (*(pbss_desc->prsn_ie)).ieee_hdr.len;
+            if (copy_to_user(ptmp_buf, pbss_desc->prsn_ie, ie_len)) {
+                PRINTM(MINFO, "Copy to user failed\n");
+                LEAVE();
+                return;
+            }
+        }
+
+        ptmp_buf += ie_len;
+    }
+
+    *ppbuffer = ptmp_buf;
+    LEAVE();
+}
+
+/**
+ *  @brief Create a wlan_ioctl_get_scan_table_entry for a given BSS
+ *         Descriptor for inclusion in the ioctl response to the user space
+ *         application.
+ *
+ *
+ *  @param pbss_desc   Pointer to a BSS entry in the scan table to form
+ *                     scan response from for delivery to the application layer
+ *  @param ppbuffer    Output parameter: Buffer used to output scan return struct
+ *  @param pspace_left Output parameter: Number of bytes available in the
+ *                     response buffer.
+ *
+ *  @return MLAN_STATUS_SUCCESS, or < 0 with IOCTL error code
+ */
+static int
+wlan_get_scan_table_ret_entry(BSSDescriptor_t * pbss_desc,
+                              t_u8 ** ppbuffer, int *pspace_left)
+{
+    wlan_ioctl_get_scan_table_entry *prsp_entry;
+    wlan_ioctl_get_scan_table_entry tmp_rsp_entry;
+    int space_needed;
+    t_u8 *pcurrent;
+    int variable_size;
+
+    const int fixed_size = sizeof(wlan_ioctl_get_scan_table_entry);
+
+    ENTER();
+
+    pcurrent = *ppbuffer;
+
+    /* The variable size returned is the stored beacon size */
+    variable_size = pbss_desc->beacon_buf_size;
+
+    /* If we stored a beacon and its size was zero, set the variable size
+       return value to the size of the brief scan response
+       wlan_scan_create_brief_table_entry creates.  Also used if we are not
+       configured to store beacons in the first place */
+    if (!variable_size) {
+        variable_size = pbss_desc->ssid.ssid_len + 2;
+        variable_size += (sizeof(pbss_desc->beacon_period)
+                          + sizeof(pbss_desc->time_stamp)
+                          + sizeof(pbss_desc->cap_info));
+        if (pbss_desc->wmm_ie.vend_hdr.element_id == WMM_IE) {
+            variable_size += (sizeof(IEEEtypes_Header_t)
+                              + pbss_desc->wmm_ie.vend_hdr.len);
+        }
+
+        if (pbss_desc->pwpa_ie) {
+            if ((*(pbss_desc->pwpa_ie)).vend_hdr.element_id == WPA_IE) {
+                variable_size += (sizeof(IEEEtypes_Header_t)
+                                  + (*(pbss_desc->pwpa_ie)).vend_hdr.len);
+            }
+        }
+
+        if (pbss_desc->prsn_ie) {
+            if ((*(pbss_desc->prsn_ie)).ieee_hdr.element_id == RSN_IE) {
+                variable_size += (sizeof(IEEEtypes_Header_t)
+                                  + (*(pbss_desc->prsn_ie)).ieee_hdr.len);
+            }
+        }
+    }
+
+    space_needed = fixed_size + variable_size;
+
+    PRINTM(MINFO, "GetScanTable: need(%d), left(%d)\n",
+           space_needed, *pspace_left);
+
+    if (space_needed >= *pspace_left) {
+        *pspace_left = 0;
+        LEAVE();
+        return -E2BIG;
+    }
+
+    *pspace_left -= space_needed;
+
+    tmp_rsp_entry.fixed_field_length = (sizeof(tmp_rsp_entry)
+                                        -
+                                        sizeof(tmp_rsp_entry.fixed_field_length)
+                                        -
+                                        sizeof(tmp_rsp_entry.bss_info_length));
+
+    memcpy(tmp_rsp_entry.fixed_fields.bssid,
+           pbss_desc->mac_address, sizeof(prsp_entry->fixed_fields.bssid));
+
+    tmp_rsp_entry.fixed_fields.rssi = pbss_desc->rssi;
+    tmp_rsp_entry.fixed_fields.channel = pbss_desc->channel;
+    tmp_rsp_entry.fixed_fields.network_tsf = pbss_desc->network_tsf;
+    tmp_rsp_entry.bss_info_length = variable_size;
+
+    /*
+     *  Copy fixed fields to user space
+     */
+    if (copy_to_user(pcurrent, &tmp_rsp_entry, fixed_size)) {
+        PRINTM(MINFO, "Copy to user failed\n");
+        LEAVE();
+        return -EFAULT;
+    }
+
+    pcurrent += fixed_size;
+
+    if (pbss_desc->pbeacon_buf) {
+        /*
+         *  Copy variable length elements to user space
+         */
+        if (copy_to_user(pcurrent, pbss_desc->pbeacon_buf,
+                         pbss_desc->beacon_buf_size)) {
+            PRINTM(MINFO, "Copy to user failed\n");
+            LEAVE();
+            return -EFAULT;
+        }
+
+        pcurrent += pbss_desc->beacon_buf_size;
+    } else {
+        wlan_scan_create_brief_table_entry(&pcurrent, pbss_desc);
+    }
+
+    *ppbuffer = pcurrent;
+
+    LEAVE();
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Retrieve the scan response/beacon table
+ *
+ *  @param wrq          A pointer to iwreq structure
+ *  @param scan_resp    A pointer to mlan_scan_resp structure
+ *  @param scan_start   argument
+ *
+ *  @return             MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static int
+moal_ret_get_scan_table_ioctl(struct iwreq *wrq,
+                              mlan_scan_resp * scan_resp, t_u32 scan_start)
+{
+    pBSSDescriptor_t pbss_desc, scan_table;
+    wlan_ioctl_get_scan_table_info *prsp_info;
+    int ret_code;
+    int ret_len;
+    int space_left;
+    t_u8 *pcurrent;
+    t_u8 *pbuffer_end;
+    t_u32 num_scans_done;
+
+    ENTER();
+
+    num_scans_done = 0;
+    ret_code = MLAN_STATUS_SUCCESS;
+
+    prsp_info = (wlan_ioctl_get_scan_table_info *) wrq->u.data.pointer;
+    pcurrent = (t_u8 *) prsp_info->scan_table_entry_buf;
+
+    pbuffer_end = wrq->u.data.pointer + wrq->u.data.length - 1;
+    space_left = pbuffer_end - pcurrent;
+    scan_table = (BSSDescriptor_t *) (scan_resp->pscan_table);
+
+    PRINTM(MINFO, "GetScanTable: scan_start req = %d\n", scan_start);
+    PRINTM(MINFO, "GetScanTable: length avail = %d\n", wrq->u.data.length);
+
+    if (!scan_start) {
+        PRINTM(MINFO, "GetScanTable: get current BSS Descriptor\n");
+
+        /* Use to get current association saved descriptor */
+        pbss_desc = scan_table;
+
+        ret_code = wlan_get_scan_table_ret_entry(pbss_desc,
+                                                 &pcurrent, &space_left);
+
+        if (ret_code == MLAN_STATUS_SUCCESS) {
+            num_scans_done = 1;
+        }
+    } else {
+        scan_start--;
+
+        while (space_left
+               && (scan_start + num_scans_done < scan_resp->num_in_scan_table)
+               && (ret_code == MLAN_STATUS_SUCCESS)) {
+
+            pbss_desc = (scan_table + (scan_start + num_scans_done));
+
+            PRINTM(MINFO, "GetScanTable: get current BSS Descriptor [%d]\n",
+                   scan_start + num_scans_done);
+
+            ret_code = wlan_get_scan_table_ret_entry(pbss_desc,
+                                                     &pcurrent, &space_left);
+
+            if (ret_code == MLAN_STATUS_SUCCESS) {
+                num_scans_done++;
+            }
+        }
+    }
+
+    prsp_info->scan_number = num_scans_done;
+    ret_len = pcurrent - (t_u8 *) wrq->u.data.pointer;
+
+    wrq->u.data.length = ret_len;
+
+    /* Return ret_code (EFAULT or E2BIG) in the case where no scan results were
+       successfully encoded. */
+    LEAVE();
+    return (num_scans_done ? MLAN_STATUS_SUCCESS : ret_code);
+}
+
+/**
+ *  @brief Get scan table ioctl
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq 		A pointer to iwreq structure
+ *
+ *  @return         MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+static mlan_status
+woal_get_scan_table_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_scan *scan = NULL;
+    t_u32 scan_start;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_scan));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    scan = (mlan_ds_scan *) req->pbuf;
+    req->req_id = MLAN_IOCTL_SCAN;
+    req->action = MLAN_ACT_GET;
+
+    /* get the whole command from user */
+    if (copy_from_user(&scan_start, wrq->u.data.pointer, sizeof(scan_start))) {
+        PRINTM(MERROR, "copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (scan_start) {
+        scan->sub_command = MLAN_OID_SCAN_NORMAL;
+    } else {
+        scan->sub_command = MLAN_OID_SCAN_GET_CURRENT_BSS;
+    }
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT);
+    if (status == MLAN_STATUS_SUCCESS) {
+        status = moal_ret_get_scan_table_ioctl(wrq,
+                                               &scan->param.scan_resp,
+                                               scan_start);
+    }
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Set user scan ext -- Async mode, without wait
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq 		A pointer to iwreq structure
+ *
+ *  @return         0 -- success, otherwise fail
+ */
+static int
+woal_set_user_scan_ext_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    wlan_user_scan_cfg scan_req;
+    ENTER();
+    memset(&scan_req, 0x00, sizeof(scan_req));
+    if (copy_from_user
+        (&scan_req, wrq->u.data.pointer,
+         MIN(wrq->u.data.length, sizeof(scan_req)))) {
+        PRINTM(MINFO, "Copy from user failed\n");
+        LEAVE();
+        return -EFAULT;
+    }
+    if (MLAN_STATUS_FAILURE == woal_do_scan(priv, &scan_req))
+        ret = -EFAULT;
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set user scan
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq 		A pointer to iwreq structure
+ *
+ *  @return         MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+static mlan_status
+woal_set_user_scan_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_scan *scan = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    union iwreq_data wrqu;
+    moal_handle *handle = priv->phandle;
+
+    ENTER();
+
+    if (handle->scan_pending_on_block == MTRUE) {
+        PRINTM(MINFO, "scan already in processing...\n");
+        LEAVE();
+        return ret;
+    }
+    if (MOAL_ACQ_SEMAPHORE_BLOCK(&handle->async_sem)) {
+        PRINTM(MERROR, "Acquire semaphore error, request_scan\n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    handle->scan_pending_on_block = MTRUE;
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_scan) + wrq->u.data.length);
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    scan = (mlan_ds_scan *) req->pbuf;
+    scan->sub_command = MLAN_OID_SCAN_USER_CONFIG;
+    req->req_id = MLAN_IOCTL_SCAN;
+    req->action = MLAN_ACT_SET;
+
+    if (copy_from_user(scan->param.user_scan.scan_cfg_buf,
+                       wrq->u.data.pointer, wrq->u.data.length)) {
+        PRINTM(MINFO, "Copy from user failed\n");
+        LEAVE();
+        return -EFAULT;
+    }
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT);
+    if (status == MLAN_STATUS_SUCCESS) {
+        memset(&wrqu, 0, sizeof(union iwreq_data));
+        wireless_send_event(priv->netdev, SIOCGIWSCAN, &wrqu, NULL);
+    }
+    handle->scan_pending_on_block = MFALSE;
+    MOAL_REL_SEMAPHORE(&handle->async_sem);
+
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Cmd52 read/write register
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wrq          A pointer to iwreq structure
+ *  @return             MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static int
+woal_cmd52rdwr_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    t_u8 rw = 0, func, data = 0;
+    int buf[3], reg, ret = MLAN_STATUS_SUCCESS;
+    int data_length = wrq->u.data.length;
+
+    ENTER();
+
+    if (data_length < 2 || data_length > 3) {
+        PRINTM(MERROR, "Invalid number of arguments\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    if (copy_from_user(buf, wrq->u.data.pointer, sizeof(int) * data_length)) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    func = (t_u8) buf[0];
+    if (func > 7) {
+        PRINTM(MERROR, "Invalid function number!\n");
+        ret = -EINVAL;
+        goto done;
+    }
+    reg = (t_u32) buf[1];
+    if (data_length == 2) {
+        rw = 0;                 /* CMD52 read */
+        PRINTM(MINFO, "Cmd52 read, func=%d, reg=0x%08X\n", func, reg);
+    }
+    if (data_length == 3) {
+        rw = 1;                 /* CMD52 write */
+        data = (t_u8) buf[2];
+        PRINTM(MINFO, "Cmd52 write, func=%d, reg=0x%08X, data=0x%02X\n", func,
+               reg, data);
+    }
+
+    if (!rw) {
+        sdio_claim_host(((struct sdio_mmc_card *) priv->phandle->card)->func);
+        if (func)
+            data =
+                sdio_readb(((struct sdio_mmc_card *) priv->phandle->card)->func,
+                           reg, &ret);
+        else
+            data =
+                sdio_f0_readb(((struct sdio_mmc_card *) priv->phandle->card)->
+                              func, reg, &ret);
+        sdio_release_host(((struct sdio_mmc_card *) priv->phandle->card)->func);
+        if (ret) {
+            PRINTM(MERROR, "sdio_readb: reading register 0x%X failed\n", reg);
+            goto done;
+        }
+    } else {
+        sdio_claim_host(((struct sdio_mmc_card *) priv->phandle->card)->func);
+        if (func)
+            sdio_writeb(((struct sdio_mmc_card *) priv->phandle->card)->func,
+                        data, reg, &ret);
+        else
+            sdio_f0_writeb(((struct sdio_mmc_card *) priv->phandle->card)->func,
+                           data, reg, &ret);
+        sdio_release_host(((struct sdio_mmc_card *) priv->phandle->card)->func);
+        if (ret) {
+            PRINTM(MERROR, "sdio_writeb: writing register 0x%X failed\n", reg);
+            goto done;
+        }
+    }
+
+    buf[0] = data;
+    wrq->u.data.length = 1;
+    if (copy_to_user(wrq->u.data.pointer, buf, sizeof(int))) {
+        PRINTM(MERROR, "Copy to user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Cmd53 read/write register
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wrq          A pointer to iwreq structure
+ *  @return             MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static int
+woal_cmd53rdwr_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    t_u8 *buf = NULL;
+    t_u8 rw, func, mode;
+    t_u16 blklen = 0, blknum = 0;
+    int reg = 0, pattern_len = 0, pos = 0, ret = MLAN_STATUS_SUCCESS;
+    t_u32 total_len = 0;
+    t_u8 *data = NULL;
+
+    ENTER();
+
+    if (!(buf = (t_u8 *) kmalloc(WOAL_2K_BYTES, GFP_KERNEL))) {
+        PRINTM(MERROR, "Cannot allocate buffer for command!\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (!(data = (t_u8 *) kmalloc(WOAL_2K_BYTES, GFP_KERNEL))) {
+        PRINTM(MERROR, "Cannot allocate buffer for command!\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (wrq->u.data.length > WOAL_2K_BYTES) {
+        PRINTM(MERROR, "Data lengh is too large!\n");
+        ret = -EINVAL;
+        goto done;
+    }
+    if (copy_from_user(buf, wrq->u.data.pointer, wrq->u.data.length)) {
+        PRINTM(MINFO, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    rw = buf[0];                /* read/write (0/1) */
+    func = buf[1];              /* func (0/1/2) */
+    reg = buf[5];               /* address */
+    reg = (reg << 8) + buf[4];
+    reg = (reg << 8) + buf[3];
+    reg = (reg << 8) + buf[2];
+    mode = buf[6];              /* byte mode/block mode (0/1) */
+    blklen = buf[8];            /* block size */
+    blklen = (blklen << 8) + buf[7];
+    blknum = buf[10];           /* block number or byte number */
+    blknum = (blknum << 8) + buf[9];
+
+    if (mode != BYTE_MODE)
+        mode = BLOCK_MODE;
+    total_len = (mode == BLOCK_MODE) ? blknum * blklen : blknum;
+    if (total_len > WOAL_2K_BYTES) {
+        PRINTM(MERROR, "Total data length is too large!\n");
+        ret = -EINVAL;
+        goto done;
+    }
+    PRINTM(MINFO, "CMD53 read/write, func = %d, addr = %#x, mode = %d, "
+           "block size = %d, block(byte) number = %d\n",
+           func, reg, mode, blklen, blknum);
+
+    if (!rw) {
+        sdio_claim_host(((struct sdio_mmc_card *) priv->phandle->card)->func);
+        if (sdio_readsb
+            (((struct sdio_mmc_card *) priv->phandle->card)->func, data, reg,
+             total_len))
+            PRINTM(MERROR, "sdio_readsb: reading memory 0x%x failed\n", reg);
+        sdio_release_host(((struct sdio_mmc_card *) priv->phandle->card)->func);
+
+        if (copy_to_user(wrq->u.data.pointer, data, total_len)) {
+            PRINTM(MINFO, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = total_len;
+    } else {
+        pattern_len = wrq->u.data.length - 11;
+        if (pattern_len > total_len)
+            pattern_len = total_len;
+        memset(data, 0, sizeof(data));
+
+        /* Copy/duplicate the pattern to data buffer */
+        for (pos = 0; pos < total_len; pos++)
+            data[pos] = buf[11 + (pos % pattern_len)];
+
+        sdio_claim_host(((struct sdio_mmc_card *) priv->phandle->card)->func);
+        if (sdio_writesb
+            (((struct sdio_mmc_card *) priv->phandle->card)->func, reg, data,
+             total_len))
+            PRINTM(MERROR, "sdio_writesb: writing memory 0x%x failed\n", reg);
+        sdio_release_host(((struct sdio_mmc_card *) priv->phandle->card)->func);
+    }
+
+  done:
+    if (buf)
+        kfree(buf);
+    if (data)
+        kfree(data);
+    LEAVE();
+    return ret;
+}
+
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+/**
+ * @brief Set SDIO Multi-point aggregation control parameters
+ *
+ * @param priv     A pointer to moal_private structure
+ * @param wrq      A pointer to iwreq structure
+ *
+ * @return         0/MLAN_STATUS_PENDING --success, otherwise fail
+ */
+static int
+woal_do_sdio_mpa_ctrl(moal_private * priv, struct iwreq *wrq)
+{
+    int data[6], data_length = wrq->u.data.length;
+    int ret = 0;
+    mlan_ds_misc_cfg *misc = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    if (sizeof(int) * wrq->u.data.length > sizeof(data)) {
+        PRINTM(MERROR, "Too many arguments\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    misc = (mlan_ds_misc_cfg *) req->pbuf;
+    memset(misc, 0, sizeof(mlan_ds_misc_cfg));
+
+    misc->sub_command = MLAN_OID_MISC_SDIO_MPA_CTRL;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+
+    /* Get the values first, then modify these values if user had modified them
+     */
+
+    req->action = MLAN_ACT_GET;
+    if ((ret = woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) !=
+        MLAN_STATUS_SUCCESS) {
+        PRINTM(MERROR, "woal_request_ioctl returned %d\n", ret);
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (data_length == 0) {
+        data[0] = misc->param.mpa_ctrl.tx_enable;
+        data[1] = misc->param.mpa_ctrl.rx_enable;
+        data[2] = misc->param.mpa_ctrl.tx_buf_size;
+        data[3] = misc->param.mpa_ctrl.rx_buf_size;
+        data[4] = misc->param.mpa_ctrl.tx_max_ports;
+        data[5] = misc->param.mpa_ctrl.rx_max_ports;
+
+        PRINTM(MINFO, "Get Param: %d %d %d %d %d %d\n", data[0], data[1],
+               data[2], data[3], data[4], data[5]);
+
+        if (copy_to_user(wrq->u.data.pointer, data, sizeof(data))) {
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = sizeof(data) / sizeof(int);
+        goto done;
+    }
+
+    if (copy_from_user(data, wrq->u.data.pointer, sizeof(int) * data_length)) {
+        PRINTM(MINFO, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    switch (data_length) {
+    case 6:
+        misc->param.mpa_ctrl.rx_max_ports = data[5];
+    case 5:
+        misc->param.mpa_ctrl.tx_max_ports = data[4];
+    case 4:
+        misc->param.mpa_ctrl.rx_buf_size = data[3];
+    case 3:
+        misc->param.mpa_ctrl.tx_buf_size = data[2];
+    case 2:
+        misc->param.mpa_ctrl.rx_enable = data[1];
+    case 1:
+        /* Set cmd */
+        req->action = MLAN_ACT_SET;
+
+        PRINTM(MINFO, "Set Param: %d %d %d %d %d %d\n", data[0], data[1],
+               data[2], data[3], data[4], data[5]);
+
+        misc->param.mpa_ctrl.tx_enable = data[0];
+        break;
+    default:
+        PRINTM(MERROR, "Default case error\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    if ((ret = woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT) !=
+         MLAN_STATUS_SUCCESS)) {
+        PRINTM(MERROR, "woal_request_ioctl returned %d\n", ret);
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+#endif /* SDIO_MULTI_PORT_TX_AGGR || SDIO_MULTI_PORT_RX_AGGR */
+
+/**
+ * @brief Set/Get scan configuration parameters
+ *
+ * @param priv     A pointer to moal_private structure
+ * @param wrq      A pointer to iwreq structure
+ *
+ * @return         0 --success, otherwise fail
+ */
+static int
+woal_set_get_scan_cfg(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    int arg_len = 7;
+    int data[arg_len];
+    mlan_ds_scan *scan = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_scan));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    if (wrq->u.data.length > arg_len) {
+        ret = -EINVAL;
+        goto done;
+    }
+    scan = (mlan_ds_scan *) req->pbuf;
+    scan->sub_command = MLAN_OID_SCAN_CONFIG;
+    req->req_id = MLAN_IOCTL_SCAN;
+    memset(data, 0, sizeof(data));
+    if (wrq->u.data.length) {
+        if (copy_from_user
+            (data, wrq->u.data.pointer, (wrq->u.data.length * sizeof(int)))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if ((data[0] < 0) || (data[0] > MLAN_SCAN_TYPE_PASSIVE)) {
+            PRINTM(MERROR, "Invalid argument for scan type\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        if ((data[1] < 0) || (data[1] > MLAN_SCAN_MODE_ANY)) {
+            PRINTM(MERROR, "Invalid argument for scan mode\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        if ((data[2] < 0) || (data[2] > MAX_PROBES)) {
+            PRINTM(MERROR, "Invalid argument for scan probes\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        if (((data[3] < 0) || (data[3] > MRVDRV_MAX_ACTIVE_SCAN_CHAN_TIME)) ||
+            ((data[4] < 0) || (data[4] > MRVDRV_MAX_ACTIVE_SCAN_CHAN_TIME)) ||
+            ((data[5] < 0) || (data[5] > MRVDRV_MAX_PASSIVE_SCAN_CHAN_TIME))) {
+            PRINTM(MERROR, "Invalid argument for scan time\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        if ((data[6] < 0) || (data[6] > 1)) {
+            PRINTM(MERROR, "Invalid argument for extended scan\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        req->action = MLAN_ACT_SET;
+        memcpy(&scan->param.scan_cfg, data, sizeof(data));
+    } else
+        req->action = MLAN_ACT_GET;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (!wrq->u.data.length) {
+        memcpy(data, &scan->param.scan_cfg, sizeof(data));
+        if (copy_to_user(wrq->u.data.pointer, data, sizeof(data))) {
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = sizeof(data) / sizeof(int);
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Set/Get PS configuration parameters
+ *
+ * @param priv     A pointer to moal_private structure
+ * @param wrq      A pointer to iwreq structure
+ *
+ * @return         0 --success, otherwise fail
+ */
+static int
+woal_set_get_ps_cfg(moal_private * priv, struct iwreq *wrq)
+{
+    int data[7], ret = 0;
+    mlan_ds_pm_cfg *pm_cfg = NULL;
+    mlan_ioctl_req *req = NULL;
+    int allowed = 3;
+    int i = 3;
+
+    ENTER();
+
+    allowed++;                  /* For ad-hoc awake period parameter */
+    allowed++;                  /* For beacon missing timeout parameter */
+    allowed += 2;               /* For delay to PS and PS mode parameters */
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_pm_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    if (wrq->u.data.length > allowed) {
+        ret = -EINVAL;
+        goto done;
+    }
+    pm_cfg = (mlan_ds_pm_cfg *) req->pbuf;
+    pm_cfg->sub_command = MLAN_OID_PM_CFG_PS_CFG;
+    req->req_id = MLAN_IOCTL_PM_CFG;
+    memset(data, 0, sizeof(data));
+    if (wrq->u.data.length) {
+        if (copy_from_user
+            (data, wrq->u.data.pointer, (wrq->u.data.length * sizeof(int)))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if ((data[0] < PS_NULL_DISABLE)) {
+            PRINTM(MERROR, "Invalid argument for PS null interval\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        if ((data[1] != MRVDRV_IGNORE_MULTIPLE_DTIM)
+            && (data[1] != MRVDRV_MATCH_CLOSEST_DTIM)
+            && ((data[1] < MRVDRV_MIN_MULTIPLE_DTIM)
+                || (data[1] > MRVDRV_MAX_MULTIPLE_DTIM))) {
+            PRINTM(MERROR, "Invalid argument for multiple DTIM\n");
+            ret = -EINVAL;
+            goto done;
+        }
+
+        if ((data[2] < MRVDRV_MIN_LISTEN_INTERVAL)
+            && (data[2] != MRVDRV_LISTEN_INTERVAL_DISABLE)) {
+            PRINTM(MERROR, "Invalid argument for listen interval\n");
+            ret = -EINVAL;
+            goto done;
+        }
+
+        if ((data[i] != SPECIAL_ADHOC_AWAKE_PD) &&
+            ((data[i] < MIN_ADHOC_AWAKE_PD) ||
+             (data[i] > MAX_ADHOC_AWAKE_PD))) {
+            PRINTM(MERROR, "Invalid argument for adhoc awake period\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        i++;
+        if ((data[i] != DISABLE_BCN_MISS_TO) &&
+            ((data[i] < MIN_BCN_MISS_TO) || (data[i] > MAX_BCN_MISS_TO))) {
+            PRINTM(MERROR, "Invalid argument for beacon miss timeout\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        i++;
+        if (wrq->u.data.length < allowed - 1)
+            data[i] = DELAY_TO_PS_UNCHANGED;
+        else if ((data[i] < MIN_DELAY_TO_PS) || (data[i] > MAX_DELAY_TO_PS)) {
+            PRINTM(MERROR, "Invalid argument for delay to PS\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        i++;
+        if ((data[i] != PS_MODE_UNCHANGED) && (data[i] != PS_MODE_AUTO) &&
+            (data[i] != PS_MODE_POLL) && (data[i] != PS_MODE_NULL)) {
+            PRINTM(MERROR, "Invalid argument for PS mode\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        i++;
+        req->action = MLAN_ACT_SET;
+        memcpy(&pm_cfg->param.ps_cfg, data,
+               MIN(sizeof(pm_cfg->param.ps_cfg), sizeof(data)));
+    } else
+        req->action = MLAN_ACT_GET;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    memcpy(data, &pm_cfg->param.ps_cfg,
+           MIN((sizeof(int) * allowed), sizeof(pm_cfg->param.ps_cfg)));
+    if (copy_to_user(wrq->u.data.pointer, data, sizeof(int) * allowed)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    wrq->u.data.length = allowed;
+
+    if (req->action == MLAN_ACT_SET) {
+        pm_cfg = (mlan_ds_pm_cfg *) req->pbuf;
+        pm_cfg->sub_command = MLAN_OID_PM_CFG_IEEE_PS;
+        pm_cfg->param.ps_mode = 1;
+        req->req_id = MLAN_IOCTL_PM_CFG;
+        req->action = MLAN_ACT_SET;
+        woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT);
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Private IOCTL entry to send an ADDTS TSPEC
+ *
+ *  Receive a ADDTS command from the application.  The command structure
+ *    contains a TSPEC and timeout in milliseconds.  The timeout is performed
+ *    in the firmware after the ADDTS command frame is sent.
+ *
+ *  The TSPEC is received in the API as an opaque block. The firmware will
+ *    send the entire data block, including the bytes after the TSPEC.  This
+ *    is done to allow extra IEs to be packaged with the TSPEC in the ADDTS
+ *    action frame.
+ *
+ *  The IOCTL structure contains two return fields:
+ *    - The firmware command result, which indicates failure and timeouts
+ *    - The IEEE Status code which contains the corresponding value from
+ *      any ADDTS response frame received.
+ *
+ *  In addition, the opaque TSPEC data block passed in is replaced with the
+ *    TSPEC received in the ADDTS response frame.  In case of failure, the
+ *    AP may modify the TSPEC on return and in the case of success, the
+ *    medium time is returned as calculated by the AP.  Along with the TSPEC,
+ *    any IEs that are sent in the ADDTS response are also returned and can be
+ *    parsed using the IOCTL length as an indicator of extra elements.
+ *
+ *  The return value to the application layer indicates a driver execution
+ *    success or failure.  A successful return could still indicate a firmware
+ *    failure or AP negotiation failure via the commandResult field copied
+ *    back to the application.
+ *
+ *  @param priv    Pointer to the mlan_private driver data struct
+ *  @param wrq     A pointer to iwreq structure containing the
+ *                 wlan_ioctl_wmm_addts_req_t struct for this ADDTS request
+ *
+ *  @return        0 if successful; IOCTL error code otherwise
+ */
+static int
+woal_wmm_addts_req_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_wmm_cfg *cfg = NULL;
+    wlan_ioctl_wmm_addts_req_t addts_ioctl;
+    int ret = 0;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_wmm_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    req->req_id = MLAN_IOCTL_WMM_CFG;
+    cfg = (mlan_ds_wmm_cfg *) req->pbuf;
+    cfg->sub_command = MLAN_OID_WMM_CFG_ADDTS;
+
+    memset(&addts_ioctl, 0x00, sizeof(addts_ioctl));
+
+    if (wrq->u.data.length) {
+        if (copy_from_user(&addts_ioctl, wrq->u.data.pointer,
+                           MIN(wrq->u.data.length, sizeof(addts_ioctl)))) {
+            PRINTM(MERROR, "TSPEC: ADDTS copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+
+        cfg->param.addts.timeout = addts_ioctl.timeout_ms;
+        cfg->param.addts.ie_data_len = (t_u8) addts_ioctl.ie_data_len;
+
+        memcpy(cfg->param.addts.ie_data,
+               addts_ioctl.ie_data, cfg->param.addts.ie_data_len);
+
+        if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req,
+                                                      MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        addts_ioctl.cmd_result = cfg->param.addts.result;
+        addts_ioctl.ieee_status_code = (t_u8) cfg->param.addts.status_code;
+        addts_ioctl.ie_data_len = cfg->param.addts.ie_data_len;
+
+        memcpy(addts_ioctl.ie_data,
+               cfg->param.addts.ie_data, cfg->param.addts.ie_data_len);
+
+        wrq->u.data.length = (sizeof(addts_ioctl)
+                              - sizeof(addts_ioctl.ie_data)
+                              + cfg->param.addts.ie_data_len);
+
+        if (copy_to_user(wrq->u.data.pointer, &addts_ioctl, wrq->u.data.length)) {
+            PRINTM(MERROR, "TSPEC: ADDTS copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Private IOCTL entry to send a DELTS TSPEC
+ *
+ *  Receive a DELTS command from the application.  The command structure
+ *    contains a TSPEC and reason code along with space for a command result
+ *    to be returned.  The information is packaged is sent to the wlan_cmd.c
+ *    firmware command prep and send routines for execution in the firmware.
+ *
+ *  The reason code is not used for WMM implementations but is indicated in
+ *    the 802.11e specification.
+ *
+ *  The return value to the application layer indicates a driver execution
+ *    success or failure.  A successful return could still indicate a firmware
+ *    failure via the cmd_result field copied back to the application.
+ *
+ *  @param priv    Pointer to the mlan_private driver data struct
+ *  @param wrq     A pointer to iwreq structure containing the
+ *                 wlan_ioctl_wmm_delts_req_t struct for this DELTS request
+ *
+ *  @return        0 if successful; IOCTL error code otherwise
+ */
+static int
+woal_wmm_delts_req_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_wmm_cfg *cfg = NULL;
+    wlan_ioctl_wmm_delts_req_t delts_ioctl;
+    int ret = 0;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_wmm_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    req->req_id = MLAN_IOCTL_WMM_CFG;
+    cfg = (mlan_ds_wmm_cfg *) req->pbuf;
+    cfg->sub_command = MLAN_OID_WMM_CFG_DELTS;
+
+    memset(&delts_ioctl, 0x00, sizeof(delts_ioctl));
+
+    if (wrq->u.data.length) {
+        if (copy_from_user(&delts_ioctl, wrq->u.data.pointer,
+                           MIN(wrq->u.data.length, sizeof(delts_ioctl)))) {
+            PRINTM(MERROR, "TSPEC: DELTS copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+
+        cfg->param.delts.status_code = (t_u32) delts_ioctl.ieee_reason_code;
+        cfg->param.delts.ie_data_len = (t_u8) delts_ioctl.ie_data_len;
+
+        memcpy(cfg->param.delts.ie_data,
+               delts_ioctl.ie_data, cfg->param.delts.ie_data_len);
+
+        if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req,
+                                                      MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+
+        /* Return the firmware command result back to the application layer */
+        delts_ioctl.cmd_result = cfg->param.delts.result;
+        wrq->u.data.length = sizeof(delts_ioctl);
+
+        if (copy_to_user(wrq->u.data.pointer, &delts_ioctl, wrq->u.data.length)) {
+            PRINTM(MERROR, "TSPEC: DELTS copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Private IOCTL entry to get/set a specified AC Queue's parameters
+ *
+ *  Receive a AC Queue configuration command which is used to get, set, or
+ *    default the parameters associated with a specific WMM AC Queue.
+ *
+ *  @param priv    Pointer to the mlan_private driver data struct
+ *  @param wrq     A pointer to iwreq structure containing the
+ *                 wlan_ioctl_wmm_queue_config_t struct
+ *
+ *  @return        0 if successful; IOCTL error code otherwise
+ */
+static int
+woal_wmm_queue_config_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_wmm_cfg *pwmm = NULL;
+    mlan_ds_wmm_queue_config *pqcfg = NULL;
+    wlan_ioctl_wmm_queue_config_t qcfg_ioctl;
+    int ret = 0;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_wmm_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    req->req_id = MLAN_IOCTL_WMM_CFG;
+    pwmm = (mlan_ds_wmm_cfg *) req->pbuf;
+    pwmm->sub_command = MLAN_OID_WMM_CFG_QUEUE_CONFIG;
+
+    memset(&qcfg_ioctl, 0x00, sizeof(qcfg_ioctl));
+    pqcfg = (mlan_ds_wmm_queue_config *) & pwmm->param.q_cfg;
+
+    if (wrq->u.data.length) {
+        if (copy_from_user(&qcfg_ioctl, wrq->u.data.pointer,
+                           MIN(wrq->u.data.length, sizeof(qcfg_ioctl)))) {
+            PRINTM(MERROR, "QCONFIG: copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+
+        pqcfg->action = qcfg_ioctl.action;
+        pqcfg->access_category = qcfg_ioctl.access_category;
+        pqcfg->msdu_lifetime_expiry = qcfg_ioctl.msdu_lifetime_expiry;
+
+        if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req,
+                                                      MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        memset(&qcfg_ioctl, 0x00, sizeof(qcfg_ioctl));
+        qcfg_ioctl.action = pqcfg->action;
+        qcfg_ioctl.access_category = pqcfg->access_category;
+        qcfg_ioctl.msdu_lifetime_expiry = pqcfg->msdu_lifetime_expiry;
+        wrq->u.data.length = sizeof(qcfg_ioctl);
+
+        if (copy_to_user(wrq->u.data.pointer, &qcfg_ioctl, wrq->u.data.length)) {
+            PRINTM(MERROR, "QCONFIG: copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Private IOCTL entry to get and start/stop queue stats on a WMM AC
+ *
+ *  Receive a AC Queue statistics command from the application for a specific
+ *    WMM AC.  The command can:
+ *         - Turn stats on
+ *         - Turn stats off
+ *         - Collect and clear the stats
+ *
+ *  @param priv    Pointer to the moal_private driver data struct
+ *  @param wrq     A pointer to iwreq structure containing the
+ *                 wlan_ioctl_wmm_queue_stats_t struct
+ *
+ *  @return        0 if successful; IOCTL error code otherwise
+ */
+static int
+woal_wmm_queue_stats_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_wmm_cfg *pwmm = NULL;
+    mlan_ds_wmm_queue_stats *pqstats = NULL;
+    wlan_ioctl_wmm_queue_stats_t qstats_ioctl;
+    int ret = 0;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_wmm_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    req->req_id = MLAN_IOCTL_WMM_CFG;
+    pwmm = (mlan_ds_wmm_cfg *) req->pbuf;
+    pwmm->sub_command = MLAN_OID_WMM_CFG_QUEUE_STATS;
+
+    memset(&qstats_ioctl, 0x00, sizeof(qstats_ioctl));
+    pqstats = (mlan_ds_wmm_queue_stats *) & pwmm->param.q_stats;
+
+    if (wrq->u.data.length) {
+        if (copy_from_user(&qstats_ioctl, wrq->u.data.pointer,
+                           MIN(wrq->u.data.length, sizeof(qstats_ioctl)))) {
+            PRINTM(MERROR, "QSTATS: copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+
+        memcpy((void *) pqstats, (void *) &qstats_ioctl, sizeof(qstats_ioctl));
+        PRINTM(MINFO, "QSTATS: IOCTL [%d,%d]\n", qstats_ioctl.action,
+               qstats_ioctl.user_priority);
+
+        if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req,
+                                                      MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+
+        memset(&qstats_ioctl, 0x00, sizeof(qstats_ioctl));
+        memcpy((void *) &qstats_ioctl, (void *) pqstats, sizeof(qstats_ioctl));
+        wrq->u.data.length = sizeof(qstats_ioctl);
+
+        if (copy_to_user
+            (wrq->u.data.pointer, &qstats_ioctl, wrq->u.data.length)) {
+            PRINTM(MERROR, "QSTATS: copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Private IOCTL entry to get the status of the WMM queues
+ *
+ *  Return the following information for each WMM AC:
+ *        - WMM IE Acm Required
+ *        - Firmware Flow Required
+ *        - Firmware Flow Established
+ *        - Firmware Queue Enabled
+ *        - Firmware Delivery Enabled
+ *        - Firmware Trigger Enabled
+ *
+ *  @param priv    Pointer to the moal_private driver data struct
+ *  @param wrq     A pointer to iwreq structure containing the
+ *                 wlan_ioctl_wmm_queue_status_t struct for request
+ *
+ *  @return        0 if successful; IOCTL error code otherwise
+ */
+static int
+woal_wmm_queue_status_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_wmm_cfg *pwmm = NULL;
+    wlan_ioctl_wmm_queue_status_t qstatus_ioctl;
+    int ret = 0;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_wmm_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    req->req_id = MLAN_IOCTL_WMM_CFG;
+    pwmm = (mlan_ds_wmm_cfg *) req->pbuf;
+    pwmm->sub_command = MLAN_OID_WMM_CFG_QUEUE_STATUS;
+
+    if (wrq->u.data.length == sizeof(qstatus_ioctl)) {
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+
+        memset(&qstatus_ioctl, 0x00, sizeof(qstatus_ioctl));
+        memcpy((void *) &qstatus_ioctl, (void *) &pwmm->param.q_status,
+               sizeof(qstatus_ioctl));
+        wrq->u.data.length = sizeof(qstatus_ioctl);
+    } else {
+        wrq->u.data.length = 0;
+    }
+
+    if (copy_to_user(wrq->u.data.pointer, &qstatus_ioctl, wrq->u.data.length)) {
+        PRINTM(MERROR, "QSTATUS: copy to user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Private IOCTL entry to get the status of the WMM Traffic Streams
+ *
+ *  @param priv    Pointer to the moal_private driver data struct
+ *  @param wrq     A pointer to iwreq structure containing the
+ *                 wlan_ioctl_wmm_ts_status_t struct for request
+ *
+ *  @return        0 if successful; IOCTL error code otherwise
+ */
+static int
+woal_wmm_ts_status_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_wmm_cfg *pwmm = NULL;
+    wlan_ioctl_wmm_ts_status_t ts_status_ioctl;
+    int ret = 0;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_wmm_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    req->req_id = MLAN_IOCTL_WMM_CFG;
+    pwmm = (mlan_ds_wmm_cfg *) req->pbuf;
+    pwmm->sub_command = MLAN_OID_WMM_CFG_TS_STATUS;
+
+    memset(&ts_status_ioctl, 0x00, sizeof(ts_status_ioctl));
+
+    if (wrq->u.data.length == sizeof(ts_status_ioctl)) {
+        if (copy_from_user(&ts_status_ioctl, wrq->u.data.pointer,
+                           MIN(wrq->u.data.length, sizeof(ts_status_ioctl)))) {
+            PRINTM(MERROR, "TS_STATUS: copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+
+        memset(&pwmm->param.ts_status, 0x00, sizeof(ts_status_ioctl));
+        pwmm->param.ts_status.tid = ts_status_ioctl.tid;
+
+        if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req,
+                                                      MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+
+        memset(&ts_status_ioctl, 0x00, sizeof(ts_status_ioctl));
+        memcpy((void *) &ts_status_ioctl, (void *) &pwmm->param.ts_status,
+               sizeof(ts_status_ioctl));
+        wrq->u.data.length = sizeof(ts_status_ioctl);
+    } else {
+        wrq->u.data.length = 0;
+    }
+
+    if (copy_to_user(wrq->u.data.pointer, &ts_status_ioctl, wrq->u.data.length)) {
+        PRINTM(MERROR, "TS_STATUS: copy to user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Private IOCTL entry to get the By-passed TX packet from upper layer
+ *
+ *  @param priv    Pointer to the moal_private driver data struct
+ *  @param wrq     A pointer to iwreq structure containing the packet
+ *
+ *  @return        0 if successful; IOCTL error code otherwise
+ */
+static int
+woal_bypassed_packet_ioctl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    struct sk_buff *skb = NULL;
+    struct ethhdr *eth;
+    t_u16 moreLen = 0, copyLen = 0;
+    ENTER();
+
+#define MLAN_BYPASS_PKT_EXTRA_OFFSET        (4)
+
+    copyLen = wrq->u.data.length;
+    moreLen = MLAN_MIN_DATA_HEADER_LEN + MLAN_BYPASS_PKT_EXTRA_OFFSET
+        + sizeof(mlan_buffer);
+
+    skb = alloc_skb(copyLen + moreLen, GFP_KERNEL);
+    if (skb == NULL) {
+        PRINTM(MERROR, "kmalloc no memory !!\n");
+        LEAVE();
+        return -ENOMEM;
+    }
+
+    skb_reserve(skb, moreLen);
+
+    if (copy_from_user(skb_put(skb, copyLen), wrq->u.data.pointer, copyLen)) {
+        PRINTM(MERROR, "PortBlock: copy from user failed\n");
+        dev_kfree_skb_any(skb);
+        ret = -EFAULT;
+        goto done;
+    }
+
+    eth = (struct ethhdr *) skb->data;
+    eth->h_proto = __constant_htons(eth->h_proto);
+    skb->dev = priv->netdev;
+
+    HEXDUMP("Bypass TX Data", skb->data, MIN(skb->len, 100));
+
+    woal_hard_start_xmit(skb, priv->netdev);
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get auth type
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wrq	    A pointer to iwreq structure
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_auth_type(moal_private * priv, struct iwreq *wrq)
+{
+    int auth_type;
+    t_u32 auth_mode;
+    int ret = 0;
+
+    ENTER();
+    if (wrq->u.data.length == 0) {
+        if (MLAN_STATUS_SUCCESS !=
+            woal_get_auth_mode(priv, MOAL_IOCTL_WAIT, &auth_mode)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        auth_type = auth_mode;
+        if (copy_to_user(wrq->u.data.pointer, &auth_type, sizeof(auth_type))) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = 1;
+    } else {
+        if (copy_from_user(&auth_type, wrq->u.data.pointer, sizeof(auth_type))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        PRINTM(MINFO, "SET: auth_type %d\n", auth_type);
+        if (((auth_type < MLAN_AUTH_MODE_OPEN) ||
+             (auth_type > MLAN_AUTH_MODE_SHARED))
+            && (auth_type != MLAN_AUTH_MODE_AUTO)) {
+            ret = -EINVAL;
+            goto done;
+        }
+        auth_mode = auth_type;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_auth_mode(priv, MOAL_IOCTL_WAIT, auth_mode)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get Port Control mode
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wrq	    A pointer to iwreq structure
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_port_ctrl(moal_private * priv, struct iwreq *wrq)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    int ret = 0;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_PORT_CTRL_ENABLED;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+
+    if (wrq->u.data.length) {
+        if (copy_from_user(&sec->param.port_ctrl_enabled,
+                           wrq->u.data.pointer, sizeof(int)) != 0) {
+            PRINTM(MERROR, "port_ctrl:Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        req->action = MLAN_ACT_SET;
+    } else {
+        req->action = MLAN_ACT_GET;
+    }
+
+    /* Send IOCTL request to MLAN */
+    if (woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT) != MLAN_STATUS_SUCCESS) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (!wrq->u.data.length) {
+        if (copy_to_user(wrq->u.data.pointer, &sec->param.port_ctrl_enabled,
+                         sizeof(int))) {
+            PRINTM(MERROR, "port_ctrl:Copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = 1;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+#if defined(DFS_TESTING_SUPPORT)
+/**
+ *  @brief Set/Get DFS Testing settings
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wrq	    A pointer to iwreq structure
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_dfs_testing(moal_private * priv, struct iwreq *wrq)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_11h_cfg *ds_11hcfg = NULL;
+    int ret = 0;
+    int data[4];
+    int data_length = wrq->u.data.length;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11h_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    ds_11hcfg = (mlan_ds_11h_cfg *) req->pbuf;
+    ds_11hcfg->sub_command = MLAN_OID_11H_DFS_TESTING;
+    req->req_id = MLAN_IOCTL_11H_CFG;
+
+    if (!data_length) {
+        req->action = MLAN_ACT_GET;
+    } else if (data_length == 4) {
+        if (copy_from_user
+            (data, wrq->u.data.pointer, sizeof(int) * data_length)) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if ((unsigned) data[0] > 0xFFFF) {
+            PRINTM(MERROR, "The maximum user CAC is 65535 msec.\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        if ((unsigned) data[1] > 0xFFFF) {
+            PRINTM(MERROR, "The maximum user NOP is 65535 sec.\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        if ((unsigned) data[3] > 0xFF) {
+            PRINTM(MERROR, "The maximum user fixed channel is 255.\n");
+            ret = -EINVAL;
+            goto done;
+        }
+        ds_11hcfg->param.dfs_testing.usr_cac_period_msec = (t_u16) data[0];
+        ds_11hcfg->param.dfs_testing.usr_nop_period_sec = (t_u16) data[1];
+        ds_11hcfg->param.dfs_testing.usr_no_chan_change = data[2] ? 1 : 0;
+        ds_11hcfg->param.dfs_testing.usr_fixed_new_chan = (t_u8) data[3];
+        priv->phandle->cac_period_jiffies = (t_u16) data[0] * HZ / 1000;
+        req->action = MLAN_ACT_SET;
+    } else {
+        PRINTM(MERROR, "Invalid number of args!\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    /* Send IOCTL request to MLAN */
+    if (woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT) != MLAN_STATUS_SUCCESS) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (!data_length) {
+        data[0] = ds_11hcfg->param.dfs_testing.usr_cac_period_msec;
+        data[1] = ds_11hcfg->param.dfs_testing.usr_nop_period_sec;
+        data[2] = ds_11hcfg->param.dfs_testing.usr_no_chan_change;
+        data[3] = ds_11hcfg->param.dfs_testing.usr_fixed_new_chan;
+        if (copy_to_user(wrq->u.data.pointer, &data, sizeof(int) * 4)) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = 4;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+#endif /* DFS_SUPPORT && DFS_TESTING_SUPPORT */
+
+/**
+ *  @brief Set/Get Mgmt Frame passthru mask
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param wrq      A pointer to iwreq structure
+ *
+ *  @return         0 -- success, otherwise fail
+ */
+static int
+woal_mgmt_frame_passthru_ctrl(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0, data_length = wrq->u.data.length;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *mgmt_cfg = NULL;
+    int mask = 0;
+
+    ENTER();
+
+    if (data_length > 1) {
+        PRINTM(MERROR, "Invalid no of arguments!\n");
+        ret = -EINVAL;
+        goto done;
+    }
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    mgmt_cfg = (mlan_ds_misc_cfg *) req->pbuf;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+    mgmt_cfg->sub_command = MLAN_OID_MISC_RX_MGMT_IND;
+
+    if (data_length) {          /* SET */
+        if (copy_from_user(&mask, wrq->u.data.pointer, sizeof(int))) {
+            PRINTM(MERROR, "copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        mgmt_cfg->param.mgmt_subtype_mask = mask;
+        req->action = MLAN_ACT_SET;
+    } else {
+        req->action = MLAN_ACT_GET;
+    }
+
+    /* Send IOCTL request to MLAN */
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    mask = mgmt_cfg->param.mgmt_subtype_mask;
+    if (copy_to_user(wrq->u.data.pointer, &mask, sizeof(int))) {
+        ret = -EFAULT;
+        goto done;
+    }
+    wrq->u.data.length = 1;
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set/Get CFP table codes
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param wrq      A pointer to iwreq structure
+ *
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_cfp_code(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    int data[2];
+    int data_length = wrq->u.data.length;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *misc_cfg = NULL;
+    mlan_ds_misc_cfp_code *cfp_code = NULL;
+
+    ENTER();
+
+    if (data_length > 2) {
+        PRINTM(MERROR, "Invalid number of argument!\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    misc_cfg = (mlan_ds_misc_cfg *) req->pbuf;
+    cfp_code = &misc_cfg->param.cfp_code;
+    misc_cfg->sub_command = MLAN_OID_MISC_CFP_CODE;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+
+    if (!data_length) {
+        req->action = MLAN_ACT_GET;
+    } else {
+        if (copy_from_user
+            (data, wrq->u.data.pointer, sizeof(int) * data_length)) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        cfp_code->cfp_code_bg = data[0];
+        if (data_length == 2)
+            cfp_code->cfp_code_a = data[1];
+        req->action = MLAN_ACT_SET;
+    }
+
+    /* Send IOCTL request to MLAN */
+    if (woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT) != MLAN_STATUS_SUCCESS) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (!data_length) {
+        data[0] = cfp_code->cfp_code_bg;
+        data[1] = cfp_code->cfp_code_a;
+        data_length = 2;
+        if (copy_to_user(wrq->u.data.pointer, &data, sizeof(int) * data_length)) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        wrq->u.data.length = data_length;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Set/Get Tx/Rx antenna
+ *
+ * @param priv     A pointer to moal_private structure
+ * @param wrq      A pointer to iwreq structure
+ *
+ * @return         0 --success, otherwise fail
+ */
+static int
+woal_set_get_tx_rx_ant(moal_private * priv, struct iwreq *wrq)
+{
+    int ret = 0;
+    mlan_ds_radio_cfg *radio = NULL;
+    mlan_ioctl_req *req = NULL;
+    int data[2] = { 0 };
+
+    ENTER();
+
+    if (wrq->u.data.length > 2) {
+        PRINTM(MERROR, "Invalid number of argument!\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_radio_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    radio = (mlan_ds_radio_cfg *) req->pbuf;
+    radio->sub_command = MLAN_OID_ANT_CFG;
+    req->req_id = MLAN_IOCTL_RADIO_CFG;
+    if (wrq->u.data.length) {
+        if (copy_from_user(data, wrq->u.data.pointer, sizeof(data))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        radio->param.ant_cfg.tx_antenna = data[0];
+        radio->param.ant_cfg.rx_antenna = data[0];
+        if (wrq->u.data.length == 2)
+            radio->param.ant_cfg.rx_antenna = data[1];
+        req->action = MLAN_ACT_SET;
+    } else
+        req->action = MLAN_ACT_GET;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (!wrq->u.data.length) {
+        wrq->u.data.length = 1;
+        data[0] = radio->param.ant_cfg.tx_antenna;
+        data[1] = radio->param.ant_cfg.rx_antenna;
+        if (data[0] && data[1] && (data[0] != data[1]))
+            wrq->u.data.length = 2;
+        if (copy_to_user(wrq->u.data.pointer, data, sizeof(data))) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/********************************************************
+		Global Functions
+********************************************************/
+/**
+ *  @brief ioctl function - entry point
+ *
+ *  @param dev		A pointer to net_device structure
+ *  @param req	   	A pointer to ifreq structure
+ *  @param cmd 		Command
+ *
+ *  @return          0 --success, otherwise fail
+ */
+int
+woal_wext_do_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    struct iwreq *wrq = (struct iwreq *) req;
+    int ret = 0;
+
+    if (!IS_STA_WEXT(cfg80211_wext))
+        return -EOPNOTSUPP;
+
+    ENTER();
+
+    PRINTM(MINFO, "woal_wext_do_ioctl: ioctl cmd = 0x%x\n", cmd);
+    switch (cmd) {
+    case WOAL_SETONEINT_GETWORDCHAR:
+        switch (wrq->u.data.flags) {
+        case WOAL_VERSION:     /* Get driver version */
+            ret = woal_get_driver_version(priv, req);
+            break;
+        case WOAL_VEREXT:      /* Get extended driver version */
+            ret = woal_get_driver_verext(priv, req);
+            break;
+        default:
+            ret = -EOPNOTSUPP;
+            break;
+        }
+        break;
+    case WOAL_SETNONE_GETNONE:
+        switch (wrq->u.data.flags) {
+        case WOAL_WARMRESET:
+            ret = woal_warm_reset(priv);
+            break;
+        case WOAL_11D_CLR_CHAN_TABLE:
+            ret = woal_11d_clr_chan_table(priv, wrq);
+            break;
+        default:
+            ret = -EOPNOTSUPP;
+            break;
+        }
+        break;
+    case WOAL_SETONEINT_GETONEINT:
+        switch (wrq->u.data.flags) {
+        case WOAL_SET_GET_TXRATE:
+            ret = woal_set_get_txrate(priv, wrq);
+            break;
+        case WOAL_SET_GET_REGIONCODE:
+            ret = woal_set_get_regioncode(priv, wrq);
+            break;
+        case WOAL_SET_RADIO:
+            ret = woal_set_get_radio(priv, wrq);
+            break;
+        case WOAL_WMM_ENABLE:
+            ret = woal_wmm_enable_ioctl(priv, wrq);
+            break;
+        case WOAL_11D_ENABLE:
+            ret = woal_11d_enable_ioctl(priv, wrq);
+            break;
+        case WOAL_SET_GET_QOS_CFG:
+            ret = woal_set_get_qos_cfg(priv, wrq);
+            break;
+#if defined(REASSOCIATION)
+        case WOAL_SET_GET_REASSOC:
+            ret = woal_set_get_reassoc(priv, wrq);
+            break;
+#endif /* REASSOCIATION */
+        case WOAL_TXBUF_CFG:
+            ret = woal_txbuf_cfg(priv, wrq);
+            break;
+        case WOAL_SET_GET_WWS_CFG:
+            ret = woal_wws_cfg(priv, wrq);
+            break;
+        case WOAL_SLEEP_PD:
+            ret = woal_sleep_pd(priv, wrq);
+            break;
+        case WOAL_AUTH_TYPE:
+            ret = woal_auth_type(priv, wrq);
+            break;
+        case WOAL_PORT_CTRL:
+            ret = woal_port_ctrl(priv, wrq);
+            break;
+#if defined(WIFI_DIRECT_SUPPORT)
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+        case WOAL_SET_GET_BSS_ROLE:
+            ret = woal_set_get_bss_role(priv, wrq);
+            break;
+#endif
+#endif
+        case WOAL_SET_GET_11H_LOCAL_PWR_CONSTRAINT:
+            ret = woal_set_get_11h_local_pwr_constraint(priv, wrq);
+            break;
+        case WOAL_HT_STREAM_CFG:
+            ret = woal_ht_stream_cfg_ioctl(priv, wrq);
+            break;
+        case WOAL_MAC_CONTROL:
+            ret = woal_mac_control_ioctl(priv, wrq);
+            break;
+        case WOAL_THERMAL:
+            ret = woal_thermal_ioctl(priv, wrq);
+            break;
+        default:
+            ret = -EOPNOTSUPP;
+            break;
+        }
+        break;
+
+    case WOAL_SET_GET_SIXTEEN_INT:
+        switch ((int) wrq->u.data.flags) {
+        case WOAL_TX_POWERCFG:
+            ret = woal_tx_power_cfg(priv, wrq);
+            break;
+#ifdef DEBUG_LEVEL1
+        case WOAL_DRV_DBG:
+            ret = woal_drv_dbg(priv, wrq);
+            break;
+#endif
+        case WOAL_BEACON_INTERVAL:
+            ret = woal_beacon_interval(priv, wrq);
+            break;
+        case WOAL_ATIM_WINDOW:
+            ret = woal_atim_window(priv, wrq);
+            break;
+        case WOAL_SIGNAL:
+            ret = woal_get_signal(priv, wrq);
+            break;
+        case WOAL_DEEP_SLEEP:
+            ret = woal_deep_sleep_ioctl(priv, wrq);
+            break;
+        case WOAL_11N_TX_CFG:
+            ret = woal_11n_tx_cfg(priv, wrq);
+            break;
+        case WOAL_11N_AMSDU_AGGR_CTRL:
+            ret = woal_11n_amsdu_aggr_ctrl(priv, wrq);
+            break;
+        case WOAL_11N_HTCAP_CFG:
+            ret = woal_11n_htcap_cfg(priv, wrq);
+            break;
+        case WOAL_PRIO_TBL:
+            ret = woal_11n_prio_tbl(priv, wrq);
+            break;
+        case WOAL_ADDBA_UPDT:
+            ret = woal_addba_para_updt(priv, wrq);
+            break;
+        case WOAL_ADDBA_REJECT:
+            ret = woal_addba_reject(priv, wrq);
+            break;
+        case WOAL_TX_BF_CAP:
+            ret = woal_tx_bf_cap_ioctl(priv, wrq);
+            break;
+        case WOAL_HS_CFG:
+            ret = woal_hs_cfg(priv, wrq, MTRUE);
+            break;
+        case WOAL_HS_SETPARA:
+            ret = woal_hs_setpara(priv, wrq);
+            break;
+        case WOAL_REG_READ_WRITE:
+            ret = woal_reg_read_write(priv, wrq);
+            break;
+        case WOAL_INACTIVITY_TIMEOUT_EXT:
+            ret = woal_inactivity_timeout_ext(priv, wrq);
+            break;
+        case WOAL_SDIO_CLOCK:
+            ret = woal_sdio_clock_ioctl(priv, wrq);
+            break;
+        case WOAL_CMD_52RDWR:
+            ret = woal_cmd52rdwr_ioctl(priv, wrq);
+            break;
+        case WOAL_BAND_CFG:
+            ret = woal_band_cfg(priv, wrq);
+            break;
+        case WOAL_SCAN_CFG:
+            ret = woal_set_get_scan_cfg(priv, wrq);
+            break;
+        case WOAL_PS_CFG:
+            ret = woal_set_get_ps_cfg(priv, wrq);
+            break;
+        case WOAL_MEM_READ_WRITE:
+            ret = woal_mem_read_write(priv, wrq);
+            break;
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+        case WOAL_SDIO_MPA_CTRL:
+            ret = woal_do_sdio_mpa_ctrl(priv, wrq);
+            break;
+#endif
+        case WOAL_SLEEP_PARAMS:
+            ret = woal_sleep_params_ioctl(priv, wrq);
+            break;
+#if defined(DFS_TESTING_SUPPORT)
+        case WOAL_DFS_TESTING:
+            ret = woal_dfs_testing(priv, wrq);
+            break;
+#endif
+        case WOAL_MGMT_FRAME_CTRL:
+            ret = woal_mgmt_frame_passthru_ctrl(priv, wrq);
+            break;
+        case WOAL_CFP_CODE:
+            ret = woal_cfp_code(priv, wrq);
+            break;
+        case WOAL_SET_GET_TX_RX_ANT:
+            ret = woal_set_get_tx_rx_ant(priv, wrq);
+            break;
+        default:
+            ret = -EINVAL;
+            break;
+        }
+        break;
+
+    case WOALGETLOG:
+        ret = woal_get_log(priv, wrq);
+        break;
+
+    case WOAL_SET_GET_256_CHAR:
+        switch (wrq->u.data.flags) {
+        case WOAL_PASSPHRASE:
+            ret = woal_passphrase(priv, wrq);
+            break;
+        case WOAL_ADHOC_AES:
+            ret = woal_adhoc_aes_ioctl(priv, wrq);
+            break;
+        case WOAL_WMM_QUEUE_STATUS:
+            ret = woal_wmm_queue_status_ioctl(priv, wrq);
+            break;
+
+        case WOAL_WMM_TS_STATUS:
+            ret = woal_wmm_ts_status_ioctl(priv, wrq);
+            break;
+        case WOAL_IP_ADDRESS:
+            ret = woal_set_get_ip_addr(priv, wrq);
+            break;
+        case WOAL_TX_BF_CFG:
+            ret = woal_tx_bf_cfg_ioctl(priv, wrq);
+            break;
+        default:
+            ret = -EINVAL;
+            break;
+        }
+        break;
+
+    case WOAL_SETADDR_GETNONE:
+        switch ((int) wrq->u.data.flags) {
+        case WOAL_DEAUTH:
+            ret = woal_deauth(priv, wrq);
+            break;
+        default:
+            ret = -EINVAL;
+            break;
+        }
+        break;
+
+    case WOAL_SETNONE_GETTWELVE_CHAR:
+        /*
+         * We've not used IW_PRIV_TYPE_FIXED so sub-ioctl number is
+         * in flags of iwreq structure, otherwise it will be in
+         * mode member of iwreq structure.
+         */
+        switch ((int) wrq->u.data.flags) {
+        case WOAL_WPS_SESSION:
+            ret = woal_wps_cfg_ioctl(priv, wrq);
+            break;
+        default:
+            ret = -EINVAL;
+            break;
+        }
+        break;
+    case WOAL_SETNONE_GET_FOUR_INT:
+        switch ((int) wrq->u.data.flags) {
+        case WOAL_DATA_RATE:
+            ret = woal_get_txrx_rate(priv, wrq);
+            break;
+        case WOAL_ESUPP_MODE:
+            ret = woal_get_esupp_mode(priv, wrq);
+            break;
+        default:
+            ret = -EINVAL;
+            break;
+        }
+        break;
+
+    case WOAL_SET_GET_64_INT:
+        switch ((int) wrq->u.data.flags) {
+        case WOAL_ECL_SYS_CLOCK:
+            ret = woal_ecl_sys_clock(priv, wrq);
+            break;
+        }
+
+        break;
+
+    case WOAL_HOST_CMD:
+        ret = woal_host_command(priv, wrq);
+        break;
+    case WOAL_ARP_FILTER:
+        ret = woal_arp_filter(priv, wrq);
+        break;
+    case WOAL_SET_INTS_GET_CHARS:
+        switch ((int) wrq->u.data.flags) {
+        case WOAL_READ_EEPROM:
+            ret = woal_read_eeprom(priv, wrq);
+            break;
+        }
+        break;
+    case WOAL_SET_GET_2K_BYTES:
+        switch ((int) wrq->u.data.flags) {
+        case WOAL_CMD_53RDWR:
+            ret = woal_cmd53rdwr_ioctl(priv, wrq);
+            break;
+        case WOAL_SET_USER_SCAN:
+            ret = woal_set_user_scan_ioctl(priv, wrq);
+            break;
+        case WOAL_GET_SCAN_TABLE:
+            ret = woal_get_scan_table_ioctl(priv, wrq);
+            break;
+        case WOAL_SET_USER_SCAN_EXT:
+            ret = woal_set_user_scan_ext_ioctl(priv, wrq);
+            break;
+        case WOAL_WMM_ADDTS:
+            ret = woal_wmm_addts_req_ioctl(priv, wrq);
+            break;
+        case WOAL_WMM_DELTS:
+            ret = woal_wmm_delts_req_ioctl(priv, wrq);
+            break;
+        case WOAL_WMM_QUEUE_CONFIG:
+            ret = woal_wmm_queue_config_ioctl(priv, wrq);
+            break;
+        case WOAL_WMM_QUEUE_STATS:
+            ret = woal_wmm_queue_stats_ioctl(priv, wrq);
+            break;
+        case WOAL_BYPASSED_PACKET:
+            ret = woal_bypassed_packet_ioctl(priv, wrq);
+            break;
+        default:
+            ret = -EINVAL;
+            break;
+        }
+        break;
+
+#ifdef UAP_WEXT
+    case WOAL_FROYO_START:
+        break;
+    case WOAL_FROYO_WL_FW_RELOAD:
+        break;
+    case WOAL_FROYO_STOP:
+        if (IS_UAP_WEXT(cfg80211_wext) && MLAN_STATUS_SUCCESS !=
+            woal_disconnect(priv, MOAL_IOCTL_WAIT, NULL)) {
+            ret = -EFAULT;
+        }
+        break;
+#endif
+    default:
+        ret = -EINVAL;
+        break;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get mode
+ *
+ *  @param priv          A pointer to moal_private structure
+ *  @param wait_option   Wait option (MOAL_WAIT or MOAL_NO_WAIT)
+ *
+ *  @return              Wireless mode
+ */
+t_u32
+woal_get_mode(moal_private * priv, t_u8 wait_option)
+{
+    int ret = 0;
+    mlan_ds_bss *bss = NULL;
+    mlan_ioctl_req *req = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    t_u32 mode = priv->w_stats.status;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_BSS_MODE;
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = MLAN_ACT_GET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+    if (status == MLAN_STATUS_SUCCESS) {
+        switch (bss->param.bss_mode) {
+        case MLAN_BSS_MODE_INFRA:
+            mode = IW_MODE_INFRA;
+            break;
+        case MLAN_BSS_MODE_IBSS:
+            mode = IW_MODE_ADHOC;
+            break;
+        default:
+            mode = IW_MODE_AUTO;
+            break;
+        }
+    }
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return mode;
+}
+
+/**
+ *  @brief Get statistics information
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wait_option  Wait option
+ *  @param stats        A pointer to mlan_ds_get_stats structure
+ *
+ *  @return             MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_get_stats_info(moal_private * priv, t_u8 wait_option,
+                    mlan_ds_get_stats * stats)
+{
+    int ret = 0;
+    mlan_ds_get_info *info = NULL;
+    mlan_ioctl_req *req = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_get_info));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    info = (mlan_ds_get_info *) req->pbuf;
+    info->sub_command = MLAN_OID_GET_STATS;
+    req->req_id = MLAN_IOCTL_GET_INFO;
+    req->action = MLAN_ACT_GET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+    if (status == MLAN_STATUS_SUCCESS) {
+        if (stats)
+            memcpy(stats, &info->param.stats, sizeof(mlan_ds_get_stats));
+        priv->w_stats.discard.fragment = info->param.stats.fcs_error;
+        priv->w_stats.discard.retries = info->param.stats.retry;
+        priv->w_stats.discard.misc = info->param.stats.ack_failure;
+    }
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Get data rates
+ *
+ *  @param priv          A pointer to moal_private structure
+ *  @param wait_option   Wait option
+ *  @param m_rates       A pointer to moal_802_11_rates structure
+ *
+ *  @return              MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_get_data_rates(moal_private * priv, t_u8 wait_option,
+                    moal_802_11_rates * m_rates)
+{
+    int ret = 0;
+    mlan_ds_rate *rate = NULL;
+    mlan_ioctl_req *req = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_rate));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    rate = (mlan_ds_rate *) req->pbuf;
+    rate->sub_command = MLAN_OID_SUPPORTED_RATES;
+    req->req_id = MLAN_IOCTL_RATE;
+    req->action = MLAN_ACT_GET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+    if (status == MLAN_STATUS_SUCCESS) {
+        if (m_rates)
+            m_rates->num_of_rates =
+                woal_copy_rates(m_rates->rates, m_rates->num_of_rates,
+                                rate->param.rates, MLAN_SUPPORTED_RATES);
+    }
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Get channel list
+ *
+ *  @param priv            A pointer to moal_private structure
+ *  @param wait_option     Wait option
+ *  @param chan_list       A pointer to mlan_chan_list structure
+ *
+ *  @return                MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_get_channel_list(moal_private * priv, t_u8 wait_option,
+                      mlan_chan_list * chan_list)
+{
+    int ret = 0;
+    mlan_ds_bss *bss = NULL;
+    mlan_ioctl_req *req = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_BSS_CHANNEL_LIST;
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = MLAN_ACT_GET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+    if (status == MLAN_STATUS_SUCCESS) {
+        if (chan_list) {
+            memcpy(chan_list, &bss->param.chanlist, sizeof(mlan_chan_list));
+        }
+    }
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Handle get info resp
+ *
+ *  @param priv 	Pointer to moal_private structure
+ *  @param info 	Pointer to mlan_ds_get_info structure
+ *
+ *  @return    		N/A
+ */
+void
+woal_ioctl_get_info_resp(moal_private * priv, mlan_ds_get_info * info)
+{
+    ENTER();
+    switch (info->sub_command) {
+    case MLAN_OID_GET_STATS:
+        priv->w_stats.discard.fragment = info->param.stats.fcs_error;
+        priv->w_stats.discard.retries = info->param.stats.retry;
+        priv->w_stats.discard.misc = info->param.stats.ack_failure;
+        break;
+    case MLAN_OID_GET_SIGNAL:
+        if (info->param.signal.selector & BCN_RSSI_AVG_MASK)
+            priv->w_stats.qual.level = info->param.signal.bcn_rssi_avg;
+        if (info->param.signal.selector & BCN_NF_AVG_MASK)
+            priv->w_stats.qual.noise = info->param.signal.bcn_nf_avg;
+        break;
+    default:
+        break;
+    }
+    LEAVE();
+}
+
+/**
+ *  @brief Handle get BSS resp
+ *
+ *  @param priv 	Pointer to moal_private structure
+ *  @param bss 		Pointer to mlan_ds_bss structure
+ *
+ *  @return    		N/A
+ */
+void
+woal_ioctl_get_bss_resp(moal_private * priv, mlan_ds_bss * bss)
+{
+    t_u32 mode = 0;
+
+    ENTER();
+
+    switch (bss->sub_command) {
+    case MLAN_OID_BSS_MODE:
+        if (bss->param.bss_mode == MLAN_BSS_MODE_INFRA)
+            mode = IW_MODE_INFRA;
+        else if (bss->param.bss_mode == MLAN_BSS_MODE_IBSS)
+            mode = IW_MODE_ADHOC;
+        else
+            mode = IW_MODE_AUTO;
+        priv->w_stats.status = mode;
+        break;
+    default:
+        break;
+    }
+
+    LEAVE();
+    return;
+}
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_priv.h b/drivers/net/wireless/sd8797/mlinux/moal_priv.h
new file mode 100644
index 000000000000..1c24f30502f6
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_priv.h
@@ -0,0 +1,729 @@
+/** @file moal_priv.h
+ *
+ * @brief This file contains definition for extended private IOCTL call.
+ *  
+ * Copyright (C) 2008-2011, Marvell International Ltd.  
+ *
+ * This software file (the "File") is distributed by Marvell International 
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+ * (the "License").  You may use, redistribute and/or modify this File in 
+ * accordance with the terms and conditions of the License, a copy of which 
+ * is available by writing to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+ * this warranty disclaimer.
+ *
+ */
+
+/********************************************************
+Change log:
+    10/31/2008: initial version
+********************************************************/
+
+#ifndef _WOAL_PRIV_H_
+#define _WOAL_PRIV_H_
+
+/** Command disabled */
+#define	CMD_DISABLED				0
+/** Command enabled */
+#define	CMD_ENABLED				1
+/** Command get */
+#define	CMD_GET					2
+
+/** 2K bytes */
+#define WOAL_2K_BYTES       2000
+
+/** PRIVATE CMD ID */
+#define WOAL_IOCTL                  (SIOCIWFIRSTPRIV)   /* 0x8BE0 defined in
+                                                           wireless.h */
+
+/** Private command ID to set one int/get word char */
+#define WOAL_SETONEINT_GETWORDCHAR  (WOAL_IOCTL + 1)
+/** Private command ID to get version */
+#define WOAL_VERSION                1
+/** Private command ID to get extended version */
+#define WOAL_VEREXT                 2
+
+/** Private command ID to set/get none */
+#define WOAL_SETNONE_GETNONE        (WOAL_IOCTL + 2)
+/** Private command ID for warm reset */
+#define WOAL_WARMRESET              1
+/** Private command ID to clear 11d chan table */
+#define WOAL_11D_CLR_CHAN_TABLE     4
+
+/** Private command ID to set/get sixteen int */
+#define WOAL_SET_GET_SIXTEEN_INT    (WOAL_IOCTL + 3)
+/** Private command ID to set/get TX power configurations */
+#define WOAL_TX_POWERCFG            1
+#ifdef DEBUG_LEVEL1
+/** Private command ID to set/get driver debug */
+#define WOAL_DRV_DBG                2
+#endif
+/** Private command ID to set/get beacon interval */
+#define WOAL_BEACON_INTERVAL        3
+/** Private command ID to set/get ATIM window */
+#define WOAL_ATIM_WINDOW            4
+/** Private command ID to get RSSI */
+#define WOAL_SIGNAL                 5
+/** Private command ID to set/get Deep Sleep mode */
+#define WOAL_DEEP_SLEEP             7
+/** Private command ID for 11n ht configration */
+#define WOAL_11N_TX_CFG             8
+/** Private command ID for 11n usr ht configration */
+#define WOAL_11N_HTCAP_CFG          9
+/** Private command ID for TX Aggregation */
+#define WOAL_PRIO_TBL               10
+/** Private command ID for Updating ADDBA variables */
+#define WOAL_ADDBA_UPDT             11
+/** Private command ID to set/get Host Sleep configuration */
+#define WOAL_HS_CFG                 12
+/** Private command ID to set Host Sleep parameters */
+#define WOAL_HS_SETPARA             13
+/** Private command ID to read/write registers */
+#define WOAL_REG_READ_WRITE         14
+/** Private command ID to set/get band/adhocband */
+#define WOAL_BAND_CFG               15
+/** Private command ID for TX Aggregation */
+#define WOAL_11N_AMSDU_AGGR_CTRL    17
+/** Private command ID to set/get Inactivity timeout */
+#define WOAL_INACTIVITY_TIMEOUT_EXT 18
+/** Private command ID to turn on/off sdio clock */
+#define WOAL_SDIO_CLOCK             19
+/** Private command ID to read/write Command 52 */
+#define WOAL_CMD_52RDWR             20
+/** Private command ID to set/get scan configuration parameter */
+#define WOAL_SCAN_CFG               21
+/** Private command ID to set/get PS configuration parameter */
+#define WOAL_PS_CFG                 22
+/** Private command ID to read/write memory */
+#define WOAL_MEM_READ_WRITE         23
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+/** Private command ID to control SDIO MP-A */
+#define WOAL_SDIO_MPA_CTRL          25
+#endif
+/** Private command ID for Updating ADDBA variables */
+#define WOAL_ADDBA_REJECT           27
+/** Private command ID to set/get sleep parameters */
+#define WOAL_SLEEP_PARAMS           28
+/** Private command ID to set/get TX BF capabilities */
+#define WOAL_TX_BF_CAP              31
+#if defined(DFS_TESTING_SUPPORT)
+/** Private command ID to set/get dfs testing settings */
+#define WOAL_DFS_TESTING            33
+#endif
+/** Private command ID to set/get CFP table codes */
+#define WOAL_CFP_CODE               34
+/** Private command ID to set/get tx/rx antenna */
+#define WOAL_SET_GET_TX_RX_ANT      35
+/** Private command ID to set/get management frame passthru mask */
+#define WOAL_MGMT_FRAME_CTRL        36
+
+/** Private command ID to set one int/get one int */
+#define WOAL_SETONEINT_GETONEINT    (WOAL_IOCTL + 5)
+/** Private command ID to set/get Tx rate */
+#define WOAL_SET_GET_TXRATE         1
+/** Private command ID to set/get region code */
+#define WOAL_SET_GET_REGIONCODE     2
+/** Private command ID to turn on/off radio */
+#define WOAL_SET_RADIO              3
+/** Private command ID to enable WMM */
+#define WOAL_WMM_ENABLE	    	    4
+/** Private command ID to enable 802.11D */
+#define WOAL_11D_ENABLE	    	    5
+/** Private command ID to set/get QoS configuration */
+#define WOAL_SET_GET_QOS_CFG        7
+#if defined(REASSOCIATION)
+/** Private command ID to set/get reassociation setting */
+#define WOAL_SET_GET_REASSOC        9
+#endif /* REASSOCIATION */
+/** Private command ID for Updating Transmit buffer configration */
+#define WOAL_TXBUF_CFG              10
+/** Private command ID to set/get WWS mode */
+#define	WOAL_SET_GET_WWS_CFG        12
+/** Private command ID to set/get sleep period */
+#define WOAL_SLEEP_PD               13
+/** Private command ID to set/get auth type */
+#define WOAL_AUTH_TYPE              18
+/** Private command ID to set/get port control */
+#define WOAL_PORT_CTRL              19
+#if defined(WIFI_DIRECT_SUPPORT)
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+/** Private command ID for set/get BSS role */
+#define WOAL_SET_GET_BSS_ROLE       21
+#endif
+#endif
+/** Private command ID for set/get 11h local power constraint */
+#define WOAL_SET_GET_11H_LOCAL_PWR_CONSTRAINT 22
+/** Private command ID to set/get 11N HT stream configuration */
+#define WOAL_HT_STREAM_CFG          23
+/** Private command ID to set/get MAC control */
+#define WOAL_MAC_CONTROL            24
+/** Private command ID to get thermal value */
+#define WOAL_THERMAL                25
+
+/** Private command ID to get log */
+#define WOALGETLOG                  (WOAL_IOCTL + 7)
+
+/** Private command ID to set a wext address variable */
+#define WOAL_SETADDR_GETNONE        (WOAL_IOCTL + 8)
+/** Private command ID to send deauthentication */
+#define WOAL_DEAUTH                 1
+
+/** Private command to get/set 256 chars */
+#define WOAL_SET_GET_256_CHAR       (WOAL_IOCTL + 9)
+/** Private command to read/write passphrase */
+#define WOAL_PASSPHRASE             1
+/** Private command to get/set Ad-Hoc AES */
+#define WOAL_ADHOC_AES              2
+/** Private command ID to get WMM queue status */
+#define WOAL_WMM_QUEUE_STATUS       4
+/** Private command ID to get Traffic stream status */
+#define WOAL_WMM_TS_STATUS          5
+#define WOAL_IP_ADDRESS             7
+/** Private command ID to set/get TX bemaforming */
+#define WOAL_TX_BF_CFG              8
+
+/** Get log buffer size */
+#define GETLOG_BUFSIZE              512
+
+/** Private command ID to set none/get twelve chars*/
+#define WOAL_SETNONE_GETTWELVE_CHAR (WOAL_IOCTL + 11)
+/** Private command ID for WPS session */
+#define WOAL_WPS_SESSION            1
+
+/** Private command ID to set none/get four int */
+#define WOAL_SETNONE_GET_FOUR_INT   (WOAL_IOCTL + 13)
+/** Private command ID to get data rates */
+#define WOAL_DATA_RATE              1
+/** Private command ID to get E-Supplicant mode */
+#define WOAL_ESUPP_MODE             2
+
+/** Private command to get/set 64 ints */
+#define WOAL_SET_GET_64_INT         (WOAL_IOCTL + 15)
+/** Private command ID to set/get ECL system clock */
+#define WOAL_ECL_SYS_CLOCK          1
+
+/** Private command ID for hostcmd */
+#define WOAL_HOST_CMD               (WOAL_IOCTL + 17)
+
+/** Private command ID for arpfilter */
+#define WOAL_ARP_FILTER             (WOAL_IOCTL + 19)
+
+/** Private command ID to set ints and get chars */
+#define WOAL_SET_INTS_GET_CHARS     (WOAL_IOCTL + 21)
+/** Private command ID to read EEPROM data */
+#define WOAL_READ_EEPROM            1
+
+/** Private command ID to set/get 2K bytes */
+#define WOAL_SET_GET_2K_BYTES       (WOAL_IOCTL + 23)
+
+/** Private command ID to read/write Command 53 */
+#define WOAL_CMD_53RDWR             2
+
+/** Private command ID for setuserscan */
+#define WOAL_SET_USER_SCAN          3
+/** Private command ID for getscantable */
+#define WOAL_GET_SCAN_TABLE         4
+/** Private command ID for setuserscanext: async without wait */
+#define WOAL_SET_USER_SCAN_EXT      5
+
+/** Private command ID to request ADDTS */
+#define WOAL_WMM_ADDTS              7
+/** Private command ID to request DELTS */
+#define WOAL_WMM_DELTS              8
+/** Private command ID to queue configuration */
+#define WOAL_WMM_QUEUE_CONFIG       9
+/** Private command ID to queue stats */
+#define WOAL_WMM_QUEUE_STATS        10
+/** Private command ID to Bypass auth packet */
+#define WOAL_BYPASSED_PACKET		11
+
+#ifdef UAP_WEXT
+/** The following command IDs are for Froyo app */
+/** Private command ID to start driver */
+#define WOAL_FROYO_START            (WOAL_IOCTL + 28)
+/** Private command ID to reload FW */
+#define WOAL_FROYO_WL_FW_RELOAD     (WOAL_IOCTL + 29)
+/** Private command ID to stop driver */
+#define WOAL_FROYO_STOP             (WOAL_IOCTL + 30)
+#endif
+
+/**
+ * iwpriv ioctl handlers
+ */
+static const struct iw_priv_args woal_private_args[] = {
+    {
+     WOAL_SETONEINT_GETWORDCHAR,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_CHAR | 128,
+     ""},
+    {
+     WOAL_VERSION,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_CHAR | 128,
+     "version"},
+    {
+     WOAL_VEREXT,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_CHAR | 128,
+     "verext"},
+    {
+     WOAL_SETNONE_GETNONE,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_NONE,
+     ""},
+    {
+     WOAL_WARMRESET,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_NONE,
+     "warmreset"},
+    {
+     WOAL_SETONEINT_GETONEINT,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     ""},
+    {
+     WOAL_SET_GET_TXRATE,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "txratecfg"},
+    {
+     WOAL_SET_GET_REGIONCODE,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "regioncode"},
+    {
+     WOAL_SET_RADIO,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "radioctrl"},
+    {
+     WOAL_WMM_ENABLE,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "wmmcfg"},
+    {
+     WOAL_11D_ENABLE,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "11dcfg"},
+    {
+     WOAL_11D_CLR_CHAN_TABLE,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_NONE,
+     "11dclrtbl"},
+    {
+     WOAL_SET_GET_QOS_CFG,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "qoscfg"},
+    {
+     WOAL_SET_GET_WWS_CFG,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "wwscfg"},
+#if defined(REASSOCIATION)
+    {
+     WOAL_SET_GET_REASSOC,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "reassoctrl"},
+#endif
+    {
+     WOAL_TXBUF_CFG,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "txbufcfg"},
+    {
+     WOAL_SLEEP_PD,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "sleeppd"},
+    {
+     WOAL_AUTH_TYPE,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "authtype"},
+    {
+     WOAL_PORT_CTRL,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "port_ctrl"},
+#if defined(WIFI_DIRECT_SUPPORT)
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+    {
+     WOAL_SET_GET_BSS_ROLE,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "bssrole"},
+#endif
+#endif
+    {
+     WOAL_SET_GET_11H_LOCAL_PWR_CONSTRAINT,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "powercons"},
+    {
+     WOAL_HT_STREAM_CFG,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "htstreamcfg"},
+    {
+     WOAL_MAC_CONTROL,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "macctrl"},
+    {
+     WOAL_THERMAL,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "thermal"},
+    {
+     WOAL_SET_GET_SIXTEEN_INT,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     ""},
+    {
+     WOAL_TX_POWERCFG,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "txpowercfg"},
+#ifdef DEBUG_LEVEL1
+    {
+     WOAL_DRV_DBG,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "drvdbg"},
+#endif
+    {
+     WOAL_BEACON_INTERVAL,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "bcninterval"},
+    {
+     WOAL_ATIM_WINDOW,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "atimwindow"},
+    {
+     WOAL_SIGNAL,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "getsignal"},
+    {
+     WOAL_DEEP_SLEEP,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "deepsleep",
+     },
+    {
+     WOAL_11N_TX_CFG,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "httxcfg"},
+    {
+     WOAL_11N_HTCAP_CFG,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "htcapinfo"},
+    {
+     WOAL_PRIO_TBL,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "aggrpriotbl"},
+    {
+     WOAL_11N_AMSDU_AGGR_CTRL,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "amsduaggrctrl"},
+    {
+     WOAL_ADDBA_UPDT,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "addbapara"},
+    {
+     WOAL_ADDBA_REJECT,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "addbareject"},
+    {
+     WOAL_TX_BF_CAP,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "httxbfcap"},
+    {
+     WOAL_HS_CFG,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "hscfg"},
+    {
+     WOAL_HS_SETPARA,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "hssetpara"},
+    {
+     WOAL_REG_READ_WRITE,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "regrdwr"},
+    {
+     WOAL_BAND_CFG,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "bandcfg"},
+    {
+     WOAL_INACTIVITY_TIMEOUT_EXT,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "inactivityto"},
+    {
+     WOAL_SDIO_CLOCK,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "sdioclock"},
+    {
+     WOAL_CMD_52RDWR,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "sdcmd52rw"},
+    {
+     WOAL_SCAN_CFG,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "scancfg"},
+    {
+     WOAL_PS_CFG,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "pscfg"},
+    {
+     WOAL_MEM_READ_WRITE,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "memrdwr"},
+#if defined(SDIO_MULTI_PORT_TX_AGGR) || defined(SDIO_MULTI_PORT_RX_AGGR)
+    {
+     WOAL_SDIO_MPA_CTRL,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "mpactrl"},
+#endif
+    {
+     WOAL_SLEEP_PARAMS,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "sleepparams"},
+#if defined(DFS_TESTING_SUPPORT)
+    {
+     WOAL_DFS_TESTING,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "dfstesting"},
+#endif
+    {
+     WOAL_MGMT_FRAME_CTRL,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "mgmtframectrl"},
+    {
+     WOAL_CFP_CODE,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "cfpcode"},
+    {
+     WOAL_SET_GET_TX_RX_ANT,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_INT | 16,
+     "antcfg"},
+    {
+     WOALGETLOG,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_CHAR | GETLOG_BUFSIZE,
+     "getlog"},
+    {
+     WOAL_SETADDR_GETNONE,
+     IW_PRIV_TYPE_ADDR | 1,
+     IW_PRIV_TYPE_NONE,
+     ""},
+    {
+     WOAL_DEAUTH,
+     IW_PRIV_TYPE_ADDR | 1,
+     IW_PRIV_TYPE_NONE,
+     "deauth"},
+    {
+     WOAL_SET_GET_256_CHAR,
+     IW_PRIV_TYPE_CHAR | 256,
+     IW_PRIV_TYPE_CHAR | 256,
+     ""},
+    {
+     WOAL_PASSPHRASE,
+     IW_PRIV_TYPE_CHAR | 256,
+     IW_PRIV_TYPE_CHAR | 256,
+     "passphrase"},
+    {
+     WOAL_ADHOC_AES,
+     IW_PRIV_TYPE_CHAR | 256,
+     IW_PRIV_TYPE_CHAR | 256,
+     "adhocaes"},
+    {
+     WOAL_WMM_QUEUE_STATUS,
+     IW_PRIV_TYPE_CHAR | 256,
+     IW_PRIV_TYPE_CHAR | 256,
+     "qstatus"},
+    {
+     WOAL_WMM_TS_STATUS,
+     IW_PRIV_TYPE_CHAR | 256,
+     IW_PRIV_TYPE_CHAR | 256,
+     "ts_status"},
+    {
+     WOAL_IP_ADDRESS,
+     IW_PRIV_TYPE_CHAR | 256,
+     IW_PRIV_TYPE_CHAR | 256,
+     "ipaddr"},
+    {
+     WOAL_TX_BF_CFG,
+     IW_PRIV_TYPE_CHAR | 256,
+     IW_PRIV_TYPE_CHAR | 256,
+     "httxbfcfg"},
+    {
+     WOAL_SETNONE_GETTWELVE_CHAR,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_CHAR | 12,
+     ""},
+    {
+     WOAL_WPS_SESSION,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_CHAR | 12,
+     "wpssession"},
+    {
+     WOAL_SETNONE_GET_FOUR_INT,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_INT | 4,
+     ""},
+    {
+     WOAL_DATA_RATE,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_INT | 4,
+     "getdatarate"},
+    {
+     WOAL_ESUPP_MODE,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_INT | 4,
+     "esuppmode"},
+    {
+     WOAL_SET_GET_64_INT,
+     IW_PRIV_TYPE_INT | 64,
+     IW_PRIV_TYPE_INT | 64,
+     ""},
+    {
+     WOAL_ECL_SYS_CLOCK,
+     IW_PRIV_TYPE_INT | 64,
+     IW_PRIV_TYPE_INT | 64,
+     "sysclock"},
+    {
+     WOAL_HOST_CMD,
+     IW_PRIV_TYPE_BYTE | 2047,
+     IW_PRIV_TYPE_BYTE | 2047,
+     "hostcmd"},
+    {
+     WOAL_ARP_FILTER,
+     IW_PRIV_TYPE_BYTE | 2047,
+     IW_PRIV_TYPE_BYTE | 2047,
+     "arpfilter"},
+    {
+     WOAL_SET_INTS_GET_CHARS,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_BYTE | 256,
+     ""},
+    {
+     WOAL_READ_EEPROM,
+     IW_PRIV_TYPE_INT | 16,
+     IW_PRIV_TYPE_BYTE | 256,
+     "rdeeprom"},
+    {
+     WOAL_SET_GET_2K_BYTES,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     ""},
+    {
+     WOAL_CMD_53RDWR,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     "sdcmd53rw"},
+    {
+     WOAL_SET_USER_SCAN,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     "setuserscan"},
+    {
+     WOAL_GET_SCAN_TABLE,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     "getscantable"},
+    {
+     WOAL_SET_USER_SCAN_EXT,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     "setuserscanext"},
+    {
+     WOAL_WMM_ADDTS,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     "addts"},
+    {
+     WOAL_WMM_DELTS,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     "delts"},
+    {
+     WOAL_WMM_QUEUE_CONFIG,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     "qconfig"},
+    {
+     WOAL_WMM_QUEUE_STATS,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     "qstats"},
+    {
+     WOAL_BYPASSED_PACKET,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     IW_PRIV_TYPE_BYTE | WOAL_2K_BYTES,
+     "pb_bypass"},
+#ifdef UAP_WEXT
+    {
+     WOAL_FROYO_START,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_NONE,
+     "START"},
+    {
+     WOAL_FROYO_STOP,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_NONE,
+     "STOP"},
+    {
+     WOAL_FROYO_WL_FW_RELOAD,
+     IW_PRIV_TYPE_CHAR | 256,
+     IW_PRIV_TYPE_CHAR | 256,
+     "WL_FW_RELOAD"},
+#endif
+};
+
+/** moal_802_11_rates  */
+typedef struct _moal_802_11_rates
+{
+        /** Num of rates */
+    t_u8 num_of_rates;
+        /** Rates */
+    t_u8 rates[MLAN_SUPPORTED_RATES];
+} moal_802_11_rates;
+
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+int woal_wext_do_ioctl(struct net_device *dev, struct ifreq *req, int cmd);
+#endif
+
+#endif /* _WOAL_PRIV_H_ */
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_proc.c b/drivers/net/wireless/sd8797/mlinux/moal_proc.c
new file mode 100644
index 000000000000..fb7c63a29cab
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_proc.c
@@ -0,0 +1,631 @@
+/**  @file moal_proc.c
+  *
+  * @brief This file contains functions for proc file.
+  * 
+  * Copyright (C) 2008-2010, Marvell International Ltd. 
+  *
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+
+/********************************************************
+Change log:
+    10/21/2008: initial version
+********************************************************/
+
+#include	"moal_main.h"
+#ifdef UAP_SUPPORT
+#include    "moal_uap.h"
+#endif
+#include 	"moal_sdio.h"
+
+/********************************************************
+		Local Variables
+********************************************************/
+#ifdef CONFIG_PROC_FS
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+#define PROC_DIR	NULL
+#define MWLAN_PROC_DIR  "mwlan/"
+/** Proc top level directory entry */
+struct proc_dir_entry *proc_mwlan = NULL;
+#elif LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,24)
+#define PROC_DIR	&proc_root
+#else
+#define PROC_DIR	proc_net
+#endif
+
+#ifdef STA_SUPPORT
+static char *szModes[] = {
+    "Unknown",
+    "Managed",
+    "Ad-hoc",
+    "Auto",
+};
+#endif
+
+extern int drv_mode;
+
+/********************************************************
+		Global Variables
+********************************************************/
+
+/********************************************************
+		Local Functions
+********************************************************/
+/**
+ *  @brief Proc read function for info
+ *
+ *  @param page	   Pointer to buffer
+ *  @param start   Read data starting position
+ *  @param offset  Offset
+ *  @param count   Counter
+ *  @param eof     End of file flag
+ *  @param data    Data to output
+ *
+ *  @return 	   Number of output data
+ */
+static int
+woal_info_proc_read(char *page, char **start, off_t offset,
+                    int count, int *eof, void *data)
+{
+    char *p = page;
+    struct net_device *netdev = data;
+    char fmt[MLAN_MAX_VER_STR_LEN];
+    moal_private *priv = (moal_private *) netdev_priv(netdev);
+#ifdef STA_SUPPORT
+    int i = 0;
+    moal_handle *handle = priv->phandle;
+    mlan_bss_info info;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35)
+    struct dev_mc_list *mcptr = netdev->mc_list;
+    int mc_count = netdev->mc_count;
+#else
+    struct netdev_hw_addr *mcptr = NULL;
+    int mc_count = netdev_mc_count(netdev);
+#endif /* < 2.6.35 */
+#else
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+    int i = 0;
+#endif /* >= 2.6.29 */
+#endif
+#ifdef UAP_SUPPORT
+    mlan_ds_uap_stats ustats;
+#endif
+
+    ENTER();
+
+    if (offset) {
+        *eof = 1;
+        goto exit;
+    }
+    memset(fmt, 0, sizeof(fmt));
+#ifdef UAP_SUPPORT
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP) {
+        p += sprintf(p, "driver_name = " "\"uap\"\n");
+        woal_uap_get_version(priv, fmt, sizeof(fmt) - 1);
+        if (MLAN_STATUS_SUCCESS !=
+            woal_uap_get_stats(priv, MOAL_PROC_WAIT, &ustats)) {
+            *eof = 1;
+            goto exit;
+        }
+    }
+#endif /* UAP_SUPPORT */
+#ifdef STA_SUPPORT
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) {
+        woal_get_version(handle, fmt, sizeof(fmt) - 1);
+        memset(&info, 0, sizeof(info));
+        if (MLAN_STATUS_SUCCESS !=
+            woal_get_bss_info(priv, MOAL_PROC_WAIT, &info)) {
+            *eof = 1;
+            goto exit;
+        }
+        p += sprintf(p, "driver_name = " "\"wlan\"\n");
+    }
+#endif
+    p += sprintf(p, "driver_version = %s", fmt);
+    p += sprintf(p, "\ninterface_name=\"%s\"\n", netdev->name);
+#if defined(WIFI_DIRECT_SUPPORT)
+    if (priv->bss_type == MLAN_BSS_TYPE_WIFIDIRECT) {
+        if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA)
+            p += sprintf(p, "bss_mode = \"WIFIDIRECT-Client\"\n");
+        else
+            p += sprintf(p, "bss_mode = \"WIFIDIRECT-GO\"\n");
+    }
+#endif
+#ifdef STA_SUPPORT
+    if (priv->bss_type == MLAN_BSS_TYPE_STA)
+        p += sprintf(p, "bss_mode =\"%s\"\n", szModes[info.bss_mode]);
+#endif
+    p += sprintf(p, "media_state=\"%s\"\n",
+                 ((priv->media_connected ==
+                   MFALSE) ? "Disconnected" : "Connected"));
+    p += sprintf(p, "mac_address=\"%02x:%02x:%02x:%02x:%02x:%02x\"\n",
+                 netdev->dev_addr[0], netdev->dev_addr[1], netdev->dev_addr[2],
+                 netdev->dev_addr[3], netdev->dev_addr[4], netdev->dev_addr[5]);
+#ifdef STA_SUPPORT
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_STA) {
+        p += sprintf(p, "multicast_count=\"%d\"\n", mc_count);
+        p += sprintf(p, "essid=\"%s\"\n", info.ssid.ssid);
+        p += sprintf(p, "bssid=\"%02x:%02x:%02x:%02x:%02x:%02x\"\n",
+                     info.bssid[0], info.bssid[1],
+                     info.bssid[2], info.bssid[3],
+                     info.bssid[4], info.bssid[5]);
+        p += sprintf(p, "channel=\"%d\"\n", (int) info.bss_chan);
+        p += sprintf(p, "region_code = \"%02x\"\n", (t_u8) info.region_code);
+
+        /*
+         * Put out the multicast list
+         */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35)
+        for (i = 0; i < netdev->mc_count; i++) {
+            p += sprintf(p,
+                         "multicast_address[%d]=\"%02x:%02x:%02x:%02x:%02x:%02x\"\n",
+                         i,
+                         mcptr->dmi_addr[0], mcptr->dmi_addr[1],
+                         mcptr->dmi_addr[2], mcptr->dmi_addr[3],
+                         mcptr->dmi_addr[4], mcptr->dmi_addr[5]);
+
+            mcptr = mcptr->next;
+        }
+#else
+        netdev_for_each_mc_addr(mcptr, netdev)
+            p += sprintf(p,
+                         "multicast_address[%d]=\"%02x:%02x:%02x:%02x:%02x:%02x\"\n",
+                         i++,
+                         mcptr->addr[0], mcptr->addr[1],
+                         mcptr->addr[2], mcptr->addr[3],
+                         mcptr->addr[4], mcptr->addr[5]);
+#endif /* < 2.6.35 */
+    }
+#endif
+    p += sprintf(p, "num_tx_bytes = %lu\n", priv->stats.tx_bytes);
+    p += sprintf(p, "num_rx_bytes = %lu\n", priv->stats.rx_bytes);
+    p += sprintf(p, "num_tx_pkts = %lu\n", priv->stats.tx_packets);
+    p += sprintf(p, "num_rx_pkts = %lu\n", priv->stats.rx_packets);
+    p += sprintf(p, "num_tx_pkts_dropped = %lu\n", priv->stats.tx_dropped);
+    p += sprintf(p, "num_rx_pkts_dropped = %lu\n", priv->stats.rx_dropped);
+    p += sprintf(p, "num_tx_pkts_err = %lu\n", priv->stats.tx_errors);
+    p += sprintf(p, "num_rx_pkts_err = %lu\n", priv->stats.rx_errors);
+    p += sprintf(p, "carrier %s\n",
+                 ((netif_carrier_ok(priv->netdev)) ? "on" : "off"));
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+    for (i = 0; i < netdev->num_tx_queues; i++) {
+        p += sprintf(p, "tx queue %d:  %s\n", i,
+                     ((netif_tx_queue_stopped(netdev_get_tx_queue(netdev, 0))) ?
+                      "stopped" : "started"));
+    }
+#else
+    p += sprintf(p, "tx queue %s\n",
+                 ((netif_queue_stopped(priv->netdev)) ? "stopped" : "started"));
+#endif
+#ifdef UAP_SUPPORT
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP) {
+        p += sprintf(p, "tkip_mic_failures = %u\n", ustats.tkip_mic_failures);
+        p += sprintf(p, "ccmp_decrypt_errors = %u\n",
+                     ustats.ccmp_decrypt_errors);
+        p += sprintf(p, "wep_undecryptable_count = %u\n",
+                     ustats.wep_undecryptable_count);
+        p += sprintf(p, "wep_icv_error_count = %u\n",
+                     ustats.wep_icv_error_count);
+        p += sprintf(p, "decrypt_failure_count = %u\n",
+                     ustats.decrypt_failure_count);
+        p += sprintf(p, "mcast_tx_count = %u\n", ustats.mcast_tx_count);
+        p += sprintf(p, "failed_count = %u\n", ustats.failed_count);
+        p += sprintf(p, "retry_count = %u\n", ustats.retry_count);
+        p += sprintf(p, "multiple_retry_count = %u\n",
+                     ustats.multi_retry_count);
+        p += sprintf(p, "frame_duplicate_count = %u\n", ustats.frame_dup_count);
+        p += sprintf(p, "rts_success_count = %u\n", ustats.rts_success_count);
+        p += sprintf(p, "rts_failure_count = %u\n", ustats.rts_failure_count);
+        p += sprintf(p, "ack_failure_count = %u\n", ustats.ack_failure_count);
+        p += sprintf(p, "rx_fragment_count = %u\n", ustats.rx_fragment_count);
+        p += sprintf(p, "mcast_rx_frame_count = %u\n",
+                     ustats.mcast_rx_frame_count);
+        p += sprintf(p, "fcs_error_count = %u\n", ustats.fcs_error_count);
+        p += sprintf(p, "tx_frame_count = %u\n", ustats.tx_frame_count);
+        p += sprintf(p, "rsna_tkip_cm_invoked = %u\n",
+                     ustats.rsna_tkip_cm_invoked);
+        p += sprintf(p, "rsna_4way_hshk_failures = %u\n",
+                     ustats.rsna_4way_hshk_failures);
+    }
+#endif /* UAP_SUPPORT */
+  exit:
+    LEAVE();
+    return (p - page);
+}
+
+#define     CMD52_STR_LEN   50
+/*
+ *  @brief Parse cmd52 string
+ *
+ *  @param buffer  A pointer user buffer
+ *  @param len     Length user buffer
+ *  @param func    Parsed func number
+ *  @param reg     Parsed reg value
+ *  @param val     Parsed value to set
+ *  @return 	   BT_STATUS_SUCCESS
+ */
+static int
+parse_cmd52_string(const char __user * buffer, size_t len, int *func, int *reg,
+                   int *val)
+{
+    int ret = MLAN_STATUS_SUCCESS;
+    char *string = NULL;
+    char *pos = NULL;
+
+    ENTER();
+
+    string = (char *) kmalloc(CMD52_STR_LEN, GFP_KERNEL);
+    memset(string, 0, CMD52_STR_LEN);
+    memcpy(string, buffer + strlen("sdcmd52rw="), len - strlen("sdcmd52rw="));
+    string = strstrip(string);
+
+    *func = -1;
+    *reg = -1;
+    *val = -1;
+
+    /* Get func */
+    pos = strsep(&string, " \t");
+    if (pos) {
+        *func = woal_string_to_number(pos);
+    }
+
+    /* Get reg */
+    pos = strsep(&string, " \t");
+    if (pos) {
+        *reg = woal_string_to_number(pos);
+    }
+
+    /* Get val (optional) */
+    pos = strsep(&string, " \t");
+    if (pos) {
+        *val = woal_string_to_number(pos);
+    }
+    if (string)
+        kfree(string);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief config proc write function
+ *
+ *  @param f	   file pointer
+ *  @param buf     pointer to data buffer
+ *  @param cnt     data number to write
+ *  @param data    data to write
+ *  @return 	   number of data
+ */
+static int
+woal_config_write(struct file *f, const char *buf, unsigned long cnt,
+                  void *data)
+{
+    char databuf[101];
+    char *line;
+    t_u32 config_data = 0;
+    moal_handle *handle = (moal_handle *) data;
+    int func, reg, val;
+
+    ENTER();
+    if (!MODULE_GET) {
+        LEAVE();
+        return 0;
+    }
+
+    if (cnt >= sizeof(databuf)) {
+        MODULE_PUT;
+        LEAVE();
+        return (int) cnt;
+    }
+    memset(databuf, 0, sizeof(databuf));
+    if (copy_from_user(databuf, buf, cnt)) {
+        MODULE_PUT;
+        LEAVE();
+        return 0;
+    }
+    line = databuf;
+    if (!strncmp(databuf, "soft_reset", strlen("soft_reset"))) {
+        line += strlen("soft_reset") + 1;
+        config_data = (t_u32) woal_string_to_number(line);
+        PRINTM(MINFO, "soft_reset: %d\n", (int) config_data);
+        if (woal_request_soft_reset(handle) == MLAN_STATUS_SUCCESS) {
+            handle->hardware_status = HardwareStatusReset;
+        } else {
+            PRINTM(MERROR, "Could not perform soft reset\n");
+        }
+    }
+    if (!strncmp(databuf, "drv_mode", strlen("drv_mode"))) {
+        line += strlen("drv_mode") + 1;
+        config_data = (t_u32) woal_string_to_number(line);
+        PRINTM(MINFO, "drv_mode: %d\n", (int) config_data);
+        if (config_data != (t_u32) drv_mode)
+            if (woal_switch_drv_mode(handle, config_data) !=
+                MLAN_STATUS_SUCCESS) {
+                PRINTM(MERROR, "Could not switch drv mode\n");
+            }
+    }
+    if (!strncmp(databuf, "sdcmd52rw=", strlen("sdcmd52rw="))) {
+        parse_cmd52_string(databuf, (size_t) cnt, &func, &reg, &val);
+        woal_sdio_read_write_cmd52(handle, func, reg, val);
+    }
+    MODULE_PUT;
+    LEAVE();
+    return (int) cnt;
+}
+
+/**
+ *  @brief config proc read function
+ *
+ *  @param page	   pointer to buffer
+ *  @param s       read data starting position
+ *  @param off     offset
+ *  @param cnt     counter
+ *  @param eof     end of file flag
+ *  @param data    data to output
+ *  @return 	   number of output data
+ */
+static int
+woal_config_read(char *page, char **s, off_t off, int cnt, int *eof, void *data)
+{
+    char *p = page;
+    moal_handle *handle = (moal_handle *) data;
+
+    ENTER();
+    if (!MODULE_GET) {
+        LEAVE();
+        return 0;
+    }
+    p += sprintf(p, "hardware_status=%d\n", (int) handle->hardware_status);
+    p += sprintf(p, "netlink_num=%d\n", (int) handle->netlink_num);
+    p += sprintf(p, "drv_mode=%d\n", (int) drv_mode);
+    p += sprintf(p, "sdcmd52rw=%d 0x%0x 0x%02X\n", handle->cmd52_func,
+                 handle->cmd52_reg, handle->cmd52_val);
+    MODULE_PUT;
+    LEAVE();
+    return p - page;
+}
+
+/********************************************************
+		Global Functions
+********************************************************/
+/**
+ *  @brief Convert string to number
+ *
+ *  @param s   	   Pointer to numbered string
+ *
+ *  @return 	   Converted number from string s
+ */
+int
+woal_string_to_number(char *s)
+{
+    int r = 0;
+    int base = 0;
+    int pn = 1;
+
+    if (!strncmp(s, "-", 1)) {
+        pn = -1;
+        s++;
+    }
+    if (!strncmp(s, "0x", 2) || !strncmp(s, "0X", 2)) {
+        base = 16;
+        s += 2;
+    } else
+        base = 10;
+
+    for (s = s; *s; s++) {
+        if ((*s >= '0') && (*s <= '9'))
+            r = (r * base) + (*s - '0');
+        else if ((*s >= 'A') && (*s <= 'F'))
+            r = (r * base) + (*s - 'A' + 10);
+        else if ((*s >= 'a') && (*s <= 'f'))
+            r = (r * base) + (*s - 'a' + 10);
+        else
+            break;
+    }
+
+    return (r * pn);
+}
+
+/**
+ *  @brief Create the top level proc directory
+ *
+ *  @param handle   Pointer to woal_handle
+ *
+ *  @return 	    N/A
+ */
+void
+woal_proc_init(moal_handle * handle)
+{
+    struct proc_dir_entry *r;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,26)
+    struct proc_dir_entry *pde = PROC_DIR;
+#endif
+    char config_proc_dir[20];
+
+    ENTER();
+
+    PRINTM(MINFO, "Create Proc Interface\n");
+    if (!handle->proc_mwlan) {
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,26)
+        /* Check if directory already exists */
+        for (pde = pde->subdir; pde; pde = pde->next) {
+            if (pde->namelen && !strcmp("mwlan", pde->name)) {
+                /* Directory exists */
+                PRINTM(MWARN, "proc interface already exists!\n");
+                handle->proc_mwlan = pde;
+                break;
+            }
+        }
+        if (pde == NULL) {
+            handle->proc_mwlan = proc_mkdir("mwlan", PROC_DIR);
+            if (!handle->proc_mwlan)
+                PRINTM(MERROR, "Cannot create proc interface!\n");
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
+            else
+                atomic_set(&handle->proc_mwlan->count, 1);
+#endif
+        }
+#else
+        if (!proc_mwlan) {
+            handle->proc_mwlan = proc_mkdir("mwlan", PROC_DIR);
+            if (!handle->proc_mwlan) {
+                PRINTM(MERROR, "Cannot create proc interface!\n");
+            }
+        } else {
+            handle->proc_mwlan = proc_mwlan;
+        }
+#endif
+        if (handle->proc_mwlan) {
+            if (handle->handle_idx)
+                sprintf(config_proc_dir, "config%d", handle->handle_idx);
+            else
+                strcpy(config_proc_dir, "config");
+
+            r = create_proc_entry(config_proc_dir, 0644, handle->proc_mwlan);
+
+            if (r) {
+                r->data = handle;
+                r->read_proc = woal_config_read;
+                r->write_proc = woal_config_write;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,30)
+                r->owner = THIS_MODULE;
+#endif
+            } else
+                PRINTM(MMSG, "Fail to create proc config\n");
+        }
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,26)
+        proc_mwlan = handle->proc_mwlan;
+#endif
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief Remove the top level proc directory
+ *
+ *  @param handle  pointer moal_handle
+ *
+ *  @return 	   N/A
+ */
+void
+woal_proc_exit(moal_handle * handle)
+{
+    ENTER();
+
+    PRINTM(MINFO, "Remove Proc Interface\n");
+    if (handle->proc_mwlan) {
+        char config_proc_dir[20];
+        if (handle->handle_idx)
+            sprintf(config_proc_dir, "config%d", handle->handle_idx);
+        else
+            strcpy(config_proc_dir, "config");
+        remove_proc_entry(config_proc_dir, handle->proc_mwlan);
+
+        /* Remove only if we are the only instance using this */
+        if (atomic_read(&(handle->proc_mwlan->count)) > 1) {
+            PRINTM(MWARN, "More than one interface using proc!\n");
+        } else {
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
+            atomic_dec(&(handle->proc_mwlan->count));
+#endif
+            remove_proc_entry("mwlan", PROC_DIR);
+            handle->proc_mwlan = NULL;
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,26)
+            proc_mwlan = NULL;
+#endif
+        }
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief Create proc file for interface
+ *
+ *  @param priv	   pointer moal_private
+ *
+ *  @return 	   N/A
+ */
+void
+woal_create_proc_entry(moal_private * priv)
+{
+    struct net_device *dev = priv->netdev;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+    char proc_dir_name[20];
+#endif
+
+    ENTER();
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+    if (!priv->proc_entry) {
+        memset(proc_dir_name, 0, sizeof(proc_dir_name));
+        strcpy(proc_dir_name, MWLAN_PROC_DIR);
+        strcat(proc_dir_name, dev->name);
+        /* Try to create mwlan/mlanX first */
+        priv->proc_entry = proc_mkdir(proc_dir_name, PROC_DIR);
+        if (priv->proc_entry) {
+            /* Success. Continue normally */
+            if (!priv->phandle->proc_mwlan) {
+                priv->phandle->proc_mwlan = priv->proc_entry->parent;
+            }
+            atomic_inc(&(priv->phandle->proc_mwlan->count));
+        } else {
+            /* Failure. mwlan may not exist. Try to create that first */
+            priv->phandle->proc_mwlan = proc_mkdir("mwlan", PROC_DIR);
+            if (!priv->phandle->proc_mwlan) {
+                /* Failure. Something broken */
+                LEAVE();
+                return;
+            } else {
+                /* Success. Now retry creating mlanX */
+                priv->proc_entry = proc_mkdir(proc_dir_name, PROC_DIR);
+                atomic_inc(&(priv->phandle->proc_mwlan->count));
+            }
+        }
+#else
+    if (priv->phandle->proc_mwlan && !priv->proc_entry) {
+        priv->proc_entry = proc_mkdir(dev->name, priv->phandle->proc_mwlan);
+        atomic_inc(&(priv->phandle->proc_mwlan->count));
+#endif
+        strcpy(priv->proc_entry_name, dev->name);
+        if (priv->proc_entry) {
+            create_proc_read_entry("info", 0, priv->proc_entry,
+                                   woal_info_proc_read, dev);
+        }
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief Remove proc file
+ *
+ *  @param priv	   Pointer moal_private
+ *
+ *  @return 	   N/A
+ */
+void
+woal_proc_remove(moal_private * priv)
+{
+    ENTER();
+    if (priv->phandle->proc_mwlan && priv->proc_entry) {
+        remove_proc_entry("info", priv->proc_entry);
+        remove_proc_entry(priv->proc_entry_name, priv->phandle->proc_mwlan);
+        atomic_dec(&(priv->phandle->proc_mwlan->count));
+        priv->proc_entry = NULL;
+    }
+    LEAVE();
+}
+#endif
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_sdio.h b/drivers/net/wireless/sd8797/mlinux/moal_sdio.h
new file mode 100644
index 000000000000..5a5bc2c3718c
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_sdio.h
@@ -0,0 +1,140 @@
+/** @file moal_sdio.h
+  *
+  * @brief This file contains definitions for SDIO interface.
+  * driver. 
+  *
+  * Copyright (C) 2008-2011, Marvell International Ltd. 
+  * 
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+/****************************************************
+Change log:
+****************************************************/
+
+#ifndef	_MOAL_SDIO_H
+#define	_MOAL_SDIO_H
+
+#include        <linux/mmc/sdio.h>
+#include        <linux/mmc/sdio_ids.h>
+#include        <linux/mmc/sdio_func.h>
+#include        <linux/mmc/card.h>
+#include        <linux/mmc/host.h>
+
+#include "moal_main.h"
+
+#ifndef BLOCK_MODE
+/** Block mode */
+#define BLOCK_MODE	1
+#endif
+
+#ifndef BYTE_MODE
+/** Byte Mode */
+#define BYTE_MODE	0
+#endif
+
+#ifndef FIXED_ADDRESS
+/** Fixed address mode */
+#define FIXED_ADDRESS	0
+#endif
+
+/** SD8797 chip revision ID */
+#define SD8797_A0       0x00
+#define SD8797_B0       0x10
+
+#define SD8797_A0_FW_NAME "mrvl/sd8797_uapsta_a0.bin"
+#define SD8797_B0_FW_NAME "mrvl/sd8797_uapsta.bin"
+
+#ifdef STA_SUPPORT
+/** Default firmware name */
+
+#define DEFAULT_FW_NAME	"mrvl/sd8797_uapsta.bin"
+
+#ifndef DEFAULT_FW_NAME
+#define DEFAULT_FW_NAME ""
+#endif
+#endif /* STA_SUPPORT */
+
+#ifdef UAP_SUPPORT
+/** Default firmware name */
+
+#define DEFAULT_AP_FW_NAME "mrvl/sd8797_uapsta.bin"
+
+#ifndef DEFAULT_AP_FW_NAME
+#define DEFAULT_AP_FW_NAME ""
+#endif
+#endif /* UAP_SUPPORT */
+
+/** Default firmaware name */
+
+#define DEFAULT_AP_STA_FW_NAME "mrvl/sd8797_uapsta.bin"
+
+#ifndef DEFAULT_AP_STA_FW_NAME
+#define DEFAULT_AP_STA_FW_NAME ""
+#endif
+
+/********************************************************
+		Global Functions
+********************************************************/
+
+/** Function to write register */
+mlan_status woal_write_reg(moal_handle * handle, t_u32 reg, t_u32 data);
+/** Function to read register */
+mlan_status woal_read_reg(moal_handle * handle, t_u32 reg, t_u32 * data);
+/** Function to write data to IO memory */
+mlan_status woal_write_data_sync(moal_handle * handle, mlan_buffer * pmbuf,
+                                 t_u32 port, t_u32 timeout);
+/** Function to read data from IO memory */
+mlan_status woal_read_data_sync(moal_handle * handle, mlan_buffer * pmbuf,
+                                t_u32 port, t_u32 timeout);
+
+/** Register to bus driver function */
+mlan_status woal_bus_register(void);
+/** Unregister from bus driver function */
+void woal_bus_unregister(void);
+
+/** Register device function */
+mlan_status woal_register_dev(moal_handle * handle);
+/** Unregister device function */
+void woal_unregister_dev(moal_handle * handle);
+
+int woal_sdio_set_bus_clock(moal_handle * handle, t_u8 option);
+
+#ifdef SDIO_SUSPEND_RESUME
+#ifdef MMC_PM_FUNC_SUSPENDED
+/** Notify SDIO bus driver that WLAN is suspended */
+void woal_wlan_is_suspended(moal_handle * handle);
+#endif
+/** SDIO Suspend */
+int woal_sdio_suspend(struct device *dev);
+/** SDIO Resume */
+int woal_sdio_resume(struct device *dev);
+#endif /* SDIO_SUSPEND_RESUME */
+
+/** Structure: SDIO MMC card */
+struct sdio_mmc_card
+{
+        /** sdio_func structure pointer */
+    struct sdio_func *func;
+        /** moal_handle structure pointer */
+    moal_handle *handle;
+        /** saved host clock value */
+    unsigned int host_clock;
+};
+
+/** cmd52 read write */
+int woal_sdio_read_write_cmd52(moal_handle * handle, int func, int reg,
+                               int val);
+
+#endif /* _MOAL_SDIO_H */
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_sdio_mmc.c b/drivers/net/wireless/sd8797/mlinux/moal_sdio_mmc.c
new file mode 100644
index 000000000000..f09f33e6435d
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_sdio_mmc.c
@@ -0,0 +1,734 @@
+/** @file moal_sdio_mmc.c
+ *
+ *  @brief This file contains SDIO MMC IF (interface) module
+ *  related functions.
+ * 
+ * Copyright (C) 2008-2011, Marvell International Ltd. 
+ *
+ * This software file (the "File") is distributed by Marvell International 
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+ * (the "License").  You may use, redistribute and/or modify this File in 
+ * accordance with the terms and conditions of the License, a copy of which 
+ * is available by writing to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+ * this warranty disclaimer.
+ *
+ */
+/****************************************************
+Change log:
+	02/25/09: Initial creation -
+		  This file supports SDIO MMC only
+****************************************************/
+
+#include <linux/firmware.h>
+
+#include "moal_sdio.h"
+
+/** define marvell vendor id */
+#define MARVELL_VENDOR_ID 0x02df
+
+/********************************************************
+		Local Variables
+********************************************************/
+
+/********************************************************
+		Global Variables
+********************************************************/
+
+#ifdef SDIO_SUSPEND_RESUME
+/** PM keep power */
+extern int pm_keep_power;
+#endif
+
+/** Device ID for SD8797 */
+#define SD_DEVICE_ID_8797   (0x9129)
+
+/** WLAN IDs */
+static const struct sdio_device_id wlan_ids[] = {
+    {SDIO_DEVICE(MARVELL_VENDOR_ID, SD_DEVICE_ID_8797)},
+    {},
+};
+
+MODULE_DEVICE_TABLE(sdio, wlan_ids);
+
+int woal_sdio_probe(struct sdio_func *func, const struct sdio_device_id *id);
+void woal_sdio_remove(struct sdio_func *func);
+
+#ifdef SDIO_SUSPEND_RESUME
+#ifdef MMC_PM_KEEP_POWER
+int woal_sdio_suspend(struct device *dev);
+int woal_sdio_resume(struct device *dev);
+
+static struct dev_pm_ops wlan_sdio_pm_ops = {
+    .suspend = woal_sdio_suspend,
+    .resume = woal_sdio_resume,
+};
+#endif
+#endif
+static struct sdio_driver wlan_sdio = {
+    .name = "wlan_sdio",
+    .id_table = wlan_ids,
+    .probe = woal_sdio_probe,
+    .remove = woal_sdio_remove,
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,29)
+    .drv = {
+            .owner = THIS_MODULE,
+#ifdef SDIO_SUSPEND_RESUME
+#ifdef MMC_PM_KEEP_POWER
+            .pm = &wlan_sdio_pm_ops,
+#endif
+#endif
+            }
+#else
+#ifdef SDIO_SUSPEND_RESUME
+#ifdef MMC_PM_KEEP_POWER
+    .drv = {
+            .pm = &wlan_sdio_pm_ops,
+            }
+#endif
+#endif
+#endif
+};
+
+/********************************************************
+		Local Functions
+********************************************************/
+/**  @brief This function dump the sdio register
+ *
+ *  @param handle  A Pointer to the moal_handle structure
+ *  @return 	   N/A
+ */
+void
+woal_dump_sdio_reg(moal_handle * handle)
+{
+    int ret = 0;
+    t_u8 data;
+    data =
+        sdio_f0_readb(((struct sdio_mmc_card *) handle->card)->func, 0x05,
+                      &ret);
+    PRINTM(MMSG, "fun0: reg 0x05=0x%x ret=%d\n", data, ret);
+    data =
+        sdio_f0_readb(((struct sdio_mmc_card *) handle->card)->func, 0x04,
+                      &ret);
+    PRINTM(MMSG, "fun0: reg 0x04=0x%x ret=%d\n", data, ret);
+    data =
+        sdio_readb(((struct sdio_mmc_card *) handle->card)->func, 0x03, &ret);
+    PRINTM(MMSG, "fun1: reg 0x03=0x%x ret=%d\n", data, ret);
+    data =
+        sdio_readb(((struct sdio_mmc_card *) handle->card)->func, 0x04, &ret);
+    PRINTM(MMSG, "fun1: reg 0x04=0x%x ret=%d\n", data, ret);
+    data =
+        sdio_readb(((struct sdio_mmc_card *) handle->card)->func, 0x05, &ret);
+    PRINTM(MMSG, "fun1: reg 0x05=0x%x ret=%d\n", data, ret);
+    data =
+        sdio_readb(((struct sdio_mmc_card *) handle->card)->func, 0x60, &ret);
+    PRINTM(MMSG, "fun1: reg 0x60=0x%x ret=%d\n", data, ret);
+    data =
+        sdio_readb(((struct sdio_mmc_card *) handle->card)->func, 0x61, &ret);
+    PRINTM(MMSG, "fun1: reg 0x61=0x%x ret=%d\n", data, ret);
+    return;
+}
+
+/********************************************************
+		Global Functions
+********************************************************/
+
+/**
+ *  @brief This function handles the interrupt.
+ *
+ *  @param func	   A pointer to the sdio_func structure
+ *  @return 	   N/A
+ */
+static void
+woal_sdio_interrupt(struct sdio_func *func)
+{
+    moal_handle *handle;
+    struct sdio_mmc_card *card;
+
+    ENTER();
+
+    card = sdio_get_drvdata(func);
+    if (!card || !card->handle) {
+        PRINTM(MINFO,
+               "sdio_mmc_interrupt(func = %p) card or handle is NULL, card=%p\n",
+               func, card);
+        LEAVE();
+        return;
+    }
+    handle = card->handle;
+
+    PRINTM(MINFO, "*** IN SDIO IRQ ***\n");
+    woal_interrupt(handle);
+
+    LEAVE();
+}
+
+/**  @brief This function handles client driver probe.
+ *
+ *  @param func	   A pointer to sdio_func structure.
+ *  @param id	   A pointer to sdio_device_id structure.
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE/error code
+ */
+int
+woal_sdio_probe(struct sdio_func *func, const struct sdio_device_id *id)
+{
+    int ret = MLAN_STATUS_SUCCESS;
+    struct sdio_mmc_card *card = NULL;
+
+    ENTER();
+
+    PRINTM(MINFO, "vendor=0x%4.04X device=0x%4.04X class=%d function=%d\n",
+           func->vendor, func->device, func->class, func->num);
+
+    card = kzalloc(sizeof(struct sdio_mmc_card), GFP_KERNEL);
+    if (!card) {
+        PRINTM(MFATAL, "Failed to allocate memory in probe function!\n");
+        LEAVE();
+        return -ENOMEM;
+    }
+
+    card->func = func;
+
+#ifdef MMC_QUIRK_BLKSZ_FOR_BYTE_MODE
+    /* The byte mode patch is available in kernel MMC driver which fixes one
+       issue in MP-A transfer. bit1: use func->cur_blksize for byte mode */
+    func->card->quirks |= MMC_QUIRK_BLKSZ_FOR_BYTE_MODE;
+#endif
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+    /* wait for chip fully wake up */
+    if (!func->enable_timeout)
+        func->enable_timeout = 200;
+#endif
+    sdio_claim_host(func);
+    ret = sdio_enable_func(func);
+    if (ret) {
+        sdio_disable_func(func);
+        sdio_release_host(func);
+        kfree(card);
+        PRINTM(MFATAL, "sdio_enable_func() failed: ret=%d\n", ret);
+        LEAVE();
+        return -EIO;
+    }
+    sdio_release_host(func);
+    if (NULL == woal_add_card(card)) {
+        PRINTM(MERROR, "woal_add_card failed\n");
+        kfree(card);
+        sdio_claim_host(func);
+        sdio_disable_func(func);
+        sdio_release_host(func);
+        ret = MLAN_STATUS_FAILURE;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**  @brief This function handles client driver remove.
+ *
+ *  @param func	   A pointer to sdio_func structure.
+ *  @return 	   N/A
+ */
+void
+woal_sdio_remove(struct sdio_func *func)
+{
+    struct sdio_mmc_card *card;
+
+    ENTER();
+
+    PRINTM(MINFO, "SDIO func=%d\n", func->num);
+
+    if (func) {
+        card = sdio_get_drvdata(func);
+        if (card) {
+            woal_remove_card(card);
+            kfree(card);
+        }
+    }
+
+    LEAVE();
+}
+
+#ifdef SDIO_SUSPEND_RESUME
+#ifdef MMC_PM_KEEP_POWER
+#ifdef MMC_PM_FUNC_SUSPENDED
+/**  @brief This function tells lower driver that WLAN is suspended
+ *
+ *  @param handle  A Pointer to the moal_handle structure
+ *  @return 	   N/A
+ */
+void
+woal_wlan_is_suspended(moal_handle * handle)
+{
+    ENTER();
+    if (handle->suspend_notify_req == MTRUE) {
+        handle->is_suspended = MTRUE;
+        sdio_func_suspended(((struct sdio_mmc_card *) handle->card)->func);
+    }
+    LEAVE();
+}
+#endif
+
+/**  @brief This function handles client driver suspend
+ *
+ *  @param dev	   A pointer to device structure
+ *  @return 	   MLAN_STATUS_SUCCESS or error code
+ */
+int
+woal_sdio_suspend(struct device *dev)
+{
+    struct sdio_func *func = dev_to_sdio_func(dev);
+    mmc_pm_flag_t pm_flags = 0;
+    moal_handle *handle = NULL;
+    struct sdio_mmc_card *cardp;
+    int i;
+    int ret = MLAN_STATUS_SUCCESS;
+    int hs_actived = 0;
+    mlan_ds_ps_info pm_info;
+
+    ENTER();
+    PRINTM(MCMND, "<--- Enter woal_sdio_suspend --->\n");
+    if (func) {
+        pm_flags = sdio_get_host_pm_caps(func);
+        PRINTM(MCMND, "%s: suspend: PM flags = 0x%x\n", sdio_func_id(func),
+               pm_flags);
+        if (!(pm_flags & MMC_PM_KEEP_POWER)) {
+            PRINTM(MERROR, "%s: cannot remain alive while host is suspended\n",
+                   sdio_func_id(func));
+            LEAVE();
+            return -ENOSYS;
+        }
+        cardp = sdio_get_drvdata(func);
+        if (!cardp || !cardp->handle) {
+            PRINTM(MERROR, "Card or moal_handle structure is not valid\n");
+            LEAVE();
+            return MLAN_STATUS_SUCCESS;
+        }
+    } else {
+        PRINTM(MERROR, "sdio_func is not specified\n");
+        LEAVE();
+        return MLAN_STATUS_SUCCESS;
+    }
+
+    handle = cardp->handle;
+    if (handle->is_suspended == MTRUE) {
+        PRINTM(MWARN, "Device already suspended\n");
+        LEAVE();
+        return MLAN_STATUS_SUCCESS;
+    }
+
+    handle->suspend_fail = MFALSE;
+    memset(&pm_info, 0, sizeof(pm_info));
+    if (MLAN_STATUS_SUCCESS ==
+        woal_get_pm_info(woal_get_priv(handle, MLAN_BSS_ROLE_ANY), &pm_info)) {
+        if (pm_info.is_suspend_allowed == MFALSE) {
+            PRINTM(MMSG, "suspend not allowed!");
+            ret = -EBUSY;
+            goto done;
+        }
+    }
+    for (i = 0; i < handle->priv_num; i++)
+        netif_device_detach(handle->priv[i]->netdev);
+
+    if (pm_keep_power) {
+        /* Enable the Host Sleep */
+#ifdef MMC_PM_FUNC_SUSPENDED
+        handle->suspend_notify_req = MTRUE;
+#endif
+        hs_actived = woal_enable_hs(woal_get_priv(handle, MLAN_BSS_ROLE_ANY));
+#ifdef MMC_PM_FUNC_SUSPENDED
+        handle->suspend_notify_req = MFALSE;
+#endif
+        if (hs_actived) {
+#ifdef MMC_PM_SKIP_RESUME_PROBE
+            PRINTM(MCMND, "suspend with MMC_PM_KEEP_POWER and "
+                   "MMC_PM_SKIP_RESUME_PROBE\n");
+            ret = sdio_set_host_pm_flags(func, MMC_PM_KEEP_POWER |
+                                         MMC_PM_SKIP_RESUME_PROBE);
+#else
+            PRINTM(MCMND, "suspend with MMC_PM_KEEP_POWER\n");
+            ret = sdio_set_host_pm_flags(func, MMC_PM_KEEP_POWER);
+#endif
+        } else {
+            PRINTM(MMSG, "HS not actived, suspend fail!");
+            ret = -EBUSY;
+            goto done;
+        }
+    }
+
+    /* Indicate device suspended */
+    handle->is_suspended = MTRUE;
+  done:
+    PRINTM(MCMND, "<--- Leave woal_sdio_suspend --->\n");
+    LEAVE();
+    return ret;
+}
+
+/**  @brief This function handles client driver resume
+ *
+ *  @param dev	   A pointer to device structure
+ *  @return 	   MLAN_STATUS_SUCCESS
+ */
+int
+woal_sdio_resume(struct device *dev)
+{
+    struct sdio_func *func = dev_to_sdio_func(dev);
+    mmc_pm_flag_t pm_flags = 0;
+    moal_handle *handle = NULL;
+    struct sdio_mmc_card *cardp;
+    int i;
+
+    ENTER();
+    PRINTM(MCMND, "<--- Enter woal_sdio_resume --->\n");
+    if (func) {
+        pm_flags = sdio_get_host_pm_caps(func);
+        PRINTM(MCMND, "%s: resume: PM flags = 0x%x\n", sdio_func_id(func),
+               pm_flags);
+        cardp = sdio_get_drvdata(func);
+        if (!cardp || !cardp->handle) {
+            PRINTM(MERROR, "Card or moal_handle structure is not valid\n");
+            LEAVE();
+            return MLAN_STATUS_SUCCESS;
+        }
+    } else {
+        PRINTM(MERROR, "sdio_func is not specified\n");
+        LEAVE();
+        return MLAN_STATUS_SUCCESS;
+    }
+    handle = cardp->handle;
+
+    if (handle->is_suspended == MFALSE) {
+        PRINTM(MWARN, "Device already resumed\n");
+        LEAVE();
+        return MLAN_STATUS_SUCCESS;
+    }
+
+    handle->is_suspended = MFALSE;
+    for (i = 0; i < handle->priv_num; i++)
+        netif_device_attach(handle->priv[i]->netdev);
+
+    /* Disable Host Sleep */
+    woal_cancel_hs(woal_get_priv(handle, MLAN_BSS_ROLE_ANY), MOAL_NO_WAIT);
+    PRINTM(MCMND, "<--- Leave woal_sdio_resume --->\n");
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+#endif
+#endif /* SDIO_SUSPEND_RESUME */
+
+/**
+ *  @brief This function writes data into card register
+ *
+ *  @param handle   A Pointer to the moal_handle structure
+ *  @param reg      Register offset
+ *  @param data     Value
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_write_reg(moal_handle * handle, t_u32 reg, t_u32 data)
+{
+    mlan_status ret = MLAN_STATUS_FAILURE;
+    sdio_writeb(((struct sdio_mmc_card *) handle->card)->func, (t_u8) data, reg,
+                (int *) &ret);
+    return ret;
+}
+
+/**
+ *  @brief This function reads data from card register
+ *
+ *  @param handle   A Pointer to the moal_handle structure
+ *  @param reg      Register offset
+ *  @param data     Value
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_read_reg(moal_handle * handle, t_u32 reg, t_u32 * data)
+{
+    mlan_status ret = MLAN_STATUS_FAILURE;
+    t_u8 val;
+
+    val =
+        sdio_readb(((struct sdio_mmc_card *) handle->card)->func, reg,
+                   (int *) &ret);
+    *data = val;
+
+    return ret;
+}
+
+/**
+ *  @brief This function writes multiple bytes into card memory
+ *
+ *  @param handle   A Pointer to the moal_handle structure
+ *  @param pmbuf	Pointer to mlan_buffer structure
+ *  @param port		Port
+ *  @param timeout 	Time out value
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_write_data_sync(moal_handle * handle, mlan_buffer * pmbuf, t_u32 port,
+                     t_u32 timeout)
+{
+    mlan_status ret = MLAN_STATUS_FAILURE;
+    t_u8 *buffer = (t_u8 *) (pmbuf->pbuf + pmbuf->data_offset);
+    t_u8 blkmode = (port & MLAN_SDIO_BYTE_MODE_MASK) ? BYTE_MODE : BLOCK_MODE;
+    t_u32 blksz = (blkmode == BLOCK_MODE) ? MLAN_SDIO_BLOCK_SIZE : 1;
+    t_u32 blkcnt =
+        (blkmode ==
+         BLOCK_MODE) ? (pmbuf->data_len /
+                        MLAN_SDIO_BLOCK_SIZE) : pmbuf->data_len;
+    t_u32 ioport = (port & MLAN_SDIO_IO_PORT_MASK);
+#ifdef SDIO_MMC_DEBUG
+    handle->cmd53w = 1;
+#endif
+    if (!sdio_writesb
+        (((struct sdio_mmc_card *) handle->card)->func, ioport, buffer,
+         blkcnt * blksz))
+        ret = MLAN_STATUS_SUCCESS;
+#ifdef SDIO_MMC_DEBUG
+    handle->cmd53w = 2;
+#endif
+    return ret;
+}
+
+/**
+ *  @brief This function reads multiple bytes from card memory
+ *
+ *  @param handle   A Pointer to the moal_handle structure
+ *  @param pmbuf	Pointer to mlan_buffer structure
+ *  @param port		Port
+ *  @param timeout 	Time out value
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_read_data_sync(moal_handle * handle, mlan_buffer * pmbuf, t_u32 port,
+                    t_u32 timeout)
+{
+    mlan_status ret = MLAN_STATUS_FAILURE;
+    t_u8 *buffer = (t_u8 *) (pmbuf->pbuf + pmbuf->data_offset);
+    t_u8 blkmode = (port & MLAN_SDIO_BYTE_MODE_MASK) ? BYTE_MODE : BLOCK_MODE;
+    t_u32 blksz = (blkmode == BLOCK_MODE) ? MLAN_SDIO_BLOCK_SIZE : 1;
+    t_u32 blkcnt =
+        (blkmode ==
+         BLOCK_MODE) ? (pmbuf->data_len /
+                        MLAN_SDIO_BLOCK_SIZE) : pmbuf->data_len;
+    t_u32 ioport = (port & MLAN_SDIO_IO_PORT_MASK);
+
+#ifdef SDIO_MMC_DEBUG
+    handle->cmd53r = 1;
+#endif
+    if (!sdio_readsb
+        (((struct sdio_mmc_card *) handle->card)->func, buffer, ioport,
+         blkcnt * blksz))
+        ret = MLAN_STATUS_SUCCESS;
+    else
+        woal_dump_sdio_reg(handle);
+#ifdef SDIO_MMC_DEBUG
+    handle->cmd53r = 2;
+#endif
+    return ret;
+}
+
+/**
+ *  @brief This function registers the IF module in bus driver
+ *
+ *  @return	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_bus_register(void)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    /* SDIO Driver Registration */
+    if (sdio_register_driver(&wlan_sdio)) {
+        PRINTM(MFATAL, "SDIO Driver Registration Failed \n");
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function de-registers the IF module in bus driver
+ *
+ *  @return 	   N/A
+ */
+void
+woal_bus_unregister(void)
+{
+    ENTER();
+
+    /* SDIO Driver Unregistration */
+    sdio_unregister_driver(&wlan_sdio);
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function de-registers the device
+ *
+ *  @param handle A pointer to moal_handle structure
+ *  @return 	  N/A
+ */
+void
+woal_unregister_dev(moal_handle * handle)
+{
+    ENTER();
+    if (handle->card) {
+        /* Release the SDIO IRQ */
+        sdio_claim_host(((struct sdio_mmc_card *) handle->card)->func);
+        sdio_release_irq(((struct sdio_mmc_card *) handle->card)->func);
+        sdio_disable_func(((struct sdio_mmc_card *) handle->card)->func);
+        sdio_release_host(((struct sdio_mmc_card *) handle->card)->func);
+
+        sdio_set_drvdata(((struct sdio_mmc_card *) handle->card)->func, NULL);
+
+        PRINTM(MWARN, "Making the sdio dev card as NULL\n");
+    }
+
+    LEAVE();
+}
+
+/**
+ *  @brief This function registers the device
+ *
+ *  @param handle  A pointer to moal_handle structure
+ *  @return 	   MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_register_dev(moal_handle * handle)
+{
+    int ret = MLAN_STATUS_SUCCESS;
+    struct sdio_mmc_card *card = handle->card;
+    struct sdio_func *func;
+
+    ENTER();
+
+    func = card->func;
+    sdio_claim_host(func);
+    /* Request the SDIO IRQ */
+    ret = sdio_claim_irq(func, woal_sdio_interrupt);
+    if (ret) {
+        PRINTM(MFATAL, "sdio_claim_irq failed: ret=%d\n", ret);
+        goto release_host;
+    }
+
+    /* Set block size */
+    ret = sdio_set_block_size(card->func, MLAN_SDIO_BLOCK_SIZE);
+    if (ret) {
+        PRINTM(MERROR, "sdio_set_block_seize(): cannot set SDIO block size\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto release_irq;
+    }
+
+    sdio_release_host(func);
+    sdio_set_drvdata(func, card);
+
+    handle->hotplug_device = &func->dev;
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+
+  release_irq:
+    sdio_release_irq(func);
+  release_host:
+    sdio_release_host(func);
+    handle->card = NULL;
+
+    LEAVE();
+    return MLAN_STATUS_FAILURE;
+}
+
+/**
+ *  @brief This function set bus clock on/off
+ *
+ *  @param handle    A pointer to moal_handle structure
+ *  @param option    TRUE--on , FALSE--off
+ *  @return 	   MLAN_STATUS_SUCCESS
+ */
+int
+woal_sdio_set_bus_clock(moal_handle * handle, t_u8 option)
+{
+    struct sdio_mmc_card *cardp = (struct sdio_mmc_card *) handle->card;
+    struct mmc_host *host = cardp->func->card->host;
+
+    ENTER();
+    if (option == MTRUE) {
+        /* restore value if non-zero */
+        if (cardp->host_clock)
+            host->ios.clock = cardp->host_clock;
+    } else {
+        /* backup value if non-zero, then clear */
+        if (host->ios.clock)
+            cardp->host_clock = host->ios.clock;
+        host->ios.clock = 0;
+    }
+
+    host->ops->set_ios(host, &host->ios);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function updates card reg based on the Cmd52 value in dev structure
+ *
+ *  @param handle  	A pointer to moal_handle structure
+ *  @param func    	A pointer to store func variable
+ *  @param reg    	A pointer to store reg variable
+ *  @param val    	A pointer to store val variable
+ *  @return 	   	MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+int
+woal_sdio_read_write_cmd52(moal_handle * handle, int func, int reg, int val)
+{
+    int ret = MLAN_STATUS_SUCCESS;
+    struct sdio_mmc_card *card = (struct sdio_mmc_card *) handle->card;
+
+    ENTER();
+    /* Save current func and reg for read */
+    handle->cmd52_func = func;
+    handle->cmd52_reg = reg;
+    sdio_claim_host(card->func);
+    if (val >= 0) {
+        /* Perform actual write only if val is provided */
+        if (func)
+            sdio_writeb(card->func, val, reg, &ret);
+        else
+            sdio_f0_writeb(card->func, val, reg, &ret);
+        if (ret) {
+            PRINTM(MERROR, "Cannot write value (0x%x) to func %d reg 0x%x\n",
+                   val, func, reg);
+        } else {
+            PRINTM(MMSG, "write value (0x%x) to func %d reg 0x%x\n", (u8) val,
+                   func, reg);
+            handle->cmd52_val = val;
+        }
+    } else {
+        if (func)
+            val = sdio_readb(card->func, reg, &ret);
+        else
+            val = sdio_f0_readb(card->func, reg, &ret);
+        if (ret) {
+            PRINTM(MERROR, "Cannot read value from func %d reg 0x%x\n", func,
+                   reg);
+        } else {
+            PRINTM(MMSG, "read value (0x%x) from func %d reg 0x%x\n", (u8) val,
+                   func, reg);
+            handle->cmd52_val = val;
+        }
+    }
+    sdio_release_host(card->func);
+    LEAVE();
+    return ret;
+}
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_shim.c b/drivers/net/wireless/sd8797/mlinux/moal_shim.c
new file mode 100644
index 000000000000..25cfdc549f3d
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_shim.c
@@ -0,0 +1,1527 @@
+/** @file moal_shim.c
+  *
+  * @brief This file contains the callback functions registered to MLAN
+  *
+  * Copyright (C) 2008-2011, Marvell International Ltd. 
+  * 
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+
+/********************************************************
+Change log:
+    10/21/2008: initial version
+********************************************************/
+
+#include	"moal_main.h"
+#include	"moal_sdio.h"
+#ifdef UAP_SUPPORT
+#include    "moal_uap.h"
+#endif
+#ifdef STA_CFG80211
+#include "moal_cfg80211.h"
+#endif
+
+/********************************************************
+		Local Variables
+********************************************************/
+/** moal_lock */
+typedef struct _moal_lock
+{
+        /** Lock */
+    spinlock_t lock;
+        /** Flags */
+    unsigned long flags;
+} moal_lock;
+
+/********************************************************
+		Global Variables
+********************************************************/
+extern int cfg80211_wext;
+
+/********************************************************
+		Local Functions
+********************************************************/
+
+/********************************************************
+		Global Functions
+********************************************************/
+
+/**
+ *  @brief Alloc a buffer
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param size 	The size of the buffer to be allocated
+ *  @param flag 	The type of the buffer to be allocated
+ *  @param ppbuf	Pointer to a buffer location to store buffer pointer allocated
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_malloc(IN t_void * pmoal_handle,
+            IN t_u32 size, IN t_u32 flag, OUT t_u8 ** ppbuf)
+{
+    moal_handle *handle = (moal_handle *) pmoal_handle;
+    t_u32 mem_flag = GFP_ATOMIC;        /* Default type: GFP_ATOMIC */
+
+    if (flag & MLAN_MEM_DMA)
+        mem_flag |= GFP_DMA;
+
+    if (!(*ppbuf = kmalloc(size, mem_flag))) {
+        PRINTM(MERROR, "%s: allocate  buffer %d failed!\n", __FUNCTION__,
+               (int) size);
+        return MLAN_STATUS_FAILURE;
+    }
+    handle->malloc_count++;
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Free a buffer
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param pbuf		Pointer to the buffer to be freed
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_mfree(IN t_void * pmoal_handle, IN t_u8 * pbuf)
+{
+    moal_handle *handle = (moal_handle *) pmoal_handle;
+
+    if (!pbuf)
+        return MLAN_STATUS_FAILURE;
+    kfree(pbuf);
+    handle->malloc_count--;
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Fill memory with constant byte
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param pmem		Pointer to the memory area
+ *  @param byte		A constant byte
+ *  @param num		Number of bytes to fill
+ *
+ *  @return    		Pointer to the memory area
+ */
+t_void *
+moal_memset(IN t_void * pmoal_handle,
+            IN t_void * pmem, IN t_u8 byte, IN t_u32 num)
+{
+    t_void *p = pmem;
+
+    if (pmem && num)
+        p = memset(pmem, byte, num);
+
+    return p;
+}
+
+/**
+ *  @brief Copy memory from one area to another
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param pdest	Pointer to the dest memory
+ *  @param psrc		Pointer to the src memory
+ *  @param num		Number of bytes to move
+ *
+ *  @return    		Pointer to the dest memory
+ */
+t_void *
+moal_memcpy(IN t_void * pmoal_handle,
+            IN t_void * pdest, IN const t_void * psrc, IN t_u32 num)
+{
+    t_void *p = pdest;
+
+    if (pdest && psrc && num)
+        p = memcpy(pdest, psrc, num);
+
+    return p;
+}
+
+/**
+ *  @brief Move memory from one area to another
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param pdest	Pointer to the dest memory
+ *  @param psrc		Pointer to the src memory
+ *  @param num		Number of bytes to move
+ *
+ *  @return    		Pointer to the dest memory
+ */
+t_void *
+moal_memmove(IN t_void * pmoal_handle,
+             IN t_void * pdest, IN const t_void * psrc, IN t_u32 num)
+{
+    t_void *p = pdest;
+
+    if (pdest && psrc && num)
+        p = memmove(pdest, psrc, num);
+
+    return p;
+}
+
+/**
+ *  @brief Compare two memory areas
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param pmem1	Pointer to the first memory
+ *  @param pmem2	Pointer to the second memory
+ *  @param num		Number of bytes to compare
+ *
+ *  @return    		Compare result returns by memcmp
+ */
+t_s32
+moal_memcmp(IN t_void * pmoal_handle,
+            IN const t_void * pmem1, IN const t_void * pmem2, IN t_u32 num)
+{
+    t_s32 result;
+
+    result = memcmp(pmem1, pmem2, num);
+
+    return result;
+}
+
+/**
+ *  @brief Delay function
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param delay  delay in micro-second
+ *
+ *  @return    	  N/A
+ */
+t_void
+moal_udelay(IN t_void * pmoal_handle, IN t_u32 delay)
+{
+    if (delay >= 1000)
+        mdelay(delay / 1000);
+    if (delay % 1000)
+        udelay(delay % 1000);
+}
+
+/**
+ *  @brief Retrieves the current system time
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param psec		Pointer to buf for the seconds of system time
+ *  @param pusec 	Pointer to buf the micro seconds of system time
+ *
+ *  @return    		MLAN_STATUS_SUCCESS
+ */
+mlan_status
+moal_get_system_time(IN t_void * pmoal_handle,
+                     OUT t_u32 * psec, OUT t_u32 * pusec)
+{
+    struct timeval t;
+
+    do_gettimeofday(&t);
+    *psec = (t_u32) t.tv_sec;
+    *pusec = (t_u32) t.tv_usec;
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Initializes the timer
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param pptimer	Pointer to the timer
+ *  @param callback 	Pointer to callback function
+ *  @param pcontext 	Pointer to context
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_init_timer(IN t_void * pmoal_handle,
+                OUT t_void ** pptimer,
+                IN t_void(*callback) (t_void * pcontext), IN t_void * pcontext)
+{
+    moal_drv_timer *timer = NULL;
+
+    if (!
+        (timer =
+         (moal_drv_timer *) kmalloc(sizeof(moal_drv_timer), GFP_KERNEL)))
+        return MLAN_STATUS_FAILURE;
+    woal_initialize_timer(timer, callback, pcontext);
+    *pptimer = (t_void *) timer;
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Free the timer
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param ptimer	Pointer to the timer
+ *
+ *  @return    		MLAN_STATUS_SUCCESS
+ */
+mlan_status
+moal_free_timer(IN t_void * pmoal_handle, IN t_void * ptimer)
+{
+    moal_drv_timer *timer = (moal_drv_timer *) ptimer;
+
+    if (timer) {
+        if ((timer->timer_is_canceled == MFALSE) && timer->time_period) {
+            PRINTM(MWARN, "mlan try to free timer without stop timer!\n");
+            woal_cancel_timer(timer);
+        }
+        kfree(timer);
+    }
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Start the timer
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param ptimer	Pointer to the timer
+ *  @param periodic     Periodic timer
+ *  @param msec		Timer value in milliseconds
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_start_timer(IN t_void * pmoal_handle,
+                 IN t_void * ptimer, IN t_u8 periodic, IN t_u32 msec)
+{
+    if (!ptimer)
+        return MLAN_STATUS_FAILURE;
+
+    ((moal_drv_timer *) ptimer)->timer_is_periodic = periodic;
+    woal_mod_timer((moal_drv_timer *) ptimer, msec);
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Stop the timer
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param ptimer	Pointer to the timer
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_stop_timer(IN t_void * pmoal_handle, IN t_void * ptimer)
+{
+    if (!ptimer)
+        return MLAN_STATUS_FAILURE;
+    woal_cancel_timer((moal_drv_timer *) ptimer);
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Initializes the lock
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param pplock	Pointer to the lock
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_init_lock(IN t_void * pmoal_handle, OUT t_void ** pplock)
+{
+    moal_handle *handle = (moal_handle *) pmoal_handle;
+    moal_lock *mlock = NULL;
+
+    if (!(mlock = (moal_lock *) kmalloc(sizeof(moal_lock), GFP_ATOMIC)))
+        return MLAN_STATUS_FAILURE;
+    spin_lock_init(&mlock->lock);
+    *pplock = (t_void *) mlock;
+
+    handle->lock_count++;
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Free the lock
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param plock	Lock
+ *
+ *  @return    		MLAN_STATUS_SUCCESS
+ */
+mlan_status
+moal_free_lock(IN t_void * pmoal_handle, IN t_void * plock)
+{
+    moal_handle *handle = (moal_handle *) pmoal_handle;
+    moal_lock *mlock = plock;
+
+    if (mlock) {
+        kfree(mlock);
+        handle->lock_count--;
+    }
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief Request a spin lock
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param plock	Pointer to the lock
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_spin_lock(IN t_void * pmoal_handle, IN t_void * plock)
+{
+    moal_lock *mlock = plock;
+    unsigned long flags = 0;
+
+    if (mlock) {
+        spin_lock_irqsave(&mlock->lock, flags);
+        mlock->flags = flags;
+        return MLAN_STATUS_SUCCESS;
+    } else {
+        return MLAN_STATUS_FAILURE;
+    }
+}
+
+/**
+ *  @brief Request a spin_unlock
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param plock	Pointer to the lock
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_spin_unlock(IN t_void * pmoal_handle, IN t_void * plock)
+{
+    moal_lock *mlock = (moal_lock *) plock;
+
+    if (mlock) {
+        spin_unlock_irqrestore(&mlock->lock, mlock->flags);
+
+        return MLAN_STATUS_SUCCESS;
+    } else {
+        return MLAN_STATUS_FAILURE;
+    }
+}
+
+/**
+ *  @brief This function reads one block of firmware data from MOAL
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param offset       Offset from where the data will be copied
+ *  @param len          Length to be copied
+ *  @param pbuf         Buffer where the data will be copied
+ *
+ *  @return             MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_get_fw_data(IN t_void * pmoal_handle,
+                 IN t_u32 offset, IN t_u32 len, OUT t_u8 * pbuf)
+{
+    moal_handle *handle = (moal_handle *) pmoal_handle;
+
+    if (!pbuf || !len)
+        return MLAN_STATUS_FAILURE;
+
+    if (offset + len > handle->firmware->size)
+        return MLAN_STATUS_FAILURE;
+
+    memcpy(pbuf, handle->firmware->data + offset, len);
+
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function is called when MLAN completes the initialization firmware.
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param status	The status code for mlan_init_fw request
+ *
+ *  @return    		MLAN_STATUS_SUCCESS
+ */
+mlan_status
+moal_init_fw_complete(IN t_void * pmoal_handle, IN mlan_status status)
+{
+    moal_handle *handle = (moal_handle *) pmoal_handle;
+    ENTER();
+    if (status == MLAN_STATUS_SUCCESS)
+        handle->hardware_status = HardwareStatusReady;
+    handle->init_wait_q_woken = MTRUE;
+    wake_up_interruptible(&handle->init_wait_q);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function is called when MLAN shutdown firmware is completed.
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param status	The status code for mlan_shutdown request
+ *
+ *  @return    		MLAN_STATUS_SUCCESS
+ */
+mlan_status
+moal_shutdown_fw_complete(IN t_void * pmoal_handle, IN mlan_status status)
+{
+    moal_handle *handle = (moal_handle *) pmoal_handle;
+    ENTER();
+    handle->hardware_status = HardwareStatusNotReady;
+    handle->init_wait_q_woken = MTRUE;
+    wake_up_interruptible(&handle->init_wait_q);
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function is called when an MLAN IOCTL is completed.
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param pioctl_req	pointer to structure mlan_ioctl_req
+ *  @param status	The status code for mlan_ioctl request
+ *
+ *  @return    		MLAN_STATUS_SUCCESS
+ */
+mlan_status
+moal_ioctl_complete(IN t_void * pmoal_handle,
+                    IN pmlan_ioctl_req pioctl_req, IN mlan_status status)
+{
+    moal_handle *handle = (moal_handle *) pmoal_handle;
+    moal_private *priv = NULL;
+    wait_queue *wait;
+    ENTER();
+
+    if (!atomic_read(&handle->ioctl_pending))
+        PRINTM(MERROR, "ERR: Unexpected IOCTL completed: %p\n", pioctl_req);
+    else
+        atomic_dec(&handle->ioctl_pending);
+    priv = woal_bss_index_to_priv(handle, pioctl_req->bss_index);
+
+    wait = (wait_queue *) pioctl_req->reserved_1;
+    PRINTM(MIOCTL,
+           "IOCTL completed: %p id=0x%x sub_id=0x%x, action=%d,  status=%d, status_code=0x%x\n",
+           pioctl_req, pioctl_req->req_id, (*(t_u32 *) pioctl_req->pbuf),
+           (int) pioctl_req->action, status, pioctl_req->status_code);
+    if (wait) {
+        wait->condition = MTRUE;
+        wait->status = status;
+        if ((status != MLAN_STATUS_SUCCESS) &&
+            (pioctl_req->status_code == MLAN_ERROR_CMD_TIMEOUT)) {
+            PRINTM(MERROR, "IOCTL: command timeout\n");
+        } else {
+            wake_up_interruptible(wait->wait);
+        }
+    } else {
+        if (priv && (status == MLAN_STATUS_SUCCESS) &&
+            (pioctl_req->action == MLAN_ACT_GET))
+            woal_process_ioctl_resp(priv, pioctl_req);
+        if (status != MLAN_STATUS_SUCCESS)
+            PRINTM(MERROR,
+                   "IOCTL failed: id=0x%x, action=%d, status_code=0x%x\n",
+                   pioctl_req->req_id, (int) pioctl_req->action,
+                   pioctl_req->status_code);
+        kfree(pioctl_req);
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function allocates mlan_buffer.
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param size		allocation size requested
+ *  @param pmbuf	pointer to pointer to the allocated buffer
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_alloc_mlan_buffer(IN t_void * pmoal_handle,
+                       IN t_u32 size, OUT pmlan_buffer * pmbuf)
+{
+    if (NULL ==
+        (*pmbuf = woal_alloc_mlan_buffer((moal_handle *) pmoal_handle, size)))
+        return MLAN_STATUS_FAILURE;
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function frees mlan_buffer.
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param pmbuf	pointer to buffer to be freed
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_free_mlan_buffer(IN t_void * pmoal_handle, IN pmlan_buffer pmbuf)
+{
+    if (!pmbuf)
+        return MLAN_STATUS_FAILURE;
+    woal_free_mlan_buffer((moal_handle *) pmoal_handle, pmbuf);
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function is called when MLAN complete send data packet.
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param pmbuf	Pointer to the mlan buffer structure
+ *  @param status	The status code for mlan_send_packet request
+ *
+ *  @return    		MLAN_STATUS_SUCCESS
+ */
+mlan_status
+moal_send_packet_complete(IN t_void * pmoal_handle,
+                          IN pmlan_buffer pmbuf, IN mlan_status status)
+{
+    moal_private *priv = NULL;
+    moal_handle *handle = (moal_handle *) pmoal_handle;
+    struct sk_buff *skb = NULL;
+    int i;
+    ENTER();
+    if (pmbuf && pmbuf->buf_type == MLAN_BUF_TYPE_RAW_DATA) {
+        woal_free_mlan_buffer(handle, pmbuf);
+        atomic_dec(&handle->tx_pending);
+        LEAVE();
+        return MLAN_STATUS_SUCCESS;
+    }
+    if (pmbuf) {
+        priv = woal_bss_index_to_priv(pmoal_handle, pmbuf->bss_index);
+        skb = (struct sk_buff *) pmbuf->pdesc;
+        if (priv) {
+            woal_set_trans_start(priv->netdev);
+            if (skb) {
+                if (status == MLAN_STATUS_SUCCESS) {
+                    priv->stats.tx_packets++;
+                    priv->stats.tx_bytes += skb->len;
+                } else {
+                    priv->stats.tx_errors++;
+                }
+                if (atomic_dec_return(&handle->tx_pending) < LOW_TX_PENDING) {
+                    for (i = 0; i < handle->priv_num; i++) {
+#ifdef STA_SUPPORT
+                        if ((GET_BSS_ROLE(handle->priv[i]) == MLAN_BSS_ROLE_STA)
+                            && (handle->priv[i]->media_connected ||
+                                priv->is_adhoc_link_sensed)) {
+                            woal_wake_queue(handle->priv[i]->netdev);
+                        }
+#endif
+#ifdef UAP_SUPPORT
+                        if ((GET_BSS_ROLE(handle->priv[i]) == MLAN_BSS_ROLE_UAP)
+                            && (handle->priv[i]->media_connected)) {
+                            woal_wake_queue(handle->priv[i]->netdev);
+                        }
+#endif
+                    }
+                }
+            }
+        }
+        if (skb)
+            dev_kfree_skb_any(skb);
+    }
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function write a command/data packet to card.
+ *         This function blocks the call until it finishes
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param pmbuf	Pointer to the mlan buffer structure
+ *  @param port 	Port number for sent
+ *  @param timeout 	Timeout value in milliseconds (if 0 the wait is forever)
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_write_data_sync(IN t_void * pmoal_handle,
+                     IN pmlan_buffer pmbuf, IN t_u32 port, IN t_u32 timeout)
+{
+    return woal_write_data_sync((moal_handle *) pmoal_handle, pmbuf, port,
+                                timeout);
+}
+
+/**
+ *  @brief This function read data packet/event/command from card.
+ *         This function blocks the call until it finish
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param pmbuf	Pointer to the mlan buffer structure
+ *  @param port 	Port number for read
+ *  @param timeout 	Timeout value in milliseconds (if 0 the wait is forever)
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_read_data_sync(IN t_void * pmoal_handle,
+                    IN OUT pmlan_buffer pmbuf, IN t_u32 port, IN t_u32 timeout)
+{
+    return woal_read_data_sync((moal_handle *) pmoal_handle, pmbuf, port,
+                               timeout);
+}
+
+/**
+ *  @brief This function writes data into card register.
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param reg          register offset
+ *  @param data         value
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_write_reg(IN t_void * pmoal_handle, IN t_u32 reg, IN t_u32 data)
+{
+    return woal_write_reg((moal_handle *) pmoal_handle, reg, data);
+}
+
+/**
+ *  @brief This function reads data from card register.
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param reg          register offset
+ *  @param data         value
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_read_reg(IN t_void * pmoal_handle, IN t_u32 reg, OUT t_u32 * data)
+{
+    return woal_read_reg((moal_handle *) pmoal_handle, reg, data);
+}
+
+/**
+ *  @brief This function uploads the packet to the network stack
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param pmbuf	Pointer to the mlan buffer structure
+ *
+ *  @return    		MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+moal_recv_packet(IN t_void * pmoal_handle, IN pmlan_buffer pmbuf)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+    moal_private *priv = NULL;
+    struct sk_buff *skb = NULL;
+    ENTER();
+    if (pmbuf) {
+        priv = woal_bss_index_to_priv(pmoal_handle, pmbuf->bss_index);
+        skb = (struct sk_buff *) pmbuf->pdesc;
+        if (priv) {
+            if (skb) {
+                skb_reserve(skb, pmbuf->data_offset);
+                skb_put(skb, pmbuf->data_len);
+                pmbuf->pdesc = NULL;
+                pmbuf->pbuf = NULL;
+                pmbuf->data_offset = pmbuf->data_len = 0;
+            } else {
+                PRINTM(MERROR, "%s without skb attach!!!\n", __FUNCTION__);
+                if (!(skb = dev_alloc_skb(pmbuf->data_len + MLAN_NET_IP_ALIGN))) {
+                    PRINTM(MERROR, "%s fail to alloc skb\n", __FUNCTION__);
+                    status = MLAN_STATUS_FAILURE;
+                    priv->stats.rx_dropped++;
+                    goto done;
+                }
+                skb_reserve(skb, MLAN_NET_IP_ALIGN);
+                memcpy(skb->data, (t_u8 *) (pmbuf->pbuf + pmbuf->data_offset),
+                       pmbuf->data_len);
+                skb_put(skb, pmbuf->data_len);
+            }
+            skb->dev = priv->netdev;
+            skb->protocol = eth_type_trans(skb, priv->netdev);
+            skb->ip_summed = CHECKSUM_NONE;
+
+            if (priv->enable_tcp_ack_enh == MTRUE) {
+                woal_check_tcp_fin(priv, skb);
+            }
+
+            priv->stats.rx_bytes += skb->len;
+            priv->stats.rx_packets++;
+            if (in_interrupt())
+                netif_rx(skb);
+            else
+                netif_rx_ni(skb);
+        }
+    }
+  done:
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief This function handles event receive
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param pmevent	Pointer to the mlan event structure
+ *
+ *  @return    		MLAN_STATUS_SUCCESS
+ */
+mlan_status
+moal_recv_event(IN t_void * pmoal_handle, IN pmlan_event pmevent)
+{
+    moal_private *priv = NULL;
+#if defined(STA_WEXT) || defined(UAP_SUPPORT)
+    moal_private *pmpriv = NULL;
+#endif
+#if defined(STA_WEXT) || defined(UAP_WEXT)
+#if defined(STA_SUPPORT) || defined(UAP_WEXT)
+#if defined(UAP_SUPPORT) || defined(STA_WEXT)
+    union iwreq_data wrqu;
+#endif
+#endif
+#endif
+#if defined(SDIO_SUSPEND_RESUME)
+    mlan_ds_ps_info pm_info;
+#endif
+    ENTER();
+
+    PRINTM(MEVENT, "event id:0x%x\n", pmevent->event_id);
+    priv = woal_bss_index_to_priv(pmoal_handle, pmevent->bss_index);
+    if (priv == NULL) {
+        PRINTM(MERROR, "%s: priv is null\n", __FUNCTION__);
+        goto done;
+    }
+    switch (pmevent->event_id) {
+#ifdef STA_SUPPORT
+    case MLAN_EVENT_ID_FW_ADHOC_LINK_SENSED:
+        priv->is_adhoc_link_sensed = MTRUE;
+        if (!netif_carrier_ok(priv->netdev))
+            netif_carrier_on(priv->netdev);
+        woal_wake_queue(priv->netdev);
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_ADHOC_LINK_SENSED);
+#endif
+        break;
+
+    case MLAN_EVENT_ID_FW_ADHOC_LINK_LOST:
+        woal_stop_queue(priv->netdev);
+        if (netif_carrier_ok(priv->netdev))
+            netif_carrier_off(priv->netdev);
+        priv->is_adhoc_link_sensed = MFALSE;
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_ADHOC_LINK_LOST);
+#endif
+        break;
+
+    case MLAN_EVENT_ID_DRV_CONNECTED:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext) && pmevent->event_len == ETH_ALEN) {
+            memset(wrqu.ap_addr.sa_data, 0x00, ETH_ALEN);
+            memcpy(wrqu.ap_addr.sa_data, pmevent->event_buf, ETH_ALEN);
+            wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+            wireless_send_event(priv->netdev, SIOCGIWAP, &wrqu, NULL);
+        }
+#endif
+#ifdef STA_CFG80211
+        if (IS_STA_CFG80211(cfg80211_wext))
+            memcpy(priv->cfg_bssid, pmevent->event_buf, ETH_ALEN);
+#endif
+        priv->media_connected = MTRUE;
+        if (!netif_carrier_ok(priv->netdev))
+            netif_carrier_on(priv->netdev);
+        woal_wake_queue(priv->netdev);
+        break;
+
+    case MLAN_EVENT_ID_DRV_SCAN_REPORT:
+        PRINTM(MINFO, "Scan report\n");
+        if (priv->phandle->scan_pending_on_block == MTRUE) {
+            priv->phandle->scan_pending_on_block = MFALSE;
+            MOAL_REL_SEMAPHORE(&priv->phandle->async_sem);
+        }
+
+        if (priv->report_scan_result) {
+#ifdef STA_WEXT
+            if (IS_STA_WEXT(cfg80211_wext)) {
+                memset(&wrqu, 0, sizeof(union iwreq_data));
+                wireless_send_event(priv->netdev, SIOCGIWSCAN, &wrqu, NULL);
+            }
+#endif
+#ifdef STA_CFG80211
+            if (IS_STA_CFG80211(cfg80211_wext)) {
+                woal_inform_bss_from_scan_result(priv, NULL);
+                PRINTM(MINFO, "Reporting scan results\n");
+                if (priv->scan_request) {
+                    cfg80211_scan_done(priv->scan_request, MFALSE);
+                    priv->scan_request = NULL;
+                }
+            }
+#endif /* STA_CFG80211 */
+
+            priv->report_scan_result = MFALSE;
+        }
+        break;
+
+    case MLAN_EVENT_ID_DRV_OBSS_SCAN_PARAM:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext)) {
+            memset(&wrqu, 0, sizeof(union iwreq_data));
+            memmove((pmevent->event_buf + strlen(CUS_EVT_OBSS_SCAN_PARAM) + 1),
+                    pmevent->event_buf, pmevent->event_len);
+            memcpy(pmevent->event_buf, (t_u8 *) CUS_EVT_OBSS_SCAN_PARAM,
+                   strlen(CUS_EVT_OBSS_SCAN_PARAM));
+            pmevent->event_buf[strlen(CUS_EVT_OBSS_SCAN_PARAM)] = 0;
+
+            wrqu.data.pointer = pmevent->event_buf;
+            wrqu.data.length =
+                pmevent->event_len + strlen(CUS_EVT_OBSS_SCAN_PARAM) + 1;
+            wireless_send_event(priv->netdev, IWEVCUSTOM, &wrqu,
+                                pmevent->event_buf);
+        }
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_BW_CHANGED:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext)) {
+            memset(&wrqu, 0, sizeof(union iwreq_data));
+            memmove((pmevent->event_buf + strlen(CUS_EVT_BW_CHANGED) + 1),
+                    pmevent->event_buf, pmevent->event_len);
+            memcpy(pmevent->event_buf, (t_u8 *) CUS_EVT_BW_CHANGED,
+                   strlen(CUS_EVT_BW_CHANGED));
+            pmevent->event_buf[strlen(CUS_EVT_BW_CHANGED)] = 0;
+
+            wrqu.data.pointer = pmevent->event_buf;
+            wrqu.data.length =
+                pmevent->event_len + strlen(CUS_EVT_BW_CHANGED) + 1;
+            wireless_send_event(priv->netdev, IWEVCUSTOM, &wrqu,
+                                pmevent->event_buf);
+        }
+#endif
+        break;
+
+    case MLAN_EVENT_ID_FW_DISCONNECTED:
+        woal_send_disconnect_to_system(priv);
+#ifdef STA_CFG80211
+        if (IS_STA_CFG80211(cfg80211_wext)) {
+            if (!priv->cfg_disconnect &&
+                priv->wdev->iftype != NL80211_IFTYPE_ADHOC) {
+                PRINTM(MINFO, "Successfully disconnected from %pM:"
+                       " Reason code %d\n", priv->cfg_bssid,
+                       WLAN_REASON_DEAUTH_LEAVING);
+                /* This function must be called only when disconnect issued by
+                   the FW, i.e. disconnected by AP. For IBSS mode this call is
+                   not valid */
+                cfg80211_disconnected(priv->netdev,
+                                      WLAN_REASON_DEAUTH_LEAVING, NULL, 0,
+                                      GFP_KERNEL);
+            }
+            priv->cfg_disconnect = 0;
+        }
+#endif /* STA_CFG80211 */
+        /* Reset wireless stats signal info */
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext)) {
+            priv->w_stats.qual.level = 0;
+            priv->w_stats.qual.noise = 0;
+        }
+#endif
+
+#ifdef REASSOCIATION
+        if (priv->reassoc_on == MTRUE) {
+            PRINTM(MINFO, "Reassoc: trigger the timer\n");
+            priv->reassoc_required = MTRUE;
+            priv->phandle->is_reassoc_timer_set = MTRUE;
+            woal_mod_timer(&priv->phandle->reassoc_timer,
+                           REASSOC_TIMER_DEFAULT);
+        } else {
+            priv->rate_index = AUTO_RATE;
+        }
+#endif /* REASSOCIATION */
+        break;
+
+    case MLAN_EVENT_ID_FW_MIC_ERR_UNI:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext)) {
+#if WIRELESS_EXT >= 18
+            woal_send_mic_error_event(priv, MLAN_EVENT_ID_FW_MIC_ERR_UNI);
+#else
+            woal_send_iwevcustom_event(priv, CUS_EVT_MLME_MIC_ERR_UNI);
+#endif
+        }
+#endif /* STA_WEXT */
+#ifdef STA_CFG80211
+        if (IS_STA_CFG80211(cfg80211_wext)) {
+            cfg80211_michael_mic_failure(priv->netdev, priv->cfg_bssid,
+                                         NL80211_KEYTYPE_PAIRWISE, -1, NULL,
+                                         GFP_KERNEL);
+        }
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_MIC_ERR_MUL:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext)) {
+#if WIRELESS_EXT >= 18
+            woal_send_mic_error_event(priv, MLAN_EVENT_ID_FW_MIC_ERR_MUL);
+#else
+            woal_send_iwevcustom_event(priv, CUS_EVT_MLME_MIC_ERR_MUL);
+#endif
+        }
+#endif /* STA_WEXT */
+#ifdef STA_CFG80211
+        if (IS_STA_CFG80211(cfg80211_wext)) {
+            cfg80211_michael_mic_failure(priv->netdev, priv->cfg_bssid,
+                                         NL80211_KEYTYPE_GROUP, -1, NULL,
+                                         GFP_KERNEL);
+        }
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_BCN_RSSI_LOW:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_BEACON_RSSI_LOW);
+#endif
+#ifdef STA_CFG80211
+        if (IS_STA_CFG80211(cfg80211_wext)) {
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,35) || defined(COMPAT_WIRELESS)
+            cfg80211_cqm_rssi_notify(priv->netdev,
+                                     NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW,
+                                     GFP_KERNEL);
+            priv->rssi_status = MLAN_EVENT_ID_FW_BCN_RSSI_LOW;
+#endif
+            woal_set_rssi_threshold(priv, MLAN_EVENT_ID_FW_BCN_RSSI_LOW);
+        }
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_BCN_RSSI_HIGH:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_BEACON_RSSI_HIGH);
+#endif
+#ifdef STA_CFG80211
+        if (IS_STA_CFG80211(cfg80211_wext)) {
+            if (!priv->mrvl_rssi_low) {
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,35) || defined(COMPAT_WIRELESS)
+                cfg80211_cqm_rssi_notify(priv->netdev,
+                                         NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH,
+                                         GFP_KERNEL);
+#endif
+                woal_set_rssi_threshold(priv, MLAN_EVENT_ID_FW_BCN_RSSI_HIGH);
+            }
+        }
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_BCN_SNR_LOW:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_BEACON_SNR_LOW);
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_BCN_SNR_HIGH:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_BEACON_SNR_HIGH);
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_MAX_FAIL:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_MAX_FAIL);
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_DATA_RSSI_LOW:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_DATA_RSSI_LOW);
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_DATA_SNR_LOW:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_DATA_SNR_LOW);
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_DATA_RSSI_HIGH:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_DATA_RSSI_HIGH);
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_DATA_SNR_HIGH:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_DATA_SNR_HIGH);
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_LINK_QUALITY:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_LINK_QUALITY);
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_PORT_RELEASE:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_PORT_RELEASE);
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_PRE_BCN_LOST:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_PRE_BEACON_LOST);
+#endif
+#ifdef STA_CFG80211
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,35) || defined(COMPAT_WIRELESS)
+        if (IS_STA_CFG80211(cfg80211_wext)) {
+            struct cfg80211_bss *bss = NULL;
+            bss =
+                cfg80211_get_bss(priv->wdev->wiphy, NULL, priv->cfg_bssid, NULL,
+                                 0, WLAN_CAPABILITY_ESS, WLAN_CAPABILITY_ESS);
+            if (bss)
+                cfg80211_unlink_bss(priv->wdev->wiphy, bss);
+            cfg80211_cqm_rssi_notify(priv->netdev,
+                                     NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW,
+                                     GFP_KERNEL);
+            priv->rssi_status = MLAN_EVENT_ID_FW_PRE_BCN_LOST;
+        }
+#endif
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_WMM_CONFIG_CHANGE:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, WMM_CONFIG_CHANGE_INDICATION);
+#endif
+        break;
+
+    case MLAN_EVENT_ID_DRV_REPORT_STRING:
+        PRINTM(MINFO, "Report string %s\n", pmevent->event_buf);
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, pmevent->event_buf);
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_WEP_ICV_ERR:
+        DBG_HEXDUMP(MCMD_D, "WEP ICV error", pmevent->event_buf,
+                    pmevent->event_len);
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_WEP_ICV_ERR);
+#endif
+        break;
+
+    case MLAN_EVENT_ID_DRV_DEFER_HANDLING:
+        queue_work(priv->phandle->workqueue, &priv->phandle->main_work);
+        break;
+    case MLAN_EVENT_ID_DRV_DBG_DUMP:
+        woal_moal_debug_info(priv, NULL, MFALSE);
+        break;
+    case MLAN_EVENT_ID_FW_BG_SCAN:
+        if (priv->media_connected == MTRUE)
+            priv->bg_scan_start = MFALSE;
+        priv->bg_scan_reported = MTRUE;
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext)) {
+            memset(&wrqu, 0, sizeof(union iwreq_data));
+            wireless_send_event(priv->netdev, SIOCGIWSCAN, &wrqu, NULL);
+        }
+#endif
+#ifdef STA_CFG80211
+        if (IS_STA_CFG80211(cfg80211_wext)) {
+            woal_inform_bss_from_scan_result(priv, NULL);
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,35) || defined(COMPAT_WIRELESS)
+            if (priv->mrvl_rssi_low) {
+                cfg80211_cqm_rssi_notify(priv->netdev,
+                                         NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH,
+                                         GFP_KERNEL);
+            }
+#endif
+        }
+#endif
+        break;
+    case MLAN_EVENT_ID_FW_CHANNEL_SWITCH_ANN:
+#ifdef STA_WEXT
+        if (IS_STA_WEXT(cfg80211_wext))
+            woal_send_iwevcustom_event(priv, CUS_EVT_CHANNEL_SWITCH_ANN);
+#endif
+        break;
+#endif /* STA_SUPPORT */
+    case MLAN_EVENT_ID_FW_STOP_TX:
+        woal_stop_queue(priv->netdev);
+        if (netif_carrier_ok(priv->netdev))
+            netif_carrier_off(priv->netdev);
+        break;
+    case MLAN_EVENT_ID_FW_START_TX:
+        if (!netif_carrier_ok(priv->netdev))
+            netif_carrier_on(priv->netdev);
+        woal_wake_queue(priv->netdev);
+        break;
+    case MLAN_EVENT_ID_FW_HS_WAKEUP:
+        /* simulate HSCFG_CANCEL command */
+        woal_cancel_hs(priv, MOAL_NO_WAIT);
+#ifdef STA_WEXT
+#ifdef STA_SUPPORT
+        if (IS_STA_WEXT(cfg80211_wext) && (pmpriv = woal_get_priv((moal_handle
+                                                                   *)
+                                                                  pmoal_handle,
+                                                                  MLAN_BSS_ROLE_STA)))
+            woal_send_iwevcustom_event(pmpriv, CUS_EVT_HS_WAKEUP);
+#endif /* STA_SUPPORT */
+#endif /* STA_WEXT */
+#ifdef UAP_SUPPORT
+        if ((pmpriv =
+             woal_get_priv((moal_handle *) pmoal_handle, MLAN_BSS_ROLE_UAP))) {
+            pmevent->event_id = UAP_EVENT_ID_HS_WAKEUP;
+            woal_broadcast_event(pmpriv, (t_u8 *) & pmevent->event_id,
+                                 sizeof(t_u32));
+        }
+#endif /* UAP_SUPPORT */
+        break;
+    case MLAN_EVENT_ID_DRV_HS_ACTIVATED:
+#ifdef STA_WEXT
+#ifdef STA_SUPPORT
+        if (IS_STA_WEXT(cfg80211_wext) && (pmpriv = woal_get_priv((moal_handle
+                                                                   *)
+                                                                  pmoal_handle,
+                                                                  MLAN_BSS_ROLE_STA)))
+        {
+            woal_send_iwevcustom_event(pmpriv, CUS_EVT_HS_ACTIVATED);
+        }
+#endif /* STA_SUPPORT */
+#endif /* STA_WEXT */
+#if defined(UAP_SUPPORT)
+        if ((pmpriv =
+             woal_get_priv((moal_handle *) pmoal_handle, MLAN_BSS_ROLE_UAP))) {
+            pmevent->event_id = UAP_EVENT_ID_DRV_HS_ACTIVATED;
+            woal_broadcast_event(pmpriv, (t_u8 *) & pmevent->event_id,
+                                 sizeof(t_u32));
+        }
+#endif
+#if defined(SDIO_SUSPEND_RESUME)
+        if (priv->phandle->suspend_fail == MFALSE) {
+            woal_get_pm_info(priv, &pm_info);
+            if (pm_info.is_suspend_allowed == MTRUE) {
+                priv->phandle->hs_activated = MTRUE;
+#ifdef MMC_PM_FUNC_SUSPENDED
+                woal_wlan_is_suspended(priv->phandle);
+#endif
+            }
+            priv->phandle->hs_activate_wait_q_woken = MTRUE;
+            wake_up_interruptible(&priv->phandle->hs_activate_wait_q);
+        }
+#endif
+        break;
+    case MLAN_EVENT_ID_DRV_HS_DEACTIVATED:
+#ifdef STA_WEXT
+#ifdef STA_SUPPORT
+        if (IS_STA_WEXT(cfg80211_wext) && (pmpriv = woal_get_priv((moal_handle
+                                                                   *)
+                                                                  pmoal_handle,
+                                                                  MLAN_BSS_ROLE_STA)))
+        {
+            woal_send_iwevcustom_event(pmpriv, CUS_EVT_HS_DEACTIVATED);
+        }
+#endif
+#endif
+#if defined(UAP_SUPPORT)
+        if ((pmpriv =
+             woal_get_priv((moal_handle *) pmoal_handle, MLAN_BSS_ROLE_UAP))) {
+            pmevent->event_id = UAP_EVENT_ID_DRV_HS_DEACTIVATED;
+            woal_broadcast_event(pmpriv, (t_u8 *) & pmevent->event_id,
+                                 sizeof(t_u32));
+        }
+#endif
+#if defined(SDIO_SUSPEND_RESUME)
+        priv->phandle->hs_activated = MFALSE;
+#endif
+        break;
+#ifdef UAP_SUPPORT
+    case MLAN_EVENT_ID_UAP_FW_BSS_START:
+        priv->bss_started = MTRUE;
+        memcpy(priv->current_addr, pmevent->event_buf + 6, ETH_ALEN);
+        memcpy(priv->netdev->dev_addr, priv->current_addr, ETH_ALEN);
+        woal_broadcast_event(priv, pmevent->event_buf, pmevent->event_len);
+        break;
+    case MLAN_EVENT_ID_UAP_FW_BSS_ACTIVE:
+        priv->media_connected = MTRUE;
+        if (!netif_carrier_ok(priv->netdev))
+            netif_carrier_on(priv->netdev);
+        woal_wake_queue(priv->netdev);
+        woal_broadcast_event(priv, pmevent->event_buf, pmevent->event_len);
+        break;
+    case MLAN_EVENT_ID_UAP_FW_BSS_IDLE:
+        priv->media_connected = MFALSE;
+        woal_stop_queue(priv->netdev);
+        if (netif_carrier_ok(priv->netdev))
+            netif_carrier_off(priv->netdev);
+        woal_broadcast_event(priv, pmevent->event_buf, pmevent->event_len);
+        break;
+#ifdef WIFI_DIRECT_SUPPORT
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+    case MLAN_EVENT_ID_FW_REMAIN_ON_CHAN_EXPIRED:
+        if (IS_STA_OR_UAP_CFG80211(cfg80211_wext)) {
+            PRINTM(MEVENT, "FW_REMAIN_ON_CH0ANNEL_EXPIRED cookie = %#llx\n",
+                   priv->phandle->cookie);
+            priv->phandle->remain_on_channel = MFALSE;
+            if (priv->phandle->cookie) {
+                cfg80211_remain_on_channel_expired(priv->netdev,
+                                                   priv->phandle->cookie,
+                                                   &priv->phandle->chan,
+                                                   priv->phandle->channel_type,
+                                                   GFP_ATOMIC);
+                priv->phandle->cookie = 0;
+            }
+        }
+        break;
+#endif
+#endif
+    case MLAN_EVENT_ID_UAP_FW_STA_CONNECT:
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+        if (IS_STA_OR_UAP_CFG80211(cfg80211_wext)) {
+            struct station_info sinfo;
+            t_u8 addr[ETH_ALEN];
+
+            sinfo.filled = 0;
+            sinfo.generation = 0;
+            /* copy the station mac address */
+            memset(addr, 0xFF, ETH_ALEN);
+            memcpy(addr, pmevent->event_buf, ETH_ALEN);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0) || defined(COMPAT_WIRELESS)
+            if (pmevent->event_len > ETH_ALEN) {
+                /* set station info filled flag */
+                sinfo.filled |= STATION_INFO_ASSOC_REQ_IES;
+                /* get the assoc request ies and length */
+                sinfo.assoc_req_ies =
+                    (const t_u8 *) (pmevent->event_buf + ETH_ALEN);
+                sinfo.assoc_req_ies_len = pmevent->event_len - ETH_ALEN;
+
+            }
+#endif /* KERNEL_VERSION */
+            if (priv->netdev)
+                cfg80211_new_sta(priv->netdev,
+                                 (t_u8 *) addr, &sinfo, GFP_KERNEL);
+        }
+#endif /* UAP_CFG80211 */
+#ifdef UAP_WEXT
+        if (IS_UAP_WEXT(cfg80211_wext)) {
+            memset(&wrqu, 0, sizeof(union iwreq_data));
+            memmove((pmevent->event_buf + strlen(CUS_EVT_STA_CONNECTED) + 1),
+                    pmevent->event_buf, pmevent->event_len);
+            memcpy(pmevent->event_buf, (t_u8 *) CUS_EVT_STA_CONNECTED,
+                   strlen(CUS_EVT_STA_CONNECTED));
+            pmevent->event_buf[strlen(CUS_EVT_STA_CONNECTED)] = 0;
+            wrqu.data.pointer = pmevent->event_buf;
+            if ((pmevent->event_len + strlen(CUS_EVT_STA_CONNECTED) + 1) >
+                IW_CUSTOM_MAX)
+                wrqu.data.length = ETH_ALEN + strlen(CUS_EVT_STA_CONNECTED) + 1;
+            else
+                wrqu.data.length =
+                    pmevent->event_len + strlen(CUS_EVT_STA_CONNECTED) + 1;
+            wireless_send_event(priv->netdev, IWEVCUSTOM, &wrqu,
+                                pmevent->event_buf);
+        }
+#endif /* UAP_WEXT */
+        break;
+    case MLAN_EVENT_ID_UAP_FW_STA_DISCONNECT:
+#ifdef UAP_CFG80211
+        if (IS_UAP_CFG80211(cfg80211_wext)) {
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,0,0) || defined(COMPAT_WIRELESS)
+            /* skip 2 bytes extra header will get the mac address */
+            cfg80211_del_sta(priv->netdev, pmevent->event_buf + 2, GFP_KERNEL);
+#endif /* KERNEL_VERSION */
+        }
+#endif /* UAP_CFG80211 */
+#ifdef UAP_WEXT
+        if (IS_UAP_WEXT(cfg80211_wext)) {
+            memset(&wrqu, 0, sizeof(union iwreq_data));
+            memmove((pmevent->event_buf + strlen(CUS_EVT_STA_DISCONNECTED) + 1),
+                    pmevent->event_buf, pmevent->event_len);
+            memcpy(pmevent->event_buf, (t_u8 *) CUS_EVT_STA_DISCONNECTED,
+                   strlen(CUS_EVT_STA_DISCONNECTED));
+            pmevent->event_buf[strlen(CUS_EVT_STA_DISCONNECTED)] = 0;
+
+            wrqu.data.pointer = pmevent->event_buf;
+            wrqu.data.length =
+                pmevent->event_len + strlen(CUS_EVT_STA_DISCONNECTED) + 1;
+            wireless_send_event(priv->netdev, IWEVCUSTOM, &wrqu,
+                                pmevent->event_buf);
+        }
+#endif /* UAP_WEXT */
+        break;
+    case MLAN_EVENT_ID_DRV_MGMT_FRAME:
+#ifdef UAP_WEXT
+        if (IS_UAP_WEXT(cfg80211_wext)) {
+            woal_broadcast_event(priv, pmevent->event_buf, pmevent->event_len);
+        }
+#endif /* UAP_WEXT */
+#ifdef WIFI_DIRECT_SUPPORT
+#if defined(STA_CFG80211) || defined(UAP_CFG80211)
+        if (IS_STA_OR_UAP_CFG80211(cfg80211_wext)) {
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+            if (priv->netdev && priv->netdev->ieee80211_ptr->wiphy->mgmt_stypes) {
+                /* frmctl + durationid + addr1 + addr2 + addr3 + seqctl */
+#define PACKET_ADDR4_POS        (2 + 2 + 6 + 6 + 6 + 2)
+                t_u8 *pkt;
+                pkt = ((t_u8 *) pmevent->event_buf + sizeof(pmevent->event_id));
+
+                /* move addr4 */
+                memmove(pkt + PACKET_ADDR4_POS,
+                        pkt + PACKET_ADDR4_POS + ETH_ALEN,
+                        pmevent->event_len - sizeof(pmevent->event_id)
+                        - PACKET_ADDR4_POS - ETH_ALEN);
+                cfg80211_rx_mgmt(priv->netdev, priv->phandle->chan.center_freq,
+                                 ((const t_u8 *) pmevent->event_buf)
+                                 + sizeof(pmevent->event_id),
+                                 pmevent->event_len - sizeof(pmevent->event_id)
+                                 - MLAN_MAC_ADDR_LENGTH, GFP_ATOMIC);
+            }
+#endif /* KERNEL_VERSION */
+        }
+#endif /* STA_CFG80211 || UAP_CFG80211 */
+#endif /* WIFI_DIRECT_SUPPORT */
+        break;
+#endif /* UAP_SUPPORT */
+    case MLAN_EVENT_ID_DRV_PASSTHRU:
+        woal_broadcast_event(priv, pmevent->event_buf, pmevent->event_len);
+        break;
+    case MLAN_EVENT_ID_DRV_MEAS_REPORT:
+        /* We have received measurement report, wakeup measurement wait queue */
+        PRINTM(MINFO, "Measurement Report\n");
+        /* Going out of CAC checking period */
+        if (priv->phandle->cac_period == MTRUE) {
+            priv->phandle->cac_period = MFALSE;
+            if (priv->phandle->meas_wait_q_woken == MFALSE) {
+                priv->phandle->meas_wait_q_woken = MTRUE;
+                wake_up_interruptible(&priv->phandle->meas_wait_q);
+            }
+
+            /* Execute delayed BSS START command */
+            if (priv->phandle->delay_bss_start == MTRUE) {
+                mlan_ioctl_req *req = NULL;
+                mlan_ds_bss *bss = NULL;
+
+                /* Clear flag */
+                priv->phandle->delay_bss_start = MFALSE;
+
+                PRINTM(MMSG, "Now CAC measure period end. "
+                       "Execute delayed BSS Start command.\n");
+
+                req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+                bss = (mlan_ds_bss *) req->pbuf;
+                req->req_id = MLAN_IOCTL_BSS;
+                req->action = MLAN_ACT_SET;
+                bss->sub_command = MLAN_OID_BSS_START;
+                memcpy(&bss->param.ssid_bssid,
+                       &priv->phandle->delay_ssid_bssid,
+                       sizeof(mlan_ssid_bssid));
+
+                if (woal_request_ioctl(priv, req, MOAL_NO_WAIT)
+                    != MLAN_STATUS_PENDING) {
+                    PRINTM(MMSG, "Delayed BSS Start operation failed!\n");
+                    kfree(req);
+                }
+
+                PRINTM(MMSG, "BSS START Complete!\n");
+            }
+        }
+    default:
+        break;
+    }
+  done:
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ *  @brief This function prints the debug message in mlan
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param level	debug level
+ *  @param pformat	point to string format buf
+ *
+ *  @return    		N/A
+ */
+t_void
+moal_print(IN t_void * pmoal_handle, IN t_u32 level, IN t_s8 * pformat, IN ...)
+{
+#ifdef	DEBUG_LEVEL1
+    va_list args;
+
+    if (level & MHEX_DUMP) {
+        t_u8 *buf = NULL;
+        int len = 0;
+
+        va_start(args, pformat);
+        buf = (t_u8 *) va_arg(args, t_u8 *);
+        len = (int) va_arg(args, int);
+        va_end(args);
+
+#ifdef DEBUG_LEVEL2
+        if (level & MINFO)
+            HEXDUMP((char *) pformat, buf, len);
+        else
+#endif /* DEBUG_LEVEL2 */
+        {
+            if (level & MERROR)
+                DBG_HEXDUMP(MERROR, (char *) pformat, buf, len);
+            if (level & MCMD_D)
+                DBG_HEXDUMP(MCMD_D, (char *) pformat, buf, len);
+            if (level & MDAT_D)
+                DBG_HEXDUMP(MDAT_D, (char *) pformat, buf, len);
+            if (level & MIF_D)
+                DBG_HEXDUMP(MIF_D, (char *) pformat, buf, len);
+            if (level & MFW_D)
+                DBG_HEXDUMP(MFW_D, (char *) pformat, buf, len);
+            if (level & MEVT_D)
+                DBG_HEXDUMP(MEVT_D, (char *) pformat, buf, len);
+        }
+    } else {
+        va_start(args, pformat);
+        vprintk(pformat, args);
+        va_end(args);
+    }
+#endif /* DEBUG_LEVEL1 */
+}
+
+/**
+ *  @brief This function prints the network interface name
+ *
+ *  @param pmoal_handle Pointer to the MOAL context
+ *  @param bss_index    BSS index
+ *  @param level        debug level
+ *
+ *  @return            N/A
+ */
+t_void
+moal_print_netintf(IN t_void * pmoal_handle, IN t_u32 bss_index, IN t_u32 level)
+{
+#ifdef DEBUG_LEVEL1
+    moal_handle *phandle = (moal_handle *) pmoal_handle;
+
+    if (phandle && (drvdbg & level)) {
+        if ((bss_index < MLAN_MAX_BSS_NUM) && phandle->priv[bss_index] &&
+            phandle->priv[bss_index]->netdev)
+            printk("%s: ", phandle->priv[bss_index]->netdev->name);
+    }
+#endif /* DEBUG_LEVEL1 */
+}
+
+/**
+ *  @brief This function asserts the existence of the passed argument
+ *
+ *  @param pmoal_handle     A pointer to moal_private structure
+ *  @param cond             Condition to check
+ *
+ *  @return    		        N/A
+ */
+t_void
+moal_assert(IN t_void * pmoal_handle, IN t_u32 cond)
+{
+    if (!cond)
+        panic("Assert failed: Panic!");
+}
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_shim.h b/drivers/net/wireless/sd8797/mlinux/moal_shim.h
new file mode 100644
index 000000000000..ee50a2105f0f
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_shim.h
@@ -0,0 +1,95 @@
+/** @file moal_shim.h
+  *
+  * @brief This file contains declaration referring to
+  * functions defined in moal module
+  *
+  * Copyright (C) 2008-2011, Marvell International Ltd. 
+  * 
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+/*************************************************************
+Change Log:
+    10/21/2008: initial version
+************************************************************/
+
+#ifndef _MOAL_H
+#define _MOAL_H
+
+mlan_status moal_get_fw_data(IN t_void * pmoal_handle,
+                             IN t_u32 offset, IN t_u32 len, OUT t_u8 * pbuf);
+mlan_status moal_init_fw_complete(IN t_void * pmoal_handle,
+                                  IN mlan_status status);
+mlan_status moal_shutdown_fw_complete(IN t_void * pmoal_handle,
+                                      IN mlan_status status);
+mlan_status moal_ioctl_complete(IN t_void * pmoal_handle,
+                                IN pmlan_ioctl_req pioctl_req,
+                                IN mlan_status status);
+mlan_status moal_alloc_mlan_buffer(IN t_void * pmoal_handle, IN t_u32 size,
+                                   OUT pmlan_buffer * pmbuf);
+mlan_status moal_free_mlan_buffer(IN t_void * pmoal_handle,
+                                  IN pmlan_buffer pmbuf);
+mlan_status moal_send_packet_complete(IN t_void * pmoal_handle,
+                                      IN pmlan_buffer pmbuf,
+                                      IN mlan_status status);
+/** moal_write_reg */
+mlan_status moal_write_reg(IN t_void * pmoal_handle,
+                           IN t_u32 reg, IN t_u32 data);
+/** moal_read_reg */
+mlan_status moal_read_reg(IN t_void * pmoal_handle,
+                          IN t_u32 reg, OUT t_u32 * data);
+mlan_status moal_write_data_sync(IN t_void * pmoal_handle,
+                                 IN pmlan_buffer pmbuf,
+                                 IN t_u32 port, IN t_u32 timeout);
+mlan_status moal_read_data_sync(IN t_void * pmoal_handle,
+                                IN OUT pmlan_buffer pmbuf,
+                                IN t_u32 port, IN t_u32 timeout);
+mlan_status moal_recv_packet(IN t_void * pmoal_handle, IN pmlan_buffer pmbuf);
+mlan_status moal_recv_event(IN t_void * pmoal_handle, IN pmlan_event pmevent);
+mlan_status moal_malloc(IN t_void * pmoal_handle,
+                        IN t_u32 size, IN t_u32 flag, OUT t_u8 ** ppbuf);
+mlan_status moal_mfree(IN t_void * pmoal_handle, IN t_u8 * pbuf);
+t_void *moal_memset(IN t_void * pmoal_handle,
+                    IN t_void * pmem, IN t_u8 byte, IN t_u32 num);
+t_void *moal_memcpy(IN t_void * pmoal_handle,
+                    IN t_void * pdest, IN const t_void * psrc, IN t_u32 num);
+t_void *moal_memmove(IN t_void * pmoal_handle,
+                     IN t_void * pdest, IN const t_void * psrc, IN t_u32 num);
+t_s32 moal_memcmp(IN t_void * pmoal_handle,
+                  IN const t_void * pmem1,
+                  IN const t_void * pmem2, IN t_u32 num);
+/** moal_udelay */
+t_void moal_udelay(IN t_void * pmoal_handle, IN t_u32 udelay);
+mlan_status moal_get_system_time(IN t_void * pmoal_handle, OUT t_u32 * psec,
+                                 OUT t_u32 * pusec);
+mlan_status moal_init_lock(IN t_void * pmoal_handle, OUT t_void ** pplock);
+mlan_status moal_free_lock(IN t_void * pmoal_handle, IN t_void * plock);
+mlan_status moal_spin_lock(IN t_void * pmoal_handle, IN t_void * plock);
+mlan_status moal_spin_unlock(IN t_void * pmoal_handle, IN t_void * plock);
+t_void moal_print(IN t_void * pmoal_handle, IN t_u32 level, IN t_s8 * pformat,
+                  IN ...);
+t_void moal_print_netintf(IN t_void * pmoal_handle, IN t_u32 bss_index,
+                          IN t_u32 level);
+t_void moal_assert(IN t_void * pmoal_handle, IN t_u32 cond);
+mlan_status moal_init_timer(IN t_void * pmoal_handle,
+                            OUT t_void ** pptimer,
+                            IN t_void(*callback) (t_void * pcontext),
+                            IN t_void * pcontext);
+mlan_status moal_free_timer(IN t_void * pmoal_handle, IN t_void * ptimer);
+mlan_status moal_start_timer(IN t_void * pmoal_handle,
+                             IN t_void * ptimer,
+                             IN t_u8 periodic, IN t_u32 msec);
+mlan_status moal_stop_timer(IN t_void * pmoal_handle, IN t_void * ptimer);
+
+#endif /*_MOAL_H */
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_sta_cfg80211.c b/drivers/net/wireless/sd8797/mlinux/moal_sta_cfg80211.c
new file mode 100644
index 000000000000..8a60be2298c8
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_sta_cfg80211.c
@@ -0,0 +1,2520 @@
+/** @file moal_sta_cfg80211.c
+  *
+  * @brief This file contains the functions for STA CFG80211. 
+  *
+  * Copyright (C) 2011-2012, Marvell International Ltd. 
+  * 
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+
+#include "moal_cfg80211.h"
+#include "moal_sta_cfg80211.h"
+static int woal_cfg80211_reg_notifier(struct wiphy *wiphy,
+                                      struct regulatory_request *request);
+
+static int woal_cfg80211_scan(struct wiphy *wiphy, struct net_device *dev,
+                              struct cfg80211_scan_request *request);
+
+static int woal_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
+                                 struct cfg80211_connect_params *sme);
+
+static int woal_cfg80211_disconnect(struct wiphy *wiphy,
+                                    struct net_device *dev, t_u16 reason_code);
+
+static int woal_cfg80211_get_station(struct wiphy *wiphy,
+                                     struct net_device *dev,
+                                     t_u8 * mac, struct station_info *sinfo);
+
+static int woal_cfg80211_dump_station(struct wiphy *wiphy,
+                                      struct net_device *dev, int idx,
+                                      t_u8 * mac, struct station_info *sinfo);
+
+static int woal_cfg80211_set_power_mgmt(struct wiphy *wiphy,
+                                        struct net_device *dev, bool enabled,
+                                        int timeout);
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,35) || defined(COMPAT_WIRELESS)
+static int woal_cfg80211_set_cqm_rssi_config(struct wiphy *wiphy,
+                                             struct net_device *dev,
+                                             s32 rssi_thold, u32 rssi_hyst);
+#endif
+
+static int woal_cfg80211_set_tx_power(struct wiphy *wiphy,
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,36) && !defined(COMPAT_WIRELESS)
+                                      enum tx_power_setting type,
+#else
+                                      enum nl80211_tx_power_setting type,
+#endif
+                                      int dbm);
+
+static int woal_cfg80211_join_ibss(struct wiphy *wiphy,
+                                   struct net_device *dev,
+                                   struct cfg80211_ibss_params *params);
+
+static int woal_cfg80211_leave_ibss(struct wiphy *wiphy,
+                                    struct net_device *dev);
+
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+static int woal_cfg80211_mgmt_tx_cancel_wait(struct wiphy *wiphy,
+                                             struct net_device *dev,
+                                             u64 cookie);
+
+void woal_cfg80211_remain_on_channel_done(void *context);
+
+static int woal_cfg80211_remain_on_channel(struct wiphy *wiphy,
+                                           struct net_device *dev,
+                                           struct ieee80211_channel *chan,
+                                           enum nl80211_channel_type
+                                           channel_type, unsigned int duration,
+                                           u64 * cookie);
+
+static int woal_cfg80211_cancel_remain_on_channel(struct wiphy *wiphy,
+                                                  struct net_device *dev,
+                                                  u64 cookie);
+#endif /* KERNEL_VERSION */
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+
+/** cfg80211 STA operations */
+static struct cfg80211_ops woal_cfg80211_sta_ops = {
+    .change_virtual_intf = woal_cfg80211_change_virtual_intf,
+    .scan = woal_cfg80211_scan,
+    .connect = woal_cfg80211_connect,
+    .disconnect = woal_cfg80211_disconnect,
+    .get_station = woal_cfg80211_get_station,
+    .dump_station = woal_cfg80211_dump_station,
+    .set_wiphy_params = woal_cfg80211_set_wiphy_params,
+    .set_channel = woal_cfg80211_set_channel,
+    .join_ibss = woal_cfg80211_join_ibss,
+    .leave_ibss = woal_cfg80211_leave_ibss,
+    .add_key = woal_cfg80211_add_key,
+    .del_key = woal_cfg80211_del_key,
+    .set_default_key = woal_cfg80211_set_default_key,
+    .set_power_mgmt = woal_cfg80211_set_power_mgmt,
+    .set_tx_power = woal_cfg80211_set_tx_power,
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,35) || defined(COMPAT_WIRELESS)
+    .set_cqm_rssi_config = woal_cfg80211_set_cqm_rssi_config,
+#endif
+};
+
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+/** cfg80211 Wifi Direct operations */
+static struct cfg80211_ops woal_cfg80211_wifi_direct_ops = {
+    .add_virtual_intf = woal_cfg80211_add_virtual_intf,
+    .del_virtual_intf = woal_cfg80211_del_virtual_intf,
+    .change_virtual_intf = woal_cfg80211_change_virtual_intf,
+    .scan = woal_cfg80211_scan,
+    .connect = woal_cfg80211_connect,
+    .disconnect = woal_cfg80211_disconnect,
+    .get_station = woal_cfg80211_get_station,
+    .dump_station = woal_cfg80211_dump_station,
+    .set_wiphy_params = woal_cfg80211_set_wiphy_params,
+    .set_channel = woal_cfg80211_set_channel,
+    .add_key = woal_cfg80211_add_key,
+    .del_key = woal_cfg80211_del_key,
+    .add_beacon = woal_cfg80211_add_beacon,
+    .set_beacon = woal_cfg80211_set_beacon,
+    .del_beacon = woal_cfg80211_del_beacon,
+    .mgmt_frame_register = woal_cfg80211_mgmt_frame_register,
+    .mgmt_tx = woal_cfg80211_mgmt_tx,
+    .mgmt_tx_cancel_wait = woal_cfg80211_mgmt_tx_cancel_wait,
+    .remain_on_channel = woal_cfg80211_remain_on_channel,
+    .cancel_remain_on_channel = woal_cfg80211_cancel_remain_on_channel,
+    .set_default_key = woal_cfg80211_set_default_key,
+    .set_power_mgmt = woal_cfg80211_set_power_mgmt,
+    .set_tx_power = woal_cfg80211_set_tx_power,
+};
+#endif /* KERNEL_VERSION */
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+
+/********************************************************
+				Local Variables
+********************************************************/
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+static const struct ieee80211_txrx_stypes
+    ieee80211_mgmt_stypes[NUM_NL80211_IFTYPES] = {
+    [NL80211_IFTYPE_ADHOC] = {
+                              .tx = 0x0000,
+                              .rx = 0x0000,
+                              },
+    [NL80211_IFTYPE_STATION] = {
+                                .tx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+                                BIT(IEEE80211_STYPE_PROBE_RESP >> 4),
+                                .rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+                                BIT(IEEE80211_STYPE_PROBE_REQ >> 4),
+                                },
+    [NL80211_IFTYPE_AP] = {
+                           .tx = 0xffff,
+                           .rx = BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
+                           BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
+                           BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
+                           BIT(IEEE80211_STYPE_DISASSOC >> 4) |
+                           BIT(IEEE80211_STYPE_AUTH >> 4) |
+                           BIT(IEEE80211_STYPE_DEAUTH >> 4) |
+                           BIT(IEEE80211_STYPE_ACTION >> 4),
+                           },
+    [NL80211_IFTYPE_AP_VLAN] = {
+                                .tx = 0x0000,
+                                .rx = 0x0000,
+                                },
+    [NL80211_IFTYPE_P2P_CLIENT] = {
+                                   .tx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+                                   BIT(IEEE80211_STYPE_PROBE_RESP >> 4),
+                                   .rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+                                   BIT(IEEE80211_STYPE_PROBE_REQ >> 4),
+                                   },
+    [NL80211_IFTYPE_P2P_GO] = {
+                               .tx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+                               // BIT(IEEE80211_STYPE_PROBE_RESP >> 4) |
+                               0,
+                               .rx = BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
+                               BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
+                               BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
+                               BIT(IEEE80211_STYPE_DISASSOC >> 4) |
+                               BIT(IEEE80211_STYPE_AUTH >> 4) |
+                               BIT(IEEE80211_STYPE_DEAUTH >> 4) |
+                               BIT(IEEE80211_STYPE_ACTION >> 4),
+                               },
+    [NL80211_IFTYPE_MESH_POINT] = {
+                                   .tx = 0x0000,
+                                   .rx = 0x0000,
+                                   },
+
+};
+#endif
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+
+/********************************************************
+				Global Variables
+********************************************************/
+
+/********************************************************
+				Local Functions
+********************************************************/
+/**
+ * @brief Get the encryption mode from cipher
+ *
+ * @param cipher        Cipher cuite
+ * @param wpa_enabled   WPA enable or disable
+ *
+ * @return              MLAN_ENCRYPTION_MODE_*
+ */
+static int
+woal_cfg80211_get_encryption_mode(t_u32 cipher, int *wpa_enabled)
+{
+    int encrypt_mode;
+
+    ENTER();
+
+    *wpa_enabled = 0;
+    switch (cipher) {
+    case IW_AUTH_CIPHER_NONE:
+        encrypt_mode = MLAN_ENCRYPTION_MODE_NONE;
+        break;
+    case WLAN_CIPHER_SUITE_WEP40:
+        encrypt_mode = MLAN_ENCRYPTION_MODE_WEP40;
+        break;
+    case WLAN_CIPHER_SUITE_WEP104:
+        encrypt_mode = MLAN_ENCRYPTION_MODE_WEP104;
+        break;
+    case WLAN_CIPHER_SUITE_TKIP:
+        encrypt_mode = MLAN_ENCRYPTION_MODE_TKIP;
+        *wpa_enabled = 1;
+        break;
+    case WLAN_CIPHER_SUITE_CCMP:
+        encrypt_mode = MLAN_ENCRYPTION_MODE_CCMP;
+        *wpa_enabled = 1;
+        break;
+    default:
+        encrypt_mode = -1;
+    }
+
+    LEAVE();
+    return encrypt_mode;
+}
+
+/**
+ *  @brief Check the pairwise or group cipher for
+ *  WEP enabled or not
+ *
+ *  @param cipher       MLAN Cipher cuite
+ *
+ *  @return             1 -- enable or 0 -- disable
+ */
+static int
+woal_cfg80211_is_alg_wep(t_u32 cipher)
+{
+    int alg = 0;
+    ENTER();
+
+    if (cipher == MLAN_ENCRYPTION_MODE_WEP40 ||
+        cipher == MLAN_ENCRYPTION_MODE_WEP104)
+        alg = 1;
+
+    LEAVE();
+    return alg;
+}
+
+/**
+ *  @brief Convert NL802.11 channel type into driver channel type
+ *
+ * The mapping is as follows -
+ *      NL80211_CHAN_NO_HT     -> NO_SEC_CHANNEL
+ *      NL80211_CHAN_HT20      -> NO_SEC_CHANNEL
+ *      NL80211_CHAN_HT40PLUS  -> SEC_CHANNEL_ABOVE
+ *      NL80211_CHAN_HT40MINUS -> SEC_CHANNEL_BELOW
+ *      Others                 -> NO_SEC_CHANNEL
+ *
+ *  @param channel_type     Channel type
+ *
+ *  @return                 Driver channel type
+ */
+static int
+woal_cfg80211_channel_type_to_channel(enum nl80211_channel_type channel_type)
+{
+    int channel;
+
+    ENTER();
+
+    switch (channel_type) {
+    case NL80211_CHAN_NO_HT:
+    case NL80211_CHAN_HT20:
+        channel = NO_SEC_CHANNEL;
+        break;
+    case NL80211_CHAN_HT40PLUS:
+        channel = SEC_CHANNEL_ABOVE;
+        break;
+    case NL80211_CHAN_HT40MINUS:
+        channel = SEC_CHANNEL_BELOW;
+        break;
+    default:
+        channel = NO_SEC_CHANNEL;
+    }
+    LEAVE();
+    return channel;
+}
+
+/**
+ *  @brief Convert secondary channel type to NL80211 channel type
+ *
+ * The mapping is as follows -
+ *      NO_SEC_CHANNEL      -> NL80211_CHAN_HT20
+ *      SEC_CHANNEL_ABOVE   -> NL80211_CHAN_HT40PLUS
+ *      SEC_CHANNEL_BELOW   -> NL80211_CHAN_HT40MINUS
+ *      Others              -> NL80211_CHAN_HT20
+ *
+ *  @param channel_type     Driver channel type
+ *
+ *  @return                 nl80211_channel_type type
+ */
+static enum nl80211_channel_type
+woal_channel_to_nl80211_channel_type(int channel_type)
+{
+    enum nl80211_channel_type channel;
+
+    ENTER();
+
+    switch (channel_type) {
+    case NO_SEC_CHANNEL:
+        channel = NL80211_CHAN_HT20;
+        break;
+    case SEC_CHANNEL_ABOVE:
+        channel = NL80211_CHAN_HT40PLUS;
+        break;
+    case SEC_CHANNEL_BELOW:
+        channel = NL80211_CHAN_HT40MINUS;
+        break;
+    default:
+        channel = NL80211_CHAN_HT20;
+    }
+    LEAVE();
+    return channel;
+}
+
+/**
+ *  @brief Convert driver band configuration to IEEE band type
+ *
+ *  @param band     Driver band configuration
+ *
+ *  @return         IEEE band type
+ */
+t_u8
+woal_band_cfg_to_ieee_band(t_u32 band)
+{
+    t_u8 ret_radio_type;
+
+    ENTER();
+
+    switch (band) {
+    case BAND_A:
+    case BAND_AN:
+    case BAND_A | BAND_AN:
+        ret_radio_type = IEEE80211_BAND_5GHZ;
+        break;
+    case BAND_B:
+    case BAND_G:
+    case BAND_B | BAND_G:
+    case BAND_GN:
+    case BAND_B | BAND_GN:
+    default:
+        ret_radio_type = IEEE80211_BAND_2GHZ;
+        break;
+    }
+
+    LEAVE();
+    return ret_radio_type;
+}
+
+/**
+ *  @brief Convert NL80211 interface type to MLAN_BSS_MODE_*
+ *
+ *  @param iftype   Interface type of NL80211
+ *
+ *  @return         Driver bss mode
+ */
+static t_u32
+woal_nl80211_iftype_to_mode(enum nl80211_iftype iftype)
+{
+    switch (iftype) {
+    case NL80211_IFTYPE_ADHOC:
+        return MLAN_BSS_MODE_IBSS;
+    case NL80211_IFTYPE_STATION:
+        return MLAN_BSS_MODE_INFRA;
+    case NL80211_IFTYPE_UNSPECIFIED:
+    default:
+        return MLAN_BSS_MODE_AUTO;
+    }
+}
+
+/**
+ *  @brief Control WPS Session Enable/Disable
+ *
+ *  @param priv     Pointer to the moal_private driver data struct
+ *  @param enable   enable/disable flag
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_wps_cfg(moal_private * priv, int enable)
+{
+    int ret = 0;
+    mlan_ds_wps_cfg *pwps = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    PRINTM(MINFO, "WOAL_WPS_SESSION\n");
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_wps_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    pwps = (mlan_ds_wps_cfg *) req->pbuf;
+    req->req_id = MLAN_IOCTL_WPS_CFG;
+    req->action = MLAN_ACT_SET;
+    pwps->sub_command = MLAN_OID_WPS_CFG_SESSION;
+    if (enable)
+        pwps->param.wps_session = MLAN_WPS_CFG_SESSION_START;
+    else
+        pwps->param.wps_session = MLAN_WPS_CFG_SESSION_END;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief configure ASSOC IE
+ *
+ * @param priv				A pointer to moal private structure
+ * @param ie				A pointer to ie data
+ * @param ie_len			The length of ie data
+ *
+ * @return                  0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_assoc_ies_cfg(moal_private * priv, t_u8 * ie, int ie_len)
+{
+    int bytes_left = ie_len;
+    t_u8 *pcurrent_ptr = ie;
+    int total_ie_len;
+    t_u8 element_len;
+    int ret = MLAN_STATUS_SUCCESS;
+    IEEEtypes_ElementId_e element_id;
+    IEEEtypes_VendorSpecific_t *pvendor_ie;
+    t_u8 wps_oui[] = { 0x00, 0x50, 0xf2, 0x04 };
+
+    while (bytes_left >= 2) {
+        element_id = (IEEEtypes_ElementId_e) (*((t_u8 *) pcurrent_ptr));
+        element_len = *((t_u8 *) pcurrent_ptr + 1);
+        total_ie_len = element_len + sizeof(IEEEtypes_Header_t);
+        if (bytes_left < total_ie_len) {
+            PRINTM(MERROR, "InterpretIE: Error in processing IE, "
+                   "bytes left < IE length\n");
+            bytes_left = 0;
+            continue;
+        }
+        switch (element_id) {
+        case RSN_IE:
+            if (MLAN_STATUS_SUCCESS !=
+                woal_set_get_gen_ie(priv, MLAN_ACT_SET, pcurrent_ptr,
+                                    &total_ie_len)) {
+                PRINTM(MERROR, "Fail to set RSN IE\n");
+                ret = -EFAULT;
+                goto done;
+            }
+            PRINTM(MIOCTL, "Set RSN IE\n");
+            break;
+        case VENDOR_SPECIFIC_221:
+            pvendor_ie = (IEEEtypes_VendorSpecific_t *) pcurrent_ptr;
+            if (!memcmp(pvendor_ie->vend_hdr.oui, wps_oui, sizeof(wps_oui))) {
+                PRINTM(MIOCTL, "Enable WPS session\n");
+                woal_wps_cfg(priv, MTRUE);
+            }
+            if (MLAN_STATUS_SUCCESS !=
+                woal_set_get_gen_ie(priv, MLAN_ACT_SET, pcurrent_ptr,
+                                    &total_ie_len)) {
+                PRINTM(MERROR, "Fail to Set VENDOR SPECIFIC IE\n");
+                ret = -EFAULT;
+                goto done;
+            }
+            PRINTM(MIOCTL, "Set VENDOR SPECIFIC IE, OUI: %02x:%02x:%02x:%02x\n",
+                   pvendor_ie->vend_hdr.oui[0], pvendor_ie->vend_hdr.oui[1],
+                   pvendor_ie->vend_hdr.oui[2], pvendor_ie->vend_hdr.oui_type);
+            break;
+        default:
+            if (MLAN_STATUS_SUCCESS !=
+                woal_set_get_gen_ie(priv, MLAN_ACT_SET, pcurrent_ptr,
+                                    &total_ie_len)) {
+                PRINTM(MERROR, "Fail to set GEN IE\n");
+                ret = -EFAULT;
+                goto done;
+            }
+            PRINTM(MIOCTL, "Set GEN IE\n");
+            break;
+        }
+        pcurrent_ptr += element_len + 2;
+        /* Need to account for IE ID and IE Len */
+        bytes_left -= (element_len + 2);
+    }
+  done:
+    return ret;
+}
+
+/**
+ * @brief Send domain info command to FW
+ *
+ * @param priv      A pointer to moal_private structure
+ *
+ * @return          MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+woal_send_domain_info_cmd_fw(moal_private * priv)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    enum ieee80211_band band;
+    struct ieee80211_supported_band *sband = NULL;
+    struct ieee80211_channel *channel = NULL;
+    t_u8 no_of_sub_band = 0;
+    t_u8 no_of_parsed_chan = 0;
+    t_u8 first_chan = 0, next_chan = 0, max_pwr = 0;
+    t_u8 i, flag = 0;
+    mlan_ds_11d_cfg *cfg_11d = NULL;
+    mlan_ds_radio_cfg *radio_cfg = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    if (!priv->wdev || !priv->wdev->wiphy) {
+        PRINTM(MERROR, "No wdev or wiphy in priv\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_radio_cfg));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    radio_cfg = (mlan_ds_radio_cfg *) req->pbuf;
+    radio_cfg->sub_command = MLAN_OID_BAND_CFG;
+    req->req_id = MLAN_IOCTL_RADIO_CFG;
+    req->action = MLAN_ACT_GET;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    band = woal_band_cfg_to_ieee_band(radio_cfg->param.band_cfg.config_bands);
+    if (!priv->wdev->wiphy->bands[band]) {
+        PRINTM(MERROR, "11D: setting domain info in FW failed");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    kfree(req);
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11d_cfg));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    cfg_11d = (mlan_ds_11d_cfg *) req->pbuf;
+    cfg_11d->sub_command = MLAN_OID_11D_DOMAIN_INFO;
+    req->req_id = MLAN_IOCTL_11D_CFG;
+    req->action = MLAN_ACT_SET;
+
+    /* Set country code */
+    cfg_11d->param.domain_info.country_code[0] = priv->country_code[0];
+    cfg_11d->param.domain_info.country_code[1] = priv->country_code[1];
+    cfg_11d->param.domain_info.country_code[2] = ' ';
+    cfg_11d->param.domain_info.band = band;
+
+    sband = priv->wdev->wiphy->bands[band];
+    for (i = 0; (i < sband->n_channels) &&
+         (no_of_sub_band < MRVDRV_MAX_SUBBAND_802_11D); i++) {
+        channel = &sband->channels[i];
+        if (channel->flags & IEEE80211_CHAN_DISABLED)
+            continue;
+
+        if (!flag) {
+            flag = 1;
+            next_chan = first_chan = (t_u32) channel->hw_value;
+            max_pwr = channel->max_power;
+            no_of_parsed_chan = 1;
+            continue;
+        }
+
+        if (channel->hw_value == next_chan + 1 && channel->max_power == max_pwr) {
+            next_chan++;
+            no_of_parsed_chan++;
+        } else {
+            cfg_11d->param.domain_info.sub_band[no_of_sub_band]
+                .first_chan = first_chan;
+            cfg_11d->param.domain_info.sub_band[no_of_sub_band]
+                .no_of_chan = no_of_parsed_chan;
+            cfg_11d->param.domain_info.sub_band[no_of_sub_band]
+                .max_tx_pwr = max_pwr;
+            no_of_sub_band++;
+            next_chan = first_chan = (t_u32) channel->hw_value;
+            max_pwr = channel->max_power;
+            no_of_parsed_chan = 1;
+        }
+    }
+
+    if (flag) {
+        cfg_11d->param.domain_info.sub_band[no_of_sub_band]
+            .first_chan = first_chan;
+        cfg_11d->param.domain_info.sub_band[no_of_sub_band]
+            .no_of_chan = no_of_parsed_chan;
+        cfg_11d->param.domain_info.sub_band[no_of_sub_band]
+            .max_tx_pwr = max_pwr;
+        no_of_sub_band++;
+    }
+    cfg_11d->param.domain_info.no_of_sub_band = no_of_sub_band;
+
+    /* Send domain info command to FW */
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = MLAN_STATUS_FAILURE;
+        PRINTM(MERROR, "11D: Error setting domain info in FW\n");
+        goto done;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Request the driver to change the channel and
+ * change domain info according to that channel
+ *
+ * @param priv            A pointer to moal_private structure
+ * @param chan            A pointer to ieee80211_channel structure
+ * @param channel_type    Channel type of nl80211_channel_type
+ *
+ * @return                0 -- success, otherwise fail
+ */
+int
+woal_set_rf_channel(moal_private * priv,
+                    struct ieee80211_channel *chan,
+                    enum nl80211_channel_type channel_type)
+{
+    int ret = 0;
+    t_u32 mode, config_bands = 0;
+    mlan_ioctl_req *req1 = NULL, *req2 = NULL;
+    mlan_ds_radio_cfg *radio_cfg = NULL;
+    mlan_ds_bss *bss = NULL;
+
+    ENTER();
+
+    mode = woal_nl80211_iftype_to_mode(priv->wdev->iftype);
+
+    req1 = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_radio_cfg));
+    if (req1 == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    radio_cfg = (mlan_ds_radio_cfg *) req1->pbuf;
+    radio_cfg->sub_command = MLAN_OID_BAND_CFG;
+    req1->req_id = MLAN_IOCTL_RADIO_CFG;
+
+    if (chan) {
+        req1->action = MLAN_ACT_SET;
+        /* Set appropriate bands */
+        if (chan->band == IEEE80211_BAND_2GHZ)
+            config_bands = BAND_B | BAND_G | BAND_GN;
+        else
+            config_bands = BAND_AN | BAND_A;
+        if (mode == MLAN_BSS_MODE_IBSS)
+            radio_cfg->param.band_cfg.adhoc_start_band = config_bands;
+        radio_cfg->param.band_cfg.config_bands = config_bands;
+        /* Set channel offset */
+        radio_cfg->param.band_cfg.sec_chan_offset =
+            woal_cfg80211_channel_type_to_channel(channel_type);
+
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req1, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        woal_send_domain_info_cmd_fw(priv);
+    }
+
+    PRINTM(MINFO, "Setting band %d, channel bandwidth %d and mode = %d\n",
+           config_bands, radio_cfg->param.band_cfg.sec_chan_offset, mode);
+
+    if (!chan)
+        goto done;
+
+    req2 = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req2 == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    bss = (mlan_ds_bss *) req2->pbuf;
+
+    bss->param.bss_chan.freq = chan->center_freq;
+    /* Convert frequency to channel */
+    bss->param.bss_chan.channel =
+        ieee80211_frequency_to_channel(chan->center_freq);
+
+    bss->sub_command = MLAN_OID_BSS_CHANNEL;
+    req2->req_id = MLAN_IOCTL_BSS;
+    req2->action = MLAN_ACT_SET;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req2, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_change_adhoc_chan(priv, bss->param.bss_chan.channel)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    if (req1)
+        kfree(req1);
+    if (req2)
+        kfree(req2);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set ewpa mode
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param ssid_bssid           A pointer to mlan_ssid_bssid structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_set_ewpa_mode(moal_private * priv, t_u8 wait_option,
+                   mlan_ssid_bssid * ssid_bssid)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto error;
+    }
+    /* Fill request buffer */
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_PASSPHRASE;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    req->action = MLAN_ACT_GET;
+
+    /* Try Get All */
+    memset(&sec->param.passphrase, 0, sizeof(mlan_ds_passphrase));
+    memcpy(&sec->param.passphrase.ssid, &ssid_bssid->ssid,
+           sizeof(sec->param.passphrase.ssid));
+    memcpy(&sec->param.passphrase.bssid, &ssid_bssid->bssid,
+           MLAN_MAC_ADDR_LENGTH);
+    sec->param.passphrase.psk_type = MLAN_PSK_QUERY;
+
+    /* Send IOCTL request to MLAN */
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, wait_option))
+        goto error;
+    sec->param.ewpa_enabled = MFALSE;
+    if (sec->param.passphrase.psk_type == MLAN_PSK_PASSPHRASE) {
+        if (sec->param.passphrase.psk.passphrase.passphrase_len > 0) {
+            sec->param.ewpa_enabled = MTRUE;
+        }
+    } else if (sec->param.passphrase.psk_type == MLAN_PSK_PMK)
+        sec->param.ewpa_enabled = MTRUE;
+
+    sec->sub_command = MLAN_OID_SEC_CFG_EWPA_ENABLED;
+    req->action = MLAN_ACT_SET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, wait_option);
+
+  error:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ * @brief Set encryption mode and enable WPA
+ *
+ * @param priv          A pointer to moal_private structure
+ * @param encrypt_mode  Encryption mode
+ * @param wpa_enabled   WPA enable or not
+ *
+ * @return              0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_set_auth(moal_private * priv, int encrypt_mode, int wpa_enabled)
+{
+    int ret = 0;
+
+    ENTER();
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_encrypt_mode(priv, MOAL_IOCTL_WAIT, encrypt_mode))
+        ret = -EFAULT;
+
+    if (wpa_enabled) {
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_wpa_enable(priv, MOAL_IOCTL_WAIT, 1))
+            ret = -EFAULT;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Informs the CFG802.11 subsystem of a new BSS connection.
+ *
+ * The following information are sent to the CFG802.11 subsystem
+ * to register the new BSS connection. If we do not register the new BSS,
+ * a kernel panic will result.
+ *      - MAC address
+ *      - Capabilities
+ *      - Beacon period
+ *      - RSSI value
+ *      - Channel
+ *      - Supported rates IE
+ *      - Extended capabilities IE
+ *      - DS parameter set IE
+ *      - HT Capability IE
+ *      - Vendor Specific IE (221)
+ *      - WPA IE
+ *      - RSN IE
+ *
+ * @param priv      A pointer to moal_private structure
+ * @param ssid      A pointer to mlan_802_11_ssid structure
+ *
+ * @return          MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_inform_bss_from_scan_result(moal_private * priv, mlan_802_11_ssid * ssid)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    struct ieee80211_channel *chan;
+    mlan_scan_resp scan_resp;
+    BSSDescriptor_t *scan_table;
+    t_u64 ts = 0;
+    u16 cap_info = 0;
+    int i = 0;
+    struct cfg80211_bss *pub = NULL;
+    ENTER();
+    if (!priv->wdev || !priv->wdev->wiphy) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    memset(&scan_resp, 0, sizeof(scan_resp));
+    if (MLAN_STATUS_SUCCESS != woal_get_scan_table(priv,
+                                                   MOAL_IOCTL_WAIT,
+                                                   &scan_resp)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    if (scan_resp.num_in_scan_table) {
+        scan_table = (BSSDescriptor_t *) scan_resp.pscan_table;
+        for (i = 0; i < scan_resp.num_in_scan_table; i++) {
+            if (ssid) {
+                /* Inform specific BSS only */
+                if (memcmp(ssid->ssid, scan_table[i].ssid.ssid, ssid->ssid_len))
+                    continue;
+            }
+            if (!scan_table[i].freq) {
+                PRINTM(MERROR, "Invalid channel number %d\n",
+                       (int) scan_table[i].channel);
+                continue;
+            }
+            chan = ieee80211_get_channel(priv->wdev->wiphy, scan_table[i].freq);
+            if (!chan) {
+                PRINTM(MERROR,
+                       "Fail to get chan with freq: channel=%d freq=%d\n",
+                       (int) scan_table[i].channel, (int) scan_table[i].freq);
+                continue;
+            }
+            memcpy(&ts, scan_table[i].time_stamp, sizeof(ts));
+            memcpy(&cap_info, &scan_table[i].cap_info, sizeof(cap_info));
+            pub = cfg80211_inform_bss(priv->wdev->wiphy, chan,
+                                      scan_table[i].mac_address,
+                                      ts, cap_info,
+                                      scan_table[i].beacon_period,
+                                      scan_table[i].pbeacon_buf +
+                                      WLAN_802_11_FIXED_IE_SIZE,
+                                      scan_table[i].beacon_buf_size -
+                                      WLAN_802_11_FIXED_IE_SIZE,
+                                      -RSSI_DBM_TO_MDM(scan_table[i].rssi),
+                                      GFP_KERNEL);
+            if (pub) {
+                pub->len_information_elements = pub->len_beacon_ies;
+                cfg80211_put_bss(pub);
+            }
+        }
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Informs the CFG802.11 subsystem of a new IBSS connection.
+ *
+ * The following information are sent to the CFG802.11 subsystem
+ * to register the new IBSS connection. If we do not register the
+ * new IBSS, a kernel panic will result.
+ *      - MAC address
+ *      - Capabilities
+ *      - Beacon period
+ *      - RSSI value
+ *      - Channel
+ *      - Supported rates IE
+ *      - Extended capabilities IE
+ *      - DS parameter set IE
+ *      - HT Capability IE
+ *      - Vendor Specific IE (221)
+ *      - WPA IE
+ *      - RSN IE
+ *
+ * @param priv              A pointer to moal_private structure
+ * @param cahn              A pointer to ieee80211_channel structure
+ * @param beacon_interval   Beacon interval
+ *
+ * @return                  MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+static mlan_status
+woal_cfg80211_inform_ibss_bss(moal_private * priv,
+                              struct ieee80211_channel *chan,
+                              t_u16 beacon_interval)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_bss_info bss_info;
+    mlan_ds_get_signal signal;
+    t_u8 ie_buf[MLAN_MAX_SSID_LENGTH + sizeof(IEEEtypes_Header_t)];
+    int ie_len = 0;
+    struct cfg80211_bss *bss = NULL;
+
+    ENTER();
+
+    ret = woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info);
+    if (ret)
+        goto done;
+
+    memset(ie_buf, 0, sizeof(ie_buf));
+    ie_buf[0] = WLAN_EID_SSID;
+    ie_buf[1] = bss_info.ssid.ssid_len;
+
+    memcpy(&ie_buf[sizeof(IEEEtypes_Header_t)],
+           &bss_info.ssid.ssid, bss_info.ssid.ssid_len);
+    ie_len = ie_buf[1] + sizeof(IEEEtypes_Header_t);
+
+    /* Get signal information from the firmware */
+    memset(&signal, 0, sizeof(mlan_ds_get_signal));
+    if (MLAN_STATUS_SUCCESS !=
+        woal_get_signal_info(priv, MOAL_IOCTL_WAIT, &signal)) {
+        PRINTM(MERROR, "Error getting signal information\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    bss = cfg80211_inform_bss(priv->wdev->wiphy, chan,
+                              bss_info.bssid, 0, WLAN_CAPABILITY_IBSS,
+                              beacon_interval, ie_buf, ie_len,
+                              signal.bcn_rssi_avg, GFP_KERNEL);
+    if (bss)
+        cfg80211_put_bss(bss);
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Request the driver for (re)association
+ *
+ * @param priv            A pointer to moal_private structure
+ * @param sme             A pointer to connect parameters
+ *
+ * @return                0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_assoc(moal_private * priv, void *sme)
+{
+    struct cfg80211_ibss_params *ibss_param = NULL;
+    struct cfg80211_connect_params *conn_param = NULL;
+    mlan_802_11_ssid req_ssid;
+    mlan_ssid_bssid ssid_bssid;
+    mlan_ds_radio_cfg *radio_cfg = NULL;
+    mlan_ioctl_req *req = NULL;
+    int ret = 0;
+    t_u32 auth_type = 0, mode;
+    int wpa_enabled = 0;
+    int group_enc_mode = 0, pairwise_enc_mode = 0;
+    int alg_is_wep = 0;
+
+    t_u8 *ssid, ssid_len = 0, *bssid;
+    t_u8 *ie = NULL;
+    int ie_len = 0;
+    struct ieee80211_channel *channel = NULL;
+    t_u16 beacon_interval = 0;
+    bool privacy;
+
+    ENTER();
+
+    mode = woal_nl80211_iftype_to_mode(priv->wdev->iftype);
+
+    if (mode == MLAN_BSS_MODE_IBSS) {
+        ibss_param = (struct cfg80211_ibss_params *) sme;
+        ssid = ibss_param->ssid;
+        ssid_len = ibss_param->ssid_len;
+        bssid = ibss_param->bssid;
+        channel = ibss_param->channel;
+        if (ibss_param->ie_len)
+            ie = ibss_param->ie;
+        ie_len = ibss_param->ie_len;
+        beacon_interval = ibss_param->beacon_interval;
+        privacy = ibss_param->privacy;
+    } else {
+        conn_param = (struct cfg80211_connect_params *) sme;
+        ssid = conn_param->ssid;
+        ssid_len = conn_param->ssid_len;
+        bssid = conn_param->bssid;
+        channel = conn_param->channel;
+        if (conn_param->ie_len)
+            ie = conn_param->ie;
+        ie_len = conn_param->ie_len;
+        privacy = conn_param->privacy;
+    }
+
+    memset(&req_ssid, 0, sizeof(mlan_802_11_ssid));
+    memset(&ssid_bssid, 0, sizeof(mlan_ssid_bssid));
+
+    req_ssid.ssid_len = ssid_len;
+    if (ssid_len > IW_ESSID_MAX_SIZE) {
+        PRINTM(MERROR, "Invalid SSID - aborting\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    memcpy(req_ssid.ssid, ssid, ssid_len);
+    if (!req_ssid.ssid_len || req_ssid.ssid[0] < 0x20) {
+        PRINTM(MERROR, "Invalid SSID - aborting\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    if (mode == MLAN_BSS_MODE_IBSS && channel) {
+        /* Get the secondary channel offset */
+        req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_radio_cfg));
+        if (req == NULL) {
+            ret = -ENOMEM;
+            goto done;
+        }
+        radio_cfg = (mlan_ds_radio_cfg *) req->pbuf;
+        radio_cfg->sub_command = MLAN_OID_BAND_CFG;
+        req->req_id = MLAN_IOCTL_RADIO_CFG;
+        req->action = MLAN_ACT_GET;
+
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        if (MLAN_STATUS_SUCCESS != woal_set_rf_channel(priv,
+                                                       channel,
+                                                       woal_channel_to_nl80211_channel_type
+                                                       (radio_cfg->param.
+                                                        band_cfg.
+                                                        sec_chan_offset))) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_ewpa_mode(priv, MOAL_IOCTL_WAIT, &ssid_bssid)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_cfg80211_set_key(priv, 0, 0, NULL, 0, NULL, 0,
+                              KEY_INDEX_CLEAR_ALL, NULL, 1)) {
+        /* Disable keys and clear all previous security settings */
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (ie && ie_len) {         /* Set the IE */
+        if (MLAN_STATUS_SUCCESS !=
+            woal_cfg80211_assoc_ies_cfg(priv, ie, ie_len)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+    if (conn_param && mode != MLAN_BSS_MODE_IBSS) {
+        /* These parameters are only for managed mode */
+        if (conn_param->auth_type == NL80211_AUTHTYPE_OPEN_SYSTEM)
+            auth_type = MLAN_AUTH_MODE_OPEN;
+        else if (conn_param->auth_type == NL80211_AUTHTYPE_SHARED_KEY)
+            auth_type = MLAN_AUTH_MODE_SHARED;
+        else if (conn_param->auth_type == NL80211_AUTHTYPE_NETWORK_EAP)
+            auth_type = MLAN_AUTH_MODE_NETWORKEAP;
+        else
+            auth_type = MLAN_AUTH_MODE_AUTO;
+
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_auth_mode(priv, MOAL_IOCTL_WAIT, auth_type)) {
+            ret = -EFAULT;
+            goto done;
+        }
+
+        if (conn_param->crypto.n_ciphers_pairwise) {
+            pairwise_enc_mode =
+                woal_cfg80211_get_encryption_mode(conn_param->
+                                                  crypto.ciphers_pairwise[0],
+                                                  &wpa_enabled);
+            ret = woal_cfg80211_set_auth(priv, pairwise_enc_mode, wpa_enabled);
+            if (ret)
+                goto done;
+        }
+
+        if (conn_param->crypto.cipher_group) {
+            group_enc_mode =
+                woal_cfg80211_get_encryption_mode(conn_param->
+                                                  crypto.cipher_group,
+                                                  &wpa_enabled);
+            ret = woal_cfg80211_set_auth(priv, group_enc_mode, wpa_enabled);
+            if (ret)
+                goto done;
+        }
+
+        if (conn_param->key) {
+            alg_is_wep =
+                woal_cfg80211_is_alg_wep(pairwise_enc_mode) |
+                woal_cfg80211_is_alg_wep(group_enc_mode);
+            if (alg_is_wep) {
+                PRINTM(MINFO, "Setting wep encryption with "
+                       "key len %d\n", conn_param->key_len);
+                /* Set the WEP key */
+                if (MLAN_STATUS_SUCCESS !=
+                    woal_cfg80211_set_wep_keys(priv, conn_param->key,
+                                               conn_param->key_len,
+                                               conn_param->key_idx)) {
+                    ret = -EFAULT;
+                    goto done;
+                }
+                /* Enable the WEP key by key index */
+                if (MLAN_STATUS_SUCCESS !=
+                    woal_cfg80211_set_wep_keys(priv, NULL, 0,
+                                               conn_param->key_idx)) {
+                    ret = -EFAULT;
+                    goto done;
+                }
+            }
+        }
+    }
+
+    if (mode == MLAN_BSS_MODE_IBSS) {
+        mlan_ds_bss *bss = NULL;
+        /* Change beacon interval */
+        if ((beacon_interval < MLAN_MIN_BEACON_INTERVAL) ||
+            (beacon_interval > MLAN_MAX_BEACON_INTERVAL)) {
+            ret = -EINVAL;
+            goto done;
+        }
+        if (req)
+            kfree(req);
+        req = NULL;
+
+        req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+        if (req == NULL) {
+            ret = -ENOMEM;
+            goto done;
+        }
+        bss = (mlan_ds_bss *) req->pbuf;
+        req->req_id = MLAN_IOCTL_BSS;
+        req->action = MLAN_ACT_SET;
+        bss->sub_command = MLAN_OID_IBSS_BCN_INTERVAL;
+        bss->param.bcn_interval = beacon_interval;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+
+        /* "privacy" is set only for ad-hoc mode */
+        if (privacy) {
+            /*
+             * Keep MLAN_ENCRYPTION_MODE_WEP40 for now so that
+             * the firmware can find a matching network from the
+             * scan. cfg80211 does not give us the encryption
+             * mode at this stage so just setting it to wep here
+             */
+            if (MLAN_STATUS_SUCCESS !=
+                woal_set_auth_mode(priv, MOAL_IOCTL_WAIT,
+                                   MLAN_AUTH_MODE_OPEN)) {
+                ret = -EFAULT;
+                goto done;
+            }
+
+            wpa_enabled = 0;
+            ret = woal_cfg80211_set_auth(priv,
+                                         MLAN_ENCRYPTION_MODE_WEP104,
+                                         wpa_enabled);
+            if (ret)
+                goto done;
+        }
+    }
+    memcpy(&ssid_bssid.ssid, &req_ssid, sizeof(mlan_802_11_ssid));
+    if (bssid)
+        memcpy(&ssid_bssid.bssid, bssid, ETH_ALEN);
+    if (MLAN_STATUS_SUCCESS != woal_find_essid(priv, &ssid_bssid)) {
+        /* Do specific SSID scanning */
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_scan(priv, MOAL_IOCTL_WAIT, &req_ssid)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+    /* Disconnect before try to associate */
+    woal_disconnect(priv, MOAL_IOCTL_WAIT, NULL);
+
+    if (mode != MLAN_BSS_MODE_IBSS) {
+        if (MLAN_STATUS_SUCCESS !=
+            woal_find_best_network(priv, MOAL_IOCTL_WAIT, &ssid_bssid)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        /* Inform the BSS information to kernel, otherwise kernel will give a
+           panic after successful assoc */
+        if (MLAN_STATUS_SUCCESS !=
+            woal_inform_bss_from_scan_result(priv, &req_ssid)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    } else if (MLAN_STATUS_SUCCESS !=
+               woal_find_best_network(priv, MOAL_IOCTL_WAIT, &ssid_bssid))
+        /* Adhoc start, Check the channel command */
+        woal_11h_channel_check_ioctl(priv);
+
+    PRINTM(MINFO, "Trying to associate to %s and bssid %pM\n",
+           (char *) req_ssid.ssid, ssid_bssid.bssid);
+
+    /* Zero SSID implies use BSSID to connect */
+    if (bssid)
+        memset(&ssid_bssid.ssid, 0, sizeof(mlan_802_11_ssid));
+    else                        /* Connect to BSS by ESSID */
+        memset(&ssid_bssid.bssid, 0, MLAN_MAC_ADDR_LENGTH);
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_bss_start(priv, MOAL_IOCTL_WAIT, &ssid_bssid)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Inform the IBSS information to kernel, otherwise kernel will give a
+       panic after successful assoc */
+    if (mode == MLAN_BSS_MODE_IBSS) {
+        if (MLAN_STATUS_SUCCESS !=
+            woal_cfg80211_inform_ibss_bss(priv, channel, beacon_interval)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+  done:
+    if (ret) {
+        /* clear IE */
+        ie_len = 0;
+        woal_set_get_gen_ie(priv, MLAN_ACT_SET, NULL, &ie_len);
+    }
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,0,0) || defined(COMPAT_WIRELESS)
+/**
+ *  @brief Set/Get DTIM period
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param action               Action set or get
+ *  @param wait_option          Wait option
+ *  @param value                DTIM period
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+static mlan_status
+woal_set_get_dtim_period(moal_private * priv,
+                         t_u32 action, t_u8 wait_option, t_u8 * value)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_snmp_mib *mib = NULL;
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_snmp_mib));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    mib = (mlan_ds_snmp_mib *) req->pbuf;
+    mib->sub_command = MLAN_OID_SNMP_MIB_DTIM_PERIOD;
+    req->req_id = MLAN_IOCTL_SNMP_MIB;
+    req->action = action;
+
+    if (action == MLAN_ACT_SET) {
+        mib->param.dtim_period = *value;
+    }
+
+    /* Send IOCTL request to MLAN */
+    ret = woal_request_ioctl(priv, req, wait_option);
+    if (ret == MLAN_STATUS_SUCCESS && action == MLAN_ACT_GET) {
+        *value = (t_u8) mib->param.dtim_period;
+    }
+
+  done:
+    if (req && (ret != MLAN_STATUS_PENDING))
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+#endif
+
+/**
+ * @brief Request the driver to dump the station information
+ *
+ * @param priv            A pointer to moal_private structure
+ * @param sinfo           A pointer to station_info structure
+ *
+ * @return                0 -- success, otherwise fail
+ */
+static mlan_status
+woal_cfg80211_dump_station_info(moal_private * priv, struct station_info *sinfo)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_get_signal signal;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_rate *rate = NULL;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,0,0) || defined(COMPAT_WIRELESS)
+    mlan_bss_info bss_info;
+    t_u8 dtim_period = 0;
+#endif
+
+    ENTER();
+    sinfo->filled = STATION_INFO_RX_BYTES | STATION_INFO_TX_BYTES |
+        STATION_INFO_RX_PACKETS | STATION_INFO_TX_PACKETS |
+        STATION_INFO_SIGNAL | STATION_INFO_TX_BITRATE;
+
+    /* Get signal information from the firmware */
+    memset(&signal, 0, sizeof(mlan_ds_get_signal));
+    if (MLAN_STATUS_SUCCESS !=
+        woal_get_signal_info(priv, MOAL_IOCTL_WAIT, &signal)) {
+        PRINTM(MERROR, "Error getting signal information\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_rate));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    rate = (mlan_ds_rate *) req->pbuf;
+    rate->sub_command = MLAN_OID_GET_DATA_RATE;
+    req->req_id = MLAN_IOCTL_RATE;
+    req->action = MLAN_ACT_GET;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    sinfo->txrate.flags = RATE_INFO_FLAGS_MCS;
+    if (rate->param.data_rate.tx_ht_bw == MLAN_HT_BW40) {
+        sinfo->txrate.flags |= RATE_INFO_FLAGS_40_MHZ_WIDTH;
+    }
+    if (rate->param.data_rate.tx_ht_gi == MLAN_HT_SGI) {
+        sinfo->txrate.flags |= RATE_INFO_FLAGS_SHORT_GI;
+    }
+    sinfo->txrate.mcs = rate->param.data_rate.tx_data_rate;
+    sinfo->rx_bytes = priv->stats.rx_bytes;
+    sinfo->tx_bytes = priv->stats.tx_bytes;
+    sinfo->rx_packets = priv->stats.rx_packets;
+    sinfo->tx_packets = priv->stats.tx_packets;
+    sinfo->signal = signal.bcn_rssi_avg;
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,0,0) || defined(COMPAT_WIRELESS)
+    /* Update BSS information */
+    sinfo->filled |= STATION_INFO_BSS_PARAM;
+    sinfo->bss_param.flags = 0;
+    ret = woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info);
+    if (ret)
+        goto done;
+    if (bss_info.capability_info & WLAN_CAPABILITY_SHORT_PREAMBLE)
+        sinfo->bss_param.flags |= BSS_PARAM_FLAGS_SHORT_PREAMBLE;
+    if (bss_info.capability_info & WLAN_CAPABILITY_SHORT_SLOT_TIME)
+        sinfo->bss_param.flags |= BSS_PARAM_FLAGS_SHORT_SLOT_TIME;
+    sinfo->bss_param.beacon_interval = bss_info.beacon_interval;
+    /* Get DTIM period */
+    ret = woal_set_get_dtim_period(priv, MLAN_ACT_GET,
+                                   MOAL_IOCTL_WAIT, &dtim_period);
+    if (ret) {
+        PRINTM(MERROR, "Get DTIM period failed\n");
+        goto done;
+    }
+    sinfo->bss_param.dtim_period = dtim_period;
+#endif
+
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/********************************************************
+				Global Functions
+********************************************************/
+
+/**
+ * @brief Request the driver to change regulatory domain
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param request         A pointer to regulatory_request structure
+ *
+ * @return                0
+ */
+static int
+woal_cfg80211_reg_notifier(struct wiphy *wiphy,
+                           struct regulatory_request *request)
+{
+    moal_private *priv = (moal_private *) woal_get_wiphy_priv(wiphy);
+    int ret = 0;
+
+    ENTER();
+
+    if (!priv) {
+        PRINTM(MFATAL, "Unable to get priv in %s()\n", __FUNCTION__);
+        LEAVE();
+        return -EINVAL;
+    }
+
+    PRINTM(MINFO, "cfg80211 regulatory domain callback "
+           "%c%c\n", request->alpha2[0], request->alpha2[1]);
+
+    if (MLAN_STATUS_SUCCESS != woal_set_region_code(priv, request->alpha2))
+        PRINTM(MERROR, "Set country code failed!\n");
+    memcpy(priv->country_code, request->alpha2, COUNTRY_CODE_LEN);
+
+    switch (request->initiator) {
+    case NL80211_REGDOM_SET_BY_DRIVER:
+        PRINTM(MINFO, "Regulatory domain BY_DRIVER\n");
+        break;
+    case NL80211_REGDOM_SET_BY_CORE:
+        PRINTM(MINFO, "Regulatory domain BY_CORE\n");
+        break;
+    case NL80211_REGDOM_SET_BY_USER:
+        PRINTM(MINFO, "Regulatory domain BY_USER\n");
+        break;
+        /* TODO: apply driver specific changes in channel flags based on the
+           request initiator if necessory. * */
+    case NL80211_REGDOM_SET_BY_COUNTRY_IE:
+        PRINTM(MINFO, "Regulatory domain BY_COUNTRY_IE\n");
+        break;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_send_domain_info_cmd_fw(priv))
+        ret = -EFAULT;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Request the driver to do a scan. Always returning
+ * zero meaning that the scan request is given to driver,
+ * and will be valid until passed to cfg80211_scan_done().
+ * To inform scan results, call cfg80211_inform_bss().
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param dev             A pointer to net_device structure
+ * @param request         A pointer to cfg80211_scan_request structure
+ *
+ * @return                0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_scan(struct wiphy *wiphy, struct net_device *dev,
+                   struct cfg80211_scan_request *request)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+    wlan_user_scan_cfg scan_req;
+    struct ieee80211_channel *chan;
+    int ret = 0, i;
+
+    ENTER();
+
+    PRINTM(MINFO, "Received scan request on %s\n", dev->name);
+
+    if (priv->scan_request && priv->scan_request != request) {
+        LEAVE();
+        return -EBUSY;
+    }
+    priv->scan_request = request;
+
+    memset(&scan_req, 0x00, sizeof(scan_req));
+    for (i = 0; i < priv->scan_request->n_ssids; i++) {
+        memcpy(scan_req.ssid_list[i].ssid,
+               priv->scan_request->ssids[i].ssid,
+               priv->scan_request->ssids[i].ssid_len);
+        if (priv->scan_request->ssids[i].ssid_len)
+            scan_req.ssid_list[i].max_len = 0;
+        else
+            scan_req.ssid_list[i].max_len = 0xff;
+        PRINTM(MIOCTL, "scan: ssid=%s\n", scan_req.ssid_list[i].ssid);
+    }
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+    if (priv->bss_type == MLAN_BSS_TYPE_WIFIDIRECT &&
+        priv->scan_request->n_ssids) {
+        if (!memcmp(scan_req.ssid_list[i - 1].ssid, "DIRECT-", 7)) {
+            /* Enable wildcard ssid scan */
+            memcpy(scan_req.ssid_list[i].ssid, "DIRECT-*", 8);
+            scan_req.ssid_list[i].max_len = 0xff;
+        }
+    }
+#endif
+#endif
+    for (i = 0; i < priv->scan_request->n_channels; i++) {
+        chan = priv->scan_request->channels[i];
+        scan_req.chan_list[i].chan_number = chan->hw_value;
+        scan_req.chan_list[i].radio_type = chan->band;
+        if (chan->flags & IEEE80211_CHAN_DISABLED)
+            scan_req.chan_list[i].scan_type = MLAN_SCAN_TYPE_PASSIVE;
+        else
+            scan_req.chan_list[i].scan_type = MLAN_SCAN_TYPE_ACTIVE;
+        scan_req.chan_list[i].scan_time = 0;
+    }
+    if (priv->scan_request->ie && priv->scan_request->ie_len) {
+        if (MLAN_STATUS_SUCCESS !=
+            woal_cfg80211_mgmt_frame_ie(priv, NULL, 0,
+                                        NULL, 0, NULL, 0,
+                                        (t_u8 *) priv->scan_request->ie,
+                                        priv->scan_request->ie_len,
+                                        MGMT_MASK_PROBE_REQ)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    } else {
+                /** Clear SCAN IE in Firmware */
+        woal_cfg80211_mgmt_frame_ie(priv, NULL, 0, NULL, 0, NULL, 0, NULL, 0,
+                                    MGMT_MASK_PROBE_REQ);
+    }
+    if (MLAN_STATUS_SUCCESS != woal_do_scan(priv, &scan_req)) {
+        ret = -EAGAIN;
+    }
+  done:
+    LEAVE();
+    return 0;
+}
+
+/**
+ * @brief Request the driver to connect to the ESS with
+ * the specified parameters from kernel
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param dev             A pointer to net_device structure
+ * @param sme             A pointer to cfg80211_connect_params structure
+ *
+ * @return                0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
+                      struct cfg80211_connect_params *sme)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+    int ret = 0;
+
+    ENTER();
+
+    PRINTM(MINFO, "Received association request on %s\n", dev->name);
+
+    if (priv->wdev->iftype != NL80211_IFTYPE_STATION
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37) || defined(COMPAT_WIRELESS)
+        && priv->wdev->iftype != NL80211_IFTYPE_P2P_CLIENT
+#endif /* KERNEL_VERSION */
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+        ) {
+        PRINTM(MERROR, "Received infra assoc request "
+               "when station not in infra mode\n");
+        LEAVE();
+        return -EINVAL;
+    }
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+    if (priv->bss_type == MLAN_BSS_TYPE_WIFIDIRECT
+        && (priv->wdev->iftype == NL80211_IFTYPE_STATION
+            || priv->wdev->iftype == NL80211_IFTYPE_P2P_CLIENT)) {
+        /* if bsstype == wifi direct, and iftype == station or p2p client, that
+           means wpa_supplicant wants to enable wifi direct functionality, so
+           we should init p2p client. Note that due to kernel iftype check,
+           ICS wpa_supplicant could not updaet iftype to init p2p client, so
+           we have to done it here. */
+        if (MLAN_STATUS_SUCCESS != woal_cfg80211_init_p2p_client(priv)) {
+            PRINTM(MERROR, "Init p2p client for wpa_supplicant failed.\n");
+            ret = -EFAULT;
+
+            LEAVE();
+            return ret;
+        }
+    }
+#endif
+#endif
+
+    if (priv->scan_type == MLAN_SCAN_TYPE_PASSIVE)
+        woal_set_scan_type(priv, MLAN_SCAN_TYPE_ACTIVE);
+    ret = woal_cfg80211_assoc(priv, (void *) sme);
+
+    if (priv->scan_type == MLAN_SCAN_TYPE_PASSIVE)
+        woal_set_scan_type(priv, MLAN_SCAN_TYPE_PASSIVE);
+
+    if (!ret) {
+        cfg80211_connect_result(priv->netdev, priv->cfg_bssid, NULL, 0,
+                                NULL, 0, WLAN_STATUS_SUCCESS, GFP_KERNEL);
+        PRINTM(MINFO, "Associated to bssid %pM successfully\n",
+               priv->cfg_bssid);
+    } else {
+        PRINTM(MINFO, "Association to bssid %pM failed\n", priv->cfg_bssid);
+        cfg80211_connect_result(priv->netdev, priv->cfg_bssid, NULL, 0,
+                                NULL, 0, WLAN_STATUS_UNSPECIFIED_FAILURE,
+                                GFP_KERNEL);
+        memset(priv->cfg_bssid, 0, ETH_ALEN);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Request the driver to disconnect
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param dev             A pointer to net_device structure
+ * @param reason_code     Reason code
+ *
+ * @return                0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *dev,
+                         t_u16 reason_code)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+
+    ENTER();
+    PRINTM(MINFO, "Received disassociation request on %s\n", dev->name);
+
+    if (priv->cfg_disconnect) {
+        PRINTM(MERROR, "Disassociation already in progress\n");
+        LEAVE();
+        return -EBUSY;
+    }
+
+    if (priv->media_connected == MFALSE) {
+        LEAVE();
+        return -EINVAL;
+    }
+
+    priv->cfg_disconnect = 1;
+
+    if (woal_disconnect(priv, MOAL_IOCTL_WAIT, priv->cfg_bssid) !=
+        MLAN_STATUS_SUCCESS) {
+        LEAVE();
+        return -EFAULT;
+    }
+
+    PRINTM(MINFO, "Successfully disconnected from %pM: Reason code %d\n",
+           priv->cfg_bssid, reason_code);
+
+    memset(priv->cfg_bssid, 0, ETH_ALEN);
+
+    LEAVE();
+    return 0;
+}
+
+/**
+ * @brief Request the driver to get the station information
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param dev             A pointer to net_device structure
+ * @param mac             MAC address of the station
+ * @param sinfo           A pointer to station_info structure
+ *
+ * @return                0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_get_station(struct wiphy *wiphy,
+                          struct net_device *dev,
+                          t_u8 * mac, struct station_info *sinfo)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+
+    ENTER();
+
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+    if (GET_BSS_ROLE(priv) == MLAN_BSS_ROLE_UAP) {
+        LEAVE();
+        return woal_uap_cfg80211_get_station(wiphy, dev, mac, sinfo);
+    }
+#endif
+#endif
+    if (priv->media_connected == MFALSE) {
+        PRINTM(MINFO, "cfg80211: Media not connected!\n");
+        LEAVE();
+        return -ENOENT;
+    }
+    if (memcmp(mac, priv->cfg_bssid, ETH_ALEN)) {
+        PRINTM(MINFO, "cfg80211: Request not for this station!\n");
+        LEAVE();
+        return -ENOENT;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_cfg80211_dump_station_info(priv, sinfo)) {
+        PRINTM(MERROR, "cfg80211: Failed to get station info\n");
+        ret = -EFAULT;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Request the driver to dump the station information
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param dev             A pointer to net_device structure
+ * @param idx             Station index
+ * @param mac             MAC address of the station
+ * @param sinfo           A pointer to station_info structure
+ *
+ * @return                0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_dump_station(struct wiphy *wiphy,
+                           struct net_device *dev, int idx,
+                           t_u8 * mac, struct station_info *sinfo)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+
+    ENTER();
+
+    if (!priv->media_connected || idx != 0) {
+        PRINTM(MINFO, "cfg80211: Media not connected or"
+               " not for this station!\n");
+        LEAVE();
+        return -ENOENT;
+    }
+
+    memcpy(mac, priv->cfg_bssid, ETH_ALEN);
+
+    if (MLAN_STATUS_SUCCESS != woal_cfg80211_dump_station_info(priv, sinfo)) {
+        PRINTM(MERROR, "cfg80211: Failed to get station info\n");
+        ret = -EFAULT;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Request the driver to Join the specified
+ * IBSS (or create if necessary)
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param dev             A pointer to net_device structure
+ * @param params          A pointer to cfg80211_ibss_params structure
+ *
+ * @return                0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
+                        struct cfg80211_ibss_params *params)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+    int ret = 0;
+
+    ENTER();
+
+    if (priv->wdev->iftype != NL80211_IFTYPE_ADHOC) {
+        PRINTM(MERROR, "Request IBSS join received "
+               "when station not in ibss mode\n");
+        LEAVE();
+        return -EINVAL;
+    }
+
+    ret = woal_cfg80211_assoc(priv, (void *) params);
+
+    if (!ret) {
+        cfg80211_ibss_joined(priv->netdev, priv->cfg_bssid, GFP_KERNEL);
+        PRINTM(MINFO, "Joined/created adhoc network with bssid"
+               " %pM successfully\n", priv->cfg_bssid);
+    } else {
+        PRINTM(MINFO, "Failed creating/joining adhoc network\n");
+        memset(priv->cfg_bssid, 0, ETH_ALEN);
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Request the driver to leave the IBSS
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param dev             A pointer to net_device structure
+ *
+ * @return                0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+
+    ENTER();
+
+    if (priv->cfg_disconnect) {
+        PRINTM(MERROR, "IBSS leave already in progress\n");
+        LEAVE();
+        return -EBUSY;
+    }
+
+    if (priv->media_connected == MFALSE) {
+        LEAVE();
+        return -EINVAL;
+    }
+
+    priv->cfg_disconnect = 1;
+
+    PRINTM(MINFO, "Leaving from IBSS %pM\n", priv->cfg_bssid);
+    if (woal_disconnect(priv, MOAL_IOCTL_WAIT, priv->cfg_bssid) !=
+        MLAN_STATUS_SUCCESS) {
+        LEAVE();
+        return -EFAULT;
+    }
+
+    memset(priv->cfg_bssid, 0, ETH_ALEN);
+
+    LEAVE();
+    return 0;
+}
+
+/**
+ * @brief Request the driver to change the IEEE power save
+ * mdoe
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param dev             A pointer to net_device structure
+ * @param enabled         Enable or disable
+ * @param timeout         Timeout value
+ *
+ * @return                0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_set_power_mgmt(struct wiphy *wiphy,
+                             struct net_device *dev, bool enabled, int timeout)
+{
+    int ret = 0, disabled;
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+
+    ENTER();
+
+    if (enabled)
+        disabled = 0;
+    else
+        disabled = 1;
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_power_mgmt(priv, MLAN_ACT_SET, &disabled, timeout)) {
+        ret = -EOPNOTSUPP;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Request the driver to change the transmit power
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param type            TX power adjustment type
+ * @param dbm             TX power in dbm
+ *
+ * @return                0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_set_tx_power(struct wiphy *wiphy,
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,36) && !defined(COMPAT_WIRELESS)
+                           enum tx_power_setting type,
+#else
+                           enum nl80211_tx_power_setting type,
+#endif
+                           int dbm)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) woal_get_wiphy_priv(wiphy);
+    mlan_power_cfg_t power_cfg;
+
+    ENTER();
+
+    if (!priv) {
+        PRINTM(MFATAL, "Unable to get priv in %s()\n", __FUNCTION__);
+        LEAVE();
+        return -EFAULT;
+    }
+
+    if (type) {
+        power_cfg.is_power_auto = 0;
+        power_cfg.power_level = dbm;
+    } else
+        power_cfg.is_power_auto = 1;
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_tx_power(priv, MLAN_ACT_SET, &power_cfg))
+        ret = -EFAULT;
+
+    LEAVE();
+    return ret;
+}
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,35) || defined(COMPAT_WIRELESS)
+/**
+ * CFG802.11 operation handler for connection quality monitoring.
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param dev             A pointer to net_device structure
+ * @param rssi_thold	  rssi threshold
+ * @param rssi_hyst		  rssi hysteresis
+ */
+static int
+woal_cfg80211_set_cqm_rssi_config(struct wiphy *wiphy,
+                                  struct net_device *dev,
+                                  s32 rssi_thold, u32 rssi_hyst)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+    ENTER();
+    priv->cqm_rssi_thold = rssi_thold;
+    priv->cqm_rssi_hyst = rssi_hyst;
+
+    PRINTM(MIOCTL, "rssi_thold=%d rssi_hyst=%d\n",
+           (int) rssi_thold, (int) rssi_hyst);
+    woal_set_rssi_threshold(priv, 0);
+    LEAVE();
+    return 0;
+}
+#endif
+
+/**
+ *  @brief Sets up the CFG802.11 specific HT capability fields
+ *  with default values
+ *
+ *  @param ht_info      A pointer to ieee80211_sta_ht_cap structure
+ *  @param dev_cap      Device capability informations
+ *  @param mcs_set      Device MCS sets
+ *
+ *  @return             N/A
+ */
+static void
+woal_cfg80211_setup_sta_ht_cap(struct ieee80211_sta_ht_cap *ht_info,
+                               t_u32 dev_cap, t_u8 * mcs_set)
+{
+    ENTER();
+
+    ht_info->ht_supported = true;
+    ht_info->ampdu_factor = 0x3;
+    ht_info->ampdu_density = 0x6;
+
+    memset(&ht_info->mcs, 0, sizeof(ht_info->mcs));
+    ht_info->cap = 0;
+    if (mcs_set)
+        memcpy(ht_info->mcs.rx_mask, mcs_set, sizeof(ht_info->mcs.rx_mask));
+    if (dev_cap & MBIT(8))      /* 40Mhz intolarance enabled */
+        ht_info->cap |= IEEE80211_HT_CAP_40MHZ_INTOLERANT;
+    if (dev_cap & MBIT(17))     /* Channel width 20/40Mhz support */
+        ht_info->cap |= IEEE80211_HT_CAP_SUP_WIDTH_20_40;
+    if ((dev_cap >> 20) & 0x03) /* Delayed ACK supported */
+        ht_info->cap |= IEEE80211_HT_CAP_DELAY_BA;
+    if (dev_cap & MBIT(22))     /* Rx LDPC supported */
+        ht_info->cap |= IEEE80211_HT_CAP_LDPC_CODING;
+    if (dev_cap & MBIT(23))     /* Short GI @ 20Mhz supported */
+        ht_info->cap |= IEEE80211_HT_CAP_SGI_20;
+    if (dev_cap & MBIT(24))     /* Short GI @ 40Mhz supported */
+        ht_info->cap |= IEEE80211_HT_CAP_SGI_40;
+    if (dev_cap & MBIT(25))     /* Tx STBC supported */
+        ht_info->cap |= IEEE80211_HT_CAP_TX_STBC;
+    if (dev_cap & MBIT(26))     /* Rx STBC supported */
+        ht_info->cap |= IEEE80211_HT_CAP_RX_STBC;
+    if (dev_cap & MBIT(27))     /* MIMO PS supported */
+        ht_info->cap |= IEEE80211_HT_CAP_SM_PS;
+    if (dev_cap & MBIT(29))     /* Green field supported */
+        ht_info->cap |= IEEE80211_HT_CAP_GRN_FLD;
+    if (dev_cap & MBIT(31))     /* MAX AMSDU supported */
+        ht_info->cap |= IEEE80211_HT_CAP_MAX_AMSDU;
+    ht_info->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
+
+    LEAVE();
+}
+
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+/**
+ * @brief remain on channel config
+ *
+ * @param priv              A pointer to moal_private structure
+ * @param wait_option       Wait option
+ * @param cancel			cancel remain on channel flag
+ * @param status            A pointer to status, success, in process or reject
+ * @param chan              A pointer to ieee80211_channel structure
+ * @param channel_type      channel_type,
+ * @param duration          Duration wait to receive frame
+ *
+ * @return                  0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_remain_on_channel_cfg(moal_private * priv,
+                                    t_u8 wait_option, t_u8 remove,
+                                    t_u8 * status,
+                                    struct ieee80211_channel *chan,
+                                    enum nl80211_channel_type channel_type,
+                                    t_u32 duration)
+{
+    mlan_ds_remain_chan chan_cfg;
+    int ret = 0;
+
+    ENTER();
+    memset(&chan_cfg, 0, sizeof(mlan_ds_remain_chan));
+    if (remove) {
+        chan_cfg.remove = MTRUE;
+    } else {
+        if (chan->band == IEEE80211_BAND_2GHZ)
+            chan_cfg.bandcfg = 0;
+        else if (chan->band == IEEE80211_BAND_5GHZ)
+            chan_cfg.bandcfg = 1;
+        switch (channel_type) {
+        case NL80211_CHAN_HT40MINUS:
+            chan_cfg.bandcfg |= SEC_CHANNEL_BELOW;
+            break;
+        case NL80211_CHAN_HT40PLUS:
+            chan_cfg.bandcfg |= SEC_CHANNEL_ABOVE;
+            break;
+
+        case NL80211_CHAN_NO_HT:
+        case NL80211_CHAN_HT20:
+        default:
+            break;
+        }
+        chan_cfg.channel = ieee80211_frequency_to_channel(chan->center_freq);
+        chan_cfg.remain_period = duration;
+    }
+    if (MLAN_STATUS_SUCCESS ==
+        woal_set_remain_channel_ioctl(priv, wait_option, &chan_cfg))
+        *status = chan_cfg.status;
+    else
+        ret = -EFAULT;
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief tx mgmt frame
+ *
+ * @param wiphy                 A pointer to wiphy structure
+ * @param dev                   A pointer to net_device structure
+ * @param cookie                A pointer to frame cookie
+ *
+ * @return                0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_mgmt_tx_cancel_wait(struct wiphy *wiphy,
+                                  struct net_device *dev, u64 cookie)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+    int ret = 0;
+    t_u8 status = 1;
+
+    ENTER();
+
+    if (priv->phandle->remain_on_channel) {
+        if (woal_cfg80211_remain_on_channel_cfg(priv,
+                                                MOAL_IOCTL_WAIT, MTRUE, &status,
+                                                NULL, 0, 0)) {
+            PRINTM(MERROR, "Fail to cancel remain on channel\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        if (priv->phandle->cookie) {
+            cfg80211_remain_on_channel_expired(priv->netdev,
+                                               priv->phandle->cookie,
+                                               &priv->phandle->chan,
+                                               priv->phandle->channel_type,
+                                               GFP_ATOMIC);
+            priv->phandle->cookie = 0;
+        }
+        priv->phandle->remain_on_channel = MFALSE;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Make chip remain on channel
+ *
+ * @param wiphy                 A pointer to wiphy structure
+ * @param dev                   A pointer to net_device structure
+ * @param chan                  A pointer to ieee80211_channel structure
+ * @param channel_type          Channel type
+ * @param duration              Duration for timer
+ * @param cookie                A pointer to timer cookie
+ *
+ * @return                  0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_remain_on_channel(struct wiphy *wiphy,
+                                struct net_device *dev,
+                                struct ieee80211_channel *chan,
+                                enum nl80211_channel_type channel_type,
+                                unsigned int duration, u64 * cookie)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+    int ret = 0;
+    t_u8 status = 1;
+
+    ENTER();
+
+    if (!chan || !cookie) {
+        PRINTM(MERROR, "Invalid parameter for remain on channel\n");
+        ret = -EFAULT;
+        goto done;
+    }
+        /** cancel previous remain on channel */
+    if (priv->phandle->remain_on_channel) {
+        if (woal_cfg80211_remain_on_channel_cfg(priv,
+                                                MOAL_IOCTL_WAIT, MTRUE, &status,
+                                                NULL, 0, 0)) {
+            PRINTM(MERROR, "Fail to cancel remain on channel\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        priv->phandle->cookie = 0;
+        priv->phandle->remain_on_channel = MFALSE;
+    }
+    if (MLAN_STATUS_SUCCESS !=
+        woal_cfg80211_remain_on_channel_cfg(priv, MOAL_IOCTL_WAIT,
+                                            MFALSE, &status, chan, channel_type,
+                                            (t_u32) duration)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (status == 0) {
+        /* remain on channel operation success */
+        /* we need update the value cookie */
+        *cookie = (u64) random32() | 1;
+        priv->phandle->remain_on_channel = MTRUE;
+        priv->phandle->cookie = *cookie;
+        priv->phandle->channel_type = channel_type;
+        memcpy(&priv->phandle->chan, chan, sizeof(struct ieee80211_channel));
+        cfg80211_ready_on_channel(dev, *cookie, chan,
+                                  channel_type, duration, GFP_KERNEL);
+        PRINTM(MIOCTL, "Set remain on Channel: channel=%d cookie = %#llx\n",
+               ieee80211_frequency_to_channel(chan->center_freq),
+               priv->phandle->cookie);
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Cancel remain on channel
+ *
+ * @param wiphy                 A pointer to wiphy structure
+ * @param dev                   A pointer to net_device structure
+ * @param cookie                A pointer to timer cookie
+ *
+ * @return                  0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_cancel_remain_on_channel(struct wiphy *wiphy,
+                                       struct net_device *dev, u64 cookie)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+    int ret = 0;
+    t_u8 status = 1;
+
+    ENTER();
+    PRINTM(MIOCTL, "Cancel remain on Channel: cookie = %#llx\n", cookie);
+    if (woal_cfg80211_remain_on_channel_cfg(priv,
+                                            MOAL_IOCTL_WAIT, MTRUE, &status,
+                                            NULL, 0, 0)) {
+        PRINTM(MERROR, "Fail to cancel remain on channel\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    priv->phandle->remain_on_channel = MFALSE;
+    if (priv->phandle->cookie)
+        priv->phandle->cookie = 0;
+  done:
+    LEAVE();
+    return ret;
+}
+#endif /* KERNEL_VERSION */
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+
+/**
+ * @brief Initialize the wiphy
+ *
+ * @param priv            A pointer to moal_private structure
+ * @param wait_option     Wait option
+ *
+ * @return                MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_cfg80211_sta_init_wiphy(moal_private * priv, t_u8 wait_option)
+{
+    int retry_count, rts_thr, frag_thr, disabled;
+    struct wiphy *wiphy = NULL;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_11n_cfg *cfg_11n = NULL;
+    t_u32 hw_dev_cap;
+
+    ENTER();
+
+    if (priv->wdev)
+        wiphy = priv->wdev->wiphy;
+    else {
+        PRINTM(MERROR, "Invalid parameter when init wiphy.\n");
+        goto done;
+    }
+
+    /* Get 11n tx parameters from MLAN */
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11n_cfg));
+    if (req == NULL) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    cfg_11n = (mlan_ds_11n_cfg *) req->pbuf;
+    cfg_11n->sub_command = MLAN_OID_11N_HTCAP_CFG;
+    req->req_id = MLAN_IOCTL_11N_CFG;
+    req->action = MLAN_ACT_GET;
+    cfg_11n->param.htcap_cfg.hw_cap_req = MTRUE;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, wait_option)) {
+        kfree(req);
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    hw_dev_cap = cfg_11n->param.htcap_cfg.htcap;
+
+    /* Get supported MCS sets */
+    memset(req->pbuf, 0, sizeof(mlan_ds_11n_cfg));
+    cfg_11n->sub_command = MLAN_OID_11N_CFG_SUPPORTED_MCS_SET;
+    req->req_id = MLAN_IOCTL_11N_CFG;
+    req->action = MLAN_ACT_GET;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, wait_option)) {
+        kfree(req);
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    /* Initialize parameters for 2GHz and 5GHz bands */
+    woal_cfg80211_setup_sta_ht_cap(&wiphy->bands[IEEE80211_BAND_2GHZ]->ht_cap,
+                                   hw_dev_cap,
+                                   cfg_11n->param.supported_mcs_set);
+    /* For 2.4G band only card, this shouldn't be set
+       woal_cfg80211_setup_sta_ht_cap(&wiphy->bands[IEEE80211_BAND_5GHZ]->ht_cap,
+       hw_dev_cap, cfg_11n->param.supported_mcs_set); */
+    if (req)
+        kfree(req);
+
+    /* Set retry limit count to wiphy */
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_retry(priv, MLAN_ACT_GET, wait_option, &retry_count)) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    wiphy->retry_long = (t_u8) retry_count;
+    wiphy->retry_short = (t_u8) retry_count;
+    wiphy->max_scan_ie_len = MAX_IE_SIZE;
+
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+    /* we should add mgmt_stypes for both STA & Wifi Direct, while only
+       initialize duration_timer for Wifi Direct */
+    wiphy->mgmt_stypes = ieee80211_mgmt_stypes;
+    wiphy->max_remain_on_channel_duration = MAX_REMAIN_ON_CHANNEL_DURATION;
+#endif /* KERNEL_VERSION */
+#endif /* WIFI_DIRECT_SUPPORT && V14_FEATURE */
+
+    /* Set RTS threshold to wiphy */
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_rts(priv, MLAN_ACT_GET, wait_option, &rts_thr)) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    if (rts_thr < MLAN_RTS_MIN_VALUE || rts_thr > MLAN_RTS_MAX_VALUE)
+        rts_thr = MLAN_FRAG_RTS_DISABLED;
+    wiphy->rts_threshold = (t_u32) rts_thr;
+
+    /* Set fragment threshold to wiphy */
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_frag(priv, MLAN_ACT_GET, wait_option, &frag_thr)) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    if (frag_thr < MLAN_RTS_MIN_VALUE || frag_thr > MLAN_RTS_MAX_VALUE)
+        frag_thr = MLAN_FRAG_RTS_DISABLED;
+    wiphy->frag_threshold = (t_u32) frag_thr;
+
+    /* Get IEEE power save mode */
+    if (MLAN_STATUS_SUCCESS != woal_set_get_power_mgmt(priv,
+                                                       MLAN_ACT_GET, &disabled,
+                                                       0)) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+    /* Save the IEEE power save mode to wiphy, because after warmreset wiphy
+       power save should be updated instead of using the last saved
+       configuration */
+    if (disabled)
+        priv->wdev->ps = MFALSE;
+    else
+        priv->wdev->ps = MTRUE;
+
+  done:
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ * @brief Register the device with cfg80211
+ *
+ * @param dev       A pointer to net_device structure
+ * @param bss_type  BSS type
+ *
+ * @return          MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_register_sta_cfg80211(struct net_device * dev, t_u8 bss_type)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    void *wdev_priv = NULL;
+    struct wireless_dev *wdev = NULL;
+    mlan_fw_info fw_info;
+
+    ENTER();
+
+    /* Allocate wireless device */
+    wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
+    if (!wdev) {
+        PRINTM(MERROR, "Could not allocate wireless device\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto err_wdev;
+    }
+
+    if (bss_type == MLAN_BSS_TYPE_STA)
+        /* Allocate wiphy */
+        wdev->wiphy = wiphy_new(&woal_cfg80211_sta_ops, sizeof(moal_private *));
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+    else if (bss_type == MLAN_BSS_TYPE_WIFIDIRECT)
+        /* Allocate wiphy */
+        wdev->wiphy =
+            wiphy_new(&woal_cfg80211_wifi_direct_ops, sizeof(moal_private *));
+#endif
+#endif
+    else {
+        PRINTM(MERROR, "Unexpected bss_type when register cfg80211\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto err_wdev;
+    }
+
+    if (!wdev->wiphy) {
+        PRINTM(MERROR, "Could not allocate wiphy device\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto err_wdev;
+    }
+    if (bss_type == MLAN_BSS_TYPE_STA) {
+        wdev->iftype = NL80211_IFTYPE_STATION;
+        wdev->wiphy->interface_modes =
+            MBIT(NL80211_IFTYPE_STATION) | MBIT(NL80211_IFTYPE_ADHOC);
+        wdev->wiphy->max_scan_ssids = 10;
+    }
+#if defined(WIFI_DIRECT_SUPPORT)
+#if LINUX_VERSION_CODE >= WIFI_DIRECT_KERNEL_VERSION
+    if (bss_type == MLAN_BSS_TYPE_WIFIDIRECT) {
+        wdev->iftype = NL80211_IFTYPE_STATION;
+        wdev->wiphy->interface_modes = MBIT(NL80211_IFTYPE_STATION) |
+            MBIT(NL80211_IFTYPE_P2P_GO) |
+            MBIT(NL80211_IFTYPE_P2P_CLIENT) |
+            MBIT(NL80211_IFTYPE_ADHOC) | MBIT(NL80211_IFTYPE_AP) |
+            MBIT(NL80211_IFTYPE_MONITOR);
+        wdev->wiphy->max_scan_ssids = 10;
+    }
+#endif
+#endif
+
+    /* Set phy name like net device name */
+    dev_set_name(&wdev->wiphy->dev, dev->name);
+
+    /* Make this wiphy known to this driver only */
+    wdev->wiphy->privid = mrvl_wiphy_privid;
+
+    /* Supported bands */
+    wdev->wiphy->bands[IEEE80211_BAND_2GHZ] = &cfg80211_band_2ghz;
+    if (MLAN_STATUS_SUCCESS ==
+        woal_request_get_fw_info(priv, MOAL_CMD_WAIT, &fw_info)) {
+        if (fw_info.fw_bands & BAND_A)
+            wdev->wiphy->bands[IEEE80211_BAND_5GHZ] = &cfg80211_band_5ghz;
+    }
+    /* Initialize cipher suits */
+    wdev->wiphy->cipher_suites = cfg80211_cipher_suites;
+    wdev->wiphy->n_cipher_suites = ARRAY_SIZE(cfg80211_cipher_suites);
+
+    wdev->wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
+
+    /* We are using custom domains */
+    wdev->wiphy->flags |= WIPHY_FLAG_CUSTOM_REGULATORY;
+
+    wdev->wiphy->reg_notifier = woal_cfg80211_reg_notifier;
+
+    /* Set moal_private pointer in wiphy_priv */
+    wdev_priv = wiphy_priv(wdev->wiphy);
+
+    *(unsigned long *) wdev_priv = (unsigned long) priv;
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,39) || defined(COMPAT_WIRELESS)
+    set_wiphy_dev(wdev->wiphy, (struct device *) priv->phandle->hotplug_device);
+#endif
+
+    if (wiphy_register(wdev->wiphy) < 0) {
+        PRINTM(MERROR, "Wiphy device registration failed!\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto err_wdev;
+    }
+
+    dev_net_set(dev, wiphy_net(wdev->wiphy));
+    dev->ieee80211_ptr = wdev;
+    SET_NETDEV_DEV(dev, wiphy_dev(wdev->wiphy));
+    priv->wdev = wdev;
+
+    if (ret != MLAN_STATUS_SUCCESS) {
+        PRINTM(MERROR, "Wiphy device registration failed!\n");
+    } else {
+        PRINTM(MINFO, "Successfully registered wiphy device\n");
+        LEAVE();
+        return ret;
+    }
+
+    wiphy_unregister(wdev->wiphy);
+  err_wdev:
+    dev->ieee80211_ptr = NULL;
+    if (wdev && wdev->wiphy)
+        wiphy_free(wdev->wiphy);
+    kfree(wdev);
+    LEAVE();
+    return ret;
+}
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_sta_cfg80211.h b/drivers/net/wireless/sd8797/mlinux/moal_sta_cfg80211.h
new file mode 100644
index 000000000000..a83957c5c031
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_sta_cfg80211.h
@@ -0,0 +1,41 @@
+/** @file moal_sta_cfg80211.h
+  *
+  * @brief This file contains the STA CFG80211 specific defines.
+  *
+  * Copyright (C) 2011, Marvell International Ltd. 
+  * 
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+
+#ifndef _MOAL_STA_CFG80211_H_
+#define _MOAL_STA_CFG80211_H_
+
+/** Convert RSSI signal strength from dBm to mBm (100*dBm) */
+#define RSSI_DBM_TO_MDM(x)     		((x) * 100)
+
+mlan_status woal_register_sta_cfg80211(struct net_device *dev, t_u8 bss_type);
+mlan_status woal_cfg80211_sta_init_wiphy(moal_private * priv, t_u8 wait_option);
+
+mlan_status
+woal_cfg80211_set_key(moal_private * priv, t_u8 is_enable_wep,
+                      t_u32 cipher, const t_u8 * key, int key_len,
+                      const t_u8 * seq, int seq_len, t_u8 key_index,
+                      const t_u8 * addr, int disable);
+
+mlan_status
+woal_cfg80211_set_wep_keys(moal_private * priv, const t_u8 * key, int key_len,
+                           t_u8 index);
+
+#endif /* _MOAL_STA_CFG80211_H_ */
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_uap.c b/drivers/net/wireless/sd8797/mlinux/moal_uap.c
new file mode 100644
index 000000000000..fb8e2a7d6571
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_uap.c
@@ -0,0 +1,2692 @@
+/** @file moal_uap.c
+  *
+  * @brief This file contains the major functions in UAP
+  * driver. 
+  *
+  * Copyright (C) 2008-2012, Marvell International Ltd. 
+  * 
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+
+/********************************************************
+Change log:
+    10/21/2008: initial version
+********************************************************/
+
+#include    "moal_main.h"
+#include    "moal_uap.h"
+#include    "moal_sdio.h"
+#include    "moal_eth_ioctl.h"
+#if defined(STA_CFG80211) && defined(UAP_CFG80211)
+#include    "moal_cfg80211.h"
+#endif
+
+/********************************************************
+		Local Variables
+********************************************************/
+
+/********************************************************
+		Global Variables
+********************************************************/
+
+/********************************************************
+		Local Functions
+********************************************************/
+/**
+ *  @brief uap addba parameter handler
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_addba_param(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_11n_cfg *cfg_11n = NULL;
+    addba_param param;
+    int ret = 0;
+
+    ENTER();
+    memset(&param, 0, sizeof(param));
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "uap_addba_param() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (copy_from_user(&param, req->ifr_data, sizeof(param))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    PRINTM(MIOCTL,
+           "addba param: action=%d, timeout=%d, txwinsize=%d, rxwinsize=%d txamsdu=%d rxamsdu=%d\n",
+           (int) param.action, (int) param.timeout, (int) param.txwinsize,
+           (int) param.rxwinsize, (int) param.txamsdu, (int) param.rxamsdu);
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11n_cfg));
+    if (ioctl_req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+    cfg_11n = (mlan_ds_11n_cfg *) ioctl_req->pbuf;
+    cfg_11n->sub_command = MLAN_OID_11N_CFG_ADDBA_PARAM;
+    ioctl_req->req_id = MLAN_IOCTL_11N_CFG;
+
+    if (!param.action) {
+        /* Get addba param from MLAN */
+        ioctl_req->action = MLAN_ACT_GET;
+    } else {
+        /* Set addba param in MLAN */
+        ioctl_req->action = MLAN_ACT_SET;
+        cfg_11n->param.addba_param.timeout = param.timeout;
+        cfg_11n->param.addba_param.txwinsize = param.txwinsize;
+        cfg_11n->param.addba_param.rxwinsize = param.rxwinsize;
+        cfg_11n->param.addba_param.txamsdu = param.txamsdu;
+        cfg_11n->param.addba_param.rxamsdu = param.rxamsdu;
+    }
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    param.timeout = cfg_11n->param.addba_param.timeout;
+    param.txwinsize = cfg_11n->param.addba_param.txwinsize;
+    param.rxwinsize = cfg_11n->param.addba_param.rxwinsize;
+    param.txamsdu = cfg_11n->param.addba_param.txamsdu;
+    param.rxamsdu = cfg_11n->param.addba_param.rxamsdu;
+
+    /* Copy to user */
+    if (copy_to_user(req->ifr_data, &param, sizeof(param))) {
+        PRINTM(MERROR, "Copy to user failed!\n");
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief uap aggr priority tbl
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_aggr_priotbl(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_11n_cfg *cfg_11n = NULL;
+    aggr_prio_tbl param;
+    int ret = 0;
+    int i = 0;
+
+    ENTER();
+    memset(&param, 0, sizeof(param));
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "woal_uap_aggr_priotbl() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (copy_from_user(&param, req->ifr_data, sizeof(param))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    DBG_HEXDUMP(MCMD_D, "aggr_prio_tbl", (t_u8 *) & param, sizeof(param));
+
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11n_cfg));
+    if (ioctl_req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+    cfg_11n = (mlan_ds_11n_cfg *) ioctl_req->pbuf;
+    cfg_11n->sub_command = MLAN_OID_11N_CFG_AGGR_PRIO_TBL;
+    ioctl_req->req_id = MLAN_IOCTL_11N_CFG;
+
+    if (!param.action) {
+        /* Get aggr_prio_tbl from MLAN */
+        ioctl_req->action = MLAN_ACT_GET;
+    } else {
+        /* Set aggr_prio_tbl in MLAN */
+        ioctl_req->action = MLAN_ACT_SET;
+        for (i = 0; i < MAX_NUM_TID; i++) {
+            cfg_11n->param.aggr_prio_tbl.ampdu[i] = param.ampdu[i];
+            cfg_11n->param.aggr_prio_tbl.amsdu[i] = param.amsdu[i];
+        }
+    }
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    for (i = 0; i < MAX_NUM_TID; i++) {
+        param.ampdu[i] = cfg_11n->param.aggr_prio_tbl.ampdu[i];
+        param.amsdu[i] = cfg_11n->param.aggr_prio_tbl.amsdu[i];
+    }
+    /* Copy to user */
+    if (copy_to_user(req->ifr_data, &param, sizeof(param))) {
+        PRINTM(MERROR, "Copy to user failed!\n");
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief uap addba reject tbl
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_addba_reject(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_11n_cfg *cfg_11n = NULL;
+    addba_reject_para param;
+    int ret = 0;
+    int i = 0;
+
+    ENTER();
+    memset(&param, 0, sizeof(param));
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "woal_uap_addba_reject() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (copy_from_user(&param, req->ifr_data, sizeof(param))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    DBG_HEXDUMP(MCMD_D, "addba_reject tbl", (t_u8 *) & param, sizeof(param));
+
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11n_cfg));
+    if (ioctl_req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+    cfg_11n = (mlan_ds_11n_cfg *) ioctl_req->pbuf;
+    cfg_11n->sub_command = MLAN_OID_11N_CFG_ADDBA_REJECT;
+    ioctl_req->req_id = MLAN_IOCTL_11N_CFG;
+
+    if (!param.action) {
+        /* Get addba_reject tbl from MLAN */
+        ioctl_req->action = MLAN_ACT_GET;
+    } else {
+        /* Set addba_reject tbl in MLAN */
+        ioctl_req->action = MLAN_ACT_SET;
+        for (i = 0; i < MAX_NUM_TID; i++) {
+            cfg_11n->param.addba_reject[i] = param.addba_reject[i];
+        }
+    }
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    for (i = 0; i < MAX_NUM_TID; i++) {
+        param.addba_reject[i] = cfg_11n->param.addba_reject[i];
+    }
+    /* Copy to user */
+    if (copy_to_user(req->ifr_data, &param, sizeof(param))) {
+        PRINTM(MERROR, "Copy to user failed!\n");
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief uap get_fw_info handler
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_get_fw_info(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    fw_info fw;
+    mlan_fw_info fw_info;
+    int ret = 0;
+
+    ENTER();
+    memset(&fw, 0, sizeof(fw));
+    memset(&fw_info, 0, sizeof(fw_info));
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "woal_uap_get_fw_info() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (copy_from_user(&fw, req->ifr_data, sizeof(fw))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_get_fw_info(priv, MOAL_IOCTL_WAIT, &fw_info)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    fw.fw_release_number = fw_info.fw_ver;
+    fw.hw_dev_mcs_support = fw_info.hw_dev_mcs_support;
+    /* Copy to user */
+    if (copy_to_user(req->ifr_data, &fw, sizeof(fw))) {
+        PRINTM(MERROR, "Copy to user failed!\n");
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief configure deep sleep
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_deep_sleep(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_pm_cfg *pm = NULL;
+    deep_sleep_para param;
+    int ret = 0;
+
+    ENTER();
+    memset(&param, 0, sizeof(param));
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "woal_uap_deep_sleep() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (copy_from_user(&param, req->ifr_data, sizeof(param))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    DBG_HEXDUMP(MCMD_D, "deep_sleep_para", (t_u8 *) & param, sizeof(param));
+
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_pm_cfg));
+    if (ioctl_req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+    pm = (mlan_ds_pm_cfg *) ioctl_req->pbuf;
+    pm->sub_command = MLAN_OID_PM_CFG_DEEP_SLEEP;
+    ioctl_req->req_id = MLAN_IOCTL_PM_CFG;
+
+    if (!param.action) {
+        /* Get deep_sleep status from MLAN */
+        ioctl_req->action = MLAN_ACT_GET;
+    } else {
+        /* Set deep_sleep in MLAN */
+        ioctl_req->action = MLAN_ACT_SET;
+        if (param.deep_sleep == MTRUE) {
+            pm->param.auto_deep_sleep.auto_ds = DEEP_SLEEP_ON;
+            pm->param.auto_deep_sleep.idletime = param.idle_time;
+        } else {
+            pm->param.auto_deep_sleep.auto_ds = DEEP_SLEEP_OFF;
+        }
+    }
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (pm->param.auto_deep_sleep.auto_ds == DEEP_SLEEP_ON)
+        param.deep_sleep = MTRUE;
+    else
+        param.deep_sleep = MFALSE;
+    param.idle_time = pm->param.auto_deep_sleep.idletime;
+    /* Copy to user */
+    if (copy_to_user(req->ifr_data, &param, sizeof(param))) {
+        PRINTM(MERROR, "Copy to user failed!\n");
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief configure tx_pause settings
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_txdatapause(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+    tx_data_pause_para param;
+    int ret = 0;
+
+    ENTER();
+    memset(&param, 0, sizeof(param));
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "woal_uap_txdatapause corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (copy_from_user(&param, req->ifr_data, sizeof(param))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    DBG_HEXDUMP(MCMD_D, "tx_data_pause_para", (t_u8 *) & param, sizeof(param));
+
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (ioctl_req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+    misc = (mlan_ds_misc_cfg *) ioctl_req->pbuf;
+    misc->sub_command = MLAN_OID_MISC_TX_DATAPAUSE;
+    ioctl_req->req_id = MLAN_IOCTL_MISC_CFG;
+
+    if (!param.action) {
+        /* Get Tx data pause status from MLAN */
+        ioctl_req->action = MLAN_ACT_GET;
+    } else {
+        /* Set Tx data pause in MLAN */
+        ioctl_req->action = MLAN_ACT_SET;
+        misc->param.tx_datapause.tx_pause = param.txpause;
+        misc->param.tx_datapause.tx_buf_cnt = param.txbufcnt;
+    }
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    param.txpause = misc->param.tx_datapause.tx_pause;
+    param.txbufcnt = misc->param.tx_datapause.tx_buf_cnt;
+
+    /* Copy to user */
+    if (copy_to_user(req->ifr_data, &param, sizeof(param))) {
+        PRINTM(MERROR, "Copy to user failed!\n");
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief uap sdcmd52rw ioctl handler
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_sdcmd52_rw(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    sdcmd52_para param;
+    t_u8 func, data = 0;
+    int ret = 0, reg;
+
+    ENTER();
+    memset(&param, 0, sizeof(param));
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "woal_uap_sdcmd52_rw() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (copy_from_user(&param, req->ifr_data, sizeof(param))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    func = (t_u8) param.cmd52_params[0];
+    reg = (t_u32) param.cmd52_params[1];
+
+    if (!param.action) {
+        PRINTM(MINFO, "Cmd52 read, func=%d, reg=0x%08X\n", func, reg);
+        sdio_claim_host(((struct sdio_mmc_card *) priv->phandle->card)->func);
+        if (func)
+            data =
+                sdio_readb(((struct sdio_mmc_card *) priv->phandle->card)->func,
+                           reg, &ret);
+        else
+            data =
+                sdio_f0_readb(((struct sdio_mmc_card *) priv->phandle->card)->
+                              func, reg, &ret);
+        sdio_release_host(((struct sdio_mmc_card *) priv->phandle->card)->func);
+        if (ret) {
+            PRINTM(MERROR, "sdio_readb: reading register 0x%X failed\n", reg);
+            goto done;
+        }
+        param.cmd52_params[2] = data;
+    } else {
+        data = (t_u8) param.cmd52_params[2];
+        PRINTM(MINFO, "Cmd52 write, func=%d, reg=0x%08X, data=0x%02X\n", func,
+               reg, data);
+        sdio_claim_host(((struct sdio_mmc_card *) priv->phandle->card)->func);
+        if (func)
+            sdio_writeb(((struct sdio_mmc_card *) priv->phandle->card)->func,
+                        data, reg, &ret);
+        else
+            sdio_f0_writeb(((struct sdio_mmc_card *) priv->phandle->card)->func,
+                           data, reg, &ret);
+        sdio_release_host(((struct sdio_mmc_card *) priv->phandle->card)->func);
+        if (ret) {
+            PRINTM(MERROR, "sdio_writeb: writing register 0x%X failed\n", reg);
+            goto done;
+        }
+    }
+    /* Copy to user */
+    if (copy_to_user(req->ifr_data, &param, sizeof(param))) {
+        PRINTM(MERROR, "Copy to user failed!\n");
+        ret = -EFAULT;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief configure snmp mib
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_snmp_mib(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_snmp_mib *snmp = NULL;
+    snmp_mib_para param;
+    t_u8 value[MAX_SNMP_VALUE_SIZE];
+    int ret = 0;
+
+    ENTER();
+    memset(&param, 0, sizeof(param));
+    memset(value, 0, MAX_SNMP_VALUE_SIZE);
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "woal_uap_snmp_mib() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Copy from user */
+    if (copy_from_user(&param, req->ifr_data, sizeof(param))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    DBG_HEXDUMP(MCMD_D, "snmp_mib_para", (t_u8 *) & param, sizeof(param));
+    if (param.action) {
+        if (copy_from_user(value, req->ifr_data + sizeof(param),
+                           MIN(param.oid_val_len, MAX_SNMP_VALUE_SIZE))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        DBG_HEXDUMP(MCMD_D, "snmp_mib_para value", value,
+                    MIN(param.oid_val_len, sizeof(t_u32)));
+    }
+
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_snmp_mib));
+    if (ioctl_req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+    snmp = (mlan_ds_snmp_mib *) ioctl_req->pbuf;
+    ioctl_req->req_id = MLAN_IOCTL_SNMP_MIB;
+    switch (param.oid) {
+    case OID_80211D_ENABLE:
+        snmp->sub_command = MLAN_OID_SNMP_MIB_DOT11D;
+        break;
+    case OID_80211H_ENABLE:
+        snmp->sub_command = MLAN_OID_SNMP_MIB_DOT11H;
+        break;
+    default:
+        PRINTM(MERROR, "%s: Unsupported SNMP_MIB OID (%d).\n", __FUNCTION__,
+               param.oid);
+        goto done;
+    }
+
+    if (!param.action) {
+        /* Get mib value from MLAN */
+        ioctl_req->action = MLAN_ACT_GET;
+    } else {
+        /* Set mib value to MLAN */
+        ioctl_req->action = MLAN_ACT_SET;
+        snmp->param.oid_value = *(t_u32 *) value;
+    }
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Copy to user */
+    if (copy_to_user(req->ifr_data, &param, sizeof(param))) {
+        PRINTM(MERROR, "Copy to user failed!\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (!param.action) {        /* GET */
+        if (copy_to_user(req->ifr_data + sizeof(param), &snmp->param.oid_value,
+                         MIN(param.oid_val_len, sizeof(t_u32)))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief configure domain info
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_domain_info(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_11d_cfg *cfg11d = NULL;
+    domain_info_para param;
+    t_u8 tlv[MAX_DOMAIN_TLV_LEN];
+    t_u16 tlv_data_len = 0;
+    int ret = 0;
+
+    ENTER();
+    memset(&param, 0, sizeof(param));
+    memset(tlv, 0, MAX_DOMAIN_TLV_LEN);
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "woal_uap_domain_info() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Copy from user */
+    if (copy_from_user(&param, req->ifr_data, sizeof(param))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    DBG_HEXDUMP(MCMD_D, "domain_info_para", (t_u8 *) & param, sizeof(param));
+    if (param.action) {
+        /* get tlv header */
+        if (copy_from_user(tlv, req->ifr_data + sizeof(param), TLV_HEADER_LEN)) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        tlv_data_len = ((t_u16 *) (tlv))[1];
+        if ((TLV_HEADER_LEN + tlv_data_len) > sizeof(tlv)) {
+            PRINTM(MERROR, "TLV buffer is overflowed");
+            ret = -EINVAL;
+            goto done;
+        }
+        /* get full tlv */
+        if (copy_from_user(tlv, req->ifr_data + sizeof(param),
+                           TLV_HEADER_LEN + tlv_data_len)) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        DBG_HEXDUMP(MCMD_D, "domain_info_para tlv", tlv,
+                    TLV_HEADER_LEN + tlv_data_len);
+    }
+
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11d_cfg));
+    if (ioctl_req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+    cfg11d = (mlan_ds_11d_cfg *) ioctl_req->pbuf;
+    ioctl_req->req_id = MLAN_IOCTL_11D_CFG;
+    cfg11d->sub_command = MLAN_OID_11D_DOMAIN_INFO;
+
+    if (!param.action) {
+        /* Get mib value from MLAN */
+        ioctl_req->action = MLAN_ACT_GET;
+    } else {
+        /* Set mib value to MLAN */
+        ioctl_req->action = MLAN_ACT_SET;
+        memcpy(cfg11d->param.domain_tlv, tlv,
+               MIN(MAX_IE_SIZE, (TLV_HEADER_LEN + tlv_data_len)));
+    }
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Copy to user */
+    if (copy_to_user(req->ifr_data, &param, sizeof(param))) {
+        PRINTM(MERROR, "Copy to user failed!\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (!param.action) {        /* GET */
+        tlv_data_len = ((t_u16 *) (cfg11d->param.domain_tlv))[1];
+        if (copy_to_user
+            (req->ifr_data + sizeof(param), &cfg11d->param.domain_tlv,
+             TLV_HEADER_LEN + tlv_data_len)) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+
+#if defined(DFS_TESTING_SUPPORT)
+/**
+ *  @brief configure dfs testing settings
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_dfs_testing(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_11h_cfg *cfg11h = NULL;
+    dfs_testing_para param;
+    int ret = 0;
+
+    ENTER();
+    memset(&param, 0, sizeof(param));
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "woal_uap_dfs_testing() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Copy from user */
+    if (copy_from_user(&param, req->ifr_data, sizeof(param))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    DBG_HEXDUMP(MCMD_D, "dfs_testing_para", (t_u8 *) & param, sizeof(param));
+
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_11h_cfg));
+    if (ioctl_req == NULL) {
+        LEAVE();
+        return -ENOMEM;
+    }
+    cfg11h = (mlan_ds_11h_cfg *) ioctl_req->pbuf;
+    ioctl_req->req_id = MLAN_IOCTL_11H_CFG;
+    cfg11h->sub_command = MLAN_OID_11H_DFS_TESTING;
+
+    if (!param.action) {
+        /* Get mib value from MLAN */
+        ioctl_req->action = MLAN_ACT_GET;
+    } else {
+        /* Set mib value to MLAN */
+        ioctl_req->action = MLAN_ACT_SET;
+        cfg11h->param.dfs_testing.usr_cac_period_msec = param.usr_cac_period;
+        cfg11h->param.dfs_testing.usr_nop_period_sec = param.usr_nop_period;
+        cfg11h->param.dfs_testing.usr_no_chan_change = param.no_chan_change;
+        cfg11h->param.dfs_testing.usr_fixed_new_chan = param.fixed_new_chan;
+        priv->phandle->cac_period_jiffies = param.usr_cac_period * HZ / 1000;
+    }
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (!param.action) {        /* GET */
+        param.usr_cac_period = cfg11h->param.dfs_testing.usr_cac_period_msec;
+        param.usr_nop_period = cfg11h->param.dfs_testing.usr_nop_period_sec;
+        param.no_chan_change = cfg11h->param.dfs_testing.usr_no_chan_change;
+        param.fixed_new_chan = cfg11h->param.dfs_testing.usr_fixed_new_chan;
+    }
+    /* Copy to user */
+    if (copy_to_user(req->ifr_data, &param, sizeof(param))) {
+        PRINTM(MERROR, "Copy to user failed!\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+#endif
+
+/**
+ *  @brief Configure TX beamforming support
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_tx_bf_cfg(struct net_device *dev, struct ifreq *req)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ds_11n_tx_bf_cfg bf_cfg;
+    tx_bf_cfg_para_hdr param;
+    t_u16 action = 0;
+
+    ENTER();
+
+    memset(&param, 0, sizeof(param));
+    memset(&bf_cfg, 0, sizeof(bf_cfg));
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "woal_uap_tx_bf_cfg corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (copy_from_user(&param, req->ifr_data, sizeof(param))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (!param.action) {
+        /* Get BF configurations */
+        action = MLAN_ACT_GET;
+    } else {
+        /* Set BF configurations */
+        action = MLAN_ACT_SET;
+    }
+    if (copy_from_user(&bf_cfg, req->ifr_data + sizeof(tx_bf_cfg_para_hdr),
+                       sizeof(bf_cfg))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    DBG_HEXDUMP(MCMD_D, "bf_cfg", (t_u8 *) & bf_cfg, sizeof(bf_cfg));
+
+    if (MLAN_STATUS_SUCCESS != woal_set_get_tx_bf_cfg(priv, action, &bf_cfg)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Copy to user */
+    if (copy_to_user(req->ifr_data + sizeof(tx_bf_cfg_para_hdr),
+                     &bf_cfg, sizeof(bf_cfg))) {
+        PRINTM(MERROR, "Copy to user failed!\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief uap hs_cfg ioctl handler
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_hs_cfg(struct net_device *dev, struct ifreq *req,
+                BOOLEAN invoke_hostcmd)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ds_hs_cfg hscfg;
+    ds_hs_cfg hs_cfg;
+    mlan_bss_info bss_info;
+    t_u16 action;
+    int ret = 0;
+
+    ENTER();
+
+    memset(&hscfg, 0, sizeof(mlan_ds_hs_cfg));
+    memset(&hs_cfg, 0, sizeof(ds_hs_cfg));
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "uap_hs_cfg() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (copy_from_user(&hs_cfg, req->ifr_data, sizeof(ds_hs_cfg))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    PRINTM(MIOCTL, "ioctl hscfg: flags=0x%x condition=0x%x gpio=%d gap=0x%x\n",
+           hs_cfg.flags, hs_cfg.conditions, (int) hs_cfg.gpio, hs_cfg.gap);
+
+    /* HS config is blocked if HS is already activated */
+    if ((hs_cfg.flags & HS_CFG_FLAG_CONDITION) &&
+        (hs_cfg.conditions != HOST_SLEEP_CFG_CANCEL ||
+         invoke_hostcmd == MFALSE)) {
+        memset(&bss_info, 0, sizeof(bss_info));
+        woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info);
+        if (bss_info.is_hs_configured) {
+            PRINTM(MERROR, "HS already configured\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+    if (hs_cfg.flags & HS_CFG_FLAG_SET) {
+        action = MLAN_ACT_SET;
+        if (hs_cfg.flags != HS_CFG_FLAG_ALL) {
+            woal_set_get_hs_params(priv, MLAN_ACT_GET, MOAL_IOCTL_WAIT, &hscfg);
+        }
+        if (hs_cfg.flags & HS_CFG_FLAG_CONDITION)
+            hscfg.conditions = hs_cfg.conditions;
+        if (hs_cfg.flags & HS_CFG_FLAG_GPIO)
+            hscfg.gpio = hs_cfg.gpio;
+        if (hs_cfg.flags & HS_CFG_FLAG_GAP)
+            hscfg.gap = hs_cfg.gap;
+
+        if (invoke_hostcmd == MTRUE) {
+            /* Issue IOCTL to set up parameters */
+            hscfg.is_invoke_hostcmd = MFALSE;
+            if (MLAN_STATUS_SUCCESS !=
+                woal_set_get_hs_params(priv, action, MOAL_IOCTL_WAIT, &hscfg)) {
+                ret = -EFAULT;
+                goto done;
+            }
+        }
+    } else {
+        action = MLAN_ACT_GET;
+    }
+
+    /* Issue IOCTL to invoke hostcmd */
+    hscfg.is_invoke_hostcmd = invoke_hostcmd;
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_hs_params(priv, action, MOAL_IOCTL_WAIT, &hscfg)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (!(hs_cfg.flags & HS_CFG_FLAG_SET)) {
+        hs_cfg.flags =
+            HS_CFG_FLAG_CONDITION | HS_CFG_FLAG_GPIO | HS_CFG_FLAG_GAP;
+        hs_cfg.conditions = hscfg.conditions;
+        hs_cfg.gpio = hscfg.gpio;
+        hs_cfg.gap = hscfg.gap;
+        /* Copy to user */
+        if (copy_to_user(req->ifr_data, &hs_cfg, sizeof(ds_hs_cfg))) {
+            PRINTM(MERROR, "Copy to user failed!\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set Host Sleep parameters
+ *
+ *  @param priv         A pointer to moal_private structure
+ *  @param wrq          A pointer to iwreq structure
+ *
+ *  @return             0 --success, otherwise fail
+ */
+static int
+woal_uap_hs_set_para(struct net_device *dev, struct ifreq *req)
+{
+    int ret = 0;
+
+    ENTER();
+
+    if (req->ifr_data != NULL) {
+        ret = woal_uap_hs_cfg(dev, req, MFALSE);
+        goto done;
+    } else {
+        PRINTM(MERROR, "Invalid data\n");
+        ret = -EINVAL;
+        goto done;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief uap mgmt_frame_control ioctl handler
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_mgmt_frame_control(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int ret = 0;
+    t_u16 action = 0;
+    mgmt_frame_ctrl param;
+    mlan_uap_bss_param sys_config;
+
+    ENTER();
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "uap_mgmt_frame_ctrl() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Get user data */
+    if (copy_from_user(&param, req->ifr_data, sizeof(param))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (param.action) {
+        action = MLAN_ACT_SET;
+    } else {
+        action = MLAN_ACT_GET;
+    }
+    if (action == MLAN_ACT_SET) {
+        /* Initialize the invalid values so that the correct values below are
+           downloaded to firmware */
+        woal_set_sys_config_invalid_data(&sys_config);
+        sys_config.mgmt_ie_passthru_mask = param.mask;
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_sys_config(priv, action, MOAL_IOCTL_WAIT, &sys_config)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (action == MLAN_ACT_GET) {
+        param.mask = sys_config.mgmt_ie_passthru_mask;
+        if (copy_to_user(req->ifr_data, &param, sizeof(param))) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+        }
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Set/Get tx rate
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *
+ * @return           0 --success, otherwise fail
+ */
+static int
+woal_uap_tx_rate_cfg(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int ret = 0, i = 0;
+    mlan_ds_rate *rate = NULL;
+    mlan_ioctl_req *mreq = NULL;
+    tx_rate_cfg_t tx_rate_config;
+
+    ENTER();
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "uap_tx_rate_cfg() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    memset(&tx_rate_config, 0, sizeof(tx_rate_cfg_t));
+    /* Get user data */
+    if (copy_from_user(&tx_rate_config, req->ifr_data, sizeof(tx_rate_cfg_t))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    mreq = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_rate));
+    if (mreq == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    rate = (mlan_ds_rate *) mreq->pbuf;
+    rate->param.rate_cfg.rate_type = MLAN_RATE_INDEX;
+    rate->sub_command = MLAN_OID_RATE_CFG;
+    mreq->req_id = MLAN_IOCTL_RATE;
+    if (!(tx_rate_config.action))
+        mreq->action = MLAN_ACT_GET;
+    else {
+        mreq->action = MLAN_ACT_SET;
+        if (tx_rate_config.rate == AUTO_RATE)
+            rate->param.rate_cfg.is_rate_auto = 1;
+        else {
+            if ((tx_rate_config.rate != MLAN_RATE_INDEX_MCS32) &&
+                ((tx_rate_config.rate < 0) ||
+                 (tx_rate_config.rate > MLAN_RATE_INDEX_MCS15))) {
+                ret = -EINVAL;
+                goto done;
+            }
+        }
+        rate->param.rate_cfg.rate = tx_rate_config.rate;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, mreq, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (tx_rate_config.action) {
+        priv->rate_index = tx_rate_config.action;
+    } else {
+        if (rate->param.rate_cfg.is_rate_auto)
+            tx_rate_config.rate = AUTO_RATE;
+        else
+            tx_rate_config.rate = rate->param.rate_cfg.rate;
+        for (i = 0; i < MAX_BITMAP_RATES_SIZE; i++) {
+            tx_rate_config.bitmap_rates[i] =
+                rate->param.rate_cfg.bitmap_rates[i];
+        }
+
+        if (copy_to_user(req->ifr_data, &tx_rate_config, sizeof(tx_rate_cfg_t))) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+        }
+    }
+  done:
+    if (mreq)
+        kfree(mreq);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Set/Get RF antenna mode
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *
+ * @return           0 --success, otherwise fail
+ */
+static int
+woal_uap_antenna_cfg(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int ret = 0;
+    mlan_ds_radio_cfg *radio = NULL;
+    mlan_ioctl_req *mreq = NULL;
+    ant_cfg_t antenna_config;
+
+    ENTER();
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "uap_antenna_cfg() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    memset(&antenna_config, 0, sizeof(ant_cfg_t));
+    /* Get user data */
+    if (copy_from_user(&antenna_config, req->ifr_data, sizeof(ant_cfg_t))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    mreq = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_radio_cfg));
+    if (mreq == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    radio = (mlan_ds_radio_cfg *) mreq->pbuf;
+    radio->sub_command = MLAN_OID_ANT_CFG;
+    mreq->req_id = MLAN_IOCTL_RADIO_CFG;
+    if (!(antenna_config.action))
+        mreq->action = MLAN_ACT_GET;
+    else {
+        mreq->action = MLAN_ACT_SET;
+        radio->param.ant_cfg.tx_antenna = antenna_config.tx_mode;
+        radio->param.ant_cfg.rx_antenna = antenna_config.rx_mode;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, mreq, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (mreq->action == MLAN_ACT_GET) {
+        antenna_config.tx_mode = radio->param.ant_cfg.tx_antenna;
+        antenna_config.rx_mode = radio->param.ant_cfg.rx_antenna;
+        if (copy_to_user(req->ifr_data, &antenna_config, sizeof(ant_cfg_t))) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+        }
+    }
+  done:
+    if (mreq)
+        kfree(mreq);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief uap ioctl handler
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_ioctl(struct net_device *dev, struct ifreq *req)
+{
+    int ret = 0;
+    t_u32 subcmd = 0;
+    ENTER();
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "uap_ioctl() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (copy_from_user(&subcmd, req->ifr_data, sizeof(subcmd))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    PRINTM(MIOCTL, "ioctl subcmd=%d\n", (int) subcmd);
+    switch (subcmd) {
+    case UAP_ADDBA_PARA:
+        ret = woal_uap_addba_param(dev, req);
+        break;
+    case UAP_AGGR_PRIOTBL:
+        ret = woal_uap_aggr_priotbl(dev, req);
+        break;
+    case UAP_ADDBA_REJECT:
+        ret = woal_uap_addba_reject(dev, req);
+        break;
+    case UAP_FW_INFO:
+        ret = woal_uap_get_fw_info(dev, req);
+        break;
+    case UAP_DEEP_SLEEP:
+        ret = woal_uap_deep_sleep(dev, req);
+        break;
+    case UAP_TX_DATA_PAUSE:
+        ret = woal_uap_txdatapause(dev, req);
+        break;
+    case UAP_SDCMD52_RW:
+        ret = woal_uap_sdcmd52_rw(dev, req);
+        break;
+    case UAP_SNMP_MIB:
+        ret = woal_uap_snmp_mib(dev, req);
+        break;
+    case UAP_DOMAIN_INFO:
+        ret = woal_uap_domain_info(dev, req);
+        break;
+#ifdef DFS_TESTING_SUPPORT
+    case UAP_DFS_TESTING:
+        ret = woal_uap_dfs_testing(dev, req);
+        break;
+#endif
+    case UAP_TX_BF_CFG:
+        ret = woal_uap_tx_bf_cfg(dev, req);
+        break;
+    case UAP_HS_CFG:
+        ret = woal_uap_hs_cfg(dev, req, MTRUE);
+        break;
+    case UAP_HS_SET_PARA:
+        ret = woal_uap_hs_set_para(dev, req);
+        break;
+    case UAP_MGMT_FRAME_CONTROL:
+        ret = woal_uap_mgmt_frame_control(dev, req);
+        break;
+    case UAP_TX_RATE_CFG:
+        ret = woal_uap_tx_rate_cfg(dev, req);
+        break;
+    case UAP_ANTENNA_CFG:
+        ret = woal_uap_antenna_cfg(dev, req);
+        break;
+    default:
+        break;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief uap station deauth ioctl handler
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_sta_deauth_ioctl(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_bss *bss = NULL;
+    mlan_deauth_param deauth_param;
+    int ret = 0;
+
+    ENTER();
+
+    memset(&deauth_param, 0, sizeof(mlan_deauth_param));
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "uap_sta_deauth_ioctl() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (copy_from_user(&deauth_param, req->ifr_data, sizeof(mlan_deauth_param))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    PRINTM(MIOCTL,
+           "ioctl deauth station: %02x:%02x:%02x:%02x:%02x:%02x, reason=%d\n",
+           deauth_param.mac_addr[0], deauth_param.mac_addr[1],
+           deauth_param.mac_addr[2], deauth_param.mac_addr[3],
+           deauth_param.mac_addr[4], deauth_param.mac_addr[5],
+           deauth_param.reason_code);
+
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (ioctl_req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    bss = (mlan_ds_bss *) ioctl_req->pbuf;
+
+    bss->sub_command = MLAN_OID_UAP_DEAUTH_STA;
+    ioctl_req->req_id = MLAN_IOCTL_BSS;
+    ioctl_req->action = MLAN_ACT_SET;
+
+    memcpy(&bss->param.deauth_param, &deauth_param, sizeof(mlan_deauth_param));
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        if (copy_to_user(req->ifr_data, &ioctl_req->status_code, sizeof(t_u32)))
+            PRINTM(MERROR, "Copy to user failed!\n");
+        goto done;
+    }
+
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Set/Get radio
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *
+ * @return           0 --success, otherwise fail
+ */
+static int
+woal_uap_radio_ctl(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int ret = 0;
+    mlan_ds_radio_cfg *radio = NULL;
+    mlan_ioctl_req *mreq = NULL;
+    int data[2] = { 0, 0 };
+    mlan_bss_info bss_info;
+
+    ENTER();
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "uap_radio_ctl() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Get user data */
+    if (copy_from_user(&data, req->ifr_data, sizeof(data))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (data[0]) {
+        mreq = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_radio_cfg));
+        if (mreq == NULL) {
+            ret = -ENOMEM;
+            goto done;
+        }
+        radio = (mlan_ds_radio_cfg *) mreq->pbuf;
+        radio->sub_command = MLAN_OID_RADIO_CTRL;
+        mreq->req_id = MLAN_IOCTL_RADIO_CFG;
+        mreq->action = MLAN_ACT_SET;
+        radio->param.radio_on_off = (t_u32) data[1];
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, mreq, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+        }
+        if (mreq)
+            kfree(mreq);
+    } else {
+        /* Get radio status */
+        memset(&bss_info, 0, sizeof(bss_info));
+        woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info);
+
+        data[1] = bss_info.radio_on;
+        if (copy_to_user(req->ifr_data, data, sizeof(data))) {
+            PRINTM(MERROR, "Copy to user failed\n");
+            ret = -EFAULT;
+        }
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief uap bss control ioctl handler
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_bss_ctrl_ioctl(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int ret = 0, data = 0;
+
+    ENTER();
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "uap_bss_ctrl() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (copy_from_user(&data, req->ifr_data, sizeof(data))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    ret = woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, data);
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief uap power mode ioctl handler
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_power_mode_ioctl(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ioctl_req *ioctl_req = NULL;
+    mlan_ds_pm_cfg *pm_cfg = NULL;
+    mlan_ds_ps_mgmt ps_mgmt;
+    int ret = 0;
+
+    ENTER();
+
+    memset(&ps_mgmt, 0, sizeof(mlan_ds_ps_mgmt));
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "uap_power_mode_ioctl() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (copy_from_user(&ps_mgmt, req->ifr_data, sizeof(mlan_ds_ps_mgmt))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    PRINTM(MIOCTL,
+           "ioctl power: flag=0x%x ps_mode=%d ctrl_bitmap=%d min_sleep=%d max_sleep=%d "
+           "inact_to=%d min_awake=%d max_awake=%d\n", ps_mgmt.flags,
+           (int) ps_mgmt.ps_mode, (int) ps_mgmt.sleep_param.ctrl_bitmap,
+           (int) ps_mgmt.sleep_param.min_sleep,
+           (int) ps_mgmt.sleep_param.max_sleep,
+           (int) ps_mgmt.inact_param.inactivity_to,
+           (int) ps_mgmt.inact_param.min_awake,
+           (int) ps_mgmt.inact_param.max_awake);
+
+    if (ps_mgmt.
+        flags & ~(PS_FLAG_PS_MODE | PS_FLAG_SLEEP_PARAM |
+                  PS_FLAG_INACT_SLEEP_PARAM)) {
+        PRINTM(MERROR, "Invalid parameter: flags = 0x%x\n", ps_mgmt.flags);
+        ret = -EINVAL;
+        goto done;
+    }
+
+    if (ps_mgmt.ps_mode > PS_MODE_INACTIVITY) {
+        PRINTM(MERROR, "Invalid parameter: ps_mode = %d\n",
+               (int) ps_mgmt.flags);
+        ret = -EINVAL;
+        goto done;
+    }
+
+    ioctl_req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_pm_cfg));
+    if (ioctl_req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    pm_cfg = (mlan_ds_pm_cfg *) ioctl_req->pbuf;
+    pm_cfg->sub_command = MLAN_OID_PM_CFG_PS_MODE;
+    ioctl_req->req_id = MLAN_IOCTL_PM_CFG;
+    if (ps_mgmt.flags) {
+        ioctl_req->action = MLAN_ACT_SET;
+        memcpy(&pm_cfg->param.ps_mgmt, &ps_mgmt, sizeof(mlan_ds_ps_mgmt));
+    } else {
+        ioctl_req->action = MLAN_ACT_GET;
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        if (copy_to_user(req->ifr_data, &ioctl_req->status_code, sizeof(t_u32)))
+            PRINTM(MERROR, "Copy to user failed!\n");
+        goto done;
+    }
+    if (!ps_mgmt.flags) {
+        /* Copy to user */
+        if (copy_to_user
+            (req->ifr_data, &pm_cfg->param.ps_mgmt, sizeof(mlan_ds_ps_mgmt))) {
+            PRINTM(MERROR, "Copy to user failed!\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief uap BSS config ioctl handler
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_bss_cfg_ioctl(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int ret = 0;
+    mlan_ds_bss *bss = NULL;
+    mlan_ioctl_req *ioctl_req = NULL;
+    int offset = 0;
+    t_u32 action = 0;
+
+    ENTER();
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "uap_bss_cfg_ioctl() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Get action */
+    if (copy_from_user(&action, req->ifr_data + offset, sizeof(action))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    offset += sizeof(action);
+
+    /* Allocate an IOCTL request buffer */
+    ioctl_req =
+        (mlan_ioctl_req *) woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (ioctl_req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    bss = (mlan_ds_bss *) ioctl_req->pbuf;
+    bss->sub_command = MLAN_OID_UAP_BSS_CONFIG;
+    ioctl_req->req_id = MLAN_IOCTL_BSS;
+    if (action == 1) {
+        ioctl_req->action = MLAN_ACT_SET;
+    } else {
+        ioctl_req->action = MLAN_ACT_GET;
+    }
+
+    if (ioctl_req->action == MLAN_ACT_SET) {
+        /* Get the BSS config from user */
+        if (copy_from_user
+            (&bss->param.bss_config, req->ifr_data + offset,
+             sizeof(mlan_uap_bss_param))) {
+            PRINTM(MERROR, "Copy from user failed\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (ioctl_req->action == MLAN_ACT_GET) {
+        offset = sizeof(action);
+
+        /* Copy to user : BSS config */
+        if (copy_to_user
+            (req->ifr_data + offset, &bss->param.bss_config,
+             sizeof(mlan_uap_bss_param))) {
+            PRINTM(MERROR, "Copy to user failed!\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief uap get station list handler
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_get_sta_list_ioctl(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int ret = 0;
+    mlan_ds_get_info *info = NULL;
+    mlan_ioctl_req *ioctl_req = NULL;
+
+    ENTER();
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "uap_get_sta_list_ioctl() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Allocate an IOCTL request buffer */
+    ioctl_req =
+        (mlan_ioctl_req *) woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_get_info));
+    if (ioctl_req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    info = (mlan_ds_get_info *) ioctl_req->pbuf;
+    info->sub_command = MLAN_OID_UAP_STA_LIST;
+    ioctl_req->req_id = MLAN_IOCTL_GET_INFO;
+    ioctl_req->action = MLAN_ACT_GET;
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (ioctl_req->action == MLAN_ACT_GET) {
+        /* Copy to user : sta_list */
+        if (copy_to_user
+            (req->ifr_data, &info->param.sta_list, sizeof(mlan_ds_sta_list))) {
+            PRINTM(MERROR, "Copy to user failed!\n");
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief uAP set WAPI key ioctl
+ *
+ *  @param priv      A pointer to moal_private structure
+ *  @param msg       A pointer to wapi_msg structure
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_uap_set_wapi_key_ioctl(moal_private * priv, wapi_msg * msg)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    int ret = 0;
+    wapi_key_msg *key_msg = NULL;
+    t_u8 bcast_addr[] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+
+    ENTER();
+    if (msg->msg_len != sizeof(wapi_key_msg)) {
+        ret = -EINVAL;
+        goto done;
+    }
+    key_msg = (wapi_key_msg *) msg->msg;
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_ENCRYPT_KEY;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    req->action = MLAN_ACT_SET;
+
+    sec->param.encrypt_key.is_wapi_key = MTRUE;
+    sec->param.encrypt_key.key_len = MLAN_MAX_KEY_LENGTH;
+    memcpy(sec->param.encrypt_key.mac_addr, key_msg->mac_addr, ETH_ALEN);
+    sec->param.encrypt_key.key_index = key_msg->key_id;
+    if (0 == memcmp(key_msg->mac_addr, bcast_addr, ETH_ALEN))
+        sec->param.encrypt_key.key_flags = KEY_FLAG_GROUP_KEY;
+    else
+        sec->param.encrypt_key.key_flags = KEY_FLAG_SET_TX_KEY;
+    memcpy(sec->param.encrypt_key.key_material, key_msg->key,
+           sec->param.encrypt_key.key_len);
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT))
+        ret = -EFAULT;
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Enable/Disable wapi in firmware
+ *
+ *  @param priv          A pointer to moal_private structure
+ *  @param enable        MTRUE/MFALSE
+ *
+ *  @return              MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+static mlan_status
+woal_enable_wapi(moal_private * priv, t_u8 enable)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_bss *bss = NULL;
+    mlan_status status;
+
+    ENTER();
+
+    /* Allocate an IOCTL request buffer */
+    req = (mlan_ioctl_req *) woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        status = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    /* Fill request buffer */
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_UAP_BSS_CONFIG;
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = MLAN_ACT_GET;
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT);
+    if (status != MLAN_STATUS_SUCCESS) {
+        PRINTM(MERROR, "Get AP setting  failed! status=%d, error_code=0x%x\n",
+               status, req->status_code);
+    }
+
+    /* Change AP default setting */
+    req->action = MLAN_ACT_SET;
+    if (enable == MFALSE) {
+        bss->param.bss_config.auth_mode = MLAN_AUTH_MODE_OPEN;
+        bss->param.bss_config.protocol = PROTOCOL_NO_SECURITY;
+    } else {
+        bss->param.bss_config.auth_mode = MLAN_AUTH_MODE_OPEN;
+        bss->param.bss_config.protocol = PROTOCOL_WAPI;
+    }
+
+    /* Send IOCTL request to MLAN */
+    status = woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT);
+    if (status != MLAN_STATUS_SUCCESS) {
+        PRINTM(MERROR, "Set AP setting failed! status=%d, error_code=0x%x\n",
+               status, req->status_code);
+    }
+    if (enable)
+        woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_START);
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief uAP set WAPI flag ioctl
+ *
+ *  @param priv      A pointer to moal_private structure
+ *  @param msg       A pointer to wapi_msg structure
+ *
+ *  @return          0 --success, otherwise fail
+ */
+static int
+woal_uap_set_wapi_flag_ioctl(moal_private * priv, wapi_msg * msg)
+{
+    t_u8 wapi_psk_ie[] =
+        { 0x44, 0x14, 0x01, 0x00, 0x01, 0x00, 0x00, 0x14, 0x72, 0x02,
+        0x01, 0x00, 0x00, 0x14, 0x72, 0x01, 0x00, 0x14, 0x72, 0x01,
+        0x00, 0x00
+    };
+    t_u8 wapi_cert_ie[] =
+        { 0x44, 0x14, 0x01, 0x00, 0x01, 0x00, 0x00, 0x14, 0x72, 0x01,
+        0x01, 0x00, 0x00, 0x14, 0x72, 0x01, 0x00, 0x14, 0x72, 0x01,
+        0x00, 0x00
+    };
+    mlan_ds_misc_cfg *misc = NULL;
+    mlan_ioctl_req *req = NULL;
+    int ret = 0;
+
+    ENTER();
+
+    woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_STOP);
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    misc = (mlan_ds_misc_cfg *) req->pbuf;
+    misc->sub_command = MLAN_OID_MISC_GEN_IE;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+    req->action = MLAN_ACT_SET;
+
+    misc->param.gen_ie.type = MLAN_IE_TYPE_GEN_IE;
+    misc->param.gen_ie.len = sizeof(wapi_psk_ie);
+    if (msg->msg[0] & WAPI_MODE_PSK) {
+        memcpy(misc->param.gen_ie.ie_data, wapi_psk_ie, misc->param.gen_ie.len);
+    } else if (msg->msg[0] & WAPI_MODE_CERT) {
+        memcpy(misc->param.gen_ie.ie_data, wapi_cert_ie,
+               misc->param.gen_ie.len);
+    } else if (msg->msg[0] == 0) {
+        /* disable WAPI in driver */
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_wapi_enable(priv, MOAL_IOCTL_WAIT, 0))
+            ret = -EFAULT;
+        woal_enable_wapi(priv, MFALSE);
+        goto done;
+    } else {
+        ret = -EINVAL;
+        goto done;
+    }
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    woal_enable_wapi(priv, MTRUE);
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief set wapi ioctl function
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param req      A pointer to ifreq structure
+ *  @return         0 --success, otherwise fail
+ */
+static int
+woal_uap_set_wapi(struct net_device *dev, struct ifreq *req)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    wapi_msg msg;
+    int ret = 0;
+
+    ENTER();
+
+    /* Sanity check */
+    if (req->ifr_data == NULL) {
+        PRINTM(MERROR, "uap_set_wapi() corrupt data\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    memset(&msg, 0, sizeof(msg));
+
+    if (copy_from_user(&msg, req->ifr_data, sizeof(msg))) {
+        PRINTM(MERROR, "Copy from user failed\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    PRINTM(MIOCTL, "set wapi msg_type = %d, msg_len=%d\n", msg.msg_type,
+           msg.msg_len);
+    DBG_HEXDUMP(MCMD_D, "wapi msg", msg.msg, MIN(msg.msg_len, sizeof(msg.msg)));
+
+    switch (msg.msg_type) {
+    case P80211_PACKET_WAPIFLAG:
+        ret = woal_uap_set_wapi_flag_ioctl(priv, &msg);
+        break;
+    case P80211_PACKET_SETKEY:
+        ret = woal_uap_set_wapi_key_ioctl(priv, &msg);
+        break;
+    default:
+        ret = -EOPNOTSUPP;
+        break;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+/********************************************************
+		Global Functions
+********************************************************/
+/**
+ *  @brief Initialize the members of mlan_uap_bss_param
+ *  which are uploaded from firmware
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param sys_cfg  A pointer to mlan_uap_bss_param structure
+ *  @param wait_option      Wait option
+ *
+ *  @return         MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_uap_get_bss_param(moal_private * priv, mlan_uap_bss_param * sys_cfg,
+                       t_u8 wait_option)
+{
+    mlan_ds_bss *info = NULL;
+    mlan_ioctl_req *req = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    info = (mlan_ds_bss *) req->pbuf;
+    info->sub_command = MLAN_OID_UAP_BSS_CONFIG;
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = MLAN_ACT_GET;
+
+    status = woal_request_ioctl(priv, req, wait_option);
+    if (status != MLAN_STATUS_SUCCESS) {
+        PRINTM(MERROR, "Get bss info failed!\n");
+        status = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+    memcpy(sys_cfg, &info->param.bss_config, sizeof(mlan_uap_bss_param));
+
+  done:
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Set 11n status based on the configured security mode
+ *
+ *  @param sys_cfg  A pointer to mlan_uap_bss_param structure
+ *  @param action   MLAN_ACT_DISABLE or MLAN_ACT_ENABLE
+ *
+ *  @return         MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_uap_set_11n_status(mlan_uap_bss_param * sys_cfg, t_u8 action)
+{
+    mlan_status status = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+    if (action == MLAN_ACT_DISABLE) {
+        if ((sys_cfg->supported_mcs_set[0] == 0)
+            && (sys_cfg->supported_mcs_set[4] == 0)
+            && (sys_cfg->supported_mcs_set[1] == 0)
+            ) {
+            goto done;
+        } else {
+            sys_cfg->supported_mcs_set[0] = 0;
+            sys_cfg->supported_mcs_set[4] = 0;
+            sys_cfg->supported_mcs_set[1] = 0;
+        }
+    }
+
+    if (action == MLAN_ACT_ENABLE) {
+        if ((sys_cfg->supported_mcs_set[0] != 0)
+            || (sys_cfg->supported_mcs_set[4] != 0)
+            || (sys_cfg->supported_mcs_set[1] != 0)
+            ) {
+            goto done;
+        } else {
+            sys_cfg->supported_mcs_set[0] = 0xFF;
+            sys_cfg->supported_mcs_set[4] = 0x01;
+            sys_cfg->supported_mcs_set[1] = 0xFF;
+        }
+    }
+
+  done:
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Parse AP configuration from ASCII string
+ *
+ *  @param ap_cfg   A pointer to mlan_uap_bss_param structure
+ *  @param buf      A pointer to user data
+ *
+ *  @return         0 --success, otherwise fail
+ */
+int
+woal_uap_ap_cfg_parse_data(mlan_uap_bss_param * ap_cfg, t_s8 * buf)
+{
+    int ret = 0, atoi_ret;
+    int set_sec = 0, set_key = 0, set_chan = 0;
+    int set_preamble = 0, set_scb = 0, set_ssid = 0;
+    t_s8 *begin = buf, *value = NULL, *opt = NULL;
+
+    ENTER();
+
+    while (begin) {
+        value = woal_strsep(&begin, ',', '/');
+        opt = woal_strsep(&value, '=', '/');
+        if (opt && !strncmp(opt, "END", strlen("END"))) {
+            if (!ap_cfg->ssid.ssid_len) {
+                PRINTM(MERROR, "Minimum option required is SSID\n");
+                ret = -EINVAL;
+                goto done;
+            }
+            PRINTM(MINFO, "Parsing terminated by string END\n");
+            break;
+        }
+        if (!opt || !value || !value[0]) {
+            PRINTM(MERROR, "Invalid option\n");
+            ret = -EINVAL;
+            goto done;
+        } else if (!strncmp(opt, "ASCII_CMD", strlen("ASCII_CMD"))) {
+            if (strncmp(value, "AP_CFG", strlen("AP_CFG"))) {
+                PRINTM(MERROR, "ASCII_CMD: %s not matched with AP_CFG\n",
+                       value);
+                ret = -EFAULT;
+                goto done;
+            }
+            value = woal_strsep(&begin, ',', '/');
+            opt = woal_strsep(&value, '=', '/');
+            if (!opt || !value || !value[0]) {
+                PRINTM(MERROR, "Minimum option required is SSID\n");
+                ret = -EINVAL;
+                goto done;
+            } else if (!strncmp(opt, "SSID", strlen("SSID"))) {
+                if (set_ssid) {
+                    PRINTM(MWARN, "Skipping SSID, found again!\n");
+                    continue;
+                }
+                if (strlen(value) > MLAN_MAX_SSID_LENGTH) {
+                    PRINTM(MERROR, "SSID length exceeds max length\n");
+                    ret = -EFAULT;
+                    goto done;
+                }
+                ap_cfg->ssid.ssid_len = strlen(value);
+                strncpy((char *) ap_cfg->ssid.ssid, value, strlen(value));
+                PRINTM(MINFO, "ssid=%s, len=%d\n", ap_cfg->ssid.ssid,
+                       (int) ap_cfg->ssid.ssid_len);
+                set_ssid = 1;
+            } else {
+                PRINTM(MERROR, "AP_CFG: Invalid option %s, "
+                       "expect SSID\n", opt);
+                ret = -EINVAL;
+                goto done;
+            }
+        } else if (!strncmp(opt, "SEC", strlen("SEC"))) {
+            if (set_sec) {
+                PRINTM(MWARN, "Skipping SEC, found again!\n");
+                continue;
+            }
+            if (!strnicmp(value, "open", strlen("open"))) {
+                ap_cfg->auth_mode = MLAN_AUTH_MODE_OPEN;
+                if (set_key)
+                    ap_cfg->wpa_cfg.length = 0;
+                ap_cfg->key_mgmt = KEY_MGMT_NONE;
+                ap_cfg->protocol = PROTOCOL_NO_SECURITY;
+            } else if (!strnicmp(value, "wpa2-psk", strlen("wpa2-psk"))) {
+                ap_cfg->auth_mode = MLAN_AUTH_MODE_OPEN;
+                ap_cfg->protocol = PROTOCOL_WPA2;
+                ap_cfg->key_mgmt = KEY_MGMT_PSK;
+                ap_cfg->wpa_cfg.pairwise_cipher_wpa = CIPHER_AES_CCMP;
+                ap_cfg->wpa_cfg.pairwise_cipher_wpa2 = CIPHER_AES_CCMP;
+                ap_cfg->wpa_cfg.group_cipher = CIPHER_AES_CCMP;
+            } else if (!strnicmp(value, "wpa-psk", strlen("wpa-psk"))) {
+                ap_cfg->auth_mode = MLAN_AUTH_MODE_OPEN;
+                ap_cfg->protocol = PROTOCOL_WPA;
+                ap_cfg->key_mgmt = KEY_MGMT_PSK;
+                ap_cfg->wpa_cfg.pairwise_cipher_wpa = CIPHER_TKIP;
+                ap_cfg->wpa_cfg.group_cipher = CIPHER_TKIP;
+            } else if (!strnicmp(value, "wep128", strlen("wep128"))) {
+                ap_cfg->auth_mode = MLAN_AUTH_MODE_OPEN;
+                if (set_key)
+                    ap_cfg->wpa_cfg.length = 0;
+                ap_cfg->key_mgmt = KEY_MGMT_NONE;
+                ap_cfg->protocol = PROTOCOL_STATIC_WEP;
+            } else {
+                PRINTM(MERROR, "AP_CFG: Invalid value=%s for %s\n", value, opt);
+                ret = -EFAULT;
+                goto done;
+            }
+            set_sec = 1;
+        } else if (!strncmp(opt, "KEY", strlen("KEY"))) {
+            if (set_key) {
+                PRINTM(MWARN, "Skipping KEY, found again!\n");
+                continue;
+            }
+            if (set_sec && ap_cfg->protocol == PROTOCOL_STATIC_WEP) {
+                if (strlen(value) != MAX_WEP_KEY_SIZE) {
+                    PRINTM(MERROR, "Invalid WEP KEY length\n");
+                    ret = -EFAULT;
+                    goto done;
+                }
+                ap_cfg->wep_cfg.key0.key_index = 0;
+                ap_cfg->wep_cfg.key0.is_default = 1;
+                ap_cfg->wep_cfg.key0.length = strlen(value);
+                memcpy(ap_cfg->wep_cfg.key0.key, value, strlen(value));
+                set_key = 1;
+                continue;
+            }
+            if (set_sec && ap_cfg->protocol != PROTOCOL_WPA2
+                && ap_cfg->protocol != PROTOCOL_WPA) {
+                PRINTM(MWARN, "Warning! No KEY for open mode\n");
+                set_key = 1;
+                continue;
+            }
+            if (strlen(value) < MLAN_MIN_PASSPHRASE_LENGTH ||
+                strlen(value) > MLAN_PMK_HEXSTR_LENGTH) {
+                PRINTM(MERROR, "Invalid PSK/PMK length\n");
+                ret = -EINVAL;
+                goto done;
+            }
+            ap_cfg->wpa_cfg.length = strlen(value);
+            memcpy(ap_cfg->wpa_cfg.passphrase, value, strlen(value));
+            set_key = 1;
+        } else if (!strncmp(opt, "CHANNEL", strlen("CHANNEL"))) {
+            if (set_chan) {
+                PRINTM(MWARN, "Skipping CHANNEL, found again!\n");
+                continue;
+            }
+            if (woal_atoi(&atoi_ret, value)) {
+                ret = -EINVAL;
+                goto done;
+            }
+            if (atoi_ret < 1 || atoi_ret > MLAN_MAX_CHANNEL) {
+                PRINTM(MERROR, "AP_CFG: Channel must be between 1 and %d"
+                       "(both included)\n", MLAN_MAX_CHANNEL);
+                ret = -EINVAL;
+                goto done;
+            }
+            ap_cfg->channel = atoi_ret;
+            set_chan = 1;
+        } else if (!strncmp(opt, "PREAMBLE", strlen("PREAMBLE"))) {
+            if (set_preamble) {
+                PRINTM(MWARN, "Skipping PREAMBLE, found again!\n");
+                continue;
+            }
+            if (woal_atoi(&atoi_ret, value)) {
+                ret = -EINVAL;
+                goto done;
+            }
+            /* This is a READ only value from FW, so we can not set this and
+               pass it successfully */
+            set_preamble = 1;
+        } else if (!strncmp(opt, "MAX_SCB", strlen("MAX_SCB"))) {
+            if (set_scb) {
+                PRINTM(MWARN, "Skipping MAX_SCB, found again!\n");
+                continue;
+            }
+            if (woal_atoi(&atoi_ret, value)) {
+                ret = -EINVAL;
+                goto done;
+            }
+            if (atoi_ret < 1 || atoi_ret > MAX_STA_COUNT) {
+                PRINTM(MERROR, "AP_CFG: MAX_SCB must be between 1 to %d "
+                       "(both included)\n", MAX_STA_COUNT);
+                ret = -EINVAL;
+                goto done;
+            }
+            ap_cfg->max_sta_count = (t_u16) atoi_ret;
+            set_scb = 1;
+        } else {
+            PRINTM(MERROR, "Invalid option %s\n", opt);
+            ret = -EINVAL;
+            goto done;
+        }
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set AP configuration
+ *
+ *  @param priv     A pointer to moal_private structure
+ *  @param data     A pointer to user data
+ *  @param len      Length of buf
+ *
+ *  @return         0 --success, otherwise fail
+ */
+int
+woal_uap_set_ap_cfg(moal_private * priv, t_u8 * data, int len)
+{
+    int ret = 0;
+    static t_s8 buf[MAX_BUF_LEN];
+    mlan_uap_bss_param sys_config;
+    int restart = 0;
+
+    ENTER();
+
+#define MIN_AP_CFG_CMD_LEN   16 /* strlen("ASCII_CMD=AP_CFG") */
+    if ((len - 1) <= MIN_AP_CFG_CMD_LEN) {
+        PRINTM(MERROR, "Invalid length of command\n");
+        ret = -EINVAL;
+        goto done;
+    }
+
+    memset(buf, 0, MAX_BUF_LEN);
+    memcpy(buf, data, len);
+
+    /* Initialize the uap bss values which are uploaded from firmware */
+    woal_uap_get_bss_param(priv, &sys_config, MOAL_IOCTL_WAIT);
+
+    /* Setting the default values */
+    sys_config.channel = 6;
+    sys_config.preamble_type = 0;
+
+    if ((ret = woal_uap_ap_cfg_parse_data(&sys_config, buf)))
+        goto done;
+
+    /* If BSS already started stop it first and restart after changing the
+       setting */
+    if (priv->bss_started == MTRUE) {
+        if ((ret = woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_STOP)))
+            goto done;
+        restart = 1;
+    }
+
+    /* If the security mode is configured as WEP or WPA-PSK, it will disable
+       11n automatically, and if configured as open(off) or wpa2-psk, it will
+       automatically enable 11n */
+    if ((sys_config.protocol == PROTOCOL_STATIC_WEP) ||
+        (sys_config.protocol == PROTOCOL_WPA)) {
+        if (MLAN_STATUS_SUCCESS !=
+            woal_uap_set_11n_status(&sys_config, MLAN_ACT_DISABLE)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    } else {
+        if (MLAN_STATUS_SUCCESS !=
+            woal_uap_set_11n_status(&sys_config, MLAN_ACT_ENABLE)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_sys_config(priv, MLAN_ACT_SET, MOAL_IOCTL_WAIT,
+                                &sys_config)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Start the BSS after successful configuration */
+    if (restart)
+        ret = woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_START);
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief uap BSS control ioctl handler
+ *
+ *  @param priv             A pointer to moal_private structure
+ *  @param wait_option      Wait option
+ *  @param data             BSS control type
+ *  @return                 0 --success, otherwise fail
+ */
+int
+woal_uap_bss_ctrl(moal_private * priv, t_u8 wait_option, int data)
+{
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_bss *bss = NULL;
+    int ret = 0;
+
+    ENTER();
+
+    PRINTM(MIOCTL, "ioctl bss ctrl=%d\n", data);
+    if ((data != UAP_BSS_START) && (data != UAP_BSS_STOP) &&
+        (data != UAP_BSS_RESET)) {
+        PRINTM(MERROR, "Invalid parameter: %d\n", data);
+        ret = -EINVAL;
+        goto done;
+    }
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    bss = (mlan_ds_bss *) req->pbuf;
+    switch (data) {
+    case UAP_BSS_START:
+        if (priv->bss_started == MTRUE) {
+            PRINTM(MWARN, "Warning: BSS already started!\n");
+            // goto done;
+        } else {
+            /* about to start bss: issue channel check */
+            woal_11h_channel_check_ioctl(priv);
+        }
+        bss->sub_command = MLAN_OID_BSS_START;
+        break;
+    case UAP_BSS_STOP:
+        if (priv->bss_started == MFALSE) {
+            PRINTM(MWARN, "Warning: BSS already stopped!\n");
+            // goto done;
+        }
+        bss->sub_command = MLAN_OID_BSS_STOP;
+        break;
+    case UAP_BSS_RESET:
+        bss->sub_command = MLAN_OID_UAP_BSS_RESET;
+        woal_cancel_cac_block(priv);
+        break;
+    }
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = MLAN_ACT_SET;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (data == UAP_BSS_STOP || data == UAP_BSS_RESET)
+        priv->bss_started = MFALSE;
+
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief This function sets multicast addresses to firmware
+ *
+ *  @param dev     A pointer to net_device structure
+ *  @return        N/A
+ */
+void
+woal_uap_set_multicast_list(struct net_device *dev)
+{
+    ENTER();
+
+    LEAVE();
+}
+
+/**
+ *  @brief ioctl function - entry point
+ *
+ *  @param dev		A pointer to net_device structure
+ *  @param req	   	A pointer to ifreq structure
+ *  @param cmd 		Command
+ *
+ *  @return          0 --success, otherwise fail
+ */
+int
+woal_uap_do_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
+{
+    int ret = 0;
+    ENTER();
+    PRINTM(MIOCTL, "uap_do_ioctl: ioctl cmd = 0x%x\n", cmd);
+    switch (cmd) {
+    case UAP_HOSTCMD:
+        ret = woal_hostcmd_ioctl(dev, req);
+        break;
+    case UAP_IOCTL_CMD:
+        ret = woal_uap_ioctl(dev, req);
+        break;
+    case UAP_POWER_MODE:
+        ret = woal_uap_power_mode_ioctl(dev, req);
+        break;
+    case UAP_BSS_CTRL:
+        ret = woal_uap_bss_ctrl_ioctl(dev, req);
+        break;
+    case UAP_WAPI_MSG:
+        ret = woal_uap_set_wapi(dev, req);
+        break;
+    case UAP_BSS_CONFIG:
+        ret = woal_uap_bss_cfg_ioctl(dev, req);
+        break;
+    case UAP_STA_DEAUTH:
+        ret = woal_uap_sta_deauth_ioctl(dev, req);
+        break;
+    case UAP_RADIO_CTL:
+        ret = woal_uap_radio_ctl(dev, req);
+        break;
+    case UAP_GET_STA_LIST:
+        ret = woal_uap_get_sta_list_ioctl(dev, req);
+        break;
+    case UAP_CUSTOM_IE:
+        ret = woal_custom_ie_ioctl(dev, req);
+        break;
+    case UAP_GET_BSS_TYPE:
+        ret = woal_get_bss_type(dev, req);
+        break;
+    case WOAL_ANDROID_PRIV_CMD:
+        ret = woal_android_priv_cmd(dev, req);
+        break;
+    default:
+#ifdef UAP_WEXT
+        ret = woal_uap_do_priv_ioctl(dev, req, cmd);
+#else
+        ret = -EOPNOTSUPP;
+#endif
+        break;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get version
+ *
+ *  @param priv 		A pointer to moal_private structure
+ *  @param version		A pointer to version buffer
+ *  @param max_len		max length of version buffer
+ *
+ *  @return 	   		N/A
+ */
+void
+woal_uap_get_version(moal_private * priv, char *version, int max_len)
+{
+    mlan_ds_get_info *info = NULL;
+    mlan_ioctl_req *req = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_get_info));
+    if (req == NULL) {
+        LEAVE();
+        return;
+    }
+
+    info = (mlan_ds_get_info *) req->pbuf;
+    info->sub_command = MLAN_OID_GET_VER_EXT;
+    req->req_id = MLAN_IOCTL_GET_INFO;
+    req->action = MLAN_ACT_GET;
+
+    status = woal_request_ioctl(priv, req, MOAL_PROC_WAIT);
+    if (status == MLAN_STATUS_SUCCESS) {
+        PRINTM(MINFO, "MOAL UAP VERSION: %s\n",
+               info->param.ver_ext.version_str);
+        snprintf(version, max_len, driver_version,
+                 info->param.ver_ext.version_str);
+    }
+
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+
+    LEAVE();
+    return;
+}
+
+/**
+ *  @brief Get uap statistics
+ *
+ *  @param priv 		        A pointer to moal_private structure
+ *  @param wait_option          Wait option
+ *  @param ustats               A pointer to mlan_ds_uap_stats structure
+ *
+ *  @return                     MLAN_STATUS_SUCCESS/MLAN_STATUS_PENDING -- success, otherwise fail
+ */
+mlan_status
+woal_uap_get_stats(moal_private * priv, t_u8 wait_option,
+                   mlan_ds_uap_stats * ustats)
+{
+    mlan_ds_get_info *info = NULL;
+    mlan_ioctl_req *req = NULL;
+    mlan_status status = MLAN_STATUS_SUCCESS;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_get_info));
+    if (req == NULL) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    info = (mlan_ds_get_info *) req->pbuf;
+    info->sub_command = MLAN_OID_GET_STATS;
+    req->req_id = MLAN_IOCTL_GET_INFO;
+    req->action = MLAN_ACT_GET;
+
+    status = woal_request_ioctl(priv, req, wait_option);
+    if (status == MLAN_STATUS_SUCCESS) {
+        if (ustats)
+            memcpy(ustats, &info->param.ustats, sizeof(mlan_ds_uap_stats));
+#ifdef UAP_WEXT
+        priv->w_stats.discard.fragment = info->param.ustats.fcs_error_count;
+        priv->w_stats.discard.retries = info->param.ustats.retry_count;
+        priv->w_stats.discard.misc = info->param.ustats.ack_failure_count;
+#endif
+    }
+
+    if (req && (status != MLAN_STATUS_PENDING))
+        kfree(req);
+
+    LEAVE();
+    return status;
+}
+
+/**
+ *  @brief Set/Get system configuration parameters
+ *
+ *  @param priv 		    A pointer to moal_private structure
+ *  @param action           MLAN_ACT_SET or MLAN_ACT_GET
+ *  @param wait_option      Wait option
+ *  @param sys_cfg          A pointer to mlan_uap_bss_param structure
+ *
+ *  @return                 MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_set_get_sys_config(moal_private * priv, t_u16 action, t_u8 wait_option,
+                        mlan_uap_bss_param * sys_cfg)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    mlan_ds_bss *bss = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_UAP_BSS_CONFIG;
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = action;
+
+    if (action == MLAN_ACT_SET) {
+        memcpy(&bss->param.bss_config, sys_cfg, sizeof(mlan_uap_bss_param));
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, wait_option)) {
+        ret = MLAN_STATUS_FAILURE;
+        goto done;
+    }
+
+    if (action == MLAN_ACT_GET) {
+        memcpy(sys_cfg, &bss->param.bss_config, sizeof(mlan_uap_bss_param));
+    }
+
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set invalid data for each member of mlan_uap_bss_param
+ *  structure
+ *
+ *  @param config   A pointer to mlan_uap_bss_param structure
+ *
+ *  @return         N/A
+ */
+void
+woal_set_sys_config_invalid_data(mlan_uap_bss_param * config)
+{
+    ENTER();
+
+    memset(config, 0, sizeof(mlan_uap_bss_param));
+    config->bcast_ssid_ctl = 0x7F;
+    config->radio_ctl = 0x7F;
+    config->dtim_period = 0x7F;
+    config->beacon_period = 0x7FFF;
+    config->tx_data_rate = 0x7FFF;
+    config->mcbc_data_rate = 0x7FFF;
+    config->tx_power_level = 0x7F;
+    config->tx_antenna = 0x7F;
+    config->rx_antenna = 0x7F;
+    config->pkt_forward_ctl = 0x7F;
+    config->max_sta_count = 0x7FFF;
+    config->auth_mode = 0x7F;
+    config->sta_ageout_timer = 0x7FFFFFFF;
+    config->pairwise_update_timeout = 0x7FFFFFFF;
+    config->pwk_retries = 0x7FFFFFFF;
+    config->groupwise_update_timeout = 0x7FFFFFFF;
+    config->gwk_retries = 0x7FFFFFFF;
+    config->mgmt_ie_passthru_mask = 0x7FFFFFFF;
+    config->ps_sta_ageout_timer = 0x7FFFFFFF;
+    config->rts_threshold = 0x7FFF;
+    config->frag_threshold = 0x7FFF;
+    config->retry_limit = 0x7FFF;
+    config->filter.filter_mode = 0x7FFF;
+    config->filter.mac_count = 0x7FFF;
+    config->wpa_cfg.rsn_protection = 0x7F;
+    config->wpa_cfg.gk_rekey_time = 0x7FFFFFFF;
+    config->enable_2040coex = 0x7F;
+    config->wmm_para.qos_info = 0x7F;
+
+    LEAVE();
+}
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_uap.h b/drivers/net/wireless/sd8797/mlinux/moal_uap.h
new file mode 100644
index 000000000000..2667db84b8de
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_uap.h
@@ -0,0 +1,410 @@
+/** @file moal_uap.h
+  *
+  * @brief This file contains uap driver specific defines etc.
+  * 
+  * Copyright (C) 2009-2011, Marvell International Ltd.  
+  *
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+
+/********************************************************
+Change log:
+    02/02/2009: initial version
+********************************************************/
+
+#ifndef _MOAL_UAP_H
+#define _MOAL_UAP_H
+
+/** Maximum buffer length for WOAL_UAP_SET_GET_256_CHAR */
+#define MAX_BUF_LEN                 256
+
+/** Private command ID to Host command */
+#define	UAP_HOSTCMD			(SIOCDEVPRIVATE + 1)
+/** Private command ID to send ioctl */
+#define UAP_IOCTL_CMD		(SIOCDEVPRIVATE + 2)
+/** Updating ADDBA variables */
+#define UAP_ADDBA_PARA		0
+/** Updating priority table for AMPDU/AMSDU */
+#define UAP_AGGR_PRIOTBL    1
+/** Updating addbareject table */
+#define UAP_ADDBA_REJECT    2
+/** Get FW INFO */
+#define UAP_FW_INFO         4
+/** Updating Deep sleep variables */
+#define UAP_DEEP_SLEEP      3
+/** Tx data pause subcommand */
+#define UAP_TX_DATA_PAUSE    5
+/** sdcmd52 read write subcommand */
+#define UAP_SDCMD52_RW      6
+/** snmp mib subcommand */
+#define UAP_SNMP_MIB        7
+/** domain info subcommand */
+#define UAP_DOMAIN_INFO     8
+/** TX beamforming configuration */
+#define UAP_TX_BF_CFG       9
+#ifdef DFS_TESTING_SUPPORT
+/** dfs testing subcommand */
+#define UAP_DFS_TESTING     10
+#endif
+/** sub command ID to set/get Host Sleep configuration */
+#define UAP_HS_CFG          11
+/** sub command ID to set/get Host Sleep Parameters */
+#define UAP_HS_SET_PARA     12
+
+/** Management Frame Control Mask */
+#define UAP_MGMT_FRAME_CONTROL  13
+
+#define UAP_TX_RATE_CFG         14
+
+/** Subcommand ID to set/get antenna configuration */
+#define UAP_ANTENNA_CFG         15
+
+/** Private command ID to Power Mode */
+#define	UAP_POWER_MODE			(SIOCDEVPRIVATE + 3)
+
+/** Private command id to start/stop/reset bss */
+#define UAP_BSS_CTRL        (SIOCDEVPRIVATE + 4)
+/** BSS START */
+#define UAP_BSS_START               0
+/** BSS STOP */
+#define UAP_BSS_STOP                1
+/** BSS RESET */
+#define UAP_BSS_RESET               2
+/** Band config 5GHz */
+#define BAND_CONFIG_5GHZ            0x01
+
+/** wapi_msg */
+typedef struct _wapi_msg
+{
+    /** message type */
+    t_u16 msg_type;
+    /** message len */
+    t_u16 msg_len;
+    /** message */
+    t_u8 msg[96];
+} wapi_msg;
+
+/* wapi key msg */
+typedef struct _wapi_key_msg
+{
+    /** mac address */
+    t_u8 mac_addr[MLAN_MAC_ADDR_LENGTH];
+    /** pad */
+    t_u8 pad;
+    /** key id */
+    t_u8 key_id;
+    /** key */
+    t_u8 key[32];
+} wapi_key_msg;
+
+typedef struct _tx_rate_cfg_t
+{
+    /** sub command */
+    int subcmd;
+    /** Action */
+    int action;
+    /** Rate configured */
+    int rate;
+    /** Rate bitmap */
+    t_u16 bitmap_rates[MAX_BITMAP_RATES_SIZE];
+} tx_rate_cfg_t;
+
+/** ant_cfg structure */
+typedef struct _ant_cfg_t
+{
+   /** Subcommand */
+    int subcmd;
+   /** Action */
+    int action;
+   /** TX mode configured */
+    int tx_mode;
+   /** RX mode configured */
+    int rx_mode;
+} ant_cfg_t;
+
+/** Private command ID to set wapi info */
+#define	UAP_WAPI_MSG		(SIOCDEVPRIVATE + 10)
+/** set wapi flag */
+#define  P80211_PACKET_WAPIFLAG     0x0001
+/** set wapi key */
+#define  P80211_PACKET_SETKEY      0x0003
+/** wapi mode psk */
+#define WAPI_MODE_PSK    0x04
+/** wapi mode certificate */
+#define WAPI_MODE_CERT   0x08
+
+/** radio control command */
+#define	UAP_RADIO_CTL               (SIOCDEVPRIVATE + 5)
+
+/** Private command ID to BSS config */
+#define	UAP_BSS_CONFIG              (SIOCDEVPRIVATE + 6)
+
+/** deauth station */
+#define	UAP_STA_DEAUTH	            (SIOCDEVPRIVATE + 7)
+
+/** uap get station list */
+#define UAP_GET_STA_LIST            (SIOCDEVPRIVATE + 11)
+
+/** Private command ID to set/get custom IE buffer */
+#define	UAP_CUSTOM_IE               (SIOCDEVPRIVATE + 13)
+
+/** HS WAKE UP event id */
+#define UAP_EVENT_ID_HS_WAKEUP             0x80000001
+/** HS_ACTIVATED event id */
+#define UAP_EVENT_ID_DRV_HS_ACTIVATED      0x80000002
+/** HS DEACTIVATED event id */
+#define UAP_EVENT_ID_DRV_HS_DEACTIVATED    0x80000003
+
+/** Host sleep flag set */
+#define HS_CFG_FLAG_GET         0
+/** Host sleep flag get */
+#define HS_CFG_FLAG_SET         1
+/** Host sleep flag for condition */
+#define HS_CFG_FLAG_CONDITION   2
+/** Host sleep flag for GPIO */
+#define HS_CFG_FLAG_GPIO        4
+/** Host sleep flag for Gap */
+#define HS_CFG_FLAG_GAP         8
+/** Host sleep flag for all */
+#define HS_CFG_FLAG_ALL         0x0f
+/** Host sleep mask to get condition */
+#define HS_CFG_CONDITION_MASK   0x0f
+
+/** ds_hs_cfg */
+typedef struct _ds_hs_cfg
+{
+    /** subcmd */
+    t_u32 subcmd;
+    /** Bit0: 0 - Get, 1 Set
+     *  Bit1: 1 - conditions is valid
+     *  Bit2: 2 - gpio is valid
+     *  Bit3: 3 - gap is valid
+     */
+    t_u32 flags;
+    /** Host sleep config condition */
+    /** Bit0: non-unicast data
+     *  Bit1: unicast data
+     *  Bit2: mac events
+     *  Bit3: magic packet
+     */
+    t_u32 conditions;
+    /** GPIO */
+    t_u32 gpio;
+    /** Gap in milliseconds */
+    t_u32 gap;
+} ds_hs_cfg;
+
+/** Private command ID to get BSS type */
+#define	UAP_GET_BSS_TYPE            (SIOCDEVPRIVATE + 15)
+
+/** addba_param */
+typedef struct _addba_param
+{
+    /** subcmd */
+    t_u32 subcmd;
+    /** Set/Get */
+    t_u32 action;
+    /** block ack timeout for ADDBA request */
+    t_u32 timeout;
+    /** Buffer size for ADDBA request */
+    t_u32 txwinsize;
+    /** Buffer size for ADDBA response */
+    t_u32 rxwinsize;
+    /** amsdu for ADDBA request */
+    t_u8 txamsdu;
+    /** amsdu for ADDBA response */
+    t_u8 rxamsdu;
+} addba_param;
+
+/** aggr_prio_tbl */
+typedef struct _aggr_prio_tbl
+{
+    /** subcmd */
+    t_u32 subcmd;
+    /** Set/Get */
+    t_u32 action;
+    /** ampdu priority table */
+    t_u8 ampdu[MAX_NUM_TID];
+    /** amsdu priority table */
+    t_u8 amsdu[MAX_NUM_TID];
+} aggr_prio_tbl;
+
+/** addba_reject parameters */
+typedef struct _addba_reject_para
+{
+    /** subcmd */
+    t_u32 subcmd;
+    /** Set/Get */
+    t_u32 action;
+    /** BA Reject paramters */
+    t_u8 addba_reject[MAX_NUM_TID];
+} addba_reject_para;
+
+/** fw_info */
+typedef struct _fw_info
+{
+    /** subcmd */
+    t_u32 subcmd;
+    /** Get */
+    t_u32 action;
+    /** Firmware release number */
+    t_u32 fw_release_number;
+    /** Device support for MIMO abstraction of MCSs */
+    t_u8 hw_dev_mcs_support;
+} fw_info;
+
+typedef struct _tx_bf_cfg_para_hdr
+{
+    /** Sub command */
+    t_u32 subcmd;
+    /** Action: Set/Get */
+    t_u32 action;
+} tx_bf_cfg_para_hdr;
+
+/** sdcmd52rw parameters */
+typedef struct _sdcmd52_para
+{
+    /** subcmd */
+    t_u32 subcmd;
+    /** Write /Read */
+    t_u32 action;
+    /** Command 52 paramters */
+    t_u8 cmd52_params[3];
+} sdcmd52_para;
+
+/** deep_sleep parameters */
+typedef struct _deep_sleep_para
+{
+    /** subcmd */
+    t_u32 subcmd;
+    /** Set/Get */
+    t_u32 action;
+    /** enable/disable deepsleep*/
+    t_u16 deep_sleep;
+    /** idle_time */
+    t_u16 idle_time;
+} deep_sleep_para;
+
+/** tx_data_pause parameters */
+typedef struct _tx_data_pause_para
+{
+    /** subcmd */
+    t_u32 subcmd;
+    /** Set/Get */
+    t_u32 action;
+    /** enable/disable Tx data pause*/
+    t_u16 txpause;
+    /** Max number of TX buffer allowed for all PS client*/
+    t_u16 txbufcnt;
+} tx_data_pause_para;
+
+/** mgmt_frame_ctrl */
+typedef struct _mgmt_frame_ctrl
+{
+    /** subcmd */
+    t_u32 subcmd;
+    /** Set/Get */
+    t_u32 action;
+    /** mask */
+    t_u32 mask;
+} mgmt_frame_ctrl;
+
+typedef struct _snmp_mib_para
+{
+    /** subcmd */
+    t_u32 subcmd;
+    /** Set/Get */
+    t_u32 action;
+    /** oid to set/get */
+    t_u16 oid;
+    /** length of oid value */
+    t_u16 oid_val_len;
+    /** oid value to set/get */
+    t_u8 oid_value[0];
+} snmp_mib_para;
+
+/** Max length for oid_value field */
+#define MAX_SNMP_VALUE_SIZE         128
+
+/** Oid for 802.11D enable/disable */
+#define OID_80211D_ENABLE           0x0009
+/** Oid for 802.11H enable/disable */
+#define OID_80211H_ENABLE           0x000a
+
+/** domain_info parameters */
+typedef struct _domain_info_param
+{
+    /** subcmd */
+    t_u32 subcmd;
+    /** Set/Get */
+    t_u32 action;
+    /** domain_param TLV (incl. header) */
+    t_u8 tlv[0];
+} domain_info_para;
+
+/** DOMAIN_INFO param sizes */
+#define TLV_HEADER_LEN          (2 + 2)
+#define SUB_BAND_LEN            3
+#define MAX_SUB_BANDS           40
+
+/** MAX domain TLV length */
+#define MAX_DOMAIN_TLV_LEN      (TLV_HEADER_LEN + COUNTRY_CODE_LEN \
+                                 + (SUB_BAND_LEN * MAX_SUB_BANDS))
+
+#ifdef DFS_TESTING_SUPPORT
+/** dfs_testing parameters */
+typedef struct _dfs_testing_param
+{
+    /** subcmd */
+    t_u32 subcmd;
+    /** Set/Get */
+    t_u32 action;
+    /** user CAC period (msec) */
+    t_u16 usr_cac_period;
+    /** user NOP period (sec) */
+    t_u16 usr_nop_period;
+    /** don't change channel on radar */
+    t_u8 no_chan_change;
+    /** fixed channel to change to on radar */
+    t_u8 fixed_new_chan;
+} dfs_testing_para;
+#endif
+
+void woal_uap_set_multicast_list(struct net_device *dev);
+int woal_uap_do_ioctl(struct net_device *dev, struct ifreq *req, int cmd);
+int woal_uap_bss_ctrl(moal_private * priv, t_u8 wait_option, int data);
+void woal_uap_get_version(moal_private * priv, char *version, int max_len);
+mlan_status woal_uap_get_stats(moal_private * priv, t_u8 wait_option,
+                               mlan_ds_uap_stats * ustats);
+#if defined(UAP_WEXT) || defined(UAP_CFG80211)
+extern struct iw_handler_def woal_uap_handler_def;
+struct iw_statistics *woal_get_uap_wireless_stats(struct net_device *dev);
+/** IOCTL function for wireless private IOCTLs */
+int woal_uap_do_priv_ioctl(struct net_device *dev, struct ifreq *req, int cmd);
+#endif
+/** Set invalid data for each member of mlan_uap_bss_param */
+void woal_set_sys_config_invalid_data(mlan_uap_bss_param * config);
+/** Set/Get system configuration parameters */
+mlan_status woal_set_get_sys_config(moal_private * priv,
+                                    t_u16 action, t_u8 wait_option,
+                                    mlan_uap_bss_param * sys_cfg);
+int woal_uap_set_ap_cfg(moal_private * priv, t_u8 * data, int len);
+mlan_status woal_uap_set_11n_status(mlan_uap_bss_param * sys_cfg, t_u8 action);
+#ifdef UAP_WEXT
+void woal_ioctl_get_uap_info_resp(moal_private * priv, mlan_ds_get_info * info);
+int woal_set_get_custom_ie(moal_private * priv, t_u16 mask, t_u8 * ie,
+                           int ie_len);
+#endif /* UAP_WEXT */
+#endif /* _MOAL_UAP_H */
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_uap_cfg80211.c b/drivers/net/wireless/sd8797/mlinux/moal_uap_cfg80211.c
new file mode 100644
index 000000000000..18d18328d0ed
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_uap_cfg80211.c
@@ -0,0 +1,1288 @@
+/** @file moal_uap_cfg80211.c
+  *
+  * @brief This file contains the functions for uAP CFG80211. 
+  *
+  * Copyright (C) 2011-2012, Marvell International Ltd. 
+  * 
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+
+#include "moal_cfg80211.h"
+#include "moal_uap_cfg80211.h"
+
+/* these 3 function will be called in woal_cfg80211_wifi_direct_ops */
+int woal_cfg80211_add_beacon(struct wiphy *wiphy,
+                             struct net_device *dev,
+                             struct beacon_parameters *params);
+
+int woal_cfg80211_set_beacon(struct wiphy *wiphy,
+                             struct net_device *dev,
+                             struct beacon_parameters *params);
+
+int woal_cfg80211_del_beacon(struct wiphy *wiphy, struct net_device *dev);
+
+static int woal_uap_cfg80211_scan(struct wiphy *wiphy, struct net_device *dev,
+                                  struct cfg80211_scan_request *request);
+
+static int woal_uap_cfg80211_connect(struct wiphy *wiphy,
+                                     struct net_device *dev,
+                                     struct cfg80211_connect_params *sme);
+
+static int woal_uap_cfg80211_disconnect(struct wiphy *wiphy,
+                                        struct net_device *dev,
+                                        t_u16 reason_code);
+
+int woal_uap_cfg80211_get_station(struct wiphy *wiphy, struct net_device *dev,
+                                  u8 * mac, struct station_info *stainfo);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37) || defined(COMPAT_WIRELESS)
+static const struct ieee80211_txrx_stypes
+    ieee80211_uap_mgmt_stypes[NUM_NL80211_IFTYPES] = {
+    [NL80211_IFTYPE_ADHOC] = {
+                              .tx = 0x0000,
+                              .rx = 0x0000,
+                              },
+    [NL80211_IFTYPE_STATION] = {
+                                .tx = BIT(IEEE80211_STYPE_ACTION >> 4),
+                                .rx = BIT(IEEE80211_STYPE_ACTION >> 4),
+                                },
+    [NL80211_IFTYPE_AP] = {
+                           .tx = 0xffff,
+                           .rx = BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
+                           BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
+                           BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
+                           BIT(IEEE80211_STYPE_DISASSOC >> 4) |
+                           BIT(IEEE80211_STYPE_AUTH >> 4) |
+                           BIT(IEEE80211_STYPE_DEAUTH >> 4) |
+                           BIT(IEEE80211_STYPE_ACTION >> 4),
+                           },
+    [NL80211_IFTYPE_AP_VLAN] = {
+                                .tx = 0x0000,
+                                .rx = 0x0000,
+                                },
+    [NL80211_IFTYPE_P2P_CLIENT] = {
+                                   .tx = 0x0000,
+                                   .rx = 0x0000,
+                                   },
+    [NL80211_IFTYPE_P2P_GO] = {
+                               .tx = 0x0000,
+                               .rx = 0x0000,
+                               },
+    [NL80211_IFTYPE_MESH_POINT] = {
+                                   .tx = 0x0000,
+                                   .rx = 0x0000,
+                                   },
+};
+#endif
+
+/** cfg80211 uAP operations */
+static struct cfg80211_ops woal_cfg80211_uap_ops = {
+    .add_virtual_intf = woal_cfg80211_add_virtual_intf,
+    .del_virtual_intf = woal_cfg80211_del_virtual_intf,
+    .set_channel = woal_cfg80211_set_channel,
+    .scan = woal_uap_cfg80211_scan,
+    .connect = woal_uap_cfg80211_connect,
+    .disconnect = woal_uap_cfg80211_disconnect,
+    .set_wiphy_params = woal_cfg80211_set_wiphy_params,
+    .change_virtual_intf = woal_cfg80211_change_virtual_intf,
+    .add_key = woal_cfg80211_add_key,
+    .del_key = woal_cfg80211_del_key,
+    .set_default_key = woal_cfg80211_set_default_key,
+    .add_beacon = woal_cfg80211_add_beacon,
+    .set_beacon = woal_cfg80211_set_beacon,
+    .del_beacon = woal_cfg80211_del_beacon,
+    .get_station = woal_uap_cfg80211_get_station,
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37) || defined(COMPAT_WIRELESS)
+    .mgmt_frame_register = woal_cfg80211_mgmt_frame_register,
+    .mgmt_tx = woal_cfg80211_mgmt_tx,
+#endif
+};
+
+/********************************************************
+				Local Variables
+********************************************************/
+
+/********************************************************
+				Global Variables
+********************************************************/
+
+/********************************************************
+				Local Functions
+********************************************************/
+
+/********************************************************
+				Global Functions
+********************************************************/
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3,2,0) && !defined(COMPAT_WIRELESS)
+/**
+ * @brief Verify RSN IE
+ *
+ * @param rsn_ie          Pointer RSN IE
+ * @param sys_config      Pointer to mlan_uap_bss_param structure
+ *
+ * @return                MTRUE/MFALSE
+ */
+static t_u8
+woal_check_rsn_ie(IEEEtypes_Rsn_t * rsn_ie, mlan_uap_bss_param * sys_config)
+{
+    int left = 0;
+    int count = 0;
+    int i = 0;
+    wpa_suite_auth_key_mgmt_t *key_mgmt = NULL;
+    left = rsn_ie->len + 2;
+    if (left < sizeof(IEEEtypes_Rsn_t))
+        return MFALSE;
+    sys_config->wpa_cfg.group_cipher = 0;
+    sys_config->wpa_cfg.pairwise_cipher_wpa2 = 0;
+    /* check the group cipher */
+    switch (rsn_ie->group_cipher.type) {
+    case WPA_CIPHER_TKIP:
+        sys_config->wpa_cfg.group_cipher = CIPHER_TKIP;
+        break;
+    case WPA_CIPHER_AES_CCM:
+        sys_config->wpa_cfg.group_cipher = CIPHER_AES_CCMP;
+        break;
+    default:
+        break;
+    }
+    count = le16_to_cpu(rsn_ie->pairwise_cipher.count);
+    for (i = 0; i < count; i++) {
+        switch (rsn_ie->pairwise_cipher.list[i].type) {
+        case WPA_CIPHER_TKIP:
+            sys_config->wpa_cfg.pairwise_cipher_wpa2 |= CIPHER_TKIP;
+            break;
+        case WPA_CIPHER_AES_CCM:
+            sys_config->wpa_cfg.pairwise_cipher_wpa2 |= CIPHER_AES_CCMP;
+            break;
+        default:
+            break;
+        }
+    }
+    left -= sizeof(IEEEtypes_Rsn_t) + (count - 1) * sizeof(wpa_suite);
+    if (left < sizeof(wpa_suite_auth_key_mgmt_t))
+        return MFALSE;
+    key_mgmt =
+        (wpa_suite_auth_key_mgmt_t *) ((u8 *) rsn_ie + sizeof(IEEEtypes_Rsn_t) +
+                                       (count - 1) * sizeof(wpa_suite));
+    count = le16_to_cpu(key_mgmt->count);
+    if (left <
+        (sizeof(wpa_suite_auth_key_mgmt_t) + (count - 1) * sizeof(wpa_suite)))
+        return MFALSE;
+
+    for (i = 0; i < count; i++) {
+        switch (key_mgmt->list[i].type) {
+        case RSN_AKM_8021X:
+            sys_config->key_mgmt = KEY_MGMT_EAP;
+            break;
+        case RSN_AKM_PSK:
+            sys_config->key_mgmt = KEY_MGMT_PSK;
+            break;
+        }
+    }
+    return MTRUE;
+}
+
+/**
+ * @brief Verify WPA IE
+ *
+ * @param wpa_ie          Pointer WPA IE
+ * @param sys_config      Pointer to mlan_uap_bss_param structure
+ *
+ * @return                MTRUE/MFALSE
+ */
+static t_u8
+woal_check_wpa_ie(IEEEtypes_Wpa_t * wpa_ie, mlan_uap_bss_param * sys_config)
+{
+    int left = 0;
+    int count = 0;
+    int i = 0;
+    wpa_suite_auth_key_mgmt_t *key_mgmt = NULL;
+    left = wpa_ie->len + 2;
+    if (left < sizeof(IEEEtypes_Wpa_t))
+        return MFALSE;
+    sys_config->wpa_cfg.group_cipher = 0;
+    sys_config->wpa_cfg.pairwise_cipher_wpa = 0;
+    switch (wpa_ie->group_cipher.type) {
+    case WPA_CIPHER_TKIP:
+        sys_config->wpa_cfg.group_cipher = CIPHER_TKIP;
+        break;
+    case WPA_CIPHER_AES_CCM:
+        sys_config->wpa_cfg.group_cipher = CIPHER_AES_CCMP;
+        break;
+    default:
+        break;
+    }
+    count = le16_to_cpu(wpa_ie->pairwise_cipher.count);
+    for (i = 0; i < count; i++) {
+        switch (wpa_ie->pairwise_cipher.list[i].type) {
+        case WPA_CIPHER_TKIP:
+            sys_config->wpa_cfg.pairwise_cipher_wpa |= CIPHER_TKIP;
+            break;
+        case WPA_CIPHER_AES_CCM:
+            sys_config->wpa_cfg.pairwise_cipher_wpa |= CIPHER_AES_CCMP;
+            break;
+        default:
+            break;
+        }
+    }
+    left -= sizeof(IEEEtypes_Wpa_t) + (count - 1) * sizeof(wpa_suite);
+    if (left < sizeof(wpa_suite_auth_key_mgmt_t))
+        return MFALSE;
+    key_mgmt =
+        (wpa_suite_auth_key_mgmt_t *) ((u8 *) wpa_ie + sizeof(IEEEtypes_Wpa_t) +
+                                       (count - 1) * sizeof(wpa_suite));
+    count = le16_to_cpu(key_mgmt->count);
+    if (left <
+        (sizeof(wpa_suite_auth_key_mgmt_t) + (count - 1) * sizeof(wpa_suite)))
+        return MFALSE;
+    for (i = 0; i < count; i++) {
+        switch (key_mgmt->list[i].type) {
+        case RSN_AKM_8021X:
+            sys_config->key_mgmt = KEY_MGMT_EAP;
+            break;
+        case RSN_AKM_PSK:
+            sys_config->key_mgmt = KEY_MGMT_PSK;
+            break;
+        }
+    }
+    return MTRUE;
+}
+
+/**
+ * @brief Find RSN/WPA IES
+ *
+ * @param ie              Pointer IE buffer
+ * @param sys_config      Pointer to mlan_uap_bss_param structure
+ *
+ * @return                MTRUE/MFALSE
+ */
+static t_u8
+woal_find_wpa_ies(t_u8 * ie, int len, mlan_uap_bss_param * sys_config)
+{
+    int bytes_left = len;
+    t_u8 *pcurrent_ptr = ie;
+    t_u16 total_ie_len;
+    t_u8 element_len;
+    t_u8 wpa2 = 0;
+    t_u8 wpa = 0;
+    t_u8 ret = MFALSE;
+    IEEEtypes_ElementId_e element_id;
+    IEEEtypes_VendorSpecific_t *pvendor_ie;
+    const t_u8 wpa_oui[4] = { 0x00, 0x50, 0xf2, 0x01 };
+
+    while (bytes_left >= 2) {
+        element_id = (IEEEtypes_ElementId_e) (*((t_u8 *) pcurrent_ptr));
+        element_len = *((t_u8 *) pcurrent_ptr + 1);
+        total_ie_len = element_len + sizeof(IEEEtypes_Header_t);
+        if (bytes_left < total_ie_len) {
+            PRINTM(MERROR, "InterpretIE: Error in processing IE, "
+                   "bytes left < IE length\n");
+            bytes_left = 0;
+            continue;
+        }
+        switch (element_id) {
+        case RSN_IE:
+            wpa2 =
+                woal_check_rsn_ie((IEEEtypes_Rsn_t *) pcurrent_ptr, sys_config);
+            break;
+        case VENDOR_SPECIFIC_221:
+            pvendor_ie = (IEEEtypes_VendorSpecific_t *) pcurrent_ptr;
+            if (!memcmp(pvendor_ie->vend_hdr.oui, wpa_oui, sizeof(wpa_oui)))
+                wpa =
+                    woal_check_wpa_ie((IEEEtypes_Wpa_t *) pcurrent_ptr,
+                                      sys_config);
+            break;
+        default:
+            break;
+        }
+        pcurrent_ptr += element_len + 2;
+        /* Need to account for IE ID and IE Len */
+        bytes_left -= (element_len + 2);
+    }
+    if (wpa && wpa2) {
+        sys_config->protocol = PROTOCOL_WPA | PROTOCOL_WPA2;
+        ret = MTRUE;
+    } else if (wpa2) {
+        sys_config->protocol = PROTOCOL_WPA2;
+        ret = MTRUE;
+    } else if (wpa) {
+        sys_config->protocol = PROTOCOL_WPA;
+        ret = MTRUE;
+    }
+    return ret;
+}
+#endif
+
+/**
+ * @brief initialize AP or GO bss config
+ *
+ * @param priv            A pointer to moal private structure
+ * @param params          A pointer to beacon_parameters structure
+ * @return                0 -- success, otherwise fail
+ */
+static int
+woal_cfg80211_beacon_config(moal_private * priv,
+                            struct beacon_parameters *params)
+{
+    int ret = 0;
+    mlan_uap_bss_param sys_config;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0) || defined(COMPAT_WIRELESS)
+    int i = 0;
+#else
+    const t_u8 *ssid_ie = NULL;
+    struct ieee80211_mgmt *head = NULL;
+    t_u16 capab_info = 0;
+#endif
+
+    ENTER();
+
+    if (params == NULL) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (priv->bss_type != MLAN_BSS_TYPE_UAP
+#ifdef WIFI_DIRECT_SUPPORT
+        && priv->bss_type != MLAN_BSS_TYPE_WIFIDIRECT
+#endif
+        ) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Initialize the uap bss values which are uploaded from firmware */
+    if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                       MLAN_ACT_GET,
+                                                       MOAL_IOCTL_WAIT,
+                                                       &sys_config)) {
+        PRINTM(MERROR, "Error getting AP confiruration\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Setting the default values */
+    sys_config.channel = 6;
+    sys_config.preamble_type = 0;
+
+    if (priv->bss_type == MLAN_BSS_TYPE_UAP) {
+        if (params->interval)
+            sys_config.beacon_period = params->interval;
+        if (params->dtim_period)
+            sys_config.dtim_period = params->dtim_period;
+    }
+    if (priv->channel)
+        sys_config.channel = priv->channel;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0) || defined(COMPAT_WIRELESS)
+    if (!params->ssid || !params->ssid_len) {
+        ret = -EINVAL;
+        goto done;
+    }
+    memcpy(sys_config.ssid.ssid, params->ssid,
+           MIN(MLAN_MAX_SSID_LENGTH, params->ssid_len));
+    sys_config.ssid.ssid_len = MIN(MLAN_MAX_SSID_LENGTH, params->ssid_len);
+    if (params->hidden_ssid)
+        sys_config.bcast_ssid_ctl = 0;
+    else
+        sys_config.bcast_ssid_ctl = 1;
+    if (params->auth_type == NL80211_AUTHTYPE_SHARED_KEY)
+        sys_config.auth_mode = MLAN_AUTH_MODE_SHARED;
+    else
+        sys_config.auth_mode = MLAN_AUTH_MODE_OPEN;
+
+    for (i = 0; i < params->crypto.n_akm_suites; i++) {
+        switch (params->crypto.akm_suites[i]) {
+        case WLAN_AKM_SUITE_8021X:
+            sys_config.key_mgmt = KEY_MGMT_EAP;
+            if ((params->crypto.wpa_versions & NL80211_WPA_VERSION_1) &&
+                (params->crypto.wpa_versions & NL80211_WPA_VERSION_2))
+                sys_config.protocol = PROTOCOL_WPA | PROTOCOL_WPA2;
+            else if (params->crypto.wpa_versions & NL80211_WPA_VERSION_2)
+                sys_config.protocol = PROTOCOL_WPA2;
+            else if (params->crypto.wpa_versions & NL80211_WPA_VERSION_1)
+                sys_config.protocol = PROTOCOL_WPA;
+            break;
+        case WLAN_AKM_SUITE_PSK:
+            sys_config.key_mgmt = KEY_MGMT_PSK;
+            if ((params->crypto.wpa_versions & NL80211_WPA_VERSION_1) &&
+                (params->crypto.wpa_versions & NL80211_WPA_VERSION_2))
+                sys_config.protocol = PROTOCOL_WPA | PROTOCOL_WPA2;
+            else if (params->crypto.wpa_versions & NL80211_WPA_VERSION_2)
+                sys_config.protocol = PROTOCOL_WPA2;
+            else if (params->crypto.wpa_versions & NL80211_WPA_VERSION_1)
+                sys_config.protocol = PROTOCOL_WPA;
+            break;
+        }
+    }
+    sys_config.wpa_cfg.pairwise_cipher_wpa = 0;
+    sys_config.wpa_cfg.pairwise_cipher_wpa2 = 0;
+    for (i = 0; i < params->crypto.n_ciphers_pairwise; i++) {
+        switch (params->crypto.ciphers_pairwise[i]) {
+        case WLAN_CIPHER_SUITE_WEP40:
+        case WLAN_CIPHER_SUITE_WEP104:
+            break;
+        case WLAN_CIPHER_SUITE_TKIP:
+            if (params->crypto.wpa_versions & NL80211_WPA_VERSION_1)
+                sys_config.wpa_cfg.pairwise_cipher_wpa |= CIPHER_TKIP;
+            if (params->crypto.wpa_versions & NL80211_WPA_VERSION_2)
+                sys_config.wpa_cfg.pairwise_cipher_wpa2 |= CIPHER_TKIP;
+            break;
+        case WLAN_CIPHER_SUITE_CCMP:
+            if (params->crypto.wpa_versions & NL80211_WPA_VERSION_1)
+                sys_config.wpa_cfg.pairwise_cipher_wpa |= CIPHER_AES_CCMP;
+            if (params->crypto.wpa_versions & NL80211_WPA_VERSION_2)
+                sys_config.wpa_cfg.pairwise_cipher_wpa2 |= CIPHER_AES_CCMP;
+            break;
+        }
+    }
+    switch (params->crypto.cipher_group) {
+    case WLAN_CIPHER_SUITE_WEP40:
+    case WLAN_CIPHER_SUITE_WEP104:
+        if ((priv->cipher == WLAN_CIPHER_SUITE_WEP40) ||
+            (priv->cipher == WLAN_CIPHER_SUITE_WEP104)) {
+            sys_config.protocol = PROTOCOL_STATIC_WEP;
+            sys_config.key_mgmt = KEY_MGMT_NONE;
+            sys_config.wpa_cfg.length = 0;
+            sys_config.wep_cfg.key0.key_index = priv->key_index;
+            sys_config.wep_cfg.key0.is_default = 1;
+            sys_config.wep_cfg.key0.length = priv->key_len;
+            memcpy(sys_config.wep_cfg.key0.key, priv->key_material,
+                   priv->key_len);
+        }
+        break;
+    case WLAN_CIPHER_SUITE_TKIP:
+        sys_config.wpa_cfg.group_cipher = CIPHER_TKIP;
+        break;
+    case WLAN_CIPHER_SUITE_CCMP:
+        sys_config.wpa_cfg.group_cipher = CIPHER_AES_CCMP;
+        break;
+    }
+#else
+    /* Since in Android ICS 4.0.1's wpa_supplicant, there is no way to set ssid
+       when GO (AP) starts up, so get it from beacon head parameter TODO: right
+       now use hard code 24 -- ieee80211 header lenth, 12 -- fixed element
+       length for beacon */
+#define BEACON_IE_OFFSET	36
+    /* Find SSID in head SSID IE id: 0, right now use hard code */
+    ssid_ie = woal_parse_ie_tlv(params->head + BEACON_IE_OFFSET,
+                                params->head_len - BEACON_IE_OFFSET, 0);
+    if (!ssid_ie) {
+        PRINTM(MERROR, "No ssid IE found.\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    if (*(ssid_ie + 1) > 32) {
+        PRINTM(MERROR, "ssid len error: %d\n", *(ssid_ie + 1));
+        ret = -EFAULT;
+        goto done;
+    }
+    memcpy(sys_config.ssid.ssid, ssid_ie + 2, *(ssid_ie + 1));
+    sys_config.ssid.ssid_len = *(ssid_ie + 1);
+    head = (struct ieee80211_mgmt *) params->head;
+    capab_info = le16_to_cpu(head->u.beacon.capab_info);
+    PRINTM(MIOCTL, "capab_info=0x%x\n", head->u.beacon.capab_info);
+    sys_config.auth_mode = MLAN_AUTH_MODE_OPEN;
+        /** For ICS, we don't support OPEN mode */
+    if ((priv->cipher == WLAN_CIPHER_SUITE_WEP40) ||
+        (priv->cipher == WLAN_CIPHER_SUITE_WEP104)) {
+        sys_config.protocol = PROTOCOL_STATIC_WEP;
+        sys_config.key_mgmt = KEY_MGMT_NONE;
+        sys_config.wpa_cfg.length = 0;
+        sys_config.wep_cfg.key0.key_index = priv->key_index;
+        sys_config.wep_cfg.key0.is_default = 1;
+        sys_config.wep_cfg.key0.length = priv->key_len;
+        memcpy(sys_config.wep_cfg.key0.key, priv->key_material, priv->key_len);
+    } else {
+                /** Get cipher and key_mgmt from RSN/WPA IE */
+        if (capab_info & WLAN_CAPABILITY_PRIVACY) {
+            if (MFALSE ==
+                woal_find_wpa_ies(params->tail, params->tail_len,
+                                  &sys_config)) {
+                /* hard code setting to wpa2-psk */
+                sys_config.protocol = PROTOCOL_WPA2;
+                sys_config.key_mgmt = KEY_MGMT_PSK;
+                sys_config.wpa_cfg.pairwise_cipher_wpa2 = CIPHER_AES_CCMP;
+                sys_config.wpa_cfg.group_cipher = CIPHER_AES_CCMP;
+            }
+        }
+    }
+#endif /* COMPAT_WIRELESS */
+    /* If the security mode is configured as WEP or WPA-PSK, it will disable
+       11n automatically, and if configured as open(off) or wpa2-psk, it will
+       automatically enable 11n */
+    if ((sys_config.protocol == PROTOCOL_STATIC_WEP) ||
+        (sys_config.protocol == PROTOCOL_WPA))
+        woal_uap_set_11n_status(&sys_config, MLAN_ACT_DISABLE);
+    else
+        woal_uap_set_11n_status(&sys_config, MLAN_ACT_ENABLE);
+    if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                       MLAN_ACT_SET,
+                                                       MOAL_IOCTL_WAIT,
+                                                       &sys_config)) {
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    LEAVE();
+    return ret;
+}
+
+static int
+woal_mon_open(struct net_device *ndev)
+{
+    ENTER();
+    LEAVE();
+    return 0;
+}
+
+static int
+woal_mon_close(struct net_device *ndev)
+{
+    ENTER();
+    LEAVE();
+    return 0;
+}
+
+static int
+woal_mon_set_mac_address(struct net_device *ndev, void *addr)
+{
+    ENTER();
+    LEAVE();
+    return 0;
+}
+
+static void
+woal_mon_set_multicast_list(struct net_device *ndev)
+{
+    ENTER();
+    LEAVE();
+}
+
+static int
+woal_mon_hard_start_xmit(struct sk_buff *skb, struct net_device *ndev)
+{
+    int len_rthdr;
+    int qos_len = 0;
+    int dot11_hdr_len = 24;
+    int snap_len = 6;
+    unsigned char *pdata;
+    unsigned short fc;
+    unsigned char src_mac_addr[6];
+    unsigned char dst_mac_addr[6];
+    struct ieee80211_hdr *dot11_hdr;
+    struct ieee80211_radiotap_header *prthdr =
+        (struct ieee80211_radiotap_header *) skb->data;
+    monitor_iface *mon_if = netdev_priv(ndev);
+
+    if (mon_if == NULL || mon_if->base_ndev == NULL) {
+        goto fail;
+    }
+
+    ENTER();
+    /* check for not even having the fixed radiotap header part */
+    if (unlikely(skb->len < sizeof(struct ieee80211_radiotap_header))) {
+        PRINTM(MERROR, "Invalid radiotap hdr length,"
+               "skb->len: %d\n", skb->len);
+        goto fail;              /* too short to be possibly valid */
+    }
+
+    /* is it a header version we can trust to find length from? */
+    if (unlikely(prthdr->it_version))
+        goto fail;              /* only version 0 is supported */
+
+    /* then there must be a radiotap header with a length we can use */
+    len_rthdr = ieee80211_get_radiotap_len(skb->data);
+
+    /* does the skb contain enough to deliver on the alleged length? */
+    if (unlikely(skb->len < len_rthdr)) {
+        PRINTM(MERROR, "Invalid data length," "skb->len: %d\n", skb->len);
+        goto fail;              /* skb too short for claimed rt header extent */
+    }
+
+    /* Skip the ratiotap header */
+    skb_pull(skb, len_rthdr);
+
+    dot11_hdr = (struct ieee80211_hdr *) skb->data;
+    fc = le16_to_cpu(dot11_hdr->frame_control);
+    if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) {
+        /* Check if this ia a Wireless Distribution System (WDS) frame which
+           has 4 MAC addresses */
+        if (dot11_hdr->frame_control & 0x0080)
+            qos_len = 2;
+        if ((dot11_hdr->frame_control & 0x0300) == 0x0300)
+            dot11_hdr_len += 6;
+
+        memcpy(dst_mac_addr, dot11_hdr->addr1, sizeof(dst_mac_addr));
+        memcpy(src_mac_addr, dot11_hdr->addr2, sizeof(src_mac_addr));
+
+        /* Skip the 802.11 header, QoS (if any) and SNAP, but leave spaces for
+           for two MAC addresses */
+        skb_pull(skb,
+                 dot11_hdr_len + qos_len + snap_len - sizeof(src_mac_addr) * 2);
+        pdata = (unsigned char *) skb->data;
+        memcpy(pdata, dst_mac_addr, sizeof(dst_mac_addr));
+        memcpy(pdata + sizeof(dst_mac_addr), src_mac_addr,
+               sizeof(src_mac_addr));
+
+        LEAVE();
+        return woal_hard_start_xmit(skb, mon_if->base_ndev);
+    }
+
+  fail:
+    dev_kfree_skb(skb);
+    return NETDEV_TX_OK;
+}
+
+static const struct net_device_ops woal_cfg80211_mon_if_ops = {
+    .ndo_open = woal_mon_open,
+    .ndo_start_xmit = woal_mon_hard_start_xmit,
+    .ndo_stop = woal_mon_close,
+    .ndo_set_mac_address = woal_mon_set_mac_address,
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,2,0)
+    .ndo_set_rx_mode = woal_mon_set_multicast_list,
+#else
+    .ndo_set_multicast_list = woal_mon_set_multicast_list,
+#endif
+};
+
+static void
+woal_mon_if_setup(struct net_device *dev)
+{
+    ENTER();
+    ether_setup(dev);
+    dev->netdev_ops = &woal_cfg80211_mon_if_ops;
+    dev->destructor = free_netdev;
+    LEAVE();
+}
+
+/**
+ * @brief Request the driver to add a monitor interface
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param name            Virtual interface name
+ * @param flags           Flags for the virtual interface
+ * @param params          A pointer to vif_params structure
+ * @param new_dev         Netdevice to be passed out
+ *
+ * @return                A pointer to net_device -- success, otherwise null
+ */
+static int
+woal_cfg80211_add_mon_if(struct wiphy *wiphy, char *name,
+                         u32 * flags, struct vif_params *params,
+                         struct net_device **new_dev)
+{
+    int ret;
+    struct net_device *ndev;
+    monitor_iface *mon_if;
+    moal_private *priv;
+
+    ENTER();
+
+    ASSERT_RTNL();
+
+    priv = (moal_private *) woal_get_wiphy_priv(wiphy);
+
+    ndev = alloc_netdev_mq(sizeof(*mon_if), name, woal_mon_if_setup, 1);
+    if (!ndev) {
+        PRINTM(MFATAL, "Init virtual ethernet device failed\n");
+        ret = -EFAULT;
+        goto fail;
+    }
+
+    dev_net_set(ndev, wiphy_net(wiphy));
+
+    ret = dev_alloc_name(ndev, ndev->name);
+    if (ret < 0) {
+        PRINTM(MFATAL, "Net device alloc name fail.\n");
+        goto fail;
+    }
+
+    memcpy(ndev->perm_addr, wiphy->perm_addr, ETH_ALEN);
+    memcpy(ndev->dev_addr, ndev->perm_addr, ETH_ALEN);
+    SET_NETDEV_DEV(ndev, wiphy_dev(wiphy));
+
+    mon_if = netdev_priv(ndev);
+    ndev->ieee80211_ptr = &mon_if->wdev;
+    mon_if->wdev.iftype = NL80211_IFTYPE_MONITOR;
+    mon_if->wdev.wiphy = wiphy;
+    memcpy(mon_if->ifname, ndev->name, IFNAMSIZ);
+
+    ndev->type = ARPHRD_IEEE80211_RADIOTAP;
+    ndev->netdev_ops = &woal_cfg80211_mon_if_ops;
+
+    mon_if->priv = priv;
+    mon_if->mon_ndev = ndev;
+    mon_if->base_ndev = priv->netdev;
+    mon_if->radiotap_enabled = 1;
+    mon_if->flag = 1;
+
+    ret = register_netdevice(ndev);
+    if (ret) {
+        PRINTM(MFATAL, "register net_device failed, ret=%d\n", ret);
+        goto fail;
+    }
+
+    if (new_dev)
+        *new_dev = ndev;
+
+  fail:
+    if (ret && ndev)
+        free_netdev(ndev);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Request the driver to add a virtual interface
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param name            Virtual interface name
+ * @param type            Virtual interface type
+ * @param flags           Flags for the virtual interface
+ * @param params          A pointer to vif_params structure
+ *
+ * @return                A pointer to net_device -- success, otherwise null
+ */
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,37) || defined(COMPAT_WIRELESS)
+struct net_device *
+woal_cfg80211_add_virtual_intf(struct wiphy *wiphy,
+                               char *name, enum nl80211_iftype type,
+                               u32 * flags, struct vif_params *params)
+#else
+int
+woal_cfg80211_add_virtual_intf(struct wiphy *wiphy,
+                               char *name, enum nl80211_iftype type,
+                               u32 * flags, struct vif_params *params)
+#endif
+{
+    struct net_device *ndev = NULL;
+    int ret = 0;
+
+    ENTER();
+    PRINTM(MIOCTL, "add virtual intf: %d\n", type);
+    switch (type) {
+    case NL80211_IFTYPE_MONITOR:
+        ret = woal_cfg80211_add_mon_if(wiphy, name, flags, params, &ndev);
+        break;
+    default:
+        PRINTM(MWARN, "Not supported if type: %d\n", type);
+        ret = -EFAULT;
+        break;
+    }
+    LEAVE();
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,37) || defined(COMPAT_WIRELESS)
+    if (ret)
+        return NULL;
+    else
+        return ndev;
+#else
+    return ret;
+#endif
+}
+
+/**
+ * @brief Request the driver to del a virtual interface
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param dev             The pointer to net_device
+ *
+ * @return               0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_del_virtual_intf(struct wiphy *wiphy, struct net_device *dev)
+{
+    ENTER();
+
+    PRINTM(MIOCTL, "del virtual intf\n");
+    ASSERT_RTNL();
+    unregister_netdevice(dev);
+
+    LEAVE();
+    return 0;
+}
+
+static int
+woal_uap_cfg80211_scan(struct wiphy *wiphy, struct net_device *dev,
+                       struct cfg80211_scan_request *request)
+{
+    ENTER();
+
+    cfg80211_scan_done(request, MTRUE);
+
+    LEAVE();
+    return 0;
+}
+
+static int
+woal_uap_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
+                          struct cfg80211_connect_params *sme)
+{
+    ENTER();
+    LEAVE();
+    return 0;
+}
+
+static int
+woal_uap_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *dev,
+                             t_u16 reason_code)
+{
+    ENTER();
+    LEAVE();
+    return 0;
+}
+
+/**
+ * @brief initialize AP or GO parameters
+
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param dev             A pointer to net_device structure
+ * @param params          A pointer to beacon_parameters structure
+ * @return                0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_add_beacon(struct wiphy *wiphy,
+                         struct net_device *dev,
+                         struct beacon_parameters *params)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+    int ret = 0;
+
+    ENTER();
+
+    PRINTM(MIOCTL, "add beacon\n");
+    if (params != NULL) {
+        /* bss config */
+        if (MLAN_STATUS_SUCCESS != woal_cfg80211_beacon_config(priv, params)) {
+            ret = -EFAULT;
+            goto done;
+        }
+
+        /* set mgmt frame ies */
+        if (MLAN_STATUS_SUCCESS != woal_cfg80211_mgmt_frame_ie(priv,
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3,2,0) && !defined(COMPAT_WIRELESS)
+                                                               params->tail,
+                                                               params->tail_len,
+                                                               NULL, 0, NULL, 0,
+                                                               NULL, 0,
+                                                               MGMT_MASK_BEACON
+#else
+                                                               params->tail,
+                                                               params->tail_len,
+                                                               params->
+                                                               proberesp_ies,
+                                                               params->
+                                                               proberesp_ies_len,
+                                                               params->
+                                                               assocresp_ies,
+                                                               params->
+                                                               assocresp_ies_len,
+                                                               NULL, 0,
+                                                               MGMT_MASK_BEACON
+                                                               |
+                                                               MGMT_MASK_PROBE_RESP
+                                                               |
+                                                               MGMT_MASK_ASSOC_RESP
+#endif
+            )) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+    /* if the bss is stopped, then start it */
+    if (priv->bss_started == MFALSE) {
+        if (MLAN_STATUS_SUCCESS !=
+            woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_START)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief set AP or GO parameter
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param dev             A pointer to net_device structure
+ * @param params          A pointer to beacon_parameters structure
+ * @return                0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_set_beacon(struct wiphy *wiphy,
+                         struct net_device *dev,
+                         struct beacon_parameters *params)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+    int ret = 0;
+
+    ENTER();
+
+    PRINTM(MIOCTL, "set beacon\n");
+    if (params != NULL) {
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3,2,0) && !defined(COMPAT_WIRELESS)
+        if (params->tail && params->tail_len) {
+            if (MLAN_STATUS_SUCCESS !=
+                woal_cfg80211_mgmt_frame_ie(priv,
+                                            params->tail, params->tail_len,
+                                            NULL, 0, NULL, 0, NULL, 0,
+                                            MGMT_MASK_BEACON)) {
+                ret = -EFAULT;
+                goto done;
+            }
+        }
+#else
+        if (params->beacon_ies && params->beacon_ies_len) {
+            if (MLAN_STATUS_SUCCESS !=
+                woal_cfg80211_mgmt_frame_ie(priv, params->tail,
+                                            params->tail_len, NULL, 0, NULL, 0,
+                                            NULL, 0, MGMT_MASK_BEACON)) {
+                ret = -EFAULT;
+                goto done;
+            }
+        }
+
+        if (params->proberesp_ies && params->proberesp_ies_len) {
+            if (MLAN_STATUS_SUCCESS !=
+                woal_cfg80211_mgmt_frame_ie(priv, NULL, 0,
+                                            params->proberesp_ies,
+                                            params->proberesp_ies_len, NULL, 0,
+                                            NULL, 0, MGMT_MASK_PROBE_RESP)) {
+                ret = -EFAULT;
+                goto done;
+            }
+        }
+
+        if (params->assocresp_ies && params->assocresp_ies_len) {
+            if (MLAN_STATUS_SUCCESS !=
+                woal_cfg80211_mgmt_frame_ie(priv, NULL, 0, NULL, 0,
+                                            params->assocresp_ies,
+                                            params->assocresp_ies_len, NULL, 0,
+                                            MGMT_MASK_ASSOC_RESP)) {
+                ret = -EFAULT;
+                goto done;
+            }
+        }
+#endif
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief reset AP or GO parameters
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param dev             A pointer to net_device structure
+ *
+ * @return                0 -- success, otherwise fail
+ */
+int
+woal_cfg80211_del_beacon(struct wiphy *wiphy, struct net_device *dev)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+    int ret = 0;
+
+    ENTER();
+
+    PRINTM(MIOCTL, "del beacon\n");
+    /* if the bss is still running, then stop it */
+    if (priv->bss_started == MTRUE) {
+        if (MLAN_STATUS_SUCCESS !=
+            woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_STOP)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        if (MLAN_STATUS_SUCCESS !=
+            woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_RESET)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+    /* clear mgmt frame ies */
+    if (MLAN_STATUS_SUCCESS !=
+        woal_cfg80211_mgmt_frame_ie(priv, NULL, 0, NULL, 0, NULL, 0, NULL, 0,
+                                    MGMT_MASK_BEACON | MGMT_MASK_PROBE_RESP |
+                                    MGMT_MASK_ASSOC_RESP)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    priv->cipher = 0;
+    priv->key_len = 0;
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ * @brief Get station info
+ *
+ * @param wiphy           A pointer to wiphy structure
+ * @param dev             A pointer to net_device structure
+ * @param mac			  A pointer to station mac address
+ * @param stainfo		  A pointer to station_info structure
+ *
+ * @return                0 -- success, otherwise fail
+ */
+int
+woal_uap_cfg80211_get_station(struct wiphy *wiphy, struct net_device *dev,
+                              u8 * mac, struct station_info *stainfo)
+{
+    moal_private *priv = (moal_private *) woal_get_netdev_priv(dev);
+    int ret = -EFAULT;
+    int i = 0;
+    mlan_ds_get_info *info = NULL;
+    mlan_ioctl_req *ioctl_req = NULL;
+
+    ENTER();
+    if (priv->media_connected == MFALSE) {
+        PRINTM(MINFO, "cfg80211: Media not connected!\n");
+        LEAVE();
+        return -ENOENT;
+    }
+
+    /* Allocate an IOCTL request buffer */
+    ioctl_req =
+        (mlan_ioctl_req *) woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_get_info));
+    if (ioctl_req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    info = (mlan_ds_get_info *) ioctl_req->pbuf;
+    info->sub_command = MLAN_OID_UAP_STA_LIST;
+    ioctl_req->req_id = MLAN_IOCTL_GET_INFO;
+    ioctl_req->action = MLAN_ACT_GET;
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_request_ioctl(priv, ioctl_req, MOAL_IOCTL_WAIT)) {
+        goto done;
+    }
+    for (i = 0; i < info->param.sta_list.sta_count; i++) {
+        if (!memcmp(info->param.sta_list.info[i].mac_address, mac, ETH_ALEN)) {
+            PRINTM(MIOCTL,
+                   "Get station: %02x:%02x:%02x:%02x:%02x:%02x RSSI=%d\n",
+                   mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
+                   (int) info->param.sta_list.info[i].rssi);
+            stainfo->filled = STATION_INFO_INACTIVE_TIME | STATION_INFO_SIGNAL;
+            stainfo->inactive_time = 0;
+            stainfo->signal = info->param.sta_list.info[i].rssi;
+            ret = 0;
+            break;
+        }
+    }
+  done:
+    if (ioctl_req)
+        kfree(ioctl_req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Sets up the CFG802.11 specific HT capability fields
+ *  with default values
+ *
+ *  @param ht_info      A pointer to ieee80211_sta_ht_cap structure
+ *  @param ap_cfg       A pointer to mlan_uap_bss_param
+ *
+ *  @return             N/A
+ */
+static void
+woal_cfg80211_setup_uap_ht_cap(struct ieee80211_sta_ht_cap *ht_info,
+                               mlan_uap_bss_param * ap_cfg)
+{
+    ENTER();
+
+    ht_info->ht_supported = true;
+    ht_info->ampdu_factor = ap_cfg->ampdu_param & (MBIT(1) | MBIT(0));
+    ht_info->ampdu_density =
+        ap_cfg->ampdu_param & (MBIT(4) | MBIT(3) | MBIT(2));
+
+    memset(&ht_info->mcs, 0, sizeof(ht_info->mcs));
+    memcpy(ht_info->mcs.rx_mask, ap_cfg->supported_mcs_set,
+           sizeof(ht_info->mcs.rx_mask));
+
+    ht_info->cap = 0;
+    if (ap_cfg->ht_cap_info & MBIT(1))  /* 20/40 Mhz enable */
+        ht_info->cap |= IEEE80211_HT_CAP_SUP_WIDTH_20_40;
+    if (ap_cfg->ht_cap_info & MBIT(4))  /* Green field supported */
+        ht_info->cap |= IEEE80211_HT_CAP_GRN_FLD;
+    if (ap_cfg->ht_cap_info & MBIT(5))  /* Short GI @ 20Mhz supported */
+        ht_info->cap |= IEEE80211_HT_CAP_SGI_20;
+    if (ap_cfg->ht_cap_info & MBIT(6))  /* Short GI @ 40Mhz supported */
+        ht_info->cap |= IEEE80211_HT_CAP_SGI_40;
+    if (ap_cfg->ht_cap_info & MBIT(12)) /* DSS/CCK mode in 40MHz enable */
+        ht_info->cap |= IEEE80211_HT_CAP_DSSSCCK40;
+    ht_info->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
+
+    LEAVE();
+}
+
+/**
+ * @brief Initialize the uAP wiphy
+ *
+ * @param priv            A pointer to moal_private structure
+ * @param wait_option     Wait option
+ *
+ * @return                MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_cfg80211_uap_init_wiphy(moal_private * priv, t_u8 wait_option)
+{
+    struct wiphy *wiphy = priv->wdev->wiphy;
+    mlan_uap_bss_param ap_cfg;
+
+    ENTER();
+
+    if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                       MLAN_ACT_GET,
+                                                       wait_option, &ap_cfg)) {
+        LEAVE();
+        return MLAN_STATUS_FAILURE;
+    }
+
+    /* Initialize parameters for 2GHz and 5GHz bands */
+    woal_cfg80211_setup_uap_ht_cap(&wiphy->bands[IEEE80211_BAND_2GHZ]->ht_cap,
+                                   &ap_cfg);
+    if (wiphy->bands[IEEE80211_BAND_5GHZ])
+        woal_cfg80211_setup_uap_ht_cap(&wiphy->bands[IEEE80211_BAND_5GHZ]->
+                                       ht_cap, &ap_cfg);
+
+    /* Set retry limit count to wiphy */
+    wiphy->retry_long = (t_u8) ap_cfg.retry_limit;
+    wiphy->retry_short = (t_u8) ap_cfg.retry_limit;
+    wiphy->max_scan_ie_len = MAX_IE_SIZE;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,37) || defined(COMPAT_WIRELESS)
+    wiphy->mgmt_stypes = ieee80211_uap_mgmt_stypes;
+#endif
+    /* Set RTS threshold to wiphy */
+    wiphy->rts_threshold = (t_u32) ap_cfg.rts_threshold;
+
+    /* Set fragment threshold to wiphy */
+    wiphy->frag_threshold = (t_u32) ap_cfg.frag_threshold;
+
+    LEAVE();
+    return MLAN_STATUS_SUCCESS;
+}
+
+/**
+ * @brief Register the device with cfg80211
+ *
+ * @param dev       A pointer to net_device structure
+ * @param bss_type  BSS type
+ *
+ * @return          MLAN_STATUS_SUCCESS or MLAN_STATUS_FAILURE
+ */
+mlan_status
+woal_register_uap_cfg80211(struct net_device * dev, t_u8 bss_type)
+{
+    mlan_status ret = MLAN_STATUS_SUCCESS;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    void *wdev_priv = NULL;
+    struct wireless_dev *wdev = NULL;
+    mlan_fw_info fw_info;
+
+    ENTER();
+
+    /* Allocate wireless device */
+    wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
+    if (!wdev) {
+        PRINTM(MERROR, "Could not allocate wireless device\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto err_wdev;
+    }
+
+    /* Allocate wiphy */
+    wdev->wiphy = wiphy_new(&woal_cfg80211_uap_ops, sizeof(moal_private *));
+    if (!wdev->wiphy) {
+        PRINTM(MERROR, "Could not allocate wiphy device\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto err_wdev;
+    }
+    if (bss_type == MLAN_BSS_TYPE_UAP) {
+        dev_set_name(&wdev->wiphy->dev, dev->name);
+        wdev->iftype = NL80211_IFTYPE_AP;
+        wdev->wiphy->interface_modes =
+            MBIT(NL80211_IFTYPE_AP) | MBIT(NL80211_IFTYPE_STATION) |
+            MBIT(NL80211_IFTYPE_MONITOR);
+        wdev->wiphy->max_scan_ssids = 10;
+    }
+
+    /* Make this wiphy known to this driver only */
+    wdev->wiphy->privid = mrvl_wiphy_privid;
+
+    /* Supported bands */
+    wdev->wiphy->bands[IEEE80211_BAND_2GHZ] = &cfg80211_band_2ghz;
+    if (MLAN_STATUS_SUCCESS ==
+        woal_request_get_fw_info(priv, MOAL_CMD_WAIT, &fw_info)) {
+        if (fw_info.fw_bands & BAND_A)
+            wdev->wiphy->bands[IEEE80211_BAND_5GHZ] = &cfg80211_band_5ghz;
+    }
+
+    /* Initialize cipher suits */
+    wdev->wiphy->cipher_suites = cfg80211_cipher_suites;
+    wdev->wiphy->n_cipher_suites = ARRAY_SIZE(cfg80211_cipher_suites);
+
+    wdev->wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
+
+    /* We are using custom domains */
+    wdev->wiphy->flags |= WIPHY_FLAG_CUSTOM_REGULATORY;
+
+    wdev->wiphy->reg_notifier = NULL;   // TODO: woal_cfg80211_reg_notifier;
+
+    /* Set moal_private pointer in wiphy_priv */
+    wdev_priv = wiphy_priv(wdev->wiphy);
+
+    *(unsigned long *) wdev_priv = (unsigned long) priv;
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,39) || defined(COMPAT_WIRELESS)
+    set_wiphy_dev(wdev->wiphy, (struct device *) priv->phandle->hotplug_device);
+#endif
+
+    if (wiphy_register(wdev->wiphy) < 0) {
+        PRINTM(MERROR, "Wiphy device registration failed!\n");
+        ret = MLAN_STATUS_FAILURE;
+        goto err_wdev;
+    }
+
+    dev_net_set(dev, wiphy_net(wdev->wiphy));
+    dev->ieee80211_ptr = wdev;
+    SET_NETDEV_DEV(dev, wiphy_dev(wdev->wiphy));
+    priv->wdev = wdev;
+
+    if (ret != MLAN_STATUS_SUCCESS) {
+        PRINTM(MERROR, "Wiphy device registration failed!\n");
+    } else {
+        PRINTM(MINFO, "Successfully registered wiphy device\n");
+        LEAVE();
+        return ret;
+    }
+
+    wiphy_unregister(wdev->wiphy);
+  err_wdev:
+    dev->ieee80211_ptr = NULL;
+    if (wdev && wdev->wiphy)
+        wiphy_free(wdev->wiphy);
+    kfree(wdev);
+    LEAVE();
+    return ret;
+}
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_uap_cfg80211.h b/drivers/net/wireless/sd8797/mlinux/moal_uap_cfg80211.h
new file mode 100644
index 000000000000..74c6f1a4e59b
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_uap_cfg80211.h
@@ -0,0 +1,30 @@
+/** @file moal_uap_cfg80211.h
+  *
+  * @brief This file contains the uAP CFG80211 specific defines.
+  *
+  * Copyright (C) 2011, Marvell International Ltd. 
+  * 
+  * This software file (the "File") is distributed by Marvell International 
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+  * (the "License").  You may use, redistribute and/or modify this File in 
+  * accordance with the terms and conditions of the License, a copy of which 
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+  * this warranty disclaimer.
+  *
+  */
+
+#ifndef _MOAL_UAP_CFG80211_H_
+#define _MOAL_UAP_CFG80211_H_
+
+#include    "moal_uap.h"
+
+mlan_status woal_register_uap_cfg80211(struct net_device *dev, t_u8 bss_type);
+mlan_status woal_cfg80211_uap_init_wiphy(moal_private * priv, t_u8 wait_option);
+
+#endif /* _MOAL_UAP_CFG80211_H_ */
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_uap_priv.c b/drivers/net/wireless/sd8797/mlinux/moal_uap_priv.c
new file mode 100644
index 000000000000..21f71cc4fe54
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_uap_priv.c
@@ -0,0 +1,175 @@
+/** @file  moal_uap_priv.c
+  *
+  * @brief This file contains standard ioctl functions
+  *
+  * Copyright (C) 2010-2011, Marvell International Ltd.
+  *
+  * This software file (the "File") is distributed by Marvell International
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+  * (the "License").  You may use, redistribute and/or modify this File in
+  * accordance with the terms and conditions of the License, a copy of which
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+  * this warranty disclaimer.
+  *
+  */
+
+/************************************************************************
+Change log:
+    08/06/2010: initial version
+************************************************************************/
+
+#include	"moal_main.h"
+#include    "moal_uap.h"
+#include    "moal_uap_priv.h"
+
+/********************************************************
+                Local Variables
+********************************************************/
+
+/********************************************************
+		Global Variables
+********************************************************/
+
+/********************************************************
+		Local Functions
+********************************************************/
+
+/********************************************************
+		Global Functions
+********************************************************/
+
+/**
+ *  @brief ioctl function for wireless IOCTLs
+ *
+ *  @param dev		A pointer to net_device structure
+ *  @param req	   	A pointer to ifreq structure
+ *  @param cmd 		Command
+ *
+ *  @return          0 --success, otherwise fail
+ */
+int
+woal_uap_do_priv_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    struct iwreq *wrq = (struct iwreq *) req;
+    int ret = 0;
+
+    ENTER();
+
+    switch (cmd) {
+    case WOAL_UAP_SETNONE_GETNONE:
+        switch (wrq->u.data.flags) {
+        case WOAL_UAP_START:
+            break;
+        case WOAL_UAP_STOP:
+            ret = woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_STOP);
+            break;
+        case WOAL_AP_BSS_START:
+            ret = woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_START);
+            break;
+        case WOAL_AP_BSS_STOP:
+            ret = woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_STOP);
+            break;
+        default:
+            ret = -EINVAL;
+            break;
+        }
+        break;
+    case WOAL_UAP_SETONEINT_GETWORDCHAR:
+        switch (wrq->u.data.flags) {
+        case WOAL_UAP_VERSION:
+            ret = woal_get_driver_version(priv, req);
+            break;
+        case WOAL_UAP_VEREXT:
+            ret = woal_get_driver_verext(priv, req);
+            break;
+        default:
+            ret = -EOPNOTSUPP;
+            break;
+        }
+        break;
+    case WOAL_UAP_SET_GET_256_CHAR:
+        switch (wrq->u.data.flags) {
+        case WOAL_WL_FW_RELOAD:
+            break;
+        case WOAL_AP_SET_CFG:
+            ret = woal_uap_set_ap_cfg(priv, wrq->u.data.pointer,
+                                      wrq->u.data.length);
+            break;
+        default:
+            ret = -EINVAL;
+            break;
+        }
+        break;
+#if defined(WIFI_DIRECT_SUPPORT)
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+    case WOAL_UAP_SETONEINT_GETONEINT:
+        switch (wrq->u.data.flags) {
+        case WOAL_UAP_SET_GET_BSS_ROLE:
+            ret = woal_set_get_bss_role(priv, wrq);
+            break;
+        default:
+            ret = -EINVAL;
+            break;
+        }
+        break;
+#endif
+#endif
+    case WOAL_UAP_HOST_CMD:
+        ret = woal_host_command(priv, wrq);
+        break;
+    case WOAL_UAP_FROYO_START:
+        break;
+    case WOAL_UAP_FROYO_STOP:
+        ret = woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_STOP);
+        break;
+    case WOAL_UAP_FROYO_AP_BSS_START:
+        ret = woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_START);
+        break;
+    case WOAL_UAP_FROYO_AP_BSS_STOP:
+        ret = woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_STOP);
+        break;
+    case WOAL_UAP_FROYO_WL_FW_RELOAD:
+        break;
+    case WOAL_UAP_FROYO_AP_SET_CFG:
+        ret = woal_uap_set_ap_cfg(priv, wrq->u.data.pointer,
+                                  wrq->u.data.length);
+        break;
+    default:
+        ret = -EINVAL;
+        break;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Handle get info resp
+ *
+ *  @param priv 	Pointer to moal_private structure
+ *  @param info 	Pointer to mlan_ds_get_info structure
+ *
+ *  @return    		N/A
+ */
+void
+woal_ioctl_get_uap_info_resp(moal_private * priv, mlan_ds_get_info * info)
+{
+    ENTER();
+    switch (info->sub_command) {
+    case MLAN_OID_GET_STATS:
+        priv->w_stats.discard.fragment = info->param.ustats.fcs_error_count;
+        priv->w_stats.discard.retries = info->param.ustats.retry_count;
+        priv->w_stats.discard.misc = info->param.ustats.ack_failure_count;
+        break;
+    default:
+        break;
+    }
+    LEAVE();
+}
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_uap_priv.h b/drivers/net/wireless/sd8797/mlinux/moal_uap_priv.h
new file mode 100644
index 000000000000..fcd811efb168
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_uap_priv.h
@@ -0,0 +1,194 @@
+/** @file  moal_uap_priv.h
+  *
+  * @brief This file contains definition for extended private IOCTL call.
+  *
+  * Copyright (C) 2010-2011, Marvell International Ltd.
+  *
+  * This software file (the "File") is distributed by Marvell International
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+  * (the "License").  You may use, redistribute and/or modify this File in
+  * accordance with the terms and conditions of the License, a copy of which
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+  * this warranty disclaimer.
+  *
+  */
+
+/************************************************************************
+Change log:
+    08/06/2010: initial version
+************************************************************************/
+
+#ifndef _MOAL_UAP_PRIV_H_
+#define _MOAL_UAP_PRIV_H_
+
+/** Private command ID */
+#define WOAL_UAP_IOCTL                          0x8BE0
+
+/** Private command to get/set 256 chars */
+#define WOAL_UAP_SET_GET_256_CHAR               (WOAL_UAP_IOCTL + 1)
+/** Private command ID to FW reload */
+#define WOAL_WL_FW_RELOAD           1
+/** Private command ID to set AP configuration */
+#define WOAL_AP_SET_CFG             2
+
+/** Private command ID to set/get none */
+#define WOAL_UAP_SETNONE_GETNONE                (WOAL_UAP_IOCTL + 2)
+/** Private command ID to start UAP */
+#define WOAL_UAP_START              1
+/** Private command ID to stop UAP */
+#define WOAL_UAP_STOP               2
+/** Private command ID to start AP BSS */
+#define WOAL_AP_BSS_START           3
+/** Private command ID to stop AP BSS */
+#define WOAL_AP_BSS_STOP            4
+
+/** Private command ID to set one int/get word char */
+#define WOAL_UAP_SETONEINT_GETWORDCHAR          (WOAL_UAP_IOCTL + 3)
+/** Private command ID to get version */
+#define WOAL_UAP_VERSION            1
+/** Private command ID to get extended version */
+#define WOAL_UAP_VEREXT             2
+
+#if defined(WIFI_DIRECT_SUPPORT)
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+/** Private command ID to set one int/get one int */
+#define WOAL_UAP_SETONEINT_GETONEINT            (WOAL_UAP_IOCTL + 5)
+/** Private command ID for set/get BSS role */
+#define WOAL_UAP_SET_GET_BSS_ROLE       1
+#endif
+#endif
+
+/** Private command ID for hostcmd */
+#define WOAL_UAP_HOST_CMD                       (WOAL_UAP_IOCTL + 17)
+
+/** The following command IDs are for Froyo app */
+/** Private command ID to start AP BSS */
+#define WOAL_UAP_FROYO_AP_BSS_START             (WOAL_UAP_IOCTL + 24)
+/** Private command ID to stop AP BSS */
+#define WOAL_UAP_FROYO_AP_BSS_STOP              (WOAL_UAP_IOCTL + 26)
+/** Private command ID to set AP config */
+#define WOAL_UAP_FROYO_AP_SET_CFG               (WOAL_UAP_IOCTL + 27)
+/** Private command ID to start driver */
+#define WOAL_UAP_FROYO_START                    (WOAL_UAP_IOCTL + 28)
+/** Private command ID to reload FW */
+#define WOAL_UAP_FROYO_WL_FW_RELOAD             (WOAL_UAP_IOCTL + 29)
+/** Private command ID to stop driver */
+#define WOAL_UAP_FROYO_STOP                     (WOAL_UAP_IOCTL + 30)
+
+/**
+ * iwpriv ioctl handlers
+ */
+static const struct iw_priv_args woal_uap_priv_args[] = {
+    {
+     WOAL_UAP_SETNONE_GETNONE,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_NONE,
+     ""},
+    {
+     WOAL_UAP_START,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_NONE,
+     "start"},
+    {
+     WOAL_UAP_STOP,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_NONE,
+     "stop"},
+    {
+     WOAL_AP_BSS_START,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_NONE,
+     "bssstart"},
+    {
+     WOAL_AP_BSS_STOP,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_NONE,
+     "bssstop"},
+    {
+     WOAL_UAP_SETONEINT_GETWORDCHAR,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_CHAR | 128,
+     ""},
+    {
+     WOAL_UAP_VERSION,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_CHAR | 128,
+     "version"},
+    {
+     WOAL_UAP_VEREXT,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_CHAR | 128,
+     "verext"},
+#if defined(WIFI_DIRECT_SUPPORT)
+#if defined(STA_SUPPORT) && defined(UAP_SUPPORT)
+    {
+     WOAL_UAP_SETONEINT_GETONEINT,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     ""},
+    {
+     WOAL_UAP_SET_GET_BSS_ROLE,
+     IW_PRIV_TYPE_INT | 1,
+     IW_PRIV_TYPE_INT | 1,
+     "bssrole"},
+#endif
+#endif
+    {
+     WOAL_UAP_SET_GET_256_CHAR,
+     IW_PRIV_TYPE_CHAR | 256,
+     IW_PRIV_TYPE_CHAR | 256,
+     ""},
+    {
+     WOAL_WL_FW_RELOAD,
+     IW_PRIV_TYPE_CHAR | 256,
+     IW_PRIV_TYPE_CHAR | 256,
+     "fwreload"},
+    {
+     WOAL_AP_SET_CFG,
+     IW_PRIV_TYPE_CHAR | 256,
+     IW_PRIV_TYPE_CHAR | 256,
+     "apcfg"},
+    {
+     WOAL_UAP_HOST_CMD,
+     IW_PRIV_TYPE_BYTE | 2047,
+     IW_PRIV_TYPE_BYTE | 2047,
+     "hostcmd"},
+    {
+     WOAL_UAP_FROYO_START,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_NONE,
+     "START"},
+    {
+     WOAL_UAP_FROYO_STOP,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_NONE,
+     "STOP"},
+    {
+     WOAL_UAP_FROYO_AP_BSS_START,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_NONE,
+     "AP_BSS_START"},
+    {
+     WOAL_UAP_FROYO_AP_BSS_STOP,
+     IW_PRIV_TYPE_NONE,
+     IW_PRIV_TYPE_NONE,
+     "AP_BSS_STOP"},
+    {
+     WOAL_UAP_FROYO_WL_FW_RELOAD,
+     IW_PRIV_TYPE_CHAR | 256,
+     IW_PRIV_TYPE_CHAR | 256,
+     "WL_FW_RELOAD"},
+    {
+     WOAL_UAP_FROYO_AP_SET_CFG,
+     IW_PRIV_TYPE_CHAR | 256,
+     IW_PRIV_TYPE_CHAR | 256,
+     "AP_SET_CFG"},
+};
+
+#endif /* _MOAL_UAP_PRIV_H_ */
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_uap_wext.c b/drivers/net/wireless/sd8797/mlinux/moal_uap_wext.c
new file mode 100644
index 000000000000..eb1d048cafd6
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_uap_wext.c
@@ -0,0 +1,1766 @@
+/** @file  moal_uap_wext.c
+  *
+  * @brief This file contains wireless extension standard ioctl functions
+  *
+  * Copyright (C) 2010-2011, Marvell International Ltd.
+  *
+  * This software file (the "File") is distributed by Marvell International
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+  * (the "License").  You may use, redistribute and/or modify this File in
+  * accordance with the terms and conditions of the License, a copy of which
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+  * this warranty disclaimer.
+  *
+  */
+
+/************************************************************************
+Change log:
+    08/06/2010: initial version
+************************************************************************/
+
+#include        "moal_main.h"
+#include        "moal_uap.h"
+#include        "moal_wext.h"
+#include        "moal_uap_priv.h"
+
+/********************************************************
+                Global Variables
+********************************************************/
+typedef struct _chan_to_freq_t
+{
+    /** Channel */
+    t_u16 channel;
+    /** Frequency */
+    t_u32 freq;
+    /** Band */
+    t_u8 band;
+} chan_to_freq_t;
+
+const chan_to_freq_t chan_to_freq[] = {
+    {1, 2412, 0},
+    {2, 2417, 0},
+    {3, 2422, 0},
+    {4, 2427, 0},
+    {5, 2432, 0},
+    {6, 2437, 0},
+    {7, 2442, 0},
+    {8, 2447, 0},
+    {9, 2452, 0},
+    {10, 2457, 0},
+    {11, 2462, 0},
+    {12, 2467, 0},
+    {13, 2472, 0},
+    {14, 2484, 0},
+    {183, 4915, 1},
+    {184, 4920, 1},
+    {185, 4925, 1},
+    {187, 4935, 1},
+    {188, 4940, 1},
+    {189, 4945, 1},
+    {192, 4960, 1},
+    {196, 4980, 1},
+    {7, 5035, 1},
+    {8, 5040, 1},
+    {9, 5045, 1},
+    {11, 5055, 1},
+    {12, 5060, 1},
+    {16, 5080, 1},
+    {34, 5170, 1},
+    {36, 5180, 1},
+    {38, 5190, 1},
+    {40, 5200, 1},
+    {42, 5210, 1},
+    {44, 5220, 1},
+    {46, 5230, 1},
+    {48, 5240, 1},
+    {52, 5260, 1},
+    {56, 5280, 1},
+    {60, 5300, 1},
+    {64, 5320, 1},
+    {100, 5500, 1},
+    {104, 5520, 1},
+    {108, 5540, 1},
+    {112, 5560, 1},
+    {116, 5580, 1},
+    {120, 5600, 1},
+    {124, 5620, 1},
+    {128, 5640, 1},
+    {132, 5660, 1},
+    {136, 5680, 1},
+    {140, 5700, 1},
+    {149, 5745, 1},
+    {153, 5765, 1},
+    {157, 5785, 1},
+    {161, 5805, 1},
+    {165, 5825, 1},
+};
+
+/** Convertion from frequency to channel */
+#define freq_to_chan(x) ((((x) - 2412) / 5) + 1)
+
+/********************************************************
+                Local Functions
+********************************************************/
+
+/**
+ *  @brief Sort Channels
+ *
+ *  @param freq                 A pointer to iw_freq structure
+ *  @param num                  Number of Channels
+ *
+ *  @return                     N/A
+ */
+static inline void
+woal_sort_channels(struct iw_freq *freq, int num)
+{
+    int i, j;
+    struct iw_freq temp;
+
+    for (i = 0; i < num; i++)
+        for (j = i + 1; j < num; j++)
+            if (freq[i].i > freq[j].i) {
+                temp.i = freq[i].i;
+                temp.m = freq[i].m;
+
+                freq[i].i = freq[j].i;
+                freq[i].m = freq[j].m;
+
+                freq[j].i = temp.i;
+                freq[j].m = temp.m;
+            }
+}
+
+/**
+ *  @brief Get frequency for channel in given band
+ *
+ *  @param channel      channel
+ *  @param band         band
+ *
+ *  @return             freq
+ */
+static int
+channel_to_frequency(t_u16 channel, t_u8 band)
+{
+    int i = 0;
+
+    ENTER();
+    for (i = 0; i < sizeof(chan_to_freq) / sizeof(chan_to_freq_t); i++) {
+        if (channel == chan_to_freq[i].channel && band == chan_to_freq[i].band) {
+            LEAVE();
+            return chan_to_freq[i].freq;
+        }
+    }
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Commit handler: called after a bunch of SET operations
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param cwrq         A pointer to char buffer
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0 --success
+ */
+static int
+woal_config_commit(struct net_device *dev,
+                   struct iw_request_info *info, char *cwrq, char *extra)
+{
+    ENTER();
+
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Get name
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param cwrq         A pointer to char buffer
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0 --success
+ */
+static int
+woal_get_name(struct net_device *dev, struct iw_request_info *info,
+              char *cwrq, char *extra)
+{
+    ENTER();
+    strcpy(cwrq, "IEEE 802.11-DS");
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Get current BSSID
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param awrq         A pointer to sockaddr structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0 --success
+ */
+static int
+woal_get_wap(struct net_device *dev, struct iw_request_info *info,
+             struct sockaddr *awrq, char *extra)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int ret = 0;
+
+    ENTER();
+
+    if (priv->bss_started) {
+        memcpy(awrq->sa_data, priv->current_addr, MLAN_MAC_ADDR_LENGTH);
+    } else {
+        memset(awrq->sa_data, 0, MLAN_MAC_ADDR_LENGTH);
+    }
+    awrq->sa_family = ARPHRD_ETHER;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Change the AP BSSID
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param awrq         A pointer to iw_param structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0 --success, otherwise fail
+ */
+static int
+woal_set_wap(struct net_device *dev, struct iw_request_info *info,
+             struct sockaddr *awrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    const t_u8 zero_mac[MLAN_MAC_ADDR_LENGTH] = { 0, 0, 0, 0, 0, 0 };
+
+    ENTER();
+
+    if (awrq->sa_family != ARPHRD_ETHER) {
+        ret = -EINVAL;
+        goto done;
+    }
+
+    PRINTM(MINFO, "ASSOC: WAP: uAP bss : %02x:%02x:%02x:%02x:%02x:%02x\n",
+           (t_u8) awrq->sa_data[0], (t_u8) awrq->sa_data[1],
+           (t_u8) awrq->sa_data[2], (t_u8) awrq->sa_data[3],
+           (t_u8) awrq->sa_data[4], (t_u8) awrq->sa_data[5]);
+
+    /*
+     * Using this ioctl to start/stop the BSS, return if bss
+     * is already started/stopped.
+     */
+    if (memcmp(zero_mac, awrq->sa_data, MLAN_MAC_ADDR_LENGTH)) {
+        if (priv->bss_started == MFALSE)
+            ret = woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_START);
+        else
+            PRINTM(MINFO, "BSS is already started.\n");
+    } else {
+        /* zero_mac means bss_stop */
+        if (priv->bss_started == MTRUE)
+            ret = woal_uap_bss_ctrl(priv, MOAL_IOCTL_WAIT, UAP_BSS_STOP);
+        else
+            PRINTM(MINFO, "BSS is already stopped.\n");
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set frequency/channel
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param fwrq                 A pointer to iw_freq structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_freq(struct net_device *dev, struct iw_request_info *info,
+              struct iw_freq *fwrq, char *extra)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_uap_bss_param *sys_cfg = NULL, *ap_cfg = NULL;
+    int ret = 0, chan = 0, i = 0;
+
+    ENTER();
+
+    ap_cfg = kmalloc(sizeof(mlan_uap_bss_param), GFP_KERNEL);
+    if (ap_cfg == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    sys_cfg = kmalloc(sizeof(mlan_uap_bss_param), GFP_KERNEL);
+    if (sys_cfg == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                       MLAN_ACT_GET,
+                                                       MOAL_IOCTL_WAIT,
+                                                       ap_cfg)) {
+        PRINTM(MERROR, "Error getting AP confiruration\n");
+        ret = -EFAULT;
+        goto done;
+    }
+    i = ap_cfg->num_of_chan;
+
+    /* Initialize the invalid values so that the correct values below are
+       downloaded to firmware */
+    woal_set_sys_config_invalid_data(sys_cfg);
+
+    /* If setting by frequency, convert to a channel */
+    if (fwrq->e == 1)
+        chan = freq_to_chan(fwrq->m / 100000);
+    else
+        chan = fwrq->m;
+    if (chan > 0 && chan < MLAN_MAX_CHANNEL)
+        sys_cfg->channel = chan;
+    else {
+        ret = -EINVAL;
+        goto done;
+    }
+    for (i = 0; i < ap_cfg->num_of_chan; i++)
+        if (ap_cfg->chan_list[i].chan_number == chan)
+            break;
+    if (i == ap_cfg->num_of_chan) {
+        PRINTM(MERROR, "Channel %d is not supported\n", chan);
+        ret = -EINVAL;
+        goto done;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                       MLAN_ACT_SET,
+                                                       MOAL_IOCTL_WAIT,
+                                                       sys_cfg)) {
+        PRINTM(MERROR, "Error setting AP confiruration\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    if (sys_cfg)
+        kfree(sys_cfg);
+    if (ap_cfg)
+        kfree(ap_cfg);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get frequency and channel
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param fwrq                 A pointer to iw_freq structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_get_freq(struct net_device *dev, struct iw_request_info *info,
+              struct iw_freq *fwrq, char *extra)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_uap_bss_param ap_cfg;
+    t_u8 band = 0;
+    int ret = 0;
+
+    ENTER();
+
+    if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                       MLAN_ACT_GET,
+                                                       MOAL_IOCTL_WAIT,
+                                                       &ap_cfg)) {
+        PRINTM(MERROR, "Error getting AP confiruration\n");
+        LEAVE();
+        return -EFAULT;
+    }
+    band = ap_cfg.band_cfg & BAND_CONFIG_5GHZ;
+    fwrq->i = (long) ap_cfg.channel;
+    fwrq->m = (long) (channel_to_frequency(ap_cfg.channel, band)) * 100000;
+    fwrq->e = 1;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set wlan mode
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param uwrq                 A pointer to t_u32 string
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_bss_mode(struct net_device *dev, struct iw_request_info *info,
+                  t_u32 * uwrq, char *extra)
+{
+    int ret = 0;
+    ENTER();
+
+    switch (*uwrq) {
+    case IW_MODE_AUTO:
+    case IW_MODE_MASTER:
+        PRINTM(MINFO, "This is correct mode in AP mode\n");
+        break;
+    default:
+        PRINTM(MERROR, "Invalid mode for AP\n");
+        ret = -EINVAL;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get wlan mode
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param uwrq                 A pointer to t_u32 string
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success
+ */
+static int
+woal_get_bss_mode(struct net_device *dev, struct iw_request_info *info,
+                  t_u32 * uwrq, char *extra)
+{
+    ENTER();
+
+    *uwrq = IW_MODE_MASTER;
+
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Set encryption key
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param dwrq                 A pointer to iw_point structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_encode(struct net_device *dev, struct iw_request_info *info,
+                struct iw_point *dwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_uap_bss_param *sys_cfg = NULL, *ap_cfg = NULL;
+    wep_key *pkey = NULL;
+    int key_index = 0;
+
+    ENTER();
+
+    /* Check index */
+    key_index = (dwrq->flags & IW_ENCODE_INDEX) - 1;
+    if (key_index > 3) {
+        PRINTM(MERROR, "Key index #%d out of range\n", key_index);
+        ret = -EINVAL;
+        goto done;
+    }
+
+    ap_cfg = kmalloc(sizeof(mlan_uap_bss_param), GFP_KERNEL);
+    if (ap_cfg == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    sys_cfg = kmalloc(sizeof(mlan_uap_bss_param), GFP_KERNEL);
+    if (sys_cfg == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                       MLAN_ACT_GET,
+                                                       MOAL_IOCTL_WAIT,
+                                                       ap_cfg)) {
+        PRINTM(MERROR, "Error getting AP confiruration\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    /* Initialize the invalid values so that the correct values below are
+       downloaded to firmware */
+    woal_set_sys_config_invalid_data(sys_cfg);
+    sys_cfg->wep_cfg.key0.key_index = 0;
+    sys_cfg->wep_cfg.key1.key_index = 1;
+    sys_cfg->wep_cfg.key2.key_index = 2;
+    sys_cfg->wep_cfg.key3.key_index = 3;
+
+    if (key_index >= 0 && key_index <= 3) {
+        if (key_index == 0)
+            pkey = &sys_cfg->wep_cfg.key0;
+        else if (key_index == 1)
+            pkey = &sys_cfg->wep_cfg.key1;
+        else if (key_index == 2)
+            pkey = &sys_cfg->wep_cfg.key2;
+        else if (key_index == 3)
+            pkey = &sys_cfg->wep_cfg.key3;
+    }
+
+    if (!(dwrq->flags & IW_ENCODE_NOKEY) && dwrq->length) {
+        if (dwrq->length > MAX_WEP_KEY_SIZE) {
+            PRINTM(MERROR, "Key length (%d) out of range\n", dwrq->length);
+            ret = -E2BIG;
+            goto done;
+        }
+        if (key_index < 0) {
+            /* Get current default key index */
+            if (ap_cfg->wep_cfg.key0.is_default)
+                pkey = &sys_cfg->wep_cfg.key0;
+            if (ap_cfg->wep_cfg.key1.is_default)
+                pkey = &sys_cfg->wep_cfg.key1;
+            if (ap_cfg->wep_cfg.key2.is_default)
+                pkey = &sys_cfg->wep_cfg.key2;
+            if (ap_cfg->wep_cfg.key3.is_default)
+                pkey = &sys_cfg->wep_cfg.key3;
+        }
+
+        sys_cfg->protocol = PROTOCOL_STATIC_WEP;
+        memcpy(pkey->key, extra, dwrq->length);
+        /* Set the length */
+        if (dwrq->length > MIN_WEP_KEY_SIZE)
+            pkey->length = MAX_WEP_KEY_SIZE;
+        else
+            pkey->length = MIN_WEP_KEY_SIZE;
+        /* Set current key index as default */
+        pkey->is_default = MTRUE;
+    } else {
+        /*
+         * No key provided so it is either enable key,
+         * on or off
+         */
+        if (dwrq->flags & IW_ENCODE_DISABLED) {
+            PRINTM(MINFO, "*** iwconfig mlanX key off ***\n");
+            sys_cfg->protocol = PROTOCOL_NO_SECURITY;
+        } else {
+            /*
+             * iwconfig mlanX key [n]
+             * iwconfig mlanX key on
+             * Do we want to just set the transmit key index ?
+             */
+            if (key_index < 0) {
+                PRINTM(MINFO, "*** iwconfig mlanX key on ***\n");
+            } else {
+                /* Get current key configuration at key_index */
+                if (key_index == 0)
+                    memcpy(pkey, &ap_cfg->wep_cfg.key0, sizeof(wep_key));
+                if (key_index == 1)
+                    memcpy(pkey, &ap_cfg->wep_cfg.key1, sizeof(wep_key));
+                if (key_index == 2)
+                    memcpy(pkey, &ap_cfg->wep_cfg.key2, sizeof(wep_key));
+                if (key_index == 3)
+                    memcpy(pkey, &ap_cfg->wep_cfg.key3, sizeof(wep_key));
+                /* Set current key index as default */
+                pkey->is_default = MTRUE;
+            }
+        }
+    }
+    if (dwrq->flags & (IW_ENCODE_RESTRICTED | IW_ENCODE_OPEN)) {
+        switch (dwrq->flags & 0xf000) {
+        case IW_ENCODE_RESTRICTED:
+            /* iwconfig mlanX restricted key [1] */
+            sys_cfg->auth_mode = MLAN_AUTH_MODE_SHARED;
+            PRINTM(MINFO, "Auth mode restricted!\n");
+            break;
+        case IW_ENCODE_OPEN:
+            /* iwconfig mlanX key [2] open */
+            sys_cfg->auth_mode = MLAN_AUTH_MODE_OPEN;
+            PRINTM(MINFO, "Auth mode open!\n");
+            break;
+        case IW_ENCODE_RESTRICTED | IW_ENCODE_OPEN:
+        default:
+            /* iwconfig mlanX key [2] open restricted */
+            PRINTM(MINFO, "Auth mode auto!\n");
+            break;
+        }
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                       MLAN_ACT_SET,
+                                                       MOAL_IOCTL_WAIT,
+                                                       sys_cfg)) {
+        PRINTM(MERROR, "Error setting AP confiruration\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    if (sys_cfg)
+        kfree(sys_cfg);
+    if (ap_cfg)
+        kfree(ap_cfg);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get encryption key
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param dwrq                 A pointer to iw_point structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_get_encode(struct net_device *dev, struct iw_request_info *info,
+                struct iw_point *dwrq, char *extra)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int index = (dwrq->flags & IW_ENCODE_INDEX);
+    wep_key *pkey = NULL;
+    mlan_uap_bss_param ap_cfg;
+    int ret = 0;
+
+    ENTER();
+    if (index < 0 || index > 4) {
+        PRINTM(MERROR, "Key index #%d out of range\n", index);
+        ret = -EINVAL;
+        goto done;
+    }
+    if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                       MLAN_ACT_GET,
+                                                       MOAL_IOCTL_WAIT,
+                                                       &ap_cfg)) {
+        PRINTM(MERROR, "Error getting AP confiruration\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    dwrq->flags = 0;
+    /*
+     * Check encryption mode
+     */
+    switch (ap_cfg.auth_mode) {
+    case MLAN_AUTH_MODE_OPEN:
+        dwrq->flags = IW_ENCODE_OPEN;
+        break;
+    case MLAN_AUTH_MODE_SHARED:
+    case MLAN_AUTH_MODE_NETWORKEAP:
+        dwrq->flags = IW_ENCODE_RESTRICTED;
+        break;
+    default:
+        dwrq->flags = IW_ENCODE_DISABLED | IW_ENCODE_OPEN;
+        break;
+    }
+
+    switch (ap_cfg.protocol) {
+    case PROTOCOL_NO_SECURITY:
+        dwrq->flags |= IW_ENCODE_DISABLED;
+        break;
+    case PROTOCOL_STATIC_WEP:
+        if (ap_cfg.wep_cfg.key0.is_default)
+            pkey = &ap_cfg.wep_cfg.key0;
+        else if (ap_cfg.wep_cfg.key1.is_default)
+            pkey = &ap_cfg.wep_cfg.key1;
+        else if (ap_cfg.wep_cfg.key2.is_default)
+            pkey = &ap_cfg.wep_cfg.key2;
+        else if (ap_cfg.wep_cfg.key3.is_default)
+            pkey = &ap_cfg.wep_cfg.key3;
+        if (pkey) {
+            dwrq->flags |= (pkey->key_index + 1);
+            dwrq->length = pkey->length;
+            memcpy(extra, pkey->key, pkey->length);
+            dwrq->flags &= ~IW_ENCODE_DISABLED;
+        } else {
+            ret = -EFAULT;
+        }
+        break;
+    case PROTOCOL_WPA:
+    case PROTOCOL_WPA2:
+    case PROTOCOL_WPA2_MIXED:
+        memcpy(extra, ap_cfg.wpa_cfg.passphrase, ap_cfg.wpa_cfg.length);
+        dwrq->length = ap_cfg.wpa_cfg.length;
+        dwrq->flags |= 1;
+        dwrq->flags &= ~IW_ENCODE_DISABLED;
+        break;
+    default:
+        dwrq->flags &= ~IW_ENCODE_DISABLED;
+        break;
+    }
+    dwrq->flags |= IW_ENCODE_NOKEY;
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+#if (WIRELESS_EXT >= 18)
+/**
+ *  @brief Get IE
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param dwrq                 A pointer to iw_point structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     -EOPNOTSUPP
+ */
+static int
+woal_get_gen_ie(struct net_device *dev, struct iw_request_info *info,
+                struct iw_point *dwrq, char *extra)
+{
+    ENTER();
+    LEAVE();
+    return -EOPNOTSUPP;
+}
+
+/**
+ *  @brief Set IE
+ *
+ *  Pass an opaque block of data, expected to be IEEE IEs, to the driver
+ *    for eventual passthrough to the firmware in an associate/join
+ *    (and potentially start) command.
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param dwrq                 A pointer to iw_point structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_gen_ie(struct net_device *dev, struct iw_request_info *info,
+                struct iw_point *dwrq, char *extra)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_uap_bss_param sys_cfg;
+    IEEEtypes_Header_t *tlv = NULL;
+    int tlv_hdr_len = sizeof(IEEEtypes_Header_t), tlv_buf_left = 0;
+    int ret = 0;
+
+    ENTER();
+
+    /* Initialize the invalid values so that the correct values below are
+       downloaded to firmware */
+    woal_set_sys_config_invalid_data(&sys_cfg);
+
+    tlv_buf_left = dwrq->length;
+    tlv = (IEEEtypes_Header_t *) extra;
+    while (tlv_buf_left >= tlv_hdr_len) {
+        if (tlv->element_id == WPA_IE) {
+            sys_cfg.protocol |= PROTOCOL_WPA;
+            if (priv->pairwise_cipher == CIPHER_TKIP) {
+                sys_cfg.wpa_cfg.pairwise_cipher_wpa = CIPHER_TKIP;
+                PRINTM(MINFO, "Set IE Cipher TKIP\n");
+            }
+            if (priv->pairwise_cipher == CIPHER_AES_CCMP) {
+                sys_cfg.wpa_cfg.pairwise_cipher_wpa = CIPHER_AES_CCMP;
+                PRINTM(MINFO, "Set IE Cipher CCMP\n");
+            }
+            if (priv->pairwise_cipher == (CIPHER_TKIP | CIPHER_AES_CCMP)) {
+                sys_cfg.wpa_cfg.pairwise_cipher_wpa =
+                    CIPHER_TKIP | CIPHER_AES_CCMP;
+                PRINTM(MINFO, "Set IE Cipher TKIP + CCMP\n");
+            }
+            memcpy(priv->bcn_ie_buf + priv->bcn_ie_len,
+                   ((t_u8 *) tlv), sizeof(IEEEtypes_Header_t) + tlv->len);
+            priv->bcn_ie_len += sizeof(IEEEtypes_Header_t) + tlv->len;
+        }
+        if (tlv->element_id == RSN_IE) {
+            sys_cfg.protocol |= PROTOCOL_WPA2;
+            if (priv->pairwise_cipher == CIPHER_TKIP) {
+                sys_cfg.wpa_cfg.pairwise_cipher_wpa2 = CIPHER_TKIP;
+            }
+            if (priv->pairwise_cipher == CIPHER_AES_CCMP) {
+                sys_cfg.wpa_cfg.pairwise_cipher_wpa2 = CIPHER_AES_CCMP;
+            }
+            if (priv->pairwise_cipher == (CIPHER_TKIP | CIPHER_AES_CCMP)) {
+                sys_cfg.wpa_cfg.pairwise_cipher_wpa2 =
+                    (CIPHER_TKIP | CIPHER_AES_CCMP);
+            }
+            memcpy(priv->bcn_ie_buf + priv->bcn_ie_len,
+                   ((t_u8 *) tlv), sizeof(IEEEtypes_Header_t) + tlv->len);
+            priv->bcn_ie_len += sizeof(IEEEtypes_Header_t) + tlv->len;
+        }
+        if (priv->group_cipher == CIPHER_TKIP)
+            sys_cfg.wpa_cfg.group_cipher = CIPHER_TKIP;
+        if (priv->group_cipher == CIPHER_AES_CCMP)
+            sys_cfg.wpa_cfg.group_cipher = CIPHER_AES_CCMP;
+        tlv_buf_left -= (tlv_hdr_len + tlv->len);
+        tlv = (IEEEtypes_Header_t *) ((t_u8 *) tlv + tlv_hdr_len + tlv->len);
+    }
+    sys_cfg.key_mgmt = priv->uap_key_mgmt;
+    if (sys_cfg.key_mgmt & KEY_MGMT_PSK)
+        sys_cfg.key_mgmt_operation |= 0x01;
+    if (sys_cfg.key_mgmt & KEY_MGMT_EAP)
+        sys_cfg.key_mgmt_operation |= 0x03;
+
+    if (sys_cfg.protocol) {
+        if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                           MLAN_ACT_SET,
+                                                           MOAL_IOCTL_WAIT,
+                                                           &sys_cfg)) {
+            PRINTM(MERROR, "Error setting AP configuration\n");
+            ret = -EFAULT;
+            goto done;
+        }
+        priv->pairwise_cipher = 0;
+        priv->group_cipher = 0;
+
+        /* custom IE command to set priv->bcn_ie_buf */
+        if (MLAN_STATUS_SUCCESS !=
+#define UAP_RSN_MASK  (BIT(8) | BIT(5) | BIT(1) | BIT(3))
+            woal_set_get_custom_ie(priv, UAP_RSN_MASK, priv->bcn_ie_buf,
+                                   priv->bcn_ie_len)) {
+            PRINTM(MERROR, "Error setting wpa-rsn IE\n");
+            ret = -EFAULT;
+        }
+    } else if (dwrq->length == 0) {
+        /* custom IE command to re-set priv->bcn_ie_buf */
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_get_custom_ie(priv, 0, priv->bcn_ie_buf,
+                                   priv->bcn_ie_len)) {
+            PRINTM(MERROR, "Error resetting wpa-rsn IE\n");
+            ret = -EFAULT;
+        }
+        priv->bcn_ie_len = 0;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief  Extended version of encoding configuration
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param dwrq         A pointer to iw_point structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return              0 --success, otherwise fail
+ */
+static int
+woal_set_encode_ext(struct net_device *dev,
+                    struct iw_request_info *info,
+                    struct iw_point *dwrq, char *extra)
+{
+    struct iw_encode_ext *ext = (struct iw_encode_ext *) extra;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int key_index;
+    t_u8 *pkey_material = NULL;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_uap_bss_param sys_cfg;
+    wep_key *pwep_key = NULL;
+    int ret = 0;
+
+    ENTER();
+
+    key_index = (dwrq->flags & IW_ENCODE_INDEX) - 1;
+    if (key_index < 0 || key_index > 3) {
+        ret = -EINVAL;
+        goto done;
+    }
+    if (ext->key_len > (dwrq->length - sizeof(struct iw_encode_ext))) {
+        ret = -EINVAL;
+        goto done;
+    }
+
+    /* Initialize the invalid values so that the correct values below are
+       downloaded to firmware */
+    woal_set_sys_config_invalid_data(&sys_cfg);
+
+    pkey_material = (t_u8 *) (ext + 1);
+    /* Disable Key */
+    if ((dwrq->flags & IW_ENCODE_DISABLED) && !ext->key_len) {
+        sys_cfg.protocol = PROTOCOL_NO_SECURITY;
+    } else if (ext->alg == IW_ENCODE_ALG_WEP) {
+        sys_cfg.protocol = PROTOCOL_STATIC_WEP;
+        /* Set WEP key */
+        switch (key_index) {
+        case 0:
+            pwep_key = &sys_cfg.wep_cfg.key0;
+            break;
+        case 1:
+            pwep_key = &sys_cfg.wep_cfg.key1;
+            break;
+        case 2:
+            pwep_key = &sys_cfg.wep_cfg.key2;
+            break;
+        case 3:
+            pwep_key = &sys_cfg.wep_cfg.key3;
+            break;
+        }
+        pwep_key->key_index = key_index;
+        pwep_key->is_default = MTRUE;
+        pwep_key->length = ext->key_len;
+        memcpy(pwep_key->key, pkey_material, ext->key_len);
+    } else {
+        /* Set GTK/PTK key */
+        req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+        if (req == NULL) {
+            ret = -ENOMEM;
+            goto done;
+        }
+        sec = (mlan_ds_sec_cfg *) req->pbuf;
+        sec->sub_command = MLAN_OID_SEC_CFG_ENCRYPT_KEY;
+        req->req_id = MLAN_IOCTL_SEC_CFG;
+        req->action = MLAN_ACT_SET;
+        sec->param.encrypt_key.key_len = ext->key_len;
+        sec->param.encrypt_key.key_index = key_index;
+        memcpy(sec->param.encrypt_key.key_material, pkey_material,
+               ext->key_len);
+        memcpy(sec->param.encrypt_key.mac_addr, ext->addr.sa_data, ETH_ALEN);
+        sec->param.encrypt_key.key_flags = ext->ext_flags;
+        if (ext->ext_flags & IW_ENCODE_EXT_RX_SEQ_VALID) {
+            memcpy(sec->param.encrypt_key.pn, (t_u8 *) ext->rx_seq,
+                   SEQ_MAX_SIZE);
+            DBG_HEXDUMP(MCMD_D, "Uap Rx PN", sec->param.encrypt_key.pn,
+                        SEQ_MAX_SIZE);
+        }
+        if (ext->ext_flags & IW_ENCODE_EXT_TX_SEQ_VALID) {
+            memcpy(sec->param.encrypt_key.pn, (t_u8 *) ext->tx_seq,
+                   SEQ_MAX_SIZE);
+            DBG_HEXDUMP(MCMD_D, "Uap Tx PN", sec->param.encrypt_key.pn,
+                        SEQ_MAX_SIZE);
+        }
+        PRINTM(MIOCTL,
+               "set uap wpa key key_index=%d, key_len=%d key_flags=0x%x %02x:%02x:%02x:%02x:%02x:%02x\n",
+               key_index, ext->key_len, sec->param.encrypt_key.key_flags,
+               sec->param.encrypt_key.mac_addr[0],
+               sec->param.encrypt_key.mac_addr[1],
+               sec->param.encrypt_key.mac_addr[2],
+               sec->param.encrypt_key.mac_addr[3],
+               sec->param.encrypt_key.mac_addr[4],
+               sec->param.encrypt_key.mac_addr[5]);
+        DBG_HEXDUMP(MCMD_D, "uap wpa key", pkey_material, ext->key_len);
+
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+        }
+        /* Cipher set will be done in set generic IE */
+        priv->pairwise_cipher = ext->alg;
+        priv->group_cipher = ext->alg;
+        goto done;              /* No AP configuration */
+    }
+    if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                       MLAN_ACT_SET,
+                                                       MOAL_IOCTL_WAIT,
+                                                       &sys_cfg)) {
+        PRINTM(MERROR, "Error setting AP confiruration\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief  Extended version of encoding configuration
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param dwrq         A pointer to iw_point structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             -EOPNOTSUPP
+ */
+static int
+woal_get_encode_ext(struct net_device *dev,
+                    struct iw_request_info *info,
+                    struct iw_point *dwrq, char *extra)
+{
+    ENTER();
+    LEAVE();
+    return -EOPNOTSUPP;
+}
+
+/**
+ *  @brief  Request MLME operation
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param dwrq         A pointer to iw_point structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0--success, otherwise fail
+ */
+static int
+woal_set_mlme(struct net_device *dev,
+              struct iw_request_info *info, struct iw_point *dwrq, char *extra)
+{
+    struct iw_mlme *mlme = (struct iw_mlme *) extra;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ds_bss *bss = NULL;
+    mlan_ds_get_info *pinfo = NULL;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sta_list *sta_list = NULL;
+    const t_u8 bc_addr[] = { 0XFF, 0XFF, 0XFF, 0XFF, 0XFF, 0XFF };
+    t_u8 sta_addr[ETH_ALEN];
+    int ret = 0, i;
+
+    ENTER();
+
+    memset(sta_addr, 0, ETH_ALEN);
+    if ((mlme->cmd == IW_MLME_DEAUTH) || (mlme->cmd == IW_MLME_DISASSOC)) {
+        memcpy(sta_addr, (t_u8 *) mlme->addr.sa_data, ETH_ALEN);
+        PRINTM(MIOCTL, "Deauth station: %02x:%02x:%02x:%02x:%02x:%02x, "
+               "reason=%d\n", sta_addr[0], sta_addr[1], sta_addr[2],
+               sta_addr[3], sta_addr[4], sta_addr[5], mlme->reason_code);
+
+        /* FIXME: For flushing all stations we need to use zero MAC, but right
+           now the FW does not support this. So, manually delete each one
+           individually. */
+        /* If deauth all station, get the connected STA list first */
+        if (!memcmp(bc_addr, sta_addr, ETH_ALEN)) {
+            PRINTM(MIOCTL, "Deauth all stations\n");
+            req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_get_info));
+            if (req == NULL) {
+                LEAVE();
+                return -ENOMEM;
+            }
+            pinfo = (mlan_ds_get_info *) req->pbuf;
+            pinfo->sub_command = MLAN_OID_UAP_STA_LIST;
+            req->req_id = MLAN_IOCTL_GET_INFO;
+            req->action = MLAN_ACT_GET;
+            if (MLAN_STATUS_SUCCESS !=
+                woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+                ret = -EFAULT;
+                goto done;
+            }
+            sta_list =
+                (mlan_ds_sta_list *) kmalloc(sizeof(mlan_ds_sta_list),
+                                             GFP_KERNEL);
+            if (sta_list == NULL) {
+                PRINTM(MERROR, "Memory allocation failed!\n");
+                ret = -ENOMEM;
+                goto done;
+            }
+            memcpy(sta_list, &pinfo->param.sta_list, sizeof(mlan_ds_sta_list));
+            if (req)
+                kfree(req);
+        }
+        req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+        if (req == NULL) {
+            LEAVE();
+            return -ENOMEM;
+        }
+        bss = (mlan_ds_bss *) req->pbuf;
+        bss->sub_command = MLAN_OID_UAP_DEAUTH_STA;
+        req->req_id = MLAN_IOCTL_BSS;
+        req->action = MLAN_ACT_SET;
+
+        if (!memcmp(bc_addr, sta_addr, ETH_ALEN)) {
+            for (i = 0; i < sta_list->sta_count; i++) {
+                memcpy(bss->param.deauth_param.mac_addr,
+                       sta_list->info[i].mac_address, ETH_ALEN);
+                bss->param.deauth_param.reason_code = mlme->reason_code;
+
+                if (MLAN_STATUS_SUCCESS !=
+                    woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+                    ret = -EFAULT;
+                    goto done;
+                }
+            }
+        } else {
+            memcpy(bss->param.deauth_param.mac_addr, sta_addr, ETH_ALEN);
+            bss->param.deauth_param.reason_code = mlme->reason_code;
+
+            if (MLAN_STATUS_SUCCESS !=
+                woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+                ret = -EFAULT;
+                goto done;
+            }
+        }
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    if (sta_list)
+        kfree(sta_list);
+
+    LEAVE();
+    return ret;
+}
+
+/** @brief Set authentication mode parameters
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_auth(struct net_device *dev, struct iw_request_info *info,
+              struct iw_param *vwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_uap_bss_param sys_cfg;
+
+    ENTER();
+
+    /* Initialize the invalid values so that the correct values below are
+       downloaded to firmware */
+    woal_set_sys_config_invalid_data(&sys_cfg);
+
+    switch (vwrq->flags & IW_AUTH_INDEX) {
+    case IW_AUTH_CIPHER_PAIRWISE:
+        /* Rest are not supported now */
+        if (vwrq->value & IW_AUTH_CIPHER_NONE);
+        else if (vwrq->value & IW_AUTH_CIPHER_WEP40);
+        else if (vwrq->value & IW_AUTH_CIPHER_WEP104);
+        else if (vwrq->value == IW_AUTH_CIPHER_TKIP) {
+            sys_cfg.wpa_cfg.pairwise_cipher_wpa = CIPHER_TKIP;
+            sys_cfg.wpa_cfg.pairwise_cipher_wpa2 = CIPHER_TKIP;
+            priv->pairwise_cipher = CIPHER_TKIP;
+            if (!priv->uap_key_mgmt)
+                priv->uap_key_mgmt = KEY_MGMT_PSK;
+            PRINTM(MINFO, "Set Auth Cipher TKIP\n");
+        } else if (vwrq->value == IW_AUTH_CIPHER_CCMP) {
+            sys_cfg.wpa_cfg.pairwise_cipher_wpa = CIPHER_AES_CCMP;
+            sys_cfg.wpa_cfg.pairwise_cipher_wpa2 = CIPHER_AES_CCMP;
+            priv->pairwise_cipher = CIPHER_AES_CCMP;
+            if (!priv->uap_key_mgmt)
+                priv->uap_key_mgmt = KEY_MGMT_PSK;
+            PRINTM(MINFO, "Set Auth Cipher CCMP\n");
+        } else if (vwrq->value == (IW_AUTH_CIPHER_TKIP | IW_AUTH_CIPHER_CCMP)) {
+            sys_cfg.wpa_cfg.pairwise_cipher_wpa =
+                (CIPHER_TKIP | CIPHER_AES_CCMP);
+            sys_cfg.wpa_cfg.pairwise_cipher_wpa2 =
+                (CIPHER_TKIP | CIPHER_AES_CCMP);
+            priv->pairwise_cipher = (CIPHER_TKIP | CIPHER_AES_CCMP);
+            if (!priv->uap_key_mgmt)
+                priv->uap_key_mgmt = KEY_MGMT_PSK;
+            PRINTM(MINFO, "Set Auth Cipher TKIP + CCMP\n");
+        }
+        break;
+    case IW_AUTH_CIPHER_GROUP:
+        /* Rest are not supported now */
+        if (vwrq->value & IW_AUTH_CIPHER_NONE);
+        else if (vwrq->value & IW_AUTH_CIPHER_WEP40);
+        else if (vwrq->value & IW_AUTH_CIPHER_WEP104);
+        else if (vwrq->value & IW_AUTH_CIPHER_TKIP) {
+            sys_cfg.wpa_cfg.group_cipher = CIPHER_TKIP;
+            priv->group_cipher = CIPHER_TKIP;
+            if (!priv->uap_key_mgmt)
+                priv->uap_key_mgmt = KEY_MGMT_PSK;
+            PRINTM(MINFO, "Set Auth Cipher TKIP\n");
+        } else if (vwrq->value & IW_AUTH_CIPHER_CCMP) {
+            sys_cfg.wpa_cfg.group_cipher = CIPHER_AES_CCMP;
+            priv->group_cipher = CIPHER_AES_CCMP;
+            if (!priv->uap_key_mgmt)
+                priv->uap_key_mgmt = KEY_MGMT_PSK;
+            PRINTM(MINFO, "Set Auth Cipher CCMP\n");
+        }
+        break;
+    case IW_AUTH_80211_AUTH_ALG:
+        switch (vwrq->value) {
+        case IW_AUTH_ALG_SHARED_KEY:
+            PRINTM(MINFO, "Auth mode shared key!\n");
+            sys_cfg.auth_mode = MLAN_AUTH_MODE_SHARED;
+            break;
+        case IW_AUTH_ALG_LEAP:
+            break;
+        case IW_AUTH_ALG_OPEN_SYSTEM:
+            PRINTM(MINFO, "Auth mode open!\n");
+            sys_cfg.auth_mode = MLAN_AUTH_MODE_OPEN;
+            break;
+        default:
+            PRINTM(MINFO, "Auth mode auto!\n");
+            break;
+        }
+        break;
+    case IW_AUTH_WPA_VERSION:
+        switch (vwrq->value) {
+        case IW_AUTH_WPA_VERSION_DISABLED:
+            sys_cfg.protocol = PROTOCOL_NO_SECURITY;
+            break;
+        case IW_AUTH_WPA_VERSION_WPA:
+            sys_cfg.protocol = PROTOCOL_WPA;
+            break;
+        case IW_AUTH_WPA_VERSION_WPA2:
+            sys_cfg.protocol = PROTOCOL_WPA2;
+            break;
+        case IW_AUTH_WPA_VERSION_WPA | IW_AUTH_WPA_VERSION_WPA2:
+            sys_cfg.protocol = PROTOCOL_WPA2_MIXED;
+            break;
+        default:
+            break;
+        }
+        priv->uap_protocol = sys_cfg.protocol;
+        break;
+    case IW_AUTH_KEY_MGMT:
+        switch (vwrq->value) {
+        case IW_AUTH_KEY_MGMT_802_1X:
+            sys_cfg.key_mgmt |= KEY_MGMT_EAP;
+            priv->uap_key_mgmt |= KEY_MGMT_EAP;
+            break;
+        case IW_AUTH_KEY_MGMT_PSK:
+            sys_cfg.key_mgmt |= KEY_MGMT_PSK;
+            priv->uap_key_mgmt |= KEY_MGMT_PSK;
+            break;
+        default:
+            break;
+        }
+        break;
+    case IW_AUTH_WPA_ENABLED:
+    case IW_AUTH_TKIP_COUNTERMEASURES:
+    case IW_AUTH_DROP_UNENCRYPTED:
+    case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+    case IW_AUTH_ROAMING_CONTROL:
+    case IW_AUTH_PRIVACY_INVOKED:
+    default:
+        LEAVE();
+        return -EOPNOTSUPP;     /* No AP configuration */
+    }
+    if (!sys_cfg.key_mgmt)
+        sys_cfg.key_mgmt = priv->uap_key_mgmt;
+    if (sys_cfg.key_mgmt & KEY_MGMT_PSK)
+        sys_cfg.key_mgmt_operation |= 0x01;
+    if (sys_cfg.key_mgmt & KEY_MGMT_EAP)
+        sys_cfg.key_mgmt_operation |= 0x03;
+    if (!sys_cfg.protocol)
+        sys_cfg.protocol = priv->uap_protocol;
+
+    /* Set AP configuration */
+    if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                       MLAN_ACT_SET,
+                                                       MOAL_IOCTL_WAIT,
+                                                       &sys_cfg)) {
+        PRINTM(MERROR, "Error setting AP confiruration\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Get authentication mode parameters
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_get_auth(struct net_device *dev, struct iw_request_info *info,
+              struct iw_param *vwrq, char *extra)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_uap_bss_param ap_cfg;
+
+    ENTER();
+
+    if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                       MLAN_ACT_GET,
+                                                       MOAL_IOCTL_WAIT,
+                                                       &ap_cfg)) {
+        PRINTM(MERROR, "Error getting AP confiruration\n");
+        LEAVE();
+        return -EFAULT;
+    }
+    switch (vwrq->flags & IW_AUTH_INDEX) {
+    case IW_AUTH_CIPHER_PAIRWISE:
+        if (ap_cfg.wpa_cfg.pairwise_cipher_wpa == CIPHER_TKIP ||
+            ap_cfg.wpa_cfg.pairwise_cipher_wpa2 == CIPHER_TKIP)
+            vwrq->value = IW_AUTH_CIPHER_TKIP;
+        else if (ap_cfg.wpa_cfg.pairwise_cipher_wpa == CIPHER_AES_CCMP ||
+                 ap_cfg.wpa_cfg.pairwise_cipher_wpa2 == CIPHER_AES_CCMP)
+            vwrq->value = IW_AUTH_CIPHER_CCMP;
+        else
+            vwrq->value = IW_AUTH_CIPHER_NONE;
+        break;
+    case IW_AUTH_CIPHER_GROUP:
+        if (ap_cfg.wpa_cfg.group_cipher == CIPHER_TKIP)
+            vwrq->value = IW_AUTH_CIPHER_TKIP;
+        else if (ap_cfg.wpa_cfg.group_cipher == CIPHER_AES_CCMP)
+            vwrq->value = IW_AUTH_CIPHER_CCMP;
+        else
+            vwrq->value = IW_AUTH_CIPHER_NONE;
+        break;
+    case IW_AUTH_80211_AUTH_ALG:
+        if (ap_cfg.auth_mode == MLAN_AUTH_MODE_SHARED)
+            vwrq->value = IW_AUTH_ALG_SHARED_KEY;
+        else if (ap_cfg.auth_mode == MLAN_AUTH_MODE_NETWORKEAP)
+            vwrq->value = IW_AUTH_ALG_LEAP;
+        else
+            vwrq->value = IW_AUTH_ALG_OPEN_SYSTEM;
+        break;
+    case IW_AUTH_WPA_ENABLED:
+        if (ap_cfg.protocol == PROTOCOL_WPA ||
+            ap_cfg.protocol == PROTOCOL_WPA2 ||
+            ap_cfg.protocol == PROTOCOL_WPA2_MIXED)
+            vwrq->value = 1;
+        else
+            vwrq->value = 0;
+        break;
+    case IW_AUTH_KEY_MGMT:
+        if (ap_cfg.key_mgmt & KEY_MGMT_EAP)
+            vwrq->value |= IW_AUTH_KEY_MGMT_802_1X;
+        if (ap_cfg.key_mgmt & KEY_MGMT_PSK)
+            vwrq->value |= IW_AUTH_KEY_MGMT_PSK;
+        break;
+    case IW_AUTH_WPA_VERSION:
+    case IW_AUTH_TKIP_COUNTERMEASURES:
+    case IW_AUTH_DROP_UNENCRYPTED:
+    case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+    case IW_AUTH_ROAMING_CONTROL:
+    case IW_AUTH_PRIVACY_INVOKED:
+    default:
+        LEAVE();
+        return -EOPNOTSUPP;
+    }
+
+    LEAVE();
+    return 0;
+}
+#endif /* WE >= 18 */
+
+/* Data rate listing
+ *      MULTI_BANDS:
+ *              abg             a       b       b/g
+ *  Infra       G(12)           A(8)    B(4)    G(12)
+ *  Adhoc       A+B(12)         A(8)    B(4)    B(4)
+ *      non-MULTI_BANDS:
+                                        b       b/g
+ *  Infra                               B(4)    G(12)
+ *  Adhoc                               B(4)    B(4)
+ */
+/**
+ *  @brief Get Range Info
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param dwrq                 A pointer to iw_point structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_get_range(struct net_device *dev, struct iw_request_info *info,
+               struct iw_point *dwrq, char *extra)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_uap_bss_param ap_cfg;
+    struct iw_range *range = (struct iw_range *) extra;
+    t_u8 band = 0;
+    int i;
+
+    ENTER();
+
+    if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                       MLAN_ACT_GET,
+                                                       MOAL_IOCTL_WAIT,
+                                                       &ap_cfg)) {
+        PRINTM(MERROR, "Error getting AP confiruration\n");
+        LEAVE();
+        return -EFAULT;
+    }
+    dwrq->length = sizeof(struct iw_range);
+    memset(range, 0, sizeof(struct iw_range));
+
+    range->min_nwid = 0;
+    range->max_nwid = 0;
+
+    range->num_bitrates = MAX_DATA_RATES;
+    for (i = 0; i < MIN(MAX_DATA_RATES, IW_MAX_BITRATES) &&
+         ap_cfg.rates[i]; i++) {
+        range->bitrate[i] = (ap_cfg.rates[i] & 0x7f) * 500000;
+    }
+    range->num_bitrates = i;
+    PRINTM(MINFO, "IW_MAX_BITRATES=%d num_bitrates=%d\n",
+           IW_MAX_BITRATES, range->num_bitrates);
+
+    range->num_frequency = MIN(ap_cfg.num_of_chan, IW_MAX_FREQUENCIES);
+
+    for (i = 0; i < range->num_frequency; i++) {
+        range->freq[i].i = (long) ap_cfg.chan_list[i].chan_number;
+        band = ap_cfg.chan_list[i].band_config_type & BAND_CONFIG_5GHZ;
+        range->freq[i].m =
+            (long) channel_to_frequency(ap_cfg.chan_list[i].chan_number,
+                                        band) * 100000;
+        range->freq[i].e = 1;
+    }
+
+    PRINTM(MINFO, "IW_MAX_FREQUENCIES=%d num_frequency=%d\n",
+           IW_MAX_FREQUENCIES, range->num_frequency);
+
+    range->num_channels = range->num_frequency;
+
+    woal_sort_channels(&range->freq[0], range->num_frequency);
+
+    /*
+     * Set an indication of the max TCP throughput in bit/s that we can
+     * expect using this interface
+     */
+    if (i > 2)
+        range->throughput = 5000 * 1000;
+    else
+        range->throughput = 1500 * 1000;
+
+    range->min_rts = MLAN_RTS_MIN_VALUE;
+    range->max_rts = MLAN_RTS_MAX_VALUE;
+    range->min_frag = MLAN_FRAG_MIN_VALUE;
+    range->max_frag = MLAN_FRAG_MAX_VALUE;
+
+    range->encoding_size[0] = 5;
+    range->encoding_size[1] = 13;
+    range->num_encoding_sizes = 2;
+    range->max_encoding_tokens = 4;
+
+/** Minimum power period */
+#define IW_POWER_PERIOD_MIN 1000000     /* 1 sec */
+/** Maximum power period */
+#define IW_POWER_PERIOD_MAX 120000000   /* 2 min */
+/** Minimum power timeout value */
+#define IW_POWER_TIMEOUT_MIN 1000       /* 1 ms */
+/** Maximim power timeout value */
+#define IW_POWER_TIMEOUT_MAX 1000000    /* 1 sec */
+
+    /* Power Management duration & timeout */
+    range->min_pmp = IW_POWER_PERIOD_MIN;
+    range->max_pmp = IW_POWER_PERIOD_MAX;
+    range->min_pmt = IW_POWER_TIMEOUT_MIN;
+    range->max_pmt = IW_POWER_TIMEOUT_MAX;
+    range->pmp_flags = IW_POWER_PERIOD;
+    range->pmt_flags = IW_POWER_TIMEOUT;
+    range->pm_capa = IW_POWER_PERIOD | IW_POWER_TIMEOUT | IW_POWER_ALL_R;
+
+    /*
+     * Minimum version we recommend
+     */
+    range->we_version_source = 15;
+
+    /*
+     * Version we are compiled with
+     */
+    range->we_version_compiled = WIRELESS_EXT;
+
+    range->retry_capa = IW_RETRY_LIMIT;
+    range->retry_flags = IW_RETRY_LIMIT | IW_RETRY_MAX;
+
+    range->min_retry = MLAN_TX_RETRY_MIN;
+    range->max_retry = MLAN_TX_RETRY_MAX;
+
+#if (WIRELESS_EXT >= 18)
+    if (ap_cfg.protocol & PROTOCOL_WPA)
+        range->enc_capa |= IW_ENC_CAPA_WPA;
+    if (ap_cfg.protocol & PROTOCOL_WPA2)
+        range->enc_capa |= IW_ENC_CAPA_WPA2;
+    if (ap_cfg.wpa_cfg.pairwise_cipher_wpa == CIPHER_TKIP ||
+        ap_cfg.wpa_cfg.pairwise_cipher_wpa2 == CIPHER_TKIP ||
+        ap_cfg.wpa_cfg.group_cipher == CIPHER_TKIP)
+        range->enc_capa |= IW_ENC_CAPA_CIPHER_TKIP;
+    if (ap_cfg.wpa_cfg.pairwise_cipher_wpa == CIPHER_AES_CCMP ||
+        ap_cfg.wpa_cfg.pairwise_cipher_wpa2 == CIPHER_AES_CCMP ||
+        ap_cfg.wpa_cfg.group_cipher == CIPHER_AES_CCMP)
+        range->enc_capa |= IW_ENC_CAPA_CIPHER_CCMP;
+#endif
+
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Set priv command
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param dwrq         A pointer to iw_point structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_priv(struct net_device *dev, struct iw_request_info *info,
+              struct iw_point *dwrq, char *extra)
+{
+    ENTER();
+    LEAVE();
+    return -EOPNOTSUPP;
+}
+
+/**
+ *  @brief Set essid
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param dwrq         A pointer to iw_point structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0--success, otherwise fail
+ */
+static int
+woal_set_essid(struct net_device *dev, struct iw_request_info *info,
+               struct iw_point *dwrq, char *extra)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_uap_bss_param sys_cfg;
+    int ret = 0;
+
+    ENTER();
+
+    /* Check the size of the string */
+    if (dwrq->length > IW_ESSID_MAX_SIZE + 1) {
+        ret = -E2BIG;
+        goto done;
+    }
+
+    /* Initialize the invalid values so that the correct values below are
+       downloaded to firmware */
+    woal_set_sys_config_invalid_data(&sys_cfg);
+
+    /* Set the SSID */
+#if WIRELESS_EXT > 20
+    sys_cfg.ssid.ssid_len = dwrq->length;
+#else
+    sys_cfg.ssid.ssid_len = dwrq->length - 1;
+#endif
+
+    memcpy(sys_cfg.ssid.ssid, extra,
+           MIN(sys_cfg.ssid.ssid_len, MLAN_MAX_SSID_LENGTH));
+    if (!sys_cfg.ssid.ssid_len || sys_cfg.ssid.ssid[0] < 0x20) {
+        PRINTM(MERROR, "Invalid SSID - aborting set_essid\n");
+        ret = -EINVAL;
+        goto done;
+    }
+    PRINTM(MINFO, "Requested new SSID = %s\n",
+           (sys_cfg.ssid.ssid_len > 0) ? (char *) sys_cfg.ssid.ssid : "NULL");
+
+    /* Set AP configuration */
+    if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                       MLAN_ACT_SET,
+                                                       MOAL_IOCTL_WAIT,
+                                                       &sys_cfg)) {
+        PRINTM(MERROR, "Error setting AP confiruration\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get current essid
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param info     A pointer to iw_request_info structure
+ *  @param dwrq     A pointer to iw_point structure
+ *  @param extra    A pointer to extra data buf
+ *
+ *  @return         0--success, otherwise fail
+ */
+static int
+woal_get_essid(struct net_device *dev, struct iw_request_info *info,
+               struct iw_point *dwrq, char *extra)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_uap_bss_param ap_cfg;
+
+    ENTER();
+
+    if (MLAN_STATUS_SUCCESS != woal_set_get_sys_config(priv,
+                                                       MLAN_ACT_GET,
+                                                       MOAL_IOCTL_WAIT,
+                                                       &ap_cfg)) {
+        PRINTM(MERROR, "Error getting AP confiruration\n");
+        LEAVE();
+        return -EFAULT;
+    }
+
+    if (priv->bss_started) {
+        dwrq->length = MIN(dwrq->length, ap_cfg.ssid.ssid_len);
+        memcpy(extra, ap_cfg.ssid.ssid, dwrq->length);
+    } else
+        dwrq->length = 0;
+
+    dwrq->flags = 1;
+
+    LEAVE();
+    return 0;
+}
+
+/**
+ * iwconfig settable callbacks
+ */
+static const iw_handler woal_handler[] = {
+    (iw_handler) woal_config_commit,    /* SIOCSIWCOMMIT */
+    (iw_handler) woal_get_name, /* SIOCGIWNAME */
+    (iw_handler) NULL,          /* SIOCSIWNWID */
+    (iw_handler) NULL,          /* SIOCGIWNWID */
+    (iw_handler) woal_set_freq, /* SIOCSIWFREQ */
+    (iw_handler) woal_get_freq, /* SIOCGIWFREQ */
+    (iw_handler) woal_set_bss_mode,     /* SIOCSIWMODE */
+    (iw_handler) woal_get_bss_mode,     /* SIOCGIWMODE */
+    (iw_handler) NULL,          /* SIOCSIWSENS */
+    (iw_handler) NULL,          /* SIOCGIWSENS */
+    (iw_handler) NULL,          /* SIOCSIWRANGE */
+    (iw_handler) woal_get_range,        /* SIOCGIWRANGE */
+    (iw_handler) woal_set_priv, /* SIOCSIWPRIV */
+    (iw_handler) NULL,          /* SIOCGIWPRIV */
+    (iw_handler) NULL,          /* SIOCSIWSTATS */
+    (iw_handler) NULL,          /* SIOCGIWSTATS */
+#if WIRELESS_EXT > 15
+    iw_handler_set_spy,         /* SIOCSIWSPY */
+    iw_handler_get_spy,         /* SIOCGIWSPY */
+    iw_handler_set_thrspy,      /* SIOCSIWTHRSPY */
+    iw_handler_get_thrspy,      /* SIOCGIWTHRSPY */
+#else /* WIRELESS_EXT > 15 */
+#ifdef WIRELESS_SPY
+    (iw_handler) NULL,          /* SIOCSIWSPY */
+    (iw_handler) NULL,          /* SIOCGIWSPY */
+#else /* WIRELESS_SPY */
+    (iw_handler) NULL,          /* SIOCSIWSPY */
+    (iw_handler) NULL,          /* SIOCGIWSPY */
+#endif /* WIRELESS_SPY */
+    (iw_handler) NULL,          /* -- hole -- */
+    (iw_handler) NULL,          /* -- hole -- */
+#endif /* WIRELESS_EXT > 15 */
+    (iw_handler) woal_set_wap,  /* SIOCSIWAP */
+    (iw_handler) woal_get_wap,  /* SIOCGIWAP */
+#if WIRELESS_EXT >= 18
+    (iw_handler) woal_set_mlme, /* SIOCSIWMLME */
+#else
+    (iw_handler) NULL,          /* -- hole -- */
+#endif
+    /* (iw_handler) wlan_get_aplist, *//* SIOCGIWAPLIST */
+    NULL,                       /* SIOCGIWAPLIST */
+#if WIRELESS_EXT > 13
+    (iw_handler) NULL,          /* SIOCSIWSCAN */
+    (iw_handler) NULL,          /* SIOCGIWSCAN */
+#else /* WIRELESS_EXT > 13 */
+    (iw_handler) NULL,          /* SIOCSIWSCAN */
+    (iw_handler) NULL,          /* SIOCGIWSCAN */
+#endif /* WIRELESS_EXT > 13 */
+    (iw_handler) woal_set_essid,        /* SIOCSIWESSID */
+    (iw_handler) woal_get_essid,        /* SIOCGIWESSID */
+    (iw_handler) NULL,          /* SIOCSIWNICKN */
+    (iw_handler) NULL,          /* SIOCGIWNICKN */
+    (iw_handler) NULL,          /* -- hole -- */
+    (iw_handler) NULL,          /* -- hole -- */
+    (iw_handler) NULL,          /* SIOCSIWRATE */
+    (iw_handler) NULL,          /* SIOCGIWRATE */
+    (iw_handler) NULL,          /* SIOCSIWRTS */
+    (iw_handler) NULL,          /* SIOCGIWRTS */
+    (iw_handler) NULL,          /* SIOCSIWFRAG */
+    (iw_handler) NULL,          /* SIOCGIWFRAG */
+    (iw_handler) NULL,          /* SIOCSIWTXPOW */
+    (iw_handler) NULL,          /* SIOCGIWTXPOW */
+    (iw_handler) NULL,          /* SIOCSIWRETRY */
+    (iw_handler) NULL,          /* SIOCGIWRETRY */
+    (iw_handler) woal_set_encode,       /* SIOCSIWENCODE */
+    (iw_handler) woal_get_encode,       /* SIOCGIWENCODE */
+    (iw_handler) NULL,          /* SIOCSIWPOWER */
+    (iw_handler) NULL,          /* SIOCGIWPOWER */
+#if (WIRELESS_EXT >= 18)
+    (iw_handler) NULL,          /* -- hole -- */
+    (iw_handler) NULL,          /* -- hole -- */
+    (iw_handler) woal_set_gen_ie,       /* SIOCSIWGENIE */
+    (iw_handler) woal_get_gen_ie,       /* SIOCGIWGENIE */
+    (iw_handler) woal_set_auth, /* SIOCSIWAUTH */
+    (iw_handler) woal_get_auth, /* SIOCGIWAUTH */
+    (iw_handler) woal_set_encode_ext,   /* SIOCSIWENCODEEXT */
+    (iw_handler) woal_get_encode_ext,   /* SIOCGIWENCODEEXT */
+#endif /* WIRELESSS_EXT >= 18 */
+};
+
+/**
+ * iwpriv settable callbacks
+ */
+static const iw_handler woal_private_handler[] = {
+    NULL,                       /* SIOCIWFIRSTPRIV */
+};
+
+/********************************************************
+                Global Functions
+********************************************************/
+
+/** wlan_handler_def */
+struct iw_handler_def woal_uap_handler_def = {
+  num_standard:sizeof(woal_handler) / sizeof(iw_handler),
+  num_private:sizeof(woal_private_handler) / sizeof(iw_handler),
+  num_private_args:sizeof(woal_uap_priv_args) /
+        sizeof(struct iw_priv_args),
+  standard:(iw_handler *) woal_handler,
+  private:(iw_handler *) woal_private_handler,
+  private_args:(struct iw_priv_args *) woal_uap_priv_args,
+#if WIRELESS_EXT > 20
+  get_wireless_stats:woal_get_uap_wireless_stats,
+#endif
+};
+
+/**
+ *  @brief Get wireless statistics
+ *
+ *  @param dev          A pointer to net_device structure
+ *
+ *  @return             A pointer to iw_statistics buf
+ */
+struct iw_statistics *
+woal_get_uap_wireless_stats(struct net_device *dev)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    t_u16 wait_option = MOAL_NO_WAIT;
+
+    ENTER();
+
+    /*
+     * Since schedule() is not allowed from an atomic context
+     * such as when dev_base_lock for netdevices is acquired
+     * for reading/writing in kernel before this call, HostCmd
+     * is issued in non-blocking way in such contexts and
+     * blocking in other cases.
+     */
+    if (write_can_lock(&dev_base_lock)
+        && (!in_atomic() || current->exit_state))
+        wait_option = MOAL_WSTATS_WAIT;
+
+    priv->w_stats.qual.qual = 0;
+    priv->w_stats.qual.level = 0;
+    priv->w_stats.discard.code = 0;
+    priv->w_stats.status = IW_MODE_MASTER;
+    woal_uap_get_stats(priv, wait_option, NULL);
+
+    LEAVE();
+    return &priv->w_stats;
+}
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_wext.c b/drivers/net/wireless/sd8797/mlinux/moal_wext.c
new file mode 100644
index 000000000000..a967c939f009
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_wext.c
@@ -0,0 +1,3051 @@
+/** @file  moal_wext.c
+  *
+  * @brief This file contains wireless extension standard ioctl functions
+  *
+  * Copyright (C) 2008-2011, Marvell International Ltd.
+  *
+  * This software file (the "File") is distributed by Marvell International
+  * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+  * (the "License").  You may use, redistribute and/or modify this File in
+  * accordance with the terms and conditions of the License, a copy of which
+  * is available by writing to the Free Software Foundation, Inc.,
+  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+  * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+  *
+  * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+  * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+  * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+  * this warranty disclaimer.
+  *
+  */
+
+/************************************************************************
+Change log:
+    10/21/2008: initial version
+************************************************************************/
+
+#include        "moal_main.h"
+
+#ifdef STA_SUPPORT
+/** Approximate amount of data needed to pass a scan result back to iwlist */
+#define MAX_SCAN_CELL_SIZE  (IW_EV_ADDR_LEN             \
+                             + MLAN_MAX_SSID_LENGTH   \
+                             + IW_EV_UINT_LEN           \
+                             + IW_EV_FREQ_LEN           \
+                             + IW_EV_QUAL_LEN           \
+                             + MLAN_MAX_SSID_LENGTH    \
+                             + IW_EV_PARAM_LEN          \
+                             + 40)      /* 40 for WPAIE */
+/** Macro for minimum size of scan buffer */
+#define MIN_ACCEPTED_GET_SCAN_BUF 8000
+
+/********************************************************
+                Global Variables
+********************************************************/
+
+/********************************************************
+                Local Functions
+********************************************************/
+/**
+ *  @brief This function validates a SSID as being able to be printed
+ *
+ *  @param pssid   SSID structure to validate
+ *
+ *  @return        MTRUE or MFALSE
+ */
+static BOOLEAN
+woal_ssid_valid(mlan_802_11_ssid * pssid)
+{
+#ifdef ASCII_SSID_CHECK
+    unsigned int ssid_idx;
+
+    ENTER();
+
+    for (ssid_idx = 0; ssid_idx < pssid->ssid_len; ssid_idx++) {
+        if ((pssid->ssid[ssid_idx] < 0x20) || (pssid->ssid[ssid_idx] > 0x7e)) {
+            LEAVE();
+            return MFALSE;
+        }
+    }
+    LEAVE();
+#endif
+    return MTRUE;
+}
+
+/**
+ *  @brief Compare two SSIDs
+ *
+ *  @param ssid1    A pointer to ssid to compare
+ *  @param ssid2    A pointer to ssid to compare
+ *
+ *  @return         0--ssid is same, otherwise is different
+ */
+static t_s32
+woal_ssid_cmp(mlan_802_11_ssid * ssid1, mlan_802_11_ssid * ssid2)
+{
+    ENTER();
+
+    if (!ssid1 || !ssid2) {
+        LEAVE();
+        return -1;
+    }
+    if (ssid1->ssid_len != ssid2->ssid_len) {
+        LEAVE();
+        return -1;
+    }
+
+    LEAVE();
+    return memcmp(ssid1->ssid, ssid2->ssid, ssid1->ssid_len);
+}
+
+/**
+ *  @brief Sort Channels
+ *
+ *  @param freq                 A pointer to iw_freq structure
+ *  @param num                  Number of Channels
+ *
+ *  @return                     N/A
+ */
+static inline void
+woal_sort_channels(struct iw_freq *freq, int num)
+{
+    int i, j;
+    struct iw_freq temp;
+
+    for (i = 0; i < num; i++)
+        for (j = i + 1; j < num; j++)
+            if (freq[i].i > freq[j].i) {
+                temp.i = freq[i].i;
+                temp.m = freq[i].m;
+
+                freq[i].i = freq[j].i;
+                freq[i].m = freq[j].m;
+
+                freq[j].i = temp.i;
+                freq[j].m = temp.m;
+            }
+}
+
+/**
+ *  @brief Convert RSSI to quality
+ *
+ *  @param rssi     RSSI in dBm
+ *
+ *  @return         Quality of the link (0-5)
+ */
+static t_u8
+woal_rssi_to_quality(t_s16 rssi)
+{
+/** Macro for RSSI range */
+#define MOAL_RSSI_NO_SIGNAL -90
+#define MOAL_RSSI_VERY_LOW  -80
+#define MOAL_RSSI_LOW       -70
+#define MOAL_RSSI_GOOD      -60
+#define MOAL_RSSI_VERY_GOOD -50
+#define MOAL_RSSI_INVALID   0
+    if (rssi <= MOAL_RSSI_NO_SIGNAL || rssi == MOAL_RSSI_INVALID)
+        return 0;
+    else if (rssi <= MOAL_RSSI_VERY_LOW)
+        return 1;
+    else if (rssi <= MOAL_RSSI_LOW)
+        return 2;
+    else if (rssi <= MOAL_RSSI_GOOD)
+        return 3;
+    else if (rssi <= MOAL_RSSI_VERY_GOOD)
+        return 4;
+    else
+        return 5;
+}
+
+/**
+ *  @brief Set Adapter Node Name
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param dwrq                 A pointer to iw_point structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_nick(struct net_device *dev, struct iw_request_info *info,
+              struct iw_point *dwrq, char *extra)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    ENTER();
+    /*
+     * Check the size of the string
+     */
+    if (dwrq->length > 16) {
+        LEAVE();
+        return -E2BIG;
+    }
+    memset(priv->nick_name, 0, sizeof(priv->nick_name));
+    memcpy(priv->nick_name, extra, dwrq->length);
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Get Adapter Node Name
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param dwrq                 A pointer to iw_point structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success
+ */
+static int
+woal_get_nick(struct net_device *dev, struct iw_request_info *info,
+              struct iw_point *dwrq, char *extra)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    ENTER();
+    /*
+     * Get the Nick Name saved
+     */
+    strncpy(extra, (char *) priv->nick_name, 16);
+    extra[16] = '\0';
+    /*
+     * If none, we may want to get the one that was set
+     */
+
+    /*
+     * Push it out !
+     */
+    dwrq->length = strlen(extra) + 1;
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Commit handler: called after a bunch of SET operations
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param cwrq         A pointer to char buffer
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0 --success
+ */
+static int
+woal_config_commit(struct net_device *dev,
+                   struct iw_request_info *info, char *cwrq, char *extra)
+{
+    ENTER();
+
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Get name
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param cwrq         A pointer to char buffer
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0 --success
+ */
+static int
+woal_get_name(struct net_device *dev, struct iw_request_info *info,
+              char *cwrq, char *extra)
+{
+    ENTER();
+    strcpy(cwrq, "IEEE 802.11-DS");
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Set frequency
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param fwrq                 A pointer to iw_freq structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_freq(struct net_device *dev, struct iw_request_info *info,
+              struct iw_freq *fwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ds_bss *bss = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    bss = (mlan_ds_bss *) req->pbuf;
+    /*
+     * If setting by frequency, convert to a channel
+     */
+    if (fwrq->e == 1) {
+        long f = fwrq->m / 100000;
+        bss->param.bss_chan.freq = f;
+    } else
+        bss->param.bss_chan.channel = fwrq->m;
+
+    bss->sub_command = MLAN_OID_BSS_CHANNEL;
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = MLAN_ACT_SET;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (MLAN_STATUS_SUCCESS !=
+        woal_change_adhoc_chan(priv, bss->param.bss_chan.channel))
+        ret = -EFAULT;
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get frequency
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param fwrq                 A pointer to iw_freq structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_get_freq(struct net_device *dev, struct iw_request_info *info,
+              struct iw_freq *fwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ds_bss *bss = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_BSS_CHANNEL;
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = MLAN_ACT_GET;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    fwrq->m = (long) bss->param.bss_chan.freq * 100000;
+    fwrq->i = (long) bss->param.bss_chan.channel;
+    fwrq->e = 1;
+    fwrq->flags = IW_FREQ_FIXED;
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set wlan mode
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param uwrq                 Wireless mode to set
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_bss_mode(struct net_device *dev, struct iw_request_info *info,
+                  t_u32 * uwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ds_bss *bss = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_bss));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    bss = (mlan_ds_bss *) req->pbuf;
+    bss->sub_command = MLAN_OID_BSS_MODE;
+    req->req_id = MLAN_IOCTL_BSS;
+    req->action = MLAN_ACT_SET;
+
+    switch (*uwrq) {
+    case IW_MODE_INFRA:
+        bss->param.bss_mode = MLAN_BSS_MODE_INFRA;
+        break;
+    case IW_MODE_ADHOC:
+        bss->param.bss_mode = MLAN_BSS_MODE_IBSS;
+        break;
+    case IW_MODE_AUTO:
+        bss->param.bss_mode = MLAN_BSS_MODE_AUTO;
+        break;
+    default:
+        ret = -EINVAL;
+        break;
+    }
+    if (ret)
+        goto done;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get current BSSID
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param awrq         A pointer to sockaddr structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0 --success
+ */
+static int
+woal_get_wap(struct net_device *dev, struct iw_request_info *info,
+             struct sockaddr *awrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_bss_info bss_info;
+
+    ENTER();
+
+    memset(&bss_info, 0, sizeof(bss_info));
+
+    woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info);
+
+    if (bss_info.media_connected == MTRUE) {
+        memcpy(awrq->sa_data, &bss_info.bssid, MLAN_MAC_ADDR_LENGTH);
+    } else {
+        memset(awrq->sa_data, 0, MLAN_MAC_ADDR_LENGTH);
+    }
+    awrq->sa_family = ARPHRD_ETHER;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Connect to the AP or Ad-hoc Network with specific bssid
+ *
+ * NOTE: Scan should be issued by application before this function is called
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param awrq         A pointer to iw_param structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0 --success, otherwise fail
+ */
+static int
+woal_set_wap(struct net_device *dev, struct iw_request_info *info,
+             struct sockaddr *awrq, char *extra)
+{
+    int ret = 0;
+    const t_u8 bcast[MLAN_MAC_ADDR_LENGTH] = { 255, 255, 255, 255, 255, 255 };
+    const t_u8 zero_mac[MLAN_MAC_ADDR_LENGTH] = { 0, 0, 0, 0, 0, 0 };
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ssid_bssid ssid_bssid;
+    mlan_bss_info bss_info;
+
+    ENTER();
+
+    if (awrq->sa_family != ARPHRD_ETHER) {
+        ret = -EINVAL;
+        goto done;
+    }
+
+    PRINTM(MINFO, "ASSOC: WAP: sa_data: %02x:%02x:%02x:%02x:%02x:%02x\n",
+           (t_u8) awrq->sa_data[0], (t_u8) awrq->sa_data[1],
+           (t_u8) awrq->sa_data[2], (t_u8) awrq->sa_data[3],
+           (t_u8) awrq->sa_data[4], (t_u8) awrq->sa_data[5]);
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info)) {
+        ret = -EFAULT;
+        goto done;
+    }
+#ifdef REASSOCIATION
+    /* Cancel re-association */
+    priv->reassoc_required = MFALSE;
+#endif
+
+    /* zero_mac means disconnect */
+    if (!memcmp(zero_mac, awrq->sa_data, MLAN_MAC_ADDR_LENGTH)) {
+        woal_disconnect(priv, MOAL_IOCTL_WAIT, NULL);
+        goto done;
+    }
+
+    /* Broadcast MAC means search for best network */
+    memset(&ssid_bssid, 0, sizeof(mlan_ssid_bssid));
+
+    if (memcmp(bcast, awrq->sa_data, MLAN_MAC_ADDR_LENGTH)) {
+        /* Check if we are already assoicated to the AP */
+        if (bss_info.media_connected == MTRUE) {
+            if (!memcmp(awrq->sa_data, &bss_info.bssid, ETH_ALEN))
+                goto done;
+            /* disconnect before try to assoicate to the new AP */
+            woal_disconnect(priv, MOAL_IOCTL_WAIT, NULL);
+        }
+        memcpy(&ssid_bssid.bssid, awrq->sa_data, ETH_ALEN);
+    }
+
+    if (MLAN_STATUS_SUCCESS != woal_find_best_network(priv,
+                                                      MOAL_IOCTL_WAIT,
+                                                      &ssid_bssid)) {
+        PRINTM(MERROR, "ASSOC: WAP: MAC address not found in BSSID List\n");
+        ret = -ENETUNREACH;
+        goto done;
+    }
+    /* Zero SSID implies use BSSID to connect */
+    memset(&ssid_bssid.ssid, 0, sizeof(mlan_802_11_ssid));
+    if (MLAN_STATUS_SUCCESS != woal_bss_start(priv,
+                                              MOAL_IOCTL_WAIT, &ssid_bssid)) {
+        ret = -EFAULT;
+        goto done;
+    }
+#ifdef REASSOCIATION
+    memset(&bss_info, 0, sizeof(bss_info));
+    if (MLAN_STATUS_SUCCESS != woal_get_bss_info(priv,
+                                                 MOAL_IOCTL_WAIT, &bss_info)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    memcpy(&priv->prev_ssid_bssid.ssid, &bss_info.ssid,
+           sizeof(mlan_802_11_ssid));
+    memcpy(&priv->prev_ssid_bssid.bssid, &bss_info.bssid, MLAN_MAC_ADDR_LENGTH);
+#endif /* REASSOCIATION */
+
+  done:
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get wlan mode
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param uwrq                 A pointer to t_u32 string
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success
+ */
+static int
+woal_get_bss_mode(struct net_device *dev, struct iw_request_info *info,
+                  t_u32 * uwrq, char *extra)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    ENTER();
+    *uwrq = woal_get_mode(priv, MOAL_IOCTL_WAIT);
+    LEAVE();
+    return 0;
+}
+
+/**
+ *  @brief Set sensitivity
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success
+ */
+static int
+woal_set_sens(struct net_device *dev, struct iw_request_info *info,
+              struct iw_param *vwrq, char *extra)
+{
+    int ret = 0;
+
+    ENTER();
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get sensitivity
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     -1
+ */
+static int
+woal_get_sens(struct net_device *dev, struct iw_request_info *info,
+              struct iw_param *vwrq, char *extra)
+{
+    int ret = -1;
+
+    ENTER();
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set Tx power
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_txpow(struct net_device *dev, struct iw_request_info *info,
+               struct iw_param *vwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_power_cfg_t power_cfg;
+
+    ENTER();
+    if (vwrq->disabled) {
+        woal_set_radio(priv, 0);
+        goto done;
+    }
+    woal_set_radio(priv, 1);
+
+    if (!vwrq->fixed)
+        power_cfg.is_power_auto = 1;
+    else {
+        power_cfg.is_power_auto = 0;
+        power_cfg.power_level = vwrq->value;
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_tx_power(priv, MLAN_ACT_SET, &power_cfg)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get Tx power
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_get_txpow(struct net_device *dev, struct iw_request_info *info,
+               struct iw_param *vwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_power_cfg_t power_cfg;
+    mlan_bss_info bss_info;
+
+    ENTER();
+
+    memset(&power_cfg, 0, sizeof(mlan_power_cfg_t));
+    memset(&bss_info, 0, sizeof(bss_info));
+    woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info);
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_tx_power(priv, MLAN_ACT_GET, &power_cfg)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    vwrq->value = power_cfg.power_level;
+    if (power_cfg.is_power_auto)
+        vwrq->fixed = 0;
+    else
+        vwrq->fixed = 1;
+    if (bss_info.radio_on) {
+        vwrq->disabled = 0;
+        vwrq->flags = IW_TXPOW_DBM;
+    } else {
+        vwrq->disabled = 1;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief  Set power management
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static int
+woal_set_power(struct net_device *dev, struct iw_request_info *info,
+               struct iw_param *vwrq, char *extra)
+{
+    int ret = 0, disabled;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+
+    ENTER();
+
+    disabled = vwrq->disabled;
+
+    if (MLAN_STATUS_SUCCESS != woal_set_get_power_mgmt(priv,
+                                                       MLAN_ACT_SET, &disabled,
+                                                       vwrq->flags)) {
+        ret = -EFAULT;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief  Get power management
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     MLAN_STATUS_SUCCESS --success, otherwise fail
+ */
+static int
+woal_get_power(struct net_device *dev, struct iw_request_info *info,
+               struct iw_param *vwrq, char *extra)
+{
+    int ret = 0, ps_mode;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+
+    ENTER();
+
+    if (MLAN_STATUS_SUCCESS != woal_set_get_power_mgmt(priv,
+                                                       MLAN_ACT_GET, &ps_mode,
+                                                       0)) {
+        ret = -EFAULT;
+    }
+
+    if (ps_mode)
+        vwrq->disabled = 0;
+    else
+        vwrq->disabled = 1;
+
+    vwrq->value = 0;
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set Tx retry count
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_retry(struct net_device *dev, struct iw_request_info *info,
+               struct iw_param *vwrq, char *extra)
+{
+    int ret = 0, retry_val = vwrq->value;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+
+    ENTER();
+
+    if (vwrq->flags == IW_RETRY_LIMIT) {
+        /*
+         * The MAC has a 4-bit Total_Tx_Count register
+         * Total_Tx_Count = 1 + Tx_Retry_Count
+         */
+
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_get_retry(priv, MLAN_ACT_SET,
+                               MOAL_IOCTL_WAIT, &retry_val)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    } else {
+        ret = -EOPNOTSUPP;
+        goto done;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get Tx retry count
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_get_retry(struct net_device *dev, struct iw_request_info *info,
+               struct iw_param *vwrq, char *extra)
+{
+    int retry_val, ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+
+    ENTER();
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_retry(priv, MLAN_ACT_GET, MOAL_IOCTL_WAIT, &retry_val)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    vwrq->disabled = 0;
+    if (!vwrq->flags) {
+        vwrq->flags = IW_RETRY_LIMIT;
+        /* Get Tx retry count */
+        vwrq->value = retry_val;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set encryption key
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param dwrq                 A pointer to iw_point structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_encode(struct net_device *dev, struct iw_request_info *info,
+                struct iw_point *dwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_ioctl_req *req = NULL;
+    int index = 0;
+    t_u32 auth_mode = 0;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_ENCRYPT_KEY;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    req->action = MLAN_ACT_SET;
+
+    /* Check index */
+    index = (dwrq->flags & IW_ENCODE_INDEX) - 1;
+    if (index > 3) {
+        PRINTM(MERROR, "Key index #%d out of range\n", index);
+        ret = -EINVAL;
+        goto done;
+    }
+
+    sec->param.encrypt_key.key_len = 0;
+    if (!(dwrq->flags & IW_ENCODE_NOKEY) && dwrq->length) {
+        if (dwrq->length > MAX_WEP_KEY_SIZE) {
+            PRINTM(MERROR, "Key length (%d) out of range\n", dwrq->length);
+            ret = -EINVAL;
+            goto done;
+        }
+        if (index < 0)
+            sec->param.encrypt_key.key_index = MLAN_KEY_INDEX_DEFAULT;
+        else
+            sec->param.encrypt_key.key_index = index;
+        memcpy(sec->param.encrypt_key.key_material, extra, dwrq->length);
+        /* Set the length */
+        if (dwrq->length > MIN_WEP_KEY_SIZE)
+            sec->param.encrypt_key.key_len = MAX_WEP_KEY_SIZE;
+        else
+            sec->param.encrypt_key.key_len = MIN_WEP_KEY_SIZE;
+    } else {
+        /*
+         * No key provided so it is either enable key,
+         * on or off
+         */
+        if (dwrq->flags & IW_ENCODE_DISABLED) {
+            PRINTM(MINFO, "*** iwconfig mlanX key off ***\n");
+            sec->param.encrypt_key.key_disable = MTRUE;
+        } else {
+            /*
+             * iwconfig mlanX key [n]
+             * iwconfig mlanX key on
+             * iwconfig mlanX key open
+             * iwconfig mlanX key restricted
+             * Do we want to just set the transmit key index ?
+             */
+            if (index < 0) {
+                PRINTM(MINFO, "*** iwconfig mlanX key on ***\n");
+                sec->param.encrypt_key.key_index = MLAN_KEY_INDEX_DEFAULT;
+            } else
+                sec->param.encrypt_key.key_index = index;
+            sec->param.encrypt_key.is_current_wep_key = MTRUE;
+        }
+    }
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    if (dwrq->flags & (IW_ENCODE_RESTRICTED | IW_ENCODE_OPEN)) {
+        switch (dwrq->flags & 0xf000) {
+        case IW_ENCODE_RESTRICTED:
+            /* iwconfig mlanX restricted key [1] */
+            auth_mode = MLAN_AUTH_MODE_SHARED;
+            PRINTM(MINFO, "Auth mode restricted!\n");
+            break;
+        case IW_ENCODE_OPEN:
+            /* iwconfig mlanX key [2] open */
+            auth_mode = MLAN_AUTH_MODE_OPEN;
+            PRINTM(MINFO, "Auth mode open!\n");
+            break;
+        case IW_ENCODE_RESTRICTED | IW_ENCODE_OPEN:
+        default:
+            /* iwconfig mlanX key [2] open restricted */
+            auth_mode = MLAN_AUTH_MODE_AUTO;
+            PRINTM(MINFO, "Auth mode auto!\n");
+            break;
+        }
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_auth_mode(priv, MOAL_IOCTL_WAIT, auth_mode))
+            ret = -EFAULT;
+    }
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get encryption key
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param dwrq                 A pointer to iw_point structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_get_encode(struct net_device *dev, struct iw_request_info *info,
+                struct iw_point *dwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_ds_sec_cfg *sec = NULL;
+    mlan_ioctl_req *req = NULL;
+    t_u32 auth_mode;
+    int index = (dwrq->flags & IW_ENCODE_INDEX);
+
+    ENTER();
+    if (index < 0 || index > 4) {
+        PRINTM(MERROR, "Key index #%d out of range\n", index);
+        ret = -EINVAL;
+        goto done;
+    }
+    if (MLAN_STATUS_SUCCESS !=
+        woal_get_auth_mode(priv, MOAL_IOCTL_WAIT, &auth_mode)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    dwrq->flags = 0;
+    /*
+     * Check encryption mode
+     */
+    switch (auth_mode) {
+    case MLAN_AUTH_MODE_OPEN:
+        dwrq->flags = IW_ENCODE_OPEN;
+        break;
+
+    case MLAN_AUTH_MODE_SHARED:
+    case MLAN_AUTH_MODE_NETWORKEAP:
+        dwrq->flags = IW_ENCODE_RESTRICTED;
+        break;
+
+    case MLAN_AUTH_MODE_AUTO:
+        dwrq->flags = IW_ENCODE_OPEN | IW_ENCODE_RESTRICTED;
+        break;
+
+    default:
+        dwrq->flags = IW_ENCODE_DISABLED | IW_ENCODE_OPEN;
+        break;
+    }
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_ENCRYPT_KEY;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    req->action = MLAN_ACT_GET;
+
+    if (!index)
+        sec->param.encrypt_key.key_index = MLAN_KEY_INDEX_DEFAULT;
+    else
+        sec->param.encrypt_key.key_index = index - 1;
+
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    memset(extra, 0, 16);
+    if (sec->param.encrypt_key.key_len) {
+        memcpy(extra, sec->param.encrypt_key.key_material,
+               sec->param.encrypt_key.key_len);
+        dwrq->length = sec->param.encrypt_key.key_len;
+        dwrq->flags |= (sec->param.encrypt_key.key_index + 1);
+        dwrq->flags &= ~IW_ENCODE_DISABLED;
+    } else if (sec->param.encrypt_key.key_disable)
+        dwrq->flags |= IW_ENCODE_DISABLED;
+    else
+        dwrq->flags &= ~IW_ENCODE_DISABLED;
+
+    dwrq->flags |= IW_ENCODE_NOKEY;
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set data rate
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param vwrq         A pointer to iw_param structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0 --success, otherwise fail
+ */
+static int
+woal_set_rate(struct net_device *dev, struct iw_request_info *info,
+              struct iw_param *vwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_rate_cfg_t rate_cfg;
+
+    ENTER();
+
+    if (vwrq->value == -1) {
+        rate_cfg.is_rate_auto = 1;
+    } else {
+        rate_cfg.is_rate_auto = 0;
+        rate_cfg.rate_type = MLAN_RATE_VALUE;
+        rate_cfg.rate = vwrq->value / 500000;
+    }
+    if (MLAN_STATUS_SUCCESS != woal_set_get_data_rate(priv,
+                                                      MLAN_ACT_SET,
+                                                      &rate_cfg)) {
+        ret = -EFAULT;
+    }
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get data rate
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param vwrq         A pointer to iw_param structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0 --success, otherwise fail
+ */
+static int
+woal_get_rate(struct net_device *dev, struct iw_request_info *info,
+              struct iw_param *vwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_rate_cfg_t rate_cfg;
+
+    ENTER();
+
+    if (MLAN_STATUS_SUCCESS != woal_set_get_data_rate(priv,
+                                                      MLAN_ACT_GET,
+                                                      &rate_cfg)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (rate_cfg.is_rate_auto)
+        vwrq->fixed = 0;
+    else
+        vwrq->fixed = 1;
+    vwrq->value = rate_cfg.rate * 500000;
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set RTS threshold
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_rts(struct net_device *dev, struct iw_request_info *info,
+             struct iw_param *vwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int rthr = vwrq->value;
+
+    ENTER();
+
+    if (vwrq->disabled) {
+        rthr = MLAN_RTS_MAX_VALUE;
+    } else {
+        if (rthr < MLAN_RTS_MIN_VALUE || rthr > MLAN_RTS_MAX_VALUE) {
+            ret = -EINVAL;
+            goto done;
+        }
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_rts(priv, MLAN_ACT_SET, MOAL_IOCTL_WAIT, &rthr)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get RTS threshold
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_get_rts(struct net_device *dev, struct iw_request_info *info,
+             struct iw_param *vwrq, char *extra)
+{
+    int rthr, ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+
+    ENTER();
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_rts(priv, MLAN_ACT_GET, MOAL_IOCTL_WAIT, &rthr)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    vwrq->value = rthr;
+    vwrq->disabled = ((vwrq->value < MLAN_RTS_MIN_VALUE)
+                      || (vwrq->value > MLAN_RTS_MAX_VALUE));
+    vwrq->fixed = 1;
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set Fragment threshold
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_frag(struct net_device *dev, struct iw_request_info *info,
+              struct iw_param *vwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int fthr = vwrq->value;
+
+    ENTER();
+
+    if (vwrq->disabled) {
+        fthr = MLAN_FRAG_MAX_VALUE;
+    } else {
+        if (fthr < MLAN_FRAG_MIN_VALUE || fthr > MLAN_FRAG_MAX_VALUE) {
+            ret = -EINVAL;
+            goto done;
+        }
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_frag(priv, MLAN_ACT_SET, MOAL_IOCTL_WAIT, &fthr)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get Fragment threshold
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_get_frag(struct net_device *dev, struct iw_request_info *info,
+              struct iw_param *vwrq, char *extra)
+{
+    int ret = 0, fthr;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+
+    ENTER();
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_frag(priv, MLAN_ACT_GET, MOAL_IOCTL_WAIT, &fthr)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    vwrq->value = fthr;
+    vwrq->disabled = ((vwrq->value < MLAN_FRAG_MIN_VALUE)
+                      || (vwrq->value > MLAN_FRAG_MAX_VALUE));
+    vwrq->fixed = 1;
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+#if (WIRELESS_EXT >= 18)
+/**
+ *  @brief Get IE
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param dwrq                 A pointer to iw_point structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_get_gen_ie(struct net_device *dev, struct iw_request_info *info,
+                struct iw_point *dwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int copy_size = 0, ie_len;
+    t_u8 ie[MAX_IE_SIZE];
+
+    ENTER();
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_gen_ie(priv, MLAN_ACT_GET, ie, &ie_len)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    copy_size = MIN(ie_len, dwrq->length);
+    memcpy(extra, ie, copy_size);
+    dwrq->length = copy_size;
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set IE
+ *
+ *  Pass an opaque block of data, expected to be IEEE IEs, to the driver
+ *    for eventual passthrough to the firmware in an associate/join
+ *    (and potentially start) command.
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param dwrq                 A pointer to iw_point structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_gen_ie(struct net_device *dev, struct iw_request_info *info,
+                struct iw_point *dwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int ie_len = dwrq->length;
+    const t_u8 wps_oui[] = { 0x00, 0x50, 0xf2, 0x04 };
+    mlan_ds_wps_cfg *pwps = NULL;
+    mlan_ioctl_req *req = NULL;
+
+    ENTER();
+
+    /* extra + 2 to skip element id and length */
+    if (!memcmp((t_u8 *) (extra + 2), wps_oui, sizeof(wps_oui))) {
+        req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_wps_cfg));
+        if (req == NULL) {
+            ret = -ENOMEM;
+            goto done;
+        }
+
+        pwps = (mlan_ds_wps_cfg *) req->pbuf;
+        req->req_id = MLAN_IOCTL_WPS_CFG;
+        req->action = MLAN_ACT_SET;
+        pwps->sub_command = MLAN_OID_WPS_CFG_SESSION;
+        pwps->param.wps_session = MLAN_WPS_CFG_SESSION_START;
+
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+            ret = -EFAULT;
+            goto done;
+        }
+    }
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_set_get_gen_ie(priv, MLAN_ACT_SET, (t_u8 *) extra, &ie_len)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief  Extended version of encoding configuration
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param dwrq         A pointer to iw_point structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return              0 --success, otherwise fail
+ */
+static int
+woal_set_encode_ext(struct net_device *dev,
+                    struct iw_request_info *info,
+                    struct iw_point *dwrq, char *extra)
+{
+    struct iw_encode_ext *ext = (struct iw_encode_ext *) extra;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int key_index;
+    t_u8 *pkey_material = NULL;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_sec_cfg *sec = NULL;
+    int ret = 0;
+
+    ENTER();
+    key_index = (dwrq->flags & IW_ENCODE_INDEX) - 1;
+    if (key_index < 0 || key_index > 3) {
+        ret = -EINVAL;
+        goto done;
+    }
+    if (ext->key_len > (dwrq->length - sizeof(struct iw_encode_ext))) {
+        ret = -EINVAL;
+        goto done;
+    }
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_sec_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    sec = (mlan_ds_sec_cfg *) req->pbuf;
+    sec->sub_command = MLAN_OID_SEC_CFG_ENCRYPT_KEY;
+    req->req_id = MLAN_IOCTL_SEC_CFG;
+    req->action = MLAN_ACT_SET;
+    pkey_material = (t_u8 *) (ext + 1);
+    sec->param.encrypt_key.key_len = ext->key_len;
+    memcpy(sec->param.encrypt_key.mac_addr, (u8 *) ext->addr.sa_data, ETH_ALEN);
+    /* Disable and Remove Key */
+    if ((dwrq->flags & IW_ENCODE_DISABLED) && !ext->key_len) {
+        sec->param.encrypt_key.key_remove = MTRUE;
+        sec->param.encrypt_key.key_index = key_index;
+        sec->param.encrypt_key.key_flags = KEY_FLAG_REMOVE_KEY;
+        PRINTM(MIOCTL,
+               "Remove key key_index=%d, dwrq->flags=0x%x %02x:%02x:%02x:%02x:%02x:%02x\n",
+               key_index, dwrq->flags, sec->param.encrypt_key.mac_addr[0],
+               sec->param.encrypt_key.mac_addr[1],
+               sec->param.encrypt_key.mac_addr[2],
+               sec->param.encrypt_key.mac_addr[3],
+               sec->param.encrypt_key.mac_addr[4],
+               sec->param.encrypt_key.mac_addr[5]);
+    } else if (ext->key_len <= MAX_WEP_KEY_SIZE) {
+        /* Set WEP key */
+        sec->param.encrypt_key.key_index = key_index;
+        sec->param.encrypt_key.key_flags = ext->ext_flags;
+        memcpy(sec->param.encrypt_key.key_material, pkey_material,
+               ext->key_len);
+        if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+            sec->param.encrypt_key.is_current_wep_key = MTRUE;
+    } else {
+        /* Set WPA key */
+        sec->param.encrypt_key.key_index = key_index;
+        sec->param.encrypt_key.key_flags = ext->ext_flags;
+        if (ext->ext_flags & IW_ENCODE_EXT_RX_SEQ_VALID) {
+            memcpy(sec->param.encrypt_key.pn, (t_u8 *) ext->rx_seq,
+                   SEQ_MAX_SIZE);
+            DBG_HEXDUMP(MCMD_D, "Rx PN", sec->param.encrypt_key.pn,
+                        SEQ_MAX_SIZE);
+        }
+        if (ext->ext_flags & IW_ENCODE_EXT_TX_SEQ_VALID) {
+            memcpy(sec->param.encrypt_key.pn, (t_u8 *) ext->tx_seq,
+                   SEQ_MAX_SIZE);
+            DBG_HEXDUMP(MCMD_D, "Tx PN", sec->param.encrypt_key.pn,
+                        SEQ_MAX_SIZE);
+        }
+        memcpy(sec->param.encrypt_key.key_material, pkey_material,
+               ext->key_len);
+        PRINTM(MIOCTL,
+               "set wpa key key_index=%d, key_len=%d key_flags=0x%x %02x:%02x:%02x:%02x:%02x:%02x\n",
+               key_index, ext->key_len, sec->param.encrypt_key.key_flags,
+               sec->param.encrypt_key.mac_addr[0],
+               sec->param.encrypt_key.mac_addr[1],
+               sec->param.encrypt_key.mac_addr[2],
+               sec->param.encrypt_key.mac_addr[3],
+               sec->param.encrypt_key.mac_addr[4],
+               sec->param.encrypt_key.mac_addr[5]);
+        DBG_HEXDUMP(MCMD_D, "wpa key", pkey_material, ext->key_len);
+#define IW_ENCODE_ALG_SMS4   0x20
+        /* Set WAPI key */
+        if (ext->alg == IW_ENCODE_ALG_SMS4) {
+            sec->param.encrypt_key.is_wapi_key = MTRUE;
+            memcpy(sec->param.encrypt_key.pn, (t_u8 *) ext->tx_seq,
+                   SEQ_MAX_SIZE);
+            memcpy(&sec->param.encrypt_key.pn[SEQ_MAX_SIZE],
+                   (t_u8 *) ext->rx_seq, SEQ_MAX_SIZE);
+            DBG_HEXDUMP(MCMD_D, "WAPI PN", sec->param.encrypt_key.pn, PN_SIZE);
+        }
+    }
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT))
+        ret = -EFAULT;
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief  Extended version of encoding configuration
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param dwrq         A pointer to iw_point structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             -EOPNOTSUPP
+ */
+static int
+woal_get_encode_ext(struct net_device *dev,
+                    struct iw_request_info *info,
+                    struct iw_point *dwrq, char *extra)
+{
+    ENTER();
+    LEAVE();
+    return -EOPNOTSUPP;
+}
+
+/**
+ *  @brief  Request MLME operation
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param dwrq         A pointer to iw_point structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0--success, otherwise fail
+ */
+static int
+woal_set_mlme(struct net_device *dev,
+              struct iw_request_info *info, struct iw_point *dwrq, char *extra)
+{
+    struct iw_mlme *mlme = (struct iw_mlme *) extra;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int ret = 0;
+
+    ENTER();
+    if ((mlme->cmd == IW_MLME_DEAUTH) || (mlme->cmd == IW_MLME_DISASSOC)) {
+
+        if (MLAN_STATUS_SUCCESS !=
+            woal_disconnect(priv, MOAL_IOCTL_WAIT, (t_u8 *) mlme->addr.sa_data))
+            ret = -EFAULT;
+    }
+    LEAVE();
+    return ret;
+}
+
+/** @brief Set authentication mode parameters
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_auth(struct net_device *dev, struct iw_request_info *info,
+              struct iw_param *vwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    t_u32 auth_mode = 0;
+    t_u32 encrypt_mode = 0;
+    ENTER();
+
+    switch (vwrq->flags & IW_AUTH_INDEX) {
+    case IW_AUTH_CIPHER_PAIRWISE:
+    case IW_AUTH_CIPHER_GROUP:
+        if (vwrq->value & IW_AUTH_CIPHER_NONE)
+            encrypt_mode = MLAN_ENCRYPTION_MODE_NONE;
+        else if (vwrq->value & IW_AUTH_CIPHER_WEP40)
+            encrypt_mode = MLAN_ENCRYPTION_MODE_WEP40;
+        else if (vwrq->value & IW_AUTH_CIPHER_WEP104)
+            encrypt_mode = MLAN_ENCRYPTION_MODE_WEP104;
+        else if (vwrq->value & IW_AUTH_CIPHER_TKIP)
+            encrypt_mode = MLAN_ENCRYPTION_MODE_TKIP;
+        else if (vwrq->value & IW_AUTH_CIPHER_CCMP)
+            encrypt_mode = MLAN_ENCRYPTION_MODE_CCMP;
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_encrypt_mode(priv, MOAL_IOCTL_WAIT, encrypt_mode))
+            ret = -EFAULT;
+        break;
+    case IW_AUTH_80211_AUTH_ALG:
+        switch (vwrq->value) {
+        case IW_AUTH_ALG_SHARED_KEY:
+            PRINTM(MINFO, "Auth mode shared key!\n");
+            auth_mode = MLAN_AUTH_MODE_SHARED;
+            break;
+        case IW_AUTH_ALG_LEAP:
+            PRINTM(MINFO, "Auth mode LEAP!\n");
+            auth_mode = MLAN_AUTH_MODE_NETWORKEAP;
+            break;
+        case IW_AUTH_ALG_OPEN_SYSTEM:
+            PRINTM(MINFO, "Auth mode open!\n");
+            auth_mode = MLAN_AUTH_MODE_OPEN;
+            break;
+        case IW_AUTH_ALG_SHARED_KEY | IW_AUTH_ALG_OPEN_SYSTEM:
+        default:
+            PRINTM(MINFO, "Auth mode auto!\n");
+            auth_mode = MLAN_AUTH_MODE_AUTO;
+            break;
+        }
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_auth_mode(priv, MOAL_IOCTL_WAIT, auth_mode))
+            ret = -EFAULT;
+        break;
+    case IW_AUTH_WPA_ENABLED:
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_wpa_enable(priv, MOAL_IOCTL_WAIT, vwrq->value))
+            ret = -EFAULT;
+        break;
+#define IW_AUTH_WAPI_ENABLED    0x20
+    case IW_AUTH_WAPI_ENABLED:
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_wapi_enable(priv, MOAL_IOCTL_WAIT, vwrq->value))
+            ret = -EFAULT;
+        break;
+    case IW_AUTH_WPA_VERSION:
+        /* set WPA_VERSION_DISABLED/VERSION_WPA/VERSION_WP2 */
+        priv->wpa_version = vwrq->value;
+        break;
+    case IW_AUTH_KEY_MGMT:
+        /* set KEY_MGMT_802_1X/KEY_MGMT_PSK */
+        priv->key_mgmt = vwrq->value;
+        break;
+    case IW_AUTH_TKIP_COUNTERMEASURES:
+    case IW_AUTH_DROP_UNENCRYPTED:
+    case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+    case IW_AUTH_ROAMING_CONTROL:
+    case IW_AUTH_PRIVACY_INVOKED:
+        break;
+    default:
+        ret = -EOPNOTSUPP;
+        break;
+    }
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get authentication mode parameters
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param vwrq                 A pointer to iw_param structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_get_auth(struct net_device *dev, struct iw_request_info *info,
+              struct iw_param *vwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    t_u32 encrypt_mode = 0;
+    t_u32 auth_mode;
+    t_u32 wpa_enable;
+    ENTER();
+    switch (vwrq->flags & IW_AUTH_INDEX) {
+    case IW_AUTH_CIPHER_PAIRWISE:
+    case IW_AUTH_CIPHER_GROUP:
+        if (MLAN_STATUS_SUCCESS !=
+            woal_get_encrypt_mode(priv, MOAL_IOCTL_WAIT, &encrypt_mode))
+            ret = -EFAULT;
+        else {
+            if (encrypt_mode == MLAN_ENCRYPTION_MODE_NONE)
+                vwrq->value = IW_AUTH_CIPHER_NONE;
+            else if (encrypt_mode == MLAN_ENCRYPTION_MODE_WEP40)
+                vwrq->value = IW_AUTH_CIPHER_WEP40;
+            else if (encrypt_mode == MLAN_ENCRYPTION_MODE_TKIP)
+                vwrq->value = IW_AUTH_CIPHER_TKIP;
+            else if (encrypt_mode == MLAN_ENCRYPTION_MODE_CCMP)
+                vwrq->value = IW_AUTH_CIPHER_CCMP;
+            else if (encrypt_mode == MLAN_ENCRYPTION_MODE_WEP104)
+                vwrq->value = IW_AUTH_CIPHER_WEP104;
+        }
+        break;
+    case IW_AUTH_80211_AUTH_ALG:
+        if (MLAN_STATUS_SUCCESS !=
+            woal_get_auth_mode(priv, MOAL_IOCTL_WAIT, &auth_mode))
+            ret = -EFAULT;
+        else {
+            if (auth_mode == MLAN_AUTH_MODE_SHARED)
+                vwrq->value = IW_AUTH_ALG_SHARED_KEY;
+            else if (auth_mode == MLAN_AUTH_MODE_NETWORKEAP)
+                vwrq->value = IW_AUTH_ALG_LEAP;
+            else
+                vwrq->value = IW_AUTH_ALG_OPEN_SYSTEM;
+        }
+        break;
+    case IW_AUTH_WPA_ENABLED:
+        if (MLAN_STATUS_SUCCESS !=
+            woal_get_wpa_enable(priv, MOAL_IOCTL_WAIT, &wpa_enable))
+            ret = -EFAULT;
+        else
+            vwrq->value = wpa_enable;
+        break;
+    case IW_AUTH_WPA_VERSION:
+        vwrq->value = priv->wpa_version;
+        break;
+    case IW_AUTH_KEY_MGMT:
+        vwrq->value = priv->key_mgmt;
+        break;
+    case IW_AUTH_TKIP_COUNTERMEASURES:
+    case IW_AUTH_DROP_UNENCRYPTED:
+    case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+    case IW_AUTH_ROAMING_CONTROL:
+    case IW_AUTH_PRIVACY_INVOKED:
+    default:
+        ret = -EOPNOTSUPP;
+        goto done;
+    }
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set PMKSA Cache
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param info     A pointer to iw_request_info structure
+ *  @param vwrq     A pointer to iw_param structure
+ *  @param extra    A pointer to extra data buf
+ *
+ *  @return         -EOPNOTSUPP
+ */
+static int
+woal_set_pmksa(struct net_device *dev, struct iw_request_info *info,
+               struct iw_param *vwrq, char *extra)
+{
+    ENTER();
+    LEAVE();
+    return -EOPNOTSUPP;
+}
+
+#endif /* WE >= 18 */
+
+/* Data rate listing
+ *      MULTI_BANDS:
+ *              abg             a       b       b/g
+ *  Infra       G(12)           A(8)    B(4)    G(12)
+ *  Adhoc       A+B(12)         A(8)    B(4)    B(4)
+ *      non-MULTI_BANDS:
+                                        b       b/g
+ *  Infra                               B(4)    G(12)
+ *  Adhoc                               B(4)    B(4)
+ */
+/**
+ *  @brief Get Range Info
+ *
+ *  @param dev                  A pointer to net_device structure
+ *  @param info                 A pointer to iw_request_info structure
+ *  @param dwrq                 A pointer to iw_point structure
+ *  @param extra                A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_get_range(struct net_device *dev, struct iw_request_info *info,
+               struct iw_point *dwrq, char *extra)
+{
+    int i;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    struct iw_range *range = (struct iw_range *) extra;
+    moal_802_11_rates rates;
+    mlan_chan_list *pchan_list = NULL;
+    mlan_bss_info bss_info;
+
+    ENTER();
+
+    if (!(pchan_list = kmalloc(sizeof(mlan_chan_list), GFP_KERNEL))) {
+        LEAVE();
+        return -ENOMEM;
+    }
+
+    dwrq->length = sizeof(struct iw_range);
+    memset(range, 0, sizeof(struct iw_range));
+
+    range->min_nwid = 0;
+    range->max_nwid = 0;
+
+    memset(&rates, 0, sizeof(rates));
+    woal_get_data_rates(priv, MOAL_IOCTL_WAIT, &rates);
+    range->num_bitrates = rates.num_of_rates;
+
+    for (i = 0; i < MIN(range->num_bitrates, IW_MAX_BITRATES) && rates.rates[i];
+         i++) {
+        range->bitrate[i] = (rates.rates[i] & 0x7f) * 500000;
+    }
+    range->num_bitrates = i;
+    PRINTM(MINFO, "IW_MAX_BITRATES=%d num_bitrates=%d\n", IW_MAX_BITRATES,
+           range->num_bitrates);
+
+    range->num_frequency = 0;
+
+    memset(pchan_list, 0, sizeof(mlan_chan_list));
+
+    woal_get_channel_list(priv, MOAL_IOCTL_WAIT, pchan_list);
+
+    range->num_frequency = MIN(pchan_list->num_of_chan, IW_MAX_FREQUENCIES);
+
+    for (i = 0; i < range->num_frequency; i++) {
+        range->freq[i].i = (long) pchan_list->cf[i].channel;
+        range->freq[i].m = (long) pchan_list->cf[i].freq * 100000;
+        range->freq[i].e = 1;
+    }
+    kfree(pchan_list);
+
+    PRINTM(MINFO, "IW_MAX_FREQUENCIES=%d num_frequency=%d\n",
+           IW_MAX_FREQUENCIES, range->num_frequency);
+
+    range->num_channels = range->num_frequency;
+
+    woal_sort_channels(&range->freq[0], range->num_frequency);
+
+    /*
+     * Set an indication of the max TCP throughput in bit/s that we can
+     * expect using this interface
+     */
+    if (i > 2)
+        range->throughput = 5000 * 1000;
+    else
+        range->throughput = 1500 * 1000;
+
+    range->min_rts = MLAN_RTS_MIN_VALUE;
+    range->max_rts = MLAN_RTS_MAX_VALUE;
+    range->min_frag = MLAN_FRAG_MIN_VALUE;
+    range->max_frag = MLAN_FRAG_MAX_VALUE;
+
+    range->encoding_size[0] = 5;
+    range->encoding_size[1] = 13;
+    range->num_encoding_sizes = 2;
+    range->max_encoding_tokens = 4;
+
+/** Minimum power period */
+#define IW_POWER_PERIOD_MIN 1000000     /* 1 sec */
+/** Maximum power period */
+#define IW_POWER_PERIOD_MAX 120000000   /* 2 min */
+/** Minimum power timeout value */
+#define IW_POWER_TIMEOUT_MIN 1000       /* 1 ms */
+/** Maximim power timeout value */
+#define IW_POWER_TIMEOUT_MAX 1000000    /* 1 sec */
+
+    /* Power Management duration & timeout */
+    range->min_pmp = IW_POWER_PERIOD_MIN;
+    range->max_pmp = IW_POWER_PERIOD_MAX;
+    range->min_pmt = IW_POWER_TIMEOUT_MIN;
+    range->max_pmt = IW_POWER_TIMEOUT_MAX;
+    range->pmp_flags = IW_POWER_PERIOD;
+    range->pmt_flags = IW_POWER_TIMEOUT;
+    range->pm_capa = IW_POWER_PERIOD | IW_POWER_TIMEOUT | IW_POWER_ALL_R;
+
+    /*
+     * Minimum version we recommend
+     */
+    range->we_version_source = 15;
+
+    /*
+     * Version we are compiled with
+     */
+    range->we_version_compiled = WIRELESS_EXT;
+
+    range->retry_capa = IW_RETRY_LIMIT;
+    range->retry_flags = IW_RETRY_LIMIT | IW_RETRY_MAX;
+
+    range->min_retry = MLAN_TX_RETRY_MIN;
+    range->max_retry = MLAN_TX_RETRY_MAX;
+
+    /*
+     * Set the qual, level and noise range values
+     */
+    /*
+     * need to put the right values here
+     */
+/** Maximum quality percentage */
+#define IW_MAX_QUAL_PERCENT 5
+/** Average quality percentage */
+#define IW_AVG_QUAL_PERCENT 3
+    range->max_qual.qual = IW_MAX_QUAL_PERCENT;
+    range->max_qual.level = 0;
+    range->max_qual.noise = 0;
+
+    range->avg_qual.qual = IW_AVG_QUAL_PERCENT;
+    range->avg_qual.level = 0;
+    range->avg_qual.noise = 0;
+
+    range->sensitivity = 0;
+
+    /*
+     * Setup the supported power level ranges
+     */
+    memset(range->txpower, 0, sizeof(range->txpower));
+
+    memset(&bss_info, 0, sizeof(bss_info));
+
+    woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info);
+
+    range->txpower[0] = bss_info.min_power_level;
+    range->txpower[1] = bss_info.max_power_level;
+    range->num_txpower = 2;
+    range->txpower_capa = IW_TXPOW_DBM | IW_TXPOW_RANGE;
+
+    LEAVE();
+    return 0;
+}
+
+#ifdef MEF_CFG_RX_FILTER
+/**
+ *  @brief Enable/disable Rx broadcast/multicast filter in non-HS mode
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param enable               MTRUE/MFALSE: enable/disable
+ *
+ *  @return                     0 -- success, otherwise fail
+ */
+static int
+woal_set_rxfilter(moal_private * priv, BOOLEAN enable)
+{
+    int ret = 0;
+    mlan_ioctl_req *req = NULL;
+    mlan_ds_misc_cfg *misc = NULL;
+    mlan_ds_misc_mef_cfg *mef_cfg = NULL;
+
+    ENTER();
+
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_misc_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    misc = (mlan_ds_misc_cfg *) req->pbuf;
+    mef_cfg = &misc->param.mef_cfg;
+    req->req_id = MLAN_IOCTL_MISC_CFG;
+    misc->sub_command = MLAN_OID_MISC_MEF_CFG;
+    req->action = MLAN_ACT_SET;
+
+    mef_cfg->sub_id = (enable ? MEF_CFG_RX_FILTER_ENABLE : MEF_CFG_DISABLE);
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+#endif
+
+/**
+ *  @brief Set priv command
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param dwrq         A pointer to iw_point structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+static int
+woal_set_priv(struct net_device *dev, struct iw_request_info *info,
+              struct iw_point *dwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    char *buf = NULL;
+    int power_mode = 0;
+    int band = 0;
+    char *pband = NULL;
+    mlan_bss_info bss_info;
+    mlan_ds_get_signal signal;
+    mlan_rate_cfg_t rate;
+    char *pdata;
+    t_u8 country_code[COUNTRY_CODE_LEN];
+    int len = 0;
+    ENTER();
+    if (!(buf = kmalloc(dwrq->length + 1, GFP_KERNEL))) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    memset(buf, 0, dwrq->length + 1);
+    if (copy_from_user(buf, dwrq->pointer, dwrq->length)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    PRINTM(MIOCTL, "SIOCSIWPRIV requst = %s\n", buf);
+    if (strncmp(buf, "RSSILOW-THRESHOLD", strlen("RSSILOW-THRESHOLD")) == 0) {
+        pdata = buf + strlen("RSSILOW-THRESHOLD") + 1;
+        if (MLAN_STATUS_SUCCESS != woal_set_rssi_low_threshold(priv, pdata)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "RSSI", strlen("RSSI")) == 0) {
+        if (MLAN_STATUS_SUCCESS != woal_get_bss_info(priv,
+                                                     MOAL_IOCTL_WAIT,
+                                                     &bss_info)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        if (bss_info.media_connected) {
+            if (MLAN_STATUS_SUCCESS !=
+                woal_get_signal_info(priv, MOAL_IOCTL_WAIT, &signal)) {
+                ret = -EFAULT;
+                goto done;
+            }
+            len =
+                sprintf(buf, "%s rssi %d\n", bss_info.ssid.ssid,
+                        signal.bcn_rssi_avg) + 1;
+        } else {
+            len = sprintf(buf, "OK\n") + 1;
+        }
+    } else if (strncmp(buf, "LINKSPEED", strlen("LINKSPEED")) == 0) {
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_get_data_rate(priv, MLAN_ACT_GET, &rate)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        PRINTM(MIOCTL, "tx rate=%d\n", (int) rate.rate);
+        len =
+            sprintf(buf, "LinkSpeed %d\n",
+                    (int) (rate.rate * 500000 / 1000000)) + 1;
+    } else if (strncmp(buf, "MACADDR", strlen("MACADDR")) == 0) {
+        len =
+            sprintf(buf, "Macaddr = %02X:%02X:%02X:%02X:%02X:%02X\n",
+                    priv->current_addr[0], priv->current_addr[1],
+                    priv->current_addr[2], priv->current_addr[3],
+                    priv->current_addr[4], priv->current_addr[5]) + 1;
+    } else if (strncmp(buf, "GETPOWER", strlen("GETPOWER")) == 0) {
+        if (MLAN_STATUS_SUCCESS != woal_get_powermode(priv, &power_mode)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "powermode = %d\n", power_mode) + 1;
+    } else if (strncmp(buf, "SCAN-ACTIVE", strlen("SCAN-ACTIVE")) == 0) {
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_scan_type(priv, MLAN_SCAN_TYPE_ACTIVE)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        priv->scan_type = MLAN_SCAN_TYPE_ACTIVE;
+        PRINTM(MIOCTL, "Set Active Scan\n");
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "SCAN-PASSIVE", strlen("SCAN-PASSIVE")) == 0) {
+        if (MLAN_STATUS_SUCCESS !=
+            woal_set_scan_type(priv, MLAN_SCAN_TYPE_PASSIVE)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        priv->scan_type = MLAN_SCAN_TYPE_PASSIVE;
+        PRINTM(MIOCTL, "Set Passive Scan\n");
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "POWERMODE", strlen("POWERMODE")) == 0) {
+        pdata = buf + strlen("POWERMODE") + 1;
+        if (MLAN_STATUS_SUCCESS != woal_set_powermode(priv, pdata)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "COUNTRY", strlen("COUNTRY")) == 0) {
+        memset(country_code, 0, sizeof(country_code));
+        memcpy(country_code, buf + strlen("COUNTRY") + 1,
+               strlen(buf) - strlen("COUNTRY") - 1);
+        PRINTM(MIOCTL, "Set COUNTRY %s\n", country_code);
+        if (MLAN_STATUS_SUCCESS != woal_set_region_code(priv, country_code)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (memcmp(buf, WEXT_CSCAN_HEADER, WEXT_CSCAN_HEADER_SIZE) == 0) {
+        PRINTM(MIOCTL, "Set Combo Scan\n");
+        if (MLAN_STATUS_SUCCESS != woal_set_combo_scan(priv, buf, dwrq->length)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "GETBAND", strlen("GETBAND")) == 0) {
+        if (MLAN_STATUS_SUCCESS != woal_get_band(priv, &band)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "Band %d\n", band) + 1;
+    } else if (strncmp(buf, "SETBAND", strlen("SETBAND")) == 0) {
+        pband = buf + strlen("SETBAND") + 1;
+        if (MLAN_STATUS_SUCCESS != woal_set_band(priv, pband)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "START", strlen("START")) == 0) {
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "STOP", strlen("STOP")) == 0) {
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "SETSUSPENDOPT", strlen("SETSUSPENDOPT")) == 0) {
+        /* it will be done by GUI */
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "BTCOEXMODE", strlen("BTCOEXMODE")) == 0) {
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "BTCOEXSCAN-START", strlen("BTCOEXSCAN-START")) ==
+               0) {
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "BTCOEXSCAN-STOP", strlen("BTCOEXSCAN-STOP")) == 0) {
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "BTCOEXSCAN-START", strlen("BTCOEXSCAN-START")) ==
+               0) {
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "BTCOEXSCAN-STOP", strlen("BTCOEXSCAN-STOP")) == 0) {
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "BGSCAN-START", strlen("BGSCAN-START")) == 0) {
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "BGSCAN-CONFIG", strlen("BGSCAN-CONFIG")) == 0) {
+        if (MLAN_STATUS_SUCCESS != woal_set_bg_scan(priv, buf, dwrq->length)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        priv->bg_scan_start = MTRUE;
+        priv->bg_scan_reported = MFALSE;
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "BGSCAN-STOP", strlen("BGSCAN-STOP")) == 0) {
+        if (priv->bg_scan_start && !priv->scan_cfg.rssi_threshold) {
+            if (MLAN_STATUS_SUCCESS != woal_stop_bg_scan(priv)) {
+                ret = -EFAULT;
+                goto done;
+            }
+            priv->bg_scan_start = MFALSE;
+            priv->bg_scan_reported = MFALSE;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "RXFILTER-START", strlen("RXFILTER-START")) == 0) {
+#ifdef MEF_CFG_RX_FILTER
+        if ((ret = woal_set_rxfilter(priv, MTRUE))) {
+            goto done;
+        }
+#endif
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "RXFILTER-STOP", strlen("RXFILTER-STOP")) == 0) {
+#ifdef MEF_CFG_RX_FILTER
+        if ((ret = woal_set_rxfilter(priv, MFALSE))) {
+            goto done;
+        }
+#endif
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "RXFILTER-ADD", strlen("RXFILTER-ADD")) == 0) {
+        pdata = buf + strlen("RXFILTER-ADD") + 1;
+        if (MLAN_STATUS_SUCCESS != woal_add_rxfilter(priv, pdata)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "RXFILTER-REMOVE", strlen("RXFILTER-REMOVE")) == 0) {
+        pdata = buf + strlen("RXFILTER-REMOVE") + 1;
+        if (MLAN_STATUS_SUCCESS != woal_remove_rxfilter(priv, pdata)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "QOSINFO", strlen("QOSINFO")) == 0) {
+        pdata = buf + strlen("QOSINFO") + 1;
+        if (MLAN_STATUS_SUCCESS != woal_set_qos_cfg(priv, pdata)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else if (strncmp(buf, "SLEEPPD", strlen("SLEEPPD")) == 0) {
+        pdata = buf + strlen("SLEEPPD") + 1;
+        if (MLAN_STATUS_SUCCESS != woal_set_sleeppd(priv, pdata)) {
+            ret = -EFAULT;
+            goto done;
+        }
+        len = sprintf(buf, "OK\n") + 1;
+    } else {
+        PRINTM(MIOCTL, "Unknow PRIVATE command: %s, ignored\n", buf);
+        ret = -EFAULT;
+        goto done;
+    }
+    PRINTM(MIOCTL, "PRIV Command return: %s, length=%d\n", buf, len);
+    dwrq->length = (t_u16) len;
+    if (copy_to_user(dwrq->pointer, buf, dwrq->length)) {
+        ret = -EFAULT;
+    }
+  done:
+    if (buf)
+        kfree(buf);
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Scan Network
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param vwrq         A pointer to iw_param structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0--success, otherwise fail
+ */
+static int
+woal_set_scan(struct net_device *dev, struct iw_request_info *info,
+              struct iw_param *vwrq, char *extra)
+{
+    int ret = 0;
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    moal_handle *handle = priv->phandle;
+#if WIRELESS_EXT >= 18
+    struct iw_scan_req *req;
+    struct iw_point *dwrq = (struct iw_point *) vwrq;
+#endif
+    mlan_802_11_ssid req_ssid;
+
+    ENTER();
+    if (handle->scan_pending_on_block == MTRUE) {
+        PRINTM(MINFO, "scan already in processing...\n");
+        LEAVE();
+        return ret;
+    }
+#ifdef REASSOCIATION
+    if (MOAL_ACQ_SEMAPHORE_BLOCK(&handle->reassoc_sem)) {
+        PRINTM(MERROR, "Acquire semaphore error, woal_set_scan\n");
+        LEAVE();
+        return -EBUSY;
+    }
+#endif /* REASSOCIATION */
+    priv->report_scan_result = MTRUE;
+
+    memset(&req_ssid, 0x00, sizeof(mlan_802_11_ssid));
+
+#if WIRELESS_EXT >= 18
+    if ((dwrq->flags & IW_SCAN_THIS_ESSID) &&
+        (dwrq->length == sizeof(struct iw_scan_req))) {
+        req = (struct iw_scan_req *) extra;
+
+        if (req->essid_len <= MLAN_MAX_SSID_LENGTH) {
+
+            req_ssid.ssid_len = req->essid_len;
+            memcpy(req_ssid.ssid, (t_u8 *) req->essid, req->essid_len);
+            if (MLAN_STATUS_SUCCESS !=
+                woal_request_scan(priv, MOAL_NO_WAIT, &req_ssid)) {
+                ret = -EFAULT;
+                goto done;
+            }
+        }
+    } else {
+#endif
+        if (MLAN_STATUS_SUCCESS != woal_request_scan(priv, MOAL_NO_WAIT, NULL)) {
+            ret = -EFAULT;
+            goto done;
+        }
+#if WIRELESS_EXT >= 18
+    }
+#endif
+
+    if (priv->phandle->surprise_removed) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+  done:
+#ifdef REASSOCIATION
+    MOAL_REL_SEMAPHORE(&handle->reassoc_sem);
+#endif
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Set essid
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param dwrq         A pointer to iw_point structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0--success, otherwise fail
+ */
+static int
+woal_set_essid(struct net_device *dev, struct iw_request_info *info,
+               struct iw_point *dwrq, char *extra)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_802_11_ssid req_ssid;
+    mlan_ssid_bssid ssid_bssid;
+#ifdef REASSOCIATION
+    moal_handle *handle = priv->phandle;
+    mlan_bss_info bss_info;
+#endif
+    int ret = 0;
+    t_u32 mode = 0;
+
+    ENTER();
+
+#ifdef REASSOCIATION
+    /* Cancel re-association */
+    priv->reassoc_required = MFALSE;
+
+    if (MOAL_ACQ_SEMAPHORE_BLOCK(&handle->reassoc_sem)) {
+        PRINTM(MERROR, "Acquire semaphore error, woal_set_essid\n");
+        LEAVE();
+        return -EBUSY;
+    }
+#endif /* REASSOCIATION */
+
+    /* Check the size of the string */
+    if (dwrq->length > IW_ESSID_MAX_SIZE + 1) {
+        ret = -E2BIG;
+        goto setessid_ret;
+    }
+    if (priv->scan_type == MLAN_SCAN_TYPE_PASSIVE)
+        woal_set_scan_type(priv, MLAN_SCAN_TYPE_ACTIVE);
+    memset(&req_ssid, 0, sizeof(mlan_802_11_ssid));
+    memset(&ssid_bssid, 0, sizeof(mlan_ssid_bssid));
+
+#if WIRELESS_EXT > 20
+    req_ssid.ssid_len = dwrq->length;
+#else
+    req_ssid.ssid_len = dwrq->length - 1;
+#endif
+
+    /*
+     * Check if we asked for `any' or 'particular'
+     */
+    if (!dwrq->flags) {
+#ifdef REASSOCIATION
+        if (!req_ssid.ssid_len) {
+            memset(&priv->prev_ssid_bssid.ssid, 0x00, sizeof(mlan_802_11_ssid));
+            memset(&priv->prev_ssid_bssid.bssid, 0x00, MLAN_MAC_ADDR_LENGTH);
+            goto setessid_ret;
+        }
+#endif
+        /* Do normal SSID scanning */
+        if (MLAN_STATUS_SUCCESS !=
+            woal_request_scan(priv, MOAL_IOCTL_WAIT, NULL)) {
+            ret = -EFAULT;
+            goto setessid_ret;
+        }
+    } else {
+        /* Set the SSID */
+        memcpy(req_ssid.ssid, extra,
+               MIN(req_ssid.ssid_len, MLAN_MAX_SSID_LENGTH));
+        if (!req_ssid.ssid_len || (MFALSE == woal_ssid_valid(&req_ssid))) {
+            PRINTM(MERROR, "Invalid SSID - aborting set_essid\n");
+            ret = -EINVAL;
+            goto setessid_ret;
+        }
+
+        PRINTM(MINFO, "Requested new SSID = %s\n", (char *) req_ssid.ssid);
+        memcpy(&ssid_bssid.ssid, &req_ssid, sizeof(mlan_802_11_ssid));
+
+        if (dwrq->flags != 0xFFFF) {
+            if (MLAN_STATUS_SUCCESS != woal_find_essid(priv, &ssid_bssid)) {
+                /* Do specific SSID scanning */
+                if (MLAN_STATUS_SUCCESS !=
+                    woal_request_scan(priv, MOAL_IOCTL_WAIT, &req_ssid)) {
+                    ret = -EFAULT;
+                    goto setessid_ret;
+                }
+            }
+        }
+
+    }
+
+    /* disconnect before try to associate */
+    woal_disconnect(priv, MOAL_IOCTL_WAIT, NULL);
+    mode = woal_get_mode(priv, MOAL_IOCTL_WAIT);
+
+    if (mode != IW_MODE_ADHOC) {
+        if (MLAN_STATUS_SUCCESS !=
+            woal_find_best_network(priv, MOAL_IOCTL_WAIT, &ssid_bssid)) {
+            ret = -EFAULT;
+            goto setessid_ret;
+        }
+    } else if (MLAN_STATUS_SUCCESS !=
+               woal_find_best_network(priv, MOAL_IOCTL_WAIT, &ssid_bssid))
+        /* Adhoc start, Check the channel command */
+        woal_11h_channel_check_ioctl(priv);
+
+    /* Connect to BSS by ESSID */
+    memset(&ssid_bssid.bssid, 0, MLAN_MAC_ADDR_LENGTH);
+
+    if (MLAN_STATUS_SUCCESS != woal_bss_start(priv,
+                                              MOAL_IOCTL_WAIT, &ssid_bssid)) {
+        ret = -EFAULT;
+        goto setessid_ret;
+    }
+#ifdef REASSOCIATION
+    memset(&bss_info, 0, sizeof(bss_info));
+    if (MLAN_STATUS_SUCCESS != woal_get_bss_info(priv,
+                                                 MOAL_IOCTL_WAIT, &bss_info)) {
+        ret = -EFAULT;
+        goto setessid_ret;
+    }
+    memcpy(&priv->prev_ssid_bssid.ssid, &bss_info.ssid,
+           sizeof(mlan_802_11_ssid));
+    memcpy(&priv->prev_ssid_bssid.bssid, &bss_info.bssid, MLAN_MAC_ADDR_LENGTH);
+#endif /* REASSOCIATION */
+
+  setessid_ret:
+    if (priv->scan_type == MLAN_SCAN_TYPE_PASSIVE)
+        woal_set_scan_type(priv, MLAN_SCAN_TYPE_PASSIVE);
+#ifdef REASSOCIATION
+    MOAL_REL_SEMAPHORE(&handle->reassoc_sem);
+#endif
+
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief Get current essid
+ *
+ *  @param dev      A pointer to net_device structure
+ *  @param info     A pointer to iw_request_info structure
+ *  @param dwrq     A pointer to iw_point structure
+ *  @param extra    A pointer to extra data buf
+ *
+ *  @return         0--success, otherwise fail
+ */
+static int
+woal_get_essid(struct net_device *dev, struct iw_request_info *info,
+               struct iw_point *dwrq, char *extra)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    mlan_bss_info bss_info;
+    int ret = 0;
+
+    ENTER();
+
+    memset(&bss_info, 0, sizeof(bss_info));
+
+    if (MLAN_STATUS_SUCCESS !=
+        woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info)) {
+        ret = -EFAULT;
+        goto done;
+    }
+
+    if (bss_info.media_connected) {
+        dwrq->length = MIN(dwrq->length, bss_info.ssid.ssid_len);
+        memcpy(extra, bss_info.ssid.ssid, dwrq->length);
+    } else
+        dwrq->length = 0;
+
+    if (bss_info.scan_table_idx)
+        dwrq->flags = (bss_info.scan_table_idx + 1) & IW_ENCODE_INDEX;
+    else
+        dwrq->flags = 1;
+
+  done:
+    LEAVE();
+    return ret;
+}
+
+/**
+ *  @brief  Retrieve the scan table entries via wireless tools IOCTL call
+ *
+ *  @param dev          A pointer to net_device structure
+ *  @param info         A pointer to iw_request_info structure
+ *  @param dwrq         A pointer to iw_point structure
+ *  @param extra        A pointer to extra data buf
+ *
+ *  @return             0--success, otherwise fail
+ */
+static int
+woal_get_scan(struct net_device *dev, struct iw_request_info *info,
+              struct iw_point *dwrq, char *extra)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    int ret = 0;
+    char *current_ev = extra;
+    char *end_buf = extra + IW_SCAN_MAX_DATA;
+    char *current_val;          /* For rates */
+    struct iw_event iwe;        /* Temporary buffer */
+    unsigned int i;
+    unsigned int j;
+    mlan_scan_resp scan_resp;
+    mlan_bss_info bss_info;
+    BSSDescriptor_t *scan_table;
+    mlan_ds_get_signal rssi;
+    t_u16 buf_size = 16 + 256 * 2;
+    char *buf = NULL;
+    char *ptr;
+#if WIRELESS_EXT >= 18
+    t_u8 *praw_data;
+#endif
+    int beacon_size;
+    t_u8 *pbeacon;
+    IEEEtypes_ElementId_e element_id;
+    t_u8 element_len;
+
+    ENTER();
+
+    if (priv->phandle->scan_pending_on_block == MTRUE) {
+        LEAVE();
+        return -EAGAIN;
+    }
+
+    if (!(buf = kmalloc((buf_size), GFP_KERNEL))) {
+        PRINTM(MERROR, "Cannot allocate buffer!\n");
+        ret = -EFAULT;
+        goto done;
+    }
+
+    memset(&bss_info, 0, sizeof(bss_info));
+    if (MLAN_STATUS_SUCCESS !=
+        woal_get_bss_info(priv, MOAL_IOCTL_WAIT, &bss_info)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    memset(&scan_resp, 0, sizeof(scan_resp));
+    if (MLAN_STATUS_SUCCESS != woal_get_scan_table(priv,
+                                                   MOAL_IOCTL_WAIT,
+                                                   &scan_resp)) {
+        ret = -EFAULT;
+        goto done;
+    }
+    scan_table = (BSSDescriptor_t *) scan_resp.pscan_table;
+    if (dwrq->length)
+        end_buf = extra + dwrq->length;
+    if (priv->media_connected == MTRUE) {
+        PRINTM(MINFO, "Current Ssid: %-32s\n", bss_info.ssid.ssid);
+    }
+    PRINTM(MINFO, "Scan: Get: NumInScanTable = %d\n",
+           (int) scan_resp.num_in_scan_table);
+
+#if WIRELESS_EXT > 13
+    /* The old API using SIOCGIWAPLIST had a hard limit of IW_MAX_AP. The new
+       API using SIOCGIWSCAN is only limited by buffer size WE-14 -> WE-16 the
+       buffer is limited to IW_SCAN_MAX_DATA bytes which is 4096. */
+    for (i = 0; i < scan_resp.num_in_scan_table; i++) {
+        if ((current_ev + MAX_SCAN_CELL_SIZE) >= end_buf) {
+            PRINTM(MINFO, "i=%d break out: current_ev=%p end_buf=%p "
+                   "MAX_SCAN_CELL_SIZE=%d\n",
+                   i, current_ev, end_buf, (t_u32) MAX_SCAN_CELL_SIZE);
+            ret = -E2BIG;
+            break;
+        }
+        if (!scan_table[i].freq) {
+            PRINTM(MERROR, "Invalid channel number %d\n",
+                   (int) scan_table[i].channel);
+            continue;
+        }
+        PRINTM(MINFO, "i=%d  Ssid: %-32s\n", i, scan_table[i].ssid.ssid);
+
+        /* check ssid is valid or not, ex. hidden ssid will be filter out */
+        if (woal_ssid_valid(&scan_table[i].ssid) == MFALSE) {
+            continue;
+        }
+
+        /* First entry *MUST* be the AP MAC address */
+        iwe.cmd = SIOCGIWAP;
+        iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+        memcpy(iwe.u.ap_addr.sa_data, &scan_table[i].mac_address, ETH_ALEN);
+
+        iwe.len = IW_EV_ADDR_LEN;
+        current_ev =
+            IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe, iwe.len);
+
+        /* Add the ESSID */
+        iwe.u.data.length = scan_table[i].ssid.ssid_len;
+
+        if (iwe.u.data.length > 32) {
+            iwe.u.data.length = 32;
+        }
+
+        iwe.cmd = SIOCGIWESSID;
+        iwe.u.essid.flags = (i + 1) & IW_ENCODE_INDEX;
+        iwe.len = IW_EV_POINT_LEN + iwe.u.data.length;
+        current_ev =
+            IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,
+                                 (t_s8 *) scan_table[i].ssid.ssid);
+
+        /* Add mode */
+        iwe.cmd = SIOCGIWMODE;
+        if (scan_table[i].bss_mode == MLAN_BSS_MODE_IBSS)
+            iwe.u.mode = IW_MODE_ADHOC;
+        else if (scan_table[i].bss_mode == MLAN_BSS_MODE_INFRA)
+            iwe.u.mode = IW_MODE_MASTER;
+        else
+            iwe.u.mode = IW_MODE_AUTO;
+
+        iwe.len = IW_EV_UINT_LEN;
+        current_ev =
+            IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe, iwe.len);
+
+        /* Frequency */
+        iwe.cmd = SIOCGIWFREQ;
+        iwe.u.freq.m = (long) scan_table[i].freq * 100000;
+        iwe.u.freq.e = 1;
+        iwe.u.freq.flags = IW_FREQ_FIXED;
+        iwe.len = IW_EV_FREQ_LEN;
+        current_ev =
+            IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe, iwe.len);
+
+        memset(&iwe, 0, sizeof(iwe));
+        /* Add quality statistics */
+        iwe.cmd = IWEVQUAL;
+        iwe.u.qual.level = SCAN_RSSI(scan_table[i].rssi);
+        if (!bss_info.bcn_nf_last) {
+            iwe.u.qual.noise = MRVDRV_NF_DEFAULT_SCAN_VALUE;
+        } else {
+            iwe.u.qual.noise = bss_info.bcn_nf_last;
+        }
+        if ((bss_info.bss_mode == MLAN_BSS_MODE_IBSS) &&
+            !woal_ssid_cmp(&bss_info.ssid, &scan_table[i].ssid)
+            && bss_info.adhoc_state == ADHOC_STARTED) {
+            memset(&rssi, 0, sizeof(mlan_ds_get_signal));
+            if (MLAN_STATUS_SUCCESS !=
+                woal_get_signal_info(priv, MOAL_IOCTL_WAIT, &rssi)) {
+                ret = -EFAULT;
+                break;
+            }
+            iwe.u.qual.level = rssi.data_rssi_avg;
+        }
+        iwe.u.qual.qual =
+            woal_rssi_to_quality((t_s16) (iwe.u.qual.level - 0x100));
+        iwe.len = IW_EV_QUAL_LEN;
+        current_ev =
+            IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe, iwe.len);
+
+        /* Add encryption capability */
+        iwe.cmd = SIOCGIWENCODE;
+        if (scan_table[i].privacy) {
+            iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+        } else {
+            iwe.u.data.flags = IW_ENCODE_DISABLED;
+        }
+        iwe.u.data.length = 0;
+        iwe.len = IW_EV_POINT_LEN + iwe.u.data.length;
+        current_ev =
+            IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, NULL);
+
+        current_val = current_ev + IW_EV_LCP_LEN;
+
+        iwe.cmd = SIOCGIWRATE;
+
+        iwe.u.bitrate.fixed = 0;
+        iwe.u.bitrate.disabled = 0;
+        iwe.u.bitrate.value = 0;
+
+        /* Bit rate given in 500 kb/s units (+ 0x80) */
+        for (j = 0; j < sizeof(scan_table[i].supported_rates); j++) {
+            if (!scan_table[i].supported_rates[j]) {
+                break;
+            }
+
+            iwe.u.bitrate.value =
+                (scan_table[i].supported_rates[j] & 0x7f) * 500000;
+            iwe.len = IW_EV_PARAM_LEN;
+            current_val =
+                IWE_STREAM_ADD_VALUE(info, current_ev, current_val, end_buf,
+                                     &iwe, iwe.len);
+
+        }
+        if ((bss_info.bss_mode == MLAN_BSS_MODE_IBSS) &&
+            !woal_ssid_cmp(&bss_info.ssid, &scan_table[i].ssid)
+            && bss_info.adhoc_state == ADHOC_STARTED) {
+            iwe.u.bitrate.value = 22 * 500000;
+            iwe.len = IW_EV_PARAM_LEN;
+            current_val =
+                IWE_STREAM_ADD_VALUE(info, current_ev, current_val, end_buf,
+                                     &iwe, iwe.len);
+        }
+
+        /* Check if an event is added */
+        if ((unsigned int) (current_val - current_ev) >= IW_EV_PARAM_LEN)
+            current_ev = current_val;
+
+        /* Beacon Interval */
+        memset(&iwe, 0, sizeof(iwe));
+        memset(buf, 0, buf_size);
+        ptr = buf;
+        ptr += sprintf(ptr, "Beacon interval=%d", scan_table[i].beacon_period);
+
+        iwe.u.data.length = strlen(buf);
+        iwe.cmd = IWEVCUSTOM;
+        iwe.len = IW_EV_POINT_LEN + iwe.u.data.length;
+        current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, buf);
+        current_val = current_ev + IW_EV_LCP_LEN + strlen(buf);
+
+        /* Parse and send the IEs */
+        pbeacon = scan_table[i].pbeacon_buf;
+        beacon_size = scan_table[i].beacon_buf_size;
+
+        /* Skip time stamp, beacon interval and capability */
+        if (pbeacon) {
+            pbeacon += sizeof(scan_table[i].beacon_period) +
+                sizeof(scan_table[i].time_stamp) +
+                sizeof(scan_table[i].cap_info);
+            beacon_size -= sizeof(scan_table[i].beacon_period) +
+                sizeof(scan_table[i].time_stamp) +
+                sizeof(scan_table[i].cap_info);
+
+            while ((unsigned int) beacon_size >= sizeof(IEEEtypes_Header_t)) {
+                element_id = (IEEEtypes_ElementId_e) (*(t_u8 *) pbeacon);
+                element_len = *((t_u8 *) pbeacon + 1);
+                if ((unsigned int) beacon_size <
+                    (unsigned int) element_len + sizeof(IEEEtypes_Header_t)) {
+                    PRINTM(MERROR,
+                           "Get scan: Error in processing IE, "
+                           "bytes left < IE length\n");
+                    break;
+                }
+
+                switch (element_id) {
+#if WIRELESS_EXT >= 18
+                case VENDOR_SPECIFIC_221:
+                case RSN_IE:
+                case WAPI_IE:
+                    praw_data = (t_u8 *) pbeacon;
+                    memset(&iwe, 0, sizeof(iwe));
+                    memset(buf, 0, buf_size);
+                    ptr = buf;
+                    memcpy(buf, praw_data,
+                           element_len + sizeof(IEEEtypes_Header_t));
+                    iwe.cmd = IWEVGENIE;
+                    iwe.u.data.length =
+                        element_len + sizeof(IEEEtypes_Header_t);
+                    iwe.len = IW_EV_POINT_LEN + iwe.u.data.length;
+                    current_ev =
+                        IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,
+                                             buf);
+                    current_val = current_ev + IW_EV_LCP_LEN + strlen(buf);
+                    break;
+#endif
+                default:
+                    break;
+                }
+                pbeacon += element_len + sizeof(IEEEtypes_Header_t);
+                beacon_size -= element_len + sizeof(IEEEtypes_Header_t);
+            }
+        }
+#if WIRELESS_EXT > 14
+        memset(&iwe, 0, sizeof(iwe));
+        memset(buf, 0, buf_size);
+        ptr = buf;
+        ptr += sprintf(ptr, "band=");
+        memset(&iwe, 0, sizeof(iwe));
+        if (scan_table[i].bss_band == BAND_A)
+            ptr += sprintf(ptr, "a");
+        else
+            ptr += sprintf(ptr, "bg");
+        iwe.u.data.length = strlen(buf);
+        PRINTM(MINFO, "iwe.u.data.length %d\n", iwe.u.data.length);
+        PRINTM(MINFO, "BUF: %s \n", buf);
+        iwe.cmd = IWEVCUSTOM;
+        iwe.len = IW_EV_POINT_LEN + iwe.u.data.length;
+        current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, buf);
+        current_val = current_ev + IW_EV_LCP_LEN + strlen(buf);
+#endif
+        current_val = current_ev + IW_EV_LCP_LEN;
+
+        /*
+         * Check if we added any event
+         */
+        if ((unsigned int) (current_val - current_ev) > IW_EV_LCP_LEN)
+            current_ev = current_val;
+    }
+
+    dwrq->length = (current_ev - extra);
+    dwrq->flags = 0;
+#endif
+
+  done:
+    if (buf)
+        kfree(buf);
+    LEAVE();
+    return ret;
+}
+
+/**
+ * iwconfig settable callbacks
+ */
+static const iw_handler woal_handler[] = {
+    (iw_handler) woal_config_commit,    /* SIOCSIWCOMMIT */
+    (iw_handler) woal_get_name, /* SIOCGIWNAME */
+    (iw_handler) NULL,          /* SIOCSIWNWID */
+    (iw_handler) NULL,          /* SIOCGIWNWID */
+    (iw_handler) woal_set_freq, /* SIOCSIWFREQ */
+    (iw_handler) woal_get_freq, /* SIOCGIWFREQ */
+    (iw_handler) woal_set_bss_mode,     /* SIOCSIWMODE */
+    (iw_handler) woal_get_bss_mode,     /* SIOCGIWMODE */
+    (iw_handler) woal_set_sens, /* SIOCSIWSENS */
+    (iw_handler) woal_get_sens, /* SIOCGIWSENS */
+    (iw_handler) NULL,          /* SIOCSIWRANGE */
+    (iw_handler) woal_get_range,        /* SIOCGIWRANGE */
+    (iw_handler) woal_set_priv, /* SIOCSIWPRIV */
+    (iw_handler) NULL,          /* SIOCGIWPRIV */
+    (iw_handler) NULL,          /* SIOCSIWSTATS */
+    (iw_handler) NULL,          /* SIOCGIWSTATS */
+#if WIRELESS_EXT > 15
+    iw_handler_set_spy,         /* SIOCSIWSPY */
+    iw_handler_get_spy,         /* SIOCGIWSPY */
+    iw_handler_set_thrspy,      /* SIOCSIWTHRSPY */
+    iw_handler_get_thrspy,      /* SIOCGIWTHRSPY */
+#else /* WIRELESS_EXT > 15 */
+#ifdef WIRELESS_SPY
+    (iw_handler) NULL,          /* SIOCSIWSPY */
+    (iw_handler) NULL,          /* SIOCGIWSPY */
+#else /* WIRELESS_SPY */
+    (iw_handler) NULL,          /* SIOCSIWSPY */
+    (iw_handler) NULL,          /* SIOCGIWSPY */
+#endif /* WIRELESS_SPY */
+    (iw_handler) NULL,          /* -- hole -- */
+    (iw_handler) NULL,          /* -- hole -- */
+#endif /* WIRELESS_EXT > 15 */
+    (iw_handler) woal_set_wap,  /* SIOCSIWAP */
+    (iw_handler) woal_get_wap,  /* SIOCGIWAP */
+#if WIRELESS_EXT >= 18
+    (iw_handler) woal_set_mlme, /* SIOCSIWMLME */
+#else
+    (iw_handler) NULL,          /* -- hole -- */
+#endif
+    /* (iw_handler) wlan_get_aplist, *//* SIOCGIWAPLIST */
+    NULL,                       /* SIOCGIWAPLIST */
+#if WIRELESS_EXT > 13
+    (iw_handler) woal_set_scan, /* SIOCSIWSCAN */
+    (iw_handler) woal_get_scan, /* SIOCGIWSCAN */
+#else /* WIRELESS_EXT > 13 */
+    (iw_handler) NULL,          /* SIOCSIWSCAN */
+    (iw_handler) NULL,          /* SIOCGIWSCAN */
+#endif /* WIRELESS_EXT > 13 */
+    (iw_handler) woal_set_essid,        /* SIOCSIWESSID */
+    (iw_handler) woal_get_essid,        /* SIOCGIWESSID */
+    (iw_handler) woal_set_nick, /* SIOCSIWNICKN */
+    (iw_handler) woal_get_nick, /* SIOCGIWNICKN */
+    (iw_handler) NULL,          /* -- hole -- */
+    (iw_handler) NULL,          /* -- hole -- */
+    (iw_handler) woal_set_rate, /* SIOCSIWRATE */
+    (iw_handler) woal_get_rate, /* SIOCGIWRATE */
+    (iw_handler) woal_set_rts,  /* SIOCSIWRTS */
+    (iw_handler) woal_get_rts,  /* SIOCGIWRTS */
+    (iw_handler) woal_set_frag, /* SIOCSIWFRAG */
+    (iw_handler) woal_get_frag, /* SIOCGIWFRAG */
+    (iw_handler) woal_set_txpow,        /* SIOCSIWTXPOW */
+    (iw_handler) woal_get_txpow,        /* SIOCGIWTXPOW */
+    (iw_handler) woal_set_retry,        /* SIOCSIWRETRY */
+    (iw_handler) woal_get_retry,        /* SIOCGIWRETRY */
+    (iw_handler) woal_set_encode,       /* SIOCSIWENCODE */
+    (iw_handler) woal_get_encode,       /* SIOCGIWENCODE */
+    (iw_handler) woal_set_power,        /* SIOCSIWPOWER */
+    (iw_handler) woal_get_power,        /* SIOCGIWPOWER */
+#if (WIRELESS_EXT >= 18)
+    (iw_handler) NULL,          /* -- hole -- */
+    (iw_handler) NULL,          /* -- hole -- */
+    (iw_handler) woal_set_gen_ie,       /* SIOCSIWGENIE */
+    (iw_handler) woal_get_gen_ie,       /* SIOCGIWGENIE */
+    (iw_handler) woal_set_auth, /* SIOCSIWAUTH */
+    (iw_handler) woal_get_auth, /* SIOCGIWAUTH */
+    (iw_handler) woal_set_encode_ext,   /* SIOCSIWENCODEEXT */
+    (iw_handler) woal_get_encode_ext,   /* SIOCGIWENCODEEXT */
+    (iw_handler) woal_set_pmksa,        /* SIOCSIWPMKSA */
+#endif /* WIRELESSS_EXT >= 18 */
+};
+
+/**
+ * iwpriv settable callbacks
+ */
+static const iw_handler woal_private_handler[] = {
+    NULL,                       /* SIOCIWFIRSTPRIV */
+};
+#endif /* STA_SUPPORT */
+
+/********************************************************
+                Global Functions
+********************************************************/
+
+#if WIRELESS_EXT > 14
+
+/**
+ *  @brief This function sends customized event to application.
+ *
+ *  @param priv    A pointer to moal_private structure
+ *  @param str	   A pointer to event string
+ *
+ *  @return 	   N/A
+ */
+void
+woal_send_iwevcustom_event(moal_private * priv, t_s8 * str)
+{
+    union iwreq_data iwrq;
+    char buf[IW_CUSTOM_MAX];
+
+    ENTER();
+
+    memset(&iwrq, 0, sizeof(union iwreq_data));
+    memset(buf, 0, sizeof(buf));
+
+    snprintf(buf, sizeof(buf) - 1, "%s", str);
+
+    iwrq.data.pointer = buf;
+    iwrq.data.length = strlen(buf) + 1;
+
+    /* Send Event to upper layer */
+    wireless_send_event(priv->netdev, IWEVCUSTOM, &iwrq, buf);
+    PRINTM(MINFO, "Wireless event %s is sent to application\n", str);
+
+    LEAVE();
+    return;
+}
+#endif
+
+#if WIRELESS_EXT >= 18
+/**
+ *  @brief This function sends mic error event to application.
+ *
+ *  @param priv    A pointer to moal_private structure
+ *  @param event   MIC MERROR EVENT.
+ *
+ *  @return 	   N/A
+ */
+void
+woal_send_mic_error_event(moal_private * priv, t_u32 event)
+{
+    union iwreq_data iwrq;
+    struct iw_michaelmicfailure mic;
+
+    ENTER();
+
+    memset(&iwrq, 0, sizeof(iwrq));
+    memset(&mic, 0, sizeof(mic));
+    if (event == MLAN_EVENT_ID_FW_MIC_ERR_UNI)
+        mic.flags = IW_MICFAILURE_PAIRWISE;
+    else
+        mic.flags = IW_MICFAILURE_GROUP;
+    iwrq.data.pointer = &mic;
+    iwrq.data.length = sizeof(mic);
+
+    wireless_send_event(priv->netdev, IWEVMICHAELMICFAILURE, &iwrq,
+                        (char *) &mic);
+
+    LEAVE();
+    return;
+}
+#endif
+
+#ifdef STA_SUPPORT
+/**
+ *  @brief Set Radio On/OFF
+ *
+ *  @param priv                 A pointer to moal_private structure
+ *  @param option               Radio Option
+ *
+ *  @return                     0 --success, otherwise fail
+ */
+int
+woal_set_radio(moal_private * priv, t_u8 option)
+{
+    int ret = 0;
+    mlan_ds_radio_cfg *radio = NULL;
+    mlan_ioctl_req *req = NULL;
+    ENTER();
+    if ((option != 0) && (option != 1)) {
+        ret = -EINVAL;
+        goto done;
+    }
+    req = woal_alloc_mlan_ioctl_req(sizeof(mlan_ds_radio_cfg));
+    if (req == NULL) {
+        ret = -ENOMEM;
+        goto done;
+    }
+    radio = (mlan_ds_radio_cfg *) req->pbuf;
+    radio->sub_command = MLAN_OID_RADIO_CTRL;
+    req->req_id = MLAN_IOCTL_RADIO_CFG;
+    req->action = MLAN_ACT_SET;
+    radio->param.radio_on_off = option;
+    if (MLAN_STATUS_SUCCESS != woal_request_ioctl(priv, req, MOAL_IOCTL_WAIT)) {
+        ret = -EFAULT;
+        goto done;
+    }
+  done:
+    if (req)
+        kfree(req);
+    LEAVE();
+    return ret;
+}
+
+/** wlan_handler_def */
+struct iw_handler_def woal_handler_def = {
+  num_standard:sizeof(woal_handler) / sizeof(iw_handler),
+  num_private:sizeof(woal_private_handler) / sizeof(iw_handler),
+  num_private_args:sizeof(woal_private_args) / sizeof(struct iw_priv_args),
+  standard:(iw_handler *) woal_handler,
+  private:(iw_handler *) woal_private_handler,
+  private_args:(struct iw_priv_args *) woal_private_args,
+#if WIRELESS_EXT > 20
+  get_wireless_stats:woal_get_wireless_stats,
+#endif
+};
+
+/**
+ *  @brief Get wireless statistics
+ *
+ *  @param dev          A pointer to net_device structure
+ *
+ *  @return             A pointer to iw_statistics buf
+ */
+struct iw_statistics *
+woal_get_wireless_stats(struct net_device *dev)
+{
+    moal_private *priv = (moal_private *) netdev_priv(dev);
+    t_u16 wait_option = MOAL_NO_WAIT;
+
+    ENTER();
+
+    /*
+     * Since schedule() is not allowed from an atomic context
+     * such as when dev_base_lock for netdevices is acquired
+     * for reading/writing in kernel before this call, HostCmd
+     * is issued in non-blocking way in such contexts and
+     * blocking in other cases.
+     */
+    if (write_can_lock(&dev_base_lock)
+        && (!in_atomic() || current->exit_state))
+        wait_option = MOAL_WSTATS_WAIT;
+
+    priv->w_stats.status = woal_get_mode(priv, wait_option);
+    priv->w_stats.discard.retries = priv->stats.tx_errors;
+    priv->w_stats.qual.qual = 0;
+
+    /* Send RSSI command to get beacon RSSI/NF, valid only if associated */
+    if (priv->media_connected == MTRUE) {
+        woal_get_signal_info(priv, wait_option, NULL);
+        priv->w_stats.qual.qual = woal_rssi_to_quality((t_s16)
+                                                       (priv->w_stats.qual.
+                                                        level - 0x100));
+    }
+#if WIRELESS_EXT > 18
+    priv->w_stats.qual.updated |= (IW_QUAL_ALL_UPDATED | IW_QUAL_DBM);
+#else
+    priv->w_stats.qual.updated |= 7;
+#endif
+    if (!priv->w_stats.qual.noise && priv->media_connected == MTRUE)
+        priv->w_stats.qual.noise = MRVDRV_NF_DEFAULT_SCAN_VALUE;
+
+    PRINTM(MINFO, "Link Quality = %#x\n", priv->w_stats.qual.qual);
+    PRINTM(MINFO, "Signal Level = %#x\n", priv->w_stats.qual.level);
+    PRINTM(MINFO, "Noise = %#x\n", priv->w_stats.qual.noise);
+    priv->w_stats.discard.code = 0;
+    woal_get_stats_info(priv, wait_option, NULL);
+
+    LEAVE();
+    return &priv->w_stats;
+}
+#endif /* STA_SUPPORT */
diff --git a/drivers/net/wireless/sd8797/mlinux/moal_wext.h b/drivers/net/wireless/sd8797/mlinux/moal_wext.h
new file mode 100644
index 000000000000..dfafff866b96
--- /dev/null
+++ b/drivers/net/wireless/sd8797/mlinux/moal_wext.h
@@ -0,0 +1,115 @@
+/** @file moal_wext.h
+ *
+ * @brief This file contains definition for wireless extension IOCTL call.
+ *  
+ * Copyright (C) 2008-2011, Marvell International Ltd.  
+ *
+ * This software file (the "File") is distributed by Marvell International 
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991 
+ * (the "License").  You may use, redistribute and/or modify this File in 
+ * accordance with the terms and conditions of the License, a copy of which 
+ * is available by writing to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE 
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE 
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about 
+ * this warranty disclaimer.
+ *
+ */
+
+/********************************************************
+Change log:
+    10/21/2008: initial version
+********************************************************/
+
+#ifndef _WOAL_WEXT_H_
+#define _WOAL_WEXT_H_
+
+/** Custom event : AdHoc link sensed */
+#define CUS_EVT_ADHOC_LINK_SENSED	"EVENT=ADHOC_LINK_SENSED"
+/** Custom event : AdHoc link lost */
+#define CUS_EVT_ADHOC_LINK_LOST		"EVENT=ADHOC_LINK_LOST"
+/** Custom event : MIC failure, unicast */
+#define CUS_EVT_MLME_MIC_ERR_UNI	"MLME-MICHAELMICFAILURE.indication unicast "
+/** Custom event : MIC failure, multicast */
+#define CUS_EVT_MLME_MIC_ERR_MUL	"MLME-MICHAELMICFAILURE.indication multicast "
+/** Custom event : Beacon RSSI low */
+#define CUS_EVT_BEACON_RSSI_LOW		"EVENT=BEACON_RSSI_LOW"
+/** Custom event : Beacon SNR low */
+#define CUS_EVT_BEACON_SNR_LOW		"EVENT=BEACON_SNR_LOW"
+/** Custom event : Beacon RSSI high */
+#define CUS_EVT_BEACON_RSSI_HIGH	"EVENT=BEACON_RSSI_HIGH"
+/** Custom event : Beacon SNR high */
+#define CUS_EVT_BEACON_SNR_HIGH		"EVENT=BEACON_SNR_HIGH"
+/** Custom event : Max fail */
+#define CUS_EVT_MAX_FAIL		"EVENT=MAX_FAIL"
+/** Custom event : Data RSSI low */
+#define CUS_EVT_DATA_RSSI_LOW		"EVENT=DATA_RSSI_LOW"
+/** Custom event : Data SNR low */
+#define CUS_EVT_DATA_SNR_LOW		"EVENT=DATA_SNR_LOW"
+/** Custom event : Data RSSI high */
+#define CUS_EVT_DATA_RSSI_HIGH		"EVENT=DATA_RSSI_HIGH"
+/** Custom event : Data SNR high */
+#define CUS_EVT_DATA_SNR_HIGH		"EVENT=DATA_SNR_HIGH"
+/** Custom event : Link Quality */
+#define CUS_EVT_LINK_QUALITY		"EVENT=LINK_QUALITY"
+/** Custom event : Port Release */
+#define CUS_EVT_PORT_RELEASE		"EVENT=PORT_RELEASE"
+/** Custom event : Pre-Beacon Lost */
+#define CUS_EVT_PRE_BEACON_LOST		"EVENT=PRE_BEACON_LOST"
+
+/** Custom event : Deep Sleep awake */
+#define CUS_EVT_DEEP_SLEEP_AWAKE	"EVENT=DS_AWAKE"
+
+/** Custom event : Host Sleep activated */
+#define CUS_EVT_HS_ACTIVATED		"HS_ACTIVATED "
+/** Custom event : Host Sleep deactivated */
+#define CUS_EVT_HS_DEACTIVATED		"HS_DEACTIVATED "
+/** Custom event : Host Sleep wakeup */
+#define CUS_EVT_HS_WAKEUP		"HS_WAKEUP"
+
+/** Custom event : WEP ICV error */
+#define CUS_EVT_WEP_ICV_ERR		"EVENT=WEP_ICV_ERR"
+
+/** Custom event : Channel Switch Announcment */
+#define CUS_EVT_CHANNEL_SWITCH_ANN	"EVENT=CHANNEL_SWITCH_ANN"
+
+/** Custom event : BW changed */
+#define CUS_EVT_BW_CHANGED		"EVENT=BW_CHANGED"
+/** Custom event : OBSS scan parameter */
+#define CUS_EVT_OBSS_SCAN_PARAM		"EVENT=OBSS_SCAN_PARAM"
+/** Custom indiciation message sent to the application layer for WMM changes */
+#define WMM_CONFIG_CHANGE_INDICATION  "WMM_CONFIG_CHANGE.indication"
+
+#ifdef UAP_SUPPORT
+#ifdef UAP_WEXT
+/** Custom event : STA connected */
+#define CUS_EVT_STA_CONNECTED           "EVENT=STA_CONNECTED"
+/** Custom event : STA disconnected */
+#define CUS_EVT_STA_DISCONNECTED        "EVENT=STA_DISCONNECTED"
+#endif
+#endif
+/** NF value for default scan */
+#define MRVDRV_NF_DEFAULT_SCAN_VALUE		(-96)
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+/** Add event */
+#define IWE_STREAM_ADD_EVENT(i, c, e, w, l) iwe_stream_add_event((i), (c), (e), (w), (l))
+/** Add point */
+#define IWE_STREAM_ADD_POINT(i, c, e, w, p) iwe_stream_add_point((i), (c), (e), (w), (p))
+/** Add value */
+#define IWE_STREAM_ADD_VALUE(i, c, v, e, w, l)  iwe_stream_add_value((i), (c), (v), (e), (w), (l))
+#else
+/** Add event */
+#define IWE_STREAM_ADD_EVENT(i, c, e, w, l) iwe_stream_add_event((c), (e), (w), (l))
+/** Add point */
+#define IWE_STREAM_ADD_POINT(i, c, e, w, p) iwe_stream_add_point((c), (e), (w), (p))
+/** Add value */
+#define IWE_STREAM_ADD_VALUE(i, c, v, e, w, l)  iwe_stream_add_value((c), (v), (e), (w), (l))
+#endif
+
+extern struct iw_handler_def woal_handler_def;
+struct iw_statistics *woal_get_wireless_stats(struct net_device *dev);
+#endif /* _WOAL_WEXT_H_ */
diff --git a/drivers/net/wireless/wl12xx/boot.c b/drivers/net/wireless/wl12xx/boot.c
index 5ebc64d89407..454e913bec67 100644
--- a/drivers/net/wireless/wl12xx/boot.c
+++ b/drivers/net/wireless/wl12xx/boot.c
@@ -358,6 +358,9 @@ static int wl1271_boot_upload_nvs(struct wl1271 *wl)
 		nvs_ptr += 3;
 
 		for (i = 0; i < burst_len; i++) {
+			if (nvs_ptr + 3 >= (u8 *) wl->nvs + nvs_len)
+				goto out_badnvs;
+
 			val = (nvs_ptr[0] | (nvs_ptr[1] << 8)
 			       | (nvs_ptr[2] << 16) | (nvs_ptr[3] << 24));
 
@@ -369,6 +372,9 @@ static int wl1271_boot_upload_nvs(struct wl1271 *wl)
 			nvs_ptr += 4;
 			dest_addr += 4;
 		}
+
+		if (nvs_ptr >= (u8 *) wl->nvs + nvs_len)
+			goto out_badnvs;
 	}
 
 	/*
@@ -380,6 +386,10 @@ static int wl1271_boot_upload_nvs(struct wl1271 *wl)
 	 */
 	nvs_ptr = (u8 *)wl->nvs +
 			ALIGN(nvs_ptr - (u8 *)wl->nvs + 7, 4);
+
+	if (nvs_ptr >= (u8 *) wl->nvs + nvs_len)
+		goto out_badnvs;
+
 	nvs_len -= nvs_ptr - (u8 *)wl->nvs;
 
 	/* Now we must set the partition correctly */
@@ -395,6 +405,10 @@ static int wl1271_boot_upload_nvs(struct wl1271 *wl)
 
 	kfree(nvs_aligned);
 	return 0;
+
+out_badnvs:
+	wl1271_error("nvs data is malformed");
+	return -EILSEQ;
 }
 
 static void wl1271_boot_enable_interrupts(struct wl1271 *wl)
diff --git a/drivers/net/wireless/wl12xx/cmd.c b/drivers/net/wireless/wl12xx/cmd.c
index 97dd237a9580..2144f60b4e6b 100644
--- a/drivers/net/wireless/wl12xx/cmd.c
+++ b/drivers/net/wireless/wl12xx/cmd.c
@@ -120,6 +120,11 @@ int wl1271_cmd_general_parms(struct wl1271 *wl)
 	if (!wl->nvs)
 		return -ENODEV;
 
+	if (gp->tx_bip_fem_manufacturer >= WL1271_INI_FEM_MODULE_COUNT) {
+		wl1271_warning("FEM index from INI out of bounds");
+		return -EINVAL;
+	}
+
 	gen_parms = kzalloc(sizeof(*gen_parms), GFP_KERNEL);
 	if (!gen_parms)
 		return -ENOMEM;
@@ -148,6 +153,12 @@ int wl1271_cmd_general_parms(struct wl1271 *wl)
 	gp->tx_bip_fem_manufacturer =
 		gen_parms->general_params.tx_bip_fem_manufacturer;
 
+	if (gp->tx_bip_fem_manufacturer >= WL1271_INI_FEM_MODULE_COUNT) {
+		wl1271_warning("FEM index from FW out of bounds");
+		ret = -EINVAL;
+		goto out;
+	}
+
 	wl1271_debug(DEBUG_CMD, "FEM autodetect: %s, manufacturer: %d\n",
 		     answer ? "auto" : "manual", gp->tx_bip_fem_manufacturer);
 
@@ -167,6 +178,11 @@ int wl128x_cmd_general_parms(struct wl1271 *wl)
 	if (!wl->nvs)
 		return -ENODEV;
 
+	if (gp->tx_bip_fem_manufacturer >= WL1271_INI_FEM_MODULE_COUNT) {
+		wl1271_warning("FEM index from ini out of bounds");
+		return -EINVAL;
+	}
+
 	gen_parms = kzalloc(sizeof(*gen_parms), GFP_KERNEL);
 	if (!gen_parms)
 		return -ENOMEM;
@@ -191,6 +207,12 @@ int wl128x_cmd_general_parms(struct wl1271 *wl)
 	gp->tx_bip_fem_manufacturer =
 		gen_parms->general_params.tx_bip_fem_manufacturer;
 
+	if (gp->tx_bip_fem_manufacturer >= WL1271_INI_FEM_MODULE_COUNT) {
+		wl1271_warning("FEM index from FW out of bounds");
+		ret = -EINVAL;
+		goto out;
+	}
+
 	wl1271_debug(DEBUG_CMD, "FEM autodetect: %s, manufacturer: %d\n",
 		     answer ? "auto" : "manual", gp->tx_bip_fem_manufacturer);
 
diff --git a/drivers/net/wireless/wl12xx/scan.c b/drivers/net/wireless/wl12xx/scan.c
index edfe01c321ca..afb7356bd48d 100644
--- a/drivers/net/wireless/wl12xx/scan.c
+++ b/drivers/net/wireless/wl12xx/scan.c
@@ -83,14 +83,18 @@ static int wl1271_get_scan_channels(struct wl1271 *wl,
 	for (i = 0, j = 0;
 	     i < req->n_channels && j < WL1271_SCAN_MAX_CHANNELS;
 	     i++) {
-
 		flags = req->channels[i]->flags;
 
 		if (!test_bit(i, wl->scan.scanned_ch) &&
 		    !(flags & IEEE80211_CHAN_DISABLED) &&
-		    ((!!(flags & IEEE80211_CHAN_PASSIVE_SCAN)) == passive) &&
-		    (req->channels[i]->band == band)) {
-
+		    (req->channels[i]->band == band) &&
+		    /*
+		     * In passive scans, we scan all remaining
+		     * channels, even if not marked as such.
+		     * In active scans, we only scan channels not
+		     * marked as passive.
+		     */
+		    (passive || !(flags & IEEE80211_CHAN_PASSIVE_SCAN))) {
 			wl1271_debug(DEBUG_SCAN, "band %d, center_freq %d ",
 				     req->channels[i]->band,
 				     req->channels[i]->center_freq);
@@ -142,6 +146,10 @@ static int wl1271_scan_send(struct wl1271 *wl, enum ieee80211_band band,
 	int ret;
 	u16 scan_options = 0;
 
+	/* skip active scans if we don't have SSIDs */
+	if (!passive && wl->scan.req->n_ssids == 0)
+		return WL1271_NOTHING_TO_SCAN;
+
 	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
 	trigger = kzalloc(sizeof(*trigger), GFP_KERNEL);
 	if (!cmd || !trigger) {
@@ -152,8 +160,7 @@ static int wl1271_scan_send(struct wl1271 *wl, enum ieee80211_band band,
 	/* We always use high priority scans */
 	scan_options = WL1271_SCAN_OPT_PRIORITY_HIGH;
 
-	/* No SSIDs means that we have a forced passive scan */
-	if (passive || wl->scan.req->n_ssids == 0)
+	if (passive)
 		scan_options |= WL1271_SCAN_OPT_PASSIVE;
 
 	cmd->params.scan_options = cpu_to_le16(scan_options);
diff --git a/drivers/net/wireless/wl12xx/testmode.c b/drivers/net/wireless/wl12xx/testmode.c
index 4ae8effaee22..abfb120df118 100644
--- a/drivers/net/wireless/wl12xx/testmode.c
+++ b/drivers/net/wireless/wl12xx/testmode.c
@@ -36,6 +36,7 @@ enum wl1271_tm_commands {
 	WL1271_TM_CMD_TEST,
 	WL1271_TM_CMD_INTERROGATE,
 	WL1271_TM_CMD_CONFIGURE,
+	WL1271_TM_CMD_NVS_PUSH,		/* Not in use. Keep to not break ABI */
 	WL1271_TM_CMD_SET_PLT_MODE,
 	WL1271_TM_CMD_RECOVER,
 
diff --git a/drivers/nfc/pn544.c b/drivers/nfc/pn544.c
index 724f65d8f9e4..91216e465e12 100644
--- a/drivers/nfc/pn544.c
+++ b/drivers/nfc/pn544.c
@@ -1,893 +1,400 @@
 /*
- * Driver for the PN544 NFC chip.
+ * Copyright (C) 2010 Trusted Logic S.A.
  *
- * Copyright (C) Nokia Corporation
- *
- * Author: Jari Vanhala <ext-jari.vanhala@nokia.com>
- * Contact: Matti Aaltonen <matti.j.aaltonen@nokia.com>
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * version 2 as published by the Free Software Foundation.
+ * 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
+ * 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
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
  */
 
-#include <linux/completion.h>
-#include <linux/crc-ccitt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/i2c.h>
+#include <linux/irq.h>
+#include <linux/jiffies.h>
+#include <linux/uaccess.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
-#include <linux/kernel.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/gpio.h>
 #include <linux/miscdevice.h>
-#include <linux/module.h>
-#include <linux/mutex.h>
+#include <linux/spinlock.h>
 #include <linux/nfc/pn544.h>
-#include <linux/poll.h>
-#include <linux/regulator/consumer.h>
-#include <linux/serial_core.h> /* for TCGETS */
-#include <linux/slab.h>
-
-#define DRIVER_CARD	"PN544 NFC"
-#define DRIVER_DESC	"NFC driver for PN544"
-
-static struct i2c_device_id pn544_id_table[] = {
-	{ PN544_DRIVER_NAME, 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, pn544_id_table);
-
-#define HCI_MODE	0
-#define FW_MODE		1
 
-enum pn544_state {
-	PN544_ST_COLD,
-	PN544_ST_FW_READY,
-	PN544_ST_READY,
+#define MAX_BUFFER_SIZE	512
+
+struct pn544_dev	{
+	wait_queue_head_t	read_wq;
+	struct mutex		read_mutex;
+	struct i2c_client	*client;
+	struct miscdevice	pn544_device;
+	unsigned int 		ven_gpio;
+	unsigned int 		firm_gpio;
+	unsigned int		irq_gpio;
+	bool			irq_enabled;
+	spinlock_t		irq_enabled_lock;
 };
 
-enum pn544_irq {
-	PN544_NONE,
-	PN544_INT,
-};
-
-struct pn544_info {
-	struct miscdevice miscdev;
-	struct i2c_client *i2c_dev;
-	struct regulator_bulk_data regs[3];
-
-	enum pn544_state state;
-	wait_queue_head_t read_wait;
-	loff_t read_offset;
-	enum pn544_irq read_irq;
-	struct mutex read_mutex; /* Serialize read_irq access */
-	struct mutex mutex; /* Serialize info struct access */
-	u8 *buf;
-	size_t buflen;
-};
-
-static const char reg_vdd_io[]	= "Vdd_IO";
-static const char reg_vbat[]	= "VBat";
-static const char reg_vsim[]	= "VSim";
-
-/* sysfs interface */
-static ssize_t pn544_test(struct device *dev,
-			  struct device_attribute *attr, char *buf)
-{
-	struct pn544_info *info = dev_get_drvdata(dev);
-	struct i2c_client *client = info->i2c_dev;
-	struct pn544_nfc_platform_data *pdata = client->dev.platform_data;
-
-	return snprintf(buf, PAGE_SIZE, "%d\n", pdata->test());
-}
-
-static int pn544_enable(struct pn544_info *info, int mode)
-{
-	struct pn544_nfc_platform_data *pdata;
-	struct i2c_client *client = info->i2c_dev;
-
-	int r;
-
-	r = regulator_bulk_enable(ARRAY_SIZE(info->regs), info->regs);
-	if (r < 0)
-		return r;
-
-	pdata = client->dev.platform_data;
-	info->read_irq = PN544_NONE;
-	if (pdata->enable)
-		pdata->enable(mode);
-
-	if (mode) {
-		info->state = PN544_ST_FW_READY;
-		dev_dbg(&client->dev, "now in FW-mode\n");
-	} else {
-		info->state = PN544_ST_READY;
-		dev_dbg(&client->dev, "now in HCI-mode\n");
-	}
-
-	usleep_range(10000, 15000);
-
-	return 0;
-}
-
-static void pn544_disable(struct pn544_info *info)
-{
-	struct pn544_nfc_platform_data *pdata;
-	struct i2c_client *client = info->i2c_dev;
-
-	pdata = client->dev.platform_data;
-	if (pdata->disable)
-		pdata->disable();
-
-	info->state = PN544_ST_COLD;
-
-	dev_dbg(&client->dev, "Now in OFF-mode\n");
-
-	msleep(PN544_RESETVEN_TIME);
-
-	info->read_irq = PN544_NONE;
-	regulator_bulk_disable(ARRAY_SIZE(info->regs), info->regs);
-}
-
-static int check_crc(u8 *buf, int buflen)
+static void pn544_disable_irq(struct pn544_dev *pn544_dev)
 {
-	u8 len;
-	u16 crc;
-
-	len = buf[0] + 1;
-	if (len < 4 || len != buflen || len > PN544_MSG_MAX_SIZE) {
-		pr_err(PN544_DRIVER_NAME
-		       ": CRC; corrupt packet len %u (%d)\n", len, buflen);
-		print_hex_dump(KERN_DEBUG, "crc: ", DUMP_PREFIX_NONE,
-			       16, 2, buf, buflen, false);
-		return -EPERM;
-	}
-	crc = crc_ccitt(0xffff, buf, len - 2);
-	crc = ~crc;
+	unsigned long flags;
 
-	if (buf[len-2] != (crc & 0xff) || buf[len-1] != (crc >> 8)) {
-		pr_err(PN544_DRIVER_NAME ": CRC error 0x%x != 0x%x 0x%x\n",
-		       crc, buf[len-1], buf[len-2]);
-
-		print_hex_dump(KERN_DEBUG, "crc: ", DUMP_PREFIX_NONE,
-			       16, 2, buf, buflen, false);
-		return -EPERM;
+	spin_lock_irqsave(&pn544_dev->irq_enabled_lock, flags);
+	if (pn544_dev->irq_enabled) {
+		disable_irq_nosync(pn544_dev->client->irq);
+		pn544_dev->irq_enabled = false;
 	}
-	return 0;
+	spin_unlock_irqrestore(&pn544_dev->irq_enabled_lock, flags);
 }
 
-static int pn544_i2c_write(struct i2c_client *client, u8 *buf, int len)
+static irqreturn_t pn544_dev_irq_handler(int irq, void *dev_id)
 {
-	int r;
-
-	if (len < 4 || len != (buf[0] + 1)) {
-		dev_err(&client->dev, "%s: Illegal message length: %d\n",
-			__func__, len);
-		return -EINVAL;
-	}
-
-	if (check_crc(buf, len))
-		return -EINVAL;
+	struct pn544_dev *pn544_dev = dev_id;
 
-	usleep_range(3000, 6000);
-
-	r = i2c_master_send(client, buf, len);
-	dev_dbg(&client->dev, "send: %d\n", r);
-
-	if (r == -EREMOTEIO) { /* Retry, chip was in standby */
-		usleep_range(6000, 10000);
-		r = i2c_master_send(client, buf, len);
-		dev_dbg(&client->dev, "send2: %d\n", r);
-	}
+	pn544_disable_irq(pn544_dev);
 
-	if (r != len)
-		return -EREMOTEIO;
+	/* Wake up waiting readers */
+	wake_up(&pn544_dev->read_wq);
 
-	return r;
+	return IRQ_HANDLED;
 }
 
-static int pn544_i2c_read(struct i2c_client *client, u8 *buf, int buflen)
+static ssize_t pn544_dev_read(struct file *filp, char __user *buf,
+		size_t count, loff_t *offset)
 {
-	int r;
-	u8 len;
-
-	/*
-	 * You could read a packet in one go, but then you'd need to read
-	 * max size and rest would be 0xff fill, so we do split reads.
-	 */
-	r = i2c_master_recv(client, &len, 1);
-	dev_dbg(&client->dev, "recv1: %d\n", r);
-
-	if (r != 1)
-		return -EREMOTEIO;
-
-	if (len < PN544_LLC_HCI_OVERHEAD)
-		len = PN544_LLC_HCI_OVERHEAD;
-	else if (len > (PN544_MSG_MAX_SIZE - 1))
-		len = PN544_MSG_MAX_SIZE - 1;
-
-	if (1 + len > buflen) /* len+(data+crc16) */
-		return -EMSGSIZE;
-
-	buf[0] = len;
+	struct pn544_dev *pn544_dev = filp->private_data;
+	char tmp[MAX_BUFFER_SIZE];
+	int ret;
 
-	r = i2c_master_recv(client, buf + 1, len);
-	dev_dbg(&client->dev, "recv2: %d\n", r);
+	if (count > MAX_BUFFER_SIZE)
+		count = MAX_BUFFER_SIZE;
 
-	if (r != len)
-		return -EREMOTEIO;
+	pr_debug("%s : reading %zu bytes.\n", __func__, count);
 
-	usleep_range(3000, 6000);
+	mutex_lock(&pn544_dev->read_mutex);
 
-	return r + 1;
-}
+	if (!gpio_get_value(pn544_dev->irq_gpio)) {
+		if (filp->f_flags & O_NONBLOCK) {
+			ret = -EAGAIN;
+			goto fail;
+		}
 
-static int pn544_fw_write(struct i2c_client *client, u8 *buf, int len)
-{
-	int r;
+		pn544_dev->irq_enabled = true;
+		enable_irq(pn544_dev->client->irq);
+		ret = wait_event_interruptible(pn544_dev->read_wq,
+				gpio_get_value(pn544_dev->irq_gpio));
 
-	dev_dbg(&client->dev, "%s\n", __func__);
+		pn544_disable_irq(pn544_dev);
 
-	if (len < PN544_FW_HEADER_SIZE ||
-	    (PN544_FW_HEADER_SIZE + (buf[1] << 8) + buf[2]) != len)
-		return -EINVAL;
+		if (ret)
+			goto fail;
 
-	r = i2c_master_send(client, buf, len);
-	dev_dbg(&client->dev, "fw send: %d\n", r);
-
-	if (r == -EREMOTEIO) { /* Retry, chip was in standby */
-		usleep_range(6000, 10000);
-		r = i2c_master_send(client, buf, len);
-		dev_dbg(&client->dev, "fw send2: %d\n", r);
 	}
 
-	if (r != len)
-		return -EREMOTEIO;
-
-	return r;
-}
-
-static int pn544_fw_read(struct i2c_client *client, u8 *buf, int buflen)
-{
-	int r, len;
-
-	if (buflen < PN544_FW_HEADER_SIZE)
-		return -EINVAL;
-
-	r = i2c_master_recv(client, buf, PN544_FW_HEADER_SIZE);
-	dev_dbg(&client->dev, "FW recv1: %d\n", r);
-
-	if (r < 0)
-		return r;
-
-	if (r < PN544_FW_HEADER_SIZE)
-		return -EINVAL;
-
-	len = (buf[1] << 8) + buf[2];
-	if (len == 0) /* just header, no additional data */
-		return r;
-
-	if (len > buflen - PN544_FW_HEADER_SIZE)
-		return -EMSGSIZE;
-
-	r = i2c_master_recv(client, buf + PN544_FW_HEADER_SIZE, len);
-	dev_dbg(&client->dev, "fw recv2: %d\n", r);
-
-	if (r != len)
-		return -EINVAL;
-
-	return r + PN544_FW_HEADER_SIZE;
-}
-
-static irqreturn_t pn544_irq_thread_fn(int irq, void *dev_id)
-{
-	struct pn544_info *info = dev_id;
-	struct i2c_client *client = info->i2c_dev;
-
-	BUG_ON(!info);
-	BUG_ON(irq != info->i2c_dev->irq);
-
-	dev_dbg(&client->dev, "IRQ\n");
-
-	mutex_lock(&info->read_mutex);
-	info->read_irq = PN544_INT;
-	mutex_unlock(&info->read_mutex);
-
-	wake_up_interruptible(&info->read_wait);
+	/* Read data */
+	ret = i2c_master_recv(pn544_dev->client, tmp, count);
 
-	return IRQ_HANDLED;
-}
-
-static enum pn544_irq pn544_irq_state(struct pn544_info *info)
-{
-	enum pn544_irq irq;
-
-	mutex_lock(&info->read_mutex);
-	irq = info->read_irq;
-	mutex_unlock(&info->read_mutex);
-	/*
-	 * XXX: should we check GPIO-line status directly?
-	 * return pdata->irq_status() ? PN544_INT : PN544_NONE;
-	 */
-
-	return irq;
-}
+	mutex_unlock(&pn544_dev->read_mutex);
 
-static ssize_t pn544_read(struct file *file, char __user *buf,
-			  size_t count, loff_t *offset)
-{
-	struct pn544_info *info = container_of(file->private_data,
-					       struct pn544_info, miscdev);
-	struct i2c_client *client = info->i2c_dev;
-	enum pn544_irq irq;
-	size_t len;
-	int r = 0;
-
-	dev_dbg(&client->dev, "%s: info: %p, count: %zu\n", __func__,
-		info, count);
+	/* pn544 seems to be slow in handling I2C read requests
+	 * so add 1ms delay after recv operation */
+	udelay(1000);
 
-	mutex_lock(&info->mutex);
-
-	if (info->state == PN544_ST_COLD) {
-		r = -ENODEV;
-		goto out;
+	if (ret < 0) {
+		pr_err("%s: i2c_master_recv returned %d\n", __func__, ret);
+		return ret;
 	}
-
-	irq = pn544_irq_state(info);
-	if (irq == PN544_NONE) {
-		if (file->f_flags & O_NONBLOCK) {
-			r = -EAGAIN;
-			goto out;
-		}
-
-		if (wait_event_interruptible(info->read_wait,
-					     (info->read_irq == PN544_INT))) {
-			r = -ERESTARTSYS;
-			goto out;
-		}
+	if (ret > count) {
+		pr_err("%s: received too many bytes from i2c (%d)\n",
+			__func__, ret);
+		return -EIO;
 	}
-
-	if (info->state == PN544_ST_FW_READY) {
-		len = min(count, info->buflen);
-
-		mutex_lock(&info->read_mutex);
-		r = pn544_fw_read(info->i2c_dev, info->buf, len);
-		info->read_irq = PN544_NONE;
-		mutex_unlock(&info->read_mutex);
-
-		if (r < 0) {
-			dev_err(&info->i2c_dev->dev, "FW read failed: %d\n", r);
-			goto out;
-		}
-
-		print_hex_dump(KERN_DEBUG, "FW read: ", DUMP_PREFIX_NONE,
-			       16, 2, info->buf, r, false);
-
-		*offset += r;
-		if (copy_to_user(buf, info->buf, r)) {
-			r = -EFAULT;
-			goto out;
-		}
-	} else {
-		len = min(count, info->buflen);
-
-		mutex_lock(&info->read_mutex);
-		r = pn544_i2c_read(info->i2c_dev, info->buf, len);
-		info->read_irq = PN544_NONE;
-		mutex_unlock(&info->read_mutex);
-
-		if (r < 0) {
-			dev_err(&info->i2c_dev->dev, "read failed (%d)\n", r);
-			goto out;
-		}
-		print_hex_dump(KERN_DEBUG, "read: ", DUMP_PREFIX_NONE,
-			       16, 2, info->buf, r, false);
-
-		*offset += r;
-		if (copy_to_user(buf, info->buf, r)) {
-			r = -EFAULT;
-			goto out;
-		}
+	if (copy_to_user(buf, tmp, ret)) {
+		pr_warning("%s : failed to copy to user space\n", __func__);
+		return -EFAULT;
 	}
+	return ret;
 
-out:
-	mutex_unlock(&info->mutex);
-
-	return r;
+fail:
+	mutex_unlock(&pn544_dev->read_mutex);
+	return ret;
 }
 
-static unsigned int pn544_poll(struct file *file, poll_table *wait)
+static ssize_t pn544_dev_write(struct file *filp, const char __user *buf,
+		size_t count, loff_t *offset)
 {
-	struct pn544_info *info = container_of(file->private_data,
-					       struct pn544_info, miscdev);
-	struct i2c_client *client = info->i2c_dev;
-	int r = 0;
+	struct pn544_dev  *pn544_dev;
+	char tmp[MAX_BUFFER_SIZE];
+	int ret;
 
-	dev_dbg(&client->dev, "%s: info: %p\n", __func__, info);
+	pn544_dev = filp->private_data;
 
-	mutex_lock(&info->mutex);
+	if (count > MAX_BUFFER_SIZE)
+		count = MAX_BUFFER_SIZE;
 
-	if (info->state == PN544_ST_COLD) {
-		r = -ENODEV;
-		goto out;
+	if (copy_from_user(tmp, buf, count)) {
+		pr_err("%s : failed to copy from user space\n", __func__);
+		return -EFAULT;
 	}
 
-	poll_wait(file, &info->read_wait, wait);
-
-	if (pn544_irq_state(info) == PN544_INT) {
-		r = POLLIN | POLLRDNORM;
-		goto out;
+	pr_debug("%s : writing %zu bytes.\n", __func__, count);
+	/* Write data */
+	ret = i2c_master_send(pn544_dev->client, tmp, count);
+	if (ret != count) {
+		pr_err("%s : i2c_master_send returned %d\n", __func__, ret);
+		ret = -EIO;
 	}
-out:
-	mutex_unlock(&info->mutex);
 
-	return r;
+	/* pn544 seems to be slow in handling I2C write requests
+	 * so add 1ms delay after I2C send oparation */
+	udelay(1000);
+
+	return ret;
 }
 
-static ssize_t pn544_write(struct file *file, const char __user *buf,
-			   size_t count, loff_t *ppos)
+static int pn544_dev_open(struct inode *inode, struct file *filp)
 {
-	struct pn544_info *info = container_of(file->private_data,
-					       struct pn544_info, miscdev);
-	struct i2c_client *client = info->i2c_dev;
-	ssize_t	len;
-	int r;
-
-	dev_dbg(&client->dev, "%s: info: %p, count %zu\n", __func__,
-		info, count);
-
-	mutex_lock(&info->mutex);
-
-	if (info->state == PN544_ST_COLD) {
-		r = -ENODEV;
-		goto out;
-	}
-
-	/*
-	 * XXX: should we detect rset-writes and clean possible
-	 * read_irq state
-	 */
-	if (info->state == PN544_ST_FW_READY) {
-		size_t fw_len;
-
-		if (count < PN544_FW_HEADER_SIZE) {
-			r = -EINVAL;
-			goto out;
-		}
-
-		len = min(count, info->buflen);
-		if (copy_from_user(info->buf, buf, len)) {
-			r = -EFAULT;
-			goto out;
-		}
-
-		print_hex_dump(KERN_DEBUG, "FW write: ", DUMP_PREFIX_NONE,
-			       16, 2, info->buf, len, false);
-
-		fw_len = PN544_FW_HEADER_SIZE + (info->buf[1] << 8) +
-			info->buf[2];
-
-		if (len > fw_len) /* 1 msg at a time */
-			len = fw_len;
+	struct pn544_dev *pn544_dev = container_of(filp->private_data,
+						struct pn544_dev,
+						pn544_device);
 
-		r = pn544_fw_write(info->i2c_dev, info->buf, len);
-	} else {
-		if (count < PN544_LLC_MIN_SIZE) {
-			r = -EINVAL;
-			goto out;
-		}
-
-		len = min(count, info->buflen);
-		if (copy_from_user(info->buf, buf, len)) {
-			r = -EFAULT;
-			goto out;
-		}
-
-		print_hex_dump(KERN_DEBUG, "write: ", DUMP_PREFIX_NONE,
-			       16, 2, info->buf, len, false);
-
-		if (len > (info->buf[0] + 1)) /* 1 msg at a time */
-			len  = info->buf[0] + 1;
-
-		r = pn544_i2c_write(info->i2c_dev, info->buf, len);
-	}
-out:
-	mutex_unlock(&info->mutex);
+	filp->private_data = pn544_dev;
 
-	return r;
+	pr_debug("%s : %d,%d\n", __func__, imajor(inode), iminor(inode));
 
+	return 0;
 }
 
-static long pn544_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+static long  pn544_dev_ioctl(struct file *filp, unsigned int cmd,
+				unsigned long arg)
 {
-	struct pn544_info *info = container_of(file->private_data,
-					       struct pn544_info, miscdev);
-	struct i2c_client *client = info->i2c_dev;
-	struct pn544_nfc_platform_data *pdata;
-	unsigned int val;
-	int r = 0;
-
-	dev_dbg(&client->dev, "%s: info: %p, cmd: 0x%x\n", __func__, info, cmd);
+	struct pn544_dev *pn544_dev = filp->private_data;
 
-	mutex_lock(&info->mutex);
-
-	if (info->state == PN544_ST_COLD) {
-		r = -ENODEV;
-		goto out;
-	}
-
-	pdata = info->i2c_dev->dev.platform_data;
 	switch (cmd) {
-	case PN544_GET_FW_MODE:
-		dev_dbg(&client->dev, "%s:  PN544_GET_FW_MODE\n", __func__);
-
-		val = (info->state == PN544_ST_FW_READY);
-		if (copy_to_user((void __user *)arg, &val, sizeof(val))) {
-			r = -EFAULT;
-			goto out;
-		}
-
-		break;
-
-	case PN544_SET_FW_MODE:
-		dev_dbg(&client->dev, "%s:  PN544_SET_FW_MODE\n", __func__);
-
-		if (copy_from_user(&val, (void __user *)arg, sizeof(val))) {
-			r = -EFAULT;
-			goto out;
-		}
-
-		if (val) {
-			if (info->state == PN544_ST_FW_READY)
-				break;
-
-			pn544_disable(info);
-			r = pn544_enable(info, FW_MODE);
-			if (r < 0)
-				goto out;
+	case PN544_SET_PWR:
+		if (arg == 2) {
+			/* power on with firmware download (requires hw reset)
+			 */
+			pr_info("%s power on with firmware\n", __func__);
+			gpio_set_value(pn544_dev->ven_gpio, 1);
+			msleep(20);
+			if (pn544_dev->firm_gpio)
+				gpio_set_value(pn544_dev->firm_gpio, 1);
+			msleep(20);
+			gpio_set_value(pn544_dev->ven_gpio, 0);
+			msleep(100);
+			gpio_set_value(pn544_dev->ven_gpio, 1);
+			msleep(20);
+		} else if (arg == 1) {
+			/* power on */
+			pr_info("%s power on\n", __func__);
+			if (pn544_dev->firm_gpio)
+				gpio_set_value(pn544_dev->firm_gpio, 0);
+			gpio_set_value(pn544_dev->ven_gpio, 1);
+			msleep(100);
+		} else  if (arg == 0) {
+			/* power off */
+			pr_info("%s power off\n", __func__);
+			if (pn544_dev->firm_gpio)
+				gpio_set_value(pn544_dev->firm_gpio, 0);
+			gpio_set_value(pn544_dev->ven_gpio, 0);
+			msleep(100);
 		} else {
-			if (info->state == PN544_ST_READY)
-				break;
-			pn544_disable(info);
-			r = pn544_enable(info, HCI_MODE);
-			if (r < 0)
-				goto out;
+			pr_err("%s bad arg %lu\n", __func__, arg);
+			return -EINVAL;
 		}
-		file->f_pos = info->read_offset;
 		break;
-
-	case TCGETS:
-		dev_dbg(&client->dev, "%s:  TCGETS\n", __func__);
-
-		r = -ENOIOCTLCMD;
-		break;
-
 	default:
-		dev_err(&client->dev, "Unknown ioctl 0x%x\n", cmd);
-		r = -ENOIOCTLCMD;
-		break;
-	}
-
-out:
-	mutex_unlock(&info->mutex);
-
-	return r;
-}
-
-static int pn544_open(struct inode *inode, struct file *file)
-{
-	struct pn544_info *info = container_of(file->private_data,
-					       struct pn544_info, miscdev);
-	struct i2c_client *client = info->i2c_dev;
-	int r = 0;
-
-	dev_dbg(&client->dev, "%s: info: %p, client %p\n", __func__,
-		info, info->i2c_dev);
-
-	mutex_lock(&info->mutex);
-
-	/*
-	 * Only 1 at a time.
-	 * XXX: maybe user (counter) would work better
-	 */
-	if (info->state != PN544_ST_COLD) {
-		r = -EBUSY;
-		goto out;
+		pr_err("%s bad ioctl %u\n", __func__, cmd);
+		return -EINVAL;
 	}
 
-	file->f_pos = info->read_offset;
-	r = pn544_enable(info, HCI_MODE);
-
-out:
-	mutex_unlock(&info->mutex);
-	return r;
-}
-
-static int pn544_close(struct inode *inode, struct file *file)
-{
-	struct pn544_info *info = container_of(file->private_data,
-					       struct pn544_info, miscdev);
-	struct i2c_client *client = info->i2c_dev;
-
-	dev_dbg(&client->dev, "%s: info: %p, client %p\n",
-		__func__, info, info->i2c_dev);
-
-	mutex_lock(&info->mutex);
-	pn544_disable(info);
-	mutex_unlock(&info->mutex);
-
 	return 0;
 }
 
-static const struct file_operations pn544_fops = {
-	.owner		= THIS_MODULE,
-	.llseek		= no_llseek,
-	.read		= pn544_read,
-	.write		= pn544_write,
-	.poll		= pn544_poll,
-	.open		= pn544_open,
-	.release	= pn544_close,
-	.unlocked_ioctl	= pn544_ioctl,
+static const struct file_operations pn544_dev_fops = {
+	.owner	= THIS_MODULE,
+	.llseek	= no_llseek,
+	.read	= pn544_dev_read,
+	.write	= pn544_dev_write,
+	.open	= pn544_dev_open,
+	.unlocked_ioctl  = pn544_dev_ioctl,
 };
 
-#ifdef CONFIG_PM
-static int pn544_suspend(struct device *dev)
-{
-	struct i2c_client *client = to_i2c_client(dev);
-	struct pn544_info *info;
-	int r = 0;
-
-	dev_info(&client->dev, "***\n%s: client %p\n***\n", __func__, client);
-
-	info = i2c_get_clientdata(client);
-	dev_info(&client->dev, "%s: info: %p, client %p\n", __func__,
-		 info, client);
-
-	mutex_lock(&info->mutex);
-
-	switch (info->state) {
-	case PN544_ST_FW_READY:
-		/* Do not suspend while upgrading FW, please! */
-		r = -EPERM;
-		break;
-
-	case PN544_ST_READY:
-		/*
-		 * CHECK: Device should be in standby-mode. No way to check?
-		 * Allowing low power mode for the regulator is potentially
-		 * dangerous if pn544 does not go to suspension.
-		 */
-		break;
-
-	case PN544_ST_COLD:
-		break;
-	};
-
-	mutex_unlock(&info->mutex);
-	return r;
-}
-
-static int pn544_resume(struct device *dev)
-{
-	struct i2c_client *client = to_i2c_client(dev);
-	struct pn544_info *info = i2c_get_clientdata(client);
-	int r = 0;
-
-	dev_dbg(&client->dev, "%s: info: %p, client %p\n", __func__,
-		info, client);
-
-	mutex_lock(&info->mutex);
-
-	switch (info->state) {
-	case PN544_ST_READY:
-		/*
-		 * CHECK: If regulator low power mode is allowed in
-		 * pn544_suspend, we should go back to normal mode
-		 * here.
-		 */
-		break;
-
-	case PN544_ST_COLD:
-		break;
-
-	case PN544_ST_FW_READY:
-		break;
-	};
-
-	mutex_unlock(&info->mutex);
-
-	return r;
-}
-
-static SIMPLE_DEV_PM_OPS(pn544_pm_ops, pn544_suspend, pn544_resume);
-#endif
-
-static struct device_attribute pn544_attr =
-	__ATTR(nfc_test, S_IRUGO, pn544_test, NULL);
-
-static int __devinit pn544_probe(struct i2c_client *client,
-				 const struct i2c_device_id *id)
+static int pn544_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
 {
-	struct pn544_info *info;
-	struct pn544_nfc_platform_data *pdata;
-	int r = 0;
+	int ret;
+	struct pn544_i2c_platform_data *platform_data;
+	struct pn544_dev *pn544_dev;
 
-	dev_dbg(&client->dev, "%s\n", __func__);
-	dev_dbg(&client->dev, "IRQ: %d\n", client->irq);
+	platform_data = client->dev.platform_data;
 
-	/* private data allocation */
-	info = kzalloc(sizeof(struct pn544_info), GFP_KERNEL);
-	if (!info) {
-		dev_err(&client->dev,
-			"Cannot allocate memory for pn544_info.\n");
-		r = -ENOMEM;
-		goto err_info_alloc;
+	if (platform_data == NULL) {
+		pr_err("%s : nfc probe fail\n", __func__);
+		return  -ENODEV;
 	}
 
-	info->buflen = max(PN544_MSG_MAX_SIZE, PN544_MAX_I2C_TRANSFER);
-	info->buf = kzalloc(info->buflen, GFP_KERNEL);
-	if (!info->buf) {
-		dev_err(&client->dev,
-			"Cannot allocate memory for pn544_info->buf.\n");
-		r = -ENOMEM;
-		goto err_buf_alloc;
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		pr_err("%s : need I2C_FUNC_I2C\n", __func__);
+		return  -ENODEV;
 	}
 
-	info->regs[0].supply = reg_vdd_io;
-	info->regs[1].supply = reg_vbat;
-	info->regs[2].supply = reg_vsim;
-	r = regulator_bulk_get(&client->dev, ARRAY_SIZE(info->regs),
-				 info->regs);
-	if (r < 0)
-		goto err_kmalloc;
-
-	info->i2c_dev = client;
-	info->state = PN544_ST_COLD;
-	info->read_irq = PN544_NONE;
-	mutex_init(&info->read_mutex);
-	mutex_init(&info->mutex);
-	init_waitqueue_head(&info->read_wait);
-	i2c_set_clientdata(client, info);
-	pdata = client->dev.platform_data;
-	if (!pdata) {
-		dev_err(&client->dev, "No platform data\n");
-		r = -EINVAL;
-		goto err_reg;
+	ret = gpio_request(platform_data->irq_gpio, "nfc_int");
+	if (ret)
+		return  -ENODEV;
+	ret = gpio_request(platform_data->ven_gpio, "nfc_ven");
+	if (ret)
+		goto err_ven;
+	if (platform_data->firm_gpio) {
+		ret = gpio_request(platform_data->firm_gpio, "nfc_firm");
+		if (ret)
+			goto err_firm;
 	}
 
-	if (!pdata->request_resources) {
-		dev_err(&client->dev, "request_resources() missing\n");
-		r = -EINVAL;
-		goto err_reg;
+	pn544_dev = kzalloc(sizeof(*pn544_dev), GFP_KERNEL);
+	if (pn544_dev == NULL) {
+		dev_err(&client->dev,
+				"failed to allocate memory for module data\n");
+		ret = -ENOMEM;
+		goto err_exit;
 	}
 
-	r = pdata->request_resources(client);
-	if (r) {
-		dev_err(&client->dev, "Cannot get platform resources\n");
-		goto err_reg;
-	}
+	pn544_dev->irq_gpio = platform_data->irq_gpio;
+	pn544_dev->ven_gpio  = platform_data->ven_gpio;
+	pn544_dev->firm_gpio  = platform_data->firm_gpio;
+	pn544_dev->client   = client;
 
-	r = request_threaded_irq(client->irq, NULL, pn544_irq_thread_fn,
-				 IRQF_TRIGGER_RISING, PN544_DRIVER_NAME,
-				 info);
-	if (r < 0) {
-		dev_err(&client->dev, "Unable to register IRQ handler\n");
-		goto err_res;
+	ret = gpio_direction_input(pn544_dev->irq_gpio);
+	if (ret < 0) {
+		pr_err("%s :not able to set irq_gpio as input\n", __func__);
+		goto err_ven;
 	}
-
-	/* If we don't have the test we don't need the sysfs file */
-	if (pdata->test) {
-		r = device_create_file(&client->dev, &pn544_attr);
-		if (r) {
-			dev_err(&client->dev,
-				"sysfs registration failed, error %d\n", r);
-			goto err_irq;
+	ret = gpio_direction_output(pn544_dev->ven_gpio, 0);
+	if (ret < 0) {
+		pr_err("%s : not able to set ven_gpio as output\n", __func__);
+		goto err_firm;
+	}
+	if (platform_data->firm_gpio) {
+		ret = gpio_direction_output(pn544_dev->firm_gpio, 0);
+		if (ret < 0) {
+			pr_err("%s : not able to set firm_gpio as output\n",
+				 __func__);
+			goto err_exit;
 		}
 	}
 
-	info->miscdev.minor = MISC_DYNAMIC_MINOR;
-	info->miscdev.name = PN544_DRIVER_NAME;
-	info->miscdev.fops = &pn544_fops;
-	info->miscdev.parent = &client->dev;
-	r = misc_register(&info->miscdev);
-	if (r < 0) {
-		dev_err(&client->dev, "Device registration failed\n");
-		goto err_sysfs;
+	/* init mutex and queues */
+	init_waitqueue_head(&pn544_dev->read_wq);
+	mutex_init(&pn544_dev->read_mutex);
+	spin_lock_init(&pn544_dev->irq_enabled_lock);
+
+	pn544_dev->pn544_device.minor = MISC_DYNAMIC_MINOR;
+	pn544_dev->pn544_device.name = "pn544";
+	pn544_dev->pn544_device.fops = &pn544_dev_fops;
+
+	ret = misc_register(&pn544_dev->pn544_device);
+	if (ret) {
+		pr_err("%s : misc_register failed\n", __FILE__);
+		goto err_misc_register;
 	}
 
-	dev_dbg(&client->dev, "%s: info: %p, pdata %p, client %p\n",
-		__func__, info, pdata, client);
+	/* request irq.  the irq is set whenever the chip has data available
+	 * for reading.  it is cleared when all data has been read.
+	 */
+	pr_info("%s : requesting IRQ %d\n", __func__, client->irq);
+	pn544_dev->irq_enabled = true;
+	ret = request_irq(client->irq, pn544_dev_irq_handler,
+			  IRQF_TRIGGER_HIGH, client->name, pn544_dev);
+	if (ret) {
+		dev_err(&client->dev, "request_irq failed\n");
+		goto err_request_irq_failed;
+	}
+	pn544_disable_irq(pn544_dev);
+	i2c_set_clientdata(client, pn544_dev);
 
 	return 0;
 
-err_sysfs:
-	if (pdata->test)
-		device_remove_file(&client->dev, &pn544_attr);
-err_irq:
-	free_irq(client->irq, info);
-err_res:
-	if (pdata->free_resources)
-		pdata->free_resources();
-err_reg:
-	regulator_bulk_free(ARRAY_SIZE(info->regs), info->regs);
-err_kmalloc:
-	kfree(info->buf);
-err_buf_alloc:
-	kfree(info);
-err_info_alloc:
-	return r;
+err_request_irq_failed:
+	misc_deregister(&pn544_dev->pn544_device);
+err_misc_register:
+	mutex_destroy(&pn544_dev->read_mutex);
+	kfree(pn544_dev);
+err_exit:
+	if (pn544_dev->firm_gpio)
+		gpio_free(platform_data->firm_gpio);
+err_firm:
+	gpio_free(platform_data->ven_gpio);
+err_ven:
+	gpio_free(platform_data->irq_gpio);
+	return ret;
 }
 
-static __devexit int pn544_remove(struct i2c_client *client)
+static int pn544_remove(struct i2c_client *client)
 {
-	struct pn544_info *info = i2c_get_clientdata(client);
-	struct pn544_nfc_platform_data *pdata = client->dev.platform_data;
-
-	dev_dbg(&client->dev, "%s\n", __func__);
-
-	misc_deregister(&info->miscdev);
-	if (pdata->test)
-		device_remove_file(&client->dev, &pn544_attr);
-
-	if (info->state != PN544_ST_COLD) {
-		if (pdata->disable)
-			pdata->disable();
-
-		info->read_irq = PN544_NONE;
-	}
-
-	free_irq(client->irq, info);
-	if (pdata->free_resources)
-		pdata->free_resources();
-
-	regulator_bulk_free(ARRAY_SIZE(info->regs), info->regs);
-	kfree(info->buf);
-	kfree(info);
+	struct pn544_dev *pn544_dev;
+
+	pn544_dev = i2c_get_clientdata(client);
+	free_irq(client->irq, pn544_dev);
+	misc_deregister(&pn544_dev->pn544_device);
+	mutex_destroy(&pn544_dev->read_mutex);
+	gpio_free(pn544_dev->irq_gpio);
+	gpio_free(pn544_dev->ven_gpio);
+	if (pn544_dev->firm_gpio)
+		gpio_free(pn544_dev->firm_gpio);
+	kfree(pn544_dev);
 
 	return 0;
 }
 
+static const struct i2c_device_id pn544_id[] = {
+	{ "pn544", 0 },
+	{ }
+};
+
 static struct i2c_driver pn544_driver = {
-	.driver = {
-		.name = PN544_DRIVER_NAME,
-#ifdef CONFIG_PM
-		.pm = &pn544_pm_ops,
-#endif
+	.id_table	= pn544_id,
+	.probe		= pn544_probe,
+	.remove		= pn544_remove,
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= "pn544",
 	},
-	.probe = pn544_probe,
-	.id_table = pn544_id_table,
-	.remove = __devexit_p(pn544_remove),
 };
 
-static int __init pn544_init(void)
-{
-	int r;
-
-	pr_debug(DRIVER_DESC ": %s\n", __func__);
-
-	r = i2c_add_driver(&pn544_driver);
-	if (r) {
-		pr_err(PN544_DRIVER_NAME ": driver registration failed\n");
-		return r;
-	}
+/*
+ * module load/unload record keeping
+ */
 
-	return 0;
+static int __init pn544_dev_init(void)
+{
+	pr_info("Loading pn544 driver\n");
+	return i2c_add_driver(&pn544_driver);
 }
+module_init(pn544_dev_init);
 
-static void __exit pn544_exit(void)
+static void __exit pn544_dev_exit(void)
 {
+	pr_info("Unloading pn544 driver\n");
 	i2c_del_driver(&pn544_driver);
-	pr_info(DRIVER_DESC ", Exiting.\n");
 }
+module_exit(pn544_dev_exit);
 
-module_init(pn544_init);
-module_exit(pn544_exit);
-
+MODULE_AUTHOR("Sylvain Fonteneau");
+MODULE_DESCRIPTION("NFC PN544 driver");
 MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/oprofile/oprof.c b/drivers/oprofile/oprof.c
index dccd8636095c..f8c752e408a6 100644
--- a/drivers/oprofile/oprof.c
+++ b/drivers/oprofile/oprof.c
@@ -239,26 +239,45 @@ int oprofile_set_ulong(unsigned long *addr, unsigned long val)
 	return err;
 }
 
+static int timer_mode;
+
 static int __init oprofile_init(void)
 {
 	int err;
 
+	/* always init architecture to setup backtrace support */
 	err = oprofile_arch_init(&oprofile_ops);
-	if (err < 0 || timer) {
-		printk(KERN_INFO "oprofile: using timer interrupt.\n");
+
+	timer_mode = err || timer;	/* fall back to timer mode on errors */
+	if (timer_mode) {
+		if (!err)
+			oprofile_arch_exit();
 		err = oprofile_timer_init(&oprofile_ops);
 		if (err)
 			return err;
 	}
-	return oprofilefs_register();
+
+	err = oprofilefs_register();
+	if (!err)
+		return 0;
+
+	/* failed */
+	if (timer_mode)
+		oprofile_timer_exit();
+	else
+		oprofile_arch_exit();
+
+	return err;
 }
 
 
 static void __exit oprofile_exit(void)
 {
-	oprofile_timer_exit();
 	oprofilefs_unregister();
-	oprofile_arch_exit();
+	if (timer_mode)
+		oprofile_timer_exit();
+	else
+		oprofile_arch_exit();
 }
 
 
diff --git a/drivers/oprofile/oprofile_files.c b/drivers/oprofile/oprofile_files.c
index 89f63456646f..84a208dbed93 100644
--- a/drivers/oprofile/oprofile_files.c
+++ b/drivers/oprofile/oprofile_files.c
@@ -45,7 +45,7 @@ static ssize_t timeout_write(struct file *file, char const __user *buf,
 		return -EINVAL;
 
 	retval = oprofilefs_ulong_from_user(&val, buf, count);
-	if (retval)
+	if (retval <= 0)
 		return retval;
 
 	retval = oprofile_set_timeout(val);
@@ -84,7 +84,7 @@ static ssize_t depth_write(struct file *file, char const __user *buf, size_t cou
 		return -EINVAL;
 
 	retval = oprofilefs_ulong_from_user(&val, buf, count);
-	if (retval)
+	if (retval <= 0)
 		return retval;
 
 	retval = oprofile_set_ulong(&oprofile_backtrace_depth, val);
@@ -141,9 +141,10 @@ static ssize_t enable_write(struct file *file, char const __user *buf, size_t co
 		return -EINVAL;
 
 	retval = oprofilefs_ulong_from_user(&val, buf, count);
-	if (retval)
+	if (retval <= 0)
 		return retval;
 
+	retval = 0;
 	if (val)
 		retval = oprofile_start();
 	else
diff --git a/drivers/oprofile/oprofilefs.c b/drivers/oprofile/oprofilefs.c
index e9ff6f7770be..1c0b799b30bc 100644
--- a/drivers/oprofile/oprofilefs.c
+++ b/drivers/oprofile/oprofilefs.c
@@ -60,6 +60,13 @@ ssize_t oprofilefs_ulong_to_user(unsigned long val, char __user *buf, size_t cou
 }
 
 
+/*
+ * Note: If oprofilefs_ulong_from_user() returns 0, then *val remains
+ * unchanged and might be uninitialized. This follows write syscall
+ * implementation when count is zero: "If count is zero ... [and if]
+ * no errors are detected, 0 will be returned without causing any
+ * other effect." (man 2 write)
+ */
 int oprofilefs_ulong_from_user(unsigned long *val, char const __user *buf, size_t count)
 {
 	char tmpbuf[TMPBUFSIZE];
@@ -79,7 +86,7 @@ int oprofilefs_ulong_from_user(unsigned long *val, char const __user *buf, size_
 	spin_lock_irqsave(&oprofilefs_lock, flags);
 	*val = simple_strtoul(tmpbuf, NULL, 0);
 	spin_unlock_irqrestore(&oprofilefs_lock, flags);
-	return 0;
+	return count;
 }
 
 
@@ -99,7 +106,7 @@ static ssize_t ulong_write_file(struct file *file, char const __user *buf, size_
 		return -EINVAL;
 
 	retval = oprofilefs_ulong_from_user(&value, buf, count);
-	if (retval)
+	if (retval <= 0)
 		return retval;
 
 	retval = oprofile_set_ulong(file->private_data, value);
diff --git a/drivers/oprofile/timer_int.c b/drivers/oprofile/timer_int.c
index 3ef44624f510..878fba126582 100644
--- a/drivers/oprofile/timer_int.c
+++ b/drivers/oprofile/timer_int.c
@@ -110,6 +110,7 @@ int oprofile_timer_init(struct oprofile_operations *ops)
 	ops->start = oprofile_hrtimer_start;
 	ops->stop = oprofile_hrtimer_stop;
 	ops->cpu_type = "timer";
+	printk(KERN_INFO "oprofile: using timer interrupt.\n");
 	return 0;
 }
 
diff --git a/drivers/pci/hotplug/shpchp_core.c b/drivers/pci/hotplug/shpchp_core.c
index aca972bbfb4c..dd7e0c51a33e 100644
--- a/drivers/pci/hotplug/shpchp_core.c
+++ b/drivers/pci/hotplug/shpchp_core.c
@@ -278,8 +278,8 @@ static int get_adapter_status (struct hotplug_slot *hotplug_slot, u8 *value)
 
 static int is_shpc_capable(struct pci_dev *dev)
 {
-	if ((dev->vendor == PCI_VENDOR_ID_AMD) || (dev->device ==
-						PCI_DEVICE_ID_AMD_GOLAM_7450))
+	if (dev->vendor == PCI_VENDOR_ID_AMD &&
+	    dev->device == PCI_DEVICE_ID_AMD_GOLAM_7450)
 		return 1;
 	if (!pci_find_capability(dev, PCI_CAP_ID_SHPC))
 		return 0;
diff --git a/drivers/pci/hotplug/shpchp_hpc.c b/drivers/pci/hotplug/shpchp_hpc.c
index 36547f0ce305..75ba2311b54f 100644
--- a/drivers/pci/hotplug/shpchp_hpc.c
+++ b/drivers/pci/hotplug/shpchp_hpc.c
@@ -944,8 +944,8 @@ int shpc_init(struct controller *ctrl, struct pci_dev *pdev)
 	ctrl->pci_dev = pdev;  /* pci_dev of the P2P bridge */
 	ctrl_dbg(ctrl, "Hotplug Controller:\n");
 
-	if ((pdev->vendor == PCI_VENDOR_ID_AMD) || (pdev->device ==
-				PCI_DEVICE_ID_AMD_GOLAM_7450)) {
+	if (pdev->vendor == PCI_VENDOR_ID_AMD &&
+	    pdev->device == PCI_DEVICE_ID_AMD_GOLAM_7450) {
 		/* amd shpc driver doesn't use Base Offset; assume 0 */
 		ctrl->mmio_base = pci_resource_start(pdev, 0);
 		ctrl->mmio_size = pci_resource_len(pdev, 0);
diff --git a/drivers/pci/msi.c b/drivers/pci/msi.c
index 2f10328bf661..e1749825008d 100644
--- a/drivers/pci/msi.c
+++ b/drivers/pci/msi.c
@@ -869,5 +869,15 @@ EXPORT_SYMBOL(pci_msi_enabled);
 
 void pci_msi_init_pci_dev(struct pci_dev *dev)
 {
+	int pos;
 	INIT_LIST_HEAD(&dev->msi_list);
+
+	/* Disable the msi hardware to avoid screaming interrupts
+	 * during boot.  This is the power on reset default so
+	 * usually this should be a noop.
+	 */
+	pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
+	if (pos)
+		msi_set_enable(dev, pos, 0);
+	msix_set_enable(dev, 0);
 }
diff --git a/drivers/pci/pcie/Kconfig b/drivers/pci/pcie/Kconfig
index dc29348264c6..c3706246a3b7 100644
--- a/drivers/pci/pcie/Kconfig
+++ b/drivers/pci/pcie/Kconfig
@@ -55,6 +55,31 @@ config PCIEASPM_DEBUG
 	  This enables PCI Express ASPM debug support. It will add per-device
 	  interface to control ASPM.
 
+choice
+	prompt "Default ASPM policy"
+	default PCIEASPM_DEFAULT
+	depends on PCIEASPM
+
+config PCIEASPM_DEFAULT
+	bool "BIOS default"
+	depends on PCIEASPM
+	help
+	  Use the BIOS defaults for PCI Express ASPM.
+
+config PCIEASPM_POWERSAVE
+	bool "Powersave"
+	depends on PCIEASPM
+	help
+	  Enable PCI Express ASPM L0s and L1 where possible, even if the
+	  BIOS did not.
+
+config PCIEASPM_PERFORMANCE
+	bool "Performance"
+	depends on PCIEASPM
+	help
+	  Disable PCI Express ASPM L0s and L1, even if the BIOS enabled them.
+endchoice
+
 config PCIE_PME
 	def_bool y
 	depends on PCIEPORTBUS && PM_RUNTIME && EXPERIMENTAL && ACPI
diff --git a/drivers/pci/pcie/aspm.c b/drivers/pci/pcie/aspm.c
index cbfbab18be91..d65b56893506 100644
--- a/drivers/pci/pcie/aspm.c
+++ b/drivers/pci/pcie/aspm.c
@@ -76,7 +76,15 @@ static LIST_HEAD(link_list);
 #define POLICY_DEFAULT 0	/* BIOS default setting */
 #define POLICY_PERFORMANCE 1	/* high performance */
 #define POLICY_POWERSAVE 2	/* high power saving */
+
+#ifdef CONFIG_PCIEASPM_PERFORMANCE
+static int aspm_policy = POLICY_PERFORMANCE;
+#elif defined CONFIG_PCIEASPM_POWERSAVE
+static int aspm_policy = POLICY_POWERSAVE;
+#else
 static int aspm_policy;
+#endif
+
 static const char *policy_str[] = {
 	[POLICY_DEFAULT] = "default",
 	[POLICY_PERFORMANCE] = "performance",
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index 1196f61a4ab6..37d35c938b5a 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -2745,20 +2745,6 @@ static void ricoh_mmc_fixup_r5c832(struct pci_dev *dev)
 	/* disable must be done via function #0 */
 	if (PCI_FUNC(dev->devfn))
 		return;
-
-	pci_read_config_byte(dev, 0xCB, &disable);
-
-	if (disable & 0x02)
-		return;
-
-	pci_read_config_byte(dev, 0xCA, &write_enable);
-	pci_write_config_byte(dev, 0xCA, 0x57);
-	pci_write_config_byte(dev, 0xCB, disable | 0x02);
-	pci_write_config_byte(dev, 0xCA, write_enable);
-
-	dev_notice(&dev->dev, "proprietary Ricoh MMC controller disabled (via firewire function)\n");
-	dev_notice(&dev->dev, "MMC cards are now supported by standard SDHCI controller\n");
-
 	/*
 	 * RICOH 0xe823 SD/MMC card reader fails to recognize
 	 * certain types of SD/MMC cards. Lowering the SD base
@@ -2781,6 +2767,20 @@ static void ricoh_mmc_fixup_r5c832(struct pci_dev *dev)
 
 		dev_notice(&dev->dev, "MMC controller base frequency changed to 50Mhz.\n");
 	}
+
+	pci_read_config_byte(dev, 0xCB, &disable);
+
+	if (disable & 0x02)
+		return;
+
+	pci_read_config_byte(dev, 0xCA, &write_enable);
+	pci_write_config_byte(dev, 0xCA, 0x57);
+	pci_write_config_byte(dev, 0xCB, disable | 0x02);
+	pci_write_config_byte(dev, 0xCA, write_enable);
+
+	dev_notice(&dev->dev, "proprietary Ricoh MMC controller disabled (via firewire function)\n");
+	dev_notice(&dev->dev, "MMC cards are now supported by standard SDHCI controller\n");
+
 }
 DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_RICOH, PCI_DEVICE_ID_RICOH_R5C832, ricoh_mmc_fixup_r5c832);
 DECLARE_PCI_FIXUP_RESUME_EARLY(PCI_VENDOR_ID_RICOH, PCI_DEVICE_ID_RICOH_R5C832, ricoh_mmc_fixup_r5c832);
@@ -2788,7 +2788,7 @@ DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_RICOH, PCI_DEVICE_ID_RICOH_R5CE823, ricoh_
 DECLARE_PCI_FIXUP_RESUME_EARLY(PCI_VENDOR_ID_RICOH, PCI_DEVICE_ID_RICOH_R5CE823, ricoh_mmc_fixup_r5c832);
 #endif /*CONFIG_MMC_RICOH_MMC*/
 
-#if defined(CONFIG_DMAR) || defined(CONFIG_INTR_REMAP)
+#ifdef CONFIG_DMAR_TABLE
 #define VTUNCERRMSK_REG	0x1ac
 #define VTD_MSK_SPEC_ERRORS	(1 << 31)
 /*
diff --git a/drivers/pci/xen-pcifront.c b/drivers/pci/xen-pcifront.c
index 6fa215a38615..90832a955991 100644
--- a/drivers/pci/xen-pcifront.c
+++ b/drivers/pci/xen-pcifront.c
@@ -400,9 +400,8 @@ static int pcifront_claim_resource(struct pci_dev *dev, void *data)
 			dev_info(&pdev->xdev->dev, "claiming resource %s/%d\n",
 				pci_name(dev), i);
 			if (pci_claim_resource(dev, i)) {
-				dev_err(&pdev->xdev->dev, "Could not claim "
-					"resource %s/%d! Device offline. Try "
-					"giving less than 4GB to domain.\n",
+				dev_err(&pdev->xdev->dev, "Could not claim resource %s/%d! "
+					"Device offline. Try using e820_host=1 in the guest config.\n",
 					pci_name(dev), i);
 			}
 		}
diff --git a/drivers/platform/x86/ideapad-laptop.c b/drivers/platform/x86/ideapad-laptop.c
index 0c595410e788..0d94eec00f4d 100644
--- a/drivers/platform/x86/ideapad-laptop.c
+++ b/drivers/platform/x86/ideapad-laptop.c
@@ -493,6 +493,8 @@ static void ideapad_backlight_notify_power(struct ideapad_private *priv)
 	unsigned long power;
 	struct backlight_device *blightdev = priv->blightdev;
 
+	if (!blightdev)
+		return;
 	if (read_ec_data(ideapad_handle, 0x18, &power))
 		return;
 	blightdev->props.power = power ? FB_BLANK_UNBLANK : FB_BLANK_POWERDOWN;
diff --git a/drivers/platform/x86/samsung-laptop.c b/drivers/platform/x86/samsung-laptop.c
index 359163011044..59ac26c6a2ad 100644
--- a/drivers/platform/x86/samsung-laptop.c
+++ b/drivers/platform/x86/samsung-laptop.c
@@ -370,15 +370,17 @@ static u8 read_brightness(void)
 				  &sretval);
 	if (!retval) {
 		user_brightness = sretval.retval[0];
-		if (user_brightness != 0)
+		if (user_brightness > sabi_config->min_brightness)
 			user_brightness -= sabi_config->min_brightness;
+		else
+			user_brightness = 0;
 	}
 	return user_brightness;
 }
 
 static void set_brightness(u8 user_brightness)
 {
-	u8 user_level = user_brightness - sabi_config->min_brightness;
+	u8 user_level = user_brightness + sabi_config->min_brightness;
 
 	sabi_set_command(sabi_config->commands.set_brightness, user_level);
 }
@@ -641,6 +643,15 @@ static struct dmi_system_id __initdata samsung_dmi_table[] = {
 		.callback = dmi_check_cb,
 	},
 	{
+		.ident = "R700",
+		.matches = {
+		      DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
+		      DMI_MATCH(DMI_PRODUCT_NAME, "SR700"),
+		      DMI_MATCH(DMI_BOARD_NAME, "SR700"),
+		},
+		.callback = dmi_check_cb,
+	},
+	{
 		.ident = "R530/R730",
 		.matches = {
 		      DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
@@ -686,6 +697,24 @@ static struct dmi_system_id __initdata samsung_dmi_table[] = {
 		},
 		.callback = dmi_check_cb,
 	},
+		{
+		.ident = "X520",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X520"),
+			DMI_MATCH(DMI_BOARD_NAME, "X520"),
+		},
+		.callback = dmi_check_cb,
+	},
+	{
+		.ident = "R528/R728",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "R528/R728"),
+			DMI_MATCH(DMI_BOARD_NAME, "R528/R728"),
+		},
+		.callback = dmi_check_cb,
+	},
 	{ },
 };
 MODULE_DEVICE_TABLE(dmi, samsung_dmi_table);
@@ -770,7 +799,7 @@ static int __init samsung_init(void)
 	sabi_iface = ioremap_nocache(ifaceP, 16);
 	if (!sabi_iface) {
 		pr_err("Can't remap %x\n", ifaceP);
-		goto exit;
+		goto error_no_signature;
 	}
 	if (debug) {
 		printk(KERN_DEBUG "ifaceP = 0x%08x\n", ifaceP);
@@ -802,7 +831,8 @@ static int __init samsung_init(void)
 	/* create a backlight device to talk to this one */
 	memset(&props, 0, sizeof(struct backlight_properties));
 	props.type = BACKLIGHT_PLATFORM;
-	props.max_brightness = sabi_config->max_brightness;
+	props.max_brightness = sabi_config->max_brightness -
+				sabi_config->min_brightness;
 	backlight_device = backlight_device_register("samsung", &sdev->dev,
 						     NULL, &backlight_ops,
 						     &props);
@@ -821,7 +851,6 @@ static int __init samsung_init(void)
 	if (retval)
 		goto error_file_create;
 
-exit:
 	return 0;
 
 error_file_create:
diff --git a/drivers/platform/x86/wmi.c b/drivers/platform/x86/wmi.c
index f23d5a84e7b1..9b88be42b6cd 100644
--- a/drivers/platform/x86/wmi.c
+++ b/drivers/platform/x86/wmi.c
@@ -754,9 +754,13 @@ static void wmi_free_devices(void)
 	struct wmi_block *wblock, *next;
 
 	/* Delete devices for all the GUIDs */
-	list_for_each_entry_safe(wblock, next, &wmi_block_list, list)
+	list_for_each_entry_safe(wblock, next, &wmi_block_list, list) {
+		list_del(&wblock->list);
 		if (wblock->dev.class)
 			device_unregister(&wblock->dev);
+		else
+			kfree(wblock);
+	}
 }
 
 static bool guid_already_parsed(const char *guid_string)
diff --git a/drivers/pnp/quirks.c b/drivers/pnp/quirks.c
index dfbd5a6cc58b..258fef272ea7 100644
--- a/drivers/pnp/quirks.c
+++ b/drivers/pnp/quirks.c
@@ -295,6 +295,45 @@ static void quirk_system_pci_resources(struct pnp_dev *dev)
 	}
 }
 
+#ifdef CONFIG_AMD_NB
+
+#include <asm/amd_nb.h>
+
+static void quirk_amd_mmconfig_area(struct pnp_dev *dev)
+{
+	resource_size_t start, end;
+	struct pnp_resource *pnp_res;
+	struct resource *res;
+	struct resource mmconfig_res, *mmconfig;
+
+	mmconfig = amd_get_mmconfig_range(&mmconfig_res);
+	if (!mmconfig)
+		return;
+
+	list_for_each_entry(pnp_res, &dev->resources, list) {
+		res = &pnp_res->res;
+		if (res->end < mmconfig->start || res->start > mmconfig->end ||
+		    (res->start == mmconfig->start && res->end == mmconfig->end))
+			continue;
+
+		dev_info(&dev->dev, FW_BUG
+			 "%pR covers only part of AMD MMCONFIG area %pR; adding more reservations\n",
+			 res, mmconfig);
+		if (mmconfig->start < res->start) {
+			start = mmconfig->start;
+			end = res->start - 1;
+			pnp_add_mem_resource(dev, start, end, 0);
+		}
+		if (mmconfig->end > res->end) {
+			start = res->end + 1;
+			end = mmconfig->end;
+			pnp_add_mem_resource(dev, start, end, 0);
+		}
+		break;
+	}
+}
+#endif
+
 /*
  *  PnP Quirks
  *  Cards or devices that need some tweaking due to incomplete resource info
@@ -322,6 +361,9 @@ static struct pnp_fixup pnp_fixups[] = {
 	/* PnP resources that might overlap PCI BARs */
 	{"PNP0c01", quirk_system_pci_resources},
 	{"PNP0c02", quirk_system_pci_resources},
+#ifdef CONFIG_AMD_NB
+	{"PNP0c01", quirk_amd_mmconfig_area},
+#endif
 	{""}
 };
 
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index 57de051a74b3..cf6d5ec23c45 100644
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -36,6 +36,13 @@ config MAX8925_POWER
 	  Say Y here to enable support for the battery charger in the Maxim
 	  MAX8925 PMIC.
 
+config MAX8907C_CHARGER
+	tristate "MAX8907c charger support"
+	depends on MFD_MAX8907C
+	help
+	  Say Y here to enable support for the charger in the Maxim
+	  MAX8907c PMIC.
+
 config WM831X_BACKUP
 	tristate "WM831X backup battery charger support"
 	depends on MFD_WM831X
@@ -142,6 +149,26 @@ config BATTERY_BQ27X00_PLATFORM
 	help
 	  Say Y here to enable support for batteries with BQ27000 (HDQ) chips.
 
+config CHARGER_TPS8003X
+	tristate "TPS8003x battery charger driver"
+	depends on MFD_TPS80031
+	default n
+	help
+	  Say Y here to enable support for battery charging with TPS80031x chips.
+
+config BATTERY_GAUGE_TPS8003X
+	tristate "TPS8003x battery gauge driver"
+	depends on MFD_TPS80031
+	default n
+	help
+	 Say Y here to enable support for battery gauge for TPS80031x chips.
+
+config CHARGER_SMB349
+	tristate "SMB349 battery charger driver"
+	depends on I2C
+	help
+	  Say Y here to enable support for battery charging with SMB349 chips.
+
 config BATTERY_DA9030
 	tristate "DA9030 battery driver"
 	depends on PMIC_DA903X
@@ -167,6 +194,15 @@ config BATTERY_MAX17042
 	  multi-function devices that include fuel gauages that are compatible
 	  with MAX17042.
 
+config BATTERY_MAX17048
+	tristate "Maxim MAX17048/17049 Fuel Gauge"
+	depends on I2C
+	help
+	  MAX17048 is fuel-gauge systems for lithium-ion (Li+) batteries
+	  in handheld and portable equipment. The MAX17048 is configured
+	  to operate with a single lithium cell, and MAX17049 for two lithium
+	  cells.
+
 config BATTERY_Z2
 	tristate "Z2 battery driver"
 	depends on I2C && MACH_ZIPIT2
@@ -250,3 +286,10 @@ config CHARGER_MAX8998
 	  platform data of MAX8998/LP3974 PMICs.
 
 endif # POWER_SUPPLY
+
+config TEGRA_BPC_MGMT
+	tristate "battery peak current management"
+	depends on ARCH_TEGRA
+	help
+	  This driver reduces cpu frequency/voltage on gpio event from the
+	  battery current monitor device.
diff --git a/drivers/power/Makefile b/drivers/power/Makefile
index b4af13dd8b66..35dfc506b984 100644
--- a/drivers/power/Makefile
+++ b/drivers/power/Makefile
@@ -24,9 +24,13 @@ obj-$(CONFIG_BATTERY_COLLIE)	+= collie_battery.o
 obj-$(CONFIG_BATTERY_WM97XX)	+= wm97xx_battery.o
 obj-$(CONFIG_BATTERY_BQ20Z75)	+= bq20z75.o
 obj-$(CONFIG_BATTERY_BQ27x00)	+= bq27x00_battery.o
+obj-$(CONFIG_CHARGER_TPS8003X)	+= tps80031-charger.o
+obj-$(CONFIG_BATTERY_GAUGE_TPS8003X) += tps80031_battery_gauge.o
+obj-$(CONFIG_CHARGER_SMB349)	+= smb349-charger.o
 obj-$(CONFIG_BATTERY_DA9030)	+= da9030_battery.o
 obj-$(CONFIG_BATTERY_MAX17040)	+= max17040_battery.o
 obj-$(CONFIG_BATTERY_MAX17042)	+= max17042_battery.o
+obj-$(CONFIG_BATTERY_MAX17048)	+= max17048_battery.o
 obj-$(CONFIG_BATTERY_Z2)	+= z2_battery.o
 obj-$(CONFIG_BATTERY_S3C_ADC)	+= s3c_adc_battery.o
 obj-$(CONFIG_CHARGER_PCF50633)	+= pcf50633-charger.o
@@ -38,3 +42,6 @@ obj-$(CONFIG_CHARGER_TWL4030)	+= twl4030_charger.o
 obj-$(CONFIG_CHARGER_GPIO)	+= gpio-charger.o
 obj-$(CONFIG_CHARGER_MAX8997)	+= max8997_charger.o
 obj-$(CONFIG_CHARGER_MAX8998)	+= max8998_charger.o
+obj-$(CONFIG_MAX8907C_CHARGER)	+= max8907c-charger.o
+CFLAGS_tegra_bpc_mgmt.o		= -Werror
+obj-$(CONFIG_TEGRA_BPC_MGMT)	+= tegra_bpc_mgmt.o
diff --git a/drivers/power/bq20z75.c b/drivers/power/bq20z75.c
index 9c5e5beda3a8..32e5a8c5c3df 100644
--- a/drivers/power/bq20z75.c
+++ b/drivers/power/bq20z75.c
@@ -147,7 +147,7 @@ static enum power_supply_property bq20z75_properties[] = {
 struct bq20z75_info {
 	struct i2c_client		*client;
 	struct power_supply		power_supply;
-	struct bq20z75_platform_data	*pdata;
+	struct bq20z75_platform_data	plat_data;
 	bool				is_present;
 	bool				gpio_detect;
 	bool				enable_detection;
@@ -164,8 +164,7 @@ static int bq20z75_read_word_data(struct i2c_client *client, u8 address)
 	s32 ret = 0;
 	int retries = 1;
 
-	if (bq20z75_device->pdata)
-		retries = max(bq20z75_device->pdata->i2c_retry_count + 1, 1);
+	retries = max(bq20z75_device->plat_data.i2c_retry_count + 1, 1);
 
 	while (retries > 0) {
 		ret = i2c_smbus_read_word_data(client, address);
@@ -191,8 +190,7 @@ static int bq20z75_write_word_data(struct i2c_client *client, u8 address,
 	s32 ret = 0;
 	int retries = 1;
 
-	if (bq20z75_device->pdata)
-		retries = max(bq20z75_device->pdata->i2c_retry_count + 1, 1);
+	retries = max(bq20z75_device->plat_data.i2c_retry_count + 1, 1);
 
 	while (retries > 0) {
 		ret = i2c_smbus_write_word_data(client, address,
@@ -222,8 +220,8 @@ static int bq20z75_get_battery_presence_and_health(
 	if (psp == POWER_SUPPLY_PROP_PRESENT &&
 		bq20z75_device->gpio_detect) {
 		ret = gpio_get_value(
-			bq20z75_device->pdata->battery_detect);
-		if (ret == bq20z75_device->pdata->battery_detect_present)
+			bq20z75_device->plat_data.battery_detect);
+		if (ret == bq20z75_device->plat_data.battery_detect_present)
 			val->intval = 1;
 		else
 			val->intval = 0;
@@ -574,7 +572,7 @@ static void bq20z75_external_power_changed(struct power_supply *psy)
 	cancel_delayed_work_sync(&bq20z75_device->work);
 
 	schedule_delayed_work(&bq20z75_device->work, HZ);
-	bq20z75_device->poll_time = bq20z75_device->pdata->poll_retry_count;
+	bq20z75_device->poll_time = bq20z75_device->plat_data.poll_retry_count;
 }
 
 static void bq20z75_delayed_work(struct work_struct *work)
@@ -645,11 +643,22 @@ static int __devinit bq20z75_probe(struct i2c_client *client,
 	if (pdata) {
 		bq20z75_device->gpio_detect =
 			gpio_is_valid(pdata->battery_detect);
-		bq20z75_device->pdata = pdata;
+		memcpy(&bq20z75_device->plat_data, pdata, sizeof(struct bq20z75_platform_data));
 	}
 
 	i2c_set_clientdata(client, bq20z75_device);
 
+	/* Probing for the presence of the bq20z75 */
+	rc = bq20z75_read_word_data(client,
+		bq20z75_data[REG_SERIAL_NUMBER].addr);
+
+	if (rc < 0) {
+		dev_err(&client->dev,
+			"%s: bq20z75 is not responding\n", __func__);
+		rc = -ENODEV;
+		goto exit_mem_free;
+	}
+
 	if (!bq20z75_device->gpio_detect)
 		goto skip_gpio;
 
@@ -689,7 +698,6 @@ static int __devinit bq20z75_probe(struct i2c_client *client,
 	bq20z75_device->irq = irq;
 
 skip_gpio:
-
 	rc = power_supply_register(&client->dev, &bq20z75_device->power_supply);
 	if (rc) {
 		dev_err(&client->dev,
@@ -712,6 +720,7 @@ exit_psupply:
 	if (bq20z75_device->gpio_detect)
 		gpio_free(pdata->battery_detect);
 
+exit_mem_free:
 	kfree(bq20z75_device);
 
 	return rc;
@@ -724,7 +733,7 @@ static int __devexit bq20z75_remove(struct i2c_client *client)
 	if (bq20z75_device->irq)
 		free_irq(bq20z75_device->irq, &bq20z75_device->power_supply);
 	if (bq20z75_device->gpio_detect)
-		gpio_free(bq20z75_device->pdata->battery_detect);
+		gpio_free(bq20z75_device->plat_data.battery_detect);
 
 	power_supply_unregister(&bq20z75_device->power_supply);
 
@@ -755,11 +764,18 @@ static int bq20z75_suspend(struct i2c_client *client,
 
 	return 0;
 }
+
+static int bq20z75_resume(struct i2c_client *client)
+{
+	struct bq20z75_info *bq20z75_device = i2c_get_clientdata(client);
+
+	schedule_delayed_work(&bq20z75_device->work, HZ);
+	return 0;
+}
 #else
 #define bq20z75_suspend		NULL
-#endif
-/* any smbus transaction will wake up bq20z75 */
 #define bq20z75_resume		NULL
+#endif
 
 static const struct i2c_device_id bq20z75_id[] = {
 	{ "bq20z75", 0 },
diff --git a/drivers/power/bq27x00_battery.c b/drivers/power/bq27x00_battery.c
index bb16f5b7e167..857deb8faa0a 100644
--- a/drivers/power/bq27x00_battery.c
+++ b/drivers/power/bq27x00_battery.c
@@ -5,6 +5,7 @@
  * Copyright (C) 2008 Eurotech S.p.A. <info@eurotech.it>
  * Copyright (C) 2010-2011 Lars-Peter Clausen <lars@metafoo.de>
  * Copyright (C) 2011 Pali Rohár <pali.rohar@gmail.com>
+ * Copyright (C) 2011 NVIDIA Corporation.
  *
  * Based on a previous work by Copyright (C) 2008 Texas Instruments, Inc.
  *
@@ -60,16 +61,36 @@
 #define BQ27500_REG_SOC			0x2C
 #define BQ27500_REG_DCAP		0x3C /* Design capacity */
 #define BQ27500_FLAG_DSC		BIT(0)
+#define BQ27500_FLAG_SOCF		BIT(1)
+#define BQ27500_FLAG_BAT_DET		BIT(3)
 #define BQ27500_FLAG_FC			BIT(9)
+#define BQ27500_FLAG_OTC		BIT(15)
 
 #define BQ27000_RS			20 /* Resistor sense */
 
+#define BQ27510_CNTL			0x00
+#define BQ27510_ATRATE			0x02
+#define BQ27510_ENERGY_AVAIL		0x22
+#define BQ27510_POWER_AVG		0x24
+
+/* bq27510-g2 control register sub-commands*/
+#define BQ27510_CNTL_DEVICE_TYPE	0x0001
+#define BQ27510_CNTL_SET_SLEEP		0x0013
+#define BQ27510_CNTL_CLEAR_SLEEP	0x0014
+
+/* bq27x00 requires 3 to 4 second to update charging status */
+#define CHARGING_STATUS_UPDATE_DELAY_SECS	4
+
 struct bq27x00_device_info;
 struct bq27x00_access_methods {
 	int (*read)(struct bq27x00_device_info *di, u8 reg, bool single);
+	int (*ctrl_read)(struct bq27x00_device_info *di, u8 ctrl_reg,
+				u16 ctrl_func_reg);
+	int (*write)(struct bq27x00_device_info *di, u8 reg, u16 val,
+				bool single);
 };
 
-enum bq27x00_chip { BQ27000, BQ27500 };
+enum bq27x00_chip { BQ27000, BQ27500, BQ27510 };
 
 struct bq27x00_reg_cache {
 	int temperature;
@@ -94,12 +115,14 @@ struct bq27x00_device_info {
 
 	unsigned long last_update;
 	struct delayed_work work;
+	struct delayed_work external_power_changed_work;
 
 	struct power_supply	bat;
 
 	struct bq27x00_access_methods bus;
 
 	struct mutex lock;
+	struct mutex update_lock;
 };
 
 static enum power_supply_property bq27x00_battery_props[] = {
@@ -118,6 +141,9 @@ static enum power_supply_property bq27x00_battery_props[] = {
 	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
 	POWER_SUPPLY_PROP_CYCLE_COUNT,
 	POWER_SUPPLY_PROP_ENERGY_NOW,
+	POWER_SUPPLY_PROP_POWER_AVG,
+	POWER_SUPPLY_PROP_SERIAL_NUMBER,
+	POWER_SUPPLY_PROP_HEALTH,
 };
 
 static unsigned int poll_interval = 360;
@@ -135,6 +161,43 @@ static inline int bq27x00_read(struct bq27x00_device_info *di, u8 reg,
 	return di->bus.read(di, reg, single);
 }
 
+static inline int bq27x00_ctrl_read(struct bq27x00_device_info *di,
+					u8 ctrl_reg, u16 ctrl_func_reg)
+{
+	return di->bus.ctrl_read(di, ctrl_reg, ctrl_func_reg);
+}
+
+static inline int bq27x00_write(struct bq27x00_device_info *di, u8 reg,
+		u16 val, bool single)
+{
+	return di->bus.write(di, reg, val, single);
+}
+
+static int bq27510_battery_health(struct bq27x00_device_info *di,
+				union power_supply_propval *val)
+{
+	int ret;
+
+	if ((di->chip == BQ27500) || (di->chip == BQ27510)) {
+		ret = bq27x00_read(di, BQ27x00_REG_FLAGS, false);
+		if (ret < 0) {
+			dev_err(di->dev, "read failure\n");
+			return ret;
+		}
+
+		if (ret & BQ27500_FLAG_SOCF)
+			val->intval = POWER_SUPPLY_HEALTH_DEAD;
+		else if (ret & BQ27500_FLAG_OTC)
+			val->intval = POWER_SUPPLY_HEALTH_OVERHEAT;
+		else
+			val->intval = POWER_SUPPLY_HEALTH_GOOD;
+
+		return 0;
+	}
+
+	return -1;
+}
+
 /*
  * Return the battery Relative State-of-Charge
  * Or < 0 if something fails.
@@ -143,7 +206,7 @@ static int bq27x00_battery_read_rsoc(struct bq27x00_device_info *di)
 {
 	int rsoc;
 
-	if (di->chip == BQ27500)
+	if ((di->chip == BQ27500) || (di->chip == BQ27510))
 		rsoc = bq27x00_read(di, BQ27500_REG_SOC, false);
 	else
 		rsoc = bq27x00_read(di, BQ27000_REG_RSOC, true);
@@ -168,7 +231,7 @@ static int bq27x00_battery_read_charge(struct bq27x00_device_info *di, u8 reg)
 		return charge;
 	}
 
-	if (di->chip == BQ27500)
+	if ((di->chip == BQ27500) || (di->chip == BQ27510))
 		charge *= 1000;
 	else
 		charge = charge * 3570 / BQ27000_RS;
@@ -202,7 +265,7 @@ static int bq27x00_battery_read_ilmd(struct bq27x00_device_info *di)
 {
 	int ilmd;
 
-	if (di->chip == BQ27500)
+	if ((di->chip == BQ27500) || (di->chip == BQ27510))
 		ilmd = bq27x00_read(di, BQ27500_REG_DCAP, false);
 	else
 		ilmd = bq27x00_read(di, BQ27000_REG_ILMD, true);
@@ -212,7 +275,7 @@ static int bq27x00_battery_read_ilmd(struct bq27x00_device_info *di)
 		return ilmd;
 	}
 
-	if (di->chip == BQ27500)
+	if ((di->chip == BQ27500) || (di->chip == BQ27510))
 		ilmd *= 1000;
 	else
 		ilmd = ilmd * 256 * 3570 / BQ27000_RS;
@@ -258,8 +321,9 @@ static int bq27x00_battery_read_time(struct bq27x00_device_info *di, u8 reg)
 static void bq27x00_update(struct bq27x00_device_info *di)
 {
 	struct bq27x00_reg_cache cache = {0, };
-	bool is_bq27500 = di->chip == BQ27500;
+	bool is_bq27500 = (di->chip == BQ27500 || di->chip == BQ27510);
 
+	mutex_lock(&di->update_lock);
 	cache.flags = bq27x00_read(di, BQ27x00_REG_FLAGS, is_bq27500);
 	if (cache.flags >= 0) {
 		cache.capacity = bq27x00_battery_read_rsoc(di);
@@ -286,6 +350,7 @@ static void bq27x00_update(struct bq27x00_device_info *di)
 	}
 
 	di->last_update = jiffies;
+	mutex_unlock(&di->update_lock);
 }
 
 static void bq27x00_battery_poll(struct work_struct *work)
@@ -302,6 +367,14 @@ static void bq27x00_battery_poll(struct work_struct *work)
 	}
 }
 
+static void bq27x00_external_power_changed_work(struct work_struct *work)
+{
+	struct bq27x00_device_info *di =
+		container_of(work, struct bq27x00_device_info,
+				external_power_changed_work.work);
+
+	bq27x00_update(di);
+}
 
 /*
  * Return the battery temperature in tenths of degree Celsius
@@ -313,7 +386,7 @@ static int bq27x00_battery_temperature(struct bq27x00_device_info *di,
 	if (di->cache.temperature < 0)
 		return di->cache.temperature;
 
-	if (di->chip == BQ27500)
+	if ((di->chip == BQ27500) || (di->chip == BQ27510))
 		val->intval = di->cache.temperature - 2731;
 	else
 		val->intval = ((di->cache.temperature * 5) - 5463) / 2;
@@ -331,15 +404,15 @@ static int bq27x00_battery_current(struct bq27x00_device_info *di,
 {
 	int curr;
 
-	if (di->chip == BQ27500)
-	    curr = bq27x00_read(di, BQ27x00_REG_AI, false);
+	if ((di->chip == BQ27500) || (di->chip == BQ27510))
+		curr = bq27x00_read(di, BQ27x00_REG_AI, false);
 	else
-	    curr = di->cache.current_now;
+		curr = di->cache.current_now;
 
 	if (curr < 0)
 		return curr;
 
-	if (di->chip == BQ27500) {
+	if ((di->chip == BQ27500) || (di->chip == BQ27510)) {
 		/* bq27500 returns signed value */
 		val->intval = (int)((s16)curr) * 1000;
 	} else {
@@ -359,7 +432,7 @@ static int bq27x00_battery_status(struct bq27x00_device_info *di,
 {
 	int status;
 
-	if (di->chip == BQ27500) {
+	if ((di->chip == BQ27500) || (di->chip == BQ27510)) {
 		if (di->cache.flags & BQ27500_FLAG_FC)
 			status = POWER_SUPPLY_STATUS_FULL;
 		else if (di->cache.flags & BQ27500_FLAG_DSC)
@@ -415,7 +488,7 @@ static int bq27x00_battery_energy(struct bq27x00_device_info *di,
 		return ae;
 	}
 
-	if (di->chip == BQ27500)
+	if ((di->chip == BQ27500) || (di->chip == BQ27510))
 		ae *= 1000;
 	else
 		ae = ae * 29200 / BQ27000_RS;
@@ -437,6 +510,58 @@ static int bq27x00_simple_value(int value,
 	return 0;
 }
 
+static int bq27510_battery_present(struct bq27x00_device_info *di,
+					union power_supply_propval *val)
+{
+	int ret;
+
+	ret = bq27x00_read(di, BQ27x00_REG_FLAGS, false);
+	if (ret < 0) {
+		dev_err(di->dev, "error reading flags\n");
+		return ret;
+	}
+
+	if (ret & BQ27500_FLAG_BAT_DET)
+		val->intval = 1;
+	else
+		val->intval = 0;
+
+	return 0;
+}
+
+static char bq27510_serial[5];
+static int bq27510_get_battery_serial_number(struct bq27x00_device_info *di,
+					union power_supply_propval *val)
+{
+	int ret;
+
+	if (di->chip == BQ27510) {
+		ret = bq27x00_ctrl_read(di, BQ27510_CNTL,
+					BQ27510_CNTL_DEVICE_TYPE);
+		ret = sprintf(bq27510_serial, "%04x", ret);
+		val->strval = bq27510_serial;
+		return 0;
+	} else {
+		return 1;
+	}
+}
+
+static int bq27510_battery_power_avg(struct bq27x00_device_info *di,
+					union power_supply_propval *val)
+{
+	int ret;
+	if (di->chip == BQ27510) {
+		ret = bq27x00_read(di, BQ27510_POWER_AVG, false);
+		if (ret < 0) {
+			dev_err(di->dev, "read failure\n");
+			return ret;
+		}
+		val->intval = ret;
+		return 0;
+	}
+	return -1;
+}
+
 #define to_bq27x00_device_info(x) container_of((x), \
 				struct bq27x00_device_info, bat);
 
@@ -465,7 +590,7 @@ static int bq27x00_battery_get_property(struct power_supply *psy,
 		ret = bq27x00_battery_voltage(di, val);
 		break;
 	case POWER_SUPPLY_PROP_PRESENT:
-		val->intval = di->cache.flags < 0 ? 0 : 1;
+		ret = bq27510_battery_present(di, val);
 		break;
 	case POWER_SUPPLY_PROP_CURRENT_NOW:
 		ret = bq27x00_battery_current(di, val);
@@ -503,6 +628,16 @@ static int bq27x00_battery_get_property(struct power_supply *psy,
 	case POWER_SUPPLY_PROP_ENERGY_NOW:
 		ret = bq27x00_battery_energy(di, val);
 		break;
+	case POWER_SUPPLY_PROP_POWER_AVG:
+		ret = bq27510_battery_power_avg(di, val);
+		break;
+	case POWER_SUPPLY_PROP_SERIAL_NUMBER:
+		if (bq27510_get_battery_serial_number(di, val))
+			return -EINVAL;
+		break;
+	case POWER_SUPPLY_PROP_HEALTH:
+		ret = bq27510_battery_health(di, val);
+		break;
 	default:
 		return -EINVAL;
 	}
@@ -510,12 +645,19 @@ static int bq27x00_battery_get_property(struct power_supply *psy,
 	return ret;
 }
 
+static unsigned int charging_update_delay_secs =
+				CHARGING_STATUS_UPDATE_DELAY_SECS;
+module_param(charging_update_delay_secs, uint, 0644);
+MODULE_PARM_DESC(charging_update_delay_secs, "battery charging " \
+			"status update delay in seconds");
+
 static void bq27x00_external_power_changed(struct power_supply *psy)
 {
 	struct bq27x00_device_info *di = to_bq27x00_device_info(psy);
 
-	cancel_delayed_work_sync(&di->work);
-	schedule_delayed_work(&di->work, 0);
+	cancel_delayed_work_sync(&di->external_power_changed_work);
+	schedule_delayed_work(&di->external_power_changed_work,
+		charging_update_delay_secs * HZ);
 }
 
 static int bq27x00_powersupply_init(struct bq27x00_device_info *di)
@@ -529,7 +671,10 @@ static int bq27x00_powersupply_init(struct bq27x00_device_info *di)
 	di->bat.external_power_changed = bq27x00_external_power_changed;
 
 	INIT_DELAYED_WORK(&di->work, bq27x00_battery_poll);
+	INIT_DELAYED_WORK(&di->external_power_changed_work,
+				bq27x00_external_power_changed_work);
 	mutex_init(&di->lock);
+	mutex_init(&di->update_lock);
 
 	ret = power_supply_register(di->dev, &di->bat);
 	if (ret) {
@@ -547,10 +692,11 @@ static int bq27x00_powersupply_init(struct bq27x00_device_info *di)
 static void bq27x00_powersupply_unregister(struct bq27x00_device_info *di)
 {
 	cancel_delayed_work_sync(&di->work);
-
+	cancel_delayed_work_sync(&di->external_power_changed_work);
 	power_supply_unregister(&di->bat);
 
 	mutex_destroy(&di->lock);
+	mutex_destroy(&di->update_lock);
 }
 
 
@@ -597,12 +743,55 @@ static int bq27x00_read_i2c(struct bq27x00_device_info *di, u8 reg, bool single)
 	return ret;
 }
 
+static int bq27x00_write_i2c(struct bq27x00_device_info *di, u8 reg,
+				u16 val, bool single)
+{
+	struct i2c_client *client = to_i2c_client(di->dev);
+	unsigned char i2c_data[3];
+	int ret, len;
+
+	i2c_data[0] = reg;
+	i2c_data[1] = val & 0xff;
+
+	if (single) {
+		len = 2;
+	} else {
+		i2c_data[2] = (val >> 8) & 0xff;
+		len = 3;
+	}
+
+	ret = i2c_master_send(client, i2c_data, len);
+	if (ret == len)
+		return 0;
+
+	return (ret < 0) ? ret : -EIO;
+}
+
+static int bq27x00_ctrl_read_i2c(struct bq27x00_device_info *di,
+					u8 ctrl_reg, u16 ctrl_func_reg)
+{
+	int ret = bq27x00_write(di, ctrl_reg, ctrl_func_reg, false);
+	if (ret < 0) {
+		dev_err(di->dev, "write failure\n");
+		return ret;
+	}
+
+	ret = bq27x00_read(di, ctrl_reg, false);
+	if (ret < 0) {
+		dev_err(di->dev, "read failure\n");
+		return ret;
+	}
+
+	return ret;
+}
+
 static int bq27x00_battery_probe(struct i2c_client *client,
 				 const struct i2c_device_id *id)
 {
 	char *name;
 	struct bq27x00_device_info *di;
 	int num;
+	u16 read_data;
 	int retval = 0;
 
 	/* Get new ID for the new battery device */
@@ -634,11 +823,29 @@ static int bq27x00_battery_probe(struct i2c_client *client,
 	di->chip = id->driver_data;
 	di->bat.name = name;
 	di->bus.read = &bq27x00_read_i2c;
+	di->bus.ctrl_read = &bq27x00_ctrl_read_i2c;
+	di->bus.write = &bq27x00_write_i2c;
 
-	if (bq27x00_powersupply_init(di))
+	i2c_set_clientdata(client, di);
+
+	/* Let's see whether this adapter can support what we need. */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "insufficient functionality!\n");
+		retval = -ENODEV;
 		goto batt_failed_3;
+	}
 
-	i2c_set_clientdata(client, di);
+	read_data = bq27x00_read(di, BQ27x00_REG_FLAGS, false);
+
+	if (!(read_data & BQ27500_FLAG_BAT_DET)) {
+		dev_err(&client->dev, "no battery present\n");
+		retval = -ENODEV;
+		goto batt_failed_3;
+	}
+
+	retval = bq27x00_powersupply_init(di);
+	if (retval < 0)
+		goto batt_failed_3;
 
 	return 0;
 
@@ -671,20 +878,82 @@ static int bq27x00_battery_remove(struct i2c_client *client)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int bq27x00_battery_suspend(struct device *dev)
+{
+	int ret;
+	struct platform_device *pdev = to_platform_device(dev);
+	struct bq27x00_device_info *di = platform_get_drvdata(pdev);
+
+	cancel_delayed_work_sync(&di->work);
+	cancel_delayed_work_sync(&di->external_power_changed_work);
+
+	if (di->chip == BQ27510) {
+		ret = bq27x00_write(di, BQ27510_CNTL,
+					BQ27510_CNTL_SET_SLEEP, false);
+		if (ret < 0) {
+			dev_err(di->dev, "write failure\n");
+			return ret;
+		}
+		ret = bq27x00_write(di, BQ27510_CNTL, 0x01, false);
+		if (ret < 0) {
+			dev_err(di->dev, "write failure\n");
+			return ret;
+		}
+	}
+	return 0;
+}
+
+static int bq27x00_battery_resume(struct device *dev)
+{
+	int ret;
+	struct platform_device *pdev = to_platform_device(dev);
+	struct bq27x00_device_info *di = platform_get_drvdata(pdev);
+
+	if (di->chip == BQ27510) {
+		ret = bq27x00_write(di, BQ27510_CNTL,
+				BQ27510_CNTL_CLEAR_SLEEP, false);
+		if (ret < 0) {
+			dev_err(di->dev, "write failure\n");
+			return ret;
+		}
+		ret = bq27x00_write(di, BQ27510_CNTL, 0x01, false);
+		if (ret < 0) {
+			dev_err(di->dev, "write failure\n");
+			return ret;
+		}
+	}
+
+	schedule_delayed_work(&di->work, HZ);
+
+	return 0;
+}
+
+static const struct dev_pm_ops bq27x00_battery_pm_ops = {
+	.suspend	= bq27x00_battery_suspend,
+	.resume		= bq27x00_battery_resume,
+};
+
+#endif
+
 static const struct i2c_device_id bq27x00_id[] = {
 	{ "bq27200", BQ27000 },	/* bq27200 is same as bq27000, but with i2c */
 	{ "bq27500", BQ27500 },
+	{ "bq27510", BQ27510 },
 	{},
 };
 MODULE_DEVICE_TABLE(i2c, bq27x00_id);
 
 static struct i2c_driver bq27x00_battery_driver = {
+	.probe		= bq27x00_battery_probe,
+	.remove		= bq27x00_battery_remove,
+	.id_table = bq27x00_id,
 	.driver = {
 		.name = "bq27x00-battery",
+#if defined(CONFIG_PM)
+		.pm		= &bq27x00_battery_pm_ops,
+#endif
 	},
-	.probe = bq27x00_battery_probe,
-	.remove = bq27x00_battery_remove,
-	.id_table = bq27x00_id,
 };
 
 static inline int bq27x00_battery_i2c_init(void)
diff --git a/drivers/power/ds2780_battery.c b/drivers/power/ds2780_battery.c
index 1fefe82e12e3..91a783d72360 100644
--- a/drivers/power/ds2780_battery.c
+++ b/drivers/power/ds2780_battery.c
@@ -39,6 +39,7 @@ struct ds2780_device_info {
 	struct device *dev;
 	struct power_supply bat;
 	struct device *w1_dev;
+	struct task_struct *mutex_holder;
 };
 
 enum current_types {
@@ -49,8 +50,8 @@ enum current_types {
 static const char model[] = "DS2780";
 static const char manufacturer[] = "Maxim/Dallas";
 
-static inline struct ds2780_device_info *to_ds2780_device_info(
-	struct power_supply *psy)
+static inline struct ds2780_device_info *
+to_ds2780_device_info(struct power_supply *psy)
 {
 	return container_of(psy, struct ds2780_device_info, bat);
 }
@@ -60,17 +61,28 @@ static inline struct power_supply *to_power_supply(struct device *dev)
 	return dev_get_drvdata(dev);
 }
 
-static inline int ds2780_read8(struct device *dev, u8 *val, int addr)
+static inline int ds2780_battery_io(struct ds2780_device_info *dev_info,
+	char *buf, int addr, size_t count, int io)
 {
-	return w1_ds2780_io(dev, val, addr, sizeof(u8), 0);
+	if (dev_info->mutex_holder == current)
+		return w1_ds2780_io_nolock(dev_info->w1_dev, buf, addr, count, io);
+	else
+		return w1_ds2780_io(dev_info->w1_dev, buf, addr, count, io);
+}
+
+static inline int ds2780_read8(struct ds2780_device_info *dev_info, u8 *val,
+	int addr)
+{
+	return ds2780_battery_io(dev_info, val, addr, sizeof(u8), 0);
 }
 
-static int ds2780_read16(struct device *dev, s16 *val, int addr)
+static int ds2780_read16(struct ds2780_device_info *dev_info, s16 *val,
+	int addr)
 {
 	int ret;
 	u8 raw[2];
 
-	ret = w1_ds2780_io(dev, raw, addr, sizeof(u8) * 2, 0);
+	ret = ds2780_battery_io(dev_info, raw, addr, sizeof(raw), 0);
 	if (ret < 0)
 		return ret;
 
@@ -79,16 +91,16 @@ static int ds2780_read16(struct device *dev, s16 *val, int addr)
 	return 0;
 }
 
-static inline int ds2780_read_block(struct device *dev, u8 *val, int addr,
-	size_t count)
+static inline int ds2780_read_block(struct ds2780_device_info *dev_info,
+	u8 *val, int addr, size_t count)
 {
-	return w1_ds2780_io(dev, val, addr, count, 0);
+	return ds2780_battery_io(dev_info, val, addr, count, 0);
 }
 
-static inline int ds2780_write(struct device *dev, u8 *val, int addr,
-	size_t count)
+static inline int ds2780_write(struct ds2780_device_info *dev_info, u8 *val,
+	int addr, size_t count)
 {
-	return w1_ds2780_io(dev, val, addr, count, 1);
+	return ds2780_battery_io(dev_info, val, addr, count, 1);
 }
 
 static inline int ds2780_store_eeprom(struct device *dev, int addr)
@@ -122,7 +134,7 @@ static int ds2780_set_sense_register(struct ds2780_device_info *dev_info,
 {
 	int ret;
 
-	ret = ds2780_write(dev_info->w1_dev, &conductance,
+	ret = ds2780_write(dev_info, &conductance,
 				DS2780_RSNSP_REG, sizeof(u8));
 	if (ret < 0)
 		return ret;
@@ -134,7 +146,7 @@ static int ds2780_set_sense_register(struct ds2780_device_info *dev_info,
 static int ds2780_get_rsgain_register(struct ds2780_device_info *dev_info,
 	u16 *rsgain)
 {
-	return ds2780_read16(dev_info->w1_dev, rsgain, DS2780_RSGAIN_MSB_REG);
+	return ds2780_read16(dev_info, rsgain, DS2780_RSGAIN_MSB_REG);
 }
 
 /* Set RSGAIN value from 0 to 1.999 in steps of 0.001 */
@@ -144,8 +156,8 @@ static int ds2780_set_rsgain_register(struct ds2780_device_info *dev_info,
 	int ret;
 	u8 raw[] = {rsgain >> 8, rsgain & 0xFF};
 
-	ret = ds2780_write(dev_info->w1_dev, raw,
-				DS2780_RSGAIN_MSB_REG, sizeof(u8) * 2);
+	ret = ds2780_write(dev_info, raw,
+				DS2780_RSGAIN_MSB_REG, sizeof(raw));
 	if (ret < 0)
 		return ret;
 
@@ -167,7 +179,7 @@ static int ds2780_get_voltage(struct ds2780_device_info *dev_info,
 	 * Bits 2 - 0 of the voltage value are in bits 7 - 5 of the
 	 * voltage LSB register
 	 */
-	ret = ds2780_read16(dev_info->w1_dev, &voltage_raw,
+	ret = ds2780_read16(dev_info, &voltage_raw,
 				DS2780_VOLT_MSB_REG);
 	if (ret < 0)
 		return ret;
@@ -196,7 +208,7 @@ static int ds2780_get_temperature(struct ds2780_device_info *dev_info,
 	 * Bits 2 - 0 of the temperature value are in bits 7 - 5 of the
 	 * temperature LSB register
 	 */
-	ret = ds2780_read16(dev_info->w1_dev, &temperature_raw,
+	ret = ds2780_read16(dev_info, &temperature_raw,
 				DS2780_TEMP_MSB_REG);
 	if (ret < 0)
 		return ret;
@@ -222,13 +234,13 @@ static int ds2780_get_current(struct ds2780_device_info *dev_info,
 	 * The units of measurement for current are dependent on the value of
 	 * the sense resistor.
 	 */
-	ret = ds2780_read8(dev_info->w1_dev, &sense_res_raw, DS2780_RSNSP_REG);
+	ret = ds2780_read8(dev_info, &sense_res_raw, DS2780_RSNSP_REG);
 	if (ret < 0)
 		return ret;
 
 	if (sense_res_raw == 0) {
 		dev_err(dev_info->dev, "sense resistor value is 0\n");
-		return -ENXIO;
+		return -EINVAL;
 	}
 	sense_res = 1000 / sense_res_raw;
 
@@ -248,7 +260,7 @@ static int ds2780_get_current(struct ds2780_device_info *dev_info,
 	 * Bits 7 - 0 of the current value are in bits 7 - 0 of the current
 	 * LSB register
 	 */
-	ret = ds2780_read16(dev_info->w1_dev, &current_raw, reg_msb);
+	ret = ds2780_read16(dev_info, &current_raw, reg_msb);
 	if (ret < 0)
 		return ret;
 
@@ -267,7 +279,7 @@ static int ds2780_get_accumulated_current(struct ds2780_device_info *dev_info,
 	 * The units of measurement for accumulated current are dependent on
 	 * the value of the sense resistor.
 	 */
-	ret = ds2780_read8(dev_info->w1_dev, &sense_res_raw, DS2780_RSNSP_REG);
+	ret = ds2780_read8(dev_info, &sense_res_raw, DS2780_RSNSP_REG);
 	if (ret < 0)
 		return ret;
 
@@ -285,7 +297,7 @@ static int ds2780_get_accumulated_current(struct ds2780_device_info *dev_info,
 	 * Bits 7 - 0 of the ACR value are in bits 7 - 0 of the ACR
 	 * LSB register
 	 */
-	ret = ds2780_read16(dev_info->w1_dev, &current_raw, DS2780_ACR_MSB_REG);
+	ret = ds2780_read16(dev_info, &current_raw, DS2780_ACR_MSB_REG);
 	if (ret < 0)
 		return ret;
 
@@ -299,7 +311,7 @@ static int ds2780_get_capacity(struct ds2780_device_info *dev_info,
 	int ret;
 	u8 raw;
 
-	ret = ds2780_read8(dev_info->w1_dev, &raw, DS2780_RARC_REG);
+	ret = ds2780_read8(dev_info, &raw, DS2780_RARC_REG);
 	if (ret < 0)
 		return ret;
 
@@ -345,7 +357,7 @@ static int ds2780_get_charge_now(struct ds2780_device_info *dev_info,
 	 * Bits 7 - 0 of the RAAC value are in bits 7 - 0 of the RAAC
 	 * LSB register
 	 */
-	ret = ds2780_read16(dev_info->w1_dev, &charge_raw, DS2780_RAAC_MSB_REG);
+	ret = ds2780_read16(dev_info, &charge_raw, DS2780_RAAC_MSB_REG);
 	if (ret < 0)
 		return ret;
 
@@ -356,7 +368,7 @@ static int ds2780_get_charge_now(struct ds2780_device_info *dev_info,
 static int ds2780_get_control_register(struct ds2780_device_info *dev_info,
 	u8 *control_reg)
 {
-	return ds2780_read8(dev_info->w1_dev, control_reg, DS2780_CONTROL_REG);
+	return ds2780_read8(dev_info, control_reg, DS2780_CONTROL_REG);
 }
 
 static int ds2780_set_control_register(struct ds2780_device_info *dev_info,
@@ -364,7 +376,7 @@ static int ds2780_set_control_register(struct ds2780_device_info *dev_info,
 {
 	int ret;
 
-	ret = ds2780_write(dev_info->w1_dev, &control_reg,
+	ret = ds2780_write(dev_info, &control_reg,
 				DS2780_CONTROL_REG, sizeof(u8));
 	if (ret < 0)
 		return ret;
@@ -503,7 +515,7 @@ static ssize_t ds2780_get_sense_resistor_value(struct device *dev,
 	struct power_supply *psy = to_power_supply(dev);
 	struct ds2780_device_info *dev_info = to_ds2780_device_info(psy);
 
-	ret = ds2780_read8(dev_info->w1_dev, &sense_resistor, DS2780_RSNSP_REG);
+	ret = ds2780_read8(dev_info, &sense_resistor, DS2780_RSNSP_REG);
 	if (ret < 0)
 		return ret;
 
@@ -584,7 +596,7 @@ static ssize_t ds2780_get_pio_pin(struct device *dev,
 	struct power_supply *psy = to_power_supply(dev);
 	struct ds2780_device_info *dev_info = to_ds2780_device_info(psy);
 
-	ret = ds2780_read8(dev_info->w1_dev, &sfr, DS2780_SFR_REG);
+	ret = ds2780_read8(dev_info, &sfr, DS2780_SFR_REG);
 	if (ret < 0)
 		return ret;
 
@@ -611,7 +623,7 @@ static ssize_t ds2780_set_pio_pin(struct device *dev,
 		return -EINVAL;
 	}
 
-	ret = ds2780_write(dev_info->w1_dev, &new_setting,
+	ret = ds2780_write(dev_info, &new_setting,
 				DS2780_SFR_REG, sizeof(u8));
 	if (ret < 0)
 		return ret;
@@ -632,7 +644,7 @@ static ssize_t ds2780_read_param_eeprom_bin(struct file *filp,
 		DS2780_EEPROM_BLOCK1_END -
 		DS2780_EEPROM_BLOCK1_START + 1 - off);
 
-	return ds2780_read_block(dev_info->w1_dev, buf,
+	return ds2780_read_block(dev_info, buf,
 				DS2780_EEPROM_BLOCK1_START + off, count);
 }
 
@@ -650,7 +662,7 @@ static ssize_t ds2780_write_param_eeprom_bin(struct file *filp,
 		DS2780_EEPROM_BLOCK1_END -
 		DS2780_EEPROM_BLOCK1_START + 1 - off);
 
-	ret = ds2780_write(dev_info->w1_dev, buf,
+	ret = ds2780_write(dev_info, buf,
 				DS2780_EEPROM_BLOCK1_START + off, count);
 	if (ret < 0)
 		return ret;
@@ -685,9 +697,8 @@ static ssize_t ds2780_read_user_eeprom_bin(struct file *filp,
 		DS2780_EEPROM_BLOCK0_END -
 		DS2780_EEPROM_BLOCK0_START + 1 - off);
 
-	return ds2780_read_block(dev_info->w1_dev, buf,
+	return ds2780_read_block(dev_info, buf,
 				DS2780_EEPROM_BLOCK0_START + off, count);
-
 }
 
 static ssize_t ds2780_write_user_eeprom_bin(struct file *filp,
@@ -704,7 +715,7 @@ static ssize_t ds2780_write_user_eeprom_bin(struct file *filp,
 		DS2780_EEPROM_BLOCK0_END -
 		DS2780_EEPROM_BLOCK0_START + 1 - off);
 
-	ret = ds2780_write(dev_info->w1_dev, buf,
+	ret = ds2780_write(dev_info, buf,
 				DS2780_EEPROM_BLOCK0_START + off, count);
 	if (ret < 0)
 		return ret;
@@ -768,6 +779,7 @@ static int __devinit ds2780_battery_probe(struct platform_device *pdev)
 	dev_info->bat.properties	= ds2780_battery_props;
 	dev_info->bat.num_properties	= ARRAY_SIZE(ds2780_battery_props);
 	dev_info->bat.get_property	= ds2780_battery_get_property;
+	dev_info->mutex_holder		= current;
 
 	ret = power_supply_register(&pdev->dev, &dev_info->bat);
 	if (ret) {
@@ -797,6 +809,8 @@ static int __devinit ds2780_battery_probe(struct platform_device *pdev)
 		goto fail_remove_bin_file;
 	}
 
+	dev_info->mutex_holder = NULL;
+
 	return 0;
 
 fail_remove_bin_file:
@@ -816,6 +830,8 @@ static int __devexit ds2780_battery_remove(struct platform_device *pdev)
 {
 	struct ds2780_device_info *dev_info = platform_get_drvdata(pdev);
 
+	dev_info->mutex_holder = current;
+
 	/* remove attributes */
 	sysfs_remove_group(&dev_info->bat.dev->kobj, &ds2780_attr_group);
 
diff --git a/drivers/power/gpio-charger.c b/drivers/power/gpio-charger.c
index a64b8854cfd5..020b415ef598 100644
--- a/drivers/power/gpio-charger.c
+++ b/drivers/power/gpio-charger.c
@@ -54,7 +54,7 @@ static int gpio_charger_get_property(struct power_supply *psy,
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_ONLINE:
-		val->intval = gpio_get_value(pdata->gpio);
+		val->intval = gpio_get_value_cansleep(pdata->gpio);
 		val->intval ^= pdata->gpio_active_low;
 		break;
 	default:
diff --git a/drivers/power/max17048_battery.c b/drivers/power/max17048_battery.c
new file mode 100644
index 000000000000..ba3d6440ab3d
--- /dev/null
+++ b/drivers/power/max17048_battery.c
@@ -0,0 +1,639 @@
+/*
+ *  max17048_battery.c
+ *  fuel-gauge systems for lithium-ion (Li+) batteries
+ *
+ *  Copyright (C) 2012 Nvidia Cooperation
+ *  Chandler Zhang <chazhang@nvidia.com>
+ *  Syed Rafiuddin <srafiuddin@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <asm/unaligned.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/mutex.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/power_supply.h>
+#include <linux/slab.h>
+#include <linux/max17048_battery.h>
+
+#define MAX17048_VCELL		0x02
+#define MAX17048_SOC		0x04
+#define MAX17048_VER		0x08
+#define MAX17048_HIBRT		0x0A
+#define MAX17048_CONFIG		0x0C
+#define MAX17048_OCV		0x0E
+#define MAX17048_VLRT		0x14
+#define MAX17048_VRESET		0x18
+#define MAX17048_STATUS		0x1A
+#define MAX17048_UNLOCK		0x3E
+#define MAX17048_TABLE		0x40
+#define MAX17048_RCOMPSEG1	0x80
+#define MAX17048_RCOMPSEG2	0x90
+#define MAX17048_CMD		0xFF
+#define MAX17048_UNLOCK_VALUE	0x4a57
+#define MAX17048_RESET_VALUE	0x5400
+#define MAX17048_DELAY		1000
+#define MAX17048_BATTERY_FULL	100
+#define MAX17048_BATTERY_LOW	15
+#define MAX17048_VERSION_NO	0x11
+
+struct max17048_chip {
+	struct i2c_client		*client;
+	struct delayed_work		work;
+	struct power_supply		battery;
+	struct power_supply		ac;
+	struct power_supply		usb;
+	struct max17048_battery_model	*model_data;
+
+	/* State Of Connect */
+	int ac_online;
+	int usb_online;
+	/* battery voltage */
+	int vcell;
+	/* battery capacity */
+	int soc;
+	/* State Of Charge */
+	int status;
+	/* battery health */
+	int health;
+	/* battery capacity */
+	int capacity_level;
+
+	int lasttime_vcell;
+	int lasttime_soc;
+	int lasttime_status;
+};
+
+uint8_t max17048_custom_data[] = {
+	0xAA, 0x00, 0xB1, 0xF0, 0xB7, 0xE0, 0xB9, 0x60, 0xBB, 0x80,
+	0xBC, 0x40, 0xBD, 0x30, 0xBD, 0x50, 0xBD, 0xF0, 0xBE, 0x40,
+	0xBF, 0xD0, 0xC0, 0x90, 0xC4, 0x30, 0xC7, 0xC0, 0xCA, 0x60,
+	0xCF, 0x30, 0x01, 0x20, 0x09, 0xC0, 0x1F, 0xC0, 0x2B, 0xE0,
+	0x4F, 0xC0, 0x30, 0x00, 0x47, 0x80, 0x4F, 0xE0, 0x77, 0x00,
+	0x15, 0x60, 0x46, 0x20, 0x13, 0x80, 0x1A, 0x60, 0x12, 0x20,
+	0x14, 0xA0, 0x14, 0xA0};
+
+static int max17048_write_word(struct i2c_client *client, int reg, u16 value)
+{
+	int ret;
+
+	ret = i2c_smbus_write_word_data(client, reg, swab16(value));
+
+	if (ret < 0)
+		dev_err(&client->dev, "%s(): Failed in writing register"
+					"0x%02x err %d\n", __func__, reg, ret);
+
+	return ret;
+}
+
+static int max17048_read_word(struct i2c_client *client, int reg)
+{
+	int ret;
+
+	ret = i2c_smbus_read_word_data(client, reg);
+
+	if (ret < 0) {
+		dev_err(&client->dev, "%s(): Failed in reading register"
+					"0x%02x err %d\n", __func__, reg, ret);
+		return ret;
+	} else {
+		ret = (int)swab16((uint16_t)(ret & 0x0000ffff));
+		return ret;
+	}
+}
+
+static int max17048_get_property(struct power_supply *psy,
+			    enum power_supply_property psp,
+			    union power_supply_propval *val)
+{
+	struct max17048_chip *chip = container_of(psy,
+				struct max17048_chip, battery);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		val->intval = chip->status;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		val->intval = chip->vcell;
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY:
+		val->intval = chip->soc;
+		break;
+	case POWER_SUPPLY_PROP_HEALTH:
+		val->intval = chip->health;
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY_LEVEL:
+		val->intval = chip->capacity_level;
+		break;
+	default:
+	return -EINVAL;
+	}
+	return 0;
+}
+
+static int max17048_ac_get_property(struct power_supply *psy,
+				enum power_supply_property psp,
+				union power_supply_propval *val)
+{
+	struct max17048_chip *chip = container_of(psy,
+				struct max17048_chip, ac);
+
+	if (psp == POWER_SUPPLY_PROP_ONLINE)
+		val->intval = chip->ac_online;
+	else
+		return -EINVAL;
+
+	return 0;
+}
+
+static int max17048_usb_get_property(struct power_supply *psy,
+				enum power_supply_property psp,
+				union power_supply_propval *val)
+{
+	struct max17048_chip *chip = container_of(psy,
+					struct max17048_chip, usb);
+
+	if (psp == POWER_SUPPLY_PROP_ONLINE)
+		val->intval = chip->usb_online;
+	else
+		return -EINVAL;
+
+	return 0;
+}
+
+static void max17048_get_vcell(struct i2c_client *client)
+{
+	struct max17048_chip *chip = i2c_get_clientdata(client);
+	int vcell;
+
+	vcell = max17048_read_word(client, MAX17048_VCELL);
+	if (vcell < 0)
+		dev_err(&client->dev, "%s: err %d\n", __func__, vcell);
+	else
+		chip->vcell = (uint16_t)(((vcell >> 4) * 125) / 100);
+}
+
+static void max17048_get_soc(struct i2c_client *client)
+{
+	struct max17048_chip *chip = i2c_get_clientdata(client);
+	int soc;
+
+	soc = max17048_read_word(client, MAX17048_SOC);
+	if (soc < 0)
+		dev_err(&client->dev, "%s: err %d\n", __func__, soc);
+	else
+		chip->soc = (uint16_t)soc >> 9;
+
+	if (chip->soc > MAX17048_BATTERY_FULL) {
+		chip->soc = MAX17048_BATTERY_FULL;
+		chip->status = POWER_SUPPLY_STATUS_FULL;
+		chip->capacity_level = POWER_SUPPLY_CAPACITY_LEVEL_FULL;
+		chip->health = POWER_SUPPLY_HEALTH_GOOD;
+	} else if (chip->soc < MAX17048_BATTERY_LOW) {
+		chip->health = POWER_SUPPLY_HEALTH_DEAD;
+		chip->capacity_level = POWER_SUPPLY_CAPACITY_LEVEL_CRITICAL;
+	} else {
+		chip->health = POWER_SUPPLY_HEALTH_GOOD;
+		chip->capacity_level = POWER_SUPPLY_CAPACITY_LEVEL_NORMAL;
+	}
+}
+
+static uint16_t max17048_get_version(struct i2c_client *client)
+{
+	return swab16(i2c_smbus_read_word_data(client, MAX17048_VER));
+}
+
+static void max17048_work(struct work_struct *work)
+{
+	struct max17048_chip *chip;
+
+	chip = container_of(work, struct max17048_chip, work.work);
+
+	max17048_get_vcell(chip->client);
+	max17048_get_soc(chip->client);
+
+	if (chip->vcell != chip->lasttime_vcell ||
+		chip->soc != chip->lasttime_soc ||
+		chip->status != chip->lasttime_status) {
+
+		chip->lasttime_vcell = chip->vcell;
+		chip->lasttime_soc = chip->soc;
+		chip->lasttime_status = chip->status;
+
+		power_supply_changed(&chip->battery);
+	}
+	schedule_delayed_work(&chip->work, MAX17048_DELAY);
+}
+
+static void max17048_battery_status(enum charging_states status,
+				enum charger_type chrg_type, void *data)
+{
+	struct max17048_chip *chip = data;
+
+	chip->ac_online = 0;
+	chip->usb_online = 0;
+
+	if (status == progress) {
+		chip->status = POWER_SUPPLY_STATUS_CHARGING;
+		if (chrg_type == AC)
+			chip->ac_online = 1;
+		else if (chrg_type == USB)
+			chip->usb_online = 1;
+	} else
+		chip->status = POWER_SUPPLY_STATUS_DISCHARGING;
+
+
+	power_supply_changed(&chip->battery);
+	power_supply_changed(&chip->usb);
+	power_supply_changed(&chip->ac);
+}
+
+static enum power_supply_property max17048_battery_props[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CAPACITY,
+	POWER_SUPPLY_PROP_HEALTH,
+	POWER_SUPPLY_PROP_CAPACITY_LEVEL,
+};
+
+static enum power_supply_property max17048_ac_props[] = {
+	POWER_SUPPLY_PROP_ONLINE,
+};
+
+static enum power_supply_property max17048_usb_props[] = {
+	POWER_SUPPLY_PROP_ONLINE,
+};
+
+static int max17048_write_rcomp_seg(struct i2c_client *client,
+						uint16_t rcomp_seg)
+{
+	uint8_t rs1, rs2;
+	int ret;
+	uint8_t rcomp_seg_table[16];
+
+	rs2 = rcomp_seg | 0x00FF;
+	rs1 = rcomp_seg >> 8;
+
+	rcomp_seg_table[0] = rcomp_seg_table[2] = rcomp_seg_table[4] =
+		rcomp_seg_table[6] = rcomp_seg_table[8] = rcomp_seg_table[10] =
+			rcomp_seg_table[12] = rcomp_seg_table[14] = rs1;
+
+	rcomp_seg_table[1] = rcomp_seg_table[3] = rcomp_seg_table[5] =
+		rcomp_seg_table[7] = rcomp_seg_table[9] = rcomp_seg_table[11] =
+			rcomp_seg_table[13] = rcomp_seg_table[15] = rs2;
+
+	ret = i2c_smbus_write_i2c_block_data(client, MAX17048_RCOMPSEG1,
+				16, (uint8_t *)rcomp_seg_table);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+		return ret;
+	}
+
+	ret = i2c_smbus_write_i2c_block_data(client, MAX17048_RCOMPSEG2,
+				16, (uint8_t *)rcomp_seg_table);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int max17048_load_model_data(struct max17048_chip *chip)
+{
+	struct i2c_client *client = chip->client;
+	struct max17048_battery_model *mdata = chip->model_data;
+	uint16_t soc_tst, ocv;
+	int i, ret = 0;
+
+	/* read OCV */
+	ret = max17048_read_word(client, MAX17048_OCV);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+		return ret;
+	}
+	ocv = (uint16_t)ret;
+	if (ocv == 0xffff) {
+		dev_err(&client->dev, "%s: Failed in unlocking"
+					"max17048 err: %d\n", __func__, ocv);
+		return -1;
+	}
+
+	/* write custom model data */
+	for (i = 0; i < 4; i += 1) {
+		if (i2c_smbus_write_i2c_block_data(client,
+			(MAX17048_TABLE+i*16), 16,
+				&max17048_custom_data[i*0x10]) < 0) {
+			dev_err(&client->dev, "%s: error writing model data:\n",
+								__func__);
+			return -1;
+		}
+	}
+
+	/* Write OCV Test value */
+	ret = max17048_write_word(client, MAX17048_OCV, mdata->ocvtest);
+	if (ret < 0)
+		return ret;
+
+	ret = max17048_write_rcomp_seg(client, mdata->rcomp_seg);
+	if (ret < 0)
+		return ret;
+
+	/* Disable hibernate */
+	ret = max17048_write_word(client, MAX17048_HIBRT, 0x0000);
+	if (ret < 0)
+		return ret;
+
+	/* Lock model access */
+	ret = max17048_write_word(client, MAX17048_UNLOCK, 0x0000);
+	if (ret < 0)
+		return ret;
+
+	/* Delay between 150ms to 600ms */
+	mdelay(200);
+
+	/* Read SOC Register and compare to expected result */
+	ret = max17048_read_word(client, MAX17048_SOC);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+		return ret;
+	}
+	soc_tst = (uint16_t)ret;
+	if (!((soc_tst >> 8) >= mdata->soccheck_A &&
+				(soc_tst >> 8) <=  mdata->soccheck_B)) {
+		dev_err(&client->dev, "%s: soc comparison failed %d\n",
+					__func__, ret);
+		return ret;
+	} else {
+		dev_info(&client->dev, "MAX17048 Custom data"
+						" loading successfull\n");
+	}
+
+	/* unlock model access */
+	ret = max17048_write_word(client, MAX17048_UNLOCK,
+					MAX17048_UNLOCK_VALUE);
+	if (ret < 0)
+		return ret;
+
+	/* Restore OCV */
+	ret = max17048_write_word(client, MAX17048_OCV, ocv);
+	if (ret < 0)
+		return ret;
+
+	return ret;
+}
+
+static int max17048_initialize(struct max17048_chip *chip)
+{
+	uint8_t ret, config, status;
+	uint16_t wrt_status;
+	struct i2c_client *client = chip->client;
+	struct max17048_battery_model *mdata = chip->model_data;
+
+	/* unlock model access */
+	ret = max17048_write_word(client, MAX17048_UNLOCK,
+			MAX17048_UNLOCK_VALUE);
+	if (ret < 0)
+		return ret;
+
+	/* load model data */
+	ret = max17048_load_model_data(chip);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+		return ret;
+	}
+
+	if (mdata->bits == 19)
+		config = 32 - (mdata->alert_threshold * 2);
+	else if (mdata->bits == 18)
+		config = 32 - mdata->alert_threshold;
+
+	config = mdata->one_percent_alerts | config;
+
+	ret = max17048_write_word(client, MAX17048_CONFIG,
+			((mdata->rcomp << 8) | config));
+	if (ret < 0)
+		return ret;
+
+	/* Voltage Alert configuration */
+	ret = max17048_write_word(client, MAX17048_VLRT, mdata->valert);
+	if (ret < 0)
+		return ret;
+
+	/* Hibernate configuration */
+	ret = max17048_write_word(client, MAX17048_HIBRT, mdata->hibernate);
+	if (ret < 0)
+		return ret;
+
+	ret = max17048_write_word(client, MAX17048_VRESET, mdata->vreset);
+	if (ret < 0)
+		return ret;
+
+	/* clears the reset indicator */
+	ret = max17048_read_word(client, MAX17048_STATUS);
+	if (ret < 0)
+		return ret;
+
+	/* Sets the EnVR bit if selected */
+	status = (ret & 0xFE) | mdata->alert_on_reset;
+	wrt_status = status << 8;
+
+	ret = max17048_write_word(client, MAX17048_STATUS, wrt_status);
+	if (ret < 0)
+		return ret;
+
+	/* Lock model access */
+	ret = max17048_write_word(client, MAX17048_UNLOCK, 0x0000);
+	if (ret < 0)
+		return ret;
+
+	/* Add delay */
+	mdelay(200);
+	return 0;
+}
+
+static int __devinit max17048_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct max17048_chip *chip;
+	int ret;
+	uint16_t version;
+
+	chip = kzalloc(sizeof(*chip), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	chip->client = client;
+	chip->model_data = client->dev.platform_data;
+	chip->ac_online = 0;
+	chip->usb_online = 0;
+
+	i2c_set_clientdata(client, chip);
+
+	version = max17048_get_version(client);
+	if (version != MAX17048_VERSION_NO) {
+		ret = -ENODEV;
+		goto error2;
+	}
+	dev_info(&client->dev, "MAX17048 Fuel-Gauge Ver 0x%x\n", version);
+
+	ret = max17048_initialize(chip);
+	if (ret < 0) {
+		dev_err(&client->dev, "Error: Initializing fuel-gauge\n");
+		goto error2;
+	}
+
+	ret = register_callback(max17048_battery_status, chip);
+	if (ret < 0)
+		goto error2;
+
+	chip->battery.name		= "battery";
+	chip->battery.type		= POWER_SUPPLY_TYPE_BATTERY;
+	chip->battery.get_property	= max17048_get_property;
+	chip->battery.properties	= max17048_battery_props;
+	chip->battery.num_properties	= ARRAY_SIZE(max17048_battery_props);
+
+	ret = power_supply_register(&client->dev, &chip->battery);
+	if (ret) {
+		dev_err(&client->dev, "failed: power supply register\n");
+		goto error2;
+	}
+
+	chip->ac.name		= "maxim-ac";
+	chip->ac.type		= POWER_SUPPLY_TYPE_MAINS;
+	chip->ac.get_property	= max17048_ac_get_property;
+	chip->ac.properties	= max17048_ac_props;
+	chip->ac.num_properties	= ARRAY_SIZE(max17048_ac_props);
+
+	ret = power_supply_register(&client->dev, &chip->ac);
+	if (ret) {
+		dev_err(&client->dev, "failed: power supply register\n");
+		goto error1;
+	}
+
+	chip->usb.name		= "maxim-usb";
+	chip->usb.type		= POWER_SUPPLY_TYPE_USB;
+	chip->usb.get_property	= max17048_usb_get_property;
+	chip->usb.properties	= max17048_usb_props;
+	chip->usb.num_properties	= ARRAY_SIZE(max17048_usb_props);
+
+	ret = power_supply_register(&client->dev, &chip->usb);
+	if (ret) {
+		dev_err(&client->dev, "failed: power supply register\n");
+		goto error;
+	}
+
+	INIT_DELAYED_WORK_DEFERRABLE(&chip->work, max17048_work);
+	schedule_delayed_work(&chip->work, MAX17048_DELAY);
+
+	ret = update_charger_status();
+	if (ret) {
+		dev_err(&client->dev, "failed: update_charger_status\n");
+		goto error;
+	}
+
+	return 0;
+error:
+	power_supply_unregister(&chip->ac);
+error1:
+	power_supply_unregister(&chip->battery);
+error2:
+	kfree(chip);
+	return ret;
+}
+
+static int __devexit max17048_remove(struct i2c_client *client)
+{
+	struct max17048_chip *chip = i2c_get_clientdata(client);
+
+	power_supply_unregister(&chip->battery);
+	power_supply_unregister(&chip->usb);
+	power_supply_unregister(&chip->ac);
+	cancel_delayed_work(&chip->work);
+	kfree(chip);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+
+static int max17048_suspend(struct i2c_client *client,
+		pm_message_t state)
+{
+	struct max17048_chip *chip = i2c_get_clientdata(client);
+	int ret;
+
+	cancel_delayed_work(&chip->work);
+
+	ret = max17048_write_word(client, MAX17048_HIBRT, 0xffff);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed in entering hibernate mode\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int max17048_resume(struct i2c_client *client)
+{
+	struct max17048_chip *chip = i2c_get_clientdata(client);
+	int ret;
+	struct max17048_battery_model *mdata = chip->model_data;
+
+	ret = max17048_write_word(client, MAX17048_HIBRT, mdata->hibernate);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed in exiting hibernate mode\n");
+		return ret;
+	}
+
+	update_charger_status();
+
+	schedule_work(&chip->work);
+	return 0;
+}
+
+#else
+
+#define max17048_suspend NULL
+#define max17048_resume NULL
+
+#endif /* CONFIG_PM */
+
+static const struct i2c_device_id max17048_id[] = {
+	{ "max17048", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, max17048_id);
+
+static struct i2c_driver max17048_i2c_driver = {
+	.driver	= {
+		.name	= "max17048",
+	},
+	.probe		= max17048_probe,
+	.remove		= __devexit_p(max17048_remove),
+	.suspend	= max17048_suspend,
+	.resume		= max17048_resume,
+	.id_table	= max17048_id,
+};
+
+static int __init max17048_init(void)
+{
+	return i2c_add_driver(&max17048_i2c_driver);
+}
+module_init(max17048_init);
+
+static void __exit max17048_exit(void)
+{
+	i2c_del_driver(&max17048_i2c_driver);
+}
+module_exit(max17048_exit);
+
+MODULE_AUTHOR("Chandler Zhang <chazhang@nvidia.com>");
+MODULE_DESCRIPTION("MAX17048 Fuel Gauge");
+MODULE_LICENSE("GPL");
diff --git a/drivers/power/max8907c-charger.c b/drivers/power/max8907c-charger.c
new file mode 100644
index 000000000000..64855c589b15
--- /dev/null
+++ b/drivers/power/max8907c-charger.c
@@ -0,0 +1,228 @@
+/*
+ * Battery driver for Maxim MAX8907C
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ * Copyright (C) 2010 Gyungoh Yoo <jack.yoo@maxim-ic.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/mfd/max8907c.h>
+#include <linux/power/max8907c-charger.h>
+#include <linux/slab.h>
+
+struct max8907c_charger {
+	struct max8907c_charger_pdata *pdata;
+	struct max8907c *chip;
+	struct i2c_client *i2c;
+	int online;
+};
+
+static void max8907c_set_charger(struct max8907c_charger *charger)
+{
+	struct max8907c_charger_pdata *pdata = charger->pdata;
+	int ret;
+	if (charger->online) {
+		ret = max8907c_reg_write(charger->i2c, MAX8907C_REG_CHG_CNTL1,
+					 (pdata->topoff_threshold << 5) |
+					 (pdata->restart_hysteresis << 3) |
+					 (pdata->fast_charging_current));
+		if (unlikely(ret != 0))
+			pr_err("Failed to set CHG_CNTL1: %d\n", ret);
+
+		ret = max8907c_set_bits(charger->i2c, MAX8907C_REG_CHG_CNTL2,
+					0x30, pdata->fast_charger_time << 4);
+		if (unlikely(ret != 0))
+			pr_err("Failed to set CHG_CNTL2: %d\n", ret);
+	} else {
+		ret = max8907c_set_bits(charger->i2c, MAX8907C_REG_CHG_CNTL1, 0x80, 0x1);
+		if (unlikely(ret != 0))
+			pr_err("Failed to set CHG_CNTL1: %d\n", ret);
+	}
+}
+
+static irqreturn_t max8907c_charger_isr(int irq, void *dev_id)
+{
+	struct max8907c_charger *charger = dev_id;
+	struct max8907c *chip = charger->chip;
+
+	switch (irq - chip->irq_base) {
+	case MAX8907C_IRQ_VCHG_DC_R:
+		charger->online = 1;
+		max8907c_set_charger(charger);
+		break;
+	case MAX8907C_IRQ_VCHG_DC_F:
+		charger->online = 0;
+		max8907c_set_charger(charger);
+		break;
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int max8907c_charger_get_property(struct power_supply *psy,
+				enum power_supply_property psp,
+				union power_supply_propval *val)
+{
+	const static int types[] = {
+		POWER_SUPPLY_CHARGE_TYPE_TRICKLE,
+		POWER_SUPPLY_CHARGE_TYPE_FAST,
+		POWER_SUPPLY_CHARGE_TYPE_FAST,
+		POWER_SUPPLY_CHARGE_TYPE_NONE,
+	};
+	int ret = -ENODEV;
+	int status;
+
+	struct max8907c_charger *charger = dev_get_drvdata(psy->dev->parent);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = charger->online;
+		ret = 0;
+		break;
+
+	case POWER_SUPPLY_PROP_STATUS:
+		/* Get charger status from CHG_EN_STAT */
+		status = max8907c_reg_read(charger->i2c, MAX8907C_REG_CHG_STAT);
+		val->intval = ((status & 0x10) == 0x10) ?
+				POWER_SUPPLY_STATUS_CHARGING :
+				POWER_SUPPLY_STATUS_NOT_CHARGING;
+		ret = 0;
+		break;
+
+	case POWER_SUPPLY_PROP_CHARGE_TYPE:
+		/* Get charging type from CHG_MODE */
+		status = max8907c_reg_read(charger->i2c, MAX8907C_REG_CHG_STAT);
+		val->intval = types[(status & 0x0C) >> 2];
+		ret = 0;
+		break;
+
+	default:
+		val->intval = 0;
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+static enum power_supply_property max8907c_charger_props[] = {
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_CHARGE_TYPE,
+};
+
+static struct power_supply max8907c_charger_ps = {
+	.name = "charger",
+	.type = POWER_SUPPLY_TYPE_MAINS,
+	.properties = max8907c_charger_props,
+	.num_properties = ARRAY_SIZE(max8907c_charger_props),
+	.get_property = max8907c_charger_get_property,
+};
+
+static __devinit int max8907c_charger_probe(struct platform_device *pdev)
+{
+	struct max8907c_charger_pdata *pdata = pdev->dev.platform_data;
+	struct max8907c_charger *charger = 0;
+	struct max8907c *chip = dev_get_drvdata(pdev->dev.parent);
+	int ret;
+
+	charger = kzalloc(sizeof(*charger), GFP_KERNEL);
+	if (!charger)
+		return -ENOMEM;
+
+	charger->pdata = pdata;
+	charger->online = 0;
+	charger->chip = chip;
+	charger->i2c = chip->i2c_power;
+
+	platform_set_drvdata(pdev, charger);
+
+	ret = max8907c_reg_read(charger->i2c, MAX8907C_REG_CHG_STAT);
+	if (ret & (1 << 7)) {
+		charger->online = 1;
+		max8907c_set_charger(charger);
+	}
+
+	ret = request_threaded_irq(chip->irq_base + MAX8907C_IRQ_VCHG_DC_F, NULL,
+				   max8907c_charger_isr, IRQF_ONESHOT,
+				   "power-remove", charger);
+	if (unlikely(ret < 0)) {
+		pr_debug("max8907c: failed to request IRQ	%X\n", ret);
+		goto out;
+	}
+
+	ret = request_threaded_irq(chip->irq_base + MAX8907C_IRQ_VCHG_DC_R, NULL,
+				   max8907c_charger_isr, IRQF_ONESHOT,
+				   "power-insert", charger);
+	if (unlikely(ret < 0)) {
+		pr_debug("max8907c: failed to request IRQ	%X\n", ret);
+		goto out1;
+	}
+
+
+	ret = power_supply_register(&pdev->dev, &max8907c_charger_ps);
+	if (unlikely(ret != 0)) {
+		pr_err("Failed to register max8907c_charger driver: %d\n", ret);
+		goto out2;
+	}
+
+	return 0;
+out2:
+	free_irq(chip->irq_base + MAX8907C_IRQ_VCHG_DC_R, charger);
+out1:
+	free_irq(chip->irq_base + MAX8907C_IRQ_VCHG_DC_F, charger);
+out:
+	kfree(charger);
+	return ret;
+}
+
+static __devexit int max8907c_charger_remove(struct platform_device *pdev)
+{
+	struct max8907c_charger *charger = platform_get_drvdata(pdev);
+	struct max8907c *chip = charger->chip;
+	int ret;
+
+	ret = max8907c_reg_write(charger->i2c, MAX8907C_REG_CHG_IRQ1_MASK, 0xFF);
+	if (unlikely(ret != 0)) {
+		pr_err("Failed to set IRQ1_MASK: %d\n", ret);
+		goto out;
+	}
+
+	free_irq(chip->irq_base + MAX8907C_IRQ_VCHG_DC_R, charger);
+	free_irq(chip->irq_base + MAX8907C_IRQ_VCHG_DC_F, charger);
+	power_supply_unregister(&max8907c_charger_ps);
+out:
+	kfree(charger);
+	return 0;
+}
+
+static struct platform_driver max8907c_charger_driver = {
+	.probe		= max8907c_charger_probe,
+	.remove		= __devexit_p(max8907c_charger_remove),
+	.driver		= {
+		.name	= "max8907c-charger",
+	},
+};
+
+static int __init max8907c_charger_init(void)
+{
+	return platform_driver_register(&max8907c_charger_driver);
+}
+module_init(max8907c_charger_init);
+
+static void __exit max8907c_charger_exit(void)
+{
+	platform_driver_unregister(&max8907c_charger_driver);
+}
+module_exit(max8907c_charger_exit);
+
+MODULE_DESCRIPTION("Charger driver for MAX8907C");
+MODULE_AUTHOR("Gyungoh Yoo <jack.yoo@maxim-ic.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/power/pda_power.c b/drivers/power/pda_power.c
index 69f8aa3a6a4b..81b720107c3a 100644
--- a/drivers/power/pda_power.c
+++ b/drivers/power/pda_power.c
@@ -14,6 +14,7 @@
 #include <linux/platform_device.h>
 #include <linux/err.h>
 #include <linux/interrupt.h>
+#include <linux/notifier.h>
 #include <linux/power_supply.h>
 #include <linux/pda_power.h>
 #include <linux/regulator/consumer.h>
@@ -38,9 +39,8 @@ static struct timer_list supply_timer;
 static struct timer_list polling_timer;
 static int polling;
 
-#ifdef CONFIG_USB_OTG_UTILS
 static struct otg_transceiver *transceiver;
-#endif
+static struct notifier_block otg_nb;
 static struct regulator *ac_draw;
 
 enum {
@@ -222,7 +222,42 @@ static void polling_timer_func(unsigned long unused)
 #ifdef CONFIG_USB_OTG_UTILS
 static int otg_is_usb_online(void)
 {
-	return (transceiver->state == OTG_STATE_B_PERIPHERAL);
+	return (transceiver->last_event == USB_EVENT_VBUS ||
+		transceiver->last_event == USB_EVENT_ENUMERATED);
+}
+
+static int otg_is_ac_online(void)
+{
+	return (transceiver->last_event == USB_EVENT_CHARGER);
+}
+
+static int otg_handle_notification(struct notifier_block *nb,
+		unsigned long event, void *unused)
+{
+	switch (event) {
+	case USB_EVENT_CHARGER:
+		ac_status = PDA_PSY_TO_CHANGE;
+		break;
+	case USB_EVENT_VBUS:
+	case USB_EVENT_ENUMERATED:
+		usb_status = PDA_PSY_TO_CHANGE;
+		break;
+	case USB_EVENT_NONE:
+		ac_status = PDA_PSY_TO_CHANGE;
+		usb_status = PDA_PSY_TO_CHANGE;
+		break;
+	default:
+		return NOTIFY_OK;
+	}
+
+	/*
+	 * Wait a bit before reading ac/usb line status and setting charger,
+	 * because ac/usb status readings may lag from irq.
+	 */
+	mod_timer(&charger_timer,
+		  jiffies + msecs_to_jiffies(pdata->wait_for_status));
+
+	return NOTIFY_OK;
 }
 #endif
 
@@ -282,6 +317,14 @@ static int pda_power_probe(struct platform_device *pdev)
 		ret = PTR_ERR(ac_draw);
 	}
 
+	transceiver = otg_get_transceiver();
+	if (transceiver && !pdata->is_usb_online) {
+		pdata->is_usb_online = otg_is_usb_online;
+	}
+	if (transceiver && !pdata->is_ac_online) {
+		pdata->is_ac_online = otg_is_ac_online;
+	}
+
 	if (pdata->is_ac_online) {
 		ret = power_supply_register(&pdev->dev, &pda_psy_ac);
 		if (ret) {
@@ -303,13 +346,6 @@ static int pda_power_probe(struct platform_device *pdev)
 		}
 	}
 
-#ifdef CONFIG_USB_OTG_UTILS
-	transceiver = otg_get_transceiver();
-	if (transceiver && !pdata->is_usb_online) {
-		pdata->is_usb_online = otg_is_usb_online;
-	}
-#endif
-
 	if (pdata->is_usb_online) {
 		ret = power_supply_register(&pdev->dev, &pda_psy_usb);
 		if (ret) {
@@ -331,6 +367,16 @@ static int pda_power_probe(struct platform_device *pdev)
 		}
 	}
 
+	if (transceiver && pdata->use_otg_notifier) {
+		otg_nb.notifier_call = otg_handle_notification;
+		ret = otg_register_notifier(transceiver, &otg_nb);
+		if (ret) {
+			dev_err(dev, "failure to register otg notifier\n");
+			goto otg_reg_notifier_failed;
+		}
+		polling = 0;
+	}
+
 	if (polling) {
 		dev_dbg(dev, "will poll for status\n");
 		setup_timer(&polling_timer, polling_timer_func, 0);
@@ -343,16 +389,17 @@ static int pda_power_probe(struct platform_device *pdev)
 
 	return 0;
 
+otg_reg_notifier_failed:
+	if (pdata->is_usb_online && usb_irq)
+		free_irq(usb_irq->start, &pda_psy_usb);
 usb_irq_failed:
 	if (pdata->is_usb_online)
 		power_supply_unregister(&pda_psy_usb);
 usb_supply_failed:
 	if (pdata->is_ac_online && ac_irq)
 		free_irq(ac_irq->start, &pda_psy_ac);
-#ifdef CONFIG_USB_OTG_UTILS
 	if (transceiver)
 		otg_put_transceiver(transceiver);
-#endif
 ac_irq_failed:
 	if (pdata->is_ac_online)
 		power_supply_unregister(&pda_psy_ac);
diff --git a/drivers/power/power_supply_core.c b/drivers/power/power_supply_core.c
index 329b46b2327d..03810ce5633f 100644
--- a/drivers/power/power_supply_core.c
+++ b/drivers/power/power_supply_core.c
@@ -41,23 +41,40 @@ static int __power_supply_changed_work(struct device *dev, void *data)
 
 static void power_supply_changed_work(struct work_struct *work)
 {
+	unsigned long flags;
 	struct power_supply *psy = container_of(work, struct power_supply,
 						changed_work);
 
 	dev_dbg(psy->dev, "%s\n", __func__);
 
-	class_for_each_device(power_supply_class, NULL, psy,
-			      __power_supply_changed_work);
+	spin_lock_irqsave(&psy->changed_lock, flags);
+	if (psy->changed) {
+		psy->changed = false;
+		spin_unlock_irqrestore(&psy->changed_lock, flags);
 
-	power_supply_update_leds(psy);
+		class_for_each_device(power_supply_class, NULL, psy,
+				      __power_supply_changed_work);
 
-	kobject_uevent(&psy->dev->kobj, KOBJ_CHANGE);
+		power_supply_update_leds(psy);
+
+		kobject_uevent(&psy->dev->kobj, KOBJ_CHANGE);
+		spin_lock_irqsave(&psy->changed_lock, flags);
+	}
+	if (!psy->changed)
+		wake_unlock(&psy->work_wake_lock);
+	spin_unlock_irqrestore(&psy->changed_lock, flags);
 }
 
 void power_supply_changed(struct power_supply *psy)
 {
+	unsigned long flags;
+
 	dev_dbg(psy->dev, "%s\n", __func__);
 
+	spin_lock_irqsave(&psy->changed_lock, flags);
+	psy->changed = true;
+	wake_lock(&psy->work_wake_lock);
+	spin_unlock_irqrestore(&psy->changed_lock, flags);
 	schedule_work(&psy->changed_work);
 }
 EXPORT_SYMBOL_GPL(power_supply_changed);
@@ -181,6 +198,9 @@ int power_supply_register(struct device *parent, struct power_supply *psy)
 	if (rc)
 		goto device_add_failed;
 
+	spin_lock_init(&psy->changed_lock);
+	wake_lock_init(&psy->work_wake_lock, WAKE_LOCK_SUSPEND, "power-supply");
+
 	rc = power_supply_create_triggers(psy);
 	if (rc)
 		goto create_triggers_failed;
@@ -190,6 +210,7 @@ int power_supply_register(struct device *parent, struct power_supply *psy)
 	goto success;
 
 create_triggers_failed:
+	wake_lock_destroy(&psy->work_wake_lock);
 	device_del(dev);
 kobject_set_name_failed:
 device_add_failed:
@@ -203,6 +224,7 @@ void power_supply_unregister(struct power_supply *psy)
 {
 	cancel_work_sync(&psy->changed_work);
 	power_supply_remove_triggers(psy);
+	wake_lock_destroy(&psy->work_wake_lock);
 	device_unregister(psy->dev);
 }
 EXPORT_SYMBOL_GPL(power_supply_unregister);
diff --git a/drivers/power/smb349-charger.c b/drivers/power/smb349-charger.c
new file mode 100644
index 000000000000..c6dc43158b82
--- /dev/null
+++ b/drivers/power/smb349-charger.c
@@ -0,0 +1,742 @@
+/*
+ * drivers/power/smb349-charger.c
+ *
+ * Battery charger driver for smb349 from summit microelectronics
+ *
+ * Copyright (c) 2012, NVIDIA 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;
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/mutex.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/power_supply.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/smb349-charger.h>
+#include <linux/slab.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/usb/otg.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+
+#define SMB349_CHARGE		0x00
+#define SMB349_CHRG_CRNTS	0x01
+#define SMB349_VRS_FUNC		0x02
+#define SMB349_FLOAT_VLTG	0x03
+#define SMB349_CHRG_CTRL	0x04
+#define SMB349_STAT_TIME_CTRL	0x05
+#define SMB349_PIN_CTRL		0x06
+#define SMB349_THERM_CTRL	0x07
+#define SMB349_CTRL_REG		0x09
+
+#define SMB349_OTG_TLIM_REG	0x0A
+#define SMB349_HRD_SFT_TEMP	0x0B
+#define SMB349_FAULT_INTR	0x0C
+#define SMB349_STS_INTR_1	0x0D
+#define SMB349_SYSOK_USB3	0x0E
+#define SMB349_IN_CLTG_DET	0x10
+#define SMB349_STS_INTR_2	0x11
+
+#define SMB349_CMD_REG		0x30
+#define SMB349_CMD_REG_B	0x31
+#define SMB349_CMD_REG_c	0x33
+
+#define SMB349_INTR_STS_A	0x35
+#define SMB349_INTR_STS_B	0x36
+#define SMB349_INTR_STS_C	0x37
+#define SMB349_INTR_STS_D	0x38
+#define SMB349_INTR_STS_E	0x39
+#define SMB349_INTR_STS_F	0x3A
+
+#define SMB349_STS_REG_A	0x3B
+#define SMB349_STS_REG_B	0x3C
+#define SMB349_STS_REG_C	0x3D
+#define SMB349_STS_REG_D	0x3E
+#define SMB349_STS_REG_E	0x3F
+
+#define SMB349_ENABLE_WRITE	1
+#define SMB349_DISABLE_WRITE	0
+#define ENABLE_WRT_ACCESS	0x80
+#define THERM_CTRL		0x10
+#define BATTERY_MISSING		0x10
+#define CHARGING		0x06
+#define DEDICATED_CHARGER	0x04
+#define CHRG_DOWNSTRM_PORT	0x08
+#define ENABLE_CHARGE		0x02
+
+static struct smb349_charger *charger;
+static int smb349_configure_charger(struct i2c_client *client, int value);
+
+static int smb349_read(struct i2c_client *client, int reg)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, reg);
+
+	if (ret < 0)
+		dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+
+	return ret;
+}
+
+static int smb349_write(struct i2c_client *client, int reg, u8 value)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, reg, value);
+
+	if (ret < 0)
+		dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+
+	return ret;
+}
+
+static int smb349_update_reg(struct i2c_client *client, int reg, u8 value)
+{
+	int ret, retval;
+
+	retval = smb349_read(client, reg);
+	if (retval < 0) {
+		dev_err(&client->dev, "%s: err %d\n", __func__, retval);
+		return retval;
+	}
+
+	ret = smb349_write(client, reg, retval | value);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+		return ret;
+	}
+
+	return ret;
+}
+
+int smb349_volatile_writes(struct i2c_client *client, uint8_t value)
+{
+	int ret = 0;
+
+	if (value == SMB349_ENABLE_WRITE) {
+		/* Enable volatile write to config registers */
+		ret = smb349_update_reg(client, SMB349_CMD_REG,
+						ENABLE_WRT_ACCESS);
+		if (ret < 0) {
+			dev_err(&client->dev, "%s(): Failed in writing"
+				"register 0x%02x\n", __func__, SMB349_CMD_REG);
+			return ret;
+		}
+	} else {
+		ret = smb349_read(client, SMB349_CMD_REG);
+		if (ret < 0) {
+			dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+			return ret;
+		}
+
+		ret = smb349_write(client, SMB349_CMD_REG, ret & (~(1<<7)));
+		if (ret < 0) {
+			dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+			return ret;
+		}
+	}
+	return ret;
+}
+
+static int smb349_configure_otg(struct i2c_client *client, int enable)
+{
+	int ret = 0;
+
+	/*Enable volatile writes to registers*/
+	ret = smb349_volatile_writes(client, SMB349_ENABLE_WRITE);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s error in configuring otg..\n",
+								__func__);
+		goto error;
+	}
+
+	if (enable) {
+		/* Configure PGOOD to be active low if no 5V on VBUS */
+		ret = smb349_read(client, SMB349_STS_REG_C);
+		if (ret < 0) {
+			dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+			goto error;
+		}
+
+		if (!(ret & 0x01)) {
+			ret = smb349_read(client, SMB349_SYSOK_USB3);
+			if (ret < 0) {
+				dev_err(&client->dev, "%s: err %d\n",
+					__func__, ret);
+				goto error;
+			}
+
+			ret = smb349_write(client, SMB349_SYSOK_USB3,
+						(ret & (~(1))));
+			if (ret < 0) {
+				dev_err(&client->dev, "%s: err %d\n",
+					__func__, ret);
+				goto error;
+			}
+		}
+
+		/* Enable OTG */
+		ret = smb349_update_reg(client, SMB349_CMD_REG, 0x10);
+		if (ret < 0) {
+			dev_err(&client->dev, "%s: Failed in writing register"
+				"0x%02x\n", __func__, SMB349_CMD_REG);
+			goto error;
+		}
+	} else {
+		/* Disable OTG */
+		ret = smb349_read(client, SMB349_CMD_REG);
+		if (ret < 0) {
+			dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+			goto error;
+		}
+
+		ret = smb349_write(client, SMB349_CMD_REG, (ret & (~(1<<4))));
+		if (ret < 0) {
+			dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+			goto error;
+		}
+
+		/* Configure PGOOD to be active high */
+		ret = smb349_update_reg(client, SMB349_SYSOK_USB3, 0x01);
+		if (ret < 0) {
+			dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+			goto error;
+		}
+	}
+
+	/* Disable volatile writes to registers */
+	ret = smb349_volatile_writes(client, SMB349_DISABLE_WRITE);
+	if (ret < 0)
+		dev_err(&client->dev, "%s error in configuring OTG..\n",
+								__func__);
+error:
+	return ret;
+}
+
+static int smb349_configure_charger(struct i2c_client *client, int value)
+{
+	int ret = 0;
+
+	/* Enable volatile writes to registers */
+	ret = smb349_volatile_writes(client, SMB349_ENABLE_WRITE);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s() error in configuring charger..\n",
+								__func__);
+		goto error;
+	}
+
+	if (value) {
+		 /* Enable charging */
+		ret = smb349_update_reg(client, SMB349_CMD_REG, ENABLE_CHARGE);
+		if (ret < 0) {
+			dev_err(&client->dev, "%s(): Failed in writing register"
+					"0x%02x\n", __func__, SMB349_CMD_REG);
+			goto error;
+		}
+
+		/* Configure THERM ctrl */
+		ret = smb349_update_reg(client, SMB349_THERM_CTRL, THERM_CTRL);
+		if (ret < 0) {
+			dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+			goto error;
+		}
+	} else {
+		ret = smb349_read(client, SMB349_CMD_REG);
+		if (ret < 0) {
+			dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+			goto error;
+		}
+
+		ret = smb349_write(client, SMB349_CMD_REG, (ret & (~(1<<1))));
+		if (ret < 0) {
+			dev_err(&client->dev, "%s: err %d\n", __func__, ret);
+			goto error;
+		}
+	}
+	/* Disable volatile writes to registers */
+	ret = smb349_volatile_writes(client, SMB349_DISABLE_WRITE);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s() error in configuring charger..\n",
+								__func__);
+		goto error;
+	}
+error:
+	return ret;
+}
+
+int update_charger_status(void)
+{
+	struct i2c_client *client;
+	int val;
+
+	if (!charger)
+		return -ENODEV;
+	else
+		client = charger->client;
+
+	val =  smb349_read(client, SMB349_STS_REG_D);
+	if (val < 0) {
+		dev_err(&client->dev, "%s(): Failed in reading register"
+			"0x%02x\n", __func__, SMB349_STS_REG_D);
+		goto val_error;
+	} else if (val != 0) {
+		if (val & DEDICATED_CHARGER)
+			charger->chrg_type = AC;
+		else
+			charger->chrg_type = USB;
+
+		charger->state = progress;
+	} else {
+		charger->state = stopped;
+	}
+
+	if (charger->charger_cb)
+		charger->charger_cb(charger->state, charger->chrg_type,
+						charger->charger_cb_data);
+	return 0;
+val_error:
+	return val;
+}
+EXPORT_SYMBOL_GPL(update_charger_status);
+
+int register_callback(charging_callback_t cb, void *args)
+{
+	struct smb349_charger *charger_data = charger;
+	if (!charger_data)
+		return -ENODEV;
+
+	charger_data->charger_cb = cb;
+	charger_data->charger_cb_data = args;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(register_callback);
+
+int smb349_battery_online(void)
+{
+	int val;
+	struct i2c_client *client = charger->client;
+
+	val = smb349_read(charger->client, SMB349_INTR_STS_B);
+	if (val < 0) {
+		dev_err(&client->dev, "%s(): Failed in reading register"
+				"0x%02x\n", __func__, SMB349_INTR_STS_B);
+		return val;
+	}
+	if (val & BATTERY_MISSING)
+		return 0;
+	else
+		return 1;
+}
+
+static int smb349_enable_otg(struct regulator_dev *otg_rdev)
+{
+	struct i2c_client *client = charger->client;
+	int ret;
+
+	/* configure charger */
+	ret = smb349_configure_charger(client, 0);
+	if (ret < 0)
+		goto error;
+	/* ENABLE OTG */
+	ret = smb349_configure_otg(client, 1);
+	if (ret < 0)
+		goto error;
+
+	charger->is_otg_enabled = 1;
+	return 0;
+error:
+	dev_err(&client->dev, "%s() error in enabling"
+			"otg..\n", __func__);
+	return ret;
+}
+
+static int smb349_disable_otg(struct regulator_dev *otg_rdev)
+{
+	struct i2c_client *client = charger->client;
+	int ret;
+
+	/* Disable OTG */
+	ret = smb349_configure_otg(client, 0);
+	if (ret < 0)
+		goto error;
+	/* configure charger */
+	ret = smb349_configure_charger(client, 1);
+	if (ret < 0)
+		goto error;
+
+	charger->is_otg_enabled = 0;
+	return 0;
+error:
+	dev_err(&client->dev, "%s() error in disabling"
+			"otg..\n", __func__);
+	return ret;
+}
+
+static int smb349_is_otg_enabled(struct regulator_dev *otg_rdev)
+{
+	return charger->is_otg_enabled;
+}
+
+static int smb349_enable_charging(struct regulator_dev *rdev,
+					int min_uA, int max_uA)
+{
+	struct i2c_client *client = charger->client;
+	int ret;
+
+	if (!max_uA) {
+		/* Wait for SMB349 to debounce and get reset to POR when cable is unpluged */
+		msleep(50);
+
+		ret =  smb349_read(client, SMB349_STS_REG_C);
+		if (ret < 0) {
+			dev_err(&client->dev, "%s(): Failed in reading register"
+				"0x%02x\n", __func__, SMB349_STS_REG_C);
+			return ret;
+		}
+
+		if (ret & CHARGING)
+			return 0;
+
+		charger->state = stopped;
+		charger->chrg_type = NONE;
+	} else {
+		/* Wait for SMB349 to reload OTP setting and detect type*/
+		msleep(500);
+
+		ret =  smb349_read(client, SMB349_STS_REG_D);
+		if (ret < 0) {
+			dev_err(&client->dev, "%s(): Failed in reading register"
+					"0x%02x\n", __func__, SMB349_STS_REG_D);
+			return ret;
+		} else if (ret != 0) {
+			if (ret & DEDICATED_CHARGER)
+				charger->chrg_type = AC;
+			else
+				charger->chrg_type = USB;
+
+			/* configure charger */
+			ret = smb349_configure_charger(client, 1);
+			if (ret < 0) {
+				dev_err(&client->dev, "%s() error in"
+					"configuring charger..\n", __func__);
+				return ret;
+			}
+			charger->state = progress;
+		}
+	}
+	if (charger->charger_cb)
+		charger->charger_cb(charger->state, charger->chrg_type,
+						charger->charger_cb_data);
+	return 0;
+}
+
+static struct regulator_ops smb349_tegra_regulator_ops = {
+	.set_current_limit = smb349_enable_charging,
+};
+
+static struct regulator_ops smb349_tegra_otg_regulator_ops = {
+	.enable = smb349_enable_otg,
+	.disable = smb349_disable_otg,
+	.is_enabled = smb349_is_otg_enabled,
+};
+
+#if defined(CONFIG_DEBUG_FS)
+static struct dentry *smb349_dentry_regs;
+
+static int smb349_dump_regs(struct i2c_client *client, u8 *addrs, int num_addrs,
+		char *buf, ssize_t *len)
+{
+	ssize_t count = *len;
+	int ret = 0;
+	int i;
+
+	if (count >= PAGE_SIZE - 1)
+		return -ERANGE;
+
+	for (i = 0; i < num_addrs; i++) {
+		count += sprintf(buf + count, "0x%02x: ", addrs[i]);
+		if (count >= PAGE_SIZE - 1)
+			return -ERANGE;
+
+		ret = smb349_read(client, addrs[i]);
+		if (ret < 0)
+			count += sprintf(buf + count, "<read fail: %d\n", ret);
+		else
+			count += sprintf(buf + count, "0x%02x\n", ret);
+
+		if (count >= PAGE_SIZE - 1)
+			return -ERANGE;
+	}
+	*len = count;
+
+	return 0;
+}
+
+static ssize_t smb349_debugfs_open(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+
+static u8 regs[] = {
+	0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x09, 0x0A, 0x0B,
+	0x0C, 0x0D, 0x0E, 0x10, 0x11, 0x12, 0x30, 0x31, 0x33, 0x35, 0x36,
+	0x37, 0x38, 0x39, 0x3A, 0x3B, 0x3C, 0x3D, 0x3E, 0x3F
+};
+static ssize_t smb349_debugfs_read(struct file *file, char __user *user_buf,
+				size_t count, loff_t *ppos)
+{
+	ssize_t ret;
+	struct i2c_client *client = file->private_data;
+	char *buf;
+	size_t len = 0;
+
+	buf = kzalloc(PAGE_SIZE, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	len += sprintf(buf + len, "SMB349 Registers\n");
+	smb349_dump_regs(client, regs, ARRAY_SIZE(regs), buf, &len);
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, len);
+	kfree(buf);
+	return ret;
+}
+
+static const struct file_operations smb349_debugfs_fops = {
+	.open = smb349_debugfs_open,
+	.read = smb349_debugfs_read,
+};
+
+static void smb349_debugfs_init(struct i2c_client *client)
+{
+	smb349_dentry_regs = debugfs_create_file(client->name,
+						0444, 0, client,
+						&smb349_debugfs_fops);
+}
+
+static void smb349_debugfs_exit(struct i2c_client *client)
+{
+	debugfs_remove(smb349_dentry_regs);
+}
+#else
+static void smb349_debugfs_init(struct i2c_client *client){}
+static void smb349_debugfs_exit(struct i2c_client *client){}
+#endif /* CONFIG_DEBUG_FS */
+
+static int __devinit smb349_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct smb349_charger_platform_data *pdata;
+	int ret;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE))
+		return -EIO;
+
+	charger = kzalloc(sizeof(*charger), GFP_KERNEL);
+	if (!charger)
+		return -ENOMEM;
+
+	charger->client = client;
+	charger->dev = &client->dev;
+	pdata = client->dev.platform_data;
+	if(!pdata) {
+		ret = -ENXIO;
+		goto error;
+	}
+
+	i2c_set_clientdata(client, charger);
+
+	/* Check battery presence */
+	if (!smb349_battery_online()) {
+		dev_err(&client->dev, "%s() No Battery present, exiting..\n",
+					__func__);
+		ret = -ENODEV;
+		goto regulator_error;
+	}
+
+	charger->is_otg_enabled = 0;
+
+	charger->reg_desc.name  = "vbus_charger";
+	charger->reg_desc.ops   = &smb349_tegra_regulator_ops;
+	charger->reg_desc.type  = REGULATOR_CURRENT;
+	charger->reg_desc.id    = pdata->regulator_id;
+	charger->reg_desc.owner = THIS_MODULE;
+
+	charger->reg_init_data.supply_regulator         = NULL;
+	charger->reg_init_data.num_consumer_supplies    =
+				       pdata->num_consumer_supplies;
+	charger->reg_init_data.regulator_init           = NULL;
+	charger->reg_init_data.consumer_supplies        =
+				       pdata->consumer_supplies;
+	charger->reg_init_data.driver_data              = charger;
+	charger->reg_init_data.constraints.name         = "vbus_charger";
+	charger->reg_init_data.constraints.min_uA       = 0;
+	charger->reg_init_data.constraints.max_uA       =
+					pdata->max_charge_current_mA * 1000;
+
+	charger->reg_init_data.constraints.valid_modes_mask =
+					REGULATOR_MODE_NORMAL |
+					REGULATOR_MODE_STANDBY;
+
+	charger->reg_init_data.constraints.valid_ops_mask =
+					REGULATOR_CHANGE_MODE |
+					REGULATOR_CHANGE_STATUS |
+					REGULATOR_CHANGE_CURRENT;
+
+	charger->rdev = regulator_register(&charger->reg_desc, charger->dev,
+					&charger->reg_init_data, charger);
+
+	smb349_debugfs_init(client);
+
+	if (IS_ERR(charger->rdev)) {
+		dev_err(&client->dev, "failed to register %s\n",
+				charger->reg_desc.name);
+		ret = PTR_ERR(charger->rdev);
+		goto regulator_error;
+	}
+
+	charger->otg_reg_desc.name  = "vbus_otg";
+	charger->otg_reg_desc.ops   = &smb349_tegra_otg_regulator_ops;
+	charger->otg_reg_desc.type  = REGULATOR_CURRENT;
+	charger->otg_reg_desc.id    = pdata->otg_regulator_id;
+	charger->otg_reg_desc.type  = REGULATOR_CURRENT;
+	charger->otg_reg_desc.owner = THIS_MODULE;
+
+	charger->otg_reg_init_data.supply_regulator         = NULL;
+	charger->otg_reg_init_data.num_consumer_supplies    =
+				       pdata->num_otg_consumer_supplies;
+	charger->otg_reg_init_data.regulator_init           = NULL;
+	charger->otg_reg_init_data.consumer_supplies        =
+				       pdata->otg_consumer_supplies;
+	charger->otg_reg_init_data.driver_data              = charger;
+	charger->otg_reg_init_data.constraints.name         = "vbus_otg";
+	charger->otg_reg_init_data.constraints.min_uA       = 0;
+	charger->otg_reg_init_data.constraints.max_uA       = 500000;
+
+	charger->otg_reg_init_data.constraints.valid_modes_mask =
+					REGULATOR_MODE_NORMAL |
+					REGULATOR_MODE_STANDBY;
+
+	charger->otg_reg_init_data.constraints.valid_ops_mask =
+					REGULATOR_CHANGE_MODE |
+					REGULATOR_CHANGE_STATUS |
+					REGULATOR_CHANGE_CURRENT;
+
+	charger->otg_rdev = regulator_register(&charger->otg_reg_desc, charger->dev,
+					&charger->otg_reg_init_data, charger);
+	if (IS_ERR(charger->otg_rdev)) {
+		dev_err(&client->dev, "failed to register %s\n",
+				charger->otg_reg_desc.name);
+		ret = PTR_ERR(charger->otg_rdev);
+		goto otg_regulator_error;
+	}
+
+	/* disable OTG */
+	ret = smb349_configure_otg(client, 0);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s() error in configuring"
+			"charger..\n", __func__);
+		goto error;
+	}
+
+	ret =  smb349_read(client, SMB349_STS_REG_D);
+	if (ret < 0) {
+		dev_err(&client->dev, "%s(): Failed in reading register"
+			"0x%02x\n", __func__, SMB349_STS_REG_D);
+		goto error;
+	} else if (ret != 0) {
+		/* configure charger */
+		ret = smb349_configure_charger(client, 1);
+		if (ret < 0) {
+			dev_err(&client->dev, "%s() error in configuring"
+				"charger..\n", __func__);
+			goto error;
+		}
+	} else {
+		/* disable charger */
+		ret = smb349_configure_charger(client, 0);
+		if (ret < 0) {
+			dev_err(&client->dev, "%s() error in configuring"
+				"charger..\n", __func__);
+			goto error;
+		}
+	}
+
+	return 0;
+error:
+	smb349_debugfs_exit(client);
+	regulator_unregister(charger->otg_rdev);
+otg_regulator_error:
+	regulator_unregister(charger->rdev);
+regulator_error:
+	kfree(charger);
+	charger = NULL;
+	return ret;
+}
+
+static int __devexit smb349_remove(struct i2c_client *client)
+{
+	struct smb349_charger *charger = i2c_get_clientdata(client);
+
+	smb349_debugfs_exit(client);
+	regulator_unregister(charger->rdev);
+	regulator_unregister(charger->otg_rdev);
+	kfree(charger);
+
+	return 0;
+}
+
+static const struct i2c_device_id smb349_id[] = {
+	{ "smb349", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, smb349_id);
+
+static struct i2c_driver smb349_i2c_driver = {
+	.driver	= {
+		.name	= "smb349",
+	},
+	.probe		= smb349_probe,
+	.remove		= __devexit_p(smb349_remove),
+	.id_table	= smb349_id,
+};
+
+static int __init smb349_init(void)
+{
+	return i2c_add_driver(&smb349_i2c_driver);
+}
+subsys_initcall(smb349_init);
+
+static void __exit smb349_exit(void)
+{
+	i2c_del_driver(&smb349_i2c_driver);
+}
+module_exit(smb349_exit);
+
+MODULE_AUTHOR("Syed Rafiuddin <srafiuddin@nvidia.com>");
+MODULE_DESCRIPTION("SMB349 Battery-Charger");
+MODULE_LICENSE("GPL");
diff --git a/drivers/power/tegra_bpc_mgmt.c b/drivers/power/tegra_bpc_mgmt.c
new file mode 100644
index 000000000000..fba0cfef277a
--- /dev/null
+++ b/drivers/power/tegra_bpc_mgmt.c
@@ -0,0 +1,142 @@
+/*
+ * Copyright (C) 2010-2011 NVIDIA Corporation
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/module.h>
+#include <linux/gpio.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/irqflags.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/sched.h>
+#include <linux/jiffies.h>
+#include <linux/platform_data/tegra_bpc_mgmt.h>
+
+#include <mach/edp.h>
+
+static irqreturn_t tegra_bpc_mgmt_bh(int irq, void *data)
+{
+	int ret = -1;
+	int gpio_val = 0;
+	struct tegra_bpc_mgmt_platform_data *bpc_platform_data;
+	bpc_platform_data = (struct tegra_bpc_mgmt_platform_data *)data;
+
+	/**
+	 * Keep on checking whether event has passed or not.
+	 */
+	while (!gpio_val) {
+		if (ret)
+			ret = tegra_system_edp_alarm(true);
+
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(msecs_to_jiffies(
+			bpc_platform_data->bpc_mgmt_timeout));
+
+		gpio_val = gpio_get_value(bpc_platform_data->gpio_trigger);
+	}
+
+	tegra_system_edp_alarm(false);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t tegra_bpc_mgmt_isr(int irq, void *data)
+{
+	tegra_edp_throttle_cpu_now(2);
+	return IRQ_WAKE_THREAD;
+}
+
+static __devinit int tegra_bpc_mgmt_probe(struct platform_device *pdev)
+{
+	u32 ret;
+	struct task_struct *bh_thread;
+	struct irq_desc *bat_desc;
+	struct sched_param param = { .sched_priority = MAX_RT_PRIO-1 };
+	struct tegra_bpc_mgmt_platform_data *bpc_platform_data;
+
+	bpc_platform_data = pdev->dev.platform_data;
+	if (!bpc_platform_data)
+		return -ENODEV;
+
+	if (gpio_is_valid(bpc_platform_data->gpio_trigger)) {
+		ret = gpio_request(bpc_platform_data->gpio_trigger,
+							"tegra-bpc-mgmt");
+
+		if (ret < 0) {
+			pr_err("BPC: GPIO request failed");
+			return -ENODEV;
+		}
+	} else {
+		pr_err("BPC: GPIO check failed, gpio %d",
+					bpc_platform_data->gpio_trigger);
+		return -ENODEV;
+	}
+
+	gpio_direction_input(bpc_platform_data->gpio_trigger);
+
+	ret = request_threaded_irq(
+		  gpio_to_irq(bpc_platform_data->gpio_trigger),
+		  tegra_bpc_mgmt_isr,
+		  tegra_bpc_mgmt_bh, IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+		  "tegra-bpc-mgmt", bpc_platform_data);
+	if (ret < 0) {
+		pr_err("BPC:IRQ Installation failed\n");
+		return -ENODEV;
+	}
+	bat_desc = irq_to_desc(
+		gpio_to_irq(bpc_platform_data->gpio_trigger));
+
+	if (bat_desc) {
+		bh_thread = bat_desc->action->thread;
+		if (bh_thread)
+			sched_setscheduler_nocheck(bh_thread,
+						   SCHED_FIFO, &param);
+	}
+
+	return 0;
+}
+
+static __devexit int tegra_bpc_mgmt_remove(struct platform_device *pdev)
+{
+	struct tegra_bpc_mgmt_platform_data *bpc_platform_data;
+	bpc_platform_data = pdev->dev.platform_data;
+	free_irq(gpio_to_irq(bpc_platform_data->gpio_trigger), NULL);
+	return 0;
+}
+
+static struct platform_driver tegra_bpc_mgmt_driver = {
+	.probe = tegra_bpc_mgmt_probe,
+	.remove = tegra_bpc_mgmt_remove,
+	.driver = {
+		.name = "tegra-bpc-mgmt",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init tegra_bpc_mgmt_init(void)
+{
+	return platform_driver_register(&tegra_bpc_mgmt_driver);
+}
+
+static void __exit tegra_bpc_mgmt_exit(void)
+{
+	platform_driver_unregister(&tegra_bpc_mgmt_driver);
+}
+
+module_init(tegra_bpc_mgmt_init);
+module_exit(tegra_bpc_mgmt_exit);
+
+MODULE_DESCRIPTION("TEGRA Battery Peak current Management");
+MODULE_AUTHOR("NVIDIA");
+MODULE_LICENSE("GPL");
diff --git a/drivers/power/tps80031-charger.c b/drivers/power/tps80031-charger.c
new file mode 100644
index 000000000000..9190b7201fd6
--- /dev/null
+++ b/drivers/power/tps80031-charger.c
@@ -0,0 +1,532 @@
+/*
+ * drivers/power/tps80031_charger.c
+ *
+ * Battery charger driver for TI's tps80031
+ *
+ * Copyright (c) 2011-2012, NVIDIA 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/mfd/tps80031.h>
+#include <linux/tps80031-charger.h>
+
+#define CONTROLLER_CTRL1	0xe1
+#define CONTROLLER_STAT1	0xe3
+#define CHARGERUSB_CTRL2	0xe9
+#define CHARGERUSB_CTRL3	0xea
+#define CHARGERUSB_VOREG	0xec
+#define CHARGERUSB_VICHRG	0xed
+#define CHARGERUSB_CINLIMIT	0xee
+#define CHARGERUSB_CTRLLIMIT2	0xf0
+#define CHARGERUSB_CTRLLIMIT1	0xef
+#define CHARGERUSB_VICHRG_PC	0xdd
+#define CONTROLLER_WDG		0xe2
+#define LINEAR_CHRG_STS		0xde
+
+#define TPS80031_VBUS_DET	BIT(2)
+#define TPS80031_VAC_DET	BIT(3)
+
+struct tps80031_charger {
+	int			max_charge_current_mA;
+	int			max_charge_volt_mV;
+	struct device		*dev;
+	struct regulator_dev	*rdev;
+	struct regulator_desc	reg_desc;
+	struct regulator_init_data		reg_init_data;
+	struct tps80031_charger_platform_data	*pdata;
+	int (*board_init)(void *board_data);
+	void			*board_data;
+	int			irq_base;
+	int			watch_time_sec;
+	enum charging_states	state;
+	int			charging_term_current_mA;
+	charging_callback_t	charger_cb;
+	void			*charger_cb_data;
+};
+
+static struct tps80031_charger *charger_data;
+
+static uint8_t tps80031_get_vbus_input_current_limit_code(int max_uA)
+{
+	const uint8_t current_to_code[] = {
+		0x0,				    /* 0 - 50 mA */
+		0x0,  0x1,  0x2,  0x3,  0x4,  0x5,  /* 50,  100,  ..., 300mA */
+		0x6,  0x7,  0x8,  0x9,  0xA,  0xB,  /* 350, 400,  ..., 600mA */
+		0xC,  0xD,  0xE,  0x27, 0x37, 0x28, /* 650, 700,  ..., 900mA */
+		0x38, 0x29, 0x39, 0x2A, 0x3A, 0x2B, /* 950, 700,  ..., 1200mA */
+		0x3B, 0x2C, 0x3C, 0x2D, 0x3D, 0x2E, /* 1200,1250, ..., 1500mA */
+	};
+	int charge_mA;
+	uint8_t code;
+
+	charge_mA = max_uA / 1000;
+	if (charge_mA < 0)
+		BUG();
+	else if (charge_mA < 1800)
+		code = current_to_code[charge_mA / 50];
+	else if (charge_mA < 2100)
+		code = 0x20; /* use 1800mA code */
+	else if (charge_mA < 2250)
+		code = 0x21; /* use 2100mA code */
+	else
+		code = 0x22; /* use 2250mA code */
+
+	return code;
+};
+
+static int set_charge_current_limit(struct regulator_dev *rdev,
+		int min_uA, int max_uA)
+{
+	struct tps80031_charger *charger = rdev_get_drvdata(rdev);
+	int max_charge_current = 1500;
+	uint8_t code;
+	int ret;
+
+	dev_info(charger->dev, "%s(): Min curr %dmA and max current %dmA\n",
+		__func__, min_uA/1000, max_uA/1000);
+
+	if (!max_uA) {
+		ret = tps80031_write(charger->dev->parent, SLAVE_ID2,
+						CONTROLLER_CTRL1, 0x0);
+		if (ret < 0)
+			dev_err(charger->dev, "%s(): Failed in writing register 0x%02x\n",
+				__func__, CONTROLLER_CTRL1);
+
+		ret = tps80031_write(charger->dev->parent, SLAVE_ID2,
+						CONTROLLER_WDG, 0x0);
+		if (ret < 0)
+			dev_err(charger->dev, "%s(): Failed in writing register 0x%02x\n",
+				__func__, CONTROLLER_WDG);
+		charger->state = charging_state_charging_stopped;
+		if (charger->charger_cb)
+			charger->charger_cb(charger->state,
+					charger->charger_cb_data);
+		return ret;
+	}
+
+	code = tps80031_get_vbus_input_current_limit_code(max_uA);
+	ret = tps80031_update(charger->dev->parent, SLAVE_ID2,
+			CHARGERUSB_CINLIMIT, code, 0x3F);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): Failed in writing register 0x%02x\n",
+				__func__, CHARGERUSB_CINLIMIT);
+		return ret;
+	}
+
+	max_charge_current = min(max_uA/1000, max_charge_current);
+	if (max_charge_current <= 300)
+		max_charge_current = 0;
+	else if ((max_charge_current > 300) && (max_charge_current <= 500))
+		max_charge_current = (max_charge_current - 300)/50;
+	else
+		max_charge_current = (max_charge_current - 500) / 100 + 4;
+	ret = tps80031_update(charger->dev->parent, SLAVE_ID2,
+			CHARGERUSB_VICHRG, (uint8_t)max_charge_current, 0xF);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): Failed in writing register 0x%02x\n",
+				__func__, CHARGERUSB_VICHRG);
+		return ret;
+	}
+
+	/* Enable watchdog timer */
+	ret = tps80031_write(charger->dev->parent, SLAVE_ID2,
+				CONTROLLER_WDG, charger->watch_time_sec);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): Failed in writing register 0x%02x\n",
+				__func__, CONTROLLER_WDG);
+		return ret;
+	}
+
+	/* Enable the charging */
+	ret = tps80031_write(charger->dev->parent, SLAVE_ID2,
+				CONTROLLER_CTRL1, 0x30);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): Failed in writing register 0x%02x\n",
+				__func__, CONTROLLER_CTRL1);
+		return ret;
+	}
+	charger->state = charging_state_charging_in_progress;
+	if (charger->charger_cb)
+		charger->charger_cb(charger->state,
+				charger->charger_cb_data);
+	return 0;
+}
+
+static struct regulator_ops tegra_regulator_ops = {
+	.set_current_limit = set_charge_current_limit,
+};
+
+int register_charging_state_callback(charging_callback_t cb, void *args)
+{
+	struct tps80031_charger *charger = charger_data;
+	if (!charger_data)
+		return -ENODEV;
+
+	charger->charger_cb = cb;
+	charger->charger_cb_data = args;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(register_charging_state_callback);
+
+static int configure_charging_parameter(struct tps80031_charger *charger)
+{
+	int ret;
+	int max_charge_current;
+	int max_charge_volt;
+	int term_current;
+
+	/* Disable watchdog timer */
+	ret = tps80031_write(charger->dev->parent, SLAVE_ID2,
+				CONTROLLER_WDG, 0x0);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): Failed in writing register 0x%02x\n",
+				__func__, CONTROLLER_WDG);
+		return ret;
+	}
+
+	/* Disable the charging if any */
+	ret = tps80031_write(charger->dev->parent, SLAVE_ID2,
+				CONTROLLER_CTRL1, 0x0);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): Failed in writing register 0x%02x\n",
+				__func__, CONTROLLER_CTRL1);
+		return ret;
+	}
+
+	if (charger->board_init) {
+		ret = charger->board_init(charger->board_data);
+		if (ret < 0) {
+			dev_err(charger->dev, "%s(): Failed in board init\n",
+				__func__);
+			return ret;
+		}
+	}
+
+	/* Unlock value */
+	ret = tps80031_write(charger->dev->parent, SLAVE_ID2,
+			CHARGERUSB_CTRLLIMIT2, 0);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): Failed in writing register 0x%02x\n",
+				__func__, CHARGERUSB_CTRLLIMIT2);
+		return ret;
+	}
+
+	/* Set max current limit */
+	max_charge_current = min(1500, charger->max_charge_current_mA);
+	if (max_charge_current < 100)
+		max_charge_current = 0;
+	else
+		max_charge_current = (max_charge_current - 100)/100;
+	max_charge_current &= 0xF;
+	ret = tps80031_write(charger->dev->parent, SLAVE_ID2,
+		CHARGERUSB_CTRLLIMIT2, (uint8_t)max_charge_current);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): Failed in writing register "
+				"0x%02x\n", __func__, CHARGERUSB_CTRLLIMIT2);
+		return ret;
+	}
+
+	/* Set max voltage limit */
+	max_charge_volt = min(4760, charger->max_charge_volt_mV);
+	max_charge_volt = max(3500, max_charge_volt);
+	max_charge_volt -= 3500;
+	max_charge_volt = max_charge_volt/20;
+	ret = tps80031_write(charger->dev->parent, SLAVE_ID2,
+		CHARGERUSB_CTRLLIMIT1, (uint8_t)max_charge_volt);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): Failed in writing register 0x%02x\n",
+				__func__, CHARGERUSB_CTRLLIMIT1);
+		return ret;
+	}
+
+	/* Lock value */
+	ret = tps80031_set_bits(charger->dev->parent, SLAVE_ID2,
+			CHARGERUSB_CTRLLIMIT2, (1 << 4));
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): Failed in writing register 0x%02x\n",
+				__func__, CHARGERUSB_CTRLLIMIT2);
+		return ret;
+	}
+
+	/* set Pre Charge current to 400mA */
+	ret = tps80031_write(charger->dev->parent, SLAVE_ID2,
+			CHARGERUSB_VICHRG_PC, 0x3);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): Failed in writing register 0x%02x\n",
+				__func__, 0xDD);
+		return ret;
+	}
+
+	/* set charging termination current*/
+	if (charger->charging_term_current_mA > 400)
+		term_current =  7;
+	else
+		term_current = (charger->charging_term_current_mA - 50)/50;
+	term_current = term_current << 5;
+	ret = tps80031_write(charger->dev->parent, SLAVE_ID2,
+			CHARGERUSB_CTRL2, term_current);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): Failed in writing register 0x%02x\n",
+				__func__, CHARGERUSB_CTRL2);
+		return ret;
+	}
+
+	return 0;
+}
+
+static bool tps80031_check_charging_completed(struct tps80031_charger *charger)
+{
+	int ret;
+	uint8_t linch_status;
+
+	ret = tps80031_read(charger->dev->parent, SLAVE_ID2,
+			LINEAR_CHRG_STS, &linch_status);
+	if (ret < 0) {
+		dev_err(charger->dev, "%s(): Failed in reading register 0x%02x\n",
+				__func__, LINEAR_CHRG_STS);
+		return false;
+	}
+
+	if (linch_status & 0x20) {
+		charger->state = charging_state_charging_completed;
+		ret = true;
+	} else {
+		charger->state = charging_state_charging_in_progress;
+		ret = false;
+	}
+
+	return ret;
+}
+
+static irqreturn_t linch_status_isr(int irq, void *dev_id)
+{
+	struct tps80031_charger *charger = dev_id;
+
+	dev_info(charger->dev, "%s() got called\n", __func__);
+
+	if (tps80031_check_charging_completed(charger)) {
+		charger->state = charging_state_charging_completed;
+		if (charger->charger_cb)
+			charger->charger_cb(charger->state,
+					charger->charger_cb_data);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t watchdog_expire_isr(int irq, void *dev_id)
+{
+	struct tps80031_charger *charger = dev_id;
+	int ret;
+
+	dev_info(charger->dev, "%s()\n", __func__);
+	if (charger->state != charging_state_charging_in_progress) {
+		/*
+		 * After the charge completed, the chip can enable the
+		 * charging again if battery voltage is 120mV below the
+		 * charging voltage (defined by VOREG register).
+		 */
+		if (tps80031_check_charging_completed(charger)) {
+			return IRQ_HANDLED;
+		} else {
+			/* "recharging" after charging completed happened */
+			charger->state = charging_state_charging_in_progress;
+			if (charger->charger_cb)
+				charger->charger_cb(charger->state,
+						charger->charger_cb_data);
+		}
+	}
+
+	/* Enable watchdog timer again*/
+	ret = tps80031_write(charger->dev->parent, SLAVE_ID2, CONTROLLER_WDG,
+			charger->watch_time_sec);
+	if (ret < 0)
+		dev_err(charger->dev, "%s(): Failed in writing register 0x%02x\n",
+				__func__, CONTROLLER_WDG);
+
+	/* Rewrite to enable the charging */
+	if (!ret) {
+		ret = tps80031_write(charger->dev->parent, SLAVE_ID2,
+			CONTROLLER_CTRL1, 0x30);
+		if (ret < 0)
+			dev_err(charger->dev, "%s(): Failed in writing "
+				"register 0x%02x\n",
+				__func__, CONTROLLER_CTRL1);
+	}
+	return IRQ_HANDLED;
+}
+
+static int tps80031_charger_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct device *dev = &pdev->dev;
+	struct tps80031_charger *charger;
+	struct tps80031_platform_data *tps80031_pdata;
+	struct tps80031_charger_platform_data *pdata;
+
+	dev_info(dev, "%s()\n", __func__);
+
+	tps80031_pdata = dev_get_platdata(pdev->dev.parent);
+	if (!tps80031_pdata) {
+		dev_err(&pdev->dev, "no tps80031 platform_data specified\n");
+		return -EINVAL;
+	}
+
+	pdata = tps80031_pdata->battery_charger_pdata;
+	if (!pdata) {
+		dev_err(dev, "%s() No platform data, exiting..\n", __func__);
+		return -ENODEV;
+	}
+
+	if (!pdata->num_consumer_supplies) {
+		dev_err(dev, "%s() No consumer supply list, exiting..\n",
+				__func__);
+		return -ENODEV;
+	}
+
+	charger = kzalloc(sizeof(*charger), GFP_KERNEL);
+	if (!charger) {
+		dev_err(dev, "failed to allocate memory status\n");
+		return -ENOMEM;
+	}
+
+	charger->dev =  &pdev->dev;
+
+	charger->max_charge_current_mA = (pdata->max_charge_current_mA) ?
+					pdata->max_charge_current_mA : 1000;
+	charger->max_charge_volt_mV = (pdata->max_charge_volt_mV) ?
+					pdata->max_charge_volt_mV : 4200;
+	charger->irq_base = pdata->irq_base;
+	charger->watch_time_sec = min(pdata->watch_time_sec, 127);
+	if (!charger->watch_time_sec)
+		charger->watch_time_sec = 127;
+	charger->charging_term_current_mA =
+			min(50, pdata->charging_term_current_mA);
+	if (charger->charging_term_current_mA < 50)
+		charger->charging_term_current_mA = 50;
+
+	charger->reg_desc.name = "vbus_charger";
+	charger->reg_desc.id = pdata->regulator_id;
+	charger->reg_desc.ops = &tegra_regulator_ops;
+	charger->reg_desc.type = REGULATOR_CURRENT;
+	charger->reg_desc.owner = THIS_MODULE;
+
+	charger->reg_init_data.supply_regulator = NULL;
+	charger->reg_init_data.num_consumer_supplies =
+					pdata->num_consumer_supplies;
+	charger->reg_init_data.consumer_supplies = pdata->consumer_supplies;
+	charger->reg_init_data.regulator_init = NULL;
+	charger->reg_init_data.driver_data = charger;
+	charger->reg_init_data.constraints.name = "vbus_charger";
+	charger->reg_init_data.constraints.min_uA = 0;
+	charger->reg_init_data.constraints.max_uA =
+					pdata->max_charge_current_mA * 1000;
+	charger->reg_init_data.constraints.valid_modes_mask =
+					REGULATOR_MODE_NORMAL |
+					REGULATOR_MODE_STANDBY;
+	charger->reg_init_data.constraints.valid_ops_mask =
+					REGULATOR_CHANGE_MODE |
+					REGULATOR_CHANGE_STATUS |
+					REGULATOR_CHANGE_CURRENT;
+
+	charger->board_init = pdata->board_init;
+	charger->board_data = pdata->board_data;
+	charger->state = charging_state_idle;
+
+	charger->rdev = regulator_register(&charger->reg_desc, &pdev->dev,
+					&charger->reg_init_data, charger);
+	if (IS_ERR(charger->rdev)) {
+		dev_err(&pdev->dev, "failed to register %s\n",
+						charger->reg_desc.name);
+		ret = PTR_ERR(charger->rdev);
+		goto regulator_fail;
+	}
+
+	ret = request_threaded_irq(charger->irq_base + TPS80031_INT_LINCH_GATED,
+			NULL, linch_status_isr,	0, "tps80031-linch", charger);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to register irq %d; error %d\n",
+			charger->irq_base + TPS80031_INT_LINCH_GATED, ret);
+		goto irq_linch_fail;
+	}
+
+	ret = request_threaded_irq(charger->irq_base + TPS80031_INT_FAULT_WDG,
+			NULL, watchdog_expire_isr, 0, "tps80031-wdg", charger);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to register irq %d; error %d\n",
+			charger->irq_base + TPS80031_INT_FAULT_WDG, ret);
+		goto irq_wdg_fail;
+	}
+
+	ret = configure_charging_parameter(charger);
+	if (ret)
+		goto config_fail;
+
+	dev_set_drvdata(&pdev->dev, charger);
+	charger_data = charger;
+	return ret;
+
+config_fail:
+	free_irq(charger->irq_base + TPS80031_INT_FAULT_WDG, charger);
+irq_wdg_fail:
+	free_irq(charger->irq_base + TPS80031_INT_LINCH_GATED, charger);
+irq_linch_fail:
+	regulator_unregister(charger->rdev);
+regulator_fail:
+	kfree(charger);
+	return ret;
+}
+
+static int tps80031_charger_remove(struct platform_device *pdev)
+{
+	struct tps80031_charger *charger = dev_get_drvdata(&pdev->dev);
+
+	regulator_unregister(charger->rdev);
+	kfree(charger);
+	return 0;
+}
+
+static struct platform_driver tps80031_charger_driver = {
+	.driver	= {
+		.name	= "tps80031-charger",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= tps80031_charger_probe,
+	.remove = tps80031_charger_remove,
+};
+
+static int __init tps80031_charger_init(void)
+{
+	return platform_driver_register(&tps80031_charger_driver);
+}
+
+static void __exit tps80031_charger_exit(void)
+{
+	platform_driver_unregister(&tps80031_charger_driver);
+}
+
+subsys_initcall(tps80031_charger_init);
+module_exit(tps80031_charger_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("tps80031 battery charger driver");
diff --git a/drivers/power/tps80031_battery_gauge.c b/drivers/power/tps80031_battery_gauge.c
new file mode 100644
index 000000000000..f339b41cf039
--- /dev/null
+++ b/drivers/power/tps80031_battery_gauge.c
@@ -0,0 +1,621 @@
+/*
+ * drivers/power/tps80031_battery_gauge.c
+ *
+ * Gas Gauge driver for TI's tps80031
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/err.h>
+#include <linux/regulator/machine.h>
+#include <linux/mutex.h>
+
+#include <linux/module.h>
+#include <linux/param.h>
+#include <linux/jiffies.h>
+#include <linux/workqueue.h>
+#include <linux/pm.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+
+#include <linux/mfd/core.h>
+#include <linux/mfd/tps80031.h>
+#include <linux/tps80031-charger.h>
+
+#define CHARGERUSB_CINLIMIT	0xee
+#define CONTROLLER_STAT1	0xe3
+#define LINEAR_CHARGE_STS	0xde
+#define STS_HW_CONDITIONS	0x21
+#define TOGGLE1			0x90
+#define TOGGLE1_FGS		BIT(5)
+#define TOGGLE1_GPADCR		BIT(1)
+#define GPCH0_LSB		0x3b
+#define GPCH0_MSB		0x3c
+#define GPCH0_MSB_COLLISION_GP	BIT(4)
+#define GPSELECT_ISB		0x35
+#define GPADC_CTRL		0x2e
+#define MISC1			0xe4
+#define CTRL_P1			0x36
+#define CTRL_P1_SP1		BIT(3)
+#define CTRL_P1_EOCRT		BIT(2)
+#define CTRL_P1_EOCP1		BIT(1)
+#define CTRL_P1_BUSY		BIT(0)
+#define FG_REG_00		0xc0
+#define FG_REG_00_CC_CAL_EN	BIT(1)
+#define FG_REG_00_CC_AUTOCLEAR	BIT(2)
+#define FG_REG_01		0xc1	/* CONV_NR (unsigned) 0 - 7 */
+#define FG_REG_02		0xc2	/* CONV_NR (unsigned) 8 - 15 */
+#define FG_REG_03		0xc3	/* CONV_NR (unsigned) 16 - 23 */
+#define FG_REG_04		0xc4	/* ACCM	(signed) 0 - 7 */
+#define FG_REG_05		0xc5	/* ACCM	(signed) 8 - 15 */
+#define FG_REG_06		0xc6	/* ACCM	(signed) 16 - 23 */
+#define FG_REG_07		0xc7	/* ACCM	(signed) 24 - 31 */
+#define FG_REG_08		0xc8	/* OFFSET (signed) 0 - 7 */
+#define FG_REG_09		0xc9	/* OFFSET (signed) 8 - 9 */
+#define FG_REG_10		0xca	/* LAST_READ (signed) 0 - 7 */
+#define FG_REG_11		0xcb	/* LAST_READ (signed) 8 - 13 */
+
+#define TPS80031_VBUS_DET	BIT(2)
+#define TPS80031_VAC_DET	BIT(3)
+#define TPS80031_STS_VYSMIN_HI	BIT(4)
+#define END_OF_CHARGE		BIT(5)
+
+#define DRIVER_VERSION		"1.1.0"
+#define BATTERY_POLL_PERIOD	30000
+
+static int tps80031_temp_table[] = {
+	/* adc code for temperature in degree C */
+	929, 925, /* -2, -1 */
+	920, 917, 912, 908, 904, 899, 895, 890, 885, 880, /* 00 - 09 */
+	875, 869, 864, 858, 853, 847, 841, 835, 829, 823, /* 10 - 19 */
+	816, 810, 804, 797, 790, 783, 776, 769, 762, 755, /* 20 - 29 */
+	748, 740, 732, 725, 718, 710, 703, 695, 687, 679, /* 30 - 39 */
+	671, 663, 655, 647, 639, 631, 623, 615, 607, 599, /* 40 - 49 */
+	591, 583, 575, 567, 559, 551, 543, 535, 527, 519, /* 50 - 59 */
+	511, 504, 496 /* 60 - 62 */
+};
+
+struct tps80031_device_info {
+	struct device		*dev;
+	struct i2c_client	*client;
+	struct power_supply	bat;
+	struct power_supply	ac;
+	struct power_supply	usb;
+	struct timer_list	battery_poll_timer;
+	uint32_t vsys;
+	uint8_t usb_online;
+	uint8_t ac_online;
+	uint8_t usb_status;
+	uint8_t capacity_sts;
+	uint8_t health;
+	uint8_t sys_vlow_intr;
+	uint8_t fg_calib_intr;
+	int16_t fg_offset;
+	struct mutex adc_lock;
+};
+
+static enum power_supply_property tps80031_bat_props[] = {
+	POWER_SUPPLY_PROP_TECHNOLOGY,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_TEMP,
+	POWER_SUPPLY_PROP_CAPACITY,
+	POWER_SUPPLY_PROP_HEALTH,
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_CHARGE_NOW,
+	POWER_SUPPLY_PROP_CHARGE_COUNTER,
+};
+
+static enum power_supply_property tps80031_usb_props[] = {
+	POWER_SUPPLY_PROP_ONLINE,
+};
+
+static enum power_supply_property tps80031_ac_props[] = {
+	POWER_SUPPLY_PROP_ONLINE,
+};
+
+static int tps80031_reg_read(struct tps80031_device_info *di, int sid, int reg,
+					uint8_t *val)
+{
+	int ret;
+
+	ret = tps80031_read(di->dev->parent, sid, reg, val);
+	if (ret < 0)
+		dev_err(di->dev, "Failed read register 0x%02x\n",
+					reg);
+	return ret;
+}
+
+static int tps80031_reg_write(struct tps80031_device_info *di, int sid, int reg,
+					uint8_t val)
+{
+	int ret;
+
+	ret = tps80031_write(di->dev->parent, sid, reg, val);
+	if (ret < 0)
+		dev_err(di->dev, "Failed write register 0x%02x\n",
+					reg);
+	return ret;
+}
+
+static int tps80031_battery_capacity(struct tps80031_device_info *di,
+			union power_supply_propval *val)
+{
+	uint8_t hwsts;
+	int ret;
+
+	ret = tps80031_reg_read(di, SLAVE_ID2, LINEAR_CHARGE_STS, &hwsts);
+	if (ret < 0)
+		return ret;
+
+	di->capacity_sts = di->vsys;
+	if (hwsts & END_OF_CHARGE)
+		di->capacity_sts = 100;
+
+	if (di->sys_vlow_intr) {
+		di->capacity_sts = 10;
+		di->sys_vlow_intr = 0;
+	}
+
+	if (di->capacity_sts <= 10)
+		di->health = POWER_SUPPLY_HEALTH_DEAD;
+	else
+		di->health = POWER_SUPPLY_HEALTH_GOOD;
+
+	return  di->capacity_sts;
+}
+
+static int tps80031_battery_voltage(struct tps80031_device_info *di,
+			union power_supply_propval *val)
+{
+	int voltage;
+
+	voltage = tps80031_gpadc_conversion(SYSTEM_SUPPLY);
+	if (voltage < 0)
+		return voltage;
+	voltage = ((voltage * 1000) / 4) * 5;
+
+	if (voltage < 3700000)
+		di->vsys = 10;
+	else if (voltage > 3700000 && voltage <= 3800000)
+		di->vsys = 20;
+	else if (voltage > 3800000 && voltage <= 3900000)
+		di->vsys = 50;
+	else if (voltage > 3900000 && voltage <= 4000000)
+		di->vsys = 75;
+	else if (voltage >= 4000000)
+		di->vsys = 90;
+
+	return voltage;
+}
+
+static int tps80031_battery_charge_now(struct tps80031_device_info *di,
+			union power_supply_propval *val)
+{
+	int charge;
+
+	charge = tps80031_gpadc_conversion(BATTERY_CHARGING_CURRENT);
+	if (charge < 0)
+		return charge;
+	charge = charge * 78125 / 40;
+
+	return charge;
+}
+
+static int tps80031_battery_charge_counter(struct tps80031_device_info *di,
+			union power_supply_propval *val)
+{
+	int retval, ret;
+	uint32_t cnt_byte;
+	uint32_t acc_byte;
+
+	/* check if calibrated */
+	if (di->fg_calib_intr == 0)
+		return 0;
+
+	/* get current accumlator */
+	ret = tps80031_reads(di->dev->parent, SLAVE_ID2, FG_REG_04, 4,
+							(uint8_t *) &acc_byte);
+	if (ret < 0)
+		return ret;
+	/* counter value is mAs, need report uAh */
+	retval = (int32_t) acc_byte / 18 * 5;
+
+	/* get counter */
+	ret = tps80031_reads(di->dev->parent, SLAVE_ID2, FG_REG_01, 3,
+							(uint8_t *) &cnt_byte);
+	if (ret < 0)
+		return ret;
+	/* we need calibrate the offset current in uAh*/
+	retval = retval - (di->fg_offset / 4 * cnt_byte);
+
+	/* @todo, counter value will overflow if battery get continuously
+	 * charged discharged for more than 108Ah using 250mS integration
+	 * period althrough it is hightly impossible.
+	 */
+
+	return retval;
+}
+
+static int tps80031_battery_temp(struct tps80031_device_info *di,
+					union power_supply_propval *val)
+{
+	int adc_code, temp;
+
+	adc_code = tps80031_gpadc_conversion(BATTERY_TEMPERATURE);
+	if (adc_code < 0)
+		return adc_code;
+
+	for (temp = 0; temp < ARRAY_SIZE(tps80031_temp_table); temp++) {
+		if (adc_code >= tps80031_temp_table[temp])
+			break;
+	}
+	/* first 2 values are for negative temperature */
+	val->intval = (temp - 2) * 10; /* in tenths of degree Celsius */
+
+	return  val->intval;
+}
+
+#define to_tps80031_device_info_bat(x) container_of((x), \
+				struct tps80031_device_info, bat);
+
+static int tps80031_bat_get_property(struct power_supply *psy,
+	enum power_supply_property psp, union power_supply_propval *val)
+{
+	struct tps80031_device_info *di = to_tps80031_device_info_bat(psy);
+
+	switch (psp) {
+
+	case POWER_SUPPLY_PROP_TECHNOLOGY:
+		val->intval = POWER_SUPPLY_TECHNOLOGY_LION;
+		break;
+
+	case POWER_SUPPLY_PROP_HEALTH:
+		val->intval = di->health;
+		break;
+
+	case POWER_SUPPLY_PROP_CAPACITY:
+		val->intval =  tps80031_battery_capacity(di, val);
+		break;
+
+	case POWER_SUPPLY_PROP_CHARGE_NOW:
+		val->intval =  tps80031_battery_charge_now(di, val);
+		break;
+
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		val->intval =  tps80031_battery_voltage(di, val);
+		break;
+
+	case POWER_SUPPLY_PROP_CHARGE_COUNTER:
+		val->intval =  tps80031_battery_charge_counter(di, val);
+		break;
+
+	case POWER_SUPPLY_PROP_STATUS:
+		val->intval = di->usb_status;
+		break;
+
+	case POWER_SUPPLY_PROP_TEMP:
+		val->intval = tps80031_battery_temp(di, val);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+#define to_tps80031_device_info_usb(x) container_of((x), \
+				struct tps80031_device_info, usb);
+
+static int tps80031_usb_get_property(struct power_supply *psy,
+		enum power_supply_property psp, union power_supply_propval *val)
+{
+	struct tps80031_device_info *di = to_tps80031_device_info_usb(psy);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = di->usb_online;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+#define to_tps80031_device_info_ac(x) container_of((x), \
+				struct tps80031_device_info, ac);
+
+static int tps80031_ac_get_property(struct power_supply *psy,
+		enum power_supply_property psp, union power_supply_propval *val)
+{
+	struct tps80031_device_info *di = to_tps80031_device_info_ac(psy);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = di->ac_online;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static irqreturn_t tps80031_sys_vlow(int irq, void *data)
+{
+	struct tps80031_device_info *di = data;
+
+	di->sys_vlow_intr = 1;
+	power_supply_changed(&di->bat);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t tps80031_fg_calibrated(int irq, void *data)
+{
+	struct tps80031_device_info *di = data;
+	uint8_t acc_byte0;
+	uint8_t acc_byte1;
+	int ret;
+
+	ret = tps80031_reg_read(di, SLAVE_ID2, FG_REG_08, &acc_byte0);
+	if (ret < 0)
+		return IRQ_HANDLED;
+	ret = tps80031_reg_read(di, SLAVE_ID2, FG_REG_09, &acc_byte1);
+	if (ret < 0)
+		return IRQ_HANDLED;
+	/* sign extension */
+	if (acc_byte1 & 0x02)
+		acc_byte1 = acc_byte1 | 0xFC;
+	else
+		acc_byte1 = acc_byte1 & 0x03;
+
+	di->fg_offset = (int16_t) ((acc_byte1 << 8) | acc_byte0);
+	/* fuel gauge auto calibration finished */
+	di->fg_calib_intr = 1;
+	return IRQ_HANDLED;
+}
+
+static int tps80031_fg_start_gas_gauge(struct tps80031_device_info *di)
+{
+	int ret = 0;
+	di->fg_calib_intr = 0;
+
+	/* start gas gauge */
+	ret = tps80031_reg_write(di, SLAVE_ID2, TOGGLE1, 0x20);
+	if (ret < 0)
+		return ret;
+	/* set ADC update time to 3.9ms and start calibration */
+	ret = tps80031_reg_write(di, SLAVE_ID2, FG_REG_00, FG_REG_00_CC_CAL_EN);
+	if (ret < 0)
+		return ret;
+	return ret;
+}
+
+void tps80031_battery_status(enum charging_states status, void *data)
+{
+	struct tps80031_device_info *di = data;
+	int ret;
+	uint8_t retval;
+
+	if ((status == charging_state_charging_in_progress)) {
+		di->usb_status	= POWER_SUPPLY_STATUS_CHARGING;
+		di->health	= POWER_SUPPLY_HEALTH_GOOD;
+		ret = tps80031_reg_read(di, SLAVE_ID2,
+				CHARGERUSB_CINLIMIT, &retval);
+		if (ret < 0) {
+			di->ac_online = 0;
+			di->usb_online = 0;
+		}
+		if (retval == 0x9) {
+			di->ac_online = 0;
+			di->usb_online = 1;
+		} else {
+			di->usb_online = 0;
+			di->ac_online = 1;
+		}
+	} else if (status == charging_state_charging_stopped) {
+		di->usb_status = POWER_SUPPLY_STATUS_DISCHARGING;
+		di->ac_online = 0;
+		di->usb_online = 0;
+	} else if (status == charging_state_charging_completed) {
+		di->usb_status = POWER_SUPPLY_STATUS_FULL;
+	}
+	power_supply_changed(&di->usb);
+	power_supply_changed(&di->bat);
+	power_supply_changed(&di->ac);
+}
+
+static void battery_poll_timer_func(unsigned long pdi)
+{
+	struct tps80031_device_info *di = (void *)pdi;
+	power_supply_changed(&di->bat);
+	mod_timer(&di->battery_poll_timer,
+		jiffies + msecs_to_jiffies(BATTERY_POLL_PERIOD));
+}
+
+static int tps80031_battery_probe(struct platform_device *pdev)
+{
+	int ret;
+	uint8_t retval;
+	struct device *dev = &pdev->dev;
+	struct tps80031_device_info *di;
+	struct tps80031_platform_data *tps80031_pdata;
+	struct tps80031_bg_platform_data *pdata;
+
+	tps80031_pdata = dev_get_platdata(pdev->dev.parent);
+	if (!tps80031_pdata) {
+		dev_err(&pdev->dev, "no tps80031 platform_data specified\n");
+		return -EINVAL;
+	}
+
+	pdata = tps80031_pdata->bg_pdata;
+	if (!pdata) {
+		dev_err(&pdev->dev, "no battery_gauge platform data\n");
+		return -EINVAL;
+	}
+
+	di = devm_kzalloc(&pdev->dev, sizeof *di, GFP_KERNEL);
+	if (!di) {
+		dev_err(dev->parent, "failed to allocate device info data\n");
+		return -ENOMEM;
+	}
+
+	if (!pdata->battery_present) {
+		dev_err(dev, "%s() No battery detected, exiting..\n",
+				__func__);
+		return -ENODEV;
+	}
+
+	di->dev =  &pdev->dev;
+
+	ret = tps80031_reg_read(di, SLAVE_ID2, CONTROLLER_STAT1, &retval);
+	if (ret < 0)
+		return ret;
+
+	if ((retval & TPS80031_VAC_DET) | (retval & TPS80031_VBUS_DET)) {
+		di->usb_status = POWER_SUPPLY_STATUS_CHARGING;
+		di->usb_online = 1;
+	} else {
+		di->usb_status = POWER_SUPPLY_STATUS_DISCHARGING;
+		di->usb_online = 0;
+	}
+
+	di->capacity_sts = 50;
+	di->health = POWER_SUPPLY_HEALTH_GOOD;
+
+	di->bat.name		= "tps80031-bat";
+	di->bat.type		= POWER_SUPPLY_TYPE_BATTERY;
+	di->bat.properties	= tps80031_bat_props;
+	di->bat.num_properties	= ARRAY_SIZE(tps80031_bat_props);
+	di->bat.get_property	= tps80031_bat_get_property;
+
+	ret = power_supply_register(dev->parent, &di->bat);
+	if (ret) {
+		dev_err(dev->parent, "failed to register bat power supply\n");
+		return ret;
+	}
+
+	di->usb.name		= "tps80031-usb";
+	di->usb.type		= POWER_SUPPLY_TYPE_USB;
+	di->usb.properties	= tps80031_usb_props;
+	di->usb.num_properties	= ARRAY_SIZE(tps80031_usb_props);
+	di->usb.get_property	= tps80031_usb_get_property;
+
+	ret = power_supply_register(dev->parent, &di->usb);
+	if (ret) {
+		dev_err(dev->parent, "failed to register ac power supply\n");
+		goto power_supply_fail2;
+	}
+
+	di->ac.name		= "tps80031-ac";
+	di->ac.type		= POWER_SUPPLY_TYPE_MAINS;
+	di->ac.properties	= tps80031_ac_props;
+	di->ac.num_properties	= ARRAY_SIZE(tps80031_ac_props);
+	di->ac.get_property	= tps80031_ac_get_property;
+
+	ret = power_supply_register(dev->parent, &di->ac);
+	if (ret) {
+		dev_err(dev->parent, "failed to register ac power supply\n");
+		goto power_supply_fail1;
+	}
+
+	dev_set_drvdata(&pdev->dev, di);
+
+	ret = register_charging_state_callback(tps80031_battery_status, di);
+	if (ret < 0)
+		goto power_supply_fail0;
+
+	ret = request_threaded_irq(pdata->irq_base + TPS80031_INT_SYS_VLOW,
+				NULL, tps80031_sys_vlow,
+					IRQF_ONESHOT, "tps80031_sys_vlow", di);
+	if (ret < 0) {
+		dev_err(dev->parent, "request IRQ %d fail\n", pdata->irq_base);
+		goto power_supply_fail0;
+	}
+
+	ret = request_threaded_irq(pdata->irq_base + TPS80031_INT_CC_AUTOCAL,
+				NULL, tps80031_fg_calibrated, IRQF_ONESHOT,
+				"tps80031_fuel_gauge_calibration", di);
+	if (ret < 0) {
+		dev_err(dev->parent, "request IRQ %d fail\n", pdata->irq_base);
+		goto irq_fail2;
+	}
+	setup_timer(&di->battery_poll_timer,
+		battery_poll_timer_func, (unsigned long) di);
+	mod_timer(&di->battery_poll_timer,
+		jiffies + msecs_to_jiffies(BATTERY_POLL_PERIOD));
+
+	ret = tps80031_fg_start_gas_gauge(di);
+	if (ret < 0) {
+		dev_err(dev->parent, "failed to start fuel-gauge\n");
+		goto irq_fail1;
+	}
+	dev_info(dev->parent, "support ver. %s enabled\n", DRIVER_VERSION);
+
+	return ret;
+
+irq_fail1:
+	free_irq(pdata->irq_base + TPS80031_INT_CC_AUTOCAL, di);
+irq_fail2:
+	free_irq(pdata->irq_base + TPS80031_INT_SYS_VLOW, di);
+power_supply_fail0:
+	power_supply_unregister(&di->ac);
+power_supply_fail1:
+	power_supply_unregister(&di->usb);
+power_supply_fail2:
+	power_supply_unregister(&di->bat);
+	return ret;
+}
+
+static int tps80031_battery_remove(struct platform_device *pdev)
+{
+	struct tps80031_device_info *di = dev_get_drvdata(&pdev->dev);
+
+	power_supply_unregister(&di->bat);
+	power_supply_unregister(&di->usb);
+	power_supply_unregister(&di->ac);
+
+	return 0;
+}
+
+static struct platform_driver tps80031_battery_driver = {
+	.driver	= {
+		.name	= "tps80031-battery-gauge",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= tps80031_battery_probe,
+	.remove = tps80031_battery_remove,
+};
+
+static int __init tps80031_battery_init(void)
+{
+	return platform_driver_register(&tps80031_battery_driver);
+}
+
+static void __exit tps80031_battery_exit(void)
+{
+	platform_driver_unregister(&tps80031_battery_driver);
+}
+
+module_init(tps80031_battery_init);
+module_exit(tps80031_battery_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Syed Rafiuddin <srafiuddin@nvidia.com> ");
+MODULE_DESCRIPTION("tps80031 battery gauge driver");
diff --git a/drivers/ptp/ptp_clock.c b/drivers/ptp/ptp_clock.c
index cf3f9997546d..17731231bb2d 100644
--- a/drivers/ptp/ptp_clock.c
+++ b/drivers/ptp/ptp_clock.c
@@ -101,7 +101,9 @@ static s32 scaled_ppm_to_ppb(long ppm)
 
 static int ptp_clock_getres(struct posix_clock *pc, struct timespec *tp)
 {
-	return 1; /* always round timer functions to one nanosecond */
+	tp->tv_sec = 0;
+	tp->tv_nsec = 1;
+	return 0;
 }
 
 static int ptp_clock_settime(struct posix_clock *pc, const struct timespec *tp)
@@ -302,6 +304,12 @@ void ptp_clock_event(struct ptp_clock *ptp, struct ptp_clock_event *event)
 }
 EXPORT_SYMBOL(ptp_clock_event);
 
+int ptp_clock_index(struct ptp_clock *ptp)
+{
+	return ptp->index;
+}
+EXPORT_SYMBOL(ptp_clock_index);
+
 /* module operations */
 
 static void __exit ptp_exit(void)
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index c7fd2c0e3f2b..88d36153ec70 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -64,6 +64,15 @@ config REGULATOR_USERSPACE_CONSUMER
 
           If unsure, say no.
 
+config REGULATOR_GPIO
+	tristate "GPIO regulator support"
+	help
+	  This driver provides support for regulators that can be
+	  controlled via gpios.
+	  It is capable of supporting current and voltage regulators
+	  and the platform has to provide a mapping of GPIO-states
+	  to target volts/amps.
+
 config REGULATOR_BQ24022
 	tristate "TI bq24022 Dual Input 1-Cell Li-Ion Charger IC"
 	help
@@ -108,6 +117,15 @@ config REGULATOR_MAX8952
 	  via I2C bus. Maxim 8952 has one voltage output and supports 4 DVS
 	  modes ranging from 0.77V to 1.40V by 0.01V steps.
 
+config REGULATOR_MAX8973
+	tristate "Maxim MAX8973 Power Regulator"
+	depends on I2C
+	select REGMAP_I2C
+	help
+	  This driver supports MAX8973 voltage regulator chip.
+	  The MAX8973 high-efficiency, three-phase, DC-DC step-down switching
+	  regulator delivers up to 9A of output current.
+
 config REGULATOR_MAX8997
 	tristate "Maxim 8997/8966 regulator"
 	depends on MFD_MAX8997
@@ -125,6 +143,22 @@ config REGULATOR_MAX8998
 	  via I2C bus. The provided regulator is suitable for S3C6410
 	  and S5PC1XX chips to control VCC_CORE and VCC_USIM voltages.
 
+config REGULATOR_MAX8907C
+	tristate "Maxim 8907C voltage regulator"
+	depends on MFD_MAX8907C
+	help
+	  This driver controls a Maxim 8907C voltage output regulator
+	  via I2C bus. The provided regulator is suitable for Tegra
+	  chip to control Step-Down DC-DC and LDOs.
+
+config REGULATOR_MAX77663
+	tristate "Maxim 77663 voltage regulator"
+	depends on MFD_MAX77663
+	help
+	  This driver controls a Maxim 77663 voltage output regulator
+	  via I2C bus. The provided regulator is suitable for Tegra
+	  chip to control Step-Down DC-DC and LDOs.
+
 config REGULATOR_TWL4030
 	bool "TI TWL4030/TWL5030/TWL6030/TPS659x0 PMIC"
 	depends on TWL4030_CORE
@@ -223,6 +257,16 @@ config REGULATOR_AB3100
 	 AB3100 analog baseband dealing with power regulators
 	 for the system.
 
+config REGULATOR_RC5T583
+	tristate "RICOH RC5T583 Power regulators"
+	depends on MFD_RC5T583
+	help
+	  Select this option to enable the power regulator of RICOH
+	  PMIC RC5T583.
+	  This driver supports the control of different power rails of device
+	  through regulator interface. The device supports multiple DCDC/LDO
+	  outputs which can be controlled by i2c communication.
+
 config REGULATOR_TPS6105X
 	tristate "TI TPS6105X Power regulators"
 	depends on TPS6105X
@@ -288,12 +332,28 @@ config REGULATOR_DB8500_PRCMU
 	  This driver supports the voltage domain regulators controlled by the
 	  DB8500 PRCMU
 
+config REGULATOR_TPS51632
+	tristate "TI TPS51632 Power Regulator"
+	depends on I2C
+	select REGMAP_I2C
+	help
+	  This driver supports TPS51632 voltage regulator chip.
+	  The TPS52632 is 3-2-1 Phase D-Cap+ Step Down Driverless Controller
+	  with Serial VID control and DVFS.
+
 config REGULATOR_TPS6586X
 	tristate "TI TPS6586X Power regulators"
 	depends on MFD_TPS6586X
 	help
 	  This driver supports TPS6586X voltage regulator chips.
 
+config REGULATOR_TPS65090
+	tristate "TI TPS65090 Power regulator"
+	depends on MFD_TPS65090
+	help
+	  This driver provides support for the voltage regulators on the
+	  TI TPS65090 PMIC.
+
 config REGULATOR_TPS6524X
 	tristate "TI TPS6524X Power regulators"
 	depends on SPI
@@ -305,10 +365,30 @@ config REGULATOR_TPS6524X
 	  port controller.
 
 config REGULATOR_TPS65910
-	tristate "TI TPS65910 Power Regulator"
+	tristate "TI TPS65910/TPS65911 Power Regulators"
 	depends on MFD_TPS65910
 	help
-	  This driver supports TPS65910 voltage regulator chips.
+	  This driver supports TPS65910/TPS65911 voltage regulator chips.
+
+config REGULATOR_TPS62360
+	tristate "TI TPS62360 Power Regulator"
+	depends on I2C
+	select REGMAP_I2C
+	help
+	  This driver supports TPS62360 voltage regulator chip. This
+	  regulator is meant for processor core supply. This chip is
+	  high-frequency synchronous step down dc-dc converter optimized
+	  for battery-powered portable applications.
+
+config REGULATOR_TPS6238X0
+	tristate "TI TPS623850/TPS623860/TPS623870 Power Regulator"
+	depends on I2C
+	select REGMAP_I2C
+	help
+	  This driver supports TPS6238X0 voltage regulator chip. This
+	  regulator is meant for processor core supply. This chip is
+	  high-frequency synchronous step down dc-dc converter optimized
+	  for battery-powered portable applications.
 
 config REGULATOR_AAT2870
 	tristate "AnalogicTech AAT2870 Regulators"
@@ -317,5 +397,39 @@ config REGULATOR_AAT2870
 	  If you have a AnalogicTech AAT2870 say Y to enable the
 	  regulator driver.
 
+config REGULATOR_TPS6591X
+	tristate "TI TPS6591X Power regulators"
+	depends on MFD_TPS6591X
+	default n
+	help
+	  This driver supports TPS6591X voltage regulator chips.
+
+config REGULATOR_TPS80031
+	tristate "TI TPS80031 Power regulators"
+	depends on MFD_TPS80031
+	default n
+	help
+	  This driver supports TPS80031 voltage regulator chips.
+
+config REGULATOR_RICOH583
+	tristate "RICOH 583 Power regulators"
+	depends on MFD_RICOH583
+	default n
+	help
+	  This driver supports regulator driver for RICOH583 PMIC.
+
+config REGULATOR_AAT2870
+	tristate "AnalogicTech AAT2870 Regulators"
+	depends on MFD_AAT2870_CORE
+	help
+	  If you have a AnalogicTech AAT2870 say Y to enable the
+	  regulator driver.
+
+config REGULATOR_FAN53555
+	tristate "Fairchild FAN53555 DC-DC CPU power supply regulators"
+	default n
+	help
+	  This driver supports FAN53555 DC-DC CPU power supply.
+
 endif
 
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index 040d5aa63535..4265ea06992c 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -1,13 +1,14 @@
 #
 # Makefile for regulator drivers.
 #
-
+GCOV_PROFILE := y
 
 obj-$(CONFIG_REGULATOR) += core.o dummy.o
 obj-$(CONFIG_REGULATOR_FIXED_VOLTAGE) += fixed.o
 obj-$(CONFIG_REGULATOR_VIRTUAL_CONSUMER) += virtual.o
 obj-$(CONFIG_REGULATOR_USERSPACE_CONSUMER) += userspace-consumer.o
 
+obj-$(CONFIG_REGULATOR_GPIO) += gpio-regulator.o
 obj-$(CONFIG_REGULATOR_AD5398) += ad5398.o
 obj-$(CONFIG_REGULATOR_BQ24022) += bq24022.o
 obj-$(CONFIG_REGULATOR_LP3971) += lp3971.o
@@ -18,8 +19,11 @@ obj-$(CONFIG_REGULATOR_MAX8649)	+= max8649.o
 obj-$(CONFIG_REGULATOR_MAX8660) += max8660.o
 obj-$(CONFIG_REGULATOR_MAX8925) += max8925-regulator.o
 obj-$(CONFIG_REGULATOR_MAX8952) += max8952.o
+obj-$(CONFIG_REGULATOR_MAX8973) += max8973-regulator.o
 obj-$(CONFIG_REGULATOR_MAX8997) += max8997.o
 obj-$(CONFIG_REGULATOR_MAX8998) += max8998.o
+obj-$(CONFIG_REGULATOR_MAX8907C) += max8907c-regulator.o
+obj-$(CONFIG_REGULATOR_MAX77663) += max77663-regulator.o
 obj-$(CONFIG_REGULATOR_WM831X) += wm831x-dcdc.o
 obj-$(CONFIG_REGULATOR_WM831X) += wm831x-isink.o
 obj-$(CONFIG_REGULATOR_WM831X) += wm831x-ldo.o
@@ -27,6 +31,8 @@ obj-$(CONFIG_REGULATOR_WM8350) += wm8350-regulator.o
 obj-$(CONFIG_REGULATOR_WM8400) += wm8400-regulator.o
 obj-$(CONFIG_REGULATOR_WM8994) += wm8994-regulator.o
 obj-$(CONFIG_REGULATOR_TPS6586X) += tps6586x-regulator.o
+obj-$(CONFIG_REGULATOR_TPS6591X) += tps6591x-regulator.o
+obj-$(CONFIG_REGULATOR_TPS65090) += tps65090-regulator.o
 obj-$(CONFIG_REGULATOR_DA903X)	+= da903x.o
 obj-$(CONFIG_REGULATOR_PCF50633) += pcf50633-regulator.o
 obj-$(CONFIG_REGULATOR_PCAP) += pcap-regulator.o
@@ -34,16 +40,22 @@ obj-$(CONFIG_REGULATOR_MC13783) += mc13783-regulator.o
 obj-$(CONFIG_REGULATOR_MC13892) += mc13892-regulator.o
 obj-$(CONFIG_REGULATOR_MC13XXX_CORE) +=  mc13xxx-regulator-core.o
 obj-$(CONFIG_REGULATOR_AB3100) += ab3100.o
+obj-$(CONFIG_REGULATOR_RC5T583)  += rc5t583-regulator.o
+obj-$(CONFIG_REGULATOR_TPS51632) += tps51632-regulator.o
 obj-$(CONFIG_REGULATOR_TPS6105X) += tps6105x-regulator.o
 obj-$(CONFIG_REGULATOR_TPS65023) += tps65023-regulator.o
 obj-$(CONFIG_REGULATOR_TPS6507X) += tps6507x-regulator.o
 obj-$(CONFIG_REGULATOR_TPS6524X) += tps6524x-regulator.o
 obj-$(CONFIG_REGULATOR_TPS65912) += tps65912-regulator.o
+obj-$(CONFIG_REGULATOR_TPS80031) += tps80031-regulator.o
+obj-$(CONFIG_REGULATOR_RICOH583) += ricoh583-regulator.o
 obj-$(CONFIG_REGULATOR_88PM8607) += 88pm8607.o
 obj-$(CONFIG_REGULATOR_ISL6271A) += isl6271a-regulator.o
 obj-$(CONFIG_REGULATOR_AB8500)	+= ab8500.o
 obj-$(CONFIG_REGULATOR_DB8500_PRCMU) += db8500-prcmu.o
 obj-$(CONFIG_REGULATOR_TPS65910) += tps65910-regulator.o
+obj-$(CONFIG_REGULATOR_TPS62360) += tps62360-regulator.o
+obj-$(CONFIG_REGULATOR_TPS6238X0) += tps6238x0-regulator.o
 obj-$(CONFIG_REGULATOR_AAT2870) += aat2870-regulator.o
-
+obj-$(CONFIG_REGULATOR_FAN53555) += fan53555-regulator.o
 ccflags-$(CONFIG_REGULATOR_DEBUG) += -DDEBUG
diff --git a/drivers/regulator/aat2870-regulator.c b/drivers/regulator/aat2870-regulator.c
index cd4104542f0d..896361f03e03 100644
--- a/drivers/regulator/aat2870-regulator.c
+++ b/drivers/regulator/aat2870-regulator.c
@@ -27,6 +27,7 @@
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
+#include <linux/mfd/core.h>
 #include <linux/mfd/aat2870.h>
 
 struct aat2870_regulator {
@@ -159,7 +160,7 @@ static struct aat2870_regulator *aat2870_get_regulator(int id)
 			break;
 	}
 
-	if (!ri)
+	if (i == ARRAY_SIZE(aat2870_regulators))
 		return NULL;
 
 	ri->enable_addr = AAT2870_LDO_EN;
@@ -186,8 +187,8 @@ static int aat2870_regulator_probe(struct platform_device *pdev)
 	}
 	ri->pdev = pdev;
 
-	rdev = regulator_register(&ri->desc, &pdev->dev,
-				  pdev->dev.platform_data, ri);
+	rdev = regulator_register(&ri->desc, &pdev->dev, mfd_get_data(pdev),
+				  ri);
 	if (IS_ERR(rdev)) {
 		dev_err(&pdev->dev, "Failed to register regulator %s\n",
 			ri->desc.name);
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index d8e6a429e8ba..26f8776f8eee 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -31,6 +31,9 @@
 
 #define CREATE_TRACE_POINTS
 #include <trace/events/regulator.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
 
 #include "dummy.h"
 
@@ -88,6 +91,8 @@ struct regulator {
 
 static int _regulator_is_enabled(struct regulator_dev *rdev);
 static int _regulator_disable(struct regulator_dev *rdev);
+static int _regulator_enable(struct regulator_dev *rdev);
+static int _regulator_get_enable_time(struct regulator_dev *rdev);
 static int _regulator_get_voltage(struct regulator_dev *rdev);
 static int _regulator_get_current_limit(struct regulator_dev *rdev);
 static unsigned int _regulator_get_mode(struct regulator_dev *rdev);
@@ -355,7 +360,65 @@ static ssize_t regulator_state_show(struct device *dev,
 
 	return ret;
 }
-static DEVICE_ATTR(state, 0444, regulator_state_show, NULL);
+
+static ssize_t regulator_state_set(struct device *dev,
+		   struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct regulator_dev *rdev = dev_get_drvdata(dev);
+	int ret;
+	bool enabled;
+
+	if ((*buf == 'E') || (*buf == 'e'))
+		enabled = true;
+	else if ((*buf == 'D') || (*buf == 'd'))
+		enabled = false;
+	else
+		return -EINVAL;
+
+	if ((_regulator_is_enabled(rdev) && enabled) ||
+		(!_regulator_is_enabled(rdev) && !enabled))
+		return count;
+
+	mutex_lock(&rdev->mutex);
+	if (enabled) {
+		int delay = 0;
+		if (!rdev->desc->ops->enable) {
+			ret = -EINVAL;
+			goto end;
+		}
+		ret = _regulator_get_enable_time(rdev);
+		if (ret >= 0)
+			delay = ret;
+		ret = rdev->desc->ops->enable(rdev);
+		if (ret < 0) {
+			rdev_warn(rdev, "enable() failed: %d\n", ret);
+			goto end;
+		}
+		if (delay >= 1000) {
+			mdelay(delay / 1000);
+			udelay(delay % 1000);
+		} else if (delay) {
+			udelay(delay);
+		}
+	} else {
+		if (!rdev->desc->ops->disable) {
+			ret = -EINVAL;
+			goto end;
+		}
+		ret = rdev->desc->ops->disable(rdev);
+		if (ret < 0) {
+			rdev_warn(rdev, "disable() failed: %d\n", ret);
+			goto end;
+		}
+	}
+
+end:
+	mutex_unlock(&rdev->mutex);
+	if (ret < 0)
+		return ret;
+	return count;
+}
+static DEVICE_ATTR(state, 0644, regulator_state_show, regulator_state_set);
 
 static ssize_t regulator_status_show(struct device *dev,
 				   struct device_attribute *attr, char *buf)
@@ -1368,6 +1431,8 @@ static int _regulator_enable(struct regulator_dev *rdev)
 			}
 
 			trace_regulator_enable(rdev_get_name(rdev));
+			_notifier_call_chain(
+				rdev, REGULATOR_EVENT_PRE_ENABLE, NULL);
 
 			/* Allow the regulator to ramp; it would be useful
 			 * to extend this for bulk operations so that the
@@ -1385,6 +1450,8 @@ static int _regulator_enable(struct regulator_dev *rdev)
 				udelay(delay);
 			}
 
+			_notifier_call_chain(
+				rdev, REGULATOR_EVENT_POST_ENABLE, NULL);
 			trace_regulator_enable_complete(rdev_get_name(rdev));
 
 		} else if (ret < 0) {
@@ -1425,7 +1492,7 @@ int regulator_enable(struct regulator *regulator)
 	ret = _regulator_enable(rdev);
 	mutex_unlock(&rdev->mutex);
 
-	if (ret != 0)
+	if (ret != 0 && rdev->supply)
 		regulator_disable(rdev->supply);
 
 	return ret;
@@ -1676,6 +1743,10 @@ static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 	min_uV += rdev->constraints->uV_offset;
 	max_uV += rdev->constraints->uV_offset;
 
+	if (_regulator_is_enabled(rdev))
+		_notifier_call_chain(rdev, REGULATOR_EVENT_OUT_PRECHANGE,
+				     NULL);
+
 	if (rdev->desc->ops->set_voltage) {
 		ret = rdev->desc->ops->set_voltage(rdev, min_uV, max_uV,
 						   &selector);
@@ -1743,6 +1814,10 @@ static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 		_notifier_call_chain(rdev, REGULATOR_EVENT_VOLTAGE_CHANGE,
 				     NULL);
 
+	if (_regulator_is_enabled(rdev))
+		_notifier_call_chain(rdev, REGULATOR_EVENT_OUT_POSTCHANGE,
+				     NULL);
+
 	trace_regulator_set_voltage_complete(rdev_get_name(rdev), selector);
 
 	return ret;
@@ -2579,6 +2654,7 @@ struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 	static atomic_t regulator_no = ATOMIC_INIT(0);
 	struct regulator_dev *rdev;
 	int ret, i;
+	const char *supply = NULL;
 
 	if (regulator_desc == NULL)
 		return ERR_PTR(-EINVAL);
@@ -2653,21 +2729,24 @@ struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 	if (ret < 0)
 		goto scrub;
 
-	if (init_data->supply_regulator) {
+	if (init_data->supply_regulator)
+		supply = init_data->supply_regulator;
+	else if (regulator_desc->supply_name)
+		supply = regulator_desc->supply_name;
+
+	if (supply) {
 		struct regulator_dev *r;
 		int found = 0;
 
 		list_for_each_entry(r, &regulator_list, list) {
-			if (strcmp(rdev_get_name(r),
-				   init_data->supply_regulator) == 0) {
+			if (strcmp(rdev_get_name(r), supply) == 0) {
 				found = 1;
 				break;
 			}
 		}
 
 		if (!found) {
-			dev_err(dev, "Failed to find supply %s\n",
-				init_data->supply_regulator);
+			dev_err(dev, "Failed to find supply %s\n", supply);
 			ret = -ENODEV;
 			goto scrub;
 		}
@@ -2675,6 +2754,14 @@ struct regulator_dev *regulator_register(struct regulator_desc *regulator_desc,
 		ret = set_supply(rdev, r);
 		if (ret < 0)
 			goto scrub;
+
+		/* Enable supply if rail is enabled */
+		if (rdev->desc->ops->is_enabled &&
+				rdev->desc->ops->is_enabled(rdev)) {
+			ret = regulator_enable(rdev->supply);
+			if (ret < 0)
+				goto scrub;
+		}
 	}
 
 	/* add consumers devices */
@@ -2988,4 +3075,59 @@ unlock:
 
 	return 0;
 }
+
+#ifdef CONFIG_DEBUG_FS
+static int regulator_syncevent(struct file *file, const char __user *user_buf,
+				size_t count, loff_t *ppos)
+{
+	struct regulator_dev *rdev;
+	char buffer[40];
+	int buf_size;
+
+	memset(buffer, 0, sizeof(buffer));
+	buf_size = min(count, (sizeof(buffer)-1));
+
+	if (copy_from_user(buffer, user_buf, buf_size))
+		return -EFAULT;
+
+	if (!strnicmp("all", buffer, 3)) {
+
+		mutex_lock(&regulator_list_mutex);
+
+		list_for_each_entry(rdev, &regulator_list, list) {
+			mutex_lock(&rdev->mutex);
+
+			if (_regulator_is_enabled(rdev))
+				trace_regulator_enable(rdev_get_name(rdev));
+			else
+				trace_regulator_disable(rdev_get_name(rdev));
+
+			trace_regulator_set_voltage(rdev_get_name(rdev),
+				_regulator_get_voltage(rdev),
+				_regulator_get_voltage(rdev));
+
+			mutex_unlock(&rdev->mutex);
+		}
+	}
+
+	mutex_unlock(&regulator_list_mutex);
+
+	return count;
+}
+
+static const struct file_operations regulator_syncevent_fops = {
+	.write		= regulator_syncevent,
+};
+
+static int __init regulator_init_debugfs(void)
+{
+	debugfs_create_file("syncevent_regulators", S_IWUSR, NULL, NULL,
+			&regulator_syncevent_fops);
+
+	return 0;
+}
+
+late_initcall(regulator_init_debugfs);
+#endif
+
 late_initcall(regulator_init_complete);
diff --git a/drivers/regulator/fan53555-regulator.c b/drivers/regulator/fan53555-regulator.c
new file mode 100644
index 000000000000..13fa79c4ba3a
--- /dev/null
+++ b/drivers/regulator/fan53555-regulator.c
@@ -0,0 +1,567 @@
+/*
+ * driver/regultor/fan53555-regulator.c
+ *
+ * Driver for FAN53555UC00X, FAN53555UC01X, FAN53555UC03X,
+ *            FAN53555UC04X, FAN53555UC05X
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * 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 version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any kind,
+ * whether express or implied; 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/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/fan53555-regulator.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+
+/* Register definitions */
+#define FAN53555_REG_VSEL0		0
+#define FAN53555_REG_VSEL1		1
+#define FAN53555_REG_CONTROL		2
+#define FAN53555_REG_ID1		3
+#define FAN53555_REG_ID2		4
+#define FAN53555_REG_MONITOR		5
+
+#define FAN53555_VSEL_BUCK_EN		BIT(7)
+#define FAN53555_VSEL_MODE		BIT(6)
+#define FAN53555_VSEL_NSEL_SHIFT	0
+#define FAN53555_VSEL_NSEL_MASK		0x3F
+
+#define FAN53555_CONTROL_DISCHARGE	BIT(7)
+#define FAN53555_CONTROL_SLEW_SHIFT	4
+#define FAN53555_CONTROL_SLEW_MASK	0x70
+#define FAN53555_CONTROL_RESET		BIT(2)
+
+#define FAN53555_ID1_VENDOR_SHIFT	4
+#define FAN53555_ID1_VENDOR_MASK	0xF0
+#define FAN53555_ID1_DIE_ID_SHIFT	0
+#define FAN53555_ID1_DIE_ID_MASK	0x0F
+
+#define FAN53555_ID2_REV_SHIFT		0
+#define FAN53555_ID2_REV_MASK		0x0F
+
+#define FAN53555_MONITOR_ILIM		BIT(7)
+#define FAN53555_MONITOR_UVLO		BIT(6)
+#define FAN53555_MONITOR_OVP		BIT(5)
+#define FAN53555_MONITOR_POS		BIT(4)
+#define FAN53555_MONITOR_NEG		BIT(3)
+#define FAN53555_MONITOR_RESET_STAT	BIT(2)
+#define FAN53555_MONITOR_OT		BIT(1)
+#define FAN53555_MONITOR_BUCK_STATUS	BIT(0)
+
+#define FAN53555_VSEL0_ID		0
+#define FAN53555_VSEL1_ID		1
+
+#define FAN53555UC00X_ID		0x80
+#define FAN53555UC01X_ID		0x81
+#define FAN53555UC03X_ID		0x83
+#define FAN53555UC04X_ID		0x84
+#define FAN53555UC05X_ID		0x85
+
+#define FAN53555_N_VOLTAGES		64
+
+/* FAN53555 chip information */
+struct fan53555_chip {
+	const char *name;
+	struct device *dev;
+	struct regulator_desc desc;
+	struct i2c_client *client;
+	struct regulator_dev *rdev;
+	struct mutex io_lock;
+	int chip_id;
+	int vsel_id;
+	u8 shadow[6];
+};
+
+#define FAN53555_VOLTAGE(chip_id, vsel)				\
+	(((chip_id) == FAN53555UC04X_ID) ?			\
+	 ((vsel) * 12826 + 600000) : ((vsel) * 10000 + 600000))
+
+static int fan53555_read(struct fan53555_chip *fan, u8 reg)
+{
+	u8 data;
+	u8 val;
+	int ret;
+
+	data = reg;
+
+	ret = i2c_master_send(fan->client, &data, 1);
+	if (ret < 0)
+		goto out;
+
+	ret = i2c_master_recv(fan->client, &val, 1);
+	if (ret < 0)
+		goto out;
+
+	ret = val;
+out:
+	return ret;
+}
+
+static inline int fan53555_write(struct fan53555_chip *fan, u8 reg, u8 val)
+{
+	u8 msg[2];
+	int ret;
+
+	msg[0] = reg;
+	msg[1] = val;
+
+	ret = i2c_master_send(fan->client, msg, 2);
+	if (ret < 0)
+		return ret;
+	if (ret != 2)
+		return -EIO;
+	return 0;
+}
+
+static int fan53555_read_reg(struct fan53555_chip *fan, u8 reg)
+{
+	int data;
+
+	mutex_lock(&fan->io_lock);
+	data = fan53555_read(fan, reg);
+	if (data < 0)
+		dev_err(fan->dev, "Read from reg 0x%x failed\n", reg);
+	mutex_unlock(&fan->io_lock);
+
+	return data;
+}
+
+static int fan53555_set_bits(struct fan53555_chip *fan, u8 reg, u8 mask, u8 val)
+{
+	int err;
+	u8 data;
+
+	mutex_lock(&fan->io_lock);
+	data = fan->shadow[reg];
+	data &= ~mask;
+	val &= mask;
+	data |= val;
+	err = fan53555_write(fan, reg, data);
+	if (err)
+		dev_err(fan->dev, "write for reg 0x%x failed\n", reg);
+	else
+		fan->shadow[reg] = data;
+	mutex_unlock(&fan->io_lock);
+
+	return err;
+}
+
+static int __fan53555_dcdc_set_voltage(struct fan53555_chip *fan,
+				       int vsel_id, int min_uV, int max_uV,
+				       unsigned *selector)
+{
+	int nsel;
+	int uV;
+	int chip_id;
+	int n_voltages;
+
+	chip_id = fan->chip_id;
+	n_voltages = fan->desc.n_voltages;
+
+	if (max_uV < min_uV) {
+		dev_err(fan->dev, "max_uV(%d) < min_uV(%d)\n", max_uV, min_uV);
+		return -EINVAL;
+	}
+	if (min_uV > FAN53555_VOLTAGE(chip_id, n_voltages - 1)) {
+		dev_err(fan->dev, "min_uV(%d) > %d[uV]\n",
+			min_uV, FAN53555_VOLTAGE(chip_id, n_voltages - 1));
+		return -EINVAL;
+	}
+	if (max_uV < FAN53555_VOLTAGE(chip_id, 0)) {
+		dev_err(fan->dev, "max_uV(%d) < %d[uV]\n",
+			max_uV, FAN53555_VOLTAGE(chip_id, 0));
+		return -EINVAL;
+	}
+	if ((vsel_id != FAN53555_VSEL0_ID) && (vsel_id != FAN53555_VSEL1_ID)) {
+		dev_err(fan->dev,
+			"%d is not valid VSEL register ID\n", vsel_id);
+		return -EINVAL;
+	}
+	for (nsel = 0; nsel < n_voltages; nsel++) {
+		uV = FAN53555_VOLTAGE(chip_id, nsel);
+		if (min_uV <= uV && uV <= max_uV) {
+			if (selector)
+				*selector = nsel;
+			return fan53555_set_bits(fan,
+						 FAN53555_REG_VSEL0 + vsel_id,
+						 FAN53555_VSEL_NSEL_MASK,
+						 nsel <<
+						 FAN53555_VSEL_NSEL_SHIFT);
+		}
+	}
+
+	return -EINVAL;
+}
+
+#ifdef CONFIG_DEBUG_FS
+
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+
+static int dbg_fan_show(struct seq_file *s, void *unused)
+{
+	struct fan53555_chip *fan = s->private;
+	int val;
+
+	seq_printf(s, "FAN53555 Registers\n");
+	seq_printf(s, "------------------\n");
+
+	val = fan53555_read_reg(fan, FAN53555_REG_VSEL0);
+	if (val >= 0)
+		seq_printf(s, "Reg VSEL0   Value 0x%02x\n", val);
+
+	val = fan53555_read_reg(fan, FAN53555_REG_VSEL1);
+	if (val >= 0)
+		seq_printf(s, "Reg VSEL1   Value 0x%02x\n", val);
+
+	val = fan53555_read_reg(fan, FAN53555_REG_CONTROL);
+	if (val >= 0)
+		seq_printf(s, "Reg CONTROL Value 0x%02x\n", val);
+
+	val = fan53555_read_reg(fan, FAN53555_REG_ID1);
+	if (val >= 0)
+		seq_printf(s, "Reg ID1     Value 0x%02x\n", val);
+
+	val = fan53555_read_reg(fan, FAN53555_REG_ID2);
+	if (val >= 0)
+		seq_printf(s, "Reg ID2     Value 0x%02x\n", val);
+
+	val = fan53555_read_reg(fan, FAN53555_REG_MONITOR);
+	if (val >= 0)
+		seq_printf(s, "Reg MONITOR Value 0x%02x\n", val);
+
+	return 0;
+}
+
+static int dbg_fan_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dbg_fan_show, inode->i_private);
+}
+
+static const struct file_operations debug_fops = {
+	.open = dbg_fan_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static void __init fan53555_debuginit(struct fan53555_chip *fan)
+{
+	(void)debugfs_create_file("fan53555", S_IRUGO, NULL, fan, &debug_fops);
+}
+#else
+static void __init fan53555_debuginit(struct fan53555_chip *fan)
+{
+}
+#endif
+
+static int fan53555_dcdc_init(struct fan53555_chip *fan,
+			      struct i2c_client *client,
+			      struct fan53555_regulator_platform_data *pdata)
+{
+	int err;
+	int val;
+
+	err = fan53555_read_reg(fan, FAN53555_REG_VSEL0);
+	if (err < 0)
+		return err;
+	fan->shadow[FAN53555_REG_VSEL0] = (u8)err;
+
+	err = fan53555_read_reg(fan, FAN53555_REG_VSEL1);
+	if (err < 0)
+		return err;
+	fan->shadow[FAN53555_REG_VSEL1] = (u8)err;
+
+	err = fan53555_read_reg(fan, FAN53555_REG_CONTROL);
+	if (err < 0)
+		return err;
+	fan->shadow[FAN53555_REG_CONTROL] = (u8)err;
+
+	err = __fan53555_dcdc_set_voltage(fan,
+					  FAN53555_VSEL0_ID,
+					  pdata->init_vsel0_min_uV,
+					  pdata->init_vsel0_max_uV,
+					  NULL);
+	if (err < 0)
+		return err;
+
+	val = pdata->vsel0_buck_en ? FAN53555_VSEL_BUCK_EN : 0;
+	val |= pdata->vsel0_mode ? FAN53555_VSEL_MODE : 0;
+	err = fan53555_set_bits(fan,
+				FAN53555_REG_VSEL0,
+				FAN53555_VSEL_BUCK_EN | FAN53555_VSEL_MODE,
+				val);
+	if (err < 0)
+		return err;
+
+	err = __fan53555_dcdc_set_voltage(fan,
+					  FAN53555_VSEL1_ID,
+					  pdata->init_vsel1_min_uV,
+					  pdata->init_vsel1_max_uV,
+					  NULL);
+	if (err < 0)
+		return err;
+
+	val = pdata->vsel1_buck_en ? FAN53555_VSEL_BUCK_EN : 0;
+	val |= pdata->vsel1_mode ? FAN53555_VSEL_MODE : 0;
+	err = fan53555_set_bits(fan,
+				FAN53555_REG_VSEL1,
+				FAN53555_VSEL_BUCK_EN | FAN53555_VSEL_MODE,
+				val);
+	if (err < 0)
+		return err;
+
+	val = pdata->slew_rate;
+	val <<= FAN53555_CONTROL_SLEW_SHIFT;
+	val |= pdata->output_discharge ? FAN53555_CONTROL_DISCHARGE : 0;
+	err = fan53555_set_bits(fan,
+				FAN53555_REG_CONTROL,
+				FAN53555_CONTROL_DISCHARGE |
+				FAN53555_CONTROL_SLEW_MASK, val);
+	return err;
+}
+
+static int fan53555_dcdc_list_voltage(struct regulator_dev *dev,
+				      unsigned selector)
+{
+	struct fan53555_chip *fan = rdev_get_drvdata(dev);
+
+	if ((selector < 0) || (selector >= fan->desc.n_voltages))
+		return -EINVAL;
+
+	return FAN53555_VOLTAGE(fan->chip_id, selector);
+}
+
+static int fan53555_dcdc_set_voltage(struct regulator_dev *dev,
+				     int min_uV, int max_uV,
+				     unsigned *selector)
+{
+	struct fan53555_chip *fan = rdev_get_drvdata(dev);
+
+	return __fan53555_dcdc_set_voltage(fan, fan->vsel_id, min_uV, max_uV,
+					   selector);
+}
+
+static int fan53555_dcdc_get_voltage(struct regulator_dev *dev)
+{
+	struct fan53555_chip *fan = rdev_get_drvdata(dev);
+	u8 data;
+
+	if ((fan->vsel_id != FAN53555_VSEL0_ID) &&
+	    (fan->vsel_id != FAN53555_VSEL1_ID)) {
+		dev_err(fan->dev,
+			"%d is not valid VSEL register ID\n", fan->vsel_id);
+		return -EINVAL;
+	}
+	data = fan->shadow[FAN53555_REG_VSEL0 + fan->vsel_id];
+	data &= FAN53555_VSEL_NSEL_MASK;
+	data >>= FAN53555_VSEL_NSEL_SHIFT;
+
+	return FAN53555_VOLTAGE(fan->chip_id, data);
+}
+
+static int fan53555_dcdc_enable(struct regulator_dev *dev)
+{
+	struct fan53555_chip *fan = rdev_get_drvdata(dev);
+
+	if ((fan->vsel_id != FAN53555_VSEL0_ID) &&
+	    (fan->vsel_id != FAN53555_VSEL1_ID)) {
+		dev_err(fan->dev,
+			"%d is not valid VSEL register ID\n", fan->vsel_id);
+		return -EINVAL;
+	}
+
+	return fan53555_set_bits(fan,
+				 FAN53555_REG_VSEL0 + fan->vsel_id,
+				 FAN53555_VSEL_BUCK_EN, FAN53555_VSEL_BUCK_EN);
+}
+
+static int fan53555_dcdc_disable(struct regulator_dev *dev)
+{
+	struct fan53555_chip *fan = rdev_get_drvdata(dev);
+
+	if ((fan->vsel_id != FAN53555_VSEL0_ID) &&
+	    (fan->vsel_id != FAN53555_VSEL1_ID)) {
+		dev_err(fan->dev,
+			"%d is not valid VSEL register ID\n", fan->vsel_id);
+		return -EINVAL;
+	}
+
+	return fan53555_set_bits(fan,
+				 FAN53555_REG_VSEL0 + fan->vsel_id,
+				 FAN53555_VSEL_BUCK_EN, 0);
+}
+
+static int fan53555_dcdc_is_enabled(struct regulator_dev *dev)
+{
+	struct fan53555_chip *fan = rdev_get_drvdata(dev);
+	u8 data;
+
+	if ((fan->vsel_id != FAN53555_VSEL0_ID) &&
+	    (fan->vsel_id != FAN53555_VSEL1_ID)) {
+		dev_err(fan->dev,
+			"%d is not valid VSEL register ID\n", fan->vsel_id);
+		return -EINVAL;
+	}
+	data = fan->shadow[FAN53555_REG_VSEL0 + fan->vsel_id];
+
+	return (data & FAN53555_VSEL_BUCK_EN) ? 1 : 0;
+}
+
+static struct regulator_ops fan53555_dcdc_ops = {
+	.list_voltage = fan53555_dcdc_list_voltage,
+	.set_voltage = fan53555_dcdc_set_voltage,
+	.get_voltage = fan53555_dcdc_get_voltage,
+	.enable = fan53555_dcdc_enable,
+	.disable = fan53555_dcdc_disable,
+	.is_enabled = fan53555_dcdc_is_enabled,
+};
+
+static int __devinit fan53555_probe(struct i2c_client *client,
+				    const struct i2c_device_id *id)
+{
+	struct fan53555_regulator_platform_data *pdata;
+	struct regulator_init_data *init_data;
+	struct regulator_dev *rdev;
+	struct fan53555_chip *fan;
+	int chip_id;
+	int err;
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->dev, "Err: Platform data not found\n");
+		return -EIO;
+	}
+	init_data = &pdata->reg_init_data;
+	fan = kzalloc(sizeof(*fan), GFP_KERNEL);
+	if (!fan) {
+		dev_err(&client->dev, "Err: Memory allocation fails\n");
+		return -ENOMEM;
+	}
+	mutex_init(&fan->io_lock);
+	fan->client = client;
+	fan->dev = &client->dev;
+	fan->vsel_id = pdata->vsel_id;
+	fan->name = id->name;
+	fan->desc.name = id->name;
+	fan->desc.id = 0;
+	fan->desc.irq = 0;
+	fan->desc.ops = &fan53555_dcdc_ops;
+	fan->desc.type = REGULATOR_VOLTAGE;
+	fan->desc.owner = THIS_MODULE;
+	fan->desc.n_voltages = FAN53555_N_VOLTAGES;
+	i2c_set_clientdata(client, fan);
+
+	chip_id = fan53555_read_reg(fan, FAN53555_REG_ID1);
+	if (chip_id < 0) {
+		err = chip_id;
+		dev_err(fan->dev, "Error in reading device %d\n", err);
+		goto fail;
+	}
+
+	switch (chip_id) {
+	case FAN53555UC00X_ID:
+	case FAN53555UC01X_ID:
+	case FAN53555UC03X_ID:
+	case FAN53555UC04X_ID:
+	case FAN53555UC05X_ID:
+		fan->chip_id = chip_id;
+		break;
+	default:
+		dev_err(fan->dev, "Err: not supported device chip id 0x%x",
+			chip_id);
+		err = -ENODEV;
+		goto fail;
+	}
+
+	err = fan53555_dcdc_init(fan, client, pdata);
+	if (err < 0) {
+		dev_err(fan->dev, "FAN53555 init fails with %d\n", err);
+		goto fail;
+	}
+
+	rdev = regulator_register(&fan->desc, &client->dev, init_data, fan);
+	if (IS_ERR(rdev)) {
+		dev_err(fan->dev, "Failed to register %s\n", id->name);
+		err = PTR_ERR(rdev);
+		goto fail;
+	}
+	fan->rdev = rdev;
+
+	fan53555_debuginit(fan);
+	return 0;
+
+fail:
+	kfree(fan);
+	return err;
+}
+
+/**
+ * fan53555_remove - fan53555 driver i2c remove handler
+ * @client: i2c driver client device structure
+ *
+ * Unregister fan53555 driver as an i2c client device driver
+ */
+static int __devexit fan53555_remove(struct i2c_client *client)
+{
+	struct fan53555_chip *chip = i2c_get_clientdata(client);
+
+	regulator_unregister(chip->rdev);
+	kfree(chip);
+	return 0;
+}
+
+static const struct i2c_device_id fan53555_id[] = {
+	{.name = "fan53555", .driver_data = 0 },
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, fan53555_id);
+
+static struct i2c_driver fan53555_i2c_driver = {
+	.driver = {
+		.name = "fan53555",
+		.owner = THIS_MODULE,
+	},
+	.probe = fan53555_probe,
+	.remove = __devexit_p(fan53555_remove),
+	.id_table = fan53555_id,
+};
+
+/* Module init function */
+static int __init fan53555_init(void)
+{
+	return i2c_add_driver(&fan53555_i2c_driver);
+}
+subsys_initcall_sync(fan53555_init);
+
+/* Module exit function */
+static void __exit fan53555_cleanup(void)
+{
+	i2c_del_driver(&fan53555_i2c_driver);
+}
+module_exit(fan53555_cleanup);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Jake Park<jakep@nvidia.com>");
+MODULE_DESCRIPTION("Regulator Driver for Fairchild FAN53555 Regulator");
+MODULE_ALIAS("platform:fan53555-regulator");
diff --git a/drivers/regulator/fixed.c b/drivers/regulator/fixed.c
index 2fe9d99c9f23..a3c06f648bae 100644
--- a/drivers/regulator/fixed.c
+++ b/drivers/regulator/fixed.c
@@ -131,6 +131,7 @@ static int __devinit reg_fixed_voltage_probe(struct platform_device *pdev)
 	drvdata->startup_delay = config->startup_delay;
 
 	if (gpio_is_valid(config->gpio)) {
+		int gpio_flag;
 		drvdata->enable_high = config->enable_high;
 
 		/* FIXME: Remove below print warning
@@ -148,27 +149,25 @@ static int __devinit reg_fixed_voltage_probe(struct platform_device *pdev)
 			dev_warn(&pdev->dev,
 				"using GPIO 0 for regulator enable control\n");
 
-		ret = gpio_request(config->gpio, config->supply_name);
-		if (ret) {
-			dev_err(&pdev->dev,
-			   "Could not obtain regulator enable GPIO %d: %d\n",
-							config->gpio, ret);
-			goto err_name;
-		}
-
-		/* set output direction without changing state
+		/*
+		 * set output direction without changing state
 		 * to prevent glitch
 		 */
 		drvdata->is_enabled = config->enabled_at_boot;
 		ret = drvdata->is_enabled ?
 				config->enable_high : !config->enable_high;
+		gpio_flag = ret ? GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
+
+		if (config->gpio_is_open_drain)
+			gpio_flag |= GPIOF_OPEN_DRAIN;
 
-		ret = gpio_direction_output(config->gpio, ret);
+		ret = gpio_request_one(config->gpio, gpio_flag,
+						config->supply_name);
 		if (ret) {
 			dev_err(&pdev->dev,
-			   "Could not configure regulator enable GPIO %d direction: %d\n",
+			   "Could not obtain regulator enable GPIO %d: %d\n",
 							config->gpio, ret);
-			goto err_gpio;
+			goto err_name;
 		}
 
 	} else {
diff --git a/drivers/regulator/gpio-regulator.c b/drivers/regulator/gpio-regulator.c
new file mode 100644
index 000000000000..f0acf52498bd
--- /dev/null
+++ b/drivers/regulator/gpio-regulator.c
@@ -0,0 +1,358 @@
+/*
+ * gpio-regulator.c
+ *
+ * Copyright 2011 Heiko Stuebner <heiko@sntech.de>
+ *
+ * based on fixed.c
+ *
+ * Copyright 2008 Wolfson Microelectronics PLC.
+ *
+ * Author: Mark Brown <broonie@opensource.wolfsonmicro.com>
+ *
+ * Copyright (c) 2009 Nokia Corporation
+ * Roger Quadros <ext-roger.quadros@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version.
+ *
+ * This is useful for systems with mixed controllable and
+ * non-controllable regulators, as well as for allowing testing on
+ * systems with no controllable regulators.
+ */
+
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/gpio-regulator.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+
+struct gpio_regulator_data {
+	struct regulator_desc desc;
+	struct regulator_dev *dev;
+
+	int enable_gpio;
+	bool enable_high;
+	bool is_enabled;
+	unsigned startup_delay;
+
+	struct gpio *gpios;
+	int nr_gpios;
+
+	struct gpio_regulator_state *states;
+	int nr_states;
+
+	int state;
+};
+
+static int gpio_regulator_is_enabled(struct regulator_dev *dev)
+{
+	struct gpio_regulator_data *data = rdev_get_drvdata(dev);
+
+	return data->is_enabled;
+}
+
+static int gpio_regulator_enable(struct regulator_dev *dev)
+{
+	struct gpio_regulator_data *data = rdev_get_drvdata(dev);
+
+	if (gpio_is_valid(data->enable_gpio)) {
+		gpio_set_value_cansleep(data->enable_gpio, data->enable_high);
+		data->is_enabled = true;
+	}
+
+	return 0;
+}
+
+static int gpio_regulator_disable(struct regulator_dev *dev)
+{
+	struct gpio_regulator_data *data = rdev_get_drvdata(dev);
+
+	if (gpio_is_valid(data->enable_gpio)) {
+		gpio_set_value_cansleep(data->enable_gpio, !data->enable_high);
+		data->is_enabled = false;
+	}
+
+	return 0;
+}
+
+static int gpio_regulator_enable_time(struct regulator_dev *dev)
+{
+	struct gpio_regulator_data *data = rdev_get_drvdata(dev);
+
+	return data->startup_delay;
+}
+
+static int gpio_regulator_get_value(struct regulator_dev *dev)
+{
+	struct gpio_regulator_data *data = rdev_get_drvdata(dev);
+	int ptr;
+
+	for (ptr = 0; ptr < data->nr_states; ptr++)
+		if (data->states[ptr].gpios == data->state)
+			return data->states[ptr].value;
+
+	return -EINVAL;
+}
+
+static int gpio_regulator_set_value(struct regulator_dev *dev,
+					int min, int max)
+{
+	struct gpio_regulator_data *data = rdev_get_drvdata(dev);
+	int ptr, target, state;
+
+	target = -1;
+	for (ptr = 0; ptr < data->nr_states; ptr++)
+		if (data->states[ptr].value >= min &&
+		    data->states[ptr].value <= max)
+			target = data->states[ptr].gpios;
+
+	if (target < 0)
+		return -EINVAL;
+
+	for (ptr = 0; ptr < data->nr_gpios; ptr++) {
+		state = (target & (1 << ptr)) >> ptr;
+		gpio_set_value(data->gpios[ptr].gpio, state);
+	}
+	data->state = target;
+
+	return 0;
+}
+
+static int gpio_regulator_set_voltage(struct regulator_dev *dev,
+					int min_uV, int max_uV,
+					unsigned *selector)
+{
+	return gpio_regulator_set_value(dev, min_uV, max_uV);
+}
+
+static int gpio_regulator_list_voltage(struct regulator_dev *dev,
+				      unsigned selector)
+{
+	struct gpio_regulator_data *data = rdev_get_drvdata(dev);
+
+	if (selector >= data->nr_states)
+		return -EINVAL;
+
+	return data->states[selector].value;
+}
+
+static int gpio_regulator_set_current_limit(struct regulator_dev *dev,
+					int min_uA, int max_uA)
+{
+	return gpio_regulator_set_value(dev, min_uA, max_uA);
+}
+
+static struct regulator_ops gpio_regulator_voltage_ops = {
+	.is_enabled = gpio_regulator_is_enabled,
+	.enable = gpio_regulator_enable,
+	.disable = gpio_regulator_disable,
+	.enable_time = gpio_regulator_enable_time,
+	.get_voltage = gpio_regulator_get_value,
+	.set_voltage = gpio_regulator_set_voltage,
+	.list_voltage = gpio_regulator_list_voltage,
+};
+
+static struct regulator_ops gpio_regulator_current_ops = {
+	.is_enabled = gpio_regulator_is_enabled,
+	.enable = gpio_regulator_enable,
+	.disable = gpio_regulator_disable,
+	.enable_time = gpio_regulator_enable_time,
+	.get_current_limit = gpio_regulator_get_value,
+	.set_current_limit = gpio_regulator_set_current_limit,
+};
+
+static int __devinit gpio_regulator_probe(struct platform_device *pdev)
+{
+	struct gpio_regulator_config *config = pdev->dev.platform_data;
+	struct gpio_regulator_data *drvdata;
+	int ptr, ret, state;
+
+	drvdata = kzalloc(sizeof(struct gpio_regulator_data), GFP_KERNEL);
+	if (drvdata == NULL) {
+		dev_err(&pdev->dev, "Failed to allocate device data\n");
+		return -ENOMEM;
+	}
+
+	drvdata->desc.name = kstrdup(config->supply_name, GFP_KERNEL);
+	if (drvdata->desc.name == NULL) {
+		dev_err(&pdev->dev, "Failed to allocate supply name\n");
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	drvdata->gpios = kmemdup(config->gpios,
+				 config->nr_gpios * sizeof(struct gpio),
+				 GFP_KERNEL);
+	if (drvdata->gpios == NULL) {
+		dev_err(&pdev->dev, "Failed to allocate gpio data\n");
+		ret = -ENOMEM;
+		goto err_name;
+	}
+
+	drvdata->states = kmemdup(config->states,
+				  config->nr_states *
+					 sizeof(struct gpio_regulator_state),
+				  GFP_KERNEL);
+	if (drvdata->states == NULL) {
+		dev_err(&pdev->dev, "Failed to allocate state data\n");
+		ret = -ENOMEM;
+		goto err_memgpio;
+	}
+	drvdata->nr_states = config->nr_states;
+
+	drvdata->desc.owner = THIS_MODULE;
+
+	/* handle regulator type*/
+	switch (config->type) {
+	case REGULATOR_VOLTAGE:
+		drvdata->desc.type = REGULATOR_VOLTAGE;
+		drvdata->desc.ops = &gpio_regulator_voltage_ops;
+		drvdata->desc.n_voltages = config->nr_states;
+		break;
+	case REGULATOR_CURRENT:
+		drvdata->desc.type = REGULATOR_CURRENT;
+		drvdata->desc.ops = &gpio_regulator_current_ops;
+		break;
+	default:
+		dev_err(&pdev->dev, "No regulator type set\n");
+		ret = -EINVAL;
+		goto err_memgpio;
+		break;
+	}
+
+	drvdata->enable_gpio = config->enable_gpio;
+	drvdata->startup_delay = config->startup_delay;
+
+	if (gpio_is_valid(config->enable_gpio)) {
+		drvdata->enable_high = config->enable_high;
+
+		ret = gpio_request(config->enable_gpio, config->supply_name);
+		if (ret) {
+			dev_err(&pdev->dev,
+			   "Could not obtain regulator enable GPIO %d: %d\n",
+						config->enable_gpio, ret);
+			goto err_memstate;
+		}
+
+		/* set output direction without changing state
+		 * to prevent glitch
+		 */
+		if (config->enabled_at_boot) {
+			drvdata->is_enabled = true;
+			ret = gpio_direction_output(config->enable_gpio,
+						    config->enable_high);
+		} else {
+			drvdata->is_enabled = false;
+			ret = gpio_direction_output(config->enable_gpio,
+						    !config->enable_high);
+		}
+
+		if (ret) {
+			dev_err(&pdev->dev,
+			   "Could not configure regulator enable GPIO %d direction: %d\n",
+						config->enable_gpio, ret);
+			goto err_enablegpio;
+		}
+	} else {
+		/* Regulator without GPIO control is considered
+		 * always enabled
+		 */
+		drvdata->is_enabled = true;
+	}
+
+	drvdata->nr_gpios = config->nr_gpios;
+	ret = gpio_request_array(drvdata->gpios, drvdata->nr_gpios);
+	if (ret) {
+		dev_err(&pdev->dev,
+		   "Could not obtain regulator setting GPIOs: %d\n", ret);
+		goto err_enablegpio;
+	}
+
+	/* build initial state from gpio init data. */
+	state = 0;
+	for (ptr = 0; ptr < drvdata->nr_gpios; ptr++) {
+		if (config->gpios[ptr].flags & GPIOF_OUT_INIT_HIGH)
+			state |= (1 << ptr);
+	}
+	drvdata->state = state;
+
+	drvdata->dev = regulator_register(&drvdata->desc, &pdev->dev,
+					  config->init_data, drvdata);
+	if (IS_ERR(drvdata->dev)) {
+		ret = PTR_ERR(drvdata->dev);
+		dev_err(&pdev->dev, "Failed to register regulator: %d\n", ret);
+		goto err_stategpio;
+	}
+
+	platform_set_drvdata(pdev, drvdata);
+
+	return 0;
+
+err_stategpio:
+	gpio_free_array(drvdata->gpios, drvdata->nr_gpios);
+err_enablegpio:
+	if (gpio_is_valid(config->enable_gpio))
+		gpio_free(config->enable_gpio);
+err_memstate:
+	kfree(drvdata->states);
+err_memgpio:
+	kfree(drvdata->gpios);
+err_name:
+	kfree(drvdata->desc.name);
+err:
+	kfree(drvdata);
+	return ret;
+}
+
+static int __devexit gpio_regulator_remove(struct platform_device *pdev)
+{
+	struct gpio_regulator_data *drvdata = platform_get_drvdata(pdev);
+
+	regulator_unregister(drvdata->dev);
+
+	gpio_free_array(drvdata->gpios, drvdata->nr_gpios);
+
+	kfree(drvdata->states);
+	kfree(drvdata->gpios);
+
+	if (gpio_is_valid(drvdata->enable_gpio))
+		gpio_free(drvdata->enable_gpio);
+
+	kfree(drvdata->desc.name);
+	kfree(drvdata);
+
+	return 0;
+}
+
+static struct platform_driver gpio_regulator_driver = {
+	.probe		= gpio_regulator_probe,
+	.remove		= __devexit_p(gpio_regulator_remove),
+	.driver		= {
+		.name		= "gpio-regulator",
+		.owner		= THIS_MODULE,
+	},
+};
+
+static int __init gpio_regulator_init(void)
+{
+	return platform_driver_register(&gpio_regulator_driver);
+}
+subsys_initcall(gpio_regulator_init);
+
+static void __exit gpio_regulator_exit(void)
+{
+	platform_driver_unregister(&gpio_regulator_driver);
+}
+module_exit(gpio_regulator_exit);
+
+MODULE_AUTHOR("Heiko Stuebner <heiko@sntech.de>");
+MODULE_DESCRIPTION("gpio voltage regulator");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:gpio-regulator");
diff --git a/drivers/regulator/max77663-regulator.c b/drivers/regulator/max77663-regulator.c
new file mode 100644
index 000000000000..55d2526b4490
--- /dev/null
+++ b/drivers/regulator/max77663-regulator.c
@@ -0,0 +1,926 @@
+/*
+ * drivers/regulator/max77663-regulator.c
+ * Maxim LDO and Buck regulators driver
+ *
+ * Copyright 2011-2012 Maxim Integrated Products, Inc.
+ * Copyright (C) 2011-2012 NVIDIA Corporation
+ *
+ * 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.
+ *
+ */
+
+#include <linux/err.h>
+#include <linux/string.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/debugfs.h>
+#include <linux/uaccess.h>
+#include <linux/i2c.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/max77663-core.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/max77663-regulator.h>
+
+/* Regulator types */
+#define REGULATOR_TYPE_SD		0
+#define REGULATOR_TYPE_LDO_N		1
+#define REGULATOR_TYPE_LDO_P		2
+
+/* SD and LDO Registers */
+#define MAX77663_REG_SD0		0x16
+#define MAX77663_REG_SD1		0x17
+#define MAX77663_REG_SD2		0x18
+#define MAX77663_REG_SD3		0x19
+#define MAX77663_REG_SD4		0x1A
+#define MAX77663_REG_DVSSD0		0x1B
+#define MAX77663_REG_DVSSD1		0x1C
+#define MAX77663_REG_SD0_CFG		0x1D
+#define MAX77663_REG_DVSSD0_CFG		MAX77663_REG_SD0_CFG
+#define MAX77663_REG_SD1_CFG		0x1E
+#define MAX77663_REG_DVSSD1_CFG		MAX77663_REG_SD1_CFG
+#define MAX77663_REG_SD2_CFG		0x1F
+#define MAX77663_REG_SD3_CFG		0x20
+#define MAX77663_REG_SD4_CFG		0x21
+#define MAX77663_REG_LDO0_CFG		0x23
+#define MAX77663_REG_LDO0_CFG2		0x24
+#define MAX77663_REG_LDO1_CFG		0x25
+#define MAX77663_REG_LDO1_CFG2		0x26
+#define MAX77663_REG_LDO2_CFG		0x27
+#define MAX77663_REG_LDO2_CFG2		0x28
+#define MAX77663_REG_LDO3_CFG		0x29
+#define MAX77663_REG_LDO3_CFG2		0x2A
+#define MAX77663_REG_LDO4_CFG		0x2B
+#define MAX77663_REG_LDO4_CFG2		0x2C
+#define MAX77663_REG_LDO5_CFG		0x2D
+#define MAX77663_REG_LDO5_CFG2		0x2E
+#define MAX77663_REG_LDO6_CFG		0x2F
+#define MAX77663_REG_LDO6_CFG2		0x30
+#define MAX77663_REG_LDO7_CFG		0x31
+#define MAX77663_REG_LDO7_CFG2		0x32
+#define MAX77663_REG_LDO8_CFG		0x33
+#define MAX77663_REG_LDO8_CFG2		0x34
+#define MAX77663_REG_LDO_CFG3		0x35
+
+/* Power Mode */
+#define POWER_MODE_NORMAL		3
+#define POWER_MODE_LPM			2
+#define POWER_MODE_GLPM			1
+#define POWER_MODE_DISABLE		0
+#define SD_POWER_MODE_MASK		0x30
+#define SD_POWER_MODE_SHIFT		4
+#define LDO_POWER_MODE_MASK		0xC0
+#define LDO_POWER_MODE_SHIFT		6
+
+/* SD Slew Rate */
+#define SD_SR_13_75			0
+#define SD_SR_27_5			1
+#define SD_SR_55			2
+#define SD_SR_100			3
+#define SD_SR_MASK			0xC0
+#define SD_SR_SHIFT			6
+
+/* SD Forced PWM Mode */
+#define SD_FPWM_MASK			0x04
+#define SD_FPWM_SHIFT			2
+
+/* SD Failling slew rate Active-Discharge Mode */
+#define SD_FSRADE_MASK			0x01
+#define SD_FSRADE_SHIFT		0
+
+/* LDO Configuration 3 */
+#define TRACK4_MASK			0x20
+#define TRACK4_SHIFT			5
+
+/* Voltage */
+#define SDX_VOLT_MASK			0xFF
+#define SD1_VOLT_MASK			0x3F
+#define LDO_VOLT_MASK			0x3F
+
+/* FPS Registers */
+#define MAX77663_REG_FPS_CFG0		0x43
+#define MAX77663_REG_FPS_CFG1		0x44
+#define MAX77663_REG_FPS_CFG2		0x45
+#define MAX77663_REG_FPS_LDO0		0x46
+#define MAX77663_REG_FPS_LDO1		0x47
+#define MAX77663_REG_FPS_LDO2		0x48
+#define MAX77663_REG_FPS_LDO3		0x49
+#define MAX77663_REG_FPS_LDO4		0x4A
+#define MAX77663_REG_FPS_LDO5		0x4B
+#define MAX77663_REG_FPS_LDO6		0x4C
+#define MAX77663_REG_FPS_LDO7		0x4D
+#define MAX77663_REG_FPS_LDO8		0x4E
+#define MAX77663_REG_FPS_SD0		0x4F
+#define MAX77663_REG_FPS_SD1		0x50
+#define MAX77663_REG_FPS_SD2		0x51
+#define MAX77663_REG_FPS_SD3		0x52
+#define MAX77663_REG_FPS_SD4		0x53
+#define MAX77663_REG_FPS_NONE		0
+
+#define FPS_TIME_PERIOD_MASK		0x38
+#define FPS_TIME_PERIOD_SHIFT		3
+#define FPS_EN_SRC_MASK			0x06
+#define FPS_EN_SRC_SHIFT		1
+#define FPS_SW_EN_MASK			0x01
+#define FPS_SW_EN_SHIFT			0
+#define FPS_SRC_MASK			0xC0
+#define FPS_SRC_SHIFT			6
+#define FPS_PU_PERIOD_MASK		0x38
+#define FPS_PU_PERIOD_SHIFT		3
+#define FPS_PD_PERIOD_MASK		0x07
+#define FPS_PD_PERIOD_SHIFT		0
+
+/* Chip Identification Register */
+#define MAX77663_REG_CID5		0x5D
+
+#define CID_DIDM_MASK			0xF0
+#define CID_DIDM_SHIFT			4
+
+#define SD_SAFE_DOWN_UV			50000 /* 50mV */
+
+enum {
+	VOLT_REG = 0,
+	CFG_REG,
+	FPS_REG,
+};
+
+struct max77663_register {
+	u8 addr;
+	u8 val;
+};
+
+struct max77663_regulator {
+	struct regulator_dev *rdev;
+	struct device *dev;
+	struct max77663_regulator_platform_data *pdata;
+
+	u8 id;
+	u8 type;
+	u32 min_uV;
+	u32 max_uV;
+	u32 step_uV;
+	int safe_down_uV; /* for stable down scaling */
+	u32 regulator_mode;
+
+	struct max77663_register regs[3]; /* volt, cfg, fps */
+	enum max77663_regulator_fps_src fps_src;
+
+	u8 volt_mask;
+
+	u8 power_mode;
+	u8 power_mode_mask;
+	u8 power_mode_shift;
+};
+
+#define fps_src_name(fps_src)	\
+	(fps_src == FPS_SRC_0 ? "FPS_SRC_0" :	\
+	fps_src == FPS_SRC_1 ? "FPS_SRC_1" :	\
+	fps_src == FPS_SRC_2 ? "FPS_SRC_2" : "FPS_SRC_NONE")
+
+static int fps_cfg_init;
+static struct max77663_register fps_cfg_regs[] = {
+	{
+		.addr = MAX77663_REG_FPS_CFG0,
+	},
+	{
+		.addr = MAX77663_REG_FPS_CFG1,
+	},
+	{
+		.addr = MAX77663_REG_FPS_CFG2,
+	},
+};
+
+static inline struct max77663_regulator_platform_data
+*_to_pdata(struct max77663_regulator *reg)
+{
+	return reg->pdata;
+}
+
+static inline struct device *_to_parent(struct max77663_regulator *reg)
+{
+	return reg->dev->parent;
+}
+
+static inline int max77663_regulator_cache_write(struct max77663_regulator *reg,
+					u8 addr, u8 mask, u8 val, u8 *cache)
+{
+	struct device *parent = _to_parent(reg);
+	u8 new_val;
+	int ret;
+
+	new_val = (*cache & ~mask) | (val & mask);
+	if (*cache != new_val) {
+		ret = max77663_write(parent, addr, &new_val, 1, 0);
+		if (ret < 0)
+			return ret;
+
+		*cache = new_val;
+	}
+	return 0;
+}
+
+static int
+max77663_regulator_set_fps_src(struct max77663_regulator *reg,
+			       enum max77663_regulator_fps_src fps_src)
+{
+	int ret;
+
+	if ((reg->regs[FPS_REG].addr == MAX77663_REG_FPS_NONE) ||
+			(reg->fps_src == fps_src))
+		return 0;
+
+	switch (fps_src) {
+	case FPS_SRC_0:
+	case FPS_SRC_1:
+	case FPS_SRC_2:
+	case FPS_SRC_NONE:
+		break;
+	case FPS_SRC_DEF:
+		return 0;
+	default:
+		return -EINVAL;
+	}
+
+	ret = max77663_regulator_cache_write(reg, reg->regs[FPS_REG].addr,
+					FPS_SRC_MASK, fps_src << FPS_SRC_SHIFT,
+					&reg->regs[FPS_REG].val);
+	if (ret < 0)
+		return ret;
+
+	reg->fps_src = fps_src;
+	return 0;
+}
+
+static int max77663_regulator_set_fps(struct max77663_regulator *reg)
+{
+	struct max77663_regulator_platform_data *pdata = _to_pdata(reg);
+	u8 fps_val = 0, fps_mask = 0;
+	int ret = 0;
+
+	if (reg->regs[FPS_REG].addr == MAX77663_REG_FPS_NONE)
+		return 0;
+
+	if (reg->fps_src == FPS_SRC_NONE)
+		return 0;
+
+	/* FPS power up period setting */
+	if (pdata->fps_pu_period != FPS_POWER_PERIOD_DEF) {
+		fps_val |= (pdata->fps_pu_period << FPS_PU_PERIOD_SHIFT);
+		fps_mask |= FPS_PU_PERIOD_MASK;
+	}
+
+	/* FPS power down period setting */
+	if (pdata->fps_pd_period != FPS_POWER_PERIOD_DEF) {
+		fps_val |= (pdata->fps_pd_period << FPS_PD_PERIOD_SHIFT);
+		fps_mask |= FPS_PD_PERIOD_MASK;
+	}
+
+	if (fps_val || fps_mask)
+		ret = max77663_regulator_cache_write(reg,
+					reg->regs[FPS_REG].addr, fps_mask,
+					fps_val, &reg->regs[FPS_REG].val);
+
+	return ret;
+}
+
+static int
+max77663_regulator_set_fps_cfg(struct max77663_regulator *reg,
+			       struct max77663_regulator_fps_cfg *fps_cfg)
+{
+	u8 val, mask;
+
+	if ((fps_cfg->src < FPS_SRC_0) || (fps_cfg->src > FPS_SRC_2))
+		return -EINVAL;
+
+	val = (fps_cfg->en_src << FPS_EN_SRC_SHIFT);
+	mask = FPS_EN_SRC_MASK;
+
+	if (fps_cfg->time_period != FPS_TIME_PERIOD_DEF) {
+		val |= (fps_cfg->time_period << FPS_TIME_PERIOD_SHIFT);
+		mask |= FPS_TIME_PERIOD_MASK;
+	}
+
+	return max77663_regulator_cache_write(reg,
+					fps_cfg_regs[fps_cfg->src].addr, mask,
+					val, &fps_cfg_regs[fps_cfg->src].val);
+}
+
+static int
+max77663_regulator_set_fps_cfgs(struct max77663_regulator *reg,
+				struct max77663_regulator_fps_cfg *fps_cfgs,
+				int num_fps_cfgs)
+{
+	struct device *parent = _to_parent(reg);
+	int i, ret;
+
+	if (fps_cfg_init)
+		return 0;
+
+	for (i = 0; i <= FPS_SRC_2; i++) {
+		ret = max77663_read(parent, fps_cfg_regs[i].addr,
+				    &fps_cfg_regs[i].val, 1, 0);
+		if (ret < 0)
+			return ret;
+	}
+
+	for (i = 0; i < num_fps_cfgs; i++) {
+		ret = max77663_regulator_set_fps_cfg(reg, &fps_cfgs[i]);
+		if (ret < 0)
+			return ret;
+	}
+	fps_cfg_init = 1;
+
+	return 0;
+}
+
+static int
+max77663_regulator_set_power_mode(struct max77663_regulator *reg, u8 power_mode)
+{
+	u8 mask = reg->power_mode_mask;
+	u8 shift = reg->power_mode_shift;
+	int ret;
+
+	if (reg->type == REGULATOR_TYPE_SD)
+		ret = max77663_regulator_cache_write(reg,
+						     reg->regs[CFG_REG].addr,
+						     mask, power_mode << shift,
+						     &reg->regs[CFG_REG].val);
+	else
+		ret = max77663_regulator_cache_write(reg,
+						     reg->regs[VOLT_REG].addr,
+						     mask, power_mode << shift,
+						     &reg->regs[VOLT_REG].val);
+
+	if (ret < 0)
+		return ret;
+
+	reg->power_mode = power_mode;
+	return ret;
+}
+
+static u8 max77663_regulator_get_power_mode(struct max77663_regulator *reg)
+{
+	u8 mask = reg->power_mode_mask;
+	u8 shift = reg->power_mode_shift;
+
+	if (reg->type == REGULATOR_TYPE_SD)
+		reg->power_mode = (reg->regs[CFG_REG].val & mask) >> shift;
+	else
+		reg->power_mode = (reg->regs[VOLT_REG].val & mask) >> shift;
+
+	return reg->power_mode;
+}
+
+static int max77663_regulator_do_set_voltage(struct max77663_regulator *reg,
+					     int min_uV, int max_uV)
+{
+	u8 addr = reg->regs[VOLT_REG].addr;
+	u8 mask = reg->volt_mask;
+	u8 *cache = &reg->regs[VOLT_REG].val;
+	u8 val;
+	int old_uV, new_uV, safe_uV;
+	int i, steps = 1;
+	int ret = 0;
+
+	if (min_uV < reg->min_uV || max_uV > reg->max_uV)
+		return -EDOM;
+
+	old_uV = (*cache & mask) * reg->step_uV + reg->min_uV;
+
+	if ((old_uV > min_uV) && (reg->safe_down_uV >= reg->step_uV)) {
+		steps = DIV_ROUND_UP(old_uV - min_uV, reg->safe_down_uV);
+		safe_uV = -reg->safe_down_uV;
+	}
+
+	if (steps == 1) {
+		val = (min_uV - reg->min_uV) / reg->step_uV;
+		ret = max77663_regulator_cache_write(reg, addr, mask, val,
+						     cache);
+	} else {
+		for (i = 0; i < steps; i++) {
+			if (abs(min_uV - old_uV) > abs(safe_uV))
+				new_uV = old_uV + safe_uV;
+			else
+				new_uV = min_uV;
+
+			dev_dbg(&reg->rdev->dev, "do_set_voltage: name=%s, "
+				"%d/%d, old_uV=%d, new_uV=%d\n",
+				reg->rdev->desc->name, i + 1, steps, old_uV,
+				new_uV);
+
+			val = (new_uV - reg->min_uV) / reg->step_uV;
+			ret = max77663_regulator_cache_write(reg, addr, mask,
+							     val, cache);
+			if (ret < 0)
+				return ret;
+
+			old_uV = new_uV;
+		}
+	}
+
+	return ret;
+}
+
+static int max77663_regulator_set_voltage(struct regulator_dev *rdev,
+					  int min_uV, int max_uV,
+					  unsigned *selector)
+{
+	struct max77663_regulator *reg = rdev_get_drvdata(rdev);
+
+	dev_dbg(&rdev->dev, "set_voltage: name=%s, min_uV=%d, max_uV=%d\n",
+		rdev->desc->name, min_uV, max_uV);
+	return max77663_regulator_do_set_voltage(reg, min_uV, max_uV);
+}
+
+static int max77663_regulator_get_voltage(struct regulator_dev *rdev)
+{
+	struct max77663_regulator *reg = rdev_get_drvdata(rdev);
+	int volt;
+
+	volt = (reg->regs[VOLT_REG].val & reg->volt_mask)
+		* reg->step_uV + reg->min_uV;
+
+	dev_dbg(&rdev->dev, "get_voltage: name=%s, volt=%d, val=0x%02x\n",
+		rdev->desc->name, volt, reg->regs[VOLT_REG].val);
+	return volt;
+}
+
+static int max77663_regulator_enable(struct regulator_dev *rdev)
+{
+	struct max77663_regulator *reg = rdev_get_drvdata(rdev);
+	struct max77663_regulator_platform_data *pdata = _to_pdata(reg);
+	int power_mode = (pdata->flags & GLPM_ENABLE) ?
+			 POWER_MODE_GLPM : POWER_MODE_NORMAL;
+
+	if (reg->fps_src != FPS_SRC_NONE) {
+		dev_dbg(&rdev->dev, "enable: Regulator %s using %s\n",
+			rdev->desc->name, fps_src_name(reg->fps_src));
+		return 0;
+	}
+
+	if ((reg->id == MAX77663_REGULATOR_ID_SD0)
+			&& (pdata->flags & EN2_CTRL_SD0)) {
+		dev_dbg(&rdev->dev,
+			"enable: Regulator %s is controlled by EN2\n",
+			rdev->desc->name);
+		return 0;
+	}
+
+	/* N-Channel LDOs don't support Low-Power mode. */
+	if ((reg->type != REGULATOR_TYPE_LDO_N) &&
+			(reg->regulator_mode == REGULATOR_MODE_STANDBY))
+		power_mode = POWER_MODE_LPM;
+
+	return max77663_regulator_set_power_mode(reg, power_mode);
+}
+
+static int max77663_regulator_disable(struct regulator_dev *rdev)
+{
+	struct max77663_regulator *reg = rdev_get_drvdata(rdev);
+	struct max77663_regulator_platform_data *pdata = _to_pdata(reg);
+	int power_mode = POWER_MODE_DISABLE;
+
+	if (reg->fps_src != FPS_SRC_NONE) {
+		dev_dbg(&rdev->dev, "disable: Regulator %s using %s\n",
+			rdev->desc->name, fps_src_name(reg->fps_src));
+		return 0;
+	}
+
+	if ((reg->id == MAX77663_REGULATOR_ID_SD0)
+			&& (pdata->flags & EN2_CTRL_SD0)) {
+		dev_dbg(&rdev->dev,
+			"disable: Regulator %s is controlled by EN2\n",
+			rdev->desc->name);
+		return 0;
+	}
+
+	return max77663_regulator_set_power_mode(reg, power_mode);
+}
+
+static int max77663_regulator_is_enabled(struct regulator_dev *rdev)
+{
+	struct max77663_regulator *reg = rdev_get_drvdata(rdev);
+	struct max77663_regulator_platform_data *pdata = _to_pdata(reg);
+	int ret = 1;
+
+	if (reg->fps_src != FPS_SRC_NONE) {
+		dev_dbg(&rdev->dev, "is_enable: Regulator %s using %s\n",
+			rdev->desc->name, fps_src_name(reg->fps_src));
+		return 1;
+	}
+
+	if ((reg->id == MAX77663_REGULATOR_ID_SD0)
+			&& (pdata->flags & EN2_CTRL_SD0)) {
+		dev_dbg(&rdev->dev,
+			"is_enable: Regulator %s is controlled by EN2\n",
+			rdev->desc->name);
+		return 1;
+	}
+
+	if (max77663_regulator_get_power_mode(reg) == POWER_MODE_DISABLE)
+		ret = 0;
+
+	return ret;
+}
+
+static int max77663_regulator_set_mode(struct regulator_dev *rdev,
+				       unsigned int mode)
+{
+	struct max77663_regulator *reg = rdev_get_drvdata(rdev);
+	struct max77663_regulator_platform_data *pdata = _to_pdata(reg);
+	u8 power_mode;
+	int ret;
+
+	if (mode == REGULATOR_MODE_NORMAL)
+		power_mode = (pdata->flags & GLPM_ENABLE) ?
+			     POWER_MODE_GLPM : POWER_MODE_NORMAL;
+	else if (mode == REGULATOR_MODE_STANDBY) {
+		/* N-Channel LDOs don't support Low-Power mode. */
+		power_mode = (reg->type != REGULATOR_TYPE_LDO_N) ?
+			     POWER_MODE_LPM : POWER_MODE_NORMAL;
+	} else
+		return -EINVAL;
+
+	ret = max77663_regulator_set_power_mode(reg, power_mode);
+	if (!ret)
+		reg->regulator_mode = mode;
+
+	return ret;
+}
+
+static unsigned int max77663_regulator_get_mode(struct regulator_dev *rdev)
+{
+	struct max77663_regulator *reg = rdev_get_drvdata(rdev);
+
+	return reg->regulator_mode;
+}
+
+static struct regulator_ops max77663_ldo_ops = {
+	.set_voltage = max77663_regulator_set_voltage,
+	.get_voltage = max77663_regulator_get_voltage,
+	.enable = max77663_regulator_enable,
+	.disable = max77663_regulator_disable,
+	.is_enabled = max77663_regulator_is_enabled,
+	.set_mode = max77663_regulator_set_mode,
+	.get_mode = max77663_regulator_get_mode,
+};
+
+static int max77663_regulator_preinit(struct max77663_regulator *reg)
+{
+	struct max77663_regulator_platform_data *pdata = _to_pdata(reg);
+	struct device *parent = _to_parent(reg);
+	int i;
+	u8 val, mask;
+	int ret;
+
+	/* Update registers */
+	for (i = 0; i <= FPS_REG; i++) {
+		ret = max77663_read(parent, reg->regs[i].addr,
+				    &reg->regs[i].val, 1, 0);
+		if (ret < 0) {
+			dev_err(reg->dev,
+				"preinit: Failed to get register 0x%x\n",
+				reg->regs[i].addr);
+			return ret;
+		}
+	}
+
+	/* Update FPS source */
+	if (reg->regs[FPS_REG].addr == MAX77663_REG_FPS_NONE)
+		reg->fps_src = FPS_SRC_NONE;
+	else
+		reg->fps_src = (reg->regs[FPS_REG].val & FPS_SRC_MASK)
+				>> FPS_SRC_SHIFT;
+
+	dev_dbg(reg->dev, "preinit: initial fps_src=%s\n",
+		fps_src_name(reg->fps_src));
+
+	/* Update power mode */
+	max77663_regulator_get_power_mode(reg);
+
+	/* Check Chip Identification */
+	ret = max77663_read(parent, MAX77663_REG_CID5, &val, 1, 0);
+	if (ret < 0) {
+		dev_err(reg->dev, "preinit: Failed to get register 0x%x\n",
+			MAX77663_REG_CID5);
+		return ret;
+	}
+
+	/* If metal revision is less than rev.3,
+	 * set safe_down_uV for stable down scaling. */
+	if ((reg->type == REGULATOR_TYPE_SD) &&
+			((val & CID_DIDM_MASK) >> CID_DIDM_SHIFT) <= 2)
+		reg->safe_down_uV = SD_SAFE_DOWN_UV;
+	else
+		reg->safe_down_uV = 0;
+
+	/* Set FPS */
+	ret = max77663_regulator_set_fps_cfgs(reg, pdata->fps_cfgs,
+					      pdata->num_fps_cfgs);
+	if (ret < 0) {
+		dev_err(reg->dev, "preinit: Failed to set FPSCFG\n");
+		return ret;
+	}
+
+	/* N-Channel LDOs don't support Low-Power mode. */
+	if ((reg->type == REGULATOR_TYPE_LDO_N) &&
+			(pdata->flags & GLPM_ENABLE))
+		pdata->flags &= ~GLPM_ENABLE;
+
+	/* To prevent power rail turn-off when change FPS source,
+	 * it must set power mode to NORMAL before change FPS source to NONE
+	 * from SRC_0, SRC_1 and SRC_2. */
+	if ((reg->fps_src != FPS_SRC_NONE) && (pdata->fps_src == FPS_SRC_NONE)
+			&& (reg->power_mode != POWER_MODE_NORMAL)) {
+		val = (pdata->flags & GLPM_ENABLE) ?
+		      POWER_MODE_GLPM : POWER_MODE_NORMAL;
+		ret = max77663_regulator_set_power_mode(reg, val);
+		if (ret < 0) {
+			dev_err(reg->dev, "preinit: Failed to "
+				"set power mode to POWER_MODE_NORMAL\n");
+			return ret;
+		}
+	}
+
+	ret = max77663_regulator_set_fps_src(reg, pdata->fps_src);
+	if (ret < 0) {
+		dev_err(reg->dev, "preinit: Failed to set FPSSRC to %d\n",
+			pdata->fps_src);
+		return ret;
+	}
+
+	ret = max77663_regulator_set_fps(reg);
+	if (ret < 0) {
+		dev_err(reg->dev, "preinit: Failed to set FPS\n");
+		return ret;
+	}
+
+	/* Set initial state */
+	if (!pdata->init_apply)
+		goto skip_init_apply;
+
+	if (pdata->init_uV >= 0) {
+		ret = max77663_regulator_do_set_voltage(reg, pdata->init_uV,
+							pdata->init_uV);
+		if (ret < 0) {
+			dev_err(reg->dev, "preinit: Failed to set voltage to "
+				"%d\n", pdata->init_uV);
+			return ret;
+		}
+	}
+
+	if (pdata->init_enable)
+		val = (pdata->flags & GLPM_ENABLE) ?
+		      POWER_MODE_GLPM : POWER_MODE_NORMAL;
+	else
+		val = POWER_MODE_DISABLE;
+
+	ret = max77663_regulator_set_power_mode(reg, val);
+	if (ret < 0) {
+		dev_err(reg->dev,
+			"preinit: Failed to set power mode to %d\n", val);
+		return ret;
+	}
+
+skip_init_apply:
+	if (reg->type == REGULATOR_TYPE_SD) {
+		val = 0;
+		mask = 0;
+
+		if (pdata->flags & SD_SLEW_RATE_MASK) {
+			mask |= SD_SR_MASK;
+			if (pdata->flags & SD_SLEW_RATE_SLOWEST)
+				val |= (SD_SR_13_75 << SD_SR_SHIFT);
+			else if (pdata->flags & SD_SLEW_RATE_SLOW)
+				val |= (SD_SR_27_5 << SD_SR_SHIFT);
+			else if (pdata->flags & SD_SLEW_RATE_FAST)
+				val |= (SD_SR_55 << SD_SR_SHIFT);
+			else
+				val |= (SD_SR_100 << SD_SR_SHIFT);
+		}
+
+		mask |= SD_FPWM_MASK;
+		if (pdata->flags & SD_FORCED_PWM_MODE)
+			val |= SD_FPWM_MASK;
+
+		mask |= SD_FSRADE_MASK;
+		if (pdata->flags & SD_FSRADE_DISABLE)
+			val |= SD_FSRADE_MASK;
+
+		ret = max77663_regulator_cache_write(reg,
+				reg->regs[CFG_REG].addr, mask, val,
+				&reg->regs[CFG_REG].val);
+		if (ret < 0) {
+			dev_err(reg->dev, "preinit: "
+				"Failed to set register 0x%x\n",
+				reg->regs[CFG_REG].addr);
+			return ret;
+		}
+
+		if ((reg->id == MAX77663_REGULATOR_ID_SD0)
+				&& (pdata->flags & EN2_CTRL_SD0)) {
+			val = POWER_MODE_DISABLE;
+			ret = max77663_regulator_set_power_mode(reg, val);
+			if (ret < 0) {
+				dev_err(reg->dev, "preinit: "
+					"Failed to set power mode to %d for "
+					"EN2_CTRL_SD0\n", val);
+				return ret;
+			}
+
+			ret = max77663_regulator_set_fps_src(reg, FPS_SRC_NONE);
+			if (ret < 0) {
+				dev_err(reg->dev, "preinit: "
+					"Failed to set FPSSRC to FPS_SRC_NONE "
+					"for EN2_CTRL_SD0\n");
+				return ret;
+			}
+		}
+	}
+
+	if ((reg->id == MAX77663_REGULATOR_ID_LDO4)
+			&& (pdata->flags & LDO4_EN_TRACKING)) {
+		val = TRACK4_MASK;
+		ret = max77663_write(parent, MAX77663_REG_LDO_CFG3, &val, 1, 0);
+		if (ret < 0) {
+			dev_err(reg->dev, "preinit: "
+				"Failed to set register 0x%x\n",
+				MAX77663_REG_LDO_CFG3);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+#define REGULATOR_SD(_id, _volt_mask, _fps_reg, _min_uV, _max_uV, _step_uV) \
+	[MAX77663_REGULATOR_ID_##_id] = {			\
+		.id = MAX77663_REGULATOR_ID_##_id,		\
+		.type = REGULATOR_TYPE_SD,			\
+		.volt_mask = _volt_mask##_VOLT_MASK,		\
+		.regs = {					\
+			[VOLT_REG] = {				\
+				.addr = MAX77663_REG_##_id,	\
+			},					\
+			[CFG_REG] = {				\
+				.addr = MAX77663_REG_##_id##_CFG, \
+			},					\
+			[FPS_REG] = {				\
+				.addr = MAX77663_REG_FPS_##_fps_reg, \
+			},					\
+		},						\
+		.min_uV = _min_uV,				\
+		.max_uV = _max_uV,				\
+		.step_uV = _step_uV,				\
+		.regulator_mode = REGULATOR_MODE_NORMAL,	\
+		.power_mode = POWER_MODE_NORMAL,		\
+		.power_mode_mask = SD_POWER_MODE_MASK,		\
+		.power_mode_shift = SD_POWER_MODE_SHIFT,	\
+	}
+
+#define REGULATOR_LDO(_id, _type, _min_uV, _max_uV, _step_uV)	\
+	[MAX77663_REGULATOR_ID_##_id] = {			\
+		.id = MAX77663_REGULATOR_ID_##_id,		\
+		.type = REGULATOR_TYPE_LDO_##_type,		\
+		.volt_mask = LDO_VOLT_MASK,			\
+		.regs = {					\
+			[VOLT_REG] = {				\
+				.addr = MAX77663_REG_##_id##_CFG, \
+			},					\
+			[CFG_REG] = {				\
+				.addr = MAX77663_REG_##_id##_CFG2, \
+			},					\
+			[FPS_REG] = {				\
+				.addr = MAX77663_REG_FPS_##_id,	\
+			},					\
+		},						\
+		.min_uV = _min_uV,				\
+		.max_uV = _max_uV,				\
+		.step_uV = _step_uV,				\
+		.regulator_mode = REGULATOR_MODE_NORMAL,	\
+		.power_mode = POWER_MODE_NORMAL,		\
+		.power_mode_mask = LDO_POWER_MODE_MASK,		\
+		.power_mode_shift = LDO_POWER_MODE_SHIFT,	\
+	}
+
+static struct max77663_regulator max77663_regs[MAX77663_REGULATOR_ID_NR] = {
+	REGULATOR_SD(SD0,    SDX, SD0,  600000, 3387500, 12500),
+	REGULATOR_SD(DVSSD0, SDX, NONE, 600000, 3387500, 12500),
+	REGULATOR_SD(SD1,    SD1, SD1,  800000, 1587500, 12500),
+	REGULATOR_SD(DVSSD1, SD1, NONE, 800000, 1587500, 12500),
+	REGULATOR_SD(SD2,    SDX, SD2,  600000, 3387500, 12500),
+	REGULATOR_SD(SD3,    SDX, SD3,  600000, 3387500, 12500),
+	REGULATOR_SD(SD4,    SDX, SD4,  600000, 3387500, 12500),
+
+	REGULATOR_LDO(LDO0, N, 800000, 2350000, 25000),
+	REGULATOR_LDO(LDO1, N, 800000, 2350000, 25000),
+	REGULATOR_LDO(LDO2, P, 800000, 3950000, 50000),
+	REGULATOR_LDO(LDO3, P, 800000, 3950000, 50000),
+	REGULATOR_LDO(LDO4, P, 800000, 1587500, 12500),
+	REGULATOR_LDO(LDO5, P, 800000, 3950000, 50000),
+	REGULATOR_LDO(LDO6, P, 800000, 3950000, 50000),
+	REGULATOR_LDO(LDO7, N, 800000, 3950000, 50000),
+	REGULATOR_LDO(LDO8, N, 800000, 3950000, 50000),
+};
+
+#define REGULATOR_DESC(_id, _name)			\
+	[MAX77663_REGULATOR_ID_##_id] = {		\
+		.name = max77663_rails(_name),		\
+		.id = MAX77663_REGULATOR_ID_##_id,	\
+		.ops = &max77663_ldo_ops,		\
+		.type = REGULATOR_VOLTAGE,		\
+		.owner = THIS_MODULE,			\
+	}
+
+static struct regulator_desc max77663_rdesc[MAX77663_REGULATOR_ID_NR] = {
+	REGULATOR_DESC(SD0, sd0),
+	REGULATOR_DESC(DVSSD0, dvssd0),
+	REGULATOR_DESC(SD1, sd1),
+	REGULATOR_DESC(DVSSD1, dvssd1),
+	REGULATOR_DESC(SD2, sd2),
+	REGULATOR_DESC(SD3, sd3),
+	REGULATOR_DESC(SD4, sd4),
+	REGULATOR_DESC(LDO0, ldo0),
+	REGULATOR_DESC(LDO1, ldo1),
+	REGULATOR_DESC(LDO2, ldo2),
+	REGULATOR_DESC(LDO3, ldo3),
+	REGULATOR_DESC(LDO4, ldo4),
+	REGULATOR_DESC(LDO5, ldo5),
+	REGULATOR_DESC(LDO6, ldo6),
+	REGULATOR_DESC(LDO7, ldo7),
+	REGULATOR_DESC(LDO8, ldo8),
+};
+
+static int max77663_regulator_probe(struct platform_device *pdev)
+{
+	struct regulator_desc *rdesc;
+	struct max77663_regulator *reg;
+	int ret = 0;
+
+	if ((pdev->id < 0) || (pdev->id >= MAX77663_REGULATOR_ID_NR)) {
+		dev_err(&pdev->dev, "Invalid device id %d\n", pdev->id);
+		return -ENODEV;
+	}
+
+	rdesc = &max77663_rdesc[pdev->id];
+	reg = &max77663_regs[pdev->id];
+	reg->dev = &pdev->dev;
+	reg->pdata = dev_get_platdata(&pdev->dev);
+
+	dev_dbg(&pdev->dev, "probe: name=%s\n", rdesc->name);
+
+	ret = max77663_regulator_preinit(reg);
+	if (ret) {
+		dev_err(&pdev->dev, "probe: Failed to preinit regulator %s\n",
+			rdesc->name);
+		return ret;
+	}
+
+	reg->rdev = regulator_register(rdesc, &pdev->dev,
+				       &reg->pdata->init_data, reg);
+	if (IS_ERR(reg->rdev)) {
+		dev_err(&pdev->dev, "probe: Failed to register regulator %s\n",
+			rdesc->name);
+		return PTR_ERR(reg->rdev);
+	}
+
+	return 0;
+}
+
+static int max77663_regulator_remove(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev = platform_get_drvdata(pdev);
+
+	regulator_unregister(rdev);
+	return 0;
+}
+
+static struct platform_driver max77663_regulator_driver = {
+	.probe = max77663_regulator_probe,
+	.remove = __devexit_p(max77663_regulator_remove),
+	.driver = {
+		.name = "max77663-regulator",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init max77663_regulator_init(void)
+{
+	return platform_driver_register(&max77663_regulator_driver);
+}
+subsys_initcall(max77663_regulator_init);
+
+static void __exit max77663_reg_exit(void)
+{
+	platform_driver_unregister(&max77663_regulator_driver);
+}
+module_exit(max77663_reg_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("max77663 regulator driver");
+MODULE_VERSION("1.0");
diff --git a/drivers/regulator/max8907c-regulator.c b/drivers/regulator/max8907c-regulator.c
new file mode 100644
index 000000000000..bc86f927ead3
--- /dev/null
+++ b/drivers/regulator/max8907c-regulator.c
@@ -0,0 +1,437 @@
+/*
+ * max8907c-regulator.c -- support regulators in max8907c
+ *
+ * Copyright (C) 2010 Gyungoh Yoo <jack.yoo@maxim-ic.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/mfd/max8907c.h>
+#include <linux/regulator/max8907c-regulator.h>
+
+#define MAX8907C_II2RR_VERSION_MASK	0xF0
+#define MAX8907C_II2RR_VERSION_REV_A	0x00
+#define MAX8907C_II2RR_VERSION_REV_B	0x10
+#define MAX8907C_II2RR_VERSION_REV_C	0x30
+
+#define MAX8907C_REGULATOR_CNT (ARRAY_SIZE(max8907c_regulators))
+
+struct max8907c_regulator_info {
+	u32 min_uV;
+	u32 max_uV;
+	u32 step_uV;
+	u8 reg_base;
+	u32 enable_time_us;
+	struct regulator_desc desc;
+	struct i2c_client *i2c;
+};
+
+#define REG_LDO(ids, base, min, max, step) \
+	{ \
+		.min_uV = (min), \
+		.max_uV = (max), \
+		.step_uV = (step), \
+		.reg_base = (base), \
+		.desc = { \
+			.name = #ids, \
+			.id = MAX8907C_##ids, \
+			.n_voltages = ((max) - (min)) / (step) + 1, \
+			.ops = &max8907c_ldo_ops, \
+			.type = REGULATOR_VOLTAGE, \
+			.owner = THIS_MODULE, \
+		}, \
+	}
+
+#define REG_FIXED(ids, voltage) \
+	{ \
+		.min_uV = (voltage), \
+		.max_uV = (voltage), \
+		.desc = { \
+			.name = #ids, \
+			.id = MAX8907C_##ids, \
+			.n_voltages = 1, \
+			.ops = &max8907c_fixed_ops, \
+			.type = REGULATOR_VOLTAGE, \
+			.owner = THIS_MODULE, \
+		}, \
+	}
+
+#define REG_OUT5V(ids, base, voltage) \
+	{ \
+		.min_uV = (voltage), \
+		.max_uV = (voltage), \
+		.reg_base = (base), \
+		.desc = { \
+			.name = #ids, \
+			.id = MAX8907C_##ids, \
+			.n_voltages = 1, \
+			.ops = &max8907c_out5v_ops, \
+			.type = REGULATOR_VOLTAGE, \
+			.owner = THIS_MODULE, \
+		}, \
+	}
+
+#define REG_BBAT(ids, base, min, max, step) \
+	{ \
+		.min_uV = (min), \
+		.max_uV = (max), \
+		.step_uV = (step), \
+		.reg_base = (base), \
+		.desc = { \
+			.name = #ids, \
+			.id = MAX8907C_##ids, \
+			.n_voltages = ((max) - (min)) / (step) + 1, \
+			.ops = &max8907c_bbat_ops, \
+			.type = REGULATOR_VOLTAGE, \
+			.owner = THIS_MODULE, \
+		}, \
+	}
+
+#define REG_WLED(ids, base, voltage) \
+	{ \
+		.min_uV = (voltage), \
+		.max_uV = (voltage), \
+		.reg_base = (base), \
+		.desc = { \
+			.name = #ids, \
+			.id = MAX8907C_##ids, \
+			.n_voltages = 1, \
+			.ops = &max8907c_wled_ops, \
+			.type = REGULATOR_CURRENT, \
+			.owner = THIS_MODULE, \
+		}, \
+	}
+
+#define LDO_750_50(id, base) REG_LDO(id, (base), 750000, 3900000, 50000)
+#define LDO_650_25(id, base) REG_LDO(id, (base), 650000, 2225000, 25000)
+
+static int max8907c_regulator_list_voltage(struct regulator_dev *dev,
+					   unsigned index);
+static int max8907c_regulator_ldo_set_voltage(struct regulator_dev *dev,
+					      int min_uV, int max_uV);
+static int max8907c_regulator_bbat_set_voltage(struct regulator_dev *dev,
+					       int min_uV, int max_uV);
+static int max8907c_regulator_ldo_get_voltage(struct regulator_dev *dev);
+static int max8907c_regulator_fixed_get_voltage(struct regulator_dev *dev);
+static int max8907c_regulator_bbat_get_voltage(struct regulator_dev *dev);
+static int max8907c_regulator_wled_set_current_limit(struct regulator_dev *dev,
+						     int min_uA, int max_uA);
+static int max8907c_regulator_wled_get_current_limit(struct regulator_dev *dev);
+static int max8907c_regulator_ldo_enable(struct regulator_dev *dev);
+static int max8907c_regulator_out5v_enable(struct regulator_dev *dev);
+static int max8907c_regulator_ldo_disable(struct regulator_dev *dev);
+static int max8907c_regulator_out5v_disable(struct regulator_dev *dev);
+static int max8907c_regulator_ldo_is_enabled(struct regulator_dev *dev);
+static int max8907c_regulator_ldo_enable_time(struct regulator_dev *dev);
+static int max8907c_regulator_out5v_is_enabled(struct regulator_dev *dev);
+
+static struct regulator_ops max8907c_ldo_ops = {
+	.list_voltage = max8907c_regulator_list_voltage,
+	.set_voltage = max8907c_regulator_ldo_set_voltage,
+	.get_voltage = max8907c_regulator_ldo_get_voltage,
+	.enable = max8907c_regulator_ldo_enable,
+	.disable = max8907c_regulator_ldo_disable,
+	.is_enabled = max8907c_regulator_ldo_is_enabled,
+	.enable_time = max8907c_regulator_ldo_enable_time,
+};
+
+static struct regulator_ops max8907c_fixed_ops = {
+	.list_voltage = max8907c_regulator_list_voltage,
+	.get_voltage = max8907c_regulator_fixed_get_voltage,
+};
+
+static struct regulator_ops max8907c_out5v_ops = {
+	.list_voltage = max8907c_regulator_list_voltage,
+	.get_voltage = max8907c_regulator_fixed_get_voltage,
+	.enable = max8907c_regulator_out5v_enable,
+	.disable = max8907c_regulator_out5v_disable,
+	.is_enabled = max8907c_regulator_out5v_is_enabled,
+};
+
+static struct regulator_ops max8907c_bbat_ops = {
+	.list_voltage = max8907c_regulator_list_voltage,
+	.set_voltage = max8907c_regulator_bbat_set_voltage,
+	.get_voltage = max8907c_regulator_bbat_get_voltage,
+};
+
+static struct regulator_ops max8907c_wled_ops = {
+	.list_voltage = max8907c_regulator_list_voltage,
+	.set_current_limit = max8907c_regulator_wled_set_current_limit,
+	.get_current_limit = max8907c_regulator_wled_get_current_limit,
+	.get_voltage = max8907c_regulator_fixed_get_voltage,
+};
+
+static struct max8907c_regulator_info max8907c_regulators[] = {
+	REG_LDO(SD1, MAX8907C_REG_SDCTL1, 650000, 2225000, 25000),
+	REG_LDO(SD2, MAX8907C_REG_SDCTL2, 637500, 1425000, 12500),
+	REG_LDO(SD3, MAX8907C_REG_SDCTL3, 750000, 3900000, 50000),
+	LDO_750_50(LDO1, MAX8907C_REG_LDOCTL1),
+	LDO_650_25(LDO2, MAX8907C_REG_LDOCTL2),
+	LDO_650_25(LDO3, MAX8907C_REG_LDOCTL3),
+	LDO_750_50(LDO4, MAX8907C_REG_LDOCTL4),
+	LDO_750_50(LDO5, MAX8907C_REG_LDOCTL5),
+	LDO_750_50(LDO6, MAX8907C_REG_LDOCTL6),
+	LDO_750_50(LDO7, MAX8907C_REG_LDOCTL7),
+	LDO_750_50(LDO8, MAX8907C_REG_LDOCTL8),
+	LDO_750_50(LDO9, MAX8907C_REG_LDOCTL9),
+	LDO_750_50(LDO10, MAX8907C_REG_LDOCTL10),
+	LDO_750_50(LDO11, MAX8907C_REG_LDOCTL11),
+	LDO_750_50(LDO12, MAX8907C_REG_LDOCTL12),
+	LDO_750_50(LDO13, MAX8907C_REG_LDOCTL13),
+	LDO_750_50(LDO14, MAX8907C_REG_LDOCTL14),
+	LDO_750_50(LDO15, MAX8907C_REG_LDOCTL15),
+	LDO_750_50(LDO16, MAX8907C_REG_LDOCTL16),
+	LDO_650_25(LDO17, MAX8907C_REG_LDOCTL17),
+	LDO_650_25(LDO18, MAX8907C_REG_LDOCTL18),
+	LDO_750_50(LDO19, MAX8907C_REG_LDOCTL19),
+	LDO_750_50(LDO20, MAX8907C_REG_LDOCTL20),
+	REG_OUT5V(OUT5V, MAX8907C_REG_OUT5VEN, 5000000),
+	REG_OUT5V(OUT33V, MAX8907C_REG_OUT33VEN, 3300000),
+	REG_BBAT(BBAT, MAX8907C_REG_BBAT_CNFG, 2400000, 3000000, 200000),
+	REG_FIXED(SDBY, 1200000),
+	REG_FIXED(VRTC, 3300000),
+	REG_WLED(WLED, MAX8907C_REG_ILED_CNTL, 0),
+};
+
+static int max8907c_regulator_list_voltage(struct regulator_dev *rdev,
+					   unsigned index)
+{
+	const struct max8907c_regulator_info *info = rdev_get_drvdata(rdev);
+
+	return info->min_uV + info->step_uV * index;
+}
+
+static int max8907c_regulator_ldo_set_voltage(struct regulator_dev *rdev,
+					      int min_uV, int max_uV)
+{
+	const struct max8907c_regulator_info *info = rdev_get_drvdata(rdev);
+	int val;
+
+	if (min_uV < info->min_uV || max_uV > info->max_uV)
+		return -EDOM;
+
+	val = (min_uV - info->min_uV) / info->step_uV;
+
+	return max8907c_reg_write(info->i2c, info->reg_base + MAX8907C_VOUT, val);
+}
+
+static int max8907c_regulator_bbat_set_voltage(struct regulator_dev *rdev,
+					       int min_uV, int max_uV)
+{
+	const struct max8907c_regulator_info *info = rdev_get_drvdata(rdev);
+	int val;
+
+	if (min_uV < info->min_uV || max_uV > info->max_uV)
+		return -EDOM;
+
+	val = (min_uV - info->min_uV) / info->step_uV;
+
+	return max8907c_set_bits(info->i2c, info->reg_base, MAX8907C_MASK_VBBATTCV,
+				 val);
+}
+
+static int max8907c_regulator_ldo_get_voltage(struct regulator_dev *rdev)
+{
+	const struct max8907c_regulator_info *info = rdev_get_drvdata(rdev);
+	int val;
+
+	val = max8907c_reg_read(info->i2c, info->reg_base + MAX8907C_VOUT);
+	return val * info->step_uV + info->min_uV;
+}
+
+static int max8907c_regulator_fixed_get_voltage(struct regulator_dev *rdev)
+{
+	const struct max8907c_regulator_info *info = rdev_get_drvdata(rdev);
+
+	return info->min_uV;
+}
+
+static int max8907c_regulator_bbat_get_voltage(struct regulator_dev *rdev)
+{
+	const struct max8907c_regulator_info *info = rdev_get_drvdata(rdev);
+	int val;
+
+	val =
+	    max8907c_reg_read(info->i2c, info->reg_base) & MAX8907C_MASK_VBBATTCV;
+	return val * info->step_uV + info->min_uV;
+}
+
+static int max8907c_regulator_wled_set_current_limit(struct regulator_dev *rdev,
+						     int min_uA, int max_uA)
+{
+	const struct max8907c_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (min_uA > 25500)
+		return -EDOM;
+
+	return max8907c_reg_write(info->i2c, info->reg_base, min_uA / 100);
+}
+
+static int max8907c_regulator_wled_get_current_limit(struct regulator_dev *rdev)
+{
+	const struct max8907c_regulator_info *info = rdev_get_drvdata(rdev);
+	int val;
+
+	val = max8907c_reg_read(info->i2c, info->reg_base);
+	return val * 100;
+}
+
+static int max8907c_regulator_ldo_enable(struct regulator_dev *rdev)
+{
+	const struct max8907c_regulator_info *info = rdev_get_drvdata(rdev);
+
+	return max8907c_set_bits(info->i2c, info->reg_base + MAX8907C_CTL,
+				 MAX8907C_MASK_LDO_EN | MAX8907C_MASK_LDO_SEQ,
+				 MAX8907C_MASK_LDO_EN | MAX8907C_MASK_LDO_SEQ);
+}
+
+static int max8907c_regulator_out5v_enable(struct regulator_dev *rdev)
+{
+	const struct max8907c_regulator_info *info = rdev_get_drvdata(rdev);
+
+	return max8907c_set_bits(info->i2c, info->reg_base,
+				 MAX8907C_MASK_OUT5V_VINEN |
+				 MAX8907C_MASK_OUT5V_ENSRC |
+				 MAX8907C_MASK_OUT5V_EN,
+				 MAX8907C_MASK_OUT5V_ENSRC |
+				 MAX8907C_MASK_OUT5V_EN);
+}
+
+static int max8907c_regulator_ldo_disable(struct regulator_dev *rdev)
+{
+	const struct max8907c_regulator_info *info = rdev_get_drvdata(rdev);
+
+	return max8907c_set_bits(info->i2c, info->reg_base + MAX8907C_CTL,
+				 MAX8907C_MASK_LDO_EN | MAX8907C_MASK_LDO_SEQ,
+				 MAX8907C_MASK_LDO_SEQ);
+}
+
+static int max8907c_regulator_out5v_disable(struct regulator_dev *rdev)
+{
+	const struct max8907c_regulator_info *info = rdev_get_drvdata(rdev);
+
+	return max8907c_set_bits(info->i2c, info->reg_base,
+				 MAX8907C_MASK_OUT5V_VINEN |
+				 MAX8907C_MASK_OUT5V_ENSRC |
+				 MAX8907C_MASK_OUT5V_EN,
+				 MAX8907C_MASK_OUT5V_ENSRC);
+}
+
+static int max8907c_regulator_ldo_is_enabled(struct regulator_dev *rdev)
+{
+	const struct max8907c_regulator_info *info = rdev_get_drvdata(rdev);
+	int val;
+
+	val = max8907c_reg_read(info->i2c, info->reg_base + MAX8907C_CTL);
+	if (val < 0)
+		return -EDOM;
+
+	return (val & MAX8907C_MASK_LDO_EN) || !(val & MAX8907C_MASK_LDO_SEQ);
+}
+
+static int max8907c_regulator_ldo_enable_time(struct regulator_dev *rdev)
+{
+	struct max8907c_regulator_info *info = rdev_get_drvdata(rdev);
+
+	return info->enable_time_us;
+}
+
+static int max8907c_regulator_out5v_is_enabled(struct regulator_dev *rdev)
+{
+	const struct max8907c_regulator_info *info = rdev_get_drvdata(rdev);
+	int val;
+
+	val = max8907c_reg_read(info->i2c, info->reg_base);
+	if (val < 0)
+		return -EDOM;
+
+	if ((val &
+	     (MAX8907C_MASK_OUT5V_VINEN | MAX8907C_MASK_OUT5V_ENSRC |
+	      MAX8907C_MASK_OUT5V_EN))
+	    == MAX8907C_MASK_OUT5V_ENSRC)
+		return 1;
+
+	return 0;
+}
+
+static int max8907c_regulator_probe(struct platform_device *pdev)
+{
+	struct max8907c *max8907c = dev_get_drvdata(pdev->dev.parent);
+	struct max8907c_regulator_info *info;
+	struct regulator_dev *rdev;
+	struct regulator_init_data *p = pdev->dev.platform_data;
+	struct max8907c_chip_regulator_data *chip_data = p->driver_data;;
+	u8 version;
+
+	/* Backwards compatibility with max8907b, SD1 uses different voltages */
+	version = max8907c_reg_read(max8907c->i2c_power, MAX8907C_REG_II2RR);
+	if ((version & MAX8907C_II2RR_VERSION_MASK) == MAX8907C_II2RR_VERSION_REV_B) {
+		max8907c_regulators[MAX8907C_SD1].min_uV = 637500;
+		max8907c_regulators[MAX8907C_SD1].max_uV = 1425000;
+		max8907c_regulators[MAX8907C_SD1].step_uV = 12500;
+	}
+
+	info = &max8907c_regulators[pdev->id];
+	info->i2c = max8907c->i2c_power;
+
+	if (chip_data != NULL)
+		info->enable_time_us = chip_data->enable_time_us;
+
+	rdev = regulator_register(&info->desc,
+				  &pdev->dev, pdev->dev.platform_data, info);
+	if (IS_ERR(rdev)) {
+		dev_err(&pdev->dev, "Cannot register regulator \"%s\", %ld\n",
+			info->desc.name, PTR_ERR(rdev));
+		goto error;
+	}
+
+	platform_set_drvdata(pdev, rdev);
+	return 0;
+
+error:
+	return PTR_ERR(rdev);
+}
+
+static int max8907c_regulator_remove(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev = platform_get_drvdata(pdev);
+
+	regulator_unregister(rdev);
+	return 0;
+}
+
+static struct platform_driver max8907c_regulator_driver = {
+	.driver = {
+		   .name = "max8907c-regulator",
+		   .owner = THIS_MODULE,
+		   },
+	.probe = max8907c_regulator_probe,
+	.remove = __devexit_p(max8907c_regulator_remove),
+};
+
+static int __init max8907c_regulator_init(void)
+{
+	return platform_driver_register(&max8907c_regulator_driver);
+}
+
+subsys_initcall(max8907c_regulator_init);
+
+static void __exit max8907c_reg_exit(void)
+{
+	platform_driver_unregister(&max8907c_regulator_driver);
+}
+
+module_exit(max8907c_reg_exit);
+
+MODULE_DESCRIPTION("MAX8907C regulator driver");
+MODULE_AUTHOR("Gyungoh Yoo <jack.yoo@maxim-ic.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/max8973-regulator.c b/drivers/regulator/max8973-regulator.c
new file mode 100644
index 000000000000..8d87970a1b31
--- /dev/null
+++ b/drivers/regulator/max8973-regulator.c
@@ -0,0 +1,606 @@
+/*
+ * max8973-regulator.c -- Maxim max8973
+ *
+ * Regulator driver for MAXIM 8973 DC-DC step-down switching regulator.
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * Author: Laxman Dewangan <ldewangan@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any kind,
+ * whether express or implied; 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/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/max8973-regulator.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/regmap.h>
+
+/* Register definitions */
+#define MAX8973_VOUT			0x0
+#define MAX8973_VOUT_DVS		0x1
+#define MAX8973_CONTROL1		0x2
+#define MAX8973_CONTROL2		0x3
+#define MAX8973_CHIPID1			0x4
+#define MAX8973_CHIPID2			0x5
+
+#define MAX8973_MAX_VOUT_REG		2
+
+/* MAX8973_VOUT */
+#define MAX8973_VOUT_ENABLE		BIT(7)
+#define MAX8973_VOUT_MASK		0x7F
+
+/* MAX8973_VOUT_DVS */
+#define MAX8973_DVS_VOUT_MASK		0x7F
+
+/* MAX8973_CONTROL1 */
+#define MAX8973_SNS_ENABLE		BIT(7)
+#define MAX8973_FPWM_EN_M		BIT(6)
+#define MAX8973_NFSR_ENABLE		BIT(5)
+#define MAX8973_AD_ENABLE		BIT(4)
+#define MAX8973_BIAS_ENABLE		BIT(3)
+#define MAX8973_FREQSHIFT_9PER		BIT(2)
+
+#define MAX8973_RAMP_12mV_PER_US	0x0
+#define MAX8973_RAMP_25mV_PER_US	0x1
+#define MAX8973_RAMP_50mV_PER_US	0x2
+#define MAX8973_RAMP_200mV_PER_US	0x3
+
+/* MAX8973_CONTROL2 */
+#define MAX8973_WDTMR_ENABLE		BIT(6)
+#define MAX8973_DISCH_ENBABLE		BIT(5)
+#define MAX8973_FT_ENABLE		BIT(4)
+
+#define MAX8973_CKKADV_TRIP_DISABLE			0xC
+#define MAX8973_CKKADV_TRIP_75mV_PER_US			0x0
+#define MAX8973_CKKADV_TRIP_150mV_PER_US		0x4
+#define MAX8973_CKKADV_TRIP_75mV_PER_US_HIST_DIS	0x8
+
+#define MAX8973_INDUCTOR_MIN_30_PER	0x0
+#define MAX8973_INDUCTOR_NOMINAL	0x1
+#define MAX8973_INDUCTOR_PLUS_30_PER	0x2
+#define MAX8973_INDUCTOR_PLUS_60_PER	0x3
+
+#define MAX8973_MIN_VOLATGE		606250
+#define MAX8973_MAX_VOLATGE		1400000
+#define MAX8973_VOLATGE_STEP		6250
+#define MAX8973_BUCK_N_VOLTAGE	\
+	(((MAX8973_MAX_VOLATGE - MAX8973_MIN_VOLATGE) / MAX8973_VOLATGE_STEP) \
+			+ 1)
+
+/* Maxim 8973 chip information */
+struct max8973_chip {
+	struct device *dev;
+	struct regulator_desc desc;
+	struct regulator_dev *rdev;
+	struct regmap *regmap;
+	bool enable_external_control;
+	int dvs_gpio;
+	int lru_index[MAX8973_MAX_VOUT_REG];
+	int curr_vout_val[MAX8973_MAX_VOUT_REG];
+	int curr_vout_reg;
+	int curr_gpio_val;
+	int change_uv_per_us;
+	bool valid_dvs_gpio;
+};
+
+/*
+ * find_voltage_set_register: Find new voltage configuration register (VOUT).
+ * The finding of the new VOUT register will be based on the LRU mechanism.
+ * Each VOUT register will have different voltage configured . This
+ * Function will look if any of the VOUT register have requested voltage set
+ * or not.
+ *     - If it is already there then it will make that register as most
+ *       recently used and return as found so that caller need not to set
+ *       the VOUT register but need to set the proper gpios to select this
+ *       VOUT register.
+ *     - If requested voltage is not found then it will use the least
+ *       recently mechanism to get new VOUT register for new configuration
+ *       and will return not_found so that caller need to set new VOUT
+ *       register and then gpios (both).
+ */
+static bool find_voltage_set_register(struct max8973_chip *tps,
+		int req_vsel, int *vout_reg, int *gpio_val)
+{
+	int i;
+	bool found = false;
+	int new_vout_reg = tps->lru_index[MAX8973_MAX_VOUT_REG - 1];
+	int found_index = MAX8973_MAX_VOUT_REG - 1;
+
+	for (i = 0; i < MAX8973_MAX_VOUT_REG; ++i) {
+		if (tps->curr_vout_val[tps->lru_index[i]] == req_vsel) {
+			new_vout_reg = tps->lru_index[i];
+			found_index = i;
+			found = true;
+			goto update_lru_index;
+		}
+	}
+
+update_lru_index:
+	for (i = found_index; i > 0; i--)
+		tps->lru_index[i] = tps->lru_index[i - 1];
+
+	tps->lru_index[0] = new_vout_reg;
+	*gpio_val = new_vout_reg;
+	*vout_reg = MAX8973_VOUT + new_vout_reg;
+	return found;
+}
+
+static int max8973_dcdc_get_voltage_sel(struct regulator_dev *rdev)
+{
+	struct max8973_chip *max = rdev_get_drvdata(rdev);
+	unsigned int data;
+	int ret;
+
+	ret = regmap_read(max->regmap, max->curr_vout_reg, &data);
+	if (ret < 0) {
+		dev_err(max->dev, "%s(): register %d read failed with err %d\n",
+			__func__, max->curr_vout_reg, ret);
+		return ret;
+	}
+	return data & MAX8973_VOUT_MASK;
+}
+
+static int max8973_dcdc_set_voltage(struct regulator_dev *rdev,
+	     int min_uV, int max_uV, unsigned *selector)
+{
+	struct max8973_chip *max = rdev_get_drvdata(rdev);
+	int vsel;
+	int ret;
+	bool found = false;
+	int vout_reg = max->curr_vout_reg;
+	int gpio_val = max->curr_gpio_val;
+
+	if ((max_uV < min_uV) || (max_uV < MAX8973_MIN_VOLATGE) ||
+			(min_uV > MAX8973_MAX_VOLATGE))
+		return -EINVAL;
+
+	vsel = DIV_ROUND_UP(min_uV - MAX8973_MIN_VOLATGE, MAX8973_VOLATGE_STEP);
+	if (selector)
+		*selector = (vsel & MAX8973_VOUT_MASK);
+
+	/*
+	 * If gpios are available to select the VOUT register then least
+	 * recently used register for new configuration.
+	 */
+	if (max->valid_dvs_gpio)
+		found = find_voltage_set_register(max, vsel,
+					&vout_reg, &gpio_val);
+
+	if (!found) {
+		ret = regmap_update_bits(max->regmap, vout_reg,
+						MAX8973_VOUT_MASK, vsel);
+		if (ret < 0) {
+			dev_err(max->dev,
+				"%s(): register %d update failed with err %d\n",
+				 __func__, vout_reg, ret);
+			return ret;
+		}
+		max->curr_vout_reg = vout_reg;
+		max->curr_vout_val[gpio_val] = vsel;
+	}
+
+	/* Select proper VOUT register vio gpios */
+	if (max->valid_dvs_gpio) {
+		gpio_set_value_cansleep(max->dvs_gpio, gpio_val & 0x1);
+		max->curr_gpio_val = gpio_val;
+	}
+	return 0;
+}
+
+static int max8973_dcdc_list_voltage(struct regulator_dev *rdev,
+					unsigned selector)
+{
+	if (selector >= MAX8973_BUCK_N_VOLTAGE)
+		return -EINVAL;
+
+	return MAX8973_MIN_VOLATGE + selector * MAX8973_VOLATGE_STEP;
+}
+
+static int max8973_dcdc_set_voltage_time_sel(struct regulator_dev *rdev,
+		unsigned int old_selector, unsigned int new_selector)
+{
+	struct max8973_chip *max = rdev_get_drvdata(rdev);
+	int old_uV, new_uV;
+
+	old_uV = max8973_dcdc_list_voltage(rdev, old_selector);
+	if (old_uV < 0)
+		return old_uV;
+
+	new_uV = max8973_dcdc_list_voltage(rdev, new_selector);
+	if (new_uV < 0)
+		return new_uV;
+
+	return DIV_ROUND_UP(abs(old_uV - new_uV), max->change_uv_per_us);
+}
+static int max8973_dcdc_enable(struct regulator_dev *rdev)
+{
+	struct max8973_chip *max = rdev_get_drvdata(rdev);
+	int ret;
+
+	if (max->enable_external_control)
+		return 0;
+
+	ret = regmap_update_bits(max->regmap, MAX8973_VOUT,
+			MAX8973_VOUT_ENABLE, MAX8973_VOUT_ENABLE);
+	if (ret < 0)
+		dev_err(max->dev, "%s(): register %d update failed with err %d",
+			 __func__, MAX8973_VOUT, ret);
+	return ret;
+}
+
+static int max8973_dcdc_disable(struct regulator_dev *rdev)
+{
+	struct max8973_chip *max = rdev_get_drvdata(rdev);
+	int ret = 0;
+
+	if (max->enable_external_control)
+		return 0;
+
+	ret = regmap_update_bits(max->regmap, MAX8973_VOUT,
+					MAX8973_VOUT_ENABLE, 0);
+	if (ret < 0)
+		dev_err(max->dev, "%s(): register %d update failed with err %d",
+			 __func__, MAX8973_VOUT, ret);
+	return ret;
+}
+
+static int max8973_dcdc_is_enabled(struct regulator_dev *rdev)
+{
+	struct max8973_chip *max = rdev_get_drvdata(rdev);
+	int ret;
+	unsigned int data;
+
+	if (max->enable_external_control)
+		return 1;
+
+	ret = regmap_read(max->regmap, MAX8973_VOUT, &data);
+	if (ret < 0) {
+		dev_err(max->dev, "%s(): register %d read failed with err %d",
+			__func__, max->curr_vout_reg, ret);
+		return ret;
+	}
+
+	return !!(data & MAX8973_VOUT_ENABLE);
+}
+
+static int max8973_dcdc_set_mode(struct regulator_dev *rdev, unsigned int mode)
+{
+	struct max8973_chip *max = rdev_get_drvdata(rdev);
+	int ret;
+	int pwm;
+
+	/* Enable force PWM mode in FAST mode only. */
+	switch (mode) {
+	case REGULATOR_MODE_FAST:
+		pwm = MAX8973_FPWM_EN_M;
+		break;
+
+	case REGULATOR_MODE_NORMAL:
+		pwm = 0;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	ret = regmap_update_bits(max->regmap, MAX8973_CONTROL1,
+				MAX8973_FPWM_EN_M, pwm);
+	if (ret < 0)
+		dev_err(max->dev,
+			"%s(): register %d update failed with err %d\n",
+			__func__, MAX8973_CONTROL1, ret);
+	return ret;
+}
+
+static unsigned int max8973_dcdc_get_mode(struct regulator_dev *rdev)
+{
+	struct max8973_chip *max = rdev_get_drvdata(rdev);
+	unsigned int data;
+	int ret;
+
+	ret = regmap_read(max->regmap, MAX8973_CONTROL1, &data);
+	if (ret < 0) {
+		dev_err(max->dev, "%s(): register %d read failed with err %d\n",
+			__func__, MAX8973_CONTROL1, ret);
+		return ret;
+	}
+	return (data & MAX8973_FPWM_EN_M) ?
+		REGULATOR_MODE_FAST : REGULATOR_MODE_NORMAL;
+}
+
+static struct regulator_ops max8973_dcdc_ops = {
+	.get_voltage_sel	= max8973_dcdc_get_voltage_sel,
+	.set_voltage		= max8973_dcdc_set_voltage,
+	.list_voltage		= max8973_dcdc_list_voltage,
+	.set_voltage_time_sel	= max8973_dcdc_set_voltage_time_sel,
+	.enable			= max8973_dcdc_enable,
+	.disable		= max8973_dcdc_disable,
+	.is_enabled		= max8973_dcdc_is_enabled,
+	.set_mode		= max8973_dcdc_set_mode,
+	.get_mode		= max8973_dcdc_get_mode,
+};
+
+static int __devinit max8973_init_dcdc(struct max8973_chip *max,
+		struct max8973_regulator_platform_data *pdata)
+{
+	int ret;
+	uint8_t	control1 = 0;
+	uint8_t control2 = 0;
+
+	if (pdata->control_flags & MAX8973_CONTROL_REMOTE_SENSE_ENABLE)
+		control1 |= MAX8973_SNS_ENABLE;
+
+	if (!(pdata->control_flags & MAX8973_CONTROL_FALLING_SLEW_RATE_ENABLE))
+		control1 |= MAX8973_NFSR_ENABLE;
+
+	if (pdata->control_flags & MAX8973_CONTROL_OUTPUT_ACTIVE_DISCH_ENABLE)
+		control1 |= MAX8973_AD_ENABLE;
+
+	if (pdata->control_flags & MAX8973_CONTROL_BIAS_ENABLE)
+		control1 |= MAX8973_BIAS_ENABLE;
+
+	if (pdata->control_flags & MAX8973_CONTROL_FREQ_SHIFT_9PER_ENABLE)
+		control1 |= MAX8973_FREQSHIFT_9PER;
+
+	switch (pdata->control_flags & MAX8973_CONTROL_SLEW_RATE_200MV_PER_US) {
+	case MAX8973_CONTROL_SLEW_RATE_12_5mV_PER_US:
+		control1 = MAX8973_RAMP_12mV_PER_US;
+		max->change_uv_per_us = 12500;
+		break;
+
+	case MAX8973_CONTROL_SLEW_RATE_25mV_PER_US:
+		control1 = MAX8973_RAMP_25mV_PER_US;
+		max->change_uv_per_us = 25000;
+		break;
+
+	case MAX8973_CONTROL_SLEW_RATE_50mV_PER_US:
+		control1 = MAX8973_RAMP_50mV_PER_US;
+		max->change_uv_per_us = 50000;
+		break;
+
+	case MAX8973_CONTROL_SLEW_RATE_200MV_PER_US:
+		control1 = MAX8973_RAMP_200mV_PER_US;
+		max->change_uv_per_us = 200000;
+		break;
+	}
+
+	if (!(pdata->control_flags & MAX8973_CONTROL_PULL_DOWN_ENABLE))
+		control2 |= MAX8973_DISCH_ENBABLE;
+
+	switch (pdata->control_flags &
+				MAX8973_CONTROL_CLKADV_TRIP_75mV_PER_US) {
+	case MAX8973_CONTROL_CLKADV_TRIP_DISABLED:
+		control2 |= MAX8973_CKKADV_TRIP_DISABLE;
+		break;
+
+	case MAX8973_CONTROL_CLKADV_TRIP_75mV_PER_US:
+		control2 |= MAX8973_CKKADV_TRIP_75mV_PER_US;
+		break;
+
+	case MAX8973_CONTROL_CLKADV_TRIP_150mV_PER_US:
+		control2 |= MAX8973_CKKADV_TRIP_150mV_PER_US;
+		break;
+
+	case MAX8973_CONTROL_CLKADV_TRIP_75mV_PER_US_HIST_DIS:
+		control2 |= MAX8973_CKKADV_TRIP_75mV_PER_US_HIST_DIS;
+		break;
+	}
+
+	switch (pdata->control_flags &
+			MAX8973_CONTROL_INDUCTOR_VALUE_PLUS_60_PER) {
+	case MAX8973_CONTROL_INDUCTOR_VALUE_NOMINAL:
+		control2 |= MAX8973_INDUCTOR_NOMINAL;
+		break;
+
+	case MAX8973_CONTROL_INDUCTOR_VALUE_MINUS_30_PER:
+		control2 |= MAX8973_INDUCTOR_MIN_30_PER;
+		break;
+
+	case MAX8973_CONTROL_INDUCTOR_VALUE_PLUS_30_PER:
+		control2 |= MAX8973_INDUCTOR_PLUS_30_PER;
+		break;
+
+	case MAX8973_CONTROL_INDUCTOR_VALUE_PLUS_60_PER:
+		control2 |= MAX8973_INDUCTOR_PLUS_60_PER;
+		break;
+	}
+
+	ret = regmap_write(max->regmap, MAX8973_CONTROL1, control1);
+	if (ret < 0) {
+		dev_err(max->dev, "%s(): register %d write failed with err %d",
+			__func__, MAX8973_CONTROL1, ret);
+		return ret;
+	}
+
+	ret = regmap_write(max->regmap, MAX8973_CONTROL2, control2);
+	if (ret < 0) {
+		dev_err(max->dev, "%s(): register %d write failed with err %d",
+			__func__, MAX8973_CONTROL2, ret);
+		return ret;
+	}
+
+	/* If external control is enabled then disable EN bit */
+	if (max->enable_external_control) {
+		ret = regmap_update_bits(max->regmap, MAX8973_VOUT,
+						MAX8973_VOUT_ENABLE, 0);
+		if (ret < 0)
+			dev_err(max->dev, "%s(): register %d update failed with err %d",
+			__func__, MAX8973_VOUT, ret);
+	}
+	return ret;
+}
+
+static const struct regmap_config max8973_regmap_config = {
+	.reg_bits		= 8,
+	.val_bits		= 8,
+	.max_register		= MAX8973_CHIPID2,
+	.cache_type		= REGCACHE_RBTREE,
+};
+
+static int __devinit max8973_probe(struct i2c_client *client,
+				     const struct i2c_device_id *id)
+{
+	struct max8973_regulator_platform_data *pdata;
+	struct regulator_dev *rdev;
+	struct max8973_chip *max;
+	int ret;
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->dev, "%s(): No Platform data", __func__);
+		return -EIO;
+	}
+
+	max = devm_kzalloc(&client->dev, sizeof(*max), GFP_KERNEL);
+	if (!max) {
+		dev_err(&client->dev, "%s(): Memory allocation failed\n",
+						__func__);
+		return -ENOMEM;
+	}
+
+	max->dev = &client->dev;
+
+	max->desc.name = id->name;
+	max->desc.id = 0;
+	max->desc.ops = &max8973_dcdc_ops;
+	max->desc.type = REGULATOR_VOLTAGE;
+	max->desc.owner = THIS_MODULE;
+	max->regmap = devm_regmap_init_i2c(client, &max8973_regmap_config);
+	if (IS_ERR(max->regmap)) {
+		ret = PTR_ERR(max->regmap);
+		dev_err(&client->dev,
+			"%s(): regmap allocation failed with err %d\n",
+			__func__, ret);
+		return ret;
+	}
+	i2c_set_clientdata(client, max);
+
+	max->enable_external_control = pdata->enable_ext_control;
+	max->dvs_gpio = pdata->dvs_gpio;
+	max->curr_gpio_val = pdata->dvs_def_state;
+	max->curr_vout_reg = MAX8973_VOUT + pdata->dvs_def_state;
+	max->lru_index[0] = max->curr_vout_reg;
+	max->valid_dvs_gpio = false;
+
+	if (gpio_is_valid(max->dvs_gpio)) {
+		int gpio_flags;
+		int i;
+
+		gpio_flags = (pdata->dvs_def_state) ?
+				GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
+		ret = gpio_request_one(max->dvs_gpio,
+				gpio_flags, "max8973-dvs");
+		if (ret) {
+			dev_err(&client->dev,
+				"%s(): Could not obtain dvs GPIO %d: %d\n",
+				__func__, max->dvs_gpio, ret);
+			return ret;
+		}
+		max->valid_dvs_gpio = true;
+
+		/*
+		 * Initialize the lru index with vout_reg id
+		 * The index 0 will be most recently used and
+		 * set with the max->curr_vout_reg */
+		for (i = 0; i < MAX8973_MAX_VOUT_REG; ++i)
+			max->lru_index[i] = i;
+		max->lru_index[0] = max->curr_vout_reg;
+		max->lru_index[max->curr_vout_reg] = 0;
+	}
+
+	ret = max8973_init_dcdc(max, pdata);
+	if (ret < 0) {
+		dev_err(max->dev, "%s(): Init failed with err = %d\n",
+				__func__, ret);
+		goto err_init;
+	}
+
+	/* Register the regulators */
+	rdev = regulator_register(&max->desc, &client->dev,
+			pdata->reg_init_data, max);
+	if (IS_ERR(rdev)) {
+		dev_err(max->dev,
+			"%s(): regulator register failed with err %s\n",
+			__func__, id->name);
+		ret = PTR_ERR(rdev);
+		goto err_init;
+	}
+
+	max->rdev = rdev;
+	return 0;
+
+err_init:
+	if (gpio_is_valid(max->dvs_gpio))
+		gpio_free(max->dvs_gpio);
+	return ret;
+}
+
+/**
+ * max8973_remove - max8973 driver i2c remove handler
+ * @client: i2c driver client device structure
+ *
+ * Unregister TPS driver as an i2c client device driver
+ */
+static int __devexit max8973_remove(struct i2c_client *client)
+{
+	struct max8973_chip *max = i2c_get_clientdata(client);
+
+	if (gpio_is_valid(max->dvs_gpio))
+		gpio_free(max->dvs_gpio);
+
+	regulator_unregister(max->rdev);
+	return 0;
+}
+
+static const struct i2c_device_id max8973_id[] = {
+	{.name = "max8973",},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, max8973_id);
+
+static struct i2c_driver max8973_i2c_driver = {
+	.driver = {
+		.name = "max8973",
+		.owner = THIS_MODULE,
+	},
+	.probe = max8973_probe,
+	.remove = __devexit_p(max8973_remove),
+	.id_table = max8973_id,
+};
+
+static int __init max8973_init(void)
+{
+	return i2c_add_driver(&max8973_i2c_driver);
+}
+subsys_initcall(max8973_init);
+
+static void __exit max8973_cleanup(void)
+{
+	i2c_del_driver(&max8973_i2c_driver);
+}
+module_exit(max8973_cleanup);
+
+MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
+MODULE_DESCRIPTION("MAX8973 voltage regulator driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/rc5t583-regulator.c b/drivers/regulator/rc5t583-regulator.c
new file mode 100644
index 000000000000..81a03b281fa0
--- /dev/null
+++ b/drivers/regulator/rc5t583-regulator.c
@@ -0,0 +1,363 @@
+/*
+ * Regulator driver for RICOH RC5T583 power management chip.
+ *
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION.  All rights reserved.
+ * Author: Laxman dewangan <ldewangan@nvidia.com>
+ *
+ * based on code
+ *      Copyright (C) 2011 RICOH COMPANY,LTD
+ *
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/gpio.h>
+#include <linux/mfd/rc5t583.h>
+
+struct rc5t583_regulator_info {
+	int			deepsleep_id;
+
+	/* Regulator register address.*/
+	uint8_t			reg_en_reg;
+	uint8_t			en_bit;
+	uint8_t			reg_disc_reg;
+	uint8_t			disc_bit;
+	uint8_t			vout_reg;
+	uint8_t			vout_mask;
+	uint8_t			deepsleep_reg;
+
+	/* Chip constraints on regulator behavior */
+	int			min_uV;
+	int			max_uV;
+	int			step_uV;
+
+	/* Regulator specific turn-on delay  and voltage settling time*/
+	int			enable_uv_per_us;
+	int			change_uv_per_us;
+
+	/* Used by regulator core */
+	struct regulator_desc	desc;
+};
+
+struct rc5t583_regulator {
+	struct rc5t583_regulator_info *reg_info;
+
+	/* Devices */
+	struct device		*dev;
+	struct rc5t583		*mfd;
+	struct regulator_dev	*rdev;
+};
+
+static int rc5t583_reg_is_enabled(struct regulator_dev *rdev)
+{
+	struct rc5t583_regulator *reg = rdev_get_drvdata(rdev);
+	struct rc5t583_regulator_info *ri = reg->reg_info;
+	uint8_t control;
+	int ret;
+
+	ret = rc5t583_read(reg->mfd->dev, ri->reg_en_reg, &control);
+	if (ret < 0) {
+		dev_err(&rdev->dev,
+			"Error in reading the control register 0x%02x\n",
+			ri->reg_en_reg);
+		return ret;
+	}
+	return !!(control & BIT(ri->en_bit));
+}
+
+static int rc5t583_reg_enable(struct regulator_dev *rdev)
+{
+	struct rc5t583_regulator *reg = rdev_get_drvdata(rdev);
+	struct rc5t583_regulator_info *ri = reg->reg_info;
+	int ret;
+
+	ret = rc5t583_set_bits(reg->mfd->dev, ri->reg_en_reg,
+				(1 << ri->en_bit));
+	if (ret < 0) {
+		dev_err(&rdev->dev,
+			"Error in setting bit of STATE register 0x%02x\n",
+			ri->reg_en_reg);
+		return ret;
+	}
+	return ret;
+}
+
+static int rc5t583_reg_disable(struct regulator_dev *rdev)
+{
+	struct rc5t583_regulator *reg = rdev_get_drvdata(rdev);
+	struct rc5t583_regulator_info *ri = reg->reg_info;
+	int ret;
+
+	ret = rc5t583_clear_bits(reg->mfd->dev, ri->reg_en_reg,
+				(1 << ri->en_bit));
+	if (ret < 0)
+		dev_err(&rdev->dev,
+			"Error in clearing bit of STATE register 0x%02x\n",
+			ri->reg_en_reg);
+
+	return ret;
+}
+
+static int rc5t583_list_voltage(struct regulator_dev *rdev, unsigned selector)
+{
+	struct rc5t583_regulator *reg = rdev_get_drvdata(rdev);
+	struct rc5t583_regulator_info *ri = reg->reg_info;
+	return ri->min_uV + (ri->step_uV * selector);
+}
+
+static int rc5t583_set_voltage_sel(struct regulator_dev *rdev,
+		unsigned int selector)
+{
+	struct rc5t583_regulator *reg = rdev_get_drvdata(rdev);
+	struct rc5t583_regulator_info *ri = reg->reg_info;
+	int ret;
+	if (selector >= rdev->desc->n_voltages) {
+		dev_err(&rdev->dev, "Invalid selector 0x%02x\n", selector);
+		return -EINVAL;
+	}
+
+	ret = rc5t583_update(reg->mfd->dev, ri->vout_reg,
+				selector, ri->vout_mask);
+	if (ret < 0)
+		dev_err(&rdev->dev,
+		    "Error in update voltage register 0x%02x\n", ri->vout_reg);
+	return ret;
+}
+
+static int rc5t583_get_voltage_sel(struct regulator_dev *rdev)
+{
+	struct rc5t583_regulator *reg = rdev_get_drvdata(rdev);
+	struct rc5t583_regulator_info *ri = reg->reg_info;
+	uint8_t vsel;
+	int ret;
+	ret = rc5t583_read(reg->mfd->dev, ri->vout_reg, &vsel);
+	if (ret < 0) {
+		dev_err(&rdev->dev,
+		    "Error in reading voltage register 0x%02x\n", ri->vout_reg);
+		return ret;
+	}
+	return vsel & ri->vout_mask;
+}
+
+static int rc5t583_regulator_enable_time(struct regulator_dev *rdev)
+{
+	struct rc5t583_regulator *reg = rdev_get_drvdata(rdev);
+	int vsel = rc5t583_get_voltage_sel(rdev);
+	int curr_uV = rc5t583_list_voltage(rdev, vsel);
+	return DIV_ROUND_UP(curr_uV, reg->reg_info->enable_uv_per_us);
+}
+
+static int rc5t583_set_voltage_time_sel(struct regulator_dev *rdev,
+		unsigned int old_selector, unsigned int new_selector)
+{
+	struct rc5t583_regulator *reg = rdev_get_drvdata(rdev);
+	int old_uV, new_uV;
+	old_uV = rc5t583_list_voltage(rdev, old_selector);
+
+	if (old_uV < 0)
+		return old_uV;
+
+	new_uV = rc5t583_list_voltage(rdev, new_selector);
+	if (new_uV < 0)
+		return new_uV;
+
+	return DIV_ROUND_UP(abs(old_uV - new_uV),
+				reg->reg_info->change_uv_per_us);
+}
+
+
+static struct regulator_ops rc5t583_ops = {
+	.is_enabled		= rc5t583_reg_is_enabled,
+	.enable			= rc5t583_reg_enable,
+	.disable		= rc5t583_reg_disable,
+	.enable_time		= rc5t583_regulator_enable_time,
+	.get_voltage_sel	= rc5t583_get_voltage_sel,
+	.set_voltage_sel	= rc5t583_set_voltage_sel,
+	.list_voltage		= rc5t583_list_voltage,
+	.set_voltage_time_sel	= rc5t583_set_voltage_time_sel,
+};
+
+#define RC5T583_REG(_id, _en_reg, _en_bit, _disc_reg, _disc_bit, _vout_reg,  \
+		_vout_mask, _ds_reg, _min_mv, _max_mv, _step_uV, _enable_mv) \
+{								\
+	.reg_en_reg	= RC5T583_REG_##_en_reg,		\
+	.en_bit		= _en_bit,				\
+	.reg_disc_reg	= RC5T583_REG_##_disc_reg,		\
+	.disc_bit	= _disc_bit,				\
+	.vout_reg	= RC5T583_REG_##_vout_reg,		\
+	.vout_mask	= _vout_mask,				\
+	.deepsleep_reg	= RC5T583_REG_##_ds_reg,		\
+	.min_uV		= _min_mv * 1000,			\
+	.max_uV		= _max_mv * 1000,			\
+	.step_uV	= _step_uV,				\
+	.enable_uv_per_us = _enable_mv * 1000,			\
+	.change_uv_per_us = 40 * 1000,				\
+	.deepsleep_id	= RC5T583_DS_##_id,			\
+	.desc = {						\
+		.name = "rc5t583-regulator-"#_id,		\
+		.id = RC5T583_REGULATOR_##_id,			\
+		.n_voltages = (_max_mv - _min_mv) * 1000 / _step_uV + 1, \
+		.ops = &rc5t583_ops,				\
+		.type = REGULATOR_VOLTAGE,			\
+		.owner = THIS_MODULE,				\
+	},							\
+}
+
+static struct rc5t583_regulator_info rc5t583_reg_info[RC5T583_REGULATOR_MAX] = {
+	RC5T583_REG(DC0, DC0CTL, 0, DC0CTL, 1, DC0DAC, 0x7F, DC0DAC_DS,
+			700, 1500, 12500, 4),
+	RC5T583_REG(DC1, DC1CTL, 0, DC1CTL, 1, DC1DAC, 0x7F, DC1DAC_DS,
+			700, 1500, 12500, 14),
+	RC5T583_REG(DC2, DC2CTL, 0, DC2CTL, 1, DC2DAC, 0x7F, DC2DAC_DS,
+			900, 2400, 12500, 14),
+	RC5T583_REG(DC3, DC3CTL, 0, DC3CTL, 1, DC3DAC, 0x7F, DC3DAC_DS,
+			900, 2400, 12500, 14),
+	RC5T583_REG(LDO0, LDOEN2, 0, LDODIS2, 0, LDO0DAC, 0x7F, LDO0DAC_DS,
+			900, 3400, 25000, 160),
+	RC5T583_REG(LDO1, LDOEN2, 1, LDODIS2, 1, LDO1DAC, 0x7F, LDO1DAC_DS,
+			900, 3400, 25000, 160),
+	RC5T583_REG(LDO2, LDOEN2, 2, LDODIS2, 2, LDO2DAC, 0x7F, LDO2DAC_DS,
+			900, 3400, 25000, 160),
+	RC5T583_REG(LDO3, LDOEN2, 3, LDODIS2, 3, LDO3DAC, 0x7F, LDO3DAC_DS,
+			900, 3400, 25000, 160),
+	RC5T583_REG(LDO4, LDOEN2, 4, LDODIS2, 4, LDO4DAC, 0x3F, LDO4DAC_DS,
+			750, 1500, 12500, 133),
+	RC5T583_REG(LDO5, LDOEN2, 5, LDODIS2, 5, LDO5DAC, 0x7F, LDO5DAC_DS,
+			900, 3400, 25000, 267),
+	RC5T583_REG(LDO6, LDOEN2, 6, LDODIS2, 6, LDO6DAC, 0x7F, LDO6DAC_DS,
+			900, 3400, 25000, 133),
+	RC5T583_REG(LDO7, LDOEN2, 7, LDODIS2, 7, LDO7DAC, 0x7F, LDO7DAC_DS,
+			900, 3400, 25000, 233),
+	RC5T583_REG(LDO8, LDOEN1, 0, LDODIS1, 0, LDO8DAC, 0x7F, LDO8DAC_DS,
+			900, 3400, 25000, 233),
+	RC5T583_REG(LDO9, LDOEN1, 1, LDODIS1, 1, LDO9DAC, 0x7F, LDO9DAC_DS,
+			900, 3400, 25000, 133),
+};
+
+static int __devinit rc5t583_regulator_probe(struct platform_device *pdev)
+{
+	struct rc5t583 *rc5t583 = dev_get_drvdata(pdev->dev.parent);
+	struct rc5t583_platform_data *pdata = dev_get_platdata(rc5t583->dev);
+	struct regulator_init_data *reg_data;
+	struct rc5t583_regulator *reg = NULL;
+	struct rc5t583_regulator *regs;
+	struct regulator_dev *rdev;
+	struct rc5t583_regulator_info *ri;
+	int ret;
+	int id;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "No platform data, exiting...\n");
+		return -ENODEV;
+	}
+
+	regs = devm_kzalloc(&pdev->dev, RC5T583_REGULATOR_MAX *
+			sizeof(struct rc5t583_regulator), GFP_KERNEL);
+	if (!regs) {
+		dev_err(&pdev->dev, "Memory allocation failed exiting..\n");
+		return -ENOMEM;
+	}
+
+
+	for (id = 0; id < RC5T583_REGULATOR_MAX; ++id) {
+		reg_data = pdata->reg_init_data[id];
+
+		/* No need to register if there is no regulator data */
+		if (!reg_data)
+			continue;
+
+		reg = &regs[id];
+		ri = &rc5t583_reg_info[id];
+		reg->reg_info = ri;
+		reg->mfd = rc5t583;
+		reg->dev = &pdev->dev;
+
+		if (ri->deepsleep_id == RC5T583_DS_NONE)
+			goto skip_ext_pwr_config;
+
+		ret = rc5t583_ext_power_req_config(rc5t583->dev,
+				ri->deepsleep_id,
+				pdata->regulator_ext_pwr_control[id],
+				pdata->regulator_deepsleep_slot[id]);
+		/*
+		 * Configuring external control is not a major issue,
+		 * just give warning.
+		 */
+		if (ret < 0)
+			dev_warn(&pdev->dev,
+				"Failed to configure ext control %d\n", id);
+
+skip_ext_pwr_config:
+		rdev = regulator_register(&ri->desc, &pdev->dev, reg_data, reg);
+		if (IS_ERR(rdev)) {
+			dev_err(&pdev->dev, "Failed to register regulator %s\n",
+						ri->desc.name);
+			ret = PTR_ERR(rdev);
+			goto clean_exit;
+		}
+		reg->rdev = rdev;
+	}
+	platform_set_drvdata(pdev, regs);
+	return 0;
+
+clean_exit:
+	while (--id >= 0)
+		regulator_unregister(regs[id].rdev);
+
+	return ret;
+}
+
+static int __devexit rc5t583_regulator_remove(struct platform_device *pdev)
+{
+	struct rc5t583_regulator *regs = platform_get_drvdata(pdev);
+	int id;
+
+	for (id = 0; id < RC5T583_REGULATOR_MAX; ++id)
+		regulator_unregister(regs[id].rdev);
+	return 0;
+}
+
+static struct platform_driver rc5t583_regulator_driver = {
+	.driver	= {
+		.name	= "rc5t583-regulator",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= rc5t583_regulator_probe,
+	.remove		= __devexit_p(rc5t583_regulator_remove),
+};
+
+static int __init rc5t583_regulator_init(void)
+{
+	return platform_driver_register(&rc5t583_regulator_driver);
+}
+subsys_initcall(rc5t583_regulator_init);
+
+static void __exit rc5t583_regulator_exit(void)
+{
+	platform_driver_unregister(&rc5t583_regulator_driver);
+}
+module_exit(rc5t583_regulator_exit);
+
+MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
+MODULE_DESCRIPTION("RC5T583 regulator driver");
+MODULE_ALIAS("platform:rc5t583-regulator");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/ricoh583-regulator.c b/drivers/regulator/ricoh583-regulator.c
new file mode 100644
index 000000000000..867ffe629ed2
--- /dev/null
+++ b/drivers/regulator/ricoh583-regulator.c
@@ -0,0 +1,412 @@
+/*
+ * drivers/regulator/ricoh583-regulator.c
+ *
+ * Regulator driver for RICOH583 power management chip.
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ *
+ * Copyright (C) 2011 RICOH COMPANY,LTD
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ */
+
+/*#define DEBUG			1*/
+/*#define VERBOSE_DEBUG		1*/
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/mfd/ricoh583.h>
+#include <linux/regulator/ricoh583-regulator.h>
+
+struct ricoh583_regulator {
+	int		id;
+	int		deepsleep_id;
+	/* Regulator register address.*/
+	u8		reg_en_reg;
+	u8		en_bit;
+	u8		reg_disc_reg;
+	u8		disc_bit;
+	u8		vout_reg;
+	u8		vout_mask;
+	u8		vout_reg_cache;
+	u8		deepsleep_reg;
+
+	/* chip constraints on regulator behavior */
+	int			min_uV;
+	int			max_uV;
+	int			step_uV;
+	int			nsteps;
+
+	/* regulator specific turn-on delay */
+	u16			delay;
+
+	/* used by regulator core */
+	struct regulator_desc	desc;
+
+	/* Device */
+	struct device		*dev;
+};
+
+
+static inline struct device *to_ricoh583_dev(struct regulator_dev *rdev)
+{
+	return rdev_get_dev(rdev)->parent->parent;
+}
+
+static int ricoh583_regulator_enable_time(struct regulator_dev *rdev)
+{
+	struct ricoh583_regulator *ri = rdev_get_drvdata(rdev);
+
+	return ri->delay;
+}
+
+static int ricoh583_reg_is_enabled(struct regulator_dev *rdev)
+{
+	struct ricoh583_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_ricoh583_dev(rdev);
+	uint8_t control;
+	int ret;
+
+	ret = ricoh583_read(parent, ri->reg_en_reg, &control);
+	if (ret < 0) {
+		dev_err(&rdev->dev, "Error in reading the control register\n");
+		return ret;
+	}
+	return (((control >> ri->en_bit) & 1) == 1);
+}
+
+static int ricoh583_reg_enable(struct regulator_dev *rdev)
+{
+	struct ricoh583_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_ricoh583_dev(rdev);
+	int ret;
+
+	ret = ricoh583_set_bits(parent, ri->reg_en_reg, (1 << ri->en_bit));
+	if (ret < 0) {
+		dev_err(&rdev->dev, "Error in updating the STATE register\n");
+		return ret;
+	}
+	udelay(ri->delay);
+	return ret;
+}
+
+static int ricoh583_reg_disable(struct regulator_dev *rdev)
+{
+	struct ricoh583_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_ricoh583_dev(rdev);
+	int ret;
+
+	ret = ricoh583_clr_bits(parent, ri->reg_en_reg, (1 << ri->en_bit));
+	if (ret < 0)
+		dev_err(&rdev->dev, "Error in updating the STATE register\n");
+
+	return ret;
+}
+
+static int ricoh583_list_voltage(struct regulator_dev *rdev, unsigned index)
+{
+	struct ricoh583_regulator *ri = rdev_get_drvdata(rdev);
+
+	return ri->min_uV + (ri->step_uV * index);
+}
+
+static int __ricoh583_set_ds_voltage(struct device *parent,
+		struct ricoh583_regulator *ri, int min_uV, int max_uV)
+{
+	int vsel;
+	int ret;
+
+	if ((min_uV < ri->min_uV) || (max_uV > ri->max_uV))
+		return -EDOM;
+
+	vsel = (min_uV - ri->min_uV + ri->step_uV - 1)/ri->step_uV;
+	if (vsel > ri->nsteps)
+		return -EDOM;
+
+	ret = ricoh583_update(parent, ri->deepsleep_reg, vsel, ri->vout_mask);
+	if (ret < 0)
+		dev_err(ri->dev, "Error in writing the deepsleep register\n");
+	return ret;
+}
+
+static int __ricoh583_set_voltage(struct device *parent,
+		struct ricoh583_regulator *ri, int min_uV, int max_uV,
+		unsigned *selector)
+{
+	int vsel;
+	int ret;
+	uint8_t vout_val;
+
+	if ((min_uV < ri->min_uV) || (max_uV > ri->max_uV))
+		return -EDOM;
+
+	vsel = (min_uV - ri->min_uV + ri->step_uV - 1)/ri->step_uV;
+	if (vsel > ri->nsteps)
+		return -EDOM;
+
+	if (selector)
+		*selector = vsel;
+
+	vout_val = (ri->vout_reg_cache & ~ri->vout_mask) |
+				(vsel & ri->vout_mask);
+	ret = ricoh583_write(parent, ri->vout_reg, vout_val);
+	if (ret < 0)
+		dev_err(ri->dev, "Error in writing the Voltage register\n");
+	else
+		ri->vout_reg_cache = vout_val;
+
+	return ret;
+}
+
+static int ricoh583_set_voltage(struct regulator_dev *rdev,
+		int min_uV, int max_uV, unsigned *selector)
+{
+	struct ricoh583_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_ricoh583_dev(rdev);
+
+	return __ricoh583_set_voltage(parent, ri, min_uV, max_uV, selector);
+}
+
+static int ricoh583_get_voltage(struct regulator_dev *rdev)
+{
+	struct ricoh583_regulator *ri = rdev_get_drvdata(rdev);
+	uint8_t vsel;
+
+	vsel = ri->vout_reg_cache & ri->vout_mask;
+	return ri->min_uV + vsel * ri->step_uV;
+}
+
+static struct regulator_ops ricoh583_ops = {
+	.list_voltage	= ricoh583_list_voltage,
+	.set_voltage	= ricoh583_set_voltage,
+	.get_voltage	= ricoh583_get_voltage,
+	.enable		= ricoh583_reg_enable,
+	.disable	= ricoh583_reg_disable,
+	.is_enabled	= ricoh583_reg_is_enabled,
+	.enable_time	= ricoh583_regulator_enable_time,
+};
+
+#define RICOH583_REG(_id, _en_reg, _en_bit, _disc_reg, _disc_bit, _vout_reg, \
+		_vout_mask, _ds_reg, _min_mv, _max_mv, _step_uV, _nsteps,    \
+		_ops, _delay)		\
+{								\
+	.reg_en_reg	= _en_reg,				\
+	.en_bit		= _en_bit,				\
+	.reg_disc_reg	= _disc_reg,				\
+	.disc_bit	= _disc_bit,				\
+	.vout_reg	= _vout_reg,				\
+	.vout_mask	= _vout_mask,				\
+	.deepsleep_reg	= _ds_reg,				\
+	.min_uV		= _min_mv * 1000,			\
+	.max_uV		= _max_mv * 1000,			\
+	.step_uV	= _step_uV,				\
+	.nsteps		= _nsteps,				\
+	.delay		= _delay,				\
+	.id		= RICOH583_ID_##_id,			\
+	.deepsleep_id	= RICOH583_DS_##_id,			\
+	.desc = {						\
+		.name = ricoh583_rails(_id),			\
+		.id = RICOH583_ID_##_id,			\
+		.n_voltages = _nsteps,				\
+		.ops = &_ops,					\
+		.type = REGULATOR_VOLTAGE,			\
+		.owner = THIS_MODULE,				\
+	},							\
+}
+
+static struct ricoh583_regulator ricoh583_regulator[] = {
+	RICOH583_REG(DC0, 0x30, 0, 0x30, 1, 0x31, 0x7F, 0x60,
+			700, 1500, 12500, 0x41, ricoh583_ops, 500),
+	RICOH583_REG(DC1, 0x34, 0, 0x34, 1, 0x35, 0x7F, 0x61,
+			700, 1500, 12500, 0x41, ricoh583_ops, 500),
+	RICOH583_REG(DC2, 0x38, 0, 0x38, 1, 0x39, 0x7F, 0x62,
+			900, 2400, 12500, 0x79, ricoh583_ops, 500),
+	RICOH583_REG(DC3, 0x3C, 0, 0x3C, 1, 0x3D, 0x7F, 0x63,
+			900, 2400, 12500, 0x79, ricoh583_ops, 500),
+	RICOH583_REG(LDO0, 0x51, 0, 0x53, 0, 0x54, 0x7F, 0x64,
+			900, 3400, 25000, 0x65, ricoh583_ops, 500),
+	RICOH583_REG(LDO1, 0x51, 1, 0x53, 1, 0x55, 0x7F, 0x65,
+			900, 3400, 25000, 0x65, ricoh583_ops, 500),
+	RICOH583_REG(LDO2, 0x51, 2, 0x53, 2, 0x56, 0x7F, 0x66,
+			900, 3400, 25000, 0x65, ricoh583_ops, 500),
+	RICOH583_REG(LDO3, 0x51, 3, 0x53, 3, 0x57, 0x7F, 0x67,
+			900, 3400, 25000, 0x65, ricoh583_ops, 500),
+	RICOH583_REG(LDO4, 0x51, 4, 0x53, 4, 0x58, 0x3F, 0x68,
+			750, 1500, 12500, 0x3D, ricoh583_ops, 500),
+	RICOH583_REG(LDO5, 0x51, 5, 0x53, 5, 0x59, 0x7F, 0x69,
+			900, 3400, 25000, 0x65, ricoh583_ops, 500),
+	RICOH583_REG(LDO6, 0x51, 6, 0x53, 6, 0x5A, 0x7F, 0x6A,
+			900, 3400, 25000, 0x65, ricoh583_ops, 500),
+	RICOH583_REG(LDO7, 0x51, 7, 0x53, 7, 0x5B, 0x7F, 0x6B,
+			900, 3400, 25000, 0x65, ricoh583_ops, 500),
+	RICOH583_REG(LDO8, 0x50, 0, 0x52, 0, 0x5C, 0x7F, 0x6C,
+			900, 3400, 25000, 0x65, ricoh583_ops, 500),
+	RICOH583_REG(LDO9, 0x50, 1, 0x52, 1, 0x5D, 0x7F, 0x6D,
+			900, 3400, 25000, 0x65, ricoh583_ops, 500),
+};
+static inline struct ricoh583_regulator *find_regulator_info(int id)
+{
+	struct ricoh583_regulator *ri;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ricoh583_regulator); i++) {
+		ri = &ricoh583_regulator[i];
+		if (ri->desc.id == id)
+			return ri;
+	}
+	return NULL;
+}
+
+static int ricoh583_regulator_preinit(struct device *parent,
+		struct ricoh583_regulator *ri,
+		struct ricoh583_regulator_platform_data *ricoh583_pdata)
+{
+	int ret = 0;
+
+	if (ri->deepsleep_id != RICOH583_DS_NONE) {
+		ret = ricoh583_ext_power_req_config(parent, ri->deepsleep_id,
+			ricoh583_pdata->ext_pwr_req,
+			ricoh583_pdata->deepsleep_slots);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (!ricoh583_pdata->init_apply)
+		return 0;
+
+	if (ricoh583_pdata->deepsleep_uV) {
+		ret = __ricoh583_set_ds_voltage(parent, ri,
+				ricoh583_pdata->deepsleep_uV,
+				ricoh583_pdata->deepsleep_uV);
+		if (ret < 0) {
+			dev_err(ri->dev, "Not able to initialize ds voltage %d"
+				" for rail %d err %d\n",
+				ricoh583_pdata->deepsleep_uV, ri->desc.id, ret);
+			return ret;
+		}
+	}
+
+	if (ricoh583_pdata->init_uV >= 0) {
+		ret = __ricoh583_set_voltage(parent, ri,
+				ricoh583_pdata->init_uV,
+				ricoh583_pdata->init_uV, 0);
+		if (ret < 0) {
+			dev_err(ri->dev, "Not able to initialize voltage %d "
+				"for rail %d err %d\n", ricoh583_pdata->init_uV,
+				ri->desc.id, ret);
+			return ret;
+		}
+	}
+
+	if (ricoh583_pdata->init_enable)
+		ret = ricoh583_set_bits(parent, ri->reg_en_reg,
+							(1 << ri->en_bit));
+	else
+		ret = ricoh583_clr_bits(parent, ri->reg_en_reg,
+							(1 << ri->en_bit));
+	if (ret < 0)
+		dev_err(ri->dev, "Not able to %s rail %d err %d\n",
+			(ricoh583_pdata->init_enable) ? "enable" : "disable",
+			ri->desc.id, ret);
+
+	return ret;
+}
+
+static inline int ricoh583_cache_regulator_register(struct device *parent,
+	struct ricoh583_regulator *ri)
+{
+	ri->vout_reg_cache = 0;
+	return ricoh583_read(parent, ri->vout_reg, &ri->vout_reg_cache);
+}
+
+static int __devinit ricoh583_regulator_probe(struct platform_device *pdev)
+{
+	struct ricoh583_regulator *ri = NULL;
+	struct regulator_dev *rdev;
+	struct ricoh583_regulator_platform_data *tps_pdata;
+	int id = pdev->id;
+	int err;
+
+	dev_dbg(&pdev->dev, "Probing reulator %d\n", id);
+
+	ri = find_regulator_info(id);
+	if (ri == NULL) {
+		dev_err(&pdev->dev, "invalid regulator ID specified\n");
+		return -EINVAL;
+	}
+	tps_pdata = pdev->dev.platform_data;
+	ri->dev = &pdev->dev;
+
+	err = ricoh583_cache_regulator_register(pdev->dev.parent, ri);
+	if (err) {
+		dev_err(&pdev->dev, "Fail in caching register\n");
+		return err;
+	}
+
+	err = ricoh583_regulator_preinit(pdev->dev.parent, ri, tps_pdata);
+	if (err) {
+		dev_err(&pdev->dev, "Fail in pre-initialisation\n");
+		return err;
+	}
+	rdev = regulator_register(&ri->desc, &pdev->dev,
+				&tps_pdata->regulator, ri);
+	if (IS_ERR_OR_NULL(rdev)) {
+		dev_err(&pdev->dev, "failed to register regulator %s\n",
+				ri->desc.name);
+		return PTR_ERR(rdev);
+	}
+
+	platform_set_drvdata(pdev, rdev);
+	return 0;
+}
+
+static int __devexit ricoh583_regulator_remove(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev = platform_get_drvdata(pdev);
+
+	regulator_unregister(rdev);
+	return 0;
+}
+
+static struct platform_driver ricoh583_regulator_driver = {
+	.driver	= {
+		.name	= "ricoh583-regulator",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= ricoh583_regulator_probe,
+	.remove		= __devexit_p(ricoh583_regulator_remove),
+};
+
+static int __init ricoh583_regulator_init(void)
+{
+	return platform_driver_register(&ricoh583_regulator_driver);
+}
+subsys_initcall(ricoh583_regulator_init);
+
+static void __exit ricoh583_regulator_exit(void)
+{
+	platform_driver_unregister(&ricoh583_regulator_driver);
+}
+module_exit(ricoh583_regulator_exit);
+
+MODULE_DESCRIPTION("RICOH583 regulator driver");
+MODULE_ALIAS("platform:ricoh583-regulator");
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/tps51632-regulator.c b/drivers/regulator/tps51632-regulator.c
new file mode 100644
index 000000000000..420362b5f116
--- /dev/null
+++ b/drivers/regulator/tps51632-regulator.c
@@ -0,0 +1,307 @@
+/*
+ * tps51632-regulator.c -- Maxim tps51632
+ *
+ * Regulator driver for TPS51632 3-2-1 Phase D-Cap Step Down Driverless
+ * Controller with serial VID control and DVFS.
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * Author: Laxman Dewangan <ldewangan@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any kind,
+ * whether express or implied; 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/err.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/tps51632-regulator.h>
+#include <linux/slab.h>
+
+/* Register definitions */
+#define TPS51632_VOLTAGE_SELECT_REG		0x0
+#define TPS51632_VOLTAGE_BASE_REG		0x1
+#define TPS51632_OFFSET_REG			0x2
+#define TPS51632_IMON_REG			0x3
+#define TPS51632_VMAX_REG			0x4
+#define TPS51632_DVFS_CONTROL_REG		0x5
+#define TPS51632_POWER_STATE_REG		0x6
+#define TPS51632_SLEW_REGS			0x7
+#define TPS51632_FAULT_REG			0x14
+
+#define TPS51632_MAX_REG			0x15
+
+#define TPS51632_VOUT_MASK		0x7F
+#define TPS51632_VOUT_OFFSET_MASK	0x1F
+#define TPS51632_VMAX_MASK		0x7F
+#define TPS51632_VMAX_LOCK		0x80
+
+/* TPS51632_DVFS_CONTROL_REG */
+#define TPS51632_DVFS_PWMEN		0x1
+#define TPS51632_DVFS_STEP_20		0x2
+#define TPS51632_DVFS_VMAX_PG		0x4
+#define TPS51632_DVFS_PWMRST		0x8
+#define TPS51632_DVFS_OCA_EN		0x10
+#define TPS51632_DVFS_FCCM		0x20
+
+/* TPS51632_POWER_STATE_REG */
+#define TPS51632_POWER_STATE_MASK		0x03
+#define TPS51632_POWER_STATE_MULTI_PHASE_CCM	0x0
+#define TPS51632_POWER_STATE_SINGLE_PHASE_CCM	0x1
+#define TPS51632_POWER_STATE_SINGLE_PHASE_DCM	0x2
+
+#define TPS51632_MIN_VOLATGE	500000
+#define TPS51632_MAX_VOLATGE	1520000
+#define TPS51632_VOLATGE_STEP	10000
+#define TPS51632_MAX_SEL	0x7F
+
+/* TPS51632 chip information */
+struct tps51632_chip {
+	struct device *dev;
+	struct regulator_desc desc;
+	struct regulator_dev *rdev;
+	struct regmap *regmap;
+
+	bool pwm_enabled;
+	unsigned int change_uv_per_us;
+};
+
+static int tps51632_dcdc_get_voltage_sel(struct regulator_dev *rdev)
+{
+	struct tps51632_chip *tps = rdev_get_drvdata(rdev);
+	unsigned int data;
+	int ret;
+	unsigned int reg = TPS51632_VOLTAGE_SELECT_REG;
+
+	if (tps->pwm_enabled)
+		reg = TPS51632_VOLTAGE_BASE_REG;
+	ret = regmap_read(tps->regmap, reg, &data);
+	if (ret < 0) {
+		dev_err(tps->dev, "reg read failed, err %d\n", ret);
+		return ret;
+	}
+	return data & TPS51632_VOUT_MASK;
+}
+
+static int tps51632_dcdc_set_voltage(struct regulator_dev *rdev,
+	     int min_uV, int max_uV, unsigned *selector)
+{
+	struct tps51632_chip *tps = rdev_get_drvdata(rdev);
+	int vsel;
+	int ret;
+
+	if ((max_uV < min_uV) || (max_uV < TPS51632_MIN_VOLATGE) ||
+			(min_uV > TPS51632_MAX_VOLATGE))
+		return -EINVAL;
+
+	vsel = DIV_ROUND_UP(min_uV - TPS51632_MIN_VOLATGE,
+			TPS51632_VOLATGE_STEP) + 0x19;
+	if (selector)
+		*selector = (vsel & TPS51632_VOUT_MASK);
+
+	ret = regmap_write(tps->regmap, TPS51632_VOLTAGE_SELECT_REG, vsel);
+	if (ret < 0)
+		dev_err(tps->dev, "reg write failed, err %d\n", ret);
+	return ret;
+}
+
+static int tps51632_dcdc_list_voltage(struct regulator_dev *rdev,
+					unsigned selector)
+{
+	if (selector > TPS51632_MAX_SEL)
+		return -EINVAL;
+
+	return TPS51632_MIN_VOLATGE + (selector - 0x19) * TPS51632_VOLATGE_STEP;
+}
+
+static int tps51632_dcdc_set_voltage_time_sel(struct regulator_dev *rdev,
+		unsigned int old_selector, unsigned int new_selector)
+{
+	struct tps51632_chip *tps = rdev_get_drvdata(rdev);
+	int old_uV, new_uV;
+
+	old_uV = tps51632_dcdc_list_voltage(rdev, old_selector);
+	if (old_uV < 0)
+		return old_uV;
+
+	new_uV = tps51632_dcdc_list_voltage(rdev, new_selector);
+	if (new_uV < 0)
+		return new_uV;
+
+	return DIV_ROUND_UP(abs(old_uV - new_uV), tps->change_uv_per_us);
+}
+
+static struct regulator_ops tps51632_dcdc_ops = {
+	.get_voltage_sel	= tps51632_dcdc_get_voltage_sel,
+	.set_voltage		= tps51632_dcdc_set_voltage,
+	.list_voltage		= tps51632_dcdc_list_voltage,
+	.set_voltage_time_sel	= tps51632_dcdc_set_voltage_time_sel,
+};
+
+static int __devinit tps51632_init_dcdc(struct tps51632_chip *tps,
+		struct tps51632_regulator_platform_data *pdata)
+{
+	int ret;
+	uint8_t	control = 0;
+	int vsel;
+
+	if (pdata->enable_pwm) {
+		control = TPS51632_DVFS_PWMEN;
+		tps->pwm_enabled = pdata->enable_pwm;
+		vsel = DIV_ROUND_UP(pdata->base_voltage_uV -
+			TPS51632_MIN_VOLATGE, TPS51632_VOLATGE_STEP) + 0x19;
+		ret = regmap_write(tps->regmap, TPS51632_VOLTAGE_BASE_REG,
+							vsel);
+		if (ret < 0) {
+			dev_err(tps->dev, "BASE reg write failed, err %d\n",
+					ret);
+			return ret;
+		}
+	}
+	if (pdata->dvfs_step_20mV)
+		control = TPS51632_DVFS_STEP_20;
+	if (pdata->enable_vmax_alarm)
+		control = TPS51632_DVFS_VMAX_PG;
+	if (pdata->enable_overcurrent_alram)
+		control = TPS51632_DVFS_OCA_EN;
+	if (pdata->max_voltage_uV) {
+		vsel = DIV_ROUND_UP(pdata->max_voltage_uV -
+			TPS51632_MIN_VOLATGE, TPS51632_VOLATGE_STEP) + 0x19;
+		ret = regmap_write(tps->regmap, TPS51632_VMAX_REG, vsel);
+		if (ret < 0) {
+			dev_err(tps->dev, "VMAX write failed, err %d\n", ret);
+			return ret;
+		}
+	}
+	ret = regmap_write(tps->regmap, TPS51632_DVFS_CONTROL_REG, control);
+	if (ret < 0) {
+		dev_err(tps->dev, "DVFS reg write failed, err %d\n", ret);
+		return ret;
+	}
+
+	tps->change_uv_per_us = max(6000u, pdata->slew_rate_uv_per_us);
+
+	vsel = BIT(tps->change_uv_per_us/6000 - 1);
+
+	ret = regmap_write(tps->regmap, TPS51632_SLEW_REGS, vsel);
+	if (ret < 0)
+		dev_err(tps->dev, "SLEW reg write failed, err %d\n", ret);
+	return ret;
+}
+
+static const struct regmap_config tps51632_regmap_config = {
+	.reg_bits		= 8,
+	.val_bits		= 8,
+	.max_register		= TPS51632_MAX_REG - 1,
+	.cache_type		= REGCACHE_RBTREE,
+};
+
+static int __devinit tps51632_probe(struct i2c_client *client,
+				const struct i2c_device_id *id)
+{
+	struct tps51632_regulator_platform_data *pdata;
+	struct regulator_dev *rdev;
+	struct tps51632_chip *tps;
+	int ret;
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->dev, "No Platform data\n");
+		return -EINVAL;
+	}
+
+	tps = devm_kzalloc(&client->dev, sizeof(*tps), GFP_KERNEL);
+	if (!tps) {
+		dev_err(&client->dev, "Memory allocation failed\n");
+		return -ENOMEM;
+	}
+
+	tps->dev = &client->dev;
+	tps->desc.name = id->name;
+	tps->desc.id = 0;
+	tps->desc.ops = &tps51632_dcdc_ops;
+	tps->desc.type = REGULATOR_VOLTAGE;
+	tps->desc.owner = THIS_MODULE;
+	tps->regmap = devm_regmap_init_i2c(client, &tps51632_regmap_config);
+	if (IS_ERR(tps->regmap)) {
+		ret = PTR_ERR(tps->regmap);
+		dev_err(&client->dev, "regmap init failed, err %d\n", ret);
+		return ret;
+	}
+	i2c_set_clientdata(client, tps);
+
+	ret = tps51632_init_dcdc(tps, pdata);
+	if (ret < 0) {
+		dev_err(tps->dev, "Init failed, err = %d\n", ret);
+		return ret;
+	}
+
+	/* Register the regulators */
+	rdev = regulator_register(&tps->desc, &client->dev,
+			pdata->reg_init_data, tps);
+	if (IS_ERR(rdev)) {
+		dev_err(tps->dev, "regulator register failed\n");
+		return PTR_ERR(rdev);
+	}
+
+	tps->rdev = rdev;
+	return 0;
+}
+
+static int __devexit tps51632_remove(struct i2c_client *client)
+{
+	struct tps51632_chip *tps = i2c_get_clientdata(client);
+
+	regulator_unregister(tps->rdev);
+	return 0;
+}
+
+static const struct i2c_device_id tps51632_id[] = {
+	{.name = "tps51632",},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, tps51632_id);
+
+static struct i2c_driver tps51632_i2c_driver = {
+	.driver = {
+		.name = "tps51632",
+		.owner = THIS_MODULE,
+	},
+	.probe = tps51632_probe,
+	.remove = __devexit_p(tps51632_remove),
+	.id_table = tps51632_id,
+};
+
+static int __init tps51632_init(void)
+{
+	return i2c_add_driver(&tps51632_i2c_driver);
+}
+subsys_initcall(tps51632_init);
+
+static void __exit tps51632_cleanup(void)
+{
+	i2c_del_driver(&tps51632_i2c_driver);
+}
+module_exit(tps51632_cleanup);
+
+MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
+MODULE_DESCRIPTION("TPS51632 voltage regulator driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/tps62360-regulator.c b/drivers/regulator/tps62360-regulator.c
new file mode 100644
index 000000000000..095104f73b57
--- /dev/null
+++ b/drivers/regulator/tps62360-regulator.c
@@ -0,0 +1,542 @@
+/*
+ * tps62360.c -- TI tps62360
+ *
+ * Driver for processor core supply tps62360, tps62361B, tps62362 and tps62363.
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * Author: Laxman Dewangan <ldewangan@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any kind,
+ * whether express or implied; 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/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/tps62360.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/regmap.h>
+
+/* Register definitions */
+#define REG_VSET0		0
+#define REG_VSET1		1
+#define REG_VSET2		2
+#define REG_VSET3		3
+#define REG_CONTROL		4
+#define REG_TEMP		5
+#define REG_RAMPCTRL		6
+#define REG_CHIPID		8
+
+#define FORCE_PWM_ENABLE	BIT(7)
+
+enum chips {TPS62360, TPS62361, TPS62362, TPS62363};
+
+#define TPS62360_BASE_VOLTAGE	770000
+#define TPS62360_N_VOLTAGES	64
+
+#define TPS62361_BASE_VOLTAGE	500000
+#define TPS62361_N_VOLTAGES	128
+
+/* tps 62360 chip information */
+struct tps62360_chip {
+	struct device *dev;
+	struct regulator_desc desc;
+	struct regulator_dev *rdev;
+	struct regmap *regmap;
+	int chip_id;
+	int vsel0_gpio;
+	int vsel1_gpio;
+	int voltage_base;
+	u8 voltage_reg_mask;
+	bool en_internal_pulldn;
+	bool en_discharge;
+	bool valid_gpios;
+	int lru_index[4];
+	int curr_vset_vsel[4];
+	int curr_vset_id;
+	int change_uv_per_us;
+};
+
+/*
+ * find_voltage_set_register: Find new voltage configuration register
+ * (VSET) id.
+ * The finding of the new VSET register will be based on the LRU mechanism.
+ * Each VSET register will have different voltage configured . This
+ * Function will look if any of the VSET register have requested voltage set
+ * or not.
+ *     - If it is already there then it will make that register as most
+ *       recently used and return as found so that caller need not to set
+ *       the VSET register but need to set the proper gpios to select this
+ *       VSET register.
+ *     - If requested voltage is not found then it will use the least
+ *       recently mechanism to get new VSET register for new configuration
+ *       and will return not_found so that caller need to set new VSET
+ *       register and then gpios (both).
+ */
+static bool find_voltage_set_register(struct tps62360_chip *tps,
+		int req_vsel, int *vset_reg_id)
+{
+	int i;
+	bool found = false;
+	int new_vset_reg = tps->lru_index[3];
+	int found_index = 3;
+
+	for (i = 0; i < 4; ++i) {
+		if (tps->curr_vset_vsel[tps->lru_index[i]] == req_vsel) {
+			new_vset_reg = tps->lru_index[i];
+			found_index = i;
+			found = true;
+			goto update_lru_index;
+		}
+	}
+
+update_lru_index:
+	for (i = found_index; i > 0; i--)
+		tps->lru_index[i] = tps->lru_index[i - 1];
+
+	tps->lru_index[0] = new_vset_reg;
+	*vset_reg_id = new_vset_reg;
+	return found;
+}
+
+static int tps62360_dcdc_get_voltage_sel(struct regulator_dev *dev)
+{
+	struct tps62360_chip *tps = rdev_get_drvdata(dev);
+	int vsel;
+	unsigned int data;
+	int ret;
+
+	ret = regmap_read(tps->regmap, REG_VSET0 + tps->curr_vset_id, &data);
+	if (ret < 0) {
+		dev_err(tps->dev, "%s(): register %d read failed with err %d\n",
+			__func__, REG_VSET0 + tps->curr_vset_id, ret);
+		return ret;
+	}
+	vsel = (int)data & tps->voltage_reg_mask;
+	return vsel;
+}
+
+static int tps62360_dcdc_set_voltage(struct regulator_dev *dev,
+	     int min_uV, int max_uV, unsigned *selector)
+{
+	struct tps62360_chip *tps = rdev_get_drvdata(dev);
+	int vsel;
+	int ret;
+	bool found = false;
+	int new_vset_id = tps->curr_vset_id;
+
+	if ((max_uV < min_uV) || (max_uV < tps->voltage_base))
+		return -EINVAL;
+
+	if (min_uV > (tps->voltage_base + (tps->desc.n_voltages - 1) * 10000))
+		return -EINVAL;
+
+	vsel = DIV_ROUND_UP(min_uV - tps->voltage_base, 10000);
+	if (selector)
+		*selector = (vsel & tps->voltage_reg_mask);
+
+	/*
+	 * If gpios are available to select the VSET register then least
+	 * recently used register for new configuration.
+	 */
+	if (tps->valid_gpios)
+		found = find_voltage_set_register(tps, vsel, &new_vset_id);
+
+	if (!found) {
+		ret = regmap_update_bits(tps->regmap, REG_VSET0 + new_vset_id,
+				tps->voltage_reg_mask, vsel);
+		if (ret < 0) {
+			dev_err(tps->dev,
+				"%s(): register %d update failed with err %d\n",
+				 __func__, REG_VSET0 + new_vset_id, ret);
+			return ret;
+		}
+		tps->curr_vset_id = new_vset_id;
+		tps->curr_vset_vsel[new_vset_id] = vsel;
+	}
+
+	/* Select proper VSET register vio gpios */
+	if (tps->valid_gpios) {
+		gpio_set_value_cansleep(tps->vsel0_gpio, new_vset_id & 0x1);
+		gpio_set_value_cansleep(tps->vsel1_gpio,
+					(new_vset_id >> 1) & 0x1);
+	}
+	return 0;
+}
+
+static int tps62360_dcdc_list_voltage(struct regulator_dev *dev,
+					unsigned selector)
+{
+	struct tps62360_chip *tps = rdev_get_drvdata(dev);
+
+	if (selector >= tps->desc.n_voltages)
+		return -EINVAL;
+
+	return tps->voltage_base + selector * 10000;
+}
+
+static int tps62360_set_voltage_time_sel(struct regulator_dev *rdev,
+		unsigned int old_selector, unsigned int new_selector)
+{
+	struct tps62360_chip *tps = rdev_get_drvdata(rdev);
+	int old_uV, new_uV;
+
+	old_uV = tps62360_dcdc_list_voltage(rdev, old_selector);
+	if (old_uV < 0)
+		return old_uV;
+
+	new_uV = tps62360_dcdc_list_voltage(rdev, new_selector);
+	if (new_uV < 0)
+		return new_uV;
+
+	return DIV_ROUND_UP(abs(old_uV - new_uV), tps->change_uv_per_us);
+}
+
+static int tps62360_set_mode(struct regulator_dev *rdev, unsigned int mode)
+{
+	struct tps62360_chip *tps = rdev_get_drvdata(rdev);
+	int i;
+	int val;
+	int ret;
+
+	/* Enable force PWM mode in FAST mode only. */
+	switch (mode) {
+	case REGULATOR_MODE_FAST:
+		val = FORCE_PWM_ENABLE;
+		break;
+
+	case REGULATOR_MODE_NORMAL:
+		val = 0;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	if (!tps->valid_gpios) {
+		ret = regmap_update_bits(tps->regmap,
+			REG_VSET0 + tps->curr_vset_id, FORCE_PWM_ENABLE, val);
+		if (ret < 0)
+			dev_err(tps->dev,
+				"%s(): register %d update failed with err %d\n",
+				__func__, REG_VSET0 + tps->curr_vset_id, ret);
+		return ret;
+	}
+
+	/* If gpios are valid then all register set need to be control */
+	for (i = 0; i < 4; ++i) {
+		ret = regmap_update_bits(tps->regmap,
+					REG_VSET0 + i, FORCE_PWM_ENABLE, val);
+		if (ret < 0) {
+			dev_err(tps->dev,
+				"%s(): register %d update failed with err %d\n",
+				__func__, REG_VSET0 + i, ret);
+			return ret;
+		}
+	}
+	return ret;
+}
+
+static unsigned int tps62360_get_mode(struct regulator_dev *rdev)
+{
+	struct tps62360_chip *tps = rdev_get_drvdata(rdev);
+	unsigned int data;
+	int ret;
+
+	ret = regmap_read(tps->regmap, REG_VSET0 + tps->curr_vset_id, &data);
+	if (ret < 0) {
+		dev_err(tps->dev, "%s(): register %d read failed with err %d\n",
+			__func__, REG_VSET0 + tps->curr_vset_id, ret);
+		return ret;
+	}
+	return (data & FORCE_PWM_ENABLE) ?
+				REGULATOR_MODE_FAST : REGULATOR_MODE_NORMAL;
+}
+
+static struct regulator_ops tps62360_dcdc_ops = {
+	.get_voltage_sel	= tps62360_dcdc_get_voltage_sel,
+	.set_voltage		= tps62360_dcdc_set_voltage,
+	.list_voltage		= tps62360_dcdc_list_voltage,
+	.set_voltage_time_sel	= tps62360_set_voltage_time_sel,
+	.set_mode		= tps62360_set_mode,
+	.get_mode		= tps62360_get_mode,
+};
+
+static int __devinit tps62360_init_dcdc(struct tps62360_chip *tps,
+		struct tps62360_regulator_platform_data *pdata)
+{
+	int ret;
+	unsigned int ramp_ctrl;
+
+	/* Initialize internal pull up/down control */
+	if (tps->en_internal_pulldn)
+		ret = regmap_write(tps->regmap, REG_CONTROL, 0xE0);
+	else
+		ret = regmap_write(tps->regmap, REG_CONTROL, 0x0);
+	if (ret < 0) {
+		dev_err(tps->dev,
+			"%s(): register %d write failed with err %d\n",
+			__func__, REG_CONTROL, ret);
+		return ret;
+	}
+
+	/* Reset output discharge path to reduce power consumption */
+	ret = regmap_update_bits(tps->regmap, REG_RAMPCTRL, BIT(2), 0);
+	if (ret < 0) {
+		dev_err(tps->dev,
+			"%s(): register %d update failed with err %d\n",
+			__func__, REG_RAMPCTRL, ret);
+		return ret;
+	}
+
+	/* Get ramp value from ramp control register */
+	ret = regmap_read(tps->regmap, REG_RAMPCTRL, &ramp_ctrl);
+	if (ret < 0) {
+		dev_err(tps->dev,
+			"%s(): register %d read failed with err %d\n",
+			__func__, REG_RAMPCTRL, ret);
+		return ret;
+	}
+	ramp_ctrl = (ramp_ctrl >> 4) & 0x7;
+
+	/* ramp mV/us = 32/(2^ramp_ctrl) */
+	tps->change_uv_per_us = DIV_ROUND_UP(32000, BIT(ramp_ctrl));
+	return ret;
+}
+
+static bool is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	return false;
+}
+
+static const struct regmap_config tps62360_regmap_config = {
+	.reg_bits		= 8,
+	.val_bits		= 8,
+	.volatile_reg		= is_volatile_reg,
+	.max_register		= REG_CHIPID,
+	.cache_type		= REGCACHE_RBTREE,
+};
+
+static int __devinit tps62360_probe(struct i2c_client *client,
+				     const struct i2c_device_id *id)
+{
+	struct tps62360_regulator_platform_data *pdata;
+	struct regulator_dev *rdev;
+	struct tps62360_chip *tps;
+	int ret;
+	int i;
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->dev, "%s(): Platform data not found\n",
+						__func__);
+		return -EIO;
+	}
+
+	tps = devm_kzalloc(&client->dev, sizeof(*tps), GFP_KERNEL);
+	if (!tps) {
+		dev_err(&client->dev, "%s(): Memory allocation failed\n",
+						__func__);
+		return -ENOMEM;
+	}
+
+	tps->en_discharge = pdata->en_discharge;
+	tps->en_internal_pulldn = pdata->en_internal_pulldn;
+	tps->vsel0_gpio = pdata->vsel0_gpio;
+	tps->vsel1_gpio = pdata->vsel1_gpio;
+	tps->dev = &client->dev;
+
+	switch (id->driver_data) {
+	case TPS62360:
+	case TPS62362:
+		tps->voltage_base = TPS62360_BASE_VOLTAGE;
+		tps->voltage_reg_mask = 0x3F;
+		tps->desc.n_voltages = TPS62360_N_VOLTAGES;
+		break;
+	case TPS62361:
+	case TPS62363:
+		tps->voltage_base = TPS62361_BASE_VOLTAGE;
+		tps->voltage_reg_mask = 0x7F;
+		tps->desc.n_voltages = TPS62361_N_VOLTAGES;
+		break;
+	default:
+		return -ENODEV;
+	}
+
+	tps->desc.name = id->name;
+	tps->desc.id = 0;
+	tps->desc.ops = &tps62360_dcdc_ops;
+	tps->desc.type = REGULATOR_VOLTAGE;
+	tps->desc.owner = THIS_MODULE;
+	tps->regmap = devm_regmap_init_i2c(client, &tps62360_regmap_config);
+	if (IS_ERR(tps->regmap)) {
+		ret = PTR_ERR(tps->regmap);
+		dev_err(&client->dev,
+			"%s(): regmap allocation failed with err %d\n",
+			__func__, ret);
+		return ret;
+	}
+	i2c_set_clientdata(client, tps);
+
+	tps->curr_vset_id = (pdata->vsel1_def_state & 1) * 2 +
+				(pdata->vsel0_def_state & 1);
+	tps->lru_index[0] = tps->curr_vset_id;
+	tps->valid_gpios = false;
+
+	if (gpio_is_valid(tps->vsel0_gpio) && gpio_is_valid(tps->vsel1_gpio)) {
+		int gpio_flags;
+		gpio_flags = (pdata->vsel0_def_state) ?
+				GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
+		ret = gpio_request_one(tps->vsel0_gpio,
+				gpio_flags, "tps62360-vsel0");
+		if (ret) {
+			dev_err(&client->dev,
+				"%s(): Could not obtain vsel0 GPIO %d: %d\n",
+				__func__, tps->vsel0_gpio, ret);
+			goto err_gpio0;
+		}
+
+		gpio_flags = (pdata->vsel1_def_state) ?
+				GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
+		ret = gpio_request_one(tps->vsel1_gpio,
+				gpio_flags, "tps62360-vsel1");
+		if (ret) {
+			dev_err(&client->dev,
+				"%s(): Could not obtain vsel1 GPIO %d: %d\n",
+				__func__, tps->vsel1_gpio, ret);
+			goto err_gpio1;
+		}
+		tps->valid_gpios = true;
+
+		/*
+		 * Initialize the lru index with vset_reg id
+		 * The index 0 will be most recently used and
+		 * set with the tps->curr_vset_id */
+		for (i = 0; i < 4; ++i)
+			tps->lru_index[i] = i;
+		tps->lru_index[0] = tps->curr_vset_id;
+		tps->lru_index[tps->curr_vset_id] = 0;
+	}
+
+	ret = tps62360_init_dcdc(tps, pdata);
+	if (ret < 0) {
+		dev_err(tps->dev, "%s(): Init failed with err = %d\n",
+				__func__, ret);
+		goto err_init;
+	}
+
+	/* Register the regulators */
+	rdev = regulator_register(&tps->desc, &client->dev,
+				&pdata->reg_init_data, tps);
+	if (IS_ERR(rdev)) {
+		dev_err(tps->dev,
+			"%s(): regulator register failed with err %s\n",
+			__func__, id->name);
+		ret = PTR_ERR(rdev);
+		goto err_init;
+	}
+
+	tps->rdev = rdev;
+	return 0;
+
+err_init:
+	if (gpio_is_valid(tps->vsel1_gpio))
+		gpio_free(tps->vsel1_gpio);
+err_gpio1:
+	if (gpio_is_valid(tps->vsel0_gpio))
+		gpio_free(tps->vsel0_gpio);
+err_gpio0:
+	return ret;
+}
+
+/**
+ * tps62360_remove - tps62360 driver i2c remove handler
+ * @client: i2c driver client device structure
+ *
+ * Unregister TPS driver as an i2c client device driver
+ */
+static int __devexit tps62360_remove(struct i2c_client *client)
+{
+	struct tps62360_chip *tps = i2c_get_clientdata(client);
+
+	if (gpio_is_valid(tps->vsel1_gpio))
+		gpio_free(tps->vsel1_gpio);
+
+	if (gpio_is_valid(tps->vsel0_gpio))
+		gpio_free(tps->vsel0_gpio);
+
+	regulator_unregister(tps->rdev);
+	return 0;
+}
+
+static void tps62360_shutdown(struct i2c_client *client)
+{
+	struct tps62360_chip *tps = i2c_get_clientdata(client);
+	int st;
+
+	if (!tps->en_discharge)
+		return;
+
+	/* Configure the output discharge path */
+	st = regmap_update_bits(tps->regmap, REG_RAMPCTRL, BIT(2), BIT(2));
+	if (st < 0)
+		dev_err(tps->dev,
+			"%s(): register %d update failed with err %d\n",
+			__func__, REG_RAMPCTRL, st);
+}
+
+static const struct i2c_device_id tps62360_id[] = {
+	{.name = "tps62360", .driver_data = TPS62360},
+	{.name = "tps62361", .driver_data = TPS62361},
+	{.name = "tps62362", .driver_data = TPS62362},
+	{.name = "tps62363", .driver_data = TPS62363},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, tps62360_id);
+
+static struct i2c_driver tps62360_i2c_driver = {
+	.driver = {
+		.name = "tps62360",
+		.owner = THIS_MODULE,
+	},
+	.probe = tps62360_probe,
+	.remove = __devexit_p(tps62360_remove),
+	.shutdown = tps62360_shutdown,
+	.id_table = tps62360_id,
+};
+
+static int __init tps62360_init(void)
+{
+	return i2c_add_driver(&tps62360_i2c_driver);
+}
+subsys_initcall(tps62360_init);
+
+static void __exit tps62360_cleanup(void)
+{
+	i2c_del_driver(&tps62360_i2c_driver);
+}
+module_exit(tps62360_cleanup);
+
+MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
+MODULE_DESCRIPTION("TPS6236x voltage regulator driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/tps6238x0-regulator.c b/drivers/regulator/tps6238x0-regulator.c
new file mode 100644
index 000000000000..611b9425a263
--- /dev/null
+++ b/drivers/regulator/tps6238x0-regulator.c
@@ -0,0 +1,470 @@
+/*
+ * tps6238x0-regulator.c -- TI tps623850/tps623860/tps623870
+ *
+ * Driver for processor core supply tps623850, tps623860 and tps623870
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * Author: Laxman Dewangan <ldewangan@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any kind,
+ * whether express or implied; 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/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/tps6238x0-regulator.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/regmap.h>
+
+/* Register definitions */
+#define REG_VSET0		0
+#define REG_VSET1		1
+#define REG_MODE0		2
+#define REG_MODE1		3
+#define REG_CONTROL		4
+#define REG_EXCEPTION		5
+#define REG_RAMPCTRL		6
+#define REG_IOUT		7
+#define REG_CHIPID		8
+
+#define TPS6238X0_BASE_VOLTAGE	500000
+#define TPS6238X0_N_VOLTAGES	128
+#define TPS6238X0_MAX_VSET	2
+#define TPS6238X0_VOUT_MASK	0x7F
+
+/* tps 6238x0 chip information */
+struct tps6238x0_chip {
+	const char *name;
+	struct device *dev;
+	struct regulator_desc desc;
+	struct regulator_dev *rdev;
+	struct regmap *regmap;
+	int vsel_gpio;
+	int change_uv_per_us;
+	bool en_internal_pulldn;
+	bool valid_gpios;
+	int lru_index[TPS6238X0_MAX_VSET];
+	int curr_vset_vsel[TPS6238X0_MAX_VSET];
+	int curr_vset_id;
+};
+
+/*
+ * find_voltage_set_register: Find new voltage configuration register
+ * (VSET) id.
+ * The finding of the new VSET register will be based on the LRU mechanism.
+ * Each VSET register will have different voltage configured . This
+ * Function will look if any of the VSET register have requested voltage set
+ * or not.
+ *     - If it is already there then it will make that register as most
+ *       recently used and return as found so that caller need not to set
+ *       the VSET register but need to set the proper gpios to select this
+ *       VSET register.
+ *     - If requested voltage is not found then it will use the least
+ *       recently mechanism to get new VSET register for new configuration
+ *       and will return not_found so that caller need to set new VSET
+ *       register and then gpios (both).
+ */
+static bool find_voltage_set_register(struct tps6238x0_chip *tps,
+		int req_vsel, int *vset_reg_id)
+{
+	int i;
+	bool found = false;
+	int new_vset_reg = tps->lru_index[1];
+	int found_index = 1;
+	for (i = 0; i < TPS6238X0_MAX_VSET; ++i) {
+		if (tps->curr_vset_vsel[tps->lru_index[i]] == req_vsel) {
+			new_vset_reg = tps->lru_index[i];
+			found_index = i;
+			found = true;
+			goto update_lru_index;
+		}
+	}
+
+update_lru_index:
+	for (i = found_index; i > 0; i--)
+		tps->lru_index[i] = tps->lru_index[i - 1];
+
+	tps->lru_index[0] = new_vset_reg;
+	*vset_reg_id = new_vset_reg;
+	return found;
+}
+
+static int tps6238x0_get_voltage_sel(struct regulator_dev *dev)
+{
+	struct tps6238x0_chip *tps = rdev_get_drvdata(dev);
+	unsigned int data;
+	int ret;
+
+	ret = regmap_read(tps->regmap, REG_VSET0 + tps->curr_vset_id, &data);
+	if (ret < 0) {
+		dev_err(tps->dev, "%s: Error in reading register %d\n",
+			__func__, REG_VSET0 + tps->curr_vset_id);
+		return ret;
+	}
+	return data & TPS6238X0_VOUT_MASK;
+}
+
+static int tps6238x0_set_voltage(struct regulator_dev *dev,
+	     int min_uV, int max_uV, unsigned *selector)
+{
+	struct tps6238x0_chip *tps = rdev_get_drvdata(dev);
+	int vsel;
+	int ret;
+	bool found = false;
+	int new_vset_id = tps->curr_vset_id;
+
+	if (min_uV >
+		((TPS6238X0_BASE_VOLTAGE + (TPS6238X0_N_VOLTAGES - 1) * 10000)))
+		return -EINVAL;
+
+	if ((max_uV < min_uV) || (max_uV < TPS6238X0_BASE_VOLTAGE))
+		return -EINVAL;
+
+	vsel = DIV_ROUND_UP(min_uV - TPS6238X0_BASE_VOLTAGE, 10000);
+	if (selector)
+		*selector = (vsel & TPS6238X0_VOUT_MASK);
+
+	/*
+	 * If gpios are available to select the VSET register then least
+	 * recently used register for new configuration.
+	 */
+	if (tps->valid_gpios)
+		found = find_voltage_set_register(tps, vsel, &new_vset_id);
+
+	if (!found) {
+		ret = regmap_update_bits(tps->regmap, REG_VSET0 + new_vset_id,
+				TPS6238X0_VOUT_MASK, vsel);
+		if (ret < 0) {
+			dev_err(tps->dev, "%s: Error in updating register %d\n",
+				 __func__, REG_VSET0 + new_vset_id);
+			return ret;
+		}
+		tps->curr_vset_id = new_vset_id;
+		tps->curr_vset_vsel[new_vset_id] = vsel;
+	}
+
+	/* Select proper VSET register vio gpios */
+	if (tps->valid_gpios)
+		gpio_set_value_cansleep(tps->vsel_gpio, new_vset_id & 0x1);
+	return 0;
+}
+
+static int tps6238x0_list_voltage(struct regulator_dev *dev,
+			unsigned selector)
+{
+	struct tps6238x0_chip *tps = rdev_get_drvdata(dev);
+
+	if ((selector < 0) || (selector >= tps->desc.n_voltages))
+		return -EINVAL;
+
+	return TPS6238X0_BASE_VOLTAGE + selector * 10000;
+}
+
+static int tps6238x0_regulator_enable_time(struct regulator_dev *rdev)
+{
+	return 300;
+}
+
+static int tps6238x0_set_voltage_time_sel(struct regulator_dev *rdev,
+		unsigned int old_selector, unsigned int new_selector)
+{
+	struct tps6238x0_chip *tps = rdev_get_drvdata(rdev);
+	int old_uV, new_uV;
+	old_uV = tps6238x0_list_voltage(rdev, old_selector);
+
+	if (old_uV < 0)
+		return old_uV;
+
+	new_uV = tps6238x0_list_voltage(rdev, new_selector);
+	if (new_uV < 0)
+		return new_uV;
+
+	return DIV_ROUND_UP(abs(old_uV - new_uV),
+				tps->change_uv_per_us);
+}
+
+static int tps6238x0_is_enable(struct regulator_dev *rdev)
+{
+	struct tps6238x0_chip *tps = rdev_get_drvdata(rdev);
+	unsigned int data;
+	int ret;
+
+	ret = regmap_read(tps->regmap, REG_VSET0 + tps->curr_vset_id, &data);
+	if (ret < 0) {
+		dev_err(tps->dev, "%s: Error in reading register %d\n",
+			__func__, REG_VSET0 + tps->curr_vset_id);
+		return ret;
+	}
+	return !!(data & BIT(7));
+}
+
+static int tps6238x0_enable(struct regulator_dev *rdev)
+{
+	struct tps6238x0_chip *tps = rdev_get_drvdata(rdev);
+	int ret;
+	int i;
+
+	/* Enable required VSET configuration */
+	for (i = 0; i < TPS6238X0_MAX_VSET; ++i) {
+		unsigned int en = 0;
+		if (tps->valid_gpios || (i == tps->curr_vset_id))
+			en = BIT(7);
+
+		ret = regmap_update_bits(tps->regmap, REG_VSET0 + i,
+				BIT(7), en);
+		if (ret < 0) {
+			dev_err(tps->dev, "%s() fails in updating reg %d\n",
+					__func__, REG_VSET0 + i);
+			return ret;
+		}
+	}
+	return ret;
+}
+
+static int tps6238x0_disable(struct regulator_dev *rdev)
+{
+	struct tps6238x0_chip *tps = rdev_get_drvdata(rdev);
+	int ret;
+	int i;
+
+	/* Disable required VSET configuration */
+	for (i = 0; i < TPS6238X0_MAX_VSET; ++i) {
+		ret = regmap_update_bits(tps->regmap, REG_VSET0 + i,
+				BIT(7), 0);
+		if (ret < 0) {
+			dev_err(tps->dev, "%s() fails in updating reg %d\n",
+					__func__, REG_VSET0 + i);
+			return ret;
+		}
+	}
+	return ret;
+}
+
+static struct regulator_ops tps6238x0_ops = {
+	.is_enabled		= tps6238x0_is_enable,
+	.enable			= tps6238x0_enable,
+	.disable		= tps6238x0_disable,
+	.enable_time		= tps6238x0_regulator_enable_time,
+	.set_voltage_time_sel	= tps6238x0_set_voltage_time_sel,
+	.get_voltage_sel	= tps6238x0_get_voltage_sel,
+	.set_voltage		= tps6238x0_set_voltage,
+	.list_voltage		= tps6238x0_list_voltage,
+};
+
+static int __devinit tps6238x0_configure(struct tps6238x0_chip *tps,
+		struct tps6238x0_regulator_platform_data *pdata)
+{
+	int ret;
+	int i;
+
+	/* Initailize internal pull up/down control */
+	if (tps->en_internal_pulldn)
+		ret = regmap_write(tps->regmap, REG_CONTROL, 0xC0);
+	else
+		ret = regmap_write(tps->regmap, REG_CONTROL, 0x0);
+	if (ret < 0) {
+		dev_err(tps->dev, "%s() fails in writing reg %d\n",
+			__func__, REG_CONTROL);
+		return ret;
+	}
+
+	/* Enable required VSET configuration */
+	for (i = 0; i < TPS6238X0_MAX_VSET; ++i) {
+		unsigned int en = 0;
+		if (tps->valid_gpios || (i == tps->curr_vset_id))
+			en = BIT(7);
+
+		ret = regmap_update_bits(tps->regmap, REG_VSET0 + i,
+				BIT(7), en);
+		if (ret < 0) {
+			dev_err(tps->dev, "%s() fails in updating reg %d\n",
+					__func__, REG_VSET0 + i);
+			return ret;
+		}
+	}
+
+	/* Enable output discharge path to have faster discharge */
+	ret = regmap_update_bits(tps->regmap, REG_RAMPCTRL, BIT(2), BIT(2));
+	if (ret < 0)
+		dev_err(tps->dev, "%s() fails in updating reg %d\n",
+			__func__, REG_RAMPCTRL);
+	tps->change_uv_per_us = 312;
+	return ret;
+}
+
+static const struct regmap_config tps6238x0_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = REG_CHIPID,
+	.num_reg_defaults_raw = REG_CHIPID + 1,
+	.cache_type = REGCACHE_RBTREE,
+};
+
+static int __devinit tps6238x0_probe(struct i2c_client *client,
+				     const struct i2c_device_id *id)
+{
+	struct tps6238x0_regulator_platform_data *pdata;
+	struct regulator_dev *rdev;
+	struct tps6238x0_chip *tps;
+	int ret;
+	int i;
+
+	pdata = client->dev.platform_data;
+	if (!pdata) {
+		dev_err(&client->dev, "%s() Err: Platform data not found\n",
+						__func__);
+		return -EIO;
+	}
+
+	tps = devm_kzalloc(&client->dev, sizeof(*tps), GFP_KERNEL);
+	if (!tps) {
+		dev_err(&client->dev, "%s() Err: Memory allocation fails\n",
+						__func__);
+		return -ENOMEM;
+	}
+
+	tps->en_internal_pulldn = pdata->en_internal_pulldn;
+	tps->vsel_gpio = pdata->vsel_gpio;
+	tps->dev = &client->dev;
+
+	tps->desc.name = id->name;
+	tps->desc.id = 0;
+	tps->desc.n_voltages = TPS6238X0_N_VOLTAGES;
+	tps->desc.ops = &tps6238x0_ops;
+	tps->desc.type = REGULATOR_VOLTAGE;
+	tps->desc.owner = THIS_MODULE;
+	tps->regmap = devm_regmap_init_i2c(client, &tps6238x0_regmap_config);
+	if (IS_ERR(tps->regmap)) {
+		ret = PTR_ERR(tps->regmap);
+		dev_err(&client->dev, "%s() Err: Failed to allocate register"
+			"map: %d\n", __func__, ret);
+		return ret;
+	}
+	i2c_set_clientdata(client, tps);
+
+	tps->curr_vset_id = (pdata->vsel_def_state & 1);
+	tps->lru_index[0] = tps->curr_vset_id;
+	tps->valid_gpios = false;
+
+	if (gpio_is_valid(tps->vsel_gpio)) {
+		int gpio_flag;
+		gpio_flag = (tps->curr_vset_id) ?
+				GPIOF_OUT_INIT_HIGH : GPIOF_OUT_INIT_LOW;
+		ret = gpio_request_one(tps->vsel_gpio,
+					gpio_flag, "tps6238x0-vsel0");
+		if (ret) {
+			dev_err(&client->dev,
+				"Err: Could not obtain vsel GPIO %d: %d\n",
+						tps->vsel_gpio, ret);
+			return ret;
+		}
+		tps->valid_gpios = true;
+
+		/*
+		 * Initialize the lru index with vset_reg id
+		 * The index 0 will be most recently used and
+		 * set with the tps->curr_vset_id */
+		for (i = 0; i < TPS6238X0_MAX_VSET; ++i)
+			tps->lru_index[i] = i;
+		tps->lru_index[0] = tps->curr_vset_id;
+		tps->lru_index[tps->curr_vset_id] = 0;
+	}
+
+	ret = tps6238x0_configure(tps, pdata);
+	if (ret < 0) {
+		dev_err(tps->dev, "%s() Err: Init fails with = %d\n",
+				__func__, ret);
+		goto err_init;
+	}
+
+	/* Register the regulators */
+	rdev = regulator_register(&tps->desc, &client->dev,
+				pdata->init_data, tps);
+	if (IS_ERR(rdev)) {
+		dev_err(tps->dev, "%s() Err: Failed to register %s\n",
+				__func__, id->name);
+		ret = PTR_ERR(rdev);
+		goto err_init;
+	}
+
+	tps->rdev = rdev;
+	return 0;
+
+err_init:
+	if (gpio_is_valid(tps->vsel_gpio))
+		gpio_free(tps->vsel_gpio);
+
+	return ret;
+}
+
+/**
+ * tps6238x0_remove - tps62360 driver i2c remove handler
+ * @client: i2c driver client device structure
+ *
+ * Unregister TPS driver as an i2c client device driver
+ */
+static int __devexit tps6238x0_remove(struct i2c_client *client)
+{
+	struct tps6238x0_chip *tps = i2c_get_clientdata(client);
+
+	if (gpio_is_valid(tps->vsel_gpio))
+		gpio_free(tps->vsel_gpio);
+
+	regulator_unregister(tps->rdev);
+	return 0;
+}
+
+static const struct i2c_device_id tps6238x0_id[] = {
+	{.name = "tps623850", },
+	{.name = "tps623860", },
+	{.name = "tps623870", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, tps6238x0_id);
+
+static struct i2c_driver tps6238x0_i2c_driver = {
+	.driver = {
+		.name = "tps6238x0",
+		.owner = THIS_MODULE,
+	},
+	.probe = tps6238x0_probe,
+	.remove = __devexit_p(tps6238x0_remove),
+	.id_table = tps6238x0_id,
+};
+
+static int __init tps6238x0_init(void)
+{
+	return i2c_add_driver(&tps6238x0_i2c_driver);
+}
+subsys_initcall(tps6238x0_init);
+
+static void __exit tps6238x0_cleanup(void)
+{
+	i2c_del_driver(&tps6238x0_i2c_driver);
+}
+module_exit(tps6238x0_cleanup);
+
+MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
+MODULE_DESCRIPTION("TPS623850/TPS623860/TPS623870 voltage regulator driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/tps65090-regulator.c b/drivers/regulator/tps65090-regulator.c
new file mode 100644
index 000000000000..aca2f56aa172
--- /dev/null
+++ b/drivers/regulator/tps65090-regulator.c
@@ -0,0 +1,360 @@
+/*
+ * Regulator driver for tps65090 power management chip.
+ *
+ * Copyright (c) 2012, NVIDIA 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 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, see <http://www.gnu.org/licenses/>
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/mfd/tps65090.h>
+#include <linux/regulator/tps65090-regulator.h>
+
+struct tps65090_regulator_info {
+	int		id;
+	/* Regulator register address.*/
+	u8		reg_en_reg;
+	u8		en_bit;
+
+	/* used by regulator core */
+	struct regulator_desc	desc;
+};
+
+struct tps65090_regulator {
+	struct tps65090_regulator_info	*rinfo;
+	struct device			*dev;
+	struct regulator_dev		*rdev;
+	bool				enable_ext_control;
+	int				gpio;
+	int				gpio_state;
+};
+
+static inline struct device *to_tps65090_dev(struct regulator_dev *rdev)
+{
+	return rdev_get_dev(rdev)->parent->parent;
+}
+
+static inline bool is_dcdc(int id)
+{
+	if ((id == TPS65090_REGULATOR_DCDC1) ||
+			(id == TPS65090_REGULATOR_DCDC2) ||
+			(id == TPS65090_REGULATOR_DCDC2))
+		return true;
+	return false;
+}
+
+static int tps65090_reg_is_enabled(struct regulator_dev *rdev)
+{
+	struct tps65090_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps65090_dev(rdev);
+	uint8_t control = 0;
+	int ret;
+
+	if (is_dcdc(ri->rinfo->desc.id) && ri->enable_ext_control) {
+		if (gpio_is_valid(ri->gpio))
+			return ri->gpio_state;
+		return 1;
+	}
+
+	ret = tps65090_read(parent, ri->rinfo->reg_en_reg, &control);
+	if (ret < 0) {
+		dev_err(&rdev->dev, "Error in reading reg 0x%x\n",
+			ri->rinfo->reg_en_reg);
+		return ret;
+	}
+	return (((control >> ri->rinfo->en_bit) & 1) == 1);
+}
+
+static int tps65090_reg_enable(struct regulator_dev *rdev)
+{
+	struct tps65090_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps65090_dev(rdev);
+	int ret;
+
+	if (is_dcdc(ri->rinfo->desc.id) && ri->enable_ext_control) {
+		if (gpio_is_valid(ri->gpio)) {
+			gpio_set_value(ri->gpio, 1);
+			ri->gpio_state = 1;
+		}
+		return 0;
+	}
+
+	ret = tps65090_set_bits(parent, ri->rinfo->reg_en_reg,
+				ri->rinfo->en_bit);
+	if (ret < 0)
+		dev_err(&rdev->dev, "Error in updating reg 0x%x\n",
+			ri->rinfo->reg_en_reg);
+	return ret;
+}
+
+static int tps65090_reg_disable(struct regulator_dev *rdev)
+{
+	struct tps65090_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps65090_dev(rdev);
+	int ret;
+
+	if (is_dcdc(ri->rinfo->desc.id) && ri->enable_ext_control) {
+		if (gpio_is_valid(ri->gpio)) {
+			gpio_set_value(ri->gpio, 0);
+			ri->gpio_state = 0;
+		}
+		return 0;
+	}
+
+	ret = tps65090_clr_bits(parent, ri->rinfo->reg_en_reg,
+				ri->rinfo->en_bit);
+	if (ret < 0)
+		dev_err(&rdev->dev, "Error in updating reg 0x%x\n",
+			ri->rinfo->reg_en_reg);
+
+	return ret;
+}
+
+static struct regulator_ops tps65090_ops = {
+	.enable		= tps65090_reg_enable,
+	.disable	= tps65090_reg_disable,
+	.is_enabled	= tps65090_reg_is_enabled,
+};
+
+static struct regulator_ops tps65090_ldo_ops = {
+};
+
+#define tps65090_REG(_id, _sname, _en_reg, _en_bit, _ops)	\
+{							\
+	.reg_en_reg	= _en_reg,			\
+	.en_bit		= _en_bit,			\
+	.id		= TPS65090_REGULATOR_##_id,	\
+	.desc = {					\
+		.name = tps65090_rails(_id),		\
+		.supply_name = _sname,			\
+		.id = TPS65090_REGULATOR_##_id,		\
+		.ops = &_ops,				\
+		.type = REGULATOR_VOLTAGE,		\
+		.owner = THIS_MODULE,			\
+	},						\
+}
+
+static struct tps65090_regulator_info TPS65090_regulator_info[] = {
+	tps65090_REG(DCDC1, "VSYS1",   12, 0, tps65090_ops),
+	tps65090_REG(DCDC2, "VSYS2",   13, 0, tps65090_ops),
+	tps65090_REG(DCDC3, "VSYS3",   14, 0, tps65090_ops),
+	tps65090_REG(LDO1,  "VSYS_L1", 0,  0, tps65090_ldo_ops),
+	tps65090_REG(LDO2,  "VSYS_L2", 0,  0, tps65090_ldo_ops),
+	tps65090_REG(FET1,  "INFET1",  15, 0, tps65090_ops),
+	tps65090_REG(FET2,  "INFET2",  16, 0, tps65090_ops),
+	tps65090_REG(FET3,  "INFET3",  17, 0, tps65090_ops),
+	tps65090_REG(FET4,  "INFET4",  18, 0, tps65090_ops),
+	tps65090_REG(FET5,  "INFET5",  19, 0, tps65090_ops),
+	tps65090_REG(FET6,  "INFET6",  20, 0, tps65090_ops),
+	tps65090_REG(FET7,  "INFET7",  21, 0, tps65090_ops),
+};
+
+static inline struct tps65090_regulator_info *find_regulator_info(int id)
+{
+	struct tps65090_regulator_info *rinfo;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(TPS65090_regulator_info); i++) {
+		rinfo = &TPS65090_regulator_info[i];
+		if (rinfo->desc.id == id)
+			return rinfo;
+	}
+	return NULL;
+}
+
+static int __devinit tps65090_regulator_preinit(int id,
+		struct tps65090_regulator *ri,
+		struct tps65090_regulator_platform_data *tps_pdata)
+{
+	int ret = 0;
+	struct device *parent = ri->dev->parent;
+
+	if (!tps_pdata->enable_ext_control) {
+		ret =  tps65090_clr_bits(parent,
+				ri->rinfo->reg_en_reg, 1);
+		if (ret < 0) {
+			dev_err(ri->dev, "Error in clr reg 0x%x\n",
+				ri->rinfo->reg_en_reg);
+			return ret;
+		}
+	}
+
+	if (gpio_is_valid(tps_pdata->gpio)) {
+		int gpio_flag = GPIOF_OUT_INIT_LOW;
+		const char *sname;
+
+		sname = tps_pdata->reg_init_data->constraints.name;
+		if (!sname)
+			sname = ri->rinfo->desc.name;
+		ri->gpio_state = 0;
+		if (tps_pdata->reg_init_data->constraints.always_on ||
+				tps_pdata->reg_init_data->constraints.boot_on) {
+			gpio_flag = GPIOF_OUT_INIT_HIGH;
+			ri->gpio_state = 1;
+		}
+
+		ret = gpio_request_one(tps_pdata->gpio, gpio_flag, sname);
+		if (ret < 0) {
+			dev_err(ri->dev, "gpio request failed, e %d\n", ret);
+			return ret;
+		}
+	}
+	ret = tps65090_set_bits(parent, ri->rinfo->reg_en_reg, 1);
+	if (ret < 0) {
+		dev_err(ri->dev, "Error in setting reg 0x%x\n",
+				ri->rinfo->reg_en_reg);
+		return ret;
+	}
+	ri->enable_ext_control = true;
+	ri->gpio = tps_pdata->gpio;
+	return ret;
+}
+
+static int __devinit tps65090_regulator_probe(struct platform_device *pdev)
+{
+	struct tps65090_regulator_info *rinfo = NULL;
+	struct tps65090_regulator *ri;
+	struct tps65090_regulator *pmic;
+	struct regulator_dev *rdev;
+	struct tps65090_regulator_platform_data *tps_pdata;
+	struct tps65090_platform_data *tps65090_pdata;
+	int id;
+	int num;
+	int ret;
+
+	dev_dbg(&pdev->dev, "Probing regulator\n");
+
+	tps65090_pdata = dev_get_platdata(pdev->dev.parent);
+	if (!tps65090_pdata || !tps65090_pdata->num_reg_pdata) {
+		dev_err(&pdev->dev, "Proper platform data missing\n");
+		return -EINVAL;
+	}
+
+	pmic = devm_kzalloc(&pdev->dev,
+			tps65090_pdata->num_reg_pdata * sizeof(*pmic),
+			GFP_KERNEL);
+	if (!pmic) {
+		dev_err(&pdev->dev, "mem alloc for pmic failed\n");
+		return -ENOMEM;
+	}
+
+	for (num = 0; num < tps65090_pdata->num_reg_pdata; ++num) {
+		tps_pdata = tps65090_pdata->reg_pdata[num];
+		if (!tps_pdata || !tps_pdata->reg_init_data) {
+			dev_err(&pdev->dev,
+				"Null platform data for regultor %d\n", num);
+			ret = -EINVAL;
+			goto scrub;
+		}
+
+		id = tps_pdata->id;
+		rinfo = find_regulator_info(id);
+		if (!rinfo) {
+			dev_err(&pdev->dev,
+				"invalid regulator ID %d specified\n", id);
+			ret = -EINVAL;
+			goto scrub;
+		}
+
+		ri = &pmic[num];
+		ri->dev = &pdev->dev;
+		ri->rinfo = rinfo;
+
+		if (is_dcdc(id)) {
+			ret = tps65090_regulator_preinit(id, ri, tps_pdata);
+			if (ret < 0) {
+				dev_err(&pdev->dev,
+					"failed to preinit regulator %d\n", id);
+				goto scrub;
+			}
+		}
+		rdev = regulator_register(&ri->rinfo->desc, &pdev->dev,
+				tps_pdata->reg_init_data, ri);
+		if (IS_ERR(rdev)) {
+			dev_err(&pdev->dev, "failed to register regulator %s\n",
+				ri->rinfo->desc.name);
+			ret = PTR_ERR(rdev);
+			goto scrub;
+		}
+		ri->rdev = rdev;
+	}
+
+	platform_set_drvdata(pdev, pmic);
+	return 0;
+
+scrub:
+	while (--num >= 0) {
+		ri = &pmic[num];
+		regulator_unregister(ri->rdev);
+		if (is_dcdc(ri->rinfo->desc.id) && (ri->enable_ext_control)) {
+				if (gpio_is_valid(ri->gpio))
+					gpio_free(ri->gpio);
+		}
+	}
+	return ret;
+}
+
+static int __devexit tps65090_regulator_remove(struct platform_device *pdev)
+{
+	struct tps65090_regulator *pmic = platform_get_drvdata(pdev);
+	struct tps65090_platform_data *tps65090_pdata;
+	struct tps65090_regulator *ri;
+	int num;
+
+	tps65090_pdata = dev_get_platdata(pdev->dev.parent);
+	if (!tps65090_pdata || !tps65090_pdata->num_reg_pdata)
+		return 0;
+
+	for (num = 0; num < tps65090_pdata->num_reg_pdata; ++num) {
+		ri = &pmic[num];
+		regulator_unregister(ri->rdev);
+		if (is_dcdc(ri->rinfo->desc.id) && (ri->enable_ext_control)) {
+				if (gpio_is_valid(ri->gpio))
+					gpio_free(ri->gpio);
+		}
+	}
+	return 0;
+}
+
+static struct platform_driver tps65090_regulator_driver = {
+	.driver	= {
+		.name	= "tps65090-pmic",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= tps65090_regulator_probe,
+	.remove		= __devexit_p(tps65090_regulator_remove),
+};
+
+static int __init tps65090_regulator_init(void)
+{
+	return platform_driver_register(&tps65090_regulator_driver);
+}
+subsys_initcall(tps65090_regulator_init);
+
+static void __exit tps65090_regulator_exit(void)
+{
+	platform_driver_unregister(&tps65090_regulator_driver);
+}
+module_exit(tps65090_regulator_exit);
+
+MODULE_DESCRIPTION("tps65090 regulator driver");
+MODULE_AUTHOR("Venu Byravarasu <vbyravarasu@nvidia.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/tps6586x-regulator.c b/drivers/regulator/tps6586x-regulator.c
index bb04a75a4c98..c9cac10c6a3e 100644
--- a/drivers/regulator/tps6586x-regulator.c
+++ b/drivers/regulator/tps6586x-regulator.c
@@ -21,6 +21,7 @@
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include <linux/mfd/tps6586x.h>
+#include <linux/delay.h>
 
 /* supply control and voltage setting  */
 #define TPS6586X_SUPPLYENA	0x10
@@ -61,8 +62,9 @@ struct tps6586x_regulator {
 	int volt_nbits;
 	int enable_bit[2];
 	int enable_reg[2];
-
 	int *voltages;
+	int delay; /* delay in us for regulator to stabilize */
+	enum tps6586x_type type;
 
 	/* for DVM regulators */
 	int go_reg;
@@ -94,10 +96,6 @@ static int __tps6586x_ldo_set_voltage(struct device *parent,
 	for (val = 0; val < ri->desc.n_voltages; val++) {
 		uV = ri->voltages[val] * 1000;
 
-		/* LDO0 has minimal voltage 1.2 rather than 1.25 */
-		if (ri->desc.id == TPS6586X_ID_LDO_0 && val == 0)
-			uV -= 50 * 1000;
-
 		/* use the first in-range value */
 		if (min_uV <= uV && uV <= max_uV) {
 
@@ -190,11 +188,18 @@ static int tps6586x_regulator_is_enabled(struct regulator_dev *rdev)
 	return !!(reg_val & (1 << ri->enable_bit[0]));
 }
 
+static int tps6586x_regulator_enable_time(struct regulator_dev *rdev)
+{
+	struct tps6586x_regulator *ri = rdev_get_drvdata(rdev);
+
+	return ri->delay;
+}
+
 static struct regulator_ops tps6586x_regulator_ldo_ops = {
 	.list_voltage = tps6586x_ldo_list_voltage,
 	.get_voltage = tps6586x_ldo_get_voltage,
 	.set_voltage = tps6586x_ldo_set_voltage,
-
+	.enable_time = tps6586x_regulator_enable_time,
 	.is_enabled = tps6586x_regulator_is_enabled,
 	.enable = tps6586x_regulator_enable,
 	.disable = tps6586x_regulator_disable,
@@ -204,7 +209,7 @@ static struct regulator_ops tps6586x_regulator_dvm_ops = {
 	.list_voltage = tps6586x_ldo_list_voltage,
 	.get_voltage = tps6586x_ldo_get_voltage,
 	.set_voltage = tps6586x_dvm_set_voltage,
-
+	.enable_time = tps6586x_regulator_enable_time,
 	.is_enabled = tps6586x_regulator_is_enabled,
 	.enable = tps6586x_regulator_enable,
 	.disable = tps6586x_regulator_disable,
@@ -214,6 +219,10 @@ static int tps6586x_ldo_voltages[] = {
 	1250, 1500, 1800, 2500, 2700, 2850, 3100, 3300,
 };
 
+static int tps6586x_ldo0_voltages[] = {
+	1200, 1500, 1800, 2500, 2700, 2850, 3100, 3300,
+};
+
 static int tps6586x_ldo4_voltages[] = {
 	1700, 1725, 1750, 1775, 1800, 1825, 1850, 1875,
 	1900, 1925, 1950, 1975, 2000, 2025, 2050, 2075,
@@ -228,6 +237,13 @@ static int tps6586x_sm2_voltages[] = {
 	4200, 4250, 4300, 4350, 4400, 4450, 4500, 4550,
 };
 
+static int tps6586x_sm2fortythree_voltages[] = {
+	1025, 1050, 1075, 1100, 1125, 1150, 1175, 1200,
+	1225, 1250, 1275, 1300, 1325, 1350, 1375, 1400,
+	1425, 1450, 1475, 1500, 1525, 1550, 1575, 1600,
+	1625, 1650, 1675, 1700, 1725, 1750, 1775, 1800,
+};
+
 static int tps6586x_dvm_voltages[] = {
 	 725,  750,  775,  800,  825,  850,  875,  900,
 	 925,  950,  975, 1000, 1025, 1050, 1075, 1100,
@@ -235,8 +251,8 @@ static int tps6586x_dvm_voltages[] = {
 	1325, 1350, 1375, 1400, 1425, 1450, 1475, 1500,
 };
 
-#define TPS6586X_REGULATOR(_id, vdata, _ops, vreg, shift, nbits,	\
-			   ereg0, ebit0, ereg1, ebit1)			\
+#define TPS6586X_REGULATOR(_id, _type, vdata, _ops, vreg, shift, nbits,	\
+			   ereg0, ebit0, ereg1, ebit1, en_time)		\
 	.desc	= {							\
 		.name	= "REG-" #_id,					\
 		.ops	= &tps6586x_regulator_##_ops,			\
@@ -252,43 +268,63 @@ static int tps6586x_dvm_voltages[] = {
 	.enable_bit[0]	= (ebit0),					\
 	.enable_reg[1]	= TPS6586X_SUPPLY##ereg1,			\
 	.enable_bit[1]	= (ebit1),					\
-	.voltages	= tps6586x_##vdata##_voltages,
+	.voltages	= tps6586x_##vdata##_voltages,			\
+	.delay		= en_time,					\
+	.type		= (_type),
 
 #define TPS6586X_REGULATOR_DVM_GOREG(goreg, gobit)			\
 	.go_reg = TPS6586X_##goreg,					\
 	.go_bit = (gobit),
 
-#define TPS6586X_LDO(_id, vdata, vreg, shift, nbits,			\
-		     ereg0, ebit0, ereg1, ebit1)			\
+#define TPS6586X_LDO(_id, type, vdata, vreg, shift, nbits,		\
+		     ereg0, ebit0, ereg1, ebit1, en_time)		\
 {									\
-	TPS6586X_REGULATOR(_id, vdata, ldo_ops, vreg, shift, nbits,	\
-			   ereg0, ebit0, ereg1, ebit1)			\
+	TPS6586X_REGULATOR(_id, type, vdata, ldo_ops, vreg, shift, 	\
+			   nbits, ereg0, ebit0, ereg1, ebit1, en_time)	\
 }
 
-#define TPS6586X_DVM(_id, vdata, vreg, shift, nbits,			\
-		     ereg0, ebit0, ereg1, ebit1, goreg, gobit)		\
+#define TPS6586X_DVM(_id, type, vdata, vreg, shift, nbits,		\
+		     ereg0, ebit0, ereg1, ebit1, goreg, gobit, en_time)	\
 {									\
-	TPS6586X_REGULATOR(_id, vdata, dvm_ops, vreg, shift, nbits,	\
-			   ereg0, ebit0, ereg1, ebit1)			\
+	TPS6586X_REGULATOR(_id, type, vdata, dvm_ops, vreg, shift,	\
+			   nbits, ereg0, ebit0, ereg1, ebit1, en_time)	\
 	TPS6586X_REGULATOR_DVM_GOREG(goreg, gobit)			\
 }
 
+/* Note: type ANY means universal, search order matters, place ANY last */
 static struct tps6586x_regulator tps6586x_regulator[] = {
-	TPS6586X_LDO(LDO_0, ldo, SUPPLYV1, 5, 3, ENC, 0, END, 0),
-	TPS6586X_LDO(LDO_3, ldo, SUPPLYV4, 0, 3, ENC, 2, END, 2),
-	TPS6586X_LDO(LDO_5, ldo, SUPPLYV6, 0, 3, ENE, 6, ENE, 6),
-	TPS6586X_LDO(LDO_6, ldo, SUPPLYV3, 0, 3, ENC, 4, END, 4),
-	TPS6586X_LDO(LDO_7, ldo, SUPPLYV3, 3, 3, ENC, 5, END, 5),
-	TPS6586X_LDO(LDO_8, ldo, SUPPLYV2, 5, 3, ENC, 6, END, 6),
-	TPS6586X_LDO(LDO_9, ldo, SUPPLYV6, 3, 3, ENE, 7, ENE, 7),
-	TPS6586X_LDO(LDO_RTC, ldo, SUPPLYV4, 3, 3, V4, 7, V4, 7),
-	TPS6586X_LDO(LDO_1, dvm, SUPPLYV1, 0, 5, ENC, 1, END, 1),
-	TPS6586X_LDO(SM_2, sm2, SUPPLYV2, 0, 5, ENC, 7, END, 7),
-
-	TPS6586X_DVM(LDO_2, dvm, LDO2BV1, 0, 5, ENA, 3, ENB, 3, VCC2, 6),
-	TPS6586X_DVM(LDO_4, ldo4, LDO4V1, 0, 5, ENC, 3, END, 3, VCC1, 6),
-	TPS6586X_DVM(SM_0, dvm, SM0V1, 0, 5, ENA, 1, ENB, 1, VCC1, 2),
-	TPS6586X_DVM(SM_1, dvm, SM1V1, 0, 5, ENA, 0, ENB, 0, VCC1, 0),
+	TPS6586X_LDO(LDO_0, TPS6586X_ANY, ldo0, SUPPLYV1, 5, 3, ENC, 0, END, 0,
+		     4000),
+	TPS6586X_LDO(LDO_1, TPS6586X_ANY, dvm, SUPPLYV1, 0, 5, ENC, 1, END, 1,
+		     4000),
+	TPS6586X_DVM(LDO_2, TPS6586X_ANY, dvm, LDO2BV1, 0, 5, ENA, 3, ENB, 3,
+		     VCC2, 6, 3000),
+	TPS6586X_LDO(LDO_3, TPS6586X_ANY, ldo, SUPPLYV4, 0, 3, ENC, 2, END, 2,
+		     3000),
+	TPS6586X_DVM(LDO_4, TPS6586X_ANY, ldo4, LDO4V1, 0, 5, ENC, 3, END, 3,
+		     VCC1, 6, 15000),
+	TPS6586X_LDO(LDO_5, TPS6586X_ANY, ldo, SUPPLYV6, 0, 3, ENE, 6, ENE, 6,
+		     3000),
+	TPS6586X_LDO(LDO_6, TPS6586X_ANY, ldo, SUPPLYV3, 0, 3, ENC, 4, END, 4,
+		     15000),
+	TPS6586X_LDO(LDO_7, TPS6586X_ANY, ldo, SUPPLYV3, 3, 3, ENC, 5, END, 5,
+		     15000),
+	TPS6586X_LDO(LDO_8, TPS6586X_ANY, ldo, SUPPLYV2, 5, 3, ENC, 6, END, 6,
+		     15000),
+	TPS6586X_LDO(LDO_9, TPS6586X_ANY, ldo, SUPPLYV6, 3, 3, ENE, 7, ENE, 7,
+		     3000),
+	TPS6586X_LDO(LDO_RTC, TPS6586X_ANY, ldo, SUPPLYV4, 3, 3, V4, 7, V4, 7,
+		     0),
+	TPS6586X_DVM(SM_0, TPS6586X_ANY, dvm, SM0V1, 0, 5, ENA, 1, ENB, 1,
+		     VCC1, 2, 4000),
+	TPS6586X_DVM(SM_1, TPS6586X_ANY, dvm, SM1V1, 0, 5, ENA, 0, ENB, 0,
+		     VCC1, 0, 4000),
+	TPS6586X_DVM(SM_2, TPS658623, ldo4, SUPPLYV2, 0, 5, ENC, 3, END, 3,
+		     VCC1, 6, 15000),
+	TPS6586X_DVM(SM_2, TPS658643, sm2fortythree, SUPPLYV2, 0, 5, ENC, 3,
+		     END, 3, VCC1, 6, 15000),
+	TPS6586X_LDO(SM_2, TPS6586X_ANY, sm2, SUPPLYV2, 0, 5, ENC, 7, END, 7,
+		     0),
 };
 
 /*
@@ -332,14 +368,74 @@ static inline int tps6586x_regulator_preinit(struct device *parent,
 				 1 << ri->enable_bit[1]);
 }
 
-static inline struct tps6586x_regulator *find_regulator_info(int id)
+static inline int tps6586x_regulator_set_pwm_mode(struct platform_device *pdev)
+{
+	struct device *parent = pdev->dev.parent;
+	struct regulator_init_data *p = pdev->dev.platform_data;
+	struct tps6586x_settings *setting = p->driver_data;
+	int ret = 0;
+	uint8_t mask;
+
+	if (setting == NULL)
+		return 0;
+
+	switch (pdev->id) {
+	case TPS6586X_ID_SM_0:
+		mask = 1 << SM0_PWM_BIT;
+		break;
+	case TPS6586X_ID_SM_1:
+		mask = 1 << SM1_PWM_BIT;
+		break;
+	case TPS6586X_ID_SM_2:
+		mask = 1 << SM2_PWM_BIT;
+		break;
+	default:
+		/* not all regulators have PWM/PFM option */
+		return 0;
+	}
+
+	if (setting->sm_pwm_mode == PWM_ONLY)
+		ret = tps6586x_set_bits(parent, TPS6586X_SMODE1, mask);
+	else if (setting->sm_pwm_mode == AUTO_PWM_PFM)
+		ret = tps6586x_clr_bits(parent, TPS6586X_SMODE1, mask);
+
+	return ret;
+}
+
+static inline int tps6586x_regulator_set_slew_rate(struct platform_device *pdev)
+{
+	struct device *parent = pdev->dev.parent;
+	struct regulator_init_data *p = pdev->dev.platform_data;
+	struct tps6586x_settings *setting = p->driver_data;
+	uint8_t reg;
+
+	if (setting == NULL)
+		return 0;
+
+	/* only SM0 and SM1 can have the slew rate settings */
+	switch (pdev->id) {
+	case TPS6586X_ID_SM_0:
+		reg = TPS6586X_SM0SL;
+		break;
+	case TPS6586X_ID_SM_1:
+		reg = TPS6586X_SM1SL;
+		break;
+	default:
+		return 0;
+	}
+	return tps6586x_write(parent, reg, setting->slew_rate);
+}
+
+static inline struct tps6586x_regulator *find_regulator_info(int id,
+		enum tps6586x_type type)
 {
 	struct tps6586x_regulator *ri;
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(tps6586x_regulator); i++) {
 		ri = &tps6586x_regulator[i];
-		if (ri->desc.id == id)
+		if ((ri->desc.id == id) && ((ri->type == type) ||
+					    (ri->type == TPS6586X_ANY)))
 			return ri;
 	}
 	return NULL;
@@ -354,7 +450,7 @@ static int __devinit tps6586x_regulator_probe(struct platform_device *pdev)
 
 	dev_dbg(&pdev->dev, "Probing reulator %d\n", id);
 
-	ri = find_regulator_info(id);
+	ri = find_regulator_info(id, tps6586x_gettype(pdev->dev.parent));
 	if (ri == NULL) {
 		dev_err(&pdev->dev, "invalid regulator ID specified\n");
 		return -EINVAL;
@@ -374,7 +470,11 @@ static int __devinit tps6586x_regulator_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, rdev);
 
-	return 0;
+	err = tps6586x_regulator_set_slew_rate(pdev);
+	if (err)
+		return err;
+
+	return tps6586x_regulator_set_pwm_mode(pdev);
 }
 
 static int __devexit tps6586x_regulator_remove(struct platform_device *pdev)
diff --git a/drivers/regulator/tps65910-regulator.c b/drivers/regulator/tps65910-regulator.c
index 66d2d60b436a..a2fef3880065 100644
--- a/drivers/regulator/tps65910-regulator.c
+++ b/drivers/regulator/tps65910-regulator.c
@@ -25,31 +25,11 @@
 #include <linux/gpio.h>
 #include <linux/mfd/tps65910.h>
 
-#define TPS65910_REG_VRTC		0
-#define TPS65910_REG_VIO		1
-#define TPS65910_REG_VDD1		2
-#define TPS65910_REG_VDD2		3
-#define TPS65910_REG_VDD3		4
-#define TPS65910_REG_VDIG1		5
-#define TPS65910_REG_VDIG2		6
-#define TPS65910_REG_VPLL		7
-#define TPS65910_REG_VDAC		8
-#define TPS65910_REG_VAUX1		9
-#define TPS65910_REG_VAUX2		10
-#define TPS65910_REG_VAUX33		11
-#define TPS65910_REG_VMMC		12
-
-#define TPS65911_REG_VDDCTRL		4
-#define TPS65911_REG_LDO1		5
-#define TPS65911_REG_LDO2		6
-#define TPS65911_REG_LDO3		7
-#define TPS65911_REG_LDO4		8
-#define TPS65911_REG_LDO5		9
-#define TPS65911_REG_LDO6		10
-#define TPS65911_REG_LDO7		11
-#define TPS65911_REG_LDO8		12
-
 #define TPS65910_SUPPLY_STATE_ENABLED	0x1
+#define EXT_SLEEP_CONTROL (TPS65910_SLEEP_CONTROL_EXT_INPUT_EN1 |	\
+			TPS65910_SLEEP_CONTROL_EXT_INPUT_EN2 |		\
+			TPS65910_SLEEP_CONTROL_EXT_INPUT_EN3 |		\
+			TPS65911_SLEEP_CONTROL_EXT_INPUT_SLEEP)
 
 /* supported VIO voltages in milivolts */
 static const u16 VIO_VSEL_table[] = {
@@ -107,161 +87,235 @@ struct tps_info {
 	const char *name;
 	unsigned min_uV;
 	unsigned max_uV;
-	u8 table_len;
-	const u16 *table;
+	u8 n_voltages;
+	const u16 *voltage_table;
+	int enable_time_us;
 };
 
 static struct tps_info tps65910_regs[] = {
 	{
 		.name = "VRTC",
+		.enable_time_us = 2200,
 	},
 	{
 		.name = "VIO",
 		.min_uV = 1500000,
 		.max_uV = 3300000,
-		.table_len = ARRAY_SIZE(VIO_VSEL_table),
-		.table = VIO_VSEL_table,
+		.n_voltages = ARRAY_SIZE(VIO_VSEL_table),
+		.voltage_table = VIO_VSEL_table,
+		.enable_time_us = 350,
 	},
 	{
 		.name = "VDD1",
 		.min_uV = 600000,
 		.max_uV = 4500000,
+		.enable_time_us = 350,
 	},
 	{
 		.name = "VDD2",
 		.min_uV = 600000,
 		.max_uV = 4500000,
+		.enable_time_us = 350,
 	},
 	{
 		.name = "VDD3",
 		.min_uV = 5000000,
 		.max_uV = 5000000,
-		.table_len = ARRAY_SIZE(VDD3_VSEL_table),
-		.table = VDD3_VSEL_table,
+		.n_voltages = ARRAY_SIZE(VDD3_VSEL_table),
+		.voltage_table = VDD3_VSEL_table,
+		.enable_time_us = 200,
 	},
 	{
 		.name = "VDIG1",
 		.min_uV = 1200000,
 		.max_uV = 2700000,
-		.table_len = ARRAY_SIZE(VDIG1_VSEL_table),
-		.table = VDIG1_VSEL_table,
+		.n_voltages = ARRAY_SIZE(VDIG1_VSEL_table),
+		.voltage_table = VDIG1_VSEL_table,
+		.enable_time_us = 100,
 	},
 	{
 		.name = "VDIG2",
 		.min_uV = 1000000,
 		.max_uV = 1800000,
-		.table_len = ARRAY_SIZE(VDIG2_VSEL_table),
-		.table = VDIG2_VSEL_table,
+		.n_voltages = ARRAY_SIZE(VDIG2_VSEL_table),
+		.voltage_table = VDIG2_VSEL_table,
+		.enable_time_us = 100,
 	},
 	{
 		.name = "VPLL",
 		.min_uV = 1000000,
 		.max_uV = 2500000,
-		.table_len = ARRAY_SIZE(VPLL_VSEL_table),
-		.table = VPLL_VSEL_table,
+		.n_voltages = ARRAY_SIZE(VPLL_VSEL_table),
+		.voltage_table = VPLL_VSEL_table,
+		.enable_time_us = 100,
 	},
 	{
 		.name = "VDAC",
 		.min_uV = 1800000,
 		.max_uV = 2850000,
-		.table_len = ARRAY_SIZE(VDAC_VSEL_table),
-		.table = VDAC_VSEL_table,
+		.n_voltages = ARRAY_SIZE(VDAC_VSEL_table),
+		.voltage_table = VDAC_VSEL_table,
+		.enable_time_us = 100,
 	},
 	{
 		.name = "VAUX1",
 		.min_uV = 1800000,
 		.max_uV = 2850000,
-		.table_len = ARRAY_SIZE(VAUX1_VSEL_table),
-		.table = VAUX1_VSEL_table,
+		.n_voltages = ARRAY_SIZE(VAUX1_VSEL_table),
+		.voltage_table = VAUX1_VSEL_table,
+		.enable_time_us = 100,
 	},
 	{
 		.name = "VAUX2",
 		.min_uV = 1800000,
 		.max_uV = 3300000,
-		.table_len = ARRAY_SIZE(VAUX2_VSEL_table),
-		.table = VAUX2_VSEL_table,
+		.n_voltages = ARRAY_SIZE(VAUX2_VSEL_table),
+		.voltage_table = VAUX2_VSEL_table,
+		.enable_time_us = 100,
 	},
 	{
 		.name = "VAUX33",
 		.min_uV = 1800000,
 		.max_uV = 3300000,
-		.table_len = ARRAY_SIZE(VAUX33_VSEL_table),
-		.table = VAUX33_VSEL_table,
+		.n_voltages = ARRAY_SIZE(VAUX33_VSEL_table),
+		.voltage_table = VAUX33_VSEL_table,
+		.enable_time_us = 100,
 	},
 	{
 		.name = "VMMC",
 		.min_uV = 1800000,
 		.max_uV = 3300000,
-		.table_len = ARRAY_SIZE(VMMC_VSEL_table),
-		.table = VMMC_VSEL_table,
+		.n_voltages = ARRAY_SIZE(VMMC_VSEL_table),
+		.voltage_table = VMMC_VSEL_table,
+		.enable_time_us = 100,
 	},
 };
 
 static struct tps_info tps65911_regs[] = {
 	{
+		.name = "VRTC",
+		.enable_time_us = 2200,
+	},
+	{
 		.name = "VIO",
 		.min_uV = 1500000,
 		.max_uV = 3300000,
-		.table_len = ARRAY_SIZE(VIO_VSEL_table),
-		.table = VIO_VSEL_table,
+		.n_voltages = ARRAY_SIZE(VIO_VSEL_table),
+		.voltage_table = VIO_VSEL_table,
+		.enable_time_us = 350,
 	},
 	{
 		.name = "VDD1",
 		.min_uV = 600000,
 		.max_uV = 4500000,
+		.n_voltages = 73,
+		.enable_time_us = 350,
 	},
 	{
 		.name = "VDD2",
 		.min_uV = 600000,
 		.max_uV = 4500000,
+		.n_voltages = 73,
+		.enable_time_us = 350,
 	},
 	{
 		.name = "VDDCTRL",
 		.min_uV = 600000,
 		.max_uV = 1400000,
+		.n_voltages = 65,
+		.enable_time_us = 900,
 	},
 	{
 		.name = "LDO1",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
+		.n_voltages = 47,
+		.enable_time_us = 420,
 	},
 	{
 		.name = "LDO2",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
+		.n_voltages = 47,
+		.enable_time_us = 420,
 	},
 	{
 		.name = "LDO3",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
+		.n_voltages = 24,
+		.enable_time_us = 230,
 	},
 	{
 		.name = "LDO4",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
+		.n_voltages = 47,
+		.enable_time_us = 230,
 	},
 	{
 		.name = "LDO5",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
+		.n_voltages = 24,
+		.enable_time_us = 230,
 	},
 	{
 		.name = "LDO6",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
+		.n_voltages = 24,
+		.enable_time_us = 230,
 	},
 	{
 		.name = "LDO7",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
+		.n_voltages = 24,
+		.enable_time_us = 230,
 	},
 	{
 		.name = "LDO8",
 		.min_uV = 1000000,
 		.max_uV = 3300000,
+		.n_voltages = 24,
+		.enable_time_us = 230,
 	},
 };
 
+#define EXT_CONTROL_REG_BITS(id, regs_offs, bits) (((regs_offs) << 8) | (bits))
+static unsigned int tps65910_ext_sleep_control[] = {
+	0,
+	EXT_CONTROL_REG_BITS(VIO,    1, 0),
+	EXT_CONTROL_REG_BITS(VDD1,   1, 1),
+	EXT_CONTROL_REG_BITS(VDD2,   1, 2),
+	EXT_CONTROL_REG_BITS(VDD3,   1, 3),
+	EXT_CONTROL_REG_BITS(VDIG1,  0, 1),
+	EXT_CONTROL_REG_BITS(VDIG2,  0, 2),
+	EXT_CONTROL_REG_BITS(VPLL,   0, 6),
+	EXT_CONTROL_REG_BITS(VDAC,   0, 7),
+	EXT_CONTROL_REG_BITS(VAUX1,  0, 3),
+	EXT_CONTROL_REG_BITS(VAUX2,  0, 4),
+	EXT_CONTROL_REG_BITS(VAUX33, 0, 5),
+	EXT_CONTROL_REG_BITS(VMMC,   0, 0),
+};
+
+static unsigned int tps65911_ext_sleep_control[] = {
+	0,
+	EXT_CONTROL_REG_BITS(VIO,     1, 0),
+	EXT_CONTROL_REG_BITS(VDD1,    1, 1),
+	EXT_CONTROL_REG_BITS(VDD2,    1, 2),
+	EXT_CONTROL_REG_BITS(VDDCTRL, 1, 3),
+	EXT_CONTROL_REG_BITS(LDO1,    0, 1),
+	EXT_CONTROL_REG_BITS(LDO2,    0, 2),
+	EXT_CONTROL_REG_BITS(LDO3,    0, 7),
+	EXT_CONTROL_REG_BITS(LDO4,    0, 6),
+	EXT_CONTROL_REG_BITS(LDO5,    0, 3),
+	EXT_CONTROL_REG_BITS(LDO6,    0, 0),
+	EXT_CONTROL_REG_BITS(LDO7,    0, 5),
+	EXT_CONTROL_REG_BITS(LDO8,    0, 4),
+};
+
 struct tps65910_reg {
 	struct regulator_desc *desc;
 	struct tps65910 *mfd;
@@ -271,25 +325,22 @@ struct tps65910_reg {
 	int num_regulators;
 	int mode;
 	int  (*get_ctrl_reg)(int);
+	unsigned int *ext_sleep_control;
+	unsigned int board_ext_control[TPS65910_NUM_REGS];
 };
 
 static inline int tps65910_read(struct tps65910_reg *pmic, u8 reg)
 {
-	u8 val;
+	unsigned int val;
 	int err;
 
-	err = pmic->mfd->read(pmic->mfd, reg, 1, &val);
+	err = tps65910_reg_read(pmic->mfd, reg, &val);
 	if (err)
 		return err;
 
 	return val;
 }
 
-static inline int tps65910_write(struct tps65910_reg *pmic, u8 reg, u8 val)
-{
-	return pmic->mfd->write(pmic->mfd, reg, 1, &val);
-}
-
 static int tps65910_modify_bits(struct tps65910_reg *pmic, u8 reg,
 					u8 set_mask, u8 clear_mask)
 {
@@ -306,7 +357,7 @@ static int tps65910_modify_bits(struct tps65910_reg *pmic, u8 reg,
 
 	data &= ~clear_mask;
 	data |= set_mask;
-	err = tps65910_write(pmic, reg, data);
+	err = tps65910_reg_write(pmic->mfd, reg, data);
 	if (err)
 		dev_err(pmic->mfd->dev, "Write for reg 0x%x failed\n", reg);
 
@@ -315,7 +366,7 @@ out:
 	return err;
 }
 
-static int tps65910_reg_read(struct tps65910_reg *pmic, u8 reg)
+static int tps65910_reg_read_locked(struct tps65910_reg *pmic, u8 reg)
 {
 	int data;
 
@@ -329,13 +380,13 @@ static int tps65910_reg_read(struct tps65910_reg *pmic, u8 reg)
 	return data;
 }
 
-static int tps65910_reg_write(struct tps65910_reg *pmic, u8 reg, u8 val)
+static int tps65910_reg_write_locked(struct tps65910_reg *pmic, u8 reg, u8 val)
 {
 	int err;
 
 	mutex_lock(&pmic->mutex);
 
-	err = tps65910_write(pmic, reg, val);
+	err = tps65910_reg_write(pmic->mfd, reg, val);
 	if (err < 0)
 		dev_err(pmic->mfd->dev, "Write for reg 0x%x failed\n", reg);
 
@@ -420,7 +471,7 @@ static int tps65910_is_enabled(struct regulator_dev *dev)
 	if (reg < 0)
 		return reg;
 
-	value = tps65910_reg_read(pmic, reg);
+	value = tps65910_reg_read_locked(pmic, reg);
 	if (value < 0)
 		return value;
 
@@ -437,7 +488,7 @@ static int tps65910_enable(struct regulator_dev *dev)
 	if (reg < 0)
 		return reg;
 
-	return tps65910_set_bits(mfd, reg, TPS65910_SUPPLY_STATE_ENABLED);
+	return tps65910_reg_set_bits(mfd, reg, TPS65910_SUPPLY_STATE_ENABLED);
 }
 
 static int tps65910_disable(struct regulator_dev *dev)
@@ -450,9 +501,15 @@ static int tps65910_disable(struct regulator_dev *dev)
 	if (reg < 0)
 		return reg;
 
-	return tps65910_clear_bits(mfd, reg, TPS65910_SUPPLY_STATE_ENABLED);
+	return tps65910_reg_clear_bits(mfd, reg, TPS65910_SUPPLY_STATE_ENABLED);
 }
 
+static int tps65910_enable_time(struct regulator_dev *dev)
+{
+	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
+	int id = rdev_get_id(dev);
+	return pmic->info[id]->enable_time_us;
+}
 
 static int tps65910_set_mode(struct regulator_dev *dev, unsigned int mode)
 {
@@ -470,9 +527,9 @@ static int tps65910_set_mode(struct regulator_dev *dev, unsigned int mode)
 							LDO_ST_MODE_BIT);
 	case REGULATOR_MODE_IDLE:
 		value = LDO_ST_ON_BIT | LDO_ST_MODE_BIT;
-		return tps65910_set_bits(mfd, reg, value);
+		return tps65910_reg_set_bits(mfd, reg, value);
 	case REGULATOR_MODE_STANDBY:
-		return tps65910_clear_bits(mfd, reg, LDO_ST_ON_BIT);
+		return tps65910_reg_clear_bits(mfd, reg, LDO_ST_ON_BIT);
 	}
 
 	return -EINVAL;
@@ -487,11 +544,11 @@ static unsigned int tps65910_get_mode(struct regulator_dev *dev)
 	if (reg < 0)
 		return reg;
 
-	value = tps65910_reg_read(pmic, reg);
+	value = tps65910_reg_read_locked(pmic, reg);
 	if (value < 0)
 		return value;
 
-	if (value & LDO_ST_ON_BIT)
+	if (!(value & LDO_ST_ON_BIT))
 		return REGULATOR_MODE_STANDBY;
 	else if (value & LDO_ST_MODE_BIT)
 		return REGULATOR_MODE_IDLE;
@@ -499,36 +556,36 @@ static unsigned int tps65910_get_mode(struct regulator_dev *dev)
 		return REGULATOR_MODE_NORMAL;
 }
 
-static int tps65910_get_voltage_dcdc(struct regulator_dev *dev)
+static int tps65910_get_voltage_dcdc_sel(struct regulator_dev *dev)
 {
 	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
-	int id = rdev_get_id(dev), voltage = 0;
+	int id = rdev_get_id(dev);
 	int opvsel = 0, srvsel = 0, vselmax = 0, mult = 0, sr = 0;
 
 	switch (id) {
 	case TPS65910_REG_VDD1:
-		opvsel = tps65910_reg_read(pmic, TPS65910_VDD1_OP);
-		mult = tps65910_reg_read(pmic, TPS65910_VDD1);
+		opvsel = tps65910_reg_read_locked(pmic, TPS65910_VDD1_OP);
+		mult = tps65910_reg_read_locked(pmic, TPS65910_VDD1);
 		mult = (mult & VDD1_VGAIN_SEL_MASK) >> VDD1_VGAIN_SEL_SHIFT;
-		srvsel = tps65910_reg_read(pmic, TPS65910_VDD1_SR);
+		srvsel = tps65910_reg_read_locked(pmic, TPS65910_VDD1_SR);
 		sr = opvsel & VDD1_OP_CMD_MASK;
 		opvsel &= VDD1_OP_SEL_MASK;
 		srvsel &= VDD1_SR_SEL_MASK;
 		vselmax = 75;
 		break;
 	case TPS65910_REG_VDD2:
-		opvsel = tps65910_reg_read(pmic, TPS65910_VDD2_OP);
-		mult = tps65910_reg_read(pmic, TPS65910_VDD2);
+		opvsel = tps65910_reg_read_locked(pmic, TPS65910_VDD2_OP);
+		mult = tps65910_reg_read_locked(pmic, TPS65910_VDD2);
 		mult = (mult & VDD2_VGAIN_SEL_MASK) >> VDD2_VGAIN_SEL_SHIFT;
-		srvsel = tps65910_reg_read(pmic, TPS65910_VDD2_SR);
+		srvsel = tps65910_reg_read_locked(pmic, TPS65910_VDD2_SR);
 		sr = opvsel & VDD2_OP_CMD_MASK;
 		opvsel &= VDD2_OP_SEL_MASK;
 		srvsel &= VDD2_SR_SEL_MASK;
 		vselmax = 75;
 		break;
 	case TPS65911_REG_VDDCTRL:
-		opvsel = tps65910_reg_read(pmic, TPS65911_VDDCTRL_OP);
-		srvsel = tps65910_reg_read(pmic, TPS65911_VDDCTRL_SR);
+		opvsel = tps65910_reg_read_locked(pmic, TPS65911_VDDCTRL_OP);
+		srvsel = tps65910_reg_read_locked(pmic, TPS65911_VDDCTRL_SR);
 		sr = opvsel & VDDCTRL_OP_CMD_MASK;
 		opvsel &= VDDCTRL_OP_SEL_MASK;
 		srvsel &= VDDCTRL_SR_SEL_MASK;
@@ -546,9 +603,7 @@ static int tps65910_get_voltage_dcdc(struct regulator_dev *dev)
 			srvsel = 3;
 		if (srvsel > vselmax)
 			srvsel = vselmax;
-		srvsel -= 3;
-
-		voltage = (srvsel * VDD1_2_OFFSET + VDD1_2_MIN_VOLT) * 100;
+		return srvsel - 3;
 	} else {
 
 		/* normalise to valid range*/
@@ -556,14 +611,9 @@ static int tps65910_get_voltage_dcdc(struct regulator_dev *dev)
 			opvsel = 3;
 		if (opvsel > vselmax)
 			opvsel = vselmax;
-		opvsel -= 3;
-
-		voltage = (opvsel * VDD1_2_OFFSET + VDD1_2_MIN_VOLT) * 100;
+		return opvsel - 3;
 	}
-
-	voltage *= mult;
-
-	return voltage;
+	return -EINVAL;
 }
 
 static int tps65910_get_voltage(struct regulator_dev *dev)
@@ -575,7 +625,7 @@ static int tps65910_get_voltage(struct regulator_dev *dev)
 	if (reg < 0)
 		return reg;
 
-	value = tps65910_reg_read(pmic, reg);
+	value = tps65910_reg_read_locked(pmic, reg);
 	if (value < 0)
 		return value;
 
@@ -596,7 +646,7 @@ static int tps65910_get_voltage(struct regulator_dev *dev)
 		return -EINVAL;
 	}
 
-	voltage = pmic->info[id]->table[value] * 1000;
+	voltage = pmic->info[id]->voltage_table[value] * 1000;
 
 	return voltage;
 }
@@ -614,7 +664,7 @@ static int tps65911_get_voltage(struct regulator_dev *dev)
 
 	reg = pmic->get_ctrl_reg(id);
 
-	value = tps65910_reg_read(pmic, reg);
+	value = tps65910_reg_read_locked(pmic, reg);
 
 	switch (id) {
 	case TPS65911_REG_LDO1:
@@ -646,8 +696,9 @@ static int tps65911_get_voltage(struct regulator_dev *dev)
 		step_mv = 100;
 		break;
 	case TPS65910_REG_VIO:
-		return pmic->info[id]->table[value] * 1000;
-		break;
+		value &= LDO_SEL_MASK;
+		value >>= LDO_SEL_SHIFT;
+		return pmic->info[id]->voltage_table[value] * 1000;
 	default:
 		return -EINVAL;
 	}
@@ -655,8 +706,8 @@ static int tps65911_get_voltage(struct regulator_dev *dev)
 	return (LDO_MIN_VOLT + value * step_mv) * 1000;
 }
 
-static int tps65910_set_voltage_dcdc(struct regulator_dev *dev,
-				unsigned selector)
+static int tps65910_set_voltage_dcdc_sel(struct regulator_dev *dev,
+					 unsigned selector)
 {
 	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
 	int id = rdev_get_id(dev), vsel;
@@ -664,36 +715,37 @@ static int tps65910_set_voltage_dcdc(struct regulator_dev *dev,
 
 	switch (id) {
 	case TPS65910_REG_VDD1:
-		dcdc_mult = (selector / VDD1_2_NUM_VOLTS) + 1;
+		dcdc_mult = (selector / VDD1_2_NUM_VOLT_FINE) + 1;
 		if (dcdc_mult == 1)
 			dcdc_mult--;
-		vsel = (selector % VDD1_2_NUM_VOLTS) + 3;
+		vsel = (selector % VDD1_2_NUM_VOLT_FINE) + 3;
 
 		tps65910_modify_bits(pmic, TPS65910_VDD1,
 				(dcdc_mult << VDD1_VGAIN_SEL_SHIFT),
 						VDD1_VGAIN_SEL_MASK);
-		tps65910_reg_write(pmic, TPS65910_VDD1_OP, vsel);
+		tps65910_reg_write_locked(pmic, TPS65910_VDD1_OP, vsel);
 		break;
 	case TPS65910_REG_VDD2:
-		dcdc_mult = (selector / VDD1_2_NUM_VOLTS) + 1;
+		dcdc_mult = (selector / VDD1_2_NUM_VOLT_FINE) + 1;
 		if (dcdc_mult == 1)
 			dcdc_mult--;
-		vsel = (selector % VDD1_2_NUM_VOLTS) + 3;
+		vsel = (selector % VDD1_2_NUM_VOLT_FINE) + 3;
 
 		tps65910_modify_bits(pmic, TPS65910_VDD2,
 				(dcdc_mult << VDD2_VGAIN_SEL_SHIFT),
 						VDD1_VGAIN_SEL_MASK);
-		tps65910_reg_write(pmic, TPS65910_VDD2_OP, vsel);
+		tps65910_reg_write_locked(pmic, TPS65910_VDD2_OP, vsel);
 		break;
 	case TPS65911_REG_VDDCTRL:
-		vsel = selector;
-		tps65910_reg_write(pmic, TPS65911_VDDCTRL_OP, vsel);
+		vsel = selector + 3;
+		tps65910_reg_write_locked(pmic, TPS65911_VDDCTRL_OP, vsel);
 	}
 
 	return 0;
 }
 
-static int tps65910_set_voltage(struct regulator_dev *dev, unsigned selector)
+static int tps65910_set_voltage_sel(struct regulator_dev *dev,
+				    unsigned selector)
 {
 	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
 	int reg, id = rdev_get_id(dev);
@@ -719,7 +771,8 @@ static int tps65910_set_voltage(struct regulator_dev *dev, unsigned selector)
 	return -EINVAL;
 }
 
-static int tps65911_set_voltage(struct regulator_dev *dev, unsigned selector)
+static int tps65911_set_voltage_sel(struct regulator_dev *dev,
+				    unsigned selector)
 {
 	struct tps65910_reg *pmic = rdev_get_drvdata(dev);
 	int reg, id = rdev_get_id(dev);
@@ -739,9 +792,11 @@ static int tps65911_set_voltage(struct regulator_dev *dev, unsigned selector)
 	case TPS65911_REG_LDO6:
 	case TPS65911_REG_LDO7:
 	case TPS65911_REG_LDO8:
-	case TPS65910_REG_VIO:
 		return tps65910_modify_bits(pmic, reg,
 				(selector << LDO_SEL_SHIFT), LDO3_SEL_MASK);
+	case TPS65910_REG_VIO:
+		return tps65910_modify_bits(pmic, reg,
+				(selector << LDO_SEL_SHIFT), LDO_SEL_MASK);
 	}
 
 	return -EINVAL;
@@ -756,9 +811,9 @@ static int tps65910_list_voltage_dcdc(struct regulator_dev *dev,
 	switch (id) {
 	case TPS65910_REG_VDD1:
 	case TPS65910_REG_VDD2:
-		mult = (selector / VDD1_2_NUM_VOLTS) + 1;
+		mult = (selector / VDD1_2_NUM_VOLT_FINE) + 1;
 		volt = VDD1_2_MIN_VOLT +
-				(selector % VDD1_2_NUM_VOLTS) * VDD1_2_OFFSET;
+				(selector % VDD1_2_NUM_VOLT_FINE) * VDD1_2_OFFSET;
 		break;
 	case TPS65911_REG_VDDCTRL:
 		volt = VDDCTRL_MIN_VOLT + (selector * VDDCTRL_OFFSET);
@@ -780,10 +835,10 @@ static int tps65910_list_voltage(struct regulator_dev *dev,
 	if (id < TPS65910_REG_VIO || id > TPS65910_REG_VMMC)
 		return -EINVAL;
 
-	if (selector >= pmic->info[id]->table_len)
+	if (selector >= pmic->info[id]->n_voltages)
 		return -EINVAL;
 	else
-		voltage = pmic->info[id]->table[selector] * 1000;
+		voltage = pmic->info[id]->voltage_table[selector] * 1000;
 
 	return voltage;
 }
@@ -819,7 +874,7 @@ static int tps65911_list_voltage(struct regulator_dev *dev, unsigned selector)
 		step_mv = 100;
 		break;
 	case TPS65910_REG_VIO:
-		return pmic->info[id]->table[selector] * 1000;
+		return pmic->info[id]->voltage_table[selector] * 1000;
 	default:
 		return -EINVAL;
 	}
@@ -827,15 +882,42 @@ static int tps65911_list_voltage(struct regulator_dev *dev, unsigned selector)
 	return (LDO_MIN_VOLT + selector * step_mv) * 1000;
 }
 
+static int tps65910_set_voltage_dcdc_time_sel(struct regulator_dev *dev,
+		unsigned int old_selector, unsigned int new_selector)
+{
+	int id = rdev_get_id(dev);
+	int old_volt, new_volt;
+
+	old_volt = tps65910_list_voltage_dcdc(dev, old_selector);
+	if (old_volt < 0)
+		return old_volt;
+
+	new_volt = tps65910_list_voltage_dcdc(dev, new_selector);
+	if (new_volt < 0)
+		return new_volt;
+
+	/* VDD1 and VDD2 are 12.5mV/us, VDDCTRL is 100mV/20us */
+	switch (id) {
+	case TPS65910_REG_VDD1:
+	case TPS65910_REG_VDD2:
+		return DIV_ROUND_UP(abs(old_volt - new_volt), 12500);
+	case TPS65911_REG_VDDCTRL:
+		return DIV_ROUND_UP(abs(old_volt - new_volt), 5000);
+	}
+	return -EINVAL;
+}
+
 /* Regulator ops (except VRTC) */
 static struct regulator_ops tps65910_ops_dcdc = {
 	.is_enabled		= tps65910_is_enabled,
 	.enable			= tps65910_enable,
 	.disable		= tps65910_disable,
+	.enable_time		= tps65910_enable_time,
 	.set_mode		= tps65910_set_mode,
 	.get_mode		= tps65910_get_mode,
-	.get_voltage		= tps65910_get_voltage_dcdc,
-	.set_voltage_sel	= tps65910_set_voltage_dcdc,
+	.get_voltage_sel	= tps65910_get_voltage_dcdc_sel,
+	.set_voltage_sel	= tps65910_set_voltage_dcdc_sel,
+	.set_voltage_time_sel	= tps65910_set_voltage_dcdc_time_sel,
 	.list_voltage		= tps65910_list_voltage_dcdc,
 };
 
@@ -843,6 +925,7 @@ static struct regulator_ops tps65910_ops_vdd3 = {
 	.is_enabled		= tps65910_is_enabled,
 	.enable			= tps65910_enable,
 	.disable		= tps65910_disable,
+	.enable_time		= tps65910_enable_time,
 	.set_mode		= tps65910_set_mode,
 	.get_mode		= tps65910_get_mode,
 	.get_voltage		= tps65910_get_voltage_vdd3,
@@ -853,10 +936,11 @@ static struct regulator_ops tps65910_ops = {
 	.is_enabled		= tps65910_is_enabled,
 	.enable			= tps65910_enable,
 	.disable		= tps65910_disable,
+	.enable_time		= tps65910_enable_time,
 	.set_mode		= tps65910_set_mode,
 	.get_mode		= tps65910_get_mode,
 	.get_voltage		= tps65910_get_voltage,
-	.set_voltage_sel	= tps65910_set_voltage,
+	.set_voltage_sel	= tps65910_set_voltage_sel,
 	.list_voltage		= tps65910_list_voltage,
 };
 
@@ -864,13 +948,148 @@ static struct regulator_ops tps65911_ops = {
 	.is_enabled		= tps65910_is_enabled,
 	.enable			= tps65910_enable,
 	.disable		= tps65910_disable,
+	.enable_time		= tps65910_enable_time,
 	.set_mode		= tps65910_set_mode,
 	.get_mode		= tps65910_get_mode,
 	.get_voltage		= tps65911_get_voltage,
-	.set_voltage_sel	= tps65911_set_voltage,
+	.set_voltage_sel	= tps65911_set_voltage_sel,
 	.list_voltage		= tps65911_list_voltage,
 };
 
+static int tps65910_set_ext_sleep_config(struct tps65910_reg *pmic,
+		int id, int ext_sleep_config)
+{
+	struct tps65910 *mfd = pmic->mfd;
+	u8 regoffs = (pmic->ext_sleep_control[id] >> 8) & 0xFF;
+	u8 bit_pos = (1 << pmic->ext_sleep_control[id] & 0xFF);
+	int ret;
+
+	/*
+	 * Regulator can not be control from multiple external input EN1, EN2
+	 * and EN3 together.
+	 */
+	if (ext_sleep_config & EXT_SLEEP_CONTROL) {
+		int en_count;
+		en_count = ((ext_sleep_config &
+				TPS65910_SLEEP_CONTROL_EXT_INPUT_EN1) != 0);
+		en_count += ((ext_sleep_config &
+				TPS65910_SLEEP_CONTROL_EXT_INPUT_EN2) != 0);
+		en_count += ((ext_sleep_config &
+				TPS65910_SLEEP_CONTROL_EXT_INPUT_EN3) != 0);
+		en_count += ((ext_sleep_config &
+				TPS65911_SLEEP_CONTROL_EXT_INPUT_SLEEP) != 0);
+		if (en_count > 1) {
+			dev_err(mfd->dev,
+				"External sleep control flag is not proper\n");
+			return -EINVAL;
+		}
+	}
+
+	pmic->board_ext_control[id] = ext_sleep_config;
+
+	/* External EN1 control */
+	if (ext_sleep_config & TPS65910_SLEEP_CONTROL_EXT_INPUT_EN1)
+		ret = tps65910_reg_set_bits(mfd,
+				TPS65910_EN1_LDO_ASS + regoffs, bit_pos);
+	else
+		ret = tps65910_reg_clear_bits(mfd,
+				TPS65910_EN1_LDO_ASS + regoffs, bit_pos);
+	if (ret < 0) {
+		dev_err(mfd->dev,
+			"Error in configuring external control EN1\n");
+		return ret;
+	}
+
+	/* External EN2 control */
+	if (ext_sleep_config & TPS65910_SLEEP_CONTROL_EXT_INPUT_EN2)
+		ret = tps65910_reg_set_bits(mfd,
+				TPS65910_EN2_LDO_ASS + regoffs, bit_pos);
+	else
+		ret = tps65910_reg_clear_bits(mfd,
+				TPS65910_EN2_LDO_ASS + regoffs, bit_pos);
+	if (ret < 0) {
+		dev_err(mfd->dev,
+			"Error in configuring external control EN2\n");
+		return ret;
+	}
+
+	/* External EN3 control for TPS65910 LDO only */
+	if ((tps65910_chip_id(mfd) == TPS65910) &&
+			(id >= TPS65910_REG_VDIG1)) {
+		if (ext_sleep_config & TPS65910_SLEEP_CONTROL_EXT_INPUT_EN3)
+			ret = tps65910_reg_set_bits(mfd,
+				TPS65910_EN3_LDO_ASS + regoffs, bit_pos);
+		else
+			ret = tps65910_reg_clear_bits(mfd,
+				TPS65910_EN3_LDO_ASS + regoffs, bit_pos);
+		if (ret < 0) {
+			dev_err(mfd->dev,
+				"Error in configuring external control EN3\n");
+			return ret;
+		}
+	}
+
+	/* Return if no external control is selected */
+	if (!(ext_sleep_config & EXT_SLEEP_CONTROL)) {
+		/* Clear all sleep controls */
+		ret = tps65910_reg_clear_bits(mfd,
+			TPS65910_SLEEP_KEEP_LDO_ON + regoffs, bit_pos);
+		if (!ret)
+			ret = tps65910_reg_clear_bits(mfd,
+				TPS65910_SLEEP_SET_LDO_OFF + regoffs, bit_pos);
+		if (ret < 0)
+			dev_err(mfd->dev,
+				"Error in configuring SLEEP register\n");
+		return ret;
+	}
+
+	/*
+	 * For regulator that has separate operational and sleep register make
+	 * sure that operational is used and clear sleep register to turn
+	 * regulator off when external control is inactive
+	 */
+	if ((id == TPS65910_REG_VDD1) ||
+		(id == TPS65910_REG_VDD2) ||
+			((id == TPS65911_REG_VDDCTRL) &&
+				(tps65910_chip_id(mfd) == TPS65911))) {
+		int op_reg_add = pmic->get_ctrl_reg(id) + 1;
+		int sr_reg_add = pmic->get_ctrl_reg(id) + 2;
+		int opvsel = tps65910_reg_read_locked(pmic, op_reg_add);
+		int srvsel = tps65910_reg_read_locked(pmic, sr_reg_add);
+		if (opvsel & VDD1_OP_CMD_MASK) {
+			u8 reg_val = srvsel & VDD1_OP_SEL_MASK;
+			ret = tps65910_reg_write_locked(pmic, op_reg_add,
+							reg_val);
+			if (ret < 0) {
+				dev_err(mfd->dev,
+					"Error in configuring op register\n");
+				return ret;
+			}
+		}
+		ret = tps65910_reg_write_locked(pmic, sr_reg_add, 0);
+		if (ret < 0) {
+			dev_err(mfd->dev, "Error in settting sr register\n");
+			return ret;
+		}
+	}
+
+	ret = tps65910_reg_clear_bits(mfd,
+			TPS65910_SLEEP_KEEP_LDO_ON + regoffs, bit_pos);
+	if (!ret) {
+		if (ext_sleep_config & TPS65911_SLEEP_CONTROL_EXT_INPUT_SLEEP)
+			ret = tps65910_reg_set_bits(mfd,
+				TPS65910_SLEEP_SET_LDO_OFF + regoffs, bit_pos);
+		else
+			ret = tps65910_reg_clear_bits(mfd,
+				TPS65910_SLEEP_SET_LDO_OFF + regoffs, bit_pos);
+	}
+	if (ret < 0)
+		dev_err(mfd->dev,
+			"Error in configuring SLEEP register\n");
+
+	return ret;
+}
+
 static __devinit int tps65910_probe(struct platform_device *pdev)
 {
 	struct tps65910 *tps65910 = dev_get_drvdata(pdev->dev.parent);
@@ -885,8 +1104,6 @@ static __devinit int tps65910_probe(struct platform_device *pdev)
 	if (!pmic_plat_data)
 		return -EINVAL;
 
-	reg_data = pmic_plat_data->tps65910_pmic_init_data;
-
 	pmic = kzalloc(sizeof(*pmic), GFP_KERNEL);
 	if (!pmic)
 		return -ENOMEM;
@@ -896,18 +1113,20 @@ static __devinit int tps65910_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, pmic);
 
 	/* Give control of all register to control port */
-	tps65910_set_bits(pmic->mfd, TPS65910_DEVCTRL,
+	tps65910_reg_set_bits(pmic->mfd, TPS65910_DEVCTRL,
 				DEVCTRL_SR_CTL_I2C_SEL_MASK);
 
 	switch(tps65910_chip_id(tps65910)) {
 	case TPS65910:
 		pmic->get_ctrl_reg = &tps65910_get_ctrl_register;
 		pmic->num_regulators = ARRAY_SIZE(tps65910_regs);
+		pmic->ext_sleep_control = tps65910_ext_sleep_control;
 		info = tps65910_regs;
 		break;
 	case TPS65911:
 		pmic->get_ctrl_reg = &tps65911_get_ctrl_register;
 		pmic->num_regulators = ARRAY_SIZE(tps65911_regs);
+		pmic->ext_sleep_control = tps65911_ext_sleep_control;
 		info = tps65911_regs;
 		break;
 	default:
@@ -937,16 +1156,27 @@ static __devinit int tps65910_probe(struct platform_device *pdev)
 		goto err_free_info;
 	}
 
-	for (i = 0; i < pmic->num_regulators; i++, info++, reg_data++) {
+	for (i = 0; i < pmic->num_regulators && i < TPS65910_NUM_REGS;
+			i++, info++) {
+
+		reg_data = pmic_plat_data->tps65910_pmic_init_data[i];
+
+		/* Regulator API handles empty constraints but not NULL
+		 * constraints */
+		if (!reg_data)
+			continue;
+
 		/* Register the regulators */
 		pmic->info[i] = info;
 
 		pmic->desc[i].name = info->name;
 		pmic->desc[i].id = i;
-		pmic->desc[i].n_voltages = info->table_len;
+		pmic->desc[i].n_voltages = info->n_voltages;
 
 		if (i == TPS65910_REG_VDD1 || i == TPS65910_REG_VDD2) {
 			pmic->desc[i].ops = &tps65910_ops_dcdc;
+			pmic->desc[i].n_voltages = VDD1_2_NUM_VOLT_FINE *
+							VDD1_2_NUM_VOLT_COARSE;
 		} else if (i == TPS65910_REG_VDD3) {
 			if (tps65910_chip_id(tps65910) == TPS65910)
 				pmic->desc[i].ops = &tps65910_ops_vdd3;
@@ -959,6 +1189,16 @@ static __devinit int tps65910_probe(struct platform_device *pdev)
 				pmic->desc[i].ops = &tps65911_ops;
 		}
 
+		err = tps65910_set_ext_sleep_config(pmic, i,
+				pmic_plat_data->regulator_ext_sleep_control[i]);
+		/*
+		 * Failing on regulator for configuring externally control
+		 * is not a serious issue, just throw warning.
+		 */
+		if (err < 0)
+			dev_warn(tps65910->dev,
+				"Failed to initialise ext control config\n");
+
 		pmic->desc[i].type = REGULATOR_VOLTAGE;
 		pmic->desc[i].owner = THIS_MODULE;
 
@@ -1005,6 +1245,36 @@ static int __devexit tps65910_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static void tps65910_shutdown(struct platform_device *pdev)
+{
+	struct tps65910_reg *pmic = platform_get_drvdata(pdev);
+	int i;
+
+	/*
+	 * Before bootloader jumps to kernel, it makes sure that required
+	 * external control signals are in desired state so that given rails
+	 * can be configure accordingly.
+	 * If rails are configured to be controlled from external control
+	 * then before shutting down/rebooting the system, the external
+	 * control configuration need to be remove from the rails so that
+	 * its output will be available as per register programming even
+	 * if external controls are removed. This is require when the POR
+	 * value of the control signals are not in active state and before
+	 * bootloader initializes it, the system requires the rail output
+	 * to be active for booting.
+	 */
+	for (i = 0; i < pmic->num_regulators; i++) {
+		int err;
+		if (!pmic->rdev[i])
+			continue;
+
+		err = tps65910_set_ext_sleep_config(pmic, i, 0);
+		if (err < 0)
+			dev_err(&pdev->dev,
+				"Error in clearing external control\n");
+	}
+}
+
 static struct platform_driver tps65910_driver = {
 	.driver = {
 		.name = "tps65910-pmic",
@@ -1012,6 +1282,7 @@ static struct platform_driver tps65910_driver = {
 	},
 	.probe = tps65910_probe,
 	.remove = __devexit_p(tps65910_remove),
+	.shutdown = tps65910_shutdown,
 };
 
 static int __init tps65910_init(void)
@@ -1027,6 +1298,6 @@ static void __exit tps65910_cleanup(void)
 module_exit(tps65910_cleanup);
 
 MODULE_AUTHOR("Graeme Gregory <gg@slimlogic.co.uk>");
-MODULE_DESCRIPTION("TPS6507x voltage regulator driver");
+MODULE_DESCRIPTION("TPS65910/TPS65911 voltage regulator driver");
 MODULE_LICENSE("GPL v2");
 MODULE_ALIAS("platform:tps65910-pmic");
diff --git a/drivers/regulator/tps65912-regulator.c b/drivers/regulator/tps65912-regulator.c
index 3a9313e00fac..3bf29427a28d 100644
--- a/drivers/regulator/tps65912-regulator.c
+++ b/drivers/regulator/tps65912-regulator.c
@@ -568,8 +568,46 @@ static int tps65912_get_voltage_dcdc(struct regulator_dev *dev)
 	return voltage;
 }
 
-static int tps65912_set_voltage_dcdc(struct regulator_dev *dev,
-						unsigned selector)
+static int tps65912_get_voltage_dcdc(struct regulator_dev *dev)
+{
+	struct tps65912_reg *pmic = rdev_get_drvdata(dev);
+	struct tps65912 *mfd = pmic->mfd;
+	int id = rdev_get_id(dev);
+	int opvsel = 0, avsel = 0, sr, vsel;
+
+	switch (id) {
+	case TPS65912_REG_DCDC1:
+		opvsel = tps65912_reg_read(mfd, TPS65912_DCDC1_OP);
+		avsel = tps65912_reg_read(mfd, TPS65912_DCDC1_AVS);
+		break;
+	case TPS65912_REG_DCDC2:
+		opvsel = tps65912_reg_read(mfd, TPS65912_DCDC2_OP);
+		avsel = tps65912_reg_read(mfd, TPS65912_DCDC2_AVS);
+		break;
+	case TPS65912_REG_DCDC3:
+		opvsel = tps65912_reg_read(mfd, TPS65912_DCDC3_OP);
+		avsel = tps65912_reg_read(mfd, TPS65912_DCDC3_AVS);
+		break;
+	case TPS65912_REG_DCDC4:
+		opvsel = tps65912_reg_read(mfd, TPS65912_DCDC4_OP);
+		avsel = tps65912_reg_read(mfd, TPS65912_DCDC4_AVS);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	sr = (opvsel & OP_SELREG_MASK) >> OP_SELREG_SHIFT;
+	if (sr)
+		vsel = avsel;
+	else
+		vsel = opvsel;
+	vsel &= 0x3F;
+
+	return tps65912_list_voltage_dcdc(dev, vsel);
+}
+
+static int tps65912_set_voltage_dcdc_sel(struct regulator_dev *dev,
+					 unsigned selector)
 {
 	struct tps65912_reg *pmic = rdev_get_drvdata(dev);
 	struct tps65912 *mfd = pmic->mfd;
@@ -598,8 +636,8 @@ static int tps65912_get_voltage_ldo(struct regulator_dev *dev)
 	return tps65912_vsel_to_uv_ldo(vsel);
 }
 
-static int tps65912_set_voltage_ldo(struct regulator_dev *dev,
-						unsigned selector)
+static int tps65912_set_voltage_ldo_sel(struct regulator_dev *dev,
+					unsigned selector)
 {
 	struct tps65912_reg *pmic = rdev_get_drvdata(dev);
 	struct tps65912 *mfd = pmic->mfd;
@@ -674,7 +712,7 @@ static struct regulator_ops tps65912_ops_dcdc = {
 	.set_mode = tps65912_set_mode,
 	.get_mode = tps65912_get_mode,
 	.get_voltage = tps65912_get_voltage_dcdc,
-	.set_voltage_sel = tps65912_set_voltage_dcdc,
+	.set_voltage_sel = tps65912_set_voltage_dcdc_sel,
 	.list_voltage = tps65912_list_voltage_dcdc,
 };
 
@@ -684,7 +722,7 @@ static struct regulator_ops tps65912_ops_ldo = {
 	.enable = tps65912_reg_enable,
 	.disable = tps65912_reg_disable,
 	.get_voltage = tps65912_get_voltage_ldo,
-	.set_voltage_sel = tps65912_set_voltage_ldo,
+	.set_voltage_sel = tps65912_set_voltage_ldo_sel,
 	.list_voltage = tps65912_list_voltage_ldo,
 };
 
diff --git a/drivers/regulator/tps6591x-regulator.c b/drivers/regulator/tps6591x-regulator.c
new file mode 100644
index 000000000000..5336f3d82576
--- /dev/null
+++ b/drivers/regulator/tps6591x-regulator.c
@@ -0,0 +1,955 @@
+/*
+ * driver/regulator/tps6591x-regulator.c
+ *
+ * Regulator driver for TI TPS6591x PMIC family
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/tps6591x-regulator.h>
+#include <linux/mfd/tps6591x.h>
+
+/* supply control and voltage setting  */
+#define TPS6591X_VIO_ADD		0x20
+#define TPS6591X_VDD1_ADD		0x21
+#define TPS6591X_VDD1_OP_ADD		0x22
+#define TPS6591X_VDD1_SR_ADD		0x23
+#define TPS6591X_VDD2_ADD		0x24
+#define TPS6591X_VDD2_OP_ADD		0x25
+#define TPS6591X_VDD2_SR_ADD		0x26
+#define TPS6591X_VDDCTRL_ADD		0x27
+#define TPS6591X_VDDCTRL_OP_ADD		0x28
+#define TPS6591X_VDDCTRL_SR_ADD		0x29
+#define TPS6591X_LDO1_ADD		0x30
+#define TPS6591X_LDO2_ADD		0x31
+#define TPS6591X_LDO3_ADD		0x37
+#define TPS6591X_LDO4_ADD		0x36
+#define TPS6591X_LDO5_ADD		0x32
+#define TPS6591X_LDO6_ADD		0x35
+#define TPS6591X_LDO7_ADD		0x34
+#define TPS6591X_LDO8_ADD		0x33
+#define TPS6591X_SLEEP_SET_LDO_OFF_ADD	0x43
+#define TPS6591X_SLEEP_SET_RES_OFF_ADD	0x44
+#define TPS6591X_EN1_LDO_ADD		0x45
+#define TPS6591X_EN1_SMPS_ADD		0x46
+#define TPS6591X_EN2_LDO_ADD		0x47
+#define TPS6591X_EN2_SMPS_ADD		0x48
+#define TPS6591X_INVALID_ADD		0xFF
+
+#define EN1_EN2_OFFSET			2
+
+struct tps6591x_register_info {
+	unsigned char addr;
+	unsigned char nbits;
+	unsigned char shift_bits;
+	uint8_t cache_val;
+};
+
+enum {
+	supply_type_none = 0x0,
+	supply_type_single_reg,
+	supply_type_sr_op_reg
+};
+
+struct tps6591x_regulator {
+	struct regulator_desc desc;
+	int supply_type;
+
+	struct tps6591x_register_info supply_reg;
+	struct tps6591x_register_info op_reg;
+	struct tps6591x_register_info sr_reg;
+	struct tps6591x_register_info en1_reg;
+	struct tps6591x_register_info slp_off_reg;
+
+	int *voltages;
+
+	int enable_delay; /* delay in us for regulator to stabilize */
+	enum tps6591x_ext_control ectrl;
+	int current_volt_uv;
+
+	/* Time (micro sec) taken for 1uV change */
+	int voltage_change_uv_per_us;
+	unsigned int config_flags;
+};
+
+static inline struct device *to_tps6591x_dev(struct regulator_dev *rdev)
+{
+	return rdev_get_dev(rdev)->parent->parent;
+}
+
+static int tps6591x_regulator_enable_time(struct regulator_dev *rdev)
+{
+	struct tps6591x_regulator *ri = rdev_get_drvdata(rdev);
+
+	return ri->enable_delay;
+}
+
+static int __tps6591x_ext_control_set(struct device *parent,
+				      struct tps6591x_regulator *ri,
+				      enum tps6591x_ext_control ectrl)
+{
+	int ret;
+	uint8_t mask, reg_val, addr, offset;
+	struct tps6591x_register_info *ext_reg;
+
+	/* For regulator that has separate operational and sleep register make
+	   sure that operational is used and clear sleep register to turn
+	   regulator off when external control is inactive */
+	if (ri->supply_type == supply_type_sr_op_reg) {
+		reg_val = ri->op_reg.cache_val;
+		if (reg_val & 0x80) {	/* boot has used sr - switch to op */
+			reg_val = ri->sr_reg.cache_val;
+			mask = ((1 << ri->sr_reg.nbits) - 1)
+				<< ri->sr_reg.shift_bits;
+			reg_val &= mask;
+			ret = tps6591x_write(parent, ri->op_reg.addr, reg_val);
+			if (ret)
+				return ret;
+			ri->op_reg.cache_val = reg_val;
+		}
+		ret = tps6591x_write(parent, ri->sr_reg.addr, 0);
+		if (ret)
+			return ret;
+		ri->sr_reg.cache_val = 0;
+	}
+
+	offset = 0;
+	switch (ectrl) {
+	case EXT_CTRL_EN2:
+		offset = EN1_EN2_OFFSET;
+		/* fall through to EXT_CTRL_EN1 */
+	case EXT_CTRL_EN1:
+		ext_reg = &(ri->en1_reg);
+		break;
+	case EXT_CTRL_SLEEP_OFF:
+		ext_reg = &(ri->slp_off_reg);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	addr = ext_reg->addr + offset;
+	mask = ((1 << ext_reg->nbits) - 1) << ext_reg->shift_bits;
+
+	return tps6591x_update(parent, addr, mask, mask);
+}
+
+static void wait_for_voltage_change(struct tps6591x_regulator *ri, int uV)
+{
+	int change_uv;
+	int change_us;
+
+	change_uv = abs(uV - ri->current_volt_uv);
+	change_us = change_uv/ri->voltage_change_uv_per_us + 1;
+	if (change_us >= 1000) {
+		mdelay(change_us/1000);
+		change_us -= (change_us/1000);
+	}
+	if (change_us)
+		udelay(change_us);
+	ri->current_volt_uv = uV;
+}
+
+static int __tps6591x_vio_set_voltage(struct device *parent,
+				      struct tps6591x_regulator *ri,
+				      int min_uV, int max_uV,
+				      unsigned *selector)
+{
+	int uV;
+	uint8_t mask;
+	uint8_t val;
+	int ret;
+	uint8_t reg_val;
+
+	for (val = 0; val < ri->desc.n_voltages; val++) {
+		uV = ri->voltages[val] * 1000;
+
+		/* use the first in-range value */
+		if (min_uV <= uV && uV <= max_uV) {
+			if (selector)
+				*selector = val;
+
+			reg_val = ri->supply_reg.cache_val;
+			val <<= ri->supply_reg.shift_bits;
+
+			mask = ((1 << ri->supply_reg.nbits) - 1) <<
+					ri->supply_reg.shift_bits;
+			reg_val = (reg_val & ~mask) | (val & mask);
+
+			ret = tps6591x_write(parent, ri->supply_reg.addr,
+					reg_val);
+			if (ret >= 0) {
+				wait_for_voltage_change(ri, uV);
+				ri->supply_reg.cache_val = reg_val;
+			}
+			return ret;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int tps6591x_vio_set_voltage(struct regulator_dev *rdev,
+				    int min_uV, int max_uV,
+				    unsigned *selector)
+{
+	struct tps6591x_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps6591x_dev(rdev);
+
+	return __tps6591x_vio_set_voltage(parent, ri, min_uV, max_uV,
+					  selector);
+}
+
+static int tps6591x_vio_get_voltage(struct regulator_dev *rdev)
+{
+	struct tps6591x_regulator *ri = rdev_get_drvdata(rdev);
+	uint8_t val, mask;
+
+	val = ri->supply_reg.cache_val;
+
+	mask = ((1 << ri->supply_reg.nbits) - 1) << ri->supply_reg.shift_bits;
+	val = (val & mask) >> ri->supply_reg.shift_bits;
+
+	if (val >= ri->desc.n_voltages)
+		BUG();
+
+	return ri->voltages[val] * 1000;
+}
+
+
+static int tps6591x_ldo_list_voltage(struct regulator_dev *rdev,
+				     unsigned selector)
+{
+	struct tps6591x_regulator *info = rdev_get_drvdata(rdev);
+
+	return info->voltages[selector] * 1000;
+}
+
+static int __tps6591x_ldo1_set_voltage(struct device *parent,
+				       struct tps6591x_regulator *ri,
+				       int min_uV, int max_uV,
+				       unsigned *selector)
+{
+	int val, uV;
+	uint8_t mask;
+	uint8_t reg_val;
+	int ret;
+
+	for (val = 0; val < ri->desc.n_voltages; val++) {
+		uV = ri->voltages[val] * 1000;
+
+		/* use the first in-range value */
+		if (min_uV <= uV && uV <= max_uV) {
+			if (selector)
+				*selector = val;
+			reg_val = ri->supply_reg.cache_val;
+			val += 4;
+			val <<= ri->supply_reg.shift_bits;
+			mask = ((1 << ri->supply_reg.nbits) - 1) <<
+					ri->supply_reg.shift_bits;
+
+			reg_val = (reg_val & ~mask) | (val & mask);
+			ret = tps6591x_write(parent, ri->supply_reg.addr,
+					reg_val);
+			if (ret >= 0) {
+				wait_for_voltage_change(ri, uV);
+				ri->supply_reg.cache_val = reg_val;
+			}
+			return ret;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int tps6591x_ldo1_set_voltage(struct regulator_dev *rdev,
+				     int min_uV, int max_uV,
+				     unsigned *selector)
+{
+	struct tps6591x_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps6591x_dev(rdev);
+
+	return __tps6591x_ldo1_set_voltage(parent, ri, min_uV, max_uV,
+					   selector);
+}
+
+static int tps6591x_ldo1_get_voltage(struct regulator_dev *rdev)
+{
+	struct tps6591x_regulator *ri = rdev_get_drvdata(rdev);
+	uint8_t val, mask;
+
+	val = ri->supply_reg.cache_val;
+	mask = ((1 << ri->supply_reg.nbits) - 1) << ri->supply_reg.shift_bits;
+	val = (val & mask) >> ri->supply_reg.shift_bits;
+
+	if (val < 4)
+		return 1000 * 1000;
+	else if (val > 0x32)
+		return 3300 * 1000;
+	else
+		val -= 4;
+	if (val >= ri->desc.n_voltages)
+		BUG();
+
+	return ri->voltages[val] * 1000;
+}
+
+static int __tps6591x_ldo3_set_voltage(struct device *parent,
+				       struct tps6591x_regulator *ri,
+				       int min_uV, int max_uV,
+				       unsigned *selector)
+{
+	int val, uV;
+	uint8_t mask;
+	int ret;
+	uint8_t reg_val;
+
+	for (val = 0; val < ri->desc.n_voltages; val++) {
+		uV = ri->voltages[val] * 1000;
+
+		/* use the first in-range value */
+		if (min_uV <= uV && uV <= max_uV) {
+			if (selector)
+				*selector = val;
+			reg_val = ri->supply_reg.cache_val;
+			val += 2;
+			val <<= ri->supply_reg.shift_bits;
+			mask = ((1 << ri->supply_reg.nbits) - 1) <<
+						ri->supply_reg.shift_bits;
+
+			reg_val = (reg_val & ~mask) | (val & mask);
+
+			ret = tps6591x_write(parent, ri->supply_reg.addr,
+					reg_val);
+			if (ret >= 0) {
+				wait_for_voltage_change(ri, uV);
+				ri->supply_reg.cache_val = reg_val;
+			}
+			return ret;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int tps6591x_ldo3_set_voltage(struct regulator_dev *rdev,
+				     int min_uV, int max_uV,
+				     unsigned *selector)
+{
+	struct tps6591x_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps6591x_dev(rdev);
+
+	return __tps6591x_ldo3_set_voltage(parent, ri, min_uV, max_uV,
+					   selector);
+}
+
+static int tps6591x_ldo3_get_voltage(struct regulator_dev *rdev)
+{
+	struct tps6591x_regulator *ri = rdev_get_drvdata(rdev);
+	uint8_t val, mask;
+
+	val = ri->supply_reg.cache_val;
+	mask = ((1 << ri->supply_reg.nbits) - 1) << ri->supply_reg.shift_bits;
+	val = (val & mask) >> ri->supply_reg.shift_bits;
+
+	if (val < 2)
+		return 1000 * 1000;
+	else if (val > 0x19)
+		return 3300 * 1000;
+	else
+		val -= 2;
+	if (val >= ri->desc.n_voltages)
+		BUG();
+
+	return ri->voltages[val] * 1000;
+}
+
+static int __tps6591x_vdd_set_voltage(struct device *parent,
+				      struct tps6591x_regulator *ri,
+				      int min_uV, int max_uV,
+				      unsigned *selector)
+{
+	int val, uV, ret;
+	uint8_t mask;
+	uint8_t op_reg_val;
+	uint8_t sr_reg_val;
+
+	for (val = 0; val < ri->desc.n_voltages; val++) {
+		uV = ri->voltages[val] * 1000;
+
+		/* use the first in-range value */
+		if (min_uV <= uV && uV <= max_uV) {
+			if (selector)
+				*selector = val;
+			op_reg_val = ri->op_reg.cache_val;
+			val += 3;
+			if (op_reg_val & 0x80) {
+				sr_reg_val = ri->sr_reg.cache_val;
+				val <<= ri->sr_reg.shift_bits;
+				mask = ((1 << ri->sr_reg.nbits) - 1)
+					<< ri->sr_reg.shift_bits;
+				sr_reg_val = (sr_reg_val & ~mask) |
+							(val & mask);
+				ret = tps6591x_write(parent,
+					ri->sr_reg.addr, sr_reg_val);
+				if (!ret)
+					ri->sr_reg.cache_val = sr_reg_val;
+			} else {
+				val <<= ri->op_reg.shift_bits;
+				mask = ((1 << ri->op_reg.nbits) - 1)
+					<< ri->op_reg.shift_bits;
+				op_reg_val = (op_reg_val & ~mask) |
+							(val & mask);
+				ret = tps6591x_write(parent,
+					ri->op_reg.addr, op_reg_val);
+				if (!ret)
+					ri->op_reg.cache_val = op_reg_val;
+			}
+			if (ret >= 0)
+				wait_for_voltage_change(ri, uV);
+			return ret;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int tps6591x_vdd_set_voltage(struct regulator_dev *rdev,
+				    int min_uV, int max_uV,
+				    unsigned *selector)
+{
+	struct tps6591x_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps6591x_dev(rdev);
+
+	return __tps6591x_vdd_set_voltage(parent, ri, min_uV, max_uV,
+					  selector);
+}
+
+static int tps6591x_vdd_get_voltage(struct regulator_dev *rdev)
+{
+	struct tps6591x_regulator *ri = rdev_get_drvdata(rdev);
+	uint8_t op_val, sr_val, val;
+
+	op_val = ri->op_reg.cache_val;
+	sr_val = ri->sr_reg.cache_val;
+
+	val = (op_val & 0x80) ? (sr_val & 0x7F) : (op_val & 0x7F);
+
+	if (!val)
+		return 0;
+	else if (val < 0x3)
+		return 600 * 1000;
+	else if (val > 0x4B)
+		return 1500 * 1000;
+	else
+		val -= 3;
+
+	if (val >= ri->desc.n_voltages)
+		BUG();
+
+	return ri->voltages[val] * 1000;
+}
+
+static int tps6591x_regulator_enable(struct regulator_dev *rdev)
+{
+	struct tps6591x_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps6591x_dev(rdev);
+	uint8_t reg_val;
+	int ret;
+
+	reg_val = ri->supply_reg.cache_val;
+	reg_val |= 0x1;
+
+	ret = tps6591x_write(parent, ri->supply_reg.addr, reg_val);
+	if (!ret)
+		ri->supply_reg.cache_val = reg_val;
+	return ret;
+}
+
+static int tps6591x_regulator_disable(struct regulator_dev *rdev)
+{
+	struct tps6591x_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps6591x_dev(rdev);
+	uint8_t reg_val;
+	int ret;
+
+	reg_val = ri->supply_reg.cache_val;
+	reg_val &= ~0x1;
+	ret = tps6591x_write(parent, ri->supply_reg.addr, reg_val);
+	if (!ret)
+		ri->supply_reg.cache_val = reg_val;
+	return ret;
+}
+
+static int tps6591x_regulator_is_enabled(struct regulator_dev *rdev)
+{
+	struct tps6591x_regulator *ri = rdev_get_drvdata(rdev);
+	uint8_t reg_val;
+
+	reg_val = ri->supply_reg.cache_val;
+	reg_val &= 0x1;
+	return reg_val & 0x1;
+}
+
+static struct regulator_ops tps6591x_regulator_vio_ops = {
+	.list_voltage = tps6591x_ldo_list_voltage,
+	.get_voltage = tps6591x_vio_get_voltage,
+	.set_voltage = tps6591x_vio_set_voltage,
+
+	.enable_time = tps6591x_regulator_enable_time,
+	.is_enabled = tps6591x_regulator_is_enabled,
+	.enable = tps6591x_regulator_enable,
+	.disable = tps6591x_regulator_disable,
+};
+
+static struct regulator_ops tps6591x_regulator_ldo1_ops = {
+	.list_voltage = tps6591x_ldo_list_voltage,
+	.get_voltage = tps6591x_ldo1_get_voltage,
+	.set_voltage = tps6591x_ldo1_set_voltage,
+
+	.enable_time = tps6591x_regulator_enable_time,
+	.is_enabled = tps6591x_regulator_is_enabled,
+	.enable = tps6591x_regulator_enable,
+	.disable = tps6591x_regulator_disable,
+};
+
+static struct regulator_ops tps6591x_regulator_ldo3_ops = {
+	.list_voltage = tps6591x_ldo_list_voltage,
+	.get_voltage = tps6591x_ldo3_get_voltage,
+	.set_voltage = tps6591x_ldo3_set_voltage,
+
+	.enable_time = tps6591x_regulator_enable_time,
+	.is_enabled = tps6591x_regulator_is_enabled,
+	.enable = tps6591x_regulator_enable,
+	.disable = tps6591x_regulator_disable,
+};
+
+static struct regulator_ops tps6591x_regulator_vdd_ops = {
+	.list_voltage = tps6591x_ldo_list_voltage,
+	.get_voltage = tps6591x_vdd_get_voltage,
+	.set_voltage = tps6591x_vdd_set_voltage,
+
+	.enable_time = tps6591x_regulator_enable_time,
+	.is_enabled = tps6591x_regulator_is_enabled,
+	.enable = tps6591x_regulator_enable,
+	.disable = tps6591x_regulator_disable,
+};
+
+static int tps6591x_vio_voltages[] = {
+	1500, 1800, 2500, 3300,
+};
+
+/* SEL[7:2]=000100:1000mV --> 110010:3300mV */
+static int tps6591x_ldo124_voltages[] = {
+	1000, 1050, 1100, 1150, 1200, 1250, 1300, 1350, 1400, 1450,
+	1500, 1550, 1600, 1650, 1700, 1750, 1800, 1850, 1900, 1950,
+	2000, 2050, 2100, 2150, 2200, 2250, 2300, 2350, 2400, 2450,
+	2500, 2550, 2600, 2650, 2700, 2750, 2800, 2850, 2900, 2950,
+	3000, 3050, 3100, 3150, 3200, 3250, 3300,
+};
+
+/* SEL[6:2]=00010:1000mv --> 11001:3300mV */
+static int tps6591x_ldo35678_voltages[] = {
+	1000, 1100, 1200, 1300, 1400, 1500, 1600, 1700, 1800, 1900,
+	2000, 2100, 2200, 2300, 2400, 2500, 2600, 2700, 2800, 2900,
+	3000, 3100, 3200, 3300,
+};
+
+static int tps6591x_vdd_voltages[] = {
+	600, 612, 625, 637, 650, 662, 675, 687, 700, 712, 725, 737,
+	750, 762, 775, 787, 800, 812, 825, 837, 850, 862, 875, 887,
+	900, 912, 925, 937, 950, 962, 975, 987, 1000, 1012, 1025,
+	1037, 1050, 1062, 1075, 1087, 1100, 1112, 1125, 1137, 1150,
+	1162, 1175, 1187, 1200, 1212, 1225, 1237, 1250, 1262, 1275,
+	1287, 1300, 1312, 1325, 1337, 1350, 1362, 1375, 1387, 1400,
+	1412, 1425, 1437, 1450, 1462, 1475, 1487, 1500,
+};
+
+static int tps6591x_vddctrl_voltages[] = {
+	600, 612, 625, 637, 650, 662, 675, 687, 700, 712, 725, 737,
+	750, 762, 775, 787, 800, 812, 825, 837, 850, 862, 875, 887,
+	900, 912, 925, 937, 950, 962, 975, 987, 1000, 1012, 1025,
+	1037, 1050, 1062, 1075, 1087, 1100, 1112, 1125, 1137, 1150,
+	1162, 1175, 1187, 1200, 1212, 1225, 1237, 1250, 1262, 1275,
+	1287, 1300, 1312, 1325, 1337, 1350, 1362, 1375, 1387, 1400,
+};
+
+#define TPS6591X_REGULATOR(_id, vdata, _ops, s_addr, s_nbits, s_shift,	\
+			s_type, op_addr, op_nbits, op_shift, sr_addr,	\
+			sr_nbits, sr_shift, en1_addr, en1_shift,	\
+			slp_off_addr, slp_off_shift, en_time,		\
+			change_rate)					\
+	.desc	= {							\
+		.name	= tps6591x_rails(_id),				\
+		.ops	= &tps6591x_regulator_##_ops,			\
+		.type	= REGULATOR_VOLTAGE,				\
+		.id	= TPS6591X_ID_##_id,				\
+		.n_voltages = ARRAY_SIZE(tps6591x_##vdata##_voltages),	\
+		.owner	= THIS_MODULE,					\
+	},								\
+	.supply_type	= supply_type_##s_type,				\
+	.supply_reg	= {						\
+		.addr	= TPS6591X_##s_addr##_ADD,			\
+		.nbits	= s_nbits,					\
+		.shift_bits = s_shift,					\
+	},								\
+	.op_reg		= {						\
+		.addr	= TPS6591X_##op_addr##_ADD,			\
+		.nbits	= op_nbits,					\
+		.shift_bits = op_shift,					\
+	},								\
+	.sr_reg		= {						\
+		.addr	= TPS6591X_##sr_addr##_ADD,			\
+		.nbits	= sr_nbits,					\
+		.shift_bits = sr_shift,					\
+	},								\
+	.en1_reg	= {						\
+		.addr	= TPS6591X_##en1_addr##_ADD,			\
+		.nbits	= 1,						\
+		.shift_bits = en1_shift,				\
+	},								\
+	.slp_off_reg	= {						\
+		.addr	= TPS6591X_SLEEP_SET_##slp_off_addr##_ADD,	\
+		.nbits	= 1,						\
+		.shift_bits = slp_off_shift,				\
+	},								\
+	.voltages	= tps6591x_##vdata##_voltages,			\
+	.enable_delay		= en_time,				\
+	.voltage_change_uv_per_us = change_rate,
+
+#define TPS6591X_VIO(_id, vdata, s_addr, s_nbits, s_shift, s_type,	\
+			en1_shift, slp_off_shift, en_time)		\
+{									\
+	TPS6591X_REGULATOR(_id, vdata, vio_ops, s_addr, s_nbits,	\
+			s_shift, s_type, INVALID, 0, 0,	INVALID, 0, 0,	\
+			EN1_SMPS, en1_shift, RES_OFF, slp_off_shift,	\
+			en_time, 10000)					\
+}
+
+#define TPS6591X_LDO1(_id, vdata, s_addr, s_nbits, s_shift, s_type,	\
+			en1_shift, slp_off_shift, en_time)		\
+{									\
+	TPS6591X_REGULATOR(_id, vdata, ldo1_ops, s_addr, s_nbits,	\
+			s_shift, s_type, INVALID, 0, 0, INVALID, 0, 0,	\
+			EN1_LDO, en1_shift, LDO_OFF, slp_off_shift,	\
+			en_time, 6000)					\
+}
+
+#define TPS6591X_LDO3(_id, vdata, s_addr, s_nbits, s_shift, s_type,	\
+			en1_shift, slp_off_shift, en_time)		\
+{									\
+	TPS6591X_REGULATOR(_id, vdata, ldo3_ops, s_addr, s_nbits,	\
+			s_shift, s_type, INVALID, 0, 0, INVALID, 0, 0,	\
+			EN1_LDO, en1_shift, LDO_OFF, slp_off_shift,	\
+			en_time, 11000)					\
+}
+
+#define TPS6591X_VDD(_id, vdata, s_addr, s_nbits, s_shift, s_type,	\
+			op_addr, op_nbits, op_shift, sr_addr, sr_nbits,	\
+			sr_shift, en1_shift, slp_off_shift, en_time)	\
+{									\
+	TPS6591X_REGULATOR(_id, vdata, vdd_ops, s_addr, s_nbits,	\
+			s_shift, s_type, op_addr, op_nbits, op_shift,	\
+			sr_addr, sr_nbits, sr_shift, EN1_SMPS,		\
+			en1_shift, RES_OFF, slp_off_shift, en_time,	\
+			5000)						\
+}
+
+static struct tps6591x_regulator tps6591x_regulator[] = {
+	TPS6591X_VIO(VIO, vio, VIO, 2, 2, single_reg, 0, 0, 350),
+	TPS6591X_LDO1(LDO_1, ldo124, LDO1, 6, 2, single_reg, 1, 1, 420),
+	TPS6591X_LDO1(LDO_2, ldo124, LDO2, 6, 2, single_reg, 2, 2, 420),
+	TPS6591X_LDO3(LDO_3, ldo35678, LDO3, 5, 2, single_reg, 7, 7, 230),
+	TPS6591X_LDO1(LDO_4, ldo124, LDO4, 6, 2, single_reg, 6, 6, 230),
+	TPS6591X_LDO3(LDO_5, ldo35678, LDO5, 5, 2, single_reg, 3, 3, 230),
+	TPS6591X_LDO3(LDO_6, ldo35678, LDO6, 5, 2, single_reg, 0, 0, 230),
+	TPS6591X_LDO3(LDO_7, ldo35678, LDO7, 5, 2, single_reg, 5, 5, 230),
+	TPS6591X_LDO3(LDO_8, ldo35678, LDO8, 5, 2, single_reg, 4, 4, 230),
+	TPS6591X_VDD(VDD_1, vdd, VDD1, 2, 0, sr_op_reg, VDD1_OP,
+		7, 0, VDD1_SR, 7, 0, 1, 1, 350),
+	TPS6591X_VDD(VDD_2, vdd, VDD2, 2, 0, sr_op_reg, VDD2_OP,
+		7, 0, VDD2_SR, 7, 0, 2, 2, 350),
+	TPS6591X_VDD(VDDCTRL, vddctrl, VDDCTRL, 2, 0, sr_op_reg,
+		VDDCTRL_OP, 7, 0, VDDCTRL_SR, 7, 0, 3, 3, 900),
+};
+
+static inline int tps6591x_regulator_preinit(struct device *parent,
+		struct tps6591x_regulator *ri,
+		struct tps6591x_regulator_platform_data *tps6591x_pdata)
+{
+	int ret;
+	uint8_t reg_val;
+
+	if (tps6591x_pdata->ectrl != EXT_CTRL_NONE) {
+		ret = __tps6591x_ext_control_set(
+			parent, ri, tps6591x_pdata->ectrl);
+		if (ret < 0) {
+			pr_err("Not able to configure external control %d"
+			       " for rail %d err %d\n", tps6591x_pdata->ectrl,
+			       ri->desc.id, ret);
+			return ret;
+		}
+	}
+
+	if (!tps6591x_pdata->init_apply)
+		return 0;
+
+	if (tps6591x_pdata->init_uV >= 0) {
+		switch (ri->desc.id) {
+		case TPS6591X_ID_VIO:
+			ret = __tps6591x_vio_set_voltage(parent, ri,
+					tps6591x_pdata->init_uV,
+					tps6591x_pdata->init_uV, 0);
+			break;
+
+		case TPS6591X_ID_LDO_1:
+		case TPS6591X_ID_LDO_2:
+		case TPS6591X_ID_LDO_4:
+			ret = __tps6591x_ldo1_set_voltage(parent, ri,
+					tps6591x_pdata->init_uV,
+					tps6591x_pdata->init_uV, 0);
+			break;
+
+		case TPS6591X_ID_LDO_3:
+		case TPS6591X_ID_LDO_5:
+		case TPS6591X_ID_LDO_6:
+		case TPS6591X_ID_LDO_7:
+		case TPS6591X_ID_LDO_8:
+			ret = __tps6591x_ldo3_set_voltage(parent, ri,
+					tps6591x_pdata->init_uV,
+					tps6591x_pdata->init_uV, 0);
+			break;
+
+		case TPS6591X_ID_VDD_1:
+		case TPS6591X_ID_VDD_2:
+		case TPS6591X_ID_VDDCTRL:
+			ret = __tps6591x_vdd_set_voltage(parent, ri,
+					tps6591x_pdata->init_uV,
+					tps6591x_pdata->init_uV, 0);
+			break;
+
+		default:
+			ret = -EINVAL;
+			break;
+		}
+		if (ret < 0) {
+			pr_err("Not able to initialize voltage %d for rail "
+				"%d err %d\n", tps6591x_pdata->init_uV,
+				ri->desc.id, ret);
+			return ret;
+		}
+	}
+
+	reg_val = ri->supply_reg.cache_val;
+	if (tps6591x_pdata->init_enable)
+		reg_val |= 0x1;
+	else
+		reg_val &= ~0x1;
+	ret = tps6591x_write(parent, ri->supply_reg.addr, reg_val);
+
+	if (ret < 0)
+		pr_err("Not able to %s rail %d err %d\n",
+			(tps6591x_pdata->init_enable) ? "enable" : "disable",
+			ri->desc.id, ret);
+	else
+		ri->supply_reg.cache_val = reg_val;
+	return ret;
+}
+
+static inline int tps6591x_cache_regulator_register(struct device *parent,
+		struct tps6591x_regulator *ri)
+{
+	int ret;
+	ret = tps6591x_read(parent, ri->supply_reg.addr,
+				&ri->supply_reg.cache_val);
+	if (!ret && (ri->supply_type == supply_type_sr_op_reg)) {
+		ret = tps6591x_read(parent, ri->op_reg.addr,
+				&ri->op_reg.cache_val);
+		if (!ret)
+			ret = tps6591x_read(parent, ri->sr_reg.addr,
+				&ri->sr_reg.cache_val);
+	}
+	return ret;
+}
+
+static inline struct tps6591x_regulator *find_regulator_info(int id)
+{
+	struct tps6591x_regulator *ri;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(tps6591x_regulator); i++) {
+		ri = &tps6591x_regulator[i];
+		if (ri->desc.id == id)
+			return ri;
+	}
+	return NULL;
+}
+
+
+static int __devinit tps6591x_regulator_probe(struct platform_device *pdev)
+{
+	struct tps6591x_regulator *ri = NULL;
+	struct regulator_dev *rdev;
+	struct tps6591x_regulator_platform_data *tps_pdata;
+	int id = pdev->id;
+	int err;
+
+	dev_dbg(&pdev->dev, "Probing reulator %d\n", id);
+
+	ri = find_regulator_info(id);
+	if (ri == NULL) {
+		dev_err(&pdev->dev, "invalid regulator ID specified\n");
+		return -EINVAL;
+	}
+	tps_pdata = pdev->dev.platform_data;
+	ri->ectrl = tps_pdata->ectrl;
+	ri->config_flags = tps_pdata->flags;
+
+	if (tps_pdata->slew_rate_uV_per_us)
+		ri->voltage_change_uv_per_us = tps_pdata->slew_rate_uV_per_us;
+
+	err = tps6591x_cache_regulator_register(pdev->dev.parent, ri);
+	if (err) {
+		dev_err(&pdev->dev, "Error in caching registers error %d\n",
+							err);
+		return err;
+	}
+
+	err = tps6591x_regulator_preinit(pdev->dev.parent, ri, tps_pdata);
+	if (err) {
+		dev_err(&pdev->dev, "Error in pre-initialization of regulator "
+					"error %d\n", err);
+		return err;
+	}
+
+	rdev = regulator_register(&ri->desc, &pdev->dev,
+				&tps_pdata->regulator, ri);
+	if (IS_ERR_OR_NULL(rdev)) {
+		dev_err(&pdev->dev, "failed to register regulator %s\n",
+				ri->desc.name);
+		return PTR_ERR(rdev);
+	}
+	ri->current_volt_uv = ri->desc.ops->get_voltage(rdev);
+
+	platform_set_drvdata(pdev, rdev);
+
+	return 0;
+}
+
+static int __devexit tps6591x_regulator_remove(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev = platform_get_drvdata(pdev);
+
+	regulator_unregister(rdev);
+	return 0;
+}
+
+static void tps6591x_regulator_shutdown(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev = platform_get_drvdata(pdev);
+	struct tps6591x_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps6591x_dev(rdev);
+	int ret;
+
+	if (ri->ectrl == EXT_CTRL_EN1) {
+		ret = tps6591x_clr_bits(parent, ri->en1_reg.addr,
+				(1 << ri->en1_reg.shift_bits));
+		if (ret < 0)
+			dev_err(&pdev->dev, "Error in clearing external control\n");
+	}
+}
+
+static int tps6591x_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct regulator_dev *rdev = platform_get_drvdata(pdev);
+	struct tps6591x_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps6591x_dev(rdev);
+	int ret = 0;
+	uint8_t reg_val;
+
+	if (ri->config_flags & LDO_LOW_POWER_ON_SUSPEND) {
+		ret = tps6591x_clr_bits(parent, ri->en1_reg.addr,
+				(1 << ri->en1_reg.shift_bits));
+		reg_val = ri->supply_reg.cache_val;
+		reg_val = (reg_val & ~0x3) | (0x3);
+		ret = tps6591x_write(parent, ri->supply_reg.addr, reg_val);
+		if (ret >= 0)
+			ri->supply_reg.cache_val = reg_val;
+		else
+			dev_err(&pdev->dev, "Error in updating the supply state\n");
+	}
+	return ret;
+}
+
+static int tps6591x_resume(struct platform_device *pdev)
+{
+	struct regulator_dev *rdev = platform_get_drvdata(pdev);
+	struct tps6591x_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps6591x_dev(rdev);
+	int ret = 0;
+	uint8_t reg_val;
+
+	if (ri->config_flags & LDO_LOW_POWER_ON_SUSPEND) {
+		ret = tps6591x_clr_bits(parent, ri->en1_reg.addr,
+				(1 << ri->en1_reg.shift_bits));
+		reg_val = ri->supply_reg.cache_val;
+		reg_val = (reg_val & ~0x3) | (0x1);
+		ret = tps6591x_write(parent, ri->supply_reg.addr, reg_val);
+		if (ret >= 0)
+			ri->supply_reg.cache_val = reg_val;
+		else
+			dev_err(&pdev->dev, "Error in updating the supply state\n");
+	}
+	return ret;
+}
+
+static struct platform_driver tps6591x_regulator_driver = {
+	.driver	= {
+		.name	= "tps6591x-regulator",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= tps6591x_regulator_probe,
+	.remove		= __devexit_p(tps6591x_regulator_remove),
+	.shutdown	= tps6591x_regulator_shutdown,
+	.suspend	= tps6591x_suspend,
+	.resume		= tps6591x_resume,
+};
+
+static int __init tps6591x_regulator_init(void)
+{
+	return platform_driver_register(&tps6591x_regulator_driver);
+}
+subsys_initcall(tps6591x_regulator_init);
+
+static void __exit tps6591x_regulator_exit(void)
+{
+	platform_driver_unregister(&tps6591x_regulator_driver);
+}
+module_exit(tps6591x_regulator_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Regulator Driver for TI TPS6591X PMIC");
+MODULE_ALIAS("platform:tps6591x-regulator");
diff --git a/drivers/regulator/tps80031-regulator.c b/drivers/regulator/tps80031-regulator.c
new file mode 100644
index 000000000000..39b5f452692a
--- /dev/null
+++ b/drivers/regulator/tps80031-regulator.c
@@ -0,0 +1,1223 @@
+/*
+ * driver/regulator/tps80031-regulator.c
+ *
+ * Regulator driver for TI TPS80031
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/tps80031-regulator.h>
+#include <linux/mfd/tps80031.h>
+
+/* Flags for DCDC Voltage reading */
+#define DCDC_OFFSET_EN		BIT(0)
+#define DCDC_EXTENDED_EN	BIT(1)
+#define TRACK_MODE_ENABLE	BIT(2)
+
+#define SMPS_MULTOFFSET_VIO	BIT(1)
+#define SMPS_MULTOFFSET_SMPS1	BIT(3)
+#define SMPS_MULTOFFSET_SMPS2	BIT(4)
+#define SMPS_MULTOFFSET_SMPS3	BIT(6)
+#define SMPS_MULTOFFSET_SMPS4	BIT(0)
+
+#define PMC_SMPS_OFFSET_ADD	0xE0
+#define PMC_SMPS_MULT_ADD	0xE3
+
+#define STATE_OFF		0x00
+#define STATE_ON		0x01
+#define STATE_MASK		0x03
+
+#define TRANS_SLEEP_OFF		0x00
+#define TRANS_SLEEP_ON		0x04
+#define TRANS_SLEEP_MASK	0x0C
+
+#define SMPS_CMD_MASK		0xC0
+#define SMPS_VSEL_MASK		0x3F
+#define LDO_VSEL_MASK		0x1F
+
+#define TPS80031_MISC2_ADD	0xE5
+#define MISC2_LDOUSB_IN_VSYS	0x10
+#define MISC2_LDOUSB_IN_PMID	0x08
+#define MISC2_LDOUSB_IN_MASK	0x18
+
+#define MISC2_LDO3_SEL_VIB_VAL	BIT(0)
+#define MISC2_LDO3_SEL_VIB_MASK	0x1
+
+#define CHARGERUSB_CTRL3_ADD	0xEA
+#define BOOST_HW_PWR_EN		BIT(5)
+#define BOOST_HW_PWR_EN_MASK	BIT(5)
+
+#define CHARGERUSB_CTRL1_ADD	0xE8
+#define OPA_MODE_EN		BIT(6)
+#define OPA_MODE_EN_MASK	BIT(6)
+
+#define USB_VBUS_CTRL_SET	0x04
+#define USB_VBUS_CTRL_CLR	0x05
+#define VBUS_DISCHRG		0x20
+
+#define EXT_PWR_REQ (PWR_REQ_INPUT_PREQ1 | PWR_REQ_INPUT_PREQ2 | \
+			PWR_REQ_INPUT_PREQ3)
+
+struct tps80031_regulator_info {
+	/* Regulator register address.*/
+	u8		trans_reg;
+	u8		state_reg;
+	u8		force_reg;
+	u8		volt_reg;
+	u8		volt_id;
+
+	/* chip constraints on regulator behavior */
+	u16			min_mV;
+	u16			max_mV;
+
+	/* regulator specific turn-on delay as per datasheet*/
+	int			delay;
+
+	/* used by regulator core */
+	struct regulator_desc	desc;
+
+	/*Power request bits */
+	int preq_bit;
+};
+
+struct tps80031_regulator {
+	struct device			*dev;
+	struct regulator_dev		*rdev;
+	struct tps80031_regulator_info	*rinfo;
+	unsigned int			tolerance_uv;
+
+	/* Regulator specific turn-on delay if board file provided */
+	int				delay;
+
+	u8				flags;
+	unsigned int			platform_flags;
+	unsigned int			ext_ctrl_flag;
+
+	/* Cached register */
+	uint8_t		trans_reg_cache;
+	uint8_t		state_reg_cache;
+	uint8_t		force_reg_cache;
+	uint8_t		volt_reg_cache;
+};
+
+static inline struct device *to_tps80031_dev(struct regulator_dev *rdev)
+{
+	return rdev_get_dev(rdev)->parent->parent;
+}
+
+static int tps80031_regulator_enable_time(struct regulator_dev *rdev)
+{
+	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
+
+	return ri->delay;
+}
+
+static u8 tps80031_get_smps_offset(struct device *parent)
+{
+	u8 value;
+	int ret;
+
+	ret = tps80031_read(parent, SLAVE_ID1, PMC_SMPS_OFFSET_ADD, &value);
+	if (ret < 0) {
+		dev_err(parent, "Error in reading smps offset register\n");
+		return 0;
+	}
+	return value;
+}
+
+static u8 tps80031_get_smps_mult(struct device *parent)
+{
+	u8 value;
+	int ret;
+
+	ret = tps80031_read(parent, SLAVE_ID1, PMC_SMPS_MULT_ADD, &value);
+	if (ret < 0) {
+		dev_err(parent, "Error in reading smps mult register\n");
+		return 0;
+	}
+	return value;
+}
+
+static int tps80031_reg_is_enabled(struct regulator_dev *rdev)
+{
+	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
+
+	if (ri->ext_ctrl_flag & EXT_PWR_REQ)
+		return true;
+	return ((ri->state_reg_cache & STATE_MASK) == STATE_ON);
+}
+
+static int tps80031_reg_enable(struct regulator_dev *rdev)
+{
+	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps80031_dev(rdev);
+	int ret;
+	uint8_t reg_val;
+
+	if (ri->ext_ctrl_flag & EXT_PWR_REQ)
+		return 0;
+
+	reg_val = (ri->state_reg_cache & ~STATE_MASK) |
+					(STATE_ON & STATE_MASK);
+	ret = tps80031_write(parent, SLAVE_ID1, ri->rinfo->state_reg, reg_val);
+	if (ret < 0) {
+		dev_err(&rdev->dev, "Error in writing the STATE register\n");
+		return ret;
+	}
+	ri->state_reg_cache = reg_val;
+	udelay(ri->delay);
+	return ret;
+}
+
+static int tps80031_reg_disable(struct regulator_dev *rdev)
+{
+	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps80031_dev(rdev);
+	int ret;
+	uint8_t reg_val;
+
+	if (ri->ext_ctrl_flag & EXT_PWR_REQ)
+		return 0;
+
+	reg_val = (ri->state_reg_cache & ~STATE_MASK) |
+					(STATE_OFF & STATE_MASK);
+	ret = tps80031_write(parent, SLAVE_ID1, ri->rinfo->state_reg, reg_val);
+	if (ret < 0)
+		dev_err(&rdev->dev, "Error in writing the STATE register\n");
+	else
+		ri->state_reg_cache = reg_val;
+	return ret;
+}
+
+/*
+ * DCDC status and control
+ */
+static int tps80031dcdc_list_voltage(struct regulator_dev *rdev, unsigned index)
+{
+	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
+	int voltage = 0;
+
+	switch (ri->flags) {
+	case 0:
+		if (index == 0)
+			voltage = 0;
+		else if (index < 58)
+			voltage = (607700 + (12660 * (index - 1)));
+		else if (index == 58)
+			voltage = 1350 * 1000;
+		else if (index == 59)
+			voltage = 1500 * 1000;
+		else if (index == 60)
+			voltage = 1800 * 1000;
+		else if (index == 61)
+			voltage = 1900 * 1000;
+		else if (index == 62)
+			voltage = 2100 * 1000;
+		break;
+
+	case DCDC_OFFSET_EN:
+		if (index == 0)
+			voltage = 0;
+		else if (index < 58)
+			voltage = (700000 + (12500 * (index - 1)));
+		else if (index == 58)
+			voltage = 1350 * 1000;
+		else if (index == 59)
+			voltage = 1500 * 1000;
+		else if (index == 60)
+			voltage = 1800 * 1000;
+		else if (index == 61)
+			voltage = 1900 * 1000;
+		else if (index == 62)
+			voltage = 2100 * 1000;
+		break;
+
+	case DCDC_EXTENDED_EN:
+		if (index == 0)
+			voltage = 0;
+		else if (index < 58)
+			voltage = (1852000 + (38600 * (index - 1)));
+		else if (index == 58)
+			voltage = 2084 * 1000;
+		else if (index == 59)
+			voltage = 2315 * 1000;
+		else if (index == 60)
+			voltage = 2778 * 1000;
+		else if (index == 61)
+			voltage = 2932 * 1000;
+		else if (index == 62)
+			voltage = 3241 * 1000;
+		break;
+
+	case DCDC_OFFSET_EN|DCDC_EXTENDED_EN:
+		if (index == 0)
+			voltage = 0;
+		else if (index < 58)
+			voltage = (2161000 + (38600 * (index - 1)));
+		else if (index == 58)
+			voltage = 4167 * 1000;
+		else if (index == 59)
+			voltage = 2315 * 1000;
+		else if (index == 60)
+			voltage = 2778 * 1000;
+		else if (index == 61)
+			voltage = 2932 * 1000;
+		else if (index == 62)
+			voltage = 3241 * 1000;
+		break;
+	}
+
+	return voltage;
+}
+
+static int __tps80031_dcdc_set_voltage(struct device *parent,
+				       struct tps80031_regulator *ri,
+				       int min_uV, int max_uV,
+				       unsigned *selector)
+{
+	int vsel = 0;
+	int ret;
+
+	switch (ri->flags) {
+	case 0:
+		if (min_uV >= (607700 + ri->tolerance_uv))
+			min_uV = min_uV - ri->tolerance_uv;
+
+		if (min_uV == 0)
+			vsel = 0;
+		else if ((min_uV >= 607700) && (min_uV <= 1300000)) {
+			int cal_volt;
+			vsel = (10 * (min_uV - 607700)) / 1266;
+			if (vsel % 100)
+				vsel += 100;
+			vsel /= 100;
+			vsel++;
+			cal_volt = (607700 + (12660 * (vsel - 1)));
+			if (cal_volt > max_uV)
+				return -EINVAL;
+		} else if ((min_uV > 1900000) && (max_uV >= 2100000))
+			vsel = 62;
+		else if ((min_uV > 1800000) && (max_uV >= 1900000))
+			vsel = 61;
+		else if ((min_uV > 1500000) && (max_uV >= 1800000))
+			vsel = 60;
+		else if ((min_uV > 1350000) && (max_uV >= 1500000))
+			vsel = 59;
+		else if ((min_uV > 1300000) && (max_uV >= 1350000))
+			vsel = 58;
+		else
+			return -EINVAL;
+		break;
+
+	case DCDC_OFFSET_EN:
+		if (min_uV >= (700000 + ri->tolerance_uv))
+			min_uV = min_uV - ri->tolerance_uv;
+		if (min_uV == 0)
+			vsel = 0;
+		else if ((min_uV >= 700000) && (min_uV <= 1420000)) {
+			int cal_volt;
+			vsel = (min_uV - 700000) / 125;
+			if (vsel % 100)
+				vsel += 100;
+			vsel /= 100;
+			vsel++;
+			cal_volt = (700000 + (12500 * (vsel - 1)));
+			if (cal_volt > max_uV)
+				return -EINVAL;
+		} else if ((min_uV > 1900000) && (max_uV >= 2100000))
+			vsel = 62;
+		else if ((min_uV > 1800000) && (max_uV >= 1900000))
+			vsel = 61;
+		else if ((min_uV > 1500000) && (max_uV >= 1800000))
+			vsel = 60;
+		else if ((min_uV > 1350000) && (max_uV >= 1500000))
+			vsel = 59;
+		else if ((min_uV > 1300000) && (max_uV >= 1350000))
+			vsel = 58;
+		else
+			return -EINVAL;
+		break;
+
+	case DCDC_EXTENDED_EN:
+		if (min_uV >= (1852000 + ri->tolerance_uv))
+			min_uV = min_uV - ri->tolerance_uv;
+		if (min_uV == 0)
+			vsel = 0;
+		else if ((min_uV >= 1852000) && (max_uV <= 4013600)) {
+			vsel = (min_uV - 1852000) / 386;
+			if (vsel % 100)
+				vsel += 100;
+			vsel++;
+		}
+		break;
+
+	case DCDC_OFFSET_EN|DCDC_EXTENDED_EN:
+		if (min_uV >= (2161000 + ri->tolerance_uv))
+			min_uV = min_uV - ri->tolerance_uv;
+		if (min_uV == 0)
+			vsel = 0;
+		else if ((min_uV >= 2161000) && (max_uV <= 4321000)) {
+			vsel = (min_uV - 2161000) / 386;
+			if (vsel % 100)
+				vsel += 100;
+			vsel /= 100;
+			vsel++;
+		}
+		break;
+	}
+
+	if (selector)
+		*selector = vsel;
+
+	if (ri->rinfo->force_reg) {
+		if (((ri->force_reg_cache >> 6) & 0x3) == 0) {
+			ret = tps80031_write(parent, ri->rinfo->volt_id,
+						ri->rinfo->force_reg, vsel);
+			if (ret < 0)
+				dev_err(ri->dev, "Error in writing the "
+						"force register\n");
+			else
+				ri->force_reg_cache = vsel;
+			return ret;
+		}
+	}
+	ret = tps80031_write(parent, ri->rinfo->volt_id,
+					ri->rinfo->volt_reg, vsel);
+	if (ret < 0)
+		dev_err(ri->dev, "Error in writing the Voltage register\n");
+	else
+		ri->volt_reg_cache = vsel;
+	return ret;
+}
+
+static int tps80031dcdc_set_voltage(struct regulator_dev *rdev,
+			int min_uV, int max_uV, unsigned *selector)
+{
+	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps80031_dev(rdev);
+	return __tps80031_dcdc_set_voltage(parent, ri, min_uV, max_uV,
+					   selector);
+}
+
+static int tps80031dcdc_get_voltage(struct regulator_dev *rdev)
+{
+	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
+	uint8_t vsel = 0;
+	int voltage = 0;
+
+	if (ri->rinfo->force_reg) {
+		vsel  = ri->force_reg_cache;
+		if ((vsel & SMPS_CMD_MASK) == 0)
+			goto decode;
+	}
+
+	vsel =  ri->volt_reg_cache;
+
+decode:
+	vsel &= SMPS_VSEL_MASK;
+
+	switch (ri->flags) {
+	case 0:
+		if (vsel == 0)
+			voltage = 0;
+		else if (vsel < 58)
+			voltage = (607700 + (12660 * (vsel - 1)));
+		else if (vsel == 58)
+			voltage = 1350 * 1000;
+		else if (vsel == 59)
+			voltage = 1500 * 1000;
+		else if (vsel == 60)
+			voltage = 1800 * 1000;
+		else if (vsel == 61)
+			voltage = 1900 * 1000;
+		else if (vsel == 62)
+			voltage = 2100 * 1000;
+		break;
+
+	case DCDC_OFFSET_EN:
+		if (vsel == 0)
+			voltage = 0;
+		else if (vsel < 58)
+			voltage = (700000 + (12500 * (vsel - 1)));
+		else if (vsel == 58)
+			voltage = 1350 * 1000;
+		else if (vsel == 59)
+			voltage = 1500 * 1000;
+		else if (vsel == 60)
+			voltage = 1800 * 1000;
+		else if (vsel == 61)
+			voltage = 1900 * 1000;
+		else if (vsel == 62)
+			voltage = 2100 * 1000;
+		break;
+
+	case DCDC_EXTENDED_EN:
+		if (vsel == 0)
+			voltage = 0;
+		else if (vsel < 58)
+			voltage = (1852000 + (38600 * (vsel - 1)));
+		else if (vsel == 58)
+			voltage = 2084 * 1000;
+		else if (vsel == 59)
+			voltage = 2315 * 1000;
+		else if (vsel == 60)
+			voltage = 2778 * 1000;
+		else if (vsel == 61)
+			voltage = 2932 * 1000;
+		else if (vsel == 62)
+			voltage = 3241 * 1000;
+		break;
+
+	case DCDC_EXTENDED_EN|DCDC_OFFSET_EN:
+		if (vsel == 0)
+			voltage = 0;
+		else if (vsel < 58)
+			voltage = (2161000 + (38600 * (vsel - 1)));
+		else if (vsel == 58)
+			voltage = 4167 * 1000;
+		else if (vsel == 59)
+			voltage = 2315 * 1000;
+		else if (vsel == 60)
+			voltage = 2778 * 1000;
+		else if (vsel == 61)
+			voltage = 2932 * 1000;
+		else if (vsel == 62)
+			voltage = 3241 * 1000;
+		break;
+	}
+
+	return voltage;
+}
+
+static int tps80031ldo_list_voltage(struct regulator_dev *rdev, unsigned index)
+{
+	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
+
+	if (index == 0)
+		return 0;
+
+	if ((ri->rinfo->desc.id == TPS80031_REGULATOR_LDO2) &&
+			(ri->flags &  TRACK_MODE_ENABLE))
+		return (ri->rinfo->min_mV + (((index - 1) * 125))/10) * 1000;
+
+	return (ri->rinfo->min_mV + ((index - 1) * 100)) * 1000;
+}
+
+static int __tps80031_ldo2_set_voltage_track_mode(struct device *parent,
+		struct tps80031_regulator *ri, int min_uV, int max_uV)
+{
+	int vsel = 0;
+	int ret;
+	int nvsel;
+
+	if (min_uV < 600000) {
+		vsel = 0;
+	} else if ((min_uV >= 600000) && (max_uV <= 1300000)) {
+		vsel = (min_uV - 600000) / 125;
+		if (vsel % 100)
+			vsel += 100;
+		vsel /= 100;
+		vsel++;
+	} else {
+		return -EINVAL;
+	}
+
+	/* Check for valid setting for TPS80031 or TPS80032-ES1.0 */
+	if ((tps80031_get_chip_info(parent) == TPS80031) ||
+		((tps80031_get_chip_info(parent) == TPS80032) &&
+		(tps80031_get_pmu_version(parent) == 0x0))) {
+		nvsel = vsel & 0x1F;
+		if ((nvsel == 0x0) || (nvsel >= 0x19 && nvsel <= 0x1F)) {
+			dev_err(ri->dev, "Invalid value for track mode LDO2 "
+				"configuration for TPS8003x PMU\n");
+			return -EINVAL;
+		}
+	}
+
+	ret = tps80031_write(parent, ri->rinfo->volt_id,
+				ri->rinfo->volt_reg, vsel);
+	if (ret < 0)
+		dev_err(ri->dev, "Error in writing the Voltage register\n");
+	else
+		ri->volt_reg_cache = vsel;
+	return ret;
+}
+
+
+static int __tps80031_ldo_set_voltage(struct device *parent,
+				      struct tps80031_regulator *ri,
+				      int min_uV, int max_uV,
+				      unsigned *selector)
+{
+	int vsel;
+	int ret;
+
+	if ((min_uV/1000 < ri->rinfo->min_mV) ||
+			(max_uV/1000 > ri->rinfo->max_mV))
+		return -EDOM;
+
+	if ((ri->rinfo->desc.id == TPS80031_REGULATOR_LDO2) &&
+			(ri->flags &  TRACK_MODE_ENABLE))
+		return __tps80031_ldo2_set_voltage_track_mode(parent, ri,
+				min_uV, max_uV);
+
+	/*
+	 * Use the below formula to calculate vsel
+	 * mV = 1000mv + 100mv * (vsel - 1)
+	 */
+	vsel = (min_uV/1000 - 1000)/100 + 1;
+	if (selector)
+		*selector = vsel;
+	ret = tps80031_write(parent, ri->rinfo->volt_id,
+				ri->rinfo->volt_reg, vsel);
+	if (ret < 0)
+		dev_err(ri->dev, "Error in writing the Voltage register\n");
+	else
+		ri->volt_reg_cache = vsel;
+	return ret;
+}
+
+static int tps80031ldo_set_voltage(struct regulator_dev *rdev,
+		int min_uV, int max_uV, unsigned *selector)
+{
+	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps80031_dev(rdev);
+
+	return __tps80031_ldo_set_voltage(parent, ri, min_uV, max_uV,
+					  selector);
+}
+
+static int tps80031ldo_get_voltage(struct regulator_dev *rdev)
+{
+	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
+	uint8_t vsel;
+
+
+	if ((ri->rinfo->desc.id == TPS80031_REGULATOR_LDO2) &&
+			(ri->flags &  TRACK_MODE_ENABLE)) {
+		vsel = ri->volt_reg_cache & 0x3F;
+		return (ri->rinfo->min_mV + (((vsel - 1) * 125))/10) * 1000;
+	}
+
+	vsel = ri->volt_reg_cache & LDO_VSEL_MASK;
+	/*
+	 * Use the below formula to calculate vsel
+	 * mV = 1000mv + 100mv * (vsel - 1)
+	 */
+	return (1000 + (100 * (vsel - 1))) * 1000;
+}
+
+/* VBUS */
+static int tps80031_vbus_enable_time(struct regulator_dev *rdev)
+{
+	/* Enable and settling time for vbus is 3ms */
+	return 3000;
+}
+static int tps80031_vbus_is_enabled(struct regulator_dev *rdev)
+{
+	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps80031_dev(rdev);
+	uint8_t ctrl1, ctrl3;
+	int ret;
+
+	if (ri->platform_flags & VBUS_SW_ONLY) {
+		ret = tps80031_read(parent, SLAVE_ID2,
+				CHARGERUSB_CTRL1_ADD, &ctrl1);
+		if (!ret)
+			ret = tps80031_read(parent, SLAVE_ID2,
+					CHARGERUSB_CTRL3_ADD, &ctrl3);
+		if (ret < 0) {
+			dev_err(&rdev->dev, "Error in reading control reg\n");
+			return ret;
+		}
+		if ((ctrl1 & OPA_MODE_EN) && (ctrl3 & BOOST_HW_PWR_EN))
+			return 1;
+		return 0;
+	} else {
+		return -EIO;
+	}
+}
+
+static int tps80031_vbus_enable(struct regulator_dev *rdev)
+{
+	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps80031_dev(rdev);
+	int ret;
+
+	if (ri->platform_flags & VBUS_SW_ONLY) {
+		ret = tps80031_set_bits(parent, SLAVE_ID2,
+				CHARGERUSB_CTRL1_ADD,  OPA_MODE_EN);
+		if (!ret)
+			ret = tps80031_set_bits(parent, SLAVE_ID2,
+					CHARGERUSB_CTRL3_ADD, BOOST_HW_PWR_EN);
+		if (ret < 0) {
+			dev_err(&rdev->dev, "Error in reading control reg\n");
+			return ret;
+		}
+		udelay(ri->delay);
+		return ret;
+	}
+	dev_err(&rdev->dev, "%s() is not supported with flag 0x%08x\n",
+		 __func__, ri->platform_flags);
+	return -EIO;
+}
+
+static int tps80031_vbus_disable(struct regulator_dev *rdev)
+{
+	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
+	struct device *parent = to_tps80031_dev(rdev);
+	int ret = 0;
+
+	if (ri->platform_flags & VBUS_SW_ONLY) {
+
+		if (ri->platform_flags & VBUS_DISCHRG_EN_PDN)
+			ret = tps80031_write(parent, SLAVE_ID2,
+				USB_VBUS_CTRL_SET, VBUS_DISCHRG);
+		if (!ret)
+			ret = tps80031_clr_bits(parent, SLAVE_ID2,
+				CHARGERUSB_CTRL1_ADD,  OPA_MODE_EN);
+		if (!ret)
+			ret = tps80031_clr_bits(parent, SLAVE_ID2,
+					CHARGERUSB_CTRL3_ADD, BOOST_HW_PWR_EN);
+		if (!ret)
+			mdelay((ri->delay + 999)/1000);
+
+		if (ri->platform_flags & VBUS_DISCHRG_EN_PDN)
+			tps80031_write(parent, SLAVE_ID2,
+				USB_VBUS_CTRL_CLR, VBUS_DISCHRG);
+
+		if (ret < 0)
+			dev_err(&rdev->dev, "Error in reading control reg\n");
+		return ret;
+	}
+	dev_err(&rdev->dev, "%s() is not supported with flag 0x%08x\n",
+		 __func__, ri->platform_flags);
+	return -EIO;
+}
+
+static int tps80031vbus_get_voltage(struct regulator_dev *rdev)
+{
+	int ret;
+	ret = tps80031_vbus_is_enabled(rdev);
+	if (ret > 0)
+		return 5000000;
+	return ret;
+}
+
+static int tps80031_extreg_enable_time(struct regulator_dev *rdev)
+{
+	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
+	return ri->delay;
+}
+
+static int tps80031_extreg_get_voltage(struct regulator_dev *rdev)
+{
+	struct tps80031_regulator *ri = rdev_get_drvdata(rdev);
+	int ret;
+	ret = tps80031_reg_is_enabled(rdev);
+	if (ret > 0)
+		return ri->rinfo->max_mV * 1000;
+	return 0;
+}
+
+static struct regulator_ops tps80031dcdc_ops = {
+	.list_voltage	= tps80031dcdc_list_voltage,
+	.set_voltage	= tps80031dcdc_set_voltage,
+	.get_voltage	= tps80031dcdc_get_voltage,
+	.enable		= tps80031_reg_enable,
+	.disable	= tps80031_reg_disable,
+	.is_enabled	= tps80031_reg_is_enabled,
+	.enable_time	= tps80031_regulator_enable_time,
+};
+
+static struct regulator_ops tps80031ldo_ops = {
+	.list_voltage	= tps80031ldo_list_voltage,
+	.set_voltage	= tps80031ldo_set_voltage,
+	.get_voltage	= tps80031ldo_get_voltage,
+	.enable		= tps80031_reg_enable,
+	.disable	= tps80031_reg_disable,
+	.is_enabled	= tps80031_reg_is_enabled,
+	.enable_time	= tps80031_regulator_enable_time,
+};
+
+static struct regulator_ops tps80031vbus_ops = {
+	.get_voltage	= tps80031vbus_get_voltage,
+	.enable		= tps80031_vbus_enable,
+	.disable	= tps80031_vbus_disable,
+	.is_enabled	= tps80031_vbus_is_enabled,
+	.enable_time	= tps80031_vbus_enable_time,
+};
+
+static struct regulator_ops tps80031_ext_reg_ops = {
+	.enable		= tps80031_reg_enable,
+	.disable	= tps80031_reg_disable,
+	.is_enabled	= tps80031_reg_is_enabled,
+	.enable_time	= tps80031_extreg_enable_time,
+	.get_voltage	= tps80031_extreg_get_voltage,
+};
+
+
+
+#define TPS80031_REG(_id, _trans_reg, _state_reg, _force_reg, _volt_reg, \
+		_volt_id, min_mVolts, max_mVolts, _ops, _n_volt, _delay, \
+		_preq_bit)						 \
+{								\
+	.trans_reg = _trans_reg,				\
+	.state_reg = _state_reg,				\
+	.force_reg = _force_reg,				\
+	.volt_reg = _volt_reg,					\
+	.volt_id = _volt_id,					\
+	.min_mV = min_mVolts,					\
+	.max_mV = max_mVolts,					\
+	.desc = {						\
+		.name = tps80031_rails(_id),			\
+		.id = TPS80031_REGULATOR_##_id,			\
+		.n_voltages = _n_volt,				\
+		.ops = &_ops,					\
+		.type = REGULATOR_VOLTAGE,			\
+		.owner = THIS_MODULE,				\
+	},							\
+	.delay = _delay,					\
+	.preq_bit = _preq_bit,					\
+}
+
+static struct tps80031_regulator_info tps80031_regulator_info[] = {
+	TPS80031_REG(VIO,   0x47, 0x48, 0x49, 0x4A, SLAVE_ID0, 600, 2100,
+				tps80031dcdc_ops, 63, 500, 4),
+	TPS80031_REG(SMPS1, 0x53, 0x54, 0x55, 0x56, SLAVE_ID0, 600, 2100,
+				tps80031dcdc_ops, 63, 500, 0),
+	TPS80031_REG(SMPS2, 0x59, 0x5A, 0x5B, 0x5C, SLAVE_ID0, 600, 2100,
+				tps80031dcdc_ops, 63, 500, 1),
+	TPS80031_REG(SMPS3, 0x65, 0x66, 0x00, 0x68, SLAVE_ID1, 600, 2100,
+				tps80031dcdc_ops, 63, 500, 2),
+	TPS80031_REG(SMPS4, 0x41, 0x42, 0x00, 0x44, SLAVE_ID1, 600, 2100,
+				tps80031dcdc_ops, 63, 500, 3),
+
+	TPS80031_REG(LDO1,   0x9D, 0x9E, 0x00, 0x9F, SLAVE_ID1, 1000, 3300,
+				tps80031ldo_ops, 25, 500, 8),
+	TPS80031_REG(LDO2,   0x85, 0x86, 0x00, 0x87, SLAVE_ID1, 1000, 3300,
+				tps80031ldo_ops, 25, 500, 9),
+	TPS80031_REG(LDO3,   0x8D, 0x8E, 0x00, 0x8F, SLAVE_ID1, 1000, 3300,
+				tps80031ldo_ops, 25, 500, 10),
+	TPS80031_REG(LDO4,   0x89, 0x8A, 0x00, 0x8B, SLAVE_ID1, 1000, 3300,
+				tps80031ldo_ops, 25, 500, 11),
+	TPS80031_REG(LDO5,   0x99, 0x9A, 0x00, 0x9B, SLAVE_ID1, 1000, 3300,
+				tps80031ldo_ops, 25, 500, 12),
+	TPS80031_REG(LDO6,   0x91, 0x92, 0x00, 0x93, SLAVE_ID1, 1000, 3300,
+				tps80031ldo_ops, 25, 500, 13),
+	TPS80031_REG(LDO7,   0xA5, 0xA6, 0x00, 0xA7, SLAVE_ID1, 1000, 3300,
+				tps80031ldo_ops, 25, 500, 14),
+	TPS80031_REG(LDOUSB, 0xA1, 0xA2, 0x00, 0xA3, SLAVE_ID1, 1000, 3300,
+				tps80031ldo_ops, 25, 500, 5),
+	TPS80031_REG(LDOLN,  0x95, 0x96, 0x00, 0x97, SLAVE_ID1, 1000, 3300,
+				tps80031ldo_ops, 25, 500, 15),
+	TPS80031_REG(VANA,   0x81, 0x82, 0x00, 0x83, SLAVE_ID1, 1000, 3300,
+				tps80031ldo_ops, 25, 500, -1),
+	TPS80031_REG(VBUS,   0x0,  0x0,  0x00, 0x0,  SLAVE_ID1, 0,    5000,
+				tps80031vbus_ops, 2, 200000, -1),
+	TPS80031_REG(REGEN1, 0xAE,  0xAF,  0x00, 0x0,  SLAVE_ID1, 0,    3300,
+				tps80031_ext_reg_ops, 2, 500, 16),
+	TPS80031_REG(REGEN2, 0xB1,  0xB2,  0x00, 0x0,  SLAVE_ID1, 0,    3300,
+				tps80031_ext_reg_ops, 2, 500, 17),
+	TPS80031_REG(SYSEN,  0xB4,  0xB5,  0x00, 0x0,  SLAVE_ID1, 0,    3300,
+				tps80031_ext_reg_ops, 2, 500, 18),
+};
+
+static int tps80031_power_req_config(struct device *parent,
+		struct tps80031_regulator *ri,
+		struct tps80031_regulator_platform_data *tps80031_pdata)
+{
+	int ret = 0;
+	uint8_t reg_val;
+
+	if (ri->rinfo->preq_bit < 0)
+		goto skip_pwr_req_config;
+
+	ret = tps80031_ext_power_req_config(parent, ri->ext_ctrl_flag,
+			ri->rinfo->preq_bit, ri->rinfo->state_reg,
+			ri->rinfo->trans_reg);
+	if (!ret)
+		ret = tps80031_read(parent, SLAVE_ID1, ri->rinfo->trans_reg,
+			&ri->trans_reg_cache);
+
+	if (!ret && ri->rinfo->state_reg)
+		ret = tps80031_read(parent, SLAVE_ID1, ri->rinfo->state_reg,
+			&ri->state_reg_cache);
+	if (ret < 0) {
+		dev_err(ri->dev, "%s() fails\n", __func__);
+		return ret;
+	}
+
+skip_pwr_req_config:
+	if (tps80031_pdata->ext_ctrl_flag &
+			(PWR_OFF_ON_SLEEP | PWR_ON_ON_SLEEP)) {
+		reg_val = (ri->trans_reg_cache & ~0xC);
+		if (tps80031_pdata->ext_ctrl_flag & PWR_ON_ON_SLEEP)
+			reg_val |= 0x4;
+
+		ret = tps80031_write(parent, SLAVE_ID1, ri->rinfo->trans_reg,
+			reg_val);
+		if (ret < 0)
+			dev_err(ri->dev, "Not able to write reg 0x%02x\n",
+				ri->rinfo->trans_reg);
+		else
+			ri->trans_reg_cache = reg_val;
+	}
+	return ret;
+}
+
+static int tps80031_regulator_preinit(struct device *parent,
+		struct tps80031_regulator *ri,
+		struct tps80031_regulator_platform_data *tps80031_pdata)
+{
+	int ret = 0;
+	uint8_t reg_val;
+
+	if (ri->rinfo->desc.id == TPS80031_REGULATOR_LDOUSB) {
+		if (ri->platform_flags & USBLDO_INPUT_VSYS)
+			ret = tps80031_update(parent, SLAVE_ID1,
+				TPS80031_MISC2_ADD,
+				MISC2_LDOUSB_IN_VSYS, MISC2_LDOUSB_IN_MASK);
+		if (ri->platform_flags & USBLDO_INPUT_PMID)
+			ret = tps80031_update(parent, SLAVE_ID1,
+				TPS80031_MISC2_ADD,
+				MISC2_LDOUSB_IN_PMID, MISC2_LDOUSB_IN_MASK);
+		if (ret < 0) {
+			dev_err(ri->dev, "Not able to configure the rail "
+				"LDOUSB as per platform data error %d\n", ret);
+			return ret;
+		}
+	}
+
+	if (ri->rinfo->desc.id == TPS80031_REGULATOR_LDO3) {
+		if (ri->platform_flags & LDO3_OUTPUT_VIB)
+			ret = tps80031_update(parent, SLAVE_ID1,
+				TPS80031_MISC2_ADD,
+				MISC2_LDO3_SEL_VIB_VAL,
+				MISC2_LDO3_SEL_VIB_MASK);
+		if (ret < 0) {
+			dev_err(ri->dev, "Not able to configure the rail "
+				"LDO3 as per platform data error %d\n", ret);
+			return ret;
+		}
+	}
+
+	switch (ri->rinfo->desc.id) {
+	case TPS80031_REGULATOR_REGEN1:
+	case TPS80031_REGULATOR_REGEN2:
+	case TPS80031_REGULATOR_SYSEN:
+		if (tps80031_pdata->reg_init_data->constraints.always_on ||
+			tps80031_pdata->reg_init_data->constraints.boot_on)
+			ret = tps80031_update(parent, SLAVE_ID1,
+				ri->rinfo->state_reg, STATE_ON, STATE_MASK);
+		else
+			ret = tps80031_update(parent, SLAVE_ID1,
+				ri->rinfo->state_reg, STATE_OFF, STATE_MASK);
+		if (ret < 0) {
+			dev_err(ri->dev,
+				"state reg update failed, e %d\n", ret);
+			return ret;
+		}
+		ret = tps80031_update(parent, SLAVE_ID1,
+					ri->rinfo->trans_reg, 1, 0x3);
+		if (ret < 0) {
+			dev_err(ri->dev,
+				"trans reg update failed, e %d\n", ret);
+			return ret;
+		}
+		break;
+	default:
+		break;
+	}
+
+	if (!tps80031_pdata->init_apply)
+		return 0;
+
+	if (tps80031_pdata->init_uV >= 0) {
+		switch (ri->rinfo->desc.id) {
+		case TPS80031_REGULATOR_VIO:
+		case TPS80031_REGULATOR_SMPS1:
+		case TPS80031_REGULATOR_SMPS2:
+		case TPS80031_REGULATOR_SMPS3:
+		case TPS80031_REGULATOR_SMPS4:
+			ret = __tps80031_dcdc_set_voltage(parent, ri,
+					tps80031_pdata->init_uV,
+					tps80031_pdata->init_uV, 0);
+			break;
+
+		case TPS80031_REGULATOR_LDO1:
+		case TPS80031_REGULATOR_LDO2:
+		case TPS80031_REGULATOR_LDO3:
+		case TPS80031_REGULATOR_LDO4:
+		case TPS80031_REGULATOR_LDO5:
+		case TPS80031_REGULATOR_LDO6:
+		case TPS80031_REGULATOR_LDO7:
+		case TPS80031_REGULATOR_LDOUSB:
+		case TPS80031_REGULATOR_LDOLN:
+		case TPS80031_REGULATOR_VANA:
+			ret = __tps80031_ldo_set_voltage(parent, ri,
+					tps80031_pdata->init_uV,
+					tps80031_pdata->init_uV, 0);
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		if (ret < 0) {
+			dev_err(ri->dev, "Not able to initialize voltage %d "
+				"for rail %d err %d\n", tps80031_pdata->init_uV,
+				ri->rinfo->desc.id, ret);
+			return ret;
+		}
+	}
+
+	if (tps80031_pdata->init_enable)
+		reg_val = (ri->state_reg_cache & ~STATE_MASK) |
+				(STATE_ON & STATE_MASK);
+	else
+		reg_val = (ri->state_reg_cache & ~STATE_MASK) |
+				(STATE_OFF & STATE_MASK);
+
+	ret = tps80031_write(parent, SLAVE_ID1, ri->rinfo->state_reg, reg_val);
+	if (ret < 0)
+		dev_err(ri->dev, "Not able to %s rail %d err %d\n",
+			(tps80031_pdata->init_enable) ? "enable" : "disable",
+			ri->rinfo->desc.id, ret);
+	else
+		ri->state_reg_cache = reg_val;
+	return ret;
+}
+
+static inline struct tps80031_regulator_info *find_regulator_info(int id)
+{
+	struct tps80031_regulator_info *rinfo;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(tps80031_regulator_info); i++) {
+		rinfo = &tps80031_regulator_info[i];
+		if (rinfo->desc.id == id)
+			return rinfo;
+	}
+	return NULL;
+}
+static void check_smps_mode_mult(struct device *parent,
+	struct tps80031_regulator *ri)
+{
+	int mult_offset;
+	switch (ri->rinfo->desc.id) {
+	case TPS80031_REGULATOR_VIO:
+		mult_offset = SMPS_MULTOFFSET_VIO;
+		break;
+	case TPS80031_REGULATOR_SMPS1:
+		mult_offset = SMPS_MULTOFFSET_SMPS1;
+		break;
+	case TPS80031_REGULATOR_SMPS2:
+		mult_offset = SMPS_MULTOFFSET_SMPS2;
+		break;
+	case TPS80031_REGULATOR_SMPS3:
+		mult_offset = SMPS_MULTOFFSET_SMPS3;
+		break;
+	case TPS80031_REGULATOR_SMPS4:
+		mult_offset = SMPS_MULTOFFSET_SMPS4;
+		break;
+	case TPS80031_REGULATOR_LDO2:
+		ri->flags = (tps80031_get_smps_mult(parent) & (1 << 5)) ?
+						TRACK_MODE_ENABLE : 0;
+		/* TRACK mode the ldo2 varies from 600mV to 1300mV */
+		if (ri->flags & TRACK_MODE_ENABLE) {
+			ri->rinfo->min_mV = 600;
+			ri->rinfo->max_mV = 1300;
+			ri->rinfo->desc.n_voltages = 57;
+		}
+		return;
+	default:
+		return;
+	}
+
+	ri->flags = (tps80031_get_smps_offset(parent) & mult_offset) ?
+						DCDC_OFFSET_EN : 0;
+	ri->flags |= (tps80031_get_smps_mult(parent) & mult_offset) ?
+						DCDC_EXTENDED_EN : 0;
+	return;
+}
+
+static inline int tps80031_cache_regulator_register(struct device *parent,
+	struct tps80031_regulator *ri)
+{
+	int ret;
+
+	ret = tps80031_read(parent, SLAVE_ID1, ri->rinfo->trans_reg,
+			&ri->trans_reg_cache);
+	if (!ret && ri->rinfo->state_reg)
+		ret = tps80031_read(parent, SLAVE_ID1, ri->rinfo->state_reg,
+			&ri->state_reg_cache);
+	if (!ret && ri->rinfo->force_reg)
+		ret = tps80031_read(parent, ri->rinfo->volt_id,
+				ri->rinfo->force_reg, &ri->force_reg_cache);
+	if (!ret && ri->rinfo->volt_reg)
+		ret = tps80031_read(parent, ri->rinfo->volt_id,
+				ri->rinfo->volt_reg, &ri->volt_reg_cache);
+	return ret;
+}
+
+static int __devinit tps80031_regulator_probe(struct platform_device *pdev)
+{
+	struct tps80031_platform_data *pdata = dev_get_platdata(pdev->dev.parent);
+	struct tps80031_regulator_platform_data *tps_pdata;
+	struct tps80031_regulator_info *rinfo;
+	struct tps80031_regulator *ri;
+	struct tps80031_regulator *pmic;
+	struct regulator_dev *rdev;
+	int id;
+	int ret;
+	int num;
+
+	if (!pdata || !pdata->num_regulator_pdata) {
+		dev_err(&pdev->dev, "Number of regulator is 0\n");
+		return -EINVAL;
+	}
+
+	pmic = devm_kzalloc(&pdev->dev,
+			pdata->num_regulator_pdata * sizeof(*pmic), GFP_KERNEL);
+	if (!pmic) {
+		dev_err(&pdev->dev, "mem alloc for pmic failed\n");
+		return -ENOMEM;
+	}
+
+	for (num = 0; num < pdata->num_regulator_pdata; ++num) {
+		tps_pdata = pdata->regulator_pdata[num];
+		if (!tps_pdata->reg_init_data) {
+			dev_err(&pdev->dev,
+				"No regulator init data for index %d\n", num);
+			ret = -EINVAL;
+			goto fail;
+		}
+
+		id = tps_pdata->regulator_id;
+		rinfo = find_regulator_info(id);
+		if (!rinfo) {
+			dev_err(&pdev->dev, "invalid regulator ID specified\n");
+			ret = -EINVAL;
+			goto fail;
+		}
+
+		ri = &pmic[num];
+		ri->rinfo = rinfo;
+		ri->dev = &pdev->dev;
+		if (tps_pdata->delay_us)
+			ri->delay = tps_pdata->delay_us;
+		else
+			ri->delay = rinfo->delay;
+		ri->tolerance_uv = tps_pdata->tolerance_uv;
+
+		check_smps_mode_mult(pdev->dev.parent, ri);
+		ri->platform_flags = tps_pdata->flags;
+		ri->ext_ctrl_flag = tps_pdata->ext_ctrl_flag;
+
+		ret = tps80031_cache_regulator_register(pdev->dev.parent, ri);
+		if (ret < 0) {
+			dev_err(&pdev->dev,
+				"Register cache failed, err %d\n", ret);
+			goto fail;
+		}
+		ret = tps80031_regulator_preinit(pdev->dev.parent, ri, tps_pdata);
+		if (ret < 0) {
+			dev_err(&pdev->dev,
+				"regulator preinit failed, err %d\n", ret);
+			goto fail;
+		}
+
+		ret = tps80031_power_req_config(pdev->dev.parent, ri, tps_pdata);
+		if (ret < 0) {
+			dev_err(&pdev->dev,
+				"power req config failed, err %d\n", ret);
+			goto fail;
+		}
+
+		rdev = regulator_register(&ri->rinfo->desc, &pdev->dev,
+				tps_pdata->reg_init_data, ri);
+		if (IS_ERR_OR_NULL(rdev)) {
+			dev_err(&pdev->dev,
+				"register regulator failed %s\n",
+					ri->rinfo->desc.name);
+			ret = PTR_ERR(rdev);
+			goto fail;
+		}
+		ri->rdev = rdev;
+	}
+
+	platform_set_drvdata(pdev, pmic);
+	return 0;
+fail:
+	while(--num >= 0) {
+		ri = &pmic[num];
+		regulator_unregister(ri->rdev);
+	}
+	return ret;
+}
+
+static int __devexit tps80031_regulator_remove(struct platform_device *pdev)
+{
+	struct tps80031_platform_data *pdata = pdev->dev.parent->platform_data;
+	struct tps80031_regulator *pmic = platform_get_drvdata(pdev);
+	struct tps80031_regulator *ri = NULL;
+	int num;
+
+	if (!pdata || !pdata->num_regulator_pdata)
+		return 0;
+
+	for (num = 0; num < pdata->num_regulator_pdata; ++num) {
+		ri = &pmic[num];
+		regulator_unregister(ri->rdev);
+	}
+	return 0;
+}
+
+static struct platform_driver tps80031_regulator_driver = {
+	.driver	= {
+		.name	= "tps80031-regulators",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= tps80031_regulator_probe,
+	.remove		= __devexit_p(tps80031_regulator_remove),
+};
+
+static int __init tps80031_regulator_init(void)
+{
+	return platform_driver_register(&tps80031_regulator_driver);
+}
+subsys_initcall(tps80031_regulator_init);
+
+static void __exit tps80031_regulator_exit(void)
+{
+	platform_driver_unregister(&tps80031_regulator_driver);
+}
+module_exit(tps80031_regulator_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Regulator Driver for TI TPS80031 PMIC");
+MODULE_ALIAS("platform:tps80031-regulator");
diff --git a/drivers/regulator/twl-regulator.c b/drivers/regulator/twl-regulator.c
index ee8747f4fa08..11cc308d66e9 100644
--- a/drivers/regulator/twl-regulator.c
+++ b/drivers/regulator/twl-regulator.c
@@ -71,6 +71,7 @@ struct twlreg_info {
 #define VREG_TYPE		1
 #define VREG_REMAP		2
 #define VREG_DEDICATED		3	/* LDO control */
+#define VREG_VOLTAGE_SMPS_4030	9
 /* TWL6030 register offsets */
 #define VREG_TRANS		1
 #define VREG_STATE		2
@@ -514,6 +515,32 @@ static struct regulator_ops twl4030ldo_ops = {
 	.get_status	= twl4030reg_get_status,
 };
 
+static int
+twl4030smps_set_voltage(struct regulator_dev *rdev, int min_uV, int max_uV,
+			unsigned *selector)
+{
+	struct twlreg_info *info = rdev_get_drvdata(rdev);
+	int vsel = DIV_ROUND_UP(min_uV - 600000, 12500);
+
+	twlreg_write(info, TWL_MODULE_PM_RECEIVER, VREG_VOLTAGE_SMPS_4030,
+		vsel);
+	return 0;
+}
+
+static int twl4030smps_get_voltage(struct regulator_dev *rdev)
+{
+	struct twlreg_info *info = rdev_get_drvdata(rdev);
+	int vsel = twlreg_read(info, TWL_MODULE_PM_RECEIVER,
+		VREG_VOLTAGE_SMPS_4030);
+
+	return vsel * 12500 + 600000;
+}
+
+static struct regulator_ops twl4030smps_ops = {
+	.set_voltage	= twl4030smps_set_voltage,
+	.get_voltage	= twl4030smps_get_voltage,
+};
+
 static int twl6030ldo_list_voltage(struct regulator_dev *rdev, unsigned index)
 {
 	struct twlreg_info	*info = rdev_get_drvdata(rdev);
@@ -856,6 +883,21 @@ static struct regulator_ops twlsmps_ops = {
 		}, \
 	}
 
+#define TWL4030_ADJUSTABLE_SMPS(label, offset, num, turnon_delay, remap_conf) \
+	{ \
+	.base = offset, \
+	.id = num, \
+	.delay = turnon_delay, \
+	.remap = remap_conf, \
+	.desc = { \
+		.name = #label, \
+		.id = TWL4030_REG_##label, \
+		.ops = &twl4030smps_ops, \
+		.type = REGULATOR_VOLTAGE, \
+		.owner = THIS_MODULE, \
+		}, \
+	}
+
 #define TWL6030_ADJUSTABLE_LDO(label, offset, min_mVolts, max_mVolts) { \
 	.base = offset, \
 	.min_mV = min_mVolts, \
@@ -947,8 +989,8 @@ static struct twlreg_info twl_regs[] = {
 	TWL4030_ADJUSTABLE_LDO(VINTANA2, 0x43, 12, 100, 0x08),
 	TWL4030_FIXED_LDO(VINTDIG, 0x47, 1500, 13, 100, 0x08),
 	TWL4030_ADJUSTABLE_LDO(VIO, 0x4b, 14, 1000, 0x08),
-	TWL4030_ADJUSTABLE_LDO(VDD1, 0x55, 15, 1000, 0x08),
-	TWL4030_ADJUSTABLE_LDO(VDD2, 0x63, 16, 1000, 0x08),
+	TWL4030_ADJUSTABLE_SMPS(VDD1, 0x55, 15, 1000, 0x08),
+	TWL4030_ADJUSTABLE_SMPS(VDD2, 0x63, 16, 1000, 0x08),
 	TWL4030_FIXED_LDO(VUSB1V5, 0x71, 1500, 17, 100, 0x08),
 	TWL4030_FIXED_LDO(VUSB1V8, 0x74, 1800, 18, 100, 0x08),
 	TWL4030_FIXED_LDO(VUSB3V1, 0x77, 3100, 19, 150, 0x08),
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index 5a538fc1cc85..45f4c8040845 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -106,6 +106,24 @@ config RTC_INTF_DEV_UIE_EMUL
 	  clock several times per second, please enable this option
 	  only if you know that you really need it.
 
+config RTC_INTF_ALARM
+	bool "Android alarm driver"
+	depends on RTC_CLASS
+	default y
+	help
+	  Provides non-wakeup and rtc backed wakeup alarms based on rtc or
+	  elapsed realtime, and a non-wakeup alarm on the monotonic clock.
+	  Also provides an interface to set the wall time which must be used
+	  for elapsed realtime to work.
+
+config RTC_INTF_ALARM_DEV
+	bool "Android alarm device"
+	depends on RTC_INTF_ALARM
+	default y
+	help
+	  Exports the alarm interface to user-space.
+
+
 config RTC_DRV_TEST
 	tristate "Test driver/device"
 	help
@@ -213,6 +231,26 @@ config RTC_DRV_MAX8998
 	  This driver can also be built as a module. If so, the module
 	  will be called rtc-max8998.
 
+config RTC_DRV_MAX8907C
+	tristate "Maxim MAX8907C"
+	depends on MFD_MAX8907C
+	help
+	  If you say yes here you will get support for the
+	  RTC of Maxim MAX8907C PMIC.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-max8907c.
+
+config RTC_DRV_MAX77663
+	tristate "Maxim MAX77663"
+	depends on MFD_MAX77663
+	help
+	  If you say yes here you will get support for the
+	  RTC of Maxim MAX77663 PMIC.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-max77663.
+
 config RTC_DRV_RS5C372
 	tristate "Ricoh R2025S/D, RS5C372A/B, RV5C386, RV5C387A"
 	help
@@ -304,6 +342,12 @@ config RTC_DRV_DM355EVM
 	help
 	  Supports the RTC firmware in the MSP430 on the DM355 EVM.
 
+config RTC_DRV_TPS6586X
+	tristate "TI TPS6586X RTC"
+	depends on MFD_TPS6586X
+	help
+	  This driver supports TPS6586X RTC
+
 config RTC_DRV_TWL92330
 	boolean "TI TWL92330/Menelaus"
 	depends on MENELAUS
@@ -1048,8 +1092,7 @@ config RTC_DRV_TEGRA
 	tristate "NVIDIA Tegra Internal RTC driver"
 	depends on RTC_CLASS && ARCH_TEGRA
 	help
-	  If you say yes here you get support for the
-	  Tegra 200 series internal RTC module.
+	  If you say yes here you get support for the Tegra internal RTC module.
 
 	  This drive can also be built as a module. If so, the module
 	  will be called rtc-tegra.
@@ -1061,6 +1104,26 @@ config RTC_DRV_TILE
 	  Enable support for the Linux driver side of the Tilera
 	  hypervisor's real-time clock interface.
 
+config RTC_DRV_TPS6591x
+	tristate "TPS6591x RTC driver"
+	depends on MFD_TPS6591X
+	default n
+	help
+	  If you say yes here you get support for the TPS6591x RTC module.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-tps6591x.
+
+config RTC_DRV_TPS80031
+	tristate "TPS80031 RTC driver"
+	depends on MFD_TPS80031
+	default n
+	help
+	  If you say yes here you get support for the TPS80031 RTC module.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-tps6591x.
+
 config RTC_DRV_PUV3
 	tristate "PKUnity v3 RTC support"
 	depends on ARCH_PUV3
@@ -1070,4 +1133,14 @@ config RTC_DRV_PUV3
 	  This drive can also be built as a module. If so, the module
 	  will be called rtc-puv3.
 
+config RTC_DRV_RC5T583
+	tristate "RICOH RC5T583 PMU RTC driver"
+	depends on MFD_RICOH583
+	default n
+	help
+	  If you say yes here you get support for the RICOH RC5T583 RTC module.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-rc5t583.
+
 endif # RTC_CLASS
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index 6e6982335c10..249c25d2df88 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -9,6 +9,8 @@ obj-$(CONFIG_RTC_HCTOSYS)	+= hctosys.o
 obj-$(CONFIG_RTC_CLASS)		+= rtc-core.o
 rtc-core-y			:= class.o interface.o
 
+obj-$(CONFIG_RTC_INTF_ALARM) += alarm.o
+obj-$(CONFIG_RTC_INTF_ALARM_DEV) += alarm-dev.o
 rtc-core-$(CONFIG_RTC_INTF_DEV)	+= rtc-dev.o
 rtc-core-$(CONFIG_RTC_INTF_PROC) += rtc-proc.o
 rtc-core-$(CONFIG_RTC_INTF_SYSFS) += rtc-sysfs.o
@@ -63,8 +65,10 @@ obj-$(CONFIG_RTC_MXC)		+= rtc-mxc.o
 obj-$(CONFIG_RTC_DRV_MAX6900)	+= rtc-max6900.o
 obj-$(CONFIG_RTC_DRV_MAX8925)	+= rtc-max8925.o
 obj-$(CONFIG_RTC_DRV_MAX8998)	+= rtc-max8998.o
+obj-$(CONFIG_RTC_DRV_MAX8907C)	+= rtc-max8907c.o
 obj-$(CONFIG_RTC_DRV_MAX6902)	+= rtc-max6902.o
 obj-$(CONFIG_RTC_DRV_MC13XXX)	+= rtc-mc13xxx.o
+obj-$(CONFIG_RTC_DRV_MAX77663)	+= rtc-max77663.o
 obj-$(CONFIG_RTC_DRV_MSM6242)	+= rtc-msm6242.o
 obj-$(CONFIG_RTC_DRV_MPC5121)	+= rtc-mpc5121.o
 obj-$(CONFIG_RTC_DRV_MV)	+= rtc-mv.o
@@ -98,9 +102,14 @@ obj-$(CONFIG_RTC_DRV_STARFIRE)	+= rtc-starfire.o
 obj-$(CONFIG_RTC_DRV_STK17TA8)	+= rtc-stk17ta8.o
 obj-$(CONFIG_RTC_DRV_STMP)	+= rtc-stmp3xxx.o
 obj-$(CONFIG_RTC_DRV_SUN4V)	+= rtc-sun4v.o
+CFLAGS_rtc-tegra.o               = -Werror
 obj-$(CONFIG_RTC_DRV_TEGRA)	+= rtc-tegra.o
 obj-$(CONFIG_RTC_DRV_TEST)	+= rtc-test.o
 obj-$(CONFIG_RTC_DRV_TILE)	+= rtc-tile.o
+obj-$(CONFIG_RTC_DRV_TPS6586X)	+= rtc-tps6586x.o
+obj-$(CONFIG_RTC_DRV_TPS6591x)	+= rtc-tps6591x.o
+obj-$(CONFIG_RTC_DRV_TPS80031)	+= rtc-tps80031.o
+obj-$(CONFIG_RTC_DRV_RC5T583)	+= rtc-ricoh583.o
 obj-$(CONFIG_RTC_DRV_TWL4030)	+= rtc-twl.o
 obj-$(CONFIG_RTC_DRV_TX4939)	+= rtc-tx4939.o
 obj-$(CONFIG_RTC_DRV_V3020)	+= rtc-v3020.o
diff --git a/drivers/rtc/alarm-dev.c b/drivers/rtc/alarm-dev.c
new file mode 100644
index 000000000000..686e6f7ed480
--- /dev/null
+++ b/drivers/rtc/alarm-dev.c
@@ -0,0 +1,286 @@
+/* drivers/rtc/alarm-dev.c
+ *
+ * Copyright (C) 2007-2009 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <asm/mach/time.h>
+#include <linux/android_alarm.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+#include <linux/fs.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/spinlock.h>
+#include <linux/sysdev.h>
+#include <linux/uaccess.h>
+#include <linux/wakelock.h>
+
+#define ANDROID_ALARM_PRINT_INFO (1U << 0)
+#define ANDROID_ALARM_PRINT_IO (1U << 1)
+#define ANDROID_ALARM_PRINT_INT (1U << 2)
+
+static int debug_mask = ANDROID_ALARM_PRINT_INFO;
+module_param_named(debug_mask, debug_mask, int, S_IRUGO | S_IWUSR | S_IWGRP);
+
+#define pr_alarm(debug_level_mask, args...) \
+	do { \
+		if (debug_mask & ANDROID_ALARM_PRINT_##debug_level_mask) { \
+			pr_info(args); \
+		} \
+	} while (0)
+
+#define ANDROID_ALARM_WAKEUP_MASK ( \
+	ANDROID_ALARM_RTC_WAKEUP_MASK | \
+	ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP_MASK)
+
+/* support old usespace code */
+#define ANDROID_ALARM_SET_OLD               _IOW('a', 2, time_t) /* set alarm */
+#define ANDROID_ALARM_SET_AND_WAIT_OLD      _IOW('a', 3, time_t)
+
+static int alarm_opened;
+static DEFINE_SPINLOCK(alarm_slock);
+static struct wake_lock alarm_wake_lock;
+static DECLARE_WAIT_QUEUE_HEAD(alarm_wait_queue);
+static uint32_t alarm_pending;
+static uint32_t alarm_enabled;
+static uint32_t wait_pending;
+
+static struct alarm alarms[ANDROID_ALARM_TYPE_COUNT];
+
+static long alarm_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	int rv = 0;
+	unsigned long flags;
+	struct timespec new_alarm_time;
+	struct timespec new_rtc_time;
+	struct timespec tmp_time;
+	enum android_alarm_type alarm_type = ANDROID_ALARM_IOCTL_TO_TYPE(cmd);
+	uint32_t alarm_type_mask = 1U << alarm_type;
+
+	if (alarm_type >= ANDROID_ALARM_TYPE_COUNT)
+		return -EINVAL;
+
+	if (ANDROID_ALARM_BASE_CMD(cmd) != ANDROID_ALARM_GET_TIME(0)) {
+		if ((file->f_flags & O_ACCMODE) == O_RDONLY)
+			return -EPERM;
+		if (file->private_data == NULL &&
+		    cmd != ANDROID_ALARM_SET_RTC) {
+			spin_lock_irqsave(&alarm_slock, flags);
+			if (alarm_opened) {
+				spin_unlock_irqrestore(&alarm_slock, flags);
+				return -EBUSY;
+			}
+			alarm_opened = 1;
+			file->private_data = (void *)1;
+			spin_unlock_irqrestore(&alarm_slock, flags);
+		}
+	}
+
+	switch (ANDROID_ALARM_BASE_CMD(cmd)) {
+	case ANDROID_ALARM_CLEAR(0):
+		spin_lock_irqsave(&alarm_slock, flags);
+		pr_alarm(IO, "alarm %d clear\n", alarm_type);
+		alarm_try_to_cancel(&alarms[alarm_type]);
+		if (alarm_pending) {
+			alarm_pending &= ~alarm_type_mask;
+			if (!alarm_pending && !wait_pending)
+				wake_unlock(&alarm_wake_lock);
+		}
+		alarm_enabled &= ~alarm_type_mask;
+		spin_unlock_irqrestore(&alarm_slock, flags);
+		break;
+
+	case ANDROID_ALARM_SET_OLD:
+	case ANDROID_ALARM_SET_AND_WAIT_OLD:
+		if (get_user(new_alarm_time.tv_sec, (int __user *)arg)) {
+			rv = -EFAULT;
+			goto err1;
+		}
+		new_alarm_time.tv_nsec = 0;
+		goto from_old_alarm_set;
+
+	case ANDROID_ALARM_SET_AND_WAIT(0):
+	case ANDROID_ALARM_SET(0):
+		if (copy_from_user(&new_alarm_time, (void __user *)arg,
+		    sizeof(new_alarm_time))) {
+			rv = -EFAULT;
+			goto err1;
+		}
+from_old_alarm_set:
+		spin_lock_irqsave(&alarm_slock, flags);
+		pr_alarm(IO, "alarm %d set %ld.%09ld\n", alarm_type,
+			new_alarm_time.tv_sec, new_alarm_time.tv_nsec);
+		alarm_enabled |= alarm_type_mask;
+		alarm_start_range(&alarms[alarm_type],
+			timespec_to_ktime(new_alarm_time),
+			timespec_to_ktime(new_alarm_time));
+		spin_unlock_irqrestore(&alarm_slock, flags);
+		if (ANDROID_ALARM_BASE_CMD(cmd) != ANDROID_ALARM_SET_AND_WAIT(0)
+		    && cmd != ANDROID_ALARM_SET_AND_WAIT_OLD)
+			break;
+		/* fall though */
+	case ANDROID_ALARM_WAIT:
+		spin_lock_irqsave(&alarm_slock, flags);
+		pr_alarm(IO, "alarm wait\n");
+		if (!alarm_pending && wait_pending) {
+			wake_unlock(&alarm_wake_lock);
+			wait_pending = 0;
+		}
+		spin_unlock_irqrestore(&alarm_slock, flags);
+		rv = wait_event_interruptible(alarm_wait_queue, alarm_pending);
+		if (rv)
+			goto err1;
+		spin_lock_irqsave(&alarm_slock, flags);
+		rv = alarm_pending;
+		wait_pending = 1;
+		alarm_pending = 0;
+		spin_unlock_irqrestore(&alarm_slock, flags);
+		break;
+	case ANDROID_ALARM_SET_RTC:
+		if (copy_from_user(&new_rtc_time, (void __user *)arg,
+		    sizeof(new_rtc_time))) {
+			rv = -EFAULT;
+			goto err1;
+		}
+		rv = alarm_set_rtc(new_rtc_time);
+		spin_lock_irqsave(&alarm_slock, flags);
+		alarm_pending |= ANDROID_ALARM_TIME_CHANGE_MASK;
+		wake_up(&alarm_wait_queue);
+		spin_unlock_irqrestore(&alarm_slock, flags);
+		if (rv < 0)
+			goto err1;
+		break;
+	case ANDROID_ALARM_GET_TIME(0):
+		switch (alarm_type) {
+		case ANDROID_ALARM_RTC_WAKEUP:
+		case ANDROID_ALARM_RTC:
+			getnstimeofday(&tmp_time);
+			break;
+		case ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP:
+		case ANDROID_ALARM_ELAPSED_REALTIME:
+			tmp_time =
+				ktime_to_timespec(alarm_get_elapsed_realtime());
+			break;
+		case ANDROID_ALARM_TYPE_COUNT:
+		case ANDROID_ALARM_SYSTEMTIME:
+			ktime_get_ts(&tmp_time);
+			break;
+		}
+		if (copy_to_user((void __user *)arg, &tmp_time,
+		    sizeof(tmp_time))) {
+			rv = -EFAULT;
+			goto err1;
+		}
+		break;
+
+	default:
+		rv = -EINVAL;
+		goto err1;
+	}
+err1:
+	return rv;
+}
+
+static int alarm_open(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+
+static int alarm_release(struct inode *inode, struct file *file)
+{
+	int i;
+	unsigned long flags;
+
+	spin_lock_irqsave(&alarm_slock, flags);
+	if (file->private_data != 0) {
+		for (i = 0; i < ANDROID_ALARM_TYPE_COUNT; i++) {
+			uint32_t alarm_type_mask = 1U << i;
+			if (alarm_enabled & alarm_type_mask) {
+				pr_alarm(INFO, "alarm_release: clear alarm, "
+					"pending %d\n",
+					!!(alarm_pending & alarm_type_mask));
+				alarm_enabled &= ~alarm_type_mask;
+			}
+			spin_unlock_irqrestore(&alarm_slock, flags);
+			alarm_cancel(&alarms[i]);
+			spin_lock_irqsave(&alarm_slock, flags);
+		}
+		if (alarm_pending | wait_pending) {
+			if (alarm_pending)
+				pr_alarm(INFO, "alarm_release: clear "
+					"pending alarms %x\n", alarm_pending);
+			wake_unlock(&alarm_wake_lock);
+			wait_pending = 0;
+			alarm_pending = 0;
+		}
+		alarm_opened = 0;
+	}
+	spin_unlock_irqrestore(&alarm_slock, flags);
+	return 0;
+}
+
+static void alarm_triggered(struct alarm *alarm)
+{
+	unsigned long flags;
+	uint32_t alarm_type_mask = 1U << alarm->type;
+
+	pr_alarm(INT, "alarm_triggered type %d\n", alarm->type);
+	spin_lock_irqsave(&alarm_slock, flags);
+	if (alarm_enabled & alarm_type_mask) {
+		wake_lock_timeout(&alarm_wake_lock, 5 * HZ);
+		alarm_enabled &= ~alarm_type_mask;
+		alarm_pending |= alarm_type_mask;
+		wake_up(&alarm_wait_queue);
+	}
+	spin_unlock_irqrestore(&alarm_slock, flags);
+}
+
+static const struct file_operations alarm_fops = {
+	.owner = THIS_MODULE,
+	.unlocked_ioctl = alarm_ioctl,
+	.open = alarm_open,
+	.release = alarm_release,
+};
+
+static struct miscdevice alarm_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "alarm",
+	.fops = &alarm_fops,
+};
+
+static int __init alarm_dev_init(void)
+{
+	int err;
+	int i;
+
+	err = misc_register(&alarm_device);
+	if (err)
+		return err;
+
+	for (i = 0; i < ANDROID_ALARM_TYPE_COUNT; i++)
+		alarm_init(&alarms[i], i, alarm_triggered);
+	wake_lock_init(&alarm_wake_lock, WAKE_LOCK_SUSPEND, "alarm");
+
+	return 0;
+}
+
+static void  __exit alarm_dev_exit(void)
+{
+	misc_deregister(&alarm_device);
+	wake_lock_destroy(&alarm_wake_lock);
+}
+
+module_init(alarm_dev_init);
+module_exit(alarm_dev_exit);
+
diff --git a/drivers/rtc/alarm.c b/drivers/rtc/alarm.c
new file mode 100644
index 000000000000..12e6f3c8acfb
--- /dev/null
+++ b/drivers/rtc/alarm.c
@@ -0,0 +1,591 @@
+/* drivers/rtc/alarm.c
+ *
+ * Copyright (C) 2007-2009 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <asm/mach/time.h>
+#include <linux/android_alarm.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+#include <linux/platform_device.h>
+#include <linux/rtc.h>
+#include <linux/sched.h>
+#include <linux/spinlock.h>
+#include <linux/sysdev.h>
+#include <linux/wakelock.h>
+
+#define ANDROID_ALARM_PRINT_ERROR (1U << 0)
+#define ANDROID_ALARM_PRINT_INIT_STATUS (1U << 1)
+#define ANDROID_ALARM_PRINT_TSET (1U << 2)
+#define ANDROID_ALARM_PRINT_CALL (1U << 3)
+#define ANDROID_ALARM_PRINT_SUSPEND (1U << 4)
+#define ANDROID_ALARM_PRINT_INT (1U << 5)
+#define ANDROID_ALARM_PRINT_FLOW (1U << 6)
+
+static int debug_mask = ANDROID_ALARM_PRINT_ERROR | \
+			ANDROID_ALARM_PRINT_SUSPEND | \
+			ANDROID_ALARM_PRINT_INIT_STATUS;
+module_param_named(debug_mask, debug_mask, int, S_IRUGO | S_IWUSR | S_IWGRP);
+
+#define pr_alarm(debug_level_mask, args...) \
+	do { \
+		if (debug_mask & ANDROID_ALARM_PRINT_##debug_level_mask) { \
+			pr_info(args); \
+		} \
+	} while (0)
+
+#define ANDROID_ALARM_WAKEUP_MASK ( \
+	ANDROID_ALARM_RTC_WAKEUP_MASK | \
+	ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP_MASK)
+
+/* support old usespace code */
+#define ANDROID_ALARM_SET_OLD               _IOW('a', 2, time_t) /* set alarm */
+#define ANDROID_ALARM_SET_AND_WAIT_OLD      _IOW('a', 3, time_t)
+
+struct alarm_queue {
+	struct rb_root alarms;
+	struct rb_node *first;
+	struct hrtimer timer;
+	ktime_t delta;
+	bool stopped;
+	ktime_t stopped_time;
+};
+
+static struct rtc_device *alarm_rtc_dev;
+static DEFINE_SPINLOCK(alarm_slock);
+static DEFINE_MUTEX(alarm_setrtc_mutex);
+static struct wake_lock alarm_rtc_wake_lock;
+static struct platform_device *alarm_platform_dev;
+struct alarm_queue alarms[ANDROID_ALARM_TYPE_COUNT];
+static bool suspended;
+
+static void update_timer_locked(struct alarm_queue *base, bool head_removed)
+{
+	struct alarm *alarm;
+	bool is_wakeup = base == &alarms[ANDROID_ALARM_RTC_WAKEUP] ||
+			base == &alarms[ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP];
+
+	if (base->stopped) {
+		pr_alarm(FLOW, "changed alarm while setting the wall time\n");
+		return;
+	}
+
+	if (is_wakeup && !suspended && head_removed)
+		wake_unlock(&alarm_rtc_wake_lock);
+
+	if (!base->first)
+		return;
+
+	alarm = container_of(base->first, struct alarm, node);
+
+	pr_alarm(FLOW, "selected alarm, type %d, func %pF at %lld\n",
+		alarm->type, alarm->function, ktime_to_ns(alarm->expires));
+
+	if (is_wakeup && suspended) {
+		pr_alarm(FLOW, "changed alarm while suspened\n");
+		wake_lock_timeout(&alarm_rtc_wake_lock, 1 * HZ);
+		return;
+	}
+
+	hrtimer_try_to_cancel(&base->timer);
+	base->timer.node.expires = ktime_add(base->delta, alarm->expires);
+	base->timer._softexpires = ktime_add(base->delta, alarm->softexpires);
+	hrtimer_start_expires(&base->timer, HRTIMER_MODE_ABS);
+}
+
+static void alarm_enqueue_locked(struct alarm *alarm)
+{
+	struct alarm_queue *base = &alarms[alarm->type];
+	struct rb_node **link = &base->alarms.rb_node;
+	struct rb_node *parent = NULL;
+	struct alarm *entry;
+	int leftmost = 1;
+	bool was_first = false;
+
+	pr_alarm(FLOW, "added alarm, type %d, func %pF at %lld\n",
+		alarm->type, alarm->function, ktime_to_ns(alarm->expires));
+
+	if (base->first == &alarm->node) {
+		base->first = rb_next(&alarm->node);
+		was_first = true;
+	}
+	if (!RB_EMPTY_NODE(&alarm->node)) {
+		rb_erase(&alarm->node, &base->alarms);
+		RB_CLEAR_NODE(&alarm->node);
+	}
+
+	while (*link) {
+		parent = *link;
+		entry = rb_entry(parent, struct alarm, node);
+		/*
+		* We dont care about collisions. Nodes with
+		* the same expiry time stay together.
+		*/
+		if (alarm->expires.tv64 < entry->expires.tv64) {
+			link = &(*link)->rb_left;
+		} else {
+			link = &(*link)->rb_right;
+			leftmost = 0;
+		}
+	}
+	if (leftmost)
+		base->first = &alarm->node;
+	if (leftmost || was_first)
+		update_timer_locked(base, was_first);
+
+	rb_link_node(&alarm->node, parent, link);
+	rb_insert_color(&alarm->node, &base->alarms);
+}
+
+/**
+ * alarm_init - initialize an alarm
+ * @alarm:	the alarm to be initialized
+ * @type:	the alarm type to be used
+ * @function:	alarm callback function
+ */
+void alarm_init(struct alarm *alarm,
+	enum android_alarm_type type, void (*function)(struct alarm *))
+{
+	RB_CLEAR_NODE(&alarm->node);
+	alarm->type = type;
+	alarm->function = function;
+
+	pr_alarm(FLOW, "created alarm, type %d, func %pF\n", type, function);
+}
+
+
+/**
+ * alarm_start_range - (re)start an alarm
+ * @alarm:	the alarm to be added
+ * @start:	earliest expiry time
+ * @end:	expiry time
+ */
+void alarm_start_range(struct alarm *alarm, ktime_t start, ktime_t end)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&alarm_slock, flags);
+	alarm->softexpires = start;
+	alarm->expires = end;
+	alarm_enqueue_locked(alarm);
+	spin_unlock_irqrestore(&alarm_slock, flags);
+}
+
+/**
+ * alarm_try_to_cancel - try to deactivate an alarm
+ * @alarm:	alarm to stop
+ *
+ * Returns:
+ *  0 when the alarm was not active
+ *  1 when the alarm was active
+ * -1 when the alarm may currently be excuting the callback function and
+ *    cannot be stopped (it may also be inactive)
+ */
+int alarm_try_to_cancel(struct alarm *alarm)
+{
+	struct alarm_queue *base = &alarms[alarm->type];
+	unsigned long flags;
+	bool first = false;
+	int ret = 0;
+
+	spin_lock_irqsave(&alarm_slock, flags);
+	if (!RB_EMPTY_NODE(&alarm->node)) {
+		pr_alarm(FLOW, "canceled alarm, type %d, func %pF at %lld\n",
+			alarm->type, alarm->function,
+			ktime_to_ns(alarm->expires));
+		ret = 1;
+		if (base->first == &alarm->node) {
+			base->first = rb_next(&alarm->node);
+			first = true;
+		}
+		rb_erase(&alarm->node, &base->alarms);
+		RB_CLEAR_NODE(&alarm->node);
+		if (first)
+			update_timer_locked(base, true);
+	} else
+		pr_alarm(FLOW, "tried to cancel alarm, type %d, func %pF\n",
+			alarm->type, alarm->function);
+	spin_unlock_irqrestore(&alarm_slock, flags);
+	if (!ret && hrtimer_callback_running(&base->timer))
+		ret = -1;
+	return ret;
+}
+
+/**
+ * alarm_cancel - cancel an alarm and wait for the handler to finish.
+ * @alarm:	the alarm to be cancelled
+ *
+ * Returns:
+ *  0 when the alarm was not active
+ *  1 when the alarm was active
+ */
+int alarm_cancel(struct alarm *alarm)
+{
+	for (;;) {
+		int ret = alarm_try_to_cancel(alarm);
+		if (ret >= 0)
+			return ret;
+		cpu_relax();
+	}
+}
+
+/**
+ * alarm_set_rtc - set the kernel and rtc walltime
+ * @new_time:	timespec value containing the new time
+ */
+int alarm_set_rtc(struct timespec new_time)
+{
+	int i;
+	int ret;
+	unsigned long flags;
+	struct rtc_time rtc_new_rtc_time;
+	struct timespec tmp_time;
+
+	rtc_time_to_tm(new_time.tv_sec, &rtc_new_rtc_time);
+
+	pr_alarm(TSET, "set rtc %ld %ld - rtc %02d:%02d:%02d %02d/%02d/%04d\n",
+		new_time.tv_sec, new_time.tv_nsec,
+		rtc_new_rtc_time.tm_hour, rtc_new_rtc_time.tm_min,
+		rtc_new_rtc_time.tm_sec, rtc_new_rtc_time.tm_mon + 1,
+		rtc_new_rtc_time.tm_mday,
+		rtc_new_rtc_time.tm_year + 1900);
+
+	mutex_lock(&alarm_setrtc_mutex);
+	spin_lock_irqsave(&alarm_slock, flags);
+	wake_lock(&alarm_rtc_wake_lock);
+	getnstimeofday(&tmp_time);
+	for (i = 0; i < ANDROID_ALARM_SYSTEMTIME; i++) {
+		hrtimer_try_to_cancel(&alarms[i].timer);
+		alarms[i].stopped = true;
+		alarms[i].stopped_time = timespec_to_ktime(tmp_time);
+	}
+	alarms[ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP].delta =
+		alarms[ANDROID_ALARM_ELAPSED_REALTIME].delta =
+		ktime_sub(alarms[ANDROID_ALARM_ELAPSED_REALTIME].delta,
+			timespec_to_ktime(timespec_sub(tmp_time, new_time)));
+	spin_unlock_irqrestore(&alarm_slock, flags);
+	ret = do_settimeofday(&new_time);
+	spin_lock_irqsave(&alarm_slock, flags);
+	for (i = 0; i < ANDROID_ALARM_SYSTEMTIME; i++) {
+		alarms[i].stopped = false;
+		update_timer_locked(&alarms[i], false);
+	}
+	spin_unlock_irqrestore(&alarm_slock, flags);
+	if (ret < 0) {
+		pr_alarm(ERROR, "alarm_set_rtc: Failed to set time\n");
+		goto err;
+	}
+	if (!alarm_rtc_dev) {
+		pr_alarm(ERROR,
+			"alarm_set_rtc: no RTC, time will be lost on reboot\n");
+		goto err;
+	}
+	ret = rtc_set_time(alarm_rtc_dev, &rtc_new_rtc_time);
+	if (ret < 0)
+		pr_alarm(ERROR, "alarm_set_rtc: "
+			"Failed to set RTC, time will be lost on reboot\n");
+err:
+	wake_unlock(&alarm_rtc_wake_lock);
+	mutex_unlock(&alarm_setrtc_mutex);
+	return ret;
+}
+
+/**
+ * alarm_get_elapsed_realtime - get the elapsed real time in ktime_t format
+ *
+ * returns the time in ktime_t format
+ */
+ktime_t alarm_get_elapsed_realtime(void)
+{
+	ktime_t now;
+	unsigned long flags;
+	struct alarm_queue *base = &alarms[ANDROID_ALARM_ELAPSED_REALTIME];
+
+	spin_lock_irqsave(&alarm_slock, flags);
+	now = base->stopped ? base->stopped_time : ktime_get_real();
+	now = ktime_sub(now, base->delta);
+	spin_unlock_irqrestore(&alarm_slock, flags);
+	return now;
+}
+
+static enum hrtimer_restart alarm_timer_triggered(struct hrtimer *timer)
+{
+	struct alarm_queue *base;
+	struct alarm *alarm;
+	unsigned long flags;
+	ktime_t now;
+
+	spin_lock_irqsave(&alarm_slock, flags);
+
+	base = container_of(timer, struct alarm_queue, timer);
+	now = base->stopped ? base->stopped_time : hrtimer_cb_get_time(timer);
+	now = ktime_sub(now, base->delta);
+
+	pr_alarm(INT, "alarm_timer_triggered type %d at %lld\n",
+		base - alarms, ktime_to_ns(now));
+
+	while (base->first) {
+		alarm = container_of(base->first, struct alarm, node);
+		if (alarm->softexpires.tv64 > now.tv64) {
+			pr_alarm(FLOW, "don't call alarm, %pF, %lld (s %lld)\n",
+				alarm->function, ktime_to_ns(alarm->expires),
+				ktime_to_ns(alarm->softexpires));
+			break;
+		}
+		base->first = rb_next(&alarm->node);
+		rb_erase(&alarm->node, &base->alarms);
+		RB_CLEAR_NODE(&alarm->node);
+		pr_alarm(CALL, "call alarm, type %d, func %pF, %lld (s %lld)\n",
+			alarm->type, alarm->function,
+			ktime_to_ns(alarm->expires),
+			ktime_to_ns(alarm->softexpires));
+		spin_unlock_irqrestore(&alarm_slock, flags);
+		alarm->function(alarm);
+		spin_lock_irqsave(&alarm_slock, flags);
+	}
+	if (!base->first)
+		pr_alarm(FLOW, "no more alarms of type %d\n", base - alarms);
+	update_timer_locked(base, true);
+	spin_unlock_irqrestore(&alarm_slock, flags);
+	return HRTIMER_NORESTART;
+}
+
+static void alarm_triggered_func(void *p)
+{
+	struct rtc_device *rtc = alarm_rtc_dev;
+	if (!(rtc->irq_data & RTC_AF))
+		return;
+	pr_alarm(INT, "rtc alarm triggered\n");
+	wake_lock_timeout(&alarm_rtc_wake_lock, 1 * HZ);
+}
+
+static int alarm_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	int                 err = 0;
+	unsigned long       flags;
+	struct rtc_wkalrm   rtc_alarm;
+	struct rtc_time     rtc_current_rtc_time;
+	unsigned long       rtc_current_time;
+	unsigned long       rtc_alarm_time;
+	struct timespec     rtc_delta;
+	struct timespec     wall_time;
+	struct alarm_queue *wakeup_queue = NULL;
+	struct alarm_queue *tmp_queue = NULL;
+
+	pr_alarm(SUSPEND, "alarm_suspend(%p, %d)\n", pdev, state.event);
+
+	spin_lock_irqsave(&alarm_slock, flags);
+	suspended = true;
+	spin_unlock_irqrestore(&alarm_slock, flags);
+
+	hrtimer_cancel(&alarms[ANDROID_ALARM_RTC_WAKEUP].timer);
+	hrtimer_cancel(&alarms[
+			ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP].timer);
+
+	tmp_queue = &alarms[ANDROID_ALARM_RTC_WAKEUP];
+	if (tmp_queue->first)
+		wakeup_queue = tmp_queue;
+	tmp_queue = &alarms[ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP];
+	if (tmp_queue->first && (!wakeup_queue ||
+				hrtimer_get_expires(&tmp_queue->timer).tv64 <
+				hrtimer_get_expires(&wakeup_queue->timer).tv64))
+		wakeup_queue = tmp_queue;
+	if (wakeup_queue) {
+		rtc_read_time(alarm_rtc_dev, &rtc_current_rtc_time);
+		getnstimeofday(&wall_time);
+		rtc_tm_to_time(&rtc_current_rtc_time, &rtc_current_time);
+		set_normalized_timespec(&rtc_delta,
+					wall_time.tv_sec - rtc_current_time,
+					wall_time.tv_nsec);
+
+		rtc_alarm_time = timespec_sub(ktime_to_timespec(
+			hrtimer_get_expires(&wakeup_queue->timer)),
+			rtc_delta).tv_sec;
+
+		rtc_time_to_tm(rtc_alarm_time, &rtc_alarm.time);
+		rtc_alarm.enabled = 1;
+		rtc_set_alarm(alarm_rtc_dev, &rtc_alarm);
+		rtc_read_time(alarm_rtc_dev, &rtc_current_rtc_time);
+		rtc_tm_to_time(&rtc_current_rtc_time, &rtc_current_time);
+		pr_alarm(SUSPEND,
+			"rtc alarm set at %ld, now %ld, rtc delta %ld.%09ld\n",
+			rtc_alarm_time, rtc_current_time,
+			rtc_delta.tv_sec, rtc_delta.tv_nsec);
+		if (rtc_current_time + 1 >= rtc_alarm_time) {
+			pr_alarm(SUSPEND, "alarm about to go off\n");
+			memset(&rtc_alarm, 0, sizeof(rtc_alarm));
+			rtc_alarm.enabled = 0;
+			rtc_set_alarm(alarm_rtc_dev, &rtc_alarm);
+
+			spin_lock_irqsave(&alarm_slock, flags);
+			suspended = false;
+			wake_lock_timeout(&alarm_rtc_wake_lock, 2 * HZ);
+			update_timer_locked(&alarms[ANDROID_ALARM_RTC_WAKEUP],
+									false);
+			update_timer_locked(&alarms[
+				ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP], false);
+			err = -EBUSY;
+			spin_unlock_irqrestore(&alarm_slock, flags);
+		}
+	}
+	return err;
+}
+
+static int alarm_resume(struct platform_device *pdev)
+{
+	struct rtc_wkalrm alarm;
+	unsigned long       flags;
+
+	pr_alarm(SUSPEND, "alarm_resume(%p)\n", pdev);
+
+	memset(&alarm, 0, sizeof(alarm));
+	alarm.enabled = 0;
+	rtc_set_alarm(alarm_rtc_dev, &alarm);
+
+	spin_lock_irqsave(&alarm_slock, flags);
+	suspended = false;
+	update_timer_locked(&alarms[ANDROID_ALARM_RTC_WAKEUP], false);
+	update_timer_locked(&alarms[ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP],
+									false);
+	spin_unlock_irqrestore(&alarm_slock, flags);
+
+	return 0;
+}
+
+static struct rtc_task alarm_rtc_task = {
+	.func = alarm_triggered_func
+};
+
+static int rtc_alarm_add_device(struct device *dev,
+				struct class_interface *class_intf)
+{
+	int err;
+	struct rtc_device *rtc = to_rtc_device(dev);
+
+	mutex_lock(&alarm_setrtc_mutex);
+
+	if (alarm_rtc_dev) {
+		err = -EBUSY;
+		goto err1;
+	}
+
+	alarm_platform_dev =
+		platform_device_register_simple("alarm", -1, NULL, 0);
+	if (IS_ERR(alarm_platform_dev)) {
+		err = PTR_ERR(alarm_platform_dev);
+		goto err2;
+	}
+	err = rtc_irq_register(rtc, &alarm_rtc_task);
+	if (err)
+		goto err3;
+	alarm_rtc_dev = rtc;
+	pr_alarm(INIT_STATUS, "using rtc device, %s, for alarms", rtc->name);
+	mutex_unlock(&alarm_setrtc_mutex);
+
+	return 0;
+
+err3:
+	platform_device_unregister(alarm_platform_dev);
+err2:
+err1:
+	mutex_unlock(&alarm_setrtc_mutex);
+	return err;
+}
+
+static void rtc_alarm_remove_device(struct device *dev,
+				    struct class_interface *class_intf)
+{
+	if (dev == &alarm_rtc_dev->dev) {
+		pr_alarm(INIT_STATUS, "lost rtc device for alarms");
+		rtc_irq_unregister(alarm_rtc_dev, &alarm_rtc_task);
+		platform_device_unregister(alarm_platform_dev);
+		alarm_rtc_dev = NULL;
+	}
+}
+
+static struct class_interface rtc_alarm_interface = {
+	.add_dev = &rtc_alarm_add_device,
+	.remove_dev = &rtc_alarm_remove_device,
+};
+
+static struct platform_driver alarm_driver = {
+	.suspend = alarm_suspend,
+	.resume = alarm_resume,
+	.driver = {
+		.name = "alarm"
+	}
+};
+
+static int __init alarm_late_init(void)
+{
+	unsigned long   flags;
+	struct timespec tmp_time, system_time;
+
+	/* this needs to run after the rtc is read at boot */
+	spin_lock_irqsave(&alarm_slock, flags);
+	/* We read the current rtc and system time so we can later calulate
+	 * elasped realtime to be (boot_systemtime + rtc - boot_rtc) ==
+	 * (rtc - (boot_rtc - boot_systemtime))
+	 */
+	getnstimeofday(&tmp_time);
+	ktime_get_ts(&system_time);
+	alarms[ANDROID_ALARM_ELAPSED_REALTIME_WAKEUP].delta =
+		alarms[ANDROID_ALARM_ELAPSED_REALTIME].delta =
+			timespec_to_ktime(timespec_sub(tmp_time, system_time));
+
+	spin_unlock_irqrestore(&alarm_slock, flags);
+	return 0;
+}
+
+static int __init alarm_driver_init(void)
+{
+	int err;
+	int i;
+
+	for (i = 0; i < ANDROID_ALARM_SYSTEMTIME; i++) {
+		hrtimer_init(&alarms[i].timer,
+				CLOCK_REALTIME, HRTIMER_MODE_ABS);
+		alarms[i].timer.function = alarm_timer_triggered;
+	}
+	hrtimer_init(&alarms[ANDROID_ALARM_SYSTEMTIME].timer,
+		     CLOCK_MONOTONIC, HRTIMER_MODE_ABS);
+	alarms[ANDROID_ALARM_SYSTEMTIME].timer.function = alarm_timer_triggered;
+	err = platform_driver_register(&alarm_driver);
+	if (err < 0)
+		goto err1;
+	wake_lock_init(&alarm_rtc_wake_lock, WAKE_LOCK_SUSPEND, "alarm_rtc");
+	rtc_alarm_interface.class = rtc_class;
+	err = class_interface_register(&rtc_alarm_interface);
+	if (err < 0)
+		goto err2;
+
+	return 0;
+
+err2:
+	wake_lock_destroy(&alarm_rtc_wake_lock);
+	platform_driver_unregister(&alarm_driver);
+err1:
+	return err;
+}
+
+static void  __exit alarm_exit(void)
+{
+	class_interface_unregister(&rtc_alarm_interface);
+	wake_lock_destroy(&alarm_rtc_wake_lock);
+	platform_driver_unregister(&alarm_driver);
+}
+
+late_initcall(alarm_late_init);
+module_init(alarm_driver_init);
+module_exit(alarm_exit);
+
diff --git a/drivers/rtc/class.c b/drivers/rtc/class.c
index 01a7df5317c1..b82a1554cdc1 100644
--- a/drivers/rtc/class.c
+++ b/drivers/rtc/class.c
@@ -66,7 +66,7 @@ static int rtc_suspend(struct device *dev, pm_message_t mesg)
 	 */
 	delta = timespec_sub(old_system, old_rtc);
 	delta_delta = timespec_sub(delta, old_delta);
-	if (abs(delta_delta.tv_sec)  >= 2) {
+	if (delta_delta.tv_sec < -2 || delta_delta.tv_sec >= 2) {
 		/*
 		 * if delta_delta is too large, assume time correction
 		 * has occured and set old_delta to the current delta.
@@ -100,9 +100,8 @@ static int rtc_resume(struct device *dev)
 	rtc_tm_to_time(&tm, &new_rtc.tv_sec);
 	new_rtc.tv_nsec = 0;
 
-	if (new_rtc.tv_sec <= old_rtc.tv_sec) {
-		if (new_rtc.tv_sec < old_rtc.tv_sec)
-			pr_debug("%s:  time travel!\n", dev_name(&rtc->dev));
+	if (new_rtc.tv_sec < old_rtc.tv_sec) {
+		pr_debug("%s:  time travel!\n", dev_name(&rtc->dev));
 		return 0;
 	}
 
@@ -119,7 +118,8 @@ static int rtc_resume(struct device *dev)
 	sleep_time = timespec_sub(sleep_time,
 			timespec_sub(new_system, old_system));
 
-	timekeeping_inject_sleeptime(&sleep_time);
+	if (sleep_time.tv_sec >= 0)
+		timekeeping_inject_sleeptime(&sleep_time);
 	return 0;
 }
 
diff --git a/drivers/rtc/interface.c b/drivers/rtc/interface.c
index 44e91e598f8d..a86f3013747b 100644
--- a/drivers/rtc/interface.c
+++ b/drivers/rtc/interface.c
@@ -227,11 +227,11 @@ int __rtc_read_alarm(struct rtc_device *rtc, struct rtc_wkalrm *alarm)
 		alarm->time.tm_hour = now.tm_hour;
 
 	/* For simplicity, only support date rollover for now */
-	if (alarm->time.tm_mday == -1) {
+	if (alarm->time.tm_mday < 1 || alarm->time.tm_mday > 31) {
 		alarm->time.tm_mday = now.tm_mday;
 		missing = day;
 	}
-	if (alarm->time.tm_mon == -1) {
+	if ((unsigned)alarm->time.tm_mon >= 12) {
 		alarm->time.tm_mon = now.tm_mon;
 		if (missing == none)
 			missing = month;
diff --git a/drivers/rtc/rtc-m41t80.c b/drivers/rtc/rtc-m41t80.c
index eda128fc1d38..64aedd8cc095 100644
--- a/drivers/rtc/rtc-m41t80.c
+++ b/drivers/rtc/rtc-m41t80.c
@@ -357,10 +357,19 @@ static int m41t80_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *t)
 static struct rtc_class_ops m41t80_rtc_ops = {
 	.read_time = m41t80_rtc_read_time,
 	.set_time = m41t80_rtc_set_time,
+	/*
+	 * XXX - m41t80 alarm functionality is reported broken.
+	 * until it is fixed, don't register alarm functions.
+	 *
 	.read_alarm = m41t80_rtc_read_alarm,
 	.set_alarm = m41t80_rtc_set_alarm,
+	*/
 	.proc = m41t80_rtc_proc,
+	/*
+	 * See above comment on broken alarm
+	 *
 	.alarm_irq_enable = m41t80_rtc_alarm_irq_enable,
+	*/
 };
 
 #if defined(CONFIG_RTC_INTF_SYSFS) || defined(CONFIG_RTC_INTF_SYSFS_MODULE)
diff --git a/drivers/rtc/rtc-max77663.c b/drivers/rtc/rtc-max77663.c
new file mode 100644
index 000000000000..d6dac76944d6
--- /dev/null
+++ b/drivers/rtc/rtc-max77663.c
@@ -0,0 +1,636 @@
+/*
+ * drivers/rtc/rtc-max77663.c
+ * Max77663 RTC driver
+ *
+ * Copyright 2011-2012, Maxim Integrated Products, Inc.
+ * Copyright (c) 2011-2012, NVIDIA 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.
+ *
+ */
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/rtc.h>
+#include <linux/mfd/max77663-core.h>
+
+/* RTC Registers */
+#define MAX77663_RTC_IRQ		0x00
+#define MAX77663_RTC_IRQ_MASK		0x01
+#define MAX77663_RTC_CTRL_MODE		0x02
+#define MAX77663_RTC_CTRL		0x03
+#define MAX77663_RTC_UPDATE0		0x04
+#define MAX77663_RTC_UPDATE1		0x05
+#define MAX77663_RTC_SEC		0x07
+#define MAX77663_RTC_MIN		0x08
+#define MAX77663_RTC_HOUR		0x09
+#define MAX77663_RTC_WEEKDAY		0x0A
+#define MAX77663_RTC_MONTH		0x0B
+#define MAX77663_RTC_YEAR		0x0C
+#define MAX77663_RTC_MONTHDAY		0x0D
+#define MAX77663_RTC_ALARM_SEC1		0x0E
+#define MAX77663_RTC_ALARM_MIN1		0x0F
+#define MAX77663_RTC_ALARM_HOUR1	0x10
+#define MAX77663_RTC_ALARM_WEEKDAY1	0x11
+#define MAX77663_RTC_ALARM_MONTH1	0x12
+#define MAX77663_RTC_ALARM_YEAR1	0x13
+#define MAX77663_RTC_ALARM_MONTHDAY1	0x14
+
+#define RTC_IRQ_60SEC_MASK		(1 << 0)
+#define RTC_IRQ_ALARM1_MASK		(1 << 1)
+#define RTC_IRQ_ALARM2_MASK		(1 << 2)
+#define RTC_IRQ_SMPL_MASK		(1 << 3)
+#define RTC_IRQ_1SEC_MASK		(1 << 4)
+#define RTC_IRQ_MASK			0x1F
+
+#define BCD_MODE_MASK			(1 << 0)
+#define HR_MODE_MASK			(1 << 1)
+
+#define WB_UPDATE_MASK			(1 << 0)
+#define FLAG_AUTO_CLEAR_MASK		(1 << 1)
+#define FREEZE_SEC_MASK			(1 << 2)
+#define RTC_WAKE_MASK			(1 << 3)
+#define RB_UPDATE_MASK			(1 << 4)
+
+#define WB_UPDATE_FLAG_MASK		(1 << 0)
+#define RB_UPDATE_FLAG_MASK		(1 << 1)
+
+#define SEC_MASK			0x7F
+#define MIN_MASK			0x7F
+#define HOUR_MASK			0x3F
+#define WEEKDAY_MASK			0x7F
+#define MONTH_MASK			0x1F
+#define YEAR_MASK			0xFF
+#define MONTHDAY_MASK			0x3F
+
+#define ALARM_EN_MASK			0x80
+#define ALARM_EN_SHIFT			7
+
+#define RTC_YEAR_BASE			100
+#define RTC_YEAR_MAX			99
+
+/* ON/OFF Registers */
+#define MAX77663_REG_ONOFF_CFG2		0x42
+
+#define ONOFF_WK_ALARM1_MASK		(1 << 2)
+
+enum {
+	RTC_SEC,
+	RTC_MIN,
+	RTC_HOUR,
+	RTC_WEEKDAY,
+	RTC_MONTH,
+	RTC_YEAR,
+	RTC_MONTHDAY,
+	RTC_NR
+};
+
+struct max77663_rtc {
+	struct rtc_device *rtc;
+	struct device *dev;
+
+	struct mutex io_lock;
+	int irq;
+	u8 irq_mask;
+	bool shutdown_ongoing;
+};
+
+static inline struct device *_to_parent(struct max77663_rtc *rtc)
+{
+	return rtc->dev->parent;
+}
+
+static inline int max77663_rtc_update_buffer(struct max77663_rtc *rtc,
+					     int write)
+{
+	struct device *parent = _to_parent(rtc);
+	u8 val =  FLAG_AUTO_CLEAR_MASK | RTC_WAKE_MASK;
+	int ret;
+
+	if (write)
+		val |= WB_UPDATE_MASK;
+	else
+		val |= RB_UPDATE_MASK;
+
+	dev_dbg(rtc->dev, "rtc_update_buffer: write=%d, addr=0x%x, val=0x%x\n",
+		write, MAX77663_RTC_UPDATE0, val);
+	ret = max77663_write(parent, MAX77663_RTC_UPDATE0, &val, 1, 1);
+	if (ret < 0) {
+		dev_err(rtc->dev, "rtc_update_buffer: "
+			"Failed to get rtc update0\n");
+		return ret;
+	}
+
+	/*
+	 * Must wait 14ms for buffer update.
+	 * If the sleeping time is 10us - 20ms, usleep_range() is recommended.
+	 * Please refer Documentation/timers/timers-howto.txt.
+	 */
+	usleep_range(14000, 14000);
+
+	return 0;
+}
+
+static inline int max77663_rtc_write(struct max77663_rtc *rtc, u8 addr,
+				     void *values, u32 len, int update_buffer)
+{
+	struct device *parent = _to_parent(rtc);
+	int ret;
+
+	mutex_lock(&rtc->io_lock);
+
+	dev_dbg(rtc->dev, "rtc_write: addr=0x%x, values=0x%x, len=%u, "
+		"update_buffer=%d\n",
+		addr, *((u8 *)values), len, update_buffer);
+	ret = max77663_write(parent, addr, values, len, 1);
+	if (ret < 0)
+		goto out;
+
+	if (update_buffer)
+		ret = max77663_rtc_update_buffer(rtc, 1);
+
+out:
+	mutex_unlock(&rtc->io_lock);
+	return ret;
+}
+
+static inline int max77663_rtc_read(struct max77663_rtc *rtc, u8 addr,
+				    void *values, u32 len, int update_buffer)
+{
+	struct device *parent = _to_parent(rtc);
+	int ret;
+
+	mutex_lock(&rtc->io_lock);
+
+	if (update_buffer) {
+		ret = max77663_rtc_update_buffer(rtc, 0);
+		if (ret < 0)
+			goto out;
+	}
+
+	ret = max77663_read(parent, addr, values, len, 1);
+	dev_dbg(rtc->dev, "rtc_read: addr=0x%x, values=0x%x, len=%u, "
+		"update_buffer=%d\n",
+		addr, *((u8 *)values), len, update_buffer);
+
+out:
+	mutex_unlock(&rtc->io_lock);
+	return ret;
+}
+
+static inline int max77663_rtc_reg_to_tm(struct max77663_rtc *rtc, u8 *buf,
+					 struct rtc_time *tm)
+{
+	int wday = buf[RTC_WEEKDAY] & WEEKDAY_MASK;
+
+	if (unlikely(!wday)) {
+		dev_err(rtc->dev,
+			"rtc_reg_to_tm: Invalid day of week, %d\n", wday);
+		return -EINVAL;
+	}
+
+	tm->tm_sec = (int)(buf[RTC_SEC] & SEC_MASK);
+	tm->tm_min = (int)(buf[RTC_MIN] & MIN_MASK);
+	tm->tm_hour = (int)(buf[RTC_HOUR] & HOUR_MASK);
+	tm->tm_mday = (int)(buf[RTC_MONTHDAY] & MONTHDAY_MASK);
+	tm->tm_mon = (int)(buf[RTC_MONTH] & MONTH_MASK) - 1;
+	tm->tm_year = (int)(buf[RTC_YEAR] & YEAR_MASK) + RTC_YEAR_BASE;
+	tm->tm_wday = ffs(wday) - 1;
+
+	return 0;
+}
+
+static inline int max77663_rtc_tm_to_reg(struct max77663_rtc *rtc, u8 *buf,
+					 struct rtc_time *tm, int alarm)
+{
+	u8 alarm_mask = alarm ? ALARM_EN_MASK : 0;
+
+	if (unlikely((tm->tm_year < RTC_YEAR_BASE) ||
+			(tm->tm_year > RTC_YEAR_BASE + RTC_YEAR_MAX))) {
+		dev_err(rtc->dev,
+			"rtc_tm_to_reg: Invalid year, %d\n", tm->tm_year);
+		return -EINVAL;
+	}
+
+	buf[RTC_SEC] = tm->tm_sec | alarm_mask;
+	buf[RTC_MIN] = tm->tm_min | alarm_mask;
+	buf[RTC_HOUR] = tm->tm_hour | alarm_mask;
+	buf[RTC_MONTHDAY] = tm->tm_mday | alarm_mask;
+	buf[RTC_MONTH] = (tm->tm_mon + 1) | alarm_mask;
+	buf[RTC_YEAR] = (tm->tm_year - RTC_YEAR_BASE) | alarm_mask;
+
+	/* The wday is configured only when disabled alarm. */
+	if (!alarm)
+		buf[RTC_WEEKDAY] = (1 << tm->tm_wday);
+	else {
+	/* Configure its default reset value 0x01, and not enable it. */
+		buf[RTC_WEEKDAY] = 0x01;
+	}
+	return 0;
+}
+
+static inline int max77663_rtc_irq_mask(struct max77663_rtc *rtc, u8 irq)
+{
+	struct device *parent = _to_parent(rtc);
+	u8 irq_mask = rtc->irq_mask | irq;
+	int ret = 0;
+
+	ret = max77663_write(parent, MAX77663_RTC_IRQ_MASK, &irq_mask, 1, 1);
+	if (ret < 0) {
+		dev_err(rtc->dev, "rtc_irq_mask: Failed to set rtc irq mask\n");
+		goto out;
+	}
+	rtc->irq_mask = irq_mask;
+
+out:
+	return ret;
+}
+
+static inline int max77663_rtc_irq_unmask(struct max77663_rtc *rtc, u8 irq)
+{
+	struct device *parent = _to_parent(rtc);
+	u8 irq_mask = rtc->irq_mask & ~irq;
+	int ret = 0;
+
+	ret = max77663_write(parent, MAX77663_RTC_IRQ_MASK, &irq_mask, 1, 1);
+	if (ret < 0) {
+		dev_err(rtc->dev,
+			"rtc_irq_unmask: Failed to set rtc irq mask\n");
+		goto out;
+	}
+	rtc->irq_mask = irq_mask;
+
+out:
+	return ret;
+}
+
+static inline int max77663_rtc_do_irq(struct max77663_rtc *rtc)
+{
+	struct device *parent = _to_parent(rtc);
+	u8 irq_status;
+	int ret;
+
+	ret = max77663_rtc_update_buffer(rtc, 0);
+	if (ret < 0) {
+		dev_err(rtc->dev, "rtc_irq: Failed to get rtc update buffer\n");
+		return ret;
+	}
+
+	ret = max77663_read(parent, MAX77663_RTC_IRQ, &irq_status, 1, 1);
+	if (ret < 0) {
+		dev_err(rtc->dev, "rtc_irq: Failed to get rtc irq status\n");
+		return ret;
+	}
+
+	dev_dbg(rtc->dev, "rtc_do_irq: irq_mask=0x%02x, irq_status=0x%02x\n",
+		rtc->irq_mask, irq_status);
+
+	if (!(rtc->irq_mask & RTC_IRQ_ALARM1_MASK) &&
+			(irq_status & RTC_IRQ_ALARM1_MASK))
+		rtc_update_irq(rtc->rtc, 1, RTC_IRQF | RTC_AF);
+
+	if (!(rtc->irq_mask & RTC_IRQ_1SEC_MASK) &&
+			(irq_status & RTC_IRQ_1SEC_MASK))
+		rtc_update_irq(rtc->rtc, 1, RTC_IRQF | RTC_UF);
+
+	return ret;
+}
+
+static irqreturn_t max77663_rtc_irq(int irq, void *data)
+{
+	struct max77663_rtc *rtc = (struct max77663_rtc *)data;
+
+	max77663_rtc_do_irq(rtc);
+
+	return IRQ_HANDLED;
+}
+
+static int max77663_rtc_alarm_irq_enable(struct device *dev,
+					 unsigned int enabled)
+{
+	struct max77663_rtc *rtc = dev_get_drvdata(dev);
+	int ret = 0;
+
+	if (rtc->irq < 0)
+		return -ENXIO;
+
+	mutex_lock(&rtc->io_lock);
+
+	/* Handle pending interrupt */
+	ret = max77663_rtc_do_irq(rtc);
+	if (ret < 0)
+		goto out;
+
+	/* Config alarm interrupt */
+	if (enabled) {
+		ret = max77663_rtc_irq_unmask(rtc, RTC_IRQ_ALARM1_MASK);
+		if (ret < 0)
+			goto out;
+	} else {
+		ret = max77663_rtc_irq_mask(rtc, RTC_IRQ_ALARM1_MASK);
+		if (ret < 0)
+			goto out;
+	}
+out:
+	mutex_unlock(&rtc->io_lock);
+	return ret;
+}
+
+static int max77663_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct max77663_rtc *rtc = dev_get_drvdata(dev);
+	u8 buf[RTC_NR];
+	int ret;
+
+	ret = max77663_rtc_read(rtc, MAX77663_RTC_SEC, buf, sizeof(buf), 1);
+	if (ret < 0) {
+		dev_err(rtc->dev, "rtc_read_time: Failed to read rtc time\n");
+		return ret;
+	}
+
+	dev_dbg(rtc->dev, "rtc_read_time: "
+		"buf: 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x\n",
+		buf[RTC_SEC], buf[RTC_MIN], buf[RTC_HOUR], buf[RTC_WEEKDAY],
+		buf[RTC_MONTH], buf[RTC_YEAR], buf[RTC_MONTHDAY]);
+
+	ret = max77663_rtc_reg_to_tm(rtc, buf, tm);
+	if (ret < 0) {
+		dev_err(rtc->dev, "rtc_read_time: "
+			"Failed to convert register format into time format\n");
+		return ret;
+	}
+
+	dev_dbg(rtc->dev, "rtc_read_time: "
+		"tm: %d-%02d-%02d %02d:%02d:%02d, wday=%d\n",
+		tm->tm_year, tm->tm_mon, tm->tm_mday, tm->tm_hour, tm->tm_min,
+		tm->tm_sec, tm->tm_wday);
+
+	return ret;
+}
+
+static int max77663_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct max77663_rtc *rtc = dev_get_drvdata(dev);
+	u8 buf[RTC_NR];
+	int ret;
+
+	dev_dbg(rtc->dev, "rtc_set_time: "
+		"tm: %d-%02d-%02d %02d:%02d:%02d, wday=%d\n",
+		tm->tm_year, tm->tm_mon, tm->tm_mday, tm->tm_hour, tm->tm_min,
+		tm->tm_sec, tm->tm_wday);
+
+	ret = max77663_rtc_tm_to_reg(rtc, buf, tm, 0);
+	if (ret < 0) {
+		dev_err(rtc->dev, "rtc_set_time: "
+			"Failed to convert time format into register format\n");
+		return ret;
+	}
+
+	dev_dbg(rtc->dev, "rtc_set_time: "
+		"buf: 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x\n",
+		buf[RTC_SEC], buf[RTC_MIN], buf[RTC_HOUR], buf[RTC_WEEKDAY],
+		buf[RTC_MONTH], buf[RTC_YEAR], buf[RTC_MONTHDAY]);
+
+	return max77663_rtc_write(rtc, MAX77663_RTC_SEC, buf, sizeof(buf), 1);
+}
+
+static int max77663_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct max77663_rtc *rtc = dev_get_drvdata(dev);
+	u8 buf[RTC_NR];
+	int ret;
+
+	ret = max77663_rtc_read(rtc, MAX77663_RTC_ALARM_SEC1, buf, sizeof(buf),
+				1);
+	if (ret < 0) {
+		dev_err(rtc->dev,
+			"rtc_read_alarm: Failed to read rtc alarm time\n");
+		return ret;
+	}
+
+	dev_dbg(rtc->dev, "rtc_read_alarm: "
+		"buf: 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x\n",
+		buf[RTC_SEC], buf[RTC_MIN], buf[RTC_HOUR], buf[RTC_WEEKDAY],
+		buf[RTC_MONTH], buf[RTC_YEAR], buf[RTC_MONTHDAY]);
+
+	ret = max77663_rtc_reg_to_tm(rtc, buf, &alrm->time);
+	if (ret < 0) {
+		dev_err(rtc->dev, "rtc_read_alarm: "
+			"Failed to convert register format into time format\n");
+		return ret;
+	}
+
+	dev_dbg(rtc->dev, "rtc_read_alarm: "
+		"tm: %d-%02d-%02d %02d:%02d:%02d, wday=%d\n",
+		alrm->time.tm_year, alrm->time.tm_mon, alrm->time.tm_mday,
+		alrm->time.tm_hour, alrm->time.tm_min, alrm->time.tm_sec,
+		alrm->time.tm_wday);
+
+	if (rtc->irq_mask & RTC_IRQ_ALARM1_MASK)
+		alrm->enabled = 0;
+	else
+		alrm->enabled = 1;
+
+	return 0;
+}
+
+static int max77663_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct max77663_rtc *rtc = dev_get_drvdata(dev);
+	u8 buf[RTC_NR];
+	int ret;
+
+	if (rtc->shutdown_ongoing) {
+		dev_warn(rtc->dev, "rtc_set_alarm: "
+			 "Device shutdown on-going, skip alarm setting.\n");
+		return -ESHUTDOWN;
+	}
+	dev_dbg(rtc->dev, "rtc_set_alarm: "
+		"tm: %d-%02d-%02d %02d:%02d:%02d, wday=%d [%s]\n",
+		alrm->time.tm_year, alrm->time.tm_mon, alrm->time.tm_mday,
+		alrm->time.tm_hour, alrm->time.tm_min, alrm->time.tm_sec,
+		alrm->time.tm_wday, alrm->enabled?"enable":"disable");
+
+	ret = max77663_rtc_tm_to_reg(rtc, buf, &alrm->time, 1);
+	if (ret < 0) {
+		dev_err(rtc->dev, "rtc_set_alarm: "
+			"Failed to convert time format into register format\n");
+		return ret;
+	}
+
+	dev_dbg(rtc->dev, "rtc_set_alarm: "
+		"buf: 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x\n",
+		buf[RTC_SEC], buf[RTC_MIN], buf[RTC_HOUR], buf[RTC_WEEKDAY],
+		buf[RTC_MONTH], buf[RTC_YEAR], buf[RTC_MONTHDAY]);
+
+	ret = max77663_rtc_write(rtc, MAX77663_RTC_ALARM_SEC1, buf, sizeof(buf),
+				 1);
+	if (ret < 0) {
+		dev_err(rtc->dev,
+			"rtc_set_alarm: Failed to write rtc alarm time\n");
+		return ret;
+	}
+
+	ret = max77663_rtc_alarm_irq_enable(dev, alrm->enabled);
+	if (ret < 0) {
+		dev_err(rtc->dev,
+			"rtc_set_alarm: Failed to enable rtc alarm\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+static const struct rtc_class_ops max77663_rtc_ops = {
+	.read_time = max77663_rtc_read_time,
+	.set_time = max77663_rtc_set_time,
+	.read_alarm = max77663_rtc_read_alarm,
+	.set_alarm = max77663_rtc_set_alarm,
+	.alarm_irq_enable = max77663_rtc_alarm_irq_enable,
+};
+
+static int max77663_rtc_preinit(struct max77663_rtc *rtc)
+{
+	struct device *parent = _to_parent(rtc);
+	u8 val;
+	int ret;
+
+	/* Mask all interrupts */
+	rtc->irq_mask = 0xFF;
+	ret = max77663_rtc_write(rtc, MAX77663_RTC_IRQ_MASK, &rtc->irq_mask, 1,
+				 0);
+	if (ret < 0) {
+		dev_err(rtc->dev, "preinit: Failed to set rtc irq mask\n");
+		return ret;
+	}
+
+	/* Configure Binary mode and 24hour mode */
+	val = HR_MODE_MASK;
+	ret = max77663_rtc_write(rtc, MAX77663_RTC_CTRL, &val, 1, 0);
+	if (ret < 0) {
+		dev_err(rtc->dev, "preinit: Failed to set rtc control\n");
+		return ret;
+	}
+
+	/* It should be disabled alarm wakeup to wakeup from sleep
+	 * by EN1 input signal */
+	ret = max77663_set_bits(parent, MAX77663_REG_ONOFF_CFG2,
+				ONOFF_WK_ALARM1_MASK, 0, 0);
+	if (ret < 0) {
+		dev_err(rtc->dev, "preinit: Failed to set onoff cfg2\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int max77663_rtc_probe(struct platform_device *pdev)
+{
+	struct max77663_platform_data *parent_pdata =
+						pdev->dev.parent->platform_data;
+	static struct max77663_rtc *rtc;
+	int ret = 0;
+
+	rtc = kzalloc(sizeof(struct max77663_rtc), GFP_KERNEL);
+	if (!rtc) {
+		dev_err(&pdev->dev, "probe: kzalloc() failed\n");
+		return -ENOMEM;
+	}
+	rtc->shutdown_ongoing = false;
+	dev_set_drvdata(&pdev->dev, rtc);
+	rtc->dev = &pdev->dev;
+	mutex_init(&rtc->io_lock);
+
+	ret = max77663_rtc_preinit(rtc);
+	if (ret) {
+		dev_err(&pdev->dev, "probe: Failed to rtc preinit\n");
+		goto out_kfree;
+	}
+
+	rtc->rtc = rtc_device_register("max77663-rtc", &pdev->dev,
+				       &max77663_rtc_ops, THIS_MODULE);
+	if (IS_ERR_OR_NULL(rtc->rtc)) {
+		dev_err(&pdev->dev, "probe: Failed to register rtc\n");
+		ret = PTR_ERR(rtc->rtc);
+		goto out_kfree;
+	}
+
+	if (parent_pdata->irq_base < 0)
+		goto out;
+
+	rtc->irq = parent_pdata->irq_base + MAX77663_IRQ_RTC;
+	ret = request_threaded_irq(rtc->irq, NULL, max77663_rtc_irq,
+				   IRQF_ONESHOT, "max77663-rtc", rtc);
+	if (ret < 0) {
+		dev_err(rtc->dev, "probe: Failed to request irq %d\n",
+			rtc->irq);
+		rtc->irq = -1;
+	} else {
+		device_init_wakeup(rtc->dev, 1);
+		enable_irq_wake(rtc->irq);
+	}
+
+	return 0;
+
+out_kfree:
+	mutex_destroy(&rtc->io_lock);
+	kfree(rtc->rtc);
+out:
+	return ret;
+}
+
+static int __devexit max77663_rtc_remove(struct platform_device *pdev)
+{
+	struct max77663_rtc *rtc = dev_get_drvdata(&pdev->dev);
+
+	if (rtc->irq != -1)
+		free_irq(rtc->irq, rtc);
+
+	rtc_device_unregister(rtc->rtc);
+	mutex_destroy(&rtc->io_lock);
+	kfree(rtc);
+
+	return 0;
+}
+
+static void max77663_rtc_shutdown(struct platform_device *pdev)
+{
+	struct max77663_rtc *rtc = dev_get_drvdata(&pdev->dev);
+	u8 buf[RTC_NR] = { 0x0, 0x0, 0x0, 0x1, 0x1, 0x0, 0x1 };
+
+	rtc->shutdown_ongoing = true;
+	dev_info(rtc->dev, "rtc_shutdown: clean alarm\n");
+	max77663_rtc_write(rtc, MAX77663_RTC_ALARM_SEC1, buf, sizeof(buf), 1);
+	max77663_rtc_alarm_irq_enable(&pdev->dev, 0);
+}
+
+static struct platform_driver max77663_rtc_driver = {
+	.probe = max77663_rtc_probe,
+	.remove = __devexit_p(max77663_rtc_remove),
+	.driver = {
+		   .name = "max77663-rtc",
+		   .owner = THIS_MODULE,
+	},
+	.shutdown = max77663_rtc_shutdown,
+};
+
+static int __init max77663_rtc_init(void)
+{
+	return platform_driver_register(&max77663_rtc_driver);
+}
+module_init(max77663_rtc_init);
+
+static void __exit max77663_rtc_exit(void)
+{
+	platform_driver_unregister(&max77663_rtc_driver);
+}
+module_exit(max77663_rtc_exit);
+
+MODULE_DESCRIPTION("max77663 RTC driver");
+MODULE_LICENSE("GPL v2");
+MODULE_VERSION("1.0");
diff --git a/drivers/rtc/rtc-max8907c.c b/drivers/rtc/rtc-max8907c.c
new file mode 100644
index 000000000000..f7287021da3c
--- /dev/null
+++ b/drivers/rtc/rtc-max8907c.c
@@ -0,0 +1,323 @@
+/*
+ * RTC driver for Maxim MAX8907c
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ * Based on drivers/rtc/rtc-max8925.c, Copyright (C) 2009-2010 Marvell International Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/rtc.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/max8907c.h>
+
+enum {
+	RTC_SEC = 0,
+	RTC_MIN,
+	RTC_HOUR,
+	RTC_WEEKDAY,
+	RTC_DATE,
+	RTC_MONTH,
+	RTC_YEAR1,
+	RTC_YEAR2,
+};
+
+#define TIME_NUM			8
+#define ALARM_1SEC			(1 << 7)
+#define HOUR_12				(1 << 7)
+#define HOUR_AM_PM			(1 << 5)
+#define ALARM0_IRQ			(1 << 3)
+#define ALARM1_IRQ			(1 << 2)
+#define ALARM0_STATUS			(1 << 2)
+#define ALARM1_STATUS			(1 << 1)
+
+struct max8907c_rtc_info {
+	struct rtc_device	*rtc_dev;
+	struct i2c_client	*i2c;
+	struct max8907c		*chip;
+};
+
+static irqreturn_t rtc_update_handler(int irq, void *data)
+{
+	struct max8907c_rtc_info *info = (struct max8907c_rtc_info *)data;
+
+	/* disable ALARM0 except for 1SEC alarm */
+	max8907c_set_bits(info->i2c, MAX8907C_REG_ALARM0_CNTL, 0x7f, 0);
+	rtc_update_irq(info->rtc_dev, 1, RTC_IRQF | RTC_AF);
+	return IRQ_HANDLED;
+}
+
+static int tm_calc(struct rtc_time *tm, u8 *buf, int len)
+{
+	if (len < TIME_NUM)
+		return -EINVAL;
+	tm->tm_year = (buf[RTC_YEAR2] >> 4) * 1000
+			+ (buf[RTC_YEAR2] & 0xf) * 100
+			+ (buf[RTC_YEAR1] >> 4) * 10
+			+ (buf[RTC_YEAR1] & 0xf);
+	tm->tm_year -= 1900;
+	/* RTC month index issue in max8907c
+	    : January index is 1 but kernel assumes it as 0 */
+	tm->tm_mon = ((buf[RTC_MONTH] >> 4) & 0x01) * 10
+			+ (buf[RTC_MONTH] & 0x0f) - 1;
+	tm->tm_mday = ((buf[RTC_DATE] >> 4) & 0x03) * 10
+			+ (buf[RTC_DATE] & 0x0f);
+	tm->tm_wday = buf[RTC_WEEKDAY] & 0x07;
+	if (buf[RTC_HOUR] & HOUR_12) {
+		tm->tm_hour = ((buf[RTC_HOUR] >> 4) & 0x1) * 10
+				+ (buf[RTC_HOUR] & 0x0f);
+		if (buf[RTC_HOUR] & HOUR_AM_PM)
+			tm->tm_hour += 12;
+	} else {
+		tm->tm_hour = ((buf[RTC_HOUR] >> 4) & 0x03) * 10
+				+ (buf[RTC_HOUR] & 0x0f);
+	}
+	tm->tm_min = ((buf[RTC_MIN] >> 4) & 0x7) * 10
+			+ (buf[RTC_MIN] & 0x0f);
+	tm->tm_sec = ((buf[RTC_SEC] >> 4) & 0x7) * 10
+			+ (buf[RTC_SEC] & 0x0f);
+	return 0;
+}
+
+static int data_calc(u8 *buf, struct rtc_time *tm, int len)
+{
+	u8 high, low;
+
+	if (len < TIME_NUM)
+		return -EINVAL;
+
+	high = (tm->tm_year + 1900) / 1000;
+	low = (tm->tm_year + 1900) / 100;
+	low = low - high * 10;
+	buf[RTC_YEAR2] = (high << 4) + low;
+	high = (tm->tm_year + 1900) / 10;
+	low = tm->tm_year + 1900;
+	low = low - high * 10;
+	high = high - (high / 10) * 10;
+	buf[RTC_YEAR1] = (high << 4) + low;
+
+	/* RTC month index issue in max8907c
+	    : January index is 1 but kernel assumes it as 0 */
+	high = (tm->tm_mon + 1) / 10;
+	low = (tm->tm_mon + 1) % 10;
+	buf[RTC_MONTH] = (high << 4) + low;
+
+	high = tm->tm_mday / 10;
+	low = tm->tm_mday;
+	low = low - high * 10;
+	buf[RTC_DATE] = (high << 4) + low;
+	buf[RTC_WEEKDAY] = tm->tm_wday;
+	high = tm->tm_hour / 10;
+	low = tm->tm_hour;
+	low = low - high * 10;
+	buf[RTC_HOUR] = (high << 4) + low;
+	high = tm->tm_min / 10;
+	low = tm->tm_min;
+	low = low - high * 10;
+	buf[RTC_MIN] = (high << 4) + low;
+	high = tm->tm_sec / 10;
+	low = tm->tm_sec;
+	low = low - high * 10;
+	buf[RTC_SEC] = (high << 4) + low;
+	return 0;
+}
+
+static int max8907c_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct max8907c_rtc_info *info = dev_get_drvdata(dev);
+	u8 buf[TIME_NUM];
+	int ret;
+
+	ret = max8907c_reg_bulk_read(info->i2c, MAX8907C_REG_RTC_SEC, TIME_NUM, buf);
+
+	if (ret < 0)
+		return ret;
+	ret = tm_calc(tm, buf, TIME_NUM);
+
+	return ret;
+}
+
+static int max8907c_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct max8907c_rtc_info *info = dev_get_drvdata(dev);
+	u8 buf[TIME_NUM];
+	int ret;
+
+	ret = data_calc(buf, tm, TIME_NUM);
+
+	if (ret < 0)
+		return ret;
+	ret = max8907c_reg_bulk_write(info->i2c, MAX8907C_REG_RTC_SEC, TIME_NUM, buf);
+
+	return ret;
+}
+
+static int max8907c_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct max8907c_rtc_info *info = dev_get_drvdata(dev);
+	unsigned char buf[TIME_NUM];
+	int ret;
+
+	ret = max8907c_reg_bulk_read(info->i2c, MAX8907C_REG_ALARM0_SEC, TIME_NUM, buf);
+	if (ret < 0)
+		return ret;
+	ret = tm_calc(&alrm->time, buf, TIME_NUM);
+	if (ret < 0)
+		return ret;
+	ret = max8907c_reg_read(info->i2c, MAX8907C_REG_RTC_IRQ_MASK);
+	if (ret < 0)
+		return ret;
+	if ((ret & ALARM0_IRQ) == 0)
+		alrm->enabled = 1;
+	else
+		alrm->enabled = 0;
+	ret = max8907c_reg_read(info->i2c, MAX8907C_REG_RTC_STATUS);
+	if (ret < 0)
+		return ret;
+	if (ret & ALARM0_STATUS)
+		alrm->pending = 1;
+	else
+		alrm->pending = 0;
+
+	return ret;
+}
+
+static int max8907c_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct max8907c_rtc_info *info = dev_get_drvdata(dev);
+	unsigned char buf[TIME_NUM];
+	int ret;
+
+	ret = data_calc(buf, &alrm->time, TIME_NUM);
+	if (ret < 0)
+		return ret;
+	ret = max8907c_reg_bulk_write(info->i2c, MAX8907C_REG_ALARM0_SEC, TIME_NUM, buf);
+	if (ret < 0)
+		return ret;
+	/* only enable alarm on year/month/day/hour/min/sec */
+	ret = max8907c_reg_write(info->i2c, MAX8907C_REG_ALARM0_CNTL, 0x77);
+
+	return ret;
+}
+
+static const struct rtc_class_ops max8907c_rtc_ops = {
+	.read_time	= max8907c_rtc_read_time,
+	.set_time	= max8907c_rtc_set_time,
+	.read_alarm	= max8907c_rtc_read_alarm,
+	.set_alarm	= max8907c_rtc_set_alarm,
+};
+
+static int __devinit max8907c_rtc_probe(struct platform_device *pdev)
+{
+	struct max8907c *chip = dev_get_drvdata(pdev->dev.parent);
+	struct max8907c_rtc_info *info;
+	int irq, ret;
+
+	info = kzalloc(sizeof(struct max8907c_rtc_info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+	info->i2c = chip->i2c_rtc;
+	info->chip = chip;
+
+	irq = chip->irq_base + MAX8907C_IRQ_RTC_ALARM0;
+
+	ret = request_threaded_irq(irq, NULL, rtc_update_handler,
+				   IRQF_ONESHOT, "rtc-alarm0", info);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to request IRQ: #%d: %d\n",
+			irq, ret);
+		goto out_irq;
+	}
+
+	dev_set_drvdata(&pdev->dev, info);
+	info->rtc_dev = rtc_device_register("max8907c-rtc", &pdev->dev,
+					&max8907c_rtc_ops, THIS_MODULE);
+	ret = PTR_ERR(info->rtc_dev);
+	if (IS_ERR(info->rtc_dev)) {
+		dev_err(&pdev->dev, "Failed to register RTC device: %d\n", ret);
+		goto out_rtc;
+	}
+
+	max8907c_set_bits(chip->i2c_power, MAX8907C_REG_SYSENSEL, 0x2, 0x2);
+
+	platform_set_drvdata(pdev, info);
+
+	device_init_wakeup(&pdev->dev, 1);
+
+	return 0;
+out_rtc:
+	free_irq(chip->irq_base + MAX8907C_IRQ_RTC_ALARM0, info);
+
+out_irq:
+	kfree(info);
+	return ret;
+}
+
+static int __devexit max8907c_rtc_remove(struct platform_device *pdev)
+{
+	struct max8907c_rtc_info *info = platform_get_drvdata(pdev);
+
+	if (info) {
+		free_irq(info->chip->irq_base + MAX8907C_IRQ_RTC_ALARM0, info);
+
+		rtc_device_unregister(info->rtc_dev);
+		kfree(info);
+	}
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int max8907c_rtc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct device *dev=&pdev->dev;
+	struct max8907c_rtc_info *info = platform_get_drvdata(pdev);
+
+	if (device_may_wakeup(dev))
+		enable_irq_wake(info->chip->irq_base + MAX8907C_IRQ_RTC_ALARM0);
+	return 0;
+}
+
+static int max8907c_rtc_resume(struct platform_device *pdev)
+{
+	struct device *dev=&pdev->dev;
+	struct max8907c_rtc_info *info = platform_get_drvdata(pdev);
+
+	if (device_may_wakeup(dev))
+		disable_irq_wake(info->chip->irq_base + MAX8907C_IRQ_RTC_ALARM0);
+	return 0;
+}
+#endif
+
+static struct platform_driver max8907c_rtc_driver = {
+	.driver		= {
+		.name	= "max8907c-rtc",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= max8907c_rtc_probe,
+	.remove		= __devexit_p(max8907c_rtc_remove),
+#ifdef CONFIG_PM
+	.suspend	= max8907c_rtc_suspend,
+	.resume		= max8907c_rtc_resume,
+#endif
+};
+
+static int __init max8907c_rtc_init(void)
+{
+	return platform_driver_register(&max8907c_rtc_driver);
+}
+module_init(max8907c_rtc_init);
+
+static void __exit max8907c_rtc_exit(void)
+{
+	platform_driver_unregister(&max8907c_rtc_driver);
+}
+module_exit(max8907c_rtc_exit);
+
+MODULE_DESCRIPTION("Maxim MAX8907C RTC driver");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/rtc/rtc-ricoh583.c b/drivers/rtc/rtc-ricoh583.c
new file mode 100644
index 000000000000..8bc17d9a1013
--- /dev/null
+++ b/drivers/rtc/rtc-ricoh583.c
@@ -0,0 +1,403 @@
+/*
+ * drivers/rtc/rtc_ricoh583.c
+ *
+ * rtc driver for ricoh rc5t583 pmu
+ *
+ * copyright (c) 2011, nvidia corporation.
+ *
+ * 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.,
+ * 51 franklin street, fifth floor, boston, ma  02110-1301, usa.
+ */
+
+/* #define debug		1 */
+/* #define verbose_debug	1 */
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/mfd/ricoh583.h>
+#include <linux/platform_device.h>
+#include <linux/rtc.h>
+#include <linux/slab.h>
+
+#define rtc_ctrl1		0xED
+#define rtc_ctrl2		0xEE
+#define rtc_seconds_reg		0xE0
+#define rtc_alarm_y		0xF0
+#define rtc_adjust		0xE7
+
+/*
+linux rtc driver refers 1900 as base year in many calculations.
+(e.g. refer drivers/rtc/rtc-lib.c)
+*/
+#define os_ref_year 1900
+
+/*
+	pmu rtc have only 2 nibbles to store year information, so using an
+	offset of 100 to set the base year as 2000 for our driver.
+*/
+#define rtc_year_offset 100
+
+struct ricoh583_rtc {
+	unsigned long		epoch_start;
+	int			irq;
+	struct rtc_device	*rtc;
+	bool			irq_en;
+};
+
+static int ricoh583_read_regs(struct device *dev, int reg, int len,
+	uint8_t *val)
+{
+	int ret;
+
+	ret = ricoh583_bulk_reads(dev->parent, reg, len, val);
+	if (ret < 0) {
+		dev_err(dev->parent, "\n %s failed reading from 0x%02x\n",
+			__func__, reg);
+		WARN_ON(1);
+	}
+	return ret;
+}
+
+static int ricoh583_write_regs(struct device *dev, int reg, int len,
+	uint8_t *val)
+{
+	int ret;
+	ret = ricoh583_bulk_writes(dev->parent, reg, len, val);
+	if (ret < 0) {
+		dev_err(dev->parent, "\n %s failed writing\n", __func__);
+		WARN_ON(1);
+	}
+
+	return ret;
+}
+
+static int ricoh583_rtc_valid_tm(struct device *dev, struct rtc_time *tm)
+{
+	if (tm->tm_year >= (rtc_year_offset + 99)
+		|| tm->tm_mon > 12
+		|| tm->tm_mday < 1
+		|| tm->tm_mday > rtc_month_days(tm->tm_mon,
+			tm->tm_year + os_ref_year)
+		|| tm->tm_hour >= 24
+		|| tm->tm_min >= 60
+		|| tm->tm_sec >= 60) {
+		dev_err(dev->parent, "\n returning error due to time"
+		"%d/%d/%d %d:%d:%d", tm->tm_mon, tm->tm_mday,
+		tm->tm_year, tm->tm_hour, tm->tm_min, tm->tm_sec);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static u8 dec2bcd(u8 dec)
+{
+	return ((dec/10)<<4)+(dec%10);
+}
+
+static u8 bcd2dec(u8 bcd)
+{
+	return (bcd >> 4)*10+(bcd & 0xf);
+}
+
+static void convert_bcd_to_decimal(u8 *buf, u8 len)
+{
+	int i = 0;
+	for (i = 0; i < len; i++)
+		buf[i] = bcd2dec(buf[i]);
+}
+
+static void convert_decimal_to_bcd(u8 *buf, u8 len)
+{
+	int i = 0;
+	for (i = 0; i < len; i++)
+		buf[i] = dec2bcd(buf[i]);
+}
+
+static void print_time(struct device *dev, struct rtc_time *tm)
+{
+	dev_info(dev, "rtc-time : %d/%d/%d %d:%d\n",
+		(tm->tm_mon + 1), tm->tm_mday, (tm->tm_year + os_ref_year),
+		tm->tm_hour, tm->tm_min);
+}
+
+static int ricoh583_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	u8 buff[7];
+	int err;
+	err = ricoh583_read_regs(dev, rtc_seconds_reg, sizeof(buff), buff);
+	if (err < 0) {
+		dev_err(dev, "\n %s :: failed to read time\n", __FILE__);
+		return err;
+	}
+	convert_bcd_to_decimal(buff, sizeof(buff));
+	tm->tm_sec  = buff[0];
+	tm->tm_min  = buff[1];
+	tm->tm_hour = buff[2];
+	tm->tm_wday = buff[3];
+	tm->tm_mday = buff[4];
+	tm->tm_mon  = buff[5] - 1;
+	tm->tm_year = buff[6] + rtc_year_offset;
+	print_time(dev, tm);
+	return ricoh583_rtc_valid_tm(dev, tm);
+}
+
+static int ricoh583_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	u8 buff[7];
+	int err;
+
+	print_time(dev, tm);
+	buff[0] = tm->tm_sec;
+	buff[1] = tm->tm_min;
+	buff[2] = tm->tm_hour;
+	buff[3] = tm->tm_wday;
+	buff[4] = tm->tm_mday;
+	buff[5] = tm->tm_mon + 1;
+	buff[6] = tm->tm_year - rtc_year_offset;
+
+	convert_decimal_to_bcd(buff, sizeof(buff));
+	err = ricoh583_write_regs(dev, rtc_seconds_reg, sizeof(buff), buff);
+	if (err < 0) {
+		dev_err(dev->parent, "\n failed to program new time\n");
+		return err;
+	}
+
+	return 0;
+}
+static int ricoh583_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm);
+
+static int ricoh583_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct ricoh583_rtc *rtc = dev_get_drvdata(dev);
+	unsigned long seconds;
+	u8 buff[5];
+	int err;
+	struct rtc_time tm;
+
+	if (rtc->irq == -1)
+		return -EIO;
+
+	rtc_tm_to_time(&alrm->time, &seconds);
+	ricoh583_rtc_read_time(dev, &tm);
+	rtc_tm_to_time(&tm, &rtc->epoch_start);
+	/*
+		work around: As YAL does not provide the seconds register,
+		program minute register to next minute, in cases when alarm
+		is requested within a minute from the current time.
+	*/
+	if (seconds - rtc->epoch_start < 60)
+		alrm->time.tm_min += 1;
+	dev_info(dev->parent, "\n setting alarm to requested time::\n");
+	print_time(dev->parent, &alrm->time);
+
+	if (WARN_ON(alrm->enabled && (seconds < rtc->epoch_start))) {
+		dev_err(dev->parent, "\n can't set alarm to requested time\n");
+		return -EINVAL;
+	}
+
+	if (alrm->enabled && !rtc->irq_en)
+		rtc->irq_en = true;
+	else if (!alrm->enabled && rtc->irq_en)
+		rtc->irq_en = false;
+
+	buff[0] = alrm->time.tm_min;
+	buff[1] = alrm->time.tm_hour;
+	buff[2] = alrm->time.tm_mday;
+	buff[3] = alrm->time.tm_mon + 1;
+	buff[4] = alrm->time.tm_year - rtc_year_offset;
+	convert_decimal_to_bcd(buff, sizeof(buff));
+	err = ricoh583_write_regs(dev, rtc_alarm_y, sizeof(buff), buff);
+	if (err) {
+		dev_err(dev->parent, "\n unable to set alarm\n");
+		return -EBUSY;
+	}
+	buff[0] = 0x20; /* to enable alarm_y */
+	buff[1] = 0x20; /* to enable 24-hour format */
+	err = ricoh583_write_regs(dev, rtc_ctrl1, 2, buff);
+	if (err) {
+		dev_err(dev, "failed programming rtc ctrl regs\n");
+		return -EBUSY;
+	}
+return err;
+}
+
+static int ricoh583_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	u8 buff[5];
+	int err;
+
+	err = ricoh583_read_regs(dev, rtc_alarm_y, sizeof(buff), buff);
+	if (err)
+		return err;
+	convert_bcd_to_decimal(buff, sizeof(buff));
+
+	alrm->time.tm_min  = buff[0];
+	alrm->time.tm_hour = buff[1];
+	alrm->time.tm_mday = buff[2];
+	alrm->time.tm_mon  = buff[3] - 1;
+	alrm->time.tm_year = buff[4] + rtc_year_offset;
+
+	dev_info(dev->parent, "\n getting alarm time::\n");
+	print_time(dev, &alrm->time);
+
+	return 0;
+}
+
+static const struct rtc_class_ops ricoh583_rtc_ops = {
+	.read_time	= ricoh583_rtc_read_time,
+	.set_time	= ricoh583_rtc_set_time,
+	.set_alarm	= ricoh583_rtc_set_alarm,
+	.read_alarm	= ricoh583_rtc_read_alarm,
+};
+
+static irqreturn_t ricoh583_rtc_irq(int irq, void *data)
+{
+	struct device *dev = data;
+	struct ricoh583_rtc *rtc = dev_get_drvdata(dev);
+	u8 reg;
+	int err;
+
+	/* clear alarm-Y status bits.*/
+	err = ricoh583_read_regs(dev, rtc_ctrl2, 1, &reg);
+	if (err) {
+		dev_err(dev->parent, "unable to read rtc_ctrl2 reg\n");
+		return -EBUSY;
+	}
+	reg &= ~0x8;
+	err = ricoh583_write_regs(dev, rtc_ctrl2, 1, &reg);
+	if (err) {
+		dev_err(dev->parent, "unable to program rtc_status reg\n");
+		return -EBUSY;
+	}
+
+	rtc_update_irq(rtc->rtc, 1, RTC_IRQF | RTC_AF);
+	return IRQ_HANDLED;
+}
+
+static int __devinit ricoh583_rtc_probe(struct platform_device *pdev)
+{
+	struct ricoh583_rtc_platform_data *pdata = pdev->dev.platform_data;
+	struct ricoh583_rtc *rtc;
+	struct rtc_time tm;
+	int err;
+	u8 reg[2];
+	rtc = kzalloc(sizeof(*rtc), GFP_KERNEL);
+
+	if (!rtc)
+		return -ENOMEM;
+
+	rtc->irq = -1;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform_data specified\n");
+		return -EINVAL;
+	}
+
+	if (pdata->irq < 0)
+		dev_err(&pdev->dev, "\n no irq specified, wakeup is disabled\n");
+
+	dev_set_drvdata(&pdev->dev, rtc);
+	device_init_wakeup(&pdev->dev, 1);
+	rtc->rtc = rtc_device_register(pdev->name, &pdev->dev,
+				       &ricoh583_rtc_ops, THIS_MODULE);
+
+	if (IS_ERR(rtc->rtc)) {
+		err = PTR_ERR(rtc->rtc);
+		goto fail;
+	}
+	reg[0] =  0; /* clearing RTC Adjust register */
+	err = ricoh583_write_regs(&pdev->dev, rtc_adjust, 1, reg);
+	if (err) {
+		dev_err(&pdev->dev, "unable to program rtc_adjust reg\n");
+		return -EBUSY;
+	}
+
+	reg[0] = 0x20; /* to enable alarm_y */
+	reg[1] = 0x20; /* to enable 24-hour format */
+	err = ricoh583_write_regs(&pdev->dev, rtc_ctrl1, 2, reg);
+	if (err) {
+		dev_err(&pdev->dev, "failed rtc setup\n");
+		return -EBUSY;
+	}
+
+	ricoh583_rtc_read_time(&pdev->dev, &tm);
+	if (ricoh583_rtc_valid_tm(&pdev->dev, &tm)) {
+		if (pdata->time.tm_year < 2000 || pdata->time.tm_year > 2100) {
+			memset(&pdata->time, 0, sizeof(pdata->time));
+			pdata->time.tm_year = rtc_year_offset;
+			pdata->time.tm_mday = 1;
+		} else
+		pdata->time.tm_year -= os_ref_year;
+		ricoh583_rtc_set_time(&pdev->dev, &pdata->time);
+	}
+	if (pdata && (pdata->irq >= 0)) {
+		rtc->irq = pdata->irq;
+		err = request_threaded_irq(pdata->irq, NULL, ricoh583_rtc_irq,
+					IRQF_ONESHOT, "rtc_ricoh583",
+					&pdev->dev);
+		if (err) {
+			dev_err(&pdev->dev, "request IRQ:%d fail\n", rtc->irq);
+			rtc->irq = -1;
+		} else {
+			device_init_wakeup(&pdev->dev, 1);
+			enable_irq_wake(rtc->irq);
+		}
+	}
+	return 0;
+
+fail:
+	if (!IS_ERR_OR_NULL(rtc->rtc))
+		rtc_device_unregister(rtc->rtc);
+	kfree(rtc);
+	return err;
+}
+
+static int __devexit ricoh583_rtc_remove(struct platform_device *pdev)
+{
+	struct ricoh583_rtc *rtc = dev_get_drvdata(&pdev->dev);
+
+	if (rtc->irq != -1)
+		free_irq(rtc->irq, rtc);
+	rtc_device_unregister(rtc->rtc);
+	kfree(rtc);
+	return 0;
+}
+
+static struct platform_driver ricoh583_rtc_driver = {
+	.driver	= {
+		.name	= "rtc_ricoh583",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= ricoh583_rtc_probe,
+	.remove	= __devexit_p(ricoh583_rtc_remove),
+};
+
+static int __init ricoh583_rtc_init(void)
+{
+	return platform_driver_register(&ricoh583_rtc_driver);
+}
+module_init(ricoh583_rtc_init);
+
+static void __exit ricoh583_rtc_exit(void)
+{
+	platform_driver_unregister(&ricoh583_rtc_driver);
+}
+module_exit(ricoh583_rtc_exit);
+
+MODULE_DESCRIPTION("RICOH PMU ricoh583 RTC driver");
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:rtc_ricoh583");
diff --git a/drivers/rtc/rtc-s3c.c b/drivers/rtc/rtc-s3c.c
index 7639ab906f02..5b979d9cc332 100644
--- a/drivers/rtc/rtc-s3c.c
+++ b/drivers/rtc/rtc-s3c.c
@@ -202,7 +202,6 @@ static int s3c_rtc_settime(struct device *dev, struct rtc_time *tm)
 	void __iomem *base = s3c_rtc_base;
 	int year = tm->tm_year - 100;
 
-	clk_enable(rtc_clk);
 	pr_debug("set time %04d.%02d.%02d %02d:%02d:%02d\n",
 		 1900 + tm->tm_year, tm->tm_mon, tm->tm_mday,
 		 tm->tm_hour, tm->tm_min, tm->tm_sec);
@@ -214,6 +213,7 @@ static int s3c_rtc_settime(struct device *dev, struct rtc_time *tm)
 		return -EINVAL;
 	}
 
+	clk_enable(rtc_clk);
 	writeb(bin2bcd(tm->tm_sec),  base + S3C2410_RTCSEC);
 	writeb(bin2bcd(tm->tm_min),  base + S3C2410_RTCMIN);
 	writeb(bin2bcd(tm->tm_hour), base + S3C2410_RTCHOUR);
diff --git a/drivers/rtc/rtc-tegra.c b/drivers/rtc/rtc-tegra.c
index 75259fe38602..773adffac277 100644
--- a/drivers/rtc/rtc-tegra.c
+++ b/drivers/rtc/rtc-tegra.c
@@ -1,7 +1,8 @@
 /*
  * An RTC driver for the NVIDIA Tegra 200 series internal RTC.
  *
- * Copyright (c) 2010, NVIDIA Corporation.
+ * Copyright (c) 2010-2011, NVIDIA Corporation.
+ * Copyright (c) 2010 Jon Mayo <jmayo@nvidia.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -376,6 +377,36 @@ static int __devinit tegra_rtc_probe(struct platform_device *pdev)
 
 	dev_notice(&pdev->dev, "Tegra internal Real Time Clock\n");
 
+#ifndef CONFIG_TEGRA_SILICON_PLATFORM
+	{
+		struct rtc_time tm;
+
+		/* Get the current time from the RTC. */
+		ret = tegra_rtc_read_time(&pdev->dev, &tm);
+		if (ret) {
+			/* Report but ignore this error. */
+			dev_err(&pdev->dev,
+				"Failed to get FPGA internal RTC time (err=%d)\n",
+				ret);
+		} else if (tm.tm_year < 2010) {
+			/* The RTC's default reset time is soooo last century. */
+			tm.tm_year = 2010-1900;
+			tm.tm_mon  = 0;
+			tm.tm_mday = 1;
+			tm.tm_hour = 0;
+			tm.tm_min  = 0;
+			tm.tm_sec  = 0;
+			ret = tegra_rtc_set_time(&pdev->dev, &tm);
+			if (ret) {
+				/* Report but ignore this error. */
+				dev_err(&pdev->dev,
+					"Failed to set FPGA internal RTC time (err=%d)\n",
+					ret);
+			}
+		}
+	}
+#endif
+
 	return 0;
 
 err_dev_unreg:
diff --git a/drivers/rtc/rtc-tps6586x.c b/drivers/rtc/rtc-tps6586x.c
new file mode 100644
index 000000000000..c41edabf0b2c
--- /dev/null
+++ b/drivers/rtc/rtc-tps6586x.c
@@ -0,0 +1,387 @@
+/*
+ * drivers/rtc/rtc-tps6586x.c
+ *
+ * RTC driver for TI TPS6586x
+ *
+ * Copyright (c) 2010, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/mfd/tps6586x.h>
+#include <linux/platform_device.h>
+#include <linux/rtc.h>
+#include <linux/slab.h>
+
+#define RTC_CTRL	0xc0
+#define POR_RESET_N	BIT(7)
+#define OSC_SRC_SEL	BIT(6)
+#define RTC_ENABLE	BIT(5)	/* enables alarm */
+#define RTC_BUF_ENABLE	BIT(4)	/* 32 KHz buffer enable */
+#define PRE_BYPASS	BIT(3)	/* 0=1KHz or 1=32KHz updates */
+#define CL_SEL_MASK	(BIT(2)|BIT(1))
+#define CL_SEL_POS	1
+#define RTC_ALARM1_HI	0xc1
+#define RTC_COUNT4	0xc6
+#define RTC_COUNT4_DUMMYREAD 0xc5  /* start a PMU RTC access by reading the register prior to the RTC_COUNT4 */
+#define ALM1_VALID_RANGE_IN_SEC 0x3FFF /*only 14-bits width in second*/
+
+struct tps6586x_rtc {
+	unsigned long		epoch_start;
+	int			irq;
+	struct rtc_device	*rtc;
+	bool			irq_en;
+};
+
+static inline struct device *to_tps6586x_dev(struct device *dev)
+{
+	return dev->parent;
+}
+
+static int tps6586x_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct tps6586x_rtc *rtc = dev_get_drvdata(dev);
+	struct device *tps_dev = to_tps6586x_dev(dev);
+	unsigned long long ticks = 0;
+	unsigned long seconds;
+	u8 buff[6];
+	int err;
+	int i;
+
+	err = tps6586x_reads(tps_dev, RTC_COUNT4_DUMMYREAD, sizeof(buff), buff);
+	if (err < 0) {
+		dev_err(dev, "failed to read counter\n");
+		return err;
+	}
+
+	for (i = 1; i < sizeof(buff); i++) {
+		ticks <<= 8;
+		ticks |= buff[i];
+	}
+
+	seconds = ticks >> 10;
+
+	seconds += rtc->epoch_start;
+	rtc_time_to_tm(seconds, tm);
+	return rtc_valid_tm(tm);
+}
+
+static int tps6586x_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct tps6586x_rtc *rtc = dev_get_drvdata(dev);
+	struct device *tps_dev = to_tps6586x_dev(dev);
+	unsigned long long ticks;
+	unsigned long seconds;
+	u8 buff[5];
+	int err;
+
+	rtc_tm_to_time(tm, &seconds);
+
+	if (WARN_ON(seconds < rtc->epoch_start)) {
+		dev_err(dev, "requested time unsupported\n");
+		return -EINVAL;
+	}
+
+	seconds -= rtc->epoch_start;
+
+	ticks = (unsigned long long)seconds << 10;
+	buff[0] = (ticks >> 32) & 0xff;
+	buff[1] = (ticks >> 24) & 0xff;
+	buff[2] = (ticks >> 16) & 0xff;
+	buff[3] = (ticks >> 8) & 0xff;
+	buff[4] = ticks & 0xff;
+
+	err = tps6586x_clr_bits(tps_dev, RTC_CTRL, RTC_ENABLE);
+	if (err < 0) {
+		dev_err(dev, "failed to clear RTC_ENABLE\n");
+		return err;
+	}
+
+	err = tps6586x_writes(tps_dev, RTC_COUNT4, sizeof(buff), buff);
+	if (err < 0) {
+		dev_err(dev, "failed to program new time\n");
+		return err;
+	}
+
+	err = tps6586x_set_bits(tps_dev, RTC_CTRL, RTC_ENABLE);
+	if (err < 0) {
+		dev_err(dev, "failed to set RTC_ENABLE\n");
+		return err;
+	}
+
+	return 0;
+}
+
+static int tps6586x_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct tps6586x_rtc *rtc = dev_get_drvdata(dev);
+	struct device *tps_dev = to_tps6586x_dev(dev);
+	unsigned long seconds;
+	unsigned long ticks;
+	unsigned long rtc_current_time;
+	unsigned long long rticks = 0;
+	u8 buff[3];
+	u8 rbuff[6];
+	int err;
+	int i;
+
+	if (rtc->irq == -1)
+		return -EIO;
+
+	rtc_tm_to_time(&alrm->time, &seconds);
+
+	if (WARN_ON(alrm->enabled && (seconds < rtc->epoch_start))) {
+		dev_err(dev, "can't set alarm to requested time\n");
+		return -EINVAL;
+	}
+
+	if (alrm->enabled && !rtc->irq_en) {
+		enable_irq(rtc->irq);
+		rtc->irq_en = true;
+	} else if (!alrm->enabled && rtc->irq_en) {
+		disable_irq(rtc->irq);
+		rtc->irq_en = false;
+	}
+
+	seconds -= rtc->epoch_start;
+
+	err = tps6586x_reads(tps_dev, RTC_COUNT4_DUMMYREAD, sizeof(rbuff), rbuff);
+	if (err < 0) {
+		dev_err(dev, "failed to read counter\n");
+		return err;
+	}
+
+	for (i = 1; i < sizeof(rbuff); i++) {
+		rticks <<= 8;
+		rticks |= rbuff[i];
+	}
+
+	rtc_current_time = rticks >> 10;
+	if ((seconds - rtc_current_time) > ALM1_VALID_RANGE_IN_SEC)
+		seconds = rtc_current_time - 1;
+
+	ticks = (unsigned long long)seconds << 10;
+
+	buff[0] = (ticks >> 16) & 0xff;
+	buff[1] = (ticks >> 8) & 0xff;
+	buff[2] = ticks & 0xff;
+
+	err = tps6586x_writes(tps_dev, RTC_ALARM1_HI, sizeof(buff), buff);
+	if (err)
+		dev_err(tps_dev, "unable to program alarm\n");
+
+	return err;
+}
+
+static int tps6586x_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct tps6586x_rtc *rtc = dev_get_drvdata(dev);
+	struct device *tps_dev = to_tps6586x_dev(dev);
+	unsigned long ticks;
+	unsigned long seconds;
+	u8 buff[3];
+	int err;
+
+	err = tps6586x_reads(tps_dev, RTC_ALARM1_HI, sizeof(buff), buff);
+	if (err)
+		return err;
+
+	ticks = (buff[0] << 16) | (buff[1] << 8) | buff[2];
+	seconds = ticks >> 10;
+	seconds += rtc->epoch_start;
+
+	rtc_time_to_tm(seconds, &alrm->time);
+
+	return 0;
+}
+
+static int tps6586x_rtc_alarm_irq_enable(struct device *dev,
+					 unsigned int enabled)
+{
+	struct tps6586x_rtc *rtc = dev_get_drvdata(dev);
+	struct device *tps_dev = to_tps6586x_dev(dev);
+	u8 buff;
+	int err;
+
+	if (rtc->irq == -1)
+		return -EIO;
+
+	err = tps6586x_read(tps_dev, RTC_CTRL, &buff);
+	if (err < 0) {
+		dev_err(dev, "failed to read RTC_CTRL\n");
+		return err;
+	}
+
+	if ((enabled && (buff & RTC_ENABLE)) ||
+	    (!enabled && !(buff & RTC_ENABLE)))
+		return 0;
+
+	if (enabled) {
+		err = tps6586x_set_bits(tps_dev, RTC_CTRL, RTC_ENABLE);
+		if (err < 0) {
+			dev_err(dev, "failed to set RTC_ENABLE\n");
+			return err;
+		}
+
+		if (!rtc->irq_en) {
+			enable_irq(rtc->irq);
+			rtc->irq_en = true;
+		}
+	} else {
+		err = tps6586x_clr_bits(tps_dev, RTC_CTRL, RTC_ENABLE);
+		if (err < 0) {
+			dev_err(dev, "failed to clear RTC_ENABLE\n");
+			return err;
+		}
+
+		if (rtc->irq_en) {
+			disable_irq(rtc->irq);
+			rtc->irq_en = false;
+		}
+	}
+
+	return 0;
+}
+
+static const struct rtc_class_ops tps6586x_rtc_ops = {
+	.read_time	= tps6586x_rtc_read_time,
+	.set_time	= tps6586x_rtc_set_time,
+	.set_alarm	= tps6586x_rtc_set_alarm,
+	.read_alarm	= tps6586x_rtc_read_alarm,
+	.alarm_irq_enable = tps6586x_rtc_alarm_irq_enable,
+};
+
+static irqreturn_t tps6586x_rtc_irq(int irq, void *data)
+{
+	struct device *dev = data;
+	struct tps6586x_rtc *rtc = dev_get_drvdata(dev);
+
+	rtc_update_irq(rtc->rtc, 1, RTC_IRQF | RTC_AF);
+	return IRQ_HANDLED;
+}
+
+static int __devinit tps6586x_rtc_probe(struct platform_device *pdev)
+{
+	struct tps6586x_rtc_platform_data *pdata = pdev->dev.platform_data;
+	struct device *tps_dev = to_tps6586x_dev(&pdev->dev);
+	struct tps6586x_rtc *rtc;
+	int err;
+	struct tps6586x_epoch_start *epoch;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform_data specified\n");
+		return -EINVAL;
+	}
+
+	rtc = kzalloc(sizeof(*rtc), GFP_KERNEL);
+
+	if (!rtc)
+		return -ENOMEM;
+
+	rtc->irq = -1;
+
+	if (pdata->irq < 0)
+		dev_warn(&pdev->dev, "no IRQ specified, wakeup is disabled\n");
+
+	epoch = &pdata->start;
+	rtc->epoch_start = mktime(epoch->year, epoch->month, epoch->day,
+				  epoch->hour, epoch->min, epoch->sec);
+
+	dev_set_drvdata(&pdev->dev, rtc);
+
+	device_init_wakeup(&pdev->dev, 1);
+
+	rtc->rtc = rtc_device_register("tps6586x-rtc", &pdev->dev,
+				       &tps6586x_rtc_ops, THIS_MODULE);
+
+	if (IS_ERR(rtc->rtc)) {
+		err = PTR_ERR(rtc->rtc);
+		goto fail;
+	}
+
+	/* 1 kHz tick mode, enable tick counting */
+	err = tps6586x_update(tps_dev, RTC_CTRL,
+		RTC_ENABLE | OSC_SRC_SEL | ((pdata->cl_sel << CL_SEL_POS) &
+					    CL_SEL_MASK),
+		RTC_ENABLE | OSC_SRC_SEL | PRE_BYPASS | CL_SEL_MASK);
+	if (err < 0) {
+		dev_err(&pdev->dev, "unable to start counter\n");
+		goto fail;
+	}
+
+	if (pdata && (pdata->irq >= 0)) {
+		rtc->irq = pdata->irq;
+		err = request_threaded_irq(pdata->irq, NULL, tps6586x_rtc_irq,
+					   IRQF_ONESHOT, "tps6586x-rtc",
+					   &pdev->dev);
+		if (err) {
+			dev_warn(&pdev->dev, "unable to request IRQ(%d)\n", rtc->irq);
+			rtc->irq = -1;
+		} else {
+			enable_irq_wake(rtc->irq);
+			disable_irq(rtc->irq);
+		}
+	}
+
+	return 0;
+
+fail:
+	if (!IS_ERR_OR_NULL(rtc->rtc))
+		rtc_device_unregister(rtc->rtc);
+	device_init_wakeup(&pdev->dev, 0);
+	kfree(rtc);
+	return err;
+}
+
+static int __devexit tps6586x_rtc_remove(struct platform_device *pdev)
+{
+	struct tps6586x_rtc *rtc = dev_get_drvdata(&pdev->dev);
+
+	if (rtc->irq != -1)
+		free_irq(rtc->irq, rtc);
+	rtc_device_unregister(rtc->rtc);
+	kfree(rtc);
+	return 0;
+}
+
+static struct platform_driver tps6586x_rtc_driver = {
+	.driver	= {
+		.name	= "tps6586x-rtc",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= tps6586x_rtc_probe,
+	.remove	= __devexit_p(tps6586x_rtc_remove),
+};
+
+static int __init tps6586x_rtc_init(void)
+{
+	return platform_driver_register(&tps6586x_rtc_driver);
+}
+module_init(tps6586x_rtc_init);
+
+static void __exit tps6586x_rtc_exit(void)
+{
+	platform_driver_unregister(&tps6586x_rtc_driver);
+}
+module_exit(tps6586x_rtc_exit);
+
+MODULE_DESCRIPTION("TI TPS6586x RTC driver");
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:rtc-tps6586x");
diff --git a/drivers/rtc/rtc-tps6591x.c b/drivers/rtc/rtc-tps6591x.c
new file mode 100644
index 000000000000..9644dc4afb42
--- /dev/null
+++ b/drivers/rtc/rtc-tps6591x.c
@@ -0,0 +1,566 @@
+/*
+ * drivers/rtc/rtc_tps6591x.c
+ *
+ * RTC driver for TI TPS6591x
+ *
+ * Copyright (c) 2011-2012, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/* #define DEBUG		1 */
+/* #define VERBOSE_DEBUG	1 */
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/mfd/tps6591x.h>
+#include <linux/platform_device.h>
+#include <linux/rtc.h>
+#include <linux/slab.h>
+
+#define RTC_CTRL		0x10
+#define RTC_STATUS		0x11
+#define RTC_SECONDS_REG		0x0
+#define RTC_ALARM		0x8
+#define RTC_INT			0x12
+#define RTC_RESET_STATUS	0x16
+#define RTC_BBCH_REG		0x39
+
+#define RTC_BBCH_SEL		0x02
+#define RTC_BBCH_EN     	0x01
+#define ENABLE_ALARM_INT 0x8
+#define RTC_RESET_VALUE 0x80
+#define ALARM_INT_STATUS 0x40
+
+/*
+Linux RTC driver refers 1900 as base year in many calculations.
+(e.g. refer drivers/rtc/rtc-lib.c)
+*/
+#define OS_REF_YEAR 1900
+
+/*
+	PMU RTC have only 2 nibbles to store year information, so using an offset
+	of 100 to set the base year as 2000 for our driver.
+*/
+#define RTC_YEAR_OFFSET 100
+
+struct tps6591x_rtc {
+	unsigned long		epoch_start;
+	int			irq;
+	struct rtc_device	*rtc;
+	bool			irq_en;
+};
+
+static int tps6591x_read_regs(struct device *dev, int reg, int len,
+	uint8_t *val)
+{
+	int ret;
+
+	/* dummy read of STATUS_REG as per data sheet */
+	ret = tps6591x_reads(dev->parent, RTC_STATUS, 1, val);
+	if (ret < 0) {
+		dev_err(dev->parent, "\n %s failed reading from RTC_STATUS\n",
+			__func__);
+		WARN_ON(1);
+		return ret;
+	}
+
+	ret = tps6591x_reads(dev->parent, reg, len, val);
+	if (ret < 0) {
+		dev_err(dev->parent, "\n %s failed reading from 0x%02x\n",
+			__func__, reg);
+		WARN_ON(1);
+		return ret;
+	}
+	return 0;
+}
+
+static int tps6591x_write_regs(struct device *dev, int reg, int len,
+	uint8_t *val)
+{
+	int ret;
+	ret = tps6591x_writes(dev->parent, reg, len, val);
+	if (ret < 0) {
+		dev_err(dev->parent, "\n %s failed writing\n", __func__);
+		WARN_ON(1);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int tps6591x_rtc_valid_tm(struct rtc_time *tm)
+{
+	if (tm->tm_year >= (RTC_YEAR_OFFSET + 99)
+		|| tm->tm_year < (RTC_YEAR_OFFSET)
+		|| tm->tm_mon >= 12
+		|| tm->tm_mday < 1
+		|| tm->tm_mday > rtc_month_days(tm->tm_mon, tm->tm_year + OS_REF_YEAR)
+		|| tm->tm_hour >= 24
+		|| tm->tm_min >= 60
+		|| tm->tm_sec >= 60)
+		return -EINVAL;
+	return 0;
+}
+
+static u8 dec2bcd(u8 dec)
+{
+	return ((dec/10)<<4)+(dec%10);
+}
+
+static u8 bcd2dec(u8 bcd)
+{
+	return (bcd >> 4)*10+(bcd & 0xF);
+}
+
+static void convert_bcd_to_decimal(u8 *buf, u8 len)
+{
+	int i = 0;
+	for (i = 0; i < len; i++)
+		buf[i] = bcd2dec(buf[i]);
+}
+
+static void convert_decimal_to_bcd(u8 *buf, u8 len)
+{
+	int i = 0;
+	for (i = 0; i < len; i++)
+		buf[i] = dec2bcd(buf[i]);
+}
+
+static void print_time(struct device *dev, struct rtc_time *tm)
+{
+	dev_info(dev, "RTC Time : %d/%d/%d %d:%d:%d\n",
+		(tm->tm_mon + 1), tm->tm_mday, (tm->tm_year + OS_REF_YEAR),
+		tm->tm_hour, tm->tm_min , tm->tm_sec);
+}
+
+static int tps6591x_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	u8 buff[7];
+	int err;
+	err = tps6591x_read_regs(dev, RTC_SECONDS_REG, sizeof(buff), buff);
+	if (err < 0) {
+		dev_err(dev, "\n %s :: failed to read time\n", __FILE__);
+		return err;
+	}
+	convert_bcd_to_decimal(buff, sizeof(buff));
+	tm->tm_sec = buff[0];
+	tm->tm_min = buff[1];
+	tm->tm_hour = buff[2];
+	tm->tm_mday = buff[3];
+	tm->tm_mon = buff[4] - 1;
+	tm->tm_year = buff[5] + RTC_YEAR_OFFSET;
+	tm->tm_wday = buff[6];
+	print_time(dev, tm);
+	return tps6591x_rtc_valid_tm(tm);
+}
+
+static int tps6591x_rtc_stop(struct device *dev)
+{
+	u8 reg = 0;
+	u8 retries = 0;
+	int err;
+	do {
+		err = tps6591x_read_regs(dev, RTC_CTRL, 1, &reg);
+		if (err < 0) {
+			dev_err(dev->parent, "\n failed to read RTC_CTRL reg\n");
+			return err;
+		}
+
+		/* clear STOP bit alone */
+		reg &= ~0x1;
+
+		err = tps6591x_write_regs(dev, RTC_CTRL, 1, &reg);
+		if (err < 0) {
+			dev_err(dev->parent, "\n failed to program RTC_CTRL reg\n");
+			return err;
+		}
+
+		err = tps6591x_read_regs(dev, RTC_STATUS, 1, &reg);
+		if (err < 0) {
+			dev_err(dev->parent, "\n failed to read RTC_CTRL reg\n");
+			return err;
+		}
+		/* FixMe: Is allowing up to 5 retries sufficient?? */
+		if (retries++ == 5) {
+			dev_err(dev->parent, "\n failed to stop RTC\n");
+			return -EBUSY;
+		}
+	}	while (reg & 2);
+	return 0;
+}
+
+static int tps6591x_rtc_start(struct device *dev)
+{
+	u8 reg = 0;
+	u8 retries = 0;
+	int err;
+
+	do {
+		err = tps6591x_read_regs(dev, RTC_CTRL, 1, &reg);
+		if (err < 0) {
+			dev_err(dev->parent, "\n failed to read RTC_CTRL reg\n");
+			return err;
+		}
+
+		/* set STOP bit alone */
+		reg |= 0x1;
+
+		err = tps6591x_write_regs(dev, RTC_CTRL, 1, &reg);
+		if (err < 0) {
+			dev_err(dev->parent, "\n failed to program RTC_CTRL reg\n");
+			return err;
+		}
+
+		err = tps6591x_read_regs(dev, RTC_STATUS, 1, &reg);
+		if (err < 0) {
+			dev_err(dev->parent, "\n failed to read RTC_CTRL reg\n");
+			return err;
+		}
+		/* FixMe: Is allowing up to 5 retries sufficient?? */
+		if (retries++ == 5) {
+			dev_err(dev->parent, "\n failed to stop RTC\n");
+			return -EBUSY;
+		}
+	}	while (!(reg & 2));
+	return 0;
+}
+
+
+static int tps6591x_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	u8 buff[7];
+	int err;
+
+	err = tps6591x_rtc_valid_tm(tm);
+	if (err < 0) {
+		dev_err(dev->parent, "\n Invalid Time\n");
+		return err;
+	}
+
+	buff[0] = tm->tm_sec;
+	buff[1] = tm->tm_min;
+	buff[2] = tm->tm_hour;
+	buff[3] = tm->tm_mday;
+	buff[4] = tm->tm_mon + 1;
+	buff[5] = tm->tm_year % RTC_YEAR_OFFSET;
+	buff[6] = tm->tm_wday;
+
+	print_time(dev, tm);
+	convert_decimal_to_bcd(buff, sizeof(buff));
+	err = tps6591x_rtc_stop(dev);
+	if (err < 0) {
+		dev_err(dev->parent, "\n failed to clear RTC_ENABLE\n");
+		return err;
+	}
+
+	err = tps6591x_write_regs(dev, RTC_SECONDS_REG, sizeof(buff), buff);
+	if (err < 0) {
+		dev_err(dev->parent, "\n failed to program new time\n");
+		return err;
+	}
+
+	err = tps6591x_rtc_start(dev);
+	if (err < 0) {
+		dev_err(dev->parent, "\n failed to set RTC_ENABLE\n");
+		return err;
+	}
+
+	return 0;
+}
+
+static int tps6591x_rtc_alarm_irq_enable(struct device *dev,
+					 unsigned int enable)
+{
+	struct tps6591x_rtc *rtc = dev_get_drvdata(dev);
+	u8 reg;
+	int err;
+
+	if (rtc->irq == -1)
+		return -EIO;
+
+	if (enable) {
+		if (rtc->irq_en == true)
+			return 0;
+		err = tps6591x_read_regs(dev, RTC_INT, 1, &reg);
+		if (err)
+			return err;
+		reg |= 0x8;
+		err = tps6591x_write_regs(dev, RTC_INT, 1, &reg);
+		if (err)
+			return err;
+		rtc->irq_en = true;
+	} else {
+		if (rtc->irq_en == false)
+			return 0;
+		err = tps6591x_read_regs(dev, RTC_INT, 1, &reg);
+		if (err)
+			return err;
+		reg &= ~0x8;
+		err = tps6591x_write_regs(dev, RTC_INT, 1, &reg);
+		if (err)
+			return err;
+		rtc->irq_en = false;
+	}
+	return 0;
+}
+
+static int tps6591x_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct tps6591x_rtc *rtc = dev_get_drvdata(dev);
+	unsigned long seconds;
+	u8 buff[6];
+	int err;
+	struct rtc_time tm;
+
+	if (rtc->irq == -1)
+		return -EIO;
+
+	err = tps6591x_rtc_valid_tm(&alrm->time);
+	if (err < 0) {
+		dev_err(dev->parent, "\n Invalid alarm time\n");
+		return err;
+	}
+
+	dev_info(dev->parent, "\n setting alarm to requested time::\n");
+	print_time(dev->parent, &alrm->time);
+	rtc_tm_to_time(&alrm->time, &seconds);
+	tps6591x_rtc_read_time(dev, &tm);
+	rtc_tm_to_time(&tm, &rtc->epoch_start);
+
+	if (WARN_ON(alrm->enabled && (seconds < rtc->epoch_start))) {
+		dev_err(dev->parent, "\n can't set alarm to requested time\n");
+		return -EINVAL;
+	}
+
+	err = tps6591x_rtc_alarm_irq_enable(dev, alrm->enabled);
+	if(err) {
+		dev_err(dev->parent, "\n can't set alarm irq\n");
+		return err;
+	}
+
+	buff[0] = alrm->time.tm_sec;
+	buff[1] = alrm->time.tm_min;
+	buff[2] = alrm->time.tm_hour;
+	buff[3] = alrm->time.tm_mday;
+	buff[4] = alrm->time.tm_mon + 1;
+	buff[5] = alrm->time.tm_year % RTC_YEAR_OFFSET;
+	convert_decimal_to_bcd(buff, sizeof(buff));
+	err = tps6591x_write_regs(dev, RTC_ALARM, sizeof(buff), buff);
+	if (err)
+		dev_err(dev->parent, "\n unable to program alarm\n");
+
+	return err;
+}
+
+static int tps6591x_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	u8 buff[6];
+	int err;
+
+	err = tps6591x_read_regs(dev, RTC_ALARM, sizeof(buff), buff);
+	if (err)
+		return err;
+	convert_bcd_to_decimal(buff, sizeof(buff));
+
+	alrm->time.tm_sec = buff[0];
+	alrm->time.tm_min = buff[1];
+	alrm->time.tm_hour = buff[2];
+	alrm->time.tm_mday = buff[3];
+	alrm->time.tm_mon = buff[4] - 1;
+	alrm->time.tm_year = buff[5] + RTC_YEAR_OFFSET;
+
+	dev_info(dev->parent, "\n getting alarm time::\n");
+	print_time(dev, &alrm->time);
+
+	return 0;
+}
+
+static const struct rtc_class_ops tps6591x_rtc_ops = {
+	.read_time	= tps6591x_rtc_read_time,
+	.set_time	= tps6591x_rtc_set_time,
+	.set_alarm	= tps6591x_rtc_set_alarm,
+	.read_alarm	= tps6591x_rtc_read_alarm,
+	.alarm_irq_enable = tps6591x_rtc_alarm_irq_enable,
+};
+
+static irqreturn_t tps6591x_rtc_irq(int irq, void *data)
+{
+	struct device *dev = data;
+	struct tps6591x_rtc *rtc = dev_get_drvdata(dev);
+	u8 reg;
+	int err;
+
+	/* clear Alarm status bits.*/
+	err = tps6591x_read_regs(dev, RTC_STATUS, 1, &reg);
+	if (err) {
+		dev_err(dev->parent, "unable to read RTC_STATUS reg\n");
+		return -EBUSY;
+	}
+
+	reg = ALARM_INT_STATUS;
+	err = tps6591x_write_regs(dev, RTC_STATUS, 1, &reg);
+	if (err) {
+		dev_err(dev->parent, "unable to program RTC_STATUS reg\n");
+		return -EBUSY;
+	}
+
+	rtc_update_irq(rtc->rtc, 1, RTC_IRQF | RTC_AF);
+	return IRQ_HANDLED;
+}
+
+static int __devinit tps6591x_rtc_probe(struct platform_device *pdev)
+{
+	struct tps6591x_rtc_platform_data *pdata = pdev->dev.platform_data;
+	struct tps6591x_rtc *rtc;
+	struct rtc_time tm;
+	int err;
+	u8 reg;
+
+	rtc = kzalloc(sizeof(*rtc), GFP_KERNEL);
+
+	if (!rtc)
+		return -ENOMEM;
+
+	rtc->irq = -1;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform_data specified\n");
+		return -EINVAL;
+	}
+
+	if (pdata->irq < 0)
+		dev_err(&pdev->dev, "\n no IRQ specified, wakeup is disabled\n");
+
+	dev_set_drvdata(&pdev->dev, rtc);
+	device_init_wakeup(&pdev->dev, 1);
+	rtc->rtc = rtc_device_register(pdev->name, &pdev->dev,
+				       &tps6591x_rtc_ops, THIS_MODULE);
+
+	if (IS_ERR(rtc->rtc)) {
+		err = PTR_ERR(rtc->rtc);
+		goto fail;
+	}
+
+	if ((int)pdev && (int)&pdev->dev)
+		err = tps6591x_read_regs(&pdev->dev, RTC_STATUS, 1, &reg);
+	else {
+		dev_err(&pdev->dev, "\n %s Input params incorrect\n", __func__);
+		return -EBUSY;
+	}
+	if (err) {
+		dev_err(&pdev->dev, "\n %s unable to read status\n", __func__);
+		return -EBUSY;
+	}
+
+	reg = RTC_BBCH_SEL | RTC_BBCH_EN;
+	tps6591x_write_regs(&pdev->dev, RTC_BBCH_REG, 1, &reg);
+	if (err) {
+		dev_err(&pdev->dev, "unable to program Charger reg\n");
+		return -EBUSY;
+	}
+
+	err = tps6591x_rtc_start(&pdev->dev);
+	if (err) {
+		dev_err(&pdev->dev, "unable to start RTC\n");
+		return -EBUSY;
+	}
+
+	tps6591x_rtc_read_time(&pdev->dev, &tm);
+
+	if (tps6591x_rtc_valid_tm(&tm) < 0) {
+		if (pdata->time.tm_year < 2000 || pdata->time.tm_year >= 2100) {
+			memset(&pdata->time, 0, sizeof(pdata->time));
+			pdata->time.tm_year = 2000;
+			pdata->time.tm_mday = 1;
+		}
+		pdata->time.tm_year -= OS_REF_YEAR;
+		tps6591x_rtc_set_time(&pdev->dev, &pdata->time);
+	}
+
+	reg = ALARM_INT_STATUS;
+	err = tps6591x_write_regs(&pdev->dev, RTC_STATUS, 1, &reg);
+	if (err) {
+		dev_err(&pdev->dev, "unable to program RTC_STATUS reg\n");
+		return -EBUSY;
+	}
+
+	reg = ENABLE_ALARM_INT;
+	tps6591x_write_regs(&pdev->dev, RTC_INT, 1, &reg);
+	if (err) {
+		dev_err(&pdev->dev, "unable to program Interrupt Mask reg\n");
+		return -EBUSY;
+	}
+
+	if (pdata && (pdata->irq >= 0)) {
+		rtc->irq = pdata->irq;
+		err = request_threaded_irq(pdata->irq, NULL, tps6591x_rtc_irq,
+					IRQF_ONESHOT, "rtc_tps6591x",
+					&pdev->dev);
+		if (err) {
+			dev_err(&pdev->dev, "request IRQ:%d fail\n", rtc->irq);
+			rtc->irq = -1;
+		} else {
+			device_init_wakeup(&pdev->dev, 1);
+			enable_irq_wake(rtc->irq);
+		}
+	}
+	return 0;
+
+fail:
+	if (!IS_ERR_OR_NULL(rtc->rtc))
+		rtc_device_unregister(rtc->rtc);
+	kfree(rtc);
+	return err;
+}
+
+static int __devexit tps6591x_rtc_remove(struct platform_device *pdev)
+{
+	struct tps6591x_rtc *rtc = dev_get_drvdata(&pdev->dev);
+
+	if (rtc->irq != -1)
+		free_irq(rtc->irq, rtc);
+	rtc_device_unregister(rtc->rtc);
+	kfree(rtc);
+	return 0;
+}
+
+static struct platform_driver tps6591x_rtc_driver = {
+	.driver	= {
+		.name	= "rtc_tps6591x",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= tps6591x_rtc_probe,
+	.remove	= __devexit_p(tps6591x_rtc_remove),
+};
+
+static int __init tps6591x_rtc_init(void)
+{
+	return platform_driver_register(&tps6591x_rtc_driver);
+}
+module_init(tps6591x_rtc_init);
+
+static void __exit tps6591x_rtc_exit(void)
+{
+	platform_driver_unregister(&tps6591x_rtc_driver);
+}
+module_exit(tps6591x_rtc_exit);
+
+MODULE_DESCRIPTION("TI TPS6591x RTC driver");
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:rtc_tps6591x");
diff --git a/drivers/rtc/rtc-tps80031.c b/drivers/rtc/rtc-tps80031.c
new file mode 100644
index 000000000000..44013e13fe43
--- /dev/null
+++ b/drivers/rtc/rtc-tps80031.c
@@ -0,0 +1,502 @@
+/*
+ * drivers/rtc/rtc_tps80031.c
+ *
+ * RTC driver for TI TPS80031
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/* #define DEBUG		1 */
+/* #define VERBOSE_DEBUG	1 */
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/mfd/tps80031.h>
+#include <linux/platform_device.h>
+#include <linux/rtc.h>
+#include <linux/slab.h>
+#include <linux/gpio.h>
+
+#define RTC_CTRL		0x10
+#define RTC_STATUS		0x11
+#define RTC_SECONDS_REG		0x0
+#define RTC_ALARM		0x8
+#define RTC_INT			0x12
+#define RTC_RESET_STATUS	0x16
+
+#define ENABLE_ALARM_INT 0x8
+#define ALARM_INT_STATUS 0x40
+#define STOP_RTC 1
+
+/* Power on reset Values of RTC registers */
+#define RTC_POR_YEAR 0
+#define RTC_POR_MONTH 1
+#define RTC_POR_DAY 1
+
+/*
+Linux RTC driver refers 19th centaury as base year in many
+calculations. (e.g. refer drivers/rtc/rtc-lib.c)
+*/
+#define OS_REF_YEAR 1900
+
+/*
+	PMU RTC have only 2 nibbles to store year information, so using an
+	offset of 100 to set the base year as 2000 for our driver.
+*/
+#define RTC_YEAR_OFFSET 100
+
+struct tps80031_rtc {
+	unsigned long		epoch_start;
+	int			irq;
+	struct rtc_device	*rtc;
+	u8 			alarm_irq_enabled;
+	int			msecure_gpio;
+};
+
+static inline void tps80031_enable_rtc_write(struct device *dev)
+{
+	struct tps80031_rtc *rtc = dev_get_drvdata(dev);
+
+	if (rtc->msecure_gpio >= 0)
+		gpio_set_value(rtc->msecure_gpio, 1);
+}
+
+static inline void tps80031_disable_rtc_write(struct device *dev)
+{
+	struct tps80031_rtc *rtc = dev_get_drvdata(dev);
+
+	if (rtc->msecure_gpio >= 0)
+		gpio_set_value(rtc->msecure_gpio, 0);
+}
+
+static int tps80031_read_regs(struct device *dev, int reg, int len,
+	uint8_t *val)
+{
+	int ret;
+
+	/* dummy read of STATUS_REG as per data sheet */
+	ret = tps80031_reads(dev->parent, 1, RTC_STATUS, 1, val);
+	if (ret < 0) {
+		dev_err(dev->parent, "failed reading RTC_STATUS\n");
+		WARN_ON(1);
+		return ret;
+	}
+
+	ret = tps80031_reads(dev->parent, 1, reg, len, val);
+	if (ret < 0) {
+		dev_err(dev->parent, "failed reading from reg %d\n", reg);
+		WARN_ON(1);
+		return ret;
+	}
+	return 0;
+}
+
+static int tps80031_write_regs(struct device *dev, int reg, int len,
+	uint8_t *val)
+{
+	int ret;
+
+	tps80031_enable_rtc_write(dev);
+	ret = tps80031_writes(dev->parent, 1, reg, len, val);
+	if (ret < 0) {
+		tps80031_disable_rtc_write(dev);
+		dev_err(dev->parent, "failed writing reg: %d\n", reg);
+		WARN_ON(1);
+		return ret;
+	}
+	tps80031_disable_rtc_write(dev);
+	return 0;
+}
+
+static u8 dec2bcd(u8 dec)
+{
+	return ((dec/10)<<4)+(dec%10);
+}
+
+static u8 bcd2dec(u8 bcd)
+{
+	return (bcd >> 4)*10+(bcd & 0xF);
+}
+
+static void convert_bcd_to_decimal(u8 *buf, u8 len)
+{
+	int i = 0;
+	for (i = 0; i < len; i++)
+		buf[i] = bcd2dec(buf[i]);
+}
+
+static void convert_decimal_to_bcd(u8 *buf, u8 len)
+{
+	int i = 0;
+	for (i = 0; i < len; i++)
+		buf[i] = dec2bcd(buf[i]);
+}
+
+static int tps80031_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	u8 buff[7];
+	int err;
+	err = tps80031_read_regs(dev, RTC_SECONDS_REG, sizeof(buff), buff);
+	if (err < 0) {
+		dev_err(dev->parent, "failed reading time\n");
+		return err;
+	}
+	convert_bcd_to_decimal(buff, sizeof(buff));
+	tm->tm_sec = buff[0];
+	tm->tm_min = buff[1];
+	tm->tm_hour = buff[2];
+	tm->tm_mday = buff[3];
+	tm->tm_mon = buff[4] - 1;
+	tm->tm_year = buff[5] + RTC_YEAR_OFFSET;
+	tm->tm_wday = buff[6];
+	return 0;
+}
+
+static int tps80031_rtc_stop(struct device *dev)
+{
+	int err;
+
+	tps80031_enable_rtc_write(dev);
+	err = tps80031_clr_bits(dev->parent, 1, RTC_CTRL, STOP_RTC);
+	if (err < 0)
+		dev_err(dev->parent, "failed to stop RTC. err: %d\n", err);
+	tps80031_disable_rtc_write(dev);
+	return err;
+}
+
+static int tps80031_rtc_start(struct device *dev)
+{
+	int err;
+
+	tps80031_enable_rtc_write(dev);
+	err = tps80031_set_bits(dev->parent, 1, RTC_CTRL, STOP_RTC);
+	if (err < 0)
+		dev_err(dev->parent, "failed to start RTC. err: %d\n", err);
+	tps80031_disable_rtc_write(dev);
+	return err;
+}
+
+
+static int tps80031_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	u8 buff[7];
+	int err;
+
+	buff[0] = tm->tm_sec;
+	buff[1] = tm->tm_min;
+	buff[2] = tm->tm_hour;
+	buff[3] = tm->tm_mday;
+	buff[4] = tm->tm_mon + 1;
+	buff[5] = tm->tm_year % RTC_YEAR_OFFSET;
+	buff[6] = tm->tm_wday;
+
+	convert_decimal_to_bcd(buff, sizeof(buff));
+	err = tps80031_rtc_stop(dev);
+	if (err < 0)
+		return err;
+
+	err = tps80031_write_regs(dev, RTC_SECONDS_REG, sizeof(buff), buff);
+	if (err < 0) {
+		dev_err(dev->parent, "failed to program new time\n");
+		return err;
+	}
+
+	err = tps80031_rtc_start(dev);
+	return err;
+}
+
+static int tps80031_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct tps80031_rtc *rtc = dev_get_drvdata(dev);
+	unsigned long seconds;
+	u8 buff[6];
+	int err;
+	struct rtc_time tm;
+
+	if (rtc->irq == -1)
+		return -EIO;
+
+	rtc_tm_to_time(&alrm->time, &seconds);
+	tps80031_rtc_read_time(dev, &tm);
+	rtc_tm_to_time(&tm, &rtc->epoch_start);
+
+	if (WARN_ON(alrm->enabled && (seconds < rtc->epoch_start))) {
+		dev_err(dev->parent, "can't set alarm to requested time\n");
+		return -EINVAL;
+	}
+
+	buff[0] = alrm->time.tm_sec;
+	buff[1] = alrm->time.tm_min;
+	buff[2] = alrm->time.tm_hour;
+	buff[3] = alrm->time.tm_mday;
+	buff[4] = alrm->time.tm_mon + 1;
+	buff[5] = alrm->time.tm_year % RTC_YEAR_OFFSET;
+	convert_decimal_to_bcd(buff, sizeof(buff));
+	err = tps80031_write_regs(dev, RTC_ALARM, sizeof(buff), buff);
+	if (err)
+		dev_err(dev->parent, "unable to program alarm\n");
+
+	return err;
+}
+
+static int tps80031_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	u8 buff[6];
+	int err;
+
+	err = tps80031_read_regs(dev, RTC_ALARM, sizeof(buff), buff);
+	if (err)
+		return err;
+	convert_bcd_to_decimal(buff, sizeof(buff));
+
+	alrm->time.tm_sec = buff[0];
+	alrm->time.tm_min = buff[1];
+	alrm->time.tm_hour = buff[2];
+	alrm->time.tm_mday = buff[3];
+	alrm->time.tm_mon = buff[4] - 1;
+	alrm->time.tm_year = buff[5] + RTC_YEAR_OFFSET;
+
+	return 0;
+}
+
+static int tps80031_rtc_alarm_irq_enable(struct device *dev,
+					 unsigned int enable)
+{
+	struct tps80031_rtc *rtc = dev_get_drvdata(dev);
+	int err;
+	struct device *p = dev->parent;
+
+	if (rtc->irq == -1)
+		return -EIO;
+
+	if (enable) {
+		if (rtc->alarm_irq_enabled)
+			return 0;
+
+		tps80031_enable_rtc_write(dev);
+		err = tps80031_set_bits(p, 1, RTC_INT, ENABLE_ALARM_INT);
+		tps80031_disable_rtc_write(dev);
+		if (err < 0) {
+			dev_err(p, "failed to set ALRM int. err: %d\n", err);
+			return err;
+		} else
+			rtc->alarm_irq_enabled = 1;
+	} else {
+		if(!rtc->alarm_irq_enabled)
+			return 0;
+		tps80031_enable_rtc_write(dev);
+		err = tps80031_clr_bits(p, 1, RTC_INT, ENABLE_ALARM_INT);
+		tps80031_disable_rtc_write(dev);
+		if (err < 0) {
+			dev_err(p, "failed to clear ALRM int. err: %d\n", err);
+			return err;
+		} else
+			rtc->alarm_irq_enabled = 0;
+	}
+	return 0;
+}
+
+static const struct rtc_class_ops tps80031_rtc_ops = {
+	.read_time = tps80031_rtc_read_time,
+	.set_time = tps80031_rtc_set_time,
+	.set_alarm = tps80031_rtc_set_alarm,
+	.read_alarm = tps80031_rtc_read_alarm,
+	.alarm_irq_enable = tps80031_rtc_alarm_irq_enable,
+};
+
+static irqreturn_t tps80031_rtc_irq(int irq, void *data)
+{
+	struct device *dev = data;
+	struct tps80031_rtc *rtc = dev_get_drvdata(dev);
+	u8 reg;
+	int err;
+
+	/* clear Alarm status bits.*/
+	err = tps80031_read_regs(dev, RTC_STATUS, 1, &reg);
+	if (err) {
+		dev_err(dev->parent, "unable to read RTC_STATUS reg\n");
+		return -EBUSY;
+	}
+
+	tps80031_enable_rtc_write(dev);
+	err = tps80031_force_update(dev->parent, 1, RTC_STATUS,
+		ALARM_INT_STATUS, ALARM_INT_STATUS);
+	tps80031_disable_rtc_write(dev);
+	if (err) {
+		dev_err(dev->parent, "unable to set Alarm INT\n");
+		return -EBUSY;
+	}
+
+	rtc_update_irq(rtc->rtc, 1, RTC_IRQF | RTC_AF);
+	return IRQ_HANDLED;
+}
+
+static int __devinit tps80031_rtc_probe(struct platform_device *pdev)
+{
+	struct tps80031_platform_data *tps80031_pdata;
+	struct tps80031_rtc_platform_data *pdata;
+	struct tps80031_rtc *rtc;
+	struct rtc_time tm;
+	int err;
+	u8 reg;
+
+	tps80031_pdata = dev_get_platdata(pdev->dev.parent);
+	if (!tps80031_pdata) {
+		dev_err(&pdev->dev, "no tps80031 platform_data specified\n");
+		return -EINVAL;
+	}
+
+	pdata = tps80031_pdata->rtc_pdata;
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform_data specified\n");
+		return -EINVAL;
+	}
+
+	rtc = kzalloc(sizeof(*rtc), GFP_KERNEL);
+	if (!rtc)
+		return -ENOMEM;
+
+	rtc->irq = -1;
+	if (pdata->irq < 0)
+		dev_err(&pdev->dev, "no IRQ specified, wakeup is disabled\n");
+
+	rtc->msecure_gpio = -1;
+	if (gpio_is_valid(pdata->msecure_gpio)) {
+		err = gpio_request(pdata->msecure_gpio, "tps80031 msecure");
+		if (err == 0) {
+			rtc->msecure_gpio = pdata->msecure_gpio;
+			gpio_direction_output(rtc->msecure_gpio, 0);
+		} else
+			dev_warn(&pdev->dev, "could not get msecure GPIO\n");
+	}
+
+	rtc->rtc = rtc_device_register(pdev->name, &pdev->dev,
+				       &tps80031_rtc_ops, THIS_MODULE);
+	dev_set_drvdata(&pdev->dev, rtc);
+
+	if (IS_ERR(rtc->rtc)) {
+		err = PTR_ERR(rtc->rtc);
+		goto fail;
+	}
+
+	if ((int)pdev && (int)&pdev->dev)
+		err = tps80031_read_regs(&pdev->dev, RTC_STATUS, 1, &reg);
+	else {
+		dev_err(&pdev->dev, "%s Input params incorrect\n", __func__);
+		err = -EBUSY;
+		goto fail;
+	}
+
+	if (err) {
+		dev_err(&pdev->dev, "%s unable to read status\n", __func__);
+		err = -EBUSY;
+		goto fail;
+	}
+
+	/* If RTC have POR values, set time using platform data*/
+	tps80031_rtc_read_time(&pdev->dev, &tm);
+	if ((tm.tm_year == RTC_YEAR_OFFSET + RTC_POR_YEAR) &&
+		(tm.tm_mon == (RTC_POR_MONTH - 1)) &&
+		(tm.tm_mday == RTC_POR_DAY)) {
+		if (pdata->time.tm_year < 2000 ||
+			pdata->time.tm_year > 2100) {
+			dev_err(&pdev->dev, "Invalid platform data\n");
+			memset(&pdata->time, 0, sizeof(pdata->time));
+			pdata->time.tm_year = 2011;
+			pdata->time.tm_mday = 1;
+		}
+		tps80031_rtc_set_time(&pdev->dev, &pdata->time);
+	}
+
+	reg = ALARM_INT_STATUS;
+	err = tps80031_write_regs(&pdev->dev, RTC_STATUS, 1, &reg);
+	if (err) {
+		dev_err(&pdev->dev, "unable to program RTC_STATUS reg\n");
+		return -EBUSY;
+	}
+
+	tps80031_enable_rtc_write(&pdev->dev);
+	err = tps80031_set_bits(pdev->dev.parent, 1, RTC_INT, ENABLE_ALARM_INT);
+	tps80031_disable_rtc_write(&pdev->dev);
+	if (err) {
+		dev_err(&pdev->dev, "unable to program Interrupt Mask reg\n");
+		err = -EBUSY;
+		rtc->alarm_irq_enabled = 0;
+		goto fail;
+	} else
+		rtc->alarm_irq_enabled = 1;
+
+	if (pdata && (pdata->irq >= 0)) {
+		rtc->irq = pdata->irq;
+		err = request_threaded_irq(pdata->irq, NULL, tps80031_rtc_irq,
+					IRQF_ONESHOT, "rtc_tps80031",
+					&pdev->dev);
+		if (err) {
+			dev_err(&pdev->dev, "request IRQ:%d fail\n", rtc->irq);
+			rtc->irq = -1;
+		} else {
+			device_init_wakeup(&pdev->dev, 1);
+			enable_irq_wake(rtc->irq);
+		}
+	}
+	return 0;
+
+fail:
+	if (!IS_ERR_OR_NULL(rtc->rtc))
+		rtc_device_unregister(rtc->rtc);
+	kfree(rtc);
+	return err;
+}
+
+static int __devexit tps80031_rtc_remove(struct platform_device *pdev)
+{
+	struct tps80031_rtc *rtc = dev_get_drvdata(&pdev->dev);
+
+	if (rtc->irq != -1)
+		free_irq(rtc->irq, rtc);
+	rtc_device_unregister(rtc->rtc);
+	kfree(rtc);
+	return 0;
+}
+
+static struct platform_driver tps80031_rtc_driver = {
+	.driver	= {
+		.name	= "tps80031-rtc",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= tps80031_rtc_probe,
+	.remove	= __devexit_p(tps80031_rtc_remove),
+};
+
+static int __init tps80031_rtc_init(void)
+{
+	return platform_driver_register(&tps80031_rtc_driver);
+}
+module_init(tps80031_rtc_init);
+
+static void __exit tps80031_rtc_exit(void)
+{
+	platform_driver_unregister(&tps80031_rtc_driver);
+}
+module_exit(tps80031_rtc_exit);
+
+MODULE_DESCRIPTION("TI TPS80031 RTC driver");
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:rtc_tps80031");
diff --git a/drivers/s390/cio/ccwgroup.c b/drivers/s390/cio/ccwgroup.c
index 5c567414c4bb..cda9bd6e48e8 100644
--- a/drivers/s390/cio/ccwgroup.c
+++ b/drivers/s390/cio/ccwgroup.c
@@ -87,6 +87,12 @@ static void __ccwgroup_remove_cdev_refs(struct ccwgroup_device *gdev)
 	}
 }
 
+static ssize_t ccwgroup_online_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count);
+static ssize_t ccwgroup_online_show(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf);
 /*
  * Provide an 'ungroup' attribute so the user can remove group devices no
  * longer needed or accidentially created. Saves memory :)
@@ -134,6 +140,20 @@ out:
 }
 
 static DEVICE_ATTR(ungroup, 0200, NULL, ccwgroup_ungroup_store);
+static DEVICE_ATTR(online, 0644, ccwgroup_online_show, ccwgroup_online_store);
+
+static struct attribute *ccwgroup_attrs[] = {
+	&dev_attr_online.attr,
+	&dev_attr_ungroup.attr,
+	NULL,
+};
+static struct attribute_group ccwgroup_attr_group = {
+	.attrs = ccwgroup_attrs,
+};
+static const struct attribute_group *ccwgroup_attr_groups[] = {
+	&ccwgroup_attr_group,
+	NULL,
+};
 
 static void
 ccwgroup_release (struct device *dev)
@@ -293,25 +313,17 @@ int ccwgroup_create_from_string(struct device *root, unsigned int creator_id,
 	}
 
 	dev_set_name(&gdev->dev, "%s", dev_name(&gdev->cdev[0]->dev));
-
+	gdev->dev.groups = ccwgroup_attr_groups;
 	rc = device_add(&gdev->dev);
 	if (rc)
 		goto error;
 	get_device(&gdev->dev);
-	rc = device_create_file(&gdev->dev, &dev_attr_ungroup);
-
-	if (rc) {
-		device_unregister(&gdev->dev);
-		goto error;
-	}
-
 	rc = __ccwgroup_create_symlinks(gdev);
 	if (!rc) {
 		mutex_unlock(&gdev->reg_mutex);
 		put_device(&gdev->dev);
 		return 0;
 	}
-	device_remove_file(&gdev->dev, &dev_attr_ungroup);
 	device_unregister(&gdev->dev);
 error:
 	for (i = 0; i < num_devices; i++)
@@ -423,7 +435,7 @@ ccwgroup_online_store (struct device *dev, struct device_attribute *attr, const
 	int ret;
 
 	if (!dev->driver)
-		return -ENODEV;
+		return -EINVAL;
 
 	gdev = to_ccwgroupdev(dev);
 	gdrv = to_ccwgroupdrv(dev->driver);
@@ -456,8 +468,6 @@ ccwgroup_online_show (struct device *dev, struct device_attribute *attr, char *b
 	return sprintf(buf, online ? "1\n" : "0\n");
 }
 
-static DEVICE_ATTR(online, 0644, ccwgroup_online_show, ccwgroup_online_store);
-
 static int
 ccwgroup_probe (struct device *dev)
 {
@@ -469,12 +479,7 @@ ccwgroup_probe (struct device *dev)
 	gdev = to_ccwgroupdev(dev);
 	gdrv = to_ccwgroupdrv(dev->driver);
 
-	if ((ret = device_create_file(dev, &dev_attr_online)))
-		return ret;
-
 	ret = gdrv->probe ? gdrv->probe(gdev) : -ENODEV;
-	if (ret)
-		device_remove_file(dev, &dev_attr_online);
 
 	return ret;
 }
@@ -485,9 +490,6 @@ ccwgroup_remove (struct device *dev)
 	struct ccwgroup_device *gdev;
 	struct ccwgroup_driver *gdrv;
 
-	device_remove_file(dev, &dev_attr_online);
-	device_remove_file(dev, &dev_attr_ungroup);
-
 	if (!dev->driver)
 		return 0;
 
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index fafb8c299540..c74e8670fab0 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -2740,11 +2740,13 @@ int inline qeth_l3_get_cast_type(struct qeth_card *card, struct sk_buff *skb)
 	struct neighbour *n = NULL;
 	struct dst_entry *dst;
 
+	rcu_read_lock();
 	dst = skb_dst(skb);
 	if (dst)
 		n = dst_get_neighbour(dst);
 	if (n) {
 		cast_type = n->type;
+		rcu_read_unlock();
 		if ((cast_type == RTN_BROADCAST) ||
 		    (cast_type == RTN_MULTICAST) ||
 		    (cast_type == RTN_ANYCAST))
@@ -2752,6 +2754,8 @@ int inline qeth_l3_get_cast_type(struct qeth_card *card, struct sk_buff *skb)
 		else
 			return RTN_UNSPEC;
 	}
+	rcu_read_unlock();
+
 	/* try something else */
 	if (skb->protocol == ETH_P_IPV6)
 		return (skb_network_header(skb)[24] == 0xff) ?
@@ -2807,6 +2811,8 @@ static void qeth_l3_fill_header(struct qeth_card *card, struct qeth_hdr *hdr,
 	}
 
 	hdr->hdr.l3.length = skb->len - sizeof(struct qeth_hdr);
+
+	rcu_read_lock();
 	dst = skb_dst(skb);
 	if (dst)
 		n = dst_get_neighbour(dst);
@@ -2853,6 +2859,7 @@ static void qeth_l3_fill_header(struct qeth_card *card, struct qeth_hdr *hdr,
 				QETH_CAST_UNICAST | QETH_HDR_PASSTHRU;
 		}
 	}
+	rcu_read_unlock();
 }
 
 static inline void qeth_l3_hdr_csum(struct qeth_card *card,
diff --git a/drivers/s390/scsi/zfcp_scsi.c b/drivers/s390/scsi/zfcp_scsi.c
index 7cac873c7383..169ba7b75042 100644
--- a/drivers/s390/scsi/zfcp_scsi.c
+++ b/drivers/s390/scsi/zfcp_scsi.c
@@ -57,6 +57,10 @@ static void zfcp_scsi_slave_destroy(struct scsi_device *sdev)
 {
 	struct zfcp_scsi_dev *zfcp_sdev = sdev_to_zfcp(sdev);
 
+	/* if previous slave_alloc returned early, there is nothing to do */
+	if (!zfcp_sdev->port)
+		return;
+
 	zfcp_erp_lun_shutdown_wait(sdev, "scssd_1");
 	put_device(&zfcp_sdev->port->dev);
 }
diff --git a/drivers/scsi/aacraid/linit.c b/drivers/scsi/aacraid/linit.c
index 3382475dc22d..c7b6fed88735 100644
--- a/drivers/scsi/aacraid/linit.c
+++ b/drivers/scsi/aacraid/linit.c
@@ -38,6 +38,7 @@
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/pci.h>
+#include <linux/pci-aspm.h>
 #include <linux/slab.h>
 #include <linux/mutex.h>
 #include <linux/spinlock.h>
@@ -1108,6 +1109,9 @@ static int __devinit aac_probe_one(struct pci_dev *pdev,
 		unique_id++;
 	}
 
+	pci_disable_link_state(pdev, PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1 |
+			       PCIE_LINK_STATE_CLKPM);
+
 	error = pci_enable_device(pdev);
 	if (error)
 		goto out;
diff --git a/drivers/scsi/device_handler/scsi_dh.c b/drivers/scsi/device_handler/scsi_dh.c
index 0119b8147797..d973325ded2a 100644
--- a/drivers/scsi/device_handler/scsi_dh.c
+++ b/drivers/scsi/device_handler/scsi_dh.c
@@ -398,7 +398,15 @@ int scsi_dh_activate(struct request_queue *q, activate_complete fn, void *data)
 
 	spin_lock_irqsave(q->queue_lock, flags);
 	sdev = q->queuedata;
-	if (sdev && sdev->scsi_dh_data)
+	if (!sdev) {
+		spin_unlock_irqrestore(q->queue_lock, flags);
+		err = SCSI_DH_NOSYS;
+		if (fn)
+			fn(data, err);
+		return err;
+	}
+
+	if (sdev->scsi_dh_data)
 		scsi_dh = sdev->scsi_dh_data->scsi_dh;
 	dev = get_device(&sdev->sdev_gendev);
 	if (!scsi_dh || !dev ||
diff --git a/drivers/scsi/fcoe/fcoe.c b/drivers/scsi/fcoe/fcoe.c
index 5d0e9a24ae94..8858170add3d 100644
--- a/drivers/scsi/fcoe/fcoe.c
+++ b/drivers/scsi/fcoe/fcoe.c
@@ -1635,6 +1635,7 @@ static inline int fcoe_filter_frames(struct fc_lport *lport,
 	stats->InvalidCRCCount++;
 	if (stats->InvalidCRCCount < 5)
 		printk(KERN_WARNING "fcoe: dropping frame with CRC error\n");
+	put_cpu();
 	return -EINVAL;
 }
 
diff --git a/drivers/scsi/hosts.c b/drivers/scsi/hosts.c
index 4f7a5829ea4c..351dc0b86fab 100644
--- a/drivers/scsi/hosts.c
+++ b/drivers/scsi/hosts.c
@@ -286,6 +286,7 @@ static void scsi_host_dev_release(struct device *dev)
 {
 	struct Scsi_Host *shost = dev_to_shost(dev);
 	struct device *parent = dev->parent;
+	struct request_queue *q;
 
 	scsi_proc_hostdir_rm(shost->hostt);
 
@@ -293,9 +294,11 @@ static void scsi_host_dev_release(struct device *dev)
 		kthread_stop(shost->ehandler);
 	if (shost->work_q)
 		destroy_workqueue(shost->work_q);
-	if (shost->uspace_req_q) {
-		kfree(shost->uspace_req_q->queuedata);
-		scsi_free_queue(shost->uspace_req_q);
+	q = shost->uspace_req_q;
+	if (q) {
+		kfree(q->queuedata);
+		q->queuedata = NULL;
+		scsi_free_queue(q);
 	}
 
 	scsi_destroy_command_freelist(shost);
diff --git a/drivers/scsi/hpsa.c b/drivers/scsi/hpsa.c
index b200b736b000..418ce83694a6 100644
--- a/drivers/scsi/hpsa.c
+++ b/drivers/scsi/hpsa.c
@@ -23,6 +23,7 @@
 #include <linux/interrupt.h>
 #include <linux/types.h>
 #include <linux/pci.h>
+#include <linux/pci-aspm.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/delay.h>
@@ -3300,6 +3301,13 @@ static int hpsa_controller_hard_reset(struct pci_dev *pdev,
 		pmcsr &= ~PCI_PM_CTRL_STATE_MASK;
 		pmcsr |= PCI_D0;
 		pci_write_config_word(pdev, pos + PCI_PM_CTRL, pmcsr);
+
+		/*
+		 * The P600 requires a small delay when changing states.
+		 * Otherwise we may think the board did not reset and we bail.
+		 * This for kdump only and is particular to the P600.
+		 */
+		msleep(500);
 	}
 	return 0;
 }
@@ -3880,6 +3888,10 @@ static int __devinit hpsa_pci_init(struct ctlr_info *h)
 		dev_warn(&h->pdev->dev, "controller appears to be disabled\n");
 		return -ENODEV;
 	}
+
+	pci_disable_link_state(h->pdev, PCIE_LINK_STATE_L0S |
+			       PCIE_LINK_STATE_L1 | PCIE_LINK_STATE_CLKPM);
+
 	err = pci_enable_device(h->pdev);
 	if (err) {
 		dev_warn(&h->pdev->dev, "unable to enable PCI device\n");
diff --git a/drivers/scsi/ipr.c b/drivers/scsi/ipr.c
index 8d636301e32c..acbb9241262d 100644
--- a/drivers/scsi/ipr.c
+++ b/drivers/scsi/ipr.c
@@ -8812,7 +8812,7 @@ static int __devinit ipr_probe_ioa(struct pci_dev *pdev,
 	uproc = readl(ioa_cfg->regs.sense_uproc_interrupt_reg32);
 	if ((mask & IPR_PCII_HRRQ_UPDATED) == 0 || (uproc & IPR_UPROCI_RESET_ALERT))
 		ioa_cfg->needs_hard_reset = 1;
-	if (interrupts & IPR_PCII_ERROR_INTERRUPTS)
+	if ((interrupts & IPR_PCII_ERROR_INTERRUPTS) || reset_devices)
 		ioa_cfg->needs_hard_reset = 1;
 	if (interrupts & IPR_PCII_IOA_UNIT_CHECKED)
 		ioa_cfg->ioa_unit_checked = 1;
diff --git a/drivers/scsi/isci/isci.h b/drivers/scsi/isci/isci.h
index d1de63312e7f..8efeb6b08321 100644
--- a/drivers/scsi/isci/isci.h
+++ b/drivers/scsi/isci/isci.h
@@ -97,7 +97,7 @@
 #define SCU_MAX_COMPLETION_QUEUE_SHIFT	  (ilog2(SCU_MAX_COMPLETION_QUEUE_ENTRIES))
 
 #define SCU_ABSOLUTE_MAX_UNSOLICITED_FRAMES (4096)
-#define SCU_UNSOLICITED_FRAME_BUFFER_SIZE   (1024)
+#define SCU_UNSOLICITED_FRAME_BUFFER_SIZE   (1024U)
 #define SCU_INVALID_FRAME_INDEX             (0xFFFF)
 
 #define SCU_IO_REQUEST_MAX_SGE_SIZE         (0x00FFFFFF)
diff --git a/drivers/scsi/isci/port_config.c b/drivers/scsi/isci/port_config.c
index 486b113c634a..38a99d281141 100644
--- a/drivers/scsi/isci/port_config.c
+++ b/drivers/scsi/isci/port_config.c
@@ -678,7 +678,7 @@ static void apc_agent_timeout(unsigned long data)
 	configure_phy_mask = ~port_agent->phy_configured_mask & port_agent->phy_ready_mask;
 
 	if (!configure_phy_mask)
-		return;
+		goto done;
 
 	for (index = 0; index < SCI_MAX_PHYS; index++) {
 		if ((configure_phy_mask & (1 << index)) == 0)
diff --git a/drivers/scsi/isci/request.c b/drivers/scsi/isci/request.c
index b5d3a8c4d329..225b196800a2 100644
--- a/drivers/scsi/isci/request.c
+++ b/drivers/scsi/isci/request.c
@@ -1490,29 +1490,30 @@ sci_io_request_frame_handler(struct isci_request *ireq,
 		return SCI_SUCCESS;
 
 	case SCI_REQ_SMP_WAIT_RESP: {
-		struct smp_resp *rsp_hdr = &ireq->smp.rsp;
-		void *frame_header;
+		struct sas_task *task = isci_request_access_task(ireq);
+		struct scatterlist *sg = &task->smp_task.smp_resp;
+		void *frame_header, *kaddr;
+		u8 *rsp;
 
 		sci_unsolicited_frame_control_get_header(&ihost->uf_control,
-							      frame_index,
-							      &frame_header);
-
-		/* byte swap the header. */
-		word_cnt = SMP_RESP_HDR_SZ / sizeof(u32);
-		sci_swab32_cpy(rsp_hdr, frame_header, word_cnt);
+							 frame_index,
+							 &frame_header);
+		kaddr = kmap_atomic(sg_page(sg), KM_IRQ0);
+		rsp = kaddr + sg->offset;
+		sci_swab32_cpy(rsp, frame_header, 1);
 
-		if (rsp_hdr->frame_type == SMP_RESPONSE) {
+		if (rsp[0] == SMP_RESPONSE) {
 			void *smp_resp;
 
 			sci_unsolicited_frame_control_get_buffer(&ihost->uf_control,
-								      frame_index,
-								      &smp_resp);
+								 frame_index,
+								 &smp_resp);
 
-			word_cnt = (sizeof(struct smp_resp) - SMP_RESP_HDR_SZ) /
-				sizeof(u32);
-
-			sci_swab32_cpy(((u8 *) rsp_hdr) + SMP_RESP_HDR_SZ,
-				       smp_resp, word_cnt);
+			word_cnt = (sg->length/4)-1;
+			if (word_cnt > 0)
+				word_cnt = min_t(unsigned int, word_cnt,
+						 SCU_UNSOLICITED_FRAME_BUFFER_SIZE/4);
+			sci_swab32_cpy(rsp + 4, smp_resp, word_cnt);
 
 			ireq->scu_status = SCU_TASK_DONE_GOOD;
 			ireq->sci_status = SCI_SUCCESS;
@@ -1528,12 +1529,13 @@ sci_io_request_frame_handler(struct isci_request *ireq,
 				__func__,
 				ireq,
 				frame_index,
-				rsp_hdr->frame_type);
+				rsp[0]);
 
 			ireq->scu_status = SCU_TASK_DONE_SMP_FRM_TYPE_ERR;
 			ireq->sci_status = SCI_FAILURE_CONTROLLER_SPECIFIC_IO_ERR;
 			sci_change_state(&ireq->sm, SCI_REQ_COMPLETED);
 		}
+		kunmap_atomic(kaddr, KM_IRQ0);
 
 		sci_controller_release_frame(ihost, frame_index);
 
@@ -2603,18 +2605,7 @@ static void isci_request_io_request_complete(struct isci_host *ihost,
 			status   = SAM_STAT_GOOD;
 			set_bit(IREQ_COMPLETE_IN_TARGET, &request->flags);
 
-			if (task->task_proto == SAS_PROTOCOL_SMP) {
-				void *rsp = &request->smp.rsp;
-
-				dev_dbg(&ihost->pdev->dev,
-					"%s: SMP protocol completion\n",
-					__func__);
-
-				sg_copy_from_buffer(
-					&task->smp_task.smp_resp, 1,
-					rsp, sizeof(struct smp_resp));
-			} else if (completion_status
-				   == SCI_IO_SUCCESS_IO_DONE_EARLY) {
+			if (completion_status == SCI_IO_SUCCESS_IO_DONE_EARLY) {
 
 				/* This was an SSP / STP / SATA transfer.
 				 * There is a possibility that less data than
diff --git a/drivers/scsi/isci/request.h b/drivers/scsi/isci/request.h
index 7a1d5a9778eb..58d70b6606ef 100644
--- a/drivers/scsi/isci/request.h
+++ b/drivers/scsi/isci/request.h
@@ -174,9 +174,6 @@ struct isci_request {
 			};
 		} ssp;
 		struct {
-			struct smp_resp rsp;
-		} smp;
-		struct {
 			struct isci_stp_request req;
 			struct host_to_dev_fis cmd;
 			struct dev_to_host_fis rsp;
diff --git a/drivers/scsi/isci/sas.h b/drivers/scsi/isci/sas.h
index 462b15174d3f..dc26b4aea99e 100644
--- a/drivers/scsi/isci/sas.h
+++ b/drivers/scsi/isci/sas.h
@@ -204,8 +204,6 @@ struct smp_req {
 	u8 req_data[0];
 }  __packed;
 
-#define SMP_RESP_HDR_SZ	4
-
 /*
  * struct sci_sas_address - This structure depicts how a SAS address is
  *    represented by SCI.
diff --git a/drivers/scsi/libsas/sas_expander.c b/drivers/scsi/libsas/sas_expander.c
index 16ad97df5ba6..37cbe4d3bb9c 100644
--- a/drivers/scsi/libsas/sas_expander.c
+++ b/drivers/scsi/libsas/sas_expander.c
@@ -199,6 +199,8 @@ static void sas_set_ex_phy(struct domain_device *dev, int phy_id,
 	phy->virtual = dr->virtual;
 	phy->last_da_index = -1;
 
+	phy->phy->identify.sas_address = SAS_ADDR(phy->attached_sas_addr);
+	phy->phy->identify.device_type = phy->attached_dev_type;
 	phy->phy->identify.initiator_port_protocols = phy->attached_iproto;
 	phy->phy->identify.target_port_protocols = phy->attached_tproto;
 	phy->phy->identify.phy_identifier = phy_id;
diff --git a/drivers/scsi/megaraid/megaraid_sas_base.c b/drivers/scsi/megaraid/megaraid_sas_base.c
index 776d01988660..839ad7ba32f9 100644
--- a/drivers/scsi/megaraid/megaraid_sas_base.c
+++ b/drivers/scsi/megaraid/megaraid_sas_base.c
@@ -1907,7 +1907,6 @@ static int megasas_generic_reset(struct scsi_cmnd *scmd)
 static enum
 blk_eh_timer_return megasas_reset_timer(struct scsi_cmnd *scmd)
 {
-	struct megasas_cmd *cmd = (struct megasas_cmd *)scmd->SCp.ptr;
 	struct megasas_instance *instance;
 	unsigned long flags;
 
@@ -1916,7 +1915,7 @@ blk_eh_timer_return megasas_reset_timer(struct scsi_cmnd *scmd)
 		return BLK_EH_NOT_HANDLED;
 	}
 
-	instance = cmd->instance;
+	instance = (struct megasas_instance *)scmd->device->host->hostdata;
 	if (!(instance->flag & MEGASAS_FW_BUSY)) {
 		/* FW is busy, throttle IO */
 		spin_lock_irqsave(instance->host->host_lock, flags);
diff --git a/drivers/scsi/mpt2sas/mpt2sas_base.c b/drivers/scsi/mpt2sas/mpt2sas_base.c
index 83035bd1c489..10f16a306e54 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_base.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_base.c
@@ -66,6 +66,8 @@ static MPT_CALLBACK	mpt_callbacks[MPT_MAX_CALLBACKS];
 
 #define FAULT_POLLING_INTERVAL 1000 /* in milliseconds */
 
+#define MAX_HBA_QUEUE_DEPTH	30000
+#define MAX_CHAIN_DEPTH		100000
 static int max_queue_depth = -1;
 module_param(max_queue_depth, int, 0);
 MODULE_PARM_DESC(max_queue_depth, " max controller queue depth ");
@@ -1082,41 +1084,6 @@ _base_config_dma_addressing(struct MPT2SAS_ADAPTER *ioc, struct pci_dev *pdev)
 }
 
 /**
- * _base_save_msix_table - backup msix vector table
- * @ioc: per adapter object
- *
- * This address an errata where diag reset clears out the table
- */
-static void
-_base_save_msix_table(struct MPT2SAS_ADAPTER *ioc)
-{
-	int i;
-
-	if (!ioc->msix_enable || ioc->msix_table_backup == NULL)
-		return;
-
-	for (i = 0; i < ioc->msix_vector_count; i++)
-		ioc->msix_table_backup[i] = ioc->msix_table[i];
-}
-
-/**
- * _base_restore_msix_table - this restores the msix vector table
- * @ioc: per adapter object
- *
- */
-static void
-_base_restore_msix_table(struct MPT2SAS_ADAPTER *ioc)
-{
-	int i;
-
-	if (!ioc->msix_enable || ioc->msix_table_backup == NULL)
-		return;
-
-	for (i = 0; i < ioc->msix_vector_count; i++)
-		ioc->msix_table[i] = ioc->msix_table_backup[i];
-}
-
-/**
  * _base_check_enable_msix - checks MSIX capabable.
  * @ioc: per adapter object
  *
@@ -1128,7 +1095,7 @@ _base_check_enable_msix(struct MPT2SAS_ADAPTER *ioc)
 {
 	int base;
 	u16 message_control;
-	u32 msix_table_offset;
+
 
 	base = pci_find_capability(ioc->pdev, PCI_CAP_ID_MSIX);
 	if (!base) {
@@ -1141,14 +1108,8 @@ _base_check_enable_msix(struct MPT2SAS_ADAPTER *ioc)
 	pci_read_config_word(ioc->pdev, base + 2, &message_control);
 	ioc->msix_vector_count = (message_control & 0x3FF) + 1;
 
-	/* get msix table  */
-	pci_read_config_dword(ioc->pdev, base + 4, &msix_table_offset);
-	msix_table_offset &= 0xFFFFFFF8;
-	ioc->msix_table = (u32 *)((void *)ioc->chip + msix_table_offset);
-
 	dinitprintk(ioc, printk(MPT2SAS_INFO_FMT "msix is supported, "
-	    "vector_count(%d), table_offset(0x%08x), table(%p)\n", ioc->name,
-	    ioc->msix_vector_count, msix_table_offset, ioc->msix_table));
+	    "vector_count(%d)\n", ioc->name, ioc->msix_vector_count));
 	return 0;
 }
 
@@ -1162,8 +1123,6 @@ _base_disable_msix(struct MPT2SAS_ADAPTER *ioc)
 {
 	if (ioc->msix_enable) {
 		pci_disable_msix(ioc->pdev);
-		kfree(ioc->msix_table_backup);
-		ioc->msix_table_backup = NULL;
 		ioc->msix_enable = 0;
 	}
 }
@@ -1189,14 +1148,6 @@ _base_enable_msix(struct MPT2SAS_ADAPTER *ioc)
 	if (_base_check_enable_msix(ioc) != 0)
 		goto try_ioapic;
 
-	ioc->msix_table_backup = kcalloc(ioc->msix_vector_count,
-	    sizeof(u32), GFP_KERNEL);
-	if (!ioc->msix_table_backup) {
-		dfailprintk(ioc, printk(MPT2SAS_INFO_FMT "allocation for "
-		    "msix_table_backup failed!!!\n", ioc->name));
-		goto try_ioapic;
-	}
-
 	memset(&entries, 0, sizeof(struct msix_entry));
 	r = pci_enable_msix(ioc->pdev, &entries, 1);
 	if (r) {
@@ -2149,8 +2100,6 @@ _base_release_memory_pools(struct MPT2SAS_ADAPTER *ioc)
 		}
 		if (ioc->chain_dma_pool)
 			pci_pool_destroy(ioc->chain_dma_pool);
-	}
-	if (ioc->chain_lookup) {
 		free_pages((ulong)ioc->chain_lookup, ioc->chain_pages);
 		ioc->chain_lookup = NULL;
 	}
@@ -2168,9 +2117,7 @@ static int
 _base_allocate_memory_pools(struct MPT2SAS_ADAPTER *ioc,  int sleep_flag)
 {
 	struct mpt2sas_facts *facts;
-	u32 queue_size, queue_diff;
 	u16 max_sge_elements;
-	u16 num_of_reply_frames;
 	u16 chains_needed_per_io;
 	u32 sz, total_sz;
 	u32 retry_sz;
@@ -2197,7 +2144,8 @@ _base_allocate_memory_pools(struct MPT2SAS_ADAPTER *ioc,  int sleep_flag)
 		max_request_credit = (max_queue_depth < facts->RequestCredit)
 		    ? max_queue_depth : facts->RequestCredit;
 	else
-		max_request_credit = facts->RequestCredit;
+		max_request_credit = min_t(u16, facts->RequestCredit,
+		    MAX_HBA_QUEUE_DEPTH);
 
 	ioc->hba_queue_depth = max_request_credit;
 	ioc->hi_priority_depth = facts->HighPriorityCredit;
@@ -2238,50 +2186,25 @@ _base_allocate_memory_pools(struct MPT2SAS_ADAPTER *ioc,  int sleep_flag)
 	}
 	ioc->chains_needed_per_io = chains_needed_per_io;
 
-	/* reply free queue sizing - taking into account for events */
-	num_of_reply_frames = ioc->hba_queue_depth + 32;
-
-	/* number of replies frames can't be a multiple of 16 */
-	/* decrease number of reply frames by 1 */
-	if (!(num_of_reply_frames % 16))
-		num_of_reply_frames--;
-
-	/* calculate number of reply free queue entries
-	 *  (must be multiple of 16)
-	 */
-
-	/* (we know reply_free_queue_depth is not a multiple of 16) */
-	queue_size = num_of_reply_frames;
-	queue_size += 16 - (queue_size % 16);
-	ioc->reply_free_queue_depth = queue_size;
+	/* reply free queue sizing - taking into account for 64 FW events */
+	ioc->reply_free_queue_depth = ioc->hba_queue_depth + 64;
 
-	/* reply descriptor post queue sizing */
-	/* this size should be the number of request frames + number of reply
-	 * frames
-	 */
-
-	queue_size = ioc->hba_queue_depth + num_of_reply_frames + 1;
-	/* round up to 16 byte boundary */
-	if (queue_size % 16)
-		queue_size += 16 - (queue_size % 16);
-
-	/* check against IOC maximum reply post queue depth */
-	if (queue_size > facts->MaxReplyDescriptorPostQueueDepth) {
-		queue_diff = queue_size -
-		    facts->MaxReplyDescriptorPostQueueDepth;
-
-		/* round queue_diff up to multiple of 16 */
-		if (queue_diff % 16)
-			queue_diff += 16 - (queue_diff % 16);
-
-		/* adjust hba_queue_depth, reply_free_queue_depth,
-		 * and queue_size
-		 */
-		ioc->hba_queue_depth -= (queue_diff / 2);
-		ioc->reply_free_queue_depth -= (queue_diff / 2);
-		queue_size = facts->MaxReplyDescriptorPostQueueDepth;
+	/* align the reply post queue on the next 16 count boundary */
+	if (!ioc->reply_free_queue_depth % 16)
+		ioc->reply_post_queue_depth = ioc->reply_free_queue_depth + 16;
+	else
+		ioc->reply_post_queue_depth = ioc->reply_free_queue_depth +
+				32 - (ioc->reply_free_queue_depth % 16);
+	if (ioc->reply_post_queue_depth >
+	    facts->MaxReplyDescriptorPostQueueDepth) {
+		ioc->reply_post_queue_depth = min_t(u16,
+		    (facts->MaxReplyDescriptorPostQueueDepth -
+		    (facts->MaxReplyDescriptorPostQueueDepth % 16)),
+		    (ioc->hba_queue_depth - (ioc->hba_queue_depth % 16)));
+		ioc->reply_free_queue_depth = ioc->reply_post_queue_depth - 16;
+		ioc->hba_queue_depth = ioc->reply_free_queue_depth - 64;
 	}
-	ioc->reply_post_queue_depth = queue_size;
+
 
 	dinitprintk(ioc, printk(MPT2SAS_INFO_FMT "scatter gather: "
 	    "sge_in_main_msg(%d), sge_per_chain(%d), sge_per_io(%d), "
@@ -2367,15 +2290,12 @@ _base_allocate_memory_pools(struct MPT2SAS_ADAPTER *ioc,  int sleep_flag)
 	    "depth(%d)\n", ioc->name, ioc->request,
 	    ioc->scsiio_depth));
 
-	/* loop till the allocation succeeds */
-	do {
-		sz = ioc->chain_depth * sizeof(struct chain_tracker);
-		ioc->chain_pages = get_order(sz);
-		ioc->chain_lookup = (struct chain_tracker *)__get_free_pages(
-		    GFP_KERNEL, ioc->chain_pages);
-		if (ioc->chain_lookup == NULL)
-			ioc->chain_depth -= 100;
-	} while (ioc->chain_lookup == NULL);
+	ioc->chain_depth = min_t(u32, ioc->chain_depth, MAX_CHAIN_DEPTH);
+	sz = ioc->chain_depth * sizeof(struct chain_tracker);
+	ioc->chain_pages = get_order(sz);
+
+	ioc->chain_lookup = (struct chain_tracker *)__get_free_pages(
+	    GFP_KERNEL, ioc->chain_pages);
 	ioc->chain_dma_pool = pci_pool_create("chain pool", ioc->pdev,
 	    ioc->request_sz, 16, 0);
 	if (!ioc->chain_dma_pool) {
@@ -3513,9 +3433,6 @@ _base_diag_reset(struct MPT2SAS_ADAPTER *ioc, int sleep_flag)
 	u32 hcb_size;
 
 	printk(MPT2SAS_INFO_FMT "sending diag reset !!\n", ioc->name);
-
-	_base_save_msix_table(ioc);
-
 	drsprintk(ioc, printk(MPT2SAS_INFO_FMT "clear interrupts\n",
 	    ioc->name));
 
@@ -3611,7 +3528,6 @@ _base_diag_reset(struct MPT2SAS_ADAPTER *ioc, int sleep_flag)
 		goto out;
 	}
 
-	_base_restore_msix_table(ioc);
 	printk(MPT2SAS_INFO_FMT "diag reset: SUCCESS\n", ioc->name);
 	return 0;
 
diff --git a/drivers/scsi/mpt2sas/mpt2sas_base.h b/drivers/scsi/mpt2sas/mpt2sas_base.h
index 8d5be2120c63..7df640fb54dd 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_base.h
+++ b/drivers/scsi/mpt2sas/mpt2sas_base.h
@@ -636,8 +636,6 @@ enum mutex_type {
  * @wait_for_port_enable_to_complete:
  * @msix_enable: flag indicating msix is enabled
  * @msix_vector_count: number msix vectors
- * @msix_table: virt address to the msix table
- * @msix_table_backup: backup msix table
  * @scsi_io_cb_idx: shost generated commands
  * @tm_cb_idx: task management commands
  * @scsih_cb_idx: scsih internal commands
@@ -779,8 +777,6 @@ struct MPT2SAS_ADAPTER {
 
 	u8		msix_enable;
 	u16		msix_vector_count;
-	u32		*msix_table;
-	u32		*msix_table_backup;
 	u32		ioc_reset_count;
 
 	/* internal commands, callback index */
diff --git a/drivers/scsi/mpt2sas/mpt2sas_scsih.c b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
index 6abd2fcc43e2..011b8648ab13 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_scsih.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
@@ -978,8 +978,8 @@ _scsih_get_chain_buffer_tracker(struct MPT2SAS_ADAPTER *ioc, u16 smid)
 	spin_lock_irqsave(&ioc->scsi_lookup_lock, flags);
 	if (list_empty(&ioc->free_chain_list)) {
 		spin_unlock_irqrestore(&ioc->scsi_lookup_lock, flags);
-		printk(MPT2SAS_WARN_FMT "chain buffers not available\n",
-		    ioc->name);
+		dfailprintk(ioc, printk(MPT2SAS_WARN_FMT "chain buffers not "
+			"available\n", ioc->name));
 		return NULL;
 	}
 	chain_req = list_entry(ioc->free_chain_list.next,
@@ -4210,7 +4210,7 @@ _scsih_smart_predicted_fault(struct MPT2SAS_ADAPTER *ioc, u16 handle)
 	/* insert into event log */
 	sz = offsetof(Mpi2EventNotificationReply_t, EventData) +
 	     sizeof(Mpi2EventDataSasDeviceStatusChange_t);
-	event_reply = kzalloc(sz, GFP_KERNEL);
+	event_reply = kzalloc(sz, GFP_ATOMIC);
 	if (!event_reply) {
 		printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
 		    ioc->name, __FILE__, __LINE__, __func__);
@@ -6564,6 +6564,7 @@ _scsih_mark_responding_raid_device(struct MPT2SAS_ADAPTER *ioc, u64 wwid,
 			} else
 				sas_target_priv_data = NULL;
 			raid_device->responding = 1;
+			spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
 			starget_printk(KERN_INFO, raid_device->starget,
 			    "handle(0x%04x), wwid(0x%016llx)\n", handle,
 			    (unsigned long long)raid_device->wwid);
@@ -6574,16 +6575,16 @@ _scsih_mark_responding_raid_device(struct MPT2SAS_ADAPTER *ioc, u64 wwid,
 			 */
 			_scsih_init_warpdrive_properties(ioc, raid_device);
 			if (raid_device->handle == handle)
-				goto out;
+				return;
 			printk(KERN_INFO "\thandle changed from(0x%04x)!!!\n",
 			    raid_device->handle);
 			raid_device->handle = handle;
 			if (sas_target_priv_data)
 				sas_target_priv_data->handle = handle;
-			goto out;
+			return;
 		}
 	}
- out:
+
 	spin_unlock_irqrestore(&ioc->raid_device_lock, flags);
 }
 
@@ -7354,6 +7355,7 @@ _scsih_remove(struct pci_dev *pdev)
 	}
 
 	sas_remove_host(shost);
+	mpt2sas_base_detach(ioc);
 	list_del(&ioc->list);
 	scsi_remove_host(shost);
 	scsi_host_put(shost);
@@ -7461,22 +7463,27 @@ _scsih_probe_sas(struct MPT2SAS_ADAPTER *ioc)
 	/* SAS Device List */
 	list_for_each_entry_safe(sas_device, next, &ioc->sas_device_init_list,
 	    list) {
-		spin_lock_irqsave(&ioc->sas_device_lock, flags);
-		list_move_tail(&sas_device->list, &ioc->sas_device_list);
-		spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 
 		if (ioc->hide_drives)
 			continue;
 
 		if (!mpt2sas_transport_port_add(ioc, sas_device->handle,
 		    sas_device->sas_address_parent)) {
-			_scsih_sas_device_remove(ioc, sas_device);
+			list_del(&sas_device->list);
+			kfree(sas_device);
+			continue;
 		} else if (!sas_device->starget) {
 			mpt2sas_transport_port_remove(ioc,
 			    sas_device->sas_address,
 			    sas_device->sas_address_parent);
-			_scsih_sas_device_remove(ioc, sas_device);
+			list_del(&sas_device->list);
+			kfree(sas_device);
+			continue;
+
 		}
+		spin_lock_irqsave(&ioc->sas_device_lock, flags);
+		list_move_tail(&sas_device->list, &ioc->sas_device_list);
+		spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 	}
 }
 
diff --git a/drivers/scsi/scsi_lib.c b/drivers/scsi/scsi_lib.c
index fc3f168decb4..6d219e48d62f 100644
--- a/drivers/scsi/scsi_lib.c
+++ b/drivers/scsi/scsi_lib.c
@@ -1408,6 +1408,8 @@ static void scsi_kill_request(struct request *req, struct request_queue *q)
 
 	blk_start_request(req);
 
+	scmd_printk(KERN_INFO, cmd, "killing request\n");
+
 	sdev = cmd->device;
 	starget = scsi_target(sdev);
 	shost = sdev->host;
@@ -1489,7 +1491,6 @@ static void scsi_request_fn(struct request_queue *q)
 	struct request *req;
 
 	if (!sdev) {
-		printk("scsi: killing requests for dead queue\n");
 		while ((req = blk_peek_request(q)) != NULL)
 			scsi_kill_request(req, q);
 		return;
@@ -1698,6 +1699,15 @@ struct request_queue *scsi_alloc_queue(struct scsi_device *sdev)
 
 void scsi_free_queue(struct request_queue *q)
 {
+	unsigned long flags;
+
+	WARN_ON(q->queuedata);
+
+	/* cause scsi_request_fn() to kill all non-finished requests */
+	spin_lock_irqsave(q->queue_lock, flags);
+	q->request_fn(q);
+	spin_unlock_irqrestore(q->queue_lock, flags);
+
 	blk_cleanup_queue(q);
 }
 
diff --git a/drivers/scsi/scsi_scan.c b/drivers/scsi/scsi_scan.c
index 44e8ca398efa..b3c6d957fbd8 100644
--- a/drivers/scsi/scsi_scan.c
+++ b/drivers/scsi/scsi_scan.c
@@ -319,10 +319,7 @@ static struct scsi_device *scsi_alloc_sdev(struct scsi_target *starget,
 	return sdev;
 
 out_device_destroy:
-	scsi_device_set_state(sdev, SDEV_DEL);
-	transport_destroy_device(&sdev->sdev_gendev);
-	put_device(&sdev->sdev_dev);
-	put_device(&sdev->sdev_gendev);
+	__scsi_remove_device(sdev);
 out:
 	if (display_failure_msg)
 		printk(ALLOC_FAILURE_MSG, __func__);
diff --git a/drivers/scsi/st.c b/drivers/scsi/st.c
index 1871b8ae83ae..9b28f39bac26 100644
--- a/drivers/scsi/st.c
+++ b/drivers/scsi/st.c
@@ -462,14 +462,16 @@ static void st_scsi_execute_end(struct request *req, int uptodate)
 {
 	struct st_request *SRpnt = req->end_io_data;
 	struct scsi_tape *STp = SRpnt->stp;
+	struct bio *tmp;
 
 	STp->buffer->cmdstat.midlevel_result = SRpnt->result = req->errors;
 	STp->buffer->cmdstat.residual = req->resid_len;
 
+	tmp = SRpnt->bio;
 	if (SRpnt->waiting)
 		complete(SRpnt->waiting);
 
-	blk_rq_unmap_user(SRpnt->bio);
+	blk_rq_unmap_user(tmp);
 	__blk_put_request(req->q, req);
 }
 
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 52e2900d9d8e..8f7ee0e1ee46 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -338,6 +338,16 @@ config SPI_TEGRA
 	help
 	  SPI driver for NVidia Tegra SoCs
 
+config SPI_SLAVE_TEGRA
+	tristate "Nvidia Tegra SPI slave controller"
+	depends on ARCH_TEGRA
+	select TEGRA_SYSTEM_DMA
+	default n
+	help
+	  SPI Slave driver for NVidia Tegra SoCs.
+	  Say yes if slave spi functionality is required from the tegra spi controller.
+	  The interface is same as the master spi.
+
 config SPI_TI_SSP
 	tristate "TI Sequencer Serial Port - SPI Support"
 	depends on MFD_TI_SSP
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index 61c3261c388c..831e6eabee64 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -1,6 +1,7 @@
 #
 # Makefile for kernel SPI drivers.
 #
+GCOV_PROFILE := y
 
 ccflags-$(CONFIG_SPI_DEBUG) := -DDEBUG
 
@@ -52,7 +53,10 @@ obj-$(CONFIG_SPI_SH)			+= spi-sh.o
 obj-$(CONFIG_SPI_SH_MSIOF)		+= spi-sh-msiof.o
 obj-$(CONFIG_SPI_SH_SCI)		+= spi-sh-sci.o
 obj-$(CONFIG_SPI_STMP3XXX)		+= spi-stmp.o
+CFLAGS_spi-tegra.o			= -Werror
 obj-$(CONFIG_SPI_TEGRA)			+= spi-tegra.o
+CFLAGS_spi_slave_tegra.o		= -Werror
+obj-$(CONFIG_SPI_SLAVE_TEGRA)		+= spi_slave_tegra.o
 obj-$(CONFIG_SPI_TI_SSP)		+= spi-ti-ssp.o
 obj-$(CONFIG_SPI_TLE62X0)		+= spi-tle62x0.o
 obj-$(CONFIG_SPI_TOPCLIFF_PCH)		+= spi-topcliff-pch.o
diff --git a/drivers/spi/spi-dw-mid.c b/drivers/spi/spi-dw-mid.c
index 130e55537db6..e9f1e40cc75e 100644
--- a/drivers/spi/spi-dw-mid.c
+++ b/drivers/spi/spi-dw-mid.c
@@ -144,7 +144,7 @@ static int mid_spi_dma_transfer(struct dw_spi *dws, int cs_change)
 	dws->tx_sgl.dma_address = dws->tx_dma;
 	dws->tx_sgl.length = dws->len;
 
-	txdesc = txchan->device->device_prep_slave_sg(txchan,
+	txdesc = dmaengine_prep_slave_sg(txchan,
 				&dws->tx_sgl,
 				1,
 				DMA_TO_DEVICE,
@@ -166,7 +166,7 @@ static int mid_spi_dma_transfer(struct dw_spi *dws, int cs_change)
 	dws->rx_sgl.dma_address = dws->rx_dma;
 	dws->rx_sgl.length = dws->len;
 
-	rxdesc = rxchan->device->device_prep_slave_sg(rxchan,
+	rxdesc = dmaengine_prep_slave_sg(rxchan,
 				&dws->rx_sgl,
 				1,
 				DMA_FROM_DEVICE,
diff --git a/drivers/spi/spi-ep93xx.c b/drivers/spi/spi-ep93xx.c
index 1cf645479bfe..9a9e27b52a24 100644
--- a/drivers/spi/spi-ep93xx.c
+++ b/drivers/spi/spi-ep93xx.c
@@ -629,8 +629,8 @@ ep93xx_spi_dma_prepare(struct ep93xx_spi *espi, enum dma_data_direction dir)
 	if (!nents)
 		return ERR_PTR(-ENOMEM);
 
-	txd = chan->device->device_prep_slave_sg(chan, sgt->sgl, nents,
-						 dir, DMA_CTRL_ACK);
+	txd = dmaengine_prep_slave_sg(chan, sgt->sgl, nents,
+					slave_dirn, DMA_CTRL_ACK);
 	if (!txd) {
 		dma_unmap_sg(chan->device->dev, sgt->sgl, sgt->nents, dir);
 		return ERR_PTR(-ENOMEM);
diff --git a/drivers/spi/spi-omap2-mcspi.c b/drivers/spi/spi-omap2-mcspi.c
index fde3a2d4f120..322be7aea8b4 100644
--- a/drivers/spi/spi-omap2-mcspi.c
+++ b/drivers/spi/spi-omap2-mcspi.c
@@ -1116,15 +1116,16 @@ static int __init omap2_mcspi_probe(struct platform_device *pdev)
 		status = -ENODEV;
 		goto err1;
 	}
+
+	r->start += pdata->regs_offset;
+	r->end += pdata->regs_offset;
+	mcspi->phys = r->start;
 	if (!request_mem_region(r->start, resource_size(r),
 				dev_name(&pdev->dev))) {
 		status = -EBUSY;
 		goto err1;
 	}
 
-	r->start += pdata->regs_offset;
-	r->end += pdata->regs_offset;
-	mcspi->phys = r->start;
 	mcspi->base = ioremap(r->start, resource_size(r));
 	if (!mcspi->base) {
 		dev_dbg(&pdev->dev, "can't ioremap MCSPI\n");
diff --git a/drivers/spi/spi-pl022.c b/drivers/spi/spi-pl022.c
index 730b4a37b823..45d7381ae272 100644
--- a/drivers/spi/spi-pl022.c
+++ b/drivers/spi/spi-pl022.c
@@ -1048,7 +1048,7 @@ static int configure_dma(struct pl022 *pl022)
 		goto err_tx_sgmap;
 
 	/* Send both scatterlists */
-	rxdesc = rxchan->device->device_prep_slave_sg(rxchan,
+	rxdesc = dmaengine_prep_slave_sg(rxchan,
 				      pl022->sgt_rx.sgl,
 				      rx_sglen,
 				      DMA_FROM_DEVICE,
@@ -1056,7 +1056,7 @@ static int configure_dma(struct pl022 *pl022)
 	if (!rxdesc)
 		goto err_rxdesc;
 
-	txdesc = txchan->device->device_prep_slave_sg(txchan,
+	txdesc = dmaengine_prep_slave_sg(txchan,
 				      pl022->sgt_tx.sgl,
 				      tx_sglen,
 				      DMA_TO_DEVICE,
diff --git a/drivers/spi/spi-tegra.c b/drivers/spi/spi-tegra.c
index a5a6302dc8e0..5fca5e62706c 100644
--- a/drivers/spi/spi-tegra.c
+++ b/drivers/spi/spi-tegra.c
@@ -6,6 +6,8 @@
  * Author:
  *     Erik Gilling <konkers@android.com>
  *
+ * Copyright (C) 2010-2011 NVIDIA Corporation
+ *
  * This software is licensed under the terms of the GNU General Public
  * License version 2, as published by the Free Software Foundation, and
  * may be copied, distributed, and modified under those terms.
@@ -17,6 +19,9 @@
  *
  */
 
+/*#define DEBUG           1*/
+/*#define VERBOSE_DEBUG   1*/
+
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/err.h>
@@ -27,10 +32,15 @@
 #include <linux/clk.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
+#include <linux/completion.h>
+#include <linux/kthread.h>
+#include <linux/pm_runtime.h>
 
 #include <linux/spi/spi.h>
+#include <linux/spi-tegra.h>
 
 #include <mach/dma.h>
+#include <mach/clk.h>
 
 #define SLINK_COMMAND		0x000
 #define   SLINK_BIT_LENGTH(x)		(((x) & 0x1f) << 0)
@@ -125,259 +135,720 @@
 
 #define SLINK_STATUS2		0x01c
 #define   SLINK_TX_FIFO_EMPTY_COUNT(val)	(((val) & 0x3f) >> 0)
-#define   SLINK_RX_FIFO_FULL_COUNT(val)		(((val) & 0x3f) >> 16)
+#define   SLINK_RX_FIFO_FULL_COUNT(val)		(((val) & 0x3f0000) >> 16)
+#define   SLINK_SS_HOLD_TIME(val)		(((val) & 0xF) << 6)
 
 #define SLINK_TX_FIFO		0x100
 #define SLINK_RX_FIFO		0x180
 
+#define DATA_DIR_TX		(1 << 0)
+#define DATA_DIR_RX		(1 << 1)
+
+#define SLINK_DMA_TIMEOUT (msecs_to_jiffies(1000))
+
+
 static const unsigned long spi_tegra_req_sels[] = {
 	TEGRA_DMA_REQ_SEL_SL2B1,
 	TEGRA_DMA_REQ_SEL_SL2B2,
 	TEGRA_DMA_REQ_SEL_SL2B3,
 	TEGRA_DMA_REQ_SEL_SL2B4,
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+	TEGRA_DMA_REQ_SEL_SL2B5,
+	TEGRA_DMA_REQ_SEL_SL2B6,
+#endif
+
 };
 
-#define BB_LEN			32
+#define DEFAULT_SPI_DMA_BUF_LEN		(16*1024)
+#define TX_FIFO_EMPTY_COUNT_MAX		SLINK_TX_FIFO_EMPTY_COUNT(0x20)
+#define RX_FIFO_FULL_COUNT_ZERO		SLINK_RX_FIFO_FULL_COUNT(0)
+
+#define SLINK_STATUS2_RESET \
+	(TX_FIFO_EMPTY_COUNT_MAX | \
+	RX_FIFO_FULL_COUNT_ZERO << 16)
+
+#define MAX_CHIP_SELECT		4
+#define SLINK_FIFO_DEPTH	32
 
 struct spi_tegra_data {
 	struct spi_master	*master;
 	struct platform_device	*pdev;
 	spinlock_t		lock;
+	spinlock_t		reg_lock;
+	char			port_name[32];
 
 	struct clk		*clk;
+	struct clk		*sclk;
 	void __iomem		*base;
-	unsigned long		phys;
+	phys_addr_t		phys;
+	unsigned		irq;
 
 	u32			cur_speed;
 
 	struct list_head	queue;
 	struct spi_transfer	*cur;
+	struct spi_device	*cur_spi;
 	unsigned		cur_pos;
 	unsigned		cur_len;
-	unsigned		cur_bytes_per_word;
+	unsigned		words_per_32bit;
+	unsigned		bytes_per_word;
+	unsigned		curr_dma_words;
+
+	unsigned		cur_direction;
+
+	bool			is_dma_allowed;
 
-	/* The tegra spi controller has a bug which causes the first word
-	 * in PIO transactions to be garbage.  Since packed DMA transactions
-	 * require transfers to be 4 byte aligned we need a bounce buffer
-	 * for the generic case.
-	 */
 	struct tegra_dma_req	rx_dma_req;
 	struct tegra_dma_channel *rx_dma;
-	u32			*rx_bb;
-	dma_addr_t		rx_bb_phys;
+	u32			*rx_buf;
+	dma_addr_t		rx_buf_phys;
+	unsigned		cur_rx_pos;
+
+	struct tegra_dma_req	tx_dma_req;
+	struct tegra_dma_channel *tx_dma;
+	u32			*tx_buf;
+	dma_addr_t		tx_buf_phys;
+	unsigned		cur_tx_pos;
+
+	unsigned		dma_buf_size;
+	unsigned		max_buf_size;
+	bool			is_curr_dma_xfer;
+
+	bool			is_clkon_always;
+	int			clk_state;
+	bool			is_suspended;
+
+	bool			is_hw_based_cs;
+
+	struct completion	rx_dma_complete;
+	struct completion	tx_dma_complete;
+	bool			is_transfer_in_progress;
+
+	u32			rx_complete;
+	u32			tx_complete;
+	u32			tx_status;
+	u32			rx_status;
+	u32			status_reg;
+	bool			is_packed;
+	unsigned long		packed_size;
+
+	u32			command_reg;
+	u32			command2_reg;
+	u32			dma_control_reg;
+	u32			def_command_reg;
+	u32			def_command2_reg;
+
+	struct spi_clk_parent	*parent_clk_list;
+	int			parent_clk_count;
+	unsigned long		max_rate;
+	unsigned long		max_parent_rate;
+	int			min_div;
+	struct workqueue_struct *spi_workqueue;
+	struct work_struct spi_transfer_work;
 };
 
-
 static inline unsigned long spi_tegra_readl(struct spi_tegra_data *tspi,
-					    unsigned long reg)
+		    unsigned long reg)
 {
-	return readl(tspi->base + reg);
+	unsigned long flags;
+	unsigned long val;
+
+	spin_lock_irqsave(&tspi->reg_lock, flags);
+	if (tspi->clk_state < 1)
+		BUG();
+	val = readl(tspi->base + reg);
+	spin_unlock_irqrestore(&tspi->reg_lock, flags);
+	return val;
 }
 
 static inline void spi_tegra_writel(struct spi_tegra_data *tspi,
-				    unsigned long val,
-				    unsigned long reg)
+		    unsigned long val, unsigned long reg)
 {
+	unsigned long flags;
+
+	spin_lock_irqsave(&tspi->reg_lock, flags);
+	if (tspi->clk_state < 1)
+		BUG();
 	writel(val, tspi->base + reg);
+
+	/* Synchronize write by reading back the register */
+	readl(tspi->base + SLINK_MAS_DATA);
+	spin_unlock_irqrestore(&tspi->reg_lock, flags);
 }
 
-static void spi_tegra_go(struct spi_tegra_data *tspi)
+static int tegra_spi_clk_disable(struct spi_tegra_data *tspi)
 {
-	unsigned long val;
-
-	wmb();
-
-	val = spi_tegra_readl(tspi, SLINK_DMA_CTL);
-	val &= ~SLINK_DMA_BLOCK_SIZE(~0) & ~SLINK_DMA_EN;
-	val |= SLINK_DMA_BLOCK_SIZE(tspi->rx_dma_req.size / 4 - 1);
-	spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+	unsigned long flags;
 
-	tegra_dma_enqueue_req(tspi->rx_dma, &tspi->rx_dma_req);
+	/* Flush all write which are in PPSB queue by reading back */
+	spi_tegra_readl(tspi, SLINK_MAS_DATA);
 
-	val |= SLINK_DMA_EN;
-	spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+	spin_lock_irqsave(&tspi->reg_lock, flags);
+	tspi->clk_state--;
+	spin_unlock_irqrestore(&tspi->reg_lock, flags);
+	clk_disable(tspi->clk);
+	clk_disable(tspi->sclk);
+	return 0;
 }
 
-static unsigned spi_tegra_fill_tx_fifo(struct spi_tegra_data *tspi,
-				  struct spi_transfer *t)
+static int tegra_spi_clk_enable(struct spi_tegra_data *tspi)
 {
-	unsigned len = min(t->len - tspi->cur_pos, BB_LEN *
-			   tspi->cur_bytes_per_word);
-	u8 *tx_buf = (u8 *)t->tx_buf + tspi->cur_pos;
-	int i, j;
-	unsigned long val;
+	unsigned long flags;
 
-	val = spi_tegra_readl(tspi, SLINK_COMMAND);
-	val &= ~SLINK_WORD_SIZE(~0);
-	val |= SLINK_WORD_SIZE(len / tspi->cur_bytes_per_word - 1);
-	spi_tegra_writel(tspi, val, SLINK_COMMAND);
+	clk_enable(tspi->sclk);
+	clk_enable(tspi->clk);
+	spin_lock_irqsave(&tspi->reg_lock, flags);
+	tspi->clk_state++;
+	spin_unlock_irqrestore(&tspi->reg_lock, flags);
+	return 0;
+}
 
-	for (i = 0; i < len; i += tspi->cur_bytes_per_word) {
-		val = 0;
-		for (j = 0; j < tspi->cur_bytes_per_word; j++)
-			val |= tx_buf[i + j] << j * 8;
+static void cancel_dma(struct tegra_dma_channel *dma_chan,
+	struct tegra_dma_req *req)
+{
+	tegra_dma_cancel(dma_chan);
+	if (req->status == -TEGRA_DMA_REQ_ERROR_ABORTED)
+		req->complete(req);
+}
 
-		spi_tegra_writel(tspi, val, SLINK_TX_FIFO);
-	}
+static void spi_tegra_clear_status(struct spi_tegra_data *tspi)
+{
+	unsigned long val;
+	unsigned long val_write = 0;
 
-	tspi->rx_dma_req.size = len / tspi->cur_bytes_per_word * 4;
+	val = spi_tegra_readl(tspi, SLINK_STATUS);
 
-	return len;
+	val_write = SLINK_RDY;
+	if (val & SLINK_TX_OVF)
+		val_write |= SLINK_TX_OVF;
+	if (val & SLINK_RX_OVF)
+		val_write |= SLINK_RX_OVF;
+	if (val & SLINK_RX_UNF)
+		val_write |= SLINK_RX_UNF;
+	if (val & SLINK_TX_UNF)
+		val_write |= SLINK_TX_UNF;
+
+	spi_tegra_writel(tspi, val_write, SLINK_STATUS);
 }
 
-static unsigned spi_tegra_drain_rx_fifo(struct spi_tegra_data *tspi,
+static unsigned long spi_tegra_get_packed_size(struct spi_tegra_data *tspi,
 				  struct spi_transfer *t)
 {
-	unsigned len = tspi->cur_len;
-	u8 *rx_buf = (u8 *)t->rx_buf + tspi->cur_pos;
-	int i, j;
 	unsigned long val;
 
-	for (i = 0; i < len; i += tspi->cur_bytes_per_word) {
-		val = tspi->rx_bb[i / tspi->cur_bytes_per_word];
-		for (j = 0; j < tspi->cur_bytes_per_word; j++)
-			rx_buf[i + j] = (val >> (j * 8)) & 0xff;
+	switch (tspi->bytes_per_word) {
+	case 0:
+		val = SLINK_PACK_SIZE_4;
+		break;
+	case 1:
+		val = SLINK_PACK_SIZE_8;
+		break;
+	case 2:
+		val = SLINK_PACK_SIZE_16;
+		break;
+	case 4:
+		val = SLINK_PACK_SIZE_32;
+		break;
+	default:
+		val = 0;
+	}
+	return val;
+}
+
+static unsigned spi_tegra_calculate_curr_xfer_param(
+	struct spi_device *spi, struct spi_tegra_data *tspi,
+	struct spi_transfer *t)
+{
+	unsigned remain_len = t->len - tspi->cur_pos;
+	unsigned max_word;
+	unsigned bits_per_word ;
+	unsigned max_len;
+	unsigned total_fifo_words;
+
+	bits_per_word = t->bits_per_word ? t->bits_per_word :
+						spi->bits_per_word;
+	tspi->bytes_per_word = (bits_per_word - 1) / 8 + 1;
+
+	if (bits_per_word == 8 || bits_per_word == 16) {
+		tspi->is_packed = 1;
+		tspi->words_per_32bit = 32/bits_per_word;
+	} else {
+		tspi->is_packed = 0;
+		tspi->words_per_32bit = 1;
 	}
+	tspi->packed_size = spi_tegra_get_packed_size(tspi, t);
 
-	return len;
+	if (tspi->is_packed) {
+		max_len = min(remain_len, tspi->max_buf_size);
+		tspi->curr_dma_words = max_len/tspi->bytes_per_word;
+		total_fifo_words = max_len/4;
+	} else {
+		max_word = (remain_len - 1) / tspi->bytes_per_word + 1;
+		max_word = min(max_word, tspi->max_buf_size/4);
+		tspi->curr_dma_words = max_word;
+		total_fifo_words = max_word;
+	}
+	return total_fifo_words;
 }
 
-static void spi_tegra_start_transfer(struct spi_device *spi,
-				    struct spi_transfer *t)
+static unsigned spi_tegra_fill_tx_fifo_from_client_txbuf(
+	struct spi_tegra_data *tspi, struct spi_transfer *t)
 {
-	struct spi_tegra_data *tspi = spi_master_get_devdata(spi->master);
-	u32 speed;
-	u8 bits_per_word;
-	unsigned long val;
+	unsigned nbytes;
+	unsigned tx_empty_count;
+	unsigned long fifo_status;
+	u8 *tx_buf = (u8 *)t->tx_buf + tspi->cur_tx_pos;
+	unsigned max_n_32bit;
+	unsigned i, count;
+	unsigned long x;
+	unsigned int written_words;
+
+	fifo_status = spi_tegra_readl(tspi, SLINK_STATUS2);
+	tx_empty_count = SLINK_TX_FIFO_EMPTY_COUNT(fifo_status);
+
+	if (tspi->is_packed) {
+		nbytes = tspi->curr_dma_words * tspi->bytes_per_word;
+		max_n_32bit = (min(nbytes,  tx_empty_count*4) - 1)/4 + 1;
+		for (count = 0; count < max_n_32bit; ++count) {
+			x = 0;
+			for (i = 0; (i < 4) && nbytes; i++, nbytes--)
+				x |= (*tx_buf++) << (i*8);
+			spi_tegra_writel(tspi, x, SLINK_TX_FIFO);
+		}
+		written_words =  min(max_n_32bit * tspi->words_per_32bit,
+					tspi->curr_dma_words);
+	} else {
+		max_n_32bit = min(tspi->curr_dma_words,  tx_empty_count);
+		nbytes = max_n_32bit * tspi->bytes_per_word;
+		for (count = 0; count < max_n_32bit; ++count) {
+			x = 0;
+			for (i = 0; nbytes && (i < tspi->bytes_per_word);
+							++i, nbytes--)
+				x |= ((*tx_buf++) << i*8);
+			spi_tegra_writel(tspi, x, SLINK_TX_FIFO);
+		}
+		written_words = max_n_32bit;
+	}
+	tspi->cur_tx_pos += written_words * tspi->bytes_per_word;
+	return written_words;
+}
 
-	speed = t->speed_hz ? t->speed_hz : spi->max_speed_hz;
-	bits_per_word = t->bits_per_word ? t->bits_per_word  :
-		spi->bits_per_word;
+static unsigned int spi_tegra_read_rx_fifo_to_client_rxbuf(
+		struct spi_tegra_data *tspi, struct spi_transfer *t)
+{
+	unsigned rx_full_count;
+	unsigned long fifo_status;
+	u8 *rx_buf = (u8 *)t->rx_buf + tspi->cur_rx_pos;
+	unsigned i, count;
+	unsigned long x;
+	unsigned int read_words = 0;
+	unsigned len;
+
+	fifo_status = spi_tegra_readl(tspi, SLINK_STATUS2);
+	rx_full_count = SLINK_RX_FIFO_FULL_COUNT(fifo_status);
+	dev_dbg(&tspi->pdev->dev, "Rx fifo count %d\n", rx_full_count);
+	if (tspi->is_packed) {
+		len = tspi->curr_dma_words * tspi->bytes_per_word;
+		for (count = 0; count < rx_full_count; ++count) {
+			x = spi_tegra_readl(tspi, SLINK_RX_FIFO);
+			for (i = 0; len && (i < 4); ++i, len--)
+				*rx_buf++ = (x >> i*8) & 0xFF;
+		}
+		tspi->cur_rx_pos += tspi->curr_dma_words * tspi->bytes_per_word;
+		read_words += tspi->curr_dma_words;
+	} else {
+		unsigned int rx_mask, bits_per_word;
+
+		bits_per_word = t->bits_per_word ? t->bits_per_word :
+						tspi->cur_spi->bits_per_word;
+		rx_mask = (1 << bits_per_word) - 1;
+		for (count = 0; count < rx_full_count; ++count) {
+			x = spi_tegra_readl(tspi, SLINK_RX_FIFO);
+			x &= rx_mask;
+			for (i = 0; (i < tspi->bytes_per_word); ++i)
+				*rx_buf++ = (x >> (i*8)) & 0xFF;
+		}
+		tspi->cur_rx_pos += rx_full_count * tspi->bytes_per_word;
+		read_words += rx_full_count;
+	}
+	return read_words;
+}
 
-	tspi->cur_bytes_per_word = (bits_per_word - 1) / 8 + 1;
+static void spi_tegra_copy_client_txbuf_to_spi_txbuf(
+		struct spi_tegra_data *tspi, struct spi_transfer *t)
+{
+	unsigned len;
 
-	if (speed != tspi->cur_speed)
-		clk_set_rate(tspi->clk, speed);
+	/* Make the dma buffer to read by cpu */
+	dma_sync_single_for_cpu(&tspi->pdev->dev, tspi->tx_buf_phys,
+				tspi->dma_buf_size, DMA_TO_DEVICE);
 
-	if (tspi->cur_speed == 0)
-		clk_enable(tspi->clk);
+	if (tspi->is_packed) {
+		len = tspi->curr_dma_words * tspi->bytes_per_word;
+		memcpy(tspi->tx_buf, t->tx_buf + tspi->cur_pos, len);
+	} else {
+		unsigned int i;
+		unsigned int count;
+		u8 *tx_buf = (u8 *)t->tx_buf + tspi->cur_tx_pos;
+		unsigned consume = tspi->curr_dma_words * tspi->bytes_per_word;
+		unsigned int x;
+
+		for (count = 0; count < tspi->curr_dma_words; ++count) {
+			x = 0;
+			for (i = 0; consume && (i < tspi->bytes_per_word);
+							++i, consume--)
+				x |= ((*tx_buf++) << i*8);
+			tspi->tx_buf[count] = x;
+		}
+	}
+	tspi->cur_tx_pos += tspi->curr_dma_words * tspi->bytes_per_word;
 
-	tspi->cur_speed = speed;
+	/* Make the dma buffer to read by dma */
+	dma_sync_single_for_device(&tspi->pdev->dev, tspi->tx_buf_phys,
+				tspi->dma_buf_size, DMA_TO_DEVICE);
+}
 
-	val = spi_tegra_readl(tspi, SLINK_COMMAND2);
-	val &= ~SLINK_SS_EN_CS(~0) | SLINK_RXEN | SLINK_TXEN;
-	if (t->rx_buf)
-		val |= SLINK_RXEN;
-	if (t->tx_buf)
-		val |= SLINK_TXEN;
-	val |= SLINK_SS_EN_CS(spi->chip_select);
-	val |= SLINK_SPIE;
-	spi_tegra_writel(tspi, val, SLINK_COMMAND2);
+static void spi_tegra_copy_spi_rxbuf_to_client_rxbuf(
+		struct spi_tegra_data *tspi, struct spi_transfer *t)
+{
+	unsigned len;
 
-	val = spi_tegra_readl(tspi, SLINK_COMMAND);
-	val &= ~SLINK_BIT_LENGTH(~0);
-	val |= SLINK_BIT_LENGTH(bits_per_word - 1);
+	/* Make the dma buffer to read by cpu */
+	dma_sync_single_for_cpu(&tspi->pdev->dev, tspi->rx_buf_phys,
+		tspi->dma_buf_size, DMA_FROM_DEVICE);
+
+	if (tspi->is_packed) {
+		len = tspi->curr_dma_words * tspi->bytes_per_word;
+		memcpy(t->rx_buf + tspi->cur_rx_pos, tspi->rx_buf, len);
+	} else {
+		unsigned int i;
+		unsigned int count;
+		unsigned char *rx_buf = t->rx_buf + tspi->cur_rx_pos;
+		unsigned int x;
+		unsigned int rx_mask, bits_per_word;
+
+		bits_per_word = t->bits_per_word ? t->bits_per_word :
+						tspi->cur_spi->bits_per_word;
+		rx_mask = (1 << bits_per_word) - 1;
+		for (count = 0; count < tspi->curr_dma_words; ++count) {
+			x = tspi->rx_buf[count];
+			x &= rx_mask;
+			for (i = 0; (i < tspi->bytes_per_word); ++i)
+				*rx_buf++ = (x >> (i*8)) & 0xFF;
+		}
+	}
+	tspi->cur_rx_pos += tspi->curr_dma_words * tspi->bytes_per_word;
 
-	/* FIXME: should probably control CS manually so that we can be sure
-	 * it does not go low between transfer and to support delay_usecs
-	 * correctly.
-	 */
-	val &= ~SLINK_IDLE_SCLK_MASK & ~SLINK_CK_SDA & ~SLINK_CS_SW;
+	/* Make the dma buffer to read by dma */
+	dma_sync_single_for_device(&tspi->pdev->dev, tspi->rx_buf_phys,
+		tspi->dma_buf_size, DMA_FROM_DEVICE);
+}
 
-	if (spi->mode & SPI_CPHA)
-		val |= SLINK_CK_SDA;
+static int spi_tegra_start_dma_based_transfer(
+		struct spi_tegra_data *tspi, struct spi_transfer *t)
+{
+	unsigned long val;
+	unsigned long test_val;
+	unsigned int len;
+	int ret = 0;
+
+	INIT_COMPLETION(tspi->rx_dma_complete);
+	INIT_COMPLETION(tspi->tx_dma_complete);
+
+	/* Make sure that Rx and Tx fifo are empty */
+	test_val = spi_tegra_readl(tspi, SLINK_STATUS);
+	if (((test_val >> 20) & 0xF) != 0xA)
+		dev_err(&tspi->pdev->dev,
+			"The Rx and Tx fifo are not empty status 0x%08lx\n",
+				test_val);
+
+	val = SLINK_DMA_BLOCK_SIZE(tspi->curr_dma_words - 1);
+	val |= tspi->packed_size;
+	if (tspi->is_packed)
+		len = DIV_ROUND_UP(tspi->curr_dma_words * tspi->bytes_per_word,
+					4) * 4;
+	else
+		len = tspi->curr_dma_words * 4;
 
-	if (spi->mode & SPI_CPOL)
-		val |= SLINK_IDLE_SCLK_DRIVE_HIGH;
+	if (len & 0xF)
+		val |= SLINK_TX_TRIG_1 | SLINK_RX_TRIG_1;
+	else if (((len) >> 4) & 0x1)
+		val |= SLINK_TX_TRIG_4 | SLINK_RX_TRIG_4;
 	else
-		val |= SLINK_IDLE_SCLK_DRIVE_LOW;
+		val |= SLINK_TX_TRIG_8 | SLINK_RX_TRIG_8;
 
-	val |= SLINK_M_S;
+	if (tspi->cur_direction & DATA_DIR_TX)
+		val |= SLINK_IE_TXC;
 
-	spi_tegra_writel(tspi, val, SLINK_COMMAND);
+	if (tspi->cur_direction & DATA_DIR_RX)
+		val |= SLINK_IE_RXC;
 
-	spi_tegra_writel(tspi, SLINK_RX_FLUSH | SLINK_TX_FLUSH, SLINK_STATUS);
+	spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+	tspi->dma_control_reg = val;
+
+	if (tspi->cur_direction & DATA_DIR_TX) {
+		spi_tegra_copy_client_txbuf_to_spi_txbuf(tspi, t);
+		wmb();
+		tspi->tx_dma_req.size = len;
+		ret = tegra_dma_enqueue_req(tspi->tx_dma, &tspi->tx_dma_req);
+		if (ret < 0) {
+			dev_err(&tspi->pdev->dev,
+				"Error in starting tx dma error = %d\n", ret);
+			return ret;
+		}
 
-	tspi->cur = t;
-	tspi->cur_pos = 0;
-	tspi->cur_len = spi_tegra_fill_tx_fifo(tspi, t);
+		/* Wait for tx fifo to be fill before starting slink */
+		test_val = spi_tegra_readl(tspi, SLINK_STATUS);
+		while (!(test_val & SLINK_TX_FULL))
+			test_val = spi_tegra_readl(tspi, SLINK_STATUS);
+	}
 
-	spi_tegra_go(tspi);
+	if (tspi->cur_direction & DATA_DIR_RX) {
+		/* Make the dma buffer to read by dma */
+		dma_sync_single_for_device(&tspi->pdev->dev, tspi->rx_buf_phys,
+				tspi->dma_buf_size, DMA_FROM_DEVICE);
+
+		tspi->rx_dma_req.size = len;
+		ret = tegra_dma_enqueue_req(tspi->rx_dma, &tspi->rx_dma_req);
+		if (ret < 0) {
+			dev_err(&tspi->pdev->dev,
+				"Error in starting rx dma error = %d\n", ret);
+			if (tspi->cur_direction & DATA_DIR_TX)
+				cancel_dma(tspi->tx_dma, &tspi->tx_dma_req);
+			return ret;
+		}
+	}
+	tspi->is_curr_dma_xfer = true;
+	if (tspi->is_packed) {
+		val |= SLINK_PACKED;
+		spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+		udelay(1);
+		wmb();
+	}
+	tspi->dma_control_reg = val;
+
+	val |= SLINK_DMA_EN;
+	spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+	return ret;
 }
 
-static void spi_tegra_start_message(struct spi_device *spi,
-				    struct spi_message *m)
+static int spi_tegra_start_cpu_based_transfer(
+		struct spi_tegra_data *tspi, struct spi_transfer *t)
 {
-	struct spi_transfer *t;
+	unsigned long val;
+	unsigned curr_words;
 
-	m->actual_length = 0;
-	m->status = 0;
+	val = tspi->packed_size;
+	if (tspi->cur_direction & DATA_DIR_TX)
+		val |= SLINK_IE_TXC;
 
-	t = list_first_entry(&m->transfers, struct spi_transfer, transfer_list);
-	spi_tegra_start_transfer(spi, t);
+	if (tspi->cur_direction & DATA_DIR_RX)
+		val |= SLINK_IE_RXC;
+
+	spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+	tspi->dma_control_reg = val;
+
+	if (tspi->cur_direction & DATA_DIR_TX)
+		curr_words = spi_tegra_fill_tx_fifo_from_client_txbuf(tspi, t);
+	else
+		curr_words = tspi->curr_dma_words;
+	val |= SLINK_DMA_BLOCK_SIZE(curr_words - 1);
+	spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+	tspi->dma_control_reg = val;
+
+	tspi->is_curr_dma_xfer = false;
+	if (tspi->is_packed) {
+		val |= SLINK_PACKED;
+		spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+		udelay(1);
+		wmb();
+	}
+	tspi->dma_control_reg = val;
+	val |= SLINK_DMA_EN;
+	spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+	return 0;
 }
 
-static void tegra_spi_rx_dma_complete(struct tegra_dma_req *req)
+static void set_best_clk_source(struct spi_tegra_data *tspi,
+		unsigned long speed)
 {
-	struct spi_tegra_data *tspi = req->dev;
-	unsigned long flags;
-	struct spi_message *m;
-	struct spi_device *spi;
-	int timeout = 0;
-	unsigned long val;
+	long new_rate;
+	unsigned long err_rate;
+	int rate = speed * 4;
+	unsigned int fin_err = speed * 4;
+	int final_index = -1;
+	int count;
+	int ret;
+	struct clk *pclk;
+	unsigned long prate, crate, nrate;
+	unsigned long cdiv;
+
+	if (!tspi->parent_clk_count || !tspi->parent_clk_list)
+		return;
+
+	/* make sure divisor is more than min_div */
+	pclk = clk_get_parent(tspi->clk);
+	prate = clk_get_rate(pclk);
+	crate = clk_get_rate(tspi->clk);
+	cdiv = DIV_ROUND_UP(prate, crate);
+	if (cdiv < tspi->min_div) {
+		nrate = DIV_ROUND_UP(prate, tspi->min_div);
+		clk_set_rate(tspi->clk, nrate);
+	}
 
-	/* the SPI controller may come back with both the BSY and RDY bits
-	 * set.  In this case we need to wait for the BSY bit to clear so
-	 * that we are sure the DMA is finished.  1000 reads was empirically
-	 * determined to be long enough.
-	 */
-	while (timeout++ < 1000) {
-		if (!(spi_tegra_readl(tspi, SLINK_STATUS) & SLINK_BSY))
-			break;
+	for (count = 0; count < tspi->parent_clk_count; ++count) {
+		if (!tspi->parent_clk_list[count].parent_clk)
+			continue;
+		ret = clk_set_parent(tspi->clk,
+			tspi->parent_clk_list[count].parent_clk);
+		if (ret < 0) {
+			dev_warn(&tspi->pdev->dev,
+				"Error in setting parent clk src %s\n",
+				tspi->parent_clk_list[count].name);
+			continue;
+		}
+
+		new_rate = clk_round_rate(tspi->clk, rate);
+		if (new_rate < 0)
+			continue;
+
+		err_rate = abs(new_rate - rate);
+		if (err_rate < fin_err) {
+			final_index = count;
+			fin_err = err_rate;
+		}
 	}
 
-	spin_lock_irqsave(&tspi->lock, flags);
+	if (final_index >= 0) {
+		dev_info(&tspi->pdev->dev, "Setting clk_src %s\n",
+				tspi->parent_clk_list[final_index].name);
+		clk_set_parent(tspi->clk,
+			tspi->parent_clk_list[final_index].parent_clk);
+	}
+}
 
-	val = spi_tegra_readl(tspi, SLINK_STATUS);
-	val |= SLINK_RDY;
-	spi_tegra_writel(tspi, val, SLINK_STATUS);
+static void spi_tegra_start_transfer(struct spi_device *spi,
+		    struct spi_transfer *t, bool is_first_of_msg,
+		    bool is_single_xfer)
+{
+	struct spi_tegra_data *tspi = spi_master_get_devdata(spi->master);
+	u32 speed;
+	u8 bits_per_word;
+	unsigned total_fifo_words;
+	int ret;
+	struct tegra_spi_device_controller_data *cdata = spi->controller_data;
+	unsigned long command;
+	unsigned long command2;
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+	unsigned long status2;
+#endif
+	int cs_setup_count;
+	int cs_hold_count;
+
+	unsigned int cs_pol_bit[] = {
+			SLINK_CS_POLARITY,
+			SLINK_CS_POLARITY1,
+			SLINK_CS_POLARITY2,
+			SLINK_CS_POLARITY3,
+	};
+
+	bits_per_word = t->bits_per_word ? t->bits_per_word :
+					spi->bits_per_word;
 
-	m = list_first_entry(&tspi->queue, struct spi_message, queue);
+	speed = t->speed_hz ? t->speed_hz : spi->max_speed_hz;
+	if (speed != tspi->cur_speed) {
+		set_best_clk_source(tspi, speed);
+		clk_set_rate(tspi->clk, speed * 4);
+		tspi->cur_speed = speed;
+	}
 
-	if (timeout >= 1000)
-		m->status = -EIO;
+	tspi->cur = t;
+	tspi->cur_spi = spi;
+	tspi->cur_pos = 0;
+	tspi->cur_rx_pos = 0;
+	tspi->cur_tx_pos = 0;
+	tspi->rx_complete = 0;
+	tspi->tx_complete = 0;
+	total_fifo_words = spi_tegra_calculate_curr_xfer_param(spi, tspi, t);
+
+	command2 = tspi->def_command2_reg;
+	if (is_first_of_msg) {
+		pm_runtime_get_sync(&tspi->pdev->dev);
+		tegra_spi_clk_enable(tspi);
+
+		spi_tegra_clear_status(tspi);
+
+		command = tspi->def_command_reg;
+		command |= SLINK_BIT_LENGTH(bits_per_word - 1);
+
+		/* possibly use the hw based chip select */
+		tspi->is_hw_based_cs = false;
+		if (cdata && cdata->is_hw_based_cs && is_single_xfer) {
+			if ((tspi->curr_dma_words * tspi->bytes_per_word) ==
+						(t->len - tspi->cur_pos)) {
+				cs_setup_count = cdata->cs_setup_clk_count >> 1;
+				if (cs_setup_count > 3)
+					cs_setup_count = 3;
+				cs_hold_count = cdata->cs_hold_clk_count;
+				if (cs_hold_count > 0xF)
+					cs_hold_count = 0xF;
+				tspi->is_hw_based_cs = true;
+
+				command &= ~SLINK_CS_SW;
+				command2 &= ~SLINK_SS_SETUP(3);
+				command2 |= SLINK_SS_SETUP(cs_setup_count);
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+				status2 = spi_tegra_readl(tspi, SLINK_STATUS2);
+				status2 &= ~SLINK_SS_HOLD_TIME(0xF);
+				status2 |= SLINK_SS_HOLD_TIME(cs_hold_count);
+				spi_tegra_writel(tspi, status2, SLINK_STATUS2);
+#endif
+			}
+		}
+		if (!tspi->is_hw_based_cs) {
+			command |= SLINK_CS_SW;
+			command ^= cs_pol_bit[spi->chip_select];
+		}
 
-	spi = m->state;
+		command &= ~SLINK_IDLE_SCLK_MASK & ~SLINK_CK_SDA;
+		if (spi->mode & SPI_CPHA)
+			command |= SLINK_CK_SDA;
 
-	tspi->cur_pos += spi_tegra_drain_rx_fifo(tspi, tspi->cur);
-	m->actual_length += tspi->cur_pos;
-
-	if (tspi->cur_pos < tspi->cur->len) {
-		tspi->cur_len = spi_tegra_fill_tx_fifo(tspi, tspi->cur);
-		spi_tegra_go(tspi);
-	} else if (!list_is_last(&tspi->cur->transfer_list,
-				 &m->transfers)) {
-		tspi->cur =  list_first_entry(&tspi->cur->transfer_list,
-					      struct spi_transfer,
-					      transfer_list);
-		spi_tegra_start_transfer(spi, tspi->cur);
+		if (spi->mode & SPI_CPOL)
+			command |= SLINK_IDLE_SCLK_DRIVE_HIGH;
+		else
+			command |= SLINK_IDLE_SCLK_DRIVE_LOW;
 	} else {
-		list_del(&m->queue);
+		command = tspi->command_reg;
+		command &= ~SLINK_BIT_LENGTH(~0);
+		command |= SLINK_BIT_LENGTH(bits_per_word - 1);
+	}
 
-		m->complete(m->context);
+	spi_tegra_writel(tspi, command, SLINK_COMMAND);
+	tspi->command_reg = command;
 
-		if (!list_empty(&tspi->queue)) {
-			m = list_first_entry(&tspi->queue, struct spi_message,
-					     queue);
-			spi = m->state;
-			spi_tegra_start_message(spi, m);
-		} else {
-			clk_disable(tspi->clk);
-			tspi->cur_speed = 0;
-		}
+	dev_dbg(&tspi->pdev->dev, "The def 0x%x and written 0x%lx\n",
+				tspi->def_command_reg, command);
+
+	command2 &= ~(SLINK_SS_EN_CS(~0) | SLINK_RXEN | SLINK_TXEN);
+	tspi->cur_direction = 0;
+	if (t->rx_buf) {
+		command2 |= SLINK_RXEN;
+		tspi->cur_direction |= DATA_DIR_RX;
+	}
+	if (t->tx_buf) {
+		command2 |= SLINK_TXEN;
+		tspi->cur_direction |= DATA_DIR_TX;
 	}
+	command2 |= SLINK_SS_EN_CS(spi->chip_select);
+	spi_tegra_writel(tspi, command2, SLINK_COMMAND2);
+	tspi->command2_reg = command2;
 
-	spin_unlock_irqrestore(&tspi->lock, flags);
+	if (total_fifo_words > SLINK_FIFO_DEPTH)
+		ret = spi_tegra_start_dma_based_transfer(tspi, t);
+	else
+		ret = spi_tegra_start_cpu_based_transfer(tspi, t);
+	WARN_ON(ret < 0);
 }
 
 static int spi_tegra_setup(struct spi_device *spi)
@@ -393,7 +864,7 @@ static int spi_tegra_setup(struct spi_device *spi)
 		spi->mode & SPI_CPHA ? "" : "~",
 		spi->max_speed_hz);
 
-
+	BUG_ON(spi->chip_select >= MAX_CHIP_SELECT);
 	switch (spi->chip_select) {
 	case 0:
 		cs_bit = SLINK_CS_POLARITY;
@@ -407,7 +878,7 @@ static int spi_tegra_setup(struct spi_device *spi)
 		cs_bit = SLINK_CS_POLARITY2;
 		break;
 
-	case 4:
+	case 3:
 		cs_bit = SLINK_CS_POLARITY3;
 		break;
 
@@ -415,26 +886,65 @@ static int spi_tegra_setup(struct spi_device *spi)
 		return -EINVAL;
 	}
 
-	spin_lock_irqsave(&tspi->lock, flags);
+	pm_runtime_get_sync(&tspi->pdev->dev);
+	tegra_spi_clk_enable(tspi);
 
-	val = spi_tegra_readl(tspi, SLINK_COMMAND);
+	spin_lock_irqsave(&tspi->lock, flags);
+	val = tspi->def_command_reg;
 	if (spi->mode & SPI_CS_HIGH)
 		val |= cs_bit;
 	else
 		val &= ~cs_bit;
-	spi_tegra_writel(tspi, val, SLINK_COMMAND);
-
+	tspi->def_command_reg = val;
+	spi_tegra_writel(tspi, tspi->def_command_reg, SLINK_COMMAND);
 	spin_unlock_irqrestore(&tspi->lock, flags);
 
+	tegra_spi_clk_disable(tspi);
+	pm_runtime_put_sync(&tspi->pdev->dev);
 	return 0;
 }
 
+static void tegra_spi_transfer_work(struct work_struct *work)
+{
+	struct spi_tegra_data *tspi;
+	struct spi_device *spi;
+	struct spi_message *m;
+	struct spi_transfer *t;
+	int single_xfer = 0;
+	unsigned long flags;
+
+	tspi = container_of(work, struct spi_tegra_data, spi_transfer_work);
+
+	spin_lock_irqsave(&tspi->lock, flags);
+
+	if (tspi->is_transfer_in_progress || tspi->is_suspended) {
+		spin_unlock_irqrestore(&tspi->lock, flags);
+		return;
+	}
+	if (list_empty(&tspi->queue)) {
+		spin_unlock_irqrestore(&tspi->lock, flags);
+		return;
+	}
+
+	m = list_first_entry(&tspi->queue, struct spi_message, queue);
+	spi = m->state;
+	single_xfer = list_is_singular(&m->transfers);
+	m->actual_length = 0;
+	m->status = 0;
+	t = list_first_entry(&m->transfers, struct spi_transfer, transfer_list);
+	tspi->is_transfer_in_progress = true;
+
+	spin_unlock_irqrestore(&tspi->lock, flags);
+	spi_tegra_start_transfer(spi, t, true, single_xfer);
+}
+
 static int spi_tegra_transfer(struct spi_device *spi, struct spi_message *m)
 {
 	struct spi_tegra_data *tspi = spi_master_get_devdata(spi->master);
 	struct spi_transfer *t;
 	unsigned long flags;
 	int was_empty;
+	int bytes_per_word;
 
 	if (list_empty(&m->transfers) || !m->complete)
 		return -EINVAL;
@@ -446,21 +956,365 @@ static int spi_tegra_transfer(struct spi_device *spi, struct spi_message *m)
 		if (t->len == 0)
 			return -EINVAL;
 
+		/* Check that the all words are available */
+		if (t->bits_per_word)
+			bytes_per_word = (t->bits_per_word + 7)/8;
+		else
+			bytes_per_word = (spi->bits_per_word + 7)/8;
+
+		if (t->len % bytes_per_word != 0)
+			return -EINVAL;
+
 		if (!t->rx_buf && !t->tx_buf)
 			return -EINVAL;
 	}
 
-	m->state = spi;
-
 	spin_lock_irqsave(&tspi->lock, flags);
+
+	if (WARN_ON(tspi->is_suspended)) {
+		spin_unlock_irqrestore(&tspi->lock, flags);
+		return -EBUSY;
+	}
+
+	m->state = spi;
 	was_empty = list_empty(&tspi->queue);
 	list_add_tail(&m->queue, &tspi->queue);
-
 	if (was_empty)
-		spi_tegra_start_message(spi, m);
+		queue_work(tspi->spi_workqueue, &tspi->spi_transfer_work);
 
 	spin_unlock_irqrestore(&tspi->lock, flags);
+	return 0;
+}
+
+static void spi_tegra_curr_transfer_complete(struct spi_tegra_data *tspi,
+	unsigned err, unsigned cur_xfer_size, unsigned long *irq_flags)
+{
+	struct spi_message *m;
+	struct spi_device *spi;
+	struct spi_transfer *t;
+	int single_xfer = 0;
+
+	/* Check if CS need to be toggele here */
+	if (tspi->cur && tspi->cur->cs_change &&
+				tspi->cur->delay_usecs) {
+		udelay(tspi->cur->delay_usecs);
+	}
+
+	if (list_empty(&tspi->queue)) {
+		dev_err(&tspi->pdev->dev, "Handling empty list\n");
+		return;
+	}
+
+	m = list_first_entry(&tspi->queue, struct spi_message, queue);
+	if (err)
+		m->status = -EIO;
+	spi = m->state;
+
+	m->actual_length += cur_xfer_size;
+
+	if (!list_is_last(&tspi->cur->transfer_list, &m->transfers)) {
+		tspi->cur = list_first_entry(&tspi->cur->transfer_list,
+			struct spi_transfer, transfer_list);
+		spin_unlock_irqrestore(&tspi->lock, *irq_flags);
+		spi_tegra_start_transfer(spi, tspi->cur, false, 0);
+		spin_lock_irqsave(&tspi->lock, *irq_flags);
+	} else {
+		list_del(&m->queue);
+		m->complete(m->context);
+		if (!list_empty(&tspi->queue)) {
+			if (tspi->is_suspended) {
+				spi_tegra_writel(tspi, tspi->def_command_reg,
+						SLINK_COMMAND);
+				spi_tegra_writel(tspi, tspi->def_command2_reg,
+						SLINK_COMMAND2);
+				tspi->is_transfer_in_progress = false;
+				return;
+			}
+			m = list_first_entry(&tspi->queue, struct spi_message,
+				queue);
+			spi = m->state;
+			single_xfer = list_is_singular(&m->transfers);
+			m->actual_length = 0;
+			m->status = 0;
+
+			t = list_first_entry(&m->transfers, struct spi_transfer,
+						transfer_list);
+			spin_unlock_irqrestore(&tspi->lock, *irq_flags);
+			spi_tegra_start_transfer(spi, t, true, single_xfer);
+			spin_lock_irqsave(&tspi->lock, *irq_flags);
+		} else {
+			spi_tegra_writel(tspi, tspi->def_command_reg,
+								SLINK_COMMAND);
+			spi_tegra_writel(tspi, tspi->def_command2_reg,
+								SLINK_COMMAND2);
+			/* Provide delay to stablize the signal state */
+			spin_unlock_irqrestore(&tspi->lock, *irq_flags);
+			udelay(10);
+			tegra_spi_clk_disable(tspi);
+			pm_runtime_put_sync(&tspi->pdev->dev);
+			spin_lock_irqsave(&tspi->lock, *irq_flags);
+			tspi->is_transfer_in_progress = false;
+			/* Check if any new request has come between
+			 * clock disable */
+			queue_work(tspi->spi_workqueue,
+					&tspi->spi_transfer_work);
+		}
+	}
+	return;
+}
+
+static void tegra_spi_tx_dma_complete(struct tegra_dma_req *req)
+{
+	struct spi_tegra_data *tspi = req->dev;
+	complete(&tspi->tx_dma_complete);
+}
+
+static void tegra_spi_rx_dma_complete(struct tegra_dma_req *req)
+{
+	struct spi_tegra_data *tspi = req->dev;
+	complete(&tspi->rx_dma_complete);
+}
+
+static void handle_cpu_based_xfer(void *context_data)
+{
+	struct spi_tegra_data *tspi = context_data;
+	struct spi_transfer *t = tspi->cur;
+	unsigned long flags;
+
+	spin_lock_irqsave(&tspi->lock, flags);
+	if (tspi->tx_status ||  tspi->rx_status ||
+				(tspi->status_reg & SLINK_BSY)) {
+		dev_err(&tspi->pdev->dev, "%s ERROR bit set 0x%x\n",
+					 __func__, tspi->status_reg);
+		dev_err(&tspi->pdev->dev, "%s 0x%08x:0x%08x:0x%08x\n",
+				__func__, tspi->command_reg, tspi->command2_reg,
+				tspi->dma_control_reg);
+		tegra_periph_reset_assert(tspi->clk);
+		udelay(2);
+		tegra_periph_reset_deassert(tspi->clk);
+		WARN_ON(1);
+		spi_tegra_curr_transfer_complete(tspi,
+			tspi->tx_status ||  tspi->rx_status, t->len, &flags);
+		goto exit;
+	}
+
+	dev_vdbg(&tspi->pdev->dev, "Current direction %x\n",
+					tspi->cur_direction);
+	if (tspi->cur_direction & DATA_DIR_RX)
+		spi_tegra_read_rx_fifo_to_client_rxbuf(tspi, t);
+
+	if (tspi->cur_direction & DATA_DIR_TX)
+		tspi->cur_pos = tspi->cur_tx_pos;
+	else if (tspi->cur_direction & DATA_DIR_RX)
+		tspi->cur_pos = tspi->cur_rx_pos;
+	else
+		WARN_ON(1);
+
+	dev_vdbg(&tspi->pdev->dev,
+		"current position %d and length of the transfer %d\n",
+			tspi->cur_pos, t->len);
+	if (tspi->cur_pos == t->len) {
+		spi_tegra_curr_transfer_complete(tspi,
+			tspi->tx_status || tspi->rx_status, t->len, &flags);
+		goto exit;
+	}
+
+	spi_tegra_calculate_curr_xfer_param(tspi->cur_spi, tspi, t);
+	spi_tegra_start_cpu_based_transfer(tspi, t);
+exit:
+	spin_unlock_irqrestore(&tspi->lock, flags);
+	return;
+}
+
+static irqreturn_t spi_tegra_isr_thread(int irq, void *context_data)
+{
+	struct spi_tegra_data *tspi = context_data;
+	struct spi_transfer *t = tspi->cur;
+	long wait_status;
+	int err = 0;
+	unsigned total_fifo_words;
+	unsigned long flags;
+
+	if (!tspi->is_curr_dma_xfer) {
+		handle_cpu_based_xfer(context_data);
+		return IRQ_HANDLED;
+	}
+
+	/* Abort dmas if any error */
+	if (tspi->cur_direction & DATA_DIR_TX) {
+		if (tspi->tx_status) {
+			cancel_dma(tspi->tx_dma, &tspi->tx_dma_req);
+			err += 1;
+		} else {
+			wait_status = wait_for_completion_interruptible_timeout(
+				&tspi->tx_dma_complete, SLINK_DMA_TIMEOUT);
+			if (wait_status <= 0) {
+				cancel_dma(tspi->tx_dma, &tspi->tx_dma_req);
+				dev_err(&tspi->pdev->dev,
+					"Error in Dma Tx transfer\n");
+				err += 1;
+			}
+		}
+	}
+
+	if (tspi->cur_direction & DATA_DIR_RX) {
+		if (tspi->rx_status) {
+			cancel_dma(tspi->rx_dma, &tspi->rx_dma_req);
+			err += 2;
+		} else {
+			wait_status = wait_for_completion_interruptible_timeout(
+				&tspi->rx_dma_complete, SLINK_DMA_TIMEOUT);
+			if (wait_status <= 0) {
+				cancel_dma(tspi->rx_dma, &tspi->rx_dma_req);
+				dev_err(&tspi->pdev->dev,
+					"Error in Dma Rx transfer\n");
+				err += 2;
+			}
+		}
+	}
+
+	spin_lock_irqsave(&tspi->lock, flags);
+	if (err) {
+		dev_err(&tspi->pdev->dev, "%s ERROR bit set 0x%x\n",
+					 __func__, tspi->status_reg);
+		dev_err(&tspi->pdev->dev, "%s 0x%08x:0x%08x:0x%08x\n",
+				__func__, tspi->command_reg, tspi->command2_reg,
+				tspi->dma_control_reg);
+		tegra_periph_reset_assert(tspi->clk);
+		udelay(2);
+		tegra_periph_reset_deassert(tspi->clk);
+		WARN_ON(1);
+		spi_tegra_curr_transfer_complete(tspi, err, t->len, &flags);
+		spin_unlock_irqrestore(&tspi->lock, flags);
+		return IRQ_HANDLED;
+	}
+
+	if (tspi->cur_direction & DATA_DIR_RX)
+		spi_tegra_copy_spi_rxbuf_to_client_rxbuf(tspi, t);
+
+	if (tspi->cur_direction & DATA_DIR_TX)
+		tspi->cur_pos = tspi->cur_tx_pos;
+	else if (tspi->cur_direction & DATA_DIR_RX)
+		tspi->cur_pos = tspi->cur_rx_pos;
+	else
+		WARN_ON(1);
+
+	if (tspi->cur_pos == t->len) {
+		spi_tegra_curr_transfer_complete(tspi,
+			tspi->tx_status || tspi->rx_status, t->len, &flags);
+		spin_unlock_irqrestore(&tspi->lock, flags);
+		return IRQ_HANDLED;
+	}
+
+	/* Continue transfer in current message */
+	total_fifo_words = spi_tegra_calculate_curr_xfer_param(tspi->cur_spi,
+							tspi, t);
+	if (total_fifo_words > SLINK_FIFO_DEPTH)
+		err = spi_tegra_start_dma_based_transfer(tspi, t);
+	else
+		err = spi_tegra_start_cpu_based_transfer(tspi, t);
+
+	spin_unlock_irqrestore(&tspi->lock, flags);
+	WARN_ON(err < 0);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t spi_tegra_isr(int irq, void *context_data)
+{
+	struct spi_tegra_data *tspi = context_data;
+
+	tspi->status_reg = spi_tegra_readl(tspi, SLINK_STATUS);
+	if (tspi->cur_direction & DATA_DIR_TX)
+		tspi->tx_status = tspi->status_reg &
+					(SLINK_TX_OVF | SLINK_TX_UNF);
 
+	if (tspi->cur_direction & DATA_DIR_RX)
+		tspi->rx_status = tspi->status_reg &
+					(SLINK_RX_OVF | SLINK_RX_UNF);
+	spi_tegra_clear_status(tspi);
+
+
+	return IRQ_WAKE_THREAD;
+}
+
+static void spi_tegra_deinit_dma_param(struct spi_tegra_data *tspi,
+	bool dma_to_memory)
+{
+	struct tegra_dma_channel *tdc;
+	u32 *dma_buf;
+	dma_addr_t dma_phys;
+
+	if (dma_to_memory) {
+		dma_buf = tspi->rx_buf;
+		tdc = tspi->rx_dma;
+		dma_phys = tspi->rx_buf_phys;
+		tspi->rx_dma = NULL;
+		tspi->rx_buf = NULL;
+	} else {
+		dma_buf = tspi->tx_buf;
+		tdc = tspi->tx_dma;
+		dma_phys = tspi->tx_buf_phys;
+		tspi->tx_buf = NULL;
+		tspi->tx_dma = NULL;
+	}
+
+	dma_free_coherent(&tspi->pdev->dev, tspi->dma_buf_size,
+			dma_buf, dma_phys);
+	tegra_dma_free_channel(tdc);
+}
+
+static int __init spi_tegra_init_dma_param(struct spi_tegra_data *tspi,
+			bool dma_to_memory)
+{
+	struct tegra_dma_req *dma_req;
+	struct tegra_dma_channel *tdc;
+	u32 *dma_buf;
+	dma_addr_t dma_phys;
+
+	tdc = tegra_dma_allocate_channel(TEGRA_DMA_MODE_ONESHOT, "spi_%s_%d",
+			(dma_to_memory) ? "rx" : "tx", tspi->pdev->id);
+	if (!tdc) {
+		dev_err(&tspi->pdev->dev, "can not allocate rx dma channel\n");
+		return -ENODEV;
+	}
+
+	dma_buf = dma_alloc_coherent(&tspi->pdev->dev, tspi->dma_buf_size,
+				&dma_phys, GFP_KERNEL);
+	if (!dma_buf) {
+		dev_err(&tspi->pdev->dev, "can not allocate rx bounce buffer");
+		tegra_dma_free_channel(tdc);
+		return -ENOMEM;
+	}
+
+	dma_req = (dma_to_memory) ? &tspi->rx_dma_req : &tspi->tx_dma_req;
+	memset(dma_req, 0, sizeof(*dma_req));
+
+	dma_req->req_sel = spi_tegra_req_sels[tspi->pdev->id];
+	dma_req->dev = tspi;
+	dma_req->dest_bus_width = 32;
+	dma_req->source_bus_width = 32;
+	dma_req->to_memory = (dma_to_memory) ? 1 : 0;
+	dma_req->virt_addr = dma_buf;
+	dma_req->dest_wrap = 0;
+	dma_req->source_wrap = 0;
+
+	if (dma_to_memory) {
+		dma_req->complete = tegra_spi_rx_dma_complete;
+		dma_req->dest_addr = dma_phys;
+		dma_req->source_addr = tspi->phys + SLINK_RX_FIFO;
+		dma_req->source_wrap = 4;
+		tspi->rx_buf_phys = dma_phys;
+		tspi->rx_buf = dma_buf;
+		tspi->rx_dma = tdc;
+	} else {
+		dma_req->complete = tegra_spi_tx_dma_complete;
+		dma_req->dest_addr = tspi->phys + SLINK_TX_FIFO;
+		dma_req->source_addr = dma_phys;
+		dma_req->dest_wrap = 4;
+		tspi->tx_buf = dma_buf;
+		tspi->tx_buf_phys = dma_phys;
+		tspi->tx_dma = tdc;
+	}
 	return 0;
 }
 
@@ -469,7 +1323,10 @@ static int __init spi_tegra_probe(struct platform_device *pdev)
 	struct spi_master	*master;
 	struct spi_tegra_data	*tspi;
 	struct resource		*r;
-	int ret;
+	struct tegra_spi_platform_data *pdata = pdev->dev.platform_data;
+	int ret, spi_irq;
+	int i;
+	char spi_wq_name[20];
 
 	master = spi_alloc_master(&pdev->dev, sizeof *tspi);
 	if (master == NULL) {
@@ -480,92 +1337,167 @@ static int __init spi_tegra_probe(struct platform_device *pdev)
 	/* the spi->mode bits understood by this driver: */
 	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_CS_HIGH;
 
-	master->bus_num = pdev->id;
+	if (pdev->id != -1)
+		master->bus_num = pdev->id;
 
 	master->setup = spi_tegra_setup;
 	master->transfer = spi_tegra_transfer;
-	master->num_chipselect = 4;
+	master->num_chipselect = MAX_CHIP_SELECT;
 
 	dev_set_drvdata(&pdev->dev, master);
 	tspi = spi_master_get_devdata(master);
 	tspi->master = master;
 	tspi->pdev = pdev;
+	tspi->is_transfer_in_progress = false;
+	tspi->is_suspended = false;
 	spin_lock_init(&tspi->lock);
+	spin_lock_init(&tspi->reg_lock);
 
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (r == NULL) {
+	if (!r) {
+		dev_err(&pdev->dev, "No IO memory resource\n");
 		ret = -ENODEV;
-		goto err0;
-	}
-
-	if (!request_mem_region(r->start, resource_size(r),
-				dev_name(&pdev->dev))) {
-		ret = -EBUSY;
-		goto err0;
+		goto exit_free_master;
 	}
-
 	tspi->phys = r->start;
-	tspi->base = ioremap(r->start, resource_size(r));
+	tspi->base = devm_request_and_ioremap(&pdev->dev, r);
 	if (!tspi->base) {
-		dev_err(&pdev->dev, "can't ioremap iomem\n");
-		ret = -ENOMEM;
-		goto err1;
+		dev_err(&pdev->dev,
+			"Cannot request memregion/iomap dma address\n");
+		ret = -EADDRNOTAVAIL;
+		goto exit_free_master;
 	}
 
-	tspi->clk = clk_get(&pdev->dev, NULL);
+	spi_irq = platform_get_irq(pdev, 0);
+	if (unlikely(spi_irq < 0)) {
+		dev_err(&pdev->dev, "can't find irq resource\n");
+		ret = -ENXIO;
+		goto exit_free_master;
+	}
+	tspi->irq = spi_irq;
+
+	sprintf(tspi->port_name, "tegra_spi_%d", pdev->id);
+	ret = devm_request_threaded_irq(&pdev->dev, tspi->irq,
+			spi_tegra_isr, spi_tegra_isr_thread, IRQF_ONESHOT,
+			tspi->port_name, tspi);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to register ISR for IRQ %d\n",
+					tspi->irq);
+		goto exit_free_master;
+	}
+
+	tspi->clk = devm_clk_get(&pdev->dev, "spi");
 	if (IS_ERR(tspi->clk)) {
 		dev_err(&pdev->dev, "can not get clock\n");
 		ret = PTR_ERR(tspi->clk);
-		goto err2;
+		goto exit_free_master;
+	}
+
+	tspi->sclk = devm_clk_get(&pdev->dev, "sclk");
+	if (IS_ERR(tspi->sclk)) {
+		dev_err(&pdev->dev, "can not get sclock\n");
+		ret = PTR_ERR(tspi->sclk);
+		goto exit_free_master;
 	}
 
 	INIT_LIST_HEAD(&tspi->queue);
 
-	tspi->rx_dma = tegra_dma_allocate_channel(TEGRA_DMA_MODE_ONESHOT);
-	if (!tspi->rx_dma) {
-		dev_err(&pdev->dev, "can not allocate rx dma channel\n");
-		ret = -ENODEV;
-		goto err3;
+	if (pdata) {
+		tspi->is_clkon_always = pdata->is_clkon_always;
+		tspi->is_dma_allowed = pdata->is_dma_based;
+		tspi->dma_buf_size = (pdata->max_dma_buffer) ?
+				pdata->max_dma_buffer : DEFAULT_SPI_DMA_BUF_LEN;
+		tspi->parent_clk_count = pdata->parent_clk_count;
+		tspi->parent_clk_list = pdata->parent_clk_list;
+		tspi->max_rate = pdata->max_rate;
+	} else {
+		tspi->is_clkon_always = false;
+		tspi->is_dma_allowed = true;
+		tspi->dma_buf_size = DEFAULT_SPI_DMA_BUF_LEN;
+		tspi->parent_clk_count = 0;
+		tspi->parent_clk_list = NULL;
+		tspi->max_rate = 0;
+	}
+
+	tspi->max_parent_rate = 0;
+	tspi->min_div = 0;
+
+	if (tspi->parent_clk_count) {
+		tspi->max_parent_rate = tspi->parent_clk_list[0].fixed_clk_rate;
+		for (i = 1; i < tspi->parent_clk_count; ++i) {
+			tspi->max_parent_rate = max(tspi->max_parent_rate,
+				tspi->parent_clk_list[i].fixed_clk_rate);
+		}
+		if (tspi->max_rate)
+			tspi->min_div = DIV_ROUND_UP(tspi->max_parent_rate,
+						tspi->max_rate);
+	}
+	tspi->max_buf_size = SLINK_FIFO_DEPTH << 2;
+
+	if (!tspi->is_dma_allowed)
+		goto skip_dma_alloc;
+
+	init_completion(&tspi->tx_dma_complete);
+	init_completion(&tspi->rx_dma_complete);
+
+	ret = spi_tegra_init_dma_param(tspi, true);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Error in rx dma init\n");
+		goto exit_free_master;
 	}
 
-	tspi->rx_bb = dma_alloc_coherent(&pdev->dev, sizeof(u32) * BB_LEN,
-					 &tspi->rx_bb_phys, GFP_KERNEL);
-	if (!tspi->rx_bb) {
-		dev_err(&pdev->dev, "can not allocate rx bounce buffer\n");
-		ret = -ENOMEM;
-		goto err4;
+	ret = spi_tegra_init_dma_param(tspi, false);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Error in tx dma init\n");
+		goto exit_rx_dma_free;
 	}
 
-	tspi->rx_dma_req.complete = tegra_spi_rx_dma_complete;
-	tspi->rx_dma_req.to_memory = 1;
-	tspi->rx_dma_req.dest_addr = tspi->rx_bb_phys;
-	tspi->rx_dma_req.dest_bus_width = 32;
-	tspi->rx_dma_req.source_addr = tspi->phys + SLINK_RX_FIFO;
-	tspi->rx_dma_req.source_bus_width = 32;
-	tspi->rx_dma_req.source_wrap = 4;
-	tspi->rx_dma_req.req_sel = spi_tegra_req_sels[pdev->id];
-	tspi->rx_dma_req.dev = tspi;
+	tspi->max_buf_size = tspi->dma_buf_size;
+	tspi->def_command_reg  = SLINK_CS_SW | SLINK_M_S;
+	tspi->def_command2_reg = SLINK_CS_ACTIVE_BETWEEN;
+
+skip_dma_alloc:
+	pm_runtime_enable(&pdev->dev);
+
+	/* Enable clock if it is require to be enable always */
+	if (tspi->is_clkon_always)
+		tegra_spi_clk_enable(tspi);
+
+	/* create the workqueue for the spi transfer */
+	snprintf(spi_wq_name, sizeof(spi_wq_name), "spi_tegra-%d", pdev->id);
+	tspi->spi_workqueue = create_singlethread_workqueue(spi_wq_name);
+	if (!tspi->spi_workqueue) {
+		dev_err(&pdev->dev, "Failed to create work queue\n");
+		ret = -ENODEV;
+		goto exit_fail_wq;
+	}
+
+	INIT_WORK(&tspi->spi_transfer_work, tegra_spi_transfer_work);
 
 	master->dev.of_node = pdev->dev.of_node;
 	ret = spi_register_master(master);
-
-	if (ret < 0)
-		goto err5;
+	if (ret < 0) {
+		dev_err(&pdev->dev, "can not register to master err %d\n", ret);
+		goto exit_destry_wq;
+	}
 
 	return ret;
 
-err5:
-	dma_free_coherent(&pdev->dev, sizeof(u32) * BB_LEN,
-			  tspi->rx_bb, tspi->rx_bb_phys);
-err4:
-	tegra_dma_free_channel(tspi->rx_dma);
-err3:
-	clk_put(tspi->clk);
-err2:
-	iounmap(tspi->base);
-err1:
-	release_mem_region(r->start, resource_size(r));
-err0:
+exit_destry_wq:
+	destroy_workqueue(tspi->spi_workqueue);
+
+exit_fail_wq:
+	if (tspi->is_clkon_always)
+		tegra_spi_clk_disable(tspi);
+
+	pm_runtime_disable(&pdev->dev);
+
+	spi_tegra_deinit_dma_param(tspi, false);
+
+exit_rx_dma_free:
+	spi_tegra_deinit_dma_param(tspi, true);
+
+exit_free_master:
 	spi_master_put(master);
 	return ret;
 }
@@ -574,25 +1506,129 @@ static int __devexit spi_tegra_remove(struct platform_device *pdev)
 {
 	struct spi_master	*master;
 	struct spi_tegra_data	*tspi;
-	struct resource		*r;
 
 	master = dev_get_drvdata(&pdev->dev);
 	tspi = spi_master_get_devdata(master);
 
 	spi_unregister_master(master);
-	tegra_dma_free_channel(tspi->rx_dma);
 
-	dma_free_coherent(&pdev->dev, sizeof(u32) * BB_LEN,
-			  tspi->rx_bb, tspi->rx_bb_phys);
+	if (tspi->tx_dma)
+		spi_tegra_deinit_dma_param(tspi, false);
 
-	clk_put(tspi->clk);
-	iounmap(tspi->base);
+	if (tspi->rx_dma)
+		spi_tegra_deinit_dma_param(tspi, true);
 
-	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	release_mem_region(r->start, resource_size(r));
+	/* Disable clock if it is always enabled */
+	if (tspi->is_clkon_always)
+		tegra_spi_clk_disable(tspi);
+
+	pm_runtime_disable(&pdev->dev);
+
+	destroy_workqueue(tspi->spi_workqueue);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int spi_tegra_suspend(struct device *dev)
+{
+	struct spi_master *master = dev_get_drvdata(dev);
+	struct spi_tegra_data *tspi = spi_master_get_devdata(master);
+	unsigned limit = 50;
+	unsigned long flags;
+
+	spin_lock_irqsave(&tspi->lock, flags);
+
+	/* Wait for all transfer completes */
+	if (!list_empty(&tspi->queue))
+		dev_warn(dev, "The transfer list is not empty "
+			"Waiting for time %d ms to complete transfer\n",
+			limit * 20);
+
+	while (!list_empty(&tspi->queue) && limit--) {
+		spin_unlock_irqrestore(&tspi->lock, flags);
+		msleep(20);
+		spin_lock_irqsave(&tspi->lock, flags);
+	}
+
+	/* Wait for current transfer completes only */
+	tspi->is_suspended = true;
+	if (!list_empty(&tspi->queue)) {
+		limit = 50;
+		dev_err(dev, "All transfer has not completed, "
+			"Waiting for %d ms current transfer to complete\n",
+			limit * 20);
+		while (tspi->is_transfer_in_progress && limit--) {
+			spin_unlock_irqrestore(&tspi->lock, flags);
+			msleep(20);
+			spin_lock_irqsave(&tspi->lock, flags);
+		}
+	}
+
+	if (tspi->is_transfer_in_progress) {
+		dev_err(dev,
+			"Spi transfer is in progress Avoiding suspend\n");
+		tspi->is_suspended = false;
+		spin_unlock_irqrestore(&tspi->lock, flags);
+		return -EBUSY;
+	}
+
+	spin_unlock_irqrestore(&tspi->lock, flags);
+
+	/* Disable clock if it is always enabled */
+	if (tspi->is_clkon_always)
+		tegra_spi_clk_disable(tspi);
+
+	return 0;
+}
+
+static int spi_tegra_resume(struct device *dev)
+{
+	struct spi_master *master = dev_get_drvdata(dev);
+	struct spi_tegra_data *tspi = spi_master_get_devdata(master);
+	struct spi_message *m;
+	struct spi_device *spi;
+	struct spi_transfer *t = NULL;
+	int single_xfer = 0;
+	unsigned long flags;
 
+	/* Enable clock if it is always enabled */
+	if (tspi->is_clkon_always)
+		tegra_spi_clk_enable(tspi);
+
+	pm_runtime_get_sync(dev);
+	tegra_spi_clk_enable(tspi);
+	spi_tegra_writel(tspi, tspi->command_reg, SLINK_COMMAND);
+	tegra_spi_clk_disable(tspi);
+	pm_runtime_put_sync(dev);
+
+	spin_lock_irqsave(&tspi->lock, flags);
+
+	tspi->cur_speed = 0;
+	tspi->is_suspended = false;
+	if (!list_empty(&tspi->queue)) {
+		m = list_first_entry(&tspi->queue, struct spi_message, queue);
+		spi = m->state;
+		single_xfer = list_is_singular(&m->transfers);
+		m->actual_length = 0;
+		m->status = 0;
+		t = list_first_entry(&m->transfers, struct spi_transfer,
+						transfer_list);
+		tspi->is_transfer_in_progress = true;
+	}
+	spin_unlock_irqrestore(&tspi->lock, flags);
+	if (t)
+		spi_tegra_start_transfer(spi, t, true, single_xfer);
 	return 0;
 }
+#endif
+
+static const struct dev_pm_ops tegra_spi_dev_pm_ops = {
+#ifdef CONFIG_PM
+	.suspend = spi_tegra_suspend,
+	.resume = spi_tegra_resume,
+#endif
+};
 
 MODULE_ALIAS("platform:spi_tegra");
 
@@ -610,6 +1646,7 @@ static struct platform_driver spi_tegra_driver = {
 	.driver = {
 		.name =		"spi_tegra",
 		.owner =	THIS_MODULE,
+		.pm =		&tegra_spi_dev_pm_ops,
 		.of_match_table = spi_tegra_of_match_table,
 	},
 	.remove =	__devexit_p(spi_tegra_remove),
@@ -619,7 +1656,7 @@ static int __init spi_tegra_init(void)
 {
 	return platform_driver_probe(&spi_tegra_driver, spi_tegra_probe);
 }
-module_init(spi_tegra_init);
+subsys_initcall(spi_tegra_init);
 
 static void __exit spi_tegra_exit(void)
 {
diff --git a/drivers/spi/spi-topcliff-pch.c b/drivers/spi/spi-topcliff-pch.c
index 6a80749391db..7d22559e54c4 100644
--- a/drivers/spi/spi-topcliff-pch.c
+++ b/drivers/spi/spi-topcliff-pch.c
@@ -1075,8 +1075,8 @@ static void pch_spi_handle_dma(struct pch_spi_data *data, int *bpw)
 		sg_dma_address(sg) = dma->rx_buf_dma + sg->offset;
 	}
 	sg = dma->sg_rx_p;
-	desc_rx = dma->chan_rx->device->device_prep_slave_sg(dma->chan_rx, sg,
-					num, DMA_FROM_DEVICE,
+	desc_rx = dmaengine_prep_slave_sg(dma->chan_rx, sg,
+					num, DMA_DEV_TO_MEM,
 					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	if (!desc_rx) {
 		dev_err(&data->master->dev, "%s:device_prep_slave_sg Failed\n",
@@ -1120,8 +1120,8 @@ static void pch_spi_handle_dma(struct pch_spi_data *data, int *bpw)
 		sg_dma_address(sg) = dma->tx_buf_dma + sg->offset;
 	}
 	sg = dma->sg_tx_p;
-	desc_tx = dma->chan_tx->device->device_prep_slave_sg(dma->chan_tx,
-					sg, num, DMA_TO_DEVICE,
+	desc_tx = dmaengine_prep_slave_sg(dma->chan_tx,
+					sg, num, DMA_MEM_TO_DEV,
 					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	if (!desc_tx) {
 		dev_err(&data->master->dev, "%s:device_prep_slave_sg Failed\n",
diff --git a/drivers/spi/spi_slave_tegra.c b/drivers/spi/spi_slave_tegra.c
new file mode 100644
index 000000000000..e6bce127cae3
--- /dev/null
+++ b/drivers/spi/spi_slave_tegra.c
@@ -0,0 +1,1406 @@
+/*
+ * Driver for Nvidia TEGRA spi controller in slave mode.
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/*#define DEBUG           1*/
+/*#define VERBOSE_DEBUG   1*/
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/completion.h>
+
+#include <linux/spi/spi.h>
+#include <linux/spi-tegra.h>
+
+#include <mach/dma.h>
+#include <mach/clk.h>
+#include <mach/spi.h>
+
+#define SLINK_COMMAND		0x000
+#define   SLINK_BIT_LENGTH(x)		(((x) & 0x1f) << 0)
+#define   SLINK_WORD_SIZE(x)		(((x) & 0x1f) << 5)
+#define   SLINK_BOTH_EN			(1 << 10)
+#define   SLINK_CS_SW			(1 << 11)
+#define   SLINK_CS_VALUE		(1 << 12)
+#define   SLINK_CS_POLARITY		(1 << 13)
+#define   SLINK_IDLE_SDA_DRIVE_LOW	(0 << 16)
+#define   SLINK_IDLE_SDA_DRIVE_HIGH	(1 << 16)
+#define   SLINK_IDLE_SDA_PULL_LOW	(2 << 16)
+#define   SLINK_IDLE_SDA_PULL_HIGH	(3 << 16)
+#define   SLINK_IDLE_SDA_MASK		(3 << 16)
+#define   SLINK_CS_POLARITY1		(1 << 20)
+#define   SLINK_CK_SDA			(1 << 21)
+#define   SLINK_CS_POLARITY2		(1 << 22)
+#define   SLINK_CS_POLARITY3		(1 << 23)
+#define   SLINK_IDLE_SCLK_DRIVE_LOW	(0 << 24)
+#define   SLINK_IDLE_SCLK_DRIVE_HIGH	(1 << 24)
+#define   SLINK_IDLE_SCLK_PULL_LOW	(2 << 24)
+#define   SLINK_IDLE_SCLK_PULL_HIGH	(3 << 24)
+#define   SLINK_IDLE_SCLK_MASK		(3 << 24)
+#define   SLINK_M_S			(1 << 28)
+#define   SLINK_WAIT			(1 << 29)
+#define   SLINK_GO			(1 << 30)
+#define   SLINK_ENB			(1 << 31)
+
+#define SLINK_COMMAND2		0x004
+#define   SLINK_LSBFE			(1 << 0)
+#define   SLINK_SSOE			(1 << 1)
+#define   SLINK_SPIE			(1 << 4)
+#define   SLINK_BIDIROE			(1 << 6)
+#define   SLINK_MODFEN			(1 << 7)
+#define   SLINK_INT_SIZE(x)		(((x) & 0x1f) << 8)
+#define   SLINK_CS_ACTIVE_BETWEEN	(1 << 17)
+#define   SLINK_SS_EN_CS(x)		(((x) & 0x3) << 18)
+#define   SLINK_SS_SETUP(x)		(((x) & 0x3) << 20)
+#define   SLINK_FIFO_REFILLS_0		(0 << 22)
+#define   SLINK_FIFO_REFILLS_1		(1 << 22)
+#define   SLINK_FIFO_REFILLS_2		(2 << 22)
+#define   SLINK_FIFO_REFILLS_3		(3 << 22)
+#define   SLINK_FIFO_REFILLS_MASK	(3 << 22)
+#define   SLINK_WAIT_PACK_INT(x)	(((x) & 0x7) << 26)
+#define   SLINK_SPC0			(1 << 29)
+#define   SLINK_TXEN			(1 << 30)
+#define   SLINK_RXEN			(1 << 31)
+
+#define SLINK_STATUS		0x008
+#define   SLINK_COUNT(val)		(((val) >> 0) & 0x1f)
+#define   SLINK_WORD(val)		(((val) >> 5) & 0x1f)
+#define   SLINK_BLK_CNT(val)		(((val) >> 0) & 0xffff)
+#define   SLINK_MODF			(1 << 16)
+#define   SLINK_RX_UNF			(1 << 18)
+#define   SLINK_TX_OVF			(1 << 19)
+#define   SLINK_TX_FULL			(1 << 20)
+#define   SLINK_TX_EMPTY		(1 << 21)
+#define   SLINK_RX_FULL			(1 << 22)
+#define   SLINK_RX_EMPTY		(1 << 23)
+#define   SLINK_TX_UNF			(1 << 24)
+#define   SLINK_RX_OVF			(1 << 25)
+#define   SLINK_TX_FLUSH		(1 << 26)
+#define   SLINK_RX_FLUSH		(1 << 27)
+#define   SLINK_SCLK			(1 << 28)
+#define   SLINK_ERR			(1 << 29)
+#define   SLINK_RDY			(1 << 30)
+#define   SLINK_BSY			(1 << 31)
+
+#define SLINK_MAS_DATA		0x010
+#define SLINK_SLAVE_DATA	0x014
+
+#define SLINK_DMA_CTL		0x018
+#define   SLINK_DMA_BLOCK_SIZE(x)	(((x) & 0xffff) << 0)
+#define   SLINK_TX_TRIG_1		(0 << 16)
+#define   SLINK_TX_TRIG_4		(1 << 16)
+#define   SLINK_TX_TRIG_8		(2 << 16)
+#define   SLINK_TX_TRIG_16		(3 << 16)
+#define   SLINK_TX_TRIG_MASK		(3 << 16)
+#define   SLINK_RX_TRIG_1		(0 << 18)
+#define   SLINK_RX_TRIG_4		(1 << 18)
+#define   SLINK_RX_TRIG_8		(2 << 18)
+#define   SLINK_RX_TRIG_16		(3 << 18)
+#define   SLINK_RX_TRIG_MASK		(3 << 18)
+#define   SLINK_PACKED			(1 << 20)
+#define   SLINK_PACK_SIZE_4		(0 << 21)
+#define   SLINK_PACK_SIZE_8		(1 << 21)
+#define   SLINK_PACK_SIZE_16		(2 << 21)
+#define   SLINK_PACK_SIZE_32		(3 << 21)
+#define   SLINK_PACK_SIZE_MASK		(3 << 21)
+#define   SLINK_IE_TXC			(1 << 26)
+#define   SLINK_IE_RXC			(1 << 27)
+#define   SLINK_DMA_EN			(1 << 31)
+
+#define SLINK_STATUS2		0x01c
+#define   SLINK_TX_FIFO_EMPTY_COUNT(val)	(((val) & 0x3f) >> 0)
+#define   SLINK_RX_FIFO_FULL_COUNT(val)		(((val) & 0x3f0000) >> 16)
+#define   SLINK_SS_HOLD_TIME(val)		(((val) & 0xF) << 6)
+
+#define SLINK_TX_FIFO		0x100
+#define SLINK_RX_FIFO		0x180
+
+#define DATA_DIR_TX		(1 << 0)
+#define DATA_DIR_RX		(1 << 1)
+
+#define SPI_FIFO_DEPTH		32
+#define SLINK_DMA_TIMEOUT (msecs_to_jiffies(1000))
+
+
+static const unsigned long spi_tegra_req_sels[] = {
+	TEGRA_DMA_REQ_SEL_SL2B1,
+	TEGRA_DMA_REQ_SEL_SL2B2,
+	TEGRA_DMA_REQ_SEL_SL2B3,
+	TEGRA_DMA_REQ_SEL_SL2B4,
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+	TEGRA_DMA_REQ_SEL_SL2B5,
+	TEGRA_DMA_REQ_SEL_SL2B6,
+#endif
+
+};
+
+#define DEFAULT_SPI_DMA_BUF_LEN		(16*1024)
+#define TX_FIFO_EMPTY_COUNT_MAX		SLINK_TX_FIFO_EMPTY_COUNT(0x20)
+#define RX_FIFO_FULL_COUNT_ZERO		SLINK_RX_FIFO_FULL_COUNT(0)
+
+#define SLINK_STATUS2_RESET \
+	(TX_FIFO_EMPTY_COUNT_MAX | \
+	RX_FIFO_FULL_COUNT_ZERO << 16)
+
+#define MAX_CHIP_SELECT		4
+#define SLINK_FIFO_DEPTH	4
+
+struct spi_tegra_data {
+	struct spi_master	*master;
+	struct platform_device	*pdev;
+	spinlock_t		lock;
+	char			port_name[32];
+
+	struct clk		*clk;
+	void __iomem		*base;
+	unsigned long		phys;
+	unsigned		irq;
+
+	u32			cur_speed;
+
+	struct list_head	queue;
+	struct spi_transfer	*cur;
+	struct spi_device	*cur_spi;
+	unsigned		cur_pos;
+	unsigned		cur_len;
+	unsigned		words_per_32bit;
+	unsigned		bytes_per_word;
+	unsigned		curr_dma_words;
+
+	unsigned		cur_direction;
+
+	bool			is_dma_allowed;
+
+	struct tegra_dma_req	rx_dma_req;
+	struct tegra_dma_channel *rx_dma;
+	u32			*rx_buf;
+	dma_addr_t		rx_buf_phys;
+	unsigned		cur_rx_pos;
+
+	struct tegra_dma_req	tx_dma_req;
+	struct tegra_dma_channel *tx_dma;
+	u32			*tx_buf;
+	dma_addr_t		tx_buf_phys;
+	unsigned		cur_tx_pos;
+
+	unsigned		dma_buf_size;
+	unsigned		max_buf_size;
+	bool			is_curr_dma_xfer;
+
+	bool			is_clkon_always;
+	bool			clk_state;
+	bool			is_suspended;
+
+	bool			is_hw_based_cs;
+
+	struct completion	rx_dma_complete;
+	struct completion	tx_dma_complete;
+
+	u32			rx_complete;
+	u32			tx_complete;
+	u32			tx_status;
+	u32			rx_status;
+	u32			status_reg;
+	bool			is_packed;
+	unsigned long		packed_size;
+
+	u32			command_reg;
+	u32			command2_reg;
+	u32			dma_control_reg;
+	u32			def_command_reg;
+	u32			def_command2_reg;
+
+	callback		client_slave_ready_cb;
+	void			*client_data;
+
+	struct spi_clk_parent	*parent_clk_list;
+	int			parent_clk_count;
+	unsigned long		max_rate;
+	unsigned long		max_parent_rate;
+	int			min_div;
+};
+
+static inline unsigned long spi_tegra_readl(struct spi_tegra_data *tspi,
+		    unsigned long reg)
+{
+	if (!tspi->clk_state)
+		BUG();
+	return readl(tspi->base + reg);
+}
+
+static inline void spi_tegra_writel(struct spi_tegra_data *tspi,
+		    unsigned long val, unsigned long reg)
+{
+	if (!tspi->clk_state)
+		BUG();
+	writel(val, tspi->base + reg);
+}
+
+static void cancel_dma(struct tegra_dma_channel *dma_chan,
+		struct tegra_dma_req *req)
+{
+	tegra_dma_cancel(dma_chan);
+	if (req->status == -TEGRA_DMA_REQ_ERROR_ABORTED)
+		req->complete(req);
+}
+
+int spi_tegra_register_callback(struct spi_device *spi, callback func,
+			void *client_data)
+{
+	struct spi_tegra_data *tspi = spi_master_get_devdata(spi->master);
+
+	if (!tspi || !func)
+		return -EINVAL;
+	tspi->client_slave_ready_cb = func;
+	tspi->client_data = client_data;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(spi_tegra_register_callback);
+
+static void spi_tegra_clear_status(struct spi_tegra_data *tspi)
+{
+	unsigned long val;
+	unsigned long val_write = 0;
+
+	val = spi_tegra_readl(tspi, SLINK_STATUS);
+
+	val_write = SLINK_RDY;
+	if (val & SLINK_TX_OVF)
+		val_write |= SLINK_TX_OVF;
+	if (val & SLINK_RX_OVF)
+		val_write |= SLINK_RX_OVF;
+	if (val & SLINK_RX_UNF)
+		val_write |= SLINK_RX_UNF;
+	if (val & SLINK_TX_UNF)
+		val_write |= SLINK_TX_UNF;
+
+	spi_tegra_writel(tspi, val_write, SLINK_STATUS);
+}
+
+static unsigned long spi_tegra_get_packed_size(struct spi_tegra_data *tspi,
+				  struct spi_transfer *t)
+{
+	unsigned long val;
+
+	switch (tspi->bytes_per_word) {
+	case 0:
+		val = SLINK_PACK_SIZE_4;
+		break;
+	case 1:
+		val = SLINK_PACK_SIZE_8;
+		break;
+	case 2:
+		val = SLINK_PACK_SIZE_16;
+		break;
+	case 4:
+		val = SLINK_PACK_SIZE_32;
+		break;
+	default:
+		val = 0;
+	}
+	return val;
+}
+
+static unsigned spi_tegra_calculate_curr_xfer_param(
+	struct spi_device *spi, struct spi_tegra_data *tspi,
+	struct spi_transfer *t)
+{
+	unsigned remain_len = t->len - tspi->cur_pos;
+	unsigned max_word;
+	unsigned bits_per_word ;
+	unsigned max_len;
+	unsigned total_fifo_words;
+
+	bits_per_word = t->bits_per_word ? t->bits_per_word :
+						spi->bits_per_word;
+	tspi->bytes_per_word = (bits_per_word - 1) / 8 + 1;
+
+	if (bits_per_word == 8 || bits_per_word == 16) {
+		tspi->is_packed = 1;
+		tspi->words_per_32bit = 32/bits_per_word;
+	} else {
+		tspi->is_packed = 0;
+		tspi->words_per_32bit = 1;
+	}
+	tspi->packed_size = spi_tegra_get_packed_size(tspi, t);
+
+	if (tspi->is_packed) {
+		max_len = min(remain_len, tspi->max_buf_size);
+		tspi->curr_dma_words = max_len/tspi->bytes_per_word;
+		total_fifo_words = remain_len/4;
+	} else {
+		max_word = (remain_len - 1) / tspi->bytes_per_word + 1;
+		max_word = min(max_word, tspi->max_buf_size/4);
+		tspi->curr_dma_words = max_word;
+		total_fifo_words = remain_len/tspi->bytes_per_word;
+	}
+	/* All transfer should be in one shot */
+	if (tspi->curr_dma_words * tspi->bytes_per_word != t->len) {
+		dev_err(&tspi->pdev->dev, "The requested length can not be"
+					" transferred in one shot\n");
+		BUG();
+	}
+	return total_fifo_words;
+}
+
+static unsigned spi_tegra_fill_tx_fifo_from_client_txbuf(
+	struct spi_tegra_data *tspi, struct spi_transfer *t)
+{
+	unsigned nbytes;
+	unsigned tx_empty_count;
+	unsigned long fifo_status;
+	u8 *tx_buf = (u8 *)t->tx_buf + tspi->cur_tx_pos;
+	unsigned max_n_32bit;
+	unsigned i, count;
+	unsigned long x;
+	unsigned int written_words;
+
+	fifo_status = spi_tegra_readl(tspi, SLINK_STATUS2);
+	tx_empty_count = SLINK_TX_FIFO_EMPTY_COUNT(fifo_status);
+
+	if (tspi->is_packed) {
+		nbytes = tspi->curr_dma_words * tspi->bytes_per_word;
+		max_n_32bit = (min(nbytes,  tx_empty_count*4) - 1)/4 + 1;
+		for (count = 0; count < max_n_32bit; ++count) {
+			x = 0;
+			for (i = 0; (i < 4) && nbytes; i++, nbytes--)
+				x |= (*tx_buf++) << (i*8);
+			spi_tegra_writel(tspi, x, SLINK_TX_FIFO);
+		}
+		written_words =  min(max_n_32bit * tspi->words_per_32bit,
+					tspi->curr_dma_words);
+	} else {
+		max_n_32bit = min(tspi->curr_dma_words,  tx_empty_count);
+		nbytes = max_n_32bit * tspi->bytes_per_word;
+		for (count = 0; count < max_n_32bit; ++count) {
+			x = 0;
+			for (i = 0; nbytes && (i < tspi->bytes_per_word);
+							++i, nbytes--)
+				x |= ((*tx_buf++) << i*8);
+			spi_tegra_writel(tspi, x, SLINK_TX_FIFO);
+		}
+		written_words = max_n_32bit;
+	}
+	tspi->cur_tx_pos += written_words * tspi->bytes_per_word;
+	return written_words;
+}
+
+static unsigned int spi_tegra_read_rx_fifo_to_client_rxbuf(
+		struct spi_tegra_data *tspi, struct spi_transfer *t)
+{
+	unsigned rx_full_count;
+	unsigned long fifo_status;
+	u8 *rx_buf = (u8 *)t->rx_buf + tspi->cur_rx_pos;
+	unsigned i, count;
+	unsigned long x;
+	unsigned int read_words = 0;
+	unsigned len;
+
+	fifo_status = spi_tegra_readl(tspi, SLINK_STATUS2);
+	rx_full_count = SLINK_RX_FIFO_FULL_COUNT(fifo_status);
+	dev_dbg(&tspi->pdev->dev, "Rx fifo count %d\n", rx_full_count);
+	if (tspi->is_packed) {
+		len = tspi->curr_dma_words * tspi->bytes_per_word;
+		for (count = 0; count < rx_full_count; ++count) {
+			x = spi_tegra_readl(tspi, SLINK_RX_FIFO);
+			for (i = 0; len && (i < 4); ++i, len--)
+				*rx_buf++ = (x >> i*8) & 0xFF;
+		}
+		tspi->cur_rx_pos += tspi->curr_dma_words * tspi->bytes_per_word;
+		read_words += tspi->curr_dma_words;
+	} else {
+		for (count = 0; count < rx_full_count; ++count) {
+			x = spi_tegra_readl(tspi, SLINK_RX_FIFO);
+			for (i = 0; (i < tspi->bytes_per_word); ++i)
+				*rx_buf++ = (x >> (i*8)) & 0xFF;
+		}
+		tspi->cur_rx_pos += rx_full_count * tspi->bytes_per_word;
+		read_words += rx_full_count;
+	}
+	return read_words;
+}
+
+static void spi_tegra_copy_client_txbuf_to_spi_txbuf(
+		struct spi_tegra_data *tspi, struct spi_transfer *t)
+{
+	unsigned len;
+	if (tspi->is_packed) {
+		len = tspi->curr_dma_words * tspi->bytes_per_word;
+		memcpy(tspi->tx_buf, t->tx_buf + tspi->cur_pos, len);
+	} else {
+		unsigned int i;
+		unsigned int count;
+		u8 *tx_buf = (u8 *)t->tx_buf + tspi->cur_tx_pos;
+		unsigned consume = tspi->curr_dma_words * tspi->bytes_per_word;
+		unsigned int x;
+
+		for (count = 0; count < tspi->curr_dma_words; ++count) {
+			x = 0;
+			for (i = 0; consume && (i < tspi->bytes_per_word);
+							++i, consume--)
+				x |= ((*tx_buf++) << i*8);
+			tspi->tx_buf[count] = x;
+		}
+	}
+	tspi->cur_tx_pos += tspi->curr_dma_words * tspi->bytes_per_word;
+}
+
+static void spi_tegra_copy_spi_rxbuf_to_client_rxbuf(
+		struct spi_tegra_data *tspi, struct spi_transfer *t)
+{
+	unsigned len;
+	if (tspi->is_packed) {
+		len = tspi->curr_dma_words * tspi->bytes_per_word;
+		memcpy(t->rx_buf + tspi->cur_rx_pos, tspi->rx_buf, len);
+	} else {
+		unsigned int i;
+		unsigned int count;
+		unsigned char *rx_buf = t->rx_buf + tspi->cur_rx_pos;
+		unsigned int x;
+		for (count = 0; count < tspi->curr_dma_words; ++count) {
+			x = tspi->rx_buf[count];
+			for (i = 0; (i < tspi->bytes_per_word); ++i)
+				*rx_buf++ = (x >> (i*8)) & 0xFF;
+		}
+	}
+	tspi->cur_rx_pos += tspi->curr_dma_words * tspi->bytes_per_word;
+}
+
+static int spi_tegra_start_dma_based_transfer(
+		struct spi_tegra_data *tspi, struct spi_transfer *t)
+{
+	unsigned long val;
+	unsigned long test_val;
+	unsigned int len;
+	int ret = 0;
+
+	INIT_COMPLETION(tspi->rx_dma_complete);
+	INIT_COMPLETION(tspi->tx_dma_complete);
+
+	val = SLINK_DMA_BLOCK_SIZE(tspi->curr_dma_words - 1);
+	val |= tspi->packed_size;
+	if (tspi->is_packed)
+		len = DIV_ROUND_UP(tspi->curr_dma_words * tspi->bytes_per_word,
+					4) * 4;
+	else
+		len = tspi->curr_dma_words * 4;
+
+	if (len & 0xF)
+		val |= SLINK_TX_TRIG_1 | SLINK_RX_TRIG_1;
+	else if (((len) >> 4) & 0x1)
+		val |= SLINK_TX_TRIG_4 | SLINK_RX_TRIG_4;
+	else
+		val |= SLINK_TX_TRIG_8 | SLINK_RX_TRIG_8;
+
+	if (tspi->cur_direction & DATA_DIR_TX)
+		val |= SLINK_IE_TXC;
+
+	if (tspi->cur_direction & DATA_DIR_RX)
+		val |= SLINK_IE_RXC;
+
+	spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+	tspi->dma_control_reg = val;
+
+	if (tspi->cur_direction & DATA_DIR_TX) {
+		spi_tegra_copy_client_txbuf_to_spi_txbuf(tspi, t);
+		wmb();
+		tspi->tx_dma_req.size = len;
+		ret = tegra_dma_enqueue_req(tspi->tx_dma, &tspi->tx_dma_req);
+		if (ret < 0) {
+			dev_err(&tspi->pdev->dev, "Error in starting tx dma "
+						" error = %d\n", ret);
+			return ret;
+		}
+
+		/* Wait for tx fifo to be fill before starting slink */
+		test_val = spi_tegra_readl(tspi, SLINK_STATUS);
+		while (!(test_val & SLINK_TX_FULL))
+			test_val = spi_tegra_readl(tspi, SLINK_STATUS);
+	}
+
+	if (tspi->cur_direction & DATA_DIR_RX) {
+		tspi->rx_dma_req.size = len;
+		ret = tegra_dma_enqueue_req(tspi->rx_dma, &tspi->rx_dma_req);
+		if (ret < 0) {
+			dev_err(&tspi->pdev->dev, "Error in starting rx dma "
+						" error = %d\n", ret);
+			if (tspi->cur_direction & DATA_DIR_TX)
+				cancel_dma(tspi->tx_dma, &tspi->tx_dma_req);
+			return ret;
+		}
+	}
+	tspi->is_curr_dma_xfer = true;
+	if (tspi->is_packed) {
+		val |= SLINK_PACKED;
+		spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+		udelay(1);
+		wmb();
+	}
+
+	val |= SLINK_DMA_EN;
+	spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+	return ret;
+}
+
+static int spi_tegra_start_cpu_based_transfer(
+		struct spi_tegra_data *tspi, struct spi_transfer *t)
+{
+	unsigned long val;
+	unsigned curr_words;
+
+	val = tspi->packed_size;
+	if (tspi->cur_direction & DATA_DIR_TX)
+		val |= SLINK_IE_TXC;
+
+	if (tspi->cur_direction & DATA_DIR_RX)
+		val |= SLINK_IE_RXC;
+
+	spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+	tspi->dma_control_reg = val;
+
+	if (tspi->cur_direction & DATA_DIR_TX)
+		curr_words = spi_tegra_fill_tx_fifo_from_client_txbuf(tspi, t);
+	else
+		curr_words = tspi->curr_dma_words;
+	val |= SLINK_DMA_BLOCK_SIZE(curr_words - 1);
+	spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+	tspi->dma_control_reg = val;
+
+	tspi->is_curr_dma_xfer = false;
+	if (tspi->is_packed) {
+		val |= SLINK_PACKED;
+		spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+		udelay(1);
+		wmb();
+	}
+	val |= SLINK_DMA_EN;
+	spi_tegra_writel(tspi, val, SLINK_DMA_CTL);
+	return 0;
+}
+
+static void set_best_clk_source(struct spi_tegra_data *tspi,
+		unsigned long speed)
+{
+	long new_rate;
+	unsigned long err_rate;
+	int rate = speed * 4;
+	unsigned int fin_err = speed * 4;
+	int final_index = -1;
+	int count;
+	int ret;
+	struct clk *pclk;
+	unsigned long prate, crate, nrate;
+	unsigned long cdiv;
+
+	if (!tspi->parent_clk_count || !tspi->parent_clk_list)
+		return;
+
+	/* make sure divisor is more than min_div */
+	pclk = clk_get_parent(tspi->clk);
+	prate = clk_get_rate(pclk);
+	crate = clk_get_rate(tspi->clk);
+	cdiv = DIV_ROUND_UP(prate, crate);
+	if (cdiv < tspi->min_div) {
+		nrate = DIV_ROUND_UP(prate, tspi->min_div);
+		clk_set_rate(tspi->clk, nrate);
+	}
+
+	for (count = 0; count < tspi->parent_clk_count; ++count) {
+		if (!tspi->parent_clk_list[count].parent_clk)
+			continue;
+		ret = clk_set_parent(tspi->clk,
+			tspi->parent_clk_list[count].parent_clk);
+		if (ret < 0) {
+			dev_warn(&tspi->pdev->dev, "Error in setting parent "
+				" clk src %s\n",
+				tspi->parent_clk_list[count].name);
+			continue;
+		}
+
+		new_rate = clk_round_rate(tspi->clk, rate);
+		if (new_rate < 0)
+			continue;
+
+		err_rate = abs(new_rate - rate);
+		if (err_rate < fin_err) {
+			final_index = count;
+			fin_err = err_rate;
+		}
+	}
+
+	if (final_index >= 0) {
+		dev_info(&tspi->pdev->dev, "Setting clk_src %s\n",
+				tspi->parent_clk_list[final_index].name);
+		clk_set_parent(tspi->clk,
+			tspi->parent_clk_list[final_index].parent_clk);
+	}
+}
+
+static void spi_tegra_start_transfer(struct spi_device *spi,
+		    struct spi_transfer *t)
+{
+	struct spi_tegra_data *tspi = spi_master_get_devdata(spi->master);
+	u32 speed;
+	u8 bits_per_word;
+	unsigned total_fifo_words;
+	int ret;
+	unsigned long command;
+	unsigned long command2;
+
+	bits_per_word = t->bits_per_word ? t->bits_per_word :
+					spi->bits_per_word;
+
+	speed = t->speed_hz ? t->speed_hz : spi->max_speed_hz;
+	if (speed != tspi->cur_speed) {
+		set_best_clk_source(tspi, speed);
+		clk_set_rate(tspi->clk, speed * 4);
+		tspi->cur_speed = speed;
+	}
+
+	tspi->cur = t;
+	tspi->cur_spi = spi;
+	tspi->cur_pos = 0;
+	tspi->cur_rx_pos = 0;
+	tspi->cur_tx_pos = 0;
+	tspi->rx_complete = 0;
+	tspi->tx_complete = 0;
+	total_fifo_words = spi_tegra_calculate_curr_xfer_param(spi, tspi, t);
+
+	command2 = tspi->def_command2_reg;
+	if (!tspi->is_clkon_always) {
+		if (!tspi->clk_state) {
+			clk_enable(tspi->clk);
+			tspi->clk_state = 1;
+		}
+	}
+
+	spi_tegra_clear_status(tspi);
+
+	command = tspi->def_command_reg;
+	command |= SLINK_BIT_LENGTH(bits_per_word - 1);
+
+	command |= SLINK_CS_SW;
+
+	command &= ~SLINK_IDLE_SCLK_MASK & ~SLINK_CK_SDA;
+	if (spi->mode & SPI_CPHA)
+		command |= SLINK_CK_SDA;
+
+	if (spi->mode & SPI_CPOL)
+		command |= SLINK_IDLE_SCLK_DRIVE_HIGH;
+	else
+		command |= SLINK_IDLE_SCLK_DRIVE_LOW;
+
+	spi_tegra_writel(tspi, command, SLINK_COMMAND);
+	tspi->command_reg = command;
+
+	dev_dbg(&tspi->pdev->dev, "The def 0x%x and written 0x%lx\n",
+				tspi->def_command_reg, command);
+
+	command2 &= ~(SLINK_SS_EN_CS(~0) | SLINK_RXEN | SLINK_TXEN);
+	tspi->cur_direction = 0;
+	if (t->rx_buf) {
+		command2 |= SLINK_RXEN;
+		tspi->cur_direction |= DATA_DIR_RX;
+	}
+	if (t->tx_buf) {
+		command2 |= SLINK_TXEN;
+		tspi->cur_direction |= DATA_DIR_TX;
+	}
+	command2 |= SLINK_SS_EN_CS(spi->chip_select);
+	spi_tegra_writel(tspi, command2, SLINK_COMMAND2);
+	tspi->command2_reg = command2;
+
+	if (total_fifo_words > SPI_FIFO_DEPTH)
+		ret = spi_tegra_start_dma_based_transfer(tspi, t);
+	else
+		ret = spi_tegra_start_cpu_based_transfer(tspi, t);
+	WARN_ON(ret < 0);
+
+	if (tspi->client_slave_ready_cb)
+		tspi->client_slave_ready_cb(tspi->client_data);
+}
+
+static void spi_tegra_start_message(struct spi_device *spi,
+				    struct spi_message *m)
+{
+	struct spi_transfer *t;
+	m->actual_length = 0;
+	m->status = 0;
+	t = list_first_entry(&m->transfers, struct spi_transfer, transfer_list);
+	spi_tegra_start_transfer(spi, t);
+}
+
+static int spi_tegra_setup(struct spi_device *spi)
+{
+	struct spi_tegra_data *tspi = spi_master_get_devdata(spi->master);
+	unsigned long cs_bit;
+	unsigned long val;
+	unsigned long flags;
+
+	dev_dbg(&spi->dev, "setup %d bpw, %scpol, %scpha, %dHz\n",
+		spi->bits_per_word,
+		spi->mode & SPI_CPOL ? "" : "~",
+		spi->mode & SPI_CPHA ? "" : "~",
+		spi->max_speed_hz);
+
+	BUG_ON(spi->chip_select >= MAX_CHIP_SELECT);
+	switch (spi->chip_select) {
+	case 0:
+		cs_bit = SLINK_CS_POLARITY;
+		break;
+
+	case 1:
+		cs_bit = SLINK_CS_POLARITY1;
+		break;
+
+	case 2:
+		cs_bit = SLINK_CS_POLARITY2;
+		break;
+
+	case 3:
+		cs_bit = SLINK_CS_POLARITY3;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&tspi->lock, flags);
+	val = tspi->def_command_reg;
+	if (spi->mode & SPI_CS_HIGH)
+		val |= cs_bit;
+	else
+		val &= ~cs_bit;
+	tspi->def_command_reg = val;
+
+	if (!tspi->is_clkon_always && !tspi->clk_state) {
+		clk_enable(tspi->clk);
+		tspi->clk_state = 1;
+	}
+	spi_tegra_writel(tspi, tspi->def_command_reg, SLINK_COMMAND);
+	if (!tspi->is_clkon_always && tspi->clk_state) {
+		clk_disable(tspi->clk);
+		tspi->clk_state = 0;
+	}
+
+	spin_unlock_irqrestore(&tspi->lock, flags);
+	return 0;
+}
+
+static int spi_tegra_transfer(struct spi_device *spi, struct spi_message *m)
+{
+	struct spi_tegra_data *tspi = spi_master_get_devdata(spi->master);
+	struct spi_transfer *t;
+	unsigned long flags;
+	int was_empty;
+	int bytes_per_word;
+	u8 bits_per_word;
+	int fifo_word;
+
+	/* Support only one transfer per message */
+	if (!list_is_singular(&m->transfers))
+		return -EINVAL;
+
+	if (list_empty(&m->transfers) || !m->complete)
+		return -EINVAL;
+
+	t = list_first_entry(&m->transfers, struct spi_transfer, transfer_list);
+	if (t->bits_per_word < 0 || t->bits_per_word > 32)
+		return -EINVAL;
+
+	if (t->len == 0)
+		return -EINVAL;
+
+	bits_per_word = (t->bits_per_word) ? : spi->bits_per_word;
+
+	/* Check that the all words are available */
+	bytes_per_word = (bits_per_word + 7)/8;
+
+	if (t->len % bytes_per_word != 0)
+		return -EINVAL;
+
+	if (!t->rx_buf && !t->tx_buf)
+		return -EINVAL;
+
+	if ((bits_per_word == 8) || (bits_per_word == 16))
+		fifo_word = t->len/4;
+	else
+		fifo_word = t->len/bytes_per_word;
+	if (fifo_word >= tspi->max_buf_size/4)
+		return -EINVAL;
+
+	spin_lock_irqsave(&tspi->lock, flags);
+
+	if (WARN_ON(tspi->is_suspended)) {
+		spin_unlock_irqrestore(&tspi->lock, flags);
+		return -EBUSY;
+	}
+
+	m->state = spi;
+
+	was_empty = list_empty(&tspi->queue);
+	list_add_tail(&m->queue, &tspi->queue);
+
+	if (was_empty)
+		spi_tegra_start_message(spi, m);
+
+	spin_unlock_irqrestore(&tspi->lock, flags);
+
+	return 0;
+}
+
+static void spi_tegra_curr_transfer_complete(struct spi_tegra_data *tspi,
+	unsigned err, unsigned cur_xfer_size)
+{
+	struct spi_message *m;
+	struct spi_device *spi;
+
+	m = list_first_entry(&tspi->queue, struct spi_message, queue);
+	if (err)
+		m->status = -EIO;
+	spi = m->state;
+
+	m->actual_length += cur_xfer_size;
+	list_del(&m->queue);
+	m->complete(m->context);
+	if (!list_empty(&tspi->queue)) {
+		m = list_first_entry(&tspi->queue, struct spi_message, queue);
+		spi = m->state;
+		spi_tegra_start_message(spi, m);
+	} else {
+		spi_tegra_writel(tspi, tspi->def_command_reg, SLINK_COMMAND);
+		spi_tegra_writel(tspi, tspi->def_command2_reg, SLINK_COMMAND2);
+		if (!tspi->is_clkon_always) {
+			if (tspi->clk_state) {
+				/* Provide delay to stablize the signal
+				   state */
+				udelay(10);
+				clk_disable(tspi->clk);
+				tspi->clk_state = 0;
+			}
+		}
+	}
+}
+
+static void tegra_spi_tx_dma_complete(struct tegra_dma_req *req)
+{
+	struct spi_tegra_data *tspi = req->dev;
+	complete(&tspi->tx_dma_complete);
+}
+
+static void tegra_spi_rx_dma_complete(struct tegra_dma_req *req)
+{
+	struct spi_tegra_data *tspi = req->dev;
+	complete(&tspi->rx_dma_complete);
+}
+
+static void handle_cpu_based_xfer(void *context_data)
+{
+	struct spi_tegra_data *tspi = context_data;
+	struct spi_transfer *t = tspi->cur;
+	unsigned long flags;
+
+	spin_lock_irqsave(&tspi->lock, flags);
+	if (tspi->tx_status ||  tspi->rx_status ||
+				(tspi->status_reg & SLINK_BSY)) {
+		dev_err(&tspi->pdev->dev, "%s ERROR bit set 0x%x\n",
+					 __func__, tspi->status_reg);
+		tegra_periph_reset_assert(tspi->clk);
+		udelay(2);
+		tegra_periph_reset_deassert(tspi->clk);
+		WARN_ON(1);
+		spi_tegra_curr_transfer_complete(tspi,
+				tspi->tx_status ||  tspi->rx_status, t->len);
+		goto exit;
+	}
+
+	dev_vdbg(&tspi->pdev->dev, " Current direction %x\n",
+						tspi->cur_direction);
+	if (tspi->cur_direction & DATA_DIR_RX)
+		spi_tegra_read_rx_fifo_to_client_rxbuf(tspi, t);
+
+	if (tspi->cur_direction & DATA_DIR_TX)
+		tspi->cur_pos = tspi->cur_tx_pos;
+	else if (tspi->cur_direction & DATA_DIR_RX)
+		tspi->cur_pos = tspi->cur_rx_pos;
+	else
+		WARN_ON(1);
+
+	dev_vdbg(&tspi->pdev->dev, "current position %d and length of the "
+				"transfer %d\n", tspi->cur_pos, t->len);
+	if (tspi->cur_pos == t->len) {
+		spi_tegra_curr_transfer_complete(tspi,
+			tspi->tx_status || tspi->rx_status, t->len);
+		goto exit;
+	}
+
+	/* There should not be remaining transfer */
+	BUG();
+exit:
+	spin_unlock_irqrestore(&tspi->lock, flags);
+	return;
+}
+
+static irqreturn_t spi_tegra_isr_thread(int irq, void *context_data)
+{
+	struct spi_tegra_data *tspi = context_data;
+	struct spi_transfer *t = tspi->cur;
+	long wait_status;
+	int err = 0;
+	unsigned long flags;
+
+	if (!tspi->is_curr_dma_xfer) {
+		handle_cpu_based_xfer(context_data);
+		return IRQ_HANDLED;
+	}
+
+	/* Abort dmas if any error */
+	if (tspi->cur_direction & DATA_DIR_TX) {
+		if (tspi->tx_status) {
+			cancel_dma(tspi->tx_dma, &tspi->tx_dma_req);
+			err += 1;
+		} else {
+			wait_status = wait_for_completion_interruptible_timeout(
+				&tspi->tx_dma_complete, SLINK_DMA_TIMEOUT);
+			if (wait_status <= 0) {
+				cancel_dma(tspi->tx_dma, &tspi->tx_dma_req);
+				dev_err(&tspi->pdev->dev, "Error in Dma Tx "
+							"transfer\n");
+				err += 1;
+			}
+		}
+	}
+
+	if (tspi->cur_direction & DATA_DIR_RX) {
+		if (tspi->rx_status) {
+			cancel_dma(tspi->rx_dma, &tspi->rx_dma_req);
+			err += 2;
+		} else {
+			wait_status = wait_for_completion_interruptible_timeout(
+				&tspi->rx_dma_complete, SLINK_DMA_TIMEOUT);
+			if (wait_status <= 0) {
+				cancel_dma(tspi->rx_dma, &tspi->rx_dma_req);
+				dev_err(&tspi->pdev->dev, "Error in Dma Rx "
+							"transfer\n");
+				err += 2;
+			}
+		}
+	}
+
+	spin_lock_irqsave(&tspi->lock, flags);
+	if (err) {
+		dev_err(&tspi->pdev->dev, "%s ERROR bit set 0x%x\n",
+					 __func__, tspi->status_reg);
+		tegra_periph_reset_assert(tspi->clk);
+		udelay(2);
+		tegra_periph_reset_deassert(tspi->clk);
+		WARN_ON(1);
+		spi_tegra_curr_transfer_complete(tspi, err, t->len);
+		spin_unlock_irqrestore(&tspi->lock, flags);
+		return IRQ_HANDLED;
+	}
+
+	if (tspi->cur_direction & DATA_DIR_RX)
+		spi_tegra_copy_spi_rxbuf_to_client_rxbuf(tspi, t);
+
+	if (tspi->cur_direction & DATA_DIR_TX)
+		tspi->cur_pos = tspi->cur_tx_pos;
+	else if (tspi->cur_direction & DATA_DIR_RX)
+		tspi->cur_pos = tspi->cur_rx_pos;
+	else
+		WARN_ON(1);
+
+	if (tspi->cur_pos == t->len) {
+		spi_tegra_curr_transfer_complete(tspi,
+			tspi->tx_status || tspi->rx_status, t->len);
+		spin_unlock_irqrestore(&tspi->lock, flags);
+		return IRQ_HANDLED;
+	}
+
+	spin_unlock_irqrestore(&tspi->lock, flags);
+
+	/* There should not be remaining transfer */
+	BUG();
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t spi_tegra_isr(int irq, void *context_data)
+{
+	struct spi_tegra_data *tspi = context_data;
+
+	tspi->status_reg = spi_tegra_readl(tspi, SLINK_STATUS);
+	if (tspi->cur_direction & DATA_DIR_TX)
+		tspi->tx_status = tspi->status_reg &
+					(SLINK_TX_OVF | SLINK_TX_UNF);
+
+	if (tspi->cur_direction & DATA_DIR_RX)
+		tspi->rx_status = tspi->status_reg &
+					(SLINK_RX_OVF | SLINK_RX_UNF);
+	spi_tegra_clear_status(tspi);
+
+
+	return IRQ_WAKE_THREAD;
+}
+
+static int __init spi_tegra_probe(struct platform_device *pdev)
+{
+	struct spi_master	*master;
+	struct spi_tegra_data	*tspi;
+	struct resource		*r;
+	struct tegra_spi_platform_data *pdata = pdev->dev.platform_data;
+	int ret;
+	int i;
+
+	master = spi_alloc_master(&pdev->dev, sizeof *tspi);
+	if (master == NULL) {
+		dev_err(&pdev->dev, "master allocation failed\n");
+		return -ENOMEM;
+	}
+
+	/* the spi->mode bits understood by this driver: */
+	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_CS_HIGH;
+
+	if (pdev->id != -1)
+		master->bus_num = pdev->id;
+
+	master->setup = spi_tegra_setup;
+	master->transfer = spi_tegra_transfer;
+	master->num_chipselect = MAX_CHIP_SELECT;
+
+	dev_set_drvdata(&pdev->dev, master);
+	tspi = spi_master_get_devdata(master);
+	tspi->master = master;
+	tspi->pdev = pdev;
+	spin_lock_init(&tspi->lock);
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (r == NULL) {
+		ret = -ENODEV;
+		goto fail_no_mem;
+	}
+
+	if (!request_mem_region(r->start, (r->end - r->start) + 1,
+				dev_name(&pdev->dev))) {
+		ret = -EBUSY;
+		goto fail_no_mem;
+	}
+
+	tspi->phys = r->start;
+	tspi->base = ioremap(r->start, r->end - r->start + 1);
+	if (!tspi->base) {
+		dev_err(&pdev->dev, "can't ioremap iomem\n");
+		ret = -ENOMEM;
+		goto fail_io_map;
+	}
+
+	tspi->irq = platform_get_irq(pdev, 0);
+	if (unlikely(tspi->irq < 0)) {
+		dev_err(&pdev->dev, "can't find irq resource\n");
+		ret = -ENXIO;
+		goto fail_irq_req;
+	}
+
+	sprintf(tspi->port_name, "tegra_spi_%d", pdev->id);
+	ret = request_threaded_irq(tspi->irq, spi_tegra_isr,
+			spi_tegra_isr_thread, IRQF_DISABLED,
+			tspi->port_name, tspi);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to register ISR for IRQ %d\n",
+					tspi->irq);
+		goto fail_irq_req;
+	}
+
+	tspi->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR_OR_NULL(tspi->clk)) {
+		dev_err(&pdev->dev, "can not get clock\n");
+		ret = PTR_ERR(tspi->clk);
+		goto fail_clk_get;
+	}
+
+	INIT_LIST_HEAD(&tspi->queue);
+
+	if (pdata) {
+		tspi->is_clkon_always = pdata->is_clkon_always;
+		tspi->is_dma_allowed = pdata->is_dma_based;
+		tspi->dma_buf_size = (pdata->max_dma_buffer) ?
+				pdata->max_dma_buffer : DEFAULT_SPI_DMA_BUF_LEN;
+		tspi->parent_clk_count = pdata->parent_clk_count;
+		tspi->parent_clk_list = pdata->parent_clk_list;
+		tspi->max_rate = pdata->max_rate;
+	} else {
+		tspi->is_clkon_always = false;
+		tspi->is_dma_allowed = true;
+		tspi->dma_buf_size = DEFAULT_SPI_DMA_BUF_LEN;
+		tspi->parent_clk_count = 0;
+		tspi->parent_clk_list = NULL;
+		tspi->max_rate = 0;
+	}
+
+	tspi->max_parent_rate = 0;
+	tspi->min_div = 0;
+
+	if (tspi->parent_clk_count) {
+		tspi->max_parent_rate = tspi->parent_clk_list[0].fixed_clk_rate;
+		for (i = 1; i < tspi->parent_clk_count; ++i) {
+			tspi->max_parent_rate = max(tspi->max_parent_rate,
+				tspi->parent_clk_list[i].fixed_clk_rate);
+		}
+		if (tspi->max_rate)
+			tspi->min_div = DIV_ROUND_UP(tspi->max_parent_rate,
+						tspi->max_rate);
+	}
+	tspi->max_buf_size = SLINK_FIFO_DEPTH << 2;
+
+	if (!tspi->is_dma_allowed)
+		goto skip_dma_alloc;
+
+	init_completion(&tspi->tx_dma_complete);
+	init_completion(&tspi->rx_dma_complete);
+
+
+	tspi->rx_dma = tegra_dma_allocate_channel(TEGRA_DMA_MODE_ONESHOT,
+				"spi_rx_%d", pdev->id);
+	if (!tspi->rx_dma) {
+		dev_err(&pdev->dev, "can not allocate rx dma channel\n");
+		ret = -ENODEV;
+		goto fail_rx_dma_alloc;
+	}
+
+	tspi->rx_buf = dma_alloc_coherent(&pdev->dev, tspi->dma_buf_size,
+					 &tspi->rx_buf_phys, GFP_KERNEL);
+	if (!tspi->rx_buf) {
+		dev_err(&pdev->dev, "can not allocate rx bounce buffer\n");
+		ret = -ENOMEM;
+		goto fail_rx_buf_alloc;
+	}
+
+	memset(&tspi->rx_dma_req, 0, sizeof(struct tegra_dma_req));
+	tspi->rx_dma_req.complete = tegra_spi_rx_dma_complete;
+	tspi->rx_dma_req.to_memory = 1;
+	tspi->rx_dma_req.dest_addr = tspi->rx_buf_phys;
+	tspi->rx_dma_req.virt_addr = tspi->rx_buf;
+	tspi->rx_dma_req.dest_bus_width = 32;
+	tspi->rx_dma_req.source_addr = tspi->phys + SLINK_RX_FIFO;
+	tspi->rx_dma_req.source_bus_width = 32;
+	tspi->rx_dma_req.source_wrap = 4;
+	tspi->rx_dma_req.dest_wrap = 0;
+	tspi->rx_dma_req.req_sel = spi_tegra_req_sels[pdev->id];
+	tspi->rx_dma_req.dev = tspi;
+
+	tspi->tx_dma = tegra_dma_allocate_channel(TEGRA_DMA_MODE_ONESHOT,
+				"spi_tx_%d", pdev->id);
+	if (!tspi->tx_dma) {
+		dev_err(&pdev->dev, "can not allocate tx dma channel\n");
+		ret = -ENODEV;
+		goto fail_tx_dma_alloc;
+	}
+
+	tspi->tx_buf = dma_alloc_coherent(&pdev->dev, tspi->dma_buf_size,
+					 &tspi->tx_buf_phys, GFP_KERNEL);
+	if (!tspi->tx_buf) {
+		dev_err(&pdev->dev, "can not allocate tx bounce buffer\n");
+		ret = -ENOMEM;
+		goto fail_tx_buf_alloc;
+	}
+
+	memset(&tspi->tx_dma_req, 0, sizeof(struct tegra_dma_req));
+	tspi->tx_dma_req.complete = tegra_spi_tx_dma_complete;
+	tspi->tx_dma_req.to_memory = 0;
+	tspi->tx_dma_req.dest_addr = tspi->phys + SLINK_TX_FIFO;
+	tspi->tx_dma_req.virt_addr = tspi->tx_buf;
+	tspi->tx_dma_req.dest_bus_width = 32;
+	tspi->tx_dma_req.dest_wrap = 4;
+	tspi->tx_dma_req.source_wrap = 0;
+	tspi->tx_dma_req.source_addr = tspi->tx_buf_phys;
+	tspi->tx_dma_req.source_bus_width = 32;
+	tspi->tx_dma_req.req_sel = spi_tegra_req_sels[pdev->id];
+	tspi->tx_dma_req.dev = tspi;
+	tspi->max_buf_size = tspi->dma_buf_size;
+	tspi->def_command_reg  = SLINK_CS_SW;
+	tspi->def_command2_reg = SLINK_CS_ACTIVE_BETWEEN;
+
+skip_dma_alloc:
+	clk_enable(tspi->clk);
+	tspi->clk_state = 1;
+	spi_tegra_writel(tspi, tspi->def_command2_reg, SLINK_COMMAND2);
+	ret = spi_register_master(master);
+	if (!tspi->is_clkon_always) {
+		if (tspi->clk_state) {
+			clk_disable(tspi->clk);
+			tspi->clk_state = 0;
+		}
+	}
+
+	if (ret < 0) {
+		dev_err(&pdev->dev, "can not register to master err %d\n", ret);
+		goto fail_master_register;
+	}
+	return ret;
+
+fail_master_register:
+	if (tspi->tx_buf)
+		dma_free_coherent(&pdev->dev, tspi->dma_buf_size,
+				tspi->tx_buf, tspi->tx_buf_phys);
+fail_tx_buf_alloc:
+	if (tspi->tx_dma)
+		tegra_dma_free_channel(tspi->tx_dma);
+fail_tx_dma_alloc:
+	if (tspi->rx_buf)
+		dma_free_coherent(&pdev->dev, tspi->dma_buf_size,
+			  tspi->rx_buf, tspi->rx_buf_phys);
+fail_rx_buf_alloc:
+	if (tspi->rx_dma)
+		tegra_dma_free_channel(tspi->rx_dma);
+fail_rx_dma_alloc:
+	clk_put(tspi->clk);
+fail_clk_get:
+	free_irq(tspi->irq, tspi);
+fail_irq_req:
+	iounmap(tspi->base);
+fail_io_map:
+	release_mem_region(r->start, (r->end - r->start) + 1);
+fail_no_mem:
+	spi_master_put(master);
+	return ret;
+}
+
+static int __devexit spi_tegra_remove(struct platform_device *pdev)
+{
+	struct spi_master	*master;
+	struct spi_tegra_data	*tspi;
+	struct resource		*r;
+
+	master = dev_get_drvdata(&pdev->dev);
+	tspi = spi_master_get_devdata(master);
+
+	if (tspi->tx_buf)
+		dma_free_coherent(&pdev->dev, tspi->dma_buf_size,
+				tspi->tx_buf, tspi->tx_buf_phys);
+	if (tspi->tx_dma)
+		tegra_dma_free_channel(tspi->tx_dma);
+	if (tspi->rx_buf)
+		dma_free_coherent(&pdev->dev, tspi->dma_buf_size,
+			  tspi->rx_buf, tspi->rx_buf_phys);
+	if (tspi->rx_dma)
+		tegra_dma_free_channel(tspi->rx_dma);
+
+	if (tspi->is_clkon_always) {
+		clk_disable(tspi->clk);
+		tspi->clk_state = 0;
+	}
+
+	clk_put(tspi->clk);
+	iounmap(tspi->base);
+
+	spi_master_put(master);
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	release_mem_region(r->start, (r->end - r->start) + 1);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int spi_tegra_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct spi_master	*master;
+	struct spi_tegra_data	*tspi;
+	unsigned long		flags;
+	unsigned		limit = 50;
+
+	master = dev_get_drvdata(&pdev->dev);
+	tspi = spi_master_get_devdata(master);
+	spin_lock_irqsave(&tspi->lock, flags);
+	tspi->is_suspended = true;
+
+	WARN_ON(!list_empty(&tspi->queue));
+
+	while (!list_empty(&tspi->queue) && limit--) {
+		spin_unlock_irqrestore(&tspi->lock, flags);
+		msleep(20);
+		spin_lock_irqsave(&tspi->lock, flags);
+	}
+
+	spin_unlock_irqrestore(&tspi->lock, flags);
+	if (tspi->is_clkon_always) {
+		clk_disable(tspi->clk);
+		tspi->clk_state = 0;
+	}
+	return 0;
+}
+
+static int spi_tegra_resume(struct platform_device *pdev)
+{
+	struct spi_master	*master;
+	struct spi_tegra_data	*tspi;
+	unsigned long		flags;
+
+	master = dev_get_drvdata(&pdev->dev);
+	tspi = spi_master_get_devdata(master);
+
+	spin_lock_irqsave(&tspi->lock, flags);
+	clk_enable(tspi->clk);
+	tspi->clk_state = 1;
+	spi_tegra_writel(tspi, tspi->command_reg, SLINK_COMMAND);
+	if (!tspi->is_clkon_always) {
+		clk_disable(tspi->clk);
+		tspi->clk_state = 0;
+	}
+
+	tspi->cur_speed = 0;
+	tspi->is_suspended = false;
+	spin_unlock_irqrestore(&tspi->lock, flags);
+	return 0;
+}
+#endif
+
+MODULE_ALIAS("platform:spi_slave_tegra");
+
+static struct platform_driver spi_tegra_driver = {
+	.driver = {
+		.name =		"spi_slave_tegra",
+		.owner =	THIS_MODULE,
+	},
+	.remove =	__devexit_p(spi_tegra_remove),
+#ifdef CONFIG_PM
+	.suspend =	spi_tegra_suspend,
+	.resume  =	spi_tegra_resume,
+#endif
+};
+
+static int __init spi_tegra_init(void)
+{
+	return platform_driver_probe(&spi_tegra_driver, spi_tegra_probe);
+}
+subsys_initcall(spi_tegra_init);
+
+static void __exit spi_tegra_exit(void)
+{
+	platform_driver_unregister(&spi_tegra_driver);
+}
+module_exit(spi_tegra_exit);
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/ssb/driver_pcicore.c b/drivers/ssb/driver_pcicore.c
index e6ac3177fbbe..32c535f5ce31 100644
--- a/drivers/ssb/driver_pcicore.c
+++ b/drivers/ssb/driver_pcicore.c
@@ -516,10 +516,14 @@ static void ssb_pcicore_pcie_setup_workarounds(struct ssb_pcicore *pc)
 
 static void __devinit ssb_pcicore_init_clientmode(struct ssb_pcicore *pc)
 {
-	ssb_pcicore_fix_sprom_core_index(pc);
+	struct ssb_device *pdev = pc->dev;
+	struct ssb_bus *bus = pdev->bus;
+
+	if (bus->bustype == SSB_BUSTYPE_PCI)
+		ssb_pcicore_fix_sprom_core_index(pc);
 
 	/* Disable PCI interrupts. */
-	ssb_write32(pc->dev, SSB_INTVEC, 0);
+	ssb_write32(pdev, SSB_INTVEC, 0);
 
 	/* Additional PCIe always once-executed workarounds */
 	if (pc->dev->id.coreid == SSB_DEV_PCIE) {
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 06c9081d596d..8541202a32e4 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -66,6 +66,8 @@ source "drivers/staging/rts_pstor/Kconfig"
 
 source "drivers/staging/frontier/Kconfig"
 
+source "drivers/staging/android/Kconfig"
+
 source "drivers/staging/pohmelfs/Kconfig"
 
 source "drivers/staging/phison/Kconfig"
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index f3c5e33bb263..c491642760f4 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -27,6 +27,7 @@ obj-$(CONFIG_R8712U)		+= rtl8712/
 obj-$(CONFIG_RTS_PSTOR)		+= rts_pstor/
 obj-$(CONFIG_SPECTRA)		+= spectra/
 obj-$(CONFIG_TRANZPORT)		+= frontier/
+obj-$(CONFIG_ANDROID)		+= android/
 obj-$(CONFIG_POHMELFS)		+= pohmelfs/
 obj-$(CONFIG_IDE_PHISON)	+= phison/
 obj-$(CONFIG_LINE6_USB)		+= line6/
diff --git a/drivers/staging/android/Kconfig b/drivers/staging/android/Kconfig
new file mode 100644
index 000000000000..247194992374
--- /dev/null
+++ b/drivers/staging/android/Kconfig
@@ -0,0 +1,95 @@
+menu "Android"
+
+config ANDROID
+	bool "Android Drivers"
+	default N
+	---help---
+	  Enable support for various drivers needed on the Android platform
+
+if ANDROID
+
+config ANDROID_BINDER_IPC
+	bool "Android Binder IPC Driver"
+	default n
+
+config ANDROID_LOGGER
+	tristate "Android log driver"
+	default n
+
+config ANDROID_RAM_CONSOLE
+	bool "Android RAM buffer console"
+	default n
+
+config ANDROID_RAM_CONSOLE_ENABLE_VERBOSE
+	bool "Enable verbose console messages on Android RAM console"
+	default y
+	depends on ANDROID_RAM_CONSOLE
+
+menuconfig ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	bool "Android RAM Console Enable error correction"
+	default n
+	depends on ANDROID_RAM_CONSOLE
+	depends on !ANDROID_RAM_CONSOLE_EARLY_INIT
+	select REED_SOLOMON
+	select REED_SOLOMON_ENC8
+	select REED_SOLOMON_DEC8
+
+if ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_DATA_SIZE
+	int "Android RAM Console Data data size"
+	default 128
+	help
+	  Must be a power of 2.
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_ECC_SIZE
+	int "Android RAM Console ECC size"
+	default 16
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE
+	int "Android RAM Console Symbol size"
+	default 8
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_POLYNOMIAL
+	hex "Android RAM Console Polynomial"
+	default 0x19 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 4)
+	default 0x29 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 5)
+	default 0x61 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 6)
+	default 0x89 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 7)
+	default 0x11d if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 8)
+
+endif # ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+
+config ANDROID_RAM_CONSOLE_EARLY_INIT
+	bool "Start Android RAM console early"
+	default n
+	depends on ANDROID_RAM_CONSOLE
+
+config ANDROID_RAM_CONSOLE_EARLY_ADDR
+	hex "Android RAM console virtual address"
+	default 0
+	depends on ANDROID_RAM_CONSOLE_EARLY_INIT
+
+config ANDROID_RAM_CONSOLE_EARLY_SIZE
+	hex "Android RAM console buffer size"
+	default 0
+	depends on ANDROID_RAM_CONSOLE_EARLY_INIT
+
+config ANDROID_TIMED_OUTPUT
+	bool "Timed output class driver"
+	default y
+
+config ANDROID_TIMED_GPIO
+	tristate "Android timed gpio driver"
+	depends on GENERIC_GPIO && ANDROID_TIMED_OUTPUT
+	default n
+
+config ANDROID_LOW_MEMORY_KILLER
+	bool "Android Low Memory Killer"
+	default N
+	---help---
+	  Register processes to be killed when memory is low
+
+endif # if ANDROID
+
+endmenu
diff --git a/drivers/staging/android/Makefile b/drivers/staging/android/Makefile
new file mode 100644
index 000000000000..8e057e626d11
--- /dev/null
+++ b/drivers/staging/android/Makefile
@@ -0,0 +1,6 @@
+obj-$(CONFIG_ANDROID_BINDER_IPC)	+= binder.o
+obj-$(CONFIG_ANDROID_LOGGER)		+= logger.o
+obj-$(CONFIG_ANDROID_RAM_CONSOLE)	+= ram_console.o
+obj-$(CONFIG_ANDROID_TIMED_OUTPUT)	+= timed_output.o
+obj-$(CONFIG_ANDROID_TIMED_GPIO)	+= timed_gpio.o
+obj-$(CONFIG_ANDROID_LOW_MEMORY_KILLER)	+= lowmemorykiller.o
diff --git a/drivers/staging/android/binder.c b/drivers/staging/android/binder.c
new file mode 100644
index 000000000000..e13b4c483407
--- /dev/null
+++ b/drivers/staging/android/binder.c
@@ -0,0 +1,3600 @@
+/* binder.c
+ *
+ * Android IPC Subsystem
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <asm/cacheflush.h>
+#include <linux/fdtable.h>
+#include <linux/file.h>
+#include <linux/fs.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/nsproxy.h>
+#include <linux/poll.h>
+#include <linux/debugfs.h>
+#include <linux/rbtree.h>
+#include <linux/sched.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/vmalloc.h>
+
+#include "binder.h"
+
+static DEFINE_MUTEX(binder_lock);
+static DEFINE_MUTEX(binder_deferred_lock);
+
+static HLIST_HEAD(binder_procs);
+static HLIST_HEAD(binder_deferred_list);
+static HLIST_HEAD(binder_dead_nodes);
+
+static struct dentry *binder_debugfs_dir_entry_root;
+static struct dentry *binder_debugfs_dir_entry_proc;
+static struct binder_node *binder_context_mgr_node;
+static uid_t binder_context_mgr_uid = -1;
+static int binder_last_id;
+static struct workqueue_struct *binder_deferred_workqueue;
+
+#define BINDER_DEBUG_ENTRY(name) \
+static int binder_##name##_open(struct inode *inode, struct file *file) \
+{ \
+	return single_open(file, binder_##name##_show, inode->i_private); \
+} \
+\
+static const struct file_operations binder_##name##_fops = { \
+	.owner = THIS_MODULE, \
+	.open = binder_##name##_open, \
+	.read = seq_read, \
+	.llseek = seq_lseek, \
+	.release = single_release, \
+}
+
+static int binder_proc_show(struct seq_file *m, void *unused);
+BINDER_DEBUG_ENTRY(proc);
+
+/* This is only defined in include/asm-arm/sizes.h */
+#ifndef SZ_1K
+#define SZ_1K                               0x400
+#endif
+
+#ifndef SZ_4M
+#define SZ_4M                               0x400000
+#endif
+
+#define FORBIDDEN_MMAP_FLAGS                (VM_WRITE)
+
+#define BINDER_SMALL_BUF_SIZE (PAGE_SIZE * 64)
+
+enum {
+	BINDER_DEBUG_USER_ERROR             = 1U << 0,
+	BINDER_DEBUG_FAILED_TRANSACTION     = 1U << 1,
+	BINDER_DEBUG_DEAD_TRANSACTION       = 1U << 2,
+	BINDER_DEBUG_OPEN_CLOSE             = 1U << 3,
+	BINDER_DEBUG_DEAD_BINDER            = 1U << 4,
+	BINDER_DEBUG_DEATH_NOTIFICATION     = 1U << 5,
+	BINDER_DEBUG_READ_WRITE             = 1U << 6,
+	BINDER_DEBUG_USER_REFS              = 1U << 7,
+	BINDER_DEBUG_THREADS                = 1U << 8,
+	BINDER_DEBUG_TRANSACTION            = 1U << 9,
+	BINDER_DEBUG_TRANSACTION_COMPLETE   = 1U << 10,
+	BINDER_DEBUG_FREE_BUFFER            = 1U << 11,
+	BINDER_DEBUG_INTERNAL_REFS          = 1U << 12,
+	BINDER_DEBUG_BUFFER_ALLOC           = 1U << 13,
+	BINDER_DEBUG_PRIORITY_CAP           = 1U << 14,
+	BINDER_DEBUG_BUFFER_ALLOC_ASYNC     = 1U << 15,
+};
+static uint32_t binder_debug_mask = BINDER_DEBUG_USER_ERROR |
+	BINDER_DEBUG_FAILED_TRANSACTION | BINDER_DEBUG_DEAD_TRANSACTION;
+module_param_named(debug_mask, binder_debug_mask, uint, S_IWUSR | S_IRUGO);
+
+static int binder_debug_no_lock;
+module_param_named(proc_no_lock, binder_debug_no_lock, bool, S_IWUSR | S_IRUGO);
+
+static DECLARE_WAIT_QUEUE_HEAD(binder_user_error_wait);
+static int binder_stop_on_user_error;
+
+static int binder_set_stop_on_user_error(const char *val,
+					 struct kernel_param *kp)
+{
+	int ret;
+	ret = param_set_int(val, kp);
+	if (binder_stop_on_user_error < 2)
+		wake_up(&binder_user_error_wait);
+	return ret;
+}
+module_param_call(stop_on_user_error, binder_set_stop_on_user_error,
+	param_get_int, &binder_stop_on_user_error, S_IWUSR | S_IRUGO);
+
+#define binder_debug(mask, x...) \
+	do { \
+		if (binder_debug_mask & mask) \
+			printk(KERN_INFO x); \
+	} while (0)
+
+#define binder_user_error(x...) \
+	do { \
+		if (binder_debug_mask & BINDER_DEBUG_USER_ERROR) \
+			printk(KERN_INFO x); \
+		if (binder_stop_on_user_error) \
+			binder_stop_on_user_error = 2; \
+	} while (0)
+
+enum binder_stat_types {
+	BINDER_STAT_PROC,
+	BINDER_STAT_THREAD,
+	BINDER_STAT_NODE,
+	BINDER_STAT_REF,
+	BINDER_STAT_DEATH,
+	BINDER_STAT_TRANSACTION,
+	BINDER_STAT_TRANSACTION_COMPLETE,
+	BINDER_STAT_COUNT
+};
+
+struct binder_stats {
+	int br[_IOC_NR(BR_FAILED_REPLY) + 1];
+	int bc[_IOC_NR(BC_DEAD_BINDER_DONE) + 1];
+	int obj_created[BINDER_STAT_COUNT];
+	int obj_deleted[BINDER_STAT_COUNT];
+};
+
+static struct binder_stats binder_stats;
+
+static inline void binder_stats_deleted(enum binder_stat_types type)
+{
+	binder_stats.obj_deleted[type]++;
+}
+
+static inline void binder_stats_created(enum binder_stat_types type)
+{
+	binder_stats.obj_created[type]++;
+}
+
+struct binder_transaction_log_entry {
+	int debug_id;
+	int call_type;
+	int from_proc;
+	int from_thread;
+	int target_handle;
+	int to_proc;
+	int to_thread;
+	int to_node;
+	int data_size;
+	int offsets_size;
+};
+struct binder_transaction_log {
+	int next;
+	int full;
+	struct binder_transaction_log_entry entry[32];
+};
+static struct binder_transaction_log binder_transaction_log;
+static struct binder_transaction_log binder_transaction_log_failed;
+
+static struct binder_transaction_log_entry *binder_transaction_log_add(
+	struct binder_transaction_log *log)
+{
+	struct binder_transaction_log_entry *e;
+	e = &log->entry[log->next];
+	memset(e, 0, sizeof(*e));
+	log->next++;
+	if (log->next == ARRAY_SIZE(log->entry)) {
+		log->next = 0;
+		log->full = 1;
+	}
+	return e;
+}
+
+struct binder_work {
+	struct list_head entry;
+	enum {
+		BINDER_WORK_TRANSACTION = 1,
+		BINDER_WORK_TRANSACTION_COMPLETE,
+		BINDER_WORK_NODE,
+		BINDER_WORK_DEAD_BINDER,
+		BINDER_WORK_DEAD_BINDER_AND_CLEAR,
+		BINDER_WORK_CLEAR_DEATH_NOTIFICATION,
+	} type;
+};
+
+struct binder_node {
+	int debug_id;
+	struct binder_work work;
+	union {
+		struct rb_node rb_node;
+		struct hlist_node dead_node;
+	};
+	struct binder_proc *proc;
+	struct hlist_head refs;
+	int internal_strong_refs;
+	int local_weak_refs;
+	int local_strong_refs;
+	void __user *ptr;
+	void __user *cookie;
+	unsigned has_strong_ref:1;
+	unsigned pending_strong_ref:1;
+	unsigned has_weak_ref:1;
+	unsigned pending_weak_ref:1;
+	unsigned has_async_transaction:1;
+	unsigned accept_fds:1;
+	unsigned min_priority:8;
+	struct list_head async_todo;
+};
+
+struct binder_ref_death {
+	struct binder_work work;
+	void __user *cookie;
+};
+
+struct binder_ref {
+	/* Lookups needed: */
+	/*   node + proc => ref (transaction) */
+	/*   desc + proc => ref (transaction, inc/dec ref) */
+	/*   node => refs + procs (proc exit) */
+	int debug_id;
+	struct rb_node rb_node_desc;
+	struct rb_node rb_node_node;
+	struct hlist_node node_entry;
+	struct binder_proc *proc;
+	struct binder_node *node;
+	uint32_t desc;
+	int strong;
+	int weak;
+	struct binder_ref_death *death;
+};
+
+struct binder_buffer {
+	struct list_head entry; /* free and allocated entries by addesss */
+	struct rb_node rb_node; /* free entry by size or allocated entry */
+				/* by address */
+	unsigned free:1;
+	unsigned allow_user_free:1;
+	unsigned async_transaction:1;
+	unsigned debug_id:29;
+
+	struct binder_transaction *transaction;
+
+	struct binder_node *target_node;
+	size_t data_size;
+	size_t offsets_size;
+	uint8_t data[0];
+};
+
+enum binder_deferred_state {
+	BINDER_DEFERRED_PUT_FILES    = 0x01,
+	BINDER_DEFERRED_FLUSH        = 0x02,
+	BINDER_DEFERRED_RELEASE      = 0x04,
+};
+
+struct binder_proc {
+	struct hlist_node proc_node;
+	struct rb_root threads;
+	struct rb_root nodes;
+	struct rb_root refs_by_desc;
+	struct rb_root refs_by_node;
+	int pid;
+	struct vm_area_struct *vma;
+	struct task_struct *tsk;
+	struct files_struct *files;
+	struct hlist_node deferred_work_node;
+	int deferred_work;
+	void *buffer;
+	ptrdiff_t user_buffer_offset;
+
+	struct list_head buffers;
+	struct rb_root free_buffers;
+	struct rb_root allocated_buffers;
+	size_t free_async_space;
+
+	struct page **pages;
+	size_t buffer_size;
+	uint32_t buffer_free;
+	struct list_head todo;
+	wait_queue_head_t wait;
+	struct binder_stats stats;
+	struct list_head delivered_death;
+	int max_threads;
+	int requested_threads;
+	int requested_threads_started;
+	int ready_threads;
+	long default_priority;
+	struct dentry *debugfs_entry;
+};
+
+enum {
+	BINDER_LOOPER_STATE_REGISTERED  = 0x01,
+	BINDER_LOOPER_STATE_ENTERED     = 0x02,
+	BINDER_LOOPER_STATE_EXITED      = 0x04,
+	BINDER_LOOPER_STATE_INVALID     = 0x08,
+	BINDER_LOOPER_STATE_WAITING     = 0x10,
+	BINDER_LOOPER_STATE_NEED_RETURN = 0x20
+};
+
+struct binder_thread {
+	struct binder_proc *proc;
+	struct rb_node rb_node;
+	int pid;
+	int looper;
+	struct binder_transaction *transaction_stack;
+	struct list_head todo;
+	uint32_t return_error; /* Write failed, return error code in read buf */
+	uint32_t return_error2; /* Write failed, return error code in read */
+		/* buffer. Used when sending a reply to a dead process that */
+		/* we are also waiting on */
+	wait_queue_head_t wait;
+	struct binder_stats stats;
+};
+
+struct binder_transaction {
+	int debug_id;
+	struct binder_work work;
+	struct binder_thread *from;
+	struct binder_transaction *from_parent;
+	struct binder_proc *to_proc;
+	struct binder_thread *to_thread;
+	struct binder_transaction *to_parent;
+	unsigned need_reply:1;
+	/* unsigned is_dead:1; */	/* not used at the moment */
+
+	struct binder_buffer *buffer;
+	unsigned int	code;
+	unsigned int	flags;
+	long	priority;
+	long	saved_priority;
+	uid_t	sender_euid;
+};
+
+static void
+binder_defer_work(struct binder_proc *proc, enum binder_deferred_state defer);
+
+/*
+ * copied from get_unused_fd_flags
+ */
+int task_get_unused_fd_flags(struct binder_proc *proc, int flags)
+{
+	struct files_struct *files = proc->files;
+	int fd, error;
+	struct fdtable *fdt;
+	unsigned long rlim_cur;
+	unsigned long irqs;
+
+	if (files == NULL)
+		return -ESRCH;
+
+	error = -EMFILE;
+	spin_lock(&files->file_lock);
+
+repeat:
+	fdt = files_fdtable(files);
+	fd = find_next_zero_bit(fdt->open_fds->fds_bits, fdt->max_fds,
+				files->next_fd);
+
+	/*
+	 * N.B. For clone tasks sharing a files structure, this test
+	 * will limit the total number of files that can be opened.
+	 */
+	rlim_cur = 0;
+	if (lock_task_sighand(proc->tsk, &irqs)) {
+		rlim_cur = proc->tsk->signal->rlim[RLIMIT_NOFILE].rlim_cur;
+		unlock_task_sighand(proc->tsk, &irqs);
+	}
+	if (fd >= rlim_cur)
+		goto out;
+
+	/* Do we need to expand the fd array or fd set?  */
+	error = expand_files(files, fd);
+	if (error < 0)
+		goto out;
+
+	if (error) {
+		/*
+		 * If we needed to expand the fs array we
+		 * might have blocked - try again.
+		 */
+		error = -EMFILE;
+		goto repeat;
+	}
+
+	FD_SET(fd, fdt->open_fds);
+	if (flags & O_CLOEXEC)
+		FD_SET(fd, fdt->close_on_exec);
+	else
+		FD_CLR(fd, fdt->close_on_exec);
+	files->next_fd = fd + 1;
+#if 1
+	/* Sanity check */
+	if (fdt->fd[fd] != NULL) {
+		printk(KERN_WARNING "get_unused_fd: slot %d not NULL!\n", fd);
+		fdt->fd[fd] = NULL;
+	}
+#endif
+	error = fd;
+
+out:
+	spin_unlock(&files->file_lock);
+	return error;
+}
+
+/*
+ * copied from fd_install
+ */
+static void task_fd_install(
+	struct binder_proc *proc, unsigned int fd, struct file *file)
+{
+	struct files_struct *files = proc->files;
+	struct fdtable *fdt;
+
+	if (files == NULL)
+		return;
+
+	spin_lock(&files->file_lock);
+	fdt = files_fdtable(files);
+	BUG_ON(fdt->fd[fd] != NULL);
+	rcu_assign_pointer(fdt->fd[fd], file);
+	spin_unlock(&files->file_lock);
+}
+
+/*
+ * copied from __put_unused_fd in open.c
+ */
+static void __put_unused_fd(struct files_struct *files, unsigned int fd)
+{
+	struct fdtable *fdt = files_fdtable(files);
+	__FD_CLR(fd, fdt->open_fds);
+	if (fd < files->next_fd)
+		files->next_fd = fd;
+}
+
+/*
+ * copied from sys_close
+ */
+static long task_close_fd(struct binder_proc *proc, unsigned int fd)
+{
+	struct file *filp;
+	struct files_struct *files = proc->files;
+	struct fdtable *fdt;
+	int retval;
+
+	if (files == NULL)
+		return -ESRCH;
+
+	spin_lock(&files->file_lock);
+	fdt = files_fdtable(files);
+	if (fd >= fdt->max_fds)
+		goto out_unlock;
+	filp = fdt->fd[fd];
+	if (!filp)
+		goto out_unlock;
+	rcu_assign_pointer(fdt->fd[fd], NULL);
+	FD_CLR(fd, fdt->close_on_exec);
+	__put_unused_fd(files, fd);
+	spin_unlock(&files->file_lock);
+	retval = filp_close(filp, files);
+
+	/* can't restart close syscall because file table entry was cleared */
+	if (unlikely(retval == -ERESTARTSYS ||
+		     retval == -ERESTARTNOINTR ||
+		     retval == -ERESTARTNOHAND ||
+		     retval == -ERESTART_RESTARTBLOCK))
+		retval = -EINTR;
+
+	return retval;
+
+out_unlock:
+	spin_unlock(&files->file_lock);
+	return -EBADF;
+}
+
+static void binder_set_nice(long nice)
+{
+	long min_nice;
+	if (can_nice(current, nice)) {
+		set_user_nice(current, nice);
+		return;
+	}
+	min_nice = 20 - current->signal->rlim[RLIMIT_NICE].rlim_cur;
+	binder_debug(BINDER_DEBUG_PRIORITY_CAP,
+		     "binder: %d: nice value %ld not allowed use "
+		     "%ld instead\n", current->pid, nice, min_nice);
+	set_user_nice(current, min_nice);
+	if (min_nice < 20)
+		return;
+	binder_user_error("binder: %d RLIMIT_NICE not set\n", current->pid);
+}
+
+static size_t binder_buffer_size(struct binder_proc *proc,
+				 struct binder_buffer *buffer)
+{
+	if (list_is_last(&buffer->entry, &proc->buffers))
+		return proc->buffer + proc->buffer_size - (void *)buffer->data;
+	else
+		return (size_t)list_entry(buffer->entry.next,
+			struct binder_buffer, entry) - (size_t)buffer->data;
+}
+
+static void binder_insert_free_buffer(struct binder_proc *proc,
+				      struct binder_buffer *new_buffer)
+{
+	struct rb_node **p = &proc->free_buffers.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_buffer *buffer;
+	size_t buffer_size;
+	size_t new_buffer_size;
+
+	BUG_ON(!new_buffer->free);
+
+	new_buffer_size = binder_buffer_size(proc, new_buffer);
+
+	binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+		     "binder: %d: add free buffer, size %zd, "
+		     "at %p\n", proc->pid, new_buffer_size, new_buffer);
+
+	while (*p) {
+		parent = *p;
+		buffer = rb_entry(parent, struct binder_buffer, rb_node);
+		BUG_ON(!buffer->free);
+
+		buffer_size = binder_buffer_size(proc, buffer);
+
+		if (new_buffer_size < buffer_size)
+			p = &parent->rb_left;
+		else
+			p = &parent->rb_right;
+	}
+	rb_link_node(&new_buffer->rb_node, parent, p);
+	rb_insert_color(&new_buffer->rb_node, &proc->free_buffers);
+}
+
+static void binder_insert_allocated_buffer(struct binder_proc *proc,
+					   struct binder_buffer *new_buffer)
+{
+	struct rb_node **p = &proc->allocated_buffers.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_buffer *buffer;
+
+	BUG_ON(new_buffer->free);
+
+	while (*p) {
+		parent = *p;
+		buffer = rb_entry(parent, struct binder_buffer, rb_node);
+		BUG_ON(buffer->free);
+
+		if (new_buffer < buffer)
+			p = &parent->rb_left;
+		else if (new_buffer > buffer)
+			p = &parent->rb_right;
+		else
+			BUG();
+	}
+	rb_link_node(&new_buffer->rb_node, parent, p);
+	rb_insert_color(&new_buffer->rb_node, &proc->allocated_buffers);
+}
+
+static struct binder_buffer *binder_buffer_lookup(struct binder_proc *proc,
+						  void __user *user_ptr)
+{
+	struct rb_node *n = proc->allocated_buffers.rb_node;
+	struct binder_buffer *buffer;
+	struct binder_buffer *kern_ptr;
+
+	kern_ptr = user_ptr - proc->user_buffer_offset
+		- offsetof(struct binder_buffer, data);
+
+	while (n) {
+		buffer = rb_entry(n, struct binder_buffer, rb_node);
+		BUG_ON(buffer->free);
+
+		if (kern_ptr < buffer)
+			n = n->rb_left;
+		else if (kern_ptr > buffer)
+			n = n->rb_right;
+		else
+			return buffer;
+	}
+	return NULL;
+}
+
+static int binder_update_page_range(struct binder_proc *proc, int allocate,
+				    void *start, void *end,
+				    struct vm_area_struct *vma)
+{
+	void *page_addr;
+	unsigned long user_page_addr;
+	struct vm_struct tmp_area;
+	struct page **page;
+	struct mm_struct *mm;
+
+	binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+		     "binder: %d: %s pages %p-%p\n", proc->pid,
+		     allocate ? "allocate" : "free", start, end);
+
+	if (end <= start)
+		return 0;
+
+	if (vma)
+		mm = NULL;
+	else
+		mm = get_task_mm(proc->tsk);
+
+	if (mm) {
+		down_write(&mm->mmap_sem);
+		vma = proc->vma;
+	}
+
+	if (allocate == 0)
+		goto free_range;
+
+	if (vma == NULL) {
+		printk(KERN_ERR "binder: %d: binder_alloc_buf failed to "
+		       "map pages in userspace, no vma\n", proc->pid);
+		goto err_no_vma;
+	}
+
+	for (page_addr = start; page_addr < end; page_addr += PAGE_SIZE) {
+		int ret;
+		struct page **page_array_ptr;
+		page = &proc->pages[(page_addr - proc->buffer) / PAGE_SIZE];
+
+		BUG_ON(*page);
+		*page = alloc_page(GFP_KERNEL | __GFP_ZERO);
+		if (*page == NULL) {
+			printk(KERN_ERR "binder: %d: binder_alloc_buf failed "
+			       "for page at %p\n", proc->pid, page_addr);
+			goto err_alloc_page_failed;
+		}
+		tmp_area.addr = page_addr;
+		tmp_area.size = PAGE_SIZE + PAGE_SIZE /* guard page? */;
+		page_array_ptr = page;
+		ret = map_vm_area(&tmp_area, PAGE_KERNEL, &page_array_ptr);
+		if (ret) {
+			printk(KERN_ERR "binder: %d: binder_alloc_buf failed "
+			       "to map page at %p in kernel\n",
+			       proc->pid, page_addr);
+			goto err_map_kernel_failed;
+		}
+		user_page_addr =
+			(uintptr_t)page_addr + proc->user_buffer_offset;
+		ret = vm_insert_page(vma, user_page_addr, page[0]);
+		if (ret) {
+			printk(KERN_ERR "binder: %d: binder_alloc_buf failed "
+			       "to map page at %lx in userspace\n",
+			       proc->pid, user_page_addr);
+			goto err_vm_insert_page_failed;
+		}
+		/* vm_insert_page does not seem to increment the refcount */
+	}
+	if (mm) {
+		up_write(&mm->mmap_sem);
+		mmput(mm);
+	}
+	return 0;
+
+free_range:
+	for (page_addr = end - PAGE_SIZE; page_addr >= start;
+	     page_addr -= PAGE_SIZE) {
+		page = &proc->pages[(page_addr - proc->buffer) / PAGE_SIZE];
+		if (vma)
+			zap_page_range(vma, (uintptr_t)page_addr +
+				proc->user_buffer_offset, PAGE_SIZE, NULL);
+err_vm_insert_page_failed:
+		unmap_kernel_range((unsigned long)page_addr, PAGE_SIZE);
+err_map_kernel_failed:
+		__free_page(*page);
+		*page = NULL;
+err_alloc_page_failed:
+		;
+	}
+err_no_vma:
+	if (mm) {
+		up_write(&mm->mmap_sem);
+		mmput(mm);
+	}
+	return -ENOMEM;
+}
+
+static struct binder_buffer *binder_alloc_buf(struct binder_proc *proc,
+					      size_t data_size,
+					      size_t offsets_size, int is_async)
+{
+	struct rb_node *n = proc->free_buffers.rb_node;
+	struct binder_buffer *buffer;
+	size_t buffer_size;
+	struct rb_node *best_fit = NULL;
+	void *has_page_addr;
+	void *end_page_addr;
+	size_t size;
+
+	if (proc->vma == NULL) {
+		printk(KERN_ERR "binder: %d: binder_alloc_buf, no vma\n",
+		       proc->pid);
+		return NULL;
+	}
+
+	size = ALIGN(data_size, sizeof(void *)) +
+		ALIGN(offsets_size, sizeof(void *));
+
+	if (size < data_size || size < offsets_size) {
+		binder_user_error("binder: %d: got transaction with invalid "
+			"size %zd-%zd\n", proc->pid, data_size, offsets_size);
+		return NULL;
+	}
+
+	if (is_async &&
+	    proc->free_async_space < size + sizeof(struct binder_buffer)) {
+		binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+			     "binder: %d: binder_alloc_buf size %zd"
+			     "failed, no async space left\n", proc->pid, size);
+		return NULL;
+	}
+
+	while (n) {
+		buffer = rb_entry(n, struct binder_buffer, rb_node);
+		BUG_ON(!buffer->free);
+		buffer_size = binder_buffer_size(proc, buffer);
+
+		if (size < buffer_size) {
+			best_fit = n;
+			n = n->rb_left;
+		} else if (size > buffer_size)
+			n = n->rb_right;
+		else {
+			best_fit = n;
+			break;
+		}
+	}
+	if (best_fit == NULL) {
+		printk(KERN_ERR "binder: %d: binder_alloc_buf size %zd failed, "
+		       "no address space\n", proc->pid, size);
+		return NULL;
+	}
+	if (n == NULL) {
+		buffer = rb_entry(best_fit, struct binder_buffer, rb_node);
+		buffer_size = binder_buffer_size(proc, buffer);
+	}
+
+	binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+		     "binder: %d: binder_alloc_buf size %zd got buff"
+		     "er %p size %zd\n", proc->pid, size, buffer, buffer_size);
+
+	has_page_addr =
+		(void *)(((uintptr_t)buffer->data + buffer_size) & PAGE_MASK);
+	if (n == NULL) {
+		if (size + sizeof(struct binder_buffer) + 4 >= buffer_size)
+			buffer_size = size; /* no room for other buffers */
+		else
+			buffer_size = size + sizeof(struct binder_buffer);
+	}
+	end_page_addr =
+		(void *)PAGE_ALIGN((uintptr_t)buffer->data + buffer_size);
+	if (end_page_addr > has_page_addr)
+		end_page_addr = has_page_addr;
+	if (binder_update_page_range(proc, 1,
+	    (void *)PAGE_ALIGN((uintptr_t)buffer->data), end_page_addr, NULL))
+		return NULL;
+
+	rb_erase(best_fit, &proc->free_buffers);
+	buffer->free = 0;
+	binder_insert_allocated_buffer(proc, buffer);
+	if (buffer_size != size) {
+		struct binder_buffer *new_buffer = (void *)buffer->data + size;
+		list_add(&new_buffer->entry, &buffer->entry);
+		new_buffer->free = 1;
+		binder_insert_free_buffer(proc, new_buffer);
+	}
+	binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+		     "binder: %d: binder_alloc_buf size %zd got "
+		     "%p\n", proc->pid, size, buffer);
+	buffer->data_size = data_size;
+	buffer->offsets_size = offsets_size;
+	buffer->async_transaction = is_async;
+	if (is_async) {
+		proc->free_async_space -= size + sizeof(struct binder_buffer);
+		binder_debug(BINDER_DEBUG_BUFFER_ALLOC_ASYNC,
+			     "binder: %d: binder_alloc_buf size %zd "
+			     "async free %zd\n", proc->pid, size,
+			     proc->free_async_space);
+	}
+
+	return buffer;
+}
+
+static void *buffer_start_page(struct binder_buffer *buffer)
+{
+	return (void *)((uintptr_t)buffer & PAGE_MASK);
+}
+
+static void *buffer_end_page(struct binder_buffer *buffer)
+{
+	return (void *)(((uintptr_t)(buffer + 1) - 1) & PAGE_MASK);
+}
+
+static void binder_delete_free_buffer(struct binder_proc *proc,
+				      struct binder_buffer *buffer)
+{
+	struct binder_buffer *prev, *next = NULL;
+	int free_page_end = 1;
+	int free_page_start = 1;
+
+	BUG_ON(proc->buffers.next == &buffer->entry);
+	prev = list_entry(buffer->entry.prev, struct binder_buffer, entry);
+	BUG_ON(!prev->free);
+	if (buffer_end_page(prev) == buffer_start_page(buffer)) {
+		free_page_start = 0;
+		if (buffer_end_page(prev) == buffer_end_page(buffer))
+			free_page_end = 0;
+		binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+			     "binder: %d: merge free, buffer %p "
+			     "share page with %p\n", proc->pid, buffer, prev);
+	}
+
+	if (!list_is_last(&buffer->entry, &proc->buffers)) {
+		next = list_entry(buffer->entry.next,
+				  struct binder_buffer, entry);
+		if (buffer_start_page(next) == buffer_end_page(buffer)) {
+			free_page_end = 0;
+			if (buffer_start_page(next) ==
+			    buffer_start_page(buffer))
+				free_page_start = 0;
+			binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+				     "binder: %d: merge free, buffer"
+				     " %p share page with %p\n", proc->pid,
+				     buffer, prev);
+		}
+	}
+	list_del(&buffer->entry);
+	if (free_page_start || free_page_end) {
+		binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+			     "binder: %d: merge free, buffer %p do "
+			     "not share page%s%s with with %p or %p\n",
+			     proc->pid, buffer, free_page_start ? "" : " end",
+			     free_page_end ? "" : " start", prev, next);
+		binder_update_page_range(proc, 0, free_page_start ?
+			buffer_start_page(buffer) : buffer_end_page(buffer),
+			(free_page_end ? buffer_end_page(buffer) :
+			buffer_start_page(buffer)) + PAGE_SIZE, NULL);
+	}
+}
+
+static void binder_free_buf(struct binder_proc *proc,
+			    struct binder_buffer *buffer)
+{
+	size_t size, buffer_size;
+
+	buffer_size = binder_buffer_size(proc, buffer);
+
+	size = ALIGN(buffer->data_size, sizeof(void *)) +
+		ALIGN(buffer->offsets_size, sizeof(void *));
+
+	binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+		     "binder: %d: binder_free_buf %p size %zd buffer"
+		     "_size %zd\n", proc->pid, buffer, size, buffer_size);
+
+	BUG_ON(buffer->free);
+	BUG_ON(size > buffer_size);
+	BUG_ON(buffer->transaction != NULL);
+	BUG_ON((void *)buffer < proc->buffer);
+	BUG_ON((void *)buffer > proc->buffer + proc->buffer_size);
+
+	if (buffer->async_transaction) {
+		proc->free_async_space += size + sizeof(struct binder_buffer);
+
+		binder_debug(BINDER_DEBUG_BUFFER_ALLOC_ASYNC,
+			     "binder: %d: binder_free_buf size %zd "
+			     "async free %zd\n", proc->pid, size,
+			     proc->free_async_space);
+	}
+
+	binder_update_page_range(proc, 0,
+		(void *)PAGE_ALIGN((uintptr_t)buffer->data),
+		(void *)(((uintptr_t)buffer->data + buffer_size) & PAGE_MASK),
+		NULL);
+	rb_erase(&buffer->rb_node, &proc->allocated_buffers);
+	buffer->free = 1;
+	if (!list_is_last(&buffer->entry, &proc->buffers)) {
+		struct binder_buffer *next = list_entry(buffer->entry.next,
+						struct binder_buffer, entry);
+		if (next->free) {
+			rb_erase(&next->rb_node, &proc->free_buffers);
+			binder_delete_free_buffer(proc, next);
+		}
+	}
+	if (proc->buffers.next != &buffer->entry) {
+		struct binder_buffer *prev = list_entry(buffer->entry.prev,
+						struct binder_buffer, entry);
+		if (prev->free) {
+			binder_delete_free_buffer(proc, buffer);
+			rb_erase(&prev->rb_node, &proc->free_buffers);
+			buffer = prev;
+		}
+	}
+	binder_insert_free_buffer(proc, buffer);
+}
+
+static struct binder_node *binder_get_node(struct binder_proc *proc,
+					   void __user *ptr)
+{
+	struct rb_node *n = proc->nodes.rb_node;
+	struct binder_node *node;
+
+	while (n) {
+		node = rb_entry(n, struct binder_node, rb_node);
+
+		if (ptr < node->ptr)
+			n = n->rb_left;
+		else if (ptr > node->ptr)
+			n = n->rb_right;
+		else
+			return node;
+	}
+	return NULL;
+}
+
+static struct binder_node *binder_new_node(struct binder_proc *proc,
+					   void __user *ptr,
+					   void __user *cookie)
+{
+	struct rb_node **p = &proc->nodes.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_node *node;
+
+	while (*p) {
+		parent = *p;
+		node = rb_entry(parent, struct binder_node, rb_node);
+
+		if (ptr < node->ptr)
+			p = &(*p)->rb_left;
+		else if (ptr > node->ptr)
+			p = &(*p)->rb_right;
+		else
+			return NULL;
+	}
+
+	node = kzalloc(sizeof(*node), GFP_KERNEL);
+	if (node == NULL)
+		return NULL;
+	binder_stats_created(BINDER_STAT_NODE);
+	rb_link_node(&node->rb_node, parent, p);
+	rb_insert_color(&node->rb_node, &proc->nodes);
+	node->debug_id = ++binder_last_id;
+	node->proc = proc;
+	node->ptr = ptr;
+	node->cookie = cookie;
+	node->work.type = BINDER_WORK_NODE;
+	INIT_LIST_HEAD(&node->work.entry);
+	INIT_LIST_HEAD(&node->async_todo);
+	binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+		     "binder: %d:%d node %d u%p c%p created\n",
+		     proc->pid, current->pid, node->debug_id,
+		     node->ptr, node->cookie);
+	return node;
+}
+
+static int binder_inc_node(struct binder_node *node, int strong, int internal,
+			   struct list_head *target_list)
+{
+	if (strong) {
+		if (internal) {
+			if (target_list == NULL &&
+			    node->internal_strong_refs == 0 &&
+			    !(node == binder_context_mgr_node &&
+			    node->has_strong_ref)) {
+				printk(KERN_ERR "binder: invalid inc strong "
+					"node for %d\n", node->debug_id);
+				return -EINVAL;
+			}
+			node->internal_strong_refs++;
+		} else
+			node->local_strong_refs++;
+		if (!node->has_strong_ref && target_list) {
+			list_del_init(&node->work.entry);
+			list_add_tail(&node->work.entry, target_list);
+		}
+	} else {
+		if (!internal)
+			node->local_weak_refs++;
+		if (!node->has_weak_ref && list_empty(&node->work.entry)) {
+			if (target_list == NULL) {
+				printk(KERN_ERR "binder: invalid inc weak node "
+					"for %d\n", node->debug_id);
+				return -EINVAL;
+			}
+			list_add_tail(&node->work.entry, target_list);
+		}
+	}
+	return 0;
+}
+
+static int binder_dec_node(struct binder_node *node, int strong, int internal)
+{
+	if (strong) {
+		if (internal)
+			node->internal_strong_refs--;
+		else
+			node->local_strong_refs--;
+		if (node->local_strong_refs || node->internal_strong_refs)
+			return 0;
+	} else {
+		if (!internal)
+			node->local_weak_refs--;
+		if (node->local_weak_refs || !hlist_empty(&node->refs))
+			return 0;
+	}
+	if (node->proc && (node->has_strong_ref || node->has_weak_ref)) {
+		if (list_empty(&node->work.entry)) {
+			list_add_tail(&node->work.entry, &node->proc->todo);
+			wake_up_interruptible(&node->proc->wait);
+		}
+	} else {
+		if (hlist_empty(&node->refs) && !node->local_strong_refs &&
+		    !node->local_weak_refs) {
+			list_del_init(&node->work.entry);
+			if (node->proc) {
+				rb_erase(&node->rb_node, &node->proc->nodes);
+				binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+					     "binder: refless node %d deleted\n",
+					     node->debug_id);
+			} else {
+				hlist_del(&node->dead_node);
+				binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+					     "binder: dead node %d deleted\n",
+					     node->debug_id);
+			}
+			kfree(node);
+			binder_stats_deleted(BINDER_STAT_NODE);
+		}
+	}
+
+	return 0;
+}
+
+
+static struct binder_ref *binder_get_ref(struct binder_proc *proc,
+					 uint32_t desc)
+{
+	struct rb_node *n = proc->refs_by_desc.rb_node;
+	struct binder_ref *ref;
+
+	while (n) {
+		ref = rb_entry(n, struct binder_ref, rb_node_desc);
+
+		if (desc < ref->desc)
+			n = n->rb_left;
+		else if (desc > ref->desc)
+			n = n->rb_right;
+		else
+			return ref;
+	}
+	return NULL;
+}
+
+static struct binder_ref *binder_get_ref_for_node(struct binder_proc *proc,
+						  struct binder_node *node)
+{
+	struct rb_node *n;
+	struct rb_node **p = &proc->refs_by_node.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_ref *ref, *new_ref;
+
+	while (*p) {
+		parent = *p;
+		ref = rb_entry(parent, struct binder_ref, rb_node_node);
+
+		if (node < ref->node)
+			p = &(*p)->rb_left;
+		else if (node > ref->node)
+			p = &(*p)->rb_right;
+		else
+			return ref;
+	}
+	new_ref = kzalloc(sizeof(*ref), GFP_KERNEL);
+	if (new_ref == NULL)
+		return NULL;
+	binder_stats_created(BINDER_STAT_REF);
+	new_ref->debug_id = ++binder_last_id;
+	new_ref->proc = proc;
+	new_ref->node = node;
+	rb_link_node(&new_ref->rb_node_node, parent, p);
+	rb_insert_color(&new_ref->rb_node_node, &proc->refs_by_node);
+
+	new_ref->desc = (node == binder_context_mgr_node) ? 0 : 1;
+	for (n = rb_first(&proc->refs_by_desc); n != NULL; n = rb_next(n)) {
+		ref = rb_entry(n, struct binder_ref, rb_node_desc);
+		if (ref->desc > new_ref->desc)
+			break;
+		new_ref->desc = ref->desc + 1;
+	}
+
+	p = &proc->refs_by_desc.rb_node;
+	while (*p) {
+		parent = *p;
+		ref = rb_entry(parent, struct binder_ref, rb_node_desc);
+
+		if (new_ref->desc < ref->desc)
+			p = &(*p)->rb_left;
+		else if (new_ref->desc > ref->desc)
+			p = &(*p)->rb_right;
+		else
+			BUG();
+	}
+	rb_link_node(&new_ref->rb_node_desc, parent, p);
+	rb_insert_color(&new_ref->rb_node_desc, &proc->refs_by_desc);
+	if (node) {
+		hlist_add_head(&new_ref->node_entry, &node->refs);
+
+		binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+			     "binder: %d new ref %d desc %d for "
+			     "node %d\n", proc->pid, new_ref->debug_id,
+			     new_ref->desc, node->debug_id);
+	} else {
+		binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+			     "binder: %d new ref %d desc %d for "
+			     "dead node\n", proc->pid, new_ref->debug_id,
+			      new_ref->desc);
+	}
+	return new_ref;
+}
+
+static void binder_delete_ref(struct binder_ref *ref)
+{
+	binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+		     "binder: %d delete ref %d desc %d for "
+		     "node %d\n", ref->proc->pid, ref->debug_id,
+		     ref->desc, ref->node->debug_id);
+
+	rb_erase(&ref->rb_node_desc, &ref->proc->refs_by_desc);
+	rb_erase(&ref->rb_node_node, &ref->proc->refs_by_node);
+	if (ref->strong)
+		binder_dec_node(ref->node, 1, 1);
+	hlist_del(&ref->node_entry);
+	binder_dec_node(ref->node, 0, 1);
+	if (ref->death) {
+		binder_debug(BINDER_DEBUG_DEAD_BINDER,
+			     "binder: %d delete ref %d desc %d "
+			     "has death notification\n", ref->proc->pid,
+			     ref->debug_id, ref->desc);
+		list_del(&ref->death->work.entry);
+		kfree(ref->death);
+		binder_stats_deleted(BINDER_STAT_DEATH);
+	}
+	kfree(ref);
+	binder_stats_deleted(BINDER_STAT_REF);
+}
+
+static int binder_inc_ref(struct binder_ref *ref, int strong,
+			  struct list_head *target_list)
+{
+	int ret;
+	if (strong) {
+		if (ref->strong == 0) {
+			ret = binder_inc_node(ref->node, 1, 1, target_list);
+			if (ret)
+				return ret;
+		}
+		ref->strong++;
+	} else {
+		if (ref->weak == 0) {
+			ret = binder_inc_node(ref->node, 0, 1, target_list);
+			if (ret)
+				return ret;
+		}
+		ref->weak++;
+	}
+	return 0;
+}
+
+
+static int binder_dec_ref(struct binder_ref *ref, int strong)
+{
+	if (strong) {
+		if (ref->strong == 0) {
+			binder_user_error("binder: %d invalid dec strong, "
+					  "ref %d desc %d s %d w %d\n",
+					  ref->proc->pid, ref->debug_id,
+					  ref->desc, ref->strong, ref->weak);
+			return -EINVAL;
+		}
+		ref->strong--;
+		if (ref->strong == 0) {
+			int ret;
+			ret = binder_dec_node(ref->node, strong, 1);
+			if (ret)
+				return ret;
+		}
+	} else {
+		if (ref->weak == 0) {
+			binder_user_error("binder: %d invalid dec weak, "
+					  "ref %d desc %d s %d w %d\n",
+					  ref->proc->pid, ref->debug_id,
+					  ref->desc, ref->strong, ref->weak);
+			return -EINVAL;
+		}
+		ref->weak--;
+	}
+	if (ref->strong == 0 && ref->weak == 0)
+		binder_delete_ref(ref);
+	return 0;
+}
+
+static void binder_pop_transaction(struct binder_thread *target_thread,
+				   struct binder_transaction *t)
+{
+	if (target_thread) {
+		BUG_ON(target_thread->transaction_stack != t);
+		BUG_ON(target_thread->transaction_stack->from != target_thread);
+		target_thread->transaction_stack =
+			target_thread->transaction_stack->from_parent;
+		t->from = NULL;
+	}
+	t->need_reply = 0;
+	if (t->buffer)
+		t->buffer->transaction = NULL;
+	kfree(t);
+	binder_stats_deleted(BINDER_STAT_TRANSACTION);
+}
+
+static void binder_send_failed_reply(struct binder_transaction *t,
+				     uint32_t error_code)
+{
+	struct binder_thread *target_thread;
+	BUG_ON(t->flags & TF_ONE_WAY);
+	while (1) {
+		target_thread = t->from;
+		if (target_thread) {
+			if (target_thread->return_error != BR_OK &&
+			   target_thread->return_error2 == BR_OK) {
+				target_thread->return_error2 =
+					target_thread->return_error;
+				target_thread->return_error = BR_OK;
+			}
+			if (target_thread->return_error == BR_OK) {
+				binder_debug(BINDER_DEBUG_FAILED_TRANSACTION,
+					     "binder: send failed reply for "
+					     "transaction %d to %d:%d\n",
+					      t->debug_id, target_thread->proc->pid,
+					      target_thread->pid);
+
+				binder_pop_transaction(target_thread, t);
+				target_thread->return_error = error_code;
+				wake_up_interruptible(&target_thread->wait);
+			} else {
+				printk(KERN_ERR "binder: reply failed, target "
+					"thread, %d:%d, has error code %d "
+					"already\n", target_thread->proc->pid,
+					target_thread->pid,
+					target_thread->return_error);
+			}
+			return;
+		} else {
+			struct binder_transaction *next = t->from_parent;
+
+			binder_debug(BINDER_DEBUG_FAILED_TRANSACTION,
+				     "binder: send failed reply "
+				     "for transaction %d, target dead\n",
+				     t->debug_id);
+
+			binder_pop_transaction(target_thread, t);
+			if (next == NULL) {
+				binder_debug(BINDER_DEBUG_DEAD_BINDER,
+					     "binder: reply failed,"
+					     " no target thread at root\n");
+				return;
+			}
+			t = next;
+			binder_debug(BINDER_DEBUG_DEAD_BINDER,
+				     "binder: reply failed, no target "
+				     "thread -- retry %d\n", t->debug_id);
+		}
+	}
+}
+
+static void binder_transaction_buffer_release(struct binder_proc *proc,
+					      struct binder_buffer *buffer,
+					      size_t *failed_at)
+{
+	size_t *offp, *off_end;
+	int debug_id = buffer->debug_id;
+
+	binder_debug(BINDER_DEBUG_TRANSACTION,
+		     "binder: %d buffer release %d, size %zd-%zd, failed at %p\n",
+		     proc->pid, buffer->debug_id,
+		     buffer->data_size, buffer->offsets_size, failed_at);
+
+	if (buffer->target_node)
+		binder_dec_node(buffer->target_node, 1, 0);
+
+	offp = (size_t *)(buffer->data + ALIGN(buffer->data_size, sizeof(void *)));
+	if (failed_at)
+		off_end = failed_at;
+	else
+		off_end = (void *)offp + buffer->offsets_size;
+	for (; offp < off_end; offp++) {
+		struct flat_binder_object *fp;
+		if (*offp > buffer->data_size - sizeof(*fp) ||
+		    buffer->data_size < sizeof(*fp) ||
+		    !IS_ALIGNED(*offp, sizeof(void *))) {
+			printk(KERN_ERR "binder: transaction release %d bad"
+					"offset %zd, size %zd\n", debug_id,
+					*offp, buffer->data_size);
+			continue;
+		}
+		fp = (struct flat_binder_object *)(buffer->data + *offp);
+		switch (fp->type) {
+		case BINDER_TYPE_BINDER:
+		case BINDER_TYPE_WEAK_BINDER: {
+			struct binder_node *node = binder_get_node(proc, fp->binder);
+			if (node == NULL) {
+				printk(KERN_ERR "binder: transaction release %d"
+				       " bad node %p\n", debug_id, fp->binder);
+				break;
+			}
+			binder_debug(BINDER_DEBUG_TRANSACTION,
+				     "        node %d u%p\n",
+				     node->debug_id, node->ptr);
+			binder_dec_node(node, fp->type == BINDER_TYPE_BINDER, 0);
+		} break;
+		case BINDER_TYPE_HANDLE:
+		case BINDER_TYPE_WEAK_HANDLE: {
+			struct binder_ref *ref = binder_get_ref(proc, fp->handle);
+			if (ref == NULL) {
+				printk(KERN_ERR "binder: transaction release %d"
+				       " bad handle %ld\n", debug_id,
+				       fp->handle);
+				break;
+			}
+			binder_debug(BINDER_DEBUG_TRANSACTION,
+				     "        ref %d desc %d (node %d)\n",
+				     ref->debug_id, ref->desc, ref->node->debug_id);
+			binder_dec_ref(ref, fp->type == BINDER_TYPE_HANDLE);
+		} break;
+
+		case BINDER_TYPE_FD:
+			binder_debug(BINDER_DEBUG_TRANSACTION,
+				     "        fd %ld\n", fp->handle);
+			if (failed_at)
+				task_close_fd(proc, fp->handle);
+			break;
+
+		default:
+			printk(KERN_ERR "binder: transaction release %d bad "
+			       "object type %lx\n", debug_id, fp->type);
+			break;
+		}
+	}
+}
+
+static void binder_transaction(struct binder_proc *proc,
+			       struct binder_thread *thread,
+			       struct binder_transaction_data *tr, int reply)
+{
+	struct binder_transaction *t;
+	struct binder_work *tcomplete;
+	size_t *offp, *off_end;
+	struct binder_proc *target_proc;
+	struct binder_thread *target_thread = NULL;
+	struct binder_node *target_node = NULL;
+	struct list_head *target_list;
+	wait_queue_head_t *target_wait;
+	struct binder_transaction *in_reply_to = NULL;
+	struct binder_transaction_log_entry *e;
+	uint32_t return_error;
+
+	e = binder_transaction_log_add(&binder_transaction_log);
+	e->call_type = reply ? 2 : !!(tr->flags & TF_ONE_WAY);
+	e->from_proc = proc->pid;
+	e->from_thread = thread->pid;
+	e->target_handle = tr->target.handle;
+	e->data_size = tr->data_size;
+	e->offsets_size = tr->offsets_size;
+
+	if (reply) {
+		in_reply_to = thread->transaction_stack;
+		if (in_reply_to == NULL) {
+			binder_user_error("binder: %d:%d got reply transaction "
+					  "with no transaction stack\n",
+					  proc->pid, thread->pid);
+			return_error = BR_FAILED_REPLY;
+			goto err_empty_call_stack;
+		}
+		binder_set_nice(in_reply_to->saved_priority);
+		if (in_reply_to->to_thread != thread) {
+			binder_user_error("binder: %d:%d got reply transaction "
+				"with bad transaction stack,"
+				" transaction %d has target %d:%d\n",
+				proc->pid, thread->pid, in_reply_to->debug_id,
+				in_reply_to->to_proc ?
+				in_reply_to->to_proc->pid : 0,
+				in_reply_to->to_thread ?
+				in_reply_to->to_thread->pid : 0);
+			return_error = BR_FAILED_REPLY;
+			in_reply_to = NULL;
+			goto err_bad_call_stack;
+		}
+		thread->transaction_stack = in_reply_to->to_parent;
+		target_thread = in_reply_to->from;
+		if (target_thread == NULL) {
+			return_error = BR_DEAD_REPLY;
+			goto err_dead_binder;
+		}
+		if (target_thread->transaction_stack != in_reply_to) {
+			binder_user_error("binder: %d:%d got reply transaction "
+				"with bad target transaction stack %d, "
+				"expected %d\n",
+				proc->pid, thread->pid,
+				target_thread->transaction_stack ?
+				target_thread->transaction_stack->debug_id : 0,
+				in_reply_to->debug_id);
+			return_error = BR_FAILED_REPLY;
+			in_reply_to = NULL;
+			target_thread = NULL;
+			goto err_dead_binder;
+		}
+		target_proc = target_thread->proc;
+	} else {
+		if (tr->target.handle) {
+			struct binder_ref *ref;
+			ref = binder_get_ref(proc, tr->target.handle);
+			if (ref == NULL) {
+				binder_user_error("binder: %d:%d got "
+					"transaction to invalid handle\n",
+					proc->pid, thread->pid);
+				return_error = BR_FAILED_REPLY;
+				goto err_invalid_target_handle;
+			}
+			target_node = ref->node;
+		} else {
+			target_node = binder_context_mgr_node;
+			if (target_node == NULL) {
+				return_error = BR_DEAD_REPLY;
+				goto err_no_context_mgr_node;
+			}
+		}
+		e->to_node = target_node->debug_id;
+		target_proc = target_node->proc;
+		if (target_proc == NULL) {
+			return_error = BR_DEAD_REPLY;
+			goto err_dead_binder;
+		}
+		if (!(tr->flags & TF_ONE_WAY) && thread->transaction_stack) {
+			struct binder_transaction *tmp;
+			tmp = thread->transaction_stack;
+			if (tmp->to_thread != thread) {
+				binder_user_error("binder: %d:%d got new "
+					"transaction with bad transaction stack"
+					", transaction %d has target %d:%d\n",
+					proc->pid, thread->pid, tmp->debug_id,
+					tmp->to_proc ? tmp->to_proc->pid : 0,
+					tmp->to_thread ?
+					tmp->to_thread->pid : 0);
+				return_error = BR_FAILED_REPLY;
+				goto err_bad_call_stack;
+			}
+			while (tmp) {
+				if (tmp->from && tmp->from->proc == target_proc)
+					target_thread = tmp->from;
+				tmp = tmp->from_parent;
+			}
+		}
+	}
+	if (target_thread) {
+		e->to_thread = target_thread->pid;
+		target_list = &target_thread->todo;
+		target_wait = &target_thread->wait;
+	} else {
+		target_list = &target_proc->todo;
+		target_wait = &target_proc->wait;
+	}
+	e->to_proc = target_proc->pid;
+
+	/* TODO: reuse incoming transaction for reply */
+	t = kzalloc(sizeof(*t), GFP_KERNEL);
+	if (t == NULL) {
+		return_error = BR_FAILED_REPLY;
+		goto err_alloc_t_failed;
+	}
+	binder_stats_created(BINDER_STAT_TRANSACTION);
+
+	tcomplete = kzalloc(sizeof(*tcomplete), GFP_KERNEL);
+	if (tcomplete == NULL) {
+		return_error = BR_FAILED_REPLY;
+		goto err_alloc_tcomplete_failed;
+	}
+	binder_stats_created(BINDER_STAT_TRANSACTION_COMPLETE);
+
+	t->debug_id = ++binder_last_id;
+	e->debug_id = t->debug_id;
+
+	if (reply)
+		binder_debug(BINDER_DEBUG_TRANSACTION,
+			     "binder: %d:%d BC_REPLY %d -> %d:%d, "
+			     "data %p-%p size %zd-%zd\n",
+			     proc->pid, thread->pid, t->debug_id,
+			     target_proc->pid, target_thread->pid,
+			     tr->data.ptr.buffer, tr->data.ptr.offsets,
+			     tr->data_size, tr->offsets_size);
+	else
+		binder_debug(BINDER_DEBUG_TRANSACTION,
+			     "binder: %d:%d BC_TRANSACTION %d -> "
+			     "%d - node %d, data %p-%p size %zd-%zd\n",
+			     proc->pid, thread->pid, t->debug_id,
+			     target_proc->pid, target_node->debug_id,
+			     tr->data.ptr.buffer, tr->data.ptr.offsets,
+			     tr->data_size, tr->offsets_size);
+
+	if (!reply && !(tr->flags & TF_ONE_WAY))
+		t->from = thread;
+	else
+		t->from = NULL;
+	t->sender_euid = proc->tsk->cred->euid;
+	t->to_proc = target_proc;
+	t->to_thread = target_thread;
+	t->code = tr->code;
+	t->flags = tr->flags;
+	t->priority = task_nice(current);
+	t->buffer = binder_alloc_buf(target_proc, tr->data_size,
+		tr->offsets_size, !reply && (t->flags & TF_ONE_WAY));
+	if (t->buffer == NULL) {
+		return_error = BR_FAILED_REPLY;
+		goto err_binder_alloc_buf_failed;
+	}
+	t->buffer->allow_user_free = 0;
+	t->buffer->debug_id = t->debug_id;
+	t->buffer->transaction = t;
+	t->buffer->target_node = target_node;
+	if (target_node)
+		binder_inc_node(target_node, 1, 0, NULL);
+
+	offp = (size_t *)(t->buffer->data + ALIGN(tr->data_size, sizeof(void *)));
+
+	if (copy_from_user(t->buffer->data, tr->data.ptr.buffer, tr->data_size)) {
+		binder_user_error("binder: %d:%d got transaction with invalid "
+			"data ptr\n", proc->pid, thread->pid);
+		return_error = BR_FAILED_REPLY;
+		goto err_copy_data_failed;
+	}
+	if (copy_from_user(offp, tr->data.ptr.offsets, tr->offsets_size)) {
+		binder_user_error("binder: %d:%d got transaction with invalid "
+			"offsets ptr\n", proc->pid, thread->pid);
+		return_error = BR_FAILED_REPLY;
+		goto err_copy_data_failed;
+	}
+	if (!IS_ALIGNED(tr->offsets_size, sizeof(size_t))) {
+		binder_user_error("binder: %d:%d got transaction with "
+			"invalid offsets size, %zd\n",
+			proc->pid, thread->pid, tr->offsets_size);
+		return_error = BR_FAILED_REPLY;
+		goto err_bad_offset;
+	}
+	off_end = (void *)offp + tr->offsets_size;
+	for (; offp < off_end; offp++) {
+		struct flat_binder_object *fp;
+		if (*offp > t->buffer->data_size - sizeof(*fp) ||
+		    t->buffer->data_size < sizeof(*fp) ||
+		    !IS_ALIGNED(*offp, sizeof(void *))) {
+			binder_user_error("binder: %d:%d got transaction with "
+				"invalid offset, %zd\n",
+				proc->pid, thread->pid, *offp);
+			return_error = BR_FAILED_REPLY;
+			goto err_bad_offset;
+		}
+		fp = (struct flat_binder_object *)(t->buffer->data + *offp);
+		switch (fp->type) {
+		case BINDER_TYPE_BINDER:
+		case BINDER_TYPE_WEAK_BINDER: {
+			struct binder_ref *ref;
+			struct binder_node *node = binder_get_node(proc, fp->binder);
+			if (node == NULL) {
+				node = binder_new_node(proc, fp->binder, fp->cookie);
+				if (node == NULL) {
+					return_error = BR_FAILED_REPLY;
+					goto err_binder_new_node_failed;
+				}
+				node->min_priority = fp->flags & FLAT_BINDER_FLAG_PRIORITY_MASK;
+				node->accept_fds = !!(fp->flags & FLAT_BINDER_FLAG_ACCEPTS_FDS);
+			}
+			if (fp->cookie != node->cookie) {
+				binder_user_error("binder: %d:%d sending u%p "
+					"node %d, cookie mismatch %p != %p\n",
+					proc->pid, thread->pid,
+					fp->binder, node->debug_id,
+					fp->cookie, node->cookie);
+				goto err_binder_get_ref_for_node_failed;
+			}
+			ref = binder_get_ref_for_node(target_proc, node);
+			if (ref == NULL) {
+				return_error = BR_FAILED_REPLY;
+				goto err_binder_get_ref_for_node_failed;
+			}
+			if (fp->type == BINDER_TYPE_BINDER)
+				fp->type = BINDER_TYPE_HANDLE;
+			else
+				fp->type = BINDER_TYPE_WEAK_HANDLE;
+			fp->handle = ref->desc;
+			binder_inc_ref(ref, fp->type == BINDER_TYPE_HANDLE,
+				       &thread->todo);
+
+			binder_debug(BINDER_DEBUG_TRANSACTION,
+				     "        node %d u%p -> ref %d desc %d\n",
+				     node->debug_id, node->ptr, ref->debug_id,
+				     ref->desc);
+		} break;
+		case BINDER_TYPE_HANDLE:
+		case BINDER_TYPE_WEAK_HANDLE: {
+			struct binder_ref *ref = binder_get_ref(proc, fp->handle);
+			if (ref == NULL) {
+				binder_user_error("binder: %d:%d got "
+					"transaction with invalid "
+					"handle, %ld\n", proc->pid,
+					thread->pid, fp->handle);
+				return_error = BR_FAILED_REPLY;
+				goto err_binder_get_ref_failed;
+			}
+			if (ref->node->proc == target_proc) {
+				if (fp->type == BINDER_TYPE_HANDLE)
+					fp->type = BINDER_TYPE_BINDER;
+				else
+					fp->type = BINDER_TYPE_WEAK_BINDER;
+				fp->binder = ref->node->ptr;
+				fp->cookie = ref->node->cookie;
+				binder_inc_node(ref->node, fp->type == BINDER_TYPE_BINDER, 0, NULL);
+				binder_debug(BINDER_DEBUG_TRANSACTION,
+					     "        ref %d desc %d -> node %d u%p\n",
+					     ref->debug_id, ref->desc, ref->node->debug_id,
+					     ref->node->ptr);
+			} else {
+				struct binder_ref *new_ref;
+				new_ref = binder_get_ref_for_node(target_proc, ref->node);
+				if (new_ref == NULL) {
+					return_error = BR_FAILED_REPLY;
+					goto err_binder_get_ref_for_node_failed;
+				}
+				fp->handle = new_ref->desc;
+				binder_inc_ref(new_ref, fp->type == BINDER_TYPE_HANDLE, NULL);
+				binder_debug(BINDER_DEBUG_TRANSACTION,
+					     "        ref %d desc %d -> ref %d desc %d (node %d)\n",
+					     ref->debug_id, ref->desc, new_ref->debug_id,
+					     new_ref->desc, ref->node->debug_id);
+			}
+		} break;
+
+		case BINDER_TYPE_FD: {
+			int target_fd;
+			struct file *file;
+
+			if (reply) {
+				if (!(in_reply_to->flags & TF_ACCEPT_FDS)) {
+					binder_user_error("binder: %d:%d got reply with fd, %ld, but target does not allow fds\n",
+						proc->pid, thread->pid, fp->handle);
+					return_error = BR_FAILED_REPLY;
+					goto err_fd_not_allowed;
+				}
+			} else if (!target_node->accept_fds) {
+				binder_user_error("binder: %d:%d got transaction with fd, %ld, but target does not allow fds\n",
+					proc->pid, thread->pid, fp->handle);
+				return_error = BR_FAILED_REPLY;
+				goto err_fd_not_allowed;
+			}
+
+			file = fget(fp->handle);
+			if (file == NULL) {
+				binder_user_error("binder: %d:%d got transaction with invalid fd, %ld\n",
+					proc->pid, thread->pid, fp->handle);
+				return_error = BR_FAILED_REPLY;
+				goto err_fget_failed;
+			}
+			target_fd = task_get_unused_fd_flags(target_proc, O_CLOEXEC);
+			if (target_fd < 0) {
+				fput(file);
+				return_error = BR_FAILED_REPLY;
+				goto err_get_unused_fd_failed;
+			}
+			task_fd_install(target_proc, target_fd, file);
+			binder_debug(BINDER_DEBUG_TRANSACTION,
+				     "        fd %ld -> %d\n", fp->handle, target_fd);
+			/* TODO: fput? */
+			fp->handle = target_fd;
+		} break;
+
+		default:
+			binder_user_error("binder: %d:%d got transactio"
+				"n with invalid object type, %lx\n",
+				proc->pid, thread->pid, fp->type);
+			return_error = BR_FAILED_REPLY;
+			goto err_bad_object_type;
+		}
+	}
+	if (reply) {
+		BUG_ON(t->buffer->async_transaction != 0);
+		binder_pop_transaction(target_thread, in_reply_to);
+	} else if (!(t->flags & TF_ONE_WAY)) {
+		BUG_ON(t->buffer->async_transaction != 0);
+		t->need_reply = 1;
+		t->from_parent = thread->transaction_stack;
+		thread->transaction_stack = t;
+	} else {
+		BUG_ON(target_node == NULL);
+		BUG_ON(t->buffer->async_transaction != 1);
+		if (target_node->has_async_transaction) {
+			target_list = &target_node->async_todo;
+			target_wait = NULL;
+		} else
+			target_node->has_async_transaction = 1;
+	}
+	t->work.type = BINDER_WORK_TRANSACTION;
+	list_add_tail(&t->work.entry, target_list);
+	tcomplete->type = BINDER_WORK_TRANSACTION_COMPLETE;
+	list_add_tail(&tcomplete->entry, &thread->todo);
+	if (target_wait)
+		wake_up_interruptible(target_wait);
+	return;
+
+err_get_unused_fd_failed:
+err_fget_failed:
+err_fd_not_allowed:
+err_binder_get_ref_for_node_failed:
+err_binder_get_ref_failed:
+err_binder_new_node_failed:
+err_bad_object_type:
+err_bad_offset:
+err_copy_data_failed:
+	binder_transaction_buffer_release(target_proc, t->buffer, offp);
+	t->buffer->transaction = NULL;
+	binder_free_buf(target_proc, t->buffer);
+err_binder_alloc_buf_failed:
+	kfree(tcomplete);
+	binder_stats_deleted(BINDER_STAT_TRANSACTION_COMPLETE);
+err_alloc_tcomplete_failed:
+	kfree(t);
+	binder_stats_deleted(BINDER_STAT_TRANSACTION);
+err_alloc_t_failed:
+err_bad_call_stack:
+err_empty_call_stack:
+err_dead_binder:
+err_invalid_target_handle:
+err_no_context_mgr_node:
+	binder_debug(BINDER_DEBUG_FAILED_TRANSACTION,
+		     "binder: %d:%d transaction failed %d, size %zd-%zd\n",
+		     proc->pid, thread->pid, return_error,
+		     tr->data_size, tr->offsets_size);
+
+	{
+		struct binder_transaction_log_entry *fe;
+		fe = binder_transaction_log_add(&binder_transaction_log_failed);
+		*fe = *e;
+	}
+
+	BUG_ON(thread->return_error != BR_OK);
+	if (in_reply_to) {
+		thread->return_error = BR_TRANSACTION_COMPLETE;
+		binder_send_failed_reply(in_reply_to, return_error);
+	} else
+		thread->return_error = return_error;
+}
+
+int binder_thread_write(struct binder_proc *proc, struct binder_thread *thread,
+			void __user *buffer, int size, signed long *consumed)
+{
+	uint32_t cmd;
+	void __user *ptr = buffer + *consumed;
+	void __user *end = buffer + size;
+
+	while (ptr < end && thread->return_error == BR_OK) {
+		if (get_user(cmd, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+		if (_IOC_NR(cmd) < ARRAY_SIZE(binder_stats.bc)) {
+			binder_stats.bc[_IOC_NR(cmd)]++;
+			proc->stats.bc[_IOC_NR(cmd)]++;
+			thread->stats.bc[_IOC_NR(cmd)]++;
+		}
+		switch (cmd) {
+		case BC_INCREFS:
+		case BC_ACQUIRE:
+		case BC_RELEASE:
+		case BC_DECREFS: {
+			uint32_t target;
+			struct binder_ref *ref;
+			const char *debug_string;
+
+			if (get_user(target, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (target == 0 && binder_context_mgr_node &&
+			    (cmd == BC_INCREFS || cmd == BC_ACQUIRE)) {
+				ref = binder_get_ref_for_node(proc,
+					       binder_context_mgr_node);
+				if (ref->desc != target) {
+					binder_user_error("binder: %d:"
+						"%d tried to acquire "
+						"reference to desc 0, "
+						"got %d instead\n",
+						proc->pid, thread->pid,
+						ref->desc);
+				}
+			} else
+				ref = binder_get_ref(proc, target);
+			if (ref == NULL) {
+				binder_user_error("binder: %d:%d refcou"
+					"nt change on invalid ref %d\n",
+					proc->pid, thread->pid, target);
+				break;
+			}
+			switch (cmd) {
+			case BC_INCREFS:
+				debug_string = "IncRefs";
+				binder_inc_ref(ref, 0, NULL);
+				break;
+			case BC_ACQUIRE:
+				debug_string = "Acquire";
+				binder_inc_ref(ref, 1, NULL);
+				break;
+			case BC_RELEASE:
+				debug_string = "Release";
+				binder_dec_ref(ref, 1);
+				break;
+			case BC_DECREFS:
+			default:
+				debug_string = "DecRefs";
+				binder_dec_ref(ref, 0);
+				break;
+			}
+			binder_debug(BINDER_DEBUG_USER_REFS,
+				     "binder: %d:%d %s ref %d desc %d s %d w %d for node %d\n",
+				     proc->pid, thread->pid, debug_string, ref->debug_id,
+				     ref->desc, ref->strong, ref->weak, ref->node->debug_id);
+			break;
+		}
+		case BC_INCREFS_DONE:
+		case BC_ACQUIRE_DONE: {
+			void __user *node_ptr;
+			void *cookie;
+			struct binder_node *node;
+
+			if (get_user(node_ptr, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			if (get_user(cookie, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			node = binder_get_node(proc, node_ptr);
+			if (node == NULL) {
+				binder_user_error("binder: %d:%d "
+					"%s u%p no match\n",
+					proc->pid, thread->pid,
+					cmd == BC_INCREFS_DONE ?
+					"BC_INCREFS_DONE" :
+					"BC_ACQUIRE_DONE",
+					node_ptr);
+				break;
+			}
+			if (cookie != node->cookie) {
+				binder_user_error("binder: %d:%d %s u%p node %d"
+					" cookie mismatch %p != %p\n",
+					proc->pid, thread->pid,
+					cmd == BC_INCREFS_DONE ?
+					"BC_INCREFS_DONE" : "BC_ACQUIRE_DONE",
+					node_ptr, node->debug_id,
+					cookie, node->cookie);
+				break;
+			}
+			if (cmd == BC_ACQUIRE_DONE) {
+				if (node->pending_strong_ref == 0) {
+					binder_user_error("binder: %d:%d "
+						"BC_ACQUIRE_DONE node %d has "
+						"no pending acquire request\n",
+						proc->pid, thread->pid,
+						node->debug_id);
+					break;
+				}
+				node->pending_strong_ref = 0;
+			} else {
+				if (node->pending_weak_ref == 0) {
+					binder_user_error("binder: %d:%d "
+						"BC_INCREFS_DONE node %d has "
+						"no pending increfs request\n",
+						proc->pid, thread->pid,
+						node->debug_id);
+					break;
+				}
+				node->pending_weak_ref = 0;
+			}
+			binder_dec_node(node, cmd == BC_ACQUIRE_DONE, 0);
+			binder_debug(BINDER_DEBUG_USER_REFS,
+				     "binder: %d:%d %s node %d ls %d lw %d\n",
+				     proc->pid, thread->pid,
+				     cmd == BC_INCREFS_DONE ? "BC_INCREFS_DONE" : "BC_ACQUIRE_DONE",
+				     node->debug_id, node->local_strong_refs, node->local_weak_refs);
+			break;
+		}
+		case BC_ATTEMPT_ACQUIRE:
+			printk(KERN_ERR "binder: BC_ATTEMPT_ACQUIRE not supported\n");
+			return -EINVAL;
+		case BC_ACQUIRE_RESULT:
+			printk(KERN_ERR "binder: BC_ACQUIRE_RESULT not supported\n");
+			return -EINVAL;
+
+		case BC_FREE_BUFFER: {
+			void __user *data_ptr;
+			struct binder_buffer *buffer;
+
+			if (get_user(data_ptr, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+
+			buffer = binder_buffer_lookup(proc, data_ptr);
+			if (buffer == NULL) {
+				binder_user_error("binder: %d:%d "
+					"BC_FREE_BUFFER u%p no match\n",
+					proc->pid, thread->pid, data_ptr);
+				break;
+			}
+			if (!buffer->allow_user_free) {
+				binder_user_error("binder: %d:%d "
+					"BC_FREE_BUFFER u%p matched "
+					"unreturned buffer\n",
+					proc->pid, thread->pid, data_ptr);
+				break;
+			}
+			binder_debug(BINDER_DEBUG_FREE_BUFFER,
+				     "binder: %d:%d BC_FREE_BUFFER u%p found buffer %d for %s transaction\n",
+				     proc->pid, thread->pid, data_ptr, buffer->debug_id,
+				     buffer->transaction ? "active" : "finished");
+
+			if (buffer->transaction) {
+				buffer->transaction->buffer = NULL;
+				buffer->transaction = NULL;
+			}
+			if (buffer->async_transaction && buffer->target_node) {
+				BUG_ON(!buffer->target_node->has_async_transaction);
+				if (list_empty(&buffer->target_node->async_todo))
+					buffer->target_node->has_async_transaction = 0;
+				else
+					list_move_tail(buffer->target_node->async_todo.next, &thread->todo);
+			}
+			binder_transaction_buffer_release(proc, buffer, NULL);
+			binder_free_buf(proc, buffer);
+			break;
+		}
+
+		case BC_TRANSACTION:
+		case BC_REPLY: {
+			struct binder_transaction_data tr;
+
+			if (copy_from_user(&tr, ptr, sizeof(tr)))
+				return -EFAULT;
+			ptr += sizeof(tr);
+			binder_transaction(proc, thread, &tr, cmd == BC_REPLY);
+			break;
+		}
+
+		case BC_REGISTER_LOOPER:
+			binder_debug(BINDER_DEBUG_THREADS,
+				     "binder: %d:%d BC_REGISTER_LOOPER\n",
+				     proc->pid, thread->pid);
+			if (thread->looper & BINDER_LOOPER_STATE_ENTERED) {
+				thread->looper |= BINDER_LOOPER_STATE_INVALID;
+				binder_user_error("binder: %d:%d ERROR:"
+					" BC_REGISTER_LOOPER called "
+					"after BC_ENTER_LOOPER\n",
+					proc->pid, thread->pid);
+			} else if (proc->requested_threads == 0) {
+				thread->looper |= BINDER_LOOPER_STATE_INVALID;
+				binder_user_error("binder: %d:%d ERROR:"
+					" BC_REGISTER_LOOPER called "
+					"without request\n",
+					proc->pid, thread->pid);
+			} else {
+				proc->requested_threads--;
+				proc->requested_threads_started++;
+			}
+			thread->looper |= BINDER_LOOPER_STATE_REGISTERED;
+			break;
+		case BC_ENTER_LOOPER:
+			binder_debug(BINDER_DEBUG_THREADS,
+				     "binder: %d:%d BC_ENTER_LOOPER\n",
+				     proc->pid, thread->pid);
+			if (thread->looper & BINDER_LOOPER_STATE_REGISTERED) {
+				thread->looper |= BINDER_LOOPER_STATE_INVALID;
+				binder_user_error("binder: %d:%d ERROR:"
+					" BC_ENTER_LOOPER called after "
+					"BC_REGISTER_LOOPER\n",
+					proc->pid, thread->pid);
+			}
+			thread->looper |= BINDER_LOOPER_STATE_ENTERED;
+			break;
+		case BC_EXIT_LOOPER:
+			binder_debug(BINDER_DEBUG_THREADS,
+				     "binder: %d:%d BC_EXIT_LOOPER\n",
+				     proc->pid, thread->pid);
+			thread->looper |= BINDER_LOOPER_STATE_EXITED;
+			break;
+
+		case BC_REQUEST_DEATH_NOTIFICATION:
+		case BC_CLEAR_DEATH_NOTIFICATION: {
+			uint32_t target;
+			void __user *cookie;
+			struct binder_ref *ref;
+			struct binder_ref_death *death;
+
+			if (get_user(target, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (get_user(cookie, (void __user * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			ref = binder_get_ref(proc, target);
+			if (ref == NULL) {
+				binder_user_error("binder: %d:%d %s "
+					"invalid ref %d\n",
+					proc->pid, thread->pid,
+					cmd == BC_REQUEST_DEATH_NOTIFICATION ?
+					"BC_REQUEST_DEATH_NOTIFICATION" :
+					"BC_CLEAR_DEATH_NOTIFICATION",
+					target);
+				break;
+			}
+
+			binder_debug(BINDER_DEBUG_DEATH_NOTIFICATION,
+				     "binder: %d:%d %s %p ref %d desc %d s %d w %d for node %d\n",
+				     proc->pid, thread->pid,
+				     cmd == BC_REQUEST_DEATH_NOTIFICATION ?
+				     "BC_REQUEST_DEATH_NOTIFICATION" :
+				     "BC_CLEAR_DEATH_NOTIFICATION",
+				     cookie, ref->debug_id, ref->desc,
+				     ref->strong, ref->weak, ref->node->debug_id);
+
+			if (cmd == BC_REQUEST_DEATH_NOTIFICATION) {
+				if (ref->death) {
+					binder_user_error("binder: %d:%"
+						"d BC_REQUEST_DEATH_NOTI"
+						"FICATION death notific"
+						"ation already set\n",
+						proc->pid, thread->pid);
+					break;
+				}
+				death = kzalloc(sizeof(*death), GFP_KERNEL);
+				if (death == NULL) {
+					thread->return_error = BR_ERROR;
+					binder_debug(BINDER_DEBUG_FAILED_TRANSACTION,
+						     "binder: %d:%d "
+						     "BC_REQUEST_DEATH_NOTIFICATION failed\n",
+						     proc->pid, thread->pid);
+					break;
+				}
+				binder_stats_created(BINDER_STAT_DEATH);
+				INIT_LIST_HEAD(&death->work.entry);
+				death->cookie = cookie;
+				ref->death = death;
+				if (ref->node->proc == NULL) {
+					ref->death->work.type = BINDER_WORK_DEAD_BINDER;
+					if (thread->looper & (BINDER_LOOPER_STATE_REGISTERED | BINDER_LOOPER_STATE_ENTERED)) {
+						list_add_tail(&ref->death->work.entry, &thread->todo);
+					} else {
+						list_add_tail(&ref->death->work.entry, &proc->todo);
+						wake_up_interruptible(&proc->wait);
+					}
+				}
+			} else {
+				if (ref->death == NULL) {
+					binder_user_error("binder: %d:%"
+						"d BC_CLEAR_DEATH_NOTIFI"
+						"CATION death notificat"
+						"ion not active\n",
+						proc->pid, thread->pid);
+					break;
+				}
+				death = ref->death;
+				if (death->cookie != cookie) {
+					binder_user_error("binder: %d:%"
+						"d BC_CLEAR_DEATH_NOTIFI"
+						"CATION death notificat"
+						"ion cookie mismatch "
+						"%p != %p\n",
+						proc->pid, thread->pid,
+						death->cookie, cookie);
+					break;
+				}
+				ref->death = NULL;
+				if (list_empty(&death->work.entry)) {
+					death->work.type = BINDER_WORK_CLEAR_DEATH_NOTIFICATION;
+					if (thread->looper & (BINDER_LOOPER_STATE_REGISTERED | BINDER_LOOPER_STATE_ENTERED)) {
+						list_add_tail(&death->work.entry, &thread->todo);
+					} else {
+						list_add_tail(&death->work.entry, &proc->todo);
+						wake_up_interruptible(&proc->wait);
+					}
+				} else {
+					BUG_ON(death->work.type != BINDER_WORK_DEAD_BINDER);
+					death->work.type = BINDER_WORK_DEAD_BINDER_AND_CLEAR;
+				}
+			}
+		} break;
+		case BC_DEAD_BINDER_DONE: {
+			struct binder_work *w;
+			void __user *cookie;
+			struct binder_ref_death *death = NULL;
+			if (get_user(cookie, (void __user * __user *)ptr))
+				return -EFAULT;
+
+			ptr += sizeof(void *);
+			list_for_each_entry(w, &proc->delivered_death, entry) {
+				struct binder_ref_death *tmp_death = container_of(w, struct binder_ref_death, work);
+				if (tmp_death->cookie == cookie) {
+					death = tmp_death;
+					break;
+				}
+			}
+			binder_debug(BINDER_DEBUG_DEAD_BINDER,
+				     "binder: %d:%d BC_DEAD_BINDER_DONE %p found %p\n",
+				     proc->pid, thread->pid, cookie, death);
+			if (death == NULL) {
+				binder_user_error("binder: %d:%d BC_DEAD"
+					"_BINDER_DONE %p not found\n",
+					proc->pid, thread->pid, cookie);
+				break;
+			}
+
+			list_del_init(&death->work.entry);
+			if (death->work.type == BINDER_WORK_DEAD_BINDER_AND_CLEAR) {
+				death->work.type = BINDER_WORK_CLEAR_DEATH_NOTIFICATION;
+				if (thread->looper & (BINDER_LOOPER_STATE_REGISTERED | BINDER_LOOPER_STATE_ENTERED)) {
+					list_add_tail(&death->work.entry, &thread->todo);
+				} else {
+					list_add_tail(&death->work.entry, &proc->todo);
+					wake_up_interruptible(&proc->wait);
+				}
+			}
+		} break;
+
+		default:
+			printk(KERN_ERR "binder: %d:%d unknown command %d\n",
+			       proc->pid, thread->pid, cmd);
+			return -EINVAL;
+		}
+		*consumed = ptr - buffer;
+	}
+	return 0;
+}
+
+void binder_stat_br(struct binder_proc *proc, struct binder_thread *thread,
+		    uint32_t cmd)
+{
+	if (_IOC_NR(cmd) < ARRAY_SIZE(binder_stats.br)) {
+		binder_stats.br[_IOC_NR(cmd)]++;
+		proc->stats.br[_IOC_NR(cmd)]++;
+		thread->stats.br[_IOC_NR(cmd)]++;
+	}
+}
+
+static int binder_has_proc_work(struct binder_proc *proc,
+				struct binder_thread *thread)
+{
+	return !list_empty(&proc->todo) ||
+		(thread->looper & BINDER_LOOPER_STATE_NEED_RETURN);
+}
+
+static int binder_has_thread_work(struct binder_thread *thread)
+{
+	return !list_empty(&thread->todo) || thread->return_error != BR_OK ||
+		(thread->looper & BINDER_LOOPER_STATE_NEED_RETURN);
+}
+
+static int binder_thread_read(struct binder_proc *proc,
+			      struct binder_thread *thread,
+			      void  __user *buffer, int size,
+			      signed long *consumed, int non_block)
+{
+	void __user *ptr = buffer + *consumed;
+	void __user *end = buffer + size;
+
+	int ret = 0;
+	int wait_for_proc_work;
+
+	if (*consumed == 0) {
+		if (put_user(BR_NOOP, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+	}
+
+retry:
+	wait_for_proc_work = thread->transaction_stack == NULL &&
+				list_empty(&thread->todo);
+
+	if (thread->return_error != BR_OK && ptr < end) {
+		if (thread->return_error2 != BR_OK) {
+			if (put_user(thread->return_error2, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (ptr == end)
+				goto done;
+			thread->return_error2 = BR_OK;
+		}
+		if (put_user(thread->return_error, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+		thread->return_error = BR_OK;
+		goto done;
+	}
+
+
+	thread->looper |= BINDER_LOOPER_STATE_WAITING;
+	if (wait_for_proc_work)
+		proc->ready_threads++;
+	mutex_unlock(&binder_lock);
+	if (wait_for_proc_work) {
+		if (!(thread->looper & (BINDER_LOOPER_STATE_REGISTERED |
+					BINDER_LOOPER_STATE_ENTERED))) {
+			binder_user_error("binder: %d:%d ERROR: Thread waiting "
+				"for process work before calling BC_REGISTER_"
+				"LOOPER or BC_ENTER_LOOPER (state %x)\n",
+				proc->pid, thread->pid, thread->looper);
+			wait_event_interruptible(binder_user_error_wait,
+						 binder_stop_on_user_error < 2);
+		}
+		binder_set_nice(proc->default_priority);
+		if (non_block) {
+			if (!binder_has_proc_work(proc, thread))
+				ret = -EAGAIN;
+		} else
+			ret = wait_event_interruptible_exclusive(proc->wait, binder_has_proc_work(proc, thread));
+	} else {
+		if (non_block) {
+			if (!binder_has_thread_work(thread))
+				ret = -EAGAIN;
+		} else
+			ret = wait_event_interruptible(thread->wait, binder_has_thread_work(thread));
+	}
+	mutex_lock(&binder_lock);
+	if (wait_for_proc_work)
+		proc->ready_threads--;
+	thread->looper &= ~BINDER_LOOPER_STATE_WAITING;
+
+	if (ret)
+		return ret;
+
+	while (1) {
+		uint32_t cmd;
+		struct binder_transaction_data tr;
+		struct binder_work *w;
+		struct binder_transaction *t = NULL;
+
+		if (!list_empty(&thread->todo))
+			w = list_first_entry(&thread->todo, struct binder_work, entry);
+		else if (!list_empty(&proc->todo) && wait_for_proc_work)
+			w = list_first_entry(&proc->todo, struct binder_work, entry);
+		else {
+			if (ptr - buffer == 4 && !(thread->looper & BINDER_LOOPER_STATE_NEED_RETURN)) /* no data added */
+				goto retry;
+			break;
+		}
+
+		if (end - ptr < sizeof(tr) + 4)
+			break;
+
+		switch (w->type) {
+		case BINDER_WORK_TRANSACTION: {
+			t = container_of(w, struct binder_transaction, work);
+		} break;
+		case BINDER_WORK_TRANSACTION_COMPLETE: {
+			cmd = BR_TRANSACTION_COMPLETE;
+			if (put_user(cmd, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+
+			binder_stat_br(proc, thread, cmd);
+			binder_debug(BINDER_DEBUG_TRANSACTION_COMPLETE,
+				     "binder: %d:%d BR_TRANSACTION_COMPLETE\n",
+				     proc->pid, thread->pid);
+
+			list_del(&w->entry);
+			kfree(w);
+			binder_stats_deleted(BINDER_STAT_TRANSACTION_COMPLETE);
+		} break;
+		case BINDER_WORK_NODE: {
+			struct binder_node *node = container_of(w, struct binder_node, work);
+			uint32_t cmd = BR_NOOP;
+			const char *cmd_name;
+			int strong = node->internal_strong_refs || node->local_strong_refs;
+			int weak = !hlist_empty(&node->refs) || node->local_weak_refs || strong;
+			if (weak && !node->has_weak_ref) {
+				cmd = BR_INCREFS;
+				cmd_name = "BR_INCREFS";
+				node->has_weak_ref = 1;
+				node->pending_weak_ref = 1;
+				node->local_weak_refs++;
+			} else if (strong && !node->has_strong_ref) {
+				cmd = BR_ACQUIRE;
+				cmd_name = "BR_ACQUIRE";
+				node->has_strong_ref = 1;
+				node->pending_strong_ref = 1;
+				node->local_strong_refs++;
+			} else if (!strong && node->has_strong_ref) {
+				cmd = BR_RELEASE;
+				cmd_name = "BR_RELEASE";
+				node->has_strong_ref = 0;
+			} else if (!weak && node->has_weak_ref) {
+				cmd = BR_DECREFS;
+				cmd_name = "BR_DECREFS";
+				node->has_weak_ref = 0;
+			}
+			if (cmd != BR_NOOP) {
+				if (put_user(cmd, (uint32_t __user *)ptr))
+					return -EFAULT;
+				ptr += sizeof(uint32_t);
+				if (put_user(node->ptr, (void * __user *)ptr))
+					return -EFAULT;
+				ptr += sizeof(void *);
+				if (put_user(node->cookie, (void * __user *)ptr))
+					return -EFAULT;
+				ptr += sizeof(void *);
+
+				binder_stat_br(proc, thread, cmd);
+				binder_debug(BINDER_DEBUG_USER_REFS,
+					     "binder: %d:%d %s %d u%p c%p\n",
+					     proc->pid, thread->pid, cmd_name, node->debug_id, node->ptr, node->cookie);
+			} else {
+				list_del_init(&w->entry);
+				if (!weak && !strong) {
+					binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+						     "binder: %d:%d node %d u%p c%p deleted\n",
+						     proc->pid, thread->pid, node->debug_id,
+						     node->ptr, node->cookie);
+					rb_erase(&node->rb_node, &proc->nodes);
+					kfree(node);
+					binder_stats_deleted(BINDER_STAT_NODE);
+				} else {
+					binder_debug(BINDER_DEBUG_INTERNAL_REFS,
+						     "binder: %d:%d node %d u%p c%p state unchanged\n",
+						     proc->pid, thread->pid, node->debug_id, node->ptr,
+						     node->cookie);
+				}
+			}
+		} break;
+		case BINDER_WORK_DEAD_BINDER:
+		case BINDER_WORK_DEAD_BINDER_AND_CLEAR:
+		case BINDER_WORK_CLEAR_DEATH_NOTIFICATION: {
+			struct binder_ref_death *death;
+			uint32_t cmd;
+
+			death = container_of(w, struct binder_ref_death, work);
+			if (w->type == BINDER_WORK_CLEAR_DEATH_NOTIFICATION)
+				cmd = BR_CLEAR_DEATH_NOTIFICATION_DONE;
+			else
+				cmd = BR_DEAD_BINDER;
+			if (put_user(cmd, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (put_user(death->cookie, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			binder_debug(BINDER_DEBUG_DEATH_NOTIFICATION,
+				     "binder: %d:%d %s %p\n",
+				      proc->pid, thread->pid,
+				      cmd == BR_DEAD_BINDER ?
+				      "BR_DEAD_BINDER" :
+				      "BR_CLEAR_DEATH_NOTIFICATION_DONE",
+				      death->cookie);
+
+			if (w->type == BINDER_WORK_CLEAR_DEATH_NOTIFICATION) {
+				list_del(&w->entry);
+				kfree(death);
+				binder_stats_deleted(BINDER_STAT_DEATH);
+			} else
+				list_move(&w->entry, &proc->delivered_death);
+			if (cmd == BR_DEAD_BINDER)
+				goto done; /* DEAD_BINDER notifications can cause transactions */
+		} break;
+		}
+
+		if (!t)
+			continue;
+
+		BUG_ON(t->buffer == NULL);
+		if (t->buffer->target_node) {
+			struct binder_node *target_node = t->buffer->target_node;
+			tr.target.ptr = target_node->ptr;
+			tr.cookie =  target_node->cookie;
+			t->saved_priority = task_nice(current);
+			if (t->priority < target_node->min_priority &&
+			    !(t->flags & TF_ONE_WAY))
+				binder_set_nice(t->priority);
+			else if (!(t->flags & TF_ONE_WAY) ||
+				 t->saved_priority > target_node->min_priority)
+				binder_set_nice(target_node->min_priority);
+			cmd = BR_TRANSACTION;
+		} else {
+			tr.target.ptr = NULL;
+			tr.cookie = NULL;
+			cmd = BR_REPLY;
+		}
+		tr.code = t->code;
+		tr.flags = t->flags;
+		tr.sender_euid = t->sender_euid;
+
+		if (t->from) {
+			struct task_struct *sender = t->from->proc->tsk;
+			tr.sender_pid = task_tgid_nr_ns(sender,
+							current->nsproxy->pid_ns);
+		} else {
+			tr.sender_pid = 0;
+		}
+
+		tr.data_size = t->buffer->data_size;
+		tr.offsets_size = t->buffer->offsets_size;
+		tr.data.ptr.buffer = (void *)t->buffer->data +
+					proc->user_buffer_offset;
+		tr.data.ptr.offsets = tr.data.ptr.buffer +
+					ALIGN(t->buffer->data_size,
+					    sizeof(void *));
+
+		if (put_user(cmd, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+		if (copy_to_user(ptr, &tr, sizeof(tr)))
+			return -EFAULT;
+		ptr += sizeof(tr);
+
+		binder_stat_br(proc, thread, cmd);
+		binder_debug(BINDER_DEBUG_TRANSACTION,
+			     "binder: %d:%d %s %d %d:%d, cmd %d"
+			     "size %zd-%zd ptr %p-%p\n",
+			     proc->pid, thread->pid,
+			     (cmd == BR_TRANSACTION) ? "BR_TRANSACTION" :
+			     "BR_REPLY",
+			     t->debug_id, t->from ? t->from->proc->pid : 0,
+			     t->from ? t->from->pid : 0, cmd,
+			     t->buffer->data_size, t->buffer->offsets_size,
+			     tr.data.ptr.buffer, tr.data.ptr.offsets);
+
+		list_del(&t->work.entry);
+		t->buffer->allow_user_free = 1;
+		if (cmd == BR_TRANSACTION && !(t->flags & TF_ONE_WAY)) {
+			t->to_parent = thread->transaction_stack;
+			t->to_thread = thread;
+			thread->transaction_stack = t;
+		} else {
+			t->buffer->transaction = NULL;
+			kfree(t);
+			binder_stats_deleted(BINDER_STAT_TRANSACTION);
+		}
+		break;
+	}
+
+done:
+
+	*consumed = ptr - buffer;
+	if (proc->requested_threads + proc->ready_threads == 0 &&
+	    proc->requested_threads_started < proc->max_threads &&
+	    (thread->looper & (BINDER_LOOPER_STATE_REGISTERED |
+	     BINDER_LOOPER_STATE_ENTERED)) /* the user-space code fails to */
+	     /*spawn a new thread if we leave this out */) {
+		proc->requested_threads++;
+		binder_debug(BINDER_DEBUG_THREADS,
+			     "binder: %d:%d BR_SPAWN_LOOPER\n",
+			     proc->pid, thread->pid);
+		if (put_user(BR_SPAWN_LOOPER, (uint32_t __user *)buffer))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+static void binder_release_work(struct list_head *list)
+{
+	struct binder_work *w;
+	while (!list_empty(list)) {
+		w = list_first_entry(list, struct binder_work, entry);
+		list_del_init(&w->entry);
+		switch (w->type) {
+		case BINDER_WORK_TRANSACTION: {
+			struct binder_transaction *t;
+
+			t = container_of(w, struct binder_transaction, work);
+			if (t->buffer->target_node && !(t->flags & TF_ONE_WAY))
+				binder_send_failed_reply(t, BR_DEAD_REPLY);
+		} break;
+		case BINDER_WORK_TRANSACTION_COMPLETE: {
+			kfree(w);
+			binder_stats_deleted(BINDER_STAT_TRANSACTION_COMPLETE);
+		} break;
+		default:
+			break;
+		}
+	}
+
+}
+
+static struct binder_thread *binder_get_thread(struct binder_proc *proc)
+{
+	struct binder_thread *thread = NULL;
+	struct rb_node *parent = NULL;
+	struct rb_node **p = &proc->threads.rb_node;
+
+	while (*p) {
+		parent = *p;
+		thread = rb_entry(parent, struct binder_thread, rb_node);
+
+		if (current->pid < thread->pid)
+			p = &(*p)->rb_left;
+		else if (current->pid > thread->pid)
+			p = &(*p)->rb_right;
+		else
+			break;
+	}
+	if (*p == NULL) {
+		thread = kzalloc(sizeof(*thread), GFP_KERNEL);
+		if (thread == NULL)
+			return NULL;
+		binder_stats_created(BINDER_STAT_THREAD);
+		thread->proc = proc;
+		thread->pid = current->pid;
+		init_waitqueue_head(&thread->wait);
+		INIT_LIST_HEAD(&thread->todo);
+		rb_link_node(&thread->rb_node, parent, p);
+		rb_insert_color(&thread->rb_node, &proc->threads);
+		thread->looper |= BINDER_LOOPER_STATE_NEED_RETURN;
+		thread->return_error = BR_OK;
+		thread->return_error2 = BR_OK;
+	}
+	return thread;
+}
+
+static int binder_free_thread(struct binder_proc *proc,
+			      struct binder_thread *thread)
+{
+	struct binder_transaction *t;
+	struct binder_transaction *send_reply = NULL;
+	int active_transactions = 0;
+
+	rb_erase(&thread->rb_node, &proc->threads);
+	t = thread->transaction_stack;
+	if (t && t->to_thread == thread)
+		send_reply = t;
+	while (t) {
+		active_transactions++;
+		binder_debug(BINDER_DEBUG_DEAD_TRANSACTION,
+			     "binder: release %d:%d transaction %d "
+			     "%s, still active\n", proc->pid, thread->pid,
+			     t->debug_id,
+			     (t->to_thread == thread) ? "in" : "out");
+
+		if (t->to_thread == thread) {
+			t->to_proc = NULL;
+			t->to_thread = NULL;
+			if (t->buffer) {
+				t->buffer->transaction = NULL;
+				t->buffer = NULL;
+			}
+			t = t->to_parent;
+		} else if (t->from == thread) {
+			t->from = NULL;
+			t = t->from_parent;
+		} else
+			BUG();
+	}
+	if (send_reply)
+		binder_send_failed_reply(send_reply, BR_DEAD_REPLY);
+	binder_release_work(&thread->todo);
+	kfree(thread);
+	binder_stats_deleted(BINDER_STAT_THREAD);
+	return active_transactions;
+}
+
+static unsigned int binder_poll(struct file *filp,
+				struct poll_table_struct *wait)
+{
+	struct binder_proc *proc = filp->private_data;
+	struct binder_thread *thread = NULL;
+	int wait_for_proc_work;
+
+	mutex_lock(&binder_lock);
+	thread = binder_get_thread(proc);
+
+	wait_for_proc_work = thread->transaction_stack == NULL &&
+		list_empty(&thread->todo) && thread->return_error == BR_OK;
+	mutex_unlock(&binder_lock);
+
+	if (wait_for_proc_work) {
+		if (binder_has_proc_work(proc, thread))
+			return POLLIN;
+		poll_wait(filp, &proc->wait, wait);
+		if (binder_has_proc_work(proc, thread))
+			return POLLIN;
+	} else {
+		if (binder_has_thread_work(thread))
+			return POLLIN;
+		poll_wait(filp, &thread->wait, wait);
+		if (binder_has_thread_work(thread))
+			return POLLIN;
+	}
+	return 0;
+}
+
+static long binder_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	int ret;
+	struct binder_proc *proc = filp->private_data;
+	struct binder_thread *thread;
+	unsigned int size = _IOC_SIZE(cmd);
+	void __user *ubuf = (void __user *)arg;
+
+	/*printk(KERN_INFO "binder_ioctl: %d:%d %x %lx\n", proc->pid, current->pid, cmd, arg);*/
+
+	ret = wait_event_interruptible(binder_user_error_wait, binder_stop_on_user_error < 2);
+	if (ret)
+		return ret;
+
+	mutex_lock(&binder_lock);
+	thread = binder_get_thread(proc);
+	if (thread == NULL) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	switch (cmd) {
+	case BINDER_WRITE_READ: {
+		struct binder_write_read bwr;
+		if (size != sizeof(struct binder_write_read)) {
+			ret = -EINVAL;
+			goto err;
+		}
+		if (copy_from_user(&bwr, ubuf, sizeof(bwr))) {
+			ret = -EFAULT;
+			goto err;
+		}
+		binder_debug(BINDER_DEBUG_READ_WRITE,
+			     "binder: %d:%d write %ld at %08lx, read %ld at %08lx\n",
+			     proc->pid, thread->pid, bwr.write_size, bwr.write_buffer,
+			     bwr.read_size, bwr.read_buffer);
+
+		if (bwr.write_size > 0) {
+			ret = binder_thread_write(proc, thread, (void __user *)bwr.write_buffer, bwr.write_size, &bwr.write_consumed);
+			if (ret < 0) {
+				bwr.read_consumed = 0;
+				if (copy_to_user(ubuf, &bwr, sizeof(bwr)))
+					ret = -EFAULT;
+				goto err;
+			}
+		}
+		if (bwr.read_size > 0) {
+			ret = binder_thread_read(proc, thread, (void __user *)bwr.read_buffer, bwr.read_size, &bwr.read_consumed, filp->f_flags & O_NONBLOCK);
+			if (!list_empty(&proc->todo))
+				wake_up_interruptible(&proc->wait);
+			if (ret < 0) {
+				if (copy_to_user(ubuf, &bwr, sizeof(bwr)))
+					ret = -EFAULT;
+				goto err;
+			}
+		}
+		binder_debug(BINDER_DEBUG_READ_WRITE,
+			     "binder: %d:%d wrote %ld of %ld, read return %ld of %ld\n",
+			     proc->pid, thread->pid, bwr.write_consumed, bwr.write_size,
+			     bwr.read_consumed, bwr.read_size);
+		if (copy_to_user(ubuf, &bwr, sizeof(bwr))) {
+			ret = -EFAULT;
+			goto err;
+		}
+		break;
+	}
+	case BINDER_SET_MAX_THREADS:
+		if (copy_from_user(&proc->max_threads, ubuf, sizeof(proc->max_threads))) {
+			ret = -EINVAL;
+			goto err;
+		}
+		break;
+	case BINDER_SET_CONTEXT_MGR:
+		if (binder_context_mgr_node != NULL) {
+			printk(KERN_ERR "binder: BINDER_SET_CONTEXT_MGR already set\n");
+			ret = -EBUSY;
+			goto err;
+		}
+		if (binder_context_mgr_uid != -1) {
+			if (binder_context_mgr_uid != current->cred->euid) {
+				printk(KERN_ERR "binder: BINDER_SET_"
+				       "CONTEXT_MGR bad uid %d != %d\n",
+				       current->cred->euid,
+				       binder_context_mgr_uid);
+				ret = -EPERM;
+				goto err;
+			}
+		} else
+			binder_context_mgr_uid = current->cred->euid;
+		binder_context_mgr_node = binder_new_node(proc, NULL, NULL);
+		if (binder_context_mgr_node == NULL) {
+			ret = -ENOMEM;
+			goto err;
+		}
+		binder_context_mgr_node->local_weak_refs++;
+		binder_context_mgr_node->local_strong_refs++;
+		binder_context_mgr_node->has_strong_ref = 1;
+		binder_context_mgr_node->has_weak_ref = 1;
+		break;
+	case BINDER_THREAD_EXIT:
+		binder_debug(BINDER_DEBUG_THREADS, "binder: %d:%d exit\n",
+			     proc->pid, thread->pid);
+		binder_free_thread(proc, thread);
+		thread = NULL;
+		break;
+	case BINDER_VERSION:
+		if (size != sizeof(struct binder_version)) {
+			ret = -EINVAL;
+			goto err;
+		}
+		if (put_user(BINDER_CURRENT_PROTOCOL_VERSION, &((struct binder_version *)ubuf)->protocol_version)) {
+			ret = -EINVAL;
+			goto err;
+		}
+		break;
+	default:
+		ret = -EINVAL;
+		goto err;
+	}
+	ret = 0;
+err:
+	if (thread)
+		thread->looper &= ~BINDER_LOOPER_STATE_NEED_RETURN;
+	mutex_unlock(&binder_lock);
+	wait_event_interruptible(binder_user_error_wait, binder_stop_on_user_error < 2);
+	if (ret && ret != -ERESTARTSYS)
+		printk(KERN_INFO "binder: %d:%d ioctl %x %lx returned %d\n", proc->pid, current->pid, cmd, arg, ret);
+	return ret;
+}
+
+static void binder_vma_open(struct vm_area_struct *vma)
+{
+	struct binder_proc *proc = vma->vm_private_data;
+	binder_debug(BINDER_DEBUG_OPEN_CLOSE,
+		     "binder: %d open vm area %lx-%lx (%ld K) vma %lx pagep %lx\n",
+		     proc->pid, vma->vm_start, vma->vm_end,
+		     (vma->vm_end - vma->vm_start) / SZ_1K, vma->vm_flags,
+		     (unsigned long)pgprot_val(vma->vm_page_prot));
+	dump_stack();
+}
+
+static void binder_vma_close(struct vm_area_struct *vma)
+{
+	struct binder_proc *proc = vma->vm_private_data;
+	binder_debug(BINDER_DEBUG_OPEN_CLOSE,
+		     "binder: %d close vm area %lx-%lx (%ld K) vma %lx pagep %lx\n",
+		     proc->pid, vma->vm_start, vma->vm_end,
+		     (vma->vm_end - vma->vm_start) / SZ_1K, vma->vm_flags,
+		     (unsigned long)pgprot_val(vma->vm_page_prot));
+	proc->vma = NULL;
+	binder_defer_work(proc, BINDER_DEFERRED_PUT_FILES);
+}
+
+static struct vm_operations_struct binder_vm_ops = {
+	.open = binder_vma_open,
+	.close = binder_vma_close,
+};
+
+static int binder_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	int ret;
+	struct vm_struct *area;
+	struct binder_proc *proc = filp->private_data;
+	const char *failure_string;
+	struct binder_buffer *buffer;
+
+	if ((vma->vm_end - vma->vm_start) > SZ_4M)
+		vma->vm_end = vma->vm_start + SZ_4M;
+
+	binder_debug(BINDER_DEBUG_OPEN_CLOSE,
+		     "binder_mmap: %d %lx-%lx (%ld K) vma %lx pagep %lx\n",
+		     proc->pid, vma->vm_start, vma->vm_end,
+		     (vma->vm_end - vma->vm_start) / SZ_1K, vma->vm_flags,
+		     (unsigned long)pgprot_val(vma->vm_page_prot));
+
+	if (vma->vm_flags & FORBIDDEN_MMAP_FLAGS) {
+		ret = -EPERM;
+		failure_string = "bad vm_flags";
+		goto err_bad_arg;
+	}
+	vma->vm_flags = (vma->vm_flags | VM_DONTCOPY) & ~VM_MAYWRITE;
+
+	if (proc->buffer) {
+		ret = -EBUSY;
+		failure_string = "already mapped";
+		goto err_already_mapped;
+	}
+
+	area = get_vm_area(vma->vm_end - vma->vm_start, VM_IOREMAP);
+	if (area == NULL) {
+		ret = -ENOMEM;
+		failure_string = "get_vm_area";
+		goto err_get_vm_area_failed;
+	}
+	proc->buffer = area->addr;
+	proc->user_buffer_offset = vma->vm_start - (uintptr_t)proc->buffer;
+
+#ifdef CONFIG_CPU_CACHE_VIPT
+	if (cache_is_vipt_aliasing()) {
+		while (CACHE_COLOUR((vma->vm_start ^ (uint32_t)proc->buffer))) {
+			printk(KERN_INFO "binder_mmap: %d %lx-%lx maps %p bad alignment\n", proc->pid, vma->vm_start, vma->vm_end, proc->buffer);
+			vma->vm_start += PAGE_SIZE;
+		}
+	}
+#endif
+	proc->pages = kzalloc(sizeof(proc->pages[0]) * ((vma->vm_end - vma->vm_start) / PAGE_SIZE), GFP_KERNEL);
+	if (proc->pages == NULL) {
+		ret = -ENOMEM;
+		failure_string = "alloc page array";
+		goto err_alloc_pages_failed;
+	}
+	proc->buffer_size = vma->vm_end - vma->vm_start;
+
+	vma->vm_ops = &binder_vm_ops;
+	vma->vm_private_data = proc;
+
+	if (binder_update_page_range(proc, 1, proc->buffer, proc->buffer + PAGE_SIZE, vma)) {
+		ret = -ENOMEM;
+		failure_string = "alloc small buf";
+		goto err_alloc_small_buf_failed;
+	}
+	buffer = proc->buffer;
+	INIT_LIST_HEAD(&proc->buffers);
+	list_add(&buffer->entry, &proc->buffers);
+	buffer->free = 1;
+	binder_insert_free_buffer(proc, buffer);
+	proc->free_async_space = proc->buffer_size / 2;
+	barrier();
+	proc->files = get_files_struct(current);
+	proc->vma = vma;
+
+	/*printk(KERN_INFO "binder_mmap: %d %lx-%lx maps %p\n",
+		 proc->pid, vma->vm_start, vma->vm_end, proc->buffer);*/
+	return 0;
+
+err_alloc_small_buf_failed:
+	kfree(proc->pages);
+	proc->pages = NULL;
+err_alloc_pages_failed:
+	vfree(proc->buffer);
+	proc->buffer = NULL;
+err_get_vm_area_failed:
+err_already_mapped:
+err_bad_arg:
+	printk(KERN_ERR "binder_mmap: %d %lx-%lx %s failed %d\n",
+	       proc->pid, vma->vm_start, vma->vm_end, failure_string, ret);
+	return ret;
+}
+
+static int binder_open(struct inode *nodp, struct file *filp)
+{
+	struct binder_proc *proc;
+
+	binder_debug(BINDER_DEBUG_OPEN_CLOSE, "binder_open: %d:%d\n",
+		     current->group_leader->pid, current->pid);
+
+	proc = kzalloc(sizeof(*proc), GFP_KERNEL);
+	if (proc == NULL)
+		return -ENOMEM;
+	get_task_struct(current);
+	proc->tsk = current;
+	INIT_LIST_HEAD(&proc->todo);
+	init_waitqueue_head(&proc->wait);
+	proc->default_priority = task_nice(current);
+	mutex_lock(&binder_lock);
+	binder_stats_created(BINDER_STAT_PROC);
+	hlist_add_head(&proc->proc_node, &binder_procs);
+	proc->pid = current->group_leader->pid;
+	INIT_LIST_HEAD(&proc->delivered_death);
+	filp->private_data = proc;
+	mutex_unlock(&binder_lock);
+
+	if (binder_debugfs_dir_entry_proc) {
+		char strbuf[11];
+		snprintf(strbuf, sizeof(strbuf), "%u", proc->pid);
+		proc->debugfs_entry = debugfs_create_file(strbuf, S_IRUGO,
+			binder_debugfs_dir_entry_proc, proc, &binder_proc_fops);
+	}
+
+	return 0;
+}
+
+static int binder_flush(struct file *filp, fl_owner_t id)
+{
+	struct binder_proc *proc = filp->private_data;
+
+	binder_defer_work(proc, BINDER_DEFERRED_FLUSH);
+
+	return 0;
+}
+
+static void binder_deferred_flush(struct binder_proc *proc)
+{
+	struct rb_node *n;
+	int wake_count = 0;
+	for (n = rb_first(&proc->threads); n != NULL; n = rb_next(n)) {
+		struct binder_thread *thread = rb_entry(n, struct binder_thread, rb_node);
+		thread->looper |= BINDER_LOOPER_STATE_NEED_RETURN;
+		if (thread->looper & BINDER_LOOPER_STATE_WAITING) {
+			wake_up_interruptible(&thread->wait);
+			wake_count++;
+		}
+	}
+	wake_up_interruptible_all(&proc->wait);
+
+	binder_debug(BINDER_DEBUG_OPEN_CLOSE,
+		     "binder_flush: %d woke %d threads\n", proc->pid,
+		     wake_count);
+}
+
+static int binder_release(struct inode *nodp, struct file *filp)
+{
+	struct binder_proc *proc = filp->private_data;
+	debugfs_remove(proc->debugfs_entry);
+	binder_defer_work(proc, BINDER_DEFERRED_RELEASE);
+
+	return 0;
+}
+
+static void binder_deferred_release(struct binder_proc *proc)
+{
+	struct hlist_node *pos;
+	struct binder_transaction *t;
+	struct rb_node *n;
+	int threads, nodes, incoming_refs, outgoing_refs, buffers, active_transactions, page_count;
+
+	BUG_ON(proc->vma);
+	BUG_ON(proc->files);
+
+	hlist_del(&proc->proc_node);
+	if (binder_context_mgr_node && binder_context_mgr_node->proc == proc) {
+		binder_debug(BINDER_DEBUG_DEAD_BINDER,
+			     "binder_release: %d context_mgr_node gone\n",
+			     proc->pid);
+		binder_context_mgr_node = NULL;
+	}
+
+	threads = 0;
+	active_transactions = 0;
+	while ((n = rb_first(&proc->threads))) {
+		struct binder_thread *thread = rb_entry(n, struct binder_thread, rb_node);
+		threads++;
+		active_transactions += binder_free_thread(proc, thread);
+	}
+	nodes = 0;
+	incoming_refs = 0;
+	while ((n = rb_first(&proc->nodes))) {
+		struct binder_node *node = rb_entry(n, struct binder_node, rb_node);
+
+		nodes++;
+		rb_erase(&node->rb_node, &proc->nodes);
+		list_del_init(&node->work.entry);
+		if (hlist_empty(&node->refs)) {
+			kfree(node);
+			binder_stats_deleted(BINDER_STAT_NODE);
+		} else {
+			struct binder_ref *ref;
+			int death = 0;
+
+			node->proc = NULL;
+			node->local_strong_refs = 0;
+			node->local_weak_refs = 0;
+			hlist_add_head(&node->dead_node, &binder_dead_nodes);
+
+			hlist_for_each_entry(ref, pos, &node->refs, node_entry) {
+				incoming_refs++;
+				if (ref->death) {
+					death++;
+					if (list_empty(&ref->death->work.entry)) {
+						ref->death->work.type = BINDER_WORK_DEAD_BINDER;
+						list_add_tail(&ref->death->work.entry, &ref->proc->todo);
+						wake_up_interruptible(&ref->proc->wait);
+					} else
+						BUG();
+				}
+			}
+			binder_debug(BINDER_DEBUG_DEAD_BINDER,
+				     "binder: node %d now dead, "
+				     "refs %d, death %d\n", node->debug_id,
+				     incoming_refs, death);
+		}
+	}
+	outgoing_refs = 0;
+	while ((n = rb_first(&proc->refs_by_desc))) {
+		struct binder_ref *ref = rb_entry(n, struct binder_ref,
+						  rb_node_desc);
+		outgoing_refs++;
+		binder_delete_ref(ref);
+	}
+	binder_release_work(&proc->todo);
+	buffers = 0;
+
+	while ((n = rb_first(&proc->allocated_buffers))) {
+		struct binder_buffer *buffer = rb_entry(n, struct binder_buffer,
+							rb_node);
+		t = buffer->transaction;
+		if (t) {
+			t->buffer = NULL;
+			buffer->transaction = NULL;
+			printk(KERN_ERR "binder: release proc %d, "
+			       "transaction %d, not freed\n",
+			       proc->pid, t->debug_id);
+			/*BUG();*/
+		}
+		binder_free_buf(proc, buffer);
+		buffers++;
+	}
+
+	binder_stats_deleted(BINDER_STAT_PROC);
+
+	page_count = 0;
+	if (proc->pages) {
+		int i;
+		for (i = 0; i < proc->buffer_size / PAGE_SIZE; i++) {
+			if (proc->pages[i]) {
+				void *page_addr = proc->buffer + i * PAGE_SIZE;
+				binder_debug(BINDER_DEBUG_BUFFER_ALLOC,
+					     "binder_release: %d: "
+					     "page %d at %p not freed\n",
+					     proc->pid, i,
+					     page_addr);
+				unmap_kernel_range((unsigned long)page_addr,
+					PAGE_SIZE);
+				__free_page(proc->pages[i]);
+				page_count++;
+			}
+		}
+		kfree(proc->pages);
+		vfree(proc->buffer);
+	}
+
+	put_task_struct(proc->tsk);
+
+	binder_debug(BINDER_DEBUG_OPEN_CLOSE,
+		     "binder_release: %d threads %d, nodes %d (ref %d), "
+		     "refs %d, active transactions %d, buffers %d, "
+		     "pages %d\n",
+		     proc->pid, threads, nodes, incoming_refs, outgoing_refs,
+		     active_transactions, buffers, page_count);
+
+	kfree(proc);
+}
+
+static void binder_deferred_func(struct work_struct *work)
+{
+	struct binder_proc *proc;
+	struct files_struct *files;
+
+	int defer;
+	do {
+		mutex_lock(&binder_lock);
+		mutex_lock(&binder_deferred_lock);
+		if (!hlist_empty(&binder_deferred_list)) {
+			proc = hlist_entry(binder_deferred_list.first,
+					struct binder_proc, deferred_work_node);
+			hlist_del_init(&proc->deferred_work_node);
+			defer = proc->deferred_work;
+			proc->deferred_work = 0;
+		} else {
+			proc = NULL;
+			defer = 0;
+		}
+		mutex_unlock(&binder_deferred_lock);
+
+		files = NULL;
+		if (defer & BINDER_DEFERRED_PUT_FILES) {
+			files = proc->files;
+			if (files)
+				proc->files = NULL;
+		}
+
+		if (defer & BINDER_DEFERRED_FLUSH)
+			binder_deferred_flush(proc);
+
+		if (defer & BINDER_DEFERRED_RELEASE)
+			binder_deferred_release(proc); /* frees proc */
+
+		mutex_unlock(&binder_lock);
+		if (files)
+			put_files_struct(files);
+	} while (proc);
+}
+static DECLARE_WORK(binder_deferred_work, binder_deferred_func);
+
+static void
+binder_defer_work(struct binder_proc *proc, enum binder_deferred_state defer)
+{
+	mutex_lock(&binder_deferred_lock);
+	proc->deferred_work |= defer;
+	if (hlist_unhashed(&proc->deferred_work_node)) {
+		hlist_add_head(&proc->deferred_work_node,
+				&binder_deferred_list);
+		queue_work(binder_deferred_workqueue, &binder_deferred_work);
+	}
+	mutex_unlock(&binder_deferred_lock);
+}
+
+static void print_binder_transaction(struct seq_file *m, const char *prefix,
+				     struct binder_transaction *t)
+{
+	seq_printf(m,
+		   "%s %d: %p from %d:%d to %d:%d code %x flags %x pri %ld r%d",
+		   prefix, t->debug_id, t,
+		   t->from ? t->from->proc->pid : 0,
+		   t->from ? t->from->pid : 0,
+		   t->to_proc ? t->to_proc->pid : 0,
+		   t->to_thread ? t->to_thread->pid : 0,
+		   t->code, t->flags, t->priority, t->need_reply);
+	if (t->buffer == NULL) {
+		seq_puts(m, " buffer free\n");
+		return;
+	}
+	if (t->buffer->target_node)
+		seq_printf(m, " node %d",
+			   t->buffer->target_node->debug_id);
+	seq_printf(m, " size %zd:%zd data %p\n",
+		   t->buffer->data_size, t->buffer->offsets_size,
+		   t->buffer->data);
+}
+
+static void print_binder_buffer(struct seq_file *m, const char *prefix,
+				struct binder_buffer *buffer)
+{
+	seq_printf(m, "%s %d: %p size %zd:%zd %s\n",
+		   prefix, buffer->debug_id, buffer->data,
+		   buffer->data_size, buffer->offsets_size,
+		   buffer->transaction ? "active" : "delivered");
+}
+
+static void print_binder_work(struct seq_file *m, const char *prefix,
+			      const char *transaction_prefix,
+			      struct binder_work *w)
+{
+	struct binder_node *node;
+	struct binder_transaction *t;
+
+	switch (w->type) {
+	case BINDER_WORK_TRANSACTION:
+		t = container_of(w, struct binder_transaction, work);
+		print_binder_transaction(m, transaction_prefix, t);
+		break;
+	case BINDER_WORK_TRANSACTION_COMPLETE:
+		seq_printf(m, "%stransaction complete\n", prefix);
+		break;
+	case BINDER_WORK_NODE:
+		node = container_of(w, struct binder_node, work);
+		seq_printf(m, "%snode work %d: u%p c%p\n",
+			   prefix, node->debug_id, node->ptr, node->cookie);
+		break;
+	case BINDER_WORK_DEAD_BINDER:
+		seq_printf(m, "%shas dead binder\n", prefix);
+		break;
+	case BINDER_WORK_DEAD_BINDER_AND_CLEAR:
+		seq_printf(m, "%shas cleared dead binder\n", prefix);
+		break;
+	case BINDER_WORK_CLEAR_DEATH_NOTIFICATION:
+		seq_printf(m, "%shas cleared death notification\n", prefix);
+		break;
+	default:
+		seq_printf(m, "%sunknown work: type %d\n", prefix, w->type);
+		break;
+	}
+}
+
+static void print_binder_thread(struct seq_file *m,
+				struct binder_thread *thread,
+				int print_always)
+{
+	struct binder_transaction *t;
+	struct binder_work *w;
+	size_t start_pos = m->count;
+	size_t header_pos;
+
+	seq_printf(m, "  thread %d: l %02x\n", thread->pid, thread->looper);
+	header_pos = m->count;
+	t = thread->transaction_stack;
+	while (t) {
+		if (t->from == thread) {
+			print_binder_transaction(m,
+						 "    outgoing transaction", t);
+			t = t->from_parent;
+		} else if (t->to_thread == thread) {
+			print_binder_transaction(m,
+						 "    incoming transaction", t);
+			t = t->to_parent;
+		} else {
+			print_binder_transaction(m, "    bad transaction", t);
+			t = NULL;
+		}
+	}
+	list_for_each_entry(w, &thread->todo, entry) {
+		print_binder_work(m, "    ", "    pending transaction", w);
+	}
+	if (!print_always && m->count == header_pos)
+		m->count = start_pos;
+}
+
+static void print_binder_node(struct seq_file *m, struct binder_node *node)
+{
+	struct binder_ref *ref;
+	struct hlist_node *pos;
+	struct binder_work *w;
+	int count;
+
+	count = 0;
+	hlist_for_each_entry(ref, pos, &node->refs, node_entry)
+		count++;
+
+	seq_printf(m, "  node %d: u%p c%p hs %d hw %d ls %d lw %d is %d iw %d",
+		   node->debug_id, node->ptr, node->cookie,
+		   node->has_strong_ref, node->has_weak_ref,
+		   node->local_strong_refs, node->local_weak_refs,
+		   node->internal_strong_refs, count);
+	if (count) {
+		seq_puts(m, " proc");
+		hlist_for_each_entry(ref, pos, &node->refs, node_entry)
+			seq_printf(m, " %d", ref->proc->pid);
+	}
+	seq_puts(m, "\n");
+	list_for_each_entry(w, &node->async_todo, entry)
+		print_binder_work(m, "    ",
+				  "    pending async transaction", w);
+}
+
+static void print_binder_ref(struct seq_file *m, struct binder_ref *ref)
+{
+	seq_printf(m, "  ref %d: desc %d %snode %d s %d w %d d %p\n",
+		   ref->debug_id, ref->desc, ref->node->proc ? "" : "dead ",
+		   ref->node->debug_id, ref->strong, ref->weak, ref->death);
+}
+
+static void print_binder_proc(struct seq_file *m,
+			      struct binder_proc *proc, int print_all)
+{
+	struct binder_work *w;
+	struct rb_node *n;
+	size_t start_pos = m->count;
+	size_t header_pos;
+
+	seq_printf(m, "proc %d\n", proc->pid);
+	header_pos = m->count;
+
+	for (n = rb_first(&proc->threads); n != NULL; n = rb_next(n))
+		print_binder_thread(m, rb_entry(n, struct binder_thread,
+						rb_node), print_all);
+	for (n = rb_first(&proc->nodes); n != NULL; n = rb_next(n)) {
+		struct binder_node *node = rb_entry(n, struct binder_node,
+						    rb_node);
+		if (print_all || node->has_async_transaction)
+			print_binder_node(m, node);
+	}
+	if (print_all) {
+		for (n = rb_first(&proc->refs_by_desc);
+		     n != NULL;
+		     n = rb_next(n))
+			print_binder_ref(m, rb_entry(n, struct binder_ref,
+						     rb_node_desc));
+	}
+	for (n = rb_first(&proc->allocated_buffers); n != NULL; n = rb_next(n))
+		print_binder_buffer(m, "  buffer",
+				    rb_entry(n, struct binder_buffer, rb_node));
+	list_for_each_entry(w, &proc->todo, entry)
+		print_binder_work(m, "  ", "  pending transaction", w);
+	list_for_each_entry(w, &proc->delivered_death, entry) {
+		seq_puts(m, "  has delivered dead binder\n");
+		break;
+	}
+	if (!print_all && m->count == header_pos)
+		m->count = start_pos;
+}
+
+static const char *binder_return_strings[] = {
+	"BR_ERROR",
+	"BR_OK",
+	"BR_TRANSACTION",
+	"BR_REPLY",
+	"BR_ACQUIRE_RESULT",
+	"BR_DEAD_REPLY",
+	"BR_TRANSACTION_COMPLETE",
+	"BR_INCREFS",
+	"BR_ACQUIRE",
+	"BR_RELEASE",
+	"BR_DECREFS",
+	"BR_ATTEMPT_ACQUIRE",
+	"BR_NOOP",
+	"BR_SPAWN_LOOPER",
+	"BR_FINISHED",
+	"BR_DEAD_BINDER",
+	"BR_CLEAR_DEATH_NOTIFICATION_DONE",
+	"BR_FAILED_REPLY"
+};
+
+static const char *binder_command_strings[] = {
+	"BC_TRANSACTION",
+	"BC_REPLY",
+	"BC_ACQUIRE_RESULT",
+	"BC_FREE_BUFFER",
+	"BC_INCREFS",
+	"BC_ACQUIRE",
+	"BC_RELEASE",
+	"BC_DECREFS",
+	"BC_INCREFS_DONE",
+	"BC_ACQUIRE_DONE",
+	"BC_ATTEMPT_ACQUIRE",
+	"BC_REGISTER_LOOPER",
+	"BC_ENTER_LOOPER",
+	"BC_EXIT_LOOPER",
+	"BC_REQUEST_DEATH_NOTIFICATION",
+	"BC_CLEAR_DEATH_NOTIFICATION",
+	"BC_DEAD_BINDER_DONE"
+};
+
+static const char *binder_objstat_strings[] = {
+	"proc",
+	"thread",
+	"node",
+	"ref",
+	"death",
+	"transaction",
+	"transaction_complete"
+};
+
+static void print_binder_stats(struct seq_file *m, const char *prefix,
+			       struct binder_stats *stats)
+{
+	int i;
+
+	BUILD_BUG_ON(ARRAY_SIZE(stats->bc) !=
+		     ARRAY_SIZE(binder_command_strings));
+	for (i = 0; i < ARRAY_SIZE(stats->bc); i++) {
+		if (stats->bc[i])
+			seq_printf(m, "%s%s: %d\n", prefix,
+				   binder_command_strings[i], stats->bc[i]);
+	}
+
+	BUILD_BUG_ON(ARRAY_SIZE(stats->br) !=
+		     ARRAY_SIZE(binder_return_strings));
+	for (i = 0; i < ARRAY_SIZE(stats->br); i++) {
+		if (stats->br[i])
+			seq_printf(m, "%s%s: %d\n", prefix,
+				   binder_return_strings[i], stats->br[i]);
+	}
+
+	BUILD_BUG_ON(ARRAY_SIZE(stats->obj_created) !=
+		     ARRAY_SIZE(binder_objstat_strings));
+	BUILD_BUG_ON(ARRAY_SIZE(stats->obj_created) !=
+		     ARRAY_SIZE(stats->obj_deleted));
+	for (i = 0; i < ARRAY_SIZE(stats->obj_created); i++) {
+		if (stats->obj_created[i] || stats->obj_deleted[i])
+			seq_printf(m, "%s%s: active %d total %d\n", prefix,
+				binder_objstat_strings[i],
+				stats->obj_created[i] - stats->obj_deleted[i],
+				stats->obj_created[i]);
+	}
+}
+
+static void print_binder_proc_stats(struct seq_file *m,
+				    struct binder_proc *proc)
+{
+	struct binder_work *w;
+	struct rb_node *n;
+	int count, strong, weak;
+
+	seq_printf(m, "proc %d\n", proc->pid);
+	count = 0;
+	for (n = rb_first(&proc->threads); n != NULL; n = rb_next(n))
+		count++;
+	seq_printf(m, "  threads: %d\n", count);
+	seq_printf(m, "  requested threads: %d+%d/%d\n"
+			"  ready threads %d\n"
+			"  free async space %zd\n", proc->requested_threads,
+			proc->requested_threads_started, proc->max_threads,
+			proc->ready_threads, proc->free_async_space);
+	count = 0;
+	for (n = rb_first(&proc->nodes); n != NULL; n = rb_next(n))
+		count++;
+	seq_printf(m, "  nodes: %d\n", count);
+	count = 0;
+	strong = 0;
+	weak = 0;
+	for (n = rb_first(&proc->refs_by_desc); n != NULL; n = rb_next(n)) {
+		struct binder_ref *ref = rb_entry(n, struct binder_ref,
+						  rb_node_desc);
+		count++;
+		strong += ref->strong;
+		weak += ref->weak;
+	}
+	seq_printf(m, "  refs: %d s %d w %d\n", count, strong, weak);
+
+	count = 0;
+	for (n = rb_first(&proc->allocated_buffers); n != NULL; n = rb_next(n))
+		count++;
+	seq_printf(m, "  buffers: %d\n", count);
+
+	count = 0;
+	list_for_each_entry(w, &proc->todo, entry) {
+		switch (w->type) {
+		case BINDER_WORK_TRANSACTION:
+			count++;
+			break;
+		default:
+			break;
+		}
+	}
+	seq_printf(m, "  pending transactions: %d\n", count);
+
+	print_binder_stats(m, "  ", &proc->stats);
+}
+
+
+static int binder_state_show(struct seq_file *m, void *unused)
+{
+	struct binder_proc *proc;
+	struct hlist_node *pos;
+	struct binder_node *node;
+	int do_lock = !binder_debug_no_lock;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+
+	seq_puts(m, "binder state:\n");
+
+	if (!hlist_empty(&binder_dead_nodes))
+		seq_puts(m, "dead nodes:\n");
+	hlist_for_each_entry(node, pos, &binder_dead_nodes, dead_node)
+		print_binder_node(m, node);
+
+	hlist_for_each_entry(proc, pos, &binder_procs, proc_node)
+		print_binder_proc(m, proc, 1);
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+	return 0;
+}
+
+static int binder_stats_show(struct seq_file *m, void *unused)
+{
+	struct binder_proc *proc;
+	struct hlist_node *pos;
+	int do_lock = !binder_debug_no_lock;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+
+	seq_puts(m, "binder stats:\n");
+
+	print_binder_stats(m, "", &binder_stats);
+
+	hlist_for_each_entry(proc, pos, &binder_procs, proc_node)
+		print_binder_proc_stats(m, proc);
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+	return 0;
+}
+
+static int binder_transactions_show(struct seq_file *m, void *unused)
+{
+	struct binder_proc *proc;
+	struct hlist_node *pos;
+	int do_lock = !binder_debug_no_lock;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+
+	seq_puts(m, "binder transactions:\n");
+	hlist_for_each_entry(proc, pos, &binder_procs, proc_node)
+		print_binder_proc(m, proc, 0);
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+	return 0;
+}
+
+static int binder_proc_show(struct seq_file *m, void *unused)
+{
+	struct binder_proc *proc = m->private;
+	int do_lock = !binder_debug_no_lock;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+	seq_puts(m, "binder proc state:\n");
+	print_binder_proc(m, proc, 1);
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+	return 0;
+}
+
+static void print_binder_transaction_log_entry(struct seq_file *m,
+					struct binder_transaction_log_entry *e)
+{
+	seq_printf(m,
+		   "%d: %s from %d:%d to %d:%d node %d handle %d size %d:%d\n",
+		   e->debug_id, (e->call_type == 2) ? "reply" :
+		   ((e->call_type == 1) ? "async" : "call "), e->from_proc,
+		   e->from_thread, e->to_proc, e->to_thread, e->to_node,
+		   e->target_handle, e->data_size, e->offsets_size);
+}
+
+static int binder_transaction_log_show(struct seq_file *m, void *unused)
+{
+	struct binder_transaction_log *log = m->private;
+	int i;
+
+	if (log->full) {
+		for (i = log->next; i < ARRAY_SIZE(log->entry); i++)
+			print_binder_transaction_log_entry(m, &log->entry[i]);
+	}
+	for (i = 0; i < log->next; i++)
+		print_binder_transaction_log_entry(m, &log->entry[i]);
+	return 0;
+}
+
+static const struct file_operations binder_fops = {
+	.owner = THIS_MODULE,
+	.poll = binder_poll,
+	.unlocked_ioctl = binder_ioctl,
+	.mmap = binder_mmap,
+	.open = binder_open,
+	.flush = binder_flush,
+	.release = binder_release,
+};
+
+static struct miscdevice binder_miscdev = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "binder",
+	.fops = &binder_fops
+};
+
+BINDER_DEBUG_ENTRY(state);
+BINDER_DEBUG_ENTRY(stats);
+BINDER_DEBUG_ENTRY(transactions);
+BINDER_DEBUG_ENTRY(transaction_log);
+
+static int __init binder_init(void)
+{
+	int ret;
+
+	binder_deferred_workqueue = create_singlethread_workqueue("binder");
+	if (!binder_deferred_workqueue)
+		return -ENOMEM;
+
+	binder_debugfs_dir_entry_root = debugfs_create_dir("binder", NULL);
+	if (binder_debugfs_dir_entry_root)
+		binder_debugfs_dir_entry_proc = debugfs_create_dir("proc",
+						 binder_debugfs_dir_entry_root);
+	ret = misc_register(&binder_miscdev);
+	if (binder_debugfs_dir_entry_root) {
+		debugfs_create_file("state",
+				    S_IRUGO,
+				    binder_debugfs_dir_entry_root,
+				    NULL,
+				    &binder_state_fops);
+		debugfs_create_file("stats",
+				    S_IRUGO,
+				    binder_debugfs_dir_entry_root,
+				    NULL,
+				    &binder_stats_fops);
+		debugfs_create_file("transactions",
+				    S_IRUGO,
+				    binder_debugfs_dir_entry_root,
+				    NULL,
+				    &binder_transactions_fops);
+		debugfs_create_file("transaction_log",
+				    S_IRUGO,
+				    binder_debugfs_dir_entry_root,
+				    &binder_transaction_log,
+				    &binder_transaction_log_fops);
+		debugfs_create_file("failed_transaction_log",
+				    S_IRUGO,
+				    binder_debugfs_dir_entry_root,
+				    &binder_transaction_log_failed,
+				    &binder_transaction_log_fops);
+	}
+	return ret;
+}
+
+device_initcall(binder_init);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/android/binder.h b/drivers/staging/android/binder.h
new file mode 100644
index 000000000000..863ae1ad5d55
--- /dev/null
+++ b/drivers/staging/android/binder.h
@@ -0,0 +1,330 @@
+/*
+ * Copyright (C) 2008 Google, Inc.
+ *
+ * Based on, but no longer compatible with, the original
+ * OpenBinder.org binder driver interface, which is:
+ *
+ * Copyright (c) 2005 Palmsource, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 _LINUX_BINDER_H
+#define _LINUX_BINDER_H
+
+#include <linux/ioctl.h>
+
+#define B_PACK_CHARS(c1, c2, c3, c4) \
+	((((c1)<<24)) | (((c2)<<16)) | (((c3)<<8)) | (c4))
+#define B_TYPE_LARGE 0x85
+
+enum {
+	BINDER_TYPE_BINDER	= B_PACK_CHARS('s', 'b', '*', B_TYPE_LARGE),
+	BINDER_TYPE_WEAK_BINDER	= B_PACK_CHARS('w', 'b', '*', B_TYPE_LARGE),
+	BINDER_TYPE_HANDLE	= B_PACK_CHARS('s', 'h', '*', B_TYPE_LARGE),
+	BINDER_TYPE_WEAK_HANDLE	= B_PACK_CHARS('w', 'h', '*', B_TYPE_LARGE),
+	BINDER_TYPE_FD		= B_PACK_CHARS('f', 'd', '*', B_TYPE_LARGE),
+};
+
+enum {
+	FLAT_BINDER_FLAG_PRIORITY_MASK = 0xff,
+	FLAT_BINDER_FLAG_ACCEPTS_FDS = 0x100,
+};
+
+/*
+ * This is the flattened representation of a Binder object for transfer
+ * between processes.  The 'offsets' supplied as part of a binder transaction
+ * contains offsets into the data where these structures occur.  The Binder
+ * driver takes care of re-writing the structure type and data as it moves
+ * between processes.
+ */
+struct flat_binder_object {
+	/* 8 bytes for large_flat_header. */
+	unsigned long		type;
+	unsigned long		flags;
+
+	/* 8 bytes of data. */
+	union {
+		void		*binder;	/* local object */
+		signed long	handle;		/* remote object */
+	};
+
+	/* extra data associated with local object */
+	void			*cookie;
+};
+
+/*
+ * On 64-bit platforms where user code may run in 32-bits the driver must
+ * translate the buffer (and local binder) addresses apropriately.
+ */
+
+struct binder_write_read {
+	signed long	write_size;	/* bytes to write */
+	signed long	write_consumed;	/* bytes consumed by driver */
+	unsigned long	write_buffer;
+	signed long	read_size;	/* bytes to read */
+	signed long	read_consumed;	/* bytes consumed by driver */
+	unsigned long	read_buffer;
+};
+
+/* Use with BINDER_VERSION, driver fills in fields. */
+struct binder_version {
+	/* driver protocol version -- increment with incompatible change */
+	signed long	protocol_version;
+};
+
+/* This is the current protocol version. */
+#define BINDER_CURRENT_PROTOCOL_VERSION 7
+
+#define BINDER_WRITE_READ   		_IOWR('b', 1, struct binder_write_read)
+#define	BINDER_SET_IDLE_TIMEOUT		_IOW('b', 3, int64_t)
+#define	BINDER_SET_MAX_THREADS		_IOW('b', 5, size_t)
+#define	BINDER_SET_IDLE_PRIORITY	_IOW('b', 6, int)
+#define	BINDER_SET_CONTEXT_MGR		_IOW('b', 7, int)
+#define	BINDER_THREAD_EXIT		_IOW('b', 8, int)
+#define BINDER_VERSION			_IOWR('b', 9, struct binder_version)
+
+/*
+ * NOTE: Two special error codes you should check for when calling
+ * in to the driver are:
+ *
+ * EINTR -- The operation has been interupted.  This should be
+ * handled by retrying the ioctl() until a different error code
+ * is returned.
+ *
+ * ECONNREFUSED -- The driver is no longer accepting operations
+ * from your process.  That is, the process is being destroyed.
+ * You should handle this by exiting from your process.  Note
+ * that once this error code is returned, all further calls to
+ * the driver from any thread will return this same code.
+ */
+
+enum transaction_flags {
+	TF_ONE_WAY	= 0x01,	/* this is a one-way call: async, no return */
+	TF_ROOT_OBJECT	= 0x04,	/* contents are the component's root object */
+	TF_STATUS_CODE	= 0x08,	/* contents are a 32-bit status code */
+	TF_ACCEPT_FDS	= 0x10,	/* allow replies with file descriptors */
+};
+
+struct binder_transaction_data {
+	/* The first two are only used for bcTRANSACTION and brTRANSACTION,
+	 * identifying the target and contents of the transaction.
+	 */
+	union {
+		size_t	handle;	/* target descriptor of command transaction */
+		void	*ptr;	/* target descriptor of return transaction */
+	} target;
+	void		*cookie;	/* target object cookie */
+	unsigned int	code;		/* transaction command */
+
+	/* General information about the transaction. */
+	unsigned int	flags;
+	pid_t		sender_pid;
+	uid_t		sender_euid;
+	size_t		data_size;	/* number of bytes of data */
+	size_t		offsets_size;	/* number of bytes of offsets */
+
+	/* If this transaction is inline, the data immediately
+	 * follows here; otherwise, it ends with a pointer to
+	 * the data buffer.
+	 */
+	union {
+		struct {
+			/* transaction data */
+			const void	*buffer;
+			/* offsets from buffer to flat_binder_object structs */
+			const void	*offsets;
+		} ptr;
+		uint8_t	buf[8];
+	} data;
+};
+
+struct binder_ptr_cookie {
+	void *ptr;
+	void *cookie;
+};
+
+struct binder_pri_desc {
+	int priority;
+	int desc;
+};
+
+struct binder_pri_ptr_cookie {
+	int priority;
+	void *ptr;
+	void *cookie;
+};
+
+enum BinderDriverReturnProtocol {
+	BR_ERROR = _IOR('r', 0, int),
+	/*
+	 * int: error code
+	 */
+
+	BR_OK = _IO('r', 1),
+	/* No parameters! */
+
+	BR_TRANSACTION = _IOR('r', 2, struct binder_transaction_data),
+	BR_REPLY = _IOR('r', 3, struct binder_transaction_data),
+	/*
+	 * binder_transaction_data: the received command.
+	 */
+
+	BR_ACQUIRE_RESULT = _IOR('r', 4, int),
+	/*
+	 * not currently supported
+	 * int: 0 if the last bcATTEMPT_ACQUIRE was not successful.
+	 * Else the remote object has acquired a primary reference.
+	 */
+
+	BR_DEAD_REPLY = _IO('r', 5),
+	/*
+	 * The target of the last transaction (either a bcTRANSACTION or
+	 * a bcATTEMPT_ACQUIRE) is no longer with us.  No parameters.
+	 */
+
+	BR_TRANSACTION_COMPLETE = _IO('r', 6),
+	/*
+	 * No parameters... always refers to the last transaction requested
+	 * (including replies).  Note that this will be sent even for
+	 * asynchronous transactions.
+	 */
+
+	BR_INCREFS = _IOR('r', 7, struct binder_ptr_cookie),
+	BR_ACQUIRE = _IOR('r', 8, struct binder_ptr_cookie),
+	BR_RELEASE = _IOR('r', 9, struct binder_ptr_cookie),
+	BR_DECREFS = _IOR('r', 10, struct binder_ptr_cookie),
+	/*
+	 * void *:	ptr to binder
+	 * void *: cookie for binder
+	 */
+
+	BR_ATTEMPT_ACQUIRE = _IOR('r', 11, struct binder_pri_ptr_cookie),
+	/*
+	 * not currently supported
+	 * int:	priority
+	 * void *: ptr to binder
+	 * void *: cookie for binder
+	 */
+
+	BR_NOOP = _IO('r', 12),
+	/*
+	 * No parameters.  Do nothing and examine the next command.  It exists
+	 * primarily so that we can replace it with a BR_SPAWN_LOOPER command.
+	 */
+
+	BR_SPAWN_LOOPER = _IO('r', 13),
+	/*
+	 * No parameters.  The driver has determined that a process has no
+	 * threads waiting to service incomming transactions.  When a process
+	 * receives this command, it must spawn a new service thread and
+	 * register it via bcENTER_LOOPER.
+	 */
+
+	BR_FINISHED = _IO('r', 14),
+	/*
+	 * not currently supported
+	 * stop threadpool thread
+	 */
+
+	BR_DEAD_BINDER = _IOR('r', 15, void *),
+	/*
+	 * void *: cookie
+	 */
+	BR_CLEAR_DEATH_NOTIFICATION_DONE = _IOR('r', 16, void *),
+	/*
+	 * void *: cookie
+	 */
+
+	BR_FAILED_REPLY = _IO('r', 17),
+	/*
+	 * The the last transaction (either a bcTRANSACTION or
+	 * a bcATTEMPT_ACQUIRE) failed (e.g. out of memory).  No parameters.
+	 */
+};
+
+enum BinderDriverCommandProtocol {
+	BC_TRANSACTION = _IOW('c', 0, struct binder_transaction_data),
+	BC_REPLY = _IOW('c', 1, struct binder_transaction_data),
+	/*
+	 * binder_transaction_data: the sent command.
+	 */
+
+	BC_ACQUIRE_RESULT = _IOW('c', 2, int),
+	/*
+	 * not currently supported
+	 * int:  0 if the last BR_ATTEMPT_ACQUIRE was not successful.
+	 * Else you have acquired a primary reference on the object.
+	 */
+
+	BC_FREE_BUFFER = _IOW('c', 3, int),
+	/*
+	 * void *: ptr to transaction data received on a read
+	 */
+
+	BC_INCREFS = _IOW('c', 4, int),
+	BC_ACQUIRE = _IOW('c', 5, int),
+	BC_RELEASE = _IOW('c', 6, int),
+	BC_DECREFS = _IOW('c', 7, int),
+	/*
+	 * int:	descriptor
+	 */
+
+	BC_INCREFS_DONE = _IOW('c', 8, struct binder_ptr_cookie),
+	BC_ACQUIRE_DONE = _IOW('c', 9, struct binder_ptr_cookie),
+	/*
+	 * void *: ptr to binder
+	 * void *: cookie for binder
+	 */
+
+	BC_ATTEMPT_ACQUIRE = _IOW('c', 10, struct binder_pri_desc),
+	/*
+	 * not currently supported
+	 * int: priority
+	 * int: descriptor
+	 */
+
+	BC_REGISTER_LOOPER = _IO('c', 11),
+	/*
+	 * No parameters.
+	 * Register a spawned looper thread with the device.
+	 */
+
+	BC_ENTER_LOOPER = _IO('c', 12),
+	BC_EXIT_LOOPER = _IO('c', 13),
+	/*
+	 * No parameters.
+	 * These two commands are sent as an application-level thread
+	 * enters and exits the binder loop, respectively.  They are
+	 * used so the binder can have an accurate count of the number
+	 * of looping threads it has available.
+	 */
+
+	BC_REQUEST_DEATH_NOTIFICATION = _IOW('c', 14, struct binder_ptr_cookie),
+	/*
+	 * void *: ptr to binder
+	 * void *: cookie
+	 */
+
+	BC_CLEAR_DEATH_NOTIFICATION = _IOW('c', 15, struct binder_ptr_cookie),
+	/*
+	 * void *: ptr to binder
+	 * void *: cookie
+	 */
+
+	BC_DEAD_BINDER_DONE = _IOW('c', 16, void *),
+	/*
+	 * void *: cookie
+	 */
+};
+
+#endif /* _LINUX_BINDER_H */
+
diff --git a/drivers/staging/android/logger.c b/drivers/staging/android/logger.c
new file mode 100644
index 000000000000..3e09d57dc6d9
--- /dev/null
+++ b/drivers/staging/android/logger.c
@@ -0,0 +1,756 @@
+/*
+ * drivers/misc/logger.c
+ *
+ * A Logging Subsystem
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * Robert Love <rlove@google.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/sched.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/poll.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+#include "logger.h"
+
+#include <asm/ioctls.h>
+
+/*
+ * struct logger_log - represents a specific log, such as 'main' or 'radio'
+ *
+ * This structure lives from module insertion until module removal, so it does
+ * not need additional reference counting. The structure is protected by the
+ * mutex 'mutex'.
+ */
+struct logger_log {
+	unsigned char 		*buffer;/* the ring buffer itself */
+	struct miscdevice	misc;	/* misc device representing the log */
+	wait_queue_head_t	wq;	/* wait queue for readers */
+	struct list_head	readers; /* this log's readers */
+	struct mutex		mutex;	/* mutex protecting buffer */
+	size_t			w_off;	/* current write head offset */
+	size_t			head;	/* new readers start here */
+	size_t			size;	/* size of the log */
+};
+
+/*
+ * struct logger_reader - a logging device open for reading
+ *
+ * This object lives from open to release, so we don't need additional
+ * reference counting. The structure is protected by log->mutex.
+ */
+struct logger_reader {
+	struct logger_log	*log;	/* associated log */
+	struct list_head	list;	/* entry in logger_log's list */
+	size_t			r_off;	/* current read head offset */
+	bool			r_all;	/* reader can read all entries */
+	int			r_ver;	/* reader ABI version */
+};
+
+/* logger_offset - returns index 'n' into the log via (optimized) modulus */
+#define logger_offset(n)	((n) & (log->size - 1))
+
+/*
+ * file_get_log - Given a file structure, return the associated log
+ *
+ * This isn't aesthetic. We have several goals:
+ *
+ * 	1) Need to quickly obtain the associated log during an I/O operation
+ * 	2) Readers need to maintain state (logger_reader)
+ * 	3) Writers need to be very fast (open() should be a near no-op)
+ *
+ * In the reader case, we can trivially go file->logger_reader->logger_log.
+ * For a writer, we don't want to maintain a logger_reader, so we just go
+ * file->logger_log. Thus what file->private_data points at depends on whether
+ * or not the file was opened for reading. This function hides that dirtiness.
+ */
+static inline struct logger_log *file_get_log(struct file *file)
+{
+	if (file->f_mode & FMODE_READ) {
+		struct logger_reader *reader = file->private_data;
+		return reader->log;
+	} else
+		return file->private_data;
+}
+
+/*
+ * get_entry_header - returns a pointer to the logger_entry header within
+ * 'log' starting at offset 'off'. A temporary logger_entry 'scratch' must
+ * be provided. Typically the return value will be a pointer within
+ * 'logger->buf'.  However, a pointer to 'scratch' may be returned if
+ * the log entry spans the end and beginning of the circular buffer.
+ */
+static struct logger_entry *get_entry_header(struct logger_log *log,
+		size_t off, struct logger_entry *scratch)
+{
+	size_t len = min(sizeof(struct logger_entry), log->size - off);
+	if (len != sizeof(struct logger_entry)) {
+		memcpy(((void *) scratch), log->buffer + off, len);
+		memcpy(((void *) scratch) + len, log->buffer,
+			sizeof(struct logger_entry) - len);
+		return scratch;
+	}
+
+	return (struct logger_entry *) (log->buffer + off);
+}
+
+/*
+ * get_entry_msg_len - Grabs the length of the message of the entry
+ * starting from from 'off'.
+ *
+ * Caller needs to hold log->mutex.
+ */
+static __u32 get_entry_msg_len(struct logger_log *log, size_t off)
+{
+	struct logger_entry scratch;
+	struct logger_entry *entry;
+
+	entry = get_entry_header(log, off, &scratch);
+	return entry->len;
+}
+
+static size_t get_user_hdr_len(int ver)
+{
+	if (ver < 2)
+		return sizeof(struct user_logger_entry_compat);
+	else
+		return sizeof(struct logger_entry);
+}
+
+static ssize_t copy_header_to_user(int ver, struct logger_entry *entry,
+					 char __user *buf)
+{
+	void *hdr;
+	size_t hdr_len;
+	struct user_logger_entry_compat v1;
+
+	if (ver < 2) {
+		v1.len      = entry->len;
+		v1.__pad    = 0;
+		v1.pid      = entry->pid;
+		v1.tid      = entry->tid;
+		v1.sec      = entry->sec;
+		v1.nsec     = entry->nsec;
+		hdr         = &v1;
+		hdr_len     = sizeof(struct user_logger_entry_compat);
+	} else {
+		hdr         = entry;
+		hdr_len     = sizeof(struct logger_entry);
+	}
+
+	return copy_to_user(buf, hdr, hdr_len);
+}
+
+/*
+ * do_read_log_to_user - reads exactly 'count' bytes from 'log' into the
+ * user-space buffer 'buf'. Returns 'count' on success.
+ *
+ * Caller must hold log->mutex.
+ */
+static ssize_t do_read_log_to_user(struct logger_log *log,
+				   struct logger_reader *reader,
+				   char __user *buf,
+				   size_t count)
+{
+	struct logger_entry scratch;
+	struct logger_entry *entry;
+	size_t len;
+	size_t msg_start;
+
+	/*
+	 * First, copy the header to userspace, using the version of
+	 * the header requested
+	 */
+	entry = get_entry_header(log, reader->r_off, &scratch);
+	if (copy_header_to_user(reader->r_ver, entry, buf))
+		return -EFAULT;
+
+	count -= get_user_hdr_len(reader->r_ver);
+	buf += get_user_hdr_len(reader->r_ver);
+	msg_start = logger_offset(reader->r_off + sizeof(struct logger_entry));
+
+	/*
+	 * We read from the msg in two disjoint operations. First, we read from
+	 * the current msg head offset up to 'count' bytes or to the end of
+	 * the log, whichever comes first.
+	 */
+	len = min(count, log->size - msg_start);
+	if (copy_to_user(buf, log->buffer + msg_start, len))
+		return -EFAULT;
+
+	/*
+	 * Second, we read any remaining bytes, starting back at the head of
+	 * the log.
+	 */
+	if (count != len)
+		if (copy_to_user(buf + len, log->buffer, count - len))
+			return -EFAULT;
+
+	reader->r_off = logger_offset(reader->r_off +
+		sizeof(struct logger_entry) + count);
+
+	return count + get_user_hdr_len(reader->r_ver);
+}
+
+/*
+ * get_next_entry_by_uid - Starting at 'off', returns an offset into
+ * 'log->buffer' which contains the first entry readable by 'euid'
+ */
+static size_t get_next_entry_by_uid(struct logger_log *log,
+		size_t off, uid_t euid)
+{
+	while (off != log->w_off) {
+		struct logger_entry *entry;
+		struct logger_entry scratch;
+		size_t next_len;
+
+		entry = get_entry_header(log, off, &scratch);
+
+		if (entry->euid == euid)
+			return off;
+
+		next_len = sizeof(struct logger_entry) + entry->len;
+		off = logger_offset(off + next_len);
+	}
+
+	return off;
+}
+
+/*
+ * logger_read - our log's read() method
+ *
+ * Behavior:
+ *
+ * 	- O_NONBLOCK works
+ * 	- If there are no log entries to read, blocks until log is written to
+ * 	- Atomically reads exactly one log entry
+ *
+ * Will set errno to EINVAL if read
+ * buffer is insufficient to hold next entry.
+ */
+static ssize_t logger_read(struct file *file, char __user *buf,
+			   size_t count, loff_t *pos)
+{
+	struct logger_reader *reader = file->private_data;
+	struct logger_log *log = reader->log;
+	ssize_t ret;
+	DEFINE_WAIT(wait);
+
+start:
+	while (1) {
+		prepare_to_wait(&log->wq, &wait, TASK_INTERRUPTIBLE);
+
+		mutex_lock(&log->mutex);
+		ret = (log->w_off == reader->r_off);
+		mutex_unlock(&log->mutex);
+		if (!ret)
+			break;
+
+		if (file->f_flags & O_NONBLOCK) {
+			ret = -EAGAIN;
+			break;
+		}
+
+		if (signal_pending(current)) {
+			ret = -EINTR;
+			break;
+		}
+
+		schedule();
+	}
+
+	finish_wait(&log->wq, &wait);
+	if (ret)
+		return ret;
+
+	mutex_lock(&log->mutex);
+
+	if (!reader->r_all)
+		reader->r_off = get_next_entry_by_uid(log,
+			reader->r_off, current_euid());
+
+	/* is there still something to read or did we race? */
+	if (unlikely(log->w_off == reader->r_off)) {
+		mutex_unlock(&log->mutex);
+		goto start;
+	}
+
+	/* get the size of the next entry */
+	ret = get_user_hdr_len(reader->r_ver) +
+		get_entry_msg_len(log, reader->r_off);
+	if (count < ret) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* get exactly one entry from the log */
+	ret = do_read_log_to_user(log, reader, buf, ret);
+
+out:
+	mutex_unlock(&log->mutex);
+
+	return ret;
+}
+
+/*
+ * get_next_entry - return the offset of the first valid entry at least 'len'
+ * bytes after 'off'.
+ *
+ * Caller must hold log->mutex.
+ */
+static size_t get_next_entry(struct logger_log *log, size_t off, size_t len)
+{
+	size_t count = 0;
+
+	do {
+		size_t nr = sizeof(struct logger_entry) +
+			get_entry_msg_len(log, off);
+		off = logger_offset(off + nr);
+		count += nr;
+	} while (count < len);
+
+	return off;
+}
+
+/*
+ * clock_interval - is a < c < b in mod-space? Put another way, does the line
+ * from a to b cross c?
+ */
+static inline int clock_interval(size_t a, size_t b, size_t c)
+{
+	if (b < a) {
+		if (a < c || b >= c)
+			return 1;
+	} else {
+		if (a < c && b >= c)
+			return 1;
+	}
+
+	return 0;
+}
+
+/*
+ * fix_up_readers - walk the list of all readers and "fix up" any who were
+ * lapped by the writer; also do the same for the default "start head".
+ * We do this by "pulling forward" the readers and start head to the first
+ * entry after the new write head.
+ *
+ * The caller needs to hold log->mutex.
+ */
+static void fix_up_readers(struct logger_log *log, size_t len)
+{
+	size_t old = log->w_off;
+	size_t new = logger_offset(old + len);
+	struct logger_reader *reader;
+
+	if (clock_interval(old, new, log->head))
+		log->head = get_next_entry(log, log->head, len);
+
+	list_for_each_entry(reader, &log->readers, list)
+		if (clock_interval(old, new, reader->r_off))
+			reader->r_off = get_next_entry(log, reader->r_off, len);
+}
+
+/*
+ * do_write_log - writes 'len' bytes from 'buf' to 'log'
+ *
+ * The caller needs to hold log->mutex.
+ */
+static void do_write_log(struct logger_log *log, const void *buf, size_t count)
+{
+	size_t len;
+
+	len = min(count, log->size - log->w_off);
+	memcpy(log->buffer + log->w_off, buf, len);
+
+	if (count != len)
+		memcpy(log->buffer, buf + len, count - len);
+
+	log->w_off = logger_offset(log->w_off + count);
+
+}
+
+/*
+ * do_write_log_user - writes 'len' bytes from the user-space buffer 'buf' to
+ * the log 'log'
+ *
+ * The caller needs to hold log->mutex.
+ *
+ * Returns 'count' on success, negative error code on failure.
+ */
+static ssize_t do_write_log_from_user(struct logger_log *log,
+				      const void __user *buf, size_t count)
+{
+	size_t len;
+
+	len = min(count, log->size - log->w_off);
+	if (len && copy_from_user(log->buffer + log->w_off, buf, len))
+		return -EFAULT;
+
+	if (count != len)
+		if (copy_from_user(log->buffer, buf + len, count - len))
+			return -EFAULT;
+
+	log->w_off = logger_offset(log->w_off + count);
+
+	return count;
+}
+
+/*
+ * logger_aio_write - our write method, implementing support for write(),
+ * writev(), and aio_write(). Writes are our fast path, and we try to optimize
+ * them above all else.
+ */
+ssize_t logger_aio_write(struct kiocb *iocb, const struct iovec *iov,
+			 unsigned long nr_segs, loff_t ppos)
+{
+	struct logger_log *log = file_get_log(iocb->ki_filp);
+	size_t orig = log->w_off;
+	struct logger_entry header;
+	struct timespec now;
+	ssize_t ret = 0;
+
+	now = current_kernel_time();
+
+	header.pid = current->tgid;
+	header.tid = current->pid;
+	header.sec = now.tv_sec;
+	header.nsec = now.tv_nsec;
+	header.euid = current_euid();
+	header.len = min_t(size_t, iocb->ki_left, LOGGER_ENTRY_MAX_PAYLOAD);
+	header.hdr_size = sizeof(struct logger_entry);
+
+	/* null writes succeed, return zero */
+	if (unlikely(!header.len))
+		return 0;
+
+	mutex_lock(&log->mutex);
+
+	/*
+	 * Fix up any readers, pulling them forward to the first readable
+	 * entry after (what will be) the new write offset. We do this now
+	 * because if we partially fail, we can end up with clobbered log
+	 * entries that encroach on readable buffer.
+	 */
+	fix_up_readers(log, sizeof(struct logger_entry) + header.len);
+
+	do_write_log(log, &header, sizeof(struct logger_entry));
+
+	while (nr_segs-- > 0) {
+		size_t len;
+		ssize_t nr;
+
+		/* figure out how much of this vector we can keep */
+		len = min_t(size_t, iov->iov_len, header.len - ret);
+
+		/* write out this segment's payload */
+		nr = do_write_log_from_user(log, iov->iov_base, len);
+		if (unlikely(nr < 0)) {
+			log->w_off = orig;
+			mutex_unlock(&log->mutex);
+			return nr;
+		}
+
+		iov++;
+		ret += nr;
+	}
+
+	mutex_unlock(&log->mutex);
+
+	/* wake up any blocked readers */
+	wake_up_interruptible(&log->wq);
+
+	return ret;
+}
+
+static struct logger_log *get_log_from_minor(int);
+
+/*
+ * logger_open - the log's open() file operation
+ *
+ * Note how near a no-op this is in the write-only case. Keep it that way!
+ */
+static int logger_open(struct inode *inode, struct file *file)
+{
+	struct logger_log *log;
+	int ret;
+
+	ret = nonseekable_open(inode, file);
+	if (ret)
+		return ret;
+
+	log = get_log_from_minor(MINOR(inode->i_rdev));
+	if (!log)
+		return -ENODEV;
+
+	if (file->f_mode & FMODE_READ) {
+		struct logger_reader *reader;
+
+		reader = kmalloc(sizeof(struct logger_reader), GFP_KERNEL);
+		if (!reader)
+			return -ENOMEM;
+
+		reader->log = log;
+		reader->r_ver = 1;
+		reader->r_all = in_egroup_p(inode->i_gid) ||
+			capable(CAP_SYSLOG);
+
+		INIT_LIST_HEAD(&reader->list);
+
+		mutex_lock(&log->mutex);
+		reader->r_off = log->head;
+		list_add_tail(&reader->list, &log->readers);
+		mutex_unlock(&log->mutex);
+
+		file->private_data = reader;
+	} else
+		file->private_data = log;
+
+	return 0;
+}
+
+/*
+ * logger_release - the log's release file operation
+ *
+ * Note this is a total no-op in the write-only case. Keep it that way!
+ */
+static int logger_release(struct inode *ignored, struct file *file)
+{
+	if (file->f_mode & FMODE_READ) {
+		struct logger_reader *reader = file->private_data;
+		list_del(&reader->list);
+		kfree(reader);
+	}
+
+	return 0;
+}
+
+/*
+ * logger_poll - the log's poll file operation, for poll/select/epoll
+ *
+ * Note we always return POLLOUT, because you can always write() to the log.
+ * Note also that, strictly speaking, a return value of POLLIN does not
+ * guarantee that the log is readable without blocking, as there is a small
+ * chance that the writer can lap the reader in the interim between poll()
+ * returning and the read() request.
+ */
+static unsigned int logger_poll(struct file *file, poll_table *wait)
+{
+	struct logger_reader *reader;
+	struct logger_log *log;
+	unsigned int ret = POLLOUT | POLLWRNORM;
+
+	if (!(file->f_mode & FMODE_READ))
+		return ret;
+
+	reader = file->private_data;
+	log = reader->log;
+
+	poll_wait(file, &log->wq, wait);
+
+	mutex_lock(&log->mutex);
+	if (!reader->r_all)
+		reader->r_off = get_next_entry_by_uid(log,
+			reader->r_off, current_euid());
+
+	if (log->w_off != reader->r_off)
+		ret |= POLLIN | POLLRDNORM;
+	mutex_unlock(&log->mutex);
+
+	return ret;
+}
+
+static long logger_set_version(struct logger_reader *reader, void __user *arg)
+{
+	int version;
+	if (copy_from_user(&version, arg, sizeof(int)))
+		return -EFAULT;
+
+	if ((version < 1) || (version > 2))
+		return -EINVAL;
+
+	reader->r_ver = version;
+	return 0;
+}
+
+static long logger_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct logger_log *log = file_get_log(file);
+	struct logger_reader *reader;
+	long ret = -EINVAL;
+	void __user *argp = (void __user *) arg;
+
+	mutex_lock(&log->mutex);
+
+	switch (cmd) {
+	case LOGGER_GET_LOG_BUF_SIZE:
+		ret = log->size;
+		break;
+	case LOGGER_GET_LOG_LEN:
+		if (!(file->f_mode & FMODE_READ)) {
+			ret = -EBADF;
+			break;
+		}
+		reader = file->private_data;
+		if (log->w_off >= reader->r_off)
+			ret = log->w_off - reader->r_off;
+		else
+			ret = (log->size - reader->r_off) + log->w_off;
+		break;
+	case LOGGER_GET_NEXT_ENTRY_LEN:
+		if (!(file->f_mode & FMODE_READ)) {
+			ret = -EBADF;
+			break;
+		}
+		reader = file->private_data;
+
+		if (!reader->r_all)
+			reader->r_off = get_next_entry_by_uid(log,
+				reader->r_off, current_euid());
+
+		if (log->w_off != reader->r_off)
+			ret = get_user_hdr_len(reader->r_ver) +
+				get_entry_msg_len(log, reader->r_off);
+		else
+			ret = 0;
+		break;
+	case LOGGER_FLUSH_LOG:
+		if (!(file->f_mode & FMODE_WRITE)) {
+			ret = -EBADF;
+			break;
+		}
+		list_for_each_entry(reader, &log->readers, list)
+			reader->r_off = log->w_off;
+		log->head = log->w_off;
+		ret = 0;
+		break;
+	case LOGGER_GET_VERSION:
+		if (!(file->f_mode & FMODE_READ)) {
+			ret = -EBADF;
+			break;
+		}
+		reader = file->private_data;
+		ret = reader->r_ver;
+		break;
+	case LOGGER_SET_VERSION:
+		if (!(file->f_mode & FMODE_READ)) {
+			ret = -EBADF;
+			break;
+		}
+		reader = file->private_data;
+		ret = logger_set_version(reader, argp);
+		break;
+	}
+
+	mutex_unlock(&log->mutex);
+
+	return ret;
+}
+
+static const struct file_operations logger_fops = {
+	.owner = THIS_MODULE,
+	.read = logger_read,
+	.aio_write = logger_aio_write,
+	.poll = logger_poll,
+	.unlocked_ioctl = logger_ioctl,
+	.compat_ioctl = logger_ioctl,
+	.open = logger_open,
+	.release = logger_release,
+};
+
+/*
+ * Defines a log structure with name 'NAME' and a size of 'SIZE' bytes, which
+ * must be a power of two, and greater than
+ * (LOGGER_ENTRY_MAX_PAYLOAD + sizeof(struct logger_entry)).
+ */
+#define DEFINE_LOGGER_DEVICE(VAR, NAME, SIZE) \
+static unsigned char _buf_ ## VAR[SIZE]; \
+static struct logger_log VAR = { \
+	.buffer = _buf_ ## VAR, \
+	.misc = { \
+		.minor = MISC_DYNAMIC_MINOR, \
+		.name = NAME, \
+		.fops = &logger_fops, \
+		.parent = NULL, \
+	}, \
+	.wq = __WAIT_QUEUE_HEAD_INITIALIZER(VAR .wq), \
+	.readers = LIST_HEAD_INIT(VAR .readers), \
+	.mutex = __MUTEX_INITIALIZER(VAR .mutex), \
+	.w_off = 0, \
+	.head = 0, \
+	.size = SIZE, \
+};
+
+DEFINE_LOGGER_DEVICE(log_main, LOGGER_LOG_MAIN, 256*1024)
+DEFINE_LOGGER_DEVICE(log_events, LOGGER_LOG_EVENTS, 256*1024)
+DEFINE_LOGGER_DEVICE(log_radio, LOGGER_LOG_RADIO, 256*1024)
+DEFINE_LOGGER_DEVICE(log_system, LOGGER_LOG_SYSTEM, 256*1024)
+
+static struct logger_log *get_log_from_minor(int minor)
+{
+	if (log_main.misc.minor == minor)
+		return &log_main;
+	if (log_events.misc.minor == minor)
+		return &log_events;
+	if (log_radio.misc.minor == minor)
+		return &log_radio;
+	if (log_system.misc.minor == minor)
+		return &log_system;
+	return NULL;
+}
+
+static int __init init_log(struct logger_log *log)
+{
+	int ret;
+
+	ret = misc_register(&log->misc);
+	if (unlikely(ret)) {
+		printk(KERN_ERR "logger: failed to register misc "
+		       "device for log '%s'!\n", log->misc.name);
+		return ret;
+	}
+
+	printk(KERN_INFO "logger: created %luK log '%s'\n",
+	       (unsigned long) log->size >> 10, log->misc.name);
+
+	return 0;
+}
+
+static int __init logger_init(void)
+{
+	int ret;
+
+	ret = init_log(&log_main);
+	if (unlikely(ret))
+		goto out;
+
+	ret = init_log(&log_events);
+	if (unlikely(ret))
+		goto out;
+
+	ret = init_log(&log_radio);
+	if (unlikely(ret))
+		goto out;
+
+	ret = init_log(&log_system);
+	if (unlikely(ret))
+		goto out;
+
+out:
+	return ret;
+}
+device_initcall(logger_init);
diff --git a/drivers/staging/android/logger.h b/drivers/staging/android/logger.h
new file mode 100644
index 000000000000..3f612a3b101c
--- /dev/null
+++ b/drivers/staging/android/logger.h
@@ -0,0 +1,70 @@
+/* include/linux/logger.h
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ * Author: Robert Love <rlove@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 _LINUX_LOGGER_H
+#define _LINUX_LOGGER_H
+
+#include <linux/types.h>
+#include <linux/ioctl.h>
+
+/*
+ * The userspace structure for version 1 of the logger_entry ABI.
+ * This structure is returned to userspace unless the caller requests
+ * an upgrade to a newer ABI version.
+ */
+struct user_logger_entry_compat {
+	__u16		len;	/* length of the payload */
+	__u16		__pad;	/* no matter what, we get 2 bytes of padding */
+	__s32		pid;	/* generating process's pid */
+	__s32		tid;	/* generating process's tid */
+	__s32		sec;	/* seconds since Epoch */
+	__s32		nsec;	/* nanoseconds */
+	char		msg[0];	/* the entry's payload */
+};
+
+/*
+ * The structure for version 2 of the logger_entry ABI.
+ * This structure is returned to userspace if ioctl(LOGGER_SET_VERSION)
+ * is called with version >= 2
+ */
+struct logger_entry {
+	__u16		len;		/* length of the payload */
+	__u16		hdr_size;	/* sizeof(struct logger_entry_v2) */
+	__s32		pid;		/* generating process's pid */
+	__s32		tid;		/* generating process's tid */
+	__s32		sec;		/* seconds since Epoch */
+	__s32		nsec;		/* nanoseconds */
+	uid_t		euid;		/* effective UID of logger */
+	char		msg[0];		/* the entry's payload */
+};
+
+#define LOGGER_LOG_RADIO	"log_radio"	/* radio-related messages */
+#define LOGGER_LOG_EVENTS	"log_events"	/* system/hardware events */
+#define LOGGER_LOG_SYSTEM	"log_system"	/* system/framework messages */
+#define LOGGER_LOG_MAIN		"log_main"	/* everything else */
+
+#define LOGGER_ENTRY_MAX_PAYLOAD	4076
+
+#define __LOGGERIO	0xAE
+
+#define LOGGER_GET_LOG_BUF_SIZE		_IO(__LOGGERIO, 1) /* size of log */
+#define LOGGER_GET_LOG_LEN		_IO(__LOGGERIO, 2) /* used log len */
+#define LOGGER_GET_NEXT_ENTRY_LEN	_IO(__LOGGERIO, 3) /* next entry len */
+#define LOGGER_FLUSH_LOG		_IO(__LOGGERIO, 4) /* flush log */
+#define LOGGER_GET_VERSION		_IO(__LOGGERIO, 5) /* abi version */
+#define LOGGER_SET_VERSION		_IO(__LOGGERIO, 6) /* abi version */
+
+#endif /* _LINUX_LOGGER_H */
diff --git a/drivers/staging/android/lowmemorykiller.c b/drivers/staging/android/lowmemorykiller.c
new file mode 100644
index 000000000000..86d51959b29f
--- /dev/null
+++ b/drivers/staging/android/lowmemorykiller.c
@@ -0,0 +1,213 @@
+/* drivers/misc/lowmemorykiller.c
+ *
+ * The lowmemorykiller driver lets user-space specify a set of memory thresholds
+ * where processes with a range of oom_adj values will get killed. Specify the
+ * minimum oom_adj values in /sys/module/lowmemorykiller/parameters/adj and the
+ * number of free pages in /sys/module/lowmemorykiller/parameters/minfree. Both
+ * files take a comma separated list of numbers in ascending order.
+ *
+ * For example, write "0,8" to /sys/module/lowmemorykiller/parameters/adj and
+ * "1024,4096" to /sys/module/lowmemorykiller/parameters/minfree to kill processes
+ * with a oom_adj value of 8 or higher when the free memory drops below 4096 pages
+ * and kill processes with a oom_adj value of 0 or higher when the free memory
+ * drops below 1024 pages.
+ *
+ * The driver considers memory used for caches to be free, but if a large
+ * percentage of the cached memory is locked this can be very inaccurate
+ * and processes may not get killed until the normal oom killer is triggered.
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/oom.h>
+#include <linux/sched.h>
+#include <linux/notifier.h>
+
+static uint32_t lowmem_debug_level = 2;
+static int lowmem_adj[6] = {
+	0,
+	1,
+	6,
+	12,
+};
+static int lowmem_adj_size = 4;
+static size_t lowmem_minfree[6] = {
+	3 * 512,	/* 6MB */
+	2 * 1024,	/* 8MB */
+	4 * 1024,	/* 16MB */
+	16 * 1024,	/* 64MB */
+};
+static int lowmem_minfree_size = 4;
+
+static struct task_struct *lowmem_deathpending;
+static unsigned long lowmem_deathpending_timeout;
+
+#define lowmem_print(level, x...)			\
+	do {						\
+		if (lowmem_debug_level >= (level))	\
+			printk(x);			\
+	} while (0)
+
+static int
+task_notify_func(struct notifier_block *self, unsigned long val, void *data);
+
+static struct notifier_block task_nb = {
+	.notifier_call	= task_notify_func,
+};
+
+static int
+task_notify_func(struct notifier_block *self, unsigned long val, void *data)
+{
+	struct task_struct *task = data;
+
+	if (task == lowmem_deathpending)
+		lowmem_deathpending = NULL;
+
+	return NOTIFY_OK;
+}
+
+static int lowmem_shrink(struct shrinker *s, struct shrink_control *sc)
+{
+	struct task_struct *p;
+	struct task_struct *selected = NULL;
+	int rem = 0;
+	int tasksize;
+	int i;
+	int min_adj = OOM_ADJUST_MAX + 1;
+	int selected_tasksize = 0;
+	int selected_oom_adj;
+	int array_size = ARRAY_SIZE(lowmem_adj);
+	int other_free = global_page_state(NR_FREE_PAGES);
+	int other_file = global_page_state(NR_FILE_PAGES) -
+						global_page_state(NR_SHMEM);
+
+	/*
+	 * If we already have a death outstanding, then
+	 * bail out right away; indicating to vmscan
+	 * that we have nothing further to offer on
+	 * this pass.
+	 *
+	 */
+	if (lowmem_deathpending &&
+	    time_before_eq(jiffies, lowmem_deathpending_timeout))
+		return 0;
+
+	if (lowmem_adj_size < array_size)
+		array_size = lowmem_adj_size;
+	if (lowmem_minfree_size < array_size)
+		array_size = lowmem_minfree_size;
+	for (i = 0; i < array_size; i++) {
+		if (other_free < lowmem_minfree[i] &&
+		    other_file < lowmem_minfree[i]) {
+			min_adj = lowmem_adj[i];
+			break;
+		}
+	}
+	if (sc->nr_to_scan > 0)
+		lowmem_print(3, "lowmem_shrink %lu, %x, ofree %d %d, ma %d\n",
+			     sc->nr_to_scan, sc->gfp_mask, other_free, other_file,
+			     min_adj);
+	rem = global_page_state(NR_ACTIVE_ANON) +
+		global_page_state(NR_ACTIVE_FILE) +
+		global_page_state(NR_INACTIVE_ANON) +
+		global_page_state(NR_INACTIVE_FILE);
+	if (sc->nr_to_scan <= 0 || min_adj == OOM_ADJUST_MAX + 1) {
+		lowmem_print(5, "lowmem_shrink %lu, %x, return %d\n",
+			     sc->nr_to_scan, sc->gfp_mask, rem);
+		return rem;
+	}
+	selected_oom_adj = min_adj;
+
+	read_lock(&tasklist_lock);
+	for_each_process(p) {
+		struct mm_struct *mm;
+		struct signal_struct *sig;
+		int oom_adj;
+
+		task_lock(p);
+		mm = p->mm;
+		sig = p->signal;
+		if (!mm || !sig) {
+			task_unlock(p);
+			continue;
+		}
+		oom_adj = sig->oom_adj;
+		if (oom_adj < min_adj) {
+			task_unlock(p);
+			continue;
+		}
+		tasksize = get_mm_rss(mm);
+		task_unlock(p);
+		if (tasksize <= 0)
+			continue;
+		if (selected) {
+			if (oom_adj < selected_oom_adj)
+				continue;
+			if (oom_adj == selected_oom_adj &&
+			    tasksize <= selected_tasksize)
+				continue;
+		}
+		selected = p;
+		selected_tasksize = tasksize;
+		selected_oom_adj = oom_adj;
+		lowmem_print(2, "select %d (%s), adj %d, size %d, to kill\n",
+			     p->pid, p->comm, oom_adj, tasksize);
+	}
+	if (selected) {
+		lowmem_print(1, "send sigkill to %d (%s), adj %d, size %d\n",
+			     selected->pid, selected->comm,
+			     selected_oom_adj, selected_tasksize);
+		lowmem_deathpending = selected;
+		lowmem_deathpending_timeout = jiffies + HZ;
+		force_sig(SIGKILL, selected);
+		rem -= selected_tasksize;
+	}
+	lowmem_print(4, "lowmem_shrink %lu, %x, return %d\n",
+		     sc->nr_to_scan, sc->gfp_mask, rem);
+	read_unlock(&tasklist_lock);
+	return rem;
+}
+
+static struct shrinker lowmem_shrinker = {
+	.shrink = lowmem_shrink,
+	.seeks = DEFAULT_SEEKS * 16
+};
+
+static int __init lowmem_init(void)
+{
+	task_free_register(&task_nb);
+	register_shrinker(&lowmem_shrinker);
+	return 0;
+}
+
+static void __exit lowmem_exit(void)
+{
+	unregister_shrinker(&lowmem_shrinker);
+	task_free_unregister(&task_nb);
+}
+
+module_param_named(cost, lowmem_shrinker.seeks, int, S_IRUGO | S_IWUSR);
+module_param_array_named(adj, lowmem_adj, int, &lowmem_adj_size,
+			 S_IRUGO | S_IWUSR);
+module_param_array_named(minfree, lowmem_minfree, uint, &lowmem_minfree_size,
+			 S_IRUGO | S_IWUSR);
+module_param_named(debug_level, lowmem_debug_level, uint, S_IRUGO | S_IWUSR);
+
+module_init(lowmem_init);
+module_exit(lowmem_exit);
+
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/staging/android/ram_console.c b/drivers/staging/android/ram_console.c
new file mode 100644
index 000000000000..b278d62e4b89
--- /dev/null
+++ b/drivers/staging/android/ram_console.c
@@ -0,0 +1,443 @@
+/* drivers/android/ram_console.c
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/console.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/proc_fs.h>
+#include <linux/string.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/platform_data/ram_console.h>
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+#include <linux/rslib.h>
+#endif
+
+struct ram_console_buffer {
+	uint32_t    sig;
+	uint32_t    start;
+	uint32_t    size;
+	uint8_t     data[0];
+};
+
+#define RAM_CONSOLE_SIG (0x43474244) /* DBGC */
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+static char __initdata
+	ram_console_old_log_init_buffer[CONFIG_ANDROID_RAM_CONSOLE_EARLY_SIZE];
+#endif
+static char *ram_console_old_log;
+static size_t ram_console_old_log_size;
+
+static struct ram_console_buffer *ram_console_buffer;
+static size_t ram_console_buffer_size;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+static char *ram_console_par_buffer;
+static struct rs_control *ram_console_rs_decoder;
+static int ram_console_corrected_bytes;
+static int ram_console_bad_blocks;
+#define ECC_BLOCK_SIZE CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_DATA_SIZE
+#define ECC_SIZE CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_ECC_SIZE
+#define ECC_SYMSIZE CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE
+#define ECC_POLY CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_POLYNOMIAL
+#endif
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+static void ram_console_encode_rs8(uint8_t *data, size_t len, uint8_t *ecc)
+{
+	int i;
+	uint16_t par[ECC_SIZE];
+	/* Initialize the parity buffer */
+	memset(par, 0, sizeof(par));
+	encode_rs8(ram_console_rs_decoder, data, len, par, 0);
+	for (i = 0; i < ECC_SIZE; i++)
+		ecc[i] = par[i];
+}
+
+static int ram_console_decode_rs8(void *data, size_t len, uint8_t *ecc)
+{
+	int i;
+	uint16_t par[ECC_SIZE];
+	for (i = 0; i < ECC_SIZE; i++)
+		par[i] = ecc[i];
+	return decode_rs8(ram_console_rs_decoder, data, par, len,
+				NULL, 0, NULL, 0, NULL);
+}
+#endif
+
+static void ram_console_update(const char *s, unsigned int count)
+{
+	struct ram_console_buffer *buffer = ram_console_buffer;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	uint8_t *buffer_end = buffer->data + ram_console_buffer_size;
+	uint8_t *block;
+	uint8_t *par;
+	int size = ECC_BLOCK_SIZE;
+#endif
+	memcpy(buffer->data + buffer->start, s, count);
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	block = buffer->data + (buffer->start & ~(ECC_BLOCK_SIZE - 1));
+	par = ram_console_par_buffer +
+	      (buffer->start / ECC_BLOCK_SIZE) * ECC_SIZE;
+	do {
+		if (block + ECC_BLOCK_SIZE > buffer_end)
+			size = buffer_end - block;
+		ram_console_encode_rs8(block, size, par);
+		block += ECC_BLOCK_SIZE;
+		par += ECC_SIZE;
+	} while (block < buffer->data + buffer->start + count);
+#endif
+}
+
+static void ram_console_update_header(void)
+{
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	struct ram_console_buffer *buffer = ram_console_buffer;
+	uint8_t *par;
+	par = ram_console_par_buffer +
+	      DIV_ROUND_UP(ram_console_buffer_size, ECC_BLOCK_SIZE) * ECC_SIZE;
+	ram_console_encode_rs8((uint8_t *)buffer, sizeof(*buffer), par);
+#endif
+}
+
+static void
+ram_console_write(struct console *console, const char *s, unsigned int count)
+{
+	int rem;
+	struct ram_console_buffer *buffer = ram_console_buffer;
+
+	if (count > ram_console_buffer_size) {
+		s += count - ram_console_buffer_size;
+		count = ram_console_buffer_size;
+	}
+	rem = ram_console_buffer_size - buffer->start;
+	if (rem < count) {
+		ram_console_update(s, rem);
+		s += rem;
+		count -= rem;
+		buffer->start = 0;
+		buffer->size = ram_console_buffer_size;
+	}
+	ram_console_update(s, count);
+
+	buffer->start += count;
+	if (buffer->size < ram_console_buffer_size)
+		buffer->size += count;
+	ram_console_update_header();
+}
+
+static struct console ram_console = {
+	.name	= "ram",
+	.write	= ram_console_write,
+	.flags	= CON_PRINTBUFFER | CON_ENABLED | CON_ANYTIME,
+	.index	= -1,
+};
+
+void ram_console_enable_console(int enabled)
+{
+	if (enabled)
+		ram_console.flags |= CON_ENABLED;
+	else
+		ram_console.flags &= ~CON_ENABLED;
+}
+
+static void __init
+ram_console_save_old(struct ram_console_buffer *buffer, const char *bootinfo,
+	char *dest)
+{
+	size_t old_log_size = buffer->size;
+	size_t bootinfo_size = 0;
+	size_t total_size = old_log_size;
+	char *ptr;
+	const char *bootinfo_label = "Boot info:\n";
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	uint8_t *block;
+	uint8_t *par;
+	char strbuf[80];
+	int strbuf_len = 0;
+
+	block = buffer->data;
+	par = ram_console_par_buffer;
+	while (block < buffer->data + buffer->size) {
+		int numerr;
+		int size = ECC_BLOCK_SIZE;
+		if (block + size > buffer->data + ram_console_buffer_size)
+			size = buffer->data + ram_console_buffer_size - block;
+		numerr = ram_console_decode_rs8(block, size, par);
+		if (numerr > 0) {
+#if 0
+			printk(KERN_INFO "ram_console: error in block %p, %d\n",
+			       block, numerr);
+#endif
+			ram_console_corrected_bytes += numerr;
+		} else if (numerr < 0) {
+#if 0
+			printk(KERN_INFO "ram_console: uncorrectable error in "
+			       "block %p\n", block);
+#endif
+			ram_console_bad_blocks++;
+		}
+		block += ECC_BLOCK_SIZE;
+		par += ECC_SIZE;
+	}
+	if (ram_console_corrected_bytes || ram_console_bad_blocks)
+		strbuf_len = snprintf(strbuf, sizeof(strbuf),
+			"\n%d Corrected bytes, %d unrecoverable blocks\n",
+			ram_console_corrected_bytes, ram_console_bad_blocks);
+	else
+		strbuf_len = snprintf(strbuf, sizeof(strbuf),
+				      "\nNo errors detected\n");
+	if (strbuf_len >= sizeof(strbuf))
+		strbuf_len = sizeof(strbuf) - 1;
+	total_size += strbuf_len;
+#endif
+
+	if (bootinfo)
+		bootinfo_size = strlen(bootinfo) + strlen(bootinfo_label);
+	total_size += bootinfo_size;
+
+	if (dest == NULL) {
+		dest = kmalloc(total_size, GFP_KERNEL);
+		if (dest == NULL) {
+			printk(KERN_ERR
+			       "ram_console: failed to allocate buffer\n");
+			return;
+		}
+	}
+
+	ram_console_old_log = dest;
+	ram_console_old_log_size = total_size;
+	memcpy(ram_console_old_log,
+	       &buffer->data[buffer->start], buffer->size - buffer->start);
+	memcpy(ram_console_old_log + buffer->size - buffer->start,
+	       &buffer->data[0], buffer->start);
+	ptr = ram_console_old_log + old_log_size;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	memcpy(ptr, strbuf, strbuf_len);
+	ptr += strbuf_len;
+#endif
+	if (bootinfo) {
+		memcpy(ptr, bootinfo_label, strlen(bootinfo_label));
+		ptr += strlen(bootinfo_label);
+		memcpy(ptr, bootinfo, bootinfo_size);
+		ptr += bootinfo_size;
+	}
+}
+
+static int __init ram_console_init(struct ram_console_buffer *buffer,
+				   size_t buffer_size, const char *bootinfo,
+				   char *old_buf)
+{
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	int numerr;
+	uint8_t *par;
+#endif
+	ram_console_buffer = buffer;
+	ram_console_buffer_size =
+		buffer_size - sizeof(struct ram_console_buffer);
+
+	if (ram_console_buffer_size > buffer_size) {
+		pr_err("ram_console: buffer %p, invalid size %zu, "
+		       "datasize %zu\n", buffer, buffer_size,
+		       ram_console_buffer_size);
+		return 0;
+	}
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	ram_console_buffer_size -= (DIV_ROUND_UP(ram_console_buffer_size,
+						ECC_BLOCK_SIZE) + 1) * ECC_SIZE;
+
+	if (ram_console_buffer_size > buffer_size) {
+		pr_err("ram_console: buffer %p, invalid size %zu, "
+		       "non-ecc datasize %zu\n",
+		       buffer, buffer_size, ram_console_buffer_size);
+		return 0;
+	}
+
+	ram_console_par_buffer = buffer->data + ram_console_buffer_size;
+
+
+	/* first consecutive root is 0
+	 * primitive element to generate roots = 1
+	 */
+	ram_console_rs_decoder = init_rs(ECC_SYMSIZE, ECC_POLY, 0, 1, ECC_SIZE);
+	if (ram_console_rs_decoder == NULL) {
+		printk(KERN_INFO "ram_console: init_rs failed\n");
+		return 0;
+	}
+
+	ram_console_corrected_bytes = 0;
+	ram_console_bad_blocks = 0;
+
+	par = ram_console_par_buffer +
+	      DIV_ROUND_UP(ram_console_buffer_size, ECC_BLOCK_SIZE) * ECC_SIZE;
+
+	numerr = ram_console_decode_rs8(buffer, sizeof(*buffer), par);
+	if (numerr > 0) {
+		printk(KERN_INFO "ram_console: error in header, %d\n", numerr);
+		ram_console_corrected_bytes += numerr;
+	} else if (numerr < 0) {
+		printk(KERN_INFO
+		       "ram_console: uncorrectable error in header\n");
+		ram_console_bad_blocks++;
+	}
+#endif
+
+	if (buffer->sig == RAM_CONSOLE_SIG) {
+		if (buffer->size > ram_console_buffer_size
+		    || buffer->start > buffer->size)
+			printk(KERN_INFO "ram_console: found existing invalid "
+			       "buffer, size %d, start %d\n",
+			       buffer->size, buffer->start);
+		else {
+			printk(KERN_INFO "ram_console: found existing buffer, "
+			       "size %d, start %d\n",
+			       buffer->size, buffer->start);
+			ram_console_save_old(buffer, bootinfo, old_buf);
+		}
+	} else {
+		printk(KERN_INFO "ram_console: no valid data in buffer "
+		       "(sig = 0x%08x)\n", buffer->sig);
+	}
+
+	buffer->sig = RAM_CONSOLE_SIG;
+	buffer->start = 0;
+	buffer->size = 0;
+
+	register_console(&ram_console);
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ENABLE_VERBOSE
+	console_verbose();
+#endif
+	return 0;
+}
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+static int __init ram_console_early_init(void)
+{
+	return ram_console_init((struct ram_console_buffer *)
+		CONFIG_ANDROID_RAM_CONSOLE_EARLY_ADDR,
+		CONFIG_ANDROID_RAM_CONSOLE_EARLY_SIZE,
+		NULL,
+		ram_console_old_log_init_buffer);
+}
+#else
+static int ram_console_driver_probe(struct platform_device *pdev)
+{
+	struct resource *res = pdev->resource;
+	size_t start;
+	size_t buffer_size;
+	void *buffer;
+	const char *bootinfo = NULL;
+	struct ram_console_platform_data *pdata = pdev->dev.platform_data;
+
+	if (res == NULL || pdev->num_resources != 1 ||
+	    !(res->flags & IORESOURCE_MEM)) {
+		printk(KERN_ERR "ram_console: invalid resource, %p %d flags "
+		       "%lx\n", res, pdev->num_resources, res ? res->flags : 0);
+		return -ENXIO;
+	}
+	buffer_size = res->end - res->start + 1;
+	start = res->start;
+	printk(KERN_INFO "ram_console: got buffer at %zx, size %zx\n",
+	       start, buffer_size);
+	buffer = ioremap(res->start, buffer_size);
+	if (buffer == NULL) {
+		printk(KERN_ERR "ram_console: failed to map memory\n");
+		return -ENOMEM;
+	}
+
+	if (pdata)
+		bootinfo = pdata->bootinfo;
+
+	return ram_console_init(buffer, buffer_size, bootinfo, NULL/* allocate */);
+}
+
+static struct platform_driver ram_console_driver = {
+	.probe = ram_console_driver_probe,
+	.driver		= {
+		.name	= "ram_console",
+	},
+};
+
+static int __init ram_console_module_init(void)
+{
+	int err;
+	err = platform_driver_register(&ram_console_driver);
+	return err;
+}
+#endif
+
+static ssize_t ram_console_read_old(struct file *file, char __user *buf,
+				    size_t len, loff_t *offset)
+{
+	loff_t pos = *offset;
+	ssize_t count;
+
+	if (pos >= ram_console_old_log_size)
+		return 0;
+
+	count = min(len, (size_t)(ram_console_old_log_size - pos));
+	if (copy_to_user(buf, ram_console_old_log + pos, count))
+		return -EFAULT;
+
+	*offset += count;
+	return count;
+}
+
+static const struct file_operations ram_console_file_ops = {
+	.owner = THIS_MODULE,
+	.read = ram_console_read_old,
+};
+
+static int __init ram_console_late_init(void)
+{
+	struct proc_dir_entry *entry;
+
+	if (ram_console_old_log == NULL)
+		return 0;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+	ram_console_old_log = kmalloc(ram_console_old_log_size, GFP_KERNEL);
+	if (ram_console_old_log == NULL) {
+		printk(KERN_ERR
+		       "ram_console: failed to allocate buffer for old log\n");
+		ram_console_old_log_size = 0;
+		return 0;
+	}
+	memcpy(ram_console_old_log,
+	       ram_console_old_log_init_buffer, ram_console_old_log_size);
+#endif
+	entry = create_proc_entry("last_kmsg", S_IFREG | S_IRUGO, NULL);
+	if (!entry) {
+		printk(KERN_ERR "ram_console: failed to create proc entry\n");
+		kfree(ram_console_old_log);
+		ram_console_old_log = NULL;
+		return 0;
+	}
+
+	entry->proc_fops = &ram_console_file_ops;
+	entry->size = ram_console_old_log_size;
+	return 0;
+}
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+console_initcall(ram_console_early_init);
+#else
+postcore_initcall(ram_console_module_init);
+#endif
+late_initcall(ram_console_late_init);
+
diff --git a/drivers/staging/android/timed_gpio.c b/drivers/staging/android/timed_gpio.c
new file mode 100644
index 000000000000..a64481c3e86d
--- /dev/null
+++ b/drivers/staging/android/timed_gpio.c
@@ -0,0 +1,176 @@
+/* drivers/misc/timed_gpio.c
+ *
+ * Copyright (C) 2008 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/hrtimer.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+
+#include "timed_output.h"
+#include "timed_gpio.h"
+
+
+struct timed_gpio_data {
+	struct timed_output_dev dev;
+	struct hrtimer timer;
+	spinlock_t lock;
+	unsigned 	gpio;
+	int 		max_timeout;
+	u8 		active_low;
+};
+
+static enum hrtimer_restart gpio_timer_func(struct hrtimer *timer)
+{
+	struct timed_gpio_data *data =
+		container_of(timer, struct timed_gpio_data, timer);
+
+	gpio_direction_output(data->gpio, data->active_low ? 1 : 0);
+	return HRTIMER_NORESTART;
+}
+
+static int gpio_get_time(struct timed_output_dev *dev)
+{
+	struct timed_gpio_data	*data =
+		container_of(dev, struct timed_gpio_data, dev);
+
+	if (hrtimer_active(&data->timer)) {
+		ktime_t r = hrtimer_get_remaining(&data->timer);
+		struct timeval t = ktime_to_timeval(r);
+		return t.tv_sec * 1000 + t.tv_usec / 1000;
+	} else
+		return 0;
+}
+
+static void gpio_enable(struct timed_output_dev *dev, int value)
+{
+	struct timed_gpio_data	*data =
+		container_of(dev, struct timed_gpio_data, dev);
+	unsigned long	flags;
+
+	spin_lock_irqsave(&data->lock, flags);
+
+	/* cancel previous timer and set GPIO according to value */
+	hrtimer_cancel(&data->timer);
+	gpio_direction_output(data->gpio, data->active_low ? !value : !!value);
+
+	if (value > 0) {
+		if (value > data->max_timeout)
+			value = data->max_timeout;
+
+		hrtimer_start(&data->timer,
+			ktime_set(value / 1000, (value % 1000) * 1000000),
+			HRTIMER_MODE_REL);
+	}
+
+	spin_unlock_irqrestore(&data->lock, flags);
+}
+
+static int timed_gpio_probe(struct platform_device *pdev)
+{
+	struct timed_gpio_platform_data *pdata = pdev->dev.platform_data;
+	struct timed_gpio *cur_gpio;
+	struct timed_gpio_data *gpio_data, *gpio_dat;
+	int i, j, ret = 0;
+
+	if (!pdata)
+		return -EBUSY;
+
+	gpio_data = kzalloc(sizeof(struct timed_gpio_data) * pdata->num_gpios,
+			GFP_KERNEL);
+	if (!gpio_data)
+		return -ENOMEM;
+
+	for (i = 0; i < pdata->num_gpios; i++) {
+		cur_gpio = &pdata->gpios[i];
+		gpio_dat = &gpio_data[i];
+
+		hrtimer_init(&gpio_dat->timer, CLOCK_MONOTONIC,
+				HRTIMER_MODE_REL);
+		gpio_dat->timer.function = gpio_timer_func;
+		spin_lock_init(&gpio_dat->lock);
+
+		gpio_dat->dev.name = cur_gpio->name;
+		gpio_dat->dev.get_time = gpio_get_time;
+		gpio_dat->dev.enable = gpio_enable;
+		ret = gpio_request(cur_gpio->gpio, cur_gpio->name);
+		if (ret >= 0) {
+			ret = timed_output_dev_register(&gpio_dat->dev);
+			if (ret < 0)
+				gpio_free(cur_gpio->gpio);
+		}
+		if (ret < 0) {
+			for (j = 0; j < i; j++) {
+				timed_output_dev_unregister(&gpio_data[i].dev);
+				gpio_free(gpio_data[i].gpio);
+			}
+			kfree(gpio_data);
+			return ret;
+		}
+
+		gpio_dat->gpio = cur_gpio->gpio;
+		gpio_dat->max_timeout = cur_gpio->max_timeout;
+		gpio_dat->active_low = cur_gpio->active_low;
+		gpio_direction_output(gpio_dat->gpio, gpio_dat->active_low);
+	}
+
+	platform_set_drvdata(pdev, gpio_data);
+
+	return 0;
+}
+
+static int timed_gpio_remove(struct platform_device *pdev)
+{
+	struct timed_gpio_platform_data *pdata = pdev->dev.platform_data;
+	struct timed_gpio_data *gpio_data = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < pdata->num_gpios; i++) {
+		timed_output_dev_unregister(&gpio_data[i].dev);
+		gpio_free(gpio_data[i].gpio);
+	}
+
+	kfree(gpio_data);
+
+	return 0;
+}
+
+static struct platform_driver timed_gpio_driver = {
+	.probe		= timed_gpio_probe,
+	.remove		= timed_gpio_remove,
+	.driver		= {
+		.name		= TIMED_GPIO_NAME,
+		.owner		= THIS_MODULE,
+	},
+};
+
+static int __init timed_gpio_init(void)
+{
+	return platform_driver_register(&timed_gpio_driver);
+}
+
+static void __exit timed_gpio_exit(void)
+{
+	platform_driver_unregister(&timed_gpio_driver);
+}
+
+module_init(timed_gpio_init);
+module_exit(timed_gpio_exit);
+
+MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
+MODULE_DESCRIPTION("timed gpio driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/android/timed_gpio.h b/drivers/staging/android/timed_gpio.h
new file mode 100644
index 000000000000..a0e15f8be3f7
--- /dev/null
+++ b/drivers/staging/android/timed_gpio.h
@@ -0,0 +1,33 @@
+/* include/linux/timed_gpio.h
+ *
+ * Copyright (C) 2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 _LINUX_TIMED_GPIO_H
+#define _LINUX_TIMED_GPIO_H
+
+#define TIMED_GPIO_NAME "timed-gpio"
+
+struct timed_gpio {
+	const char *name;
+	unsigned 	gpio;
+	int		max_timeout;
+	u8 		active_low;
+};
+
+struct timed_gpio_platform_data {
+	int 		num_gpios;
+	struct timed_gpio *gpios;
+};
+
+#endif
diff --git a/drivers/staging/android/timed_output.c b/drivers/staging/android/timed_output.c
new file mode 100644
index 000000000000..f373422308e0
--- /dev/null
+++ b/drivers/staging/android/timed_output.c
@@ -0,0 +1,123 @@
+/* drivers/misc/timed_output.c
+ *
+ * Copyright (C) 2009 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/module.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/err.h>
+
+#include "timed_output.h"
+
+static struct class *timed_output_class;
+static atomic_t device_count;
+
+static ssize_t enable_show(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct timed_output_dev *tdev = dev_get_drvdata(dev);
+	int remaining = tdev->get_time(tdev);
+
+	return sprintf(buf, "%d\n", remaining);
+}
+
+static ssize_t enable_store(
+		struct device *dev, struct device_attribute *attr,
+		const char *buf, size_t size)
+{
+	struct timed_output_dev *tdev = dev_get_drvdata(dev);
+	int value;
+
+	if (sscanf(buf, "%d", &value) != 1)
+		return -EINVAL;
+
+	tdev->enable(tdev, value);
+
+	return size;
+}
+
+static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR, enable_show, enable_store);
+
+static int create_timed_output_class(void)
+{
+	if (!timed_output_class) {
+		timed_output_class = class_create(THIS_MODULE, "timed_output");
+		if (IS_ERR(timed_output_class))
+			return PTR_ERR(timed_output_class);
+		atomic_set(&device_count, 0);
+	}
+
+	return 0;
+}
+
+int timed_output_dev_register(struct timed_output_dev *tdev)
+{
+	int ret;
+
+	if (!tdev || !tdev->name || !tdev->enable || !tdev->get_time)
+		return -EINVAL;
+
+	ret = create_timed_output_class();
+	if (ret < 0)
+		return ret;
+
+	tdev->index = atomic_inc_return(&device_count);
+	tdev->dev = device_create(timed_output_class, NULL,
+		MKDEV(0, tdev->index), NULL, tdev->name);
+	if (IS_ERR(tdev->dev))
+		return PTR_ERR(tdev->dev);
+
+	ret = device_create_file(tdev->dev, &dev_attr_enable);
+	if (ret < 0)
+		goto err_create_file;
+
+	dev_set_drvdata(tdev->dev, tdev);
+	tdev->state = 0;
+	return 0;
+
+err_create_file:
+	device_destroy(timed_output_class, MKDEV(0, tdev->index));
+	printk(KERN_ERR "timed_output: Failed to register driver %s\n",
+			tdev->name);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(timed_output_dev_register);
+
+void timed_output_dev_unregister(struct timed_output_dev *tdev)
+{
+	device_remove_file(tdev->dev, &dev_attr_enable);
+	device_destroy(timed_output_class, MKDEV(0, tdev->index));
+	dev_set_drvdata(tdev->dev, NULL);
+}
+EXPORT_SYMBOL_GPL(timed_output_dev_unregister);
+
+static int __init timed_output_init(void)
+{
+	return create_timed_output_class();
+}
+
+static void __exit timed_output_exit(void)
+{
+	class_destroy(timed_output_class);
+}
+
+module_init(timed_output_init);
+module_exit(timed_output_exit);
+
+MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
+MODULE_DESCRIPTION("timed output class driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/android/timed_output.h b/drivers/staging/android/timed_output.h
new file mode 100644
index 000000000000..ec907ab2ff54
--- /dev/null
+++ b/drivers/staging/android/timed_output.h
@@ -0,0 +1,37 @@
+/* include/linux/timed_output.h
+ *
+ * Copyright (C) 2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 _LINUX_TIMED_OUTPUT_H
+#define _LINUX_TIMED_OUTPUT_H
+
+struct timed_output_dev {
+	const char	*name;
+
+	/* enable the output and set the timer */
+	void	(*enable)(struct timed_output_dev *sdev, int timeout);
+
+	/* returns the current number of milliseconds remaining on the timer */
+	int		(*get_time)(struct timed_output_dev *sdev);
+
+	/* private data */
+	struct device	*dev;
+	int		index;
+	int		state;
+};
+
+extern int timed_output_dev_register(struct timed_output_dev *dev);
+extern void timed_output_dev_unregister(struct timed_output_dev *dev);
+
+#endif
diff --git a/drivers/staging/brcm80211/Kconfig b/drivers/staging/brcm80211/Kconfig
index 379cf16e89f7..586de7f74f5f 100644
--- a/drivers/staging/brcm80211/Kconfig
+++ b/drivers/staging/brcm80211/Kconfig
@@ -21,7 +21,6 @@ config BRCMFMAC
 	default n
 	depends on MMC
 	depends on WLAN && CFG80211
-	depends on X86 || MIPS
 	select BRCMUTIL
 	select FW_LOADER
 	select WIRELESS_EXT
diff --git a/drivers/staging/brcm80211/Makefile b/drivers/staging/brcm80211/Makefile
index 8b01f5e7ba25..b595cab91e0b 100644
--- a/drivers/staging/brcm80211/Makefile
+++ b/drivers/staging/brcm80211/Makefile
@@ -16,8 +16,8 @@
 # CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 
 # common flags
-subdir-ccflags-y					:= -DBCMDMA32
-subdir-ccflags-$(CONFIG_BRCMDBG)	+= -DBCMDBG
+subdir-ccflags-y			:= -DBCMDMA32
+subdir-ccflags-$(CONFIG_BRCMDBG)	+= -DBCMDBG -DBCMDBG_ASSERT
 
 obj-$(CONFIG_BRCMUTIL)	+= brcmutil/
 obj-$(CONFIG_BRCMFMAC)	+= brcmfmac/
diff --git a/drivers/staging/brcm80211/README b/drivers/staging/brcm80211/README
index bb86b1b3e58e..8ad558675bd3 100644
--- a/drivers/staging/brcm80211/README
+++ b/drivers/staging/brcm80211/README
@@ -1 +1,64 @@
-refer to: http://linuxwireless.org/en/users/Drivers/brcm80211
+Broadcom brcmsmac (mac80211-based softmac PCIe) and brcmfmac (SDIO) drivers.
+
+Completely open source host drivers, no binary object files.
+
+Support for the following chips:
+===============================
+
+    brcmsmac (PCIe)
+    Name        Device ID
+    BCM4313     0x4727
+    BCM43224    0x4353
+    BCM43225    0x4357
+
+    brcmfmac (SDIO)
+    Name
+    BCM4329
+
+Both brcmsmac and brcmfmac drivers require firmware files that need to be
+separately downloaded.
+
+Firmware
+======================
+Firmware is available from the Linux firmware repository at:
+
+    git://git.kernel.org/pub/scm/linux/kernel/git/dwmw2/linux-firmware.git
+    http://git.kernel.org/?p=linux/kernel/git/dwmw2/linux-firmware.git
+    https://git.kernel.org/?p=linux/kernel/git/dwmw2/linux-firmware.git
+
+
+===============================================================
+Broadcom brcmsmac driver
+===============================================================
+- Support for both 32 and 64 bit Linux kernels
+
+
+Firmware installation
+======================
+Copy brcm/bcm43xx-0.fw and brcm/bcm43xx_hdr-0.fw to
+/lib/firmware/brcm (or wherever firmware is normally installed
+on your system).
+
+
+===============================================================
+Broadcom brcmfmac driver
+===============================================================
+- Support for 32 bit Linux kernel, 64 bit untested
+
+
+Firmware installation
+======================
+Copy brcm/bcm4329-fullmac-4.bin and brcm/bcm4329-fullmac-4.txt
+to /lib/firmware/brcm (or wherever firmware is normally installed on your
+system).
+
+
+Contact Info:
+=============
+Brett Rudley		brudley@broadcom.com
+Henry Ptasinski		henryp@broadcom.com
+Dowan Kim		dowan@broadcom.com
+Roland Vossen		rvossen@broadcom.com
+Arend van Spriel	arend@broadcom.com
+
+For more info, refer to: http://linuxwireless.org/en/users/Drivers/brcm80211
diff --git a/drivers/staging/brcm80211/TODO b/drivers/staging/brcm80211/TODO
deleted file mode 100644
index e2e2ef9bd7ac..000000000000
--- a/drivers/staging/brcm80211/TODO
+++ /dev/null
@@ -1,13 +0,0 @@
-To Do List for Broadcom Mac80211 driver before getting in mainline
-
-Bugs
-====
-- none known at this moment
-
-brcmfmac
-=====================
-- ASSERTS deprecated in mainline, replace by warning + error handling
-
-brcm80211 info page
-=====================
-http://linuxwireless.org/en/users/Drivers/brcm80211
diff --git a/drivers/staging/brcm80211/brcmfmac/Makefile b/drivers/staging/brcm80211/brcmfmac/Makefile
index da3c80575907..71c7fe94ece8 100644
--- a/drivers/staging/brcm80211/brcmfmac/Makefile
+++ b/drivers/staging/brcm80211/brcmfmac/Makefile
@@ -15,17 +15,12 @@
 # OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
 # CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 
-ccflags-y :=			\
-	-DBRCMF_FIRSTREAD=64	\
-	-DBRCMF_SDALIGN=64	\
-	-DMAX_HDR_READ=64
-
-ccflags-$(CONFIG_BRCMDBG)	+= -DSHOW_EVENTS
-
 ccflags-y += \
 	-Idrivers/staging/brcm80211/brcmfmac	\
 	-Idrivers/staging/brcm80211/include
 
+ccflags-y += -DISR_THREAD
+
 DHDOFILES = \
 	wl_cfg80211.o \
 	dhd_cdc.o \
@@ -37,3 +32,4 @@ DHDOFILES = \
 
 obj-$(CONFIG_BRCMFMAC) += brcmfmac.o
 brcmfmac-objs += $(DHDOFILES)
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/staging/brcm80211/brcmfmac/bcmsdh.c b/drivers/staging/brcm80211/brcmfmac/bcmsdh.c
index f4e72ed126b0..bff9dcd6fadc 100644
--- a/drivers/staging/brcm80211/brcmfmac/bcmsdh.c
+++ b/drivers/staging/brcm80211/brcmfmac/bcmsdh.c
@@ -21,6 +21,9 @@
 #include <linux/pci_ids.h>
 #include <linux/sched.h>
 #include <linux/completion.h>
+#include <linux/mmc/sdio.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/card.h>
 
 #include <defs.h>
 #include <brcm_hw_ids.h>
@@ -29,304 +32,103 @@
 #include <soc.h>
 #include "dhd.h"
 #include "dhd_bus.h"
+#include "dhd_dbg.h"
 #include "sdio_host.h"
 
 #define SDIOH_API_ACCESS_RETRY_LIMIT	2
 
-#define BRCMF_SD_ERROR_VAL	0x0001	/* Error */
-#define BRCMF_SD_INFO_VAL		0x0002	/* Info */
-
-
-#ifdef BCMDBG
-#define BRCMF_SD_ERROR(x) \
-	do { \
-		if ((brcmf_sdio_msglevel & BRCMF_SD_ERROR_VAL) && \
-		    net_ratelimit()) \
-			printk x; \
-	} while (0)
-#define BRCMF_SD_INFO(x)	\
-	do { \
-		if ((brcmf_sdio_msglevel & BRCMF_SD_INFO_VAL) && \
-		    net_ratelimit()) \
-			printk x; \
-	} while (0)
-#else				/* BCMDBG */
-#define BRCMF_SD_ERROR(x)
-#define BRCMF_SD_INFO(x)
-#endif				/* BCMDBG */
-
-/* debugging macros */
-#define SDLX_MSG(x)
-
-#define SDIOH_CMD_TYPE_NORMAL   0	/* Normal command */
-#define SDIOH_CMD_TYPE_APPEND   1	/* Append command */
-#define SDIOH_CMD_TYPE_CUTTHRU  2	/* Cut-through command */
-
-#define SDIOH_DATA_PIO          0	/* PIO mode */
-#define SDIOH_DATA_DMA          1	/* DMA mode */
-
-struct brcmf_sdio_card {
-	bool init_success;	/* underlying driver successfully attached */
-	void *sdioh;		/* handler for sdioh */
-	u32 vendevid;	/* Target Vendor and Device ID on SD bus */
-	bool regfail;		/* Save status of last
-				 reg_read/reg_write call */
-	u32 sbwad;		/* Save backplane window address */
-};
-
-/**
- * SDIO Host Controller info
- */
-struct sdio_hc {
-	struct sdio_hc *next;
-	struct device *dev;	/* platform device handle */
-	void *regs;		/* SDIO Host Controller address */
-	struct brcmf_sdio_card *card;
-	void *ch;
-	unsigned int oob_irq;
-	unsigned long oob_flags;	/* OOB Host specifiction
-					as edge and etc */
-	bool oob_irq_registered;
-};
-
-/* local copy of bcm sd handler */
-static struct brcmf_sdio_card *l_card;
-
-const uint brcmf_sdio_msglevel = BRCMF_SD_ERROR_VAL;
-
-static struct sdio_hc *sdhcinfo;
-
-/* driver info, initialized when brcmf_sdio_register is called */
-static struct brcmf_sdioh_driver drvinfo = { NULL, NULL };
-
-/* Module parameters specific to each host-controller driver */
-
-module_param(sd_msglevel, uint, 0);
-
-extern uint sd_f2_blocksize;
-module_param(sd_f2_blocksize, int, 0);
-
-/* forward declarations */
-int brcmf_sdio_probe(struct device *dev);
-EXPORT_SYMBOL(brcmf_sdio_probe);
-
-int brcmf_sdio_remove(struct device *dev);
-EXPORT_SYMBOL(brcmf_sdio_remove);
-
-struct brcmf_sdio_card*
-brcmf_sdcard_attach(void *cfghdl, u32 *regsva, uint irq)
+static void brcmf_sdioh_irqhandler(struct sdio_func *func)
 {
-	struct brcmf_sdio_card *card;
+	struct brcmf_sdio_dev *sdiodev = dev_get_drvdata(&func->card->dev);
 
-	card = kzalloc(sizeof(struct brcmf_sdio_card), GFP_ATOMIC);
-	if (card == NULL) {
-		BRCMF_SD_ERROR(("sdcard_attach: out of memory"));
-		return NULL;
-	}
-
-	/* save the handler locally */
-	l_card = card;
-
-	card->sdioh = brcmf_sdioh_attach(cfghdl, irq);
-	if (!card->sdioh) {
-		brcmf_sdcard_detach(card);
-		return NULL;
-	}
+	brcmf_dbg(TRACE, "***IRQHandler\n");
 
-	card->init_success = true;
+	sdio_release_host(func);
 
-	*regsva = SI_ENUM_BASE;
+	brcmf_sdbrcm_isr(sdiodev->bus);
 
-	/* Report the BAR, to fix if needed */
-	card->sbwad = SI_ENUM_BASE;
-	return card;
+	sdio_claim_host(func);
 }
 
-int brcmf_sdcard_detach(struct brcmf_sdio_card *card)
+int brcmf_sdcard_intr_reg(struct brcmf_sdio_dev *sdiodev)
 {
-	if (card != NULL) {
-		if (card->sdioh) {
-			brcmf_sdioh_detach(card->sdioh);
-			card->sdioh = NULL;
-		}
-		kfree(card);
-	}
-
-	l_card = NULL;
-	return 0;
-}
+	brcmf_dbg(TRACE, "Entering\n");
 
-int
-brcmf_sdcard_iovar_op(struct brcmf_sdio_card *card, const char *name,
-		void *params, int plen, void *arg, int len, bool set)
-{
-	return brcmf_sdioh_iovar_op(card->sdioh, name, params, plen, arg,
-				    len, set);
-}
+	sdio_claim_host(sdiodev->func[1]);
+	sdio_claim_irq(sdiodev->func[1], brcmf_sdioh_irqhandler);
+	sdio_release_host(sdiodev->func[1]);
 
-int brcmf_sdcard_intr_enable(struct brcmf_sdio_card *card)
-{
-	return brcmf_sdioh_interrupt_set(card->sdioh, true);
+	return 0;
 }
 
-int brcmf_sdcard_intr_disable(struct brcmf_sdio_card *card)
+int brcmf_sdcard_intr_dereg(struct brcmf_sdio_dev *sdiodev)
 {
-	return brcmf_sdioh_interrupt_set(card->sdioh, false);
-}
+	brcmf_dbg(TRACE, "Entering\n");
 
-int brcmf_sdcard_intr_reg(struct brcmf_sdio_card *card,
-			  void (*fn)(void *), void *argh)
-{
-	return brcmf_sdioh_interrupt_register(card->sdioh, fn, argh);
-}
+	sdio_claim_host(sdiodev->func[1]);
+	sdio_release_irq(sdiodev->func[1]);
+	sdio_release_host(sdiodev->func[1]);
 
-int brcmf_sdcard_intr_dereg(struct brcmf_sdio_card *card)
-{
-	return brcmf_sdioh_interrupt_deregister(card->sdioh);
+	return 0;
 }
 
-u8 brcmf_sdcard_cfg_read(struct brcmf_sdio_card *card, uint fnc_num, u32 addr,
+u8 brcmf_sdcard_cfg_read(struct brcmf_sdio_dev *sdiodev, uint fnc_num, u32 addr,
 			 int *err)
 {
 	int status;
 	s32 retry = 0;
 	u8 data = 0;
 
-	if (!card)
-		card = l_card;
-
 	do {
 		if (retry)	/* wait for 1 ms till bus get settled down */
 			udelay(1000);
-		status =
-		    brcmf_sdioh_cfg_read(card->sdioh, fnc_num, addr,
-				   (u8 *) &data);
+		status = brcmf_sdioh_request_byte(sdiodev, SDIOH_READ, fnc_num,
+						  addr, (u8 *) &data);
 	} while (status != 0
 		 && (retry++ < SDIOH_API_ACCESS_RETRY_LIMIT));
 	if (err)
 		*err = status;
 
-	BRCMF_SD_INFO(("%s:fun = %d, addr = 0x%x, u8data = 0x%x\n",
-		     __func__, fnc_num, addr, data));
+	brcmf_dbg(INFO, "fun = %d, addr = 0x%x, u8data = 0x%x\n",
+		  fnc_num, addr, data);
 
 	return data;
 }
 
 void
-brcmf_sdcard_cfg_write(struct brcmf_sdio_card *card, uint fnc_num, u32 addr,
+brcmf_sdcard_cfg_write(struct brcmf_sdio_dev *sdiodev, uint fnc_num, u32 addr,
 		       u8 data, int *err)
 {
 	int status;
 	s32 retry = 0;
 
-	if (!card)
-		card = l_card;
-
 	do {
 		if (retry)	/* wait for 1 ms till bus get settled down */
 			udelay(1000);
-		status =
-		    brcmf_sdioh_cfg_write(card->sdioh, fnc_num, addr,
-				    (u8 *) &data);
+		status = brcmf_sdioh_request_byte(sdiodev, SDIOH_WRITE, fnc_num,
+						  addr, (u8 *) &data);
 	} while (status != 0
 		 && (retry++ < SDIOH_API_ACCESS_RETRY_LIMIT));
 	if (err)
 		*err = status;
 
-	BRCMF_SD_INFO(("%s:fun = %d, addr = 0x%x, u8data = 0x%x\n",
-		     __func__, fnc_num, addr, data));
+	brcmf_dbg(INFO, "fun = %d, addr = 0x%x, u8data = 0x%x\n",
+		  fnc_num, addr, data);
 }
 
-u32 brcmf_sdcard_cfg_read_word(struct brcmf_sdio_card *card, uint fnc_num,
-			       u32 addr, int *err)
-{
-	int status;
-	u32 data = 0;
-
-	if (!card)
-		card = l_card;
-
-	status = brcmf_sdioh_request_word(card->sdioh, SDIOH_CMD_TYPE_NORMAL,
-		SDIOH_READ, fnc_num, addr, &data, 4);
-
-	if (err)
-		*err = status;
-
-	BRCMF_SD_INFO(("%s:fun = %d, addr = 0x%x, u32data = 0x%x\n",
-		     __func__, fnc_num, addr, data));
-
-	return data;
-}
-
-void
-brcmf_sdcard_cfg_write_word(struct brcmf_sdio_card *card, uint fnc_num,
-			    u32 addr, u32 data, int *err)
-{
-	int status;
-
-	if (!card)
-		card = l_card;
-
-	status =
-	    brcmf_sdioh_request_word(card->sdioh, SDIOH_CMD_TYPE_NORMAL,
-			       SDIOH_WRITE, fnc_num, addr, &data, 4);
-
-	if (err)
-		*err = status;
-
-	BRCMF_SD_INFO(("%s:fun = %d, addr = 0x%x, u32data = 0x%x\n",
-		     __func__, fnc_num, addr, data));
-}
-
-int brcmf_sdcard_cis_read(struct brcmf_sdio_card *card, uint func, u8 * cis,
-			  uint length)
-{
-	int status;
-
-	u8 *tmp_buf, *tmp_ptr;
-	u8 *ptr;
-	bool ascii = func & ~0xf;
-	func &= 0x7;
-
-	if (!card)
-		card = l_card;
-
-	status = brcmf_sdioh_cis_read(card->sdioh, func, cis, length);
-
-	if (ascii) {
-		/* Move binary bits to tmp and format them
-			 into the provided buffer. */
-		tmp_buf = kmalloc(length, GFP_ATOMIC);
-		if (tmp_buf == NULL) {
-			BRCMF_SD_ERROR(("%s: out of memory\n", __func__));
-			return -ENOMEM;
-		}
-		memcpy(tmp_buf, cis, length);
-		for (tmp_ptr = tmp_buf, ptr = cis; ptr < (cis + length - 4);
-		     tmp_ptr++) {
-			ptr += sprintf((char *)ptr, "%.2x ", *tmp_ptr & 0xff);
-			if ((((tmp_ptr - tmp_buf) + 1) & 0xf) == 0)
-				ptr += sprintf((char *)ptr, "\n");
-		}
-		kfree(tmp_buf);
-	}
-
-	return status;
-}
-
-static int
-brcmf_sdcard_set_sbaddr_window(struct brcmf_sdio_card *card, u32 address)
+int
+brcmf_sdcard_set_sbaddr_window(struct brcmf_sdio_dev *sdiodev, u32 address)
 {
 	int err = 0;
-	brcmf_sdcard_cfg_write(card, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRLOW,
+	brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRLOW,
 			 (address >> 8) & SBSDIO_SBADDRLOW_MASK, &err);
 	if (!err)
-		brcmf_sdcard_cfg_write(card, SDIO_FUNC_1,
+		brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_1,
 				       SBSDIO_FUNC1_SBADDRMID,
 				       (address >> 16) & SBSDIO_SBADDRMID_MASK,
 				       &err);
 	if (!err)
-		brcmf_sdcard_cfg_write(card, SDIO_FUNC_1,
+		brcmf_sdcard_cfg_write(sdiodev, SDIO_FUNC_1,
 				       SBSDIO_FUNC1_SBADDRHIGH,
 				       (address >> 24) & SBSDIO_SBADDRHIGH_MASK,
 				       &err);
@@ -334,34 +136,31 @@ brcmf_sdcard_set_sbaddr_window(struct brcmf_sdio_card *card, u32 address)
 	return err;
 }
 
-u32 brcmf_sdcard_reg_read(struct brcmf_sdio_card *card, u32 addr, uint size)
+u32 brcmf_sdcard_reg_read(struct brcmf_sdio_dev *sdiodev, u32 addr, uint size)
 {
 	int status;
 	u32 word = 0;
 	uint bar0 = addr & ~SBSDIO_SB_OFT_ADDR_MASK;
 
-	BRCMF_SD_INFO(("%s:fun = 1, addr = 0x%x, ", __func__, addr));
-
-	if (!card)
-		card = l_card;
+	brcmf_dbg(INFO, "fun = 1, addr = 0x%x\n", addr);
 
-	if (bar0 != card->sbwad) {
-		if (brcmf_sdcard_set_sbaddr_window(card, bar0))
+	if (bar0 != sdiodev->sbwad) {
+		if (brcmf_sdcard_set_sbaddr_window(sdiodev, bar0))
 			return 0xFFFFFFFF;
 
-		card->sbwad = bar0;
+		sdiodev->sbwad = bar0;
 	}
 
 	addr &= SBSDIO_SB_OFT_ADDR_MASK;
 	if (size == 4)
 		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
 
-	status = brcmf_sdioh_request_word(card->sdioh, SDIOH_CMD_TYPE_NORMAL,
-				    SDIOH_READ, SDIO_FUNC_1, addr, &word, size);
+	status = brcmf_sdioh_request_word(sdiodev, SDIOH_READ, SDIO_FUNC_1,
+					  addr, &word, size);
 
-	card->regfail = (status != 0);
+	sdiodev->regfail = (status != 0);
 
-	BRCMF_SD_INFO(("u32data = 0x%x\n", word));
+	brcmf_dbg(INFO, "u32data = 0x%x\n", word);
 
 	/* if ok, return appropriately masked word */
 	if (status == 0) {
@@ -373,66 +172,59 @@ u32 brcmf_sdcard_reg_read(struct brcmf_sdio_card *card, u32 addr, uint size)
 		case sizeof(u32):
 			return word;
 		default:
-			card->regfail = true;
+			sdiodev->regfail = true;
 
 		}
 	}
 
 	/* otherwise, bad sdio access or invalid size */
-	BRCMF_SD_ERROR(("%s: error reading addr 0x%04x size %d\n", __func__,
-		      addr, size));
+	brcmf_dbg(ERROR, "error reading addr 0x%04x size %d\n", addr, size);
 	return 0xFFFFFFFF;
 }
 
-u32 brcmf_sdcard_reg_write(struct brcmf_sdio_card *card, u32 addr, uint size,
+u32 brcmf_sdcard_reg_write(struct brcmf_sdio_dev *sdiodev, u32 addr, uint size,
 			   u32 data)
 {
 	int status;
 	uint bar0 = addr & ~SBSDIO_SB_OFT_ADDR_MASK;
 	int err = 0;
 
-	BRCMF_SD_INFO(("%s:fun = 1, addr = 0x%x, uint%ddata = 0x%x\n",
-		     __func__, addr, size * 8, data));
-
-	if (!card)
-		card = l_card;
+	brcmf_dbg(INFO, "fun = 1, addr = 0x%x, uint%ddata = 0x%x\n",
+		  addr, size * 8, data);
 
-	if (bar0 != card->sbwad) {
-		err = brcmf_sdcard_set_sbaddr_window(card, bar0);
+	if (bar0 != sdiodev->sbwad) {
+		err = brcmf_sdcard_set_sbaddr_window(sdiodev, bar0);
 		if (err)
 			return err;
 
-		card->sbwad = bar0;
+		sdiodev->sbwad = bar0;
 	}
 
 	addr &= SBSDIO_SB_OFT_ADDR_MASK;
 	if (size == 4)
 		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
 	status =
-	    brcmf_sdioh_request_word(card->sdioh, SDIOH_CMD_TYPE_NORMAL,
-			       SDIOH_WRITE, SDIO_FUNC_1, addr, &data, size);
-	card->regfail = (status != 0);
+	    brcmf_sdioh_request_word(sdiodev, SDIOH_WRITE, SDIO_FUNC_1,
+				     addr, &data, size);
+	sdiodev->regfail = (status != 0);
 
 	if (status == 0)
 		return 0;
 
-	BRCMF_SD_ERROR(("%s: error writing 0x%08x to addr 0x%04x size %d\n",
-		      __func__, data, addr, size));
+	brcmf_dbg(ERROR, "error writing 0x%08x to addr 0x%04x size %d\n",
+		  data, addr, size);
 	return 0xFFFFFFFF;
 }
 
-bool brcmf_sdcard_regfail(struct brcmf_sdio_card *card)
+bool brcmf_sdcard_regfail(struct brcmf_sdio_dev *sdiodev)
 {
-	return card->regfail;
+	return sdiodev->regfail;
 }
 
 int
-brcmf_sdcard_recv_buf(struct brcmf_sdio_card *card, u32 addr, uint fn,
+brcmf_sdcard_recv_buf(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
 		      uint flags,
-		      u8 *buf, uint nbytes, struct sk_buff *pkt,
-		      void (*complete)(void *handle, int status,
-				       bool sync_waiting),
-		      void *handle)
+		      u8 *buf, uint nbytes, struct sk_buff *pkt)
 {
 	int status;
 	uint incr_fix;
@@ -440,19 +232,18 @@ brcmf_sdcard_recv_buf(struct brcmf_sdio_card *card, u32 addr, uint fn,
 	uint bar0 = addr & ~SBSDIO_SB_OFT_ADDR_MASK;
 	int err = 0;
 
-	BRCMF_SD_INFO(("%s:fun = %d, addr = 0x%x, size = %d\n",
-		     __func__, fn, addr, nbytes));
+	brcmf_dbg(INFO, "fun = %d, addr = 0x%x, size = %d\n", fn, addr, nbytes);
 
 	/* Async not implemented yet */
 	if (flags & SDIO_REQ_ASYNC)
 		return -ENOTSUPP;
 
-	if (bar0 != card->sbwad) {
-		err = brcmf_sdcard_set_sbaddr_window(card, bar0);
+	if (bar0 != sdiodev->sbwad) {
+		err = brcmf_sdcard_set_sbaddr_window(sdiodev, bar0);
 		if (err)
 			return err;
 
-		card->sbwad = bar0;
+		sdiodev->sbwad = bar0;
 	}
 
 	addr &= SBSDIO_SB_OFT_ADDR_MASK;
@@ -462,37 +253,33 @@ brcmf_sdcard_recv_buf(struct brcmf_sdio_card *card, u32 addr, uint fn,
 	if (width == 4)
 		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
 
-	status = brcmf_sdioh_request_buffer(card->sdioh, SDIOH_DATA_PIO,
-		incr_fix, SDIOH_READ, fn, addr, width, nbytes, buf, pkt);
+	status = brcmf_sdioh_request_buffer(sdiodev, incr_fix, SDIOH_READ,
+					    fn, addr, width, nbytes, buf, pkt);
 
 	return status;
 }
 
 int
-brcmf_sdcard_send_buf(struct brcmf_sdio_card *card, u32 addr, uint fn,
-		      uint flags, u8 *buf, uint nbytes, void *pkt,
-		      void (*complete)(void *handle, int status,
-				       bool sync_waiting),
-		      void *handle)
+brcmf_sdcard_send_buf(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
+		      uint flags, u8 *buf, uint nbytes, struct sk_buff *pkt)
 {
 	uint incr_fix;
 	uint width;
 	uint bar0 = addr & ~SBSDIO_SB_OFT_ADDR_MASK;
 	int err = 0;
 
-	BRCMF_SD_INFO(("%s:fun = %d, addr = 0x%x, size = %d\n",
-		     __func__, fn, addr, nbytes));
+	brcmf_dbg(INFO, "fun = %d, addr = 0x%x, size = %d\n", fn, addr, nbytes);
 
 	/* Async not implemented yet */
 	if (flags & SDIO_REQ_ASYNC)
 		return -ENOTSUPP;
 
-	if (bar0 != card->sbwad) {
-		err = brcmf_sdcard_set_sbaddr_window(card, bar0);
+	if (bar0 != sdiodev->sbwad) {
+		err = brcmf_sdcard_set_sbaddr_window(sdiodev, bar0);
 		if (err)
 			return err;
 
-		card->sbwad = bar0;
+		sdiodev->sbwad = bar0;
 	}
 
 	addr &= SBSDIO_SB_OFT_ADDR_MASK;
@@ -502,141 +289,83 @@ brcmf_sdcard_send_buf(struct brcmf_sdio_card *card, u32 addr, uint fn,
 	if (width == 4)
 		addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
 
-	return brcmf_sdioh_request_buffer(card->sdioh, SDIOH_DATA_PIO,
-		incr_fix, SDIOH_WRITE, fn, addr, width, nbytes, buf, pkt);
+	return brcmf_sdioh_request_buffer(sdiodev, incr_fix, SDIOH_WRITE, fn,
+					  addr, width, nbytes, buf, pkt);
 }
 
-int brcmf_sdcard_rwdata(struct brcmf_sdio_card *card, uint rw, u32 addr,
+int brcmf_sdcard_rwdata(struct brcmf_sdio_dev *sdiodev, uint rw, u32 addr,
 			u8 *buf, uint nbytes)
 {
 	addr &= SBSDIO_SB_OFT_ADDR_MASK;
 	addr |= SBSDIO_SB_ACCESS_2_4B_FLAG;
 
-	return brcmf_sdioh_request_buffer(card->sdioh, SDIOH_DATA_PIO,
-		SDIOH_DATA_INC, (rw ? SDIOH_WRITE : SDIOH_READ), SDIO_FUNC_1,
+	return brcmf_sdioh_request_buffer(sdiodev, SDIOH_DATA_INC,
+		(rw ? SDIOH_WRITE : SDIOH_READ), SDIO_FUNC_1,
 		addr, 4, nbytes, buf, NULL);
 }
 
-int brcmf_sdcard_abort(struct brcmf_sdio_card *card, uint fn)
+int brcmf_sdcard_abort(struct brcmf_sdio_dev *sdiodev, uint fn)
 {
-	return brcmf_sdioh_abort(card->sdioh, fn);
-}
-
-int brcmf_sdcard_query_device(struct brcmf_sdio_card *card)
-{
-	card->vendevid = (PCI_VENDOR_ID_BROADCOM << 16) | 0;
-	return card->vendevid;
-}
+	char t_func = (char)fn;
+	brcmf_dbg(TRACE, "Enter\n");
 
-u32 brcmf_sdcard_cur_sbwad(struct brcmf_sdio_card *card)
-{
-	if (!card)
-		card = l_card;
+	/* issue abort cmd52 command through F0 */
+	brcmf_sdioh_request_byte(sdiodev, SDIOH_WRITE, SDIO_FUNC_0,
+				 SDIO_CCCR_ABORT, &t_func);
 
-	return card->sbwad;
+	brcmf_dbg(TRACE, "Exit\n");
+	return 0;
 }
 
-int brcmf_sdio_probe(struct device *dev)
+int brcmf_sdio_probe(struct brcmf_sdio_dev *sdiodev)
 {
-	struct sdio_hc *sdhc = NULL;
 	u32 regs = 0;
-	struct brcmf_sdio_card *card = NULL;
-	int irq = 0;
-	u32 vendevid;
-	unsigned long irq_flags = 0;
-
-	/* allocate SDIO Host Controller state info */
-	sdhc = kzalloc(sizeof(struct sdio_hc), GFP_ATOMIC);
-	if (!sdhc) {
-		SDLX_MSG(("%s: out of memory\n", __func__));
-		goto err;
-	}
-	sdhc->dev = (void *)dev;
+	int ret = 0;
 
-	card = brcmf_sdcard_attach((void *)0, &regs, irq);
-	if (!card) {
-		SDLX_MSG(("%s: attach failed\n", __func__));
-		goto err;
-	}
+	ret = brcmf_sdioh_attach(sdiodev);
+	if (ret)
+		goto out;
 
-	sdhc->card = card;
-	sdhc->oob_irq = irq;
-	sdhc->oob_flags = irq_flags;
-	sdhc->oob_irq_registered = false;	/* to make sure.. */
+	regs = SI_ENUM_BASE;
 
-	/* chain SDIO Host Controller info together */
-	sdhc->next = sdhcinfo;
-	sdhcinfo = sdhc;
-	/* Read the vendor/device ID from the CIS */
-	vendevid = brcmf_sdcard_query_device(card);
+	/* Report the BAR, to fix if needed */
+	sdiodev->sbwad = SI_ENUM_BASE;
 
 	/* try to attach to the target device */
-	sdhc->ch = drvinfo.attach((vendevid >> 16), (vendevid & 0xFFFF),
-				  0, 0, 0, 0, regs, card);
-	if (!sdhc->ch) {
-		SDLX_MSG(("%s: device attach failed\n", __func__));
-		goto err;
+	sdiodev->bus = brcmf_sdbrcm_probe(0, 0, 0, 0, regs, sdiodev);
+	if (!sdiodev->bus) {
+		brcmf_dbg(ERROR, "device attach failed\n");
+		ret = -ENODEV;
+		goto out;
 	}
 
-	return 0;
-
-	/* error handling */
-err:
-	if (sdhc) {
-		if (sdhc->card)
-			brcmf_sdcard_detach(sdhc->card);
-		kfree(sdhc);
-	}
+out:
+	if (ret)
+		brcmf_sdio_remove(sdiodev);
 
-	return -ENODEV;
+	return ret;
 }
+EXPORT_SYMBOL(brcmf_sdio_probe);
 
-int brcmf_sdio_remove(struct device *dev)
+int brcmf_sdio_remove(struct brcmf_sdio_dev *sdiodev)
 {
-	struct sdio_hc *sdhc, *prev;
-
-	sdhc = sdhcinfo;
-	drvinfo.detach(sdhc->ch);
-	brcmf_sdcard_detach(sdhc->card);
-	/* find the SDIO Host Controller state for this pdev
-		 and take it out from the list */
-	for (sdhc = sdhcinfo, prev = NULL; sdhc; sdhc = sdhc->next) {
-		if (sdhc->dev == (void *)dev) {
-			if (prev)
-				prev->next = sdhc->next;
-			else
-				sdhcinfo = NULL;
-			break;
-		}
-		prev = sdhc;
-	}
-	if (!sdhc) {
-		SDLX_MSG(("%s: failed\n", __func__));
-		return 0;
+	if (sdiodev->bus) {
+		brcmf_sdbrcm_disconnect(sdiodev->bus);
+		sdiodev->bus = NULL;
 	}
 
-	/* release SDIO Host Controller info */
-	kfree(sdhc);
-	return 0;
-}
-
-int brcmf_sdio_register(struct brcmf_sdioh_driver *driver)
-{
-	drvinfo = *driver;
+	brcmf_sdioh_detach(sdiodev);
 
-	SDLX_MSG(("Linux Kernel SDIO/MMC Driver\n"));
-	return brcmf_sdio_function_init();
-}
+	sdiodev->sbwad = 0;
 
-void brcmf_sdio_unregister(void)
-{
-	brcmf_sdio_function_cleanup();
+	return 0;
 }
+EXPORT_SYMBOL(brcmf_sdio_remove);
 
-void brcmf_sdio_wdtmr_enable(bool enable)
+void brcmf_sdio_wdtmr_enable(struct brcmf_sdio_dev *sdiodev, bool enable)
 {
 	if (enable)
-		brcmf_sdbrcm_wd_timer(sdhcinfo->ch, brcmf_watchdog_ms);
+		brcmf_sdbrcm_wd_timer(sdiodev->bus, BRCMF_WD_POLL_MS);
 	else
-		brcmf_sdbrcm_wd_timer(sdhcinfo->ch, 0);
+		brcmf_sdbrcm_wd_timer(sdiodev->bus, 0);
 }
diff --git a/drivers/staging/brcm80211/brcmfmac/bcmsdh_sdmmc.c b/drivers/staging/brcm80211/brcmfmac/bcmsdh_sdmmc.c
index 38bd9ba3096f..bbaeb2d5c93a 100644
--- a/drivers/staging/brcm80211/brcmfmac/bcmsdh_sdmmc.c
+++ b/drivers/staging/brcm80211/brcmfmac/bcmsdh_sdmmc.c
@@ -23,6 +23,7 @@
 #include <linux/suspend.h>
 #include <linux/errno.h>
 #include <linux/sched.h>	/* request_irq() */
+#include <linux/module.h>
 #include <net/cfg80211.h>
 
 #include <defs.h>
@@ -34,771 +35,179 @@
 #include "dhd_dbg.h"
 #include "wl_cfg80211.h"
 
-#define BLOCK_SIZE_64 64
-#define BLOCK_SIZE_512 512
-#define BLOCK_SIZE_4318 64
-#define BLOCK_SIZE_4328 512
-
-/* private bus modes */
-#define SDIOH_MODE_SD4		2
-
-#define CLIENT_INTR		0x100	/* Get rid of this! */
-
-#if !defined(SDIO_VENDOR_ID_BROADCOM)
 #define SDIO_VENDOR_ID_BROADCOM		0x02d0
-#endif				/* !defined(SDIO_VENDOR_ID_BROADCOM) */
-
-#define SDIO_DEVICE_ID_BROADCOM_DEFAULT	0x0000
 
 #define DMA_ALIGN_MASK	0x03
 
-#if !defined(SDIO_DEVICE_ID_BROADCOM_4325_SDGWB)
-#define SDIO_DEVICE_ID_BROADCOM_4325_SDGWB	0x0492	/* BCM94325SDGWB */
-#endif		/* !defined(SDIO_DEVICE_ID_BROADCOM_4325_SDGWB) */
-#if !defined(SDIO_DEVICE_ID_BROADCOM_4325)
-#define SDIO_DEVICE_ID_BROADCOM_4325	0x0493
-#endif		/* !defined(SDIO_DEVICE_ID_BROADCOM_4325) */
-#if !defined(SDIO_DEVICE_ID_BROADCOM_4329)
 #define SDIO_DEVICE_ID_BROADCOM_4329	0x4329
-#endif		/* !defined(SDIO_DEVICE_ID_BROADCOM_4329) */
-#if !defined(SDIO_DEVICE_ID_BROADCOM_4319)
-#define SDIO_DEVICE_ID_BROADCOM_4319	0x4319
-#endif		/* !defined(SDIO_DEVICE_ID_BROADCOM_4329) */
-
-/* Common msglevel constants */
-#define SDH_ERROR_VAL		0x0001	/* Error */
-#define SDH_TRACE_VAL		0x0002	/* Trace */
-#define SDH_INFO_VAL		0x0004	/* Info */
-#define SDH_DEBUG_VAL		0x0008	/* Debug */
-#define SDH_DATA_VAL		0x0010	/* Data */
-#define SDH_CTRL_VAL		0x0020	/* Control Regs */
-#define SDH_LOG_VAL		0x0040	/* Enable bcmlog */
-#define SDH_DMA_VAL		0x0080	/* DMA */
-
-#ifdef BCMDBG
-#define sd_err(x)	\
-	do { \
-		if ((sd_msglevel & SDH_ERROR_VAL) && net_ratelimit()) \
-			printk x; \
-	} while (0)
-#define sd_trace(x)	\
-	do { \
-		if ((sd_msglevel & SDH_TRACE_VAL) && net_ratelimit()) \
-			printk x; \
-	} while (0)
-#define sd_info(x)	\
-	do { \
-		if ((sd_msglevel & SDH_INFO_VAL) && net_ratelimit()) \
-			printk x; \
-	} while (0)
-#define sd_debug(x)	\
-	do { \
-		if ((sd_msglevel & SDH_DEBUG_VAL) && net_ratelimit()) \
-			printk x; \
-	} while (0)
-#define sd_data(x)	\
-	do { \
-		if ((sd_msglevel & SDH_DATA_VAL) && net_ratelimit()) \
-			printk x; \
-	} while (0)
-#define sd_ctrl(x)	\
-	do { \
-		if ((sd_msglevel & SDH_CTRL_VAL) && net_ratelimit()) \
-			printk x; \
-	} while (0)
-#else
-#define sd_err(x)
-#define sd_trace(x)
-#define sd_info(x)
-#define sd_debug(x)
-#define sd_data(x)
-#define sd_ctrl(x)
-#endif
-
-struct sdos_info {
-	struct sdioh_info *sd;
-	spinlock_t lock;
-};
-
-static void brcmf_sdioh_irqhandler(struct sdio_func *func);
-static void brcmf_sdioh_irqhandler_f2(struct sdio_func *func);
-static int brcmf_sdioh_get_cisaddr(struct sdioh_info *sd, u32 regaddr);
-static int brcmf_ops_sdio_probe(struct sdio_func *func,
-				const struct sdio_device_id *id);
-static void brcmf_ops_sdio_remove(struct sdio_func *func);
-
-#ifdef CONFIG_PM
-static int brcmf_sdio_suspend(struct device *dev);
-static int brcmf_sdio_resume(struct device *dev);
-#endif /* CONFIG_PM */
 
-uint sd_f2_blocksize = 512;	/* Default blocksize */
-
-uint sd_msglevel = 0x01;
-
-/* module param defaults */
-static int clockoverride;
-
-module_param(clockoverride, int, 0644);
-MODULE_PARM_DESC(clockoverride, "SDIO card clock override");
-
-struct brcmf_sdmmc_instance *gInstance;
-
-struct device sdmmc_dev;
+#define SDIO_FUNC1_BLOCKSIZE		64
+#define SDIO_FUNC2_BLOCKSIZE		512
 
 /* devices we support, null terminated */
 static const struct sdio_device_id brcmf_sdmmc_ids[] = {
-	{SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_DEFAULT)},
-	{SDIO_DEVICE
-	 (SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4325_SDGWB)},
-	{SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4325)},
 	{SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4329)},
-	{SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4319)},
 	{ /* end: all zeroes */ },
 };
-
-#ifdef CONFIG_PM
-static const struct dev_pm_ops brcmf_sdio_pm_ops = {
-	.suspend	= brcmf_sdio_suspend,
-	.resume		= brcmf_sdio_resume,
-};
-#endif	/* CONFIG_PM */
-
-static struct sdio_driver brcmf_sdmmc_driver = {
-	.probe = brcmf_ops_sdio_probe,
-	.remove = brcmf_ops_sdio_remove,
-	.name = "brcmfmac",
-	.id_table = brcmf_sdmmc_ids,
-#ifdef CONFIG_PM
-	.drv = {
-		.pm = &brcmf_sdio_pm_ops,
-	},
-#endif	/* CONFIG_PM */
-};
-
 MODULE_DEVICE_TABLE(sdio, brcmf_sdmmc_ids);
 
-BRCMF_PM_RESUME_WAIT_INIT(sdioh_request_byte_wait);
-BRCMF_PM_RESUME_WAIT_INIT(sdioh_request_word_wait);
-BRCMF_PM_RESUME_WAIT_INIT(sdioh_request_packet_wait);
-BRCMF_PM_RESUME_WAIT_INIT(sdioh_request_buffer_wait);
-
-static int
-brcmf_sdioh_card_regread(struct sdioh_info *sd, int func, u32 regaddr,
-			 int regsize, u32 *data);
-
-static int brcmf_sdioh_enablefuncs(struct sdioh_info *sd)
+static bool
+brcmf_pm_resume_error(struct brcmf_sdio_dev *sdiodev)
 {
-	int err_ret;
-	u32 fbraddr;
-	u8 func;
-
-	sd_trace(("%s\n", __func__));
-
-	/* Get the Card's common CIS address */
-	sd->com_cis_ptr = brcmf_sdioh_get_cisaddr(sd, SDIO_CCCR_CIS);
-	sd->func_cis_ptr[0] = sd->com_cis_ptr;
-	sd_info(("%s: Card's Common CIS Ptr = 0x%x\n", __func__,
-		 sd->com_cis_ptr));
-
-	/* Get the Card's function CIS (for each function) */
-	for (fbraddr = SDIO_FBR_BASE(1), func = 1;
-	     func <= sd->num_funcs; func++, fbraddr += SDIOD_FBR_SIZE) {
-		sd->func_cis_ptr[func] =
-		    brcmf_sdioh_get_cisaddr(sd, SDIO_FBR_CIS + fbraddr);
-		sd_info(("%s: Function %d CIS Ptr = 0x%x\n", __func__, func,
-			 sd->func_cis_ptr[func]));
-	}
-
-	sd->func_cis_ptr[0] = sd->com_cis_ptr;
-	sd_info(("%s: Card's Common CIS Ptr = 0x%x\n", __func__,
-		 sd->com_cis_ptr));
-
-	/* Enable Function 1 */
-	sdio_claim_host(gInstance->func[1]);
-	err_ret = sdio_enable_func(gInstance->func[1]);
-	sdio_release_host(gInstance->func[1]);
-	if (err_ret) {
-		sd_err(("brcmf_sdioh_enablefuncs: Failed to enable F1 "
-			"Err: 0x%08x\n", err_ret));
-	}
-
-	return false;
-}
-
-/*
- *	Public entry points & extern's
- */
-struct sdioh_info *brcmf_sdioh_attach(void *bar0, uint irq)
-{
-	struct sdioh_info *sd;
-	int err_ret;
-
-	sd_trace(("%s\n", __func__));
-
-	if (gInstance == NULL) {
-		sd_err(("%s: SDIO Device not present\n", __func__));
-		return NULL;
-	}
-
-	sd = kzalloc(sizeof(struct sdioh_info), GFP_ATOMIC);
-	if (sd == NULL) {
-		sd_err(("sdioh_attach: out of memory\n"));
-		return NULL;
-	}
-	if (brcmf_sdioh_osinit(sd) != 0) {
-		sd_err(("%s:sdioh_sdmmc_osinit() failed\n", __func__));
-		kfree(sd);
-		return NULL;
-	}
-
-	sd->num_funcs = 2;
-	sd->use_client_ints = true;
-	sd->client_block_size[0] = 64;
-
-	gInstance->sd = sd;
-
-	/* Claim host controller */
-	sdio_claim_host(gInstance->func[1]);
-
-	sd->client_block_size[1] = 64;
-	err_ret = sdio_set_block_size(gInstance->func[1], 64);
-	if (err_ret)
-		sd_err(("brcmf_sdioh_attach: Failed to set F1 blocksize\n"));
-
-	/* Release host controller F1 */
-	sdio_release_host(gInstance->func[1]);
-
-	if (gInstance->func[2]) {
-		/* Claim host controller F2 */
-		sdio_claim_host(gInstance->func[2]);
-
-		sd->client_block_size[2] = sd_f2_blocksize;
-		err_ret =
-		    sdio_set_block_size(gInstance->func[2], sd_f2_blocksize);
-		if (err_ret)
-			sd_err(("brcmf_sdioh_attach: Failed to set F2 blocksize"
-				" to %d\n", sd_f2_blocksize));
-
-		/* Release host controller F2 */
-		sdio_release_host(gInstance->func[2]);
-	}
-
-	brcmf_sdioh_enablefuncs(sd);
-
-	sd_trace(("%s: Done\n", __func__));
-	return sd;
-}
-
-extern int brcmf_sdioh_detach(struct sdioh_info *sd)
-{
-	sd_trace(("%s\n", __func__));
-
-	if (sd) {
-
-		/* Disable Function 2 */
-		sdio_claim_host(gInstance->func[2]);
-		sdio_disable_func(gInstance->func[2]);
-		sdio_release_host(gInstance->func[2]);
-
-		/* Disable Function 1 */
-		sdio_claim_host(gInstance->func[1]);
-		sdio_disable_func(gInstance->func[1]);
-		sdio_release_host(gInstance->func[1]);
-
-		/* deregister irq */
-		brcmf_sdioh_osfree(sd);
-
-		kfree(sd);
-	}
-	return 0;
-}
-
-/* Configure callback to client when we receive client interrupt */
-extern int
-brcmf_sdioh_interrupt_register(struct sdioh_info *sd, void (*fn)(void *),
-			       void *argh)
-{
-	sd_trace(("%s: Entering\n", __func__));
-	if (fn == NULL) {
-		sd_err(("%s: interrupt handler is NULL, not registering\n",
-			__func__));
-		return -EINVAL;
-	}
-
-	sd->intr_handler = fn;
-	sd->intr_handler_arg = argh;
-	sd->intr_handler_valid = true;
-
-	/* register and unmask irq */
-	if (gInstance->func[2]) {
-		sdio_claim_host(gInstance->func[2]);
-		sdio_claim_irq(gInstance->func[2], brcmf_sdioh_irqhandler_f2);
-		sdio_release_host(gInstance->func[2]);
-	}
-
-	if (gInstance->func[1]) {
-		sdio_claim_host(gInstance->func[1]);
-		sdio_claim_irq(gInstance->func[1], brcmf_sdioh_irqhandler);
-		sdio_release_host(gInstance->func[1]);
-	}
-
-	return 0;
+	bool is_err = false;
+#ifdef CONFIG_PM_SLEEP
+	is_err = atomic_read(&sdiodev->suspend);
+#endif
+	return is_err;
 }
 
-extern int brcmf_sdioh_interrupt_deregister(struct sdioh_info *sd)
+static void
+brcmf_pm_resume_wait(struct brcmf_sdio_dev *sdiodev, wait_queue_head_t *wq)
 {
-	sd_trace(("%s: Entering\n", __func__));
-
-	if (gInstance->func[1]) {
-		/* register and unmask irq */
-		sdio_claim_host(gInstance->func[1]);
-		sdio_release_irq(gInstance->func[1]);
-		sdio_release_host(gInstance->func[1]);
-	}
-
-	if (gInstance->func[2]) {
-		/* Claim host controller F2 */
-		sdio_claim_host(gInstance->func[2]);
-		sdio_release_irq(gInstance->func[2]);
-		/* Release host controller F2 */
-		sdio_release_host(gInstance->func[2]);
-	}
-
-	sd->intr_handler_valid = false;
-	sd->intr_handler = NULL;
-	sd->intr_handler_arg = NULL;
-
-	return 0;
+#ifdef CONFIG_PM_SLEEP
+	int retry = 0;
+	while (atomic_read(&sdiodev->suspend) && retry++ != 30)
+		wait_event_timeout(*wq, false, HZ/100);
+#endif
 }
 
-/* IOVar table */
-enum {
-	IOV_MSGLEVEL = 1,
-	IOV_BLOCKSIZE,
-	IOV_USEINTS,
-	IOV_NUMINTS,
-	IOV_DEVREG,
-	IOV_HCIREGS,
-	IOV_RXCHAIN
-};
-
-const struct brcmu_iovar sdioh_iovars[] = {
-	{"sd_msglevel", IOV_MSGLEVEL, 0, IOVT_UINT32, 0},
-	{"sd_blocksize", IOV_BLOCKSIZE, 0, IOVT_UINT32, 0},/* ((fn << 16) |
-								 size) */
-	{"sd_ints", IOV_USEINTS, 0, IOVT_BOOL, 0},
-	{"sd_numints", IOV_NUMINTS, 0, IOVT_UINT32, 0},
-	{"sd_devreg", IOV_DEVREG, 0, IOVT_BUFFER, sizeof(struct brcmf_sdreg)}
-	,
-	{"sd_rxchain", IOV_RXCHAIN, 0, IOVT_BOOL, 0}
-	,
-	{NULL, 0, 0, 0, 0}
-};
-
-int
-brcmf_sdioh_iovar_op(struct sdioh_info *si, const char *name,
-		     void *params, int plen, void *arg, int len, bool set)
+static inline int brcmf_sdioh_f0_write_byte(struct brcmf_sdio_dev *sdiodev,
+					    uint regaddr, u8 *byte)
 {
-	const struct brcmu_iovar *vi = NULL;
-	int bcmerror = 0;
-	int val_size;
-	s32 int_val = 0;
-	bool bool_val;
-	u32 actionid;
-
-	if (name == NULL || len <= 0)
-		return -EINVAL;
-
-	/* Set does not take qualifiers */
-	if (set && (params || plen))
-		return -EINVAL;
-
-	/* Get must have return space;*/
-	if (!set && !(arg && len))
-		return -EINVAL;
-
-	sd_trace(("%s: Enter (%s %s)\n", __func__, (set ? "set" : "get"),
-		  name));
-
-	vi = brcmu_iovar_lookup(sdioh_iovars, name);
-	if (vi == NULL) {
-		bcmerror = -ENOTSUPP;
-		goto exit;
-	}
-
-	bcmerror = brcmu_iovar_lencheck(vi, arg, len, set);
-	if (bcmerror != 0)
-		goto exit;
-
-	/* Set up params so get and set can share the convenience variables */
-	if (params == NULL) {
-		params = arg;
-		plen = len;
-	}
-
-	if (vi->type == IOVT_VOID)
-		val_size = 0;
-	else if (vi->type == IOVT_BUFFER)
-		val_size = len;
-	else
-		val_size = sizeof(int);
-
-	if (plen >= (int)sizeof(int_val))
-		memcpy(&int_val, params, sizeof(int_val));
-
-	bool_val = (int_val != 0) ? true : false;
-
-	actionid = set ? IOV_SVAL(vi->varid) : IOV_GVAL(vi->varid);
-	switch (actionid) {
-	case IOV_GVAL(IOV_MSGLEVEL):
-		int_val = (s32) sd_msglevel;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_MSGLEVEL):
-		sd_msglevel = int_val;
-		break;
-
-	case IOV_GVAL(IOV_BLOCKSIZE):
-		if ((u32) int_val > si->num_funcs) {
-			bcmerror = -EINVAL;
-			break;
-		}
-		int_val = (s32) si->client_block_size[int_val];
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_BLOCKSIZE):
-		{
-			uint func = ((u32) int_val >> 16);
-			uint blksize = (u16) int_val;
-			uint maxsize;
-
-			if (func > si->num_funcs) {
-				bcmerror = -EINVAL;
-				break;
-			}
-
-			switch (func) {
-			case 0:
-				maxsize = 32;
-				break;
-			case 1:
-				maxsize = BLOCK_SIZE_4318;
-				break;
-			case 2:
-				maxsize = BLOCK_SIZE_4328;
-				break;
-			default:
-				maxsize = 0;
-			}
-			if (blksize > maxsize) {
-				bcmerror = -EINVAL;
-				break;
-			}
-			if (!blksize)
-				blksize = maxsize;
-
-			/* Now set it */
-			si->client_block_size[func] = blksize;
-
-			break;
-		}
-
-	case IOV_GVAL(IOV_RXCHAIN):
-		int_val = false;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_GVAL(IOV_USEINTS):
-		int_val = (s32) si->use_client_ints;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_USEINTS):
-		si->use_client_ints = (bool) int_val;
-		if (si->use_client_ints)
-			si->intmask |= CLIENT_INTR;
-		else
-			si->intmask &= ~CLIENT_INTR;
-
-		break;
-
-	case IOV_GVAL(IOV_NUMINTS):
-		int_val = (s32) si->intrcount;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_GVAL(IOV_DEVREG):
-		{
-			struct brcmf_sdreg *sd_ptr =
-					(struct brcmf_sdreg *) params;
-			u8 data = 0;
-
-			if (brcmf_sdioh_cfg_read
-			    (si, sd_ptr->func, sd_ptr->offset, &data)) {
-				bcmerror = -EIO;
-				break;
-			}
-
-			int_val = (int)data;
-			memcpy(arg, &int_val, sizeof(int_val));
-			break;
-		}
-
-	case IOV_SVAL(IOV_DEVREG):
-		{
-			struct brcmf_sdreg *sd_ptr =
-					(struct brcmf_sdreg *) params;
-			u8 data = (u8) sd_ptr->value;
+	struct sdio_func *sdfunc = sdiodev->func[0];
+	int err_ret;
 
-			if (brcmf_sdioh_cfg_write
-			    (si, sd_ptr->func, sd_ptr->offset, &data)) {
-				bcmerror = -EIO;
-				break;
+	/*
+	 * Can only directly write to some F0 registers.
+	 * Handle F2 enable/disable and Abort command
+	 * as a special case.
+	 */
+	if (regaddr == SDIO_CCCR_IOEx) {
+		sdfunc = sdiodev->func[2];
+		if (sdfunc) {
+			sdio_claim_host(sdfunc);
+			if (*byte & SDIO_FUNC_ENABLE_2) {
+				/* Enable Function 2 */
+				err_ret = sdio_enable_func(sdfunc);
+				if (err_ret)
+					brcmf_dbg(ERROR,
+						  "enable F2 failed:%d\n",
+						  err_ret);
+			} else {
+				/* Disable Function 2 */
+				err_ret = sdio_disable_func(sdfunc);
+				if (err_ret)
+					brcmf_dbg(ERROR,
+						  "Disable F2 failed:%d\n",
+						  err_ret);
 			}
-			break;
-		}
-
-	default:
-		bcmerror = -ENOTSUPP;
-		break;
-	}
-exit:
-
-	return bcmerror;
-}
-
-extern int
-brcmf_sdioh_cfg_read(struct sdioh_info *sd, uint fnc_num, u32 addr, u8 *data)
-{
-	int status;
-	/* No lock needed since brcmf_sdioh_request_byte does locking */
-	status = brcmf_sdioh_request_byte(sd, SDIOH_READ, fnc_num, addr, data);
-	return status;
-}
-
-extern int
-brcmf_sdioh_cfg_write(struct sdioh_info *sd, uint fnc_num, u32 addr, u8 *data)
-{
-	/* No lock needed since brcmf_sdioh_request_byte does locking */
-	int status;
-	status = brcmf_sdioh_request_byte(sd, SDIOH_WRITE, fnc_num, addr, data);
-	return status;
-}
-
-static int brcmf_sdioh_get_cisaddr(struct sdioh_info *sd, u32 regaddr)
-{
-	/* read 24 bits and return valid 17 bit addr */
-	int i;
-	u32 scratch, regdata;
-	u8 *ptr = (u8 *)&scratch;
-	for (i = 0; i < 3; i++) {
-		if ((brcmf_sdioh_card_regread(sd, 0, regaddr, 1, &regdata)) !=
-		    SUCCESS)
-			sd_err(("%s: Can't read!\n", __func__));
-
-		*ptr++ = (u8) regdata;
-		regaddr++;
-	}
-
-	/* Only the lower 17-bits are valid */
-	scratch = le32_to_cpu(scratch);
-	scratch &= 0x0001FFFF;
-	return scratch;
-}
-
-extern int
-brcmf_sdioh_cis_read(struct sdioh_info *sd, uint func, u8 *cisd, u32 length)
-{
-	u32 count;
-	int offset;
-	u32 foo;
-	u8 *cis = cisd;
-
-	sd_trace(("%s: Func = %d\n", __func__, func));
-
-	if (!sd->func_cis_ptr[func]) {
-		memset(cis, 0, length);
-		sd_err(("%s: no func_cis_ptr[%d]\n", __func__, func));
-		return -ENOTSUPP;
-	}
-
-	sd_err(("%s: func_cis_ptr[%d]=0x%04x\n", __func__, func,
-		sd->func_cis_ptr[func]));
-
-	for (count = 0; count < length; count++) {
-		offset = sd->func_cis_ptr[func] + count;
-		if (brcmf_sdioh_card_regread(sd, 0, offset, 1, &foo) < 0) {
-			sd_err(("%s: regread failed: Can't read CIS\n",
-				__func__));
-			return -EIO;
+			sdio_release_host(sdfunc);
 		}
-
-		*cis = (u8) (foo & 0xff);
-		cis++;
+	} else if (regaddr == SDIO_CCCR_ABORT) {
+		sdio_claim_host(sdfunc);
+		sdio_writeb(sdfunc, *byte, regaddr, &err_ret);
+		sdio_release_host(sdfunc);
+	} else if (regaddr < 0xF0) {
+		brcmf_dbg(ERROR, "F0 Wr:0x%02x: write disallowed\n", regaddr);
+		err_ret = -EPERM;
+	} else {
+		sdio_claim_host(sdfunc);
+		sdio_f0_writeb(sdfunc, *byte, regaddr, &err_ret);
+		sdio_release_host(sdfunc);
 	}
 
-	return 0;
+	return err_ret;
 }
 
-extern int
-brcmf_sdioh_request_byte(struct sdioh_info *sd, uint rw, uint func,
-			 uint regaddr, u8 *byte)
+int brcmf_sdioh_request_byte(struct brcmf_sdio_dev *sdiodev, uint rw, uint func,
+			     uint regaddr, u8 *byte)
 {
 	int err_ret;
 
-	sd_info(("%s: rw=%d, func=%d, addr=0x%05x\n", __func__, rw, func,
-		 regaddr));
+	brcmf_dbg(INFO, "rw=%d, func=%d, addr=0x%05x\n", rw, func, regaddr);
 
-	BRCMF_PM_RESUME_WAIT(sdioh_request_byte_wait);
-	BRCMF_PM_RESUME_RETURN_ERROR(-EIO);
-	if (rw) {		/* CMD52 Write */
-		if (func == 0) {
-			/* Can only directly write to some F0 registers.
-			 * Handle F2 enable
-			 * as a special case.
-			 */
-			if (regaddr == SDIO_CCCR_IOEx) {
-				if (gInstance->func[2]) {
-					sdio_claim_host(gInstance->func[2]);
-					if (*byte & SDIO_FUNC_ENABLE_2) {
-						/* Enable Function 2 */
-						err_ret =
-						    sdio_enable_func
-						    (gInstance->func[2]);
-						if (err_ret)
-							sd_err(("request_byte: "
-								"enable F2 "
-								"failed:%d\n",
-								 err_ret));
-					} else {
-						/* Disable Function 2 */
-						err_ret =
-						    sdio_disable_func
-						    (gInstance->func[2]);
-						if (err_ret)
-							sd_err(("request_byte: "
-								"Disab F2 "
-								"failed:%d\n",
-								 err_ret));
-					}
-					sdio_release_host(gInstance->func[2]);
-				}
-			}
-			/* to allow abort command through F1 */
-			else if (regaddr == SDIO_CCCR_ABORT) {
-				sdio_claim_host(gInstance->func[func]);
-				/*
-				 * this sdio_f0_writeb() can be replaced
-				 * with another api
-				 * depending upon MMC driver change.
-				 * As of this time, this is temporaray one
-				 */
-				sdio_writeb(gInstance->func[func], *byte,
-					    regaddr, &err_ret);
-				sdio_release_host(gInstance->func[func]);
-			} else if (regaddr < 0xF0) {
-				sd_err(("brcmf: F0 Wr:0x%02x: write "
-					"disallowed\n", regaddr));
-			} else {
-				/* Claim host controller, perform F0 write,
-				 and release */
-				sdio_claim_host(gInstance->func[func]);
-				sdio_f0_writeb(gInstance->func[func], *byte,
-					       regaddr, &err_ret);
-				sdio_release_host(gInstance->func[func]);
-			}
-		} else {
-			/* Claim host controller, perform Fn write,
-			 and release */
-			sdio_claim_host(gInstance->func[func]);
-			sdio_writeb(gInstance->func[func], *byte, regaddr,
-				    &err_ret);
-			sdio_release_host(gInstance->func[func]);
-		}
-	} else {		/* CMD52 Read */
-		/* Claim host controller, perform Fn read, and release */
-		sdio_claim_host(gInstance->func[func]);
+	brcmf_pm_resume_wait(sdiodev, &sdiodev->request_byte_wait);
+	if (brcmf_pm_resume_error(sdiodev))
+		return -EIO;
 
-		if (func == 0) {
-			*byte =
-			    sdio_f0_readb(gInstance->func[func], regaddr,
-					  &err_ret);
+	if (rw && func == 0) {
+		/* handle F0 separately */
+		err_ret = brcmf_sdioh_f0_write_byte(sdiodev, regaddr, byte);
+	} else {
+		sdio_claim_host(sdiodev->func[func]);
+		if (rw) /* CMD52 Write */
+			sdio_writeb(sdiodev->func[func], *byte, regaddr,
+				    &err_ret);
+		else if (func == 0) {
+			*byte = sdio_f0_readb(sdiodev->func[func], regaddr,
+					      &err_ret);
 		} else {
-			*byte =
-			    sdio_readb(gInstance->func[func], regaddr,
-				       &err_ret);
+			*byte = sdio_readb(sdiodev->func[func], regaddr,
+					   &err_ret);
 		}
-
-		sdio_release_host(gInstance->func[func]);
+		sdio_release_host(sdiodev->func[func]);
 	}
 
 	if (err_ret)
-		sd_err(("brcmf: Failed to %s byte F%d:@0x%05x=%02x, "
-			"Err: %d\n", rw ? "Write" : "Read", func, regaddr,
-			*byte, err_ret));
+		brcmf_dbg(ERROR, "Failed to %s byte F%d:@0x%05x=%02x, Err: %d\n",
+			  rw ? "write" : "read", func, regaddr, *byte, err_ret);
 
 	return err_ret;
 }
 
-extern int
-brcmf_sdioh_request_word(struct sdioh_info *sd, uint cmd_type, uint rw,
-			 uint func, uint addr, u32 *word, uint nbytes)
+int brcmf_sdioh_request_word(struct brcmf_sdio_dev *sdiodev,
+			     uint rw, uint func, uint addr, u32 *word,
+			     uint nbytes)
 {
 	int err_ret = -EIO;
 
 	if (func == 0) {
-		sd_err(("%s: Only CMD52 allowed to F0.\n", __func__));
+		brcmf_dbg(ERROR, "Only CMD52 allowed to F0\n");
 		return -EINVAL;
 	}
 
-	sd_info(("%s: cmd_type=%d, rw=%d, func=%d, addr=0x%05x, nbytes=%d\n",
-		 __func__, cmd_type, rw, func, addr, nbytes));
+	brcmf_dbg(INFO, "rw=%d, func=%d, addr=0x%05x, nbytes=%d\n",
+		  rw, func, addr, nbytes);
 
-	BRCMF_PM_RESUME_WAIT(sdioh_request_word_wait);
-	BRCMF_PM_RESUME_RETURN_ERROR(-EIO);
+	brcmf_pm_resume_wait(sdiodev, &sdiodev->request_word_wait);
+	if (brcmf_pm_resume_error(sdiodev))
+		return -EIO;
 	/* Claim host controller */
-	sdio_claim_host(gInstance->func[func]);
+	sdio_claim_host(sdiodev->func[func]);
 
 	if (rw) {		/* CMD52 Write */
-		if (nbytes == 4) {
-			sdio_writel(gInstance->func[func], *word, addr,
+		if (nbytes == 4)
+			sdio_writel(sdiodev->func[func], *word, addr,
 				    &err_ret);
-		} else if (nbytes == 2) {
-			sdio_writew(gInstance->func[func], (*word & 0xFFFF),
+		else if (nbytes == 2)
+			sdio_writew(sdiodev->func[func], (*word & 0xFFFF),
 				    addr, &err_ret);
-		} else {
-			sd_err(("%s: Invalid nbytes: %d\n", __func__, nbytes));
-		}
+		else
+			brcmf_dbg(ERROR, "Invalid nbytes: %d\n", nbytes);
 	} else {		/* CMD52 Read */
-		if (nbytes == 4) {
-			*word =
-			    sdio_readl(gInstance->func[func], addr, &err_ret);
-		} else if (nbytes == 2) {
-			*word =
-			    sdio_readw(gInstance->func[func], addr,
-				       &err_ret) & 0xFFFF;
-		} else {
-			sd_err(("%s: Invalid nbytes: %d\n", __func__, nbytes));
-		}
+		if (nbytes == 4)
+			*word = sdio_readl(sdiodev->func[func], addr, &err_ret);
+		else if (nbytes == 2)
+			*word = sdio_readw(sdiodev->func[func], addr,
+					   &err_ret) & 0xFFFF;
+		else
+			brcmf_dbg(ERROR, "Invalid nbytes: %d\n", nbytes);
 	}
 
 	/* Release host controller */
-	sdio_release_host(gInstance->func[func]);
+	sdio_release_host(sdiodev->func[func]);
 
-	if (err_ret) {
-		sd_err(("brcmf: Failed to %s word, Err: 0x%08x\n",
-			rw ? "Write" : "Read", err_ret));
-	}
+	if (err_ret)
+		brcmf_dbg(ERROR, "Failed to %s word, Err: 0x%08x\n",
+			  rw ? "write" : "read", err_ret);
 
 	return err_ret;
 }
 
 static int
-brcmf_sdioh_request_packet(struct sdioh_info *sd, uint fix_inc, uint write,
-			   uint func, uint addr, struct sk_buff *pkt)
+brcmf_sdioh_request_packet(struct brcmf_sdio_dev *sdiodev, uint fix_inc,
+			   uint write, uint func, uint addr,
+			   struct sk_buff *pkt)
 {
 	bool fifo = (fix_inc == SDIOH_DATA_FIX);
 	u32 SGCount = 0;
@@ -806,46 +215,45 @@ brcmf_sdioh_request_packet(struct sdioh_info *sd, uint fix_inc, uint write,
 
 	struct sk_buff *pnext;
 
-	sd_trace(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
-	BRCMF_PM_RESUME_WAIT(sdioh_request_packet_wait);
-	BRCMF_PM_RESUME_RETURN_ERROR(-EIO);
+	brcmf_pm_resume_wait(sdiodev, &sdiodev->request_packet_wait);
+	if (brcmf_pm_resume_error(sdiodev))
+		return -EIO;
 
 	/* Claim host controller */
-	sdio_claim_host(gInstance->func[func]);
+	sdio_claim_host(sdiodev->func[func]);
 	for (pnext = pkt; pnext; pnext = pnext->next) {
 		uint pkt_len = pnext->len;
 		pkt_len += 3;
 		pkt_len &= 0xFFFFFFFC;
 
 		if ((write) && (!fifo)) {
-			err_ret = sdio_memcpy_toio(gInstance->func[func], addr,
+			err_ret = sdio_memcpy_toio(sdiodev->func[func], addr,
 						   ((u8 *) (pnext->data)),
 						   pkt_len);
 		} else if (write) {
-			err_ret = sdio_memcpy_toio(gInstance->func[func], addr,
+			err_ret = sdio_memcpy_toio(sdiodev->func[func], addr,
 						   ((u8 *) (pnext->data)),
 						   pkt_len);
 		} else if (fifo) {
-			err_ret = sdio_readsb(gInstance->func[func],
+			err_ret = sdio_readsb(sdiodev->func[func],
 					      ((u8 *) (pnext->data)),
 					      addr, pkt_len);
 		} else {
-			err_ret = sdio_memcpy_fromio(gInstance->func[func],
+			err_ret = sdio_memcpy_fromio(sdiodev->func[func],
 						     ((u8 *) (pnext->data)),
 						     addr, pkt_len);
 		}
 
 		if (err_ret) {
-			sd_err(("%s: %s FAILED %p[%d], addr=0x%05x, pkt_len=%d,"
-				 "ERR=0x%08x\n", __func__,
-				 (write) ? "TX" : "RX",
-				 pnext, SGCount, addr, pkt_len, err_ret));
+			brcmf_dbg(ERROR, "%s FAILED %p[%d], addr=0x%05x, pkt_len=%d, ERR=0x%08x\n",
+				  write ? "TX" : "RX", pnext, SGCount, addr,
+				  pkt_len, err_ret);
 		} else {
-			sd_trace(("%s: %s xfr'd %p[%d], addr=0x%05x, len=%d\n",
-				  __func__,
-				  (write) ? "TX" : "RX",
-				  pnext, SGCount, addr, pkt_len));
+			brcmf_dbg(TRACE, "%s xfr'd %p[%d], addr=0x%05x, len=%d\n",
+				  write ? "TX" : "RX", pnext, SGCount, addr,
+				  pkt_len);
 		}
 
 		if (!fifo)
@@ -855,9 +263,9 @@ brcmf_sdioh_request_packet(struct sdioh_info *sd, uint fix_inc, uint write,
 	}
 
 	/* Release host controller */
-	sdio_release_host(gInstance->func[func]);
+	sdio_release_host(sdiodev->func[func]);
 
-	sd_trace(("%s: Exit\n", __func__));
+	brcmf_dbg(TRACE, "Exit\n");
 	return err_ret;
 }
 
@@ -876,26 +284,27 @@ brcmf_sdioh_request_packet(struct sdioh_info *sd, uint fix_inc, uint write,
  * aligned packet.
  *
  */
-extern int
-brcmf_sdioh_request_buffer(struct sdioh_info *sd, uint pio_dma, uint fix_inc,
-			   uint write, uint func, uint addr, uint reg_width,
-			   uint buflen_u, u8 *buffer, struct sk_buff *pkt)
+int brcmf_sdioh_request_buffer(struct brcmf_sdio_dev *sdiodev,
+			       uint fix_inc, uint write, uint func, uint addr,
+			       uint reg_width, uint buflen_u, u8 *buffer,
+			       struct sk_buff *pkt)
 {
 	int Status;
 	struct sk_buff *mypkt = NULL;
 
-	sd_trace(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
-	BRCMF_PM_RESUME_WAIT(sdioh_request_buffer_wait);
-	BRCMF_PM_RESUME_RETURN_ERROR(-EIO);
+	brcmf_pm_resume_wait(sdiodev, &sdiodev->request_buffer_wait);
+	if (brcmf_pm_resume_error(sdiodev))
+		return -EIO;
 	/* Case 1: we don't have a packet. */
 	if (pkt == NULL) {
-		sd_data(("%s: Creating new %s Packet, len=%d\n",
-			 __func__, write ? "TX" : "RX", buflen_u));
+		brcmf_dbg(DATA, "Creating new %s Packet, len=%d\n",
+			  write ? "TX" : "RX", buflen_u);
 		mypkt = brcmu_pkt_buf_get_skb(buflen_u);
 		if (!mypkt) {
-			sd_err(("%s: brcmu_pkt_buf_get_skb failed: len %d\n",
-				__func__, buflen_u));
+			brcmf_dbg(ERROR, "brcmu_pkt_buf_get_skb failed: len %d\n",
+				  buflen_u);
 			return -EIO;
 		}
 
@@ -903,8 +312,8 @@ brcmf_sdioh_request_buffer(struct sdioh_info *sd, uint pio_dma, uint fix_inc,
 		if (write)
 			memcpy(mypkt->data, buffer, buflen_u);
 
-		Status = brcmf_sdioh_request_packet(sd, fix_inc, write, func,
-						    addr, mypkt);
+		Status = brcmf_sdioh_request_packet(sdiodev, fix_inc, write,
+						    func, addr, mypkt);
 
 		/* For a read, copy the packet data back to the buffer. */
 		if (!write)
@@ -916,12 +325,12 @@ brcmf_sdioh_request_buffer(struct sdioh_info *sd, uint pio_dma, uint fix_inc,
 		 * Case 2: We have a packet, but it is unaligned.
 		 * In this case, we cannot have a chain (pkt->next == NULL)
 		 */
-		sd_data(("%s: Creating aligned %s Packet, len=%d\n",
-			 __func__, write ? "TX" : "RX", pkt->len));
+		brcmf_dbg(DATA, "Creating aligned %s Packet, len=%d\n",
+			  write ? "TX" : "RX", pkt->len);
 		mypkt = brcmu_pkt_buf_get_skb(pkt->len);
 		if (!mypkt) {
-			sd_err(("%s: brcmu_pkt_buf_get_skb failed: len %d\n",
-				__func__, pkt->len));
+			brcmf_dbg(ERROR, "brcmu_pkt_buf_get_skb failed: len %d\n",
+				  pkt->len);
 			return -EIO;
 		}
 
@@ -929,8 +338,8 @@ brcmf_sdioh_request_buffer(struct sdioh_info *sd, uint pio_dma, uint fix_inc,
 		if (write)
 			memcpy(mypkt->data, pkt->data, pkt->len);
 
-		Status = brcmf_sdioh_request_packet(sd, fix_inc, write, func,
-						    addr, mypkt);
+		Status = brcmf_sdioh_request_packet(sdiodev, fix_inc, write,
+						    func, addr, mypkt);
 
 		/* For a read, copy the packet data back to the buffer. */
 		if (!write)
@@ -939,128 +348,185 @@ brcmf_sdioh_request_buffer(struct sdioh_info *sd, uint pio_dma, uint fix_inc,
 		brcmu_pkt_buf_free_skb(mypkt);
 	} else {		/* case 3: We have a packet and
 				 it is aligned. */
-		sd_data(("%s: Aligned %s Packet, direct DMA\n",
-			 __func__, write ? "Tx" : "Rx"));
-		Status = brcmf_sdioh_request_packet(sd, fix_inc, write, func,
-						    addr, pkt);
+		brcmf_dbg(DATA, "Aligned %s Packet, direct DMA\n",
+			  write ? "Tx" : "Rx");
+		Status = brcmf_sdioh_request_packet(sdiodev, fix_inc, write,
+						    func, addr, pkt);
 	}
 
 	return Status;
 }
 
-/* this function performs "abort" for both of host & device */
-extern int brcmf_sdioh_abort(struct sdioh_info *sd, uint func)
-{
-	char t_func = (char)func;
-	sd_trace(("%s: Enter\n", __func__));
-
-	/* issue abort cmd52 command through F0 */
-	brcmf_sdioh_request_byte(sd, SDIOH_WRITE, SDIO_FUNC_0, SDIO_CCCR_ABORT,
-			   &t_func);
-
-	sd_trace(("%s: Exit\n", __func__));
-	return 0;
-}
-
-/* Disable device interrupt */
-void brcmf_sdioh_dev_intr_off(struct sdioh_info *sd)
-{
-	sd_trace(("%s: %d\n", __func__, sd->use_client_ints));
-	sd->intmask &= ~CLIENT_INTR;
-}
-
-/* Enable device interrupt */
-void brcmf_sdioh_dev_intr_on(struct sdioh_info *sd)
-{
-	sd_trace(("%s: %d\n", __func__, sd->use_client_ints));
-	sd->intmask |= CLIENT_INTR;
-}
-
 /* Read client card reg */
-int
-brcmf_sdioh_card_regread(struct sdioh_info *sd, int func, u32 regaddr,
+static int
+brcmf_sdioh_card_regread(struct brcmf_sdio_dev *sdiodev, int func, u32 regaddr,
 			 int regsize, u32 *data)
 {
 
 	if ((func == 0) || (regsize == 1)) {
 		u8 temp = 0;
 
-		brcmf_sdioh_request_byte(sd, SDIOH_READ, func, regaddr, &temp);
+		brcmf_sdioh_request_byte(sdiodev, SDIOH_READ, func, regaddr,
+					 &temp);
 		*data = temp;
 		*data &= 0xff;
-		sd_data(("%s: byte read data=0x%02x\n", __func__, *data));
+		brcmf_dbg(DATA, "byte read data=0x%02x\n", *data);
 	} else {
-		brcmf_sdioh_request_word(sd, 0, SDIOH_READ, func, regaddr, data,
-				   regsize);
+		brcmf_sdioh_request_word(sdiodev, SDIOH_READ, func, regaddr,
+					 data, regsize);
 		if (regsize == 2)
 			*data &= 0xffff;
 
-		sd_data(("%s: word read data=0x%08x\n", __func__, *data));
+		brcmf_dbg(DATA, "word read data=0x%08x\n", *data);
 	}
 
 	return SUCCESS;
 }
 
-static void brcmf_sdioh_irqhandler(struct sdio_func *func)
+static int brcmf_sdioh_get_cisaddr(struct brcmf_sdio_dev *sdiodev, u32 regaddr)
 {
-	struct sdioh_info *sd;
+	/* read 24 bits and return valid 17 bit addr */
+	int i;
+	u32 scratch, regdata;
+	__le32 scratch_le;
+	u8 *ptr = (u8 *)&scratch_le;
 
-	sd_trace(("brcmf: ***IRQHandler\n"));
-	sd = gInstance->sd;
+	for (i = 0; i < 3; i++) {
+		if ((brcmf_sdioh_card_regread(sdiodev, 0, regaddr, 1,
+				&regdata)) != SUCCESS)
+			brcmf_dbg(ERROR, "Can't read!\n");
 
-	sdio_release_host(gInstance->func[0]);
+		*ptr++ = (u8) regdata;
+		regaddr++;
+	}
 
-	if (sd->use_client_ints) {
-		sd->intrcount++;
-		(sd->intr_handler) (sd->intr_handler_arg);
-	} else {
-		sd_err(("brcmf: ***IRQHandler\n"));
+	/* Only the lower 17-bits are valid */
+	scratch = le32_to_cpu(scratch_le);
+	scratch &= 0x0001FFFF;
+	return scratch;
+}
 
-		sd_err(("%s: Not ready for intr: enabled %d, handler %p\n",
-			__func__, sd->client_intr_enabled, sd->intr_handler));
+static int brcmf_sdioh_enablefuncs(struct brcmf_sdio_dev *sdiodev)
+{
+	int err_ret;
+	u32 fbraddr;
+	u8 func;
+
+	brcmf_dbg(TRACE, "\n");
+
+	/* Get the Card's common CIS address */
+	sdiodev->func_cis_ptr[0] = brcmf_sdioh_get_cisaddr(sdiodev,
+							   SDIO_CCCR_CIS);
+	brcmf_dbg(INFO, "Card's Common CIS Ptr = 0x%x\n",
+		  sdiodev->func_cis_ptr[0]);
+
+	/* Get the Card's function CIS (for each function) */
+	for (fbraddr = SDIO_FBR_BASE(1), func = 1;
+	     func <= sdiodev->num_funcs; func++, fbraddr += SDIOD_FBR_SIZE) {
+		sdiodev->func_cis_ptr[func] =
+		    brcmf_sdioh_get_cisaddr(sdiodev, SDIO_FBR_CIS + fbraddr);
+		brcmf_dbg(INFO, "Function %d CIS Ptr = 0x%x\n",
+			  func, sdiodev->func_cis_ptr[func]);
+	}
+
+	/* Enable Function 1 */
+	sdio_claim_host(sdiodev->func[1]);
+	err_ret = sdio_enable_func(sdiodev->func[1]);
+	sdio_release_host(sdiodev->func[1]);
+	if (err_ret)
+		brcmf_dbg(ERROR, "Failed to enable F1 Err: 0x%08x\n", err_ret);
+
+	return false;
+}
+
+/*
+ *	Public entry points & extern's
+ */
+int brcmf_sdioh_attach(struct brcmf_sdio_dev *sdiodev)
+{
+	int err_ret = 0;
+
+	brcmf_dbg(TRACE, "\n");
+
+	sdiodev->num_funcs = 2;
+
+	sdio_claim_host(sdiodev->func[1]);
+	err_ret = sdio_set_block_size(sdiodev->func[1], SDIO_FUNC1_BLOCKSIZE);
+	sdio_release_host(sdiodev->func[1]);
+	if (err_ret) {
+		brcmf_dbg(ERROR, "Failed to set F1 blocksize\n");
+		goto out;
+	}
+
+	sdio_claim_host(sdiodev->func[2]);
+	err_ret = sdio_set_block_size(sdiodev->func[2], SDIO_FUNC2_BLOCKSIZE);
+	sdio_release_host(sdiodev->func[2]);
+	if (err_ret) {
+		brcmf_dbg(ERROR, "Failed to set F2 blocksize\n");
+		goto out;
 	}
 
-	sdio_claim_host(gInstance->func[0]);
+	brcmf_sdioh_enablefuncs(sdiodev);
+
+out:
+	brcmf_dbg(TRACE, "Done\n");
+	return err_ret;
 }
 
-/* interrupt handler for F2 (dummy handler) */
-static void brcmf_sdioh_irqhandler_f2(struct sdio_func *func)
+void brcmf_sdioh_detach(struct brcmf_sdio_dev *sdiodev)
 {
-	struct sdioh_info *sd;
+	brcmf_dbg(TRACE, "\n");
+
+	/* Disable Function 2 */
+	sdio_claim_host(sdiodev->func[2]);
+	sdio_disable_func(sdiodev->func[2]);
+	sdio_release_host(sdiodev->func[2]);
 
-	sd_trace(("brcmf: ***IRQHandlerF2\n"));
+	/* Disable Function 1 */
+	sdio_claim_host(sdiodev->func[1]);
+	sdio_disable_func(sdiodev->func[1]);
+	sdio_release_host(sdiodev->func[1]);
 
-	sd = gInstance->sd;
 }
 
 static int brcmf_ops_sdio_probe(struct sdio_func *func,
 			      const struct sdio_device_id *id)
 {
 	int ret = 0;
-	static struct sdio_func sdio_func_0;
-	sd_trace(("sdio_probe: %s Enter\n", __func__));
-	sd_trace(("sdio_probe: func->class=%x\n", func->class));
-	sd_trace(("sdio_vendor: 0x%04x\n", func->vendor));
-	sd_trace(("sdio_device: 0x%04x\n", func->device));
-	sd_trace(("Function#: 0x%04x\n", func->num));
+	struct brcmf_sdio_dev *sdiodev;
+	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_dbg(TRACE, "func->class=%x\n", func->class);
+	brcmf_dbg(TRACE, "sdio_vendor: 0x%04x\n", func->vendor);
+	brcmf_dbg(TRACE, "sdio_device: 0x%04x\n", func->device);
+	brcmf_dbg(TRACE, "Function#: 0x%04x\n", func->num);
 
 	if (func->num == 1) {
-		sdio_func_0.num = 0;
-		sdio_func_0.card = func->card;
-		gInstance->func[0] = &sdio_func_0;
-		if (func->device == 0x4) {	/* 4318 */
-			gInstance->func[2] = NULL;
-			sd_trace(("NIC found, calling brcmf_sdio_probe...\n"));
-			ret = brcmf_sdio_probe(&sdmmc_dev);
+		if (dev_get_drvdata(&func->card->dev)) {
+			brcmf_dbg(ERROR, "card private drvdata occupied\n");
+			return -ENXIO;
 		}
-	}
+		sdiodev = kzalloc(sizeof(struct brcmf_sdio_dev), GFP_KERNEL);
+		if (!sdiodev)
+			return -ENOMEM;
+		sdiodev->func[0] = func->card->sdio_func[0];
+		sdiodev->func[1] = func;
+		dev_set_drvdata(&func->card->dev, sdiodev);
 
-	gInstance->func[func->num] = func;
+		atomic_set(&sdiodev->suspend, false);
+		init_waitqueue_head(&sdiodev->request_byte_wait);
+		init_waitqueue_head(&sdiodev->request_word_wait);
+		init_waitqueue_head(&sdiodev->request_packet_wait);
+		init_waitqueue_head(&sdiodev->request_buffer_wait);
+	}
 
 	if (func->num == 2) {
-		brcmf_cfg80211_sdio_func(func);
-		sd_trace(("F2 found, calling brcmf_sdio_probe...\n"));
-		ret = brcmf_sdio_probe(&sdmmc_dev);
+		sdiodev = dev_get_drvdata(&func->card->dev);
+		if ((!sdiodev) || (sdiodev->func[1]->card != func->card))
+			return -ENODEV;
+		sdiodev->func[2] = func;
+
+		brcmf_dbg(TRACE, "F2 found, calling brcmf_sdio_probe...\n");
+		ret = brcmf_sdio_probe(sdiodev);
 	}
 
 	return ret;
@@ -1068,129 +534,93 @@ static int brcmf_ops_sdio_probe(struct sdio_func *func,
 
 static void brcmf_ops_sdio_remove(struct sdio_func *func)
 {
-	sd_trace(("%s Enter\n", __func__));
-	sd_info(("func->class=%x\n", func->class));
-	sd_info(("sdio_vendor: 0x%04x\n", func->vendor));
-	sd_info(("sdio_device: 0x%04x\n", func->device));
-	sd_info(("Function#: 0x%04x\n", func->num));
+	struct brcmf_sdio_dev *sdiodev;
+	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_dbg(INFO, "func->class=%x\n", func->class);
+	brcmf_dbg(INFO, "sdio_vendor: 0x%04x\n", func->vendor);
+	brcmf_dbg(INFO, "sdio_device: 0x%04x\n", func->device);
+	brcmf_dbg(INFO, "Function#: 0x%04x\n", func->num);
 
 	if (func->num == 2) {
-		sd_trace(("F2 found, calling brcmf_sdio_remove...\n"));
-		brcmf_sdio_remove(&sdmmc_dev);
+		sdiodev = dev_get_drvdata(&func->card->dev);
+		brcmf_dbg(TRACE, "F2 found, calling brcmf_sdio_remove...\n");
+		brcmf_sdio_remove(sdiodev);
+		dev_set_drvdata(&func->card->dev, NULL);
+		kfree(sdiodev);
 	}
 }
 
-
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int brcmf_sdio_suspend(struct device *dev)
 {
 	mmc_pm_flag_t sdio_flags;
+	struct brcmf_sdio_dev *sdiodev;
+	struct sdio_func *func = dev_to_sdio_func(dev);
 	int ret = 0;
 
-	sd_trace(("%s\n", __func__));
+	brcmf_dbg(TRACE, "\n");
+
+	sdiodev = dev_get_drvdata(&func->card->dev);
+
+	atomic_set(&sdiodev->suspend, true);
 
-	sdio_flags = sdio_get_host_pm_caps(gInstance->func[1]);
+	sdio_flags = sdio_get_host_pm_caps(sdiodev->func[1]);
 	if (!(sdio_flags & MMC_PM_KEEP_POWER)) {
-		sd_err(("Host can't keep power while suspended\n"));
+		brcmf_dbg(ERROR, "Host can't keep power while suspended\n");
 		return -EINVAL;
 	}
 
-	ret = sdio_set_host_pm_flags(gInstance->func[1], MMC_PM_KEEP_POWER);
+	ret = sdio_set_host_pm_flags(sdiodev->func[1], MMC_PM_KEEP_POWER);
 	if (ret) {
-		sd_err(("Failed to set pm_flags\n"));
+		brcmf_dbg(ERROR, "Failed to set pm_flags\n");
 		return ret;
 	}
 
-	brcmf_sdio_wdtmr_enable(false);
+	brcmf_sdio_wdtmr_enable(sdiodev, false);
 
 	return ret;
 }
 
 static int brcmf_sdio_resume(struct device *dev)
 {
-	brcmf_sdio_wdtmr_enable(true);
-	return 0;
-}
-#endif		/* CONFIG_PM */
-
-int brcmf_sdioh_osinit(struct sdioh_info *sd)
-{
-	struct sdos_info *sdos;
-
-	sdos = kmalloc(sizeof(struct sdos_info), GFP_ATOMIC);
-	sd->sdos_info = (void *)sdos;
-	if (sdos == NULL)
-		return -ENOMEM;
+	struct brcmf_sdio_dev *sdiodev;
+	struct sdio_func *func = dev_to_sdio_func(dev);
 
-	sdos->sd = sd;
-	spin_lock_init(&sdos->lock);
+	sdiodev = dev_get_drvdata(&func->card->dev);
+	brcmf_sdio_wdtmr_enable(sdiodev, true);
+	atomic_set(&sdiodev->suspend, false);
 	return 0;
 }
 
-void brcmf_sdioh_osfree(struct sdioh_info *sd)
-{
-	struct sdos_info *sdos;
-
-	sdos = (struct sdos_info *)sd->sdos_info;
-	kfree(sdos);
-}
-
-/* Interrupt enable/disable */
-int brcmf_sdioh_interrupt_set(struct sdioh_info *sd, bool enable)
-{
-	unsigned long flags;
-	struct sdos_info *sdos;
-
-	sd_trace(("%s: %s\n", __func__, enable ? "Enabling" : "Disabling"));
-
-	sdos = (struct sdos_info *)sd->sdos_info;
-
-	if (enable && !(sd->intr_handler && sd->intr_handler_arg)) {
-		sd_err(("%s: no handler registered, will not enable\n",
-			__func__));
-		return -EINVAL;
-	}
-
-	/* Ensure atomicity for enable/disable calls */
-	spin_lock_irqsave(&sdos->lock, flags);
-
-	sd->client_intr_enabled = enable;
-	if (enable)
-		brcmf_sdioh_dev_intr_on(sd);
-	else
-		brcmf_sdioh_dev_intr_off(sd);
-
-	spin_unlock_irqrestore(&sdos->lock, flags);
+static const struct dev_pm_ops brcmf_sdio_pm_ops = {
+	.suspend	= brcmf_sdio_suspend,
+	.resume		= brcmf_sdio_resume,
+};
+#endif	/* CONFIG_PM_SLEEP */
 
-	return 0;
-}
+static struct sdio_driver brcmf_sdmmc_driver = {
+	.probe = brcmf_ops_sdio_probe,
+	.remove = brcmf_ops_sdio_remove,
+	.name = "brcmfmac",
+	.id_table = brcmf_sdmmc_ids,
+#ifdef CONFIG_PM_SLEEP
+	.drv = {
+		.pm = &brcmf_sdio_pm_ops,
+	},
+#endif	/* CONFIG_PM_SLEEP */
+};
 
-/*
- * module init
-*/
-int brcmf_sdio_function_init(void)
+/* bus register interface */
+int brcmf_bus_register(void)
 {
-	int error = 0;
-	sd_trace(("brcmf_sdio_function_init: %s Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
-	gInstance = kzalloc(sizeof(struct brcmf_sdmmc_instance), GFP_KERNEL);
-	if (!gInstance)
-		return -ENOMEM;
-
-	memset(&sdmmc_dev, 0, sizeof(sdmmc_dev));
-	error = sdio_register_driver(&brcmf_sdmmc_driver);
-
-	return error;
+	return sdio_register_driver(&brcmf_sdmmc_driver);
 }
 
-/*
- * module cleanup
-*/
-void brcmf_sdio_function_cleanup(void)
+void brcmf_bus_unregister(void)
 {
-	sd_trace(("%s Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	sdio_unregister_driver(&brcmf_sdmmc_driver);
-
-	kfree(gInstance);
 }
diff --git a/drivers/staging/brcm80211/brcmfmac/dhd.h b/drivers/staging/brcm80211/brcmfmac/dhd.h
index 82bf04df16d0..4645766b4070 100644
--- a/drivers/staging/brcm80211/brcmfmac/dhd.h
+++ b/drivers/staging/brcm80211/brcmfmac/dhd.h
@@ -23,10 +23,9 @@
 
 #define BRCMF_VERSION_STR		"4.218.248.5"
 
-#define	BRCMF_C_IOCTL_SMLEN	256	/* "small" ioctl buffer required */
-#define BRCMF_C_IOCTL_MEDLEN	1536	/* "med" ioctl buffer required */
-#define	BRCMF_C_IOCTL_MAXLEN	8192
-
+/*******************************************************************************
+ * IO codes that are interpreted by dongle firmware
+ ******************************************************************************/
 #define BRCMF_C_UP				2
 #define BRCMF_C_SET_PROMISC			10
 #define BRCMF_C_GET_RATE			12
@@ -83,18 +82,11 @@
 #define	WLC_PHY_TYPE_LCN	8
 #define	WLC_PHY_TYPE_NULL	0xf
 
-#define BRCMF_PKT_FILTER_FIXED_LEN	offsetof(struct brcmf_pkt_filter, u)
-#define BRCMF_PKT_FILTER_PATTERN_FIXED_LEN	\
-	offsetof(struct brcmf_pkt_filter_pattern, mask_and_pattern)
-
 #define BRCMF_EVENTING_MASK_LEN	16
 
 #define TOE_TX_CSUM_OL		0x00000001
 #define TOE_RX_CSUM_OL		0x00000002
 
-/* maximum channels returned by the get valid channels iovar */
-#define WL_NUMCHANNELS		64
-
 #define	BRCMF_BSS_INFO_VERSION	108 /* current ver of brcmf_bss_info struct */
 
 /* size of brcmf_scan_params not including variable length array */
@@ -117,11 +109,16 @@
 #define BRCMF_SCAN_RESULTS_ABORTED	3
 #define BRCMF_SCAN_RESULTS_NO_MEM	4
 
-#define WL_SOFT_KEY	(1 << 0)	/* Indicates this key is using soft encrypt */
-#define BRCMF_PRIMARY_KEY	(1 << 1)	/* primary (ie tx) key */
-#define WL_KF_RES_4	(1 << 4)	/* Reserved for backward compat */
-#define WL_KF_RES_5	(1 << 5)	/* Reserved for backward compat */
-#define WL_IBSS_PEER_GROUP_KEY	(1 << 6)	/* Indicates a group key for a IBSS PEER */
+/* Indicates this key is using soft encrypt */
+#define WL_SOFT_KEY	(1 << 0)
+/* primary (ie tx) key */
+#define BRCMF_PRIMARY_KEY	(1 << 1)
+/* Reserved for backward compat */
+#define WL_KF_RES_4	(1 << 4)
+/* Reserved for backward compat */
+#define WL_KF_RES_5	(1 << 5)
+/* Indicates a group key for a IBSS PEER */
+#define WL_IBSS_PEER_GROUP_KEY	(1 << 6)
 
 /* For supporting multiple interfaces */
 #define BRCMF_MAX_IFS	16
@@ -136,13 +133,13 @@
 #define BRCMF_EVENT_MSG_GROUP		0x04
 
 struct brcmf_event_msg {
-	u16 version;
-	u16 flags;
-	u32 event_type;
-	u32 status;
-	u32 reason;
-	u32 auth_type;
-	u32 datalen;
+	__be16 version;
+	__be16 flags;
+	__be32 event_type;
+	__be32 status;
+	__be32 reason;
+	__be32 auth_type;
+	__be32 datalen;
 	u8 addr[ETH_ALEN];
 	char ifname[IFNAMSIZ];
 } __packed;
@@ -173,6 +170,7 @@ struct dngl_stats {
 	unsigned long multicast;	/* multicast packets received */
 };
 
+/* event codes sent by the dongle to this driver */
 #define BRCMF_E_SET_SSID			0
 #define BRCMF_E_JOIN				1
 #define BRCMF_E_START				2
@@ -332,30 +330,35 @@ enum brcmf_bus_state {
  * start matching, the pattern to match, the size of the pattern, and a bitmask
  * that indicates which bits within the pattern should be matched.
  */
-struct brcmf_pkt_filter_pattern {
-	u32 offset;		/* Offset within received packet to start pattern matching.
-				 * Offset '0' is the first byte of the ethernet header.
-				 */
-	u32 size_bytes;	/* Size of the pattern.  Bitmask must be the same size. */
-	u8 mask_and_pattern[1];	/* Variable length mask and pattern data.  mask starts
-					 * at offset 0.  Pattern immediately follows mask.
-					 */
+struct brcmf_pkt_filter_pattern_le {
+	/*
+	 * Offset within received packet to start pattern matching.
+	 * Offset '0' is the first byte of the ethernet header.
+	 */
+	__le32 offset;
+	/* Size of the pattern.  Bitmask must be the same size.*/
+	__le32 size_bytes;
+	/*
+	 * Variable length mask and pattern data. mask starts at offset 0.
+	 * Pattern immediately follows mask.
+	 */
+	u8 mask_and_pattern[1];
 };
 
 /* IOVAR "pkt_filter_add" parameter. Used to install packet filters. */
-struct brcmf_pkt_filter {
-	u32 id;		/* Unique filter id, specified by app. */
-	u32 type;		/* Filter type (WL_PKT_FILTER_TYPE_xxx). */
-	u32 negate_match;	/* Negate the result of filter matches */
+struct brcmf_pkt_filter_le {
+	__le32 id;		/* Unique filter id, specified by app. */
+	__le32 type;		/* Filter type (WL_PKT_FILTER_TYPE_xxx). */
+	__le32 negate_match;	/* Negate the result of filter matches */
 	union {			/* Filter definitions */
-		struct brcmf_pkt_filter_pattern pattern; /* Filter pattern */
+		struct brcmf_pkt_filter_pattern_le pattern; /* Filter pattern */
 	} u;
 };
 
 /* IOVAR "pkt_filter_enable" parameter. */
-struct brcmf_pkt_filter_enable {
-	u32 id;		/* Unique filter id */
-	u32 enable;		/* Enable/disable bool */
+struct brcmf_pkt_filter_enable_le {
+	__le32 id;		/* Unique filter id */
+	__le32 enable;		/* Enable/disable bool */
 };
 
 /* BSS info structure
@@ -363,63 +366,76 @@ struct brcmf_pkt_filter_enable {
  * next bss_info structure in a vector (in struct brcmf_scan_results)
  */
 struct brcmf_bss_info {
-	u32 version;		/* version field */
-	u32 length;		/* byte length of data in this record,
+	__le32 version;		/* version field */
+	__le32 length;		/* byte length of data in this record,
 				 * starting at version and including IEs
 				 */
 	u8 BSSID[ETH_ALEN];
-	u16 beacon_period;	/* units are Kusec */
-	u16 capability;	/* Capability information */
+	__le16 beacon_period;	/* units are Kusec */
+	__le16 capability;	/* Capability information */
 	u8 SSID_len;
 	u8 SSID[32];
 	struct {
-		uint count;	/* # rates in this set */
-		u8 rates[16];	/* rates in 500kbps units w/hi bit set if basic */
+		__le32 count;   /* # rates in this set */
+		u8 rates[16]; /* rates in 500kbps units w/hi bit set if basic */
 	} rateset;		/* supported rates */
-	chanspec_t chanspec;	/* chanspec for bss */
-	u16 atim_window;	/* units are Kusec */
+	__le16 chanspec;	/* chanspec for bss */
+	__le16 atim_window;	/* units are Kusec */
 	u8 dtim_period;	/* DTIM period */
-	s16 RSSI;		/* receive signal strength (in dBm) */
+	__le16 RSSI;		/* receive signal strength (in dBm) */
 	s8 phy_noise;		/* noise (in dBm) */
 
 	u8 n_cap;		/* BSS is 802.11N Capable */
-	u32 nbss_cap;	/* 802.11N BSS Capabilities (based on HT_CAP_*) */
+	/* 802.11N BSS Capabilities (based on HT_CAP_*): */
+	__le32 nbss_cap;
 	u8 ctl_ch;		/* 802.11N BSS control channel number */
-	u32 reserved32[1];	/* Reserved for expansion of BSS properties */
+	__le32 reserved32[1];	/* Reserved for expansion of BSS properties */
 	u8 flags;		/* flags */
 	u8 reserved[3];	/* Reserved for expansion of BSS properties */
 	u8 basic_mcs[MCSSET_LEN];	/* 802.11N BSS required MCS set */
 
-	u16 ie_offset;	/* offset at which IEs start, from beginning */
-	u32 ie_length;	/* byte length of Information Elements */
-	s16 SNR;		/* average SNR of during frame reception */
+	__le16 ie_offset;	/* offset at which IEs start, from beginning */
+	__le32 ie_length;	/* byte length of Information Elements */
+	__le16 SNR;		/* average SNR of during frame reception */
 	/* Add new fields here */
 	/* variable length Information Elements */
 };
 
+struct brcm_rateset_le {
+	/* # rates in this set */
+	__le32 count;
+	/* rates in 500kbps units w/hi bit set if basic */
+	u8 rates[WL_NUMRATES];
+};
+
 struct brcmf_ssid {
 	u32 SSID_len;
 	unsigned char SSID[32];
 };
 
-struct brcmf_scan_params {
-	struct brcmf_ssid ssid;	/* default: {0, ""} */
+struct brcmf_ssid_le {
+	__le32 SSID_len;
+	unsigned char SSID[32];
+};
+
+struct brcmf_scan_params_le {
+	struct brcmf_ssid_le ssid_le;	/* default: {0, ""} */
 	u8 bssid[ETH_ALEN];	/* default: bcast */
 	s8 bss_type;		/* default: any,
 				 * DOT11_BSSTYPE_ANY/INFRASTRUCTURE/INDEPENDENT
 				 */
 	u8 scan_type;	/* flags, 0 use default */
-	s32 nprobes;		/* -1 use default, number of probes per channel */
-	s32 active_time;	/* -1 use default, dwell time per channel for
+	__le32 nprobes;	  /* -1 use default, number of probes per channel */
+	__le32 active_time;	/* -1 use default, dwell time per channel for
 				 * active scanning
 				 */
-	s32 passive_time;	/* -1 use default, dwell time per channel
+	__le32 passive_time;	/* -1 use default, dwell time per channel
 				 * for passive scanning
 				 */
-	s32 home_time;	/* -1 use default, dwell time for the home channel
-				 * between channel scans
+	__le32 home_time;	/* -1 use default, dwell time for the
+				 * home channel between channel scans
 				 */
-	s32 channel_num;	/* count of channels and ssids that follow
+	__le32 channel_num;	/* count of channels and ssids that follow
 				 *
 				 * low half is count of channels in
 				 * channel_list, 0 means default (use all
@@ -435,22 +451,17 @@ struct brcmf_scan_params {
 				 * fixed parameter portion is assumed, otherwise
 				 * ssid in the fixed portion is ignored
 				 */
-	u16 channel_list[1];	/* list of chanspecs */
+	__le16 channel_list[1];	/* list of chanspecs */
 };
 
 /* incremental scan struct */
-struct brcmf_iscan_params {
-	u32 version;
-	u16 action;
-	u16 scan_duration;
-	struct brcmf_scan_params params;
+struct brcmf_iscan_params_le {
+	__le32 version;
+	__le16 action;
+	__le16 scan_duration;
+	struct brcmf_scan_params_le params_le;
 };
 
-/* 3 fields + size of brcmf_scan_params, not including variable length array */
-#define BRCMF_ISCAN_PARAMS_FIXED_SIZE \
-	(offsetof(struct brcmf_iscan_params, params) + \
-	 sizeof(struct brcmf_ssid))
-
 struct brcmf_scan_results {
 	u32 buflen;
 	u32 version;
@@ -458,21 +469,28 @@ struct brcmf_scan_results {
 	struct brcmf_bss_info bss_info[1];
 };
 
+struct brcmf_scan_results_le {
+	__le32 buflen;
+	__le32 version;
+	__le32 count;
+	struct brcmf_bss_info bss_info[1];
+};
+
 /* used for association with a specific BSSID and chanspec list */
-struct brcmf_assoc_params {
-	u8 bssid[ETH_ALEN];	/* 00:00:00:00:00:00: broadcast scan */
-	s32 chanspec_num;	/* 0: all available channels,
-				 * otherwise count of chanspecs in chanspec_list
-				 */
-	chanspec_t chanspec_list[1];	/* list of chanspecs */
+struct brcmf_assoc_params_le {
+	/* 00:00:00:00:00:00: broadcast scan */
+	u8 bssid[ETH_ALEN];
+	/* 0: all available channels, otherwise count of chanspecs in
+	 * chanspec_list */
+	__le32 chanspec_num;
+	/* list of chanspecs */
+	__le16 chanspec_list[1];
 };
-#define BRCMF_ASSOC_PARAMS_FIXED_SIZE \
-	(sizeof(struct brcmf_assoc_params) - sizeof(chanspec_t))
 
 /* used for join with or without a specific bssid and channel list */
 struct brcmf_join_params {
-	struct brcmf_ssid ssid;
-	struct brcmf_assoc_params params;
+	struct brcmf_ssid_le ssid_le;
+	struct brcmf_assoc_params_le params_le;
 };
 
 /* size of brcmf_scan_results not including variable length array */
@@ -481,8 +499,14 @@ struct brcmf_join_params {
 
 /* incremental scan results struct */
 struct brcmf_iscan_results {
-	u32 status;
-	struct brcmf_scan_results results;
+	union {
+		u32 status;
+		__le32 status_le;
+	};
+	union {
+		struct brcmf_scan_results results;
+		struct brcmf_scan_results_le results_le;
+	};
 };
 
 /* size of brcmf_iscan_results not including variable length array */
@@ -495,40 +519,61 @@ struct brcmf_wsec_key {
 	u32 len;		/* key length */
 	u8 data[WLAN_MAX_KEY_LEN];	/* key data */
 	u32 pad_1[18];
-	u32 algo;		/* CRYPTO_ALGO_AES_CCM, CRYPTO_ALGO_WEP128, etc */
-	u32 flags;		/* misc flags */
-	u32 pad_2[2];
-	int pad_3;
-	int iv_initialized;	/* has IV been initialized already? */
-	int pad_4;
+	u32 algo;	/* CRYPTO_ALGO_AES_CCM, CRYPTO_ALGO_WEP128, etc */
+	u32 flags;	/* misc flags */
+	u32 pad_2[3];
+	u32 iv_initialized;	/* has IV been initialized already? */
+	u32 pad_3;
 	/* Rx IV */
 	struct {
 		u32 hi;	/* upper 32 bits of IV */
 		u16 lo;	/* lower 16 bits of IV */
 	} rxiv;
-	u32 pad_5[2];
+	u32 pad_4[2];
+	u8 ea[ETH_ALEN];	/* per station */
+};
+
+/*
+ * dongle requires same struct as above but with fields in little endian order
+ */
+struct brcmf_wsec_key_le {
+	__le32 index;		/* key index */
+	__le32 len;		/* key length */
+	u8 data[WLAN_MAX_KEY_LEN];	/* key data */
+	__le32 pad_1[18];
+	__le32 algo;	/* CRYPTO_ALGO_AES_CCM, CRYPTO_ALGO_WEP128, etc */
+	__le32 flags;	/* misc flags */
+	__le32 pad_2[3];
+	__le32 iv_initialized;	/* has IV been initialized already? */
+	__le32 pad_3;
+	/* Rx IV */
+	struct {
+		__le32 hi;	/* upper 32 bits of IV */
+		__le16 lo;	/* lower 16 bits of IV */
+	} rxiv;
+	__le32 pad_4[2];
 	u8 ea[ETH_ALEN];	/* per station */
 };
 
 /* Used to get specific STA parameters */
-struct brcmf_scb_val {
-	u32 val;
+struct brcmf_scb_val_le {
+	__le32 val;
 	u8 ea[ETH_ALEN];
 };
 
 /* channel encoding */
-struct brcmf_channel_info {
-	int hw_channel;
-	int target_channel;
-	int scan_channel;
+struct brcmf_channel_info_le {
+	__le32 hw_channel;
+	__le32 target_channel;
+	__le32 scan_channel;
 };
 
-/* Linux network driver ioctl encoding */
-struct brcmf_ioctl {
-	uint cmd;		/* common ioctl definition */
+/* Bus independent dongle command */
+struct brcmf_dcmd {
+	uint cmd;		/* common dongle cmd definition */
 	void *buf;		/* pointer to user buffer */
 	uint len;		/* length of user buffer */
-	u8 set;		/* get or set request (optional) */
+	u8 set;			/* get or set request (optional) */
 	uint used;		/* bytes read or written (optional) */
 	uint needed;		/* bytes needed (optional) */
 };
@@ -537,6 +582,7 @@ struct brcmf_ioctl {
 struct brcmf_bus;		/* device bus info */
 struct brcmf_proto;	/* device communication protocol info */
 struct brcmf_info;	/* device driver info */
+struct brcmf_cfg80211_dev; /* cfg80211 device info */
 
 /* Common structure for module and instance linkage */
 struct brcmf_pub {
@@ -544,11 +590,11 @@ struct brcmf_pub {
 	struct brcmf_bus *bus;
 	struct brcmf_proto *prot;
 	struct brcmf_info *info;
+	struct brcmf_cfg80211_dev *config;
 
 	/* Internal brcmf items */
 	bool up;		/* Driver up/down (to OS) */
 	bool txoff;		/* Transmit flow-controlled */
-	bool dongle_reset;	/* true = DEVRESET put dongle into reset */
 	enum brcmf_bus_state busstate;
 	uint hdrlen;		/* Total BRCMF header length (proto + bus) */
 	uint maxctl;		/* Max size rxctl request from proto to bus */
@@ -558,32 +604,45 @@ struct brcmf_pub {
 	/* 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 */
+	u8 mac[ETH_ALEN];		/* MAC address obtained from dongle */
 	struct dngl_stats dstats;	/* Stats for dongle-based data */
 
 	/* Additional stats for the bus level */
-	unsigned long tx_packets;	/* Data packets sent to dongle */
-	unsigned long tx_multicast;	/* Multicast data packets sent to dongle */
-	unsigned long tx_errors;	/* Errors in sending data to dongle */
-	unsigned long tx_ctlpkts;	/* Control packets sent to dongle */
-	unsigned long tx_ctlerrs;	/* Errors sending control frames to dongle */
-	unsigned long rx_packets;	/* Packets sent up the network interface */
-	unsigned long rx_multicast;	/* Multicast packets sent up the network
-					 interface */
-	unsigned long rx_errors;	/* Errors processing rx data packets */
-	unsigned long rx_ctlpkts;	/* Control frames processed from dongle */
-	unsigned long rx_ctlerrs;	/* Errors in processing rx control frames */
-	unsigned long rx_dropped;	/* Packets dropped locally (no memory) */
-	unsigned long rx_flushed;	/* Packets flushed due to
-				unscheduled sendup thread */
-	unsigned long wd_dpc_sched;	/* Number of times dpc scheduled by
-					 watchdog timer */
-
-	unsigned long rx_readahead_cnt;	/* Number of packets where header read-ahead
-					 was used. */
-	unsigned long tx_realloc;	/* Number of tx packets we had to realloc for
-					 headroom */
-	unsigned long fc_packets;	/* Number of flow control pkts recvd */
+
+	/* Data packets sent to dongle */
+	unsigned long tx_packets;
+	/* Multicast data packets sent to dongle */
+	unsigned long tx_multicast;
+	/* Errors in sending data to dongle */
+	unsigned long tx_errors;
+	/* Control packets sent to dongle */
+	unsigned long tx_ctlpkts;
+	/* Errors sending control frames to dongle */
+	unsigned long tx_ctlerrs;
+	/* Packets sent up the network interface */
+	unsigned long rx_packets;
+	/* Multicast packets sent up the network interface */
+	unsigned long rx_multicast;
+	/* Errors processing rx data packets */
+	unsigned long rx_errors;
+	/* Control frames processed from dongle */
+	unsigned long rx_ctlpkts;
+
+	/* Errors in processing rx control frames */
+	unsigned long rx_ctlerrs;
+	/* Packets dropped locally (no memory) */
+	unsigned long rx_dropped;
+	/* Packets flushed due to unscheduled sendup thread */
+	unsigned long rx_flushed;
+	/* Number of times dpc scheduled by watchdog timer */
+	unsigned long wd_dpc_sched;
+
+	/* Number of packets where header read-ahead was used. */
+	unsigned long rx_readahead_cnt;
+	/* Number of tx packets we had to realloc for headroom */
+	unsigned long tx_realloc;
+	/* Number of flow control pkts recvd */
+	unsigned long fc_packets;
 
 	/* Last error return */
 	int bcmerror;
@@ -614,146 +673,15 @@ struct brcmf_if_event {
 	u8 bssidx;
 };
 
-struct brcmf_timeout {
-	u32 limit;		/* Expiration time (usec) */
-	u32 increment;	/* Current expiration increment (usec) */
-	u32 elapsed;		/* Current elapsed time (usec) */
-	u32 tick;		/* O/S tick time (usec) */
-};
-
 struct bcmevent_name {
 	uint event;
 	const char *name;
 };
 
-#if defined(CONFIG_PM_SLEEP)
-extern atomic_t brcmf_mmc_suspend;
-#define BRCMF_PM_RESUME_WAIT_INIT(a) DECLARE_WAIT_QUEUE_HEAD(a);
-#define _BRCMF_PM_RESUME_WAIT(a, b) do { \
-		int retry = 0; \
-		while (atomic_read(&brcmf_mmc_suspend) && retry++ != b) { \
-			wait_event_timeout(a, false, HZ/100); \
-		} \
-	}	while (0)
-#define BRCMF_PM_RESUME_WAIT(a)	_BRCMF_PM_RESUME_WAIT(a, 30)
-#define BRCMF_PM_RESUME_RETURN_ERROR(a)	\
-	do { if (atomic_read(&brcmf_mmc_suspend)) return a; } while (0)
-
-#define BRCMF_SPINWAIT_SLEEP_INIT(a) DECLARE_WAIT_QUEUE_HEAD(a);
-#define BRCMF_SPINWAIT_SLEEP(a, exp, us) do { \
-		uint countdown = (us) + 9999; \
-		while ((exp) && (countdown >= 10000)) { \
-			wait_event_timeout(a, false, HZ/100); \
-			countdown -= 10000; \
-		} \
-	} while (0)
-
-#else
-
-#define BRCMF_PM_RESUME_WAIT_INIT(a)
-#define BRCMF_PM_RESUME_WAIT(a)
-#define BRCMF_PM_RESUME_RETURN_ERROR(a)
-
-#define BRCMF_SPINWAIT_SLEEP_INIT(a)
-#define BRCMF_SPINWAIT_SLEEP(a, exp, us)  do { \
-		uint countdown = (us) + 9; \
-		while ((exp) && (countdown >= 10)) { \
-			udelay(10);  \
-			countdown -= 10;  \
-		} \
-	} while (0)
-
-#endif	/* defined(CONFIG_PM_SLEEP) */
-
-/*
- * Insmod parameters for debug/test
- */
-
-/* Use interrupts */
-extern uint brcmf_intr;
-
-/* Use polling */
-extern uint brcmf_poll;
-
-/* ARP offload agent mode */
-extern uint brcmf_arp_mode;
-
-/* ARP offload enable */
-extern uint brcmf_arp_enable;
-
-/* Pkt filte enable control */
-extern uint brcmf_pkt_filter_enable;
-
-/*  Pkt filter init setup */
-extern uint brcmf_pkt_filter_init;
-
-/* Pkt filter mode control */
-extern uint brcmf_master_mode;
-
-/* Roaming mode control */
-extern uint brcmf_roam;
-
-/* Roaming mode control */
-extern uint brcmf_radio_up;
-
-/* Initial idletime ticks (may be -1 for immediate idle, 0 for no idle) */
-extern int brcmf_idletime;
-#define BRCMF_IDLETIME_TICKS 1
-
-/* SDIO Drive Strength */
-extern uint brcmf_sdiod_drive_strength;
-
-/* Override to force tx queueing all the time */
-extern uint brcmf_force_tx_queueing;
-
-#ifdef SDTEST
-/* Echo packet generator (SDIO), pkts/s */
-extern uint brcmf_pktgen;
-
-/* Echo packet len (0 => sawtooth, max 1800) */
-extern uint brcmf_pktgen_len;
-#define BRCMF_MAX_PKTGEN_LEN 1800
-#endif
-
 extern const struct bcmevent_name bcmevent_names[];
-extern const int bcmevent_names_size;
-
-
-static inline void MUTEX_LOCK_INIT(struct brcmf_pub *drvr)
-{
-}
-
-static inline void MUTEX_LOCK(struct brcmf_pub *drvr)
-{
-}
-
-static inline void MUTEX_UNLOCK(struct brcmf_pub *drvr)
-{
-}
-
-static inline void MUTEX_LOCK_SOFTAP_SET_INIT(struct brcmf_pub *drvr)
-{
-}
-
-static inline void MUTEX_LOCK_SOFTAP_SET(struct brcmf_pub *drvr)
-{
-}
-
-static inline void MUTEX_UNLOCK_SOFTAP_SET(struct brcmf_pub *drvr)
-{
-}
 
-static inline void MUTEX_LOCK_WL_SCAN_SET_INIT(void)
-{
-}
-
-static inline void MUTEX_LOCK_WL_SCAN_SET(void)
-{
-}
-
-static inline void MUTEX_UNLOCK_WL_SCAN_SET(void)
-{
-}
+extern uint brcmf_c_mkiovar(char *name, char *data, uint datalen,
+			  char *buf, uint len);
 
 /* Indication from bus module regarding presence/insertion of dongle.
  * Return struct brcmf_pub pointer, used as handle to OS module in later calls.
@@ -763,7 +691,9 @@ static inline void MUTEX_UNLOCK_WL_SCAN_SET(void)
 extern struct brcmf_pub *brcmf_attach(struct brcmf_bus *bus,
 				      uint bus_hdrlen);
 extern int brcmf_net_attach(struct brcmf_pub *drvr, int idx);
-extern int brcmf_netdev_wait_pend8021x(struct net_device *dev);
+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);
 
 /* Indication from bus module regarding removal/absence of dongle */
 extern void brcmf_detach(struct brcmf_pub *drvr);
@@ -785,25 +715,17 @@ extern char *brcmf_ifname(struct brcmf_pub *drvr, int idx);
 extern void brcmf_txcomplete(struct brcmf_pub *drvr, struct sk_buff *txp,
 			     bool success);
 
-/* Query ioctl */
-extern int brcmf_proto_cdc_query_ioctl(struct brcmf_pub *drvr, int ifidx,
+/* Query dongle */
+extern int brcmf_proto_cdc_query_dcmd(struct brcmf_pub *drvr, int ifidx,
 				       uint cmd, void *buf, uint len);
 
 /* OS independent layer functions */
 extern int brcmf_os_proto_block(struct brcmf_pub *drvr);
 extern int brcmf_os_proto_unblock(struct brcmf_pub *drvr);
-extern int brcmf_os_ioctl_resp_wait(struct brcmf_pub *drvr, uint *condition,
-				  bool *pending);
-extern int brcmf_os_ioctl_resp_wake(struct brcmf_pub *drvr);
-extern unsigned int brcmf_os_get_ioctl_resp_timeout(void);
-extern void brcmf_os_set_ioctl_resp_timeout(unsigned int timeout_msec);
 #ifdef BCMDBG
-extern int brcmf_write_to_file(struct brcmf_pub *drvr, u8 *buf, int size);
+extern int brcmf_write_to_file(struct brcmf_pub *drvr, const u8 *buf, int size);
 #endif				/* BCMDBG */
 
-extern void brcmf_timeout_start(struct brcmf_timeout *tmo, uint usec);
-extern int brcmf_timeout_expired(struct brcmf_timeout *tmo);
-
 extern int brcmf_ifname2idx(struct brcmf_info *drvr_priv, char *name);
 extern int brcmf_c_host_event(struct brcmf_info *drvr_priv, int *idx,
 			      void *pktdata, struct brcmf_event_msg *,
@@ -811,44 +733,24 @@ extern int brcmf_c_host_event(struct brcmf_info *drvr_priv, int *idx,
 
 extern void brcmf_c_init(void);
 
-extern int brcmf_add_if(struct brcmf_info *drvr_priv, int ifidx, void *handle,
-		      char *name, u8 *mac_addr, u32 flags, u8 bssidx);
+extern int brcmf_add_if(struct brcmf_info *drvr_priv, int ifidx,
+			struct net_device *ndev, char *name, u8 *mac_addr,
+			u32 flags, u8 bssidx);
 extern void brcmf_del_if(struct brcmf_info *drvr_priv, int ifidx);
 
 /* Send packet to dongle via data channel */
 extern int brcmf_sendpkt(struct brcmf_pub *drvr, int ifidx,\
 			 struct sk_buff *pkt);
 
-extern int brcmf_bus_devreset(struct brcmf_pub *drvr, u8 flag);
 extern int brcmf_bus_start(struct brcmf_pub *drvr);
 
 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);
 
-/* Linux network driver ioctl encoding */
-struct brcmf_c_ioctl {
-	uint cmd;		/* common ioctl definition */
-	void *buf;		/* pointer to user buffer */
-	uint len;		/* length of user buffer */
-	bool set;		/* get or set request (optional) */
-	uint used;		/* bytes read or written (optional) */
-	uint needed;		/* bytes needed (optional) */
-	uint driver;		/* to identify target driver */
-};
-
-/* per-driver magic numbers */
-#define BRCMF_IOCTL_MAGIC		0x00444944
-
-/* bump this number if you change the ioctl interface */
-#define BRCMF_IOCTL_VERSION	1
-#define	BRCMF_IOCTL_MAXLEN	8192	/* max length ioctl buffer required */
-
-/* common ioctl definitions */
-#define BRCMF_GET_MAGIC				0
-#define BRCMF_GET_VERSION				1
-#define BRCMF_GET_VAR				2
-#define BRCMF_SET_VAR				3
+#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 */
 
 /* message levels */
 #define BRCMF_ERROR_VAL	0x0001
@@ -865,40 +767,10 @@ struct brcmf_c_ioctl {
 #define BRCMF_BTA_VAL	0x1000
 #define BRCMF_ISCAN_VAL 0x2000
 
-#ifdef SDTEST
-/* For pktgen iovar */
-struct brcmf_pktgen {
-	uint version;		/* To allow structure change tracking */
-	uint freq;		/* Max ticks between tx/rx attempts */
-	uint count;		/* Test packets to send/rcv each attempt */
-	uint print;		/* Print counts every <print> attempts */
-	uint total;		/* Total packets (or bursts) */
-	uint minlen;		/* Minimum length of packets to send */
-	uint maxlen;		/* Maximum length of packets to send */
-	uint numsent;		/* Count of test packets sent */
-	uint numrcvd;		/* Count of test packets received */
-	uint numfail;		/* Count of test send failures */
-	uint mode;		/* Test mode (type of test packets) */
-	uint stop;		/* Stop after this many tx failures */
-};
-
-/* Version in case structure changes */
-#define BRCMF_PKTGEN_VERSION	2
-
-/* Type of test packets to use */
-#define BRCMF_PKTGEN_ECHO	1	/* Send echo requests */
-#define BRCMF_PKTGEN_SEND	2	/* Send discard packets */
-#define BRCMF_PKTGEN_RXBURST	3	/* Request dongle send N packets */
-#define BRCMF_PKTGEN_RECV		4	/* Continuous rx from continuous
-					 tx dongle */
-#endif				/* SDTEST */
-
 /* Enter idle immediately (no timeout) */
 #define BRCMF_IDLE_IMMEDIATE	(-1)
-
-/* Values for idleclock iovar: other values are the sd_divisor to use
-	 when idle */
 #define BRCMF_IDLE_ACTIVE	0	/* Do not request any SD clock change
 				 when idle */
+#define BRCMF_IDLE_INTERVAL	1
 
 #endif				/* _BRCMF_H_ */
diff --git a/drivers/staging/brcm80211/brcmfmac/dhd_bus.h b/drivers/staging/brcm80211/brcmfmac/dhd_bus.h
index 653cf0daa0eb..a249407c9a1b 100644
--- a/drivers/staging/brcm80211/brcmfmac/dhd_bus.h
+++ b/drivers/staging/brcm80211/brcmfmac/dhd_bus.h
@@ -18,36 +18,27 @@
 #define _BRCMF_BUS_H_
 
 /* Packet alignment for most efficient SDIO (can change based on platform) */
-#ifndef BRCMF_SDALIGN
-#define BRCMF_SDALIGN	32
-#endif
-#if !ISPOWEROF2(BRCMF_SDALIGN)
-#error BRCMF_SDALIGN is not a power of 2!
-#endif
+#define BRCMF_SDALIGN	(1 << 6)
+
+/* watchdog polling interval in ms */
+#define BRCMF_WD_POLL_MS	10
 
 /*
  * Exported from brcmf bus module (brcmf_usb, brcmf_sdio)
  */
 
-/* dongle ram module parameter */
-extern int brcmf_dongle_memsize;
-
-/* Tx/Rx bounds module parameters */
-extern uint brcmf_txbound;
-extern uint brcmf_rxbound;
-
-/* Watchdog timer interval */
-extern uint brcmf_watchdog_ms;
-
 /* Indicate (dis)interest in finding dongles. */
 extern int brcmf_bus_register(void);
 extern void brcmf_bus_unregister(void);
 
+/* obtain linux device object providing bus function */
+extern struct device *brcmf_bus_get_device(struct brcmf_bus *bus);
+
 /* Stop bus module: clear pending frames, disable data flow */
-extern void brcmf_sdbrcm_bus_stop(struct brcmf_bus *bus, bool enforce_mutex);
+extern void brcmf_sdbrcm_bus_stop(struct brcmf_bus *bus);
 
 /* Initialize bus module: prepare for communication w/dongle */
-extern int brcmf_sdbrcm_bus_init(struct brcmf_pub *drvr, bool enforce_mutex);
+extern int brcmf_sdbrcm_bus_init(struct brcmf_pub *drvr);
 
 /* Send a data frame to the dongle.  Callee disposes of txp. */
 extern int brcmf_sdbrcm_bus_txdata(struct brcmf_bus *bus, struct sk_buff *txp);
@@ -61,18 +52,6 @@ brcmf_sdbrcm_bus_txctl(struct brcmf_bus *bus, unsigned char *msg, uint msglen);
 extern int
 brcmf_sdbrcm_bus_rxctl(struct brcmf_bus *bus, unsigned char *msg, uint msglen);
 
-/* Check for and handle local prot-specific iovar commands */
-extern int brcmf_sdbrcm_bus_iovar_op(struct brcmf_pub *drvr, const char *name,
-			    void *params, int plen, void *arg, int len,
-			    bool set);
-
-/* Add bus dump output to a buffer */
-extern void brcmf_sdbrcm_bus_dump(struct brcmf_pub *drvr,
-				  struct brcmu_strbuf *strbuf);
-
-/* Clear any bus counters */
-extern void brcmf_bus_clearcounts(struct brcmf_pub *drvr);
-
 extern void brcmf_sdbrcm_wd_timer(struct brcmf_bus *bus, uint wdtick);
 
 #endif				/* _BRCMF_BUS_H_ */
diff --git a/drivers/staging/brcm80211/brcmfmac/dhd_cdc.c b/drivers/staging/brcm80211/brcmfmac/dhd_cdc.c
index 345acabe935e..e34c5c3d1d55 100644
--- a/drivers/staging/brcm80211/brcmfmac/dhd_cdc.c
+++ b/drivers/staging/brcm80211/brcmfmac/dhd_cdc.c
@@ -14,6 +14,11 @@
  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+/*******************************************************************************
+ * Communicates with the dongle by using dcmd codes.
+ * For certain dcmd codes, the dongle interprets string data from the host.
+ ******************************************************************************/
+
 #include <linux/types.h>
 #include <linux/netdevice.h>
 #include <linux/sched.h>
@@ -27,29 +32,26 @@
 #include "dhd_bus.h"
 #include "dhd_dbg.h"
 
-struct brcmf_proto_cdc_ioctl {
-	u32 cmd;	/* ioctl command value */
-	u32 len;	/* lower 16: output buflen;
+struct brcmf_proto_cdc_dcmd {
+	__le32 cmd;	/* dongle command value */
+	__le32 len;	/* lower 16: output buflen;
 			 * upper 16: input buflen (excludes header) */
-	u32 flags;	/* flag defns given below */
-	u32 status;	/* status code returned from the device */
+	__le32 flags;	/* flag defns given below */
+	__le32 status;	/* status code returned from the device */
 };
 
 /* Max valid buffer size that can be sent to the dongle */
 #define CDC_MAX_MSG_SIZE	(ETH_FRAME_LEN+ETH_FCS_LEN)
 
 /* CDC flag definitions */
-#define CDCF_IOC_ERROR		0x01		/* 1=ioctl cmd failed */
-#define CDCF_IOC_SET		0x02		/* 0=get, 1=set cmd */
-#define CDCF_IOC_IF_MASK	0xF000		/* I/F index */
-#define CDCF_IOC_IF_SHIFT	12
-#define CDCF_IOC_ID_MASK	0xFFFF0000	/* id an ioctl pairing */
-#define CDCF_IOC_ID_SHIFT	16		/* ID Mask shift bits */
-#define CDC_IOC_ID(flags)	\
-	(((flags) & CDCF_IOC_ID_MASK) >> CDCF_IOC_ID_SHIFT)
-#define CDC_SET_IF_IDX(hdr, idx) \
-	((hdr)->flags = (((hdr)->flags & ~CDCF_IOC_IF_MASK) | \
-	((idx) << CDCF_IOC_IF_SHIFT)))
+#define CDC_DCMD_ERROR		0x01	/* 1=cmd failed */
+#define CDC_DCMD_SET		0x02	/* 0=get, 1=set cmd */
+#define CDC_DCMD_IF_MASK	0xF000		/* I/F index */
+#define CDC_DCMD_IF_SHIFT	12
+#define CDC_DCMD_ID_MASK	0xFFFF0000	/* id an cmd pairing */
+#define CDC_DCMD_ID_SHIFT	16		/* ID Mask shift bits */
+#define CDC_DCMD_ID(flags)	\
+	(((flags) & CDC_DCMD_ID_MASK) >> CDC_DCMD_ID_SHIFT)
 
 /*
  * BDC header - Broadcom specific extension of CDC.
@@ -79,7 +81,7 @@ struct brcmf_proto_bdc_header {
 };
 
 
-#define RETRIES 2	/* # of retries to retrieve matching ioctl response */
+#define RETRIES 2 /* # of retries to retrieve matching dcmd response */
 #define BUS_HEADER_LEN	(16+BRCMF_SDALIGN) /* Must be atleast SDPCM_RESERVE
 					 * (amount of header tha might be added)
 					 * plus any space that might be needed
@@ -94,17 +96,17 @@ struct brcmf_proto {
 	u8 pending;
 	u32 lastcmd;
 	u8 bus_header[BUS_HEADER_LEN];
-	struct brcmf_proto_cdc_ioctl msg;
-	unsigned char buf[BRCMF_C_IOCTL_MAXLEN + ROUND_UP_MARGIN];
+	struct brcmf_proto_cdc_dcmd msg;
+	unsigned char buf[BRCMF_DCMD_MAXLEN + ROUND_UP_MARGIN];
 };
 
 static int brcmf_proto_cdc_msg(struct brcmf_pub *drvr)
 {
 	struct brcmf_proto *prot = drvr->prot;
 	int len = le32_to_cpu(prot->msg.len) +
-			sizeof(struct brcmf_proto_cdc_ioctl);
+			sizeof(struct brcmf_proto_cdc_dcmd);
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	/* NOTE : cdc->msg.len holds the desired length of the buffer to be
 	 *        returned. Only up to CDC_MAX_MSG_SIZE of this buffer area
@@ -123,31 +125,31 @@ static int brcmf_proto_cdc_cmplt(struct brcmf_pub *drvr, u32 id, u32 len)
 	int ret;
 	struct brcmf_proto *prot = drvr->prot;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	do {
 		ret = brcmf_sdbrcm_bus_rxctl(drvr->bus,
 				(unsigned char *)&prot->msg,
-				len + sizeof(struct brcmf_proto_cdc_ioctl));
+				len + sizeof(struct brcmf_proto_cdc_dcmd));
 		if (ret < 0)
 			break;
-	} while (CDC_IOC_ID(le32_to_cpu(prot->msg.flags)) != id);
+	} while (CDC_DCMD_ID(le32_to_cpu(prot->msg.flags)) != id);
 
 	return ret;
 }
 
 int
-brcmf_proto_cdc_query_ioctl(struct brcmf_pub *drvr, int ifidx, uint cmd,
-			    void *buf, uint len)
+brcmf_proto_cdc_query_dcmd(struct brcmf_pub *drvr, int ifidx, uint cmd,
+			       void *buf, uint len)
 {
 	struct brcmf_proto *prot = drvr->prot;
-	struct brcmf_proto_cdc_ioctl *msg = &prot->msg;
+	struct brcmf_proto_cdc_dcmd *msg = &prot->msg;
 	void *info;
 	int ret = 0, retries = 0;
-	u32 id, flags = 0;
+	u32 id, flags;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-	BRCMF_CTL(("%s: cmd %d len %d\n", __func__, cmd, len));
+	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_dbg(CTL, "cmd %d len %d\n", cmd, len);
 
 	/* Respond "bcmerror" and "bcmerrorstr" with local cache */
 	if (cmd == BRCMF_C_GET_VAR && buf) {
@@ -161,21 +163,22 @@ brcmf_proto_cdc_query_ioctl(struct brcmf_pub *drvr, int ifidx, uint cmd,
 		}
 	}
 
-	memset(msg, 0, sizeof(struct brcmf_proto_cdc_ioctl));
+	memset(msg, 0, sizeof(struct brcmf_proto_cdc_dcmd));
 
 	msg->cmd = cpu_to_le32(cmd);
 	msg->len = cpu_to_le32(len);
-	msg->flags = (++prot->reqid << CDCF_IOC_ID_SHIFT);
-	CDC_SET_IF_IDX(msg, ifidx);
-	msg->flags = cpu_to_le32(msg->flags);
+	flags = (++prot->reqid << CDC_DCMD_ID_SHIFT);
+	flags = (flags & ~CDC_DCMD_IF_MASK) |
+		(ifidx << CDC_DCMD_IF_SHIFT);
+	msg->flags = cpu_to_le32(flags);
 
 	if (buf)
 		memcpy(prot->buf, buf, len);
 
 	ret = brcmf_proto_cdc_msg(drvr);
 	if (ret < 0) {
-		BRCMF_ERROR(("brcmf_proto_cdc_query_ioctl: brcmf_proto_cdc_msg "
-			     "failed w/status %d\n", ret));
+		brcmf_dbg(ERROR, "brcmf_proto_cdc_msg failed w/status %d\n",
+			  ret);
 		goto done;
 	}
 
@@ -186,14 +189,13 @@ retry:
 		goto done;
 
 	flags = le32_to_cpu(msg->flags);
-	id = (flags & CDCF_IOC_ID_MASK) >> CDCF_IOC_ID_SHIFT;
+	id = (flags & CDC_DCMD_ID_MASK) >> CDC_DCMD_ID_SHIFT;
 
 	if ((id < prot->reqid) && (++retries < RETRIES))
 		goto retry;
 	if (id != prot->reqid) {
-		BRCMF_ERROR(("%s: %s: unexpected request id %d (expected %d)\n",
-			     brcmf_ifname(drvr, ifidx), __func__, id,
-			     prot->reqid));
+		brcmf_dbg(ERROR, "%s: unexpected request id %d (expected %d)\n",
+			  brcmf_ifname(drvr, ifidx), id, prot->reqid);
 		ret = -EINVAL;
 		goto done;
 	}
@@ -209,7 +211,7 @@ retry:
 	}
 
 	/* Check the ERROR flag */
-	if (flags & CDCF_IOC_ERROR) {
+	if (flags & CDC_DCMD_ERROR) {
 		ret = le32_to_cpu(msg->status);
 		/* Cache error from dongle */
 		drvr->dongle_error = ret;
@@ -219,24 +221,25 @@ done:
 	return ret;
 }
 
-int brcmf_proto_cdc_set_ioctl(struct brcmf_pub *drvr, int ifidx, uint cmd,
-			      void *buf, uint len)
+int brcmf_proto_cdc_set_dcmd(struct brcmf_pub *drvr, int ifidx, uint cmd,
+				 void *buf, uint len)
 {
 	struct brcmf_proto *prot = drvr->prot;
-	struct brcmf_proto_cdc_ioctl *msg = &prot->msg;
+	struct brcmf_proto_cdc_dcmd *msg = &prot->msg;
 	int ret = 0;
 	u32 flags, id;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-	BRCMF_CTL(("%s: cmd %d len %d\n", __func__, cmd, len));
+	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_dbg(CTL, "cmd %d len %d\n", cmd, len);
 
-	memset(msg, 0, sizeof(struct brcmf_proto_cdc_ioctl));
+	memset(msg, 0, sizeof(struct brcmf_proto_cdc_dcmd));
 
 	msg->cmd = cpu_to_le32(cmd);
 	msg->len = cpu_to_le32(len);
-	msg->flags = (++prot->reqid << CDCF_IOC_ID_SHIFT) | CDCF_IOC_SET;
-	CDC_SET_IF_IDX(msg, ifidx);
-	msg->flags = cpu_to_le32(msg->flags);
+	flags = (++prot->reqid << CDC_DCMD_ID_SHIFT) | CDC_DCMD_SET;
+	flags = (flags & ~CDC_DCMD_IF_MASK) |
+		(ifidx << CDC_DCMD_IF_SHIFT);
+	msg->flags = cpu_to_le32(flags);
 
 	if (buf)
 		memcpy(prot->buf, buf, len);
@@ -250,18 +253,17 @@ int brcmf_proto_cdc_set_ioctl(struct brcmf_pub *drvr, int ifidx, uint cmd,
 		goto done;
 
 	flags = le32_to_cpu(msg->flags);
-	id = (flags & CDCF_IOC_ID_MASK) >> CDCF_IOC_ID_SHIFT;
+	id = (flags & CDC_DCMD_ID_MASK) >> CDC_DCMD_ID_SHIFT;
 
 	if (id != prot->reqid) {
-		BRCMF_ERROR(("%s: %s: unexpected request id %d (expected %d)\n",
-			     brcmf_ifname(drvr, ifidx), __func__, id,
-			     prot->reqid));
+		brcmf_dbg(ERROR, "%s: unexpected request id %d (expected %d)\n",
+			  brcmf_ifname(drvr, ifidx), id, prot->reqid);
 		ret = -EINVAL;
 		goto done;
 	}
 
 	/* Check the ERROR flag */
-	if (flags & CDCF_IOC_ERROR) {
+	if (flags & CDC_DCMD_ERROR) {
 		ret = le32_to_cpu(msg->status);
 		/* Cache error from dongle */
 		drvr->dongle_error = ret;
@@ -272,65 +274,64 @@ done:
 }
 
 int
-brcmf_proto_ioctl(struct brcmf_pub *drvr, int ifidx, struct brcmf_ioctl *ioc,
-		  void *buf, int len)
+brcmf_proto_dcmd(struct brcmf_pub *drvr, int ifidx, struct brcmf_dcmd *dcmd,
+		  int len)
 {
 	struct brcmf_proto *prot = drvr->prot;
 	int ret = -1;
 
 	if (drvr->busstate == BRCMF_BUS_DOWN) {
-		BRCMF_ERROR(("%s : bus is down. we have nothing to do\n",
-			     __func__));
+		brcmf_dbg(ERROR, "bus is down. we have nothing to do.\n");
 		return ret;
 	}
 	brcmf_os_proto_block(drvr);
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
-	if (len > BRCMF_C_IOCTL_MAXLEN)
+	if (len > BRCMF_DCMD_MAXLEN)
 		goto done;
 
 	if (prot->pending == true) {
-		BRCMF_TRACE(("CDC packet is pending!!!! cmd=0x%x (%lu) "
-			     "lastcmd=0x%x (%lu)\n",
-			     ioc->cmd, (unsigned long)ioc->cmd, prot->lastcmd,
-			     (unsigned long)prot->lastcmd));
-		if ((ioc->cmd == BRCMF_C_SET_VAR) ||
-		    (ioc->cmd == BRCMF_C_GET_VAR))
-			BRCMF_TRACE(("iovar cmd=%s\n", (char *)buf));
+		brcmf_dbg(TRACE, "CDC packet is pending!!!! cmd=0x%x (%lu) lastcmd=0x%x (%lu)\n",
+			  dcmd->cmd, (unsigned long)dcmd->cmd, prot->lastcmd,
+			  (unsigned long)prot->lastcmd);
+		if (dcmd->cmd == BRCMF_C_SET_VAR ||
+		    dcmd->cmd == BRCMF_C_GET_VAR)
+			brcmf_dbg(TRACE, "iovar cmd=%s\n", (char *)dcmd->buf);
 
 		goto done;
 	}
 
 	prot->pending = true;
-	prot->lastcmd = ioc->cmd;
-	if (ioc->set)
-		ret = brcmf_proto_cdc_set_ioctl(drvr, ifidx, ioc->cmd,
-						buf, len);
+	prot->lastcmd = dcmd->cmd;
+	if (dcmd->set)
+		ret = brcmf_proto_cdc_set_dcmd(drvr, ifidx, dcmd->cmd,
+						   dcmd->buf, len);
 	else {
-		ret = brcmf_proto_cdc_query_ioctl(drvr, ifidx, ioc->cmd,
-						  buf, len);
+		ret = brcmf_proto_cdc_query_dcmd(drvr, ifidx, dcmd->cmd,
+						     dcmd->buf, len);
 		if (ret > 0)
-			ioc->used = ret - sizeof(struct brcmf_proto_cdc_ioctl);
+			dcmd->used = ret -
+					sizeof(struct brcmf_proto_cdc_dcmd);
 	}
 
-	/* Too many programs assume ioctl() returns 0 on success */
 	if (ret >= 0)
 		ret = 0;
 	else {
-		struct brcmf_proto_cdc_ioctl *msg = &prot->msg;
+		struct brcmf_proto_cdc_dcmd *msg = &prot->msg;
 		/* len == needed when set/query fails from dongle */
-		ioc->needed = le32_to_cpu(msg->len);
+		dcmd->needed = le32_to_cpu(msg->len);
 	}
 
-	/* Intercept the wme_dp ioctl here */
-	if (!ret && ioc->cmd == BRCMF_C_SET_VAR &&
-	    !strcmp(buf, "wme_dp")) {
-		int slen, val = 0;
+	/* Intercept the wme_dp dongle cmd here */
+	if (!ret && dcmd->cmd == BRCMF_C_SET_VAR &&
+	    !strcmp(dcmd->buf, "wme_dp")) {
+		int slen;
+		__le32 val = 0;
 
 		slen = strlen("wme_dp") + 1;
 		if (len >= (int)(slen + sizeof(int)))
-			memcpy(&val, (char *)buf + slen, sizeof(int));
+			memcpy(&val, (char *)dcmd->buf + slen, sizeof(int));
 		drvr->wme_dp = (u8) le32_to_cpu(val);
 	}
 
@@ -342,15 +343,14 @@ done:
 	return ret;
 }
 
-#define PKTSUMNEEDED(skb) \
-		(((struct sk_buff *)(skb))->ip_summed == CHECKSUM_PARTIAL)
-#define PKTSETSUMGOOD(skb, x) \
-		(((struct sk_buff *)(skb))->ip_summed = \
-		((x) ? CHECKSUM_UNNECESSARY : CHECKSUM_NONE))
+static bool pkt_sum_needed(struct sk_buff *skb)
+{
+	return skb->ip_summed == CHECKSUM_PARTIAL;
+}
 
-void brcmf_proto_dump(struct brcmf_pub *drvr, struct brcmu_strbuf *strbuf)
+static void pkt_set_sum_good(struct sk_buff *skb, bool x)
 {
-	brcmu_bprintf(strbuf, "Protocol CDC: reqid %d\n", drvr->prot->reqid);
+	skb->ip_summed = (x ? CHECKSUM_UNNECESSARY : CHECKSUM_NONE);
 }
 
 void brcmf_proto_hdrpush(struct brcmf_pub *drvr, int ifidx,
@@ -358,7 +358,7 @@ void brcmf_proto_hdrpush(struct brcmf_pub *drvr, int ifidx,
 {
 	struct brcmf_proto_bdc_header *h;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	/* Push BDC header used to convey priority for buses that don't */
 
@@ -367,7 +367,7 @@ void brcmf_proto_hdrpush(struct brcmf_pub *drvr, int ifidx,
 	h = (struct brcmf_proto_bdc_header *)(pktbuf->data);
 
 	h->flags = (BDC_PROTO_VER << BDC_FLAG_VER_SHIFT);
-	if (PKTSUMNEEDED(pktbuf))
+	if (pkt_sum_needed(pktbuf))
 		h->flags |= BDC_FLAG_SUM_NEEDED;
 
 	h->priority = (pktbuf->priority & BDC_PRIORITY_MASK);
@@ -381,13 +381,13 @@ int brcmf_proto_hdrpull(struct brcmf_pub *drvr, int *ifidx,
 {
 	struct brcmf_proto_bdc_header *h;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	/* Pop BDC header used to convey priority for buses that don't */
 
 	if (pktbuf->len < BDC_HEADER_LEN) {
-		BRCMF_ERROR(("%s: rx data too short (%d < %d)\n", __func__,
-			     pktbuf->len, BDC_HEADER_LEN));
+		brcmf_dbg(ERROR, "rx data too short (%d < %d)\n",
+			  pktbuf->len, BDC_HEADER_LEN);
 		return -EBADE;
 	}
 
@@ -395,23 +395,21 @@ int brcmf_proto_hdrpull(struct brcmf_pub *drvr, int *ifidx,
 
 	*ifidx = BDC_GET_IF_IDX(h);
 	if (*ifidx >= BRCMF_MAX_IFS) {
-		BRCMF_ERROR(("%s: rx data ifnum out of range (%d)\n",
-			     __func__, *ifidx));
+		brcmf_dbg(ERROR, "rx data ifnum out of range (%d)\n", *ifidx);
 		return -EBADE;
 	}
 
 	if (((h->flags & BDC_FLAG_VER_MASK) >> BDC_FLAG_VER_SHIFT) !=
 	    BDC_PROTO_VER) {
-		BRCMF_ERROR(("%s: non-BDC packet received, flags 0x%x\n",
-			     brcmf_ifname(drvr, *ifidx), h->flags));
+		brcmf_dbg(ERROR, "%s: non-BDC packet received, flags 0x%x\n",
+			  brcmf_ifname(drvr, *ifidx), h->flags);
 		return -EBADE;
 	}
 
 	if (h->flags & BDC_FLAG_SUM_GOOD) {
-		BRCMF_INFO(("%s: BDC packet received with good rx-csum, "
-			    "flags 0x%x\n",
-			    brcmf_ifname(drvr, *ifidx), h->flags));
-		PKTSETSUMGOOD(pktbuf, true);
+		brcmf_dbg(INFO, "%s: BDC packet received with good rx-csum, flags 0x%x\n",
+			  brcmf_ifname(drvr, *ifidx), h->flags);
+		pkt_set_sum_good(pktbuf, true);
 	}
 
 	pktbuf->priority = h->priority & BDC_PRIORITY_MASK;
@@ -426,21 +424,19 @@ int brcmf_proto_attach(struct brcmf_pub *drvr)
 	struct brcmf_proto *cdc;
 
 	cdc = kzalloc(sizeof(struct brcmf_proto), GFP_ATOMIC);
-	if (!cdc) {
-		BRCMF_ERROR(("%s: kmalloc failed\n", __func__));
+	if (!cdc)
 		goto fail;
-	}
 
 	/* ensure that the msg buf directly follows the cdc msg struct */
 	if ((unsigned long)(&cdc->msg + 1) != (unsigned long)cdc->buf) {
-		BRCMF_ERROR(("struct brcmf_proto is not correctly defined\n"));
+		brcmf_dbg(ERROR, "struct brcmf_proto is not correctly defined\n");
 		goto fail;
 	}
 
 	drvr->prot = cdc;
 	drvr->hdrlen += BDC_HEADER_LEN;
-	drvr->maxctl = BRCMF_C_IOCTL_MAXLEN +
-			sizeof(struct brcmf_proto_cdc_ioctl) + ROUND_UP_MARGIN;
+	drvr->maxctl = BRCMF_DCMD_MAXLEN +
+			sizeof(struct brcmf_proto_cdc_dcmd) + ROUND_UP_MARGIN;
 	return 0;
 
 fail:
@@ -472,13 +468,13 @@ int brcmf_proto_init(struct brcmf_pub *drvr)
 	int ret = 0;
 	char buf[128];
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	brcmf_os_proto_block(drvr);
 
 	/* Get the device MAC address */
 	strcpy(buf, "cur_etheraddr");
-	ret = brcmf_proto_cdc_query_ioctl(drvr, 0, BRCMF_C_GET_VAR,
+	ret = brcmf_proto_cdc_query_dcmd(drvr, 0, BRCMF_C_GET_VAR,
 					  buf, sizeof(buf));
 	if (ret < 0) {
 		brcmf_os_proto_unblock(drvr);
@@ -488,7 +484,7 @@ int brcmf_proto_init(struct brcmf_pub *drvr)
 
 	brcmf_os_proto_unblock(drvr);
 
-	ret = brcmf_c_preinit_ioctls(drvr);
+	ret = brcmf_c_preinit_dcmds(drvr);
 
 	/* Always assumes wl for now */
 	drvr->iswl = true;
diff --git a/drivers/staging/brcm80211/brcmfmac/dhd_common.c b/drivers/staging/brcm80211/brcmfmac/dhd_common.c
index fdec4683c422..891826197f96 100644
--- a/drivers/staging/brcm80211/brcmfmac/dhd_common.c
+++ b/drivers/staging/brcm80211/brcmfmac/dhd_common.c
@@ -30,73 +30,61 @@
 #define DOT11_OUI_LEN			3
 #define BCMILCP_BCM_SUBTYPE_EVENT	1
 #define PKTFILTER_BUF_SIZE		2048
+#define BRCMF_ARPOL_MODE		0xb	/* agent|snoop|peer_autoreply */
 
 int brcmf_msg_level;
 
 #define MSGTRACE_VERSION	1
 
+#define BRCMF_PKT_FILTER_FIXED_LEN	offsetof(struct brcmf_pkt_filter_le, u)
+#define BRCMF_PKT_FILTER_PATTERN_FIXED_LEN	\
+	offsetof(struct brcmf_pkt_filter_pattern_le, mask_and_pattern)
+
 #ifdef BCMDBG
-const char brcmf_version[] =
-"Dongle Host Driver, version " BRCMF_VERSION_STR "\nCompiled on " __DATE__
-" at " __TIME__;
+static const char brcmf_version[] =
+	"Dongle Host Driver, version " BRCMF_VERSION_STR "\nCompiled on "
+	__DATE__ " at " __TIME__;
 #else
-const char brcmf_version[] = "Dongle Host Driver, version " BRCMF_VERSION_STR;
+static const char brcmf_version[] =
+	"Dongle Host Driver, version " BRCMF_VERSION_STR;
 #endif
 
-/* IOVar table */
-enum {
-	IOV_VERSION = 1,
-	IOV_MSGLEVEL,
-	IOV_BCMERRORSTR,
-	IOV_BCMERROR,
-	IOV_DUMP,
-	IOV_CLEARCOUNTS,
-	IOV_LOGDUMP,
-	IOV_LOGCAL,
-	IOV_LOGSTAMP,
-	IOV_GPIOOB,
-	IOV_IOCTLTIMEOUT,
-	IOV_LAST
-};
-
-const struct brcmu_iovar brcmf_iovars[] = {
-	{"version", IOV_VERSION, 0, IOVT_BUFFER, sizeof(brcmf_version)}
-	,
-#ifdef BCMDBG
-	{"msglevel", IOV_MSGLEVEL, 0, IOVT_UINT32, 0}
-	,
-#endif				/* BCMDBG */
-	{"bcmerrorstr", IOV_BCMERRORSTR, 0, IOVT_BUFFER, BCME_STRLEN}
-	,
-	{"bcmerror", IOV_BCMERROR, 0, IOVT_INT8, 0}
-	,
-	{"dump", IOV_DUMP, 0, IOVT_BUFFER, BRCMF_IOCTL_MAXLEN}
-	,
-	{"clearcounts", IOV_CLEARCOUNTS, 0, IOVT_VOID, 0}
-	,
-	{"gpioob", IOV_GPIOOB, 0, IOVT_UINT32, 0}
-	,
-	{"ioctl_timeout", IOV_IOCTLTIMEOUT, 0, IOVT_UINT32, 0}
-	,
-	{NULL, 0, 0, 0, 0}
-};
-
 /* Message trace header */
 struct msgtrace_hdr {
 	u8 version;
 	u8 spare;
-	u16 len;		/* Len of the trace */
-	u32 seqnum;		/* Sequence number of message. Useful
+	__be16 len;		/* Len of the trace */
+	__be32 seqnum;		/* Sequence number of message. Useful
 				 * if the messsage has been lost
 				 * because of DMA error or a bus reset
 				 * (ex: SDIO Func2)
 				 */
-	u32 discarded_bytes;	/* Number of discarded bytes because of
+	__be32 discarded_bytes;	/* Number of discarded bytes because of
 				 trace overflow  */
-	u32 discarded_printf;	/* Number of discarded printf
+	__be32 discarded_printf;	/* Number of discarded printf
 				 because of trace overflow */
 } __packed;
 
+
+uint
+brcmf_c_mkiovar(char *name, char *data, uint datalen, char *buf, uint buflen)
+{
+	uint len;
+
+	len = strlen(name) + 1;
+
+	if ((len + datalen) > buflen)
+		return 0;
+
+	strncpy(buf, name, buflen);
+
+	/* append data onto the end of the name string */
+	memcpy(&buf[len], data, datalen);
+	len += datalen;
+
+	return len;
+}
+
 void brcmf_c_init(void)
 {
 	/* Init global variables at run-time, not as part of the declaration.
@@ -110,147 +98,6 @@ void brcmf_c_init(void)
 	brcmf_msg_level = BRCMF_ERROR_VAL;
 }
 
-static int brcmf_c_dump(struct brcmf_pub *drvr, char *buf, int buflen)
-{
-	struct brcmu_strbuf b;
-	struct brcmu_strbuf *strbuf = &b;
-
-	brcmu_binit(strbuf, buf, buflen);
-
-	/* Base info */
-	brcmu_bprintf(strbuf, "%s\n", brcmf_version);
-	brcmu_bprintf(strbuf, "\n");
-	brcmu_bprintf(strbuf, "pub.up %d pub.txoff %d pub.busstate %d\n",
-		    drvr->up, drvr->txoff, drvr->busstate);
-	brcmu_bprintf(strbuf, "pub.hdrlen %d pub.maxctl %d pub.rxsz %d\n",
-		    drvr->hdrlen, drvr->maxctl, drvr->rxsz);
-	brcmu_bprintf(strbuf, "pub.iswl %d pub.drv_version %ld pub.mac %pM\n",
-		    drvr->iswl, drvr->drv_version, &drvr->mac);
-	brcmu_bprintf(strbuf, "pub.bcmerror %d tickcnt %d\n", drvr->bcmerror,
-		    drvr->tickcnt);
-
-	brcmu_bprintf(strbuf, "dongle stats:\n");
-	brcmu_bprintf(strbuf,
-		    "tx_packets %ld tx_bytes %ld tx_errors %ld tx_dropped %ld\n",
-		    drvr->dstats.tx_packets, drvr->dstats.tx_bytes,
-		    drvr->dstats.tx_errors, drvr->dstats.tx_dropped);
-	brcmu_bprintf(strbuf,
-		    "rx_packets %ld rx_bytes %ld rx_errors %ld rx_dropped %ld\n",
-		    drvr->dstats.rx_packets, drvr->dstats.rx_bytes,
-		    drvr->dstats.rx_errors, drvr->dstats.rx_dropped);
-	brcmu_bprintf(strbuf, "multicast %ld\n", drvr->dstats.multicast);
-
-	brcmu_bprintf(strbuf, "bus stats:\n");
-	brcmu_bprintf(strbuf, "tx_packets %ld tx_multicast %ld tx_errors %ld\n",
-		    drvr->tx_packets, drvr->tx_multicast, drvr->tx_errors);
-	brcmu_bprintf(strbuf, "tx_ctlpkts %ld tx_ctlerrs %ld\n",
-		    drvr->tx_ctlpkts, drvr->tx_ctlerrs);
-	brcmu_bprintf(strbuf, "rx_packets %ld rx_multicast %ld rx_errors %ld\n",
-		    drvr->rx_packets, drvr->rx_multicast, drvr->rx_errors);
-	brcmu_bprintf(strbuf,
-		    "rx_ctlpkts %ld rx_ctlerrs %ld rx_dropped %ld rx_flushed %ld\n",
-		    drvr->rx_ctlpkts, drvr->rx_ctlerrs, drvr->rx_dropped,
-		    drvr->rx_flushed);
-	brcmu_bprintf(strbuf,
-		    "rx_readahead_cnt %ld tx_realloc %ld fc_packets %ld\n",
-		    drvr->rx_readahead_cnt, drvr->tx_realloc, drvr->fc_packets);
-	brcmu_bprintf(strbuf, "wd_dpc_sched %ld\n", drvr->wd_dpc_sched);
-	brcmu_bprintf(strbuf, "\n");
-
-	/* Add any prot info */
-	brcmf_proto_dump(drvr, strbuf);
-	brcmu_bprintf(strbuf, "\n");
-
-	/* Add any bus info */
-	brcmf_sdbrcm_bus_dump(drvr, strbuf);
-
-	return !strbuf->size ? -EOVERFLOW : 0;
-}
-
-static int
-brcmf_c_doiovar(struct brcmf_pub *drvr, const struct brcmu_iovar *vi,
-		u32 actionid, const char *name, void *params, int plen,
-		void *arg, int len, int val_size)
-{
-	int bcmerror = 0;
-	s32 int_val = 0;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	bcmerror = brcmu_iovar_lencheck(vi, arg, len, IOV_ISSET(actionid));
-	if (bcmerror != 0)
-		goto exit;
-
-	if (plen >= (int)sizeof(int_val))
-		memcpy(&int_val, params, sizeof(int_val));
-
-	switch (actionid) {
-	case IOV_GVAL(IOV_VERSION):
-		/* Need to have checked buffer length */
-		strncpy((char *)arg, brcmf_version, len);
-		break;
-
-	case IOV_GVAL(IOV_MSGLEVEL):
-		int_val = (s32) brcmf_msg_level;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_MSGLEVEL):
-		brcmf_msg_level = int_val;
-		break;
-
-	case IOV_GVAL(IOV_BCMERRORSTR):
-		strncpy((char *)arg, "bcm_error",
-			BCME_STRLEN);
-		((char *)arg)[BCME_STRLEN - 1] = 0x00;
-		break;
-
-	case IOV_GVAL(IOV_BCMERROR):
-		int_val = (s32) drvr->bcmerror;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_GVAL(IOV_DUMP):
-		bcmerror = brcmf_c_dump(drvr, arg, len);
-		break;
-
-	case IOV_SVAL(IOV_CLEARCOUNTS):
-		drvr->tx_packets = drvr->rx_packets = 0;
-		drvr->tx_errors = drvr->rx_errors = 0;
-		drvr->tx_ctlpkts = drvr->rx_ctlpkts = 0;
-		drvr->tx_ctlerrs = drvr->rx_ctlerrs = 0;
-		drvr->rx_dropped = 0;
-		drvr->rx_readahead_cnt = 0;
-		drvr->tx_realloc = 0;
-		drvr->wd_dpc_sched = 0;
-		memset(&drvr->dstats, 0, sizeof(drvr->dstats));
-		brcmf_bus_clearcounts(drvr);
-		break;
-
-	case IOV_GVAL(IOV_IOCTLTIMEOUT):{
-			int_val = (s32) brcmf_os_get_ioctl_resp_timeout();
-			memcpy(arg, &int_val, sizeof(int_val));
-			break;
-		}
-
-	case IOV_SVAL(IOV_IOCTLTIMEOUT):{
-			if (int_val <= 0)
-				bcmerror = -EINVAL;
-			else
-				brcmf_os_set_ioctl_resp_timeout((unsigned int)
-							      int_val);
-			break;
-		}
-
-	default:
-		bcmerror = -ENOTSUPP;
-		break;
-	}
-
-exit:
-	return bcmerror;
-}
-
 bool brcmf_c_prec_enq(struct brcmf_pub *drvr, struct pktq *q,
 		      struct sk_buff *pkt, int prec)
 {
@@ -278,159 +125,28 @@ bool brcmf_c_prec_enq(struct brcmf_pub *drvr, struct pktq *q,
 	/* Evict if needed */
 	if (eprec >= 0) {
 		/* Detect queueing to unconfigured precedence */
-		discard_oldest = AC_BITMAP_TST(drvr->wme_dp, eprec);
+		discard_oldest = ac_bitmap_tst(drvr->wme_dp, eprec);
 		if (eprec == prec && !discard_oldest)
 			return false;	/* refuse newer (incoming) packet */
 		/* Evict packet according to discard policy */
 		p = discard_oldest ? brcmu_pktq_pdeq(q, eprec) :
 			brcmu_pktq_pdeq_tail(q, eprec);
-		if (p == NULL) {
-			BRCMF_ERROR(("%s: brcmu_pktq_penq() failed, oldest %d.",
-				     __func__, discard_oldest));
-		}
+		if (p == NULL)
+			brcmf_dbg(ERROR, "brcmu_pktq_penq() failed, oldest %d\n",
+				  discard_oldest);
+
 		brcmu_pkt_buf_free_skb(p);
 	}
 
 	/* Enqueue */
 	p = brcmu_pktq_penq(q, prec, pkt);
-	if (p == NULL) {
-		BRCMF_ERROR(("%s: brcmu_pktq_penq() failed.", __func__));
-	}
+	if (p == NULL)
+		brcmf_dbg(ERROR, "brcmu_pktq_penq() failed\n");
 
 	return p != NULL;
 }
 
-static int
-brcmf_c_iovar_op(struct brcmf_pub *drvr, const char *name,
-	     void *params, int plen, void *arg, int len, bool set)
-{
-	int bcmerror = 0;
-	int val_size;
-	const struct brcmu_iovar *vi = NULL;
-	u32 actionid;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	if (name == NULL || len <= 0)
-		return -EINVAL;
-
-	/* Set does not take qualifiers */
-	if (set && (params || plen))
-		return -EINVAL;
-
-	/* Get must have return space;*/
-	if (!set && !(arg && len))
-		return -EINVAL;
-
-	vi = brcmu_iovar_lookup(brcmf_iovars, name);
-	if (vi == NULL) {
-		bcmerror = -ENOTSUPP;
-		goto exit;
-	}
-
-	BRCMF_CTL(("%s: %s %s, len %d plen %d\n", __func__,
-		   name, (set ? "set" : "get"), len, plen));
-
-	/* set up 'params' pointer in case this is a set command so that
-	 * the convenience int and bool code can be common to set and get
-	 */
-	if (params == NULL) {
-		params = arg;
-		plen = len;
-	}
-
-	if (vi->type == IOVT_VOID)
-		val_size = 0;
-	else if (vi->type == IOVT_BUFFER)
-		val_size = len;
-	else
-		/* all other types are integer sized */
-		val_size = sizeof(int);
-
-	actionid = set ? IOV_SVAL(vi->varid) : IOV_GVAL(vi->varid);
-	bcmerror =
-	    brcmf_c_doiovar(drvr, vi, actionid, name, params, plen, arg, len,
-			val_size);
-
-exit:
-	return bcmerror;
-}
-
-int brcmf_c_ioctl(struct brcmf_pub *drvr, struct brcmf_c_ioctl *ioc, void *buf,
-		  uint buflen)
-{
-	int bcmerror = 0;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	if (!buf)
-		return -EINVAL;
-
-	switch (ioc->cmd) {
-	case BRCMF_GET_MAGIC:
-		if (buflen < sizeof(int))
-			bcmerror = -EOVERFLOW;
-		else
-			*(int *)buf = BRCMF_IOCTL_MAGIC;
-		break;
-
-	case BRCMF_GET_VERSION:
-		if (buflen < sizeof(int))
-			bcmerror = -EOVERFLOW;
-		else
-			*(int *)buf = BRCMF_IOCTL_VERSION;
-		break;
-
-	case BRCMF_GET_VAR:
-	case BRCMF_SET_VAR:{
-			char *arg;
-			uint arglen;
-
-			/* scan past the name to any arguments */
-			for (arg = buf, arglen = buflen; *arg && arglen;
-			     arg++, arglen--)
-				;
-
-			if (*arg) {
-				bcmerror = -EOVERFLOW;
-				break;
-			}
-
-			/* account for the NUL terminator */
-			arg++, arglen--;
-
-			/* call with the appropriate arguments */
-			if (ioc->cmd == BRCMF_GET_VAR)
-				bcmerror = brcmf_c_iovar_op(drvr, buf, arg,
-						arglen, buf, buflen, IOV_GET);
-			else
-				bcmerror =
-				    brcmf_c_iovar_op(drvr, buf, NULL, 0, arg,
-						     arglen, IOV_SET);
-			if (bcmerror != -ENOTSUPP)
-				break;
-
-			/* if still not found, try bus module */
-			if (ioc->cmd == BRCMF_GET_VAR)
-				bcmerror = brcmf_sdbrcm_bus_iovar_op(drvr,
-						buf, arg, arglen, buf, buflen,
-						IOV_GET);
-			else
-				bcmerror = brcmf_sdbrcm_bus_iovar_op(drvr,
-						buf, NULL, 0, arg, arglen,
-						IOV_SET);
-
-			break;
-		}
-
-	default:
-		bcmerror = -ENOTSUPP;
-	}
-
-	return bcmerror;
-}
-
-#ifdef SHOW_EVENTS
+#ifdef BCMDBG
 static void
 brcmf_c_show_host_event(struct brcmf_event_msg *event, void *event_data)
 {
@@ -502,6 +218,11 @@ brcmf_c_show_host_event(struct brcmf_event_msg *event, void *event_data)
 		BRCMF_E_PFN_SCAN_COMPLETE, "SCAN_COMPLETE"}
 	};
 	uint event_type, flags, auth_type, datalen;
+	static u32 seqnum_prev;
+	struct msgtrace_hdr hdr;
+	u32 nblost;
+	char *s, *p;
+
 	event_type = be32_to_cpu(event->event_type);
 	flags = be16_to_cpu(event->flags);
 	status = be32_to_cpu(event->status);
@@ -517,9 +238,9 @@ brcmf_c_show_host_event(struct brcmf_event_msg *event, void *event_data)
 			event_name = event_names[i].event_name;
 	}
 
-	BRCMF_EVENT(("EVENT: %s, event ID = %d\n", event_name, event_type));
-	BRCMF_EVENT(("flags 0x%04x, status %d, reason %d, auth_type %d"
-		     " MAC %s\n", flags, status, reason, auth_type, eabuf));
+	brcmf_dbg(EVENT, "EVENT: %s, event ID = %d\n", event_name, event_type);
+	brcmf_dbg(EVENT, "flags 0x%04x, status %d, reason %d, auth_type %d MAC %s\n",
+		  flags, status, reason, auth_type, eabuf);
 
 	if (flags & BRCMF_EVENT_MSG_LINK)
 		link = true;
@@ -532,36 +253,34 @@ brcmf_c_show_host_event(struct brcmf_event_msg *event, void *event_data)
 	case BRCMF_E_START:
 	case BRCMF_E_DEAUTH:
 	case BRCMF_E_DISASSOC:
-		BRCMF_EVENT(("MACEVENT: %s, MAC %s\n", event_name, eabuf));
+		brcmf_dbg(EVENT, "MACEVENT: %s, MAC %s\n", event_name, eabuf);
 		break;
 
 	case BRCMF_E_ASSOC_IND:
 	case BRCMF_E_REASSOC_IND:
-		BRCMF_EVENT(("MACEVENT: %s, MAC %s\n", event_name, eabuf));
+		brcmf_dbg(EVENT, "MACEVENT: %s, MAC %s\n", event_name, eabuf);
 		break;
 
 	case BRCMF_E_ASSOC:
 	case BRCMF_E_REASSOC:
-		if (status == BRCMF_E_STATUS_SUCCESS) {
-			BRCMF_EVENT(("MACEVENT: %s, MAC %s, SUCCESS\n",
-				     event_name, eabuf));
-		} else if (status == BRCMF_E_STATUS_TIMEOUT) {
-			BRCMF_EVENT(("MACEVENT: %s, MAC %s, TIMEOUT\n",
-				     event_name, eabuf));
-		} else if (status == BRCMF_E_STATUS_FAIL) {
-			BRCMF_EVENT(("MACEVENT: %s, MAC %s, FAILURE,"
-				     " reason %d\n", event_name, eabuf,
-				     (int)reason));
-		} else {
-			BRCMF_EVENT(("MACEVENT: %s, MAC %s, unexpected status "
-				     "%d\n", event_name, eabuf, (int)status));
-		}
+		if (status == BRCMF_E_STATUS_SUCCESS)
+			brcmf_dbg(EVENT, "MACEVENT: %s, MAC %s, SUCCESS\n",
+				  event_name, eabuf);
+		else if (status == BRCMF_E_STATUS_TIMEOUT)
+			brcmf_dbg(EVENT, "MACEVENT: %s, MAC %s, TIMEOUT\n",
+				  event_name, eabuf);
+		else if (status == BRCMF_E_STATUS_FAIL)
+			brcmf_dbg(EVENT, "MACEVENT: %s, MAC %s, FAILURE, reason %d\n",
+				  event_name, eabuf, (int)reason);
+		else
+			brcmf_dbg(EVENT, "MACEVENT: %s, MAC %s, unexpected status %d\n",
+				  event_name, eabuf, (int)status);
 		break;
 
 	case BRCMF_E_DEAUTH_IND:
 	case BRCMF_E_DISASSOC_IND:
-		BRCMF_EVENT(("MACEVENT: %s, MAC %s, reason %d\n", event_name,
-			     eabuf, (int)reason));
+		brcmf_dbg(EVENT, "MACEVENT: %s, MAC %s, reason %d\n",
+			  event_name, eabuf, (int)reason);
 		break;
 
 	case BRCMF_E_AUTH:
@@ -574,19 +293,18 @@ brcmf_c_show_host_event(struct brcmf_event_msg *event, void *event_data)
 			sprintf(err_msg, "AUTH unknown: %d", (int)auth_type);
 			auth_str = err_msg;
 		}
-		if (event_type == BRCMF_E_AUTH_IND) {
-			BRCMF_EVENT(("MACEVENT: %s, MAC %s, %s\n", event_name,
-				     eabuf, auth_str));
-		} else if (status == BRCMF_E_STATUS_SUCCESS) {
-			BRCMF_EVENT(("MACEVENT: %s, MAC %s, %s, SUCCESS\n",
-				     event_name, eabuf, auth_str));
-		} else if (status == BRCMF_E_STATUS_TIMEOUT) {
-			BRCMF_EVENT(("MACEVENT: %s, MAC %s, %s, TIMEOUT\n",
-				     event_name, eabuf, auth_str));
-		} else if (status == BRCMF_E_STATUS_FAIL) {
-			BRCMF_EVENT(("MACEVENT: %s, MAC %s, %s, FAILURE, "
-				     "reason %d\n",
-				     event_name, eabuf, auth_str, (int)reason));
+		if (event_type == BRCMF_E_AUTH_IND)
+			brcmf_dbg(EVENT, "MACEVENT: %s, MAC %s, %s\n",
+				  event_name, eabuf, auth_str);
+		else if (status == BRCMF_E_STATUS_SUCCESS)
+			brcmf_dbg(EVENT, "MACEVENT: %s, MAC %s, %s, SUCCESS\n",
+				  event_name, eabuf, auth_str);
+		else if (status == BRCMF_E_STATUS_TIMEOUT)
+			brcmf_dbg(EVENT, "MACEVENT: %s, MAC %s, %s, TIMEOUT\n",
+				  event_name, eabuf, auth_str);
+		else if (status == BRCMF_E_STATUS_FAIL) {
+			brcmf_dbg(EVENT, "MACEVENT: %s, MAC %s, %s, FAILURE, reason %d\n",
+				  event_name, eabuf, auth_str, (int)reason);
 		}
 
 		break;
@@ -594,149 +312,138 @@ brcmf_c_show_host_event(struct brcmf_event_msg *event, void *event_data)
 	case BRCMF_E_JOIN:
 	case BRCMF_E_ROAM:
 	case BRCMF_E_SET_SSID:
-		if (status == BRCMF_E_STATUS_SUCCESS) {
-			BRCMF_EVENT(("MACEVENT: %s, MAC %s\n", event_name,
-				     eabuf));
-		} else if (status == BRCMF_E_STATUS_FAIL) {
-			BRCMF_EVENT(("MACEVENT: %s, failed\n", event_name));
-		} else if (status == BRCMF_E_STATUS_NO_NETWORKS) {
-			BRCMF_EVENT(("MACEVENT: %s, no networks found\n",
-				     event_name));
-		} else {
-			BRCMF_EVENT(("MACEVENT: %s, unexpected status %d\n",
-				     event_name, (int)status));
-		}
+		if (status == BRCMF_E_STATUS_SUCCESS)
+			brcmf_dbg(EVENT, "MACEVENT: %s, MAC %s\n",
+				  event_name, eabuf);
+		else if (status == BRCMF_E_STATUS_FAIL)
+			brcmf_dbg(EVENT, "MACEVENT: %s, failed\n", event_name);
+		else if (status == BRCMF_E_STATUS_NO_NETWORKS)
+			brcmf_dbg(EVENT, "MACEVENT: %s, no networks found\n",
+				  event_name);
+		else
+			brcmf_dbg(EVENT, "MACEVENT: %s, unexpected status %d\n",
+				  event_name, (int)status);
 		break;
 
 	case BRCMF_E_BEACON_RX:
-		if (status == BRCMF_E_STATUS_SUCCESS) {
-			BRCMF_EVENT(("MACEVENT: %s, SUCCESS\n", event_name));
-		} else if (status == BRCMF_E_STATUS_FAIL) {
-			BRCMF_EVENT(("MACEVENT: %s, FAIL\n", event_name));
-		} else {
-			BRCMF_EVENT(("MACEVENT: %s, status %d\n", event_name,
-				     status));
-		}
+		if (status == BRCMF_E_STATUS_SUCCESS)
+			brcmf_dbg(EVENT, "MACEVENT: %s, SUCCESS\n", event_name);
+		else if (status == BRCMF_E_STATUS_FAIL)
+			brcmf_dbg(EVENT, "MACEVENT: %s, FAIL\n", event_name);
+		else
+			brcmf_dbg(EVENT, "MACEVENT: %s, status %d\n",
+				  event_name, status);
 		break;
 
 	case BRCMF_E_LINK:
-		BRCMF_EVENT(("MACEVENT: %s %s\n", event_name,
-			     link ? "UP" : "DOWN"));
+		brcmf_dbg(EVENT, "MACEVENT: %s %s\n",
+			  event_name, link ? "UP" : "DOWN");
 		break;
 
 	case BRCMF_E_MIC_ERROR:
-		BRCMF_EVENT(("MACEVENT: %s, MAC %s, Group %d, Flush %d\n",
-			     event_name, eabuf, group, flush_txq));
+		brcmf_dbg(EVENT, "MACEVENT: %s, MAC %s, Group %d, Flush %d\n",
+			  event_name, eabuf, group, flush_txq);
 		break;
 
 	case BRCMF_E_ICV_ERROR:
 	case BRCMF_E_UNICAST_DECODE_ERROR:
 	case BRCMF_E_MULTICAST_DECODE_ERROR:
-		BRCMF_EVENT(("MACEVENT: %s, MAC %s\n", event_name, eabuf));
+		brcmf_dbg(EVENT, "MACEVENT: %s, MAC %s\n", event_name, eabuf);
 		break;
 
 	case BRCMF_E_TXFAIL:
-		BRCMF_EVENT(("MACEVENT: %s, RA %s\n", event_name, eabuf));
+		brcmf_dbg(EVENT, "MACEVENT: %s, RA %s\n", event_name, eabuf);
 		break;
 
 	case BRCMF_E_SCAN_COMPLETE:
 	case BRCMF_E_PMKID_CACHE:
-		BRCMF_EVENT(("MACEVENT: %s\n", event_name));
+		brcmf_dbg(EVENT, "MACEVENT: %s\n", event_name);
 		break;
 
 	case BRCMF_E_PFN_NET_FOUND:
 	case BRCMF_E_PFN_NET_LOST:
 	case BRCMF_E_PFN_SCAN_COMPLETE:
-		BRCMF_EVENT(("PNOEVENT: %s\n", event_name));
+		brcmf_dbg(EVENT, "PNOEVENT: %s\n", event_name);
 		break;
 
 	case BRCMF_E_PSK_SUP:
 	case BRCMF_E_PRUNE:
-		BRCMF_EVENT(("MACEVENT: %s, status %d, reason %d\n",
-			   event_name, (int)status, (int)reason));
+		brcmf_dbg(EVENT, "MACEVENT: %s, status %d, reason %d\n",
+			  event_name, (int)status, (int)reason);
 		break;
 
 	case BRCMF_E_TRACE:
-		{
-			static u32 seqnum_prev;
-			struct msgtrace_hdr hdr;
-			u32 nblost;
-			char *s, *p;
-
-			buf = (unsigned char *) event_data;
-			memcpy(&hdr, buf, sizeof(struct msgtrace_hdr));
-
-			if (hdr.version != MSGTRACE_VERSION) {
-				BRCMF_ERROR(
-				    ("\nMACEVENT: %s [unsupported version --> "
-				     "brcmf version:%d dongle version:%d]\n",
-				     event_name, MSGTRACE_VERSION, hdr.version)
-				);
-				/* Reset datalen to avoid display below */
-				datalen = 0;
-				break;
-			}
-
-			/* There are 2 bytes available at the end of data */
-			*(buf + sizeof(struct msgtrace_hdr)
-				 + be16_to_cpu(hdr.len)) = '\0';
-
-			if (be32_to_cpu(hdr.discarded_bytes)
-			    || be32_to_cpu(hdr.discarded_printf)) {
-				BRCMF_ERROR(
-				    ("\nWLC_E_TRACE: [Discarded traces in dongle -->"
-				     "discarded_bytes %d discarded_printf %d]\n",
-				     be32_to_cpu(hdr.discarded_bytes),
-				     be32_to_cpu(hdr.discarded_printf)));
-			}
-
-			nblost = be32_to_cpu(hdr.seqnum) - seqnum_prev - 1;
-			if (nblost > 0) {
-				BRCMF_ERROR(
-				    ("\nWLC_E_TRACE: [Event lost --> seqnum %d nblost %d\n",
-				     be32_to_cpu(hdr.seqnum), nblost));
-			}
-			seqnum_prev = be32_to_cpu(hdr.seqnum);
-
-			/* Display the trace buffer. Advance from \n to \n to
-			 * avoid display big
-			 * printf (issue with Linux printk )
-			 */
-			p = (char *)&buf[sizeof(struct msgtrace_hdr)];
-			while ((s = strstr(p, "\n")) != NULL) {
-				*s = '\0';
-				printk(KERN_DEBUG"%s\n", p);
-				p = s + 1;
-			}
-			printk(KERN_DEBUG "%s\n", p);
+		buf = (unsigned char *) event_data;
+		memcpy(&hdr, buf, sizeof(struct msgtrace_hdr));
 
+		if (hdr.version != MSGTRACE_VERSION) {
+			brcmf_dbg(ERROR,
+				  "MACEVENT: %s [unsupported version --> brcmf"
+				  " version:%d dongle version:%d]\n",
+				  event_name, MSGTRACE_VERSION, hdr.version);
 			/* Reset datalen to avoid display below */
 			datalen = 0;
+			break;
+		}
+
+		/* There are 2 bytes available at the end of data */
+		*(buf + sizeof(struct msgtrace_hdr)
+			 + be16_to_cpu(hdr.len)) = '\0';
+
+		if (be32_to_cpu(hdr.discarded_bytes)
+		    || be32_to_cpu(hdr.discarded_printf))
+			brcmf_dbg(ERROR,
+				  "WLC_E_TRACE: [Discarded traces in dongle -->"
+				  " discarded_bytes %d discarded_printf %d]\n",
+				  be32_to_cpu(hdr.discarded_bytes),
+				  be32_to_cpu(hdr.discarded_printf));
+
+		nblost = be32_to_cpu(hdr.seqnum) - seqnum_prev - 1;
+		if (nblost > 0)
+			brcmf_dbg(ERROR, "WLC_E_TRACE: [Event lost --> seqnum "
+				  " %d nblost %d\n", be32_to_cpu(hdr.seqnum),
+				  nblost);
+		seqnum_prev = be32_to_cpu(hdr.seqnum);
+
+		/* Display the trace buffer. Advance from \n to \n to
+		 * avoid display big
+		 * printf (issue with Linux printk )
+		 */
+		p = (char *)&buf[sizeof(struct msgtrace_hdr)];
+		while ((s = strstr(p, "\n")) != NULL) {
+			*s = '\0';
+			printk(KERN_DEBUG"%s\n", p);
+			p = s + 1;
 		}
+		printk(KERN_DEBUG "%s\n", p);
+
+		/* Reset datalen to avoid display below */
+		datalen = 0;
 		break;
 
 	case BRCMF_E_RSSI:
-		BRCMF_EVENT(("MACEVENT: %s %d\n", event_name,
-			     be32_to_cpu(*((int *)event_data))));
+		brcmf_dbg(EVENT, "MACEVENT: %s %d\n",
+			  event_name, be32_to_cpu(*((__be32 *)event_data)));
 		break;
 
 	default:
-		BRCMF_EVENT(("MACEVENT: %s %d, MAC %s, status %d, reason %d, "
-			     "auth %d\n", event_name, event_type, eabuf,
-			     (int)status, (int)reason, (int)auth_type));
+		brcmf_dbg(EVENT,
+			  "MACEVENT: %s %d, MAC %s, status %d, reason %d, "
+			  "auth %d\n", event_name, event_type, eabuf,
+			  (int)status, (int)reason, (int)auth_type);
 		break;
 	}
 
 	/* show any appended data */
 	if (datalen) {
 		buf = (unsigned char *) event_data;
-		BRCMF_EVENT((" data (%d) : ", datalen));
+		brcmf_dbg(EVENT, " data (%d) : ", datalen);
 		for (i = 0; i < datalen; i++)
-			BRCMF_EVENT((" 0x%02x ", *buf++));
-		BRCMF_EVENT(("\n"));
+			brcmf_dbg(EVENT, " 0x%02x ", *buf++);
+		brcmf_dbg(EVENT, "\n");
 	}
 }
-#endif				/* SHOW_EVENTS */
+#endif				/* BCMDBG */
 
 int
 brcmf_c_host_event(struct brcmf_info *drvr_priv, int *ifidx, void *pktdata,
@@ -744,20 +451,21 @@ brcmf_c_host_event(struct brcmf_info *drvr_priv, 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;
 	char *event_data;
 	u32 type, status;
 	u16 flags;
 	int evlen;
 
 	if (memcmp(BRCM_OUI, &pvt_data->hdr.oui[0], DOT11_OUI_LEN)) {
-		BRCMF_ERROR(("%s: mismatched OUI, bailing\n", __func__));
+		brcmf_dbg(ERROR, "mismatched OUI, bailing\n");
 		return -EBADE;
 	}
 
 	/* BRCM event pkt may be unaligned - use xxx_ua to load user_subtype. */
 	if (get_unaligned_be16(&pvt_data->hdr.usr_subtype) !=
 	    BCMILCP_BCM_SUBTYPE_EVENT) {
-		BRCMF_ERROR(("%s: mismatched subtype, bailing\n", __func__));
+		brcmf_dbg(ERROR, "mismatched subtype, bailing\n");
 		return -EBADE;
 	}
 
@@ -775,27 +483,22 @@ brcmf_c_host_event(struct brcmf_info *drvr_priv, int *ifidx, void *pktdata,
 
 	switch (type) {
 	case BRCMF_E_IF:
-		{
-			struct brcmf_if_event *ifevent =
-					(struct brcmf_if_event *) event_data;
-			BRCMF_TRACE(("%s: if event\n", __func__));
-
-			if (ifevent->ifidx > 0 &&
-				 ifevent->ifidx < BRCMF_MAX_IFS) {
-				if (ifevent->action == BRCMF_E_IF_ADD)
-					brcmf_add_if(drvr_priv, ifevent->ifidx,
-						   NULL, event->ifname,
-						   pvt_data->eth.h_dest,
-						   ifevent->flags,
-						   ifevent->bssidx);
-				else
-					brcmf_del_if(drvr_priv, ifevent->ifidx);
-			} else {
-				BRCMF_ERROR(("%s: Invalid ifidx %d for %s\n",
-					     __func__, ifevent->ifidx,
-					     event->ifname));
-			}
+		ifevent = (struct brcmf_if_event *) event_data;
+		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_priv, ifevent->ifidx, NULL,
+					     event->ifname,
+					     pvt_data->eth.h_dest,
+					     ifevent->flags, ifevent->bssidx);
+			else
+				brcmf_del_if(drvr_priv, ifevent->ifidx);
+		} else {
+			brcmf_dbg(ERROR, "Invalid ifidx %d for %s\n",
+				  ifevent->ifidx, event->ifname);
 		}
+
 		/* send up the if event: btamp user needs it */
 		*ifidx = brcmf_ifname2idx(drvr_priv, event->ifname);
 		break;
@@ -810,27 +513,28 @@ brcmf_c_host_event(struct brcmf_info *drvr_priv, int *ifidx, void *pktdata,
 		/* Fall through: this should get _everything_  */
 
 		*ifidx = brcmf_ifname2idx(drvr_priv, event->ifname);
-		BRCMF_TRACE(("%s: MAC event %d, flags %x, status %x\n",
-			     __func__, type, flags, status));
+		brcmf_dbg(TRACE, "MAC event %d, flags %x, status %x\n",
+			  type, flags, status);
 
 		/* put it back to BRCMF_E_NDIS_LINK */
 		if (type == BRCMF_E_NDIS_LINK) {
-			u32 temp;
+			u32 temp1;
+			__be32 temp2;
 
-			temp = get_unaligned_be32(&event->event_type);
-			BRCMF_TRACE(("Converted to WLC_E_LINK type %d\n",
-				     temp));
+			temp1 = get_unaligned_be32(&event->event_type);
+			brcmf_dbg(TRACE, "Converted to WLC_E_LINK type %d\n",
+				  temp1);
 
-			temp = be32_to_cpu(BRCMF_E_NDIS_LINK);
-			memcpy((void *)(&pvt_data->msg.event_type), &temp,
+			temp2 = cpu_to_be32(BRCMF_E_NDIS_LINK);
+			memcpy((void *)(&pvt_data->msg.event_type), &temp2,
 			       sizeof(pvt_data->msg.event_type));
 		}
 		break;
 	}
 
-#ifdef SHOW_EVENTS
+#ifdef BCMDBG
 	brcmf_c_show_host_event(event, event_data);
-#endif				/* SHOW_EVENTS */
+#endif				/* BCMDBG */
 
 	return 0;
 }
@@ -840,19 +544,22 @@ static int brcmf_c_pattern_atoh(char *src, char *dst)
 {
 	int i;
 	if (strncmp(src, "0x", 2) != 0 && strncmp(src, "0X", 2) != 0) {
-		BRCMF_ERROR(("Mask invalid format. Needs to start with 0x\n"));
-		return -1;
+		brcmf_dbg(ERROR, "Mask invalid format. Needs to start with 0x\n");
+		return -EINVAL;
 	}
 	src = src + 2;		/* Skip past 0x */
 	if (strlen(src) % 2 != 0) {
-		BRCMF_ERROR(("Mask invalid format. Length must be even.\n"));
-		return -1;
+		brcmf_dbg(ERROR, "Mask invalid format. Length must be even.\n");
+		return -EINVAL;
 	}
 	for (i = 0; *src != '\0'; i++) {
+		unsigned long res;
 		char num[3];
 		strncpy(num, src, 2);
 		num[2] = '\0';
-		dst[i] = (u8) simple_strtoul(num, NULL, 16);
+		if (kstrtoul(num, 16, &res))
+			return -EINVAL;
+		dst[i] = (u8)res;
 		src += 2;
 	}
 	return i;
@@ -862,22 +569,23 @@ void
 brcmf_c_pktfilter_offload_enable(struct brcmf_pub *drvr, 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 *arg_save = 0, *arg_org = 0;
+	char *arg_save = NULL, *arg_org = NULL;
 	int rc;
 	char buf[128];
-	struct brcmf_pkt_filter_enable enable_parm;
-	struct brcmf_pkt_filter_enable *pkt_filterp;
+	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);
-	if (!arg_save) {
-		BRCMF_ERROR(("%s: kmalloc failed\n", __func__));
+	if (!arg_save)
 		goto fail;
-	}
+
 	arg_org = arg_save;
 	memcpy(arg_save, arg, strlen(arg) + 1);
 
@@ -885,7 +593,7 @@ brcmf_c_pktfilter_offload_enable(struct brcmf_pub *drvr, char *arg, int enable,
 
 	i = 0;
 	if (NULL == argv[i]) {
-		BRCMF_ERROR(("No args provided\n"));
+		brcmf_dbg(ERROR, "No args provided\n");
 		goto fail;
 	}
 
@@ -895,36 +603,38 @@ brcmf_c_pktfilter_offload_enable(struct brcmf_pub *drvr, char *arg, int enable,
 	buf[str_len] = '\0';
 	buf_len = str_len + 1;
 
-	pkt_filterp = (struct brcmf_pkt_filter_enable *) (buf + str_len + 1);
+	pkt_filterp = (struct brcmf_pkt_filter_enable_le *) (buf + str_len + 1);
 
 	/* Parse packet filter id. */
-	enable_parm.id = simple_strtoul(argv[i], NULL, 0);
+	enable_parm.id = 0;
+	if (!kstrtoul(argv[i], 0, &res))
+		enable_parm.id = cpu_to_le32((u32)res);
 
 	/* Parse enable/disable value. */
-	enable_parm.enable = enable;
+	enable_parm.enable = cpu_to_le32(enable);
 
 	buf_len += sizeof(enable_parm);
 	memcpy((char *)pkt_filterp, &enable_parm, sizeof(enable_parm));
 
 	/* Enable/disable the specified filter. */
-	rc = brcmf_proto_cdc_set_ioctl(drvr, 0, BRCMF_C_SET_VAR, buf, buf_len);
+	rc = brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, buf, buf_len);
 	rc = rc >= 0 ? 0 : rc;
 	if (rc)
-		BRCMF_TRACE(("%s: failed to add pktfilter %s, retcode = %d\n",
-			     __func__, arg, rc));
+		brcmf_dbg(TRACE, "failed to add pktfilter %s, retcode = %d\n",
+			  arg, rc);
 	else
-		BRCMF_TRACE(("%s: successfully added pktfilter %s\n",
-			     __func__, arg));
+		brcmf_dbg(TRACE, "successfully added pktfilter %s\n", arg);
 
 	/* Contorl the master mode */
-	brcmu_mkiovar("pkt_filter_mode", (char *)&master_mode, 4, buf,
+	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_ioctl(drvr, 0, BRCMF_C_SET_VAR, buf,
+	rc = brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, buf,
 				       sizeof(buf));
 	rc = rc >= 0 ? 0 : rc;
 	if (rc)
-		BRCMF_TRACE(("%s: failed to add pktfilter %s, retcode = %d\n",
-			     __func__, arg, rc));
+		brcmf_dbg(TRACE, "failed to add pktfilter %s, retcode = %d\n",
+			  arg, rc);
 
 fail:
 	kfree(arg_org);
@@ -933,32 +643,27 @@ fail:
 void brcmf_c_pktfilter_offload_set(struct brcmf_pub *drvr, char *arg)
 {
 	const char *str;
-	struct brcmf_pkt_filter pkt_filter;
-	struct brcmf_pkt_filter *pkt_filterp;
+	struct brcmf_pkt_filter_le pkt_filter;
+	struct brcmf_pkt_filter_le *pkt_filterp;
+	unsigned long res;
 	int buf_len;
 	int str_len;
 	int rc;
 	u32 mask_size;
 	u32 pattern_size;
-	char *argv[8], *buf = 0;
+	char *argv[8], *buf = NULL;
 	int i = 0;
-	char *arg_save = 0, *arg_org = 0;
+	char *arg_save = NULL, *arg_org = NULL;
 
-	arg_save = kmalloc(strlen(arg) + 1, GFP_ATOMIC);
-	if (!arg_save) {
-		BRCMF_ERROR(("%s: kmalloc failed\n", __func__));
+	arg_save = kstrdup(arg, GFP_ATOMIC);
+	if (!arg_save)
 		goto fail;
-	}
 
 	arg_org = arg_save;
 
 	buf = kmalloc(PKTFILTER_BUF_SIZE, GFP_ATOMIC);
-	if (!buf) {
-		BRCMF_ERROR(("%s: kmalloc failed\n", __func__));
+	if (!buf)
 		goto fail;
-	}
-
-	strcpy(arg_save, arg);
 
 	argv[i] = strsep(&arg_save, " ");
 	while (argv[i++])
@@ -966,7 +671,7 @@ void brcmf_c_pktfilter_offload_set(struct brcmf_pub *drvr, char *arg)
 
 	i = 0;
 	if (NULL == argv[i]) {
-		BRCMF_ERROR(("No args provided\n"));
+		brcmf_dbg(ERROR, "No args provided\n");
 		goto fail;
 	}
 
@@ -975,37 +680,45 @@ void brcmf_c_pktfilter_offload_set(struct brcmf_pub *drvr, char *arg)
 	str_len = strlen(str);
 	buf_len = str_len + 1;
 
-	pkt_filterp = (struct brcmf_pkt_filter *) (buf + str_len + 1);
+	pkt_filterp = (struct brcmf_pkt_filter_le *) (buf + str_len + 1);
 
 	/* Parse packet filter id. */
-	pkt_filter.id = simple_strtoul(argv[i], NULL, 0);
+	pkt_filter.id = 0;
+	if (!kstrtoul(argv[i], 0, &res))
+		pkt_filter.id = cpu_to_le32((u32)res);
 
 	if (NULL == argv[++i]) {
-		BRCMF_ERROR(("Polarity not provided\n"));
+		brcmf_dbg(ERROR, "Polarity not provided\n");
 		goto fail;
 	}
 
 	/* Parse filter polarity. */
-	pkt_filter.negate_match = simple_strtoul(argv[i], NULL, 0);
+	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_ERROR(("Filter type not provided\n"));
+		brcmf_dbg(ERROR, "Filter type not provided\n");
 		goto fail;
 	}
 
 	/* Parse filter type. */
-	pkt_filter.type = simple_strtoul(argv[i], NULL, 0);
+	pkt_filter.type = 0;
+	if (!kstrtoul(argv[i], 0, &res))
+		pkt_filter.type = cpu_to_le32((u32)res);
 
 	if (NULL == argv[++i]) {
-		BRCMF_ERROR(("Offset not provided\n"));
+		brcmf_dbg(ERROR, "Offset not provided\n");
 		goto fail;
 	}
 
 	/* Parse pattern filter offset. */
-	pkt_filter.u.pattern.offset = simple_strtoul(argv[i], NULL, 0);
+	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_ERROR(("Bitmask not provided\n"));
+		brcmf_dbg(ERROR, "Bitmask not provided\n");
 		goto fail;
 	}
 
@@ -1015,7 +728,7 @@ void brcmf_c_pktfilter_offload_set(struct brcmf_pub *drvr, char *arg)
 		   (argv[i], (char *)pkt_filterp->u.pattern.mask_and_pattern);
 
 	if (NULL == argv[++i]) {
-		BRCMF_ERROR(("Pattern not provided\n"));
+		brcmf_dbg(ERROR, "Pattern not provided\n");
 		goto fail;
 	}
 
@@ -1026,11 +739,11 @@ void brcmf_c_pktfilter_offload_set(struct brcmf_pub *drvr, char *arg)
 				   mask_and_pattern[mask_size]);
 
 	if (mask_size != pattern_size) {
-		BRCMF_ERROR(("Mask and pattern not the same size\n"));
+		brcmf_dbg(ERROR, "Mask and pattern not the same size\n");
 		goto fail;
 	}
 
-	pkt_filter.u.pattern.size_bytes = mask_size;
+	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);
 
@@ -1043,15 +756,14 @@ void brcmf_c_pktfilter_offload_set(struct brcmf_pub *drvr, char *arg)
 	       &pkt_filter,
 	       BRCMF_PKT_FILTER_FIXED_LEN + BRCMF_PKT_FILTER_PATTERN_FIXED_LEN);
 
-	rc = brcmf_proto_cdc_set_ioctl(drvr, 0, BRCMF_C_SET_VAR, buf, buf_len);
+	rc = brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, buf, buf_len);
 	rc = rc >= 0 ? 0 : rc;
 
 	if (rc)
-		BRCMF_TRACE(("%s: failed to add pktfilter %s, retcode = %d\n",
-			     __func__, arg, rc));
+		brcmf_dbg(TRACE, "failed to add pktfilter %s, retcode = %d\n",
+			  arg, rc);
 	else
-		BRCMF_TRACE(("%s: successfully added pktfilter %s\n",
-			     __func__, arg));
+		brcmf_dbg(TRACE, "successfully added pktfilter %s\n", arg);
 
 fail:
 	kfree(arg_org);
@@ -1059,49 +771,50 @@ fail:
 	kfree(buf);
 }
 
-void brcmf_c_arp_offload_set(struct brcmf_pub *drvr, int arp_mode)
+static void brcmf_c_arp_offload_set(struct brcmf_pub *drvr, int arp_mode)
 {
 	char iovbuf[32];
 	int retcode;
 
-	brcmu_mkiovar("arp_ol", (char *)&arp_mode, 4, iovbuf, sizeof(iovbuf));
-	retcode = brcmf_proto_cdc_set_ioctl(drvr, 0, BRCMF_C_SET_VAR,
+	brcmf_c_mkiovar("arp_ol", (char *)&arp_mode, 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_TRACE(("%s: failed to set ARP offload mode to 0x%x, "
-			     "retcode = %d\n", __func__, arp_mode, retcode));
+		brcmf_dbg(TRACE, "failed to set ARP offload mode to 0x%x, retcode = %d\n",
+			  arp_mode, retcode);
 	else
-		BRCMF_TRACE(("%s: successfully set ARP offload mode to 0x%x\n",
-			     __func__, arp_mode));
+		brcmf_dbg(TRACE, "successfully set ARP offload mode to 0x%x\n",
+			  arp_mode);
 }
 
-void brcmf_c_arp_offload_enable(struct brcmf_pub *drvr, int arp_enable)
+static void brcmf_c_arp_offload_enable(struct brcmf_pub *drvr, int arp_enable)
 {
 	char iovbuf[32];
 	int retcode;
 
-	brcmu_mkiovar("arpoe", (char *)&arp_enable, 4, iovbuf, sizeof(iovbuf));
-	retcode = brcmf_proto_cdc_set_ioctl(drvr, 0, BRCMF_C_SET_VAR,
+	brcmf_c_mkiovar("arpoe", (char *)&arp_enable, 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_TRACE(("%s: failed to enabe ARP offload to %d, "
-			     "retcode = %d\n", __func__, arp_enable, retcode));
+		brcmf_dbg(TRACE, "failed to enable ARP offload to %d, retcode = %d\n",
+			  arp_enable, retcode);
 	else
-		BRCMF_TRACE(("%s: successfully enabed ARP offload to %d\n",
-			     __func__, arp_enable));
+		brcmf_dbg(TRACE, "successfully enabled ARP offload to %d\n",
+			  arp_enable);
 }
 
-int brcmf_c_preinit_ioctls(struct brcmf_pub *drvr)
+int brcmf_c_preinit_dcmds(struct brcmf_pub *drvr)
 {
 	char iovbuf[BRCMF_EVENTING_MASK_LEN + 12];	/*  Room for
 				 "event_msgs" + '\0' + bitvec  */
 	uint up = 0;
 	char buf[128], *ptr;
-	uint power_mode = PM_FAST;
 	u32 dongle_align = BRCMF_SDALIGN;
 	u32 glom = 0;
+	u32 roaming = 1;
 	uint bcn_timeout = 3;
 	int scan_assoc_time = 40;
 	int scan_unassoc_time = 40;
@@ -1111,83 +824,69 @@ int brcmf_c_preinit_ioctls(struct brcmf_pub *drvr)
 
 	/* Set Country code */
 	if (drvr->country_code[0] != 0) {
-		if (brcmf_proto_cdc_set_ioctl(drvr, 0, BRCMF_C_SET_COUNTRY,
-				     drvr->country_code,
-				     sizeof(drvr->country_code)) < 0) {
-			BRCMF_ERROR(("%s: country code setting failed\n",
-				     __func__));
-		}
+		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");
 	}
 
 	/* query for 'ver' to get version info from firmware */
 	memset(buf, 0, sizeof(buf));
 	ptr = buf;
-	brcmu_mkiovar("ver", 0, 0, buf, sizeof(buf));
-	brcmf_proto_cdc_query_ioctl(drvr, 0, BRCMF_C_GET_VAR, buf, sizeof(buf));
+	brcmf_c_mkiovar("ver", NULL, 0, buf, sizeof(buf));
+	brcmf_proto_cdc_query_dcmd(drvr, 0, BRCMF_C_GET_VAR, buf, sizeof(buf));
 	strsep(&ptr, "\n");
 	/* Print fw version info */
-	BRCMF_ERROR(("Firmware version = %s\n", buf));
-
-	/* Set PowerSave mode */
-	brcmf_proto_cdc_set_ioctl(drvr, 0, BRCMF_C_SET_PM, (char *)&power_mode,
-			 sizeof(power_mode));
+	brcmf_dbg(ERROR, "Firmware version = %s\n", buf);
 
 	/* Match Host and Dongle rx alignment */
-	brcmu_mkiovar("bus:txglomalign", (char *)&dongle_align, 4, iovbuf,
+	brcmf_c_mkiovar("bus:txglomalign", (char *)&dongle_align, 4, iovbuf,
 		    sizeof(iovbuf));
-	brcmf_proto_cdc_set_ioctl(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
+	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
 				  sizeof(iovbuf));
 
 	/* disable glom option per default */
-	brcmu_mkiovar("bus:txglom", (char *)&glom, 4, iovbuf, sizeof(iovbuf));
-	brcmf_proto_cdc_set_ioctl(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
+	brcmf_c_mkiovar("bus:txglom", (char *)&glom, 4, iovbuf, sizeof(iovbuf));
+	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
 				  sizeof(iovbuf));
 
 	/* Setup timeout if Beacons are lost and roam is off to report
 		 link down */
-	brcmu_mkiovar("bcn_timeout", (char *)&bcn_timeout, 4, iovbuf,
+	brcmf_c_mkiovar("bcn_timeout", (char *)&bcn_timeout, 4, iovbuf,
 		    sizeof(iovbuf));
-	brcmf_proto_cdc_set_ioctl(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
+	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
 				  sizeof(iovbuf));
 
 	/* Enable/Disable build-in roaming to allowed ext supplicant to take
 		 of romaing */
-	brcmu_mkiovar("roam_off", (char *)&brcmf_roam, 4,
+	brcmf_c_mkiovar("roam_off", (char *)&roaming, 4,
 		      iovbuf, sizeof(iovbuf));
-	brcmf_proto_cdc_set_ioctl(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
+	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
 				  sizeof(iovbuf));
 
 	/* Force STA UP */
-	if (brcmf_radio_up)
-		brcmf_proto_cdc_set_ioctl(drvr, 0, BRCMF_C_UP, (char *)&up,
-					  sizeof(up));
+	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_UP, (char *)&up, sizeof(up));
 
 	/* Setup event_msgs */
-	brcmu_mkiovar("event_msgs", drvr->eventmask, BRCMF_EVENTING_MASK_LEN,
+	brcmf_c_mkiovar("event_msgs", drvr->eventmask, BRCMF_EVENTING_MASK_LEN,
 		      iovbuf, sizeof(iovbuf));
-	brcmf_proto_cdc_set_ioctl(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
+	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
 				  sizeof(iovbuf));
 
-	brcmf_proto_cdc_set_ioctl(drvr, 0, BRCMF_C_SET_SCAN_CHANNEL_TIME,
+	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_SCAN_CHANNEL_TIME,
 			 (char *)&scan_assoc_time, sizeof(scan_assoc_time));
-	brcmf_proto_cdc_set_ioctl(drvr, 0, BRCMF_C_SET_SCAN_UNASSOC_TIME,
+	brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_SCAN_UNASSOC_TIME,
 			 (char *)&scan_unassoc_time, sizeof(scan_unassoc_time));
 
 	/* Set and enable ARP offload feature */
-	if (brcmf_arp_enable)
-		brcmf_c_arp_offload_set(drvr, brcmf_arp_mode);
-	brcmf_c_arp_offload_enable(drvr, brcmf_arp_enable);
+	brcmf_c_arp_offload_set(drvr, BRCMF_ARPOL_MODE);
+	brcmf_c_arp_offload_enable(drvr, true);
 
 	/* Set up pkt filter */
-	if (brcmf_pkt_filter_enable) {
-		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],
-			     brcmf_pkt_filter_init,
-			     brcmf_master_mode);
-		}
+	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);
 	}
 
 	brcmf_os_proto_unblock(drvr);
diff --git a/drivers/staging/brcm80211/brcmfmac/dhd_dbg.h b/drivers/staging/brcm80211/brcmfmac/dhd_dbg.h
index 5be4d7a609c2..7467922f0536 100644
--- a/drivers/staging/brcm80211/brcmfmac/dhd_dbg.h
+++ b/drivers/staging/brcm80211/brcmfmac/dhd_dbg.h
@@ -19,25 +19,21 @@
 
 #if defined(BCMDBG)
 
-#define BRCMF_ERROR(args) \
-	do {if ((brcmf_msg_level & BRCMF_ERROR_VAL) && (net_ratelimit())) \
-		printk args; } while (0)
-#define BRCMF_TRACE(args)	do {if (brcmf_msg_level & BRCMF_TRACE_VAL) \
-					printk args; } while (0)
-#define BRCMF_INFO(args)	do {if (brcmf_msg_level & BRCMF_INFO_VAL) \
-					printk args; } while (0)
-#define BRCMF_DATA(args)	do {if (brcmf_msg_level & BRCMF_DATA_VAL) \
-					printk args; } while (0)
-#define BRCMF_CTL(args)		do {if (brcmf_msg_level & BRCMF_CTL_VAL) \
-					printk args; } while (0)
-#define BRCMF_TIMER(args)	do {if (brcmf_msg_level & BRCMF_TIMER_VAL) \
-					printk args; } while (0)
-#define BRCMF_INTR(args)	do {if (brcmf_msg_level & BRCMF_INTR_VAL) \
-					printk args; } while (0)
-#define BRCMF_GLOM(args)	do {if (brcmf_msg_level & BRCMF_GLOM_VAL) \
-					printk args; } while (0)
-#define BRCMF_EVENT(args)	do {if (brcmf_msg_level & BRCMF_EVENT_VAL) \
-					printk args; } while (0)
+#define brcmf_dbg(level, fmt, ...)					\
+do {									\
+	if (BRCMF_ERROR_VAL == BRCMF_##level##_VAL) {			\
+		if (brcmf_msg_level & BRCMF_##level##_VAL) {		\
+			if (net_ratelimit())				\
+				printk(KERN_DEBUG "%s: " fmt,		\
+				       __func__, ##__VA_ARGS__);	\
+		}							\
+	} else {							\
+		if (brcmf_msg_level & BRCMF_##level##_VAL) {		\
+			printk(KERN_DEBUG "%s: " fmt,			\
+			       __func__, ##__VA_ARGS__);		\
+		}							\
+	}								\
+} while (0)
 
 #define BRCMF_DATA_ON()		(brcmf_msg_level & BRCMF_DATA_VAL)
 #define BRCMF_CTL_ON()		(brcmf_msg_level & BRCMF_CTL_VAL)
@@ -47,20 +43,12 @@
 
 #else	/* (defined BCMDBG) || (defined BCMDBG) */
 
-#define BRCMF_ERROR(args)  do {if (net_ratelimit()) printk args; } while (0)
-#define BRCMF_TRACE(args)
-#define BRCMF_INFO(args)
-#define BRCMF_DATA(args)
-#define BRCMF_CTL(args)
-#define BRCMF_TIMER(args)
-#define BRCMF_INTR(args)
-#define BRCMF_GLOM(args)
-#define BRCMF_EVENT(args)
+#define brcmf_dbg(level, fmt, ...) no_printk(fmt, ##__VA_ARGS__)
 
 #define BRCMF_DATA_ON()		0
 #define BRCMF_CTL_ON()		0
 #define BRCMF_HDRS_ON()		0
-#define BRCMF_BYTES_ON()		0
+#define BRCMF_BYTES_ON()	0
 #define BRCMF_GLOM_ON()		0
 
 #endif				/* defined(BCMDBG) */
diff --git a/drivers/staging/brcm80211/brcmfmac/dhd_linux.c b/drivers/staging/brcm80211/brcmfmac/dhd_linux.c
index 05dada98eb6b..4acbac5a74c6 100644
--- a/drivers/staging/brcm80211/brcmfmac/dhd_linux.c
+++ b/drivers/staging/brcm80211/brcmfmac/dhd_linux.c
@@ -28,9 +28,12 @@
 #include <linux/fcntl.h>
 #include <linux/fs.h>
 #include <linux/uaccess.h>
-#include <linux/interrupt.h>
 #include <linux/hardirq.h>
+#include <linux/mutex.h>
+#include <linux/wait.h>
+#include <linux/module.h>
 #include <net/cfg80211.h>
+#include <net/rtnetlink.h>
 #include <defs.h>
 #include <brcmu_utils.h>
 #include <brcmu_wifi.h>
@@ -42,11 +45,6 @@
 #include "wl_cfg80211.h"
 #include "bcmchip.h"
 
-#if defined(CONFIG_PM_SLEEP)
-#include <linux/suspend.h>
-atomic_t brcmf_mmc_suspend;
-#endif	/*  defined(CONFIG_PM_SLEEP) */
-
 MODULE_AUTHOR("Broadcom Corporation");
 MODULE_DESCRIPTION("Broadcom 802.11n wireless LAN fullmac driver.");
 MODULE_SUPPORTED_DEVICE("Broadcom 802.11n WLAN fullmac cards");
@@ -57,15 +55,11 @@ MODULE_LICENSE("Dual BSD/GPL");
 struct brcmf_if {
 	struct brcmf_info *info;	/* back pointer to brcmf_info */
 	/* OS/stack specifics */
-	struct net_device *net;
+	struct net_device *ndev;
 	struct net_device_stats stats;
 	int idx;		/* iface idx in dongle */
 	int state;		/* interface state */
-	uint subunit;		/* subunit */
 	u8 mac_addr[ETH_ALEN];	/* assigned MAC address */
-	bool attached;		/* Delayed attachment when unset */
-	bool txflowcontrol;	/* Per interface flow control indicator */
-	char name[IFNAMSIZ];	/* linux interface name */
 };
 
 /* Local private structure (extension of pub) */
@@ -75,14 +69,10 @@ struct brcmf_info {
 	/* OS/stack specifics */
 	struct brcmf_if *iflist[BRCMF_MAX_IFS];
 
-	struct semaphore proto_sem;
-	wait_queue_head_t ioctl_resp_wait;
+	struct mutex proto_block;
 
-	/* Thread to issue ioctl for multicast */
-	struct task_struct *sysioc_tsk;
-	struct semaphore sysioc_sem;
-	bool set_multicast;
-	bool set_macaddress;
+	struct work_struct setmacaddr_work;
+	struct work_struct multicast_work;
 	u8 macvalue[ETH_ALEN];
 	atomic_t pend_8021x_cnt;
 };
@@ -90,148 +80,13 @@ struct brcmf_info {
 /* Error bits */
 module_param(brcmf_msg_level, int, 0);
 
-/* Spawn a thread for system ioctls (set mac, set mcast) */
-uint brcmf_sysioc = true;
-module_param(brcmf_sysioc, uint, 0);
-
-/* ARP offload agent mode : Enable ARP Host Auto-Reply
-and ARP Peer Auto-Reply */
-uint brcmf_arp_mode = 0xb;
-module_param(brcmf_arp_mode, uint, 0);
-
-/* ARP offload enable */
-uint brcmf_arp_enable = true;
-module_param(brcmf_arp_enable, uint, 0);
-
-/* Global Pkt filter enable control */
-uint brcmf_pkt_filter_enable = true;
-module_param(brcmf_pkt_filter_enable, uint, 0);
-
-/*  Pkt filter init setup */
-uint brcmf_pkt_filter_init;
-module_param(brcmf_pkt_filter_init, uint, 0);
-
-/* Pkt filter mode control */
-uint brcmf_master_mode = true;
-module_param(brcmf_master_mode, uint, 0);
-
-module_param(brcmf_dongle_memsize, int, 0);
-
-/* Contorl fw roaming */
-uint brcmf_roam = 1;
-
-/* Control radio state */
-uint brcmf_radio_up = 1;
-
-/* Network inteface name */
-char iface_name[IFNAMSIZ] = "wlan";
-module_param_string(iface_name, iface_name, IFNAMSIZ, 0);
-
-/* The following are specific to the SDIO dongle */
-
-/* IOCTL response timeout */
-int brcmf_ioctl_timeout_msec = IOCTL_RESP_TIMEOUT;
-
-/* Idle timeout for backplane clock */
-int brcmf_idletime = BRCMF_IDLETIME_TICKS;
-module_param(brcmf_idletime, int, 0);
-
-/* Use polling */
-uint brcmf_poll;
-module_param(brcmf_poll, uint, 0);
 
-/* Use interrupts */
-uint brcmf_intr = true;
-module_param(brcmf_intr, uint, 0);
-
-/* SDIO Drive Strength (in milliamps) */
-uint brcmf_sdiod_drive_strength = 6;
-module_param(brcmf_sdiod_drive_strength, uint, 0);
-
-/* Tx/Rx bounds */
-module_param(brcmf_txbound, uint, 0);
-module_param(brcmf_rxbound, uint, 0);
-
-#ifdef SDTEST
-/* Echo packet generator (pkts/s) */
-uint brcmf_pktgen;
-module_param(brcmf_pktgen, uint, 0);
-
-/* Echo packet len (0 => sawtooth, max 2040) */
-uint brcmf_pktgen_len;
-module_param(brcmf_pktgen_len, uint, 0);
-#endif
-
-static int brcmf_toe_get(struct brcmf_info *drvr_priv, int idx, u32 *toe_ol);
-static int brcmf_toe_set(struct brcmf_info *drvr_priv, int idx, u32 toe_ol);
-static int brcmf_host_event(struct brcmf_info *drvr_priv, int *ifidx, void *pktdata,
-			    struct brcmf_event_msg *event_ptr,
-			    void **data_ptr);
-
-/*
- * Generalized timeout mechanism.  Uses spin sleep with exponential
- * back-off until
- * the sleep time reaches one jiffy, then switches over to task delay.  Usage:
- *
- *      brcmf_timeout_start(&tmo, usec);
- *      while (!brcmf_timeout_expired(&tmo))
- *              if (poll_something())
- *                      break;
- *      if (brcmf_timeout_expired(&tmo))
- *              fatal();
- */
-
-void brcmf_timeout_start(struct brcmf_timeout *tmo, uint usec)
-{
-	tmo->limit = usec;
-	tmo->increment = 0;
-	tmo->elapsed = 0;
-	tmo->tick = 1000000 / HZ;
-}
-
-int brcmf_timeout_expired(struct brcmf_timeout *tmo)
-{
-	/* Does nothing the first call */
-	if (tmo->increment == 0) {
-		tmo->increment = 1;
-		return 0;
-	}
-
-	if (tmo->elapsed >= tmo->limit)
-		return 1;
-
-	/* Add the delay that's about to take place */
-	tmo->elapsed += tmo->increment;
-
-	if (tmo->increment < tmo->tick) {
-		udelay(tmo->increment);
-		tmo->increment *= 2;
-		if (tmo->increment > tmo->tick)
-			tmo->increment = tmo->tick;
-	} else {
-		wait_queue_head_t delay_wait;
-		DECLARE_WAITQUEUE(wait, current);
-		int pending;
-		init_waitqueue_head(&delay_wait);
-		add_wait_queue(&delay_wait, &wait);
-		set_current_state(TASK_INTERRUPTIBLE);
-		schedule_timeout(1);
-		pending = signal_pending(current);
-		remove_wait_queue(&delay_wait, &wait);
-		set_current_state(TASK_RUNNING);
-		if (pending)
-			return 1;	/* Interrupted */
-	}
-
-	return 0;
-}
-
-static int brcmf_net2idx(struct brcmf_info *drvr_priv, struct net_device *net)
+static int brcmf_net2idx(struct brcmf_info *drvr_priv, struct net_device *ndev)
 {
 	int i = 0;
 
 	while (i < BRCMF_MAX_IFS) {
-		if (drvr_priv->iflist[i] && (drvr_priv->iflist[i]->net == net))
+		if (drvr_priv->iflist[i] && drvr_priv->iflist[i]->ndev == ndev)
 			return i;
 		i++;
 	}
@@ -242,16 +97,18 @@ static int brcmf_net2idx(struct brcmf_info *drvr_priv, struct net_device *net)
 int brcmf_ifname2idx(struct brcmf_info *drvr_priv, char *name)
 {
 	int i = BRCMF_MAX_IFS;
+	struct brcmf_if *ifp;
 
 	if (name == NULL || *name == '\0')
 		return 0;
 
-	while (--i > 0)
-		if (drvr_priv->iflist[i]
-		    && !strncmp(drvr_priv->iflist[i]->name, name, IFNAMSIZ))
+	while (--i > 0) {
+		ifp = drvr_priv->iflist[i];
+		if (ifp && !strncmp(ifp->ndev->name, name, IFNAMSIZ))
 			break;
+	}
 
-	BRCMF_TRACE(("%s: return idx %d for \"%s\"\n", __func__, i, name));
+	brcmf_dbg(TRACE, "return idx %d for \"%s\"\n", i, name);
 
 	return i;		/* default - the primary interface */
 }
@@ -261,56 +118,58 @@ char *brcmf_ifname(struct brcmf_pub *drvr, int ifidx)
 	struct brcmf_info *drvr_priv = drvr->info;
 
 	if (ifidx < 0 || ifidx >= BRCMF_MAX_IFS) {
-		BRCMF_ERROR(("%s: ifidx %d out of range\n", __func__, ifidx));
+		brcmf_dbg(ERROR, "ifidx %d out of range\n", ifidx);
 		return "<if_bad>";
 	}
 
 	if (drvr_priv->iflist[ifidx] == NULL) {
-		BRCMF_ERROR(("%s: null i/f %d\n", __func__, ifidx));
+		brcmf_dbg(ERROR, "null i/f %d\n", ifidx);
 		return "<if_null>";
 	}
 
-	if (drvr_priv->iflist[ifidx]->net)
-		return drvr_priv->iflist[ifidx]->net->name;
+	if (drvr_priv->iflist[ifidx]->ndev)
+		return drvr_priv->iflist[ifidx]->ndev->name;
 
 	return "<if_none>";
 }
 
-static void _brcmf_set_multicast_list(struct brcmf_info *drvr_priv, int ifidx)
+static void _brcmf_set_multicast_list(struct work_struct *work)
 {
-	struct net_device *dev;
+	struct net_device *ndev;
 	struct netdev_hw_addr *ha;
-	u32 allmulti, cnt;
+	u32 dcmd_value, cnt;
+	__le32 cnt_le;
+	__le32 dcmd_le_value;
 
-	struct brcmf_ioctl ioc;
+	struct brcmf_dcmd dcmd;
 	char *buf, *bufp;
 	uint buflen;
 	int ret;
 
-	dev = drvr_priv->iflist[ifidx]->net;
-	cnt = netdev_mc_count(dev);
+	struct brcmf_info *drvr_priv = container_of(work, struct brcmf_info,
+						    multicast_work);
+
+	ndev = drvr_priv->iflist[0]->ndev;
+	cnt = netdev_mc_count(ndev);
 
 	/* Determine initial value of allmulti flag */
-	allmulti = (dev->flags & IFF_ALLMULTI) ? true : false;
+	dcmd_value = (ndev->flags & IFF_ALLMULTI) ? true : false;
 
 	/* Send down the multicast list first. */
 
 	buflen = sizeof("mcast_list") + sizeof(cnt) + (cnt * ETH_ALEN);
 	bufp = buf = kmalloc(buflen, GFP_ATOMIC);
-	if (!bufp) {
-		BRCMF_ERROR(("%s: out of memory for mcast_list, cnt %d\n",
-			     brcmf_ifname(&drvr_priv->pub, ifidx), cnt));
+	if (!bufp)
 		return;
-	}
 
 	strcpy(bufp, "mcast_list");
 	bufp += strlen("mcast_list") + 1;
 
-	cnt = cpu_to_le32(cnt);
-	memcpy(bufp, &cnt, sizeof(cnt));
-	bufp += sizeof(cnt);
+	cnt_le = cpu_to_le32(cnt);
+	memcpy(bufp, &cnt_le, sizeof(cnt));
+	bufp += sizeof(cnt_le);
 
-	netdev_for_each_mc_addr(ha, dev) {
+	netdev_for_each_mc_addr(ha, ndev) {
 		if (!cnt)
 			break;
 		memcpy(bufp, ha->addr, ETH_ALEN);
@@ -318,17 +177,17 @@ static void _brcmf_set_multicast_list(struct brcmf_info *drvr_priv, int ifidx)
 		cnt--;
 	}
 
-	memset(&ioc, 0, sizeof(ioc));
-	ioc.cmd = BRCMF_C_SET_VAR;
-	ioc.buf = buf;
-	ioc.len = buflen;
-	ioc.set = true;
+	memset(&dcmd, 0, sizeof(dcmd));
+	dcmd.cmd = BRCMF_C_SET_VAR;
+	dcmd.buf = buf;
+	dcmd.len = buflen;
+	dcmd.set = true;
 
-	ret = brcmf_proto_ioctl(&drvr_priv->pub, ifidx, &ioc, ioc.buf, ioc.len);
+	ret = brcmf_proto_dcmd(&drvr_priv->pub, 0, &dcmd, dcmd.len);
 	if (ret < 0) {
-		BRCMF_ERROR(("%s: set mcast_list failed, cnt %d\n",
-			     brcmf_ifname(&drvr_priv->pub, ifidx), cnt));
-		allmulti = cnt ? true : allmulti;
+		brcmf_dbg(ERROR, "%s: set mcast_list failed, cnt %d\n",
+			  brcmf_ifname(&drvr_priv->pub, 0), cnt);
+		dcmd_value = cnt ? true : dcmd_value;
 	}
 
 	kfree(buf);
@@ -338,36 +197,34 @@ static void _brcmf_set_multicast_list(struct brcmf_info *drvr_priv, int ifidx)
 	 * were trying to set some addresses and dongle rejected it...
 	 */
 
-	buflen = sizeof("allmulti") + sizeof(allmulti);
+	buflen = sizeof("allmulti") + sizeof(dcmd_value);
 	buf = kmalloc(buflen, GFP_ATOMIC);
-	if (!buf) {
-		BRCMF_ERROR(("%s: out of memory for allmulti\n",
-			     brcmf_ifname(&drvr_priv->pub, ifidx)));
+	if (!buf)
 		return;
-	}
-	allmulti = cpu_to_le32(allmulti);
-
-	if (!brcmu_mkiovar
-	    ("allmulti", (void *)&allmulti, sizeof(allmulti), buf, buflen)) {
-		BRCMF_ERROR(("%s: mkiovar failed for allmulti, datalen %d "
-			     "buflen %u\n",
-			     brcmf_ifname(&drvr_priv->pub, ifidx),
-			     (int)sizeof(allmulti), buflen));
+
+	dcmd_le_value = cpu_to_le32(dcmd_value);
+
+	if (!brcmf_c_mkiovar
+	    ("allmulti", (void *)&dcmd_le_value,
+	    sizeof(dcmd_le_value), buf, buflen)) {
+		brcmf_dbg(ERROR, "%s: mkiovar failed for allmulti, datalen %d buflen %u\n",
+			  brcmf_ifname(&drvr_priv->pub, 0),
+			  (int)sizeof(dcmd_value), buflen);
 		kfree(buf);
 		return;
 	}
 
-	memset(&ioc, 0, sizeof(ioc));
-	ioc.cmd = BRCMF_C_SET_VAR;
-	ioc.buf = buf;
-	ioc.len = buflen;
-	ioc.set = true;
+	memset(&dcmd, 0, sizeof(dcmd));
+	dcmd.cmd = BRCMF_C_SET_VAR;
+	dcmd.buf = buf;
+	dcmd.len = buflen;
+	dcmd.set = true;
 
-	ret = brcmf_proto_ioctl(&drvr_priv->pub, ifidx, &ioc, ioc.buf, ioc.len);
+	ret = brcmf_proto_dcmd(&drvr_priv->pub, 0, &dcmd, dcmd.len);
 	if (ret < 0) {
-		BRCMF_ERROR(("%s: set allmulti %d failed\n",
-			     brcmf_ifname(&drvr_priv->pub, ifidx),
-			     le32_to_cpu(allmulti)));
+		brcmf_dbg(ERROR, "%s: set allmulti %d failed\n",
+			  brcmf_ifname(&drvr_priv->pub, 0),
+			  le32_to_cpu(dcmd_le_value));
 	}
 
 	kfree(buf);
@@ -375,237 +232,84 @@ static void _brcmf_set_multicast_list(struct brcmf_info *drvr_priv, int ifidx)
 	/* Finally, pick up the PROMISC flag as well, like the NIC
 		 driver does */
 
-	allmulti = (dev->flags & IFF_PROMISC) ? true : false;
-	allmulti = cpu_to_le32(allmulti);
+	dcmd_value = (ndev->flags & IFF_PROMISC) ? true : false;
+	dcmd_le_value = cpu_to_le32(dcmd_value);
 
-	memset(&ioc, 0, sizeof(ioc));
-	ioc.cmd = BRCMF_C_SET_PROMISC;
-	ioc.buf = &allmulti;
-	ioc.len = sizeof(allmulti);
-	ioc.set = true;
+	memset(&dcmd, 0, sizeof(dcmd));
+	dcmd.cmd = BRCMF_C_SET_PROMISC;
+	dcmd.buf = &dcmd_le_value;
+	dcmd.len = sizeof(dcmd_le_value);
+	dcmd.set = true;
 
-	ret = brcmf_proto_ioctl(&drvr_priv->pub, ifidx, &ioc, ioc.buf, ioc.len);
+	ret = brcmf_proto_dcmd(&drvr_priv->pub, 0, &dcmd, dcmd.len);
 	if (ret < 0) {
-		BRCMF_ERROR(("%s: set promisc %d failed\n",
-			     brcmf_ifname(&drvr_priv->pub, ifidx),
-			     le32_to_cpu(allmulti)));
+		brcmf_dbg(ERROR, "%s: set promisc %d failed\n",
+			  brcmf_ifname(&drvr_priv->pub, 0),
+			  le32_to_cpu(dcmd_le_value));
 	}
 }
 
-static int _brcmf_set_mac_address(struct brcmf_info *drvr_priv, int ifidx, u8 *addr)
+static void
+_brcmf_set_mac_address(struct work_struct *work)
 {
 	char buf[32];
-	struct brcmf_ioctl ioc;
+	struct brcmf_dcmd dcmd;
 	int ret;
 
-	BRCMF_TRACE(("%s enter\n", __func__));
-	if (!brcmu_mkiovar
-	    ("cur_etheraddr", (char *)addr, ETH_ALEN, buf, 32)) {
-		BRCMF_ERROR(("%s: mkiovar failed for cur_etheraddr\n",
-			     brcmf_ifname(&drvr_priv->pub, ifidx)));
-		return -1;
-	}
-	memset(&ioc, 0, sizeof(ioc));
-	ioc.cmd = BRCMF_C_SET_VAR;
-	ioc.buf = buf;
-	ioc.len = 32;
-	ioc.set = true;
-
-	ret = brcmf_proto_ioctl(&drvr_priv->pub, ifidx, &ioc, ioc.buf, ioc.len);
-	if (ret < 0) {
-		BRCMF_ERROR(("%s: set cur_etheraddr failed\n",
-			     brcmf_ifname(&drvr_priv->pub, ifidx)));
-	} else {
-		memcpy(drvr_priv->iflist[ifidx]->net->dev_addr, addr, ETH_ALEN);
-	}
-
-	return ret;
-}
-
-#ifdef SOFTAP
-extern struct net_device *ap_net_dev;
-#endif
-
-/* Virtual interfaces only ((ifp && ifp->info && ifp->idx == true) */
-static void brcmf_op_if(struct brcmf_if *ifp)
-{
-	struct brcmf_info *drvr_priv;
-	int ret = 0, err = 0;
-
-	drvr_priv = ifp->info;
-
-	BRCMF_TRACE(("%s: idx %d, state %d\n", __func__, ifp->idx, ifp->state));
-
-	switch (ifp->state) {
-	case BRCMF_E_IF_ADD:
-		/*
-		 * Delete the existing interface before overwriting it
-		 * in case we missed the BRCMF_E_IF_DEL event.
-		 */
-		if (ifp->net != NULL) {
-			BRCMF_ERROR(("%s: ERROR: netdev:%s already exists, "
-				     "try free & unregister\n",
-				     __func__, ifp->net->name));
-			netif_stop_queue(ifp->net);
-			unregister_netdev(ifp->net);
-			free_netdev(ifp->net);
-		}
-		/* Allocate etherdev, including space for private structure */
-		ifp->net = alloc_etherdev(sizeof(drvr_priv));
-		if (!ifp->net) {
-			BRCMF_ERROR(("%s: OOM - alloc_etherdev\n", __func__));
-			ret = -ENOMEM;
-		}
-		if (ret == 0) {
-			strcpy(ifp->net->name, ifp->name);
-			memcpy(netdev_priv(ifp->net), &drvr_priv, sizeof(drvr_priv));
-			err = brcmf_net_attach(&drvr_priv->pub, ifp->idx);
-			if (err != 0) {
-				BRCMF_ERROR(("%s: brcmf_net_attach failed, "
-					     "err %d\n",
-					     __func__, err));
-				ret = -EOPNOTSUPP;
-			} else {
-#ifdef SOFTAP
-				/* semaphore that the soft AP CODE
-					 waits on */
-				extern struct semaphore ap_eth_sema;
-
-				/* save ptr to wl0.1 netdev for use
-					 in wl_iw.c  */
-				ap_net_dev = ifp->net;
-				/* signal to the SOFTAP 'sleeper' thread,
-					 wl0.1 is ready */
-				up(&ap_eth_sema);
-#endif
-				BRCMF_TRACE(("\n ==== pid:%x, net_device for "
-					     "if:%s created ===\n\n",
-					     current->pid, ifp->net->name));
-				ifp->state = 0;
-			}
-		}
-		break;
-	case BRCMF_E_IF_DEL:
-		if (ifp->net != NULL) {
-			BRCMF_TRACE(("\n%s: got 'WLC_E_IF_DEL' state\n",
-				     __func__));
-			netif_stop_queue(ifp->net);
-			unregister_netdev(ifp->net);
-			ret = BRCMF_DEL_IF;	/* Make sure the free_netdev()
-							 is called */
-		}
-		break;
-	default:
-		BRCMF_ERROR(("%s: bad op %d\n", __func__, ifp->state));
-		break;
-	}
+	struct brcmf_info *drvr_priv = container_of(work, struct brcmf_info,
+						    setmacaddr_work);
 
-	if (ret < 0) {
-		if (ifp->net)
-			free_netdev(ifp->net);
-
-		drvr_priv->iflist[ifp->idx] = NULL;
-		kfree(ifp);
-#ifdef SOFTAP
-		if (ifp->net == ap_net_dev)
-			ap_net_dev = NULL;	/*  NULL  SOFTAP global
-							 wl0.1 as well */
-#endif				/*  SOFTAP */
+	brcmf_dbg(TRACE, "enter\n");
+	if (!brcmf_c_mkiovar("cur_etheraddr", (char *)drvr_priv->macvalue,
+			   ETH_ALEN, buf, 32)) {
+		brcmf_dbg(ERROR, "%s: mkiovar failed for cur_etheraddr\n",
+			  brcmf_ifname(&drvr_priv->pub, 0));
+		return;
 	}
-}
-
-static int _brcmf_sysioc_thread(void *data)
-{
-	struct brcmf_info *drvr_priv = (struct brcmf_info *) data;
-	int i;
-#ifdef SOFTAP
-	bool in_ap = false;
-#endif
+	memset(&dcmd, 0, sizeof(dcmd));
+	dcmd.cmd = BRCMF_C_SET_VAR;
+	dcmd.buf = buf;
+	dcmd.len = 32;
+	dcmd.set = true;
 
-	allow_signal(SIGTERM);
+	ret = brcmf_proto_dcmd(&drvr_priv->pub, 0, &dcmd, dcmd.len);
+	if (ret < 0)
+		brcmf_dbg(ERROR, "%s: set cur_etheraddr failed\n",
+			  brcmf_ifname(&drvr_priv->pub, 0));
+	else
+		memcpy(drvr_priv->iflist[0]->ndev->dev_addr,
+		       drvr_priv->macvalue, ETH_ALEN);
 
-	while (down_interruptible(&drvr_priv->sysioc_sem) == 0) {
-		if (kthread_should_stop())
-			break;
-		for (i = 0; i < BRCMF_MAX_IFS; i++) {
-			struct brcmf_if *ifentry = drvr_priv->iflist[i];
-			if (ifentry) {
-#ifdef SOFTAP
-				in_ap = (ap_net_dev != NULL);
-#endif				/* SOFTAP */
-				if (ifentry->state)
-					brcmf_op_if(ifentry);
-#ifdef SOFTAP
-				if (drvr_priv->iflist[i] == NULL) {
-					BRCMF_TRACE(("\n\n %s: interface %d "
-						     "removed!\n", __func__,
-						     i));
-					continue;
-				}
-
-				if (in_ap && drvr_priv->set_macaddress) {
-					BRCMF_TRACE(("attempt to set MAC for"
-						     " %s in AP Mode,"
-						     " blocked.\n",
-						     ifentry->net->name));
-					drvr_priv->set_macaddress = false;
-					continue;
-				}
-
-				if (in_ap && drvr_priv->set_multicast) {
-					BRCMF_TRACE(("attempt to set MULTICAST "
-						     "list for %s in AP Mode, "
-						     "blocked.\n",
-						     ifentry->net->name));
-					drvr_priv->set_multicast = false;
-					continue;
-				}
-#endif				/* SOFTAP */
-				if (drvr_priv->set_multicast) {
-					drvr_priv->set_multicast = false;
-					_brcmf_set_multicast_list(drvr_priv, i);
-				}
-				if (drvr_priv->set_macaddress) {
-					drvr_priv->set_macaddress = false;
-					_brcmf_set_mac_address(drvr_priv, i,
-						drvr_priv->macvalue);
-				}
-			}
-		}
-	}
-	return 0;
+	return;
 }
 
-static int brcmf_netdev_set_mac_address(struct net_device *dev, void *addr)
+static int brcmf_netdev_set_mac_address(struct net_device *ndev, void *addr)
 {
-	int ret = 0;
-
-	struct brcmf_info *drvr_priv = *(struct brcmf_info **) netdev_priv(dev);
+	struct brcmf_info *drvr_priv = *(struct brcmf_info **)
+					netdev_priv(ndev);
 	struct sockaddr *sa = (struct sockaddr *)addr;
 	int ifidx;
 
-	ifidx = brcmf_net2idx(drvr_priv, dev);
+	ifidx = brcmf_net2idx(drvr_priv, ndev);
 	if (ifidx == BRCMF_BAD_IF)
 		return -1;
 
 	memcpy(&drvr_priv->macvalue, sa->sa_data, ETH_ALEN);
-	drvr_priv->set_macaddress = true;
-	up(&drvr_priv->sysioc_sem);
-
-	return ret;
+	schedule_work(&drvr_priv->setmacaddr_work);
+	return 0;
 }
 
-static void brcmf_netdev_set_multicast_list(struct net_device *dev)
+static void brcmf_netdev_set_multicast_list(struct net_device *ndev)
 {
-	struct brcmf_info *drvr_priv = *(struct brcmf_info **) netdev_priv(dev);
+	struct brcmf_info *drvr_priv = *(struct brcmf_info **)
+					netdev_priv(ndev);
 	int ifidx;
 
-	ifidx = brcmf_net2idx(drvr_priv, dev);
+	ifidx = brcmf_net2idx(drvr_priv, ndev);
 	if (ifidx == BRCMF_BAD_IF)
 		return;
 
-	drvr_priv->set_multicast = true;
-	up(&drvr_priv->sysioc_sem);
+	schedule_work(&drvr_priv->multicast_work);
 }
 
 int brcmf_sendpkt(struct brcmf_pub *drvr, int ifidx, struct sk_buff *pktbuf)
@@ -634,27 +338,27 @@ int brcmf_sendpkt(struct brcmf_pub *drvr, int ifidx, struct sk_buff *pktbuf)
 	return brcmf_sdbrcm_bus_txdata(drvr->bus, pktbuf);
 }
 
-static int brcmf_netdev_start_xmit(struct sk_buff *skb, struct net_device *net)
+static int brcmf_netdev_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 {
 	int ret;
-	struct brcmf_info *drvr_priv = *(struct brcmf_info **) netdev_priv(net);
+	struct brcmf_info *drvr_priv = *(struct brcmf_info **)
+					netdev_priv(ndev);
 	int ifidx;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	/* Reject if down */
 	if (!drvr_priv->pub.up || (drvr_priv->pub.busstate == BRCMF_BUS_DOWN)) {
-		BRCMF_ERROR(("%s: xmit rejected pub.up=%d busstate=%d\n",
-			     __func__, drvr_priv->pub.up,
-			     drvr_priv->pub.busstate));
-		netif_stop_queue(net);
+		brcmf_dbg(ERROR, "xmit rejected pub.up=%d busstate=%d\n",
+			  drvr_priv->pub.up, drvr_priv->pub.busstate);
+		netif_stop_queue(ndev);
 		return -ENODEV;
 	}
 
-	ifidx = brcmf_net2idx(drvr_priv, net);
+	ifidx = brcmf_net2idx(drvr_priv, ndev);
 	if (ifidx == BRCMF_BAD_IF) {
-		BRCMF_ERROR(("%s: bad ifidx %d\n", __func__, ifidx));
-		netif_stop_queue(net);
+		brcmf_dbg(ERROR, "bad ifidx %d\n", ifidx);
+		netif_stop_queue(ndev);
 		return -ENODEV;
 	}
 
@@ -662,15 +366,15 @@ static int brcmf_netdev_start_xmit(struct sk_buff *skb, struct net_device *net)
 	if (skb_headroom(skb) < drvr_priv->pub.hdrlen) {
 		struct sk_buff *skb2;
 
-		BRCMF_INFO(("%s: insufficient headroom\n",
-			    brcmf_ifname(&drvr_priv->pub, ifidx)));
+		brcmf_dbg(INFO, "%s: insufficient headroom\n",
+			  brcmf_ifname(&drvr_priv->pub, ifidx));
 		drvr_priv->pub.tx_realloc++;
 		skb2 = skb_realloc_headroom(skb, drvr_priv->pub.hdrlen);
 		dev_kfree_skb(skb);
 		skb = skb2;
 		if (skb == NULL) {
-			BRCMF_ERROR(("%s: skb_realloc_headroom failed\n",
-				     brcmf_ifname(&drvr_priv->pub, ifidx)));
+			brcmf_dbg(ERROR, "%s: skb_realloc_headroom failed\n",
+				  brcmf_ifname(&drvr_priv->pub, ifidx));
 			ret = -ENOMEM;
 			goto done;
 		}
@@ -690,17 +394,34 @@ done:
 
 void brcmf_txflowcontrol(struct brcmf_pub *drvr, int ifidx, bool state)
 {
-	struct net_device *net;
+	struct net_device *ndev;
 	struct brcmf_info *drvr_priv = drvr->info;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	drvr->txoff = state;
-	net = drvr_priv->iflist[ifidx]->net;
+	ndev = drvr_priv->iflist[ifidx]->ndev;
 	if (state == ON)
-		netif_stop_queue(net);
+		netif_stop_queue(ndev);
 	else
-		netif_wake_queue(net);
+		netif_wake_queue(ndev);
+}
+
+static int brcmf_host_event(struct brcmf_info *drvr_priv, int *ifidx,
+			    void *pktdata, struct brcmf_event_msg *event,
+			    void **data)
+{
+	int bcmerror = 0;
+
+	bcmerror = brcmf_c_host_event(drvr_priv, ifidx, pktdata, event, data);
+	if (bcmerror != 0)
+		return bcmerror;
+
+	if (drvr_priv->iflist[*ifidx]->ndev)
+		brcmf_cfg80211_event(drvr_priv->iflist[*ifidx]->ndev,
+				     event, *data);
+
+	return bcmerror;
 }
 
 void brcmf_rx_frame(struct brcmf_pub *drvr, int ifidx, struct sk_buff *skb,
@@ -715,7 +436,7 @@ void brcmf_rx_frame(struct brcmf_pub *drvr, int ifidx, struct sk_buff *skb,
 	struct brcmf_if *ifp;
 	struct brcmf_event_msg event;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	save_pktbuf = skb;
 
@@ -743,7 +464,7 @@ void brcmf_rx_frame(struct brcmf_pub *drvr, int ifidx, struct sk_buff *skb,
 		if (ifp == NULL)
 			ifp = drvr_priv->iflist[0];
 
-		skb->dev = ifp->net;
+		skb->dev = ifp->ndev;
 		skb->protocol = eth_type_trans(skb, skb->dev);
 
 		if (skb->pkt_type == PACKET_MULTICAST)
@@ -765,15 +486,15 @@ void brcmf_rx_frame(struct brcmf_pub *drvr, int ifidx, struct sk_buff *skb,
 		    !drvr_priv->iflist[ifidx]->state)
 			ifp = drvr_priv->iflist[ifidx];
 
-		if (ifp->net)
-			ifp->net->last_rx = jiffies;
+		if (ifp->ndev)
+			ifp->ndev->last_rx = jiffies;
 
 		drvr->dstats.rx_bytes += skb->len;
 		drvr->rx_packets++;	/* Local count */
 
-		if (in_interrupt()) {
+		if (in_interrupt())
 			netif_rx(skb);
-		} else {
+		else
 			/* If the receive is not processed inside an ISR,
 			 * the softirqd must be woken explicitly to service
 			 * the NET_RX_SOFTIRQ.  In 2.6 kernels, this is handled
@@ -781,7 +502,6 @@ void brcmf_rx_frame(struct brcmf_pub *drvr, int ifidx, struct sk_buff *skb,
 			 * to do it manually.
 			 */
 			netif_rx_ni(skb);
-		}
 	}
 }
 
@@ -802,24 +522,24 @@ void brcmf_txcomplete(struct brcmf_pub *drvr, struct sk_buff *txp, bool success)
 
 }
 
-static struct net_device_stats *brcmf_netdev_get_stats(struct net_device *net)
+static struct net_device_stats *brcmf_netdev_get_stats(struct net_device *ndev)
 {
-	struct brcmf_info *drvr_priv = *(struct brcmf_info **) netdev_priv(net);
+	struct brcmf_info *drvr_priv = *(struct brcmf_info **)
+					netdev_priv(ndev);
 	struct brcmf_if *ifp;
 	int ifidx;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
-	ifidx = brcmf_net2idx(drvr_priv, net);
+	ifidx = brcmf_net2idx(drvr_priv, ndev);
 	if (ifidx == BRCMF_BAD_IF)
 		return NULL;
 
 	ifp = drvr_priv->iflist[ifidx];
 
-	if (drvr_priv->pub.up) {
+	if (drvr_priv->pub.up)
 		/* Use the protocol to get dongle stats */
 		brcmf_proto_dstats(&drvr_priv->pub);
-	}
 
 	/* Copy dongle stats to net device stats */
 	ifp->stats.rx_packets = drvr_priv->pub.dstats.rx_packets;
@@ -839,33 +559,35 @@ static struct net_device_stats *brcmf_netdev_get_stats(struct net_device *net)
 	 as a bitmap in toe_ol iovar */
 static int brcmf_toe_get(struct brcmf_info *drvr_priv, int ifidx, u32 *toe_ol)
 {
-	struct brcmf_ioctl ioc;
+	struct brcmf_dcmd dcmd;
+	__le32 toe_le;
 	char buf[32];
 	int ret;
 
-	memset(&ioc, 0, sizeof(ioc));
+	memset(&dcmd, 0, sizeof(dcmd));
 
-	ioc.cmd = BRCMF_C_GET_VAR;
-	ioc.buf = buf;
-	ioc.len = (uint) sizeof(buf);
-	ioc.set = false;
+	dcmd.cmd = BRCMF_C_GET_VAR;
+	dcmd.buf = buf;
+	dcmd.len = (uint) sizeof(buf);
+	dcmd.set = false;
 
 	strcpy(buf, "toe_ol");
-	ret = brcmf_proto_ioctl(&drvr_priv->pub, ifidx, &ioc, ioc.buf, ioc.len);
+	ret = brcmf_proto_dcmd(&drvr_priv->pub, ifidx, &dcmd, dcmd.len);
 	if (ret < 0) {
 		/* Check for older dongle image that doesn't support toe_ol */
 		if (ret == -EIO) {
-			BRCMF_ERROR(("%s: toe not supported by device\n",
-				     brcmf_ifname(&drvr_priv->pub, ifidx)));
+			brcmf_dbg(ERROR, "%s: toe not supported by device\n",
+				  brcmf_ifname(&drvr_priv->pub, ifidx));
 			return -EOPNOTSUPP;
 		}
 
-		BRCMF_INFO(("%s: could not get toe_ol: ret=%d\n",
-			    brcmf_ifname(&drvr_priv->pub, ifidx), ret));
+		brcmf_dbg(INFO, "%s: could not get toe_ol: ret=%d\n",
+			  brcmf_ifname(&drvr_priv->pub, ifidx), ret);
 		return ret;
 	}
 
-	memcpy(toe_ol, buf, sizeof(u32));
+	memcpy(&toe_le, buf, sizeof(u32));
+	*toe_ol = le32_to_cpu(toe_le);
 	return 0;
 }
 
@@ -873,63 +595,63 @@ static int brcmf_toe_get(struct brcmf_info *drvr_priv, int ifidx, u32 *toe_ol)
 	 and set toe global enable iovar */
 static int brcmf_toe_set(struct brcmf_info *drvr_priv, int ifidx, u32 toe_ol)
 {
-	struct brcmf_ioctl ioc;
+	struct brcmf_dcmd dcmd;
 	char buf[32];
-	int toe, ret;
+	int ret;
+	__le32 toe_le = cpu_to_le32(toe_ol);
 
-	memset(&ioc, 0, sizeof(ioc));
+	memset(&dcmd, 0, sizeof(dcmd));
 
-	ioc.cmd = BRCMF_C_SET_VAR;
-	ioc.buf = buf;
-	ioc.len = (uint) sizeof(buf);
-	ioc.set = true;
+	dcmd.cmd = BRCMF_C_SET_VAR;
+	dcmd.buf = buf;
+	dcmd.len = (uint) sizeof(buf);
+	dcmd.set = true;
 
 	/* Set toe_ol as requested */
-
 	strcpy(buf, "toe_ol");
-	memcpy(&buf[sizeof("toe_ol")], &toe_ol, sizeof(u32));
+	memcpy(&buf[sizeof("toe_ol")], &toe_le, sizeof(u32));
 
-	ret = brcmf_proto_ioctl(&drvr_priv->pub, ifidx, &ioc, ioc.buf, ioc.len);
+	ret = brcmf_proto_dcmd(&drvr_priv->pub, ifidx, &dcmd, dcmd.len);
 	if (ret < 0) {
-		BRCMF_ERROR(("%s: could not set toe_ol: ret=%d\n",
-			     brcmf_ifname(&drvr_priv->pub, ifidx), ret));
+		brcmf_dbg(ERROR, "%s: could not set toe_ol: ret=%d\n",
+			  brcmf_ifname(&drvr_priv->pub, ifidx), ret);
 		return ret;
 	}
 
 	/* Enable toe globally only if any components are enabled. */
-
-	toe = (toe_ol != 0);
+	toe_le = cpu_to_le32(toe_ol != 0);
 
 	strcpy(buf, "toe");
-	memcpy(&buf[sizeof("toe")], &toe, sizeof(u32));
+	memcpy(&buf[sizeof("toe")], &toe_le, sizeof(u32));
 
-	ret = brcmf_proto_ioctl(&drvr_priv->pub, ifidx, &ioc, ioc.buf, ioc.len);
+	ret = brcmf_proto_dcmd(&drvr_priv->pub, ifidx, &dcmd, dcmd.len);
 	if (ret < 0) {
-		BRCMF_ERROR(("%s: could not set toe: ret=%d\n",
-			     brcmf_ifname(&drvr_priv->pub, ifidx), ret));
+		brcmf_dbg(ERROR, "%s: could not set toe: ret=%d\n",
+			  brcmf_ifname(&drvr_priv->pub, ifidx), ret);
 		return ret;
 	}
 
 	return 0;
 }
 
-static void brcmf_ethtool_get_drvinfo(struct net_device *net,
+static void brcmf_ethtool_get_drvinfo(struct net_device *ndev,
 				    struct ethtool_drvinfo *info)
 {
-	struct brcmf_info *drvr_priv = *(struct brcmf_info **) netdev_priv(net);
+	struct brcmf_info *drvr_priv = *(struct brcmf_info **)
+					netdev_priv(ndev);
 
 	sprintf(info->driver, KBUILD_MODNAME);
 	sprintf(info->version, "%lu", drvr_priv->pub.drv_version);
 	sprintf(info->fw_version, "%s", BCM4329_FW_NAME);
 	sprintf(info->bus_info, "%s",
-		dev_name(&brcmf_cfg80211_get_sdio_func()->dev));
+		dev_name(brcmf_bus_get_device(drvr_priv->pub.bus)));
 }
 
-struct ethtool_ops brcmf_ethtool_ops = {
+static struct ethtool_ops brcmf_ethtool_ops = {
 	.get_drvinfo = brcmf_ethtool_get_drvinfo
 };
 
-static int brcmf_ethtool(struct brcmf_info *drvr_priv, void *uaddr)
+static int brcmf_ethtool(struct brcmf_info *drvr_priv, void __user *uaddr)
 {
 	struct ethtool_drvinfo info;
 	char drvname[sizeof(info.driver)];
@@ -938,7 +660,7 @@ static int brcmf_ethtool(struct brcmf_info *drvr_priv, void *uaddr)
 	u32 toe_cmpnt, csum_dir;
 	int ret;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	/* all ethtool calls start with a cmd word */
 	if (copy_from_user(&cmd, uaddr, sizeof(u32)))
@@ -964,7 +686,7 @@ static int brcmf_ethtool(struct brcmf_info *drvr_priv, void *uaddr)
 
 		/* otherwise, require dongle to be up */
 		else if (!drvr_priv->pub.up) {
-			BRCMF_ERROR(("%s: dongle is not up\n", __func__));
+			brcmf_dbg(ERROR, "dongle is not up\n");
 			return -ENODEV;
 		}
 
@@ -977,8 +699,8 @@ static int brcmf_ethtool(struct brcmf_info *drvr_priv, void *uaddr)
 		sprintf(info.version, "%lu", drvr_priv->pub.drv_version);
 		if (copy_to_user(uaddr, &info, sizeof(info)))
 			return -EFAULT;
-		BRCMF_CTL(("%s: given %*s, returning %s\n", __func__,
-			   (int)sizeof(drvname), drvname, info.driver));
+		brcmf_dbg(CTL, "given %*s, returning %s\n",
+			  (int)sizeof(drvname), drvname, info.driver);
 		break;
 
 		/* Get toe offload components from dongle */
@@ -1024,10 +746,10 @@ static int brcmf_ethtool(struct brcmf_info *drvr_priv, void *uaddr)
 		/* If setting TX checksum mode, tell Linux the new mode */
 		if (cmd == ETHTOOL_STXCSUM) {
 			if (edata.data)
-				drvr_priv->iflist[0]->net->features |=
+				drvr_priv->iflist[0]->ndev->features |=
 				    NETIF_F_IP_CSUM;
 			else
-				drvr_priv->iflist[0]->net->features &=
+				drvr_priv->iflist[0]->ndev->features &=
 				    ~NETIF_F_IP_CSUM;
 		}
 
@@ -1040,180 +762,131 @@ static int brcmf_ethtool(struct brcmf_info *drvr_priv, void *uaddr)
 	return 0;
 }
 
-static int brcmf_netdev_ioctl_entry(struct net_device *net, struct ifreq *ifr,
+static int brcmf_netdev_ioctl_entry(struct net_device *ndev, struct ifreq *ifr,
 				    int cmd)
 {
-	struct brcmf_info *drvr_priv = *(struct brcmf_info **) netdev_priv(net);
-	struct brcmf_c_ioctl ioc;
-	int bcmerror = 0;
-	int buflen = 0;
-	void *buf = NULL;
-	uint driver = 0;
+	struct brcmf_info *drvr_priv = *(struct brcmf_info **)
+					netdev_priv(ndev);
 	int ifidx;
-	bool is_set_key_cmd;
 
-	ifidx = brcmf_net2idx(drvr_priv, net);
-	BRCMF_TRACE(("%s: ifidx %d, cmd 0x%04x\n", __func__, ifidx, cmd));
+	ifidx = brcmf_net2idx(drvr_priv, ndev);
+	brcmf_dbg(TRACE, "ifidx %d, cmd 0x%04x\n", ifidx, cmd);
 
 	if (ifidx == BRCMF_BAD_IF)
 		return -1;
 
 	if (cmd == SIOCETHTOOL)
-		return brcmf_ethtool(drvr_priv, (void *)ifr->ifr_data);
-
-	if (cmd != SIOCDEVPRIVATE)
-		return -EOPNOTSUPP;
-
-	memset(&ioc, 0, sizeof(ioc));
+		return brcmf_ethtool(drvr_priv, ifr->ifr_data);
 
-	/* Copy the ioc control structure part of ioctl request */
-	if (copy_from_user(&ioc, ifr->ifr_data, sizeof(struct brcmf_ioctl))) {
-		bcmerror = -EINVAL;
-		goto done;
-	}
+	return -EOPNOTSUPP;
+}
 
-	/* Copy out any buffer passed */
-	if (ioc.buf) {
-		buflen = min_t(int, ioc.len, BRCMF_IOCTL_MAXLEN);
-		/* optimization for direct ioctl calls from kernel */
-		/*
-		   if (segment_eq(get_fs(), KERNEL_DS)) {
-		   buf = ioc.buf;
-		   } else {
-		 */
-		{
-			buf = kmalloc(buflen, GFP_ATOMIC);
-			if (!buf) {
-				bcmerror = -ENOMEM;
-				goto done;
-			}
-			if (copy_from_user(buf, ioc.buf, buflen)) {
-				bcmerror = -EINVAL;
-				goto done;
-			}
-		}
-	}
+/* 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_info *drvr_priv = *(struct brcmf_info **)
+					netdev_priv(ndev);
+	int ifidx;
 
-	/* To differentiate read 4 more byes */
-	if ((copy_from_user(&driver, (char *)ifr->ifr_data +
-			    sizeof(struct brcmf_ioctl), sizeof(uint)) != 0)) {
-		bcmerror = -EINVAL;
-		goto done;
-	}
+	memset(&dcmd, 0, sizeof(dcmd));
+	dcmd.cmd = cmd;
+	dcmd.buf = arg;
+	dcmd.len = len;
 
-	if (!capable(CAP_NET_ADMIN)) {
-		bcmerror = -EPERM;
-		goto done;
-	}
+	ifidx = brcmf_net2idx(drvr_priv, ndev);
 
-	/* check for local brcmf ioctl and handle it */
-	if (driver == BRCMF_IOCTL_MAGIC) {
-		bcmerror = brcmf_c_ioctl((void *)&drvr_priv->pub, &ioc, buf, buflen);
-		if (bcmerror)
-			drvr_priv->pub.bcmerror = bcmerror;
-		goto done;
-	}
+	if (dcmd.buf != NULL)
+		buflen = min_t(uint, dcmd.len, BRCMF_DCMD_MAXLEN);
 
 	/* send to dongle (must be up, and wl) */
 	if ((drvr_priv->pub.busstate != BRCMF_BUS_DATA)) {
-		BRCMF_ERROR(("%s DONGLE_DOWN,__func__\n", __func__));
-		bcmerror = -EIO;
+		brcmf_dbg(ERROR, "DONGLE_DOWN\n");
+		err = -EIO;
 		goto done;
 	}
 
 	if (!drvr_priv->pub.iswl) {
-		bcmerror = -EIO;
+		err = -EIO;
 		goto done;
 	}
 
 	/*
-	 * Intercept BRCMF_C_SET_KEY IOCTL - serialize M4 send and
-	 * set key IOCTL to prevent M4 encryption.
+	 * Intercept BRCMF_C_SET_KEY CMD - serialize M4 send and
+	 * set key CMD to prevent M4 encryption.
 	 */
-	is_set_key_cmd = ((ioc.cmd == BRCMF_C_SET_KEY) ||
-			  ((ioc.cmd == BRCMF_C_SET_VAR) &&
-			   !(strncmp("wsec_key", ioc.buf, 9))) ||
-			  ((ioc.cmd == BRCMF_C_SET_VAR) &&
-			   !(strncmp("bsscfg:wsec_key", ioc.buf, 15))));
+	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(net);
+		brcmf_netdev_wait_pend8021x(ndev);
 
-	bcmerror =
-	    brcmf_proto_ioctl(&drvr_priv->pub, ifidx, (struct brcmf_ioctl *)&ioc,
-			      buf, buflen);
+	err = brcmf_proto_dcmd(&drvr_priv->pub, ifidx, &dcmd, buflen);
 
 done:
-	if (!bcmerror && buf && ioc.buf) {
-		if (copy_to_user(ioc.buf, buf, buflen))
-			bcmerror = -EFAULT;
-	}
-
-	kfree(buf);
-
-	if (bcmerror > 0)
-		bcmerror = 0;
+	if (err > 0)
+		err = 0;
 
-	return bcmerror;
+	return err;
 }
 
-static int brcmf_netdev_stop(struct net_device *net)
+static int brcmf_netdev_stop(struct net_device *ndev)
 {
-#if !defined(IGNORE_ETH0_DOWN)
-	struct brcmf_info *drvr_priv = *(struct brcmf_info **) netdev_priv(net);
+	struct brcmf_pub *drvr = *(struct brcmf_pub **) netdev_priv(ndev);
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-	brcmf_cfg80211_down();
-	if (drvr_priv->pub.up == 0)
+	brcmf_dbg(TRACE, "Enter\n");
+	brcmf_cfg80211_down(drvr->config);
+	if (drvr->up == 0)
 		return 0;
 
 	/* Set state and stop OS transmissions */
-	drvr_priv->pub.up = 0;
-	netif_stop_queue(net);
-#else
-	BRCMF_ERROR(("BYPASS %s:due to BRCM compilation: under investigation\n",
-		     __func__));
-#endif				/* !defined(IGNORE_ETH0_DOWN) */
+	drvr->up = 0;
+	netif_stop_queue(ndev);
 
 	return 0;
 }
 
-static int brcmf_netdev_open(struct net_device *net)
+static int brcmf_netdev_open(struct net_device *ndev)
 {
-	struct brcmf_info *drvr_priv = *(struct brcmf_info **) netdev_priv(net);
+	struct brcmf_info *drvr_priv = *(struct brcmf_info **)
+					netdev_priv(ndev);
 	u32 toe_ol;
-	int ifidx = brcmf_net2idx(drvr_priv, net);
+	int ifidx = brcmf_net2idx(drvr_priv, ndev);
 	s32 ret = 0;
 
-	BRCMF_TRACE(("%s: ifidx %d\n", __func__, ifidx));
+	brcmf_dbg(TRACE, "ifidx %d\n", ifidx);
 
 	if (ifidx == 0) {	/* do it only for primary eth0 */
 
 		/* try to bring up bus */
 		ret = brcmf_bus_start(&drvr_priv->pub);
 		if (ret != 0) {
-			BRCMF_ERROR(("%s: failed with code %d\n",
-				     __func__, ret));
+			brcmf_dbg(ERROR, "failed with code %d\n", ret);
 			return -1;
 		}
 		atomic_set(&drvr_priv->pend_8021x_cnt, 0);
 
-		memcpy(net->dev_addr, drvr_priv->pub.mac, ETH_ALEN);
+		memcpy(ndev->dev_addr, drvr_priv->pub.mac, ETH_ALEN);
 
 		/* Get current TOE mode from dongle */
 		if (brcmf_toe_get(drvr_priv, ifidx, &toe_ol) >= 0
 		    && (toe_ol & TOE_TX_CSUM_OL) != 0)
-			drvr_priv->iflist[ifidx]->net->features |=
+			drvr_priv->iflist[ifidx]->ndev->features |=
 				NETIF_F_IP_CSUM;
 		else
-			drvr_priv->iflist[ifidx]->net->features &=
+			drvr_priv->iflist[ifidx]->ndev->features &=
 				~NETIF_F_IP_CSUM;
 	}
 	/* Allow transmit calls */
-	netif_start_queue(net);
+	netif_start_queue(ndev);
 	drvr_priv->pub.up = 1;
-	if (unlikely(brcmf_cfg80211_up())) {
-		BRCMF_ERROR(("%s: failed to bring up cfg80211\n",
-			     __func__));
+	if (brcmf_cfg80211_up(drvr_priv->pub.config)) {
+		brcmf_dbg(ERROR, "failed to bring up cfg80211\n");
 		return -1;
 	}
 
@@ -1221,35 +894,74 @@ static int brcmf_netdev_open(struct net_device *net)
 }
 
 int
-brcmf_add_if(struct brcmf_info *drvr_priv, int ifidx, void *handle, char *name,
-	   u8 *mac_addr, u32 flags, u8 bssidx)
+brcmf_add_if(struct brcmf_info *drvr_priv, int ifidx, struct net_device *ndev,
+	     char *name, u8 *mac_addr, u32 flags, u8 bssidx)
 {
 	struct brcmf_if *ifp;
+	int ret = 0, err = 0;
 
-	BRCMF_TRACE(("%s: idx %d, handle->%p\n", __func__, ifidx, handle));
+	brcmf_dbg(TRACE, "idx %d, handle->%p\n", ifidx, ndev);
 
 	ifp = drvr_priv->iflist[ifidx];
 	if (!ifp) {
 		ifp = kmalloc(sizeof(struct brcmf_if), GFP_ATOMIC);
-		if (!ifp) {
-			BRCMF_ERROR(("%s: OOM - struct brcmf_if\n", __func__));
+		if (!ifp)
 			return -ENOMEM;
-		}
 	}
 
 	memset(ifp, 0, sizeof(struct brcmf_if));
 	ifp->info = drvr_priv;
 	drvr_priv->iflist[ifidx] = ifp;
-	strlcpy(ifp->name, name, IFNAMSIZ);
 	if (mac_addr != NULL)
 		memcpy(&ifp->mac_addr, mac_addr, ETH_ALEN);
 
-	if (handle == NULL) {
+	if (ndev == NULL) {
 		ifp->state = BRCMF_E_IF_ADD;
 		ifp->idx = ifidx;
-		up(&drvr_priv->sysioc_sem);
+		/*
+		 * Delete the existing interface before overwriting it
+		 * in case we missed the BRCMF_E_IF_DEL event.
+		 */
+		if (ifp->ndev != NULL) {
+			brcmf_dbg(ERROR, "ERROR: netdev:%s already exists, try free & unregister\n",
+				  ifp->ndev->name);
+			netif_stop_queue(ifp->ndev);
+			unregister_netdev(ifp->ndev);
+			free_netdev(ifp->ndev);
+		}
+
+		/* Allocate netdev, including space for private structure */
+		ifp->ndev = alloc_netdev(sizeof(drvr_priv), "wlan%d",
+					 ether_setup);
+		if (!ifp->ndev) {
+			brcmf_dbg(ERROR, "OOM - alloc_netdev\n");
+			ret = -ENOMEM;
+		}
+
+		if (ret == 0) {
+			memcpy(netdev_priv(ifp->ndev), &drvr_priv,
+			       sizeof(drvr_priv));
+			err = brcmf_net_attach(&drvr_priv->pub, ifp->idx);
+			if (err != 0) {
+				brcmf_dbg(ERROR, "brcmf_net_attach failed, err %d\n",
+					  err);
+				ret = -EOPNOTSUPP;
+			} else {
+				brcmf_dbg(TRACE, " ==== pid:%x, net_device for if:%s created ===\n",
+					  current->pid, ifp->ndev->name);
+				ifp->state = 0;
+			}
+		}
+
+		if (ret < 0) {
+			if (ifp->ndev)
+				free_netdev(ifp->ndev);
+
+			drvr_priv->iflist[ifp->idx] = NULL;
+			kfree(ifp);
+		}
 	} else
-		ifp->net = (struct net_device *)handle;
+		ifp->ndev = ndev;
 
 	return 0;
 }
@@ -1258,65 +970,55 @@ void brcmf_del_if(struct brcmf_info *drvr_priv, int ifidx)
 {
 	struct brcmf_if *ifp;
 
-	BRCMF_TRACE(("%s: idx %d\n", __func__, ifidx));
+	brcmf_dbg(TRACE, "idx %d\n", ifidx);
 
 	ifp = drvr_priv->iflist[ifidx];
 	if (!ifp) {
-		BRCMF_ERROR(("%s: Null interface\n", __func__));
+		brcmf_dbg(ERROR, "Null interface\n");
 		return;
 	}
 
 	ifp->state = BRCMF_E_IF_DEL;
 	ifp->idx = ifidx;
-	up(&drvr_priv->sysioc_sem);
+	if (ifp->ndev != NULL) {
+		netif_stop_queue(ifp->ndev);
+		unregister_netdev(ifp->ndev);
+		free_netdev(ifp->ndev);
+		drvr_priv->iflist[ifidx] = NULL;
+		kfree(ifp);
+	}
 }
 
 struct brcmf_pub *brcmf_attach(struct brcmf_bus *bus, uint bus_hdrlen)
 {
 	struct brcmf_info *drvr_priv = NULL;
-	struct net_device *net;
+	struct net_device *ndev;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
-	/* Allocate etherdev, including space for private structure */
-	net = alloc_etherdev(sizeof(drvr_priv));
-	if (!net) {
-		BRCMF_ERROR(("%s: OOM - alloc_etherdev\n", __func__));
+	/* Allocate netdev, including space for private structure */
+	ndev = alloc_netdev(sizeof(drvr_priv), "wlan%d", ether_setup);
+	if (!ndev) {
+		brcmf_dbg(ERROR, "OOM - alloc_netdev\n");
 		goto fail;
 	}
 
 	/* Allocate primary brcmf_info */
 	drvr_priv = kzalloc(sizeof(struct brcmf_info), GFP_ATOMIC);
-	if (!drvr_priv) {
-		BRCMF_ERROR(("%s: OOM - alloc brcmf_info\n", __func__));
+	if (!drvr_priv)
 		goto fail;
-	}
 
 	/*
 	 * Save the brcmf_info into the priv
 	 */
-	memcpy(netdev_priv(net), &drvr_priv, sizeof(drvr_priv));
-
-	/* Set network interface name if it was provided as module parameter */
-	if (iface_name[0]) {
-		int len;
-		char ch;
-		strncpy(net->name, iface_name, IFNAMSIZ);
-		net->name[IFNAMSIZ - 1] = 0;
-		len = strlen(net->name);
-		ch = net->name[len - 1];
-		if ((ch > '9' || ch < '0') && (len < IFNAMSIZ - 2))
-			strcat(net->name, "%d");
-	}
+	memcpy(netdev_priv(ndev), &drvr_priv, sizeof(drvr_priv));
 
-	if (brcmf_add_if(drvr_priv, 0, (void *)net, net->name, NULL, 0, 0) ==
+	if (brcmf_add_if(drvr_priv, 0, ndev, ndev->name, NULL, 0, 0) ==
 	    BRCMF_BAD_IF)
 		goto fail;
 
-	net->netdev_ops = NULL;
-	sema_init(&drvr_priv->proto_sem, 1);
-	/* Initialize other structure content */
-	init_waitqueue_head(&drvr_priv->ioctl_resp_wait);
+	ndev->netdev_ops = NULL;
+	mutex_init(&drvr_priv->proto_block);
 
 	/* Link to info module */
 	drvr_priv->pub.info = drvr_priv;
@@ -1327,41 +1029,33 @@ struct brcmf_pub *brcmf_attach(struct brcmf_bus *bus, uint bus_hdrlen)
 
 	/* Attach and link in the protocol */
 	if (brcmf_proto_attach(&drvr_priv->pub) != 0) {
-		BRCMF_ERROR(("brcmf_prot_attach failed\n"));
+		brcmf_dbg(ERROR, "brcmf_prot_attach failed\n");
 		goto fail;
 	}
 
 	/* Attach and link in the cfg80211 */
-	if (unlikely(brcmf_cfg80211_attach(net, &drvr_priv->pub))) {
-		BRCMF_ERROR(("wl_cfg80211_attach failed\n"));
+	drvr_priv->pub.config =
+			brcmf_cfg80211_attach(ndev,
+					      brcmf_bus_get_device(bus),
+					      &drvr_priv->pub);
+	if (drvr_priv->pub.config == NULL) {
+		brcmf_dbg(ERROR, "wl_cfg80211_attach failed\n");
 		goto fail;
 	}
 
-	if (brcmf_sysioc) {
-		sema_init(&drvr_priv->sysioc_sem, 0);
-		drvr_priv->sysioc_tsk = kthread_run(_brcmf_sysioc_thread, drvr_priv,
-						"_brcmf_sysioc");
-		if (IS_ERR(drvr_priv->sysioc_tsk)) {
-			printk(KERN_WARNING
-				"_brcmf_sysioc thread failed to start\n");
-			drvr_priv->sysioc_tsk = NULL;
-		}
-	} else
-		drvr_priv->sysioc_tsk = NULL;
+	INIT_WORK(&drvr_priv->setmacaddr_work, _brcmf_set_mac_address);
+	INIT_WORK(&drvr_priv->multicast_work, _brcmf_set_multicast_list);
 
 	/*
 	 * Save the brcmf_info into the priv
 	 */
-	memcpy(netdev_priv(net), &drvr_priv, sizeof(drvr_priv));
+	memcpy(netdev_priv(ndev), &drvr_priv, sizeof(drvr_priv));
 
-#if defined(CONFIG_PM_SLEEP)
-	atomic_set(&brcmf_mmc_suspend, false);
-#endif	/* defined(CONFIG_PM_SLEEP) */
 	return &drvr_priv->pub;
 
 fail:
-	if (net)
-		free_netdev(net);
+	if (ndev)
+		free_netdev(ndev);
 	if (drvr_priv)
 		brcmf_detach(&drvr_priv->pub);
 
@@ -1375,25 +1069,24 @@ int brcmf_bus_start(struct brcmf_pub *drvr)
 	/* Room for "event_msgs" + '\0' + bitvec */
 	char iovbuf[BRCMF_EVENTING_MASK_LEN + 12];
 
-	BRCMF_TRACE(("%s:\n", __func__));
+	brcmf_dbg(TRACE, "\n");
 
 	/* Bring up the bus */
-	ret = brcmf_sdbrcm_bus_init(&drvr_priv->pub, true);
+	ret = brcmf_sdbrcm_bus_init(&drvr_priv->pub);
 	if (ret != 0) {
-		BRCMF_ERROR(("%s, brcmf_sdbrcm_bus_init failed %d\n", __func__,
-			     ret));
+		brcmf_dbg(ERROR, "brcmf_sdbrcm_bus_init failed %d\n", ret);
 		return ret;
 	}
 
 	/* If bus is not ready, can't come up */
 	if (drvr_priv->pub.busstate != BRCMF_BUS_DATA) {
-		BRCMF_ERROR(("%s failed bus is not ready\n", __func__));
+		brcmf_dbg(ERROR, "failed bus is not ready\n");
 		return -ENODEV;
 	}
 
-	brcmu_mkiovar("event_msgs", drvr->eventmask, BRCMF_EVENTING_MASK_LEN,
+	brcmf_c_mkiovar("event_msgs", drvr->eventmask, BRCMF_EVENTING_MASK_LEN,
 		      iovbuf, sizeof(iovbuf));
-	brcmf_proto_cdc_query_ioctl(drvr, 0, BRCMF_C_GET_VAR, iovbuf,
+	brcmf_proto_cdc_query_dcmd(drvr, 0, BRCMF_C_GET_VAR, iovbuf,
 				    sizeof(iovbuf));
 	memcpy(drvr->eventmask, iovbuf, BRCMF_EVENTING_MASK_LEN);
 
@@ -1437,20 +1130,20 @@ static struct net_device_ops brcmf_netdev_ops_pri = {
 	.ndo_do_ioctl = brcmf_netdev_ioctl_entry,
 	.ndo_start_xmit = brcmf_netdev_start_xmit,
 	.ndo_set_mac_address = brcmf_netdev_set_mac_address,
-	.ndo_set_multicast_list = brcmf_netdev_set_multicast_list
+	.ndo_set_rx_mode = brcmf_netdev_set_multicast_list
 };
 
 int brcmf_net_attach(struct brcmf_pub *drvr, int ifidx)
 {
 	struct brcmf_info *drvr_priv = drvr->info;
-	struct net_device *net;
+	struct net_device *ndev;
 	u8 temp_addr[ETH_ALEN] = {
 		0x00, 0x90, 0x4c, 0x11, 0x22, 0x33};
 
-	BRCMF_TRACE(("%s: ifidx %d\n", __func__, ifidx));
+	brcmf_dbg(TRACE, "ifidx %d\n", ifidx);
 
-	net = drvr_priv->iflist[ifidx]->net;
-	net->netdev_ops = &brcmf_netdev_ops_pri;
+	ndev = drvr_priv->iflist[ifidx]->ndev;
+	ndev->netdev_ops = &brcmf_netdev_ops_pri;
 
 	/*
 	 * We have to use the primary MAC for virtual interfaces
@@ -1462,32 +1155,31 @@ int brcmf_net_attach(struct brcmf_pub *drvr, int ifidx)
 	}
 
 	if (ifidx == 1) {
-		BRCMF_TRACE(("%s ACCESS POINT MAC:\n", __func__));
+		brcmf_dbg(TRACE, "ACCESS POINT MAC:\n");
 		/*  ACCESSPOINT INTERFACE CASE */
 		temp_addr[0] |= 0X02;	/* set bit 2 ,
 			 - Locally Administered address  */
 
 	}
-	net->hard_header_len = ETH_HLEN + drvr_priv->pub.hdrlen;
-	net->ethtool_ops = &brcmf_ethtool_ops;
+	ndev->hard_header_len = ETH_HLEN + drvr_priv->pub.hdrlen;
+	ndev->ethtool_ops = &brcmf_ethtool_ops;
 
-	drvr_priv->pub.rxsz = net->mtu + net->hard_header_len +
-				drvr_priv->pub.hdrlen;
+	drvr_priv->pub.rxsz = ndev->mtu + ndev->hard_header_len +
+			      drvr_priv->pub.hdrlen;
 
-	memcpy(net->dev_addr, temp_addr, ETH_ALEN);
+	memcpy(ndev->dev_addr, temp_addr, ETH_ALEN);
 
-	if (register_netdev(net) != 0) {
-		BRCMF_ERROR(("%s: couldn't register the net device\n",
-			     __func__));
+	if (register_netdev(ndev) != 0) {
+		brcmf_dbg(ERROR, "couldn't register the net device\n");
 		goto fail;
 	}
 
-	BRCMF_INFO(("%s: Broadcom Dongle Host Driver\n", net->name));
+	brcmf_dbg(INFO, "%s: Broadcom Dongle Host Driver\n", ndev->name);
 
 	return 0;
 
 fail:
-	net->netdev_ops = NULL;
+	ndev->netdev_ops = NULL;
 	return -EBADE;
 }
 
@@ -1495,7 +1187,7 @@ static void brcmf_bus_detach(struct brcmf_pub *drvr)
 {
 	struct brcmf_info *drvr_priv;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	if (drvr) {
 		drvr_priv = drvr->info;
@@ -1504,7 +1196,7 @@ static void brcmf_bus_detach(struct brcmf_pub *drvr)
 			brcmf_proto_stop(&drvr_priv->pub);
 
 			/* Stop the bus module */
-			brcmf_sdbrcm_bus_stop(drvr_priv->pub.bus, true);
+			brcmf_sdbrcm_bus_stop(drvr_priv->pub.bus);
 		}
 	}
 }
@@ -1513,7 +1205,7 @@ void brcmf_detach(struct brcmf_pub *drvr)
 {
 	struct brcmf_info *drvr_priv;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	if (drvr) {
 		drvr_priv = drvr->info;
@@ -1526,25 +1218,24 @@ void brcmf_detach(struct brcmf_pub *drvr)
 					brcmf_del_if(drvr_priv, i);
 
 			ifp = drvr_priv->iflist[0];
-			if (ifp->net->netdev_ops == &brcmf_netdev_ops_pri) {
-				brcmf_netdev_stop(ifp->net);
-				unregister_netdev(ifp->net);
+			if (ifp->ndev->netdev_ops == &brcmf_netdev_ops_pri) {
+				rtnl_lock();
+				brcmf_netdev_stop(ifp->ndev);
+				rtnl_unlock();
+				unregister_netdev(ifp->ndev);
 			}
 
-			if (drvr_priv->sysioc_tsk) {
-				send_sig(SIGTERM, drvr_priv->sysioc_tsk, 1);
-				kthread_stop(drvr_priv->sysioc_tsk);
-				drvr_priv->sysioc_tsk = NULL;
-			}
+			cancel_work_sync(&drvr_priv->setmacaddr_work);
+			cancel_work_sync(&drvr_priv->multicast_work);
 
 			brcmf_bus_detach(drvr);
 
 			if (drvr->prot)
 				brcmf_proto_detach(drvr);
 
-			brcmf_cfg80211_detach();
+			brcmf_cfg80211_detach(drvr->config);
 
-			free_netdev(ifp->net);
+			free_netdev(ifp->ndev);
 			kfree(ifp);
 			kfree(drvr_priv);
 		}
@@ -1553,7 +1244,7 @@ void brcmf_detach(struct brcmf_pub *drvr)
 
 static void __exit brcmf_module_cleanup(void)
 {
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	brcmf_bus_unregister();
 }
@@ -1562,12 +1253,12 @@ static int __init brcmf_module_init(void)
 {
 	int error;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	error = brcmf_bus_register();
 
 	if (error) {
-		BRCMF_ERROR(("%s: brcmf_bus_register failed\n", __func__));
+		brcmf_dbg(ERROR, "brcmf_bus_register failed\n");
 		goto failed;
 	}
 	return 0;
@@ -1584,7 +1275,7 @@ int brcmf_os_proto_block(struct brcmf_pub *drvr)
 	struct brcmf_info *drvr_priv = drvr->info;
 
 	if (drvr_priv) {
-		down(&drvr_priv->proto_sem);
+		mutex_lock(&drvr_priv->proto_block);
 		return 1;
 	}
 	return 0;
@@ -1595,84 +1286,13 @@ int brcmf_os_proto_unblock(struct brcmf_pub *drvr)
 	struct brcmf_info *drvr_priv = drvr->info;
 
 	if (drvr_priv) {
-		up(&drvr_priv->proto_sem);
+		mutex_unlock(&drvr_priv->proto_block);
 		return 1;
 	}
 
 	return 0;
 }
 
-unsigned int brcmf_os_get_ioctl_resp_timeout(void)
-{
-	return (unsigned int)brcmf_ioctl_timeout_msec;
-}
-
-void brcmf_os_set_ioctl_resp_timeout(unsigned int timeout_msec)
-{
-	brcmf_ioctl_timeout_msec = (int)timeout_msec;
-}
-
-int brcmf_os_ioctl_resp_wait(struct brcmf_pub *drvr, uint *condition,
-			     bool *pending)
-{
-	struct brcmf_info *drvr_priv = drvr->info;
-	DECLARE_WAITQUEUE(wait, current);
-	int timeout = brcmf_ioctl_timeout_msec;
-
-	/* Convert timeout in millsecond to jiffies */
-	timeout = timeout * HZ / 1000;
-
-	/* Wait until control frame is available */
-	add_wait_queue(&drvr_priv->ioctl_resp_wait, &wait);
-	set_current_state(TASK_INTERRUPTIBLE);
-
-	while (!(*condition) && (!signal_pending(current) && timeout))
-		timeout = schedule_timeout(timeout);
-
-	if (signal_pending(current))
-		*pending = true;
-
-	set_current_state(TASK_RUNNING);
-	remove_wait_queue(&drvr_priv->ioctl_resp_wait, &wait);
-
-	return timeout;
-}
-
-int brcmf_os_ioctl_resp_wake(struct brcmf_pub *drvr)
-{
-	struct brcmf_info *drvr_priv = drvr->info;
-
-	if (waitqueue_active(&drvr_priv->ioctl_resp_wait))
-		wake_up_interruptible(&drvr_priv->ioctl_resp_wait);
-
-	return 0;
-}
-
-static int brcmf_host_event(struct brcmf_info *drvr_priv, int *ifidx, void *pktdata,
-			    struct brcmf_event_msg *event, void **data)
-{
-	int bcmerror = 0;
-
-	bcmerror = brcmf_c_host_event(drvr_priv, ifidx, pktdata, event, data);
-	if (bcmerror != 0)
-		return bcmerror;
-
-	if (drvr_priv->iflist[*ifidx]->net)
-		brcmf_cfg80211_event(drvr_priv->iflist[*ifidx]->net,
-				     event, *data);
-
-	return bcmerror;
-}
-
-int brcmf_netdev_reset(struct net_device *dev, u8 flag)
-{
-	struct brcmf_info *drvr_priv = *(struct brcmf_info **)netdev_priv(dev);
-
-	brcmf_bus_devreset(&drvr_priv->pub, flag);
-
-	return 1;
-}
-
 static int brcmf_get_pend_8021x_cnt(struct brcmf_info *drvr_priv)
 {
 	return atomic_read(&drvr_priv->pend_8021x_cnt);
@@ -1680,9 +1300,9 @@ static int brcmf_get_pend_8021x_cnt(struct brcmf_info *drvr_priv)
 
 #define MAX_WAIT_FOR_8021X_TX	10
 
-int brcmf_netdev_wait_pend8021x(struct net_device *dev)
+int brcmf_netdev_wait_pend8021x(struct net_device *ndev)
 {
-	struct brcmf_info *drvr_priv = *(struct brcmf_info **)netdev_priv(dev);
+	struct brcmf_info *drvr_priv = *(struct brcmf_info **)netdev_priv(ndev);
 	int timeout = 10 * HZ / 1000;
 	int ntimes = MAX_WAIT_FOR_8021X_TX;
 	int pend = brcmf_get_pend_8021x_cnt(drvr_priv);
@@ -1700,7 +1320,7 @@ int brcmf_netdev_wait_pend8021x(struct net_device *dev)
 }
 
 #ifdef BCMDBG
-int brcmf_write_to_file(struct brcmf_pub *drvr, u8 *buf, int size)
+int brcmf_write_to_file(struct brcmf_pub *drvr, const u8 *buf, int size)
 {
 	int ret = 0;
 	struct file *fp;
@@ -1714,13 +1334,13 @@ int brcmf_write_to_file(struct brcmf_pub *drvr, u8 *buf, int size)
 	/* open file to write */
 	fp = filp_open("/tmp/mem_dump", O_WRONLY | O_CREAT, 0640);
 	if (!fp) {
-		BRCMF_ERROR(("%s: open file error\n", __func__));
+		brcmf_dbg(ERROR, "open file error\n");
 		ret = -1;
 		goto exit;
 	}
 
 	/* Write buf to file */
-	fp->f_op->write(fp, buf, size, &pos);
+	fp->f_op->write(fp, (char __user *)buf, size, &pos);
 
 exit:
 	/* free buf before return */
diff --git a/drivers/staging/brcm80211/brcmfmac/dhd_proto.h b/drivers/staging/brcm80211/brcmfmac/dhd_proto.h
index ff788b37afdc..4ee1ea846f6d 100644
--- a/drivers/staging/brcm80211/brcmfmac/dhd_proto.h
+++ b/drivers/staging/brcm80211/brcmfmac/dhd_proto.h
@@ -17,14 +17,6 @@
 #ifndef _BRCMF_PROTO_H_
 #define _BRCMF_PROTO_H_
 
-#ifndef IOCTL_RESP_TIMEOUT
-#define IOCTL_RESP_TIMEOUT  2000	/* In milli second */
-#endif
-
-#ifndef IOCTL_CHIP_ACTIVE_TIMEOUT
-#define IOCTL_CHIP_ACTIVE_TIMEOUT  10	/* In milli second */
-#endif
-
 /*
  * Exported from the brcmf protocol module (brcmf_cdc)
  */
@@ -53,23 +45,16 @@ extern void brcmf_proto_hdrpush(struct brcmf_pub *, int ifidx,
 extern int brcmf_proto_hdrpull(struct brcmf_pub *, int *ifidx,
 			       struct sk_buff *rxp);
 
-/* Use protocol to issue ioctl to dongle */
-extern int brcmf_proto_ioctl(struct brcmf_pub *drvr, int ifidx,
-			     struct brcmf_ioctl *ioc, void *buf, int len);
-
-/* Add prot dump output to a buffer */
-extern void brcmf_proto_dump(struct brcmf_pub *drvr,
-			     struct brcmu_strbuf *strbuf);
+/* Use protocol to issue command to dongle */
+extern int brcmf_proto_dcmd(struct brcmf_pub *drvr, int ifidx,
+				struct brcmf_dcmd *dcmd, int len);
 
 /* Update local copy of dongle statistics */
 extern void brcmf_proto_dstats(struct brcmf_pub *drvr);
 
-extern int brcmf_c_ioctl(struct brcmf_pub *drvr, struct brcmf_c_ioctl *ioc,
-			 void *buf, uint buflen);
-
-extern int brcmf_c_preinit_ioctls(struct brcmf_pub *drvr);
+extern int brcmf_c_preinit_dcmds(struct brcmf_pub *drvr);
 
-extern int brcmf_proto_cdc_set_ioctl(struct brcmf_pub *drvr, int ifidx,
+extern int brcmf_proto_cdc_set_dcmd(struct brcmf_pub *drvr, int ifidx,
 				     uint cmd, void *buf, uint len);
 
 #endif				/* _BRCMF_PROTO_H_ */
diff --git a/drivers/staging/brcm80211/brcmfmac/dhd_sdio.c b/drivers/staging/brcm80211/brcmfmac/dhd_sdio.c
index 7fa95b6213c5..b716e3124330 100644
--- a/drivers/staging/brcm80211/brcmfmac/dhd_sdio.c
+++ b/drivers/staging/brcm80211/brcmfmac/dhd_sdio.c
@@ -24,8 +24,10 @@
 #include <linux/sched.h>
 #include <linux/mmc/sdio.h>
 #include <linux/mmc/sdio_func.h>
+#include <linux/mmc/card.h>
 #include <linux/semaphore.h>
 #include <linux/firmware.h>
+#include <linux/module.h>
 #include <asm/unaligned.h>
 #include <defs.h>
 #include <brcmu_wifi.h>
@@ -34,85 +36,18 @@
 #include <soc.h>
 #include "sdio_host.h"
 
-/* register access macros */
-#ifndef __BIG_ENDIAN
-#ifndef __mips__
-#define R_REG(r, typ) \
-	brcmf_sdcard_reg_read(NULL, (r), sizeof(typ))
-#else				/* __mips__ */
-#define R_REG(r, typ) \
-	({ \
-		__typeof(*(r)) __osl_v; \
-		__asm__ __volatile__("sync"); \
-		__osl_v = brcmf_sdcard_reg_read(NULL, (r),\
-					  sizeof(typ)); \
-		__asm__ __volatile__("sync"); \
-		__osl_v; \
-	})
-#endif				/* __mips__ */
-
-#else				/* __BIG_ENDIAN */
-#define R_REG(r, typ) \
-	brcmf_sdcard_reg_read(NULL, (r), sizeof(typ))
-#endif				/* __BIG_ENDIAN */
-
-#define OR_REG(r, v, typ) \
-	brcmf_sdcard_reg_write(NULL, (r), sizeof(typ), R_REG(r, typ) | (v))
+#define DCMD_RESP_TIMEOUT  2000	/* In milli second */
 
 #ifdef BCMDBG
 
-/* ARM trap handling */
-
-/* Trap types defined by ARM (see arminc.h) */
-
-#if defined(__ARM_ARCH_4T__)
-#define	MAX_TRAP_TYPE	(TR_FIQ + 1)
-#elif defined(__ARM_ARCH_7M__)
-#define	MAX_TRAP_TYPE	(TR_ISR + ARMCM3_NUMINTS)
-#endif				/* __ARM_ARCH_7M__ */
-
-/* The trap structure is defined here as offsets for assembly */
-#define	TR_TYPE		0x00
-#define	TR_EPC		0x04
-#define	TR_CPSR		0x08
-#define	TR_SPSR		0x0c
-#define	TR_REGS		0x10
-#define	TR_REG(n)	(TR_REGS + (n) * 4)
-#define	TR_SP		TR_REG(13)
-#define	TR_LR		TR_REG(14)
-#define	TR_PC		TR_REG(15)
-
-#define	TRAP_T_SIZE	80
-
-struct brcmf_trap {
-	u32 type;
-	u32 epc;
-	u32 cpsr;
-	u32 spsr;
-	u32 r0;
-	u32 r1;
-	u32 r2;
-	u32 r3;
-	u32 r4;
-	u32 r5;
-	u32 r6;
-	u32 r7;
-	u32 r8;
-	u32 r9;
-	u32 r10;
-	u32 r11;
-	u32 r12;
-	u32 r13;
-	u32 r14;
-	u32 pc;
-};
+#define BRCMF_TRAP_INFO_SIZE	80
 
 #define CBUF_LEN	(128)
 
-struct rte_log {
-	u32 buf;		/* Can't be pointer on (64-bit) hosts */
-	uint buf_size;
-	uint idx;
+struct rte_log_le {
+	__le32 buf;		/* Can't be pointer on (64-bit) hosts */
+	__le32 buf_size;
+	__le32 idx;
 	char *_buf_compat;	/* Redundant pointer for backward compat. */
 };
 
@@ -126,8 +61,8 @@ struct rte_console {
 	 * (at risk of conflicting with
 	 * the real UART).  vcons_out is currently unused.
 	 */
-	volatile uint vcons_in;
-	volatile uint vcons_out;
+	uint vcons_in;
+	uint vcons_out;
 
 	/* Output (logging) buffer
 	 * Console output is written to a ring buffer log_buf at index log_idx.
@@ -135,7 +70,7 @@ struct rte_console {
 	 * Output will be lost if the output wraps around faster than the host
 	 * polls.
 	 */
-	struct rte_log log;
+	struct rte_log_le log_le;
 
 	/* Console input line buffer
 	 * Characters are read one at a time into cbuf
@@ -176,57 +111,67 @@ struct rte_console {
 #define MAX_DATA_BUF	(32 * 1024)	/* Must be large enough to hold
 				 biggest possible glom */
 
-#ifndef BRCMF_FIRSTREAD
-#define BRCMF_FIRSTREAD	32
-#endif
+#define BRCMF_FIRSTREAD	(1 << 6)
 
-#if !ISPOWEROF2(BRCMF_FIRSTREAD)
-#error BRCMF_FIRSTREAD is not a power of 2!
-#endif
 
 /* SBSDIO_DEVICE_CTL */
-#define SBSDIO_DEVCTL_SETBUSY		0x01	/* 1: device will assert busy signal when
-						 * receiving CMD53
-						 */
-#define SBSDIO_DEVCTL_SPI_INTR_SYNC	0x02	/* 1: assertion of sdio interrupt is
-						 * synchronous to the sdio clock
-						 */
-#define SBSDIO_DEVCTL_CA_INT_ONLY	0x04	/* 1: mask all interrupts to host
-						 * except the chipActive (rev 8)
-						 */
-#define SBSDIO_DEVCTL_PADS_ISO		0x08	/* 1: isolate internal sdio signals, put
-						 * external pads in tri-state; requires
-						 * sdio bus power cycle to clear (rev 9)
-						 */
-#define SBSDIO_DEVCTL_SB_RST_CTL	0x30	/* Force SD->SB reset mapping (rev 11) */
-#define SBSDIO_DEVCTL_RST_CORECTL	0x00	/*   Determined by CoreControl bit */
-#define SBSDIO_DEVCTL_RST_BPRESET	0x10	/*   Force backplane reset */
-#define SBSDIO_DEVCTL_RST_NOBPRESET	0x20	/*   Force no backplane reset */
+
+/* 1: device will assert busy signal when receiving CMD53 */
+#define SBSDIO_DEVCTL_SETBUSY		0x01
+/* 1: assertion of sdio interrupt is synchronous to the sdio clock */
+#define SBSDIO_DEVCTL_SPI_INTR_SYNC	0x02
+/* 1: mask all interrupts to host except the chipActive (rev 8) */
+#define SBSDIO_DEVCTL_CA_INT_ONLY	0x04
+/* 1: isolate internal sdio signals, put external pads in tri-state; requires
+ * sdio bus power cycle to clear (rev 9) */
+#define SBSDIO_DEVCTL_PADS_ISO		0x08
+/* Force SD->SB reset mapping (rev 11) */
+#define SBSDIO_DEVCTL_SB_RST_CTL	0x30
+/*   Determined by CoreControl bit */
+#define SBSDIO_DEVCTL_RST_CORECTL	0x00
+/*   Force backplane reset */
+#define SBSDIO_DEVCTL_RST_BPRESET	0x10
+/*   Force no backplane reset */
+#define SBSDIO_DEVCTL_RST_NOBPRESET	0x20
 
 /* SBSDIO_FUNC1_CHIPCLKCSR */
-#define SBSDIO_FORCE_ALP		0x01	/* Force ALP request to backplane */
-#define SBSDIO_FORCE_HT			0x02	/* Force HT request to backplane */
-#define SBSDIO_FORCE_ILP		0x04	/* Force ILP request to backplane */
-#define SBSDIO_ALP_AVAIL_REQ		0x08	/* Make ALP ready (power up xtal) */
-#define SBSDIO_HT_AVAIL_REQ		0x10	/* Make HT ready (power up PLL) */
-#define SBSDIO_FORCE_HW_CLKREQ_OFF	0x20	/* Squelch clock requests from HW */
-#define SBSDIO_ALP_AVAIL		0x40	/* Status: ALP is ready */
-#define SBSDIO_HT_AVAIL			0x80	/* Status: HT is ready */
-
-#define SBSDIO_AVBITS			(SBSDIO_HT_AVAIL | SBSDIO_ALP_AVAIL)
-#define SBSDIO_ALPAV(regval)		((regval) & SBSDIO_AVBITS)
-#define SBSDIO_HTAV(regval)		(((regval) & SBSDIO_AVBITS) == SBSDIO_AVBITS)
-#define SBSDIO_ALPONLY(regval)		(SBSDIO_ALPAV(regval) && !SBSDIO_HTAV(regval))
-#define SBSDIO_CLKAV(regval, alponly)	(SBSDIO_ALPAV(regval) && \
-					(alponly ? 1 : SBSDIO_HTAV(regval)))
+
+/* Force ALP request to backplane */
+#define SBSDIO_FORCE_ALP		0x01
+/* Force HT request to backplane */
+#define SBSDIO_FORCE_HT			0x02
+/* Force ILP request to backplane */
+#define SBSDIO_FORCE_ILP		0x04
+/* Make ALP ready (power up xtal) */
+#define SBSDIO_ALP_AVAIL_REQ		0x08
+/* Make HT ready (power up PLL) */
+#define SBSDIO_HT_AVAIL_REQ		0x10
+/* Squelch clock requests from HW */
+#define SBSDIO_FORCE_HW_CLKREQ_OFF	0x20
+/* Status: ALP is ready */
+#define SBSDIO_ALP_AVAIL		0x40
+/* Status: HT is ready */
+#define SBSDIO_HT_AVAIL			0x80
+
+#define SBSDIO_AVBITS		(SBSDIO_HT_AVAIL | SBSDIO_ALP_AVAIL)
+#define SBSDIO_ALPAV(regval)	((regval) & SBSDIO_AVBITS)
+#define SBSDIO_HTAV(regval)	(((regval) & SBSDIO_AVBITS) == SBSDIO_AVBITS)
+#define SBSDIO_ALPONLY(regval)	(SBSDIO_ALPAV(regval) && !SBSDIO_HTAV(regval))
+
+#define SBSDIO_CLKAV(regval, alponly) \
+	(SBSDIO_ALPAV(regval) && (alponly ? 1 : SBSDIO_HTAV(regval)))
+
 /* direct(mapped) cis space */
-#define SBSDIO_CIS_BASE_COMMON		0x1000	/* MAPPED common CIS address */
-#define SBSDIO_CIS_SIZE_LIMIT		0x200	/* maximum bytes in one CIS */
-#define SBSDIO_CIS_OFT_ADDR_MASK	0x1FFFF	/* cis offset addr is < 17 bits */
 
-#define SBSDIO_CIS_MANFID_TUPLE_LEN	6	/* manfid tuple length, include tuple,
-						 * link bytes
-						 */
+/* MAPPED common CIS address */
+#define SBSDIO_CIS_BASE_COMMON		0x1000
+/* maximum bytes in one CIS */
+#define SBSDIO_CIS_SIZE_LIMIT		0x200
+/* cis offset addr is < 17 bits */
+#define SBSDIO_CIS_OFT_ADDR_MASK	0x1FFFF
+
+/* manfid tuple length, include tuple, link bytes */
+#define SBSDIO_CIS_MANFID_TUPLE_LEN	6
 
 /* intstatus */
 #define I_SMB_SW0	(1 << 0)	/* To SB Mail S/W interrupt 0 */
@@ -282,11 +227,7 @@ struct rte_console {
 
 /* Total length of frame header for dongle protocol */
 #define SDPCM_HDRLEN	(SDPCM_FRAMETAG_LEN + SDPCM_SWHEADER_LEN)
-#ifdef SDTEST
-#define SDPCM_RESERVE	(SDPCM_HDRLEN + SDPCM_TEST_HDRLEN + BRCMF_SDALIGN)
-#else
 #define SDPCM_RESERVE	(SDPCM_HDRLEN + BRCMF_SDALIGN)
-#endif
 
 /*
  * Software allocation of To SB Mailbox resources
@@ -362,29 +303,6 @@ struct rte_console {
 
 #define SDPCM_GLOMDESC(p)	(((u8 *)p)[1] & 0x80)
 
-/* For TEST_CHANNEL packets, define another 4-byte header */
-#define SDPCM_TEST_HDRLEN	4	/*
-					 * Generally: Cmd(1), Ext(1), Len(2);
-					 * Semantics of Ext byte depend on
-					 * command. Len is current or requested
-					 * frame length, not including test
-					 * header; sent little-endian.
-					 */
-#define SDPCM_TEST_DISCARD	0x01	/* Receiver discards. Ext:pattern id. */
-#define SDPCM_TEST_ECHOREQ	0x02	/* Echo request. Ext:pattern id. */
-#define SDPCM_TEST_ECHORSP	0x03	/* Echo response. Ext:pattern id. */
-#define SDPCM_TEST_BURST	0x04	/*
-					 * Receiver to send a burst.
-					 * Ext is a frame count
-					 */
-#define SDPCM_TEST_SEND		0x05	/*
-					 * Receiver sets send mode.
-					 * Ext is boolean on/off
-					 */
-
-/* Handy macro for filling in datagen packets with a pattern */
-#define SDPCM_TEST_FILL(byteno, id)	((u8)(id + byteno))
-
 /*
  * Shared structure between dongle and the host.
  * The structure contains pointers to trap or assert information.
@@ -395,15 +313,8 @@ struct rte_console {
 #define SDPCM_SHARED_ASSERT        0x0200
 #define SDPCM_SHARED_TRAP          0x0400
 
-
 /* Space for header read, limit for data packets */
-#ifndef MAX_HDR_READ
-#define MAX_HDR_READ	32
-#endif
-#if !ISPOWEROF2(MAX_HDR_READ)
-#error MAX_HDR_READ is not a power of 2!
-#endif
-
+#define MAX_HDR_READ	(1 << 6)
 #define MAX_RX_DATASZ	2048
 
 /* Maximum milliseconds to wait for F2 to come up */
@@ -414,10 +325,8 @@ struct rte_console {
  * for HT availability, it could take a couple hundred ms more, so
  * max out at a 1 second (1000000us).
  */
-#if (PMU_MAX_TRANSITION_DLY <= 1000000)
 #undef PMU_MAX_TRANSITION_DLY
 #define PMU_MAX_TRANSITION_DLY 1000000
-#endif
 
 /* Value for ChipClockCSR during initial setup */
 #define BRCMF_INIT_CLKCTL1	(SBSDIO_FORCE_HW_CLKREQ_OFF |	\
@@ -433,12 +342,18 @@ struct rte_console {
 #define	SBIM_RJ			0x02000000	/* reject (sonics >= 2.3) */
 
 /* sbtmstatelow */
-#define	SBTML_RESET		0x0001	/* reset */
-#define	SBTML_REJ_MASK		0x0006	/* reject field */
-#define	SBTML_REJ		0x0002	/* reject */
-#define	SBTML_TMPREJ		0x0004	/* temporary reject, for error recovery */
 
-#define	SBTML_SICF_SHIFT	16	/* Shift to locate the SI control flags in sbtml */
+/* reset */
+#define	SBTML_RESET		0x0001
+/* reject field */
+#define	SBTML_REJ_MASK		0x0006
+/* reject */
+#define	SBTML_REJ		0x0002
+/* temporary reject, for error recovery */
+#define	SBTML_TMPREJ		0x0004
+
+/* Shift to locate the SI control flags in sbtml */
+#define	SBTML_SICF_SHIFT	16
 
 /* sbtmstatehigh */
 #define	SBTMH_SERR		0x0001	/* serror */
@@ -446,7 +361,8 @@ struct rte_console {
 #define	SBTMH_BUSY		0x0004	/* busy */
 #define	SBTMH_TO		0x0020	/* timeout (sonics >= 2.3) */
 
-#define	SBTMH_SISF_SHIFT	16	/* Shift to locate the SI status flags in sbtmh */
+/* Shift to locate the SI status flags in sbtmh */
+#define	SBTMH_SISF_SHIFT	16
 
 /* sbidlow */
 #define	SBIDL_INIT		0x80	/* initiator */
@@ -456,7 +372,8 @@ struct rte_console {
 #define	SBIDH_RCE_MASK		0x7000	/* revision code extension field */
 #define	SBIDH_RCE_SHIFT		8
 #define	SBCOREREV(sbidh) \
-	((((sbidh) & SBIDH_RCE_MASK) >> SBIDH_RCE_SHIFT) | ((sbidh) & SBIDH_RC_MASK))
+	((((sbidh) & SBIDH_RCE_MASK) >> SBIDH_RCE_SHIFT) | \
+	  ((sbidh) & SBIDH_RC_MASK))
 #define	SBIDH_CC_MASK		0x8ff0	/* core code */
 #define	SBIDH_CC_SHIFT		4
 #define	SBIDH_VC_MASK		0xffff0000	/* vendor code */
@@ -465,17 +382,18 @@ struct rte_console {
 /*
  * Conversion of 802.1D priority to precedence level
  */
-#define PRIO2PREC(prio) \
-	(((prio) == PRIO_8021D_NONE || (prio) == PRIO_8021D_BE) ? \
-	((prio^2)) : (prio))
-
-BRCMF_SPINWAIT_SLEEP_INIT(sdioh_spinwait_sleep);
+static uint prio2prec(u32 prio)
+{
+	return (prio == PRIO_8021D_NONE || prio == PRIO_8021D_BE) ?
+	       (prio^2) : prio;
+}
 
 /*
  * Core reg address translation.
  * Both macro's returns a 32 bits byte address on the backplane bus.
  */
-#define CORE_CC_REG(base, field)	(base + offsetof(chipcregs_t, field))
+#define CORE_CC_REG(base, field) \
+		(base + offsetof(struct chipcregs, field))
 #define CORE_BUS_REG(base, field) \
 		(base + offsetof(struct sdpcmd_regs, field))
 #define CORE_SB(base, field) \
@@ -575,7 +493,7 @@ struct sdpcmd_regs {
 struct brcmf_console {
 	uint count;		/* Poll interval msec counter */
 	uint log_addr;		/* Log struct address (fixed) */
-	struct rte_log log;	/* Log struct (host copy) */
+	struct rte_log_le log_le;	/* Log struct (host copy) */
 	uint bufsize;		/* Size of log buffer */
 	u8 *buf;		/* Log buffer (host copy) */
 	uint last;		/* Last buffer read index */
@@ -593,6 +511,17 @@ struct sdpcm_shared {
 	u8 tag[32];
 };
 
+struct sdpcm_shared_le {
+	__le32 flags;
+	__le32 trap_addr;
+	__le32 assert_exp_addr;
+	__le32 assert_file_addr;
+	__le32 assert_line;
+	__le32 console_addr;	/* Address of struct rte_console */
+	__le32 msgtrace_addr;
+	u8 tag[32];
+};
+
 
 /* misc chip info needed by some of the routines */
 struct chip_info {
@@ -614,22 +543,18 @@ struct chip_info {
 struct brcmf_bus {
 	struct brcmf_pub *drvr;
 
-	struct brcmf_sdio_card *card;	/* Handle for sdio card calls */
+	struct brcmf_sdio_dev *sdiodev;	/* sdio device handler */
 	struct chip_info *ci;	/* Chip info struct */
 	char *vars;		/* Variables (from CIS and/or other) */
 	uint varsz;		/* Size of variables buffer */
 
 	u32 ramsize;		/* Size of RAM in SOCRAM (bytes) */
-	u32 orig_ramsize;	/* Size of RAM in SOCRAM (bytes) */
 
-	u32 bus;		/* gSPI or SDIO bus */
 	u32 hostintmask;	/* Copy of Host Interrupt Mask */
 	u32 intstatus;	/* Intstatus bits (events) pending */
 	bool dpc_sched;		/* Indicates DPC schedule (intrpt rcvd) */
 	bool fcstate;		/* State of dongle flow-control */
 
-	u16 cl_devid;	/* cached devid for brcmf_sdio_probe_attach() */
-
 	uint blocksize;		/* Block size of SDIO transfers */
 	uint roundup;		/* Max roundup limit */
 
@@ -644,6 +569,10 @@ struct brcmf_bus {
 	u8 rx_seq;		/* Receive sequence number (expected) */
 	bool rxskip;		/* Skip receive (awaiting NAK ACK) */
 
+	uint rxbound;		/* Rx frames to read before resched */
+	uint txbound;		/* Tx frames to send before resched */
+	uint txminmax;
+
 	struct sk_buff *glomd;	/* Packet containing glomming descriptor */
 	struct sk_buff *glom;	/* Packet chain for glommed superframe */
 	uint glomerr;		/* Glom packet read errors */
@@ -660,7 +589,6 @@ struct brcmf_bus {
 	bool intr;		/* Use interrupts */
 	bool poll;		/* Use polling */
 	bool ipend;		/* Device interrupt is pending */
-	bool intdis;		/* Interrupts disabled by isr */
 	uint intrcount;		/* Count of device interrupt callbacks */
 	uint lastintrs;		/* Count as of last watchdog timer */
 	uint spurious;		/* Count of spurious interrupts */
@@ -669,6 +597,7 @@ struct brcmf_bus {
 	uint pollcnt;		/* Count of active polls */
 
 #ifdef BCMDBG
+	uint console_interval;
 	struct brcmf_console console;	/* Console output polling support */
 	uint console_addr;	/* Console address from shared struct */
 #endif				/* BCMDBG */
@@ -689,30 +618,6 @@ struct brcmf_bus {
 /* Field to decide if rx of control frames happen in rxbuf or lb-pool */
 	bool usebufpool;
 
-#ifdef SDTEST
-	/* external loopback */
-	bool ext_loop;
-	u8 loopid;
-
-	/* pktgen configuration */
-	uint pktgen_freq;	/* Ticks between bursts */
-	uint pktgen_count;	/* Packets to send each burst */
-	uint pktgen_print;	/* Bursts between count displays */
-	uint pktgen_total;	/* Stop after this many */
-	uint pktgen_minlen;	/* Minimum packet data len */
-	uint pktgen_maxlen;	/* Maximum packet data len */
-	uint pktgen_mode;	/* Configured mode: tx, rx, or echo */
-	uint pktgen_stop;	/* Number of tx failures causing stop */
-
-	/* active pktgen fields */
-	uint pktgen_tick;	/* Tick counter for bursts */
-	uint pktgen_ptick;	/* Burst counter for printing */
-	uint pktgen_sent;	/* Number of test packets generated */
-	uint pktgen_rcvd;	/* Number of test packets received */
-	uint pktgen_fail;	/* Number of failed send attempts */
-	u16 pktgen_len;	/* Length of next packet to send */
-#endif				/* SDTEST */
-
 	/* Some additional counters */
 	uint tx_sderrs;		/* Count of tx attempts with sd errors */
 	uint fcqueued;		/* Tx packets that got queued */
@@ -739,19 +644,18 @@ struct brcmf_bus {
 
 	spinlock_t txqlock;
 	wait_queue_head_t ctrl_wait;
+	wait_queue_head_t dcmd_resp_wait;
 
 	struct timer_list timer;
 	struct completion watchdog_wait;
 	struct task_struct *watchdog_tsk;
 	bool wd_timer_valid;
+	uint save_ms;
 
-	struct tasklet_struct tasklet;
 	struct task_struct *dpc_tsk;
 	struct completion dpc_wait;
 
-	bool threads_only;
 	struct semaphore sdsem;
-	spinlock_t sdlock;
 
 	const char *fw_name;
 	const struct firmware *firmware;
@@ -805,82 +709,39 @@ struct sbconfig {
 #define CLK_PENDING	2	/* Not used yet */
 #define CLK_AVAIL	3
 
-#define BRCMF_NOPMU(brcmf)	(false)
-
 #ifdef BCMDBG
 static int qcount[NUMPRIO];
 static int tx_packets[NUMPRIO];
 #endif				/* BCMDBG */
 
-/* Deferred transmit */
-uint brcmf_deferred_tx = 1;
-module_param(brcmf_deferred_tx, uint, 0);
+#define SDIO_DRIVE_STRENGTH	6	/* in milliamps */
 
-/* Watchdog thread priority, -1 to use kernel timer */
-int brcmf_watchdog_prio = 97;
-module_param(brcmf_watchdog_prio, int, 0);
-
-/* Watchdog interval */
-uint brcmf_watchdog_ms = 10;
-module_param(brcmf_watchdog_ms, uint, 0);
-
-/* DPC thread priority, -1 to use tasklet */
-int brcmf_dpc_prio = 98;
-module_param(brcmf_dpc_prio, int, 0);
-
-#ifdef BCMDBG
-/* Console poll interval */
-uint brcmf_console_ms;
-module_param(brcmf_console_ms, uint, 0);
-#endif		/* BCMDBG */
-
-/* Tx/Rx bounds */
-uint brcmf_txbound;
-uint brcmf_rxbound;
-uint brcmf_txminmax;
-
-/* override the RAM size if possible */
-#define DONGLE_MIN_MEMSIZE (128 * 1024)
-int brcmf_dongle_memsize;
-
-static bool brcmf_alignctl;
-
-static bool sd1idle;
-
-static bool retrydata;
-#define RETRYCHAN(chan) (((chan) == SDPCM_EVENT_CHANNEL) || retrydata)
-
-static const uint watermark = 8;
-static const uint firstread = BRCMF_FIRSTREAD;
+#define RETRYCHAN(chan) ((chan) == SDPCM_EVENT_CHANNEL)
 
 /* Retry count for register access failures */
 static const uint retry_limit = 2;
 
-/* Force even SD lengths (some host controllers mess up on odd bytes) */
-static bool forcealign;
-
-#define ALIGNMENT  4
-
-#define PKTALIGN(_p, _len, _align)				\
-	do {								\
-		uint datalign;						\
-		datalign = (unsigned long)((_p)->data);			\
-		datalign = roundup(datalign, (_align)) - datalign;	\
-		if (datalign)						\
-			skb_pull((_p), datalign);			\
-		__skb_trim((_p), (_len));				\
-	} while (0)
-
 /* Limit on rounding up frames */
 static const uint max_roundup = 512;
 
-/* Try doing readahead */
-static bool brcmf_readahead;
+#define ALIGNMENT  4
+
+static void pkt_align(struct sk_buff *p, int len, int align)
+{
+	uint datalign;
+	datalign = (unsigned long)(p->data);
+	datalign = roundup(datalign, (align)) - datalign;
+	if (datalign)
+		skb_pull(p, datalign);
+	__skb_trim(p, len);
+}
 
 /* To check if there's window offered */
-#define DATAOK(bus) \
-	(((u8)(bus->tx_max - bus->tx_seq) != 0) && \
-	(((u8)(bus->tx_max - bus->tx_seq) & 0x80) == 0))
+static bool data_ok(struct brcmf_bus *bus)
+{
+	return (u8)(bus->tx_max - bus->tx_seq) != 0 &&
+	       ((u8)(bus->tx_max - bus->tx_seq) & 0x80) == 0;
+}
 
 /*
  * Reads a register in the SDIO hardware block. This block occupies a series of
@@ -891,13 +752,14 @@ r_sdreg32(struct brcmf_bus *bus, u32 *regvar, u32 reg_offset, u32 *retryvar)
 {
 	*retryvar = 0;
 	do {
-		*regvar = R_REG(bus->ci->buscorebase + reg_offset, u32);
-	} while (brcmf_sdcard_regfail(bus->card) &&
+		*regvar = brcmf_sdcard_reg_read(bus->sdiodev,
+				bus->ci->buscorebase + reg_offset, sizeof(u32));
+	} while (brcmf_sdcard_regfail(bus->sdiodev) &&
 		 (++(*retryvar) <= retry_limit));
 	if (*retryvar) {
 		bus->regfails += (*retryvar-1);
 		if (*retryvar > retry_limit) {
-			BRCMF_ERROR(("FAILED READ %Xh\n", reg_offset));
+			brcmf_dbg(ERROR, "FAILED READ %Xh\n", reg_offset);
 			*regvar = 0;
 		}
 	}
@@ -908,133 +770,42 @@ w_sdreg32(struct brcmf_bus *bus, u32 regval, u32 reg_offset, u32 *retryvar)
 {
 	*retryvar = 0;
 	do {
-		brcmf_sdcard_reg_write(NULL, bus->ci->buscorebase + reg_offset,
+		brcmf_sdcard_reg_write(bus->sdiodev,
+				       bus->ci->buscorebase + reg_offset,
 				       sizeof(u32), regval);
-	} while (brcmf_sdcard_regfail(bus->card) &&
+	} while (brcmf_sdcard_regfail(bus->sdiodev) &&
 		 (++(*retryvar) <= retry_limit));
 	if (*retryvar) {
 		bus->regfails += (*retryvar-1);
 		if (*retryvar > retry_limit)
-			BRCMF_ERROR(("FAILED REGISTER WRITE"
-				     " %Xh\n", reg_offset));
+			brcmf_dbg(ERROR, "FAILED REGISTER WRITE %Xh\n",
+				  reg_offset);
 	}
 }
 
-#define BRCMF_BUS			SDIO_BUS
-
 #define PKT_AVAILABLE()		(intstatus & I_HMB_FRAME_IND)
 
 #define HOSTINTMASK		(I_HMB_SW_MASK | I_CHIPACTIVE)
 
-#ifdef SDTEST
-static void brcmf_sdbrcm_checkdied(struct brcmf_bus *bus, void *pkt, uint seq);
-static void brcmf_sdbrcm_sdtest_set(struct brcmf_bus *bus, bool start);
-#endif
-
-#ifdef BCMDBG
-static int brcmf_sdbrcm_bus_console_in(struct brcmf_pub *drvr,
-				       unsigned char *msg, uint msglen);
-static int brcmf_sdbrcm_checkdied(struct brcmf_bus *bus, u8 *data, uint size);
-static int brcmf_sdbrcm_mem_dump(struct brcmf_bus *bus);
-#endif				/* BCMDBG  */
-static int brcmf_sdbrcm_download_state(struct brcmf_bus *bus, bool enter);
-
-static void brcmf_sdbrcm_release(struct brcmf_bus *bus);
-static void brcmf_sdbrcm_release_malloc(struct brcmf_bus *bus);
-static void brcmf_sdbrcm_disconnect(void *ptr);
-static bool brcmf_sdbrcm_chipmatch(u16 chipid);
-static bool brcmf_sdbrcm_probe_attach(struct brcmf_bus *bus, void *card,
-				      u32 regsva, u16 devid);
-static bool brcmf_sdbrcm_probe_malloc(struct brcmf_bus *bus, void *card);
-static bool brcmf_sdbrcm_probe_init(struct brcmf_bus *bus, void *card);
-static void brcmf_sdbrcm_release_dongle(struct brcmf_bus *bus);
-
-static uint brcmf_process_nvram_vars(char *varbuf, uint len);
-
-static void brcmf_sdbrcm_setmemsize(struct brcmf_bus *bus, int mem_size);
-static int brcmf_sdbrcm_send_buf(struct brcmf_bus *bus, u32 addr, uint fn,
-			       uint flags, u8 *buf, uint nbytes,
-			       struct sk_buff *pkt,
-			       void (*complete)(void *handle, int status,
-						      bool sync_waiting),
-			       void *handle);
-
-static bool brcmf_sdbrcm_download_firmware(struct brcmf_bus *bus, void *card);
-static int  _brcmf_sdbrcm_download_firmware(struct brcmf_bus *bus);
-
-static int brcmf_sdbrcm_download_code_file(struct brcmf_bus *bus);
-static int brcmf_sdbrcm_download_nvram(struct brcmf_bus *bus);
-
-static void
-brcmf_sdbrcm_chip_disablecore(struct brcmf_sdio_card *card, u32 corebase);
-
-static int brcmf_sdbrcm_chip_attach(struct brcmf_bus *bus, u32 regs);
-
-static void
-brcmf_sdbrcm_chip_resetcore(struct brcmf_sdio_card *card, u32 corebase);
-
-static void brcmf_sdbrcm_sdiod_drive_strength_init(struct brcmf_bus *bus,
-					u32 drivestrength);
-static void brcmf_sdbrcm_chip_detach(struct brcmf_bus *bus);
-static void brcmf_sdbrcm_wait_for_event(struct brcmf_bus *bus, bool *lockvar);
-static void brcmf_sdbrcm_wait_event_wakeup(struct brcmf_bus *bus);
-static void brcmf_sdbrcm_watchdog(unsigned long data);
-static int brcmf_sdbrcm_watchdog_thread(void *data);
-static int brcmf_sdbrcm_dpc_thread(void *data);
-static void brcmf_sdbrcm_dpc_tasklet(unsigned long data);
-static void brcmf_sdbrcm_sched_dpc(struct brcmf_bus *bus);
-static void brcmf_sdbrcm_sdlock(struct brcmf_bus *bus);
-static void brcmf_sdbrcm_sdunlock(struct brcmf_bus *bus);
-static int brcmf_sdbrcm_get_image(char *buf, int len, struct brcmf_bus *bus);
-
 /* Packet free applicable unconditionally for sdio and sdspi.
  * Conditional if bufpool was present for gspi bus.
  */
 static void brcmf_sdbrcm_pktfree2(struct brcmf_bus *bus, struct sk_buff *pkt)
 {
-	if ((bus->bus != SPI_BUS) || bus->usebufpool)
+	if (bus->usebufpool)
 		brcmu_pkt_buf_free_skb(pkt);
 }
 
-static void brcmf_sdbrcm_setmemsize(struct brcmf_bus *bus, int mem_size)
-{
-	s32 min_size = DONGLE_MIN_MEMSIZE;
-	/* Restrict the memsize to user specified limit */
-	BRCMF_ERROR(("user: Restrict the dongle ram size to %d, min %d\n",
-		     brcmf_dongle_memsize, min_size));
-	if ((brcmf_dongle_memsize > min_size) &&
-	    (brcmf_dongle_memsize < (s32) bus->orig_ramsize))
-		bus->ramsize = brcmf_dongle_memsize;
-}
-
-static int brcmf_sdbrcm_set_siaddr_window(struct brcmf_bus *bus, u32 address)
-{
-	int err = 0;
-	brcmf_sdcard_cfg_write(bus->card, SDIO_FUNC_1, SBSDIO_FUNC1_SBADDRLOW,
-			 (address >> 8) & SBSDIO_SBADDRLOW_MASK, &err);
-	if (!err)
-		brcmf_sdcard_cfg_write(bus->card, SDIO_FUNC_1,
-				 SBSDIO_FUNC1_SBADDRMID,
-				 (address >> 16) & SBSDIO_SBADDRMID_MASK, &err);
-	if (!err)
-		brcmf_sdcard_cfg_write(bus->card, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_SBADDRHIGH,
-				       (address >> 24) & SBSDIO_SBADDRHIGH_MASK,
-				       &err);
-	return err;
-}
-
 /* Turn backplane clock on or off */
 static int brcmf_sdbrcm_htclk(struct brcmf_bus *bus, bool on, bool pendok)
 {
 	int err;
 	u8 clkctl, clkreq, devctl;
-	struct brcmf_sdio_card *card;
+	unsigned long timeout;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	clkctl = 0;
-	card = bus->card;
 
 	if (on) {
 		/* Request HT Avail */
@@ -1045,11 +816,10 @@ static int brcmf_sdbrcm_htclk(struct brcmf_bus *bus, bool on, bool pendok)
 		    && (bus->ci->chiprev == 0))
 			clkreq |= SBSDIO_FORCE_ALP;
 
-		brcmf_sdcard_cfg_write(card, SDIO_FUNC_1,
+		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
 				       SBSDIO_FUNC1_CHIPCLKCSR, clkreq, &err);
 		if (err) {
-			BRCMF_ERROR(("%s: HT Avail request error: %d\n",
-				     __func__, err));
+			brcmf_dbg(ERROR, "HT Avail request error: %d\n", err);
 			return -EBADE;
 		}
 
@@ -1062,74 +832,73 @@ static int brcmf_sdbrcm_htclk(struct brcmf_bus *bus, bool on, bool pendok)
 		}
 
 		/* Check current status */
-		clkctl = brcmf_sdcard_cfg_read(card, SDIO_FUNC_1,
+		clkctl = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
 					       SBSDIO_FUNC1_CHIPCLKCSR, &err);
 		if (err) {
-			BRCMF_ERROR(("%s: HT Avail read error: %d\n",
-				     __func__, err));
+			brcmf_dbg(ERROR, "HT Avail read error: %d\n", err);
 			return -EBADE;
 		}
 
 		/* Go to pending and await interrupt if appropriate */
 		if (!SBSDIO_CLKAV(clkctl, bus->alp_only) && pendok) {
 			/* Allow only clock-available interrupt */
-			devctl = brcmf_sdcard_cfg_read(card, SDIO_FUNC_1,
+			devctl = brcmf_sdcard_cfg_read(bus->sdiodev,
+					SDIO_FUNC_1,
 					SBSDIO_DEVICE_CTL, &err);
 			if (err) {
-				BRCMF_ERROR(("%s: Devctl error setting CA:"
-					     " %d\n", __func__, err));
+				brcmf_dbg(ERROR, "Devctl error setting CA: %d\n",
+					  err);
 				return -EBADE;
 			}
 
 			devctl |= SBSDIO_DEVCTL_CA_INT_ONLY;
-			brcmf_sdcard_cfg_write(card, SDIO_FUNC_1,
+			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
 					       SBSDIO_DEVICE_CTL, devctl, &err);
-			BRCMF_INFO(("CLKCTL: set PENDING\n"));
+			brcmf_dbg(INFO, "CLKCTL: set PENDING\n");
 			bus->clkstate = CLK_PENDING;
 
 			return 0;
 		} else if (bus->clkstate == CLK_PENDING) {
 			/* Cancel CA-only interrupt filter */
 			devctl =
-			    brcmf_sdcard_cfg_read(card, SDIO_FUNC_1,
+			    brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
 						  SBSDIO_DEVICE_CTL, &err);
 			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
-			brcmf_sdcard_cfg_write(card, SDIO_FUNC_1,
+			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
 				SBSDIO_DEVICE_CTL, devctl, &err);
 		}
 
 		/* Otherwise, wait here (polling) for HT Avail */
-		if (!SBSDIO_CLKAV(clkctl, bus->alp_only)) {
-			BRCMF_SPINWAIT_SLEEP(sdioh_spinwait_sleep,
-			       ((clkctl =
-				 brcmf_sdcard_cfg_read(card, SDIO_FUNC_1,
-					 SBSDIO_FUNC1_CHIPCLKCSR,
-						 &err)),
-				!SBSDIO_CLKAV(clkctl, bus->alp_only)),
-			       PMU_MAX_TRANSITION_DLY);
+		timeout = jiffies +
+			  msecs_to_jiffies(PMU_MAX_TRANSITION_DLY/1000);
+		while (!SBSDIO_CLKAV(clkctl, bus->alp_only)) {
+			clkctl = brcmf_sdcard_cfg_read(bus->sdiodev,
+						       SDIO_FUNC_1,
+						       SBSDIO_FUNC1_CHIPCLKCSR,
+						       &err);
+			if (time_after(jiffies, timeout))
+				break;
+			else
+				usleep_range(5000, 10000);
 		}
 		if (err) {
-			BRCMF_ERROR(("%s: HT Avail request error: %d\n",
-				     __func__, err));
+			brcmf_dbg(ERROR, "HT Avail request error: %d\n", err);
 			return -EBADE;
 		}
 		if (!SBSDIO_CLKAV(clkctl, bus->alp_only)) {
-			BRCMF_ERROR(("%s: HT Avail timeout (%d): "
-				     "clkctl 0x%02x\n", __func__,
-				     PMU_MAX_TRANSITION_DLY, clkctl));
+			brcmf_dbg(ERROR, "HT Avail timeout (%d): clkctl 0x%02x\n",
+				  PMU_MAX_TRANSITION_DLY, clkctl);
 			return -EBADE;
 		}
 
 		/* Mark clock available */
 		bus->clkstate = CLK_AVAIL;
-		BRCMF_INFO(("CLKCTL: turned ON\n"));
+		brcmf_dbg(INFO, "CLKCTL: turned ON\n");
 
 #if defined(BCMDBG)
 		if (bus->alp_only != true) {
-			if (SBSDIO_ALPONLY(clkctl)) {
-				BRCMF_ERROR(("%s: HT Clock should be on.\n",
-					     __func__));
-			}
+			if (SBSDIO_ALPONLY(clkctl))
+				brcmf_dbg(ERROR, "HT Clock should be on\n");
 		}
 #endif				/* defined (BCMDBG) */
 
@@ -1139,20 +908,21 @@ static int brcmf_sdbrcm_htclk(struct brcmf_bus *bus, bool on, bool pendok)
 
 		if (bus->clkstate == CLK_PENDING) {
 			/* Cancel CA-only interrupt filter */
-			devctl = brcmf_sdcard_cfg_read(card, SDIO_FUNC_1,
+			devctl = brcmf_sdcard_cfg_read(bus->sdiodev,
+					SDIO_FUNC_1,
 					SBSDIO_DEVICE_CTL, &err);
 			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
-			brcmf_sdcard_cfg_write(card, SDIO_FUNC_1,
+			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
 				SBSDIO_DEVICE_CTL, devctl, &err);
 		}
 
 		bus->clkstate = CLK_SDONLY;
-		brcmf_sdcard_cfg_write(card, SDIO_FUNC_1,
+		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
 			SBSDIO_FUNC1_CHIPCLKCSR, clkreq, &err);
-		BRCMF_INFO(("CLKCTL: turned OFF\n"));
+		brcmf_dbg(INFO, "CLKCTL: turned OFF\n");
 		if (err) {
-			BRCMF_ERROR(("%s: Failed access turning clock off:"
-				     " %d\n", __func__, err));
+			brcmf_dbg(ERROR, "Failed access turning clock off: %d\n",
+				  err);
 			return -EBADE;
 		}
 	}
@@ -1162,7 +932,7 @@ static int brcmf_sdbrcm_htclk(struct brcmf_bus *bus, bool on, bool pendok)
 /* Change idle/active SD state */
 static int brcmf_sdbrcm_sdclk(struct brcmf_bus *bus, bool on)
 {
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	if (on)
 		bus->clkstate = CLK_SDONLY;
@@ -1179,12 +949,12 @@ static int brcmf_sdbrcm_clkctl(struct brcmf_bus *bus, uint target, bool pendok)
 	uint oldstate = bus->clkstate;
 #endif				/* BCMDBG */
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	/* Early exit if we're already there */
 	if (bus->clkstate == target) {
 		if (target == CLK_AVAIL) {
-			brcmf_sdbrcm_wd_timer(bus, brcmf_watchdog_ms);
+			brcmf_sdbrcm_wd_timer(bus, BRCMF_WD_POLL_MS);
 			bus->activity = true;
 		}
 		return 0;
@@ -1197,7 +967,7 @@ static int brcmf_sdbrcm_clkctl(struct brcmf_bus *bus, uint target, bool pendok)
 			brcmf_sdbrcm_sdclk(bus, true);
 		/* Now request HT Avail on the backplane */
 		brcmf_sdbrcm_htclk(bus, true, pendok);
-		brcmf_sdbrcm_wd_timer(bus, brcmf_watchdog_ms);
+		brcmf_sdbrcm_wd_timer(bus, BRCMF_WD_POLL_MS);
 		bus->activity = true;
 		break;
 
@@ -1208,9 +978,9 @@ static int brcmf_sdbrcm_clkctl(struct brcmf_bus *bus, uint target, bool pendok)
 		else if (bus->clkstate == CLK_AVAIL)
 			brcmf_sdbrcm_htclk(bus, false, false);
 		else
-			BRCMF_ERROR(("brcmf_sdbrcm_clkctl: request for %d -> %d"
-				     "\n", bus->clkstate, target));
-		brcmf_sdbrcm_wd_timer(bus, brcmf_watchdog_ms);
+			brcmf_dbg(ERROR, "request for %d -> %d\n",
+				  bus->clkstate, target);
+		brcmf_sdbrcm_wd_timer(bus, BRCMF_WD_POLL_MS);
 		break;
 
 	case CLK_NONE:
@@ -1223,21 +993,19 @@ static int brcmf_sdbrcm_clkctl(struct brcmf_bus *bus, uint target, bool pendok)
 		break;
 	}
 #ifdef BCMDBG
-	BRCMF_INFO(("brcmf_sdbrcm_clkctl: %d -> %d\n",
-		    oldstate, bus->clkstate));
+	brcmf_dbg(INFO, "%d -> %d\n", oldstate, bus->clkstate);
 #endif				/* BCMDBG */
 
 	return 0;
 }
 
-int brcmf_sdbrcm_bussleep(struct brcmf_bus *bus, bool sleep)
+static int brcmf_sdbrcm_bussleep(struct brcmf_bus *bus, bool sleep)
 {
-	struct brcmf_sdio_card *card = bus->card;
 	uint retries = 0;
 
-	BRCMF_INFO(("brcmf_sdbrcm_bussleep: request %s (currently %s)\n",
-		    (sleep ? "SLEEP" : "WAKE"),
-		    (bus->sleeping ? "SLEEP" : "WAKE")));
+	brcmf_dbg(INFO, "request %s (currently %s)\n",
+		  sleep ? "SLEEP" : "WAKE",
+		  bus->sleeping ? "SLEEP" : "WAKE");
 
 	/* Done if we're already in the requested state */
 	if (sleep == bus->sleeping)
@@ -1249,9 +1017,6 @@ int brcmf_sdbrcm_bussleep(struct brcmf_bus *bus, bool sleep)
 		if (bus->dpc_sched || bus->rxskip || pktq_len(&bus->txq))
 			return -EBUSY;
 
-		/* Disable SDIO interrupts (no longer interested) */
-		brcmf_sdcard_intr_disable(bus->card);
-
 		/* Make sure the controller has the bus up */
 		brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
 
@@ -1259,20 +1024,18 @@ int brcmf_sdbrcm_bussleep(struct brcmf_bus *bus, bool sleep)
 		w_sdreg32(bus, SMB_USE_OOB,
 			  offsetof(struct sdpcmd_regs, tosbmailbox), &retries);
 		if (retries > retry_limit)
-			BRCMF_ERROR(("CANNOT SIGNAL CHIP, "
-				     "WILL NOT WAKE UP!!\n"));
+			brcmf_dbg(ERROR, "CANNOT SIGNAL CHIP, WILL NOT WAKE UP!!\n");
 
 		/* Turn off our contribution to the HT clock request */
 		brcmf_sdbrcm_clkctl(bus, CLK_SDONLY, false);
 
-		brcmf_sdcard_cfg_write(card, SDIO_FUNC_1,
+		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
 			SBSDIO_FUNC1_CHIPCLKCSR,
 			SBSDIO_FORCE_HW_CLKREQ_OFF, NULL);
 
 		/* Isolate the bus */
-		if (bus->ci->chip != BCM4329_CHIP_ID
-		    && bus->ci->chip != BCM4319_CHIP_ID) {
-			brcmf_sdcard_cfg_write(card, SDIO_FUNC_1,
+		if (bus->ci->chip != BCM4329_CHIP_ID) {
+			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
 				SBSDIO_DEVICE_CTL,
 				SBSDIO_DEVCTL_PADS_ISO, NULL);
 		}
@@ -1283,14 +1046,14 @@ int brcmf_sdbrcm_bussleep(struct brcmf_bus *bus, bool sleep)
 	} else {
 		/* Waking up: bus power up is ok, set local state */
 
-		brcmf_sdcard_cfg_write(card, SDIO_FUNC_1,
+		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
 			SBSDIO_FUNC1_CHIPCLKCSR, 0, NULL);
 
 		/* Force pad isolation off if possible
 			 (in case power never toggled) */
 		if ((bus->ci->buscoretype == PCMCIA_CORE_ID)
 		    && (bus->ci->buscorerev >= 10))
-			brcmf_sdcard_cfg_write(card, SDIO_FUNC_1,
+			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
 				SBSDIO_DEVICE_CTL, 0, NULL);
 
 		/* Make sure the controller has the bus up */
@@ -1305,2142 +1068,124 @@ int brcmf_sdbrcm_bussleep(struct brcmf_bus *bus, bool sleep)
 				  &retries);
 
 		if (retries > retry_limit)
-			BRCMF_ERROR(("CANNOT SIGNAL CHIP TO CLEAR OOB!!\n"));
+			brcmf_dbg(ERROR, "CANNOT SIGNAL CHIP TO CLEAR OOB!!\n");
 
 		/* Make sure we have SD bus access */
 		brcmf_sdbrcm_clkctl(bus, CLK_SDONLY, false);
 
 		/* Change state */
 		bus->sleeping = false;
-
-		/* Enable interrupts again */
-		if (bus->intr && (bus->drvr->busstate == BRCMF_BUS_DATA)) {
-			bus->intdis = false;
-			brcmf_sdcard_intr_enable(bus->card);
-		}
 	}
 
 	return 0;
 }
 
-#define BUS_WAKE(bus) \
-	do { \
-		if ((bus)->sleeping) \
-			brcmf_sdbrcm_bussleep((bus), false); \
-	} while (0);
-
-/* Writes a HW/SW header into the packet and sends it. */
-/* Assumes: (a) header space already there, (b) caller holds lock */
-static int brcmf_sdbrcm_txpkt(struct brcmf_bus *bus, struct sk_buff *pkt, uint chan,
-			 bool free_pkt)
+static void bus_wake(struct brcmf_bus *bus)
 {
-	int ret;
-	u8 *frame;
-	u16 len, pad = 0;
-	u32 swheader;
-	uint retries = 0;
-	struct brcmf_sdio_card *card;
-	struct sk_buff *new;
-	int i;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	card = bus->card;
-
-	if (bus->drvr->dongle_reset) {
-		ret = -EPERM;
-		goto done;
-	}
-
-	frame = (u8 *) (pkt->data);
-
-	/* Add alignment padding, allocate new packet if needed */
-	pad = ((unsigned long)frame % BRCMF_SDALIGN);
-	if (pad) {
-		if (skb_headroom(pkt) < pad) {
-			BRCMF_INFO(("%s: insufficient headroom %d for %d pad\n",
-				    __func__, skb_headroom(pkt), pad));
-			bus->drvr->tx_realloc++;
-			new = brcmu_pkt_buf_get_skb(pkt->len + BRCMF_SDALIGN);
-			if (!new) {
-				BRCMF_ERROR(("%s: couldn't allocate new "
-					     "%d-byte packet\n", __func__,
-					     pkt->len + BRCMF_SDALIGN));
-				ret = -ENOMEM;
-				goto done;
-			}
-
-			PKTALIGN(new, pkt->len, BRCMF_SDALIGN);
-			memcpy(new->data, pkt->data, pkt->len);
-			if (free_pkt)
-				brcmu_pkt_buf_free_skb(pkt);
-			/* free the pkt if canned one is not used */
-			free_pkt = true;
-			pkt = new;
-			frame = (u8 *) (pkt->data);
-			/* precondition: (frame % BRCMF_SDALIGN) == 0) */
-			pad = 0;
-		} else {
-			skb_push(pkt, pad);
-			frame = (u8 *) (pkt->data);
-			/* precondition: pad + SDPCM_HDRLEN <= pkt->len */
-			memset(frame, 0, pad + SDPCM_HDRLEN);
-		}
-	}
-	/* precondition: pad < BRCMF_SDALIGN */
-
-	/* Hardware tag: 2 byte len followed by 2 byte ~len check (all LE) */
-	len = (u16) (pkt->len);
-	*(u16 *) frame = cpu_to_le16(len);
-	*(((u16 *) frame) + 1) = cpu_to_le16(~len);
-
-	/* Software tag: channel, sequence number, data offset */
-	swheader =
-	    ((chan << SDPCM_CHANNEL_SHIFT) & SDPCM_CHANNEL_MASK) | bus->tx_seq |
-	    (((pad +
-	       SDPCM_HDRLEN) << SDPCM_DOFFSET_SHIFT) & SDPCM_DOFFSET_MASK);
-
-	put_unaligned_le32(swheader, frame + SDPCM_FRAMETAG_LEN);
-	put_unaligned_le32(0, frame + SDPCM_FRAMETAG_LEN + sizeof(swheader));
-
-#ifdef BCMDBG
-	tx_packets[pkt->priority]++;
-	if (BRCMF_BYTES_ON() &&
-	    (((BRCMF_CTL_ON() && (chan == SDPCM_CONTROL_CHANNEL)) ||
-	      (BRCMF_DATA_ON() && (chan != SDPCM_CONTROL_CHANNEL))))) {
-		printk(KERN_DEBUG "Tx Frame:\n");
-		print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, frame, len);
-	} else if (BRCMF_HDRS_ON()) {
-		printk(KERN_DEBUG "TxHdr:\n");
-		print_hex_dump_bytes("", DUMP_PREFIX_OFFSET,
-				     frame, min_t(u16, len, 16));
-	}
-#endif
-
-	/* Raise len to next SDIO block to eliminate tail command */
-	if (bus->roundup && bus->blocksize && (len > bus->blocksize)) {
-		u16 pad = bus->blocksize - (len % bus->blocksize);
-		if ((pad <= bus->roundup) && (pad < bus->blocksize))
-				len += pad;
-	} else if (len % BRCMF_SDALIGN) {
-		len += BRCMF_SDALIGN - (len % BRCMF_SDALIGN);
-	}
-
-	/* Some controllers have trouble with odd bytes -- round to even */
-	if (forcealign && (len & (ALIGNMENT - 1))) {
-			len = roundup(len, ALIGNMENT);
-	}
-
-	do {
-		ret = brcmf_sdbrcm_send_buf(bus, brcmf_sdcard_cur_sbwad(card),
-			SDIO_FUNC_2, F2SYNC, frame, len, pkt, NULL, NULL);
-		bus->f2txdata++;
-
-		if (ret < 0) {
-			/* On failure, abort the command
-			 and terminate the frame */
-			BRCMF_INFO(("%s: sdio error %d, abort command and "
-				    "terminate frame.\n", __func__, ret));
-			bus->tx_sderrs++;
-
-			brcmf_sdcard_abort(card, SDIO_FUNC_2);
-			brcmf_sdcard_cfg_write(card, SDIO_FUNC_1,
-					 SBSDIO_FUNC1_FRAMECTRL, SFC_WF_TERM,
-					 NULL);
-			bus->f1regdata++;
-
-			for (i = 0; i < 3; i++) {
-				u8 hi, lo;
-				hi = brcmf_sdcard_cfg_read(card, SDIO_FUNC_1,
-						     SBSDIO_FUNC1_WFRAMEBCHI,
-						     NULL);
-				lo = brcmf_sdcard_cfg_read(card, SDIO_FUNC_1,
-						     SBSDIO_FUNC1_WFRAMEBCLO,
-						     NULL);
-				bus->f1regdata += 2;
-				if ((hi == 0) && (lo == 0))
-					break;
-			}
-
-		}
-		if (ret == 0)
-			bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQUENCE_WRAP;
-
-	} while ((ret < 0) && retrydata && retries++ < TXRETRIES);
-
-done:
-	/* restore pkt buffer pointer before calling tx complete routine */
-	skb_pull(pkt, SDPCM_HDRLEN + pad);
-	brcmf_sdbrcm_sdunlock(bus);
-	brcmf_txcomplete(bus->drvr, pkt, ret != 0);
-	brcmf_sdbrcm_sdlock(bus);
-
-	if (free_pkt)
-		brcmu_pkt_buf_free_skb(pkt);
-
-	return ret;
-}
-
-int brcmf_sdbrcm_bus_txdata(struct brcmf_bus *bus, struct sk_buff *pkt)
-{
-	int ret = -EBADE;
-	uint datalen, prec;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	datalen = pkt->len;
-
-#ifdef SDTEST
-	/* Push the test header if doing loopback */
-	if (bus->ext_loop) {
-		u8 *data;
-		skb_push(pkt, SDPCM_TEST_HDRLEN);
-		data = pkt->data;
-		*data++ = SDPCM_TEST_ECHOREQ;
-		*data++ = (u8) bus->loopid++;
-		*data++ = (datalen >> 0);
-		*data++ = (datalen >> 8);
-		datalen += SDPCM_TEST_HDRLEN;
-	}
-#endif				/* SDTEST */
-
-	/* Add space for the header */
-	skb_push(pkt, SDPCM_HDRLEN);
-	/* precondition: IS_ALIGNED((unsigned long)(pkt->data), 2) */
-
-	prec = PRIO2PREC((pkt->priority & PRIOMASK));
-
-	/* Check for existing queue, current flow-control,
-			 pending event, or pending clock */
-	if (brcmf_deferred_tx || bus->fcstate || pktq_len(&bus->txq)
-	    || bus->dpc_sched || (!DATAOK(bus))
-	    || (bus->flowcontrol & NBITVAL(prec))
-	    || (bus->clkstate != CLK_AVAIL)) {
-		BRCMF_TRACE(("%s: deferring pktq len %d\n", __func__,
-			     pktq_len(&bus->txq)));
-		bus->fcqueued++;
-
-		/* Priority based enq */
-		spin_lock_bh(&bus->txqlock);
-		if (brcmf_c_prec_enq(bus->drvr, &bus->txq, pkt, prec) == false) {
-			skb_pull(pkt, SDPCM_HDRLEN);
-			brcmf_txcomplete(bus->drvr, pkt, false);
-			brcmu_pkt_buf_free_skb(pkt);
-			BRCMF_ERROR(("%s: out of bus->txq !!!\n", __func__));
-			ret = -ENOSR;
-		} else {
-			ret = 0;
-		}
-		spin_unlock_bh(&bus->txqlock);
-
-		if (pktq_len(&bus->txq) >= TXHI)
-			brcmf_txflowcontrol(bus->drvr, 0, ON);
-
-#ifdef BCMDBG
-		if (pktq_plen(&bus->txq, prec) > qcount[prec])
-			qcount[prec] = pktq_plen(&bus->txq, prec);
-#endif
-		/* Schedule DPC if needed to send queued packet(s) */
-		if (brcmf_deferred_tx && !bus->dpc_sched) {
-			bus->dpc_sched = true;
-			brcmf_sdbrcm_sched_dpc(bus);
-		}
-	} else {
-		/* Lock: we're about to use shared data/code (and SDIO) */
-		brcmf_sdbrcm_sdlock(bus);
-
-		/* Otherwise, send it now */
-		BUS_WAKE(bus);
-		/* Make sure back plane ht clk is on, no pending allowed */
-		brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, true);
-
-#ifndef SDTEST
-		BRCMF_TRACE(("%s: calling txpkt\n", __func__));
-		ret = brcmf_sdbrcm_txpkt(bus, pkt, SDPCM_DATA_CHANNEL, true);
-#else
-		ret = brcmf_sdbrcm_txpkt(bus, pkt,
-				    (bus->ext_loop ? SDPCM_TEST_CHANNEL :
-				     SDPCM_DATA_CHANNEL), true);
-#endif
-		if (ret)
-			bus->drvr->tx_errors++;
-		else
-			bus->drvr->dstats.tx_bytes += datalen;
-
-		if (bus->idletime == BRCMF_IDLE_IMMEDIATE &&
-		    !bus->dpc_sched) {
-			bus->activity = false;
-			brcmf_sdbrcm_clkctl(bus, CLK_NONE, true);
-		}
-
-		brcmf_sdbrcm_sdunlock(bus);
-	}
-
-	return ret;
+	if (bus->sleeping)
+		brcmf_sdbrcm_bussleep(bus, false);
 }
 
-static uint brcmf_sdbrcm_sendfromq(struct brcmf_bus *bus, uint maxframes)
+static u32 brcmf_sdbrcm_hostmail(struct brcmf_bus *bus)
 {
-	struct sk_buff *pkt;
 	u32 intstatus = 0;
+	u32 hmb_data;
+	u8 fcbits;
 	uint retries = 0;
-	int ret = 0, prec_out;
-	uint cnt = 0;
-	uint datalen;
-	u8 tx_prec_map;
-
-	struct brcmf_pub *drvr = bus->drvr;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	tx_prec_map = ~bus->flowcontrol;
-
-	/* Send frames until the limit or some other event */
-	for (cnt = 0; (cnt < maxframes) && DATAOK(bus); cnt++) {
-		spin_lock_bh(&bus->txqlock);
-		pkt = brcmu_pktq_mdeq(&bus->txq, tx_prec_map, &prec_out);
-		if (pkt == NULL) {
-			spin_unlock_bh(&bus->txqlock);
-			break;
-		}
-		spin_unlock_bh(&bus->txqlock);
-		datalen = pkt->len - SDPCM_HDRLEN;
-
-#ifndef SDTEST
-		ret = brcmf_sdbrcm_txpkt(bus, pkt, SDPCM_DATA_CHANNEL, true);
-#else
-		ret = brcmf_sdbrcm_txpkt(bus, pkt,
-				    (bus->ext_loop ? SDPCM_TEST_CHANNEL :
-				     SDPCM_DATA_CHANNEL), true);
-#endif
-		if (ret)
-			bus->drvr->tx_errors++;
-		else
-			bus->drvr->dstats.tx_bytes += datalen;
-
-		/* In poll mode, need to check for other events */
-		if (!bus->intr && cnt) {
-			/* Check device status, signal pending interrupt */
-			r_sdreg32(bus, &intstatus,
-				  offsetof(struct sdpcmd_regs, intstatus),
-				  &retries);
-			bus->f2txdata++;
-			if (brcmf_sdcard_regfail(bus->card))
-				break;
-			if (intstatus & bus->hostintmask)
-				bus->ipend = true;
-		}
-	}
-
-	/* Deflow-control stack if needed */
-	if (drvr->up && (drvr->busstate == BRCMF_BUS_DATA) &&
-	    drvr->txoff && (pktq_len(&bus->txq) < TXLOW))
-		brcmf_txflowcontrol(drvr, 0, OFF);
-
-	return cnt;
-}
-
-int
-brcmf_sdbrcm_bus_txctl(struct brcmf_bus *bus, unsigned char *msg, uint msglen)
-{
-	u8 *frame;
-	u16 len;
-	u32 swheader;
-	uint retries = 0;
-	struct brcmf_sdio_card *card = bus->card;
-	u8 doff = 0;
-	int ret = -1;
-	int i;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	if (bus->drvr->dongle_reset)
-		return -EIO;
-
-	/* Back the pointer to make a room for bus header */
-	frame = msg - SDPCM_HDRLEN;
-	len = (msglen += SDPCM_HDRLEN);
-
-	/* Add alignment padding (optional for ctl frames) */
-	if (brcmf_alignctl) {
-		doff = ((unsigned long)frame % BRCMF_SDALIGN);
-		if (doff) {
-			frame -= doff;
-			len += doff;
-			msglen += doff;
-			memset(frame, 0, doff + SDPCM_HDRLEN);
-		}
-		/* precondition: doff < BRCMF_SDALIGN */
-	}
-	doff += SDPCM_HDRLEN;
-
-	/* Round send length to next SDIO block */
-	if (bus->roundup && bus->blocksize && (len > bus->blocksize)) {
-		u16 pad = bus->blocksize - (len % bus->blocksize);
-		if ((pad <= bus->roundup) && (pad < bus->blocksize))
-			len += pad;
-	} else if (len % BRCMF_SDALIGN) {
-		len += BRCMF_SDALIGN - (len % BRCMF_SDALIGN);
-	}
-
-	/* Satisfy length-alignment requirements */
-	if (forcealign && (len & (ALIGNMENT - 1)))
-		len = roundup(len, ALIGNMENT);
-
-	/* precondition: IS_ALIGNED((unsigned long)frame, 2) */
-
-	/* Need to lock here to protect txseq and SDIO tx calls */
-	brcmf_sdbrcm_sdlock(bus);
-
-	BUS_WAKE(bus);
-
-	/* Make sure backplane clock is on */
-	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
-
-	/* Hardware tag: 2 byte len followed by 2 byte ~len check (all LE) */
-	*(u16 *) frame = cpu_to_le16((u16) msglen);
-	*(((u16 *) frame) + 1) = cpu_to_le16(~msglen);
-
-	/* Software tag: channel, sequence number, data offset */
-	swheader =
-	    ((SDPCM_CONTROL_CHANNEL << SDPCM_CHANNEL_SHIFT) &
-	     SDPCM_CHANNEL_MASK)
-	    | bus->tx_seq | ((doff << SDPCM_DOFFSET_SHIFT) &
-			     SDPCM_DOFFSET_MASK);
-	put_unaligned_le32(swheader, frame + SDPCM_FRAMETAG_LEN);
-	put_unaligned_le32(0, frame + SDPCM_FRAMETAG_LEN + sizeof(swheader));
-
-	if (!DATAOK(bus)) {
-		BRCMF_INFO(("%s: No bus credit bus->tx_max %d,"
-			    " bus->tx_seq %d\n", __func__,
-			    bus->tx_max, bus->tx_seq));
-		bus->ctrl_frame_stat = true;
-		/* Send from dpc */
-		bus->ctrl_frame_buf = frame;
-		bus->ctrl_frame_len = len;
-
-		brcmf_sdbrcm_wait_for_event(bus, &bus->ctrl_frame_stat);
-
-		if (bus->ctrl_frame_stat == false) {
-			BRCMF_INFO(("%s: ctrl_frame_stat == false\n",
-				    __func__));
-			ret = 0;
-		} else {
-			BRCMF_INFO(("%s: ctrl_frame_stat == true\n", __func__));
-			ret = -1;
-		}
-	}
-
-	if (ret == -1) {
-#ifdef BCMDBG
-		if (BRCMF_BYTES_ON() && BRCMF_CTL_ON()) {
-			printk(KERN_DEBUG "Tx Frame:\n");
-			print_hex_dump_bytes("", DUMP_PREFIX_OFFSET,
-					     frame, len);
-		} else if (BRCMF_HDRS_ON()) {
-			printk(KERN_DEBUG "TxHdr:\n");
-			print_hex_dump_bytes("", DUMP_PREFIX_OFFSET,
-					     frame, min_t(u16, len, 16));
-		}
-#endif
-
-		do {
-			bus->ctrl_frame_stat = false;
-			ret = brcmf_sdbrcm_send_buf(bus,
-				brcmf_sdcard_cur_sbwad(card), SDIO_FUNC_2,
-				F2SYNC, frame, len, NULL, NULL, NULL);
-
-			if (ret < 0) {
-				/* On failure, abort the command and
-				 terminate the frame */
-				BRCMF_INFO(("%s: sdio error %d, abort command "
-					    "and terminate frame.\n",
-					    __func__, ret));
-				bus->tx_sderrs++;
-
-				brcmf_sdcard_abort(card, SDIO_FUNC_2);
-
-				brcmf_sdcard_cfg_write(card, SDIO_FUNC_1,
-						 SBSDIO_FUNC1_FRAMECTRL,
-						 SFC_WF_TERM, NULL);
-				bus->f1regdata++;
-
-				for (i = 0; i < 3; i++) {
-					u8 hi, lo;
-					hi = brcmf_sdcard_cfg_read(card,
-					     SDIO_FUNC_1,
-					     SBSDIO_FUNC1_WFRAMEBCHI,
-					     NULL);
-					lo = brcmf_sdcard_cfg_read(card,
-					     SDIO_FUNC_1,
-					     SBSDIO_FUNC1_WFRAMEBCLO,
-					     NULL);
-					bus->f1regdata += 2;
-					if ((hi == 0) && (lo == 0))
-						break;
-				}
-
-			}
-			if (ret == 0) {
-				bus->tx_seq =
-				    (bus->tx_seq + 1) % SDPCM_SEQUENCE_WRAP;
-			}
-		} while ((ret < 0) && retries++ < TXRETRIES);
-	}
-
-	if ((bus->idletime == BRCMF_IDLE_IMMEDIATE) && !bus->dpc_sched) {
-		bus->activity = false;
-		brcmf_sdbrcm_clkctl(bus, CLK_NONE, true);
-	}
-
-	brcmf_sdbrcm_sdunlock(bus);
-
-	if (ret)
-		bus->drvr->tx_ctlerrs++;
-	else
-		bus->drvr->tx_ctlpkts++;
-
-	return ret ? -EIO : 0;
-}
-
-int brcmf_sdbrcm_bus_rxctl(struct brcmf_bus *bus, unsigned char *msg, uint msglen)
-{
-	int timeleft;
-	uint rxlen = 0;
-	bool pending;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	if (bus->drvr->dongle_reset)
-		return -EIO;
-
-	/* Wait until control frame is available */
-	timeleft = brcmf_os_ioctl_resp_wait(bus->drvr, &bus->rxlen, &pending);
-
-	brcmf_sdbrcm_sdlock(bus);
-	rxlen = bus->rxlen;
-	memcpy(msg, bus->rxctl, min(msglen, rxlen));
-	bus->rxlen = 0;
-	brcmf_sdbrcm_sdunlock(bus);
-
-	if (rxlen) {
-		BRCMF_CTL(("%s: resumed on rxctl frame, got %d expected %d\n",
-			   __func__, rxlen, msglen));
-	} else if (timeleft == 0) {
-		BRCMF_ERROR(("%s: resumed on timeout\n", __func__));
-#ifdef BCMDBG
-		brcmf_sdbrcm_sdlock(bus);
-		brcmf_sdbrcm_checkdied(bus, NULL, 0);
-		brcmf_sdbrcm_sdunlock(bus);
-#endif				/* BCMDBG */
-	} else if (pending == true) {
-		BRCMF_CTL(("%s: cancelled\n", __func__));
-		return -ERESTARTSYS;
-	} else {
-		BRCMF_CTL(("%s: resumed for unknown reason?\n", __func__));
-#ifdef BCMDBG
-		brcmf_sdbrcm_sdlock(bus);
-		brcmf_sdbrcm_checkdied(bus, NULL, 0);
-		brcmf_sdbrcm_sdunlock(bus);
-#endif				/* BCMDBG */
-	}
-
-	if (rxlen)
-		bus->drvr->rx_ctlpkts++;
-	else
-		bus->drvr->rx_ctlerrs++;
-
-	return rxlen ? (int)rxlen : -ETIMEDOUT;
-}
-
-/* IOVar table */
-enum {
-	IOV_INTR = 1,
-	IOV_POLLRATE,
-	IOV_SDREG,
-	IOV_SBREG,
-	IOV_SDCIS,
-	IOV_MEMBYTES,
-	IOV_MEMSIZE,
-#ifdef BCMDBG
-	IOV_CHECKDIED,
-	IOV_CONS,
-	IOV_DCONSOLE_POLL,
-#endif
-	IOV_DOWNLOAD,
-	IOV_FORCEEVEN,
-	IOV_SDIOD_DRIVE,
-	IOV_READAHEAD,
-	IOV_SDRXCHAIN,
-	IOV_ALIGNCTL,
-	IOV_SDALIGN,
-	IOV_DEVRESET,
-	IOV_CPU,
-#ifdef SDTEST
-	IOV_PKTGEN,
-	IOV_EXTLOOP,
-#endif				/* SDTEST */
-	IOV_SPROM,
-	IOV_TXBOUND,
-	IOV_RXBOUND,
-	IOV_TXMINMAX,
-	IOV_IDLETIME,
-	IOV_IDLECLOCK,
-	IOV_SD1IDLE,
-	IOV_SLEEP,
-	IOV_WDTICK,
-	IOV_VARS
-};
-
-const struct brcmu_iovar brcmf_sdio_iovars[] = {
-	{"intr", IOV_INTR, 0, IOVT_BOOL, 0},
-	{"sleep", IOV_SLEEP, 0, IOVT_BOOL, 0},
-	{"pollrate", IOV_POLLRATE, 0, IOVT_UINT32, 0},
-	{"idletime", IOV_IDLETIME, 0, IOVT_INT32, 0},
-	{"idleclock", IOV_IDLECLOCK, 0, IOVT_INT32, 0},
-	{"sd1idle", IOV_SD1IDLE, 0, IOVT_BOOL, 0},
-	{"membytes", IOV_MEMBYTES, 0, IOVT_BUFFER, 2 * sizeof(int)},
-	{"memsize", IOV_MEMSIZE, 0, IOVT_UINT32, 0},
-	{"download", IOV_DOWNLOAD, 0, IOVT_BOOL, 0},
-	{"vars", IOV_VARS, 0, IOVT_BUFFER, 0},
-	{"sdiod_drive", IOV_SDIOD_DRIVE, 0, IOVT_UINT32, 0},
-	{"readahead", IOV_READAHEAD, 0, IOVT_BOOL, 0},
-	{"sdrxchain", IOV_SDRXCHAIN, 0, IOVT_BOOL, 0},
-	{"alignctl", IOV_ALIGNCTL, 0, IOVT_BOOL, 0},
-	{"sdalign", IOV_SDALIGN, 0, IOVT_BOOL, 0},
-	{"devreset", IOV_DEVRESET, 0, IOVT_BOOL, 0},
-	{"wdtick", IOV_WDTICK, 0, IOVT_UINT32, 0},
-#ifdef BCMDBG
-	{"cons", IOV_CONS, 0, IOVT_BUFFER, 0}
-	,
-	{"dconpoll", IOV_DCONSOLE_POLL, 0, IOVT_UINT32, 0}
-	,
-	{"sdreg", IOV_SDREG, 0, IOVT_BUFFER, sizeof(struct brcmf_sdreg)}
-	,
-	{"sbreg", IOV_SBREG, 0, IOVT_BUFFER, sizeof(struct brcmf_sdreg)}
-	,
-	{"sd_cis", IOV_SDCIS, 0, IOVT_BUFFER, BRCMF_IOCTL_MAXLEN}
-	,
-	{"forcealign", IOV_FORCEEVEN, 0, IOVT_BOOL, 0}
-	,
-	{"txbound", IOV_TXBOUND, 0, IOVT_UINT32, 0}
-	,
-	{"rxbound", IOV_RXBOUND, 0, IOVT_UINT32, 0}
-	,
-	{"txminmax", IOV_TXMINMAX, 0, IOVT_UINT32, 0}
-	,
-	{"cpu", IOV_CPU, 0, IOVT_BOOL, 0}
-	,
-	{"checkdied", IOV_CHECKDIED, 0, IOVT_BUFFER, 0}
-	,
-#endif				/* BCMDBG */
-#ifdef SDTEST
-	{"extloop", IOV_EXTLOOP, 0, IOVT_BOOL, 0}
-	,
-	{"pktgen", IOV_PKTGEN, 0, IOVT_BUFFER, sizeof(struct brcmf_pktgen)}
-	,
-#endif				/* SDTEST */
-
-	{NULL, 0, 0, 0, 0}
-};
-
-static void
-brcmf_dump_pct(struct brcmu_strbuf *strbuf, char *desc, uint num, uint div)
-{
-	uint q1, q2;
-
-	if (!div) {
-		brcmu_bprintf(strbuf, "%s N/A", desc);
-	} else {
-		q1 = num / div;
-		q2 = (100 * (num - (q1 * div))) / div;
-		brcmu_bprintf(strbuf, "%s %d.%02d", desc, q1, q2);
-	}
-}
 
-void brcmf_sdbrcm_bus_dump(struct brcmf_pub *drvr, struct brcmu_strbuf *strbuf)
-{
-	struct brcmf_bus *bus = drvr->bus;
-
-	brcmu_bprintf(strbuf, "Bus SDIO structure:\n");
-	brcmu_bprintf(strbuf,
-		    "hostintmask 0x%08x intstatus 0x%08x sdpcm_ver %d\n",
-		    bus->hostintmask, bus->intstatus, bus->sdpcm_ver);
-	brcmu_bprintf(strbuf,
-		    "fcstate %d qlen %d tx_seq %d, max %d, rxskip %d rxlen %d rx_seq %d\n",
-		    bus->fcstate, pktq_len(&bus->txq), bus->tx_seq, bus->tx_max,
-		    bus->rxskip, bus->rxlen, bus->rx_seq);
-	brcmu_bprintf(strbuf, "intr %d intrcount %d lastintrs %d spurious %d\n",
-		    bus->intr, bus->intrcount, bus->lastintrs, bus->spurious);
-	brcmu_bprintf(strbuf, "pollrate %d pollcnt %d regfails %d\n",
-		    bus->pollrate, bus->pollcnt, bus->regfails);
-
-	brcmu_bprintf(strbuf, "\nAdditional counters:\n");
-	brcmu_bprintf(strbuf,
-		    "tx_sderrs %d fcqueued %d rxrtx %d rx_toolong %d rxc_errors %d\n",
-		    bus->tx_sderrs, bus->fcqueued, bus->rxrtx, bus->rx_toolong,
-		    bus->rxc_errors);
-	brcmu_bprintf(strbuf, "rx_hdrfail %d badhdr %d badseq %d\n",
-		    bus->rx_hdrfail, bus->rx_badhdr, bus->rx_badseq);
-	brcmu_bprintf(strbuf, "fc_rcvd %d, fc_xoff %d, fc_xon %d\n",
-		      bus->fc_rcvd, bus->fc_xoff, bus->fc_xon);
-	brcmu_bprintf(strbuf, "rxglomfail %d, rxglomframes %d, rxglompkts %d\n",
-		    bus->rxglomfail, bus->rxglomframes, bus->rxglompkts);
-	brcmu_bprintf(strbuf, "f2rx (hdrs/data) %d (%d/%d), f2tx %d f1regs"
-		      " %d\n",
-		      (bus->f2rxhdrs + bus->f2rxdata), bus->f2rxhdrs,
-		      bus->f2rxdata, bus->f2txdata, bus->f1regdata);
-	{
-		brcmf_dump_pct(strbuf, "\nRx: pkts/f2rd", bus->drvr->rx_packets,
-			     (bus->f2rxhdrs + bus->f2rxdata));
-		brcmf_dump_pct(strbuf, ", pkts/f1sd", bus->drvr->rx_packets,
-			     bus->f1regdata);
-		brcmf_dump_pct(strbuf, ", pkts/sd", bus->drvr->rx_packets,
-			     (bus->f2rxhdrs + bus->f2rxdata + bus->f1regdata));
-		brcmf_dump_pct(strbuf, ", pkts/int", bus->drvr->rx_packets,
-			     bus->intrcount);
-		brcmu_bprintf(strbuf, "\n");
-
-		brcmf_dump_pct(strbuf, "Rx: glom pct", (100 * bus->rxglompkts),
-			     bus->drvr->rx_packets);
-		brcmf_dump_pct(strbuf, ", pkts/glom", bus->rxglompkts,
-			     bus->rxglomframes);
-		brcmu_bprintf(strbuf, "\n");
-
-		brcmf_dump_pct(strbuf, "Tx: pkts/f2wr", bus->drvr->tx_packets,
-			     bus->f2txdata);
-		brcmf_dump_pct(strbuf, ", pkts/f1sd", bus->drvr->tx_packets,
-			     bus->f1regdata);
-		brcmf_dump_pct(strbuf, ", pkts/sd", bus->drvr->tx_packets,
-			     (bus->f2txdata + bus->f1regdata));
-		brcmf_dump_pct(strbuf, ", pkts/int", bus->drvr->tx_packets,
-			     bus->intrcount);
-		brcmu_bprintf(strbuf, "\n");
-
-		brcmf_dump_pct(strbuf, "Total: pkts/f2rw",
-			     (bus->drvr->tx_packets + bus->drvr->rx_packets),
-			     (bus->f2txdata + bus->f2rxhdrs + bus->f2rxdata));
-		brcmf_dump_pct(strbuf, ", pkts/f1sd",
-			     (bus->drvr->tx_packets + bus->drvr->rx_packets),
-			     bus->f1regdata);
-		brcmf_dump_pct(strbuf, ", pkts/sd",
-			     (bus->drvr->tx_packets + bus->drvr->rx_packets),
-			     (bus->f2txdata + bus->f2rxhdrs + bus->f2rxdata +
-			      bus->f1regdata));
-		brcmf_dump_pct(strbuf, ", pkts/int",
-			     (bus->drvr->tx_packets + bus->drvr->rx_packets),
-			     bus->intrcount);
-		brcmu_bprintf(strbuf, "\n\n");
-	}
-
-#ifdef SDTEST
-	if (bus->pktgen_count) {
-		brcmu_bprintf(strbuf, "pktgen config and count:\n");
-		brcmu_bprintf(strbuf,
-			    "freq %d count %d print %d total %d min %d len %d\n",
-			    bus->pktgen_freq, bus->pktgen_count,
-			    bus->pktgen_print, bus->pktgen_total,
-			    bus->pktgen_minlen, bus->pktgen_maxlen);
-		brcmu_bprintf(strbuf, "send attempts %d rcvd %d fail %d\n",
-			    bus->pktgen_sent, bus->pktgen_rcvd,
-			    bus->pktgen_fail);
-	}
-#endif				/* SDTEST */
-#ifdef BCMDBG
-	brcmu_bprintf(strbuf, "dpc_sched %d host interrupt%spending\n",
-		      bus->dpc_sched, " not ");
-	brcmu_bprintf(strbuf, "blocksize %d roundup %d\n", bus->blocksize,
-		    bus->roundup);
-#endif				/* BCMDBG */
-	brcmu_bprintf(strbuf,
-		    "clkstate %d activity %d idletime %d idlecount %d sleeping %d\n",
-		    bus->clkstate, bus->activity, bus->idletime, bus->idlecount,
-		    bus->sleeping);
-}
-
-void brcmf_bus_clearcounts(struct brcmf_pub *drvr)
-{
-	struct brcmf_bus *bus = (struct brcmf_bus *) drvr->bus;
-
-	bus->intrcount = bus->lastintrs = bus->spurious = bus->regfails = 0;
-	bus->rxrtx = bus->rx_toolong = bus->rxc_errors = 0;
-	bus->rx_hdrfail = bus->rx_badhdr = bus->rx_badseq = 0;
-	bus->tx_sderrs = bus->fc_rcvd = bus->fc_xoff = bus->fc_xon = 0;
-	bus->rxglomfail = bus->rxglomframes = bus->rxglompkts = 0;
-	bus->f2rxhdrs = bus->f2rxdata = bus->f2txdata = bus->f1regdata = 0;
-}
+	brcmf_dbg(TRACE, "Enter\n");
 
-#ifdef SDTEST
-static int brcmf_sdbrcm_pktgen_get(struct brcmf_bus *bus, u8 *arg)
-{
-	struct brcmf_pktgen pktgen;
-
-	pktgen.version = BRCMF_PKTGEN_VERSION;
-	pktgen.freq = bus->pktgen_freq;
-	pktgen.count = bus->pktgen_count;
-	pktgen.print = bus->pktgen_print;
-	pktgen.total = bus->pktgen_total;
-	pktgen.minlen = bus->pktgen_minlen;
-	pktgen.maxlen = bus->pktgen_maxlen;
-	pktgen.numsent = bus->pktgen_sent;
-	pktgen.numrcvd = bus->pktgen_rcvd;
-	pktgen.numfail = bus->pktgen_fail;
-	pktgen.mode = bus->pktgen_mode;
-	pktgen.stop = bus->pktgen_stop;
-
-	memcpy(arg, &pktgen, sizeof(pktgen));
-
-	return 0;
-}
-
-static int brcmf_sdbrcm_pktgen_set(struct brcmf_bus *bus, u8 *arg)
-{
-	struct brcmf_pktgen pktgen;
-	uint oldcnt, oldmode;
-
-	memcpy(&pktgen, arg, sizeof(pktgen));
-	if (pktgen.version != BRCMF_PKTGEN_VERSION)
-		return -EINVAL;
-
-	oldcnt = bus->pktgen_count;
-	oldmode = bus->pktgen_mode;
-
-	bus->pktgen_freq = pktgen.freq;
-	bus->pktgen_count = pktgen.count;
-	bus->pktgen_print = pktgen.print;
-	bus->pktgen_total = pktgen.total;
-	bus->pktgen_minlen = pktgen.minlen;
-	bus->pktgen_maxlen = pktgen.maxlen;
-	bus->pktgen_mode = pktgen.mode;
-	bus->pktgen_stop = pktgen.stop;
-
-	bus->pktgen_tick = bus->pktgen_ptick = 0;
-	bus->pktgen_len = max(bus->pktgen_len, bus->pktgen_minlen);
-	bus->pktgen_len = min(bus->pktgen_len, bus->pktgen_maxlen);
-
-	/* Clear counts for a new pktgen (mode change, or was stopped) */
-	if (bus->pktgen_count && (!oldcnt || oldmode != bus->pktgen_mode))
-		bus->pktgen_sent = bus->pktgen_rcvd = bus->pktgen_fail = 0;
-
-	return 0;
-}
-#endif				/* SDTEST */
-
-static int
-brcmf_sdbrcm_membytes(struct brcmf_bus *bus, bool write, u32 address, u8 *data,
-		 uint size)
-{
-	int bcmerror = 0;
-	u32 sdaddr;
-	uint dsize;
-
-	/* Determine initial transfer parameters */
-	sdaddr = address & SBSDIO_SB_OFT_ADDR_MASK;
-	if ((sdaddr + size) & SBSDIO_SBWINDOW_MASK)
-		dsize = (SBSDIO_SB_OFT_ADDR_LIMIT - sdaddr);
-	else
-		dsize = size;
-
-	/* Set the backplane window to include the start address */
-	bcmerror = brcmf_sdbrcm_set_siaddr_window(bus, address);
-	if (bcmerror) {
-		BRCMF_ERROR(("%s: window change failed\n", __func__));
-		goto xfer_done;
-	}
+	/* Read mailbox data and ack that we did so */
+	r_sdreg32(bus, &hmb_data,
+		  offsetof(struct sdpcmd_regs, tohostmailboxdata), &retries);
 
-	/* Do the transfer(s) */
-	while (size) {
-		BRCMF_INFO(("%s: %s %d bytes at offset 0x%08x in window"
-			    " 0x%08x\n", __func__, (write ? "write" : "read"),
-			    dsize, sdaddr, (address & SBSDIO_SBWINDOW_MASK)));
-		bcmerror =
-		     brcmf_sdcard_rwdata(bus->card, write, sdaddr, data, dsize);
-		if (bcmerror) {
-			BRCMF_ERROR(("%s: membytes transfer failed\n",
-				     __func__));
-			break;
-		}
+	if (retries <= retry_limit)
+		w_sdreg32(bus, SMB_INT_ACK,
+			  offsetof(struct sdpcmd_regs, tosbmailbox), &retries);
+	bus->f1regdata += 2;
 
-		/* Adjust for next transfer (if any) */
-		size -= dsize;
-		if (size) {
-			data += dsize;
-			address += dsize;
-			bcmerror = brcmf_sdbrcm_set_siaddr_window(bus, address);
-			if (bcmerror) {
-				BRCMF_ERROR(("%s: window change failed\n",
-					     __func__));
-				break;
-			}
-			sdaddr = 0;
-			dsize = min_t(uint, SBSDIO_SB_OFT_ADDR_LIMIT, size);
-		}
-	}
+	/* Dongle recomposed rx frames, accept them again */
+	if (hmb_data & HMB_DATA_NAKHANDLED) {
+		brcmf_dbg(INFO, "Dongle reports NAK handled, expect rtx of %d\n",
+			  bus->rx_seq);
+		if (!bus->rxskip)
+			brcmf_dbg(ERROR, "unexpected NAKHANDLED!\n");
 
-xfer_done:
-	/* Return the window to backplane enumeration space for core access */
-	if (brcmf_sdbrcm_set_siaddr_window(bus,
-					   brcmf_sdcard_cur_sbwad(bus->card))) {
-		BRCMF_ERROR(("%s: FAILED to set window back to 0x%x\n",
-			     __func__, brcmf_sdcard_cur_sbwad(bus->card)));
+		bus->rxskip = false;
+		intstatus |= I_HMB_FRAME_IND;
 	}
 
-	return bcmerror;
-}
-
-#ifdef BCMDBG
-static int brcmf_sdbrcm_readshared(struct brcmf_bus *bus, struct sdpcm_shared *sh)
-{
-	u32 addr;
-	int rv;
-
-	/* Read last word in memory to determine address of
-			 sdpcm_shared structure */
-	rv = brcmf_sdbrcm_membytes(bus, false, bus->ramsize - 4, (u8 *)&addr,
-				   4);
-	if (rv < 0)
-		return rv;
-
-	addr = le32_to_cpu(addr);
-
-	BRCMF_INFO(("sdpcm_shared address 0x%08X\n", addr));
-
 	/*
-	 * Check if addr is valid.
-	 * NVRAM length at the end of memory should have been overwritten.
+	 * DEVREADY does not occur with gSPI.
 	 */
-	if (addr == 0 || ((~addr >> 16) & 0xffff) == (addr & 0xffff)) {
-		BRCMF_ERROR(("%s: address (0x%08x) of sdpcm_shared invalid\n",
-			     __func__, addr));
-		return -EBADE;
-	}
-
-	/* Read rte_shared structure */
-	rv = brcmf_sdbrcm_membytes(bus, false, addr, (u8 *) sh,
-			      sizeof(struct sdpcm_shared));
-	if (rv < 0)
-		return rv;
-
-	/* Endianness */
-	sh->flags = le32_to_cpu(sh->flags);
-	sh->trap_addr = le32_to_cpu(sh->trap_addr);
-	sh->assert_exp_addr = le32_to_cpu(sh->assert_exp_addr);
-	sh->assert_file_addr = le32_to_cpu(sh->assert_file_addr);
-	sh->assert_line = le32_to_cpu(sh->assert_line);
-	sh->console_addr = le32_to_cpu(sh->console_addr);
-	sh->msgtrace_addr = le32_to_cpu(sh->msgtrace_addr);
-
-	if ((sh->flags & SDPCM_SHARED_VERSION_MASK) != SDPCM_SHARED_VERSION) {
-		BRCMF_ERROR(("%s: sdpcm_shared version %d in brcmf "
-			     "is different than sdpcm_shared version %d in dongle\n",
-			     __func__, SDPCM_SHARED_VERSION,
-			     sh->flags & SDPCM_SHARED_VERSION_MASK));
-		return -EBADE;
-	}
-
-	return 0;
-}
-
-static int brcmf_sdbrcm_checkdied(struct brcmf_bus *bus, u8 *data, uint size)
-{
-	int bcmerror = 0;
-	uint msize = 512;
-	char *mbuffer = NULL;
-	uint maxstrlen = 256;
-	char *str = NULL;
-	struct brcmf_trap tr;
-	struct sdpcm_shared sdpcm_shared;
-	struct brcmu_strbuf strbuf;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	if (data == NULL) {
-		/*
-		 * Called after a rx ctrl timeout. "data" is NULL.
-		 * allocate memory to trace the trap or assert.
-		 */
-		size = msize;
-		mbuffer = data = kmalloc(msize, GFP_ATOMIC);
-		if (mbuffer == NULL) {
-			BRCMF_ERROR(("%s: kmalloc(%d) failed\n", __func__,
-				     msize));
-			bcmerror = -ENOMEM;
-			goto done;
-		}
-	}
-
-	str = kmalloc(maxstrlen, GFP_ATOMIC);
-	if (str == NULL) {
-		BRCMF_ERROR(("%s: kmalloc(%d) failed\n", __func__, maxstrlen));
-		bcmerror = -ENOMEM;
-		goto done;
-	}
-
-	bcmerror = brcmf_sdbrcm_readshared(bus, &sdpcm_shared);
-	if (bcmerror < 0)
-		goto done;
-
-	brcmu_binit(&strbuf, data, size);
-
-	brcmu_bprintf(&strbuf,
-		    "msgtrace address : 0x%08X\nconsole address  : 0x%08X\n",
-		    sdpcm_shared.msgtrace_addr, sdpcm_shared.console_addr);
-
-	if ((sdpcm_shared.flags & SDPCM_SHARED_ASSERT_BUILT) == 0) {
-		/* NOTE: Misspelled assert is intentional - DO NOT FIX.
-		 * (Avoids conflict with real asserts for programmatic
-		 * parsing of output.)
-		 */
-		brcmu_bprintf(&strbuf, "Assrt not built in dongle\n");
-	}
-
-	if ((sdpcm_shared.flags & (SDPCM_SHARED_ASSERT | SDPCM_SHARED_TRAP)) ==
-	    0) {
-		/* NOTE: Misspelled assert is intentional - DO NOT FIX.
-		 * (Avoids conflict with real asserts for programmatic
-		 * parsing of output.)
-		 */
-		brcmu_bprintf(&strbuf, "No trap%s in dongle",
-			    (sdpcm_shared.flags & SDPCM_SHARED_ASSERT_BUILT)
-			    ? "/assrt" : "");
-	} else {
-		if (sdpcm_shared.flags & SDPCM_SHARED_ASSERT) {
-			/* Download assert */
-			brcmu_bprintf(&strbuf, "Dongle assert");
-			if (sdpcm_shared.assert_exp_addr != 0) {
-				str[0] = '\0';
-				bcmerror = brcmf_sdbrcm_membytes(bus, false,
-						sdpcm_shared.assert_exp_addr,
-						(u8 *) str, maxstrlen);
-				if (bcmerror < 0)
-					goto done;
-
-				str[maxstrlen - 1] = '\0';
-				brcmu_bprintf(&strbuf, " expr \"%s\"", str);
-			}
-
-			if (sdpcm_shared.assert_file_addr != 0) {
-				str[0] = '\0';
-				bcmerror = brcmf_sdbrcm_membytes(bus, false,
-						sdpcm_shared.assert_file_addr,
-						(u8 *) str, maxstrlen);
-				if (bcmerror < 0)
-					goto done;
-
-				str[maxstrlen - 1] = '\0';
-				brcmu_bprintf(&strbuf, " file \"%s\"", str);
-			}
-
-			brcmu_bprintf(&strbuf, " line %d ",
-				    sdpcm_shared.assert_line);
-		}
-
-		if (sdpcm_shared.flags & SDPCM_SHARED_TRAP) {
-			bcmerror = brcmf_sdbrcm_membytes(bus, false,
-					sdpcm_shared.trap_addr, (u8 *)&tr,
-					sizeof(struct brcmf_trap));
-			if (bcmerror < 0)
-				goto done;
-
-			brcmu_bprintf(&strbuf,
-				    "Dongle trap type 0x%x @ epc 0x%x, cpsr 0x%x, spsr 0x%x, sp 0x%x,"
-				    "lp 0x%x, rpc 0x%x Trap offset 0x%x, "
-				    "r0 0x%x, r1 0x%x, r2 0x%x, r3 0x%x, r4 0x%x, r5 0x%x, r6 0x%x, r7 0x%x\n",
-				    tr.type, tr.epc, tr.cpsr, tr.spsr, tr.r13,
-				    tr.r14, tr.pc, sdpcm_shared.trap_addr,
-				    tr.r0, tr.r1, tr.r2, tr.r3, tr.r4, tr.r5,
-				    tr.r6, tr.r7);
-		}
-	}
-
-	if (sdpcm_shared.flags & (SDPCM_SHARED_ASSERT | SDPCM_SHARED_TRAP))
-		BRCMF_ERROR(("%s: %s\n", __func__, strbuf.origbuf));
-
-#ifdef BCMDBG
-	if (sdpcm_shared.flags & SDPCM_SHARED_TRAP) {
-		/* Mem dump to a file on device */
-		brcmf_sdbrcm_mem_dump(bus);
-	}
-#endif				/* BCMDBG */
-
-done:
-	kfree(mbuffer);
-	kfree(str);
-
-	return bcmerror;
-}
-
-static int brcmf_sdbrcm_mem_dump(struct brcmf_bus *bus)
-{
-	int ret = 0;
-	int size;		/* Full mem size */
-	int start = 0;		/* Start address */
-	int read_size = 0;	/* Read size of each iteration */
-	u8 *buf = NULL, *databuf = NULL;
-
-	/* Get full mem size */
-	size = bus->ramsize;
-	buf = kmalloc(size, GFP_ATOMIC);
-	if (!buf) {
-		BRCMF_ERROR(("%s: Out of memory (%d bytes)\n", __func__, size));
-		return -1;
-	}
-
-	/* Read mem content */
-	printk(KERN_DEBUG "Dump dongle memory");
-	databuf = buf;
-	while (size) {
-		read_size = min(MEMBLOCK, size);
-		ret = brcmf_sdbrcm_membytes(bus, false, start, databuf,
-					  read_size);
-		if (ret) {
-			BRCMF_ERROR(("%s: Error membytes %d\n", __func__, ret));
-			kfree(buf);
-			return -1;
-		}
-		printk(".");
-
-		/* Decrement size and increment start address */
-		size -= read_size;
-		start += read_size;
-		databuf += read_size;
-	}
-	printk(KERN_DEBUG "Done\n");
-
-	/* free buf before return !!! */
-	if (brcmf_write_to_file(bus->drvr, buf, bus->ramsize)) {
-		BRCMF_ERROR(("%s: Error writing to files\n", __func__));
-		return -1;
-	}
-
-	/* buf free handled in brcmf_write_to_file, not here */
-	return 0;
-}
-
-#define CONSOLE_LINE_MAX	192
-
-static int brcmf_sdbrcm_readconsole(struct brcmf_bus *bus)
-{
-	struct brcmf_console *c = &bus->console;
-	u8 line[CONSOLE_LINE_MAX], ch;
-	u32 n, idx, addr;
-	int rv;
-
-	/* Don't do anything until FWREADY updates console address */
-	if (bus->console_addr == 0)
-		return 0;
-
-	/* Read console log struct */
-	addr = bus->console_addr + offsetof(struct rte_console, log);
-	rv = brcmf_sdbrcm_membytes(bus, false, addr, (u8 *)&c->log,
-				sizeof(c->log));
-	if (rv < 0)
-		return rv;
-
-	/* Allocate console buffer (one time only) */
-	if (c->buf == NULL) {
-		c->bufsize = le32_to_cpu(c->log.buf_size);
-		c->buf = kmalloc(c->bufsize, GFP_ATOMIC);
-		if (c->buf == NULL)
-			return -ENOMEM;
-	}
-
-	idx = le32_to_cpu(c->log.idx);
-
-	/* Protect against corrupt value */
-	if (idx > c->bufsize)
-		return -EBADE;
-
-	/* Skip reading the console buffer if the index pointer
-	 has not moved */
-	if (idx == c->last)
-		return 0;
-
-	/* Read the console buffer */
-	addr = le32_to_cpu(c->log.buf);
-	rv = brcmf_sdbrcm_membytes(bus, false, addr, c->buf, c->bufsize);
-	if (rv < 0)
-		return rv;
-
-	while (c->last != idx) {
-		for (n = 0; n < CONSOLE_LINE_MAX - 2; n++) {
-			if (c->last == idx) {
-				/* This would output a partial line.
-				 * Instead, back up
-				 * the buffer pointer and output this
-				 * line next time around.
-				 */
-				if (c->last >= n)
-					c->last -= n;
-				else
-					c->last = c->bufsize - n;
-				goto break2;
-			}
-			ch = c->buf[c->last];
-			c->last = (c->last + 1) % c->bufsize;
-			if (ch == '\n')
-				break;
-			line[n] = ch;
-		}
-
-		if (n > 0) {
-			if (line[n - 1] == '\r')
-				n--;
-			line[n] = 0;
-			printk(KERN_DEBUG "CONSOLE: %s\n", line);
-		}
-	}
-break2:
-
-	return 0;
-}
-#endif				/* BCMDBG */
-
-int brcmf_sdbrcm_downloadvars(struct brcmf_bus *bus, void *arg, int len)
-{
-	int bcmerror = 0;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	/* Basic sanity checks */
-	if (bus->drvr->up) {
-		bcmerror = -EISCONN;
-		goto err;
-	}
-	if (!len) {
-		bcmerror = -EOVERFLOW;
-		goto err;
-	}
-
-	/* Free the old ones and replace with passed variables */
-	kfree(bus->vars);
-
-	bus->vars = kmalloc(len, GFP_ATOMIC);
-	bus->varsz = bus->vars ? len : 0;
-	if (bus->vars == NULL) {
-		bcmerror = -ENOMEM;
-		goto err;
-	}
-
-	/* Copy the passed variables, which should include the
-		 terminating double-null */
-	memcpy(bus->vars, arg, bus->varsz);
-err:
-	return bcmerror;
-}
-
-static int
-brcmf_sdbrcm_doiovar(struct brcmf_bus *bus, const struct brcmu_iovar *vi, u32 actionid,
-		const char *name, void *params, int plen, void *arg, int len,
-		int val_size)
-{
-	int bcmerror = 0;
-	s32 int_val = 0;
-	bool bool_val = 0;
-
-	BRCMF_TRACE(("%s: Enter, action %d name %s params %p plen %d arg %p "
-		     "len %d val_size %d\n", __func__, actionid, name, params,
-		     plen, arg, len, val_size));
-
-	bcmerror = brcmu_iovar_lencheck(vi, arg, len, IOV_ISSET(actionid));
-	if (bcmerror != 0)
-		goto exit;
-
-	if (plen >= (int)sizeof(int_val))
-		memcpy(&int_val, params, sizeof(int_val));
-
-	bool_val = (int_val != 0) ? true : false;
-
-	/* Some ioctls use the bus */
-	brcmf_sdbrcm_sdlock(bus);
-
-	/* Check if dongle is in reset. If so, only allow DEVRESET iovars */
-	if (bus->drvr->dongle_reset && !(actionid == IOV_SVAL(IOV_DEVRESET) ||
-					actionid == IOV_GVAL(IOV_DEVRESET))) {
-		bcmerror = -EPERM;
-		goto exit;
-	}
-
-	/* Handle sleep stuff before any clock mucking */
-	if (vi->varid == IOV_SLEEP) {
-		if (IOV_ISSET(actionid)) {
-			bcmerror = brcmf_sdbrcm_bussleep(bus, bool_val);
-		} else {
-			int_val = (s32) bus->sleeping;
-			memcpy(arg, &int_val, val_size);
-		}
-		goto exit;
-	}
-
-	/* Request clock to allow SDIO accesses */
-	if (!bus->drvr->dongle_reset) {
-		BUS_WAKE(bus);
-		brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
-	}
-
-	switch (actionid) {
-	case IOV_GVAL(IOV_INTR):
-		int_val = (s32) bus->intr;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_INTR):
-		bus->intr = bool_val;
-		bus->intdis = false;
-		if (bus->drvr->up) {
-			BRCMF_INTR(("%s: %s SDIO interrupts\n", __func__,
-				    bus->intr ? "enable" : "disable"));
-			if (bus->intr) {
-				brcmf_sdcard_intr_enable(bus->card);
-			} else {
-				brcmf_sdcard_intr_disable(bus->card);
-			}
-		}
-		break;
-
-	case IOV_GVAL(IOV_POLLRATE):
-		int_val = (s32) bus->pollrate;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_POLLRATE):
-		bus->pollrate = (uint) int_val;
-		bus->poll = (bus->pollrate != 0);
-		break;
-
-	case IOV_GVAL(IOV_IDLETIME):
-		int_val = bus->idletime;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_IDLETIME):
-		if ((int_val < 0) && (int_val != BRCMF_IDLE_IMMEDIATE))
-			bcmerror = -EINVAL;
-		else
-			bus->idletime = int_val;
-		break;
-
-	case IOV_GVAL(IOV_IDLECLOCK):
-		int_val = (s32) bus->idleclock;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_IDLECLOCK):
-		bus->idleclock = int_val;
-		break;
-
-	case IOV_GVAL(IOV_SD1IDLE):
-		int_val = (s32) sd1idle;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_SD1IDLE):
-		sd1idle = bool_val;
-		break;
-
-	case IOV_SVAL(IOV_MEMBYTES):
-	case IOV_GVAL(IOV_MEMBYTES):
-		{
-			u32 address;
-			uint size, dsize;
-			u8 *data;
-
-			bool set = (actionid == IOV_SVAL(IOV_MEMBYTES));
-
-			address = (u32) int_val;
-			memcpy(&int_val, (char *)params + sizeof(int_val),
-			       sizeof(int_val));
-			size = (uint) int_val;
-
-			/* Do some validation */
-			dsize = set ? plen - (2 * sizeof(int)) : len;
-			if (dsize < size) {
-				BRCMF_ERROR(("%s: error on %s membytes, addr "
-					     "0x%08x size %d dsize %d\n",
-					     __func__, (set ? "set" : "get"),
-					     address, size, dsize));
-				bcmerror = -EINVAL;
-				break;
-			}
-
-			BRCMF_INFO(("%s: Request to %s %d bytes at address "
-				    "0x%08x\n", __func__,
-				    (set ? "write" : "read"), size, address));
-
-			/* If we know about SOCRAM, check for a fit */
-			if ((bus->orig_ramsize) &&
-			    ((address > bus->orig_ramsize)
-			     || (address + size > bus->orig_ramsize))) {
-				BRCMF_ERROR(("%s: ramsize 0x%08x doesn't have"
-					     " %d bytes at 0x%08x\n", __func__,
-					     bus->orig_ramsize, size, address));
-				bcmerror = -EINVAL;
-				break;
-			}
-
-			/* Generate the actual data pointer */
-			data =
-			    set ? (u8 *) params +
-			    2 * sizeof(int) : (u8 *) arg;
-
-			/* Call to do the transfer */
-			bcmerror = brcmf_sdbrcm_membytes(bus, set, address,
-							 data, size);
-
-			break;
-		}
-
-	case IOV_GVAL(IOV_MEMSIZE):
-		int_val = (s32) bus->ramsize;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_GVAL(IOV_SDIOD_DRIVE):
-		int_val = (s32) brcmf_sdiod_drive_strength;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_SDIOD_DRIVE):
-		brcmf_sdiod_drive_strength = int_val;
-		brcmf_sdbrcm_sdiod_drive_strength_init(bus,
-					     brcmf_sdiod_drive_strength);
-		break;
-
-	case IOV_SVAL(IOV_DOWNLOAD):
-		bcmerror = brcmf_sdbrcm_download_state(bus, bool_val);
-		break;
-
-	case IOV_SVAL(IOV_VARS):
-		bcmerror = brcmf_sdbrcm_downloadvars(bus, arg, len);
-		break;
-
-	case IOV_GVAL(IOV_READAHEAD):
-		int_val = (s32) brcmf_readahead;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_READAHEAD):
-		if (bool_val && !brcmf_readahead)
-			bus->nextlen = 0;
-		brcmf_readahead = bool_val;
-		break;
-
-	case IOV_GVAL(IOV_SDRXCHAIN):
-		int_val = (s32) bus->use_rxchain;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_SDRXCHAIN):
-		if (bool_val && !bus->sd_rxchain)
-			bcmerror = -ENOTSUPP;
-		else
-			bus->use_rxchain = bool_val;
-		break;
-	case IOV_GVAL(IOV_ALIGNCTL):
-		int_val = (s32) brcmf_alignctl;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_ALIGNCTL):
-		brcmf_alignctl = bool_val;
-		break;
-
-	case IOV_GVAL(IOV_SDALIGN):
-		int_val = BRCMF_SDALIGN;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-#ifdef BCMDBG
-	case IOV_GVAL(IOV_VARS):
-		if (bus->varsz < (uint) len)
-			memcpy(arg, bus->vars, bus->varsz);
+	if (hmb_data & (HMB_DATA_DEVREADY | HMB_DATA_FWREADY)) {
+		bus->sdpcm_ver =
+		    (hmb_data & HMB_DATA_VERSION_MASK) >>
+		    HMB_DATA_VERSION_SHIFT;
+		if (bus->sdpcm_ver != SDPCM_PROT_VERSION)
+			brcmf_dbg(ERROR, "Version mismatch, dongle reports %d, "
+				  "expecting %d\n",
+				  bus->sdpcm_ver, SDPCM_PROT_VERSION);
 		else
-			bcmerror = -EOVERFLOW;
-		break;
-#endif				/* BCMDBG */
-
-#ifdef BCMDBG
-	case IOV_GVAL(IOV_DCONSOLE_POLL):
-		int_val = (s32) brcmf_console_ms;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_DCONSOLE_POLL):
-		brcmf_console_ms = (uint) int_val;
-		break;
-
-	case IOV_SVAL(IOV_CONS):
-		if (len > 0)
-			bcmerror = brcmf_sdbrcm_bus_console_in(bus->drvr,
-							       arg, len - 1);
-		break;
-
-	case IOV_GVAL(IOV_SDREG):
-		{
-			struct brcmf_sdreg *sd_ptr;
-			u32 addr, size;
-
-			sd_ptr = (struct brcmf_sdreg *) params;
-
-			addr = bus->ci->buscorebase + sd_ptr->offset;
-			size = sd_ptr->func;
-			int_val = (s32) brcmf_sdcard_reg_read(bus->card, addr,
-							      size);
-			if (brcmf_sdcard_regfail(bus->card))
-				bcmerror = -EIO;
-			memcpy(arg, &int_val, sizeof(s32));
-			break;
-		}
-
-	case IOV_SVAL(IOV_SDREG):
-		{
-			struct brcmf_sdreg *sd_ptr;
-			u32 addr, size;
-
-			sd_ptr = (struct brcmf_sdreg *) params;
-
-			addr = bus->ci->buscorebase + sd_ptr->offset;
-			size = sd_ptr->func;
-			brcmf_sdcard_reg_write(bus->card, addr, size,
-					       sd_ptr->value);
-			if (brcmf_sdcard_regfail(bus->card))
-				bcmerror = -EIO;
-			break;
-		}
-
-		/* Same as above, but offset is not backplane
-		 (not SDIO core) */
-	case IOV_GVAL(IOV_SBREG):
-		{
-			struct brcmf_sdreg sdreg;
-			u32 addr, size;
-
-			memcpy(&sdreg, params, sizeof(sdreg));
-
-			addr = SI_ENUM_BASE + sdreg.offset;
-			size = sdreg.func;
-			int_val = (s32) brcmf_sdcard_reg_read(bus->card, addr,
-							      size);
-			if (brcmf_sdcard_regfail(bus->card))
-				bcmerror = -EIO;
-			memcpy(arg, &int_val, sizeof(s32));
-			break;
-		}
-
-	case IOV_SVAL(IOV_SBREG):
-		{
-			struct brcmf_sdreg sdreg;
-			u32 addr, size;
-
-			memcpy(&sdreg, params, sizeof(sdreg));
-
-			addr = SI_ENUM_BASE + sdreg.offset;
-			size = sdreg.func;
-			brcmf_sdcard_reg_write(bus->card, addr, size,
-					       sdreg.value);
-			if (brcmf_sdcard_regfail(bus->card))
-				bcmerror = -EIO;
-			break;
-		}
-
-	case IOV_GVAL(IOV_SDCIS):
-		{
-			*(char *)arg = 0;
-
-			strcat(arg, "\nFunc 0\n");
-			brcmf_sdcard_cis_read(bus->card, 0x10,
-					(u8 *) arg + strlen(arg),
-					SBSDIO_CIS_SIZE_LIMIT);
-			strcat(arg, "\nFunc 1\n");
-			brcmf_sdcard_cis_read(bus->card, 0x11,
-					(u8 *) arg + strlen(arg),
-					SBSDIO_CIS_SIZE_LIMIT);
-			strcat(arg, "\nFunc 2\n");
-			brcmf_sdcard_cis_read(bus->card, 0x12,
-					(u8 *) arg + strlen(arg),
-					SBSDIO_CIS_SIZE_LIMIT);
-			break;
-		}
-
-	case IOV_GVAL(IOV_FORCEEVEN):
-		int_val = (s32) forcealign;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_FORCEEVEN):
-		forcealign = bool_val;
-		break;
-
-	case IOV_GVAL(IOV_TXBOUND):
-		int_val = (s32) brcmf_txbound;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_TXBOUND):
-		brcmf_txbound = (uint) int_val;
-		break;
-
-	case IOV_GVAL(IOV_RXBOUND):
-		int_val = (s32) brcmf_rxbound;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_RXBOUND):
-		brcmf_rxbound = (uint) int_val;
-		break;
-
-	case IOV_GVAL(IOV_TXMINMAX):
-		int_val = (s32) brcmf_txminmax;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_TXMINMAX):
-		brcmf_txminmax = (uint) int_val;
-		break;
-#endif				/* BCMDBG */
-
-#ifdef SDTEST
-	case IOV_GVAL(IOV_EXTLOOP):
-		int_val = (s32) bus->ext_loop;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_EXTLOOP):
-		bus->ext_loop = bool_val;
-		break;
-
-	case IOV_GVAL(IOV_PKTGEN):
-		bcmerror = brcmf_sdbrcm_pktgen_get(bus, arg);
-		break;
-
-	case IOV_SVAL(IOV_PKTGEN):
-		bcmerror = brcmf_sdbrcm_pktgen_set(bus, arg);
-		break;
-#endif				/* SDTEST */
-
-	case IOV_SVAL(IOV_DEVRESET):
-		BRCMF_TRACE(("%s: Called set IOV_DEVRESET=%d dongle_reset=%d "
-			     "busstate=%d\n",
-			     __func__, bool_val, bus->drvr->dongle_reset,
-			     bus->drvr->busstate));
-
-		brcmf_bus_devreset(bus->drvr, (u8) bool_val);
-
-		break;
-
-	case IOV_GVAL(IOV_DEVRESET):
-		BRCMF_TRACE(("%s: Called get IOV_DEVRESET\n", __func__));
-
-		/* Get its status */
-		int_val = (bool) bus->drvr->dongle_reset;
-		memcpy(arg, &int_val, val_size);
-
-		break;
-
-	case IOV_GVAL(IOV_WDTICK):
-		int_val = (s32) brcmf_watchdog_ms;
-		memcpy(arg, &int_val, val_size);
-		break;
-
-	case IOV_SVAL(IOV_WDTICK):
-		if (!bus->drvr->up) {
-			bcmerror = -ENOLINK;
-			break;
-		}
-		brcmf_sdbrcm_wd_timer(bus, (uint) int_val);
-		break;
-
-	default:
-		bcmerror = -ENOTSUPP;
-		break;
+			brcmf_dbg(INFO, "Dongle ready, protocol version %d\n",
+				  bus->sdpcm_ver);
 	}
 
-exit:
-	if ((bus->idletime == BRCMF_IDLE_IMMEDIATE) && !bus->dpc_sched) {
-		bus->activity = false;
-		brcmf_sdbrcm_clkctl(bus, CLK_NONE, true);
-	}
-
-	brcmf_sdbrcm_sdunlock(bus);
-
-	if (actionid == IOV_SVAL(IOV_DEVRESET) && bool_val == false)
-		brcmf_c_preinit_ioctls(bus->drvr);
-
-	return bcmerror;
-}
-
-static int brcmf_sdbrcm_write_vars(struct brcmf_bus *bus)
-{
-	int bcmerror = 0;
-	u32 varsize;
-	u32 varaddr;
-	u8 *vbuffer;
-	u32 varsizew;
-#ifdef BCMDBG
-	char *nvram_ularray;
-#endif				/* BCMDBG */
-
-	/* Even if there are no vars are to be written, we still
-		 need to set the ramsize. */
-	varsize = bus->varsz ? roundup(bus->varsz, 4) : 0;
-	varaddr = (bus->ramsize - 4) - varsize;
-
-	if (bus->vars) {
-		vbuffer = kzalloc(varsize, GFP_ATOMIC);
-		if (!vbuffer)
-			return -ENOMEM;
-
-		memcpy(vbuffer, bus->vars, bus->varsz);
-
-		/* Write the vars list */
-		bcmerror =
-		    brcmf_sdbrcm_membytes(bus, true, varaddr, vbuffer, varsize);
-#ifdef BCMDBG
-		/* Verify NVRAM bytes */
-		BRCMF_INFO(("Compare NVRAM dl & ul; varsize=%d\n", varsize));
-		nvram_ularray = kmalloc(varsize, GFP_ATOMIC);
-		if (!nvram_ularray)
-			return -ENOMEM;
-
-		/* Upload image to verify downloaded contents. */
-		memset(nvram_ularray, 0xaa, varsize);
-
-		/* Read the vars list to temp buffer for comparison */
-		bcmerror =
-		    brcmf_sdbrcm_membytes(bus, false, varaddr, nvram_ularray,
-				     varsize);
-		if (bcmerror) {
-			BRCMF_ERROR(("%s: error %d on reading %d nvram bytes"
-				     " at 0x%08x\n", __func__, bcmerror,
-				     varsize, varaddr));
-		}
-		/* Compare the org NVRAM with the one read from RAM */
-		if (memcmp(vbuffer, nvram_ularray, varsize)) {
-			BRCMF_ERROR(("%s: Downloaded NVRAM image is "
-				     "corrupted.\n", __func__));
-		} else
-			BRCMF_ERROR(("%s: Download/Upload/Compare of"
-				     " NVRAM ok.\n", __func__));
-
-		kfree(nvram_ularray);
-#endif				/* BCMDBG */
-
-		kfree(vbuffer);
-	}
-
-	/* adjust to the user specified RAM */
-	BRCMF_INFO(("Physical memory size: %d, usable memory size: %d\n",
-		    bus->orig_ramsize, bus->ramsize));
-	BRCMF_INFO(("Vars are at %d, orig varsize is %d\n", varaddr, varsize));
-	varsize = ((bus->orig_ramsize - 4) - varaddr);
-
 	/*
-	 * Determine the length token:
-	 * Varsize, converted to words, in lower 16-bits, checksum
-	 * in upper 16-bits.
-	 */
-	if (bcmerror) {
-		varsizew = 0;
-	} else {
-		varsizew = varsize / 4;
-		varsizew = (~varsizew << 16) | (varsizew & 0x0000FFFF);
-		varsizew = cpu_to_le32(varsizew);
-	}
-
-	BRCMF_INFO(("New varsize is %d, length token=0x%08x\n", varsize,
-		    varsizew));
-
-	/* Write the length token to the last word */
-	bcmerror = brcmf_sdbrcm_membytes(bus, true, (bus->orig_ramsize - 4),
-				    (u8 *)&varsizew, 4);
-
-	return bcmerror;
-}
-
-static int brcmf_sdbrcm_download_state(struct brcmf_bus *bus, bool enter)
-{
-	uint retries;
-	u32 regdata;
-	int bcmerror = 0;
-
-	/* To enter download state, disable ARM and reset SOCRAM.
-	 * To exit download state, simply reset ARM (default is RAM boot).
-	 */
-	if (enter) {
-		bus->alp_only = true;
-
-		brcmf_sdbrcm_chip_disablecore(bus->card, bus->ci->armcorebase);
-
-		brcmf_sdbrcm_chip_resetcore(bus->card, bus->ci->ramcorebase);
-
-		/* Clear the top bit of memory */
-		if (bus->ramsize) {
-			u32 zeros = 0;
-			brcmf_sdbrcm_membytes(bus, true, bus->ramsize - 4,
-					 (u8 *)&zeros, 4);
-		}
-	} else {
-		regdata = brcmf_sdcard_reg_read(bus->card,
-			CORE_SB(bus->ci->ramcorebase, sbtmstatelow), 4);
-		regdata &= (SBTML_RESET | SBTML_REJ_MASK |
-			(SICF_CLOCK_EN << SBTML_SICF_SHIFT));
-		if ((SICF_CLOCK_EN << SBTML_SICF_SHIFT) != regdata) {
-			BRCMF_ERROR(("%s: SOCRAM core is down after reset?\n",
-				     __func__));
-			bcmerror = -EBADE;
-			goto fail;
-		}
-
-		bcmerror = brcmf_sdbrcm_write_vars(bus);
-		if (bcmerror) {
-			BRCMF_ERROR(("%s: no vars written to RAM\n", __func__));
-			bcmerror = 0;
-		}
-
-		w_sdreg32(bus, 0xFFFFFFFF,
-			  offsetof(struct sdpcmd_regs, intstatus), &retries);
-
-		brcmf_sdbrcm_chip_resetcore(bus->card, bus->ci->armcorebase);
-
-		/* Allow HT Clock now that the ARM is running. */
-		bus->alp_only = false;
-
-		bus->drvr->busstate = BRCMF_BUS_LOAD;
-	}
-fail:
-	return bcmerror;
-}
-
-int
-brcmf_sdbrcm_bus_iovar_op(struct brcmf_pub *drvr, const char *name,
-			  void *params, int plen, void *arg, int len, bool set)
-{
-	struct brcmf_bus *bus = drvr->bus;
-	const struct brcmu_iovar *vi = NULL;
-	int bcmerror = 0;
-	int val_size;
-	u32 actionid;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	if (name == NULL || len <= 0)
-		return -EINVAL;
-
-	/* Set does not take qualifiers */
-	if (set && (params || plen))
-		return -EINVAL;
-
-	/* Get must have return space;*/
-	if (!set && !(arg && len))
-		return -EINVAL;
-
-	/* Look up var locally; if not found pass to host driver */
-	vi = brcmu_iovar_lookup(brcmf_sdio_iovars, name);
-	if (vi == NULL) {
-		brcmf_sdbrcm_sdlock(bus);
-
-		BUS_WAKE(bus);
-
-		/* Turn on clock in case SD command needs backplane */
-		brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
-
-		bcmerror = brcmf_sdcard_iovar_op(bus->card, name, params, plen,
-						 arg, len, set);
-
-		/* Similar check for blocksize change */
-		if (set && strcmp(name, "sd_blocksize") == 0) {
-			s32 fnum = 2;
-			if (brcmf_sdcard_iovar_op
-			    (bus->card, "sd_blocksize", &fnum, sizeof(s32),
-			     &bus->blocksize, sizeof(s32),
-			     false) != 0) {
-				bus->blocksize = 0;
-				BRCMF_ERROR(("%s: fail on %s get\n", __func__,
-					     "sd_blocksize"));
-			} else {
-				BRCMF_INFO(("%s: noted sd_blocksize update,"
-					    " value now %d\n", __func__,
-					    bus->blocksize));
-			}
-		}
-		bus->roundup = min(max_roundup, bus->blocksize);
-
-		if (bus->idletime == BRCMF_IDLE_IMMEDIATE &&
-		    !bus->dpc_sched) {
-			bus->activity = false;
-			brcmf_sdbrcm_clkctl(bus, CLK_NONE, true);
-		}
-
-		brcmf_sdbrcm_sdunlock(bus);
-		goto exit;
-	}
-
-	BRCMF_CTL(("%s: %s %s, len %d plen %d\n", __func__,
-		   name, (set ? "set" : "get"), len, plen));
-
-	/* set up 'params' pointer in case this is a set command so that
-	 * the convenience int and bool code can be common to set and get
+	 * Flow Control has been moved into the RX headers and this out of band
+	 * method isn't used any more.
+	 * remaining backward compatible with older dongles.
 	 */
-	if (params == NULL) {
-		params = arg;
-		plen = len;
-	}
-
-	if (vi->type == IOVT_VOID)
-		val_size = 0;
-	else if (vi->type == IOVT_BUFFER)
-		val_size = len;
-	else
-		/* all other types are integer sized */
-		val_size = sizeof(int);
-
-	actionid = set ? IOV_SVAL(vi->varid) : IOV_GVAL(vi->varid);
-	bcmerror = brcmf_sdbrcm_doiovar(bus, vi, actionid, name, params, plen,
-					arg, len, val_size);
-
-exit:
-	return bcmerror;
-}
-
-void brcmf_sdbrcm_bus_stop(struct brcmf_bus *bus, bool enforce_mutex)
-{
-	u32 local_hostintmask;
-	u8 saveclk;
-	uint retries;
-	int err;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	if (enforce_mutex)
-		brcmf_sdbrcm_sdlock(bus);
-
-	BUS_WAKE(bus);
-
-	/* Enable clock for device interrupts */
-	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
-
-	if (bus->watchdog_tsk) {
-		send_sig(SIGTERM, bus->watchdog_tsk, 1);
-		kthread_stop(bus->watchdog_tsk);
-		bus->watchdog_tsk = NULL;
-	}
-
-	if (bus->dpc_tsk) {
-		send_sig(SIGTERM, bus->dpc_tsk, 1);
-		kthread_stop(bus->dpc_tsk);
-		bus->dpc_tsk = NULL;
-	} else
-		tasklet_kill(&bus->tasklet);
-
-	/* Disable and clear interrupts at the chip level also */
-	w_sdreg32(bus, 0, offsetof(struct sdpcmd_regs, hostintmask), &retries);
-	local_hostintmask = bus->hostintmask;
-	bus->hostintmask = 0;
-
-	/* Change our idea of bus state */
-	bus->drvr->busstate = BRCMF_BUS_DOWN;
-
-	/* Force clocks on backplane to be sure F2 interrupt propagates */
-	saveclk = brcmf_sdcard_cfg_read(bus->card, SDIO_FUNC_1,
-					SBSDIO_FUNC1_CHIPCLKCSR, &err);
-	if (!err) {
-		brcmf_sdcard_cfg_write(bus->card, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_CHIPCLKCSR,
-				       (saveclk | SBSDIO_FORCE_HT), &err);
-	}
-	if (err) {
-		BRCMF_ERROR(("%s: Failed to force clock for F2: err %d\n",
-			     __func__, err));
-	}
-
-	/* Turn off the bus (F2), free any pending packets */
-	BRCMF_INTR(("%s: disable SDIO interrupts\n", __func__));
-	brcmf_sdcard_intr_disable(bus->card);
-	brcmf_sdcard_cfg_write(bus->card, SDIO_FUNC_0, SDIO_CCCR_IOEx,
-			 SDIO_FUNC_ENABLE_1, NULL);
-
-	/* Clear any pending interrupts now that F2 is disabled */
-	w_sdreg32(bus, local_hostintmask,
-		  offsetof(struct sdpcmd_regs, intstatus), &retries);
-
-	/* Turn off the backplane clock (only) */
-	brcmf_sdbrcm_clkctl(bus, CLK_SDONLY, false);
-
-	/* Clear the data packet queues */
-	brcmu_pktq_flush(&bus->txq, true, NULL, NULL);
-
-	/* Clear any held glomming stuff */
-	if (bus->glomd)
-		brcmu_pkt_buf_free_skb(bus->glomd);
-
-	if (bus->glom)
-		brcmu_pkt_buf_free_skb(bus->glom);
-
-	bus->glom = bus->glomd = NULL;
-
-	/* Clear rx control and wake any waiters */
-	bus->rxlen = 0;
-	brcmf_os_ioctl_resp_wake(bus->drvr);
-
-	/* Reset some F2 state stuff */
-	bus->rxskip = false;
-	bus->tx_seq = bus->rx_seq = 0;
-
-	if (enforce_mutex)
-		brcmf_sdbrcm_sdunlock(bus);
-}
-
-int brcmf_sdbrcm_bus_init(struct brcmf_pub *drvr, bool enforce_mutex)
-{
-	struct brcmf_bus *bus = drvr->bus;
-	struct brcmf_timeout tmo;
-	uint retries = 0;
-	u8 ready, enable;
-	int err, ret = 0;
-	u8 saveclk;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	/* try to download image and nvram to the dongle */
-	if (drvr->busstate == BRCMF_BUS_DOWN) {
-		if (!(brcmf_sdbrcm_download_firmware(bus, bus->card)))
-			return -1;
-	}
-
-	if (!bus->drvr)
-		return 0;
-
-	/* Start the watchdog timer */
-	bus->drvr->tickcnt = 0;
-	brcmf_sdbrcm_wd_timer(bus, brcmf_watchdog_ms);
-
-	if (enforce_mutex)
-		brcmf_sdbrcm_sdlock(bus);
-
-	/* Make sure backplane clock is on, needed to generate F2 interrupt */
-	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
-	if (bus->clkstate != CLK_AVAIL)
-		goto exit;
-
-	/* Force clocks on backplane to be sure F2 interrupt propagates */
-	saveclk =
-	    brcmf_sdcard_cfg_read(bus->card, SDIO_FUNC_1,
-				  SBSDIO_FUNC1_CHIPCLKCSR, &err);
-	if (!err) {
-		brcmf_sdcard_cfg_write(bus->card, SDIO_FUNC_1,
-				       SBSDIO_FUNC1_CHIPCLKCSR,
-				       (saveclk | SBSDIO_FORCE_HT), &err);
-	}
-	if (err) {
-		BRCMF_ERROR(("%s: Failed to force clock for F2: err %d\n",
-			     __func__, err));
-		goto exit;
-	}
-
-	/* Enable function 2 (frame transfers) */
-	w_sdreg32(bus, SDPCM_PROT_VERSION << SMB_DATA_VERSION_SHIFT,
-		  offsetof(struct sdpcmd_regs, tosbmailboxdata), &retries);
-	enable = (SDIO_FUNC_ENABLE_1 | SDIO_FUNC_ENABLE_2);
-
-	brcmf_sdcard_cfg_write(bus->card, SDIO_FUNC_0, SDIO_CCCR_IOEx, enable,
-			       NULL);
-
-	/* Give the dongle some time to do its thing and set IOR2 */
-	brcmf_timeout_start(&tmo, BRCMF_WAIT_F2RDY * 1000);
-
-	ready = 0;
-	while (ready != enable && !brcmf_timeout_expired(&tmo))
-		ready = brcmf_sdcard_cfg_read(bus->card, SDIO_FUNC_0,
-					      SDIO_CCCR_IORx, NULL);
-
-	BRCMF_INFO(("%s: enable 0x%02x, ready 0x%02x (waited %uus)\n",
-		    __func__, enable, ready, tmo.elapsed));
-
-	/* If F2 successfully enabled, set core and enable interrupts */
-	if (ready == enable) {
-		/* Set up the interrupt mask and enable interrupts */
-		bus->hostintmask = HOSTINTMASK;
-		w_sdreg32(bus, bus->hostintmask,
-			  offsetof(struct sdpcmd_regs, hostintmask), &retries);
-
-		brcmf_sdcard_cfg_write(bus->card, SDIO_FUNC_1, SBSDIO_WATERMARK,
-				 (u8) watermark, &err);
-
-		/* Set bus state according to enable result */
-		drvr->busstate = BRCMF_BUS_DATA;
-
-		bus->intdis = false;
-		if (bus->intr) {
-			BRCMF_INTR(("%s: enable SDIO device interrupts\n",
-				    __func__));
-			brcmf_sdcard_intr_enable(bus->card);
-		} else {
-			BRCMF_INTR(("%s: disable SDIO interrupts\n", __func__));
-			brcmf_sdcard_intr_disable(bus->card);
-		}
-
-	}
+	if (hmb_data & HMB_DATA_FC) {
+		fcbits = (hmb_data & HMB_DATA_FCDATA_MASK) >>
+							HMB_DATA_FCDATA_SHIFT;
 
-	else {
-		/* Disable F2 again */
-		enable = SDIO_FUNC_ENABLE_1;
-		brcmf_sdcard_cfg_write(bus->card, SDIO_FUNC_0, SDIO_CCCR_IOEx,
-				       enable, NULL);
-	}
+		if (fcbits & ~bus->flowcontrol)
+			bus->fc_xoff++;
 
-	/* Restore previous clock setting */
-	brcmf_sdcard_cfg_write(bus->card, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
-			 saveclk, &err);
+		if (bus->flowcontrol & ~fcbits)
+			bus->fc_xon++;
 
-#if defined(OOB_INTR_ONLY)
-	/* Host registration for OOB interrupt */
-	if (brcmf_sdio_register_oob_intr(bus->dhd)) {
-		brcmf_sdbrcm_wd_timer(bus, 0);
-		BRCMF_ERROR(("%s Host failed to resgister for OOB\n",
-			     __func__));
-		ret = -ENODEV;
-		goto exit;
+		bus->fc_rcvd++;
+		bus->flowcontrol = fcbits;
 	}
 
-	/* Enable oob at firmware */
-	brcmf_sdbrcm_enable_oob_intr(bus, true);
-#endif		/* defined(OOB_INTR_ONLY) */
-
-	/* If we didn't come up, turn off backplane clock */
-	if (drvr->busstate != BRCMF_BUS_DATA)
-		brcmf_sdbrcm_clkctl(bus, CLK_NONE, false);
-
-exit:
-	if (enforce_mutex)
-		brcmf_sdbrcm_sdunlock(bus);
+	/* Shouldn't be any others */
+	if (hmb_data & ~(HMB_DATA_DEVREADY |
+			 HMB_DATA_NAKHANDLED |
+			 HMB_DATA_FC |
+			 HMB_DATA_FWREADY |
+			 HMB_DATA_FCDATA_MASK | HMB_DATA_VERSION_MASK))
+		brcmf_dbg(ERROR, "Unknown mailbox data content: 0x%02x\n",
+			  hmb_data);
 
-	return ret;
+	return intstatus;
 }
 
 static void brcmf_sdbrcm_rxfail(struct brcmf_bus *bus, bool abort, bool rtx)
 {
-	struct brcmf_sdio_card *card = bus->card;
 	uint retries = 0;
 	u16 lastrbc;
 	u8 hi, lo;
 	int err;
 
-	BRCMF_ERROR(("%s: %sterminate frame%s\n", __func__,
-		     (abort ? "abort command, " : ""),
-		     (rtx ? ", send NAK" : "")));
+	brcmf_dbg(ERROR, "%sterminate frame%s\n",
+		  abort ? "abort command, " : "",
+		  rtx ? ", send NAK" : "");
 
 	if (abort)
-		brcmf_sdcard_abort(card, SDIO_FUNC_2);
+		brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
 
-	brcmf_sdcard_cfg_write(card, SDIO_FUNC_1, SBSDIO_FUNC1_FRAMECTRL,
+	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
+			       SBSDIO_FUNC1_FRAMECTRL,
 			       SFC_RF_TERM, &err);
 	bus->f1regdata++;
 
 	/* Wait until the packet has been flushed (device/FIFO stable) */
 	for (lastrbc = retries = 0xffff; retries > 0; retries--) {
-		hi = brcmf_sdcard_cfg_read(card, SDIO_FUNC_1,
+		hi = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
 					   SBSDIO_FUNC1_RFRAMEBCHI, NULL);
-		lo = brcmf_sdcard_cfg_read(card, SDIO_FUNC_1,
+		lo = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
 					   SBSDIO_FUNC1_RFRAMEBCLO, NULL);
 		bus->f1regdata += 2;
 
@@ -3448,20 +1193,16 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_bus *bus, bool abort, bool rtx)
 			break;
 
 		if ((hi > (lastrbc >> 8)) && (lo > (lastrbc & 0x00ff))) {
-			BRCMF_ERROR(("%s: count growing: last 0x%04x now "
-				     "0x%04x\n",
-				     __func__, lastrbc, ((hi << 8) + lo)));
+			brcmf_dbg(ERROR, "count growing: last 0x%04x now 0x%04x\n",
+				  lastrbc, (hi << 8) + lo);
 		}
 		lastrbc = (hi << 8) + lo;
 	}
 
-	if (!retries) {
-		BRCMF_ERROR(("%s: count never zeroed: last 0x%04x\n",
-			     __func__, lastrbc));
-	} else {
-		BRCMF_INFO(("%s: flush took %d iterations\n", __func__,
-			    (0xffff - retries)));
-	}
+	if (!retries)
+		brcmf_dbg(ERROR, "count never zeroed: last 0x%04x\n", lastrbc);
+	else
+		brcmf_dbg(INFO, "flush took %d iterations\n", 0xffff - retries);
 
 	if (rtx) {
 		bus->rxrtx++;
@@ -3477,113 +1218,10 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_bus *bus, bool abort, bool rtx)
 	bus->nextlen = 0;
 
 	/* If we can't reach the device, signal failure */
-	if (err || brcmf_sdcard_regfail(card))
+	if (err || brcmf_sdcard_regfail(bus->sdiodev))
 		bus->drvr->busstate = BRCMF_BUS_DOWN;
 }
 
-static void
-brcmf_sdbrcm_read_control(struct brcmf_bus *bus, u8 *hdr, uint len, uint doff)
-{
-	struct brcmf_sdio_card *card = bus->card;
-	uint rdlen, pad;
-
-	int sdret;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	/* Control data already received in aligned rxctl */
-	if ((bus->bus == SPI_BUS) && (!bus->usebufpool))
-		goto gotpkt;
-
-	/* Set rxctl for frame (w/optional alignment) */
-	bus->rxctl = bus->rxbuf;
-	if (brcmf_alignctl) {
-		bus->rxctl += firstread;
-		pad = ((unsigned long)bus->rxctl % BRCMF_SDALIGN);
-		if (pad)
-			bus->rxctl += (BRCMF_SDALIGN - pad);
-		bus->rxctl -= firstread;
-	}
-
-	/* Copy the already-read portion over */
-	memcpy(bus->rxctl, hdr, firstread);
-	if (len <= firstread)
-		goto gotpkt;
-
-	/* Copy the full data pkt in gSPI case and process ioctl. */
-	if (bus->bus == SPI_BUS) {
-		memcpy(bus->rxctl, hdr, len);
-		goto gotpkt;
-	}
-
-	/* Raise rdlen to next SDIO block to avoid tail command */
-	rdlen = len - firstread;
-	if (bus->roundup && bus->blocksize && (rdlen > bus->blocksize)) {
-		pad = bus->blocksize - (rdlen % bus->blocksize);
-		if ((pad <= bus->roundup) && (pad < bus->blocksize) &&
-		    ((len + pad) < bus->drvr->maxctl))
-			rdlen += pad;
-	} else if (rdlen % BRCMF_SDALIGN) {
-		rdlen += BRCMF_SDALIGN - (rdlen % BRCMF_SDALIGN);
-	}
-
-	/* Satisfy length-alignment requirements */
-	if (forcealign && (rdlen & (ALIGNMENT - 1)))
-		rdlen = roundup(rdlen, ALIGNMENT);
-
-	/* Drop if the read is too big or it exceeds our maximum */
-	if ((rdlen + firstread) > bus->drvr->maxctl) {
-		BRCMF_ERROR(("%s: %d-byte control read exceeds %d-byte"
-			     " buffer\n", __func__, rdlen, bus->drvr->maxctl));
-		bus->drvr->rx_errors++;
-		brcmf_sdbrcm_rxfail(bus, false, false);
-		goto done;
-	}
-
-	if ((len - doff) > bus->drvr->maxctl) {
-		BRCMF_ERROR(("%s: %d-byte ctl frame (%d-byte ctl data) exceeds "
-			     "%d-byte limit\n",
-			     __func__, len, (len - doff), bus->drvr->maxctl));
-		bus->drvr->rx_errors++;
-		bus->rx_toolong++;
-		brcmf_sdbrcm_rxfail(bus, false, false);
-		goto done;
-	}
-
-	/* Read remainder of frame body into the rxctl buffer */
-	sdret = brcmf_sdcard_recv_buf(card, brcmf_sdcard_cur_sbwad(card),
-				SDIO_FUNC_2,
-				F2SYNC, (bus->rxctl + firstread), rdlen,
-				NULL, NULL, NULL);
-	bus->f2rxdata++;
-
-	/* Control frame failures need retransmission */
-	if (sdret < 0) {
-		BRCMF_ERROR(("%s: read %d control bytes failed: %d\n",
-			     __func__, rdlen, sdret));
-		bus->rxc_errors++;
-		brcmf_sdbrcm_rxfail(bus, true, true);
-		goto done;
-	}
-
-gotpkt:
-
-#ifdef BCMDBG
-	if (BRCMF_BYTES_ON() && BRCMF_CTL_ON()) {
-		printk(KERN_DEBUG "RxCtrl:\n");
-		print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, bus->rxctl, len);
-	}
-#endif
-
-	/* Point to valid data and indicate its length */
-	bus->rxctl += doff;
-	bus->rxlen = len - doff;
-
-done:
-	/* Awake any waiters */
-	brcmf_os_ioctl_resp_wake(bus->drvr);
-}
-
 static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 {
 	u16 dlen, totlen;
@@ -3602,8 +1240,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 	/* If packets, issue read(s) and send up packet chain */
 	/* Return sequence numbers consumed? */
 
-	BRCMF_TRACE(("brcmf_sdbrcm_rxglom: start: glomd %p glom %p\n",
-		     bus->glomd, bus->glom));
+	brcmf_dbg(TRACE, "start: glomd %p glom %p\n", bus->glomd, bus->glom);
 
 	/* If there's a descriptor, generate the packet chain */
 	if (bus->glomd) {
@@ -3611,9 +1248,8 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 		dlen = (u16) (bus->glomd->len);
 		dptr = bus->glomd->data;
 		if (!dlen || (dlen & 1)) {
-			BRCMF_ERROR(("%s: bad glomd len(%d),"
-				     " ignore descriptor\n",
-				     __func__, dlen));
+			brcmf_dbg(ERROR, "bad glomd len(%d), ignore descriptor\n",
+				  dlen);
 			dlen = 0;
 		}
 
@@ -3624,15 +1260,14 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 			dptr += sizeof(u16);
 			if ((sublen < SDPCM_HDRLEN) ||
 			    ((num == 0) && (sublen < (2 * SDPCM_HDRLEN)))) {
-				BRCMF_ERROR(("%s: descriptor len %d bad: %d\n",
-					     __func__, num, sublen));
+				brcmf_dbg(ERROR, "descriptor len %d bad: %d\n",
+					  num, sublen);
 				pnext = NULL;
 				break;
 			}
 			if (sublen % BRCMF_SDALIGN) {
-				BRCMF_ERROR(("%s: sublen %d not multiple of"
-					     " %d\n", __func__, sublen,
-					     BRCMF_SDALIGN));
+				brcmf_dbg(ERROR, "sublen %d not multiple of %d\n",
+					  sublen, BRCMF_SDALIGN);
 				usechain = false;
 			}
 			totlen += sublen;
@@ -3648,9 +1283,8 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 			/* Allocate/chain packet for next subframe */
 			pnext = brcmu_pkt_buf_get_skb(sublen + BRCMF_SDALIGN);
 			if (pnext == NULL) {
-				BRCMF_ERROR(("%s: bcm_pkt_buf_get_skb failed, "
-					     "num %d len %d\n", __func__,
-					     num, sublen));
+				brcmf_dbg(ERROR, "bcm_pkt_buf_get_skb failed, num %d len %d\n",
+					  num, sublen);
 				break;
 			}
 			if (!pfirst) {
@@ -3661,22 +1295,18 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 			}
 
 			/* Adhere to start alignment requirements */
-			PKTALIGN(pnext, sublen, BRCMF_SDALIGN);
+			pkt_align(pnext, sublen, BRCMF_SDALIGN);
 		}
 
 		/* If all allocations succeeded, save packet chain
 			 in bus structure */
 		if (pnext) {
-			BRCMF_GLOM(("%s: allocated %d-byte packet chain for %d "
-				    "subframes\n", __func__, totlen, num));
-			if (BRCMF_GLOM_ON() && bus->nextlen) {
-				if (totlen != bus->nextlen) {
-					BRCMF_GLOM(("%s: glomdesc mismatch: "
-						    "nextlen %d glomdesc %d "
-						    "rxseq %d\n", __func__,
-						    bus->nextlen,
-						    totlen, rxseq));
-				}
+			brcmf_dbg(GLOM, "allocated %d-byte packet chain for %d subframes\n",
+				  totlen, num);
+			if (BRCMF_GLOM_ON() && bus->nextlen &&
+			    totlen != bus->nextlen) {
+				brcmf_dbg(GLOM, "glomdesc mismatch: nextlen %d glomdesc %d rxseq %d\n",
+					  bus->nextlen, totlen, rxseq);
 			}
 			bus->glom = pfirst;
 			pfirst = pnext = NULL;
@@ -3697,12 +1327,11 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 		 or had one from before */
 	if (bus->glom) {
 		if (BRCMF_GLOM_ON()) {
-			BRCMF_GLOM(("%s: try superframe read, packet chain:\n",
-				    __func__));
+			brcmf_dbg(GLOM, "try superframe read, packet chain:\n");
 			for (pnext = bus->glom; pnext; pnext = pnext->next) {
-				BRCMF_GLOM(("    %p: %p len 0x%04x (%d)\n",
-					    pnext, (u8 *) (pnext->data),
-					    pnext->len, pnext->len));
+				brcmf_dbg(GLOM, "    %p: %p len 0x%04x (%d)\n",
+					  pnext, (u8 *) (pnext->data),
+					  pnext->len, pnext->len);
 			}
 		}
 
@@ -3714,37 +1343,36 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 		 * packet and and copy into the chain.
 		 */
 		if (usechain) {
-			errcode = brcmf_sdcard_recv_buf(bus->card,
-					brcmf_sdcard_cur_sbwad(bus->card),
+			errcode = brcmf_sdcard_recv_buf(bus->sdiodev,
+					bus->sdiodev->sbwad,
 					SDIO_FUNC_2,
 					F2SYNC, (u8 *) pfirst->data, dlen,
-					pfirst, NULL, NULL);
+					pfirst);
 		} else if (bus->dataptr) {
-			errcode = brcmf_sdcard_recv_buf(bus->card,
-					brcmf_sdcard_cur_sbwad(bus->card),
+			errcode = brcmf_sdcard_recv_buf(bus->sdiodev,
+					bus->sdiodev->sbwad,
 					SDIO_FUNC_2,
 					F2SYNC, bus->dataptr, dlen,
-					NULL, NULL, NULL);
+					NULL);
 			sublen = (u16) brcmu_pktfrombuf(pfirst, 0, dlen,
 						bus->dataptr);
 			if (sublen != dlen) {
-				BRCMF_ERROR(("%s: FAILED TO COPY, dlen %d "
-					     "sublen %d\n",
-					     __func__, dlen, sublen));
+				brcmf_dbg(ERROR, "FAILED TO COPY, dlen %d sublen %d\n",
+					  dlen, sublen);
 				errcode = -1;
 			}
 			pnext = NULL;
 		} else {
-			BRCMF_ERROR(("COULDN'T ALLOC %d-BYTE GLOM, "
-				     "FORCE FAILURE\n", dlen));
+			brcmf_dbg(ERROR, "COULDN'T ALLOC %d-BYTE GLOM, FORCE FAILURE\n",
+				  dlen);
 			errcode = -1;
 		}
 		bus->f2rxdata++;
 
 		/* On failure, kill the superframe, allow a couple retries */
 		if (errcode < 0) {
-			BRCMF_ERROR(("%s: glom read of %d bytes failed: %d\n",
-				     __func__, dlen, errcode));
+			brcmf_dbg(ERROR, "glom read of %d bytes failed: %d\n",
+				  dlen, errcode);
 			bus->drvr->rx_errors++;
 
 			if (bus->glomerr++ < 3) {
@@ -3775,8 +1403,8 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 		seq = SDPCM_PACKET_SEQUENCE(&dptr[SDPCM_FRAMETAG_LEN]);
 		bus->nextlen = dptr[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
 		if ((bus->nextlen << 4) > MAX_RX_DATASZ) {
-			BRCMF_INFO(("%s: nextlen too large (%d) seq %d\n",
-				    __func__, bus->nextlen, seq));
+			brcmf_dbg(INFO, "nextlen too large (%d) seq %d\n",
+				  bus->nextlen, seq);
 			bus->nextlen = 0;
 		}
 		doff = SDPCM_DOFFSET_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
@@ -3784,48 +1412,42 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 
 		errcode = 0;
 		if ((u16)~(sublen ^ check)) {
-			BRCMF_ERROR(("%s (superframe): HW hdr error: len/check "
-				     "0x%04x/0x%04x\n", __func__, 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_ERROR(("%s (superframe): len 0x%04x, rounded "
-				     "0x%04x, expect 0x%04x\n",
-				     __func__, sublen,
-				     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_ERROR(("%s (superframe): bad channel %d\n",
-				   __func__,
-				   SDPCM_PACKET_CHANNEL(&dptr
-							[SDPCM_FRAMETAG_LEN])));
+			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_ERROR(("%s (superframe): got 2nd descriptor?\n",
-				     __func__));
+			brcmf_dbg(ERROR, "(superframe): got 2nd descriptor?\n");
 			errcode = -1;
 		} else if ((doff < SDPCM_HDRLEN) ||
 			   (doff > (pfirst->len - SDPCM_HDRLEN))) {
-			BRCMF_ERROR(("%s (superframe): Bad data offset %d: "
-				     "HW %d pkt %d min %d\n",
-				     __func__, doff, sublen,
-				     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_INFO(("%s: (superframe) rx_seq %d, expected %d\n",
-				    __func__, seq, rxseq));
+			brcmf_dbg(INFO, "(superframe) rx_seq %d, expected %d\n",
+				  seq, rxseq);
 			bus->rx_badseq++;
 			rxseq = seq;
 		}
 
 		/* Check window for sanity */
 		if ((u8) (txmax - bus->tx_seq) > 0x40) {
-			BRCMF_ERROR(("%s: unlikely tx max %d with tx_seq %d\n",
-				     __func__, txmax, bus->tx_seq));
+			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;
@@ -3852,25 +1474,21 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 #endif
 
 			if ((u16)~(sublen ^ check)) {
-				BRCMF_ERROR(("%s (subframe %d): HW hdr error: "
-					     "len/check 0x%04x/0x%04x\n",
-					     __func__, num, 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_ERROR(("%s (subframe %d): length mismatch"
-					     ": len 0x%04x, expect 0x%04x\n",
-					     __func__, num, sublen, dlen));
+				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_ERROR(("%s (subframe %d): bad channel"
-					     " %d\n", __func__, num, chan));
+				brcmf_dbg(ERROR, "(subframe %d): bad channel %d\n",
+					  num, chan);
 				errcode = -1;
 			} else if ((doff < SDPCM_HDRLEN) || (doff > sublen)) {
-				BRCMF_ERROR(("%s (subframe %d): Bad data offset"
-					     " %d: HW %d min %d\n",
-					     __func__, num, doff, sublen,
-					     SDPCM_HDRLEN));
+				brcmf_dbg(ERROR, "(subframe %d): Bad data offset %d: HW %d min %d\n",
+					  num, doff, sublen, SDPCM_HDRLEN);
 				errcode = -1;
 			}
 		}
@@ -3908,17 +1526,16 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 			seq = SDPCM_PACKET_SEQUENCE(&dptr[SDPCM_FRAMETAG_LEN]);
 			doff = SDPCM_DOFFSET_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
 
-			BRCMF_GLOM(("%s: Get subframe %d, %p(%p/%d), sublen %d "
-				    "chan %d seq %d\n",
-				    __func__, num, pfirst, pfirst->data,
-				    pfirst->len, sublen, chan, seq));
+			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_GLOM(("%s: rx_seq %d, expected %d\n",
-					    __func__, seq, rxseq));
+				brcmf_dbg(GLOM, "rx_seq %d, expected %d\n",
+					  seq, rxseq);
 				bus->rx_badseq++;
 				rxseq = seq;
 			}
@@ -3935,23 +1552,22 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 
 			if (pfirst->len == 0) {
 				brcmu_pkt_buf_free_skb(pfirst);
-				if (plast) {
+				if (plast)
 					plast->next = pnext;
-				} else {
+				else
 					save_pfirst = pnext;
-				}
+
 				continue;
-			} else if (brcmf_proto_hdrpull(bus->drvr, &ifidx, pfirst)
-					!= 0) {
-				BRCMF_ERROR(("%s: rx protocol error\n",
-					     __func__));
+			} else if (brcmf_proto_hdrpull(bus->drvr, &ifidx,
+						       pfirst) != 0) {
+				brcmf_dbg(ERROR, "rx protocol error\n");
 				bus->drvr->rx_errors++;
 				brcmu_pkt_buf_free_skb(pfirst);
-				if (plast) {
+				if (plast)
 					plast->next = pnext;
-				} else {
+				else
 					save_pfirst = pnext;
-				}
+
 				continue;
 			}
 
@@ -3963,11 +1579,10 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 
 #ifdef BCMDBG
 			if (BRCMF_GLOM_ON()) {
-				BRCMF_GLOM(("%s subframe %d to stack, %p"
-					    "(%p/%d) nxt/lnk %p/%p\n",
-					    __func__, num, pfirst, pfirst->data,
-					    pfirst->len, pfirst->next,
-					    pfirst->prev));
+				brcmf_dbg(GLOM, "subframe %d to stack, %p (%p/%d) nxt/lnk %p/%p\n",
+					  num, pfirst, pfirst->data,
+					  pfirst->len, pfirst->next,
+					  pfirst->prev);
 				print_hex_dump_bytes("", DUMP_PREFIX_OFFSET,
 						pfirst->data,
 						min_t(int, pfirst->len, 32));
@@ -3975,9 +1590,9 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 #endif				/* BCMDBG */
 		}
 		if (num) {
-			brcmf_sdbrcm_sdunlock(bus);
+			up(&bus->sdsem);
 			brcmf_rx_frame(bus->drvr, ifidx, save_pfirst, num);
-			brcmf_sdbrcm_sdlock(bus);
+			down(&bus->sdsem);
 		}
 
 		bus->rxglomframes++;
@@ -3986,12 +1601,230 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_bus *bus, u8 rxseq)
 	return num;
 }
 
+static int brcmf_sdbrcm_dcmd_resp_wait(struct brcmf_bus *bus, uint *condition,
+					bool *pending)
+{
+	DECLARE_WAITQUEUE(wait, current);
+	int timeout = msecs_to_jiffies(DCMD_RESP_TIMEOUT);
+
+	/* Wait until control frame is available */
+	add_wait_queue(&bus->dcmd_resp_wait, &wait);
+	set_current_state(TASK_INTERRUPTIBLE);
+
+	while (!(*condition) && (!signal_pending(current) && timeout))
+		timeout = schedule_timeout(timeout);
+
+	if (signal_pending(current))
+		*pending = true;
+
+	set_current_state(TASK_RUNNING);
+	remove_wait_queue(&bus->dcmd_resp_wait, &wait);
+
+	return timeout;
+}
+
+static int brcmf_sdbrcm_dcmd_resp_wake(struct brcmf_bus *bus)
+{
+	if (waitqueue_active(&bus->dcmd_resp_wait))
+		wake_up_interruptible(&bus->dcmd_resp_wait);
+
+	return 0;
+}
+static void
+brcmf_sdbrcm_read_control(struct brcmf_bus *bus, u8 *hdr, uint len, uint doff)
+{
+	uint rdlen, pad;
+
+	int sdret;
+
+	brcmf_dbg(TRACE, "Enter\n");
+
+	/* Set rxctl for frame (w/optional alignment) */
+	bus->rxctl = bus->rxbuf;
+	bus->rxctl += BRCMF_FIRSTREAD;
+	pad = ((unsigned long)bus->rxctl % BRCMF_SDALIGN);
+	if (pad)
+		bus->rxctl += (BRCMF_SDALIGN - pad);
+	bus->rxctl -= BRCMF_FIRSTREAD;
+
+	/* Copy the already-read portion over */
+	memcpy(bus->rxctl, hdr, BRCMF_FIRSTREAD);
+	if (len <= BRCMF_FIRSTREAD)
+		goto gotpkt;
+
+	/* Raise rdlen to next SDIO block to avoid tail command */
+	rdlen = len - BRCMF_FIRSTREAD;
+	if (bus->roundup && bus->blocksize && (rdlen > bus->blocksize)) {
+		pad = bus->blocksize - (rdlen % bus->blocksize);
+		if ((pad <= bus->roundup) && (pad < bus->blocksize) &&
+		    ((len + pad) < bus->drvr->maxctl))
+			rdlen += pad;
+	} else if (rdlen % BRCMF_SDALIGN) {
+		rdlen += BRCMF_SDALIGN - (rdlen % BRCMF_SDALIGN);
+	}
+
+	/* Satisfy length-alignment requirements */
+	if (rdlen & (ALIGNMENT - 1))
+		rdlen = roundup(rdlen, ALIGNMENT);
+
+	/* Drop if the read is too big or it exceeds our maximum */
+	if ((rdlen + BRCMF_FIRSTREAD) > bus->drvr->maxctl) {
+		brcmf_dbg(ERROR, "%d-byte control read exceeds %d-byte buffer\n",
+			  rdlen, bus->drvr->maxctl);
+		bus->drvr->rx_errors++;
+		brcmf_sdbrcm_rxfail(bus, false, false);
+		goto done;
+	}
+
+	if ((len - doff) > bus->drvr->maxctl) {
+		brcmf_dbg(ERROR, "%d-byte ctl frame (%d-byte ctl data) exceeds %d-byte limit\n",
+			  len, len - doff, bus->drvr->maxctl);
+		bus->drvr->rx_errors++;
+		bus->rx_toolong++;
+		brcmf_sdbrcm_rxfail(bus, false, false);
+		goto done;
+	}
+
+	/* Read remainder of frame body into the rxctl buffer */
+	sdret = brcmf_sdcard_recv_buf(bus->sdiodev,
+				bus->sdiodev->sbwad,
+				SDIO_FUNC_2,
+				F2SYNC, (bus->rxctl + BRCMF_FIRSTREAD), rdlen,
+				NULL);
+	bus->f2rxdata++;
+
+	/* Control frame failures need retransmission */
+	if (sdret < 0) {
+		brcmf_dbg(ERROR, "read %d control bytes failed: %d\n",
+			  rdlen, sdret);
+		bus->rxc_errors++;
+		brcmf_sdbrcm_rxfail(bus, true, true);
+		goto done;
+	}
+
+gotpkt:
+
+#ifdef BCMDBG
+	if (BRCMF_BYTES_ON() && BRCMF_CTL_ON()) {
+		printk(KERN_DEBUG "RxCtrl:\n");
+		print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, bus->rxctl, len);
+	}
+#endif
+
+	/* Point to valid data and indicate its length */
+	bus->rxctl += doff;
+	bus->rxlen = len - doff;
+
+done:
+	/* Awake any waiters */
+	brcmf_sdbrcm_dcmd_resp_wake(bus);
+}
+
+/* Pad read to blocksize for efficiency */
+static void brcmf_pad(struct brcmf_bus *bus, u16 *pad, u16 *rdlen)
+{
+	if (bus->roundup && bus->blocksize && *rdlen > bus->blocksize) {
+		*pad = bus->blocksize - (*rdlen % bus->blocksize);
+		if (*pad <= bus->roundup && *pad < bus->blocksize &&
+		    *rdlen + *pad + BRCMF_FIRSTREAD < MAX_RX_DATASZ)
+			*rdlen += *pad;
+	} else if (*rdlen % BRCMF_SDALIGN) {
+		*rdlen += BRCMF_SDALIGN - (*rdlen % BRCMF_SDALIGN);
+	}
+}
+
+static void
+brcmf_alloc_pkt_and_read(struct brcmf_bus *bus, u16 rdlen,
+			 struct sk_buff **pkt, u8 **rxbuf)
+{
+	int sdret;		/* Return code from calls */
+
+	*pkt = brcmu_pkt_buf_get_skb(rdlen + BRCMF_SDALIGN);
+	if (*pkt == NULL)
+		return;
+
+	pkt_align(*pkt, rdlen, BRCMF_SDALIGN);
+	*rxbuf = (u8 *) ((*pkt)->data);
+	/* Read the entire frame */
+	sdret = brcmf_sdcard_recv_buf(bus->sdiodev, bus->sdiodev->sbwad,
+				      SDIO_FUNC_2, F2SYNC,
+				      *rxbuf, rdlen, *pkt);
+	bus->f2rxdata++;
+
+	if (sdret < 0) {
+		brcmf_dbg(ERROR, "(nextlen): read %d bytes failed: %d\n",
+			  rdlen, sdret);
+		brcmu_pkt_buf_free_skb(*pkt);
+		bus->drvr->rx_errors++;
+		/* Force retry w/normal header read.
+		 * Don't attempt NAK for
+		 * gSPI
+		 */
+		brcmf_sdbrcm_rxfail(bus, true, true);
+		*pkt = NULL;
+	}
+}
+
+/* Checks the header */
+static int
+brcmf_check_rxbuf(struct brcmf_bus *bus, struct sk_buff *pkt, u8 *rxbuf,
+		  u8 rxseq, u16 nextlen, u16 *len)
+{
+	u16 check;
+	bool len_consistent;	/* Result of comparing readahead len and
+				   len from hw-hdr */
+
+	memcpy(bus->rxhdr, rxbuf, SDPCM_HDRLEN);
+
+	/* Extract hardware header fields */
+	*len = get_unaligned_le16(bus->rxhdr);
+	check = get_unaligned_le16(bus->rxhdr + sizeof(u16));
+
+	/* All zeros means readahead info was bad */
+	if (!(*len | check)) {
+		brcmf_dbg(INFO, "(nextlen): read zeros in HW header???\n");
+		goto fail;
+	}
+
+	/* Validate check bytes */
+	if ((u16)~(*len ^ check)) {
+		brcmf_dbg(ERROR, "(nextlen): HW hdr error: nextlen/len/check 0x%04x/0x%04x/0x%04x\n",
+			  nextlen, *len, check);
+		bus->rx_badhdr++;
+		brcmf_sdbrcm_rxfail(bus, false, false);
+		goto fail;
+	}
+
+	/* Validate frame length */
+	if (*len < SDPCM_HDRLEN) {
+		brcmf_dbg(ERROR, "(nextlen): HW hdr length invalid: %d\n",
+			  *len);
+		goto fail;
+	}
+
+	/* Check for consistency with readahead info */
+	len_consistent = (nextlen != (roundup(*len, 16) >> 4));
+	if (len_consistent) {
+		/* Mismatch, force retry w/normal
+			header (may be >4K) */
+		brcmf_dbg(ERROR, "(nextlen): mismatch, nextlen %d len %d rnd %d; expected rxseq %d\n",
+			  nextlen, *len, roundup(*len, 16),
+			  rxseq);
+		brcmf_sdbrcm_rxfail(bus, true, true);
+		goto fail;
+	}
+
+	return 0;
+
+fail:
+	brcmf_sdbrcm_pktfree2(bus, pkt);
+	return -EINVAL;
+}
+
 /* Return true if there may be more frames to read */
 static uint
 brcmf_sdbrcm_readframes(struct brcmf_bus *bus, uint maxframes, bool *finished)
 {
-	struct brcmf_sdio_card *card = bus->card;
-
 	u16 len, check;	/* Extracted hardware header fields */
 	u8 chan, seq, doff;	/* Extracted software header fields */
 	u8 fcbits;		/* Extracted fcbits from software header */
@@ -4003,25 +1836,11 @@ brcmf_sdbrcm_readframes(struct brcmf_bus *bus, uint maxframes, bool *finished)
 	uint rxleft = 0;	/* Remaining number of frames allowed */
 	int sdret;		/* Return code from calls */
 	u8 txmax;		/* Maximum tx sequence offered */
-	bool len_consistent;	/* Result of comparing readahead len and
-					 len from hw-hdr */
 	u8 *rxbuf;
 	int ifidx = 0;
 	uint rxcount = 0;	/* Total frames read */
 
-#if defined(BCMDBG) || defined(SDTEST)
-	bool sdtest = false;	/* To limit message spew from test mode */
-#endif
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-#ifdef SDTEST
-	/* Allow pktgen to override maxframes */
-	if (bus->pktgen_count && (bus->pktgen_mode == BRCMF_PKTGEN_RECV)) {
-		maxframes = bus->pktgen_count;
-		sdtest = true;
-	}
-#endif
+	brcmf_dbg(TRACE, "Enter\n");
 
 	/* Not finished unless we encounter no more frames indication */
 	*finished = false;
@@ -4033,185 +1852,39 @@ brcmf_sdbrcm_readframes(struct brcmf_bus *bus, uint maxframes, bool *finished)
 		/* Handle glomming separately */
 		if (bus->glom || bus->glomd) {
 			u8 cnt;
-			BRCMF_GLOM(("%s: calling rxglom: glomd %p, glom %p\n",
-				    __func__, bus->glomd, bus->glom));
+			brcmf_dbg(GLOM, "calling rxglom: glomd %p, glom %p\n",
+				  bus->glomd, bus->glom);
 			cnt = brcmf_sdbrcm_rxglom(bus, rxseq);
-			BRCMF_GLOM(("%s: rxglom returned %d\n", __func__, cnt));
+			brcmf_dbg(GLOM, "rxglom returned %d\n", cnt);
 			rxseq += cnt - 1;
 			rxleft = (rxleft > cnt) ? (rxleft - cnt) : 1;
 			continue;
 		}
 
 		/* Try doing single read if we can */
-		if (brcmf_readahead && bus->nextlen) {
+		if (bus->nextlen) {
 			u16 nextlen = bus->nextlen;
 			bus->nextlen = 0;
 
-			if (bus->bus == SPI_BUS) {
-				rdlen = len = nextlen;
-			} else {
-				rdlen = len = nextlen << 4;
-
-				/* Pad read to blocksize for efficiency */
-				if (bus->roundup && bus->blocksize
-				    && (rdlen > bus->blocksize)) {
-					pad =
-					    bus->blocksize -
-					    (rdlen % bus->blocksize);
-					if ((pad <= bus->roundup)
-					    && (pad < bus->blocksize)
-					    && ((rdlen + pad + firstread) <
-						MAX_RX_DATASZ))
-						rdlen += pad;
-				} else if (rdlen % BRCMF_SDALIGN) {
-					rdlen +=
-					    BRCMF_SDALIGN - (rdlen % BRCMF_SDALIGN);
-				}
-			}
+			rdlen = len = nextlen << 4;
+			brcmf_pad(bus, &pad, &rdlen);
 
-			/* We use bus->rxctl buffer in WinXP for initial
-			 * control pkt receives.
-			 * Later we use buffer-poll for data as well
-			 * as control packets.
-			 * This is required because dhd receives full
-			 * frame in gSPI unlike SDIO.
+			/*
 			 * After the frame is received we have to
 			 * distinguish whether it is data
 			 * or non-data frame.
 			 */
-			/* Allocate a packet buffer */
-			pkt = brcmu_pkt_buf_get_skb(rdlen + BRCMF_SDALIGN);
-			if (!pkt) {
-				if (bus->bus == SPI_BUS) {
-					bus->usebufpool = false;
-					bus->rxctl = bus->rxbuf;
-					if (brcmf_alignctl) {
-						bus->rxctl += firstread;
-						pad = ((unsigned long)bus->rxctl %
-						      BRCMF_SDALIGN);
-						if (pad)
-							bus->rxctl +=
-							    (BRCMF_SDALIGN - pad);
-						bus->rxctl -= firstread;
-					}
-					rxbuf = bus->rxctl;
-					/* Read the entire frame */
-					sdret = brcmf_sdcard_recv_buf(card,
-						   brcmf_sdcard_cur_sbwad(card),
-						   SDIO_FUNC_2, F2SYNC,
-						   rxbuf, rdlen,
-						   NULL, NULL, NULL);
-					bus->f2rxdata++;
-
-					/* Control frame failures need
-					 retransmission */
-					if (sdret < 0) {
-						BRCMF_ERROR(("%s: read %d "
-							     "control bytes "
-							     "failed: %d\n",
-							     __func__,
-							     rdlen, sdret));
-						/* dhd.rx_ctlerrs is higher */
-						bus->rxc_errors++;
-						brcmf_sdbrcm_rxfail(bus, true,
-						       (bus->bus ==
-							SPI_BUS) ? false
-						       : true);
-						continue;
-					}
-				} else {
-					/* Give up on data,
-					request rtx of events */
-					BRCMF_ERROR(("%s (nextlen): "
-						     "brcmu_pkt_buf_get_skb "
-						     "failed:"
-						     " len %d rdlen %d expected"
-						     " rxseq %d\n", __func__,
-						     len, rdlen, rxseq));
-					continue;
-				}
-			} else {
-				if (bus->bus == SPI_BUS)
-					bus->usebufpool = true;
-
-				PKTALIGN(pkt, rdlen, BRCMF_SDALIGN);
-				rxbuf = (u8 *) (pkt->data);
-				/* Read the entire frame */
-				sdret = brcmf_sdcard_recv_buf(card,
-						brcmf_sdcard_cur_sbwad(card),
-						SDIO_FUNC_2, F2SYNC,
-						rxbuf, rdlen,
-						pkt, NULL, NULL);
-				bus->f2rxdata++;
-
-				if (sdret < 0) {
-					BRCMF_ERROR(("%s (nextlen): read %d"
-						     " bytes failed: %d\n",
-						     __func__, rdlen, sdret));
-					brcmu_pkt_buf_free_skb(pkt);
-					bus->drvr->rx_errors++;
-					/* Force retry w/normal header read.
-					 * Don't attempt NAK for
-					 * gSPI
-					 */
-					brcmf_sdbrcm_rxfail(bus, true,
-						       (bus->bus ==
-							SPI_BUS) ? false :
-						       true);
-					continue;
-				}
-			}
-
-			/* Now check the header */
-			memcpy(bus->rxhdr, rxbuf, SDPCM_HDRLEN);
-
-			/* Extract hardware header fields */
-			len = get_unaligned_le16(bus->rxhdr);
-			check = get_unaligned_le16(bus->rxhdr + sizeof(u16));
-
-			/* All zeros means readahead info was bad */
-			if (!(len | check)) {
-				BRCMF_INFO(("%s (nextlen): read zeros in HW "
-					    "header???\n", __func__));
-				brcmf_sdbrcm_pktfree2(bus, pkt);
-				continue;
-			}
-
-			/* Validate check bytes */
-			if ((u16)~(len ^ check)) {
-				BRCMF_ERROR(("%s (nextlen): HW hdr error:"
-					     " nextlen/len/check"
-					     " 0x%04x/0x%04x/0x%04x\n",
-					     __func__, nextlen, len, check));
-				bus->rx_badhdr++;
-				brcmf_sdbrcm_rxfail(bus, false, false);
-				brcmf_sdbrcm_pktfree2(bus, pkt);
+			brcmf_alloc_pkt_and_read(bus, rdlen, &pkt, &rxbuf);
+			if (pkt == NULL) {
+				/* Give up on data, request rtx of events */
+				brcmf_dbg(ERROR, "(nextlen): brcmf_alloc_pkt_and_read failed: len %d rdlen %d expected rxseq %d\n",
+					  len, rdlen, rxseq);
 				continue;
 			}
 
-			/* Validate frame length */
-			if (len < SDPCM_HDRLEN) {
-				BRCMF_ERROR(("%s (nextlen): HW hdr length "
-					     "invalid: %d\n", __func__, len));
-				brcmf_sdbrcm_pktfree2(bus, pkt);
+			if (brcmf_check_rxbuf(bus, pkt, rxbuf, rxseq, nextlen,
+					      &len) < 0)
 				continue;
-			}
-
-			/* Check for consistency withreadahead info */
-			len_consistent = (nextlen != (roundup(len, 16) >> 4));
-			if (len_consistent) {
-				/* Mismatch, force retry w/normal
-					header (may be >4K) */
-				BRCMF_ERROR(("%s (nextlen): mismatch, "
-					     "nextlen %d len %d rnd %d; "
-					     "expected rxseq %d\n",
-					     __func__, nextlen,
-					     len, roundup(len, 16), rxseq));
-				brcmf_sdbrcm_rxfail(bus, true,
-						  bus->bus != SPI_BUS);
-				brcmf_sdbrcm_pktfree2(bus, pkt);
-				continue;
-			}
 
 			/* Extract software header fields */
 			chan = SDPCM_PACKET_CHANNEL(
@@ -4227,9 +1900,8 @@ brcmf_sdbrcm_readframes(struct brcmf_bus *bus, uint maxframes, bool *finished)
 			    bus->rxhdr[SDPCM_FRAMETAG_LEN +
 				       SDPCM_NEXTLEN_OFFSET];
 			if ((bus->nextlen << 4) > MAX_RX_DATASZ) {
-				BRCMF_INFO(("%s (nextlen): got frame w/nextlen"
-					    " too large (%d), seq %d\n",
-					    __func__, bus->nextlen, seq));
+				brcmf_dbg(INFO, "(nextlen): got frame w/nextlen too large (%d), seq %d\n",
+					  bus->nextlen, seq);
 				bus->nextlen = 0;
 			}
 
@@ -4252,17 +1924,16 @@ brcmf_sdbrcm_readframes(struct brcmf_bus *bus, uint maxframes, bool *finished)
 
 			/* Check and update sequence number */
 			if (rxseq != seq) {
-				BRCMF_INFO(("%s (nextlen): rx_seq %d, expected "
-					    "%d\n", __func__, seq, rxseq));
+				brcmf_dbg(INFO, "(nextlen): rx_seq %d, expected %d\n",
+					  seq, rxseq);
 				bus->rx_badseq++;
 				rxseq = seq;
 			}
 
 			/* Check window for sanity */
 			if ((u8) (txmax - bus->tx_seq) > 0x40) {
-				BRCMF_ERROR(("%s: got unlikely tx max %d with "
-					     "tx_seq %d\n",
-					     __func__, txmax, bus->tx_seq));
+				brcmf_dbg(ERROR, "got unlikely tx max %d with tx_seq %d\n",
+					  txmax, bus->tx_seq);
 				txmax = bus->tx_seq + 2;
 			}
 			bus->tx_max = txmax;
@@ -4280,34 +1951,19 @@ brcmf_sdbrcm_readframes(struct brcmf_bus *bus, uint maxframes, bool *finished)
 #endif
 
 			if (chan == SDPCM_CONTROL_CHANNEL) {
-				if (bus->bus == SPI_BUS) {
-					brcmf_sdbrcm_read_control(bus, rxbuf,
-								  len, doff);
-				} else {
-					BRCMF_ERROR(("%s (nextlen): readahead"
-						     " on control packet %d?\n",
-						     __func__, seq));
-					/* Force retry w/normal header read */
-					bus->nextlen = 0;
-					brcmf_sdbrcm_rxfail(bus, false, true);
-				}
+				brcmf_dbg(ERROR, "(nextlen): readahead on control packet %d?\n",
+					  seq);
+				/* Force retry w/normal header read */
+				bus->nextlen = 0;
+				brcmf_sdbrcm_rxfail(bus, false, true);
 				brcmf_sdbrcm_pktfree2(bus, pkt);
 				continue;
 			}
 
-			if ((bus->bus == SPI_BUS) && !bus->usebufpool) {
-				BRCMF_ERROR(("Received %d bytes on %d channel."
-					     " Running out of " "rx pktbuf's or"
-					     " not yet malloced.\n",
-					     len, chan));
-				continue;
-			}
-
 			/* Validate data offset */
 			if ((doff < SDPCM_HDRLEN) || (doff > len)) {
-				BRCMF_ERROR(("%s (nextlen): bad data offset %d:"
-					     " HW len %d min %d\n", __func__,
-					     doff, len, SDPCM_HDRLEN));
+				brcmf_dbg(ERROR, "(nextlen): bad data offset %d: HW len %d min %d\n",
+					  doff, len, SDPCM_HDRLEN);
 				brcmf_sdbrcm_rxfail(bus, false, false);
 				brcmf_sdbrcm_pktfree2(bus, pkt);
 				continue;
@@ -4316,20 +1972,15 @@ brcmf_sdbrcm_readframes(struct brcmf_bus *bus, uint maxframes, bool *finished)
 			/* All done with this one -- now deliver the packet */
 			goto deliver;
 		}
-		/* gSPI frames should not be handled in fractions */
-		if (bus->bus == SPI_BUS)
-			break;
 
 		/* Read frame header (hardware and software) */
-		sdret = brcmf_sdcard_recv_buf(card,
-				brcmf_sdcard_cur_sbwad(card),
-				SDIO_FUNC_2, F2SYNC, bus->rxhdr, firstread,
-				NULL, NULL, NULL);
+		sdret = brcmf_sdcard_recv_buf(bus->sdiodev, bus->sdiodev->sbwad,
+					      SDIO_FUNC_2, F2SYNC, bus->rxhdr,
+					      BRCMF_FIRSTREAD, NULL);
 		bus->f2rxhdrs++;
 
 		if (sdret < 0) {
-			BRCMF_ERROR(("%s: RXHEADER FAILED: %d\n", __func__,
-				     sdret));
+			brcmf_dbg(ERROR, "RXHEADER FAILED: %d\n", sdret);
 			bus->rx_hdrfail++;
 			brcmf_sdbrcm_rxfail(bus, true, true);
 			continue;
@@ -4354,8 +2005,8 @@ brcmf_sdbrcm_readframes(struct brcmf_bus *bus, uint maxframes, bool *finished)
 
 		/* Validate check bytes */
 		if ((u16) ~(len ^ check)) {
-			BRCMF_ERROR(("%s: HW hdr err: len/check "
-				     "0x%04x/0x%04x\n", __func__, len, check));
+			brcmf_dbg(ERROR, "HW hdr err: len/check 0x%04x/0x%04x\n",
+				  len, check);
 			bus->rx_badhdr++;
 			brcmf_sdbrcm_rxfail(bus, false, false);
 			continue;
@@ -4363,8 +2014,7 @@ brcmf_sdbrcm_readframes(struct brcmf_bus *bus, uint maxframes, bool *finished)
 
 		/* Validate frame length */
 		if (len < SDPCM_HDRLEN) {
-			BRCMF_ERROR(("%s: HW hdr length invalid: %d\n",
-				     __func__, len));
+			brcmf_dbg(ERROR, "HW hdr length invalid: %d\n", len);
 			continue;
 		}
 
@@ -4376,9 +2026,8 @@ brcmf_sdbrcm_readframes(struct brcmf_bus *bus, uint maxframes, bool *finished)
 
 		/* Validate data offset */
 		if ((doff < SDPCM_HDRLEN) || (doff > len)) {
-			BRCMF_ERROR(("%s: Bad data offset %d: HW len %d,"
-				     " min %d seq %d\n", __func__, doff,
-				     len, SDPCM_HDRLEN, seq));
+			brcmf_dbg(ERROR, "Bad data offset %d: HW len %d, min %d seq %d\n",
+				  doff, len, SDPCM_HDRLEN, seq);
 			bus->rx_badhdr++;
 			brcmf_sdbrcm_rxfail(bus, false, false);
 			continue;
@@ -4388,9 +2037,8 @@ brcmf_sdbrcm_readframes(struct brcmf_bus *bus, uint maxframes, bool *finished)
 		bus->nextlen =
 		    bus->rxhdr[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
 		if ((bus->nextlen << 4) > MAX_RX_DATASZ) {
-			BRCMF_INFO(("%s (nextlen): got frame w/nextlen too"
-				    " large (%d), seq %d\n",
-				    __func__, bus->nextlen, seq));
+			brcmf_dbg(INFO, "(nextlen): got frame w/nextlen too large (%d), seq %d\n",
+				  bus->nextlen, seq);
 			bus->nextlen = 0;
 		}
 
@@ -4410,16 +2058,15 @@ brcmf_sdbrcm_readframes(struct brcmf_bus *bus, uint maxframes, bool *finished)
 
 		/* Check and update sequence number */
 		if (rxseq != seq) {
-			BRCMF_INFO(("%s: rx_seq %d, expected %d\n", __func__,
-				    seq, rxseq));
+			brcmf_dbg(INFO, "rx_seq %d, expected %d\n", seq, rxseq);
 			bus->rx_badseq++;
 			rxseq = seq;
 		}
 
 		/* Check window for sanity */
 		if ((u8) (txmax - bus->tx_seq) > 0x40) {
-			BRCMF_ERROR(("%s: unlikely tx max %d with tx_seq %d\n",
-				     __func__, txmax, bus->tx_seq));
+			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;
@@ -4435,61 +2082,59 @@ brcmf_sdbrcm_readframes(struct brcmf_bus *bus, uint maxframes, bool *finished)
 		   SDPCM_GLOM_CHANNEL */
 
 		/* Length to read */
-		rdlen = (len > firstread) ? (len - firstread) : 0;
+		rdlen = (len > BRCMF_FIRSTREAD) ? (len - BRCMF_FIRSTREAD) : 0;
 
 		/* May pad read to blocksize for efficiency */
 		if (bus->roundup && bus->blocksize &&
 			(rdlen > bus->blocksize)) {
 			pad = bus->blocksize - (rdlen % bus->blocksize);
 			if ((pad <= bus->roundup) && (pad < bus->blocksize) &&
-			    ((rdlen + pad + firstread) < MAX_RX_DATASZ))
+			    ((rdlen + pad + BRCMF_FIRSTREAD) < MAX_RX_DATASZ))
 				rdlen += pad;
 		} else if (rdlen % BRCMF_SDALIGN) {
 			rdlen += BRCMF_SDALIGN - (rdlen % BRCMF_SDALIGN);
 		}
 
 		/* Satisfy length-alignment requirements */
-		if (forcealign && (rdlen & (ALIGNMENT - 1)))
+		if (rdlen & (ALIGNMENT - 1))
 			rdlen = roundup(rdlen, ALIGNMENT);
 
-		if ((rdlen + firstread) > MAX_RX_DATASZ) {
+		if ((rdlen + BRCMF_FIRSTREAD) > MAX_RX_DATASZ) {
 			/* Too long -- skip this frame */
-			BRCMF_ERROR(("%s: too long: len %d rdlen %d\n",
-				     __func__, len, rdlen));
+			brcmf_dbg(ERROR, "too long: len %d rdlen %d\n",
+				  len, rdlen);
 			bus->drvr->rx_errors++;
 			bus->rx_toolong++;
 			brcmf_sdbrcm_rxfail(bus, false, false);
 			continue;
 		}
 
-		pkt = brcmu_pkt_buf_get_skb(rdlen + firstread + BRCMF_SDALIGN);
+		pkt = brcmu_pkt_buf_get_skb(rdlen +
+					    BRCMF_FIRSTREAD + BRCMF_SDALIGN);
 		if (!pkt) {
 			/* Give up on data, request rtx of events */
-			BRCMF_ERROR(("%s: brcmu_pkt_buf_get_skb failed:"
-				     " rdlen %d chan %d\n", __func__, rdlen,
-				     chan));
+			brcmf_dbg(ERROR, "brcmu_pkt_buf_get_skb failed: rdlen %d chan %d\n",
+				  rdlen, chan);
 			bus->drvr->rx_dropped++;
 			brcmf_sdbrcm_rxfail(bus, false, RETRYCHAN(chan));
 			continue;
 		}
 
 		/* Leave room for what we already read, and align remainder */
-		skb_pull(pkt, firstread);
-		PKTALIGN(pkt, rdlen, BRCMF_SDALIGN);
+		skb_pull(pkt, BRCMF_FIRSTREAD);
+		pkt_align(pkt, rdlen, BRCMF_SDALIGN);
 
 		/* Read the remaining frame data */
-		sdret = brcmf_sdcard_recv_buf(card,
-				brcmf_sdcard_cur_sbwad(card),
+		sdret = brcmf_sdcard_recv_buf(bus->sdiodev, bus->sdiodev->sbwad,
 				SDIO_FUNC_2, F2SYNC, ((u8 *) (pkt->data)),
-				rdlen, pkt, NULL, NULL);
+				rdlen, pkt);
 		bus->f2rxdata++;
 
 		if (sdret < 0) {
-			BRCMF_ERROR(("%s: read %d %s bytes failed: %d\n",
-				     __func__, rdlen,
-				     ((chan == SDPCM_EVENT_CHANNEL) ? "event"
-				     : ((chan == SDPCM_DATA_CHANNEL) ? "data"
-				     : "test")), sdret));
+			brcmf_dbg(ERROR, "read %d %s bytes failed: %d\n", rdlen,
+				  ((chan == SDPCM_EVENT_CHANNEL) ? "event"
+				   : ((chan == SDPCM_DATA_CHANNEL) ? "data"
+				      : "test")), sdret);
 			brcmu_pkt_buf_free_skb(pkt);
 			bus->drvr->rx_errors++;
 			brcmf_sdbrcm_rxfail(bus, true, RETRYCHAN(chan));
@@ -4497,8 +2142,8 @@ brcmf_sdbrcm_readframes(struct brcmf_bus *bus, uint maxframes, bool *finished)
 		}
 
 		/* Copy the already-read portion */
-		skb_push(pkt, firstread);
-		memcpy(pkt->data, bus->rxhdr, firstread);
+		skb_push(pkt, BRCMF_FIRSTREAD);
+		memcpy(pkt->data, bus->rxhdr, BRCMF_FIRSTREAD);
 
 #ifdef BCMDBG
 		if (BRCMF_BYTES_ON() && BRCMF_DATA_ON()) {
@@ -4512,8 +2157,8 @@ deliver:
 		/* Save superframe descriptor and allocate packet frame */
 		if (chan == SDPCM_GLOM_CHANNEL) {
 			if (SDPCM_GLOMDESC(&bus->rxhdr[SDPCM_FRAMETAG_LEN])) {
-				BRCMF_GLOM(("%s: glom descriptor, %d bytes:\n",
-					    __func__, len));
+				brcmf_dbg(GLOM, "glom descriptor, %d bytes:\n",
+					  len);
 #ifdef BCMDBG
 				if (BRCMF_GLOM_ON()) {
 					printk(KERN_DEBUG "Glom Data:\n");
@@ -4526,8 +2171,8 @@ deliver:
 				skb_pull(pkt, SDPCM_HDRLEN);
 				bus->glomd = pkt;
 			} else {
-				BRCMF_ERROR(("%s: glom superframe w/o "
-					     "descriptor!\n", __func__));
+				brcmf_dbg(ERROR, "%s: glom superframe w/o "
+					  "descriptor!\n", __func__);
 				brcmf_sdbrcm_rxfail(bus, false, false);
 			}
 			continue;
@@ -4537,38 +2182,30 @@ deliver:
 		__skb_trim(pkt, len);
 		skb_pull(pkt, doff);
 
-#ifdef SDTEST
-		/* Test channel packets are processed separately */
-		if (chan == SDPCM_TEST_CHANNEL) {
-			brcmf_sdbrcm_checkdied(bus, pkt, seq);
-			continue;
-		}
-#endif				/* SDTEST */
-
 		if (pkt->len == 0) {
 			brcmu_pkt_buf_free_skb(pkt);
 			continue;
 		} else if (brcmf_proto_hdrpull(bus->drvr, &ifidx, pkt) != 0) {
-			BRCMF_ERROR(("%s: rx protocol error\n", __func__));
+			brcmf_dbg(ERROR, "rx protocol error\n");
 			brcmu_pkt_buf_free_skb(pkt);
 			bus->drvr->rx_errors++;
 			continue;
 		}
 
 		/* Unlock during rx call */
-		brcmf_sdbrcm_sdunlock(bus);
+		up(&bus->sdsem);
 		brcmf_rx_frame(bus->drvr, ifidx, pkt, 1);
-		brcmf_sdbrcm_sdlock(bus);
+		down(&bus->sdsem);
 	}
 	rxcount = maxframes - rxleft;
 #ifdef BCMDBG
 	/* Message if we hit the limit */
-	if (!rxleft && !sdtest)
-		BRCMF_DATA(("%s: hit rx limit of %d frames\n", __func__,
-			    maxframes));
+	if (!rxleft)
+		brcmf_dbg(DATA, "hit rx limit of %d frames\n",
+			  maxframes);
 	else
 #endif				/* BCMDBG */
-		BRCMF_DATA(("%s: processed %d frames\n", __func__, rxcount));
+		brcmf_dbg(DATA, "processed %d frames\n", rxcount);
 	/* Back off rxseq if awaiting rtx, update rx_seq */
 	if (bus->rxskip)
 		rxseq--;
@@ -4577,100 +2214,243 @@ deliver:
 	return rxcount;
 }
 
-static u32 brcmf_sdbrcm_hostmail(struct brcmf_bus *bus)
+static int
+brcmf_sdbrcm_send_buf(struct brcmf_bus *bus, u32 addr, uint fn, uint flags,
+		    u8 *buf, uint nbytes, struct sk_buff *pkt)
 {
-	u32 intstatus = 0;
-	u32 hmb_data;
-	u8 fcbits;
-	uint retries = 0;
+	return brcmf_sdcard_send_buf
+		(bus->sdiodev, addr, fn, flags, buf, nbytes, pkt);
+}
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+static void
+brcmf_sdbrcm_wait_for_event(struct brcmf_bus *bus, bool *lockvar)
+{
+	up(&bus->sdsem);
+	wait_event_interruptible_timeout(bus->ctrl_wait,
+					 (*lockvar == false), HZ * 2);
+	down(&bus->sdsem);
+	return;
+}
 
-	/* Read mailbox data and ack that we did so */
-	r_sdreg32(bus, &hmb_data,
-		  offsetof(struct sdpcmd_regs, tohostmailboxdata), &retries);
+static void
+brcmf_sdbrcm_wait_event_wakeup(struct brcmf_bus *bus)
+{
+	if (waitqueue_active(&bus->ctrl_wait))
+		wake_up_interruptible(&bus->ctrl_wait);
+	return;
+}
 
-	if (retries <= retry_limit)
-		w_sdreg32(bus, SMB_INT_ACK,
-			  offsetof(struct sdpcmd_regs, tosbmailbox), &retries);
-	bus->f1regdata += 2;
+/* Writes a HW/SW header into the packet and sends it. */
+/* Assumes: (a) header space already there, (b) caller holds lock */
+static int brcmf_sdbrcm_txpkt(struct brcmf_bus *bus, struct sk_buff *pkt,
+			      uint chan, bool free_pkt)
+{
+	int ret;
+	u8 *frame;
+	u16 len, pad = 0;
+	u32 swheader;
+	struct sk_buff *new;
+	int i;
 
-	/* Dongle recomposed rx frames, accept them again */
-	if (hmb_data & HMB_DATA_NAKHANDLED) {
-		BRCMF_INFO(("Dongle reports NAK handled, expect rtx of %d\n",
-			    bus->rx_seq));
-		if (!bus->rxskip)
-			BRCMF_ERROR(("%s: unexpected NAKHANDLED!\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
-		bus->rxskip = false;
-		intstatus |= I_HMB_FRAME_IND;
+	frame = (u8 *) (pkt->data);
+
+	/* Add alignment padding, allocate new packet if needed */
+	pad = ((unsigned long)frame % BRCMF_SDALIGN);
+	if (pad) {
+		if (skb_headroom(pkt) < pad) {
+			brcmf_dbg(INFO, "insufficient headroom %d for %d pad\n",
+				  skb_headroom(pkt), pad);
+			bus->drvr->tx_realloc++;
+			new = brcmu_pkt_buf_get_skb(pkt->len + BRCMF_SDALIGN);
+			if (!new) {
+				brcmf_dbg(ERROR, "couldn't allocate new %d-byte packet\n",
+					  pkt->len + BRCMF_SDALIGN);
+				ret = -ENOMEM;
+				goto done;
+			}
+
+			pkt_align(new, pkt->len, BRCMF_SDALIGN);
+			memcpy(new->data, pkt->data, pkt->len);
+			if (free_pkt)
+				brcmu_pkt_buf_free_skb(pkt);
+			/* free the pkt if canned one is not used */
+			free_pkt = true;
+			pkt = new;
+			frame = (u8 *) (pkt->data);
+			/* precondition: (frame % BRCMF_SDALIGN) == 0) */
+			pad = 0;
+		} else {
+			skb_push(pkt, pad);
+			frame = (u8 *) (pkt->data);
+			/* precondition: pad + SDPCM_HDRLEN <= pkt->len */
+			memset(frame, 0, pad + SDPCM_HDRLEN);
+		}
 	}
+	/* precondition: pad < BRCMF_SDALIGN */
 
-	/*
-	 * DEVREADY does not occur with gSPI.
-	 */
-	if (hmb_data & (HMB_DATA_DEVREADY | HMB_DATA_FWREADY)) {
-		bus->sdpcm_ver =
-		    (hmb_data & HMB_DATA_VERSION_MASK) >>
-		    HMB_DATA_VERSION_SHIFT;
-		if (bus->sdpcm_ver != SDPCM_PROT_VERSION)
-			BRCMF_ERROR(("Version mismatch, dongle reports %d, "
-				     "expecting %d\n",
-				     bus->sdpcm_ver, SDPCM_PROT_VERSION));
-		else
-			BRCMF_INFO(("Dongle ready, protocol version %d\n",
-				    bus->sdpcm_ver));
+	/* Hardware tag: 2 byte len followed by 2 byte ~len check (all LE) */
+	len = (u16) (pkt->len);
+	*(__le16 *) frame = cpu_to_le16(len);
+	*(((__le16 *) frame) + 1) = cpu_to_le16(~len);
+
+	/* Software tag: channel, sequence number, data offset */
+	swheader =
+	    ((chan << SDPCM_CHANNEL_SHIFT) & SDPCM_CHANNEL_MASK) | bus->tx_seq |
+	    (((pad +
+	       SDPCM_HDRLEN) << SDPCM_DOFFSET_SHIFT) & SDPCM_DOFFSET_MASK);
+
+	put_unaligned_le32(swheader, frame + SDPCM_FRAMETAG_LEN);
+	put_unaligned_le32(0, frame + SDPCM_FRAMETAG_LEN + sizeof(swheader));
+
+#ifdef BCMDBG
+	tx_packets[pkt->priority]++;
+	if (BRCMF_BYTES_ON() &&
+	    (((BRCMF_CTL_ON() && (chan == SDPCM_CONTROL_CHANNEL)) ||
+	      (BRCMF_DATA_ON() && (chan != SDPCM_CONTROL_CHANNEL))))) {
+		printk(KERN_DEBUG "Tx Frame:\n");
+		print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, frame, len);
+	} else if (BRCMF_HDRS_ON()) {
+		printk(KERN_DEBUG "TxHdr:\n");
+		print_hex_dump_bytes("", DUMP_PREFIX_OFFSET,
+				     frame, min_t(u16, len, 16));
 	}
+#endif
 
-	/*
-	 * Flow Control has been moved into the RX headers and this out of band
-	 * method isn't used any more.
-	 * remaining backward compatible with older dongles.
-	 */
-	if (hmb_data & HMB_DATA_FC) {
-		fcbits = (hmb_data & HMB_DATA_FCDATA_MASK) >>
-							HMB_DATA_FCDATA_SHIFT;
+	/* Raise len to next SDIO block to eliminate tail command */
+	if (bus->roundup && bus->blocksize && (len > bus->blocksize)) {
+		u16 pad = bus->blocksize - (len % bus->blocksize);
+		if ((pad <= bus->roundup) && (pad < bus->blocksize))
+				len += pad;
+	} else if (len % BRCMF_SDALIGN) {
+		len += BRCMF_SDALIGN - (len % BRCMF_SDALIGN);
+	}
 
-		if (fcbits & ~bus->flowcontrol)
-			bus->fc_xoff++;
+	/* Some controllers have trouble with odd bytes -- round to even */
+	if (len & (ALIGNMENT - 1))
+			len = roundup(len, ALIGNMENT);
 
-		if (bus->flowcontrol & ~fcbits)
-			bus->fc_xon++;
+	ret = brcmf_sdbrcm_send_buf(bus, bus->sdiodev->sbwad,
+				    SDIO_FUNC_2, F2SYNC, frame,
+				    len, pkt);
+	bus->f2txdata++;
+
+	if (ret < 0) {
+		/* On failure, abort the command and terminate the frame */
+		brcmf_dbg(INFO, "sdio error %d, abort command and terminate frame\n",
+			  ret);
+		bus->tx_sderrs++;
+
+		brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
+		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
+				 SBSDIO_FUNC1_FRAMECTRL, SFC_WF_TERM,
+				 NULL);
+		bus->f1regdata++;
+
+		for (i = 0; i < 3; i++) {
+			u8 hi, lo;
+			hi = brcmf_sdcard_cfg_read(bus->sdiodev,
+					     SDIO_FUNC_1,
+					     SBSDIO_FUNC1_WFRAMEBCHI,
+					     NULL);
+			lo = brcmf_sdcard_cfg_read(bus->sdiodev,
+					     SDIO_FUNC_1,
+					     SBSDIO_FUNC1_WFRAMEBCLO,
+					     NULL);
+			bus->f1regdata += 2;
+			if ((hi == 0) && (lo == 0))
+				break;
+		}
 
-		bus->fc_rcvd++;
-		bus->flowcontrol = fcbits;
 	}
+	if (ret == 0)
+		bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQUENCE_WRAP;
 
-	/* Shouldn't be any others */
-	if (hmb_data & ~(HMB_DATA_DEVREADY |
-			 HMB_DATA_NAKHANDLED |
-			 HMB_DATA_FC |
-			 HMB_DATA_FWREADY |
-			 HMB_DATA_FCDATA_MASK | HMB_DATA_VERSION_MASK)) {
-		BRCMF_ERROR(("Unknown mailbox data content: 0x%02x\n",
-			     hmb_data));
+done:
+	/* restore pkt buffer pointer before calling tx complete routine */
+	skb_pull(pkt, SDPCM_HDRLEN + pad);
+	up(&bus->sdsem);
+	brcmf_txcomplete(bus->drvr, pkt, ret != 0);
+	down(&bus->sdsem);
+
+	if (free_pkt)
+		brcmu_pkt_buf_free_skb(pkt);
+
+	return ret;
+}
+
+static uint brcmf_sdbrcm_sendfromq(struct brcmf_bus *bus, uint maxframes)
+{
+	struct sk_buff *pkt;
+	u32 intstatus = 0;
+	uint retries = 0;
+	int ret = 0, prec_out;
+	uint cnt = 0;
+	uint datalen;
+	u8 tx_prec_map;
+
+	struct brcmf_pub *drvr = bus->drvr;
+
+	brcmf_dbg(TRACE, "Enter\n");
+
+	tx_prec_map = ~bus->flowcontrol;
+
+	/* Send frames until the limit or some other event */
+	for (cnt = 0; (cnt < maxframes) && data_ok(bus); cnt++) {
+		spin_lock_bh(&bus->txqlock);
+		pkt = brcmu_pktq_mdeq(&bus->txq, tx_prec_map, &prec_out);
+		if (pkt == NULL) {
+			spin_unlock_bh(&bus->txqlock);
+			break;
+		}
+		spin_unlock_bh(&bus->txqlock);
+		datalen = pkt->len - SDPCM_HDRLEN;
+
+		ret = brcmf_sdbrcm_txpkt(bus, pkt, SDPCM_DATA_CHANNEL, true);
+		if (ret)
+			bus->drvr->tx_errors++;
+		else
+			bus->drvr->dstats.tx_bytes += datalen;
+
+		/* In poll mode, need to check for other events */
+		if (!bus->intr && cnt) {
+			/* Check device status, signal pending interrupt */
+			r_sdreg32(bus, &intstatus,
+				  offsetof(struct sdpcmd_regs, intstatus),
+				  &retries);
+			bus->f2txdata++;
+			if (brcmf_sdcard_regfail(bus->sdiodev))
+				break;
+			if (intstatus & bus->hostintmask)
+				bus->ipend = true;
+		}
 	}
 
-	return intstatus;
+	/* Deflow-control stack if needed */
+	if (drvr->up && (drvr->busstate == BRCMF_BUS_DATA) &&
+	    drvr->txoff && (pktq_len(&bus->txq) < TXLOW))
+		brcmf_txflowcontrol(drvr, 0, OFF);
+
+	return cnt;
 }
 
 static bool brcmf_sdbrcm_dpc(struct brcmf_bus *bus)
 {
-	struct brcmf_sdio_card *card = bus->card;
 	u32 intstatus, newstatus = 0;
 	uint retries = 0;
-	uint rxlimit = brcmf_rxbound;	/* Rx frames to read before resched */
-	uint txlimit = brcmf_txbound;	/* Tx frames to send before resched */
+	uint rxlimit = bus->rxbound;	/* Rx frames to read before resched */
+	uint txlimit = bus->txbound;	/* Tx frames to send before resched */
 	uint framecnt = 0;	/* Temporary counter of tx/rx frames */
 	bool rxdone = true;	/* Flag for no more read data */
 	bool resched = false;	/* Flag indicating resched wanted */
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
 	/* Start with leftover status bits */
 	intstatus = bus->intstatus;
 
-	brcmf_sdbrcm_sdlock(bus);
+	down(&bus->sdsem);
 
 	/* If waiting for HTAVAIL, check status */
 	if (bus->clkstate == CLK_PENDING) {
@@ -4679,41 +2459,41 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_bus *bus)
 
 #ifdef BCMDBG
 		/* Check for inconsistent device control */
-		devctl = brcmf_sdcard_cfg_read(card, SDIO_FUNC_1,
+		devctl = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
 					       SBSDIO_DEVICE_CTL, &err);
 		if (err) {
-			BRCMF_ERROR(("%s: error reading DEVCTL: %d\n",
-				     __func__, err));
+			brcmf_dbg(ERROR, "error reading DEVCTL: %d\n", err);
 			bus->drvr->busstate = BRCMF_BUS_DOWN;
 		}
 #endif				/* BCMDBG */
 
 		/* Read CSR, if clock on switch to AVAIL, else ignore */
-		clkctl = brcmf_sdcard_cfg_read(card, SDIO_FUNC_1,
+		clkctl = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
 					       SBSDIO_FUNC1_CHIPCLKCSR, &err);
 		if (err) {
-			BRCMF_ERROR(("%s: error reading CSR: %d\n", __func__,
-				     err));
+			brcmf_dbg(ERROR, "error reading CSR: %d\n",
+				  err);
 			bus->drvr->busstate = BRCMF_BUS_DOWN;
 		}
 
-		BRCMF_INFO(("DPC: PENDING, devctl 0x%02x clkctl 0x%02x\n",
-			    devctl, clkctl));
+		brcmf_dbg(INFO, "DPC: PENDING, devctl 0x%02x clkctl 0x%02x\n",
+			  devctl, clkctl);
 
 		if (SBSDIO_HTAV(clkctl)) {
-			devctl = brcmf_sdcard_cfg_read(card, SDIO_FUNC_1,
+			devctl = brcmf_sdcard_cfg_read(bus->sdiodev,
+						       SDIO_FUNC_1,
 						       SBSDIO_DEVICE_CTL, &err);
 			if (err) {
-				BRCMF_ERROR(("%s: error reading DEVCTL: %d\n",
-					     __func__, err));
+				brcmf_dbg(ERROR, "error reading DEVCTL: %d\n",
+					  err);
 				bus->drvr->busstate = BRCMF_BUS_DOWN;
 			}
 			devctl &= ~SBSDIO_DEVCTL_CA_INT_ONLY;
-			brcmf_sdcard_cfg_write(card, SDIO_FUNC_1,
+			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
 				SBSDIO_DEVICE_CTL, devctl, &err);
 			if (err) {
-				BRCMF_ERROR(("%s: error writing DEVCTL: %d\n",
-					     __func__, err));
+				brcmf_dbg(ERROR, "error writing DEVCTL: %d\n",
+					  err);
 				bus->drvr->busstate = BRCMF_BUS_DOWN;
 			}
 			bus->clkstate = CLK_AVAIL;
@@ -4722,7 +2502,7 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_bus *bus)
 		}
 	}
 
-	BUS_WAKE(bus);
+	bus_wake(bus);
 
 	/* Make sure backplane clock is on */
 	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, true);
@@ -4735,7 +2515,7 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_bus *bus)
 		r_sdreg32(bus, &newstatus,
 			  offsetof(struct sdpcmd_regs, intstatus), &retries);
 		bus->f1regdata++;
-		if (brcmf_sdcard_regfail(bus->card))
+		if (brcmf_sdcard_regfail(bus->sdiodev))
 			newstatus = 0;
 		newstatus &= bus->hostintmask;
 		bus->fcstate = !!(newstatus & I_HMB_FC_STATE);
@@ -4776,23 +2556,23 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_bus *bus)
 
 	/* Generally don't ask for these, can get CRC errors... */
 	if (intstatus & I_WR_OOSYNC) {
-		BRCMF_ERROR(("Dongle reports WR_OOSYNC\n"));
+		brcmf_dbg(ERROR, "Dongle reports WR_OOSYNC\n");
 		intstatus &= ~I_WR_OOSYNC;
 	}
 
 	if (intstatus & I_RD_OOSYNC) {
-		BRCMF_ERROR(("Dongle reports RD_OOSYNC\n"));
+		brcmf_dbg(ERROR, "Dongle reports RD_OOSYNC\n");
 		intstatus &= ~I_RD_OOSYNC;
 	}
 
 	if (intstatus & I_SBINT) {
-		BRCMF_ERROR(("Dongle reports SBINT\n"));
+		brcmf_dbg(ERROR, "Dongle reports SBINT\n");
 		intstatus &= ~I_SBINT;
 	}
 
 	/* Would be active due to wake-wlan in gSPI */
 	if (intstatus & I_CHIPACTIVE) {
-		BRCMF_INFO(("Dongle reports CHIPACTIVE\n"));
+		brcmf_dbg(INFO, "Dongle reports CHIPACTIVE\n");
 		intstatus &= ~I_CHIPACTIVE;
 	}
 
@@ -4812,45 +2592,36 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_bus *bus)
 	bus->intstatus = intstatus;
 
 clkwait:
-	/* Re-enable interrupts to detect new device events (mailbox, rx frame)
-	 * or clock availability.  (Allows tx loop to check ipend if desired.)
-	 * (Unless register access seems hosed, as we may not be able to ACK...)
-	 */
-	if (bus->intr && bus->intdis && !brcmf_sdcard_regfail(card)) {
-		BRCMF_INTR(("%s: enable SDIO interrupts, rxdone %d"
-			    " framecnt %d\n", __func__, rxdone, framecnt));
-		bus->intdis = false;
-		brcmf_sdcard_intr_enable(card);
-	}
-
-	if (DATAOK(bus) && bus->ctrl_frame_stat &&
+	if (data_ok(bus) && bus->ctrl_frame_stat &&
 		(bus->clkstate == CLK_AVAIL)) {
 		int ret, i;
 
-		ret = brcmf_sdbrcm_send_buf(bus, brcmf_sdcard_cur_sbwad(card),
+		ret = brcmf_sdbrcm_send_buf(bus, bus->sdiodev->sbwad,
 			SDIO_FUNC_2, F2SYNC, (u8 *) bus->ctrl_frame_buf,
-			(u32) bus->ctrl_frame_len, NULL, NULL, NULL);
+			(u32) bus->ctrl_frame_len, NULL);
 
 		if (ret < 0) {
 			/* On failure, abort the command and
 				terminate the frame */
-			BRCMF_INFO(("%s: sdio error %d, abort command and "
-				    "terminate frame.\n", __func__, ret));
+			brcmf_dbg(INFO, "sdio error %d, abort command and terminate frame\n",
+				  ret);
 			bus->tx_sderrs++;
 
-			brcmf_sdcard_abort(card, SDIO_FUNC_2);
+			brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
 
-			brcmf_sdcard_cfg_write(card, SDIO_FUNC_1,
+			brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
 					 SBSDIO_FUNC1_FRAMECTRL, SFC_WF_TERM,
 					 NULL);
 			bus->f1regdata++;
 
 			for (i = 0; i < 3; i++) {
 				u8 hi, lo;
-				hi = brcmf_sdcard_cfg_read(card, SDIO_FUNC_1,
+				hi = brcmf_sdcard_cfg_read(bus->sdiodev,
+						     SDIO_FUNC_1,
 						     SBSDIO_FUNC1_WFRAMEBCHI,
 						     NULL);
-				lo = brcmf_sdcard_cfg_read(card, SDIO_FUNC_1,
+				lo = brcmf_sdcard_cfg_read(bus->sdiodev,
+						     SDIO_FUNC_1,
 						     SBSDIO_FUNC1_WFRAMEBCLO,
 						     NULL);
 				bus->f1regdata += 2;
@@ -4862,15 +2633,15 @@ clkwait:
 		if (ret == 0)
 			bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQUENCE_WRAP;
 
-		BRCMF_INFO(("Return_dpc value is : %d\n", ret));
+		brcmf_dbg(INFO, "Return_dpc value is : %d\n", ret);
 		bus->ctrl_frame_stat = false;
 		brcmf_sdbrcm_wait_event_wakeup(bus);
 	}
 	/* Send queued frames (limit 1 if rx may still be pending) */
 	else if ((bus->clkstate == CLK_AVAIL) && !bus->fcstate &&
 		 brcmu_pktq_mlen(&bus->txq, ~bus->flowcontrol) && txlimit
-		 && DATAOK(bus)) {
-		framecnt = rxdone ? txlimit : min(txlimit, brcmf_txminmax);
+		 && data_ok(bus)) {
+		framecnt = rxdone ? txlimit : min(txlimit, bus->txminmax);
 		framecnt = brcmf_sdbrcm_sendfromq(bus, framecnt);
 		txlimit -= framecnt;
 	}
@@ -4880,19 +2651,17 @@ clkwait:
 	/* On failed register access, all bets are off:
 		 no resched or interrupts */
 	if ((bus->drvr->busstate == BRCMF_BUS_DOWN) ||
-	    brcmf_sdcard_regfail(card)) {
-		BRCMF_ERROR(("%s: failed backplane access over SDIO, halting "
-			     "operation %d\n", __func__,
-			     brcmf_sdcard_regfail(card)));
+	    brcmf_sdcard_regfail(bus->sdiodev)) {
+		brcmf_dbg(ERROR, "failed backplane access over SDIO, halting operation %d\n",
+			  brcmf_sdcard_regfail(bus->sdiodev));
 		bus->drvr->busstate = BRCMF_BUS_DOWN;
 		bus->intstatus = 0;
 	} else if (bus->clkstate == CLK_PENDING) {
-		BRCMF_INFO(("%s: rescheduled due to CLK_PENDING awaiting "
-			    "I_CHIPACTIVE interrupt\n", __func__));
+		brcmf_dbg(INFO, "rescheduled due to CLK_PENDING awaiting I_CHIPACTIVE interrupt\n");
 		resched = true;
 	} else if (bus->intstatus || bus->ipend ||
 		(!bus->fcstate && brcmu_pktq_mlen(&bus->txq, ~bus->flowcontrol)
-		 && DATAOK(bus)) || PKT_AVAILABLE()) {
+		 && data_ok(bus)) || PKT_AVAILABLE()) {
 		resched = true;
 	}
 
@@ -4905,1010 +2674,733 @@ clkwait:
 		brcmf_sdbrcm_clkctl(bus, CLK_NONE, false);
 	}
 
-	brcmf_sdbrcm_sdunlock(bus);
+	up(&bus->sdsem);
 
 	return resched;
 }
 
-void brcmf_sdbrcm_isr(void *arg)
+static int brcmf_sdbrcm_dpc_thread(void *data)
 {
-	struct brcmf_bus *bus = (struct brcmf_bus *) arg;
-	struct brcmf_sdio_card *card;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	if (!bus) {
-		BRCMF_ERROR(("%s : bus is null pointer , exit\n", __func__));
-		return;
-	}
-	card = bus->card;
-
-	if (bus->drvr->busstate == BRCMF_BUS_DOWN) {
-		BRCMF_ERROR(("%s : bus is down. we have nothing to do\n",
-			   __func__));
-		return;
-	}
-	/* Count the interrupt call */
-	bus->intrcount++;
-	bus->ipend = true;
+	struct brcmf_bus *bus = (struct brcmf_bus *) data;
 
-	/* Shouldn't get this interrupt if we're sleeping? */
-	if (bus->sleeping) {
-		BRCMF_ERROR(("INTERRUPT WHILE SLEEPING??\n"));
-		return;
+	allow_signal(SIGTERM);
+	/* Run until signal received */
+	while (1) {
+		if (kthread_should_stop())
+			break;
+		if (!wait_for_completion_interruptible(&bus->dpc_wait)) {
+			/* Call bus dpc unless it indicated down
+			(then clean stop) */
+			if (bus->drvr->busstate != BRCMF_BUS_DOWN) {
+				if (brcmf_sdbrcm_dpc(bus))
+					complete(&bus->dpc_wait);
+			} else {
+				/* after stopping the bus, exit thread */
+				brcmf_sdbrcm_bus_stop(bus);
+				bus->dpc_tsk = NULL;
+				break;
+			}
+		} else
+			break;
 	}
-
-	/* Disable additional interrupts (is this needed now)? */
-	if (bus->intr)
-		BRCMF_INTR(("%s: disable SDIO interrupts\n", __func__));
-	else
-		BRCMF_ERROR(("brcmf_sdbrcm_isr() w/o interrupt configured!\n"));
-
-	brcmf_sdcard_intr_disable(card);
-	bus->intdis = true;
-
-#if defined(SDIO_ISR_THREAD)
-	BRCMF_TRACE(("Calling brcmf_sdbrcm_dpc() from %s\n", __func__));
-	while (brcmf_sdbrcm_dpc(bus))
-		;
-#else
-	bus->dpc_sched = true;
-	brcmf_sdbrcm_sched_dpc(bus);
-#endif
-
+	return 0;
 }
 
-#ifdef SDTEST
-static void brcmf_sdbrcm_pktgen_init(struct brcmf_bus *bus)
+int brcmf_sdbrcm_bus_txdata(struct brcmf_bus *bus, struct sk_buff *pkt)
 {
-	/* Default to specified length, or full range */
-	if (brcmf_pktgen_len) {
-		bus->pktgen_maxlen = min(brcmf_pktgen_len,
-					 BRCMF_MAX_PKTGEN_LEN);
-		bus->pktgen_minlen = bus->pktgen_maxlen;
-	} else {
-		bus->pktgen_maxlen = BRCMF_MAX_PKTGEN_LEN;
-		bus->pktgen_minlen = 0;
-	}
-	bus->pktgen_len = (u16) bus->pktgen_minlen;
+	int ret = -EBADE;
+	uint datalen, prec;
 
-	/* Default to per-watchdog burst with 10s print time */
-	bus->pktgen_freq = 1;
-	bus->pktgen_print = 10000 / brcmf_watchdog_ms;
-	bus->pktgen_count = (brcmf_pktgen * brcmf_watchdog_ms + 999) / 1000;
+	brcmf_dbg(TRACE, "Enter\n");
 
-	/* Default to echo mode */
-	bus->pktgen_mode = BRCMF_PKTGEN_ECHO;
-	bus->pktgen_stop = 1;
-}
+	datalen = pkt->len;
 
-static void brcmf_sdbrcm_pktgen(struct brcmf_bus *bus)
-{
-	struct sk_buff *pkt;
-	u8 *data;
-	uint pktcount;
-	uint fillbyte;
-	u16 len;
+	/* Add space for the header */
+	skb_push(pkt, SDPCM_HDRLEN);
+	/* precondition: IS_ALIGNED((unsigned long)(pkt->data), 2) */
 
-	/* Display current count if appropriate */
-	if (bus->pktgen_print && (++bus->pktgen_ptick >= bus->pktgen_print)) {
-		bus->pktgen_ptick = 0;
-		printk(KERN_DEBUG "%s: send attempts %d rcvd %d\n",
-		       __func__, bus->pktgen_sent, bus->pktgen_rcvd);
-	}
+	prec = prio2prec((pkt->priority & PRIOMASK));
 
-	/* For recv mode, just make sure dongle has started sending */
-	if (bus->pktgen_mode == BRCMF_PKTGEN_RECV) {
-		if (!bus->pktgen_rcvd)
-			brcmf_sdbrcm_sdtest_set(bus, true);
-		return;
+	/* Check for existing queue, current flow-control,
+			 pending event, or pending clock */
+	brcmf_dbg(TRACE, "deferring pktq len %d\n", pktq_len(&bus->txq));
+	bus->fcqueued++;
+
+	/* Priority based enq */
+	spin_lock_bh(&bus->txqlock);
+	if (brcmf_c_prec_enq(bus->drvr, &bus->txq, pkt, prec) == false) {
+		skb_pull(pkt, SDPCM_HDRLEN);
+		brcmf_txcomplete(bus->drvr, pkt, false);
+		brcmu_pkt_buf_free_skb(pkt);
+		brcmf_dbg(ERROR, "out of bus->txq !!!\n");
+		ret = -ENOSR;
+	} else {
+		ret = 0;
 	}
+	spin_unlock_bh(&bus->txqlock);
 
-	/* Otherwise, generate or request the specified number of packets */
-	for (pktcount = 0; pktcount < bus->pktgen_count; pktcount++) {
-		/* Stop if total has been reached */
-		if (bus->pktgen_total
-		    && (bus->pktgen_sent >= bus->pktgen_total)) {
-			bus->pktgen_count = 0;
-			break;
-		}
-
-		/* Allocate an appropriate-sized packet */
-		len = bus->pktgen_len;
-		pkt = brcmu_pkt_buf_get_skb(
-			len + SDPCM_HDRLEN + SDPCM_TEST_HDRLEN + BRCMF_SDALIGN,
-			true);
-		if (!pkt) {
-			BRCMF_ERROR(("%s: brcmu_pkt_buf_get_skb failed!\n",
-				     __func__));
-			break;
-		}
-		PKTALIGN(pkt, (len + SDPCM_HDRLEN + SDPCM_TEST_HDRLEN),
-			 BRCMF_SDALIGN);
-		data = (u8 *) (pkt->data) + SDPCM_HDRLEN;
-
-		/* Write test header cmd and extra based on mode */
-		switch (bus->pktgen_mode) {
-		case BRCMF_PKTGEN_ECHO:
-			*data++ = SDPCM_TEST_ECHOREQ;
-			*data++ = (u8) bus->pktgen_sent;
-			break;
-
-		case BRCMF_PKTGEN_SEND:
-			*data++ = SDPCM_TEST_DISCARD;
-			*data++ = (u8) bus->pktgen_sent;
-			break;
-
-		case BRCMF_PKTGEN_RXBURST:
-			*data++ = SDPCM_TEST_BURST;
-			*data++ = (u8) bus->pktgen_count;
-			break;
-
-		default:
-			BRCMF_ERROR(("Unrecognized pktgen mode %d\n",
-				     bus->pktgen_mode));
-			brcmu_pkt_buf_free_skb(pkt, true);
-			bus->pktgen_count = 0;
-			return;
-		}
-
-		/* Write test header length field */
-		*data++ = (len >> 0);
-		*data++ = (len >> 8);
-
-		/* Then fill in the remainder -- N/A for burst,
-			 but who cares... */
-		for (fillbyte = 0; fillbyte < len; fillbyte++)
-			*data++ =
-			    SDPCM_TEST_FILL(fillbyte, (u8) bus->pktgen_sent);
+	if (pktq_len(&bus->txq) >= TXHI)
+		brcmf_txflowcontrol(bus->drvr, 0, ON);
 
 #ifdef BCMDBG
-		if (BRCMF_BYTES_ON() && BRCMF_DATA_ON()) {
-			data = (u8 *) (pkt->data) + SDPCM_HDRLEN;
-			printk(KERN_DEBUG "brcmf_sdbrcm_pktgen: Tx Data:\n");
-			print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, data,
-					     pkt->len - SDPCM_HDRLEN);
-		}
+	if (pktq_plen(&bus->txq, prec) > qcount[prec])
+		qcount[prec] = pktq_plen(&bus->txq, prec);
 #endif
-
-		/* Send it */
-		if (brcmf_sdbrcm_txpkt(bus, pkt, SDPCM_TEST_CHANNEL, true)) {
-			bus->pktgen_fail++;
-			if (bus->pktgen_stop
-			    && bus->pktgen_stop == bus->pktgen_fail)
-				bus->pktgen_count = 0;
-		}
-		bus->pktgen_sent++;
-
-		/* Bump length if not fixed, wrap at max */
-		if (++bus->pktgen_len > bus->pktgen_maxlen)
-			bus->pktgen_len = (u16) bus->pktgen_minlen;
-
-		/* Special case for burst mode: just send one request! */
-		if (bus->pktgen_mode == BRCMF_PKTGEN_RXBURST)
-			break;
+	/* Schedule DPC if needed to send queued packet(s) */
+	if (!bus->dpc_sched) {
+		bus->dpc_sched = true;
+		if (bus->dpc_tsk)
+			complete(&bus->dpc_wait);
 	}
-}
-
-static void brcmf_sdbrcm_sdtest_set(struct brcmf_bus *bus, bool start)
-{
-	struct sk_buff *pkt;
-	u8 *data;
 
-	/* Allocate the packet */
-	pkt = brcmu_pkt_buf_get_skb(SDPCM_HDRLEN + SDPCM_TEST_HDRLEN +
-		BRCMF_SDALIGN, true);
-	if (!pkt) {
-		BRCMF_ERROR(("%s: brcmu_pkt_buf_get_skb failed!\n", __func__));
-		return;
-	}
-	PKTALIGN(pkt, (SDPCM_HDRLEN + SDPCM_TEST_HDRLEN), BRCMF_SDALIGN);
-	data = (u8 *) (pkt->data) + SDPCM_HDRLEN;
-
-	/* Fill in the test header */
-	*data++ = SDPCM_TEST_SEND;
-	*data++ = start;
-	*data++ = (bus->pktgen_maxlen >> 0);
-	*data++ = (bus->pktgen_maxlen >> 8);
-
-	/* Send it */
-	if (brcmf_sdbrcm_txpkt(bus, pkt, SDPCM_TEST_CHANNEL, true))
-		bus->pktgen_fail++;
+	return ret;
 }
 
-static void
-brcmf_sdbrcm_checkdied(struct brcmf_bus *bus, struct sk_buff *pkt, uint seq)
+static int
+brcmf_sdbrcm_membytes(struct brcmf_bus *bus, bool write, u32 address, u8 *data,
+		 uint size)
 {
-	u8 *data;
-	uint pktlen;
+	int bcmerror = 0;
+	u32 sdaddr;
+	uint dsize;
 
-	u8 cmd;
-	u8 extra;
-	u16 len;
-	u16 offset;
-
-	/* Check for min length */
-	pktlen = pkt->len;
-	if (pktlen < SDPCM_TEST_HDRLEN) {
-		BRCMF_ERROR(("brcmf_sdbrcm_checkdied: toss runt frame, pktlen "
-			     "%d\n", pktlen));
-		brcmu_pkt_buf_free_skb(pkt, false);
-		return;
-	}
+	/* Determine initial transfer parameters */
+	sdaddr = address & SBSDIO_SB_OFT_ADDR_MASK;
+	if ((sdaddr + size) & SBSDIO_SBWINDOW_MASK)
+		dsize = (SBSDIO_SB_OFT_ADDR_LIMIT - sdaddr);
+	else
+		dsize = size;
 
-	/* Extract header fields */
-	data = pkt->data;
-	cmd = *data++;
-	extra = *data++;
-	len = *data++;
-	len += *data++ << 8;
-
-	/* Check length for relevant commands */
-	if (cmd == SDPCM_TEST_DISCARD || cmd == SDPCM_TEST_ECHOREQ
-	    || cmd == SDPCM_TEST_ECHORSP) {
-		if (pktlen != len + SDPCM_TEST_HDRLEN) {
-			BRCMF_ERROR(("brcmf_sdbrcm_checkdied: frame length "
-				     "mismatch, pktlen %d seq %d"
-				     " cmd %d extra %d len %d\n",
-				     pktlen, seq, cmd, extra, len));
-			brcmu_pkt_buf_free_skb(pkt, false);
-			return;
-		}
+	/* Set the backplane window to include the start address */
+	bcmerror = brcmf_sdcard_set_sbaddr_window(bus->sdiodev, address);
+	if (bcmerror) {
+		brcmf_dbg(ERROR, "window change failed\n");
+		goto xfer_done;
 	}
 
-	/* Process as per command */
-	switch (cmd) {
-	case SDPCM_TEST_ECHOREQ:
-		/* Rx->Tx turnaround ok (even on NDIS w/current
-			 implementation) */
-		*(u8 *) (pkt->data) = SDPCM_TEST_ECHORSP;
-		if (brcmf_sdbrcm_txpkt(bus, pkt, SDPCM_TEST_CHANNEL, true) == 0)
-			bus->pktgen_sent++;
-		else {
-			bus->pktgen_fail++;
-			brcmu_pkt_buf_free_skb(pkt, false);
-		}
-		bus->pktgen_rcvd++;
-		break;
-
-	case SDPCM_TEST_ECHORSP:
-		if (bus->ext_loop) {
-			brcmu_pkt_buf_free_skb(pkt, false);
-			bus->pktgen_rcvd++;
+	/* Do the transfer(s) */
+	while (size) {
+		brcmf_dbg(INFO, "%s %d bytes at offset 0x%08x in window 0x%08x\n",
+			  write ? "write" : "read", dsize,
+			  sdaddr, address & SBSDIO_SBWINDOW_MASK);
+		bcmerror = brcmf_sdcard_rwdata(bus->sdiodev, write,
+					       sdaddr, data, dsize);
+		if (bcmerror) {
+			brcmf_dbg(ERROR, "membytes transfer failed\n");
 			break;
 		}
 
-		for (offset = 0; offset < len; offset++, data++) {
-			if (*data != SDPCM_TEST_FILL(offset, extra)) {
-				BRCMF_ERROR(("brcmf_sdbrcm_checkdied: echo"
-					     " data mismatch: "
-					     "offset %d (len %d) "
-					     "expect 0x%02x rcvd 0x%02x\n",
-					     offset, len,
-					     SDPCM_TEST_FILL(offset, extra),
-					     *data));
+		/* Adjust for next transfer (if any) */
+		size -= dsize;
+		if (size) {
+			data += dsize;
+			address += dsize;
+			bcmerror = brcmf_sdcard_set_sbaddr_window(bus->sdiodev,
+								  address);
+			if (bcmerror) {
+				brcmf_dbg(ERROR, "window change failed\n");
 				break;
 			}
-		}
-		brcmu_pkt_buf_free_skb(pkt, false);
-		bus->pktgen_rcvd++;
-		break;
-
-	case SDPCM_TEST_DISCARD:
-		brcmu_pkt_buf_free_skb(pkt, false);
-		bus->pktgen_rcvd++;
-		break;
-
-	case SDPCM_TEST_BURST:
-	case SDPCM_TEST_SEND:
-	default:
-		BRCMF_INFO(("brcmf_sdbrcm_checkdied: unsupported or unknown "
-			    "command, pktlen %d seq %d" " cmd %d extra %d"
-			    " len %d\n", pktlen, seq, cmd, extra, len));
-		brcmu_pkt_buf_free_skb(pkt, false);
-		break;
-	}
-
-	/* For recv mode, stop at limie (and tell dongle to stop sending) */
-	if (bus->pktgen_mode == BRCMF_PKTGEN_RECV) {
-		if (bus->pktgen_total
-		    && (bus->pktgen_rcvd >= bus->pktgen_total)) {
-			bus->pktgen_count = 0;
-			brcmf_sdbrcm_sdtest_set(bus, false);
-		}
-	}
-}
-#endif				/* SDTEST */
-
-extern bool brcmf_sdbrcm_bus_watchdog(struct brcmf_pub *drvr)
-{
-	struct brcmf_bus *bus;
-
-	BRCMF_TIMER(("%s: Enter\n", __func__));
-
-	bus = drvr->bus;
-
-	if (bus->drvr->dongle_reset)
-		return false;
-
-	/* Ignore the timer if simulating bus down */
-	if (bus->sleeping)
-		return false;
-
-	brcmf_sdbrcm_sdlock(bus);
-
-	/* Poll period: check device if appropriate. */
-	if (bus->poll && (++bus->polltick >= bus->pollrate)) {
-		u32 intstatus = 0;
-
-		/* Reset poll tick */
-		bus->polltick = 0;
-
-		/* Check device if no interrupts */
-		if (!bus->intr || (bus->intrcount == bus->lastintrs)) {
-
-			if (!bus->dpc_sched) {
-				u8 devpend;
-				devpend = brcmf_sdcard_cfg_read(bus->card,
-						SDIO_FUNC_0, SDIO_CCCR_INTx,
-						NULL);
-				intstatus =
-				    devpend & (INTR_STATUS_FUNC1 |
-					       INTR_STATUS_FUNC2);
-			}
-
-			/* If there is something, make like the ISR and
-				 schedule the DPC */
-			if (intstatus) {
-				bus->pollcnt++;
-				bus->ipend = true;
-				if (bus->intr)
-					brcmf_sdcard_intr_disable(bus->card);
-
-				bus->dpc_sched = true;
-				brcmf_sdbrcm_sched_dpc(bus);
-
-			}
-		}
-
-		/* Update interrupt tracking */
-		bus->lastintrs = bus->intrcount;
-	}
-#ifdef BCMDBG
-	/* Poll for console output periodically */
-	if (drvr->busstate == BRCMF_BUS_DATA && brcmf_console_ms != 0) {
-		bus->console.count += brcmf_watchdog_ms;
-		if (bus->console.count >= brcmf_console_ms) {
-			bus->console.count -= brcmf_console_ms;
-			/* Make sure backplane clock is on */
-			brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
-			if (brcmf_sdbrcm_readconsole(bus) < 0)
-				brcmf_console_ms = 0;	/* On error,
-							 stop trying */
-		}
-	}
-#endif				/* BCMDBG */
-
-#ifdef SDTEST
-	/* Generate packets if configured */
-	if (bus->pktgen_count && (++bus->pktgen_tick >= bus->pktgen_freq)) {
-		/* Make sure backplane clock is on */
-		brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
-		bus->pktgen_tick = 0;
-		brcmf_sdbrcm_pktgen(bus);
-	}
-#endif
-
-	/* On idle timeout clear activity flag and/or turn off clock */
-	if ((bus->idletime > 0) && (bus->clkstate == CLK_AVAIL)) {
-		if (++bus->idlecount >= bus->idletime) {
-			bus->idlecount = 0;
-			if (bus->activity) {
-				bus->activity = false;
-				brcmf_sdbrcm_wd_timer(bus, brcmf_watchdog_ms);
-			} else {
-				brcmf_sdbrcm_clkctl(bus, CLK_NONE, false);
-			}
+			sdaddr = 0;
+			dsize = min_t(uint, SBSDIO_SB_OFT_ADDR_LIMIT, size);
 		}
 	}
 
-	brcmf_sdbrcm_sdunlock(bus);
+xfer_done:
+	/* Return the window to backplane enumeration space for core access */
+	if (brcmf_sdcard_set_sbaddr_window(bus->sdiodev, bus->sdiodev->sbwad))
+		brcmf_dbg(ERROR, "FAILED to set window back to 0x%x\n",
+			  bus->sdiodev->sbwad);
 
-	return bus->ipend;
+	return bcmerror;
 }
 
 #ifdef BCMDBG
-static int brcmf_sdbrcm_bus_console_in(struct brcmf_pub *drvr,
-				       unsigned char *msg, uint msglen)
+#define CONSOLE_LINE_MAX	192
+
+static int brcmf_sdbrcm_readconsole(struct brcmf_bus *bus)
 {
-	struct brcmf_bus *bus = drvr->bus;
-	u32 addr, val;
+	struct brcmf_console *c = &bus->console;
+	u8 line[CONSOLE_LINE_MAX], ch;
+	u32 n, idx, addr;
 	int rv;
-	struct sk_buff *pkt;
 
-	/* Address could be zero if CONSOLE := 0 in dongle Makefile */
+	/* Don't do anything until FWREADY updates console address */
 	if (bus->console_addr == 0)
-		return -ENOTSUPP;
+		return 0;
 
-	/* Exclusive bus access */
-	brcmf_sdbrcm_sdlock(bus);
+	/* Read console log struct */
+	addr = bus->console_addr + offsetof(struct rte_console, log_le);
+	rv = brcmf_sdbrcm_membytes(bus, false, addr, (u8 *)&c->log_le,
+				   sizeof(c->log_le));
+	if (rv < 0)
+		return rv;
 
-	/* Don't allow input if dongle is in reset */
-	if (bus->drvr->dongle_reset) {
-		brcmf_sdbrcm_sdunlock(bus);
-		return -EPERM;
+	/* Allocate console buffer (one time only) */
+	if (c->buf == NULL) {
+		c->bufsize = le32_to_cpu(c->log_le.buf_size);
+		c->buf = kmalloc(c->bufsize, GFP_ATOMIC);
+		if (c->buf == NULL)
+			return -ENOMEM;
 	}
 
-	/* Request clock to allow SDIO accesses */
-	BUS_WAKE(bus);
-	/* No pend allowed since txpkt is called later, ht clk has to be on */
-	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
+	idx = le32_to_cpu(c->log_le.idx);
 
-	/* Zero cbuf_index */
-	addr = bus->console_addr + offsetof(struct rte_console, cbuf_idx);
-	val = cpu_to_le32(0);
-	rv = brcmf_sdbrcm_membytes(bus, true, addr, (u8 *)&val, sizeof(val));
-	if (rv < 0)
-		goto done;
+	/* Protect against corrupt value */
+	if (idx > c->bufsize)
+		return -EBADE;
 
-	/* Write message into cbuf */
-	addr = bus->console_addr + offsetof(struct rte_console, cbuf);
-	rv = brcmf_sdbrcm_membytes(bus, true, addr, (u8 *)msg, msglen);
-	if (rv < 0)
-		goto done;
+	/* Skip reading the console buffer if the index pointer
+	 has not moved */
+	if (idx == c->last)
+		return 0;
 
-	/* Write length into vcons_in */
-	addr = bus->console_addr + offsetof(struct rte_console, vcons_in);
-	val = cpu_to_le32(msglen);
-	rv = brcmf_sdbrcm_membytes(bus, true, addr, (u8 *)&val, sizeof(val));
+	/* Read the console buffer */
+	addr = le32_to_cpu(c->log_le.buf);
+	rv = brcmf_sdbrcm_membytes(bus, false, addr, c->buf, c->bufsize);
 	if (rv < 0)
-		goto done;
+		return rv;
 
-	/* Bump dongle by sending an empty event pkt.
-	 * sdpcm_sendup (RX) checks for virtual console input.
-	 */
-	pkt = brcmu_pkt_buf_get_skb(4 + SDPCM_RESERVE);
-	if ((pkt != NULL) && bus->clkstate == CLK_AVAIL)
-		brcmf_sdbrcm_txpkt(bus, pkt, SDPCM_EVENT_CHANNEL, true);
+	while (c->last != idx) {
+		for (n = 0; n < CONSOLE_LINE_MAX - 2; n++) {
+			if (c->last == idx) {
+				/* This would output a partial line.
+				 * Instead, back up
+				 * the buffer pointer and output this
+				 * line next time around.
+				 */
+				if (c->last >= n)
+					c->last -= n;
+				else
+					c->last = c->bufsize - n;
+				goto break2;
+			}
+			ch = c->buf[c->last];
+			c->last = (c->last + 1) % c->bufsize;
+			if (ch == '\n')
+				break;
+			line[n] = ch;
+		}
 
-done:
-	if ((bus->idletime == BRCMF_IDLE_IMMEDIATE) && !bus->dpc_sched) {
-		bus->activity = false;
-		brcmf_sdbrcm_clkctl(bus, CLK_NONE, true);
+		if (n > 0) {
+			if (line[n - 1] == '\r')
+				n--;
+			line[n] = 0;
+			printk(KERN_DEBUG "CONSOLE: %s\n", line);
+		}
 	}
+break2:
 
-	brcmf_sdbrcm_sdunlock(bus);
-
-	return rv;
+	return 0;
 }
 #endif				/* BCMDBG */
 
-static bool brcmf_sdbrcm_chipmatch(u16 chipid)
-{
-	if (chipid == BCM4325_CHIP_ID)
-		return true;
-	if (chipid == BCM4329_CHIP_ID)
-		return true;
-	if (chipid == BCM4319_CHIP_ID)
-		return true;
-	return false;
-}
-
-static void *brcmf_sdbrcm_probe(u16 venid, u16 devid, u16 bus_no,
-			   u16 slot, u16 func, uint bustype, u32 regsva,
-			   void *card)
+static int brcmf_tx_frame(struct brcmf_bus *bus, u8 *frame, u16 len)
 {
+	int i;
 	int ret;
-	struct brcmf_bus *bus;
-
-	/* Init global variables at run-time, not as part of the declaration.
-	 * This is required to support init/de-init of the driver.
-	 * Initialization
-	 * of globals as part of the declaration results in non-deterministic
-	 * behavior since the value of the globals may be different on the
-	 * first time that the driver is initialized vs subsequent
-	 * initializations.
-	 */
-	brcmf_txbound = BRCMF_TXBOUND;
-	brcmf_rxbound = BRCMF_RXBOUND;
-	brcmf_alignctl = true;
-	sd1idle = true;
-	brcmf_readahead = true;
-	retrydata = false;
-	brcmf_dongle_memsize = 0;
-	brcmf_txminmax = BRCMF_TXMINMAX;
 
-	forcealign = true;
+	bus->ctrl_frame_stat = false;
+	ret = brcmf_sdbrcm_send_buf(bus, bus->sdiodev->sbwad,
+				    SDIO_FUNC_2, F2SYNC, frame, len, NULL);
 
-	brcmf_c_init();
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-	BRCMF_INFO(("%s: venid 0x%04x devid 0x%04x\n", __func__, venid, devid));
+	if (ret < 0) {
+		/* On failure, abort the command and terminate the frame */
+		brcmf_dbg(INFO, "sdio error %d, abort command and terminate frame\n",
+			  ret);
+		bus->tx_sderrs++;
 
-	/* We make an assumption about address window mappings:
-	 * regsva == SI_ENUM_BASE*/
+		brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
 
-	/* SDIO car passes venid and devid based on CIS parsing -- but
-	 * low-power start
-	 * means early parse could fail, so here we should get either an ID
-	 * we recognize OR (-1) indicating we must request power first.
-	 */
-	/* Check the Vendor ID */
-	switch (venid) {
-	case 0x0000:
-	case PCI_VENDOR_ID_BROADCOM:
-		break;
-	default:
-		BRCMF_ERROR(("%s: unknown vendor: 0x%04x\n", __func__, venid));
-		return NULL;
-	}
-
-	/* Check the Device ID and make sure it's one that we support */
-	switch (devid) {
-	case BCM4325_D11DUAL_ID:	/* 4325 802.11a/g id */
-	case BCM4325_D11G_ID:	/* 4325 802.11g 2.4Ghz band id */
-	case BCM4325_D11A_ID:	/* 4325 802.11a 5Ghz band id */
-		BRCMF_INFO(("%s: found 4325 Dongle\n", __func__));
-		break;
-	case BCM4329_D11NDUAL_ID:	/* 4329 802.11n dualband device */
-	case BCM4329_D11N2G_ID:	/* 4329 802.11n 2.4G device */
-	case BCM4329_D11N5G_ID:	/* 4329 802.11n 5G device */
-	case 0x4329:
-		BRCMF_INFO(("%s: found 4329 Dongle\n", __func__));
-		break;
-	case BCM4319_D11N_ID:	/* 4319 802.11n id */
-	case BCM4319_D11N2G_ID:	/* 4319 802.11n2g id */
-	case BCM4319_D11N5G_ID:	/* 4319 802.11n5g id */
-		BRCMF_INFO(("%s: found 4319 Dongle\n", __func__));
-		break;
-	case 0:
-		BRCMF_INFO(("%s: allow device id 0, will check chip"
-			    " internals\n", __func__));
-		break;
+		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
+				       SBSDIO_FUNC1_FRAMECTRL,
+				       SFC_WF_TERM, NULL);
+		bus->f1regdata++;
 
-	default:
-		BRCMF_ERROR(("%s: skipping 0x%04x/0x%04x, not a dongle\n",
-			     __func__, venid, devid));
-		return NULL;
+		for (i = 0; i < 3; i++) {
+			u8 hi, lo;
+			hi = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
+						   SBSDIO_FUNC1_WFRAMEBCHI,
+						   NULL);
+			lo = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
+						   SBSDIO_FUNC1_WFRAMEBCLO,
+						   NULL);
+			bus->f1regdata += 2;
+			if (hi == 0 && lo == 0)
+				break;
+		}
+		return ret;
 	}
 
-	/* Allocate private bus interface state */
-	bus = kzalloc(sizeof(struct brcmf_bus), GFP_ATOMIC);
-	if (!bus) {
-		BRCMF_ERROR(("%s: kmalloc of struct dhd_bus failed\n",
-			     __func__));
-		goto fail;
-	}
-	bus->card = card;
-	bus->cl_devid = (u16) devid;
-	bus->bus = BRCMF_BUS;
-	bus->tx_seq = SDPCM_SEQUENCE_WRAP - 1;
-	bus->usebufpool = false;	/* Use bufpool if allocated,
-					 else use locally malloced rxbuf */
+	bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQUENCE_WRAP;
 
-	/* attempt to attach to the dongle */
-	if (!(brcmf_sdbrcm_probe_attach(bus, card, regsva, devid))) {
-		BRCMF_ERROR(("%s: brcmf_sdbrcm_probe_attach failed\n",
-			     __func__));
-		goto fail;
-	}
-
-	spin_lock_init(&bus->txqlock);
-	init_waitqueue_head(&bus->ctrl_wait);
+	return ret;
+}
 
-	/* Set up the watchdog timer */
-	init_timer(&bus->timer);
-	bus->timer.data = (unsigned long)bus;
-	bus->timer.function = brcmf_sdbrcm_watchdog;
+int
+brcmf_sdbrcm_bus_txctl(struct brcmf_bus *bus, unsigned char *msg, uint msglen)
+{
+	u8 *frame;
+	u16 len;
+	u32 swheader;
+	uint retries = 0;
+	u8 doff = 0;
+	int ret = -1;
 
-	/* Initialize thread based operation and lock */
-	if ((brcmf_watchdog_prio >= 0) && (brcmf_dpc_prio >= 0)) {
-		bus->threads_only = true;
-		sema_init(&bus->sdsem, 1);
-	} else {
-		bus->threads_only = false;
-		spin_lock_init(&bus->sdlock);
-	}
+	brcmf_dbg(TRACE, "Enter\n");
 
-	if (brcmf_dpc_prio >= 0) {
-		/* Initialize watchdog thread */
-		init_completion(&bus->watchdog_wait);
-		bus->watchdog_tsk = kthread_run(brcmf_sdbrcm_watchdog_thread,
-						bus, "brcmf_watchdog");
-		if (IS_ERR(bus->watchdog_tsk)) {
-			printk(KERN_WARNING
-			       "brcmf_watchdog thread failed to start\n");
-			bus->watchdog_tsk = NULL;
-		}
-	} else
-		bus->watchdog_tsk = NULL;
+	/* Back the pointer to make a room for bus header */
+	frame = msg - SDPCM_HDRLEN;
+	len = (msglen += SDPCM_HDRLEN);
 
-	/* Set up the bottom half handler */
-	if (brcmf_dpc_prio >= 0) {
-		/* Initialize DPC thread */
-		init_completion(&bus->dpc_wait);
-		bus->dpc_tsk = kthread_run(brcmf_sdbrcm_dpc_thread,
-					   bus, "brcmf_dpc");
-		if (IS_ERR(bus->dpc_tsk)) {
-			printk(KERN_WARNING
-			       "brcmf_dpc thread failed to start\n");
-			bus->dpc_tsk = NULL;
-		}
-	} else {
-		tasklet_init(&bus->tasklet, brcmf_sdbrcm_dpc_tasklet,
-			     (unsigned long)bus);
-		bus->dpc_tsk = NULL;
-	}
+	/* Add alignment padding (optional for ctl frames) */
+	doff = ((unsigned long)frame % BRCMF_SDALIGN);
+	if (doff) {
+		frame -= doff;
+		len += doff;
+		msglen += doff;
+		memset(frame, 0, doff + SDPCM_HDRLEN);
+	}
+	/* precondition: doff < BRCMF_SDALIGN */
+	doff += SDPCM_HDRLEN;
 
-	/* Attach to the brcmf/OS/network interface */
-	bus->drvr = brcmf_attach(bus, SDPCM_RESERVE);
-	if (!bus->drvr) {
-		BRCMF_ERROR(("%s: brcmf_attach failed\n", __func__));
-		goto fail;
+	/* Round send length to next SDIO block */
+	if (bus->roundup && bus->blocksize && (len > bus->blocksize)) {
+		u16 pad = bus->blocksize - (len % bus->blocksize);
+		if ((pad <= bus->roundup) && (pad < bus->blocksize))
+			len += pad;
+	} else if (len % BRCMF_SDALIGN) {
+		len += BRCMF_SDALIGN - (len % BRCMF_SDALIGN);
 	}
 
-	/* Allocate buffers */
-	if (!(brcmf_sdbrcm_probe_malloc(bus, card))) {
-		BRCMF_ERROR(("%s: brcmf_sdbrcm_probe_malloc failed\n",
-			     __func__));
-		goto fail;
-	}
+	/* Satisfy length-alignment requirements */
+	if (len & (ALIGNMENT - 1))
+		len = roundup(len, ALIGNMENT);
 
-	if (!(brcmf_sdbrcm_probe_init(bus, card))) {
-		BRCMF_ERROR(("%s: brcmf_sdbrcm_probe_init failed\n", __func__));
-		goto fail;
-	}
+	/* precondition: IS_ALIGNED((unsigned long)frame, 2) */
 
-	/* Register interrupt callback, but mask it (not operational yet). */
-	BRCMF_INTR(("%s: disable SDIO interrupts (not interested yet)\n",
-		    __func__));
-	brcmf_sdcard_intr_disable(card);
-	ret = brcmf_sdcard_intr_reg(card, brcmf_sdbrcm_isr, bus);
-	if (ret != 0) {
-		BRCMF_ERROR(("%s: FAILED: sdcard_intr_reg returned %d\n",
-			     __func__, ret));
-		goto fail;
-	}
-	BRCMF_INTR(("%s: registered SDIO interrupt function ok\n", __func__));
+	/* Need to lock here to protect txseq and SDIO tx calls */
+	down(&bus->sdsem);
 
-	BRCMF_INFO(("%s: completed!!\n", __func__));
+	bus_wake(bus);
 
-	/* if firmware path present try to download and bring up bus */
-	ret = brcmf_bus_start(bus->drvr);
-	if (ret != 0) {
-		if (ret == -ENOLINK) {
-			BRCMF_ERROR(("%s: dongle is not responding\n",
-				     __func__));
-			goto fail;
-		}
-	}
-	/* Ok, have the per-port tell the stack we're open for business */
-	if (brcmf_net_attach(bus->drvr, 0) != 0) {
-		BRCMF_ERROR(("%s: Net attach failed!!\n", __func__));
-		goto fail;
-	}
+	/* Make sure backplane clock is on */
+	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
 
-	return bus;
+	/* Hardware tag: 2 byte len followed by 2 byte ~len check (all LE) */
+	*(__le16 *) frame = cpu_to_le16((u16) msglen);
+	*(((__le16 *) frame) + 1) = cpu_to_le16(~msglen);
 
-fail:
-	brcmf_sdbrcm_release(bus);
-	return NULL;
-}
+	/* Software tag: channel, sequence number, data offset */
+	swheader =
+	    ((SDPCM_CONTROL_CHANNEL << SDPCM_CHANNEL_SHIFT) &
+	     SDPCM_CHANNEL_MASK)
+	    | bus->tx_seq | ((doff << SDPCM_DOFFSET_SHIFT) &
+			     SDPCM_DOFFSET_MASK);
+	put_unaligned_le32(swheader, frame + SDPCM_FRAMETAG_LEN);
+	put_unaligned_le32(0, frame + SDPCM_FRAMETAG_LEN + sizeof(swheader));
 
-static bool
-brcmf_sdbrcm_probe_attach(struct brcmf_bus *bus, void *card, u32 regsva,
-			  u16 devid)
-{
-	u8 clkctl = 0;
-	int err = 0;
+	if (!data_ok(bus)) {
+		brcmf_dbg(INFO, "No bus credit bus->tx_max %d, bus->tx_seq %d\n",
+			  bus->tx_max, bus->tx_seq);
+		bus->ctrl_frame_stat = true;
+		/* Send from dpc */
+		bus->ctrl_frame_buf = frame;
+		bus->ctrl_frame_len = len;
 
-	bus->alp_only = true;
+		brcmf_sdbrcm_wait_for_event(bus, &bus->ctrl_frame_stat);
 
-	/* Return the window to backplane enumeration space for core access */
-	if (brcmf_sdbrcm_set_siaddr_window(bus, SI_ENUM_BASE))
-		BRCMF_ERROR(("%s: FAILED to return to SI_ENUM_BASE\n",
-			     __func__));
+		if (bus->ctrl_frame_stat == false) {
+			brcmf_dbg(INFO, "ctrl_frame_stat == false\n");
+			ret = 0;
+		} else {
+			brcmf_dbg(INFO, "ctrl_frame_stat == true\n");
+			ret = -1;
+		}
+	}
 
+	if (ret == -1) {
 #ifdef BCMDBG
-	printk(KERN_DEBUG "F1 signature read @0x18000000=0x%4x\n",
-	       brcmf_sdcard_reg_read(bus->card, SI_ENUM_BASE, 4));
-
-#endif				/* BCMDBG */
-
-	/*
-	 * Force PLL off until brcmf_sdbrcm_chip_attach()
-	 * programs PLL control regs
-	 */
-
-	brcmf_sdcard_cfg_write(card, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
-			 BRCMF_INIT_CLKCTL1, &err);
-	if (!err)
-		clkctl =
-		    brcmf_sdcard_cfg_read(card, SDIO_FUNC_1,
-					  SBSDIO_FUNC1_CHIPCLKCSR, &err);
+		if (BRCMF_BYTES_ON() && BRCMF_CTL_ON()) {
+			printk(KERN_DEBUG "Tx Frame:\n");
+			print_hex_dump_bytes("", DUMP_PREFIX_OFFSET,
+					     frame, len);
+		} else if (BRCMF_HDRS_ON()) {
+			printk(KERN_DEBUG "TxHdr:\n");
+			print_hex_dump_bytes("", DUMP_PREFIX_OFFSET,
+					     frame, min_t(u16, len, 16));
+		}
+#endif
 
-	if (err || ((clkctl & ~SBSDIO_AVBITS) != BRCMF_INIT_CLKCTL1)) {
-		BRCMF_ERROR(("brcmf_sdbrcm_probe: ChipClkCSR access: err %d"
-			     " wrote 0x%02x read 0x%02x\n",
-			     err, BRCMF_INIT_CLKCTL1, clkctl));
-		goto fail;
+		do {
+			ret = brcmf_tx_frame(bus, frame, len);
+		} while (ret < 0 && retries++ < TXRETRIES);
 	}
 
-	if (brcmf_sdbrcm_chip_attach(bus, regsva)) {
-		BRCMF_ERROR(("%s: brcmf_sdbrcm_chip_attach failed!\n",
-			     __func__));
-		goto fail;
+	if ((bus->idletime == BRCMF_IDLE_IMMEDIATE) && !bus->dpc_sched) {
+		bus->activity = false;
+		brcmf_sdbrcm_clkctl(bus, CLK_NONE, true);
 	}
 
-	if (!brcmf_sdbrcm_chipmatch((u16) bus->ci->chip)) {
-		BRCMF_ERROR(("%s: unsupported chip: 0x%04x\n",
-			     __func__, bus->ci->chip));
-		goto fail;
-	}
+	up(&bus->sdsem);
 
-	brcmf_sdbrcm_sdiod_drive_strength_init(bus, brcmf_sdiod_drive_strength);
+	if (ret)
+		bus->drvr->tx_ctlerrs++;
+	else
+		bus->drvr->tx_ctlpkts++;
 
-	/* Get info on the ARM and SOCRAM cores... */
-	if (!BRCMF_NOPMU(bus)) {
-		brcmf_sdcard_reg_read(bus->card,
-			  CORE_SB(bus->ci->armcorebase, sbidhigh), 4);
-		bus->orig_ramsize = bus->ci->ramsize;
-		if (!(bus->orig_ramsize)) {
-			BRCMF_ERROR(("%s: failed to find SOCRAM memory!\n",
-				     __func__));
-			goto fail;
-		}
-		bus->ramsize = bus->orig_ramsize;
-		if (brcmf_dongle_memsize)
-			brcmf_sdbrcm_setmemsize(bus, brcmf_dongle_memsize);
+	return ret ? -EIO : 0;
+}
 
-		BRCMF_ERROR(("DHD: dongle ram size is set to %d(orig %d)\n",
-			     bus->ramsize, bus->orig_ramsize));
-	}
+int
+brcmf_sdbrcm_bus_rxctl(struct brcmf_bus *bus, unsigned char *msg, uint msglen)
+{
+	int timeleft;
+	uint rxlen = 0;
+	bool pending;
 
-	/* Set core control so an SDIO reset does a backplane reset */
-	OR_REG(bus->ci->buscorebase + offsetof(struct sdpcmd_regs,
-						       corecontrol),
-	       CC_BPRESEN, u32);
+	brcmf_dbg(TRACE, "Enter\n");
 
-	brcmu_pktq_init(&bus->txq, (PRIOMASK + 1), TXQLEN);
+	/* Wait until control frame is available */
+	timeleft = brcmf_sdbrcm_dcmd_resp_wait(bus, &bus->rxlen, &pending);
 
-	/* Locate an appropriately-aligned portion of hdrbuf */
-	bus->rxhdr = (u8 *) roundup((unsigned long)&bus->hdrbuf[0],
-				    BRCMF_SDALIGN);
+	down(&bus->sdsem);
+	rxlen = bus->rxlen;
+	memcpy(msg, bus->rxctl, min(msglen, rxlen));
+	bus->rxlen = 0;
+	up(&bus->sdsem);
 
-	/* Set the poll and/or interrupt flags */
-	bus->intr = (bool) brcmf_intr;
-	bus->poll = (bool) brcmf_poll;
-	if (bus->poll)
-		bus->pollrate = 1;
+	if (rxlen) {
+		brcmf_dbg(CTL, "resumed on rxctl frame, got %d expected %d\n",
+			  rxlen, msglen);
+	} else if (timeleft == 0) {
+		brcmf_dbg(ERROR, "resumed on timeout\n");
+	} else if (pending == true) {
+		brcmf_dbg(CTL, "cancelled\n");
+		return -ERESTARTSYS;
+	} else {
+		brcmf_dbg(CTL, "resumed for unknown reason?\n");
+	}
 
-	return true;
+	if (rxlen)
+		bus->drvr->rx_ctlpkts++;
+	else
+		bus->drvr->rx_ctlerrs++;
 
-fail:
-	return false;
+	return rxlen ? (int)rxlen : -ETIMEDOUT;
 }
 
-static bool brcmf_sdbrcm_probe_malloc(struct brcmf_bus *bus, void *card)
+static int brcmf_sdbrcm_downloadvars(struct brcmf_bus *bus, void *arg, int len)
 {
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	int bcmerror = 0;
 
-	if (bus->drvr->maxctl) {
-		bus->rxblen =
-		    roundup((bus->drvr->maxctl + SDPCM_HDRLEN),
-			    ALIGNMENT) + BRCMF_SDALIGN;
-		bus->rxbuf = kmalloc(bus->rxblen, GFP_ATOMIC);
-		if (!(bus->rxbuf)) {
-			BRCMF_ERROR(("%s: kmalloc of %d-byte rxbuf failed\n",
-				     __func__, bus->rxblen));
-			goto fail;
-		}
-	}
+	brcmf_dbg(TRACE, "Enter\n");
 
-	/* Allocate buffer to receive glomed packet */
-	bus->databuf = kmalloc(MAX_DATA_BUF, GFP_ATOMIC);
-	if (!(bus->databuf)) {
-		BRCMF_ERROR(("%s: kmalloc of %d-byte databuf failed\n",
-			     __func__, MAX_DATA_BUF));
-		/* release rxbuf which was already located as above */
-		if (!bus->rxblen)
-			kfree(bus->rxbuf);
-		goto fail;
+	/* Basic sanity checks */
+	if (bus->drvr->up) {
+		bcmerror = -EISCONN;
+		goto err;
+	}
+	if (!len) {
+		bcmerror = -EOVERFLOW;
+		goto err;
 	}
 
-	/* Align the buffer */
-	if ((unsigned long)bus->databuf % BRCMF_SDALIGN)
-		bus->dataptr = bus->databuf + (BRCMF_SDALIGN -
-			       ((unsigned long)bus->databuf % BRCMF_SDALIGN));
-	else
-		bus->dataptr = bus->databuf;
+	/* Free the old ones and replace with passed variables */
+	kfree(bus->vars);
 
-	return true;
+	bus->vars = kmalloc(len, GFP_ATOMIC);
+	bus->varsz = bus->vars ? len : 0;
+	if (bus->vars == NULL) {
+		bcmerror = -ENOMEM;
+		goto err;
+	}
 
-fail:
-	return false;
+	/* Copy the passed variables, which should include the
+		 terminating double-null */
+	memcpy(bus->vars, arg, bus->varsz);
+err:
+	return bcmerror;
 }
 
-static bool brcmf_sdbrcm_probe_init(struct brcmf_bus *bus, void *card)
+static int brcmf_sdbrcm_write_vars(struct brcmf_bus *bus)
 {
-	s32 fnum;
+	int bcmerror = 0;
+	u32 varsize;
+	u32 varaddr;
+	u8 *vbuffer;
+	u32 varsizew;
+	__le32 varsizew_le;
+#ifdef BCMDBG
+	char *nvram_ularray;
+#endif				/* BCMDBG */
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	/* Even if there are no vars are to be written, we still
+		 need to set the ramsize. */
+	varsize = bus->varsz ? roundup(bus->varsz, 4) : 0;
+	varaddr = (bus->ramsize - 4) - varsize;
 
-#ifdef SDTEST
-	brcmf_sdbrcm_pktgen_init(bus);
-#endif				/* SDTEST */
+	if (bus->vars) {
+		vbuffer = kzalloc(varsize, GFP_ATOMIC);
+		if (!vbuffer)
+			return -ENOMEM;
 
-	/* Disable F2 to clear any intermediate frame state on the dongle */
-	brcmf_sdcard_cfg_write(card, SDIO_FUNC_0, SDIO_CCCR_IOEx,
-			       SDIO_FUNC_ENABLE_1, NULL);
+		memcpy(vbuffer, bus->vars, bus->varsz);
 
-	bus->drvr->busstate = BRCMF_BUS_DOWN;
-	bus->sleeping = false;
-	bus->rxflow = false;
+		/* Write the vars list */
+		bcmerror =
+		    brcmf_sdbrcm_membytes(bus, true, varaddr, vbuffer, varsize);
+#ifdef BCMDBG
+		/* Verify NVRAM bytes */
+		brcmf_dbg(INFO, "Compare NVRAM dl & ul; varsize=%d\n", varsize);
+		nvram_ularray = kmalloc(varsize, GFP_ATOMIC);
+		if (!nvram_ularray)
+			return -ENOMEM;
 
-	/* Done with backplane-dependent accesses, can drop clock... */
-	brcmf_sdcard_cfg_write(card, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR, 0,
-			       NULL);
+		/* Upload image to verify downloaded contents. */
+		memset(nvram_ularray, 0xaa, varsize);
 
-	/* ...and initialize clock/power states */
-	bus->clkstate = CLK_SDONLY;
-	bus->idletime = (s32) brcmf_idletime;
-	bus->idleclock = BRCMF_IDLE_ACTIVE;
+		/* Read the vars list to temp buffer for comparison */
+		bcmerror =
+		    brcmf_sdbrcm_membytes(bus, false, varaddr, nvram_ularray,
+				     varsize);
+		if (bcmerror) {
+			brcmf_dbg(ERROR, "error %d on reading %d nvram bytes at 0x%08x\n",
+				  bcmerror, varsize, varaddr);
+		}
+		/* Compare the org NVRAM with the one read from RAM */
+		if (memcmp(vbuffer, nvram_ularray, varsize))
+			brcmf_dbg(ERROR, "Downloaded NVRAM image is corrupted\n");
+		else
+			brcmf_dbg(ERROR, "Download/Upload/Compare of NVRAM ok\n");
 
-	/* Query the F2 block size, set roundup accordingly */
-	fnum = 2;
-	if (brcmf_sdcard_iovar_op(card, "sd_blocksize", &fnum, sizeof(s32),
-			    &bus->blocksize, sizeof(s32), false) != 0) {
-		bus->blocksize = 0;
-		BRCMF_ERROR(("%s: fail on %s get\n", __func__, "sd_blocksize"));
-	} else {
-		BRCMF_INFO(("%s: Initial value for %s is %d\n",
-			    __func__, "sd_blocksize", bus->blocksize));
+		kfree(nvram_ularray);
+#endif				/* BCMDBG */
+
+		kfree(vbuffer);
 	}
-	bus->roundup = min(max_roundup, bus->blocksize);
 
-	/* Query if bus module supports packet chaining,
-		 default to use if supported */
-	if (brcmf_sdcard_iovar_op(card, "sd_rxchain", NULL, 0,
-			    &bus->sd_rxchain, sizeof(s32),
-			    false) != 0) {
-		bus->sd_rxchain = false;
+	/* adjust to the user specified RAM */
+	brcmf_dbg(INFO, "Physical memory size: %d\n", bus->ramsize);
+	brcmf_dbg(INFO, "Vars are at %d, orig varsize is %d\n",
+		  varaddr, varsize);
+	varsize = ((bus->ramsize - 4) - varaddr);
+
+	/*
+	 * Determine the length token:
+	 * Varsize, converted to words, in lower 16-bits, checksum
+	 * in upper 16-bits.
+	 */
+	if (bcmerror) {
+		varsizew = 0;
+		varsizew_le = cpu_to_le32(0);
 	} else {
-		BRCMF_INFO(("%s: bus module (through sdiocard API) %s"
-			    " chaining\n", __func__, bus->sd_rxchain
-			    ? "supports" : "does not support"));
+		varsizew = varsize / 4;
+		varsizew = (~varsizew << 16) | (varsizew & 0x0000FFFF);
+		varsizew_le = cpu_to_le32(varsizew);
 	}
-	bus->use_rxchain = (bool) bus->sd_rxchain;
 
-	return true;
-}
+	brcmf_dbg(INFO, "New varsize is %d, length token=0x%08x\n",
+		  varsize, varsizew);
 
-static bool
-brcmf_sdbrcm_download_firmware(struct brcmf_bus *bus, void *card)
-{
-	bool ret;
+	/* Write the length token to the last word */
+	bcmerror = brcmf_sdbrcm_membytes(bus, true, (bus->ramsize - 4),
+					 (u8 *)&varsizew_le, 4);
 
-	/* Download the firmware */
-	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
+	return bcmerror;
+}
 
-	ret = _brcmf_sdbrcm_download_firmware(bus) == 0;
+static void
+brcmf_sdbrcm_chip_disablecore(struct brcmf_sdio_dev *sdiodev, u32 corebase)
+{
+	u32 regdata;
 
-	brcmf_sdbrcm_clkctl(bus, CLK_SDONLY, false);
+	regdata = brcmf_sdcard_reg_read(sdiodev,
+		CORE_SB(corebase, sbtmstatelow), 4);
+	if (regdata & SBTML_RESET)
+		return;
 
-	return ret;
-}
+	regdata = brcmf_sdcard_reg_read(sdiodev,
+		CORE_SB(corebase, sbtmstatelow), 4);
+	if ((regdata & (SICF_CLOCK_EN << SBTML_SICF_SHIFT)) != 0) {
+		/*
+		 * set target reject and spin until busy is clear
+		 * (preserve core-specific bits)
+		 */
+		regdata = brcmf_sdcard_reg_read(sdiodev,
+			CORE_SB(corebase, sbtmstatelow), 4);
+		brcmf_sdcard_reg_write(sdiodev, CORE_SB(corebase, sbtmstatelow),
+				       4, regdata | SBTML_REJ);
 
-/* Detach and free everything */
-static void brcmf_sdbrcm_release(struct brcmf_bus *bus)
-{
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+		regdata = brcmf_sdcard_reg_read(sdiodev,
+			CORE_SB(corebase, sbtmstatelow), 4);
+		udelay(1);
+		SPINWAIT((brcmf_sdcard_reg_read(sdiodev,
+			CORE_SB(corebase, sbtmstatehigh), 4) &
+			SBTMH_BUSY), 100000);
 
-	if (bus) {
-		/* De-register interrupt handler */
-		brcmf_sdcard_intr_disable(bus->card);
-		brcmf_sdcard_intr_dereg(bus->card);
+		regdata = brcmf_sdcard_reg_read(sdiodev,
+			CORE_SB(corebase, sbtmstatehigh), 4);
+		if (regdata & SBTMH_BUSY)
+			brcmf_dbg(ERROR, "ARM core still busy\n");
 
-		if (bus->drvr) {
-			brcmf_detach(bus->drvr);
-			brcmf_sdbrcm_release_dongle(bus);
-			bus->drvr = NULL;
+		regdata = brcmf_sdcard_reg_read(sdiodev,
+			CORE_SB(corebase, sbidlow), 4);
+		if (regdata & SBIDL_INIT) {
+			regdata = brcmf_sdcard_reg_read(sdiodev,
+				CORE_SB(corebase, sbimstate), 4) |
+				SBIM_RJ;
+			brcmf_sdcard_reg_write(sdiodev,
+				CORE_SB(corebase, sbimstate), 4,
+				regdata);
+			regdata = brcmf_sdcard_reg_read(sdiodev,
+				CORE_SB(corebase, sbimstate), 4);
+			udelay(1);
+			SPINWAIT((brcmf_sdcard_reg_read(sdiodev,
+				CORE_SB(corebase, sbimstate), 4) &
+				SBIM_BY), 100000);
 		}
 
-		brcmf_sdbrcm_release_malloc(bus);
+		/* set reset and reject while enabling the clocks */
+		brcmf_sdcard_reg_write(sdiodev,
+			CORE_SB(corebase, sbtmstatelow), 4,
+			(((SICF_FGC | SICF_CLOCK_EN) << SBTML_SICF_SHIFT) |
+			SBTML_REJ | SBTML_RESET));
+		regdata = brcmf_sdcard_reg_read(sdiodev,
+			CORE_SB(corebase, sbtmstatelow), 4);
+		udelay(10);
 
-		kfree(bus);
+		/* clear the initiator reject bit */
+		regdata = brcmf_sdcard_reg_read(sdiodev,
+			CORE_SB(corebase, sbidlow), 4);
+		if (regdata & SBIDL_INIT) {
+			regdata = brcmf_sdcard_reg_read(sdiodev,
+				CORE_SB(corebase, sbimstate), 4) &
+				~SBIM_RJ;
+			brcmf_sdcard_reg_write(sdiodev,
+				CORE_SB(corebase, sbimstate), 4,
+				regdata);
+		}
 	}
 
-	BRCMF_TRACE(("%s: Disconnected\n", __func__));
+	/* leave reset and reject asserted */
+	brcmf_sdcard_reg_write(sdiodev, CORE_SB(corebase, sbtmstatelow), 4,
+		(SBTML_REJ | SBTML_RESET));
+	udelay(1);
 }
 
-static void brcmf_sdbrcm_release_malloc(struct brcmf_bus *bus)
+static void
+brcmf_sdbrcm_chip_resetcore(struct brcmf_sdio_dev *sdiodev, u32 corebase)
 {
-	BRCMF_TRACE(("%s: Enter\n", __func__));
-
-	if (bus->drvr && bus->drvr->dongle_reset)
-		return;
+	u32 regdata;
 
-	kfree(bus->rxbuf);
-	bus->rxctl = bus->rxbuf = NULL;
-	bus->rxlen = 0;
+	/*
+	 * Must do the disable sequence first to work for
+	 * arbitrary current core state.
+	 */
+	brcmf_sdbrcm_chip_disablecore(sdiodev, corebase);
 
-	kfree(bus->databuf);
-	bus->databuf = NULL;
-}
+	/*
+	 * Now do the initialization sequence.
+	 * set reset while enabling the clock and
+	 * forcing them on throughout the core
+	 */
+	brcmf_sdcard_reg_write(sdiodev, CORE_SB(corebase, sbtmstatelow), 4,
+		((SICF_FGC | SICF_CLOCK_EN) << SBTML_SICF_SHIFT) |
+		SBTML_RESET);
+	udelay(1);
 
-static void brcmf_sdbrcm_release_dongle(struct brcmf_bus *bus)
-{
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	regdata = brcmf_sdcard_reg_read(sdiodev,
+					CORE_SB(corebase, sbtmstatehigh), 4);
+	if (regdata & SBTMH_SERR)
+		brcmf_sdcard_reg_write(sdiodev,
+				       CORE_SB(corebase, sbtmstatehigh), 4, 0);
 
-	if (bus->drvr && bus->drvr->dongle_reset)
-		return;
+	regdata = brcmf_sdcard_reg_read(sdiodev,
+					CORE_SB(corebase, sbimstate), 4);
+	if (regdata & (SBIM_IBE | SBIM_TO))
+		brcmf_sdcard_reg_write(sdiodev, CORE_SB(corebase, sbimstate), 4,
+			regdata & ~(SBIM_IBE | SBIM_TO));
 
-	if (bus->ci) {
-		brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
-		brcmf_sdbrcm_clkctl(bus, CLK_NONE, false);
-		brcmf_sdbrcm_chip_detach(bus);
-		if (bus->vars && bus->varsz)
-			kfree(bus->vars);
-		bus->vars = NULL;
-	}
+	/* clear reset and allow it to propagate throughout the core */
+	brcmf_sdcard_reg_write(sdiodev, CORE_SB(corebase, sbtmstatelow), 4,
+		(SICF_FGC << SBTML_SICF_SHIFT) |
+		(SICF_CLOCK_EN << SBTML_SICF_SHIFT));
+	udelay(1);
 
-	BRCMF_TRACE(("%s: Disconnected\n", __func__));
+	/* leave clock enabled */
+	brcmf_sdcard_reg_write(sdiodev, CORE_SB(corebase, sbtmstatelow), 4,
+		(SICF_CLOCK_EN << SBTML_SICF_SHIFT));
+	udelay(1);
 }
 
-static void brcmf_sdbrcm_disconnect(void *ptr)
+static int brcmf_sdbrcm_download_state(struct brcmf_bus *bus, bool enter)
 {
-	struct brcmf_bus *bus = (struct brcmf_bus *)ptr;
-
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	uint retries;
+	u32 regdata;
+	int bcmerror = 0;
 
-	if (bus) {
-		brcmf_sdbrcm_release(bus);
-	}
+	/* To enter download state, disable ARM and reset SOCRAM.
+	 * To exit download state, simply reset ARM (default is RAM boot).
+	 */
+	if (enter) {
+		bus->alp_only = true;
 
-	BRCMF_TRACE(("%s: Disconnected\n", __func__));
-}
+		brcmf_sdbrcm_chip_disablecore(bus->sdiodev,
+					      bus->ci->armcorebase);
 
-/* Register/Unregister functions are called by the main DHD entry
- * point (e.g. module insertion) to link with the bus driver, in
- * order to look for or await the device.
- */
+		brcmf_sdbrcm_chip_resetcore(bus->sdiodev, bus->ci->ramcorebase);
 
-static struct brcmf_sdioh_driver brcmf_sdio = {
-	brcmf_sdbrcm_probe,
-	brcmf_sdbrcm_disconnect
-};
+		/* Clear the top bit of memory */
+		if (bus->ramsize) {
+			u32 zeros = 0;
+			brcmf_sdbrcm_membytes(bus, true, bus->ramsize - 4,
+					 (u8 *)&zeros, 4);
+		}
+	} else {
+		regdata = brcmf_sdcard_reg_read(bus->sdiodev,
+			CORE_SB(bus->ci->ramcorebase, sbtmstatelow), 4);
+		regdata &= (SBTML_RESET | SBTML_REJ_MASK |
+			(SICF_CLOCK_EN << SBTML_SICF_SHIFT));
+		if ((SICF_CLOCK_EN << SBTML_SICF_SHIFT) != regdata) {
+			brcmf_dbg(ERROR, "SOCRAM core is down after reset?\n");
+			bcmerror = -EBADE;
+			goto fail;
+		}
 
-int brcmf_bus_register(void)
-{
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+		bcmerror = brcmf_sdbrcm_write_vars(bus);
+		if (bcmerror) {
+			brcmf_dbg(ERROR, "no vars written to RAM\n");
+			bcmerror = 0;
+		}
 
-	/* Sanity check on the module parameters */
-	do {
-		/* Both watchdog and DPC as tasklets are ok */
-		if ((brcmf_watchdog_prio < 0) && (brcmf_dpc_prio < 0))
-			break;
+		w_sdreg32(bus, 0xFFFFFFFF,
+			  offsetof(struct sdpcmd_regs, intstatus), &retries);
 
-		/* If both watchdog and DPC are threads, TX must be deferred */
-		if ((brcmf_watchdog_prio >= 0) && (brcmf_dpc_prio >= 0)
-		    && brcmf_deferred_tx)
-			break;
+		brcmf_sdbrcm_chip_resetcore(bus->sdiodev, bus->ci->armcorebase);
 
-		BRCMF_ERROR(("Invalid module parameters.\n"));
-		return -EINVAL;
-	} while (0);
+		/* Allow HT Clock now that the ARM is running. */
+		bus->alp_only = false;
 
-	return brcmf_sdio_register(&brcmf_sdio);
+		bus->drvr->busstate = BRCMF_BUS_LOAD;
+	}
+fail:
+	return bcmerror;
 }
 
-void brcmf_bus_unregister(void)
+static int brcmf_sdbrcm_get_image(char *buf, int len, struct brcmf_bus *bus)
 {
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	if (bus->firmware->size < bus->fw_ptr + len)
+		len = bus->firmware->size - bus->fw_ptr;
 
-	brcmf_sdio_unregister();
+	memcpy(buf, &bus->firmware->data[bus->fw_ptr], len);
+	bus->fw_ptr += len;
+	return len;
 }
 
+MODULE_FIRMWARE(BCM4329_FW_NAME);
+MODULE_FIRMWARE(BCM4329_NV_NAME);
+
 static int brcmf_sdbrcm_download_code_file(struct brcmf_bus *bus)
 {
 	int offset = 0;
@@ -5916,22 +3408,19 @@ static int brcmf_sdbrcm_download_code_file(struct brcmf_bus *bus)
 	u8 *memblock = NULL, *memptr;
 	int ret;
 
-	BRCMF_INFO(("%s: Enter\n", __func__));
+	brcmf_dbg(INFO, "Enter\n");
 
 	bus->fw_name = BCM4329_FW_NAME;
 	ret = request_firmware(&bus->firmware, bus->fw_name,
-			       &gInstance->func[2]->dev);
+			       &bus->sdiodev->func[2]->dev);
 	if (ret) {
-		BRCMF_ERROR(("%s: Fail to request firmware %d\n",
-			     __func__, ret));
+		brcmf_dbg(ERROR, "Fail to request firmware %d\n", ret);
 		return ret;
 	}
 	bus->fw_ptr = 0;
 
 	memptr = memblock = kmalloc(MEMBLOCK + BRCMF_SDALIGN, GFP_ATOMIC);
 	if (memblock == NULL) {
-		BRCMF_ERROR(("%s: Failed to allocate memory %d bytes\n",
-			     __func__, MEMBLOCK));
 		ret = -ENOMEM;
 		goto err;
 	}
@@ -5944,9 +3433,8 @@ static int brcmf_sdbrcm_download_code_file(struct brcmf_bus *bus)
 		brcmf_sdbrcm_get_image((char *)memptr, MEMBLOCK, bus))) {
 		ret = brcmf_sdbrcm_membytes(bus, true, offset, memptr, len);
 		if (ret) {
-			BRCMF_ERROR(("%s: error %d on writing %d membytes at "
-				     "0x%08x\n", __func__, ret, MEMBLOCK,
-				     offset));
+			brcmf_dbg(ERROR, "error %d on writing %d membytes at 0x%08x\n",
+				  ret, MEMBLOCK, offset);
 			goto err;
 		}
 
@@ -6022,17 +3510,15 @@ static int brcmf_sdbrcm_download_nvram(struct brcmf_bus *bus)
 
 	bus->nv_name = BCM4329_NV_NAME;
 	ret = request_firmware(&bus->firmware, bus->nv_name,
-			       &gInstance->func[2]->dev);
+			       &bus->sdiodev->func[2]->dev);
 	if (ret) {
-		BRCMF_ERROR(("%s: Fail to request nvram %d\n", __func__, ret));
+		brcmf_dbg(ERROR, "Fail to request nvram %d\n", ret);
 		return ret;
 	}
 	bus->fw_ptr = 0;
 
 	memblock = kmalloc(MEMBLOCK, GFP_ATOMIC);
 	if (memblock == NULL) {
-		BRCMF_ERROR(("%s: Failed to allocate memory %d bytes\n",
-			     __func__, MEMBLOCK));
 		ret = -ENOMEM;
 		goto err;
 	}
@@ -6048,11 +3534,9 @@ static int brcmf_sdbrcm_download_nvram(struct brcmf_bus *bus)
 		if (len)
 			ret = brcmf_sdbrcm_downloadvars(bus, memblock, len + 1);
 		if (ret)
-			BRCMF_ERROR(("%s: error downloading vars: %d\n",
-				     __func__, ret));
+			brcmf_dbg(ERROR, "error downloading vars: %d\n", ret);
 	} else {
-		BRCMF_ERROR(("%s: error reading nvram file: %d\n",
-			     __func__, len));
+		brcmf_dbg(ERROR, "error reading nvram file: %d\n", len);
 		ret = -EIO;
 	}
 
@@ -6071,28 +3555,23 @@ static int _brcmf_sdbrcm_download_firmware(struct brcmf_bus *bus)
 
 	/* Keep arm in reset */
 	if (brcmf_sdbrcm_download_state(bus, true)) {
-		BRCMF_ERROR(("%s: error placing ARM core in reset\n",
-			     __func__));
+		brcmf_dbg(ERROR, "error placing ARM core in reset\n");
 		goto err;
 	}
 
 	/* External image takes precedence if specified */
 	if (brcmf_sdbrcm_download_code_file(bus)) {
-		BRCMF_ERROR(("%s: dongle image file download failed\n",
-			     __func__));
+		brcmf_dbg(ERROR, "dongle image file download failed\n");
 		goto err;
 	}
 
 	/* External nvram takes precedence if specified */
-	if (brcmf_sdbrcm_download_nvram(bus)) {
-		BRCMF_ERROR(("%s: dongle nvram file download failed\n",
-			     __func__));
-	}
+	if (brcmf_sdbrcm_download_nvram(bus))
+		brcmf_dbg(ERROR, "dongle nvram file download failed\n");
 
 	/* Take arm out of reset */
 	if (brcmf_sdbrcm_download_state(bus, false)) {
-		BRCMF_ERROR(("%s: error getting out of ARM core reset\n",
-			     __func__));
+		brcmf_dbg(ERROR, "error getting out of ARM core reset\n");
 		goto err;
 	}
 
@@ -6102,359 +3581,379 @@ err:
 	return bcmerror;
 }
 
-
-static int
-brcmf_sdbrcm_send_buf(struct brcmf_bus *bus, u32 addr, uint fn, uint flags,
-		    u8 *buf, uint nbytes, struct sk_buff *pkt,
-		    void (*complete)(void *handle, int status,
-				     bool sync_waiting),
-		    void *handle)
+static bool
+brcmf_sdbrcm_download_firmware(struct brcmf_bus *bus)
 {
-	return brcmf_sdcard_send_buf
-		(bus->card, addr, fn, flags, buf, nbytes, pkt, complete,
-		 handle);
+	bool ret;
+
+	/* Download the firmware */
+	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
+
+	ret = _brcmf_sdbrcm_download_firmware(bus) == 0;
+
+	brcmf_sdbrcm_clkctl(bus, CLK_SDONLY, false);
+
+	return ret;
 }
 
-int brcmf_bus_devreset(struct brcmf_pub *drvr, u8 flag)
+void brcmf_sdbrcm_bus_stop(struct brcmf_bus *bus)
 {
-	int bcmerror = 0;
-	struct brcmf_bus *bus;
-
-	bus = drvr->bus;
+	u32 local_hostintmask;
+	u8 saveclk;
+	uint retries;
+	int err;
 
-	if (flag == true) {
-		brcmf_sdbrcm_wd_timer(bus, 0);
-		if (!bus->drvr->dongle_reset) {
-			/* Expect app to have torn down any
-			 connection before calling */
-			/* Stop the bus, disable F2 */
-			brcmf_sdbrcm_bus_stop(bus, false);
-
-			/* Clean tx/rx buffer pointers,
-			 detach from the dongle */
-			brcmf_sdbrcm_release_dongle(bus);
+	brcmf_dbg(TRACE, "Enter\n");
 
-			bus->drvr->dongle_reset = true;
-			bus->drvr->up = false;
+	if (bus->watchdog_tsk) {
+		send_sig(SIGTERM, bus->watchdog_tsk, 1);
+		kthread_stop(bus->watchdog_tsk);
+		bus->watchdog_tsk = NULL;
+	}
 
-			BRCMF_TRACE(("%s:  WLAN OFF DONE\n", __func__));
-			/* App can now remove power from device */
-		} else
-			bcmerror = -EIO;
-	} else {
-		/* App must have restored power to device before calling */
-
-		BRCMF_TRACE(("\n\n%s: == WLAN ON ==\n", __func__));
-
-		if (bus->drvr->dongle_reset) {
-			/* Turn on WLAN */
-
-			/* Attempt to re-attach & download */
-			if (brcmf_sdbrcm_probe_attach(bus, bus->card,
-						      SI_ENUM_BASE,
-						      bus->cl_devid)) {
-				/* Attempt to download binary to the dongle */
-				if (brcmf_sdbrcm_probe_init(bus, bus->card)) {
-					/* Re-init bus, enable F2 transfer */
-					brcmf_sdbrcm_bus_init(bus->drvr, false);
-
-					bus->drvr->dongle_reset = false;
-					bus->drvr->up = true;
-
-					BRCMF_TRACE(("%s: WLAN ON DONE\n",
-						     __func__));
-				} else
-					bcmerror = -EIO;
-			} else
-				bcmerror = -EIO;
-		} else {
-			bcmerror = -EISCONN;
-			BRCMF_ERROR(("%s: Set DEVRESET=false invoked when"
-				     " device is on\n", __func__));
-			bcmerror = -EIO;
-		}
-		brcmf_sdbrcm_wd_timer(bus, brcmf_watchdog_ms);
+	if (bus->dpc_tsk && bus->dpc_tsk != current) {
+		send_sig(SIGTERM, bus->dpc_tsk, 1);
+		kthread_stop(bus->dpc_tsk);
+		bus->dpc_tsk = NULL;
 	}
-	return bcmerror;
-}
 
-static int
-brcmf_sdbrcm_chip_recognition(struct brcmf_sdio_card *card,
-			      struct chip_info *ci, u32 regs)
-{
-	u32 regdata;
+	down(&bus->sdsem);
 
-	/*
-	 * Get CC core rev
-	 * Chipid is assume to be at offset 0 from regs arg
-	 * For different chiptypes or old sdio hosts w/o chipcommon,
-	 * other ways of recognition should be added here.
-	 */
-	ci->cccorebase = regs;
-	regdata = brcmf_sdcard_reg_read(card,
-				CORE_CC_REG(ci->cccorebase, chipid), 4);
-	ci->chip = regdata & CID_ID_MASK;
-	ci->chiprev = (regdata & CID_REV_MASK) >> CID_REV_SHIFT;
+	bus_wake(bus);
 
-	BRCMF_INFO(("%s: chipid=0x%x chiprev=%d\n",
-		    __func__, ci->chip, ci->chiprev));
+	/* Enable clock for device interrupts */
+	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
 
-	/* Address of cores for new chips should be added here */
-	switch (ci->chip) {
-	case BCM4329_CHIP_ID:
-		ci->buscorebase = BCM4329_CORE_BUS_BASE;
-		ci->ramcorebase = BCM4329_CORE_SOCRAM_BASE;
-		ci->armcorebase	= BCM4329_CORE_ARM_BASE;
-		ci->ramsize = BCM4329_RAMSIZE;
-		break;
-	default:
-		BRCMF_ERROR(("%s: chipid 0x%x is not supported\n",
-			     __func__, ci->chip));
-		return -ENODEV;
+	/* Disable and clear interrupts at the chip level also */
+	w_sdreg32(bus, 0, offsetof(struct sdpcmd_regs, hostintmask), &retries);
+	local_hostintmask = bus->hostintmask;
+	bus->hostintmask = 0;
+
+	/* Change our idea of bus state */
+	bus->drvr->busstate = BRCMF_BUS_DOWN;
+
+	/* Force clocks on backplane to be sure F2 interrupt propagates */
+	saveclk = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
+					SBSDIO_FUNC1_CHIPCLKCSR, &err);
+	if (!err) {
+		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
+				       SBSDIO_FUNC1_CHIPCLKCSR,
+				       (saveclk | SBSDIO_FORCE_HT), &err);
 	}
+	if (err)
+		brcmf_dbg(ERROR, "Failed to force clock for F2: err %d\n", err);
 
-	regdata = brcmf_sdcard_reg_read(card,
-		CORE_SB(ci->cccorebase, sbidhigh), 4);
-	ci->ccrev = SBCOREREV(regdata);
+	/* Turn off the bus (F2), free any pending packets */
+	brcmf_dbg(INTR, "disable SDIO interrupts\n");
+	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_0, SDIO_CCCR_IOEx,
+			 SDIO_FUNC_ENABLE_1, NULL);
 
-	regdata = brcmf_sdcard_reg_read(card,
-		CORE_CC_REG(ci->cccorebase, pmucapabilities), 4);
-	ci->pmurev = regdata & PCAP_REV_MASK;
+	/* Clear any pending interrupts now that F2 is disabled */
+	w_sdreg32(bus, local_hostintmask,
+		  offsetof(struct sdpcmd_regs, intstatus), &retries);
 
-	regdata = brcmf_sdcard_reg_read(card,
-					CORE_SB(ci->buscorebase, sbidhigh), 4);
-	ci->buscorerev = SBCOREREV(regdata);
-	ci->buscoretype = (regdata & SBIDH_CC_MASK) >> SBIDH_CC_SHIFT;
+	/* Turn off the backplane clock (only) */
+	brcmf_sdbrcm_clkctl(bus, CLK_SDONLY, false);
 
-	BRCMF_INFO(("%s: ccrev=%d, pmurev=%d, buscore rev/type=%d/0x%x\n",
-		    __func__, ci->ccrev, ci->pmurev,
-		    ci->buscorerev, ci->buscoretype));
+	/* Clear the data packet queues */
+	brcmu_pktq_flush(&bus->txq, true, NULL, NULL);
 
-	/* get chipcommon capabilites */
-	ci->cccaps = brcmf_sdcard_reg_read(card,
-		CORE_CC_REG(ci->cccorebase, capabilities), 4);
+	/* Clear any held glomming stuff */
+	if (bus->glomd)
+		brcmu_pkt_buf_free_skb(bus->glomd);
 
-	return 0;
+	if (bus->glom)
+		brcmu_pkt_buf_free_skb(bus->glom);
+
+	bus->glom = bus->glomd = NULL;
+
+	/* Clear rx control and wake any waiters */
+	bus->rxlen = 0;
+	brcmf_sdbrcm_dcmd_resp_wake(bus);
+
+	/* Reset some F2 state stuff */
+	bus->rxskip = false;
+	bus->tx_seq = bus->rx_seq = 0;
+
+	up(&bus->sdsem);
 }
 
-static void
-brcmf_sdbrcm_chip_disablecore(struct brcmf_sdio_card *card, u32 corebase)
+int brcmf_sdbrcm_bus_init(struct brcmf_pub *drvr)
 {
-	u32 regdata;
+	struct brcmf_bus *bus = drvr->bus;
+	unsigned long timeout;
+	uint retries = 0;
+	u8 ready, enable;
+	int err, ret = 0;
+	u8 saveclk;
 
-	regdata = brcmf_sdcard_reg_read(card,
-		CORE_SB(corebase, sbtmstatelow), 4);
-	if (regdata & SBTML_RESET)
-		return;
+	brcmf_dbg(TRACE, "Enter\n");
 
-	regdata = brcmf_sdcard_reg_read(card,
-		CORE_SB(corebase, sbtmstatelow), 4);
-	if ((regdata & (SICF_CLOCK_EN << SBTML_SICF_SHIFT)) != 0) {
-		/*
-		 * set target reject and spin until busy is clear
-		 * (preserve core-specific bits)
-		 */
-		regdata = brcmf_sdcard_reg_read(card,
-			CORE_SB(corebase, sbtmstatelow), 4);
-		brcmf_sdcard_reg_write(card, CORE_SB(corebase, sbtmstatelow), 4,
-			regdata | SBTML_REJ);
+	/* try to download image and nvram to the dongle */
+	if (drvr->busstate == BRCMF_BUS_DOWN) {
+		if (!(brcmf_sdbrcm_download_firmware(bus)))
+			return -1;
+	}
 
-		regdata = brcmf_sdcard_reg_read(card,
-			CORE_SB(corebase, sbtmstatelow), 4);
-		udelay(1);
-		SPINWAIT((brcmf_sdcard_reg_read(card,
-			CORE_SB(corebase, sbtmstatehigh), 4) &
-			SBTMH_BUSY), 100000);
+	if (!bus->drvr)
+		return 0;
 
-		regdata = brcmf_sdcard_reg_read(card,
-			CORE_SB(corebase, sbtmstatehigh), 4);
-		if (regdata & SBTMH_BUSY)
-			BRCMF_ERROR(("%s: ARM core still busy\n", __func__));
+	/* Start the watchdog timer */
+	bus->drvr->tickcnt = 0;
+	brcmf_sdbrcm_wd_timer(bus, BRCMF_WD_POLL_MS);
 
-		regdata = brcmf_sdcard_reg_read(card,
-			CORE_SB(corebase, sbidlow), 4);
-		if (regdata & SBIDL_INIT) {
-			regdata = brcmf_sdcard_reg_read(card,
-				CORE_SB(corebase, sbimstate), 4) |
-				SBIM_RJ;
-			brcmf_sdcard_reg_write(card,
-				CORE_SB(corebase, sbimstate), 4,
-				regdata);
-			regdata = brcmf_sdcard_reg_read(card,
-				CORE_SB(corebase, sbimstate), 4);
-			udelay(1);
-			SPINWAIT((brcmf_sdcard_reg_read(card,
-				CORE_SB(corebase, sbimstate), 4) &
-				SBIM_BY), 100000);
-		}
+	down(&bus->sdsem);
 
-		/* set reset and reject while enabling the clocks */
-		brcmf_sdcard_reg_write(card,
-			CORE_SB(corebase, sbtmstatelow), 4,
-			(((SICF_FGC | SICF_CLOCK_EN) << SBTML_SICF_SHIFT) |
-			SBTML_REJ | SBTML_RESET));
-		regdata = brcmf_sdcard_reg_read(card,
-			CORE_SB(corebase, sbtmstatelow), 4);
-		udelay(10);
+	/* Make sure backplane clock is on, needed to generate F2 interrupt */
+	brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
+	if (bus->clkstate != CLK_AVAIL)
+		goto exit;
 
-		/* clear the initiator reject bit */
-		regdata = brcmf_sdcard_reg_read(card,
-			CORE_SB(corebase, sbidlow), 4);
-		if (regdata & SBIDL_INIT) {
-			regdata = brcmf_sdcard_reg_read(card,
-				CORE_SB(corebase, sbimstate), 4) &
-				~SBIM_RJ;
-			brcmf_sdcard_reg_write(card,
-				CORE_SB(corebase, sbimstate), 4,
-				regdata);
-		}
+	/* Force clocks on backplane to be sure F2 interrupt propagates */
+	saveclk =
+	    brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
+				  SBSDIO_FUNC1_CHIPCLKCSR, &err);
+	if (!err) {
+		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
+				       SBSDIO_FUNC1_CHIPCLKCSR,
+				       (saveclk | SBSDIO_FORCE_HT), &err);
+	}
+	if (err) {
+		brcmf_dbg(ERROR, "Failed to force clock for F2: err %d\n", err);
+		goto exit;
 	}
 
-	/* leave reset and reject asserted */
-	brcmf_sdcard_reg_write(card, CORE_SB(corebase, sbtmstatelow), 4,
-		(SBTML_REJ | SBTML_RESET));
-	udelay(1);
+	/* Enable function 2 (frame transfers) */
+	w_sdreg32(bus, SDPCM_PROT_VERSION << SMB_DATA_VERSION_SHIFT,
+		  offsetof(struct sdpcmd_regs, tosbmailboxdata), &retries);
+	enable = (SDIO_FUNC_ENABLE_1 | SDIO_FUNC_ENABLE_2);
+
+	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_0, SDIO_CCCR_IOEx,
+			       enable, NULL);
+
+	timeout = jiffies + msecs_to_jiffies(BRCMF_WAIT_F2RDY);
+	ready = 0;
+	while (enable != ready) {
+		ready = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_0,
+					      SDIO_CCCR_IORx, NULL);
+		if (time_after(jiffies, timeout))
+			break;
+		else if (time_after(jiffies, timeout - BRCMF_WAIT_F2RDY + 50))
+			/* prevent busy waiting if it takes too long */
+			msleep_interruptible(20);
+	}
+
+	brcmf_dbg(INFO, "enable 0x%02x, ready 0x%02x\n", enable, ready);
+
+	/* If F2 successfully enabled, set core and enable interrupts */
+	if (ready == enable) {
+		/* Set up the interrupt mask and enable interrupts */
+		bus->hostintmask = HOSTINTMASK;
+		w_sdreg32(bus, bus->hostintmask,
+			  offsetof(struct sdpcmd_regs, hostintmask), &retries);
+
+		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
+				       SBSDIO_WATERMARK, 8, &err);
+
+		/* Set bus state according to enable result */
+		drvr->busstate = BRCMF_BUS_DATA;
+	}
+
+	else {
+		/* Disable F2 again */
+		enable = SDIO_FUNC_ENABLE_1;
+		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_0,
+				       SDIO_CCCR_IOEx, enable, NULL);
+	}
+
+	/* Restore previous clock setting */
+	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
+			       SBSDIO_FUNC1_CHIPCLKCSR, saveclk, &err);
+
+	/* If we didn't come up, turn off backplane clock */
+	if (drvr->busstate != BRCMF_BUS_DATA)
+		brcmf_sdbrcm_clkctl(bus, CLK_NONE, false);
+
+exit:
+	up(&bus->sdsem);
+
+	return ret;
 }
 
-static int
-brcmf_sdbrcm_chip_attach(struct brcmf_bus *bus, u32 regs)
+void brcmf_sdbrcm_isr(void *arg)
 {
-	struct chip_info *ci;
-	int err;
-	u8 clkval, clkset;
+	struct brcmf_bus *bus = (struct brcmf_bus *) arg;
 
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	brcmf_dbg(TRACE, "Enter\n");
 
-	/* alloc chip_info_t */
-	ci = kmalloc(sizeof(struct chip_info), GFP_ATOMIC);
-	if (NULL == ci) {
-		BRCMF_ERROR(("%s: malloc failed!\n", __func__));
-		return -ENOMEM;
+	if (!bus) {
+		brcmf_dbg(ERROR, "bus is null pointer, exiting\n");
+		return;
 	}
 
-	memset((unsigned char *)ci, 0, sizeof(struct chip_info));
-
-	/* bus/core/clk setup for register access */
-	/* Try forcing SDIO core to do ALPAvail request only */
-	clkset = SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_ALP_AVAIL_REQ;
-	brcmf_sdcard_cfg_write(bus->card, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
-			clkset, &err);
-	if (err) {
-		BRCMF_ERROR(("%s: error writing for HT off\n", __func__));
-		goto fail;
+	if (bus->drvr->busstate == BRCMF_BUS_DOWN) {
+		brcmf_dbg(ERROR, "bus is down. we have nothing to do\n");
+		return;
 	}
+	/* Count the interrupt call */
+	bus->intrcount++;
+	bus->ipend = true;
 
-	/* If register supported, wait for ALPAvail and then force ALP */
-	/* This may take up to 15 milliseconds */
-	clkval = brcmf_sdcard_cfg_read(bus->card, SDIO_FUNC_1,
-			SBSDIO_FUNC1_CHIPCLKCSR, NULL);
-	if ((clkval & ~SBSDIO_AVBITS) == clkset) {
-		SPINWAIT(((clkval =
-				brcmf_sdcard_cfg_read(bus->card, SDIO_FUNC_1,
-						SBSDIO_FUNC1_CHIPCLKCSR,
-						NULL)),
-				!SBSDIO_ALPAV(clkval)),
-				PMU_MAX_TRANSITION_DLY);
-		if (!SBSDIO_ALPAV(clkval)) {
-			BRCMF_ERROR(("%s: timeout on ALPAV wait,"
-				     " clkval 0x%02x\n", __func__, clkval));
-			err = -EBUSY;
-			goto fail;
-		}
-		clkset = SBSDIO_FORCE_HW_CLKREQ_OFF |
-				SBSDIO_FORCE_ALP;
-		brcmf_sdcard_cfg_write(bus->card, SDIO_FUNC_1,
-				SBSDIO_FUNC1_CHIPCLKCSR,
-				clkset, &err);
-		udelay(65);
-	} else {
-		BRCMF_ERROR(("%s: ChipClkCSR access: wrote 0x%02x"
-			     " read 0x%02x\n", __func__, clkset, clkval));
-		err = -EACCES;
-		goto fail;
+	/* Shouldn't get this interrupt if we're sleeping? */
+	if (bus->sleeping) {
+		brcmf_dbg(ERROR, "INTERRUPT WHILE SLEEPING??\n");
+		return;
 	}
 
-	/* Also, disable the extra SDIO pull-ups */
-	brcmf_sdcard_cfg_write(bus->card, SDIO_FUNC_1, SBSDIO_FUNC1_SDIOPULLUP,
-			       0, NULL);
+	/* Disable additional interrupts (is this needed now)? */
+	if (!bus->intr)
+		brcmf_dbg(ERROR, "isr w/o interrupt configured!\n");
 
-	err = brcmf_sdbrcm_chip_recognition(bus->card, ci, regs);
-	if (err)
-		goto fail;
+#if defined(ISR_THREAD)
+	while (brcmf_sdbrcm_dpc(bus))
+		;
+#else
+	bus->dpc_sched = true;
+	if (bus->dpc_tsk)
+		complete(&bus->dpc_wait);
+#endif
 
-	/*
-	 * Make sure any on-chip ARM is off (in case strapping is wrong),
-	 * or downloaded code was already running.
-	 */
-	brcmf_sdbrcm_chip_disablecore(bus->card, ci->armcorebase);
+}
 
-	brcmf_sdcard_reg_write(bus->card,
-		CORE_CC_REG(ci->cccorebase, gpiopullup), 4, 0);
-	brcmf_sdcard_reg_write(bus->card,
-		CORE_CC_REG(ci->cccorebase, gpiopulldown), 4, 0);
+static bool brcmf_sdbrcm_bus_watchdog(struct brcmf_pub *drvr)
+{
+	struct brcmf_bus *bus;
 
-	/* Disable F2 to clear any intermediate frame state on the dongle */
-	brcmf_sdcard_cfg_write(bus->card, SDIO_FUNC_0, SDIO_CCCR_IOEx,
-		SDIO_FUNC_ENABLE_1, NULL);
+	brcmf_dbg(TIMER, "Enter\n");
 
-	/* WAR: cmd52 backplane read so core HW will drop ALPReq */
-	clkval = brcmf_sdcard_cfg_read(bus->card, SDIO_FUNC_1,
-			0, NULL);
+	bus = drvr->bus;
 
-	/* Done with backplane-dependent accesses, can drop clock... */
-	brcmf_sdcard_cfg_write(bus->card, SDIO_FUNC_1, SBSDIO_FUNC1_CHIPCLKCSR,
-			       0, NULL);
+	/* Ignore the timer if simulating bus down */
+	if (bus->sleeping)
+		return false;
 
-	bus->ci = ci;
-	return 0;
-fail:
-	bus->ci = NULL;
-	kfree(ci);
-	return err;
+	down(&bus->sdsem);
+
+	/* Poll period: check device if appropriate. */
+	if (bus->poll && (++bus->polltick >= bus->pollrate)) {
+		u32 intstatus = 0;
+
+		/* Reset poll tick */
+		bus->polltick = 0;
+
+		/* Check device if no interrupts */
+		if (!bus->intr || (bus->intrcount == bus->lastintrs)) {
+
+			if (!bus->dpc_sched) {
+				u8 devpend;
+				devpend = brcmf_sdcard_cfg_read(bus->sdiodev,
+						SDIO_FUNC_0, SDIO_CCCR_INTx,
+						NULL);
+				intstatus =
+				    devpend & (INTR_STATUS_FUNC1 |
+					       INTR_STATUS_FUNC2);
+			}
+
+			/* If there is something, make like the ISR and
+				 schedule the DPC */
+			if (intstatus) {
+				bus->pollcnt++;
+				bus->ipend = true;
+
+				bus->dpc_sched = true;
+				if (bus->dpc_tsk)
+					complete(&bus->dpc_wait);
+			}
+		}
+
+		/* Update interrupt tracking */
+		bus->lastintrs = bus->intrcount;
+	}
+#ifdef BCMDBG
+	/* Poll for console output periodically */
+	if (drvr->busstate == BRCMF_BUS_DATA && bus->console_interval != 0) {
+		bus->console.count += BRCMF_WD_POLL_MS;
+		if (bus->console.count >= bus->console_interval) {
+			bus->console.count -= bus->console_interval;
+			/* Make sure backplane clock is on */
+			brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
+			if (brcmf_sdbrcm_readconsole(bus) < 0)
+				/* stop on error */
+				bus->console_interval = 0;
+		}
+	}
+#endif				/* BCMDBG */
+
+	/* On idle timeout clear activity flag and/or turn off clock */
+	if ((bus->idletime > 0) && (bus->clkstate == CLK_AVAIL)) {
+		if (++bus->idlecount >= bus->idletime) {
+			bus->idlecount = 0;
+			if (bus->activity) {
+				bus->activity = false;
+				brcmf_sdbrcm_wd_timer(bus, BRCMF_WD_POLL_MS);
+			} else {
+				brcmf_sdbrcm_clkctl(bus, CLK_NONE, false);
+			}
+		}
+	}
+
+	up(&bus->sdsem);
+
+	return bus->ipend;
 }
 
-static void
-brcmf_sdbrcm_chip_resetcore(struct brcmf_sdio_card *card, u32 corebase)
+static bool brcmf_sdbrcm_chipmatch(u16 chipid)
 {
-	u32 regdata;
+	if (chipid == BCM4329_CHIP_ID)
+		return true;
+	return false;
+}
 
-	/*
-	 * Must do the disable sequence first to work for
-	 * arbitrary current core state.
-	 */
-	brcmf_sdbrcm_chip_disablecore(card, corebase);
+static void brcmf_sdbrcm_release_malloc(struct brcmf_bus *bus)
+{
+	brcmf_dbg(TRACE, "Enter\n");
 
-	/*
-	 * Now do the initialization sequence.
-	 * set reset while enabling the clock and
-	 * forcing them on throughout the core
-	 */
-	brcmf_sdcard_reg_write(card, CORE_SB(corebase, sbtmstatelow), 4,
-		((SICF_FGC | SICF_CLOCK_EN) << SBTML_SICF_SHIFT) |
-		SBTML_RESET);
-	udelay(1);
+	kfree(bus->rxbuf);
+	bus->rxctl = bus->rxbuf = NULL;
+	bus->rxlen = 0;
 
-	regdata = brcmf_sdcard_reg_read(card, CORE_SB(corebase, sbtmstatehigh),
-					4);
-	if (regdata & SBTMH_SERR)
-		brcmf_sdcard_reg_write(card, CORE_SB(corebase, sbtmstatehigh),
-				       4, 0);
+	kfree(bus->databuf);
+	bus->databuf = NULL;
+}
 
-	regdata = brcmf_sdcard_reg_read(card, CORE_SB(corebase, sbimstate), 4);
-	if (regdata & (SBIM_IBE | SBIM_TO))
-		brcmf_sdcard_reg_write(card, CORE_SB(corebase, sbimstate), 4,
-			regdata & ~(SBIM_IBE | SBIM_TO));
+static bool brcmf_sdbrcm_probe_malloc(struct brcmf_bus *bus)
+{
+	brcmf_dbg(TRACE, "Enter\n");
 
-	/* clear reset and allow it to propagate throughout the core */
-	brcmf_sdcard_reg_write(card, CORE_SB(corebase, sbtmstatelow), 4,
-		(SICF_FGC << SBTML_SICF_SHIFT) |
-		(SICF_CLOCK_EN << SBTML_SICF_SHIFT));
-	udelay(1);
+	if (bus->drvr->maxctl) {
+		bus->rxblen =
+		    roundup((bus->drvr->maxctl + SDPCM_HDRLEN),
+			    ALIGNMENT) + BRCMF_SDALIGN;
+		bus->rxbuf = kmalloc(bus->rxblen, GFP_ATOMIC);
+		if (!(bus->rxbuf))
+			goto fail;
+	}
 
-	/* leave clock enabled */
-	brcmf_sdcard_reg_write(card, CORE_SB(corebase, sbtmstatelow), 4,
-		(SICF_CLOCK_EN << SBTML_SICF_SHIFT));
-	udelay(1);
+	/* Allocate buffer to receive glomed packet */
+	bus->databuf = kmalloc(MAX_DATA_BUF, GFP_ATOMIC);
+	if (!(bus->databuf)) {
+		/* release rxbuf which was already located as above */
+		if (!bus->rxblen)
+			kfree(bus->rxbuf);
+		goto fail;
+	}
+
+	/* Align the buffer */
+	if ((unsigned long)bus->databuf % BRCMF_SDALIGN)
+		bus->dataptr = bus->databuf + (BRCMF_SDALIGN -
+			       ((unsigned long)bus->databuf % BRCMF_SDALIGN));
+	else
+		bus->dataptr = bus->databuf;
+
+	return true;
+
+fail:
+	return false;
 }
 
 /* SDIO Pad drive strength to select value mappings */
@@ -6499,8 +3998,17 @@ static const struct sdiod_drive_str sdiod_drive_strength_tab3[] = {
 
 #define SDIOD_DRVSTR_KEY(chip, pmu)     (((chip) << 16) | (pmu))
 
-static void
-brcmf_sdbrcm_sdiod_drive_strength_init(struct brcmf_bus *bus, u32 drivestrength) {
+static char *brcmf_chipname(uint chipid, char *buf, uint len)
+{
+	const char *fmt;
+
+	fmt = ((chipid > 0xa000) || (chipid < 0x4000)) ? "%d" : "%x";
+	snprintf(buf, len, fmt, chipid);
+	return buf;
+}
+
+static void brcmf_sdbrcm_sdiod_drive_strength_init(struct brcmf_bus *bus,
+						   u32 drivestrength) {
 	struct sdiod_drive_str *str_tab = NULL;
 	u32 str_mask = 0;
 	u32 str_shift = 0;
@@ -6527,10 +4035,9 @@ brcmf_sdbrcm_sdiod_drive_strength_init(struct brcmf_bus *bus, u32 drivestrength)
 		str_shift = 11;
 		break;
 	default:
-		BRCMF_ERROR(("No SDIO Drive strength init"
-			     "done for chip %s rev %d pmurev %d\n",
-			     brcmu_chipname(bus->ci->chip, chn, 8),
-			     bus->ci->chiprev, bus->ci->pmurev));
+		brcmf_dbg(ERROR, "No SDIO Drive strength init done for chip %s rev %d pmurev %d\n",
+			  brcmf_chipname(bus->ci->chip, chn, 8),
+			  bus->ci->chiprev, bus->ci->pmurev);
 		break;
 	}
 
@@ -6546,48 +4053,287 @@ brcmf_sdbrcm_sdiod_drive_strength_init(struct brcmf_bus *bus, u32 drivestrength)
 			}
 		}
 
-		brcmf_sdcard_reg_write(bus->card,
+		brcmf_sdcard_reg_write(bus->sdiodev,
 			CORE_CC_REG(bus->ci->cccorebase, chipcontrol_addr),
 			4, 1);
-		cc_data_temp = brcmf_sdcard_reg_read(bus->card,
+		cc_data_temp = brcmf_sdcard_reg_read(bus->sdiodev,
 			CORE_CC_REG(bus->ci->cccorebase, chipcontrol_addr), 4);
 		cc_data_temp &= ~str_mask;
 		drivestrength_sel <<= str_shift;
 		cc_data_temp |= drivestrength_sel;
-		brcmf_sdcard_reg_write(bus->card,
+		brcmf_sdcard_reg_write(bus->sdiodev,
 			CORE_CC_REG(bus->ci->cccorebase, chipcontrol_addr),
 			4, cc_data_temp);
 
-		BRCMF_INFO(("SDIO: %dmA drive strength selected, "
-			    "set to 0x%08x\n", drivestrength, cc_data_temp));
+		brcmf_dbg(INFO, "SDIO: %dmA drive strength selected, set to 0x%08x\n",
+			  drivestrength, cc_data_temp);
 	}
 }
 
-static void
-brcmf_sdbrcm_chip_detach(struct brcmf_bus *bus)
+static int
+brcmf_sdbrcm_chip_recognition(struct brcmf_sdio_dev *sdiodev,
+			      struct chip_info *ci, u32 regs)
 {
-	BRCMF_TRACE(("%s: Enter\n", __func__));
+	u32 regdata;
 
-	kfree(bus->ci);
+	/*
+	 * Get CC core rev
+	 * Chipid is assume to be at offset 0 from regs arg
+	 * For different chiptypes or old sdio hosts w/o chipcommon,
+	 * other ways of recognition should be added here.
+	 */
+	ci->cccorebase = regs;
+	regdata = brcmf_sdcard_reg_read(sdiodev,
+				CORE_CC_REG(ci->cccorebase, chipid), 4);
+	ci->chip = regdata & CID_ID_MASK;
+	ci->chiprev = (regdata & CID_REV_MASK) >> CID_REV_SHIFT;
+
+	brcmf_dbg(INFO, "chipid=0x%x chiprev=%d\n", ci->chip, ci->chiprev);
+
+	/* Address of cores for new chips should be added here */
+	switch (ci->chip) {
+	case BCM4329_CHIP_ID:
+		ci->buscorebase = BCM4329_CORE_BUS_BASE;
+		ci->ramcorebase = BCM4329_CORE_SOCRAM_BASE;
+		ci->armcorebase	= BCM4329_CORE_ARM_BASE;
+		ci->ramsize = BCM4329_RAMSIZE;
+		break;
+	default:
+		brcmf_dbg(ERROR, "chipid 0x%x is not supported\n", ci->chip);
+		return -ENODEV;
+	}
+
+	regdata = brcmf_sdcard_reg_read(sdiodev,
+		CORE_SB(ci->cccorebase, sbidhigh), 4);
+	ci->ccrev = SBCOREREV(regdata);
+
+	regdata = brcmf_sdcard_reg_read(sdiodev,
+		CORE_CC_REG(ci->cccorebase, pmucapabilities), 4);
+	ci->pmurev = regdata & PCAP_REV_MASK;
+
+	regdata = brcmf_sdcard_reg_read(sdiodev,
+					CORE_SB(ci->buscorebase, sbidhigh), 4);
+	ci->buscorerev = SBCOREREV(regdata);
+	ci->buscoretype = (regdata & SBIDH_CC_MASK) >> SBIDH_CC_SHIFT;
+
+	brcmf_dbg(INFO, "ccrev=%d, pmurev=%d, buscore rev/type=%d/0x%x\n",
+		  ci->ccrev, ci->pmurev, ci->buscorerev, ci->buscoretype);
+
+	/* get chipcommon capabilites */
+	ci->cccaps = brcmf_sdcard_reg_read(sdiodev,
+		CORE_CC_REG(ci->cccorebase, capabilities), 4);
+
+	return 0;
+}
+
+static int
+brcmf_sdbrcm_chip_attach(struct brcmf_bus *bus, u32 regs)
+{
+	struct chip_info *ci;
+	int err;
+	u8 clkval, clkset;
+
+	brcmf_dbg(TRACE, "Enter\n");
+
+	/* alloc chip_info_t */
+	ci = kzalloc(sizeof(struct chip_info), GFP_ATOMIC);
+	if (NULL == ci)
+		return -ENOMEM;
+
+	/* bus/core/clk setup for register access */
+	/* Try forcing SDIO core to do ALPAvail request only */
+	clkset = SBSDIO_FORCE_HW_CLKREQ_OFF | SBSDIO_ALP_AVAIL_REQ;
+	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
+			       SBSDIO_FUNC1_CHIPCLKCSR,	clkset, &err);
+	if (err) {
+		brcmf_dbg(ERROR, "error writing for HT off\n");
+		goto fail;
+	}
+
+	/* If register supported, wait for ALPAvail and then force ALP */
+	/* This may take up to 15 milliseconds */
+	clkval = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
+			SBSDIO_FUNC1_CHIPCLKCSR, NULL);
+	if ((clkval & ~SBSDIO_AVBITS) == clkset) {
+		SPINWAIT(((clkval =
+				brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
+						SBSDIO_FUNC1_CHIPCLKCSR,
+						NULL)),
+				!SBSDIO_ALPAV(clkval)),
+				PMU_MAX_TRANSITION_DLY);
+		if (!SBSDIO_ALPAV(clkval)) {
+			brcmf_dbg(ERROR, "timeout on ALPAV wait, clkval 0x%02x\n",
+				  clkval);
+			err = -EBUSY;
+			goto fail;
+		}
+		clkset = SBSDIO_FORCE_HW_CLKREQ_OFF |
+				SBSDIO_FORCE_ALP;
+		brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
+				SBSDIO_FUNC1_CHIPCLKCSR,
+				clkset, &err);
+		udelay(65);
+	} else {
+		brcmf_dbg(ERROR, "ChipClkCSR access: wrote 0x%02x read 0x%02x\n",
+			  clkset, clkval);
+		err = -EACCES;
+		goto fail;
+	}
+
+	/* Also, disable the extra SDIO pull-ups */
+	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
+			       SBSDIO_FUNC1_SDIOPULLUP, 0, NULL);
+
+	err = brcmf_sdbrcm_chip_recognition(bus->sdiodev, ci, regs);
+	if (err)
+		goto fail;
+
+	/*
+	 * Make sure any on-chip ARM is off (in case strapping is wrong),
+	 * or downloaded code was already running.
+	 */
+	brcmf_sdbrcm_chip_disablecore(bus->sdiodev, ci->armcorebase);
+
+	brcmf_sdcard_reg_write(bus->sdiodev,
+		CORE_CC_REG(ci->cccorebase, gpiopullup), 4, 0);
+	brcmf_sdcard_reg_write(bus->sdiodev,
+		CORE_CC_REG(ci->cccorebase, gpiopulldown), 4, 0);
+
+	/* Disable F2 to clear any intermediate frame state on the dongle */
+	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_0, SDIO_CCCR_IOEx,
+		SDIO_FUNC_ENABLE_1, NULL);
+
+	/* WAR: cmd52 backplane read so core HW will drop ALPReq */
+	clkval = brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
+			0, NULL);
+
+	/* Done with backplane-dependent accesses, can drop clock... */
+	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
+			       SBSDIO_FUNC1_CHIPCLKCSR, 0, NULL);
+
+	bus->ci = ci;
+	return 0;
+fail:
 	bus->ci = NULL;
+	kfree(ci);
+	return err;
 }
 
-static void
-brcmf_sdbrcm_wait_for_event(struct brcmf_bus *bus, bool *lockvar)
+static bool
+brcmf_sdbrcm_probe_attach(struct brcmf_bus *bus, u32 regsva)
 {
-	brcmf_sdbrcm_sdunlock(bus);
-	wait_event_interruptible_timeout(bus->ctrl_wait,
-					 (*lockvar == false), HZ * 2);
-	brcmf_sdbrcm_sdlock(bus);
-	return;
+	u8 clkctl = 0;
+	int err = 0;
+	int reg_addr;
+	u32 reg_val;
+
+	bus->alp_only = true;
+
+	/* Return the window to backplane enumeration space for core access */
+	if (brcmf_sdcard_set_sbaddr_window(bus->sdiodev, SI_ENUM_BASE))
+		brcmf_dbg(ERROR, "FAILED to return to SI_ENUM_BASE\n");
+
+#ifdef BCMDBG
+	printk(KERN_DEBUG "F1 signature read @0x18000000=0x%4x\n",
+	       brcmf_sdcard_reg_read(bus->sdiodev, SI_ENUM_BASE, 4));
+
+#endif				/* BCMDBG */
+
+	/*
+	 * Force PLL off until brcmf_sdbrcm_chip_attach()
+	 * programs PLL control regs
+	 */
+
+	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
+			       SBSDIO_FUNC1_CHIPCLKCSR,
+			       BRCMF_INIT_CLKCTL1, &err);
+	if (!err)
+		clkctl =
+		    brcmf_sdcard_cfg_read(bus->sdiodev, SDIO_FUNC_1,
+					  SBSDIO_FUNC1_CHIPCLKCSR, &err);
+
+	if (err || ((clkctl & ~SBSDIO_AVBITS) != BRCMF_INIT_CLKCTL1)) {
+		brcmf_dbg(ERROR, "ChipClkCSR access: err %d wrote 0x%02x read 0x%02x\n",
+			  err, BRCMF_INIT_CLKCTL1, clkctl);
+		goto fail;
+	}
+
+	if (brcmf_sdbrcm_chip_attach(bus, regsva)) {
+		brcmf_dbg(ERROR, "brcmf_sdbrcm_chip_attach failed!\n");
+		goto fail;
+	}
+
+	if (!brcmf_sdbrcm_chipmatch((u16) bus->ci->chip)) {
+		brcmf_dbg(ERROR, "unsupported chip: 0x%04x\n", bus->ci->chip);
+		goto fail;
+	}
+
+	brcmf_sdbrcm_sdiod_drive_strength_init(bus, SDIO_DRIVE_STRENGTH);
+
+	/* Get info on the ARM and SOCRAM cores... */
+	brcmf_sdcard_reg_read(bus->sdiodev,
+		  CORE_SB(bus->ci->armcorebase, sbidhigh), 4);
+	bus->ramsize = bus->ci->ramsize;
+	if (!(bus->ramsize)) {
+		brcmf_dbg(ERROR, "failed to find SOCRAM memory!\n");
+		goto fail;
+	}
+
+	/* Set core control so an SDIO reset does a backplane reset */
+	reg_addr = bus->ci->buscorebase +
+		   offsetof(struct sdpcmd_regs, corecontrol);
+	reg_val = brcmf_sdcard_reg_read(bus->sdiodev, reg_addr, sizeof(u32));
+	brcmf_sdcard_reg_write(bus->sdiodev, reg_addr, sizeof(u32),
+			       reg_val | CC_BPRESEN);
+
+	brcmu_pktq_init(&bus->txq, (PRIOMASK + 1), TXQLEN);
+
+	/* Locate an appropriately-aligned portion of hdrbuf */
+	bus->rxhdr = (u8 *) roundup((unsigned long)&bus->hdrbuf[0],
+				    BRCMF_SDALIGN);
+
+	/* Set the poll and/or interrupt flags */
+	bus->intr = true;
+	bus->poll = false;
+	if (bus->poll)
+		bus->pollrate = 1;
+
+	return true;
+
+fail:
+	return false;
 }
 
-static void
-brcmf_sdbrcm_wait_event_wakeup(struct brcmf_bus *bus)
+static bool brcmf_sdbrcm_probe_init(struct brcmf_bus *bus)
 {
-	if (waitqueue_active(&bus->ctrl_wait))
-		wake_up_interruptible(&bus->ctrl_wait);
-	return;
+	brcmf_dbg(TRACE, "Enter\n");
+
+	/* Disable F2 to clear any intermediate frame state on the dongle */
+	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_0, SDIO_CCCR_IOEx,
+			       SDIO_FUNC_ENABLE_1, NULL);
+
+	bus->drvr->busstate = BRCMF_BUS_DOWN;
+	bus->sleeping = false;
+	bus->rxflow = false;
+
+	/* Done with backplane-dependent accesses, can drop clock... */
+	brcmf_sdcard_cfg_write(bus->sdiodev, SDIO_FUNC_1,
+			       SBSDIO_FUNC1_CHIPCLKCSR, 0, NULL);
+
+	/* ...and initialize clock/power states */
+	bus->clkstate = CLK_SDONLY;
+	bus->idletime = BRCMF_IDLE_INTERVAL;
+	bus->idleclock = BRCMF_IDLE_ACTIVE;
+
+	/* Query the F2 block size, set roundup accordingly */
+	bus->blocksize = bus->sdiodev->func[2]->cur_blksize;
+	bus->roundup = min(max_roundup, bus->blocksize);
+
+	/* bus module does not support packet chaining */
+	bus->use_rxchain = false;
+	bus->sd_rxchain = false;
+
+	return true;
 }
 
 static int
@@ -6595,24 +4341,13 @@ brcmf_sdbrcm_watchdog_thread(void *data)
 {
 	struct brcmf_bus *bus = (struct brcmf_bus *)data;
 
-	/* This thread doesn't need any user-level access,
-	* so get rid of all our resources
-	*/
-	if (brcmf_watchdog_prio > 0) {
-		struct sched_param param;
-		param.sched_priority = (brcmf_watchdog_prio < MAX_RT_PRIO) ?
-				       brcmf_watchdog_prio : (MAX_RT_PRIO - 1);
-		sched_setscheduler(current, SCHED_FIFO, &param);
-	}
-
 	allow_signal(SIGTERM);
 	/* Run until signal received */
 	while (1) {
 		if (kthread_should_stop())
 			break;
 		if (!wait_for_completion_interruptible(&bus->watchdog_wait)) {
-			if (bus->drvr->dongle_reset == false)
-				brcmf_sdbrcm_bus_watchdog(bus->drvr);
+			brcmf_sdbrcm_bus_watchdog(bus->drvr);
 			/* Count the tick for reference */
 			bus->drvr->tickcnt++;
 		} else
@@ -6626,147 +4361,237 @@ brcmf_sdbrcm_watchdog(unsigned long data)
 {
 	struct brcmf_bus *bus = (struct brcmf_bus *)data;
 
-	if (brcmf_watchdog_prio >= 0) {
-		if (bus->watchdog_tsk)
-			complete(&bus->watchdog_wait);
-		else
-			return;
-	} else {
-		brcmf_sdbrcm_bus_watchdog(bus->drvr);
-
-		/* Count the tick for reference */
-		bus->drvr->tickcnt++;
+	if (bus->watchdog_tsk) {
+		complete(&bus->watchdog_wait);
+		/* Reschedule the watchdog */
+		if (bus->wd_timer_valid)
+			mod_timer(&bus->timer,
+				  jiffies + BRCMF_WD_POLL_MS * HZ / 1000);
 	}
-
-	/* Reschedule the watchdog */
-	if (bus->wd_timer_valid)
-		mod_timer(&bus->timer, jiffies + brcmf_watchdog_ms * HZ / 1000);
 }
 
-void
-brcmf_sdbrcm_wd_timer(struct brcmf_bus *bus, uint wdtick)
+static void
+brcmf_sdbrcm_chip_detach(struct brcmf_bus *bus)
 {
-	static uint save_ms;
+	brcmf_dbg(TRACE, "Enter\n");
 
-	/* don't start the wd until fw is loaded */
-	if (bus->drvr->busstate == BRCMF_BUS_DOWN)
-		return;
+	kfree(bus->ci);
+	bus->ci = NULL;
+}
 
-	/* Totally stop the timer */
-	if (!wdtick && bus->wd_timer_valid == true) {
-		del_timer_sync(&bus->timer);
-		bus->wd_timer_valid = false;
-		save_ms = wdtick;
-		return;
+static void brcmf_sdbrcm_release_dongle(struct brcmf_bus *bus)
+{
+	brcmf_dbg(TRACE, "Enter\n");
+
+	if (bus->ci) {
+		brcmf_sdbrcm_clkctl(bus, CLK_AVAIL, false);
+		brcmf_sdbrcm_clkctl(bus, CLK_NONE, false);
+		brcmf_sdbrcm_chip_detach(bus);
+		if (bus->vars && bus->varsz)
+			kfree(bus->vars);
+		bus->vars = NULL;
 	}
 
-	if (wdtick) {
-		brcmf_watchdog_ms = (uint) wdtick;
+	brcmf_dbg(TRACE, "Disconnected\n");
+}
 
-		if (save_ms != brcmf_watchdog_ms) {
-			if (bus->wd_timer_valid == true)
-				/* Stop timer and restart at new value */
-				del_timer_sync(&bus->timer);
+/* Detach and free everything */
+static void brcmf_sdbrcm_release(struct brcmf_bus *bus)
+{
+	brcmf_dbg(TRACE, "Enter\n");
 
-			/* Create timer again when watchdog period is
-			   dynamically changed or in the first instance
-			 */
-			bus->timer.expires =
-				jiffies + brcmf_watchdog_ms * HZ / 1000;
-			add_timer(&bus->timer);
+	if (bus) {
+		/* De-register interrupt handler */
+		brcmf_sdcard_intr_dereg(bus->sdiodev);
 
-		} else {
-			/* Re arm the timer, at last watchdog period */
-			mod_timer(&bus->timer,
-				jiffies + brcmf_watchdog_ms * HZ / 1000);
+		if (bus->drvr) {
+			brcmf_detach(bus->drvr);
+			brcmf_sdbrcm_release_dongle(bus);
+			bus->drvr = NULL;
 		}
 
-		bus->wd_timer_valid = true;
-		save_ms = wdtick;
+		brcmf_sdbrcm_release_malloc(bus);
+
+		kfree(bus);
 	}
+
+	brcmf_dbg(TRACE, "Disconnected\n");
 }
 
-static int brcmf_sdbrcm_dpc_thread(void *data)
+void *brcmf_sdbrcm_probe(u16 bus_no, u16 slot, u16 func, uint bustype,
+			 u32 regsva, struct brcmf_sdio_dev *sdiodev)
 {
-	struct brcmf_bus *bus = (struct brcmf_bus *) data;
+	int ret;
+	struct brcmf_bus *bus;
 
-	/* This thread doesn't need any user-level access,
-	 * so get rid of all our resources
+	/* Init global variables at run-time, not as part of the declaration.
+	 * This is required to support init/de-init of the driver.
+	 * Initialization
+	 * of globals as part of the declaration results in non-deterministic
+	 * behavior since the value of the globals may be different on the
+	 * first time that the driver is initialized vs subsequent
+	 * initializations.
 	 */
-	if (brcmf_dpc_prio > 0) {
-		struct sched_param param;
-		param.sched_priority = (brcmf_dpc_prio < MAX_RT_PRIO) ?
-				       brcmf_dpc_prio : (MAX_RT_PRIO - 1);
-		sched_setscheduler(current, SCHED_FIFO, &param);
+	brcmf_c_init();
+
+	brcmf_dbg(TRACE, "Enter\n");
+
+	/* We make an assumption about address window mappings:
+	 * regsva == SI_ENUM_BASE*/
+
+	/* Allocate private bus interface state */
+	bus = kzalloc(sizeof(struct brcmf_bus), GFP_ATOMIC);
+	if (!bus)
+		goto fail;
+
+	bus->sdiodev = sdiodev;
+	sdiodev->bus = bus;
+	bus->txbound = BRCMF_TXBOUND;
+	bus->rxbound = BRCMF_RXBOUND;
+	bus->txminmax = BRCMF_TXMINMAX;
+	bus->tx_seq = SDPCM_SEQUENCE_WRAP - 1;
+	bus->usebufpool = false;	/* Use bufpool if allocated,
+					 else use locally malloced rxbuf */
+
+	/* attempt to attach to the dongle */
+	if (!(brcmf_sdbrcm_probe_attach(bus, regsva))) {
+		brcmf_dbg(ERROR, "brcmf_sdbrcm_probe_attach failed\n");
+		goto fail;
 	}
 
-	allow_signal(SIGTERM);
-	/* Run until signal received */
-	while (1) {
-		if (kthread_should_stop())
-			break;
-		if (!wait_for_completion_interruptible(&bus->dpc_wait)) {
-			/* Call bus dpc unless it indicated down
-			(then clean stop) */
-			if (bus->drvr->busstate != BRCMF_BUS_DOWN) {
-				if (brcmf_sdbrcm_dpc(bus))
-					complete(&bus->dpc_wait);
-			} else {
-				brcmf_sdbrcm_bus_stop(bus, true);
-			}
-		} else
-			break;
+	spin_lock_init(&bus->txqlock);
+	init_waitqueue_head(&bus->ctrl_wait);
+	init_waitqueue_head(&bus->dcmd_resp_wait);
+
+	/* Set up the watchdog timer */
+	init_timer(&bus->timer);
+	bus->timer.data = (unsigned long)bus;
+	bus->timer.function = brcmf_sdbrcm_watchdog;
+
+	/* Initialize thread based operation and lock */
+	sema_init(&bus->sdsem, 1);
+
+	/* Initialize watchdog thread */
+	init_completion(&bus->watchdog_wait);
+	bus->watchdog_tsk = kthread_run(brcmf_sdbrcm_watchdog_thread,
+					bus, "brcmf_watchdog");
+	if (IS_ERR(bus->watchdog_tsk)) {
+		printk(KERN_WARNING
+		       "brcmf_watchdog thread failed to start\n");
+		bus->watchdog_tsk = NULL;
+	}
+	/* Initialize DPC thread */
+	init_completion(&bus->dpc_wait);
+	bus->dpc_tsk = kthread_run(brcmf_sdbrcm_dpc_thread,
+				   bus, "brcmf_dpc");
+	if (IS_ERR(bus->dpc_tsk)) {
+		printk(KERN_WARNING
+		       "brcmf_dpc thread failed to start\n");
+		bus->dpc_tsk = NULL;
 	}
-	return 0;
-}
 
-static void brcmf_sdbrcm_dpc_tasklet(unsigned long data)
-{
-	struct brcmf_bus *bus = (struct brcmf_bus *) data;
+	/* Attach to the brcmf/OS/network interface */
+	bus->drvr = brcmf_attach(bus, SDPCM_RESERVE);
+	if (!bus->drvr) {
+		brcmf_dbg(ERROR, "brcmf_attach failed\n");
+		goto fail;
+	}
 
-	/* Call bus dpc unless it indicated down (then clean stop) */
-	if (bus->drvr->busstate != BRCMF_BUS_DOWN) {
-		if (brcmf_sdbrcm_dpc(bus))
-			tasklet_schedule(&bus->tasklet);
-	} else
-		brcmf_sdbrcm_bus_stop(bus, true);
-}
+	/* Allocate buffers */
+	if (!(brcmf_sdbrcm_probe_malloc(bus))) {
+		brcmf_dbg(ERROR, "brcmf_sdbrcm_probe_malloc failed\n");
+		goto fail;
+	}
 
-static void brcmf_sdbrcm_sched_dpc(struct brcmf_bus *bus)
-{
-	if (bus->dpc_tsk) {
-		complete(&bus->dpc_wait);
-		return;
+	if (!(brcmf_sdbrcm_probe_init(bus))) {
+		brcmf_dbg(ERROR, "brcmf_sdbrcm_probe_init failed\n");
+		goto fail;
 	}
 
-	tasklet_schedule(&bus->tasklet);
+	/* Register interrupt callback, but mask it (not operational yet). */
+	brcmf_dbg(INTR, "disable SDIO interrupts (not interested yet)\n");
+	ret = brcmf_sdcard_intr_reg(bus->sdiodev);
+	if (ret != 0) {
+		brcmf_dbg(ERROR, "FAILED: sdcard_intr_reg returned %d\n", ret);
+		goto fail;
+	}
+	brcmf_dbg(INTR, "registered SDIO interrupt function ok\n");
+
+	brcmf_dbg(INFO, "completed!!\n");
+
+	/* if firmware path present try to download and bring up bus */
+	ret = brcmf_bus_start(bus->drvr);
+	if (ret != 0) {
+		if (ret == -ENOLINK) {
+			brcmf_dbg(ERROR, "dongle is not responding\n");
+			goto fail;
+		}
+	}
+	/* Ok, have the per-port tell the stack we're open for business */
+	if (brcmf_net_attach(bus->drvr, 0) != 0) {
+		brcmf_dbg(ERROR, "Net attach failed!!\n");
+		goto fail;
+	}
+
+	return bus;
+
+fail:
+	brcmf_sdbrcm_release(bus);
+	return NULL;
 }
 
-static void brcmf_sdbrcm_sdlock(struct brcmf_bus *bus)
+void brcmf_sdbrcm_disconnect(void *ptr)
 {
-	if (bus->threads_only)
-		down(&bus->sdsem);
-	else
-		spin_lock_bh(&bus->sdlock);
+	struct brcmf_bus *bus = (struct brcmf_bus *)ptr;
+
+	brcmf_dbg(TRACE, "Enter\n");
+
+	if (bus)
+		brcmf_sdbrcm_release(bus);
+
+	brcmf_dbg(TRACE, "Disconnected\n");
 }
 
-static void brcmf_sdbrcm_sdunlock(struct brcmf_bus *bus)
+struct device *brcmf_bus_get_device(struct brcmf_bus *bus)
 {
-	if (bus->threads_only)
-		up(&bus->sdsem);
-	else
-		spin_unlock_bh(&bus->sdlock);
+	return &bus->sdiodev->func[2]->dev;
 }
 
-static int brcmf_sdbrcm_get_image(char *buf, int len, struct brcmf_bus *bus)
+void
+brcmf_sdbrcm_wd_timer(struct brcmf_bus *bus, uint wdtick)
 {
-	if (bus->firmware->size < bus->fw_ptr + len)
-		len = bus->firmware->size - bus->fw_ptr;
+	/* don't start the wd until fw is loaded */
+	if (bus->drvr->busstate == BRCMF_BUS_DOWN)
+		return;
 
-	memcpy(buf, &bus->firmware->data[bus->fw_ptr], len);
-	bus->fw_ptr += len;
-	return len;
-}
+	/* Totally stop the timer */
+	if (!wdtick && bus->wd_timer_valid == true) {
+		del_timer_sync(&bus->timer);
+		bus->wd_timer_valid = false;
+		bus->save_ms = wdtick;
+		return;
+	}
 
-MODULE_FIRMWARE(BCM4329_FW_NAME);
-MODULE_FIRMWARE(BCM4329_NV_NAME);
+	if (wdtick) {
+		if (bus->save_ms != BRCMF_WD_POLL_MS) {
+			if (bus->wd_timer_valid == true)
+				/* Stop timer and restart at new value */
+				del_timer_sync(&bus->timer);
+
+			/* Create timer again when watchdog period is
+			   dynamically changed or in the first instance
+			 */
+			bus->timer.expires =
+				jiffies + BRCMF_WD_POLL_MS * HZ / 1000;
+			add_timer(&bus->timer);
+
+		} else {
+			/* Re arm the timer, at last watchdog period */
+			mod_timer(&bus->timer,
+				jiffies + BRCMF_WD_POLL_MS * HZ / 1000);
+		}
+
+		bus->wd_timer_valid = true;
+		bus->save_ms = wdtick;
+	}
+}
diff --git a/drivers/staging/brcm80211/brcmfmac/sdio_host.h b/drivers/staging/brcm80211/brcmfmac/sdio_host.h
index d34547215060..726fa8981113 100644
--- a/drivers/staging/brcm80211/brcmfmac/sdio_host.h
+++ b/drivers/staging/brcm80211/brcmfmac/sdio_host.h
@@ -18,7 +18,6 @@
 #define	_BRCM_SDH_H_
 
 #include <linux/skbuff.h>
-extern const uint brcmf_sdio_msglevel;
 
 #define SDIO_FUNC_0		0
 #define SDIO_FUNC_1		1
@@ -41,46 +40,71 @@ extern const uint brcmf_sdio_msglevel;
 /* Maximum number of I/O funcs */
 #define SDIOD_MAX_IOFUNCS	7
 
-#define SBSDIO_NUM_FUNCTION		3	/* as of sdiod rev 0, supports 3 functions */
+/* as of sdiod rev 0, supports 3 functions */
+#define SBSDIO_NUM_FUNCTION		3
 
 /* function 1 miscellaneous registers */
-#define SBSDIO_SPROM_CS			0x10000	/* sprom command and status */
-#define SBSDIO_SPROM_INFO		0x10001	/* sprom info register */
-#define SBSDIO_SPROM_DATA_LOW		0x10002	/* sprom indirect access data byte 0 */
-#define SBSDIO_SPROM_DATA_HIGH		0x10003	/* sprom indirect access data byte 1 */
-#define SBSDIO_SPROM_ADDR_LOW		0x10004	/* sprom indirect access addr byte 0 */
-#define SBSDIO_SPROM_ADDR_HIGH		0x10005	/* sprom indirect access addr byte 0 */
-#define SBSDIO_CHIP_CTRL_DATA		0x10006	/* xtal_pu (gpio) output */
-#define SBSDIO_CHIP_CTRL_EN		0x10007	/* xtal_pu (gpio) enable */
-#define SBSDIO_WATERMARK		0x10008	/* rev < 7, watermark for sdio device */
-#define SBSDIO_DEVICE_CTL		0x10009	/* control busy signal generation */
-
-/* registers introduced in rev 8, some content (mask/bits) defs in sbsdpcmdev.h */
-#define SBSDIO_FUNC1_SBADDRLOW		0x1000A	/* SB Address Window Low (b15) */
-#define SBSDIO_FUNC1_SBADDRMID		0x1000B	/* SB Address Window Mid (b23:b16) */
-#define SBSDIO_FUNC1_SBADDRHIGH		0x1000C	/* SB Address Window High (b31:b24)    */
-#define SBSDIO_FUNC1_FRAMECTRL		0x1000D	/* Frame Control (frame term/abort) */
-#define SBSDIO_FUNC1_CHIPCLKCSR		0x1000E	/* ChipClockCSR (ALP/HT ctl/status) */
-#define SBSDIO_FUNC1_SDIOPULLUP		0x1000F	/* SdioPullUp (on cmd, d0-d2) */
-#define SBSDIO_FUNC1_WFRAMEBCLO		0x10019	/* Write Frame Byte Count Low */
-#define SBSDIO_FUNC1_WFRAMEBCHI		0x1001A	/* Write Frame Byte Count High */
-#define SBSDIO_FUNC1_RFRAMEBCLO		0x1001B	/* Read Frame Byte Count Low */
-#define SBSDIO_FUNC1_RFRAMEBCHI		0x1001C	/* Read Frame Byte Count High */
+
+/* sprom command and status */
+#define SBSDIO_SPROM_CS			0x10000
+/* sprom info register */
+#define SBSDIO_SPROM_INFO		0x10001
+/* sprom indirect access data byte 0 */
+#define SBSDIO_SPROM_DATA_LOW		0x10002
+/* sprom indirect access data byte 1 */
+#define SBSDIO_SPROM_DATA_HIGH		0x10003
+/* sprom indirect access addr byte 0 */
+#define SBSDIO_SPROM_ADDR_LOW		0x10004
+/* sprom indirect access addr byte 0 */
+#define SBSDIO_SPROM_ADDR_HIGH		0x10005
+/* xtal_pu (gpio) output */
+#define SBSDIO_CHIP_CTRL_DATA		0x10006
+/* xtal_pu (gpio) enable */
+#define SBSDIO_CHIP_CTRL_EN		0x10007
+/* rev < 7, watermark for sdio device */
+#define SBSDIO_WATERMARK		0x10008
+/* control busy signal generation */
+#define SBSDIO_DEVICE_CTL		0x10009
+
+/* SB Address Window Low (b15) */
+#define SBSDIO_FUNC1_SBADDRLOW		0x1000A
+/* SB Address Window Mid (b23:b16) */
+#define SBSDIO_FUNC1_SBADDRMID		0x1000B
+/* SB Address Window High (b31:b24)    */
+#define SBSDIO_FUNC1_SBADDRHIGH		0x1000C
+/* Frame Control (frame term/abort) */
+#define SBSDIO_FUNC1_FRAMECTRL		0x1000D
+/* ChipClockCSR (ALP/HT ctl/status) */
+#define SBSDIO_FUNC1_CHIPCLKCSR		0x1000E
+/* SdioPullUp (on cmd, d0-d2) */
+#define SBSDIO_FUNC1_SDIOPULLUP		0x1000F
+/* Write Frame Byte Count Low */
+#define SBSDIO_FUNC1_WFRAMEBCLO		0x10019
+/* Write Frame Byte Count High */
+#define SBSDIO_FUNC1_WFRAMEBCHI		0x1001A
+/* Read Frame Byte Count Low */
+#define SBSDIO_FUNC1_RFRAMEBCLO		0x1001B
+/* Read Frame Byte Count High */
+#define SBSDIO_FUNC1_RFRAMEBCHI		0x1001C
 
 #define SBSDIO_FUNC1_MISC_REG_START	0x10000	/* f1 misc register start */
 #define SBSDIO_FUNC1_MISC_REG_LIMIT	0x1001C	/* f1 misc register end */
 
 /* function 1 OCP space */
-#define SBSDIO_SB_OFT_ADDR_MASK		0x07FFF	/* sb offset addr is <= 15 bits, 32k */
+
+/* sb offset addr is <= 15 bits, 32k */
+#define SBSDIO_SB_OFT_ADDR_MASK		0x07FFF
 #define SBSDIO_SB_OFT_ADDR_LIMIT	0x08000
-#define SBSDIO_SB_ACCESS_2_4B_FLAG	0x08000	/* with b15, maps to 32-bit SB access */
+/* with b15, maps to 32-bit SB access */
+#define SBSDIO_SB_ACCESS_2_4B_FLAG	0x08000
 
-/* some duplication with sbsdpcmdev.h here */
 /* valid bits in SBSDIO_FUNC1_SBADDRxxx regs */
+
 #define SBSDIO_SBADDRLOW_MASK		0x80	/* Valid bits in SBADDRLOW */
 #define SBSDIO_SBADDRMID_MASK		0xff	/* Valid bits in SBADDRMID */
 #define SBSDIO_SBADDRHIGH_MASK		0xffU	/* Valid bits in SBADDRHIGH */
-#define SBSDIO_SBWINDOW_MASK		0xffff8000	/* Address bits from SBADDR regs */
+/* Address bits from SBADDR regs */
+#define SBSDIO_SBWINDOW_MASK		0xffff8000
 
 #define SDIOH_READ              0	/* Read request */
 #define SDIOH_WRITE             1	/* Write request */
@@ -92,68 +116,32 @@ extern const uint brcmf_sdio_msglevel;
 #define SUCCESS	0
 #define ERROR	1
 
-/* forward declarations */
-struct brcmf_sdio_card;
-
 struct brcmf_sdreg {
 	int func;
 	int offset;
 	int value;
 };
 
-struct sdioh_info {
-	struct osl_info *osh;		/* osh handler */
-	bool client_intr_enabled;	/* interrupt connnected flag */
-	bool intr_handler_valid;	/* client driver interrupt handler valid */
-	void (*intr_handler)(void *);	/* registered interrupt handler */
-	void *intr_handler_arg;	/* argument to call interrupt handler */
-	u16 intmask;		/* Current active interrupts */
-	void *sdos_info;	/* Pointer to per-OS private data */
-
-	uint irq;		/* Client irq */
-	int intrcount;		/* Client interrupts */
-	bool sd_blockmode;	/* sd_blockmode == false => 64 Byte Cmd 53s. */
-	/*  Must be on for sd_multiblock to be effective */
-	bool use_client_ints;	/* If this is false, make sure to restore */
-	int client_block_size[SDIOD_MAX_IOFUNCS];	/* Blocksize */
-	u8 num_funcs;	/* Supported funcs on client */
-	u32 com_cis_ptr;
+struct brcmf_sdio_dev {
+	struct sdio_func *func[SDIO_MAX_FUNCS];
+	u8 num_funcs;			/* Supported funcs on client */
 	u32 func_cis_ptr[SDIOD_MAX_IOFUNCS];
-	uint max_dma_len;
-	uint max_dma_descriptors;	/* DMA Descriptors supported by this controller. */
-	/*	SDDMA_DESCRIPTOR	SGList[32]; *//* Scatter/Gather DMA List */
-};
+	u32 sbwad;			/* Save backplane window address */
+	bool regfail;			/* status of last reg_r/w call */
+	void *bus;
+	atomic_t suspend;		/* suspend flag */
+	wait_queue_head_t request_byte_wait;
+	wait_queue_head_t request_word_wait;
+	wait_queue_head_t request_packet_wait;
+	wait_queue_head_t request_buffer_wait;
 
-struct brcmf_sdmmc_instance {
-	struct sdioh_info *sd;
-	struct sdio_func *func[SDIOD_MAX_IOFUNCS];
-	u32 host_claimed;
 };
 
-/* Attach and build an interface to the underlying SD host driver.
- *  - Allocates resources (structs, arrays, mem, OS handles, etc) needed by
- *    brcmf_sdcard.
- *  - Returns the sdio card handle and virtual address base for register access.
- *    The returned handle should be used in all subsequent calls, but the bcmsh
- *    implementation may maintain a single "default" handle (e.g. the first or
- *    most recent one) to enable single-instance implementations to pass NULL.
- */
-extern struct brcmf_sdio_card*
-brcmf_sdcard_attach(void *cfghdl, u32 *regsva, uint irq);
-
-/* Detach - freeup resources allocated in attach */
-extern int brcmf_sdcard_detach(struct brcmf_sdio_card *card);
-
-/* Enable/disable SD interrupt */
-extern int brcmf_sdcard_intr_enable(struct brcmf_sdio_card *card);
-extern int brcmf_sdcard_intr_disable(struct brcmf_sdio_card *card);
-
 /* Register/deregister device interrupt handler. */
 extern int
-brcmf_sdcard_intr_reg(struct brcmf_sdio_card *card,
-		      void (*fn)(void *), void *argh);
+brcmf_sdcard_intr_reg(struct brcmf_sdio_dev *sdiodev);
 
-extern int brcmf_sdcard_intr_dereg(struct brcmf_sdio_card *card);
+extern int brcmf_sdcard_intr_dereg(struct brcmf_sdio_dev *sdiodev);
 
 /* Access SDIO address space (e.g. CCCR) using CMD52 (single-byte interface).
  *   fn:   function number
@@ -161,44 +149,25 @@ extern int brcmf_sdcard_intr_dereg(struct brcmf_sdio_card *card);
  *   data: data byte to write
  *   err:  pointer to error code (or NULL)
  */
-extern u8 brcmf_sdcard_cfg_read(struct brcmf_sdio_card *card, uint func,
+extern u8 brcmf_sdcard_cfg_read(struct brcmf_sdio_dev *sdiodev, uint func,
 				u32 addr, int *err);
-extern void brcmf_sdcard_cfg_write(struct brcmf_sdio_card *card, uint func,
+extern void brcmf_sdcard_cfg_write(struct brcmf_sdio_dev *sdiodev, uint func,
 				   u32 addr, u8 data, int *err);
 
-/* Read/Write 4bytes from/to cfg space */
-extern u32
-brcmf_sdcard_cfg_read_word(struct brcmf_sdio_card *card, uint fnc_num,
-			   u32 addr, int *err);
-
-extern void brcmf_sdcard_cfg_write_word(struct brcmf_sdio_card *card,
-					uint fnc_num, u32 addr,
-					u32 data, int *err);
-
-/* Read CIS content for specified function.
- *   fn:     function whose CIS is being requested (0 is common CIS)
- *   cis:    pointer to memory location to place results
- *   length: number of bytes to read
- * Internally, this routine uses the values from the cis base regs (0x9-0xB)
- * to form an SDIO-space address to read the data from.
- */
-extern int brcmf_sdcard_cis_read(struct brcmf_sdio_card *card, uint func,
-				 u8 *cis, uint length);
-
 /* Synchronous access to device (client) core registers via CMD53 to F1.
  *   addr: backplane address (i.e. >= regsva from attach)
  *   size: register width in bytes (2 or 4)
  *   data: data for register write
  */
 extern u32
-brcmf_sdcard_reg_read(struct brcmf_sdio_card *card, u32 addr, uint size);
+brcmf_sdcard_reg_read(struct brcmf_sdio_dev *sdiodev, u32 addr, uint size);
 
 extern u32
-brcmf_sdcard_reg_write(struct brcmf_sdio_card *card, u32 addr, uint size,
+brcmf_sdcard_reg_write(struct brcmf_sdio_dev *sdiodev, u32 addr, uint size,
 		       u32 data);
 
 /* Indicate if last reg read/write failed */
-extern bool brcmf_sdcard_regfail(struct brcmf_sdio_card *card);
+extern bool brcmf_sdcard_regfail(struct brcmf_sdio_dev *sdiodev);
 
 /* Buffer transfer to/from device (client) core via cmd53.
  *   fn:       function number
@@ -213,25 +182,20 @@ extern bool brcmf_sdcard_regfail(struct brcmf_sdio_card *card);
  * NOTE: Async operation is not currently supported.
  */
 extern int
-brcmf_sdcard_send_buf(struct brcmf_sdio_card *card, u32 addr, uint fn,
-		      uint flags, u8 *buf, uint nbytes, void *pkt,
-		      void (*complete)(void *handle, int status,
-				       bool sync_waiting),
-		      void *handle);
+brcmf_sdcard_send_buf(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
+		      uint flags, u8 *buf, uint nbytes, struct sk_buff *pkt);
 extern int
-brcmf_sdcard_recv_buf(struct brcmf_sdio_card *card, u32 addr, uint fn,
-		      uint flags, u8 *buf, uint nbytes, struct sk_buff *pkt,
-		      void (*complete)(void *handle, int status,
-				       bool sync_waiting),
-		      void *handle);
+brcmf_sdcard_recv_buf(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
+		      uint flags, u8 *buf, uint nbytes, struct sk_buff *pkt);
 
 /* Flags bits */
-#define SDIO_REQ_4BYTE	0x1	/* Four-byte target (backplane) width (vs. two-byte) */
-#define SDIO_REQ_FIXED	0x2	/* Fixed address (FIFO) (vs. incrementing address) */
-#define SDIO_REQ_ASYNC	0x4	/* Async request (vs. sync request) */
 
-/* Pending (non-error) return code */
-#define BCME_PENDING	1
+/* Four-byte target (backplane) width (vs. two-byte) */
+#define SDIO_REQ_4BYTE	0x1
+/* Fixed address (FIFO) (vs. incrementing address) */
+#define SDIO_REQ_FIXED	0x2
+/* Async request (vs. sync request) */
+#define SDIO_REQ_ASYNC	0x4
 
 /* Read/write to memory block (F1, no FIFO) via CMD53 (sync only).
  *   rw:       read or write (0/1)
@@ -240,108 +204,49 @@ brcmf_sdcard_recv_buf(struct brcmf_sdio_card *card, u32 addr, uint fn,
  *   nbytes:   number of bytes to transfer to/from buf
  * Returns 0 or error code.
  */
-extern int brcmf_sdcard_rwdata(struct brcmf_sdio_card *card, uint rw, u32 addr,
-			       u8 *buf, uint nbytes);
+extern int brcmf_sdcard_rwdata(struct brcmf_sdio_dev *sdiodev, uint rw,
+			       u32 addr, u8 *buf, uint nbytes);
 
 /* Issue an abort to the specified function */
-extern int brcmf_sdcard_abort(struct brcmf_sdio_card *card, uint fn);
-
-/* Returns the "Device ID" of target device on the SDIO bus. */
-extern int brcmf_sdcard_query_device(struct brcmf_sdio_card *card);
-
-/* Miscellaneous knob tweaker. */
-extern int brcmf_sdcard_iovar_op(struct brcmf_sdio_card *card, const char *name,
-				 void *params, int plen, void *arg, int len,
-				 bool set);
-
-/* helper functions */
-
-/* callback functions */
-struct brcmf_sdioh_driver {
-	/* attach to device */
-	void *(*attach) (u16 vend_id, u16 dev_id, u16 bus, u16 slot,
-			 u16 func, uint bustype, u32 regsva, void *param);
-	/* detach from device */
-	void (*detach) (void *ch);
-};
-
-struct sdioh_info;
+extern int brcmf_sdcard_abort(struct brcmf_sdio_dev *sdiodev, uint fn);
 
 /* platform specific/high level functions */
-extern int brcmf_sdio_function_init(void);
-extern int brcmf_sdio_register(struct brcmf_sdioh_driver *driver);
-extern void brcmf_sdio_unregister(void);
-extern void brcmf_sdio_function_cleanup(void);
-extern int brcmf_sdio_probe(struct device *dev);
-extern int brcmf_sdio_remove(struct device *dev);
+extern int brcmf_sdio_probe(struct brcmf_sdio_dev *sdiodev);
+extern int brcmf_sdio_remove(struct brcmf_sdio_dev *sdiodev);
 
-/* Function to return current window addr */
-extern u32 brcmf_sdcard_cur_sbwad(struct brcmf_sdio_card *card);
-
-/* Allocate/init/free per-OS private data */
-extern int  brcmf_sdioh_osinit(struct sdioh_info *sd);
-extern void brcmf_sdioh_osfree(struct sdioh_info *sd);
-
-/* Core interrupt enable/disable of device interrupts */
-extern void brcmf_sdioh_dev_intr_on(struct sdioh_info *sd);
-extern void brcmf_sdioh_dev_intr_off(struct sdioh_info *sd);
+extern int brcmf_sdcard_set_sbaddr_window(struct brcmf_sdio_dev *sdiodev,
+					  u32 address);
 
 /* attach, return handler on success, NULL if failed.
  *  The handler shall be provided by all subsequent calls. No local cache
  *  cfghdl points to the starting address of pci device mapped memory
  */
-extern struct sdioh_info *brcmf_sdioh_attach(void *cfghdl, uint irq);
-extern int brcmf_sdioh_detach(struct sdioh_info *si);
-
-extern int
-brcmf_sdioh_interrupt_register(struct sdioh_info *si,
-			       void (*sdioh_cb_fn)(void *), void *argh);
-
-extern int brcmf_sdioh_interrupt_deregister(struct sdioh_info *si);
-
-/* enable or disable SD interrupt */
-extern int
-brcmf_sdioh_interrupt_set(struct sdioh_info *si, bool enable_disable);
+extern int brcmf_sdioh_attach(struct brcmf_sdio_dev *sdiodev);
+extern void brcmf_sdioh_detach(struct brcmf_sdio_dev *sdiodev);
 
 /* read or write one byte using cmd52 */
-extern int
-brcmf_sdioh_request_byte(struct sdioh_info *si, uint rw, uint fnc, uint addr,
-			 u8 *byte);
+extern int brcmf_sdioh_request_byte(struct brcmf_sdio_dev *sdiodev, uint rw,
+				    uint fnc, uint addr, u8 *byte);
 
 /* read or write 2/4 bytes using cmd53 */
 extern int
-brcmf_sdioh_request_word(struct sdioh_info *si, uint cmd_type,
+brcmf_sdioh_request_word(struct brcmf_sdio_dev *sdiodev,
 			 uint rw, uint fnc, uint addr,
 			 u32 *word, uint nbyte);
 
 /* read or write any buffer using cmd53 */
 extern int
-brcmf_sdioh_request_buffer(struct sdioh_info *si, uint pio_dma,
+brcmf_sdioh_request_buffer(struct brcmf_sdio_dev *sdiodev,
 			   uint fix_inc, uint rw, uint fnc_num,
 			   u32 addr, uint regwidth,
 			   u32 buflen, u8 *buffer, struct sk_buff *pkt);
 
-/* get cis data */
-extern int
-brcmf_sdioh_cis_read(struct sdioh_info *si, uint fuc, u8 *cis, u32 length);
-
-extern int
-brcmf_sdioh_cfg_read(struct sdioh_info *si, uint fuc, u32 addr, u8 *data);
-extern int
-brcmf_sdioh_cfg_write(struct sdioh_info *si, uint fuc, u32 addr, u8 *data);
-
-/* handle iovars */
-extern int brcmf_sdioh_iovar_op(struct sdioh_info *si, const char *name,
-			  void *params, int plen, void *arg, int len, bool set);
-
-/* Issue abort to the specified function and clear controller as needed */
-extern int brcmf_sdioh_abort(struct sdioh_info *si, uint fnc);
-
 /* Watchdog timer interface for pm ops */
-extern void brcmf_sdio_wdtmr_enable(bool enable);
-
-extern uint sd_msglevel;	/* Debug message level */
-
-extern struct brcmf_sdmmc_instance *gInstance;
+extern void brcmf_sdio_wdtmr_enable(struct brcmf_sdio_dev *sdiodev,
+				    bool enable);
 
+extern void *brcmf_sdbrcm_probe(u16 bus_no, u16 slot, u16 func, uint bustype,
+				u32 regsva, struct brcmf_sdio_dev *sdiodev);
+extern void brcmf_sdbrcm_disconnect(void *ptr);
+extern void brcmf_sdbrcm_isr(void *arg);
 #endif				/* _BRCM_SDH_H_ */
diff --git a/drivers/staging/brcm80211/brcmfmac/wl_cfg80211.c b/drivers/staging/brcm80211/brcmfmac/wl_cfg80211.c
index 821206d3e536..d626ebd05aea 100644
--- a/drivers/staging/brcm80211/brcmfmac/wl_cfg80211.c
+++ b/drivers/staging/brcm80211/brcmfmac/wl_cfg80211.c
@@ -14,19 +14,18 @@
  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+/* Toplevel file. Relies on dhd_linux.c to send commands to the dongle. */
+
 #include <linux/kernel.h>
 #include <linux/if_arp.h>
 #include <linux/sched.h>
 #include <linux/kthread.h>
 #include <linux/netdevice.h>
-#include <linux/sched.h>
+#include <linux/bitops.h>
 #include <linux/etherdevice.h>
-#include <linux/wireless.h>
 #include <linux/ieee80211.h>
-#include <linux/mmc/sdio_func.h>
 #include <linux/uaccess.h>
 #include <net/cfg80211.h>
-#include <net/rtnetlink.h>
 
 #include <brcmu_utils.h>
 #include <defs.h>
@@ -34,306 +33,50 @@
 #include "dhd.h"
 #include "wl_cfg80211.h"
 
-static struct sdio_func *cfg80211_sdio_func;
-static struct brcmf_cfg80211_dev *cfg80211_dev;
-static const u8 ether_bcast[ETH_ALEN] = {255, 255, 255, 255, 255, 255};
-
-u32 brcmf_dbg_level = WL_DBG_ERR;
+#define BRCMF_ASSOC_PARAMS_FIXED_SIZE \
+	(sizeof(struct brcmf_assoc_params_le) - sizeof(u16))
 
-/*
-** cfg80211_ops api/callback list
-*/
-static s32 brcmf_cfg80211_change_iface(struct wiphy *wiphy,
-				       struct net_device *ndev,
-				       enum nl80211_iftype type, u32 *flags,
-				       struct vif_params *params);
-static s32 __brcmf_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
-				 struct cfg80211_scan_request *request,
-				 struct cfg80211_ssid *this_ssid);
-static s32 brcmf_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
-			       struct cfg80211_scan_request *request);
-static s32 brcmf_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed);
-static s32 brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
-				    struct cfg80211_ibss_params *params);
-static s32 brcmf_cfg80211_leave_ibss(struct wiphy *wiphy,
-				     struct net_device *dev);
-static s32 brcmf_cfg80211_get_station(struct wiphy *wiphy,
-				      struct net_device *dev, u8 *mac,
-				      struct station_info *sinfo);
-static s32 brcmf_cfg80211_set_power_mgmt(struct wiphy *wiphy,
-					 struct net_device *dev, bool enabled,
-					 s32 timeout);
-static s32 brcmf_cfg80211_set_bitrate_mask(struct wiphy *wiphy,
-					   struct net_device *dev,
-					   const u8 *addr,
-					   const struct cfg80211_bitrate_mask
-					   *mask);
-static int brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
-				  struct cfg80211_connect_params *sme);
-static s32 brcmf_cfg80211_disconnect(struct wiphy *wiphy,
-				     struct net_device *dev,
-				     u16 reason_code);
-static s32 brcmf_cfg80211_set_tx_power(struct wiphy *wiphy,
-				      enum nl80211_tx_power_setting type,
-				      s32 dbm);
-static s32 brcmf_cfg80211_get_tx_power(struct wiphy *wiphy, s32 *dbm);
-static s32 brcmf_cfg80211_config_default_key(struct wiphy *wiphy,
-					  struct net_device *dev, u8 key_idx,
-					  bool unicast, bool multicast);
-static s32 brcmf_cfg80211_add_key(struct wiphy *wiphy, struct net_device *dev,
-				 u8 key_idx, bool pairwise, const u8 *mac_addr,
-				 struct key_params *params);
-static s32 brcmf_cfg80211_del_key(struct wiphy *wiphy, struct net_device *dev,
-				 u8 key_idx, bool pairwise, const u8 *mac_addr);
-static s32 brcmf_cfg80211_get_key(struct wiphy *wiphy, struct net_device *dev,
-				 u8 key_idx, bool pairwise, const u8 *mac_addr,
-				 void *cookie, void (*callback) (void *cookie,
-								 struct
-								 key_params *
-								 params));
-static s32 brcmf_cfg80211_config_default_mgmt_key(struct wiphy *wiphy,
-						 struct net_device *dev,
-						 u8 key_idx);
-static s32 brcmf_cfg80211_resume(struct wiphy *wiphy);
-static s32 brcmf_cfg80211_suspend(struct wiphy *wiphy,
-				 struct cfg80211_wowlan *wow);
-static s32 brcmf_cfg80211_set_pmksa(struct wiphy *wiphy, struct net_device *dev,
-				   struct cfg80211_pmksa *pmksa);
-static s32 brcmf_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *dev,
-				   struct cfg80211_pmksa *pmksa);
-static s32 brcmf_cfg80211_flush_pmksa(struct wiphy *wiphy,
-				     struct net_device *dev);
-/*
-** event & event Q handlers for cfg80211 interfaces
-*/
-static s32 brcmf_create_event_handler(struct brcmf_cfg80211_priv *cfg_priv);
-static void brcmf_destroy_event_handler(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 brcmf_event_handler(void *data);
-static void brcmf_init_eq(struct brcmf_cfg80211_priv *cfg_priv);
-static void brcmf_flush_eq(struct brcmf_cfg80211_priv *cfg_priv);
-static void brcmf_lock_eq(struct brcmf_cfg80211_priv *cfg_priv);
-static void brcmf_unlock_eq(struct brcmf_cfg80211_priv *cfg_priv);
-static void brcmf_init_eq_lock(struct brcmf_cfg80211_priv *cfg_priv);
-static void brcmf_init_eloop_handler(struct brcmf_cfg80211_event_loop *el);
-static struct brcmf_cfg80211_event_q *
-brcmf_deq_event(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 brcmf_enq_event(struct brcmf_cfg80211_priv *cfg_priv, u32 type,
-			  const struct brcmf_event_msg *msg, void *data);
-static void brcmf_put_event(struct brcmf_cfg80211_event_q *e);
-static void brcmf_wakeup_event(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 brcmf_notify_connect_status(struct brcmf_cfg80211_priv *cfg_priv,
-				       struct net_device *ndev,
-				       const struct brcmf_event_msg *e,
-				       void *data);
-static s32 brcmf_notify_roaming_status(struct brcmf_cfg80211_priv *cfg_priv,
-				       struct net_device *ndev,
-				       const struct brcmf_event_msg *e,
-				       void *data);
-static s32 brcmf_notify_scan_status(struct brcmf_cfg80211_priv *cfg_priv,
-				    struct net_device *ndev,
-				    const struct brcmf_event_msg *e,
-				    void *data);
-static s32 brcmf_bss_connect_done(struct brcmf_cfg80211_priv *cfg_priv,
-				  struct net_device *ndev,
-				  const struct brcmf_event_msg *e, void *data,
-				  bool completed);
-static s32 brcmf_bss_roaming_done(struct brcmf_cfg80211_priv *cfg_priv,
-				  struct net_device *ndev,
-				  const struct brcmf_event_msg *e, void *data);
-static s32 brcmf_notify_mic_status(struct brcmf_cfg80211_priv *cfg_priv,
-				   struct net_device *ndev,
-				   const struct brcmf_event_msg *e, void *data);
-
-/*
-** register/deregister sdio function
-*/
-static void brcmf_clear_sdio_func(void);
-
-/*
-** ioctl utilites
-*/
-static s32 brcmf_dev_bufvar_get(struct net_device *dev, s8 *name, s8 *buf,
-			       s32 buf_len);
-static __used s32 brcmf_dev_bufvar_set(struct net_device *dev, s8 *name,
-				      s8 *buf, s32 len);
-static s32 brcmf_dev_intvar_set(struct net_device *dev, s8 *name, s32 val);
-static s32 brcmf_dev_intvar_get(struct net_device *dev, s8 *name,
-			       s32 *retval);
-static s32 brcmf_dev_ioctl(struct net_device *dev, u32 cmd, void *arg,
-			  u32 len);
-
-/*
-** cfg80211 set_wiphy_params utilities
-*/
-static s32 brcmf_set_frag(struct net_device *dev, u32 frag_threshold);
-static s32 brcmf_set_rts(struct net_device *dev, u32 frag_threshold);
-static s32 brcmf_set_retry(struct net_device *dev, u32 retry, bool l);
-
-/*
-** wl profile utilities
-*/
-static s32 brcmf_update_prof(struct brcmf_cfg80211_priv *cfg_priv,
-			     const struct brcmf_event_msg *e,
-			     void *data, s32 item);
-static void *brcmf_read_prof(struct brcmf_cfg80211_priv *cfg_priv, s32 item);
-static void brcmf_init_prof(struct brcmf_cfg80211_profile *prof);
-
-/*
-** cfg80211 connect utilites
-*/
-static s32 brcmf_set_wpa_version(struct net_device *dev,
-			struct cfg80211_connect_params *sme);
-static s32 brcmf_set_auth_type(struct net_device *dev,
-			struct cfg80211_connect_params *sme);
-static s32 brcmf_set_set_cipher(struct net_device *dev,
-			struct cfg80211_connect_params *sme);
-static s32 brcmf_set_key_mgmt(struct net_device *dev,
-			struct cfg80211_connect_params *sme);
-static s32 brcmf_set_set_sharedkey(struct net_device *dev,
-			struct cfg80211_connect_params *sme);
-static s32 brcmf_get_assoc_ies(struct brcmf_cfg80211_priv *cfg_priv);
-static void brcmf_clear_assoc_ies(struct brcmf_cfg80211_priv *cfg_priv);
-static void brcmf_ch_to_chanspec(int ch,
-	struct brcmf_join_params *join_params, size_t *join_params_size);
-
-/*
-** information element utilities
-*/
-static __used s32 brcmf_add_ie(struct brcmf_cfg80211_priv *cfg_priv,
-			       u8 t, u8 l, u8 *v);
-static s32 brcmf_mode_to_nl80211_iftype(s32 mode);
-static struct wireless_dev *brcmf_alloc_wdev(s32 sizeof_iface,
-			struct device *dev);
-static void brcmf_free_wdev(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 brcmf_inform_bss(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 brcmf_inform_single_bss(struct brcmf_cfg80211_priv *cfg_priv,
-				   struct brcmf_bss_info *bi);
-static s32 brcmf_update_bss_info(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 brcmf_add_keyext(struct wiphy *wiphy, struct net_device *dev,
-			u8 key_idx, const u8 *mac_addr,
-			struct key_params *params);
-
-/*
-** key indianess swap utilities
-*/
-static void swap_key_from_BE(struct brcmf_wsec_key *key);
-static void swap_key_to_BE(struct brcmf_wsec_key *key);
-
-/*
-** brcmf_cfg80211_priv memory init/deinit utilities
-*/
-static s32 brcmf_init_priv_mem(struct brcmf_cfg80211_priv *cfg_priv);
-static void brcmf_deinit_priv_mem(struct brcmf_cfg80211_priv *cfg_priv);
-
-static void brcmf_delay(u32 ms);
+static const u8 ether_bcast[ETH_ALEN] = {255, 255, 255, 255, 255, 255};
 
-/*
-** store/restore cfg80211 instance data
-*/
-static void brcmf_set_drvdata(struct brcmf_cfg80211_dev *dev, void *data);
-static void *brcmf_get_drvdata(struct brcmf_cfg80211_dev *dev);
+static u32 brcmf_dbg_level = WL_DBG_ERR;
 
-/*
-** ibss mode utilities
-*/
 static bool brcmf_is_ibssmode(struct brcmf_cfg80211_priv *cfg_priv);
+static s32
+brcmf_bss_connect_done(struct brcmf_cfg80211_priv *cfg_priv,
+		       struct net_device *ndev, const struct brcmf_event_msg *e,
+		       bool completed);
 
-/*
-** dongle up/down , default configuration utilities
-*/
-static bool brcmf_is_linkdown(struct brcmf_cfg80211_priv *cfg_priv,
-			      const struct brcmf_event_msg *e);
-static bool brcmf_is_linkup(struct brcmf_cfg80211_priv *cfg_priv,
-			    const struct brcmf_event_msg *e);
-static bool brcmf_is_nonetwork(struct brcmf_cfg80211_priv *cfg_priv,
-			       const struct brcmf_event_msg *e);
-static void brcmf_link_down(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 brcmf_dongle_mode(struct net_device *ndev, s32 iftype);
-static s32 __brcmf_cfg80211_up(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 __brcmf_cfg80211_down(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 brcmf_dongle_probecap(struct brcmf_cfg80211_priv *cfg_priv);
-static void brcmf_init_conf(struct brcmf_cfg80211_conf *conf);
-
-/*
-** dongle configuration utilities
-*/
-static s32 brcmf_dongle_eventmsg(struct net_device *ndev);
-static s32 brcmf_dongle_scantime(struct net_device *ndev, s32 scan_assoc_time,
-				 s32 scan_unassoc_time, s32 scan_passive_time);
-static s32 brcmf_config_dongle(struct brcmf_cfg80211_priv *cfg_priv,
-			       bool need_lock);
-static s32 brcmf_dongle_roam(struct net_device *ndev, u32 roamvar,
-			    u32 bcn_timeout);
-
-/*
-** iscan handler
-*/
-static void brcmf_iscan_timer(unsigned long data);
-static void brcmf_term_iscan(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 brcmf_init_iscan(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 brcmf_iscan_thread(void *data);
-static s32 brcmf_dev_iovar_setbuf(struct net_device *dev, s8 *iovar,
-				 void *param, s32 paramlen, void *bufptr,
-				 s32 buflen);
-static s32 brcmf_dev_iovar_getbuf(struct net_device *dev, s8 *iovar,
-				 void *param, s32 paramlen, void *bufptr,
-				 s32 buflen);
-static s32 brcmf_run_iscan(struct brcmf_cfg80211_iscan_ctrl *iscan,
-			   struct brcmf_ssid *ssid, u16 action);
-static s32 brcmf_do_iscan(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 brcmf_wakeup_iscan(struct brcmf_cfg80211_iscan_ctrl *iscan);
-static s32 brcmf_invoke_iscan(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 brcmf_get_iscan_results(struct brcmf_cfg80211_iscan_ctrl *iscan,
-				   u32 *status,
-				   struct brcmf_scan_results **bss_list);
-static void brcmf_notify_iscan_complete(struct brcmf_cfg80211_iscan_ctrl *iscan,
-					bool aborted);
-static void brcmf_init_iscan_eloop(struct brcmf_cfg80211_iscan_eloop *el);
-static s32 brcmf_iscan_done(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 brcmf_iscan_pending(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 brcmf_iscan_inprogress(struct brcmf_cfg80211_priv *cfg_priv);
-static s32 brcmf_iscan_aborted(struct brcmf_cfg80211_priv *cfg_priv);
+static void brcmf_set_drvdata(struct brcmf_cfg80211_dev *dev, void *data)
+{
+	dev->driver_data = data;
+}
 
-/*
-* find most significant bit set
-*/
-static __used u32 brcmf_find_msb(u16 bit16);
+static void *brcmf_get_drvdata(struct brcmf_cfg80211_dev *dev)
+{
+	void *data = NULL;
 
-/*
-* update pmklist to dongle
-*/
-static __used s32 brcmf_update_pmklist(struct net_device *dev,
-				       struct brcmf_cfg80211_pmk_list *pmk_list,
-				       s32 err);
+	if (dev)
+		data = dev->driver_data;
+	return data;
+}
 
-static void brcmf_set_mpc(struct net_device *ndev, int mpc);
+static
+struct brcmf_cfg80211_priv *brcmf_priv_get(struct brcmf_cfg80211_dev *cfg_dev)
+{
+	struct brcmf_cfg80211_iface *ci = brcmf_get_drvdata(cfg_dev);
+	return ci->cfg_priv;
+}
 
-/*
-* debufs support
-*/
-static int
-brcmf_debugfs_add_netdev_params(struct brcmf_cfg80211_priv *cfg_priv);
-static void brcmf_debugfs_remove_netdev(struct brcmf_cfg80211_priv *cfg_priv);
-
-#define WL_PRIV_GET()							\
-	({								\
-	struct brcmf_cfg80211_iface *ci = brcmf_get_drvdata(cfg80211_dev); \
-	if (unlikely(!ci)) {						\
-		WL_ERR("wl_cfg80211_dev is unavailable\n");		\
-		BUG();							\
-	}								\
-	ci->cfg_priv;							\
-})
-
-#define CHECK_SYS_UP()							\
-do {									\
-	struct brcmf_cfg80211_priv *cfg_priv = wiphy_to_cfg(wiphy);	\
-	if (unlikely(!test_bit(WL_STATUS_READY, &cfg_priv->status))) {	\
-		WL_INFO("device is not ready : status (%d)\n",		\
-			(int)cfg_priv->status);				\
-		return -EIO;						\
-	}								\
-} while (0)
+static bool check_sys_up(struct wiphy *wiphy)
+{
+	struct brcmf_cfg80211_priv *cfg_priv = wiphy_to_cfg(wiphy);
+	if (!test_bit(WL_STATUS_READY, &cfg_priv->status)) {
+		WL_INFO("device is not ready : status (%d)\n",
+			(int)cfg_priv->status);
+		return false;
+	}
+	return true;
+}
 
 #define CHAN2G(_channel, _freq, _flags) {			\
 	.band			= IEEE80211_BAND_2GHZ,		\
@@ -510,48 +253,134 @@ static const u32 __wl_cipher_suites[] = {
 	WLAN_CIPHER_SUITE_AES_CMAC,
 };
 
-static void swap_key_from_BE(struct brcmf_wsec_key *key)
+/* tag_ID/length/value_buffer tuple */
+struct brcmf_tlv {
+	u8 id;
+	u8 len;
+	u8 data[1];
+};
+
+/* Quarter dBm units to mW
+ * Table starts at QDBM_OFFSET, so the first entry is mW for qdBm=153
+ * Table is offset so the last entry is largest mW value that fits in
+ * a u16.
+ */
+
+#define QDBM_OFFSET 153		/* Offset for first entry */
+#define QDBM_TABLE_LEN 40	/* Table size */
+
+/* Smallest mW value that will round up to the first table entry, QDBM_OFFSET.
+ * Value is ( mW(QDBM_OFFSET - 1) + mW(QDBM_OFFSET) ) / 2
+ */
+#define QDBM_TABLE_LOW_BOUND 6493	/* Low bound */
+
+/* Largest mW value that will round down to the last table entry,
+ * QDBM_OFFSET + QDBM_TABLE_LEN-1.
+ * Value is ( mW(QDBM_OFFSET + QDBM_TABLE_LEN - 1) +
+ * mW(QDBM_OFFSET + QDBM_TABLE_LEN) ) / 2.
+ */
+#define QDBM_TABLE_HIGH_BOUND 64938	/* High bound */
+
+static const u16 nqdBm_to_mW_map[QDBM_TABLE_LEN] = {
+/* qdBm:	+0	+1	+2	+3	+4	+5	+6	+7 */
+/* 153: */ 6683, 7079, 7499, 7943, 8414, 8913, 9441, 10000,
+/* 161: */ 10593, 11220, 11885, 12589, 13335, 14125, 14962, 15849,
+/* 169: */ 16788, 17783, 18836, 19953, 21135, 22387, 23714, 25119,
+/* 177: */ 26607, 28184, 29854, 31623, 33497, 35481, 37584, 39811,
+/* 185: */ 42170, 44668, 47315, 50119, 53088, 56234, 59566, 63096
+};
+
+static u16 brcmf_qdbm_to_mw(u8 qdbm)
 {
-	key->index = cpu_to_le32(key->index);
-	key->len = cpu_to_le32(key->len);
-	key->algo = cpu_to_le32(key->algo);
-	key->flags = cpu_to_le32(key->flags);
-	key->rxiv.hi = cpu_to_le32(key->rxiv.hi);
-	key->rxiv.lo = cpu_to_le16(key->rxiv.lo);
-	key->iv_initialized = cpu_to_le32(key->iv_initialized);
+	uint factor = 1;
+	int idx = qdbm - QDBM_OFFSET;
+
+	if (idx >= QDBM_TABLE_LEN)
+		/* clamp to max u16 mW value */
+		return 0xFFFF;
+
+	/* scale the qdBm index up to the range of the table 0-40
+	 * where an offset of 40 qdBm equals a factor of 10 mW.
+	 */
+	while (idx < 0) {
+		idx += 40;
+		factor *= 10;
+	}
+
+	/* return the mW value scaled down to the correct factor of 10,
+	 * adding in factor/2 to get proper rounding.
+	 */
+	return (nqdBm_to_mW_map[idx] + factor / 2) / factor;
 }
 
-static void swap_key_to_BE(struct brcmf_wsec_key *key)
+static u8 brcmf_mw_to_qdbm(u16 mw)
 {
-	key->index = le32_to_cpu(key->index);
-	key->len = le32_to_cpu(key->len);
-	key->algo = le32_to_cpu(key->algo);
-	key->flags = le32_to_cpu(key->flags);
-	key->rxiv.hi = le32_to_cpu(key->rxiv.hi);
-	key->rxiv.lo = le16_to_cpu(key->rxiv.lo);
-	key->iv_initialized = le32_to_cpu(key->iv_initialized);
+	u8 qdbm;
+	int offset;
+	uint mw_uint = mw;
+	uint boundary;
+
+	/* handle boundary case */
+	if (mw_uint <= 1)
+		return 0;
+
+	offset = QDBM_OFFSET;
+
+	/* move mw into the range of the table */
+	while (mw_uint < QDBM_TABLE_LOW_BOUND) {
+		mw_uint *= 10;
+		offset -= 40;
+	}
+
+	for (qdbm = 0; qdbm < QDBM_TABLE_LEN - 1; qdbm++) {
+		boundary = nqdBm_to_mW_map[qdbm] + (nqdBm_to_mW_map[qdbm + 1] -
+						    nqdBm_to_mW_map[qdbm]) / 2;
+		if (mw_uint < boundary)
+			break;
+	}
+
+	qdbm += (u8) offset;
+
+	return qdbm;
 }
 
-static s32
-brcmf_dev_ioctl(struct net_device *dev, u32 cmd, void *arg, u32 len)
+/* 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)
 {
-	struct ifreq ifr;
-	struct brcmf_ioctl ioc;
-	mm_segment_t fs;
-	s32 err = 0;
+	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;
+}
 
-	memset(&ioc, 0, sizeof(ioc));
-	ioc.cmd = cmd;
-	ioc.buf = arg;
-	ioc.len = len;
-	strcpy(ifr.ifr_name, dev->name);
-	ifr.ifr_data = (caddr_t)&ioc;
+static void convert_key_from_CPU(struct brcmf_wsec_key *key,
+				 struct brcmf_wsec_key_le *key_le)
+{
+	key_le->index = cpu_to_le32(key->index);
+	key_le->len = cpu_to_le32(key->len);
+	key_le->algo = cpu_to_le32(key->algo);
+	key_le->flags = cpu_to_le32(key->flags);
+	key_le->rxiv.hi = cpu_to_le32(key->rxiv.hi);
+	key_le->rxiv.lo = cpu_to_le16(key->rxiv.lo);
+	key_le->iv_initialized = cpu_to_le32(key->iv_initialized);
+	memcpy(key_le->data, key->data, sizeof(key->data));
+	memcpy(key_le->ea, key->ea, sizeof(key->ea));
+}
 
-	fs = get_fs();
-	set_fs(get_ds());
-	err = dev->netdev_ops->ndo_do_ioctl(dev, &ifr, SIOCDEVPRIVATE);
-	set_fs(fs);
+static int 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_exec_dcmd(ndev, BRCMF_C_SET_KEY, &key_le, sizeof(key_le));
+	if (err)
+		WL_ERR("WLC_SET_KEY error (%d)\n", err);
 	return err;
 }
 
@@ -566,7 +395,8 @@ brcmf_cfg80211_change_iface(struct wiphy *wiphy, struct net_device *ndev,
 	s32 err = 0;
 
 	WL_TRACE("Enter\n");
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
 	switch (type) {
 	case NL80211_IFTYPE_MONITOR:
@@ -587,9 +417,8 @@ brcmf_cfg80211_change_iface(struct wiphy *wiphy, struct net_device *ndev,
 		goto done;
 	}
 
-	infra = cpu_to_le32(infra);
-	err = brcmf_dev_ioctl(ndev, BRCMF_C_SET_INFRA, &infra, sizeof(infra));
-	if (unlikely(err)) {
+	err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_INFRA, &infra);
+	if (err) {
 		WL_ERR("WLC_SET_INFRA error (%d)\n", err);
 		err = -EAGAIN;
 	} else {
@@ -606,83 +435,134 @@ done:
 	return err;
 }
 
-static void wl_iscan_prep(struct brcmf_scan_params *params,
+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 void brcmf_set_mpc(struct net_device *ndev, int mpc)
+{
+	s32 err = 0;
+	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(ndev);
+
+	if (test_bit(WL_STATUS_READY, &cfg_priv->status)) {
+		err = brcmf_dev_intvar_set(ndev, "mpc", mpc);
+		if (err) {
+			WL_ERR("fail to set mpc\n");
+			return;
+		}
+		WL_INFO("MPC : %d\n", mpc);
+	}
+}
+
+static void wl_iscan_prep(struct brcmf_scan_params_le *params_le,
 			  struct brcmf_ssid *ssid)
 {
-	memcpy(params->bssid, ether_bcast, ETH_ALEN);
-	params->bss_type = DOT11_BSSTYPE_ANY;
-	params->scan_type = 0;
-	params->nprobes = -1;
-	params->active_time = -1;
-	params->passive_time = -1;
-	params->home_time = -1;
-	params->channel_num = 0;
-
-	params->nprobes = cpu_to_le32(params->nprobes);
-	params->active_time = cpu_to_le32(params->active_time);
-	params->passive_time = cpu_to_le32(params->passive_time);
-	params->home_time = cpu_to_le32(params->home_time);
+	memcpy(params_le->bssid, ether_bcast, ETH_ALEN);
+	params_le->bss_type = DOT11_BSSTYPE_ANY;
+	params_le->scan_type = 0;
+	params_le->channel_num = 0;
+	params_le->nprobes = cpu_to_le32(-1);
+	params_le->active_time = cpu_to_le32(-1);
+	params_le->passive_time = cpu_to_le32(-1);
+	params_le->home_time = cpu_to_le32(-1);
 	if (ssid && ssid->SSID_len)
-		memcpy(&params->ssid, ssid, sizeof(struct brcmf_ssid));
-
+		memcpy(&params_le->ssid_le, ssid, sizeof(struct brcmf_ssid));
 }
 
 static s32
-brcmf_dev_iovar_setbuf(struct net_device *dev, s8 * iovar, void *param,
+brcmf_dev_iovar_setbuf(struct net_device *ndev, s8 * iovar, void *param,
 		    s32 paramlen, void *bufptr, s32 buflen)
 {
 	s32 iolen;
 
-	iolen = brcmu_mkiovar(iovar, param, paramlen, bufptr, buflen);
+	iolen = brcmf_c_mkiovar(iovar, param, paramlen, bufptr, buflen);
 	BUG_ON(!iolen);
 
-	return brcmf_dev_ioctl(dev, BRCMF_C_SET_VAR, bufptr, iolen);
+	return brcmf_exec_dcmd(ndev, BRCMF_C_SET_VAR, bufptr, iolen);
 }
 
 static s32
-brcmf_dev_iovar_getbuf(struct net_device *dev, s8 * iovar, void *param,
+brcmf_dev_iovar_getbuf(struct net_device *ndev, s8 * iovar, void *param,
 		    s32 paramlen, void *bufptr, s32 buflen)
 {
 	s32 iolen;
 
-	iolen = brcmu_mkiovar(iovar, param, paramlen, bufptr, buflen);
+	iolen = brcmf_c_mkiovar(iovar, param, paramlen, bufptr, buflen);
 	BUG_ON(!iolen);
 
-	return brcmf_dev_ioctl(dev, BRCMF_C_GET_VAR, bufptr, buflen);
+	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)
 {
-	s32 params_size = (BRCMF_SCAN_PARAMS_FIXED_SIZE +
-				offsetof(struct brcmf_iscan_params, params));
-	struct brcmf_iscan_params *params;
+	s32 params_size = BRCMF_SCAN_PARAMS_FIXED_SIZE +
+			  offsetof(struct brcmf_iscan_params_le, params_le);
+	struct brcmf_iscan_params_le *params;
 	s32 err = 0;
 
 	if (ssid && ssid->SSID_len)
 		params_size += sizeof(struct brcmf_ssid);
 	params = kzalloc(params_size, GFP_KERNEL);
-	if (unlikely(!params))
+	if (!params)
 		return -ENOMEM;
-	BUG_ON(params_size >= BRCMF_C_IOCTL_SMLEN);
+	BUG_ON(params_size >= BRCMF_DCMD_SMLEN);
 
-	wl_iscan_prep(&params->params, ssid);
+	wl_iscan_prep(&params->params_le, ssid);
 
 	params->version = cpu_to_le32(BRCMF_ISCAN_REQ_VERSION);
 	params->action = cpu_to_le16(action);
 	params->scan_duration = cpu_to_le16(0);
 
-	/* params_size += offsetof(struct brcmf_iscan_params, params); */
-	err = brcmf_dev_iovar_setbuf(iscan->dev, "iscan", params, params_size,
-				iscan->ioctl_buf, BRCMF_C_IOCTL_SMLEN);
-	if (unlikely(err)) {
-		if (err == -EBUSY) {
+	err = brcmf_dev_iovar_setbuf(iscan->ndev, "iscan", params, params_size,
+				     iscan->dcmd_buf, BRCMF_DCMD_SMLEN);
+	if (err) {
+		if (err == -EBUSY)
 			WL_INFO("system busy : iscan canceled\n");
-		} else {
+		else
 			WL_ERR("error (%d)\n", err);
-		}
 	}
+
 	kfree(params);
 	return err;
 }
@@ -692,7 +572,7 @@ static s32 brcmf_do_iscan(struct brcmf_cfg80211_priv *cfg_priv)
 	struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_to_iscan(cfg_priv);
 	struct net_device *ndev = cfg_to_ndev(cfg_priv);
 	struct brcmf_ssid ssid;
-	s32 passive_scan;
+	__le32 passive_scan;
 	s32 err = 0;
 
 	/* Broadcast scan by default */
@@ -700,19 +580,23 @@ static s32 brcmf_do_iscan(struct brcmf_cfg80211_priv *cfg_priv)
 
 	iscan->state = WL_ISCAN_STATE_SCANING;
 
-	passive_scan = cfg_priv->active_scan ? 0 : 1;
-	err = brcmf_dev_ioctl(cfg_to_ndev(cfg_priv), BRCMF_C_SET_PASSIVE_SCAN,
+	passive_scan = cfg_priv->active_scan ? 0 : cpu_to_le32(1);
+	err = brcmf_exec_dcmd(cfg_to_ndev(cfg_priv), BRCMF_C_SET_PASSIVE_SCAN,
 			&passive_scan, sizeof(passive_scan));
-	if (unlikely(err)) {
+	if (err) {
 		WL_ERR("error (%d)\n", err);
 		return err;
 	}
 	brcmf_set_mpc(ndev, 0);
 	cfg_priv->iscan_kickstart = true;
-	brcmf_run_iscan(iscan, &ssid, BRCMF_SCAN_ACTION_START);
+	err = brcmf_run_iscan(iscan, &ssid, BRCMF_SCAN_ACTION_START);
+	if (err) {
+		brcmf_set_mpc(ndev, 1);
+		cfg_priv->iscan_kickstart = false;
+		return err;
+	}
 	mod_timer(&iscan->timer, jiffies + iscan->timer_ms * HZ / 1000);
 	iscan->timer_on = 1;
-
 	return err;
 }
 
@@ -724,16 +608,17 @@ __brcmf_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(ndev);
 	struct cfg80211_ssid *ssids;
 	struct brcmf_cfg80211_scan_req *sr = cfg_priv->scan_req_int;
-	s32 passive_scan;
+	__le32 passive_scan;
 	bool iscan_req;
 	bool spec_scan;
 	s32 err = 0;
+	u32 SSID_len;
 
-	if (unlikely(test_bit(WL_STATUS_SCANNING, &cfg_priv->status))) {
+	if (test_bit(WL_STATUS_SCANNING, &cfg_priv->status)) {
 		WL_ERR("Scanning already : status (%lu)\n", cfg_priv->status);
 		return -EAGAIN;
 	}
-	if (unlikely(test_bit(WL_STATUS_SCAN_ABORTING, &cfg_priv->status))) {
+	if (test_bit(WL_STATUS_SCAN_ABORTING, &cfg_priv->status)) {
 		WL_ERR("Scanning being aborted : status (%lu)\n",
 		       cfg_priv->status);
 		return -EAGAIN;
@@ -761,41 +646,41 @@ __brcmf_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 	set_bit(WL_STATUS_SCANNING, &cfg_priv->status);
 	if (iscan_req) {
 		err = brcmf_do_iscan(cfg_priv);
-		if (likely(!err))
+		if (!err)
 			return err;
 		else
 			goto scan_out;
 	} else {
 		WL_SCAN("ssid \"%s\", ssid_len (%d)\n",
 		       ssids->ssid, ssids->ssid_len);
-		memset(&sr->ssid, 0, sizeof(sr->ssid));
-		sr->ssid.SSID_len =
-			    min_t(u8, sizeof(sr->ssid.SSID), ssids->ssid_len);
-		if (sr->ssid.SSID_len) {
-			memcpy(sr->ssid.SSID, ssids->ssid, sr->ssid.SSID_len);
-			sr->ssid.SSID_len = cpu_to_le32(sr->ssid.SSID_len);
+		memset(&sr->ssid_le, 0, sizeof(sr->ssid_le));
+		SSID_len = min_t(u8, sizeof(sr->ssid_le.SSID), ssids->ssid_len);
+		sr->ssid_le.SSID_len = cpu_to_le32(0);
+		if (SSID_len) {
+			memcpy(sr->ssid_le.SSID, ssids->ssid, SSID_len);
+			sr->ssid_le.SSID_len = cpu_to_le32(SSID_len);
 			spec_scan = true;
 		} else {
 			WL_SCAN("Broadcast scan\n");
 		}
 
-		passive_scan = cfg_priv->active_scan ? 0 : 1;
-		err = brcmf_dev_ioctl(ndev, BRCMF_C_SET_PASSIVE_SCAN,
+		passive_scan = cfg_priv->active_scan ? 0 : cpu_to_le32(1);
+		err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_PASSIVE_SCAN,
 				&passive_scan, sizeof(passive_scan));
-		if (unlikely(err)) {
+		if (err) {
 			WL_ERR("WLC_SET_PASSIVE_SCAN error (%d)\n", err);
 			goto scan_out;
 		}
 		brcmf_set_mpc(ndev, 0);
-		err = brcmf_dev_ioctl(ndev, BRCMF_C_SCAN, &sr->ssid,
-				sizeof(sr->ssid));
+		err = brcmf_exec_dcmd(ndev, BRCMF_C_SCAN, &sr->ssid_le,
+				      sizeof(sr->ssid_le));
 		if (err) {
-			if (err == -EBUSY) {
-				WL_INFO("system busy : scan for \"%s\" canceled\n",
-					sr->ssid.SSID);
-			} else {
+			if (err == -EBUSY)
+				WL_INFO("system busy : scan for \"%s\" "
+					"canceled\n", sr->ssid_le.SSID);
+			else
 				WL_ERR("WLC_SCAN error (%d)\n", err);
-			}
+
 			brcmf_set_mpc(ndev, 1);
 			goto scan_out;
 		}
@@ -817,88 +702,46 @@ brcmf_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 
 	WL_TRACE("Enter\n");
 
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
 	err = __brcmf_cfg80211_scan(wiphy, ndev, request, NULL);
-	if (unlikely(err))
+	if (err)
 		WL_ERR("scan error (%d)\n", err);
 
 	WL_TRACE("Exit\n");
 	return err;
 }
 
-static s32 brcmf_dev_intvar_set(struct net_device *dev, s8 *name, s32 val)
+static s32 brcmf_set_rts(struct net_device *ndev, u32 rts_threshold)
 {
-	s8 buf[BRCMF_C_IOCTL_SMLEN];
-	u32 len;
 	s32 err = 0;
 
-	val = cpu_to_le32(val);
-	len = brcmu_mkiovar(name, (char *)(&val), sizeof(val), buf,
-			    sizeof(buf));
-	BUG_ON(!len);
-
-	err = brcmf_dev_ioctl(dev, BRCMF_C_SET_VAR, buf, len);
-	if (unlikely(err))
-		WL_ERR("error (%d)\n", err);
-
-	return err;
-}
-
-static s32
-brcmf_dev_intvar_get(struct net_device *dev, s8 *name, s32 *retval)
-{
-	union {
-		s8 buf[BRCMF_C_IOCTL_SMLEN];
-		s32 val;
-	} var;
-	u32 len;
-	u32 data_null;
-	s32 err = 0;
-
-	len =
-	    brcmu_mkiovar(name, (char *)(&data_null), 0, (char *)(&var),
-			sizeof(var.buf));
-	BUG_ON(!len);
-	err = brcmf_dev_ioctl(dev, BRCMF_C_GET_VAR, &var, len);
-	if (unlikely(err))
-		WL_ERR("error (%d)\n", err);
-
-	*retval = le32_to_cpu(var.val);
-
-	return err;
-}
-
-static s32 brcmf_set_rts(struct net_device *dev, u32 rts_threshold)
-{
-	s32 err = 0;
-
-	err = brcmf_dev_intvar_set(dev, "rtsthresh", rts_threshold);
-	if (unlikely(err))
+	err = brcmf_dev_intvar_set(ndev, "rtsthresh", rts_threshold);
+	if (err)
 		WL_ERR("Error (%d)\n", err);
 
 	return err;
 }
 
-static s32 brcmf_set_frag(struct net_device *dev, u32 frag_threshold)
+static s32 brcmf_set_frag(struct net_device *ndev, u32 frag_threshold)
 {
 	s32 err = 0;
 
-	err = brcmf_dev_intvar_set(dev, "fragthresh", frag_threshold);
-	if (unlikely(err))
+	err = brcmf_dev_intvar_set(ndev, "fragthresh", frag_threshold);
+	if (err)
 		WL_ERR("Error (%d)\n", err);
 
 	return err;
 }
 
-static s32 brcmf_set_retry(struct net_device *dev, u32 retry, bool l)
+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);
 
-	retry = cpu_to_le32(retry);
-	err = brcmf_dev_ioctl(dev, cmd, &retry, sizeof(retry));
-	if (unlikely(err)) {
+	err = brcmf_exec_dcmd_u32(ndev, cmd, &retry);
+	if (err) {
 		WL_ERR("cmd (%d) , error (%d)\n", cmd, err);
 		return err;
 	}
@@ -912,7 +755,8 @@ static s32 brcmf_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed)
 	s32 err = 0;
 
 	WL_TRACE("Enter\n");
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
 	if (changed & WIPHY_PARAM_RTS_THRESHOLD &&
 	    (cfg_priv->conf->rts_threshold != wiphy->rts_threshold)) {
@@ -948,8 +792,125 @@ done:
 	return err;
 }
 
+static void *brcmf_read_prof(struct brcmf_cfg80211_priv *cfg_priv, s32 item)
+{
+	switch (item) {
+	case WL_PROF_SEC:
+		return &cfg_priv->profile->sec;
+	case WL_PROF_BSSID:
+		return &cfg_priv->profile->bssid;
+	case WL_PROF_SSID:
+		return &cfg_priv->profile->ssid;
+	}
+	WL_ERR("invalid item (%d)\n", item);
+	return NULL;
+}
+
+static s32
+brcmf_update_prof(struct brcmf_cfg80211_priv *cfg_priv,
+		  const struct brcmf_event_msg *e, void *data, s32 item)
+{
+	s32 err = 0;
+	struct brcmf_ssid *ssid;
+
+	switch (item) {
+	case WL_PROF_SSID:
+		ssid = (struct brcmf_ssid *) data;
+		memset(cfg_priv->profile->ssid.SSID, 0,
+		       sizeof(cfg_priv->profile->ssid.SSID));
+		memcpy(cfg_priv->profile->ssid.SSID,
+		       ssid->SSID, ssid->SSID_len);
+		cfg_priv->profile->ssid.SSID_len = ssid->SSID_len;
+		break;
+	case WL_PROF_BSSID:
+		if (data)
+			memcpy(cfg_priv->profile->bssid, data, ETH_ALEN);
+		else
+			memset(cfg_priv->profile->bssid, 0, ETH_ALEN);
+		break;
+	case WL_PROF_SEC:
+		memcpy(&cfg_priv->profile->sec, data,
+		       sizeof(cfg_priv->profile->sec));
+		break;
+	case WL_PROF_BEACONINT:
+		cfg_priv->profile->beacon_interval = *(u16 *)data;
+		break;
+	case WL_PROF_DTIMPERIOD:
+		cfg_priv->profile->dtim_period = *(u8 *)data;
+		break;
+	default:
+		WL_ERR("unsupported item (%d)\n", item);
+		err = -EOPNOTSUPP;
+		break;
+	}
+
+	return err;
+}
+
+static void brcmf_init_prof(struct brcmf_cfg80211_profile *prof)
+{
+	memset(prof, 0, sizeof(*prof));
+}
+
+static void brcmf_ch_to_chanspec(int ch, struct brcmf_join_params *join_params,
+	size_t *join_params_size)
+{
+	u16 chanspec = 0;
+
+	if (ch != 0) {
+		if (ch <= CH_MAX_2G_CHANNEL)
+			chanspec |= WL_CHANSPEC_BAND_2G;
+		else
+			chanspec |= WL_CHANSPEC_BAND_5G;
+
+		chanspec |= WL_CHANSPEC_BW_20;
+		chanspec |= WL_CHANSPEC_CTL_SB_NONE;
+
+		*join_params_size += BRCMF_ASSOC_PARAMS_FIXED_SIZE +
+				     sizeof(u16);
+
+		chanspec |= (ch & WL_CHANSPEC_CHAN_MASK);
+		join_params->params_le.chanspec_list[0] = cpu_to_le16(chanspec);
+		join_params->params_le.chanspec_num = cpu_to_le32(1);
+
+		WL_CONN("join_params->params.chanspec_list[0]= %#X,"
+			"channel %d, chanspec %#X\n",
+			chanspec, ch, chanspec);
+	}
+}
+
+static void brcmf_link_down(struct brcmf_cfg80211_priv *cfg_priv)
+{
+	struct net_device *ndev = cfg_to_ndev(cfg_priv);
+	s32 err = 0;
+
+	WL_TRACE("Enter\n");
+
+	if (test_and_clear_bit(WL_STATUS_CONNECTED, &cfg_priv->status)) {
+		if (!brcmf_is_ibssmode(cfg_priv)) {
+			WL_CONN("Calling cfg80211_disconnected\n ");
+			cfg80211_disconnected(ndev, 0, NULL, 0,
+				GFP_KERNEL);
+		}
+		if (cfg_priv->link_up) {
+			WL_INFO("Call WLC_DISASSOC to stop excess roaming\n ");
+			err = brcmf_exec_dcmd(ndev, BRCMF_C_DISASSOC, NULL, 0);
+			if (unlikely(err))
+				WL_ERR("WLC_DISASSOC failed (%d)\n", err);
+			cfg_priv->link_up = false;
+		}
+	}
+
+	if (brcmf_is_ibssmode(cfg_priv))
+		clear_bit(WL_STATUS_CONNECTING, &cfg_priv->status);
+	else
+		brcmf_bss_connect_done(cfg_priv, ndev, NULL, false);
+
+	WL_TRACE("Exit\n");
+}
+
 static s32
-brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
+brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
 		      struct cfg80211_ibss_params *params)
 {
 	struct brcmf_cfg80211_priv *cfg_priv = wiphy_to_cfg(wiphy);
@@ -958,9 +919,11 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 	s32 err = 0;
 	s32 wsec = 0;
 	s32 bcnprd;
+	struct brcmf_ssid ssid;
 
 	WL_TRACE("Enter\n");
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
 	if (params->ssid)
 		WL_CONN("SSID: %s\n", params->ssid);
@@ -1012,20 +975,20 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 	if (params->privacy)
 		wsec |= WEP_ENABLED;
 
-	err = brcmf_dev_intvar_set(dev, "wsec", wsec);
-	if (unlikely(err)) {
+	err = brcmf_dev_intvar_set(ndev, "wsec", wsec);
+	if (err) {
 		WL_ERR("wsec failed (%d)\n", err);
 		goto done;
 	}
 
 	/* Configure Beacon Interval for starter */
 	if (params->beacon_interval)
-		bcnprd = cpu_to_le32(params->beacon_interval);
+		bcnprd = params->beacon_interval;
 	else
-		bcnprd = cpu_to_le32(100);
+		bcnprd = 100;
 
-	err = brcmf_dev_ioctl(dev, BRCM_SET_BCNPRD, &bcnprd, sizeof(bcnprd));
-	if (unlikely(err)) {
+	err = brcmf_exec_dcmd_u32(ndev, BRCM_SET_BCNPRD, &bcnprd);
+	if (err) {
 		WL_ERR("WLC_SET_BCNPRD failed (%d)\n", err);
 		goto done;
 	}
@@ -1034,23 +997,24 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 	memset(&join_params, 0, sizeof(struct brcmf_join_params));
 
 	/* SSID */
-	join_params.ssid.SSID_len =
-			(params->ssid_len > 32) ? 32 : params->ssid_len;
-	memcpy(join_params.ssid.SSID, params->ssid, join_params.ssid.SSID_len);
-	join_params.ssid.SSID_len = cpu_to_le32(join_params.ssid.SSID_len);
-	join_params_size = sizeof(join_params.ssid);
-	brcmf_update_prof(cfg_priv, NULL, &join_params.ssid, WL_PROF_SSID);
+	ssid.SSID_len = min_t(u32, params->ssid_len, 32);
+	memcpy(ssid.SSID, params->ssid, ssid.SSID_len);
+	memcpy(join_params.ssid_le.SSID, params->ssid, ssid.SSID_len);
+	join_params.ssid_le.SSID_len = cpu_to_le32(ssid.SSID_len);
+	join_params_size = sizeof(join_params.ssid_le);
+	brcmf_update_prof(cfg_priv, NULL, &ssid, WL_PROF_SSID);
 
 	/* BSSID */
 	if (params->bssid) {
-		memcpy(join_params.params.bssid, params->bssid, ETH_ALEN);
-		join_params_size = sizeof(join_params.ssid) +
-					BRCMF_ASSOC_PARAMS_FIXED_SIZE;
+		memcpy(join_params.params_le.bssid, params->bssid, ETH_ALEN);
+		join_params_size = sizeof(join_params.ssid_le) +
+				   BRCMF_ASSOC_PARAMS_FIXED_SIZE;
 	} else {
-		memcpy(join_params.params.bssid, ether_bcast, ETH_ALEN);
+		memcpy(join_params.params_le.bssid, ether_bcast, ETH_ALEN);
 	}
+
 	brcmf_update_prof(cfg_priv, NULL,
-			  &join_params.params.bssid, WL_PROF_BSSID);
+			  &join_params.params_le.bssid, WL_PROF_BSSID);
 
 	/* Channel */
 	if (params->channel) {
@@ -1066,10 +1030,10 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 		}
 
 		/* set channel for starter */
-		target_channel = cpu_to_le32(cfg_priv->channel);
-		err = brcmf_dev_ioctl(dev, BRCM_SET_CHANNEL,
-			&target_channel, sizeof(target_channel));
-		if (unlikely(err)) {
+		target_channel = cfg_priv->channel;
+		err = brcmf_exec_dcmd_u32(ndev, BRCM_SET_CHANNEL,
+					  &target_channel);
+		if (err) {
 			WL_ERR("WLC_SET_CHANNEL failed (%d)\n", err);
 			goto done;
 		}
@@ -1079,9 +1043,9 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 	cfg_priv->ibss_starter = false;
 
 
-	err = brcmf_dev_ioctl(dev, BRCMF_C_SET_SSID,
+	err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_SSID,
 			   &join_params, join_params_size);
-	if (unlikely(err)) {
+	if (err) {
 		WL_ERR("WLC_SET_SSID failed (%d)\n", err);
 		goto done;
 	}
@@ -1093,13 +1057,15 @@ done:
 	return err;
 }
 
-static s32 brcmf_cfg80211_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
+static s32
+brcmf_cfg80211_leave_ibss(struct wiphy *wiphy, struct net_device *ndev)
 {
 	struct brcmf_cfg80211_priv *cfg_priv = wiphy_to_cfg(wiphy);
 	s32 err = 0;
 
 	WL_TRACE("Enter\n");
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
 	brcmf_link_down(cfg_priv);
 
@@ -1108,10 +1074,10 @@ static s32 brcmf_cfg80211_leave_ibss(struct wiphy *wiphy, struct net_device *dev
 	return err;
 }
 
-static s32
-brcmf_set_wpa_version(struct net_device *dev, struct cfg80211_connect_params *sme)
+static s32 brcmf_set_wpa_version(struct net_device *ndev,
+				 struct cfg80211_connect_params *sme)
 {
-	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(dev);
+	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(ndev);
 	struct brcmf_cfg80211_security *sec;
 	s32 val = 0;
 	s32 err = 0;
@@ -1123,8 +1089,8 @@ brcmf_set_wpa_version(struct net_device *dev, struct cfg80211_connect_params *sm
 	else
 		val = WPA_AUTH_DISABLED;
 	WL_CONN("setting wpa_auth to 0x%0x\n", val);
-	err = brcmf_dev_intvar_set(dev, "wpa_auth", val);
-	if (unlikely(err)) {
+	err = brcmf_dev_intvar_set(ndev, "wpa_auth", val);
+	if (err) {
 		WL_ERR("set wpa_auth failed (%d)\n", err);
 		return err;
 	}
@@ -1133,10 +1099,10 @@ brcmf_set_wpa_version(struct net_device *dev, struct cfg80211_connect_params *sm
 	return err;
 }
 
-static s32
-brcmf_set_auth_type(struct net_device *dev, struct cfg80211_connect_params *sme)
+static s32 brcmf_set_auth_type(struct net_device *ndev,
+			       struct cfg80211_connect_params *sme)
 {
-	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(dev);
+	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(ndev);
 	struct brcmf_cfg80211_security *sec;
 	s32 val = 0;
 	s32 err = 0;
@@ -1162,8 +1128,8 @@ brcmf_set_auth_type(struct net_device *dev, struct cfg80211_connect_params *sme)
 		break;
 	}
 
-	err = brcmf_dev_intvar_set(dev, "auth", val);
-	if (unlikely(err)) {
+	err = brcmf_dev_intvar_set(ndev, "auth", val);
+	if (err) {
 		WL_ERR("set auth failed (%d)\n", err);
 		return err;
 	}
@@ -1173,9 +1139,10 @@ brcmf_set_auth_type(struct net_device *dev, struct cfg80211_connect_params *sme)
 }
 
 static s32
-brcmf_set_set_cipher(struct net_device *dev, struct cfg80211_connect_params *sme)
+brcmf_set_set_cipher(struct net_device *ndev,
+		     struct cfg80211_connect_params *sme)
 {
-	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(dev);
+	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(ndev);
 	struct brcmf_cfg80211_security *sec;
 	s32 pval = 0;
 	s32 gval = 0;
@@ -1225,8 +1192,8 @@ brcmf_set_set_cipher(struct net_device *dev, struct cfg80211_connect_params *sme
 	}
 
 	WL_CONN("pval (%d) gval (%d)\n", pval, gval);
-	err = brcmf_dev_intvar_set(dev, "wsec", pval | gval);
-	if (unlikely(err)) {
+	err = brcmf_dev_intvar_set(ndev, "wsec", pval | gval);
+	if (err) {
 		WL_ERR("error (%d)\n", err);
 		return err;
 	}
@@ -1239,16 +1206,16 @@ brcmf_set_set_cipher(struct net_device *dev, struct cfg80211_connect_params *sme
 }
 
 static s32
-brcmf_set_key_mgmt(struct net_device *dev, struct cfg80211_connect_params *sme)
+brcmf_set_key_mgmt(struct net_device *ndev, struct cfg80211_connect_params *sme)
 {
-	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(dev);
+	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(ndev);
 	struct brcmf_cfg80211_security *sec;
 	s32 val = 0;
 	s32 err = 0;
 
 	if (sme->crypto.n_akm_suites) {
-		err = brcmf_dev_intvar_get(dev, "wpa_auth", &val);
-		if (unlikely(err)) {
+		err = brcmf_dev_intvar_get(ndev, "wpa_auth", &val);
+		if (err) {
 			WL_ERR("could not get wpa_auth (%d)\n", err);
 			return err;
 		}
@@ -1281,8 +1248,8 @@ brcmf_set_key_mgmt(struct net_device *dev, struct cfg80211_connect_params *sme)
 		}
 
 		WL_CONN("setting wpa_auth to %d\n", val);
-		err = brcmf_dev_intvar_set(dev, "wpa_auth", val);
-		if (unlikely(err)) {
+		err = brcmf_dev_intvar_set(ndev, "wpa_auth", val);
+		if (err) {
 			WL_ERR("could not set wpa_auth (%d)\n", err);
 			return err;
 		}
@@ -1294,86 +1261,88 @@ brcmf_set_key_mgmt(struct net_device *dev, struct cfg80211_connect_params *sme)
 }
 
 static s32
-brcmf_set_set_sharedkey(struct net_device *dev,
+brcmf_set_wep_sharedkey(struct net_device *ndev,
 		     struct cfg80211_connect_params *sme)
 {
-	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(dev);
+	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(ndev);
 	struct brcmf_cfg80211_security *sec;
 	struct brcmf_wsec_key key;
 	s32 val;
 	s32 err = 0;
 
 	WL_CONN("key len (%d)\n", sme->key_len);
-	if (sme->key_len) {
-		sec = brcmf_read_prof(cfg_priv, WL_PROF_SEC);
-		WL_CONN("wpa_versions 0x%x cipher_pairwise 0x%x\n",
-		       sec->wpa_versions, sec->cipher_pairwise);
-		if (!
-		    (sec->wpa_versions & (NL80211_WPA_VERSION_1 |
-					  NL80211_WPA_VERSION_2))
-&& (sec->cipher_pairwise & (WLAN_CIPHER_SUITE_WEP40 |
-			    WLAN_CIPHER_SUITE_WEP104))) {
-			memset(&key, 0, sizeof(key));
-			key.len = (u32) sme->key_len;
-			key.index = (u32) sme->key_idx;
-			if (unlikely(key.len > sizeof(key.data))) {
-				WL_ERR("Too long key length (%u)\n", key.len);
-				return -EINVAL;
-			}
-			memcpy(key.data, sme->key, key.len);
-			key.flags = BRCMF_PRIMARY_KEY;
-			switch (sec->cipher_pairwise) {
-			case WLAN_CIPHER_SUITE_WEP40:
-				key.algo = CRYPTO_ALGO_WEP1;
-				break;
-			case WLAN_CIPHER_SUITE_WEP104:
-				key.algo = CRYPTO_ALGO_WEP128;
-				break;
-			default:
-				WL_ERR("Invalid algorithm (%d)\n",
-				       sme->crypto.ciphers_pairwise[0]);
-				return -EINVAL;
-			}
-			/* Set the new key/index */
-			WL_CONN("key length (%d) key index (%d) algo (%d)\n",
-			       key.len, key.index, key.algo);
-			WL_CONN("key \"%s\"\n", key.data);
-			swap_key_from_BE(&key);
-			err = brcmf_dev_ioctl(dev, BRCMF_C_SET_KEY, &key,
-					sizeof(key));
-			if (unlikely(err)) {
-				WL_ERR("WLC_SET_KEY error (%d)\n", err);
+
+	if (sme->key_len == 0)
+		return 0;
+
+	sec = brcmf_read_prof(cfg_priv, WL_PROF_SEC);
+	WL_CONN("wpa_versions 0x%x cipher_pairwise 0x%x\n",
+		sec->wpa_versions, sec->cipher_pairwise);
+
+	if (sec->wpa_versions & (NL80211_WPA_VERSION_1 | NL80211_WPA_VERSION_2))
+		return 0;
+
+	if (sec->cipher_pairwise &
+	    (WLAN_CIPHER_SUITE_WEP40 | WLAN_CIPHER_SUITE_WEP104)) {
+		memset(&key, 0, sizeof(key));
+		key.len = (u32) sme->key_len;
+		key.index = (u32) sme->key_idx;
+		if (key.len > sizeof(key.data)) {
+			WL_ERR("Too long key length (%u)\n", key.len);
+			return -EINVAL;
+		}
+		memcpy(key.data, sme->key, key.len);
+		key.flags = BRCMF_PRIMARY_KEY;
+		switch (sec->cipher_pairwise) {
+		case WLAN_CIPHER_SUITE_WEP40:
+			key.algo = CRYPTO_ALGO_WEP1;
+			break;
+		case WLAN_CIPHER_SUITE_WEP104:
+			key.algo = CRYPTO_ALGO_WEP128;
+			break;
+		default:
+			WL_ERR("Invalid algorithm (%d)\n",
+			       sme->crypto.ciphers_pairwise[0]);
+			return -EINVAL;
+		}
+		/* Set the new key/index */
+		WL_CONN("key length (%d) key index (%d) algo (%d)\n",
+			key.len, key.index, key.algo);
+		WL_CONN("key \"%s\"\n", key.data);
+		err = send_key_to_dongle(ndev, &key);
+		if (err)
+			return err;
+
+		if (sec->auth_type == NL80211_AUTHTYPE_OPEN_SYSTEM) {
+			WL_CONN("set auth_type to shared key\n");
+			val = 1;	/* shared key */
+			err = brcmf_dev_intvar_set(ndev, "auth", val);
+			if (err) {
+				WL_ERR("set auth failed (%d)\n", err);
 				return err;
 			}
-			if (sec->auth_type == NL80211_AUTHTYPE_OPEN_SYSTEM) {
-				WL_CONN("set auth_type to shared key\n");
-				val = 1;	/* shared key */
-				err = brcmf_dev_intvar_set(dev, "auth", val);
-				if (unlikely(err)) {
-					WL_ERR("set auth failed (%d)\n", err);
-					return err;
-				}
-			}
 		}
 	}
 	return err;
 }
 
 static s32
-brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
+brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
 		    struct cfg80211_connect_params *sme)
 {
 	struct brcmf_cfg80211_priv *cfg_priv = wiphy_to_cfg(wiphy);
 	struct ieee80211_channel *chan = sme->channel;
 	struct brcmf_join_params join_params;
 	size_t join_params_size;
+	struct brcmf_ssid ssid;
 
 	s32 err = 0;
 
 	WL_TRACE("Enter\n");
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
-	if (unlikely(!sme->ssid)) {
+	if (!sme->ssid) {
 		WL_ERR("Invalid ssid\n");
 		return -EOPNOTSUPP;
 	}
@@ -1390,62 +1359,54 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 
 	WL_INFO("ie (%p), ie_len (%zd)\n", sme->ie, sme->ie_len);
 
-	err = brcmf_set_wpa_version(dev, sme);
+	err = brcmf_set_wpa_version(ndev, sme);
 	if (err) {
 		WL_ERR("wl_set_wpa_version failed (%d)\n", err);
 		goto done;
 	}
 
-	err = brcmf_set_auth_type(dev, sme);
+	err = brcmf_set_auth_type(ndev, sme);
 	if (err) {
 		WL_ERR("wl_set_auth_type failed (%d)\n", err);
 		goto done;
 	}
 
-	err = brcmf_set_set_cipher(dev, sme);
+	err = brcmf_set_set_cipher(ndev, sme);
 	if (err) {
 		WL_ERR("wl_set_set_cipher failed (%d)\n", err);
 		goto done;
 	}
 
-	err = brcmf_set_key_mgmt(dev, sme);
+	err = brcmf_set_key_mgmt(ndev, sme);
 	if (err) {
 		WL_ERR("wl_set_key_mgmt failed (%d)\n", err);
 		goto done;
 	}
 
-	err = brcmf_set_set_sharedkey(dev, sme);
+	err = brcmf_set_wep_sharedkey(ndev, sme);
 	if (err) {
-		WL_ERR("wl_set_set_sharedkey failed (%d)\n", err);
+		WL_ERR("brcmf_set_wep_sharedkey failed (%d)\n", err);
 		goto done;
 	}
 
-	brcmf_update_prof(cfg_priv, NULL, sme->bssid, WL_PROF_BSSID);
-	/*
-	 **  Join with specific BSSID and cached SSID
-	 **  If SSID is zero join based on BSSID only
-	 */
 	memset(&join_params, 0, sizeof(join_params));
-	join_params_size = sizeof(join_params.ssid);
+	join_params_size = sizeof(join_params.ssid_le);
 
-	join_params.ssid.SSID_len = min(sizeof(join_params.ssid.SSID), sme->ssid_len);
-	memcpy(&join_params.ssid.SSID, sme->ssid, join_params.ssid.SSID_len);
-	join_params.ssid.SSID_len = cpu_to_le32(join_params.ssid.SSID_len);
-	brcmf_update_prof(cfg_priv, NULL, &join_params.ssid, WL_PROF_SSID);
+	ssid.SSID_len = min_t(u32, sizeof(ssid.SSID), sme->ssid_len);
+	memcpy(&join_params.ssid_le.SSID, sme->ssid, ssid.SSID_len);
+	memcpy(&ssid.SSID, sme->ssid, ssid.SSID_len);
+	join_params.ssid_le.SSID_len = cpu_to_le32(ssid.SSID_len);
+	brcmf_update_prof(cfg_priv, NULL, &ssid, WL_PROF_SSID);
 
-	if (sme->bssid)
-		memcpy(join_params.params.bssid, sme->bssid, ETH_ALEN);
-	else
-		memcpy(join_params.params.bssid, ether_bcast, ETH_ALEN);
+	memcpy(join_params.params_le.bssid, ether_bcast, ETH_ALEN);
 
-	if (join_params.ssid.SSID_len < IEEE80211_MAX_SSID_LEN) {
+	if (ssid.SSID_len < IEEE80211_MAX_SSID_LEN)
 		WL_CONN("ssid \"%s\", len (%d)\n",
-		       join_params.ssid.SSID, join_params.ssid.SSID_len);
-	}
+		       ssid.SSID, ssid.SSID_len);
 
 	brcmf_ch_to_chanspec(cfg_priv->channel,
 			     &join_params, &join_params_size);
-	err = brcmf_dev_ioctl(dev, BRCMF_C_SET_SSID,
+	err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_SSID,
 			   &join_params, join_params_size);
 	if (err)
 		WL_ERR("WLC_SET_SSID failed (%d)\n", err);
@@ -1458,24 +1419,24 @@ done:
 }
 
 static s32
-brcmf_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *dev,
+brcmf_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *ndev,
 		       u16 reason_code)
 {
 	struct brcmf_cfg80211_priv *cfg_priv = wiphy_to_cfg(wiphy);
-	struct brcmf_scb_val scbval;
+	struct brcmf_scb_val_le scbval;
 	s32 err = 0;
 
 	WL_TRACE("Enter. Reason code = %d\n", reason_code);
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
 	clear_bit(WL_STATUS_CONNECTED, &cfg_priv->status);
 
-	scbval.val = reason_code;
 	memcpy(&scbval.ea, brcmf_read_prof(cfg_priv, WL_PROF_BSSID), ETH_ALEN);
-	scbval.val = cpu_to_le32(scbval.val);
-	err = brcmf_dev_ioctl(dev, BRCMF_C_DISASSOC, &scbval,
-			sizeof(struct brcmf_scb_val));
-	if (unlikely(err))
+	scbval.val = cpu_to_le32(reason_code);
+	err = brcmf_exec_dcmd(ndev, BRCMF_C_DISASSOC, &scbval,
+			      sizeof(struct brcmf_scb_val_le));
+	if (err)
 		WL_ERR("error (%d)\n", err);
 
 	cfg_priv->link_up = false;
@@ -1496,7 +1457,8 @@ brcmf_cfg80211_set_tx_power(struct wiphy *wiphy,
 	s32 disable = 0;
 
 	WL_TRACE("Enter\n");
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
 	switch (type) {
 	case NL80211_TX_POWER_AUTOMATIC:
@@ -1518,9 +1480,8 @@ 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;
-	disable = cpu_to_le32(disable);
-	err = brcmf_dev_ioctl(ndev, BRCMF_C_SET_RADIO, &disable, sizeof(disable));
-	if (unlikely(err))
+	err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_RADIO, &disable);
+	if (err)
 		WL_ERR("WLC_SET_RADIO error (%d)\n", err);
 
 	if (dbm > 0xffff)
@@ -1528,8 +1489,8 @@ brcmf_cfg80211_set_tx_power(struct wiphy *wiphy,
 	else
 		txpwrmw = (u16) dbm;
 	err = brcmf_dev_intvar_set(ndev, "qtxpower",
-			(s32) (brcmu_mw_to_qdbm(txpwrmw)));
-	if (unlikely(err))
+			(s32) (brcmf_mw_to_qdbm(txpwrmw)));
+	if (err)
 		WL_ERR("qtxpower error (%d)\n", err);
 	cfg_priv->conf->tx_power = dbm;
 
@@ -1547,16 +1508,17 @@ static s32 brcmf_cfg80211_get_tx_power(struct wiphy *wiphy, s32 *dbm)
 	s32 err = 0;
 
 	WL_TRACE("Enter\n");
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
 	err = brcmf_dev_intvar_get(ndev, "qtxpower", &txpwrdbm);
-	if (unlikely(err)) {
+	if (err) {
 		WL_ERR("error (%d)\n", err);
 		goto done;
 	}
 
 	result = (u8) (txpwrdbm & ~WL_TXPWR_OVERRIDE);
-	*dbm = (s32) brcmu_qdbm_to_mw(result);
+	*dbm = (s32) brcmf_qdbm_to_mw(result);
 
 done:
 	WL_TRACE("Exit\n");
@@ -1564,31 +1526,30 @@ done:
 }
 
 static s32
-brcmf_cfg80211_config_default_key(struct wiphy *wiphy, struct net_device *dev,
+brcmf_cfg80211_config_default_key(struct wiphy *wiphy, struct net_device *ndev,
 			       u8 key_idx, bool unicast, bool multicast)
 {
 	u32 index;
-	s32 wsec;
+	u32 wsec;
 	s32 err = 0;
 
 	WL_TRACE("Enter\n");
 	WL_CONN("key index (%d)\n", key_idx);
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
-	err = brcmf_dev_ioctl(dev, BRCMF_C_GET_WSEC, &wsec, sizeof(wsec));
-	if (unlikely(err)) {
+	err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_GET_WSEC, &wsec);
+	if (err) {
 		WL_ERR("WLC_GET_WSEC error (%d)\n", err);
 		goto done;
 	}
 
-	wsec = le32_to_cpu(wsec);
 	if (wsec & WEP_ENABLED) {
 		/* Just select a new current key */
-		index = (u32) key_idx;
-		index = cpu_to_le32(index);
-		err = brcmf_dev_ioctl(dev, BRCMF_C_SET_KEY_PRIMARY, &index,
-				sizeof(index));
-		if (unlikely(err))
+		index = key_idx;
+		err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_KEY_PRIMARY,
+					  &index);
+		if (err)
 			WL_ERR("error (%d)\n", err);
 	}
 done:
@@ -1597,10 +1558,11 @@ done:
 }
 
 static s32
-brcmf_add_keyext(struct wiphy *wiphy, struct net_device *dev,
+brcmf_add_keyext(struct wiphy *wiphy, struct net_device *ndev,
 	      u8 key_idx, const u8 *mac_addr, struct key_params *params)
 {
 	struct brcmf_wsec_key key;
+	struct brcmf_wsec_key_le key_le;
 	s32 err = 0;
 
 	memset(&key, 0, sizeof(key));
@@ -1613,12 +1575,9 @@ brcmf_add_keyext(struct wiphy *wiphy, struct net_device *dev,
 	/* check for key index change */
 	if (key.len == 0) {
 		/* key delete */
-		swap_key_from_BE(&key);
-		err = brcmf_dev_ioctl(dev, BRCMF_C_SET_KEY, &key, sizeof(key));
-		if (unlikely(err)) {
-			WL_ERR("key delete error (%d)\n", err);
+		err = send_key_to_dongle(ndev, &key);
+		if (err)
 			return err;
-		}
 	} else {
 		if (key.len > sizeof(key.data)) {
 			WL_ERR("Invalid key length (%d)\n", key.len);
@@ -1671,11 +1630,12 @@ brcmf_add_keyext(struct wiphy *wiphy, struct net_device *dev,
 			WL_ERR("Invalid cipher (0x%x)\n", params->cipher);
 			return -EINVAL;
 		}
-		swap_key_from_BE(&key);
+		convert_key_from_CPU(&key, &key_le);
 
-		brcmf_netdev_wait_pend8021x(dev);
-		err = brcmf_dev_ioctl(dev, BRCMF_C_SET_KEY, &key, sizeof(key));
-		if (unlikely(err)) {
+		brcmf_netdev_wait_pend8021x(ndev);
+		err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_KEY, &key_le,
+				      sizeof(key_le));
+		if (err) {
 			WL_ERR("WLC_SET_KEY error (%d)\n", err);
 			return err;
 		}
@@ -1684,7 +1644,7 @@ brcmf_add_keyext(struct wiphy *wiphy, struct net_device *dev,
 }
 
 static s32
-brcmf_cfg80211_add_key(struct wiphy *wiphy, struct net_device *dev,
+brcmf_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
 		    u8 key_idx, bool pairwise, const u8 *mac_addr,
 		    struct key_params *params)
 {
@@ -1696,18 +1656,19 @@ brcmf_cfg80211_add_key(struct wiphy *wiphy, struct net_device *dev,
 
 	WL_TRACE("Enter\n");
 	WL_CONN("key index (%d)\n", key_idx);
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
 	if (mac_addr) {
 		WL_TRACE("Exit");
-		return brcmf_add_keyext(wiphy, dev, key_idx, mac_addr, params);
+		return brcmf_add_keyext(wiphy, ndev, key_idx, mac_addr, params);
 	}
 	memset(&key, 0, sizeof(key));
 
 	key.len = (u32) params->key_len;
 	key.index = (u32) key_idx;
 
-	if (unlikely(key.len > sizeof(key.data))) {
+	if (key.len > sizeof(key.data)) {
 		WL_ERR("Too long key length (%u)\n", key.len);
 		err = -EINVAL;
 		goto done;
@@ -1745,32 +1706,27 @@ brcmf_cfg80211_add_key(struct wiphy *wiphy, struct net_device *dev,
 		goto done;
 	}
 
-	/* Set the new key/index */
-	swap_key_from_BE(&key);
-	err = brcmf_dev_ioctl(dev, BRCMF_C_SET_KEY, &key, sizeof(key));
-	if (unlikely(err)) {
-		WL_ERR("WLC_SET_KEY error (%d)\n", err);
+	err = send_key_to_dongle(ndev, &key); /* Set the new key/index */
+	if (err)
 		goto done;
-	}
 
 	val = WEP_ENABLED;
-	err = brcmf_dev_intvar_get(dev, "wsec", &wsec);
-	if (unlikely(err)) {
+	err = brcmf_dev_intvar_get(ndev, "wsec", &wsec);
+	if (err) {
 		WL_ERR("get wsec error (%d)\n", err);
 		goto done;
 	}
 	wsec &= ~(WEP_ENABLED);
 	wsec |= val;
-	err = brcmf_dev_intvar_set(dev, "wsec", wsec);
-	if (unlikely(err)) {
+	err = brcmf_dev_intvar_set(ndev, "wsec", wsec);
+	if (err) {
 		WL_ERR("set wsec error (%d)\n", err);
 		goto done;
 	}
 
 	val = 1;		/* assume shared key. otherwise 0 */
-	val = cpu_to_le32(val);
-	err = brcmf_dev_ioctl(dev, BRCMF_C_SET_AUTH, &val, sizeof(val));
-	if (unlikely(err))
+	err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_AUTH, &val);
+	if (err)
 		WL_ERR("WLC_SET_AUTH error (%d)\n", err);
 done:
 	WL_TRACE("Exit\n");
@@ -1778,7 +1734,7 @@ done:
 }
 
 static s32
-brcmf_cfg80211_del_key(struct wiphy *wiphy, struct net_device *dev,
+brcmf_cfg80211_del_key(struct wiphy *wiphy, struct net_device *ndev,
 		    u8 key_idx, bool pairwise, const u8 *mac_addr)
 {
 	struct brcmf_wsec_key key;
@@ -1787,7 +1743,9 @@ brcmf_cfg80211_del_key(struct wiphy *wiphy, struct net_device *dev,
 	s32 wsec;
 
 	WL_TRACE("Enter\n");
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
+
 	memset(&key, 0, sizeof(key));
 
 	key.index = (u32) key_idx;
@@ -1795,25 +1753,23 @@ brcmf_cfg80211_del_key(struct wiphy *wiphy, struct net_device *dev,
 	key.algo = CRYPTO_ALGO_OFF;
 
 	WL_CONN("key index (%d)\n", key_idx);
+
 	/* Set the new key/index */
-	swap_key_from_BE(&key);
-	err = brcmf_dev_ioctl(dev, BRCMF_C_SET_KEY, &key, sizeof(key));
-	if (unlikely(err)) {
+	err = send_key_to_dongle(ndev, &key);
+	if (err) {
 		if (err == -EINVAL) {
 			if (key.index >= DOT11_MAX_DEFAULT_KEYS)
 				/* we ignore this key index in this case */
 				WL_ERR("invalid key index (%d)\n", key_idx);
-		} else
-			WL_ERR("WLC_SET_KEY error (%d)\n", err);
-
+		}
 		/* Ignore this error, may happen during DISASSOC */
 		err = -EAGAIN;
 		goto done;
 	}
 
 	val = 0;
-	err = brcmf_dev_intvar_get(dev, "wsec", &wsec);
-	if (unlikely(err)) {
+	err = brcmf_dev_intvar_get(ndev, "wsec", &wsec);
+	if (err) {
 		WL_ERR("get wsec error (%d)\n", err);
 		/* Ignore this error, may happen during DISASSOC */
 		err = -EAGAIN;
@@ -1821,8 +1777,8 @@ brcmf_cfg80211_del_key(struct wiphy *wiphy, struct net_device *dev,
 	}
 	wsec &= ~(WEP_ENABLED);
 	wsec |= val;
-	err = brcmf_dev_intvar_set(dev, "wsec", wsec);
-	if (unlikely(err)) {
+	err = brcmf_dev_intvar_set(ndev, "wsec", wsec);
+	if (err) {
 		WL_ERR("set wsec error (%d)\n", err);
 		/* Ignore this error, may happen during DISASSOC */
 		err = -EAGAIN;
@@ -1830,9 +1786,8 @@ brcmf_cfg80211_del_key(struct wiphy *wiphy, struct net_device *dev,
 	}
 
 	val = 0;		/* assume open key. otherwise 1 */
-	val = cpu_to_le32(val);
-	err = brcmf_dev_ioctl(dev, BRCMF_C_SET_AUTH, &val, sizeof(val));
-	if (unlikely(err)) {
+	err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_AUTH, &val);
+	if (err) {
 		WL_ERR("WLC_SET_AUTH error (%d)\n", err);
 		/* Ignore this error, may happen during DISASSOC */
 		err = -EAGAIN;
@@ -1843,12 +1798,11 @@ done:
 }
 
 static s32
-brcmf_cfg80211_get_key(struct wiphy *wiphy, struct net_device *dev,
+brcmf_cfg80211_get_key(struct wiphy *wiphy, struct net_device *ndev,
 		    u8 key_idx, bool pairwise, const u8 *mac_addr, void *cookie,
 		    void (*callback) (void *cookie, struct key_params * params))
 {
 	struct key_params params;
-	struct brcmf_wsec_key key;
 	struct brcmf_cfg80211_priv *cfg_priv = wiphy_to_cfg(wiphy);
 	struct brcmf_cfg80211_security *sec;
 	s32 wsec;
@@ -1856,23 +1810,18 @@ brcmf_cfg80211_get_key(struct wiphy *wiphy, struct net_device *dev,
 
 	WL_TRACE("Enter\n");
 	WL_CONN("key index (%d)\n", key_idx);
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
-	memset(&key, 0, sizeof(key));
-	key.index = key_idx;
-	swap_key_to_BE(&key);
 	memset(&params, 0, sizeof(params));
-	params.key_len = (u8) min_t(u8, WLAN_MAX_KEY_LEN, key.len);
-	memcpy(params.key, key.data, params.key_len);
 
-	err = brcmf_dev_ioctl(dev, BRCMF_C_GET_WSEC, &wsec, sizeof(wsec));
-	if (unlikely(err)) {
+	err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_GET_WSEC, &wsec);
+	if (err) {
 		WL_ERR("WLC_GET_WSEC error (%d)\n", err);
 		/* Ignore this error, may happen during DISASSOC */
 		err = -EAGAIN;
 		goto done;
 	}
-	wsec = le32_to_cpu(wsec);
 	switch (wsec) {
 	case WEP_ENABLED:
 		sec = brcmf_read_prof(cfg_priv, WL_PROF_SEC);
@@ -1906,30 +1855,29 @@ done:
 
 static s32
 brcmf_cfg80211_config_default_mgmt_key(struct wiphy *wiphy,
-				    struct net_device *dev, u8 key_idx)
+				    struct net_device *ndev, u8 key_idx)
 {
 	WL_INFO("Not supported\n");
 
-	CHECK_SYS_UP();
 	return -EOPNOTSUPP;
 }
 
 static s32
-brcmf_cfg80211_get_station(struct wiphy *wiphy, struct net_device *dev,
+brcmf_cfg80211_get_station(struct wiphy *wiphy, struct net_device *ndev,
 			u8 *mac, struct station_info *sinfo)
 {
 	struct brcmf_cfg80211_priv *cfg_priv = wiphy_to_cfg(wiphy);
-	struct brcmf_scb_val scb_val;
+	struct brcmf_scb_val_le scb_val;
 	int rssi;
 	s32 rate;
 	s32 err = 0;
 	u8 *bssid = brcmf_read_prof(cfg_priv, WL_PROF_BSSID);
 
 	WL_TRACE("Enter\n");
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
-	if (unlikely
-	    (memcmp(mac, bssid, ETH_ALEN))) {
+	if (memcmp(mac, bssid, ETH_ALEN)) {
 		WL_ERR("Wrong Mac address cfg_mac-%X:%X:%X:%X:%X:%X"
 			"wl_bssid-%X:%X:%X:%X:%X:%X\n",
 			mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
@@ -1940,23 +1888,22 @@ brcmf_cfg80211_get_station(struct wiphy *wiphy, struct net_device *dev,
 	}
 
 	/* Report the current tx rate */
-	err = brcmf_dev_ioctl(dev, BRCMF_C_GET_RATE, &rate, sizeof(rate));
+	err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_GET_RATE, &rate);
 	if (err) {
 		WL_ERR("Could not get rate (%d)\n", err);
 	} else {
-		rate = le32_to_cpu(rate);
 		sinfo->filled |= STATION_INFO_TX_BITRATE;
 		sinfo->txrate.legacy = rate * 5;
 		WL_CONN("Rate %d Mbps\n", rate / 2);
 	}
 
 	if (test_bit(WL_STATUS_CONNECTED, &cfg_priv->status)) {
-		scb_val.val = 0;
-		err = brcmf_dev_ioctl(dev, BRCMF_C_GET_RSSI, &scb_val,
-				sizeof(struct brcmf_scb_val));
-		if (unlikely(err)) {
+		scb_val.val = cpu_to_le32(0);
+		err = brcmf_exec_dcmd(ndev, BRCMF_C_GET_RSSI, &scb_val,
+				      sizeof(struct brcmf_scb_val_le));
+		if (err)
 			WL_ERR("Could not get rssi (%d)\n", err);
-		}
+
 		rssi = le32_to_cpu(scb_val.val);
 		sinfo->filled |= STATION_INFO_SIGNAL;
 		sinfo->signal = rssi;
@@ -1969,63 +1916,51 @@ done:
 }
 
 static s32
-brcmf_cfg80211_set_power_mgmt(struct wiphy *wiphy, struct net_device *dev,
+brcmf_cfg80211_set_power_mgmt(struct wiphy *wiphy, struct net_device *ndev,
 			   bool enabled, s32 timeout)
 {
 	s32 pm;
 	s32 err = 0;
+	struct brcmf_cfg80211_priv *cfg_priv = wiphy_to_cfg(wiphy);
 
 	WL_TRACE("Enter\n");
-	CHECK_SYS_UP();
+
+	/*
+	 * Powersave enable/disable request is coming from the
+	 * cfg80211 even before the interface is up. In that
+	 * scenario, driver will be storing the power save
+	 * preference in cfg_priv struct to apply this to
+	 * FW later while initializing the dongle
+	 */
+	cfg_priv->pwr_save = enabled;
+	if (!test_bit(WL_STATUS_READY, &cfg_priv->status)) {
+
+		WL_INFO("Device is not ready,"
+			"storing the value in cfg_priv struct\n");
+		goto done;
+	}
 
 	pm = enabled ? PM_FAST : PM_OFF;
-	pm = cpu_to_le32(pm);
 	WL_INFO("power save %s\n", (pm ? "enabled" : "disabled"));
 
-	err = brcmf_dev_ioctl(dev, BRCMF_C_SET_PM, &pm, sizeof(pm));
-	if (unlikely(err)) {
+	err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_PM, &pm);
+	if (err) {
 		if (err == -ENODEV)
 			WL_ERR("net_device is not ready yet\n");
 		else
 			WL_ERR("error (%d)\n", err);
 	}
+done:
 	WL_TRACE("Exit\n");
 	return err;
 }
 
-static __used u32 brcmf_find_msb(u16 bit16)
-{
-	u32 ret = 0;
-
-	if (bit16 & 0xff00) {
-		ret += 8;
-		bit16 >>= 8;
-	}
-
-	if (bit16 & 0xf0) {
-		ret += 4;
-		bit16 >>= 4;
-	}
-
-	if (bit16 & 0xc) {
-		ret += 2;
-		bit16 >>= 2;
-	}
-
-	if (bit16 & 2)
-		ret += bit16 & 2;
-	else if (bit16)
-		ret += bit16;
-
-	return ret;
-}
-
 static s32
-brcmf_cfg80211_set_bitrate_mask(struct wiphy *wiphy, struct net_device *dev,
+brcmf_cfg80211_set_bitrate_mask(struct wiphy *wiphy, struct net_device *ndev,
 			     const u8 *addr,
 			     const struct cfg80211_bitrate_mask *mask)
 {
-	struct wl_rateset rateset;
+	struct brcm_rateset_le rateset_le;
 	s32 rate;
 	s32 val;
 	s32 err_bg;
@@ -2034,28 +1969,28 @@ brcmf_cfg80211_set_bitrate_mask(struct wiphy *wiphy, struct net_device *dev,
 	s32 err = 0;
 
 	WL_TRACE("Enter\n");
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
 	/* addr param is always NULL. ignore it */
 	/* Get current rateset */
-	err = brcmf_dev_ioctl(dev, BRCM_GET_CURR_RATESET, &rateset,
-			sizeof(rateset));
-	if (unlikely(err)) {
+	err = brcmf_exec_dcmd(ndev, BRCM_GET_CURR_RATESET, &rateset_le,
+			      sizeof(rateset_le));
+	if (err) {
 		WL_ERR("could not get current rateset (%d)\n", err);
 		goto done;
 	}
 
-	rateset.count = le32_to_cpu(rateset.count);
-
-	legacy = brcmf_find_msb(mask->control[IEEE80211_BAND_2GHZ].legacy);
+	legacy = ffs(mask->control[IEEE80211_BAND_2GHZ].legacy & 0xFFFF);
 	if (!legacy)
-		legacy = brcmf_find_msb(mask->control[IEEE80211_BAND_5GHZ].legacy);
+		legacy = ffs(mask->control[IEEE80211_BAND_5GHZ].legacy &
+			     0xFFFF);
 
 	val = wl_g_rates[legacy - 1].bitrate * 100000;
 
-	if (val < rateset.count)
+	if (val < le32_to_cpu(rateset_le.count))
 		/* Select rate by rateset index */
-		rate = rateset.rates[val] & 0x7f;
+		rate = rateset_le.rates[val] & 0x7f;
 	else
 		/* Specified rate in bps */
 		rate = val / 500000;
@@ -2067,9 +2002,9 @@ brcmf_cfg80211_set_bitrate_mask(struct wiphy *wiphy, struct net_device *dev,
 	 *      Set rate override,
 	 *      Since the is a/b/g-blind, both a/bg_rate are enforced.
 	 */
-	err_bg = brcmf_dev_intvar_set(dev, "bg_rate", rate);
-	err_a = brcmf_dev_intvar_set(dev, "a_rate", rate);
-	if (unlikely(err_bg && err_a)) {
+	err_bg = brcmf_dev_intvar_set(ndev, "bg_rate", rate);
+	err_a = brcmf_dev_intvar_set(ndev, "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;
 	}
@@ -2079,6 +2014,483 @@ done:
 	return err;
 }
 
+static s32 brcmf_inform_single_bss(struct brcmf_cfg80211_priv *cfg_priv,
+				   struct brcmf_bss_info *bi)
+{
+	struct wiphy *wiphy = cfg_to_wiphy(cfg_priv);
+	struct ieee80211_channel *notify_channel;
+	struct cfg80211_bss *bss;
+	struct ieee80211_supported_band *band;
+	s32 err = 0;
+	u16 channel;
+	u32 freq;
+	u64 notify_timestamp;
+	u16 notify_capability;
+	u16 notify_interval;
+	u8 *notify_ie;
+	size_t notify_ielen;
+	s32 notify_signal;
+
+	if (le32_to_cpu(bi->length) > WL_BSS_INFO_MAX) {
+		WL_ERR("Bss info is larger than buffer. Discarding\n");
+		return 0;
+	}
+
+	channel = bi->ctl_ch ? bi->ctl_ch :
+				CHSPEC_CHANNEL(le16_to_cpu(bi->chanspec));
+
+	if (channel <= CH_MAX_2G_CHANNEL)
+		band = wiphy->bands[IEEE80211_BAND_2GHZ];
+	else
+		band = wiphy->bands[IEEE80211_BAND_5GHZ];
+
+	freq = ieee80211_channel_to_frequency(channel, band->band);
+	notify_channel = ieee80211_get_channel(wiphy, freq);
+
+	notify_timestamp = jiffies_to_msecs(jiffies)*1000; /* uSec */
+	notify_capability = le16_to_cpu(bi->capability);
+	notify_interval = le16_to_cpu(bi->beacon_period);
+	notify_ie = (u8 *)bi + le16_to_cpu(bi->ie_offset);
+	notify_ielen = le32_to_cpu(bi->ie_length);
+	notify_signal = (s16)le16_to_cpu(bi->RSSI) * 100;
+
+	WL_CONN("bssid: %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n",
+			bi->BSSID[0], bi->BSSID[1], bi->BSSID[2],
+			bi->BSSID[3], bi->BSSID[4], bi->BSSID[5]);
+	WL_CONN("Channel: %d(%d)\n", channel, freq);
+	WL_CONN("Capability: %X\n", notify_capability);
+	WL_CONN("Beacon interval: %d\n", notify_interval);
+	WL_CONN("Signal: %d\n", notify_signal);
+	WL_CONN("notify_timestamp: %#018llx\n", notify_timestamp);
+
+	bss = cfg80211_inform_bss(wiphy, notify_channel, (const u8 *)bi->BSSID,
+		notify_timestamp, notify_capability, notify_interval, notify_ie,
+		notify_ielen, notify_signal, GFP_KERNEL);
+
+	if (!bss) {
+		WL_ERR("cfg80211_inform_bss_frame error\n");
+		return -EINVAL;
+	}
+
+	return err;
+}
+
+static s32 brcmf_inform_bss(struct brcmf_cfg80211_priv *cfg_priv)
+{
+	struct brcmf_scan_results *bss_list;
+	struct brcmf_bss_info *bi = NULL;	/* must be initialized */
+	s32 err = 0;
+	int i;
+
+	bss_list = cfg_priv->bss_list;
+	if (bss_list->version != BRCMF_BSS_INFO_VERSION) {
+		WL_ERR("Version %d != WL_BSS_INFO_VERSION\n",
+		       bss_list->version);
+		return -EOPNOTSUPP;
+	}
+	WL_SCAN("scanned AP count (%d)\n", bss_list->count);
+	for (i = 0; i < bss_list->count && i < WL_AP_MAX; i++) {
+		bi = next_bss(bss_list, bi);
+		err = brcmf_inform_single_bss(cfg_priv, bi);
+		if (err)
+			break;
+	}
+	return err;
+}
+
+static s32 wl_inform_ibss(struct brcmf_cfg80211_priv *cfg_priv,
+			  struct net_device *ndev, const u8 *bssid)
+{
+	struct wiphy *wiphy = cfg_to_wiphy(cfg_priv);
+	struct ieee80211_channel *notify_channel;
+	struct brcmf_bss_info *bi = NULL;
+	struct ieee80211_supported_band *band;
+	u8 *buf = NULL;
+	s32 err = 0;
+	u16 channel;
+	u32 freq;
+	u64 notify_timestamp;
+	u16 notify_capability;
+	u16 notify_interval;
+	u8 *notify_ie;
+	size_t notify_ielen;
+	s32 notify_signal;
+
+	WL_TRACE("Enter\n");
+
+	buf = kzalloc(WL_BSS_INFO_MAX, GFP_KERNEL);
+	if (buf == NULL) {
+		err = -ENOMEM;
+		goto CleanUp;
+	}
+
+	*(__le32 *)buf = cpu_to_le32(WL_BSS_INFO_MAX);
+
+	err = brcmf_exec_dcmd(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;
+	}
+
+	bi = (struct brcmf_bss_info *)(buf + 4);
+
+	channel = bi->ctl_ch ? bi->ctl_ch :
+				CHSPEC_CHANNEL(le16_to_cpu(bi->chanspec));
+
+	if (channel <= CH_MAX_2G_CHANNEL)
+		band = wiphy->bands[IEEE80211_BAND_2GHZ];
+	else
+		band = wiphy->bands[IEEE80211_BAND_5GHZ];
+
+	freq = ieee80211_channel_to_frequency(channel, band->band);
+	notify_channel = ieee80211_get_channel(wiphy, freq);
+
+	notify_timestamp = jiffies_to_msecs(jiffies)*1000; /* uSec */
+	notify_capability = le16_to_cpu(bi->capability);
+	notify_interval = le16_to_cpu(bi->beacon_period);
+	notify_ie = (u8 *)bi + le16_to_cpu(bi->ie_offset);
+	notify_ielen = le32_to_cpu(bi->ie_length);
+	notify_signal = (s16)le16_to_cpu(bi->RSSI) * 100;
+
+	WL_CONN("channel: %d(%d)\n", channel, freq);
+	WL_CONN("capability: %X\n", notify_capability);
+	WL_CONN("beacon interval: %d\n", notify_interval);
+	WL_CONN("signal: %d\n", notify_signal);
+	WL_CONN("notify_timestamp: %#018llx\n", notify_timestamp);
+
+	cfg80211_inform_bss(wiphy, notify_channel, bssid,
+		notify_timestamp, notify_capability, notify_interval,
+		notify_ie, notify_ielen, notify_signal, GFP_KERNEL);
+
+CleanUp:
+
+	kfree(buf);
+
+	WL_TRACE("Exit\n");
+
+	return err;
+}
+
+static bool brcmf_is_ibssmode(struct brcmf_cfg80211_priv *cfg_priv)
+{
+	return cfg_priv->conf->mode == WL_MODE_IBSS;
+}
+
+/*
+ * Traverse a string of 1-byte tag/1-byte length/variable-length value
+ * triples, returning a pointer to the substring whose first element
+ * matches tag
+ */
+static struct brcmf_tlv *brcmf_parse_tlvs(void *buf, int buflen, uint key)
+{
+	struct brcmf_tlv *elt;
+	int totlen;
+
+	elt = (struct brcmf_tlv *) buf;
+	totlen = buflen;
+
+	/* find tagged parameter */
+	while (totlen >= 2) {
+		int len = elt->len;
+
+		/* validate remaining totlen */
+		if ((elt->id == key) && (totlen >= (len + 2)))
+			return elt;
+
+		elt = (struct brcmf_tlv *) ((u8 *) elt + (len + 2));
+		totlen -= (len + 2);
+	}
+
+	return NULL;
+}
+
+static s32 brcmf_update_bss_info(struct brcmf_cfg80211_priv *cfg_priv)
+{
+	struct brcmf_bss_info *bi;
+	struct brcmf_ssid *ssid;
+	struct brcmf_tlv *tim;
+	u16 beacon_interval;
+	u8 dtim_period;
+	size_t ie_len;
+	u8 *ie;
+	s32 err = 0;
+
+	WL_TRACE("Enter\n");
+	if (brcmf_is_ibssmode(cfg_priv))
+		return err;
+
+	ssid = (struct brcmf_ssid *)brcmf_read_prof(cfg_priv, WL_PROF_SSID);
+
+	*(__le32 *)cfg_priv->extra_buf = cpu_to_le32(WL_EXTRA_BUF_MAX);
+	err = brcmf_exec_dcmd(cfg_to_ndev(cfg_priv), BRCMF_C_GET_BSS_INFO,
+			cfg_priv->extra_buf, WL_EXTRA_BUF_MAX);
+	if (err) {
+		WL_ERR("Could not get bss info %d\n", err);
+		goto update_bss_info_out;
+	}
+
+	bi = (struct brcmf_bss_info *)(cfg_priv->extra_buf + 4);
+	err = brcmf_inform_single_bss(cfg_priv, bi);
+	if (err)
+		goto update_bss_info_out;
+
+	ie = ((u8 *)bi) + le16_to_cpu(bi->ie_offset);
+	ie_len = le32_to_cpu(bi->ie_length);
+	beacon_interval = le16_to_cpu(bi->beacon_period);
+
+	tim = brcmf_parse_tlvs(ie, ie_len, WLAN_EID_TIM);
+	if (tim)
+		dtim_period = tim->data[1];
+	else {
+		/*
+		* active scan was done so we could not get dtim
+		* information out of probe response.
+		* so we speficially query dtim information to dongle.
+		*/
+		u32 var;
+		err = brcmf_dev_intvar_get(cfg_to_ndev(cfg_priv),
+					   "dtim_assoc", &var);
+		if (err) {
+			WL_ERR("wl dtim_assoc failed (%d)\n", err);
+			goto update_bss_info_out;
+		}
+		dtim_period = (u8)var;
+	}
+
+	brcmf_update_prof(cfg_priv, NULL, &beacon_interval, WL_PROF_BEACONINT);
+	brcmf_update_prof(cfg_priv, NULL, &dtim_period, WL_PROF_DTIMPERIOD);
+
+update_bss_info_out:
+	WL_TRACE("Exit");
+	return err;
+}
+
+static void brcmf_term_iscan(struct brcmf_cfg80211_priv *cfg_priv)
+{
+	struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_to_iscan(cfg_priv);
+	struct brcmf_ssid ssid;
+
+	if (cfg_priv->iscan_on) {
+		iscan->state = WL_ISCAN_STATE_IDLE;
+
+		if (iscan->timer_on) {
+			del_timer_sync(&iscan->timer);
+			iscan->timer_on = 0;
+		}
+
+		cancel_work_sync(&iscan->work);
+
+		/* Abort iscan running in FW */
+		memset(&ssid, 0, sizeof(ssid));
+		brcmf_run_iscan(iscan, &ssid, WL_SCAN_ACTION_ABORT);
+	}
+}
+
+static void brcmf_notify_iscan_complete(struct brcmf_cfg80211_iscan_ctrl *iscan,
+					bool aborted)
+{
+	struct brcmf_cfg80211_priv *cfg_priv = iscan_to_cfg(iscan);
+	struct net_device *ndev = cfg_to_ndev(cfg_priv);
+
+	if (!test_and_clear_bit(WL_STATUS_SCANNING, &cfg_priv->status)) {
+		WL_ERR("Scan complete while device not scanning\n");
+		return;
+	}
+	if (cfg_priv->scan_request) {
+		WL_SCAN("ISCAN Completed scan: %s\n",
+				aborted ? "Aborted" : "Done");
+		cfg80211_scan_done(cfg_priv->scan_request, aborted);
+		brcmf_set_mpc(ndev, 1);
+		cfg_priv->scan_request = NULL;
+	}
+	cfg_priv->iscan_kickstart = false;
+}
+
+static s32 brcmf_wakeup_iscan(struct brcmf_cfg80211_iscan_ctrl *iscan)
+{
+	if (iscan->state != WL_ISCAN_STATE_IDLE) {
+		WL_SCAN("wake up iscan\n");
+		schedule_work(&iscan->work);
+		return 0;
+	}
+
+	return -EIO;
+}
+
+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;
+	s32 err = 0;
+
+	memset(iscan->scan_buf, 0, WL_ISCAN_BUF_MAX);
+	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;
+
+	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);
+	if (err) {
+		WL_ERR("error (%d)\n", err);
+		return err;
+	}
+	results->buflen = le32_to_cpu(results_le->buflen);
+	results->version = le32_to_cpu(results_le->version);
+	results->count = le32_to_cpu(results_le->count);
+	WL_SCAN("results->count = %d\n", results_le->count);
+	WL_SCAN("results->buflen = %d\n", results_le->buflen);
+	*status = le32_to_cpu(list_buf->status_le);
+	WL_SCAN("status = %d\n", *status);
+	*bss_list = results;
+
+	return err;
+}
+
+static s32 brcmf_iscan_done(struct brcmf_cfg80211_priv *cfg_priv)
+{
+	struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_priv->iscan;
+	s32 err = 0;
+
+	iscan->state = WL_ISCAN_STATE_IDLE;
+	brcmf_inform_bss(cfg_priv);
+	brcmf_notify_iscan_complete(iscan, false);
+
+	return err;
+}
+
+static s32 brcmf_iscan_pending(struct brcmf_cfg80211_priv *cfg_priv)
+{
+	struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_priv->iscan;
+	s32 err = 0;
+
+	/* Reschedule the timer */
+	mod_timer(&iscan->timer, jiffies + iscan->timer_ms * HZ / 1000);
+	iscan->timer_on = 1;
+
+	return err;
+}
+
+static s32 brcmf_iscan_inprogress(struct brcmf_cfg80211_priv *cfg_priv)
+{
+	struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_priv->iscan;
+	s32 err = 0;
+
+	brcmf_inform_bss(cfg_priv);
+	brcmf_run_iscan(iscan, NULL, BRCMF_SCAN_ACTION_CONTINUE);
+	/* Reschedule the timer */
+	mod_timer(&iscan->timer, jiffies + iscan->timer_ms * HZ / 1000);
+	iscan->timer_on = 1;
+
+	return err;
+}
+
+static s32 brcmf_iscan_aborted(struct brcmf_cfg80211_priv *cfg_priv)
+{
+	struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_priv->iscan;
+	s32 err = 0;
+
+	iscan->state = WL_ISCAN_STATE_IDLE;
+	brcmf_notify_iscan_complete(iscan, true);
+
+	return err;
+}
+
+static void brcmf_cfg80211_iscan_handler(struct work_struct *work)
+{
+	struct brcmf_cfg80211_iscan_ctrl *iscan =
+			container_of(work, struct brcmf_cfg80211_iscan_ctrl,
+				     work);
+	struct brcmf_cfg80211_priv *cfg_priv = iscan_to_cfg(iscan);
+	struct brcmf_cfg80211_iscan_eloop *el = &iscan->el;
+	u32 status = BRCMF_SCAN_RESULTS_PARTIAL;
+
+	if (iscan->timer_on) {
+		del_timer_sync(&iscan->timer);
+		iscan->timer_on = 0;
+	}
+
+	if (brcmf_get_iscan_results(iscan, &status, &cfg_priv->bss_list)) {
+		status = BRCMF_SCAN_RESULTS_ABORTED;
+		WL_ERR("Abort iscan\n");
+	}
+
+	el->handler[status](cfg_priv);
+}
+
+static void brcmf_iscan_timer(unsigned long data)
+{
+	struct brcmf_cfg80211_iscan_ctrl *iscan =
+			(struct brcmf_cfg80211_iscan_ctrl *)data;
+
+	if (iscan) {
+		iscan->timer_on = 0;
+		WL_SCAN("timer expired\n");
+		brcmf_wakeup_iscan(iscan);
+	}
+}
+
+static s32 brcmf_invoke_iscan(struct brcmf_cfg80211_priv *cfg_priv)
+{
+	struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_to_iscan(cfg_priv);
+
+	if (cfg_priv->iscan_on) {
+		iscan->state = WL_ISCAN_STATE_IDLE;
+		INIT_WORK(&iscan->work, brcmf_cfg80211_iscan_handler);
+	}
+
+	return 0;
+}
+
+static void brcmf_init_iscan_eloop(struct brcmf_cfg80211_iscan_eloop *el)
+{
+	memset(el, 0, sizeof(*el));
+	el->handler[BRCMF_SCAN_RESULTS_SUCCESS] = brcmf_iscan_done;
+	el->handler[BRCMF_SCAN_RESULTS_PARTIAL] = brcmf_iscan_inprogress;
+	el->handler[BRCMF_SCAN_RESULTS_PENDING] = brcmf_iscan_pending;
+	el->handler[BRCMF_SCAN_RESULTS_ABORTED] = brcmf_iscan_aborted;
+	el->handler[BRCMF_SCAN_RESULTS_NO_MEM] = brcmf_iscan_aborted;
+}
+
+static s32 brcmf_init_iscan(struct brcmf_cfg80211_priv *cfg_priv)
+{
+	struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_to_iscan(cfg_priv);
+	int err = 0;
+
+	if (cfg_priv->iscan_on) {
+		iscan->ndev = cfg_to_ndev(cfg_priv);
+		brcmf_init_iscan_eloop(&iscan->el);
+		iscan->timer_ms = WL_ISCAN_TIMER_INTERVAL_MS;
+		init_timer(&iscan->timer);
+		iscan->timer.data = (unsigned long) iscan;
+		iscan->timer.function = brcmf_iscan_timer;
+		err = brcmf_invoke_iscan(cfg_priv);
+		if (!err)
+			iscan->data = cfg_priv;
+	}
+
+	return err;
+}
+
+static void brcmf_delay(u32 ms)
+{
+	if (ms < 1000 / HZ) {
+		cond_resched();
+		mdelay(ms);
+	} else {
+		msleep(ms);
+	}
+}
+
 static s32 brcmf_cfg80211_resume(struct wiphy *wiphy)
 {
 	struct brcmf_cfg80211_priv *cfg_priv = wiphy_to_cfg(wiphy);
@@ -2090,10 +2502,6 @@ static s32 brcmf_cfg80211_resume(struct wiphy *wiphy)
 	 */
 	WL_TRACE("Enter\n");
 
-#if defined(CONFIG_PM_SLEEP)
-	atomic_set(&brcmf_mmc_suspend, false);
-#endif	/*  defined(CONFIG_PM_SLEEP) */
-
 	if (test_bit(WL_STATUS_READY, &cfg_priv->status))
 		brcmf_invoke_iscan(wiphy_to_cfg(wiphy));
 
@@ -2131,9 +2539,7 @@ static s32 brcmf_cfg80211_suspend(struct wiphy *wiphy,
 		 * generated due to DISASSOC call to the fw to keep
 		 * the state fw and WPA_Supplicant state consistent
 		 */
-		rtnl_unlock();
 		brcmf_delay(500);
-		rtnl_lock();
 	}
 
 	set_bit(WL_STATUS_SCAN_ABORTING, &cfg_priv->status);
@@ -2155,81 +2561,123 @@ static s32 brcmf_cfg80211_suspend(struct wiphy *wiphy,
 		brcmf_set_mpc(ndev, 1);
 	}
 
-#if defined(CONFIG_PM_SLEEP)
-	atomic_set(&brcmf_mmc_suspend, true);
-#endif	/*  defined(CONFIG_PM_SLEEP) */
-
 	WL_TRACE("Exit\n");
 
 	return 0;
 }
 
 static __used s32
-brcmf_update_pmklist(struct net_device *dev,
+brcmf_dev_bufvar_set(struct net_device *ndev, s8 *name, s8 *buf, s32 len)
+{
+	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(ndev);
+	u32 buflen;
+
+	buflen = brcmf_c_mkiovar(name, buf, len, cfg_priv->dcmd_buf,
+			       WL_DCMD_LEN_MAX);
+	BUG_ON(!buflen);
+
+	return brcmf_exec_dcmd(ndev, BRCMF_C_SET_VAR, cfg_priv->dcmd_buf,
+			       buflen);
+}
+
+static s32
+brcmf_dev_bufvar_get(struct net_device *ndev, s8 *name, s8 *buf,
+		  s32 buf_len)
+{
+	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(ndev);
+	u32 len;
+	s32 err = 0;
+
+	len = brcmf_c_mkiovar(name, NULL, 0, cfg_priv->dcmd_buf,
+			    WL_DCMD_LEN_MAX);
+	BUG_ON(!len);
+	err = brcmf_exec_dcmd(ndev, BRCMF_C_GET_VAR, cfg_priv->dcmd_buf,
+			      WL_DCMD_LEN_MAX);
+	if (err) {
+		WL_ERR("error (%d)\n", err);
+		return err;
+	}
+	memcpy(buf, cfg_priv->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)
 {
 	int i, j;
+	int pmkid_len;
 
-	WL_CONN("No of elements %d\n", pmk_list->pmkids.npmkid);
-	for (i = 0; i < pmk_list->pmkids.npmkid; i++) {
+	pmkid_len = le32_to_cpu(pmk_list->pmkids.npmkid);
+
+	WL_CONN("No of elements %d\n", pmkid_len);
+	for (i = 0; i < pmkid_len; i++) {
 		WL_CONN("PMKID[%d]: %pM =\n", i,
 			&pmk_list->pmkids.pmkid[i].BSSID);
 		for (j = 0; j < WLAN_PMKID_LEN; j++)
 			WL_CONN("%02x\n", pmk_list->pmkids.pmkid[i].PMKID[j]);
 	}
 
-	if (likely(!err))
-		brcmf_dev_bufvar_set(dev, "pmkid_info", (char *)pmk_list,
+	if (!err)
+		brcmf_dev_bufvar_set(ndev, "pmkid_info", (char *)pmk_list,
 					sizeof(*pmk_list));
 
 	return err;
 }
 
 static s32
-brcmf_cfg80211_set_pmksa(struct wiphy *wiphy, struct net_device *dev,
+brcmf_cfg80211_set_pmksa(struct wiphy *wiphy, struct net_device *ndev,
 			 struct cfg80211_pmksa *pmksa)
 {
 	struct brcmf_cfg80211_priv *cfg_priv = wiphy_to_cfg(wiphy);
-	struct _pmkid_list *pmkids = &cfg_priv->pmk_list->pmkids;
+	struct pmkid_list *pmkids = &cfg_priv->pmk_list->pmkids;
 	s32 err = 0;
 	int i;
+	int pmkid_len;
 
 	WL_TRACE("Enter\n");
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
-	for (i = 0; i < pmkids->npmkid; i++)
+	pmkid_len = le32_to_cpu(pmkids->npmkid);
+	for (i = 0; i < pmkid_len; i++)
 		if (!memcmp(pmksa->bssid, pmkids->pmkid[i].BSSID, ETH_ALEN))
 			break;
 	if (i < WL_NUM_PMKIDS_MAX) {
 		memcpy(pmkids->pmkid[i].BSSID, pmksa->bssid, ETH_ALEN);
 		memcpy(pmkids->pmkid[i].PMKID, pmksa->pmkid, WLAN_PMKID_LEN);
-		if (i == pmkids->npmkid)
-			pmkids->npmkid++;
+		if (i == pmkid_len) {
+			pmkid_len++;
+			pmkids->npmkid = cpu_to_le32(pmkid_len);
+		}
 	} else
 		err = -EINVAL;
 
 	WL_CONN("set_pmksa,IW_PMKSA_ADD - PMKID: %pM =\n",
-		pmkids->pmkid[pmkids->npmkid].BSSID);
+		pmkids->pmkid[pmkid_len].BSSID);
 	for (i = 0; i < WLAN_PMKID_LEN; i++)
-		WL_CONN("%02x\n", pmkids->pmkid[pmkids->npmkid].PMKID[i]);
+		WL_CONN("%02x\n", pmkids->pmkid[pmkid_len].PMKID[i]);
 
-	err = brcmf_update_pmklist(dev, cfg_priv->pmk_list, err);
+	err = brcmf_update_pmklist(ndev, cfg_priv->pmk_list, err);
 
 	WL_TRACE("Exit\n");
 	return err;
 }
 
 static s32
-brcmf_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *dev,
+brcmf_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *ndev,
 		      struct cfg80211_pmksa *pmksa)
 {
 	struct brcmf_cfg80211_priv *cfg_priv = wiphy_to_cfg(wiphy);
-	struct _pmkid_list pmkid;
+	struct pmkid_list pmkid;
 	s32 err = 0;
-	int i;
+	int i, pmkid_len;
 
 	WL_TRACE("Enter\n");
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
+
 	memcpy(&pmkid.pmkid[0].BSSID, pmksa->bssid, ETH_ALEN);
 	memcpy(&pmkid.pmkid[0].PMKID, pmksa->pmkid, WLAN_PMKID_LEN);
 
@@ -2238,17 +2686,18 @@ brcmf_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *dev,
 	for (i = 0; i < WLAN_PMKID_LEN; i++)
 		WL_CONN("%02x\n", pmkid.pmkid[0].PMKID[i]);
 
-	for (i = 0; i < cfg_priv->pmk_list->pmkids.npmkid; i++)
+	pmkid_len = le32_to_cpu(cfg_priv->pmk_list->pmkids.npmkid);
+	for (i = 0; i < pmkid_len; i++)
 		if (!memcmp
 		    (pmksa->bssid, &cfg_priv->pmk_list->pmkids.pmkid[i].BSSID,
 		     ETH_ALEN))
 			break;
 
-	if ((cfg_priv->pmk_list->pmkids.npmkid > 0)
-	    && (i < cfg_priv->pmk_list->pmkids.npmkid)) {
+	if ((pmkid_len > 0)
+	    && (i < pmkid_len)) {
 		memset(&cfg_priv->pmk_list->pmkids.pmkid[i], 0,
-		       sizeof(pmkid_t));
-		for (; i < (cfg_priv->pmk_list->pmkids.npmkid - 1); i++) {
+		       sizeof(struct pmkid));
+		for (; i < (pmkid_len - 1); i++) {
 			memcpy(&cfg_priv->pmk_list->pmkids.pmkid[i].BSSID,
 			       &cfg_priv->pmk_list->pmkids.pmkid[i + 1].BSSID,
 			       ETH_ALEN);
@@ -2256,11 +2705,11 @@ brcmf_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *dev,
 			       &cfg_priv->pmk_list->pmkids.pmkid[i + 1].PMKID,
 			       WLAN_PMKID_LEN);
 		}
-		cfg_priv->pmk_list->pmkids.npmkid--;
+		cfg_priv->pmk_list->pmkids.npmkid = cpu_to_le32(pmkid_len - 1);
 	} else
 		err = -EINVAL;
 
-	err = brcmf_update_pmklist(dev, cfg_priv->pmk_list, err);
+	err = brcmf_update_pmklist(ndev, cfg_priv->pmk_list, err);
 
 	WL_TRACE("Exit\n");
 	return err;
@@ -2268,16 +2717,17 @@ brcmf_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *dev,
 }
 
 static s32
-brcmf_cfg80211_flush_pmksa(struct wiphy *wiphy, struct net_device *dev)
+brcmf_cfg80211_flush_pmksa(struct wiphy *wiphy, struct net_device *ndev)
 {
 	struct brcmf_cfg80211_priv *cfg_priv = wiphy_to_cfg(wiphy);
 	s32 err = 0;
 
 	WL_TRACE("Enter\n");
-	CHECK_SYS_UP();
+	if (!check_sys_up(wiphy))
+		return -EIO;
 
 	memset(cfg_priv->pmk_list, 0, sizeof(*cfg_priv->pmk_list));
-	err = brcmf_update_pmklist(dev, cfg_priv->pmk_list, err);
+	err = brcmf_update_pmklist(ndev, cfg_priv->pmk_list, err);
 
 	WL_TRACE("Exit\n");
 	return err;
@@ -2326,25 +2776,24 @@ static s32 brcmf_mode_to_nl80211_iftype(s32 mode)
 }
 
 static struct wireless_dev *brcmf_alloc_wdev(s32 sizeof_iface,
-					  struct device *dev)
+					  struct device *ndev)
 {
 	struct wireless_dev *wdev;
 	s32 err = 0;
 
 	wdev = kzalloc(sizeof(*wdev), GFP_KERNEL);
-	if (unlikely(!wdev)) {
-		WL_ERR("Could not allocate wireless device\n");
+	if (!wdev)
 		return ERR_PTR(-ENOMEM);
-	}
+
 	wdev->wiphy =
 	    wiphy_new(&wl_cfg80211_ops,
 		      sizeof(struct brcmf_cfg80211_priv) + sizeof_iface);
-	if (unlikely(!wdev->wiphy)) {
+	if (!wdev->wiphy) {
 		WL_ERR("Couldn not allocate wiphy device\n");
 		err = -ENOMEM;
 		goto wiphy_new_out;
 	}
-	set_wiphy_dev(wdev->wiphy, dev);
+	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 =
@@ -2360,16 +2809,12 @@ static struct wireless_dev *brcmf_alloc_wdev(s32 sizeof_iface,
 	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);
-#ifndef WL_POWERSAVE_DISABLED
 	wdev->wiphy->flags |= WIPHY_FLAG_PS_ON_BY_DEFAULT;	/* enable power
 								 * save mode
 								 * by default
 								 */
-#else
-	wdev->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
-#endif				/* !WL_POWERSAVE_DISABLED */
 	err = wiphy_register(wdev->wiphy);
-	if (unlikely(err < 0)) {
+	if (err < 0) {
 		WL_ERR("Couldn not register wiphy device (%d)\n", err);
 		goto wiphy_register_out;
 	}
@@ -2386,175 +2831,16 @@ wiphy_new_out:
 
 static void brcmf_free_wdev(struct brcmf_cfg80211_priv *cfg_priv)
 {
-	struct wireless_dev *wdev = cfg_to_wdev(cfg_priv);
+	struct wireless_dev *wdev = cfg_priv->wdev;
 
-	if (unlikely(!wdev)) {
+	if (!wdev) {
 		WL_ERR("wdev is invalid\n");
 		return;
 	}
 	wiphy_unregister(wdev->wiphy);
 	wiphy_free(wdev->wiphy);
 	kfree(wdev);
-	cfg_to_wdev(cfg_priv) = NULL;
-}
-
-static s32 brcmf_inform_bss(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	struct brcmf_scan_results *bss_list;
-	struct brcmf_bss_info *bi = NULL;	/* must be initialized */
-	s32 err = 0;
-	int i;
-
-	bss_list = cfg_priv->bss_list;
-	if (unlikely(bss_list->version != BRCMF_BSS_INFO_VERSION)) {
-		WL_ERR("Version %d != WL_BSS_INFO_VERSION\n",
-		       bss_list->version);
-		return -EOPNOTSUPP;
-	}
-	WL_SCAN("scanned AP count (%d)\n", bss_list->count);
-	bi = next_bss(bss_list, bi);
-	for_each_bss(bss_list, bi, i) {
-		err = brcmf_inform_single_bss(cfg_priv, bi);
-		if (unlikely(err))
-			break;
-	}
-	return err;
-}
-
-
-static s32 brcmf_inform_single_bss(struct brcmf_cfg80211_priv *cfg_priv,
-				   struct brcmf_bss_info *bi)
-{
-	struct wiphy *wiphy = cfg_to_wiphy(cfg_priv);
-	struct ieee80211_channel *notify_channel;
-	struct cfg80211_bss *bss;
-	struct ieee80211_supported_band *band;
-	s32 err = 0;
-	u16 channel;
-	u32 freq;
-	u64 notify_timestamp;
-	u16 notify_capability;
-	u16 notify_interval;
-	u8 *notify_ie;
-	size_t notify_ielen;
-	s32 notify_signal;
-
-	if (unlikely(le32_to_cpu(bi->length) > WL_BSS_INFO_MAX)) {
-		WL_ERR("Bss info is larger than buffer. Discarding\n");
-		return 0;
-	}
-
-	channel = bi->ctl_ch ? bi->ctl_ch :
-				CHSPEC_CHANNEL(le16_to_cpu(bi->chanspec));
-
-	if (channel <= CH_MAX_2G_CHANNEL)
-		band = wiphy->bands[IEEE80211_BAND_2GHZ];
-	else
-		band = wiphy->bands[IEEE80211_BAND_5GHZ];
-
-	freq = ieee80211_channel_to_frequency(channel, band->band);
-	notify_channel = ieee80211_get_channel(wiphy, freq);
-
-	notify_timestamp = jiffies_to_msecs(jiffies)*1000; /* uSec */
-	notify_capability = le16_to_cpu(bi->capability);
-	notify_interval = le16_to_cpu(bi->beacon_period);
-	notify_ie = (u8 *)bi + le16_to_cpu(bi->ie_offset);
-	notify_ielen = le16_to_cpu(bi->ie_length);
-	notify_signal = (s16)le16_to_cpu(bi->RSSI) * 100;
-
-	WL_CONN("bssid: %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n",
-			bi->BSSID[0], bi->BSSID[1], bi->BSSID[2],
-			bi->BSSID[3], bi->BSSID[4], bi->BSSID[5]);
-	WL_CONN("Channel: %d(%d)\n", channel, freq);
-	WL_CONN("Capability: %X\n", notify_capability);
-	WL_CONN("Beacon interval: %d\n", notify_interval);
-	WL_CONN("Signal: %d\n", notify_signal);
-	WL_CONN("notify_timestamp: %#018llx\n", notify_timestamp);
-
-	bss = cfg80211_inform_bss(wiphy, notify_channel, (const u8 *)bi->BSSID,
-		notify_timestamp, notify_capability, notify_interval, notify_ie,
-		notify_ielen, notify_signal, GFP_KERNEL);
-
-	if (unlikely(!bss)) {
-		WL_ERR("cfg80211_inform_bss_frame error\n");
-		return -EINVAL;
-	}
-
-	return err;
-}
-
-static s32 wl_inform_ibss(struct brcmf_cfg80211_priv *cfg_priv,
-			  struct net_device *dev, const u8 *bssid)
-{
-	struct wiphy *wiphy = cfg_to_wiphy(cfg_priv);
-	struct ieee80211_channel *notify_channel;
-	struct brcmf_bss_info *bi = NULL;
-	struct ieee80211_supported_band *band;
-	u8 *buf = NULL;
-	s32 err = 0;
-	u16 channel;
-	u32 freq;
-	u64 notify_timestamp;
-	u16 notify_capability;
-	u16 notify_interval;
-	u8 *notify_ie;
-	size_t notify_ielen;
-	s32 notify_signal;
-
-	WL_TRACE("Enter\n");
-
-	buf = kzalloc(WL_BSS_INFO_MAX, GFP_KERNEL);
-	if (buf == NULL) {
-		WL_ERR("kzalloc() failed\n");
-		err = -ENOMEM;
-		goto CleanUp;
-	}
-
-	*(u32 *)buf = cpu_to_le32(WL_BSS_INFO_MAX);
-
-	err = brcmf_dev_ioctl(dev, BRCMF_C_GET_BSS_INFO, buf, WL_BSS_INFO_MAX);
-	if (unlikely(err)) {
-		WL_ERR("WLC_GET_BSS_INFO failed: %d\n", err);
-		goto CleanUp;
-	}
-
-	bi = (struct brcmf_bss_info *)(buf + 4);
-
-	channel = bi->ctl_ch ? bi->ctl_ch :
-				CHSPEC_CHANNEL(le16_to_cpu(bi->chanspec));
-
-	if (channel <= CH_MAX_2G_CHANNEL)
-		band = wiphy->bands[IEEE80211_BAND_2GHZ];
-	else
-		band = wiphy->bands[IEEE80211_BAND_5GHZ];
-
-	freq = ieee80211_channel_to_frequency(channel, band->band);
-	notify_channel = ieee80211_get_channel(wiphy, freq);
-
-	notify_timestamp = jiffies_to_msecs(jiffies)*1000; /* uSec */
-	notify_capability = le16_to_cpu(bi->capability);
-	notify_interval = le16_to_cpu(bi->beacon_period);
-	notify_ie = (u8 *)bi + le16_to_cpu(bi->ie_offset);
-	notify_ielen = le16_to_cpu(bi->ie_length);
-	notify_signal = (s16)le16_to_cpu(bi->RSSI) * 100;
-
-	WL_CONN("channel: %d(%d)\n", channel, freq);
-	WL_CONN("capability: %X\n", notify_capability);
-	WL_CONN("beacon interval: %d\n", notify_interval);
-	WL_CONN("signal: %d\n", notify_signal);
-	WL_CONN("notify_timestamp: %#018llx\n", notify_timestamp);
-
-	cfg80211_inform_bss(wiphy, notify_channel, bssid,
-		notify_timestamp, notify_capability, notify_interval,
-		notify_ie, notify_ielen, notify_signal, GFP_KERNEL);
-
-CleanUp:
-
-	kfree(buf);
-
-	WL_TRACE("Exit\n");
-
-	return err;
+	cfg_priv->wdev = NULL;
 }
 
 static bool brcmf_is_linkup(struct brcmf_cfg80211_priv *cfg_priv,
@@ -2606,110 +2892,22 @@ static bool brcmf_is_nonetwork(struct brcmf_cfg80211_priv *cfg_priv,
 	return false;
 }
 
-static s32
-brcmf_notify_connect_status(struct brcmf_cfg80211_priv *cfg_priv,
-			    struct net_device *ndev,
-			    const struct brcmf_event_msg *e, void *data)
-{
-	s32 err = 0;
-
-	if (brcmf_is_linkup(cfg_priv, e)) {
-		WL_CONN("Linkup\n");
-		if (brcmf_is_ibssmode(cfg_priv)) {
-			brcmf_update_prof(cfg_priv, NULL, (void *)e->addr,
-				WL_PROF_BSSID);
-			wl_inform_ibss(cfg_priv, ndev, e->addr);
-			cfg80211_ibss_joined(ndev, e->addr, GFP_KERNEL);
-			clear_bit(WL_STATUS_CONNECTING, &cfg_priv->status);
-			set_bit(WL_STATUS_CONNECTED, &cfg_priv->status);
-		} else
-			brcmf_bss_connect_done(cfg_priv, ndev, e, data, true);
-	} else if (brcmf_is_linkdown(cfg_priv, e)) {
-		WL_CONN("Linkdown\n");
-		if (brcmf_is_ibssmode(cfg_priv)) {
-			clear_bit(WL_STATUS_CONNECTING, &cfg_priv->status);
-			if (test_and_clear_bit(WL_STATUS_CONNECTED,
-				&cfg_priv->status))
-				brcmf_link_down(cfg_priv);
-		} else {
-			brcmf_bss_connect_done(cfg_priv, ndev, e, data, false);
-			if (test_and_clear_bit(WL_STATUS_CONNECTED,
-				&cfg_priv->status)) {
-				cfg80211_disconnected(ndev, 0, NULL, 0,
-					GFP_KERNEL);
-				brcmf_link_down(cfg_priv);
-			}
-		}
-		brcmf_init_prof(cfg_priv->profile);
-	} else if (brcmf_is_nonetwork(cfg_priv, e)) {
-		if (brcmf_is_ibssmode(cfg_priv))
-			clear_bit(WL_STATUS_CONNECTING, &cfg_priv->status);
-		else
-			brcmf_bss_connect_done(cfg_priv, ndev, e, data, false);
-	}
-
-	return err;
-}
-
-static s32
-brcmf_notify_roaming_status(struct brcmf_cfg80211_priv *cfg_priv,
-			    struct net_device *ndev,
-			    const struct brcmf_event_msg *e, void *data)
-{
-	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_priv->status))
-			brcmf_bss_roaming_done(cfg_priv, ndev, e, data);
-		else
-			brcmf_bss_connect_done(cfg_priv, ndev, e, data, true);
-	}
-
-	return err;
-}
-
-static __used s32
-brcmf_dev_bufvar_set(struct net_device *dev, s8 *name, s8 *buf, s32 len)
-{
-	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(dev);
-	u32 buflen;
-
-	buflen = brcmu_mkiovar(name, buf, len, cfg_priv->ioctl_buf,
-			       WL_IOCTL_LEN_MAX);
-	BUG_ON(!buflen);
-
-	return brcmf_dev_ioctl(dev, BRCMF_C_SET_VAR, cfg_priv->ioctl_buf,
-			       buflen);
-}
-
-static s32
-brcmf_dev_bufvar_get(struct net_device *dev, s8 *name, s8 *buf,
-		  s32 buf_len)
+static void brcmf_clear_assoc_ies(struct brcmf_cfg80211_priv *cfg_priv)
 {
-	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(dev);
-	u32 len;
-	s32 err = 0;
-
-	len = brcmu_mkiovar(name, NULL, 0, cfg_priv->ioctl_buf,
-			    WL_IOCTL_LEN_MAX);
-	BUG_ON(!len);
-	err = brcmf_dev_ioctl(dev, BRCMF_C_GET_VAR, (void *)cfg_priv->ioctl_buf,
-			WL_IOCTL_LEN_MAX);
-	if (unlikely(err)) {
-		WL_ERR("error (%d)\n", err);
-		return err;
-	}
-	memcpy(buf, cfg_priv->ioctl_buf, buf_len);
+	struct brcmf_cfg80211_connect_info *conn_info = cfg_to_conn(cfg_priv);
 
-	return err;
+	kfree(conn_info->req_ie);
+	conn_info->req_ie = NULL;
+	conn_info->req_ie_len = 0;
+	kfree(conn_info->resp_ie);
+	conn_info->resp_ie = NULL;
+	conn_info->resp_ie_len = 0;
 }
 
 static s32 brcmf_get_assoc_ies(struct brcmf_cfg80211_priv *cfg_priv)
 {
 	struct net_device *ndev = cfg_to_ndev(cfg_priv);
-	struct brcmf_cfg80211_assoc_ielen *assoc_info;
+	struct brcmf_cfg80211_assoc_ielen_le *assoc_info;
 	struct brcmf_cfg80211_connect_info *conn_info = cfg_to_conn(cfg_priv);
 	u32 req_len;
 	u32 resp_len;
@@ -2719,18 +2917,19 @@ static s32 brcmf_get_assoc_ies(struct brcmf_cfg80211_priv *cfg_priv)
 
 	err = brcmf_dev_bufvar_get(ndev, "assoc_info", cfg_priv->extra_buf,
 				WL_ASSOC_INFO_MAX);
-	if (unlikely(err)) {
+	if (err) {
 		WL_ERR("could not get assoc info (%d)\n", err);
 		return err;
 	}
-	assoc_info = (struct brcmf_cfg80211_assoc_ielen *)cfg_priv->extra_buf;
-	req_len = assoc_info->req_len;
-	resp_len = assoc_info->resp_len;
+	assoc_info =
+		(struct brcmf_cfg80211_assoc_ielen_le *)cfg_priv->extra_buf;
+	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_priv->extra_buf,
 					   WL_ASSOC_INFO_MAX);
-		if (unlikely(err)) {
+		if (err) {
 			WL_ERR("could not get assoc req (%d)\n", err);
 			return err;
 		}
@@ -2746,7 +2945,7 @@ static s32 brcmf_get_assoc_ies(struct brcmf_cfg80211_priv *cfg_priv)
 		err = brcmf_dev_bufvar_get(ndev, "assoc_resp_ies",
 					   cfg_priv->extra_buf,
 					   WL_ASSOC_INFO_MAX);
-		if (unlikely(err)) {
+		if (err) {
 			WL_ERR("could not get assoc resp (%d)\n", err);
 			return err;
 		}
@@ -2764,121 +2963,19 @@ static s32 brcmf_get_assoc_ies(struct brcmf_cfg80211_priv *cfg_priv)
 	return err;
 }
 
-static void brcmf_clear_assoc_ies(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	struct brcmf_cfg80211_connect_info *conn_info = cfg_to_conn(cfg_priv);
-
-	kfree(conn_info->req_ie);
-	conn_info->req_ie = NULL;
-	conn_info->req_ie_len = 0;
-	kfree(conn_info->resp_ie);
-	conn_info->resp_ie = NULL;
-	conn_info->resp_ie_len = 0;
-}
-
-
-static void brcmf_ch_to_chanspec(int ch, struct brcmf_join_params *join_params,
-	size_t *join_params_size)
-{
-	chanspec_t chanspec = 0;
-
-	if (ch != 0) {
-		join_params->params.chanspec_num = 1;
-		join_params->params.chanspec_list[0] = ch;
-
-		if (join_params->params.chanspec_list[0] <= CH_MAX_2G_CHANNEL)
-			chanspec |= WL_CHANSPEC_BAND_2G;
-		else
-			chanspec |= WL_CHANSPEC_BAND_5G;
-
-		chanspec |= WL_CHANSPEC_BW_20;
-		chanspec |= WL_CHANSPEC_CTL_SB_NONE;
-
-		*join_params_size += BRCMF_ASSOC_PARAMS_FIXED_SIZE +
-			join_params->params.chanspec_num * sizeof(chanspec_t);
-
-		join_params->params.chanspec_list[0] &= WL_CHANSPEC_CHAN_MASK;
-		join_params->params.chanspec_list[0] |= chanspec;
-		join_params->params.chanspec_list[0] =
-		cpu_to_le16(join_params->params.chanspec_list[0]);
-
-		join_params->params.chanspec_num =
-			cpu_to_le32(join_params->params.chanspec_num);
-
-		WL_CONN("join_params->params.chanspec_list[0]= %#X,"
-			"channel %d, chanspec %#X\n",
-		       join_params->params.chanspec_list[0], ch, chanspec);
-	}
-}
-
-static s32 brcmf_update_bss_info(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	struct brcmf_bss_info *bi;
-	struct brcmf_ssid *ssid;
-	struct brcmu_tlv *tim;
-	u16 beacon_interval;
-	u8 dtim_period;
-	size_t ie_len;
-	u8 *ie;
-	s32 err = 0;
-
-	WL_TRACE("Enter\n");
-	if (brcmf_is_ibssmode(cfg_priv))
-		return err;
-
-	ssid = (struct brcmf_ssid *)brcmf_read_prof(cfg_priv, WL_PROF_SSID);
-
-	*(u32 *)cfg_priv->extra_buf = cpu_to_le32(WL_EXTRA_BUF_MAX);
-	err = brcmf_dev_ioctl(cfg_to_ndev(cfg_priv), BRCMF_C_GET_BSS_INFO,
-			cfg_priv->extra_buf, WL_EXTRA_BUF_MAX);
-	if (unlikely(err)) {
-		WL_ERR("Could not get bss info %d\n", err);
-		goto update_bss_info_out;
-	}
-
-	bi = (struct brcmf_bss_info *)(cfg_priv->extra_buf + 4);
-	err = brcmf_inform_single_bss(cfg_priv, bi);
-	if (unlikely(err))
-		goto update_bss_info_out;
-
-	ie = ((u8 *)bi) + bi->ie_offset;
-	ie_len = bi->ie_length;
-	beacon_interval = cpu_to_le16(bi->beacon_period);
-
-	tim = brcmu_parse_tlvs(ie, ie_len, WLAN_EID_TIM);
-	if (tim)
-		dtim_period = tim->data[1];
-	else {
-		/*
-		* active scan was done so we could not get dtim
-		* information out of probe response.
-		* so we speficially query dtim information to dongle.
-		*/
-		u32 var;
-		err = brcmf_dev_intvar_get(cfg_to_ndev(cfg_priv),
-					   "dtim_assoc", &var);
-		if (unlikely(err)) {
-			WL_ERR("wl dtim_assoc failed (%d)\n", err);
-			goto update_bss_info_out;
-		}
-		dtim_period = (u8)var;
-	}
-
-	brcmf_update_prof(cfg_priv, NULL, &beacon_interval, WL_PROF_BEACONINT);
-	brcmf_update_prof(cfg_priv, NULL, &dtim_period, WL_PROF_DTIMPERIOD);
-
-update_bss_info_out:
-	WL_TRACE("Exit");
-	return err;
-}
-
 static s32
 brcmf_bss_roaming_done(struct brcmf_cfg80211_priv *cfg_priv,
 		       struct net_device *ndev,
-		       const struct brcmf_event_msg *e, void *data)
+		       const struct brcmf_event_msg *e)
 {
 	struct brcmf_cfg80211_connect_info *conn_info = cfg_to_conn(cfg_priv);
+	struct wiphy *wiphy = cfg_to_wiphy(cfg_priv);
+	struct brcmf_channel_info_le channel_le;
+	struct ieee80211_channel *notify_channel;
+	struct ieee80211_supported_band *band;
+	u32 freq;
 	s32 err = 0;
+	u32 target_channel;
 
 	WL_TRACE("Enter\n");
 
@@ -2886,7 +2983,21 @@ brcmf_bss_roaming_done(struct brcmf_cfg80211_priv *cfg_priv,
 	brcmf_update_prof(cfg_priv, NULL, &e->addr, WL_PROF_BSSID);
 	brcmf_update_bss_info(cfg_priv);
 
-	cfg80211_roamed(ndev, NULL,
+	brcmf_exec_dcmd(ndev, BRCMF_C_GET_CHANNEL, &channel_le,
+			sizeof(channel_le));
+
+	target_channel = le32_to_cpu(channel_le.target_channel);
+	WL_CONN("Roamed to channel %d\n", target_channel);
+
+	if (target_channel <= CH_MAX_2G_CHANNEL)
+		band = wiphy->bands[IEEE80211_BAND_2GHZ];
+	else
+		band = wiphy->bands[IEEE80211_BAND_5GHZ];
+
+	freq = ieee80211_channel_to_frequency(target_channel, band->band);
+	notify_channel = ieee80211_get_channel(wiphy, freq);
+
+	cfg80211_roamed(ndev, notify_channel,
 			(u8 *)brcmf_read_prof(cfg_priv, WL_PROF_BSSID),
 			conn_info->req_ie, conn_info->req_ie_len,
 			conn_info->resp_ie, conn_info->resp_ie_len, GFP_KERNEL);
@@ -2900,7 +3011,7 @@ brcmf_bss_roaming_done(struct brcmf_cfg80211_priv *cfg_priv,
 static s32
 brcmf_bss_connect_done(struct brcmf_cfg80211_priv *cfg_priv,
 		       struct net_device *ndev, const struct brcmf_event_msg *e,
-		       void *data, bool completed)
+		       bool completed)
 {
 	struct brcmf_cfg80211_connect_info *conn_info = cfg_to_conn(cfg_priv);
 	s32 err = 0;
@@ -2934,6 +3045,57 @@ brcmf_bss_connect_done(struct brcmf_cfg80211_priv *cfg_priv,
 }
 
 static s32
+brcmf_notify_connect_status(struct brcmf_cfg80211_priv *cfg_priv,
+			    struct net_device *ndev,
+			    const struct brcmf_event_msg *e, void *data)
+{
+	s32 err = 0;
+
+	if (brcmf_is_linkup(cfg_priv, e)) {
+		WL_CONN("Linkup\n");
+		if (brcmf_is_ibssmode(cfg_priv)) {
+			brcmf_update_prof(cfg_priv, NULL, (void *)e->addr,
+				WL_PROF_BSSID);
+			wl_inform_ibss(cfg_priv, ndev, e->addr);
+			cfg80211_ibss_joined(ndev, e->addr, GFP_KERNEL);
+			clear_bit(WL_STATUS_CONNECTING, &cfg_priv->status);
+			set_bit(WL_STATUS_CONNECTED, &cfg_priv->status);
+		} else
+			brcmf_bss_connect_done(cfg_priv, ndev, e, true);
+	} else if (brcmf_is_linkdown(cfg_priv, e)) {
+		WL_CONN("Linkdown\n");
+		brcmf_link_down(cfg_priv);
+		brcmf_init_prof(cfg_priv->profile);
+	} else if (brcmf_is_nonetwork(cfg_priv, e)) {
+		if (brcmf_is_ibssmode(cfg_priv))
+			clear_bit(WL_STATUS_CONNECTING, &cfg_priv->status);
+		else
+			brcmf_bss_connect_done(cfg_priv, ndev, e, false);
+	}
+
+	return err;
+}
+
+static s32
+brcmf_notify_roaming_status(struct brcmf_cfg80211_priv *cfg_priv,
+			    struct net_device *ndev,
+			    const struct brcmf_event_msg *e, void *data)
+{
+	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_priv->status))
+			brcmf_bss_roaming_done(cfg_priv, ndev, e);
+		else
+			brcmf_bss_connect_done(cfg_priv, ndev, e, true);
+	}
+
+	return err;
+}
+
+static s32
 brcmf_notify_mic_status(struct brcmf_cfg80211_priv *cfg_priv,
 			struct net_device *ndev,
 			const struct brcmf_event_msg *e, void *data)
@@ -2941,7 +3103,6 @@ brcmf_notify_mic_status(struct brcmf_cfg80211_priv *cfg_priv,
 	u16 flags = be16_to_cpu(e->flags);
 	enum nl80211_key_type key_type;
 
-	rtnl_lock();
 	if (flags & BRCMF_EVENT_MSG_GROUP)
 		key_type = NL80211_KEYTYPE_GROUP;
 	else
@@ -2949,7 +3110,6 @@ brcmf_notify_mic_status(struct brcmf_cfg80211_priv *cfg_priv,
 
 	cfg80211_michael_mic_failure(ndev, (u8 *)&e->addr, key_type, -1,
 				     NULL, GFP_KERNEL);
-	rtnl_unlock();
 
 	return 0;
 }
@@ -2959,11 +3119,12 @@ brcmf_notify_scan_status(struct brcmf_cfg80211_priv *cfg_priv,
 			 struct net_device *ndev,
 			 const struct brcmf_event_msg *e, void *data)
 {
-	struct brcmf_channel_info channel_inform;
-	struct brcmf_scan_results *bss_list;
+	struct brcmf_channel_info_le channel_inform_le;
+	struct brcmf_scan_results_le *bss_list_le;
 	u32 len = WL_SCAN_BUF_MAX;
 	s32 err = 0;
 	bool scan_abort = false;
+	u32 scan_channel;
 
 	WL_TRACE("Enter\n");
 
@@ -2972,42 +3133,39 @@ brcmf_notify_scan_status(struct brcmf_cfg80211_priv *cfg_priv,
 		return brcmf_wakeup_iscan(cfg_to_iscan(cfg_priv));
 	}
 
-	if (unlikely(!test_and_clear_bit(WL_STATUS_SCANNING,
-					 &cfg_priv->status))) {
+	if (!test_and_clear_bit(WL_STATUS_SCANNING, &cfg_priv->status)) {
 		WL_ERR("Scan complete while device not scanning\n");
 		scan_abort = true;
 		err = -EINVAL;
 		goto scan_done_out;
 	}
 
-	err = brcmf_dev_ioctl(ndev, BRCMF_C_GET_CHANNEL, &channel_inform,
-			sizeof(channel_inform));
-	if (unlikely(err)) {
+	err = brcmf_exec_dcmd(ndev, BRCMF_C_GET_CHANNEL, &channel_inform_le,
+			      sizeof(channel_inform_le));
+	if (err) {
 		WL_ERR("scan busy (%d)\n", err);
 		scan_abort = true;
 		goto scan_done_out;
 	}
-	channel_inform.scan_channel = le32_to_cpu(channel_inform.scan_channel);
-	if (unlikely(channel_inform.scan_channel)) {
-
-		WL_CONN("channel_inform.scan_channel (%d)\n",
-		       channel_inform.scan_channel);
-	}
+	scan_channel = le32_to_cpu(channel_inform_le.scan_channel);
+	if (scan_channel)
+		WL_CONN("channel_inform.scan_channel (%d)\n", scan_channel);
 	cfg_priv->bss_list = cfg_priv->scan_results;
-	bss_list = cfg_priv->bss_list;
-	memset(bss_list, 0, len);
-	bss_list->buflen = cpu_to_le32(len);
+	bss_list_le = (struct brcmf_scan_results_le *) cfg_priv->bss_list;
 
-	err = brcmf_dev_ioctl(ndev, BRCMF_C_SCAN_RESULTS, bss_list, len);
-	if (unlikely(err)) {
+	memset(cfg_priv->scan_results, 0, len);
+	bss_list_le->buflen = cpu_to_le32(len);
+	err = brcmf_exec_dcmd(ndev, BRCMF_C_SCAN_RESULTS,
+			      cfg_priv->scan_results, len);
+	if (err) {
 		WL_ERR("%s Scan_results error (%d)\n", ndev->name, err);
 		err = -EINVAL;
 		scan_abort = true;
 		goto scan_done_out;
 	}
-	bss_list->buflen = le32_to_cpu(bss_list->buflen);
-	bss_list->version = le32_to_cpu(bss_list->version);
-	bss_list->count = le32_to_cpu(bss_list->count);
+	cfg_priv->scan_results->buflen = le32_to_cpu(bss_list_le->buflen);
+	cfg_priv->scan_results->version = le32_to_cpu(bss_list_le->version);
+	cfg_priv->scan_results->count = le32_to_cpu(bss_list_le->count);
 
 	err = brcmf_inform_bss(cfg_priv);
 	if (err) {
@@ -3038,11 +3196,6 @@ static void brcmf_init_conf(struct brcmf_cfg80211_conf *conf)
 	conf->tx_power = -1;
 }
 
-static void brcmf_init_prof(struct brcmf_cfg80211_profile *prof)
-{
-	memset(prof, 0, sizeof(*prof));
-}
-
 static void brcmf_init_eloop_handler(struct brcmf_cfg80211_event_loop *el)
 {
 	memset(el, 0, sizeof(*el));
@@ -3053,54 +3206,58 @@ static void brcmf_init_eloop_handler(struct brcmf_cfg80211_event_loop *el)
 	el->handler[BRCMF_E_SET_SSID] = brcmf_notify_connect_status;
 }
 
+static void brcmf_deinit_priv_mem(struct brcmf_cfg80211_priv *cfg_priv)
+{
+	kfree(cfg_priv->scan_results);
+	cfg_priv->scan_results = NULL;
+	kfree(cfg_priv->bss_info);
+	cfg_priv->bss_info = NULL;
+	kfree(cfg_priv->conf);
+	cfg_priv->conf = NULL;
+	kfree(cfg_priv->profile);
+	cfg_priv->profile = NULL;
+	kfree(cfg_priv->scan_req_int);
+	cfg_priv->scan_req_int = NULL;
+	kfree(cfg_priv->dcmd_buf);
+	cfg_priv->dcmd_buf = NULL;
+	kfree(cfg_priv->extra_buf);
+	cfg_priv->extra_buf = NULL;
+	kfree(cfg_priv->iscan);
+	cfg_priv->iscan = NULL;
+	kfree(cfg_priv->pmk_list);
+	cfg_priv->pmk_list = NULL;
+}
+
 static s32 brcmf_init_priv_mem(struct brcmf_cfg80211_priv *cfg_priv)
 {
 	cfg_priv->scan_results = kzalloc(WL_SCAN_BUF_MAX, GFP_KERNEL);
-	if (unlikely(!cfg_priv->scan_results)) {
-		WL_ERR("Scan results alloc failed\n");
+	if (!cfg_priv->scan_results)
 		goto init_priv_mem_out;
-	}
 	cfg_priv->conf = kzalloc(sizeof(*cfg_priv->conf), GFP_KERNEL);
-	if (unlikely(!cfg_priv->conf)) {
-		WL_ERR("wl_conf alloc failed\n");
+	if (!cfg_priv->conf)
 		goto init_priv_mem_out;
-	}
 	cfg_priv->profile = kzalloc(sizeof(*cfg_priv->profile), GFP_KERNEL);
-	if (unlikely(!cfg_priv->profile)) {
-		WL_ERR("wl_profile alloc failed\n");
+	if (!cfg_priv->profile)
 		goto init_priv_mem_out;
-	}
 	cfg_priv->bss_info = kzalloc(WL_BSS_INFO_MAX, GFP_KERNEL);
-	if (unlikely(!cfg_priv->bss_info)) {
-		WL_ERR("Bss information alloc failed\n");
+	if (!cfg_priv->bss_info)
 		goto init_priv_mem_out;
-	}
 	cfg_priv->scan_req_int = kzalloc(sizeof(*cfg_priv->scan_req_int),
 					 GFP_KERNEL);
-	if (unlikely(!cfg_priv->scan_req_int)) {
-		WL_ERR("Scan req alloc failed\n");
+	if (!cfg_priv->scan_req_int)
 		goto init_priv_mem_out;
-	}
-	cfg_priv->ioctl_buf = kzalloc(WL_IOCTL_LEN_MAX, GFP_KERNEL);
-	if (unlikely(!cfg_priv->ioctl_buf)) {
-		WL_ERR("Ioctl buf alloc failed\n");
+	cfg_priv->dcmd_buf = kzalloc(WL_DCMD_LEN_MAX, GFP_KERNEL);
+	if (!cfg_priv->dcmd_buf)
 		goto init_priv_mem_out;
-	}
 	cfg_priv->extra_buf = kzalloc(WL_EXTRA_BUF_MAX, GFP_KERNEL);
-	if (unlikely(!cfg_priv->extra_buf)) {
-		WL_ERR("Extra buf alloc failed\n");
+	if (!cfg_priv->extra_buf)
 		goto init_priv_mem_out;
-	}
 	cfg_priv->iscan = kzalloc(sizeof(*cfg_priv->iscan), GFP_KERNEL);
-	if (unlikely(!cfg_priv->iscan)) {
-		WL_ERR("Iscan buf alloc failed\n");
+	if (!cfg_priv->iscan)
 		goto init_priv_mem_out;
-	}
 	cfg_priv->pmk_list = kzalloc(sizeof(*cfg_priv->pmk_list), GFP_KERNEL);
-	if (unlikely(!cfg_priv->pmk_list)) {
-		WL_ERR("pmk list alloc failed\n");
+	if (!cfg_priv->pmk_list)
 		goto init_priv_mem_out;
-	}
 
 	return 0;
 
@@ -3110,301 +3267,111 @@ init_priv_mem_out:
 	return -ENOMEM;
 }
 
-static void brcmf_deinit_priv_mem(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	kfree(cfg_priv->scan_results);
-	cfg_priv->scan_results = NULL;
-	kfree(cfg_priv->bss_info);
-	cfg_priv->bss_info = NULL;
-	kfree(cfg_priv->conf);
-	cfg_priv->conf = NULL;
-	kfree(cfg_priv->profile);
-	cfg_priv->profile = NULL;
-	kfree(cfg_priv->scan_req_int);
-	cfg_priv->scan_req_int = NULL;
-	kfree(cfg_priv->ioctl_buf);
-	cfg_priv->ioctl_buf = NULL;
-	kfree(cfg_priv->extra_buf);
-	cfg_priv->extra_buf = NULL;
-	kfree(cfg_priv->iscan);
-	cfg_priv->iscan = NULL;
-	kfree(cfg_priv->pmk_list);
-	cfg_priv->pmk_list = NULL;
-}
-
-static s32 brcmf_create_event_handler(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	sema_init(&cfg_priv->event_sync, 0);
-	cfg_priv->event_tsk = kthread_run(brcmf_event_handler, cfg_priv,
-					  "wl_event_handler");
-	if (IS_ERR(cfg_priv->event_tsk)) {
-		cfg_priv->event_tsk = NULL;
-		WL_ERR("failed to create event thread\n");
-		return -ENOMEM;
-	}
-	return 0;
-}
-
-static void brcmf_destroy_event_handler(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	if (cfg_priv->event_tsk) {
-		send_sig(SIGTERM, cfg_priv->event_tsk, 1);
-		kthread_stop(cfg_priv->event_tsk);
-		cfg_priv->event_tsk = NULL;
-	}
-}
+/*
+* retrieve first queued event from head
+*/
 
-static void brcmf_term_iscan(struct brcmf_cfg80211_priv *cfg_priv)
+static struct brcmf_cfg80211_event_q *brcmf_deq_event(
+	struct brcmf_cfg80211_priv *cfg_priv)
 {
-	struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_to_iscan(cfg_priv);
+	struct brcmf_cfg80211_event_q *e = NULL;
 
-	if (cfg_priv->iscan_on && iscan->tsk) {
-		iscan->state = WL_ISCAN_STATE_IDLE;
-		send_sig(SIGTERM, iscan->tsk, 1);
-		kthread_stop(iscan->tsk);
-		iscan->tsk = NULL;
+	spin_lock_irq(&cfg_priv->evt_q_lock);
+	if (!list_empty(&cfg_priv->evt_q_list)) {
+		e = list_first_entry(&cfg_priv->evt_q_list,
+				     struct brcmf_cfg80211_event_q, evt_q_list);
+		list_del(&e->evt_q_list);
 	}
-}
-
-static void brcmf_notify_iscan_complete(struct brcmf_cfg80211_iscan_ctrl *iscan,
-					bool aborted)
-{
-	struct brcmf_cfg80211_priv *cfg_priv = iscan_to_cfg(iscan);
-	struct net_device *ndev = cfg_to_ndev(cfg_priv);
+	spin_unlock_irq(&cfg_priv->evt_q_lock);
 
-	if (unlikely(!test_and_clear_bit(WL_STATUS_SCANNING,
-					 &cfg_priv->status))) {
-		WL_ERR("Scan complete while device not scanning\n");
-		return;
-	}
-	if (likely(cfg_priv->scan_request)) {
-		WL_SCAN("ISCAN Completed scan: %s\n",
-				aborted ? "Aborted" : "Done");
-		cfg80211_scan_done(cfg_priv->scan_request, aborted);
-		brcmf_set_mpc(ndev, 1);
-		cfg_priv->scan_request = NULL;
-	}
-	cfg_priv->iscan_kickstart = false;
+	return e;
 }
 
-static s32 brcmf_wakeup_iscan(struct brcmf_cfg80211_iscan_ctrl *iscan)
-{
-	if (likely(iscan->state != WL_ISCAN_STATE_IDLE)) {
-		WL_SCAN("wake up iscan\n");
-		up(&iscan->sync);
-		return 0;
-	}
-
-	return -EIO;
-}
+/*
+** push event to tail of the queue
+*/
 
 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_iscan_results *list_buf;
-	s32 err = 0;
-
-	memset(iscan->scan_buf, 0, WL_ISCAN_BUF_MAX);
-	list_buf = (struct brcmf_iscan_results *)iscan->scan_buf;
-	results = &list_buf->results;
-	results->buflen = BRCMF_ISCAN_RESULTS_FIXED_SIZE;
-	results->version = 0;
-	results->count = 0;
-
-	memset(&list, 0, sizeof(list));
-	list.results.buflen = cpu_to_le32(WL_ISCAN_BUF_MAX);
-	err = brcmf_dev_iovar_getbuf(iscan->dev, "iscanresults", &list,
-				BRCMF_ISCAN_RESULTS_FIXED_SIZE, iscan->scan_buf,
-				WL_ISCAN_BUF_MAX);
-	if (unlikely(err)) {
-		WL_ERR("error (%d)\n", err);
-		return err;
-	}
-	results->buflen = le32_to_cpu(results->buflen);
-	results->version = le32_to_cpu(results->version);
-	results->count = le32_to_cpu(results->count);
-	WL_SCAN("results->count = %d\n", results->count);
-	WL_SCAN("results->buflen = %d\n", results->buflen);
-	*status = le32_to_cpu(list_buf->status);
-	*bss_list = results;
-
-	return err;
-}
-
-static s32 brcmf_iscan_done(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_priv->iscan;
-	s32 err = 0;
-
-	iscan->state = WL_ISCAN_STATE_IDLE;
-	rtnl_lock();
-	brcmf_inform_bss(cfg_priv);
-	brcmf_notify_iscan_complete(iscan, false);
-	rtnl_unlock();
-
-	return err;
-}
-
-static s32 brcmf_iscan_pending(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_priv->iscan;
-	s32 err = 0;
-
-	/* Reschedule the timer */
-	mod_timer(&iscan->timer, jiffies + iscan->timer_ms * HZ / 1000);
-	iscan->timer_on = 1;
-
-	return err;
-}
-
-static s32 brcmf_iscan_inprogress(struct brcmf_cfg80211_priv *cfg_priv)
+brcmf_enq_event(struct brcmf_cfg80211_priv *cfg_priv, u32 event,
+		const struct brcmf_event_msg *msg)
 {
-	struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_priv->iscan;
+	struct brcmf_cfg80211_event_q *e;
 	s32 err = 0;
 
-	rtnl_lock();
-	brcmf_inform_bss(cfg_priv);
-	brcmf_run_iscan(iscan, NULL, BRCMF_SCAN_ACTION_CONTINUE);
-	rtnl_unlock();
-	/* Reschedule the timer */
-	mod_timer(&iscan->timer, jiffies + iscan->timer_ms * HZ / 1000);
-	iscan->timer_on = 1;
-
-	return err;
-}
+	e = kzalloc(sizeof(struct brcmf_cfg80211_event_q), GFP_KERNEL);
+	if (!e)
+		return -ENOMEM;
 
-static s32 brcmf_iscan_aborted(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_priv->iscan;
-	s32 err = 0;
+	e->etype = event;
+	memcpy(&e->emsg, msg, sizeof(struct brcmf_event_msg));
 
-	iscan->state = WL_ISCAN_STATE_IDLE;
-	rtnl_lock();
-	brcmf_notify_iscan_complete(iscan, true);
-	rtnl_unlock();
+	spin_lock_irq(&cfg_priv->evt_q_lock);
+	list_add_tail(&e->evt_q_list, &cfg_priv->evt_q_list);
+	spin_unlock_irq(&cfg_priv->evt_q_lock);
 
 	return err;
 }
 
-static s32 brcmf_iscan_thread(void *data)
+static void brcmf_put_event(struct brcmf_cfg80211_event_q *e)
 {
-	struct sched_param param = {.sched_priority = MAX_RT_PRIO - 1 };
-	struct brcmf_cfg80211_iscan_ctrl *iscan =
-			(struct brcmf_cfg80211_iscan_ctrl *)data;
-	struct brcmf_cfg80211_priv *cfg_priv = iscan_to_cfg(iscan);
-	struct brcmf_cfg80211_iscan_eloop *el = &iscan->el;
-	u32 status;
-	int err = 0;
-
-	sched_setscheduler(current, SCHED_FIFO, &param);
-	allow_signal(SIGTERM);
-	status = BRCMF_SCAN_RESULTS_PARTIAL;
-	while (likely(!down_interruptible(&iscan->sync))) {
-		if (kthread_should_stop())
-			break;
-		if (iscan->timer_on) {
-			del_timer_sync(&iscan->timer);
-			iscan->timer_on = 0;
-		}
-		rtnl_lock();
-		err = brcmf_get_iscan_results(iscan, &status,
-					      &cfg_priv->bss_list);
-		if (unlikely(err)) {
-			status = BRCMF_SCAN_RESULTS_ABORTED;
-			WL_ERR("Abort iscan\n");
-		}
-		rtnl_unlock();
-		el->handler[status](cfg_priv);
-	}
-	if (iscan->timer_on) {
-		del_timer_sync(&iscan->timer);
-		iscan->timer_on = 0;
-	}
-	WL_SCAN("ISCAN thread terminated\n");
-
-	return 0;
+	kfree(e);
 }
 
-static void brcmf_iscan_timer(unsigned long data)
+static void brcmf_cfg80211_event_handler(struct work_struct *work)
 {
-	struct brcmf_cfg80211_iscan_ctrl *iscan =
-			(struct brcmf_cfg80211_iscan_ctrl *)data;
+	struct brcmf_cfg80211_priv *cfg_priv =
+			container_of(work, struct brcmf_cfg80211_priv,
+				     event_work);
+	struct brcmf_cfg80211_event_q *e;
 
-	if (iscan) {
-		iscan->timer_on = 0;
-		WL_SCAN("timer expired\n");
-		brcmf_wakeup_iscan(iscan);
+	e = brcmf_deq_event(cfg_priv);
+	if (unlikely(!e)) {
+		WL_ERR("event queue empty...\n");
+		return;
 	}
-}
 
-static s32 brcmf_invoke_iscan(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_to_iscan(cfg_priv);
-	int err = 0;
-
-	if (cfg_priv->iscan_on && !iscan->tsk) {
-		iscan->state = WL_ISCAN_STATE_IDLE;
-		sema_init(&iscan->sync, 0);
-		iscan->tsk = kthread_run(brcmf_iscan_thread, iscan, "wl_iscan");
-		if (IS_ERR(iscan->tsk)) {
-			WL_ERR("Could not create iscan thread\n");
-			iscan->tsk = NULL;
-			return -ENOMEM;
-		}
-	}
+	do {
+		WL_INFO("event type (%d)\n", e->etype);
+		if (cfg_priv->el.handler[e->etype])
+			cfg_priv->el.handler[e->etype](cfg_priv,
+						       cfg_to_ndev(cfg_priv),
+						       &e->emsg, e->edata);
+		else
+			WL_INFO("Unknown Event (%d): ignoring\n", e->etype);
+		brcmf_put_event(e);
+	} while ((e = brcmf_deq_event(cfg_priv)));
 
-	return err;
 }
 
-static void brcmf_init_iscan_eloop(struct brcmf_cfg80211_iscan_eloop *el)
+static void brcmf_init_eq(struct brcmf_cfg80211_priv *cfg_priv)
 {
-	memset(el, 0, sizeof(*el));
-	el->handler[BRCMF_SCAN_RESULTS_SUCCESS] = brcmf_iscan_done;
-	el->handler[BRCMF_SCAN_RESULTS_PARTIAL] = brcmf_iscan_inprogress;
-	el->handler[BRCMF_SCAN_RESULTS_PENDING] = brcmf_iscan_pending;
-	el->handler[BRCMF_SCAN_RESULTS_ABORTED] = brcmf_iscan_aborted;
-	el->handler[BRCMF_SCAN_RESULTS_NO_MEM] = brcmf_iscan_aborted;
+	spin_lock_init(&cfg_priv->evt_q_lock);
+	INIT_LIST_HEAD(&cfg_priv->evt_q_list);
 }
 
-static s32 brcmf_init_iscan(struct brcmf_cfg80211_priv *cfg_priv)
+static void brcmf_flush_eq(struct brcmf_cfg80211_priv *cfg_priv)
 {
-	struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_to_iscan(cfg_priv);
-	int err = 0;
+	struct brcmf_cfg80211_event_q *e;
 
-	if (cfg_priv->iscan_on) {
-		iscan->dev = cfg_to_ndev(cfg_priv);
-		iscan->state = WL_ISCAN_STATE_IDLE;
-		brcmf_init_iscan_eloop(&iscan->el);
-		iscan->timer_ms = WL_ISCAN_TIMER_INTERVAL_MS;
-		init_timer(&iscan->timer);
-		iscan->timer.data = (unsigned long) iscan;
-		iscan->timer.function = brcmf_iscan_timer;
-		sema_init(&iscan->sync, 0);
-		iscan->tsk = kthread_run(brcmf_iscan_thread, iscan, "wl_iscan");
-		if (IS_ERR(iscan->tsk)) {
-			WL_ERR("Could not create iscan thread\n");
-			iscan->tsk = NULL;
-			return -ENOMEM;
-		}
-		iscan->data = cfg_priv;
+	spin_lock_irq(&cfg_priv->evt_q_lock);
+	while (!list_empty(&cfg_priv->evt_q_list)) {
+		e = list_first_entry(&cfg_priv->evt_q_list,
+				     struct brcmf_cfg80211_event_q, evt_q_list);
+		list_del(&e->evt_q_list);
+		kfree(e);
 	}
-
-	return err;
+	spin_unlock_irq(&cfg_priv->evt_q_lock);
 }
 
 static s32 wl_init_priv(struct brcmf_cfg80211_priv *cfg_priv)
 {
-	struct wiphy *wiphy = cfg_to_wiphy(cfg_priv);
 	s32 err = 0;
 
 	cfg_priv->scan_request = NULL;
-	cfg_priv->pwr_save = !!(wiphy->flags & WIPHY_FLAG_PS_ON_BY_DEFAULT);
+	cfg_priv->pwr_save = true;
 	cfg_priv->iscan_on = true;	/* iscan on & off switch.
 				 we enable iscan per default */
-	cfg_priv->roam_on = false;	/* roam on & off switch.
+	cfg_priv->roam_on = true;	/* roam on & off switch.
 				 we enable roam per default */
 
 	cfg_priv->iscan_kickstart = false;
@@ -3413,14 +3380,13 @@ static s32 wl_init_priv(struct brcmf_cfg80211_priv *cfg_priv)
 	cfg_priv->dongle_up = false;	/* dongle is not up yet */
 	brcmf_init_eq(cfg_priv);
 	err = brcmf_init_priv_mem(cfg_priv);
-	if (unlikely(err))
+	if (err)
 		return err;
-	if (unlikely(brcmf_create_event_handler(cfg_priv)))
-		return -ENOMEM;
+	INIT_WORK(&cfg_priv->event_work, brcmf_cfg80211_event_handler);
 	brcmf_init_eloop_handler(&cfg_priv->el);
 	mutex_init(&cfg_priv->usr_sync);
 	err = brcmf_init_iscan(cfg_priv);
-	if (unlikely(err))
+	if (err)
 		return err;
 	brcmf_init_conf(cfg_priv->conf);
 	brcmf_init_prof(cfg_priv->profile);
@@ -3431,7 +3397,7 @@ static s32 wl_init_priv(struct brcmf_cfg80211_priv *cfg_priv)
 
 static void wl_deinit_priv(struct brcmf_cfg80211_priv *cfg_priv)
 {
-	brcmf_destroy_event_handler(cfg_priv);
+	cancel_work_sync(&cfg_priv->event_work);
 	cfg_priv->dongle_up = false;	/* dongle down */
 	brcmf_flush_eq(cfg_priv);
 	brcmf_link_down(cfg_priv);
@@ -3439,27 +3405,29 @@ static void wl_deinit_priv(struct brcmf_cfg80211_priv *cfg_priv)
 	brcmf_deinit_priv_mem(cfg_priv);
 }
 
-s32 brcmf_cfg80211_attach(struct net_device *ndev, void *data)
+struct brcmf_cfg80211_dev *brcmf_cfg80211_attach(struct net_device *ndev,
+						 struct device *busdev,
+						 void *data)
 {
 	struct wireless_dev *wdev;
 	struct brcmf_cfg80211_priv *cfg_priv;
 	struct brcmf_cfg80211_iface *ci;
+	struct brcmf_cfg80211_dev *cfg_dev;
 	s32 err = 0;
 
-	if (unlikely(!ndev)) {
+	if (!ndev) {
 		WL_ERR("ndev is invalid\n");
-		return -ENODEV;
+		return NULL;
 	}
-	cfg80211_dev = kzalloc(sizeof(struct brcmf_cfg80211_dev), GFP_KERNEL);
-	if (unlikely(!cfg80211_dev)) {
-		WL_ERR("wl_cfg80211_dev is invalid\n");
-		return -ENOMEM;
+	cfg_dev = kzalloc(sizeof(struct brcmf_cfg80211_dev), GFP_KERNEL);
+	if (!cfg_dev)
+		return NULL;
+
+	wdev = brcmf_alloc_wdev(sizeof(struct brcmf_cfg80211_iface), busdev);
+	if (IS_ERR(wdev)) {
+		kfree(cfg_dev);
+		return NULL;
 	}
-	WL_INFO("func %p\n", brcmf_cfg80211_get_sdio_func());
-	wdev = brcmf_alloc_wdev(sizeof(struct brcmf_cfg80211_iface),
-				&brcmf_cfg80211_get_sdio_func()->dev);
-	if (IS_ERR(wdev))
-		return -ENOMEM;
 
 	wdev->iftype = brcmf_mode_to_nl80211_iftype(WL_MODE_BSS);
 	cfg_priv = wdev_to_cfg(wdev);
@@ -3471,67 +3439,30 @@ s32 brcmf_cfg80211_attach(struct net_device *ndev, void *data)
 	SET_NETDEV_DEV(ndev, wiphy_dev(wdev->wiphy));
 	wdev->netdev = ndev;
 	err = wl_init_priv(cfg_priv);
-	if (unlikely(err)) {
+	if (err) {
 		WL_ERR("Failed to init iwm_priv (%d)\n", err);
 		goto cfg80211_attach_out;
 	}
-	brcmf_set_drvdata(cfg80211_dev, ci);
+	brcmf_set_drvdata(cfg_dev, ci);
 
-	return err;
+	return cfg_dev;
 
 cfg80211_attach_out:
 	brcmf_free_wdev(cfg_priv);
-	return err;
+	kfree(cfg_dev);
+	return NULL;
 }
 
-void brcmf_cfg80211_detach(void)
+void brcmf_cfg80211_detach(struct brcmf_cfg80211_dev *cfg_dev)
 {
 	struct brcmf_cfg80211_priv *cfg_priv;
 
-	cfg_priv = WL_PRIV_GET();
+	cfg_priv = brcmf_priv_get(cfg_dev);
 
 	wl_deinit_priv(cfg_priv);
 	brcmf_free_wdev(cfg_priv);
-	brcmf_set_drvdata(cfg80211_dev, NULL);
-	kfree(cfg80211_dev);
-	cfg80211_dev = NULL;
-	brcmf_clear_sdio_func();
-}
-
-static void brcmf_wakeup_event(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	up(&cfg_priv->event_sync);
-}
-
-static s32 brcmf_event_handler(void *data)
-{
-	struct brcmf_cfg80211_priv *cfg_priv =
-			(struct brcmf_cfg80211_priv *)data;
-	struct sched_param param = {.sched_priority = MAX_RT_PRIO - 1 };
-	struct brcmf_cfg80211_event_q *e;
-
-	sched_setscheduler(current, SCHED_FIFO, &param);
-	allow_signal(SIGTERM);
-	while (likely(!down_interruptible(&cfg_priv->event_sync))) {
-		if (kthread_should_stop())
-			break;
-		e = brcmf_deq_event(cfg_priv);
-		if (unlikely(!e)) {
-			WL_ERR("event queue empty...\n");
-			BUG();
-		}
-		WL_INFO("event type (%d)\n", e->etype);
-		if (cfg_priv->el.handler[e->etype]) {
-			cfg_priv->el.handler[e->etype](cfg_priv,
-						       cfg_to_ndev(cfg_priv),
-						       &e->emsg, e->edata);
-		} else {
-			WL_INFO("Unknown Event (%d): ignoring\n", e->etype);
-		}
-		brcmf_put_event(e);
-	}
-	WL_INFO("was terminated\n");
-	return 0;
+	brcmf_set_drvdata(cfg_dev, NULL);
+	kfree(cfg_dev);
 }
 
 void
@@ -3541,95 +3472,8 @@ brcmf_cfg80211_event(struct net_device *ndev,
 	u32 event_type = be32_to_cpu(e->event_type);
 	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(ndev);
 
-	if (likely(!brcmf_enq_event(cfg_priv, event_type, e, data)))
-		brcmf_wakeup_event(cfg_priv);
-}
-
-static void brcmf_init_eq(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	brcmf_init_eq_lock(cfg_priv);
-	INIT_LIST_HEAD(&cfg_priv->eq_list);
-}
-
-static void brcmf_flush_eq(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	struct brcmf_cfg80211_event_q *e;
-
-	brcmf_lock_eq(cfg_priv);
-	while (!list_empty(&cfg_priv->eq_list)) {
-		e = list_first_entry(&cfg_priv->eq_list,
-				     struct brcmf_cfg80211_event_q, eq_list);
-		list_del(&e->eq_list);
-		kfree(e);
-	}
-	brcmf_unlock_eq(cfg_priv);
-}
-
-/*
-* retrieve first queued event from head
-*/
-
-static struct brcmf_cfg80211_event_q *brcmf_deq_event(
-	struct brcmf_cfg80211_priv *cfg_priv)
-{
-	struct brcmf_cfg80211_event_q *e = NULL;
-
-	brcmf_lock_eq(cfg_priv);
-	if (likely(!list_empty(&cfg_priv->eq_list))) {
-		e = list_first_entry(&cfg_priv->eq_list,
-				     struct brcmf_cfg80211_event_q, eq_list);
-		list_del(&e->eq_list);
-	}
-	brcmf_unlock_eq(cfg_priv);
-
-	return e;
-}
-
-/*
-** push event to tail of the queue
-*/
-
-static s32
-brcmf_enq_event(struct brcmf_cfg80211_priv *cfg_priv, u32 event,
-		const struct brcmf_event_msg *msg, void *data)
-{
-	struct brcmf_cfg80211_event_q *e;
-	s32 err = 0;
-
-	e = kzalloc(sizeof(struct brcmf_cfg80211_event_q), GFP_KERNEL);
-	if (unlikely(!e)) {
-		WL_ERR("event alloc failed\n");
-		return -ENOMEM;
-	}
-
-	e->etype = event;
-	memcpy(&e->emsg, msg, sizeof(struct brcmf_event_msg));
-
-	brcmf_lock_eq(cfg_priv);
-	list_add_tail(&e->eq_list, &cfg_priv->eq_list);
-	brcmf_unlock_eq(cfg_priv);
-
-	return err;
-}
-
-static void brcmf_put_event(struct brcmf_cfg80211_event_q *e)
-{
-	kfree(e);
-}
-
-void brcmf_cfg80211_sdio_func(void *func)
-{
-	cfg80211_sdio_func = (struct sdio_func *)func;
-}
-
-static void brcmf_clear_sdio_func(void)
-{
-	cfg80211_sdio_func = NULL;
-}
-
-struct sdio_func *brcmf_cfg80211_get_sdio_func(void)
-{
-	return cfg80211_sdio_func;
+	if (!brcmf_enq_event(cfg_priv, event_type, e))
+		schedule_work(&cfg_priv->event_work);
 }
 
 static s32 brcmf_dongle_mode(struct net_device *ndev, s32 iftype)
@@ -3655,9 +3499,8 @@ static s32 brcmf_dongle_mode(struct net_device *ndev, s32 iftype)
 		WL_ERR("invalid type (%d)\n", iftype);
 		return err;
 	}
-	infra = cpu_to_le32(infra);
-	err = brcmf_dev_ioctl(ndev, BRCMF_C_SET_INFRA, &infra, sizeof(infra));
-	if (unlikely(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;
 	}
@@ -3675,10 +3518,10 @@ static s32 brcmf_dongle_eventmsg(struct net_device *ndev)
 	WL_TRACE("Enter\n");
 
 	/* Setup event_msgs */
-	brcmu_mkiovar("event_msgs", eventmask, BRCMF_EVENTING_MASK_LEN, iovbuf,
-		    sizeof(iovbuf));
-	err = brcmf_dev_ioctl(ndev, BRCMF_C_GET_VAR, iovbuf, sizeof(iovbuf));
-	if (unlikely(err)) {
+	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));
+	if (err) {
 		WL_ERR("Get event_msgs error (%d)\n", err);
 		goto dongle_eventmsg_out;
 	}
@@ -3704,10 +3547,10 @@ static s32 brcmf_dongle_eventmsg(struct net_device *ndev)
 	setbit(eventmask, BRCMF_E_JOIN_START);
 	setbit(eventmask, BRCMF_E_SCAN_COMPLETE);
 
-	brcmu_mkiovar("event_msgs", eventmask, BRCMF_EVENTING_MASK_LEN, iovbuf,
-		    sizeof(iovbuf));
-	err = brcmf_dev_ioctl(ndev, BRCMF_C_SET_VAR, iovbuf, sizeof(iovbuf));
-	if (unlikely(err)) {
+	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));
+	if (err) {
 		WL_ERR("Set event_msgs error (%d)\n", err);
 		goto dongle_eventmsg_out;
 	}
@@ -3721,20 +3564,23 @@ static s32
 brcmf_dongle_roam(struct net_device *ndev, u32 roamvar, u32 bcn_timeout)
 {
 	s8 iovbuf[32];
-	s32 roamtrigger[2];
-	s32 roam_delta[2];
 	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) {
-		brcmu_mkiovar("bcn_timeout", (char *)&bcn_timeout,
-			sizeof(bcn_timeout), iovbuf, sizeof(iovbuf));
-		err = brcmf_dev_ioctl(ndev, BRCMF_C_SET_VAR,
+		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));
-		if (unlikely(err)) {
+		if (err) {
 			WL_ERR("bcn_timeout error (%d)\n", err);
 			goto dongle_rom_out;
 		}
@@ -3745,28 +3591,29 @@ 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");
-	brcmu_mkiovar("roam_off", (char *)&roamvar,
-				sizeof(roamvar), iovbuf, sizeof(iovbuf));
-	err = brcmf_dev_ioctl(ndev, BRCMF_C_SET_VAR, iovbuf, sizeof(iovbuf));
-	if (unlikely(err)) {
+	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));
+	if (err) {
 		WL_ERR("roam_off error (%d)\n", err);
 		goto dongle_rom_out;
 	}
 
-	roamtrigger[0] = WL_ROAM_TRIGGER_LEVEL;
-	roamtrigger[1] = BRCM_BAND_ALL;
-	err = brcmf_dev_ioctl(ndev, BRCMF_C_SET_ROAM_TRIGGER,
+	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));
-	if (unlikely(err)) {
+	if (err) {
 		WL_ERR("WLC_SET_ROAM_TRIGGER error (%d)\n", err);
 		goto dongle_rom_out;
 	}
 
-	roam_delta[0] = WL_ROAM_DELTA;
-	roam_delta[1] = BRCM_BAND_ALL;
-	err = brcmf_dev_ioctl(ndev, BRCMF_C_SET_ROAM_DELTA,
+	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));
-	if (unlikely(err)) {
+	if (err) {
 		WL_ERR("WLC_SET_ROAM_DELTA error (%d)\n", err);
 		goto dongle_rom_out;
 	}
@@ -3777,12 +3624,15 @@ dongle_rom_out:
 
 static s32
 brcmf_dongle_scantime(struct net_device *ndev, s32 scan_assoc_time,
-		s32 scan_unassoc_time, s32 scan_passive_time)
+		      s32 scan_unassoc_time, s32 scan_passive_time)
 {
 	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_dev_ioctl(ndev, BRCMF_C_SET_SCAN_CHANNEL_TIME,
-			   &scan_assoc_time, sizeof(scan_assoc_time));
+	err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_SCAN_CHANNEL_TIME,
+			   &scan_assoc_tm_le, sizeof(scan_assoc_tm_le));
 	if (err) {
 		if (err == -EOPNOTSUPP)
 			WL_INFO("Scan assoc time is not supported\n");
@@ -3790,8 +3640,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_dev_ioctl(ndev, BRCMF_C_SET_SCAN_UNASSOC_TIME,
-			   &scan_unassoc_time, sizeof(scan_unassoc_time));
+	err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_SCAN_UNASSOC_TIME,
+			   &scan_unassoc_tm_le, sizeof(scan_unassoc_tm_le));
 	if (err) {
 		if (err == -EOPNOTSUPP)
 			WL_INFO("Scan unassoc time is not supported\n");
@@ -3800,8 +3650,8 @@ brcmf_dongle_scantime(struct net_device *ndev, s32 scan_assoc_time,
 		goto dongle_scantime_out;
 	}
 
-	err = brcmf_dev_ioctl(ndev, BRCMF_C_SET_SCAN_PASSIVE_TIME,
-			   &scan_passive_time, sizeof(scan_passive_time));
+	err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_SCAN_PASSIVE_TIME,
+			   &scan_passive_tm_le, sizeof(scan_passive_tm_le));
 	if (err) {
 		if (err == -EOPNOTSUPP)
 			WL_INFO("Scan passive time is not supported\n");
@@ -3814,10 +3664,40 @@ dongle_scantime_out:
 	return err;
 }
 
-s32 brcmf_config_dongle(struct brcmf_cfg80211_priv *cfg_priv, bool need_lock)
+static s32 wl_update_wiphybands(struct brcmf_cfg80211_priv *cfg_priv)
+{
+	struct wiphy *wiphy;
+	s32 phy_list;
+	s8 phy;
+	s32 err = 0;
+
+	err = brcmf_exec_dcmd(cfg_to_ndev(cfg_priv), BRCM_GET_PHYLIST,
+			      &phy_list, sizeof(phy_list));
+	if (err) {
+		WL_ERR("error (%d)\n", err);
+		return err;
+	}
+
+	phy = ((char *)&phy_list)[1];
+	WL_INFO("%c phy\n", phy);
+	if (phy == 'n' || phy == 'a') {
+		wiphy = cfg_to_wiphy(cfg_priv);
+		wiphy->bands[IEEE80211_BAND_5GHZ] = &__wl_band_5ghz_n;
+	}
+
+	return err;
+}
+
+static s32 brcmf_dongle_probecap(struct brcmf_cfg80211_priv *cfg_priv)
+{
+	return wl_update_wiphybands(cfg_priv);
+}
+
+static s32 brcmf_config_dongle(struct brcmf_cfg80211_priv *cfg_priv)
 {
 	struct net_device *ndev;
 	struct wireless_dev *wdev;
+	s32 power_mode;
 	s32 err = 0;
 
 	if (cfg_priv->dongle_up)
@@ -3825,31 +3705,35 @@ s32 brcmf_config_dongle(struct brcmf_cfg80211_priv *cfg_priv, bool need_lock)
 
 	ndev = cfg_to_ndev(cfg_priv);
 	wdev = ndev->ieee80211_ptr;
-	if (need_lock)
-		rtnl_lock();
 
 	brcmf_dongle_scantime(ndev, WL_SCAN_CHANNEL_TIME,
 			WL_SCAN_UNASSOC_TIME, WL_SCAN_PASSIVE_TIME);
 
 	err = brcmf_dongle_eventmsg(ndev);
-	if (unlikely(err))
+	if (err)
 		goto default_conf_out;
+
+	power_mode = cfg_priv->pwr_save ? PM_FAST : PM_OFF;
+	err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_PM, &power_mode);
+	if (err)
+		goto default_conf_out;
+	WL_INFO("power save set to %s\n",
+		(power_mode ? "enabled" : "disabled"));
+
 	err = brcmf_dongle_roam(ndev, (cfg_priv->roam_on ? 0 : 1),
 				WL_BEACON_TIMEOUT);
-	if (unlikely(err))
+	if (err)
 		goto default_conf_out;
 	err = brcmf_dongle_mode(ndev, wdev->iftype);
-	if (unlikely(err && err != -EINPROGRESS))
+	if (err && err != -EINPROGRESS)
 		goto default_conf_out;
 	err = brcmf_dongle_probecap(cfg_priv);
-	if (unlikely(err))
+	if (err)
 		goto default_conf_out;
 
 	/* -EINPROGRESS: Call commit handler */
 
 default_conf_out:
-	if (need_lock)
-		rtnl_unlock();
 
 	cfg_priv->dongle_up = true;
 
@@ -3857,30 +3741,40 @@ default_conf_out:
 
 }
 
-static s32 wl_update_wiphybands(struct brcmf_cfg80211_priv *cfg_priv)
+static int brcmf_debugfs_add_netdev_params(struct brcmf_cfg80211_priv *cfg_priv)
 {
-	struct wiphy *wiphy;
-	s32 phy_list;
-	s8 phy;
+	char buf[10+IFNAMSIZ];
+	struct dentry *fd;
 	s32 err = 0;
 
-	err = brcmf_dev_ioctl(cfg_to_ndev(cfg_priv), BRCM_GET_PHYLIST,
-			      &phy_list, sizeof(phy_list));
-	if (unlikely(err)) {
-		WL_ERR("error (%d)\n", err);
-		return err;
+	sprintf(buf, "netdev:%s", cfg_to_ndev(cfg_priv)->name);
+	cfg_priv->debugfsdir = debugfs_create_dir(buf,
+					cfg_to_wiphy(cfg_priv)->debugfsdir);
+
+	fd = debugfs_create_u16("beacon_int", S_IRUGO, cfg_priv->debugfsdir,
+		(u16 *)&cfg_priv->profile->beacon_interval);
+	if (!fd) {
+		err = -ENOMEM;
+		goto err_out;
 	}
 
-	phy = ((char *)&phy_list)[1];
-	WL_INFO("%c phy\n", phy);
-	if (phy == 'n' || phy == 'a') {
-		wiphy = cfg_to_wiphy(cfg_priv);
-		wiphy->bands[IEEE80211_BAND_5GHZ] = &__wl_band_5ghz_n;
+	fd = debugfs_create_u8("dtim_period", S_IRUGO, cfg_priv->debugfsdir,
+		(u8 *)&cfg_priv->profile->dtim_period);
+	if (!fd) {
+		err = -ENOMEM;
+		goto err_out;
 	}
 
+err_out:
 	return err;
 }
 
+static void brcmf_debugfs_remove_netdev(struct brcmf_cfg80211_priv *cfg_priv)
+{
+	debugfs_remove_recursive(cfg_priv->debugfsdir);
+	cfg_priv->debugfsdir = NULL;
+}
+
 static s32 __brcmf_cfg80211_up(struct brcmf_cfg80211_priv *cfg_priv)
 {
 	s32 err = 0;
@@ -3889,8 +3783,8 @@ static s32 __brcmf_cfg80211_up(struct brcmf_cfg80211_priv *cfg_priv)
 
 	brcmf_debugfs_add_netdev_params(cfg_priv);
 
-	err = brcmf_config_dongle(cfg_priv, false);
-	if (unlikely(err))
+	err = brcmf_config_dongle(cfg_priv);
+	if (err)
 		return err;
 
 	brcmf_invoke_iscan(cfg_priv);
@@ -3914,9 +3808,7 @@ static s32 __brcmf_cfg80211_down(struct brcmf_cfg80211_priv *cfg_priv)
 		   generated due to DISASSOC call to the fw to keep
 		   the state fw and WPA_Supplicant state consistent
 		 */
-		rtnl_unlock();
 		brcmf_delay(500);
-		rtnl_lock();
 	}
 
 	set_bit(WL_STATUS_SCAN_ABORTING, &cfg_priv->status);
@@ -3931,17 +3823,18 @@ static s32 __brcmf_cfg80211_down(struct brcmf_cfg80211_priv *cfg_priv)
 	clear_bit(WL_STATUS_SCANNING, &cfg_priv->status);
 	clear_bit(WL_STATUS_SCAN_ABORTING, &cfg_priv->status);
 
+	brcmf_link_down(cfg_priv);
 	brcmf_debugfs_remove_netdev(cfg_priv);
 
 	return 0;
 }
 
-s32 brcmf_cfg80211_up(void)
+s32 brcmf_cfg80211_up(struct brcmf_cfg80211_dev *cfg_dev)
 {
 	struct brcmf_cfg80211_priv *cfg_priv;
 	s32 err = 0;
 
-	cfg_priv = WL_PRIV_GET();
+	cfg_priv = brcmf_priv_get(cfg_dev);
 	mutex_lock(&cfg_priv->usr_sync);
 	err = __brcmf_cfg80211_up(cfg_priv);
 	mutex_unlock(&cfg_priv->usr_sync);
@@ -3949,12 +3842,12 @@ s32 brcmf_cfg80211_up(void)
 	return err;
 }
 
-s32 brcmf_cfg80211_down(void)
+s32 brcmf_cfg80211_down(struct brcmf_cfg80211_dev *cfg_dev)
 {
 	struct brcmf_cfg80211_priv *cfg_priv;
 	s32 err = 0;
 
-	cfg_priv = WL_PRIV_GET();
+	cfg_priv = brcmf_priv_get(cfg_dev);
 	mutex_lock(&cfg_priv->usr_sync);
 	err = __brcmf_cfg80211_down(cfg_priv);
 	mutex_unlock(&cfg_priv->usr_sync);
@@ -3962,78 +3855,13 @@ s32 brcmf_cfg80211_down(void)
 	return err;
 }
 
-static s32 brcmf_dongle_probecap(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	return wl_update_wiphybands(cfg_priv);
-}
-
-static void *brcmf_read_prof(struct brcmf_cfg80211_priv *cfg_priv, s32 item)
-{
-	switch (item) {
-	case WL_PROF_SEC:
-		return &cfg_priv->profile->sec;
-	case WL_PROF_BSSID:
-		return &cfg_priv->profile->bssid;
-	case WL_PROF_SSID:
-		return &cfg_priv->profile->ssid;
-	}
-	WL_ERR("invalid item (%d)\n", item);
-	return NULL;
-}
-
-static s32
-brcmf_update_prof(struct brcmf_cfg80211_priv *cfg_priv,
-		  const struct brcmf_event_msg *e, void *data, s32 item)
-{
-	s32 err = 0;
-	struct brcmf_ssid *ssid;
-
-	switch (item) {
-	case WL_PROF_SSID:
-		ssid = (struct brcmf_ssid *) data;
-		memset(cfg_priv->profile->ssid.SSID, 0,
-		       sizeof(cfg_priv->profile->ssid.SSID));
-		memcpy(cfg_priv->profile->ssid.SSID,
-		       ssid->SSID, ssid->SSID_len);
-		cfg_priv->profile->ssid.SSID_len = ssid->SSID_len;
-		break;
-	case WL_PROF_BSSID:
-		if (data)
-			memcpy(cfg_priv->profile->bssid, data, ETH_ALEN);
-		else
-			memset(cfg_priv->profile->bssid, 0, ETH_ALEN);
-		break;
-	case WL_PROF_SEC:
-		memcpy(&cfg_priv->profile->sec, data,
-		       sizeof(cfg_priv->profile->sec));
-		break;
-	case WL_PROF_BEACONINT:
-		cfg_priv->profile->beacon_interval = *(u16 *)data;
-		break;
-	case WL_PROF_DTIMPERIOD:
-		cfg_priv->profile->dtim_period = *(u8 *)data;
-		break;
-	default:
-		WL_ERR("unsupported item (%d)\n", item);
-		err = -EOPNOTSUPP;
-		break;
-	}
-
-	return err;
-}
-
-static bool brcmf_is_ibssmode(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	return cfg_priv->conf->mode == WL_MODE_IBSS;
-}
-
 static __used s32 brcmf_add_ie(struct brcmf_cfg80211_priv *cfg_priv,
 			       u8 t, u8 l, u8 *v)
 {
 	struct brcmf_cfg80211_ie *ie = &cfg_priv->ie;
 	s32 err = 0;
 
-	if (unlikely(ie->offset + l + 2 > WL_TLV_INFO_MAX)) {
+	if (ie->offset + l + 2 > WL_TLV_INFO_MAX) {
 		WL_ERR("ei crosses buffer boundary\n");
 		return -ENOSPC;
 	}
@@ -4044,109 +3872,3 @@ static __used s32 brcmf_add_ie(struct brcmf_cfg80211_priv *cfg_priv,
 
 	return err;
 }
-
-static void brcmf_link_down(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	struct net_device *dev = NULL;
-	s32 err = 0;
-
-	WL_TRACE("Enter\n");
-
-	if (cfg_priv->link_up) {
-		dev = cfg_to_ndev(cfg_priv);
-		WL_INFO("Call WLC_DISASSOC to stop excess roaming\n ");
-		err = brcmf_dev_ioctl(dev, BRCMF_C_DISASSOC, NULL, 0);
-		if (unlikely(err))
-			WL_ERR("WLC_DISASSOC failed (%d)\n", err);
-		cfg_priv->link_up = false;
-	}
-	WL_TRACE("Exit\n");
-}
-
-static void brcmf_lock_eq(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	spin_lock_irq(&cfg_priv->eq_lock);
-}
-
-static void brcmf_unlock_eq(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	spin_unlock_irq(&cfg_priv->eq_lock);
-}
-
-static void brcmf_init_eq_lock(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	spin_lock_init(&cfg_priv->eq_lock);
-}
-
-static void brcmf_delay(u32 ms)
-{
-	if (ms < 1000 / HZ) {
-		cond_resched();
-		mdelay(ms);
-	} else {
-		msleep(ms);
-	}
-}
-
-static void brcmf_set_drvdata(struct brcmf_cfg80211_dev *dev, void *data)
-{
-	dev->driver_data = data;
-}
-
-static void *brcmf_get_drvdata(struct brcmf_cfg80211_dev *dev)
-{
-	void *data = NULL;
-
-	if (dev)
-		data = dev->driver_data;
-	return data;
-}
-
-static void brcmf_set_mpc(struct net_device *ndev, int mpc)
-{
-	s32 err = 0;
-	struct brcmf_cfg80211_priv *cfg_priv = ndev_to_cfg(ndev);
-
-	if (test_bit(WL_STATUS_READY, &cfg_priv->status)) {
-		err = brcmf_dev_intvar_set(ndev, "mpc", mpc);
-		if (unlikely(err)) {
-			WL_ERR("fail to set mpc\n");
-			return;
-		}
-		WL_INFO("MPC : %d\n", mpc);
-	}
-}
-
-static int brcmf_debugfs_add_netdev_params(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	char buf[10+IFNAMSIZ];
-	struct dentry *fd;
-	s32 err = 0;
-
-	sprintf(buf, "netdev:%s", cfg_to_ndev(cfg_priv)->name);
-	cfg_priv->debugfsdir = debugfs_create_dir(buf,
-					cfg_to_wiphy(cfg_priv)->debugfsdir);
-
-	fd = debugfs_create_u16("beacon_int", S_IRUGO, cfg_priv->debugfsdir,
-		(u16 *)&cfg_priv->profile->beacon_interval);
-	if (!fd) {
-		err = -ENOMEM;
-		goto err_out;
-	}
-
-	fd = debugfs_create_u8("dtim_period", S_IRUGO, cfg_priv->debugfsdir,
-		(u8 *)&cfg_priv->profile->dtim_period);
-	if (!fd) {
-		err = -ENOMEM;
-		goto err_out;
-	}
-
-err_out:
-	return err;
-}
-
-static void brcmf_debugfs_remove_netdev(struct brcmf_cfg80211_priv *cfg_priv)
-{
-	debugfs_remove_recursive(cfg_priv->debugfsdir);
-	cfg_priv->debugfsdir = NULL;
-}
diff --git a/drivers/staging/brcm80211/brcmfmac/wl_cfg80211.h b/drivers/staging/brcm80211/brcmfmac/wl_cfg80211.h
index f26d08793ca8..62dc46144ede 100644
--- a/drivers/staging/brcm80211/brcmfmac/wl_cfg80211.h
+++ b/drivers/staging/brcm80211/brcmfmac/wl_cfg80211.h
@@ -103,10 +103,10 @@ do {								\
 				 * report it to cfg80211 through "connect"
 				 * event
 				 */
-#define WL_IOCTL_LEN_MAX	1024
+#define WL_DCMD_LEN_MAX	1024
 #define WL_EXTRA_BUF_MAX	2048
 #define WL_ISCAN_BUF_MAX	2048	/*
-				 * the buf length can be BRCMF_C_IOCTL_MAXLEN
+				 * the buf length can be BRCMF_DCMD_MAXLEN
 				 * to reduce iteration
 				 */
 #define WL_ISCAN_TIMER_INTERVAL_MS	3000
@@ -188,7 +188,7 @@ struct brcmf_cfg80211_dev {
 
 /* basic structure of scan request */
 struct brcmf_cfg80211_scan_req {
-	struct brcmf_ssid ssid;
+	struct brcmf_ssid_le ssid_le;
 };
 
 /* basic structure of information element */
@@ -199,7 +199,7 @@ struct brcmf_cfg80211_ie {
 
 /* event queue for cfg80211 main event */
 struct brcmf_cfg80211_event_q {
-	struct list_head eq_list;
+	struct list_head evt_q_list;
 	u32 etype;
 	struct brcmf_event_msg emsg;
 	s8 edata[1];
@@ -243,16 +243,15 @@ struct brcmf_cfg80211_iscan_eloop {
 
 /* dongle iscan controller */
 struct brcmf_cfg80211_iscan_ctrl {
-	struct net_device *dev;
+	struct net_device *ndev;
 	struct timer_list timer;
 	u32 timer_ms;
 	u32 timer_on;
 	s32 state;
-	struct task_struct *tsk;
-	struct semaphore sync;
+	struct work_struct work;
 	struct brcmf_cfg80211_iscan_eloop el;
 	void *data;
-	s8 ioctl_buf[BRCMF_C_IOCTL_SMLEN];
+	s8 dcmd_buf[BRCMF_DCMD_SMLEN];
 	s8 scan_buf[WL_ISCAN_BUF_MAX];
 };
 
@@ -265,15 +264,15 @@ struct brcmf_cfg80211_connect_info {
 };
 
 /* assoc ie length */
-struct brcmf_cfg80211_assoc_ielen {
-	u32 req_len;
-	u32 resp_len;
+struct brcmf_cfg80211_assoc_ielen_le {
+	__le32 req_len;
+	__le32 resp_len;
 };
 
 /* wpa2 pmk list */
 struct brcmf_cfg80211_pmk_list {
-	pmkid_list_t pmkids;
-	pmkid_t foo[MAXPMKID - 1];
+	struct pmkid_list pmkids;
+	struct pmkid foo[MAXPMKID - 1];
 };
 
 /* dongle private data of cfg80211 interface */
@@ -283,8 +282,8 @@ struct brcmf_cfg80211_priv {
 	struct cfg80211_scan_request *scan_request;	/* scan request
 							 object */
 	struct brcmf_cfg80211_event_loop el;	/* main event loop */
-	struct list_head eq_list;	/* used for event queue */
-	spinlock_t eq_lock;	/* for event queue synchronization */
+	struct list_head evt_q_list;	/* used for event queue */
+	spinlock_t	 evt_q_lock;	/* for event queue synchronization */
 	struct mutex usr_sync;	/* maily for dongle up/down synchronization */
 	struct brcmf_scan_results *bss_list;	/* bss_list holding scanned
 						 ap information */
@@ -295,13 +294,11 @@ struct brcmf_cfg80211_priv {
 						 cfg80211 layer */
 	struct brcmf_cfg80211_ie ie;	/* information element object for
 					 internal purpose */
-	struct semaphore event_sync;	/* for synchronization of main event
-					 thread */
 	struct brcmf_cfg80211_profile *profile;	/* holding dongle profile */
 	struct brcmf_cfg80211_iscan_ctrl *iscan;	/* iscan controller */
 	struct brcmf_cfg80211_connect_info conn_info; /* association info */
 	struct brcmf_cfg80211_pmk_list *pmk_list;	/* wpa2 pmk list */
-	struct task_struct *event_tsk;	/* task of main event handler thread */
+	struct work_struct event_work;	/* event handler work struct */
 	unsigned long status;		/* current dongle status */
 	void *pub;
 	u32 channel;		/* current channel */
@@ -315,21 +312,45 @@ struct brcmf_cfg80211_priv {
 	bool dongle_up;		/* indicate whether dongle up or not */
 	bool roam_on;		/* on/off switch for dongle self-roaming */
 	bool scan_tried;	/* indicates if first scan attempted */
-	u8 *ioctl_buf;	/* ioctl buffer */
-	u8 *extra_buf;	/* maily to grab assoc information */
+	u8 *dcmd_buf;		/* dcmd buffer */
+	u8 *extra_buf;		/* maily to grab assoc information */
 	struct dentry *debugfsdir;
-	u8 ci[0] __attribute__ ((__aligned__(NETDEV_ALIGN)));
+	u8 ci[0] __aligned(NETDEV_ALIGN);
 };
 
-#define cfg_to_wiphy(w) (w->wdev->wiphy)
-#define wiphy_to_cfg(w) ((struct brcmf_cfg80211_priv *)(wiphy_priv(w)))
-#define cfg_to_wdev(w) (w->wdev)
-#define wdev_to_cfg(w) ((struct brcmf_cfg80211_priv *)(wdev_priv(w)))
-#define cfg_to_ndev(w) (w->wdev->netdev)
-#define ndev_to_cfg(n) (wdev_to_cfg(n->ieee80211_ptr))
+static inline struct wiphy *cfg_to_wiphy(struct brcmf_cfg80211_priv *w)
+{
+	return w->wdev->wiphy;
+}
+
+static inline struct brcmf_cfg80211_priv *wiphy_to_cfg(struct wiphy *w)
+{
+	return (struct brcmf_cfg80211_priv *)(wiphy_priv(w));
+}
+
+static inline struct brcmf_cfg80211_priv *wdev_to_cfg(struct wireless_dev *wd)
+{
+	return (struct brcmf_cfg80211_priv *)(wdev_priv(wd));
+}
+
+static inline struct net_device *cfg_to_ndev(struct brcmf_cfg80211_priv *cfg)
+{
+	return cfg->wdev->netdev;
+}
+
+static inline struct brcmf_cfg80211_priv *ndev_to_cfg(struct net_device *ndev)
+{
+	return wdev_to_cfg(ndev->ieee80211_ptr);
+}
+
 #define iscan_to_cfg(i) ((struct brcmf_cfg80211_priv *)(i->data))
 #define cfg_to_iscan(w) (w->iscan)
-#define cfg_to_conn(w) (&w->conn_info)
+
+static inline struct
+brcmf_cfg80211_connect_info *cfg_to_conn(struct brcmf_cfg80211_priv *cfg)
+{
+	return &cfg->conn_info;
+}
 
 static inline struct brcmf_bss_info *next_bss(struct brcmf_scan_results *list,
 					   struct brcmf_bss_info *bss)
@@ -340,17 +361,15 @@ static inline struct brcmf_bss_info *next_bss(struct brcmf_scan_results *list,
 		list->bss_info;
 }
 
-#define for_each_bss(list, bss, __i)	\
-	for (__i = 0; __i < list->count && __i < WL_AP_MAX; __i++, bss = next_bss(list, bss))
+extern struct brcmf_cfg80211_dev *brcmf_cfg80211_attach(struct net_device *ndev,
+							struct device *busdev,
+							void *data);
+extern void brcmf_cfg80211_detach(struct brcmf_cfg80211_dev *cfg);
 
-extern s32 brcmf_cfg80211_attach(struct net_device *ndev, void *data);
-extern void brcmf_cfg80211_detach(void);
 /* event handler from dongle */
 extern void brcmf_cfg80211_event(struct net_device *ndev,
 				 const struct brcmf_event_msg *e, void *data);
-extern void brcmf_cfg80211_sdio_func(void *func); /* set sdio function info */
-extern struct sdio_func *brcmf_cfg80211_get_sdio_func(void);
-extern s32 brcmf_cfg80211_up(void);	/* dongle up */
-extern s32 brcmf_cfg80211_down(void);	/* dongle down */
+extern s32 brcmf_cfg80211_up(struct brcmf_cfg80211_dev *cfg_dev);
+extern s32 brcmf_cfg80211_down(struct brcmf_cfg80211_dev *cfg_dev);
 
 #endif				/* _wl_cfg80211_h_ */
diff --git a/drivers/staging/brcm80211/brcmsmac/Makefile b/drivers/staging/brcm80211/brcmsmac/Makefile
index 1ea3e0c48f3e..c2eb2d0af386 100644
--- a/drivers/staging/brcm80211/brcmsmac/Makefile
+++ b/drivers/staging/brcm80211/brcmsmac/Makefile
@@ -15,25 +15,17 @@
 # OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
 # CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 
-ccflags-y :=				\
-	-DWLC_HIGH				\
-	-DWLC_LOW				\
-	-DSTA					\
-	-DWME					\
-	-DWL11N					\
-	-DDBAND					\
-	-DBCMNVRAMR				\
-	-Idrivers/staging/brcm80211/brcmsmac \
-	-Idrivers/staging/brcm80211/brcmsmac/phy \
-	-Idrivers/staging/brcm80211/include
+ccflags-y := \
+	-D__CHECK_ENDIAN__ \
+	-Idrivers/net/wireless/brcm80211/brcmsmac \
+	-Idrivers/net/wireless/brcm80211/brcmsmac/phy \
+	-Idrivers/net/wireless/brcm80211/include
 
 BRCMSMAC_OFILES := \
 	mac80211_if.o \
 	ucode_loader.o \
-	alloc.o \
 	ampdu.o \
 	antsel.o \
-	bmac.o \
 	channel.o \
 	main.o \
 	phy_shim.o \
@@ -50,7 +42,8 @@ BRCMSMAC_OFILES := \
 	otp.o \
 	srom.o \
 	dma.o \
-	nicpci.o
+	nicpci.o \
+	brcms_trace_events.o
 
 MODULEPFX := brcmsmac
 
diff --git a/drivers/staging/brcm80211/brcmsmac/aiutils.c b/drivers/staging/brcm80211/brcmsmac/aiutils.c
index a25901e9981b..025fa0eb6f47 100644
--- a/drivers/staging/brcm80211/brcmsmac/aiutils.c
+++ b/drivers/staging/brcm80211/brcmsmac/aiutils.c
@@ -12,7 +12,10 @@
  * 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.
+ *
+ * File contents: support functions for PCI/PCIe
  */
+
 #include <linux/delay.h>
 #include <linux/pci.h>
 
@@ -20,6 +23,7 @@
 #include <chipcommon.h>
 #include <brcmu_utils.h>
 #include <brcm_hw_ids.h>
+#include <soc.h>
 #include "types.h"
 #include "pub.h"
 #include "pmu.h"
@@ -28,48 +32,68 @@
 #include "aiutils.h"
 
 /* slow_clk_ctl */
-#define SCC_SS_MASK		0x00000007	/* slow clock source mask */
-#define	SCC_SS_LPO		0x00000000	/* source of slow clock is LPO */
-#define	SCC_SS_XTAL		0x00000001	/* source of slow clock is crystal */
-#define	SCC_SS_PCI		0x00000002	/* source of slow clock is PCI */
-#define SCC_LF			0x00000200	/* LPOFreqSel, 1: 160Khz, 0: 32KHz */
-#define SCC_LP			0x00000400	/* LPOPowerDown, 1: LPO is disabled,
-						 * 0: LPO is enabled
-						 */
-#define SCC_FS			0x00000800	/* ForceSlowClk, 1: sb/cores running on slow clock,
-						 * 0: power logic control
-						 */
-#define SCC_IP			0x00001000	/* IgnorePllOffReq, 1/0: power logic ignores/honors
-						 * PLL clock disable requests from core
-						 */
-#define SCC_XC			0x00002000	/* XtalControlEn, 1/0: power logic does/doesn't
-						 * disable crystal when appropriate
-						 */
-#define SCC_XP			0x00004000	/* XtalPU (RO), 1/0: crystal running/disabled */
-#define SCC_CD_MASK		0xffff0000	/* ClockDivider (SlowClk = 1/(4+divisor)) */
+ /* slow clock source mask */
+#define SCC_SS_MASK		0x00000007
+ /* source of slow clock is LPO */
+#define	SCC_SS_LPO		0x00000000
+ /* source of slow clock is crystal */
+#define	SCC_SS_XTAL		0x00000001
+ /* source of slow clock is PCI */
+#define	SCC_SS_PCI		0x00000002
+ /* LPOFreqSel, 1: 160Khz, 0: 32KHz */
+#define SCC_LF			0x00000200
+ /* LPOPowerDown, 1: LPO is disabled, 0: LPO is enabled */
+#define SCC_LP			0x00000400
+ /* ForceSlowClk, 1: sb/cores running on slow clock, 0: power logic control */
+#define SCC_FS			0x00000800
+ /* IgnorePllOffReq, 1/0:
+  *  power logic ignores/honors PLL clock disable requests from core
+  */
+#define SCC_IP			0x00001000
+ /* XtalControlEn, 1/0:
+  *  power logic does/doesn't disable crystal when appropriate
+  */
+#define SCC_XC			0x00002000
+ /* XtalPU (RO), 1/0: crystal running/disabled */
+#define SCC_XP			0x00004000
+ /* ClockDivider (SlowClk = 1/(4+divisor)) */
+#define SCC_CD_MASK		0xffff0000
 #define SCC_CD_SHIFT		16
 
 /* system_clk_ctl */
-#define	SYCC_IE			0x00000001	/* ILPen: Enable Idle Low Power */
-#define	SYCC_AE			0x00000002	/* ALPen: Enable Active Low Power */
-#define	SYCC_FP			0x00000004	/* ForcePLLOn */
-#define	SYCC_AR			0x00000008	/* Force ALP (or HT if ALPen is not set */
-#define	SYCC_HR			0x00000010	/* Force HT */
-#define SYCC_CD_MASK		0xffff0000	/* ClkDiv  (ILP = 1/(4 * (divisor + 1)) */
+ /* ILPen: Enable Idle Low Power */
+#define	SYCC_IE			0x00000001
+ /* ALPen: Enable Active Low Power */
+#define	SYCC_AE			0x00000002
+ /* ForcePLLOn */
+#define	SYCC_FP			0x00000004
+ /* Force ALP (or HT if ALPen is not set */
+#define	SYCC_AR			0x00000008
+ /* Force HT */
+#define	SYCC_HR			0x00000010
+ /* ClkDiv  (ILP = 1/(4 * (divisor + 1)) */
+#define SYCC_CD_MASK		0xffff0000
 #define SYCC_CD_SHIFT		16
 
 #define CST4329_SPROM_OTP_SEL_MASK	0x00000003
-#define CST4329_DEFCIS_SEL		0	/* OTP is powered up, use def. CIS, no SPROM */
-#define CST4329_SPROM_SEL		1	/* OTP is powered up, SPROM is present */
-#define CST4329_OTP_SEL			2	/* OTP is powered up, no SPROM */
-#define CST4329_OTP_PWRDN		3	/* OTP is powered down, SPROM is present */
+ /* OTP is powered up, use def. CIS, no SPROM */
+#define CST4329_DEFCIS_SEL		0
+ /* OTP is powered up, SPROM is present */
+#define CST4329_SPROM_SEL		1
+ /* OTP is powered up, no SPROM */
+#define CST4329_OTP_SEL			2
+ /* OTP is powered down, SPROM is present */
+#define CST4329_OTP_PWRDN		3
+
 #define CST4329_SPI_SDIO_MODE_MASK	0x00000004
 #define CST4329_SPI_SDIO_MODE_SHIFT	2
 
 /* 43224 chip-specific ChipControl register bits */
 #define CCTRL43224_GPIO_TOGGLE          0x8000
-#define CCTRL_43224A0_12MA_LED_DRIVE    0x00F000F0	/* 12 mA drive strength */
-#define CCTRL_43224B0_12MA_LED_DRIVE    0xF0	/* 12 mA drive strength for later 43224s */
+ /* 12 mA drive strength */
+#define CCTRL_43224A0_12MA_LED_DRIVE    0x00F000F0
+ /* 12 mA drive strength for later 43224s */
+#define CCTRL_43224B0_12MA_LED_DRIVE    0xF0
 
 /* 43236 Chip specific ChipStatus register bits */
 #define CST43236_SFLASH_MASK		0x00000040
@@ -78,29 +102,44 @@
 #define CST43236_BP_CLK			0x00000200	/* 120/96Mbps */
 #define CST43236_BOOT_MASK		0x00001800
 #define CST43236_BOOT_SHIFT		11
-#define CST43236_BOOT_FROM_SRAM		0	/* boot from SRAM, ARM in reset */
-#define CST43236_BOOT_FROM_ROM		1	/* boot from ROM */
-#define CST43236_BOOT_FROM_FLASH	2	/* boot from FLASH */
+#define CST43236_BOOT_FROM_SRAM		0 /* boot from SRAM, ARM in reset */
+#define CST43236_BOOT_FROM_ROM		1 /* boot from ROM */
+#define CST43236_BOOT_FROM_FLASH	2 /* boot from FLASH */
 #define CST43236_BOOT_FROM_INVALID	3
 
 /* 4331 chip-specific ChipControl register bits */
-#define CCTRL4331_BT_COEXIST		(1<<0)	/* 0 disable */
-#define CCTRL4331_SECI			(1<<1)	/* 0 SECI is disabled (JATG functional) */
-#define CCTRL4331_EXT_LNA		(1<<2)	/* 0 disable */
-#define CCTRL4331_SPROM_GPIO13_15       (1<<3)	/* sprom/gpio13-15 mux */
-#define CCTRL4331_EXTPA_EN		(1<<4)	/* 0 ext pa disable, 1 ext pa enabled */
-#define CCTRL4331_GPIOCLK_ON_SPROMCS	(1<<5)	/* set drive out GPIO_CLK on sprom_cs pin */
-#define CCTRL4331_PCIE_MDIO_ON_SPROMCS	(1<<6)	/* use sprom_cs pin as PCIE mdio interface */
-#define CCTRL4331_EXTPA_ON_GPIO2_5	(1<<7)	/* aband extpa will be at gpio2/5 and sprom_dout */
-#define CCTRL4331_OVR_PIPEAUXCLKEN	(1<<8)	/* override core control on pipe_AuxClkEnable */
-#define CCTRL4331_OVR_PIPEAUXPWRDOWN	(1<<9)	/* override core control on pipe_AuxPowerDown */
-#define CCTRL4331_PCIE_AUXCLKEN		(1<<10)	/* pcie_auxclkenable */
-#define CCTRL4331_PCIE_PIPE_PLLDOWN	(1<<11)	/* pcie_pipe_pllpowerdown */
-#define CCTRL4331_BT_SHD0_ON_GPIO4	(1<<16)	/* enable bt_shd0 at gpio4 */
-#define CCTRL4331_BT_SHD1_ON_GPIO5	(1<<17)	/* enable bt_shd1 at gpio5 */
+ /* 0 disable */
+#define CCTRL4331_BT_COEXIST		(1<<0)
+ /* 0 SECI is disabled (JTAG functional) */
+#define CCTRL4331_SECI			(1<<1)
+ /* 0 disable */
+#define CCTRL4331_EXT_LNA		(1<<2)
+ /* sprom/gpio13-15 mux */
+#define CCTRL4331_SPROM_GPIO13_15       (1<<3)
+ /* 0 ext pa disable, 1 ext pa enabled */
+#define CCTRL4331_EXTPA_EN		(1<<4)
+ /* set drive out GPIO_CLK on sprom_cs pin */
+#define CCTRL4331_GPIOCLK_ON_SPROMCS	(1<<5)
+ /* use sprom_cs pin as PCIE mdio interface */
+#define CCTRL4331_PCIE_MDIO_ON_SPROMCS	(1<<6)
+ /* aband extpa will be at gpio2/5 and sprom_dout */
+#define CCTRL4331_EXTPA_ON_GPIO2_5	(1<<7)
+ /* override core control on pipe_AuxClkEnable */
+#define CCTRL4331_OVR_PIPEAUXCLKEN	(1<<8)
+ /* override core control on pipe_AuxPowerDown */
+#define CCTRL4331_OVR_PIPEAUXPWRDOWN	(1<<9)
+ /* pcie_auxclkenable */
+#define CCTRL4331_PCIE_AUXCLKEN		(1<<10)
+ /* pcie_pipe_pllpowerdown */
+#define CCTRL4331_PCIE_PIPE_PLLDOWN	(1<<11)
+ /* enable bt_shd0 at gpio4 */
+#define CCTRL4331_BT_SHD0_ON_GPIO4	(1<<16)
+ /* enable bt_shd1 at gpio5 */
+#define CCTRL4331_BT_SHD1_ON_GPIO5	(1<<17)
 
 /* 4331 Chip specific ChipStatus register bits */
-#define	CST4331_XTAL_FREQ		0x00000001	/* crystal frequency 20/40Mhz */
+ /* crystal frequency 20/40Mhz */
+#define	CST4331_XTAL_FREQ		0x00000001
 #define	CST4331_SPROM_PRESENT		0x00000002
 #define	CST4331_OTP_PRESENT		0x00000004
 #define	CST4331_LDO_RF			0x00000008
@@ -110,19 +149,26 @@
 #define	CST4319_SPI_CPULESSUSB		0x00000001
 #define	CST4319_SPI_CLK_POL		0x00000002
 #define	CST4319_SPI_CLK_PH		0x00000008
-#define	CST4319_SPROM_OTP_SEL_MASK	0x000000c0	/* gpio [7:6], SDIO CIS selection */
+ /* gpio [7:6], SDIO CIS selection */
+#define	CST4319_SPROM_OTP_SEL_MASK	0x000000c0
 #define	CST4319_SPROM_OTP_SEL_SHIFT	6
-#define	CST4319_DEFCIS_SEL		0x00000000	/* use default CIS, OTP is powered up */
-#define	CST4319_SPROM_SEL		0x00000040	/* use SPROM, OTP is powered up */
-#define	CST4319_OTP_SEL			0x00000080	/* use OTP, OTP is powered up */
-#define	CST4319_OTP_PWRDN		0x000000c0	/* use SPROM, OTP is powered down */
-#define	CST4319_SDIO_USB_MODE		0x00000100	/* gpio [8], sdio/usb mode */
+ /* use default CIS, OTP is powered up */
+#define	CST4319_DEFCIS_SEL		0x00000000
+ /* use SPROM, OTP is powered up */
+#define	CST4319_SPROM_SEL		0x00000040
+ /* use OTP, OTP is powered up */
+#define	CST4319_OTP_SEL			0x00000080
+ /* use SPROM, OTP is powered down */
+#define	CST4319_OTP_PWRDN		0x000000c0
+ /* gpio [8], sdio/usb mode */
+#define	CST4319_SDIO_USB_MODE		0x00000100
 #define	CST4319_REMAP_SEL_MASK		0x00000600
 #define	CST4319_ILPDIV_EN		0x00000800
 #define	CST4319_XTAL_PD_POL		0x00001000
 #define	CST4319_LPO_SEL			0x00002000
 #define	CST4319_RES_INIT_MODE		0x0000c000
-#define	CST4319_PALDO_EXTPNP		0x00010000	/* PALDO is configured with external PNP */
+ /* PALDO is configured with external PNP */
+#define	CST4319_PALDO_EXTPNP		0x00010000
 #define	CST4319_CBUCK_MODE_MASK		0x00060000
 #define CST4319_CBUCK_MODE_BURST	0x00020000
 #define CST4319_CBUCK_MODE_LPBURST	0x00060000
@@ -153,11 +199,8 @@
 #define	CST4313_SPROM_OTP_SEL_SHIFT		0
 
 /* 4313 Chip specific ChipControl register bits */
-#define CCTRL_4313_12MA_LED_DRIVE    0x00000007	/* 12 mA drive strengh for later 4313 */
-
-#define BCM47162_DMP() ((sih->chip == BCM47162_CHIP_ID) && \
-		(sih->chiprev == 0) && \
-		(sii->coreid[sii->curidx] == MIPS74K_CORE_ID))
+ /* 12 mA drive strengh for later 4313 */
+#define CCTRL_4313_12MA_LED_DRIVE    0x00000007
 
 /* Manufacturer Ids */
 #define	MFGID_ARM		0x43b
@@ -227,9 +270,12 @@
 #define	SD_SG32			0x00000008
 #define	SD_SZ_ALIGN		0x00000fff
 
-#define	PCI_CFG_GPIO_SCS	0x10	/* PCI config space bit 4 for 4306c0 slow clock source */
-#define PCI_CFG_GPIO_XTAL	0x40	/* PCI config space GPIO 14 for Xtal power-up */
-#define PCI_CFG_GPIO_PLL	0x80	/* PCI config space GPIO 15 for PLL power-down */
+/* PCI config space bit 4 for 4306c0 slow clock source */
+#define	PCI_CFG_GPIO_SCS	0x10
+/* PCI config space GPIO 14 for Xtal power-up */
+#define PCI_CFG_GPIO_XTAL	0x40
+/* PCI config space GPIO 15 for PLL power-down */
+#define PCI_CFG_GPIO_PLL	0x80
 
 /* power control defines */
 #define PLL_DELAY		150	/* us pll on delay */
@@ -239,6 +285,82 @@
 /* resetctrl */
 #define	AIRC_RESET		1
 
+#define	NOREV		-1	/* Invalid rev */
+
+/* GPIO Based LED powersave defines */
+#define DEFAULT_GPIO_ONTIME	10	/* Default: 10% on */
+#define DEFAULT_GPIO_OFFTIME	90	/* Default: 10% on */
+
+/* When Srom support present, fields in sromcontrol */
+#define	SRC_START		0x80000000
+#define	SRC_BUSY		0x80000000
+#define	SRC_OPCODE		0x60000000
+#define	SRC_OP_READ		0x00000000
+#define	SRC_OP_WRITE		0x20000000
+#define	SRC_OP_WRDIS		0x40000000
+#define	SRC_OP_WREN		0x60000000
+#define	SRC_OTPSEL		0x00000010
+#define	SRC_LOCK		0x00000008
+#define	SRC_SIZE_MASK		0x00000006
+#define	SRC_SIZE_1K		0x00000000
+#define	SRC_SIZE_4K		0x00000002
+#define	SRC_SIZE_16K		0x00000004
+#define	SRC_SIZE_SHIFT		1
+#define	SRC_PRESENT		0x00000001
+
+/* External PA enable mask */
+#define GPIO_CTRL_EPA_EN_MASK 0x40
+
+#define DEFAULT_GPIOTIMERVAL \
+	((DEFAULT_GPIO_ONTIME << GPIO_ONTIME_SHIFT) | DEFAULT_GPIO_OFFTIME)
+
+#define	BADIDX		(SI_MAXCORES + 1)
+
+/* Newer chips can access PCI/PCIE and CC core without requiring to change
+ * PCI BAR0 WIN
+ */
+#define SI_FAST(si) (((si)->pub.buscoretype == PCIE_CORE_ID) ||	\
+		     (((si)->pub.buscoretype == PCI_CORE_ID) && \
+		      (si)->pub.buscorerev >= 13))
+
+#define CCREGS_FAST(si) (((char __iomem *)((si)->curmap) + \
+			  PCI_16KB0_CCREGS_OFFSET))
+
+#define	IS_SIM(chippkg)	\
+	((chippkg == HDLSIM_PKG_ID) || (chippkg == HWSIM_PKG_ID))
+
+/*
+ * Macros to disable/restore function core(D11, ENET, ILINE20, etc) interrupts
+ * before after core switching to avoid invalid register accesss inside ISR.
+ */
+#define INTR_OFF(si, intr_val) \
+	if ((si)->intrsoff_fn && \
+	    (si)->coreid[(si)->curidx] == (si)->dev_coreid) \
+		intr_val = (*(si)->intrsoff_fn)((si)->intr_arg)
+
+#define INTR_RESTORE(si, intr_val) \
+	if ((si)->intrsrestore_fn && \
+	    (si)->coreid[(si)->curidx] == (si)->dev_coreid) \
+		(*(si)->intrsrestore_fn)((si)->intr_arg, intr_val)
+
+#define PCI(si)		((si)->pub.buscoretype == PCI_CORE_ID)
+#define PCIE(si)	((si)->pub.buscoretype == PCIE_CORE_ID)
+
+#define PCI_FORCEHT(si)	(PCIE(si) && (si->pub.chip == BCM4716_CHIP_ID))
+
+#ifdef BCMDBG
+#define	SI_MSG(args)	printk args
+#else
+#define	SI_MSG(args)
+#endif				/* BCMDBG */
+
+#define	GOODCOREADDR(x, b) \
+	(((x) >= (b)) && ((x) < ((b) + SI_MAXCORES * SI_CORE_SIZE)) && \
+		IS_ALIGNED((x), SI_CORE_SIZE))
+
+#define PCIEREGS(si) ((__iomem char *)((si)->curmap) + \
+			PCI_16KB0_PCIREGS_OFFSET)
+
 struct aidmp {
 	u32 oobselina30;	/* 0x000 */
 	u32 oobselina74;	/* 0x004 */
@@ -360,7 +482,7 @@ struct aidmp {
 /* EROM parsing */
 
 static u32
-get_erom_ent(struct si_pub *sih, u32 **eromptr, u32 mask, u32 match)
+get_erom_ent(struct si_pub *sih, u32 __iomem **eromptr, u32 mask, u32 match)
 {
 	u32 ent;
 	uint inv = 0, nom = 0;
@@ -386,16 +508,11 @@ get_erom_ent(struct si_pub *sih, u32 **eromptr, u32 mask, u32 match)
 		nom++;
 	}
 
-	SI_VMSG(("%s: Returning ent 0x%08x\n", __func__, ent));
-	if (inv + nom) {
-		SI_VMSG(("  after %d invalid and %d non-matching entries\n",
-			 inv, nom));
-	}
 	return ent;
 }
 
 static u32
-get_asd(struct si_pub *sih, u32 **eromptr, uint sp, uint ad, uint st,
+get_asd(struct si_pub *sih, u32 __iomem **eromptr, uint sp, uint ad, uint st,
 	u32 *addrl, u32 *addrh, u32 *sizel, u32 *sizeh)
 {
 	u32 asd, sz, szd;
@@ -423,9 +540,6 @@ get_asd(struct si_pub *sih, u32 **eromptr, uint sp, uint ad, uint st,
 	} else
 		*sizel = AD_SZ_BASE << (sz >> AD_SZ_SHIFT);
 
-	SI_VMSG(("  SP %d, ad %d: st = %d, 0x%08x_0x%08x @ 0x%08x_0x%08x\n",
-		 sp, ad, st, *sizeh, *sizel, *addrh, *addrl));
-
 	return asd;
 }
 
@@ -434,45 +548,28 @@ static void ai_hwfixup(struct si_info *sii)
 }
 
 /* parse the enumeration rom to identify all cores */
-void ai_scan(struct si_pub *sih, void *regs)
+static void ai_scan(struct si_pub *sih, struct chipcregs __iomem *cc)
 {
-	struct si_info *sii = SI_INFO(sih);
-	chipcregs_t *cc = (chipcregs_t *) regs;
-	u32 erombase, *eromptr, *eromlim;
+	struct si_info *sii = (struct si_info *)sih;
 
-	erombase = R_REG(&cc->eromptr);
-
-	switch (sih->bustype) {
-	case SI_BUS:
-		eromptr = (u32 *) REG_MAP(erombase, SI_CORE_SIZE);
-		break;
+	u32 erombase;
+	u32 __iomem *eromptr, *eromlim;
+	void __iomem *regs = cc;
 
-	case PCI_BUS:
-		/* Set wrappers address */
-		sii->curwrap = (void *)((unsigned long)regs + SI_CORE_SIZE);
-
-		/* Now point the window at the erom */
-		pci_write_config_dword(sii->pbus, PCI_BAR0_WIN, erombase);
-		eromptr = regs;
-		break;
+	erombase = R_REG(&cc->eromptr);
 
-	case SPI_BUS:
-	case SDIO_BUS:
-		eromptr = (u32 *)(unsigned long)erombase;
-		break;
+	/* Set wrappers address */
+	sii->curwrap = (void *)((unsigned long)cc + SI_CORE_SIZE);
 
-	default:
-		SI_ERROR(("Don't know how to do AXI enumertion on bus %d\n",
-			  sih->bustype));
-		return;
-	}
+	/* Now point the window at the erom */
+	pci_write_config_dword(sii->pbus, PCI_BAR0_WIN, erombase);
+	eromptr = regs;
 	eromlim = eromptr + (ER_REMAPCONTROL / sizeof(u32));
 
-	SI_VMSG(("ai_scan: regs = 0x%p, erombase = 0x%08x, eromptr = 0x%p, eromlim = 0x%p\n", regs, erombase, eromptr, eromlim));
 	while (eromptr < eromlim) {
 		u32 cia, cib, cid, mfg, crev, nmw, nsw, nmp, nsp;
 		u32 mpd, asd, addrl, addrh, sizel, sizeh;
-		u32 *base;
+		u32 __iomem *base;
 		uint i, j, idx;
 		bool br;
 
@@ -481,8 +578,7 @@ void ai_scan(struct si_pub *sih, void *regs)
 		/* Grok a component */
 		cia = get_erom_ent(sih, &eromptr, ER_TAG, ER_CI);
 		if (cia == (ER_END | ER_VALID)) {
-			SI_VMSG(("Found END of erom after %d cores\n",
-				 sii->numcores));
+			/*  Found END of erom */
 			ai_hwfixup(sii);
 			return;
 		}
@@ -490,7 +586,7 @@ void ai_scan(struct si_pub *sih, void *regs)
 		cib = get_erom_ent(sih, &eromptr, 0, 0);
 
 		if ((cib & ER_TAG) != ER_CI) {
-			SI_ERROR(("CIA not followed by CIB\n"));
+			/* CIA not followed by CIB */
 			goto error;
 		}
 
@@ -502,8 +598,6 @@ void ai_scan(struct si_pub *sih, void *regs)
 		nmp = (cib & CIB_NMP_MASK) >> CIB_NMP_SHIFT;
 		nsp = (cib & CIB_NSP_MASK) >> CIB_NSP_SHIFT;
 
-		SI_VMSG(("Found component 0x%04x/0x%04x rev %d at erom addr 0x%p, with nmw = %d, " "nsw = %d, nmp = %d & nsp = %d\n", mfg, cid, crev, base, nmw, nsw, nmp, nsp));
-
 		if (((mfg == MFGID_ARM) && (cid == DEF_AI_COMP)) || (nsp == 0))
 			continue;
 		if ((nmw + nsw == 0)) {
@@ -511,9 +605,8 @@ void ai_scan(struct si_pub *sih, void *regs)
 			if (cid == OOB_ROUTER_CORE_ID) {
 				asd = get_asd(sih, &eromptr, 0, 0, AD_ST_SLAVE,
 					      &addrl, &addrh, &sizel, &sizeh);
-				if (asd != 0) {
+				if (asd != 0)
 					sii->oob_router = addrl;
-				}
 			}
 			continue;
 		}
@@ -527,12 +620,9 @@ void ai_scan(struct si_pub *sih, void *regs)
 		for (i = 0; i < nmp; i++) {
 			mpd = get_erom_ent(sih, &eromptr, ER_VALID, ER_VALID);
 			if ((mpd & ER_TAG) != ER_MP) {
-				SI_ERROR(("Not enough MP entries for component 0x%x\n", cid));
+				/* Not enough MP entries for component */
 				goto error;
 			}
-			SI_VMSG(("  Master port %d, mp: %d id: %d\n", i,
-				 (mpd & MPD_MP_MASK) >> MPD_MP_SHIFT,
-				 (mpd & MPD_MUI_MASK) >> MPD_MUI_SHIFT));
 		}
 
 		/* First Slave Address Descriptor should be port 0:
@@ -550,7 +640,7 @@ void ai_scan(struct si_pub *sih, void *regs)
 				br = true;
 			else if ((addrh != 0) || (sizeh != 0)
 				 || (sizel != SI_CORE_SIZE)) {
-				SI_ERROR(("First Slave ASD for core 0x%04x malformed " "(0x%08x)\n", cid, asd));
+				/* First Slave ASD for core malformed */
 				goto error;
 			}
 		}
@@ -578,8 +668,7 @@ void ai_scan(struct si_pub *sih, void *regs)
 					    &addrl, &addrh, &sizel, &sizeh);
 			} while (asd != 0);
 			if (j == 0) {
-				SI_ERROR((" SP %d has no address descriptors\n",
-					  i));
+				/* SP has no address descriptors */
 				goto error;
 			}
 		}
@@ -590,11 +679,11 @@ void ai_scan(struct si_pub *sih, void *regs)
 			    get_asd(sih, &eromptr, i, 0, AD_ST_MWRAP, &addrl,
 				    &addrh, &sizel, &sizeh);
 			if (asd == 0) {
-				SI_ERROR(("Missing descriptor for MW %d\n", i));
+				/* Missing descriptor for MW */
 				goto error;
 			}
 			if ((sizeh != 0) || (sizel != SI_CORE_SIZE)) {
-				SI_ERROR(("Master wrapper %d is not 4KB\n", i));
+				/* Master wrapper %d is not 4KB */
 				goto error;
 			}
 			if (i == 0)
@@ -608,11 +697,11 @@ void ai_scan(struct si_pub *sih, void *regs)
 			    get_asd(sih, &eromptr, fwp + i, 0, AD_ST_SWRAP,
 				    &addrl, &addrh, &sizel, &sizeh);
 			if (asd == 0) {
-				SI_ERROR(("Missing descriptor for SW %d\n", i));
+				/* Missing descriptor for SW */
 				goto error;
 			}
 			if ((sizeh != 0) || (sizel != SI_CORE_SIZE)) {
-				SI_ERROR(("Slave wrapper %d is not 4KB\n", i));
+				/* Slave wrapper is not 4KB */
 				goto error;
 			}
 			if ((nmw == 0) && (i == 0))
@@ -627,62 +716,35 @@ void ai_scan(struct si_pub *sih, void *regs)
 		sii->numcores++;
 	}
 
-	SI_ERROR(("Reached end of erom without finding END"));
-
  error:
+	/* Reached end of erom without finding END */
 	sii->numcores = 0;
 	return;
 }
 
-/* This function changes the logical "focus" to the indicated core.
- * Return the current core's virtual address.
+/*
+ * This function changes the logical "focus" to the indicated core.
+ * Return the current core's virtual address. Since each core starts with the
+ * same set of registers (BIST, clock control, etc), the returned address
+ * contains the first register of this 'common' register block (not to be
+ * confused with 'common core').
  */
-void *ai_setcoreidx(struct si_pub *sih, uint coreidx)
+void __iomem *ai_setcoreidx(struct si_pub *sih, uint coreidx)
 {
-	struct si_info *sii = SI_INFO(sih);
+	struct si_info *sii = (struct si_info *)sih;
 	u32 addr = sii->coresba[coreidx];
 	u32 wrap = sii->wrapba[coreidx];
-	void *regs;
 
 	if (coreidx >= sii->numcores)
 		return NULL;
 
-	switch (sih->bustype) {
-	case SI_BUS:
-		/* map new one */
-		if (!sii->regs[coreidx]) {
-			sii->regs[coreidx] = REG_MAP(addr, SI_CORE_SIZE);
-		}
-		sii->curmap = regs = sii->regs[coreidx];
-		if (!sii->wrappers[coreidx]) {
-			sii->wrappers[coreidx] = REG_MAP(wrap, SI_CORE_SIZE);
-		}
-		sii->curwrap = sii->wrappers[coreidx];
-		break;
-
-	case PCI_BUS:
-		/* point bar0 window */
-		pci_write_config_dword(sii->pbus, PCI_BAR0_WIN, addr);
-		regs = sii->curmap;
-		/* point bar0 2nd 4KB window */
-		pci_write_config_dword(sii->pbus, PCI_BAR0_WIN2, wrap);
-		break;
-
-	case SPI_BUS:
-	case SDIO_BUS:
-		sii->curmap = regs = (void *)(unsigned long)addr;
-		sii->curwrap = (void *)(unsigned long)wrap;
-		break;
-
-	default:
-		regs = NULL;
-		break;
-	}
-
-	sii->curmap = regs;
+	/* point bar0 window */
+	pci_write_config_dword(sii->pbus, PCI_BAR0_WIN, addr);
+	/* point bar0 2nd 4KB window */
+	pci_write_config_dword(sii->pbus, PCI_BAR0_WIN2, wrap);
 	sii->curidx = coreidx;
 
-	return regs;
+	return sii->curmap;
 }
 
 /* Return the number of address spaces in current core */
@@ -697,7 +759,7 @@ u32 ai_addrspace(struct si_pub *sih, uint asidx)
 	struct si_info *sii;
 	uint cidx;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 	cidx = sii->curidx;
 
 	if (asidx == 0)
@@ -705,7 +767,7 @@ u32 ai_addrspace(struct si_pub *sih, uint asidx)
 	else if (asidx == 1)
 		return sii->coresba2[cidx];
 	else {
-		SI_ERROR(("%s: Need to parse the erom again to find addr space %d\n", __func__, asidx));
+		/* Need to parse the erom again to find addr space */
 		return 0;
 	}
 }
@@ -716,7 +778,7 @@ u32 ai_addrspacesize(struct si_pub *sih, uint asidx)
 	struct si_info *sii;
 	uint cidx;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 	cidx = sii->curidx;
 
 	if (asidx == 0)
@@ -724,7 +786,7 @@ u32 ai_addrspacesize(struct si_pub *sih, uint asidx)
 	else if (asidx == 1)
 		return sii->coresba2_size[cidx];
 	else {
-		SI_ERROR(("%s: Need to parse the erom again to find addr space %d\n", __func__, asidx));
+		/* Need to parse the erom again to find addr */
 		return 0;
 	}
 }
@@ -734,11 +796,7 @@ uint ai_flag(struct si_pub *sih)
 	struct si_info *sii;
 	struct aidmp *ai;
 
-	sii = SI_INFO(sih);
-	if (BCM47162_DMP()) {
-		SI_ERROR(("%s: Attempting to read MIPS DMP registers on 47162a0", __func__));
-		return sii->curidx;
-	}
+	sii = (struct si_info *)sih;
 	ai = sii->curwrap;
 
 	return R_REG(&ai->oobselouta30) & 0x1f;
@@ -753,7 +811,7 @@ uint ai_corevendor(struct si_pub *sih)
 	struct si_info *sii;
 	u32 cia;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 	cia = sii->cia[sii->curidx];
 	return (cia & CIA_MFG_MASK) >> CIA_MFG_SHIFT;
 }
@@ -763,7 +821,7 @@ uint ai_corerev(struct si_pub *sih)
 	struct si_info *sii;
 	u32 cib;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 	cib = sii->cib[sii->curidx];
 	return (cib & CIB_REV_MASK) >> CIB_REV_SHIFT;
 }
@@ -773,7 +831,7 @@ bool ai_iscoreup(struct si_pub *sih)
 	struct si_info *sii;
 	struct aidmp *ai;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 	ai = sii->curwrap;
 
 	return (((R_REG(&ai->ioctrl) & (SICF_FGC | SICF_CLOCK_EN)) ==
@@ -787,13 +845,7 @@ void ai_core_cflags_wo(struct si_pub *sih, u32 mask, u32 val)
 	struct aidmp *ai;
 	u32 w;
 
-	sii = SI_INFO(sih);
-
-	if (BCM47162_DMP()) {
-		SI_ERROR(("%s: Accessing MIPS DMP register (ioctrl) on 47162a0",
-			  __func__));
-		return;
-	}
+	sii = (struct si_info *)sih;
 
 	ai = sii->curwrap;
 
@@ -809,13 +861,7 @@ u32 ai_core_cflags(struct si_pub *sih, u32 mask, u32 val)
 	struct aidmp *ai;
 	u32 w;
 
-	sii = SI_INFO(sih);
-	if (BCM47162_DMP()) {
-		SI_ERROR(("%s: Accessing MIPS DMP register (ioctrl) on 47162a0",
-			  __func__));
-		return 0;
-	}
-
+	sii = (struct si_info *)sih;
 	ai = sii->curwrap;
 
 	if (mask || val) {
@@ -826,18 +872,35 @@ u32 ai_core_cflags(struct si_pub *sih, u32 mask, u32 val)
 	return R_REG(&ai->ioctrl);
 }
 
+/* return true if PCIE capability exists in the pci config space */
+static bool ai_ispcie(struct si_info *sii)
+{
+	u8 cap_ptr;
+
+	cap_ptr =
+	    pcicore_find_pci_capability(sii->pbus, PCI_CAP_ID_EXP, NULL,
+					NULL);
+	if (!cap_ptr)
+		return false;
+
+	return true;
+}
+
+static bool ai_buscore_prep(struct si_info *sii)
+{
+	/* kludge to enable the clock on the 4306 which lacks a slowclock */
+	if (!ai_ispcie(sii))
+		ai_clkctl_xtal(&sii->pub, XTAL | PLL, ON);
+	return true;
+}
+
 u32 ai_core_sflags(struct si_pub *sih, u32 mask, u32 val)
 {
 	struct si_info *sii;
 	struct aidmp *ai;
 	u32 w;
 
-	sii = SI_INFO(sih);
-	if (BCM47162_DMP()) {
-		SI_ERROR(("%s: Accessing MIPS DMP register (iostatus) on 47162a0", __func__));
-		return 0;
-	}
-
+	sii = (struct si_info *)sih;
 	ai = sii->curwrap;
 
 	if (mask || val) {
@@ -848,74 +911,13 @@ u32 ai_core_sflags(struct si_pub *sih, u32 mask, u32 val)
 	return R_REG(&ai->iostatus);
 }
 
-/* *************** from siutils.c ************** */
-/* local prototypes */
-static struct si_info *ai_doattach(struct si_info *sii, void *regs,
-			      uint bustype, void *sdh, char **vars,
-			      uint *varsz);
-static bool ai_buscore_prep(struct si_info *sii, uint bustype);
-static bool ai_buscore_setup(struct si_info *sii, chipcregs_t *cc, uint bustype,
-			     u32 savewin, uint *origidx, void *regs);
-static void ai_nvram_process(struct si_info *sii, char *pvars);
-
-/* dev path concatenation util */
-static char *ai_devpathvar(struct si_pub *sih, char *var, int len,
-			   const char *name);
-static bool _ai_clkctl_cc(struct si_info *sii, uint mode);
-static bool ai_ispcie(struct si_info *sii);
-
-/* global variable to indicate reservation/release of gpio's */
-static u32 ai_gpioreservation;
-
-/*
- * Allocate a si handle.
- * devid - pci device id (used to determine chip#)
- * osh - opaque OS handle
- * regs - virtual address of initial core registers
- * bustype - pci/sb/sdio/etc
- * vars - pointer to a pointer area for "environment" variables
- * varsz - pointer to int to return the size of the vars
- */
-struct si_pub *ai_attach(void *regs, uint bustype,
-		void *sdh, char **vars, uint *varsz)
-{
-	struct si_info *sii;
-
-	/* alloc struct si_info */
-	sii = kmalloc(sizeof(struct si_info), GFP_ATOMIC);
-	if (sii == NULL) {
-		SI_ERROR(("si_attach: malloc failed!\n"));
-		return NULL;
-	}
-
-	if (ai_doattach(sii, regs, bustype, sdh, vars, varsz) ==
-	    NULL) {
-		kfree(sii);
-		return NULL;
-	}
-	sii->vars = vars ? *vars : NULL;
-	sii->varsz = varsz ? *varsz : 0;
-
-	return (struct si_pub *) sii;
-}
-
-/* global kernel resource */
-static struct si_info ksii;
-
-static bool ai_buscore_prep(struct si_info *sii, uint bustype)
-{
-	/* kludge to enable the clock on the 4306 which lacks a slowclock */
-	if (bustype == PCI_BUS && !ai_ispcie(sii))
-		ai_clkctl_xtal(&sii->pub, XTAL | PLL, ON);
-	return true;
-}
-
-static bool ai_buscore_setup(struct si_info *sii, chipcregs_t *cc, uint bustype,
-			     u32 savewin, uint *origidx, void *regs)
+static bool
+ai_buscore_setup(struct si_info *sii, u32 savewin, uint *origidx)
 {
 	bool pci, pcie;
 	uint i;
 	uint pciidx, pcieidx, pcirev, pcierev;
+	struct chipcregs __iomem *cc;
 
 	cc = ai_setcoreidx(&sii->pub, SI_CC_IDX);
 
@@ -955,25 +957,19 @@ static bool ai_buscore_setup(struct si_info *sii, chipcregs_t *cc, uint bustype,
 		cid = ai_coreid(&sii->pub);
 		crev = ai_corerev(&sii->pub);
 
-		/* Display cores found */
-		SI_VMSG(("CORE[%d]: id 0x%x rev %d base 0x%x regs 0x%p\n",
-			 i, cid, crev, sii->coresba[i], sii->regs[i]));
-
-		if (bustype == PCI_BUS) {
-			if (cid == PCI_CORE_ID) {
-				pciidx = i;
-				pcirev = crev;
-				pci = true;
-			} else if (cid == PCIE_CORE_ID) {
-				pcieidx = i;
-				pcierev = crev;
-				pcie = true;
-			}
+		if (cid == PCI_CORE_ID) {
+			pciidx = i;
+			pcirev = crev;
+			pci = true;
+		} else if (cid == PCIE_CORE_ID) {
+			pcieidx = i;
+			pcierev = crev;
+			pcie = true;
 		}
 
 		/* find the core idx before entering this func. */
 		if ((savewin && (savewin == sii->coresba[i])) ||
-		    (regs == sii->regs[i]))
+		    (cc == sii->regs[i]))
 			*origidx = i;
 	}
 
@@ -993,25 +989,19 @@ static bool ai_buscore_setup(struct si_info *sii, chipcregs_t *cc, uint bustype,
 		sii->pub.buscoreidx = pcieidx;
 	}
 
-	SI_VMSG(("Buscore id/type/rev %d/0x%x/%d\n", sii->pub.buscoreidx,
-		 sii->pub.buscoretype, sii->pub.buscorerev));
-
 	/* fixup necessary chip/core configurations */
-	if (sii->pub.bustype == PCI_BUS) {
-		if (SI_FAST(sii)) {
-			if (!sii->pch) {
-				sii->pch = (void *)pcicore_init(
-					&sii->pub, sii->pbus,
-					(void *)PCIEREGS(sii));
-				if (sii->pch == NULL)
-					return false;
-			}
-		}
-		if (ai_pci_fixcfg(&sii->pub)) {
-			SI_ERROR(("si_doattach: si_pci_fixcfg failed\n"));
-			return false;
+	if (SI_FAST(sii)) {
+		if (!sii->pch) {
+			sii->pch = pcicore_init(&sii->pub, sii->pbus,
+						(__iomem void *)PCIEREGS(sii));
+			if (sii->pch == NULL)
+				return false;
 		}
 	}
+	if (ai_pci_fixcfg(&sii->pub)) {
+		/* si_doattach: si_pci_fixcfg failed */
+		return false;
+	}
 
 	/* return to the original core */
 	ai_setcoreidx(&sii->pub, *origidx);
@@ -1019,63 +1009,27 @@ static bool ai_buscore_setup(struct si_info *sii, chipcregs_t *cc, uint bustype,
 	return true;
 }
 
-static __used void ai_nvram_process(struct si_info *sii, char *pvars)
+/*
+ * get boardtype and boardrev
+ */
+static __used void ai_nvram_process(struct si_info *sii)
 {
 	uint w = 0;
 
-	/* get boardtype and boardrev */
-	switch (sii->pub.bustype) {
-	case PCI_BUS:
-		/* do a pci config read to get subsystem id and subvendor id */
-		pci_read_config_dword(sii->pbus, PCI_SUBSYSTEM_VENDOR_ID, &w);
-		/* Let nvram variables override subsystem Vend/ID */
-		sii->pub.boardvendor = (u16)ai_getdevpathintvar(&sii->pub,
-			"boardvendor");
-		if (sii->pub.boardvendor == 0)
-			sii->pub.boardvendor = w & 0xffff;
-		else
-			SI_ERROR(("Overriding boardvendor: 0x%x instead of "
-				  "0x%x\n", sii->pub.boardvendor, w & 0xffff));
-		sii->pub.boardtype = (u16)ai_getdevpathintvar(&sii->pub,
-			"boardtype");
-		if (sii->pub.boardtype == 0)
-			sii->pub.boardtype = (w >> 16) & 0xffff;
-		else
-			SI_ERROR(("Overriding boardtype: 0x%x instead of 0x%x\n"
-				  , sii->pub.boardtype, (w >> 16) & 0xffff));
-		break;
-
-		sii->pub.boardvendor = getintvar(pvars, "manfid");
-		sii->pub.boardtype = getintvar(pvars, "prodid");
-		break;
-
-	case SI_BUS:
-	case JTAG_BUS:
-		sii->pub.boardvendor = PCI_VENDOR_ID_BROADCOM;
-		sii->pub.boardtype = getintvar(pvars, "prodid");
-		if (pvars == NULL || (sii->pub.boardtype == 0)) {
-			sii->pub.boardtype = getintvar(NULL, "boardtype");
-			if (sii->pub.boardtype == 0)
-				sii->pub.boardtype = 0xffff;
-		}
-		break;
-	}
-
-	if (sii->pub.boardtype == 0) {
-		SI_ERROR(("si_doattach: unknown board type\n"));
-	}
+	/* do a pci config read to get subsystem id and subvendor id */
+	pci_read_config_dword(sii->pbus, PCI_SUBSYSTEM_VENDOR_ID, &w);
 
-	sii->pub.boardflags = getintvar(pvars, "boardflags");
+	sii->pub.boardvendor = w & 0xffff;
+	sii->pub.boardtype = (w >> 16) & 0xffff;
+	sii->pub.boardflags = getintvar(&sii->pub, BRCMS_SROM_BOARDFLAGS);
 }
 
 static struct si_info *ai_doattach(struct si_info *sii,
-			      void *regs, uint bustype, void *pbus,
-			      char **vars, uint *varsz)
+				   void __iomem *regs, struct pci_dev *pbus)
 {
 	struct si_pub *sih = &sii->pub;
 	u32 w, savewin;
-	chipcregs_t *cc;
-	char *pvars = NULL;
+	struct chipcregs __iomem *cc;
 	uint socitype;
 	uint origidx;
 
@@ -1088,37 +1042,18 @@ static struct si_info *ai_doattach(struct si_info *sii,
 	sii->curmap = regs;
 	sii->pbus = pbus;
 
-	/* check to see if we are a si core mimic'ing a pci core */
-	if (bustype == PCI_BUS) {
-		pci_read_config_dword(sii->pbus, PCI_SPROM_CONTROL,  &w);
-		if (w == 0xffffffff) {
-			SI_ERROR(("%s: incoming bus is PCI but it's a lie, "
-				" switching to SI devid:0x%x\n",
-				__func__, devid));
-			bustype = SI_BUS;
-		}
-	}
-
 	/* find Chipcommon address */
-	if (bustype == PCI_BUS) {
-		pci_read_config_dword(sii->pbus, PCI_BAR0_WIN, &savewin);
-		if (!GOODCOREADDR(savewin, SI_ENUM_BASE))
-			savewin = SI_ENUM_BASE;
-		pci_write_config_dword(sii->pbus, PCI_BAR0_WIN,
-				       SI_ENUM_BASE);
-		cc = (chipcregs_t *) regs;
-	} else {
-		cc = (chipcregs_t *) REG_MAP(SI_ENUM_BASE, SI_CORE_SIZE);
-	}
+	pci_read_config_dword(sii->pbus, PCI_BAR0_WIN, &savewin);
+	if (!GOODCOREADDR(savewin, SI_ENUM_BASE))
+		savewin = SI_ENUM_BASE;
 
-	sih->bustype = bustype;
+	pci_write_config_dword(sii->pbus, PCI_BAR0_WIN,
+			       SI_ENUM_BASE);
+	cc = (struct chipcregs __iomem *) regs;
 
 	/* bus/core/clk setup for register access */
-	if (!ai_buscore_prep(sii, bustype)) {
-		SI_ERROR(("si_doattach: si_core_clk_prep failed %d\n",
-			  bustype));
+	if (!ai_buscore_prep(sii))
 		return NULL;
-	}
 
 	/*
 	 * ChipID recognition.
@@ -1134,67 +1069,59 @@ static struct si_info *ai_doattach(struct si_info *sii,
 	sih->chiprev = (w & CID_REV_MASK) >> CID_REV_SHIFT;
 	sih->chippkg = (w & CID_PKG_MASK) >> CID_PKG_SHIFT;
 
-	sih->issim = IS_SIM(sih->chippkg);
+	sih->issim = false;
 
 	/* scan for cores */
 	if (socitype == SOCI_AI) {
 		SI_MSG(("Found chip type AI (0x%08x)\n", w));
 		/* pass chipc address instead of original core base */
-		ai_scan(&sii->pub, (void *)cc);
+		ai_scan(&sii->pub, cc);
 	} else {
-		SI_ERROR(("Found chip of unknown type (0x%08x)\n", w));
+		/* Found chip of unknown type */
 		return NULL;
 	}
 	/* no cores found, bail out */
-	if (sii->numcores == 0) {
-		SI_ERROR(("si_doattach: could not find any cores\n"));
+	if (sii->numcores == 0)
 		return NULL;
-	}
+
 	/* bus/core/clk setup */
 	origidx = SI_CC_IDX;
-	if (!ai_buscore_setup(sii, cc, bustype, savewin, &origidx, regs)) {
-		SI_ERROR(("si_doattach: si_buscore_setup failed\n"));
+	if (!ai_buscore_setup(sii, savewin, &origidx))
 		goto exit;
-	}
 
 	/* Init nvram from sprom/otp if they exist */
-	if (srom_var_init
-	    (&sii->pub, bustype, regs, vars, varsz)) {
-		SI_ERROR(("si_doattach: srom_var_init failed: bad srom\n"));
+	if (srom_var_init(&sii->pub, cc))
 		goto exit;
-	}
-	pvars = vars ? *vars : NULL;
-	ai_nvram_process(sii, pvars);
+
+	ai_nvram_process(sii);
 
 	/* === NVRAM, clock is ready === */
-	cc = (chipcregs_t *) ai_setcore(sih, CC_CORE_ID, 0);
+	cc = (struct chipcregs __iomem *) ai_setcore(sih, CC_CORE_ID, 0);
 	W_REG(&cc->gpiopullup, 0);
 	W_REG(&cc->gpiopulldown, 0);
 	ai_setcoreidx(sih, origidx);
 
 	/* PMU specific initializations */
-	if (PMUCTL_ENAB(sih)) {
+	if (sih->cccaps & CC_CAP_PMU) {
 		u32 xtalfreq;
 		si_pmu_init(sih);
 		si_pmu_chip_init(sih);
-		xtalfreq = getintvar(pvars, "xtalfreq");
-		/* If xtalfreq var not available, try to measure it */
-		if (xtalfreq == 0)
-			xtalfreq = si_pmu_measure_alpclk(sih);
+
+		xtalfreq = si_pmu_measure_alpclk(sih);
 		si_pmu_pll_init(sih, xtalfreq);
 		si_pmu_res_init(sih);
 		si_pmu_swreg_init(sih);
 	}
 
 	/* setup the GPIO based LED powersave register */
-	w = getintvar(pvars, "leddc");
+	w = getintvar(sih, BRCMS_SROM_LEDDC);
 	if (w == 0)
 		w = DEFAULT_GPIOTIMERVAL;
-	ai_corereg(sih, SI_CC_IDX, offsetof(chipcregs_t, gpiotimerval), ~0, w);
+	ai_corereg(sih, SI_CC_IDX, offsetof(struct chipcregs, gpiotimerval),
+		   ~0, w);
 
-	if (PCIE(sii)) {
-		pcicore_attach(sii->pch, pvars, SI_DOATTACH);
-	}
+	if (PCIE(sii))
+		pcicore_attach(sii->pch, SI_DOATTACH);
 
 	if (sih->chip == BCM43224_CHIP_ID) {
 		/*
@@ -1204,7 +1131,7 @@ static struct si_info *ai_doattach(struct si_info *sii,
 		if (sih->chiprev == 0) {
 			SI_MSG(("Applying 43224A0 WARs\n"));
 			ai_corereg(sih, SI_CC_IDX,
-				   offsetof(chipcregs_t, chipcontrol),
+				   offsetof(struct chipcregs, chipcontrol),
 				   CCTRL43224_GPIO_TOGGLE,
 				   CCTRL43224_GPIO_TOGGLE);
 			si_pmu_chipcontrol(sih, 0, CCTRL_43224A0_12MA_LED_DRIVE,
@@ -1228,45 +1155,58 @@ static struct si_info *ai_doattach(struct si_info *sii,
 	}
 
 	return sii;
+
  exit:
-	if (sih->bustype == PCI_BUS) {
-		if (sii->pch)
-			pcicore_deinit(sii->pch);
-		sii->pch = NULL;
-	}
+	if (sii->pch)
+		pcicore_deinit(sii->pch);
+	sii->pch = NULL;
 
 	return NULL;
 }
 
+/*
+ * Allocate a si handle.
+ * devid - pci device id (used to determine chip#)
+ * osh - opaque OS handle
+ * regs - virtual address of initial core registers
+ */
+struct si_pub *
+ai_attach(void __iomem *regs, struct pci_dev *sdh)
+{
+	struct si_info *sii;
+
+	/* alloc struct si_info */
+	sii = kmalloc(sizeof(struct si_info), GFP_ATOMIC);
+	if (sii == NULL)
+		return NULL;
+
+	if (ai_doattach(sii, regs, sdh) == NULL) {
+		kfree(sii);
+		return NULL;
+	}
+
+	return (struct si_pub *) sii;
+}
+
 /* may be called with core in reset */
 void ai_detach(struct si_pub *sih)
 {
 	struct si_info *sii;
-	uint idx;
 
 	struct si_pub *si_local = NULL;
 	memcpy(&si_local, &sih, sizeof(struct si_pub **));
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 
 	if (sii == NULL)
 		return;
 
-	if (sih->bustype == SI_BUS)
-		for (idx = 0; idx < SI_MAXCORES; idx++)
-			if (sii->regs[idx]) {
-				iounmap(sii->regs[idx]);
-				sii->regs[idx] = NULL;
-			}
-
-	if (sih->bustype == PCI_BUS) {
-		if (sii->pch)
-			pcicore_deinit(sii->pch);
-		sii->pch = NULL;
-	}
+	if (sii->pch)
+		pcicore_deinit(sii->pch);
+	sii->pch = NULL;
 
-	if (sii != &ksii)
-		kfree(sii);
+	srom_free_vars(sih);
+	kfree(sii);
 }
 
 /* register driver interrupt disabling and restoring callback functions */
@@ -1277,11 +1217,11 @@ ai_register_intr_callback(struct si_pub *sih, void *intrsoff_fn,
 {
 	struct si_info *sii;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 	sii->intr_arg = intr_arg;
-	sii->intrsoff_fn = (si_intrsoff_t) intrsoff_fn;
-	sii->intrsrestore_fn = (si_intrsrestore_t) intrsrestore_fn;
-	sii->intrsenabled_fn = (si_intrsenabled_t) intrsenabled_fn;
+	sii->intrsoff_fn = (u32 (*)(void *)) intrsoff_fn;
+	sii->intrsrestore_fn = (void (*) (void *, u32)) intrsrestore_fn;
+	sii->intrsenabled_fn = (bool (*)(void *)) intrsenabled_fn;
 	/* save current core id.  when this function called, the current core
 	 * must be the core which provides driver functions(il, et, wl, etc.)
 	 */
@@ -1292,7 +1232,7 @@ void ai_deregister_intr_callback(struct si_pub *sih)
 {
 	struct si_info *sii;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 	sii->intrsoff_fn = NULL;
 }
 
@@ -1300,7 +1240,7 @@ uint ai_coreid(struct si_pub *sih)
 {
 	struct si_info *sii;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 	return sii->coreid[sii->curidx];
 }
 
@@ -1308,7 +1248,7 @@ uint ai_coreidx(struct si_pub *sih)
 {
 	struct si_info *sii;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 	return sii->curidx;
 }
 
@@ -1324,7 +1264,7 @@ uint ai_findcoreidx(struct si_pub *sih, uint coreid, uint coreunit)
 	uint found;
 	uint i;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 
 	found = 0;
 
@@ -1344,25 +1284,25 @@ uint ai_findcoreidx(struct si_pub *sih, uint coreid, uint coreunit)
  * Moreover, callers should keep interrupts off during switching
  * out of and back to d11 core.
  */
-void *ai_setcore(struct si_pub *sih, uint coreid, uint coreunit)
+void __iomem *ai_setcore(struct si_pub *sih, uint coreid, uint coreunit)
 {
 	uint idx;
 
 	idx = ai_findcoreidx(sih, coreid, coreunit);
-	if (!GOODIDX(idx))
+	if (idx >= SI_MAXCORES)
 		return NULL;
 
 	return ai_setcoreidx(sih, idx);
 }
 
 /* Turn off interrupt as required by ai_setcore, before switch core */
-void *ai_switch_core(struct si_pub *sih, uint coreid, uint *origidx,
-		     uint *intr_val)
+void __iomem *ai_switch_core(struct si_pub *sih, uint coreid, uint *origidx,
+			     uint *intr_val)
 {
-	void *cc;
+	void __iomem *cc;
 	struct si_info *sii;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 
 	if (SI_FAST(sii)) {
 		/* Overloading the origidx variable to remember the coreid,
@@ -1371,9 +1311,9 @@ void *ai_switch_core(struct si_pub *sih, uint coreid, uint *origidx,
 		 */
 		*origidx = coreid;
 		if (coreid == CC_CORE_ID)
-			return (void *)CCREGS_FAST(sii);
+			return CCREGS_FAST(sii);
 		else if (coreid == sih->buscoretype)
-			return (void *)PCIEREGS(sii);
+			return PCIEREGS(sii);
 	}
 	INTR_OFF(sii, *intr_val);
 	*origidx = sii->curidx;
@@ -1386,7 +1326,7 @@ void ai_restore_core(struct si_pub *sih, uint coreid, uint intr_val)
 {
 	struct si_info *sii;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 	if (SI_FAST(sii)
 	    && ((coreid == CC_CORE_ID) || (coreid == sih->buscoretype)))
 		return;
@@ -1397,7 +1337,7 @@ void ai_restore_core(struct si_pub *sih, uint coreid, uint intr_val)
 
 void ai_write_wrapperreg(struct si_pub *sih, u32 offset, u32 val)
 {
-	struct si_info *sii = SI_INFO(sih);
+	struct si_info *sii = (struct si_info *)sih;
 	u32 *w = (u32 *) sii->curwrap;
 	W_REG(w + (offset / 4), val);
 	return;
@@ -1417,54 +1357,40 @@ uint ai_corereg(struct si_pub *sih, uint coreidx, uint regoff, uint mask,
 		uint val)
 {
 	uint origidx = 0;
-	u32 *r = NULL;
+	u32 __iomem *r = NULL;
 	uint w;
 	uint intr_val = 0;
 	bool fast = false;
 	struct si_info *sii;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 
 	if (coreidx >= SI_MAXCORES)
 		return 0;
 
-	if (sih->bustype == SI_BUS) {
-		/* If internal bus, we can always get at everything */
+	/*
+	 * If pci/pcie, we can get at pci/pcie regs
+	 * and on newer cores to chipc
+	 */
+	if ((sii->coreid[coreidx] == CC_CORE_ID) && SI_FAST(sii)) {
+		/* Chipc registers are mapped at 12KB */
 		fast = true;
-		/* map if does not exist */
-		if (!sii->regs[coreidx]) {
-			sii->regs[coreidx] = REG_MAP(sii->coresba[coreidx],
-						     SI_CORE_SIZE);
-		}
-		r = (u32 *) ((unsigned char *) sii->regs[coreidx] + regoff);
-	} else if (sih->bustype == PCI_BUS) {
+		r = (u32 __iomem *)((__iomem char *)sii->curmap +
+				    PCI_16KB0_CCREGS_OFFSET + regoff);
+	} else if (sii->pub.buscoreidx == coreidx) {
 		/*
-		 * If pci/pcie, we can get at pci/pcie regs
-		 * and on newer cores to chipc
+		 * pci registers are at either in the last 2KB of
+		 * an 8KB window or, in pcie and pci rev 13 at 8KB
 		 */
-		if ((sii->coreid[coreidx] == CC_CORE_ID) && SI_FAST(sii)) {
-			/* Chipc registers are mapped at 12KB */
-
-			fast = true;
-			r = (u32 *) ((char *)sii->curmap +
-					PCI_16KB0_CCREGS_OFFSET + regoff);
-		} else if (sii->pub.buscoreidx == coreidx) {
-			/*
-			 * pci registers are at either in the last 2KB of
-			 * an 8KB window or, in pcie and pci rev 13 at 8KB
-			 */
-			fast = true;
-			if (SI_FAST(sii))
-				r = (u32 *) ((char *)sii->curmap +
-						PCI_16KB0_PCIREGS_OFFSET +
-						regoff);
-			else
-				r = (u32 *) ((char *)sii->curmap +
-						((regoff >= SBCONFIGOFF) ?
-						 PCI_BAR0_PCISBR_OFFSET :
-						 PCI_BAR0_PCIREGS_OFFSET) +
-						regoff);
-		}
+		fast = true;
+		if (SI_FAST(sii))
+			r = (u32 __iomem *)((__iomem char *)sii->curmap +
+				    PCI_16KB0_PCIREGS_OFFSET + regoff);
+		else
+			r = (u32 __iomem *)((__iomem char *)sii->curmap +
+				    ((regoff >= SBCONFIGOFF) ?
+				      PCI_BAR0_PCISBR_OFFSET :
+				      PCI_BAR0_PCIREGS_OFFSET) + regoff);
 	}
 
 	if (!fast) {
@@ -1474,8 +1400,8 @@ uint ai_corereg(struct si_pub *sih, uint coreidx, uint regoff, uint mask,
 		origidx = ai_coreidx(&sii->pub);
 
 		/* switch core */
-		r = (u32 *) ((unsigned char *) ai_setcoreidx(&sii->pub, coreidx)
-				+ regoff);
+		r = (u32 __iomem *) ((unsigned char __iomem *)
+			ai_setcoreidx(&sii->pub, coreidx) + regoff);
 	}
 
 	/* mask and set */
@@ -1504,7 +1430,7 @@ void ai_core_disable(struct si_pub *sih, u32 bits)
 	u32 dummy;
 	struct aidmp *ai;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 
 	ai = sii->curwrap;
 
@@ -1531,7 +1457,7 @@ void ai_core_reset(struct si_pub *sih, u32 bits, u32 resetbits)
 	struct aidmp *ai;
 	u32 dummy;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 	ai = sii->curwrap;
 
 	/*
@@ -1556,19 +1482,18 @@ void ai_core_reset(struct si_pub *sih, u32 bits, u32 resetbits)
 /* return the slow clock source - LPO, XTAL, or PCI */
 static uint ai_slowclk_src(struct si_info *sii)
 {
-	chipcregs_t *cc;
+	struct chipcregs __iomem *cc;
 	u32 val;
 
 	if (sii->pub.ccrev < 6) {
-		if (sii->pub.bustype == PCI_BUS) {
-			pci_read_config_dword(sii->pbus, PCI_GPIO_OUT,
-					      &val);
-			if (val & PCI_CFG_GPIO_SCS)
-				return SCC_SS_PCI;
-		}
+		pci_read_config_dword(sii->pbus, PCI_GPIO_OUT,
+				      &val);
+		if (val & PCI_CFG_GPIO_SCS)
+			return SCC_SS_PCI;
 		return SCC_SS_XTAL;
 	} else if (sii->pub.ccrev < 10) {
-		cc = (chipcregs_t *) ai_setcoreidx(&sii->pub, sii->curidx);
+		cc = (struct chipcregs __iomem *)
+			ai_setcoreidx(&sii->pub, sii->curidx);
 		return R_REG(&cc->slow_clk_ctl) & SCC_SS_MASK;
 	} else			/* Insta-clock */
 		return SCC_SS_XTAL;
@@ -1578,7 +1503,8 @@ static uint ai_slowclk_src(struct si_info *sii)
 * return the ILP (slowclock) min or max frequency
 * precondition: we've established the chip has dynamic clk control
 */
-static uint ai_slowclk_freq(struct si_info *sii, bool max_freq, chipcregs_t *cc)
+static uint ai_slowclk_freq(struct si_info *sii, bool max_freq,
+			    struct chipcregs __iomem *cc)
 {
 	u32 slowclk;
 	uint div;
@@ -1612,9 +1538,9 @@ static uint ai_slowclk_freq(struct si_info *sii, bool max_freq, chipcregs_t *cc)
 	return 0;
 }
 
-static void ai_clkctl_setdelay(struct si_info *sii, void *chipcregs)
+static void
+ai_clkctl_setdelay(struct si_info *sii, struct chipcregs __iomem *cc)
 {
-	chipcregs_t *cc = (chipcregs_t *) chipcregs;
 	uint slowmaxfreq, pll_delay, slowclk;
 	uint pll_on_delay, fref_sel_delay;
 
@@ -1646,21 +1572,22 @@ void ai_clkctl_init(struct si_pub *sih)
 {
 	struct si_info *sii;
 	uint origidx = 0;
-	chipcregs_t *cc;
+	struct chipcregs __iomem *cc;
 	bool fast;
 
-	if (!CCCTL_ENAB(sih))
+	if (!(sih->cccaps & CC_CAP_PWR_CTL))
 		return;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 	fast = SI_FAST(sii);
 	if (!fast) {
 		origidx = sii->curidx;
-		cc = (chipcregs_t *) ai_setcore(sih, CC_CORE_ID, 0);
+		cc = (struct chipcregs __iomem *)
+			ai_setcore(sih, CC_CORE_ID, 0);
 		if (cc == NULL)
 			return;
 	} else {
-		cc = (chipcregs_t *) CCREGS_FAST(sii);
+		cc = (struct chipcregs __iomem *) CCREGS_FAST(sii);
 		if (cc == NULL)
 			return;
 	}
@@ -1670,7 +1597,7 @@ void ai_clkctl_init(struct si_pub *sih)
 		SET_REG(&cc->system_clk_ctl, SYCC_CD_MASK,
 			(ILP_DIV_1MHZ << SYCC_CD_SHIFT));
 
-	ai_clkctl_setdelay(sii, (void *)cc);
+	ai_clkctl_setdelay(sii, cc);
 
 	if (!fast)
 		ai_setcoreidx(sih, origidx);
@@ -1684,21 +1611,21 @@ u16 ai_clkctl_fast_pwrup_delay(struct si_pub *sih)
 {
 	struct si_info *sii;
 	uint origidx = 0;
-	chipcregs_t *cc;
+	struct chipcregs __iomem *cc;
 	uint slowminfreq;
 	u16 fpdelay;
 	uint intr_val = 0;
 	bool fast;
 
-	sii = SI_INFO(sih);
-	if (PMUCTL_ENAB(sih)) {
+	sii = (struct si_info *)sih;
+	if (sih->cccaps & CC_CAP_PMU) {
 		INTR_OFF(sii, intr_val);
 		fpdelay = si_pmu_fast_pwrup_delay(sih);
 		INTR_RESTORE(sii, intr_val);
 		return fpdelay;
 	}
 
-	if (!CCCTL_ENAB(sih))
+	if (!(sih->cccaps & CC_CAP_PWR_CTL))
 		return 0;
 
 	fast = SI_FAST(sii);
@@ -1706,11 +1633,12 @@ u16 ai_clkctl_fast_pwrup_delay(struct si_pub *sih)
 	if (!fast) {
 		origidx = sii->curidx;
 		INTR_OFF(sii, intr_val);
-		cc = (chipcregs_t *) ai_setcore(sih, CC_CORE_ID, 0);
+		cc = (struct chipcregs __iomem *)
+			ai_setcore(sih, CC_CORE_ID, 0);
 		if (cc == NULL)
 			goto done;
 	} else {
-		cc = (chipcregs_t *) CCREGS_FAST(sii);
+		cc = (struct chipcregs __iomem *) CCREGS_FAST(sii);
 		if (cc == NULL)
 			goto done;
 	}
@@ -1733,99 +1661,68 @@ int ai_clkctl_xtal(struct si_pub *sih, uint what, bool on)
 	struct si_info *sii;
 	u32 in, out, outen;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 
-	switch (sih->bustype) {
-
-	case PCI_BUS:
-		/* pcie core doesn't have any mapping to control the xtal pu */
-		if (PCIE(sii))
-			return -1;
+	/* pcie core doesn't have any mapping to control the xtal pu */
+	if (PCIE(sii))
+		return -1;
 
-		pci_read_config_dword(sii->pbus, PCI_GPIO_IN, &in);
-		pci_read_config_dword(sii->pbus, PCI_GPIO_OUT, &out);
-		pci_read_config_dword(sii->pbus, PCI_GPIO_OUTEN, &outen);
+	pci_read_config_dword(sii->pbus, PCI_GPIO_IN, &in);
+	pci_read_config_dword(sii->pbus, PCI_GPIO_OUT, &out);
+	pci_read_config_dword(sii->pbus, PCI_GPIO_OUTEN, &outen);
 
-		/*
-		 * Avoid glitching the clock if GPRS is already using it.
-		 * We can't actually read the state of the PLLPD so we infer it
-		 * by the value of XTAL_PU which *is* readable via gpioin.
-		 */
-		if (on && (in & PCI_CFG_GPIO_XTAL))
-			return 0;
+	/*
+	 * Avoid glitching the clock if GPRS is already using it.
+	 * We can't actually read the state of the PLLPD so we infer it
+	 * by the value of XTAL_PU which *is* readable via gpioin.
+	 */
+	if (on && (in & PCI_CFG_GPIO_XTAL))
+		return 0;
 
-		if (what & XTAL)
-			outen |= PCI_CFG_GPIO_XTAL;
-		if (what & PLL)
-			outen |= PCI_CFG_GPIO_PLL;
-
-		if (on) {
-			/* turn primary xtal on */
-			if (what & XTAL) {
-				out |= PCI_CFG_GPIO_XTAL;
-				if (what & PLL)
-					out |= PCI_CFG_GPIO_PLL;
-				pci_write_config_dword(sii->pbus,
-						       PCI_GPIO_OUT, out);
-				pci_write_config_dword(sii->pbus,
-						       PCI_GPIO_OUTEN, outen);
-				udelay(XTAL_ON_DELAY);
-			}
+	if (what & XTAL)
+		outen |= PCI_CFG_GPIO_XTAL;
+	if (what & PLL)
+		outen |= PCI_CFG_GPIO_PLL;
 
-			/* turn pll on */
-			if (what & PLL) {
-				out &= ~PCI_CFG_GPIO_PLL;
-				pci_write_config_dword(sii->pbus,
-						       PCI_GPIO_OUT, out);
-				mdelay(2);
-			}
-		} else {
-			if (what & XTAL)
-				out &= ~PCI_CFG_GPIO_XTAL;
+	if (on) {
+		/* turn primary xtal on */
+		if (what & XTAL) {
+			out |= PCI_CFG_GPIO_XTAL;
 			if (what & PLL)
 				out |= PCI_CFG_GPIO_PLL;
 			pci_write_config_dword(sii->pbus,
 					       PCI_GPIO_OUT, out);
 			pci_write_config_dword(sii->pbus,
 					       PCI_GPIO_OUTEN, outen);
+			udelay(XTAL_ON_DELAY);
 		}
 
-	default:
-		return -1;
+		/* turn pll on */
+		if (what & PLL) {
+			out &= ~PCI_CFG_GPIO_PLL;
+			pci_write_config_dword(sii->pbus,
+					       PCI_GPIO_OUT, out);
+			mdelay(2);
+		}
+	} else {
+		if (what & XTAL)
+			out &= ~PCI_CFG_GPIO_XTAL;
+		if (what & PLL)
+			out |= PCI_CFG_GPIO_PLL;
+		pci_write_config_dword(sii->pbus,
+				       PCI_GPIO_OUT, out);
+		pci_write_config_dword(sii->pbus,
+				       PCI_GPIO_OUTEN, outen);
 	}
 
 	return 0;
 }
 
-/*
- *  clock control policy function throught chipcommon
- *
- *    set dynamic clk control mode (forceslow, forcefast, dynamic)
- *    returns true if we are forcing fast clock
- *    this is a wrapper over the next internal function
- *      to allow flexible policy settings for outside caller
- */
-bool ai_clkctl_cc(struct si_pub *sih, uint mode)
-{
-	struct si_info *sii;
-
-	sii = SI_INFO(sih);
-
-	/* chipcommon cores prior to rev6 don't support dynamic clock control */
-	if (sih->ccrev < 6)
-		return false;
-
-	if (PCI_FORCEHT(sii))
-		return mode == CLK_FAST;
-
-	return _ai_clkctl_cc(sii, mode);
-}
-
 /* clk control mechanism through chipcommon, no policy checking */
 static bool _ai_clkctl_cc(struct si_info *sii, uint mode)
 {
 	uint origidx = 0;
-	chipcregs_t *cc;
+	struct chipcregs __iomem *cc;
 	u32 scc;
 	uint intr_val = 0;
 	bool fast = SI_FAST(sii);
@@ -1837,20 +1734,15 @@ static bool _ai_clkctl_cc(struct si_info *sii, uint mode)
 	if (!fast) {
 		INTR_OFF(sii, intr_val);
 		origidx = sii->curidx;
-
-		if ((sii->pub.bustype == SI_BUS) &&
-		    ai_setcore(&sii->pub, MIPS33_CORE_ID, 0) &&
-		    (ai_corerev(&sii->pub) <= 7) && (sii->pub.ccrev >= 10))
-			goto done;
-
-		cc = (chipcregs_t *) ai_setcore(&sii->pub, CC_CORE_ID, 0);
+		cc = (struct chipcregs __iomem *)
+					ai_setcore(&sii->pub, CC_CORE_ID, 0);
 	} else {
-		cc = (chipcregs_t *) CCREGS_FAST(sii);
+		cc = (struct chipcregs __iomem *) CCREGS_FAST(sii);
 		if (cc == NULL)
 			goto done;
 	}
 
-	if (!CCCTL_ENAB(&sii->pub) && (sii->pub.ccrev < 20))
+	if (!(sii->pub.cccaps & CC_CAP_PWR_CTL) && (sii->pub.ccrev < 20))
 		goto done;
 
 	switch (mode) {
@@ -1870,7 +1762,7 @@ static bool _ai_clkctl_cc(struct si_info *sii, uint mode)
 		}
 
 		/* wait for the PLL */
-		if (PMUCTL_ENAB(&sii->pub)) {
+		if (sii->pub.cccaps & CC_CAP_PMU) {
 			u32 htavail = CCS_HTAVAIL;
 			SPINWAIT(((R_REG(&cc->clk_ctl_st) & htavail)
 				  == 0), PMU_MAX_TRANSITION_DLY);
@@ -1913,7 +1805,31 @@ static bool _ai_clkctl_cc(struct si_info *sii, uint mode)
 	return mode == CLK_FAST;
 }
 
-/* Build device path. Support SI, PCI, and JTAG for now. */
+/*
+ *  clock control policy function throught chipcommon
+ *
+ *    set dynamic clk control mode (forceslow, forcefast, dynamic)
+ *    returns true if we are forcing fast clock
+ *    this is a wrapper over the next internal function
+ *      to allow flexible policy settings for outside caller
+ */
+bool ai_clkctl_cc(struct si_pub *sih, uint mode)
+{
+	struct si_info *sii;
+
+	sii = (struct si_info *)sih;
+
+	/* chipcommon cores prior to rev6 don't support dynamic clock control */
+	if (sih->ccrev < 6)
+		return false;
+
+	if (PCI_FORCEHT(sii))
+		return mode == CLK_FAST;
+
+	return _ai_clkctl_cc(sii, mode);
+}
+
+/* Build device path */
 int ai_devpath(struct si_pub *sih, char *path, int size)
 {
 	int slen;
@@ -1921,22 +1837,10 @@ int ai_devpath(struct si_pub *sih, char *path, int size)
 	if (!path || size <= 0)
 		return -1;
 
-	switch (sih->bustype) {
-	case SI_BUS:
-	case JTAG_BUS:
-		slen = snprintf(path, (size_t) size, "sb/%u/", ai_coreidx(sih));
-		break;
-	case PCI_BUS:
-		slen = snprintf(path, (size_t) size, "pci/%u/%u/",
-			((struct pci_dev *)((SI_INFO(sih))->pbus))->bus->number,
-			PCI_SLOT(
-			    ((struct pci_dev *)((SI_INFO(sih))->pbus))->devfn));
-		break;
-
-	default:
-		slen = -1;
-		break;
-	}
+	slen = snprintf(path, (size_t) size, "pci/%u/%u/",
+		((struct si_info *)sih)->pbus->bus->number,
+		PCI_SLOT(((struct pci_dev *)
+				(((struct si_info *)(sih))->pbus))->devfn));
 
 	if (slen < 0 || slen >= size) {
 		path[0] = '\0';
@@ -1946,95 +1850,11 @@ int ai_devpath(struct si_pub *sih, char *path, int size)
 	return 0;
 }
 
-/* Get a variable, but only if it has a devpath prefix */
-char *ai_getdevpathvar(struct si_pub *sih, const char *name)
-{
-	char varname[SI_DEVPATH_BUFSZ + 32];
-
-	ai_devpathvar(sih, varname, sizeof(varname), name);
-
-	return getvar(NULL, varname);
-}
-
-/* Get a variable, but only if it has a devpath prefix */
-int ai_getdevpathintvar(struct si_pub *sih, const char *name)
-{
-#if defined(BCMBUSTYPE) && (BCMBUSTYPE == SI_BUS)
-	return getintvar(NULL, name);
-#else
-	char varname[SI_DEVPATH_BUFSZ + 32];
-
-	ai_devpathvar(sih, varname, sizeof(varname), name);
-
-	return getintvar(NULL, varname);
-#endif
-}
-
-char *ai_getnvramflvar(struct si_pub *sih, const char *name)
-{
-	return getvar(NULL, name);
-}
-
-/* Concatenate the dev path with a varname into the given 'var' buffer
- * and return the 'var' pointer. Nothing is done to the arguments if
- * len == 0 or var is NULL, var is still returned. On overflow, the
- * first char will be set to '\0'.
- */
-static char *ai_devpathvar(struct si_pub *sih, char *var, int len,
-			   const char *name)
-{
-	uint path_len;
-
-	if (!var || len <= 0)
-		return var;
-
-	if (ai_devpath(sih, var, len) == 0) {
-		path_len = strlen(var);
-
-		if (strlen(name) + 1 > (uint) (len - path_len))
-			var[0] = '\0';
-		else
-			strncpy(var + path_len, name, len - path_len - 1);
-	}
-
-	return var;
-}
-
-/* return true if PCIE capability exists in the pci config space */
-static bool ai_ispcie(struct si_info *sii)
-{
-	u8 cap_ptr;
-
-	if (sii->pub.bustype != PCI_BUS)
-		return false;
-
-	cap_ptr =
-	    pcicore_find_pci_capability(sii->pbus, PCI_CAP_ID_EXP, NULL,
-					NULL);
-	if (!cap_ptr)
-		return false;
-
-	return true;
-}
-
-bool ai_pci_war16165(struct si_pub *sih)
-{
-	struct si_info *sii;
-
-	sii = SI_INFO(sih);
-
-	return PCI(sii) && (sih->buscorerev <= 10);
-}
-
 void ai_pci_up(struct si_pub *sih)
 {
 	struct si_info *sii;
 
-	sii = SI_INFO(sih);
-
-	/* if not pci bus, we're done */
-	if (sih->bustype != PCI_BUS)
-		return;
+	sii = (struct si_info *)sih;
 
 	if (PCI_FORCEHT(sii))
 		_ai_clkctl_cc(sii, CLK_FAST);
@@ -2049,7 +1869,7 @@ void ai_pci_sleep(struct si_pub *sih)
 {
 	struct si_info *sii;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 
 	pcicore_sleep(sii->pch);
 }
@@ -2059,11 +1879,7 @@ void ai_pci_down(struct si_pub *sih)
 {
 	struct si_info *sii;
 
-	sii = SI_INFO(sih);
-
-	/* if not pci bus, we're done */
-	if (sih->bustype != PCI_BUS)
-		return;
+	sii = (struct si_info *)sih;
 
 	/* release FORCEHT since chip is going to "down" state */
 	if (PCI_FORCEHT(sii))
@@ -2079,14 +1895,11 @@ void ai_pci_down(struct si_pub *sih)
 void ai_pci_setup(struct si_pub *sih, uint coremask)
 {
 	struct si_info *sii;
-	void *regs = NULL;
+	struct sbpciregs __iomem *regs = NULL;
 	u32 siflag = 0, w;
 	uint idx = 0;
 
-	sii = SI_INFO(sih);
-
-	if (sii->pub.bustype != PCI_BUS)
-		return;
+	sii = (struct si_info *)sih;
 
 	if (PCI(sii)) {
 		/* get current core index */
@@ -2128,9 +1941,8 @@ void ai_pci_setup(struct si_pub *sih, uint coremask)
 int ai_pci_fixcfg(struct si_pub *sih)
 {
 	uint origidx;
-	void *regs = NULL;
-
-	struct si_info *sii = SI_INFO(sih);
+	void __iomem *regs = NULL;
+	struct si_info *sii = (struct si_info *)sih;
 
 	/* Fixup PI in SROM shadow area to enable the correct PCI core access */
 	/* save the current index */
@@ -2138,7 +1950,11 @@ int ai_pci_fixcfg(struct si_pub *sih)
 
 	/* check 'pi' is correct and fix it if not */
 	regs = ai_setcore(&sii->pub, sii->pub.buscoretype, 0);
-	pcicore_fixcfg(sii->pch, regs);
+	if (sii->pub.buscoretype == PCIE_CORE_ID)
+		pcicore_fixcfg_pcie(sii->pch,
+				    (struct sbpcieregs __iomem *)regs);
+	else if (sii->pub.buscoretype == PCI_CORE_ID)
+		pcicore_fixcfg_pci(sii->pch, (struct sbpciregs __iomem *)regs);
 
 	/* restore the original index */
 	ai_setcoreidx(&sii->pub, origidx);
@@ -2152,47 +1968,34 @@ u32 ai_gpiocontrol(struct si_pub *sih, u32 mask, u32 val, u8 priority)
 {
 	uint regoff;
 
-	regoff = 0;
-
-	/* gpios could be shared on router platforms
-	 * ignore reservation if it's high priority (e.g., test apps)
-	 */
-	if ((priority != GPIO_HI_PRIORITY) &&
-	    (sih->bustype == SI_BUS) && (val || mask)) {
-		mask = priority ? (ai_gpioreservation & mask) :
-		    ((ai_gpioreservation | mask) & ~(ai_gpioreservation));
-		val &= mask;
-	}
-
-	regoff = offsetof(chipcregs_t, gpiocontrol);
+	regoff = offsetof(struct chipcregs, gpiocontrol);
 	return ai_corereg(sih, SI_CC_IDX, regoff, mask, val);
 }
 
 void ai_chipcontrl_epa4331(struct si_pub *sih, bool on)
 {
 	struct si_info *sii;
-	chipcregs_t *cc;
+	struct chipcregs __iomem *cc;
 	uint origidx;
 	u32 val;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 	origidx = ai_coreidx(sih);
 
-	cc = (chipcregs_t *) ai_setcore(sih, CC_CORE_ID, 0);
+	cc = (struct chipcregs __iomem *) ai_setcore(sih, CC_CORE_ID, 0);
 
 	val = R_REG(&cc->chipcontrol);
 
 	if (on) {
-		if (sih->chippkg == 9 || sih->chippkg == 0xb) {
+		if (sih->chippkg == 9 || sih->chippkg == 0xb)
 			/* Ext PA Controls for 4331 12x9 Package */
 			W_REG(&cc->chipcontrol, val |
-			      (CCTRL4331_EXTPA_EN |
-			       CCTRL4331_EXTPA_ON_GPIO2_5));
-		} else {
+			      CCTRL4331_EXTPA_EN |
+			      CCTRL4331_EXTPA_ON_GPIO2_5);
+		else
 			/* Ext PA Controls for 4331 12x12 Package */
 			W_REG(&cc->chipcontrol,
-			      val | (CCTRL4331_EXTPA_EN));
-		}
+			      val | CCTRL4331_EXTPA_EN);
 	} else {
 		val &= ~(CCTRL4331_EXTPA_EN | CCTRL4331_EXTPA_ON_GPIO2_5);
 		W_REG(&cc->chipcontrol, val);
@@ -2205,13 +2008,13 @@ void ai_chipcontrl_epa4331(struct si_pub *sih, bool on)
 void ai_epa_4313war(struct si_pub *sih)
 {
 	struct si_info *sii;
-	chipcregs_t *cc;
+	struct chipcregs __iomem *cc;
 	uint origidx;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
 	origidx = ai_coreidx(sih);
 
-	cc = (chipcregs_t *) ai_setcore(sih, CC_CORE_ID, 0);
+	cc = ai_setcore(sih, CC_CORE_ID, 0);
 
 	/* EPA Fix */
 	W_REG(&cc->gpiocontrol,
@@ -2226,15 +2029,12 @@ bool ai_deviceremoved(struct si_pub *sih)
 	u32 w;
 	struct si_info *sii;
 
-	sii = SI_INFO(sih);
+	sii = (struct si_info *)sih;
+
+	pci_read_config_dword(sii->pbus, PCI_VENDOR_ID, &w);
+	if ((w & 0xFFFF) != PCI_VENDOR_ID_BROADCOM)
+		return true;
 
-	switch (sih->bustype) {
-	case PCI_BUS:
-		pci_read_config_dword(sii->pbus, PCI_VENDOR_ID, &w);
-		if ((w & 0xFFFF) != PCI_VENDOR_ID_BROADCOM)
-			return true;
-		break;
-	}
 	return false;
 }
 
@@ -2243,13 +2043,13 @@ bool ai_is_sprom_available(struct si_pub *sih)
 	if (sih->ccrev >= 31) {
 		struct si_info *sii;
 		uint origidx;
-		chipcregs_t *cc;
+		struct chipcregs __iomem *cc;
 		u32 sromctrl;
 
 		if ((sih->cccaps & CC_CAP_SROM) == 0)
 			return false;
 
-		sii = SI_INFO(sih);
+		sii = (struct si_info *)sih;
 		origidx = sii->curidx;
 		cc = ai_setcoreidx(sih, SI_CC_IDX);
 		sromctrl = R_REG(&cc->sromcontrol);
diff --git a/drivers/staging/brcm80211/brcmsmac/aiutils.h b/drivers/staging/brcm80211/brcmsmac/aiutils.h
index e245c278bebc..106a7424a7cd 100644
--- a/drivers/staging/brcm80211/brcmsmac/aiutils.h
+++ b/drivers/staging/brcm80211/brcmsmac/aiutils.h
@@ -23,26 +23,6 @@
  * SOC Interconnect Address Map.
  * All regions may not exist on all chips.
  */
-/* Physical SDRAM */
-#define SI_SDRAM_BASE		0x00000000
-/* Host Mode sb2pcitranslation0 (64 MB) */
-#define SI_PCI_MEM		0x08000000
-#define SI_PCI_MEM_SZ		(64 * 1024 * 1024)
-/* Host Mode sb2pcitranslation1 (64 MB) */
-#define SI_PCI_CFG		0x0c000000
-/* Byteswapped Physical SDRAM */
-#define	SI_SDRAM_SWAPPED	0x10000000
-/* Region 2 for sdram (512 MB) */
-#define SI_SDRAM_R2		0x80000000
-
-#ifdef SI_ENUM_BASE_VARIABLE
-#define SI_ENUM_BASE		(sii->pub.si_enum_base)
-#else
-#define SI_ENUM_BASE		0x18000000	/* Enumeration space base */
-#endif				/* SI_ENUM_BASE_VARIABLE */
-
-/* Wrapper space base */
-#define SI_WRAP_BASE		0x18100000
 /* each core gets 4Kbytes for registers */
 #define SI_CORE_SIZE		0x1000
 /*
@@ -52,39 +32,9 @@
  */
 #define	SI_MAXCORES		16
 
-/* On-chip RAM on chips that also have DDR */
-#define	SI_FASTRAM		0x19000000
-#define	SI_FASTRAM_SWAPPED	0x19800000
-
-/* Flash Region 2 (region 1 shadowed here) */
-#define	SI_FLASH2		0x1c000000
-/* Size of Flash Region 2 */
-#define	SI_FLASH2_SZ		0x02000000
-/* ARM Cortex-M3 ROM */
-#define	SI_ARMCM3_ROM		0x1e000000
-/* MIPS Flash Region 1 */
-#define	SI_FLASH1		0x1fc00000
-/* MIPS Size of Flash Region 1 */
-#define	SI_FLASH1_SZ		0x00400000
-/* ARM7TDMI-S ROM */
-#define	SI_ARM7S_ROM		0x20000000
-/* ARM Cortex-M3 SRAM Region 2 */
-#define	SI_ARMCM3_SRAM2		0x60000000
-/* ARM7TDMI-S SRAM Region 2 */
-#define	SI_ARM7S_SRAM2		0x80000000
-/* ARM Flash Region 1 */
-#define	SI_ARM_FLASH1		0xffff0000
-/* ARM Size of Flash Region 1 */
-#define	SI_ARM_FLASH1_SZ	0x00010000
-
-/* Client Mode sb2pcitranslation2 (1 GB) */
-#define SI_PCI_DMA		0x40000000
-/* Client Mode sb2pcitranslation2 (1 GB) */
-#define SI_PCI_DMA2		0x80000000
 /* Client Mode sb2pcitranslation2 size in bytes */
 #define SI_PCI_DMA_SZ		0x40000000
-/* PCIE Client Mode sb2pcitranslation2 (2 ZettaBytes), low 32 bits */
-#define SI_PCIE_DMA_L32		0x00000000
+
 /* PCIE Client Mode sb2pcitranslation2 (2 ZettaBytes), high 32 bits */
 #define SI_PCIE_DMA_H32		0x80000000
 
@@ -203,18 +153,6 @@
 /* Minumum amount of flash we support */
 #define FLASH_MIN		0x00020000	/* Minimum flash size */
 
-/* A boot/binary may have an embedded block that describes its size  */
-#define	BISZ_OFFSET		0x3e0	/* At this offset into the binary */
-#define	BISZ_MAGIC		0x4249535a	/* Marked with value: 'BISZ' */
-#define	BISZ_MAGIC_IDX		0	/* Word 0: magic */
-#define	BISZ_TXTST_IDX		1	/*      1: text start */
-#define	BISZ_TXTEND_IDX		2	/*      2: text end */
-#define	BISZ_DATAST_IDX		3	/*      3: data start */
-#define	BISZ_DATAEND_IDX	4	/*      4: data end */
-#define	BISZ_BSSST_IDX		5	/*      5: bss start */
-#define	BISZ_BSSEND_IDX		6	/*      6: bss end */
-#define BISZ_SIZE		7	/* descriptor size in 32-bit integers */
-
 #define	CC_SROM_OTP		0x800	/* SROM/OTP address space */
 
 /* gpiotimerval */
@@ -224,93 +162,15 @@
 #define	CLKD_OTP		0x000f0000
 #define	CLKD_OTP_SHIFT		16
 
-/* When Srom support present, fields in sromcontrol */
-#define	SRC_START		0x80000000
-#define	SRC_BUSY		0x80000000
-#define	SRC_OPCODE		0x60000000
-#define	SRC_OP_READ		0x00000000
-#define	SRC_OP_WRITE		0x20000000
-#define	SRC_OP_WRDIS		0x40000000
-#define	SRC_OP_WREN		0x60000000
-#define	SRC_OTPSEL		0x00000010
-#define	SRC_LOCK		0x00000008
-#define	SRC_SIZE_MASK		0x00000006
-#define	SRC_SIZE_1K		0x00000000
-#define	SRC_SIZE_4K		0x00000002
-#define	SRC_SIZE_16K		0x00000004
-#define	SRC_SIZE_SHIFT		1
-#define	SRC_PRESENT		0x00000001
-
-/* 4330 chip-specific ChipStatus register bits */
-#define CST4330_CHIPMODE_SDIOD(cs)	(((cs) & 0x7) < 6)	/* SDIO || gSPI */
-#define CST4330_CHIPMODE_USB20D(cs)	(((cs) & 0x7) >= 6)	/* USB || USBDA */
-#define CST4330_CHIPMODE_SDIO(cs)	(((cs) & 0x4) == 0)	/* SDIO */
-#define CST4330_CHIPMODE_GSPI(cs)	(((cs) & 0x6) == 4)	/* gSPI */
-#define CST4330_CHIPMODE_USB(cs)	(((cs) & 0x7) == 6)	/* USB packet-oriented */
-#define CST4330_CHIPMODE_USBDA(cs)	(((cs) & 0x7) == 7)	/* USB Direct Access */
-#define	CST4330_OTP_PRESENT		0x00000010
-#define	CST4330_LPO_AUTODET_EN		0x00000020
-#define	CST4330_ARMREMAP_0		0x00000040
-#define	CST4330_SPROM_PRESENT		0x00000080	/* takes priority over OTP if both set */
-#define	CST4330_ILPDIV_EN		0x00000100
-#define	CST4330_LPO_SEL			0x00000200
-#define	CST4330_RES_INIT_MODE_SHIFT	10
-#define	CST4330_RES_INIT_MODE_MASK	0x00000c00
-#define CST4330_CBUCK_MODE_SHIFT	12
-#define CST4330_CBUCK_MODE_MASK		0x00003000
-#define	CST4330_CBUCK_POWER_OK		0x00004000
-#define	CST4330_BB_PLL_LOCKED		0x00008000
-
 /* Package IDs */
-#define BCM4329_289PIN_PKG_ID	0	/* 4329 289-pin package id */
-#define BCM4329_182PIN_PKG_ID	1	/* 4329N 182-pin package id */
 #define	BCM4717_PKG_ID		9	/* 4717 package id */
 #define	BCM4718_PKG_ID		10	/* 4718 package id */
-#define HDLSIM_PKG_ID		14	/* HDL simulator package id */
-#define HWSIM_PKG_ID		15	/* Hardware simulator package id */
 #define BCM43224_FAB_SMIC	0xa	/* the chip is manufactured by SMIC */
 
 /* these are router chips */
 #define	BCM4716_CHIP_ID		0x4716	/* 4716 chipcommon chipid */
 #define	BCM47162_CHIP_ID	47162	/* 47162 chipcommon chipid */
 #define	BCM4748_CHIP_ID		0x4748	/* 4716 chipcommon chipid (OTP, RBBU) */
-#define	BCM5356_CHIP_ID		0x5356	/* 5356 chipcommon chipid */
-#define	BCM5357_CHIP_ID		0x5357	/* 5357 chipcommon chipid */
-
-
-#define	SI_INFO(sih)	((struct si_info *)sih)
-
-#define	GOODCOREADDR(x, b) \
-	(((x) >= (b)) && ((x) < ((b) + SI_MAXCORES * SI_CORE_SIZE)) && \
-		IS_ALIGNED((x), SI_CORE_SIZE))
-#define	GOODREGS(regs) \
-	((regs) != NULL && IS_ALIGNED((unsigned long)(regs), SI_CORE_SIZE))
-#define BADCOREADDR	0
-#define	GOODIDX(idx)	(((uint)idx) < SI_MAXCORES)
-#define	NOREV		-1	/* Invalid rev */
-
-/* Newer chips can access PCI/PCIE and CC core without requiring to change
- * PCI BAR0 WIN
- */
-#define SI_FAST(si) (((si)->pub.buscoretype == PCIE_CORE_ID) ||	\
-		     (((si)->pub.buscoretype == PCI_CORE_ID) && \
-		      (si)->pub.buscorerev >= 13))
-
-#define PCIEREGS(si) (((char *)((si)->curmap) + PCI_16KB0_PCIREGS_OFFSET))
-#define CCREGS_FAST(si) (((char *)((si)->curmap) + PCI_16KB0_CCREGS_OFFSET))
-
-/*
- * Macros to disable/restore function core(D11, ENET, ILINE20, etc) interrupts
- * before after core switching to avoid invalid register accesss inside ISR.
- */
-#define INTR_OFF(si, intr_val) \
-	if ((si)->intrsoff_fn && \
-	    (si)->coreid[(si)->curidx] == (si)->dev_coreid) \
-		intr_val = (*(si)->intrsoff_fn)((si)->intr_arg)
-#define INTR_RESTORE(si, intr_val) \
-	if ((si)->intrsrestore_fn && \
-	    (si)->coreid[(si)->curidx] == (si)->dev_coreid) \
-		(*(si)->intrsrestore_fn)((si)->intr_arg, intr_val)
 
 /* dynamic clock control defines */
 #define	LPOMINFREQ		25000	/* low power oscillator min */
@@ -323,60 +183,6 @@
 #define	ILP_DIV_5MHZ		0	/* ILP = 5 MHz */
 #define	ILP_DIV_1MHZ		4	/* ILP = 1 MHz */
 
-#define PCI(si)		(((si)->pub.bustype == PCI_BUS) &&	\
-			 ((si)->pub.buscoretype == PCI_CORE_ID))
-#define PCIE(si)	(((si)->pub.bustype == PCI_BUS) &&	\
-			 ((si)->pub.buscoretype == PCIE_CORE_ID))
-#define PCI_FORCEHT(si)	\
-	(PCIE(si) && (si->pub.chip == BCM4716_CHIP_ID))
-
-/* GPIO Based LED powersave defines */
-#define DEFAULT_GPIO_ONTIME	10	/* Default: 10% on */
-#define DEFAULT_GPIO_OFFTIME	90	/* Default: 10% on */
-
-#ifndef DEFAULT_GPIOTIMERVAL
-#define DEFAULT_GPIOTIMERVAL \
-	((DEFAULT_GPIO_ONTIME << GPIO_ONTIME_SHIFT) | DEFAULT_GPIO_OFFTIME)
-#endif
-
-/*
- * Data structure to export all chip specific common variables
- *   public (read-only) portion of aiutils handle returned by si_attach()
- */
-struct si_pub {
-	uint bustype;		/* SI_BUS, PCI_BUS */
-	uint buscoretype;	/* PCI_CORE_ID, PCIE_CORE_ID, PCMCIA_CORE_ID */
-	uint buscorerev;	/* buscore rev */
-	uint buscoreidx;	/* buscore index */
-	int ccrev;		/* chip common core rev */
-	u32 cccaps;		/* chip common capabilities */
-	u32 cccaps_ext;	/* chip common capabilities extension */
-	int pmurev;		/* pmu core rev */
-	u32 pmucaps;		/* pmu capabilities */
-	uint boardtype;		/* board type */
-	uint boardvendor;	/* board vendor */
-	uint boardflags;	/* board flags */
-	uint boardflags2;	/* board flags2 */
-	uint chip;		/* chip number */
-	uint chiprev;		/* chip revision */
-	uint chippkg;		/* chip package option */
-	u32 chipst;		/* chip status */
-	bool issim;		/* chip is in simulation or emulation */
-	uint socirev;		/* SOC interconnect rev */
-	bool pci_pr32414;
-
-};
-
-/*
- * Many of the routines below take an 'sih' handle as their first arg.
- * Allocate this by calling si_attach().  Free it by calling si_detach().
- * At any one time, the sih is logically focused on one particular si core
- * (the "current core").
- * Use si_setcore() or si_setcoreidx() to change the association to another core
- */
-
-#define	BADIDX		(SI_MAXCORES + 1)
-
 /* clkctl xtal what flags */
 #define	XTAL			0x1	/* primary crystal oscillator (2050) */
 #define	PLL			0x2	/* main chip pll */
@@ -409,49 +215,39 @@ struct si_pub {
 #define SI_PCIDOWN	2
 #define SI_PCIUP	3
 
-/* PMU clock/power control */
-#if defined(BCMPMUCTL)
-#define PMUCTL_ENAB(sih)	(BCMPMUCTL)
-#else
-#define PMUCTL_ENAB(sih)	((sih)->cccaps & CC_CAP_PMU)
-#endif
-
-/* chipcommon clock/power control (exclusive with PMU's) */
-#if defined(BCMPMUCTL) && BCMPMUCTL
-#define CCCTL_ENAB(sih)		(0)
-#define CCPLL_ENAB(sih)		(0)
-#else
-#define CCCTL_ENAB(sih)		((sih)->cccaps & CC_CAP_PWR_CTL)
-#define CCPLL_ENAB(sih)		((sih)->cccaps & CC_CAP_PLL_MASK)
-#endif
-
-typedef void (*gpio_handler_t) (u32 stat, void *arg);
-
-/* External PA enable mask */
-#define GPIO_CTRL_EPA_EN_MASK 0x40
-
-#define	SI_ERROR(args)
-
-#ifdef BCMDBG
-#define	SI_MSG(args)	printk args
-#else
-#define	SI_MSG(args)
-#endif				/* BCMDBG */
-
-/* Define SI_VMSG to printf for verbose debugging, but don't check it in */
-#define	SI_VMSG(args)
+/*
+ * Data structure to export all chip specific common variables
+ *   public (read-only) portion of aiutils handle returned by si_attach()
+ */
+struct si_pub {
+	uint buscoretype;	/* PCI_CORE_ID, PCIE_CORE_ID, PCMCIA_CORE_ID */
+	uint buscorerev;	/* buscore rev */
+	uint buscoreidx;	/* buscore index */
+	int ccrev;		/* chip common core rev */
+	u32 cccaps;		/* chip common capabilities */
+	u32 cccaps_ext;	/* chip common capabilities extension */
+	int pmurev;		/* pmu core rev */
+	u32 pmucaps;		/* pmu capabilities */
+	uint boardtype;		/* board type */
+	uint boardvendor;	/* board vendor */
+	uint boardflags;	/* board flags */
+	uint boardflags2;	/* board flags2 */
+	uint chip;		/* chip number */
+	uint chiprev;		/* chip revision */
+	uint chippkg;		/* chip package option */
+	u32 chipst;		/* chip status */
+	bool issim;		/* chip is in simulation or emulation */
+	uint socirev;		/* SOC interconnect rev */
+	bool pci_pr32414;
 
-#define	IS_SIM(chippkg)	\
-	((chippkg == HDLSIM_PKG_ID) || (chippkg == HWSIM_PKG_ID))
+};
 
-typedef u32(*si_intrsoff_t) (void *intr_arg);
-typedef void (*si_intrsrestore_t) (void *intr_arg, u32 arg);
-typedef bool(*si_intrsenabled_t) (void *intr_arg);
+struct pci_dev;
 
 struct gpioh_item {
 	void *arg;
 	bool level;
-	gpio_handler_t handler;
+	void (*handler) (u32 stat, void *arg);
 	u32 event;
 	struct gpioh_item *next;
 };
@@ -459,20 +255,21 @@ struct gpioh_item {
 /* misc si info needed by some of the routines */
 struct si_info {
 	struct si_pub pub;	/* back plane public state (must be first) */
-	void *pbus;		/* handle to bus (pci/sdio/..) */
+	struct pci_dev *pbus;	/* handle to pci bus */
 	uint dev_coreid;	/* the core provides driver functions */
 	void *intr_arg;		/* interrupt callback function arg */
-	si_intrsoff_t intrsoff_fn;	/* turns chip interrupts off */
-	si_intrsrestore_t intrsrestore_fn; /* restore chip interrupts */
-	si_intrsenabled_t intrsenabled_fn; /* check if interrupts are enabled */
+	u32 (*intrsoff_fn) (void *intr_arg); /* turns chip interrupts off */
+	/* restore chip interrupts */
+	void (*intrsrestore_fn) (void *intr_arg, u32 arg);
+	/* check if interrupts are enabled */
+	bool (*intrsenabled_fn) (void *intr_arg);
 
-	void *pch;		/* PCI/E core handle */
+	struct pcicore_info *pch; /* PCI/E core handle */
 
-	char *vars;
-	uint varsz;
+	struct list_head var_list; /* list of srom variables */
 
-	void *curmap;		/* current regs va */
-	void *regs[SI_MAXCORES];	/* other regs va */
+	void __iomem *curmap;			/* current regs va */
+	void __iomem *regs[SI_MAXCORES];	/* other regs va */
 
 	uint curidx;		/* current core index */
 	uint numcores;		/* # discovered cores */
@@ -492,16 +289,22 @@ struct si_info {
 	u32 oob_router;	/* oob router registers for axi */
 };
 
-/* AMBA Interconnect exported externs */
-extern void ai_scan(struct si_pub *sih, void *regs);
+/*
+ * Many of the routines below take an 'sih' handle as their first arg.
+ * Allocate this by calling si_attach().  Free it by calling si_detach().
+ * At any one time, the sih is logically focused on one particular si core
+ * (the "current core").
+ * Use si_setcore() or si_setcoreidx() to change the association to another core
+ */
 
+
+/* AMBA Interconnect exported externs */
 extern uint ai_flag(struct si_pub *sih);
 extern void ai_setint(struct si_pub *sih, int siflag);
 extern uint ai_coreidx(struct si_pub *sih);
 extern uint ai_corevendor(struct si_pub *sih);
 extern uint ai_corerev(struct si_pub *sih);
 extern bool ai_iscoreup(struct si_pub *sih);
-extern void *ai_setcoreidx(struct si_pub *sih, uint coreidx);
 extern u32 ai_core_cflags(struct si_pub *sih, u32 mask, u32 val);
 extern void ai_core_cflags_wo(struct si_pub *sih, u32 mask, u32 val);
 extern u32 ai_core_sflags(struct si_pub *sih, u32 mask, u32 val);
@@ -515,12 +318,8 @@ extern u32 ai_addrspacesize(struct si_pub *sih, uint asidx);
 extern void ai_write_wrap_reg(struct si_pub *sih, u32 offset, u32 val);
 
 /* === exported functions === */
-extern struct si_pub *ai_attach(void *regs, uint bustype,
-		       void *sdh, char **vars, uint *varsz);
-
+extern struct si_pub *ai_attach(void __iomem *regs, struct pci_dev *sdh);
 extern void ai_detach(struct si_pub *sih);
-extern bool ai_pci_war16165(struct si_pub *sih);
-
 extern uint ai_coreid(struct si_pub *sih);
 extern uint ai_corerev(struct si_pub *sih);
 extern uint ai_corereg(struct si_pub *sih, uint coreidx, uint regoff, uint mask,
@@ -530,10 +329,10 @@ extern u32 ai_core_cflags(struct si_pub *sih, u32 mask, u32 val);
 extern u32 ai_core_sflags(struct si_pub *sih, u32 mask, u32 val);
 extern bool ai_iscoreup(struct si_pub *sih);
 extern uint ai_findcoreidx(struct si_pub *sih, uint coreid, uint coreunit);
-extern void *ai_setcoreidx(struct si_pub *sih, uint coreidx);
-extern void *ai_setcore(struct si_pub *sih, uint coreid, uint coreunit);
-extern void *ai_switch_core(struct si_pub *sih, uint coreid, uint *origidx,
-			    uint *intr_val);
+extern void __iomem *ai_setcoreidx(struct si_pub *sih, uint coreidx);
+extern void __iomem *ai_setcore(struct si_pub *sih, uint coreid, uint coreunit);
+extern void __iomem *ai_switch_core(struct si_pub *sih, uint coreid,
+				    uint *origidx, uint *intr_val);
 extern void ai_restore_core(struct si_pub *sih, uint coreid, uint intr_val);
 extern void ai_core_reset(struct si_pub *sih, u32 bits, u32 resetbits);
 extern void ai_core_disable(struct si_pub *sih, u32 bits);
@@ -566,9 +365,6 @@ extern bool ai_is_sprom_available(struct si_pub *sih);
  * Return 0 on success, nonzero otherwise.
  */
 extern int ai_devpath(struct si_pub *sih, char *path, int size);
-/* Read variable with prepending the devpath to the name */
-extern char *ai_getdevpathvar(struct si_pub *sih, const char *name);
-extern int ai_getdevpathintvar(struct si_pub *sih, const char *name);
 
 extern void ai_pci_sleep(struct si_pub *sih);
 extern void ai_pci_down(struct si_pub *sih);
@@ -579,6 +375,4 @@ extern void ai_chipcontrl_epa4331(struct si_pub *sih, bool on);
 /* Enable Ex-PA for 4313 */
 extern void ai_epa_4313war(struct si_pub *sih);
 
-char *ai_getnvramflvar(struct si_pub *sih, const char *name);
-
 #endif				/* _BRCM_AIUTILS_H_ */
diff --git a/drivers/staging/brcm80211/brcmsmac/alloc.c b/drivers/staging/brcm80211/brcmsmac/alloc.c
deleted file mode 100644
index 7f8dd7b396bf..000000000000
--- a/drivers/staging/brcm80211/brcmsmac/alloc.c
+++ /dev/null
@@ -1,275 +0,0 @@
-/*
- * Copyright (c) 2010 Broadcom Corporation
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
- * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
- * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
- * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-
-#include <brcmu_utils.h>
-#include "types.h"
-#include "pub.h"
-#include "main.h"
-#include "alloc.h"
-
-static struct brcms_bss_cfg *brcms_c_bsscfg_malloc(uint unit);
-static void brcms_c_bsscfg_mfree(struct brcms_bss_cfg *cfg);
-static struct brcms_pub *brcms_c_pub_malloc(uint unit,
-				      uint *err, uint devid);
-static void brcms_c_pub_mfree(struct brcms_pub *pub);
-static void brcms_c_tunables_init(struct brcms_tunables *tunables, uint devid);
-
-static void brcms_c_tunables_init(struct brcms_tunables *tunables, uint devid)
-{
-	tunables->ntxd = NTXD;
-	tunables->nrxd = NRXD;
-	tunables->rxbufsz = RXBUFSZ;
-	tunables->nrxbufpost = NRXBUFPOST;
-	tunables->maxscb = MAXSCB;
-	tunables->ampdunummpdu = AMPDU_NUM_MPDU;
-	tunables->maxpktcb = MAXPKTCB;
-	tunables->maxucodebss = BRCMS_MAX_UCODE_BSS;
-	tunables->maxucodebss4 = BRCMS_MAX_UCODE_BSS4;
-	tunables->maxbss = MAXBSS;
-	tunables->datahiwat = BRCMS_DATAHIWAT;
-	tunables->ampdudatahiwat = BRCMS_AMPDUDATAHIWAT;
-	tunables->rxbnd = RXBND;
-	tunables->txsbnd = TXSBND;
-}
-
-static struct brcms_pub *brcms_c_pub_malloc(uint unit, uint *err, uint devid)
-{
-	struct brcms_pub *pub;
-
-	pub = kzalloc(sizeof(struct brcms_pub), GFP_ATOMIC);
-	if (pub == NULL) {
-		*err = 1001;
-		goto fail;
-	}
-
-	pub->tunables = kzalloc(sizeof(struct brcms_tunables), GFP_ATOMIC);
-	if (pub->tunables == NULL) {
-		*err = 1028;
-		goto fail;
-	}
-
-	/* need to init the tunables now */
-	brcms_c_tunables_init(pub->tunables, devid);
-
-	pub->multicast = kzalloc(ETH_ALEN * MAXMULTILIST, GFP_ATOMIC);
-	if (pub->multicast == NULL) {
-		*err = 1003;
-		goto fail;
-	}
-
-	return pub;
-
- fail:
-	brcms_c_pub_mfree(pub);
-	return NULL;
-}
-
-static void brcms_c_pub_mfree(struct brcms_pub *pub)
-{
-	if (pub == NULL)
-		return;
-
-	kfree(pub->multicast);
-	kfree(pub->tunables);
-	kfree(pub);
-}
-
-static struct brcms_bss_cfg *brcms_c_bsscfg_malloc(uint unit)
-{
-	struct brcms_bss_cfg *cfg;
-
-	cfg = kzalloc(sizeof(struct brcms_bss_cfg), GFP_ATOMIC);
-	if (cfg == NULL)
-		goto fail;
-
-	cfg->current_bss = kzalloc(sizeof(struct brcms_bss_info), GFP_ATOMIC);
-	if (cfg->current_bss == NULL)
-		goto fail;
-
-	return cfg;
-
- fail:
-	brcms_c_bsscfg_mfree(cfg);
-	return NULL;
-}
-
-static void brcms_c_bsscfg_mfree(struct brcms_bss_cfg *cfg)
-{
-	if (cfg == NULL)
-		return;
-
-	kfree(cfg->maclist);
-	kfree(cfg->current_bss);
-	kfree(cfg);
-}
-
-static void brcms_c_bsscfg_ID_assign(struct brcms_c_info *wlc,
-				 struct brcms_bss_cfg *bsscfg)
-{
-	bsscfg->ID = wlc->next_bsscfg_ID;
-	wlc->next_bsscfg_ID++;
-}
-
-/*
- * The common driver entry routine. Error codes should be unique
- */
-struct brcms_c_info *brcms_c_attach_malloc(uint unit, uint *err, uint devid)
-{
-	struct brcms_c_info *wlc;
-
-	wlc = kzalloc(sizeof(struct brcms_c_info), GFP_ATOMIC);
-	if (wlc == NULL) {
-		*err = 1002;
-		goto fail;
-	}
-
-	/* allocate struct brcms_c_pub state structure */
-	wlc->pub = brcms_c_pub_malloc(unit, err, devid);
-	if (wlc->pub == NULL) {
-		*err = 1003;
-		goto fail;
-	}
-	wlc->pub->wlc = wlc;
-
-	/* allocate struct brcms_hardware state structure */
-
-	wlc->hw = kzalloc(sizeof(struct brcms_hardware), GFP_ATOMIC);
-	if (wlc->hw == NULL) {
-		*err = 1005;
-		goto fail;
-	}
-	wlc->hw->wlc = wlc;
-
-	wlc->hw->bandstate[0] =
-		kzalloc(sizeof(struct brcms_hw_band) * MAXBANDS, GFP_ATOMIC);
-	if (wlc->hw->bandstate[0] == NULL) {
-		*err = 1006;
-		goto fail;
-	} else {
-		int i;
-
-		for (i = 1; i < MAXBANDS; i++) {
-			wlc->hw->bandstate[i] = (struct brcms_hw_band *)
-			    ((unsigned long)wlc->hw->bandstate[0] +
-			     (sizeof(struct brcms_hw_band) * i));
-		}
-	}
-
-	wlc->modulecb =
-		kzalloc(sizeof(struct modulecb) * BRCMS_MAXMODULES, GFP_ATOMIC);
-	if (wlc->modulecb == NULL) {
-		*err = 1009;
-		goto fail;
-	}
-
-	wlc->default_bss = kzalloc(sizeof(struct brcms_bss_info), GFP_ATOMIC);
-	if (wlc->default_bss == NULL) {
-		*err = 1010;
-		goto fail;
-	}
-
-	wlc->cfg = brcms_c_bsscfg_malloc(unit);
-	if (wlc->cfg == NULL) {
-		*err = 1011;
-		goto fail;
-	}
-	brcms_c_bsscfg_ID_assign(wlc, wlc->cfg);
-
-	wlc->wsec_def_keys[0] =
-		kzalloc(sizeof(struct wsec_key) * BRCMS_DEFAULT_KEYS,
-			GFP_ATOMIC);
-	if (wlc->wsec_def_keys[0] == NULL) {
-		*err = 1015;
-		goto fail;
-	} else {
-		int i;
-		for (i = 1; i < BRCMS_DEFAULT_KEYS; i++) {
-			wlc->wsec_def_keys[i] = (struct wsec_key *)
-			    ((unsigned long)wlc->wsec_def_keys[0] +
-			     (sizeof(struct wsec_key) * i));
-		}
-	}
-
-	wlc->protection = kzalloc(sizeof(struct brcms_protection),
-				  GFP_ATOMIC);
-	if (wlc->protection == NULL) {
-		*err = 1016;
-		goto fail;
-	}
-
-	wlc->stf = kzalloc(sizeof(struct brcms_stf), GFP_ATOMIC);
-	if (wlc->stf == NULL) {
-		*err = 1017;
-		goto fail;
-	}
-
-	wlc->bandstate[0] =
-		kzalloc(sizeof(struct brcms_band)*MAXBANDS, GFP_ATOMIC);
-	if (wlc->bandstate[0] == NULL) {
-		*err = 1025;
-		goto fail;
-	} else {
-		int i;
-
-		for (i = 1; i < MAXBANDS; i++) {
-			wlc->bandstate[i] = (struct brcms_band *)
-				((unsigned long)wlc->bandstate[0]
-				+ (sizeof(struct brcms_band)*i));
-		}
-	}
-
-	wlc->corestate = kzalloc(sizeof(struct brcms_core), GFP_ATOMIC);
-	if (wlc->corestate == NULL) {
-		*err = 1026;
-		goto fail;
-	}
-
-	wlc->corestate->macstat_snapshot =
-		kzalloc(sizeof(struct macstat), GFP_ATOMIC);
-	if (wlc->corestate->macstat_snapshot == NULL) {
-		*err = 1027;
-		goto fail;
-	}
-
-	return wlc;
-
- fail:
-	brcms_c_detach_mfree(wlc);
-	return NULL;
-}
-
-void brcms_c_detach_mfree(struct brcms_c_info *wlc)
-{
-	if (wlc == NULL)
-		return;
-
-	brcms_c_bsscfg_mfree(wlc->cfg);
-	brcms_c_pub_mfree(wlc->pub);
-	kfree(wlc->modulecb);
-	kfree(wlc->default_bss);
-	kfree(wlc->wsec_def_keys[0]);
-	kfree(wlc->protection);
-	kfree(wlc->stf);
-	kfree(wlc->bandstate[0]);
-	kfree(wlc->corestate->macstat_snapshot);
-	kfree(wlc->corestate);
-	kfree(wlc->hw->bandstate[0]);
-	kfree(wlc->hw);
-
-	/* free the wlc */
-	kfree(wlc);
-	wlc = NULL;
-}
diff --git a/drivers/staging/brcm80211/brcmsmac/ampdu.c b/drivers/staging/brcm80211/brcmsmac/ampdu.c
index fcaf61e3b134..7f27dbdb6b60 100644
--- a/drivers/staging/brcm80211/brcmsmac/ampdu.c
+++ b/drivers/staging/brcm80211/brcmsmac/ampdu.c
@@ -22,19 +22,32 @@
 #include "main.h"
 #include "ampdu.h"
 
-#define AMPDU_MAX_MPDU		32	/* max number of mpdus in an ampdu */
-#define AMPDU_NUM_MPDU_LEGACY	16	/* max number of mpdus in an ampdu to a legacy */
-#define AMPDU_TX_BA_MAX_WSIZE	64	/* max Tx ba window size (in pdu) */
-#define AMPDU_TX_BA_DEF_WSIZE	64	/* default Tx ba window size (in pdu) */
-#define AMPDU_RX_BA_DEF_WSIZE   64	/* max Rx ba window size (in pdu) */
-#define AMPDU_RX_BA_MAX_WSIZE   64	/* default Rx ba window size (in pdu) */
-#define	AMPDU_MAX_DUR		5	/* max dur of tx ampdu (in msec) */
-#define AMPDU_DEF_RETRY_LIMIT	5	/* default tx retry limit */
-#define AMPDU_DEF_RR_RETRY_LIMIT	2	/* default tx retry limit at reg rate */
-#define AMPDU_DEF_TXPKT_WEIGHT	2	/* default weight of ampdu in txfifo */
-#define AMPDU_DEF_FFPLD_RSVD	2048	/* default ffpld reserved bytes */
-#define AMPDU_INI_FREE		10	/* # of inis to be freed on detach */
-#define	AMPDU_SCB_MAX_RELEASE	20	/* max # of mpdus released at a time */
+/* max number of mpdus in an ampdu */
+#define AMPDU_MAX_MPDU			32
+/* max number of mpdus in an ampdu to a legacy */
+#define AMPDU_NUM_MPDU_LEGACY		16
+/* max Tx ba window size (in pdu) */
+#define AMPDU_TX_BA_MAX_WSIZE		64
+/* default Tx ba window size (in pdu) */
+#define AMPDU_TX_BA_DEF_WSIZE		64
+/* default Rx ba window size (in pdu) */
+#define AMPDU_RX_BA_DEF_WSIZE		64
+/* max Rx ba window size (in pdu) */
+#define AMPDU_RX_BA_MAX_WSIZE		64
+/* max dur of tx ampdu (in msec) */
+#define	AMPDU_MAX_DUR			5
+/* default tx retry limit */
+#define AMPDU_DEF_RETRY_LIMIT		5
+/* default tx retry limit at reg rate */
+#define AMPDU_DEF_RR_RETRY_LIMIT	2
+/* default weight of ampdu in txfifo */
+#define AMPDU_DEF_TXPKT_WEIGHT		2
+/* default ffpld reserved bytes */
+#define AMPDU_DEF_FFPLD_RSVD		2048
+/* # of inis to be freed on detach */
+#define AMPDU_INI_FREE			10
+/* max # of mpdus released at a time */
+#define	AMPDU_SCB_MAX_RELEASE		20
 
 #define NUM_FFPLD_FIFO 4	/* number of fifo concerned by pre-loading */
 #define FFPLD_TX_MAX_UNFL   200	/* default value of the average number of ampdu
@@ -47,6 +60,11 @@
 					 * accumulate between resets.
 					 */
 
+#define AMPDU_DELIMITER_LEN	4
+
+/* max allowed number of mpdus in an ampdu (2 streams) */
+#define AMPDU_NUM_MPDU		16
+
 #define TX_SEQ_TO_INDEX(seq) ((seq) % AMPDU_TX_BA_MAX_WSIZE)
 
 /* max possible overhead per mpdu in the ampdu; 3 is for roundup if needed */
@@ -54,49 +72,77 @@
 	AMPDU_DELIMITER_LEN + 3\
 	+ DOT11_A4_HDR_LEN + DOT11_QOS_LEN + DOT11_IV_MAX_LEN)
 
+/* modulo add/sub, bound = 2^k */
+#define MODADD_POW2(x, y, bound) (((x) + (y)) & ((bound) - 1))
+#define MODSUB_POW2(x, y, bound) (((x) - (y)) & ((bound) - 1))
+
 /* structure to hold tx fifo information and pre-loading state
  * counters specific to tx underflows of ampdus
  * some counters might be redundant with the ones in wlc or ampdu structures.
  * This allows to maintain a specific state independently of
  * how often and/or when the wlc counters are updated.
+ *
+ * ampdu_pld_size: number of bytes to be pre-loaded
+ * mcs2ampdu_table: per-mcs max # of mpdus in an ampdu
+ * prev_txfunfl: num of underflows last read from the HW macstats counter
+ * accum_txfunfl: num of underflows since we modified pld params
+ * accum_txampdu: num of tx ampdu since we modified pld params
+ * prev_txampdu: previous reading of tx ampdu
+ * dmaxferrate: estimated dma avg xfer rate in kbits/sec
  */
 struct brcms_fifo_info {
-	u16 ampdu_pld_size;	/* number of bytes to be pre-loaded */
-	u8 mcs2ampdu_table[FFPLD_MAX_MCS + 1];	/* per-mcs max # of mpdus in an ampdu */
-	u16 prev_txfunfl;	/* num of underflows last read from the HW macstats counter */
-	u32 accum_txfunfl;	/* num of underflows since we modified pld params */
-	u32 accum_txampdu;	/* num of tx ampdu since we modified pld params  */
-	u32 prev_txampdu;	/* previous reading of tx ampdu */
-	u32 dmaxferrate;	/* estimated dma avg xfer rate in kbits/sec */
+	u16 ampdu_pld_size;
+	u8 mcs2ampdu_table[FFPLD_MAX_MCS + 1];
+	u16 prev_txfunfl;
+	u32 accum_txfunfl;
+	u32 accum_txampdu;
+	u32 prev_txampdu;
+	u32 dmaxferrate;
 };
 
-/* AMPDU module specific state */
+/* AMPDU module specific state
+ *
+ * wlc: pointer to main wlc structure
+ * scb_handle: scb cubby handle to retrieve data from scb
+ * ini_enable: per-tid initiator enable/disable of ampdu
+ * ba_tx_wsize: Tx ba window size (in pdu)
+ * ba_rx_wsize: Rx ba window size (in pdu)
+ * retry_limit: mpdu transmit retry limit
+ * rr_retry_limit: mpdu transmit retry limit at regular rate
+ * retry_limit_tid: per-tid mpdu transmit retry limit
+ * rr_retry_limit_tid: per-tid mpdu transmit retry limit at regular rate
+ * mpdu_density: min mpdu spacing (0-7) ==> 2^(x-1)/8 usec
+ * max_pdu: max pdus allowed in ampdu
+ * dur: max duration of an ampdu (in msec)
+ * txpkt_weight: weight of ampdu in txfifo; reduces rate lag
+ * rx_factor: maximum rx ampdu factor (0-3) ==> 2^(13+x) bytes
+ * ffpld_rsvd: number of bytes to reserve for preload
+ * max_txlen: max size of ampdu per mcs, bw and sgi
+ * mfbr: enable multiple fallback rate
+ * tx_max_funl: underflows should be kept such that
+ *		(tx_max_funfl*underflows) < tx frames
+ * fifo_tb: table of fifo infos
+ */
 struct ampdu_info {
-	struct brcms_c_info *wlc;	/* pointer to main wlc structure */
-	int scb_handle;		/* scb cubby handle to retrieve data from scb */
-	u8 ini_enable[AMPDU_MAX_SCB_TID];	/* per-tid initiator enable/disable of ampdu */
-	u8 ba_tx_wsize;	/* Tx ba window size (in pdu) */
-	u8 ba_rx_wsize;	/* Rx ba window size (in pdu) */
-	u8 retry_limit;	/* mpdu transmit retry limit */
-	u8 rr_retry_limit;	/* mpdu transmit retry limit at regular rate */
-	u8 retry_limit_tid[AMPDU_MAX_SCB_TID];	/* per-tid mpdu transmit retry limit */
-	/* per-tid mpdu transmit retry limit at regular rate */
+	struct brcms_c_info *wlc;
+	int scb_handle;
+	u8 ini_enable[AMPDU_MAX_SCB_TID];
+	u8 ba_tx_wsize;
+	u8 ba_rx_wsize;
+	u8 retry_limit;
+	u8 rr_retry_limit;
+	u8 retry_limit_tid[AMPDU_MAX_SCB_TID];
 	u8 rr_retry_limit_tid[AMPDU_MAX_SCB_TID];
-	u8 mpdu_density;	/* min mpdu spacing (0-7) ==> 2^(x-1)/8 usec */
-	s8 max_pdu;		/* max pdus allowed in ampdu */
-	u8 dur;		/* max duration of an ampdu (in msec) */
-	u8 txpkt_weight;	/* weight of ampdu in txfifo; reduces rate lag */
-	u8 rx_factor;	/* maximum rx ampdu factor (0-3) ==> 2^(13+x) bytes */
-	u32 ffpld_rsvd;	/* number of bytes to reserve for preload */
-	u32 max_txlen[MCS_TABLE_SIZE][2][2];	/* max size of ampdu per mcs, bw and sgi */
-	void *ini_free[AMPDU_INI_FREE];	/* array of ini's to be freed on detach */
-	bool mfbr;		/* enable multiple fallback rate */
-	u32 tx_max_funl;	/* underflows should be kept such that
-				 * (tx_max_funfl*underflows) < tx frames
-				 */
-	/* table of fifo infos */
+	u8 mpdu_density;
+	s8 max_pdu;
+	u8 dur;
+	u8 txpkt_weight;
+	u8 rx_factor;
+	u32 ffpld_rsvd;
+	u32 max_txlen[MCS_TABLE_SIZE][2][2];
+	bool mfbr;
+	u32 tx_max_funl;
 	struct brcms_fifo_info fifo_tb[NUM_FFPLD_FIFO];
-
 };
 
 /* used for flushing ampdu packets */
@@ -105,32 +151,75 @@ struct cb_del_ampdu_pars {
 	u16 tid;
 };
 
-#define AMPDU_CLEANUPFLAG_RX   (0x1)
-#define AMPDU_CLEANUPFLAG_TX   (0x2)
+static void brcms_c_scb_ampdu_update_max_txlen(struct ampdu_info *ampdu, u8 dur)
+{
+	u32 rate, mcs;
 
-#define SCB_AMPDU_CUBBY(ampdu, scb) (&(scb->scb_ampdu))
-#define SCB_AMPDU_INI(scb_ampdu, tid) (&(scb_ampdu->ini[tid]))
+	for (mcs = 0; mcs < MCS_TABLE_SIZE; mcs++) {
+		/* rate is in Kbps; dur is in msec ==> len = (rate * dur) / 8 */
+		/* 20MHz, No SGI */
+		rate = mcs_2_rate(mcs, false, false);
+		ampdu->max_txlen[mcs][0][0] = (rate * dur) >> 3;
+		/* 40 MHz, No SGI */
+		rate = mcs_2_rate(mcs, true, false);
+		ampdu->max_txlen[mcs][1][0] = (rate * dur) >> 3;
+		/* 20MHz, SGI */
+		rate = mcs_2_rate(mcs, false, true);
+		ampdu->max_txlen[mcs][0][1] = (rate * dur) >> 3;
+		/* 40 MHz, SGI */
+		rate = mcs_2_rate(mcs, true, true);
+		ampdu->max_txlen[mcs][1][1] = (rate * dur) >> 3;
+	}
+}
 
-static void brcms_c_ffpld_init(struct ampdu_info *ampdu);
-static int brcms_c_ffpld_check_txfunfl(struct brcms_c_info *wlc, int f);
-static void brcms_c_ffpld_calc_mcs2ampdu_table(struct ampdu_info *ampdu, int f);
+static bool brcms_c_ampdu_cap(struct ampdu_info *ampdu)
+{
+	if (BRCMS_PHY_11N_CAP(ampdu->wlc->band))
+		return true;
+	else
+		return false;
+}
 
-static void brcms_c_scb_ampdu_update_max_txlen(struct ampdu_info *ampdu,
-					       u8 dur);
-static void brcms_c_scb_ampdu_update_config(struct ampdu_info *ampdu,
-					    struct scb *scb);
-static void brcms_c_scb_ampdu_update_config_all(struct ampdu_info *ampdu);
+static int brcms_c_ampdu_set(struct ampdu_info *ampdu, bool on)
+{
+	struct brcms_c_info *wlc = ampdu->wlc;
 
-#define brcms_c_ampdu_txflowcontrol(a, b, c)	do {} while (0)
+	wlc->pub->_ampdu = false;
 
-static void
-brcms_c_ampdu_dotxstatus_complete(struct ampdu_info *ampdu,
-				  struct scb *scb,
-				  struct sk_buff *p, struct tx_status *txs,
-				  u32 frmtxstatus, u32 frmtxstatus2);
+	if (on) {
+		if (!(wlc->pub->_n_enab & SUPPORT_11N)) {
+			wiphy_err(ampdu->wlc->wiphy, "wl%d: driver not "
+				"nmode enabled\n", wlc->pub->unit);
+			return -ENOTSUPP;
+		}
+		if (!brcms_c_ampdu_cap(ampdu)) {
+			wiphy_err(ampdu->wlc->wiphy, "wl%d: device not "
+				"ampdu capable\n", wlc->pub->unit);
+			return -ENOTSUPP;
+		}
+		wlc->pub->_ampdu = on;
+	}
+
+	return 0;
+}
+
+static void brcms_c_ffpld_init(struct ampdu_info *ampdu)
+{
+	int i, j;
+	struct brcms_fifo_info *fifo;
+
+	for (j = 0; j < NUM_FFPLD_FIFO; j++) {
+		fifo = (ampdu->fifo_tb + j);
+		fifo->ampdu_pld_size = 0;
+		for (i = 0; i <= FFPLD_MAX_MCS; i++)
+			fifo->mcs2ampdu_table[i] = 255;
+		fifo->dmaxferrate = 0;
+		fifo->accum_txampdu = 0;
+		fifo->prev_txfunfl = 0;
+		fifo->accum_txfunfl = 0;
 
-static bool brcms_c_ampdu_cap(struct ampdu_info *ampdu);
-static int brcms_c_ampdu_set(struct ampdu_info *ampdu, bool on);
+	}
+}
 
 struct ampdu_info *brcms_c_ampdu_attach(struct brcms_c_info *wlc)
 {
@@ -138,11 +227,9 @@ struct ampdu_info *brcms_c_ampdu_attach(struct brcms_c_info *wlc)
 	int i;
 
 	ampdu = kzalloc(sizeof(struct ampdu_info), GFP_ATOMIC);
-	if (!ampdu) {
-		wiphy_err(wlc->wiphy, "wl%d: brcms_c_ampdu_attach: out of mem"
-			  "\n", wlc->pub->unit);
+	if (!ampdu)
 		return NULL;
-	}
+
 	ampdu->wlc = wlc;
 
 	for (i = 0; i < AMPDU_MAX_SCB_TID; i++)
@@ -163,7 +250,10 @@ struct ampdu_info *brcms_c_ampdu_attach(struct brcms_c_info *wlc)
 	ampdu->txpkt_weight = AMPDU_DEF_TXPKT_WEIGHT;
 
 	ampdu->ffpld_rsvd = AMPDU_DEF_FFPLD_RSVD;
-	/* bump max ampdu rcv size to 64k for all 11n devices except 4321A0 and 4321A1 */
+	/*
+	 * bump max ampdu rcv size to 64k for all 11n
+	 * devices except 4321A0 and 4321A1
+	 */
 	if (BRCMS_ISNPHY(wlc->band) && NREV_LT(wlc->band->phyrev, 2))
 		ampdu->rx_factor = IEEE80211_HT_MAX_AMPDU_32K;
 	else
@@ -189,27 +279,16 @@ struct ampdu_info *brcms_c_ampdu_attach(struct brcms_c_info *wlc)
 
 void brcms_c_ampdu_detach(struct ampdu_info *ampdu)
 {
-	int i;
-
-	if (!ampdu)
-		return;
-
-	/* free all ini's which were to be freed on callbacks which were never called */
-	for (i = 0; i < AMPDU_INI_FREE; i++) {
-		kfree(ampdu->ini_free[i]);
-	}
-
-	brcms_c_module_unregister(ampdu->wlc->pub, "ampdu", ampdu);
 	kfree(ampdu);
 }
 
 static void brcms_c_scb_ampdu_update_config(struct ampdu_info *ampdu,
 					    struct scb *scb)
 {
-	struct scb_ampdu *scb_ampdu = SCB_AMPDU_CUBBY(ampdu, scb);
+	struct scb_ampdu *scb_ampdu = &scb->scb_ampdu;
 	int i;
 
-	scb_ampdu->max_pdu = (u8) ampdu->wlc->pub->tunables->ampdunummpdu;
+	scb_ampdu->max_pdu = AMPDU_NUM_MPDU;
 
 	/* go back to legacy size if some preloading is occurring */
 	for (i = 0; i < NUM_FFPLD_FIFO; i++) {
@@ -221,7 +300,8 @@ static void brcms_c_scb_ampdu_update_config(struct ampdu_info *ampdu,
 	if (ampdu->max_pdu != AUTO)
 		scb_ampdu->max_pdu = (u8) ampdu->max_pdu;
 
-	scb_ampdu->release = min_t(u8, scb_ampdu->max_pdu, AMPDU_SCB_MAX_RELEASE);
+	scb_ampdu->release = min_t(u8, scb_ampdu->max_pdu,
+				   AMPDU_SCB_MAX_RELEASE);
 
 	if (scb_ampdu->max_rx_ampdu_bytes)
 		scb_ampdu->release = min_t(u8, scb_ampdu->release,
@@ -234,24 +314,38 @@ static void brcms_c_scb_ampdu_update_config(struct ampdu_info *ampdu,
 
 static void brcms_c_scb_ampdu_update_config_all(struct ampdu_info *ampdu)
 {
-	brcms_c_scb_ampdu_update_config(ampdu, ampdu->wlc->pub->global_scb);
+	brcms_c_scb_ampdu_update_config(ampdu, &ampdu->wlc->pri_scb);
 }
 
-static void brcms_c_ffpld_init(struct ampdu_info *ampdu)
+static void brcms_c_ffpld_calc_mcs2ampdu_table(struct ampdu_info *ampdu, int f)
 {
-	int i, j;
-	struct brcms_fifo_info *fifo;
+	int i;
+	u32 phy_rate, dma_rate, tmp;
+	u8 max_mpdu;
+	struct brcms_fifo_info *fifo = (ampdu->fifo_tb + f);
 
-	for (j = 0; j < NUM_FFPLD_FIFO; j++) {
-		fifo = (ampdu->fifo_tb + j);
-		fifo->ampdu_pld_size = 0;
-		for (i = 0; i <= FFPLD_MAX_MCS; i++)
-			fifo->mcs2ampdu_table[i] = 255;
-		fifo->dmaxferrate = 0;
-		fifo->accum_txampdu = 0;
-		fifo->prev_txfunfl = 0;
-		fifo->accum_txfunfl = 0;
+	/* recompute the dma rate */
+	/* note : we divide/multiply by 100 to avoid integer overflows */
+	max_mpdu = min_t(u8, fifo->mcs2ampdu_table[FFPLD_MAX_MCS],
+			 AMPDU_NUM_MPDU_LEGACY);
+	phy_rate = mcs_2_rate(FFPLD_MAX_MCS, true, false);
+	dma_rate =
+	    (((phy_rate / 100) *
+	      (max_mpdu * FFPLD_MPDU_SIZE - fifo->ampdu_pld_size))
+	     / (max_mpdu * FFPLD_MPDU_SIZE)) * 100;
+	fifo->dmaxferrate = dma_rate;
 
+	/* fill up the mcs2ampdu table; do not recalc the last mcs */
+	dma_rate = dma_rate >> 7;
+	for (i = 0; i < FFPLD_MAX_MCS; i++) {
+		/* shifting to keep it within integer range */
+		phy_rate = mcs_2_rate(i, true, false) >> 7;
+		if (phy_rate > dma_rate) {
+			tmp = ((fifo->ampdu_pld_size * phy_rate) /
+			       ((phy_rate - dma_rate) * FFPLD_MPDU_SIZE)) + 1;
+			tmp = min_t(u32, tmp, 255);
+			fifo->mcs2ampdu_table[i] = (u8) tmp;
+		}
 	}
 }
 
@@ -264,7 +358,7 @@ static void brcms_c_ffpld_init(struct ampdu_info *ampdu)
 static int brcms_c_ffpld_check_txfunfl(struct brcms_c_info *wlc, int fid)
 {
 	struct ampdu_info *ampdu = wlc->ampdu;
-	u32 phy_rate = MCS_RATE(FFPLD_MAX_MCS, true, false);
+	u32 phy_rate = mcs_2_rate(FFPLD_MAX_MCS, true, false);
 	u32 txunfl_ratio;
 	u8 max_mpdu;
 	u32 current_ampdu_cnt = 0;
@@ -275,7 +369,7 @@ static int brcms_c_ffpld_check_txfunfl(struct brcms_c_info *wlc, int fid)
 	u16 cur_txunfl;
 
 	/* return if we got here for a different reason than underflows */
-	cur_txunfl = brcms_c_read_shm(wlc,
+	cur_txunfl = brcms_b_read_shm(wlc->hw,
 				      M_UCODE_MACSTAT +
 				      offsetof(struct macstat, txfunfl[fid]));
 	new_txunfl = (u16) (cur_txunfl - fifo->prev_txfunfl);
@@ -289,7 +383,7 @@ static int brcms_c_ffpld_check_txfunfl(struct brcms_c_info *wlc, int fid)
 		return 1;
 
 	/* check if fifo is big enough */
-	if (brcms_c_xmtfifo_sz_get(wlc, fid, &xmtfifo_sz))
+	if (brcms_b_xmtfifo_sz_get(wlc->hw, fid, &xmtfifo_sz))
 		return -1;
 
 	if ((TXFIFO_SIZE_UNIT * (u32) xmtfifo_sz) <= ampdu->ffpld_rsvd)
@@ -317,13 +411,13 @@ static int brcms_c_ffpld_check_txfunfl(struct brcms_c_info *wlc, int fid)
 	txunfl_ratio = current_ampdu_cnt / fifo->accum_txfunfl;
 
 	if (txunfl_ratio > ampdu->tx_max_funl) {
-		if (current_ampdu_cnt >= FFPLD_MAX_AMPDU_CNT) {
+		if (current_ampdu_cnt >= FFPLD_MAX_AMPDU_CNT)
 			fifo->accum_txfunfl = 0;
-		}
+
 		return 0;
 	}
-	max_mpdu =
-	    min_t(u8, fifo->mcs2ampdu_table[FFPLD_MAX_MCS], AMPDU_NUM_MPDU_LEGACY);
+	max_mpdu = min_t(u8, fifo->mcs2ampdu_table[FFPLD_MAX_MCS],
+			 AMPDU_NUM_MPDU_LEGACY);
 
 	/* In case max value max_pdu is already lower than
 	   the fifo depth, there is nothing more we can do.
@@ -345,11 +439,12 @@ static int brcms_c_ffpld_check_txfunfl(struct brcms_c_info *wlc, int fid)
 		brcms_c_scb_ampdu_update_config_all(ampdu);
 
 		/*
-		   compute a new dma xfer rate for max_mpdu @ max mcs.
-		   This is the minimum dma rate that
-		   can achieve no underflow condition for the current mpdu size.
+		 * compute a new dma xfer rate for max_mpdu @ max mcs.
+		 * This is the minimum dma rate that can achieve no
+		 * underflow condition for the current mpdu size.
+		 *
+		 * note : we divide/multiply by 100 to avoid integer overflows
 		 */
-		/* note : we divide/multiply by 100 to avoid integer overflows */
 		fifo->dmaxferrate =
 		    (((phy_rate / 100) *
 		      (max_mpdu * FFPLD_MPDU_SIZE - fifo->ampdu_pld_size))
@@ -379,38 +474,6 @@ static int brcms_c_ffpld_check_txfunfl(struct brcms_c_info *wlc, int fid)
 	return 0;
 }
 
-static void brcms_c_ffpld_calc_mcs2ampdu_table(struct ampdu_info *ampdu, int f)
-{
-	int i;
-	u32 phy_rate, dma_rate, tmp;
-	u8 max_mpdu;
-	struct brcms_fifo_info *fifo = (ampdu->fifo_tb + f);
-
-	/* recompute the dma rate */
-	/* note : we divide/multiply by 100 to avoid integer overflows */
-	max_mpdu =
-	    min_t(u8, fifo->mcs2ampdu_table[FFPLD_MAX_MCS], AMPDU_NUM_MPDU_LEGACY);
-	phy_rate = MCS_RATE(FFPLD_MAX_MCS, true, false);
-	dma_rate =
-	    (((phy_rate / 100) *
-	      (max_mpdu * FFPLD_MPDU_SIZE - fifo->ampdu_pld_size))
-	     / (max_mpdu * FFPLD_MPDU_SIZE)) * 100;
-	fifo->dmaxferrate = dma_rate;
-
-	/* fill up the mcs2ampdu table; do not recalc the last mcs */
-	dma_rate = dma_rate >> 7;
-	for (i = 0; i < FFPLD_MAX_MCS; i++) {
-		/* shifting to keep it within integer range */
-		phy_rate = MCS_RATE(i, true, false) >> 7;
-		if (phy_rate > dma_rate) {
-			tmp = ((fifo->ampdu_pld_size * phy_rate) /
-			       ((phy_rate - dma_rate) * FFPLD_MPDU_SIZE)) + 1;
-			tmp = min_t(u32, tmp, 255);
-			fifo->mcs2ampdu_table[i] = (u8) tmp;
-		}
-	}
-}
-
 void
 brcms_c_ampdu_tx_operational(struct brcms_c_info *wlc, u8 tid,
 	u8 ba_wsize,		/* negotiated ba window size (in pdu) */
@@ -419,8 +482,8 @@ brcms_c_ampdu_tx_operational(struct brcms_c_info *wlc, u8 tid,
 	struct scb_ampdu *scb_ampdu;
 	struct scb_ampdu_tid_ini *ini;
 	struct ampdu_info *ampdu = wlc->ampdu;
-	struct scb *scb = wlc->pub->global_scb;
-	scb_ampdu = SCB_AMPDU_CUBBY(ampdu, scb);
+	struct scb *scb = &wlc->pri_scb;
+	scb_ampdu = &scb->scb_ampdu;
 
 	if (!ampdu->ini_enable[tid]) {
 		wiphy_err(ampdu->wlc->wiphy, "%s: Rejecting tid %d\n",
@@ -428,7 +491,7 @@ brcms_c_ampdu_tx_operational(struct brcms_c_info *wlc, u8 tid,
 		return;
 	}
 
-	ini = SCB_AMPDU_INI(scb_ampdu, tid);
+	ini = &scb_ampdu->ini[tid];
 	ini->tid = tid;
 	ini->scb = scb_ampdu->scb;
 	ini->ba_wsize = ba_wsize;
@@ -460,8 +523,8 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
 	struct scb_ampdu_tid_ini *ini;
 	u8 mcs = 0;
 	bool use_rts = false, use_cts = false;
-	ratespec_t rspec = 0, rspec_fallback = 0;
-	ratespec_t rts_rspec = 0, rts_rspec_fallback = 0;
+	u32 rspec = 0, rspec_fallback = 0;
+	u32 rts_rspec = 0, rts_rspec_fallback = 0;
 	u16 mimo_ctlchbw = PHY_TXC1_BW_20MHZ;
 	struct ieee80211_rts *rts;
 	u8 rr_retry_limit;
@@ -479,17 +542,16 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
 
 	f = ampdu->fifo_tb + prio2fifo[tid];
 
-	scb = wlc->pub->global_scb;
-	scb_ampdu = SCB_AMPDU_CUBBY(ampdu, scb);
+	scb = &wlc->pri_scb;
+	scb_ampdu = &scb->scb_ampdu;
 	ini = &scb_ampdu->ini[tid];
 
 	/* Let pressure continue to build ... */
 	qlen = pktq_plen(&qi->q, prec);
 	if (ini->tx_in_transit > 0 &&
-	    qlen < min(scb_ampdu->max_pdu, ini->ba_wsize)) {
+	    qlen < min(scb_ampdu->max_pdu, ini->ba_wsize))
 		/* Collect multiple MPDU's to be sent in the next AMPDU */
 		return -EBUSY;
-	}
 
 	/* at this point we intend to transmit an AMPDU */
 	rr_retry_limit = ampdu->rr_retry_limit_tid[tid];
@@ -614,7 +676,7 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
 
 			}
 			is40 = (plcp0 & MIMO_PLCP_40MHZ) ? 1 : 0;
-			sgi = PLCP3_ISSGI(plcp3) ? 1 : 0;
+			sgi = plcp3_issgi(plcp3) ? 1 : 0;
 			mcs = plcp0 & ~MIMO_PLCP_40MHZ;
 			max_ampdu_bytes =
 			    min(scb_ampdu->max_rx_ampdu_bytes,
@@ -622,7 +684,8 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
 
 			if (is40)
 				mimo_ctlchbw =
-				   CHSPEC_SB_UPPER(BRCMS_BAND_PI_RADIO_CHANSPEC)
+				   CHSPEC_SB_UPPER(wlc_phy_chanspec_get(
+								 wlc->band->pi))
 				   ? PHY_TXC1_BW_20MHZ_UP : PHY_TXC1_BW_20MHZ;
 
 			/* rebuild the rspec and rspec_fallback */
@@ -632,9 +695,8 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
 				rspec |= (PHY_TXC1_BW_40MHZ << RSPEC_BW_SHIFT);
 
 			if (fbr_iscck)	/* CCK */
-				rspec_fallback =
-				    CCK_RSPEC(CCK_PHY2MAC_RATE
-					      (txh->FragPLCPFallback[0]));
+				rspec_fallback = cck_rspec(cck_phy2mac_rate
+						    (txh->FragPLCPFallback[0]));
 			else {	/* MIMO */
 				rspec_fallback = RSPEC_MIMORATE;
 				rspec_fallback |=
@@ -657,7 +719,7 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
 
 		/* if (first mpdu for host agg) */
 		/* test whether to add more */
-		if ((MCS_RATE(mcs, true, false) >= f->dmaxferrate) &&
+		if ((mcs_2_rate(mcs, true, false) >= f->dmaxferrate) &&
 		    (count == f->mcs2ampdu_table[mcs])) {
 			BCMMSG(wlc->wiphy, "wl%d: PR 37644: stopping"
 				" ampdu at %d for mcs %d\n",
@@ -665,13 +727,16 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
 			break;
 		}
 
-		if (count == scb_ampdu->max_pdu) {
+		if (count == scb_ampdu->max_pdu)
 			break;
-		}
 
-		/* check to see if the next pkt is a candidate for aggregation */
+		/*
+		 * check to see if the next pkt is
+		 * a candidate for aggregation
+		 */
 		p = pktq_ppeek(&qi->q, prec);
-		tx_info = IEEE80211_SKB_CB(p);	/* tx_info must be checked with current p */
+		/* tx_info must be checked with current p */
+		tx_info = IEEE80211_SKB_CB(p);
 
 		if (p) {
 			if ((tx_info->flags & IEEE80211_TX_CTL_AMPDU) &&
@@ -686,8 +751,11 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
 					continue;
 				}
 
-				/* check if there are enough descriptors available */
-				if (TXAVAIL(wlc, fifo) <= (seg_cnt + 1)) {
+				/*
+				 * check if there are enough
+				 * descriptors available
+				 */
+				if (*wlc->core->txavail[fifo] <= seg_cnt + 1) {
 					wiphy_err(wiphy, "%s: No fifo space  "
 						  "!!\n", __func__);
 					p = NULL;
@@ -745,7 +813,7 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
 		}
 
 		/* set the preload length */
-		if (MCS_RATE(mcs, true, false) >= f->dmaxferrate) {
+		if (mcs_2_rate(mcs, true, false) >= f->dmaxferrate) {
 			dma_len = min(dma_len, f->ampdu_pld_size);
 			txh->PreloadSize = cpu_to_le16(dma_len);
 		} else
@@ -797,10 +865,10 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
 
 		/* inform rate_sel if it this is a rate probe pkt */
 		frameid = le16_to_cpu(txh->TxFrameID);
-		if (frameid & TXFID_RATE_PROBE_MASK) {
+		if (frameid & TXFID_RATE_PROBE_MASK)
 			wiphy_err(wiphy, "%s: XXX what to do with "
 				  "TXFID_RATE_PROBE_MASK!?\n", __func__);
-		}
+
 		for (i = 0; i < count; i++)
 			brcms_c_txfifo(wlc, fifo, pkt[i], i == (count - 1),
 				   ampdu->txpkt_weight);
@@ -810,62 +878,6 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
 	return err;
 }
 
-void
-brcms_c_ampdu_dotxstatus(struct ampdu_info *ampdu, struct scb *scb,
-		     struct sk_buff *p, struct tx_status *txs)
-{
-	struct scb_ampdu *scb_ampdu;
-	struct brcms_c_info *wlc = ampdu->wlc;
-	struct scb_ampdu_tid_ini *ini;
-	u32 s1 = 0, s2 = 0;
-	struct ieee80211_tx_info *tx_info;
-
-	tx_info = IEEE80211_SKB_CB(p);
-
-	/* BMAC_NOTE: For the split driver, second level txstatus comes later
-	 * So if the ACK was received then wait for the second level else just
-	 * call the first one
-	 */
-	if (txs->status & TX_STATUS_ACK_RCV) {
-		u8 status_delay = 0;
-
-		/* wait till the next 8 bytes of txstatus is available */
-		while (((s1 = R_REG(&wlc->regs->frmtxstatus)) & TXS_V) == 0) {
-			udelay(1);
-			status_delay++;
-			if (status_delay > 10) {
-				return; /* error condition */
-			}
-		}
-
-		s2 = R_REG(&wlc->regs->frmtxstatus2);
-	}
-
-	if (likely(scb)) {
-		scb_ampdu = SCB_AMPDU_CUBBY(ampdu, scb);
-		ini = SCB_AMPDU_INI(scb_ampdu, p->priority);
-		brcms_c_ampdu_dotxstatus_complete(ampdu, scb, p, txs, s1, s2);
-	} else {
-		/* loop through all pkts and free */
-		u8 queue = txs->frameid & TXFID_QUEUE_MASK;
-		struct d11txh *txh;
-		u16 mcl;
-		while (p) {
-			tx_info = IEEE80211_SKB_CB(p);
-			txh = (struct d11txh *) p->data;
-			mcl = le16_to_cpu(txh->MacTxControlLow);
-			brcmu_pkt_buf_free_skb(p);
-			/* break out if last packet of ampdu */
-			if (((mcl & TXC_AMPDU_MASK) >> TXC_AMPDU_SHIFT) ==
-			    TXC_AMPDU_LAST)
-				break;
-			p = GETNEXTTXP(wlc, queue);
-		}
-		brcms_c_txfifo_complete(wlc, queue, ampdu->txpkt_weight);
-	}
-	brcms_c_ampdu_txflowcontrol(wlc, scb_ampdu, ini);
-}
-
 static void
 brcms_c_ampdu_rate_status(struct brcms_c_info *wlc,
 			  struct ieee80211_tx_info *tx_info,
@@ -881,8 +893,6 @@ brcms_c_ampdu_rate_status(struct brcms_c_info *wlc,
 	}
 }
 
-#define SHORTNAME "AMPDU status"
-
 static void
 brcms_c_ampdu_dotxstatus_complete(struct ampdu_info *ampdu, struct scb *scb,
 			      struct sk_buff *p, struct tx_status *txs,
@@ -912,10 +922,10 @@ brcms_c_ampdu_dotxstatus_complete(struct ampdu_info *ampdu, struct scb *scb,
 	memset(hole, 0, sizeof(hole));
 #endif
 
-	scb_ampdu = SCB_AMPDU_CUBBY(ampdu, scb);
+	scb_ampdu = &scb->scb_ampdu;
 	tid = (u8) (p->priority);
 
-	ini = SCB_AMPDU_INI(scb_ampdu, tid);
+	ini = &scb_ampdu->ini[tid];
 	retry_limit = ampdu->retry_limit_tid[tid];
 	rr_retry_limit = ampdu->rr_retry_limit_tid[tid];
 	memset(bitmap, 0, sizeof(bitmap));
@@ -923,9 +933,8 @@ brcms_c_ampdu_dotxstatus_complete(struct ampdu_info *ampdu, struct scb *scb,
 	supr_status = txs->status & TX_STATUS_SUPR_MASK;
 
 	if (txs->status & TX_STATUS_ACK_RCV) {
-		if (TX_STATUS_SUPR_UF == supr_status) {
+		if (TX_STATUS_SUPR_UF == supr_status)
 			update_rate = false;
-		}
 
 		WARN_ON(!(txs->status & TX_STATUS_INTERMEDIATE));
 		start_seq = txs->sequence >> SEQNUM_SHIFT;
@@ -967,15 +976,17 @@ brcms_c_ampdu_dotxstatus_complete(struct ampdu_info *ampdu, struct scb *scb,
 			    supr_status == TX_STATUS_SUPR_EXPTIME) {
 				retry = false;
 			} else if (supr_status == TX_STATUS_SUPR_EXPTIME) {
-				/* TX underflow : try tuning pre-loading or ampdu size */
+				/* TX underflow:
+				 *   try tuning pre-loading or ampdu size
+				 */
 			} else if (supr_status == TX_STATUS_SUPR_FRAG) {
-				/* if there were underflows, but pre-loading is not active,
-				   notify rate adaptation.
+				/*
+				 * if there were underflows, but pre-loading
+				 * is not active, notify rate adaptation.
 				 */
 				if (brcms_c_ffpld_check_txfunfl(wlc,
-							prio2fifo[tid]) > 0) {
+					prio2fifo[tid]) > 0)
 					tx_error = true;
-				}
 			}
 		} else if (txs->phyerr) {
 			update_rate = false;
@@ -983,7 +994,7 @@ brcms_c_ampdu_dotxstatus_complete(struct ampdu_info *ampdu, struct scb *scb,
 				  "error (0x%x)\n", wlc->pub->unit,
 				  txs->phyerr);
 
-			if (WL_ERROR_ON()) {
+			if (brcm_msg_level & LOG_ERROR_VAL) {
 				brcmu_prpkt("txpkt (AMPDU)", p);
 				brcms_c_print_txdesc((struct d11txh *) p->data);
 			}
@@ -1019,7 +1030,10 @@ brcms_c_ampdu_dotxstatus_complete(struct ampdu_info *ampdu, struct scb *scb,
 				ini->tx_in_transit--;
 				ini->txretry[index] = 0;
 
-				/* ampdu_ack_len: number of acked aggregated frames */
+				/*
+				 * ampdu_ack_len:
+				 *   number of acked aggregated frames
+				 */
 				/* ampdu_len: number of aggregated frames */
 				brcms_c_ampdu_rate_status(wlc, tx_info, txs,
 							  mcs);
@@ -1044,11 +1058,14 @@ brcms_c_ampdu_dotxstatus_complete(struct ampdu_info *ampdu, struct scb *scb,
 			if (retry && (txrate[0].count < (int)retry_limit)) {
 				ini->txretry[index]++;
 				ini->tx_in_transit--;
-				/* Use high prededence for retransmit to give some punch */
+				/*
+				 * Use high prededence for retransmit to
+				 * give some punch
+				 */
 				/* brcms_c_txq_enq(wlc, scb, p,
 				 * BRCMS_PRIO_TO_PREC(tid)); */
 				brcms_c_txq_enq(wlc, scb, p,
-					    BRCMS_PRIO_TO_HI_PREC(tid));
+						BRCMS_PRIO_TO_HI_PREC(tid));
 			} else {
 				/* Retry timeout */
 				ini->tx_in_transit--;
@@ -1060,7 +1077,7 @@ brcms_c_ampdu_dotxstatus_complete(struct ampdu_info *ampdu, struct scb *scb,
 				skb_pull(p, D11_PHY_HDR_LEN);
 				skb_pull(p, D11_TXH_LEN);
 				wiphy_err(wiphy, "%s: BA Timeout, seq %d, in_"
-					"transit %d\n", SHORTNAME, seq,
+					"transit %d\n", "AMPDU status", seq,
 					ini->tx_in_transit);
 				ieee80211_tx_status_irqsafe(wlc->pub->ieee_hw,
 							    p);
@@ -1073,7 +1090,7 @@ brcms_c_ampdu_dotxstatus_complete(struct ampdu_info *ampdu, struct scb *scb,
 		    TXC_AMPDU_LAST)
 			break;
 
-		p = GETNEXTTXP(wlc, queue);
+		p = dma_getnexttxp(wlc->hw->di[queue], DMA_RANGE_TRANSMITTED);
 	}
 	brcms_c_send_q(wlc);
 
@@ -1083,55 +1100,58 @@ brcms_c_ampdu_dotxstatus_complete(struct ampdu_info *ampdu, struct scb *scb,
 	brcms_c_txfifo_complete(wlc, queue, ampdu->txpkt_weight);
 }
 
-static int brcms_c_ampdu_set(struct ampdu_info *ampdu, bool on)
+void
+brcms_c_ampdu_dotxstatus(struct ampdu_info *ampdu, struct scb *scb,
+		     struct sk_buff *p, struct tx_status *txs)
 {
+	struct scb_ampdu *scb_ampdu;
 	struct brcms_c_info *wlc = ampdu->wlc;
+	struct scb_ampdu_tid_ini *ini;
+	u32 s1 = 0, s2 = 0;
+	struct ieee80211_tx_info *tx_info;
 
-	wlc->pub->_ampdu = false;
-
-	if (on) {
-		if (!N_ENAB(wlc->pub)) {
-			wiphy_err(ampdu->wlc->wiphy, "wl%d: driver not "
-				"nmode enabled\n", wlc->pub->unit);
-			return -ENOTSUPP;
-		}
-		if (!brcms_c_ampdu_cap(ampdu)) {
-			wiphy_err(ampdu->wlc->wiphy, "wl%d: device not "
-				"ampdu capable\n", wlc->pub->unit);
-			return -ENOTSUPP;
-		}
-		wlc->pub->_ampdu = on;
-	}
+	tx_info = IEEE80211_SKB_CB(p);
 
-	return 0;
-}
+	/* BMAC_NOTE: For the split driver, second level txstatus comes later
+	 * So if the ACK was received then wait for the second level else just
+	 * call the first one
+	 */
+	if (txs->status & TX_STATUS_ACK_RCV) {
+		u8 status_delay = 0;
 
-static bool brcms_c_ampdu_cap(struct ampdu_info *ampdu)
-{
-	if (BRCMS_PHY_11N_CAP(ampdu->wlc->band))
-		return true;
-	else
-		return false;
-}
+		/* wait till the next 8 bytes of txstatus is available */
+		while (((s1 = R_REG(&wlc->regs->frmtxstatus)) & TXS_V) == 0) {
+			udelay(1);
+			status_delay++;
+			if (status_delay > 10)
+				return; /* error condition */
+		}
 
-static void brcms_c_scb_ampdu_update_max_txlen(struct ampdu_info *ampdu, u8 dur)
-{
-	u32 rate, mcs;
+		s2 = R_REG(&wlc->regs->frmtxstatus2);
+	}
 
-	for (mcs = 0; mcs < MCS_TABLE_SIZE; mcs++) {
-		/* rate is in Kbps; dur is in msec ==> len = (rate * dur) / 8 */
-		/* 20MHz, No SGI */
-		rate = MCS_RATE(mcs, false, false);
-		ampdu->max_txlen[mcs][0][0] = (rate * dur) >> 3;
-		/* 40 MHz, No SGI */
-		rate = MCS_RATE(mcs, true, false);
-		ampdu->max_txlen[mcs][1][0] = (rate * dur) >> 3;
-		/* 20MHz, SGI */
-		rate = MCS_RATE(mcs, false, true);
-		ampdu->max_txlen[mcs][0][1] = (rate * dur) >> 3;
-		/* 40 MHz, SGI */
-		rate = MCS_RATE(mcs, true, true);
-		ampdu->max_txlen[mcs][1][1] = (rate * dur) >> 3;
+	if (scb) {
+		scb_ampdu = &scb->scb_ampdu;
+		ini = &scb_ampdu->ini[p->priority];
+		brcms_c_ampdu_dotxstatus_complete(ampdu, scb, p, txs, s1, s2);
+	} else {
+		/* loop through all pkts and free */
+		u8 queue = txs->frameid & TXFID_QUEUE_MASK;
+		struct d11txh *txh;
+		u16 mcl;
+		while (p) {
+			tx_info = IEEE80211_SKB_CB(p);
+			txh = (struct d11txh *) p->data;
+			mcl = le16_to_cpu(txh->MacTxControlLow);
+			brcmu_pkt_buf_free_skb(p);
+			/* break out if last packet of ampdu */
+			if (((mcl & TXC_AMPDU_MASK) >> TXC_AMPDU_SHIFT) ==
+			    TXC_AMPDU_LAST)
+				break;
+			p = dma_getnexttxp(wlc->hw->di[queue],
+					   DMA_RANGE_TRANSMITTED);
+		}
+		brcms_c_txfifo_complete(wlc, queue, ampdu->txpkt_weight);
 	}
 }
 
@@ -1142,7 +1162,7 @@ void brcms_c_ampdu_macaddr_upd(struct brcms_c_info *wlc)
 	/* driver needs to write the ta in the template; ta is at offset 16 */
 	memset(template, 0, sizeof(template));
 	memcpy(template, wlc->pub->cur_etheraddr, ETH_ALEN);
-	brcms_c_write_template_ram(wlc, (T_BA_TPL_BASE + 16),
+	brcms_b_write_template_ram(wlc->hw, (T_BA_TPL_BASE + 16),
 				  (T_RAM_ACCESS_SZ * 2),
 				  template);
 }
@@ -1156,14 +1176,17 @@ void brcms_c_ampdu_shm_upd(struct ampdu_info *ampdu)
 {
 	struct brcms_c_info *wlc = ampdu->wlc;
 
-	/* Extend ucode internal watchdog timer to match larger received frames */
+	/*
+	 * Extend ucode internal watchdog timer to
+	 * match larger received frames
+	 */
 	if ((ampdu->rx_factor & IEEE80211_HT_AMPDU_PARM_FACTOR) ==
 	    IEEE80211_HT_MAX_AMPDU_64K) {
-		brcms_c_write_shm(wlc, M_MIMO_MAXSYM, MIMO_MAXSYM_MAX);
-		brcms_c_write_shm(wlc, M_WATCHDOG_8TU, WATCHDOG_8TU_MAX);
+		brcms_b_write_shm(wlc->hw, M_MIMO_MAXSYM, MIMO_MAXSYM_MAX);
+		brcms_b_write_shm(wlc->hw, M_WATCHDOG_8TU, WATCHDOG_8TU_MAX);
 	} else {
-		brcms_c_write_shm(wlc, M_MIMO_MAXSYM, MIMO_MAXSYM_DEF);
-		brcms_c_write_shm(wlc, M_WATCHDOG_8TU, WATCHDOG_8TU_DEF);
+		brcms_b_write_shm(wlc->hw, M_MIMO_MAXSYM, MIMO_MAXSYM_DEF);
+		brcms_b_write_shm(wlc->hw, M_WATCHDOG_8TU, WATCHDOG_8TU_DEF);
 	}
 }
 
@@ -1211,9 +1234,8 @@ void brcms_c_ampdu_flush(struct brcms_c_info *wlc,
 
 	ampdu_pars.sta = sta;
 	ampdu_pars.tid = tid;
-	for (prec = 0; prec < pq->num_prec; prec++) {
+	for (prec = 0; prec < pq->num_prec; prec++)
 		brcmu_pktq_pflush(pq, prec, true, cb_del_ampdu_pkt,
 			    (void *)&ampdu_pars);
-	}
 	brcms_c_inval_dma_pkts(wlc->hw, sta, dma_cb_fn_ampdu);
 }
diff --git a/drivers/staging/brcm80211/brcmsmac/antsel.c b/drivers/staging/brcm80211/brcmsmac/antsel.c
index c4e76c093ae9..a47ce25cb9a2 100644
--- a/drivers/staging/brcm80211/brcmsmac/antsel.c
+++ b/drivers/staging/brcm80211/brcmsmac/antsel.c
@@ -18,7 +18,6 @@
 #include <net/mac80211.h>
 
 #include "types.h"
-#include "bmac.h"
 #include "main.h"
 #include "phy_shim.h"
 #include "antsel.h"
@@ -50,55 +49,76 @@
 #define ANT_SELCFG_NUM_2x4	4
 #define ANT_SELCFG_DEF_2x4	0x02	/* default antenna configuration */
 
-/* static functions */
-static int brcms_c_antsel_cfgupd(struct antsel_info *asi,
-				 struct brcms_antselcfg *antsel);
-static u8 brcms_c_antsel_id2antcfg(struct antsel_info *asi, u8 id);
-static u16 brcms_c_antsel_antcfg2antsel(struct antsel_info *asi, u8 ant_cfg);
-static void brcms_c_antsel_init_cfg(struct antsel_info *asi,
-				struct brcms_antselcfg *antsel,
-				bool auto_sel);
-
-const u16 mimo_2x4_div_antselpat_tbl[] = {
+static const u16 mimo_2x4_div_antselpat_tbl[] = {
 	0, 0, 0x9, 0xa,		/* ant0: 0 ant1: 2,3 */
 	0, 0, 0x5, 0x6,		/* ant0: 1 ant1: 2,3 */
 	0, 0, 0, 0,		/* n.a.              */
 	0, 0, 0, 0		/* n.a.              */
 };
 
-const u8 mimo_2x4_div_antselid_tbl[16] = {
+static const u8 mimo_2x4_div_antselid_tbl[16] = {
 	0, 0, 0, 0, 0, 2, 3, 0,
 	0, 0, 1, 0, 0, 0, 0, 0	/* pat to antselid */
 };
 
-const u16 mimo_2x3_div_antselpat_tbl[] = {
+static const u16 mimo_2x3_div_antselpat_tbl[] = {
 	16, 0, 1, 16,		/* ant0: 0 ant1: 1,2 */
 	16, 16, 16, 16,		/* n.a.              */
 	16, 2, 16, 16,		/* ant0: 2 ant1: 1   */
 	16, 16, 16, 16		/* n.a.              */
 };
 
-const u8 mimo_2x3_div_antselid_tbl[16] = {
+static const u8 mimo_2x3_div_antselid_tbl[16] = {
 	0, 1, 2, 0, 0, 0, 0, 0,
 	0, 0, 0, 0, 0, 0, 0, 0	/* pat to antselid */
 };
 
+/* boardlevel antenna selection: init antenna selection structure */
+static void
+brcms_c_antsel_init_cfg(struct antsel_info *asi, struct brcms_antselcfg *antsel,
+		    bool auto_sel)
+{
+	if (asi->antsel_type == ANTSEL_2x3) {
+		u8 antcfg_def = ANT_SELCFG_DEF_2x3 |
+		    ((asi->antsel_avail && auto_sel) ? ANT_SELCFG_AUTO : 0);
+		antsel->ant_config[ANT_SELCFG_TX_DEF] = antcfg_def;
+		antsel->ant_config[ANT_SELCFG_TX_UNICAST] = antcfg_def;
+		antsel->ant_config[ANT_SELCFG_RX_DEF] = antcfg_def;
+		antsel->ant_config[ANT_SELCFG_RX_UNICAST] = antcfg_def;
+		antsel->num_antcfg = ANT_SELCFG_NUM_2x3;
+
+	} else if (asi->antsel_type == ANTSEL_2x4) {
+
+		antsel->ant_config[ANT_SELCFG_TX_DEF] = ANT_SELCFG_DEF_2x4;
+		antsel->ant_config[ANT_SELCFG_TX_UNICAST] = ANT_SELCFG_DEF_2x4;
+		antsel->ant_config[ANT_SELCFG_RX_DEF] = ANT_SELCFG_DEF_2x4;
+		antsel->ant_config[ANT_SELCFG_RX_UNICAST] = ANT_SELCFG_DEF_2x4;
+		antsel->num_antcfg = ANT_SELCFG_NUM_2x4;
+
+	} else {		/* no antenna selection available */
+
+		antsel->ant_config[ANT_SELCFG_TX_DEF] = ANT_SELCFG_DEF_2x2;
+		antsel->ant_config[ANT_SELCFG_TX_UNICAST] = ANT_SELCFG_DEF_2x2;
+		antsel->ant_config[ANT_SELCFG_RX_DEF] = ANT_SELCFG_DEF_2x2;
+		antsel->ant_config[ANT_SELCFG_RX_UNICAST] = ANT_SELCFG_DEF_2x2;
+		antsel->num_antcfg = 0;
+	}
+}
+
 struct antsel_info *brcms_c_antsel_attach(struct brcms_c_info *wlc)
 {
 	struct antsel_info *asi;
+	struct si_pub *sih = wlc->hw->sih;
 
 	asi = kzalloc(sizeof(struct antsel_info), GFP_ATOMIC);
-	if (!asi) {
-		wiphy_err(wlc->wiphy, "wl%d: brcms_c_antsel_attach: out of "
-			  "mem\n", wlc->pub->unit);
+	if (!asi)
 		return NULL;
-	}
 
 	asi->wlc = wlc;
 	asi->pub = wlc->pub;
 	asi->antsel_type = ANTSEL_NA;
 	asi->antsel_avail = false;
-	asi->antsel_antswitch = (u8) getintvar(asi->pub->vars, "antswitch");
+	asi->antsel_antswitch = (u8) getintvar(sih, BRCMS_SROM_ANTSWITCH);
 
 	if ((asi->pub->sromrev >= 4) && (asi->antsel_antswitch != 0)) {
 		switch (asi->antsel_antswitch) {
@@ -108,27 +128,26 @@ struct antsel_info *brcms_c_antsel_attach(struct brcms_c_info *wlc)
 			/* 4321/2 board with 2x3 switch logic */
 			asi->antsel_type = ANTSEL_2x3;
 			/* Antenna selection availability */
-			if (((u16) getintvar(asi->pub->vars, "aa2g") == 7) ||
-			    ((u16) getintvar(asi->pub->vars, "aa5g") == 7)) {
+			if (((u16) getintvar(sih, BRCMS_SROM_AA2G) == 7) ||
+			    ((u16) getintvar(sih, BRCMS_SROM_AA5G) == 7)) {
 				asi->antsel_avail = true;
-			} else
-			    if (((u16) getintvar(asi->pub->vars, "aa2g") ==
-				 3)
-				|| ((u16) getintvar(asi->pub->vars, "aa5g")
-				    == 3)) {
+			} else if (
+				(u16) getintvar(sih, BRCMS_SROM_AA2G) == 3 ||
+				(u16) getintvar(sih, BRCMS_SROM_AA5G) == 3) {
 				asi->antsel_avail = false;
 			} else {
 				asi->antsel_avail = false;
 				wiphy_err(wlc->wiphy, "antsel_attach: 2o3 "
 					  "board cfg invalid\n");
 			}
+
 			break;
 		default:
 			break;
 		}
 	} else if ((asi->pub->sromrev == 4) &&
-		   ((u16) getintvar(asi->pub->vars, "aa2g") == 7) &&
-		   ((u16) getintvar(asi->pub->vars, "aa5g") == 0)) {
+		   ((u16) getintvar(sih, BRCMS_SROM_AA2G) == 7) &&
+		   ((u16) getintvar(sih, BRCMS_SROM_AA5G) == 0)) {
 		/* hack to match old 4321CB2 cards with 2of3 antenna switch */
 		asi->antsel_type = ANTSEL_2x3;
 		asi->antsel_avail = true;
@@ -152,6 +171,64 @@ void brcms_c_antsel_detach(struct antsel_info *asi)
 	kfree(asi);
 }
 
+/*
+ * boardlevel antenna selection:
+ *   convert ant_cfg to mimo_antsel (ucode interface)
+ */
+static u16 brcms_c_antsel_antcfg2antsel(struct antsel_info *asi, u8 ant_cfg)
+{
+	u8 idx = BRCMS_ANTIDX_11N(BRCMS_ANTSEL_11N(ant_cfg));
+	u16 mimo_antsel = 0;
+
+	if (asi->antsel_type == ANTSEL_2x4) {
+		/* 2x4 antenna diversity board, 4 cfgs: 0-2 0-3 1-2 1-3 */
+		mimo_antsel = (mimo_2x4_div_antselpat_tbl[idx] & 0xf);
+		return mimo_antsel;
+
+	} else if (asi->antsel_type == ANTSEL_2x3) {
+		/* 2x3 antenna selection, 3 cfgs: 0-1 0-2 2-1 */
+		mimo_antsel = (mimo_2x3_div_antselpat_tbl[idx] & 0xf);
+		return mimo_antsel;
+	}
+
+	return mimo_antsel;
+}
+
+/* boardlevel antenna selection: ucode interface control */
+static int brcms_c_antsel_cfgupd(struct antsel_info *asi,
+				 struct brcms_antselcfg *antsel)
+{
+	struct brcms_c_info *wlc = asi->wlc;
+	u8 ant_cfg;
+	u16 mimo_antsel;
+
+	/* 1) Update TX antconfig for all frames that are not unicast data
+	 *    (aka default TX)
+	 */
+	ant_cfg = antsel->ant_config[ANT_SELCFG_TX_DEF];
+	mimo_antsel = brcms_c_antsel_antcfg2antsel(asi, ant_cfg);
+	brcms_b_write_shm(wlc->hw, M_MIMO_ANTSEL_TXDFLT, mimo_antsel);
+	/*
+	 * Update driver stats for currently selected
+	 * default tx/rx antenna config
+	 */
+	asi->antcfg_cur.ant_config[ANT_SELCFG_TX_DEF] = ant_cfg;
+
+	/* 2) Update RX antconfig for all frames that are not unicast data
+	 *    (aka default RX)
+	 */
+	ant_cfg = antsel->ant_config[ANT_SELCFG_RX_DEF];
+	mimo_antsel = brcms_c_antsel_antcfg2antsel(asi, ant_cfg);
+	brcms_b_write_shm(wlc->hw, M_MIMO_ANTSEL_RXDFLT, mimo_antsel);
+	/*
+	 * Update driver stats for currently selected
+	 * default tx/rx antenna config
+	 */
+	asi->antcfg_cur.ant_config[ANT_SELCFG_RX_DEF] = ant_cfg;
+
+	return 0;
+}
+
 void brcms_c_antsel_init(struct antsel_info *asi)
 {
 	if ((asi->antsel_type == ANTSEL_2x3) ||
@@ -159,36 +236,23 @@ void brcms_c_antsel_init(struct antsel_info *asi)
 		brcms_c_antsel_cfgupd(asi, &asi->antcfg_11n);
 }
 
-/* boardlevel antenna selection: init antenna selection structure */
-static void
-brcms_c_antsel_init_cfg(struct antsel_info *asi, struct brcms_antselcfg *antsel,
-		    bool auto_sel)
+/* boardlevel antenna selection: convert id to ant_cfg */
+static u8 brcms_c_antsel_id2antcfg(struct antsel_info *asi, u8 id)
 {
-	if (asi->antsel_type == ANTSEL_2x3) {
-		u8 antcfg_def = ANT_SELCFG_DEF_2x3 |
-		    ((asi->antsel_avail && auto_sel) ? ANT_SELCFG_AUTO : 0);
-		antsel->ant_config[ANT_SELCFG_TX_DEF] = antcfg_def;
-		antsel->ant_config[ANT_SELCFG_TX_UNICAST] = antcfg_def;
-		antsel->ant_config[ANT_SELCFG_RX_DEF] = antcfg_def;
-		antsel->ant_config[ANT_SELCFG_RX_UNICAST] = antcfg_def;
-		antsel->num_antcfg = ANT_SELCFG_NUM_2x3;
-
-	} else if (asi->antsel_type == ANTSEL_2x4) {
-
-		antsel->ant_config[ANT_SELCFG_TX_DEF] = ANT_SELCFG_DEF_2x4;
-		antsel->ant_config[ANT_SELCFG_TX_UNICAST] = ANT_SELCFG_DEF_2x4;
-		antsel->ant_config[ANT_SELCFG_RX_DEF] = ANT_SELCFG_DEF_2x4;
-		antsel->ant_config[ANT_SELCFG_RX_UNICAST] = ANT_SELCFG_DEF_2x4;
-		antsel->num_antcfg = ANT_SELCFG_NUM_2x4;
+	u8 antcfg = ANT_SELCFG_DEF_2x2;
 
-	} else {		/* no antenna selection available */
+	if (asi->antsel_type == ANTSEL_2x4) {
+		/* 2x4 antenna diversity board, 4 cfgs: 0-2 0-3 1-2 1-3 */
+		antcfg = (((id & 0x2) << 3) | ((id & 0x1) + 2));
+		return antcfg;
 
-		antsel->ant_config[ANT_SELCFG_TX_DEF] = ANT_SELCFG_DEF_2x2;
-		antsel->ant_config[ANT_SELCFG_TX_UNICAST] = ANT_SELCFG_DEF_2x2;
-		antsel->ant_config[ANT_SELCFG_RX_DEF] = ANT_SELCFG_DEF_2x2;
-		antsel->ant_config[ANT_SELCFG_RX_UNICAST] = ANT_SELCFG_DEF_2x2;
-		antsel->num_antcfg = 0;
+	} else if (asi->antsel_type == ANTSEL_2x3) {
+		/* 2x3 antenna selection, 3 cfgs: 0-1 0-2 2-1 */
+		antcfg = (((id & 0x02) << 4) | ((id & 0x1) + 1));
+		return antcfg;
 	}
+
+	return antcfg;
 }
 
 void
@@ -241,71 +305,3 @@ u8 brcms_c_antsel_antsel2id(struct antsel_info *asi, u16 antsel)
 
 	return antselid;
 }
-
-/* boardlevel antenna selection: convert id to ant_cfg */
-static u8 brcms_c_antsel_id2antcfg(struct antsel_info *asi, u8 id)
-{
-	u8 antcfg = ANT_SELCFG_DEF_2x2;
-
-	if (asi->antsel_type == ANTSEL_2x4) {
-		/* 2x4 antenna diversity board, 4 cfgs: 0-2 0-3 1-2 1-3 */
-		antcfg = (((id & 0x2) << 3) | ((id & 0x1) + 2));
-		return antcfg;
-
-	} else if (asi->antsel_type == ANTSEL_2x3) {
-		/* 2x3 antenna selection, 3 cfgs: 0-1 0-2 2-1 */
-		antcfg = (((id & 0x02) << 4) | ((id & 0x1) + 1));
-		return antcfg;
-	}
-
-	return antcfg;
-}
-
-/* boardlevel antenna selection: convert ant_cfg to mimo_antsel (ucode interface) */
-static u16 brcms_c_antsel_antcfg2antsel(struct antsel_info *asi, u8 ant_cfg)
-{
-	u8 idx = BRCMS_ANTIDX_11N(BRCMS_ANTSEL_11N(ant_cfg));
-	u16 mimo_antsel = 0;
-
-	if (asi->antsel_type == ANTSEL_2x4) {
-		/* 2x4 antenna diversity board, 4 cfgs: 0-2 0-3 1-2 1-3 */
-		mimo_antsel = (mimo_2x4_div_antselpat_tbl[idx] & 0xf);
-		return mimo_antsel;
-
-	} else if (asi->antsel_type == ANTSEL_2x3) {
-		/* 2x3 antenna selection, 3 cfgs: 0-1 0-2 2-1 */
-		mimo_antsel = (mimo_2x3_div_antselpat_tbl[idx] & 0xf);
-		return mimo_antsel;
-	}
-
-	return mimo_antsel;
-}
-
-/* boardlevel antenna selection: ucode interface control */
-static int brcms_c_antsel_cfgupd(struct antsel_info *asi,
-				 struct brcms_antselcfg *antsel)
-{
-	struct brcms_c_info *wlc = asi->wlc;
-	u8 ant_cfg;
-	u16 mimo_antsel;
-
-	/* 1) Update TX antconfig for all frames that are not unicast data
-	 *    (aka default TX)
-	 */
-	ant_cfg = antsel->ant_config[ANT_SELCFG_TX_DEF];
-	mimo_antsel = brcms_c_antsel_antcfg2antsel(asi, ant_cfg);
-	brcms_c_write_shm(wlc, M_MIMO_ANTSEL_TXDFLT, mimo_antsel);
-	/* Update driver stats for currently selected default tx/rx antenna config */
-	asi->antcfg_cur.ant_config[ANT_SELCFG_TX_DEF] = ant_cfg;
-
-	/* 2) Update RX antconfig for all frames that are not unicast data
-	 *    (aka default RX)
-	 */
-	ant_cfg = antsel->ant_config[ANT_SELCFG_RX_DEF];
-	mimo_antsel = brcms_c_antsel_antcfg2antsel(asi, ant_cfg);
-	brcms_c_write_shm(wlc, M_MIMO_ANTSEL_RXDFLT, mimo_antsel);
-	/* Update driver stats for currently selected default tx/rx antenna config */
-	asi->antcfg_cur.ant_config[ANT_SELCFG_RX_DEF] = ant_cfg;
-
-	return 0;
-}
diff --git a/drivers/staging/brcm80211/brcmsmac/bmac.c b/drivers/staging/brcm80211/brcmsmac/bmac.c
deleted file mode 100644
index b25c51705566..000000000000
--- a/drivers/staging/brcm80211/brcmsmac/bmac.c
+++ /dev/null
@@ -1,3593 +0,0 @@
-/*
- * Copyright (c) 2010 Broadcom Corporation
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
- * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
- * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
- * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-#include <linux/pci.h>
-#include <net/mac80211.h>
-
-#include <brcm_hw_ids.h>
-#include <aiutils.h>
-#include <chipcommon.h>
-#include "types.h"
-#include "rate.h"
-#include "phy/phy_hal.h"
-#include "channel.h"
-#include "main.h"
-#include "ucode_loader.h"
-#include "mac80211_if.h"
-#include "bmac.h"
-
-#define	TIMER_INTERVAL_WATCHDOG_BMAC	1000	/* watchdog timer, in unit of ms */
-
-#define	SYNTHPU_DLY_APHY_US	3700	/* a phy synthpu_dly time in us */
-#define	SYNTHPU_DLY_BPHY_US	1050	/* b/g phy synthpu_dly time in us, default */
-#define	SYNTHPU_DLY_NPHY_US	2048	/* n phy REV3 synthpu_dly time in us, default */
-#define	SYNTHPU_DLY_LPPHY_US	300	/* lpphy synthpu_dly time in us */
-
-#define	SYNTHPU_DLY_PHY_US_QT	100	/* QT synthpu_dly time in us */
-
-#ifndef BMAC_DUP_TO_REMOVE
-
-#define	ANTCNT			10	/* vanilla M_MAX_ANTCNT value */
-
-#endif				/* BMAC_DUP_TO_REMOVE */
-
-#define DMAREG(wlc_hw, direction, fifonum) \
-	((direction == DMA_TX) ? \
-		(void *)&(wlc_hw->regs->fifo64regs[fifonum].dmaxmt) : \
-		(void *)&(wlc_hw->regs->fifo64regs[fifonum].dmarcv))
-
-#define APHY_SLOT_TIME		9
-#define BPHY_SLOT_TIME		20
-
-/*
- * The following table lists the buffer memory allocated to xmt fifos in HW.
- * the size is in units of 256bytes(one block), total size is HW dependent
- * ucode has default fifo partition, sw can overwrite if necessary
- *
- * This is documented in twiki under the topic UcodeTxFifo. Please ensure
- * the twiki is updated before making changes.
- */
-
-#define XMTFIFOTBL_STARTREV	20	/* Starting corerev for the fifo size table */
-
-static u16 xmtfifo_sz[][NFIFO] = {
-	{20, 192, 192, 21, 17, 5},	/* corerev 20: 5120, 49152, 49152, 5376, 4352, 1280 */
-	{9, 58, 22, 14, 14, 5},	/* corerev 21: 2304, 14848, 5632, 3584, 3584, 1280 */
-	{20, 192, 192, 21, 17, 5},	/* corerev 22: 5120, 49152, 49152, 5376, 4352, 1280 */
-	{20, 192, 192, 21, 17, 5},	/* corerev 23: 5120, 49152, 49152, 5376, 4352, 1280 */
-	{9, 58, 22, 14, 14, 5},	/* corerev 24: 2304, 14848, 5632, 3584, 3584, 1280 */
-};
-
-static void brcms_b_clkctl_clk(struct brcms_hardware *wlc, uint mode);
-static void brcms_b_coreinit(struct brcms_c_info *wlc);
-
-/* used by wlc_wakeucode_init() */
-static void brcms_c_write_inits(struct brcms_hardware *wlc_hw,
-			    const struct d11init *inits);
-static void brcms_ucode_write(struct brcms_hardware *wlc_hw, const u32 ucode[],
-			    const uint nbytes);
-static void brcms_ucode_download(struct brcms_hardware *wlc);
-static void brcms_c_ucode_txant_set(struct brcms_hardware *wlc_hw);
-
-/* used by brcms_c_dpc() */
-static bool brcms_b_dotxstatus(struct brcms_hardware *wlc,
-			       struct tx_status *txs, u32 s2);
-static bool brcms_b_txstatus(struct brcms_hardware *wlc, bool bound,
-			     bool *fatal);
-static bool brcms_b_recv(struct brcms_hardware *wlc_hw, uint fifo, bool bound);
-
-/* used by brcms_c_down() */
-static void brcms_c_flushqueues(struct brcms_c_info *wlc);
-
-static void brcms_c_write_mhf(struct brcms_hardware *wlc_hw, u16 *mhfs);
-static void brcms_c_mctrl_reset(struct brcms_hardware *wlc_hw);
-static void brcms_b_corerev_fifofixup(struct brcms_hardware *wlc_hw);
-static bool brcms_b_tx_fifo_suspended(struct brcms_hardware *wlc_hw,
-				       uint tx_fifo);
-static void brcms_b_tx_fifo_suspend(struct brcms_hardware *wlc_hw,
-				    uint tx_fifo);
-static void brcms_b_tx_fifo_resume(struct brcms_hardware *wlc_hw,
-				   uint tx_fifo);
-
-/* Low Level Prototypes */
-static int brcms_b_bandtype(struct brcms_hardware *wlc_hw);
-static void brcms_b_info_init(struct brcms_hardware *wlc_hw);
-static void brcms_b_xtal(struct brcms_hardware *wlc_hw, bool want);
-static u16 brcms_b_read_objmem(struct brcms_hardware *wlc_hw, uint offset,
-				   u32 sel);
-static void brcms_b_write_objmem(struct brcms_hardware *wlc_hw, uint offset,
-				  u16 v, u32 sel);
-static void brcms_b_core_phy_clk(struct brcms_hardware *wlc_hw, bool clk);
-static bool brcms_b_attach_dmapio(struct brcms_c_info *wlc, uint j, bool wme);
-static void brcms_b_detach_dmapio(struct brcms_hardware *wlc_hw);
-static void brcms_c_ucode_bsinit(struct brcms_hardware *wlc_hw);
-static bool brcms_c_validboardtype(struct brcms_hardware *wlc);
-static bool brcms_c_isgoodchip(struct brcms_hardware *wlc_hw);
-static bool brcms_b_validate_chip_access(struct brcms_hardware *wlc_hw);
-static char *brcms_c_get_macaddr(struct brcms_hardware *wlc_hw);
-static void brcms_c_mhfdef(struct brcms_c_info *wlc, u16 *mhfs, u16 mhf2_init);
-static void brcms_c_mctrl_write(struct brcms_hardware *wlc_hw);
-static void brcms_b_mute(struct brcms_hardware *wlc_hw, bool want,
-			 mbool flags);
-static void brcms_c_ucode_mute_override_set(struct brcms_hardware *wlc_hw);
-static void brcms_c_ucode_mute_override_clear(struct brcms_hardware *wlc_hw);
-static u32 brcms_c_wlintrsoff(struct brcms_c_info *wlc);
-static void brcms_c_wlintrsrestore(struct brcms_c_info *wlc, u32 macintmask);
-static void brcms_c_gpio_init(struct brcms_c_info *wlc);
-static void brcms_c_write_hw_bcntemplate0(struct brcms_hardware *wlc_hw,
-					  void *bcn, int len);
-static void brcms_c_write_hw_bcntemplate1(struct brcms_hardware *wlc_hw,
-					  void *bcn, int len);
-static void brcms_b_bsinit(struct brcms_c_info *wlc, chanspec_t chanspec);
-static u32 brcms_c_setband_inact(struct brcms_c_info *wlc, uint bandunit);
-static void brcms_b_setband(struct brcms_hardware *wlc_hw, uint bandunit,
-			     chanspec_t chanspec);
-static void brcms_b_update_slot_timing(struct brcms_hardware *wlc_hw,
-					bool shortslot);
-static void brcms_upd_ofdm_pctl1_table(struct brcms_hardware *wlc_hw);
-static u16 brcms_b_ofdm_ratetable_offset(struct brcms_hardware *wlc_hw,
-					     u8 rate);
-
-/* === Low Level functions === */
-
-void brcms_b_set_shortslot(struct brcms_hardware *wlc_hw, bool shortslot)
-{
-	wlc_hw->shortslot = shortslot;
-
-	if (BAND_2G(brcms_b_bandtype(wlc_hw)) && wlc_hw->up) {
-		brcms_c_suspend_mac_and_wait(wlc_hw->wlc);
-		brcms_b_update_slot_timing(wlc_hw, shortslot);
-		brcms_c_enable_mac(wlc_hw->wlc);
-	}
-}
-
-/*
- * Update the slot timing for standard 11b/g (20us slots)
- * or shortslot 11g (9us slots)
- * The PSM needs to be suspended for this call.
- */
-static void brcms_b_update_slot_timing(struct brcms_hardware *wlc_hw,
-					bool shortslot)
-{
-	d11regs_t *regs;
-
-	regs = wlc_hw->regs;
-
-	if (shortslot) {
-		/* 11g short slot: 11a timing */
-		W_REG(&regs->ifs_slot, 0x0207);	/* APHY_SLOT_TIME */
-		brcms_b_write_shm(wlc_hw, M_DOT11_SLOT, APHY_SLOT_TIME);
-	} else {
-		/* 11g long slot: 11b timing */
-		W_REG(&regs->ifs_slot, 0x0212);	/* BPHY_SLOT_TIME */
-		brcms_b_write_shm(wlc_hw, M_DOT11_SLOT, BPHY_SLOT_TIME);
-	}
-}
-
-static void brcms_c_ucode_bsinit(struct brcms_hardware *wlc_hw)
-{
-	struct wiphy *wiphy = wlc_hw->wlc->wiphy;
-
-	/* init microcode host flags */
-	brcms_c_write_mhf(wlc_hw, wlc_hw->band->mhfs);
-
-	/* do band-specific ucode IHR, SHM, and SCR inits */
-	if (D11REV_IS(wlc_hw->corerev, 23)) {
-		if (BRCMS_ISNPHY(wlc_hw->band)) {
-			brcms_c_write_inits(wlc_hw, d11n0bsinitvals16);
-		} else {
-			wiphy_err(wiphy, "%s: wl%d: unsupported phy in corerev"
-				  " %d\n", __func__, wlc_hw->unit,
-				  wlc_hw->corerev);
-		}
-	} else {
-		if (D11REV_IS(wlc_hw->corerev, 24)) {
-			if (BRCMS_ISLCNPHY(wlc_hw->band)) {
-				brcms_c_write_inits(wlc_hw,
-						    d11lcn0bsinitvals24);
-			} else
-				wiphy_err(wiphy, "%s: wl%d: unsupported phy in"
-					  " core rev %d\n", __func__,
-					  wlc_hw->unit, wlc_hw->corerev);
-		} else {
-			wiphy_err(wiphy, "%s: wl%d: unsupported corerev %d\n",
-				__func__, wlc_hw->unit, wlc_hw->corerev);
-		}
-	}
-}
-
-/* switch to new band but leave it inactive */
-static u32 brcms_c_setband_inact(struct brcms_c_info *wlc,
-					    uint bandunit)
-{
-	struct brcms_hardware *wlc_hw = wlc->hw;
-	u32 macintmask;
-
-	BCMMSG(wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	WARN_ON((R_REG(&wlc_hw->regs->maccontrol) & MCTL_EN_MAC) != 0);
-
-	/* disable interrupts */
-	macintmask = brcms_intrsoff(wlc->wl);
-
-	/* radio off */
-	wlc_phy_switch_radio(wlc_hw->band->pi, OFF);
-
-	brcms_b_core_phy_clk(wlc_hw, OFF);
-
-	brcms_c_setxband(wlc_hw, bandunit);
-
-	return macintmask;
-}
-
-/* Process received frames */
-/*
- * Return true if more frames need to be processed. false otherwise.
- * Param 'bound' indicates max. # frames to process before break out.
- */
-static bool
-brcms_b_recv(struct brcms_hardware *wlc_hw, uint fifo, bool bound)
-{
-	struct sk_buff *p;
-	struct sk_buff *head = NULL;
-	struct sk_buff *tail = NULL;
-	uint n = 0;
-	uint bound_limit = bound ? wlc_hw->wlc->pub->tunables->rxbnd : -1;
-	struct brcms_d11rxhdr *wlc_rxhdr = NULL;
-
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-	/* gather received frames */
-	while ((p = dma_rx(wlc_hw->di[fifo]))) {
-
-		if (!tail)
-			head = tail = p;
-		else {
-			tail->prev = p;
-			tail = p;
-		}
-
-		/* !give others some time to run! */
-		if (++n >= bound_limit)
-			break;
-	}
-
-	/* post more rbufs */
-	dma_rxfill(wlc_hw->di[fifo]);
-
-	/* process each frame */
-	while ((p = head) != NULL) {
-		head = head->prev;
-		p->prev = NULL;
-
-		wlc_rxhdr = (struct brcms_d11rxhdr *) p->data;
-
-		/* compute the RSSI from d11rxhdr and record it in wlc_rxd11hr */
-		wlc_phy_rssi_compute(wlc_hw->band->pi, wlc_rxhdr);
-
-		brcms_c_recv(wlc_hw->wlc, p);
-	}
-
-	return n >= bound_limit;
-}
-
-/* second-level interrupt processing
- *   Return true if another dpc needs to be re-scheduled. false otherwise.
- *   Param 'bounded' indicates if applicable loops should be bounded.
- */
-bool brcms_c_dpc(struct brcms_c_info *wlc, bool bounded)
-{
-	u32 macintstatus;
-	struct brcms_hardware *wlc_hw = wlc->hw;
-	d11regs_t *regs = wlc_hw->regs;
-	bool fatal = false;
-	struct wiphy *wiphy = wlc->wiphy;
-
-	if (DEVICEREMOVED(wlc)) {
-		wiphy_err(wiphy, "wl%d: %s: dead chip\n", wlc_hw->unit,
-			  __func__);
-		brcms_down(wlc->wl);
-		return false;
-	}
-
-	/* grab and clear the saved software intstatus bits */
-	macintstatus = wlc->macintstatus;
-	wlc->macintstatus = 0;
-
-	BCMMSG(wlc->wiphy, "wl%d: macintstatus 0x%x\n",
-	       wlc_hw->unit, macintstatus);
-
-	WARN_ON(macintstatus & MI_PRQ); /* PRQ Interrupt in non-MBSS */
-
-	/* BCN template is available */
-	/* ZZZ: Use AP_ACTIVE ? */
-	if (AP_ENAB(wlc->pub) && (!APSTA_ENAB(wlc->pub))
-	    && (macintstatus & MI_BCNTPL)) {
-		brcms_c_update_beacon(wlc);
-	}
-
-	/* tx status */
-	if (macintstatus & MI_TFS) {
-		if (brcms_b_txstatus(wlc->hw, bounded, &fatal))
-			wlc->macintstatus |= MI_TFS;
-		if (fatal) {
-			wiphy_err(wiphy, "MI_TFS: fatal\n");
-			goto fatal;
-		}
-	}
-
-	if (macintstatus & (MI_TBTT | MI_DTIM_TBTT))
-		brcms_c_tbtt(wlc);
-
-	/* ATIM window end */
-	if (macintstatus & MI_ATIMWINEND) {
-		BCMMSG(wlc->wiphy, "end of ATIM window\n");
-		OR_REG(&regs->maccommand, wlc->qvalid);
-		wlc->qvalid = 0;
-	}
-
-	/* received data or control frame, MI_DMAINT is indication of RX_FIFO interrupt */
-	if (macintstatus & MI_DMAINT)
-		if (brcms_b_recv(wlc_hw, RX_FIFO, bounded))
-			wlc->macintstatus |= MI_DMAINT;
-
-	/* TX FIFO suspend/flush completion */
-	if (macintstatus & MI_TXSTOP)
-		brcms_b_tx_fifo_suspended(wlc_hw, TX_DATA_FIFO);
-
-	/* noise sample collected */
-	if (macintstatus & MI_BG_NOISE) {
-		wlc_phy_noise_sample_intr(wlc_hw->band->pi);
-	}
-
-	if (macintstatus & MI_GP0) {
-		wiphy_err(wiphy, "wl%d: PSM microcode watchdog fired at %d "
-			"(seconds). Resetting.\n", wlc_hw->unit, wlc_hw->now);
-
-		printk_once("%s : PSM Watchdog, chipid 0x%x, chiprev 0x%x\n",
-					__func__, wlc_hw->sih->chip,
-					wlc_hw->sih->chiprev);
-		/* big hammer */
-		brcms_init(wlc->wl);
-	}
-
-	/* gptimer timeout */
-	if (macintstatus & MI_TO) {
-		W_REG(&regs->gptimer, 0);
-	}
-
-	if (macintstatus & MI_RFDISABLE) {
-		BCMMSG(wlc->wiphy, "wl%d: BMAC Detected a change on the"
-		       " RF Disable Input\n", wlc_hw->unit);
-		brcms_rfkill_set_hw_state(wlc->wl);
-	}
-
-	/* send any enq'd tx packets. Just makes sure to jump start tx */
-	if (!pktq_empty(&wlc->pkt_queue->q))
-		brcms_c_send_q(wlc);
-
-	/* it isn't done and needs to be resched if macintstatus is non-zero */
-	return wlc->macintstatus != 0;
-
- fatal:
-	brcms_init(wlc->wl);
-	return wlc->macintstatus != 0;
-}
-
-/* common low-level watchdog code */
-void brcms_b_watchdog(void *arg)
-{
-	struct brcms_c_info *wlc = (struct brcms_c_info *) arg;
-	struct brcms_hardware *wlc_hw = wlc->hw;
-
-	BCMMSG(wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	if (!wlc_hw->up)
-		return;
-
-	/* increment second count */
-	wlc_hw->now++;
-
-	/* Check for FIFO error interrupts */
-	brcms_b_fifoerrors(wlc_hw);
-
-	/* make sure RX dma has buffers */
-	dma_rxfill(wlc->hw->di[RX_FIFO]);
-
-	wlc_phy_watchdog(wlc_hw->band->pi);
-}
-
-void
-brcms_b_set_chanspec(struct brcms_hardware *wlc_hw, chanspec_t chanspec,
-		      bool mute, struct txpwr_limits *txpwr)
-{
-	uint bandunit;
-
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d: 0x%x\n", wlc_hw->unit, chanspec);
-
-	wlc_hw->chanspec = chanspec;
-
-	/* Switch bands if necessary */
-	if (NBANDS_HW(wlc_hw) > 1) {
-		bandunit = CHSPEC_BANDUNIT(chanspec);
-		if (wlc_hw->band->bandunit != bandunit) {
-			/* brcms_b_setband disables other bandunit,
-			 *  use light band switch if not up yet
-			 */
-			if (wlc_hw->up) {
-				wlc_phy_chanspec_radio_set(wlc_hw->
-							   bandstate[bandunit]->
-							   pi, chanspec);
-				brcms_b_setband(wlc_hw, bandunit, chanspec);
-			} else {
-				brcms_c_setxband(wlc_hw, bandunit);
-			}
-		}
-	}
-
-	wlc_phy_initcal_enable(wlc_hw->band->pi, !mute);
-
-	if (!wlc_hw->up) {
-		if (wlc_hw->clk)
-			wlc_phy_txpower_limit_set(wlc_hw->band->pi, txpwr,
-						  chanspec);
-		wlc_phy_chanspec_radio_set(wlc_hw->band->pi, chanspec);
-	} else {
-		wlc_phy_chanspec_set(wlc_hw->band->pi, chanspec);
-		wlc_phy_txpower_limit_set(wlc_hw->band->pi, txpwr, chanspec);
-
-		/* Update muting of the channel */
-		brcms_b_mute(wlc_hw, mute, 0);
-	}
-}
-
-int brcms_b_state_get(struct brcms_hardware *wlc_hw,
-		      struct brcms_b_state *state)
-{
-	state->machwcap = wlc_hw->machwcap;
-
-	return 0;
-}
-
-static bool brcms_b_attach_dmapio(struct brcms_c_info *wlc, uint j, bool wme)
-{
-	uint i;
-	char name[8];
-	/* ucode host flag 2 needed for pio mode, independent of band and fifo */
-	u16 pio_mhf2 = 0;
-	struct brcms_hardware *wlc_hw = wlc->hw;
-	uint unit = wlc_hw->unit;
-	struct brcms_tunables *tune = wlc->pub->tunables;
-	struct wiphy *wiphy = wlc->wiphy;
-
-	/* name and offsets for dma_attach */
-	snprintf(name, sizeof(name), "wl%d", unit);
-
-	if (wlc_hw->di[0] == 0) {	/* Init FIFOs */
-		uint addrwidth;
-		int dma_attach_err = 0;
-		/* Find out the DMA addressing capability and let OS know
-		 * All the channels within one DMA core have 'common-minimum' same
-		 * capability
-		 */
-		addrwidth =
-		    dma_addrwidth(wlc_hw->sih, DMAREG(wlc_hw, DMA_TX, 0));
-
-		if (!wl_alloc_dma_resources(wlc_hw->wlc->wl, addrwidth)) {
-			wiphy_err(wiphy, "wl%d: wlc_attach: alloc_dma_"
-				  "resources failed\n", unit);
-			return false;
-		}
-
-		/*
-		 * FIFO 0
-		 * TX: TX_AC_BK_FIFO (TX AC Background data packets)
-		 * RX: RX_FIFO (RX data packets)
-		 */
-		wlc_hw->di[0] = dma_attach(name, wlc_hw->sih,
-					   (wme ? DMAREG(wlc_hw, DMA_TX, 0) :
-					    NULL), DMAREG(wlc_hw, DMA_RX, 0),
-					   (wme ? tune->ntxd : 0), tune->nrxd,
-					   tune->rxbufsz, -1, tune->nrxbufpost,
-					   BRCMS_HWRXOFF, &brcm_msg_level);
-		dma_attach_err |= (NULL == wlc_hw->di[0]);
-
-		/*
-		 * FIFO 1
-		 * TX: TX_AC_BE_FIFO (TX AC Best-Effort data packets)
-		 *   (legacy) TX_DATA_FIFO (TX data packets)
-		 * RX: UNUSED
-		 */
-		wlc_hw->di[1] = dma_attach(name, wlc_hw->sih,
-					   DMAREG(wlc_hw, DMA_TX, 1), NULL,
-					   tune->ntxd, 0, 0, -1, 0, 0,
-					   &brcm_msg_level);
-		dma_attach_err |= (NULL == wlc_hw->di[1]);
-
-		/*
-		 * FIFO 2
-		 * TX: TX_AC_VI_FIFO (TX AC Video data packets)
-		 * RX: UNUSED
-		 */
-		wlc_hw->di[2] = dma_attach(name, wlc_hw->sih,
-					   DMAREG(wlc_hw, DMA_TX, 2), NULL,
-					   tune->ntxd, 0, 0, -1, 0, 0,
-					   &brcm_msg_level);
-		dma_attach_err |= (NULL == wlc_hw->di[2]);
-		/*
-		 * FIFO 3
-		 * TX: TX_AC_VO_FIFO (TX AC Voice data packets)
-		 *   (legacy) TX_CTL_FIFO (TX control & mgmt packets)
-		 */
-		wlc_hw->di[3] = dma_attach(name, wlc_hw->sih,
-					   DMAREG(wlc_hw, DMA_TX, 3),
-					   NULL, tune->ntxd, 0, 0, -1,
-					   0, 0, &brcm_msg_level);
-		dma_attach_err |= (NULL == wlc_hw->di[3]);
-/* Cleaner to leave this as if with AP defined */
-
-		if (dma_attach_err) {
-			wiphy_err(wiphy, "wl%d: wlc_attach: dma_attach failed"
-				  "\n", unit);
-			return false;
-		}
-
-		/* get pointer to dma engine tx flow control variable */
-		for (i = 0; i < NFIFO; i++)
-			if (wlc_hw->di[i])
-				wlc_hw->txavail[i] =
-				    (uint *) dma_getvar(wlc_hw->di[i],
-							"&txavail");
-	}
-
-	/* initial ucode host flags */
-	brcms_c_mhfdef(wlc, wlc_hw->band->mhfs, pio_mhf2);
-
-	return true;
-}
-
-static void brcms_b_detach_dmapio(struct brcms_hardware *wlc_hw)
-{
-	uint j;
-
-	for (j = 0; j < NFIFO; j++) {
-		if (wlc_hw->di[j]) {
-			dma_detach(wlc_hw->di[j]);
-			wlc_hw->di[j] = NULL;
-		}
-	}
-}
-
-/* low level attach
- *    run backplane attach, init nvram
- *    run phy attach
- *    initialize software state for each core and band
- *    put the whole chip in reset(driver down state), no clock
- */
-int brcms_b_attach(struct brcms_c_info *wlc, u16 vendor, u16 device, uint unit,
-		    bool piomode, void *regsva, uint bustype, void *btparam)
-{
-	struct brcms_hardware *wlc_hw;
-	d11regs_t *regs;
-	char *macaddr = NULL;
-	char *vars;
-	uint err = 0;
-	uint j;
-	bool wme = false;
-	struct shared_phy_params sha_params;
-	struct wiphy *wiphy = wlc->wiphy;
-
-	BCMMSG(wlc->wiphy, "wl%d: vendor 0x%x device 0x%x\n", unit, vendor,
-		device);
-
-	wme = true;
-
-	wlc_hw = wlc->hw;
-	wlc_hw->wlc = wlc;
-	wlc_hw->unit = unit;
-	wlc_hw->band = wlc_hw->bandstate[0];
-	wlc_hw->_piomode = piomode;
-
-	/* populate struct brcms_hardware with default values  */
-	brcms_b_info_init(wlc_hw);
-
-	/*
-	 * Do the hardware portion of the attach.
-	 * Also initialize software state that depends on the particular hardware
-	 * we are running.
-	 */
-	wlc_hw->sih = ai_attach(regsva, bustype, btparam,
-				&wlc_hw->vars, &wlc_hw->vars_size);
-	if (wlc_hw->sih == NULL) {
-		wiphy_err(wiphy, "wl%d: brcms_b_attach: si_attach failed\n",
-			  unit);
-		err = 11;
-		goto fail;
-	}
-	vars = wlc_hw->vars;
-
-	/*
-	 * Get vendid/devid nvram overwrites, which could be different
-	 * than those the BIOS recognizes for devices on PCMCIA_BUS,
-	 * SDIO_BUS, and SROMless devices on PCI_BUS.
-	 */
-#ifdef BCMBUSTYPE
-	bustype = BCMBUSTYPE;
-#endif
-	if (bustype != SI_BUS) {
-		char *var;
-
-		var = getvar(vars, "vendid");
-		if (var) {
-			vendor = (u16) simple_strtoul(var, NULL, 0);
-			wiphy_err(wiphy, "Overriding vendor id = 0x%x\n",
-				  vendor);
-		}
-		var = getvar(vars, "devid");
-		if (var) {
-			u16 devid = (u16) simple_strtoul(var, NULL, 0);
-			if (devid != 0xffff) {
-				device = devid;
-				wiphy_err(wiphy, "Overriding device id = 0x%x"
-					  "\n", device);
-			}
-		}
-
-		/* verify again the device is supported */
-		if (!brcms_c_chipmatch(vendor, device)) {
-			wiphy_err(wiphy, "wl%d: brcms_b_attach: Unsupported "
-				"vendor/device (0x%x/0x%x)\n",
-				 unit, vendor, device);
-			err = 12;
-			goto fail;
-		}
-	}
-
-	wlc_hw->vendorid = vendor;
-	wlc_hw->deviceid = device;
-
-	/* set bar0 window to point at D11 core */
-	wlc_hw->regs = (d11regs_t *) ai_setcore(wlc_hw->sih, D11_CORE_ID, 0);
-	wlc_hw->corerev = ai_corerev(wlc_hw->sih);
-
-	regs = wlc_hw->regs;
-
-	wlc->regs = wlc_hw->regs;
-
-	/* validate chip, chiprev and corerev */
-	if (!brcms_c_isgoodchip(wlc_hw)) {
-		err = 13;
-		goto fail;
-	}
-
-	/* initialize power control registers */
-	ai_clkctl_init(wlc_hw->sih);
-
-	/* request fastclock and force fastclock for the rest of attach
-	 * bring the d11 core out of reset.
-	 *   For PMU chips, the first wlc_clkctl_clk is no-op since core-clk is still false;
-	 *   But it will be called again inside wlc_corereset, after d11 is out of reset.
-	 */
-	brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
-	brcms_b_corereset(wlc_hw, BRCMS_USE_COREFLAGS);
-
-	if (!brcms_b_validate_chip_access(wlc_hw)) {
-		wiphy_err(wiphy, "wl%d: brcms_b_attach: validate_chip_access "
-			"failed\n", unit);
-		err = 14;
-		goto fail;
-	}
-
-	/* get the board rev, used just below */
-	j = getintvar(vars, "boardrev");
-	/* promote srom boardrev of 0xFF to 1 */
-	if (j == BOARDREV_PROMOTABLE)
-		j = BOARDREV_PROMOTED;
-	wlc_hw->boardrev = (u16) j;
-	if (!brcms_c_validboardtype(wlc_hw)) {
-		wiphy_err(wiphy, "wl%d: brcms_b_attach: Unsupported Broadcom "
-			"board type (0x%x)" " or revision level (0x%x)\n",
-			 unit, wlc_hw->sih->boardtype, wlc_hw->boardrev);
-		err = 15;
-		goto fail;
-	}
-	wlc_hw->sromrev = (u8) getintvar(vars, "sromrev");
-	wlc_hw->boardflags = (u32) getintvar(vars, "boardflags");
-	wlc_hw->boardflags2 = (u32) getintvar(vars, "boardflags2");
-
-	if (wlc_hw->boardflags & BFL_NOPLLDOWN)
-		brcms_b_pllreq(wlc_hw, true, BRCMS_PLLREQ_SHARED);
-
-	if ((wlc_hw->sih->bustype == PCI_BUS)
-	    && (ai_pci_war16165(wlc_hw->sih)))
-		wlc->war16165 = true;
-
-	/* check device id(srom, nvram etc.) to set bands */
-	if (wlc_hw->deviceid == BCM43224_D11N_ID ||
-	    wlc_hw->deviceid == BCM43224_D11N_ID_VEN1) {
-		/* Dualband boards */
-		wlc_hw->_nbands = 2;
-	} else
-		wlc_hw->_nbands = 1;
-
-	if ((wlc_hw->sih->chip == BCM43225_CHIP_ID))
-		wlc_hw->_nbands = 1;
-
-	/* BMAC_NOTE: remove init of pub values when brcms_c_attach()
-	 * unconditionally does the init of these values
-	 */
-	wlc->vendorid = wlc_hw->vendorid;
-	wlc->deviceid = wlc_hw->deviceid;
-	wlc->pub->sih = wlc_hw->sih;
-	wlc->pub->corerev = wlc_hw->corerev;
-	wlc->pub->sromrev = wlc_hw->sromrev;
-	wlc->pub->boardrev = wlc_hw->boardrev;
-	wlc->pub->boardflags = wlc_hw->boardflags;
-	wlc->pub->boardflags2 = wlc_hw->boardflags2;
-	wlc->pub->_nbands = wlc_hw->_nbands;
-
-	wlc_hw->physhim = wlc_phy_shim_attach(wlc_hw, wlc->wl, wlc);
-
-	if (wlc_hw->physhim == NULL) {
-		wiphy_err(wiphy, "wl%d: brcms_b_attach: wlc_phy_shim_attach "
-			"failed\n", unit);
-		err = 25;
-		goto fail;
-	}
-
-	/* pass all the parameters to wlc_phy_shared_attach in one struct */
-	sha_params.sih = wlc_hw->sih;
-	sha_params.physhim = wlc_hw->physhim;
-	sha_params.unit = unit;
-	sha_params.corerev = wlc_hw->corerev;
-	sha_params.vars = vars;
-	sha_params.vid = wlc_hw->vendorid;
-	sha_params.did = wlc_hw->deviceid;
-	sha_params.chip = wlc_hw->sih->chip;
-	sha_params.chiprev = wlc_hw->sih->chiprev;
-	sha_params.chippkg = wlc_hw->sih->chippkg;
-	sha_params.sromrev = wlc_hw->sromrev;
-	sha_params.boardtype = wlc_hw->sih->boardtype;
-	sha_params.boardrev = wlc_hw->boardrev;
-	sha_params.boardvendor = wlc_hw->sih->boardvendor;
-	sha_params.boardflags = wlc_hw->boardflags;
-	sha_params.boardflags2 = wlc_hw->boardflags2;
-	sha_params.bustype = wlc_hw->sih->bustype;
-	sha_params.buscorerev = wlc_hw->sih->buscorerev;
-
-	/* alloc and save pointer to shared phy state area */
-	wlc_hw->phy_sh = wlc_phy_shared_attach(&sha_params);
-	if (!wlc_hw->phy_sh) {
-		err = 16;
-		goto fail;
-	}
-
-	/* initialize software state for each core and band */
-	for (j = 0; j < NBANDS_HW(wlc_hw); j++) {
-		/*
-		 * band0 is always 2.4Ghz
-		 * band1, if present, is 5Ghz
-		 */
-
-		/* So if this is a single band 11a card, use band 1 */
-		if (IS_SINGLEBAND_5G(wlc_hw->deviceid))
-			j = BAND_5G_INDEX;
-
-		brcms_c_setxband(wlc_hw, j);
-
-		wlc_hw->band->bandunit = j;
-		wlc_hw->band->bandtype = j ? BRCM_BAND_5G : BRCM_BAND_2G;
-		wlc->band->bandunit = j;
-		wlc->band->bandtype = j ? BRCM_BAND_5G : BRCM_BAND_2G;
-		wlc->core->coreidx = ai_coreidx(wlc_hw->sih);
-
-		wlc_hw->machwcap = R_REG(&regs->machwcap);
-		wlc_hw->machwcap_backup = wlc_hw->machwcap;
-
-		/* init tx fifo size */
-		wlc_hw->xmtfifo_sz =
-		    xmtfifo_sz[(wlc_hw->corerev - XMTFIFOTBL_STARTREV)];
-
-		/* Get a phy for this band */
-		wlc_hw->band->pi = wlc_phy_attach(wlc_hw->phy_sh,
-			(void *)regs, brcms_b_bandtype(wlc_hw), vars,
-			wlc->wiphy);
-		if (wlc_hw->band->pi == NULL) {
-			wiphy_err(wiphy, "wl%d: brcms_b_attach: wlc_phy_"
-				  "attach failed\n", unit);
-			err = 17;
-			goto fail;
-		}
-
-		wlc_phy_machwcap_set(wlc_hw->band->pi, wlc_hw->machwcap);
-
-		wlc_phy_get_phyversion(wlc_hw->band->pi, &wlc_hw->band->phytype,
-				       &wlc_hw->band->phyrev,
-				       &wlc_hw->band->radioid,
-				       &wlc_hw->band->radiorev);
-		wlc_hw->band->abgphy_encore =
-		    wlc_phy_get_encore(wlc_hw->band->pi);
-		wlc->band->abgphy_encore = wlc_phy_get_encore(wlc_hw->band->pi);
-		wlc_hw->band->core_flags =
-		    wlc_phy_get_coreflags(wlc_hw->band->pi);
-
-		/* verify good phy_type & supported phy revision */
-		if (BRCMS_ISNPHY(wlc_hw->band)) {
-			if (NCONF_HAS(wlc_hw->band->phyrev))
-				goto good_phy;
-			else
-				goto bad_phy;
-		} else if (BRCMS_ISLCNPHY(wlc_hw->band)) {
-			if (LCNCONF_HAS(wlc_hw->band->phyrev))
-				goto good_phy;
-			else
-				goto bad_phy;
-		} else {
- bad_phy:
-			wiphy_err(wiphy, "wl%d: brcms_b_attach: unsupported "
-				  "phy type/rev (%d/%d)\n", unit,
-				  wlc_hw->band->phytype, wlc_hw->band->phyrev);
-			err = 18;
-			goto fail;
-		}
-
- good_phy:
-		/* BMAC_NOTE: wlc->band->pi should not be set below and should be done in the
-		 * high level attach. However we can not make that change until all low level access
-		 * is changed to wlc_hw->band->pi. Instead do the wlc->band->pi init below, keeping
-		 * wlc_hw->band->pi as well for incremental update of low level fns, and cut over
-		 * low only init when all fns updated.
-		 */
-		wlc->band->pi = wlc_hw->band->pi;
-		wlc->band->phytype = wlc_hw->band->phytype;
-		wlc->band->phyrev = wlc_hw->band->phyrev;
-		wlc->band->radioid = wlc_hw->band->radioid;
-		wlc->band->radiorev = wlc_hw->band->radiorev;
-
-		/* default contention windows size limits */
-		wlc_hw->band->CWmin = APHY_CWMIN;
-		wlc_hw->band->CWmax = PHY_CWMAX;
-
-		if (!brcms_b_attach_dmapio(wlc, j, wme)) {
-			err = 19;
-			goto fail;
-		}
-	}
-
-	/* disable core to match driver "down" state */
-	brcms_c_coredisable(wlc_hw);
-
-	/* Match driver "down" state */
-	if (wlc_hw->sih->bustype == PCI_BUS)
-		ai_pci_down(wlc_hw->sih);
-
-	/* register sb interrupt callback functions */
-	ai_register_intr_callback(wlc_hw->sih, (void *)brcms_c_wlintrsoff,
-				  (void *)brcms_c_wlintrsrestore, NULL, wlc);
-
-	/* turn off pll and xtal to match driver "down" state */
-	brcms_b_xtal(wlc_hw, OFF);
-
-	/* *********************************************************************
-	 * The hardware is in the DOWN state at this point. D11 core
-	 * or cores are in reset with clocks off, and the board PLLs
-	 * are off if possible.
-	 *
-	 * Beyond this point, wlc->sbclk == false and chip registers
-	 * should not be touched.
-	 *********************************************************************
-	 */
-
-	/* init etheraddr state variables */
-	macaddr = brcms_c_get_macaddr(wlc_hw);
-	if (macaddr == NULL) {
-		wiphy_err(wiphy, "wl%d: brcms_b_attach: macaddr not found\n",
-			  unit);
-		err = 21;
-		goto fail;
-	}
-	brcmu_ether_atoe(macaddr, wlc_hw->etheraddr);
-	if (is_broadcast_ether_addr(wlc_hw->etheraddr) ||
-	    is_zero_ether_addr(wlc_hw->etheraddr)) {
-		wiphy_err(wiphy, "wl%d: brcms_b_attach: bad macaddr %s\n",
-			  unit, macaddr);
-		err = 22;
-		goto fail;
-	}
-
-	BCMMSG(wlc->wiphy,
-		 "deviceid 0x%x nbands %d board 0x%x macaddr: %s\n",
-		 wlc_hw->deviceid, wlc_hw->_nbands,
-		 wlc_hw->sih->boardtype, macaddr);
-
-	return err;
-
- fail:
-	wiphy_err(wiphy, "wl%d: brcms_b_attach: failed with err %d\n", unit,
-		  err);
-	return err;
-}
-
-/*
- * Initialize brcms_c_info default values ...
- * may get overrides later in this function
- *  BMAC_NOTES, move low out and resolve the dangling ones
- */
-static void brcms_b_info_init(struct brcms_hardware *wlc_hw)
-{
-	struct brcms_c_info *wlc = wlc_hw->wlc;
-
-	/* set default sw macintmask value */
-	wlc->defmacintmask = DEF_MACINTMASK;
-
-	/* various 802.11g modes */
-	wlc_hw->shortslot = false;
-
-	wlc_hw->SFBL = RETRY_SHORT_FB;
-	wlc_hw->LFBL = RETRY_LONG_FB;
-
-	/* default mac retry limits */
-	wlc_hw->SRL = RETRY_SHORT_DEF;
-	wlc_hw->LRL = RETRY_LONG_DEF;
-	wlc_hw->chanspec = CH20MHZ_CHSPEC(1);
-}
-
-/*
- * low level detach
- */
-int brcms_b_detach(struct brcms_c_info *wlc)
-{
-	uint i;
-	struct brcms_hw_band *band;
-	struct brcms_hardware *wlc_hw = wlc->hw;
-	int callbacks;
-
-	callbacks = 0;
-
-	if (wlc_hw->sih) {
-		/* detach interrupt sync mechanism since interrupt is disabled and per-port
-		 * interrupt object may has been freed. this must be done before sb core switch
-		 */
-		ai_deregister_intr_callback(wlc_hw->sih);
-
-		if (wlc_hw->sih->bustype == PCI_BUS)
-			ai_pci_sleep(wlc_hw->sih);
-	}
-
-	brcms_b_detach_dmapio(wlc_hw);
-
-	band = wlc_hw->band;
-	for (i = 0; i < NBANDS_HW(wlc_hw); i++) {
-		if (band->pi) {
-			/* Detach this band's phy */
-			wlc_phy_detach(band->pi);
-			band->pi = NULL;
-		}
-		band = wlc_hw->bandstate[OTHERBANDUNIT(wlc)];
-	}
-
-	/* Free shared phy state */
-	kfree(wlc_hw->phy_sh);
-
-	wlc_phy_shim_detach(wlc_hw->physhim);
-
-	/* free vars */
-	kfree(wlc_hw->vars);
-	wlc_hw->vars = NULL;
-
-	if (wlc_hw->sih) {
-		ai_detach(wlc_hw->sih);
-		wlc_hw->sih = NULL;
-	}
-
-	return callbacks;
-
-}
-
-void brcms_b_reset(struct brcms_hardware *wlc_hw)
-{
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	/* reset the core */
-	if (!DEVICEREMOVED(wlc_hw->wlc))
-		brcms_b_corereset(wlc_hw, BRCMS_USE_COREFLAGS);
-
-	/* purge the dma rings */
-	brcms_c_flushqueues(wlc_hw->wlc);
-
-	brcms_c_reset_bmac_done(wlc_hw->wlc);
-}
-
-void
-brcms_b_init(struct brcms_hardware *wlc_hw, chanspec_t chanspec,
-			  bool mute) {
-	u32 macintmask;
-	bool fastclk;
-	struct brcms_c_info *wlc = wlc_hw->wlc;
-
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	/* request FAST clock if not on */
-	fastclk = wlc_hw->forcefastclk;
-	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
-
-	/* disable interrupts */
-	macintmask = brcms_intrsoff(wlc->wl);
-
-	/* set up the specified band and chanspec */
-	brcms_c_setxband(wlc_hw, CHSPEC_BANDUNIT(chanspec));
-	wlc_phy_chanspec_radio_set(wlc_hw->band->pi, chanspec);
-
-	/* do one-time phy inits and calibration */
-	wlc_phy_cal_init(wlc_hw->band->pi);
-
-	/* core-specific initialization */
-	brcms_b_coreinit(wlc);
-
-	/* suspend the tx fifos and mute the phy for preism cac time */
-	if (mute)
-		brcms_b_mute(wlc_hw, ON, PHY_MUTE_FOR_PREISM);
-
-	/* band-specific inits */
-	brcms_b_bsinit(wlc, chanspec);
-
-	/* restore macintmask */
-	brcms_intrsrestore(wlc->wl, macintmask);
-
-	/* seed wake_override with BRCMS_WAKE_OVERRIDE_MACSUSPEND since the mac
-	 * is suspended and brcms_c_enable_mac() will clear this override bit.
-	 */
-	mboolset(wlc_hw->wake_override, BRCMS_WAKE_OVERRIDE_MACSUSPEND);
-
-	/*
-	 * initialize mac_suspend_depth to 1 to match ucode initial suspended state
-	 */
-	wlc_hw->mac_suspend_depth = 1;
-
-	/* restore the clk */
-	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
-}
-
-int brcms_b_up_prep(struct brcms_hardware *wlc_hw)
-{
-	uint coremask;
-
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	/*
-	 * Enable pll and xtal, initialize the power control registers,
-	 * and force fastclock for the remainder of brcms_c_up().
-	 */
-	brcms_b_xtal(wlc_hw, ON);
-	ai_clkctl_init(wlc_hw->sih);
-	brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
-
-	/*
-	 * Configure pci/pcmcia here instead of in brcms_c_attach()
-	 * to allow mfg hotswap:  down, hotswap (chip power cycle), up.
-	 */
-	coremask = (1 << wlc_hw->wlc->core->coreidx);
-
-	if (wlc_hw->sih->bustype == PCI_BUS)
-		ai_pci_setup(wlc_hw->sih, coremask);
-
-	/*
-	 * Need to read the hwradio status here to cover the case where the system
-	 * is loaded with the hw radio disabled. We do not want to bring the driver up in this case.
-	 */
-	if (brcms_b_radio_read_hwdisabled(wlc_hw)) {
-		/* put SB PCI in down state again */
-		if (wlc_hw->sih->bustype == PCI_BUS)
-			ai_pci_down(wlc_hw->sih);
-		brcms_b_xtal(wlc_hw, OFF);
-		return -ENOMEDIUM;
-	}
-
-	if (wlc_hw->sih->bustype == PCI_BUS)
-		ai_pci_up(wlc_hw->sih);
-
-	/* reset the d11 core */
-	brcms_b_corereset(wlc_hw, BRCMS_USE_COREFLAGS);
-
-	return 0;
-}
-
-int brcms_b_up_finish(struct brcms_hardware *wlc_hw)
-{
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	wlc_hw->up = true;
-	wlc_phy_hw_state_upd(wlc_hw->band->pi, true);
-
-	/* FULLY enable dynamic power control and d11 core interrupt */
-	brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
-	brcms_intrson(wlc_hw->wlc->wl);
-	return 0;
-}
-
-int brcms_b_bmac_down_prep(struct brcms_hardware *wlc_hw)
-{
-	bool dev_gone;
-	uint callbacks = 0;
-
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	if (!wlc_hw->up)
-		return callbacks;
-
-	dev_gone = DEVICEREMOVED(wlc_hw->wlc);
-
-	/* disable interrupts */
-	if (dev_gone)
-		wlc_hw->wlc->macintmask = 0;
-	else {
-		/* now disable interrupts */
-		brcms_intrsoff(wlc_hw->wlc->wl);
-
-		/* ensure we're running on the pll clock again */
-		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
-	}
-	/* down phy at the last of this stage */
-	callbacks += wlc_phy_down(wlc_hw->band->pi);
-
-	return callbacks;
-}
-
-int brcms_b_down_finish(struct brcms_hardware *wlc_hw)
-{
-	uint callbacks = 0;
-	bool dev_gone;
-
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	if (!wlc_hw->up)
-		return callbacks;
-
-	wlc_hw->up = false;
-	wlc_phy_hw_state_upd(wlc_hw->band->pi, false);
-
-	dev_gone = DEVICEREMOVED(wlc_hw->wlc);
-
-	if (dev_gone) {
-		wlc_hw->sbclk = false;
-		wlc_hw->clk = false;
-		wlc_phy_hw_clk_state_upd(wlc_hw->band->pi, false);
-
-		/* reclaim any posted packets */
-		brcms_c_flushqueues(wlc_hw->wlc);
-	} else {
-
-		/* Reset and disable the core */
-		if (ai_iscoreup(wlc_hw->sih)) {
-			if (R_REG(&wlc_hw->regs->maccontrol) &
-			    MCTL_EN_MAC)
-				brcms_c_suspend_mac_and_wait(wlc_hw->wlc);
-			callbacks += brcms_reset(wlc_hw->wlc->wl);
-			brcms_c_coredisable(wlc_hw);
-		}
-
-		/* turn off primary xtal and pll */
-		if (!wlc_hw->noreset) {
-			if (wlc_hw->sih->bustype == PCI_BUS)
-				ai_pci_down(wlc_hw->sih);
-			brcms_b_xtal(wlc_hw, OFF);
-		}
-	}
-
-	return callbacks;
-}
-
-void brcms_b_wait_for_wake(struct brcms_hardware *wlc_hw)
-{
-	/* delay before first read of ucode state */
-	udelay(40);
-
-	/* wait until ucode is no longer asleep */
-	SPINWAIT((brcms_b_read_shm(wlc_hw, M_UCODE_DBGST) ==
-		  DBGST_ASLEEP), wlc_hw->wlc->fastpwrup_dly);
-}
-
-void brcms_b_hw_etheraddr(struct brcms_hardware *wlc_hw, u8 *ea)
-{
-	memcpy(ea, wlc_hw->etheraddr, ETH_ALEN);
-}
-
-static int brcms_b_bandtype(struct brcms_hardware *wlc_hw)
-{
-	return wlc_hw->band->bandtype;
-}
-
-/* control chip clock to save power, enable dynamic clock or force fast clock */
-static void brcms_b_clkctl_clk(struct brcms_hardware *wlc_hw, uint mode)
-{
-	if (PMUCTL_ENAB(wlc_hw->sih)) {
-		/* new chips with PMU, CCS_FORCEHT will distribute the HT clock on backplane,
-		 *  but mac core will still run on ALP(not HT) when it enters powersave mode,
-		 *      which means the FCA bit may not be set.
-		 *      should wakeup mac if driver wants it to run on HT.
-		 */
-
-		if (wlc_hw->clk) {
-			if (mode == CLK_FAST) {
-				OR_REG(&wlc_hw->regs->clk_ctl_st,
-				       CCS_FORCEHT);
-
-				udelay(64);
-
-				SPINWAIT(((R_REG
-					   (&wlc_hw->regs->
-					    clk_ctl_st) & CCS_HTAVAIL) == 0),
-					 PMU_MAX_TRANSITION_DLY);
-				WARN_ON(!(R_REG
-					  (&wlc_hw->regs->
-					   clk_ctl_st) & CCS_HTAVAIL));
-			} else {
-				if ((wlc_hw->sih->pmurev == 0) &&
-				    (R_REG
-				     (&wlc_hw->regs->
-				      clk_ctl_st) & (CCS_FORCEHT | CCS_HTAREQ)))
-					SPINWAIT(((R_REG
-						   (&wlc_hw->regs->
-						    clk_ctl_st) & CCS_HTAVAIL)
-						  == 0),
-						 PMU_MAX_TRANSITION_DLY);
-				AND_REG(&wlc_hw->regs->clk_ctl_st,
-					~CCS_FORCEHT);
-			}
-		}
-		wlc_hw->forcefastclk = (mode == CLK_FAST);
-	} else {
-
-		/* old chips w/o PMU, force HT through cc,
-		 * then use FCA to verify mac is running fast clock
-		 */
-
-		wlc_hw->forcefastclk = ai_clkctl_cc(wlc_hw->sih, mode);
-
-		/* check fast clock is available (if core is not in reset) */
-		if (wlc_hw->forcefastclk && wlc_hw->clk)
-			WARN_ON(!(ai_core_sflags(wlc_hw->sih, 0, 0) &
-				  SISF_FCLKA));
-
-		/* keep the ucode wake bit on if forcefastclk is on
-		 * since we do not want ucode to put us back to slow clock
-		 * when it dozes for PM mode.
-		 * Code below matches the wake override bit with current forcefastclk state
-		 * Only setting bit in wake_override instead of waking ucode immediately
-		 * since old code (wlc.c 1.4499) had this behavior. Older code set
-		 * wlc->forcefastclk but only had the wake happen if the wakup_ucode work
-		 * (protected by an up check) was executed just below.
-		 */
-		if (wlc_hw->forcefastclk)
-			mboolset(wlc_hw->wake_override,
-				 BRCMS_WAKE_OVERRIDE_FORCEFAST);
-		else
-			mboolclr(wlc_hw->wake_override,
-				 BRCMS_WAKE_OVERRIDE_FORCEFAST);
-	}
-}
-
-/* set initial host flags value */
-static void
-brcms_c_mhfdef(struct brcms_c_info *wlc, u16 *mhfs, u16 mhf2_init)
-{
-	struct brcms_hardware *wlc_hw = wlc->hw;
-
-	memset(mhfs, 0, MHFMAX * sizeof(u16));
-
-	mhfs[MHF2] |= mhf2_init;
-
-	/* prohibit use of slowclock on multifunction boards */
-	if (wlc_hw->boardflags & BFL_NOPLLDOWN)
-		mhfs[MHF1] |= MHF1_FORCEFASTCLK;
-
-	if (BRCMS_ISNPHY(wlc_hw->band) && NREV_LT(wlc_hw->band->phyrev, 2)) {
-		mhfs[MHF2] |= MHF2_NPHY40MHZ_WAR;
-		mhfs[MHF1] |= MHF1_IQSWAP_WAR;
-	}
-}
-
-/* set or clear ucode host flag bits
- * it has an optimization for no-change write
- * it only writes through shared memory when the core has clock;
- * pre-CLK changes should use wlc_write_mhf to get around the optimization
- *
- *
- * bands values are: BRCM_BAND_AUTO <--- Current band only
- *                   BRCM_BAND_5G   <--- 5G band only
- *                   BRCM_BAND_2G   <--- 2G band only
- *                   BRCM_BAND_ALL  <--- All bands
- */
-void
-brcms_b_mhf(struct brcms_hardware *wlc_hw, u8 idx, u16 mask, u16 val,
-	     int bands)
-{
-	u16 save;
-	u16 addr[MHFMAX] = {
-		M_HOST_FLAGS1, M_HOST_FLAGS2, M_HOST_FLAGS3, M_HOST_FLAGS4,
-		M_HOST_FLAGS5
-	};
-	struct brcms_hw_band *band;
-
-	if ((val & ~mask) || idx >= MHFMAX)
-		return; /* error condition */
-
-	switch (bands) {
-		/* Current band only or all bands,
-		 * then set the band to current band
-		 */
-	case BRCM_BAND_AUTO:
-	case BRCM_BAND_ALL:
-		band = wlc_hw->band;
-		break;
-	case BRCM_BAND_5G:
-		band = wlc_hw->bandstate[BAND_5G_INDEX];
-		break;
-	case BRCM_BAND_2G:
-		band = wlc_hw->bandstate[BAND_2G_INDEX];
-		break;
-	default:
-		band = NULL;	/* error condition */
-	}
-
-	if (band) {
-		save = band->mhfs[idx];
-		band->mhfs[idx] = (band->mhfs[idx] & ~mask) | val;
-
-		/* optimization: only write through if changed, and
-		 * changed band is the current band
-		 */
-		if (wlc_hw->clk && (band->mhfs[idx] != save)
-		    && (band == wlc_hw->band))
-			brcms_b_write_shm(wlc_hw, addr[idx],
-					   (u16) band->mhfs[idx]);
-	}
-
-	if (bands == BRCM_BAND_ALL) {
-		wlc_hw->bandstate[0]->mhfs[idx] =
-		    (wlc_hw->bandstate[0]->mhfs[idx] & ~mask) | val;
-		wlc_hw->bandstate[1]->mhfs[idx] =
-		    (wlc_hw->bandstate[1]->mhfs[idx] & ~mask) | val;
-	}
-}
-
-u16 brcms_b_mhf_get(struct brcms_hardware *wlc_hw, u8 idx, int bands)
-{
-	struct brcms_hw_band *band;
-
-	if (idx >= MHFMAX)
-		return 0; /* error condition */
-	switch (bands) {
-	case BRCM_BAND_AUTO:
-		band = wlc_hw->band;
-		break;
-	case BRCM_BAND_5G:
-		band = wlc_hw->bandstate[BAND_5G_INDEX];
-		break;
-	case BRCM_BAND_2G:
-		band = wlc_hw->bandstate[BAND_2G_INDEX];
-		break;
-	default:
-		band = NULL;		/* error condition */
-	}
-
-	if (!band)
-		return 0;
-
-	return band->mhfs[idx];
-}
-
-static void brcms_c_write_mhf(struct brcms_hardware *wlc_hw, u16 *mhfs)
-{
-	u8 idx;
-	u16 addr[] = {
-		M_HOST_FLAGS1, M_HOST_FLAGS2, M_HOST_FLAGS3, M_HOST_FLAGS4,
-		M_HOST_FLAGS5
-	};
-
-	for (idx = 0; idx < MHFMAX; idx++) {
-		brcms_b_write_shm(wlc_hw, addr[idx], mhfs[idx]);
-	}
-}
-
-/* set the maccontrol register to desired reset state and
- * initialize the sw cache of the register
- */
-static void brcms_c_mctrl_reset(struct brcms_hardware *wlc_hw)
-{
-	/* IHR accesses are always enabled, PSM disabled, HPS off and WAKE on */
-	wlc_hw->maccontrol = 0;
-	wlc_hw->suspended_fifos = 0;
-	wlc_hw->wake_override = 0;
-	wlc_hw->mute_override = 0;
-	brcms_b_mctrl(wlc_hw, ~0, MCTL_IHR_EN | MCTL_WAKE);
-}
-
-/* set or clear maccontrol bits */
-void brcms_b_mctrl(struct brcms_hardware *wlc_hw, u32 mask, u32 val)
-{
-	u32 maccontrol;
-	u32 new_maccontrol;
-
-	if (val & ~mask)
-		return; /* error condition */
-	maccontrol = wlc_hw->maccontrol;
-	new_maccontrol = (maccontrol & ~mask) | val;
-
-	/* if the new maccontrol value is the same as the old, nothing to do */
-	if (new_maccontrol == maccontrol)
-		return;
-
-	/* something changed, cache the new value */
-	wlc_hw->maccontrol = new_maccontrol;
-
-	/* write the new values with overrides applied */
-	brcms_c_mctrl_write(wlc_hw);
-}
-
-/* write the software state of maccontrol and overrides to the maccontrol register */
-static void brcms_c_mctrl_write(struct brcms_hardware *wlc_hw)
-{
-	u32 maccontrol = wlc_hw->maccontrol;
-
-	/* OR in the wake bit if overridden */
-	if (wlc_hw->wake_override)
-		maccontrol |= MCTL_WAKE;
-
-	/* set AP and INFRA bits for mute if needed */
-	if (wlc_hw->mute_override) {
-		maccontrol &= ~(MCTL_AP);
-		maccontrol |= MCTL_INFRA;
-	}
-
-	W_REG(&wlc_hw->regs->maccontrol, maccontrol);
-}
-
-void brcms_c_ucode_wake_override_set(struct brcms_hardware *wlc_hw,
-				 u32 override_bit)
-{
-	if (wlc_hw->wake_override || (wlc_hw->maccontrol & MCTL_WAKE)) {
-		mboolset(wlc_hw->wake_override, override_bit);
-		return;
-	}
-
-	mboolset(wlc_hw->wake_override, override_bit);
-
-	brcms_c_mctrl_write(wlc_hw);
-	brcms_b_wait_for_wake(wlc_hw);
-
-	return;
-}
-
-void brcms_c_ucode_wake_override_clear(struct brcms_hardware *wlc_hw,
-				   u32 override_bit)
-{
-	mboolclr(wlc_hw->wake_override, override_bit);
-
-	if (wlc_hw->wake_override || (wlc_hw->maccontrol & MCTL_WAKE))
-		return;
-
-	brcms_c_mctrl_write(wlc_hw);
-
-	return;
-}
-
-/* When driver needs ucode to stop beaconing, it has to make sure that
- * MCTL_AP is clear and MCTL_INFRA is set
- * Mode           MCTL_AP        MCTL_INFRA
- * AP                1              1
- * STA               0              1 <--- This will ensure no beacons
- * IBSS              0              0
- */
-static void brcms_c_ucode_mute_override_set(struct brcms_hardware *wlc_hw)
-{
-	wlc_hw->mute_override = 1;
-
-	/* if maccontrol already has AP == 0 and INFRA == 1 without this
-	 * override, then there is no change to write
-	 */
-	if ((wlc_hw->maccontrol & (MCTL_AP | MCTL_INFRA)) == MCTL_INFRA)
-		return;
-
-	brcms_c_mctrl_write(wlc_hw);
-
-	return;
-}
-
-/* Clear the override on AP and INFRA bits */
-static void brcms_c_ucode_mute_override_clear(struct brcms_hardware *wlc_hw)
-{
-	if (wlc_hw->mute_override == 0)
-		return;
-
-	wlc_hw->mute_override = 0;
-
-	/* if maccontrol already has AP == 0 and INFRA == 1 without this
-	 * override, then there is no change to write
-	 */
-	if ((wlc_hw->maccontrol & (MCTL_AP | MCTL_INFRA)) == MCTL_INFRA)
-		return;
-
-	brcms_c_mctrl_write(wlc_hw);
-}
-
-/*
- * Write a MAC address to the given match reg offset in the RXE match engine.
- */
-void
-brcms_b_set_addrmatch(struct brcms_hardware *wlc_hw, int match_reg_offset,
-		       const u8 *addr)
-{
-	d11regs_t *regs;
-	u16 mac_l;
-	u16 mac_m;
-	u16 mac_h;
-
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d: brcms_b_set_addrmatch\n",
-		 wlc_hw->unit);
-
-	regs = wlc_hw->regs;
-	mac_l = addr[0] | (addr[1] << 8);
-	mac_m = addr[2] | (addr[3] << 8);
-	mac_h = addr[4] | (addr[5] << 8);
-
-	/* enter the MAC addr into the RXE match registers */
-	W_REG(&regs->rcm_ctl, RCM_INC_DATA | match_reg_offset);
-	W_REG(&regs->rcm_mat_data, mac_l);
-	W_REG(&regs->rcm_mat_data, mac_m);
-	W_REG(&regs->rcm_mat_data, mac_h);
-
-}
-
-void
-brcms_b_write_template_ram(struct brcms_hardware *wlc_hw, int offset, int len,
-			    void *buf)
-{
-	d11regs_t *regs;
-	u32 word;
-	bool be_bit;
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	regs = wlc_hw->regs;
-	W_REG(&regs->tplatewrptr, offset);
-
-	/* if MCTL_BIGEND bit set in mac control register,
-	 * the chip swaps data in fifo, as well as data in
-	 * template ram
-	 */
-	be_bit = (R_REG(&regs->maccontrol) & MCTL_BIGEND) != 0;
-
-	while (len > 0) {
-		memcpy(&word, buf, sizeof(u32));
-
-		if (be_bit)
-			word = cpu_to_be32(word);
-		else
-			word = cpu_to_le32(word);
-
-		W_REG(&regs->tplatewrdata, word);
-
-		buf = (u8 *) buf + sizeof(u32);
-		len -= sizeof(u32);
-	}
-}
-
-void brcms_b_set_cwmin(struct brcms_hardware *wlc_hw, u16 newmin)
-{
-	wlc_hw->band->CWmin = newmin;
-
-	W_REG(&wlc_hw->regs->objaddr, OBJADDR_SCR_SEL | S_DOT11_CWMIN);
-	(void)R_REG(&wlc_hw->regs->objaddr);
-	W_REG(&wlc_hw->regs->objdata, newmin);
-}
-
-void brcms_b_set_cwmax(struct brcms_hardware *wlc_hw, u16 newmax)
-{
-	wlc_hw->band->CWmax = newmax;
-
-	W_REG(&wlc_hw->regs->objaddr, OBJADDR_SCR_SEL | S_DOT11_CWMAX);
-	(void)R_REG(&wlc_hw->regs->objaddr);
-	W_REG(&wlc_hw->regs->objdata, newmax);
-}
-
-void brcms_b_bw_set(struct brcms_hardware *wlc_hw, u16 bw)
-{
-	bool fastclk;
-
-	/* request FAST clock if not on */
-	fastclk = wlc_hw->forcefastclk;
-	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
-
-	wlc_phy_bw_state_set(wlc_hw->band->pi, bw);
-
-	brcms_b_phy_reset(wlc_hw);
-	wlc_phy_init(wlc_hw->band->pi, wlc_phy_chanspec_get(wlc_hw->band->pi));
-
-	/* restore the clk */
-	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
-}
-
-static void
-brcms_c_write_hw_bcntemplate0(struct brcms_hardware *wlc_hw, void *bcn,
-			      int len)
-{
-	d11regs_t *regs = wlc_hw->regs;
-
-	brcms_b_write_template_ram(wlc_hw, T_BCN0_TPL_BASE, (len + 3) & ~3,
-				    bcn);
-	/* write beacon length to SCR */
-	brcms_b_write_shm(wlc_hw, M_BCN0_FRM_BYTESZ, (u16) len);
-	/* mark beacon0 valid */
-	OR_REG(&regs->maccommand, MCMD_BCN0VLD);
-}
-
-static void
-brcms_c_write_hw_bcntemplate1(struct brcms_hardware *wlc_hw, void *bcn,
-			      int len)
-{
-	d11regs_t *regs = wlc_hw->regs;
-
-	brcms_b_write_template_ram(wlc_hw, T_BCN1_TPL_BASE, (len + 3) & ~3,
-				    bcn);
-	/* write beacon length to SCR */
-	brcms_b_write_shm(wlc_hw, M_BCN1_FRM_BYTESZ, (u16) len);
-	/* mark beacon1 valid */
-	OR_REG(&regs->maccommand, MCMD_BCN1VLD);
-}
-
-/* mac is assumed to be suspended at this point */
-void
-brcms_b_write_hw_bcntemplates(struct brcms_hardware *wlc_hw, void *bcn,
-			      int len, bool both)
-{
-	d11regs_t *regs = wlc_hw->regs;
-
-	if (both) {
-		brcms_c_write_hw_bcntemplate0(wlc_hw, bcn, len);
-		brcms_c_write_hw_bcntemplate1(wlc_hw, bcn, len);
-	} else {
-		/* bcn 0 */
-		if (!(R_REG(&regs->maccommand) & MCMD_BCN0VLD))
-			brcms_c_write_hw_bcntemplate0(wlc_hw, bcn, len);
-		/* bcn 1 */
-		else if (!
-			 (R_REG(&regs->maccommand) & MCMD_BCN1VLD))
-			brcms_c_write_hw_bcntemplate1(wlc_hw, bcn, len);
-	}
-}
-
-static void brcms_b_upd_synthpu(struct brcms_hardware *wlc_hw)
-{
-	u16 v;
-	struct brcms_c_info *wlc = wlc_hw->wlc;
-	/* update SYNTHPU_DLY */
-
-	if (BRCMS_ISLCNPHY(wlc->band)) {
-		v = SYNTHPU_DLY_LPPHY_US;
-	} else if (BRCMS_ISNPHY(wlc->band) && (NREV_GE(wlc->band->phyrev, 3))) {
-		v = SYNTHPU_DLY_NPHY_US;
-	} else {
-		v = SYNTHPU_DLY_BPHY_US;
-	}
-
-	brcms_b_write_shm(wlc_hw, M_SYNTHPU_DLY, v);
-}
-
-/* band-specific init */
-static void
-brcms_b_bsinit(struct brcms_c_info *wlc, chanspec_t chanspec)
-{
-	struct brcms_hardware *wlc_hw = wlc->hw;
-
-	BCMMSG(wlc->wiphy, "wl%d: bandunit %d\n", wlc_hw->unit,
-		wlc_hw->band->bandunit);
-
-	brcms_c_ucode_bsinit(wlc_hw);
-
-	wlc_phy_init(wlc_hw->band->pi, chanspec);
-
-	brcms_c_ucode_txant_set(wlc_hw);
-
-	/* cwmin is band-specific, update hardware with value for current band */
-	brcms_b_set_cwmin(wlc_hw, wlc_hw->band->CWmin);
-	brcms_b_set_cwmax(wlc_hw, wlc_hw->band->CWmax);
-
-	brcms_b_update_slot_timing(wlc_hw,
-				    BAND_5G(wlc_hw->band->
-					    bandtype) ? true : wlc_hw->
-				    shortslot);
-
-	/* write phytype and phyvers */
-	brcms_b_write_shm(wlc_hw, M_PHYTYPE, (u16) wlc_hw->band->phytype);
-	brcms_b_write_shm(wlc_hw, M_PHYVER, (u16) wlc_hw->band->phyrev);
-
-	/* initialize the txphyctl1 rate table since shmem is shared between bands */
-	brcms_upd_ofdm_pctl1_table(wlc_hw);
-
-	brcms_b_upd_synthpu(wlc_hw);
-}
-
-static void brcms_b_core_phy_clk(struct brcms_hardware *wlc_hw, bool clk)
-{
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d: clk %d\n", wlc_hw->unit, clk);
-
-	wlc_hw->phyclk = clk;
-
-	if (OFF == clk) {	/* clear gmode bit, put phy into reset */
-
-		ai_core_cflags(wlc_hw->sih, (SICF_PRST | SICF_FGC | SICF_GMODE),
-			       (SICF_PRST | SICF_FGC));
-		udelay(1);
-		ai_core_cflags(wlc_hw->sih, (SICF_PRST | SICF_FGC), SICF_PRST);
-		udelay(1);
-
-	} else {		/* take phy out of reset */
-
-		ai_core_cflags(wlc_hw->sih, (SICF_PRST | SICF_FGC), SICF_FGC);
-		udelay(1);
-		ai_core_cflags(wlc_hw->sih, (SICF_FGC), 0);
-		udelay(1);
-
-	}
-}
-
-/* Perform a soft reset of the PHY PLL */
-void brcms_b_core_phypll_reset(struct brcms_hardware *wlc_hw)
-{
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	ai_corereg(wlc_hw->sih, SI_CC_IDX,
-		   offsetof(chipcregs_t, chipcontrol_addr), ~0, 0);
-	udelay(1);
-	ai_corereg(wlc_hw->sih, SI_CC_IDX,
-		   offsetof(chipcregs_t, chipcontrol_data), 0x4, 0);
-	udelay(1);
-	ai_corereg(wlc_hw->sih, SI_CC_IDX,
-		   offsetof(chipcregs_t, chipcontrol_data), 0x4, 4);
-	udelay(1);
-	ai_corereg(wlc_hw->sih, SI_CC_IDX,
-		   offsetof(chipcregs_t, chipcontrol_data), 0x4, 0);
-	udelay(1);
-}
-
-/* light way to turn on phy clock without reset for NPHY only
- *  refer to brcms_b_core_phy_clk for full version
- */
-void brcms_b_phyclk_fgc(struct brcms_hardware *wlc_hw, bool clk)
-{
-	/* support(necessary for NPHY and HYPHY) only */
-	if (!BRCMS_ISNPHY(wlc_hw->band))
-		return;
-
-	if (ON == clk)
-		ai_core_cflags(wlc_hw->sih, SICF_FGC, SICF_FGC);
-	else
-		ai_core_cflags(wlc_hw->sih, SICF_FGC, 0);
-
-}
-
-void brcms_b_macphyclk_set(struct brcms_hardware *wlc_hw, bool clk)
-{
-	if (ON == clk)
-		ai_core_cflags(wlc_hw->sih, SICF_MPCLKE, SICF_MPCLKE);
-	else
-		ai_core_cflags(wlc_hw->sih, SICF_MPCLKE, 0);
-}
-
-void brcms_b_phy_reset(struct brcms_hardware *wlc_hw)
-{
-	struct brcms_phy_pub *pih = wlc_hw->band->pi;
-	u32 phy_bw_clkbits;
-	bool phy_in_reset = false;
-
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	if (pih == NULL)
-		return;
-
-	phy_bw_clkbits = wlc_phy_clk_bwbits(wlc_hw->band->pi);
-
-	/* Specific reset sequence required for NPHY rev 3 and 4 */
-	if (BRCMS_ISNPHY(wlc_hw->band) && NREV_GE(wlc_hw->band->phyrev, 3) &&
-	    NREV_LE(wlc_hw->band->phyrev, 4)) {
-		/* Set the PHY bandwidth */
-		ai_core_cflags(wlc_hw->sih, SICF_BWMASK, phy_bw_clkbits);
-
-		udelay(1);
-
-		/* Perform a soft reset of the PHY PLL */
-		brcms_b_core_phypll_reset(wlc_hw);
-
-		/* reset the PHY */
-		ai_core_cflags(wlc_hw->sih, (SICF_PRST | SICF_PCLKE),
-			       (SICF_PRST | SICF_PCLKE));
-		phy_in_reset = true;
-	} else {
-
-		ai_core_cflags(wlc_hw->sih,
-			       (SICF_PRST | SICF_PCLKE | SICF_BWMASK),
-			       (SICF_PRST | SICF_PCLKE | phy_bw_clkbits));
-	}
-
-	udelay(2);
-	brcms_b_core_phy_clk(wlc_hw, ON);
-
-	if (pih)
-		wlc_phy_anacore(pih, ON);
-}
-
-/* switch to and initialize new band */
-static void
-brcms_b_setband(struct brcms_hardware *wlc_hw, uint bandunit,
-				chanspec_t chanspec) {
-	struct brcms_c_info *wlc = wlc_hw->wlc;
-	u32 macintmask;
-
-	/* Enable the d11 core before accessing it */
-	if (!ai_iscoreup(wlc_hw->sih)) {
-		ai_core_reset(wlc_hw->sih, 0, 0);
-		brcms_c_mctrl_reset(wlc_hw);
-	}
-
-	macintmask = brcms_c_setband_inact(wlc, bandunit);
-
-	if (!wlc_hw->up)
-		return;
-
-	brcms_b_core_phy_clk(wlc_hw, ON);
-
-	/* band-specific initializations */
-	brcms_b_bsinit(wlc, chanspec);
-
-	/*
-	 * If there are any pending software interrupt bits,
-	 * then replace these with a harmless nonzero value
-	 * so brcms_c_dpc() will re-enable interrupts when done.
-	 */
-	if (wlc->macintstatus)
-		wlc->macintstatus = MI_DMAINT;
-
-	/* restore macintmask */
-	brcms_intrsrestore(wlc->wl, macintmask);
-
-	/* ucode should still be suspended.. */
-	WARN_ON((R_REG(&wlc_hw->regs->maccontrol) & MCTL_EN_MAC) != 0);
-}
-
-/* low-level band switch utility routine */
-void brcms_c_setxband(struct brcms_hardware *wlc_hw,
-				     uint bandunit)
-{
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d: bandunit %d\n", wlc_hw->unit,
-		bandunit);
-
-	wlc_hw->band = wlc_hw->bandstate[bandunit];
-
-	/* BMAC_NOTE: until we eliminate need for wlc->band refs in low level code */
-	wlc_hw->wlc->band = wlc_hw->wlc->bandstate[bandunit];
-
-	/* set gmode core flag */
-	if (wlc_hw->sbclk && !wlc_hw->noreset) {
-		ai_core_cflags(wlc_hw->sih, SICF_GMODE,
-			       ((bandunit == 0) ? SICF_GMODE : 0));
-	}
-}
-
-static bool brcms_c_isgoodchip(struct brcms_hardware *wlc_hw)
-{
-
-	/* reject unsupported corerev */
-	if (!VALID_COREREV(wlc_hw->corerev)) {
-		wiphy_err(wlc_hw->wlc->wiphy, "unsupported core rev %d\n",
-			  wlc_hw->corerev);
-		return false;
-	}
-
-	return true;
-}
-
-/* Validate some board info parameters */
-static bool brcms_c_validboardtype(struct brcms_hardware *wlc_hw)
-{
-	uint boardrev = wlc_hw->boardrev;
-
-	/* 4 bits each for board type, major, minor, and tiny version */
-	uint brt = (boardrev & 0xf000) >> 12;
-	uint b0 = (boardrev & 0xf00) >> 8;
-	uint b1 = (boardrev & 0xf0) >> 4;
-	uint b2 = boardrev & 0xf;
-
-	/* voards from other vendors are always considered valid */
-	if (wlc_hw->sih->boardvendor != PCI_VENDOR_ID_BROADCOM)
-		return true;
-
-	/* do some boardrev sanity checks when boardvendor is Broadcom */
-	if (boardrev == 0)
-		return false;
-
-	if (boardrev <= 0xff)
-		return true;
-
-	if ((brt > 2) || (brt == 0) || (b0 > 9) || (b0 == 0) || (b1 > 9)
-		|| (b2 > 9))
-		return false;
-
-	return true;
-}
-
-static char *brcms_c_get_macaddr(struct brcms_hardware *wlc_hw)
-{
-	const char *varname = "macaddr";
-	char *macaddr;
-
-	/* If macaddr exists, use it (Sromrev4, CIS, ...). */
-	macaddr = getvar(wlc_hw->vars, varname);
-	if (macaddr != NULL)
-		return macaddr;
-
-	if (NBANDS_HW(wlc_hw) > 1)
-		varname = "et1macaddr";
-	else
-		varname = "il0macaddr";
-
-	macaddr = getvar(wlc_hw->vars, varname);
-	if (macaddr == NULL) {
-		wiphy_err(wlc_hw->wlc->wiphy, "wl%d: wlc_get_macaddr: macaddr "
-			  "getvar(%s) not found\n", wlc_hw->unit, varname);
-	}
-
-	return macaddr;
-}
-
-/*
- * Return true if radio is disabled, otherwise false.
- * hw radio disable signal is an external pin, users activate it asynchronously
- * this function could be called when driver is down and w/o clock
- * it operates on different registers depending on corerev and boardflag.
- */
-bool brcms_b_radio_read_hwdisabled(struct brcms_hardware *wlc_hw)
-{
-	bool v, clk, xtal;
-	u32 resetbits = 0, flags = 0;
-
-	xtal = wlc_hw->sbclk;
-	if (!xtal)
-		brcms_b_xtal(wlc_hw, ON);
-
-	/* may need to take core out of reset first */
-	clk = wlc_hw->clk;
-	if (!clk) {
-		/*
-		 * mac no longer enables phyclk automatically when driver
-		 * accesses phyreg throughput mac. This can be skipped since
-		 * only mac reg is accessed below
-		 */
-		flags |= SICF_PCLKE;
-
-		/* AI chip doesn't restore bar0win2 on hibernation/resume, need sw fixup */
-		if ((wlc_hw->sih->chip == BCM43224_CHIP_ID) ||
-		    (wlc_hw->sih->chip == BCM43225_CHIP_ID))
-			wlc_hw->regs =
-			    (d11regs_t *) ai_setcore(wlc_hw->sih, D11_CORE_ID,
-						     0);
-		ai_core_reset(wlc_hw->sih, flags, resetbits);
-		brcms_c_mctrl_reset(wlc_hw);
-	}
-
-	v = ((R_REG(&wlc_hw->regs->phydebug) & PDBG_RFD) != 0);
-
-	/* put core back into reset */
-	if (!clk)
-		ai_core_disable(wlc_hw->sih, 0);
-
-	if (!xtal)
-		brcms_b_xtal(wlc_hw, OFF);
-
-	return v;
-}
-
-/* Initialize just the hardware when coming out of POR or S3/S5 system states */
-void brcms_b_hw_up(struct brcms_hardware *wlc_hw)
-{
-	if (wlc_hw->wlc->pub->hw_up)
-		return;
-
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	/*
-	 * Enable pll and xtal, initialize the power control registers,
-	 * and force fastclock for the remainder of brcms_c_up().
-	 */
-	brcms_b_xtal(wlc_hw, ON);
-	ai_clkctl_init(wlc_hw->sih);
-	brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
-
-	if (wlc_hw->sih->bustype == PCI_BUS) {
-		ai_pci_fixcfg(wlc_hw->sih);
-
-		/* AI chip doesn't restore bar0win2 on hibernation/resume, need sw fixup */
-		if ((wlc_hw->sih->chip == BCM43224_CHIP_ID) ||
-		    (wlc_hw->sih->chip == BCM43225_CHIP_ID))
-			wlc_hw->regs =
-			    (d11regs_t *) ai_setcore(wlc_hw->sih, D11_CORE_ID,
-						     0);
-	}
-
-	/* Inform phy that a POR reset has occurred so it does a complete phy init */
-	wlc_phy_por_inform(wlc_hw->band->pi);
-
-	wlc_hw->ucode_loaded = false;
-	wlc_hw->wlc->pub->hw_up = true;
-
-	if ((wlc_hw->boardflags & BFL_FEM)
-	    && (wlc_hw->sih->chip == BCM4313_CHIP_ID)) {
-		if (!
-		    (wlc_hw->boardrev >= 0x1250
-		     && (wlc_hw->boardflags & BFL_FEM_BT)))
-			ai_epa_4313war(wlc_hw->sih);
-	}
-}
-
-static bool wlc_dma_rxreset(struct brcms_hardware *wlc_hw, uint fifo)
-{
-	struct dma_pub *di = wlc_hw->di[fifo];
-	return dma_rxreset(di);
-}
-
-/* d11 core reset
- *   ensure fask clock during reset
- *   reset dma
- *   reset d11(out of reset)
- *   reset phy(out of reset)
- *   clear software macintstatus for fresh new start
- * one testing hack wlc_hw->noreset will bypass the d11/phy reset
- */
-void brcms_b_corereset(struct brcms_hardware *wlc_hw, u32 flags)
-{
-	d11regs_t *regs;
-	uint i;
-	bool fastclk;
-	u32 resetbits = 0;
-
-	if (flags == BRCMS_USE_COREFLAGS)
-		flags = (wlc_hw->band->pi ? wlc_hw->band->core_flags : 0);
-
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	regs = wlc_hw->regs;
-
-	/* request FAST clock if not on  */
-	fastclk = wlc_hw->forcefastclk;
-	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
-
-	/* reset the dma engines except first time thru */
-	if (ai_iscoreup(wlc_hw->sih)) {
-		for (i = 0; i < NFIFO; i++)
-			if ((wlc_hw->di[i]) && (!dma_txreset(wlc_hw->di[i]))) {
-				wiphy_err(wlc_hw->wlc->wiphy, "wl%d: %s: "
-					  "dma_txreset[%d]: cannot stop dma\n",
-					   wlc_hw->unit, __func__, i);
-			}
-
-		if ((wlc_hw->di[RX_FIFO])
-		    && (!wlc_dma_rxreset(wlc_hw, RX_FIFO))) {
-			wiphy_err(wlc_hw->wlc->wiphy, "wl%d: %s: dma_rxreset"
-				  "[%d]: cannot stop dma\n",
-				  wlc_hw->unit, __func__, RX_FIFO);
-		}
-	}
-	/* if noreset, just stop the psm and return */
-	if (wlc_hw->noreset) {
-		wlc_hw->wlc->macintstatus = 0;	/* skip wl_dpc after down */
-		brcms_b_mctrl(wlc_hw, MCTL_PSM_RUN | MCTL_EN_MAC, 0);
-		return;
-	}
-
-	/*
-	 * mac no longer enables phyclk automatically when driver accesses
-	 * phyreg throughput mac, AND phy_reset is skipped at early stage when
-	 * band->pi is invalid. need to enable PHY CLK
-	 */
-	flags |= SICF_PCLKE;
-
-	/* reset the core
-	 * In chips with PMU, the fastclk request goes through d11 core reg 0x1e0, which
-	 *  is cleared by the core_reset. have to re-request it.
-	 *  This adds some delay and we can optimize it by also requesting fastclk through
-	 *  chipcommon during this period if necessary. But that has to work coordinate
-	 *  with other driver like mips/arm since they may touch chipcommon as well.
-	 */
-	wlc_hw->clk = false;
-	ai_core_reset(wlc_hw->sih, flags, resetbits);
-	wlc_hw->clk = true;
-	if (wlc_hw->band && wlc_hw->band->pi)
-		wlc_phy_hw_clk_state_upd(wlc_hw->band->pi, true);
-
-	brcms_c_mctrl_reset(wlc_hw);
-
-	if (PMUCTL_ENAB(wlc_hw->sih))
-		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
-
-	brcms_b_phy_reset(wlc_hw);
-
-	/* turn on PHY_PLL */
-	brcms_b_core_phypll_ctl(wlc_hw, true);
-
-	/* clear sw intstatus */
-	wlc_hw->wlc->macintstatus = 0;
-
-	/* restore the clk setting */
-	if (!fastclk)
-		brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
-}
-
-/* txfifo sizes needs to be modified(increased) since the newer cores
- * have more memory.
- */
-static void brcms_b_corerev_fifofixup(struct brcms_hardware *wlc_hw)
-{
-	d11regs_t *regs = wlc_hw->regs;
-	u16 fifo_nu;
-	u16 txfifo_startblk = TXFIFO_START_BLK, txfifo_endblk;
-	u16 txfifo_def, txfifo_def1;
-	u16 txfifo_cmd;
-
-	/* tx fifos start at TXFIFO_START_BLK from the Base address */
-	txfifo_startblk = TXFIFO_START_BLK;
-
-	/* sequence of operations:  reset fifo, set fifo size, reset fifo */
-	for (fifo_nu = 0; fifo_nu < NFIFO; fifo_nu++) {
-
-		txfifo_endblk = txfifo_startblk + wlc_hw->xmtfifo_sz[fifo_nu];
-		txfifo_def = (txfifo_startblk & 0xff) |
-		    (((txfifo_endblk - 1) & 0xff) << TXFIFO_FIFOTOP_SHIFT);
-		txfifo_def1 = ((txfifo_startblk >> 8) & 0x1) |
-		    ((((txfifo_endblk -
-			1) >> 8) & 0x1) << TXFIFO_FIFOTOP_SHIFT);
-		txfifo_cmd =
-		    TXFIFOCMD_RESET_MASK | (fifo_nu << TXFIFOCMD_FIFOSEL_SHIFT);
-
-		W_REG(&regs->xmtfifocmd, txfifo_cmd);
-		W_REG(&regs->xmtfifodef, txfifo_def);
-		W_REG(&regs->xmtfifodef1, txfifo_def1);
-
-		W_REG(&regs->xmtfifocmd, txfifo_cmd);
-
-		txfifo_startblk += wlc_hw->xmtfifo_sz[fifo_nu];
-	}
-	/*
-	 * need to propagate to shm location to be in sync since ucode/hw won't
-	 * do this
-	 */
-	brcms_b_write_shm(wlc_hw, M_FIFOSIZE0,
-			   wlc_hw->xmtfifo_sz[TX_AC_BE_FIFO]);
-	brcms_b_write_shm(wlc_hw, M_FIFOSIZE1,
-			   wlc_hw->xmtfifo_sz[TX_AC_VI_FIFO]);
-	brcms_b_write_shm(wlc_hw, M_FIFOSIZE2,
-			   ((wlc_hw->xmtfifo_sz[TX_AC_VO_FIFO] << 8) | wlc_hw->
-			    xmtfifo_sz[TX_AC_BK_FIFO]));
-	brcms_b_write_shm(wlc_hw, M_FIFOSIZE3,
-			   ((wlc_hw->xmtfifo_sz[TX_ATIM_FIFO] << 8) | wlc_hw->
-			    xmtfifo_sz[TX_BCMC_FIFO]));
-}
-
-/* d11 core init
- *   reset PSM
- *   download ucode/PCM
- *   let ucode run to suspended
- *   download ucode inits
- *   config other core registers
- *   init dma
- */
-static void brcms_b_coreinit(struct brcms_c_info *wlc)
-{
-	struct brcms_hardware *wlc_hw = wlc->hw;
-	d11regs_t *regs;
-	u32 sflags;
-	uint bcnint_us;
-	uint i = 0;
-	bool fifosz_fixup = false;
-	int err = 0;
-	u16 buf[NFIFO];
-	struct wiphy *wiphy = wlc->wiphy;
-
-	regs = wlc_hw->regs;
-
-	BCMMSG(wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	/* reset PSM */
-	brcms_b_mctrl(wlc_hw, ~0, (MCTL_IHR_EN | MCTL_PSM_JMP_0 | MCTL_WAKE));
-
-	brcms_ucode_download(wlc_hw);
-	/*
-	 * FIFOSZ fixup. driver wants to controls the fifo allocation.
-	 */
-	fifosz_fixup = true;
-
-	/* let the PSM run to the suspended state, set mode to BSS STA */
-	W_REG(&regs->macintstatus, -1);
-	brcms_b_mctrl(wlc_hw, ~0,
-		       (MCTL_IHR_EN | MCTL_INFRA | MCTL_PSM_RUN | MCTL_WAKE));
-
-	/* wait for ucode to self-suspend after auto-init */
-	SPINWAIT(((R_REG(&regs->macintstatus) & MI_MACSSPNDD) == 0),
-		 1000 * 1000);
-	if ((R_REG(&regs->macintstatus) & MI_MACSSPNDD) == 0)
-		wiphy_err(wiphy, "wl%d: wlc_coreinit: ucode did not self-"
-			  "suspend!\n", wlc_hw->unit);
-
-	brcms_c_gpio_init(wlc);
-
-	sflags = ai_core_sflags(wlc_hw->sih, 0, 0);
-
-	if (D11REV_IS(wlc_hw->corerev, 23)) {
-		if (BRCMS_ISNPHY(wlc_hw->band))
-			brcms_c_write_inits(wlc_hw, d11n0initvals16);
-		else
-			wiphy_err(wiphy, "%s: wl%d: unsupported phy in corerev"
-				  " %d\n", __func__, wlc_hw->unit,
-				  wlc_hw->corerev);
-	} else if (D11REV_IS(wlc_hw->corerev, 24)) {
-		if (BRCMS_ISLCNPHY(wlc_hw->band)) {
-			brcms_c_write_inits(wlc_hw, d11lcn0initvals24);
-		} else {
-			wiphy_err(wiphy, "%s: wl%d: unsupported phy in corerev"
-				  " %d\n", __func__, wlc_hw->unit,
-				  wlc_hw->corerev);
-		}
-	} else {
-		wiphy_err(wiphy, "%s: wl%d: unsupported corerev %d\n",
-			  __func__, wlc_hw->unit, wlc_hw->corerev);
-	}
-
-	/* For old ucode, txfifo sizes needs to be modified(increased) */
-	if (fifosz_fixup == true) {
-		brcms_b_corerev_fifofixup(wlc_hw);
-	}
-
-	/* check txfifo allocations match between ucode and driver */
-	buf[TX_AC_BE_FIFO] = brcms_b_read_shm(wlc_hw, M_FIFOSIZE0);
-	if (buf[TX_AC_BE_FIFO] != wlc_hw->xmtfifo_sz[TX_AC_BE_FIFO]) {
-		i = TX_AC_BE_FIFO;
-		err = -1;
-	}
-	buf[TX_AC_VI_FIFO] = brcms_b_read_shm(wlc_hw, M_FIFOSIZE1);
-	if (buf[TX_AC_VI_FIFO] != wlc_hw->xmtfifo_sz[TX_AC_VI_FIFO]) {
-		i = TX_AC_VI_FIFO;
-		err = -1;
-	}
-	buf[TX_AC_BK_FIFO] = brcms_b_read_shm(wlc_hw, M_FIFOSIZE2);
-	buf[TX_AC_VO_FIFO] = (buf[TX_AC_BK_FIFO] >> 8) & 0xff;
-	buf[TX_AC_BK_FIFO] &= 0xff;
-	if (buf[TX_AC_BK_FIFO] != wlc_hw->xmtfifo_sz[TX_AC_BK_FIFO]) {
-		i = TX_AC_BK_FIFO;
-		err = -1;
-	}
-	if (buf[TX_AC_VO_FIFO] != wlc_hw->xmtfifo_sz[TX_AC_VO_FIFO]) {
-		i = TX_AC_VO_FIFO;
-		err = -1;
-	}
-	buf[TX_BCMC_FIFO] = brcms_b_read_shm(wlc_hw, M_FIFOSIZE3);
-	buf[TX_ATIM_FIFO] = (buf[TX_BCMC_FIFO] >> 8) & 0xff;
-	buf[TX_BCMC_FIFO] &= 0xff;
-	if (buf[TX_BCMC_FIFO] != wlc_hw->xmtfifo_sz[TX_BCMC_FIFO]) {
-		i = TX_BCMC_FIFO;
-		err = -1;
-	}
-	if (buf[TX_ATIM_FIFO] != wlc_hw->xmtfifo_sz[TX_ATIM_FIFO]) {
-		i = TX_ATIM_FIFO;
-		err = -1;
-	}
-	if (err != 0) {
-		wiphy_err(wiphy, "wlc_coreinit: txfifo mismatch: ucode size %d"
-			  " driver size %d index %d\n", buf[i],
-			  wlc_hw->xmtfifo_sz[i], i);
-	}
-
-	/* make sure we can still talk to the mac */
-	WARN_ON(R_REG(&regs->maccontrol) == 0xffffffff);
-
-	/* band-specific inits done by wlc_bsinit() */
-
-	/* Set up frame burst size and antenna swap threshold init values */
-	brcms_b_write_shm(wlc_hw, M_MBURST_SIZE, MAXTXFRAMEBURST);
-	brcms_b_write_shm(wlc_hw, M_MAX_ANTCNT, ANTCNT);
-
-	/* enable one rx interrupt per received frame */
-	W_REG(&regs->intrcvlazy[0], (1 << IRL_FC_SHIFT));
-
-	/* set the station mode (BSS STA) */
-	brcms_b_mctrl(wlc_hw,
-		       (MCTL_INFRA | MCTL_DISCARD_PMQ | MCTL_AP),
-		       (MCTL_INFRA | MCTL_DISCARD_PMQ));
-
-	/* set up Beacon interval */
-	bcnint_us = 0x8000 << 10;
-	W_REG(&regs->tsf_cfprep, (bcnint_us << CFPREP_CBI_SHIFT));
-	W_REG(&regs->tsf_cfpstart, bcnint_us);
-	W_REG(&regs->macintstatus, MI_GP1);
-
-	/* write interrupt mask */
-	W_REG(&regs->intctrlregs[RX_FIFO].intmask, DEF_RXINTMASK);
-
-	/* allow the MAC to control the PHY clock (dynamic on/off) */
-	brcms_b_macphyclk_set(wlc_hw, ON);
-
-	/* program dynamic clock control fast powerup delay register */
-	wlc->fastpwrup_dly = ai_clkctl_fast_pwrup_delay(wlc_hw->sih);
-	W_REG(&regs->scc_fastpwrup_dly, wlc->fastpwrup_dly);
-
-	/* tell the ucode the corerev */
-	brcms_b_write_shm(wlc_hw, M_MACHW_VER, (u16) wlc_hw->corerev);
-
-	/* tell the ucode MAC capabilities */
-	brcms_b_write_shm(wlc_hw, M_MACHW_CAP_L,
-			   (u16) (wlc_hw->machwcap & 0xffff));
-	brcms_b_write_shm(wlc_hw, M_MACHW_CAP_H,
-			   (u16) ((wlc_hw->
-				      machwcap >> 16) & 0xffff));
-
-	/* write retry limits to SCR, this done after PSM init */
-	W_REG(&regs->objaddr, OBJADDR_SCR_SEL | S_DOT11_SRC_LMT);
-	(void)R_REG(&regs->objaddr);
-	W_REG(&regs->objdata, wlc_hw->SRL);
-	W_REG(&regs->objaddr, OBJADDR_SCR_SEL | S_DOT11_LRC_LMT);
-	(void)R_REG(&regs->objaddr);
-	W_REG(&regs->objdata, wlc_hw->LRL);
-
-	/* write rate fallback retry limits */
-	brcms_b_write_shm(wlc_hw, M_SFRMTXCNTFBRTHSD, wlc_hw->SFBL);
-	brcms_b_write_shm(wlc_hw, M_LFRMTXCNTFBRTHSD, wlc_hw->LFBL);
-
-	AND_REG(&regs->ifs_ctl, 0x0FFF);
-	W_REG(&regs->ifs_aifsn, EDCF_AIFSN_MIN);
-
-	/* dma initializations */
-	wlc->txpend16165war = 0;
-
-	/* init the tx dma engines */
-	for (i = 0; i < NFIFO; i++) {
-		if (wlc_hw->di[i])
-			dma_txinit(wlc_hw->di[i]);
-	}
-
-	/* init the rx dma engine(s) and post receive buffers */
-	dma_rxinit(wlc_hw->di[RX_FIFO]);
-	dma_rxfill(wlc_hw->di[RX_FIFO]);
-}
-
-/* This function is used for changing the tsf frac register
- * If spur avoidance mode is off, the mac freq will be 80/120/160Mhz
- * If spur avoidance mode is on1, the mac freq will be 82/123/164Mhz
- * If spur avoidance mode is on2, the mac freq will be 84/126/168Mhz
- * HTPHY Formula is 2^26/freq(MHz) e.g.
- * For spuron2 - 126MHz -> 2^26/126 = 532610.0
- *  - 532610 = 0x82082 => tsf_clk_frac_h = 0x8, tsf_clk_frac_l = 0x2082
- * For spuron: 123MHz -> 2^26/123    = 545600.5
- *  - 545601 = 0x85341 => tsf_clk_frac_h = 0x8, tsf_clk_frac_l = 0x5341
- * For spur off: 120MHz -> 2^26/120    = 559240.5
- *  - 559241 = 0x88889 => tsf_clk_frac_h = 0x8, tsf_clk_frac_l = 0x8889
- */
-
-void brcms_b_switch_macfreq(struct brcms_hardware *wlc_hw, u8 spurmode)
-{
-	d11regs_t *regs;
-	regs = wlc_hw->regs;
-
-	if ((wlc_hw->sih->chip == BCM43224_CHIP_ID) ||
-	    (wlc_hw->sih->chip == BCM43225_CHIP_ID)) {
-		if (spurmode == WL_SPURAVOID_ON2) {	/* 126Mhz */
-			W_REG(&regs->tsf_clk_frac_l, 0x2082);
-			W_REG(&regs->tsf_clk_frac_h, 0x8);
-		} else if (spurmode == WL_SPURAVOID_ON1) {	/* 123Mhz */
-			W_REG(&regs->tsf_clk_frac_l, 0x5341);
-			W_REG(&regs->tsf_clk_frac_h, 0x8);
-		} else {	/* 120Mhz */
-			W_REG(&regs->tsf_clk_frac_l, 0x8889);
-			W_REG(&regs->tsf_clk_frac_h, 0x8);
-		}
-	} else if (BRCMS_ISLCNPHY(wlc_hw->band)) {
-		if (spurmode == WL_SPURAVOID_ON1) {	/* 82Mhz */
-			W_REG(&regs->tsf_clk_frac_l, 0x7CE0);
-			W_REG(&regs->tsf_clk_frac_h, 0xC);
-		} else {	/* 80Mhz */
-			W_REG(&regs->tsf_clk_frac_l, 0xCCCD);
-			W_REG(&regs->tsf_clk_frac_h, 0xC);
-		}
-	}
-}
-
-/* Initialize GPIOs that are controlled by D11 core */
-static void brcms_c_gpio_init(struct brcms_c_info *wlc)
-{
-	struct brcms_hardware *wlc_hw = wlc->hw;
-	d11regs_t *regs;
-	u32 gc, gm;
-
-	regs = wlc_hw->regs;
-
-	/* use GPIO select 0 to get all gpio signals from the gpio out reg */
-	brcms_b_mctrl(wlc_hw, MCTL_GPOUT_SEL_MASK, 0);
-
-	/*
-	 * Common GPIO setup:
-	 *      G0 = LED 0 = WLAN Activity
-	 *      G1 = LED 1 = WLAN 2.4 GHz Radio State
-	 *      G2 = LED 2 = WLAN 5 GHz Radio State
-	 *      G4 = radio disable input (HI enabled, LO disabled)
-	 */
-
-	gc = gm = 0;
-
-	/* Allocate GPIOs for mimo antenna diversity feature */
-	if (wlc_hw->antsel_type == ANTSEL_2x3) {
-		/* Enable antenna diversity, use 2x3 mode */
-		brcms_b_mhf(wlc_hw, MHF3, MHF3_ANTSEL_EN,
-			     MHF3_ANTSEL_EN, BRCM_BAND_ALL);
-		brcms_b_mhf(wlc_hw, MHF3, MHF3_ANTSEL_MODE,
-			     MHF3_ANTSEL_MODE, BRCM_BAND_ALL);
-
-		/* init superswitch control */
-		wlc_phy_antsel_init(wlc_hw->band->pi, false);
-
-	} else if (wlc_hw->antsel_type == ANTSEL_2x4) {
-		gm |= gc |= (BOARD_GPIO_12 | BOARD_GPIO_13);
-		/*
-		 * The board itself is powered by these GPIOs
-		 * (when not sending pattern) so set them high
-		 */
-		OR_REG(&regs->psm_gpio_oe,
-		       (BOARD_GPIO_12 | BOARD_GPIO_13));
-		OR_REG(&regs->psm_gpio_out,
-		       (BOARD_GPIO_12 | BOARD_GPIO_13));
-
-		/* Enable antenna diversity, use 2x4 mode */
-		brcms_b_mhf(wlc_hw, MHF3, MHF3_ANTSEL_EN,
-			     MHF3_ANTSEL_EN, BRCM_BAND_ALL);
-		brcms_b_mhf(wlc_hw, MHF3, MHF3_ANTSEL_MODE, 0,
-			     BRCM_BAND_ALL);
-
-		/* Configure the desired clock to be 4Mhz */
-		brcms_b_write_shm(wlc_hw, M_ANTSEL_CLKDIV,
-				   ANTSEL_CLKDIV_4MHZ);
-	}
-
-	/* gpio 9 controls the PA.  ucode is responsible for wiggling out and oe */
-	if (wlc_hw->boardflags & BFL_PACTRL)
-		gm |= gc |= BOARD_GPIO_PACTRL;
-
-	/* apply to gpiocontrol register */
-	ai_gpiocontrol(wlc_hw->sih, gm, gc, GPIO_DRV_PRIORITY);
-}
-
-static void brcms_ucode_download(struct brcms_hardware *wlc_hw)
-{
-	struct brcms_c_info *wlc;
-	wlc = wlc_hw->wlc;
-
-	if (wlc_hw->ucode_loaded)
-		return;
-
-	if (D11REV_IS(wlc_hw->corerev, 23)) {
-		if (BRCMS_ISNPHY(wlc_hw->band)) {
-			brcms_ucode_write(wlc_hw, bcm43xx_16_mimo,
-					bcm43xx_16_mimosz);
-			wlc_hw->ucode_loaded = true;
-		} else
-			wiphy_err(wlc->wiphy, "%s: wl%d: unsupported phy in "
-				  "corerev %d\n",
-				  __func__, wlc_hw->unit, wlc_hw->corerev);
-	} else if (D11REV_IS(wlc_hw->corerev, 24)) {
-		if (BRCMS_ISLCNPHY(wlc_hw->band)) {
-			brcms_ucode_write(wlc_hw, bcm43xx_24_lcn,
-					bcm43xx_24_lcnsz);
-			wlc_hw->ucode_loaded = true;
-		} else {
-			wiphy_err(wlc->wiphy, "%s: wl%d: unsupported phy in "
-				  "corerev %d\n",
-				  __func__, wlc_hw->unit, wlc_hw->corerev);
-		}
-	}
-}
-
-static void brcms_ucode_write(struct brcms_hardware *wlc_hw, const u32 ucode[],
-			      const uint nbytes) {
-	d11regs_t *regs = wlc_hw->regs;
-	uint i;
-	uint count;
-
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	count = (nbytes / sizeof(u32));
-
-	W_REG(&regs->objaddr, (OBJADDR_AUTO_INC | OBJADDR_UCM_SEL));
-	(void)R_REG(&regs->objaddr);
-	for (i = 0; i < count; i++)
-		W_REG(&regs->objdata, ucode[i]);
-}
-
-static void brcms_c_write_inits(struct brcms_hardware *wlc_hw,
-			    const struct d11init *inits)
-{
-	int i;
-	volatile u8 *base;
-
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	base = (volatile u8 *)wlc_hw->regs;
-
-	for (i = 0; inits[i].addr != 0xffff; i++) {
-		if (inits[i].size == 2)
-			W_REG((u16 *)(base + inits[i].addr),
-			      inits[i].value);
-		else if (inits[i].size == 4)
-			W_REG((u32 *)(base + inits[i].addr),
-			      inits[i].value);
-	}
-}
-
-static void brcms_c_ucode_txant_set(struct brcms_hardware *wlc_hw)
-{
-	u16 phyctl;
-	u16 phytxant = wlc_hw->bmac_phytxant;
-	u16 mask = PHY_TXC_ANT_MASK;
-
-	/* set the Probe Response frame phy control word */
-	phyctl = brcms_b_read_shm(wlc_hw, M_CTXPRS_BLK + C_CTX_PCTLWD_POS);
-	phyctl = (phyctl & ~mask) | phytxant;
-	brcms_b_write_shm(wlc_hw, M_CTXPRS_BLK + C_CTX_PCTLWD_POS, phyctl);
-
-	/* set the Response (ACK/CTS) frame phy control word */
-	phyctl = brcms_b_read_shm(wlc_hw, M_RSP_PCTLWD);
-	phyctl = (phyctl & ~mask) | phytxant;
-	brcms_b_write_shm(wlc_hw, M_RSP_PCTLWD, phyctl);
-}
-
-void brcms_b_txant_set(struct brcms_hardware *wlc_hw, u16 phytxant)
-{
-	/* update sw state */
-	wlc_hw->bmac_phytxant = phytxant;
-
-	/* push to ucode if up */
-	if (!wlc_hw->up)
-		return;
-	brcms_c_ucode_txant_set(wlc_hw);
-
-}
-
-u16 brcms_b_get_txant(struct brcms_hardware *wlc_hw)
-{
-	return (u16) wlc_hw->wlc->stf->txant;
-}
-
-void brcms_b_antsel_type_set(struct brcms_hardware *wlc_hw, u8 antsel_type)
-{
-	wlc_hw->antsel_type = antsel_type;
-
-	/* Update the antsel type for phy module to use */
-	wlc_phy_antsel_type_set(wlc_hw->band->pi, antsel_type);
-}
-
-void brcms_b_fifoerrors(struct brcms_hardware *wlc_hw)
-{
-	bool fatal = false;
-	uint unit;
-	uint intstatus, idx;
-	d11regs_t *regs = wlc_hw->regs;
-	struct wiphy *wiphy = wlc_hw->wlc->wiphy;
-
-	unit = wlc_hw->unit;
-
-	for (idx = 0; idx < NFIFO; idx++) {
-		/* read intstatus register and ignore any non-error bits */
-		intstatus =
-		    R_REG(&regs->intctrlregs[idx].intstatus) & I_ERRORS;
-		if (!intstatus)
-			continue;
-
-		BCMMSG(wlc_hw->wlc->wiphy, "wl%d: intstatus%d 0x%x\n",
-			unit, idx, intstatus);
-
-		if (intstatus & I_RO) {
-			wiphy_err(wiphy, "wl%d: fifo %d: receive fifo "
-				  "overflow\n", unit, idx);
-			fatal = true;
-		}
-
-		if (intstatus & I_PC) {
-			wiphy_err(wiphy, "wl%d: fifo %d: descriptor error\n",
-				 unit, idx);
-			fatal = true;
-		}
-
-		if (intstatus & I_PD) {
-			wiphy_err(wiphy, "wl%d: fifo %d: data error\n", unit,
-				  idx);
-			fatal = true;
-		}
-
-		if (intstatus & I_DE) {
-			wiphy_err(wiphy, "wl%d: fifo %d: descriptor protocol "
-				  "error\n", unit, idx);
-			fatal = true;
-		}
-
-		if (intstatus & I_RU) {
-			wiphy_err(wiphy, "wl%d: fifo %d: receive descriptor "
-				  "underflow\n", idx, unit);
-		}
-
-		if (intstatus & I_XU) {
-			wiphy_err(wiphy, "wl%d: fifo %d: transmit fifo "
-				  "underflow\n", idx, unit);
-			fatal = true;
-		}
-
-		if (fatal) {
-			brcms_c_fatal_error(wlc_hw->wlc);	/* big hammer */
-			break;
-		} else
-			W_REG(&regs->intctrlregs[idx].intstatus,
-			      intstatus);
-	}
-}
-
-void brcms_c_intrson(struct brcms_c_info *wlc)
-{
-	struct brcms_hardware *wlc_hw = wlc->hw;
-	wlc->macintmask = wlc->defmacintmask;
-	W_REG(&wlc_hw->regs->macintmask, wlc->macintmask);
-}
-
-/* callback for siutils.c, which has only wlc handler, no wl
- * they both check up, not only because there is no need to off/restore d11 interrupt
- *  but also because per-port code may require sync with valid interrupt.
- */
-
-static u32 brcms_c_wlintrsoff(struct brcms_c_info *wlc)
-{
-	if (!wlc->hw->up)
-		return 0;
-
-	return brcms_intrsoff(wlc->wl);
-}
-
-static void brcms_c_wlintrsrestore(struct brcms_c_info *wlc, u32 macintmask)
-{
-	if (!wlc->hw->up)
-		return;
-
-	brcms_intrsrestore(wlc->wl, macintmask);
-}
-
-u32 brcms_c_intrsoff(struct brcms_c_info *wlc)
-{
-	struct brcms_hardware *wlc_hw = wlc->hw;
-	u32 macintmask;
-
-	if (!wlc_hw->clk)
-		return 0;
-
-	macintmask = wlc->macintmask;	/* isr can still happen */
-
-	W_REG(&wlc_hw->regs->macintmask, 0);
-	(void)R_REG(&wlc_hw->regs->macintmask);	/* sync readback */
-	udelay(1);		/* ensure int line is no longer driven */
-	wlc->macintmask = 0;
-
-	/* return previous macintmask; resolve race between us and our isr */
-	return wlc->macintstatus ? 0 : macintmask;
-}
-
-void brcms_c_intrsrestore(struct brcms_c_info *wlc, u32 macintmask)
-{
-	struct brcms_hardware *wlc_hw = wlc->hw;
-	if (!wlc_hw->clk)
-		return;
-
-	wlc->macintmask = macintmask;
-	W_REG(&wlc_hw->regs->macintmask, wlc->macintmask);
-}
-
-static void brcms_b_mute(struct brcms_hardware *wlc_hw, bool on, mbool flags)
-{
-	u8 null_ether_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
-
-	if (on) {
-		/* suspend tx fifos */
-		brcms_b_tx_fifo_suspend(wlc_hw, TX_DATA_FIFO);
-		brcms_b_tx_fifo_suspend(wlc_hw, TX_CTL_FIFO);
-		brcms_b_tx_fifo_suspend(wlc_hw, TX_AC_BK_FIFO);
-		brcms_b_tx_fifo_suspend(wlc_hw, TX_AC_VI_FIFO);
-
-		/* zero the address match register so we do not send ACKs */
-		brcms_b_set_addrmatch(wlc_hw, RCM_MAC_OFFSET,
-				       null_ether_addr);
-	} else {
-		/* resume tx fifos */
-		if (!wlc_hw->wlc->tx_suspended) {
-			brcms_b_tx_fifo_resume(wlc_hw, TX_DATA_FIFO);
-		}
-		brcms_b_tx_fifo_resume(wlc_hw, TX_CTL_FIFO);
-		brcms_b_tx_fifo_resume(wlc_hw, TX_AC_BK_FIFO);
-		brcms_b_tx_fifo_resume(wlc_hw, TX_AC_VI_FIFO);
-
-		/* Restore address */
-		brcms_b_set_addrmatch(wlc_hw, RCM_MAC_OFFSET,
-				       wlc_hw->etheraddr);
-	}
-
-	wlc_phy_mute_upd(wlc_hw->band->pi, on, flags);
-
-	if (on)
-		brcms_c_ucode_mute_override_set(wlc_hw);
-	else
-		brcms_c_ucode_mute_override_clear(wlc_hw);
-}
-
-int brcms_b_xmtfifo_sz_get(struct brcms_hardware *wlc_hw, uint fifo,
-			   uint *blocks)
-{
-	if (fifo >= NFIFO)
-		return -EINVAL;
-
-	*blocks = wlc_hw->xmtfifo_sz[fifo];
-
-	return 0;
-}
-
-/* brcms_b_tx_fifo_suspended:
- * Check the MAC's tx suspend status for a tx fifo.
- *
- * When the MAC acknowledges a tx suspend, it indicates that no more
- * packets will be transmitted out the radio. This is independent of
- * DMA channel suspension---the DMA may have finished suspending, or may still
- * be pulling data into a tx fifo, by the time the MAC acks the suspend
- * request.
- */
-static bool brcms_b_tx_fifo_suspended(struct brcms_hardware *wlc_hw,
-				      uint tx_fifo)
-{
-	/* check that a suspend has been requested and is no longer pending */
-
-	/*
-	 * for DMA mode, the suspend request is set in xmtcontrol of the DMA engine,
-	 * and the tx fifo suspend at the lower end of the MAC is acknowledged in the
-	 * chnstatus register.
-	 * The tx fifo suspend completion is independent of the DMA suspend completion and
-	 *   may be acked before or after the DMA is suspended.
-	 */
-	if (dma_txsuspended(wlc_hw->di[tx_fifo]) &&
-	    (R_REG(&wlc_hw->regs->chnstatus) &
-	     (1 << tx_fifo)) == 0)
-		return true;
-
-	return false;
-}
-
-static void brcms_b_tx_fifo_suspend(struct brcms_hardware *wlc_hw,
-				    uint tx_fifo)
-{
-	u8 fifo = 1 << tx_fifo;
-
-	/* Two clients of this code, 11h Quiet period and scanning. */
-
-	/* only suspend if not already suspended */
-	if ((wlc_hw->suspended_fifos & fifo) == fifo)
-		return;
-
-	/* force the core awake only if not already */
-	if (wlc_hw->suspended_fifos == 0)
-		brcms_c_ucode_wake_override_set(wlc_hw,
-						BRCMS_WAKE_OVERRIDE_TXFIFO);
-
-	wlc_hw->suspended_fifos |= fifo;
-
-	if (wlc_hw->di[tx_fifo]) {
-		/* Suspending AMPDU transmissions in the middle can cause underflow
-		 * which may result in mismatch between ucode and driver
-		 * so suspend the mac before suspending the FIFO
-		 */
-		if (BRCMS_PHY_11N_CAP(wlc_hw->band))
-			brcms_c_suspend_mac_and_wait(wlc_hw->wlc);
-
-		dma_txsuspend(wlc_hw->di[tx_fifo]);
-
-		if (BRCMS_PHY_11N_CAP(wlc_hw->band))
-			brcms_c_enable_mac(wlc_hw->wlc);
-	}
-}
-
-static void brcms_b_tx_fifo_resume(struct brcms_hardware *wlc_hw,
-				   uint tx_fifo)
-{
-	/* BMAC_NOTE: BRCMS_TX_FIFO_ENAB is done in brcms_c_dpc() for DMA case
-	 * but need to be done here for PIO otherwise the watchdog will catch
-	 * the inconsistency and fire
-	 */
-	/* Two clients of this code, 11h Quiet period and scanning. */
-	if (wlc_hw->di[tx_fifo])
-		dma_txresume(wlc_hw->di[tx_fifo]);
-
-	/* allow core to sleep again */
-	if (wlc_hw->suspended_fifos == 0)
-		return;
-	else {
-		wlc_hw->suspended_fifos &= ~(1 << tx_fifo);
-		if (wlc_hw->suspended_fifos == 0)
-			brcms_c_ucode_wake_override_clear(wlc_hw,
-						BRCMS_WAKE_OVERRIDE_TXFIFO);
-	}
-}
-
-/*
- * Read and clear macintmask and macintstatus and intstatus registers.
- * This routine should be called with interrupts off
- * Return:
- *   -1 if DEVICEREMOVED(wlc) evaluates to true;
- *   0 if the interrupt is not for us, or we are in some special cases;
- *   device interrupt status bits otherwise.
- */
-static inline u32 wlc_intstatus(struct brcms_c_info *wlc, bool in_isr)
-{
-	struct brcms_hardware *wlc_hw = wlc->hw;
-	d11regs_t *regs = wlc_hw->regs;
-	u32 macintstatus;
-
-	/* macintstatus includes a DMA interrupt summary bit */
-	macintstatus = R_REG(&regs->macintstatus);
-
-	BCMMSG(wlc->wiphy, "wl%d: macintstatus: 0x%x\n", wlc_hw->unit,
-		 macintstatus);
-
-	/* detect cardbus removed, in power down(suspend) and in reset */
-	if (DEVICEREMOVED(wlc))
-		return -1;
-
-	/* DEVICEREMOVED succeeds even when the core is still resetting,
-	 * handle that case here.
-	 */
-	if (macintstatus == 0xffffffff)
-		return 0;
-
-	/* defer unsolicited interrupts */
-	macintstatus &= (in_isr ? wlc->macintmask : wlc->defmacintmask);
-
-	/* if not for us */
-	if (macintstatus == 0)
-		return 0;
-
-	/* interrupts are already turned off for CFE build
-	 * Caution: For CFE Turning off the interrupts again has some undesired
-	 * consequences
-	 */
-	/* turn off the interrupts */
-	W_REG(&regs->macintmask, 0);
-	(void)R_REG(&regs->macintmask);	/* sync readback */
-	wlc->macintmask = 0;
-
-	/* clear device interrupts */
-	W_REG(&regs->macintstatus, macintstatus);
-
-	/* MI_DMAINT is indication of non-zero intstatus */
-	if (macintstatus & MI_DMAINT) {
-		/*
-		 * only fifo interrupt enabled is I_RI in
-		 * RX_FIFO. If MI_DMAINT is set, assume it
-		 * is set and clear the interrupt.
-		 */
-		W_REG(&regs->intctrlregs[RX_FIFO].intstatus,
-		      DEF_RXINTMASK);
-	}
-
-	return macintstatus;
-}
-
-/* Update wlc->macintstatus and wlc->intstatus[]. */
-/* Return true if they are updated successfully. false otherwise */
-bool brcms_c_intrsupd(struct brcms_c_info *wlc)
-{
-	u32 macintstatus;
-
-	/* read and clear macintstatus and intstatus registers */
-	macintstatus = wlc_intstatus(wlc, false);
-
-	/* device is removed */
-	if (macintstatus == 0xffffffff)
-		return false;
-
-	/* update interrupt status in software */
-	wlc->macintstatus |= macintstatus;
-
-	return true;
-}
-
-/*
- * First-level interrupt processing.
- * Return true if this was our interrupt, false otherwise.
- * *wantdpc will be set to true if further brcms_c_dpc() processing is required,
- * false otherwise.
- */
-bool brcms_c_isr(struct brcms_c_info *wlc, bool *wantdpc)
-{
-	struct brcms_hardware *wlc_hw = wlc->hw;
-	u32 macintstatus;
-
-	*wantdpc = false;
-
-	if (!wlc_hw->up || !wlc->macintmask)
-		return false;
-
-	/* read and clear macintstatus and intstatus registers */
-	macintstatus = wlc_intstatus(wlc, true);
-
-	if (macintstatus == 0xffffffff)
-		wiphy_err(wlc->wiphy, "DEVICEREMOVED detected in the ISR code"
-			  " path\n");
-
-	/* it is not for us */
-	if (macintstatus == 0)
-		return false;
-
-	*wantdpc = true;
-
-	/* save interrupt status bits */
-	wlc->macintstatus = macintstatus;
-
-	return true;
-
-}
-
-static bool
-brcms_b_dotxstatus(struct brcms_hardware *wlc_hw, struct tx_status *txs,
-		   u32 s2)
-{
-	/* discard intermediate indications for ucode with one legitimate case:
-	 *   e.g. if "useRTS" is set. ucode did a successful rts/cts exchange, but the subsequent
-	 *   tx of DATA failed. so it will start rts/cts from the beginning (resetting the rts
-	 *   transmission count)
-	 */
-	if (!(txs->status & TX_STATUS_AMPDU)
-	    && (txs->status & TX_STATUS_INTERMEDIATE)) {
-		return false;
-	}
-
-	return brcms_c_dotxstatus(wlc_hw->wlc, txs, s2);
-}
-
-/* process tx completion events in BMAC
- * Return true if more tx status need to be processed. false otherwise.
- */
-static bool
-brcms_b_txstatus(struct brcms_hardware *wlc_hw, bool bound, bool *fatal)
-{
-	bool morepending = false;
-	struct brcms_c_info *wlc = wlc_hw->wlc;
-	d11regs_t *regs;
-	struct tx_status txstatus, *txs;
-	u32 s1, s2;
-	uint n = 0;
-	/*
-	 * Param 'max_tx_num' indicates max. # tx status to process before
-	 * break out.
-	 */
-	uint max_tx_num = bound ? wlc->pub->tunables->txsbnd : -1;
-
-	BCMMSG(wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	txs = &txstatus;
-	regs = wlc_hw->regs;
-	while (!(*fatal)
-	       && (s1 = R_REG(&regs->frmtxstatus)) & TXS_V) {
-
-		if (s1 == 0xffffffff) {
-			wiphy_err(wlc->wiphy, "wl%d: %s: dead chip\n",
-				wlc_hw->unit, __func__);
-			return morepending;
-		}
-
-			s2 = R_REG(&regs->frmtxstatus2);
-
-		txs->status = s1 & TXS_STATUS_MASK;
-		txs->frameid = (s1 & TXS_FID_MASK) >> TXS_FID_SHIFT;
-		txs->sequence = s2 & TXS_SEQ_MASK;
-		txs->phyerr = (s2 & TXS_PTX_MASK) >> TXS_PTX_SHIFT;
-		txs->lasttxtime = 0;
-
-		*fatal = brcms_b_dotxstatus(wlc_hw, txs, s2);
-
-		/* !give others some time to run! */
-		if (++n >= max_tx_num)
-			break;
-	}
-
-	if (*fatal)
-		return 0;
-
-	if (n >= max_tx_num)
-		morepending = true;
-
-	if (!pktq_empty(&wlc->pkt_queue->q))
-		brcms_c_send_q(wlc);
-
-	return morepending;
-}
-
-void brcms_c_suspend_mac_and_wait(struct brcms_c_info *wlc)
-{
-	struct brcms_hardware *wlc_hw = wlc->hw;
-	d11regs_t *regs = wlc_hw->regs;
-	u32 mc, mi;
-	struct wiphy *wiphy = wlc->wiphy;
-
-	BCMMSG(wlc->wiphy, "wl%d: bandunit %d\n", wlc_hw->unit,
-		wlc_hw->band->bandunit);
-
-	/*
-	 * Track overlapping suspend requests
-	 */
-	wlc_hw->mac_suspend_depth++;
-	if (wlc_hw->mac_suspend_depth > 1)
-		return;
-
-	/* force the core awake */
-	brcms_c_ucode_wake_override_set(wlc_hw, BRCMS_WAKE_OVERRIDE_MACSUSPEND);
-
-	mc = R_REG(&regs->maccontrol);
-
-	if (mc == 0xffffffff) {
-		wiphy_err(wiphy, "wl%d: %s: dead chip\n", wlc_hw->unit,
-			  __func__);
-		brcms_down(wlc->wl);
-		return;
-	}
-	WARN_ON(mc & MCTL_PSM_JMP_0);
-	WARN_ON(!(mc & MCTL_PSM_RUN));
-	WARN_ON(!(mc & MCTL_EN_MAC));
-
-	mi = R_REG(&regs->macintstatus);
-	if (mi == 0xffffffff) {
-		wiphy_err(wiphy, "wl%d: %s: dead chip\n", wlc_hw->unit,
-			  __func__);
-		brcms_down(wlc->wl);
-		return;
-	}
-	WARN_ON(mi & MI_MACSSPNDD);
-
-	brcms_b_mctrl(wlc_hw, MCTL_EN_MAC, 0);
-
-	SPINWAIT(!(R_REG(&regs->macintstatus) & MI_MACSSPNDD),
-		 BRCMS_MAX_MAC_SUSPEND);
-
-	if (!(R_REG(&regs->macintstatus) & MI_MACSSPNDD)) {
-		wiphy_err(wiphy, "wl%d: wlc_suspend_mac_and_wait: waited %d uS"
-			  " and MI_MACSSPNDD is still not on.\n",
-			  wlc_hw->unit, BRCMS_MAX_MAC_SUSPEND);
-		wiphy_err(wiphy, "wl%d: psmdebug 0x%08x, phydebug 0x%08x, "
-			  "psm_brc 0x%04x\n", wlc_hw->unit,
-			  R_REG(&regs->psmdebug),
-			  R_REG(&regs->phydebug),
-			  R_REG(&regs->psm_brc));
-	}
-
-	mc = R_REG(&regs->maccontrol);
-	if (mc == 0xffffffff) {
-		wiphy_err(wiphy, "wl%d: %s: dead chip\n", wlc_hw->unit,
-			  __func__);
-		brcms_down(wlc->wl);
-		return;
-	}
-	WARN_ON(mc & MCTL_PSM_JMP_0);
-	WARN_ON(!(mc & MCTL_PSM_RUN));
-	WARN_ON(mc & MCTL_EN_MAC);
-}
-
-void brcms_c_enable_mac(struct brcms_c_info *wlc)
-{
-	struct brcms_hardware *wlc_hw = wlc->hw;
-	d11regs_t *regs = wlc_hw->regs;
-	u32 mc, mi;
-
-	BCMMSG(wlc->wiphy, "wl%d: bandunit %d\n", wlc_hw->unit,
-		wlc->band->bandunit);
-
-	/*
-	 * Track overlapping suspend requests
-	 */
-	wlc_hw->mac_suspend_depth--;
-	if (wlc_hw->mac_suspend_depth > 0)
-		return;
-
-	mc = R_REG(&regs->maccontrol);
-	WARN_ON(mc & MCTL_PSM_JMP_0);
-	WARN_ON(mc & MCTL_EN_MAC);
-	WARN_ON(!(mc & MCTL_PSM_RUN));
-
-	brcms_b_mctrl(wlc_hw, MCTL_EN_MAC, MCTL_EN_MAC);
-	W_REG(&regs->macintstatus, MI_MACSSPNDD);
-
-	mc = R_REG(&regs->maccontrol);
-	WARN_ON(mc & MCTL_PSM_JMP_0);
-	WARN_ON(!(mc & MCTL_EN_MAC));
-	WARN_ON(!(mc & MCTL_PSM_RUN));
-
-	mi = R_REG(&regs->macintstatus);
-	WARN_ON(mi & MI_MACSSPNDD);
-
-	brcms_c_ucode_wake_override_clear(wlc_hw,
-					  BRCMS_WAKE_OVERRIDE_MACSUSPEND);
-}
-
-static void brcms_upd_ofdm_pctl1_table(struct brcms_hardware *wlc_hw)
-{
-	u8 rate;
-	u8 rates[8] = {
-		BRCM_RATE_6M, BRCM_RATE_9M, BRCM_RATE_12M, BRCM_RATE_18M,
-		BRCM_RATE_24M, BRCM_RATE_36M, BRCM_RATE_48M, BRCM_RATE_54M
-	};
-	u16 entry_ptr;
-	u16 pctl1;
-	uint i;
-
-	if (!BRCMS_PHY_11N_CAP(wlc_hw->band))
-		return;
-
-	/* walk the phy rate table and update the entries */
-	for (i = 0; i < ARRAY_SIZE(rates); i++) {
-		rate = rates[i];
-
-		entry_ptr = brcms_b_ofdm_ratetable_offset(wlc_hw, rate);
-
-		/* read the SHM Rate Table entry OFDM PCTL1 values */
-		pctl1 =
-		    brcms_b_read_shm(wlc_hw, entry_ptr + M_RT_OFDM_PCTL1_POS);
-
-		/* modify the value */
-		pctl1 &= ~PHY_TXC1_MODE_MASK;
-		pctl1 |= (wlc_hw->hw_stf_ss_opmode << PHY_TXC1_MODE_SHIFT);
-
-		/* Update the SHM Rate Table entry OFDM PCTL1 values */
-		brcms_b_write_shm(wlc_hw, entry_ptr + M_RT_OFDM_PCTL1_POS,
-				   pctl1);
-	}
-}
-
-static u16 brcms_b_ofdm_ratetable_offset(struct brcms_hardware *wlc_hw,
-					 u8 rate)
-{
-	uint i;
-	u8 plcp_rate = 0;
-	struct plcp_signal_rate_lookup {
-		u8 rate;
-		u8 signal_rate;
-	};
-	/* OFDM RATE sub-field of PLCP SIGNAL field, per 802.11 sec 17.3.4.1 */
-	const struct plcp_signal_rate_lookup rate_lookup[] = {
-		{BRCM_RATE_6M, 0xB},
-		{BRCM_RATE_9M, 0xF},
-		{BRCM_RATE_12M, 0xA},
-		{BRCM_RATE_18M, 0xE},
-		{BRCM_RATE_24M, 0x9},
-		{BRCM_RATE_36M, 0xD},
-		{BRCM_RATE_48M, 0x8},
-		{BRCM_RATE_54M, 0xC}
-	};
-
-	for (i = 0; i < ARRAY_SIZE(rate_lookup); i++) {
-		if (rate == rate_lookup[i].rate) {
-			plcp_rate = rate_lookup[i].signal_rate;
-			break;
-		}
-	}
-
-	/* Find the SHM pointer to the rate table entry by looking in the
-	 * Direct-map Table
-	 */
-	return 2 * brcms_b_read_shm(wlc_hw, M_RT_DIRMAP_A + (plcp_rate * 2));
-}
-
-void brcms_b_band_stf_ss_set(struct brcms_hardware *wlc_hw, u8 stf_mode)
-{
-	wlc_hw->hw_stf_ss_opmode = stf_mode;
-
-	if (wlc_hw->clk)
-		brcms_upd_ofdm_pctl1_table(wlc_hw);
-}
-
-void
-brcms_b_read_tsf(struct brcms_hardware *wlc_hw, u32 *tsf_l_ptr,
-		  u32 *tsf_h_ptr)
-{
-	d11regs_t *regs = wlc_hw->regs;
-
-	/* read the tsf timer low, then high to get an atomic read */
-	*tsf_l_ptr = R_REG(&regs->tsf_timerlow);
-	*tsf_h_ptr = R_REG(&regs->tsf_timerhigh);
-
-	return;
-}
-
-static bool brcms_b_validate_chip_access(struct brcms_hardware *wlc_hw)
-{
-	d11regs_t *regs;
-	u32 w, val;
-	struct wiphy *wiphy = wlc_hw->wlc->wiphy;
-
-	BCMMSG(wiphy, "wl%d\n", wlc_hw->unit);
-
-	regs = wlc_hw->regs;
-
-	/* Validate dchip register access */
-
-	W_REG(&regs->objaddr, OBJADDR_SHM_SEL | 0);
-	(void)R_REG(&regs->objaddr);
-	w = R_REG(&regs->objdata);
-
-	/* Can we write and read back a 32bit register? */
-	W_REG(&regs->objaddr, OBJADDR_SHM_SEL | 0);
-	(void)R_REG(&regs->objaddr);
-	W_REG(&regs->objdata, (u32) 0xaa5555aa);
-
-	W_REG(&regs->objaddr, OBJADDR_SHM_SEL | 0);
-	(void)R_REG(&regs->objaddr);
-	val = R_REG(&regs->objdata);
-	if (val != (u32) 0xaa5555aa) {
-		wiphy_err(wiphy, "wl%d: validate_chip_access: SHM = 0x%x, "
-			  "expected 0xaa5555aa\n", wlc_hw->unit, val);
-		return false;
-	}
-
-	W_REG(&regs->objaddr, OBJADDR_SHM_SEL | 0);
-	(void)R_REG(&regs->objaddr);
-	W_REG(&regs->objdata, (u32) 0x55aaaa55);
-
-	W_REG(&regs->objaddr, OBJADDR_SHM_SEL | 0);
-	(void)R_REG(&regs->objaddr);
-	val = R_REG(&regs->objdata);
-	if (val != (u32) 0x55aaaa55) {
-		wiphy_err(wiphy, "wl%d: validate_chip_access: SHM = 0x%x, "
-			  "expected 0x55aaaa55\n", wlc_hw->unit, val);
-		return false;
-	}
-
-	W_REG(&regs->objaddr, OBJADDR_SHM_SEL | 0);
-	(void)R_REG(&regs->objaddr);
-	W_REG(&regs->objdata, w);
-
-	/* clear CFPStart */
-	W_REG(&regs->tsf_cfpstart, 0);
-
-	w = R_REG(&regs->maccontrol);
-	if ((w != (MCTL_IHR_EN | MCTL_WAKE)) &&
-	    (w != (MCTL_IHR_EN | MCTL_GMODE | MCTL_WAKE))) {
-		wiphy_err(wiphy, "wl%d: validate_chip_access: maccontrol = "
-			  "0x%x, expected 0x%x or 0x%x\n", wlc_hw->unit, w,
-			  (MCTL_IHR_EN | MCTL_WAKE),
-			  (MCTL_IHR_EN | MCTL_GMODE | MCTL_WAKE));
-		return false;
-	}
-
-	return true;
-}
-
-#define PHYPLL_WAIT_US	100000
-
-void brcms_b_core_phypll_ctl(struct brcms_hardware *wlc_hw, bool on)
-{
-	d11regs_t *regs;
-	u32 tmp;
-
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	tmp = 0;
-	regs = wlc_hw->regs;
-
-	if (on) {
-		if ((wlc_hw->sih->chip == BCM4313_CHIP_ID)) {
-			OR_REG(&regs->clk_ctl_st,
-			       (CCS_ERSRC_REQ_HT | CCS_ERSRC_REQ_D11PLL |
-				CCS_ERSRC_REQ_PHYPLL));
-			SPINWAIT((R_REG(&regs->clk_ctl_st) &
-				  (CCS_ERSRC_AVAIL_HT)) != (CCS_ERSRC_AVAIL_HT),
-				 PHYPLL_WAIT_US);
-
-			tmp = R_REG(&regs->clk_ctl_st);
-			if ((tmp & (CCS_ERSRC_AVAIL_HT)) !=
-			    (CCS_ERSRC_AVAIL_HT)) {
-				wiphy_err(wlc_hw->wlc->wiphy, "%s: turn on PHY"
-					  " PLL failed\n", __func__);
-			}
-		} else {
-			OR_REG(&regs->clk_ctl_st,
-			       (CCS_ERSRC_REQ_D11PLL | CCS_ERSRC_REQ_PHYPLL));
-			SPINWAIT((R_REG(&regs->clk_ctl_st) &
-				  (CCS_ERSRC_AVAIL_D11PLL |
-				   CCS_ERSRC_AVAIL_PHYPLL)) !=
-				 (CCS_ERSRC_AVAIL_D11PLL |
-				  CCS_ERSRC_AVAIL_PHYPLL), PHYPLL_WAIT_US);
-
-			tmp = R_REG(&regs->clk_ctl_st);
-			if ((tmp &
-			     (CCS_ERSRC_AVAIL_D11PLL | CCS_ERSRC_AVAIL_PHYPLL))
-			    !=
-			    (CCS_ERSRC_AVAIL_D11PLL | CCS_ERSRC_AVAIL_PHYPLL)) {
-				wiphy_err(wlc_hw->wlc->wiphy, "%s: turn on "
-					  "PHY PLL failed\n", __func__);
-			}
-		}
-	} else {
-		/* Since the PLL may be shared, other cores can still be requesting it;
-		 * so we'll deassert the request but not wait for status to comply.
-		 */
-		AND_REG(&regs->clk_ctl_st, ~CCS_ERSRC_REQ_PHYPLL);
-		tmp = R_REG(&regs->clk_ctl_st);
-	}
-}
-
-void brcms_c_coredisable(struct brcms_hardware *wlc_hw)
-{
-	bool dev_gone;
-
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
-
-	dev_gone = DEVICEREMOVED(wlc_hw->wlc);
-
-	if (dev_gone)
-		return;
-
-	if (wlc_hw->noreset)
-		return;
-
-	/* radio off */
-	wlc_phy_switch_radio(wlc_hw->band->pi, OFF);
-
-	/* turn off analog core */
-	wlc_phy_anacore(wlc_hw->band->pi, OFF);
-
-	/* turn off PHYPLL to save power */
-	brcms_b_core_phypll_ctl(wlc_hw, false);
-
-	/* No need to set wlc->pub->radio_active = OFF
-	 * because this function needs down capability and
-	 * radio_active is designed for BCMNODOWN.
-	 */
-
-	/* remove gpio controls */
-	if (wlc_hw->ucode_dbgsel)
-		ai_gpiocontrol(wlc_hw->sih, ~0, 0, GPIO_DRV_PRIORITY);
-
-	wlc_hw->clk = false;
-	ai_core_disable(wlc_hw->sih, 0);
-	wlc_phy_hw_clk_state_upd(wlc_hw->band->pi, false);
-}
-
-/* power both the pll and external oscillator on/off */
-static void brcms_b_xtal(struct brcms_hardware *wlc_hw, bool want)
-{
-	BCMMSG(wlc_hw->wlc->wiphy, "wl%d: want %d\n", wlc_hw->unit, want);
-
-	/* dont power down if plldown is false or we must poll hw radio disable */
-	if (!want && wlc_hw->pllreq)
-		return;
-
-	if (wlc_hw->sih)
-		ai_clkctl_xtal(wlc_hw->sih, XTAL | PLL, want);
-
-	wlc_hw->sbclk = want;
-	if (!wlc_hw->sbclk) {
-		wlc_hw->clk = false;
-		if (wlc_hw->band && wlc_hw->band->pi)
-			wlc_phy_hw_clk_state_upd(wlc_hw->band->pi, false);
-	}
-}
-
-static void brcms_c_flushqueues(struct brcms_c_info *wlc)
-{
-	struct brcms_hardware *wlc_hw = wlc->hw;
-	uint i;
-
-	wlc->txpend16165war = 0;
-
-	/* free any posted tx packets */
-	for (i = 0; i < NFIFO; i++)
-		if (wlc_hw->di[i]) {
-			dma_txreclaim(wlc_hw->di[i], DMA_RANGE_ALL);
-			TXPKTPENDCLR(wlc, i);
-			BCMMSG(wlc->wiphy, "pktpend fifo %d clrd\n", i);
-		}
-
-	/* free any posted rx packets */
-	dma_rxreclaim(wlc_hw->di[RX_FIFO]);
-}
-
-u16 brcms_b_read_shm(struct brcms_hardware *wlc_hw, uint offset)
-{
-	return brcms_b_read_objmem(wlc_hw, offset, OBJADDR_SHM_SEL);
-}
-
-void brcms_b_write_shm(struct brcms_hardware *wlc_hw, uint offset, u16 v)
-{
-	brcms_b_write_objmem(wlc_hw, offset, v, OBJADDR_SHM_SEL);
-}
-
-static u16
-brcms_b_read_objmem(struct brcms_hardware *wlc_hw, uint offset, u32 sel)
-{
-	d11regs_t *regs = wlc_hw->regs;
-	volatile u16 *objdata_lo = (volatile u16 *)&regs->objdata;
-	volatile u16 *objdata_hi = objdata_lo + 1;
-	u16 v;
-
-	W_REG(&regs->objaddr, sel | (offset >> 2));
-	(void)R_REG(&regs->objaddr);
-	if (offset & 2) {
-		v = R_REG(objdata_hi);
-	} else {
-		v = R_REG(objdata_lo);
-	}
-
-	return v;
-}
-
-static void
-brcms_b_write_objmem(struct brcms_hardware *wlc_hw, uint offset, u16 v,
-		     u32 sel)
-{
-	d11regs_t *regs = wlc_hw->regs;
-	volatile u16 *objdata_lo = (volatile u16 *)&regs->objdata;
-	volatile u16 *objdata_hi = objdata_lo + 1;
-
-	W_REG(&regs->objaddr, sel | (offset >> 2));
-	(void)R_REG(&regs->objaddr);
-	if (offset & 2) {
-		W_REG(objdata_hi, v);
-	} else {
-		W_REG(objdata_lo, v);
-	}
-}
-
-/* Copy a buffer to shared memory of specified type .
- * SHM 'offset' needs to be an even address and
- * Buffer length 'len' must be an even number of bytes
- * 'sel' selects the type of memory
- */
-void
-brcms_b_copyto_objmem(struct brcms_hardware *wlc_hw, uint offset,
-		      const void *buf, int len, u32 sel)
-{
-	u16 v;
-	const u8 *p = (const u8 *)buf;
-	int i;
-
-	if (len <= 0 || (offset & 1) || (len & 1))
-		return;
-
-	for (i = 0; i < len; i += 2) {
-		v = p[i] | (p[i + 1] << 8);
-		brcms_b_write_objmem(wlc_hw, offset + i, v, sel);
-	}
-}
-
-/* Copy a piece of shared memory of specified type to a buffer .
- * SHM 'offset' needs to be an even address and
- * Buffer length 'len' must be an even number of bytes
- * 'sel' selects the type of memory
- */
-void
-brcms_b_copyfrom_objmem(struct brcms_hardware *wlc_hw, uint offset, void *buf,
-			 int len, u32 sel)
-{
-	u16 v;
-	u8 *p = (u8 *) buf;
-	int i;
-
-	if (len <= 0 || (offset & 1) || (len & 1))
-		return;
-
-	for (i = 0; i < len; i += 2) {
-		v = brcms_b_read_objmem(wlc_hw, offset + i, sel);
-		p[i] = v & 0xFF;
-		p[i + 1] = (v >> 8) & 0xFF;
-	}
-}
-
-void brcms_b_copyfrom_vars(struct brcms_hardware *wlc_hw, char **buf,
-			   uint *len)
-{
-	BCMMSG(wlc_hw->wlc->wiphy, "nvram vars totlen=%d\n",
-		wlc_hw->vars_size);
-
-	*buf = wlc_hw->vars;
-	*len = wlc_hw->vars_size;
-}
-
-void brcms_b_retrylimit_upd(struct brcms_hardware *wlc_hw, u16 SRL, u16 LRL)
-{
-	wlc_hw->SRL = SRL;
-	wlc_hw->LRL = LRL;
-
-	/* write retry limit to SCR, shouldn't need to suspend */
-	if (wlc_hw->up) {
-		W_REG(&wlc_hw->regs->objaddr,
-		      OBJADDR_SCR_SEL | S_DOT11_SRC_LMT);
-		(void)R_REG(&wlc_hw->regs->objaddr);
-		W_REG(&wlc_hw->regs->objdata, wlc_hw->SRL);
-		W_REG(&wlc_hw->regs->objaddr,
-		      OBJADDR_SCR_SEL | S_DOT11_LRC_LMT);
-		(void)R_REG(&wlc_hw->regs->objaddr);
-		W_REG(&wlc_hw->regs->objdata, wlc_hw->LRL);
-	}
-}
-
-void brcms_b_pllreq(struct brcms_hardware *wlc_hw, bool set, mbool req_bit)
-{
-	if (set) {
-		if (mboolisset(wlc_hw->pllreq, req_bit))
-			return;
-
-		mboolset(wlc_hw->pllreq, req_bit);
-
-		if (mboolisset(wlc_hw->pllreq, BRCMS_PLLREQ_FLIP)) {
-			if (!wlc_hw->sbclk) {
-				brcms_b_xtal(wlc_hw, ON);
-			}
-		}
-	} else {
-		if (!mboolisset(wlc_hw->pllreq, req_bit))
-			return;
-
-		mboolclr(wlc_hw->pllreq, req_bit);
-
-		if (mboolisset(wlc_hw->pllreq, BRCMS_PLLREQ_FLIP)) {
-			if (wlc_hw->sbclk) {
-				brcms_b_xtal(wlc_hw, OFF);
-			}
-		}
-	}
-
-	return;
-}
-
-u16 brcms_b_rate_shm_offset(struct brcms_hardware *wlc_hw, u8 rate)
-{
-	u16 table_ptr;
-	u8 phy_rate, index;
-
-	/* get the phy specific rate encoding for the PLCP SIGNAL field */
-	if (IS_OFDM(rate))
-		table_ptr = M_RT_DIRMAP_A;
-	else
-		table_ptr = M_RT_DIRMAP_B;
-
-	/* for a given rate, the LS-nibble of the PLCP SIGNAL field is
-	 * the index into the rate table.
-	 */
-	phy_rate = rate_info[rate] & BRCMS_RATE_MASK;
-	index = phy_rate & 0xf;
-
-	/* Find the SHM pointer to the rate table entry by looking in the
-	 * Direct-map Table
-	 */
-	return 2 * brcms_b_read_shm(wlc_hw, table_ptr + (index * 2));
-}
-
-void brcms_b_antsel_set(struct brcms_hardware *wlc_hw, u32 antsel_avail)
-{
-	wlc_hw->antsel_avail = antsel_avail;
-}
diff --git a/drivers/staging/brcm80211/brcmsmac/bmac.h b/drivers/staging/brcm80211/brcmsmac/bmac.h
deleted file mode 100644
index 3c9ad4f3bd22..000000000000
--- a/drivers/staging/brcm80211/brcmsmac/bmac.h
+++ /dev/null
@@ -1,174 +0,0 @@
-/*
- * Copyright (c) 2010 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 _BRCM_BOTTOM_MAC_H_
-#define _BRCM_BOTTOM_MAC_H_
-
-#include <brcmu_wifi.h>
-#include "types.h"
-
-/* dup state between BMAC(struct brcms_hardware) and HIGH(struct brcms_c_info)
-   driver */
-struct brcms_b_state {
-	u32 machwcap;	/* mac hw capibility */
-	u32 preamble_ovr;	/* preamble override */
-};
-
-enum {
-	IOV_BMAC_DIAG,
-	IOV_BMAC_SBGPIOTIMERVAL,
-	IOV_BMAC_SBGPIOOUT,
-	IOV_BMAC_CCGPIOCTRL,	/* CC GPIOCTRL REG */
-	IOV_BMAC_CCGPIOOUT,	/* CC GPIOOUT REG */
-	IOV_BMAC_CCGPIOOUTEN,	/* CC GPIOOUTEN REG */
-	IOV_BMAC_CCGPIOIN,	/* CC GPIOIN REG */
-	IOV_BMAC_WPSGPIO,	/* WPS push button GPIO pin */
-	IOV_BMAC_OTPDUMP,
-	IOV_BMAC_OTPSTAT,
-	IOV_BMAC_PCIEASPM,	/* obfuscation clkreq/aspm control */
-	IOV_BMAC_PCIEADVCORRMASK,	/* advanced correctable error mask */
-	IOV_BMAC_PCIECLKREQ,	/* PCIE 1.1 clockreq enab support */
-	IOV_BMAC_PCIELCREG,	/* PCIE LCREG */
-	IOV_BMAC_SBGPIOTIMERMASK,
-	IOV_BMAC_RFDISABLEDLY,
-	IOV_BMAC_PCIEREG,	/* PCIE REG */
-	IOV_BMAC_PCICFGREG,	/* PCI Config register */
-	IOV_BMAC_PCIESERDESREG,	/* PCIE SERDES REG (dev, 0}offset) */
-	IOV_BMAC_PCIEGPIOOUT,	/* PCIEOUT REG */
-	IOV_BMAC_PCIEGPIOOUTEN,	/* PCIEOUTEN REG */
-	IOV_BMAC_PCIECLKREQENCTRL,	/* clkreqenctrl REG (PCIE REV > 6.0 */
-	IOV_BMAC_DMALPBK,
-	IOV_BMAC_CCREG,
-	IOV_BMAC_COREREG,
-	IOV_BMAC_SDCIS,
-	IOV_BMAC_SDIO_DRIVE,
-	IOV_BMAC_OTPW,
-	IOV_BMAC_NVOTPW,
-	IOV_BMAC_SROM,
-	IOV_BMAC_SRCRC,
-	IOV_BMAC_CIS_SOURCE,
-	IOV_BMAC_CISVAR,
-	IOV_BMAC_OTPLOCK,
-	IOV_BMAC_OTP_CHIPID,
-	IOV_BMAC_CUSTOMVAR1,
-	IOV_BMAC_BOARDFLAGS,
-	IOV_BMAC_BOARDFLAGS2,
-	IOV_BMAC_WPSLED,
-	IOV_BMAC_NVRAM_SOURCE,
-	IOV_BMAC_OTP_RAW_READ,
-	IOV_BMAC_LAST
-};
-
-extern int brcms_b_attach(struct brcms_c_info *wlc, u16 vendor, u16 device,
-			   uint unit, bool piomode, void *regsva, uint bustype,
-			   void *btparam);
-extern int brcms_b_detach(struct brcms_c_info *wlc);
-extern void brcms_b_watchdog(void *arg);
-
-/* up/down, reset, clk */
-extern void brcms_b_copyto_objmem(struct brcms_hardware *wlc_hw,
-				   uint offset, const void *buf, int len,
-				   u32 sel);
-extern void brcms_b_copyfrom_objmem(struct brcms_hardware *wlc_hw, uint offset,
-				     void *buf, int len, u32 sel);
-#define brcms_b_copyfrom_shm(wlc_hw, offset, buf, len)                 \
-	brcms_b_copyfrom_objmem(wlc_hw, offset, buf, len, OBJADDR_SHM_SEL)
-#define brcms_b_copyto_shm(wlc_hw, offset, buf, len)                   \
-	brcms_b_copyto_objmem(wlc_hw, offset, buf, len, OBJADDR_SHM_SEL)
-
-extern void brcms_b_core_phypll_reset(struct brcms_hardware *wlc_hw);
-extern void brcms_b_core_phypll_ctl(struct brcms_hardware *wlc_hw, bool on);
-extern void brcms_b_phyclk_fgc(struct brcms_hardware *wlc_hw, bool clk);
-extern void brcms_b_macphyclk_set(struct brcms_hardware *wlc_hw, bool clk);
-extern void brcms_b_phy_reset(struct brcms_hardware *wlc_hw);
-extern void brcms_b_corereset(struct brcms_hardware *wlc_hw, u32 flags);
-extern void brcms_b_reset(struct brcms_hardware *wlc_hw);
-extern void brcms_b_init(struct brcms_hardware *wlc_hw, chanspec_t chanspec,
-			  bool mute);
-extern int brcms_b_up_prep(struct brcms_hardware *wlc_hw);
-extern int brcms_b_up_finish(struct brcms_hardware *wlc_hw);
-extern int brcms_b_bmac_down_prep(struct brcms_hardware *wlc_hw);
-extern int brcms_b_down_finish(struct brcms_hardware *wlc_hw);
-extern void brcms_b_switch_macfreq(struct brcms_hardware *wlc_hw, u8 spurmode);
-
-/* chanspec, ucode interface */
-extern void brcms_b_set_chanspec(struct brcms_hardware *wlc_hw,
-				  chanspec_t chanspec,
-				  bool mute, struct txpwr_limits *txpwr);
-
-extern int brcms_b_xmtfifo_sz_get(struct brcms_hardware *wlc_hw, uint fifo,
-				   uint *blocks);
-extern void brcms_b_mhf(struct brcms_hardware *wlc_hw, u8 idx, u16 mask,
-			 u16 val, int bands);
-extern void brcms_b_mctrl(struct brcms_hardware *wlc_hw, u32 mask, u32 val);
-extern u16 brcms_b_mhf_get(struct brcms_hardware *wlc_hw, u8 idx, int bands);
-extern void brcms_b_txant_set(struct brcms_hardware *wlc_hw, u16 phytxant);
-extern u16 brcms_b_get_txant(struct brcms_hardware *wlc_hw);
-extern void brcms_b_antsel_type_set(struct brcms_hardware *wlc_hw,
-				     u8 antsel_type);
-extern int brcms_b_state_get(struct brcms_hardware *wlc_hw,
-			      struct brcms_b_state *state);
-extern void brcms_b_write_shm(struct brcms_hardware *wlc_hw, uint offset,
-			      u16 v);
-extern u16 brcms_b_read_shm(struct brcms_hardware *wlc_hw, uint offset);
-extern void brcms_b_write_template_ram(struct brcms_hardware *wlc_hw,
-				       int offset, int len, void *buf);
-extern void brcms_b_copyfrom_vars(struct brcms_hardware *wlc_hw, char **buf,
-				   uint *len);
-
-extern void brcms_b_hw_etheraddr(struct brcms_hardware *wlc_hw,
-				  u8 *ea);
-
-extern bool brcms_b_radio_read_hwdisabled(struct brcms_hardware *wlc_hw);
-extern void brcms_b_set_shortslot(struct brcms_hardware *wlc_hw,
-				  bool shortslot);
-extern void brcms_b_band_stf_ss_set(struct brcms_hardware *wlc_hw,
-				    u8 stf_mode);
-
-extern void brcms_b_wait_for_wake(struct brcms_hardware *wlc_hw);
-
-extern void brcms_c_ucode_wake_override_set(struct brcms_hardware *wlc_hw,
-					u32 override_bit);
-extern void brcms_c_ucode_wake_override_clear(struct brcms_hardware *wlc_hw,
-					  u32 override_bit);
-
-extern void brcms_b_set_addrmatch(struct brcms_hardware *wlc_hw,
-				   int match_reg_offset,
-				   const u8 *addr);
-extern void brcms_b_write_hw_bcntemplates(struct brcms_hardware *wlc_hw,
-					   void *bcn, int len, bool both);
-
-extern void brcms_b_read_tsf(struct brcms_hardware *wlc_hw, u32 *tsf_l_ptr,
-			      u32 *tsf_h_ptr);
-extern void brcms_b_set_cwmin(struct brcms_hardware *wlc_hw, u16 newmin);
-extern void brcms_b_set_cwmax(struct brcms_hardware *wlc_hw, u16 newmax);
-
-extern void brcms_b_retrylimit_upd(struct brcms_hardware *wlc_hw, u16 SRL,
-				    u16 LRL);
-
-extern void brcms_b_fifoerrors(struct brcms_hardware *wlc_hw);
-
-
-/* API for BMAC driver (e.g. wlc_phy.c etc) */
-
-extern void brcms_b_bw_set(struct brcms_hardware *wlc_hw, u16 bw);
-extern void brcms_b_pllreq(struct brcms_hardware *wlc_hw, bool set,
-			    mbool req_bit);
-extern void brcms_b_hw_up(struct brcms_hardware *wlc_hw);
-extern u16 brcms_b_rate_shm_offset(struct brcms_hardware *wlc_hw, u8 rate);
-extern void brcms_b_antsel_set(struct brcms_hardware *wlc_hw,
-			       u32 antsel_avail);
-
-#endif /* _BRCM_BOTTOM_MAC_H_ */
diff --git a/drivers/staging/brcm80211/brcmsmac/alloc.h b/drivers/staging/brcm80211/brcmsmac/brcms_trace_events.c
index f465d3043030..52fc9eeb5fa5 100644
--- a/drivers/staging/brcm80211/brcmsmac/alloc.h
+++ b/drivers/staging/brcm80211/brcmsmac/brcms_trace_events.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2010 Broadcom Corporation
+ * Copyright (c) 2011 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
@@ -14,6 +14,10 @@
  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
-extern struct brcms_c_info *brcms_c_attach_malloc(uint unit, uint *err,
-						  uint devid);
-extern void brcms_c_detach_mfree(struct brcms_c_info *wlc);
+#include <linux/module.h> /* bug in tracepoint.h, it should include this */
+
+#ifndef __CHECKER__
+#include "mac80211_if.h"
+#define CREATE_TRACE_POINTS
+#include "brcms_trace_events.h"
+#endif
diff --git a/drivers/staging/brcm80211/brcmsmac/brcms_trace_events.h b/drivers/staging/brcm80211/brcmsmac/brcms_trace_events.h
new file mode 100644
index 000000000000..27dd73eef56d
--- /dev/null
+++ b/drivers/staging/brcm80211/brcmsmac/brcms_trace_events.h
@@ -0,0 +1,92 @@
+/*
+ * Copyright (c) 2011 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.
+ */
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM brcmsmac
+
+#if !defined(__TRACE_BRCMSMAC_H) || defined(TRACE_HEADER_MULTI_READ)
+
+#define __TRACE_BRCMSMAC_H
+
+#include <linux/tracepoint.h>
+#include "mac80211_if.h"
+
+#ifndef CONFIG_BRCMDBG
+#undef TRACE_EVENT
+#define TRACE_EVENT(name, proto, ...) \
+static inline void trace_ ## name(proto) {}
+#endif
+
+/*
+ * We define a tracepoint, its arguments, its printk format and its
+ * 'fast binary record' layout.
+ */
+TRACE_EVENT(brcms_timer,
+	/* TPPROTO is the prototype of the function called by this tracepoint */
+	TP_PROTO(struct brcms_timer *t),
+	/*
+	 * TPARGS(firstarg, p) are the parameters names, same as found in the
+	 * prototype.
+	 */
+	TP_ARGS(t),
+	/*
+	 * Fast binary tracing: define the trace record via TP_STRUCT__entry().
+	 * You can think about it like a regular C structure local variable
+	 * definition.
+	 */
+	TP_STRUCT__entry(
+		__field(uint, ms)
+		__field(uint, set)
+		__field(uint, periodic)
+	),
+	TP_fast_assign(
+		__entry->ms = t->ms;
+		__entry->set = t->set;
+		__entry->periodic = t->periodic;
+	),
+	TP_printk(
+		"ms=%u set=%u periodic=%u",
+		__entry->ms, __entry->set, __entry->periodic
+	)
+);
+
+TRACE_EVENT(brcms_dpc,
+	TP_PROTO(unsigned long data),
+	TP_ARGS(data),
+	TP_STRUCT__entry(
+		__field(unsigned long, data)
+	),
+	TP_fast_assign(
+		__entry->data = data;
+	),
+	TP_printk(
+		"data=%p",
+		(void *)__entry->data
+	)
+);
+
+#endif /* __TRACE_BRCMSMAC_H */
+
+#ifdef CONFIG_BRCMDBG
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE brcms_trace_events
+
+#include <trace/define_trace.h>
+
+#endif /* CONFIG_BRCMDBG */
diff --git a/drivers/staging/brcm80211/brcmsmac/channel.c b/drivers/staging/brcm80211/brcmsmac/channel.c
index f59693e1d8a2..89ad1b7dab8f 100644
--- a/drivers/staging/brcm80211/brcmsmac/channel.c
+++ b/drivers/staging/brcm80211/brcmsmac/channel.c
@@ -20,24 +20,138 @@
 #include <defs.h>
 #include "pub.h"
 #include "phy/phy_hal.h"
-#include "bmac.h"
 #include "main.h"
 #include "stf.h"
 #include "channel.h"
 
-#define	VALID_CHANNEL20_DB(wlc, val) brcms_c_valid_channel20_db((wlc)->cmi, val)
-#define	VALID_CHANNEL20_IN_BAND(wlc, bandunit, val) \
-	brcms_c_valid_channel20_in_band((wlc)->cmi, bandunit, val)
-#define	VALID_CHANNEL20(wlc, val) brcms_c_valid_channel20((wlc)->cmi, val)
+/* QDB() macro takes a dB value and converts to a quarter dB value */
+#define QDB(n) ((n) * BRCMS_TXPWR_DB_FACTOR)
+
+#define  LOCALE_CHAN_01_11	 (1<<0)
+#define  LOCALE_CHAN_12_13	 (1<<1)
+#define  LOCALE_CHAN_14		 (1<<2)
+#define  LOCALE_SET_5G_LOW_JP1   (1<<3)	/* 34-48, step 2 */
+#define  LOCALE_SET_5G_LOW_JP2   (1<<4)	/* 34-46, step 4 */
+#define  LOCALE_SET_5G_LOW1      (1<<5)	/* 36-48, step 4 */
+#define  LOCALE_SET_5G_LOW2      (1<<6)	/* 52 */
+#define  LOCALE_SET_5G_LOW3      (1<<7)	/* 56-64, step 4 */
+#define  LOCALE_SET_5G_MID1      (1<<8)	/* 100-116, step 4 */
+#define  LOCALE_SET_5G_MID2	 (1<<9)	/* 120-124, step 4 */
+#define  LOCALE_SET_5G_MID3      (1<<10)	/* 128 */
+#define  LOCALE_SET_5G_HIGH1     (1<<11)	/* 132-140, step 4 */
+#define  LOCALE_SET_5G_HIGH2     (1<<12)	/* 149-161, step 4 */
+#define  LOCALE_SET_5G_HIGH3     (1<<13)	/* 165 */
+#define  LOCALE_CHAN_52_140_ALL  (1<<14)
+#define  LOCALE_SET_5G_HIGH4     (1<<15)	/* 184-216 */
+
+#define  LOCALE_CHAN_36_64	(LOCALE_SET_5G_LOW1 | \
+				 LOCALE_SET_5G_LOW2 | \
+				 LOCALE_SET_5G_LOW3)
+#define  LOCALE_CHAN_52_64	(LOCALE_SET_5G_LOW2 | LOCALE_SET_5G_LOW3)
+#define  LOCALE_CHAN_100_124	(LOCALE_SET_5G_MID1 | LOCALE_SET_5G_MID2)
+#define  LOCALE_CHAN_100_140	(LOCALE_SET_5G_MID1 | LOCALE_SET_5G_MID2 | \
+				  LOCALE_SET_5G_MID3 | LOCALE_SET_5G_HIGH1)
+#define  LOCALE_CHAN_149_165	(LOCALE_SET_5G_HIGH2 | LOCALE_SET_5G_HIGH3)
+#define  LOCALE_CHAN_184_216	LOCALE_SET_5G_HIGH4
+
+#define  LOCALE_CHAN_01_14	(LOCALE_CHAN_01_11 | \
+				 LOCALE_CHAN_12_13 | \
+				 LOCALE_CHAN_14)
+
+#define  LOCALE_RADAR_SET_NONE		  0
+#define  LOCALE_RADAR_SET_1		  1
+
+#define  LOCALE_RESTRICTED_NONE		  0
+#define  LOCALE_RESTRICTED_SET_2G_SHORT   1
+#define  LOCALE_RESTRICTED_CHAN_165       2
+#define  LOCALE_CHAN_ALL_5G		  3
+#define  LOCALE_RESTRICTED_JAPAN_LEGACY   4
+#define  LOCALE_RESTRICTED_11D_2G	  5
+#define  LOCALE_RESTRICTED_11D_5G	  6
+#define  LOCALE_RESTRICTED_LOW_HI	  7
+#define  LOCALE_RESTRICTED_12_13_14	  8
+
+#define LOCALE_2G_IDX_i			0
+#define LOCALE_5G_IDX_11		0
+#define LOCALE_MIMO_IDX_bn		0
+#define LOCALE_MIMO_IDX_11n		0
+
+/* max of BAND_5G_PWR_LVLS and 6 for 2.4 GHz */
+#define BRCMS_MAXPWR_TBL_SIZE		6
+/* max of BAND_5G_PWR_LVLS and 14 for 2.4 GHz */
+#define BRCMS_MAXPWR_MIMO_TBL_SIZE	14
+
+/* power level in group of 2.4GHz band channels:
+ * maxpwr[0] - CCK  channels [1]
+ * maxpwr[1] - CCK  channels [2-10]
+ * maxpwr[2] - CCK  channels [11-14]
+ * maxpwr[3] - OFDM channels [1]
+ * maxpwr[4] - OFDM channels [2-10]
+ * maxpwr[5] - OFDM channels [11-14]
+ */
+
+/* maxpwr mapping to 5GHz band channels:
+ * maxpwr[0] - channels [34-48]
+ * maxpwr[1] - channels [52-60]
+ * maxpwr[2] - channels [62-64]
+ * maxpwr[3] - channels [100-140]
+ * maxpwr[4] - channels [149-165]
+ */
+#define BAND_5G_PWR_LVLS	5	/* 5 power levels for 5G */
+
+#define LC(id)	LOCALE_MIMO_IDX_ ## id
+
+#define LC_2G(id)	LOCALE_2G_IDX_ ## id
+
+#define LC_5G(id)	LOCALE_5G_IDX_ ## id
+
+#define LOCALES(band2, band5, mimo2, mimo5) \
+		{LC_2G(band2), LC_5G(band5), LC(mimo2), LC(mimo5)}
+
+/* macro to get 2.4 GHz channel group index for tx power */
+#define CHANNEL_POWER_IDX_2G_CCK(c) (((c) < 2) ? 0 : (((c) < 11) ? 1 : 2))
+#define CHANNEL_POWER_IDX_2G_OFDM(c) (((c) < 2) ? 3 : (((c) < 11) ? 4 : 5))
+
+/* macro to get 5 GHz channel group index for tx power */
+#define CHANNEL_POWER_IDX_5G(c) (((c) < 52) ? 0 : \
+				 (((c) < 62) ? 1 : \
+				 (((c) < 100) ? 2 : \
+				 (((c) < 149) ? 3 : 4))))
+
+#define ISDFS_EU(fl)		(((fl) & BRCMS_DFS_EU) == BRCMS_DFS_EU)
 
 struct brcms_cm_band {
-	u8 locale_flags;	/* struct locale_info flags */
-	chanvec_t valid_channels;	/* List of valid channels in the country */
-	const chanvec_t *restricted_channels;	/* List of restricted use channels */
-	const chanvec_t *radar_channels;	/* List of radar sensitive channels */
+	/* struct locale_info flags */
+	u8 locale_flags;
+	/* List of valid channels in the country */
+	struct brcms_chanvec valid_channels;
+	/* List of restricted use channels */
+	const struct brcms_chanvec *restricted_channels;
+	/* List of radar sensitive channels */
+	const struct brcms_chanvec *radar_channels;
 	u8 PAD[8];
 };
 
+ /* locale per-channel tx power limits for MIMO frames
+  * maxpwr arrays are index by channel for 2.4 GHz limits, and
+  * by sub-band for 5 GHz limits using CHANNEL_POWER_IDX_5G(channel)
+  */
+struct locale_mimo_info {
+	/* tx 20 MHz power limits, qdBm units */
+	s8 maxpwr20[BRCMS_MAXPWR_MIMO_TBL_SIZE];
+	/* tx 40 MHz power limits, qdBm units */
+	s8 maxpwr40[BRCMS_MAXPWR_MIMO_TBL_SIZE];
+	u8 flags;
+};
+
+/* Country names and abbreviations with locale defined from ISO 3166 */
+struct country_info {
+	const u8 locale_2G;	/* 2.4G band locale */
+	const u8 locale_5G;	/* 5G band locale */
+	const u8 locale_mimo_2G;	/* 2.4G mimo info */
+	const u8 locale_mimo_5G;	/* 5G mimo info */
+};
+
 struct brcms_cm_info {
 	struct brcms_pub *pub;
 	struct brcms_c_info *wlc;
@@ -50,63 +164,23 @@ struct brcms_cm_info {
 	/* per-band state (one per phy/radio) */
 	struct brcms_cm_band bandstate[MAXBANDS];
 	/* quiet channels currently for radar sensitivity or 11h support */
-	chanvec_t quiet_channels;	/* channels on which we cannot transmit */
+	/* channels on which we cannot transmit */
+	struct brcms_chanvec quiet_channels;
 };
 
-static int brcms_c_channels_init(struct brcms_cm_info *wlc_cm,
-			     const struct country_info *country);
-static void brcms_c_set_country_common(struct brcms_cm_info *wlc_cm,
-				   const char *country_abbrev,
-				   const char *ccode, uint regrev,
-				   const struct country_info *country);
-static int brcms_c_set_countrycode(struct brcms_cm_info *wlc_cm,
-				   const char *ccode);
-static int brcms_c_set_countrycode_rev(struct brcms_cm_info *wlc_cm,
-				   const char *country_abbrev,
-				   const char *ccode, int regrev);
-static int brcms_c_country_aggregate_map(struct brcms_cm_info *wlc_cm,
-				const char *ccode,
-				char *mapped_ccode, uint *mapped_regrev);
-
-static const struct country_info *
-brcms_c_country_lookup_direct(const char *ccode, uint regrev);
-
-static const struct country_info *
-brcms_c_countrycode_map(struct brcms_cm_info *wlc_cm,
-			const char *ccode, char *mapped_ccode,
-			uint *mapped_regrev);
-
-static void brcms_c_channels_commit(struct brcms_cm_info *wlc_cm);
-static void brcms_c_quiet_channels_reset(struct brcms_cm_info *wlc_cm);
-static bool brcms_c_quiet_chanspec(struct brcms_cm_info *wlc_cm,
-				   chanspec_t chspec);
-static bool brcms_c_valid_channel20_db(struct brcms_cm_info *wlc_cm, uint val);
-static bool brcms_c_valid_channel20_in_band(struct brcms_cm_info *wlc_cm,
-					    uint bandunit, uint val);
-static bool brcms_c_valid_channel20(struct brcms_cm_info *wlc_cm, uint val);
-
-static const struct country_info *
-brcms_c_country_lookup(struct brcms_c_info *wlc, const char *ccode);
-
-static void brcms_c_locale_get_channels(const struct locale_info *locale,
-				    chanvec_t *valid_channels);
-static const struct locale_info *brcms_c_get_locale_2g(u8 locale_idx);
-static const struct locale_info *brcms_c_get_locale_5g(u8 locale_idx);
-static bool brcms_c_japan(struct brcms_c_info *wlc);
-static bool brcms_c_japan_ccode(const char *ccode);
-static void brcms_c_channel_min_txpower_limits_with_local_constraint(
-	struct brcms_cm_info *wlc_cm, struct txpwr_limits *txpwr,
-	u8 local_constraint_qdbm);
-static void brcms_c_locale_add_channels(chanvec_t *target,
-				    const chanvec_t *channels);
-static const struct locale_mimo_info *brcms_c_get_mimo_2g(u8 locale_idx);
-static const struct locale_mimo_info *brcms_c_get_mimo_5g(u8 locale_idx);
-
-/* QDB() macro takes a dB value and converts to a quarter dB value */
-#ifdef QDB
-#undef QDB
-#endif
-#define QDB(n) ((n) * BRCMS_TXPWR_DB_FACTOR)
+/* locale channel and power info. */
+struct locale_info {
+	u32 valid_channels;
+	/* List of radar sensitive channels */
+	u8 radar_channels;
+	/* List of channels used only if APs are detected */
+	u8 restricted_channels;
+	/* Max tx pwr in qdBm for each sub-band */
+	s8 maxpwr[BRCMS_MAXPWR_TBL_SIZE];
+	/* Country IE advertised max tx pwr in dBm per sub-band */
+	s8 pub_maxpwr[BAND_5G_PWR_LVLS];
+	u8 flags;
+};
 
 /* Regulatory Matrix Spreadsheet (CLM) MIMO v3.7.9 */
 
@@ -115,7 +189,7 @@ static const struct locale_mimo_info *brcms_c_get_mimo_5g(u8 locale_idx);
  */
 
 /* No channels */
-static const chanvec_t chanvec_none = {
+static const struct brcms_chanvec chanvec_none = {
 	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
@@ -123,7 +197,7 @@ static const chanvec_t chanvec_none = {
 };
 
 /* All 2.4 GHz HW channels */
-const chanvec_t chanvec_all_2G = {
+static const struct brcms_chanvec chanvec_all_2G = {
 	{0xfe, 0x7f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
@@ -131,7 +205,7 @@ const chanvec_t chanvec_all_2G = {
 };
 
 /* All 5 GHz HW channels */
-const chanvec_t chanvec_all_5G = {
+static const struct brcms_chanvec chanvec_all_5G = {
 	{0x00, 0x00, 0x00, 0x00, 0x54, 0x55, 0x11, 0x11,
 	 0x01, 0x00, 0x00, 0x00, 0x10, 0x11, 0x11, 0x11,
 	 0x11, 0x11, 0x20, 0x22, 0x22, 0x00, 0x00, 0x11,
@@ -142,13 +216,11 @@ const chanvec_t chanvec_all_5G = {
  * Radar channel sets
  */
 
-/* No radar */
-#define radar_set_none chanvec_none
-
-static const chanvec_t radar_set1 = {	/* Channels 52 - 64, 100 - 140 */
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x11,	/* 52 - 60 */
-	 0x01, 0x00, 0x00, 0x00, 0x10, 0x11, 0x11, 0x11,	/* 64, 100 - 124 */
-	 0x11, 0x11, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,	/* 128 - 140 */
+/* Channels 52 - 64, 100 - 140 */
+static const struct brcms_chanvec radar_set1 = {
+	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x11,  /* 52 - 60 */
+	 0x01, 0x00, 0x00, 0x00, 0x10, 0x11, 0x11, 0x11,  /* 64, 100 - 124 */
+	 0x11, 0x11, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  /* 128 - 140 */
 	 0x00, 0x00, 0x00, 0x00}
 };
 
@@ -156,10 +228,8 @@ static const chanvec_t radar_set1 = {	/* Channels 52 - 64, 100 - 140 */
  * Restricted channel sets
  */
 
-#define restricted_set_none chanvec_none
-
 /* Channels 34, 38, 42, 46 */
-static const chanvec_t restricted_set_japan_legacy = {
+static const struct brcms_chanvec restricted_set_japan_legacy = {
 	{0x00, 0x00, 0x00, 0x00, 0x44, 0x44, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
@@ -167,7 +237,7 @@ static const chanvec_t restricted_set_japan_legacy = {
 };
 
 /* Channels 12, 13 */
-static const chanvec_t restricted_set_2g_short = {
+static const struct brcms_chanvec restricted_set_2g_short = {
 	{0x00, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
@@ -175,7 +245,7 @@ static const chanvec_t restricted_set_2g_short = {
 };
 
 /* Channel 165 */
-static const chanvec_t restricted_chan_165 = {
+static const struct brcms_chanvec restricted_chan_165 = {
 	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00,
@@ -183,7 +253,7 @@ static const chanvec_t restricted_chan_165 = {
 };
 
 /* Channels 36 - 48 & 149 - 165 */
-static const chanvec_t restricted_low_hi = {
+static const struct brcms_chanvec restricted_low_hi = {
 	{0x00, 0x00, 0x00, 0x00, 0x10, 0x11, 0x01, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x20, 0x22, 0x22, 0x00, 0x00, 0x00,
@@ -191,61 +261,21 @@ static const chanvec_t restricted_low_hi = {
 };
 
 /* Channels 12 - 14 */
-static const chanvec_t restricted_set_12_13_14 = {
+static const struct brcms_chanvec restricted_set_12_13_14 = {
 	{0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-#define  LOCALE_CHAN_01_11	 (1<<0)
-#define  LOCALE_CHAN_12_13	 (1<<1)
-#define  LOCALE_CHAN_14		 (1<<2)
-#define  LOCALE_SET_5G_LOW_JP1   (1<<3)	/* 34-48, step 2 */
-#define  LOCALE_SET_5G_LOW_JP2   (1<<4)	/* 34-46, step 4 */
-#define  LOCALE_SET_5G_LOW1      (1<<5)	/* 36-48, step 4 */
-#define  LOCALE_SET_5G_LOW2      (1<<6)	/* 52 */
-#define  LOCALE_SET_5G_LOW3      (1<<7)	/* 56-64, step 4 */
-#define  LOCALE_SET_5G_MID1      (1<<8)	/* 100-116, step 4 */
-#define  LOCALE_SET_5G_MID2	 (1<<9)	/* 120-124, step 4 */
-#define  LOCALE_SET_5G_MID3      (1<<10)	/* 128 */
-#define  LOCALE_SET_5G_HIGH1     (1<<11)	/* 132-140, step 4 */
-#define  LOCALE_SET_5G_HIGH2     (1<<12)	/* 149-161, step 4 */
-#define  LOCALE_SET_5G_HIGH3     (1<<13)	/* 165 */
-#define  LOCALE_CHAN_52_140_ALL  (1<<14)
-#define  LOCALE_SET_5G_HIGH4     (1<<15)	/* 184-216 */
-
-#define  LOCALE_CHAN_36_64       (LOCALE_SET_5G_LOW1 | LOCALE_SET_5G_LOW2 | LOCALE_SET_5G_LOW3)
-#define  LOCALE_CHAN_52_64       (LOCALE_SET_5G_LOW2 | LOCALE_SET_5G_LOW3)
-#define  LOCALE_CHAN_100_124	 (LOCALE_SET_5G_MID1 | LOCALE_SET_5G_MID2)
-#define  LOCALE_CHAN_100_140     \
-	(LOCALE_SET_5G_MID1 | LOCALE_SET_5G_MID2 | LOCALE_SET_5G_MID3 | LOCALE_SET_5G_HIGH1)
-#define  LOCALE_CHAN_149_165     (LOCALE_SET_5G_HIGH2 | LOCALE_SET_5G_HIGH3)
-#define  LOCALE_CHAN_184_216     LOCALE_SET_5G_HIGH4
-
-#define  LOCALE_CHAN_01_14	(LOCALE_CHAN_01_11 | LOCALE_CHAN_12_13 | LOCALE_CHAN_14)
-
-#define  LOCALE_RADAR_SET_NONE		  0
-#define  LOCALE_RADAR_SET_1		  1
-
-#define  LOCALE_RESTRICTED_NONE		  0
-#define  LOCALE_RESTRICTED_SET_2G_SHORT   1
-#define  LOCALE_RESTRICTED_CHAN_165       2
-#define  LOCALE_CHAN_ALL_5G		  3
-#define  LOCALE_RESTRICTED_JAPAN_LEGACY   4
-#define  LOCALE_RESTRICTED_11D_2G	  5
-#define  LOCALE_RESTRICTED_11D_5G	  6
-#define  LOCALE_RESTRICTED_LOW_HI	  7
-#define  LOCALE_RESTRICTED_12_13_14	  8
-
 /* global memory to provide working buffer for expanded locale */
 
-static const chanvec_t *g_table_radar_set[] = {
+static const struct brcms_chanvec *g_table_radar_set[] = {
 	&chanvec_none,
 	&radar_set1
 };
 
-static const chanvec_t *g_table_restricted_chan[] = {
+static const struct brcms_chanvec *g_table_restricted_chan[] = {
 	&chanvec_none,		/* restricted_set_none */
 	&restricted_set_2g_short,
 	&restricted_chan_165,
@@ -257,119 +287,119 @@ static const chanvec_t *g_table_restricted_chan[] = {
 	&restricted_set_12_13_14
 };
 
-static const chanvec_t locale_2g_01_11 = {
+static const struct brcms_chanvec locale_2g_01_11 = {
 	{0xfe, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-static const chanvec_t locale_2g_12_13 = {
+static const struct brcms_chanvec locale_2g_12_13 = {
 	{0x00, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-static const chanvec_t locale_2g_14 = {
+static const struct brcms_chanvec locale_2g_14 = {
 	{0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-static const chanvec_t locale_5g_LOW_JP1 = {
+static const struct brcms_chanvec locale_5g_LOW_JP1 = {
 	{0x00, 0x00, 0x00, 0x00, 0x54, 0x55, 0x01, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-static const chanvec_t locale_5g_LOW_JP2 = {
+static const struct brcms_chanvec locale_5g_LOW_JP2 = {
 	{0x00, 0x00, 0x00, 0x00, 0x44, 0x44, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-static const chanvec_t locale_5g_LOW1 = {
+static const struct brcms_chanvec locale_5g_LOW1 = {
 	{0x00, 0x00, 0x00, 0x00, 0x10, 0x11, 0x01, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-static const chanvec_t locale_5g_LOW2 = {
+static const struct brcms_chanvec locale_5g_LOW2 = {
 	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-static const chanvec_t locale_5g_LOW3 = {
+static const struct brcms_chanvec locale_5g_LOW3 = {
 	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11,
 	 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-static const chanvec_t locale_5g_MID1 = {
+static const struct brcms_chanvec locale_5g_MID1 = {
 	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x10, 0x11, 0x11, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-static const chanvec_t locale_5g_MID2 = {
+static const struct brcms_chanvec locale_5g_MID2 = {
 	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-static const chanvec_t locale_5g_MID3 = {
+static const struct brcms_chanvec locale_5g_MID3 = {
 	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-static const chanvec_t locale_5g_HIGH1 = {
+static const struct brcms_chanvec locale_5g_HIGH1 = {
 	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x10, 0x11, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-static const chanvec_t locale_5g_HIGH2 = {
+static const struct brcms_chanvec locale_5g_HIGH2 = {
 	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x20, 0x22, 0x02, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-static const chanvec_t locale_5g_HIGH3 = {
+static const struct brcms_chanvec locale_5g_HIGH3 = {
 	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-static const chanvec_t locale_5g_52_140_ALL = {
+static const struct brcms_chanvec locale_5g_52_140_ALL = {
 	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x11,
 	 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11,
 	 0x11, 0x11, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00}
 };
 
-static const chanvec_t locale_5g_HIGH4 = {
+static const struct brcms_chanvec locale_5g_HIGH4 = {
 	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11,
 	 0x11, 0x11, 0x11, 0x11}
 };
 
-static const chanvec_t *g_table_locale_base[] = {
+static const struct brcms_chanvec *g_table_locale_base[] = {
 	&locale_2g_01_11,
 	&locale_2g_12_13,
 	&locale_2g_14,
@@ -388,27 +418,25 @@ static const chanvec_t *g_table_locale_base[] = {
 	&locale_5g_HIGH4
 };
 
-static void brcms_c_locale_add_channels(chanvec_t *target,
-				    const chanvec_t *channels)
+static void brcms_c_locale_add_channels(struct brcms_chanvec *target,
+				    const struct brcms_chanvec *channels)
 {
 	u8 i;
-	for (i = 0; i < sizeof(chanvec_t); i++) {
+	for (i = 0; i < sizeof(struct brcms_chanvec); i++)
 		target->vec[i] |= channels->vec[i];
-	}
 }
 
 static void brcms_c_locale_get_channels(const struct locale_info *locale,
-				    chanvec_t *channels)
+				    struct brcms_chanvec *channels)
 {
 	u8 i;
 
-	memset(channels, 0, sizeof(chanvec_t));
+	memset(channels, 0, sizeof(struct brcms_chanvec));
 
 	for (i = 0; i < ARRAY_SIZE(g_table_locale_base); i++) {
-		if (locale->valid_channels & (1 << i)) {
+		if (locale->valid_channels & (1 << i))
 			brcms_c_locale_add_channels(channels,
 						g_table_locale_base[i]);
-		}
 	}
 }
 
@@ -438,12 +466,10 @@ static const struct locale_info locale_11 = {
 	BRCMS_EIRP | BRCMS_DFS_EU
 };
 
-#define LOCALE_2G_IDX_i			0
 static const struct locale_info *g_locale_2g_table[] = {
 	&locale_i
 };
 
-#define LOCALE_5G_IDX_11	0
 static const struct locale_info *g_locale_5g_table[] = {
 	&locale_11
 };
@@ -461,9 +487,6 @@ static const struct locale_mimo_info locale_bn = {
 	0
 };
 
-/* locale mimo 2g indexes */
-#define LOCALE_MIMO_IDX_bn			0
-
 static const struct locale_mimo_info *g_mimo_2g_table[] = {
 	&locale_bn
 };
@@ -477,28 +500,10 @@ static const struct locale_mimo_info locale_11n = {
 	0
 };
 
-#define LOCALE_MIMO_IDX_11n			0
 static const struct locale_mimo_info *g_mimo_5g_table[] = {
 	&locale_11n
 };
 
-#ifdef LC
-#undef LC
-#endif
-#define LC(id)	LOCALE_MIMO_IDX_ ## id
-
-#ifdef LC_2G
-#undef LC_2G
-#endif
-#define LC_2G(id)	LOCALE_2G_IDX_ ## id
-
-#ifdef LC_5G
-#undef LC_5G
-#endif
-#define LC_5G(id)	LOCALE_5G_IDX_ ## id
-
-#define LOCALES(band2, band5, mimo2, mimo5)     {LC_2G(band2), LC_5G(band5), LC(mimo2), LC(mimo5)}
-
 static const struct {
 	char abbrev[BRCM_CNTRY_BUF_SZ];	/* country abbreviation */
 	struct country_info country;
@@ -586,201 +591,68 @@ struct chan20_info chan20_info[] = {
 
 static const struct locale_info *brcms_c_get_locale_2g(u8 locale_idx)
 {
-	if (locale_idx >= ARRAY_SIZE(g_locale_2g_table)) {
+	if (locale_idx >= ARRAY_SIZE(g_locale_2g_table))
 		return NULL; /* error condition */
-	}
+
 	return g_locale_2g_table[locale_idx];
 }
 
 static const struct locale_info *brcms_c_get_locale_5g(u8 locale_idx)
 {
-	if (locale_idx >= ARRAY_SIZE(g_locale_5g_table)) {
+	if (locale_idx >= ARRAY_SIZE(g_locale_5g_table))
 		return NULL; /* error condition */
-	}
+
 	return g_locale_5g_table[locale_idx];
 }
 
 static const struct locale_mimo_info *brcms_c_get_mimo_2g(u8 locale_idx)
 {
-	if (locale_idx >= ARRAY_SIZE(g_mimo_2g_table)) {
+	if (locale_idx >= ARRAY_SIZE(g_mimo_2g_table))
 		return NULL;
-	}
+
 	return g_mimo_2g_table[locale_idx];
 }
 
 static const struct locale_mimo_info *brcms_c_get_mimo_5g(u8 locale_idx)
 {
-	if (locale_idx >= ARRAY_SIZE(g_mimo_5g_table)) {
-		return NULL;
-	}
-	return g_mimo_5g_table[locale_idx];
-}
-
-struct brcms_cm_info *brcms_c_channel_mgr_attach(struct brcms_c_info *wlc)
-{
-	struct brcms_cm_info *wlc_cm;
-	char country_abbrev[BRCM_CNTRY_BUF_SZ];
-	const struct country_info *country;
-	struct brcms_pub *pub = wlc->pub;
-	char *ccode;
-
-	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
-
-	wlc_cm = kzalloc(sizeof(struct brcms_cm_info), GFP_ATOMIC);
-	if (wlc_cm == NULL) {
-		wiphy_err(wlc->wiphy, "wl%d: %s: out of memory", pub->unit,
-			  __func__);
+	if (locale_idx >= ARRAY_SIZE(g_mimo_5g_table))
 		return NULL;
-	}
-	wlc_cm->pub = pub;
-	wlc_cm->wlc = wlc;
-	wlc->cmi = wlc_cm;
-
-	/* store the country code for passing up as a regulatory hint */
-	ccode = getvar(wlc->pub->vars, "ccode");
-	if (ccode) {
-		strncpy(wlc->pub->srom_ccode, ccode, BRCM_CNTRY_BUF_SZ - 1);
-	}
-
-	/* internal country information which must match regulatory constraints in firmware */
-	memset(country_abbrev, 0, BRCM_CNTRY_BUF_SZ);
-	strncpy(country_abbrev, "X2", sizeof(country_abbrev) - 1);
-	country = brcms_c_country_lookup(wlc, country_abbrev);
-
-	/* save default country for exiting 11d regulatory mode */
-	strncpy(wlc->country_default, country_abbrev, BRCM_CNTRY_BUF_SZ - 1);
-
-	/* initialize autocountry_default to driver default */
-	strncpy(wlc->autocountry_default, "X2", BRCM_CNTRY_BUF_SZ - 1);
-
-	brcms_c_set_countrycode(wlc_cm, country_abbrev);
-
-	return wlc_cm;
-}
-
-void brcms_c_channel_mgr_detach(struct brcms_cm_info *wlc_cm)
-{
-	kfree(wlc_cm);
-}
 
-u8
-brcms_c_channel_locale_flags_in_band(struct brcms_cm_info *wlc_cm,
-				     uint bandunit)
-{
-	return wlc_cm->bandstate[bandunit].locale_flags;
-}
-
-/* set the driver's current country and regulatory information using a country code
- * as the source. Lookup built in country information found with the country code.
- */
-static int
-brcms_c_set_countrycode(struct brcms_cm_info *wlc_cm, const char *ccode)
-{
-	char country_abbrev[BRCM_CNTRY_BUF_SZ];
-	strncpy(country_abbrev, ccode, BRCM_CNTRY_BUF_SZ);
-	return brcms_c_set_countrycode_rev(wlc_cm, country_abbrev, ccode, -1);
+	return g_mimo_5g_table[locale_idx];
 }
 
 static int
-brcms_c_set_countrycode_rev(struct brcms_cm_info *wlc_cm,
-			const char *country_abbrev,
-			const char *ccode, int regrev)
+brcms_c_country_aggregate_map(struct brcms_cm_info *wlc_cm, const char *ccode,
+			  char *mapped_ccode, uint *mapped_regrev)
 {
-	const struct country_info *country;
-	char mapped_ccode[BRCM_CNTRY_BUF_SZ];
-	uint mapped_regrev;
-
-	/* if regrev is -1, lookup the mapped country code,
-	 * otherwise use the ccode and regrev directly
-	 */
-	if (regrev == -1) {
-		/* map the country code to a built-in country code, regrev, and country_info */
-		country =
-		    brcms_c_countrycode_map(wlc_cm, ccode, mapped_ccode,
-					&mapped_regrev);
-	} else {
-		/* find the matching built-in country definition */
-		country = brcms_c_country_lookup_direct(ccode, regrev);
-		strncpy(mapped_ccode, ccode, BRCM_CNTRY_BUF_SZ);
-		mapped_regrev = regrev;
-	}
-
-	if (country == NULL)
-		return -EINVAL;
-
-	/* set the driver state for the country */
-	brcms_c_set_country_common(wlc_cm, country_abbrev, mapped_ccode,
-			       mapped_regrev, country);
-
-	return 0;
+	return false;
 }
 
-/* set the driver's current country and regulatory information using a country code
- * as the source. Look up built in country information found with the country code.
+/* Lookup a country info structure from a null terminated country
+ * abbreviation and regrev directly with no translation.
  */
-static void
-brcms_c_set_country_common(struct brcms_cm_info *wlc_cm,
-		       const char *country_abbrev,
-		       const char *ccode, uint regrev,
-		       const struct country_info *country)
+static const struct country_info *
+brcms_c_country_lookup_direct(const char *ccode, uint regrev)
 {
-	const struct locale_mimo_info *li_mimo;
-	const struct locale_info *locale;
-	struct brcms_c_info *wlc = wlc_cm->wlc;
-	char prev_country_abbrev[BRCM_CNTRY_BUF_SZ];
-
-	/* save current country state */
-	wlc_cm->country = country;
-
-	memset(&prev_country_abbrev, 0, BRCM_CNTRY_BUF_SZ);
-	strncpy(prev_country_abbrev, wlc_cm->country_abbrev,
-		BRCM_CNTRY_BUF_SZ - 1);
+	uint size, i;
 
-	strncpy(wlc_cm->country_abbrev, country_abbrev, BRCM_CNTRY_BUF_SZ - 1);
-	strncpy(wlc_cm->ccode, ccode, BRCM_CNTRY_BUF_SZ - 1);
-	wlc_cm->regrev = regrev;
+	/* Should just return 0 for single locale driver. */
+	/* Keep it this way in case we add more locales. (for now anyway) */
 
-	/* disable/restore nmode based on country regulations */
-	li_mimo = brcms_c_get_mimo_2g(country->locale_mimo_2G);
-	if (li_mimo && (li_mimo->flags & BRCMS_NO_MIMO)) {
-		brcms_c_set_nmode(wlc, OFF);
-		wlc->stf->no_cddstbc = true;
-	} else {
-		wlc->stf->no_cddstbc = false;
-		if (N_ENAB(wlc->pub) != wlc->protection->nmode_user)
-			brcms_c_set_nmode(wlc, wlc->protection->nmode_user);
-	}
+	/*
+	 * all other country def arrays are for regrev == 0, so if
+	 * regrev is non-zero, fail
+	 */
+	if (regrev > 0)
+		return NULL;
 
-	brcms_c_stf_ss_update(wlc, wlc->bandstate[BAND_2G_INDEX]);
-	brcms_c_stf_ss_update(wlc, wlc->bandstate[BAND_5G_INDEX]);
-	/* set or restore gmode as required by regulatory */
-	locale = brcms_c_get_locale_2g(country->locale_2G);
-	if (locale && (locale->flags & BRCMS_NO_OFDM)) {
-		brcms_c_set_gmode(wlc, GMODE_LEGACY_B, false);
-	} else {
-		brcms_c_set_gmode(wlc, wlc->protection->gmode_user, false);
+	/* find matched table entry from country code */
+	size = ARRAY_SIZE(cntry_locales);
+	for (i = 0; i < size; i++) {
+		if (strcmp(ccode, cntry_locales[i].abbrev) == 0)
+			return &cntry_locales[i].country;
 	}
-
-	brcms_c_channels_init(wlc_cm, country);
-
-	return;
-}
-
-/* Lookup a country info structure from a null terminated country code
- * The lookup is case sensitive.
- */
-static const struct country_info *
-brcms_c_country_lookup(struct brcms_c_info *wlc, const char *ccode)
-{
-	const struct country_info *country;
-	char mapped_ccode[BRCM_CNTRY_BUF_SZ];
-	uint mapped_regrev;
-
-	/* map the country code to a built-in country code, regrev, and country_info struct */
-	country = brcms_c_countrycode_map(wlc->cmi, ccode, mapped_ccode,
-					  &mapped_regrev);
-
-	return country;
+	return NULL;
 }
 
 static const struct country_info *
@@ -831,174 +703,67 @@ brcms_c_countrycode_map(struct brcms_cm_info *wlc_cm, const char *ccode,
 	return country;
 }
 
-static int
-brcms_c_country_aggregate_map(struct brcms_cm_info *wlc_cm, const char *ccode,
-			  char *mapped_ccode, uint *mapped_regrev)
-{
-	return false;
-}
-
-/* Lookup a country info structure from a null terminated country
- * abbreviation and regrev directly with no translation.
+/* Lookup a country info structure from a null terminated country code
+ * The lookup is case sensitive.
  */
 static const struct country_info *
-brcms_c_country_lookup_direct(const char *ccode, uint regrev)
-{
-	uint size, i;
-
-	/* Should just return 0 for single locale driver. */
-	/* Keep it this way in case we add more locales. (for now anyway) */
-
-	/* all other country def arrays are for regrev == 0, so if regrev is non-zero, fail */
-	if (regrev > 0)
-		return NULL;
-
-	/* find matched table entry from country code */
-	size = ARRAY_SIZE(cntry_locales);
-	for (i = 0; i < size; i++) {
-		if (strcmp(ccode, cntry_locales[i].abbrev) == 0) {
-			return &cntry_locales[i].country;
-		}
-	}
-	return NULL;
-}
-
-static int
-brcms_c_channels_init(struct brcms_cm_info *wlc_cm,
-		      const struct country_info *country)
-{
-	struct brcms_c_info *wlc = wlc_cm->wlc;
-	uint i, j;
-	struct brcms_band *band;
-	const struct locale_info *li;
-	chanvec_t sup_chan;
-	const struct locale_mimo_info *li_mimo;
-
-	band = wlc->band;
-	for (i = 0; i < NBANDS(wlc);
-	     i++, band = wlc->bandstate[OTHERBANDUNIT(wlc)]) {
-
-		li = BAND_5G(band->bandtype) ?
-		    brcms_c_get_locale_5g(country->locale_5G) :
-		    brcms_c_get_locale_2g(country->locale_2G);
-		wlc_cm->bandstate[band->bandunit].locale_flags = li->flags;
-		li_mimo = BAND_5G(band->bandtype) ?
-		    brcms_c_get_mimo_5g(country->locale_mimo_5G) :
-		    brcms_c_get_mimo_2g(country->locale_mimo_2G);
-
-		/* merge the mimo non-mimo locale flags */
-		wlc_cm->bandstate[band->bandunit].locale_flags |=
-		    li_mimo->flags;
-
-		wlc_cm->bandstate[band->bandunit].restricted_channels =
-		    g_table_restricted_chan[li->restricted_channels];
-		wlc_cm->bandstate[band->bandunit].radar_channels =
-		    g_table_radar_set[li->radar_channels];
-
-		/* set the channel availability,
-		 * masking out the channels that may not be supported on this phy
-		 */
-		wlc_phy_chanspec_band_validch(band->pi, band->bandtype,
-					      &sup_chan);
-		brcms_c_locale_get_channels(li,
-					&wlc_cm->bandstate[band->bandunit].
-					valid_channels);
-		for (j = 0; j < sizeof(chanvec_t); j++)
-			wlc_cm->bandstate[band->bandunit].valid_channels.
-			    vec[j] &= sup_chan.vec[j];
-	}
-
-	brcms_c_quiet_channels_reset(wlc_cm);
-	brcms_c_channels_commit(wlc_cm);
-
-	return 0;
-}
-
-/* Update the radio state (enable/disable) and tx power targets
- * based on a new set of channel/regulatory information
- */
-static void brcms_c_channels_commit(struct brcms_cm_info *wlc_cm)
+brcms_c_country_lookup(struct brcms_c_info *wlc, const char *ccode)
 {
-	struct brcms_c_info *wlc = wlc_cm->wlc;
-	uint chan;
-	struct txpwr_limits txpwr;
-
-	/* search for the existence of any valid channel */
-	for (chan = 0; chan < MAXCHANNEL; chan++) {
-		if (VALID_CHANNEL20_DB(wlc, chan)) {
-			break;
-		}
-	}
-	if (chan == MAXCHANNEL)
-		chan = INVCHANNEL;
-
-	/* based on the channel search above, set or clear WL_RADIO_COUNTRY_DISABLE */
-	if (chan == INVCHANNEL) {
-		/* country/locale with no valid channels, set the radio disable bit */
-		mboolset(wlc->pub->radio_disabled, WL_RADIO_COUNTRY_DISABLE);
-		wiphy_err(wlc->wiphy, "wl%d: %s: no valid channel for \"%s\" "
-			  "nbands %d bandlocked %d\n", wlc->pub->unit,
-			  __func__, wlc_cm->country_abbrev, NBANDS(wlc),
-			  wlc->bandlocked);
-	} else
-	    if (mboolisset(wlc->pub->radio_disabled,
-		WL_RADIO_COUNTRY_DISABLE)) {
-		/* country/locale with valid channel, clear the radio disable bit */
-		mboolclr(wlc->pub->radio_disabled, WL_RADIO_COUNTRY_DISABLE);
-	}
+	const struct country_info *country;
+	char mapped_ccode[BRCM_CNTRY_BUF_SZ];
+	uint mapped_regrev;
 
-	/* Now that the country abbreviation is set, if the radio supports 2G, then
-	 * set channel 14 restrictions based on the new locale.
+	/*
+	 * map the country code to a built-in country code, regrev, and
+	 * country_info struct
 	 */
-	if (NBANDS(wlc) > 1 || BAND_2G(wlc->band->bandtype)) {
-		wlc_phy_chanspec_ch14_widefilter_set(wlc->band->pi,
-						     brcms_c_japan(wlc) ? true :
-						     false);
-	}
+	country = brcms_c_countrycode_map(wlc->cmi, ccode, mapped_ccode,
+					  &mapped_regrev);
 
-	if (wlc->pub->up && chan != INVCHANNEL) {
-		brcms_c_channel_reg_limits(wlc_cm, wlc->chanspec, &txpwr);
-		brcms_c_channel_min_txpower_limits_with_local_constraint(wlc_cm,
-			&txpwr, BRCMS_TXPWR_MAX);
-		wlc_phy_txpower_limit_set(wlc->band->pi, &txpwr, wlc->chanspec);
-	}
+	return country;
 }
 
-/* reset the quiet channels vector to the union of the restricted and radar channel sets */
+/*
+ * reset the quiet channels vector to the union
+ * of the restricted and radar channel sets
+ */
 static void brcms_c_quiet_channels_reset(struct brcms_cm_info *wlc_cm)
 {
 	struct brcms_c_info *wlc = wlc_cm->wlc;
 	uint i, j;
 	struct brcms_band *band;
-	const chanvec_t *chanvec;
+	const struct brcms_chanvec *chanvec;
 
-	memset(&wlc_cm->quiet_channels, 0, sizeof(chanvec_t));
+	memset(&wlc_cm->quiet_channels, 0, sizeof(struct brcms_chanvec));
 
 	band = wlc->band;
-	for (i = 0; i < NBANDS(wlc);
+	for (i = 0; i < wlc->pub->_nbands;
 	     i++, band = wlc->bandstate[OTHERBANDUNIT(wlc)]) {
 
 		/* initialize quiet channels for restricted channels */
 		chanvec = wlc_cm->bandstate[band->bandunit].restricted_channels;
-		for (j = 0; j < sizeof(chanvec_t); j++)
+		for (j = 0; j < sizeof(struct brcms_chanvec); j++)
 			wlc_cm->quiet_channels.vec[j] |= chanvec->vec[j];
 
 	}
 }
 
-static bool
-brcms_c_quiet_chanspec(struct brcms_cm_info *wlc_cm, chanspec_t chspec)
+/* Is the channel valid for the current locale and current band? */
+static bool brcms_c_valid_channel20(struct brcms_cm_info *wlc_cm, uint val)
+{
+	struct brcms_c_info *wlc = wlc_cm->wlc;
+
+	return ((val < MAXCHANNEL) &&
+		isset(wlc_cm->bandstate[wlc->band->bandunit].valid_channels.vec,
+		      val));
+}
+
+/* Is the channel valid for the current locale and specified band? */
+static bool brcms_c_valid_channel20_in_band(struct brcms_cm_info *wlc_cm,
+					    uint bandunit, uint val)
 {
-	return N_ENAB(wlc_cm->wlc->pub) && CHSPEC_IS40(chspec) ?
-		(isset
-		 (wlc_cm->quiet_channels.vec,
-		  LOWER_20_SB(CHSPEC_CHANNEL(chspec)))
-		 || isset(wlc_cm->quiet_channels.vec,
-			  UPPER_20_SB(CHSPEC_CHANNEL(chspec)))) : isset(wlc_cm->
-									quiet_channels.
-									vec,
-									CHSPEC_CHANNEL
-									(chspec));
+	return ((val < MAXCHANNEL)
+		&& isset(wlc_cm->bandstate[bandunit].valid_channels.vec, val));
 }
 
 /* Is the channel valid for the current locale? (but don't consider channels not
@@ -1008,27 +773,23 @@ static bool brcms_c_valid_channel20_db(struct brcms_cm_info *wlc_cm, uint val)
 {
 	struct brcms_c_info *wlc = wlc_cm->wlc;
 
-	return VALID_CHANNEL20(wlc, val) ||
+	return brcms_c_valid_channel20(wlc->cmi, val) ||
 		(!wlc->bandlocked
-		 && VALID_CHANNEL20_IN_BAND(wlc, OTHERBANDUNIT(wlc), val));
+		 && brcms_c_valid_channel20_in_band(wlc->cmi,
+						    OTHERBANDUNIT(wlc), val));
 }
 
-/* Is the channel valid for the current locale and specified band? */
-static bool brcms_c_valid_channel20_in_band(struct brcms_cm_info *wlc_cm,
-					    uint bandunit, uint val)
+/* JP, J1 - J10 are Japan ccodes */
+static bool brcms_c_japan_ccode(const char *ccode)
 {
-	return ((val < MAXCHANNEL)
-		&& isset(wlc_cm->bandstate[bandunit].valid_channels.vec, val));
+	return (ccode[0] == 'J' &&
+		(ccode[1] == 'P' || (ccode[1] >= '1' && ccode[1] <= '9')));
 }
 
-/* Is the channel valid for the current locale and current band? */
-static bool brcms_c_valid_channel20(struct brcms_cm_info *wlc_cm, uint val)
+/* Returns true if currently set country is Japan or variant */
+static bool brcms_c_japan(struct brcms_c_info *wlc)
 {
-	struct brcms_c_info *wlc = wlc_cm->wlc;
-
-	return ((val < MAXCHANNEL) &&
-		isset(wlc_cm->bandstate[wlc->band->bandunit].valid_channels.vec,
-		      val));
+	return brcms_c_japan_ccode(wlc->cmi->country_abbrev);
 }
 
 static void
@@ -1039,50 +800,42 @@ brcms_c_channel_min_txpower_limits_with_local_constraint(
 	int j;
 
 	/* CCK Rates */
-	for (j = 0; j < WL_TX_POWER_CCK_NUM; j++) {
+	for (j = 0; j < WL_TX_POWER_CCK_NUM; j++)
 		txpwr->cck[j] = min(txpwr->cck[j], local_constraint_qdbm);
-	}
 
 	/* 20 MHz Legacy OFDM SISO */
-	for (j = 0; j < WL_TX_POWER_OFDM_NUM; j++) {
+	for (j = 0; j < WL_TX_POWER_OFDM_NUM; j++)
 		txpwr->ofdm[j] = min(txpwr->ofdm[j], local_constraint_qdbm);
-	}
 
 	/* 20 MHz Legacy OFDM CDD */
-	for (j = 0; j < BRCMS_NUM_RATES_OFDM; j++) {
+	for (j = 0; j < BRCMS_NUM_RATES_OFDM; j++)
 		txpwr->ofdm_cdd[j] =
 		    min(txpwr->ofdm_cdd[j], local_constraint_qdbm);
-	}
 
 	/* 40 MHz Legacy OFDM SISO */
-	for (j = 0; j < BRCMS_NUM_RATES_OFDM; j++) {
+	for (j = 0; j < BRCMS_NUM_RATES_OFDM; j++)
 		txpwr->ofdm_40_siso[j] =
 		    min(txpwr->ofdm_40_siso[j], local_constraint_qdbm);
-	}
 
 	/* 40 MHz Legacy OFDM CDD */
-	for (j = 0; j < BRCMS_NUM_RATES_OFDM; j++) {
+	for (j = 0; j < BRCMS_NUM_RATES_OFDM; j++)
 		txpwr->ofdm_40_cdd[j] =
 		    min(txpwr->ofdm_40_cdd[j], local_constraint_qdbm);
-	}
 
 	/* 20MHz MCS 0-7 SISO */
-	for (j = 0; j < BRCMS_NUM_RATES_MCS_1_STREAM; j++) {
+	for (j = 0; j < BRCMS_NUM_RATES_MCS_1_STREAM; j++)
 		txpwr->mcs_20_siso[j] =
 		    min(txpwr->mcs_20_siso[j], local_constraint_qdbm);
-	}
 
 	/* 20MHz MCS 0-7 CDD */
-	for (j = 0; j < BRCMS_NUM_RATES_MCS_1_STREAM; j++) {
+	for (j = 0; j < BRCMS_NUM_RATES_MCS_1_STREAM; j++)
 		txpwr->mcs_20_cdd[j] =
 		    min(txpwr->mcs_20_cdd[j], local_constraint_qdbm);
-	}
 
 	/* 20MHz MCS 0-7 STBC */
-	for (j = 0; j < BRCMS_NUM_RATES_MCS_1_STREAM; j++) {
+	for (j = 0; j < BRCMS_NUM_RATES_MCS_1_STREAM; j++)
 		txpwr->mcs_20_stbc[j] =
 		    min(txpwr->mcs_20_stbc[j], local_constraint_qdbm);
-	}
 
 	/* 20MHz MCS 8-15 MIMO */
 	for (j = 0; j < BRCMS_NUM_RATES_MCS_2_STREAM; j++)
@@ -1090,22 +843,19 @@ brcms_c_channel_min_txpower_limits_with_local_constraint(
 		    min(txpwr->mcs_20_mimo[j], local_constraint_qdbm);
 
 	/* 40MHz MCS 0-7 SISO */
-	for (j = 0; j < BRCMS_NUM_RATES_MCS_1_STREAM; j++) {
+	for (j = 0; j < BRCMS_NUM_RATES_MCS_1_STREAM; j++)
 		txpwr->mcs_40_siso[j] =
 		    min(txpwr->mcs_40_siso[j], local_constraint_qdbm);
-	}
 
 	/* 40MHz MCS 0-7 CDD */
-	for (j = 0; j < BRCMS_NUM_RATES_MCS_1_STREAM; j++) {
+	for (j = 0; j < BRCMS_NUM_RATES_MCS_1_STREAM; j++)
 		txpwr->mcs_40_cdd[j] =
 		    min(txpwr->mcs_40_cdd[j], local_constraint_qdbm);
-	}
 
 	/* 40MHz MCS 0-7 STBC */
-	for (j = 0; j < BRCMS_NUM_RATES_MCS_1_STREAM; j++) {
+	for (j = 0; j < BRCMS_NUM_RATES_MCS_1_STREAM; j++)
 		txpwr->mcs_40_stbc[j] =
 		    min(txpwr->mcs_40_stbc[j], local_constraint_qdbm);
-	}
 
 	/* 40MHz MCS 8-15 MIMO */
 	for (j = 0; j < BRCMS_NUM_RATES_MCS_2_STREAM; j++)
@@ -1117,8 +867,276 @@ brcms_c_channel_min_txpower_limits_with_local_constraint(
 
 }
 
+/* Update the radio state (enable/disable) and tx power targets
+ * based on a new set of channel/regulatory information
+ */
+static void brcms_c_channels_commit(struct brcms_cm_info *wlc_cm)
+{
+	struct brcms_c_info *wlc = wlc_cm->wlc;
+	uint chan;
+	struct txpwr_limits txpwr;
+
+	/* search for the existence of any valid channel */
+	for (chan = 0; chan < MAXCHANNEL; chan++) {
+		if (brcms_c_valid_channel20_db(wlc->cmi, chan))
+			break;
+	}
+	if (chan == MAXCHANNEL)
+		chan = INVCHANNEL;
+
+	/*
+	 * based on the channel search above, set or
+	 * clear WL_RADIO_COUNTRY_DISABLE.
+	 */
+	if (chan == INVCHANNEL) {
+		/*
+		 * country/locale with no valid channels, set
+		 * the radio disable bit
+		 */
+		mboolset(wlc->pub->radio_disabled, WL_RADIO_COUNTRY_DISABLE);
+		wiphy_err(wlc->wiphy, "wl%d: %s: no valid channel for \"%s\" "
+			  "nbands %d bandlocked %d\n", wlc->pub->unit,
+			  __func__, wlc_cm->country_abbrev, wlc->pub->_nbands,
+			  wlc->bandlocked);
+	} else if (mboolisset(wlc->pub->radio_disabled,
+			      WL_RADIO_COUNTRY_DISABLE)) {
+		/*
+		 * country/locale with valid channel, clear
+		 * the radio disable bit
+		 */
+		mboolclr(wlc->pub->radio_disabled, WL_RADIO_COUNTRY_DISABLE);
+	}
+
+	/*
+	 * Now that the country abbreviation is set, if the radio supports 2G,
+	 * then set channel 14 restrictions based on the new locale.
+	 */
+	if (wlc->pub->_nbands > 1 || wlc->band->bandtype == BRCM_BAND_2G)
+		wlc_phy_chanspec_ch14_widefilter_set(wlc->band->pi,
+						     brcms_c_japan(wlc) ? true :
+						     false);
+
+	if (wlc->pub->up && chan != INVCHANNEL) {
+		brcms_c_channel_reg_limits(wlc_cm, wlc->chanspec, &txpwr);
+		brcms_c_channel_min_txpower_limits_with_local_constraint(wlc_cm,
+			&txpwr, BRCMS_TXPWR_MAX);
+		wlc_phy_txpower_limit_set(wlc->band->pi, &txpwr, wlc->chanspec);
+	}
+}
+
+static int
+brcms_c_channels_init(struct brcms_cm_info *wlc_cm,
+		      const struct country_info *country)
+{
+	struct brcms_c_info *wlc = wlc_cm->wlc;
+	uint i, j;
+	struct brcms_band *band;
+	const struct locale_info *li;
+	struct brcms_chanvec sup_chan;
+	const struct locale_mimo_info *li_mimo;
+
+	band = wlc->band;
+	for (i = 0; i < wlc->pub->_nbands;
+	     i++, band = wlc->bandstate[OTHERBANDUNIT(wlc)]) {
+
+		li = (band->bandtype == BRCM_BAND_5G) ?
+		    brcms_c_get_locale_5g(country->locale_5G) :
+		    brcms_c_get_locale_2g(country->locale_2G);
+		wlc_cm->bandstate[band->bandunit].locale_flags = li->flags;
+		li_mimo = (band->bandtype == BRCM_BAND_5G) ?
+		    brcms_c_get_mimo_5g(country->locale_mimo_5G) :
+		    brcms_c_get_mimo_2g(country->locale_mimo_2G);
+
+		/* merge the mimo non-mimo locale flags */
+		wlc_cm->bandstate[band->bandunit].locale_flags |=
+		    li_mimo->flags;
+
+		wlc_cm->bandstate[band->bandunit].restricted_channels =
+		    g_table_restricted_chan[li->restricted_channels];
+		wlc_cm->bandstate[band->bandunit].radar_channels =
+		    g_table_radar_set[li->radar_channels];
+
+		/*
+		 * set the channel availability, masking out the channels
+		 * that may not be supported on this phy.
+		 */
+		wlc_phy_chanspec_band_validch(band->pi, band->bandtype,
+					      &sup_chan);
+		brcms_c_locale_get_channels(li,
+					&wlc_cm->bandstate[band->bandunit].
+					valid_channels);
+		for (j = 0; j < sizeof(struct brcms_chanvec); j++)
+			wlc_cm->bandstate[band->bandunit].valid_channels.
+			    vec[j] &= sup_chan.vec[j];
+	}
+
+	brcms_c_quiet_channels_reset(wlc_cm);
+	brcms_c_channels_commit(wlc_cm);
+
+	return 0;
+}
+
+/*
+ * set the driver's current country and regulatory information
+ * using a country code as the source. Look up built in country
+ * information found with the country code.
+ */
+static void
+brcms_c_set_country_common(struct brcms_cm_info *wlc_cm,
+		       const char *country_abbrev,
+		       const char *ccode, uint regrev,
+		       const struct country_info *country)
+{
+	const struct locale_info *locale;
+	struct brcms_c_info *wlc = wlc_cm->wlc;
+	char prev_country_abbrev[BRCM_CNTRY_BUF_SZ];
+
+	/* save current country state */
+	wlc_cm->country = country;
+
+	memset(&prev_country_abbrev, 0, BRCM_CNTRY_BUF_SZ);
+	strncpy(prev_country_abbrev, wlc_cm->country_abbrev,
+		BRCM_CNTRY_BUF_SZ - 1);
+
+	strncpy(wlc_cm->country_abbrev, country_abbrev, BRCM_CNTRY_BUF_SZ - 1);
+	strncpy(wlc_cm->ccode, ccode, BRCM_CNTRY_BUF_SZ - 1);
+	wlc_cm->regrev = regrev;
+
+	if ((wlc->pub->_n_enab & SUPPORT_11N) !=
+	    wlc->protection->nmode_user)
+		brcms_c_set_nmode(wlc);
+
+	brcms_c_stf_ss_update(wlc, wlc->bandstate[BAND_2G_INDEX]);
+	brcms_c_stf_ss_update(wlc, wlc->bandstate[BAND_5G_INDEX]);
+	/* set or restore gmode as required by regulatory */
+	locale = brcms_c_get_locale_2g(country->locale_2G);
+	if (locale && (locale->flags & BRCMS_NO_OFDM))
+		brcms_c_set_gmode(wlc, GMODE_LEGACY_B, false);
+	else
+		brcms_c_set_gmode(wlc, wlc->protection->gmode_user, false);
+
+	brcms_c_channels_init(wlc_cm, country);
+
+	return;
+}
+
+static int
+brcms_c_set_countrycode_rev(struct brcms_cm_info *wlc_cm,
+			const char *country_abbrev,
+			const char *ccode, int regrev)
+{
+	const struct country_info *country;
+	char mapped_ccode[BRCM_CNTRY_BUF_SZ];
+	uint mapped_regrev;
+
+	/* if regrev is -1, lookup the mapped country code,
+	 * otherwise use the ccode and regrev directly
+	 */
+	if (regrev == -1) {
+		/*
+		 * map the country code to a built-in country
+		 * code, regrev, and country_info
+		 */
+		country =
+		    brcms_c_countrycode_map(wlc_cm, ccode, mapped_ccode,
+					&mapped_regrev);
+	} else {
+		/* find the matching built-in country definition */
+		country = brcms_c_country_lookup_direct(ccode, regrev);
+		strncpy(mapped_ccode, ccode, BRCM_CNTRY_BUF_SZ);
+		mapped_regrev = regrev;
+	}
+
+	if (country == NULL)
+		return -EINVAL;
+
+	/* set the driver state for the country */
+	brcms_c_set_country_common(wlc_cm, country_abbrev, mapped_ccode,
+			       mapped_regrev, country);
+
+	return 0;
+}
+
+/*
+ * set the driver's current country and regulatory information using
+ * a country code as the source. Lookup built in country information
+ * found with the country code.
+ */
+static int
+brcms_c_set_countrycode(struct brcms_cm_info *wlc_cm, const char *ccode)
+{
+	char country_abbrev[BRCM_CNTRY_BUF_SZ];
+	strncpy(country_abbrev, ccode, BRCM_CNTRY_BUF_SZ);
+	return brcms_c_set_countrycode_rev(wlc_cm, country_abbrev, ccode, -1);
+}
+
+struct brcms_cm_info *brcms_c_channel_mgr_attach(struct brcms_c_info *wlc)
+{
+	struct brcms_cm_info *wlc_cm;
+	char country_abbrev[BRCM_CNTRY_BUF_SZ];
+	const struct country_info *country;
+	struct brcms_pub *pub = wlc->pub;
+	char *ccode;
+
+	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
+
+	wlc_cm = kzalloc(sizeof(struct brcms_cm_info), GFP_ATOMIC);
+	if (wlc_cm == NULL)
+		return NULL;
+	wlc_cm->pub = pub;
+	wlc_cm->wlc = wlc;
+	wlc->cmi = wlc_cm;
+
+	/* store the country code for passing up as a regulatory hint */
+	ccode = getvar(wlc->hw->sih, BRCMS_SROM_CCODE);
+	if (ccode)
+		strncpy(wlc->pub->srom_ccode, ccode, BRCM_CNTRY_BUF_SZ - 1);
+
+	/*
+	 * internal country information which must match
+	 * regulatory constraints in firmware
+	 */
+	memset(country_abbrev, 0, BRCM_CNTRY_BUF_SZ);
+	strncpy(country_abbrev, "X2", sizeof(country_abbrev) - 1);
+	country = brcms_c_country_lookup(wlc, country_abbrev);
+
+	/* save default country for exiting 11d regulatory mode */
+	strncpy(wlc->country_default, country_abbrev, BRCM_CNTRY_BUF_SZ - 1);
+
+	/* initialize autocountry_default to driver default */
+	strncpy(wlc->autocountry_default, "X2", BRCM_CNTRY_BUF_SZ - 1);
+
+	brcms_c_set_countrycode(wlc_cm, country_abbrev);
+
+	return wlc_cm;
+}
+
+void brcms_c_channel_mgr_detach(struct brcms_cm_info *wlc_cm)
+{
+	kfree(wlc_cm);
+}
+
+u8
+brcms_c_channel_locale_flags_in_band(struct brcms_cm_info *wlc_cm,
+				     uint bandunit)
+{
+	return wlc_cm->bandstate[bandunit].locale_flags;
+}
+
+static bool
+brcms_c_quiet_chanspec(struct brcms_cm_info *wlc_cm, u16 chspec)
+{
+	return (wlc_cm->wlc->pub->_n_enab & SUPPORT_11N) &&
+		CHSPEC_IS40(chspec) ?
+		(isset(wlc_cm->quiet_channels.vec,
+		       lower_20_sb(CHSPEC_CHANNEL(chspec))) ||
+		 isset(wlc_cm->quiet_channels.vec,
+		       upper_20_sb(CHSPEC_CHANNEL(chspec)))) :
+		isset(wlc_cm->quiet_channels.vec, CHSPEC_CHANNEL(chspec));
+}
+
 void
-brcms_c_channel_set_chanspec(struct brcms_cm_info *wlc_cm, chanspec_t chanspec,
+brcms_c_channel_set_chanspec(struct brcms_cm_info *wlc_cm, u16 chanspec,
 			 u8 local_constraint_qdbm)
 {
 	struct brcms_c_info *wlc = wlc_cm->wlc;
@@ -1126,8 +1144,9 @@ brcms_c_channel_set_chanspec(struct brcms_cm_info *wlc_cm, chanspec_t chanspec,
 
 	brcms_c_channel_reg_limits(wlc_cm, chanspec, &txpwr);
 
-	brcms_c_channel_min_txpower_limits_with_local_constraint(wlc_cm, &txpwr,
-							     local_constraint_qdbm);
+	brcms_c_channel_min_txpower_limits_with_local_constraint(
+		wlc_cm, &txpwr, local_constraint_qdbm
+	);
 
 	brcms_b_set_chanspec(wlc->hw, chanspec,
 			      (brcms_c_quiet_chanspec(wlc_cm, chanspec) != 0),
@@ -1142,112 +1161,100 @@ static void wlc_phy_txpower_limits_dump(struct txpwr_limits *txpwr)
 	char fraction[4][4] = { "   ", ".25", ".5 ", ".75" };
 
 	sprintf(buf, "CCK                ");
-	for (i = 0; i < BRCMS_NUM_RATES_CCK; i++) {
+	for (i = 0; i < BRCMS_NUM_RATES_CCK; i++)
 		sprintf(buf[strlen(buf)], " %2d%s",
 			txpwr->cck[i] / BRCMS_TXPWR_DB_FACTOR,
 			fraction[txpwr->cck[i] % BRCMS_TXPWR_DB_FACTOR]);
-	}
 	printk(KERN_DEBUG "%s\n", buf);
 
 	sprintf(buf, "20 MHz OFDM SISO   ");
-	for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++) {
+	for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++)
 		sprintf(buf[strlen(buf)], " %2d%s",
 			txpwr->ofdm[i] / BRCMS_TXPWR_DB_FACTOR,
 			fraction[txpwr->ofdm[i] % BRCMS_TXPWR_DB_FACTOR]);
-	}
 	printk(KERN_DEBUG "%s\n", buf);
 
 	sprintf(buf, "20 MHz OFDM CDD    ");
-	for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++) {
+	for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++)
 		sprintf(buf[strlen(buf)], " %2d%s",
 			txpwr->ofdm_cdd[i] / BRCMS_TXPWR_DB_FACTOR,
 			fraction[txpwr->ofdm_cdd[i] % BRCMS_TXPWR_DB_FACTOR]);
-	}
 	printk(KERN_DEBUG "%s\n", buf);
 
 	sprintf(buf, "40 MHz OFDM SISO   ");
-	for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++) {
+	for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++)
 		sprintf(buf[strlen(buf)], " %2d%s",
 			txpwr->ofdm_40_siso[i] / BRCMS_TXPWR_DB_FACTOR,
 			fraction[txpwr->ofdm_40_siso[i] %
 							BRCMS_TXPWR_DB_FACTOR]);
-	}
 	printk(KERN_DEBUG "%s\n", buf);
 
 	sprintf(buf, "40 MHz OFDM CDD    ");
-	for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++) {
+	for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++)
 		sprintf(buf[strlen(buf)], " %2d%s",
 			txpwr->ofdm_40_cdd[i] / BRCMS_TXPWR_DB_FACTOR,
 			fraction[txpwr->ofdm_40_cdd[i] %
 							BRCMS_TXPWR_DB_FACTOR]);
-	}
 	printk(KERN_DEBUG "%s\n", buf);
 
 	sprintf(buf, "20 MHz MCS0-7 SISO ");
-	for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++) {
+	for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++)
 		sprintf(buf[strlen(buf)], " %2d%s",
 			txpwr->mcs_20_siso[i] / BRCMS_TXPWR_DB_FACTOR,
 			fraction[txpwr->mcs_20_siso[i] %
 							BRCMS_TXPWR_DB_FACTOR]);
-	}
 	printk(KERN_DEBUG "%s\n", buf);
 
 	sprintf(buf, "20 MHz MCS0-7 CDD  ");
-	for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++) {
+	for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++)
 		sprintf(buf[strlen(buf)], " %2d%s",
 			txpwr->mcs_20_cdd[i] / BRCMS_TXPWR_DB_FACTOR,
 			fraction[txpwr->mcs_20_cdd[i] %
 							BRCMS_TXPWR_DB_FACTOR]);
-	}
 	printk(KERN_DEBUG "%s\n", buf);
 
 	sprintf(buf, "20 MHz MCS0-7 STBC ");
-	for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++) {
+	for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++)
 		sprintf(buf[strlen(buf)], " %2d%s",
 			txpwr->mcs_20_stbc[i] / BRCMS_TXPWR_DB_FACTOR,
 			fraction[txpwr->mcs_20_stbc[i] %
 							BRCMS_TXPWR_DB_FACTOR]);
-	}
 	printk(KERN_DEBUG "%s\n", buf);
 
 	sprintf(buf, "20 MHz MCS8-15 SDM ");
-	for (i = 0; i < BRCMS_NUM_RATES_MCS_2_STREAM; i++) {
+	for (i = 0; i < BRCMS_NUM_RATES_MCS_2_STREAM; i++)
 		sprintf(buf[strlen(buf)], " %2d%s",
 			txpwr->mcs_20_mimo[i] / BRCMS_TXPWR_DB_FACTOR,
 			fraction[txpwr->mcs_20_mimo[i] %
 							BRCMS_TXPWR_DB_FACTOR]);
-	}
 	printk(KERN_DEBUG "%s\n", buf);
 
 	sprintf(buf, "40 MHz MCS0-7 SISO ");
-	for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++) {
+	for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++)
 		sprintf(buf[strlen(buf)], " %2d%s",
 			txpwr->mcs_40_siso[i] / BRCMS_TXPWR_DB_FACTOR,
 			fraction[txpwr->mcs_40_siso[i] %
 							BRCMS_TXPWR_DB_FACTOR]);
-	}
 	printk(KERN_DEBUG "%s\n", buf);
 
 	sprintf(buf, "40 MHz MCS0-7 CDD  ");
-	for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++) {
+	for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++)
 		sprintf(buf[strlen(buf)], " %2d%s",
 			txpwr->mcs_40_cdd[i] / BRCMS_TXPWR_DB_FACTOR,
 			fraction[txpwr->mcs_40_cdd[i] %
 							BRCMS_TXPWR_DB_FACTOR]);
-	}
 	printk(KERN_DEBUG "%s\n", buf);
 
 	sprintf(buf, "40 MHz MCS0-7 STBC ");
-	for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++) {
+	for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++)
 		sprintf(buf[strlen(buf)], " %2d%s",
 			txpwr->mcs_40_stbc[i] / BRCMS_TXPWR_DB_FACTOR,
 			fraction[txpwr->mcs_40_stbc[i] %
 							BRCMS_TXPWR_DB_FACTOR]);
-	}
 	printk(KERN_DEBUG "%s\n", buf);
 
 	sprintf(buf, "40 MHz MCS8-15 SDM ");
-	for (i = 0; i < BRCMS_NUM_RATES_MCS_2_STREAM; i++) {
+	for (i = 0; i < BRCMS_NUM_RATES_MCS_2_STREAM; i++)
 		sprintf(buf[strlen(buf)], " %2d%s",
 			txpwr->mcs_40_mimo[i] / BRCMS_TXPWR_DB_FACTOR,
 			fraction[txpwr->mcs_40_mimo[i] %
@@ -1262,7 +1269,7 @@ static void wlc_phy_txpower_limits_dump(struct txpwr_limits *txpwr)
 #endif				/* POWER_DBG */
 
 void
-brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, chanspec_t chanspec,
+brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, u16 chanspec,
 		       struct txpwr_limits *txpwr)
 {
 	struct brcms_c_info *wlc = wlc_cm->wlc;
@@ -1273,8 +1280,8 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, chanspec_t chanspec,
 	const struct country_info *country;
 	struct brcms_band *band;
 	const struct locale_info *li;
-	int conducted_max;
-	int conducted_ofdm_max;
+	int conducted_max = BRCMS_TXPWR_MAX;
+	int conducted_ofdm_max = BRCMS_TXPWR_MAX;
 	const struct locale_mimo_info *li_mimo;
 	int maxpwr20, maxpwr40;
 	int maxpwr_idx;
@@ -1291,12 +1298,12 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, chanspec_t chanspec,
 	}
 
 	chan = CHSPEC_CHANNEL(chanspec);
-	band = wlc->bandstate[CHSPEC_BANDUNIT(chanspec)];
-	li = BAND_5G(band->bandtype) ?
+	band = wlc->bandstate[chspec_bandunit(chanspec)];
+	li = (band->bandtype == BRCM_BAND_5G) ?
 	    brcms_c_get_locale_5g(country->locale_5G) :
 	    brcms_c_get_locale_2g(country->locale_2G);
 
-	li_mimo = BAND_5G(band->bandtype) ?
+	li_mimo = (band->bandtype == BRCM_BAND_5G) ?
 	    brcms_c_get_mimo_5g(country->locale_mimo_5G) :
 	    brcms_c_get_mimo_2g(country->locale_mimo_2G);
 
@@ -1314,7 +1321,7 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, chanspec_t chanspec,
 	}
 
 	/* CCK txpwr limits for 2.4G band */
-	if (BAND_2G(band->bandtype)) {
+	if (band->bandtype == BRCM_BAND_2G) {
 		maxpwr = li->maxpwr[CHANNEL_POWER_IDX_2G_CCK(chan)];
 
 		maxpwr = maxpwr - delta;
@@ -1326,29 +1333,29 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, chanspec_t chanspec,
 	}
 
 	/* OFDM txpwr limits for 2.4G or 5G bands */
-	if (BAND_2G(band->bandtype)) {
+	if (band->bandtype == BRCM_BAND_2G)
 		maxpwr = li->maxpwr[CHANNEL_POWER_IDX_2G_OFDM(chan)];
-
-	} else {
+	else
 		maxpwr = li->maxpwr[CHANNEL_POWER_IDX_5G(chan)];
-	}
 
 	maxpwr = maxpwr - delta;
 	maxpwr = max(maxpwr, 0);
 	maxpwr = min(maxpwr, conducted_ofdm_max);
 
 	/* Keep OFDM lmit below CCK limit */
-	if (BAND_2G(band->bandtype))
+	if (band->bandtype == BRCM_BAND_2G)
 		maxpwr = min_t(int, maxpwr, txpwr->cck[0]);
 
 	for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++)
 		txpwr->ofdm[i] = (u8) maxpwr;
 
 	for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++) {
-		/* OFDM 40 MHz SISO has the same power as the corresponding MCS0-7 rate unless
-		 * overriden by the locale specific code. We set this value to 0 as a
-		 * flag (presumably 0 dBm isn't a possibility) and then copy the MCS0-7 value
-		 * to the 40 MHz value if it wasn't explicitly set.
+		/*
+		 * OFDM 40 MHz SISO has the same power as the corresponding
+		 * MCS0-7 rate unless overriden by the locale specific code.
+		 * We set this value to 0 as a flag (presumably 0 dBm isn't
+		 * a possibility) and then copy the MCS0-7 value to the 40 MHz
+		 * value if it wasn't explicitly set.
 		 */
 		txpwr->ofdm_40_siso[i] = 0;
 
@@ -1366,7 +1373,7 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, chanspec_t chanspec,
 			delta = band->antgain - QDB(6);	/* Excess over 6 dB */
 	}
 
-	if (BAND_2G(band->bandtype))
+	if (band->bandtype == BRCM_BAND_2G)
 		maxpwr_idx = (chan - 1);
 	else
 		maxpwr_idx = CHANNEL_POWER_IDX_5G(chan);
@@ -1382,8 +1389,9 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, chanspec_t chanspec,
 	/* Fill in the MCS 0-7 (SISO) rates */
 	for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++) {
 
-		/* 20 MHz has the same power as the corresponding OFDM rate unless
-		 * overriden by the locale specific code.
+		/*
+		 * 20 MHz has the same power as the corresponding OFDM rate
+		 * unless overriden by the locale specific code.
 		 */
 		txpwr->mcs_20_siso[i] = txpwr->ofdm[i];
 		txpwr->mcs_40_siso[i] = 0;
@@ -1395,15 +1403,17 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, chanspec_t chanspec,
 		txpwr->mcs_40_cdd[i] = (u8) maxpwr40;
 	}
 
-	/* These locales have SISO expressed in the table and override CDD later */
+	/*
+	 * These locales have SISO expressed in the
+	 * table and override CDD later
+	 */
 	if (li_mimo == &locale_bn) {
 		if (li_mimo == &locale_bn) {
 			maxpwr20 = QDB(16);
 			maxpwr40 = 0;
 
-			if (chan >= 3 && chan <= 11) {
+			if (chan >= 3 && chan <= 11)
 				maxpwr40 = QDB(16);
-			}
 		}
 
 		for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++) {
@@ -1437,10 +1447,10 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, chanspec_t chanspec,
 		}
 	}
 
-	/* Copy the 40 MHZ MCS 0-7 CDD value to the 40 MHZ MCS 0-7 SISO value if it wasn't
-	 * provided explicitly.
+	/*
+	 * Copy the 40 MHZ MCS 0-7 CDD value to the 40 MHZ MCS 0-7 SISO
+	 * value if it wasn't provided explicitly.
 	 */
-
 	for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++) {
 		if (txpwr->mcs_40_siso[i] == 0)
 			txpwr->mcs_40_siso[i] = txpwr->mcs_40_cdd[i];
@@ -1456,8 +1466,9 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, chanspec_t chanspec,
 		}
 	}
 
-	/* Copy the 20 and 40 MHz MCS0-7 CDD values to the corresponding STBC values if they weren't
-	 * provided explicitly.
+	/*
+	 * Copy the 20 and 40 MHz MCS0-7 CDD values to the corresponding
+	 * STBC values if they weren't provided explicitly.
 	 */
 	for (i = 0; i < BRCMS_NUM_RATES_MCS_1_STREAM; i++) {
 		if (txpwr->mcs_20_stbc[i] == 0)
@@ -1473,73 +1484,94 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, chanspec_t chanspec,
 	return;
 }
 
-/* Returns true if currently set country is Japan or variant */
-static bool brcms_c_japan(struct brcms_c_info *wlc)
+/*
+ * Verify the chanspec is using a legal set of parameters, i.e. that the
+ * chanspec specified a band, bw, ctl_sb and channel and that the
+ * combination could be legal given any set of circumstances.
+ * RETURNS: true is the chanspec is malformed, false if it looks good.
+ */
+static bool brcms_c_chspec_malformed(u16 chanspec)
 {
-	return brcms_c_japan_ccode(wlc->cmi->country_abbrev);
-}
+	/* must be 2G or 5G band */
+	if (!CHSPEC_IS5G(chanspec) && !CHSPEC_IS2G(chanspec))
+		return true;
+	/* must be 20 or 40 bandwidth */
+	if (!CHSPEC_IS40(chanspec) && !CHSPEC_IS20(chanspec))
+		return true;
+
+	/* 20MHZ b/w must have no ctl sb, 40 must have a ctl sb */
+	if (CHSPEC_IS20(chanspec)) {
+		if (!CHSPEC_SB_NONE(chanspec))
+			return true;
+	} else if (!CHSPEC_SB_UPPER(chanspec) && !CHSPEC_SB_LOWER(chanspec)) {
+		return true;
+	}
 
-/* JP, J1 - J10 are Japan ccodes */
-static bool brcms_c_japan_ccode(const char *ccode)
-{
-	return (ccode[0] == 'J' &&
-		(ccode[1] == 'P' || (ccode[1] >= '1' && ccode[1] <= '9')));
+	return false;
 }
 
 /*
- * Validate the chanspec for this locale, for 40MHZ we need to also check that the sidebands
- * are valid 20MZH channels in this locale and they are also a legal HT combination
+ * Validate the chanspec for this locale, for 40MHZ we need to also
+ * check that the sidebands are valid 20MZH channels in this locale
+ * and they are also a legal HT combination
  */
 static bool
-brcms_c_valid_chanspec_ext(struct brcms_cm_info *wlc_cm, chanspec_t chspec,
+brcms_c_valid_chanspec_ext(struct brcms_cm_info *wlc_cm, u16 chspec,
 			   bool dualband)
 {
 	struct brcms_c_info *wlc = wlc_cm->wlc;
 	u8 channel = CHSPEC_CHANNEL(chspec);
 
 	/* check the chanspec */
-	if (brcmu_chspec_malformed(chspec)) {
+	if (brcms_c_chspec_malformed(chspec)) {
 		wiphy_err(wlc->wiphy, "wl%d: malformed chanspec 0x%x\n",
 			wlc->pub->unit, chspec);
 		return false;
 	}
 
 	if (CHANNEL_BANDUNIT(wlc_cm->wlc, channel) !=
-	    CHSPEC_BANDUNIT(chspec))
+	    chspec_bandunit(chspec))
 		return false;
 
 	/* Check a 20Mhz channel */
 	if (CHSPEC_IS20(chspec)) {
 		if (dualband)
-			return VALID_CHANNEL20_DB(wlc_cm->wlc, channel);
+			return brcms_c_valid_channel20_db(wlc_cm->wlc->cmi,
+							  channel);
 		else
-			return VALID_CHANNEL20(wlc_cm->wlc, channel);
+			return brcms_c_valid_channel20(wlc_cm->wlc->cmi,
+						       channel);
 	}
 #ifdef SUPPORT_40MHZ
-	/* We know we are now checking a 40MHZ channel, so we should only be here
-	 * for NPHYS
+	/*
+	 * We know we are now checking a 40MHZ channel, so we should
+	 * only be here for NPHYS
 	 */
 	if (BRCMS_ISNPHY(wlc->band) || BRCMS_ISSSLPNPHY(wlc->band)) {
 		u8 upper_sideband = 0, idx;
 		u8 num_ch20_entries =
 		    sizeof(chan20_info) / sizeof(struct chan20_info);
 
-		if (!VALID_40CHANSPEC_IN_BAND(wlc, CHSPEC_BANDUNIT(chspec)))
+		if (!VALID_40CHANSPEC_IN_BAND(wlc, chspec_bandunit(chspec)))
 			return false;
 
 		if (dualband) {
-			if (!VALID_CHANNEL20_DB(wlc, LOWER_20_SB(channel)) ||
-			    !VALID_CHANNEL20_DB(wlc, UPPER_20_SB(channel)))
+			if (!brcms_c_valid_channel20_db(wlc->cmi,
+							lower_20_sb(channel)) ||
+			    !brcms_c_valid_channel20_db(wlc->cmi,
+							upper_20_sb(channel)))
 				return false;
 		} else {
-			if (!VALID_CHANNEL20(wlc, LOWER_20_SB(channel)) ||
-			    !VALID_CHANNEL20(wlc, UPPER_20_SB(channel)))
+			if (!brcms_c_valid_channel20(wlc->cmi,
+						     lower_20_sb(channel)) ||
+			    !brcms_c_valid_channel20(wlc->cmi,
+						     upper_20_sb(channel)))
 				return false;
 		}
 
 		/* find the lower sideband info in the sideband array */
 		for (idx = 0; idx < num_ch20_entries; idx++) {
-			if (chan20_info[idx].sb == LOWER_20_SB(channel))
+			if (chan20_info[idx].sb == lower_20_sb(channel))
 				upper_sideband = chan20_info[idx].adj_sbs;
 		}
 		/* check that the lower sideband allows an upper sideband */
@@ -1553,7 +1585,7 @@ brcms_c_valid_chanspec_ext(struct brcms_cm_info *wlc_cm, chanspec_t chspec,
 	return false;
 }
 
-bool brcms_c_valid_chanspec_db(struct brcms_cm_info *wlc_cm, chanspec_t chspec)
+bool brcms_c_valid_chanspec_db(struct brcms_cm_info *wlc_cm, u16 chspec)
 {
 	return brcms_c_valid_chanspec_ext(wlc_cm, chspec, true);
 }
diff --git a/drivers/staging/brcm80211/brcmsmac/channel.h b/drivers/staging/brcm80211/brcmsmac/channel.h
index d22f2f5f592d..808cb4fbfbe7 100644
--- a/drivers/staging/brcm80211/brcmsmac/channel.h
+++ b/drivers/staging/brcm80211/brcmsmac/channel.h
@@ -20,59 +20,6 @@
 /* conversion for phy txpwr calculations that use .25 dB units */
 #define BRCMS_TXPWR_DB_FACTOR 4
 
-
-/* maxpwr mapping to 5GHz band channels:
- * maxpwr[0] - channels [34-48]
- * maxpwr[1] - channels [52-60]
- * maxpwr[2] - channels [62-64]
- * maxpwr[3] - channels [100-140]
- * maxpwr[4] - channels [149-165]
- */
-#define BAND_5G_PWR_LVLS	5	/* 5 power levels for 5G */
-
-/* power level in group of 2.4GHz band channels:
- * maxpwr[0] - CCK  channels [1]
- * maxpwr[1] - CCK  channels [2-10]
- * maxpwr[2] - CCK  channels [11-14]
- * maxpwr[3] - OFDM channels [1]
- * maxpwr[4] - OFDM channels [2-10]
- * maxpwr[5] - OFDM channels [11-14]
- */
-
-/* macro to get 2.4 GHz channel group index for tx power */
-#define CHANNEL_POWER_IDX_2G_CCK(c) (((c) < 2) ? 0 : (((c) < 11) ? 1 : 2))	/* cck index */
-#define CHANNEL_POWER_IDX_2G_OFDM(c) (((c) < 2) ? 3 : (((c) < 11) ? 4 : 5))	/* ofdm index */
-
-/* macro to get 5 GHz channel group index for tx power */
-#define CHANNEL_POWER_IDX_5G(c) \
-	(((c) < 52) ? 0 : (((c) < 62) ? 1 : (((c) < 100) ? 2 : (((c) < 149) ? 3 : 4))))
-
-/* max of BAND_5G_PWR_LVLS and 6 for 2.4 GHz */
-#define BRCMS_MAXPWR_TBL_SIZE		6
-/* max of BAND_5G_PWR_LVLS and 14 for 2.4 GHz */
-#define BRCMS_MAXPWR_MIMO_TBL_SIZE	14
-
-#define NBANDS(wlc) ((wlc)->pub->_nbands)
-#define NBANDS_PUB(pub) ((pub)->_nbands)
-#define NBANDS_HW(hw) ((hw)->_nbands)
-
-#define IS_SINGLEBAND_5G(device)	0
-
-/* locale channel and power info. */
-struct locale_info {
-	u32 valid_channels;
-	/* List of radar sensitive channels */
-	u8 radar_channels;
-	/* List of channels used only if APs are detected */
-	u8 restricted_channels;
-	/* Max tx pwr in qdBm for each sub-band */
-	s8 maxpwr[BRCMS_MAXPWR_TBL_SIZE];
-	s8 pub_maxpwr[BAND_5G_PWR_LVLS];	/* Country IE advertised max tx pwr in dBm
-						 * per sub-band
-						 */
-	u8 flags;
-};
-
 /* bits for locale_info flags */
 #define BRCMS_PEAK_CONDUCTED	0x00	/* Peak for locals */
 #define BRCMS_EIRP		0x01	/* Flag for EIRP */
@@ -82,34 +29,8 @@ struct locale_info {
 #define BRCMS_NO_MIMO		0x10	/* Flag for No MIMO, 20 or 40 MHz */
 #define BRCMS_RADAR_TYPE_EU       0x20	/* Flag for EU */
 #define BRCMS_DFS_FCC             BRCMS_DFS_TPC	/* Flag for DFS FCC */
-#define BRCMS_DFS_EU (BRCMS_DFS_TPC | BRCMS_RADAR_TYPE_EU) /* Flag for DFS EU */
-
-#define ISDFS_EU(fl)		(((fl) & BRCMS_DFS_EU) == BRCMS_DFS_EU)
-
-/* locale per-channel tx power limits for MIMO frames
- * maxpwr arrays are index by channel for 2.4 GHz limits, and
- * by sub-band for 5 GHz limits using CHANNEL_POWER_IDX_5G(channel)
- */
-struct locale_mimo_info {
-	/* tx 20 MHz power limits, qdBm units */
-	s8 maxpwr20[BRCMS_MAXPWR_MIMO_TBL_SIZE];
-	/* tx 40 MHz power limits, qdBm units */
-	s8 maxpwr40[BRCMS_MAXPWR_MIMO_TBL_SIZE];
-	u8 flags;
-};
 
-extern const chanvec_t chanvec_all_2G;
-extern const chanvec_t chanvec_all_5G;
-
-/*
- * Country names and abbreviations with locale defined from ISO 3166
- */
-struct country_info {
-	const u8 locale_2G;	/* 2.4G band locale */
-	const u8 locale_5G;	/* 5G band locale */
-	const u8 locale_mimo_2G;	/* 2.4G mimo info */
-	const u8 locale_mimo_5G;	/* 5G mimo info */
-};
+#define BRCMS_DFS_EU (BRCMS_DFS_TPC | BRCMS_RADAR_TYPE_EU) /* Flag for DFS EU */
 
 extern struct brcms_cm_info *
 brcms_c_channel_mgr_attach(struct brcms_c_info *wlc);
@@ -120,13 +41,13 @@ extern u8 brcms_c_channel_locale_flags_in_band(struct brcms_cm_info *wlc_cm,
 					   uint bandunit);
 
 extern bool brcms_c_valid_chanspec_db(struct brcms_cm_info *wlc_cm,
-				      chanspec_t chspec);
+				      u16 chspec);
 
 extern void brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm,
-				   chanspec_t chanspec,
+				   u16 chanspec,
 				   struct txpwr_limits *txpwr);
 extern void brcms_c_channel_set_chanspec(struct brcms_cm_info *wlc_cm,
-				     chanspec_t chanspec,
+				     u16 chanspec,
 				     u8 local_constraint_qdbm);
 
 #endif				/* _WLC_CHANNEL_H */
diff --git a/drivers/staging/brcm80211/brcmsmac/d11.h b/drivers/staging/brcm80211/brcmsmac/d11.h
index e7ff0e6f28e0..ed51616abc85 100644
--- a/drivers/staging/brcm80211/brcmsmac/d11.h
+++ b/drivers/staging/brcm80211/brcmsmac/d11.h
@@ -23,17 +23,15 @@
 #include "pub.h"
 #include "dma.h"
 
-#define	BCN_TMPL_LEN		512	/* length of the BCN template area */
-
 /* RX FIFO numbers */
 #define	RX_FIFO			0	/* data and ctl frames */
 #define	RX_TXSTATUS_FIFO	3	/* RX fifo for tx status packages */
 
-/* TX FIFO numbers using WME Access Classes */
-#define	TX_AC_BK_FIFO		0	/* Access Category Background TX FIFO */
-#define	TX_AC_BE_FIFO		1	/* Access Category Best-Effort TX FIFO */
-#define	TX_AC_VI_FIFO		2	/* Access Class Video TX FIFO */
-#define	TX_AC_VO_FIFO		3	/* Access Class Voice TX FIFO */
+/* TX FIFO numbers using WME Access Category */
+#define	TX_AC_BK_FIFO		0	/* Background TX FIFO */
+#define	TX_AC_BE_FIFO		1	/* Best-Effort TX FIFO */
+#define	TX_AC_VI_FIFO		2	/* Video TX FIFO */
+#define	TX_AC_VO_FIFO		3	/* Voice TX FIFO */
 #define	TX_BCMC_FIFO		4	/* Broadcast/Multicast TX FIFO */
 #define	TX_ATIM_FIFO		5	/* TX fifo for ATIM window info */
 
@@ -47,11 +45,7 @@
 #define	TX_DATA_FIFO		TX_AC_BE_FIFO
 #define	TX_CTL_FIFO		TX_AC_VO_FIFO
 
-#ifndef WL_RSSI_ANT_MAX
 #define WL_RSSI_ANT_MAX		4	/* max possible rx antennas */
-#elif WL_RSSI_ANT_MAX != 4
-#error "WL_RSSI_ANT_MAX does not match"
-#endif
 
 struct intctrlregs {
 	u32 intstatus;
@@ -73,8 +67,8 @@ struct pio2regs {
 
 /* a pair of pio channels(tx and rx) */
 struct pio2regp {
-	pio2regs_t tx;
-	pio2regs_t rx;
+	struct pio2regs tx;
+	struct pio2regs rx;
 };
 
 /* 4byte-wide pio register set per channel(xmt or rcv) */
@@ -85,8 +79,8 @@ struct pio4regs {
 
 /* a pair of pio channels(tx and rx) */
 struct pio4regp {
-	pio4regs_t tx;
-	pio4regs_t rx;
+	struct pio4regs tx;
+	struct pio4regs rx;
 };
 
 /* read: 32-bit register that can be read as 32-bit or as 2 16-bit
@@ -101,10 +95,10 @@ union pmqreg {
 };
 
 struct fifo64 {
-	dma64regs_t dmaxmt;	/* dma tx */
-	pio4regs_t piotx;	/* pio tx */
-	dma64regs_t dmarcv;	/* dma rx */
-	pio4regs_t piorx;	/* pio rx */
+	struct dma64regs dmaxmt;	/* dma tx */
+	struct pio4regs piotx;	/* pio tx */
+	struct dma64regs dmarcv;	/* dma rx */
+	struct pio4regs piorx;	/* pio rx */
 };
 
 /*
@@ -120,7 +114,7 @@ struct d11regs {
 	u32 usectimer;	/* 0x1c *//* for corerev >= 26 */
 
 	/* Interrupt Control *//* 0x20 */
-	intctrlregs_t intctrlregs[8];
+	struct intctrlregs intctrlregs[8];
 
 	u32 PAD[40];		/* 0x60 - 0xFC */
 
@@ -139,7 +133,7 @@ struct d11regs {
 	u32 PAD[2];		/* 0x138 - 0x13C */
 
 	/* PMQ registers */
-	pmqreg_t pmqreg;	/* 0x140 */
+	union pmqreg pmqreg;	/* 0x140 */
 	u32 pmqpatl;		/* 0x144 */
 	u32 pmqpath;		/* 0x148 */
 	u32 PAD;		/* 0x14C */
@@ -179,10 +173,10 @@ struct d11regs {
 	u32 PAD[5];		/* 0x1ec - 0x1fc */
 
 	/* 0x200-0x37F dma/pio registers */
-	fifo64_t fifo64regs[6];
+	struct fifo64 fifo64regs[6];
 
 	/* FIFO diagnostic port access */
-	dma32diag_t dmafifo;	/* 0x380 - 0x38C */
+	struct dma32diag dmafifo;	/* 0x380 - 0x38C */
 
 	u32 aggfifocnt;	/* 0x390 */
 	u32 aggfifodata;	/* 0x394 */
@@ -457,7 +451,7 @@ struct d11regs {
 #define	IRL_FC_MASK		0xff000000	/* frame count */
 #define	IRL_FC_SHIFT		24	/* frame count */
 
-/* maccontrol register */
+/*== maccontrol register ==*/
 #define	MCTL_GMODE		(1U << 31)
 #define	MCTL_DISCARD_PMQ	(1 << 30)
 #define	MCTL_WAKE		(1 << 26)
@@ -481,75 +475,119 @@ struct d11regs {
 #define	MCTL_PSM_RUN		(1 <<  1)
 #define	MCTL_EN_MAC		(1 <<  0)
 
-/* maccommand register */
+/*== maccommand register ==*/
 #define	MCMD_BCN0VLD		(1 <<  0)
 #define	MCMD_BCN1VLD		(1 <<  1)
 #define	MCMD_DIRFRMQVAL		(1 <<  2)
 #define	MCMD_CCA		(1 <<  3)
 #define	MCMD_BG_NOISE		(1 <<  4)
 #define	MCMD_SKIP_SHMINIT	(1 <<  5)	/* only used for simulation */
-#define MCMD_SAMPLECOLL		MCMD_SKIP_SHMINIT	/* reuse for sample collect */
-
-/* macintstatus/macintmask */
-#define	MI_MACSSPNDD		(1 <<  0)	/* MAC has gracefully suspended */
-#define	MI_BCNTPL		(1 <<  1)	/* beacon template available */
-#define	MI_TBTT			(1 <<  2)	/* TBTT indication */
-#define	MI_BCNSUCCESS		(1 <<  3)	/* beacon successfully tx'd */
-#define	MI_BCNCANCLD		(1 <<  4)	/* beacon canceled (IBSS) */
-#define	MI_ATIMWINEND		(1 <<  5)	/* end of ATIM-window (IBSS) */
-#define	MI_PMQ			(1 <<  6)	/* PMQ entries available */
-#define	MI_NSPECGEN_0		(1 <<  7)	/* non-specific gen-stat bits that are set by PSM */
-#define	MI_NSPECGEN_1		(1 <<  8)	/* non-specific gen-stat bits that are set by PSM */
-#define	MI_MACTXERR		(1 <<  9)	/* MAC level Tx error */
-#define	MI_NSPECGEN_3		(1 << 10)	/* non-specific gen-stat bits that are set by PSM */
-#define	MI_PHYTXERR		(1 << 11)	/* PHY Tx error */
-#define	MI_PME			(1 << 12)	/* Power Management Event */
-#define	MI_GP0			(1 << 13)	/* General-purpose timer0 */
-#define	MI_GP1			(1 << 14)	/* General-purpose timer1 */
-#define	MI_DMAINT		(1 << 15)	/* (ORed) DMA-interrupts */
-#define	MI_TXSTOP		(1 << 16)	/* MAC has completed a TX FIFO Suspend/Flush */
-#define	MI_CCA			(1 << 17)	/* MAC has completed a CCA measurement */
-#define	MI_BG_NOISE		(1 << 18)	/* MAC has collected background noise samples */
-#define	MI_DTIM_TBTT		(1 << 19)	/* MBSS DTIM TBTT indication */
-#define MI_PRQ			(1 << 20)	/* Probe response queue needs attention */
-#define	MI_PWRUP		(1 << 21)	/* Radio/PHY has been powered back up. */
+#define MCMD_SAMPLECOLL		MCMD_SKIP_SHMINIT /* reuse for sample collect */
+
+/*== macintstatus/macintmask ==*/
+/* gracefully suspended */
+#define	MI_MACSSPNDD		(1 <<  0)
+/* beacon template available */
+#define	MI_BCNTPL		(1 <<  1)
+/* TBTT indication */
+#define	MI_TBTT			(1 <<  2)
+/* beacon successfully tx'd */
+#define	MI_BCNSUCCESS		(1 <<  3)
+/* beacon canceled (IBSS) */
+#define	MI_BCNCANCLD		(1 <<  4)
+/* end of ATIM-window (IBSS) */
+#define	MI_ATIMWINEND		(1 <<  5)
+/* PMQ entries available */
+#define	MI_PMQ			(1 <<  6)
+/* non-specific gen-stat bits that are set by PSM */
+#define	MI_NSPECGEN_0		(1 <<  7)
+/* non-specific gen-stat bits that are set by PSM */
+#define	MI_NSPECGEN_1		(1 <<  8)
+/* MAC level Tx error */
+#define	MI_MACTXERR		(1 <<  9)
+/* non-specific gen-stat bits that are set by PSM */
+#define	MI_NSPECGEN_3		(1 << 10)
+/* PHY Tx error */
+#define	MI_PHYTXERR		(1 << 11)
+/* Power Management Event */
+#define	MI_PME			(1 << 12)
+/* General-purpose timer0 */
+#define	MI_GP0			(1 << 13)
+/* General-purpose timer1 */
+#define	MI_GP1			(1 << 14)
+/* (ORed) DMA-interrupts */
+#define	MI_DMAINT		(1 << 15)
+/* MAC has completed a TX FIFO Suspend/Flush */
+#define	MI_TXSTOP		(1 << 16)
+/* MAC has completed a CCA measurement */
+#define	MI_CCA			(1 << 17)
+/* MAC has collected background noise samples */
+#define	MI_BG_NOISE		(1 << 18)
+/* MBSS DTIM TBTT indication */
+#define	MI_DTIM_TBTT		(1 << 19)
+/* Probe response queue needs attention */
+#define MI_PRQ			(1 << 20)
+/* Radio/PHY has been powered back up. */
+#define	MI_PWRUP		(1 << 21)
 #define	MI_RESERVED3		(1 << 22)
 #define	MI_RESERVED2		(1 << 23)
 #define MI_RESERVED1		(1 << 25)
 /* MAC detected change on RF Disable input*/
 #define MI_RFDISABLE		(1 << 28)
-#define	MI_TFS			(1 << 29)	/* MAC has completed a TX */
-#define	MI_PHYCHANGED		(1 << 30)	/* A phy status change wrt G mode */
-#define	MI_TO			(1U << 31)	/* general purpose timeout */
+/* MAC has completed a TX */
+#define	MI_TFS			(1 << 29)
+/* A phy status change wrt G mode */
+#define	MI_PHYCHANGED		(1 << 30)
+/* general purpose timeout */
+#define	MI_TO			(1U << 31)
 
 /* Mac capabilities registers */
-/* machwcap */
+/*== machwcap ==*/
 #define	MCAP_TKIPMIC		0x80000000	/* TKIP MIC hardware present */
 
-/* pmqhost data */
-#define	PMQH_DATA_MASK		0xffff0000	/* data entry of head pmq entry */
-#define	PMQH_BSSCFG		0x00100000	/* PM entry for BSS config */
-#define	PMQH_PMOFF		0x00010000	/* PM Mode OFF: power save off */
-#define	PMQH_PMON		0x00020000	/* PM Mode ON: power save on */
-#define	PMQH_DASAT		0x00040000	/* Dis-associated or De-authenticated */
-#define	PMQH_ATIMFAIL		0x00080000	/* ATIM not acknowledged */
-#define	PMQH_DEL_ENTRY		0x00000001	/* delete head entry */
-#define	PMQH_DEL_MULT		0x00000002	/* delete head entry to cur read pointer -1 */
-#define	PMQH_OFLO		0x00000004	/* pmq overflow indication */
-#define	PMQH_NOT_EMPTY		0x00000008	/* entries are present in pmq */
-
-/* phydebug */
-#define	PDBG_CRS		(1 << 0)	/* phy is asserting carrier sense */
-#define	PDBG_TXA		(1 << 1)	/* phy is taking xmit byte from mac this cycle */
-#define	PDBG_TXF		(1 << 2)	/* mac is instructing the phy to transmit a frame */
-#define	PDBG_TXE		(1 << 3)	/* phy is signalling a transmit Error to the mac */
-#define	PDBG_RXF		(1 << 4)	/* phy detected the end of a valid frame preamble */
-#define	PDBG_RXS		(1 << 5)	/* phy detected the end of a valid PLCP header */
-#define	PDBG_RXFRG		(1 << 6)	/* rx start not asserted */
-#define	PDBG_RXV		(1 << 7)	/* mac is taking receive byte from phy this cycle */
-#define	PDBG_RFD		(1 << 16)	/* RF portion of the radio is disabled */
-
-/* objaddr register */
+/*== pmqhost data ==*/
+/* data entry of head pmq entry */
+#define	PMQH_DATA_MASK		0xffff0000
+/* PM entry for BSS config */
+#define	PMQH_BSSCFG		0x00100000
+/* PM Mode OFF: power save off */
+#define	PMQH_PMOFF		0x00010000
+/* PM Mode ON: power save on */
+#define	PMQH_PMON		0x00020000
+/* Dis-associated or De-authenticated */
+#define	PMQH_DASAT		0x00040000
+/* ATIM not acknowledged */
+#define	PMQH_ATIMFAIL		0x00080000
+/* delete head entry */
+#define	PMQH_DEL_ENTRY		0x00000001
+/* delete head entry to cur read pointer -1 */
+#define	PMQH_DEL_MULT		0x00000002
+/* pmq overflow indication */
+#define	PMQH_OFLO		0x00000004
+/* entries are present in pmq */
+#define	PMQH_NOT_EMPTY		0x00000008
+
+/*== phydebug ==*/
+/* phy is asserting carrier sense */
+#define	PDBG_CRS		(1 << 0)
+/* phy is taking xmit byte from mac this cycle */
+#define	PDBG_TXA		(1 << 1)
+/* mac is instructing the phy to transmit a frame */
+#define	PDBG_TXF		(1 << 2)
+/* phy is signalling a transmit Error to the mac */
+#define	PDBG_TXE		(1 << 3)
+/* phy detected the end of a valid frame preamble */
+#define	PDBG_RXF		(1 << 4)
+/* phy detected the end of a valid PLCP header */
+#define	PDBG_RXS		(1 << 5)
+/* rx start not asserted */
+#define	PDBG_RXFRG		(1 << 6)
+/* mac is taking receive byte from phy this cycle */
+#define	PDBG_RXV		(1 << 7)
+/* RF portion of the radio is disabled */
+#define	PDBG_RFD		(1 << 16)
+
+/*== objaddr register ==*/
 #define	OBJADDR_SEL_MASK	0x000F0000
 #define	OBJADDR_UCM_SEL		0x00000000
 #define	OBJADDR_SHM_SEL		0x00010000
@@ -564,20 +602,20 @@ struct d11regs {
 #define	WEP_PCMADDR		0x07d4
 #define	WEP_PCMDATA		0x07d6
 
-/* frmtxstatus */
+/*== frmtxstatus ==*/
 #define	TXS_V			(1 << 0)	/* valid bit */
 #define	TXS_STATUS_MASK		0xffff
 #define	TXS_FID_MASK		0xffff0000
 #define	TXS_FID_SHIFT		16
 
-/* frmtxstatus2 */
+/*== frmtxstatus2 ==*/
 #define	TXS_SEQ_MASK		0xffff
 #define	TXS_PTX_MASK		0xff0000
 #define	TXS_PTX_SHIFT		16
 #define	TXS_MU_MASK		0x01000000
 #define	TXS_MU_SHIFT		24
 
-/* clk_ctl_st */
+/*== clk_ctl_st ==*/
 #define CCS_ERSRC_REQ_D11PLL	0x00000100	/* d11 core pll request */
 #define CCS_ERSRC_REQ_PHYPLL	0x00000200	/* PHY pll request */
 #define CCS_ERSRC_AVAIL_D11PLL	0x01000000	/* d11 core pll available */
@@ -602,22 +640,27 @@ struct d11regs {
 #define TXFIFO_SIZE_UNIT	256	/* one unit corresponds to 256 bytes */
 #define MBSS16_TEMPLMEM_MINBLKS	65	/* one unit corresponds to 256 bytes */
 
-/* phy versions, PhyVersion:Revision field */
-#define	PV_AV_MASK		0xf000	/* analog block version */
-#define	PV_AV_SHIFT		12	/* analog block version bitfield offset */
-#define	PV_PT_MASK		0x0f00	/* phy type */
-#define	PV_PT_SHIFT		8	/* phy type bitfield offset */
-#define	PV_PV_MASK		0x000f	/* phy version */
+/*== phy versions (PhyVersion:Revision field) ==*/
+/* analog block version */
+#define	PV_AV_MASK		0xf000
+/* analog block version bitfield offset */
+#define	PV_AV_SHIFT		12
+/* phy type */
+#define	PV_PT_MASK		0x0f00
+/* phy type bitfield offset */
+#define	PV_PT_SHIFT		8
+/* phy version */
+#define	PV_PV_MASK		0x000f
 #define	PHY_TYPE(v)		((v & PV_PT_MASK) >> PV_PT_SHIFT)
 
-/* phy types, PhyVersion:PhyType field */
+/*== phy types (PhyVersion:PhyType field) ==*/
 #define	PHY_TYPE_N		4	/* N-Phy value */
 #define	PHY_TYPE_SSN		6	/* SSLPN-Phy value */
 #define	PHY_TYPE_LCN		8	/* LCN-Phy value */
 #define	PHY_TYPE_LCNXN		9	/* LCNXN-Phy value */
 #define	PHY_TYPE_NULL		0xf	/* Invalid Phy value */
 
-/* analog types, PhyVersion:AnalogType field */
+/*== analog types (PhyVersion:AnalogType field) ==*/
 #define	ANA_11N_013		5
 
 /* 802.11a PLCP header def */
@@ -693,45 +736,46 @@ struct cck_phy_hdr {
 #define BRCMS_CLR_MIMO_PLCP_AMPDU(plcp) (plcp[3] &= ~MIMO_PLCP_AMPDU)
 #define BRCMS_IS_MIMO_PLCP_AMPDU(plcp) (plcp[3] & MIMO_PLCP_AMPDU)
 
-/* The dot11a PLCP header is 5 bytes.  To simplify the software (so that we
- * don't need e.g. different tx DMA headers for 11a and 11b), the PLCP header has
- * padding added in the ucode.
+/*
+ * The dot11a PLCP header is 5 bytes.  To simplify the software (so that we
+ * don't need e.g. different tx DMA headers for 11a and 11b), the PLCP header
+ * has padding added in the ucode.
  */
 #define	D11_PHY_HDR_LEN	6
 
 /* TX DMA buffer header */
 struct d11txh {
-	u16 MacTxControlLow;	/* 0x0 */
-	u16 MacTxControlHigh;	/* 0x1 */
-	u16 MacFrameControl;	/* 0x2 */
-	u16 TxFesTimeNormal;	/* 0x3 */
-	u16 PhyTxControlWord;	/* 0x4 */
-	u16 PhyTxControlWord_1;	/* 0x5 */
-	u16 PhyTxControlWord_1_Fbr;	/* 0x6 */
-	u16 PhyTxControlWord_1_Rts;	/* 0x7 */
-	u16 PhyTxControlWord_1_FbrRts;	/* 0x8 */
-	u16 MainRates;	/* 0x9 */
-	u16 XtraFrameTypes;	/* 0xa */
+	__le16 MacTxControlLow;	/* 0x0 */
+	__le16 MacTxControlHigh;	/* 0x1 */
+	__le16 MacFrameControl;	/* 0x2 */
+	__le16 TxFesTimeNormal;	/* 0x3 */
+	__le16 PhyTxControlWord;	/* 0x4 */
+	__le16 PhyTxControlWord_1;	/* 0x5 */
+	__le16 PhyTxControlWord_1_Fbr;	/* 0x6 */
+	__le16 PhyTxControlWord_1_Rts;	/* 0x7 */
+	__le16 PhyTxControlWord_1_FbrRts;	/* 0x8 */
+	__le16 MainRates;	/* 0x9 */
+	__le16 XtraFrameTypes;	/* 0xa */
 	u8 IV[16];		/* 0x0b - 0x12 */
 	u8 TxFrameRA[6];	/* 0x13 - 0x15 */
-	u16 TxFesTimeFallback;	/* 0x16 */
+	__le16 TxFesTimeFallback;	/* 0x16 */
 	u8 RTSPLCPFallback[6];	/* 0x17 - 0x19 */
-	u16 RTSDurFallback;	/* 0x1a */
+	__le16 RTSDurFallback;	/* 0x1a */
 	u8 FragPLCPFallback[6];	/* 0x1b - 1d */
-	u16 FragDurFallback;	/* 0x1e */
-	u16 MModeLen;	/* 0x1f */
-	u16 MModeFbrLen;	/* 0x20 */
-	u16 TstampLow;	/* 0x21 */
-	u16 TstampHigh;	/* 0x22 */
-	u16 ABI_MimoAntSel;	/* 0x23 */
-	u16 PreloadSize;	/* 0x24 */
-	u16 AmpduSeqCtl;	/* 0x25 */
-	u16 TxFrameID;	/* 0x26 */
-	u16 TxStatus;	/* 0x27 */
-	u16 MaxNMpdus;	/* 0x28 */
-	u16 MaxABytes_MRT;	/* 0x29 */
-	u16 MaxABytes_FBR;	/* 0x2a */
-	u16 MinMBytes;	/* 0x2b */
+	__le16 FragDurFallback;	/* 0x1e */
+	__le16 MModeLen;	/* 0x1f */
+	__le16 MModeFbrLen;	/* 0x20 */
+	__le16 TstampLow;	/* 0x21 */
+	__le16 TstampHigh;	/* 0x22 */
+	__le16 ABI_MimoAntSel;	/* 0x23 */
+	__le16 PreloadSize;	/* 0x24 */
+	__le16 AmpduSeqCtl;	/* 0x25 */
+	__le16 TxFrameID;	/* 0x26 */
+	__le16 TxStatus;	/* 0x27 */
+	__le16 MaxNMpdus;	/* 0x28 */
+	__le16 MaxABytes_MRT;	/* 0x29 */
+	__le16 MaxABytes_FBR;	/* 0x2a */
+	__le16 MinMBytes;	/* 0x2b */
 	u8 RTSPhyHeader[D11_PHY_HDR_LEN];	/* 0x2c - 0x2e */
 	struct ieee80211_rts rts_frame;	/* 0x2f - 0x36 */
 	u16 PAD;		/* 0x37 */
@@ -745,14 +789,17 @@ struct d11txh {
 #define FT_HT	2
 #define FT_N	3
 
-/* Position of MPDU inside A-MPDU; indicated with bits 10:9 of MacTxControlLow */
+/*
+ * Position of MPDU inside A-MPDU; indicated with bits 10:9
+ * of MacTxControlLow
+ */
 #define TXC_AMPDU_SHIFT		9	/* shift for ampdu settings */
 #define TXC_AMPDU_NONE		0	/* Regular MPDU, not an A-MPDU */
 #define TXC_AMPDU_FIRST		1	/* first MPDU of an A-MPDU */
 #define TXC_AMPDU_MIDDLE	2	/* intermediate MPDU of an A-MPDU */
 #define TXC_AMPDU_LAST		3	/* last (or single) MPDU of an A-MPDU */
 
-/* MacTxControlLow */
+/*== MacTxControlLow ==*/
 #define TXC_AMIC		0x8000
 #define	TXC_SENDCTS		0x0800
 #define TXC_AMPDU_MASK		0x0600
@@ -766,18 +813,25 @@ struct d11txh {
 #define	TXC_LONGFRAME		0x0002
 #define	TXC_IMMEDACK		0x0001
 
-/* MacTxControlHigh */
-#define TXC_PREAMBLE_RTS_FB_SHORT	0x8000	/* RTS fallback preamble type 1 = SHORT 0 = LONG */
-#define TXC_PREAMBLE_RTS_MAIN_SHORT	0x4000	/* RTS main rate preamble type 1 = SHORT 0 = LONG */
-#define TXC_PREAMBLE_DATA_FB_SHORT	0x2000	/* Main fallback rate preamble type
-						 * 1 = SHORT for OFDM/GF for MIMO
-						 * 0 = LONG for CCK/MM for MIMO
-						 */
+/*== MacTxControlHigh ==*/
+/* RTS fallback preamble type 1 = SHORT 0 = LONG */
+#define TXC_PREAMBLE_RTS_FB_SHORT	0x8000
+/* RTS main rate preamble type 1 = SHORT 0 = LONG */
+#define TXC_PREAMBLE_RTS_MAIN_SHORT	0x4000
+/*
+ * Main fallback rate preamble type
+ *   1 = SHORT for OFDM/GF for MIMO
+ *   0 = LONG for CCK/MM for MIMO
+ */
+#define TXC_PREAMBLE_DATA_FB_SHORT	0x2000
+
 /* TXC_PREAMBLE_DATA_MAIN is in PhyTxControl bit 5 */
-#define	TXC_AMPDU_FBR		0x1000	/* use fallback rate for this AMPDU */
+/* use fallback rate for this AMPDU */
+#define	TXC_AMPDU_FBR		0x1000
 #define	TXC_SECKEY_MASK		0x0FF0
 #define	TXC_SECKEY_SHIFT	4
-#define	TXC_ALT_TXPWR		0x0008	/* Use alternate txpwr defined at loc. M_ALT_TXPWR_IDX */
+/* Use alternate txpwr defined at loc. M_ALT_TXPWR_IDX */
+#define	TXC_ALT_TXPWR		0x0008
 #define	TXC_SECTYPE_MASK	0x0007
 #define	TXC_SECTYPE_SHIFT	0
 
@@ -817,7 +871,7 @@ struct d11txh {
 #define PHY_TXC1_MODE_SDM		3
 
 /* PhyTxControl for HTphy that are different from Mimophy */
-#define	PHY_TXC_HTANT_MASK		0x3fC0	/* bit 6, 7, 8, 9, 10, 11, 12, 13 */
+#define	PHY_TXC_HTANT_MASK		0x3fC0	/* bits 6-13 */
 
 /* XtraFrameTypes */
 #define XFTS_RTS_FT_SHIFT	2
@@ -862,23 +916,23 @@ struct tx_status {
 #define	TX_STATUS_RTS_RTX_MASK	0x0F00
 #define	TX_STATUS_RTS_RTX_SHIFT	8
 #define TX_STATUS_MASK		0x00FE
-#define	TX_STATUS_PMINDCTD	(1 << 7)	/* PM mode indicated to AP */
-#define	TX_STATUS_INTERMEDIATE	(1 << 6)	/* intermediate or 1st ampdu pkg */
-#define	TX_STATUS_AMPDU		(1 << 5)	/* AMPDU status */
-#define TX_STATUS_SUPR_MASK	0x1C	/* suppress status bits (4:2) */
+#define	TX_STATUS_PMINDCTD	(1 << 7) /* PM mode indicated to AP */
+#define	TX_STATUS_INTERMEDIATE	(1 << 6) /* intermediate or 1st ampdu pkg */
+#define	TX_STATUS_AMPDU		(1 << 5) /* AMPDU status */
+#define TX_STATUS_SUPR_MASK	0x1C	 /* suppress status bits (4:2) */
 #define TX_STATUS_SUPR_SHIFT	2
-#define	TX_STATUS_ACK_RCV	(1 << 1)	/* ACK received */
-#define	TX_STATUS_VALID		(1 << 0)	/* Tx status valid */
+#define	TX_STATUS_ACK_RCV	(1 << 1) /* ACK received */
+#define	TX_STATUS_VALID		(1 << 0) /* Tx status valid */
 #define	TX_STATUS_NO_ACK	0
 
 /* suppress status reason codes */
-#define	TX_STATUS_SUPR_PMQ	(1 << 2)	/* PMQ entry */
-#define	TX_STATUS_SUPR_FLUSH	(2 << 2)	/* flush request */
-#define	TX_STATUS_SUPR_FRAG	(3 << 2)	/* previous frag failure */
-#define	TX_STATUS_SUPR_TBTT	(3 << 2)	/* SHARED: Probe response supr for TBTT */
-#define	TX_STATUS_SUPR_BADCH	(4 << 2)	/* channel mismatch */
-#define	TX_STATUS_SUPR_EXPTIME	(5 << 2)	/* lifetime expiry */
-#define	TX_STATUS_SUPR_UF	(6 << 2)	/* underflow */
+#define	TX_STATUS_SUPR_PMQ	(1 << 2) /* PMQ entry */
+#define	TX_STATUS_SUPR_FLUSH	(2 << 2) /* flush request */
+#define	TX_STATUS_SUPR_FRAG	(3 << 2) /* previous frag failure */
+#define	TX_STATUS_SUPR_TBTT	(3 << 2) /* SHARED: Probe resp supr for TBTT */
+#define	TX_STATUS_SUPR_BADCH	(4 << 2) /* channel mismatch */
+#define	TX_STATUS_SUPR_EXPTIME	(5 << 2) /* lifetime expiry */
+#define	TX_STATUS_SUPR_UF	(6 << 2) /* underflow */
 
 /* Unexpected tx status for rate update */
 #define TX_STATUS_UNEXP(status) \
@@ -939,8 +993,8 @@ struct tx_status {
 #define ADDR_BMP_RA		(1 << 0)	/* Receiver Address (RA) */
 #define ADDR_BMP_TA		(1 << 1)	/* Transmitter Address (TA) */
 #define ADDR_BMP_BSSID		(1 << 2)	/* BSSID */
-#define ADDR_BMP_AP		(1 << 3)	/* Infra-BSS Access Point (AP) */
-#define ADDR_BMP_STA		(1 << 4)	/* Infra-BSS Station (STA) */
+#define ADDR_BMP_AP		(1 << 3)	/* Infra-BSS Access Point */
+#define ADDR_BMP_STA		(1 << 4)	/* Infra-BSS Station */
 #define ADDR_BMP_RESERVED1	(1 << 5)
 #define ADDR_BMP_RESERVED2	(1 << 6)
 #define ADDR_BMP_RESERVED3	(1 << 7)
@@ -1012,9 +1066,10 @@ struct tx_status {
 #define	T_BCN0_TPL_BASE		(0x34 * 2)
 #define	T_PRS_TPL_BASE		(0x134 * 2)
 #define	T_BCN1_TPL_BASE		(0x234 * 2)
-#define T_TX_FIFO_TXRAM_BASE	(T_ACTS_TPL_BASE + (TXFIFO_START_BLK * TXFIFO_SIZE_UNIT))
+#define T_TX_FIFO_TXRAM_BASE	(T_ACTS_TPL_BASE + \
+				 (TXFIFO_START_BLK * TXFIFO_SIZE_UNIT))
 
-#define T_BA_TPL_BASE		T_QNULL_TPL_BASE	/* template area for BA */
+#define T_BA_TPL_BASE		T_QNULL_TPL_BASE /* template area for BA */
 
 #define T_RAM_ACCESS_SZ		4	/* template ram is 4 byte access only */
 
@@ -1207,14 +1262,18 @@ struct tx_status {
 #define WATCHDOG_8TU_MAX	10
 
 /* Manufacturing Test Variables */
-#define M_PKTENG_CTRL		(0x6c * 2)	/* PER test mode */
-#define M_PKTENG_IFS		(0x6d * 2)	/* IFS for TX mode */
-#define M_PKTENG_FRMCNT_LO		(0x6e * 2)	/* Lower word of tx frmcnt/rx lostcnt */
-#define M_PKTENG_FRMCNT_HI		(0x6f * 2)	/* Upper word of tx frmcnt/rx lostcnt */
+/* PER test mode */
+#define M_PKTENG_CTRL		(0x6c * 2)
+/* IFS for TX mode */
+#define M_PKTENG_IFS		(0x6d * 2)
+/* Lower word of tx frmcnt/rx lostcnt */
+#define M_PKTENG_FRMCNT_LO	(0x6e * 2)
+/* Upper word of tx frmcnt/rx lostcnt */
+#define M_PKTENG_FRMCNT_HI	(0x6f * 2)
 
 /* Index variation in vbat ripple */
-#define M_LCN_PWR_IDX_MAX	(0x67 * 2)	/* highest index read by ucode */
-#define M_LCN_PWR_IDX_MIN	(0x66 * 2)	/* lowest index read by ucode */
+#define M_LCN_PWR_IDX_MAX	(0x67 * 2) /* highest index read by ucode */
+#define M_LCN_PWR_IDX_MIN	(0x66 * 2) /* lowest index read by ucode */
 
 /* M_PKTENG_CTRL bit definitions */
 #define M_PKTENG_MODE_TX		0x0001
@@ -1223,11 +1282,14 @@ struct tx_status {
 #define M_PKTENG_MODE_RX		0x0002
 #define M_PKTENG_MODE_RX_WITH_ACK	0x0402
 #define M_PKTENG_MODE_MASK		0x0003
-#define M_PKTENG_FRMCNT_VLD		0x0100	/* TX frames indicated in the frmcnt reg */
+/* TX frames indicated in the frmcnt reg */
+#define M_PKTENG_FRMCNT_VLD		0x0100
 
 /* Sample Collect parameters (bitmap and type) */
-#define M_SMPL_COL_BMP		(0x37d * 2)	/* Trigger bitmap for sample collect */
-#define M_SMPL_COL_CTL		(0x3b2 * 2)	/* Sample collect type */
+/* Trigger bitmap for sample collect */
+#define M_SMPL_COL_BMP		(0x37d * 2)
+/* Sample collect type */
+#define M_SMPL_COL_CTL		(0x3b2 * 2)
 
 #define ANTSEL_CLKDIV_4MHZ	6
 #define MIMO_ANTSEL_BUSY	0x4000	/* bit 14 (busy) */
@@ -1259,27 +1321,36 @@ struct shm_acparams {
 #define MHF5		4	/* Hostflag 5 index */
 
 /* Flags in M_HOST_FLAGS */
-#define	MHF1_ANTDIV		0x0001	/* Enable ucode antenna diversity help */
-#define	MHF1_EDCF		0x0100	/* Enable EDCF access control */
+/* Enable ucode antenna diversity help */
+#define	MHF1_ANTDIV		0x0001
+/* Enable EDCF access control */
+#define	MHF1_EDCF		0x0100
 #define MHF1_IQSWAP_WAR		0x0200
-#define	MHF1_FORCEFASTCLK	0x0400	/* Disable Slow clock request, for corerev < 11 */
+/* Disable Slow clock request, for corerev < 11 */
+#define	MHF1_FORCEFASTCLK	0x0400
 
 /* Flags in M_HOST_FLAGS2 */
-#define MHF2_PCISLOWCLKWAR	0x0008	/* PR16165WAR : Enable ucode PCI slow clock WAR */
-#define MHF2_TXBCMC_NOW		0x0040	/* Flush BCMC FIFO immediately */
-#define MHF2_HWPWRCTL		0x0080	/* Enable ucode/hw power control */
+
+/* Flush BCMC FIFO immediately */
+#define MHF2_TXBCMC_NOW		0x0040
+/* Enable ucode/hw power control */
+#define MHF2_HWPWRCTL		0x0080
 #define MHF2_NPHY40MHZ_WAR	0x0800
 
 /* Flags in M_HOST_FLAGS3 */
-#define MHF3_ANTSEL_EN		0x0001	/* enabled mimo antenna selection */
-#define MHF3_ANTSEL_MODE	0x0002	/* antenna selection mode: 0: 2x3, 1: 2x4 */
+/* enabled mimo antenna selection */
+#define MHF3_ANTSEL_EN		0x0001
+/* antenna selection mode: 0: 2x3, 1: 2x4 */
+#define MHF3_ANTSEL_MODE	0x0002
 #define MHF3_RESERVED1		0x0004
 #define MHF3_RESERVED2		0x0008
 #define MHF3_NPHY_MLADV_WAR	0x0010
 
 /* Flags in M_HOST_FLAGS4 */
-#define MHF4_BPHY_TXCORE0	0x0080	/* force bphy Tx on core 0 (board level WAR) */
-#define MHF4_EXTPA_ENABLE	0x4000	/* for 4313A0 FEM boards */
+/* force bphy Tx on core 0 (board level WAR) */
+#define MHF4_BPHY_TXCORE0	0x0080
+/* for 4313A0 FEM boards */
+#define MHF4_EXTPA_ENABLE	0x4000
 
 /* Flags in M_HOST_FLAGS5 */
 #define MHF5_4313_GPIOCTRL	0x0001
@@ -1292,52 +1363,80 @@ struct shm_acparams {
 #define	M_PHY_NOISE		(0x037 * 2)
 #define	PHY_NOISE_MASK		0x00ff
 
-/* Receive Frame Data Header for 802.11b DCF-only frames */
-struct d11rxhdr {
-	u16 RxFrameSize;	/* Actual byte length of the frame data received */
+/*
+ * Receive Frame Data Header for 802.11b DCF-only frames
+ *
+ * RxFrameSize: Actual byte length of the frame data received
+ * PAD: padding (not used)
+ * PhyRxStatus_0: PhyRxStatus 15:0
+ * PhyRxStatus_1: PhyRxStatus 31:16
+ * PhyRxStatus_2: PhyRxStatus 47:32
+ * PhyRxStatus_3: PhyRxStatus 63:48
+ * PhyRxStatus_4: PhyRxStatus 79:64
+ * PhyRxStatus_5: PhyRxStatus 95:80
+ * RxStatus1: MAC Rx Status
+ * RxStatus2: extended MAC Rx status
+ * RxTSFTime: RxTSFTime time of first MAC symbol + M_PHY_PLCPRX_DLY
+ * RxChan: gain code, channel radio code, and phy type
+ */
+struct d11rxhdr_le {
+	__le16 RxFrameSize;
 	u16 PAD;
-	u16 PhyRxStatus_0;	/* PhyRxStatus 15:0 */
-	u16 PhyRxStatus_1;	/* PhyRxStatus 31:16 */
-	u16 PhyRxStatus_2;	/* PhyRxStatus 47:32 */
-	u16 PhyRxStatus_3;	/* PhyRxStatus 63:48 */
-	u16 PhyRxStatus_4;	/* PhyRxStatus 79:64 */
-	u16 PhyRxStatus_5;	/* PhyRxStatus 95:80 */
-	u16 RxStatus1;	/* MAC Rx Status */
-	u16 RxStatus2;	/* extended MAC Rx status */
-	u16 RxTSFTime;	/* RxTSFTime time of first MAC symbol + M_PHY_PLCPRX_DLY */
-	u16 RxChan;		/* gain code, channel radio code, and phy type */
+	__le16 PhyRxStatus_0;
+	__le16 PhyRxStatus_1;
+	__le16 PhyRxStatus_2;
+	__le16 PhyRxStatus_3;
+	__le16 PhyRxStatus_4;
+	__le16 PhyRxStatus_5;
+	__le16 RxStatus1;
+	__le16 RxStatus2;
+	__le16 RxTSFTime;
+	__le16 RxChan;
 } __packed;
 
-#define	RXHDR_LEN		24	/* sizeof struct d11rxhdr */
-#define	FRAMELEN(h)		((h)->RxFrameSize)
-
-struct brcms_d11rxhdr {
-	struct d11rxhdr rxhdr;
-	u32 tsf_l;		/* TSF_L reading */
-	s8 rssi;		/* computed instanteneous rssi in BMAC */
-	s8 rxpwr0;		/* obsoleted, place holder for legacy ROM code. use rxpwr[] */
-	s8 rxpwr1;		/* obsoleted, place holder for legacy ROM code. use rxpwr[] */
-	s8 do_rssi_ma;	/* do per-pkt sampling for per-antenna ma in HIGH */
-	s8 rxpwr[WL_RSSI_ANT_MAX];	/* rssi for supported antennas */
+struct d11rxhdr {
+	u16 RxFrameSize;
+	u16 PAD;
+	u16 PhyRxStatus_0;
+	u16 PhyRxStatus_1;
+	u16 PhyRxStatus_2;
+	u16 PhyRxStatus_3;
+	u16 PhyRxStatus_4;
+	u16 PhyRxStatus_5;
+	u16 RxStatus1;
+	u16 RxStatus2;
+	u16 RxTSFTime;
+	u16 RxChan;
 } __packed;
 
 /* PhyRxStatus_0: */
-#define	PRXS0_FT_MASK		0x0003	/* NPHY only: CCK, OFDM, preN, N */
-#define	PRXS0_CLIP_MASK		0x000C	/* NPHY only: clip count adjustment steps by AGC */
+/* NPHY only: CCK, OFDM, preN, N */
+#define	PRXS0_FT_MASK		0x0003
+/* NPHY only: clip count adjustment steps by AGC */
+#define	PRXS0_CLIP_MASK		0x000C
 #define	PRXS0_CLIP_SHIFT	2
-#define	PRXS0_UNSRATE		0x0010	/* PHY received a frame with unsupported rate */
-#define	PRXS0_RXANT_UPSUBBAND	0x0020	/* GPHY: rx ant, NPHY: upper sideband */
-#define	PRXS0_LCRS		0x0040	/* CCK frame only: lost crs during cck frame reception */
-#define	PRXS0_SHORTH		0x0080	/* Short Preamble */
-#define	PRXS0_PLCPFV		0x0100	/* PLCP violation */
-#define	PRXS0_PLCPHCF		0x0200	/* PLCP header integrity check failed */
-#define	PRXS0_GAIN_CTL		0x4000	/* legacy PHY gain control */
-#define PRXS0_ANTSEL_MASK	0xF000	/* NPHY: Antennas used for received frame, bitmask */
+/* PHY received a frame with unsupported rate */
+#define	PRXS0_UNSRATE		0x0010
+/* GPHY: rx ant, NPHY: upper sideband */
+#define	PRXS0_RXANT_UPSUBBAND	0x0020
+/* CCK frame only: lost crs during cck frame reception */
+#define	PRXS0_LCRS		0x0040
+/* Short Preamble */
+#define	PRXS0_SHORTH		0x0080
+/* PLCP violation */
+#define	PRXS0_PLCPFV		0x0100
+/* PLCP header integrity check failed */
+#define	PRXS0_PLCPHCF		0x0200
+/* legacy PHY gain control */
+#define	PRXS0_GAIN_CTL		0x4000
+/* NPHY: Antennas used for received frame, bitmask */
+#define PRXS0_ANTSEL_MASK	0xF000
 #define PRXS0_ANTSEL_SHIFT	0x12
 
 /* subfield PRXS0_FT_MASK */
 #define	PRXS0_CCK		0x0000
-#define	PRXS0_OFDM		0x0001	/* valid only for G phy, use rxh->RxChan for A phy */
+/* valid only for G phy, use rxh->RxChan for A phy */
+#define	PRXS0_OFDM		0x0001
 #define	PRXS0_PREN		0x0002
 #define	PRXS0_STDN		0x0003
 
@@ -1364,35 +1463,51 @@ struct brcms_d11rxhdr {
 #define PRXS0_UNUSED	        0xF000	/* unused and not defined; set to 0 */
 
 /* htphy PhyRxStatus_1: */
-#define PRXS1_HTPHY_CORE_MASK	0x000F	/* core enables for {3..0}, 0=disabled, 1=enabled */
-#define PRXS1_HTPHY_ANTCFG_MASK	0x00F0	/* antenna configation */
-#define PRXS1_HTPHY_MMPLCPLenL_MASK	0xFF00	/* Mixmode PLCP Length low byte mask */
+/* core enables for {3..0}, 0=disabled, 1=enabled */
+#define PRXS1_HTPHY_CORE_MASK	0x000F
+/* antenna configation */
+#define PRXS1_HTPHY_ANTCFG_MASK	0x00F0
+/* Mixmode PLCP Length low byte mask */
+#define PRXS1_HTPHY_MMPLCPLenL_MASK	0xFF00
 
 /* htphy PhyRxStatus_2: */
-#define PRXS2_HTPHY_MMPLCPLenH_MASK	0x000F	/* Mixmode PLCP Length high byte maskw */
-#define PRXS2_HTPHY_MMPLCH_RATE_MASK	0x00F0	/* Mixmode PLCP rate mask */
-#define PRXS2_HTPHY_RXPWR_ANT0	0xFF00	/* Rx power on core 0 */
+/* Mixmode PLCP Length high byte maskw */
+#define PRXS2_HTPHY_MMPLCPLenH_MASK	0x000F
+/* Mixmode PLCP rate mask */
+#define PRXS2_HTPHY_MMPLCH_RATE_MASK	0x00F0
+/* Rx power on core 0 */
+#define PRXS2_HTPHY_RXPWR_ANT0	0xFF00
 
 /* htphy PhyRxStatus_3: */
-#define PRXS3_HTPHY_RXPWR_ANT1	0x00FF	/* Rx power on core 1 */
-#define PRXS3_HTPHY_RXPWR_ANT2	0xFF00	/* Rx power on core 2 */
+/* Rx power on core 1 */
+#define PRXS3_HTPHY_RXPWR_ANT1	0x00FF
+/* Rx power on core 2 */
+#define PRXS3_HTPHY_RXPWR_ANT2	0xFF00
 
 /* htphy PhyRxStatus_4: */
-#define PRXS4_HTPHY_RXPWR_ANT3	0x00FF	/* Rx power on core 3 */
-#define PRXS4_HTPHY_CFO		0xFF00	/* Coarse frequency offset */
+/* Rx power on core 3 */
+#define PRXS4_HTPHY_RXPWR_ANT3	0x00FF
+/* Coarse frequency offset */
+#define PRXS4_HTPHY_CFO		0xFF00
 
 /* htphy PhyRxStatus_5: */
-#define PRXS5_HTPHY_FFO	        0x00FF	/* Fine frequency offset */
-#define PRXS5_HTPHY_AR	        0xFF00	/* Advance Retard */
+/* Fine frequency offset */
+#define PRXS5_HTPHY_FFO	        0x00FF
+/* Advance Retard */
+#define PRXS5_HTPHY_AR	        0xFF00
 
-#define HTPHY_MMPLCPLen(rxs)	((((rxs)->PhyRxStatus_1 & PRXS1_HTPHY_MMPLCPLenL_MASK) >> 8) | \
+#define HTPHY_MMPLCPLen(rxs) \
+	((((rxs)->PhyRxStatus_1 & PRXS1_HTPHY_MMPLCPLenL_MASK) >> 8) | \
 	(((rxs)->PhyRxStatus_2 & PRXS2_HTPHY_MMPLCPLenH_MASK) << 8))
 /* Get Rx power on core 0 */
-#define HTPHY_RXPWR_ANT0(rxs)	((((rxs)->PhyRxStatus_2) & PRXS2_HTPHY_RXPWR_ANT0) >> 8)
+#define HTPHY_RXPWR_ANT0(rxs) \
+	((((rxs)->PhyRxStatus_2) & PRXS2_HTPHY_RXPWR_ANT0) >> 8)
 /* Get Rx power on core 1 */
-#define HTPHY_RXPWR_ANT1(rxs)	(((rxs)->PhyRxStatus_3) & PRXS3_HTPHY_RXPWR_ANT1)
+#define HTPHY_RXPWR_ANT1(rxs) \
+	(((rxs)->PhyRxStatus_3) & PRXS3_HTPHY_RXPWR_ANT1)
 /* Get Rx power on core 2 */
-#define HTPHY_RXPWR_ANT2(rxs)	((((rxs)->PhyRxStatus_3) & PRXS3_HTPHY_RXPWR_ANT2) >> 8)
+#define HTPHY_RXPWR_ANT2(rxs) \
+	((((rxs)->PhyRxStatus_3) & PRXS3_HTPHY_RXPWR_ANT2) >> 8)
 
 /* ucode RxStatus1: */
 #define	RXS_BCNSENT		0x8000
@@ -1400,7 +1515,8 @@ struct brcms_d11rxhdr {
 #define	RXS_SECKINDX_SHIFT	5
 #define	RXS_DECERR		(1 << 4)
 #define	RXS_DECATMPT		(1 << 3)
-#define	RXS_PBPRES		(1 << 2)	/* PAD bytes to make IP data 4 bytes aligned */
+/* PAD bytes to make IP data 4 bytes aligned */
+#define	RXS_PBPRES		(1 << 2)
 #define	RXS_RESPFRAMETX		(1 << 1)
 #define	RXS_FCSERR		(1 << 0)
 
@@ -1433,16 +1549,17 @@ struct brcms_d11rxhdr {
 #define	M_PSM_SOFT_REGS	0x0
 #define	M_BOM_REV_MAJOR	(M_PSM_SOFT_REGS + 0x0)
 #define	M_BOM_REV_MINOR	(M_PSM_SOFT_REGS + 0x2)
-#define	M_UCODE_DBGST	(M_PSM_SOFT_REGS + 0x40)	/* ucode debug status code */
-#define	M_UCODE_MACSTAT	(M_PSM_SOFT_REGS + 0xE0)	/* macstat counters */
+#define	M_UCODE_DBGST	(M_PSM_SOFT_REGS + 0x40) /* ucode debug status code */
+#define	M_UCODE_MACSTAT	(M_PSM_SOFT_REGS + 0xE0) /* macstat counters */
 
-#define M_AGING_THRSH	(0x3e * 2)	/* max time waiting for medium before tx */
-#define	M_MBURST_SIZE	(0x40 * 2)	/* max frames in a frameburst */
-#define	M_MBURST_TXOP	(0x41 * 2)	/* max frameburst TXOP in unit of us */
-#define M_SYNTHPU_DLY	(0x4a * 2)	/* pre-wakeup for synthpu, default: 500 */
+#define M_AGING_THRSH	(0x3e * 2) /* max time waiting for medium before tx */
+#define	M_MBURST_SIZE	(0x40 * 2) /* max frames in a frameburst */
+#define	M_MBURST_TXOP	(0x41 * 2) /* max frameburst TXOP in unit of us */
+#define M_SYNTHPU_DLY	(0x4a * 2) /* pre-wakeup for synthpu, default: 500 */
 #define	M_PRETBTT	(0x4b * 2)
 
-#define M_ALT_TXPWR_IDX		(M_PSM_SOFT_REGS + (0x3b * 2))	/* offset to the target txpwr */
+/* offset to the target txpwr */
+#define M_ALT_TXPWR_IDX		(M_PSM_SOFT_REGS + (0x3b * 2))
 #define M_PHY_TX_FLT_PTR	(M_PSM_SOFT_REGS + (0x3d * 2))
 #define M_CTS_DURATION		(M_PSM_SOFT_REGS + (0x5c * 2))
 #define M_LP_RCCAL_OVR		(M_PSM_SOFT_REGS + (0x6b * 2))
@@ -1451,11 +1568,16 @@ struct brcms_d11rxhdr {
 #define M_RXSTATS_BLK_PTR	(M_PSM_SOFT_REGS + (0x65 * 2))
 
 /* ucode debug status codes */
-#define	DBGST_INACTIVE		0	/* not valid really */
-#define	DBGST_INIT		1	/* after zeroing SHM, before suspending at init */
-#define	DBGST_ACTIVE		2	/* "normal" state */
-#define	DBGST_SUSPENDED		3	/* suspended */
-#define	DBGST_ASLEEP		4	/* asleep (PS mode) */
+/* not valid really */
+#define	DBGST_INACTIVE		0
+/* after zeroing SHM, before suspending at init */
+#define	DBGST_INIT		1
+/* "normal" state */
+#define	DBGST_ACTIVE		2
+/* suspended */
+#define	DBGST_SUSPENDED		3
+/* asleep (PS mode) */
+#define	DBGST_ASLEEP		4
 
 /* Scratch Reg defs */
 enum _ePsmScratchPadRegDefinitions {
@@ -1463,66 +1585,66 @@ enum _ePsmScratchPadRegDefinitions {
 	S_RSV1,
 	S_RSV2,
 
-	/* scratch registers for Dot11-contants */
-	S_DOT11_CWMIN,		/* CW-minimum                                   0x03 */
-	S_DOT11_CWMAX,		/* CW-maximum                                   0x04 */
-	S_DOT11_CWCUR,		/* CW-current                                   0x05 */
-	S_DOT11_SRC_LMT,	/* short retry count limit                      0x06 */
-	S_DOT11_LRC_LMT,	/* long retry count limit                       0x07 */
-	S_DOT11_DTIMCOUNT,	/* DTIM-count                                   0x08 */
-
-	/* Tx-side scratch registers */
-	S_SEQ_NUM,		/* hardware sequence number reg                 0x09 */
-	S_SEQ_NUM_FRAG,		/* seq-num for frags (Set at the start os MSDU  0x0A */
-	S_FRMRETX_CNT,		/* frame retx count                             0x0B */
-	S_SSRC,			/* Station short retry count                    0x0C */
-	S_SLRC,			/* Station long retry count                     0x0D */
-	S_EXP_RSP,		/* Expected response frame                      0x0E */
-	S_OLD_BREM,		/* Remaining backoff ctr                        0x0F */
-	S_OLD_CWWIN,		/* saved-off CW-cur                             0x10 */
-	S_TXECTL,		/* TXE-Ctl word constructed in scr-pad          0x11 */
-	S_CTXTST,		/* frm type-subtype as read from Tx-descr       0x12 */
-
-	/* Rx-side scratch registers */
-	S_RXTST,		/* Type and subtype in Rxframe                  0x13 */
+	/* offset 0x03: scratch registers for Dot11-contants */
+	S_DOT11_CWMIN,		/* CW-minimum */
+	S_DOT11_CWMAX,		/* CW-maximum */
+	S_DOT11_CWCUR,		/* CW-current */
+	S_DOT11_SRC_LMT,	/* short retry count limit */
+	S_DOT11_LRC_LMT,	/* long retry count limit */
+	S_DOT11_DTIMCOUNT,	/* DTIM-count */
+
+	/* offset 0x09: Tx-side scratch registers */
+	S_SEQ_NUM,		/* hardware sequence number reg */
+	S_SEQ_NUM_FRAG,		/* seq num for frags (at the start of MSDU) */
+	S_FRMRETX_CNT,		/* frame retx count */
+	S_SSRC,			/* Station short retry count */
+	S_SLRC,			/* Station long retry count */
+	S_EXP_RSP,		/* Expected response frame */
+	S_OLD_BREM,		/* Remaining backoff ctr */
+	S_OLD_CWWIN,		/* saved-off CW-cur */
+	S_TXECTL,		/* TXE-Ctl word constructed in scr-pad */
+	S_CTXTST,		/* frm type-subtype as read from Tx-descr */
+
+	/* offset 0x13: Rx-side scratch registers */
+	S_RXTST,		/* Type and subtype in Rxframe */
 
 	/* Global state register */
-	S_STREG,		/* state storage actual bit maps below          0x14 */
-
-	S_TXPWR_SUM,		/* Tx power control: accumulator                0x15 */
-	S_TXPWR_ITER,		/* Tx power control: iteration                  0x16 */
-	S_RX_FRMTYPE,		/* Rate and PHY type for frames                 0x17 */
-	S_THIS_AGG,		/* Size of this AGG (A-MSDU)                    0x18 */
-
-	S_KEYINDX,		/*                                              0x19 */
-	S_RXFRMLEN,		/* Receive MPDU length in bytes                 0x1A */
-
-	/* Receive TSF time stored in SCR */
-	S_RXTSFTMRVAL_WD3,	/* TSF value at the start of rx                 0x1B */
-	S_RXTSFTMRVAL_WD2,	/* TSF value at the start of rx                 0x1C */
-	S_RXTSFTMRVAL_WD1,	/* TSF value at the start of rx                 0x1D */
-	S_RXTSFTMRVAL_WD0,	/* TSF value at the start of rx                 0x1E */
-	S_RXSSN,		/* Received start seq number for A-MPDU BA      0x1F */
-	S_RXQOSFLD,		/* Rx-QoS field (if present)                    0x20 */
-
-	/* Scratch pad regs used in microcode as temp storage */
-	S_TMP0,			/* stmp0                                        0x21 */
-	S_TMP1,			/* stmp1                                        0x22 */
-	S_TMP2,			/* stmp2                                        0x23 */
-	S_TMP3,			/* stmp3                                        0x24 */
-	S_TMP4,			/* stmp4                                        0x25 */
-	S_TMP5,			/* stmp5                                        0x26 */
-	S_PRQPENALTY_CTR,	/* Probe response queue penalty counter         0x27 */
-	S_ANTCNT,		/* unsuccessful attempts on current ant.        0x28 */
-	S_SYMBOL,		/* flag for possible symbol ctl frames          0x29 */
-	S_RXTP,			/* rx frame type                                0x2A */
-	S_STREG2,		/* extra state storage                          0x2B */
-	S_STREG3,		/* even more extra state storage                0x2C */
-	S_STREG4,		/* ...                                          0x2D */
-	S_STREG5,		/* remember to initialize it to zero            0x2E */
+	S_STREG,		/* state storage actual bit maps below */
+
+	S_TXPWR_SUM,		/* Tx power control: accumulator */
+	S_TXPWR_ITER,		/* Tx power control: iteration */
+	S_RX_FRMTYPE,		/* Rate and PHY type for frames */
+	S_THIS_AGG,		/* Size of this AGG (A-MSDU) */
+
+	S_KEYINDX,
+	S_RXFRMLEN,		/* Receive MPDU length in bytes */
+
+	/* offset 0x1B: Receive TSF time stored in SCR */
+	S_RXTSFTMRVAL_WD3,	/* TSF value at the start of rx */
+	S_RXTSFTMRVAL_WD2,	/* TSF value at the start of rx */
+	S_RXTSFTMRVAL_WD1,	/* TSF value at the start of rx */
+	S_RXTSFTMRVAL_WD0,	/* TSF value at the start of rx */
+	S_RXSSN,		/* Received start seq number for A-MPDU BA */
+	S_RXQOSFLD,		/* Rx-QoS field (if present) */
+
+	/* offset 0x21: Scratch pad regs used in microcode as temp storage */
+	S_TMP0,			/* stmp0 */
+	S_TMP1,			/* stmp1 */
+	S_TMP2,			/* stmp2 */
+	S_TMP3,			/* stmp3 */
+	S_TMP4,			/* stmp4 */
+	S_TMP5,			/* stmp5 */
+	S_PRQPENALTY_CTR,	/* Probe response queue penalty counter */
+	S_ANTCNT,		/* unsuccessful attempts on current ant. */
+	S_SYMBOL,		/* flag for possible symbol ctl frames */
+	S_RXTP,			/* rx frame type */
+	S_STREG2,		/* extra state storage */
+	S_STREG3,		/* even more extra state storage */
+	S_STREG4,		/* ... */
+	S_STREG5,		/* remember to initialize it to zero */
 
 	S_ADJPWR_IDX,
-	S_CUR_PTR,		/* Temp pointer for A-MPDU re-Tx SHM table      0x32 */
+	S_CUR_PTR,		/* Temp pointer for A-MPDU re-Tx SHM table */
 	S_REVID4,		/* 0x33 */
 	S_INDX,			/* 0x34 */
 	S_ADDR0,		/* 0x35 */
@@ -1532,9 +1654,9 @@ enum _ePsmScratchPadRegDefinitions {
 	S_ADDR4,		/* 0x39 */
 	S_ADDR5,		/* 0x3A */
 	S_TMP6,			/* 0x3B */
-	S_KEYINDX_BU,		/* Backup for Key index                         0x3C */
-	S_MFGTEST_TMP0,		/* Temp register used for RX test calculations  0x3D */
-	S_RXESN,		/* Received end sequence number for A-MPDU BA   0x3E */
+	S_KEYINDX_BU,		/* Backup for Key index */
+	S_MFGTEST_TMP0,		/* Temp regs used for RX test calculations */
+	S_RXESN,		/* Received end sequence number for A-MPDU BA */
 	S_STREG6,		/* 0x3F */
 };
 
@@ -1628,7 +1750,8 @@ struct macstat {
 #define	SISF_FCLKA		0x0004	/* FastClkAvailable */
 #define	SISF_DB_PHY		0x0008	/* Dualband phy */
 
-/* === End of MAC reg, Beginning of PHY(b/a/g/n) reg, radio and LPPHY regs are separated === */
+/* === End of MAC reg, Beginning of PHY(b/a/g/n) reg === */
+/* radio and LPPHY regs are separated */
 
 #define	BPHY_REG_OFT_BASE	0x0
 /* offsets for indirect access to bphy registers */
diff --git a/drivers/staging/brcm80211/brcmsmac/dma.c b/drivers/staging/brcm80211/brcmsmac/dma.c
index ea17671efb63..b56a30297c26 100644
--- a/drivers/staging/brcm80211/brcmsmac/dma.c
+++ b/drivers/staging/brcm80211/brcmsmac/dma.c
@@ -18,17 +18,14 @@
 #include <linux/delay.h>
 #include <linux/pci.h>
 
-#if defined(__mips__)
-#include <asm/addrspace.h>
-#endif
-
 #include <brcmu_utils.h>
 #include <aiutils.h>
 #include "types.h"
 #include "dma.h"
 
 /*
- * Each descriptor ring must be 8kB aligned, and fit within a contiguous 8kB physical address.
+ * DMA hardware requires each descriptor ring to be 8kB aligned, and fit within
+ * a contiguous 8kB physical address.
  */
 #define D64RINGALIGN_BITS	13
 #define	D64MAXRINGSZ		(1 << D64RINGALIGN_BITS)
@@ -69,21 +66,32 @@
 #define	D64_XS1_XE_COREE	0x50000000	/* core error */
 
 /* receive channel control */
-#define	D64_RC_RE		0x00000001	/* receive enable */
-#define	D64_RC_RO_MASK		0x000000fe	/* receive frame offset */
+/* receive enable */
+#define	D64_RC_RE		0x00000001
+/* receive frame offset */
+#define	D64_RC_RO_MASK		0x000000fe
 #define	D64_RC_RO_SHIFT		1
-#define	D64_RC_FM		0x00000100	/* direct fifo receive (pio) mode */
-#define	D64_RC_SH		0x00000200	/* separate rx header descriptor enable */
-#define	D64_RC_OC		0x00000400	/* overflow continue */
-#define	D64_RC_PD		0x00000800	/* parity check disable */
-#define	D64_RC_AE		0x00030000	/* address extension bits */
+/* direct fifo receive (pio) mode */
+#define	D64_RC_FM		0x00000100
+/* separate rx header descriptor enable */
+#define	D64_RC_SH		0x00000200
+/* overflow continue */
+#define	D64_RC_OC		0x00000400
+/* parity check disable */
+#define	D64_RC_PD		0x00000800
+/* address extension bits */
+#define	D64_RC_AE		0x00030000
 #define	D64_RC_AE_SHIFT		16
 
 /* flags for dma controller */
-#define DMA_CTRL_PEN		(1 << 0)	/* partity enable */
-#define DMA_CTRL_ROC		(1 << 1)	/* rx overflow continue */
-#define DMA_CTRL_RXMULTI	(1 << 2)	/* allow rx scatter to multiple descriptors */
-#define DMA_CTRL_UNFRAMED	(1 << 3)	/* Unframed Rx/Tx data */
+/* partity enable */
+#define DMA_CTRL_PEN		(1 << 0)
+/* rx overflow continue */
+#define DMA_CTRL_ROC		(1 << 1)
+/* allow rx scatter to multiple descriptors */
+#define DMA_CTRL_RXMULTI	(1 << 2)
+/* Unframed Rx/Tx data */
+#define DMA_CTRL_UNFRAMED	(1 << 3)
 
 /* receive descriptor table pointer */
 #define	D64_RP_LD_MASK		0x00000fff	/* last valid descriptor */
@@ -131,10 +139,13 @@
 #define	D64_CTRL1_SOF		((u32)1 << 31)	/* start of frame */
 
 /* descriptor control flags 2 */
-#define	D64_CTRL2_BC_MASK	0x00007fff	/* buffer byte count. real data len must <= 16KB */
-#define	D64_CTRL2_AE		0x00030000	/* address extension bits */
+/* buffer byte count. real data len must <= 16KB */
+#define	D64_CTRL2_BC_MASK	0x00007fff
+/* address extension bits */
+#define	D64_CTRL2_AE		0x00030000
 #define	D64_CTRL2_AE_SHIFT	16
-#define D64_CTRL2_PARITY	0x00040000	/* parity bit */
+/* parity bit */
+#define D64_CTRL2_PARITY	0x00040000
 
 /* control flags in the range [27:20] are core-specific and not defined here */
 #define	D64_CTRL_CORE_MASK	0x0ff00000
@@ -144,15 +155,13 @@
 #define D64_RX_FRM_STS_DSCRCNT	0x0f000000  /* no. of descriptors used - 1 */
 #define D64_RX_FRM_STS_DATATYPE	0xf0000000	/* core-dependent data type */
 
-#define	DMADDRWIDTH_30  30	/* 30-bit addressing capability */
-#define	DMADDRWIDTH_32  32	/* 32-bit addressing capability */
-#define	DMADDRWIDTH_63  63	/* 64-bit addressing capability */
-#define	DMADDRWIDTH_64  64	/* 64-bit addressing capability */
-
-/* packet headroom necessary to accommodate the largest header in the system, (i.e TXOFF).
- * By doing, we avoid the need  to allocate an extra buffer for the header when bridging to WL.
- * There is a compile time check in wlc.c which ensure that this value is at least as big
- * as TXOFF. This value is used in dma_rxfill (dma.c).
+/*
+ * packet headroom necessary to accommodate the largest header
+ * in the system, (i.e TXOFF). By doing, we avoid the need to
+ * allocate an extra buffer for the header when bridging to WL.
+ * There is a compile time check in wlc.c which ensure that this
+ * value is at least as big as TXOFF. This value is used in
+ * dma_rxfill().
  */
 
 #define BCMEXTRAHDROOM 172
@@ -180,52 +189,27 @@
 
 #define	DMA_NONE(args)
 
-typedef unsigned long dmaaddr_t;
-#define PHYSADDRHI(_pa) (0)
-#define PHYSADDRHISET(_pa, _val)
-#define PHYSADDRLO(_pa) ((_pa))
-#define PHYSADDRLOSET(_pa, _val) \
-	do { \
-		(_pa) = (_val);			\
-	} while (0)
-
-#define d64txregs	dregs.d64_u.txregs_64
-#define d64rxregs	dregs.d64_u.rxregs_64
-#define txd64		dregs.d64_u.txd_64
-#define rxd64		dregs.d64_u.rxd_64
-
-/* default dma message level (if input msg_level pointer is null in dma_attach()) */
-static uint dma_msg_level;
-
 #define	MAXNAMEL	8	/* 8 char names */
 
-#define	DI_INFO(dmah)	((dma_info_t *)dmah)
-
-#define R_SM(r)		(*(r))
-#define W_SM(r, v)	(*(r) = (v))
+/* macros to convert between byte offsets and indexes */
+#define	B2I(bytes, type)	((bytes) / sizeof(type))
+#define	I2B(index, type)	((index) * sizeof(type))
 
-/* One physical DMA segment */
-struct dma_seg {
-	dmaaddr_t addr;
-	u32 length;
-};
+#define	PCI32ADDR_HIGH		0xc0000000	/* address[31:30] */
+#define	PCI32ADDR_HIGH_SHIFT	30	/* address[31:30] */
 
-struct dma_seg_map {
-	void *oshdmah;		/* Opaque handle for OSL to store its information */
-	uint origsize;		/* Size of the virtual packet */
-	uint nsegs;
-	struct dma_seg segs[MAX_DMA_SEGS];
-};
+#define	PCI64ADDR_HIGH		0x80000000	/* address[63] */
+#define	PCI64ADDR_HIGH_SHIFT	31	/* address[63] */
 
 /*
  * DMA Descriptor
  * Descriptors are only read by the hardware, never written back.
  */
 struct dma64desc {
-	u32 ctrl1;		/* misc control bits & bufcount */
-	u32 ctrl2;		/* buffer count and address extension */
-	u32 addrlow;		/* memory address of the date buffer, bits 31:0 */
-	u32 addrhigh;	/* memory address of the date buffer, bits 63:32 */
+	__le32 ctrl1;	/* misc control bits & bufcount */
+	__le32 ctrl2;	/* buffer count and address extension */
+	__le32 addrlow;	/* memory address of the date buffer, bits 31:0 */
+	__le32 addrhigh; /* memory address of the date buffer, bits 63:32 */
 };
 
 /* dma engine software state */
@@ -234,208 +218,344 @@ struct dma_info {
 	uint *msg_level;	/* message level pointer */
 	char name[MAXNAMEL];	/* callers name for diag msgs */
 
-	void *pbus;		/* bus handle */
+	struct pci_dev *pbus;		/* bus handle */
 
-	bool dma64;		/* this dma engine is operating in 64-bit mode */
-	bool addrext;		/* this dma engine supports DmaExtendedAddrChanges */
+	bool dma64;	/* this dma engine is operating in 64-bit mode */
+	bool addrext;	/* this dma engine supports DmaExtendedAddrChanges */
 
-	union {
-		struct {
-			dma64regs_t *txregs_64;	/* 64-bit dma tx engine registers */
-			dma64regs_t *rxregs_64;	/* 64-bit dma rx engine registers */
-			/* pointer to dma64 tx descriptor ring */
-			struct dma64desc *txd_64;
-			/* pointer to dma64 rx descriptor ring */
-			struct dma64desc *rxd_64;
-		} d64_u;
-	} dregs;
+	/* 64-bit dma tx engine registers */
+	struct dma64regs __iomem *d64txregs;
+	/* 64-bit dma rx engine registers */
+	struct dma64regs __iomem *d64rxregs;
+	/* pointer to dma64 tx descriptor ring */
+	struct dma64desc *txd64;
+	/* pointer to dma64 rx descriptor ring */
+	struct dma64desc *rxd64;
 
 	u16 dmadesc_align;	/* alignment requirement for dma descriptors */
 
 	u16 ntxd;		/* # tx descriptors tunable */
 	u16 txin;		/* index of next descriptor to reclaim */
 	u16 txout;		/* index of next descriptor to post */
-	void **txp;		/* pointer to parallel array of pointers to packets */
-	struct dma_seg_map *txp_dmah;	/* DMA MAP meta-data handle */
-	dmaaddr_t txdpa;	/* Aligned physical address of descriptor ring */
-	dmaaddr_t txdpaorig;	/* Original physical address of descriptor ring */
+	/* pointer to parallel array of pointers to packets */
+	struct sk_buff **txp;
+	/* Aligned physical address of descriptor ring */
+	dma_addr_t txdpa;
+	/* Original physical address of descriptor ring */
+	dma_addr_t txdpaorig;
 	u16 txdalign;	/* #bytes added to alloc'd mem to align txd */
 	u32 txdalloc;	/* #bytes allocated for the ring */
 	u32 xmtptrbase;	/* When using unaligned descriptors, the ptr register
-				 * is not just an index, it needs all 13 bits to be
-				 * an offset from the addr register.
-				 */
+			 * is not just an index, it needs all 13 bits to be
+			 * an offset from the addr register.
+			 */
 
-	u16 nrxd;		/* # rx descriptors tunable */
-	u16 rxin;		/* index of next descriptor to reclaim */
-	u16 rxout;		/* index of next descriptor to post */
-	void **rxp;		/* pointer to parallel array of pointers to packets */
-	struct dma_seg_map *rxp_dmah;	/* DMA MAP meta-data handle */
-	dmaaddr_t rxdpa;	/* Aligned physical address of descriptor ring */
-	dmaaddr_t rxdpaorig;	/* Original physical address of descriptor ring */
+	u16 nrxd;	/* # rx descriptors tunable */
+	u16 rxin;	/* index of next descriptor to reclaim */
+	u16 rxout;	/* index of next descriptor to post */
+	/* pointer to parallel array of pointers to packets */
+	struct sk_buff **rxp;
+	/* Aligned physical address of descriptor ring */
+	dma_addr_t rxdpa;
+	/* Original physical address of descriptor ring */
+	dma_addr_t rxdpaorig;
 	u16 rxdalign;	/* #bytes added to alloc'd mem to align rxd */
 	u32 rxdalloc;	/* #bytes allocated for the ring */
 	u32 rcvptrbase;	/* Base for ptr reg when using unaligned descriptors */
 
 	/* tunables */
-	unsigned int rxbufsize;	/* rx buffer size in bytes,
-				 * not including the extra headroom
+	unsigned int rxbufsize;	/* rx buffer size in bytes, not including
+				 * the extra headroom
 				 */
-	uint rxextrahdrroom;	/* extra rx headroom, reverseved to assist upper stack
-				 *  e.g. some rx pkt buffers will be bridged to tx side
-				 *  without byte copying. The extra headroom needs to be
-				 *  large enough to fit txheader needs.
-				 *  Some dongle driver may not need it.
+	uint rxextrahdrroom;	/* extra rx headroom, reverseved to assist upper
+				 * stack, e.g. some rx pkt buffers will be
+				 * bridged to tx side without byte copying.
+				 * The extra headroom needs to be large enough
+				 * to fit txheader needs. Some dongle driver may
+				 * not need it.
 				 */
 	uint nrxpost;		/* # rx buffers to keep posted */
 	unsigned int rxoffset;	/* rxcontrol offset */
-	uint ddoffsetlow;	/* add to get dma address of descriptor ring, low 32 bits */
-	uint ddoffsethigh;	/*   high 32 bits */
-	uint dataoffsetlow;	/* add to get dma address of data buffer, low 32 bits */
-	uint dataoffsethigh;	/*   high 32 bits */
-	bool aligndesc_4k;	/* descriptor base need to be aligned or not */
+	/* add to get dma address of descriptor ring, low 32 bits */
+	uint ddoffsetlow;
+	/*   high 32 bits */
+	uint ddoffsethigh;
+	/* add to get dma address of data buffer, low 32 bits */
+	uint dataoffsetlow;
+	/*   high 32 bits */
+	uint dataoffsethigh;
+	/* descriptor base need to be aligned or not */
+	bool aligndesc_4k;
 };
 
-/* DMA Scatter-gather list is supported. Note this is limited to TX direction only */
-#ifdef BCMDMASGLISTOSL
-#define DMASGLIST_ENAB true
-#else
-#define DMASGLIST_ENAB false
-#endif				/* BCMDMASGLISTOSL */
+/*
+ * default dma message level (if input msg_level
+ * pointer is null in dma_attach())
+ */
+static uint dma_msg_level;
 
-/* descriptor bumping macros */
-#define	XXD(x, n)	((x) & ((n) - 1))	/* faster than %, but n must be power of 2 */
-#define	TXD(x)		XXD((x), di->ntxd)
-#define	RXD(x)		XXD((x), di->nrxd)
-#define	NEXTTXD(i)	TXD((i) + 1)
-#define	PREVTXD(i)	TXD((i) - 1)
-#define	NEXTRXD(i)	RXD((i) + 1)
-#define	PREVRXD(i)	RXD((i) - 1)
+/* Check for odd number of 1's */
+static u32 parity32(__le32 data)
+{
+	/* no swap needed for counting 1's */
+	u32 par_data = *(u32 *)&data;
 
-#define	NTXDACTIVE(h, t)	TXD((t) - (h))
-#define	NRXDACTIVE(h, t)	RXD((t) - (h))
+	par_data ^= par_data >> 16;
+	par_data ^= par_data >> 8;
+	par_data ^= par_data >> 4;
+	par_data ^= par_data >> 2;
+	par_data ^= par_data >> 1;
 
-/* macros to convert between byte offsets and indexes */
-#define	B2I(bytes, type)	((bytes) / sizeof(type))
-#define	I2B(index, type)	((index) * sizeof(type))
+	return par_data & 1;
+}
 
-#define	PCI32ADDR_HIGH		0xc0000000	/* address[31:30] */
-#define	PCI32ADDR_HIGH_SHIFT	30	/* address[31:30] */
+static bool dma64_dd_parity(struct dma64desc *dd)
+{
+	return parity32(dd->addrlow ^ dd->addrhigh ^ dd->ctrl1 ^ dd->ctrl2);
+}
 
-#define	PCI64ADDR_HIGH		0x80000000	/* address[63] */
-#define	PCI64ADDR_HIGH_SHIFT	31	/* address[63] */
+/* descriptor bumping functions */
+
+static uint xxd(uint x, uint n)
+{
+	return x & (n - 1); /* faster than %, but n must be power of 2 */
+}
+
+static uint txd(struct dma_info *di, uint x)
+{
+	return xxd(x, di->ntxd);
+}
+
+static uint rxd(struct dma_info *di, uint x)
+{
+	return xxd(x, di->nrxd);
+}
+
+static uint nexttxd(struct dma_info *di, uint i)
+{
+	return txd(di, i + 1);
+}
+
+static uint prevtxd(struct dma_info *di, uint i)
+{
+	return txd(di, i - 1);
+}
+
+static uint nextrxd(struct dma_info *di, uint i)
+{
+	return txd(di, i + 1);
+}
+
+static uint ntxdactive(struct dma_info *di, uint h, uint t)
+{
+	return txd(di, t-h);
+}
+
+static uint nrxdactive(struct dma_info *di, uint h, uint t)
+{
+	return rxd(di, t-h);
+}
+
+static uint _dma_ctrlflags(struct dma_info *di, uint mask, uint flags)
+{
+	uint dmactrlflags = di->dma.dmactrlflags;
+
+	if (di == NULL) {
+		DMA_ERROR(("%s: _dma_ctrlflags: NULL dma handle\n", di->name));
+		return 0;
+	}
+
+	dmactrlflags &= ~mask;
+	dmactrlflags |= flags;
+
+	/* If trying to enable parity, check if parity is actually supported */
+	if (dmactrlflags & DMA_CTRL_PEN) {
+		u32 control;
+
+		control = R_REG(&di->d64txregs->control);
+		W_REG(&di->d64txregs->control,
+		      control | D64_XC_PD);
+		if (R_REG(&di->d64txregs->control) & D64_XC_PD)
+			/* We *can* disable it so it is supported,
+			 * restore control register
+			 */
+			W_REG(&di->d64txregs->control,
+			control);
+		else
+			/* Not supported, don't allow it to be enabled */
+			dmactrlflags &= ~DMA_CTRL_PEN;
+	}
+
+	di->dma.dmactrlflags = dmactrlflags;
+
+	return dmactrlflags;
+}
+
+static bool _dma64_addrext(struct dma64regs __iomem *dma64regs)
+{
+	u32 w;
+	OR_REG(&dma64regs->control, D64_XC_AE);
+	w = R_REG(&dma64regs->control);
+	AND_REG(&dma64regs->control, ~D64_XC_AE);
+	return (w & D64_XC_AE) == D64_XC_AE;
+}
+
+/*
+ * return true if this dma engine supports DmaExtendedAddrChanges,
+ * otherwise false
+ */
+static bool _dma_isaddrext(struct dma_info *di)
+{
+	/* DMA64 supports full 32- or 64-bit operation. AE is always valid */
+
+	/* not all tx or rx channel are available */
+	if (di->d64txregs != NULL) {
+		if (!_dma64_addrext(di->d64txregs))
+			DMA_ERROR(("%s: _dma_isaddrext: DMA64 tx doesn't have "
+				   "AE set\n", di->name));
+		return true;
+	} else if (di->d64rxregs != NULL) {
+		if (!_dma64_addrext(di->d64rxregs))
+			DMA_ERROR(("%s: _dma_isaddrext: DMA64 rx doesn't have "
+				   "AE set\n", di->name));
+		return true;
+	}
 
-/* Common prototypes */
-static bool _dma_isaddrext(struct dma_info *di);
-static bool _dma_descriptor_align(struct dma_info *di);
-static bool _dma_alloc(struct dma_info *di, uint direction);
-static void _dma_detach(struct dma_info *di);
-static void _dma_ddtable_init(struct dma_info *di, uint direction,
-			      dmaaddr_t pa);
-static void _dma_rxinit(struct dma_info *di);
-static void *_dma_rx(struct dma_info *di);
-static bool _dma_rxfill(struct dma_info *di);
-static void _dma_rxreclaim(struct dma_info *di);
-static void _dma_rxenable(struct dma_info *di);
-static void *_dma_getnextrxp(struct dma_info *di, bool forceall);
-static void _dma_rx_param_get(struct dma_info *di, u16 *rxoffset,
-			      u16 *rxbufsize);
-
-static void _dma_txblock(struct dma_info *di);
-static void _dma_txunblock(struct dma_info *di);
-static uint _dma_txactive(struct dma_info *di);
-static uint _dma_rxactive(struct dma_info *di);
-static uint _dma_txpending(struct dma_info *di);
-static uint _dma_txcommitted(struct dma_info *di);
-
-static void *_dma_peeknexttxp(struct dma_info *di);
-static void *_dma_peeknextrxp(struct dma_info *di);
-static unsigned long _dma_getvar(struct dma_info *di, const char *name);
-static void _dma_counterreset(struct dma_info *di);
-static void _dma_fifoloopbackenable(struct dma_info *di);
-static uint _dma_ctrlflags(struct dma_info *di, uint mask, uint flags);
-static u8 dma_align_sizetobits(uint size);
+	return false;
+}
+
+static bool _dma_descriptor_align(struct dma_info *di)
+{
+	u32 addrl;
+
+	/* Check to see if the descriptors need to be aligned on 4K/8K or not */
+	if (di->d64txregs != NULL) {
+		W_REG(&di->d64txregs->addrlow, 0xff0);
+		addrl = R_REG(&di->d64txregs->addrlow);
+		if (addrl != 0)
+			return false;
+	} else if (di->d64rxregs != NULL) {
+		W_REG(&di->d64rxregs->addrlow, 0xff0);
+		addrl = R_REG(&di->d64rxregs->addrlow);
+		if (addrl != 0)
+			return false;
+	}
+	return true;
+}
+
+/*
+ * Descriptor table must start at the DMA hardware dictated alignment, so
+ * allocated memory must be large enough to support this requirement.
+ */
+static void *dma_alloc_consistent(struct pci_dev *pdev, uint size,
+				  u16 align_bits, uint *alloced,
+				  dma_addr_t *pap)
+{
+	if (align_bits) {
+		u16 align = (1 << align_bits);
+		if (!IS_ALIGNED(PAGE_SIZE, align))
+			size += align;
+		*alloced = size;
+	}
+	return pci_alloc_consistent(pdev, size, pap);
+}
+
+static
+u8 dma_align_sizetobits(uint size)
+{
+	u8 bitpos = 0;
+	while (size >>= 1)
+		bitpos++;
+	return bitpos;
+}
+
+/* This function ensures that the DMA descriptor ring will not get allocated
+ * across Page boundary. If the allocation is done across the page boundary
+ * at the first time, then it is freed and the allocation is done at
+ * descriptor ring size aligned location. This will ensure that the ring will
+ * not cross page boundary
+ */
 static void *dma_ringalloc(struct dma_info *di, u32 boundary, uint size,
 			   u16 *alignbits, uint *alloced,
-			   dmaaddr_t *descpa);
-
-/* Prototypes for 64-bit routines */
-static bool dma64_alloc(struct dma_info *di, uint direction);
-static bool dma64_txreset(struct dma_info *di);
-static bool dma64_rxreset(struct dma_info *di);
-static bool dma64_txsuspendedidle(struct dma_info *di);
-static int dma64_txfast(struct dma_info *di, struct sk_buff *p0, bool commit);
-static int dma64_txunframed(struct dma_info *di, void *p0, uint len,
-			    bool commit);
-static void *dma64_getpos(struct dma_info *di, bool direction);
-static void *dma64_getnexttxp(struct dma_info *di, enum txd_range range);
-static void *dma64_getnextrxp(struct dma_info *di, bool forceall);
-static void dma64_txrotate(struct dma_info *di);
-
-static bool dma64_rxidle(struct dma_info *di);
-static void dma64_txinit(struct dma_info *di);
-static bool dma64_txenabled(struct dma_info *di);
-static void dma64_txsuspend(struct dma_info *di);
-static void dma64_txresume(struct dma_info *di);
-static bool dma64_txsuspended(struct dma_info *di);
-static void dma64_txreclaim(struct dma_info *di, enum txd_range range);
-static bool dma64_txstopped(struct dma_info *di);
-static bool dma64_rxstopped(struct dma_info *di);
-static bool dma64_rxenabled(struct dma_info *di);
-static bool _dma64_addrext(dma64regs_t *dma64regs);
-
-static inline u32 parity32(u32 data);
-
-const struct di_fcn_s dma64proc = {
-	(di_detach_t) _dma_detach,
-	(di_txinit_t) dma64_txinit,
-	(di_txreset_t) dma64_txreset,
-	(di_txenabled_t) dma64_txenabled,
-	(di_txsuspend_t) dma64_txsuspend,
-	(di_txresume_t) dma64_txresume,
-	(di_txsuspended_t) dma64_txsuspended,
-	(di_txsuspendedidle_t) dma64_txsuspendedidle,
-	(di_txfast_t) dma64_txfast,
-	(di_txunframed_t) dma64_txunframed,
-	(di_getpos_t) dma64_getpos,
-	(di_txstopped_t) dma64_txstopped,
-	(di_txreclaim_t) dma64_txreclaim,
-	(di_getnexttxp_t) dma64_getnexttxp,
-	(di_peeknexttxp_t) _dma_peeknexttxp,
-	(di_txblock_t) _dma_txblock,
-	(di_txunblock_t) _dma_txunblock,
-	(di_txactive_t) _dma_txactive,
-	(di_txrotate_t) dma64_txrotate,
-
-	(di_rxinit_t) _dma_rxinit,
-	(di_rxreset_t) dma64_rxreset,
-	(di_rxidle_t) dma64_rxidle,
-	(di_rxstopped_t) dma64_rxstopped,
-	(di_rxenable_t) _dma_rxenable,
-	(di_rxenabled_t) dma64_rxenabled,
-	(di_rx_t) _dma_rx,
-	(di_rxfill_t) _dma_rxfill,
-	(di_rxreclaim_t) _dma_rxreclaim,
-	(di_getnextrxp_t) _dma_getnextrxp,
-	(di_peeknextrxp_t) _dma_peeknextrxp,
-	(di_rxparam_get_t) _dma_rx_param_get,
-
-	(di_fifoloopbackenable_t) _dma_fifoloopbackenable,
-	(di_getvar_t) _dma_getvar,
-	(di_counterreset_t) _dma_counterreset,
-	(di_ctrlflags_t) _dma_ctrlflags,
-	NULL,
-	NULL,
-	NULL,
-	(di_rxactive_t) _dma_rxactive,
-	(di_txpending_t) _dma_txpending,
-	(di_txcommitted_t) _dma_txcommitted,
-	39
-};
+			   dma_addr_t *descpa)
+{
+	void *va;
+	u32 desc_strtaddr;
+	u32 alignbytes = 1 << *alignbits;
+
+	va = dma_alloc_consistent(di->pbus, size, *alignbits, alloced, descpa);
+
+	if (NULL == va)
+		return NULL;
+
+	desc_strtaddr = (u32) roundup((unsigned long)va, alignbytes);
+	if (((desc_strtaddr + size - 1) & boundary) != (desc_strtaddr
+							& boundary)) {
+		*alignbits = dma_align_sizetobits(size);
+		pci_free_consistent(di->pbus, size, va, *descpa);
+		va = dma_alloc_consistent(di->pbus, size, *alignbits,
+			alloced, descpa);
+	}
+	return va;
+}
+
+static bool dma64_alloc(struct dma_info *di, uint direction)
+{
+	u16 size;
+	uint ddlen;
+	void *va;
+	uint alloced = 0;
+	u16 align;
+	u16 align_bits;
+
+	ddlen = sizeof(struct dma64desc);
+
+	size = (direction == DMA_TX) ? (di->ntxd * ddlen) : (di->nrxd * ddlen);
+	align_bits = di->dmadesc_align;
+	align = (1 << align_bits);
+
+	if (direction == DMA_TX) {
+		va = dma_ringalloc(di, D64RINGALIGN, size, &align_bits,
+			&alloced, &di->txdpaorig);
+		if (va == NULL) {
+			DMA_ERROR(("%s: dma64_alloc: DMA_ALLOC_CONSISTENT(ntxd)"
+				   " failed\n", di->name));
+			return false;
+		}
+		align = (1 << align_bits);
+		di->txd64 = (struct dma64desc *)
+					roundup((unsigned long)va, align);
+		di->txdalign = (uint) ((s8 *)di->txd64 - (s8 *) va);
+		di->txdpa = di->txdpaorig + di->txdalign;
+		di->txdalloc = alloced;
+	} else {
+		va = dma_ringalloc(di, D64RINGALIGN, size, &align_bits,
+			&alloced, &di->rxdpaorig);
+		if (va == NULL) {
+			DMA_ERROR(("%s: dma64_alloc: DMA_ALLOC_CONSISTENT(nrxd)"
+				   " failed\n", di->name));
+			return false;
+		}
+		align = (1 << align_bits);
+		di->rxd64 = (struct dma64desc *)
+					roundup((unsigned long)va, align);
+		di->rxdalign = (uint) ((s8 *)di->rxd64 - (s8 *) va);
+		di->rxdpa = di->rxdpaorig + di->rxdalign;
+		di->rxdalloc = alloced;
+	}
+
+	return true;
+}
+
+static bool _dma_alloc(struct dma_info *di, uint direction)
+{
+	return dma64_alloc(di, direction);
+}
 
 struct dma_pub *dma_attach(char *name, struct si_pub *sih,
-		     void *dmaregstx, void *dmaregsrx, uint ntxd,
-		     uint nrxd, uint rxbufsize, int rxextheadroom,
+		     void __iomem *dmaregstx, void __iomem *dmaregsrx,
+		     uint ntxd, uint nrxd,
+		     uint rxbufsize, int rxextheadroom,
 		     uint nrxpost, uint rxoffset, uint *msg_level)
 {
 	struct dma_info *di;
@@ -443,12 +563,8 @@ struct dma_pub *dma_attach(char *name, struct si_pub *sih,
 
 	/* allocate private info structure */
 	di = kzalloc(sizeof(struct dma_info), GFP_ATOMIC);
-	if (di == NULL) {
-#ifdef BCMDBG
-		printk(KERN_ERR "dma_attach: out of memory\n");
-#endif
+	if (di == NULL)
 		return NULL;
-	}
 
 	di->msg_level = msg_level ? msg_level : &dma_msg_level;
 
@@ -456,17 +572,15 @@ struct dma_pub *dma_attach(char *name, struct si_pub *sih,
 	di->dma64 = ((ai_core_sflags(sih, 0, 0) & SISF_DMA64) == SISF_DMA64);
 
 	/* init dma reg pointer */
-	di->d64txregs = (dma64regs_t *) dmaregstx;
-	di->d64rxregs = (dma64regs_t *) dmaregsrx;
-	di->dma.di_fn = (const struct di_fcn_s *)&dma64proc;
-
-	/* Default flags (which can be changed by the driver calling dma_ctrlflags
-	 * before enable): For backwards compatibility both Rx Overflow Continue
-	 * and Parity are DISABLED.
-	 * supports it.
+	di->d64txregs = (struct dma64regs __iomem *) dmaregstx;
+	di->d64rxregs = (struct dma64regs __iomem *) dmaregsrx;
+
+	/*
+	 * Default flags (which can be changed by the driver calling
+	 * dma_ctrlflags before enable): For backwards compatibility
+	 * both Rx Overflow Continue and Parity are DISABLED.
 	 */
-	di->dma.di_fn->ctrlflags(&di->dma, DMA_CTRL_ROC | DMA_CTRL_PEN,
-				 0);
+	_dma_ctrlflags(di, DMA_CTRL_ROC | DMA_CTRL_PEN, 0);
 
 	DMA_TRACE(("%s: dma_attach: %s flags 0x%x ntxd %d nrxd %d "
 		   "rxbufsize %d rxextheadroom %d nrxpost %d rxoffset %d "
@@ -497,23 +611,18 @@ struct dma_pub *dma_attach(char *name, struct si_pub *sih,
 
 	/*
 	 * figure out the DMA physical address offset for dd and data
-	 *     PCI/PCIE: they map silicon backplace address to zero based memory, need offset
-	 *     Other bus: use zero
-	 *     SI_BUS BIGENDIAN kludge: use sdram swapped region for data buffer, not descriptor
+	 *     PCI/PCIE: they map silicon backplace address to zero
+	 *     based memory, need offset
+	 *     Other bus: use zero SI_BUS BIGENDIAN kludge: use sdram
+	 *     swapped region for data buffer, not descriptor
 	 */
 	di->ddoffsetlow = 0;
 	di->dataoffsetlow = 0;
-	/* for pci bus, add offset */
-	if (sih->bustype == PCI_BUS) {
-		/* pcie with DMA64 */
-		di->ddoffsetlow = 0;
-		di->ddoffsethigh = SI_PCIE_DMA_H32;
-		di->dataoffsetlow = di->ddoffsetlow;
-		di->dataoffsethigh = di->ddoffsethigh;
-	}
-#if defined(__mips__) && defined(IL_BIGENDIAN)
-	di->dataoffsetlow = di->dataoffsetlow + SI_SDRAM_SWAPPED;
-#endif				/* defined(__mips__) && defined(IL_BIGENDIAN) */
+	/* add offset for pcie with DMA64 bus */
+	di->ddoffsetlow = 0;
+	di->ddoffsethigh = SI_PCIE_DMA_H32;
+	di->dataoffsetlow = di->ddoffsetlow;
+	di->dataoffsethigh = di->ddoffsethigh;
 	/* WAR64450 : DMACtl.Addr ext fields are not supported in SDIOD core. */
 	if ((ai_coreid(sih) == SDIOD_CORE_ID)
 	    && ((ai_corerev(sih) > 0) && (ai_corerev(sih) <= 2)))
@@ -524,16 +633,16 @@ struct dma_pub *dma_attach(char *name, struct si_pub *sih,
 	else
 		di->addrext = _dma_isaddrext(di);
 
-	/* does the descriptors need to be aligned and if yes, on 4K/8K or not */
+	/* does the descriptor need to be aligned and if yes, on 4K/8K or not */
 	di->aligndesc_4k = _dma_descriptor_align(di);
 	if (di->aligndesc_4k) {
 		di->dmadesc_align = D64RINGALIGN_BITS;
-		if ((ntxd < D64MAXDD / 2) && (nrxd < D64MAXDD / 2)) {
+		if ((ntxd < D64MAXDD / 2) && (nrxd < D64MAXDD / 2))
 			/* for smaller dd table, HW relax alignment reqmnt */
 			di->dmadesc_align = D64RINGALIGN_BITS - 1;
-		}
-	} else
+	} else {
 		di->dmadesc_align = 4;	/* 16 byte alignment */
+	}
 
 	DMA_NONE(("DMA descriptor align_needed %d, align %d\n",
 		  di->aligndesc_4k, di->dmadesc_align));
@@ -542,148 +651,97 @@ struct dma_pub *dma_attach(char *name, struct si_pub *sih,
 	if (ntxd) {
 		size = ntxd * sizeof(void *);
 		di->txp = kzalloc(size, GFP_ATOMIC);
-		if (di->txp == NULL) {
-			DMA_ERROR(("%s: dma_attach: out of tx memory\n", di->name));
+		if (di->txp == NULL)
 			goto fail;
-		}
 	}
 
 	/* allocate rx packet pointer vector */
 	if (nrxd) {
 		size = nrxd * sizeof(void *);
 		di->rxp = kzalloc(size, GFP_ATOMIC);
-		if (di->rxp == NULL) {
-			DMA_ERROR(("%s: dma_attach: out of rx memory\n", di->name));
+		if (di->rxp == NULL)
 			goto fail;
-		}
 	}
 
-	/* allocate transmit descriptor ring, only need ntxd descriptors but it must be aligned */
+	/*
+	 * allocate transmit descriptor ring, only need ntxd descriptors
+	 * but it must be aligned
+	 */
 	if (ntxd) {
 		if (!_dma_alloc(di, DMA_TX))
 			goto fail;
 	}
 
-	/* allocate receive descriptor ring, only need nrxd descriptors but it must be aligned */
+	/*
+	 * allocate receive descriptor ring, only need nrxd descriptors
+	 * but it must be aligned
+	 */
 	if (nrxd) {
 		if (!_dma_alloc(di, DMA_RX))
 			goto fail;
 	}
 
 	if ((di->ddoffsetlow != 0) && !di->addrext) {
-		if (PHYSADDRLO(di->txdpa) > SI_PCI_DMA_SZ) {
-			DMA_ERROR(("%s: dma_attach: txdpa 0x%x: addrext not supported\n", di->name, (u32) PHYSADDRLO(di->txdpa)));
+		if (di->txdpa > SI_PCI_DMA_SZ) {
+			DMA_ERROR(("%s: dma_attach: txdpa 0x%x: addrext not "
+				   "supported\n", di->name, (u32)di->txdpa));
 			goto fail;
 		}
-		if (PHYSADDRLO(di->rxdpa) > SI_PCI_DMA_SZ) {
-			DMA_ERROR(("%s: dma_attach: rxdpa 0x%x: addrext not supported\n", di->name, (u32) PHYSADDRLO(di->rxdpa)));
+		if (di->rxdpa > SI_PCI_DMA_SZ) {
+			DMA_ERROR(("%s: dma_attach: rxdpa 0x%x: addrext not "
+				   "supported\n", di->name, (u32)di->rxdpa));
 			goto fail;
 		}
 	}
 
-	DMA_TRACE(("ddoffsetlow 0x%x ddoffsethigh 0x%x dataoffsetlow 0x%x dataoffsethigh " "0x%x addrext %d\n", di->ddoffsetlow, di->ddoffsethigh, di->dataoffsetlow, di->dataoffsethigh, di->addrext));
-
-	/* allocate DMA mapping vectors */
-	if (DMASGLIST_ENAB) {
-		if (ntxd) {
-			size = ntxd * sizeof(struct dma_seg_map);
-			di->txp_dmah = kzalloc(size, GFP_ATOMIC);
-			if (di->txp_dmah == NULL)
-				goto fail;
-		}
-
-		if (nrxd) {
-			size = nrxd * sizeof(struct dma_seg_map);
-			di->rxp_dmah = kzalloc(size, GFP_ATOMIC);
-			if (di->rxp_dmah == NULL)
-				goto fail;
-		}
-	}
+	DMA_TRACE(("ddoffsetlow 0x%x ddoffsethigh 0x%x dataoffsetlow 0x%x "
+		   "dataoffsethigh " "0x%x addrext %d\n", di->ddoffsetlow,
+		   di->ddoffsethigh, di->dataoffsetlow, di->dataoffsethigh,
+		   di->addrext));
 
 	return (struct dma_pub *) di;
 
  fail:
-	_dma_detach(di);
+	dma_detach((struct dma_pub *)di);
 	return NULL;
 }
 
-/* Check for odd number of 1's */
-static inline u32 parity32(u32 data)
-{
-	data ^= data >> 16;
-	data ^= data >> 8;
-	data ^= data >> 4;
-	data ^= data >> 2;
-	data ^= data >> 1;
-
-	return data & 1;
-}
-
-#define DMA64_DD_PARITY(dd)  parity32((dd)->addrlow ^ (dd)->addrhigh ^ (dd)->ctrl1 ^ (dd)->ctrl2)
-
 static inline void
 dma64_dd_upd(struct dma_info *di, struct dma64desc *ddring,
-	     dmaaddr_t pa, uint outidx, u32 *flags, u32 bufcount)
+	     dma_addr_t pa, uint outidx, u32 *flags, u32 bufcount)
 {
 	u32 ctrl2 = bufcount & D64_CTRL2_BC_MASK;
 
 	/* PCI bus with big(>1G) physical address, use address extension */
-#if defined(__mips__) && defined(IL_BIGENDIAN)
-	if ((di->dataoffsetlow == SI_SDRAM_SWAPPED)
-	    || !(PHYSADDRLO(pa) & PCI32ADDR_HIGH)) {
-#else
-	if ((di->dataoffsetlow == 0) || !(PHYSADDRLO(pa) & PCI32ADDR_HIGH)) {
-#endif				/* defined(__mips__) && defined(IL_BIGENDIAN) */
-
-		W_SM(&ddring[outidx].addrlow,
-		     BUS_SWAP32(PHYSADDRLO(pa) + di->dataoffsetlow));
-		W_SM(&ddring[outidx].addrhigh,
-		     BUS_SWAP32(PHYSADDRHI(pa) + di->dataoffsethigh));
-		W_SM(&ddring[outidx].ctrl1, BUS_SWAP32(*flags));
-		W_SM(&ddring[outidx].ctrl2, BUS_SWAP32(ctrl2));
+	if ((di->dataoffsetlow == 0) || !(pa & PCI32ADDR_HIGH)) {
+		ddring[outidx].addrlow = cpu_to_le32(pa + di->dataoffsetlow);
+		ddring[outidx].addrhigh = cpu_to_le32(di->dataoffsethigh);
+		ddring[outidx].ctrl1 = cpu_to_le32(*flags);
+		ddring[outidx].ctrl2 = cpu_to_le32(ctrl2);
 	} else {
 		/* address extension for 32-bit PCI */
 		u32 ae;
 
-		ae = (PHYSADDRLO(pa) & PCI32ADDR_HIGH) >> PCI32ADDR_HIGH_SHIFT;
-		PHYSADDRLO(pa) &= ~PCI32ADDR_HIGH;
+		ae = (pa & PCI32ADDR_HIGH) >> PCI32ADDR_HIGH_SHIFT;
+		pa &= ~PCI32ADDR_HIGH;
 
 		ctrl2 |= (ae << D64_CTRL2_AE_SHIFT) & D64_CTRL2_AE;
-		W_SM(&ddring[outidx].addrlow,
-		     BUS_SWAP32(PHYSADDRLO(pa) + di->dataoffsetlow));
-		W_SM(&ddring[outidx].addrhigh,
-		     BUS_SWAP32(0 + di->dataoffsethigh));
-		W_SM(&ddring[outidx].ctrl1, BUS_SWAP32(*flags));
-		W_SM(&ddring[outidx].ctrl2, BUS_SWAP32(ctrl2));
+		ddring[outidx].addrlow = cpu_to_le32(pa + di->dataoffsetlow);
+		ddring[outidx].addrhigh = cpu_to_le32(di->dataoffsethigh);
+		ddring[outidx].ctrl1 = cpu_to_le32(*flags);
+		ddring[outidx].ctrl2 = cpu_to_le32(ctrl2);
 	}
 	if (di->dma.dmactrlflags & DMA_CTRL_PEN) {
-		if (DMA64_DD_PARITY(&ddring[outidx])) {
-			W_SM(&ddring[outidx].ctrl2,
-			     BUS_SWAP32(ctrl2 | D64_CTRL2_PARITY));
-		}
-	}
-}
-
-static bool _dma_alloc(struct dma_info *di, uint direction)
-{
-	return dma64_alloc(di, direction);
-}
-
-void *dma_alloc_consistent(struct pci_dev *pdev, uint size, u16 align_bits,
-			       uint *alloced, unsigned long *pap)
-{
-	if (align_bits) {
-		u16 align = (1 << align_bits);
-		if (!IS_ALIGNED(PAGE_SIZE, align))
-			size += align;
-		*alloced = size;
+		if (dma64_dd_parity(&ddring[outidx]))
+			ddring[outidx].ctrl2 =
+			     cpu_to_le32(ctrl2 | D64_CTRL2_PARITY);
 	}
-	return pci_alloc_consistent(pdev, size, (dma_addr_t *) pap);
 }
 
 /* !! may be called with core in reset */
-static void _dma_detach(struct dma_info *di)
+void dma_detach(struct dma_pub *pub)
 {
+	struct dma_info *di = (struct dma_info *)pub;
 
 	DMA_TRACE(("%s: dma_detach\n", di->name));
 
@@ -701,117 +759,78 @@ static void _dma_detach(struct dma_info *di)
 	kfree(di->txp);
 	kfree(di->rxp);
 
-	/* free tx packet DMA handles */
-	kfree(di->txp_dmah);
-
-	/* free rx packet DMA handles */
-	kfree(di->rxp_dmah);
-
 	/* free our private info structure */
 	kfree(di);
 
 }
 
-static bool _dma_descriptor_align(struct dma_info *di)
-{
-	u32 addrl;
-
-	/* Check to see if the descriptors need to be aligned on 4K/8K or not */
-	if (di->d64txregs != NULL) {
-		W_REG(&di->d64txregs->addrlow, 0xff0);
-		addrl = R_REG(&di->d64txregs->addrlow);
-		if (addrl != 0)
-			return false;
-	} else if (di->d64rxregs != NULL) {
-		W_REG(&di->d64rxregs->addrlow, 0xff0);
-		addrl = R_REG(&di->d64rxregs->addrlow);
-		if (addrl != 0)
-			return false;
-	}
-	return true;
-}
-
-/* return true if this dma engine supports DmaExtendedAddrChanges, otherwise false */
-static bool _dma_isaddrext(struct dma_info *di)
-{
-	/* DMA64 supports full 32- or 64-bit operation. AE is always valid */
-
-	/* not all tx or rx channel are available */
-	if (di->d64txregs != NULL) {
-		if (!_dma64_addrext(di->d64txregs)) {
-			DMA_ERROR(("%s: _dma_isaddrext: DMA64 tx doesn't have "
-				   "AE set\n", di->name));
-		}
-		return true;
-	} else if (di->d64rxregs != NULL) {
-		if (!_dma64_addrext(di->d64rxregs)) {
-			DMA_ERROR(("%s: _dma_isaddrext: DMA64 rx doesn't have "
-				   "AE set\n", di->name));
-		}
-		return true;
-	}
-	return false;
-}
-
 /* initialize descriptor table base address */
-static void _dma_ddtable_init(struct dma_info *di, uint direction, dmaaddr_t pa)
+static void
+_dma_ddtable_init(struct dma_info *di, uint direction, dma_addr_t pa)
 {
 	if (!di->aligndesc_4k) {
 		if (direction == DMA_TX)
-			di->xmtptrbase = PHYSADDRLO(pa);
+			di->xmtptrbase = pa;
 		else
-			di->rcvptrbase = PHYSADDRLO(pa);
+			di->rcvptrbase = pa;
 	}
 
 	if ((di->ddoffsetlow == 0)
-	    || !(PHYSADDRLO(pa) & PCI32ADDR_HIGH)) {
+	    || !(pa & PCI32ADDR_HIGH)) {
 		if (direction == DMA_TX) {
-			W_REG(&di->d64txregs->addrlow,
-			      (PHYSADDRLO(pa) + di->ddoffsetlow));
-			W_REG(&di->d64txregs->addrhigh,
-			      (PHYSADDRHI(pa) + di->ddoffsethigh));
+			W_REG(&di->d64txregs->addrlow, pa + di->ddoffsetlow);
+			W_REG(&di->d64txregs->addrhigh, di->ddoffsethigh);
 		} else {
-			W_REG(&di->d64rxregs->addrlow,
-			      (PHYSADDRLO(pa) + di->ddoffsetlow));
-			W_REG(&di->d64rxregs->addrhigh,
-				(PHYSADDRHI(pa) + di->ddoffsethigh));
+			W_REG(&di->d64rxregs->addrlow, pa + di->ddoffsetlow);
+			W_REG(&di->d64rxregs->addrhigh, di->ddoffsethigh);
 		}
 	} else {
 		/* DMA64 32bits address extension */
 		u32 ae;
 
 		/* shift the high bit(s) from pa to ae */
-		ae = (PHYSADDRLO(pa) & PCI32ADDR_HIGH) >>
-		    PCI32ADDR_HIGH_SHIFT;
-		PHYSADDRLO(pa) &= ~PCI32ADDR_HIGH;
+		ae = (pa & PCI32ADDR_HIGH) >> PCI32ADDR_HIGH_SHIFT;
+		pa &= ~PCI32ADDR_HIGH;
 
 		if (direction == DMA_TX) {
-			W_REG(&di->d64txregs->addrlow,
-			      (PHYSADDRLO(pa) + di->ddoffsetlow));
-			W_REG(&di->d64txregs->addrhigh,
-			      di->ddoffsethigh);
+			W_REG(&di->d64txregs->addrlow, pa + di->ddoffsetlow);
+			W_REG(&di->d64txregs->addrhigh, di->ddoffsethigh);
 			SET_REG(&di->d64txregs->control,
 				D64_XC_AE, (ae << D64_XC_AE_SHIFT));
 		} else {
-			W_REG(&di->d64rxregs->addrlow,
-			      (PHYSADDRLO(pa) + di->ddoffsetlow));
-			W_REG(&di->d64rxregs->addrhigh,
-			      di->ddoffsethigh);
+			W_REG(&di->d64rxregs->addrlow, pa + di->ddoffsetlow);
+			W_REG(&di->d64rxregs->addrhigh, di->ddoffsethigh);
 			SET_REG(&di->d64rxregs->control,
 				D64_RC_AE, (ae << D64_RC_AE_SHIFT));
 		}
 	}
 }
 
-static void _dma_fifoloopbackenable(struct dma_info *di)
+static void _dma_rxenable(struct dma_info *di)
 {
-	DMA_TRACE(("%s: dma_fifoloopbackenable\n", di->name));
+	uint dmactrlflags = di->dma.dmactrlflags;
+	u32 control;
+
+	DMA_TRACE(("%s: dma_rxenable\n", di->name));
+
+	control =
+	    (R_REG(&di->d64rxregs->control) & D64_RC_AE) |
+	    D64_RC_RE;
+
+	if ((dmactrlflags & DMA_CTRL_PEN) == 0)
+		control |= D64_RC_PD;
 
-	OR_REG(&di->d64txregs->control, D64_XC_LE);
+	if (dmactrlflags & DMA_CTRL_ROC)
+		control |= D64_RC_OC;
+
+	W_REG(&di->d64rxregs->control,
+		((di->rxoffset << D64_RC_RO_SHIFT) | control));
 }
 
-static void _dma_rxinit(struct dma_info *di)
+void dma_rxinit(struct dma_pub *pub)
 {
+	struct dma_info *di = (struct dma_info *)pub;
+
 	DMA_TRACE(("%s: dma_rxinit\n", di->name));
 
 	if (di->nrxd == 0)
@@ -820,8 +839,7 @@ static void _dma_rxinit(struct dma_info *di)
 	di->rxin = di->rxout = 0;
 
 	/* clear rx descriptor ring */
-	memset((void *)di->rxd64, '\0',
-		(di->nrxd * sizeof(struct dma64desc)));
+	memset(di->rxd64, '\0', di->nrxd * sizeof(struct dma64desc));
 
 	/* DMA engine with out alignment requirement requires table to be inited
 	 * before enabling the engine
@@ -835,46 +853,64 @@ static void _dma_rxinit(struct dma_info *di)
 		_dma_ddtable_init(di, DMA_RX, di->rxdpa);
 }
 
-static void _dma_rxenable(struct dma_info *di)
+static struct sk_buff *dma64_getnextrxp(struct dma_info *di, bool forceall)
 {
-	uint dmactrlflags = di->dma.dmactrlflags;
-	u32 control;
+	uint i, curr;
+	struct sk_buff *rxp;
+	dma_addr_t pa;
 
-	DMA_TRACE(("%s: dma_rxenable\n", di->name));
+	i = di->rxin;
 
-	control =
-	    (R_REG(&di->d64rxregs->control) & D64_RC_AE) |
-	    D64_RC_RE;
+	/* return if no packets posted */
+	if (i == di->rxout)
+		return NULL;
 
-	if ((dmactrlflags & DMA_CTRL_PEN) == 0)
-		control |= D64_RC_PD;
+	curr =
+	    B2I(((R_REG(&di->d64rxregs->status0) & D64_RS0_CD_MASK) -
+		 di->rcvptrbase) & D64_RS0_CD_MASK, struct dma64desc);
 
-	if (dmactrlflags & DMA_CTRL_ROC)
-		control |= D64_RC_OC;
+	/* ignore curr if forceall */
+	if (!forceall && (i == curr))
+		return NULL;
 
-	W_REG(&di->d64rxregs->control,
-		((di->rxoffset << D64_RC_RO_SHIFT) | control));
+	/* get the packet pointer that corresponds to the rx descriptor */
+	rxp = di->rxp[i];
+	di->rxp[i] = NULL;
+
+	pa = le32_to_cpu(di->rxd64[i].addrlow) - di->dataoffsetlow;
+
+	/* clear this packet from the descriptor ring */
+	pci_unmap_single(di->pbus, pa, di->rxbufsize, PCI_DMA_FROMDEVICE);
+
+	di->rxd64[i].addrlow = cpu_to_le32(0xdeadbeef);
+	di->rxd64[i].addrhigh = cpu_to_le32(0xdeadbeef);
+
+	di->rxin = nextrxd(di, i);
+
+	return rxp;
 }
 
-static void
-_dma_rx_param_get(struct dma_info *di, u16 *rxoffset, u16 *rxbufsize)
+static struct sk_buff *_dma_getnextrxp(struct dma_info *di, bool forceall)
 {
-	/* the normal values fit into 16 bits */
-	*rxoffset = (u16) di->rxoffset;
-	*rxbufsize = (u16) di->rxbufsize;
+	if (di->nrxd == 0)
+		return NULL;
+
+	return dma64_getnextrxp(di, forceall);
 }
 
-/* !! rx entry routine
+/*
+ * !! rx entry routine
  * returns a pointer to the next frame received, or NULL if there are no more
- *   if DMA_CTRL_RXMULTI is defined, DMA scattering(multiple buffers) is supported
- *      with pkts chain
+ *   if DMA_CTRL_RXMULTI is defined, DMA scattering(multiple buffers) is
+ *   supported with pkts chain
  *   otherwise, it's treated as giant pkt and will be tossed.
- *   The DMA scattering starts with normal DMA header, followed by first buffer data.
- *   After it reaches the max size of buffer, the data continues in next DMA descriptor
- *   buffer WITHOUT DMA header
+ *   The DMA scattering starts with normal DMA header, followed by first
+ *   buffer data. After it reaches the max size of buffer, the data continues
+ *   in next DMA descriptor buffer WITHOUT DMA header
  */
-static void *_dma_rx(struct dma_info *di)
+struct sk_buff *dma_rx(struct dma_pub *pub)
 {
+	struct dma_info *di = (struct dma_info *)pub;
 	struct sk_buff *p, *head, *tail;
 	uint len;
 	uint pkt_len;
@@ -885,7 +921,7 @@ static void *_dma_rx(struct dma_info *di)
 	if (head == NULL)
 		return NULL;
 
-	len = le16_to_cpu(*(u16 *) (head->data));
+	len = le16_to_cpu(*(__le16 *) (head->data));
 	DMA_TRACE(("%s: dma_rx len %d\n", di->name, len));
 	dma_spin_for_len(len, head);
 
@@ -899,7 +935,7 @@ static void *_dma_rx(struct dma_info *di)
 		tail = head;
 		while ((resid > 0) && (p = _dma_getnextrxp(di, false))) {
 			tail->next = p;
-			pkt_len = min(resid, (int)di->rxbufsize);
+			pkt_len = min_t(uint, resid, di->rxbufsize);
 			__skb_trim(p, pkt_len);
 
 			tail = p;
@@ -914,7 +950,7 @@ static void *_dma_rx(struct dma_info *di)
 				  D64_RS0_CD_MASK) -
 				 di->rcvptrbase) & D64_RS0_CD_MASK,
 				struct dma64desc);
-			DMA_ERROR(("_dma_rx, rxin %d rxout %d, hw_curr %d\n",
+			DMA_ERROR(("dma_rx, rxin %d rxout %d, hw_curr %d\n",
 				   di->rxin, di->rxout, cur));
 		}
 #endif				/* BCMDBG */
@@ -931,19 +967,32 @@ static void *_dma_rx(struct dma_info *di)
 	return head;
 }
 
-/* post receive buffers
- *  return false is refill failed completely and ring is empty
- *  this will stall the rx dma and user might want to call rxfill again asap
- *  This unlikely happens on memory-rich NIC, but often on memory-constrained dongle
+static bool dma64_rxidle(struct dma_info *di)
+{
+	DMA_TRACE(("%s: dma_rxidle\n", di->name));
+
+	if (di->nrxd == 0)
+		return true;
+
+	return ((R_REG(&di->d64rxregs->status0) & D64_RS0_CD_MASK) ==
+		(R_REG(&di->d64rxregs->ptr) & D64_RS0_CD_MASK));
+}
+
+/*
+ * post receive buffers
+ *  return false is refill failed completely and ring is empty this will stall
+ *  the rx dma and user might want to call rxfill again asap. This unlikely
+ *  happens on memory-rich NIC, but often on memory-constrained dongle
  */
-static bool _dma_rxfill(struct dma_info *di)
+bool dma_rxfill(struct dma_pub *pub)
 {
+	struct dma_info *di = (struct dma_info *)pub;
 	struct sk_buff *p;
 	u16 rxin, rxout;
 	u32 flags = 0;
 	uint n;
 	uint i;
-	dmaaddr_t pa;
+	dma_addr_t pa;
 	uint extra_offset = 0;
 	bool ring_empty;
 
@@ -958,7 +1007,7 @@ static bool _dma_rxfill(struct dma_info *di)
 	rxin = di->rxin;
 	rxout = di->rxout;
 
-	n = di->nrxpost - NRXDACTIVE(rxin, rxout);
+	n = di->nrxpost - nrxdactive(di, rxin, rxout);
 
 	DMA_TRACE(("%s: dma_rxfill: post %d\n", di->name, n));
 
@@ -966,10 +1015,10 @@ static bool _dma_rxfill(struct dma_info *di)
 		extra_offset = di->rxextrahdrroom;
 
 	for (i = 0; i < n; i++) {
-		/* the di->rxbufsize doesn't include the extra headroom, we need to add it to the
-		   size to be allocated
+		/*
+		 * the di->rxbufsize doesn't include the extra headroom,
+		 * we need to add it to the size to be allocated
 		 */
-
 		p = brcmu_pkt_buf_get_skb(di->rxbufsize + extra_offset);
 
 		if (p == NULL) {
@@ -992,10 +1041,6 @@ static bool _dma_rxfill(struct dma_info *di)
 		 */
 		*(u32 *) (p->data) = 0;
 
-		if (DMASGLIST_ENAB)
-			memset(&di->rxp_dmah[rxout], 0,
-				sizeof(struct dma_seg_map));
-
 		pa = pci_map_single(di->pbus, p->data,
 			di->rxbufsize, PCI_DMA_FROMDEVICE);
 
@@ -1009,7 +1054,7 @@ static bool _dma_rxfill(struct dma_info *di)
 
 		dma64_dd_upd(di, di->rxd64, pa, rxout, &flags,
 			     di->rxbufsize);
-		rxout = NEXTRXD(rxout);
+		rxout = nextrxd(di, rxout);
 	}
 
 	di->rxout = rxout;
@@ -1021,49 +1066,10 @@ static bool _dma_rxfill(struct dma_info *di)
 	return ring_empty;
 }
 
-/* like getnexttxp but no reclaim */
-static void *_dma_peeknexttxp(struct dma_info *di)
+void dma_rxreclaim(struct dma_pub *pub)
 {
-	uint end, i;
-
-	if (di->ntxd == 0)
-		return NULL;
-
-	end =
-	    B2I(((R_REG(&di->d64txregs->status0) &
-		  D64_XS0_CD_MASK) - di->xmtptrbase) & D64_XS0_CD_MASK,
-		  struct dma64desc);
-
-	for (i = di->txin; i != end; i = NEXTTXD(i))
-		if (di->txp[i])
-			return di->txp[i];
-
-	return NULL;
-}
-
-/* like getnextrxp but not take off the ring */
-static void *_dma_peeknextrxp(struct dma_info *di)
-{
-	uint end, i;
-
-	if (di->nrxd == 0)
-		return NULL;
-
-	end =
-	    B2I(((R_REG(&di->d64rxregs->status0) &
-		  D64_RS0_CD_MASK) - di->rcvptrbase) & D64_RS0_CD_MASK,
-		  struct dma64desc);
-
-	for (i = di->rxin; i != end; i = NEXTRXD(i))
-		if (di->rxp[i])
-			return di->rxp[i];
-
-	return NULL;
-}
-
-static void _dma_rxreclaim(struct dma_info *di)
-{
-	void *p;
+	struct dma_info *di = (struct dma_info *)pub;
+	struct sk_buff *p;
 
 	DMA_TRACE(("%s: dma_rxreclaim\n", di->name));
 
@@ -1071,155 +1077,29 @@ static void _dma_rxreclaim(struct dma_info *di)
 		brcmu_pkt_buf_free_skb(p);
 }
 
-static void *_dma_getnextrxp(struct dma_info *di, bool forceall)
-{
-	if (di->nrxd == 0)
-		return NULL;
-
-	return dma64_getnextrxp(di, forceall);
-}
-
-static void _dma_txblock(struct dma_info *di)
+void dma_counterreset(struct dma_pub *pub)
 {
-	di->dma.txavail = 0;
-}
-
-static void _dma_txunblock(struct dma_info *di)
-{
-	di->dma.txavail = di->ntxd - NTXDACTIVE(di->txin, di->txout) - 1;
-}
-
-static uint _dma_txactive(struct dma_info *di)
-{
-	return NTXDACTIVE(di->txin, di->txout);
-}
-
-static uint _dma_txpending(struct dma_info *di)
-{
-	uint curr;
-
-	curr =
-	    B2I(((R_REG(&di->d64txregs->status0) &
-		  D64_XS0_CD_MASK) - di->xmtptrbase) & D64_XS0_CD_MASK,
-		  struct dma64desc);
-
-	return NTXDACTIVE(curr, di->txout);
-}
-
-static uint _dma_txcommitted(struct dma_info *di)
-{
-	uint ptr;
-	uint txin = di->txin;
-
-	if (txin == di->txout)
-		return 0;
-
-	ptr = B2I(R_REG(&di->d64txregs->ptr), struct dma64desc);
-
-	return NTXDACTIVE(di->txin, ptr);
-}
-
-static uint _dma_rxactive(struct dma_info *di)
-{
-	return NRXDACTIVE(di->rxin, di->rxout);
-}
-
-static void _dma_counterreset(struct dma_info *di)
-{
-	/* reset all software counter */
-	di->dma.rxgiants = 0;
-	di->dma.rxnobuf = 0;
-	di->dma.txnobuf = 0;
-}
-
-static uint _dma_ctrlflags(struct dma_info *di, uint mask, uint flags)
-{
-	uint dmactrlflags = di->dma.dmactrlflags;
-
-	if (di == NULL) {
-		DMA_ERROR(("%s: _dma_ctrlflags: NULL dma handle\n", di->name));
-		return 0;
-	}
-
-	dmactrlflags &= ~mask;
-	dmactrlflags |= flags;
-
-	/* If trying to enable parity, check if parity is actually supported */
-	if (dmactrlflags & DMA_CTRL_PEN) {
-		u32 control;
-
-		control = R_REG(&di->d64txregs->control);
-		W_REG(&di->d64txregs->control,
-		      control | D64_XC_PD);
-		if (R_REG(&di->d64txregs->control) & D64_XC_PD) {
-			/* We *can* disable it so it is supported,
-			 * restore control register
-			 */
-			W_REG(&di->d64txregs->control,
-			control);
-		} else {
-			/* Not supported, don't allow it to be enabled */
-			dmactrlflags &= ~DMA_CTRL_PEN;
-		}
-	}
-
-	di->dma.dmactrlflags = dmactrlflags;
-
-	return dmactrlflags;
+	/* reset all software counters */
+	pub->rxgiants = 0;
+	pub->rxnobuf = 0;
+	pub->txnobuf = 0;
 }
 
 /* get the address of the var in order to change later */
-static unsigned long _dma_getvar(struct dma_info *di, const char *name)
+unsigned long dma_getvar(struct dma_pub *pub, const char *name)
 {
+	struct dma_info *di = (struct dma_info *)pub;
+
 	if (!strcmp(name, "&txavail"))
 		return (unsigned long)&(di->dma.txavail);
 	return 0;
 }
 
-static
-u8 dma_align_sizetobits(uint size)
-{
-	u8 bitpos = 0;
-	while (size >>= 1) {
-		bitpos++;
-	}
-	return bitpos;
-}
-
-/* This function ensures that the DMA descriptor ring will not get allocated
- * across Page boundary. If the allocation is done across the page boundary
- * at the first time, then it is freed and the allocation is done at
- * descriptor ring size aligned location. This will ensure that the ring will
- * not cross page boundary
- */
-static void *dma_ringalloc(struct dma_info *di, u32 boundary, uint size,
-			   u16 *alignbits, uint *alloced,
-			   dmaaddr_t *descpa)
-{
-	void *va;
-	u32 desc_strtaddr;
-	u32 alignbytes = 1 << *alignbits;
-
-	va = dma_alloc_consistent(di->pbus, size, *alignbits, alloced, descpa);
-
-	if (NULL == va)
-		return NULL;
-
-	desc_strtaddr = (u32) roundup((unsigned long)va, alignbytes);
-	if (((desc_strtaddr + size - 1) & boundary) != (desc_strtaddr
-							& boundary)) {
-		*alignbits = dma_align_sizetobits(size);
-		pci_free_consistent(di->pbus, size, va, *descpa);
-		va = dma_alloc_consistent(di->pbus, size, *alignbits,
-			alloced, descpa);
-	}
-	return va;
-}
-
 /* 64-bit DMA functions */
 
-static void dma64_txinit(struct dma_info *di)
+void dma_txinit(struct dma_pub *pub)
 {
+	struct dma_info *di = (struct dma_info *)pub;
 	u32 control = D64_XC_XE;
 
 	DMA_TRACE(("%s: dma_txinit\n", di->name));
@@ -1231,7 +1111,7 @@ static void dma64_txinit(struct dma_info *di)
 	di->dma.txavail = di->ntxd - 1;
 
 	/* clear tx descriptor ring */
-	memset((void *)di->txd64, '\0', (di->ntxd * sizeof(struct dma64desc)));
+	memset(di->txd64, '\0', (di->ntxd * sizeof(struct dma64desc)));
 
 	/* DMA engine with out alignment requirement requires table to be inited
 	 * before enabling the engine
@@ -1250,17 +1130,10 @@ static void dma64_txinit(struct dma_info *di)
 		_dma_ddtable_init(di, DMA_TX, di->txdpa);
 }
 
-static bool dma64_txenabled(struct dma_info *di)
+void dma_txsuspend(struct dma_pub *pub)
 {
-	u32 xc;
+	struct dma_info *di = (struct dma_info *)pub;
 
-	/* If the chip is dead, it is not enabled :-) */
-	xc = R_REG(&di->d64txregs->control);
-	return (xc != 0xffffffff) && (xc & D64_XC_XE);
-}
-
-static void dma64_txsuspend(struct dma_info *di)
-{
 	DMA_TRACE(("%s: dma_txsuspend\n", di->name));
 
 	if (di->ntxd == 0)
@@ -1269,8 +1142,10 @@ static void dma64_txsuspend(struct dma_info *di)
 	OR_REG(&di->d64txregs->control, D64_XC_SE);
 }
 
-static void dma64_txresume(struct dma_info *di)
+void dma_txresume(struct dma_pub *pub)
 {
+	struct dma_info *di = (struct dma_info *)pub;
+
 	DMA_TRACE(("%s: dma_txresume\n", di->name));
 
 	if (di->ntxd == 0)
@@ -1279,16 +1154,19 @@ static void dma64_txresume(struct dma_info *di)
 	AND_REG(&di->d64txregs->control, ~D64_XC_SE);
 }
 
-static bool dma64_txsuspended(struct dma_info *di)
+bool dma_txsuspended(struct dma_pub *pub)
 {
+	struct dma_info *di = (struct dma_info *)pub;
+
 	return (di->ntxd == 0) ||
 	    ((R_REG(&di->d64txregs->control) & D64_XC_SE) ==
 	     D64_XC_SE);
 }
 
-static void dma64_txreclaim(struct dma_info *di, enum txd_range range)
+void dma_txreclaim(struct dma_pub *pub, enum txd_range range)
 {
-	void *p;
+	struct dma_info *di = (struct dma_info *)pub;
+	struct sk_buff *p;
 
 	DMA_TRACE(("%s: dma_txreclaim %s\n", di->name,
 		   (range == DMA_RANGE_ALL) ? "all" :
@@ -1299,77 +1177,16 @@ static void dma64_txreclaim(struct dma_info *di, enum txd_range range)
 	if (di->txin == di->txout)
 		return;
 
-	while ((p = dma64_getnexttxp(di, range))) {
+	while ((p = dma_getnexttxp(pub, range))) {
 		/* For unframed data, we don't have any packets to free */
 		if (!(di->dma.dmactrlflags & DMA_CTRL_UNFRAMED))
 			brcmu_pkt_buf_free_skb(p);
 	}
 }
 
-static bool dma64_txstopped(struct dma_info *di)
-{
-	return ((R_REG(&di->d64txregs->status0) & D64_XS0_XS_MASK) ==
-		D64_XS0_XS_STOPPED);
-}
-
-static bool dma64_rxstopped(struct dma_info *di)
-{
-	return ((R_REG(&di->d64rxregs->status0) & D64_RS0_RS_MASK) ==
-		D64_RS0_RS_STOPPED);
-}
-
-static bool dma64_alloc(struct dma_info *di, uint direction)
-{
-	u16 size;
-	uint ddlen;
-	void *va;
-	uint alloced = 0;
-	u16 align;
-	u16 align_bits;
-
-	ddlen = sizeof(struct dma64desc);
-
-	size = (direction == DMA_TX) ? (di->ntxd * ddlen) : (di->nrxd * ddlen);
-	align_bits = di->dmadesc_align;
-	align = (1 << align_bits);
-
-	if (direction == DMA_TX) {
-		va = dma_ringalloc(di, D64RINGALIGN, size, &align_bits,
-			&alloced, &di->txdpaorig);
-		if (va == NULL) {
-			DMA_ERROR(("%s: dma64_alloc: DMA_ALLOC_CONSISTENT(ntxd) failed\n", di->name));
-			return false;
-		}
-		align = (1 << align_bits);
-		di->txd64 = (struct dma64desc *)
-					roundup((unsigned long)va, align);
-		di->txdalign = (uint) ((s8 *)di->txd64 - (s8 *) va);
-		PHYSADDRLOSET(di->txdpa,
-			      PHYSADDRLO(di->txdpaorig) + di->txdalign);
-		PHYSADDRHISET(di->txdpa, PHYSADDRHI(di->txdpaorig));
-		di->txdalloc = alloced;
-	} else {
-		va = dma_ringalloc(di, D64RINGALIGN, size, &align_bits,
-			&alloced, &di->rxdpaorig);
-		if (va == NULL) {
-			DMA_ERROR(("%s: dma64_alloc: DMA_ALLOC_CONSISTENT(nrxd) failed\n", di->name));
-			return false;
-		}
-		align = (1 << align_bits);
-		di->rxd64 = (struct dma64desc *)
-					roundup((unsigned long)va, align);
-		di->rxdalign = (uint) ((s8 *)di->rxd64 - (s8 *) va);
-		PHYSADDRLOSET(di->rxdpa,
-			      PHYSADDRLO(di->rxdpaorig) + di->rxdalign);
-		PHYSADDRHISET(di->rxdpa, PHYSADDRHI(di->rxdpaorig));
-		di->rxdalloc = alloced;
-	}
-
-	return true;
-}
-
-static bool dma64_txreset(struct dma_info *di)
+bool dma_txreset(struct dma_pub *pub)
 {
+	struct dma_info *di = (struct dma_info *)pub;
 	u32 status;
 
 	if (di->ntxd == 0)
@@ -1393,19 +1210,9 @@ static bool dma64_txreset(struct dma_info *di)
 	return status == D64_XS0_XS_DISABLED;
 }
 
-static bool dma64_rxidle(struct dma_info *di)
-{
-	DMA_TRACE(("%s: dma_rxidle\n", di->name));
-
-	if (di->nrxd == 0)
-		return true;
-
-	return ((R_REG(&di->d64rxregs->status0) & D64_RS0_CD_MASK) ==
-		(R_REG(&di->d64rxregs->ptr) & D64_RS0_CD_MASK));
-}
-
-static bool dma64_rxreset(struct dma_info *di)
+bool dma_rxreset(struct dma_pub *pub)
 {
+	struct dma_info *di = (struct dma_info *)pub;
 	u32 status;
 
 	if (di->nrxd == 0)
@@ -1419,132 +1226,21 @@ static bool dma64_rxreset(struct dma_info *di)
 	return status == D64_RS0_RS_DISABLED;
 }
 
-static bool dma64_rxenabled(struct dma_info *di)
-{
-	u32 rc;
-
-	rc = R_REG(&di->d64rxregs->control);
-	return (rc != 0xffffffff) && (rc & D64_RC_RE);
-}
-
-static bool dma64_txsuspendedidle(struct dma_info *di)
-{
-
-	if (di->ntxd == 0)
-		return true;
-
-	if (!(R_REG(&di->d64txregs->control) & D64_XC_SE))
-		return 0;
-
-	if ((R_REG(&di->d64txregs->status0) & D64_XS0_XS_MASK) ==
-	    D64_XS0_XS_IDLE)
-		return 1;
-
-	return 0;
-}
-
-/* Useful when sending unframed data.  This allows us to get a progress report from the DMA.
- * We return a pointer to the beginning of the DATA buffer of the current descriptor.
- * If DMA is idle, we return NULL.
- */
-static void *dma64_getpos(struct dma_info *di, bool direction)
-{
-	void *va;
-	bool idle;
-	u32 cd_offset;
-
-	if (direction == DMA_TX) {
-		cd_offset =
-		    R_REG(&di->d64txregs->status0) & D64_XS0_CD_MASK;
-		idle = !NTXDACTIVE(di->txin, di->txout);
-		va = di->txp[B2I(cd_offset, struct dma64desc)];
-	} else {
-		cd_offset =
-		    R_REG(&di->d64rxregs->status0) & D64_XS0_CD_MASK;
-		idle = !NRXDACTIVE(di->rxin, di->rxout);
-		va = di->rxp[B2I(cd_offset, struct dma64desc)];
-	}
-
-	/* If DMA is IDLE, return NULL */
-	if (idle) {
-		DMA_TRACE(("%s: DMA idle, return NULL\n", __func__));
-		va = NULL;
-	}
-
-	return va;
-}
-
-/* TX of unframed data
- *
- * Adds a DMA ring descriptor for the data pointed to by "buf".
- * This is for DMA of a buffer of data and is unlike other dma TX functions
- * that take a pointer to a "packet"
- * Each call to this is results in a single descriptor being added for "len" bytes of
- * data starting at "buf", it doesn't handle chained buffers.
- */
-static int
-dma64_txunframed(struct dma_info *di, void *buf, uint len, bool commit)
-{
-	u16 txout;
-	u32 flags = 0;
-	dmaaddr_t pa;		/* phys addr */
-
-	txout = di->txout;
-
-	/* return nonzero if out of tx descriptors */
-	if (NEXTTXD(txout) == di->txin)
-		goto outoftxd;
-
-	if (len == 0)
-		return 0;
-
-	pa = pci_map_single(di->pbus, buf, len, PCI_DMA_TODEVICE);
-
-	flags = (D64_CTRL1_SOF | D64_CTRL1_IOC | D64_CTRL1_EOF);
-
-	if (txout == (di->ntxd - 1))
-		flags |= D64_CTRL1_EOT;
-
-	dma64_dd_upd(di, di->txd64, pa, txout, &flags, len);
-
-	/* save the buffer pointer - used by dma_getpos */
-	di->txp[txout] = buf;
-
-	txout = NEXTTXD(txout);
-	/* bump the tx descriptor index */
-	di->txout = txout;
-
-	/* kick the chip */
-	if (commit) {
-		W_REG(&di->d64txregs->ptr,
-		      di->xmtptrbase + I2B(txout, struct dma64desc));
-	}
-
-	/* tx flow control */
-	di->dma.txavail = di->ntxd - NTXDACTIVE(di->txin, di->txout) - 1;
-
-	return 0;
-
- outoftxd:
-	DMA_ERROR(("%s: %s: out of txds !!!\n", di->name, __func__));
-	di->dma.txavail = 0;
-	di->dma.txnobuf++;
-	return -1;
-}
-
-/* !! tx entry routine
+/*
+ * !! tx entry routine
  * WARNING: call must check the return value for error.
- *   the error(toss frames) could be fatal and cause many subsequent hard to debug problems
+ *   the error(toss frames) could be fatal and cause many subsequent hard
+ *   to debug problems
  */
-static int dma64_txfast(struct dma_info *di, struct sk_buff *p0,
-				    bool commit)
+int dma_txfast(struct dma_pub *pub, struct sk_buff *p0, bool commit)
 {
+	struct dma_info *di = (struct dma_info *)pub;
 	struct sk_buff *p, *next;
 	unsigned char *data;
 	uint len;
 	u16 txout;
 	u32 flags = 0;
-	dmaaddr_t pa;
+	dma_addr_t pa;
 
 	DMA_TRACE(("%s: dma_txfast\n", di->name));
 
@@ -1555,77 +1251,46 @@ static int dma64_txfast(struct dma_info *di, struct sk_buff *p0,
 	 * allocating and initializing transmit descriptor entries.
 	 */
 	for (p = p0; p; p = next) {
-		uint nsegs, j;
-		struct dma_seg_map *map;
-
 		data = p->data;
 		len = p->len;
 		next = p->next;
 
 		/* return nonzero if out of tx descriptors */
-		if (NEXTTXD(txout) == di->txin)
+		if (nexttxd(di, txout) == di->txin)
 			goto outoftxd;
 
 		if (len == 0)
 			continue;
 
 		/* get physical address of buffer start */
-		if (DMASGLIST_ENAB)
-			memset(&di->txp_dmah[txout], 0,
-				sizeof(struct dma_seg_map));
-
 		pa = pci_map_single(di->pbus, data, len, PCI_DMA_TODEVICE);
 
-		if (DMASGLIST_ENAB) {
-			map = &di->txp_dmah[txout];
-
-			/* See if all the segments can be accounted for */
-			if (map->nsegs >
-			    (uint) (di->ntxd - NTXDACTIVE(di->txin, di->txout) -
-				    1))
-				goto outoftxd;
-
-			nsegs = map->nsegs;
-		} else
-			nsegs = 1;
-
-		for (j = 1; j <= nsegs; j++) {
-			flags = 0;
-			if (p == p0 && j == 1)
-				flags |= D64_CTRL1_SOF;
+		flags = 0;
+		if (p == p0)
+			flags |= D64_CTRL1_SOF;
 
-			/* With a DMA segment list, Descriptor table is filled
-			 * using the segment list instead of looping over
-			 * buffers in multi-chain DMA. Therefore, EOF for SGLIST is when
-			 * end of segment list is reached.
-			 */
-			if ((!DMASGLIST_ENAB && next == NULL) ||
-			    (DMASGLIST_ENAB && j == nsegs))
-				flags |= (D64_CTRL1_IOC | D64_CTRL1_EOF);
-			if (txout == (di->ntxd - 1))
-				flags |= D64_CTRL1_EOT;
-
-			if (DMASGLIST_ENAB) {
-				len = map->segs[j - 1].length;
-				pa = map->segs[j - 1].addr;
-			}
-			dma64_dd_upd(di, di->txd64, pa, txout, &flags, len);
+		/* With a DMA segment list, Descriptor table is filled
+		 * using the segment list instead of looping over
+		 * buffers in multi-chain DMA. Therefore, EOF for SGLIST
+		 * is when end of segment list is reached.
+		 */
+		if (next == NULL)
+			flags |= (D64_CTRL1_IOC | D64_CTRL1_EOF);
+		if (txout == (di->ntxd - 1))
+			flags |= D64_CTRL1_EOT;
 
-			txout = NEXTTXD(txout);
-		}
+		dma64_dd_upd(di, di->txd64, pa, txout, &flags, len);
 
-		/* See above. No need to loop over individual buffers */
-		if (DMASGLIST_ENAB)
-			break;
+		txout = nexttxd(di, txout);
 	}
 
 	/* if last txd eof not set, fix it */
 	if (!(flags & D64_CTRL1_EOF))
-		W_SM(&di->txd64[PREVTXD(txout)].ctrl1,
-		     BUS_SWAP32(flags | D64_CTRL1_IOC | D64_CTRL1_EOF));
+		di->txd64[prevtxd(di, txout)].ctrl1 =
+		     cpu_to_le32(flags | D64_CTRL1_IOC | D64_CTRL1_EOF);
 
 	/* save the packet */
-	di->txp[PREVTXD(txout)] = p0;
+	di->txp[prevtxd(di, txout)] = p0;
 
 	/* bump the tx descriptor index */
 	di->txout = txout;
@@ -1636,7 +1301,7 @@ static int dma64_txfast(struct dma_info *di, struct sk_buff *p0,
 		      di->xmtptrbase + I2B(txout, struct dma64desc));
 
 	/* tx flow control */
-	di->dma.txavail = di->ntxd - NTXDACTIVE(di->txin, di->txout) - 1;
+	di->dma.txavail = di->ntxd - ntxdactive(di, di->txin, di->txout) - 1;
 
 	return 0;
 
@@ -1658,11 +1323,12 @@ static int dma64_txfast(struct dma_info *di, struct sk_buff *p0,
  * If range is DMA_RANGE_ALL, reclaim all txd(s) posted to the ring and
  * return associated packet regardless of the value of hardware pointers.
  */
-static void *dma64_getnexttxp(struct dma_info *di, enum txd_range range)
+struct sk_buff *dma_getnexttxp(struct dma_pub *pub, enum txd_range range)
 {
+	struct dma_info *di = (struct dma_info *)pub;
 	u16 start, end, i;
 	u16 active_desc;
-	void *txp;
+	struct sk_buff *txp;
 
 	DMA_TRACE(("%s: dma_getnexttxp %s\n", di->name,
 		   (range == DMA_RANGE_ALL) ? "all" :
@@ -1679,7 +1345,7 @@ static void *dma64_getnexttxp(struct dma_info *di, enum txd_range range)
 	if (range == DMA_RANGE_ALL)
 		end = di->txout;
 	else {
-		dma64regs_t *dregs = di->d64txregs;
+		struct dma64regs __iomem *dregs = di->d64txregs;
 
 		end = (u16) (B2I(((R_REG(&dregs->status0) &
 				 D64_XS0_CD_MASK) -
@@ -1694,45 +1360,28 @@ static void *dma64_getnexttxp(struct dma_info *di, enum txd_range range)
 			    (active_desc - di->xmtptrbase) & D64_XS0_CD_MASK;
 			active_desc = B2I(active_desc, struct dma64desc);
 			if (end != active_desc)
-				end = PREVTXD(active_desc);
+				end = prevtxd(di, active_desc);
 		}
 	}
 
 	if ((start == 0) && (end > di->txout))
 		goto bogus;
 
-	for (i = start; i != end && !txp; i = NEXTTXD(i)) {
-		dmaaddr_t pa;
-		struct dma_seg_map *map = NULL;
-		uint size, j, nsegs;
-
-		PHYSADDRLOSET(pa,
-			      (BUS_SWAP32(R_SM(&di->txd64[i].addrlow)) -
-			       di->dataoffsetlow));
-		PHYSADDRHISET(pa,
-			      (BUS_SWAP32(R_SM(&di->txd64[i].addrhigh)) -
-			       di->dataoffsethigh));
-
-		if (DMASGLIST_ENAB) {
-			map = &di->txp_dmah[i];
-			size = map->origsize;
-			nsegs = map->nsegs;
-		} else {
-			size =
-			    (BUS_SWAP32(R_SM(&di->txd64[i].ctrl2)) &
-			     D64_CTRL2_BC_MASK);
-			nsegs = 1;
-		}
+	for (i = start; i != end && !txp; i = nexttxd(di, i)) {
+		dma_addr_t pa;
+		uint size;
 
-		for (j = nsegs; j > 0; j--) {
-			W_SM(&di->txd64[i].addrlow, 0xdeadbeef);
-			W_SM(&di->txd64[i].addrhigh, 0xdeadbeef);
+		pa = le32_to_cpu(di->txd64[i].addrlow) - di->dataoffsetlow;
 
-			txp = di->txp[i];
-			di->txp[i] = NULL;
-			if (j > 1)
-				i = NEXTTXD(i);
-		}
+		size =
+		    (le32_to_cpu(di->txd64[i].ctrl2) &
+		     D64_CTRL2_BC_MASK);
+
+		di->txd64[i].addrlow = cpu_to_le32(0xdeadbeef);
+		di->txd64[i].addrhigh = cpu_to_le32(0xdeadbeef);
+
+		txp = di->txp[i];
+		di->txp[i] = NULL;
 
 		pci_unmap_single(di->pbus, pa, size, PCI_DMA_TODEVICE);
 	}
@@ -1740,157 +1389,16 @@ static void *dma64_getnexttxp(struct dma_info *di, enum txd_range range)
 	di->txin = i;
 
 	/* tx flow control */
-	di->dma.txavail = di->ntxd - NTXDACTIVE(di->txin, di->txout) - 1;
+	di->dma.txavail = di->ntxd - ntxdactive(di, di->txin, di->txout) - 1;
 
 	return txp;
 
  bogus:
-	DMA_NONE(("dma_getnexttxp: bogus curr: start %d end %d txout %d force %d\n", start, end, di->txout, forceall));
+	DMA_NONE(("dma_getnexttxp: bogus curr: start %d end %d txout %d "
+		  "force %d\n", start, end, di->txout, forceall));
 	return NULL;
 }
 
-static void *dma64_getnextrxp(struct dma_info *di, bool forceall)
-{
-	uint i, curr;
-	void *rxp;
-	dmaaddr_t pa;
-
-	i = di->rxin;
-
-	/* return if no packets posted */
-	if (i == di->rxout)
-		return NULL;
-
-	curr =
-	    B2I(((R_REG(&di->d64rxregs->status0) & D64_RS0_CD_MASK) -
-		 di->rcvptrbase) & D64_RS0_CD_MASK, struct dma64desc);
-
-	/* ignore curr if forceall */
-	if (!forceall && (i == curr))
-		return NULL;
-
-	/* get the packet pointer that corresponds to the rx descriptor */
-	rxp = di->rxp[i];
-	di->rxp[i] = NULL;
-
-	PHYSADDRLOSET(pa,
-		      (BUS_SWAP32(R_SM(&di->rxd64[i].addrlow)) -
-		       di->dataoffsetlow));
-	PHYSADDRHISET(pa,
-		      (BUS_SWAP32(R_SM(&di->rxd64[i].addrhigh)) -
-		       di->dataoffsethigh));
-
-	/* clear this packet from the descriptor ring */
-	pci_unmap_single(di->pbus, pa, di->rxbufsize, PCI_DMA_FROMDEVICE);
-
-	W_SM(&di->rxd64[i].addrlow, 0xdeadbeef);
-	W_SM(&di->rxd64[i].addrhigh, 0xdeadbeef);
-
-	di->rxin = NEXTRXD(i);
-
-	return rxp;
-}
-
-static bool _dma64_addrext(dma64regs_t *dma64regs)
-{
-	u32 w;
-	OR_REG(&dma64regs->control, D64_XC_AE);
-	w = R_REG(&dma64regs->control);
-	AND_REG(&dma64regs->control, ~D64_XC_AE);
-	return (w & D64_XC_AE) == D64_XC_AE;
-}
-
-/*
- * Rotate all active tx dma ring entries "forward" by (ActiveDescriptor - txin).
- */
-static void dma64_txrotate(struct dma_info *di)
-{
-	u16 ad;
-	uint nactive;
-	uint rot;
-	u16 old, new;
-	u32 w;
-	u16 first, last;
-
-	nactive = _dma_txactive(di);
-	ad = (u16) (B2I((((R_REG(&di->d64txregs->status1) &
-			   D64_XS1_AD_MASK) - di->xmtptrbase) &
-			   D64_XS1_AD_MASK), struct dma64desc));
-	rot = TXD(ad - di->txin);
-
-	/* full-ring case is a lot harder - don't worry about this */
-	if (rot >= (di->ntxd - nactive)) {
-		DMA_ERROR(("%s: dma_txrotate: ring full - punt\n", di->name));
-		return;
-	}
-
-	first = di->txin;
-	last = PREVTXD(di->txout);
-
-	/* move entries starting at last and moving backwards to first */
-	for (old = last; old != PREVTXD(first); old = PREVTXD(old)) {
-		new = TXD(old + rot);
-
-		/*
-		 * Move the tx dma descriptor.
-		 * EOT is set only in the last entry in the ring.
-		 */
-		w = BUS_SWAP32(R_SM(&di->txd64[old].ctrl1)) & ~D64_CTRL1_EOT;
-		if (new == (di->ntxd - 1))
-			w |= D64_CTRL1_EOT;
-		W_SM(&di->txd64[new].ctrl1, BUS_SWAP32(w));
-
-		w = BUS_SWAP32(R_SM(&di->txd64[old].ctrl2));
-		W_SM(&di->txd64[new].ctrl2, BUS_SWAP32(w));
-
-		W_SM(&di->txd64[new].addrlow, R_SM(&di->txd64[old].addrlow));
-		W_SM(&di->txd64[new].addrhigh, R_SM(&di->txd64[old].addrhigh));
-
-		/* zap the old tx dma descriptor address field */
-		W_SM(&di->txd64[old].addrlow, BUS_SWAP32(0xdeadbeef));
-		W_SM(&di->txd64[old].addrhigh, BUS_SWAP32(0xdeadbeef));
-
-		/* move the corresponding txp[] entry */
-		di->txp[new] = di->txp[old];
-
-		/* Move the map */
-		if (DMASGLIST_ENAB) {
-			memcpy(&di->txp_dmah[new], &di->txp_dmah[old],
-			       sizeof(struct dma_seg_map));
-			memset(&di->txp_dmah[old], 0,
-			       sizeof(struct dma_seg_map));
-		}
-
-		di->txp[old] = NULL;
-	}
-
-	/* update txin and txout */
-	di->txin = ad;
-	di->txout = TXD(di->txout + rot);
-	di->dma.txavail = di->ntxd - NTXDACTIVE(di->txin, di->txout) - 1;
-
-	/* kick the chip */
-	W_REG(&di->d64txregs->ptr,
-	      di->xmtptrbase + I2B(di->txout, struct dma64desc));
-}
-
-uint dma_addrwidth(struct si_pub *sih, void *dmaregs)
-{
-	/* Perform 64-bit checks only if we want to advertise 64-bit (> 32bit) capability) */
-	/* DMA engine is 64-bit capable */
-	if ((ai_core_sflags(sih, 0, 0) & SISF_DMA64) == SISF_DMA64) {
-		/* backplane are 64-bit capable */
-		if (ai_backplane64(sih))
-			/* If bus is System Backplane or PCIE then we can access 64-bits */
-			if ((sih->bustype == SI_BUS) ||
-			    ((sih->bustype == PCI_BUS) &&
-			     (sih->buscoretype == PCIE_CORE_ID)))
-				return DMADDRWIDTH_64;
-	}
-	/* DMA hardware not supported by this driver*/
-	return DMADDRWIDTH_64;
-}
-
 /*
  * Mac80211 initiated actions sometimes require packets in the DMA queue to be
  * modified. The modified portion of the packet is not under control of the DMA
@@ -1912,6 +1420,6 @@ void dma_walk_packets(struct dma_pub *dmah, void (*callback_fnc)
 			tx_info = (struct ieee80211_tx_info *)skb->cb;
 			(callback_fnc)(tx_info, arg_a);
 		}
-		i = NEXTTXD(i);
+		i = nexttxd(di, i);
 	}
 }
diff --git a/drivers/staging/brcm80211/brcmsmac/dma.h b/drivers/staging/brcm80211/brcmsmac/dma.h
index 9c8b9a6a557e..ebc5bc546f3b 100644
--- a/drivers/staging/brcm80211/brcmsmac/dma.h
+++ b/drivers/staging/brcm80211/brcmsmac/dma.h
@@ -17,8 +17,13 @@
 #ifndef	_BRCM_DMA_H_
 #define	_BRCM_DMA_H_
 
+#include <linux/delay.h>
 #include "types.h"		/* forward structure declarations */
 
+/* map/unmap direction */
+#define	DMA_TX	1		/* TX direction for DMA */
+#define	DMA_RX	2		/* RX direction for DMA */
+
 /* DMA structure:
  *  support two DMA engines: 32 bits address or 64 bit addressing
  *  basic DMA register set is per channel(transmit or receive)
@@ -38,19 +43,14 @@ struct dma32diag {	/* diag access */
 
 /* dma registers per channel(xmt or rcv) */
 struct dma64regs {
-	u32 control;		/* enable, et al */
-	u32 ptr;		/* last descriptor posted to chip */
-	u32 addrlow;		/* descriptor ring base address low 32-bits (8K aligned) */
-	u32 addrhigh;	/* descriptor ring base address bits 63:32 (8K aligned) */
-	u32 status0;		/* current descriptor, xmt state */
-	u32 status1;		/* active descriptor, xmt error */
+	u32 control;	/* enable, et al */
+	u32 ptr;	/* last descriptor posted to chip */
+	u32 addrlow;	/* desc ring base address low 32-bits (8K aligned) */
+	u32 addrhigh;	/* desc ring base address bits 63:32 (8K aligned) */
+	u32 status0;	/* current descriptor, xmt state */
+	u32 status1;	/* active descriptor, xmt error */
 };
 
-/* map/unmap direction */
-#define	DMA_TX	1		/* TX direction for DMA */
-#define	DMA_RX	2		/* RX direction for DMA */
-#define BUS_SWAP32(v)		(v)
-
 /* range param for dma_getnexttxp() and dma_txreclaim */
 enum txd_range {
 	DMA_RANGE_ALL = 1,
@@ -58,110 +58,11 @@ enum txd_range {
 	DMA_RANGE_TRANSFERED
 };
 
-/* dma function type */
-typedef void (*di_detach_t) (struct dma_pub *dmah);
-typedef bool(*di_txreset_t) (struct dma_pub *dmah);
-typedef bool(*di_rxreset_t) (struct dma_pub *dmah);
-typedef bool(*di_rxidle_t) (struct dma_pub *dmah);
-typedef void (*di_txinit_t) (struct dma_pub *dmah);
-typedef bool(*di_txenabled_t) (struct dma_pub *dmah);
-typedef void (*di_rxinit_t) (struct dma_pub *dmah);
-typedef void (*di_txsuspend_t) (struct dma_pub *dmah);
-typedef void (*di_txresume_t) (struct dma_pub *dmah);
-typedef bool(*di_txsuspended_t) (struct dma_pub *dmah);
-typedef bool(*di_txsuspendedidle_t) (struct dma_pub *dmah);
-typedef int (*di_txfast_t) (struct dma_pub *dmah, struct sk_buff *p,
-			    bool commit);
-typedef int (*di_txunframed_t) (struct dma_pub *dmah, void *p, uint len,
-				bool commit);
-typedef void *(*di_getpos_t) (struct dma_pub *di, bool direction);
-typedef void (*di_fifoloopbackenable_t) (struct dma_pub *dmah);
-typedef bool(*di_txstopped_t) (struct dma_pub *dmah);
-typedef bool(*di_rxstopped_t) (struct dma_pub *dmah);
-typedef bool(*di_rxenable_t) (struct dma_pub *dmah);
-typedef bool(*di_rxenabled_t) (struct dma_pub *dmah);
-typedef void *(*di_rx_t) (struct dma_pub *dmah);
-typedef bool(*di_rxfill_t) (struct dma_pub *dmah);
-typedef void (*di_txreclaim_t) (struct dma_pub *dmah, enum txd_range range);
-typedef void (*di_rxreclaim_t) (struct dma_pub *dmah);
-typedef unsigned long (*di_getvar_t) (struct dma_pub *dmah,
-				      const char *name);
-typedef void *(*di_getnexttxp_t) (struct dma_pub *dmah, enum txd_range range);
-typedef void *(*di_getnextrxp_t) (struct dma_pub *dmah, bool forceall);
-typedef void *(*di_peeknexttxp_t) (struct dma_pub *dmah);
-typedef void *(*di_peeknextrxp_t) (struct dma_pub *dmah);
-typedef void (*di_rxparam_get_t) (struct dma_pub *dmah, u16 *rxoffset,
-				  u16 *rxbufsize);
-typedef void (*di_txblock_t) (struct dma_pub *dmah);
-typedef void (*di_txunblock_t) (struct dma_pub *dmah);
-typedef uint(*di_txactive_t) (struct dma_pub *dmah);
-typedef void (*di_txrotate_t) (struct dma_pub *dmah);
-typedef void (*di_counterreset_t) (struct dma_pub *dmah);
-typedef uint(*di_ctrlflags_t) (struct dma_pub *dmah, uint mask, uint flags);
-typedef char *(*di_dump_t) (struct dma_pub *dmah, struct brcmu_strbuf *b,
-			    bool dumpring);
-typedef char *(*di_dumptx_t) (struct dma_pub *dmah, struct brcmu_strbuf *b,
-			      bool dumpring);
-typedef char *(*di_dumprx_t) (struct dma_pub *dmah, struct brcmu_strbuf *b,
-			      bool dumpring);
-typedef uint(*di_rxactive_t) (struct dma_pub *dmah);
-typedef uint(*di_txpending_t) (struct dma_pub *dmah);
-typedef uint(*di_txcommitted_t) (struct dma_pub *dmah);
-
-/* dma opsvec */
-struct di_fcn_s {
-	di_detach_t detach;
-	di_txinit_t txinit;
-	di_txreset_t txreset;
-	di_txenabled_t txenabled;
-	di_txsuspend_t txsuspend;
-	di_txresume_t txresume;
-	di_txsuspended_t txsuspended;
-	di_txsuspendedidle_t txsuspendedidle;
-	di_txfast_t txfast;
-	di_txunframed_t txunframed;
-	di_getpos_t getpos;
-	di_txstopped_t txstopped;
-	di_txreclaim_t txreclaim;
-	di_getnexttxp_t getnexttxp;
-	di_peeknexttxp_t peeknexttxp;
-	di_txblock_t txblock;
-	di_txunblock_t txunblock;
-	di_txactive_t txactive;
-	di_txrotate_t txrotate;
-
-	di_rxinit_t rxinit;
-	di_rxreset_t rxreset;
-	di_rxidle_t rxidle;
-	di_rxstopped_t rxstopped;
-	di_rxenable_t rxenable;
-	di_rxenabled_t rxenabled;
-	di_rx_t rx;
-	di_rxfill_t rxfill;
-	di_rxreclaim_t rxreclaim;
-	di_getnextrxp_t getnextrxp;
-	di_peeknextrxp_t peeknextrxp;
-	di_rxparam_get_t rxparam_get;
-
-	di_fifoloopbackenable_t fifoloopbackenable;
-	di_getvar_t d_getvar;
-	di_counterreset_t counterreset;
-	di_ctrlflags_t ctrlflags;
-	di_dump_t dump;
-	di_dumptx_t dumptx;
-	di_dumprx_t dumprx;
-	di_rxactive_t rxactive;
-	di_txpending_t txpending;
-	di_txcommitted_t txcommitted;
-	uint endnum;
-};
-
 /*
  * Exported data structure (read-only)
  */
 /* export structure */
 struct dma_pub {
-	const struct di_fcn_s *di_fn;	/* DMA function pointers */
 	uint txavail;		/* # free tx descriptors */
 	uint dmactrlflags;	/* dma control flags */
 
@@ -173,78 +74,47 @@ struct dma_pub {
 };
 
 extern struct dma_pub *dma_attach(char *name, struct si_pub *sih,
-			    void *dmaregstx, void *dmaregsrx, uint ntxd,
-			    uint nrxd, uint rxbufsize, int rxextheadroom,
+			    void __iomem *dmaregstx, void __iomem *dmaregsrx,
+			    uint ntxd, uint nrxd,
+			    uint rxbufsize, int rxextheadroom,
 			    uint nrxpost, uint rxoffset, uint *msg_level);
 
-extern const struct di_fcn_s dma64proc;
-
-#define dma_detach(di)			(dma64proc.detach(di))
-#define dma_txreset(di)			(dma64proc.txreset(di))
-#define dma_rxreset(di)			(dma64proc.rxreset(di))
-#define dma_rxidle(di)			(dma64proc.rxidle(di))
-#define dma_txinit(di)                  (dma64proc.txinit(di))
-#define dma_txenabled(di)               (dma64proc.txenabled(di))
-#define dma_rxinit(di)                  (dma64proc.rxinit(di))
-#define dma_txsuspend(di)               (dma64proc.txsuspend(di))
-#define dma_txresume(di)                (dma64proc.txresume(di))
-#define dma_txsuspended(di)             (dma64proc.txsuspended(di))
-#define dma_txsuspendedidle(di)         (dma64proc.txsuspendedidle(di))
-#define dma_txfast(di, p, commit)	(dma64proc.txfast(di, p, commit))
-#define dma_txunframed(di, p, l, commit)(dma64proc.txunframed(di, p, l, commit))
-#define dma_getpos(di, dir)		(dma64proc.getpos(di, dir))
-#define dma_fifoloopbackenable(di)      (dma64proc.fifoloopbackenable(di))
-#define dma_txstopped(di)               (dma64proc.txstopped(di))
-#define dma_rxstopped(di)               (dma64proc.rxstopped(di))
-#define dma_rxenable(di)                (dma64proc.rxenable(di))
-#define dma_rxenabled(di)               (dma64proc.rxenabled(di))
-#define dma_rx(di)                      (dma64proc.rx(di))
-#define dma_rxfill(di)                  (dma64proc.rxfill(di))
-#define dma_txreclaim(di, range)	(dma64proc.txreclaim(di, range))
-#define dma_rxreclaim(di)               (dma64proc.rxreclaim(di))
-#define dma_getvar(di, name)		(dma64proc.d_getvar(di, name))
-#define dma_getnexttxp(di, range)	(dma64proc.getnexttxp(di, range))
-#define dma_getnextrxp(di, forceall)    (dma64proc.getnextrxp(di, forceall))
-#define dma_peeknexttxp(di)             (dma64proc.peeknexttxp(di))
-#define dma_peeknextrxp(di)             (dma64proc.peeknextrxp(di))
-#define dma_rxparam_get(di, off, bufs)	(dma64proc.rxparam_get(di, off, bufs))
-
-#define dma_txblock(di)                 (dma64proc.txblock(di))
-#define dma_txunblock(di)               (dma64proc.txunblock(di))
-#define dma_txactive(di)                (dma64proc.txactive(di))
-#define dma_rxactive(di)                (dma64proc.rxactive(di))
-#define dma_txrotate(di)                (dma64proc.txrotate(di))
-#define dma_counterreset(di)            (dma64proc.counterreset(di))
-#define dma_ctrlflags(di, mask, flags)  (dma64proc.ctrlflags((di), (mask), (flags)))
-#define dma_txpending(di)		(dma64proc.txpending(di))
-#define dma_txcommitted(di)		(dma64proc.txcommitted(di))
-
-
-/* return addresswidth allowed
- * This needs to be done after SB attach but before dma attach.
- * SB attach provides ability to probe backplane and dma core capabilities
- * This info is needed by DMA_ALLOC_CONSISTENT in dma attach
- */
-extern uint dma_addrwidth(struct si_pub *sih, void *dmaregs);
+void dma_rxinit(struct dma_pub *pub);
+struct sk_buff *dma_rx(struct dma_pub *pub);
+bool dma_rxfill(struct dma_pub *pub);
+bool dma_rxreset(struct dma_pub *pub);
+bool dma_txreset(struct dma_pub *pub);
+void dma_txinit(struct dma_pub *pub);
+int dma_txfast(struct dma_pub *pub, struct sk_buff *p0, bool commit);
+void dma_txsuspend(struct dma_pub *pub);
+bool dma_txsuspended(struct dma_pub *pub);
+void dma_txresume(struct dma_pub *pub);
+void dma_txreclaim(struct dma_pub *pub, enum txd_range range);
+void dma_rxreclaim(struct dma_pub *pub);
+void dma_detach(struct dma_pub *pub);
+unsigned long dma_getvar(struct dma_pub *pub, const char *name);
+struct sk_buff *dma_getnexttxp(struct dma_pub *pub, enum txd_range range);
+void dma_counterreset(struct dma_pub *pub);
+
 void dma_walk_packets(struct dma_pub *dmah, void (*callback_fnc)
 		      (void *pkt, void *arg_a), void *arg_a);
 
 /*
- * DMA(Bug) on some chips seems to declare that the packet is ready, but the
- * packet length is not updated yet (by DMA) on the expected time.
+ * DMA(Bug) on bcm47xx chips seems to declare that the packet is ready, but
+ * the packet length is not updated yet (by DMA) on the expected time.
  * Workaround is to hold processor till DMA updates the length, and stay off
  * the bus to allow DMA update the length in buffer
  */
 static inline void dma_spin_for_len(uint len, struct sk_buff *head)
 {
-#if defined(__mips__)
+#if defined(CONFIG_BCM47XX)
 	if (!len) {
 		while (!(len = *(u16 *) KSEG1ADDR(head->data)))
 			udelay(1);
 
 		*(u16 *) (head->data) = cpu_to_le16((u16) len);
 	}
-#endif				/* defined(__mips__) */
+#endif				/* defined(CONFIG_BCM47XX) */
 }
 
 #endif				/* _BRCM_DMA_H_ */
diff --git a/drivers/staging/brcm80211/brcmsmac/mac80211_if.c b/drivers/staging/brcm80211/brcmsmac/mac80211_if.c
index d6de44e430d3..ac8d02bd34f2 100644
--- a/drivers/staging/brcm80211/brcmsmac/mac80211_if.c
+++ b/drivers/staging/brcm80211/brcmsmac/mac80211_if.c
@@ -31,38 +31,10 @@
 #include "pub.h"
 #include "ucode_loader.h"
 #include "mac80211_if.h"
+#include "main.h"
 
 #define N_TX_QUEUES	4 /* #tx queues on mac80211<->driver interface */
 
-#define LOCK(wl)	spin_lock_bh(&(wl)->lock)
-#define UNLOCK(wl)	spin_unlock_bh(&(wl)->lock)
-
-/* locking from inside brcms_isr */
-#define ISR_LOCK(wl, flags)\
-	do {\
-		spin_lock(&(wl)->isr_lock);\
-		(void)(flags); } \
-	while (0)
-
-#define ISR_UNLOCK(wl, flags)\
-	do {\
-		spin_unlock(&(wl)->isr_lock);\
-		(void)(flags); } \
-	while (0)
-
-/* locking under LOCK() to synchronize with brcms_isr */
-#define INT_LOCK(wl, flags)	spin_lock_irqsave(&(wl)->isr_lock, flags)
-#define INT_UNLOCK(wl, flags)	spin_unlock_irqrestore(&(wl)->isr_lock, flags)
-
-static void brcms_timer(unsigned long data);
-static void _brcms_timer(struct brcms_timer *t);
-
-
-static int ieee_hw_init(struct ieee80211_hw *hw);
-static int ieee_hw_rate_init(struct ieee80211_hw *hw);
-
-static int wl_linux_watchdog(void *ctx);
-
 /* Flags we support */
 #define MAC_FILTERS (FIF_PROMISC_IN_BSS | \
 	FIF_ALLMULTI | \
@@ -72,20 +44,42 @@ static int wl_linux_watchdog(void *ctx);
 	FIF_OTHER_BSS | \
 	FIF_BCN_PRBRESP_PROMISC)
 
-static int n_adapters_found;
+#define CHAN2GHZ(channel, freqency, chflags)  { \
+	.band = IEEE80211_BAND_2GHZ, \
+	.center_freq = (freqency), \
+	.hw_value = (channel), \
+	.flags = chflags, \
+	.max_antenna_gain = 0, \
+	.max_power = 19, \
+}
 
-static int brcms_request_fw(struct brcms_info *wl, struct pci_dev *pdev);
-static void brcms_release_fw(struct brcms_info *wl);
+#define CHAN5GHZ(channel, chflags)  { \
+	.band = IEEE80211_BAND_5GHZ, \
+	.center_freq = 5000 + 5*(channel), \
+	.hw_value = (channel), \
+	.flags = chflags, \
+	.max_antenna_gain = 0, \
+	.max_power = 21, \
+}
 
-/* local prototypes */
-static void brcms_dpc(unsigned long data);
-static irqreturn_t brcms_isr(int irq, void *dev_id);
+#define RATE(rate100m, _flags) { \
+	.bitrate = (rate100m), \
+	.flags = (_flags), \
+	.hw_value = (rate100m / 5), \
+}
 
-static int __devinit brcms_pci_probe(struct pci_dev *pdev,
-				  const struct pci_device_id *ent);
-static void brcms_remove(struct pci_dev *pdev);
-static void brcms_free(struct brcms_info *wl);
-static void brcms_set_basic_rate(struct wl_rateset *rs, u16 rate, bool is_br);
+struct firmware_hdr {
+	__le32 offset;
+	__le32 len;
+	__le32 idx;
+};
+
+static const char * const brcms_firmwares[MAX_FW_IMAGES] = {
+	"brcm/bcm43xx",
+	NULL
+};
+
+static int n_adapters_found;
 
 MODULE_AUTHOR("Broadcom Corporation");
 MODULE_DESCRIPTION("Broadcom 802.11n wireless LAN driver.");
@@ -94,11 +88,10 @@ MODULE_LICENSE("Dual BSD/GPL");
 
 /* recognized PCI IDs */
 static DEFINE_PCI_DEVICE_TABLE(brcms_pci_id_table) = {
-	{PCI_VENDOR_ID_BROADCOM, 0x4357, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},	/* 43225 2G */
-	{PCI_VENDOR_ID_BROADCOM, 0x4353, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},	/* 43224 DUAL */
-	{PCI_VENDOR_ID_BROADCOM, 0x4727, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},	/* 4313 DUAL */
-	/* 43224 Ven */
-	{PCI_VENDOR_ID_BROADCOM, 0x0576, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4357) }, /* 43225 2G */
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4353) }, /* 43224 DUAL */
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4727) }, /* 4313 DUAL */
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x0576) }, /* 43224 Ven */
 	{0}
 };
 
@@ -107,60 +100,178 @@ MODULE_DEVICE_TABLE(pci, brcms_pci_id_table);
 #ifdef BCMDBG
 static int msglevel = 0xdeadbeef;
 module_param(msglevel, int, 0);
-static int phymsglevel = 0xdeadbeef;
-module_param(phymsglevel, int, 0);
 #endif				/* BCMDBG */
 
-#define HW_TO_WL(hw)	 (hw->priv)
-#define WL_TO_HW(wl)	  (wl->pub->ieee_hw)
-
-/* MAC80211 callback functions */
-static int brcms_ops_start(struct ieee80211_hw *hw);
-static void brcms_ops_stop(struct ieee80211_hw *hw);
-static int brcms_ops_add_interface(struct ieee80211_hw *hw,
-				struct ieee80211_vif *vif);
-static void brcms_ops_remove_interface(struct ieee80211_hw *hw,
-				    struct ieee80211_vif *vif);
-static int brcms_ops_config(struct ieee80211_hw *hw, u32 changed);
-static void brcms_ops_bss_info_changed(struct ieee80211_hw *hw,
-				    struct ieee80211_vif *vif,
-				    struct ieee80211_bss_conf *info,
-				    u32 changed);
-static void brcms_ops_configure_filter(struct ieee80211_hw *hw,
-				    unsigned int changed_flags,
-				    unsigned int *total_flags, u64 multicast);
-static int brcms_ops_set_tim(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
-			  bool set);
-static void brcms_ops_sw_scan_start(struct ieee80211_hw *hw);
-static void brcms_ops_sw_scan_complete(struct ieee80211_hw *hw);
-static void brcms_ops_set_tsf(struct ieee80211_hw *hw, u64 tsf);
-static int brcms_ops_get_stats(struct ieee80211_hw *hw,
-			    struct ieee80211_low_level_stats *stats);
-static void brcms_ops_sta_notify(struct ieee80211_hw *hw,
-			      struct ieee80211_vif *vif,
-			      enum sta_notify_cmd cmd,
-			      struct ieee80211_sta *sta);
-static int brcms_ops_conf_tx(struct ieee80211_hw *hw, u16 queue,
-			  const struct ieee80211_tx_queue_params *params);
-static u64 brcms_ops_get_tsf(struct ieee80211_hw *hw);
-static int brcms_ops_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-		      struct ieee80211_sta *sta);
-static int brcms_ops_sta_remove(struct ieee80211_hw *hw,
-				struct ieee80211_vif *vif,
-				struct ieee80211_sta *sta);
-static int brcms_ops_ampdu_action(struct ieee80211_hw *hw,
-			       struct ieee80211_vif *vif,
-			       enum ieee80211_ampdu_mlme_action action,
-			       struct ieee80211_sta *sta, u16 tid, u16 *ssn,
-			       u8 buf_size);
-static void brcms_ops_rfkill_poll(struct ieee80211_hw *hw);
-static void brcms_ops_flush(struct ieee80211_hw *hw, bool drop);
+static struct ieee80211_channel brcms_2ghz_chantable[] = {
+	CHAN2GHZ(1, 2412, IEEE80211_CHAN_NO_HT40MINUS),
+	CHAN2GHZ(2, 2417, IEEE80211_CHAN_NO_HT40MINUS),
+	CHAN2GHZ(3, 2422, IEEE80211_CHAN_NO_HT40MINUS),
+	CHAN2GHZ(4, 2427, IEEE80211_CHAN_NO_HT40MINUS),
+	CHAN2GHZ(5, 2432, 0),
+	CHAN2GHZ(6, 2437, 0),
+	CHAN2GHZ(7, 2442, 0),
+	CHAN2GHZ(8, 2447, IEEE80211_CHAN_NO_HT40PLUS),
+	CHAN2GHZ(9, 2452, IEEE80211_CHAN_NO_HT40PLUS),
+	CHAN2GHZ(10, 2457, IEEE80211_CHAN_NO_HT40PLUS),
+	CHAN2GHZ(11, 2462, IEEE80211_CHAN_NO_HT40PLUS),
+	CHAN2GHZ(12, 2467,
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_NO_HT40PLUS),
+	CHAN2GHZ(13, 2472,
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_NO_HT40PLUS),
+	CHAN2GHZ(14, 2484,
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_NO_HT40PLUS | IEEE80211_CHAN_NO_HT40MINUS)
+};
+
+static struct ieee80211_channel brcms_5ghz_nphy_chantable[] = {
+	/* UNII-1 */
+	CHAN5GHZ(36, IEEE80211_CHAN_NO_HT40MINUS),
+	CHAN5GHZ(40, IEEE80211_CHAN_NO_HT40PLUS),
+	CHAN5GHZ(44, IEEE80211_CHAN_NO_HT40MINUS),
+	CHAN5GHZ(48, IEEE80211_CHAN_NO_HT40PLUS),
+	/* UNII-2 */
+	CHAN5GHZ(52,
+		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40MINUS),
+	CHAN5GHZ(56,
+		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS),
+	CHAN5GHZ(60,
+		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40MINUS),
+	CHAN5GHZ(64,
+		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS),
+	/* MID */
+	CHAN5GHZ(100,
+		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40MINUS),
+	CHAN5GHZ(104,
+		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS),
+	CHAN5GHZ(108,
+		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40MINUS),
+	CHAN5GHZ(112,
+		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS),
+	CHAN5GHZ(116,
+		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40MINUS),
+	CHAN5GHZ(120,
+		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS),
+	CHAN5GHZ(124,
+		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40MINUS),
+	CHAN5GHZ(128,
+		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS),
+	CHAN5GHZ(132,
+		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40MINUS),
+	CHAN5GHZ(136,
+		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS),
+	CHAN5GHZ(140,
+		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
+		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS |
+		 IEEE80211_CHAN_NO_HT40MINUS),
+	/* UNII-3 */
+	CHAN5GHZ(149, IEEE80211_CHAN_NO_HT40MINUS),
+	CHAN5GHZ(153, IEEE80211_CHAN_NO_HT40PLUS),
+	CHAN5GHZ(157, IEEE80211_CHAN_NO_HT40MINUS),
+	CHAN5GHZ(161, IEEE80211_CHAN_NO_HT40PLUS),
+	CHAN5GHZ(165, IEEE80211_CHAN_NO_HT40PLUS | IEEE80211_CHAN_NO_HT40MINUS)
+};
+
+/*
+ * The rate table is used for both 2.4G and 5G rates. The
+ * latter being a subset as it does not support CCK rates.
+ */
+static struct ieee80211_rate legacy_ratetable[] = {
+	RATE(10, 0),
+	RATE(20, IEEE80211_RATE_SHORT_PREAMBLE),
+	RATE(55, IEEE80211_RATE_SHORT_PREAMBLE),
+	RATE(110, IEEE80211_RATE_SHORT_PREAMBLE),
+	RATE(60, 0),
+	RATE(90, 0),
+	RATE(120, 0),
+	RATE(180, 0),
+	RATE(240, 0),
+	RATE(360, 0),
+	RATE(480, 0),
+	RATE(540, 0),
+};
+
+static const struct ieee80211_supported_band brcms_band_2GHz_nphy_template = {
+	.band = IEEE80211_BAND_2GHZ,
+	.channels = brcms_2ghz_chantable,
+	.n_channels = ARRAY_SIZE(brcms_2ghz_chantable),
+	.bitrates = legacy_ratetable,
+	.n_bitrates = ARRAY_SIZE(legacy_ratetable),
+	.ht_cap = {
+		   /* from include/linux/ieee80211.h */
+		   .cap = IEEE80211_HT_CAP_GRN_FLD |
+		   IEEE80211_HT_CAP_SGI_20 |
+		   IEEE80211_HT_CAP_SGI_40 | IEEE80211_HT_CAP_40MHZ_INTOLERANT,
+		   .ht_supported = true,
+		   .ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K,
+		   .ampdu_density = AMPDU_DEF_MPDU_DENSITY,
+		   .mcs = {
+			   /* placeholders for now */
+			   .rx_mask = {0xff, 0xff, 0, 0, 0, 0, 0, 0, 0, 0},
+			   .rx_highest = cpu_to_le16(500),
+			   .tx_params = IEEE80211_HT_MCS_TX_DEFINED}
+		   }
+};
+
+static const struct ieee80211_supported_band brcms_band_5GHz_nphy_template = {
+	.band = IEEE80211_BAND_5GHZ,
+	.channels = brcms_5ghz_nphy_chantable,
+	.n_channels = ARRAY_SIZE(brcms_5ghz_nphy_chantable),
+	.bitrates = legacy_ratetable + BRCMS_LEGACY_5G_RATE_OFFSET,
+	.n_bitrates = ARRAY_SIZE(legacy_ratetable) -
+			BRCMS_LEGACY_5G_RATE_OFFSET,
+	.ht_cap = {
+		   .cap = IEEE80211_HT_CAP_GRN_FLD | IEEE80211_HT_CAP_SGI_20 |
+			  IEEE80211_HT_CAP_SGI_40 |
+			  IEEE80211_HT_CAP_40MHZ_INTOLERANT, /* No 40 mhz yet */
+		   .ht_supported = true,
+		   .ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K,
+		   .ampdu_density = AMPDU_DEF_MPDU_DENSITY,
+		   .mcs = {
+			   /* placeholders for now */
+			   .rx_mask = {0xff, 0xff, 0, 0, 0, 0, 0, 0, 0, 0},
+			   .rx_highest = cpu_to_le16(500),
+			   .tx_params = IEEE80211_HT_MCS_TX_DEFINED}
+		   }
+};
+
+/* flags the given rate in rateset as requested */
+static void brcms_set_basic_rate(struct brcm_rateset *rs, u16 rate, bool is_br)
+{
+	u32 i;
+
+	for (i = 0; i < rs->count; i++) {
+		if (rate != (rs->rates[i] & 0x7f))
+			continue;
+
+		if (is_br)
+			rs->rates[i] |= BRCMS_RATE_FLAG;
+		else
+			rs->rates[i] &= BRCMS_RATE_MASK;
+		return;
+	}
+}
 
 static void brcms_ops_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct brcms_info *wl = hw->priv;
 
-	LOCK(wl);
+	spin_lock_bh(&wl->lock);
 	if (!wl->pub->up) {
 		wiphy_err(wl->wiphy, "ops->tx called while down\n");
 		kfree_skb(skb);
@@ -168,21 +279,18 @@ static void brcms_ops_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 	}
 	brcms_c_sendpkt_mac80211(wl->wlc, skb, hw);
  done:
-	UNLOCK(wl);
+	spin_unlock_bh(&wl->lock);
 }
 
 static int brcms_ops_start(struct ieee80211_hw *hw)
 {
 	struct brcms_info *wl = hw->priv;
 	bool blocked;
-	/*
-	  struct ieee80211_channel *curchan = hw->conf.channel;
-	*/
 
 	ieee80211_wake_queues(hw);
-	LOCK(wl);
+	spin_lock_bh(&wl->lock);
 	blocked = brcms_rfkill_set_hw_state(wl);
-	UNLOCK(wl);
+	spin_unlock_bh(&wl->lock);
 	if (!blocked)
 		wiphy_rfkill_stop_polling(wl->pub->ieee_hw->wiphy);
 
@@ -211,15 +319,18 @@ brcms_ops_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 		return -EOPNOTSUPP;
 	}
 
-	wl = HW_TO_WL(hw);
-	LOCK(wl);
-	err = brcms_up(wl);
-	UNLOCK(wl);
+	wl = hw->priv;
+	spin_lock_bh(&wl->lock);
+	if (!wl->pub->up)
+		err = brcms_up(wl);
+	else
+		err = -ENODEV;
+	spin_unlock_bh(&wl->lock);
 
-	if (err != 0) {
+	if (err != 0)
 		wiphy_err(hw->wiphy, "%s: brcms_up() returned %d\n", __func__,
 			  err);
-	}
+
 	return err;
 }
 
@@ -228,60 +339,26 @@ brcms_ops_remove_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 {
 	struct brcms_info *wl;
 
-	wl = HW_TO_WL(hw);
+	wl = hw->priv;
 
 	/* put driver in down state */
-	LOCK(wl);
+	spin_lock_bh(&wl->lock);
 	brcms_down(wl);
-	UNLOCK(wl);
-}
-
-/*
- * precondition: perimeter lock has been acquired
- */
-static int
-ieee_set_channel(struct ieee80211_hw *hw, struct ieee80211_channel *chan,
-		 enum nl80211_channel_type type)
-{
-	struct brcms_info *wl = HW_TO_WL(hw);
-	int err = 0;
-
-	switch (type) {
-	case NL80211_CHAN_HT20:
-	case NL80211_CHAN_NO_HT:
-		err = brcms_c_set(wl->wlc, BRCM_SET_CHANNEL, chan->hw_value);
-		break;
-	case NL80211_CHAN_HT40MINUS:
-	case NL80211_CHAN_HT40PLUS:
-		wiphy_err(hw->wiphy,
-			  "%s: Need to implement 40 Mhz Channels!\n", __func__);
-		err = 1;
-		break;
-	}
-
-	if (err)
-		return -EIO;
-	return err;
+	spin_unlock_bh(&wl->lock);
 }
 
 static int brcms_ops_config(struct ieee80211_hw *hw, u32 changed)
 {
 	struct ieee80211_conf *conf = &hw->conf;
-	struct brcms_info *wl = HW_TO_WL(hw);
+	struct brcms_info *wl = hw->priv;
 	int err = 0;
 	int new_int;
 	struct wiphy *wiphy = hw->wiphy;
 
-	LOCK(wl);
+	spin_lock_bh(&wl->lock);
 	if (changed & IEEE80211_CONF_CHANGE_LISTEN_INTERVAL) {
-		if (brcms_c_set_par(wl->wlc, IOV_BCN_LI_BCN,
-				    conf->listen_interval) < 0) {
-			wiphy_err(wiphy, "%s: Error setting listen_interval\n",
-				  __func__);
-			err = -EIO;
-			goto config_out;
-		}
-		brcms_c_get_par(wl->wlc, IOV_BCN_LI_BCN, &new_int);
+		brcms_c_set_beacon_listen_interval(wl->wlc,
+						   conf->listen_interval);
 	}
 	if (changed & IEEE80211_CONF_CHANGE_MONITOR)
 		wiphy_err(wiphy, "%s: change monitor mode: %s (implement)\n",
@@ -293,40 +370,33 @@ static int brcms_ops_config(struct ieee80211_hw *hw, u32 changed)
 			  "true" : "false");
 
 	if (changed & IEEE80211_CONF_CHANGE_POWER) {
-		if (brcms_c_set_par(wl->wlc, IOV_QTXPOWER,
-				    conf->power_level * 4) < 0) {
+		err = brcms_c_set_tx_power(wl->wlc, conf->power_level);
+		if (err < 0) {
 			wiphy_err(wiphy, "%s: Error setting power_level\n",
 				  __func__);
-			err = -EIO;
 			goto config_out;
 		}
-		brcms_c_get_par(wl->wlc, IOV_QTXPOWER, &new_int);
-		if (new_int != (conf->power_level * 4))
+		new_int = brcms_c_get_tx_power(wl->wlc);
+		if (new_int != conf->power_level)
 			wiphy_err(wiphy, "%s: Power level req != actual, %d %d"
-				  "\n", __func__, conf->power_level * 4,
+				  "\n", __func__, conf->power_level,
 				  new_int);
 	}
 	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
-		err = ieee_set_channel(hw, conf->channel, conf->channel_type);
-	}
-	if (changed & IEEE80211_CONF_CHANGE_RETRY_LIMITS) {
-		if (brcms_c_set
-		    (wl->wlc, BRCM_SET_SRL,
-		     conf->short_frame_max_tx_count) < 0) {
-			wiphy_err(wiphy, "%s: Error setting srl\n", __func__);
-			err = -EIO;
-			goto config_out;
-		}
-		if (brcms_c_set(wl->wlc, BRCM_SET_LRL,
-				conf->long_frame_max_tx_count) < 0) {
-			wiphy_err(wiphy, "%s: Error setting lrl\n", __func__);
-			err = -EIO;
-			goto config_out;
-		}
+		if (conf->channel_type == NL80211_CHAN_HT20 ||
+		    conf->channel_type == NL80211_CHAN_NO_HT)
+			err = brcms_c_set_channel(wl->wlc,
+						  conf->channel->hw_value);
+		else
+			err = -ENOTSUPP;
 	}
+	if (changed & IEEE80211_CONF_CHANGE_RETRY_LIMITS)
+		err = brcms_c_set_rate_limit(wl->wlc,
+					     conf->short_frame_max_tx_count,
+					     conf->long_frame_max_tx_count);
 
  config_out:
-	UNLOCK(wl);
+	spin_unlock_bh(&wl->lock);
 	return err;
 }
 
@@ -335,9 +405,8 @@ brcms_ops_bss_info_changed(struct ieee80211_hw *hw,
 			struct ieee80211_vif *vif,
 			struct ieee80211_bss_conf *info, u32 changed)
 {
-	struct brcms_info *wl = HW_TO_WL(hw);
+	struct brcms_info *wl = hw->priv;
 	struct wiphy *wiphy = hw->wiphy;
-	int val;
 
 	if (changed & BSS_CHANGED_ASSOC) {
 		/* association status changed (associated/disassociated)
@@ -345,51 +414,48 @@ brcms_ops_bss_info_changed(struct ieee80211_hw *hw,
 		 */
 		wiphy_err(wiphy, "%s: %s: %sassociated\n", KBUILD_MODNAME,
 			  __func__, info->assoc ? "" : "dis");
-		LOCK(wl);
+		spin_lock_bh(&wl->lock);
 		brcms_c_associate_upd(wl->wlc, info->assoc);
-		UNLOCK(wl);
+		spin_unlock_bh(&wl->lock);
 	}
 	if (changed & BSS_CHANGED_ERP_SLOT) {
+		s8 val;
+
 		/* slot timing changed */
 		if (info->use_short_slot)
 			val = 1;
 		else
 			val = 0;
-		LOCK(wl);
-		brcms_c_set(wl->wlc, BRCMS_SET_SHORTSLOT_OVERRIDE, val);
-		UNLOCK(wl);
+		spin_lock_bh(&wl->lock);
+		brcms_c_set_shortslot_override(wl->wlc, val);
+		spin_unlock_bh(&wl->lock);
 	}
 
 	if (changed & BSS_CHANGED_HT) {
 		/* 802.11n parameters changed */
 		u16 mode = info->ht_operation_mode;
 
-		LOCK(wl);
+		spin_lock_bh(&wl->lock);
 		brcms_c_protection_upd(wl->wlc, BRCMS_PROT_N_CFG,
 			mode & IEEE80211_HT_OP_MODE_PROTECTION);
 		brcms_c_protection_upd(wl->wlc, BRCMS_PROT_N_NONGF,
 			mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT);
 		brcms_c_protection_upd(wl->wlc, BRCMS_PROT_N_OBSS,
 			mode & IEEE80211_HT_OP_MODE_NON_HT_STA_PRSNT);
-		UNLOCK(wl);
+		spin_unlock_bh(&wl->lock);
 	}
 	if (changed & BSS_CHANGED_BASIC_RATES) {
 		struct ieee80211_supported_band *bi;
 		u32 br_mask, i;
 		u16 rate;
-		struct wl_rateset rs;
+		struct brcm_rateset rs;
 		int error;
 
 		/* retrieve the current rates */
-		LOCK(wl);
-		error = brcms_c_ioctl(wl->wlc, BRCM_GET_CURR_RATESET,
-				  &rs, sizeof(rs), NULL);
-		UNLOCK(wl);
-		if (error) {
-			wiphy_err(wiphy, "%s: retrieve rateset failed: %d\n",
-				  __func__, error);
-			return;
-		}
+		spin_lock_bh(&wl->lock);
+		brcms_c_get_current_rateset(wl->wlc, &rs);
+		spin_unlock_bh(&wl->lock);
+
 		br_mask = info->basic_rates;
 		bi = hw->wiphy->bands[brcms_c_get_curband(wl->wlc)];
 		for (i = 0; i < bi->n_bitrates; i++) {
@@ -402,49 +468,55 @@ brcms_ops_bss_info_changed(struct ieee80211_hw *hw,
 		}
 
 		/* update the rate set */
-		LOCK(wl);
-		brcms_c_ioctl(wl->wlc, BRCM_SET_RATESET, &rs, sizeof(rs), NULL);
-		UNLOCK(wl);
+		spin_lock_bh(&wl->lock);
+		error = brcms_c_set_rateset(wl->wlc, &rs);
+		spin_unlock_bh(&wl->lock);
+		if (error)
+			wiphy_err(wiphy, "changing basic rates failed: %d\n",
+				  error);
 	}
 	if (changed & BSS_CHANGED_BEACON_INT) {
 		/* Beacon interval changed */
-		LOCK(wl);
-		brcms_c_set(wl->wlc, BRCM_SET_BCNPRD, info->beacon_int);
-		UNLOCK(wl);
+		spin_lock_bh(&wl->lock);
+		brcms_c_set_beacon_period(wl->wlc, info->beacon_int);
+		spin_unlock_bh(&wl->lock);
 	}
 	if (changed & BSS_CHANGED_BSSID) {
 		/* BSSID changed, for whatever reason (IBSS and managed mode) */
-		LOCK(wl);
-		brcms_c_set_addrmatch(wl->wlc, RCM_BSSID_OFFSET,
-				  info->bssid);
-		UNLOCK(wl);
+		spin_lock_bh(&wl->lock);
+		brcms_c_set_addrmatch(wl->wlc, RCM_BSSID_OFFSET, info->bssid);
+		spin_unlock_bh(&wl->lock);
 	}
-	if (changed & BSS_CHANGED_BEACON) {
+	if (changed & BSS_CHANGED_BEACON)
 		/* Beacon data changed, retrieve new beacon (beaconing modes) */
 		wiphy_err(wiphy, "%s: beacon changed\n", __func__);
-	}
+
 	if (changed & BSS_CHANGED_BEACON_ENABLED) {
 		/* Beaconing should be enabled/disabled (beaconing modes) */
 		wiphy_err(wiphy, "%s: Beacon enabled: %s\n", __func__,
 			  info->enable_beacon ? "true" : "false");
 	}
+
 	if (changed & BSS_CHANGED_CQM) {
 		/* Connection quality monitor config changed */
 		wiphy_err(wiphy, "%s: cqm change: threshold %d, hys %d "
 			  " (implement)\n", __func__, info->cqm_rssi_thold,
 			  info->cqm_rssi_hyst);
 	}
+
 	if (changed & BSS_CHANGED_IBSS) {
 		/* IBSS join status changed */
 		wiphy_err(wiphy, "%s: IBSS joined: %s (implement)\n", __func__,
 			  info->ibss_joined ? "true" : "false");
 	}
+
 	if (changed & BSS_CHANGED_ARP_FILTER) {
 		/* Hardware ARP filter address list or state changed */
 		wiphy_err(wiphy, "%s: arp filtering: enabled %s, count %d"
 			  " (implement)\n", __func__, info->arp_filter_enabled ?
 			  "true" : "false", info->arp_addr_cnt);
 	}
+
 	if (changed & BSS_CHANGED_QOS) {
 		/*
 		 * QoS for this association was enabled/disabled.
@@ -479,7 +551,7 @@ brcms_ops_configure_filter(struct ieee80211_hw *hw,
 	if (changed_flags & FIF_OTHER_BSS)
 		wiphy_err(wiphy, "FIF_OTHER_BSS\n");
 	if (changed_flags & FIF_BCN_PRBRESP_PROMISC) {
-		LOCK(wl);
+		spin_lock_bh(&wl->lock);
 		if (*total_flags & FIF_BCN_PRBRESP_PROMISC) {
 			wl->pub->mac80211_state |= MAC80211_PROMISC_BCNS;
 			brcms_c_mac_bcn_promisc_change(wl->wlc, 1);
@@ -487,87 +559,39 @@ brcms_ops_configure_filter(struct ieee80211_hw *hw,
 			brcms_c_mac_bcn_promisc_change(wl->wlc, 0);
 			wl->pub->mac80211_state &= ~MAC80211_PROMISC_BCNS;
 		}
-		UNLOCK(wl);
+		spin_unlock_bh(&wl->lock);
 	}
 	return;
 }
 
-static int
-brcms_ops_set_tim(struct ieee80211_hw *hw, struct ieee80211_sta *sta, bool set)
-{
-	return 0;
-}
-
 static void brcms_ops_sw_scan_start(struct ieee80211_hw *hw)
 {
 	struct brcms_info *wl = hw->priv;
-	LOCK(wl);
+	spin_lock_bh(&wl->lock);
 	brcms_c_scan_start(wl->wlc);
-	UNLOCK(wl);
+	spin_unlock_bh(&wl->lock);
 	return;
 }
 
 static void brcms_ops_sw_scan_complete(struct ieee80211_hw *hw)
 {
 	struct brcms_info *wl = hw->priv;
-	LOCK(wl);
+	spin_lock_bh(&wl->lock);
 	brcms_c_scan_stop(wl->wlc);
-	UNLOCK(wl);
-	return;
-}
-
-static void brcms_ops_set_tsf(struct ieee80211_hw *hw, u64 tsf)
-{
-	wiphy_err(hw->wiphy, "%s: Enter\n", __func__);
+	spin_unlock_bh(&wl->lock);
 	return;
 }
 
 static int
-brcms_ops_get_stats(struct ieee80211_hw *hw,
-		 struct ieee80211_low_level_stats *stats)
+brcms_ops_conf_tx(struct ieee80211_hw *hw, struct ieee80211_vif *vif, u16 queue,
+		  const struct ieee80211_tx_queue_params *params)
 {
 	struct brcms_info *wl = hw->priv;
-	struct wl_cnt *cnt;
-
-	LOCK(wl);
-	cnt = wl->pub->_cnt;
-	stats->dot11ACKFailureCount = 0;
-	stats->dot11RTSFailureCount = 0;
-	stats->dot11FCSErrorCount = 0;
-	stats->dot11RTSSuccessCount = 0;
-	UNLOCK(wl);
-	return 0;
-}
 
-static void
-brcms_ops_sta_notify(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-		  enum sta_notify_cmd cmd, struct ieee80211_sta *sta)
-{
-	switch (cmd) {
-	default:
-		wiphy_err(hw->wiphy, "%s: Unknown cmd = %d\n", __func__,
-			  cmd);
-		break;
-	}
-	return;
-}
-
-static int
-brcms_ops_conf_tx(struct ieee80211_hw *hw, u16 queue,
-	       const struct ieee80211_tx_queue_params *params)
-{
-	struct brcms_info *wl = hw->priv;
-
-	LOCK(wl);
+	spin_lock_bh(&wl->lock);
 	brcms_c_wme_setparams(wl->wlc, queue, params, true);
-	UNLOCK(wl);
-
-	return 0;
-}
+	spin_unlock_bh(&wl->lock);
 
-static u64 brcms_ops_get_tsf(struct ieee80211_hw *hw)
-{
-	wiphy_err(hw->wiphy, "%s: Enter\n", __func__);
 	return 0;
 }
 
@@ -575,25 +599,14 @@ static int
 brcms_ops_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	       struct ieee80211_sta *sta)
 {
-	struct scb *scb;
-
-	int i;
 	struct brcms_info *wl = hw->priv;
+	struct scb *scb = &wl->wlc->pri_scb;
 
-	/* Init the scb */
-	scb = (struct scb *)sta->drv_priv;
-	memset(scb, 0, sizeof(struct scb));
-	for (i = 0; i < NUMPRIO; i++)
-		scb->seqctl[i] = 0xFFFF;
-	scb->seqctl_nonqos = 0xFFFF;
-	scb->magic = SCB_MAGIC;
+	brcms_c_init_scb(scb);
 
-	wl->pub->global_scb = scb;
 	wl->pub->global_ampdu = &(scb->scb_ampdu);
 	wl->pub->global_ampdu->scb = scb;
 	wl->pub->global_ampdu->max_pdu = 16;
-	brcmu_pktq_init(&scb->scb_ampdu.txq, AMPDU_MAX_SCB_TID,
-		  AMPDU_MAX_SCB_TID * PKTQ_LEN_DEFAULT);
 
 	sta->ht_cap.ht_supported = true;
 	sta->ht_cap.ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
@@ -602,14 +615,10 @@ brcms_ops_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	    IEEE80211_HT_CAP_SGI_20 |
 	    IEEE80211_HT_CAP_SGI_40 | IEEE80211_HT_CAP_40MHZ_INTOLERANT;
 
-	/* minstrel_ht initiates addBA on our behalf by calling ieee80211_start_tx_ba_session() */
-	return 0;
-}
-
-static int
-brcms_ops_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-		  struct ieee80211_sta *sta)
-{
+	/*
+	 * minstrel_ht initiates addBA on our behalf by calling
+	 * ieee80211_start_tx_ba_session()
+	 */
 	return 0;
 }
 
@@ -620,8 +629,8 @@ brcms_ops_ampdu_action(struct ieee80211_hw *hw,
 		    struct ieee80211_sta *sta, u16 tid, u16 *ssn,
 		    u8 buf_size)
 {
-	struct scb *scb = (struct scb *)sta->drv_priv;
 	struct brcms_info *wl = hw->priv;
+	struct scb *scb = &wl->wlc->pri_scb;
 	int status;
 
 	if (WARN_ON(scb->magic != SCB_MAGIC))
@@ -632,23 +641,21 @@ brcms_ops_ampdu_action(struct ieee80211_hw *hw,
 	case IEEE80211_AMPDU_RX_STOP:
 		break;
 	case IEEE80211_AMPDU_TX_START:
-		LOCK(wl);
+		spin_lock_bh(&wl->lock);
 		status = brcms_c_aggregatable(wl->wlc, tid);
-		UNLOCK(wl);
+		spin_unlock_bh(&wl->lock);
 		if (!status) {
 			wiphy_err(wl->wiphy, "START: tid %d is not agg\'able\n",
 				  tid);
 			return -EINVAL;
 		}
-		/* Future improvement: Use the starting sequence number provided ... */
-		*ssn = 0;
 		ieee80211_start_tx_ba_cb_irqsafe(vif, sta->addr, tid);
 		break;
 
 	case IEEE80211_AMPDU_TX_STOP:
-		LOCK(wl);
+		spin_lock_bh(&wl->lock);
 		brcms_c_ampdu_flush(wl->wlc, sta, tid);
-		UNLOCK(wl);
+		spin_unlock_bh(&wl->lock);
 		ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
 		break;
 	case IEEE80211_AMPDU_TX_OPERATIONAL:
@@ -658,11 +665,11 @@ brcms_ops_ampdu_action(struct ieee80211_hw *hw,
 		 * recipient and traffic class. 'ampdu_factor' gives maximum
 		 * AMPDU size.
 		 */
-		LOCK(wl);
+		spin_lock_bh(&wl->lock);
 		brcms_c_ampdu_tx_operational(wl->wlc, tid, buf_size,
 			(1 << (IEEE80211_HT_MAX_AMPDU_FACTOR +
 			 sta->ht_cap.ampdu_factor)) - 1);
-		UNLOCK(wl);
+		spin_unlock_bh(&wl->lock);
 		/* Power save wakeup */
 		break;
 	default:
@@ -675,26 +682,26 @@ brcms_ops_ampdu_action(struct ieee80211_hw *hw,
 
 static void brcms_ops_rfkill_poll(struct ieee80211_hw *hw)
 {
-	struct brcms_info *wl = HW_TO_WL(hw);
+	struct brcms_info *wl = hw->priv;
 	bool blocked;
 
-	LOCK(wl);
+	spin_lock_bh(&wl->lock);
 	blocked = brcms_c_check_radio_disabled(wl->wlc);
-	UNLOCK(wl);
+	spin_unlock_bh(&wl->lock);
 
 	wiphy_rfkill_set_hw_state(wl->pub->ieee_hw->wiphy, blocked);
 }
 
 static void brcms_ops_flush(struct ieee80211_hw *hw, bool drop)
 {
-	struct brcms_info *wl = HW_TO_WL(hw);
+	struct brcms_info *wl = hw->priv;
 
 	no_printk("%s: drop = %s\n", __func__, drop ? "true" : "false");
 
 	/* wait for packet queue and dma fifos to run empty */
-	LOCK(wl);
+	spin_lock_bh(&wl->lock);
 	brcms_c_wait_for_tx_completion(wl->wlc, drop);
-	UNLOCK(wl);
+	spin_unlock_bh(&wl->lock);
 }
 
 static const struct ieee80211_ops brcms_ops = {
@@ -706,16 +713,10 @@ static const struct ieee80211_ops brcms_ops = {
 	.config = brcms_ops_config,
 	.bss_info_changed = brcms_ops_bss_info_changed,
 	.configure_filter = brcms_ops_configure_filter,
-	.set_tim = brcms_ops_set_tim,
 	.sw_scan_start = brcms_ops_sw_scan_start,
 	.sw_scan_complete = brcms_ops_sw_scan_complete,
-	.set_tsf = brcms_ops_set_tsf,
-	.get_stats = brcms_ops_get_stats,
-	.sta_notify = brcms_ops_sta_notify,
 	.conf_tx = brcms_ops_conf_tx,
-	.get_tsf = brcms_ops_get_tsf,
 	.sta_add = brcms_ops_sta_add,
-	.sta_remove = brcms_ops_sta_remove,
 	.ampdu_action = brcms_ops_ampdu_action,
 	.rfkill_poll = brcms_ops_rfkill_poll,
 	.flush = brcms_ops_flush,
@@ -729,6 +730,284 @@ static int brcms_set_hint(struct brcms_info *wl, char *abbrev)
 	return regulatory_hint(wl->pub->ieee_hw->wiphy, abbrev);
 }
 
+void brcms_dpc(unsigned long data)
+{
+	struct brcms_info *wl;
+
+	wl = (struct brcms_info *) data;
+
+	spin_lock_bh(&wl->lock);
+
+	/* call the common second level interrupt handler */
+	if (wl->pub->up) {
+		if (wl->resched) {
+			unsigned long flags;
+
+			spin_lock_irqsave(&wl->isr_lock, flags);
+			brcms_c_intrsupd(wl->wlc);
+			spin_unlock_irqrestore(&wl->isr_lock, flags);
+		}
+
+		wl->resched = brcms_c_dpc(wl->wlc, true);
+	}
+
+	/* brcms_c_dpc() may bring the driver down */
+	if (!wl->pub->up)
+		goto done;
+
+	/* re-schedule dpc */
+	if (wl->resched)
+		tasklet_schedule(&wl->tasklet);
+	else
+		/* re-enable interrupts */
+		brcms_intrson(wl);
+
+ done:
+	spin_unlock_bh(&wl->lock);
+}
+
+/*
+ * Precondition: Since this function is called in brcms_pci_probe() context,
+ * no locking is required.
+ */
+static int brcms_request_fw(struct brcms_info *wl, struct pci_dev *pdev)
+{
+	int status;
+	struct device *device = &pdev->dev;
+	char fw_name[100];
+	int i;
+
+	memset(&wl->fw, 0, sizeof(struct brcms_firmware));
+	for (i = 0; i < MAX_FW_IMAGES; i++) {
+		if (brcms_firmwares[i] == NULL)
+			break;
+		sprintf(fw_name, "%s-%d.fw", brcms_firmwares[i],
+			UCODE_LOADER_API_VER);
+		status = request_firmware(&wl->fw.fw_bin[i], fw_name, device);
+		if (status) {
+			wiphy_err(wl->wiphy, "%s: fail to load firmware %s\n",
+				  KBUILD_MODNAME, fw_name);
+			return status;
+		}
+		sprintf(fw_name, "%s_hdr-%d.fw", brcms_firmwares[i],
+			UCODE_LOADER_API_VER);
+		status = request_firmware(&wl->fw.fw_hdr[i], fw_name, device);
+		if (status) {
+			wiphy_err(wl->wiphy, "%s: fail to load firmware %s\n",
+				  KBUILD_MODNAME, fw_name);
+			return status;
+		}
+		wl->fw.hdr_num_entries[i] =
+		    wl->fw.fw_hdr[i]->size / (sizeof(struct firmware_hdr));
+	}
+	wl->fw.fw_cnt = i;
+	return brcms_ucode_data_init(wl, &wl->ucode);
+}
+
+/*
+ * Precondition: Since this function is called in brcms_pci_probe() context,
+ * no locking is required.
+ */
+static void brcms_release_fw(struct brcms_info *wl)
+{
+	int i;
+	for (i = 0; i < MAX_FW_IMAGES; i++) {
+		release_firmware(wl->fw.fw_bin[i]);
+		release_firmware(wl->fw.fw_hdr[i]);
+	}
+}
+
+/**
+ * This function frees the WL per-device resources.
+ *
+ * This function frees resources owned by the WL device pointed to
+ * by the wl parameter.
+ *
+ * precondition: can both be called locked and unlocked
+ *
+ */
+static void brcms_free(struct brcms_info *wl)
+{
+	struct brcms_timer *t, *next;
+
+	/* free ucode data */
+	if (wl->fw.fw_cnt)
+		brcms_ucode_data_free(&wl->ucode);
+	if (wl->irq)
+		free_irq(wl->irq, wl);
+
+	/* kill dpc */
+	tasklet_kill(&wl->tasklet);
+
+	if (wl->pub)
+		brcms_c_module_unregister(wl->pub, "linux", wl);
+
+	/* free common resources */
+	if (wl->wlc) {
+		brcms_c_detach(wl->wlc);
+		wl->wlc = NULL;
+		wl->pub = NULL;
+	}
+
+	/* virtual interface deletion is deferred so we cannot spinwait */
+
+	/* wait for all pending callbacks to complete */
+	while (atomic_read(&wl->callbacks) > 0)
+		schedule();
+
+	/* free timers */
+	for (t = wl->timers; t; t = next) {
+		next = t->next;
+#ifdef BCMDBG
+		kfree(t->name);
+#endif
+		kfree(t);
+	}
+
+	/*
+	 * unregister_netdev() calls get_stats() which may read chip
+	 * registers so we cannot unmap the chip registers until
+	 * after calling unregister_netdev() .
+	 */
+	if (wl->regsva)
+		iounmap(wl->regsva);
+
+	wl->regsva = NULL;
+}
+
+/*
+* called from both kernel as from this kernel module.
+* precondition: perimeter lock is not acquired.
+*/
+static void brcms_remove(struct pci_dev *pdev)
+{
+	struct brcms_info *wl;
+	struct ieee80211_hw *hw;
+	int status;
+
+	hw = pci_get_drvdata(pdev);
+	wl = hw->priv;
+	if (!wl) {
+		pr_err("wl: brcms_remove: pci_get_drvdata failed\n");
+		return;
+	}
+
+	spin_lock_bh(&wl->lock);
+	status = brcms_c_chipmatch(pdev->vendor, pdev->device);
+	spin_unlock_bh(&wl->lock);
+	if (!status) {
+		wiphy_err(wl->wiphy, "wl: brcms_remove: chipmatch "
+				     "failed\n");
+		return;
+	}
+	if (wl->wlc) {
+		wiphy_rfkill_set_hw_state(wl->pub->ieee_hw->wiphy, false);
+		wiphy_rfkill_stop_polling(wl->pub->ieee_hw->wiphy);
+		ieee80211_unregister_hw(hw);
+		spin_lock_bh(&wl->lock);
+		brcms_down(wl);
+		spin_unlock_bh(&wl->lock);
+	}
+	pci_disable_device(pdev);
+
+	brcms_free(wl);
+
+	pci_set_drvdata(pdev, NULL);
+	ieee80211_free_hw(hw);
+}
+
+static irqreturn_t brcms_isr(int irq, void *dev_id)
+{
+	struct brcms_info *wl;
+	bool ours, wantdpc;
+
+	wl = (struct brcms_info *) dev_id;
+
+	spin_lock(&wl->isr_lock);
+
+	/* call common first level interrupt handler */
+	ours = brcms_c_isr(wl->wlc, &wantdpc);
+	if (ours) {
+		/* if more to do... */
+		if (wantdpc) {
+
+			/* ...and call the second level interrupt handler */
+			/* schedule dpc */
+			tasklet_schedule(&wl->tasklet);
+		}
+	}
+
+	spin_unlock(&wl->isr_lock);
+
+	return IRQ_RETVAL(ours);
+}
+
+/*
+ * is called in brcms_pci_probe() context, therefore no locking required.
+ */
+static int ieee_hw_rate_init(struct ieee80211_hw *hw)
+{
+	struct brcms_info *wl = hw->priv;
+	struct brcms_c_info *wlc = wl->wlc;
+	struct ieee80211_supported_band *band;
+	int has_5g = 0;
+	u16 phy_type;
+
+	hw->wiphy->bands[IEEE80211_BAND_2GHZ] = NULL;
+	hw->wiphy->bands[IEEE80211_BAND_5GHZ] = NULL;
+
+	phy_type = brcms_c_get_phy_type(wl->wlc, 0);
+	if (phy_type == PHY_TYPE_N || phy_type == PHY_TYPE_LCN) {
+		band = &wlc->bandstate[BAND_2G_INDEX]->band;
+		*band = brcms_band_2GHz_nphy_template;
+		if (phy_type == PHY_TYPE_LCN) {
+			/* Single stream */
+			band->ht_cap.mcs.rx_mask[1] = 0;
+			band->ht_cap.mcs.rx_highest = cpu_to_le16(72);
+		}
+		hw->wiphy->bands[IEEE80211_BAND_2GHZ] = band;
+	} else {
+		return -EPERM;
+	}
+
+	/* Assume all bands use the same phy.  True for 11n devices. */
+	if (wl->pub->_nbands > 1) {
+		has_5g++;
+		if (phy_type == PHY_TYPE_N || phy_type == PHY_TYPE_LCN) {
+			band = &wlc->bandstate[BAND_5G_INDEX]->band;
+			*band = brcms_band_5GHz_nphy_template;
+			hw->wiphy->bands[IEEE80211_BAND_5GHZ] = band;
+		} else {
+			return -EPERM;
+		}
+	}
+	return 0;
+}
+
+/*
+ * is called in brcms_pci_probe() context, therefore no locking required.
+ */
+static int ieee_hw_init(struct ieee80211_hw *hw)
+{
+	hw->flags = IEEE80211_HW_SIGNAL_DBM
+	    /* | IEEE80211_HW_CONNECTION_MONITOR  What is this? */
+	    | IEEE80211_HW_REPORTS_TX_ACK_STATUS
+	    | IEEE80211_HW_AMPDU_AGGREGATION;
+
+	hw->extra_tx_headroom = brcms_c_get_header_len();
+	hw->queues = N_TX_QUEUES;
+	hw->max_rates = 2;	/* Primary rate and 1 fallback rate */
+
+	/* channel change time is dependent on chip and band  */
+	hw->channel_change_time = 7 * 1000;
+	hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION);
+
+	hw->rate_control_algorithm = "minstrel_ht";
+
+	hw->sta_data_size = 0;
+	return ieee_hw_rate_init(hw);
+}
+
 /**
  * attach to the WL device.
  *
@@ -744,21 +1023,19 @@ static int brcms_set_hint(struct brcms_info *wl, char *abbrev)
  * is called in brcms_pci_probe() context, therefore no locking required.
  */
 static struct brcms_info *brcms_attach(u16 vendor, u16 device,
-				       unsigned long regs,
-			    uint bustype, void *btparam, uint irq)
+				       resource_size_t regs,
+				       struct pci_dev *btparam, uint irq)
 {
 	struct brcms_info *wl = NULL;
 	int unit, err;
-	unsigned long base_addr;
 	struct ieee80211_hw *hw;
 	u8 perm[ETH_ALEN];
 
 	unit = n_adapters_found;
 	err = 0;
 
-	if (unit < 0) {
+	if (unit < 0)
 		return NULL;
-	}
 
 	/* allocate private info */
 	hw = pci_get_drvdata(btparam);	/* btparam == pdev */
@@ -773,19 +1050,7 @@ static struct brcms_info *brcms_attach(u16 vendor, u16 device,
 	/* setup the bottom half handler */
 	tasklet_init(&wl->tasklet, brcms_dpc, (unsigned long) wl);
 
-
-
-	base_addr = regs;
-
-	if (bustype == PCI_BUS || bustype == RPC_BUS) {
-		/* Do nothing */
-	} else {
-		bustype = PCI_BUS;
-		BCMMSG(wl->wiphy, "force to PCI\n");
-	}
-	wl->bcm_bustype = bustype;
-
-	wl->regsva = ioremap_nocache(base_addr, PCI_BAR0_WINSZ);
+	wl->regsva = ioremap_nocache(regs, PCI_BAR0_WINSZ);
 	if (wl->regsva == NULL) {
 		wiphy_err(wl->wiphy, "wl%d: ioremap() failed\n", unit);
 		goto fail;
@@ -794,17 +1059,17 @@ static struct brcms_info *brcms_attach(u16 vendor, u16 device,
 	spin_lock_init(&wl->isr_lock);
 
 	/* prepare ucode */
-	if (brcms_request_fw(wl, (struct pci_dev *)btparam) < 0) {
+	if (brcms_request_fw(wl, btparam) < 0) {
 		wiphy_err(wl->wiphy, "%s: Failed to find firmware usually in "
 			  "%s\n", KBUILD_MODNAME, "/lib/firmware/brcm");
 		brcms_release_fw(wl);
-		brcms_remove((struct pci_dev *)btparam);
+		brcms_remove(btparam);
 		return NULL;
 	}
 
 	/* common load-time initialization */
-	wl->wlc = brcms_c_attach((void *)wl, vendor, device, unit, false,
-			     wl->regsva, wl->bcm_bustype, btparam, &err);
+	wl->wlc = brcms_c_attach(wl, vendor, device, unit, false,
+				 wl->regsva, btparam, &err);
 	brcms_release_fw(wl);
 	if (!wl->wlc) {
 		wiphy_err(wl->wiphy, "%s: attach() failed with code %d\n",
@@ -815,10 +1080,8 @@ static struct brcms_info *brcms_attach(u16 vendor, u16 device,
 
 	wl->pub->ieee_hw = hw;
 
-	if (brcms_c_set_par(wl->wlc, IOV_MPC, 0) < 0) {
-		wiphy_err(wl->wiphy, "wl%d: Error setting MPC variable to 0\n",
-			  unit);
-	}
+	/* disable mpc */
+	brcms_c_set_radio_mpc(wl->wlc, false);
 
 	/* register our interrupt handler */
 	if (request_irq(irq, brcms_isr, IRQF_SHARED, KBUILD_MODNAME, wl)) {
@@ -828,7 +1091,7 @@ static struct brcms_info *brcms_attach(u16 vendor, u16 device,
 	wl->irq = irq;
 
 	/* register module */
-	brcms_c_module_register(wl->pub, "linux", wl, wl_linux_watchdog, NULL);
+	brcms_c_module_register(wl->pub, "linux", wl, NULL);
 
 	if (ieee_hw_init(hw)) {
 		wiphy_err(wl->wiphy, "wl%d: %s: ieee_hw_init failed!\n", unit,
@@ -842,19 +1105,17 @@ static struct brcms_info *brcms_attach(u16 vendor, u16 device,
 	SET_IEEE80211_PERM_ADDR(hw, perm);
 
 	err = ieee80211_register_hw(hw);
-	if (err) {
+	if (err)
 		wiphy_err(wl->wiphy, "%s: ieee80211_register_hw failed, status"
 			  "%d\n", __func__, err);
-	}
 
 	if (wl->pub->srom_ccode[0])
 		err = brcms_set_hint(wl, wl->pub->srom_ccode);
 	else
 		err = brcms_set_hint(wl, "US");
-	if (err) {
+	if (err)
 		wiphy_err(wl->wiphy, "%s: regulatory_hint failed, status %d\n",
 			  __func__, err);
-	}
 
 	n_adapters_found++;
 	return wl;
@@ -866,236 +1127,6 @@ fail:
 
 
 
-#define CHAN2GHZ(channel, freqency, chflags)  { \
-	.band = IEEE80211_BAND_2GHZ, \
-	.center_freq = (freqency), \
-	.hw_value = (channel), \
-	.flags = chflags, \
-	.max_antenna_gain = 0, \
-	.max_power = 19, \
-}
-
-static struct ieee80211_channel brcms_2ghz_chantable[] = {
-	CHAN2GHZ(1, 2412, IEEE80211_CHAN_NO_HT40MINUS),
-	CHAN2GHZ(2, 2417, IEEE80211_CHAN_NO_HT40MINUS),
-	CHAN2GHZ(3, 2422, IEEE80211_CHAN_NO_HT40MINUS),
-	CHAN2GHZ(4, 2427, IEEE80211_CHAN_NO_HT40MINUS),
-	CHAN2GHZ(5, 2432, 0),
-	CHAN2GHZ(6, 2437, 0),
-	CHAN2GHZ(7, 2442, 0),
-	CHAN2GHZ(8, 2447, IEEE80211_CHAN_NO_HT40PLUS),
-	CHAN2GHZ(9, 2452, IEEE80211_CHAN_NO_HT40PLUS),
-	CHAN2GHZ(10, 2457, IEEE80211_CHAN_NO_HT40PLUS),
-	CHAN2GHZ(11, 2462, IEEE80211_CHAN_NO_HT40PLUS),
-	CHAN2GHZ(12, 2467,
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_NO_HT40PLUS),
-	CHAN2GHZ(13, 2472,
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_NO_HT40PLUS),
-	CHAN2GHZ(14, 2484,
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_NO_HT40PLUS | IEEE80211_CHAN_NO_HT40MINUS)
-};
-
-#define CHAN5GHZ(channel, chflags)  { \
-	.band = IEEE80211_BAND_5GHZ, \
-	.center_freq = 5000 + 5*(channel), \
-	.hw_value = (channel), \
-	.flags = chflags, \
-	.max_antenna_gain = 0, \
-	.max_power = 21, \
-}
-
-static struct ieee80211_channel brcms_5ghz_nphy_chantable[] = {
-	/* UNII-1 */
-	CHAN5GHZ(36, IEEE80211_CHAN_NO_HT40MINUS),
-	CHAN5GHZ(40, IEEE80211_CHAN_NO_HT40PLUS),
-	CHAN5GHZ(44, IEEE80211_CHAN_NO_HT40MINUS),
-	CHAN5GHZ(48, IEEE80211_CHAN_NO_HT40PLUS),
-	/* UNII-2 */
-	CHAN5GHZ(52,
-		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40MINUS),
-	CHAN5GHZ(56,
-		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS),
-	CHAN5GHZ(60,
-		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40MINUS),
-	CHAN5GHZ(64,
-		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS),
-	/* MID */
-	CHAN5GHZ(100,
-		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40MINUS),
-	CHAN5GHZ(104,
-		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS),
-	CHAN5GHZ(108,
-		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40MINUS),
-	CHAN5GHZ(112,
-		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS),
-	CHAN5GHZ(116,
-		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40MINUS),
-	CHAN5GHZ(120,
-		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS),
-	CHAN5GHZ(124,
-		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40MINUS),
-	CHAN5GHZ(128,
-		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS),
-	CHAN5GHZ(132,
-		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40MINUS),
-	CHAN5GHZ(136,
-		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS),
-	CHAN5GHZ(140,
-		 IEEE80211_CHAN_RADAR | IEEE80211_CHAN_NO_IBSS |
-		 IEEE80211_CHAN_PASSIVE_SCAN | IEEE80211_CHAN_NO_HT40PLUS |
-		 IEEE80211_CHAN_NO_HT40MINUS),
-	/* UNII-3 */
-	CHAN5GHZ(149, IEEE80211_CHAN_NO_HT40MINUS),
-	CHAN5GHZ(153, IEEE80211_CHAN_NO_HT40PLUS),
-	CHAN5GHZ(157, IEEE80211_CHAN_NO_HT40MINUS),
-	CHAN5GHZ(161, IEEE80211_CHAN_NO_HT40PLUS),
-	CHAN5GHZ(165, IEEE80211_CHAN_NO_HT40PLUS | IEEE80211_CHAN_NO_HT40MINUS)
-};
-
-#define RATE(rate100m, _flags) { \
-	.bitrate = (rate100m), \
-	.flags = (_flags), \
-	.hw_value = (rate100m / 5), \
-}
-
-static struct ieee80211_rate legacy_ratetable[] = {
-	RATE(10, 0),
-	RATE(20, IEEE80211_RATE_SHORT_PREAMBLE),
-	RATE(55, IEEE80211_RATE_SHORT_PREAMBLE),
-	RATE(110, IEEE80211_RATE_SHORT_PREAMBLE),
-	RATE(60, 0),
-	RATE(90, 0),
-	RATE(120, 0),
-	RATE(180, 0),
-	RATE(240, 0),
-	RATE(360, 0),
-	RATE(480, 0),
-	RATE(540, 0),
-};
-
-static struct ieee80211_supported_band brcms_band_2GHz_nphy = {
-	.band = IEEE80211_BAND_2GHZ,
-	.channels = brcms_2ghz_chantable,
-	.n_channels = ARRAY_SIZE(brcms_2ghz_chantable),
-	.bitrates = legacy_ratetable,
-	.n_bitrates = ARRAY_SIZE(legacy_ratetable),
-	.ht_cap = {
-		   /* from include/linux/ieee80211.h */
-		   .cap = IEEE80211_HT_CAP_GRN_FLD |
-		   IEEE80211_HT_CAP_SGI_20 |
-		   IEEE80211_HT_CAP_SGI_40 | IEEE80211_HT_CAP_40MHZ_INTOLERANT,
-		   .ht_supported = true,
-		   .ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K,
-		   .ampdu_density = AMPDU_DEF_MPDU_DENSITY,
-		   .mcs = {
-			   /* placeholders for now */
-			   .rx_mask = {0xff, 0xff, 0, 0, 0, 0, 0, 0, 0, 0},
-			   .rx_highest = 500,
-			   .tx_params = IEEE80211_HT_MCS_TX_DEFINED}
-		   }
-};
-
-static struct ieee80211_supported_band brcms_band_5GHz_nphy = {
-	.band = IEEE80211_BAND_5GHZ,
-	.channels = brcms_5ghz_nphy_chantable,
-	.n_channels = ARRAY_SIZE(brcms_5ghz_nphy_chantable),
-	.bitrates = legacy_ratetable + 4,
-	.n_bitrates = ARRAY_SIZE(legacy_ratetable) - 4,
-	.ht_cap = {
-		   /* use IEEE80211_HT_CAP_* from include/linux/ieee80211.h */
-		   .cap = IEEE80211_HT_CAP_GRN_FLD | IEEE80211_HT_CAP_SGI_20 | IEEE80211_HT_CAP_SGI_40 | IEEE80211_HT_CAP_40MHZ_INTOLERANT,	/* No 40 mhz yet */
-		   .ht_supported = true,
-		   .ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K,
-		   .ampdu_density = AMPDU_DEF_MPDU_DENSITY,
-		   .mcs = {
-			   /* placeholders for now */
-			   .rx_mask = {0xff, 0xff, 0, 0, 0, 0, 0, 0, 0, 0},
-			   .rx_highest = 500,
-			   .tx_params = IEEE80211_HT_MCS_TX_DEFINED}
-		   }
-};
-
-/*
- * is called in brcms_pci_probe() context, therefore no locking required.
- */
-static int ieee_hw_rate_init(struct ieee80211_hw *hw)
-{
-	struct brcms_info *wl = HW_TO_WL(hw);
-	int has_5g;
-	char phy_list[4];
-
-	has_5g = 0;
-
-	hw->wiphy->bands[IEEE80211_BAND_2GHZ] = NULL;
-	hw->wiphy->bands[IEEE80211_BAND_5GHZ] = NULL;
-
-	if (brcms_c_get(wl->wlc, BRCM_GET_PHYLIST, (int *)&phy_list) < 0)
-		wiphy_err(hw->wiphy, "Phy list failed\n");
-
-	if (phy_list[0] == 'n' || phy_list[0] == 'c') {
-		if (phy_list[0] == 'c') {
-			/* Single stream */
-			brcms_band_2GHz_nphy.ht_cap.mcs.rx_mask[1] = 0;
-			brcms_band_2GHz_nphy.ht_cap.mcs.rx_highest = 72;
-		}
-		hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &brcms_band_2GHz_nphy;
-	} else {
-		return -EPERM;
-	}
-
-	/* Assume all bands use the same phy.  True for 11n devices. */
-	if (NBANDS_PUB(wl->pub) > 1) {
-		has_5g++;
-		if (phy_list[0] == 'n' || phy_list[0] == 'c') {
-			hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
-			    &brcms_band_5GHz_nphy;
-		} else {
-			return -EPERM;
-		}
-	}
-	return 0;
-}
-
-/*
- * is called in brcms_pci_probe() context, therefore no locking required.
- */
-static int ieee_hw_init(struct ieee80211_hw *hw)
-{
-	hw->flags = IEEE80211_HW_SIGNAL_DBM
-	    /* | IEEE80211_HW_CONNECTION_MONITOR  What is this? */
-	    | IEEE80211_HW_REPORTS_TX_ACK_STATUS
-	    | IEEE80211_HW_AMPDU_AGGREGATION;
-
-	hw->extra_tx_headroom = brcms_c_get_header_len();
-	hw->queues = N_TX_QUEUES;
-	hw->max_rates = 2;	/* Primary rate and 1 fallback rate */
-
-	hw->channel_change_time = 7 * 1000;	/* channel change time is dependent on chip and band  */
-	hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION);
-
-	hw->rate_control_algorithm = "minstrel_ht";
-
-	hw->sta_data_size = sizeof(struct scb);
-	return ieee_hw_rate_init(hw);
-}
-
 /**
  * determines if a device is a WL device, and if so, attaches it.
  *
@@ -1149,7 +1180,7 @@ brcms_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	memset(hw->priv, 0, sizeof(*wl));
 
 	wl = brcms_attach(pdev->vendor, pdev->device,
-			  pci_resource_start(pdev, 0), PCI_BUS, pdev,
+			  pci_resource_start(pdev, 0), pdev,
 			  pdev->irq);
 
 	if (!wl) {
@@ -1166,7 +1197,7 @@ static int brcms_suspend(struct pci_dev *pdev, pm_message_t state)
 	struct ieee80211_hw *hw;
 
 	hw = pci_get_drvdata(pdev);
-	wl = HW_TO_WL(hw);
+	wl = hw->priv;
 	if (!wl) {
 		wiphy_err(wl->wiphy,
 			  "brcms_suspend: pci_get_drvdata failed\n");
@@ -1174,9 +1205,9 @@ static int brcms_suspend(struct pci_dev *pdev, pm_message_t state)
 	}
 
 	/* only need to flag hw is down for proper resume */
-	LOCK(wl);
+	spin_lock_bh(&wl->lock);
 	wl->pub->hw_up = false;
-	UNLOCK(wl);
+	spin_unlock_bh(&wl->lock);
 
 	pci_save_state(pdev);
 	pci_disable_device(pdev);
@@ -1191,7 +1222,7 @@ static int brcms_resume(struct pci_dev *pdev)
 	u32 val;
 
 	hw = pci_get_drvdata(pdev);
-	wl = HW_TO_WL(hw);
+	wl = hw->priv;
 	if (!wl) {
 		wiphy_err(wl->wiphy,
 			  "wl: brcms_resume: pci_get_drvdata failed\n");
@@ -1221,47 +1252,6 @@ static int brcms_resume(struct pci_dev *pdev)
 	return err;
 }
 
-/*
-* called from both kernel as from this kernel module.
-* precondition: perimeter lock is not acquired.
-*/
-static void brcms_remove(struct pci_dev *pdev)
-{
-	struct brcms_info *wl;
-	struct ieee80211_hw *hw;
-	int status;
-
-	hw = pci_get_drvdata(pdev);
-	wl = HW_TO_WL(hw);
-	if (!wl) {
-		pr_err("wl: brcms_remove: pci_get_drvdata failed\n");
-		return;
-	}
-
-	LOCK(wl);
-	status = brcms_c_chipmatch(pdev->vendor, pdev->device);
-	UNLOCK(wl);
-	if (!status) {
-		wiphy_err(wl->wiphy, "wl: brcms_remove: chipmatch "
-				     "failed\n");
-		return;
-	}
-	if (wl->wlc) {
-		wiphy_rfkill_set_hw_state(wl->pub->ieee_hw->wiphy, false);
-		wiphy_rfkill_stop_polling(wl->pub->ieee_hw->wiphy);
-		ieee80211_unregister_hw(hw);
-		LOCK(wl);
-		brcms_down(wl);
-		UNLOCK(wl);
-	}
-	pci_disable_device(pdev);
-
-	brcms_free(wl);
-
-	pci_set_drvdata(pdev, NULL);
-	ieee80211_free_hw(hw);
-}
-
 static struct pci_driver brcms_pci_driver = {
 	.name     = KBUILD_MODNAME,
 	.probe    = brcms_pci_probe,
@@ -1285,8 +1275,6 @@ static int __init brcms_module_init(void)
 #ifdef BCMDBG
 	if (msglevel != 0xdeadbeef)
 		brcm_msg_level = msglevel;
-	if (phymsglevel != 0xdeadbeef)
-		phyhal_msg_level = phymsglevel;
 #endif				/* BCMDBG */
 
 	error = pci_register_driver(&brcms_pci_driver);
@@ -1314,82 +1302,6 @@ static void __exit brcms_module_exit(void)
 module_init(brcms_module_init);
 module_exit(brcms_module_exit);
 
-/**
- * This function frees the WL per-device resources.
- *
- * This function frees resources owned by the WL device pointed to
- * by the wl parameter.
- *
- * precondition: can both be called locked and unlocked
- *
- */
-static void brcms_free(struct brcms_info *wl)
-{
-	struct brcms_timer *t, *next;
-
-	/* free ucode data */
-	if (wl->fw.fw_cnt)
-		brcms_ucode_data_free();
-	if (wl->irq)
-		free_irq(wl->irq, wl);
-
-	/* kill dpc */
-	tasklet_kill(&wl->tasklet);
-
-	if (wl->pub) {
-		brcms_c_module_unregister(wl->pub, "linux", wl);
-	}
-
-	/* free common resources */
-	if (wl->wlc) {
-		brcms_c_detach(wl->wlc);
-		wl->wlc = NULL;
-		wl->pub = NULL;
-	}
-
-	/* virtual interface deletion is deferred so we cannot spinwait */
-
-	/* wait for all pending callbacks to complete */
-	while (atomic_read(&wl->callbacks) > 0)
-		schedule();
-
-	/* free timers */
-	for (t = wl->timers; t; t = next) {
-		next = t->next;
-#ifdef BCMDBG
-		kfree(t->name);
-#endif
-		kfree(t);
-	}
-
-	/*
-	 * unregister_netdev() calls get_stats() which may read chip registers
-	 * so we cannot unmap the chip registers until after calling unregister_netdev() .
-	 */
-	if (wl->regsva && wl->bcm_bustype != SDIO_BUS &&
-	    wl->bcm_bustype != JTAG_BUS) {
-		iounmap((void *)wl->regsva);
-	}
-	wl->regsva = NULL;
-}
-
-/* flags the given rate in rateset as requested */
-static void brcms_set_basic_rate(struct wl_rateset *rs, u16 rate, bool is_br)
-{
-	u32 i;
-
-	for (i = 0; i < rs->count; i++) {
-		if (rate != (rs->rates[i] & 0x7f))
-			continue;
-
-		if (is_br)
-			rs->rates[i] |= BRCMS_RATE_FLAG;
-		else
-			rs->rates[i] &= BRCMS_RATE_MASK;
-		return;
-	}
-}
-
 /*
  * precondition: perimeter lock has been acquired
  */
@@ -1404,7 +1316,7 @@ void brcms_txflowcontrol(struct brcms_info *wl, struct brcms_if *wlif,
  */
 void brcms_init(struct brcms_info *wl)
 {
-	BCMMSG(WL_TO_HW(wl)->wiphy, "wl%d\n", wl->pub->unit);
+	BCMMSG(wl->pub->ieee_hw->wiphy, "wl%d\n", wl->pub->unit);
 	brcms_reset(wl);
 
 	brcms_c_init(wl->wlc);
@@ -1415,7 +1327,7 @@ void brcms_init(struct brcms_info *wl)
  */
 uint brcms_reset(struct brcms_info *wl)
 {
-	BCMMSG(WL_TO_HW(wl)->wiphy, "wl%d\n", wl->pub->unit);
+	BCMMSG(wl->pub->ieee_hw->wiphy, "wl%d\n", wl->pub->unit);
 	brcms_c_reset(wl->wlc);
 
 	/* dpc will not be rescheduled */
@@ -1432,17 +1344,9 @@ void brcms_intrson(struct brcms_info *wl)
 {
 	unsigned long flags;
 
-	INT_LOCK(wl, flags);
+	spin_lock_irqsave(&wl->isr_lock, flags);
 	brcms_c_intrson(wl->wlc);
-	INT_UNLOCK(wl, flags);
-}
-
-/*
- * precondition: perimeter lock has been acquired
- */
-bool wl_alloc_dma_resources(struct brcms_info *wl, uint addrwidth)
-{
-	return true;
+	spin_unlock_irqrestore(&wl->isr_lock, flags);
 }
 
 u32 brcms_intrsoff(struct brcms_info *wl)
@@ -1450,9 +1354,9 @@ u32 brcms_intrsoff(struct brcms_info *wl)
 	unsigned long flags;
 	u32 status;
 
-	INT_LOCK(wl, flags);
+	spin_lock_irqsave(&wl->isr_lock, flags);
 	status = brcms_c_intrsoff(wl->wlc);
-	INT_UNLOCK(wl, flags);
+	spin_unlock_irqrestore(&wl->isr_lock, flags);
 	return status;
 }
 
@@ -1460,9 +1364,9 @@ void brcms_intrsrestore(struct brcms_info *wl, u32 macintmask)
 {
 	unsigned long flags;
 
-	INT_LOCK(wl, flags);
+	spin_lock_irqsave(&wl->isr_lock, flags);
 	brcms_c_intrsrestore(wl->wlc, macintmask);
-	INT_UNLOCK(wl, flags);
+	spin_unlock_irqrestore(&wl->isr_lock, flags);
 }
 
 /*
@@ -1492,110 +1396,42 @@ void brcms_down(struct brcms_info *wl)
 	callbacks = atomic_read(&wl->callbacks) - ret_val;
 
 	/* wait for down callbacks to complete */
-	UNLOCK(wl);
+	spin_unlock_bh(&wl->lock);
 
 	/* For HIGH_only driver, it's important to actually schedule other work,
 	 * not just spin wait since everything runs at schedule level
 	 */
 	SPINWAIT((atomic_read(&wl->callbacks) > callbacks), 100 * 1000);
 
-	LOCK(wl);
-}
-
-static irqreturn_t brcms_isr(int irq, void *dev_id)
-{
-	struct brcms_info *wl;
-	bool ours, wantdpc;
-	unsigned long flags;
-
-	wl = (struct brcms_info *) dev_id;
-
-	ISR_LOCK(wl, flags);
-
-	/* call common first level interrupt handler */
-	ours = brcms_c_isr(wl->wlc, &wantdpc);
-	if (ours) {
-		/* if more to do... */
-		if (wantdpc) {
-
-			/* ...and call the second level interrupt handler */
-			/* schedule dpc */
-			tasklet_schedule(&wl->tasklet);
-		}
-	}
-
-	ISR_UNLOCK(wl, flags);
-
-	return IRQ_RETVAL(ours);
-}
-
-static void brcms_dpc(unsigned long data)
-{
-	struct brcms_info *wl;
-
-	wl = (struct brcms_info *) data;
-
-	LOCK(wl);
-
-	/* call the common second level interrupt handler */
-	if (wl->pub->up) {
-		if (wl->resched) {
-			unsigned long flags;
-
-			INT_LOCK(wl, flags);
-			brcms_c_intrsupd(wl->wlc);
-			INT_UNLOCK(wl, flags);
-		}
-
-		wl->resched = brcms_c_dpc(wl->wlc, true);
-	}
-
-	/* brcms_c_dpc() may bring the driver down */
-	if (!wl->pub->up)
-		goto done;
-
-	/* re-schedule dpc */
-	if (wl->resched)
-		tasklet_schedule(&wl->tasklet);
-	else {
-		/* re-enable interrupts */
-		brcms_intrson(wl);
-	}
-
- done:
-	UNLOCK(wl);
-}
-
-/*
- * is called by the kernel from software irq context
- */
-static void brcms_timer(unsigned long data)
-{
-	_brcms_timer((struct brcms_timer *) data);
+	spin_lock_bh(&wl->lock);
 }
 
 /*
 * precondition: perimeter lock is not acquired
  */
-static void _brcms_timer(struct brcms_timer *t)
+static void _brcms_timer(struct work_struct *work)
 {
-	LOCK(t->wl);
+	struct brcms_timer *t = container_of(work, struct brcms_timer,
+					     dly_wrk.work);
+
+	spin_lock_bh(&t->wl->lock);
 
 	if (t->set) {
 		if (t->periodic) {
-			t->timer.expires = jiffies + t->ms * HZ / 1000;
 			atomic_inc(&t->wl->callbacks);
-			add_timer(&t->timer);
-			t->set = true;
-		} else
+			ieee80211_queue_delayed_work(t->wl->pub->ieee_hw,
+						     &t->dly_wrk,
+						     msecs_to_jiffies(t->ms));
+		} else {
 			t->set = false;
+		}
 
 		t->fn(t->arg);
 	}
 
 	atomic_dec(&t->wl->callbacks);
 
-	UNLOCK(t->wl);
+	spin_unlock_bh(&t->wl->lock);
 }
 
 /*
@@ -1611,15 +1447,10 @@ struct brcms_timer *brcms_init_timer(struct brcms_info *wl,
 	struct brcms_timer *t;
 
 	t = kzalloc(sizeof(struct brcms_timer), GFP_ATOMIC);
-	if (!t) {
-		wiphy_err(wl->wiphy, "wl%d: brcms_init_timer: out of memory\n",
-			  wl->pub->unit);
-		return 0;
-	}
+	if (!t)
+		return NULL;
 
-	init_timer(&t->timer);
-	t->timer.data = (unsigned long) t;
-	t->timer.function = brcms_timer;
+	INIT_DELAYED_WORK(&t->dly_wrk, _brcms_timer);
 	t->wl = wl;
 	t->fn = fn;
 	t->arg = arg;
@@ -1635,27 +1466,28 @@ struct brcms_timer *brcms_init_timer(struct brcms_info *wl,
 	return t;
 }
 
-/* BMAC_NOTE: Add timer adds only the kernel timer since it's going to be more accurate
+/*
+ * adds only the kernel timer since it's going to be more accurate
  * as well as it's easier to make it periodic
  *
  * precondition: perimeter lock has been acquired
  */
-void brcms_add_timer(struct brcms_info *wl, struct brcms_timer *t, uint ms,
-		     int periodic)
+void brcms_add_timer(struct brcms_timer *t, uint ms, int periodic)
 {
+	struct ieee80211_hw *hw = t->wl->pub->ieee_hw;
+
 #ifdef BCMDBG
-	if (t->set) {
-		wiphy_err(wl->wiphy, "%s: Already set. Name: %s, per %d\n",
+	if (t->set)
+		wiphy_err(hw->wiphy, "%s: Already set. Name: %s, per %d\n",
 			  __func__, t->name, periodic);
-	}
 #endif
 	t->ms = ms;
 	t->periodic = (bool) periodic;
 	t->set = true;
-	t->timer.expires = jiffies + ms * HZ / 1000;
 
-	atomic_inc(&wl->callbacks);
-	add_timer(&t->timer);
+	atomic_inc(&t->wl->callbacks);
+
+	ieee80211_queue_delayed_work(hw, &t->dly_wrk, msecs_to_jiffies(ms));
 }
 
 /*
@@ -1663,14 +1495,14 @@ void brcms_add_timer(struct brcms_info *wl, struct brcms_timer *t, uint ms,
  *
  * precondition: perimeter lock has been acquired
  */
-bool brcms_del_timer(struct brcms_info *wl, struct brcms_timer *t)
+bool brcms_del_timer(struct brcms_timer *t)
 {
 	if (t->set) {
 		t->set = false;
-		if (!del_timer(&t->timer)) {
+		if (!cancel_delayed_work(&t->dly_wrk))
 			return false;
-		}
-		atomic_dec(&wl->callbacks);
+
+		atomic_dec(&t->wl->callbacks);
 	}
 
 	return true;
@@ -1679,12 +1511,13 @@ bool brcms_del_timer(struct brcms_info *wl, struct brcms_timer *t)
 /*
  * precondition: perimeter lock has been acquired
  */
-void brcms_free_timer(struct brcms_info *wl, struct brcms_timer *t)
+void brcms_free_timer(struct brcms_timer *t)
 {
+	struct brcms_info *wl = t->wl;
 	struct brcms_timer *tmp;
 
 	/* delete the timer in case it is active */
-	brcms_del_timer(wl, t);
+	brcms_del_timer(t);
 
 	if (wl->timers == t) {
 		wl->timers = wl->timers->next;
@@ -1712,27 +1545,6 @@ void brcms_free_timer(struct brcms_info *wl, struct brcms_timer *t)
 }
 
 /*
- * runs in software irq context
- *
- * precondition: perimeter lock is not acquired
- */
-static int wl_linux_watchdog(void *ctx)
-{
-	return 0;
-}
-
-struct firmware_hdr {
-	u32 offset;
-	u32 len;
-	u32 idx;
-};
-
-char *brcms_firmwares[MAX_FW_IMAGES] = {
-	"brcm/bcm43xx",
-	NULL
-};
-
-/*
  * precondition: perimeter lock has been acquired
  */
 int brcms_ucode_init_buf(struct brcms_info *wl, void **pbuf, u32 idx)
@@ -1744,15 +1556,15 @@ int brcms_ucode_init_buf(struct brcms_info *wl, void **pbuf, u32 idx)
 		hdr = (struct firmware_hdr *)wl->fw.fw_hdr[i]->data;
 		for (entry = 0; entry < wl->fw.hdr_num_entries[i];
 		     entry++, hdr++) {
-			if (hdr->idx == idx) {
-				pdata = wl->fw.fw_bin[i]->data + hdr->offset;
-				*pbuf = kmalloc(hdr->len, GFP_ATOMIC);
-				if (*pbuf == NULL) {
-					wiphy_err(wl->wiphy, "fail to alloc %d"
-						  " bytes\n", hdr->len);
+			u32 len = le32_to_cpu(hdr->len);
+			if (le32_to_cpu(hdr->idx) == idx) {
+				pdata = wl->fw.fw_bin[i]->data +
+					le32_to_cpu(hdr->offset);
+				*pbuf = kmalloc(len, GFP_ATOMIC);
+				if (*pbuf == NULL)
 					goto fail;
-				}
-				memcpy(*pbuf, pdata, hdr->len);
+
+				memcpy(*pbuf, pdata, len);
 				return 0;
 			}
 		}
@@ -1768,7 +1580,7 @@ fail:
  * Precondition: Since this function is called in brcms_pci_probe() context,
  * no locking is required.
  */
-int brcms_ucode_init_uint(struct brcms_info *wl, u32 *data, u32 idx)
+int brcms_ucode_init_uint(struct brcms_info *wl, size_t *n_bytes, u32 idx)
 {
 	int i, entry;
 	const u8 *pdata;
@@ -1777,14 +1589,15 @@ int brcms_ucode_init_uint(struct brcms_info *wl, u32 *data, u32 idx)
 		hdr = (struct firmware_hdr *)wl->fw.fw_hdr[i]->data;
 		for (entry = 0; entry < wl->fw.hdr_num_entries[i];
 		     entry++, hdr++) {
-			if (hdr->idx == idx) {
-				pdata = wl->fw.fw_bin[i]->data + hdr->offset;
-				if (hdr->len != 4) {
+			if (le32_to_cpu(hdr->idx) == idx) {
+				pdata = wl->fw.fw_bin[i]->data +
+					le32_to_cpu(hdr->offset);
+				if (le32_to_cpu(hdr->len) != 4) {
 					wiphy_err(wl->wiphy,
 						  "ERROR: fw hdr len\n");
 					return -ENOMSG;
 				}
-				*data = *((u32 *) pdata);
+				*n_bytes = le32_to_cpu(*((__le32 *) pdata));
 				return 0;
 			}
 		}
@@ -1794,44 +1607,6 @@ int brcms_ucode_init_uint(struct brcms_info *wl, u32 *data, u32 idx)
 }
 
 /*
- * Precondition: Since this function is called in brcms_pci_probe() context,
- * no locking is required.
- */
-static int brcms_request_fw(struct brcms_info *wl, struct pci_dev *pdev)
-{
-	int status;
-	struct device *device = &pdev->dev;
-	char fw_name[100];
-	int i;
-
-	memset((void *)&wl->fw, 0, sizeof(struct brcms_firmware));
-	for (i = 0; i < MAX_FW_IMAGES; i++) {
-		if (brcms_firmwares[i] == NULL)
-			break;
-		sprintf(fw_name, "%s-%d.fw", brcms_firmwares[i],
-			UCODE_LOADER_API_VER);
-		status = request_firmware(&wl->fw.fw_bin[i], fw_name, device);
-		if (status) {
-			wiphy_err(wl->wiphy, "%s: fail to load firmware %s\n",
-				  KBUILD_MODNAME, fw_name);
-			return status;
-		}
-		sprintf(fw_name, "%s_hdr-%d.fw", brcms_firmwares[i],
-			UCODE_LOADER_API_VER);
-		status = request_firmware(&wl->fw.fw_hdr[i], fw_name, device);
-		if (status) {
-			wiphy_err(wl->wiphy, "%s: fail to load firmware %s\n",
-				  KBUILD_MODNAME, fw_name);
-			return status;
-		}
-		wl->fw.hdr_num_entries[i] =
-		    wl->fw.fw_hdr[i]->size / (sizeof(struct firmware_hdr));
-	}
-	wl->fw.fw_cnt = i;
-	return brcms_ucode_data_init(wl);
-}
-
-/*
  * precondition: can both be called locked and unlocked
  */
 void brcms_ucode_free_buf(void *p)
@@ -1840,20 +1615,6 @@ void brcms_ucode_free_buf(void *p)
 }
 
 /*
- * Precondition: Since this function is called in brcms_pci_probe() context,
- * no locking is required.
- */
-static void brcms_release_fw(struct brcms_info *wl)
-{
-	int i;
-	for (i = 0; i < MAX_FW_IMAGES; i++) {
-		release_firmware(wl->fw.fw_bin[i]);
-		release_firmware(wl->fw.fw_hdr[i]);
-	}
-}
-
-
-/*
  * checks validity of all firmware images loaded from user space
  *
  * Precondition: Since this function is called in brcms_pci_probe() context,
@@ -1890,7 +1651,8 @@ int brcms_check_firmwares(struct brcms_info *wl)
 			ucode_hdr = (struct firmware_hdr *)fw_hdr->data;
 			for (entry = 0; entry < wl->fw.hdr_num_entries[i] &&
 			     !rc; entry++, ucode_hdr++) {
-				if (ucode_hdr->offset + ucode_hdr->len >
+				if (le32_to_cpu(ucode_hdr->offset) +
+				    le32_to_cpu(ucode_hdr->len) >
 				    fw->size) {
 					wiphy_err(wl->wiphy,
 						  "%s: conflicting bin/hdr\n",
@@ -1915,11 +1677,11 @@ bool brcms_rfkill_set_hw_state(struct brcms_info *wl)
 {
 	bool blocked = brcms_c_check_radio_disabled(wl->wlc);
 
-	UNLOCK(wl);
+	spin_unlock_bh(&wl->lock);
 	wiphy_rfkill_set_hw_state(wl->pub->ieee_hw->wiphy, blocked);
 	if (blocked)
 		wiphy_rfkill_start_polling(wl->pub->ieee_hw->wiphy);
-	LOCK(wl);
+	spin_lock_bh(&wl->lock);
 	return blocked;
 }
 
@@ -1928,7 +1690,7 @@ bool brcms_rfkill_set_hw_state(struct brcms_info *wl)
  */
 void brcms_msleep(struct brcms_info *wl, uint ms)
 {
-	UNLOCK(wl);
+	spin_unlock_bh(&wl->lock);
 	msleep(ms);
-	LOCK(wl);
+	spin_lock_bh(&wl->lock);
 }
diff --git a/drivers/staging/brcm80211/brcmsmac/mac80211_if.h b/drivers/staging/brcm80211/brcmsmac/mac80211_if.h
index 40e3d375ea99..177f0e44e4b6 100644
--- a/drivers/staging/brcm80211/brcmsmac/mac80211_if.h
+++ b/drivers/staging/brcm80211/brcmsmac/mac80211_if.h
@@ -19,24 +19,27 @@
 
 #include <linux/timer.h>
 #include <linux/interrupt.h>
+#include <linux/workqueue.h>
+
+#include "ucode_loader.h"
+/*
+ * Starting index for 5G rates in the
+ * legacy rate table.
+ */
+#define BRCMS_LEGACY_5G_RATE_OFFSET	4
 
 /* softmac ioctl definitions */
 #define BRCMS_SET_SHORTSLOT_OVERRIDE		146
 
-
-/* BMAC Note: High-only driver is no longer working in softirq context as it needs to block and
- * sleep so perimeter lock has to be a semaphore instead of spinlock. This requires timers to be
- * submitted to workqueue instead of being on kernel timer
- */
 struct brcms_timer {
-	struct timer_list timer;
+	struct delayed_work dly_wrk;
 	struct brcms_info *wl;
-	void (*fn) (void *);
-	void *arg;		/* argument to fn */
+	void (*fn) (void *);	/* function called upon expiration */
+	void *arg;		/* fixed argument provided to called function */
 	uint ms;
 	bool periodic;
-	bool set;
-	struct brcms_timer *next;
+	bool set;		/* indicates if timer is active */
+	struct brcms_timer *next;	/* for freeing on unload */
 #ifdef BCMDBG
 	char *name;		/* Description of the timer */
 #endif
@@ -57,7 +60,7 @@ struct brcms_firmware {
 
 struct brcms_info {
 	struct brcms_pub *pub;		/* pointer to public wlc state */
-	void *wlc;		/* pointer to private common os-independent data */
+	struct brcms_c_info *wlc;	/* pointer to private common data */
 	u32 magic;
 
 	int irq;
@@ -65,9 +68,8 @@ struct brcms_info {
 	spinlock_t lock;	/* per-device perimeter lock */
 	spinlock_t isr_lock;	/* per-device ISR synchronization lock */
 
-	/* bus type and regsva for unmap in brcms_free() */
-	uint bcm_bustype;	/* bus type */
-	void *regsva;		/* opaque chip registers virtual address */
+	/* regsva for unmap in brcms_free() */
+	void __iomem *regsva;	/* opaque chip registers virtual address */
 
 	/* timer related fields */
 	atomic_t callbacks;	/* # outstanding callback functions */
@@ -75,11 +77,9 @@ struct brcms_info {
 
 	struct tasklet_struct tasklet;	/* dpc tasklet */
 	bool resched;		/* dpc needs to be and is rescheduled */
-#ifdef LINUXSTA_PS
-	u32 pci_psstate[16];	/* pci ps-state save/restore */
-#endif
 	struct brcms_firmware fw;
 	struct wiphy *wiphy;
+	struct brcms_ucode ucode;
 };
 
 /* misc callbacks */
@@ -92,17 +92,17 @@ extern int brcms_up(struct brcms_info *wl);
 extern void brcms_down(struct brcms_info *wl);
 extern void brcms_txflowcontrol(struct brcms_info *wl, struct brcms_if *wlif,
 				bool state, int prio);
-extern bool wl_alloc_dma_resources(struct brcms_info *wl, uint dmaddrwidth);
 extern bool brcms_rfkill_set_hw_state(struct brcms_info *wl);
 
 /* timer functions */
 extern struct brcms_timer *brcms_init_timer(struct brcms_info *wl,
 				      void (*fn) (void *arg), void *arg,
 				      const char *name);
-extern void brcms_free_timer(struct brcms_info *wl, struct brcms_timer *timer);
-extern void brcms_add_timer(struct brcms_info *wl, struct brcms_timer *timer,
-			    uint ms, int periodic);
-extern bool brcms_del_timer(struct brcms_info *wl, struct brcms_timer *timer);
+extern void brcms_free_timer(struct brcms_timer *timer);
+extern void brcms_add_timer(struct brcms_timer *timer, uint ms, int periodic);
+extern bool brcms_del_timer(struct brcms_timer *timer);
 extern void brcms_msleep(struct brcms_info *wl, uint ms);
+extern void brcms_dpc(unsigned long data);
+extern void brcms_timer(struct brcms_timer *t);
 
 #endif				/* _BRCM_MAC80211_IF_H_ */
diff --git a/drivers/staging/brcm80211/brcmsmac/main.c b/drivers/staging/brcm80211/brcmsmac/main.c
index 1763c4535cd2..510e9bb52287 100644
--- a/drivers/staging/brcm80211/brcmsmac/main.c
+++ b/drivers/staging/brcm80211/brcmsmac/main.c
@@ -15,32 +15,23 @@
  */
 
 #include <linux/pci_ids.h>
+#include <linux/if_ether.h>
 #include <net/mac80211.h>
-
 #include <brcm_hw_ids.h>
 #include <aiutils.h>
+#include <chipcommon.h>
 #include "rate.h"
 #include "scb.h"
 #include "phy/phy_hal.h"
 #include "channel.h"
-#include "bmac.h"
 #include "antsel.h"
 #include "stf.h"
 #include "ampdu.h"
-#include "alloc.h"
 #include "mac80211_if.h"
+#include "ucode_loader.h"
 #include "main.h"
 
 /*
- * WPA(2) definitions
- */
-#define RSN_CAP_4_REPLAY_CNTRS		2
-#define RSN_CAP_16_REPLAY_CNTRS		3
-
-#define WPA_CAP_4_REPLAY_CNTRS		RSN_CAP_4_REPLAY_CNTRS
-#define WPA_CAP_16_REPLAY_CNTRS		RSN_CAP_16_REPLAY_CNTRS
-
-/*
  * Indication for txflowcontrol that all priority bits in
  * TXQ_STOP_FOR_PRIOFC_MASK are to be considered.
  */
@@ -51,8 +42,10 @@
  */
 #define SSID_FMT_BUF_LEN	((4 * IEEE80211_MAX_SSID_LEN) + 1)
 
-#define	TIMER_INTERVAL_WATCHDOG	1000	/* watchdog timer, in unit of ms */
-#define	TIMER_INTERVAL_RADIOCHK	800	/* radio monitor timer, in unit of ms */
+/* watchdog timer, in unit of ms */
+#define	TIMER_INTERVAL_WATCHDOG	1000
+/* radio monitor timer, in unit of ms */
+#define	TIMER_INTERVAL_RADIOCHK	800
 
 /* Max MPC timeout, in unit of watchdog */
 #ifndef BRCMS_MPC_MAX_DELAYCNT
@@ -63,20 +56,19 @@
 #define	BRCMS_MPC_MIN_DELAYCNT	1
 #define	BRCMS_MPC_THRESHOLD	3	/* MPC count threshold level */
 
-#define	BEACON_INTERVAL_DEFAULT	100	/* beacon interval, in unit of 1024TU */
-#define	DTIM_INTERVAL_DEFAULT	3	/* DTIM interval, in unit of beacon interval */
+/* beacon interval, in unit of 1024TU */
+#define	BEACON_INTERVAL_DEFAULT	100
+/* DTIM interval, in unit of beacon interval */
+#define	DTIM_INTERVAL_DEFAULT	3
 
 /* Scale down delays to accommodate QT slow speed */
-#define	BEACON_INTERVAL_DEF_QT	20	/* beacon interval, in unit of 1024TU */
-#define	DTIM_INTERVAL_DEF_QT	1	/* DTIM interval, in unit of beacon interval */
+/* beacon interval, in unit of 1024TU */
+#define	BEACON_INTERVAL_DEF_QT	20
+/* DTIM interval, in unit of beacon interval */
+#define	DTIM_INTERVAL_DEF_QT	1
 
 #define	TBTT_ALIGN_LEEWAY_US	100	/* min leeway before first TBTT in us */
 
-/* Software feature flag defines used by wlfeatureflag */
-#define WL_SWFL_NOHWRADIO	0x0004
-#define WL_SWFL_FLOWCONTROL     0x0008	/* Enable backpressure to OS stack */
-#define WL_SWFL_WLBSSSORT	0x0010	/* Per-port supports sorting of BSS */
-
 /* n-mode support capability */
 /* 2x2 includes both 1x1 & 2x2 devices
  * reserved #define 2 for future when we want to separate 1x1 & 2x2 and
@@ -160,62 +152,47 @@
 #define AC_VI			2
 #define AC_VO			3
 
-/*
- * driver maintains internal 'tick'(wlc->pub->now) which increments in 1s OS timer(soft
- * watchdog) it is not a wall clock and won't increment when driver is in "down" state
- * this low resolution driver tick can be used for maintenance tasks such as phy
- * calibration and scb update
- */
+#define	BCN_TMPL_LEN		512	/* length of the BCN template area */
 
-/* To inform the ucode of the last mcast frame posted so that it can clear moredata bit */
-#define BCMCFID(wlc, fid) brcms_b_write_shm((wlc)->hw, M_BCMC_FID, (fid))
+/* brcms_bss_info flag bit values */
+#define BRCMS_BSS_HT		0x0020	/* BSS is HT (MIMO) capable */
 
-#define BRCMS_WAR16165(wlc) (wlc->pub->sih->bustype == PCI_BUS && \
-				(!AP_ENAB(wlc->pub)) && (wlc->war16165))
+/* Flags used in brcms_c_txq_info.stopped */
+/* per prio flow control bits */
+#define TXQ_STOP_FOR_PRIOFC_MASK	0x000000FF
+/* stop txq enqueue for packet drain */
+#define TXQ_STOP_FOR_PKT_DRAIN		0x00000100
+/* stop txq enqueue for ampdu flow control */
+#define TXQ_STOP_FOR_AMPDU_FLOW_CNTRL	0x00000200
 
-/* debug/trace */
-uint brcm_msg_level =
-#if defined(BCMDBG)
-	LOG_ERROR_VAL;
-#else
-	0;
-#endif				/* BCMDBG */
+#define	BRCMS_HWRXOFF		38	/* chip rx buffer offset */
 
 /* Find basic rate for a given rate */
-#define BRCMS_BASIC_RATE(wlc, rspec)	(IS_MCS(rspec) ? \
-			(wlc)->band->basic_rate[mcs_table[rspec & RSPEC_RATE_MASK].leg_ofdm] : \
-			(wlc)->band->basic_rate[rspec & RSPEC_RATE_MASK])
+static u8 brcms_basic_rate(struct brcms_c_info *wlc, u32 rspec)
+{
+	if (is_mcs_rate(rspec))
+		return wlc->band->basic_rate[mcs_table[rspec & RSPEC_RATE_MASK]
+		       .leg_ofdm];
+	return wlc->band->basic_rate[rspec & RSPEC_RATE_MASK];
+}
 
-#define FRAMETYPE(r, mimoframe)	(IS_MCS(r) ? mimoframe	: (IS_CCK(r) ? FT_CCK : FT_OFDM))
+static u16 frametype(u32 rspec, u8 mimoframe)
+{
+	if (is_mcs_rate(rspec))
+		return mimoframe;
+	return is_cck_rate(rspec) ? FT_CCK : FT_OFDM;
+}
 
-#define RFDISABLE_DEFAULT	10000000	/* rfdisable delay timer 500 ms, runs of ALP clock */
+/* rfdisable delay timer 500 ms, runs of ALP clock */
+#define RFDISABLE_DEFAULT	10000000
 
 #define BRCMS_TEMPSENSE_PERIOD		10	/* 10 second timeout */
 
-#define SCAN_IN_PROGRESS(x)	0
-
-#define EPI_VERSION_NUM		0x054b0b00
-
-#ifdef BCMDBG
-/* pointer to most recently allocated wl/wlc */
-static struct brcms_c_info *wlc_info_dbg = (struct brcms_c_info *) (NULL);
-#endif
-
-const u8 prio2fifo[NUMPRIO] = {
-	TX_AC_BE_FIFO,		/* 0    BE      AC_BE   Best Effort */
-	TX_AC_BK_FIFO,		/* 1    BK      AC_BK   Background */
-	TX_AC_BK_FIFO,		/* 2    --      AC_BK   Background */
-	TX_AC_BE_FIFO,		/* 3    EE      AC_BE   Best Effort */
-	TX_AC_VI_FIFO,		/* 4    CL      AC_VI   Video */
-	TX_AC_VI_FIFO,		/* 5    VI      AC_VI   Video */
-	TX_AC_VO_FIFO,		/* 6    VO      AC_VO   Voice */
-	TX_AC_VO_FIFO		/* 7    NC      AC_VO   Voice */
-};
-
 /* precedences numbers for wlc queues. These are twice as may levels as
  * 802.1D priorities.
  * Odd numbers are used for HI priority traffic at same precedence levels
- * These constants are used ONLY by wlc_prio2prec_map.  Do not use them elsewhere.
+ * These constants are used ONLY by wlc_prio2prec_map.  Do not use them
+ * elsewhere.
  */
 #define	_BRCMS_PREC_NONE		0	/* None = - */
 #define	_BRCMS_PREC_BK		2	/* BK - Background */
@@ -226,17 +203,182 @@ const u8 prio2fifo[NUMPRIO] = {
 #define	_BRCMS_PREC_VO		12	/* Vo - Voice */
 #define	_BRCMS_PREC_NC		14	/* NC - Network Control */
 
-#define MAXMACLIST		64	/* max # source MAC matches */
-#define BCN_TEMPLATE_COUNT	2
-
 /* The BSS is generating beacons in HW */
 #define BRCMS_BSSCFG_HW_BCN	0x20
 
-#define HWBCN_ENAB(cfg)		(((cfg)->flags & BRCMS_BSSCFG_HW_BCN) != 0)
+#define	SYNTHPU_DLY_APHY_US	3700	/* a phy synthpu_dly time in us */
+#define	SYNTHPU_DLY_BPHY_US	1050	/* b/g phy synthpu_dly time in us */
+#define	SYNTHPU_DLY_NPHY_US	2048	/* n phy REV3 synthpu_dly time in us */
+#define	SYNTHPU_DLY_LPPHY_US	300	/* lpphy synthpu_dly time in us */
+
+#define	SYNTHPU_DLY_PHY_US_QT	100	/* QT synthpu_dly time in us */
+
+#define	ANTCNT			10	/* vanilla M_MAX_ANTCNT value */
+
+/* Per-AC retry limit register definitions; uses defs.h bitfield macros */
+#define EDCF_SHORT_S            0
+#define EDCF_SFB_S              4
+#define EDCF_LONG_S             8
+#define EDCF_LFB_S              12
+#define EDCF_SHORT_M            BITFIELD_MASK(4)
+#define EDCF_SFB_M              BITFIELD_MASK(4)
+#define EDCF_LONG_M             BITFIELD_MASK(4)
+#define EDCF_LFB_M              BITFIELD_MASK(4)
+
+#define	RETRY_SHORT_DEF			7	/* Default Short retry Limit */
+#define	RETRY_SHORT_MAX			255	/* Maximum Short retry Limit */
+#define	RETRY_LONG_DEF			4	/* Default Long retry count */
+#define	RETRY_SHORT_FB			3 /* Short count for fallback rate */
+#define	RETRY_LONG_FB			2 /* Long count for fallback rate */
+
+#define	APHY_CWMIN		15
+#define PHY_CWMAX		1023
+
+#define EDCF_AIFSN_MIN               1
+
+#define FRAGNUM_MASK		0xF
+
+#define APHY_SLOT_TIME		9
+#define BPHY_SLOT_TIME		20
+
+#define	WL_SPURAVOID_OFF	0
+#define	WL_SPURAVOID_ON1	1
+#define	WL_SPURAVOID_ON2	2
+
+/* invalid core flags, use the saved coreflags */
+#define BRCMS_USE_COREFLAGS	0xffffffff
+
+/* values for PLCPHdr_override */
+#define BRCMS_PLCP_AUTO	-1
+#define BRCMS_PLCP_SHORT	0
+#define BRCMS_PLCP_LONG	1
+
+/* values for g_protection_override and n_protection_override */
+#define BRCMS_PROTECTION_AUTO		-1
+#define BRCMS_PROTECTION_OFF		0
+#define BRCMS_PROTECTION_ON		1
+#define BRCMS_PROTECTION_MMHDR_ONLY	2
+#define BRCMS_PROTECTION_CTS_ONLY		3
+
+/* values for g_protection_control and n_protection_control */
+#define BRCMS_PROTECTION_CTL_OFF		0
+#define BRCMS_PROTECTION_CTL_LOCAL	1
+#define BRCMS_PROTECTION_CTL_OVERLAP	2
+
+/* values for n_protection */
+#define BRCMS_N_PROTECTION_OFF		0
+#define BRCMS_N_PROTECTION_OPTIONAL	1
+#define BRCMS_N_PROTECTION_20IN40		2
+#define BRCMS_N_PROTECTION_MIXEDMODE	3
+
+/* values for band specific 40MHz capabilities */
+#define BRCMS_N_BW_20ALL			0
+#define BRCMS_N_BW_40ALL			1
+#define BRCMS_N_BW_20IN2G_40IN5G		2
+
+/* bitflags for SGI support (sgi_rx iovar) */
+#define BRCMS_N_SGI_20			0x01
+#define BRCMS_N_SGI_40			0x02
+
+/* defines used by the nrate iovar */
+/* MSC in use,indicates b0-6 holds an mcs */
+#define NRATE_MCS_INUSE	0x00000080
+/* rate/mcs value */
+#define NRATE_RATE_MASK 0x0000007f
+/* stf mode mask: siso, cdd, stbc, sdm */
+#define NRATE_STF_MASK	0x0000ff00
+/* stf mode shift */
+#define NRATE_STF_SHIFT	8
+/* bit indicates override both rate & mode */
+#define NRATE_OVERRIDE	0x80000000
+/* bit indicate to override mcs only */
+#define NRATE_OVERRIDE_MCS_ONLY 0x40000000
+#define NRATE_SGI_MASK  0x00800000	/* sgi mode */
+#define NRATE_SGI_SHIFT 23	/* sgi mode */
+#define NRATE_LDPC_CODING 0x00400000	/* bit indicates adv coding in use */
+#define NRATE_LDPC_SHIFT 22	/* ldpc shift */
+
+#define NRATE_STF_SISO	0	/* stf mode SISO */
+#define NRATE_STF_CDD	1	/* stf mode CDD */
+#define NRATE_STF_STBC	2	/* stf mode STBC */
+#define NRATE_STF_SDM	3	/* stf mode SDM */
+
+#define MAX_DMA_SEGS 4
+
+/* Max # of entries in Tx FIFO based on 4kb page size */
+#define NTXD		256
+/* Max # of entries in Rx FIFO based on 4kb page size */
+#define NRXD		256
+
+/* try to keep this # rbufs posted to the chip */
+#define	NRXBUFPOST	32
+
+/* data msg txq hiwat mark */
+#define BRCMS_DATAHIWAT		50
+
+/* bounded rx loops */
+#define RXBND		8 /* max # frames to process in brcms_c_recv() */
+#define TXSBND		8 /* max # tx status to process in wlc_txstatus() */
+
+/*
+ * 32 SSID chars, max of 4 chars for each SSID char "\xFF", plus NULL.
+ */
+#define SSID_FMT_BUF_LEN	((4 * IEEE80211_MAX_SSID_LEN) + 1)
 
-#define MBSS_BCN_ENAB(cfg)       0
-#define MBSS_PRB_ENAB(cfg)       0
-#define SOFTBCN_ENAB(pub)    (0)
+/* brcmu_format_flags() bit description structure */
+struct brcms_c_bit_desc {
+	u32 bit;
+	const char *name;
+};
+
+/*
+ * The following table lists the buffer memory allocated to xmt fifos in HW.
+ * the size is in units of 256bytes(one block), total size is HW dependent
+ * ucode has default fifo partition, sw can overwrite if necessary
+ *
+ * This is documented in twiki under the topic UcodeTxFifo. Please ensure
+ * the twiki is updated before making changes.
+ */
+
+/* Starting corerev for the fifo size table */
+#define XMTFIFOTBL_STARTREV	20
+
+struct d11init {
+	__le16 addr;
+	__le16 size;
+	__le32 value;
+};
+
+struct edcf_acparam {
+	u8 ACI;
+	u8 ECW;
+	u16 TXOP;
+} __packed;
+
+const u8 prio2fifo[NUMPRIO] = {
+	TX_AC_BE_FIFO,		/* 0    BE      AC_BE   Best Effort */
+	TX_AC_BK_FIFO,		/* 1    BK      AC_BK   Background */
+	TX_AC_BK_FIFO,		/* 2    --      AC_BK   Background */
+	TX_AC_BE_FIFO,		/* 3    EE      AC_BE   Best Effort */
+	TX_AC_VI_FIFO,		/* 4    CL      AC_VI   Video */
+	TX_AC_VI_FIFO,		/* 5    VI      AC_VI   Video */
+	TX_AC_VO_FIFO,		/* 6    VO      AC_VO   Voice */
+	TX_AC_VO_FIFO		/* 7    NC      AC_VO   Voice */
+};
+
+/* debug/trace */
+uint brcm_msg_level =
+#if defined(BCMDBG)
+	LOG_ERROR_VAL;
+#else
+	0;
+#endif				/* BCMDBG */
+
+/* TX FIFO number to WME/802.1E Access Category */
+static const u8 wme_fifo2ac[] = { AC_BK, AC_BE, AC_VI, AC_VO, AC_BE, AC_BE };
+
+/* WME/802.1E Access Category to TX FIFO number */
+static const u8 wme_ac2fifo[] = { 1, 0, 2, 3 };
 
 /* 802.1D Priority to precedence queue mapping */
 const u8 wlc_prio2prec_map[] = {
@@ -250,26 +392,25 @@ const u8 wlc_prio2prec_map[] = {
 	_BRCMS_PREC_NC,		/* 7 NC - Network Control */
 };
 
-/* Check if a particular BSS config is AP or STA */
-#define BSSCFG_AP(cfg)		(0)
-#define BSSCFG_STA(cfg)		(1)
-#define BSSCFG_IBSS(cfg)	(!(cfg)->BSS)
-
-/* As above for all non-NULL BSS configs */
-#define FOREACH_BSS(wlc, idx, cfg) \
-	for (idx = 0; (int) idx < BRCMS_MAXBSSCFG; idx++) \
-		if ((cfg = (wlc)->bsscfg[idx]))
-
-/* TX FIFO number to WME/802.1E Access Category */
-const u8 wme_fifo2ac[] = { AC_BK, AC_BE, AC_VI, AC_VO, AC_BE, AC_BE };
-
-/* WME/802.1E Access Category to TX FIFO number */
-static const u8 wme_ac2fifo[] = { 1, 0, 2, 3 };
-
-static bool in_send_q;
+static const u16 xmtfifo_sz[][NFIFO] = {
+	/* corerev 20: 5120, 49152, 49152, 5376, 4352, 1280 */
+	{20, 192, 192, 21, 17, 5},
+	/* corerev 21: 2304, 14848, 5632, 3584, 3584, 1280 */
+	{9, 58, 22, 14, 14, 5},
+	/* corerev 22: 5120, 49152, 49152, 5376, 4352, 1280 */
+	{20, 192, 192, 21, 17, 5},
+	/* corerev 23: 5120, 49152, 49152, 5376, 4352, 1280 */
+	{20, 192, 192, 21, 17, 5},
+	/* corerev 24: 2304, 14848, 5632, 3584, 3584, 1280 */
+	{9, 58, 22, 14, 14, 5},
+};
 
-/* Shared memory location index for various AC params */
-#define wme_shmemacindex(ac)	wme_ac2fifo[ac]
+static const u8 acbitmap2maxprio[] = {
+	PRIO_8021D_BE, PRIO_8021D_BE, PRIO_8021D_BK, PRIO_8021D_BK,
+	PRIO_8021D_VI, PRIO_8021D_VI, PRIO_8021D_VI, PRIO_8021D_VI,
+	PRIO_8021D_VO, PRIO_8021D_VO, PRIO_8021D_VO, PRIO_8021D_VO,
+	PRIO_8021D_VO, PRIO_8021D_VO, PRIO_8021D_VO, PRIO_8021D_VO
+};
 
 #ifdef BCMDBG
 static const char * const fifo_names[] = {
@@ -278,128 +419,2800 @@ static const char * const fifo_names[] = {
 static const char fifo_names[6][0];
 #endif
 
-static const u8 acbitmap2maxprio[] = {
-	PRIO_8021D_BE, PRIO_8021D_BE, PRIO_8021D_BK, PRIO_8021D_BK,
-	PRIO_8021D_VI, PRIO_8021D_VI, PRIO_8021D_VI, PRIO_8021D_VI,
-	PRIO_8021D_VO, PRIO_8021D_VO, PRIO_8021D_VO, PRIO_8021D_VO,
-	PRIO_8021D_VO, PRIO_8021D_VO, PRIO_8021D_VO, PRIO_8021D_VO
-};
+#ifdef BCMDBG
+/* pointer to most recently allocated wl/wlc */
+static struct brcms_c_info *wlc_info_dbg = (struct brcms_c_info *) (NULL);
+#endif
 
 /* currently the best mechanism for determining SIFS is the band in use */
-#define SIFS(band) ((band)->bandtype == BRCM_BAND_5G ? APHY_SIFS_TIME : \
-						       BPHY_SIFS_TIME);
-
-/* local prototypes */
-static u16 brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc,
-					       struct ieee80211_hw *hw,
-					       struct sk_buff *p,
-					       struct scb *scb, uint frag,
-					       uint nfrags, uint queue,
-					       uint next_frag_len,
-					       struct wsec_key *key,
-					       ratespec_t rspec_override);
-static void brcms_c_bss_default_init(struct brcms_c_info *wlc);
-static void brcms_c_ucode_mac_upd(struct brcms_c_info *wlc);
-static ratespec_t mac80211_wlc_set_nrate(struct brcms_c_info *wlc,
-					 struct brcms_band *cur_band,
-					 u32 int_val);
-static void brcms_c_tx_prec_map_init(struct brcms_c_info *wlc);
-static void brcms_c_watchdog(void *arg);
-static void brcms_c_watchdog_by_timer(void *arg);
-static u16 brcms_c_rate_shm_offset(struct brcms_c_info *wlc, u8 rate);
-static int brcms_c_set_rateset(struct brcms_c_info *wlc, wlc_rateset_t *rs_arg);
-static u8 brcms_c_local_constraint_qdbm(struct brcms_c_info *wlc);
-
-/* send and receive */
-static struct brcms_txq_info *brcms_c_txq_alloc(struct brcms_c_info *wlc);
-static void brcms_c_txq_free(struct brcms_c_info *wlc,
-			 struct brcms_txq_info *qi);
-static void brcms_c_txflowcontrol_signal(struct brcms_c_info *wlc,
-				     struct brcms_txq_info *qi,
-				     bool on, int prio);
-static void brcms_c_txflowcontrol_reset(struct brcms_c_info *wlc);
-static void brcms_c_compute_cck_plcp(struct brcms_c_info *wlc, ratespec_t rate,
-				 uint length, u8 *plcp);
-static void brcms_c_compute_ofdm_plcp(ratespec_t rate, uint length, u8 *plcp);
-static void brcms_c_compute_mimo_plcp(ratespec_t rate, uint length, u8 *plcp);
-static u16 brcms_c_compute_frame_dur(struct brcms_c_info *wlc, ratespec_t rate,
-				    u8 preamble_type, uint next_frag_len);
-static u64 brcms_c_recover_tsf64(struct brcms_c_info *wlc,
-			     struct brcms_d11rxhdr *rxh);
-static void brcms_c_recvctl(struct brcms_c_info *wlc,
-			struct d11rxhdr *rxh, struct sk_buff *p);
-static uint brcms_c_calc_frame_len(struct brcms_c_info *wlc, ratespec_t rate,
-			       u8 preamble_type, uint dur);
-static uint brcms_c_calc_ack_time(struct brcms_c_info *wlc, ratespec_t rate,
-			      u8 preamble_type);
-static uint brcms_c_calc_cts_time(struct brcms_c_info *wlc, ratespec_t rate,
-			      u8 preamble_type);
-/* interrupt, up/down, band */
-static void brcms_c_setband(struct brcms_c_info *wlc, uint bandunit);
-static chanspec_t brcms_c_init_chanspec(struct brcms_c_info *wlc);
-static void brcms_c_bandinit_ordered(struct brcms_c_info *wlc,
-				     chanspec_t chanspec);
-static void brcms_c_bsinit(struct brcms_c_info *wlc);
-static int brcms_c_duty_cycle_set(struct brcms_c_info *wlc, int duty_cycle,
-			      bool isOFDM, bool writeToShm);
-static void brcms_c_radio_hwdisable_upd(struct brcms_c_info *wlc);
-static bool brcms_c_radio_monitor_start(struct brcms_c_info *wlc);
-static void brcms_c_radio_timer(void *arg);
-static void brcms_c_radio_enable(struct brcms_c_info *wlc);
-static void brcms_c_radio_upd(struct brcms_c_info *wlc);
-
-/* scan, association, BSS */
-static uint brcms_c_calc_ba_time(struct brcms_c_info *wlc, ratespec_t rate,
-			     u8 preamble_type);
-static void brcms_c_update_mimo_band_bwcap(struct brcms_c_info *wlc, u8 bwcap);
-static void brcms_c_ht_update_sgi_rx(struct brcms_c_info *wlc, int val);
-static void brcms_c_ht_update_ldpc(struct brcms_c_info *wlc, s8 val);
-static void brcms_c_war16165(struct brcms_c_info *wlc, bool tx);
-
-static void brcms_c_wme_retries_write(struct brcms_c_info *wlc);
-static bool brcms_c_attach_stf_ant_init(struct brcms_c_info *wlc);
-static uint brcms_c_attach_module(struct brcms_c_info *wlc);
-static void brcms_c_detach_module(struct brcms_c_info *wlc);
-static void brcms_c_timers_deinit(struct brcms_c_info *wlc);
-static void brcms_c_down_led_upd(struct brcms_c_info *wlc);
-static uint brcms_c_down_del_timer(struct brcms_c_info *wlc);
-static void brcms_c_ofdm_rateset_war(struct brcms_c_info *wlc);
-static int _brcms_c_ioctl(struct brcms_c_info *wlc, int cmd, void *arg, int len,
-		      struct brcms_c_if *wlcif);
-
-/* conditions under which the PM bit should be set in outgoing frames and STAY_AWAKE is meaningful
+static u16 get_sifs(struct brcms_band *band)
+{
+	return band->bandtype == BRCM_BAND_5G ? APHY_SIFS_TIME :
+				 BPHY_SIFS_TIME;
+}
+
+/*
+ * Detect Card removed.
+ * Even checking an sbconfig register read will not false trigger when the core
+ * is in reset it breaks CF address mechanism. Accessing gphy phyversion will
+ * cause SB error if aphy is in reset on 4306B0-DB. Need a simple accessible
+ * reg with fixed 0/1 pattern (some platforms return all 0).
+ * If clocks are present, call the sb routine which will figure out if the
+ * device is removed.
  */
-bool brcms_c_ps_allowed(struct brcms_c_info *wlc)
+static bool brcms_deviceremoved(struct brcms_c_info *wlc)
+{
+	if (!wlc->hw->clk)
+		return ai_deviceremoved(wlc->hw->sih);
+	return (R_REG(&wlc->hw->regs->maccontrol) &
+		(MCTL_PSM_JMP_0 | MCTL_IHR_EN)) != MCTL_IHR_EN;
+}
+
+/* sum the individual fifo tx pending packet counts */
+static s16 brcms_txpktpendtot(struct brcms_c_info *wlc)
+{
+	return wlc->core->txpktpend[0] + wlc->core->txpktpend[1] +
+	       wlc->core->txpktpend[2] + wlc->core->txpktpend[3];
+}
+
+static bool brcms_is_mband_unlocked(struct brcms_c_info *wlc)
+{
+	return wlc->pub->_nbands > 1 && !wlc->bandlocked;
+}
+
+static int brcms_chspec_bw(u16 chanspec)
+{
+	if (CHSPEC_IS40(chanspec))
+		return BRCMS_40_MHZ;
+	if (CHSPEC_IS20(chanspec))
+		return BRCMS_20_MHZ;
+
+	return BRCMS_10_MHZ;
+}
+
+/*
+ * return true if Minimum Power Consumption should
+ * be entered, false otherwise
+ */
+static bool brcms_c_is_non_delay_mpc(struct brcms_c_info *wlc)
+{
+	return false;
+}
+
+static bool brcms_c_ismpc(struct brcms_c_info *wlc)
+{
+	return (wlc->mpc_delay_off == 0) && (brcms_c_is_non_delay_mpc(wlc));
+}
+
+static void brcms_c_bsscfg_mfree(struct brcms_bss_cfg *cfg)
+{
+	if (cfg == NULL)
+		return;
+
+	kfree(cfg->current_bss);
+	kfree(cfg);
+}
+
+static void brcms_c_detach_mfree(struct brcms_c_info *wlc)
+{
+	if (wlc == NULL)
+		return;
+
+	brcms_c_bsscfg_mfree(wlc->bsscfg);
+	kfree(wlc->pub);
+	kfree(wlc->modulecb);
+	kfree(wlc->default_bss);
+	kfree(wlc->protection);
+	kfree(wlc->stf);
+	kfree(wlc->bandstate[0]);
+	kfree(wlc->corestate->macstat_snapshot);
+	kfree(wlc->corestate);
+	kfree(wlc->hw->bandstate[0]);
+	kfree(wlc->hw);
+
+	/* free the wlc */
+	kfree(wlc);
+	wlc = NULL;
+}
+
+static struct brcms_bss_cfg *brcms_c_bsscfg_malloc(uint unit)
 {
-	int idx;
 	struct brcms_bss_cfg *cfg;
 
-	/* disallow PS when one of the following global conditions meets */
-	if (!wlc->pub->associated)
+	cfg = kzalloc(sizeof(struct brcms_bss_cfg), GFP_ATOMIC);
+	if (cfg == NULL)
+		goto fail;
+
+	cfg->current_bss = kzalloc(sizeof(struct brcms_bss_info), GFP_ATOMIC);
+	if (cfg->current_bss == NULL)
+		goto fail;
+
+	return cfg;
+
+ fail:
+	brcms_c_bsscfg_mfree(cfg);
+	return NULL;
+}
+
+static struct brcms_c_info *
+brcms_c_attach_malloc(uint unit, uint *err, uint devid)
+{
+	struct brcms_c_info *wlc;
+
+	wlc = kzalloc(sizeof(struct brcms_c_info), GFP_ATOMIC);
+	if (wlc == NULL) {
+		*err = 1002;
+		goto fail;
+	}
+
+	/* allocate struct brcms_c_pub state structure */
+	wlc->pub = kzalloc(sizeof(struct brcms_pub), GFP_ATOMIC);
+	if (wlc->pub == NULL) {
+		*err = 1003;
+		goto fail;
+	}
+	wlc->pub->wlc = wlc;
+
+	/* allocate struct brcms_hardware state structure */
+
+	wlc->hw = kzalloc(sizeof(struct brcms_hardware), GFP_ATOMIC);
+	if (wlc->hw == NULL) {
+		*err = 1005;
+		goto fail;
+	}
+	wlc->hw->wlc = wlc;
+
+	wlc->hw->bandstate[0] =
+		kzalloc(sizeof(struct brcms_hw_band) * MAXBANDS, GFP_ATOMIC);
+	if (wlc->hw->bandstate[0] == NULL) {
+		*err = 1006;
+		goto fail;
+	} else {
+		int i;
+
+		for (i = 1; i < MAXBANDS; i++)
+			wlc->hw->bandstate[i] = (struct brcms_hw_band *)
+			    ((unsigned long)wlc->hw->bandstate[0] +
+			     (sizeof(struct brcms_hw_band) * i));
+	}
+
+	wlc->modulecb =
+		kzalloc(sizeof(struct modulecb) * BRCMS_MAXMODULES, GFP_ATOMIC);
+	if (wlc->modulecb == NULL) {
+		*err = 1009;
+		goto fail;
+	}
+
+	wlc->default_bss = kzalloc(sizeof(struct brcms_bss_info), GFP_ATOMIC);
+	if (wlc->default_bss == NULL) {
+		*err = 1010;
+		goto fail;
+	}
+
+	wlc->bsscfg = brcms_c_bsscfg_malloc(unit);
+	if (wlc->bsscfg == NULL) {
+		*err = 1011;
+		goto fail;
+	}
+
+	wlc->protection = kzalloc(sizeof(struct brcms_protection),
+				  GFP_ATOMIC);
+	if (wlc->protection == NULL) {
+		*err = 1016;
+		goto fail;
+	}
+
+	wlc->stf = kzalloc(sizeof(struct brcms_stf), GFP_ATOMIC);
+	if (wlc->stf == NULL) {
+		*err = 1017;
+		goto fail;
+	}
+
+	wlc->bandstate[0] =
+		kzalloc(sizeof(struct brcms_band)*MAXBANDS, GFP_ATOMIC);
+	if (wlc->bandstate[0] == NULL) {
+		*err = 1025;
+		goto fail;
+	} else {
+		int i;
+
+		for (i = 1; i < MAXBANDS; i++)
+			wlc->bandstate[i] = (struct brcms_band *)
+				((unsigned long)wlc->bandstate[0]
+				+ (sizeof(struct brcms_band)*i));
+	}
+
+	wlc->corestate = kzalloc(sizeof(struct brcms_core), GFP_ATOMIC);
+	if (wlc->corestate == NULL) {
+		*err = 1026;
+		goto fail;
+	}
+
+	wlc->corestate->macstat_snapshot =
+		kzalloc(sizeof(struct macstat), GFP_ATOMIC);
+	if (wlc->corestate->macstat_snapshot == NULL) {
+		*err = 1027;
+		goto fail;
+	}
+
+	return wlc;
+
+ fail:
+	brcms_c_detach_mfree(wlc);
+	return NULL;
+}
+
+/*
+ * Update the slot timing for standard 11b/g (20us slots)
+ * or shortslot 11g (9us slots)
+ * The PSM needs to be suspended for this call.
+ */
+static void brcms_b_update_slot_timing(struct brcms_hardware *wlc_hw,
+					bool shortslot)
+{
+	struct d11regs __iomem *regs;
+
+	regs = wlc_hw->regs;
+
+	if (shortslot) {
+		/* 11g short slot: 11a timing */
+		W_REG(&regs->ifs_slot, 0x0207);	/* APHY_SLOT_TIME */
+		brcms_b_write_shm(wlc_hw, M_DOT11_SLOT, APHY_SLOT_TIME);
+	} else {
+		/* 11g long slot: 11b timing */
+		W_REG(&regs->ifs_slot, 0x0212);	/* BPHY_SLOT_TIME */
+		brcms_b_write_shm(wlc_hw, M_DOT11_SLOT, BPHY_SLOT_TIME);
+	}
+}
+
+/*
+ * calculate frame duration of a given rate and length, return
+ * time in usec unit
+ */
+uint
+brcms_c_calc_frame_time(struct brcms_c_info *wlc, u32 ratespec,
+			u8 preamble_type, uint mac_len)
+{
+	uint nsyms, dur = 0, Ndps, kNdps;
+	uint rate = rspec2rate(ratespec);
+
+	if (rate == 0) {
+		wiphy_err(wlc->wiphy, "wl%d: WAR: using rate of 1 mbps\n",
+			  wlc->pub->unit);
+		rate = BRCM_RATE_1M;
+	}
+
+	BCMMSG(wlc->wiphy, "wl%d: rspec 0x%x, preamble_type %d, len%d\n",
+		 wlc->pub->unit, ratespec, preamble_type, mac_len);
+
+	if (is_mcs_rate(ratespec)) {
+		uint mcs = ratespec & RSPEC_RATE_MASK;
+		int tot_streams = mcs_2_txstreams(mcs) + rspec_stc(ratespec);
+
+		dur = PREN_PREAMBLE + (tot_streams * PREN_PREAMBLE_EXT);
+		if (preamble_type == BRCMS_MM_PREAMBLE)
+			dur += PREN_MM_EXT;
+		/* 1000Ndbps = kbps * 4 */
+		kNdps = mcs_2_rate(mcs, rspec_is40mhz(ratespec),
+				   rspec_issgi(ratespec)) * 4;
+
+		if (rspec_stc(ratespec) == 0)
+			nsyms =
+			    CEIL((APHY_SERVICE_NBITS + 8 * mac_len +
+				  APHY_TAIL_NBITS) * 1000, kNdps);
+		else
+			/* STBC needs to have even number of symbols */
+			nsyms =
+			    2 *
+			    CEIL((APHY_SERVICE_NBITS + 8 * mac_len +
+				  APHY_TAIL_NBITS) * 1000, 2 * kNdps);
+
+		dur += APHY_SYMBOL_TIME * nsyms;
+		if (wlc->band->bandtype == BRCM_BAND_2G)
+			dur += DOT11_OFDM_SIGNAL_EXTENSION;
+	} else if (is_ofdm_rate(rate)) {
+		dur = APHY_PREAMBLE_TIME;
+		dur += APHY_SIGNAL_TIME;
+		/* Ndbps = Mbps * 4 = rate(500Kbps) * 2 */
+		Ndps = rate * 2;
+		/* NSyms = CEILING((SERVICE + 8*NBytes + TAIL) / Ndbps) */
+		nsyms =
+		    CEIL((APHY_SERVICE_NBITS + 8 * mac_len + APHY_TAIL_NBITS),
+			 Ndps);
+		dur += APHY_SYMBOL_TIME * nsyms;
+		if (wlc->band->bandtype == BRCM_BAND_2G)
+			dur += DOT11_OFDM_SIGNAL_EXTENSION;
+	} else {
+		/*
+		 * calc # bits * 2 so factor of 2 in rate (1/2 mbps)
+		 * will divide out
+		 */
+		mac_len = mac_len * 8 * 2;
+		/* calc ceiling of bits/rate = microseconds of air time */
+		dur = (mac_len + rate - 1) / rate;
+		if (preamble_type & BRCMS_SHORT_PREAMBLE)
+			dur += BPHY_PLCP_SHORT_TIME;
+		else
+			dur += BPHY_PLCP_TIME;
+	}
+	return dur;
+}
+
+static void brcms_c_write_inits(struct brcms_hardware *wlc_hw,
+				const struct d11init *inits)
+{
+	int i;
+	u8 __iomem *base;
+	u8 __iomem *addr;
+	u16 size;
+	u32 value;
+
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
+
+	base = (u8 __iomem *)wlc_hw->regs;
+
+	for (i = 0; inits[i].addr != cpu_to_le16(0xffff); i++) {
+		size = le16_to_cpu(inits[i].size);
+		addr = base + le16_to_cpu(inits[i].addr);
+		value = le32_to_cpu(inits[i].value);
+		if (size == 2)
+			W_REG((u16 __iomem *)addr, value);
+		else if (size == 4)
+			W_REG((u32 __iomem *)addr, value);
+		else
+			break;
+	}
+}
+
+static void brcms_c_write_mhf(struct brcms_hardware *wlc_hw, u16 *mhfs)
+{
+	u8 idx;
+	u16 addr[] = {
+		M_HOST_FLAGS1, M_HOST_FLAGS2, M_HOST_FLAGS3, M_HOST_FLAGS4,
+		M_HOST_FLAGS5
+	};
+
+	for (idx = 0; idx < MHFMAX; idx++)
+		brcms_b_write_shm(wlc_hw, addr[idx], mhfs[idx]);
+}
+
+static void brcms_c_ucode_bsinit(struct brcms_hardware *wlc_hw)
+{
+	struct wiphy *wiphy = wlc_hw->wlc->wiphy;
+	struct brcms_ucode *ucode = &wlc_hw->wlc->wl->ucode;
+
+	/* init microcode host flags */
+	brcms_c_write_mhf(wlc_hw, wlc_hw->band->mhfs);
+
+	/* do band-specific ucode IHR, SHM, and SCR inits */
+	if (D11REV_IS(wlc_hw->corerev, 23)) {
+		if (BRCMS_ISNPHY(wlc_hw->band))
+			brcms_c_write_inits(wlc_hw, ucode->d11n0bsinitvals16);
+		else
+			wiphy_err(wiphy, "%s: wl%d: unsupported phy in corerev"
+				  " %d\n", __func__, wlc_hw->unit,
+				  wlc_hw->corerev);
+	} else {
+		if (D11REV_IS(wlc_hw->corerev, 24)) {
+			if (BRCMS_ISLCNPHY(wlc_hw->band))
+				brcms_c_write_inits(wlc_hw,
+						    ucode->d11lcn0bsinitvals24);
+			else
+				wiphy_err(wiphy, "%s: wl%d: unsupported phy in"
+					  " core rev %d\n", __func__,
+					  wlc_hw->unit, wlc_hw->corerev);
+		} else {
+			wiphy_err(wiphy, "%s: wl%d: unsupported corerev %d\n",
+				__func__, wlc_hw->unit, wlc_hw->corerev);
+		}
+	}
+}
+
+static void brcms_b_core_phy_clk(struct brcms_hardware *wlc_hw, bool clk)
+{
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d: clk %d\n", wlc_hw->unit, clk);
+
+	wlc_hw->phyclk = clk;
+
+	if (OFF == clk) {	/* clear gmode bit, put phy into reset */
+
+		ai_core_cflags(wlc_hw->sih, (SICF_PRST | SICF_FGC | SICF_GMODE),
+			       (SICF_PRST | SICF_FGC));
+		udelay(1);
+		ai_core_cflags(wlc_hw->sih, (SICF_PRST | SICF_FGC), SICF_PRST);
+		udelay(1);
+
+	} else {		/* take phy out of reset */
+
+		ai_core_cflags(wlc_hw->sih, (SICF_PRST | SICF_FGC), SICF_FGC);
+		udelay(1);
+		ai_core_cflags(wlc_hw->sih, (SICF_FGC), 0);
+		udelay(1);
+
+	}
+}
+
+/* low-level band switch utility routine */
+static void brcms_c_setxband(struct brcms_hardware *wlc_hw, uint bandunit)
+{
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d: bandunit %d\n", wlc_hw->unit,
+		bandunit);
+
+	wlc_hw->band = wlc_hw->bandstate[bandunit];
+
+	/*
+	 * BMAC_NOTE:
+	 *   until we eliminate need for wlc->band refs in low level code
+	 */
+	wlc_hw->wlc->band = wlc_hw->wlc->bandstate[bandunit];
+
+	/* set gmode core flag */
+	if (wlc_hw->sbclk && !wlc_hw->noreset)
+		ai_core_cflags(wlc_hw->sih, SICF_GMODE,
+			       ((bandunit == 0) ? SICF_GMODE : 0));
+}
+
+/* switch to new band but leave it inactive */
+static u32 brcms_c_setband_inact(struct brcms_c_info *wlc, uint bandunit)
+{
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	u32 macintmask;
+
+	BCMMSG(wlc->wiphy, "wl%d\n", wlc_hw->unit);
+
+	WARN_ON((R_REG(&wlc_hw->regs->maccontrol) & MCTL_EN_MAC) != 0);
+
+	/* disable interrupts */
+	macintmask = brcms_intrsoff(wlc->wl);
+
+	/* radio off */
+	wlc_phy_switch_radio(wlc_hw->band->pi, OFF);
+
+	brcms_b_core_phy_clk(wlc_hw, OFF);
+
+	brcms_c_setxband(wlc_hw, bandunit);
+
+	return macintmask;
+}
+
+/* process an individual struct tx_status */
+static bool
+brcms_c_dotxstatus(struct brcms_c_info *wlc, struct tx_status *txs)
+{
+	struct sk_buff *p;
+	uint queue;
+	struct d11txh *txh;
+	struct scb *scb = NULL;
+	bool free_pdu;
+	int tx_rts, tx_frame_count, tx_rts_count;
+	uint totlen, supr_status;
+	bool lastframe;
+	struct ieee80211_hdr *h;
+	u16 mcl;
+	struct ieee80211_tx_info *tx_info;
+	struct ieee80211_tx_rate *txrate;
+	int i;
+
+	/* discard intermediate indications for ucode with one legitimate case:
+	 *   e.g. if "useRTS" is set. ucode did a successful rts/cts exchange,
+	 *   but the subsequent tx of DATA failed. so it will start rts/cts
+	 *   from the beginning (resetting the rts transmission count)
+	 */
+	if (!(txs->status & TX_STATUS_AMPDU)
+	    && (txs->status & TX_STATUS_INTERMEDIATE)) {
+		wiphy_err(wlc->wiphy, "%s: INTERMEDIATE but not AMPDU\n",
+			  __func__);
 		return false;
+	}
 
-	/* disallow PS when one of these meets when not scanning */
-	if (AP_ACTIVE(wlc) || wlc->monitor)
+	queue = txs->frameid & TXFID_QUEUE_MASK;
+	if (queue >= NFIFO) {
+		p = NULL;
+		goto fatal;
+	}
+
+	p = dma_getnexttxp(wlc->hw->di[queue], DMA_RANGE_TRANSMITTED);
+	if (p == NULL)
+		goto fatal;
+
+	txh = (struct d11txh *) (p->data);
+	mcl = le16_to_cpu(txh->MacTxControlLow);
+
+	if (txs->phyerr) {
+		if (brcm_msg_level & LOG_ERROR_VAL) {
+			wiphy_err(wlc->wiphy, "phyerr 0x%x, rate 0x%x\n",
+				  txs->phyerr, txh->MainRates);
+			brcms_c_print_txdesc(txh);
+		}
+		brcms_c_print_txstatus(txs);
+	}
+
+	if (txs->frameid != le16_to_cpu(txh->TxFrameID))
+		goto fatal;
+	tx_info = IEEE80211_SKB_CB(p);
+	h = (struct ieee80211_hdr *)((u8 *) (txh + 1) + D11_PHY_HDR_LEN);
+
+	if (tx_info->control.sta)
+		scb = &wlc->pri_scb;
+
+	if (tx_info->flags & IEEE80211_TX_CTL_AMPDU) {
+		brcms_c_ampdu_dotxstatus(wlc->ampdu, scb, p, txs);
 		return false;
+	}
+
+	supr_status = txs->status & TX_STATUS_SUPR_MASK;
+	if (supr_status == TX_STATUS_SUPR_BADCH)
+		BCMMSG(wlc->wiphy,
+		       "%s: Pkt tx suppressed, possibly channel %d\n",
+		       __func__, CHSPEC_CHANNEL(wlc->default_bss->chanspec));
+
+	tx_rts = le16_to_cpu(txh->MacTxControlLow) & TXC_SENDRTS;
+	tx_frame_count =
+	    (txs->status & TX_STATUS_FRM_RTX_MASK) >> TX_STATUS_FRM_RTX_SHIFT;
+	tx_rts_count =
+	    (txs->status & TX_STATUS_RTS_RTX_MASK) >> TX_STATUS_RTS_RTX_SHIFT;
+
+	lastframe = !ieee80211_has_morefrags(h->frame_control);
+
+	if (!lastframe) {
+		wiphy_err(wlc->wiphy, "Not last frame!\n");
+	} else {
+		/*
+		 * Set information to be consumed by Minstrel ht.
+		 *
+		 * The "fallback limit" is the number of tx attempts a given
+		 * MPDU is sent at the "primary" rate. Tx attempts beyond that
+		 * limit are sent at the "secondary" rate.
+		 * A 'short frame' does not exceed RTS treshold.
+		 */
+		u16 sfbl,	/* Short Frame Rate Fallback Limit */
+		    lfbl,	/* Long Frame Rate Fallback Limit */
+		    fbl;
 
-	for (idx = 0; idx < BRCMS_MAXBSSCFG; idx++) {
-		cfg = wlc->bsscfg[idx];
-		if (cfg && BSSCFG_STA(cfg) && cfg->associated) {
+		if (queue < AC_COUNT) {
+			sfbl = GFIELD(wlc->wme_retries[wme_fifo2ac[queue]],
+				      EDCF_SFB);
+			lfbl = GFIELD(wlc->wme_retries[wme_fifo2ac[queue]],
+				      EDCF_LFB);
+		} else {
+			sfbl = wlc->SFBL;
+			lfbl = wlc->LFBL;
+		}
+
+		txrate = tx_info->status.rates;
+		if (txrate[0].flags & IEEE80211_TX_RC_USE_RTS_CTS)
+			fbl = lfbl;
+		else
+			fbl = sfbl;
+
+		ieee80211_tx_info_clear_status(tx_info);
+
+		if ((tx_frame_count > fbl) && (txrate[1].idx >= 0)) {
+			/*
+			 * rate selection requested a fallback rate
+			 * and we used it
+			 */
+			txrate[0].count = fbl;
+			txrate[1].count = tx_frame_count - fbl;
+		} else {
+			/*
+			 * rate selection did not request fallback rate, or
+			 * we didn't need it
+			 */
+			txrate[0].count = tx_frame_count;
 			/*
-			 * disallow PS when one of the following
-			 * bsscfg specific conditions meets
+			 * rc80211_minstrel.c:minstrel_tx_status() expects
+			 * unused rates to be marked with idx = -1
 			 */
-			if (!cfg->BSS || !BRCMS_PORTOPEN(cfg))
-				return false;
+			txrate[1].idx = -1;
+			txrate[1].count = 0;
+		}
+
+		/* clear the rest of the rates */
+		for (i = 2; i < IEEE80211_TX_MAX_RATES; i++) {
+			txrate[i].idx = -1;
+			txrate[i].count = 0;
+		}
+
+		if (txs->status & TX_STATUS_ACK_RCV)
+			tx_info->flags |= IEEE80211_TX_STAT_ACK;
+	}
+
+	totlen = brcmu_pkttotlen(p);
+	free_pdu = true;
+
+	brcms_c_txfifo_complete(wlc, queue, 1);
+
+	if (lastframe) {
+		p->next = NULL;
+		p->prev = NULL;
+		/* remove PLCP & Broadcom tx descriptor header */
+		skb_pull(p, D11_PHY_HDR_LEN);
+		skb_pull(p, D11_TXH_LEN);
+		ieee80211_tx_status_irqsafe(wlc->pub->ieee_hw, p);
+	} else {
+		wiphy_err(wlc->wiphy, "%s: Not last frame => not calling "
+			  "tx_status\n", __func__);
+	}
+
+	return false;
+
+ fatal:
+	if (p)
+		brcmu_pkt_buf_free_skb(p);
+
+	return true;
+
+}
+
+/* process tx completion events in BMAC
+ * Return true if more tx status need to be processed. false otherwise.
+ */
+static bool
+brcms_b_txstatus(struct brcms_hardware *wlc_hw, bool bound, bool *fatal)
+{
+	bool morepending = false;
+	struct brcms_c_info *wlc = wlc_hw->wlc;
+	struct d11regs __iomem *regs;
+	struct tx_status txstatus, *txs;
+	u32 s1, s2;
+	uint n = 0;
+	/*
+	 * Param 'max_tx_num' indicates max. # tx status to process before
+	 * break out.
+	 */
+	uint max_tx_num = bound ? TXSBND : -1;
+
+	BCMMSG(wlc->wiphy, "wl%d\n", wlc_hw->unit);
+
+	txs = &txstatus;
+	regs = wlc_hw->regs;
+	*fatal = false;
+	while (!(*fatal)
+	       && (s1 = R_REG(&regs->frmtxstatus)) & TXS_V) {
+
+		if (s1 == 0xffffffff) {
+			wiphy_err(wlc->wiphy, "wl%d: %s: dead chip\n",
+				wlc_hw->unit, __func__);
+			return morepending;
+		}
+
+		s2 = R_REG(&regs->frmtxstatus2);
+
+		txs->status = s1 & TXS_STATUS_MASK;
+		txs->frameid = (s1 & TXS_FID_MASK) >> TXS_FID_SHIFT;
+		txs->sequence = s2 & TXS_SEQ_MASK;
+		txs->phyerr = (s2 & TXS_PTX_MASK) >> TXS_PTX_SHIFT;
+		txs->lasttxtime = 0;
+
+		*fatal = brcms_c_dotxstatus(wlc_hw->wlc, txs);
+
+		/* !give others some time to run! */
+		if (++n >= max_tx_num)
+			break;
+	}
+
+	if (*fatal)
+		return 0;
+
+	if (n >= max_tx_num)
+		morepending = true;
+
+	if (!pktq_empty(&wlc->pkt_queue->q))
+		brcms_c_send_q(wlc);
+
+	return morepending;
+}
+
+static void brcms_c_tbtt(struct brcms_c_info *wlc)
+{
+	if (!wlc->bsscfg->BSS)
+		/*
+		 * DirFrmQ is now valid...defer setting until end
+		 * of ATIM window
+		 */
+		wlc->qvalid |= MCMD_DIRFRMQVAL;
+}
+
+/* set initial host flags value */
+static void
+brcms_c_mhfdef(struct brcms_c_info *wlc, u16 *mhfs, u16 mhf2_init)
+{
+	struct brcms_hardware *wlc_hw = wlc->hw;
+
+	memset(mhfs, 0, MHFMAX * sizeof(u16));
+
+	mhfs[MHF2] |= mhf2_init;
+
+	/* prohibit use of slowclock on multifunction boards */
+	if (wlc_hw->boardflags & BFL_NOPLLDOWN)
+		mhfs[MHF1] |= MHF1_FORCEFASTCLK;
+
+	if (BRCMS_ISNPHY(wlc_hw->band) && NREV_LT(wlc_hw->band->phyrev, 2)) {
+		mhfs[MHF2] |= MHF2_NPHY40MHZ_WAR;
+		mhfs[MHF1] |= MHF1_IQSWAP_WAR;
+	}
+}
+
+static struct dma64regs __iomem *
+dmareg(struct brcms_hardware *hw, uint direction, uint fifonum)
+{
+	if (direction == DMA_TX)
+		return &(hw->regs->fifo64regs[fifonum].dmaxmt);
+	return &(hw->regs->fifo64regs[fifonum].dmarcv);
+}
+
+static bool brcms_b_attach_dmapio(struct brcms_c_info *wlc, uint j, bool wme)
+{
+	uint i;
+	char name[8];
+	/*
+	 * ucode host flag 2 needed for pio mode, independent of band and fifo
+	 */
+	u16 pio_mhf2 = 0;
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	uint unit = wlc_hw->unit;
+	struct wiphy *wiphy = wlc->wiphy;
+
+	/* name and offsets for dma_attach */
+	snprintf(name, sizeof(name), "wl%d", unit);
+
+	if (wlc_hw->di[0] == NULL) {	/* Init FIFOs */
+		int dma_attach_err = 0;
+
+		/*
+		 * FIFO 0
+		 * TX: TX_AC_BK_FIFO (TX AC Background data packets)
+		 * RX: RX_FIFO (RX data packets)
+		 */
+		wlc_hw->di[0] = dma_attach(name, wlc_hw->sih,
+					   (wme ? dmareg(wlc_hw, DMA_TX, 0) :
+					    NULL), dmareg(wlc_hw, DMA_RX, 0),
+					   (wme ? NTXD : 0), NRXD,
+					   RXBUFSZ, -1, NRXBUFPOST,
+					   BRCMS_HWRXOFF, &brcm_msg_level);
+		dma_attach_err |= (NULL == wlc_hw->di[0]);
+
+		/*
+		 * FIFO 1
+		 * TX: TX_AC_BE_FIFO (TX AC Best-Effort data packets)
+		 *   (legacy) TX_DATA_FIFO (TX data packets)
+		 * RX: UNUSED
+		 */
+		wlc_hw->di[1] = dma_attach(name, wlc_hw->sih,
+					   dmareg(wlc_hw, DMA_TX, 1), NULL,
+					   NTXD, 0, 0, -1, 0, 0,
+					   &brcm_msg_level);
+		dma_attach_err |= (NULL == wlc_hw->di[1]);
+
+		/*
+		 * FIFO 2
+		 * TX: TX_AC_VI_FIFO (TX AC Video data packets)
+		 * RX: UNUSED
+		 */
+		wlc_hw->di[2] = dma_attach(name, wlc_hw->sih,
+					   dmareg(wlc_hw, DMA_TX, 2), NULL,
+					   NTXD, 0, 0, -1, 0, 0,
+					   &brcm_msg_level);
+		dma_attach_err |= (NULL == wlc_hw->di[2]);
+		/*
+		 * FIFO 3
+		 * TX: TX_AC_VO_FIFO (TX AC Voice data packets)
+		 *   (legacy) TX_CTL_FIFO (TX control & mgmt packets)
+		 */
+		wlc_hw->di[3] = dma_attach(name, wlc_hw->sih,
+					   dmareg(wlc_hw, DMA_TX, 3),
+					   NULL, NTXD, 0, 0, -1,
+					   0, 0, &brcm_msg_level);
+		dma_attach_err |= (NULL == wlc_hw->di[3]);
+/* Cleaner to leave this as if with AP defined */
+
+		if (dma_attach_err) {
+			wiphy_err(wiphy, "wl%d: wlc_attach: dma_attach failed"
+				  "\n", unit);
+			return false;
+		}
+
+		/* get pointer to dma engine tx flow control variable */
+		for (i = 0; i < NFIFO; i++)
+			if (wlc_hw->di[i])
+				wlc_hw->txavail[i] =
+				    (uint *) dma_getvar(wlc_hw->di[i],
+							"&txavail");
+	}
+
+	/* initial ucode host flags */
+	brcms_c_mhfdef(wlc, wlc_hw->band->mhfs, pio_mhf2);
+
+	return true;
+}
+
+static void brcms_b_detach_dmapio(struct brcms_hardware *wlc_hw)
+{
+	uint j;
+
+	for (j = 0; j < NFIFO; j++) {
+		if (wlc_hw->di[j]) {
+			dma_detach(wlc_hw->di[j]);
+			wlc_hw->di[j] = NULL;
+		}
+	}
+}
+
+/*
+ * Initialize brcms_c_info default values ...
+ * may get overrides later in this function
+ *  BMAC_NOTES, move low out and resolve the dangling ones
+ */
+static void brcms_b_info_init(struct brcms_hardware *wlc_hw)
+{
+	struct brcms_c_info *wlc = wlc_hw->wlc;
+
+	/* set default sw macintmask value */
+	wlc->defmacintmask = DEF_MACINTMASK;
+
+	/* various 802.11g modes */
+	wlc_hw->shortslot = false;
+
+	wlc_hw->SFBL = RETRY_SHORT_FB;
+	wlc_hw->LFBL = RETRY_LONG_FB;
+
+	/* default mac retry limits */
+	wlc_hw->SRL = RETRY_SHORT_DEF;
+	wlc_hw->LRL = RETRY_LONG_DEF;
+	wlc_hw->chanspec = ch20mhz_chspec(1);
+}
+
+static void brcms_b_wait_for_wake(struct brcms_hardware *wlc_hw)
+{
+	/* delay before first read of ucode state */
+	udelay(40);
+
+	/* wait until ucode is no longer asleep */
+	SPINWAIT((brcms_b_read_shm(wlc_hw, M_UCODE_DBGST) ==
+		  DBGST_ASLEEP), wlc_hw->wlc->fastpwrup_dly);
+}
+
+/* control chip clock to save power, enable dynamic clock or force fast clock */
+static void brcms_b_clkctl_clk(struct brcms_hardware *wlc_hw, uint mode)
+{
+	if (wlc_hw->sih->cccaps & CC_CAP_PMU) {
+		/* new chips with PMU, CCS_FORCEHT will distribute the HT clock
+		 * on backplane, but mac core will still run on ALP(not HT) when
+		 * it enters powersave mode, which means the FCA bit may not be
+		 * set. Should wakeup mac if driver wants it to run on HT.
+		 */
+
+		if (wlc_hw->clk) {
+			if (mode == CLK_FAST) {
+				OR_REG(&wlc_hw->regs->clk_ctl_st,
+				       CCS_FORCEHT);
+
+				udelay(64);
+
+				SPINWAIT(((R_REG
+					   (&wlc_hw->regs->
+					    clk_ctl_st) & CCS_HTAVAIL) == 0),
+					 PMU_MAX_TRANSITION_DLY);
+				WARN_ON(!(R_REG
+					  (&wlc_hw->regs->
+					   clk_ctl_st) & CCS_HTAVAIL));
+			} else {
+				if ((wlc_hw->sih->pmurev == 0) &&
+				    (R_REG
+				     (&wlc_hw->regs->
+				      clk_ctl_st) & (CCS_FORCEHT | CCS_HTAREQ)))
+					SPINWAIT(((R_REG
+						   (&wlc_hw->regs->
+						    clk_ctl_st) & CCS_HTAVAIL)
+						  == 0),
+						 PMU_MAX_TRANSITION_DLY);
+				AND_REG(&wlc_hw->regs->clk_ctl_st,
+					~CCS_FORCEHT);
+			}
+		}
+		wlc_hw->forcefastclk = (mode == CLK_FAST);
+	} else {
+
+		/* old chips w/o PMU, force HT through cc,
+		 * then use FCA to verify mac is running fast clock
+		 */
+
+		wlc_hw->forcefastclk = ai_clkctl_cc(wlc_hw->sih, mode);
+
+		/* check fast clock is available (if core is not in reset) */
+		if (wlc_hw->forcefastclk && wlc_hw->clk)
+			WARN_ON(!(ai_core_sflags(wlc_hw->sih, 0, 0) &
+				  SISF_FCLKA));
+
+		/*
+		 * keep the ucode wake bit on if forcefastclk is on since we
+		 * do not want ucode to put us back to slow clock when it dozes
+		 * for PM mode. Code below matches the wake override bit with
+		 * current forcefastclk state. Only setting bit in wake_override
+		 * instead of waking ucode immediately since old code had this
+		 * behavior. Older code set wlc->forcefastclk but only had the
+		 * wake happen if the wakup_ucode work (protected by an up
+		 * check) was executed just below.
+		 */
+		if (wlc_hw->forcefastclk)
+			mboolset(wlc_hw->wake_override,
+				 BRCMS_WAKE_OVERRIDE_FORCEFAST);
+		else
+			mboolclr(wlc_hw->wake_override,
+				 BRCMS_WAKE_OVERRIDE_FORCEFAST);
+	}
+}
+
+/* set or clear ucode host flag bits
+ * it has an optimization for no-change write
+ * it only writes through shared memory when the core has clock;
+ * pre-CLK changes should use wlc_write_mhf to get around the optimization
+ *
+ *
+ * bands values are: BRCM_BAND_AUTO <--- Current band only
+ *                   BRCM_BAND_5G   <--- 5G band only
+ *                   BRCM_BAND_2G   <--- 2G band only
+ *                   BRCM_BAND_ALL  <--- All bands
+ */
+void
+brcms_b_mhf(struct brcms_hardware *wlc_hw, u8 idx, u16 mask, u16 val,
+	     int bands)
+{
+	u16 save;
+	u16 addr[MHFMAX] = {
+		M_HOST_FLAGS1, M_HOST_FLAGS2, M_HOST_FLAGS3, M_HOST_FLAGS4,
+		M_HOST_FLAGS5
+	};
+	struct brcms_hw_band *band;
+
+	if ((val & ~mask) || idx >= MHFMAX)
+		return; /* error condition */
+
+	switch (bands) {
+		/* Current band only or all bands,
+		 * then set the band to current band
+		 */
+	case BRCM_BAND_AUTO:
+	case BRCM_BAND_ALL:
+		band = wlc_hw->band;
+		break;
+	case BRCM_BAND_5G:
+		band = wlc_hw->bandstate[BAND_5G_INDEX];
+		break;
+	case BRCM_BAND_2G:
+		band = wlc_hw->bandstate[BAND_2G_INDEX];
+		break;
+	default:
+		band = NULL;	/* error condition */
+	}
+
+	if (band) {
+		save = band->mhfs[idx];
+		band->mhfs[idx] = (band->mhfs[idx] & ~mask) | val;
+
+		/* optimization: only write through if changed, and
+		 * changed band is the current band
+		 */
+		if (wlc_hw->clk && (band->mhfs[idx] != save)
+		    && (band == wlc_hw->band))
+			brcms_b_write_shm(wlc_hw, addr[idx],
+					   (u16) band->mhfs[idx]);
+	}
+
+	if (bands == BRCM_BAND_ALL) {
+		wlc_hw->bandstate[0]->mhfs[idx] =
+		    (wlc_hw->bandstate[0]->mhfs[idx] & ~mask) | val;
+		wlc_hw->bandstate[1]->mhfs[idx] =
+		    (wlc_hw->bandstate[1]->mhfs[idx] & ~mask) | val;
+	}
+}
+
+/* set the maccontrol register to desired reset state and
+ * initialize the sw cache of the register
+ */
+static void brcms_c_mctrl_reset(struct brcms_hardware *wlc_hw)
+{
+	/* IHR accesses are always enabled, PSM disabled, HPS off and WAKE on */
+	wlc_hw->maccontrol = 0;
+	wlc_hw->suspended_fifos = 0;
+	wlc_hw->wake_override = 0;
+	wlc_hw->mute_override = 0;
+	brcms_b_mctrl(wlc_hw, ~0, MCTL_IHR_EN | MCTL_WAKE);
+}
+
+/*
+ * write the software state of maccontrol and
+ * overrides to the maccontrol register
+ */
+static void brcms_c_mctrl_write(struct brcms_hardware *wlc_hw)
+{
+	u32 maccontrol = wlc_hw->maccontrol;
+
+	/* OR in the wake bit if overridden */
+	if (wlc_hw->wake_override)
+		maccontrol |= MCTL_WAKE;
+
+	/* set AP and INFRA bits for mute if needed */
+	if (wlc_hw->mute_override) {
+		maccontrol &= ~(MCTL_AP);
+		maccontrol |= MCTL_INFRA;
+	}
+
+	W_REG(&wlc_hw->regs->maccontrol, maccontrol);
+}
+
+/* set or clear maccontrol bits */
+void brcms_b_mctrl(struct brcms_hardware *wlc_hw, u32 mask, u32 val)
+{
+	u32 maccontrol;
+	u32 new_maccontrol;
+
+	if (val & ~mask)
+		return; /* error condition */
+	maccontrol = wlc_hw->maccontrol;
+	new_maccontrol = (maccontrol & ~mask) | val;
+
+	/* if the new maccontrol value is the same as the old, nothing to do */
+	if (new_maccontrol == maccontrol)
+		return;
+
+	/* something changed, cache the new value */
+	wlc_hw->maccontrol = new_maccontrol;
+
+	/* write the new values with overrides applied */
+	brcms_c_mctrl_write(wlc_hw);
+}
+
+void brcms_c_ucode_wake_override_set(struct brcms_hardware *wlc_hw,
+				 u32 override_bit)
+{
+	if (wlc_hw->wake_override || (wlc_hw->maccontrol & MCTL_WAKE)) {
+		mboolset(wlc_hw->wake_override, override_bit);
+		return;
+	}
+
+	mboolset(wlc_hw->wake_override, override_bit);
+
+	brcms_c_mctrl_write(wlc_hw);
+	brcms_b_wait_for_wake(wlc_hw);
+}
+
+void brcms_c_ucode_wake_override_clear(struct brcms_hardware *wlc_hw,
+				   u32 override_bit)
+{
+	mboolclr(wlc_hw->wake_override, override_bit);
+
+	if (wlc_hw->wake_override || (wlc_hw->maccontrol & MCTL_WAKE))
+		return;
+
+	brcms_c_mctrl_write(wlc_hw);
+}
+
+/* When driver needs ucode to stop beaconing, it has to make sure that
+ * MCTL_AP is clear and MCTL_INFRA is set
+ * Mode           MCTL_AP        MCTL_INFRA
+ * AP                1              1
+ * STA               0              1 <--- This will ensure no beacons
+ * IBSS              0              0
+ */
+static void brcms_c_ucode_mute_override_set(struct brcms_hardware *wlc_hw)
+{
+	wlc_hw->mute_override = 1;
+
+	/* if maccontrol already has AP == 0 and INFRA == 1 without this
+	 * override, then there is no change to write
+	 */
+	if ((wlc_hw->maccontrol & (MCTL_AP | MCTL_INFRA)) == MCTL_INFRA)
+		return;
+
+	brcms_c_mctrl_write(wlc_hw);
+}
+
+/* Clear the override on AP and INFRA bits */
+static void brcms_c_ucode_mute_override_clear(struct brcms_hardware *wlc_hw)
+{
+	if (wlc_hw->mute_override == 0)
+		return;
+
+	wlc_hw->mute_override = 0;
+
+	/* if maccontrol already has AP == 0 and INFRA == 1 without this
+	 * override, then there is no change to write
+	 */
+	if ((wlc_hw->maccontrol & (MCTL_AP | MCTL_INFRA)) == MCTL_INFRA)
+		return;
+
+	brcms_c_mctrl_write(wlc_hw);
+}
+
+/*
+ * Write a MAC address to the given match reg offset in the RXE match engine.
+ */
+static void
+brcms_b_set_addrmatch(struct brcms_hardware *wlc_hw, int match_reg_offset,
+		       const u8 *addr)
+{
+	struct d11regs __iomem *regs;
+	u16 mac_l;
+	u16 mac_m;
+	u16 mac_h;
+
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d: brcms_b_set_addrmatch\n",
+		 wlc_hw->unit);
+
+	regs = wlc_hw->regs;
+	mac_l = addr[0] | (addr[1] << 8);
+	mac_m = addr[2] | (addr[3] << 8);
+	mac_h = addr[4] | (addr[5] << 8);
+
+	/* enter the MAC addr into the RXE match registers */
+	W_REG(&regs->rcm_ctl, RCM_INC_DATA | match_reg_offset);
+	W_REG(&regs->rcm_mat_data, mac_l);
+	W_REG(&regs->rcm_mat_data, mac_m);
+	W_REG(&regs->rcm_mat_data, mac_h);
+
+}
+
+void
+brcms_b_write_template_ram(struct brcms_hardware *wlc_hw, int offset, int len,
+			    void *buf)
+{
+	struct d11regs __iomem *regs;
+	u32 word;
+	__le32 word_le;
+	__be32 word_be;
+	bool be_bit;
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
+
+	regs = wlc_hw->regs;
+	W_REG(&regs->tplatewrptr, offset);
+
+	/* if MCTL_BIGEND bit set in mac control register,
+	 * the chip swaps data in fifo, as well as data in
+	 * template ram
+	 */
+	be_bit = (R_REG(&regs->maccontrol) & MCTL_BIGEND) != 0;
+
+	while (len > 0) {
+		memcpy(&word, buf, sizeof(u32));
+
+		if (be_bit) {
+			word_be = cpu_to_be32(word);
+			word = *(u32 *)&word_be;
+		} else {
+			word_le = cpu_to_le32(word);
+			word = *(u32 *)&word_le;
+		}
+
+		W_REG(&regs->tplatewrdata, word);
+
+		buf = (u8 *) buf + sizeof(u32);
+		len -= sizeof(u32);
+	}
+}
+
+static void brcms_b_set_cwmin(struct brcms_hardware *wlc_hw, u16 newmin)
+{
+	wlc_hw->band->CWmin = newmin;
+
+	W_REG(&wlc_hw->regs->objaddr, OBJADDR_SCR_SEL | S_DOT11_CWMIN);
+	(void)R_REG(&wlc_hw->regs->objaddr);
+	W_REG(&wlc_hw->regs->objdata, newmin);
+}
+
+static void brcms_b_set_cwmax(struct brcms_hardware *wlc_hw, u16 newmax)
+{
+	wlc_hw->band->CWmax = newmax;
+
+	W_REG(&wlc_hw->regs->objaddr, OBJADDR_SCR_SEL | S_DOT11_CWMAX);
+	(void)R_REG(&wlc_hw->regs->objaddr);
+	W_REG(&wlc_hw->regs->objdata, newmax);
+}
+
+void brcms_b_bw_set(struct brcms_hardware *wlc_hw, u16 bw)
+{
+	bool fastclk;
+
+	/* request FAST clock if not on */
+	fastclk = wlc_hw->forcefastclk;
+	if (!fastclk)
+		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+
+	wlc_phy_bw_state_set(wlc_hw->band->pi, bw);
+
+	brcms_b_phy_reset(wlc_hw);
+	wlc_phy_init(wlc_hw->band->pi, wlc_phy_chanspec_get(wlc_hw->band->pi));
+
+	/* restore the clk */
+	if (!fastclk)
+		brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
+}
+
+static void brcms_b_upd_synthpu(struct brcms_hardware *wlc_hw)
+{
+	u16 v;
+	struct brcms_c_info *wlc = wlc_hw->wlc;
+	/* update SYNTHPU_DLY */
+
+	if (BRCMS_ISLCNPHY(wlc->band))
+		v = SYNTHPU_DLY_LPPHY_US;
+	else if (BRCMS_ISNPHY(wlc->band) && (NREV_GE(wlc->band->phyrev, 3)))
+		v = SYNTHPU_DLY_NPHY_US;
+	else
+		v = SYNTHPU_DLY_BPHY_US;
+
+	brcms_b_write_shm(wlc_hw, M_SYNTHPU_DLY, v);
+}
+
+static void brcms_c_ucode_txant_set(struct brcms_hardware *wlc_hw)
+{
+	u16 phyctl;
+	u16 phytxant = wlc_hw->bmac_phytxant;
+	u16 mask = PHY_TXC_ANT_MASK;
+
+	/* set the Probe Response frame phy control word */
+	phyctl = brcms_b_read_shm(wlc_hw, M_CTXPRS_BLK + C_CTX_PCTLWD_POS);
+	phyctl = (phyctl & ~mask) | phytxant;
+	brcms_b_write_shm(wlc_hw, M_CTXPRS_BLK + C_CTX_PCTLWD_POS, phyctl);
+
+	/* set the Response (ACK/CTS) frame phy control word */
+	phyctl = brcms_b_read_shm(wlc_hw, M_RSP_PCTLWD);
+	phyctl = (phyctl & ~mask) | phytxant;
+	brcms_b_write_shm(wlc_hw, M_RSP_PCTLWD, phyctl);
+}
+
+static u16 brcms_b_ofdm_ratetable_offset(struct brcms_hardware *wlc_hw,
+					 u8 rate)
+{
+	uint i;
+	u8 plcp_rate = 0;
+	struct plcp_signal_rate_lookup {
+		u8 rate;
+		u8 signal_rate;
+	};
+	/* OFDM RATE sub-field of PLCP SIGNAL field, per 802.11 sec 17.3.4.1 */
+	const struct plcp_signal_rate_lookup rate_lookup[] = {
+		{BRCM_RATE_6M, 0xB},
+		{BRCM_RATE_9M, 0xF},
+		{BRCM_RATE_12M, 0xA},
+		{BRCM_RATE_18M, 0xE},
+		{BRCM_RATE_24M, 0x9},
+		{BRCM_RATE_36M, 0xD},
+		{BRCM_RATE_48M, 0x8},
+		{BRCM_RATE_54M, 0xC}
+	};
+
+	for (i = 0; i < ARRAY_SIZE(rate_lookup); i++) {
+		if (rate == rate_lookup[i].rate) {
+			plcp_rate = rate_lookup[i].signal_rate;
+			break;
+		}
+	}
+
+	/* Find the SHM pointer to the rate table entry by looking in the
+	 * Direct-map Table
+	 */
+	return 2 * brcms_b_read_shm(wlc_hw, M_RT_DIRMAP_A + (plcp_rate * 2));
+}
+
+static void brcms_upd_ofdm_pctl1_table(struct brcms_hardware *wlc_hw)
+{
+	u8 rate;
+	u8 rates[8] = {
+		BRCM_RATE_6M, BRCM_RATE_9M, BRCM_RATE_12M, BRCM_RATE_18M,
+		BRCM_RATE_24M, BRCM_RATE_36M, BRCM_RATE_48M, BRCM_RATE_54M
+	};
+	u16 entry_ptr;
+	u16 pctl1;
+	uint i;
+
+	if (!BRCMS_PHY_11N_CAP(wlc_hw->band))
+		return;
+
+	/* walk the phy rate table and update the entries */
+	for (i = 0; i < ARRAY_SIZE(rates); i++) {
+		rate = rates[i];
+
+		entry_ptr = brcms_b_ofdm_ratetable_offset(wlc_hw, rate);
+
+		/* read the SHM Rate Table entry OFDM PCTL1 values */
+		pctl1 =
+		    brcms_b_read_shm(wlc_hw, entry_ptr + M_RT_OFDM_PCTL1_POS);
+
+		/* modify the value */
+		pctl1 &= ~PHY_TXC1_MODE_MASK;
+		pctl1 |= (wlc_hw->hw_stf_ss_opmode << PHY_TXC1_MODE_SHIFT);
+
+		/* Update the SHM Rate Table entry OFDM PCTL1 values */
+		brcms_b_write_shm(wlc_hw, entry_ptr + M_RT_OFDM_PCTL1_POS,
+				   pctl1);
+	}
+}
+
+/* band-specific init */
+static void brcms_b_bsinit(struct brcms_c_info *wlc, u16 chanspec)
+{
+	struct brcms_hardware *wlc_hw = wlc->hw;
+
+	BCMMSG(wlc->wiphy, "wl%d: bandunit %d\n", wlc_hw->unit,
+		wlc_hw->band->bandunit);
+
+	brcms_c_ucode_bsinit(wlc_hw);
+
+	wlc_phy_init(wlc_hw->band->pi, chanspec);
+
+	brcms_c_ucode_txant_set(wlc_hw);
+
+	/*
+	 * cwmin is band-specific, update hardware
+	 * with value for current band
+	 */
+	brcms_b_set_cwmin(wlc_hw, wlc_hw->band->CWmin);
+	brcms_b_set_cwmax(wlc_hw, wlc_hw->band->CWmax);
+
+	brcms_b_update_slot_timing(wlc_hw,
+				   wlc_hw->band->bandtype == BRCM_BAND_5G ?
+				   true : wlc_hw->shortslot);
+
+	/* write phytype and phyvers */
+	brcms_b_write_shm(wlc_hw, M_PHYTYPE, (u16) wlc_hw->band->phytype);
+	brcms_b_write_shm(wlc_hw, M_PHYVER, (u16) wlc_hw->band->phyrev);
+
+	/*
+	 * initialize the txphyctl1 rate table since
+	 * shmem is shared between bands
+	 */
+	brcms_upd_ofdm_pctl1_table(wlc_hw);
+
+	brcms_b_upd_synthpu(wlc_hw);
+}
+
+/* Perform a soft reset of the PHY PLL */
+void brcms_b_core_phypll_reset(struct brcms_hardware *wlc_hw)
+{
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
+
+	ai_corereg(wlc_hw->sih, SI_CC_IDX,
+		   offsetof(struct chipcregs, chipcontrol_addr), ~0, 0);
+	udelay(1);
+	ai_corereg(wlc_hw->sih, SI_CC_IDX,
+		   offsetof(struct chipcregs, chipcontrol_data), 0x4, 0);
+	udelay(1);
+	ai_corereg(wlc_hw->sih, SI_CC_IDX,
+		   offsetof(struct chipcregs, chipcontrol_data), 0x4, 4);
+	udelay(1);
+	ai_corereg(wlc_hw->sih, SI_CC_IDX,
+		   offsetof(struct chipcregs, chipcontrol_data), 0x4, 0);
+	udelay(1);
+}
+
+/* light way to turn on phy clock without reset for NPHY only
+ *  refer to brcms_b_core_phy_clk for full version
+ */
+void brcms_b_phyclk_fgc(struct brcms_hardware *wlc_hw, bool clk)
+{
+	/* support(necessary for NPHY and HYPHY) only */
+	if (!BRCMS_ISNPHY(wlc_hw->band))
+		return;
+
+	if (ON == clk)
+		ai_core_cflags(wlc_hw->sih, SICF_FGC, SICF_FGC);
+	else
+		ai_core_cflags(wlc_hw->sih, SICF_FGC, 0);
+
+}
+
+void brcms_b_macphyclk_set(struct brcms_hardware *wlc_hw, bool clk)
+{
+	if (ON == clk)
+		ai_core_cflags(wlc_hw->sih, SICF_MPCLKE, SICF_MPCLKE);
+	else
+		ai_core_cflags(wlc_hw->sih, SICF_MPCLKE, 0);
+}
+
+void brcms_b_phy_reset(struct brcms_hardware *wlc_hw)
+{
+	struct brcms_phy_pub *pih = wlc_hw->band->pi;
+	u32 phy_bw_clkbits;
+	bool phy_in_reset = false;
+
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
+
+	if (pih == NULL)
+		return;
+
+	phy_bw_clkbits = wlc_phy_clk_bwbits(wlc_hw->band->pi);
+
+	/* Specific reset sequence required for NPHY rev 3 and 4 */
+	if (BRCMS_ISNPHY(wlc_hw->band) && NREV_GE(wlc_hw->band->phyrev, 3) &&
+	    NREV_LE(wlc_hw->band->phyrev, 4)) {
+		/* Set the PHY bandwidth */
+		ai_core_cflags(wlc_hw->sih, SICF_BWMASK, phy_bw_clkbits);
+
+		udelay(1);
+
+		/* Perform a soft reset of the PHY PLL */
+		brcms_b_core_phypll_reset(wlc_hw);
+
+		/* reset the PHY */
+		ai_core_cflags(wlc_hw->sih, (SICF_PRST | SICF_PCLKE),
+			       (SICF_PRST | SICF_PCLKE));
+		phy_in_reset = true;
+	} else {
+		ai_core_cflags(wlc_hw->sih,
+			       (SICF_PRST | SICF_PCLKE | SICF_BWMASK),
+			       (SICF_PRST | SICF_PCLKE | phy_bw_clkbits));
+	}
+
+	udelay(2);
+	brcms_b_core_phy_clk(wlc_hw, ON);
+
+	if (pih)
+		wlc_phy_anacore(pih, ON);
+}
+
+/* switch to and initialize new band */
+static void brcms_b_setband(struct brcms_hardware *wlc_hw, uint bandunit,
+			    u16 chanspec) {
+	struct brcms_c_info *wlc = wlc_hw->wlc;
+	u32 macintmask;
+
+	/* Enable the d11 core before accessing it */
+	if (!ai_iscoreup(wlc_hw->sih)) {
+		ai_core_reset(wlc_hw->sih, 0, 0);
+		brcms_c_mctrl_reset(wlc_hw);
+	}
+
+	macintmask = brcms_c_setband_inact(wlc, bandunit);
+
+	if (!wlc_hw->up)
+		return;
+
+	brcms_b_core_phy_clk(wlc_hw, ON);
+
+	/* band-specific initializations */
+	brcms_b_bsinit(wlc, chanspec);
+
+	/*
+	 * If there are any pending software interrupt bits,
+	 * then replace these with a harmless nonzero value
+	 * so brcms_c_dpc() will re-enable interrupts when done.
+	 */
+	if (wlc->macintstatus)
+		wlc->macintstatus = MI_DMAINT;
+
+	/* restore macintmask */
+	brcms_intrsrestore(wlc->wl, macintmask);
+
+	/* ucode should still be suspended.. */
+	WARN_ON((R_REG(&wlc_hw->regs->maccontrol) & MCTL_EN_MAC) != 0);
+}
+
+static bool brcms_c_isgoodchip(struct brcms_hardware *wlc_hw)
+{
+
+	/* reject unsupported corerev */
+	if (!CONF_HAS(D11CONF, wlc_hw->corerev)) {
+		wiphy_err(wlc_hw->wlc->wiphy, "unsupported core rev %d\n",
+			  wlc_hw->corerev);
+		return false;
+	}
+
+	return true;
+}
+
+/* Validate some board info parameters */
+static bool brcms_c_validboardtype(struct brcms_hardware *wlc_hw)
+{
+	uint boardrev = wlc_hw->boardrev;
+
+	/* 4 bits each for board type, major, minor, and tiny version */
+	uint brt = (boardrev & 0xf000) >> 12;
+	uint b0 = (boardrev & 0xf00) >> 8;
+	uint b1 = (boardrev & 0xf0) >> 4;
+	uint b2 = boardrev & 0xf;
+
+	/* voards from other vendors are always considered valid */
+	if (wlc_hw->sih->boardvendor != PCI_VENDOR_ID_BROADCOM)
+		return true;
+
+	/* do some boardrev sanity checks when boardvendor is Broadcom */
+	if (boardrev == 0)
+		return false;
+
+	if (boardrev <= 0xff)
+		return true;
+
+	if ((brt > 2) || (brt == 0) || (b0 > 9) || (b0 == 0) || (b1 > 9)
+		|| (b2 > 9))
+		return false;
+
+	return true;
+}
+
+static char *brcms_c_get_macaddr(struct brcms_hardware *wlc_hw)
+{
+	enum brcms_srom_id var_id = BRCMS_SROM_MACADDR;
+	char *macaddr;
+
+	/* If macaddr exists, use it (Sromrev4, CIS, ...). */
+	macaddr = getvar(wlc_hw->sih, var_id);
+	if (macaddr != NULL)
+		return macaddr;
+
+	if (wlc_hw->_nbands > 1)
+		var_id = BRCMS_SROM_ET1MACADDR;
+	else
+		var_id = BRCMS_SROM_IL0MACADDR;
+
+	macaddr = getvar(wlc_hw->sih, var_id);
+	if (macaddr == NULL)
+		wiphy_err(wlc_hw->wlc->wiphy, "wl%d: wlc_get_macaddr: macaddr "
+			  "getvar(%d) not found\n", wlc_hw->unit, var_id);
+
+	return macaddr;
+}
+
+/* power both the pll and external oscillator on/off */
+static void brcms_b_xtal(struct brcms_hardware *wlc_hw, bool want)
+{
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d: want %d\n", wlc_hw->unit, want);
+
+	/*
+	 * dont power down if plldown is false or
+	 * we must poll hw radio disable
+	 */
+	if (!want && wlc_hw->pllreq)
+		return;
+
+	if (wlc_hw->sih)
+		ai_clkctl_xtal(wlc_hw->sih, XTAL | PLL, want);
+
+	wlc_hw->sbclk = want;
+	if (!wlc_hw->sbclk) {
+		wlc_hw->clk = false;
+		if (wlc_hw->band && wlc_hw->band->pi)
+			wlc_phy_hw_clk_state_upd(wlc_hw->band->pi, false);
+	}
+}
+
+/*
+ * Return true if radio is disabled, otherwise false.
+ * hw radio disable signal is an external pin, users activate it asynchronously
+ * this function could be called when driver is down and w/o clock
+ * it operates on different registers depending on corerev and boardflag.
+ */
+static bool brcms_b_radio_read_hwdisabled(struct brcms_hardware *wlc_hw)
+{
+	bool v, clk, xtal;
+	u32 resetbits = 0, flags = 0;
+
+	xtal = wlc_hw->sbclk;
+	if (!xtal)
+		brcms_b_xtal(wlc_hw, ON);
+
+	/* may need to take core out of reset first */
+	clk = wlc_hw->clk;
+	if (!clk) {
+		/*
+		 * mac no longer enables phyclk automatically when driver
+		 * accesses phyreg throughput mac. This can be skipped since
+		 * only mac reg is accessed below
+		 */
+		flags |= SICF_PCLKE;
+
+		/*
+		 * AI chip doesn't restore bar0win2 on
+		 * hibernation/resume, need sw fixup
+		 */
+		if ((wlc_hw->sih->chip == BCM43224_CHIP_ID) ||
+		    (wlc_hw->sih->chip == BCM43225_CHIP_ID))
+			wlc_hw->regs = (struct d11regs __iomem *)
+					ai_setcore(wlc_hw->sih, D11_CORE_ID, 0);
+		ai_core_reset(wlc_hw->sih, flags, resetbits);
+		brcms_c_mctrl_reset(wlc_hw);
+	}
+
+	v = ((R_REG(&wlc_hw->regs->phydebug) & PDBG_RFD) != 0);
+
+	/* put core back into reset */
+	if (!clk)
+		ai_core_disable(wlc_hw->sih, 0);
+
+	if (!xtal)
+		brcms_b_xtal(wlc_hw, OFF);
+
+	return v;
+}
+
+static bool wlc_dma_rxreset(struct brcms_hardware *wlc_hw, uint fifo)
+{
+	struct dma_pub *di = wlc_hw->di[fifo];
+	return dma_rxreset(di);
+}
+
+/* d11 core reset
+ *   ensure fask clock during reset
+ *   reset dma
+ *   reset d11(out of reset)
+ *   reset phy(out of reset)
+ *   clear software macintstatus for fresh new start
+ * one testing hack wlc_hw->noreset will bypass the d11/phy reset
+ */
+void brcms_b_corereset(struct brcms_hardware *wlc_hw, u32 flags)
+{
+	struct d11regs __iomem *regs;
+	uint i;
+	bool fastclk;
+	u32 resetbits = 0;
+
+	if (flags == BRCMS_USE_COREFLAGS)
+		flags = (wlc_hw->band->pi ? wlc_hw->band->core_flags : 0);
+
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
+
+	regs = wlc_hw->regs;
+
+	/* request FAST clock if not on  */
+	fastclk = wlc_hw->forcefastclk;
+	if (!fastclk)
+		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+
+	/* reset the dma engines except first time thru */
+	if (ai_iscoreup(wlc_hw->sih)) {
+		for (i = 0; i < NFIFO; i++)
+			if ((wlc_hw->di[i]) && (!dma_txreset(wlc_hw->di[i])))
+				wiphy_err(wlc_hw->wlc->wiphy, "wl%d: %s: "
+					  "dma_txreset[%d]: cannot stop dma\n",
+					   wlc_hw->unit, __func__, i);
+
+		if ((wlc_hw->di[RX_FIFO])
+		    && (!wlc_dma_rxreset(wlc_hw, RX_FIFO)))
+			wiphy_err(wlc_hw->wlc->wiphy, "wl%d: %s: dma_rxreset"
+				  "[%d]: cannot stop dma\n",
+				  wlc_hw->unit, __func__, RX_FIFO);
+	}
+	/* if noreset, just stop the psm and return */
+	if (wlc_hw->noreset) {
+		wlc_hw->wlc->macintstatus = 0;	/* skip wl_dpc after down */
+		brcms_b_mctrl(wlc_hw, MCTL_PSM_RUN | MCTL_EN_MAC, 0);
+		return;
+	}
+
+	/*
+	 * mac no longer enables phyclk automatically when driver accesses
+	 * phyreg throughput mac, AND phy_reset is skipped at early stage when
+	 * band->pi is invalid. need to enable PHY CLK
+	 */
+	flags |= SICF_PCLKE;
+
+	/*
+	 * reset the core
+	 * In chips with PMU, the fastclk request goes through d11 core
+	 * reg 0x1e0, which is cleared by the core_reset. have to re-request it.
+	 *
+	 * This adds some delay and we can optimize it by also requesting
+	 * fastclk through chipcommon during this period if necessary. But
+	 * that has to work coordinate with other driver like mips/arm since
+	 * they may touch chipcommon as well.
+	 */
+	wlc_hw->clk = false;
+	ai_core_reset(wlc_hw->sih, flags, resetbits);
+	wlc_hw->clk = true;
+	if (wlc_hw->band && wlc_hw->band->pi)
+		wlc_phy_hw_clk_state_upd(wlc_hw->band->pi, true);
+
+	brcms_c_mctrl_reset(wlc_hw);
+
+	if (wlc_hw->sih->cccaps & CC_CAP_PMU)
+		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+
+	brcms_b_phy_reset(wlc_hw);
+
+	/* turn on PHY_PLL */
+	brcms_b_core_phypll_ctl(wlc_hw, true);
+
+	/* clear sw intstatus */
+	wlc_hw->wlc->macintstatus = 0;
+
+	/* restore the clk setting */
+	if (!fastclk)
+		brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
+}
+
+/* txfifo sizes needs to be modified(increased) since the newer cores
+ * have more memory.
+ */
+static void brcms_b_corerev_fifofixup(struct brcms_hardware *wlc_hw)
+{
+	struct d11regs __iomem *regs = wlc_hw->regs;
+	u16 fifo_nu;
+	u16 txfifo_startblk = TXFIFO_START_BLK, txfifo_endblk;
+	u16 txfifo_def, txfifo_def1;
+	u16 txfifo_cmd;
+
+	/* tx fifos start at TXFIFO_START_BLK from the Base address */
+	txfifo_startblk = TXFIFO_START_BLK;
+
+	/* sequence of operations:  reset fifo, set fifo size, reset fifo */
+	for (fifo_nu = 0; fifo_nu < NFIFO; fifo_nu++) {
+
+		txfifo_endblk = txfifo_startblk + wlc_hw->xmtfifo_sz[fifo_nu];
+		txfifo_def = (txfifo_startblk & 0xff) |
+		    (((txfifo_endblk - 1) & 0xff) << TXFIFO_FIFOTOP_SHIFT);
+		txfifo_def1 = ((txfifo_startblk >> 8) & 0x1) |
+		    ((((txfifo_endblk -
+			1) >> 8) & 0x1) << TXFIFO_FIFOTOP_SHIFT);
+		txfifo_cmd =
+		    TXFIFOCMD_RESET_MASK | (fifo_nu << TXFIFOCMD_FIFOSEL_SHIFT);
+
+		W_REG(&regs->xmtfifocmd, txfifo_cmd);
+		W_REG(&regs->xmtfifodef, txfifo_def);
+		W_REG(&regs->xmtfifodef1, txfifo_def1);
+
+		W_REG(&regs->xmtfifocmd, txfifo_cmd);
+
+		txfifo_startblk += wlc_hw->xmtfifo_sz[fifo_nu];
+	}
+	/*
+	 * need to propagate to shm location to be in sync since ucode/hw won't
+	 * do this
+	 */
+	brcms_b_write_shm(wlc_hw, M_FIFOSIZE0,
+			   wlc_hw->xmtfifo_sz[TX_AC_BE_FIFO]);
+	brcms_b_write_shm(wlc_hw, M_FIFOSIZE1,
+			   wlc_hw->xmtfifo_sz[TX_AC_VI_FIFO]);
+	brcms_b_write_shm(wlc_hw, M_FIFOSIZE2,
+			   ((wlc_hw->xmtfifo_sz[TX_AC_VO_FIFO] << 8) | wlc_hw->
+			    xmtfifo_sz[TX_AC_BK_FIFO]));
+	brcms_b_write_shm(wlc_hw, M_FIFOSIZE3,
+			   ((wlc_hw->xmtfifo_sz[TX_ATIM_FIFO] << 8) | wlc_hw->
+			    xmtfifo_sz[TX_BCMC_FIFO]));
+}
+
+/* This function is used for changing the tsf frac register
+ * If spur avoidance mode is off, the mac freq will be 80/120/160Mhz
+ * If spur avoidance mode is on1, the mac freq will be 82/123/164Mhz
+ * If spur avoidance mode is on2, the mac freq will be 84/126/168Mhz
+ * HTPHY Formula is 2^26/freq(MHz) e.g.
+ * For spuron2 - 126MHz -> 2^26/126 = 532610.0
+ *  - 532610 = 0x82082 => tsf_clk_frac_h = 0x8, tsf_clk_frac_l = 0x2082
+ * For spuron: 123MHz -> 2^26/123    = 545600.5
+ *  - 545601 = 0x85341 => tsf_clk_frac_h = 0x8, tsf_clk_frac_l = 0x5341
+ * For spur off: 120MHz -> 2^26/120    = 559240.5
+ *  - 559241 = 0x88889 => tsf_clk_frac_h = 0x8, tsf_clk_frac_l = 0x8889
+ */
+
+void brcms_b_switch_macfreq(struct brcms_hardware *wlc_hw, u8 spurmode)
+{
+	struct d11regs __iomem *regs = wlc_hw->regs;
+
+	if ((wlc_hw->sih->chip == BCM43224_CHIP_ID) ||
+	    (wlc_hw->sih->chip == BCM43225_CHIP_ID)) {
+		if (spurmode == WL_SPURAVOID_ON2) {	/* 126Mhz */
+			W_REG(&regs->tsf_clk_frac_l, 0x2082);
+			W_REG(&regs->tsf_clk_frac_h, 0x8);
+		} else if (spurmode == WL_SPURAVOID_ON1) {	/* 123Mhz */
+			W_REG(&regs->tsf_clk_frac_l, 0x5341);
+			W_REG(&regs->tsf_clk_frac_h, 0x8);
+		} else {	/* 120Mhz */
+			W_REG(&regs->tsf_clk_frac_l, 0x8889);
+			W_REG(&regs->tsf_clk_frac_h, 0x8);
+		}
+	} else if (BRCMS_ISLCNPHY(wlc_hw->band)) {
+		if (spurmode == WL_SPURAVOID_ON1) {	/* 82Mhz */
+			W_REG(&regs->tsf_clk_frac_l, 0x7CE0);
+			W_REG(&regs->tsf_clk_frac_h, 0xC);
+		} else {	/* 80Mhz */
+			W_REG(&regs->tsf_clk_frac_l, 0xCCCD);
+			W_REG(&regs->tsf_clk_frac_h, 0xC);
+		}
+	}
+}
+
+/* Initialize GPIOs that are controlled by D11 core */
+static void brcms_c_gpio_init(struct brcms_c_info *wlc)
+{
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	struct d11regs __iomem *regs;
+	u32 gc, gm;
+
+	regs = wlc_hw->regs;
+
+	/* use GPIO select 0 to get all gpio signals from the gpio out reg */
+	brcms_b_mctrl(wlc_hw, MCTL_GPOUT_SEL_MASK, 0);
+
+	/*
+	 * Common GPIO setup:
+	 *      G0 = LED 0 = WLAN Activity
+	 *      G1 = LED 1 = WLAN 2.4 GHz Radio State
+	 *      G2 = LED 2 = WLAN 5 GHz Radio State
+	 *      G4 = radio disable input (HI enabled, LO disabled)
+	 */
+
+	gc = gm = 0;
+
+	/* Allocate GPIOs for mimo antenna diversity feature */
+	if (wlc_hw->antsel_type == ANTSEL_2x3) {
+		/* Enable antenna diversity, use 2x3 mode */
+		brcms_b_mhf(wlc_hw, MHF3, MHF3_ANTSEL_EN,
+			     MHF3_ANTSEL_EN, BRCM_BAND_ALL);
+		brcms_b_mhf(wlc_hw, MHF3, MHF3_ANTSEL_MODE,
+			     MHF3_ANTSEL_MODE, BRCM_BAND_ALL);
+
+		/* init superswitch control */
+		wlc_phy_antsel_init(wlc_hw->band->pi, false);
+
+	} else if (wlc_hw->antsel_type == ANTSEL_2x4) {
+		gm |= gc |= (BOARD_GPIO_12 | BOARD_GPIO_13);
+		/*
+		 * The board itself is powered by these GPIOs
+		 * (when not sending pattern) so set them high
+		 */
+		OR_REG(&regs->psm_gpio_oe,
+		       (BOARD_GPIO_12 | BOARD_GPIO_13));
+		OR_REG(&regs->psm_gpio_out,
+		       (BOARD_GPIO_12 | BOARD_GPIO_13));
+
+		/* Enable antenna diversity, use 2x4 mode */
+		brcms_b_mhf(wlc_hw, MHF3, MHF3_ANTSEL_EN,
+			     MHF3_ANTSEL_EN, BRCM_BAND_ALL);
+		brcms_b_mhf(wlc_hw, MHF3, MHF3_ANTSEL_MODE, 0,
+			     BRCM_BAND_ALL);
+
+		/* Configure the desired clock to be 4Mhz */
+		brcms_b_write_shm(wlc_hw, M_ANTSEL_CLKDIV,
+				   ANTSEL_CLKDIV_4MHZ);
+	}
+
+	/*
+	 * gpio 9 controls the PA. ucode is responsible
+	 * for wiggling out and oe
+	 */
+	if (wlc_hw->boardflags & BFL_PACTRL)
+		gm |= gc |= BOARD_GPIO_PACTRL;
+
+	/* apply to gpiocontrol register */
+	ai_gpiocontrol(wlc_hw->sih, gm, gc, GPIO_DRV_PRIORITY);
+}
+
+static void brcms_ucode_write(struct brcms_hardware *wlc_hw,
+			      const __le32 ucode[], const size_t nbytes)
+{
+	struct d11regs __iomem *regs = wlc_hw->regs;
+	uint i;
+	uint count;
+
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
+
+	count = (nbytes / sizeof(u32));
+
+	W_REG(&regs->objaddr, (OBJADDR_AUTO_INC | OBJADDR_UCM_SEL));
+	(void)R_REG(&regs->objaddr);
+	for (i = 0; i < count; i++)
+		W_REG(&regs->objdata, le32_to_cpu(ucode[i]));
+
+}
+
+static void brcms_ucode_download(struct brcms_hardware *wlc_hw)
+{
+	struct brcms_c_info *wlc;
+	struct brcms_ucode *ucode = &wlc_hw->wlc->wl->ucode;
+
+	wlc = wlc_hw->wlc;
+
+	if (wlc_hw->ucode_loaded)
+		return;
+
+	if (D11REV_IS(wlc_hw->corerev, 23)) {
+		if (BRCMS_ISNPHY(wlc_hw->band)) {
+			brcms_ucode_write(wlc_hw, ucode->bcm43xx_16_mimo,
+					  ucode->bcm43xx_16_mimosz);
+			wlc_hw->ucode_loaded = true;
+		} else
+			wiphy_err(wlc->wiphy, "%s: wl%d: unsupported phy in "
+				  "corerev %d\n",
+				  __func__, wlc_hw->unit, wlc_hw->corerev);
+	} else if (D11REV_IS(wlc_hw->corerev, 24)) {
+		if (BRCMS_ISLCNPHY(wlc_hw->band)) {
+			brcms_ucode_write(wlc_hw, ucode->bcm43xx_24_lcn,
+					  ucode->bcm43xx_24_lcnsz);
+			wlc_hw->ucode_loaded = true;
+		} else {
+			wiphy_err(wlc->wiphy, "%s: wl%d: unsupported phy in "
+				  "corerev %d\n",
+				  __func__, wlc_hw->unit, wlc_hw->corerev);
+		}
+	}
+}
+
+void brcms_b_txant_set(struct brcms_hardware *wlc_hw, u16 phytxant)
+{
+	/* update sw state */
+	wlc_hw->bmac_phytxant = phytxant;
+
+	/* push to ucode if up */
+	if (!wlc_hw->up)
+		return;
+	brcms_c_ucode_txant_set(wlc_hw);
+
+}
+
+u16 brcms_b_get_txant(struct brcms_hardware *wlc_hw)
+{
+	return (u16) wlc_hw->wlc->stf->txant;
+}
+
+void brcms_b_antsel_type_set(struct brcms_hardware *wlc_hw, u8 antsel_type)
+{
+	wlc_hw->antsel_type = antsel_type;
+
+	/* Update the antsel type for phy module to use */
+	wlc_phy_antsel_type_set(wlc_hw->band->pi, antsel_type);
+}
+
+static void brcms_c_fatal_error(struct brcms_c_info *wlc)
+{
+	wiphy_err(wlc->wiphy, "wl%d: fatal error, reinitializing\n",
+		  wlc->pub->unit);
+	brcms_init(wlc->wl);
+}
+
+static void brcms_b_fifoerrors(struct brcms_hardware *wlc_hw)
+{
+	bool fatal = false;
+	uint unit;
+	uint intstatus, idx;
+	struct d11regs __iomem *regs = wlc_hw->regs;
+	struct wiphy *wiphy = wlc_hw->wlc->wiphy;
+
+	unit = wlc_hw->unit;
+
+	for (idx = 0; idx < NFIFO; idx++) {
+		/* read intstatus register and ignore any non-error bits */
+		intstatus =
+		    R_REG(&regs->intctrlregs[idx].intstatus) & I_ERRORS;
+		if (!intstatus)
+			continue;
+
+		BCMMSG(wlc_hw->wlc->wiphy, "wl%d: intstatus%d 0x%x\n",
+			unit, idx, intstatus);
+
+		if (intstatus & I_RO) {
+			wiphy_err(wiphy, "wl%d: fifo %d: receive fifo "
+				  "overflow\n", unit, idx);
+			fatal = true;
+		}
+
+		if (intstatus & I_PC) {
+			wiphy_err(wiphy, "wl%d: fifo %d: descriptor error\n",
+				 unit, idx);
+			fatal = true;
+		}
+
+		if (intstatus & I_PD) {
+			wiphy_err(wiphy, "wl%d: fifo %d: data error\n", unit,
+				  idx);
+			fatal = true;
+		}
+
+		if (intstatus & I_DE) {
+			wiphy_err(wiphy, "wl%d: fifo %d: descriptor protocol "
+				  "error\n", unit, idx);
+			fatal = true;
+		}
+
+		if (intstatus & I_RU)
+			wiphy_err(wiphy, "wl%d: fifo %d: receive descriptor "
+				  "underflow\n", idx, unit);
+
+		if (intstatus & I_XU) {
+			wiphy_err(wiphy, "wl%d: fifo %d: transmit fifo "
+				  "underflow\n", idx, unit);
+			fatal = true;
+		}
+
+		if (fatal) {
+			brcms_c_fatal_error(wlc_hw->wlc);	/* big hammer */
+			break;
+		} else
+			W_REG(&regs->intctrlregs[idx].intstatus,
+			      intstatus);
+	}
+}
+
+void brcms_c_intrson(struct brcms_c_info *wlc)
+{
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	wlc->macintmask = wlc->defmacintmask;
+	W_REG(&wlc_hw->regs->macintmask, wlc->macintmask);
+}
+
+/*
+ * callback for siutils.c, which has only wlc handler, no wl they both check
+ * up, not only because there is no need to off/restore d11 interrupt but also
+ * because per-port code may require sync with valid interrupt.
+ */
+static u32 brcms_c_wlintrsoff(struct brcms_c_info *wlc)
+{
+	if (!wlc->hw->up)
+		return 0;
+
+	return brcms_intrsoff(wlc->wl);
+}
+
+static void brcms_c_wlintrsrestore(struct brcms_c_info *wlc, u32 macintmask)
+{
+	if (!wlc->hw->up)
+		return;
+
+	brcms_intrsrestore(wlc->wl, macintmask);
+}
+
+u32 brcms_c_intrsoff(struct brcms_c_info *wlc)
+{
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	u32 macintmask;
+
+	if (!wlc_hw->clk)
+		return 0;
+
+	macintmask = wlc->macintmask;	/* isr can still happen */
+
+	W_REG(&wlc_hw->regs->macintmask, 0);
+	(void)R_REG(&wlc_hw->regs->macintmask);	/* sync readback */
+	udelay(1);		/* ensure int line is no longer driven */
+	wlc->macintmask = 0;
+
+	/* return previous macintmask; resolve race between us and our isr */
+	return wlc->macintstatus ? 0 : macintmask;
+}
+
+void brcms_c_intrsrestore(struct brcms_c_info *wlc, u32 macintmask)
+{
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	if (!wlc_hw->clk)
+		return;
+
+	wlc->macintmask = macintmask;
+	W_REG(&wlc_hw->regs->macintmask, wlc->macintmask);
+}
+
+static void brcms_b_tx_fifo_suspend(struct brcms_hardware *wlc_hw,
+				    uint tx_fifo)
+{
+	u8 fifo = 1 << tx_fifo;
+
+	/* Two clients of this code, 11h Quiet period and scanning. */
+
+	/* only suspend if not already suspended */
+	if ((wlc_hw->suspended_fifos & fifo) == fifo)
+		return;
+
+	/* force the core awake only if not already */
+	if (wlc_hw->suspended_fifos == 0)
+		brcms_c_ucode_wake_override_set(wlc_hw,
+						BRCMS_WAKE_OVERRIDE_TXFIFO);
+
+	wlc_hw->suspended_fifos |= fifo;
+
+	if (wlc_hw->di[tx_fifo]) {
+		/*
+		 * Suspending AMPDU transmissions in the middle can cause
+		 * underflow which may result in mismatch between ucode and
+		 * driver so suspend the mac before suspending the FIFO
+		 */
+		if (BRCMS_PHY_11N_CAP(wlc_hw->band))
+			brcms_c_suspend_mac_and_wait(wlc_hw->wlc);
+
+		dma_txsuspend(wlc_hw->di[tx_fifo]);
+
+		if (BRCMS_PHY_11N_CAP(wlc_hw->band))
+			brcms_c_enable_mac(wlc_hw->wlc);
+	}
+}
+
+static void brcms_b_tx_fifo_resume(struct brcms_hardware *wlc_hw,
+				   uint tx_fifo)
+{
+	/* BMAC_NOTE: BRCMS_TX_FIFO_ENAB is done in brcms_c_dpc() for DMA case
+	 * but need to be done here for PIO otherwise the watchdog will catch
+	 * the inconsistency and fire
+	 */
+	/* Two clients of this code, 11h Quiet period and scanning. */
+	if (wlc_hw->di[tx_fifo])
+		dma_txresume(wlc_hw->di[tx_fifo]);
+
+	/* allow core to sleep again */
+	if (wlc_hw->suspended_fifos == 0)
+		return;
+	else {
+		wlc_hw->suspended_fifos &= ~(1 << tx_fifo);
+		if (wlc_hw->suspended_fifos == 0)
+			brcms_c_ucode_wake_override_clear(wlc_hw,
+						BRCMS_WAKE_OVERRIDE_TXFIFO);
+	}
+}
+
+static void brcms_b_mute(struct brcms_hardware *wlc_hw, bool on, u32 flags)
+{
+	static const u8 null_ether_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
+
+	if (on) {
+		/* suspend tx fifos */
+		brcms_b_tx_fifo_suspend(wlc_hw, TX_DATA_FIFO);
+		brcms_b_tx_fifo_suspend(wlc_hw, TX_CTL_FIFO);
+		brcms_b_tx_fifo_suspend(wlc_hw, TX_AC_BK_FIFO);
+		brcms_b_tx_fifo_suspend(wlc_hw, TX_AC_VI_FIFO);
+
+		/* zero the address match register so we do not send ACKs */
+		brcms_b_set_addrmatch(wlc_hw, RCM_MAC_OFFSET,
+				       null_ether_addr);
+	} else {
+		/* resume tx fifos */
+		brcms_b_tx_fifo_resume(wlc_hw, TX_DATA_FIFO);
+		brcms_b_tx_fifo_resume(wlc_hw, TX_CTL_FIFO);
+		brcms_b_tx_fifo_resume(wlc_hw, TX_AC_BK_FIFO);
+		brcms_b_tx_fifo_resume(wlc_hw, TX_AC_VI_FIFO);
+
+		/* Restore address */
+		brcms_b_set_addrmatch(wlc_hw, RCM_MAC_OFFSET,
+				       wlc_hw->etheraddr);
+	}
+
+	wlc_phy_mute_upd(wlc_hw->band->pi, on, flags);
+
+	if (on)
+		brcms_c_ucode_mute_override_set(wlc_hw);
+	else
+		brcms_c_ucode_mute_override_clear(wlc_hw);
+}
+
+/*
+ * Read and clear macintmask and macintstatus and intstatus registers.
+ * This routine should be called with interrupts off
+ * Return:
+ *   -1 if brcms_deviceremoved(wlc) evaluates to true;
+ *   0 if the interrupt is not for us, or we are in some special cases;
+ *   device interrupt status bits otherwise.
+ */
+static inline u32 wlc_intstatus(struct brcms_c_info *wlc, bool in_isr)
+{
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	struct d11regs __iomem *regs = wlc_hw->regs;
+	u32 macintstatus;
+
+	/* macintstatus includes a DMA interrupt summary bit */
+	macintstatus = R_REG(&regs->macintstatus);
+
+	BCMMSG(wlc->wiphy, "wl%d: macintstatus: 0x%x\n", wlc_hw->unit,
+		 macintstatus);
+
+	/* detect cardbus removed, in power down(suspend) and in reset */
+	if (brcms_deviceremoved(wlc))
+		return -1;
+
+	/* brcms_deviceremoved() succeeds even when the core is still resetting,
+	 * handle that case here.
+	 */
+	if (macintstatus == 0xffffffff)
+		return 0;
+
+	/* defer unsolicited interrupts */
+	macintstatus &= (in_isr ? wlc->macintmask : wlc->defmacintmask);
+
+	/* if not for us */
+	if (macintstatus == 0)
+		return 0;
+
+	/* interrupts are already turned off for CFE build
+	 * Caution: For CFE Turning off the interrupts again has some undesired
+	 * consequences
+	 */
+	/* turn off the interrupts */
+	W_REG(&regs->macintmask, 0);
+	(void)R_REG(&regs->macintmask);	/* sync readback */
+	wlc->macintmask = 0;
+
+	/* clear device interrupts */
+	W_REG(&regs->macintstatus, macintstatus);
+
+	/* MI_DMAINT is indication of non-zero intstatus */
+	if (macintstatus & MI_DMAINT)
+		/*
+		 * only fifo interrupt enabled is I_RI in
+		 * RX_FIFO. If MI_DMAINT is set, assume it
+		 * is set and clear the interrupt.
+		 */
+		W_REG(&regs->intctrlregs[RX_FIFO].intstatus,
+		      DEF_RXINTMASK);
+
+	return macintstatus;
+}
+
+/* Update wlc->macintstatus and wlc->intstatus[]. */
+/* Return true if they are updated successfully. false otherwise */
+bool brcms_c_intrsupd(struct brcms_c_info *wlc)
+{
+	u32 macintstatus;
+
+	/* read and clear macintstatus and intstatus registers */
+	macintstatus = wlc_intstatus(wlc, false);
+
+	/* device is removed */
+	if (macintstatus == 0xffffffff)
+		return false;
+
+	/* update interrupt status in software */
+	wlc->macintstatus |= macintstatus;
+
+	return true;
+}
+
+/*
+ * First-level interrupt processing.
+ * Return true if this was our interrupt, false otherwise.
+ * *wantdpc will be set to true if further brcms_c_dpc() processing is required,
+ * false otherwise.
+ */
+bool brcms_c_isr(struct brcms_c_info *wlc, bool *wantdpc)
+{
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	u32 macintstatus;
+
+	*wantdpc = false;
+
+	if (!wlc_hw->up || !wlc->macintmask)
+		return false;
+
+	/* read and clear macintstatus and intstatus registers */
+	macintstatus = wlc_intstatus(wlc, true);
+
+	if (macintstatus == 0xffffffff)
+		wiphy_err(wlc->wiphy, "DEVICEREMOVED detected in the ISR code"
+			  " path\n");
+
+	/* it is not for us */
+	if (macintstatus == 0)
+		return false;
+
+	*wantdpc = true;
+
+	/* save interrupt status bits */
+	wlc->macintstatus = macintstatus;
+
+	return true;
+
+}
+
+void brcms_c_suspend_mac_and_wait(struct brcms_c_info *wlc)
+{
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	struct d11regs __iomem *regs = wlc_hw->regs;
+	u32 mc, mi;
+	struct wiphy *wiphy = wlc->wiphy;
+
+	BCMMSG(wlc->wiphy, "wl%d: bandunit %d\n", wlc_hw->unit,
+		wlc_hw->band->bandunit);
+
+	/*
+	 * Track overlapping suspend requests
+	 */
+	wlc_hw->mac_suspend_depth++;
+	if (wlc_hw->mac_suspend_depth > 1)
+		return;
+
+	/* force the core awake */
+	brcms_c_ucode_wake_override_set(wlc_hw, BRCMS_WAKE_OVERRIDE_MACSUSPEND);
+
+	mc = R_REG(&regs->maccontrol);
+
+	if (mc == 0xffffffff) {
+		wiphy_err(wiphy, "wl%d: %s: dead chip\n", wlc_hw->unit,
+			  __func__);
+		brcms_down(wlc->wl);
+		return;
+	}
+	WARN_ON(mc & MCTL_PSM_JMP_0);
+	WARN_ON(!(mc & MCTL_PSM_RUN));
+	WARN_ON(!(mc & MCTL_EN_MAC));
+
+	mi = R_REG(&regs->macintstatus);
+	if (mi == 0xffffffff) {
+		wiphy_err(wiphy, "wl%d: %s: dead chip\n", wlc_hw->unit,
+			  __func__);
+		brcms_down(wlc->wl);
+		return;
+	}
+	WARN_ON(mi & MI_MACSSPNDD);
+
+	brcms_b_mctrl(wlc_hw, MCTL_EN_MAC, 0);
+
+	SPINWAIT(!(R_REG(&regs->macintstatus) & MI_MACSSPNDD),
+		 BRCMS_MAX_MAC_SUSPEND);
+
+	if (!(R_REG(&regs->macintstatus) & MI_MACSSPNDD)) {
+		wiphy_err(wiphy, "wl%d: wlc_suspend_mac_and_wait: waited %d uS"
+			  " and MI_MACSSPNDD is still not on.\n",
+			  wlc_hw->unit, BRCMS_MAX_MAC_SUSPEND);
+		wiphy_err(wiphy, "wl%d: psmdebug 0x%08x, phydebug 0x%08x, "
+			  "psm_brc 0x%04x\n", wlc_hw->unit,
+			  R_REG(&regs->psmdebug),
+			  R_REG(&regs->phydebug),
+			  R_REG(&regs->psm_brc));
+	}
+
+	mc = R_REG(&regs->maccontrol);
+	if (mc == 0xffffffff) {
+		wiphy_err(wiphy, "wl%d: %s: dead chip\n", wlc_hw->unit,
+			  __func__);
+		brcms_down(wlc->wl);
+		return;
+	}
+	WARN_ON(mc & MCTL_PSM_JMP_0);
+	WARN_ON(!(mc & MCTL_PSM_RUN));
+	WARN_ON(mc & MCTL_EN_MAC);
+}
+
+void brcms_c_enable_mac(struct brcms_c_info *wlc)
+{
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	struct d11regs __iomem *regs = wlc_hw->regs;
+	u32 mc, mi;
+
+	BCMMSG(wlc->wiphy, "wl%d: bandunit %d\n", wlc_hw->unit,
+		wlc->band->bandunit);
+
+	/*
+	 * Track overlapping suspend requests
+	 */
+	wlc_hw->mac_suspend_depth--;
+	if (wlc_hw->mac_suspend_depth > 0)
+		return;
+
+	mc = R_REG(&regs->maccontrol);
+	WARN_ON(mc & MCTL_PSM_JMP_0);
+	WARN_ON(mc & MCTL_EN_MAC);
+	WARN_ON(!(mc & MCTL_PSM_RUN));
+
+	brcms_b_mctrl(wlc_hw, MCTL_EN_MAC, MCTL_EN_MAC);
+	W_REG(&regs->macintstatus, MI_MACSSPNDD);
+
+	mc = R_REG(&regs->maccontrol);
+	WARN_ON(mc & MCTL_PSM_JMP_0);
+	WARN_ON(!(mc & MCTL_EN_MAC));
+	WARN_ON(!(mc & MCTL_PSM_RUN));
+
+	mi = R_REG(&regs->macintstatus);
+	WARN_ON(mi & MI_MACSSPNDD);
+
+	brcms_c_ucode_wake_override_clear(wlc_hw,
+					  BRCMS_WAKE_OVERRIDE_MACSUSPEND);
+}
+
+void brcms_b_band_stf_ss_set(struct brcms_hardware *wlc_hw, u8 stf_mode)
+{
+	wlc_hw->hw_stf_ss_opmode = stf_mode;
+
+	if (wlc_hw->clk)
+		brcms_upd_ofdm_pctl1_table(wlc_hw);
+}
+
+static bool brcms_b_validate_chip_access(struct brcms_hardware *wlc_hw)
+{
+	struct d11regs __iomem *regs;
+	u32 w, val;
+	struct wiphy *wiphy = wlc_hw->wlc->wiphy;
+
+	BCMMSG(wiphy, "wl%d\n", wlc_hw->unit);
+
+	regs = wlc_hw->regs;
+
+	/* Validate dchip register access */
+
+	W_REG(&regs->objaddr, OBJADDR_SHM_SEL | 0);
+	(void)R_REG(&regs->objaddr);
+	w = R_REG(&regs->objdata);
+
+	/* Can we write and read back a 32bit register? */
+	W_REG(&regs->objaddr, OBJADDR_SHM_SEL | 0);
+	(void)R_REG(&regs->objaddr);
+	W_REG(&regs->objdata, (u32) 0xaa5555aa);
+
+	W_REG(&regs->objaddr, OBJADDR_SHM_SEL | 0);
+	(void)R_REG(&regs->objaddr);
+	val = R_REG(&regs->objdata);
+	if (val != (u32) 0xaa5555aa) {
+		wiphy_err(wiphy, "wl%d: validate_chip_access: SHM = 0x%x, "
+			  "expected 0xaa5555aa\n", wlc_hw->unit, val);
+		return false;
+	}
+
+	W_REG(&regs->objaddr, OBJADDR_SHM_SEL | 0);
+	(void)R_REG(&regs->objaddr);
+	W_REG(&regs->objdata, (u32) 0x55aaaa55);
+
+	W_REG(&regs->objaddr, OBJADDR_SHM_SEL | 0);
+	(void)R_REG(&regs->objaddr);
+	val = R_REG(&regs->objdata);
+	if (val != (u32) 0x55aaaa55) {
+		wiphy_err(wiphy, "wl%d: validate_chip_access: SHM = 0x%x, "
+			  "expected 0x55aaaa55\n", wlc_hw->unit, val);
+		return false;
+	}
+
+	W_REG(&regs->objaddr, OBJADDR_SHM_SEL | 0);
+	(void)R_REG(&regs->objaddr);
+	W_REG(&regs->objdata, w);
+
+	/* clear CFPStart */
+	W_REG(&regs->tsf_cfpstart, 0);
+
+	w = R_REG(&regs->maccontrol);
+	if ((w != (MCTL_IHR_EN | MCTL_WAKE)) &&
+	    (w != (MCTL_IHR_EN | MCTL_GMODE | MCTL_WAKE))) {
+		wiphy_err(wiphy, "wl%d: validate_chip_access: maccontrol = "
+			  "0x%x, expected 0x%x or 0x%x\n", wlc_hw->unit, w,
+			  (MCTL_IHR_EN | MCTL_WAKE),
+			  (MCTL_IHR_EN | MCTL_GMODE | MCTL_WAKE));
+		return false;
+	}
+
+	return true;
+}
+
+#define PHYPLL_WAIT_US	100000
+
+void brcms_b_core_phypll_ctl(struct brcms_hardware *wlc_hw, bool on)
+{
+	struct d11regs __iomem *regs;
+	u32 tmp;
+
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
+
+	tmp = 0;
+	regs = wlc_hw->regs;
+
+	if (on) {
+		if ((wlc_hw->sih->chip == BCM4313_CHIP_ID)) {
+			OR_REG(&regs->clk_ctl_st,
+			       (CCS_ERSRC_REQ_HT | CCS_ERSRC_REQ_D11PLL |
+				CCS_ERSRC_REQ_PHYPLL));
+			SPINWAIT((R_REG(&regs->clk_ctl_st) &
+				  (CCS_ERSRC_AVAIL_HT)) != (CCS_ERSRC_AVAIL_HT),
+				 PHYPLL_WAIT_US);
+
+			tmp = R_REG(&regs->clk_ctl_st);
+			if ((tmp & (CCS_ERSRC_AVAIL_HT)) !=
+			    (CCS_ERSRC_AVAIL_HT))
+				wiphy_err(wlc_hw->wlc->wiphy, "%s: turn on PHY"
+					  " PLL failed\n", __func__);
+		} else {
+			OR_REG(&regs->clk_ctl_st,
+			       (CCS_ERSRC_REQ_D11PLL | CCS_ERSRC_REQ_PHYPLL));
+			SPINWAIT((R_REG(&regs->clk_ctl_st) &
+				  (CCS_ERSRC_AVAIL_D11PLL |
+				   CCS_ERSRC_AVAIL_PHYPLL)) !=
+				 (CCS_ERSRC_AVAIL_D11PLL |
+				  CCS_ERSRC_AVAIL_PHYPLL), PHYPLL_WAIT_US);
+
+			tmp = R_REG(&regs->clk_ctl_st);
+			if ((tmp &
+			     (CCS_ERSRC_AVAIL_D11PLL | CCS_ERSRC_AVAIL_PHYPLL))
+			    !=
+			    (CCS_ERSRC_AVAIL_D11PLL | CCS_ERSRC_AVAIL_PHYPLL))
+				wiphy_err(wlc_hw->wlc->wiphy, "%s: turn on "
+					  "PHY PLL failed\n", __func__);
+		}
+	} else {
+		/*
+		 * Since the PLL may be shared, other cores can still
+		 * be requesting it; so we'll deassert the request but
+		 * not wait for status to comply.
+		 */
+		AND_REG(&regs->clk_ctl_st, ~CCS_ERSRC_REQ_PHYPLL);
+		tmp = R_REG(&regs->clk_ctl_st);
+	}
+}
+
+static void brcms_c_coredisable(struct brcms_hardware *wlc_hw)
+{
+	bool dev_gone;
+
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
+
+	dev_gone = brcms_deviceremoved(wlc_hw->wlc);
+
+	if (dev_gone)
+		return;
+
+	if (wlc_hw->noreset)
+		return;
+
+	/* radio off */
+	wlc_phy_switch_radio(wlc_hw->band->pi, OFF);
+
+	/* turn off analog core */
+	wlc_phy_anacore(wlc_hw->band->pi, OFF);
+
+	/* turn off PHYPLL to save power */
+	brcms_b_core_phypll_ctl(wlc_hw, false);
 
-			if (!cfg->dtim_programmed)
-				return false;
+	wlc_hw->clk = false;
+	ai_core_disable(wlc_hw->sih, 0);
+	wlc_phy_hw_clk_state_upd(wlc_hw->band->pi, false);
+}
+
+static void brcms_c_flushqueues(struct brcms_c_info *wlc)
+{
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	uint i;
+
+	/* free any posted tx packets */
+	for (i = 0; i < NFIFO; i++)
+		if (wlc_hw->di[i]) {
+			dma_txreclaim(wlc_hw->di[i], DMA_RANGE_ALL);
+			wlc->core->txpktpend[i] = 0;
+			BCMMSG(wlc->wiphy, "pktpend fifo %d clrd\n", i);
 		}
+
+	/* free any posted rx packets */
+	dma_rxreclaim(wlc_hw->di[RX_FIFO]);
+}
+
+static u16
+brcms_b_read_objmem(struct brcms_hardware *wlc_hw, uint offset, u32 sel)
+{
+	struct d11regs __iomem *regs = wlc_hw->regs;
+	u16 __iomem *objdata_lo = (u16 __iomem *)&regs->objdata;
+	u16 __iomem *objdata_hi = objdata_lo + 1;
+	u16 v;
+
+	W_REG(&regs->objaddr, sel | (offset >> 2));
+	(void)R_REG(&regs->objaddr);
+	if (offset & 2)
+		v = R_REG(objdata_hi);
+	else
+		v = R_REG(objdata_lo);
+
+	return v;
+}
+
+static void
+brcms_b_write_objmem(struct brcms_hardware *wlc_hw, uint offset, u16 v,
+		     u32 sel)
+{
+	struct d11regs __iomem *regs = wlc_hw->regs;
+	u16 __iomem *objdata_lo = (u16 __iomem *)&regs->objdata;
+	u16 __iomem *objdata_hi = objdata_lo + 1;
+
+	W_REG(&regs->objaddr, sel | (offset >> 2));
+	(void)R_REG(&regs->objaddr);
+	if (offset & 2)
+		W_REG(objdata_hi, v);
+	else
+		W_REG(objdata_lo, v);
+}
+
+/*
+ * Read a single u16 from shared memory.
+ * SHM 'offset' needs to be an even address
+ */
+u16 brcms_b_read_shm(struct brcms_hardware *wlc_hw, uint offset)
+{
+	return brcms_b_read_objmem(wlc_hw, offset, OBJADDR_SHM_SEL);
+}
+
+/*
+ * Write a single u16 to shared memory.
+ * SHM 'offset' needs to be an even address
+ */
+void brcms_b_write_shm(struct brcms_hardware *wlc_hw, uint offset, u16 v)
+{
+	brcms_b_write_objmem(wlc_hw, offset, v, OBJADDR_SHM_SEL);
+}
+
+/*
+ * Copy a buffer to shared memory of specified type .
+ * SHM 'offset' needs to be an even address and
+ * Buffer length 'len' must be an even number of bytes
+ * 'sel' selects the type of memory
+ */
+void
+brcms_b_copyto_objmem(struct brcms_hardware *wlc_hw, uint offset,
+		      const void *buf, int len, u32 sel)
+{
+	u16 v;
+	const u8 *p = (const u8 *)buf;
+	int i;
+
+	if (len <= 0 || (offset & 1) || (len & 1))
+		return;
+
+	for (i = 0; i < len; i += 2) {
+		v = p[i] | (p[i + 1] << 8);
+		brcms_b_write_objmem(wlc_hw, offset + i, v, sel);
+	}
+}
+
+/*
+ * Copy a piece of shared memory of specified type to a buffer .
+ * SHM 'offset' needs to be an even address and
+ * Buffer length 'len' must be an even number of bytes
+ * 'sel' selects the type of memory
+ */
+void
+brcms_b_copyfrom_objmem(struct brcms_hardware *wlc_hw, uint offset, void *buf,
+			 int len, u32 sel)
+{
+	u16 v;
+	u8 *p = (u8 *) buf;
+	int i;
+
+	if (len <= 0 || (offset & 1) || (len & 1))
+		return;
+
+	for (i = 0; i < len; i += 2) {
+		v = brcms_b_read_objmem(wlc_hw, offset + i, sel);
+		p[i] = v & 0xFF;
+		p[i + 1] = (v >> 8) & 0xFF;
+	}
+}
+
+/* Copy a buffer to shared memory.
+ * SHM 'offset' needs to be an even address and
+ * Buffer length 'len' must be an even number of bytes
+ */
+static void brcms_c_copyto_shm(struct brcms_c_info *wlc, uint offset,
+			const void *buf, int len)
+{
+	brcms_b_copyto_objmem(wlc->hw, offset, buf, len, OBJADDR_SHM_SEL);
+}
+
+static void brcms_b_retrylimit_upd(struct brcms_hardware *wlc_hw,
+				   u16 SRL, u16 LRL)
+{
+	wlc_hw->SRL = SRL;
+	wlc_hw->LRL = LRL;
+
+	/* write retry limit to SCR, shouldn't need to suspend */
+	if (wlc_hw->up) {
+		W_REG(&wlc_hw->regs->objaddr,
+		      OBJADDR_SCR_SEL | S_DOT11_SRC_LMT);
+		(void)R_REG(&wlc_hw->regs->objaddr);
+		W_REG(&wlc_hw->regs->objdata, wlc_hw->SRL);
+		W_REG(&wlc_hw->regs->objaddr,
+		      OBJADDR_SCR_SEL | S_DOT11_LRC_LMT);
+		(void)R_REG(&wlc_hw->regs->objaddr);
+		W_REG(&wlc_hw->regs->objdata, wlc_hw->LRL);
+	}
+}
+
+static void brcms_b_pllreq(struct brcms_hardware *wlc_hw, bool set, u32 req_bit)
+{
+	if (set) {
+		if (mboolisset(wlc_hw->pllreq, req_bit))
+			return;
+
+		mboolset(wlc_hw->pllreq, req_bit);
+
+		if (mboolisset(wlc_hw->pllreq, BRCMS_PLLREQ_FLIP)) {
+			if (!wlc_hw->sbclk)
+				brcms_b_xtal(wlc_hw, ON);
+		}
+	} else {
+		if (!mboolisset(wlc_hw->pllreq, req_bit))
+			return;
+
+		mboolclr(wlc_hw->pllreq, req_bit);
+
+		if (mboolisset(wlc_hw->pllreq, BRCMS_PLLREQ_FLIP)) {
+			if (wlc_hw->sbclk)
+				brcms_b_xtal(wlc_hw, OFF);
+		}
+	}
+}
+
+static void brcms_b_antsel_set(struct brcms_hardware *wlc_hw, u32 antsel_avail)
+{
+	wlc_hw->antsel_avail = antsel_avail;
+}
+
+/*
+ * conditions under which the PM bit should be set in outgoing frames
+ * and STAY_AWAKE is meaningful
+ */
+static bool brcms_c_ps_allowed(struct brcms_c_info *wlc)
+{
+	struct brcms_bss_cfg *cfg = wlc->bsscfg;
+
+	/* disallow PS when one of the following global conditions meets */
+	if (!wlc->pub->associated)
+		return false;
+
+	/* disallow PS when one of these meets when not scanning */
+	if (wlc->monitor)
+		return false;
+
+	if (cfg->associated) {
+		/*
+		 * disallow PS when one of the following
+		 * bsscfg specific conditions meets
+		 */
+		if (!cfg->BSS)
+			return false;
+
+		return false;
 	}
 
 	return true;
 }
 
+static void brcms_c_statsupd(struct brcms_c_info *wlc)
+{
+	int i;
+	struct macstat macstats;
+#ifdef BCMDBG
+	u16 delta;
+	u16 rxf0ovfl;
+	u16 txfunfl[NFIFO];
+#endif				/* BCMDBG */
+
+	/* if driver down, make no sense to update stats */
+	if (!wlc->pub->up)
+		return;
+
+#ifdef BCMDBG
+	/* save last rx fifo 0 overflow count */
+	rxf0ovfl = wlc->core->macstat_snapshot->rxf0ovfl;
+
+	/* save last tx fifo  underflow count */
+	for (i = 0; i < NFIFO; i++)
+		txfunfl[i] = wlc->core->macstat_snapshot->txfunfl[i];
+#endif				/* BCMDBG */
+
+	/* Read mac stats from contiguous shared memory */
+	brcms_b_copyfrom_objmem(wlc->hw, M_UCODE_MACSTAT, &macstats,
+				sizeof(struct macstat), OBJADDR_SHM_SEL);
+
+#ifdef BCMDBG
+	/* check for rx fifo 0 overflow */
+	delta = (u16) (wlc->core->macstat_snapshot->rxf0ovfl - rxf0ovfl);
+	if (delta)
+		wiphy_err(wlc->wiphy, "wl%d: %u rx fifo 0 overflows!\n",
+			  wlc->pub->unit, delta);
+
+	/* check for tx fifo underflows */
+	for (i = 0; i < NFIFO; i++) {
+		delta =
+		    (u16) (wlc->core->macstat_snapshot->txfunfl[i] -
+			      txfunfl[i]);
+		if (delta)
+			wiphy_err(wlc->wiphy, "wl%d: %u tx fifo %d underflows!"
+				  "\n", wlc->pub->unit, delta, i);
+	}
+#endif				/* BCMDBG */
+
+	/* merge counters from dma module */
+	for (i = 0; i < NFIFO; i++) {
+		if (wlc->hw->di[i])
+			dma_counterreset(wlc->hw->di[i]);
+	}
+}
+
+static void brcms_b_reset(struct brcms_hardware *wlc_hw)
+{
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
+
+	/* reset the core */
+	if (!brcms_deviceremoved(wlc_hw->wlc))
+		brcms_b_corereset(wlc_hw, BRCMS_USE_COREFLAGS);
+
+	/* purge the dma rings */
+	brcms_c_flushqueues(wlc_hw->wlc);
+}
+
 void brcms_c_reset(struct brcms_c_info *wlc)
 {
 	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
@@ -414,207 +3227,675 @@ void brcms_c_reset(struct brcms_c_info *wlc)
 	brcms_b_reset(wlc->hw);
 }
 
-void brcms_c_fatal_error(struct brcms_c_info *wlc)
-{
-	wiphy_err(wlc->wiphy, "wl%d: fatal error, reinitializing\n",
-		  wlc->pub->unit);
-	brcms_init(wlc->wl);
-}
-
 /* Return the channel the driver should initialize during brcms_c_init.
  * the channel may have to be changed from the currently configured channel
  * if other configurations are in conflict (bandlocked, 11n mode disabled,
  * invalid channel for current country, etc.)
  */
-static chanspec_t brcms_c_init_chanspec(struct brcms_c_info *wlc)
+static u16 brcms_c_init_chanspec(struct brcms_c_info *wlc)
 {
-	chanspec_t chanspec =
+	u16 chanspec =
 	    1 | WL_CHANSPEC_BW_20 | WL_CHANSPEC_CTL_SB_NONE |
 	    WL_CHANSPEC_BAND_2G;
 
 	return chanspec;
 }
 
-struct scb global_scb;
-
-static void brcms_c_init_scb(struct brcms_c_info *wlc, struct scb *scb)
+void brcms_c_init_scb(struct scb *scb)
 {
 	int i;
+
+	memset(scb, 0, sizeof(struct scb));
 	scb->flags = SCB_WMECAP | SCB_HTCAP;
-	for (i = 0; i < NUMPRIO; i++)
+	for (i = 0; i < NUMPRIO; i++) {
 		scb->seqnum[i] = 0;
+		scb->seqctl[i] = 0xFFFF;
+	}
+
+	scb->seqctl_nonqos = 0xFFFF;
+	scb->magic = SCB_MAGIC;
 }
 
-void brcms_c_init(struct brcms_c_info *wlc)
-{
-	d11regs_t *regs;
-	chanspec_t chanspec;
-	int i;
-	struct brcms_bss_cfg *bsscfg;
-	bool mute = false;
+/* d11 core init
+ *   reset PSM
+ *   download ucode/PCM
+ *   let ucode run to suspended
+ *   download ucode inits
+ *   config other core registers
+ *   init dma
+ */
+static void brcms_b_coreinit(struct brcms_c_info *wlc)
+{
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	struct d11regs __iomem *regs;
+	u32 sflags;
+	uint bcnint_us;
+	uint i = 0;
+	bool fifosz_fixup = false;
+	int err = 0;
+	u16 buf[NFIFO];
+	struct wiphy *wiphy = wlc->wiphy;
+	struct brcms_ucode *ucode = &wlc_hw->wlc->wl->ucode;
 
-	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
+	regs = wlc_hw->regs;
 
-	regs = wlc->regs;
+	BCMMSG(wlc->wiphy, "wl%d\n", wlc_hw->unit);
+
+	/* reset PSM */
+	brcms_b_mctrl(wlc_hw, ~0, (MCTL_IHR_EN | MCTL_PSM_JMP_0 | MCTL_WAKE));
 
-	/* This will happen if a big-hammer was executed. In that case, we want to go back
-	 * to the channel that we were on and not new channel
+	brcms_ucode_download(wlc_hw);
+	/*
+	 * FIFOSZ fixup. driver wants to controls the fifo allocation.
 	 */
-	if (wlc->pub->associated)
-		chanspec = wlc->home_chanspec;
-	else
-		chanspec = brcms_c_init_chanspec(wlc);
+	fifosz_fixup = true;
 
-	brcms_b_init(wlc->hw, chanspec, mute);
+	/* let the PSM run to the suspended state, set mode to BSS STA */
+	W_REG(&regs->macintstatus, -1);
+	brcms_b_mctrl(wlc_hw, ~0,
+		       (MCTL_IHR_EN | MCTL_INFRA | MCTL_PSM_RUN | MCTL_WAKE));
 
-	/* update beacon listen interval */
-	brcms_c_bcn_li_upd(wlc);
+	/* wait for ucode to self-suspend after auto-init */
+	SPINWAIT(((R_REG(&regs->macintstatus) & MI_MACSSPNDD) == 0),
+		 1000 * 1000);
+	if ((R_REG(&regs->macintstatus) & MI_MACSSPNDD) == 0)
+		wiphy_err(wiphy, "wl%d: wlc_coreinit: ucode did not self-"
+			  "suspend!\n", wlc_hw->unit);
 
-	/* the world is new again, so is our reported rate */
-	brcms_c_reprate_init(wlc);
+	brcms_c_gpio_init(wlc);
 
-	/* write ethernet address to core */
-	FOREACH_BSS(wlc, i, bsscfg) {
-		brcms_c_set_mac(bsscfg);
-		brcms_c_set_bssid(bsscfg);
+	sflags = ai_core_sflags(wlc_hw->sih, 0, 0);
+
+	if (D11REV_IS(wlc_hw->corerev, 23)) {
+		if (BRCMS_ISNPHY(wlc_hw->band))
+			brcms_c_write_inits(wlc_hw, ucode->d11n0initvals16);
+		else
+			wiphy_err(wiphy, "%s: wl%d: unsupported phy in corerev"
+				  " %d\n", __func__, wlc_hw->unit,
+				  wlc_hw->corerev);
+	} else if (D11REV_IS(wlc_hw->corerev, 24)) {
+		if (BRCMS_ISLCNPHY(wlc_hw->band))
+			brcms_c_write_inits(wlc_hw, ucode->d11lcn0initvals24);
+		else
+			wiphy_err(wiphy, "%s: wl%d: unsupported phy in corerev"
+				  " %d\n", __func__, wlc_hw->unit,
+				  wlc_hw->corerev);
+	} else {
+		wiphy_err(wiphy, "%s: wl%d: unsupported corerev %d\n",
+			  __func__, wlc_hw->unit, wlc_hw->corerev);
+	}
+
+	/* For old ucode, txfifo sizes needs to be modified(increased) */
+	if (fifosz_fixup == true)
+		brcms_b_corerev_fifofixup(wlc_hw);
+
+	/* check txfifo allocations match between ucode and driver */
+	buf[TX_AC_BE_FIFO] = brcms_b_read_shm(wlc_hw, M_FIFOSIZE0);
+	if (buf[TX_AC_BE_FIFO] != wlc_hw->xmtfifo_sz[TX_AC_BE_FIFO]) {
+		i = TX_AC_BE_FIFO;
+		err = -1;
+	}
+	buf[TX_AC_VI_FIFO] = brcms_b_read_shm(wlc_hw, M_FIFOSIZE1);
+	if (buf[TX_AC_VI_FIFO] != wlc_hw->xmtfifo_sz[TX_AC_VI_FIFO]) {
+		i = TX_AC_VI_FIFO;
+		err = -1;
+	}
+	buf[TX_AC_BK_FIFO] = brcms_b_read_shm(wlc_hw, M_FIFOSIZE2);
+	buf[TX_AC_VO_FIFO] = (buf[TX_AC_BK_FIFO] >> 8) & 0xff;
+	buf[TX_AC_BK_FIFO] &= 0xff;
+	if (buf[TX_AC_BK_FIFO] != wlc_hw->xmtfifo_sz[TX_AC_BK_FIFO]) {
+		i = TX_AC_BK_FIFO;
+		err = -1;
+	}
+	if (buf[TX_AC_VO_FIFO] != wlc_hw->xmtfifo_sz[TX_AC_VO_FIFO]) {
+		i = TX_AC_VO_FIFO;
+		err = -1;
+	}
+	buf[TX_BCMC_FIFO] = brcms_b_read_shm(wlc_hw, M_FIFOSIZE3);
+	buf[TX_ATIM_FIFO] = (buf[TX_BCMC_FIFO] >> 8) & 0xff;
+	buf[TX_BCMC_FIFO] &= 0xff;
+	if (buf[TX_BCMC_FIFO] != wlc_hw->xmtfifo_sz[TX_BCMC_FIFO]) {
+		i = TX_BCMC_FIFO;
+		err = -1;
+	}
+	if (buf[TX_ATIM_FIFO] != wlc_hw->xmtfifo_sz[TX_ATIM_FIFO]) {
+		i = TX_ATIM_FIFO;
+		err = -1;
 	}
+	if (err != 0)
+		wiphy_err(wiphy, "wlc_coreinit: txfifo mismatch: ucode size %d"
+			  " driver size %d index %d\n", buf[i],
+			  wlc_hw->xmtfifo_sz[i], i);
 
-	/* Update tsf_cfprep if associated and up */
-	if (wlc->pub->associated) {
-		FOREACH_BSS(wlc, i, bsscfg) {
-			if (bsscfg->up) {
-				u32 bi;
+	/* make sure we can still talk to the mac */
+	WARN_ON(R_REG(&regs->maccontrol) == 0xffffffff);
 
-				/* get beacon period and convert to uS */
-				bi = bsscfg->current_bss->beacon_period << 10;
-				/*
-				 * update since init path would reset
-				 * to default value
-				 */
-				W_REG(&regs->tsf_cfprep,
-				      (bi << CFPREP_CBI_SHIFT));
+	/* band-specific inits done by wlc_bsinit() */
 
-				/* Update maccontrol PM related bits */
-				brcms_c_set_ps_ctrl(wlc);
+	/* Set up frame burst size and antenna swap threshold init values */
+	brcms_b_write_shm(wlc_hw, M_MBURST_SIZE, MAXTXFRAMEBURST);
+	brcms_b_write_shm(wlc_hw, M_MAX_ANTCNT, ANTCNT);
 
-				break;
-			}
-		}
-	}
+	/* enable one rx interrupt per received frame */
+	W_REG(&regs->intrcvlazy[0], (1 << IRL_FC_SHIFT));
 
-	brcms_c_bandinit_ordered(wlc, chanspec);
+	/* set the station mode (BSS STA) */
+	brcms_b_mctrl(wlc_hw,
+		       (MCTL_INFRA | MCTL_DISCARD_PMQ | MCTL_AP),
+		       (MCTL_INFRA | MCTL_DISCARD_PMQ));
 
-	brcms_c_init_scb(wlc, &global_scb);
+	/* set up Beacon interval */
+	bcnint_us = 0x8000 << 10;
+	W_REG(&regs->tsf_cfprep, (bcnint_us << CFPREP_CBI_SHIFT));
+	W_REG(&regs->tsf_cfpstart, bcnint_us);
+	W_REG(&regs->macintstatus, MI_GP1);
 
-	/* init probe response timeout */
-	brcms_c_write_shm(wlc, M_PRS_MAXTIME, wlc->prb_resp_timeout);
+	/* write interrupt mask */
+	W_REG(&regs->intctrlregs[RX_FIFO].intmask, DEF_RXINTMASK);
 
-	/* init max burst txop (framebursting) */
-	brcms_c_write_shm(wlc, M_MBURST_TXOP,
-		      (wlc->
-		       _rifs ? (EDCF_AC_VO_TXOP_AP << 5) : MAXFRAMEBURST_TXOP));
+	/* allow the MAC to control the PHY clock (dynamic on/off) */
+	brcms_b_macphyclk_set(wlc_hw, ON);
 
-	/* initialize maximum allowed duty cycle */
-	brcms_c_duty_cycle_set(wlc, wlc->tx_duty_cycle_ofdm, true, true);
-	brcms_c_duty_cycle_set(wlc, wlc->tx_duty_cycle_cck, false, true);
+	/* program dynamic clock control fast powerup delay register */
+	wlc->fastpwrup_dly = ai_clkctl_fast_pwrup_delay(wlc_hw->sih);
+	W_REG(&regs->scc_fastpwrup_dly, wlc->fastpwrup_dly);
 
-	/* Update some shared memory locations related to max AMPDU size allowed to received */
-	brcms_c_ampdu_shm_upd(wlc->ampdu);
+	/* tell the ucode the corerev */
+	brcms_b_write_shm(wlc_hw, M_MACHW_VER, (u16) wlc_hw->corerev);
 
-	/* band-specific inits */
-	brcms_c_bsinit(wlc);
+	/* tell the ucode MAC capabilities */
+	brcms_b_write_shm(wlc_hw, M_MACHW_CAP_L,
+			   (u16) (wlc_hw->machwcap & 0xffff));
+	brcms_b_write_shm(wlc_hw, M_MACHW_CAP_H,
+			   (u16) ((wlc_hw->
+				      machwcap >> 16) & 0xffff));
 
-	/* Enable EDCF mode (while the MAC is suspended) */
-	if (EDCF_ENAB(wlc->pub)) {
-		OR_REG(&regs->ifs_ctl, IFS_USEEDCF);
-		brcms_c_edcf_setparams(wlc, false);
-	}
+	/* write retry limits to SCR, this done after PSM init */
+	W_REG(&regs->objaddr, OBJADDR_SCR_SEL | S_DOT11_SRC_LMT);
+	(void)R_REG(&regs->objaddr);
+	W_REG(&regs->objdata, wlc_hw->SRL);
+	W_REG(&regs->objaddr, OBJADDR_SCR_SEL | S_DOT11_LRC_LMT);
+	(void)R_REG(&regs->objaddr);
+	W_REG(&regs->objdata, wlc_hw->LRL);
 
-	/* Init precedence maps for empty FIFOs */
-	brcms_c_tx_prec_map_init(wlc);
+	/* write rate fallback retry limits */
+	brcms_b_write_shm(wlc_hw, M_SFRMTXCNTFBRTHSD, wlc_hw->SFBL);
+	brcms_b_write_shm(wlc_hw, M_LFRMTXCNTFBRTHSD, wlc_hw->LFBL);
 
-	/* read the ucode version if we have not yet done so */
-	if (wlc->ucode_rev == 0) {
-		wlc->ucode_rev =
-		    brcms_c_read_shm(wlc, M_BOM_REV_MAJOR) << NBITS(u16);
-		wlc->ucode_rev |= brcms_c_read_shm(wlc, M_BOM_REV_MINOR);
+	AND_REG(&regs->ifs_ctl, 0x0FFF);
+	W_REG(&regs->ifs_aifsn, EDCF_AIFSN_MIN);
+
+	/* init the tx dma engines */
+	for (i = 0; i < NFIFO; i++) {
+		if (wlc_hw->di[i])
+			dma_txinit(wlc_hw->di[i]);
 	}
 
-	/* ..now really unleash hell (allow the MAC out of suspend) */
-	brcms_c_enable_mac(wlc);
+	/* init the rx dma engine(s) and post receive buffers */
+	dma_rxinit(wlc_hw->di[RX_FIFO]);
+	dma_rxfill(wlc_hw->di[RX_FIFO]);
+}
 
-	/* clear tx flow control */
-	brcms_c_txflowcontrol_reset(wlc);
+void
+static brcms_b_init(struct brcms_hardware *wlc_hw, u16 chanspec,
+			  bool mute) {
+	u32 macintmask;
+	bool fastclk;
+	struct brcms_c_info *wlc = wlc_hw->wlc;
 
-	/* clear tx data fifo suspends */
-	wlc->tx_suspended = false;
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
 
-	/* enable the RF Disable Delay timer */
-	W_REG(&wlc->regs->rfdisabledly, RFDISABLE_DEFAULT);
+	/* request FAST clock if not on */
+	fastclk = wlc_hw->forcefastclk;
+	if (!fastclk)
+		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
 
-	/* initialize mpc delay */
-	wlc->mpc_delay_off = wlc->mpc_dlycnt = BRCMS_MPC_MIN_DELAYCNT;
+	/* disable interrupts */
+	macintmask = brcms_intrsoff(wlc->wl);
+
+	/* set up the specified band and chanspec */
+	brcms_c_setxband(wlc_hw, chspec_bandunit(chanspec));
+	wlc_phy_chanspec_radio_set(wlc_hw->band->pi, chanspec);
+
+	/* do one-time phy inits and calibration */
+	wlc_phy_cal_init(wlc_hw->band->pi);
+
+	/* core-specific initialization */
+	brcms_b_coreinit(wlc);
+
+	/* suspend the tx fifos and mute the phy for preism cac time */
+	if (mute)
+		brcms_b_mute(wlc_hw, ON, PHY_MUTE_FOR_PREISM);
+
+	/* band-specific inits */
+	brcms_b_bsinit(wlc, chanspec);
+
+	/* restore macintmask */
+	brcms_intrsrestore(wlc->wl, macintmask);
+
+	/* seed wake_override with BRCMS_WAKE_OVERRIDE_MACSUSPEND since the mac
+	 * is suspended and brcms_c_enable_mac() will clear this override bit.
+	 */
+	mboolset(wlc_hw->wake_override, BRCMS_WAKE_OVERRIDE_MACSUSPEND);
 
 	/*
-	 * Initialize WME parameters; if they haven't been set by some other
-	 * mechanism (IOVar, etc) then read them from the hardware.
+	 * initialize mac_suspend_depth to 1 to match ucode
+	 * initial suspended state
 	 */
-	if (BRCMS_WME_RETRY_SHORT_GET(wlc, 0) == 0) {
-		/* Uninitialized; read from HW */
-		int ac;
+	wlc_hw->mac_suspend_depth = 1;
 
-		for (ac = 0; ac < AC_COUNT; ac++) {
-			wlc->wme_retries[ac] =
-			    brcms_c_read_shm(wlc, M_AC_TXLMT_ADDR(ac));
+	/* restore the clk */
+	if (!fastclk)
+		brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
+}
+
+static void brcms_c_set_phy_chanspec(struct brcms_c_info *wlc,
+				     u16 chanspec)
+{
+	/* Save our copy of the chanspec */
+	wlc->chanspec = chanspec;
+
+	/* Set the chanspec and power limits for this locale */
+	brcms_c_channel_set_chanspec(wlc->cmi, chanspec, BRCMS_TXPWR_MAX);
+
+	if (wlc->stf->ss_algosel_auto)
+		brcms_c_stf_ss_algo_channel_get(wlc, &wlc->stf->ss_algo_channel,
+					    chanspec);
+
+	brcms_c_stf_ss_update(wlc, wlc->band);
+}
+
+static void
+brcms_default_rateset(struct brcms_c_info *wlc, struct brcms_c_rateset *rs)
+{
+	brcms_c_rateset_default(rs, NULL, wlc->band->phytype,
+		wlc->band->bandtype, false, BRCMS_RATE_MASK_FULL,
+		(bool) (wlc->pub->_n_enab & SUPPORT_11N),
+		brcms_chspec_bw(wlc->default_bss->chanspec),
+		wlc->stf->txstreams);
+}
+
+/* derive wlc->band->basic_rate[] table from 'rateset' */
+static void brcms_c_rate_lookup_init(struct brcms_c_info *wlc,
+			      struct brcms_c_rateset *rateset)
+{
+	u8 rate;
+	u8 mandatory;
+	u8 cck_basic = 0;
+	u8 ofdm_basic = 0;
+	u8 *br = wlc->band->basic_rate;
+	uint i;
+
+	/* incoming rates are in 500kbps units as in 802.11 Supported Rates */
+	memset(br, 0, BRCM_MAXRATE + 1);
+
+	/* For each basic rate in the rates list, make an entry in the
+	 * best basic lookup.
+	 */
+	for (i = 0; i < rateset->count; i++) {
+		/* only make an entry for a basic rate */
+		if (!(rateset->rates[i] & BRCMS_RATE_FLAG))
+			continue;
+
+		/* mask off basic bit */
+		rate = (rateset->rates[i] & BRCMS_RATE_MASK);
+
+		if (rate > BRCM_MAXRATE) {
+			wiphy_err(wlc->wiphy, "brcms_c_rate_lookup_init: "
+				  "invalid rate 0x%X in rate set\n",
+				  rateset->rates[i]);
+			continue;
+		}
+
+		br[rate] = rate;
+	}
+
+	/* The rate lookup table now has non-zero entries for each
+	 * basic rate, equal to the basic rate: br[basicN] = basicN
+	 *
+	 * To look up the best basic rate corresponding to any
+	 * particular rate, code can use the basic_rate table
+	 * like this
+	 *
+	 * basic_rate = wlc->band->basic_rate[tx_rate]
+	 *
+	 * Make sure there is a best basic rate entry for
+	 * every rate by walking up the table from low rates
+	 * to high, filling in holes in the lookup table
+	 */
+
+	for (i = 0; i < wlc->band->hw_rateset.count; i++) {
+		rate = wlc->band->hw_rateset.rates[i];
+
+		if (br[rate] != 0) {
+			/* This rate is a basic rate.
+			 * Keep track of the best basic rate so far by
+			 * modulation type.
+			 */
+			if (is_ofdm_rate(rate))
+				ofdm_basic = rate;
+			else
+				cck_basic = rate;
+
+			continue;
 		}
+
+		/* This rate is not a basic rate so figure out the
+		 * best basic rate less than this rate and fill in
+		 * the hole in the table
+		 */
+
+		br[rate] = is_ofdm_rate(rate) ? ofdm_basic : cck_basic;
+
+		if (br[rate] != 0)
+			continue;
+
+		if (is_ofdm_rate(rate)) {
+			/*
+			 * In 11g and 11a, the OFDM mandatory rates
+			 * are 6, 12, and 24 Mbps
+			 */
+			if (rate >= BRCM_RATE_24M)
+				mandatory = BRCM_RATE_24M;
+			else if (rate >= BRCM_RATE_12M)
+				mandatory = BRCM_RATE_12M;
+			else
+				mandatory = BRCM_RATE_6M;
+		} else {
+			/* In 11b, all CCK rates are mandatory 1 - 11 Mbps */
+			mandatory = rate;
+		}
+
+		br[rate] = mandatory;
 	}
 }
 
-void brcms_c_mac_bcn_promisc_change(struct brcms_c_info *wlc, bool promisc)
+static void brcms_c_bandinit_ordered(struct brcms_c_info *wlc,
+				     u16 chanspec)
 {
-	wlc->bcnmisc_monitor = promisc;
-	brcms_c_mac_bcn_promisc(wlc);
+	struct brcms_c_rateset default_rateset;
+	uint parkband;
+	uint i, band_order[2];
+
+	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
+	/*
+	 * We might have been bandlocked during down and the chip
+	 * power-cycled (hibernate). Figure out the right band to park on
+	 */
+	if (wlc->bandlocked || wlc->pub->_nbands == 1) {
+		/* updated in brcms_c_bandlock() */
+		parkband = wlc->band->bandunit;
+		band_order[0] = band_order[1] = parkband;
+	} else {
+		/* park on the band of the specified chanspec */
+		parkband = chspec_bandunit(chanspec);
+
+		/* order so that parkband initialize last */
+		band_order[0] = parkband ^ 1;
+		band_order[1] = parkband;
+	}
+
+	/* make each band operational, software state init */
+	for (i = 0; i < wlc->pub->_nbands; i++) {
+		uint j = band_order[i];
+
+		wlc->band = wlc->bandstate[j];
+
+		brcms_default_rateset(wlc, &default_rateset);
+
+		/* fill in hw_rate */
+		brcms_c_rateset_filter(&default_rateset, &wlc->band->hw_rateset,
+				   false, BRCMS_RATES_CCK_OFDM, BRCMS_RATE_MASK,
+				   (bool) (wlc->pub->_n_enab & SUPPORT_11N));
+
+		/* init basic rate lookup */
+		brcms_c_rate_lookup_init(wlc, &default_rateset);
+	}
+
+	/* sync up phy/radio chanspec */
+	brcms_c_set_phy_chanspec(wlc, chanspec);
 }
 
-void brcms_c_mac_bcn_promisc(struct brcms_c_info *wlc)
+static void brcms_c_mac_bcn_promisc(struct brcms_c_info *wlc)
 {
-	if ((AP_ENAB(wlc->pub) && (N_ENAB(wlc->pub) || wlc->band->gmode)) ||
-	    wlc->bcnmisc_ibss || wlc->bcnmisc_scan || wlc->bcnmisc_monitor)
-		brcms_c_mctrl(wlc, MCTL_BCNS_PROMISC, MCTL_BCNS_PROMISC);
+	if (wlc->bcnmisc_monitor)
+		brcms_b_mctrl(wlc->hw, MCTL_BCNS_PROMISC, MCTL_BCNS_PROMISC);
 	else
-		brcms_c_mctrl(wlc, MCTL_BCNS_PROMISC, 0);
+		brcms_b_mctrl(wlc->hw, MCTL_BCNS_PROMISC, 0);
+}
+
+void brcms_c_mac_bcn_promisc_change(struct brcms_c_info *wlc, bool promisc)
+{
+	wlc->bcnmisc_monitor = promisc;
+	brcms_c_mac_bcn_promisc(wlc);
 }
 
 /* set or clear maccontrol bits MCTL_PROMISC and MCTL_KEEPCONTROL */
-void brcms_c_mac_promisc(struct brcms_c_info *wlc)
+static void brcms_c_mac_promisc(struct brcms_c_info *wlc)
 {
 	u32 promisc_bits = 0;
 
-	/* promiscuous mode just sets MCTL_PROMISC
-	 * Note: APs get all BSS traffic without the need to set the MCTL_PROMISC bit
-	 * since all BSS data traffic is directed at the AP
+	/*
+	 * promiscuous mode just sets MCTL_PROMISC
+	 * Note: APs get all BSS traffic without the need to set
+	 * the MCTL_PROMISC bit since all BSS data traffic is
+	 * directed at the AP
 	 */
-	if (PROMISC_ENAB(wlc->pub) && !AP_ENAB(wlc->pub))
+	if (wlc->pub->promisc)
 		promisc_bits |= MCTL_PROMISC;
 
 	/* monitor mode needs both MCTL_PROMISC and MCTL_KEEPCONTROL
 	 * Note: monitor mode also needs MCTL_BCNS_PROMISC, but that is
 	 * handled in brcms_c_mac_bcn_promisc()
 	 */
-	if (MONITOR_ENAB(wlc))
+	if (wlc->monitor)
 		promisc_bits |= MCTL_PROMISC | MCTL_KEEPCONTROL;
 
-	brcms_c_mctrl(wlc, MCTL_PROMISC | MCTL_KEEPCONTROL, promisc_bits);
+	brcms_b_mctrl(wlc->hw, MCTL_PROMISC | MCTL_KEEPCONTROL, promisc_bits);
+}
+
+/*
+ * ucode, hwmac update
+ *    Channel dependent updates for ucode and hw
+ */
+static void brcms_c_ucode_mac_upd(struct brcms_c_info *wlc)
+{
+	/* enable or disable any active IBSSs depending on whether or not
+	 * we are on the home channel
+	 */
+	if (wlc->home_chanspec == wlc_phy_chanspec_get(wlc->band->pi)) {
+		if (wlc->pub->associated) {
+			/*
+			 * BMAC_NOTE: This is something that should be fixed
+			 * in ucode inits. I think that the ucode inits set
+			 * up the bcn templates and shm values with a bogus
+			 * beacon. This should not be done in the inits. If
+			 * ucode needs to set up a beacon for testing, the
+			 * test routines should write it down, not expect the
+			 * inits to populate a bogus beacon.
+			 */
+			if (BRCMS_PHY_11N_CAP(wlc->band))
+				brcms_b_write_shm(wlc->hw,
+						M_BCN_TXTSF_OFFSET, 0);
+		}
+	} else {
+		/* disable an active IBSS if we are not on the home channel */
+	}
+
+	/* update the various promisc bits */
+	brcms_c_mac_bcn_promisc(wlc);
+	brcms_c_mac_promisc(wlc);
+}
+
+static void brcms_c_write_rate_shm(struct brcms_c_info *wlc, u8 rate,
+				   u8 basic_rate)
+{
+	u8 phy_rate, index;
+	u8 basic_phy_rate, basic_index;
+	u16 dir_table, basic_table;
+	u16 basic_ptr;
+
+	/* Shared memory address for the table we are reading */
+	dir_table = is_ofdm_rate(basic_rate) ? M_RT_DIRMAP_A : M_RT_DIRMAP_B;
+
+	/* Shared memory address for the table we are writing */
+	basic_table = is_ofdm_rate(rate) ? M_RT_BBRSMAP_A : M_RT_BBRSMAP_B;
+
+	/*
+	 * for a given rate, the LS-nibble of the PLCP SIGNAL field is
+	 * the index into the rate table.
+	 */
+	phy_rate = rate_info[rate] & BRCMS_RATE_MASK;
+	basic_phy_rate = rate_info[basic_rate] & BRCMS_RATE_MASK;
+	index = phy_rate & 0xf;
+	basic_index = basic_phy_rate & 0xf;
+
+	/* Find the SHM pointer to the ACK rate entry by looking in the
+	 * Direct-map Table
+	 */
+	basic_ptr = brcms_b_read_shm(wlc->hw, (dir_table + basic_index * 2));
+
+	/* Update the SHM BSS-basic-rate-set mapping table with the pointer
+	 * to the correct basic rate for the given incoming rate
+	 */
+	brcms_b_write_shm(wlc->hw, (basic_table + index * 2), basic_ptr);
+}
+
+static const struct brcms_c_rateset *
+brcms_c_rateset_get_hwrs(struct brcms_c_info *wlc)
+{
+	const struct brcms_c_rateset *rs_dflt;
+
+	if (BRCMS_PHY_11N_CAP(wlc->band)) {
+		if (wlc->band->bandtype == BRCM_BAND_5G)
+			rs_dflt = &ofdm_mimo_rates;
+		else
+			rs_dflt = &cck_ofdm_mimo_rates;
+	} else if (wlc->band->gmode)
+		rs_dflt = &cck_ofdm_rates;
+	else
+		rs_dflt = &cck_rates;
+
+	return rs_dflt;
+}
+
+static void brcms_c_set_ratetable(struct brcms_c_info *wlc)
+{
+	const struct brcms_c_rateset *rs_dflt;
+	struct brcms_c_rateset rs;
+	u8 rate, basic_rate;
+	uint i;
+
+	rs_dflt = brcms_c_rateset_get_hwrs(wlc);
+
+	brcms_c_rateset_copy(rs_dflt, &rs);
+	brcms_c_rateset_mcs_upd(&rs, wlc->stf->txstreams);
+
+	/* walk the phy rate table and update SHM basic rate lookup table */
+	for (i = 0; i < rs.count; i++) {
+		rate = rs.rates[i] & BRCMS_RATE_MASK;
+
+		/* for a given rate brcms_basic_rate returns the rate at
+		 * which a response ACK/CTS should be sent.
+		 */
+		basic_rate = brcms_basic_rate(wlc, rate);
+		if (basic_rate == 0)
+			/* This should only happen if we are using a
+			 * restricted rateset.
+			 */
+			basic_rate = rs.rates[0] & BRCMS_RATE_MASK;
+
+		brcms_c_write_rate_shm(wlc, rate, basic_rate);
+	}
+}
+
+/* band-specific init */
+static void brcms_c_bsinit(struct brcms_c_info *wlc)
+{
+	BCMMSG(wlc->wiphy, "wl%d: bandunit %d\n",
+		 wlc->pub->unit, wlc->band->bandunit);
+
+	/* write ucode ACK/CTS rate table */
+	brcms_c_set_ratetable(wlc);
+
+	/* update some band specific mac configuration */
+	brcms_c_ucode_mac_upd(wlc);
+
+	/* init antenna selection */
+	brcms_c_antsel_init(wlc->asi);
+
+}
+
+/* formula:  IDLE_BUSY_RATIO_X_16 = (100-duty_cycle)/duty_cycle*16 */
+static int
+brcms_c_duty_cycle_set(struct brcms_c_info *wlc, int duty_cycle, bool isOFDM,
+		   bool writeToShm)
+{
+	int idle_busy_ratio_x_16 = 0;
+	uint offset =
+	    isOFDM ? M_TX_IDLE_BUSY_RATIO_X_16_OFDM :
+	    M_TX_IDLE_BUSY_RATIO_X_16_CCK;
+	if (duty_cycle > 100 || duty_cycle < 0) {
+		wiphy_err(wlc->wiphy, "wl%d:  duty cycle value off limit\n",
+			  wlc->pub->unit);
+		return -EINVAL;
+	}
+	if (duty_cycle)
+		idle_busy_ratio_x_16 = (100 - duty_cycle) * 16 / duty_cycle;
+	/* Only write to shared memory  when wl is up */
+	if (writeToShm)
+		brcms_b_write_shm(wlc->hw, offset, (u16) idle_busy_ratio_x_16);
+
+	if (isOFDM)
+		wlc->tx_duty_cycle_ofdm = (u16) duty_cycle;
+	else
+		wlc->tx_duty_cycle_cck = (u16) duty_cycle;
+
+	return 0;
+}
+
+/*
+ * Initialize the base precedence map for dequeueing
+ * from txq based on WME settings
+ */
+static void brcms_c_tx_prec_map_init(struct brcms_c_info *wlc)
+{
+	wlc->tx_prec_map = BRCMS_PREC_BMP_ALL;
+	memset(wlc->fifo2prec_map, 0, NFIFO * sizeof(u16));
+
+	wlc->fifo2prec_map[TX_AC_BK_FIFO] = BRCMS_PREC_BMP_AC_BK;
+	wlc->fifo2prec_map[TX_AC_BE_FIFO] = BRCMS_PREC_BMP_AC_BE;
+	wlc->fifo2prec_map[TX_AC_VI_FIFO] = BRCMS_PREC_BMP_AC_VI;
+	wlc->fifo2prec_map[TX_AC_VO_FIFO] = BRCMS_PREC_BMP_AC_VO;
+}
+
+static void
+brcms_c_txflowcontrol_signal(struct brcms_c_info *wlc,
+			     struct brcms_txq_info *qi, bool on, int prio)
+{
+	/* transmit flowcontrol is not yet implemented */
+}
+
+static void brcms_c_txflowcontrol_reset(struct brcms_c_info *wlc)
+{
+	struct brcms_txq_info *qi;
+
+	for (qi = wlc->tx_queues; qi != NULL; qi = qi->next) {
+		if (qi->stopped) {
+			brcms_c_txflowcontrol_signal(wlc, qi, OFF, ALLPRIO);
+			qi->stopped = 0;
+		}
+	}
 }
 
 /* push sw hps and wake state through hardware */
-void brcms_c_set_ps_ctrl(struct brcms_c_info *wlc)
+static void brcms_c_set_ps_ctrl(struct brcms_c_info *wlc)
 {
 	u32 v1, v2;
 	bool hps;
 	bool awake_before;
 
-	hps = PS_ALLOWED(wlc);
+	hps = brcms_c_ps_allowed(wlc);
 
 	BCMMSG(wlc->wiphy, "wl%d: hps %d\n", wlc->pub->unit, hps);
 
@@ -623,28 +3904,25 @@ void brcms_c_set_ps_ctrl(struct brcms_c_info *wlc)
 	if (hps)
 		v2 |= MCTL_HPS;
 
-	brcms_c_mctrl(wlc, MCTL_WAKE | MCTL_HPS, v2);
+	brcms_b_mctrl(wlc->hw, MCTL_WAKE | MCTL_HPS, v2);
 
 	awake_before = ((v1 & MCTL_WAKE) || ((v1 & MCTL_HPS) == 0));
 
 	if (!awake_before)
 		brcms_b_wait_for_wake(wlc->hw);
-
 }
 
 /*
  * Write this BSS config's MAC address to core.
  * Updates RXE match engine.
  */
-int brcms_c_set_mac(struct brcms_bss_cfg *cfg)
+static int brcms_c_set_mac(struct brcms_bss_cfg *bsscfg)
 {
 	int err = 0;
-	struct brcms_c_info *wlc = cfg->wlc;
+	struct brcms_c_info *wlc = bsscfg->wlc;
 
-	if (cfg == wlc->cfg) {
-		/* enter the MAC addr into the RXE match registers */
-		brcms_c_set_addrmatch(wlc, RCM_MAC_OFFSET, cfg->cur_etheraddr);
-	}
+	/* enter the MAC addr into the RXE match registers */
+	brcms_c_set_addrmatch(wlc, RCM_MAC_OFFSET, bsscfg->cur_etheraddr);
 
 	brcms_c_ampdu_macaddr_upd(wlc);
 
@@ -654,30 +3932,29 @@ int brcms_c_set_mac(struct brcms_bss_cfg *cfg)
 /* Write the BSS config's BSSID address to core (set_bssid in d11procs.tcl).
  * Updates RXE match engine.
  */
-void brcms_c_set_bssid(struct brcms_bss_cfg *cfg)
+static void brcms_c_set_bssid(struct brcms_bss_cfg *bsscfg)
 {
-	struct brcms_c_info *wlc = cfg->wlc;
+	/* we need to update BSSID in RXE match registers */
+	brcms_c_set_addrmatch(bsscfg->wlc, RCM_BSSID_OFFSET, bsscfg->BSSID);
+}
 
-	/* if primary config, we need to update BSSID in RXE match registers */
-	if (cfg == wlc->cfg) {
-		brcms_c_set_addrmatch(wlc, RCM_BSSID_OFFSET, cfg->BSSID);
-	}
-#ifdef SUPPORT_HWKEYS
-	else if (BSSCFG_STA(cfg) && cfg->BSS) {
-		brcms_c_rcmta_add_bssid(wlc, cfg);
+static void brcms_b_set_shortslot(struct brcms_hardware *wlc_hw, bool shortslot)
+{
+	wlc_hw->shortslot = shortslot;
+
+	if (wlc_hw->band->bandtype == BRCM_BAND_2G && wlc_hw->up) {
+		brcms_c_suspend_mac_and_wait(wlc_hw->wlc);
+		brcms_b_update_slot_timing(wlc_hw, shortslot);
+		brcms_c_enable_mac(wlc_hw->wlc);
 	}
-#endif
 }
 
 /*
  * Suspend the the MAC and update the slot timing
  * for standard 11b/g (20us slots) or shortslot 11g (9us slots).
  */
-void brcms_c_switch_shortslot(struct brcms_c_info *wlc, bool shortslot)
+static void brcms_c_switch_shortslot(struct brcms_c_info *wlc, bool shortslot)
 {
-	int idx;
-	struct brcms_bss_cfg *cfg;
-
 	/* use the override if it is set */
 	if (wlc->shortslot_override != BRCMS_SHORTSLOT_AUTO)
 		shortslot = (wlc->shortslot_override == BRCMS_SHORTSLOT_ON);
@@ -687,91 +3964,84 @@ void brcms_c_switch_shortslot(struct brcms_c_info *wlc, bool shortslot)
 
 	wlc->shortslot = shortslot;
 
-	/* update the capability based on current shortslot mode */
-	FOREACH_BSS(wlc, idx, cfg) {
-		if (!cfg->associated)
-			continue;
-		cfg->current_bss->capability &=
-					~WLAN_CAPABILITY_SHORT_SLOT_TIME;
-		if (wlc->shortslot)
-			cfg->current_bss->capability |=
-					WLAN_CAPABILITY_SHORT_SLOT_TIME;
-	}
-
 	brcms_b_set_shortslot(wlc->hw, shortslot);
 }
 
-static u8 brcms_c_local_constraint_qdbm(struct brcms_c_info *wlc)
+static void brcms_c_set_home_chanspec(struct brcms_c_info *wlc, u16 chanspec)
 {
-	u8 local;
-	s16 local_max;
-
-	local = BRCMS_TXPWR_MAX;
-	if (wlc->pub->associated &&
-	    (brcmu_chspec_ctlchan(wlc->chanspec) ==
-	     brcmu_chspec_ctlchan(wlc->home_chanspec))) {
+	if (wlc->home_chanspec != chanspec) {
+		wlc->home_chanspec = chanspec;
 
-		/* get the local power constraint if we are on the AP's
-		 * channel [802.11h, 7.3.2.13]
-		 */
-		/* Clamp the value between 0 and BRCMS_TXPWR_MAX w/o
-		 * overflowing the target */
-		local_max =
-		    (wlc->txpwr_local_max -
-		     wlc->txpwr_local_constraint) * BRCMS_TXPWR_DB_FACTOR;
-		if (local_max > 0 && local_max < BRCMS_TXPWR_MAX)
-			return (u8) local_max;
-		if (local_max < 0)
-			return 0;
+		if (wlc->bsscfg->associated)
+			wlc->bsscfg->current_bss->chanspec = chanspec;
 	}
-
-	return local;
 }
 
-/* propagate home chanspec to all bsscfgs in case bsscfg->current_bss->chanspec is referenced */
-void brcms_c_set_home_chanspec(struct brcms_c_info *wlc, chanspec_t chanspec)
+void
+brcms_b_set_chanspec(struct brcms_hardware *wlc_hw, u16 chanspec,
+		      bool mute, struct txpwr_limits *txpwr)
 {
-	if (wlc->home_chanspec != chanspec) {
-		int idx;
-		struct brcms_bss_cfg *cfg;
+	uint bandunit;
 
-		wlc->home_chanspec = chanspec;
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d: 0x%x\n", wlc_hw->unit, chanspec);
 
-		FOREACH_BSS(wlc, idx, cfg) {
-			if (!cfg->associated)
-				continue;
+	wlc_hw->chanspec = chanspec;
 
-			cfg->current_bss->chanspec = chanspec;
+	/* Switch bands if necessary */
+	if (wlc_hw->_nbands > 1) {
+		bandunit = chspec_bandunit(chanspec);
+		if (wlc_hw->band->bandunit != bandunit) {
+			/* brcms_b_setband disables other bandunit,
+			 *  use light band switch if not up yet
+			 */
+			if (wlc_hw->up) {
+				wlc_phy_chanspec_radio_set(wlc_hw->
+							   bandstate[bandunit]->
+							   pi, chanspec);
+				brcms_b_setband(wlc_hw, bandunit, chanspec);
+			} else {
+				brcms_c_setxband(wlc_hw, bandunit);
+			}
 		}
+	}
+
+	wlc_phy_initcal_enable(wlc_hw->band->pi, !mute);
 
+	if (!wlc_hw->up) {
+		if (wlc_hw->clk)
+			wlc_phy_txpower_limit_set(wlc_hw->band->pi, txpwr,
+						  chanspec);
+		wlc_phy_chanspec_radio_set(wlc_hw->band->pi, chanspec);
+	} else {
+		wlc_phy_chanspec_set(wlc_hw->band->pi, chanspec);
+		wlc_phy_txpower_limit_set(wlc_hw->band->pi, txpwr, chanspec);
+
+		/* Update muting of the channel */
+		brcms_b_mute(wlc_hw, mute, 0);
 	}
 }
 
-static void brcms_c_set_phy_chanspec(struct brcms_c_info *wlc,
-				     chanspec_t chanspec)
+/* switch to and initialize new band */
+static void brcms_c_setband(struct brcms_c_info *wlc,
+					   uint bandunit)
 {
-	/* Save our copy of the chanspec */
-	wlc->chanspec = chanspec;
-
-	/* Set the chanspec and power limits for this locale after computing
-	 * any 11h local tx power constraints.
-	 */
-	brcms_c_channel_set_chanspec(wlc->cmi, chanspec,
-				 brcms_c_local_constraint_qdbm(wlc));
+	wlc->band = wlc->bandstate[bandunit];
 
-	if (wlc->stf->ss_algosel_auto)
-		brcms_c_stf_ss_algo_channel_get(wlc, &wlc->stf->ss_algo_channel,
-					    chanspec);
+	if (!wlc->pub->up)
+		return;
 
-	brcms_c_stf_ss_update(wlc, wlc->band);
+	/* wait for at least one beacon before entering sleeping state */
+	brcms_c_set_ps_ctrl(wlc);
 
+	/* band-specific initializations */
+	brcms_c_bsinit(wlc);
 }
 
-void brcms_c_set_chanspec(struct brcms_c_info *wlc, chanspec_t chanspec)
+static void brcms_c_set_chanspec(struct brcms_c_info *wlc, u16 chanspec)
 {
 	uint bandunit;
 	bool switchband = false;
-	chanspec_t old_chanspec = wlc->chanspec;
+	u16 old_chanspec = wlc->chanspec;
 
 	if (!brcms_c_valid_chanspec_db(wlc->cmi, chanspec)) {
 		wiphy_err(wlc->wiphy, "wl%d: %s: Bad channel %d\n",
@@ -780,8 +4050,8 @@ void brcms_c_set_chanspec(struct brcms_c_info *wlc, chanspec_t chanspec)
 	}
 
 	/* Switch bands if necessary */
-	if (NBANDS(wlc) > 1) {
-		bandunit = CHSPEC_BANDUNIT(chanspec);
+	if (wlc->pub->_nbands > 1) {
+		bandunit = chspec_bandunit(chanspec);
 		if (wlc->band->bandunit != bandunit || wlc->bandinit_pending) {
 			switchband = true;
 			if (wlc->bandlocked) {
@@ -807,53 +4077,28 @@ void brcms_c_set_chanspec(struct brcms_c_info *wlc, chanspec_t chanspec)
 	brcms_c_set_phy_chanspec(wlc, chanspec);
 
 	/* init antenna selection */
-	if (CHSPEC_WLC_BW(old_chanspec) != CHSPEC_WLC_BW(chanspec)) {
+	if (brcms_chspec_bw(old_chanspec) != brcms_chspec_bw(chanspec)) {
 		brcms_c_antsel_init(wlc->asi);
 
 		/* Fix the hardware rateset based on bw.
 		 * Mainly add MCS32 for 40Mhz, remove MCS 32 for 20Mhz
 		 */
 		brcms_c_rateset_bw_mcs_filter(&wlc->band->hw_rateset,
-					  wlc->band->
-					  mimo_cap_40 ? CHSPEC_WLC_BW(chanspec)
-					  : 0);
+			wlc->band->mimo_cap_40 ? brcms_chspec_bw(chanspec) : 0);
 	}
 
 	/* update some mac configuration since chanspec changed */
 	brcms_c_ucode_mac_upd(wlc);
 }
 
-ratespec_t brcms_c_lowest_basic_rspec(struct brcms_c_info *wlc,
-				      wlc_rateset_t *rs)
-{
-	ratespec_t lowest_basic_rspec;
-	uint i;
-
-	/* Use the lowest basic rate */
-	lowest_basic_rspec = rs->rates[0] & BRCMS_RATE_MASK;
-	for (i = 0; i < rs->count; i++) {
-		if (rs->rates[i] & BRCMS_RATE_FLAG) {
-			lowest_basic_rspec = rs->rates[i] & BRCMS_RATE_MASK;
-			break;
-		}
-	}
-#if NCONF
-	/* pick siso/cdd as default for OFDM (note no basic rate MCSs are supported yet) */
-	if (IS_OFDM(lowest_basic_rspec)) {
-		lowest_basic_rspec |= (wlc->stf->ss_opmode << RSPEC_STF_SHIFT);
-	}
-#endif
-
-	return lowest_basic_rspec;
-}
-
-/* This function changes the phytxctl for beacon based on current beacon ratespec AND txant
- * setting as per this table:
+/*
+ * This function changes the phytxctl for beacon based on current
+ * beacon ratespec AND txant setting as per this table:
  *  ratespec     CCK		ant = wlc->stf->txant
  *		OFDM		ant = 3
  */
 void brcms_c_beacon_phytxctl_txant_upd(struct brcms_c_info *wlc,
-				       ratespec_t bcn_rspec)
+				       u32 bcn_rspec)
 {
 	u16 phyctl;
 	u16 phytxant = wlc->stf->phytxant;
@@ -863,14 +4108,16 @@ void brcms_c_beacon_phytxctl_txant_upd(struct brcms_c_info *wlc,
 	if (BRCMS_PHY_11N_CAP(wlc->band))
 		phytxant = brcms_c_stf_phytxchain_sel(wlc, bcn_rspec);
 
-	phyctl = brcms_c_read_shm(wlc, M_BCN_PCTLWD);
+	phyctl = brcms_b_read_shm(wlc->hw, M_BCN_PCTLWD);
 	phyctl = (phyctl & ~mask) | phytxant;
-	brcms_c_write_shm(wlc, M_BCN_PCTLWD, phyctl);
+	brcms_b_write_shm(wlc->hw, M_BCN_PCTLWD, phyctl);
 }
 
-/* centralized protection config change function to simplify debugging, no consistency checking
- * this should be called only on changes to avoid overhead in periodic function
-*/
+/*
+ * centralized protection config change function to simplify debugging, no
+ * consistency checking this should be called only on changes to avoid overhead
+ * in periodic function
+ */
 void brcms_c_protection_upd(struct brcms_c_info *wlc, uint idx, int val)
 {
 	BCMMSG(wlc->wiphy, "idx %d, val %d\n", idx, val);
@@ -918,13 +4165,6 @@ void brcms_c_protection_upd(struct brcms_c_info *wlc, uint idx, int val)
 
 static void brcms_c_ht_update_sgi_rx(struct brcms_c_info *wlc, int val)
 {
-	wlc->ht_cap.cap_info &= ~(IEEE80211_HT_CAP_SGI_20 |
-					IEEE80211_HT_CAP_SGI_40);
-	wlc->ht_cap.cap_info |= (val & BRCMS_N_SGI_20) ?
-					IEEE80211_HT_CAP_SGI_20 : 0;
-	wlc->ht_cap.cap_info |= (val & BRCMS_N_SGI_40) ?
-					IEEE80211_HT_CAP_SGI_40 : 0;
-
 	if (wlc->pub->up) {
 		brcms_c_update_beacon(wlc);
 		brcms_c_update_probe_resp(wlc, true);
@@ -935,10 +4175,6 @@ static void brcms_c_ht_update_ldpc(struct brcms_c_info *wlc, s8 val)
 {
 	wlc->stf->ldpc = val;
 
-	wlc->ht_cap.cap_info &= ~IEEE80211_HT_CAP_LDPC_CODING;
-	if (wlc->stf->ldpc != OFF)
-		wlc->ht_cap.cap_info |= IEEE80211_HT_CAP_LDPC_CODING;
-
 	if (wlc->pub->up) {
 		brcms_c_update_beacon(wlc);
 		brcms_c_update_probe_resp(wlc, true);
@@ -946,262 +4182,347 @@ static void brcms_c_ht_update_ldpc(struct brcms_c_info *wlc, s8 val)
 	}
 }
 
-/*
- * ucode, hwmac update
- *    Channel dependent updates for ucode and hw
- */
-static void brcms_c_ucode_mac_upd(struct brcms_c_info *wlc)
+void brcms_c_wme_setparams(struct brcms_c_info *wlc, u16 aci,
+		       const struct ieee80211_tx_queue_params *params,
+		       bool suspend)
 {
-	/* enable or disable any active IBSSs depending on whether or not
-	 * we are on the home channel
-	 */
-	if (wlc->home_chanspec == BRCMS_BAND_PI_RADIO_CHANSPEC) {
-		if (wlc->pub->associated) {
-			/* BMAC_NOTE: This is something that should be fixed in ucode inits.
-			 * I think that the ucode inits set up the bcn templates and shm values
-			 * with a bogus beacon. This should not be done in the inits. If ucode needs
-			 * to set up a beacon for testing, the test routines should write it down,
-			 * not expect the inits to populate a bogus beacon.
-			 */
-			if (BRCMS_PHY_11N_CAP(wlc->band)) {
-				brcms_c_write_shm(wlc, M_BCN_TXTSF_OFFSET,
-					      wlc->band->bcntsfoff);
-			}
-		}
-	} else {
-		/* disable an active IBSS if we are not on the home channel */
+	int i;
+	struct shm_acparams acp_shm;
+	u16 *shm_entry;
+
+	/* Only apply params if the core is out of reset and has clocks */
+	if (!wlc->clk) {
+		wiphy_err(wlc->wiphy, "wl%d: %s : no-clock\n", wlc->pub->unit,
+			  __func__);
+		return;
 	}
 
-	/* update the various promisc bits */
-	brcms_c_mac_bcn_promisc(wlc);
-	brcms_c_mac_promisc(wlc);
-}
+	memset((char *)&acp_shm, 0, sizeof(struct shm_acparams));
+	/* fill in shm ac params struct */
+	acp_shm.txop = params->txop;
+	/* convert from units of 32us to us for ucode */
+	wlc->edcf_txop[aci & 0x3] = acp_shm.txop =
+	    EDCF_TXOP2USEC(acp_shm.txop);
+	acp_shm.aifs = (params->aifs & EDCF_AIFSN_MASK);
 
-static void brcms_c_bandinit_ordered(struct brcms_c_info *wlc,
-				     chanspec_t chanspec)
-{
-	wlc_rateset_t default_rateset;
-	uint parkband;
-	uint i, band_order[2];
+	if (aci == AC_VI && acp_shm.txop == 0
+	    && acp_shm.aifs < EDCF_AIFSN_MAX)
+		acp_shm.aifs++;
 
-	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
-	/*
-	 * We might have been bandlocked during down and the chip power-cycled (hibernate).
-	 * figure out the right band to park on
-	 */
-	if (wlc->bandlocked || NBANDS(wlc) == 1) {
-		/* updated in brcms_c_bandlock() */
-		parkband = wlc->band->bandunit;
-		band_order[0] = band_order[1] = parkband;
+	if (acp_shm.aifs < EDCF_AIFSN_MIN
+	    || acp_shm.aifs > EDCF_AIFSN_MAX) {
+		wiphy_err(wlc->wiphy, "wl%d: edcf_setparams: bad "
+			  "aifs %d\n", wlc->pub->unit, acp_shm.aifs);
 	} else {
-		/* park on the band of the specified chanspec */
-		parkband = CHSPEC_BANDUNIT(chanspec);
+		acp_shm.cwmin = params->cw_min;
+		acp_shm.cwmax = params->cw_max;
+		acp_shm.cwcur = acp_shm.cwmin;
+		acp_shm.bslots =
+		    R_REG(&wlc->regs->tsf_random) & acp_shm.cwcur;
+		acp_shm.reggap = acp_shm.bslots + acp_shm.aifs;
+		/* Indicate the new params to the ucode */
+		acp_shm.status = brcms_b_read_shm(wlc->hw, (M_EDCF_QINFO +
+						  wme_ac2fifo[aci] *
+						  M_EDCF_QLEN +
+						  M_EDCF_STATUS_OFF));
+		acp_shm.status |= WME_STATUS_NEWAC;
 
-		/* order so that parkband initialize last */
-		band_order[0] = parkband ^ 1;
-		band_order[1] = parkband;
+		/* Fill in shm acparam table */
+		shm_entry = (u16 *) &acp_shm;
+		for (i = 0; i < (int)sizeof(struct shm_acparams); i += 2)
+			brcms_b_write_shm(wlc->hw,
+					  M_EDCF_QINFO +
+					  wme_ac2fifo[aci] * M_EDCF_QLEN + i,
+					  *shm_entry++);
 	}
 
-	/* make each band operational, software state init */
-	for (i = 0; i < NBANDS(wlc); i++) {
-		uint j = band_order[i];
+	if (suspend) {
+		brcms_c_suspend_mac_and_wait(wlc);
+		brcms_c_enable_mac(wlc);
+	}
+}
 
-		wlc->band = wlc->bandstate[j];
+void brcms_c_edcf_setparams(struct brcms_c_info *wlc, bool suspend)
+{
+	u16 aci;
+	int i_ac;
+	struct ieee80211_tx_queue_params txq_pars;
+	static const struct edcf_acparam default_edcf_acparams[] = {
+		 {EDCF_AC_BE_ACI_STA, EDCF_AC_BE_ECW_STA, EDCF_AC_BE_TXOP_STA},
+		 {EDCF_AC_BK_ACI_STA, EDCF_AC_BK_ECW_STA, EDCF_AC_BK_TXOP_STA},
+		 {EDCF_AC_VI_ACI_STA, EDCF_AC_VI_ECW_STA, EDCF_AC_VI_TXOP_STA},
+		 {EDCF_AC_VO_ACI_STA, EDCF_AC_VO_ECW_STA, EDCF_AC_VO_TXOP_STA}
+	}; /* ucode needs these parameters during its initialization */
+	const struct edcf_acparam *edcf_acp = &default_edcf_acparams[0];
 
-		brcms_default_rateset(wlc, &default_rateset);
+	for (i_ac = 0; i_ac < AC_COUNT; i_ac++, edcf_acp++) {
+		/* find out which ac this set of params applies to */
+		aci = (edcf_acp->ACI & EDCF_ACI_MASK) >> EDCF_ACI_SHIFT;
 
-		/* fill in hw_rate */
-		brcms_c_rateset_filter(&default_rateset, &wlc->band->hw_rateset,
-				   false, BRCMS_RATES_CCK_OFDM, BRCMS_RATE_MASK,
-				   (bool) N_ENAB(wlc->pub));
+		/* fill in shm ac params struct */
+		txq_pars.txop = edcf_acp->TXOP;
+		txq_pars.aifs = edcf_acp->ACI;
 
-		/* init basic rate lookup */
-		brcms_c_rate_lookup_init(wlc, &default_rateset);
+		/* CWmin = 2^(ECWmin) - 1 */
+		txq_pars.cw_min = EDCF_ECW2CW(edcf_acp->ECW & EDCF_ECWMIN_MASK);
+		/* CWmax = 2^(ECWmax) - 1 */
+		txq_pars.cw_max = EDCF_ECW2CW((edcf_acp->ECW & EDCF_ECWMAX_MASK)
+					    >> EDCF_ECWMAX_SHIFT);
+		brcms_c_wme_setparams(wlc, aci, &txq_pars, suspend);
 	}
 
-	/* sync up phy/radio chanspec */
-	brcms_c_set_phy_chanspec(wlc, chanspec);
+	if (suspend) {
+		brcms_c_suspend_mac_and_wait(wlc);
+		brcms_c_enable_mac(wlc);
+	}
 }
 
-/* band-specific init */
-static void brcms_c_bsinit(struct brcms_c_info *wlc)
+/* maintain LED behavior in down state */
+static void brcms_c_down_led_upd(struct brcms_c_info *wlc)
 {
-	BCMMSG(wlc->wiphy, "wl%d: bandunit %d\n",
-		 wlc->pub->unit, wlc->band->bandunit);
+	/*
+	 * maintain LEDs while in down state, turn on sbclk if
+	 * not available yet. Turn on sbclk if necessary
+	 */
+	brcms_b_pllreq(wlc->hw, true, BRCMS_PLLREQ_FLIP);
+	brcms_b_pllreq(wlc->hw, false, BRCMS_PLLREQ_FLIP);
+}
 
-	/* write ucode ACK/CTS rate table */
-	brcms_c_set_ratetable(wlc);
+static void brcms_c_radio_monitor_start(struct brcms_c_info *wlc)
+{
+	/* Don't start the timer if HWRADIO feature is disabled */
+	if (wlc->radio_monitor)
+		return;
 
-	/* update some band specific mac configuration */
-	brcms_c_ucode_mac_upd(wlc);
+	wlc->radio_monitor = true;
+	brcms_b_pllreq(wlc->hw, true, BRCMS_PLLREQ_RADIO_MON);
+	brcms_add_timer(wlc->radio_timer, TIMER_INTERVAL_RADIOCHK, true);
+}
 
-	/* init antenna selection */
-	brcms_c_antsel_init(wlc->asi);
+static void brcms_c_radio_disable(struct brcms_c_info *wlc)
+{
+	if (!wlc->pub->up) {
+		brcms_c_down_led_upd(wlc);
+		return;
+	}
 
+	brcms_c_radio_monitor_start(wlc);
+	brcms_down(wlc->wl);
 }
 
-/* switch to and initialize new band */
-static void brcms_c_setband(struct brcms_c_info *wlc,
-					   uint bandunit)
+static void brcms_c_radio_enable(struct brcms_c_info *wlc)
 {
-	int idx;
-	struct brcms_bss_cfg *cfg;
+	if (wlc->pub->up)
+		return;
 
-	wlc->band = wlc->bandstate[bandunit];
+	if (brcms_deviceremoved(wlc))
+		return;
 
-	if (!wlc->pub->up)
+	brcms_up(wlc->wl);
+}
+
+static bool brcms_c_radio_monitor_stop(struct brcms_c_info *wlc)
+{
+	if (!wlc->radio_monitor)
+		return true;
+
+	wlc->radio_monitor = false;
+	brcms_b_pllreq(wlc->hw, false, BRCMS_PLLREQ_RADIO_MON);
+	return brcms_del_timer(wlc->radio_timer);
+}
+
+/* read hwdisable state and propagate to wlc flag */
+static void brcms_c_radio_hwdisable_upd(struct brcms_c_info *wlc)
+{
+	if (wlc->pub->hw_off)
 		return;
 
-	/* wait for at least one beacon before entering sleeping state */
-	for (idx = 0; idx < BRCMS_MAXBSSCFG; idx++) {
-		cfg = wlc->bsscfg[idx];
-		if (cfg && BSSCFG_STA(cfg) && cfg->associated)
-			cfg->PMawakebcn = true;
-	}
-	brcms_c_set_ps_ctrl(wlc);
+	if (brcms_b_radio_read_hwdisabled(wlc->hw))
+		mboolset(wlc->pub->radio_disabled, WL_RADIO_HW_DISABLE);
+	else
+		mboolclr(wlc->pub->radio_disabled, WL_RADIO_HW_DISABLE);
+}
 
-	/* band-specific initializations */
-	brcms_c_bsinit(wlc);
+/*
+ * centralized radio disable/enable function,
+ * invoke radio enable/disable after updating hwradio status
+ */
+static void brcms_c_radio_upd(struct brcms_c_info *wlc)
+{
+	if (wlc->pub->radio_disabled)
+		brcms_c_radio_disable(wlc);
+	else
+		brcms_c_radio_enable(wlc);
 }
 
-/* Initialize a WME Parameter Info Element with default STA parameters from WMM Spec, Table 12 */
-void
-brcms_c_wme_initparams_sta(struct brcms_c_info *wlc, struct wme_param_ie *pe)
-{
-	static const struct wme_param_ie stadef = {
-		WME_OUI,
-		WME_TYPE,
-		WME_SUBTYPE_PARAM_IE,
-		WME_VER,
-		0,
-		0,
-		{
-		 {EDCF_AC_BE_ACI_STA, EDCF_AC_BE_ECW_STA,
-		  cpu_to_le16(EDCF_AC_BE_TXOP_STA)},
-		 {EDCF_AC_BK_ACI_STA, EDCF_AC_BK_ECW_STA,
-		  cpu_to_le16(EDCF_AC_BK_TXOP_STA)},
-		 {EDCF_AC_VI_ACI_STA, EDCF_AC_VI_ECW_STA,
-		  cpu_to_le16(EDCF_AC_VI_TXOP_STA)},
-		 {EDCF_AC_VO_ACI_STA, EDCF_AC_VO_ECW_STA,
-		  cpu_to_le16(EDCF_AC_VO_TXOP_STA)}
-		 }
-	};
-	memcpy(pe, &stadef, sizeof(*pe));
+/* update hwradio status and return it */
+bool brcms_c_check_radio_disabled(struct brcms_c_info *wlc)
+{
+	brcms_c_radio_hwdisable_upd(wlc);
+
+	return mboolisset(wlc->pub->radio_disabled, WL_RADIO_HW_DISABLE) ?
+			true : false;
 }
 
-void brcms_c_wme_setparams(struct brcms_c_info *wlc, u16 aci,
-		       const struct ieee80211_tx_queue_params *params,
-		       bool suspend)
+/* periodical query hw radio button while driver is "down" */
+static void brcms_c_radio_timer(void *arg)
 {
-	int i;
-	struct shm_acparams acp_shm;
-	u16 *shm_entry;
+	struct brcms_c_info *wlc = (struct brcms_c_info *) arg;
 
-	/* Only apply params if the core is out of reset and has clocks */
-	if (!wlc->clk) {
-		wiphy_err(wlc->wiphy, "wl%d: %s : no-clock\n", wlc->pub->unit,
-			  __func__);
+	if (brcms_deviceremoved(wlc)) {
+		wiphy_err(wlc->wiphy, "wl%d: %s: dead chip\n", wlc->pub->unit,
+			__func__);
+		brcms_down(wlc->wl);
 		return;
 	}
 
-	do {
-		memset((char *)&acp_shm, 0, sizeof(struct shm_acparams));
-		/* fill in shm ac params struct */
-		acp_shm.txop = le16_to_cpu(params->txop);
-		/* convert from units of 32us to us for ucode */
-		wlc->edcf_txop[aci & 0x3] = acp_shm.txop =
-		    EDCF_TXOP2USEC(acp_shm.txop);
-		acp_shm.aifs = (params->aifs & EDCF_AIFSN_MASK);
-
-		if (aci == AC_VI && acp_shm.txop == 0
-		    && acp_shm.aifs < EDCF_AIFSN_MAX)
-			acp_shm.aifs++;
-
-		if (acp_shm.aifs < EDCF_AIFSN_MIN
-		    || acp_shm.aifs > EDCF_AIFSN_MAX) {
-			wiphy_err(wlc->wiphy, "wl%d: edcf_setparams: bad "
-				  "aifs %d\n", wlc->pub->unit, acp_shm.aifs);
-			continue;
-		}
+	/* cap mpc off count */
+	if (wlc->mpc_offcnt < BRCMS_MPC_MAX_DELAYCNT)
+		wlc->mpc_offcnt++;
 
-		acp_shm.cwmin = params->cw_min;
-		acp_shm.cwmax = params->cw_max;
-		acp_shm.cwcur = acp_shm.cwmin;
-		acp_shm.bslots =
-		    R_REG(&wlc->regs->tsf_random) & acp_shm.cwcur;
-		acp_shm.reggap = acp_shm.bslots + acp_shm.aifs;
-		/* Indicate the new params to the ucode */
-		acp_shm.status = brcms_c_read_shm(wlc, (M_EDCF_QINFO +
-						    wme_shmemacindex(aci) *
-						    M_EDCF_QLEN +
-						    M_EDCF_STATUS_OFF));
-		acp_shm.status |= WME_STATUS_NEWAC;
+	brcms_c_radio_hwdisable_upd(wlc);
+	brcms_c_radio_upd(wlc);
+}
 
-		/* Fill in shm acparam table */
-		shm_entry = (u16 *) &acp_shm;
-		for (i = 0; i < (int)sizeof(struct shm_acparams); i += 2)
-			brcms_c_write_shm(wlc,
-				      M_EDCF_QINFO +
-				      wme_shmemacindex(aci) * M_EDCF_QLEN + i,
-				      *shm_entry++);
+/* common low-level watchdog code */
+static void brcms_b_watchdog(void *arg)
+{
+	struct brcms_c_info *wlc = (struct brcms_c_info *) arg;
+	struct brcms_hardware *wlc_hw = wlc->hw;
 
-	} while (0);
+	BCMMSG(wlc->wiphy, "wl%d\n", wlc_hw->unit);
 
-	if (suspend)
-		brcms_c_suspend_mac_and_wait(wlc);
+	if (!wlc_hw->up)
+		return;
 
-	if (suspend)
-		brcms_c_enable_mac(wlc);
+	/* increment second count */
+	wlc_hw->now++;
+
+	/* Check for FIFO error interrupts */
+	brcms_b_fifoerrors(wlc_hw);
+
+	/* make sure RX dma has buffers */
+	dma_rxfill(wlc->hw->di[RX_FIFO]);
 
+	wlc_phy_watchdog(wlc_hw->band->pi);
 }
 
-void brcms_c_edcf_setparams(struct brcms_c_info *wlc, bool suspend)
+static void brcms_c_radio_mpc_upd(struct brcms_c_info *wlc)
 {
-	u16 aci;
-	int i_ac;
-	struct edcf_acparam *edcf_acp;
+	bool mpc_radio, radio_state;
 
-	struct ieee80211_tx_queue_params txq_pars;
-	struct ieee80211_tx_queue_params *params = &txq_pars;
+	/*
+	 * Clear the WL_RADIO_MPC_DISABLE bit when mpc feature is disabled
+	 * in case the WL_RADIO_MPC_DISABLE bit was set. Stop the radio
+	 * monitor also when WL_RADIO_MPC_DISABLE is the only reason that
+	 * the radio is going down.
+	 */
+	if (!wlc->mpc) {
+		if (!wlc->pub->radio_disabled)
+			return;
+		mboolclr(wlc->pub->radio_disabled, WL_RADIO_MPC_DISABLE);
+		brcms_c_radio_upd(wlc);
+		if (!wlc->pub->radio_disabled)
+			brcms_c_radio_monitor_stop(wlc);
+		return;
+	}
 
 	/*
-	 * AP uses AC params from wme_param_ie_ap.
-	 * AP advertises AC params from wme_param_ie.
-	 * STA uses AC params from wme_param_ie.
+	 * sync ismpc logic with WL_RADIO_MPC_DISABLE bit in
+	 * wlc->pub->radio_disabled to go ON, always call radio_upd
+	 * synchronously to go OFF, postpone radio_upd to later when
+	 * context is safe(e.g. watchdog)
 	 */
+	radio_state =
+	    (mboolisset(wlc->pub->radio_disabled, WL_RADIO_MPC_DISABLE) ? OFF :
+	     ON);
+	mpc_radio = (brcms_c_ismpc(wlc) == true) ? OFF : ON;
 
-	edcf_acp = (struct edcf_acparam *) &wlc->wme_param_ie.acparam[0];
+	if (radio_state == ON && mpc_radio == OFF)
+		wlc->mpc_delay_off = wlc->mpc_dlycnt;
+	else if (radio_state == OFF && mpc_radio == ON) {
+		mboolclr(wlc->pub->radio_disabled, WL_RADIO_MPC_DISABLE);
+		brcms_c_radio_upd(wlc);
+		if (wlc->mpc_offcnt < BRCMS_MPC_THRESHOLD)
+			wlc->mpc_dlycnt = BRCMS_MPC_MAX_DELAYCNT;
+		else
+			wlc->mpc_dlycnt = BRCMS_MPC_MIN_DELAYCNT;
+	}
+	/*
+	 * Below logic is meant to capture the transition from mpc off
+	 * to mpc on for reasons other than wlc->mpc_delay_off keeping
+	 * the mpc off. In that case reset wlc->mpc_delay_off to
+	 * wlc->mpc_dlycnt, so that we restart the countdown of mpc_delay_off
+	 */
+	if ((wlc->prev_non_delay_mpc == false) &&
+	    (brcms_c_is_non_delay_mpc(wlc) == true) && wlc->mpc_delay_off)
+		wlc->mpc_delay_off = wlc->mpc_dlycnt;
 
-	for (i_ac = 0; i_ac < AC_COUNT; i_ac++, edcf_acp++) {
-		/* find out which ac this set of params applies to */
-		aci = (edcf_acp->ACI & EDCF_ACI_MASK) >> EDCF_ACI_SHIFT;
+	wlc->prev_non_delay_mpc = brcms_c_is_non_delay_mpc(wlc);
+}
 
-		/* fill in shm ac params struct */
-		params->txop = edcf_acp->TXOP;
-		params->aifs = edcf_acp->ACI;
+/* common watchdog code */
+static void brcms_c_watchdog(void *arg)
+{
+	struct brcms_c_info *wlc = (struct brcms_c_info *) arg;
 
-		/* CWmin = 2^(ECWmin) - 1 */
-		params->cw_min = EDCF_ECW2CW(edcf_acp->ECW & EDCF_ECWMIN_MASK);
-		/* CWmax = 2^(ECWmax) - 1 */
-		params->cw_max = EDCF_ECW2CW((edcf_acp->ECW & EDCF_ECWMAX_MASK)
-					    >> EDCF_ECWMAX_SHIFT);
-		brcms_c_wme_setparams(wlc, aci, params, suspend);
+	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
+
+	if (!wlc->pub->up)
+		return;
+
+	if (brcms_deviceremoved(wlc)) {
+		wiphy_err(wlc->wiphy, "wl%d: %s: dead chip\n", wlc->pub->unit,
+			  __func__);
+		brcms_down(wlc->wl);
+		return;
 	}
 
-	if (suspend)
-		brcms_c_suspend_mac_and_wait(wlc);
+	/* increment second count */
+	wlc->pub->now++;
 
-	if (AP_ENAB(wlc->pub) && WME_ENAB(wlc->pub)) {
-		brcms_c_update_beacon(wlc);
-		brcms_c_update_probe_resp(wlc, false);
+	/* delay radio disable */
+	if (wlc->mpc_delay_off) {
+		if (--wlc->mpc_delay_off == 0) {
+			mboolset(wlc->pub->radio_disabled,
+				 WL_RADIO_MPC_DISABLE);
+			if (wlc->mpc && brcms_c_ismpc(wlc))
+				wlc->mpc_offcnt = 0;
+		}
 	}
 
-	if (suspend)
-		brcms_c_enable_mac(wlc);
+	/* mpc sync */
+	brcms_c_radio_mpc_upd(wlc);
+	/* radio sync: sw/hw/mpc --> radio_disable/radio_enable */
+	brcms_c_radio_hwdisable_upd(wlc);
+	brcms_c_radio_upd(wlc);
+	/* if radio is disable, driver may be down, quit here */
+	if (wlc->pub->radio_disabled)
+		return;
+
+	brcms_b_watchdog(wlc);
+
+	/*
+	 * occasionally sample mac stat counters to
+	 * detect 16-bit counter wrap
+	 */
+	if ((wlc->pub->now % SW_TIMER_MAC_STAT_UPD) == 0)
+		brcms_c_statsupd(wlc);
 
+	if (BRCMS_ISNPHY(wlc->band) &&
+	    ((wlc->pub->now - wlc->tempsense_lasttime) >=
+	     BRCMS_TEMPSENSE_PERIOD)) {
+		wlc->tempsense_lasttime = wlc->pub->now;
+		brcms_c_tempsense_upd(wlc);
+	}
 }
 
-bool brcms_c_timers_init(struct brcms_c_info *wlc, int unit)
+static void brcms_c_watchdog_by_timer(void *arg)
+{
+	brcms_c_watchdog(arg);
+}
+
+static bool brcms_c_timers_init(struct brcms_c_info *wlc, int unit)
 {
 	wlc->wdtimer = brcms_init_timer(wlc->wl, brcms_c_watchdog_by_timer,
 		wlc, "watchdog");
@@ -1229,14 +4550,12 @@ bool brcms_c_timers_init(struct brcms_c_info *wlc, int unit)
  * Initialize brcms_c_info default values ...
  * may get overrides later in this function
  */
-void brcms_c_info_init(struct brcms_c_info *wlc, int unit)
+static void brcms_c_info_init(struct brcms_c_info *wlc, int unit)
 {
 	int i;
-	/* Assume the device is there until proven otherwise */
-	wlc->device_present = true;
 
 	/* Save our copy of the chanspec */
-	wlc->chanspec = CH20MHZ_CHSPEC(1);
+	wlc->chanspec = ch20mhz_chspec(1);
 
 	/* various 802.11g modes */
 	wlc->shortslot = false;
@@ -1277,44 +4596,14 @@ void brcms_c_info_init(struct brcms_c_info *wlc, int unit)
 	wlc->SRL = RETRY_SHORT_DEF;
 	wlc->LRL = RETRY_LONG_DEF;
 
-	/* Set flag to indicate that hw keys should be used when available. */
-	wlc->wsec_swkeys = false;
-
-	/* init the 4 static WEP default keys */
-	for (i = 0; i < WSEC_MAX_DEFAULT_KEYS; i++) {
-		wlc->wsec_keys[i] = wlc->wsec_def_keys[i];
-		wlc->wsec_keys[i]->idx = (u8) i;
-	}
-
 	/* WME QoS mode is Auto by default */
-	wlc->pub->_wme = AUTO;
-
-#ifdef BCMSDIODEV_ENABLED
-	wlc->pub->_priofc = true;	/* enable priority flow control for sdio dongle */
-#endif
-
 	wlc->pub->_ampdu = AMPDU_AGG_HOST;
 	wlc->pub->bcmerror = 0;
-	wlc->pub->_coex = ON;
 
 	/* initialize mpc delay */
 	wlc->mpc_delay_off = wlc->mpc_dlycnt = BRCMS_MPC_MIN_DELAYCNT;
 }
 
-static bool brcms_c_state_bmac_sync(struct brcms_c_info *wlc)
-{
-	struct brcms_b_state state_bmac;
-
-	if (brcms_b_state_get(wlc->hw, &state_bmac) != 0)
-		return false;
-
-	wlc->machwcap = state_bmac.machwcap;
-	brcms_c_protection_upd(wlc, BRCMS_PROT_N_PAM_OVR,
-			   (s8) state_bmac.preamble_ovr);
-
-	return true;
-}
-
 static uint brcms_c_attach_module(struct brcms_c_info *wlc)
 {
 	uint err = 0;
@@ -1347,262 +4636,325 @@ static uint brcms_c_attach_module(struct brcms_c_info *wlc)
 	return err;
 }
 
-struct brcms_pub *brcms_c_pub(void *wlc)
+struct brcms_pub *brcms_c_pub(struct brcms_c_info *wlc)
 {
-	return ((struct brcms_c_info *) wlc)->pub;
+	return wlc->pub;
 }
 
-#define CHIP_SUPPORTS_11N(wlc)	1
-
-/*
- * The common driver entry routine. Error codes should be unique
+/* low level attach
+ *    run backplane attach, init nvram
+ *    run phy attach
+ *    initialize software state for each core and band
+ *    put the whole chip in reset(driver down state), no clock
  */
-void *brcms_c_attach(struct brcms_info *wl, u16 vendor, u16 device, uint unit,
-		 bool piomode, void *regsva, uint bustype, void *btparam,
-		 uint *perr)
+static int brcms_b_attach(struct brcms_c_info *wlc, u16 vendor, u16 device,
+			  uint unit, bool piomode, void __iomem *regsva,
+			  struct pci_dev *btparam)
 {
-	struct brcms_c_info *wlc;
+	struct brcms_hardware *wlc_hw;
+	struct d11regs __iomem *regs;
+	char *macaddr = NULL;
 	uint err = 0;
 	uint j;
-	struct brcms_pub *pub;
-	uint n_disabled;
+	bool wme = false;
+	struct shared_phy_params sha_params;
+	struct wiphy *wiphy = wlc->wiphy;
 
-	/* allocate struct brcms_c_info state and its substructures */
-	wlc = (struct brcms_c_info *) brcms_c_attach_malloc(unit, &err, device);
-	if (wlc == NULL)
-		goto fail;
-	wlc->wiphy = wl->wiphy;
-	pub = wlc->pub;
+	BCMMSG(wlc->wiphy, "wl%d: vendor 0x%x device 0x%x\n", unit, vendor,
+		device);
 
-#if defined(BCMDBG)
-	wlc_info_dbg = wlc;
-#endif
+	wme = true;
 
-	wlc->band = wlc->bandstate[0];
-	wlc->core = wlc->corestate;
-	wlc->wl = wl;
-	pub->unit = unit;
-	pub->_piomode = piomode;
-	wlc->bandinit_pending = false;
+	wlc_hw = wlc->hw;
+	wlc_hw->wlc = wlc;
+	wlc_hw->unit = unit;
+	wlc_hw->band = wlc_hw->bandstate[0];
+	wlc_hw->_piomode = piomode;
 
-	/* populate struct brcms_c_info with default values  */
-	brcms_c_info_init(wlc, unit);
-
-	/* update sta/ap related parameters */
-	brcms_c_ap_upd(wlc);
-
-	/* 11n_disable nvram */
-	n_disabled = getintvar(pub->vars, "11n_disable");
+	/* populate struct brcms_hardware with default values  */
+	brcms_b_info_init(wlc_hw);
 
 	/*
-	 * low level attach steps(all hw accesses go
-	 * inside, no more in rest of the attach)
-	 */
-	err = brcms_b_attach(wlc, vendor, device, unit, piomode, regsva,
-			      bustype, btparam);
-	if (err)
-		goto fail;
-
-	/* for some states, due to different info pointer(e,g, wlc, wlc_hw) or master/slave split,
-	 * HIGH driver(both monolithic and HIGH_ONLY) needs to sync states FROM BMAC portion driver
+	 * Do the hardware portion of the attach. Also initialize software
+	 * state that depends on the particular hardware we are running.
 	 */
-	if (!brcms_c_state_bmac_sync(wlc)) {
-		err = 20;
+	wlc_hw->sih = ai_attach(regsva, btparam);
+	if (wlc_hw->sih == NULL) {
+		wiphy_err(wiphy, "wl%d: brcms_b_attach: si_attach failed\n",
+			  unit);
+		err = 11;
 		goto fail;
 	}
 
-	pub->phy_11ncapable = BRCMS_PHY_11N_CAP(wlc->band);
-
-	/* propagate *vars* from BMAC driver to high driver */
-	brcms_b_copyfrom_vars(wlc->hw, &pub->vars, &wlc->vars_size);
-
-
-	/* set maximum allowed duty cycle */
-	wlc->tx_duty_cycle_ofdm =
-	    (u16) getintvar(pub->vars, "tx_duty_cycle_ofdm");
-	wlc->tx_duty_cycle_cck =
-	    (u16) getintvar(pub->vars, "tx_duty_cycle_cck");
-
-	brcms_c_stf_phy_chain_calc(wlc);
-
-	/* txchain 1: txant 0, txchain 2: txant 1 */
-	if (BRCMS_ISNPHY(wlc->band) && (wlc->stf->txstreams == 1))
-		wlc->stf->txant = wlc->stf->hw_txchain - 1;
-
-	/* push to BMAC driver */
-	wlc_phy_stf_chain_init(wlc->band->pi, wlc->stf->hw_txchain,
-			       wlc->stf->hw_rxchain);
-
-	/* pull up some info resulting from the low attach */
-	{
-		int i;
-		for (i = 0; i < NFIFO; i++)
-			wlc->core->txavail[i] = wlc->hw->txavail[i];
+	/* verify again the device is supported */
+	if (!brcms_c_chipmatch(vendor, device)) {
+		wiphy_err(wiphy, "wl%d: brcms_b_attach: Unsupported "
+			"vendor/device (0x%x/0x%x)\n",
+			 unit, vendor, device);
+		err = 12;
+		goto fail;
 	}
 
-	brcms_b_hw_etheraddr(wlc->hw, wlc->perm_etheraddr);
-
-	memcpy(&pub->cur_etheraddr, &wlc->perm_etheraddr, ETH_ALEN);
-
-	for (j = 0; j < NBANDS(wlc); j++) {
-		/* Use band 1 for single band 11a */
-		if (IS_SINGLEBAND_5G(wlc->deviceid))
-			j = BAND_5G_INDEX;
-
-		wlc->band = wlc->bandstate[j];
-
-		if (!brcms_c_attach_stf_ant_init(wlc)) {
-			err = 24;
-			goto fail;
-		}
+	wlc_hw->vendorid = vendor;
+	wlc_hw->deviceid = device;
 
-		/* default contention windows size limits */
-		wlc->band->CWmin = APHY_CWMIN;
-		wlc->band->CWmax = PHY_CWMAX;
+	/* set bar0 window to point at D11 core */
+	wlc_hw->regs = (struct d11regs __iomem *)
+				ai_setcore(wlc_hw->sih, D11_CORE_ID, 0);
+	wlc_hw->corerev = ai_corerev(wlc_hw->sih);
 
-		/* init gmode value */
-		if (BAND_2G(wlc->band->bandtype)) {
-			wlc->band->gmode = GMODE_AUTO;
-			brcms_c_protection_upd(wlc, BRCMS_PROT_G_USER,
-					   wlc->band->gmode);
-		}
+	regs = wlc_hw->regs;
 
-		/* init _n_enab supported mode */
-		if (BRCMS_PHY_11N_CAP(wlc->band) && CHIP_SUPPORTS_11N(wlc)) {
-			if (n_disabled & WLFEATURE_DISABLE_11N) {
-				pub->_n_enab = OFF;
-				brcms_c_protection_upd(wlc, BRCMS_PROT_N_USER,
-						       OFF);
-			} else {
-				pub->_n_enab = SUPPORT_11N;
-				brcms_c_protection_upd(wlc, BRCMS_PROT_N_USER,
-						   ((pub->_n_enab ==
-						     SUPPORT_11N) ? WL_11N_2x2 :
-						    WL_11N_3x3));
-			}
-		}
+	wlc->regs = wlc_hw->regs;
 
-		/* init per-band default rateset, depend on band->gmode */
-		brcms_default_rateset(wlc, &wlc->band->defrateset);
-
-		/* fill in hw_rateset (used early by BRCM_SET_RATESET) */
-		brcms_c_rateset_filter(&wlc->band->defrateset,
-				   &wlc->band->hw_rateset, false,
-				   BRCMS_RATES_CCK_OFDM, BRCMS_RATE_MASK,
-				   (bool) N_ENAB(wlc->pub));
+	/* validate chip, chiprev and corerev */
+	if (!brcms_c_isgoodchip(wlc_hw)) {
+		err = 13;
+		goto fail;
 	}
 
-	/* update antenna config due to wlc->stf->txant/txchain/ant_rx_ovr change */
-	brcms_c_stf_phy_txant_upd(wlc);
+	/* initialize power control registers */
+	ai_clkctl_init(wlc_hw->sih);
 
-	/* attach each modules */
-	err = brcms_c_attach_module(wlc);
-	if (err != 0)
-		goto fail;
+	/* request fastclock and force fastclock for the rest of attach
+	 * bring the d11 core out of reset.
+	 *   For PMU chips, the first wlc_clkctl_clk is no-op since core-clk
+	 *   is still false; But it will be called again inside wlc_corereset,
+	 *   after d11 is out of reset.
+	 */
+	brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
+	brcms_b_corereset(wlc_hw, BRCMS_USE_COREFLAGS);
 
-	if (!brcms_c_timers_init(wlc, unit)) {
-		wiphy_err(wl->wiphy, "wl%d: %s: init_timer failed\n", unit,
-			  __func__);
-		err = 32;
+	if (!brcms_b_validate_chip_access(wlc_hw)) {
+		wiphy_err(wiphy, "wl%d: brcms_b_attach: validate_chip_access "
+			"failed\n", unit);
+		err = 14;
 		goto fail;
 	}
 
-	/* depend on rateset, gmode */
-	wlc->cmi = brcms_c_channel_mgr_attach(wlc);
-	if (!wlc->cmi) {
-		wiphy_err(wl->wiphy, "wl%d: %s: channel_mgr_attach failed"
-			  "\n", unit, __func__);
-		err = 33;
+	/* get the board rev, used just below */
+	j = getintvar(wlc_hw->sih, BRCMS_SROM_BOARDREV);
+	/* promote srom boardrev of 0xFF to 1 */
+	if (j == BOARDREV_PROMOTABLE)
+		j = BOARDREV_PROMOTED;
+	wlc_hw->boardrev = (u16) j;
+	if (!brcms_c_validboardtype(wlc_hw)) {
+		wiphy_err(wiphy, "wl%d: brcms_b_attach: Unsupported Broadcom "
+			"board type (0x%x)" " or revision level (0x%x)\n",
+			 unit, wlc_hw->sih->boardtype, wlc_hw->boardrev);
+		err = 15;
 		goto fail;
 	}
+	wlc_hw->sromrev = (u8) getintvar(wlc_hw->sih, BRCMS_SROM_REV);
+	wlc_hw->boardflags = (u32) getintvar(wlc_hw->sih,
+					     BRCMS_SROM_BOARDFLAGS);
+	wlc_hw->boardflags2 = (u32) getintvar(wlc_hw->sih,
+					      BRCMS_SROM_BOARDFLAGS2);
 
-	/* init default when all parameters are ready, i.e. ->rateset */
-	brcms_c_bss_default_init(wlc);
+	if (wlc_hw->boardflags & BFL_NOPLLDOWN)
+		brcms_b_pllreq(wlc_hw, true, BRCMS_PLLREQ_SHARED);
 
-	/*
-	 * Complete the wlc default state initializations..
+	/* check device id(srom, nvram etc.) to set bands */
+	if (wlc_hw->deviceid == BCM43224_D11N_ID ||
+	    wlc_hw->deviceid == BCM43224_D11N_ID_VEN1)
+		/* Dualband boards */
+		wlc_hw->_nbands = 2;
+	else
+		wlc_hw->_nbands = 1;
+
+	if ((wlc_hw->sih->chip == BCM43225_CHIP_ID))
+		wlc_hw->_nbands = 1;
+
+	/* BMAC_NOTE: remove init of pub values when brcms_c_attach()
+	 * unconditionally does the init of these values
 	 */
+	wlc->vendorid = wlc_hw->vendorid;
+	wlc->deviceid = wlc_hw->deviceid;
+	wlc->pub->sih = wlc_hw->sih;
+	wlc->pub->corerev = wlc_hw->corerev;
+	wlc->pub->sromrev = wlc_hw->sromrev;
+	wlc->pub->boardrev = wlc_hw->boardrev;
+	wlc->pub->boardflags = wlc_hw->boardflags;
+	wlc->pub->boardflags2 = wlc_hw->boardflags2;
+	wlc->pub->_nbands = wlc_hw->_nbands;
+
+	wlc_hw->physhim = wlc_phy_shim_attach(wlc_hw, wlc->wl, wlc);
+
+	if (wlc_hw->physhim == NULL) {
+		wiphy_err(wiphy, "wl%d: brcms_b_attach: wlc_phy_shim_attach "
+			"failed\n", unit);
+		err = 25;
+		goto fail;
+	}
 
-	/* allocate our initial queue */
-	wlc->pkt_queue = brcms_c_txq_alloc(wlc);
-	if (wlc->pkt_queue == NULL) {
-		wiphy_err(wl->wiphy, "wl%d: %s: failed to malloc tx queue\n",
-			  unit, __func__);
-		err = 100;
+	/* pass all the parameters to wlc_phy_shared_attach in one struct */
+	sha_params.sih = wlc_hw->sih;
+	sha_params.physhim = wlc_hw->physhim;
+	sha_params.unit = unit;
+	sha_params.corerev = wlc_hw->corerev;
+	sha_params.vid = wlc_hw->vendorid;
+	sha_params.did = wlc_hw->deviceid;
+	sha_params.chip = wlc_hw->sih->chip;
+	sha_params.chiprev = wlc_hw->sih->chiprev;
+	sha_params.chippkg = wlc_hw->sih->chippkg;
+	sha_params.sromrev = wlc_hw->sromrev;
+	sha_params.boardtype = wlc_hw->sih->boardtype;
+	sha_params.boardrev = wlc_hw->boardrev;
+	sha_params.boardvendor = wlc_hw->sih->boardvendor;
+	sha_params.boardflags = wlc_hw->boardflags;
+	sha_params.boardflags2 = wlc_hw->boardflags2;
+	sha_params.buscorerev = wlc_hw->sih->buscorerev;
+
+	/* alloc and save pointer to shared phy state area */
+	wlc_hw->phy_sh = wlc_phy_shared_attach(&sha_params);
+	if (!wlc_hw->phy_sh) {
+		err = 16;
 		goto fail;
 	}
 
-	wlc->bsscfg[0] = wlc->cfg;
-	wlc->cfg->_idx = 0;
-	wlc->cfg->wlc = wlc;
-	pub->txmaxpkts = MAXTXPKTS;
+	/* initialize software state for each core and band */
+	for (j = 0; j < wlc_hw->_nbands; j++) {
+		/*
+		 * band0 is always 2.4Ghz
+		 * band1, if present, is 5Ghz
+		 */
 
-	brcms_c_wme_initparams_sta(wlc, &wlc->wme_param_ie);
+		brcms_c_setxband(wlc_hw, j);
+
+		wlc_hw->band->bandunit = j;
+		wlc_hw->band->bandtype = j ? BRCM_BAND_5G : BRCM_BAND_2G;
+		wlc->band->bandunit = j;
+		wlc->band->bandtype = j ? BRCM_BAND_5G : BRCM_BAND_2G;
+		wlc->core->coreidx = ai_coreidx(wlc_hw->sih);
+
+		wlc_hw->machwcap = R_REG(&regs->machwcap);
+		wlc_hw->machwcap_backup = wlc_hw->machwcap;
+
+		/* init tx fifo size */
+		wlc_hw->xmtfifo_sz =
+		    xmtfifo_sz[(wlc_hw->corerev - XMTFIFOTBL_STARTREV)];
+
+		/* Get a phy for this band */
+		wlc_hw->band->pi =
+			wlc_phy_attach(wlc_hw->phy_sh, regs,
+				       wlc_hw->band->bandtype,
+				       wlc->wiphy);
+		if (wlc_hw->band->pi == NULL) {
+			wiphy_err(wiphy, "wl%d: brcms_b_attach: wlc_phy_"
+				  "attach failed\n", unit);
+			err = 17;
+			goto fail;
+		}
 
-	wlc->mimoft = FT_HT;
-	wlc->ht_cap.cap_info = HT_CAP;
-	if (HT_ENAB(wlc->pub))
-		wlc->stf->ldpc = AUTO;
+		wlc_phy_machwcap_set(wlc_hw->band->pi, wlc_hw->machwcap);
+
+		wlc_phy_get_phyversion(wlc_hw->band->pi, &wlc_hw->band->phytype,
+				       &wlc_hw->band->phyrev,
+				       &wlc_hw->band->radioid,
+				       &wlc_hw->band->radiorev);
+		wlc_hw->band->abgphy_encore =
+		    wlc_phy_get_encore(wlc_hw->band->pi);
+		wlc->band->abgphy_encore = wlc_phy_get_encore(wlc_hw->band->pi);
+		wlc_hw->band->core_flags =
+		    wlc_phy_get_coreflags(wlc_hw->band->pi);
+
+		/* verify good phy_type & supported phy revision */
+		if (BRCMS_ISNPHY(wlc_hw->band)) {
+			if (NCONF_HAS(wlc_hw->band->phyrev))
+				goto good_phy;
+			else
+				goto bad_phy;
+		} else if (BRCMS_ISLCNPHY(wlc_hw->band)) {
+			if (LCNCONF_HAS(wlc_hw->band->phyrev))
+				goto good_phy;
+			else
+				goto bad_phy;
+		} else {
+ bad_phy:
+			wiphy_err(wiphy, "wl%d: brcms_b_attach: unsupported "
+				  "phy type/rev (%d/%d)\n", unit,
+				  wlc_hw->band->phytype, wlc_hw->band->phyrev);
+			err = 18;
+			goto fail;
+		}
 
-	wlc->mimo_40txbw = AUTO;
-	wlc->ofdm_40txbw = AUTO;
-	wlc->cck_40txbw = AUTO;
-	brcms_c_update_mimo_band_bwcap(wlc, BRCMS_N_BW_20IN2G_40IN5G);
+ good_phy:
+		/*
+		 * BMAC_NOTE: wlc->band->pi should not be set below and should
+		 * be done in the high level attach. However we can not make
+		 * that change until all low level access is changed to
+		 * wlc_hw->band->pi. Instead do the wlc->band->pi init below,
+		 * keeping wlc_hw->band->pi as well for incremental update of
+		 * low level fns, and cut over low only init when all fns
+		 * updated.
+		 */
+		wlc->band->pi = wlc_hw->band->pi;
+		wlc->band->phytype = wlc_hw->band->phytype;
+		wlc->band->phyrev = wlc_hw->band->phyrev;
+		wlc->band->radioid = wlc_hw->band->radioid;
+		wlc->band->radiorev = wlc_hw->band->radiorev;
 
-	/* Set default values of SGI */
-	if (BRCMS_SGI_CAP_PHY(wlc)) {
-		brcms_c_ht_update_sgi_rx(wlc, (BRCMS_N_SGI_20 |
-					       BRCMS_N_SGI_40));
-		wlc->sgi_tx = AUTO;
-	} else if (BRCMS_ISSSLPNPHY(wlc->band)) {
-		brcms_c_ht_update_sgi_rx(wlc, (BRCMS_N_SGI_20 |
-					       BRCMS_N_SGI_40));
-		wlc->sgi_tx = AUTO;
-	} else {
-		brcms_c_ht_update_sgi_rx(wlc, 0);
-		wlc->sgi_tx = OFF;
+		/* default contention windows size limits */
+		wlc_hw->band->CWmin = APHY_CWMIN;
+		wlc_hw->band->CWmax = PHY_CWMAX;
+
+		if (!brcms_b_attach_dmapio(wlc, j, wme)) {
+			err = 19;
+			goto fail;
+		}
 	}
 
-	/* *******nvram 11n config overrides Start ********* */
+	/* disable core to match driver "down" state */
+	brcms_c_coredisable(wlc_hw);
 
-	/* apply the sgi override from nvram conf */
-	if (n_disabled & WLFEATURE_DISABLE_11N_SGI_TX)
-		wlc->sgi_tx = OFF;
+	/* Match driver "down" state */
+	ai_pci_down(wlc_hw->sih);
 
-	if (n_disabled & WLFEATURE_DISABLE_11N_SGI_RX)
-		brcms_c_ht_update_sgi_rx(wlc, 0);
+	/* register sb interrupt callback functions */
+	ai_register_intr_callback(wlc_hw->sih, (void *)brcms_c_wlintrsoff,
+				  (void *)brcms_c_wlintrsrestore, NULL, wlc);
 
-	/* apply the stbc override from nvram conf */
-	if (n_disabled & WLFEATURE_DISABLE_11N_STBC_TX) {
-		wlc->bandstate[BAND_2G_INDEX]->band_stf_stbc_tx = OFF;
-		wlc->bandstate[BAND_5G_INDEX]->band_stf_stbc_tx = OFF;
-		wlc->ht_cap.cap_info &= ~IEEE80211_HT_CAP_TX_STBC;
-	}
-	if (n_disabled & WLFEATURE_DISABLE_11N_STBC_RX)
-		brcms_c_stf_stbc_rx_set(wlc, HT_CAP_RX_STBC_NO);
+	/* turn off pll and xtal to match driver "down" state */
+	brcms_b_xtal(wlc_hw, OFF);
 
-	/* apply the GF override from nvram conf */
-	if (n_disabled & WLFEATURE_DISABLE_11N_GF)
-		wlc->ht_cap.cap_info &= ~IEEE80211_HT_CAP_GRN_FLD;
+	/* *******************************************************************
+	 * The hardware is in the DOWN state at this point. D11 core
+	 * or cores are in reset with clocks off, and the board PLLs
+	 * are off if possible.
+	 *
+	 * Beyond this point, wlc->sbclk == false and chip registers
+	 * should not be touched.
+	 *********************************************************************
+	 */
 
-	/* initialize radio_mpc_disable according to wlc->mpc */
-	brcms_c_radio_mpc_upd(wlc);
-	brcms_b_antsel_set(wlc->hw, wlc->asi->antsel_avail);
+	/* init etheraddr state variables */
+	macaddr = brcms_c_get_macaddr(wlc_hw);
+	if (macaddr == NULL) {
+		wiphy_err(wiphy, "wl%d: brcms_b_attach: macaddr not found\n",
+			  unit);
+		err = 21;
+		goto fail;
+	}
+	if (!mac_pton(macaddr, wlc_hw->etheraddr) ||
+	    is_broadcast_ether_addr(wlc_hw->etheraddr) ||
+	    is_zero_ether_addr(wlc_hw->etheraddr)) {
+		wiphy_err(wiphy, "wl%d: brcms_b_attach: bad macaddr %s\n",
+			  unit, macaddr);
+		err = 22;
+		goto fail;
+	}
 
-	if (perr)
-		*perr = 0;
+	BCMMSG(wlc->wiphy,
+		 "deviceid 0x%x nbands %d board 0x%x macaddr: %s\n",
+		 wlc_hw->deviceid, wlc_hw->_nbands,
+		 wlc_hw->sih->boardtype, macaddr);
 
-	return (void *)wlc;
+	return err;
 
  fail:
-	wiphy_err(wl->wiphy, "wl%d: %s: failed with err %d\n",
-		  unit, __func__, err);
-	if (wlc)
-		brcms_c_detach(wlc);
-
-	if (perr)
-		*perr = err;
-	return NULL;
+	wiphy_err(wiphy, "wl%d: brcms_b_attach: failed with err %d\n", unit,
+		  err);
+	return err;
 }
 
 static void brcms_c_attach_antgain_init(struct brcms_c_info *wlc)
@@ -1620,14 +4972,17 @@ static void brcms_c_attach_antgain_init(struct brcms_c_info *wlc)
 	} else {
 		s8 gain, fract;
 		/* Older sroms specified gain in whole dbm only.  In order
-		 * be able to specify qdbm granularity and remain backward compatible
-		 * the whole dbms are now encoded in only low 6 bits and remaining qdbms
-		 * are encoded in the hi 2 bits. 6 bit signed number ranges from
-		 * -32 - 31. Examples: 0x1 = 1 db,
+		 * be able to specify qdbm granularity and remain backward
+		 * compatible the whole dbms are now encoded in only
+		 * low 6 bits and remaining qdbms are encoded in the hi 2 bits.
+		 * 6 bit signed number ranges from -32 - 31.
+		 *
+		 * Examples:
+		 * 0x1 = 1 db,
 		 * 0xc1 = 1.75 db (1 + 3 quarters),
 		 * 0x3f = -1 (-1 + 0 quarters),
-		 * 0x7f = -.75 (-1 in low 6 bits + 1 quarters in hi 2 bits) = -3 qdbm.
-		 * 0xbf = -.50 (-1 in low 6 bits + 2 quarters in hi 2 bits) = -2 qdbm.
+		 * 0x7f = -.75 (-1 + 1 quarters) = -3 qdbm.
+		 * 0xbf = -.50 (-1 + 2 quarters) = -2 qdbm.
 		 */
 		gain = wlc->band->antgain & 0x3f;
 		gain <<= 2;	/* Sign extend */
@@ -1641,18 +4996,18 @@ static bool brcms_c_attach_stf_ant_init(struct brcms_c_info *wlc)
 {
 	int aa;
 	uint unit;
-	char *vars;
 	int bandtype;
+	struct si_pub *sih = wlc->hw->sih;
 
 	unit = wlc->pub->unit;
-	vars = wlc->pub->vars;
 	bandtype = wlc->band->bandtype;
 
 	/* get antennas available */
-	aa = (s8) getintvar(vars, (BAND_5G(bandtype) ? "aa5g" : "aa2g"));
-	if (aa == 0)
-		aa = (s8) getintvar(vars,
-				      (BAND_5G(bandtype) ? "aa1" : "aa0"));
+	if (bandtype == BRCM_BAND_5G)
+		aa = (s8) getintvar(sih, BRCMS_SROM_AA5G);
+	else
+		aa = (s8) getintvar(sih, BRCMS_SROM_AA2G);
+
 	if ((aa < 1) || (aa > 15)) {
 		wiphy_err(wlc->wiphy, "wl%d: %s: Invalid antennas available in"
 			  " srom (0x%x), using 3\n", unit, __func__, aa);
@@ -1670,23 +5025,129 @@ static bool brcms_c_attach_stf_ant_init(struct brcms_c_info *wlc)
 	}
 
 	/* Compute Antenna Gain */
-	wlc->band->antgain =
-	    (s8) getintvar(vars, (BAND_5G(bandtype) ? "ag1" : "ag0"));
+	if (bandtype == BRCM_BAND_5G)
+		wlc->band->antgain = (s8) getintvar(sih, BRCMS_SROM_AG1);
+	else
+		wlc->band->antgain = (s8) getintvar(sih, BRCMS_SROM_AG0);
+
 	brcms_c_attach_antgain_init(wlc);
 
 	return true;
 }
 
+static void brcms_c_bss_default_init(struct brcms_c_info *wlc)
+{
+	u16 chanspec;
+	struct brcms_band *band;
+	struct brcms_bss_info *bi = wlc->default_bss;
+
+	/* init default and target BSS with some sane initial values */
+	memset((char *)(bi), 0, sizeof(struct brcms_bss_info));
+	bi->beacon_period = BEACON_INTERVAL_DEFAULT;
+
+	/* fill the default channel as the first valid channel
+	 * starting from the 2G channels
+	 */
+	chanspec = ch20mhz_chspec(1);
+	wlc->home_chanspec = bi->chanspec = chanspec;
+
+	/* find the band of our default channel */
+	band = wlc->band;
+	if (wlc->pub->_nbands > 1 &&
+	    band->bandunit != chspec_bandunit(chanspec))
+		band = wlc->bandstate[OTHERBANDUNIT(wlc)];
+
+	/* init bss rates to the band specific default rate set */
+	brcms_c_rateset_default(&bi->rateset, NULL, band->phytype,
+		band->bandtype, false, BRCMS_RATE_MASK_FULL,
+		(bool) (wlc->pub->_n_enab & SUPPORT_11N),
+		brcms_chspec_bw(chanspec), wlc->stf->txstreams);
+
+	if (wlc->pub->_n_enab & SUPPORT_11N)
+		bi->flags |= BRCMS_BSS_HT;
+}
+
+static struct brcms_txq_info *brcms_c_txq_alloc(struct brcms_c_info *wlc)
+{
+	struct brcms_txq_info *qi, *p;
+
+	qi = kzalloc(sizeof(struct brcms_txq_info), GFP_ATOMIC);
+	if (qi != NULL) {
+		/*
+		 * Have enough room for control packets along with HI watermark
+		 * Also, add room to txq for total psq packets if all the SCBs
+		 * leave PS mode. The watermark for flowcontrol to OS packets
+		 * will remain the same
+		 */
+		brcmu_pktq_init(&qi->q, BRCMS_PREC_COUNT,
+			  2 * BRCMS_DATAHIWAT + PKTQ_LEN_DEFAULT);
+
+		/* add this queue to the the global list */
+		p = wlc->tx_queues;
+		if (p == NULL) {
+			wlc->tx_queues = qi;
+		} else {
+			while (p->next != NULL)
+				p = p->next;
+			p->next = qi;
+		}
+	}
+	return qi;
+}
+
+static void brcms_c_txq_free(struct brcms_c_info *wlc,
+			     struct brcms_txq_info *qi)
+{
+	struct brcms_txq_info *p;
+
+	if (qi == NULL)
+		return;
+
+	/* remove the queue from the linked list */
+	p = wlc->tx_queues;
+	if (p == qi)
+		wlc->tx_queues = p->next;
+	else {
+		while (p != NULL && p->next != qi)
+			p = p->next;
+		if (p != NULL)
+			p->next = p->next->next;
+	}
+
+	kfree(qi);
+}
+
+static void brcms_c_update_mimo_band_bwcap(struct brcms_c_info *wlc, u8 bwcap)
+{
+	uint i;
+	struct brcms_band *band;
+
+	for (i = 0; i < wlc->pub->_nbands; i++) {
+		band = wlc->bandstate[i];
+		if (band->bandtype == BRCM_BAND_5G) {
+			if ((bwcap == BRCMS_N_BW_40ALL)
+			    || (bwcap == BRCMS_N_BW_20IN2G_40IN5G))
+				band->mimo_cap_40 = true;
+			else
+				band->mimo_cap_40 = false;
+		} else {
+			if (bwcap == BRCMS_N_BW_40ALL)
+				band->mimo_cap_40 = true;
+			else
+				band->mimo_cap_40 = false;
+		}
+	}
+}
 
 static void brcms_c_timers_deinit(struct brcms_c_info *wlc)
 {
 	/* free timer state */
 	if (wlc->wdtimer) {
-		brcms_free_timer(wlc->wl, wlc->wdtimer);
+		brcms_free_timer(wlc->wdtimer);
 		wlc->wdtimer = NULL;
 	}
 	if (wlc->radio_timer) {
-		brcms_free_timer(wlc->wl, wlc->radio_timer);
+		brcms_free_timer(wlc->radio_timer);
 		wlc->radio_timer = NULL;
 	}
 }
@@ -1707,6 +5168,54 @@ static void brcms_c_detach_module(struct brcms_c_info *wlc)
 }
 
 /*
+ * low level detach
+ */
+static int brcms_b_detach(struct brcms_c_info *wlc)
+{
+	uint i;
+	struct brcms_hw_band *band;
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	int callbacks;
+
+	callbacks = 0;
+
+	if (wlc_hw->sih) {
+		/*
+		 * detach interrupt sync mechanism since interrupt is disabled
+		 * and per-port interrupt object may has been freed. this must
+		 * be done before sb core switch
+		 */
+		ai_deregister_intr_callback(wlc_hw->sih);
+		ai_pci_sleep(wlc_hw->sih);
+	}
+
+	brcms_b_detach_dmapio(wlc_hw);
+
+	band = wlc_hw->band;
+	for (i = 0; i < wlc_hw->_nbands; i++) {
+		if (band->pi) {
+			/* Detach this band's phy */
+			wlc_phy_detach(band->pi);
+			band->pi = NULL;
+		}
+		band = wlc_hw->bandstate[OTHERBANDUNIT(wlc)];
+	}
+
+	/* Free shared phy state */
+	kfree(wlc_hw->phy_sh);
+
+	wlc_phy_shim_detach(wlc_hw->physhim);
+
+	if (wlc_hw->sih) {
+		ai_detach(wlc_hw->sih);
+		wlc_hw->sih = NULL;
+	}
+
+	return callbacks;
+
+}
+
+/*
  * Return a count of the number of driver callbacks still pending.
  *
  * General policy is that brcms_c_detach can only dealloc/free software states.
@@ -1746,268 +5255,130 @@ uint brcms_c_detach(struct brcms_c_info *wlc)
 }
 
 /* update state that depends on the current value of "ap" */
-void brcms_c_ap_upd(struct brcms_c_info *wlc)
+static void brcms_c_ap_upd(struct brcms_c_info *wlc)
 {
-	if (AP_ENAB(wlc->pub))
-		/* AP: short not allowed, but not enforced */
-		wlc->PLCPHdr_override = BRCMS_PLCP_AUTO;
-	else
-		/* STA-BSS; short capable */
-		wlc->PLCPHdr_override = BRCMS_PLCP_SHORT;
+	/* STA-BSS; short capable */
+	wlc->PLCPHdr_override = BRCMS_PLCP_SHORT;
 
 	/* fixup mpc */
 	wlc->mpc = true;
 }
 
-/* read hwdisable state and propagate to wlc flag */
-static void brcms_c_radio_hwdisable_upd(struct brcms_c_info *wlc)
+/* Initialize just the hardware when coming out of POR or S3/S5 system states */
+static void brcms_b_hw_up(struct brcms_hardware *wlc_hw)
 {
-	if (wlc->pub->wlfeatureflag & WL_SWFL_NOHWRADIO || wlc->pub->hw_off)
+	if (wlc_hw->wlc->pub->hw_up)
 		return;
 
-	if (brcms_b_radio_read_hwdisabled(wlc->hw)) {
-		mboolset(wlc->pub->radio_disabled, WL_RADIO_HW_DISABLE);
-	} else {
-		mboolclr(wlc->pub->radio_disabled, WL_RADIO_HW_DISABLE);
-	}
-}
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
 
-/* return true if Minimum Power Consumption should be entered, false otherwise */
-bool brcms_c_is_non_delay_mpc(struct brcms_c_info *wlc)
-{
-	return false;
-}
-
-bool brcms_c_ismpc(struct brcms_c_info *wlc)
-{
-	return (wlc->mpc_delay_off == 0) && (brcms_c_is_non_delay_mpc(wlc));
-}
+	/*
+	 * Enable pll and xtal, initialize the power control registers,
+	 * and force fastclock for the remainder of brcms_c_up().
+	 */
+	brcms_b_xtal(wlc_hw, ON);
+	ai_clkctl_init(wlc_hw->sih);
+	brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
 
-void brcms_c_radio_mpc_upd(struct brcms_c_info *wlc)
-{
-	bool mpc_radio, radio_state;
+	ai_pci_fixcfg(wlc_hw->sih);
 
 	/*
-	 * Clear the WL_RADIO_MPC_DISABLE bit when mpc feature is disabled
-	 * in case the WL_RADIO_MPC_DISABLE bit was set. Stop the radio
-	 * monitor also when WL_RADIO_MPC_DISABLE is the only reason that
-	 * the radio is going down.
+	 * AI chip doesn't restore bar0win2 on
+	 * hibernation/resume, need sw fixup
 	 */
-	if (!wlc->mpc) {
-		if (!wlc->pub->radio_disabled)
-			return;
-		mboolclr(wlc->pub->radio_disabled, WL_RADIO_MPC_DISABLE);
-		brcms_c_radio_upd(wlc);
-		if (!wlc->pub->radio_disabled)
-			brcms_c_radio_monitor_stop(wlc);
-		return;
-	}
+	if ((wlc_hw->sih->chip == BCM43224_CHIP_ID) ||
+	    (wlc_hw->sih->chip == BCM43225_CHIP_ID))
+		wlc_hw->regs = (struct d11regs __iomem *)
+				ai_setcore(wlc_hw->sih, D11_CORE_ID, 0);
 
 	/*
-	 * sync ismpc logic with WL_RADIO_MPC_DISABLE bit in wlc->pub->radio_disabled
-	 * to go ON, always call radio_upd synchronously
-	 * to go OFF, postpone radio_upd to later when context is safe(e.g. watchdog)
+	 * Inform phy that a POR reset has occurred so
+	 * it does a complete phy init
 	 */
-	radio_state =
-	    (mboolisset(wlc->pub->radio_disabled, WL_RADIO_MPC_DISABLE) ? OFF :
-	     ON);
-	mpc_radio = (brcms_c_ismpc(wlc) == true) ? OFF : ON;
+	wlc_phy_por_inform(wlc_hw->band->pi);
 
-	if (radio_state == ON && mpc_radio == OFF)
-		wlc->mpc_delay_off = wlc->mpc_dlycnt;
-	else if (radio_state == OFF && mpc_radio == ON) {
-		mboolclr(wlc->pub->radio_disabled, WL_RADIO_MPC_DISABLE);
-		brcms_c_radio_upd(wlc);
-		if (wlc->mpc_offcnt < BRCMS_MPC_THRESHOLD)
-			wlc->mpc_dlycnt = BRCMS_MPC_MAX_DELAYCNT;
-		else
-			wlc->mpc_dlycnt = BRCMS_MPC_MIN_DELAYCNT;
-		wlc->mpc_dur += OSL_SYSUPTIME() - wlc->mpc_laston_ts;
-	}
-	/* Below logic is meant to capture the transition from mpc off to mpc on for reasons
-	 * other than wlc->mpc_delay_off keeping the mpc off. In that case reset
-	 * wlc->mpc_delay_off to wlc->mpc_dlycnt, so that we restart the countdown of mpc_delay_off
-	 */
-	if ((wlc->prev_non_delay_mpc == false) &&
-	    (brcms_c_is_non_delay_mpc(wlc) == true) && wlc->mpc_delay_off) {
-		wlc->mpc_delay_off = wlc->mpc_dlycnt;
-	}
-	wlc->prev_non_delay_mpc = brcms_c_is_non_delay_mpc(wlc);
-}
-
-/*
- * centralized radio disable/enable function,
- * invoke radio enable/disable after updating hwradio status
- */
-static void brcms_c_radio_upd(struct brcms_c_info *wlc)
-{
-	if (wlc->pub->radio_disabled) {
-		brcms_c_radio_disable(wlc);
-	} else {
-		brcms_c_radio_enable(wlc);
-	}
-}
-
-/* maintain LED behavior in down state */
-static void brcms_c_down_led_upd(struct brcms_c_info *wlc)
-{
-	/* maintain LEDs while in down state, turn on sbclk if not available yet */
-	/* turn on sbclk if necessary */
-	if (!AP_ENAB(wlc->pub)) {
-		brcms_c_pllreq(wlc, true, BRCMS_PLLREQ_FLIP);
+	wlc_hw->ucode_loaded = false;
+	wlc_hw->wlc->pub->hw_up = true;
 
-		brcms_c_pllreq(wlc, false, BRCMS_PLLREQ_FLIP);
+	if ((wlc_hw->boardflags & BFL_FEM)
+	    && (wlc_hw->sih->chip == BCM4313_CHIP_ID)) {
+		if (!
+		    (wlc_hw->boardrev >= 0x1250
+		     && (wlc_hw->boardflags & BFL_FEM_BT)))
+			ai_epa_4313war(wlc_hw->sih);
 	}
 }
 
-/* update hwradio status and return it */
-bool brcms_c_check_radio_disabled(struct brcms_c_info *wlc)
-{
-	brcms_c_radio_hwdisable_upd(wlc);
-
-	return mboolisset(wlc->pub->radio_disabled, WL_RADIO_HW_DISABLE) ? true : false;
-}
-
-void brcms_c_radio_disable(struct brcms_c_info *wlc)
+static int brcms_b_up_prep(struct brcms_hardware *wlc_hw)
 {
-	if (!wlc->pub->up) {
-		brcms_c_down_led_upd(wlc);
-		return;
-	}
-
-	brcms_c_radio_monitor_start(wlc);
-	brcms_down(wlc->wl);
-}
+	uint coremask;
 
-static void brcms_c_radio_enable(struct brcms_c_info *wlc)
-{
-	if (wlc->pub->up)
-		return;
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
 
-	if (DEVICEREMOVED(wlc))
-		return;
+	/*
+	 * Enable pll and xtal, initialize the power control registers,
+	 * and force fastclock for the remainder of brcms_c_up().
+	 */
+	brcms_b_xtal(wlc_hw, ON);
+	ai_clkctl_init(wlc_hw->sih);
+	brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
 
-	brcms_up(wlc->wl);
-}
+	/*
+	 * Configure pci/pcmcia here instead of in brcms_c_attach()
+	 * to allow mfg hotswap:  down, hotswap (chip power cycle), up.
+	 */
+	coremask = (1 << wlc_hw->wlc->core->coreidx);
 
-/* periodical query hw radio button while driver is "down" */
-static void brcms_c_radio_timer(void *arg)
-{
-	struct brcms_c_info *wlc = (struct brcms_c_info *) arg;
+	ai_pci_setup(wlc_hw->sih, coremask);
 
-	if (DEVICEREMOVED(wlc)) {
-		wiphy_err(wlc->wiphy, "wl%d: %s: dead chip\n", wlc->pub->unit,
-			__func__);
-		brcms_down(wlc->wl);
-		return;
+	/*
+	 * Need to read the hwradio status here to cover the case where the
+	 * system is loaded with the hw radio disabled. We do not want to
+	 * bring the driver up in this case.
+	 */
+	if (brcms_b_radio_read_hwdisabled(wlc_hw)) {
+		/* put SB PCI in down state again */
+		ai_pci_down(wlc_hw->sih);
+		brcms_b_xtal(wlc_hw, OFF);
+		return -ENOMEDIUM;
 	}
 
-	/* cap mpc off count */
-	if (wlc->mpc_offcnt < BRCMS_MPC_MAX_DELAYCNT)
-		wlc->mpc_offcnt++;
-
-	brcms_c_radio_hwdisable_upd(wlc);
-	brcms_c_radio_upd(wlc);
-}
+	ai_pci_up(wlc_hw->sih);
 
-static bool brcms_c_radio_monitor_start(struct brcms_c_info *wlc)
-{
-	/* Don't start the timer if HWRADIO feature is disabled */
-	if (wlc->radio_monitor || (wlc->pub->wlfeatureflag & WL_SWFL_NOHWRADIO))
-		return true;
+	/* reset the d11 core */
+	brcms_b_corereset(wlc_hw, BRCMS_USE_COREFLAGS);
 
-	wlc->radio_monitor = true;
-	brcms_c_pllreq(wlc, true, BRCMS_PLLREQ_RADIO_MON);
-	brcms_add_timer(wlc->wl, wlc->radio_timer, TIMER_INTERVAL_RADIOCHK,
-			true);
-	return true;
+	return 0;
 }
 
-bool brcms_c_radio_monitor_stop(struct brcms_c_info *wlc)
+static int brcms_b_up_finish(struct brcms_hardware *wlc_hw)
 {
-	if (!wlc->radio_monitor)
-		return true;
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
 
-	wlc->radio_monitor = false;
-	brcms_c_pllreq(wlc, false, BRCMS_PLLREQ_RADIO_MON);
-	return brcms_del_timer(wlc->wl, wlc->radio_timer);
-}
+	wlc_hw->up = true;
+	wlc_phy_hw_state_upd(wlc_hw->band->pi, true);
 
-static void brcms_c_watchdog_by_timer(void *arg)
-{
-	brcms_c_watchdog(arg);
+	/* FULLY enable dynamic power control and d11 core interrupt */
+	brcms_b_clkctl_clk(wlc_hw, CLK_DYNAMIC);
+	brcms_intrson(wlc_hw->wlc->wl);
+	return 0;
 }
 
-/* common watchdog code */
-static void brcms_c_watchdog(void *arg)
+/*
+ * Write WME tunable parameters for retransmit/max rate
+ * from wlc struct to ucode
+ */
+static void brcms_c_wme_retries_write(struct brcms_c_info *wlc)
 {
-	struct brcms_c_info *wlc = (struct brcms_c_info *) arg;
-	int i;
-	struct brcms_bss_cfg *cfg;
-
-	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
-
-	if (!wlc->pub->up)
-		return;
-
-	if (DEVICEREMOVED(wlc)) {
-		wiphy_err(wlc->wiphy, "wl%d: %s: dead chip\n", wlc->pub->unit,
-			  __func__);
-		brcms_down(wlc->wl);
-		return;
-	}
-
-	/* increment second count */
-	wlc->pub->now++;
-
-	/* delay radio disable */
-	if (wlc->mpc_delay_off) {
-		if (--wlc->mpc_delay_off == 0) {
-			mboolset(wlc->pub->radio_disabled,
-				 WL_RADIO_MPC_DISABLE);
-			if (wlc->mpc && brcms_c_ismpc(wlc))
-				wlc->mpc_offcnt = 0;
-			wlc->mpc_laston_ts = OSL_SYSUPTIME();
-		}
-	}
+	int ac;
 
-	/* mpc sync */
-	brcms_c_radio_mpc_upd(wlc);
-	/* radio sync: sw/hw/mpc --> radio_disable/radio_enable */
-	brcms_c_radio_hwdisable_upd(wlc);
-	brcms_c_radio_upd(wlc);
-	/* if radio is disable, driver may be down, quit here */
-	if (wlc->pub->radio_disabled)
+	/* Need clock to do this */
+	if (!wlc->clk)
 		return;
 
-	brcms_b_watchdog(wlc);
-
-	/* occasionally sample mac stat counters to detect 16-bit counter wrap */
-	if ((wlc->pub->now % SW_TIMER_MAC_STAT_UPD) == 0)
-		brcms_c_statsupd(wlc);
-
-	/* Manage TKIP countermeasures timers */
-	FOREACH_BSS(wlc, i, cfg) {
-		if (cfg->tk_cm_dt) {
-			cfg->tk_cm_dt--;
-		}
-		if (cfg->tk_cm_bt) {
-			cfg->tk_cm_bt--;
-		}
-	}
-
-	/* Call any registered watchdog handlers */
-	for (i = 0; i < BRCMS_MAXMODULES; i++) {
-		if (wlc->modulecb[i].watchdog_fn)
-			wlc->modulecb[i].watchdog_fn(wlc->modulecb[i].hdl);
-	}
-
-	if (BRCMS_ISNPHY(wlc->band) && !wlc->pub->tempsense_disable &&
-	    ((wlc->pub->now - wlc->tempsense_lasttime) >=
-	     BRCMS_TEMPSENSE_PERIOD)) {
-		wlc->tempsense_lasttime = wlc->pub->now;
-		brcms_c_tempsense_upd(wlc);
-	}
+	for (ac = 0; ac < AC_COUNT; ac++)
+		brcms_b_write_shm(wlc->hw, M_AC_TXLMT_ADDR(ac),
+				  wlc->wme_retries[ac]);
 }
 
 /* make interface operational */
@@ -2016,7 +5387,7 @@ int brcms_c_up(struct brcms_c_info *wlc)
 	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
 
 	/* HW is turned off so don't try to access it */
-	if (wlc->pub->hw_off || DEVICEREMOVED(wlc))
+	if (wlc->pub->hw_off || brcms_deviceremoved(wlc))
 		return -ENOMEDIUM;
 
 	if (!wlc->pub->hw_up) {
@@ -2027,20 +5398,20 @@ int brcms_c_up(struct brcms_c_info *wlc)
 	if ((wlc->pub->boardflags & BFL_FEM)
 	    && (wlc->pub->sih->chip == BCM4313_CHIP_ID)) {
 		if (wlc->pub->boardrev >= 0x1250
-		    && (wlc->pub->boardflags & BFL_FEM_BT)) {
-			brcms_c_mhf(wlc, MHF5, MHF5_4313_GPIOCTRL,
+		    && (wlc->pub->boardflags & BFL_FEM_BT))
+			brcms_b_mhf(wlc->hw, MHF5, MHF5_4313_GPIOCTRL,
 				MHF5_4313_GPIOCTRL, BRCM_BAND_ALL);
-		} else {
-			brcms_c_mhf(wlc, MHF4, MHF4_EXTPA_ENABLE,
+		else
+			brcms_b_mhf(wlc->hw, MHF4, MHF4_EXTPA_ENABLE,
 				    MHF4_EXTPA_ENABLE, BRCM_BAND_ALL);
-		}
 	}
 
 	/*
-	 * Need to read the hwradio status here to cover the case where the system
-	 * is loaded with the hw radio disabled. We do not want to bring the driver up in this case.
-	 * if radio is disabled, abort up, lower power, start radio timer and return 0(for NDIS)
-	 * don't call radio_update to avoid looping brcms_c_up.
+	 * Need to read the hwradio status here to cover the case where the
+	 * system is loaded with the hw radio disabled. We do not want to bring
+	 * the driver up in this case. If radio is disabled, abort up, lower
+	 * power, start radio timer and return 0(for NDIS) don't call
+	 * radio_update to avoid looping brcms_c_up.
 	 *
 	 * brcms_b_up_prep() returns either 0 or -BCME_RADIOOFF only
 	 */
@@ -2049,20 +5420,15 @@ int brcms_c_up(struct brcms_c_info *wlc)
 		if (status == -ENOMEDIUM) {
 			if (!mboolisset
 			    (wlc->pub->radio_disabled, WL_RADIO_HW_DISABLE)) {
-				int idx;
-				struct brcms_bss_cfg *bsscfg;
+				struct brcms_bss_cfg *bsscfg = wlc->bsscfg;
 				mboolset(wlc->pub->radio_disabled,
 					 WL_RADIO_HW_DISABLE);
 
-				FOREACH_BSS(wlc, idx, bsscfg) {
-					if (!BSSCFG_STA(bsscfg)
-					    || !bsscfg->enable || !bsscfg->BSS)
-						continue;
-					wiphy_err(wlc->wiphy, "wl%d.%d: up"
+				if (bsscfg->enable && bsscfg->BSS)
+					wiphy_err(wlc->wiphy, "wl%d: up"
 						  ": rfdisable -> "
 						  "bsscfg_disable()\n",
-						   wlc->pub->unit, idx);
-				}
+						   wlc->pub->unit);
 			}
 		}
 	}
@@ -2078,15 +5444,7 @@ int brcms_c_up(struct brcms_c_info *wlc)
 	brcms_c_radio_monitor_stop(wlc);
 
 	/* Set EDCF hostflags */
-	if (EDCF_ENAB(wlc->pub)) {
-		brcms_c_mhf(wlc, MHF1, MHF1_EDCF, MHF1_EDCF, BRCM_BAND_ALL);
-	} else {
-		brcms_c_mhf(wlc, MHF1, MHF1_EDCF, 0, BRCM_BAND_ALL);
-	}
-
-	if (BRCMS_WAR16165(wlc))
-		brcms_c_mhf(wlc, MHF2, MHF2_PCISLOWCLKWAR, MHF2_PCISLOWCLKWAR,
-			BRCM_BAND_ALL);
+	brcms_b_mhf(wlc->hw, MHF1, MHF1_EDCF, MHF1_EDCF, BRCM_BAND_ALL);
 
 	brcms_init(wlc->wl);
 	wlc->pub->up = true;
@@ -2100,15 +5458,11 @@ int brcms_c_up(struct brcms_c_info *wlc)
 
 	brcms_b_up_finish(wlc->hw);
 
-	/* other software states up after ISR is running */
-	/* start APs that were to be brought up but are not up  yet */
-	/* if (AP_ENAB(wlc->pub)) brcms_c_restart_ap(wlc->ap); */
-
 	/* Program the TX wme params with the current settings */
 	brcms_c_wme_retries_write(wlc);
 
 	/* start one second watchdog timer */
-	brcms_add_timer(wlc->wl, wlc->wdtimer, TIMER_INTERVAL_WATCHDOG, true);
+	brcms_add_timer(wlc->wdtimer, TIMER_INTERVAL_WATCHDOG, true);
 	wlc->WDarmed = true;
 
 	/* ensure antenna config is up to date */
@@ -2119,29 +5473,80 @@ int brcms_c_up(struct brcms_c_info *wlc)
 	return 0;
 }
 
-/* Initialize the base precedence map for dequeueing from txq based on WME settings */
-static void brcms_c_tx_prec_map_init(struct brcms_c_info *wlc)
+static uint brcms_c_down_del_timer(struct brcms_c_info *wlc)
 {
-	wlc->tx_prec_map = BRCMS_PREC_BMP_ALL;
-	memset(wlc->fifo2prec_map, 0, NFIFO * sizeof(u16));
+	uint callbacks = 0;
 
-	/* For non-WME, both fifos have overlapping MAXPRIO. So just disable all precedences
-	 * if either is full.
-	 */
-	if (!EDCF_ENAB(wlc->pub)) {
-		wlc->fifo2prec_map[TX_DATA_FIFO] = BRCMS_PREC_BMP_ALL;
-		wlc->fifo2prec_map[TX_CTL_FIFO] = BRCMS_PREC_BMP_ALL;
-	} else {
-		wlc->fifo2prec_map[TX_AC_BK_FIFO] = BRCMS_PREC_BMP_AC_BK;
-		wlc->fifo2prec_map[TX_AC_BE_FIFO] = BRCMS_PREC_BMP_AC_BE;
-		wlc->fifo2prec_map[TX_AC_VI_FIFO] = BRCMS_PREC_BMP_AC_VI;
-		wlc->fifo2prec_map[TX_AC_VO_FIFO] = BRCMS_PREC_BMP_AC_VO;
+	return callbacks;
+}
+
+static int brcms_b_bmac_down_prep(struct brcms_hardware *wlc_hw)
+{
+	bool dev_gone;
+	uint callbacks = 0;
+
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
+
+	if (!wlc_hw->up)
+		return callbacks;
+
+	dev_gone = brcms_deviceremoved(wlc_hw->wlc);
+
+	/* disable interrupts */
+	if (dev_gone)
+		wlc_hw->wlc->macintmask = 0;
+	else {
+		/* now disable interrupts */
+		brcms_intrsoff(wlc_hw->wlc->wl);
+
+		/* ensure we're running on the pll clock again */
+		brcms_b_clkctl_clk(wlc_hw, CLK_FAST);
 	}
+	/* down phy at the last of this stage */
+	callbacks += wlc_phy_down(wlc_hw->band->pi);
+
+	return callbacks;
 }
 
-static uint brcms_c_down_del_timer(struct brcms_c_info *wlc)
+static int brcms_b_down_finish(struct brcms_hardware *wlc_hw)
 {
 	uint callbacks = 0;
+	bool dev_gone;
+
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
+
+	if (!wlc_hw->up)
+		return callbacks;
+
+	wlc_hw->up = false;
+	wlc_phy_hw_state_upd(wlc_hw->band->pi, false);
+
+	dev_gone = brcms_deviceremoved(wlc_hw->wlc);
+
+	if (dev_gone) {
+		wlc_hw->sbclk = false;
+		wlc_hw->clk = false;
+		wlc_phy_hw_clk_state_upd(wlc_hw->band->pi, false);
+
+		/* reclaim any posted packets */
+		brcms_c_flushqueues(wlc_hw->wlc);
+	} else {
+
+		/* Reset and disable the core */
+		if (ai_iscoreup(wlc_hw->sih)) {
+			if (R_REG(&wlc_hw->regs->maccontrol) &
+			    MCTL_EN_MAC)
+				brcms_c_suspend_mac_and_wait(wlc_hw->wlc);
+			callbacks += brcms_reset(wlc_hw->wlc->wl);
+			brcms_c_coredisable(wlc_hw);
+		}
+
+		/* turn off primary xtal and pll */
+		if (!wlc_hw->noreset) {
+			ai_pci_down(wlc_hw->sih);
+			brcms_b_xtal(wlc_hw, OFF);
+		}
+	}
 
 	return callbacks;
 }
@@ -2175,7 +5580,7 @@ uint brcms_c_down(struct brcms_c_info *wlc)
 
 	callbacks += brcms_b_bmac_down_prep(wlc->hw);
 
-	dev_gone = DEVICEREMOVED(wlc);
+	dev_gone = brcms_deviceremoved(wlc);
 
 	/* Call any registered down handlers */
 	for (i = 0; i < BRCMS_MAXMODULES; i++) {
@@ -2186,7 +5591,7 @@ uint brcms_c_down(struct brcms_c_info *wlc)
 
 	/* cancel the watchdog timer */
 	if (wlc->WDarmed) {
-		if (!brcms_del_timer(wlc->wl, wlc->wdtimer))
+		if (!brcms_del_timer(wlc->wdtimer))
 			callbacks++;
 		wlc->WDarmed = false;
 	}
@@ -2201,9 +5606,8 @@ uint brcms_c_down(struct brcms_c_info *wlc)
 	brcms_c_txflowcontrol_reset(wlc);
 
 	/* flush tx queues */
-	for (qi = wlc->tx_queues; qi != NULL; qi = qi->next) {
+	for (qi = wlc->tx_queues; qi != NULL; qi = qi->next)
 		brcmu_pktq_flush(&qi->q, true, NULL, NULL);
-	}
 
 	callbacks += brcms_b_down_finish(wlc->hw);
 
@@ -2219,30 +5623,31 @@ int brcms_c_set_gmode(struct brcms_c_info *wlc, u8 gmode, bool config)
 {
 	int ret = 0;
 	uint i;
-	wlc_rateset_t rs;
+	struct brcms_c_rateset rs;
 	/* Default to 54g Auto */
 	/* Advertise and use shortslot (-1/0/1 Auto/Off/On) */
 	s8 shortslot = BRCMS_SHORTSLOT_AUTO;
-	bool shortslot_restrict = false;	/* Restrict association to stations that support shortslot
-						 */
+	bool shortslot_restrict = false; /* Restrict association to stations
+					  * that support shortslot
+					  */
 	bool ofdm_basic = false;	/* Make 6, 12, and 24 basic rates */
 	/* Advertise and use short preambles (-1/0/1 Auto/Off/On) */
 	int preamble = BRCMS_PLCP_LONG;
-	bool preamble_restrict = false;	/* Restrict association to stations that support short
-					 * preambles
+	bool preamble_restrict = false;	/* Restrict association to stations
+					 * that support short preambles
 					 */
 	struct brcms_band *band;
 
 	/* if N-support is enabled, allow Gmode set as long as requested
 	 * Gmode is not GMODE_LEGACY_B
 	 */
-	if (N_ENAB(wlc->pub) && gmode == GMODE_LEGACY_B)
+	if ((wlc->pub->_n_enab & SUPPORT_11N) && gmode == GMODE_LEGACY_B)
 		return -ENOTSUPP;
 
 	/* verify that we are dealing with 2G band and grab the band pointer */
 	if (wlc->band->bandtype == BRCM_BAND_2G)
 		band = wlc->band;
-	else if ((NBANDS(wlc) > 1) &&
+	else if ((wlc->pub->_nbands > 1) &&
 		 (wlc->bandstate[OTHERBANDUNIT(wlc)]->bandtype == BRCM_BAND_2G))
 		band = wlc->bandstate[OTHERBANDUNIT(wlc)];
 	else
@@ -2257,11 +5662,8 @@ int brcms_c_set_gmode(struct brcms_c_info *wlc, u8 gmode, bool config)
 	if (config == true)
 		brcms_c_protection_upd(wlc, BRCMS_PROT_G_USER, gmode);
 
-	/* Clear supported rates filter */
-	memset(&wlc->sup_rates_override, 0, sizeof(wlc_rateset_t));
-
 	/* Clear rateset override */
-	memset(&rs, 0, sizeof(wlc_rateset_t));
+	memset(&rs, 0, sizeof(struct brcms_c_rateset));
 
 	switch (gmode) {
 	case GMODE_LEGACY_B:
@@ -2271,9 +5673,6 @@ int brcms_c_set_gmode(struct brcms_c_info *wlc, u8 gmode, bool config)
 		break;
 
 	case GMODE_LRS:
-		if (AP_ENAB(wlc->pub))
-			brcms_c_rateset_copy(&cck_rates,
-					     &wlc->sup_rates_override);
 		break;
 
 	case GMODE_AUTO:
@@ -2287,10 +5686,6 @@ int brcms_c_set_gmode(struct brcms_c_info *wlc, u8 gmode, bool config)
 		break;
 
 	case GMODE_PERFORMANCE:
-		if (AP_ENAB(wlc->pub))	/* Put all rates into the Supported Rates element */
-			brcms_c_rateset_copy(&cck_ofdm_rates,
-					 &wlc->sup_rates_override);
-
 		shortslot = BRCMS_SHORTSLOT_ON;
 		shortslot_restrict = true;
 		ofdm_basic = true;
@@ -2305,46 +5700,10 @@ int brcms_c_set_gmode(struct brcms_c_info *wlc, u8 gmode, bool config)
 		return -ENOTSUPP;
 	}
 
-	/*
-	 * If we are switching to gmode == GMODE_LEGACY_B,
-	 * clean up rate info that may refer to OFDM rates.
-	 */
-	if ((gmode == GMODE_LEGACY_B) && (band->gmode != GMODE_LEGACY_B)) {
-		band->gmode = gmode;
-		if (band->rspec_override && !IS_CCK(band->rspec_override)) {
-			band->rspec_override = 0;
-			brcms_c_reprate_init(wlc);
-		}
-		if (band->mrspec_override && !IS_CCK(band->mrspec_override)) {
-			band->mrspec_override = 0;
-		}
-	}
-
 	band->gmode = gmode;
 
 	wlc->shortslot_override = shortslot;
 
-	if (AP_ENAB(wlc->pub)) {
-		/* wlc->ap->shortslot_restrict = shortslot_restrict; */
-		wlc->PLCPHdr_override =
-		    (preamble !=
-		     BRCMS_PLCP_LONG) ? BRCMS_PLCP_SHORT : BRCMS_PLCP_AUTO;
-	}
-
-	if ((AP_ENAB(wlc->pub) && preamble != BRCMS_PLCP_LONG)
-	    || preamble == BRCMS_PLCP_SHORT)
-		wlc->default_bss->capability |= WLAN_CAPABILITY_SHORT_PREAMBLE;
-	else
-		wlc->default_bss->capability &= ~WLAN_CAPABILITY_SHORT_PREAMBLE;
-
-	/* Update shortslot capability bit for AP and IBSS */
-	if ((AP_ENAB(wlc->pub) && shortslot == BRCMS_SHORTSLOT_AUTO) ||
-	    shortslot == BRCMS_SHORTSLOT_ON)
-		wlc->default_bss->capability |= WLAN_CAPABILITY_SHORT_SLOT_TIME;
-	else
-		wlc->default_bss->capability &=
-					~WLAN_CAPABILITY_SHORT_SLOT_TIME;
-
 	/* Use the default 11g rateset */
 	if (!rs.count)
 		brcms_c_rateset_copy(&cck_ofdm_rates, &rs);
@@ -2366,92 +5725,41 @@ int brcms_c_set_gmode(struct brcms_c_info *wlc, u8 gmode, bool config)
 	return ret;
 }
 
-static int brcms_c_nmode_validate(struct brcms_c_info *wlc, s32 nmode)
-{
-	int err = 0;
-
-	switch (nmode) {
-
-	case OFF:
-		break;
-
-	case AUTO:
-	case WL_11N_2x2:
-	case WL_11N_3x3:
-		if (!(BRCMS_PHY_11N_CAP(wlc->band)))
-			err = -EINVAL;
-		break;
-
-	default:
-		err = -EINVAL;
-		break;
-	}
-
-	return err;
-}
-
-int brcms_c_set_nmode(struct brcms_c_info *wlc, s32 nmode)
+int brcms_c_set_nmode(struct brcms_c_info *wlc)
 {
 	uint i;
-	int err;
+	s32 nmode = AUTO;
 
-	err = brcms_c_nmode_validate(wlc, nmode);
-	if (err)
-		return err;
-
-	switch (nmode) {
-	case OFF:
-		wlc->pub->_n_enab = OFF;
-		wlc->default_bss->flags &= ~BRCMS_BSS_HT;
-		/* delete the mcs rates from the default and hw ratesets */
-		brcms_c_rateset_mcs_clear(&wlc->default_bss->rateset);
-		for (i = 0; i < NBANDS(wlc); i++) {
-			memset(wlc->bandstate[i]->hw_rateset.mcs, 0,
-			       MCSSET_LEN);
-			if (IS_MCS(wlc->band->rspec_override)) {
-				wlc->bandstate[i]->rspec_override = 0;
-				brcms_c_reprate_init(wlc);
-			}
-			if (IS_MCS(wlc->band->mrspec_override))
-				wlc->bandstate[i]->mrspec_override = 0;
-		}
-		break;
-
-	case AUTO:
-		if (wlc->stf->txstreams == WL_11N_3x3)
-			nmode = WL_11N_3x3;
-		else
-			nmode = WL_11N_2x2;
-	case WL_11N_2x2:
-	case WL_11N_3x3:
-		/* force GMODE_AUTO if NMODE is ON */
-		brcms_c_set_gmode(wlc, GMODE_AUTO, true);
-		if (nmode == WL_11N_3x3)
-			wlc->pub->_n_enab = SUPPORT_HT;
-		else
-			wlc->pub->_n_enab = SUPPORT_11N;
-		wlc->default_bss->flags |= BRCMS_BSS_HT;
-		/* add the mcs rates to the default and hw ratesets */
-		brcms_c_rateset_mcs_build(&wlc->default_bss->rateset,
-				      wlc->stf->txstreams);
-		for (i = 0; i < NBANDS(wlc); i++)
-			memcpy(wlc->bandstate[i]->hw_rateset.mcs,
-			       wlc->default_bss->rateset.mcs, MCSSET_LEN);
-		break;
+	if (wlc->stf->txstreams == WL_11N_3x3)
+		nmode = WL_11N_3x3;
+	else
+		nmode = WL_11N_2x2;
 
-	default:
-		break;
-	}
+	/* force GMODE_AUTO if NMODE is ON */
+	brcms_c_set_gmode(wlc, GMODE_AUTO, true);
+	if (nmode == WL_11N_3x3)
+		wlc->pub->_n_enab = SUPPORT_HT;
+	else
+		wlc->pub->_n_enab = SUPPORT_11N;
+	wlc->default_bss->flags |= BRCMS_BSS_HT;
+	/* add the mcs rates to the default and hw ratesets */
+	brcms_c_rateset_mcs_build(&wlc->default_bss->rateset,
+			      wlc->stf->txstreams);
+	for (i = 0; i < wlc->pub->_nbands; i++)
+		memcpy(wlc->bandstate[i]->hw_rateset.mcs,
+		       wlc->default_bss->rateset.mcs, MCSSET_LEN);
 
-	return err;
+	return 0;
 }
 
-static int brcms_c_set_rateset(struct brcms_c_info *wlc, wlc_rateset_t *rs_arg)
+static int
+brcms_c_set_internal_rateset(struct brcms_c_info *wlc,
+			     struct brcms_c_rateset *rs_arg)
 {
-	wlc_rateset_t rs, new;
+	struct brcms_c_rateset rs, new;
 	uint bandunit;
 
-	memcpy(&rs, rs_arg, sizeof(wlc_rateset_t));
+	memcpy(&rs, rs_arg, sizeof(struct brcms_c_rateset));
 
 	/* check for bad count value */
 	if ((rs.count == 0) || (rs.count > BRCMS_NUMRATES))
@@ -2459,16 +5767,16 @@ static int brcms_c_set_rateset(struct brcms_c_info *wlc, wlc_rateset_t *rs_arg)
 
 	/* try the current band */
 	bandunit = wlc->band->bandunit;
-	memcpy(&new, &rs, sizeof(wlc_rateset_t));
+	memcpy(&new, &rs, sizeof(struct brcms_c_rateset));
 	if (brcms_c_rate_hwrs_filter_sort_validate
 	    (&new, &wlc->bandstate[bandunit]->hw_rateset, true,
 	     wlc->stf->txstreams))
 		goto good;
 
 	/* try the other band */
-	if (IS_MBAND_UNLOCKED(wlc)) {
+	if (brcms_is_mband_unlocked(wlc)) {
 		bandunit = OTHERBANDUNIT(wlc);
-		memcpy(&new, &rs, sizeof(wlc_rateset_t));
+		memcpy(&new, &rs, sizeof(struct brcms_c_rateset));
 		if (brcms_c_rate_hwrs_filter_sort_validate(&new,
 						       &wlc->
 						       bandstate[bandunit]->
@@ -2481,299 +5789,176 @@ static int brcms_c_set_rateset(struct brcms_c_info *wlc, wlc_rateset_t *rs_arg)
 
  good:
 	/* apply new rateset */
-	memcpy(&wlc->default_bss->rateset, &new, sizeof(wlc_rateset_t));
+	memcpy(&wlc->default_bss->rateset, &new,
+	       sizeof(struct brcms_c_rateset));
 	memcpy(&wlc->bandstate[bandunit]->defrateset, &new,
-	       sizeof(wlc_rateset_t));
+	       sizeof(struct brcms_c_rateset));
 	return 0;
 }
 
-/* simplified integer set interface for common ioctl handler */
-int brcms_c_set(struct brcms_c_info *wlc, int cmd, int arg)
-{
-	return brcms_c_ioctl(wlc, cmd, (void *)&arg, sizeof(arg), NULL);
-}
-
-/* simplified integer get interface for common ioctl handler */
-int brcms_c_get(struct brcms_c_info *wlc, int cmd, int *arg)
-{
-	return brcms_c_ioctl(wlc, cmd, arg, sizeof(int), NULL);
-}
-
 static void brcms_c_ofdm_rateset_war(struct brcms_c_info *wlc)
 {
 	u8 r;
 	bool war = false;
 
-	if (wlc->cfg->associated)
-		r = wlc->cfg->current_bss->rateset.rates[0];
+	if (wlc->bsscfg->associated)
+		r = wlc->bsscfg->current_bss->rateset.rates[0];
 	else
 		r = wlc->default_bss->rateset.rates[0];
 
 	wlc_phy_ofdm_rateset_war(wlc->band->pi, war);
-
-	return;
 }
 
-int
-brcms_c_ioctl(struct brcms_c_info *wlc, int cmd, void *arg, int len,
-	      struct brcms_c_if *wlcif)
+int brcms_c_set_channel(struct brcms_c_info *wlc, u16 channel)
 {
-	return _brcms_c_ioctl(wlc, cmd, arg, len, wlcif);
-}
+	u16 chspec = ch20mhz_chspec(channel);
 
-/* common ioctl handler. return: 0=ok, -1=error, positive=particular error */
-static int
-_brcms_c_ioctl(struct brcms_c_info *wlc, int cmd, void *arg, int len,
-	       struct brcms_c_if *wlcif)
-{
-	int val, *pval;
-	bool bool_val;
-	int bcmerror;
-	struct scb *nextscb;
-	bool ta_ok;
-	uint band;
-	struct brcms_bss_cfg *bsscfg;
-	struct brcms_bss_info *current_bss;
-
-	/* update bsscfg pointer */
-	bsscfg = wlc->cfg;
-	current_bss = bsscfg->current_bss;
-
-	/* initialize the following to get rid of compiler warning */
-	nextscb = NULL;
-	ta_ok = false;
-	band = 0;
-
-	/* If the device is turned off, then it's not "removed" */
-	if (!wlc->pub->hw_off && DEVICEREMOVED(wlc)) {
-		wiphy_err(wlc->wiphy, "wl%d: %s: dead chip\n", wlc->pub->unit,
-			  __func__);
-		brcms_down(wlc->wl);
-		return -EBADE;
-	}
-
-	/* default argument is generic integer */
-	pval = arg ? (int *)arg : NULL;
+	if (channel < 0 || channel > MAXCHANNEL)
+		return -EINVAL;
 
-	/* This will prevent the misaligned access */
-	if (pval && (u32) len >= sizeof(val))
-		memcpy(&val, pval, sizeof(val));
-	else
-		val = 0;
+	if (!brcms_c_valid_chanspec_db(wlc->cmi, chspec))
+		return -EINVAL;
 
-	/* bool conversion to avoid duplication below */
-	bool_val = val != 0;
-	bcmerror = 0;
 
-	if ((arg == NULL) || (len <= 0)) {
-		wiphy_err(wlc->wiphy, "wl%d: %s: Command %d needs arguments\n",
-			  wlc->pub->unit, __func__, cmd);
-		bcmerror = -EINVAL;
-		goto done;
+	if (!wlc->pub->up && brcms_is_mband_unlocked(wlc)) {
+		if (wlc->band->bandunit != chspec_bandunit(chspec))
+			wlc->bandinit_pending = true;
+		else
+			wlc->bandinit_pending = false;
 	}
 
-	switch (cmd) {
-
-	case BRCM_SET_CHANNEL:{
-			chanspec_t chspec = CH20MHZ_CHSPEC(val);
-
-			if (val < 0 || val > MAXCHANNEL) {
-				bcmerror = -EINVAL;
-				break;
-			}
-
-			if (!brcms_c_valid_chanspec_db(wlc->cmi, chspec)) {
-				bcmerror = -EINVAL;
-				break;
-			}
-
-			if (!wlc->pub->up && IS_MBAND_UNLOCKED(wlc)) {
-				if (wlc->band->bandunit !=
-				    CHSPEC_BANDUNIT(chspec))
-					wlc->bandinit_pending = true;
-				else
-					wlc->bandinit_pending = false;
-			}
-
-			wlc->default_bss->chanspec = chspec;
-			/* brcms_c_BSSinit() will sanitize the rateset before
-			 * using it.. */
-			if (wlc->pub->up &&
-			    (BRCMS_BAND_PI_RADIO_CHANSPEC != chspec)) {
-				brcms_c_set_home_chanspec(wlc, chspec);
-				brcms_c_suspend_mac_and_wait(wlc);
-				brcms_c_set_chanspec(wlc, chspec);
-				brcms_c_enable_mac(wlc);
-			}
-			break;
-		}
-
-	case BRCM_SET_SRL:
-		if (val >= 1 && val <= RETRY_SHORT_MAX) {
-			int ac;
-			wlc->SRL = (u16) val;
-
-			brcms_b_retrylimit_upd(wlc->hw, wlc->SRL, wlc->LRL);
-
-			for (ac = 0; ac < AC_COUNT; ac++) {
-				BRCMS_WME_RETRY_SHORT_SET(wlc, ac, wlc->SRL);
-			}
-			brcms_c_wme_retries_write(wlc);
-		} else
-			bcmerror = -EINVAL;
-		break;
-
-	case BRCM_SET_LRL:
-		if (val >= 1 && val <= 255) {
-			int ac;
-			wlc->LRL = (u16) val;
+	wlc->default_bss->chanspec = chspec;
+	/* brcms_c_BSSinit() will sanitize the rateset before
+	 * using it.. */
+	if (wlc->pub->up && (wlc_phy_chanspec_get(wlc->band->pi) != chspec)) {
+		brcms_c_set_home_chanspec(wlc, chspec);
+		brcms_c_suspend_mac_and_wait(wlc);
+		brcms_c_set_chanspec(wlc, chspec);
+		brcms_c_enable_mac(wlc);
+	}
+	return 0;
+}
 
-			brcms_b_retrylimit_upd(wlc->hw, wlc->SRL, wlc->LRL);
+int brcms_c_set_rate_limit(struct brcms_c_info *wlc, u16 srl, u16 lrl)
+{
+	int ac;
 
-			for (ac = 0; ac < AC_COUNT; ac++) {
-				BRCMS_WME_RETRY_LONG_SET(wlc, ac, wlc->LRL);
-			}
-			brcms_c_wme_retries_write(wlc);
-		} else
-			bcmerror = -EINVAL;
-		break;
+	if (srl < 1 || srl > RETRY_SHORT_MAX ||
+	    lrl < 1 || lrl > RETRY_SHORT_MAX)
+		return -EINVAL;
 
-	case BRCM_GET_CURR_RATESET:{
-			wl_rateset_t *ret_rs = (wl_rateset_t *) arg;
-			wlc_rateset_t *rs;
+	wlc->SRL = srl;
+	wlc->LRL = lrl;
 
-			if (wlc->pub->associated)
-				rs = &current_bss->rateset;
-			else
-				rs = &wlc->default_bss->rateset;
+	brcms_b_retrylimit_upd(wlc->hw, wlc->SRL, wlc->LRL);
 
-			if (len < (int)(rs->count + sizeof(rs->count))) {
-				bcmerror = -EOVERFLOW;
-				break;
-			}
+	for (ac = 0; ac < AC_COUNT; ac++) {
+		wlc->wme_retries[ac] =	SFIELD(wlc->wme_retries[ac],
+					       EDCF_SHORT,  wlc->SRL);
+		wlc->wme_retries[ac] =	SFIELD(wlc->wme_retries[ac],
+					       EDCF_LONG, wlc->LRL);
+	}
+	brcms_c_wme_retries_write(wlc);
 
-			/* Copy only legacy rateset section */
-			ret_rs->count = rs->count;
-			memcpy(&ret_rs->rates, &rs->rates, rs->count);
-			break;
-		}
+	return 0;
+}
 
-	case BRCM_SET_RATESET:{
-			wlc_rateset_t rs;
-			wl_rateset_t *in_rs = (wl_rateset_t *) arg;
+void brcms_c_get_current_rateset(struct brcms_c_info *wlc,
+				 struct brcm_rateset *currs)
+{
+	struct brcms_c_rateset *rs;
 
-			if (len < (int)(in_rs->count + sizeof(in_rs->count))) {
-				bcmerror = -EOVERFLOW;
-				break;
-			}
+	if (wlc->pub->associated)
+		rs = &wlc->bsscfg->current_bss->rateset;
+	else
+		rs = &wlc->default_bss->rateset;
 
-			if (in_rs->count > BRCMS_NUMRATES) {
-				bcmerror = -ENOBUFS;
-				break;
-			}
+	/* Copy only legacy rateset section */
+	currs->count = rs->count;
+	memcpy(&currs->rates, &rs->rates, rs->count);
+}
 
-			memset(&rs, 0, sizeof(wlc_rateset_t));
-
-			/* Copy only legacy rateset section */
-			rs.count = in_rs->count;
-			memcpy(&rs.rates, &in_rs->rates, rs.count);
-
-			/* merge rateset coming in with the current mcsset */
-			if (N_ENAB(wlc->pub)) {
-				if (bsscfg->associated)
-					memcpy(rs.mcs,
-					       &current_bss->rateset.mcs[0],
-					       MCSSET_LEN);
-				else
-					memcpy(rs.mcs,
-					       &wlc->default_bss->rateset.mcs[0],
-					       MCSSET_LEN);
-			}
+int brcms_c_set_rateset(struct brcms_c_info *wlc, struct brcm_rateset *rs)
+{
+	struct brcms_c_rateset internal_rs;
+	int bcmerror;
 
-			bcmerror = brcms_c_set_rateset(wlc, &rs);
+	if (rs->count > BRCMS_NUMRATES)
+		return -ENOBUFS;
 
-			if (!bcmerror)
-				brcms_c_ofdm_rateset_war(wlc);
+	memset(&internal_rs, 0, sizeof(struct brcms_c_rateset));
 
-			break;
-		}
+	/* Copy only legacy rateset section */
+	internal_rs.count = rs->count;
+	memcpy(&internal_rs.rates, &rs->rates, internal_rs.count);
 
-	case BRCM_SET_BCNPRD:
-		/* range [1, 0xffff] */
-		if (val >= DOT11_MIN_BEACON_PERIOD
-		    && val <= DOT11_MAX_BEACON_PERIOD)
-			wlc->default_bss->beacon_period = (u16) val;
+	/* merge rateset coming in with the current mcsset */
+	if (wlc->pub->_n_enab & SUPPORT_11N) {
+		struct brcms_bss_info *mcsset_bss;
+		if (wlc->bsscfg->associated)
+			mcsset_bss = wlc->bsscfg->current_bss;
 		else
-			bcmerror = -EINVAL;
-		break;
+			mcsset_bss = wlc->default_bss;
+		memcpy(internal_rs.mcs, &mcsset_bss->rateset.mcs[0],
+		       MCSSET_LEN);
+	}
 
-	case BRCM_GET_PHYLIST:
-		{
-			unsigned char *cp = arg;
-			if (len < 3) {
-				bcmerror = -EOVERFLOW;
-				break;
-			}
+	bcmerror = brcms_c_set_internal_rateset(wlc, &internal_rs);
+	if (!bcmerror)
+		brcms_c_ofdm_rateset_war(wlc);
 
-			if (BRCMS_ISNPHY(wlc->band))
-				*cp++ = 'n';
-			else if (BRCMS_ISLCNPHY(wlc->band))
-				*cp++ = 'c';
-			else if (BRCMS_ISSSLPNPHY(wlc->band))
-				*cp++ = 's';
-			*cp = '\0';
-			break;
-		}
+	return bcmerror;
+}
 
-	case BRCMS_SET_SHORTSLOT_OVERRIDE:
-		if (val != BRCMS_SHORTSLOT_AUTO && val != BRCMS_SHORTSLOT_OFF &&
-		    val != BRCMS_SHORTSLOT_ON) {
-			bcmerror = -EINVAL;
-			break;
-		}
+int brcms_c_set_beacon_period(struct brcms_c_info *wlc, u16 period)
+{
+	if (period < DOT11_MIN_BEACON_PERIOD ||
+	    period > DOT11_MAX_BEACON_PERIOD)
+		return -EINVAL;
 
-		wlc->shortslot_override = (s8) val;
+	wlc->default_bss->beacon_period = period;
+	return 0;
+}
 
-		/* shortslot is an 11g feature, so no more work if we are
-		 * currently on the 5G band
-		 */
-		if (BAND_5G(wlc->band->bandtype))
-			break;
+u16 brcms_c_get_phy_type(struct brcms_c_info *wlc, int phyidx)
+{
+	return wlc->band->phytype;
+}
 
-		if (wlc->pub->up && wlc->pub->associated) {
-			/* let watchdog or beacon processing update shortslot */
-		} else if (wlc->pub->up) {
-			/* unassociated shortslot is off */
-			brcms_c_switch_shortslot(wlc, false);
-		} else {
-			/* driver is down, so just update the brcms_c_info
-			 * value */
-			if (wlc->shortslot_override == BRCMS_SHORTSLOT_AUTO) {
-				wlc->shortslot = false;
-			} else {
-				wlc->shortslot =
-				    (wlc->shortslot_override ==
-				     BRCMS_SHORTSLOT_ON);
-			}
-		}
+void brcms_c_set_shortslot_override(struct brcms_c_info *wlc, s8 sslot_override)
+{
+	wlc->shortslot_override = sslot_override;
 
-		break;
+	/*
+	 * shortslot is an 11g feature, so no more work if we are
+	 * currently on the 5G band
+	 */
+	if (wlc->band->bandtype == BRCM_BAND_5G)
+		return;
 
+	if (wlc->pub->up && wlc->pub->associated) {
+		/* let watchdog or beacon processing update shortslot */
+	} else if (wlc->pub->up) {
+		/* unassociated shortslot is off */
+		brcms_c_switch_shortslot(wlc, false);
+	} else {
+		/* driver is down, so just update the brcms_c_info
+		 * value */
+		if (wlc->shortslot_override == BRCMS_SHORTSLOT_AUTO)
+			wlc->shortslot = false;
+		else
+			wlc->shortslot =
+			    (wlc->shortslot_override ==
+			     BRCMS_SHORTSLOT_ON);
 	}
- done:
-
-	if (bcmerror)
-		wlc->pub->bcmerror = bcmerror;
-
-	return bcmerror;
 }
 
 /*
  * register watchdog and down handlers.
  */
 int brcms_c_module_register(struct brcms_pub *pub,
-			const char *name, void *hdl,
-			watchdog_fn_t w_fn, down_fn_t d_fn)
+			    const char *name, struct brcms_info *hdl,
+			    int (*d_fn)(void *handle))
 {
 	struct brcms_c_info *wlc = (struct brcms_c_info *) pub->wlc;
 	int i;
@@ -2784,7 +5969,6 @@ int brcms_c_module_register(struct brcms_pub *pub,
 			strncpy(wlc->modulecb[i].name, name,
 				sizeof(wlc->modulecb[i].name) - 1);
 			wlc->modulecb[i].hdl = hdl;
-			wlc->modulecb[i].watchdog_fn = w_fn;
 			wlc->modulecb[i].down_fn = d_fn;
 			return 0;
 		}
@@ -2794,8 +5978,8 @@ int brcms_c_module_register(struct brcms_pub *pub,
 }
 
 /* unregister module callbacks */
-int
-brcms_c_module_unregister(struct brcms_pub *pub, const char *name, void *hdl)
+int brcms_c_module_unregister(struct brcms_pub *pub, const char *name,
+			      struct brcms_info *hdl)
 {
 	struct brcms_c_info *wlc = (struct brcms_c_info *) pub->wlc;
 	int i;
@@ -2815,21 +5999,6 @@ brcms_c_module_unregister(struct brcms_pub *pub, const char *name, void *hdl)
 	return -ENODATA;
 }
 
-/* Write WME tunable parameters for retransmit/max rate from wlc struct to ucode */
-static void brcms_c_wme_retries_write(struct brcms_c_info *wlc)
-{
-	int ac;
-
-	/* Need clock to do this */
-	if (!wlc->clk)
-		return;
-
-	for (ac = 0; ac < AC_COUNT; ac++) {
-		brcms_c_write_shm(wlc, M_AC_TXLMT_ADDR(ac),
-				  wlc->wme_retries[ac]);
-	}
-}
-
 #ifdef BCMDBG
 static const char * const supr_reason[] = {
 	"None", "PMQ Entry", "Flush request",
@@ -2882,59 +6051,6 @@ void brcms_c_print_txstatus(struct tx_status *txs)
 #endif				/* defined(BCMDBG) */
 }
 
-void brcms_c_statsupd(struct brcms_c_info *wlc)
-{
-	int i;
-	struct macstat macstats;
-#ifdef BCMDBG
-	u16 delta;
-	u16 rxf0ovfl;
-	u16 txfunfl[NFIFO];
-#endif				/* BCMDBG */
-
-	/* if driver down, make no sense to update stats */
-	if (!wlc->pub->up)
-		return;
-
-#ifdef BCMDBG
-	/* save last rx fifo 0 overflow count */
-	rxf0ovfl = wlc->core->macstat_snapshot->rxf0ovfl;
-
-	/* save last tx fifo  underflow count */
-	for (i = 0; i < NFIFO; i++)
-		txfunfl[i] = wlc->core->macstat_snapshot->txfunfl[i];
-#endif				/* BCMDBG */
-
-	/* Read mac stats from contiguous shared memory */
-	brcms_b_copyfrom_shm(wlc->hw, M_UCODE_MACSTAT,
-			     &macstats, sizeof(struct macstat));
-
-#ifdef BCMDBG
-	/* check for rx fifo 0 overflow */
-	delta = (u16) (wlc->core->macstat_snapshot->rxf0ovfl - rxf0ovfl);
-	if (delta)
-		wiphy_err(wlc->wiphy, "wl%d: %u rx fifo 0 overflows!\n",
-			  wlc->pub->unit, delta);
-
-	/* check for tx fifo underflows */
-	for (i = 0; i < NFIFO; i++) {
-		delta =
-		    (u16) (wlc->core->macstat_snapshot->txfunfl[i] -
-			      txfunfl[i]);
-		if (delta)
-			wiphy_err(wlc->wiphy, "wl%d: %u tx fifo %d underflows!"
-				  "\n", wlc->pub->unit, delta, i);
-	}
-#endif				/* BCMDBG */
-
-	/* merge counters from dma module */
-	for (i = 0; i < NFIFO; i++) {
-		if (wlc->hw->di[i]) {
-			dma_counterreset(wlc->hw->di[i]);
-		}
-	}
-}
-
 bool brcms_c_chipmatch(u16 vendor, u16 device)
 {
 	if (vendor != PCI_VENDOR_ID_BROADCOM) {
@@ -3042,6 +6158,62 @@ void brcms_c_print_txdesc(struct d11txh *txh)
 #endif				/* defined(BCMDBG) */
 
 #if defined(BCMDBG)
+int
+brcms_c_format_flags(const struct brcms_c_bit_desc *bd, u32 flags, char *buf,
+		   int len)
+{
+	int i;
+	char *p = buf;
+	char hexstr[16];
+	int slen = 0, nlen = 0;
+	u32 bit;
+	const char *name;
+
+	if (len < 2 || !buf)
+		return 0;
+
+	buf[0] = '\0';
+
+	for (i = 0; flags != 0; i++) {
+		bit = bd[i].bit;
+		name = bd[i].name;
+		if (bit == 0 && flags != 0) {
+			/* print any unnamed bits */
+			snprintf(hexstr, 16, "0x%X", flags);
+			name = hexstr;
+			flags = 0;	/* exit loop */
+		} else if ((flags & bit) == 0)
+			continue;
+		flags &= ~bit;
+		nlen = strlen(name);
+		slen += nlen;
+		/* count btwn flag space */
+		if (flags != 0)
+			slen += 1;
+		/* need NULL char as well */
+		if (len <= slen)
+			break;
+		/* copy NULL char but don't count it */
+		strncpy(p, name, nlen + 1);
+		p += nlen;
+		/* copy btwn flag space and NULL char */
+		if (flags != 0)
+			p += snprintf(p, 2, " ");
+		len -= slen;
+	}
+
+	/* indicate the str was too short */
+	if (flags != 0) {
+		if (len < 2)
+			p -= 2 - len;	/* overwrite last char */
+		p += snprintf(p, 2, ">");
+	}
+
+	return (int)(p - buf);
+}
+#endif				/* defined(BCMDBG) */
+
+#if defined(BCMDBG)
 void brcms_c_print_rxh(struct d11rxhdr *rxh)
 {
 	u16 len = rxh->RxFrameSize;
@@ -3053,7 +6225,7 @@ void brcms_c_print_rxh(struct d11rxhdr *rxh)
 	u16 macstatus2 = rxh->RxStatus2;
 	char flagstr[64];
 	char lenbuf[20];
-	static const struct brcmu_bit_desc macstat_flags[] = {
+	static const struct brcms_c_bit_desc macstat_flags[] = {
 		{RXS_FCSERR, "FCSErr"},
 		{RXS_RESPFRAMETX, "Reply"},
 		{RXS_PBPRES, "PADDING"},
@@ -3067,7 +6239,7 @@ void brcms_c_print_rxh(struct d11rxhdr *rxh)
 	print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, rxh,
 			     sizeof(struct d11rxhdr));
 
-	brcmu_format_flags(macstat_flags, macstatus1, flagstr, 64);
+	brcms_c_format_flags(macstat_flags, macstatus1, flagstr, 64);
 
 	snprintf(lenbuf, sizeof(lenbuf), "0x%x", len);
 
@@ -3082,29 +6254,30 @@ void brcms_c_print_rxh(struct d11rxhdr *rxh)
 }
 #endif				/* defined(BCMDBG) */
 
-static u16 brcms_c_rate_shm_offset(struct brcms_c_info *wlc, u8 rate)
+u16 brcms_b_rate_shm_offset(struct brcms_hardware *wlc_hw, u8 rate)
 {
-	return brcms_b_rate_shm_offset(wlc->hw, rate);
-}
+	u16 table_ptr;
+	u8 phy_rate, index;
 
-/* Callback for device removed */
+	/* get the phy specific rate encoding for the PLCP SIGNAL field */
+	if (is_ofdm_rate(rate))
+		table_ptr = M_RT_DIRMAP_A;
+	else
+		table_ptr = M_RT_DIRMAP_B;
 
-/*
- * Attempts to queue a packet onto a multiple-precedence queue,
- * if necessary evicting a lower precedence packet from the queue.
- *
- * 'prec' is the precedence number that has already been mapped
- * from the packet priority.
- *
- * Returns true if packet consumed (queued), false if not.
- */
-bool
-brcms_c_prec_enq(struct brcms_c_info *wlc, struct pktq *q, void *pkt, int prec)
-{
-	return brcms_c_prec_enq_head(wlc, q, pkt, prec, false);
+	/* for a given rate, the LS-nibble of the PLCP SIGNAL field is
+	 * the index into the rate table.
+	 */
+	phy_rate = rate_info[rate] & BRCMS_RATE_MASK;
+	index = phy_rate & 0xf;
+
+	/* Find the SHM pointer to the rate table entry by looking in the
+	 * Direct-map Table
+	 */
+	return 2 * brcms_b_read_shm(wlc_hw, table_ptr + (index * 2));
 }
 
-bool
+static bool
 brcms_c_prec_enq_head(struct brcms_c_info *wlc, struct pktq *q,
 		      struct sk_buff *pkt, int prec, bool head)
 {
@@ -3127,7 +6300,7 @@ brcms_c_prec_enq_head(struct brcms_c_info *wlc, struct pktq *q,
 	if (eprec >= 0) {
 		bool discard_oldest;
 
-		discard_oldest = AC_BITMAP_TST(wlc->wme_dp, eprec);
+		discard_oldest = ac_bitmap_tst(0, eprec);
 
 		/* Refuse newer packet unless configured to discard oldest */
 		if (eprec == prec && !discard_oldest) {
@@ -3151,10 +6324,24 @@ brcms_c_prec_enq_head(struct brcms_c_info *wlc, struct pktq *q,
 	return true;
 }
 
-void brcms_c_txq_enq(void *ctx, struct scb *scb, struct sk_buff *sdu,
-			     uint prec)
+/*
+ * Attempts to queue a packet onto a multiple-precedence queue,
+ * if necessary evicting a lower precedence packet from the queue.
+ *
+ * 'prec' is the precedence number that has already been mapped
+ * from the packet priority.
+ *
+ * Returns true if packet consumed (queued), false if not.
+ */
+static bool brcms_c_prec_enq(struct brcms_c_info *wlc, struct pktq *q,
+		      struct sk_buff *pkt, int prec)
+{
+	return brcms_c_prec_enq_head(wlc, q, pkt, prec, false);
+}
+
+void brcms_c_txq_enq(struct brcms_c_info *wlc, struct scb *scb,
+		     struct sk_buff *sdu, uint prec)
 {
-	struct brcms_c_info *wlc = (struct brcms_c_info *) ctx;
 	struct brcms_txq_info *qi = wlc->pkt_queue;	/* Check me */
 	struct pktq *q = &qi->q;
 	int prio;
@@ -3162,124 +6349,12 @@ void brcms_c_txq_enq(void *ctx, struct scb *scb, struct sk_buff *sdu,
 	prio = sdu->priority;
 
 	if (!brcms_c_prec_enq(wlc, q, sdu, prec)) {
-		if (!EDCF_ENAB(wlc->pub)
-		    || (wlc->pub->wlfeatureflag & WL_SWFL_FLOWCONTROL))
-			wiphy_err(wlc->wiphy, "wl%d: txq_enq: txq overflow"
-				  "\n", wlc->pub->unit);
-
 		/*
 		 * we might hit this condtion in case
 		 * packet flooding from mac80211 stack
 		 */
 		brcmu_pkt_buf_free_skb(sdu);
 	}
-
-	/* Check if flow control needs to be turned on after enqueuing the packet
-	 *   Don't turn on flow control if EDCF is enabled. Driver would make the decision on what
-	 *   to drop instead of relying on stack to make the right decision
-	 */
-	if (!EDCF_ENAB(wlc->pub)
-	    || (wlc->pub->wlfeatureflag & WL_SWFL_FLOWCONTROL)) {
-		if (pktq_len(q) >= wlc->pub->tunables->datahiwat) {
-			brcms_c_txflowcontrol(wlc, qi, ON, ALLPRIO);
-		}
-	} else if (wlc->pub->_priofc) {
-		if (pktq_plen(q, wlc_prio2prec_map[prio]) >=
-		    wlc->pub->tunables->datahiwat) {
-			brcms_c_txflowcontrol(wlc, qi, ON, prio);
-		}
-	}
-}
-
-bool
-brcms_c_sendpkt_mac80211(struct brcms_c_info *wlc, struct sk_buff *sdu,
-		     struct ieee80211_hw *hw)
-{
-	u8 prio;
-	uint fifo;
-	void *pkt;
-	struct scb *scb = &global_scb;
-	struct ieee80211_hdr *d11_header = (struct ieee80211_hdr *)(sdu->data);
-
-	/* 802.11 standard requires management traffic to go at highest priority */
-	prio = ieee80211_is_data(d11_header->frame_control) ? sdu->priority :
-		MAXPRIO;
-	fifo = prio2fifo[prio];
-	pkt = sdu;
-	if (unlikely
-	    (brcms_c_d11hdrs_mac80211(
-		wlc, hw, pkt, scb, 0, 1, fifo, 0, NULL, 0)))
-		return -EINVAL;
-	brcms_c_txq_enq(wlc, scb, pkt, BRCMS_PRIO_TO_PREC(prio));
-	brcms_c_send_q(wlc);
-	return 0;
-}
-
-void brcms_c_send_q(struct brcms_c_info *wlc)
-{
-	struct sk_buff *pkt[DOT11_MAXNUMFRAGS];
-	int prec;
-	u16 prec_map;
-	int err = 0, i, count;
-	uint fifo;
-	struct brcms_txq_info *qi = wlc->pkt_queue;
-	struct pktq *q = &qi->q;
-	struct ieee80211_tx_info *tx_info;
-
-	if (in_send_q)
-		return;
-	else
-		in_send_q = true;
-
-	prec_map = wlc->tx_prec_map;
-
-	/* Send all the enq'd pkts that we can.
-	 * Dequeue packets with precedence with empty HW fifo only
-	 */
-	while (prec_map && (pkt[0] = brcmu_pktq_mdeq(q, prec_map, &prec))) {
-		tx_info = IEEE80211_SKB_CB(pkt[0]);
-		if (tx_info->flags & IEEE80211_TX_CTL_AMPDU) {
-			err = brcms_c_sendampdu(wlc->ampdu, qi, pkt, prec);
-		} else {
-			count = 1;
-			err = brcms_c_prep_pdu(wlc, pkt[0], &fifo);
-			if (!err) {
-				for (i = 0; i < count; i++) {
-					brcms_c_txfifo(wlc, fifo, pkt[i], true,
-						       1);
-				}
-			}
-		}
-
-		if (err == -EBUSY) {
-			brcmu_pktq_penq_head(q, prec, pkt[0]);
-			/* If send failed due to any other reason than a change in
-			 * HW FIFO condition, quit. Otherwise, read the new prec_map!
-			 */
-			if (prec_map == wlc->tx_prec_map)
-				break;
-			prec_map = wlc->tx_prec_map;
-		}
-	}
-
-	/* Check if flow control needs to be turned off after sending the packet */
-	if (!EDCF_ENAB(wlc->pub)
-	    || (wlc->pub->wlfeatureflag & WL_SWFL_FLOWCONTROL)) {
-		if (brcms_c_txflowcontrol_prio_isset(wlc, qi, ALLPRIO)
-		    && (pktq_len(q) < wlc->pub->tunables->datahiwat / 2)) {
-			brcms_c_txflowcontrol(wlc, qi, OFF, ALLPRIO);
-		}
-	} else if (wlc->pub->_priofc) {
-		int prio;
-		for (prio = MAXPRIO; prio >= 0; prio--) {
-			if (brcms_c_txflowcontrol_prio_isset(wlc, qi, prio) &&
-			    (pktq_plen(q, wlc_prio2prec_map[prio]) <
-			     wlc->pub->tunables->datahiwat / 2)) {
-				brcms_c_txflowcontrol(wlc, qi, OFF, prio);
-			}
-		}
-	}
-	in_send_q = false;
 }
 
 /*
@@ -3303,92 +6378,284 @@ bcmc_fid_generate(struct brcms_c_info *wlc, struct brcms_bss_cfg *bsscfg,
 	return frameid;
 }
 
-void
-brcms_c_txfifo(struct brcms_c_info *wlc, uint fifo, struct sk_buff *p,
-	       bool commit, s8 txpktpend)
+static uint
+brcms_c_calc_ack_time(struct brcms_c_info *wlc, u32 rspec,
+		      u8 preamble_type)
 {
-	u16 frameid = INVALIDFID;
-	struct d11txh *txh;
-
-	txh = (struct d11txh *) (p->data);
+	uint dur = 0;
 
-	/* When a BC/MC frame is being committed to the BCMC fifo via DMA (NOT PIO), update
-	 * ucode or BSS info as appropriate.
+	BCMMSG(wlc->wiphy, "wl%d: rspec 0x%x, preamble_type %d\n",
+		wlc->pub->unit, rspec, preamble_type);
+	/*
+	 * Spec 9.6: ack rate is the highest rate in BSSBasicRateSet that
+	 * is less than or equal to the rate of the immediately previous
+	 * frame in the FES
 	 */
-	if (fifo == TX_BCMC_FIFO) {
-		frameid = le16_to_cpu(txh->TxFrameID);
+	rspec = brcms_basic_rate(wlc, rspec);
+	/* ACK frame len == 14 == 2(fc) + 2(dur) + 6(ra) + 4(fcs) */
+	dur =
+	    brcms_c_calc_frame_time(wlc, rspec, preamble_type,
+				(DOT11_ACK_LEN + FCS_LEN));
+	return dur;
+}
 
-	}
+static uint
+brcms_c_calc_cts_time(struct brcms_c_info *wlc, u32 rspec,
+		      u8 preamble_type)
+{
+	BCMMSG(wlc->wiphy, "wl%d: ratespec 0x%x, preamble_type %d\n",
+		wlc->pub->unit, rspec, preamble_type);
+	return brcms_c_calc_ack_time(wlc, rspec, preamble_type);
+}
 
-	if (BRCMS_WAR16165(wlc))
-		brcms_c_war16165(wlc, true);
+static uint
+brcms_c_calc_ba_time(struct brcms_c_info *wlc, u32 rspec,
+		     u8 preamble_type)
+{
+	BCMMSG(wlc->wiphy, "wl%d: rspec 0x%x, "
+		 "preamble_type %d\n", wlc->pub->unit, rspec, preamble_type);
+	/*
+	 * Spec 9.6: ack rate is the highest rate in BSSBasicRateSet that
+	 * is less than or equal to the rate of the immediately previous
+	 * frame in the FES
+	 */
+	rspec = brcms_basic_rate(wlc, rspec);
+	/* BA len == 32 == 16(ctl hdr) + 4(ba len) + 8(bitmap) + 4(fcs) */
+	return brcms_c_calc_frame_time(wlc, rspec, preamble_type,
+				   (DOT11_BA_LEN + DOT11_BA_BITMAP_LEN +
+				    FCS_LEN));
+}
 
+/* brcms_c_compute_frame_dur()
+ *
+ * Calculate the 802.11 MAC header DUR field for MPDU
+ * DUR for a single frame = 1 SIFS + 1 ACK
+ * DUR for a frame with following frags = 3 SIFS + 2 ACK + next frag time
+ *
+ * rate			MPDU rate in unit of 500kbps
+ * next_frag_len	next MPDU length in bytes
+ * preamble_type	use short/GF or long/MM PLCP header
+ */
+static u16
+brcms_c_compute_frame_dur(struct brcms_c_info *wlc, u32 rate,
+		      u8 preamble_type, uint next_frag_len)
+{
+	u16 dur, sifs;
 
-	/* Bump up pending count for if not using rpc. If rpc is used, this will be handled
-	 * in brcms_b_txfifo()
-	 */
-	if (commit) {
-		TXPKTPENDINC(wlc, fifo, txpktpend);
-		BCMMSG(wlc->wiphy, "pktpend inc %d to %d\n",
-			 txpktpend, TXPKTPENDGET(wlc, fifo));
-	}
+	sifs = get_sifs(wlc->band);
 
-	/* Commit BCMC sequence number in the SHM frame ID location */
-	if (frameid != INVALIDFID)
-		BCMCFID(wlc, frameid);
+	dur = sifs;
+	dur += (u16) brcms_c_calc_ack_time(wlc, rate, preamble_type);
 
-	if (dma_txfast(wlc->hw->di[fifo], p, commit) < 0) {
-		wiphy_err(wlc->wiphy, "txfifo: fatal, toss frames !!!\n");
+	if (next_frag_len) {
+		/* Double the current DUR to get 2 SIFS + 2 ACKs */
+		dur *= 2;
+		/* add another SIFS and the frag time */
+		dur += sifs;
+		dur +=
+		    (u16) brcms_c_calc_frame_time(wlc, rate, preamble_type,
+						 next_frag_len);
 	}
+	return dur;
 }
 
-void
-brcms_c_compute_plcp(struct brcms_c_info *wlc, ratespec_t rspec,
-		     uint length, u8 *plcp)
+/* The opposite of brcms_c_calc_frame_time */
+static uint
+brcms_c_calc_frame_len(struct brcms_c_info *wlc, u32 ratespec,
+		   u8 preamble_type, uint dur)
 {
-	if (IS_MCS(rspec)) {
-		brcms_c_compute_mimo_plcp(rspec, length, plcp);
-	} else if (IS_OFDM(rspec)) {
-		brcms_c_compute_ofdm_plcp(rspec, length, plcp);
+	uint nsyms, mac_len, Ndps, kNdps;
+	uint rate = rspec2rate(ratespec);
+
+	BCMMSG(wlc->wiphy, "wl%d: rspec 0x%x, preamble_type %d, dur %d\n",
+		 wlc->pub->unit, ratespec, preamble_type, dur);
+
+	if (is_mcs_rate(ratespec)) {
+		uint mcs = ratespec & RSPEC_RATE_MASK;
+		int tot_streams = mcs_2_txstreams(mcs) + rspec_stc(ratespec);
+		dur -= PREN_PREAMBLE + (tot_streams * PREN_PREAMBLE_EXT);
+		/* payload calculation matches that of regular ofdm */
+		if (wlc->band->bandtype == BRCM_BAND_2G)
+			dur -= DOT11_OFDM_SIGNAL_EXTENSION;
+		/* kNdbps = kbps * 4 */
+		kNdps =	mcs_2_rate(mcs, rspec_is40mhz(ratespec),
+				   rspec_issgi(ratespec)) * 4;
+		nsyms = dur / APHY_SYMBOL_TIME;
+		mac_len =
+		    ((nsyms * kNdps) -
+		     ((APHY_SERVICE_NBITS + APHY_TAIL_NBITS) * 1000)) / 8000;
+	} else if (is_ofdm_rate(ratespec)) {
+		dur -= APHY_PREAMBLE_TIME;
+		dur -= APHY_SIGNAL_TIME;
+		/* Ndbps = Mbps * 4 = rate(500Kbps) * 2 */
+		Ndps = rate * 2;
+		nsyms = dur / APHY_SYMBOL_TIME;
+		mac_len =
+		    ((nsyms * Ndps) -
+		     (APHY_SERVICE_NBITS + APHY_TAIL_NBITS)) / 8;
 	} else {
-		brcms_c_compute_cck_plcp(wlc, rspec, length, plcp);
+		if (preamble_type & BRCMS_SHORT_PREAMBLE)
+			dur -= BPHY_PLCP_SHORT_TIME;
+		else
+			dur -= BPHY_PLCP_TIME;
+		mac_len = dur * rate;
+		/* divide out factor of 2 in rate (1/2 mbps) */
+		mac_len = mac_len / 8 / 2;
 	}
-	return;
+	return mac_len;
 }
 
-/* Rate: 802.11 rate code, length: PSDU length in octets */
-static void brcms_c_compute_mimo_plcp(ratespec_t rspec, uint length, u8 *plcp)
+/*
+ * Return true if the specified rate is supported by the specified band.
+ * BRCM_BAND_AUTO indicates the current band.
+ */
+static bool brcms_c_valid_rate(struct brcms_c_info *wlc, u32 rspec, int band,
+		    bool verbose)
 {
-	u8 mcs = (u8) (rspec & RSPEC_RATE_MASK);
-	plcp[0] = mcs;
-	if (RSPEC_IS40MHZ(rspec) || (mcs == 32))
-		plcp[0] |= MIMO_PLCP_40MHZ;
-	BRCMS_SET_MIMO_PLCP_LEN(plcp, length);
-	plcp[3] = RSPEC_MIMOPLCP3(rspec);	/* rspec already holds this byte */
-	plcp[3] |= 0x7;		/* set smoothing, not sounding ppdu & reserved */
-	plcp[4] = 0;		/* number of extension spatial streams bit 0 & 1 */
-	plcp[5] = 0;
+	struct brcms_c_rateset *hw_rateset;
+	uint i;
+
+	if ((band == BRCM_BAND_AUTO) || (band == wlc->band->bandtype))
+		hw_rateset = &wlc->band->hw_rateset;
+	else if (wlc->pub->_nbands > 1)
+		hw_rateset = &wlc->bandstate[OTHERBANDUNIT(wlc)]->hw_rateset;
+	else
+		/* other band specified and we are a single band device */
+		return false;
+
+	/* check if this is a mimo rate */
+	if (is_mcs_rate(rspec)) {
+		if ((rspec & RSPEC_RATE_MASK) >= MCS_TABLE_SIZE)
+			goto error;
+
+		return isset(hw_rateset->mcs, (rspec & RSPEC_RATE_MASK));
+	}
+
+	for (i = 0; i < hw_rateset->count; i++)
+		if (hw_rateset->rates[i] == rspec2rate(rspec))
+			return true;
+ error:
+	if (verbose)
+		wiphy_err(wlc->wiphy, "wl%d: valid_rate: rate spec 0x%x "
+			  "not in hw_rateset\n", wlc->pub->unit, rspec);
+
+	return false;
 }
 
-/* Rate: 802.11 rate code, length: PSDU length in octets */
-static void
-brcms_c_compute_ofdm_plcp(ratespec_t rspec, u32 length, u8 *plcp)
+static u32
+mac80211_wlc_set_nrate(struct brcms_c_info *wlc, struct brcms_band *cur_band,
+		       u32 int_val)
 {
-	u8 rate_signal;
-	u32 tmp = 0;
-	int rate = RSPEC2RATE(rspec);
+	u8 stf = (int_val & NRATE_STF_MASK) >> NRATE_STF_SHIFT;
+	u8 rate = int_val & NRATE_RATE_MASK;
+	u32 rspec;
+	bool ismcs = ((int_val & NRATE_MCS_INUSE) == NRATE_MCS_INUSE);
+	bool issgi = ((int_val & NRATE_SGI_MASK) >> NRATE_SGI_SHIFT);
+	bool override_mcs_only = ((int_val & NRATE_OVERRIDE_MCS_ONLY)
+				  == NRATE_OVERRIDE_MCS_ONLY);
+	int bcmerror = 0;
 
-	/* encode rate per 802.11a-1999 sec 17.3.4.1, with lsb transmitted first */
-	rate_signal = rate_info[rate] & BRCMS_RATE_MASK;
-	memset(plcp, 0, D11_PHY_HDR_LEN);
-	D11A_PHY_HDR_SRATE((struct ofdm_phy_hdr *) plcp, rate_signal);
+	if (!ismcs)
+		return (u32) rate;
 
-	tmp = (length & 0xfff) << 5;
-	plcp[2] |= (tmp >> 16) & 0xff;
-	plcp[1] |= (tmp >> 8) & 0xff;
-	plcp[0] |= tmp & 0xff;
+	/* validate the combination of rate/mcs/stf is allowed */
+	if ((wlc->pub->_n_enab & SUPPORT_11N) && ismcs) {
+		/* mcs only allowed when nmode */
+		if (stf > PHY_TXC1_MODE_SDM) {
+			wiphy_err(wlc->wiphy, "wl%d: %s: Invalid stf\n",
+				  wlc->pub->unit, __func__);
+			bcmerror = -EINVAL;
+			goto done;
+		}
 
-	return;
+		/* mcs 32 is a special case, DUP mode 40 only */
+		if (rate == 32) {
+			if (!CHSPEC_IS40(wlc->home_chanspec) ||
+			    ((stf != PHY_TXC1_MODE_SISO)
+			     && (stf != PHY_TXC1_MODE_CDD))) {
+				wiphy_err(wlc->wiphy, "wl%d: %s: Invalid mcs "
+					  "32\n", wlc->pub->unit, __func__);
+				bcmerror = -EINVAL;
+				goto done;
+			}
+			/* mcs > 7 must use stf SDM */
+		} else if (rate > HIGHEST_SINGLE_STREAM_MCS) {
+			/* mcs > 7 must use stf SDM */
+			if (stf != PHY_TXC1_MODE_SDM) {
+				BCMMSG(wlc->wiphy, "wl%d: enabling "
+				       "SDM mode for mcs %d\n",
+				       wlc->pub->unit, rate);
+				stf = PHY_TXC1_MODE_SDM;
+			}
+		} else {
+			/*
+			 * MCS 0-7 may use SISO, CDD, and for
+			 * phy_rev >= 3 STBC
+			 */
+			if ((stf > PHY_TXC1_MODE_STBC) ||
+			    (!BRCMS_STBC_CAP_PHY(wlc)
+			     && (stf == PHY_TXC1_MODE_STBC))) {
+				wiphy_err(wlc->wiphy, "wl%d: %s: Invalid STBC"
+					  "\n", wlc->pub->unit, __func__);
+				bcmerror = -EINVAL;
+				goto done;
+			}
+		}
+	} else if (is_ofdm_rate(rate)) {
+		if ((stf != PHY_TXC1_MODE_CDD) && (stf != PHY_TXC1_MODE_SISO)) {
+			wiphy_err(wlc->wiphy, "wl%d: %s: Invalid OFDM\n",
+				  wlc->pub->unit, __func__);
+			bcmerror = -EINVAL;
+			goto done;
+		}
+	} else if (is_cck_rate(rate)) {
+		if ((cur_band->bandtype != BRCM_BAND_2G)
+		    || (stf != PHY_TXC1_MODE_SISO)) {
+			wiphy_err(wlc->wiphy, "wl%d: %s: Invalid CCK\n",
+				  wlc->pub->unit, __func__);
+			bcmerror = -EINVAL;
+			goto done;
+		}
+	} else {
+		wiphy_err(wlc->wiphy, "wl%d: %s: Unknown rate type\n",
+			  wlc->pub->unit, __func__);
+		bcmerror = -EINVAL;
+		goto done;
+	}
+	/* make sure multiple antennae are available for non-siso rates */
+	if ((stf != PHY_TXC1_MODE_SISO) && (wlc->stf->txstreams == 1)) {
+		wiphy_err(wlc->wiphy, "wl%d: %s: SISO antenna but !SISO "
+			  "request\n", wlc->pub->unit, __func__);
+		bcmerror = -EINVAL;
+		goto done;
+	}
+
+	rspec = rate;
+	if (ismcs) {
+		rspec |= RSPEC_MIMORATE;
+		/* For STBC populate the STC field of the ratespec */
+		if (stf == PHY_TXC1_MODE_STBC) {
+			u8 stc;
+			stc = 1;	/* Nss for single stream is always 1 */
+			rspec |= (stc << RSPEC_STC_SHIFT);
+		}
+	}
+
+	rspec |= (stf << RSPEC_STF_SHIFT);
+
+	if (override_mcs_only)
+		rspec |= RSPEC_OVERRIDE_MCS_ONLY;
+
+	if (issgi)
+		rspec |= RSPEC_SHORT_GI;
+
+	if ((rate != 0)
+	    && !brcms_c_valid_rate(wlc, rspec, cur_band->bandtype, true))
+		return rate;
+
+	return rspec;
+done:
+	return rate;
 }
 
 /*
@@ -3426,8 +6693,9 @@ static void brcms_c_cck_plcp_set(struct brcms_c_info *wlc, int rate_500,
 		break;
 
 	default:
-		wiphy_err(wlc->wiphy, "brcms_c_cck_plcp_set: unsupported rate %d"
-			  "\n", rate_500);
+		wiphy_err(wlc->wiphy,
+			  "brcms_c_cck_plcp_set: unsupported rate %d\n",
+			  rate_500);
 		rate_500 = BRCM_RATE_1M;
 		usec = length << 3;
 		break;
@@ -3445,45 +6713,60 @@ static void brcms_c_cck_plcp_set(struct brcms_c_info *wlc, int rate_500,
 }
 
 /* Rate: 802.11 rate code, length: PSDU length in octets */
-static void brcms_c_compute_cck_plcp(struct brcms_c_info *wlc, ratespec_t rspec,
-				 uint length, u8 *plcp)
+static void brcms_c_compute_mimo_plcp(u32 rspec, uint length, u8 *plcp)
 {
-	int rate = RSPEC2RATE(rspec);
-
-	brcms_c_cck_plcp_set(wlc, rate, length, plcp);
+	u8 mcs = (u8) (rspec & RSPEC_RATE_MASK);
+	plcp[0] = mcs;
+	if (rspec_is40mhz(rspec) || (mcs == 32))
+		plcp[0] |= MIMO_PLCP_40MHZ;
+	BRCMS_SET_MIMO_PLCP_LEN(plcp, length);
+	plcp[3] = rspec_mimoplcp3(rspec); /* rspec already holds this byte */
+	plcp[3] |= 0x7; /* set smoothing, not sounding ppdu & reserved */
+	plcp[4] = 0; /* number of extension spatial streams bit 0 & 1 */
+	plcp[5] = 0;
 }
 
-/* brcms_c_compute_frame_dur()
- *
- * Calculate the 802.11 MAC header DUR field for MPDU
- * DUR for a single frame = 1 SIFS + 1 ACK
- * DUR for a frame with following frags = 3 SIFS + 2 ACK + next frag time
- *
- * rate			MPDU rate in unit of 500kbps
- * next_frag_len	next MPDU length in bytes
- * preamble_type	use short/GF or long/MM PLCP header
- */
-static u16
-brcms_c_compute_frame_dur(struct brcms_c_info *wlc, ratespec_t rate,
-		      u8 preamble_type, uint next_frag_len)
+/* Rate: 802.11 rate code, length: PSDU length in octets */
+static void
+brcms_c_compute_ofdm_plcp(u32 rspec, u32 length, u8 *plcp)
 {
-	u16 dur, sifs;
+	u8 rate_signal;
+	u32 tmp = 0;
+	int rate = rspec2rate(rspec);
 
-	sifs = SIFS(wlc->band);
+	/*
+	 * encode rate per 802.11a-1999 sec 17.3.4.1, with lsb
+	 * transmitted first
+	 */
+	rate_signal = rate_info[rate] & BRCMS_RATE_MASK;
+	memset(plcp, 0, D11_PHY_HDR_LEN);
+	D11A_PHY_HDR_SRATE((struct ofdm_phy_hdr *) plcp, rate_signal);
 
-	dur = sifs;
-	dur += (u16) brcms_c_calc_ack_time(wlc, rate, preamble_type);
+	tmp = (length & 0xfff) << 5;
+	plcp[2] |= (tmp >> 16) & 0xff;
+	plcp[1] |= (tmp >> 8) & 0xff;
+	plcp[0] |= tmp & 0xff;
+}
 
-	if (next_frag_len) {
-		/* Double the current DUR to get 2 SIFS + 2 ACKs */
-		dur *= 2;
-		/* add another SIFS and the frag time */
-		dur += sifs;
-		dur +=
-		    (u16) brcms_c_calc_frame_time(wlc, rate, preamble_type,
-						 next_frag_len);
-	}
-	return dur;
+/* Rate: 802.11 rate code, length: PSDU length in octets */
+static void brcms_c_compute_cck_plcp(struct brcms_c_info *wlc, u32 rspec,
+				 uint length, u8 *plcp)
+{
+	int rate = rspec2rate(rspec);
+
+	brcms_c_cck_plcp_set(wlc, rate, length, plcp);
+}
+
+static void
+brcms_c_compute_plcp(struct brcms_c_info *wlc, u32 rspec,
+		     uint length, u8 *plcp)
+{
+	if (is_mcs_rate(rspec))
+		brcms_c_compute_mimo_plcp(rspec, length, plcp);
+	else if (is_ofdm_rate(rspec))
+		brcms_c_compute_ofdm_plcp(rspec, length, plcp);
+	else
+		brcms_c_compute_cck_plcp(wlc, rspec, length, plcp);
 }
 
 /* brcms_c_compute_rtscts_dur()
@@ -3499,20 +6782,22 @@ brcms_c_compute_frame_dur(struct brcms_c_info *wlc, ratespec_t rate,
  */
 u16
 brcms_c_compute_rtscts_dur(struct brcms_c_info *wlc, bool cts_only,
-			   ratespec_t rts_rate,
-			   ratespec_t frame_rate, u8 rts_preamble_type,
+			   u32 rts_rate,
+			   u32 frame_rate, u8 rts_preamble_type,
 			   u8 frame_preamble_type, uint frame_len, bool ba)
 {
 	u16 dur, sifs;
 
-	sifs = SIFS(wlc->band);
+	sifs = get_sifs(wlc->band);
 
-	if (!cts_only) {	/* RTS/CTS */
+	if (!cts_only) {
+		/* RTS/CTS */
 		dur = 3 * sifs;
 		dur +=
 		    (u16) brcms_c_calc_cts_time(wlc, rts_rate,
 					       rts_preamble_type);
-	} else {		/* CTS-TO-SELF */
+	} else {
+		/* CTS-TO-SELF */
 		dur = 2 * sifs;
 	}
 
@@ -3530,7 +6815,7 @@ brcms_c_compute_rtscts_dur(struct brcms_c_info *wlc, bool cts_only,
 	return dur;
 }
 
-u16 brcms_c_phytxctl1_calc(struct brcms_c_info *wlc, ratespec_t rspec)
+static u16 brcms_c_phytxctl1_calc(struct brcms_c_info *wlc, u32 rspec)
 {
 	u16 phyctl1 = 0;
 	u16 bw;
@@ -3538,7 +6823,7 @@ u16 brcms_c_phytxctl1_calc(struct brcms_c_info *wlc, ratespec_t rspec)
 	if (BRCMS_ISLCNPHY(wlc->band)) {
 		bw = PHY_TXC1_BW_20MHZ;
 	} else {
-		bw = RSPEC_GET_BW(rspec);
+		bw = rspec_get_bw(rspec);
 		/* 10Mhz is not supported yet */
 		if (bw < PHY_TXC1_BW_20MHZ) {
 			wiphy_err(wlc->wiphy, "phytxctl1_calc: bw %d is "
@@ -3547,23 +6832,26 @@ u16 brcms_c_phytxctl1_calc(struct brcms_c_info *wlc, ratespec_t rspec)
 		}
 	}
 
-	if (IS_MCS(rspec)) {
+	if (is_mcs_rate(rspec)) {
 		uint mcs = rspec & RSPEC_RATE_MASK;
 
-		/* bw, stf, coding-type is part of RSPEC_PHYTXBYTE2 returns */
-		phyctl1 = RSPEC_PHYTXBYTE2(rspec);
+		/* bw, stf, coding-type is part of rspec_phytxbyte2 returns */
+		phyctl1 = rspec_phytxbyte2(rspec);
 		/* set the upper byte of phyctl1 */
 		phyctl1 |= (mcs_table[mcs].tx_phy_ctl3 << 8);
-	} else if (IS_CCK(rspec) && !BRCMS_ISLCNPHY(wlc->band)
+	} else if (is_cck_rate(rspec) && !BRCMS_ISLCNPHY(wlc->band)
 		   && !BRCMS_ISSSLPNPHY(wlc->band)) {
-		/* In CCK mode LPPHY overloads OFDM Modulation bits with CCK Data Rate */
-		/* Eventually MIMOPHY would also be converted to this format */
+		/*
+		 * In CCK mode LPPHY overloads OFDM Modulation bits with CCK
+		 * Data Rate. Eventually MIMOPHY would also be converted to
+		 * this format
+		 */
 		/* 0 = 1Mbps; 1 = 2Mbps; 2 = 5.5Mbps; 3 = 11Mbps */
-		phyctl1 = (bw | (RSPEC_STF(rspec) << PHY_TXC1_MODE_SHIFT));
+		phyctl1 = (bw | (rspec_stf(rspec) << PHY_TXC1_MODE_SHIFT));
 	} else {		/* legacy OFDM/CCK */
 		s16 phycfg;
 		/* get the phyctl byte from rate phycfg table */
-		phycfg = brcms_c_rate_legacy_phyctl(RSPEC2RATE(rspec));
+		phycfg = brcms_c_rate_legacy_phyctl(rspec2rate(rspec));
 		if (phycfg == -1) {
 			wiphy_err(wlc->wiphy, "phytxctl1_calc: wrong "
 				  "legacy OFDM/CCK rate\n");
@@ -3572,58 +6860,11 @@ u16 brcms_c_phytxctl1_calc(struct brcms_c_info *wlc, ratespec_t rspec)
 		/* set the upper byte of phyctl1 */
 		phyctl1 =
 		    (bw | (phycfg << 8) |
-		     (RSPEC_STF(rspec) << PHY_TXC1_MODE_SHIFT));
+		     (rspec_stf(rspec) << PHY_TXC1_MODE_SHIFT));
 	}
 	return phyctl1;
 }
 
-ratespec_t
-brcms_c_rspec_to_rts_rspec(struct brcms_c_info *wlc, ratespec_t rspec,
-			   bool use_rspec, u16 mimo_ctlchbw)
-{
-	ratespec_t rts_rspec = 0;
-
-	if (use_rspec) {
-		/* use frame rate as rts rate */
-		rts_rspec = rspec;
-
-	} else if (wlc->band->gmode && wlc->protection->_g && !IS_CCK(rspec)) {
-		/* Use 11Mbps as the g protection RTS target rate and fallback.
-		 * Use the BRCMS_BASIC_RATE() lookup to find the best basic rate
-		 * under the target in case 11 Mbps is not Basic.
-		 * 6 and 9 Mbps are not usually selected by rate selection, but even
-		 * if the OFDM rate we are protecting is 6 or 9 Mbps, 11 is more robust.
-		 */
-		rts_rspec = BRCMS_BASIC_RATE(wlc, BRCM_RATE_11M);
-	} else {
-		/* calculate RTS rate and fallback rate based on the frame rate
-		 * RTS must be sent at a basic rate since it is a
-		 * control frame, sec 9.6 of 802.11 spec
-		 */
-		rts_rspec = BRCMS_BASIC_RATE(wlc, rspec);
-	}
-
-	if (BRCMS_PHY_11N_CAP(wlc->band)) {
-		/* set rts txbw to correct side band */
-		rts_rspec &= ~RSPEC_BW_MASK;
-
-		/* if rspec/rspec_fallback is 40MHz, then send RTS on both 20MHz channel
-		 * (DUP), otherwise send RTS on control channel
-		 */
-		if (RSPEC_IS40MHZ(rspec) && !IS_CCK(rts_rspec))
-			rts_rspec |= (PHY_TXC1_BW_40MHZ_DUP << RSPEC_BW_SHIFT);
-		else
-			rts_rspec |= (mimo_ctlchbw << RSPEC_BW_SHIFT);
-
-		/* pick siso/cdd as default for ofdm */
-		if (IS_OFDM(rts_rspec)) {
-			rts_rspec &= ~RSPEC_STF_MASK;
-			rts_rspec |= (wlc->stf->ss_opmode << RSPEC_STF_SHIFT);
-		}
-	}
-	return rts_rspec;
-}
-
 /*
  * Add struct d11txh, struct cck_phy_hdr.
  *
@@ -3636,8 +6877,7 @@ brcms_c_rspec_to_rts_rspec(struct brcms_c_info *wlc, ratespec_t rspec,
 static u16
 brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 		     struct sk_buff *p, struct scb *scb, uint frag,
-		     uint nfrags, uint queue, uint next_frag_len,
-		     struct wsec_key *key, ratespec_t rspec_override)
+		     uint nfrags, uint queue, uint next_frag_len)
 {
 	struct ieee80211_hdr *h;
 	struct d11txh *txh;
@@ -3645,8 +6885,8 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 	int len, phylen, rts_phylen;
 	u16 mch, phyctl, xfts, mainrates;
 	u16 seq = 0, mcl = 0, status = 0, frameid = 0;
-	ratespec_t rspec[2] = { BRCM_RATE_1M, BRCM_RATE_1M }, rts_rspec[2] = {
-	BRCM_RATE_1M, BRCM_RATE_1M};
+	u32 rspec[2] = { BRCM_RATE_1M, BRCM_RATE_1M };
+	u32 rts_rspec[2] = { BRCM_RATE_1M, BRCM_RATE_1M };
 	bool use_rts = false;
 	bool use_cts = false;
 	bool use_rifs = false;
@@ -3657,7 +6897,6 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 	struct ieee80211_rts *rts = NULL;
 	bool qos;
 	uint ac;
-	u32 rate_val[2];
 	bool hwtkmic = false;
 	u16 mimo_ctlchbw = PHY_TXC1_BW_20MHZ;
 #define ANTCFG_NONE 0xFF
@@ -3668,7 +6907,7 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 	struct ieee80211_tx_rate *txrate[2];
 	int k;
 	struct ieee80211_tx_info *tx_info;
-	bool is_mcs[2];
+	bool is_mcs;
 	u16 mimo_txbw;
 	u8 mimo_preamble_type;
 
@@ -3680,15 +6919,6 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 	len = brcmu_pkttotlen(p);
 	phylen = len + FCS_LEN;
 
-	/* If WEP enabled, add room in phylen for the additional bytes of
-	 * ICV which MAC generates.  We do NOT add the additional bytes to
-	 * the packet itself, thus phylen = packet length + ICV_LEN + FCS_LEN
-	 * in this case
-	 */
-	if (key) {
-		phylen += key->icv_len;
-	}
-
 	/* Get tx_info */
 	tx_info = IEEE80211_SKB_CB(p);
 
@@ -3704,17 +6934,16 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 		/* non-AP STA should never use BCMC queue */
 		if (queue == TX_BCMC_FIFO) {
 			wiphy_err(wlc->wiphy, "wl%d: %s: ASSERT queue == "
-				  "TX_BCMC!\n", BRCMS_UNIT(wlc), __func__);
+				  "TX_BCMC!\n", wlc->pub->unit, __func__);
 			frameid = bcmc_fid_generate(wlc, NULL, txh);
 		} else {
 			/* Increment the counter for first fragment */
-			if (tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT) {
-				SCB_SEQNUM(scb, p->priority)++;
-			}
+			if (tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT)
+				scb->seqnum[p->priority]++;
 
 			/* extract fragment number from frame first */
-			seq = le16_to_cpu(seq) & FRAGNUM_MASK;
-			seq |= (SCB_SEQNUM(scb, p->priority) << SEQNUM_SHIFT);
+			seq = le16_to_cpu(h->seq_ctrl) & FRAGNUM_MASK;
+			seq |= (scb->seqnum[p->priority] << SEQNUM_SHIFT);
 			h->seq_ctrl = cpu_to_le16(seq);
 
 			frameid = ((seq << TXFID_SEQ_SHIFT) & TXFID_SEQ_MASK) |
@@ -3724,25 +6953,26 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 	frameid |= queue & TXFID_QUEUE_MASK;
 
 	/* set the ignpmq bit for all pkts tx'd in PS mode and for beacons */
-	if (SCB_PS(scb) || ieee80211_is_beacon(h->frame_control))
+	if (ieee80211_is_beacon(h->frame_control))
 		mcl |= TXC_IGNOREPMQ;
 
 	txrate[0] = tx_info->control.rates;
 	txrate[1] = txrate[0] + 1;
 
-	/* if rate control algorithm didn't give us a fallback rate, use the primary rate */
-	if (txrate[1]->idx < 0) {
+	/*
+	 * if rate control algorithm didn't give us a fallback
+	 * rate, use the primary rate
+	 */
+	if (txrate[1]->idx < 0)
 		txrate[1] = txrate[0];
-	}
 
 	for (k = 0; k < hw->max_rates; k++) {
-		is_mcs[k] =
-		    txrate[k]->flags & IEEE80211_TX_RC_MCS ? true : false;
-		if (!is_mcs[k]) {
+		is_mcs = txrate[k]->flags & IEEE80211_TX_RC_MCS ? true : false;
+		if (!is_mcs) {
 			if ((txrate[k]->idx >= 0)
 			    && (txrate[k]->idx <
 				hw->wiphy->bands[tx_info->band]->n_bitrates)) {
-				rate_val[k] =
+				rspec[k] =
 				    hw->wiphy->bands[tx_info->band]->
 				    bitrates[txrate[k]->idx].hw_value;
 				short_preamble[k] =
@@ -3750,13 +6980,17 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 				    flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE ?
 				    true : false;
 			} else {
-				rate_val[k] = BRCM_RATE_1M;
+				rspec[k] = BRCM_RATE_1M;
 			}
 		} else {
-			rate_val[k] = txrate[k]->idx;
+			rspec[k] = mac80211_wlc_set_nrate(wlc, wlc->band,
+					NRATE_MCS_INUSE | txrate[k]->idx);
 		}
-		/* Currently only support same setting for primay and fallback rates.
-		 * Unify flags for each rate into a single value for the frame
+
+		/*
+		 * Currently only support same setting for primay and
+		 * fallback rates. Unify flags for each rate into a
+		 * single value for the frame
 		 */
 		use_rts |=
 		    txrate[k]->
@@ -3765,13 +6999,13 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 		    txrate[k]->
 		    flags & IEEE80211_TX_RC_USE_CTS_PROTECT ? true : false;
 
-		if (is_mcs[k])
-			rate_val[k] |= NRATE_MCS_INUSE;
 
-		rspec[k] = mac80211_wlc_set_nrate(wlc, wlc->band, rate_val[k]);
-
-		/* (1) RATE: determine and validate primary rate and fallback rates */
-		if (!RSPEC_ACTIVE(rspec[k])) {
+		/*
+		 * (1) RATE:
+		 *   determine and validate primary rate
+		 *   and fallback rates
+		 */
+		if (!rspec_active(rspec[k])) {
 			rspec[k] = BRCM_RATE_1M;
 		} else {
 			if (!is_multicast_ether_addr(h->addr1)) {
@@ -3784,41 +7018,49 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 
 	phyctl1_stf = wlc->stf->ss_opmode;
 
-	if (N_ENAB(wlc->pub)) {
+	if (wlc->pub->_n_enab & SUPPORT_11N) {
 		for (k = 0; k < hw->max_rates; k++) {
-			/* apply siso/cdd to single stream mcs's or ofdm if rspec is auto selected */
-			if (((IS_MCS(rspec[k]) &&
-			      IS_SINGLE_STREAM(rspec[k] & RSPEC_RATE_MASK)) ||
-			     IS_OFDM(rspec[k]))
+			/*
+			 * apply siso/cdd to single stream mcs's or ofdm
+			 * if rspec is auto selected
+			 */
+			if (((is_mcs_rate(rspec[k]) &&
+			      is_single_stream(rspec[k] & RSPEC_RATE_MASK)) ||
+			     is_ofdm_rate(rspec[k]))
 			    && ((rspec[k] & RSPEC_OVERRIDE_MCS_ONLY)
 				|| !(rspec[k] & RSPEC_OVERRIDE))) {
 				rspec[k] &= ~(RSPEC_STF_MASK | RSPEC_STC_MASK);
 
 				/* For SISO MCS use STBC if possible */
-				if (IS_MCS(rspec[k])
+				if (is_mcs_rate(rspec[k])
 				    && BRCMS_STF_SS_STBC_TX(wlc, scb)) {
 					u8 stc;
 
-					stc = 1;	/* Nss for single stream is always 1 */
-					rspec[k] |=
-					    (PHY_TXC1_MODE_STBC <<
-					     RSPEC_STF_SHIFT) | (stc <<
-								 RSPEC_STC_SHIFT);
+					/* Nss for single stream is always 1 */
+					stc = 1;
+					rspec[k] |= (PHY_TXC1_MODE_STBC <<
+							RSPEC_STF_SHIFT) |
+						    (stc << RSPEC_STC_SHIFT);
 				} else
 					rspec[k] |=
 					    (phyctl1_stf << RSPEC_STF_SHIFT);
 			}
 
-			/* Is the phy configured to use 40MHZ frames? If so then pick the desired txbw */
-			if (CHSPEC_WLC_BW(wlc->chanspec) == BRCMS_40_MHZ) {
+			/*
+			 * Is the phy configured to use 40MHZ frames? If
+			 * so then pick the desired txbw
+			 */
+			if (brcms_chspec_bw(wlc->chanspec) == BRCMS_40_MHZ) {
 				/* default txbw is 20in40 SB */
 				mimo_ctlchbw = mimo_txbw =
-				   CHSPEC_SB_UPPER(BRCMS_BAND_PI_RADIO_CHANSPEC)
+				   CHSPEC_SB_UPPER(wlc_phy_chanspec_get(
+								 wlc->band->pi))
 				   ? PHY_TXC1_BW_20MHZ_UP : PHY_TXC1_BW_20MHZ;
 
-				if (IS_MCS(rspec[k])) {
+				if (is_mcs_rate(rspec[k])) {
 					/* mcs 32 must be 40b/w DUP */
-					if ((rspec[k] & RSPEC_RATE_MASK) == 32) {
+					if ((rspec[k] & RSPEC_RATE_MASK)
+					    == 32) {
 						mimo_txbw =
 						    PHY_TXC1_BW_40MHZ_DUP;
 						/* use override */
@@ -3827,66 +7069,61 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 					/* else check if dst is using 40 Mhz */
 					else if (scb->flags & SCB_IS40)
 						mimo_txbw = PHY_TXC1_BW_40MHZ;
-				} else if (IS_OFDM(rspec[k])) {
+				} else if (is_ofdm_rate(rspec[k])) {
 					if (wlc->ofdm_40txbw != AUTO)
 						mimo_txbw = wlc->ofdm_40txbw;
-				} else {
-					if (wlc->cck_40txbw != AUTO)
-						mimo_txbw = wlc->cck_40txbw;
+				} else if (wlc->cck_40txbw != AUTO) {
+					mimo_txbw = wlc->cck_40txbw;
 				}
 			} else {
-				/* mcs32 is 40 b/w only.
-				 * This is possible for probe packets on a STA during SCAN
+				/*
+				 * mcs32 is 40 b/w only.
+				 * This is possible for probe packets on
+				 * a STA during SCAN
 				 */
-				if ((rspec[k] & RSPEC_RATE_MASK) == 32) {
+				if ((rspec[k] & RSPEC_RATE_MASK) == 32)
 					/* mcs 0 */
 					rspec[k] = RSPEC_MIMORATE;
-				}
+
 				mimo_txbw = PHY_TXC1_BW_20MHZ;
 			}
 
 			/* Set channel width */
 			rspec[k] &= ~RSPEC_BW_MASK;
-			if ((k == 0) || ((k > 0) && IS_MCS(rspec[k])))
+			if ((k == 0) || ((k > 0) && is_mcs_rate(rspec[k])))
 				rspec[k] |= (mimo_txbw << RSPEC_BW_SHIFT);
 			else
 				rspec[k] |= (mimo_ctlchbw << RSPEC_BW_SHIFT);
 
-			/* Set Short GI */
-#ifdef NOSGIYET
-			if (IS_MCS(rspec[k])
-			    && (txrate[k]->flags & IEEE80211_TX_RC_SHORT_GI))
-				rspec[k] |= RSPEC_SHORT_GI;
-			else if (!(txrate[k]->flags & IEEE80211_TX_RC_SHORT_GI))
-				rspec[k] &= ~RSPEC_SHORT_GI;
-#else
+			/* Disable short GI, not supported yet */
 			rspec[k] &= ~RSPEC_SHORT_GI;
-#endif
 
 			mimo_preamble_type = BRCMS_MM_PREAMBLE;
 			if (txrate[k]->flags & IEEE80211_TX_RC_GREEN_FIELD)
 				mimo_preamble_type = BRCMS_GF_PREAMBLE;
 
 			if ((txrate[k]->flags & IEEE80211_TX_RC_MCS)
-			    && (!IS_MCS(rspec[k]))) {
+			    && (!is_mcs_rate(rspec[k]))) {
 				wiphy_err(wlc->wiphy, "wl%d: %s: IEEE80211_TX_"
-					  "RC_MCS != IS_MCS(rspec)\n",
-					  BRCMS_UNIT(wlc), __func__);
+					  "RC_MCS != is_mcs_rate(rspec)\n",
+					  wlc->pub->unit, __func__);
 			}
 
-			if (IS_MCS(rspec[k])) {
+			if (is_mcs_rate(rspec[k])) {
 				preamble_type[k] = mimo_preamble_type;
 
-				/* if SGI is selected, then forced mm for single stream */
+				/*
+				 * if SGI is selected, then forced mm
+				 * for single stream
+				 */
 				if ((rspec[k] & RSPEC_SHORT_GI)
-				    && IS_SINGLE_STREAM(rspec[k] &
-							RSPEC_RATE_MASK)) {
+				    && is_single_stream(rspec[k] &
+							RSPEC_RATE_MASK))
 					preamble_type[k] = BRCMS_MM_PREAMBLE;
-				}
 			}
 
 			/* should be better conditionalized */
-			if (!IS_MCS(rspec[0])
+			if (!is_mcs_rate(rspec[0])
 			    && (tx_info->control.rates[0].
 				flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE))
 				preamble_type[k] = BRCMS_SHORT_PREAMBLE;
@@ -3898,7 +7135,7 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 			rspec[k] |= (PHY_TXC1_BW_20MHZ << RSPEC_BW_SHIFT);
 
 			/* for nphy, stf of ofdm frames must follow policies */
-			if (BRCMS_ISNPHY(wlc->band) && IS_OFDM(rspec[k])) {
+			if (BRCMS_ISNPHY(wlc->band) && is_ofdm_rate(rspec[k])) {
 				rspec[k] &= ~RSPEC_STF_MASK;
 				rspec[k] |= phyctl1_stf << RSPEC_STF_SHIFT;
 			}
@@ -3923,13 +7160,13 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 	       plcp_fallback, sizeof(txh->FragPLCPFallback));
 
 	/* Length field now put in CCK FBR CRC field */
-	if (IS_CCK(rspec[1])) {
+	if (is_cck_rate(rspec[1])) {
 		txh->FragPLCPFallback[4] = phylen & 0xff;
 		txh->FragPLCPFallback[5] = (phylen & 0xff00) >> 8;
 	}
 
 	/* MIMO-RATE: need validation ?? */
-	mainrates = IS_OFDM(rspec[0]) ?
+	mainrates = is_ofdm_rate(rspec[0]) ?
 			D11A_PHY_HDR_GRATE((struct ofdm_phy_hdr *) plcp) :
 			plcp[0];
 
@@ -3968,10 +7205,10 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 	if (!is_multicast_ether_addr(h->addr1))
 		mcl |= TXC_IMMEDACK;
 
-	if (BAND_5G(wlc->band->bandtype))
+	if (wlc->band->bandtype == BRCM_BAND_5G)
 		mcl |= TXC_FREQBAND_5G;
 
-	if (CHSPEC_IS40(BRCMS_BAND_PI_RADIO_CHANSPEC))
+	if (CHSPEC_IS40(wlc_phy_chanspec_get(wlc->band->pi)))
 		mcl |= TXC_BW_40;
 
 	/* set AMIC bit if using hardware TKIP MIC */
@@ -3986,7 +7223,7 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 	/* Set fallback rate preamble type */
 	if ((preamble_type[1] == BRCMS_SHORT_PREAMBLE) ||
 	    (preamble_type[1] == BRCMS_GF_PREAMBLE)) {
-		if (RSPEC2RATE(rspec[1]) != BRCM_RATE_1M)
+		if (rspec2rate(rspec[1]) != BRCM_RATE_1M)
 			mch |= TXC_PREAMBLE_DATA_FB_SHORT;
 	}
 
@@ -4002,12 +7239,14 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 	/* TxFrameID */
 	txh->TxFrameID = cpu_to_le16(frameid);
 
-	/* TxStatus, Note the case of recreating the first frag of a suppressed frame
-	 * then we may need to reset the retry cnt's via the status reg
+	/*
+	 * TxStatus, Note the case of recreating the first frag of a suppressed
+	 * frame then we may need to reset the retry cnt's via the status reg
 	 */
 	txh->TxStatus = cpu_to_le16(status);
 
-	/* extra fields for ucode AMPDU aggregation, the new fields are added to
+	/*
+	 * extra fields for ucode AMPDU aggregation, the new fields are added to
 	 * the END of previous structure so that it's compatible in driver.
 	 */
 	txh->MaxNMpdus = cpu_to_le16(0);
@@ -4028,15 +7267,15 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 							      mimo_ctlchbw);
 		}
 
-		if (!IS_OFDM(rts_rspec[0]) &&
-		    !((RSPEC2RATE(rts_rspec[0]) == BRCM_RATE_1M) ||
+		if (!is_ofdm_rate(rts_rspec[0]) &&
+		    !((rspec2rate(rts_rspec[0]) == BRCM_RATE_1M) ||
 		      (wlc->PLCPHdr_override == BRCMS_PLCP_LONG))) {
 			rts_preamble_type[0] = BRCMS_SHORT_PREAMBLE;
 			mch |= TXC_PREAMBLE_RTS_MAIN_SHORT;
 		}
 
-		if (!IS_OFDM(rts_rspec[1]) &&
-		    !((RSPEC2RATE(rts_rspec[1]) == BRCM_RATE_1M) ||
+		if (!is_ofdm_rate(rts_rspec[1]) &&
+		    !((rspec2rate(rts_rspec[1]) == BRCM_RATE_1M) ||
 		      (wlc->PLCPHdr_override == BRCMS_PLCP_LONG))) {
 			rts_preamble_type[1] = BRCMS_SHORT_PREAMBLE;
 			mch |= TXC_PREAMBLE_RTS_FB_SHORT;
@@ -4095,7 +7334,7 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 		 *    low 8 bits: main frag rate/mcs,
 		 *    high 8 bits: rts/cts rate/mcs
 		 */
-		mainrates |= (IS_OFDM(rts_rspec[0]) ?
+		mainrates |= (is_ofdm_rate(rts_rspec[0]) ?
 				D11A_PHY_HDR_GRATE(
 					(struct ofdm_phy_hdr *) rts_plcp) :
 				rts_plcp[0]) << 8;
@@ -4110,31 +7349,37 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 
 #ifdef SUPPORT_40MHZ
 	/* add null delimiter count */
-	if ((tx_info->flags & IEEE80211_TX_CTL_AMPDU) && IS_MCS(rspec)) {
+	if ((tx_info->flags & IEEE80211_TX_CTL_AMPDU) && is_mcs_rate(rspec))
 		txh->RTSPLCPFallback[AMPDU_FBR_NULL_DELIM] =
 		   brcm_c_ampdu_null_delim_cnt(wlc->ampdu, scb, rspec, phylen);
-	}
+
 #endif
 
-	/* Now that RTS/RTS FB preamble types are updated, write the final value */
+	/*
+	 * Now that RTS/RTS FB preamble types are updated, write
+	 * the final value
+	 */
 	txh->MacTxControlHigh = cpu_to_le16(mch);
 
-	/* MainRates (both the rts and frag plcp rates have been calculated now) */
+	/*
+	 * MainRates (both the rts and frag plcp rates have
+	 * been calculated now)
+	 */
 	txh->MainRates = cpu_to_le16(mainrates);
 
 	/* XtraFrameTypes */
-	xfts = FRAMETYPE(rspec[1], wlc->mimoft);
-	xfts |= (FRAMETYPE(rts_rspec[0], wlc->mimoft) << XFTS_RTS_FT_SHIFT);
-	xfts |= (FRAMETYPE(rts_rspec[1], wlc->mimoft) << XFTS_FBRRTS_FT_SHIFT);
-	xfts |=
-	    CHSPEC_CHANNEL(BRCMS_BAND_PI_RADIO_CHANSPEC) << XFTS_CHANNEL_SHIFT;
+	xfts = frametype(rspec[1], wlc->mimoft);
+	xfts |= (frametype(rts_rspec[0], wlc->mimoft) << XFTS_RTS_FT_SHIFT);
+	xfts |= (frametype(rts_rspec[1], wlc->mimoft) << XFTS_FBRRTS_FT_SHIFT);
+	xfts |= CHSPEC_CHANNEL(wlc_phy_chanspec_get(wlc->band->pi)) <<
+							     XFTS_CHANNEL_SHIFT;
 	txh->XtraFrameTypes = cpu_to_le16(xfts);
 
 	/* PhyTxControlWord */
-	phyctl = FRAMETYPE(rspec[0], wlc->mimoft);
+	phyctl = frametype(rspec[0], wlc->mimoft);
 	if ((preamble_type[0] == BRCMS_SHORT_PREAMBLE) ||
 	    (preamble_type[0] == BRCMS_GF_PREAMBLE)) {
-		if (RSPEC2RATE(rspec[0]) != BRCM_RATE_1M)
+		if (rspec2rate(rspec[0]) != BRCM_RATE_1M)
 			phyctl |= PHY_TXC_SHORT_HDR;
 	}
 
@@ -4159,18 +7404,18 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 		}
 
 		/*
-		 * For mcs frames, if mixedmode(overloaded with long preamble) is going to be set,
-		 * fill in non-zero MModeLen and/or MModeFbrLen
-		 *  it will be unnecessary if they are separated
+		 * For mcs frames, if mixedmode(overloaded with long preamble)
+		 * is going to be set, fill in non-zero MModeLen and/or
+		 * MModeFbrLen it will be unnecessary if they are separated
 		 */
-		if (IS_MCS(rspec[0]) &&
+		if (is_mcs_rate(rspec[0]) &&
 		    (preamble_type[0] == BRCMS_MM_PREAMBLE)) {
 			u16 mmodelen =
 			    brcms_c_calc_lsig_len(wlc, rspec[0], phylen);
 			txh->MModeLen = cpu_to_le16(mmodelen);
 		}
 
-		if (IS_MCS(rspec[1]) &&
+		if (is_mcs_rate(rspec[1]) &&
 		    (preamble_type[1] == BRCMS_MM_PREAMBLE)) {
 			u16 mmodefbrlen =
 			    brcms_c_calc_lsig_len(wlc, rspec[1], phylen);
@@ -4179,11 +7424,11 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 	}
 
 	ac = skb_get_queue_mapping(p);
-	if (SCB_WME(scb) && qos && wlc->edcf_txop[ac]) {
+	if ((scb->flags & SCB_WMECAP) && qos && wlc->edcf_txop[ac]) {
 		uint frag_dur, dur, dur_fallback;
 
 		/* WME: Update TXOP threshold */
-		if ((!(tx_info->flags & IEEE80211_TX_CTL_AMPDU)) && (frag == 0)) {
+		if (!(tx_info->flags & IEEE80211_TX_CTL_AMPDU) && frag == 0) {
 			frag_dur =
 			    brcms_c_calc_frame_time(wlc, rspec[0],
 					preamble_type[0], phylen);
@@ -4220,40 +7465,44 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 			}
 			/* NEED to set TxFesTimeNormal (hard) */
 			txh->TxFesTimeNormal = cpu_to_le16((u16) dur);
-			/* NEED to set fallback rate version of TxFesTimeNormal (hard) */
+			/*
+			 * NEED to set fallback rate version of
+			 * TxFesTimeNormal (hard)
+			 */
 			txh->TxFesTimeFallback =
 				cpu_to_le16((u16) dur_fallback);
 
-			/* update txop byte threshold (txop minus intraframe overhead) */
+			/*
+			 * update txop byte threshold (txop minus intraframe
+			 * overhead)
+			 */
 			if (wlc->edcf_txop[ac] >= (dur - frag_dur)) {
-				{
-					uint newfragthresh;
-
+				uint newfragthresh;
+
+				newfragthresh =
+				    brcms_c_calc_frame_len(wlc,
+					rspec[0], preamble_type[0],
+					(wlc->edcf_txop[ac] -
+						(dur - frag_dur)));
+				/* range bound the fragthreshold */
+				if (newfragthresh < DOT11_MIN_FRAG_LEN)
 					newfragthresh =
-					    brcms_c_calc_frame_len(wlc,
-						rspec[0], preamble_type[0],
-						(wlc->edcf_txop[ac] -
-							(dur - frag_dur)));
-					/* range bound the fragthreshold */
-					if (newfragthresh < DOT11_MIN_FRAG_LEN)
-						newfragthresh =
-						    DOT11_MIN_FRAG_LEN;
-					else if (newfragthresh >
-						 wlc->usr_fragthresh)
-						newfragthresh =
-						    wlc->usr_fragthresh;
-					/* update the fragthresh and do txc update */
-					if (wlc->fragthresh[queue] !=
-					    (u16) newfragthresh) {
-						wlc->fragthresh[queue] =
-						    (u16) newfragthresh;
-					}
-				}
-			} else
+					    DOT11_MIN_FRAG_LEN;
+				else if (newfragthresh >
+					 wlc->usr_fragthresh)
+					newfragthresh =
+					    wlc->usr_fragthresh;
+				/* update the fragthresh and do txc update */
+				if (wlc->fragthresh[queue] !=
+				    (u16) newfragthresh)
+					wlc->fragthresh[queue] =
+					    (u16) newfragthresh;
+			} else {
 				wiphy_err(wlc->wiphy, "wl%d: %s txop invalid "
 					  "for rate %d\n",
 					  wlc->pub->unit, fifo_names[queue],
-					  RSPEC2RATE(rspec[0]));
+					  rspec2rate(rspec[0]));
+			}
 
 			if (dur > wlc->edcf_txop[ac])
 				wiphy_err(wlc->wiphy, "wl%d: %s: %s txop "
@@ -4268,225 +7517,191 @@ brcms_c_d11hdrs_mac80211(struct brcms_c_info *wlc, struct ieee80211_hw *hw,
 	return 0;
 }
 
-void brcms_c_tbtt(struct brcms_c_info *wlc)
+void brcms_c_sendpkt_mac80211(struct brcms_c_info *wlc, struct sk_buff *sdu,
+			      struct ieee80211_hw *hw)
 {
-	struct brcms_bss_cfg *cfg = wlc->cfg;
-
-	if (!cfg->BSS) {
-		/* DirFrmQ is now valid...defer setting until end of ATIM window */
-		wlc->qvalid |= MCMD_DIRFRMQVAL;
-	}
-}
+	u8 prio;
+	uint fifo;
+	struct scb *scb = &wlc->pri_scb;
+	struct ieee80211_hdr *d11_header = (struct ieee80211_hdr *)(sdu->data);
 
-static void brcms_c_war16165(struct brcms_c_info *wlc, bool tx)
-{
-	if (tx) {
-		/* the post-increment is used in STAY_AWAKE macro */
-		if (wlc->txpend16165war++ == 0)
-			brcms_c_set_ps_ctrl(wlc);
-	} else {
-		wlc->txpend16165war--;
-		if (wlc->txpend16165war == 0)
-			brcms_c_set_ps_ctrl(wlc);
-	}
+	/*
+	 * 802.11 standard requires management traffic
+	 * to go at highest priority
+	 */
+	prio = ieee80211_is_data(d11_header->frame_control) ? sdu->priority :
+		MAXPRIO;
+	fifo = prio2fifo[prio];
+	if (brcms_c_d11hdrs_mac80211(wlc, hw, sdu, scb, 0, 1, fifo, 0))
+		return;
+	brcms_c_txq_enq(wlc, scb, sdu, BRCMS_PRIO_TO_PREC(prio));
+	brcms_c_send_q(wlc);
 }
 
-/* process an individual struct tx_status */
-bool
-brcms_c_dotxstatus(struct brcms_c_info *wlc, struct tx_status *txs, u32 frm_tx2)
+void brcms_c_send_q(struct brcms_c_info *wlc)
 {
-	struct sk_buff *p;
-	uint queue;
-	struct d11txh *txh;
-	struct scb *scb = NULL;
-	bool free_pdu;
-	int tx_rts, tx_frame_count, tx_rts_count;
-	uint totlen, supr_status;
-	bool lastframe;
-	struct ieee80211_hdr *h;
-	u16 mcl;
+	struct sk_buff *pkt[DOT11_MAXNUMFRAGS];
+	int prec;
+	u16 prec_map;
+	int err = 0, i, count;
+	uint fifo;
+	struct brcms_txq_info *qi = wlc->pkt_queue;
+	struct pktq *q = &qi->q;
 	struct ieee80211_tx_info *tx_info;
-	struct ieee80211_tx_rate *txrate;
-	int i;
 
-	(void)(frm_tx2);	/* Compiler reference to avoid unused variable warning */
+	prec_map = wlc->tx_prec_map;
 
-	/* discard intermediate indications for ucode with one legitimate case:
-	 *   e.g. if "useRTS" is set. ucode did a successful rts/cts exchange, but the subsequent
-	 *   tx of DATA failed. so it will start rts/cts from the beginning (resetting the rts
-	 *   transmission count)
+	/* Send all the enq'd pkts that we can.
+	 * Dequeue packets with precedence with empty HW fifo only
 	 */
-	if (!(txs->status & TX_STATUS_AMPDU)
-	    && (txs->status & TX_STATUS_INTERMEDIATE)) {
-		wiphy_err(wlc->wiphy, "%s: INTERMEDIATE but not AMPDU\n",
-			  __func__);
-		return false;
-	}
+	while (prec_map && (pkt[0] = brcmu_pktq_mdeq(q, prec_map, &prec))) {
+		tx_info = IEEE80211_SKB_CB(pkt[0]);
+		if (tx_info->flags & IEEE80211_TX_CTL_AMPDU) {
+			err = brcms_c_sendampdu(wlc->ampdu, qi, pkt, prec);
+		} else {
+			count = 1;
+			err = brcms_c_prep_pdu(wlc, pkt[0], &fifo);
+			if (!err) {
+				for (i = 0; i < count; i++)
+					brcms_c_txfifo(wlc, fifo, pkt[i], true,
+						       1);
+			}
+		}
 
-	queue = txs->frameid & TXFID_QUEUE_MASK;
-	if (queue >= NFIFO) {
-		p = NULL;
-		goto fatal;
+		if (err == -EBUSY) {
+			brcmu_pktq_penq_head(q, prec, pkt[0]);
+			/*
+			 * If send failed due to any other reason than a
+			 * change in HW FIFO condition, quit. Otherwise,
+			 * read the new prec_map!
+			 */
+			if (prec_map == wlc->tx_prec_map)
+				break;
+			prec_map = wlc->tx_prec_map;
+		}
 	}
+}
 
-	p = GETNEXTTXP(wlc, queue);
-	if (BRCMS_WAR16165(wlc))
-		brcms_c_war16165(wlc, false);
-	if (p == NULL)
-		goto fatal;
+void
+brcms_c_txfifo(struct brcms_c_info *wlc, uint fifo, struct sk_buff *p,
+	       bool commit, s8 txpktpend)
+{
+	u16 frameid = INVALIDFID;
+	struct d11txh *txh;
 
 	txh = (struct d11txh *) (p->data);
-	mcl = le16_to_cpu(txh->MacTxControlLow);
 
-	if (txs->phyerr) {
-		if (WL_ERROR_ON()) {
-			wiphy_err(wlc->wiphy, "phyerr 0x%x, rate 0x%x\n",
-				  txs->phyerr, txh->MainRates);
-			brcms_c_print_txdesc(txh);
-		}
-		brcms_c_print_txstatus(txs);
-	}
-
-	if (txs->frameid != cpu_to_le16(txh->TxFrameID))
-		goto fatal;
-	tx_info = IEEE80211_SKB_CB(p);
-	h = (struct ieee80211_hdr *)((u8 *) (txh + 1) + D11_PHY_HDR_LEN);
-
-	if (tx_info->control.sta)
-		scb = (struct scb *)tx_info->control.sta->drv_priv;
+	/* When a BC/MC frame is being committed to the BCMC fifo
+	 * via DMA (NOT PIO), update ucode or BSS info as appropriate.
+	 */
+	if (fifo == TX_BCMC_FIFO)
+		frameid = le16_to_cpu(txh->TxFrameID);
 
-	if (tx_info->flags & IEEE80211_TX_CTL_AMPDU) {
-		brcms_c_ampdu_dotxstatus(wlc->ampdu, scb, p, txs);
-		return false;
+	/*
+	 * Bump up pending count for if not using rpc. If rpc is
+	 * used, this will be handled in brcms_b_txfifo()
+	 */
+	if (commit) {
+		wlc->core->txpktpend[fifo] += txpktpend;
+		BCMMSG(wlc->wiphy, "pktpend inc %d to %d\n",
+			 txpktpend, wlc->core->txpktpend[fifo]);
 	}
 
-	supr_status = txs->status & TX_STATUS_SUPR_MASK;
-	if (supr_status == TX_STATUS_SUPR_BADCH)
-		BCMMSG(wlc->wiphy,
-		       "%s: Pkt tx suppressed, possibly channel %d\n",
-		       __func__, CHSPEC_CHANNEL(wlc->default_bss->chanspec));
+	/* Commit BCMC sequence number in the SHM frame ID location */
+	if (frameid != INVALIDFID) {
+		/*
+		 * To inform the ucode of the last mcast frame posted
+		 * so that it can clear moredata bit
+		 */
+		brcms_b_write_shm(wlc->hw, M_BCMC_FID, frameid);
+	}
 
-	tx_rts = cpu_to_le16(txh->MacTxControlLow) & TXC_SENDRTS;
-	tx_frame_count =
-	    (txs->status & TX_STATUS_FRM_RTX_MASK) >> TX_STATUS_FRM_RTX_SHIFT;
-	tx_rts_count =
-	    (txs->status & TX_STATUS_RTS_RTX_MASK) >> TX_STATUS_RTS_RTX_SHIFT;
+	if (dma_txfast(wlc->hw->di[fifo], p, commit) < 0)
+		wiphy_err(wlc->wiphy, "txfifo: fatal, toss frames !!!\n");
+}
 
-	lastframe = !ieee80211_has_morefrags(h->frame_control);
+u32
+brcms_c_rspec_to_rts_rspec(struct brcms_c_info *wlc, u32 rspec,
+			   bool use_rspec, u16 mimo_ctlchbw)
+{
+	u32 rts_rspec = 0;
 
-	if (!lastframe) {
-		wiphy_err(wlc->wiphy, "Not last frame!\n");
-	} else {
-		/*
-		 * Set information to be consumed by Minstrel ht.
-		 *
-		 * The "fallback limit" is the number of tx attempts a given
-		 * MPDU is sent at the "primary" rate. Tx attempts beyond that
-		 * limit are sent at the "secondary" rate.
-		 * A 'short frame' does not exceed RTS treshold.
+	if (use_rspec)
+		/* use frame rate as rts rate */
+		rts_rspec = rspec;
+	else if (wlc->band->gmode && wlc->protection->_g && !is_cck_rate(rspec))
+		/* Use 11Mbps as the g protection RTS target rate and fallback.
+		 * Use the brcms_basic_rate() lookup to find the best basic rate
+		 * under the target in case 11 Mbps is not Basic.
+		 * 6 and 9 Mbps are not usually selected by rate selection, but
+		 * even if the OFDM rate we are protecting is 6 or 9 Mbps, 11
+		 * is more robust.
 		 */
-		u16 sfbl,	/* Short Frame Rate Fallback Limit */
-		    lfbl,	/* Long Frame Rate Fallback Limit */
-		    fbl;
+		rts_rspec = brcms_basic_rate(wlc, BRCM_RATE_11M);
+	else
+		/* calculate RTS rate and fallback rate based on the frame rate
+		 * RTS must be sent at a basic rate since it is a
+		 * control frame, sec 9.6 of 802.11 spec
+		 */
+		rts_rspec = brcms_basic_rate(wlc, rspec);
 
-		if (queue < AC_COUNT) {
-			sfbl = BRCMS_WME_RETRY_SFB_GET(wlc, wme_fifo2ac[queue]);
-			lfbl = BRCMS_WME_RETRY_LFB_GET(wlc, wme_fifo2ac[queue]);
-		} else {
-			sfbl = wlc->SFBL;
-			lfbl = wlc->LFBL;
-		}
+	if (BRCMS_PHY_11N_CAP(wlc->band)) {
+		/* set rts txbw to correct side band */
+		rts_rspec &= ~RSPEC_BW_MASK;
 
-		txrate = tx_info->status.rates;
-		if (txrate[0].flags & IEEE80211_TX_RC_USE_RTS_CTS)
-			fbl = lfbl;
+		/*
+		 * if rspec/rspec_fallback is 40MHz, then send RTS on both
+		 * 20MHz channel (DUP), otherwise send RTS on control channel
+		 */
+		if (rspec_is40mhz(rspec) && !is_cck_rate(rts_rspec))
+			rts_rspec |= (PHY_TXC1_BW_40MHZ_DUP << RSPEC_BW_SHIFT);
 		else
-			fbl = sfbl;
-
-		ieee80211_tx_info_clear_status(tx_info);
-
-		if ((tx_frame_count > fbl) && (txrate[1].idx >= 0)) {
-			/* rate selection requested a fallback rate and we used it */
-			txrate[0].count = fbl;
-			txrate[1].count = tx_frame_count - fbl;
-		} else {
-			/* rate selection did not request fallback rate, or we didn't need it */
-			txrate[0].count = tx_frame_count;
-			/* rc80211_minstrel.c:minstrel_tx_status() expects unused rates to be marked with idx = -1 */
-			txrate[1].idx = -1;
-			txrate[1].count = 0;
-		}
+			rts_rspec |= (mimo_ctlchbw << RSPEC_BW_SHIFT);
 
-		/* clear the rest of the rates */
-		for (i = 2; i < IEEE80211_TX_MAX_RATES; i++) {
-			txrate[i].idx = -1;
-			txrate[i].count = 0;
+		/* pick siso/cdd as default for ofdm */
+		if (is_ofdm_rate(rts_rspec)) {
+			rts_rspec &= ~RSPEC_STF_MASK;
+			rts_rspec |= (wlc->stf->ss_opmode << RSPEC_STF_SHIFT);
 		}
-
-		if (txs->status & TX_STATUS_ACK_RCV)
-			tx_info->flags |= IEEE80211_TX_STAT_ACK;
-	}
-
-	totlen = brcmu_pkttotlen(p);
-	free_pdu = true;
-
-	brcms_c_txfifo_complete(wlc, queue, 1);
-
-	if (lastframe) {
-		p->next = NULL;
-		p->prev = NULL;
-		/* remove PLCP & Broadcom tx descriptor header */
-		skb_pull(p, D11_PHY_HDR_LEN);
-		skb_pull(p, D11_TXH_LEN);
-		ieee80211_tx_status_irqsafe(wlc->pub->ieee_hw, p);
-	} else {
-		wiphy_err(wlc->wiphy, "%s: Not last frame => not calling "
-			  "tx_status\n", __func__);
 	}
-
-	return false;
-
- fatal:
-	if (p)
-		brcmu_pkt_buf_free_skb(p);
-
-	return true;
-
+	return rts_rspec;
 }
 
 void
 brcms_c_txfifo_complete(struct brcms_c_info *wlc, uint fifo, s8 txpktpend)
 {
-	TXPKTPENDDEC(wlc, fifo, txpktpend);
+	wlc->core->txpktpend[fifo] -= txpktpend;
 	BCMMSG(wlc->wiphy, "pktpend dec %d to %d\n", txpktpend,
-		TXPKTPENDGET(wlc, fifo));
+	       wlc->core->txpktpend[fifo]);
 
 	/* There is more room; mark precedences related to this FIFO sendable */
-	BRCMS_TX_FIFO_ENAB(wlc, fifo);
-
-	/* Clear MHF2_TXBCMC_NOW flag if BCMC fifo has drained */
-	if (AP_ENAB(wlc->pub) &&
-	    !TXPKTPENDGET(wlc, TX_BCMC_FIFO)) {
-		brcms_c_mhf(wlc, MHF2, MHF2_TXBCMC_NOW, 0, BRCM_BAND_AUTO);
-	}
+	wlc->tx_prec_map |= wlc->fifo2prec_map[fifo];
 
 	/* figure out which bsscfg is being worked on... */
 }
 
 /* Update beacon listen interval in shared memory */
-void brcms_c_bcn_li_upd(struct brcms_c_info *wlc)
+static void brcms_c_bcn_li_upd(struct brcms_c_info *wlc)
 {
-	if (AP_ENAB(wlc->pub))
-		return;
-
 	/* wake up every DTIM is the default */
 	if (wlc->bcn_li_dtim == 1)
-		brcms_c_write_shm(wlc, M_BCN_LI, 0);
+		brcms_b_write_shm(wlc->hw, M_BCN_LI, 0);
 	else
-		brcms_c_write_shm(wlc, M_BCN_LI,
+		brcms_b_write_shm(wlc->hw, M_BCN_LI,
 			      (wlc->bcn_li_dtim << 8) | wlc->bcn_li_bcn);
 }
 
+static void
+brcms_b_read_tsf(struct brcms_hardware *wlc_hw, u32 *tsf_l_ptr,
+		  u32 *tsf_h_ptr)
+{
+	struct d11regs __iomem *regs = wlc_hw->regs;
+
+	/* read the tsf timer low, then high to get an atomic read */
+	*tsf_l_ptr = R_REG(&regs->tsf_timerlow);
+	*tsf_h_ptr = R_REG(&regs->tsf_timerhigh);
+}
+
 /*
  * recover 64bit TSF value from the 16bit TSF value in the rx header
  * given the assumption that the TSF passed in header is within 65ms
@@ -4502,7 +7717,7 @@ void brcms_c_bcn_li_upd(struct brcms_c_info *wlc)
  * are used. Finally, the tsf_h is read from the tsf register.
  */
 static u64 brcms_c_recover_tsf64(struct brcms_c_info *wlc,
-				 struct brcms_d11rxhdr *rxh)
+				 struct d11rxhdr *rxh)
 {
 	u32 tsf_h, tsf_l;
 	u16 rx_tsf_0_15, rx_tsf_16_31;
@@ -4510,7 +7725,7 @@ static u64 brcms_c_recover_tsf64(struct brcms_c_info *wlc,
 	brcms_b_read_tsf(wlc->hw, &tsf_l, &tsf_h);
 
 	rx_tsf_16_31 = (u16)(tsf_l >> 16);
-	rx_tsf_0_15 = rxh->rxhdr.RxTSFTime;
+	rx_tsf_0_15 = rxh->RxTSFTime;
 
 	/*
 	 * a greater tsf time indicates the low 16 bits of
@@ -4530,14 +7745,13 @@ prep_mac80211_status(struct brcms_c_info *wlc, struct d11rxhdr *rxh,
 		     struct sk_buff *p,
 		     struct ieee80211_rx_status *rx_status)
 {
-	struct brcms_d11rxhdr *wlc_rxh = (struct brcms_d11rxhdr *) rxh;
 	int preamble;
 	int channel;
-	ratespec_t rspec;
+	u32 rspec;
 	unsigned char *plcp;
 
 	/* fill in TSF and flag its presence */
-	rx_status->mactime = brcms_c_recover_tsf64(wlc, wlc_rxh);
+	rx_status->mactime = brcms_c_recover_tsf64(wlc, rxh);
 	rx_status->flag |= RX_FLAG_MACTIME_MPDU;
 
 	channel = BRCMS_CHAN_CHANNEL(rxh->RxChan);
@@ -4552,22 +7766,23 @@ prep_mac80211_status(struct brcms_c_info *wlc, struct d11rxhdr *rxh,
 		rx_status->freq = ieee80211_dsss_chan_to_freq(channel);
 	}
 
-	rx_status->signal = wlc_rxh->rssi;	/* signal */
+	rx_status->signal = wlc_phy_rssi_compute(wlc->hw->band->pi, rxh);
 
 	/* noise */
 	/* qual */
-	rx_status->antenna = (rxh->PhyRxStatus_0 & PRXS0_RXANT_UPSUBBAND) ? 1 : 0;	/* ant */
+	rx_status->antenna =
+		(rxh->PhyRxStatus_0 & PRXS0_RXANT_UPSUBBAND) ? 1 : 0;
 
 	plcp = p->data;
 
 	rspec = brcms_c_compute_rspec(rxh, plcp);
-	if (IS_MCS(rspec)) {
+	if (is_mcs_rate(rspec)) {
 		rx_status->rate_idx = rspec & RSPEC_RATE_MASK;
 		rx_status->flag |= RX_FLAG_HT;
-		if (RSPEC_IS40MHZ(rspec))
+		if (rspec_is40mhz(rspec))
 			rx_status->flag |= RX_FLAG_40MHZ;
 	} else {
-		switch (RSPEC2RATE(rspec)) {
+		switch (rspec2rate(rspec)) {
 		case BRCM_RATE_1M:
 			rx_status->rate_idx = 0;
 			break;
@@ -4608,12 +7823,20 @@ prep_mac80211_status(struct brcms_c_info *wlc, struct d11rxhdr *rxh,
 			wiphy_err(wlc->wiphy, "%s: Unknown rate\n", __func__);
 		}
 
+		/*
+		 * For 5GHz, we should decrease the index as it is
+		 * a subset of the 2.4G rates. See bitrates field
+		 * of brcms_band_5GHz_nphy (in mac80211_if.c).
+		 */
+		if (rx_status->band == IEEE80211_BAND_5GHZ)
+			rx_status->rate_idx -= BRCMS_LEGACY_5G_RATE_OFFSET;
+
 		/* Determine short preamble and rate_idx */
 		preamble = 0;
-		if (IS_CCK(rspec)) {
+		if (is_cck_rate(rspec)) {
 			if (rxh->PhyRxStatus_0 & PRXS0_SHORTH)
 				rx_status->flag |= RX_FLAG_SHORTPRE;
-		} else if (IS_OFDM(rspec)) {
+		} else if (is_ofdm_rate(rspec)) {
 			rx_status->flag |= RX_FLAG_SHORTPRE;
 		} else {
 			wiphy_err(wlc->wiphy, "%s: Unknown modulation\n",
@@ -4621,7 +7844,7 @@ prep_mac80211_status(struct brcms_c_info *wlc, struct d11rxhdr *rxh,
 		}
 	}
 
-	if (PLCP3_ISSGI(plcp[3]))
+	if (plcp3_issgi(plcp[3]))
 		rx_status->flag |= RX_FLAG_SHORT_GI;
 
 	if (rxh->RxStatus1 & RXS_DECERR) {
@@ -4653,102 +7876,6 @@ brcms_c_recvctl(struct brcms_c_info *wlc, struct d11rxhdr *rxh,
 
 	memcpy(IEEE80211_SKB_RXCB(p), &rx_status, sizeof(rx_status));
 	ieee80211_rx_irqsafe(wlc->pub->ieee_hw, p);
-	return;
-}
-
-/* Process received frames */
-/*
- * Return true if more frames need to be processed. false otherwise.
- * Param 'bound' indicates max. # frames to process before break out.
- */
-void brcms_c_recv(struct brcms_c_info *wlc, struct sk_buff *p)
-{
-	struct d11rxhdr *rxh;
-	struct ieee80211_hdr *h;
-	uint len;
-	bool is_amsdu;
-
-	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
-
-	/* frame starts with rxhdr */
-	rxh = (struct d11rxhdr *) (p->data);
-
-	/* strip off rxhdr */
-	skb_pull(p, BRCMS_HWRXOFF);
-
-	/* fixup rx header endianness */
-	rxh->RxFrameSize = le16_to_cpu(rxh->RxFrameSize);
-	rxh->PhyRxStatus_0 = le16_to_cpu(rxh->PhyRxStatus_0);
-	rxh->PhyRxStatus_1 = le16_to_cpu(rxh->PhyRxStatus_1);
-	rxh->PhyRxStatus_2 = le16_to_cpu(rxh->PhyRxStatus_2);
-	rxh->PhyRxStatus_3 = le16_to_cpu(rxh->PhyRxStatus_3);
-	rxh->PhyRxStatus_4 = le16_to_cpu(rxh->PhyRxStatus_4);
-	rxh->PhyRxStatus_5 = le16_to_cpu(rxh->PhyRxStatus_5);
-	rxh->RxStatus1 = le16_to_cpu(rxh->RxStatus1);
-	rxh->RxStatus2 = le16_to_cpu(rxh->RxStatus2);
-	rxh->RxTSFTime = le16_to_cpu(rxh->RxTSFTime);
-	rxh->RxChan = le16_to_cpu(rxh->RxChan);
-
-	/* MAC inserts 2 pad bytes for a4 headers or QoS or A-MSDU subframes */
-	if (rxh->RxStatus1 & RXS_PBPRES) {
-		if (p->len < 2) {
-			wiphy_err(wlc->wiphy, "wl%d: recv: rcvd runt of "
-				  "len %d\n", wlc->pub->unit, p->len);
-			goto toss;
-		}
-		skb_pull(p, 2);
-	}
-
-	h = (struct ieee80211_hdr *)(p->data + D11_PHY_HDR_LEN);
-	len = p->len;
-
-	if (rxh->RxStatus1 & RXS_FCSERR) {
-		if (wlc->pub->mac80211_state & MAC80211_PROMISC_BCNS) {
-			wiphy_err(wlc->wiphy, "FCSERR while scanning******* -"
-				  " tossing\n");
-			goto toss;
-		} else {
-			wiphy_err(wlc->wiphy, "RCSERR!!!\n");
-			goto toss;
-		}
-	}
-
-	/* check received pkt has at least frame control field */
-	if (len < D11_PHY_HDR_LEN + sizeof(h->frame_control)) {
-		goto toss;
-	}
-
-	is_amsdu = rxh->RxStatus2 & RXS_AMSDU_MASK;
-
-	/* explicitly test bad src address to avoid sending bad deauth */
-	if (!is_amsdu) {
-		/* CTS and ACK CTL frames are w/o a2 */
-
-		if (ieee80211_is_data(h->frame_control) ||
-		    ieee80211_is_mgmt(h->frame_control)) {
-			if ((is_zero_ether_addr(h->addr2) ||
-			     is_multicast_ether_addr(h->addr2))) {
-				wiphy_err(wlc->wiphy, "wl%d: %s: dropping a "
-					  "frame with invalid src mac address,"
-					  " a2: %pM\n",
-					 wlc->pub->unit, __func__, h->addr2);
-				goto toss;
-			}
-		}
-	}
-
-	/* due to sheer numbers, toss out probe reqs for now */
-	if (ieee80211_is_probe_req(h->frame_control))
-		goto toss;
-
-	if (is_amsdu)
-		goto toss;
-
-	brcms_c_recvctl(wlc, rxh, p);
-	return;
-
- toss:
-	brcmu_pkt_buf_free_skb(p);
 }
 
 /* calculate frame duration for Mixed-mode L-SIG spoofing, return
@@ -4758,76 +7885,28 @@ void brcms_c_recv(struct brcms_c_info *wlc, struct sk_buff *p)
  *   len = 3(nsyms + nstream + 3) - 3
  */
 u16
-brcms_c_calc_lsig_len(struct brcms_c_info *wlc, ratespec_t ratespec,
+brcms_c_calc_lsig_len(struct brcms_c_info *wlc, u32 ratespec,
 		      uint mac_len)
 {
 	uint nsyms, len = 0, kNdps;
 
 	BCMMSG(wlc->wiphy, "wl%d: rate %d, len%d\n",
-		 wlc->pub->unit, RSPEC2RATE(ratespec), mac_len);
-
-	if (IS_MCS(ratespec)) {
-		uint mcs = ratespec & RSPEC_RATE_MASK;
-		/* MCS_TXS(mcs) returns num tx streams - 1 */
-		int tot_streams = (MCS_TXS(mcs) + 1) + RSPEC_STC(ratespec);
-
-		/* the payload duration calculation matches that of regular ofdm */
-		/* 1000Ndbps = kbps * 4 */
-		kNdps =
-		    MCS_RATE(mcs, RSPEC_IS40MHZ(ratespec),
-			     RSPEC_ISSGI(ratespec)) * 4;
-
-		if (RSPEC_STC(ratespec) == 0)
-			/* NSyms = CEILING((SERVICE + 8*NBytes + TAIL) / Ndbps) */
-			nsyms =
-			    CEIL((APHY_SERVICE_NBITS + 8 * mac_len +
-				  APHY_TAIL_NBITS) * 1000, kNdps);
-		else
-			/* STBC needs to have even number of symbols */
-			nsyms =
-			    2 *
-			    CEIL((APHY_SERVICE_NBITS + 8 * mac_len +
-				  APHY_TAIL_NBITS) * 1000, 2 * kNdps);
-
-		nsyms += (tot_streams + 3);	/* (+3) account for HT-SIG(2) and HT-STF(1) */
-		/* 3 bytes/symbol @ legacy 6Mbps rate */
-		len = (3 * nsyms) - 3;	/* (-3) excluding service bits and tail bits */
-	}
-
-	return (u16) len;
-}
-
-/* calculate frame duration of a given rate and length, return time in usec unit */
-uint
-brcms_c_calc_frame_time(struct brcms_c_info *wlc, ratespec_t ratespec,
-			u8 preamble_type, uint mac_len)
-{
-	uint nsyms, dur = 0, Ndps, kNdps;
-	uint rate = RSPEC2RATE(ratespec);
+		 wlc->pub->unit, rspec2rate(ratespec), mac_len);
 
-	if (rate == 0) {
-		wiphy_err(wlc->wiphy, "wl%d: WAR: using rate of 1 mbps\n",
-			  wlc->pub->unit);
-		rate = BRCM_RATE_1M;
-	}
-
-	BCMMSG(wlc->wiphy, "wl%d: rspec 0x%x, preamble_type %d, len%d\n",
-		 wlc->pub->unit, ratespec, preamble_type, mac_len);
-
-	if (IS_MCS(ratespec)) {
+	if (is_mcs_rate(ratespec)) {
 		uint mcs = ratespec & RSPEC_RATE_MASK;
-		int tot_streams = MCS_TXS(mcs) + RSPEC_STC(ratespec);
+		int tot_streams = (mcs_2_txstreams(mcs) + 1) +
+				  rspec_stc(ratespec);
 
-		dur = PREN_PREAMBLE + (tot_streams * PREN_PREAMBLE_EXT);
-		if (preamble_type == BRCMS_MM_PREAMBLE)
-			dur += PREN_MM_EXT;
+		/*
+		 * the payload duration calculation matches that
+		 * of regular ofdm
+		 */
 		/* 1000Ndbps = kbps * 4 */
-		kNdps =
-		    MCS_RATE(mcs, RSPEC_IS40MHZ(ratespec),
-			     RSPEC_ISSGI(ratespec)) * 4;
+		kNdps = mcs_2_rate(mcs, rspec_is40mhz(ratespec),
+				   rspec_issgi(ratespec)) * 4;
 
-		if (RSPEC_STC(ratespec) == 0)
-			/* NSyms = CEILING((SERVICE + 8*NBytes + TAIL) / Ndbps) */
+		if (rspec_stc(ratespec) == 0)
 			nsyms =
 			    CEIL((APHY_SERVICE_NBITS + 8 * mac_len +
 				  APHY_TAIL_NBITS) * 1000, kNdps);
@@ -4838,398 +7917,62 @@ brcms_c_calc_frame_time(struct brcms_c_info *wlc, ratespec_t ratespec,
 			    CEIL((APHY_SERVICE_NBITS + 8 * mac_len +
 				  APHY_TAIL_NBITS) * 1000, 2 * kNdps);
 
-		dur += APHY_SYMBOL_TIME * nsyms;
-		if (BAND_2G(wlc->band->bandtype))
-			dur += DOT11_OFDM_SIGNAL_EXTENSION;
-	} else if (IS_OFDM(rate)) {
-		dur = APHY_PREAMBLE_TIME;
-		dur += APHY_SIGNAL_TIME;
-		/* Ndbps = Mbps * 4 = rate(500Kbps) * 2 */
-		Ndps = rate * 2;
-		/* NSyms = CEILING((SERVICE + 8*NBytes + TAIL) / Ndbps) */
-		nsyms =
-		    CEIL((APHY_SERVICE_NBITS + 8 * mac_len + APHY_TAIL_NBITS),
-			 Ndps);
-		dur += APHY_SYMBOL_TIME * nsyms;
-		if (BAND_2G(wlc->band->bandtype))
-			dur += DOT11_OFDM_SIGNAL_EXTENSION;
-	} else {
-		/* calc # bits * 2 so factor of 2 in rate (1/2 mbps) will divide out */
-		mac_len = mac_len * 8 * 2;
-		/* calc ceiling of bits/rate = microseconds of air time */
-		dur = (mac_len + rate - 1) / rate;
-		if (preamble_type & BRCMS_SHORT_PREAMBLE)
-			dur += BPHY_PLCP_SHORT_TIME;
-		else
-			dur += BPHY_PLCP_TIME;
-	}
-	return dur;
-}
-
-/* The opposite of brcms_c_calc_frame_time */
-static uint
-brcms_c_calc_frame_len(struct brcms_c_info *wlc, ratespec_t ratespec,
-		   u8 preamble_type, uint dur)
-{
-	uint nsyms, mac_len, Ndps, kNdps;
-	uint rate = RSPEC2RATE(ratespec);
-
-	BCMMSG(wlc->wiphy, "wl%d: rspec 0x%x, preamble_type %d, dur %d\n",
-		 wlc->pub->unit, ratespec, preamble_type, dur);
-
-	if (IS_MCS(ratespec)) {
-		uint mcs = ratespec & RSPEC_RATE_MASK;
-		int tot_streams = MCS_TXS(mcs) + RSPEC_STC(ratespec);
-		dur -= PREN_PREAMBLE + (tot_streams * PREN_PREAMBLE_EXT);
-		/* payload calculation matches that of regular ofdm */
-		if (BAND_2G(wlc->band->bandtype))
-			dur -= DOT11_OFDM_SIGNAL_EXTENSION;
-		/* kNdbps = kbps * 4 */
-		kNdps =
-		    MCS_RATE(mcs, RSPEC_IS40MHZ(ratespec),
-			     RSPEC_ISSGI(ratespec)) * 4;
-		nsyms = dur / APHY_SYMBOL_TIME;
-		mac_len =
-		    ((nsyms * kNdps) -
-		     ((APHY_SERVICE_NBITS + APHY_TAIL_NBITS) * 1000)) / 8000;
-	} else if (IS_OFDM(ratespec)) {
-		dur -= APHY_PREAMBLE_TIME;
-		dur -= APHY_SIGNAL_TIME;
-		/* Ndbps = Mbps * 4 = rate(500Kbps) * 2 */
-		Ndps = rate * 2;
-		nsyms = dur / APHY_SYMBOL_TIME;
-		mac_len =
-		    ((nsyms * Ndps) -
-		     (APHY_SERVICE_NBITS + APHY_TAIL_NBITS)) / 8;
-	} else {
-		if (preamble_type & BRCMS_SHORT_PREAMBLE)
-			dur -= BPHY_PLCP_SHORT_TIME;
-		else
-			dur -= BPHY_PLCP_TIME;
-		mac_len = dur * rate;
-		/* divide out factor of 2 in rate (1/2 mbps) */
-		mac_len = mac_len / 8 / 2;
-	}
-	return mac_len;
-}
-
-static uint
-brcms_c_calc_ba_time(struct brcms_c_info *wlc, ratespec_t rspec,
-		     u8 preamble_type)
-{
-	BCMMSG(wlc->wiphy, "wl%d: rspec 0x%x, "
-		 "preamble_type %d\n", wlc->pub->unit, rspec, preamble_type);
-	/* Spec 9.6: ack rate is the highest rate in BSSBasicRateSet that is less than
-	 * or equal to the rate of the immediately previous frame in the FES
-	 */
-	rspec = BRCMS_BASIC_RATE(wlc, rspec);
-	/* BA len == 32 == 16(ctl hdr) + 4(ba len) + 8(bitmap) + 4(fcs) */
-	return brcms_c_calc_frame_time(wlc, rspec, preamble_type,
-				   (DOT11_BA_LEN + DOT11_BA_BITMAP_LEN +
-				    FCS_LEN));
-}
-
-static uint
-brcms_c_calc_ack_time(struct brcms_c_info *wlc, ratespec_t rspec,
-		      u8 preamble_type)
-{
-	uint dur = 0;
-
-	BCMMSG(wlc->wiphy, "wl%d: rspec 0x%x, preamble_type %d\n",
-		wlc->pub->unit, rspec, preamble_type);
-	/* Spec 9.6: ack rate is the highest rate in BSSBasicRateSet that is less than
-	 * or equal to the rate of the immediately previous frame in the FES
-	 */
-	rspec = BRCMS_BASIC_RATE(wlc, rspec);
-	/* ACK frame len == 14 == 2(fc) + 2(dur) + 6(ra) + 4(fcs) */
-	dur =
-	    brcms_c_calc_frame_time(wlc, rspec, preamble_type,
-				(DOT11_ACK_LEN + FCS_LEN));
-	return dur;
-}
-
-static uint
-brcms_c_calc_cts_time(struct brcms_c_info *wlc, ratespec_t rspec,
-		      u8 preamble_type)
-{
-	BCMMSG(wlc->wiphy, "wl%d: ratespec 0x%x, preamble_type %d\n",
-		wlc->pub->unit, rspec, preamble_type);
-	return brcms_c_calc_ack_time(wlc, rspec, preamble_type);
-}
-
-/* derive wlc->band->basic_rate[] table from 'rateset' */
-void brcms_c_rate_lookup_init(struct brcms_c_info *wlc, wlc_rateset_t *rateset)
-{
-	u8 rate;
-	u8 mandatory;
-	u8 cck_basic = 0;
-	u8 ofdm_basic = 0;
-	u8 *br = wlc->band->basic_rate;
-	uint i;
-
-	/* incoming rates are in 500kbps units as in 802.11 Supported Rates */
-	memset(br, 0, BRCM_MAXRATE + 1);
-
-	/* For each basic rate in the rates list, make an entry in the
-	 * best basic lookup.
-	 */
-	for (i = 0; i < rateset->count; i++) {
-		/* only make an entry for a basic rate */
-		if (!(rateset->rates[i] & BRCMS_RATE_FLAG))
-			continue;
-
-		/* mask off basic bit */
-		rate = (rateset->rates[i] & BRCMS_RATE_MASK);
-
-		if (rate > BRCM_MAXRATE) {
-			wiphy_err(wlc->wiphy, "brcms_c_rate_lookup_init: "
-				  "invalid rate 0x%X in rate set\n",
-				  rateset->rates[i]);
-			continue;
-		}
-
-		br[rate] = rate;
-	}
-
-	/* The rate lookup table now has non-zero entries for each
-	 * basic rate, equal to the basic rate: br[basicN] = basicN
-	 *
-	 * To look up the best basic rate corresponding to any
-	 * particular rate, code can use the basic_rate table
-	 * like this
-	 *
-	 * basic_rate = wlc->band->basic_rate[tx_rate]
-	 *
-	 * Make sure there is a best basic rate entry for
-	 * every rate by walking up the table from low rates
-	 * to high, filling in holes in the lookup table
-	 */
-
-	for (i = 0; i < wlc->band->hw_rateset.count; i++) {
-		rate = wlc->band->hw_rateset.rates[i];
-
-		if (br[rate] != 0) {
-			/* This rate is a basic rate.
-			 * Keep track of the best basic rate so far by
-			 * modulation type.
-			 */
-			if (IS_OFDM(rate))
-				ofdm_basic = rate;
-			else
-				cck_basic = rate;
-
-			continue;
-		}
-
-		/* This rate is not a basic rate so figure out the
-		 * best basic rate less than this rate and fill in
-		 * the hole in the table
-		 */
-
-		br[rate] = IS_OFDM(rate) ? ofdm_basic : cck_basic;
-
-		if (br[rate] != 0)
-			continue;
-
-		if (IS_OFDM(rate)) {
-			/* In 11g and 11a, the OFDM mandatory rates are 6, 12, and 24 Mbps */
-			if (rate >= BRCM_RATE_24M)
-				mandatory = BRCM_RATE_24M;
-			else if (rate >= BRCM_RATE_12M)
-				mandatory = BRCM_RATE_12M;
-			else
-				mandatory = BRCM_RATE_6M;
-		} else {
-			/* In 11b, all the CCK rates are mandatory 1 - 11 Mbps */
-			mandatory = rate;
-		}
-
-		br[rate] = mandatory;
-	}
-}
-
-static void brcms_c_write_rate_shm(struct brcms_c_info *wlc, u8 rate,
-				   u8 basic_rate)
-{
-	u8 phy_rate, index;
-	u8 basic_phy_rate, basic_index;
-	u16 dir_table, basic_table;
-	u16 basic_ptr;
-
-	/* Shared memory address for the table we are reading */
-	dir_table = IS_OFDM(basic_rate) ? M_RT_DIRMAP_A : M_RT_DIRMAP_B;
-
-	/* Shared memory address for the table we are writing */
-	basic_table = IS_OFDM(rate) ? M_RT_BBRSMAP_A : M_RT_BBRSMAP_B;
-
-	/*
-	 * for a given rate, the LS-nibble of the PLCP SIGNAL field is
-	 * the index into the rate table.
-	 */
-	phy_rate = rate_info[rate] & BRCMS_RATE_MASK;
-	basic_phy_rate = rate_info[basic_rate] & BRCMS_RATE_MASK;
-	index = phy_rate & 0xf;
-	basic_index = basic_phy_rate & 0xf;
-
-	/* Find the SHM pointer to the ACK rate entry by looking in the
-	 * Direct-map Table
-	 */
-	basic_ptr = brcms_c_read_shm(wlc, (dir_table + basic_index * 2));
-
-	/* Update the SHM BSS-basic-rate-set mapping table with the pointer
-	 * to the correct basic rate for the given incoming rate
-	 */
-	brcms_c_write_shm(wlc, (basic_table + index * 2), basic_ptr);
-}
-
-static const wlc_rateset_t *brcms_c_rateset_get_hwrs(struct brcms_c_info *wlc)
-{
-	const wlc_rateset_t *rs_dflt;
-
-	if (BRCMS_PHY_11N_CAP(wlc->band)) {
-		if (BAND_5G(wlc->band->bandtype))
-			rs_dflt = &ofdm_mimo_rates;
-		else
-			rs_dflt = &cck_ofdm_mimo_rates;
-	} else if (wlc->band->gmode)
-		rs_dflt = &cck_ofdm_rates;
-	else
-		rs_dflt = &cck_rates;
-
-	return rs_dflt;
-}
-
-void brcms_c_set_ratetable(struct brcms_c_info *wlc)
-{
-	const wlc_rateset_t *rs_dflt;
-	wlc_rateset_t rs;
-	u8 rate, basic_rate;
-	uint i;
-
-	rs_dflt = brcms_c_rateset_get_hwrs(wlc);
-
-	brcms_c_rateset_copy(rs_dflt, &rs);
-	brcms_c_rateset_mcs_upd(&rs, wlc->stf->txstreams);
-
-	/* walk the phy rate table and update SHM basic rate lookup table */
-	for (i = 0; i < rs.count; i++) {
-		rate = rs.rates[i] & BRCMS_RATE_MASK;
-
-		/* for a given rate BRCMS_BASIC_RATE returns the rate at
-		 * which a response ACK/CTS should be sent.
+		/* (+3) account for HT-SIG(2) and HT-STF(1) */
+		nsyms += (tot_streams + 3);
+		/*
+		 * 3 bytes/symbol @ legacy 6Mbps rate
+		 * (-3) excluding service bits and tail bits
 		 */
-		basic_rate = BRCMS_BASIC_RATE(wlc, rate);
-		if (basic_rate == 0) {
-			/* This should only happen if we are using a
-			 * restricted rateset.
-			 */
-			basic_rate = rs.rates[0] & BRCMS_RATE_MASK;
-		}
-
-		brcms_c_write_rate_shm(wlc, rate, basic_rate);
-	}
-}
-
-/*
- * Return true if the specified rate is supported by the specified band.
- * BRCM_BAND_AUTO indicates the current band.
- */
-bool brcms_c_valid_rate(struct brcms_c_info *wlc, ratespec_t rspec, int band,
-		    bool verbose)
-{
-	wlc_rateset_t *hw_rateset;
-	uint i;
-
-	if ((band == BRCM_BAND_AUTO) || (band == wlc->band->bandtype)) {
-		hw_rateset = &wlc->band->hw_rateset;
-	} else if (NBANDS(wlc) > 1) {
-		hw_rateset = &wlc->bandstate[OTHERBANDUNIT(wlc)]->hw_rateset;
-	} else {
-		/* other band specified and we are a single band device */
-		return false;
-	}
-
-	/* check if this is a mimo rate */
-	if (IS_MCS(rspec)) {
-		if (!VALID_MCS((rspec & RSPEC_RATE_MASK)))
-			goto error;
-
-		return isset(hw_rateset->mcs, (rspec & RSPEC_RATE_MASK));
-	}
-
-	for (i = 0; i < hw_rateset->count; i++)
-		if (hw_rateset->rates[i] == RSPEC2RATE(rspec))
-			return true;
- error:
-	if (verbose) {
-		wiphy_err(wlc->wiphy, "wl%d: valid_rate: rate spec 0x%x "
-			  "not in hw_rateset\n", wlc->pub->unit, rspec);
+		len = (3 * nsyms) - 3;
 	}
 
-	return false;
-}
-
-static void brcms_c_update_mimo_band_bwcap(struct brcms_c_info *wlc, u8 bwcap)
-{
-	uint i;
-	struct brcms_band *band;
-
-	for (i = 0; i < NBANDS(wlc); i++) {
-		if (IS_SINGLEBAND_5G(wlc->deviceid))
-			i = BAND_5G_INDEX;
-		band = wlc->bandstate[i];
-		if (band->bandtype == BRCM_BAND_5G) {
-			if ((bwcap == BRCMS_N_BW_40ALL)
-			    || (bwcap == BRCMS_N_BW_20IN2G_40IN5G))
-				band->mimo_cap_40 = true;
-			else
-				band->mimo_cap_40 = false;
-		} else {
-			if (bwcap == BRCMS_N_BW_40ALL)
-				band->mimo_cap_40 = true;
-			else
-				band->mimo_cap_40 = false;
-		}
-	}
+	return (u16) len;
 }
 
-void brcms_c_mod_prb_rsp_rate_table(struct brcms_c_info *wlc, uint frame_len)
+static void
+brcms_c_mod_prb_rsp_rate_table(struct brcms_c_info *wlc, uint frame_len)
 {
-	const wlc_rateset_t *rs_dflt;
-	wlc_rateset_t rs;
+	const struct brcms_c_rateset *rs_dflt;
+	struct brcms_c_rateset rs;
 	u8 rate;
 	u16 entry_ptr;
 	u8 plcp[D11_PHY_HDR_LEN];
 	u16 dur, sifs;
 	uint i;
 
-	sifs = SIFS(wlc->band);
+	sifs = get_sifs(wlc->band);
 
 	rs_dflt = brcms_c_rateset_get_hwrs(wlc);
 
 	brcms_c_rateset_copy(rs_dflt, &rs);
 	brcms_c_rateset_mcs_upd(&rs, wlc->stf->txstreams);
 
-	/* walk the phy rate table and update MAC core SHM basic rate table entries */
+	/*
+	 * walk the phy rate table and update MAC core SHM
+	 * basic rate table entries
+	 */
 	for (i = 0; i < rs.count; i++) {
 		rate = rs.rates[i] & BRCMS_RATE_MASK;
 
-		entry_ptr = brcms_c_rate_shm_offset(wlc, rate);
+		entry_ptr = brcms_b_rate_shm_offset(wlc->hw, rate);
 
 		/* Calculate the Probe Response PLCP for the given rate */
 		brcms_c_compute_plcp(wlc, rate, frame_len, plcp);
 
-		/* Calculate the duration of the Probe Response frame plus SIFS for the MAC */
+		/*
+		 * Calculate the duration of the Probe Response
+		 * frame plus SIFS for the MAC
+		 */
 		dur = (u16) brcms_c_calc_frame_time(wlc, rate,
 						BRCMS_LONG_PREAMBLE, frame_len);
 		dur += sifs;
 
 		/* Update the SHM Rate Table entry Probe Response values */
-		brcms_c_write_shm(wlc, entry_ptr + M_RT_PRS_PLCP_POS,
+		brcms_b_write_shm(wlc->hw, entry_ptr + M_RT_PRS_PLCP_POS,
 			      (u16) (plcp[0] + (plcp[1] << 8)));
-		brcms_c_write_shm(wlc, entry_ptr + M_RT_PRS_PLCP_POS + 2,
+		brcms_b_write_shm(wlc->hw, entry_ptr + M_RT_PRS_PLCP_POS + 2,
 			      (u16) (plcp[2] + (plcp[3] << 8)));
-		brcms_c_write_shm(wlc, entry_ptr + M_RT_PRS_DUR_POS, dur);
+		brcms_b_write_shm(wlc->hw, entry_ptr + M_RT_PRS_DUR_POS, dur);
 	}
 }
 
@@ -5242,12 +7985,12 @@ void brcms_c_mod_prb_rsp_rate_table(struct brcms_c_info *wlc, uint frame_len)
  *
  *      *len on input contains the max length of the packet available.
  *
- *	The *len value is set to the number of bytes in buf used, and starts with the PLCP
- *	and included up to, but not including, the 4 byte FCS.
+ *	The *len value is set to the number of bytes in buf used, and starts
+ *	with the PLCP and included up to, but not including, the 4 byte FCS.
  */
 static void
 brcms_c_bcn_prb_template(struct brcms_c_info *wlc, u16 type,
-			 ratespec_t bcn_rspec,
+			 u32 bcn_rspec,
 			 struct brcms_bss_cfg *cfg, u16 *buf, int *len)
 {
 	static const u8 ether_bcast[ETH_ALEN] = {255, 255, 255, 255, 255, 255};
@@ -5255,36 +7998,33 @@ brcms_c_bcn_prb_template(struct brcms_c_info *wlc, u16 type,
 	struct ieee80211_mgmt *h;
 	int hdr_len, body_len;
 
-	if (MBSS_BCN_ENAB(cfg) && type == IEEE80211_STYPE_BEACON)
-		hdr_len = DOT11_MAC_HDR_LEN;
-	else
-		hdr_len = D11_PHY_HDR_LEN + DOT11_MAC_HDR_LEN;
-	body_len = *len - hdr_len;	/* calc buffer size provided for frame body */
+	hdr_len = D11_PHY_HDR_LEN + DOT11_MAC_HDR_LEN;
 
-	*len = hdr_len + body_len;	/* return actual size */
+	/* calc buffer size provided for frame body */
+	body_len = *len - hdr_len;
+	/* return actual size */
+	*len = hdr_len + body_len;
 
 	/* format PHY and MAC headers */
 	memset((char *)buf, 0, hdr_len);
 
 	plcp = (struct cck_phy_hdr *) buf;
 
-	/* PLCP for Probe Response frames are filled in from core's rate table */
-	if (type == IEEE80211_STYPE_BEACON && !MBSS_BCN_ENAB(cfg)) {
+	/*
+	 * PLCP for Probe Response frames are filled in from
+	 * core's rate table
+	 */
+	if (type == IEEE80211_STYPE_BEACON)
 		/* fill in PLCP */
 		brcms_c_compute_plcp(wlc, bcn_rspec,
 				 (DOT11_MAC_HDR_LEN + body_len + FCS_LEN),
 				 (u8 *) plcp);
 
-	}
 	/* "Regular" and 16 MBSS but not for 4 MBSS */
 	/* Update the phytxctl for the beacon based on the rspec */
-	if (!SOFTBCN_ENAB(cfg))
-		brcms_c_beacon_phytxctl_txant_upd(wlc, bcn_rspec);
+	brcms_c_beacon_phytxctl_txant_upd(wlc, bcn_rspec);
 
-	if (MBSS_BCN_ENAB(cfg) && type == IEEE80211_STYPE_BEACON)
-		h = (struct ieee80211_mgmt *)&plcp[0];
-	else
-		h = (struct ieee80211_mgmt *)&plcp[1];
+	h = (struct ieee80211_mgmt *)&plcp[1];
 
 	/* fill in 802.11 header */
 	h->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT | type);
@@ -5297,80 +8037,28 @@ brcms_c_bcn_prb_template(struct brcms_c_info *wlc, u16 type,
 	memcpy(&h->bssid, &cfg->BSSID, ETH_ALEN);
 
 	/* SEQ filled in by MAC */
-
-	return;
 }
 
-int brcms_c_get_header_len()
+int brcms_c_get_header_len(void)
 {
 	return TXOFF;
 }
 
-/* Update a beacon for a particular BSS
- * For MBSS, this updates the software template and sets "latest" to the index of the
- * template updated.
- * Otherwise, it updates the hardware template.
- */
-void brcms_c_bss_update_beacon(struct brcms_c_info *wlc,
-			       struct brcms_bss_cfg *cfg)
-{
-	int len = BCN_TMPL_LEN;
-
-	/* Clear the soft intmask */
-	wlc->defmacintmask &= ~MI_BCNTPL;
-
-	if (!cfg->up) {		/* Only allow updates on an UP bss */
-		return;
-	}
-
-	/* Optimize:  Some of if/else could be combined */
-	if (!MBSS_BCN_ENAB(cfg) && HWBCN_ENAB(cfg)) {
-		/* Hardware beaconing for this config */
-		u16 bcn[BCN_TMPL_LEN / 2];
-		u32 both_valid = MCMD_BCN0VLD | MCMD_BCN1VLD;
-		d11regs_t *regs = wlc->regs;
-
-		/* Check if both templates are in use, if so sched. an interrupt
-		 *      that will call back into this routine
-		 */
-		if ((R_REG(&regs->maccommand) & both_valid) == both_valid) {
-			/* clear any previous status */
-			W_REG(&regs->macintstatus, MI_BCNTPL);
-		}
-		/* Check that after scheduling the interrupt both of the
-		 *      templates are still busy. if not clear the int. & remask
-		 */
-		if ((R_REG(&regs->maccommand) & both_valid) == both_valid) {
-			wlc->defmacintmask |= MI_BCNTPL;
-			return;
-		}
-
-		wlc->bcn_rspec =
-		    brcms_c_lowest_basic_rspec(wlc, &cfg->current_bss->rateset);
-		/* update the template and ucode shm */
-		brcms_c_bcn_prb_template(wlc, IEEE80211_STYPE_BEACON,
-				     wlc->bcn_rspec, cfg, bcn, &len);
-		brcms_c_write_hw_bcntemplates(wlc, bcn, len, false);
-	}
-}
-
 /*
  * Update all beacons for the system.
  */
 void brcms_c_update_beacon(struct brcms_c_info *wlc)
 {
-	int idx;
-	struct brcms_bss_cfg *bsscfg;
+	struct brcms_bss_cfg *bsscfg = wlc->bsscfg;
 
-	/* update AP or IBSS beacons */
-	FOREACH_BSS(wlc, idx, bsscfg) {
-		if (bsscfg->up && (BSSCFG_AP(bsscfg) || !bsscfg->BSS))
-			brcms_c_bss_update_beacon(wlc, bsscfg);
-	}
+	if (bsscfg->up && !bsscfg->BSS)
+		/* Clear the soft intmask */
+		wlc->defmacintmask &= ~MI_BCNTPL;
 }
 
 /* Write ssid into shared memory */
-void brcms_c_shm_ssid_upd(struct brcms_c_info *wlc, struct brcms_bss_cfg *cfg)
+static void
+brcms_c_shm_ssid_upd(struct brcms_c_info *wlc, struct brcms_bss_cfg *cfg)
 {
 	u8 *ssidptr = cfg->SSID;
 	u16 base = M_SSID;
@@ -5381,24 +8069,10 @@ void brcms_c_shm_ssid_upd(struct brcms_c_info *wlc, struct brcms_bss_cfg *cfg)
 	memcpy(ssidbuf, ssidptr, cfg->SSID_len);
 
 	brcms_c_copyto_shm(wlc, base, ssidbuf, IEEE80211_MAX_SSID_LEN);
-
-	if (!MBSS_BCN_ENAB(cfg))
-		brcms_c_write_shm(wlc, M_SSIDLEN, (u16) cfg->SSID_len);
-}
-
-void brcms_c_update_probe_resp(struct brcms_c_info *wlc, bool suspend)
-{
-	int idx;
-	struct brcms_bss_cfg *bsscfg;
-
-	/* update AP or IBSS probe responses */
-	FOREACH_BSS(wlc, idx, bsscfg) {
-		if (bsscfg->up && (BSSCFG_AP(bsscfg) || !bsscfg->BSS))
-			brcms_c_bss_update_probe_resp(wlc, bsscfg, suspend);
-	}
+	brcms_b_write_shm(wlc->hw, M_SSIDLEN, (u16) cfg->SSID_len);
 }
 
-void
+static void
 brcms_c_bss_update_probe_resp(struct brcms_c_info *wlc,
 			      struct brcms_bss_cfg *cfg,
 			      bool suspend)
@@ -5406,40 +8080,48 @@ brcms_c_bss_update_probe_resp(struct brcms_c_info *wlc,
 	u16 prb_resp[BCN_TMPL_LEN / 2];
 	int len = BCN_TMPL_LEN;
 
-	/* write the probe response to hardware, or save in the config structure */
-	if (!MBSS_PRB_ENAB(cfg)) {
+	/*
+	 * write the probe response to hardware, or save in
+	 * the config structure
+	 */
 
-		/* create the probe response template */
-		brcms_c_bcn_prb_template(wlc, IEEE80211_STYPE_PROBE_RESP, 0,
-					 cfg, prb_resp, &len);
+	/* create the probe response template */
+	brcms_c_bcn_prb_template(wlc, IEEE80211_STYPE_PROBE_RESP, 0,
+				 cfg, prb_resp, &len);
 
-		if (suspend)
-			brcms_c_suspend_mac_and_wait(wlc);
+	if (suspend)
+		brcms_c_suspend_mac_and_wait(wlc);
 
-		/* write the probe response into the template region */
-		brcms_b_write_template_ram(wlc->hw, T_PRS_TPL_BASE,
-					    (len + 3) & ~3, prb_resp);
+	/* write the probe response into the template region */
+	brcms_b_write_template_ram(wlc->hw, T_PRS_TPL_BASE,
+				    (len + 3) & ~3, prb_resp);
 
-		/* write the length of the probe response frame (+PLCP/-FCS) */
-		brcms_c_write_shm(wlc, M_PRB_RESP_FRM_LEN, (u16) len);
+	/* write the length of the probe response frame (+PLCP/-FCS) */
+	brcms_b_write_shm(wlc->hw, M_PRB_RESP_FRM_LEN, (u16) len);
 
-		/* write the SSID and SSID length */
-		brcms_c_shm_ssid_upd(wlc, cfg);
+	/* write the SSID and SSID length */
+	brcms_c_shm_ssid_upd(wlc, cfg);
 
-		/*
-		 * Write PLCP headers and durations for probe response frames at all rates.
-		 * Use the actual frame length covered by the PLCP header for the call to
-		 * brcms_c_mod_prb_rsp_rate_table() by subtracting the PLCP len
-		 * and adding the FCS.
-		 */
-		len += (-D11_PHY_HDR_LEN + FCS_LEN);
-		brcms_c_mod_prb_rsp_rate_table(wlc, (u16) len);
+	/*
+	 * Write PLCP headers and durations for probe response frames
+	 * at all rates. Use the actual frame length covered by the
+	 * PLCP header for the call to brcms_c_mod_prb_rsp_rate_table()
+	 * by subtracting the PLCP len and adding the FCS.
+	 */
+	len += (-D11_PHY_HDR_LEN + FCS_LEN);
+	brcms_c_mod_prb_rsp_rate_table(wlc, (u16) len);
 
-		if (suspend)
-			brcms_c_enable_mac(wlc);
-	} else {		/* Generating probe resp in sw; update local template */
-		/* error: No software probe response support without MBSS */
-	}
+	if (suspend)
+		brcms_c_enable_mac(wlc);
+}
+
+void brcms_c_update_probe_resp(struct brcms_c_info *wlc, bool suspend)
+{
+	struct brcms_bss_cfg *bsscfg = wlc->bsscfg;
+
+	/* update AP or IBSS probe responses */
+	if (bsscfg->up && !bsscfg->BSS)
+		brcms_c_bss_update_probe_resp(wlc, bsscfg, suspend);
 }
 
 /* prepares pdu for transmission. returns BCM error codes */
@@ -5462,641 +8144,632 @@ int brcms_c_prep_pdu(struct brcms_c_info *wlc, struct sk_buff *pdu, uint *fifop)
 	*fifop = fifo;
 
 	/* return if insufficient dma resources */
-	if (TXAVAIL(wlc, fifo) < MAX_DMA_SEGS) {
+	if (*wlc->core->txavail[fifo] < MAX_DMA_SEGS) {
 		/* Mark precedences related to this FIFO, unsendable */
-		BRCMS_TX_FIFO_CLEAR(wlc, fifo);
+		/* A fifo is full. Clear precedences related to that FIFO */
+		wlc->tx_prec_map &= ~(wlc->fifo2prec_map[fifo]);
 		return -EBUSY;
 	}
 	return 0;
 }
 
-/* init tx reported rate mechanism */
-void brcms_c_reprate_init(struct brcms_c_info *wlc)
+int brcms_b_xmtfifo_sz_get(struct brcms_hardware *wlc_hw, uint fifo,
+			   uint *blocks)
 {
-	int i;
-	struct brcms_bss_cfg *bsscfg;
+	if (fifo >= NFIFO)
+		return -EINVAL;
 
-	FOREACH_BSS(wlc, i, bsscfg) {
-		brcms_c_bsscfg_reprate_init(bsscfg);
-	}
-}
+	*blocks = wlc_hw->xmtfifo_sz[fifo];
 
-/* per bsscfg init tx reported rate mechanism */
-void brcms_c_bsscfg_reprate_init(struct brcms_bss_cfg *bsscfg)
-{
-	bsscfg->txrspecidx = 0;
-	memset((char *)bsscfg->txrspec, 0, sizeof(bsscfg->txrspec));
+	return 0;
 }
 
-void brcms_default_rateset(struct brcms_c_info *wlc, wlc_rateset_t *rs)
+void
+brcms_c_set_addrmatch(struct brcms_c_info *wlc, int match_reg_offset,
+		  const u8 *addr)
 {
-	brcms_c_rateset_default(rs, NULL, wlc->band->phytype,
-		wlc->band->bandtype, false, BRCMS_RATE_MASK_FULL,
-		(bool) N_ENAB(wlc->pub),
-		CHSPEC_WLC_BW(wlc->default_bss->chanspec),
-		wlc->stf->txstreams);
+	brcms_b_set_addrmatch(wlc->hw, match_reg_offset, addr);
+	if (match_reg_offset == RCM_BSSID_OFFSET)
+		memcpy(wlc->bsscfg->BSSID, addr, ETH_ALEN);
 }
 
-static void brcms_c_bss_default_init(struct brcms_c_info *wlc)
+/*
+ * Flag 'scan in progress' to withhold dynamic phy calibration
+ */
+void brcms_c_scan_start(struct brcms_c_info *wlc)
 {
-	chanspec_t chanspec;
-	struct brcms_band *band;
-	struct brcms_bss_info *bi = wlc->default_bss;
-
-	/* init default and target BSS with some sane initial values */
-	memset((char *)(bi), 0, sizeof(struct brcms_bss_info));
-	bi->beacon_period = BEACON_INTERVAL_DEFAULT;
-	bi->dtim_period = DTIM_INTERVAL_DEFAULT;
-
-	/* fill the default channel as the first valid channel
-	 * starting from the 2G channels
-	 */
-	chanspec = CH20MHZ_CHSPEC(1);
-	wlc->home_chanspec = bi->chanspec = chanspec;
-
-	/* find the band of our default channel */
-	band = wlc->band;
-	if (NBANDS(wlc) > 1 && band->bandunit != CHSPEC_BANDUNIT(chanspec))
-		band = wlc->bandstate[OTHERBANDUNIT(wlc)];
-
-	/* init bss rates to the band specific default rate set */
-	brcms_c_rateset_default(&bi->rateset, NULL, band->phytype,
-		band->bandtype, false, BRCMS_RATE_MASK_FULL,
-		(bool) N_ENAB(wlc->pub), CHSPEC_WLC_BW(chanspec),
-		wlc->stf->txstreams);
-
-	if (N_ENAB(wlc->pub))
-		bi->flags |= BRCMS_BSS_HT;
+	wlc_phy_hold_upd(wlc->band->pi, PHY_HOLD_FOR_SCAN, true);
 }
 
-static ratespec_t
-mac80211_wlc_set_nrate(struct brcms_c_info *wlc, struct brcms_band *cur_band,
-		       u32 int_val)
+void brcms_c_scan_stop(struct brcms_c_info *wlc)
 {
-	u8 stf = (int_val & NRATE_STF_MASK) >> NRATE_STF_SHIFT;
-	u8 rate = int_val & NRATE_RATE_MASK;
-	ratespec_t rspec;
-	bool ismcs = ((int_val & NRATE_MCS_INUSE) == NRATE_MCS_INUSE);
-	bool issgi = ((int_val & NRATE_SGI_MASK) >> NRATE_SGI_SHIFT);
-	bool override_mcs_only = ((int_val & NRATE_OVERRIDE_MCS_ONLY)
-				  == NRATE_OVERRIDE_MCS_ONLY);
-	int bcmerror = 0;
-
-	if (!ismcs) {
-		return (ratespec_t) rate;
-	}
-
-	/* validate the combination of rate/mcs/stf is allowed */
-	if (N_ENAB(wlc->pub) && ismcs) {
-		/* mcs only allowed when nmode */
-		if (stf > PHY_TXC1_MODE_SDM) {
-			wiphy_err(wlc->wiphy, "wl%d: %s: Invalid stf\n",
-				 BRCMS_UNIT(wlc), __func__);
-			bcmerror = -EINVAL;
-			goto done;
-		}
-
-		/* mcs 32 is a special case, DUP mode 40 only */
-		if (rate == 32) {
-			if (!CHSPEC_IS40(wlc->home_chanspec) ||
-			    ((stf != PHY_TXC1_MODE_SISO)
-			     && (stf != PHY_TXC1_MODE_CDD))) {
-				wiphy_err(wlc->wiphy, "wl%d: %s: Invalid mcs "
-					  "32\n", BRCMS_UNIT(wlc), __func__);
-				bcmerror = -EINVAL;
-				goto done;
-			}
-			/* mcs > 7 must use stf SDM */
-		} else if (rate > HIGHEST_SINGLE_STREAM_MCS) {
-			/* mcs > 7 must use stf SDM */
-			if (stf != PHY_TXC1_MODE_SDM) {
-				BCMMSG(wlc->wiphy, "wl%d: enabling "
-					 "SDM mode for mcs %d\n",
-					 BRCMS_UNIT(wlc), rate);
-				stf = PHY_TXC1_MODE_SDM;
-			}
-		} else {
-			/* MCS 0-7 may use SISO, CDD, and for phy_rev >= 3 STBC */
-			if ((stf > PHY_TXC1_MODE_STBC) ||
-			    (!BRCMS_STBC_CAP_PHY(wlc)
-			     && (stf == PHY_TXC1_MODE_STBC))) {
-				wiphy_err(wlc->wiphy, "wl%d: %s: Invalid STBC"
-					  "\n", BRCMS_UNIT(wlc), __func__);
-				bcmerror = -EINVAL;
-				goto done;
-			}
-		}
-	} else if (IS_OFDM(rate)) {
-		if ((stf != PHY_TXC1_MODE_CDD) && (stf != PHY_TXC1_MODE_SISO)) {
-			wiphy_err(wlc->wiphy, "wl%d: %s: Invalid OFDM\n",
-				  BRCMS_UNIT(wlc), __func__);
-			bcmerror = -EINVAL;
-			goto done;
-		}
-	} else if (IS_CCK(rate)) {
-		if ((cur_band->bandtype != BRCM_BAND_2G)
-		    || (stf != PHY_TXC1_MODE_SISO)) {
-			wiphy_err(wlc->wiphy, "wl%d: %s: Invalid CCK\n",
-				  BRCMS_UNIT(wlc), __func__);
-			bcmerror = -EINVAL;
-			goto done;
-		}
-	} else {
-		wiphy_err(wlc->wiphy, "wl%d: %s: Unknown rate type\n",
-			  BRCMS_UNIT(wlc), __func__);
-		bcmerror = -EINVAL;
-		goto done;
-	}
-	/* make sure multiple antennae are available for non-siso rates */
-	if ((stf != PHY_TXC1_MODE_SISO) && (wlc->stf->txstreams == 1)) {
-		wiphy_err(wlc->wiphy, "wl%d: %s: SISO antenna but !SISO "
-			  "request\n", BRCMS_UNIT(wlc), __func__);
-		bcmerror = -EINVAL;
-		goto done;
-	}
-
-	rspec = rate;
-	if (ismcs) {
-		rspec |= RSPEC_MIMORATE;
-		/* For STBC populate the STC field of the ratespec */
-		if (stf == PHY_TXC1_MODE_STBC) {
-			u8 stc;
-			stc = 1;	/* Nss for single stream is always 1 */
-			rspec |= (stc << RSPEC_STC_SHIFT);
-		}
-	}
-
-	rspec |= (stf << RSPEC_STF_SHIFT);
-
-	if (override_mcs_only)
-		rspec |= RSPEC_OVERRIDE_MCS_ONLY;
-
-	if (issgi)
-		rspec |= RSPEC_SHORT_GI;
-
-	if ((rate != 0)
-	    && !brcms_c_valid_rate(wlc, rspec, cur_band->bandtype, true)) {
-		return rate;
-	}
-
-	return rspec;
-done:
-	return rate;
+	wlc_phy_hold_upd(wlc->band->pi, PHY_HOLD_FOR_SCAN, false);
 }
 
-/* formula:  IDLE_BUSY_RATIO_X_16 = (100-duty_cycle)/duty_cycle*16 */
-static int
-brcms_c_duty_cycle_set(struct brcms_c_info *wlc, int duty_cycle, bool isOFDM,
-		   bool writeToShm)
+void brcms_c_associate_upd(struct brcms_c_info *wlc, bool state)
 {
-	int idle_busy_ratio_x_16 = 0;
-	uint offset =
-	    isOFDM ? M_TX_IDLE_BUSY_RATIO_X_16_OFDM :
-	    M_TX_IDLE_BUSY_RATIO_X_16_CCK;
-	if (duty_cycle > 100 || duty_cycle < 0) {
-		wiphy_err(wlc->wiphy, "wl%d:  duty cycle value off limit\n",
-			  wlc->pub->unit);
-		return -EINVAL;
-	}
-	if (duty_cycle)
-		idle_busy_ratio_x_16 = (100 - duty_cycle) * 16 / duty_cycle;
-	/* Only write to shared memory  when wl is up */
-	if (writeToShm)
-		brcms_c_write_shm(wlc, offset, (u16) idle_busy_ratio_x_16);
-
-	if (isOFDM)
-		wlc->tx_duty_cycle_ofdm = (u16) duty_cycle;
-	else
-		wlc->tx_duty_cycle_cck = (u16) duty_cycle;
-
-	return 0;
+	wlc->pub->associated = state;
+	wlc->bsscfg->associated = state;
 }
 
-/* Read a single u16 from shared memory.
- * SHM 'offset' needs to be an even address
+/*
+ * When a remote STA/AP is removed by Mac80211, or when it can no longer accept
+ * AMPDU traffic, packets pending in hardware have to be invalidated so that
+ * when later on hardware releases them, they can be handled appropriately.
  */
-u16 brcms_c_read_shm(struct brcms_c_info *wlc, uint offset)
+void brcms_c_inval_dma_pkts(struct brcms_hardware *hw,
+			       struct ieee80211_sta *sta,
+			       void (*dma_callback_fn))
 {
-	return brcms_b_read_shm(wlc->hw, offset);
+	struct dma_pub *dmah;
+	int i;
+	for (i = 0; i < NFIFO; i++) {
+		dmah = hw->di[i];
+		if (dmah != NULL)
+			dma_walk_packets(dmah, dma_callback_fn, sta);
+	}
 }
 
-/* Write a single u16 to shared memory.
- * SHM 'offset' needs to be an even address
- */
-void brcms_c_write_shm(struct brcms_c_info *wlc, uint offset, u16 v)
+int brcms_c_get_curband(struct brcms_c_info *wlc)
 {
-	brcms_b_write_shm(wlc->hw, offset, v);
+	return wlc->band->bandunit;
 }
 
-/* Copy a buffer to shared memory.
- * SHM 'offset' needs to be an even address and
- * Buffer length 'len' must be an even number of bytes
- */
-void brcms_c_copyto_shm(struct brcms_c_info *wlc, uint offset, const void *buf,
-			int len)
+void brcms_c_wait_for_tx_completion(struct brcms_c_info *wlc, bool drop)
 {
-	/* offset and len need to be even */
-	if (len <= 0 || (offset & 1) || (len & 1))
-		return;
-
-	brcms_b_copyto_objmem(wlc->hw, offset, buf, len, OBJADDR_SHM_SEL);
+	/* flush packet queue when requested */
+	if (drop)
+		brcmu_pktq_flush(&wlc->pkt_queue->q, false, NULL, NULL);
 
+	/* wait for queue and DMA fifos to run dry */
+	while (!pktq_empty(&wlc->pkt_queue->q) || brcms_txpktpendtot(wlc) > 0)
+		brcms_msleep(wlc->wl, 1);
 }
 
-/* wrapper BMAC functions to for HIGH driver access */
-void brcms_c_mctrl(struct brcms_c_info *wlc, u32 mask, u32 val)
+void brcms_c_set_beacon_listen_interval(struct brcms_c_info *wlc, u8 interval)
 {
-	brcms_b_mctrl(wlc->hw, mask, val);
+	wlc->bcn_li_bcn = interval;
+	if (wlc->pub->up)
+		brcms_c_bcn_li_upd(wlc);
 }
 
-void brcms_c_mhf(struct brcms_c_info *wlc, u8 idx, u16 mask, u16 val, int bands)
+int brcms_c_set_tx_power(struct brcms_c_info *wlc, int txpwr)
 {
-	brcms_b_mhf(wlc->hw, idx, mask, val, bands);
-}
+	uint qdbm;
 
-int brcms_c_xmtfifo_sz_get(struct brcms_c_info *wlc, uint fifo, uint *blocks)
-{
-	return brcms_b_xmtfifo_sz_get(wlc->hw, fifo, blocks);
+	/* Remove override bit and clip to max qdbm value */
+	qdbm = min_t(uint, txpwr * BRCMS_TXPWR_DB_FACTOR, 0xff);
+	return wlc_phy_txpower_set(wlc->band->pi, qdbm, false);
 }
 
-void brcms_c_write_template_ram(struct brcms_c_info *wlc, int offset, int len,
-			    void *buf)
+int brcms_c_get_tx_power(struct brcms_c_info *wlc)
 {
-	brcms_b_write_template_ram(wlc->hw, offset, len, buf);
-}
+	uint qdbm;
+	bool override;
 
-void brcms_c_write_hw_bcntemplates(struct brcms_c_info *wlc, void *bcn, int len,
-			       bool both)
-{
-	brcms_b_write_hw_bcntemplates(wlc->hw, bcn, len, both);
-}
+	wlc_phy_txpower_get(wlc->band->pi, &qdbm, &override);
 
-void
-brcms_c_set_addrmatch(struct brcms_c_info *wlc, int match_reg_offset,
-		  const u8 *addr)
-{
-	brcms_b_set_addrmatch(wlc->hw, match_reg_offset, addr);
-	if (match_reg_offset == RCM_BSSID_OFFSET)
-		memcpy(wlc->cfg->BSSID, addr, ETH_ALEN);
+	/* Return qdbm units */
+	return (int)(qdbm / BRCMS_TXPWR_DB_FACTOR);
 }
 
-void brcms_c_pllreq(struct brcms_c_info *wlc, bool set, mbool req_bit)
+void brcms_c_set_radio_mpc(struct brcms_c_info *wlc, bool mpc)
 {
-	brcms_b_pllreq(wlc->hw, set, req_bit);
+	wlc->mpc = mpc;
+	brcms_c_radio_mpc_upd(wlc);
 }
 
-void brcms_c_reset_bmac_done(struct brcms_c_info *wlc)
+/* Process received frames */
+/*
+ * Return true if more frames need to be processed. false otherwise.
+ * Param 'bound' indicates max. # frames to process before break out.
+ */
+static void brcms_c_recv(struct brcms_c_info *wlc, struct sk_buff *p)
 {
-}
+	struct d11rxhdr *rxh;
+	struct ieee80211_hdr *h;
+	uint len;
+	bool is_amsdu;
 
-/* check for the particular priority flow control bit being set */
-bool
-brcms_c_txflowcontrol_prio_isset(struct brcms_c_info *wlc,
-				 struct brcms_txq_info *q,
-				 int prio)
-{
-	uint prio_mask;
+	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
 
-	if (prio == ALLPRIO) {
-		prio_mask = TXQ_STOP_FOR_PRIOFC_MASK;
-	} else {
-		prio_mask = NBITVAL(prio);
-	}
+	/* frame starts with rxhdr */
+	rxh = (struct d11rxhdr *) (p->data);
 
-	return (q->stopped & prio_mask) == prio_mask;
-}
+	/* strip off rxhdr */
+	skb_pull(p, BRCMS_HWRXOFF);
 
-/* propagate the flow control to all interfaces using the given tx queue */
-void brcms_c_txflowcontrol(struct brcms_c_info *wlc,
-			   struct brcms_txq_info *qi,
-			   bool on, int prio)
-{
-	uint prio_bits;
-	uint cur_bits;
+	/* MAC inserts 2 pad bytes for a4 headers or QoS or A-MSDU subframes */
+	if (rxh->RxStatus1 & RXS_PBPRES) {
+		if (p->len < 2) {
+			wiphy_err(wlc->wiphy, "wl%d: recv: rcvd runt of "
+				  "len %d\n", wlc->pub->unit, p->len);
+			goto toss;
+		}
+		skb_pull(p, 2);
+	}
 
-	BCMMSG(wlc->wiphy, "flow control kicks in\n");
+	h = (struct ieee80211_hdr *)(p->data + D11_PHY_HDR_LEN);
+	len = p->len;
 
-	if (prio == ALLPRIO) {
-		prio_bits = TXQ_STOP_FOR_PRIOFC_MASK;
-	} else {
-		prio_bits = NBITVAL(prio);
+	if (rxh->RxStatus1 & RXS_FCSERR) {
+		if (wlc->pub->mac80211_state & MAC80211_PROMISC_BCNS) {
+			wiphy_err(wlc->wiphy, "FCSERR while scanning******* -"
+				  " tossing\n");
+			goto toss;
+		} else {
+			wiphy_err(wlc->wiphy, "RCSERR!!!\n");
+			goto toss;
+		}
 	}
 
-	cur_bits = qi->stopped & prio_bits;
+	/* check received pkt has at least frame control field */
+	if (len < D11_PHY_HDR_LEN + sizeof(h->frame_control))
+		goto toss;
 
-	/* Check for the case of no change and return early
-	 * Otherwise update the bit and continue
-	 */
-	if (on) {
-		if (cur_bits == prio_bits) {
-			return;
-		}
-		mboolset(qi->stopped, prio_bits);
-	} else {
-		if (cur_bits == 0) {
-			return;
+	/* not supporting A-MSDU */
+	is_amsdu = rxh->RxStatus2 & RXS_AMSDU_MASK;
+	if (is_amsdu)
+		goto toss;
+
+	brcms_c_recvctl(wlc, rxh, p);
+	return;
+
+ toss:
+	brcmu_pkt_buf_free_skb(p);
+}
+
+/* Process received frames */
+/*
+ * Return true if more frames need to be processed. false otherwise.
+ * Param 'bound' indicates max. # frames to process before break out.
+ */
+static bool
+brcms_b_recv(struct brcms_hardware *wlc_hw, uint fifo, bool bound)
+{
+	struct sk_buff *p;
+	struct sk_buff *head = NULL;
+	struct sk_buff *tail = NULL;
+	uint n = 0;
+	uint bound_limit = bound ? RXBND : -1;
+
+	BCMMSG(wlc_hw->wlc->wiphy, "wl%d\n", wlc_hw->unit);
+	/* gather received frames */
+	while ((p = dma_rx(wlc_hw->di[fifo]))) {
+
+		if (!tail)
+			head = tail = p;
+		else {
+			tail->prev = p;
+			tail = p;
 		}
-		mboolclr(qi->stopped, prio_bits);
+
+		/* !give others some time to run! */
+		if (++n >= bound_limit)
+			break;
 	}
 
-	/* If there is a flow control override we will not change the external
-	 * flow control state.
-	 */
-	if (qi->stopped & ~TXQ_STOP_FOR_PRIOFC_MASK) {
-		return;
+	/* post more rbufs */
+	dma_rxfill(wlc_hw->di[fifo]);
+
+	/* process each frame */
+	while ((p = head) != NULL) {
+		struct d11rxhdr_le *rxh_le;
+		struct d11rxhdr *rxh;
+		head = head->prev;
+		p->prev = NULL;
+
+		rxh_le = (struct d11rxhdr_le *)p->data;
+		rxh = (struct d11rxhdr *)p->data;
+
+		/* fixup rx header endianness */
+		rxh->RxFrameSize = le16_to_cpu(rxh_le->RxFrameSize);
+		rxh->PhyRxStatus_0 = le16_to_cpu(rxh_le->PhyRxStatus_0);
+		rxh->PhyRxStatus_1 = le16_to_cpu(rxh_le->PhyRxStatus_1);
+		rxh->PhyRxStatus_2 = le16_to_cpu(rxh_le->PhyRxStatus_2);
+		rxh->PhyRxStatus_3 = le16_to_cpu(rxh_le->PhyRxStatus_3);
+		rxh->PhyRxStatus_4 = le16_to_cpu(rxh_le->PhyRxStatus_4);
+		rxh->PhyRxStatus_5 = le16_to_cpu(rxh_le->PhyRxStatus_5);
+		rxh->RxStatus1 = le16_to_cpu(rxh_le->RxStatus1);
+		rxh->RxStatus2 = le16_to_cpu(rxh_le->RxStatus2);
+		rxh->RxTSFTime = le16_to_cpu(rxh_le->RxTSFTime);
+		rxh->RxChan = le16_to_cpu(rxh_le->RxChan);
+
+		brcms_c_recv(wlc_hw->wlc, p);
 	}
 
-	brcms_c_txflowcontrol_signal(wlc, qi, on, prio);
+	return n >= bound_limit;
 }
 
-void
-brcms_c_txflowcontrol_override(struct brcms_c_info *wlc,
-			       struct brcms_txq_info *qi,
-			       bool on, uint override)
+/* second-level interrupt processing
+ *   Return true if another dpc needs to be re-scheduled. false otherwise.
+ *   Param 'bounded' indicates if applicable loops should be bounded.
+ */
+bool brcms_c_dpc(struct brcms_c_info *wlc, bool bounded)
 {
-	uint prev_override;
+	u32 macintstatus;
+	struct brcms_hardware *wlc_hw = wlc->hw;
+	struct d11regs __iomem *regs = wlc_hw->regs;
+	struct wiphy *wiphy = wlc->wiphy;
 
-	prev_override = (qi->stopped & ~TXQ_STOP_FOR_PRIOFC_MASK);
+	if (brcms_deviceremoved(wlc)) {
+		wiphy_err(wiphy, "wl%d: %s: dead chip\n", wlc_hw->unit,
+			  __func__);
+		brcms_down(wlc->wl);
+		return false;
+	}
 
-	/* Update the flow control bits and do an early return if there is
-	 * no change in the external flow control state.
-	 */
-	if (on) {
-		mboolset(qi->stopped, override);
-		/* if there was a previous override bit on, then setting this
-		 * makes no difference.
-		 */
-		if (prev_override) {
-			return;
-		}
+	/* grab and clear the saved software intstatus bits */
+	macintstatus = wlc->macintstatus;
+	wlc->macintstatus = 0;
 
-		brcms_c_txflowcontrol_signal(wlc, qi, ON, ALLPRIO);
-	} else {
-		mboolclr(qi->stopped, override);
-		/* clearing an override bit will only make a difference for
-		 * flow control if it was the only bit set. For any other
-		 * override setting, just return
-		 */
-		if (prev_override != override) {
-			return;
-		}
+	BCMMSG(wlc->wiphy, "wl%d: macintstatus 0x%x\n",
+	       wlc_hw->unit, macintstatus);
 
-		if (qi->stopped == 0) {
-			brcms_c_txflowcontrol_signal(wlc, qi, OFF, ALLPRIO);
-		} else {
-			int prio;
+	WARN_ON(macintstatus & MI_PRQ); /* PRQ Interrupt in non-MBSS */
 
-			for (prio = MAXPRIO; prio >= 0; prio--) {
-				if (!mboolisset(qi->stopped, NBITVAL(prio)))
-					brcms_c_txflowcontrol_signal(
-						wlc, qi, OFF, prio);
-			}
+	/* tx status */
+	if (macintstatus & MI_TFS) {
+		bool fatal;
+		if (brcms_b_txstatus(wlc->hw, bounded, &fatal))
+			wlc->macintstatus |= MI_TFS;
+		if (fatal) {
+			wiphy_err(wiphy, "MI_TFS: fatal\n");
+			goto fatal;
 		}
 	}
-}
 
-static void brcms_c_txflowcontrol_reset(struct brcms_c_info *wlc)
-{
-	struct brcms_txq_info *qi;
+	if (macintstatus & (MI_TBTT | MI_DTIM_TBTT))
+		brcms_c_tbtt(wlc);
 
-	for (qi = wlc->tx_queues; qi != NULL; qi = qi->next) {
-		if (qi->stopped) {
-			brcms_c_txflowcontrol_signal(wlc, qi, OFF, ALLPRIO);
-			qi->stopped = 0;
-		}
+	/* ATIM window end */
+	if (macintstatus & MI_ATIMWINEND) {
+		BCMMSG(wlc->wiphy, "end of ATIM window\n");
+		OR_REG(&regs->maccommand, wlc->qvalid);
+		wlc->qvalid = 0;
 	}
-}
 
-static void
-brcms_c_txflowcontrol_signal(struct brcms_c_info *wlc,
-			     struct brcms_txq_info *qi, bool on, int prio)
-{
-#ifdef NON_FUNCTIONAL
-	/* wlcif_list is never filled so this function is not functional */
-	struct brcms_c_if *wlcif;
+	/*
+	 * received data or control frame, MI_DMAINT is
+	 * indication of RX_FIFO interrupt
+	 */
+	if (macintstatus & MI_DMAINT)
+		if (brcms_b_recv(wlc_hw, RX_FIFO, bounded))
+			wlc->macintstatus |= MI_DMAINT;
 
-	for (wlcif = wlc->wlcif_list; wlcif != NULL; wlcif = wlcif->next) {
-		if (wlcif->qi == qi && wlcif->flags & BRCMS_IF_LINKED)
-			brcms_txflowcontrol(wlc->wl, wlcif->wlif, on, prio);
+	/* noise sample collected */
+	if (macintstatus & MI_BG_NOISE)
+		wlc_phy_noise_sample_intr(wlc_hw->band->pi);
+
+	if (macintstatus & MI_GP0) {
+		wiphy_err(wiphy, "wl%d: PSM microcode watchdog fired at %d "
+			"(seconds). Resetting.\n", wlc_hw->unit, wlc_hw->now);
+
+		printk_once("%s : PSM Watchdog, chipid 0x%x, chiprev 0x%x\n",
+					__func__, wlc_hw->sih->chip,
+					wlc_hw->sih->chiprev);
+		/* big hammer */
+		brcms_init(wlc->wl);
 	}
-#endif
+
+	/* gptimer timeout */
+	if (macintstatus & MI_TO)
+		W_REG(&regs->gptimer, 0);
+
+	if (macintstatus & MI_RFDISABLE) {
+		BCMMSG(wlc->wiphy, "wl%d: BMAC Detected a change on the"
+		       " RF Disable Input\n", wlc_hw->unit);
+		brcms_rfkill_set_hw_state(wlc->wl);
+	}
+
+	/* send any enq'd tx packets. Just makes sure to jump start tx */
+	if (!pktq_empty(&wlc->pkt_queue->q))
+		brcms_c_send_q(wlc);
+
+	/* it isn't done and needs to be resched if macintstatus is non-zero */
+	return wlc->macintstatus != 0;
+
+ fatal:
+	brcms_init(wlc->wl);
+	return wlc->macintstatus != 0;
 }
 
-static struct brcms_txq_info *brcms_c_txq_alloc(struct brcms_c_info *wlc)
+void brcms_c_init(struct brcms_c_info *wlc)
 {
-	struct brcms_txq_info *qi, *p;
+	struct d11regs __iomem *regs;
+	u16 chanspec;
+	bool mute = false;
 
-	qi = kzalloc(sizeof(struct brcms_txq_info), GFP_ATOMIC);
-	if (qi != NULL) {
+	BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
+
+	regs = wlc->regs;
+
+	/*
+	 * This will happen if a big-hammer was executed. In
+	 * that case, we want to go back to the channel that
+	 * we were on and not new channel
+	 */
+	if (wlc->pub->associated)
+		chanspec = wlc->home_chanspec;
+	else
+		chanspec = brcms_c_init_chanspec(wlc);
+
+	brcms_b_init(wlc->hw, chanspec, mute);
+
+	/* update beacon listen interval */
+	brcms_c_bcn_li_upd(wlc);
+
+	/* write ethernet address to core */
+	brcms_c_set_mac(wlc->bsscfg);
+	brcms_c_set_bssid(wlc->bsscfg);
+
+	/* Update tsf_cfprep if associated and up */
+	if (wlc->pub->associated && wlc->bsscfg->up) {
+		u32 bi;
+
+		/* get beacon period and convert to uS */
+		bi = wlc->bsscfg->current_bss->beacon_period << 10;
 		/*
-		 * Have enough room for control packets along with HI watermark
-		 * Also, add room to txq for total psq packets if all the SCBs
-		 * leave PS mode. The watermark for flowcontrol to OS packets
-		 * will remain the same
+		 * update since init path would reset
+		 * to default value
 		 */
-		brcmu_pktq_init(&qi->q, BRCMS_PREC_COUNT,
-			  (2 * wlc->pub->tunables->datahiwat) + PKTQ_LEN_DEFAULT
-			  + wlc->pub->psq_pkts_total);
+		W_REG(&regs->tsf_cfprep,
+		      (bi << CFPREP_CBI_SHIFT));
 
-		/* add this queue to the the global list */
-		p = wlc->tx_queues;
-		if (p == NULL) {
-			wlc->tx_queues = qi;
-		} else {
-			while (p->next != NULL)
-				p = p->next;
-			p->next = qi;
-		}
+		/* Update maccontrol PM related bits */
+		brcms_c_set_ps_ctrl(wlc);
 	}
-	return qi;
-}
 
-static void brcms_c_txq_free(struct brcms_c_info *wlc,
-			     struct brcms_txq_info *qi)
-{
-	struct brcms_txq_info *p;
+	brcms_c_bandinit_ordered(wlc, chanspec);
 
-	if (qi == NULL)
-		return;
+	/* init probe response timeout */
+	brcms_b_write_shm(wlc->hw, M_PRS_MAXTIME, wlc->prb_resp_timeout);
 
-	/* remove the queue from the linked list */
-	p = wlc->tx_queues;
-	if (p == qi)
-		wlc->tx_queues = p->next;
-	else {
-		while (p != NULL && p->next != qi)
-			p = p->next;
-		if (p != NULL)
-			p->next = p->next->next;
+	/* init max burst txop (framebursting) */
+	brcms_b_write_shm(wlc->hw, M_MBURST_TXOP,
+		      (wlc->
+		       _rifs ? (EDCF_AC_VO_TXOP_AP << 5) : MAXFRAMEBURST_TXOP));
+
+	/* initialize maximum allowed duty cycle */
+	brcms_c_duty_cycle_set(wlc, wlc->tx_duty_cycle_ofdm, true, true);
+	brcms_c_duty_cycle_set(wlc, wlc->tx_duty_cycle_cck, false, true);
+
+	/*
+	 * Update some shared memory locations related to
+	 * max AMPDU size allowed to received
+	 */
+	brcms_c_ampdu_shm_upd(wlc->ampdu);
+
+	/* band-specific inits */
+	brcms_c_bsinit(wlc);
+
+	/* Enable EDCF mode (while the MAC is suspended) */
+	OR_REG(&regs->ifs_ctl, IFS_USEEDCF);
+	brcms_c_edcf_setparams(wlc, false);
+
+	/* Init precedence maps for empty FIFOs */
+	brcms_c_tx_prec_map_init(wlc);
+
+	/* read the ucode version if we have not yet done so */
+	if (wlc->ucode_rev == 0) {
+		wlc->ucode_rev =
+		    brcms_b_read_shm(wlc->hw, M_BOM_REV_MAJOR) << NBITS(u16);
+		wlc->ucode_rev |= brcms_b_read_shm(wlc->hw, M_BOM_REV_MINOR);
 	}
 
-	kfree(qi);
-}
+	/* ..now really unleash hell (allow the MAC out of suspend) */
+	brcms_c_enable_mac(wlc);
 
-/*
- * Flag 'scan in progress' to withhold dynamic phy calibration
- */
-void brcms_c_scan_start(struct brcms_c_info *wlc)
-{
-	wlc_phy_hold_upd(wlc->band->pi, PHY_HOLD_FOR_SCAN, true);
-}
+	/* clear tx flow control */
+	brcms_c_txflowcontrol_reset(wlc);
 
-void brcms_c_scan_stop(struct brcms_c_info *wlc)
-{
-	wlc_phy_hold_upd(wlc->band->pi, PHY_HOLD_FOR_SCAN, false);
-}
+	/* enable the RF Disable Delay timer */
+	W_REG(&wlc->regs->rfdisabledly, RFDISABLE_DEFAULT);
 
-void brcms_c_associate_upd(struct brcms_c_info *wlc, bool state)
-{
-	wlc->pub->associated = state;
-	wlc->cfg->associated = state;
+	/* initialize mpc delay */
+	wlc->mpc_delay_off = wlc->mpc_dlycnt = BRCMS_MPC_MIN_DELAYCNT;
+
+	/*
+	 * Initialize WME parameters; if they haven't been set by some other
+	 * mechanism (IOVar, etc) then read them from the hardware.
+	 */
+	if (GFIELD(wlc->wme_retries[0], EDCF_SHORT) == 0) {
+		/* Uninitialized; read from HW */
+		int ac;
+
+		for (ac = 0; ac < AC_COUNT; ac++)
+			wlc->wme_retries[ac] =
+			    brcms_b_read_shm(wlc->hw, M_AC_TXLMT_ADDR(ac));
+	}
 }
 
 /*
- * When a remote STA/AP is removed by Mac80211, or when it can no longer accept
- * AMPDU traffic, packets pending in hardware have to be invalidated so that
- * when later on hardware releases them, they can be handled appropriately.
+ * The common driver entry routine. Error codes should be unique
  */
-void brcms_c_inval_dma_pkts(struct brcms_hardware *hw,
-			       struct ieee80211_sta *sta,
-			       void (*dma_callback_fn))
+struct brcms_c_info *
+brcms_c_attach(struct brcms_info *wl, u16 vendor, u16 device, uint unit,
+	       bool piomode, void __iomem *regsva, struct pci_dev *btparam,
+	       uint *perr)
 {
-	struct dma_pub *dmah;
-	int i;
-	for (i = 0; i < NFIFO; i++) {
-		dmah = hw->di[i];
-		if (dmah != NULL)
-			dma_walk_packets(dmah, dma_callback_fn, sta);
-	}
-}
+	struct brcms_c_info *wlc;
+	uint err = 0;
+	uint i, j;
+	struct brcms_pub *pub;
 
-int brcms_c_get_curband(struct brcms_c_info *wlc)
-{
-	return wlc->band->bandunit;
-}
+	/* allocate struct brcms_c_info state and its substructures */
+	wlc = (struct brcms_c_info *) brcms_c_attach_malloc(unit, &err, device);
+	if (wlc == NULL)
+		goto fail;
+	wlc->wiphy = wl->wiphy;
+	pub = wlc->pub;
 
-void brcms_c_wait_for_tx_completion(struct brcms_c_info *wlc, bool drop)
-{
-	/* flush packet queue when requested */
-	if (drop)
-		brcmu_pktq_flush(&wlc->pkt_queue->q, false, NULL, NULL);
+#if defined(BCMDBG)
+	wlc_info_dbg = wlc;
+#endif
 
-	/* wait for queue and DMA fifos to run dry */
-	while (!pktq_empty(&wlc->pkt_queue->q) ||
-	       TXPKTPENDTOT(wlc) > 0) {
-		brcms_msleep(wlc->wl, 1);
-	}
-}
+	wlc->band = wlc->bandstate[0];
+	wlc->core = wlc->corestate;
+	wlc->wl = wl;
+	pub->unit = unit;
+	pub->_piomode = piomode;
+	wlc->bandinit_pending = false;
 
-int brcms_c_set_par(struct brcms_c_info *wlc, enum wlc_par_id par_id,
-		    int int_val)
-{
-	int err = 0;
+	/* populate struct brcms_c_info with default values  */
+	brcms_c_info_init(wlc, unit);
 
-	switch (par_id) {
-	case IOV_BCN_LI_BCN:
-		wlc->bcn_li_bcn = (u8) int_val;
-		if (wlc->pub->up)
-			brcms_c_bcn_li_upd(wlc);
-		break;
-		/* As long as override is false, this only sets the *user*
-		   targets. User can twiddle this all he wants with no harm.
-		   wlc_phy_txpower_set() explicitly sets override to false if
-		   not internal or test.
-		 */
-	case IOV_QTXPOWER:{
-		u8 qdbm;
-		bool override;
-
-		/* Remove override bit and clip to max qdbm value */
-		qdbm = (u8)min_t(u32, (int_val & ~WL_TXPWR_OVERRIDE), 0xff);
-		/* Extract override setting */
-		override = (int_val & WL_TXPWR_OVERRIDE) ? true : false;
-		err =
-		    wlc_phy_txpower_set(wlc->band->pi, qdbm, override);
-		break;
+	/* update sta/ap related parameters */
+	brcms_c_ap_upd(wlc);
+
+	/*
+	 * low level attach steps(all hw accesses go
+	 * inside, no more in rest of the attach)
+	 */
+	err = brcms_b_attach(wlc, vendor, device, unit, piomode, regsva,
+			     btparam);
+	if (err)
+		goto fail;
+
+	brcms_c_protection_upd(wlc, BRCMS_PROT_N_PAM_OVR, OFF);
+
+	pub->phy_11ncapable = BRCMS_PHY_11N_CAP(wlc->band);
+
+	/* disable allowed duty cycle */
+	wlc->tx_duty_cycle_ofdm = 0;
+	wlc->tx_duty_cycle_cck = 0;
+
+	brcms_c_stf_phy_chain_calc(wlc);
+
+	/* txchain 1: txant 0, txchain 2: txant 1 */
+	if (BRCMS_ISNPHY(wlc->band) && (wlc->stf->txstreams == 1))
+		wlc->stf->txant = wlc->stf->hw_txchain - 1;
+
+	/* push to BMAC driver */
+	wlc_phy_stf_chain_init(wlc->band->pi, wlc->stf->hw_txchain,
+			       wlc->stf->hw_rxchain);
+
+	/* pull up some info resulting from the low attach */
+	for (i = 0; i < NFIFO; i++)
+		wlc->core->txavail[i] = wlc->hw->txavail[i];
+
+	memcpy(&wlc->perm_etheraddr, &wlc->hw->etheraddr, ETH_ALEN);
+	memcpy(&pub->cur_etheraddr, &wlc->hw->etheraddr, ETH_ALEN);
+
+	for (j = 0; j < wlc->pub->_nbands; j++) {
+		wlc->band = wlc->bandstate[j];
+
+		if (!brcms_c_attach_stf_ant_init(wlc)) {
+			err = 24;
+			goto fail;
 		}
-	case IOV_MPC:
-		wlc->mpc = (bool)int_val;
-		brcms_c_radio_mpc_upd(wlc);
-		break;
-	default:
-		err = -ENOTSUPP;
-	}
-	return err;
-}
 
-int brcms_c_get_par(struct brcms_c_info *wlc, enum wlc_par_id par_id,
-		    int *ret_int_ptr)
-{
-	int err = 0;
+		/* default contention windows size limits */
+		wlc->band->CWmin = APHY_CWMIN;
+		wlc->band->CWmax = PHY_CWMAX;
 
-	switch (par_id) {
-	case IOV_BCN_LI_BCN:
-		*ret_int_ptr = wlc->bcn_li_bcn;
-		break;
-	case IOV_QTXPOWER: {
-		uint qdbm;
-		bool override;
-
-		err = wlc_phy_txpower_get(wlc->band->pi, &qdbm,
-			&override);
-		if (err != 0)
-			return err;
-
-		/* Return qdbm units */
-		*ret_int_ptr =
-		    qdbm | (override ? WL_TXPWR_OVERRIDE : 0);
-		break;
+		/* init gmode value */
+		if (wlc->band->bandtype == BRCM_BAND_2G) {
+			wlc->band->gmode = GMODE_AUTO;
+			brcms_c_protection_upd(wlc, BRCMS_PROT_G_USER,
+					   wlc->band->gmode);
 		}
-	case IOV_MPC:
-		*ret_int_ptr = (s32) wlc->mpc;
-		break;
-	default:
-		err = -ENOTSUPP;
+
+		/* init _n_enab supported mode */
+		if (BRCMS_PHY_11N_CAP(wlc->band)) {
+			pub->_n_enab = SUPPORT_11N;
+			brcms_c_protection_upd(wlc, BRCMS_PROT_N_USER,
+						   ((pub->_n_enab ==
+						     SUPPORT_11N) ? WL_11N_2x2 :
+						    WL_11N_3x3));
+		}
+
+		/* init per-band default rateset, depend on band->gmode */
+		brcms_default_rateset(wlc, &wlc->band->defrateset);
+
+		/* fill in hw_rateset */
+		brcms_c_rateset_filter(&wlc->band->defrateset,
+				   &wlc->band->hw_rateset, false,
+				   BRCMS_RATES_CCK_OFDM, BRCMS_RATE_MASK,
+				   (bool) (wlc->pub->_n_enab & SUPPORT_11N));
 	}
-	return err;
-}
 
-/*
- * Search the name=value vars for a specific one and return its value.
- * Returns NULL if not found.
- */
-char *getvar(char *vars, const char *name)
-{
-	char *s;
-	int len;
+	/*
+	 * update antenna config due to
+	 * wlc->stf->txant/txchain/ant_rx_ovr change
+	 */
+	brcms_c_stf_phy_txant_upd(wlc);
 
-	if (!name)
-		return NULL;
+	/* attach each modules */
+	err = brcms_c_attach_module(wlc);
+	if (err != 0)
+		goto fail;
 
-	len = strlen(name);
-	if (len == 0)
-		return NULL;
+	if (!brcms_c_timers_init(wlc, unit)) {
+		wiphy_err(wl->wiphy, "wl%d: %s: init_timer failed\n", unit,
+			  __func__);
+		err = 32;
+		goto fail;
+	}
 
-	/* first look in vars[] */
-	for (s = vars; s && *s;) {
-		if ((memcmp(s, name, len) == 0) && (s[len] == '='))
-			return &s[len + 1];
+	/* depend on rateset, gmode */
+	wlc->cmi = brcms_c_channel_mgr_attach(wlc);
+	if (!wlc->cmi) {
+		wiphy_err(wl->wiphy, "wl%d: %s: channel_mgr_attach failed"
+			  "\n", unit, __func__);
+		err = 33;
+		goto fail;
+	}
 
-		while (*s++)
-			;
+	/* init default when all parameters are ready, i.e. ->rateset */
+	brcms_c_bss_default_init(wlc);
+
+	/*
+	 * Complete the wlc default state initializations..
+	 */
+
+	/* allocate our initial queue */
+	wlc->pkt_queue = brcms_c_txq_alloc(wlc);
+	if (wlc->pkt_queue == NULL) {
+		wiphy_err(wl->wiphy, "wl%d: %s: failed to malloc tx queue\n",
+			  unit, __func__);
+		err = 100;
+		goto fail;
 	}
-	/* nothing found */
-	return NULL;
-}
 
-/*
- * Search the vars for a specific one and return its value as
- * an integer. Returns 0 if not found.
- */
-int getintvar(char *vars, const char *name)
-{
-	char *val;
+	wlc->bsscfg->wlc = wlc;
 
-	val = getvar(vars, name);
-	if (val == NULL)
-		return 0;
+	wlc->mimoft = FT_HT;
+	wlc->mimo_40txbw = AUTO;
+	wlc->ofdm_40txbw = AUTO;
+	wlc->cck_40txbw = AUTO;
+	brcms_c_update_mimo_band_bwcap(wlc, BRCMS_N_BW_20IN2G_40IN5G);
+
+	/* Set default values of SGI */
+	if (BRCMS_SGI_CAP_PHY(wlc)) {
+		brcms_c_ht_update_sgi_rx(wlc, (BRCMS_N_SGI_20 |
+					       BRCMS_N_SGI_40));
+	} else if (BRCMS_ISSSLPNPHY(wlc->band)) {
+		brcms_c_ht_update_sgi_rx(wlc, (BRCMS_N_SGI_20 |
+					       BRCMS_N_SGI_40));
+	} else {
+		brcms_c_ht_update_sgi_rx(wlc, 0);
+	}
+
+	/* initialize radio_mpc_disable according to wlc->mpc */
+	brcms_c_radio_mpc_upd(wlc);
+	brcms_b_antsel_set(wlc->hw, wlc->asi->antsel_avail);
+
+	if (perr)
+		*perr = 0;
+
+	return wlc;
 
-	return simple_strtoul(val, NULL, 0);
+ fail:
+	wiphy_err(wl->wiphy, "wl%d: %s: failed with err %d\n",
+		  unit, __func__, err);
+	if (wlc)
+		brcms_c_detach(wlc);
+
+	if (perr)
+		*perr = err;
+	return NULL;
 }
diff --git a/drivers/staging/brcm80211/brcmsmac/main.h b/drivers/staging/brcm80211/brcmsmac/main.h
index f204b1f47475..c0e0fcfdfaf8 100644
--- a/drivers/staging/brcm80211/brcmsmac/main.h
+++ b/drivers/staging/brcm80211/brcmsmac/main.h
@@ -22,35 +22,23 @@
 #include <brcmu_utils.h>
 #include "types.h"
 #include "d11.h"
+#include "scb.h"
 
-#define MA_WINDOW_SZ		8	/* moving average window size */
-#define	BRCMS_HWRXOFF		38	/* chip rx buffer offset */
 #define	INVCHANNEL		255	/* invalid channel */
-/* max # supported core revisions (0 .. MAXCOREREV - 1) */
-#define	MAXCOREREV		28
+
 /* max # brcms_c_module_register() calls */
 #define BRCMS_MAXMODULES	22
 
 #define SEQNUM_SHIFT		4
-#define AMPDU_DELIMITER_LEN	4
 #define SEQNUM_MAX		0x1000
 
-#define	APHY_CWMIN		15
-#define PHY_CWMAX		1023
-
-#define EDCF_AIFSN_MIN               1
-#define FRAGNUM_MASK		0xF
-
 #define NTXRATE			64	/* # tx MPDUs rate is reported for */
 
-#define BRCMS_BITSCNT(x)	brcmu_bitcount((u8 *)&(x), sizeof(u8))
-
 /* Maximum wait time for a MAC suspend */
 /* uS: 83mS is max packet time (64KB ampdu @ 6Mbps) */
 #define	BRCMS_MAX_MAC_SUSPEND	83000
 
-/* Probe Response timeout - responses for probe requests older that this are tossed, zero to disable
- */
+/* responses for probe requests older that this are tossed, zero to disable */
 #define BRCMS_PRB_RESP_TIMEOUT	0	/* Disable probe response timeout */
 
 /* transmit buffer max headroom for protocol headers */
@@ -78,13 +66,14 @@
 
 #define	SW_TIMER_MAC_STAT_UPD		30	/* periodic MAC stats update */
 
+/* max # supported core revisions (0 .. MAXCOREREV - 1) */
+#define	MAXCOREREV		28
+
 /* Double check that unsupported cores are not enabled */
 #if CONF_MSK(D11CONF, 0x4f) || CONF_GE(D11CONF, MAXCOREREV)
 #error "Configuration for D11CONF includes unsupported versions."
 #endif				/* Bad versions */
 
-#define	VALID_COREREV(corerev)	CONF_HAS(D11CONF, corerev)
-
 /* values for shortslot_override */
 #define BRCMS_SHORTSLOT_AUTO	-1 /* Driver will manage Shortslot setting */
 #define BRCMS_SHORTSLOT_OFF	0  /* Turn off short slot */
@@ -98,17 +87,6 @@
 #define BRCMS_IS_MIMO_PREAMBLE(_pre) (((_pre) == BRCMS_GF_PREAMBLE) || \
 				      ((_pre) == BRCMS_MM_PREAMBLE))
 
-/* values for barker_preamble */
-#define BRCMS_BARKER_SHORT_ALLOWED	0	/* Short pre-amble allowed */
-
-/* A fifo is full. Clear precedences related to that FIFO */
-#define BRCMS_TX_FIFO_CLEAR(wlc, fifo) \
-			((wlc)->tx_prec_map &= ~(wlc)->fifo2prec_map[fifo])
-
-/* Fifo is NOT full. Enable precedences for that FIFO */
-#define BRCMS_TX_FIFO_ENAB(wlc, fifo) \
-			((wlc)->tx_prec_map |= (wlc)->fifo2prec_map[fifo])
-
 /* TxFrameID */
 /* seq and frag bits: SEQNUM_SHIFT, FRAGNUM_MASK (802.11.h) */
 /* rate epoch bits: TXFID_RATE_SHIFT, TXFID_RATE_MASK ((wlc_rate.c) */
@@ -123,18 +101,6 @@
 #define BOARDREV_PROMOTABLE	0xFF	/* from */
 #define BOARDREV_PROMOTED	1	/* to */
 
-/* if wpa is in use then portopen is true when the group key is plumbed otherwise it is always true
- */
-#define WSEC_ENABLED(wsec) ((wsec) & (WEP_ENABLED | TKIP_ENABLED | AES_ENABLED))
-#define BRCMS_SW_KEYS(wlc, bsscfg) ((((wlc)->wsec_swkeys) || \
-	((bsscfg)->wsec & WSEC_SWFLAG)))
-
-#define BRCMS_PORTOPEN(cfg) \
-	(((cfg)->WPA_auth != WPA_AUTH_DISABLED && WSEC_ENABLED((cfg)->wsec)) ? \
-	(cfg)->wsec_portopen : true)
-
-#define PS_ALLOWED(wlc)	brcms_c_ps_allowed(wlc)
-
 #define DATA_BLOCK_TX_SUPR	(1 << 4)
 
 /* 802.1D Priority to TX FIFO number for wme */
@@ -160,48 +126,14 @@ extern const u8 prio2fifo[];
 			 MI_PHYTXERR | MI_DMAINT | MI_TFS | MI_BG_NOISE | \
 			 MI_CCA | MI_TO | MI_GP0 | MI_RFDISABLE | MI_PWRUP)
 
-#define	RETRY_SHORT_DEF			7	/* Default Short retry Limit */
-#define	RETRY_SHORT_MAX			255	/* Maximum Short retry Limit */
-#define	RETRY_LONG_DEF			4	/* Default Long retry count */
-#define	RETRY_SHORT_FB			3	/* Short retry count for fallback rate */
-#define	RETRY_LONG_FB			2	/* Long retry count for fallback rate */
-
 #define	MAXTXPKTS		6	/* max # pkts pending */
 
 /* frameburst */
-#define	MAXTXFRAMEBURST		8	/* vanilla xpress mode: max frames/burst */
+#define	MAXTXFRAMEBURST		8 /* vanilla xpress mode: max frames/burst */
 #define	MAXFRAMEBURST_TXOP	10000	/* Frameburst TXOP in usec */
 
-/* Per-AC retry limit register definitions; uses defs.h bitfield macros */
-#define EDCF_SHORT_S            0
-#define EDCF_SFB_S              4
-#define EDCF_LONG_S             8
-#define EDCF_LFB_S              12
-#define EDCF_SHORT_M            BITFIELD_MASK(4)
-#define EDCF_SFB_M              BITFIELD_MASK(4)
-#define EDCF_LONG_M             BITFIELD_MASK(4)
-#define EDCF_LFB_M              BITFIELD_MASK(4)
-
 #define	NFIFO			6	/* # tx/rx fifopairs */
 
-#define BRCMS_WME_RETRY_SHORT_GET(wlc, ac) \
-					GFIELD(wlc->wme_retries[ac], EDCF_SHORT)
-#define BRCMS_WME_RETRY_SFB_GET(wlc, ac) \
-					GFIELD(wlc->wme_retries[ac], EDCF_SFB)
-#define BRCMS_WME_RETRY_LONG_GET(wlc, ac) \
-					GFIELD(wlc->wme_retries[ac], EDCF_LONG)
-#define BRCMS_WME_RETRY_LFB_GET(wlc, ac) \
-					GFIELD(wlc->wme_retries[ac], EDCF_LFB)
-
-#define BRCMS_WME_RETRY_SHORT_SET(wlc, ac, val) \
-	(wlc->wme_retries[ac] = SFIELD(wlc->wme_retries[ac], EDCF_SHORT, val))
-#define BRCMS_WME_RETRY_SFB_SET(wlc, ac, val) \
-	(wlc->wme_retries[ac] = SFIELD(wlc->wme_retries[ac], EDCF_SFB, val))
-#define BRCMS_WME_RETRY_LONG_SET(wlc, ac, val) \
-	(wlc->wme_retries[ac] = SFIELD(wlc->wme_retries[ac], EDCF_LONG, val))
-#define BRCMS_WME_RETRY_LFB_SET(wlc, ac, val) \
-	(wlc->wme_retries[ac] = SFIELD(wlc->wme_retries[ac], EDCF_LFB, val))
-
 /* PLL requests */
 
 /* pll is shared on old chips */
@@ -211,83 +143,86 @@ extern const u8 prio2fifo[];
 /* hold/release pll for some short operation */
 #define BRCMS_PLLREQ_FLIP		0x4
 
-/*
- * Macros to check if AP or STA is active.
- * AP Active means more than just configured: driver and BSS are "up";
- * that is, we are beaconing/responding as an AP (aps_associated).
- * STA Active similarly means the driver is up and a configured STA BSS
- * is up: either associated (stas_associated) or trying.
- *
- * Macro definitions vary as per AP/STA ifdefs, allowing references to
- * ifdef'd structure fields and constant values (0) for optimization.
- * Make sure to enclose blocks of code such that any routines they
- * reference can also be unused and optimized out by the linker.
- */
-/* NOTE: References structure fields defined in wlc.h */
-#define AP_ACTIVE(wlc)	(0)
-
-/*
- * Detect Card removed.
- * Even checking an sbconfig register read will not false trigger when the core is in reset.
- * it breaks CF address mechanism. Accessing gphy phyversion will cause SB error if aphy
- * is in reset on 4306B0-DB. Need a simple accessible reg with fixed 0/1 pattern
- * (some platforms return all 0).
- * If clocks are present, call the sb routine which will figure out if the device is removed.
- */
-#define DEVICEREMOVED(wlc)      \
-	((wlc->hw->clk) ?   \
-	((R_REG(&wlc->hw->regs->maccontrol) & \
-	(MCTL_PSM_JMP_0 | MCTL_IHR_EN)) != MCTL_IHR_EN) : \
-	(ai_deviceremoved(wlc->hw->sih)))
+#define	CHANNEL_BANDUNIT(wlc, ch) \
+	(((ch) <= CH_MAX_2G_CHANNEL) ? BAND_2G_INDEX : BAND_5G_INDEX)
 
-#define BRCMS_UNIT(wlc)		((wlc)->pub->unit)
+#define	OTHERBANDUNIT(wlc) \
+	((uint)((wlc)->band->bandunit ? BAND_2G_INDEX : BAND_5G_INDEX))
 
+/*
+ * 802.11 protection information
+ *
+ * _g: use g spec protection, driver internal.
+ * g_override: override for use of g spec protection.
+ * gmode_user: user config gmode, operating band->gmode is different.
+ * overlap: Overlap BSS/IBSS protection for both 11g and 11n.
+ * nmode_user: user config nmode, operating pub->nmode is different.
+ * n_cfg: use OFDM protection on MIMO frames.
+ * n_cfg_override: override for use of N protection.
+ * nongf: non-GF present protection.
+ * nongf_override: override for use of GF protection.
+ * n_pam_override: override for preamble: MM or GF.
+ * n_obss: indicated OBSS Non-HT STA present.
+*/
 struct brcms_protection {
-	bool _g;		/* use g spec protection, driver internal */
-	s8 g_override;	/* override for use of g spec protection */
-	u8 gmode_user;	/* user config gmode, operating band->gmode is different */
-	s8 overlap;		/* Overlap BSS/IBSS protection for both 11g and 11n */
-	s8 nmode_user;	/* user config nmode, operating pub->nmode is different */
-	s8 n_cfg;		/* use OFDM protection on MIMO frames */
-	s8 n_cfg_override;	/* override for use of N protection */
-	bool nongf;		/* non-GF present protection */
-	s8 nongf_override;	/* override for use of GF protection */
-	s8 n_pam_override;	/* override for preamble: MM or GF */
-	bool n_obss;		/* indicated OBSS Non-HT STA present */
+	bool _g;
+	s8 g_override;
+	u8 gmode_user;
+	s8 overlap;
+	s8 nmode_user;
+	s8 n_cfg;
+	s8 n_cfg_override;
+	bool nongf;
+	s8 nongf_override;
+	s8 n_pam_override;
+	bool n_obss;
 };
 
-/* anything affects the single/dual streams/antenna operation */
+/*
+ * anything affecting the single/dual streams/antenna operation
+ *
+ * hw_txchain: HW txchain bitmap cfg.
+ * txchain: txchain bitmap being used.
+ * txstreams: number of txchains being used.
+ * hw_rxchain: HW rxchain bitmap cfg.
+ * rxchain: rxchain bitmap being used.
+ * rxstreams: number of rxchains being used.
+ * ant_rx_ovr: rx antenna override.
+ * txant: userTx antenna setting.
+ * phytxant: phyTx antenna setting in txheader.
+ * ss_opmode: singlestream Operational mode, 0:siso; 1:cdd.
+ * ss_algosel_auto: if true, use wlc->stf->ss_algo_channel;
+ *			else use wlc->band->stf->ss_mode_band.
+ * ss_algo_channel: ss based on per-channel algo: 0: SISO, 1: CDD 2: STBC.
+ * rxchain_restore_delay: delay time to restore default rxchain.
+ * ldpc: AUTO/ON/OFF ldpc cap supported.
+ * txcore[MAX_STREAMS_SUPPORTED + 1]: bitmap of selected core for each Nsts.
+ * spatial_policy:
+ */
 struct brcms_stf {
-	u8 hw_txchain;	/* HW txchain bitmap cfg */
-	u8 txchain;		/* txchain bitmap being used */
-	u8 txstreams;	/* number of txchains being used */
-
-	u8 hw_rxchain;	/* HW rxchain bitmap cfg */
-	u8 rxchain;		/* rxchain bitmap being used */
-	u8 rxstreams;	/* number of rxchains being used */
-
-	u8 ant_rx_ovr;	/* rx antenna override */
-	s8 txant;		/* userTx antenna setting */
-	u16 phytxant;	/* phyTx antenna setting in txheader */
-
-	u8 ss_opmode;	/* singlestream Operational mode, 0:siso; 1:cdd */
-	bool ss_algosel_auto;	/* if true, use wlc->stf->ss_algo_channel; */
-	/* else use wlc->band->stf->ss_mode_band; */
-	u16 ss_algo_channel;	/* ss based on per-channel algo: 0: SISO, 1: CDD 2: STBC */
-	u8 no_cddstbc;	/* stf override, 1: no CDD (or STBC) allowed */
-
-	u8 rxchain_restore_delay;	/* delay time to restore default rxchain */
-
-	s8 ldpc;		/* AUTO/ON/OFF ldpc cap supported */
-	u8 txcore[MAX_STREAMS_SUPPORTED + 1];	/* bitmap of selected core for each Nsts */
+	u8 hw_txchain;
+	u8 txchain;
+	u8 txstreams;
+	u8 hw_rxchain;
+	u8 rxchain;
+	u8 rxstreams;
+	u8 ant_rx_ovr;
+	s8 txant;
+	u16 phytxant;
+	u8 ss_opmode;
+	bool ss_algosel_auto;
+	u16 ss_algo_channel;
+	u8 rxchain_restore_delay;
+	s8 ldpc;
+	u8 txcore[MAX_STREAMS_SUPPORTED + 1];
 	s8 spatial_policy;
 };
 
 #define BRCMS_STF_SS_STBC_TX(wlc, scb) \
-	(((wlc)->stf->txstreams > 1) && (((wlc)->band->band_stf_stbc_tx == ON) || \
-	 (SCB_STBC_CAP((scb)) &&					\
-	  (wlc)->band->band_stf_stbc_tx == AUTO &&			\
-	  isset(&((wlc)->stf->ss_algo_channel), PHY_TXC1_MODE_STBC))))
+	(((wlc)->stf->txstreams > 1) && (((wlc)->band->band_stf_stbc_tx == ON) \
+	 || (((scb)->flags & SCB_STBCCAP) && \
+	     (wlc)->band->band_stf_stbc_tx == AUTO && \
+	     isset(&((wlc)->stf->ss_algo_channel), PHY_TXC1_MODE_STBC))))
 
 #define BRCMS_STBC_CAP_PHY(wlc) (BRCMS_ISNPHY(wlc->band) && \
 				 NREV_GE(wlc->band->phyrev, 3))
@@ -300,60 +235,6 @@ struct brcms_stf {
 				   >> RXS_CHAN_PHYTYPE_SHIFT)
 #define BRCMS_CHAN_CHANNEL(x)     (((x) & RXS_CHAN_ID_MASK) \
 				   >> RXS_CHAN_ID_SHIFT)
-#define BRCMS_RX_CHANNEL(rxh)	(BRCMS_CHAN_CHANNEL((rxh)->RxChan))
-
-/* brcms_bss_info flag bit values */
-#define BRCMS_BSS_HT		0x0020	/* BSS is HT (MIMO) capable */
-
-/* Flags used in brcms_c_txq_info.stopped */
-#define TXQ_STOP_FOR_PRIOFC_MASK	0x000000FF	/* per prio flow control bits */
-#define TXQ_STOP_FOR_PKT_DRAIN		0x00000100	/* stop txq enqueue for packet drain */
-#define TXQ_STOP_FOR_AMPDU_FLOW_CNTRL	0x00000200	/* stop txq enqueue for ampdu flow control */
-
-#define BRCMS_HT_WEP_RESTRICT	0x01	/* restrict HT with WEP */
-#define BRCMS_HT_TKIP_RESTRICT	0x02	/* restrict HT with TKIP */
-
-/* Maximum # of keys that wl driver supports in S/W.
- * Keys supported in H/W is less than or equal to WSEC_MAX_KEYS.
- */
-#define WSEC_MAX_KEYS		54	/* Max # of keys (50 + 4 default keys) */
-#define BRCMS_DEFAULT_KEYS	4	/* Default # of keys */
-
-/*
-* Max # of keys currently supported:
-*
-*     s/w keys if WSEC_SW(wlc->wsec).
-*     h/w keys otherwise.
-*/
-#define BRCMS_MAX_WSEC_KEYS(wlc) WSEC_MAX_KEYS
-
-/* number of 802.11 default (non-paired, group keys) */
-#define WSEC_MAX_DEFAULT_KEYS	4	/* # of default keys */
-
-struct wsec_iv {
-	u32 hi;		/* upper 32 bits of IV */
-	u16 lo;		/* lower 16 bits of IV */
-};
-
-#define BRCMS_NUMRXIVS	16	/* # rx IVs (one per 802.11e TID) */
-
-struct wsec_key {
-	u8 ea[ETH_ALEN];	/* per station */
-	u8 idx;		/* key index in wsec_keys array */
-	u8 id;		/* key ID [0-3] */
-	u8 algo;		/* CRYPTO_ALGO_AES_CCM, CRYPTO_ALGO_WEP128, etc */
-	u8 rcmta;		/* rcmta entry index, same as idx by default */
-	u16 flags;		/* misc flags */
-	u8 algo_hw;		/* cache for hw register */
-	u8 aes_mode;		/* cache for hw register */
-	s8 iv_len;		/* IV length */
-	s8 icv_len;		/* ICV length */
-	u32 len;		/* key length..don't move this var */
-	/* data is 4byte aligned */
-	u8 data[WLAN_MAX_KEY_LEN];	/* key data */
-	struct wsec_iv rxiv[BRCMS_NUMRXIVS];	/* Rx IV (one per TID) */
-	struct wsec_iv txiv;		/* Tx IV */
-};
 
 /*
  * core state (mac)
@@ -386,92 +267,37 @@ struct brcms_band {
 
 	struct scb *hwrs_scb;	/* permanent scb for hw rateset */
 
-	wlc_rateset_t defrateset;	/* band-specific copy of default_bss.rateset */
+	/* band-specific copy of default_bss.rateset */
+	struct brcms_c_rateset defrateset;
 
-	ratespec_t rspec_override;	/* 802.11 rate override */
-	ratespec_t mrspec_override;	/* multicast rate override */
 	u8 band_stf_ss_mode;	/* Configured STF type, 0:siso; 1:cdd */
 	s8 band_stf_stbc_tx;	/* STBC TX 0:off; 1:force on; -1:auto */
-	wlc_rateset_t hw_rateset;	/* rates supported by chip (phy-specific) */
+	/* rates supported by chip (phy-specific) */
+	struct brcms_c_rateset hw_rateset;
 	u8 basic_rate[BRCM_MAXRATE + 1]; /* basic rates indexed by rate */
 	bool mimo_cap_40;	/* 40 MHz cap enabled on this band */
 	s8 antgain;		/* antenna gain from srom */
 
-	u16 CWmin;		/* The minimum size of contention window, in unit of aSlotTime */
-	u16 CWmax;		/* The maximum size of contention window, in unit of aSlotTime */
-	u16 bcntsfoff;	/* beacon tsf offset */
-};
-
-/* tx completion callback takes 3 args */
-typedef void (*pkcb_fn_t) (struct brcms_c_info *wlc, uint txstatus, void *arg);
-
-struct pkt_cb {
-	pkcb_fn_t fn;		/* function to call when tx frame completes */
-	void *arg;		/* void arg for fn */
-	u8 nextidx;		/* index of next call back if threading */
-	bool entered;		/* recursion check */
+	u16 CWmin; /* minimum size of contention window, in unit of aSlotTime */
+	u16 CWmax; /* maximum size of contention window, in unit of aSlotTime */
+	struct ieee80211_supported_band band;
 };
 
 /* module control blocks */
 struct modulecb {
-	char name[32];		/* module name : NULL indicates empty array member */
-	const struct brcmu_iovar *iovars;	/* iovar table */
-	void *hdl;		/* handle passed when handler 'doiovar' is called */
-	watchdog_fn_t watchdog_fn;	/* watchdog handler */
-	iovar_fn_t iovar_fn;	/* iovar handler */
-	down_fn_t down_fn;	/* down handler. Note: the int returned
-				 * by the down function is a count of the
-				 * number of timers that could not be
-				 * freed.
-				 */
-};
+	/* module name : NULL indicates empty array member */
+	char name[32];
+	/* handle passed when handler 'doiovar' is called */
+	struct brcms_info *hdl;
 
-/* dump control blocks */
-struct dumpcb_s {
-	const char *name;	/* dump name */
-	dump_fn_t dump_fn;	/* 'wl dump' handler */
-	void *dump_fn_arg;
-	struct dumpcb_s *next;
-};
+	int (*down_fn)(void *handle); /* down handler. Note: the int returned
+				       * by the down function is a count of the
+				       * number of timers that could not be
+				       * freed.
+				       */
 
-struct edcf_acparam {
-	u8 ACI;
-	u8 ECW;
-	u16 TXOP;
-} __packed;
-
-struct wme_param_ie {
-	u8 oui[3];
-	u8 type;
-	u8 subtype;
-	u8 version;
-	u8 qosinfo;
-	u8 rsvd;
-	struct edcf_acparam acparam[AC_COUNT];
-} __packed;
-
-/* virtual interface */
-struct brcms_c_if {
-	struct brcms_c_if *next;
-	u8 type;		/* BSS or WDS */
-	u8 index;		/* assigned in wl_add_if(), index of the wlif if any,
-				 * not necessarily corresponding to bsscfg._idx or
-				 * AID2PVBMAP(scb).
-				 */
-	u8 flags;		/* flags for the interface */
-	struct brcms_if *wlif;		/* pointer to wlif */
-	struct brcms_txq_info *qi;	/* pointer to associated tx queue */
-	union {
-		/* pointer to scb if WDS */
-		struct scb *scb;
-		/* pointer to bsscfg if BSS */
-		struct brcms_bss_cfg *bsscfg;
-	} u;
 };
 
-/* flags for the interface, this interface is linked to a brcms_if */
-#define BRCMS_IF_LINKED		0x02
-
 struct brcms_hw_band {
 	int bandtype;		/* BRCM_BAND_2G, BRCM_BAND_5G */
 	uint bandunit;		/* bandstate[] index */
@@ -508,14 +334,11 @@ struct brcms_hardware {
 	u32 boardflags2;	/* More board flags if sromrev >= 4 */
 	u32 machwcap;	/* MAC capabilities */
 	u32 machwcap_backup;	/* backup of machwcap */
-	u16 ucode_dbgsel;	/* dbgsel for ucode debug(config gpio) */
 
 	struct si_pub *sih;	/* SI handle (cookie for siutils calls) */
-	char *vars;		/* "environment" name=value */
-	uint vars_size;		/* size of vars, free vars on detach */
-	d11regs_t *regs;	/* pointer to device registers */
-	void *physhim;		/* phy shim layer handler */
-	void *phy_sh;		/* pointer to shared phy state */
+	struct d11regs __iomem *regs;	/* pointer to device registers */
+	struct phy_shim_info *physhim; /* phy shim layer handler */
+	struct shared_phy *phy_sh;	/* pointer to shared phy state */
 	struct brcms_hw_band *band;/* pointer to active per-band state */
 	/* band state per phy/radio */
 	struct brcms_hw_band *bandstate[MAXBANDS];
@@ -529,27 +352,24 @@ struct brcms_hardware {
 	bool up;		/* d11 hardware up and running */
 	uint now;		/* # elapsed seconds */
 	uint _nbands;		/* # bands supported */
-	chanspec_t chanspec;	/* bmac chanspec shadow */
+	u16 chanspec;	/* bmac chanspec shadow */
 
 	uint *txavail[NFIFO];	/* # tx descriptors available */
-	u16 *xmtfifo_sz;	/* fifo size in 256B for each xmt fifo */
+	const u16 *xmtfifo_sz;	/* fifo size in 256B for each xmt fifo */
 
-	mbool pllreq;		/* pll requests to keep PLL on */
+	u32 pllreq;		/* pll requests to keep PLL on */
 
 	u8 suspended_fifos;	/* Which TX fifo to remain awake for */
 	u32 maccontrol;	/* Cached value of maccontrol */
 	uint mac_suspend_depth;	/* current depth of mac_suspend levels */
-	u32 wake_override;	/* Various conditions to force MAC to WAKE mode */
+	u32 wake_override;	/* bit flags to force MAC to WAKE mode */
 	u32 mute_override;	/* Prevent ucode from sending beacons */
 	u8 etheraddr[ETH_ALEN];	/* currently configured ethernet address */
-	u32 led_gpio_mask;	/* LED GPIO Mask */
 	bool noreset;		/* true= do not reset hw, used by WLC_OUT */
-	bool forcefastclk;	/* true if the h/w is forcing the use of fast clk */
+	bool forcefastclk;	/* true if h/w is forcing to use fast clk */
 	bool clk;		/* core is out of reset and has clock */
 	bool sbclk;		/* sb has clock */
-	struct bmac_pmq *bmac_pmq; /*  bmac PM states derived from ucode PMQ */
 	bool phyclk;		/* phy is out of reset and has clock */
-	bool dma_lpbk;		/* core is in DMA loopback */
 
 	bool ucode_loaded;	/* true after ucode downloaded */
 
@@ -578,195 +398,212 @@ struct brcms_txq_info {
 };
 
 /*
- * Principal common (os-independent) software data structure.
+ * Principal common driver data structure.
+ *
+ * pub: pointer to driver public state.
+ * wl: pointer to specific private state.
+ * regs: pointer to device registers.
+ * hw: HW related state.
+ * clkreq_override: setting for clkreq for PCIE : Auto, 0, 1.
+ * fastpwrup_dly: time in us needed to bring up d11 fast clock.
+ * macintstatus: bit channel between isr and dpc.
+ * macintmask: sw runtime master macintmask value.
+ * defmacintmask: default "on" macintmask value.
+ * clk: core is out of reset and has clock.
+ * core: pointer to active io core.
+ * band: pointer to active per-band state.
+ * corestate: per-core state (one per hw core).
+ * bandstate: per-band state (one per phy/radio).
+ * qvalid: DirFrmQValid and BcMcFrmQValid.
+ * ampdu: ampdu module handler.
+ * asi: antsel module handler.
+ * cmi: channel manager module handler.
+ * vendorid: PCI vendor id.
+ * deviceid: PCI device id.
+ * ucode_rev: microcode revision.
+ * machwcap: MAC capabilities, BMAC shadow.
+ * perm_etheraddr: original sprom local ethernet address.
+ * bandlocked: disable auto multi-band switching.
+ * bandinit_pending: track band init in auto band.
+ * radio_monitor: radio timer is running.
+ * going_down: down path intermediate variable.
+ * mpc: enable minimum power consumption.
+ * mpc_dlycnt: # of watchdog cnt before turn disable radio.
+ * mpc_offcnt: # of watchdog cnt that radio is disabled.
+ * mpc_delay_off: delay radio disable by # of watchdog cnt.
+ * prev_non_delay_mpc: prev state brcms_c_is_non_delay_mpc.
+ * wdtimer: timer for watchdog routine.
+ * radio_timer: timer for hw radio button monitor routine.
+ * monitor: monitor (MPDU sniffing) mode.
+ * bcnmisc_monitor: bcns promisc mode override for monitor.
+ * _rifs: enable per-packet rifs.
+ * bcn_li_bcn: beacon listen interval in # beacons.
+ * bcn_li_dtim: beacon listen interval in # dtims.
+ * WDarmed: watchdog timer is armed.
+ * WDlast: last time wlc_watchdog() was called.
+ * edcf_txop[AC_COUNT]: current txop for each ac.
+ * wme_retries: per-AC retry limits.
+ * tx_prec_map: Precedence map based on HW FIFO space.
+ * fifo2prec_map[NFIFO]: pointer to fifo2_prec map based on WME.
+ * bsscfg: set of BSS configurations, idx 0 is default and always valid.
+ * cfg: the primary bsscfg (can be AP or STA).
+ * tx_queues: common TX Queue list.
+ * modulecb:
+ * mimoft: SIGN or 11N.
+ * cck_40txbw: 11N, cck tx b/w override when in 40MHZ mode.
+ * ofdm_40txbw: 11N, ofdm tx b/w override when in 40MHZ mode.
+ * mimo_40txbw: 11N, mimo tx b/w override when in 40MHZ mode.
+ * default_bss: configured BSS parameters.
+ * mc_fid_counter: BC/MC FIFO frame ID counter.
+ * country_default: saved country for leaving 802.11d auto-country mode.
+ * autocountry_default: initial country for 802.11d auto-country mode.
+ * prb_resp_timeout: do not send prb resp if request older
+ *		     than this, 0 = disable.
+ * home_chanspec: shared home chanspec.
+ * chanspec: target operational channel.
+ * usr_fragthresh: user configured fragmentation threshold.
+ * fragthresh[NFIFO]: per-fifo fragmentation thresholds.
+ * RTSThresh: 802.11 dot11RTSThreshold.
+ * SRL: 802.11 dot11ShortRetryLimit.
+ * LRL: 802.11 dot11LongRetryLimit.
+ * SFBL: Short Frame Rate Fallback Limit.
+ * LFBL: Long Frame Rate Fallback Limit.
+ * shortslot: currently using 11g ShortSlot timing.
+ * shortslot_override: 11g ShortSlot override.
+ * include_legacy_erp: include Legacy ERP info elt ID 47 as well as g ID 42.
+ * PLCPHdr_override: 802.11b Preamble Type override.
+ * stf:
+ * bcn_rspec: save bcn ratespec purpose.
+ * tempsense_lasttime;
+ * tx_duty_cycle_ofdm: maximum allowed duty cycle for OFDM.
+ * tx_duty_cycle_cck: maximum allowed duty cycle for CCK.
+ * pkt_queue: txq for transmit packets.
+ * wiphy:
+ * pri_scb: primary Station Control Block
  */
 struct brcms_c_info {
-	struct brcms_pub *pub;		/* pointer to wlc public state */
-	struct brcms_info *wl;	/* pointer to os-specific private state */
-	d11regs_t *regs;	/* pointer to device registers */
-
-	/* HW related state used primarily by BMAC */
+	struct brcms_pub *pub;
+	struct brcms_info *wl;
+	struct d11regs __iomem *regs;
 	struct brcms_hardware *hw;
 
 	/* clock */
-	int clkreq_override;	/* setting for clkreq for PCIE : Auto, 0, 1 */
-	u16 fastpwrup_dly;	/* time in us needed to bring up d11 fast clock */
+	u16 fastpwrup_dly;
 
 	/* interrupt */
-	u32 macintstatus;	/* bit channel between isr and dpc */
-	u32 macintmask;	/* sw runtime master macintmask value */
-	u32 defmacintmask;	/* default "on" macintmask value */
-
-	/* up and down */
-	bool device_present;	/* (removable) device is present */
+	u32 macintstatus;
+	u32 macintmask;
+	u32 defmacintmask;
 
-	bool clk;		/* core is out of reset and has clock */
+	bool clk;
 
 	/* multiband */
-	struct brcms_core *core;	/* pointer to active io core */
-	struct brcms_band *band;	/* pointer to active per-band state */
-	struct brcms_core *corestate;	/* per-core state (one per hw core) */
-	/* per-band state (one per phy/radio): */
+	struct brcms_core *core;
+	struct brcms_band *band;
+	struct brcms_core *corestate;
 	struct brcms_band *bandstate[MAXBANDS];
 
-	bool war16165;		/* PCI slow clock 16165 war flag */
-
-	bool tx_suspended;	/* data fifos need to remain suspended */
-
-	uint txpend16165war;
-
 	/* packet queue */
-	uint qvalid;		/* DirFrmQValid and BcMcFrmQValid */
-
-	/* Regulatory power limits */
-	s8 txpwr_local_max;	/* regulatory local txpwr max */
-	u8 txpwr_local_constraint;	/* local power contraint in dB */
-
-
-	struct ampdu_info *ampdu;	/* ampdu module handler */
-	struct antsel_info *asi;	/* antsel module handler */
-	struct brcms_cm_info *cmi;	/* channel manager module handler */
+	uint qvalid;
 
-	uint vars_size;		/* size of vars, free vars on detach */
-
-	u16 vendorid;	/* PCI vendor id */
-	u16 deviceid;	/* PCI device id */
-	uint ucode_rev;		/* microcode revision */
+	struct ampdu_info *ampdu;
+	struct antsel_info *asi;
+	struct brcms_cm_info *cmi;
 
-	u32 machwcap;	/* MAC capabilities, BMAC shadow */
+	u16 vendorid;
+	u16 deviceid;
+	uint ucode_rev;
 
-	u8 perm_etheraddr[ETH_ALEN];	/* original sprom local ethernet address */
+	u8 perm_etheraddr[ETH_ALEN];
 
-	bool bandlocked;	/* disable auto multi-band switching */
-	bool bandinit_pending;	/* track band init in auto band */
+	bool bandlocked;
+	bool bandinit_pending;
 
-	bool radio_monitor;	/* radio timer is running */
-	bool going_down;	/* down path intermediate variable */
+	bool radio_monitor;
+	bool going_down;
 
-	bool mpc;		/* enable minimum power consumption */
-	u8 mpc_dlycnt;	/* # of watchdog cnt before turn disable radio */
-	u8 mpc_offcnt;	/* # of watchdog cnt that radio is disabled */
-	u8 mpc_delay_off;	/* delay radio disable by # of watchdog cnt */
-	u8 prev_non_delay_mpc;	/* prev state brcms_c_is_non_delay_mpc */
+	bool mpc;
+	u8 mpc_dlycnt;
+	u8 mpc_offcnt;
+	u8 mpc_delay_off;
+	u8 prev_non_delay_mpc;
 
-	/* timer for watchdog routine */
 	struct brcms_timer *wdtimer;
-	/* timer for hw radio button monitor routine */
 	struct brcms_timer *radio_timer;
 
 	/* promiscuous */
-	bool monitor;		/* monitor (MPDU sniffing) mode */
-	bool bcnmisc_ibss;	/* bcns promisc mode override for IBSS */
-	bool bcnmisc_scan;	/* bcns promisc mode override for scan */
-	bool bcnmisc_monitor;	/* bcns promisc mode override for monitor */
+	bool monitor;
+	bool bcnmisc_monitor;
 
 	/* driver feature */
-	bool _rifs;		/* enable per-packet rifs */
-	s8 sgi_tx;		/* sgi tx */
+	bool _rifs;
 
 	/* AP-STA synchronization, power save */
-	u8 bcn_li_bcn;	/* beacon listen interval in # beacons */
-	u8 bcn_li_dtim;	/* beacon listen interval in # dtims */
+	u8 bcn_li_bcn;
+	u8 bcn_li_dtim;
 
-	bool WDarmed;		/* watchdog timer is armed */
-	u32 WDlast;		/* last time wlc_watchdog() was called */
+	bool WDarmed;
+	u32 WDlast;
 
 	/* WME */
-	ac_bitmap_t wme_dp;	/* Discard (oldest first) policy per AC */
-	u16 edcf_txop[AC_COUNT];	/* current txop for each ac */
-
-	/*
-	 * WME parameter info element, which on STA contains parameters in use
-	 * locally, and on AP contains parameters advertised to STA in beacons
-	 * and assoc responses.
-	 */
-	struct wme_param_ie wme_param_ie;
-	u16 wme_retries[AC_COUNT];	/* per-AC retry limits */
-
-	u16 tx_prec_map;	/* Precedence map based on HW FIFO space */
-	u16 fifo2prec_map[NFIFO];	/* pointer to fifo2_prec map based on WME */
-
-	/*
-	 * BSS Configurations set of BSS configurations, idx 0 is default and
-	 * always valid
-	 */
-	struct brcms_bss_cfg *bsscfg[BRCMS_MAXBSSCFG];
-	struct brcms_bss_cfg *cfg; /* the primary bsscfg (can be AP or STA) */
+	u16 edcf_txop[AC_COUNT];
+
+	u16 wme_retries[AC_COUNT];
+	u16 tx_prec_map;
+	u16 fifo2prec_map[NFIFO];
+
+	struct brcms_bss_cfg *bsscfg;
 
 	/* tx queue */
-	struct brcms_txq_info *tx_queues;	/* common TX Queue list */
-
-	/* security */
-	struct wsec_key *wsec_keys[WSEC_MAX_KEYS]; /* dynamic key storage */
-	/* default key storage */
-	struct wsec_key *wsec_def_keys[BRCMS_DEFAULT_KEYS];
-	bool wsec_swkeys;	/* indicates that all keys should be
-				 * treated as sw keys (used for debugging)
-				 */
+	struct brcms_txq_info *tx_queues;
+
 	struct modulecb *modulecb;
 
-	u8 mimoft;		/* SIGN or 11N */
-	s8 cck_40txbw;	/* 11N, cck tx b/w override when in 40MHZ mode */
-	s8 ofdm_40txbw;	/* 11N, ofdm tx b/w override when in 40MHZ mode */
-	s8 mimo_40txbw;	/* 11N, mimo tx b/w override when in 40MHZ mode */
-	/* HT CAP IE being advertised by this node: */
-	struct ieee80211_ht_cap ht_cap;
+	u8 mimoft;
+	s8 cck_40txbw;
+	s8 ofdm_40txbw;
+	s8 mimo_40txbw;
 
-	struct brcms_bss_info *default_bss;	/* configured BSS parameters */
+	struct brcms_bss_info *default_bss;
 
-	u16 mc_fid_counter;	/* BC/MC FIFO frame ID counter */
+	u16 mc_fid_counter;
 
-	/* saved country for leaving 802.11d auto-country mode */
 	char country_default[BRCM_CNTRY_BUF_SZ];
-	/* initial country for 802.11d auto-country mode */
 	char autocountry_default[BRCM_CNTRY_BUF_SZ];
-	u16 prb_resp_timeout;	/* do not send prb resp if request older than this,
-					 * 0 = disable
-					 */
-
-	wlc_rateset_t sup_rates_override;	/* use only these rates in 11g supported rates if
-						 * specifed
-						 */
+	u16 prb_resp_timeout;
 
-	chanspec_t home_chanspec;	/* shared home chanspec */
+	u16 home_chanspec;
 
 	/* PHY parameters */
-	chanspec_t chanspec;	/* target operational channel */
-	u16 usr_fragthresh;	/* user configured fragmentation threshold */
-	u16 fragthresh[NFIFO];	/* per-fifo fragmentation thresholds */
-	u16 RTSThresh;	/* 802.11 dot11RTSThreshold */
-	u16 SRL;		/* 802.11 dot11ShortRetryLimit */
-	u16 LRL;		/* 802.11 dot11LongRetryLimit */
-	u16 SFBL;		/* Short Frame Rate Fallback Limit */
-	u16 LFBL;		/* Long Frame Rate Fallback Limit */
+	u16 chanspec;
+	u16 usr_fragthresh;
+	u16 fragthresh[NFIFO];
+	u16 RTSThresh;
+	u16 SRL;
+	u16 LRL;
+	u16 SFBL;
+	u16 LFBL;
 
 	/* network config */
-	bool shortslot;		/* currently using 11g ShortSlot timing */
-	s8 shortslot_override;	/* 11g ShortSlot override */
-	bool include_legacy_erp;	/* include Legacy ERP info elt ID 47 as well as g ID 42 */
+	bool shortslot;
+	s8 shortslot_override;
+	bool include_legacy_erp;
 
 	struct brcms_protection *protection;
-	s8 PLCPHdr_override;	/* 802.11b Preamble Type override */
+	s8 PLCPHdr_override;
 
 	struct brcms_stf *stf;
 
-	ratespec_t bcn_rspec;	/* save bcn ratespec purpose */
+	u32 bcn_rspec;
 
 	uint tempsense_lasttime;
 
-	u16 tx_duty_cycle_ofdm;	/* maximum allowed duty cycle for OFDM */
-	u16 tx_duty_cycle_cck;	/* maximum allowed duty cycle for CCK */
+	u16 tx_duty_cycle_ofdm;
+	u16 tx_duty_cycle_cck;
 
-	u16 next_bsscfg_ID;
-
-	struct brcms_txq_info *pkt_queue; /* txq for transmit packets */
-	u32 mpc_dur;		/* total time (ms) in mpc mode except for the
-				 * portion since radio is turned off last time
-				 */
-	u32 mpc_laston_ts;	/* timestamp (ms) when radio is turned off last
-				 * time
-				 */
+	struct brcms_txq_info *pkt_queue;
 	struct wiphy *wiphy;
+	struct scb pri_scb;
 };
 
 /* antsel module specific state */
@@ -782,244 +619,117 @@ struct antsel_info {
 	struct brcms_antselcfg antcfg_cur; /* current antenna config (auto) */
 };
 
-/* BSS configuration state */
+/*
+ * BSS configuration state
+ *
+ * wlc: wlc to which this bsscfg belongs to.
+ * up: is this configuration up operational
+ * enable: is this configuration enabled
+ * associated: is BSS in ASSOCIATED state
+ * BSS: infraustructure or adhoc
+ * SSID_len: the length of SSID
+ * SSID: SSID string
+ *
+ *
+ * BSSID: BSSID (associated)
+ * cur_etheraddr: h/w address
+ * flags: BSSCFG flags; see below
+ *
+ * current_bss: BSS parms in ASSOCIATED state
+ *
+ *
+ * ID: 'unique' ID of this bsscfg, assigned at bsscfg allocation
+ */
 struct brcms_bss_cfg {
-	struct brcms_c_info *wlc; /* wlc to which this bsscfg belongs to. */
-	bool up;		/* is this configuration up operational */
-	bool enable;		/* is this configuration enabled */
-	bool associated;	/* is BSS in ASSOCIATED state */
-	bool BSS;		/* infraustructure or adhac */
-	bool dtim_programmed;
-
-	u8 SSID_len;		/* the length of SSID */
-	u8 SSID[IEEE80211_MAX_SSID_LEN]; /* SSID string */
-	struct scb *bcmc_scb[MAXBANDS];	/* one bcmc_scb per band */
-	s8 _idx;		/* the index of this bsscfg,
-				 * assigned at wlc_bsscfg_alloc()
-				 */
-	/* MAC filter */
-	uint nmac;		/* # of entries on maclist array */
-	int macmode;		/* allow/deny stations on maclist array */
-	struct ether_addr *maclist;	/* list of source MAC addrs to match */
-
-	/* security */
-	u32 wsec;		/* wireless security bitvec */
-	s16 auth;		/* 802.11 authentication: Open, Shared Key, WPA */
-	s16 openshared;	/* try Open auth first, then Shared Key */
-	bool wsec_restrict;	/* drop unencrypted packets if wsec is enabled */
-	bool eap_restrict;	/* restrict data until 802.1X auth succeeds */
-	u16 WPA_auth;	/* WPA: authenticated key management */
-	bool wpa2_preauth;	/* default is true, wpa_cap sets value */
-	bool wsec_portopen;	/* indicates keys are plumbed */
-	/* global txiv for WPA_NONE, tkip and aes */
-	struct wsec_iv wpa_none_txiv;
-	int wsec_index;		/* 0-3: default tx key, -1: not set */
-	/* default key storage: */
-	struct wsec_key *bss_def_keys[BRCMS_DEFAULT_KEYS];
-
-	/* TKIP countermeasures */
-	bool tkip_countermeasures;	/* flags TKIP no-assoc period */
-	u32 tk_cm_dt;	/* detect timer */
-	u32 tk_cm_bt;	/* blocking timer */
-	u32 tk_cm_bt_tmstmp;	/* Timestamp when TKIP BT is activated */
-	bool tk_cm_activate;	/* activate countermeasures after EAPOL-Key sent */
-
-	u8 BSSID[ETH_ALEN];	/* BSSID (associated) */
-	u8 cur_etheraddr[ETH_ALEN];	/* h/w address */
-	u16 bcmc_fid;	/* the last BCMC FID queued to TX_BCMC_FIFO */
-	u16 bcmc_fid_shm;	/* the last BCMC FID written to shared mem */
-
-	u32 flags;		/* BSSCFG flags; see below */
-
-	u8 *bcn;		/* AP beacon */
-	uint bcn_len;		/* AP beacon length */
-	bool ar_disassoc;	/* disassociated in associated recreation */
-
-	int auth_atmptd;	/* auth type (open/shared) attempted */
-
-	pmkid_cand_t pmkid_cand[MAXPMKID];	/* PMKID candidate list */
-	uint npmkid_cand;	/* num PMKID candidates */
-	pmkid_t pmkid[MAXPMKID];	/* PMKID cache */
-	uint npmkid;		/* num cached PMKIDs */
-
-	struct brcms_bss_info *current_bss; /* BSS parms in ASSOCIATED state */
-
-	/* PM states */
-	bool PMawakebcn;	/* bcn recvd during current waking state */
-	bool PMpending;		/* waiting for tx status with PM indicated set */
-	bool priorPMstate;	/* Detecting PM state transitions */
-	bool PSpoll;		/* whether there is an outstanding PS-Poll frame */
-
-	/* BSSID entry in RCMTA, use the wsec key management infrastructure to
-	 * manage the RCMTA entries.
-	 */
-	struct wsec_key *rcmta;
-
-	/* 'unique' ID of this bsscfg, assigned at bsscfg allocation */
-	u16 ID;
-
-	uint txrspecidx;	/* index into tx rate circular buffer */
-	ratespec_t txrspec[NTXRATE][2];	/* circular buffer of prev MPDUs tx rates */
+	struct brcms_c_info *wlc;
+	bool up;
+	bool enable;
+	bool associated;
+	bool BSS;
+	u8 SSID_len;
+	u8 SSID[IEEE80211_MAX_SSID_LEN];
+	u8 BSSID[ETH_ALEN];
+	u8 cur_etheraddr[ETH_ALEN];
+	struct brcms_bss_info *current_bss;
 };
 
-#define	CHANNEL_BANDUNIT(wlc, ch) (((ch) <= CH_MAX_2G_CHANNEL) ? BAND_2G_INDEX : BAND_5G_INDEX)
-#define	OTHERBANDUNIT(wlc)	((uint)((wlc)->band->bandunit ? BAND_2G_INDEX : BAND_5G_INDEX))
-
-#define IS_MBAND_UNLOCKED(wlc) \
-	((NBANDS(wlc) > 1) && !(wlc)->bandlocked)
-
-#define BRCMS_BAND_PI_RADIO_CHANSPEC wlc_phy_chanspec_get(wlc->band->pi)
-
-/* sum the individual fifo tx pending packet counts */
-#define	TXPKTPENDTOT(wlc) ((wlc)->core->txpktpend[0] + (wlc)->core->txpktpend[1] + \
-	(wlc)->core->txpktpend[2] + (wlc)->core->txpktpend[3])
-#define TXPKTPENDGET(wlc, fifo)		((wlc)->core->txpktpend[(fifo)])
-#define TXPKTPENDINC(wlc, fifo, val)	((wlc)->core->txpktpend[(fifo)] += (val))
-#define TXPKTPENDDEC(wlc, fifo, val)	((wlc)->core->txpktpend[(fifo)] -= (val))
-#define TXPKTPENDCLR(wlc, fifo)		((wlc)->core->txpktpend[(fifo)] = 0)
-#define TXAVAIL(wlc, fifo)		(*(wlc)->core->txavail[(fifo)])
-#define GETNEXTTXP(wlc, _queue)								\
-		dma_getnexttxp((wlc)->hw->di[(_queue)], DMA_RANGE_TRANSMITTED)
-
-#define BRCMS_IS_MATCH_SSID(wlc, ssid1, ssid2, len1, len2) \
-	((len1 == len2) && !memcmp(ssid1, ssid2, len1))
-
-extern void brcms_c_fatal_error(struct brcms_c_info *wlc);
-extern void brcms_b_rpc_watchdog(struct brcms_c_info *wlc);
-extern void brcms_c_recv(struct brcms_c_info *wlc, struct sk_buff *p);
-extern bool brcms_c_dotxstatus(struct brcms_c_info *wlc, struct tx_status *txs,
-			       u32 frm_tx2);
 extern void brcms_c_txfifo(struct brcms_c_info *wlc, uint fifo,
 			   struct sk_buff *p,
 			   bool commit, s8 txpktpend);
 extern void brcms_c_txfifo_complete(struct brcms_c_info *wlc, uint fifo,
 				    s8 txpktpend);
-extern void brcms_c_txq_enq(void *ctx, struct scb *scb, struct sk_buff *sdu,
-			    uint prec);
-extern void brcms_c_info_init(struct brcms_c_info *wlc, int unit);
+extern void brcms_c_txq_enq(struct brcms_c_info *wlc, struct scb *scb,
+			    struct sk_buff *sdu, uint prec);
 extern void brcms_c_print_txstatus(struct tx_status *txs);
-extern int brcms_c_xmtfifo_sz_get(struct brcms_c_info *wlc, uint fifo,
-				  uint *blocks);
-extern void brcms_c_write_template_ram(struct brcms_c_info *wlc, int offset,
-				       int len, void *buf);
-extern void brcms_c_write_hw_bcntemplates(struct brcms_c_info *wlc, void *bcn,
-					  int len, bool both);
-extern void brcms_c_pllreq(struct brcms_c_info *wlc, bool set, mbool req_bit);
-extern void brcms_c_reset_bmac_done(struct brcms_c_info *wlc);
+extern int brcms_b_xmtfifo_sz_get(struct brcms_hardware *wlc_hw, uint fifo,
+		   uint *blocks);
 
 #if defined(BCMDBG)
-extern void brcms_c_print_rxh(struct d11rxhdr *rxh);
 extern void brcms_c_print_txdesc(struct d11txh *txh);
 #else
 #define brcms_c_print_txdesc(a)
 #endif
 
-extern void brcms_c_setxband(struct brcms_hardware *wlc_hw, uint bandunit);
-extern void brcms_c_coredisable(struct brcms_hardware *wlc_hw);
-
-extern bool brcms_c_valid_rate(struct brcms_c_info *wlc, ratespec_t rate,
-			       int band, bool verbose);
-extern void brcms_c_ap_upd(struct brcms_c_info *wlc);
-
-/* helper functions */
-extern void brcms_c_shm_ssid_upd(struct brcms_c_info *wlc,
-				 struct brcms_bss_cfg *cfg);
 extern int brcms_c_set_gmode(struct brcms_c_info *wlc, u8 gmode, bool config);
-
 extern void brcms_c_mac_bcn_promisc_change(struct brcms_c_info *wlc,
 					   bool promisc);
-extern void brcms_c_mac_bcn_promisc(struct brcms_c_info *wlc);
-extern void brcms_c_mac_promisc(struct brcms_c_info *wlc);
-extern void brcms_c_txflowcontrol(struct brcms_c_info *wlc,
-				  struct brcms_txq_info *qi,
-				  bool on, int prio);
-extern void brcms_c_txflowcontrol_override(struct brcms_c_info *wlc,
-				       struct brcms_txq_info *qi,
-				       bool on, uint override);
-extern bool brcms_c_txflowcontrol_prio_isset(struct brcms_c_info *wlc,
-					     struct brcms_txq_info *qi,
-					     int prio);
 extern void brcms_c_send_q(struct brcms_c_info *wlc);
 extern int brcms_c_prep_pdu(struct brcms_c_info *wlc, struct sk_buff *pdu,
 			    uint *fifo);
-
-extern u16 brcms_c_calc_lsig_len(struct brcms_c_info *wlc, ratespec_t ratespec,
+extern u16 brcms_c_calc_lsig_len(struct brcms_c_info *wlc, u32 ratespec,
 				uint mac_len);
-extern ratespec_t brcms_c_rspec_to_rts_rspec(struct brcms_c_info *wlc,
-					     ratespec_t rspec,
+extern u32 brcms_c_rspec_to_rts_rspec(struct brcms_c_info *wlc,
+					     u32 rspec,
 					     bool use_rspec, u16 mimo_ctlchbw);
 extern u16 brcms_c_compute_rtscts_dur(struct brcms_c_info *wlc, bool cts_only,
-				      ratespec_t rts_rate,
-				      ratespec_t frame_rate,
+				      u32 rts_rate,
+				      u32 frame_rate,
 				      u8 rts_preamble_type,
 				      u8 frame_preamble_type, uint frame_len,
 				      bool ba);
-
-extern void brcms_c_tbtt(struct brcms_c_info *wlc);
 extern void brcms_c_inval_dma_pkts(struct brcms_hardware *hw,
 			       struct ieee80211_sta *sta,
 			       void (*dma_callback_fn));
-
-extern void brcms_c_reprate_init(struct brcms_c_info *wlc);
-extern void brcms_c_bsscfg_reprate_init(struct brcms_bss_cfg *bsscfg);
-
-/* Shared memory access */
-extern void brcms_c_write_shm(struct brcms_c_info *wlc, uint offset, u16 v);
-extern u16 brcms_c_read_shm(struct brcms_c_info *wlc, uint offset);
-extern void brcms_c_copyto_shm(struct brcms_c_info *wlc, uint offset,
-			       const void *buf, int len);
-
 extern void brcms_c_update_beacon(struct brcms_c_info *wlc);
-extern void brcms_c_bss_update_beacon(struct brcms_c_info *wlc,
-				  struct brcms_bss_cfg *bsscfg);
-
 extern void brcms_c_update_probe_resp(struct brcms_c_info *wlc, bool suspend);
-extern void brcms_c_bss_update_probe_resp(struct brcms_c_info *wlc,
-					  struct brcms_bss_cfg *cfg,
-					  bool suspend);
-extern bool brcms_c_ismpc(struct brcms_c_info *wlc);
-extern bool brcms_c_is_non_delay_mpc(struct brcms_c_info *wlc);
-extern void brcms_c_radio_mpc_upd(struct brcms_c_info *wlc);
-extern bool brcms_c_prec_enq(struct brcms_c_info *wlc, struct pktq *q,
-			     void *pkt, int prec);
-extern bool brcms_c_prec_enq_head(struct brcms_c_info *wlc, struct pktq *q,
-			      struct sk_buff *pkt, int prec, bool head);
-extern u16 brcms_c_phytxctl1_calc(struct brcms_c_info *wlc, ratespec_t rspec);
-extern void brcms_c_compute_plcp(struct brcms_c_info *wlc, ratespec_t rate,
-				 uint length, u8 *plcp);
-extern uint brcms_c_calc_frame_time(struct brcms_c_info *wlc,
-				    ratespec_t ratespec,
-				    u8 preamble_type, uint mac_len);
-
-extern void brcms_c_set_chanspec(struct brcms_c_info *wlc,
-				 chanspec_t chanspec);
-
-extern bool brcms_c_timers_init(struct brcms_c_info *wlc, int unit);
-
-extern int brcms_c_set_nmode(struct brcms_c_info *wlc, s32 nmode);
-extern void brcms_c_mimops_action_ht_send(struct brcms_c_info *wlc,
-				      struct brcms_bss_cfg *bsscfg,
-				      u8 mimops_mode);
-
-extern void brcms_c_switch_shortslot(struct brcms_c_info *wlc, bool shortslot);
-extern void brcms_c_set_bssid(struct brcms_bss_cfg *cfg);
-extern void brcms_c_edcf_setparams(struct brcms_c_info *wlc, bool suspend);
-
-extern void brcms_c_set_ratetable(struct brcms_c_info *wlc);
-extern int brcms_c_set_mac(struct brcms_bss_cfg *cfg);
+extern int brcms_c_set_nmode(struct brcms_c_info *wlc);
 extern void brcms_c_beacon_phytxctl_txant_upd(struct brcms_c_info *wlc,
-					  ratespec_t bcn_rate);
-extern void brcms_c_mod_prb_rsp_rate_table(struct brcms_c_info *wlc,
-					   uint frame_len);
-extern ratespec_t brcms_c_lowest_basic_rspec(struct brcms_c_info *wlc,
-					     wlc_rateset_t *rs);
-extern void brcms_c_radio_disable(struct brcms_c_info *wlc);
-extern void brcms_c_bcn_li_upd(struct brcms_c_info *wlc);
-extern void brcms_c_set_home_chanspec(struct brcms_c_info *wlc,
-				      chanspec_t chanspec);
-extern bool brcms_c_ps_allowed(struct brcms_c_info *wlc);
-extern bool brcms_c_stay_awake(struct brcms_c_info *wlc);
-extern void brcms_c_wme_initparams_sta(struct brcms_c_info *wlc,
-				       struct wme_param_ie *pe);
+					  u32 bcn_rate);
+extern void brcms_b_antsel_type_set(struct brcms_hardware *wlc_hw,
+				     u8 antsel_type);
+extern void brcms_b_set_chanspec(struct brcms_hardware *wlc_hw,
+				  u16 chanspec,
+				  bool mute, struct txpwr_limits *txpwr);
+extern void brcms_b_write_shm(struct brcms_hardware *wlc_hw, uint offset,
+			      u16 v);
+extern u16 brcms_b_read_shm(struct brcms_hardware *wlc_hw, uint offset);
+extern void brcms_b_mhf(struct brcms_hardware *wlc_hw, u8 idx, u16 mask,
+			u16 val, int bands);
+extern void brcms_b_corereset(struct brcms_hardware *wlc_hw, u32 flags);
+extern void brcms_b_mctrl(struct brcms_hardware *wlc_hw, u32 mask, u32 val);
+extern void brcms_b_phy_reset(struct brcms_hardware *wlc_hw);
+extern void brcms_b_bw_set(struct brcms_hardware *wlc_hw, u16 bw);
+extern void brcms_b_core_phypll_reset(struct brcms_hardware *wlc_hw);
+extern void brcms_c_ucode_wake_override_set(struct brcms_hardware *wlc_hw,
+					u32 override_bit);
+extern void brcms_c_ucode_wake_override_clear(struct brcms_hardware *wlc_hw,
+					  u32 override_bit);
+extern void brcms_b_write_template_ram(struct brcms_hardware *wlc_hw,
+				       int offset, int len, void *buf);
+extern u16 brcms_b_rate_shm_offset(struct brcms_hardware *wlc_hw, u8 rate);
+extern void brcms_b_copyto_objmem(struct brcms_hardware *wlc_hw,
+				   uint offset, const void *buf, int len,
+				   u32 sel);
+extern void brcms_b_copyfrom_objmem(struct brcms_hardware *wlc_hw, uint offset,
+				     void *buf, int len, u32 sel);
+extern void brcms_b_switch_macfreq(struct brcms_hardware *wlc_hw, u8 spurmode);
+extern u16 brcms_b_get_txant(struct brcms_hardware *wlc_hw);
+extern void brcms_b_phyclk_fgc(struct brcms_hardware *wlc_hw, bool clk);
+extern void brcms_b_macphyclk_set(struct brcms_hardware *wlc_hw, bool clk);
+extern void brcms_b_core_phypll_ctl(struct brcms_hardware *wlc_hw, bool on);
+extern void brcms_b_txant_set(struct brcms_hardware *wlc_hw, u16 phytxant);
+extern void brcms_b_band_stf_ss_set(struct brcms_hardware *wlc_hw,
+				    u8 stf_mode);
+extern void brcms_c_init_scb(struct scb *scb);
 
 #endif				/* _BRCM_MAIN_H_ */
diff --git a/drivers/staging/brcm80211/brcmsmac/nicpci.c b/drivers/staging/brcm80211/brcmsmac/nicpci.c
index 3d71c590fcef..0bcb26792046 100644
--- a/drivers/staging/brcm80211/brcmsmac/nicpci.c
+++ b/drivers/staging/brcm80211/brcmsmac/nicpci.c
@@ -127,6 +127,18 @@
 /* PCIE protocol TLP diagnostic registers */
 #define PCIE_TLP_WORKAROUNDSREG		0x004	/* TLP Workarounds */
 
+/* Sonics to PCI translation types */
+#define	SBTOPCI_PREF	0x4		/* prefetch enable */
+#define	SBTOPCI_BURST	0x8		/* burst enable */
+#define	SBTOPCI_RC_READMULTI	0x20	/* memory read multiple */
+
+#define PCI_CLKRUN_DSBL	0x8000	/* Bit 15 forceClkrun */
+
+/* PCI core index in SROM shadow area */
+#define SRSH_PI_OFFSET	0	/* first word */
+#define SRSH_PI_MASK	0xf000	/* bit 15:12 */
+#define SRSH_PI_SHIFT	12	/* bit 15:12 */
+
 /* Sonics side: PCI core and host control registers */
 struct sbpciregs {
 	u32 control;		/* PCI control */
@@ -194,8 +206,8 @@ struct sbpcieregs {
 
 struct pcicore_info {
 	union {
-		struct sbpcieregs *pcieregs;
-		struct sbpciregs *pciregs;
+		struct sbpcieregs __iomem *pcieregs;
+		struct sbpciregs __iomem *pciregs;
 	} regs;			/* Memory mapped register to the core */
 
 	struct si_pub *sih;	/* System interconnect handle */
@@ -211,52 +223,30 @@ struct pcicore_info {
 	bool pmecap;		/* Capable of generating PME */
 };
 
-/* debug/trace */
-#define	PCI_ERROR(args)
-#define PCIE_PUB(sih)							\
-	(((sih)->bustype == PCI_BUS) &&					\
-	 ((sih)->buscoretype == PCIE_CORE_ID))
-
-/* routines to access mdio slave device registers */
-static bool pcie_mdiosetblock(struct pcicore_info *pi, uint blk);
-static int pcie_mdioop(struct pcicore_info *pi, uint physmedia, uint regaddr,
-		       bool write, uint *val);
-static int pcie_mdiowrite(struct pcicore_info *pi, uint physmedia, uint readdr,
-			  uint val);
-static int pcie_mdioread(struct pcicore_info *pi, uint physmedia, uint readdr,
-			 uint *ret_val);
-
-static void pcie_extendL1timer(struct pcicore_info *pi, bool extend);
-static void pcie_clkreq_upd(struct pcicore_info *pi, uint state);
-
-static void pcie_war_aspm_clkreq(struct pcicore_info *pi);
-static void pcie_war_serdes(struct pcicore_info *pi);
-static void pcie_war_noplldown(struct pcicore_info *pi);
-static void pcie_war_polarity(struct pcicore_info *pi);
-static void pcie_war_pci_setup(struct pcicore_info *pi);
-
 #define PCIE_ASPM(sih)							\
-	((PCIE_PUB(sih)) &&						\
+	(((sih)->buscoretype == PCIE_CORE_ID) &&			\
 	 (((sih)->buscorerev >= 3) &&					\
 	  ((sih)->buscorerev <= 5)))
 
 
 /* delay needed between the mdio control/ mdiodata register data access */
-#define PR28829_DELAY() udelay(10)
+static void pr28829_delay(void)
+{
+	udelay(10);
+}
 
 /* Initialize the PCI core.
  * It's caller's responsibility to make sure that this is done only once
  */
-void *pcicore_init(struct si_pub *sih, void *pdev, void *regs)
+struct pcicore_info *pcicore_init(struct si_pub *sih, struct pci_dev *pdev,
+				  void __iomem *regs)
 {
 	struct pcicore_info *pi;
 
 	/* alloc struct pcicore_info */
 	pi = kzalloc(sizeof(struct pcicore_info), GFP_ATOMIC);
-	if (pi == NULL) {
-		PCI_ERROR(("pci_attach: malloc failed!\n"));
+	if (pi == NULL)
 		return NULL;
-	}
 
 	pi->sih = sih;
 	pi->dev = pdev;
@@ -273,7 +263,7 @@ void *pcicore_init(struct si_pub *sih, void *pdev, void *regs)
 	return pi;
 }
 
-void pcicore_deinit(void *pch)
+void pcicore_deinit(struct pcicore_info *pch)
 {
 	kfree(pch);
 }
@@ -281,7 +271,7 @@ void pcicore_deinit(void *pch)
 /* return cap_offset if requested capability exists in the PCI config space */
 /* Note that it's caller's responsibility to make sure it's a pci bus */
 u8
-pcicore_find_pci_capability(void *dev, u8 req_cap_id,
+pcicore_find_pci_capability(struct pci_dev *dev, u8 req_cap_id,
 			    unsigned char *buf, u32 *buflen)
 {
 	u8 cap_id;
@@ -344,7 +334,7 @@ end:
 
 /* ***** Register Access API */
 static uint
-pcie_readreg(struct sbpcieregs *pcieregs, uint addrtype, uint offset)
+pcie_readreg(struct sbpcieregs __iomem *pcieregs, uint addrtype, uint offset)
 {
 	uint retval = 0xFFFFFFFF;
 
@@ -364,8 +354,8 @@ pcie_readreg(struct sbpcieregs *pcieregs, uint addrtype, uint offset)
 	return retval;
 }
 
-static uint
-pcie_writereg(struct sbpcieregs *pcieregs, uint addrtype, uint offset, uint val)
+static uint pcie_writereg(struct sbpcieregs __iomem *pcieregs, uint addrtype,
+			  uint offset, uint val)
 {
 	switch (addrtype) {
 	case PCIE_CONFIGREGS:
@@ -384,7 +374,7 @@ pcie_writereg(struct sbpcieregs *pcieregs, uint addrtype, uint offset, uint val)
 
 static bool pcie_mdiosetblock(struct pcicore_info *pi, uint blk)
 {
-	struct sbpcieregs *pcieregs = pi->regs.pcieregs;
+	struct sbpcieregs __iomem *pcieregs = pi->regs.pcieregs;
 	uint mdiodata, i = 0;
 	uint pcie_serdes_spinwait = 200;
 
@@ -394,19 +384,18 @@ static bool pcie_mdiosetblock(struct pcicore_info *pi, uint blk)
 		    (blk << 4));
 	W_REG(&pcieregs->mdiodata, mdiodata);
 
-	PR28829_DELAY();
+	pr28829_delay();
 	/* retry till the transaction is complete */
 	while (i < pcie_serdes_spinwait) {
 		if (R_REG(&pcieregs->mdiocontrol) & MDIOCTL_ACCESS_DONE)
 			break;
+
 		udelay(1000);
 		i++;
 	}
 
-	if (i >= pcie_serdes_spinwait) {
-		PCI_ERROR(("pcie_mdiosetblock: timed out\n"));
+	if (i >= pcie_serdes_spinwait)
 		return false;
-	}
 
 	return true;
 }
@@ -415,7 +404,7 @@ static int
 pcie_mdioop(struct pcicore_info *pi, uint physmedia, uint regaddr, bool write,
 	    uint *val)
 {
-	struct sbpcieregs *pcieregs = pi->regs.pcieregs;
+	struct sbpcieregs __iomem *pcieregs = pi->regs.pcieregs;
 	uint mdiodata;
 	uint i = 0;
 	uint pcie_serdes_spinwait = 10;
@@ -445,13 +434,13 @@ pcie_mdioop(struct pcicore_info *pi, uint physmedia, uint regaddr, bool write,
 
 	W_REG(&pcieregs->mdiodata, mdiodata);
 
-	PR28829_DELAY();
+	pr28829_delay();
 
 	/* retry till the transaction is complete */
 	while (i < pcie_serdes_spinwait) {
 		if (R_REG(&pcieregs->mdiocontrol) & MDIOCTL_ACCESS_DONE) {
 			if (!write) {
-				PR28829_DELAY();
+				pr28829_delay();
 				*val = (R_REG(&pcieregs->mdiodata) &
 					MDIODATA_MASK);
 			}
@@ -463,8 +452,7 @@ pcie_mdioop(struct pcicore_info *pi, uint physmedia, uint regaddr, bool write,
 		i++;
 	}
 
-	PCI_ERROR(("pcie_mdioop: timed out op: %d\n", write));
-	/* Disable mdio access to SERDES */
+	/* Timed out. Disable mdio access to SERDES. */
 	W_REG(&pcieregs->mdiocontrol, 0);
 	return 1;
 }
@@ -485,9 +473,8 @@ pcie_mdiowrite(struct pcicore_info *pi, uint physmedia, uint regaddr, uint val)
 }
 
 /* ***** Support functions ***** */
-static u8 pcie_clkreq(void *pch, u32 mask, u32 val)
+static u8 pcie_clkreq(struct pcicore_info *pi, u32 mask, u32 val)
 {
-	struct pcicore_info *pi = pch;
 	u32 reg_val;
 	u8 offset;
 
@@ -515,9 +502,9 @@ static void pcie_extendL1timer(struct pcicore_info *pi, bool extend)
 {
 	u32 w;
 	struct si_pub *sih = pi->sih;
-	struct sbpcieregs *pcieregs = pi->regs.pcieregs;
+	struct sbpcieregs __iomem *pcieregs = pi->regs.pcieregs;
 
-	if (!PCIE_PUB(sih) || sih->buscorerev < 7)
+	if (sih->buscoretype != PCIE_CORE_ID || sih->buscorerev < 7)
 		return;
 
 	w = pcie_readreg(pcieregs, PCIE_PCIEREGS, PCIE_DLLP_PMTHRESHREG);
@@ -537,30 +524,30 @@ static void pcie_clkreq_upd(struct pcicore_info *pi, uint state)
 	switch (state) {
 	case SI_DOATTACH:
 		if (PCIE_ASPM(sih))
-			pcie_clkreq((void *)pi, 1, 0);
+			pcie_clkreq(pi, 1, 0);
 		break;
 	case SI_PCIDOWN:
 		if (sih->buscorerev == 6) {	/* turn on serdes PLL down */
 			ai_corereg(sih, SI_CC_IDX,
-				   offsetof(chipcregs_t, chipcontrol_addr),
+				   offsetof(struct chipcregs, chipcontrol_addr),
 				   ~0, 0);
 			ai_corereg(sih, SI_CC_IDX,
-				   offsetof(chipcregs_t, chipcontrol_data),
+				   offsetof(struct chipcregs, chipcontrol_data),
 				   ~0x40, 0);
 		} else if (pi->pcie_pr42767) {
-			pcie_clkreq((void *)pi, 1, 1);
+			pcie_clkreq(pi, 1, 1);
 		}
 		break;
 	case SI_PCIUP:
 		if (sih->buscorerev == 6) {	/* turn off serdes PLL down */
 			ai_corereg(sih, SI_CC_IDX,
-				   offsetof(chipcregs_t, chipcontrol_addr),
+				   offsetof(struct chipcregs, chipcontrol_addr),
 				   ~0, 0);
 			ai_corereg(sih, SI_CC_IDX,
-				   offsetof(chipcregs_t, chipcontrol_data),
+				   offsetof(struct chipcregs, chipcontrol_data),
 				   ~0x40, 0x40);
 		} else if (PCIE_ASPM(sih)) {	/* disable clkreq */
-			pcie_clkreq((void *)pi, 1, 0);
+			pcie_clkreq(pi, 1, 0);
 		}
 		break;
 	}
@@ -594,9 +581,10 @@ static void pcie_war_polarity(struct pcicore_info *pi)
  */
 static void pcie_war_aspm_clkreq(struct pcicore_info *pi)
 {
-	struct sbpcieregs *pcieregs = pi->regs.pcieregs;
+	struct sbpcieregs __iomem *pcieregs = pi->regs.pcieregs;
 	struct si_pub *sih = pi->sih;
-	u16 val16, *reg16;
+	u16 val16;
+	u16 __iomem *reg16;
 	u32 w;
 
 	if (!PCIE_ASPM(sih))
@@ -654,8 +642,9 @@ static void pcie_war_serdes(struct pcicore_info *pi)
 /* Needs to happen when coming out of 'standby'/'hibernate' */
 static void pcie_misc_config_fixup(struct pcicore_info *pi)
 {
-	struct sbpcieregs *pcieregs = pi->regs.pcieregs;
-	u16 val16, *reg16;
+	struct sbpcieregs __iomem *pcieregs = pi->regs.pcieregs;
+	u16 val16;
+	u16 __iomem *reg16;
 
 	reg16 = &pcieregs->sprom[SRSH_PCIE_MISC_CONFIG];
 	val16 = R_REG(reg16);
@@ -670,11 +659,11 @@ static void pcie_misc_config_fixup(struct pcicore_info *pi)
 /* Needs to happen when coming out of 'standby'/'hibernate' */
 static void pcie_war_noplldown(struct pcicore_info *pi)
 {
-	struct sbpcieregs *pcieregs = pi->regs.pcieregs;
-	u16 *reg16;
+	struct sbpcieregs __iomem *pcieregs = pi->regs.pcieregs;
+	u16 __iomem *reg16;
 
 	/* turn off serdes PLL down */
-	ai_corereg(pi->sih, SI_CC_IDX, offsetof(chipcregs_t, chipcontrol),
+	ai_corereg(pi->sih, SI_CC_IDX, offsetof(struct chipcregs, chipcontrol),
 		   CHIPCTRL_4321_PLL_DOWN, CHIPCTRL_4321_PLL_DOWN);
 
 	/* clear srom shadow backdoor */
@@ -686,7 +675,7 @@ static void pcie_war_noplldown(struct pcicore_info *pi)
 static void pcie_war_pci_setup(struct pcicore_info *pi)
 {
 	struct si_pub *sih = pi->sih;
-	struct sbpcieregs *pcieregs = pi->regs.pcieregs;
+	struct sbpcieregs __iomem *pcieregs = pi->regs.pcieregs;
 	u32 w;
 
 	if (sih->buscorerev == 0 || sih->buscorerev == 1) {
@@ -730,14 +719,14 @@ static void pcie_war_pci_setup(struct pcicore_info *pi)
 }
 
 /* ***** Functions called during driver state changes ***** */
-void pcicore_attach(void *pch, char *pvars, int state)
+void pcicore_attach(struct pcicore_info *pi, int state)
 {
-	struct pcicore_info *pi = pch;
 	struct si_pub *sih = pi->sih;
+	u32 bfl2 = (u32)getintvar(sih, BRCMS_SROM_BOARDFLAGS2);
 
 	/* Determine if this board needs override */
 	if (PCIE_ASPM(sih)) {
-		if ((u32)getintvar(pvars, "boardflags2") & BFL2_PCIEWAR_OVR)
+		if (bfl2 & BFL2_PCIEWAR_OVR)
 			pi->pcie_war_aspm_ovr = PCIE_ASPM_DISAB;
 		else
 			pi->pcie_war_aspm_ovr = PCIE_ASPM_ENAB;
@@ -754,21 +743,17 @@ void pcicore_attach(void *pch, char *pvars, int state)
 
 }
 
-void pcicore_hwup(void *pch)
+void pcicore_hwup(struct pcicore_info *pi)
 {
-	struct pcicore_info *pi = pch;
-
-	if (!pi || !PCIE_PUB(pi->sih))
+	if (!pi || pi->sih->buscoretype != PCIE_CORE_ID)
 		return;
 
 	pcie_war_pci_setup(pi);
 }
 
-void pcicore_up(void *pch, int state)
+void pcicore_up(struct pcicore_info *pi, int state)
 {
-	struct pcicore_info *pi = pch;
-
-	if (!pi || !PCIE_PUB(pi->sih))
+	if (!pi || pi->sih->buscoretype != PCIE_CORE_ID)
 		return;
 
 	/* Restore L1 timer for better performance */
@@ -780,9 +765,8 @@ void pcicore_up(void *pch, int state)
 /* When the device is going to enter D3 state
  * (or the system is going to enter S3/S4 states)
  */
-void pcicore_sleep(void *pch)
+void pcicore_sleep(struct pcicore_info *pi)
 {
-	struct pcicore_info *pi = pch;
 	u32 w;
 
 	if (!pi || !PCIE_ASPM(pi->sih))
@@ -795,11 +779,9 @@ void pcicore_sleep(void *pch)
 	pi->pcie_pr42767 = false;
 }
 
-void pcicore_down(void *pch, int state)
+void pcicore_down(struct pcicore_info *pi, int state)
 {
-	struct pcicore_info *pi = pch;
-
-	if (!pi || !PCIE_PUB(pi->sih))
+	if (!pi || pi->sih->buscoretype != PCIE_CORE_ID)
 		return;
 
 	pcie_clkreq_upd(pi, state);
@@ -809,20 +791,12 @@ void pcicore_down(void *pch, int state)
 }
 
 /* precondition: current core is sii->buscoretype */
-void pcicore_fixcfg(void *pch, void *regs)
+static void pcicore_fixcfg(struct pcicore_info *pi, u16 __iomem *reg16)
 {
-	struct pcicore_info *pi = pch;
-	struct si_info *sii = SI_INFO(pi->sih);
-	struct sbpciregs *pciregs = regs;
-	struct sbpcieregs *pcieregs = regs;
-	u16 val16, *reg16 = NULL;
+	struct si_info *sii = (struct si_info *)(pi->sih);
+	u16 val16;
 	uint pciidx;
 
-	/* check 'pi' is correct and fix it if not */
-	if (sii->pub.buscoretype == PCIE_CORE_ID)
-		reg16 = &pcieregs->sprom[SRSH_PI_OFFSET];
-	else if (sii->pub.buscoretype == PCI_CORE_ID)
-		reg16 = &pciregs->sprom[SRSH_PI_OFFSET];
 	pciidx = ai_coreidx(&sii->pub);
 	val16 = R_REG(reg16);
 	if (((val16 & SRSH_PI_MASK) >> SRSH_PI_SHIFT) != (u16)pciidx) {
@@ -832,16 +806,27 @@ void pcicore_fixcfg(void *pch, void *regs)
 	}
 }
 
+void
+pcicore_fixcfg_pci(struct pcicore_info *pi, struct sbpciregs __iomem *pciregs)
+{
+	pcicore_fixcfg(pi, &pciregs->sprom[SRSH_PI_OFFSET]);
+}
+
+void pcicore_fixcfg_pcie(struct pcicore_info *pi,
+			 struct sbpcieregs __iomem *pcieregs)
+{
+	pcicore_fixcfg(pi, &pcieregs->sprom[SRSH_PI_OFFSET]);
+}
+
 /* precondition: current core is pci core */
-void pcicore_pci_setup(void *pch, void *regs)
+void
+pcicore_pci_setup(struct pcicore_info *pi, struct sbpciregs __iomem *pciregs)
 {
-	struct pcicore_info *pi = pch;
-	struct sbpciregs *pciregs = regs;
 	u32 w;
 
 	OR_REG(&pciregs->sbtopci2, SBTOPCI_PREF | SBTOPCI_BURST);
 
-	if (SI_INFO(pi->sih)->pub.buscorerev >= 11) {
+	if (((struct si_info *)(pi->sih))->pub.buscorerev >= 11) {
 		OR_REG(&pciregs->sbtopci2, SBTOPCI_RC_READMULTI);
 		w = R_REG(&pciregs->clkrun);
 		W_REG(&pciregs->clkrun, w | PCI_CLKRUN_DSBL);
diff --git a/drivers/staging/brcm80211/brcmsmac/nicpci.h b/drivers/staging/brcm80211/brcmsmac/nicpci.h
index f71f842a2156..58aa80dc3329 100644
--- a/drivers/staging/brcm80211/brcmsmac/nicpci.h
+++ b/drivers/staging/brcm80211/brcmsmac/nicpci.h
@@ -58,28 +58,25 @@
 /* bar0 + 12K accesses chipc core registers */
 #define PCI_16KB0_CCREGS_OFFSET	(12 * 1024)
 
-#define PCI_CLKRUN_DSBL	0x8000	/* Bit 15 forceClkrun */
+struct sbpciregs;
+struct sbpcieregs;
 
-/* Sonics to PCI translation types */
-#define	SBTOPCI_PREF	0x4		/* prefetch enable */
-#define	SBTOPCI_BURST	0x8		/* burst enable */
-#define	SBTOPCI_RC_READMULTI	0x20	/* memory read multiple */
-
-/* PCI core index in SROM shadow area */
-#define SRSH_PI_OFFSET	0	/* first word */
-#define SRSH_PI_MASK	0xf000	/* bit 15:12 */
-#define SRSH_PI_SHIFT	12	/* bit 15:12 */
-
-extern void *pcicore_init(struct si_pub *sih, void *pdev, void *regs);
-extern void pcicore_deinit(void *pch);
-extern void pcicore_attach(void *pch, char *pvars, int state);
-extern void pcicore_hwup(void *pch);
-extern void pcicore_up(void *pch, int state);
-extern void pcicore_sleep(void *pch);
-extern void pcicore_down(void *pch, int state);
-extern u8 pcicore_find_pci_capability(void *dev, u8 req_cap_id,
-					 unsigned char *buf, u32 *buflen);
-extern void pcicore_fixcfg(void *pch, void *regs);
-extern void pcicore_pci_setup(void *pch, void *regs);
+extern struct pcicore_info *pcicore_init(struct si_pub *sih,
+					 struct pci_dev *pdev,
+					 void __iomem *regs);
+extern void pcicore_deinit(struct pcicore_info *pch);
+extern void pcicore_attach(struct pcicore_info *pch, int state);
+extern void pcicore_hwup(struct pcicore_info *pch);
+extern void pcicore_up(struct pcicore_info *pch, int state);
+extern void pcicore_sleep(struct pcicore_info *pch);
+extern void pcicore_down(struct pcicore_info *pch, int state);
+extern u8 pcicore_find_pci_capability(struct pci_dev *dev, u8 req_cap_id,
+				      unsigned char *buf, u32 *buflen);
+extern void pcicore_fixcfg_pci(struct pcicore_info *pch,
+			       struct sbpciregs __iomem *pciregs);
+extern void pcicore_fixcfg_pcie(struct pcicore_info *pch,
+				struct sbpcieregs __iomem *pciregs);
+extern void pcicore_pci_setup(struct pcicore_info *pch,
+			      struct sbpciregs __iomem *pciregs);
 
 #endif /* _BRCM_NICPCI_H_ */
diff --git a/drivers/staging/brcm80211/brcmsmac/otp.c b/drivers/staging/brcm80211/brcmsmac/otp.c
index 4a70180eba5d..edf551561fd8 100644
--- a/drivers/staging/brcm80211/brcmsmac/otp.c
+++ b/drivers/staging/brcm80211/brcmsmac/otp.c
@@ -25,10 +25,14 @@
 
 #define OTPS_GUP_MASK		0x00000f00
 #define OTPS_GUP_SHIFT		8
-#define OTPS_GUP_HW		0x00000100	/* h/w subregion is programmed */
-#define OTPS_GUP_SW		0x00000200	/* s/w subregion is programmed */
-#define OTPS_GUP_CI		0x00000400	/* chipid/pkgopt subregion is programmed */
-#define OTPS_GUP_FUSE		0x00000800	/* fuse subregion is programmed */
+/* h/w subregion is programmed */
+#define OTPS_GUP_HW		0x00000100
+/* s/w subregion is programmed */
+#define OTPS_GUP_SW		0x00000200
+/* chipid/pkgopt subregion is programmed */
+#define OTPS_GUP_CI		0x00000400
+/* fuse subregion is programmed */
+#define OTPS_GUP_FUSE		0x00000800
 
 /* Fields in otpprog in rev >= 21 */
 #define OTPP_COL_MASK		0x000000ff
@@ -60,28 +64,21 @@
 
 #define MAXNUMRDES		9	/* Maximum OTP redundancy entries */
 
-/* OTP common function type */
-typedef int (*otp_status_t) (void *oh);
-typedef int (*otp_size_t) (void *oh);
-typedef void *(*otp_init_t) (struct si_pub *sih);
-typedef u16(*otp_read_bit_t) (void *oh, chipcregs_t *cc, uint off);
-typedef int (*otp_read_region_t) (struct si_pub *sih, int region, u16 *data,
-				  uint *wlen);
-typedef int (*otp_nvread_t) (void *oh, char *data, uint *len);
+/* Fixed size subregions sizes in words */
+#define OTPGU_CI_SZ		2
+
+struct otpinfo;
 
 /* OTP function struct */
 struct otp_fn_s {
-	otp_size_t size;
-	otp_read_bit_t read_bit;
-	otp_init_t init;
-	otp_read_region_t read_region;
-	otp_nvread_t nvread;
-	otp_status_t status;
+	int (*init)(struct si_pub *sih, struct otpinfo *oi);
+	int (*read_region)(struct otpinfo *oi, int region, u16 *data,
+			   uint *wlen);
 };
 
 struct otpinfo {
 	uint ccrev;		/* chipc revision */
-	struct otp_fn_s *fn;		/* OTP functions */
+	const struct otp_fn_s *fn;	/* OTP functions */
 	struct si_pub *sih;		/* Saved sb handle */
 
 	/* IPX OTP section */
@@ -100,23 +97,6 @@ struct otpinfo {
 	int otpgu_base;		/* offset to General Use Region */
 };
 
-static struct otpinfo otpinfo;
-
-/*
- * IPX OTP Code
- *
- *   Exported functions:
- *	ipxotp_status()
- *	ipxotp_size()
- *	ipxotp_init()
- *	ipxotp_read_bit()
- *	ipxotp_read_region()
- *	ipxotp_nvread()
- *
- */
-
-#define HWSW_RGN(rgn)		(((rgn) == OTP_HW_RGN) ? "h/w" : "s/w")
-
 /* OTP layout */
 /* CC revs 21, 24 and 27 OTP General Use Region word offset */
 #define REVA4_OTPGU_BASE	12
@@ -152,60 +132,15 @@ static struct otpinfo otpinfo;
 #define OTP4315_SWREG_SZ	178	/* 178 bytes */
 #define OTP_SZ_FU_144		(144/8)	/* 144 bits */
 
-static int ipxotp_status(void *oh)
+static u16
+ipxotp_otpr(struct otpinfo *oi, struct chipcregs __iomem *cc, uint wn)
 {
-	struct otpinfo *oi = (struct otpinfo *) oh;
-	return (int)(oi->status);
-}
-
-/* Return size in bytes */
-static int ipxotp_size(void *oh)
-{
-	struct otpinfo *oi = (struct otpinfo *) oh;
-	return (int)oi->wsize * 2;
-}
-
-static u16 ipxotp_otpr(void *oh, chipcregs_t *cc, uint wn)
-{
-	struct otpinfo *oi;
-
-	oi = (struct otpinfo *) oh;
-
 	return R_REG(&cc->sromotp[wn]);
 }
 
-static u16 ipxotp_read_bit(void *oh, chipcregs_t *cc, uint off)
-{
-	struct otpinfo *oi = (struct otpinfo *) oh;
-	uint k, row, col;
-	u32 otpp, st;
-
-	row = off / oi->cols;
-	col = off % oi->cols;
-
-	otpp = OTPP_START_BUSY |
-	    ((OTPPOC_READ << OTPP_OC_SHIFT) & OTPP_OC_MASK) |
-	    ((row << OTPP_ROW_SHIFT) & OTPP_ROW_MASK) |
-	    ((col << OTPP_COL_SHIFT) & OTPP_COL_MASK);
-	W_REG(&cc->otpprog, otpp);
-
-	for (k = 0;
-	     ((st = R_REG(&cc->otpprog)) & OTPP_START_BUSY)
-	     && (k < OTPP_TRIES); k++)
-		;
-	if (k >= OTPP_TRIES) {
-		return 0xffff;
-	}
-	if (st & OTPP_READERR) {
-		return 0xffff;
-	}
-	st = (st & OTPP_VALUE_MASK) >> OTPP_VALUE_SHIFT;
-
-	return (int)st;
-}
-
-/* Calculate max HW/SW region byte size by subtracting fuse region and checksum size,
- * osizew is oi->wsize (OTP size - GU size) in words
+/*
+ * Calculate max HW/SW region byte size by subtracting fuse region
+ * and checksum size, osizew is oi->wsize (OTP size - GU size) in words
  */
 static int ipxotp_max_rgnsz(struct si_pub *sih, int osizew)
 {
@@ -226,17 +161,23 @@ static int ipxotp_max_rgnsz(struct si_pub *sih, int osizew)
 	return ret;
 }
 
-static void _ipxotp_init(struct otpinfo *oi, chipcregs_t *cc)
+static void _ipxotp_init(struct otpinfo *oi, struct chipcregs __iomem *cc)
 {
 	uint k;
 	u32 otpp, st;
 
-	/* record word offset of General Use Region for various chipcommon revs */
+	/*
+	 * record word offset of General Use Region
+	 * for various chipcommon revs
+	 */
 	if (oi->sih->ccrev == 21 || oi->sih->ccrev == 24
 	    || oi->sih->ccrev == 27) {
 		oi->otpgu_base = REVA4_OTPGU_BASE;
 	} else if (oi->sih->ccrev == 36) {
-		/* OTP size greater than equal to 2KB (128 words), otpgu_base is similar to rev23 */
+		/*
+		 * OTP size greater than equal to 2KB (128 words),
+		 * otpgu_base is similar to rev23
+		 */
 		if (oi->wsize >= 128)
 			oi->otpgu_base = REVB8_OTPGU_BASE;
 		else
@@ -254,9 +195,8 @@ static void _ipxotp_init(struct otpinfo *oi, chipcregs_t *cc)
 	     ((st = R_REG(&cc->otpprog)) & OTPP_START_BUSY)
 	     && (k < OTPP_TRIES); k++)
 		;
-	if (k >= OTPP_TRIES) {
+	if (k >= OTPP_TRIES)
 		return;
-	}
 
 	/* Read OTP lock bits and subregion programmed indication bits */
 	oi->status = R_REG(&cc->otpstatus);
@@ -272,8 +212,9 @@ static void _ipxotp_init(struct otpinfo *oi, chipcregs_t *cc)
 	}
 
 	/*
-	 * h/w region base and fuse region limit are fixed to the top and
-	 * the bottom of the general use region. Everything else can be flexible.
+	 * h/w region base and fuse region limit are fixed to
+	 * the top and the bottom of the general use region.
+	 * Everything else can be flexible.
 	 */
 	oi->hwbase = oi->otpgu_base + OTPGU_SROM_OFF;
 	oi->hwlim = oi->wsize;
@@ -297,28 +238,24 @@ static void _ipxotp_init(struct otpinfo *oi, chipcregs_t *cc)
 	oi->flim = oi->wsize;
 }
 
-static void *ipxotp_init(struct si_pub *sih)
+static int ipxotp_init(struct si_pub *sih, struct otpinfo *oi)
 {
 	uint idx;
-	chipcregs_t *cc;
-	struct otpinfo *oi;
+	struct chipcregs __iomem *cc;
 
 	/* Make sure we're running IPX OTP */
 	if (!OTPTYPE_IPX(sih->ccrev))
-		return NULL;
+		return -EBADE;
 
 	/* Make sure OTP is not disabled */
 	if (ai_is_otp_disabled(sih))
-		return NULL;
-
-	/* OTP is always powered */
-	oi = &otpinfo;
+		return -EBADE;
 
 	/* Check for otp size */
 	switch ((sih->cccaps & CC_CAP_OTPSIZE) >> CC_CAP_OTPSIZE_SHIFT) {
 	case 0:
 		/* Nothing there */
-		return NULL;
+		return -EBADE;
 	case 1:		/* 32x64 */
 		oi->rows = 32;
 		oi->cols = 64;
@@ -341,7 +278,7 @@ static void *ipxotp_init(struct si_pub *sih)
 		break;
 	default:
 		/* Don't know the geometry */
-		return NULL;
+		return -EBADE;
 	}
 
 	/* Retrieve OTP region info */
@@ -352,14 +289,14 @@ static void *ipxotp_init(struct si_pub *sih)
 
 	ai_setcoreidx(sih, idx);
 
-	return (void *)oi;
+	return 0;
 }
 
-static int ipxotp_read_region(void *oh, int region, u16 *data, uint *wlen)
+static int
+ipxotp_read_region(struct otpinfo *oi, int region, u16 *data, uint *wlen)
 {
-	struct otpinfo *oi = (struct otpinfo *) oh;
 	uint idx;
-	chipcregs_t *cc;
+	struct chipcregs __iomem *cc;
 	uint base, i, sz;
 
 	/* Validate region selection */
@@ -433,68 +370,23 @@ static int ipxotp_read_region(void *oh, int region, u16 *data, uint *wlen)
 
 	/* Read the data */
 	for (i = 0; i < sz; i++)
-		data[i] = ipxotp_otpr(oh, cc, base + i);
+		data[i] = ipxotp_otpr(oi, cc, base + i);
 
 	ai_setcoreidx(oi->sih, idx);
 	*wlen = sz;
 	return 0;
 }
 
-static int ipxotp_nvread(void *oh, char *data, uint *len)
-{
-	return -ENOTSUPP;
-}
-
-static struct otp_fn_s ipxotp_fn = {
-	(otp_size_t) ipxotp_size,
-	(otp_read_bit_t) ipxotp_read_bit,
-
-	(otp_init_t) ipxotp_init,
-	(otp_read_region_t) ipxotp_read_region,
-	(otp_nvread_t) ipxotp_nvread,
-
-	(otp_status_t) ipxotp_status
+static const struct otp_fn_s ipxotp_fn = {
+	(int (*)(struct si_pub *, struct otpinfo *)) ipxotp_init,
+	(int (*)(struct otpinfo *, int, u16 *, uint *)) ipxotp_read_region,
 };
 
-/*
- *	otp_status()
- *	otp_size()
- *	otp_read_bit()
- *	otp_init()
- *	otp_read_region()
- *	otp_nvread()
- */
-
-int otp_status(void *oh)
+static int otp_init(struct si_pub *sih, struct otpinfo *oi)
 {
-	struct otpinfo *oi = (struct otpinfo *) oh;
 
-	return oi->fn->status(oh);
-}
+	int ret;
 
-int otp_size(void *oh)
-{
-	struct otpinfo *oi = (struct otpinfo *) oh;
-
-	return oi->fn->size(oh);
-}
-
-u16 otp_read_bit(void *oh, uint offset)
-{
-	struct otpinfo *oi = (struct otpinfo *) oh;
-	uint idx = ai_coreidx(oi->sih);
-	chipcregs_t *cc = ai_setcoreidx(oi->sih, SI_CC_IDX);
-	u16 readBit = (u16) oi->fn->read_bit(oh, cc, offset);
-	ai_setcoreidx(oi->sih, idx);
-	return readBit;
-}
-
-void *otp_init(struct si_pub *sih)
-{
-	struct otpinfo *oi;
-	void *ret = NULL;
-
-	oi = &otpinfo;
 	memset(oi, 0, sizeof(struct otpinfo));
 
 	oi->ccrev = sih->ccrev;
@@ -502,21 +394,20 @@ void *otp_init(struct si_pub *sih)
 	if (OTPTYPE_IPX(oi->ccrev))
 		oi->fn = &ipxotp_fn;
 
-	if (oi->fn == NULL) {
-		return NULL;
-	}
+	if (oi->fn == NULL)
+		return -EBADE;
 
 	oi->sih = sih;
 
-	ret = (oi->fn->init) (sih);
+	ret = (oi->fn->init) (sih, oi);
 
 	return ret;
 }
 
 int
-otp_read_region(struct si_pub *sih, int region, u16 *data,
-				 uint *wlen) {
-	void *oh;
+otp_read_region(struct si_pub *sih, int region, u16 *data, uint *wlen) {
+	struct otpinfo otpinfo;
+	struct otpinfo *oi = &otpinfo;
 	int err = 0;
 
 	if (ai_is_otp_disabled(sih)) {
@@ -524,22 +415,12 @@ otp_read_region(struct si_pub *sih, int region, u16 *data,
 		goto out;
 	}
 
-	oh = otp_init(sih);
-	if (oh == NULL) {
-		err = -EBADE;
+	err = otp_init(sih, oi);
+	if (err)
 		goto out;
-	}
 
-	err = (((struct otpinfo *) oh)->fn->read_region)
-						(oh, region, data, wlen);
+	err = ((oi)->fn->read_region)(oi, region, data, wlen);
 
  out:
 	return err;
 }
-
-int otp_nvread(void *oh, char *data, uint *len)
-{
-	struct otpinfo *oi = (struct otpinfo *) oh;
-
-	return oi->fn->nvread(oh, data, len);
-}
diff --git a/drivers/staging/brcm80211/brcmsmac/otp.h b/drivers/staging/brcm80211/brcmsmac/otp.h
index f6d3a56acf1e..6b6d31cf9569 100644
--- a/drivers/staging/brcm80211/brcmsmac/otp.h
+++ b/drivers/staging/brcm80211/brcmsmac/otp.h
@@ -24,24 +24,13 @@
 #define OTP_SW_RGN	2
 #define OTP_CI_RGN	4
 #define OTP_FUSE_RGN	8
-#define OTP_ALL_RGN	0xf	/* From h/w region to end of OTP including checksum */
+/* From h/w region to end of OTP including checksum */
+#define OTP_ALL_RGN	0xf
 
 /* OTP Size */
 #define OTP_SZ_MAX		(6144/8)	/* maximum bytes in one CIS */
 
-/* Fixed size subregions sizes in words */
-#define OTPGU_CI_SZ		2
-
-/* OTP usage */
-#define OTP4325_FM_DISABLED_OFFSET	188
-
-/* Exported functions */
-extern int otp_status(void *oh);
-extern int otp_size(void *oh);
-extern u16 otp_read_bit(void *oh, uint offset);
-extern void *otp_init(struct si_pub *sih);
 extern int otp_read_region(struct si_pub *sih, int region, u16 *data,
 			   uint *wlen);
-extern int otp_nvread(void *oh, char *data, uint *len);
 
 #endif				/* _BRCM_OTP_H_ */
diff --git a/drivers/staging/brcm80211/brcmsmac/phy/phy_cmn.c b/drivers/staging/brcm80211/brcmsmac/phy/phy_cmn.c
index 17012fbe9c97..a3149254cbcd 100644
--- a/drivers/staging/brcm80211/brcmsmac/phy/phy_cmn.c
+++ b/drivers/staging/brcm80211/brcmsmac/phy/phy_cmn.c
@@ -13,8 +13,9 @@
  * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
-
+#include <linux/kernel.h>
 #include <linux/delay.h>
+#include <linux/bitops.h>
 
 #include <brcm_hw_ids.h>
 #include <chipcommon.h>
@@ -27,14 +28,32 @@
 #include "phy_lcn.h"
 #include "phyreg_n.h"
 
-u32 phyhal_msg_level = PHYHAL_ERROR;
+#define VALID_N_RADIO(radioid) ((radioid == BCM2055_ID) || \
+				 (radioid == BCM2056_ID) || \
+				 (radioid == BCM2057_ID))
+
+#define VALID_LCN_RADIO(radioid)	(radioid == BCM2064_ID)
+
+#define VALID_RADIO(pi, radioid)        ( \
+		(ISNPHY(pi) ? VALID_N_RADIO(radioid) : false) || \
+		(ISLCNPHY(pi) ? VALID_LCN_RADIO(radioid) : false))
+
+/* basic mux operation - can be optimized on several architectures */
+#define MUX(pred, true, false) ((pred) ? (true) : (false))
+
+/* modulo inc/dec - assumes x E [0, bound - 1] */
+#define MODINC(x, bound) MUX((x) == (bound) - 1, 0, (x) + 1)
+
+/* modulo inc/dec, bound = 2^k */
+#define MODDEC_POW2(x, bound) (((x) - 1) & ((bound) - 1))
+#define MODINC_POW2(x, bound) (((x) + 1) & ((bound) - 1))
 
 struct chan_info_basic {
 	u16 chan;
 	u16 freq;
 };
 
-static struct chan_info_basic chan_info_all[] = {
+static const struct chan_info_basic chan_info_all[] = {
 	{1, 2412},
 	{2, 2417},
 	{3, 2422},
@@ -93,20 +112,6 @@ static struct chan_info_basic chan_info_all[] = {
 	{216, 50800}
 };
 
-u16 ltrn_list[PHY_LTRN_LIST_LEN] = {
-	0x18f9, 0x0d01, 0x00e4, 0xdef4, 0x06f1, 0x0ffc,
-	0xfa27, 0x1dff, 0x10f0, 0x0918, 0xf20a, 0xe010,
-	0x1417, 0x1104, 0xf114, 0xf2fa, 0xf7db, 0xe2fc,
-	0xe1fb, 0x13ee, 0xff0d, 0xe91c, 0x171a, 0x0318,
-	0xda00, 0x03e8, 0x17e6, 0xe9e4, 0xfff3, 0x1312,
-	0xe105, 0xe204, 0xf725, 0xf206, 0xf1ec, 0x11fc,
-	0x14e9, 0xe0f0, 0xf2f6, 0x09e8, 0x1010, 0x1d01,
-	0xfad9, 0x0f04, 0x060f, 0xde0c, 0x001c, 0x0dff,
-	0x1807, 0xf61a, 0xe40e, 0x0f16, 0x05f9, 0x18ec,
-	0x0a1b, 0xff1e, 0x2600, 0xffe2, 0x0ae5, 0x1814,
-	0x0507, 0x0fea, 0xe4f2, 0xf6e6
-};
-
 const u8 ofdm_rate_lookup[] = {
 
 	BRCM_RATE_48M,
@@ -119,66 +124,7 @@ const u8 ofdm_rate_lookup[] = {
 	BRCM_RATE_9M
 };
 
-#define PHY_WREG_LIMIT	24
-
-static void wlc_set_phy_uninitted(struct brcms_phy *pi);
-static u32 wlc_phy_get_radio_ver(struct brcms_phy *pi);
-static void wlc_phy_timercb_phycal(void *arg);
-
-static bool wlc_phy_noise_calc_phy(struct brcms_phy *pi, u32 *cmplx_pwr,
-				   s8 *pwr_ant);
-
-static void wlc_phy_cal_perical_mphase_schedule(struct brcms_phy *pi,
-						uint delay);
-
-static void wlc_phy_noise_cb(struct brcms_phy *pi, u8 channel, s8 noise_dbm);
-static void wlc_phy_noise_sample_request(struct brcms_phy_pub *pih, u8 reason,
-					 u8 ch);
-
-static void wlc_phy_txpower_reg_limit_calc(struct brcms_phy *pi,
-					   struct txpwr_limits *tp, chanspec_t);
-static bool wlc_phy_cal_txpower_recalc_sw(struct brcms_phy *pi);
-
-static s8 wlc_user_txpwr_antport_to_rfport(struct brcms_phy *pi, uint chan,
-					     u32 band, u8 rate);
-static void wlc_phy_upd_env_txpwr_rate_limits(struct brcms_phy *pi, u32 band);
-static s8 wlc_phy_env_measure_vbat(struct brcms_phy *pi);
-static s8 wlc_phy_env_measure_temperature(struct brcms_phy *pi);
-
-char *phy_getvar(struct brcms_phy *pi, const char *name)
-{
-	char *vars = pi->vars;
-	char *s;
-	int len;
-
-	if (!name)
-		return NULL;
-
-	len = strlen(name);
-	if (len == 0)
-		return NULL;
-
-	for (s = vars; s && *s;) {
-		if ((memcmp(s, name, len) == 0) && (s[len] == '='))
-			return &s[len + 1];
-
-		while (*s++)
-			;
-	}
-
-	return NULL;
-}
-
-int phy_getintvar(struct brcms_phy *pi, const char *name)
-{
-	char *val;
-
-	val = PHY_GETVAR(pi, name);
-	if (val == NULL)
-		return 0;
-
-	return simple_strtoul(val, NULL, 0);
-}
+#define PHY_WREG_LIMIT  24
 
 void wlc_phyreg_enter(struct brcms_phy_pub *pih)
 {
@@ -203,7 +149,7 @@ void wlc_radioreg_enter(struct brcms_phy_pub *pih)
 void wlc_radioreg_exit(struct brcms_phy_pub *pih)
 {
 	struct brcms_phy *pi = (struct brcms_phy *) pih;
-	volatile u16 dummy;
+	u16 dummy;
 
 	dummy = R_REG(&pi->regs->phyversion);
 	pi->phy_wreg = 0;
@@ -217,12 +163,10 @@ u16 read_radio_reg(struct brcms_phy *pi, u16 addr)
 	if ((addr == RADIO_IDCODE))
 		return 0xffff;
 
-	if (NORADIO_ENAB(pi->pubpi))
-		return NORADIO_IDCODE & 0xffff;
-
 	switch (pi->pubpi.phy_type) {
 	case PHY_TYPE_N:
-		CASECHECK(PHYTYPE, PHY_TYPE_N);
+		if (!CONF_HAS(PHYTYPE, PHY_TYPE_N))
+			break;
 		if (NREV_GE(pi->pubpi.phy_rev, 7))
 			addr |= RADIO_2057_READ_OFF;
 		else
@@ -230,7 +174,8 @@ u16 read_radio_reg(struct brcms_phy *pi, u16 addr)
 		break;
 
 	case PHY_TYPE_LCN:
-		CASECHECK(PHYTYPE, PHY_TYPE_LCN);
+		if (!CONF_HAS(PHYTYPE, PHY_TYPE_LCN))
+			break;
 		addr |= RADIO_2064_READ_OFF;
 		break;
 
@@ -262,9 +207,6 @@ u16 read_radio_reg(struct brcms_phy *pi, u16 addr)
 
 void write_radio_reg(struct brcms_phy *pi, u16 addr, u16 val)
 {
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
-
 	if ((D11REV_GE(pi->sh->corerev, 24)) ||
 	    (D11REV_IS(pi->sh->corerev, 22)
 	     && (pi->pubpi.phy_type != PHY_TYPE_SSN))) {
@@ -276,11 +218,9 @@ void write_radio_reg(struct brcms_phy *pi, u16 addr, u16 val)
 		W_REG(&pi->regs->phy4wdatalo, val);
 	}
 
-	if (pi->sh->bustype == PCI_BUS) {
-		if (++pi->phy_wreg >= pi->phy_wreg_limit) {
-			(void)R_REG(&pi->regs->maccontrol);
-			pi->phy_wreg = 0;
-		}
+	if (++pi->phy_wreg >= pi->phy_wreg_limit) {
+		(void)R_REG(&pi->regs->maccontrol);
+		pi->phy_wreg = 0;
 	}
 }
 
@@ -288,9 +228,6 @@ static u32 read_radio_id(struct brcms_phy *pi)
 {
 	u32 id;
 
-	if (NORADIO_ENAB(pi->pubpi))
-		return NORADIO_IDCODE;
-
 	if (D11REV_GE(pi->sh->corerev, 24)) {
 		u32 b0, b1, b2;
 
@@ -316,9 +253,6 @@ void and_radio_reg(struct brcms_phy *pi, u16 addr, u16 val)
 {
 	u16 rval;
 
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
-
 	rval = read_radio_reg(pi, addr);
 	write_radio_reg(pi, addr, (rval & val));
 }
@@ -327,9 +261,6 @@ void or_radio_reg(struct brcms_phy *pi, u16 addr, u16 val)
 {
 	u16 rval;
 
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
-
 	rval = read_radio_reg(pi, addr);
 	write_radio_reg(pi, addr, (rval | val));
 }
@@ -338,9 +269,6 @@ void xor_radio_reg(struct brcms_phy *pi, u16 addr, u16 mask)
 {
 	u16 rval;
 
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
-
 	rval = read_radio_reg(pi, addr);
 	write_radio_reg(pi, addr, (rval ^ mask));
 }
@@ -349,9 +277,6 @@ void mod_radio_reg(struct brcms_phy *pi, u16 addr, u16 mask, u16 val)
 {
 	u16 rval;
 
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
-
 	rval = read_radio_reg(pi, addr);
 	write_radio_reg(pi, addr, (rval & ~mask) | (val & mask));
 }
@@ -363,7 +288,7 @@ void write_phy_channel_reg(struct brcms_phy *pi, uint val)
 
 u16 read_phy_reg(struct brcms_phy *pi, u16 addr)
 {
-	d11regs_t *regs;
+	struct d11regs __iomem *regs;
 
 	regs = pi->regs;
 
@@ -375,30 +300,27 @@ u16 read_phy_reg(struct brcms_phy *pi, u16 addr)
 
 void write_phy_reg(struct brcms_phy *pi, u16 addr, u16 val)
 {
-	d11regs_t *regs;
+	struct d11regs __iomem *regs;
 
 	regs = pi->regs;
 
-#ifdef __mips__
+#ifdef CONFIG_BCM47XX
 	W_REG_FLUSH(&regs->phyregaddr, addr);
 	W_REG(&regs->phyregdata, val);
 	if (addr == 0x72)
 		(void)R_REG(&regs->phyregdata);
 #else
-	W_REG((u32 *)(&regs->phyregaddr),
-	      addr | (val << 16));
-	if (pi->sh->bustype == PCI_BUS) {
-		if (++pi->phy_wreg >= pi->phy_wreg_limit) {
-			pi->phy_wreg = 0;
-			(void)R_REG(&regs->phyversion);
-		}
+	W_REG((u32 __iomem *)(&regs->phyregaddr), addr | (val << 16));
+	if (++pi->phy_wreg >= pi->phy_wreg_limit) {
+		pi->phy_wreg = 0;
+		(void)R_REG(&regs->phyversion);
 	}
 #endif
 }
 
 void and_phy_reg(struct brcms_phy *pi, u16 addr, u16 val)
 {
-	d11regs_t *regs;
+	struct d11regs __iomem *regs;
 
 	regs = pi->regs;
 
@@ -410,7 +332,7 @@ void and_phy_reg(struct brcms_phy *pi, u16 addr, u16 val)
 
 void or_phy_reg(struct brcms_phy *pi, u16 addr, u16 val)
 {
-	d11regs_t *regs;
+	struct d11regs __iomem *regs;
 
 	regs = pi->regs;
 
@@ -422,7 +344,7 @@ void or_phy_reg(struct brcms_phy *pi, u16 addr, u16 val)
 
 void mod_phy_reg(struct brcms_phy *pi, u16 addr, u16 mask, u16 val)
 {
-	d11regs_t *regs;
+	struct d11regs __iomem *regs;
 
 	regs = pi->regs;
 
@@ -464,9 +386,8 @@ static void wlc_set_phy_uninitted(struct brcms_phy *pi)
 	}
 	pi->radiopwr = 0xffff;
 	for (i = 0; i < STATIC_NUM_RF; i++) {
-		for (j = 0; j < STATIC_NUM_BB; j++) {
+		for (j = 0; j < STATIC_NUM_BB; j++)
 			pi->stats_11b_txpower[i][j] = -1;
-		}
 	}
 }
 
@@ -475,9 +396,8 @@ struct shared_phy *wlc_phy_shared_attach(struct shared_phy_params *shp)
 	struct shared_phy *sh;
 
 	sh = kzalloc(sizeof(struct shared_phy), GFP_ATOMIC);
-	if (sh == NULL) {
+	if (sh == NULL)
 		return NULL;
-	}
 
 	sh->sih = shp->sih;
 	sh->physhim = shp->physhim;
@@ -495,7 +415,6 @@ struct shared_phy *wlc_phy_shared_attach(struct shared_phy_params *shp)
 	sh->boardvendor = shp->boardvendor;
 	sh->boardflags = shp->boardflags;
 	sh->boardflags2 = shp->boardflags2;
-	sh->bustype = shp->bustype;
 	sh->buscorerev = shp->buscorerev;
 
 	sh->fast_timer = PHY_SW_TIMER_FAST;
@@ -507,9 +426,40 @@ struct shared_phy *wlc_phy_shared_attach(struct shared_phy_params *shp)
 	return sh;
 }
 
+static void wlc_phy_timercb_phycal(struct brcms_phy *pi)
+{
+	uint delay = 5;
+
+	if (PHY_PERICAL_MPHASE_PENDING(pi)) {
+		if (!pi->sh->up) {
+			wlc_phy_cal_perical_mphase_reset(pi);
+			return;
+		}
+
+		if (SCAN_RM_IN_PROGRESS(pi) || PLT_INPROG_PHY(pi)) {
+
+			delay = 1000;
+			wlc_phy_cal_perical_mphase_restart(pi);
+		} else
+			wlc_phy_cal_perical_nphy_run(pi, PHY_PERICAL_AUTO);
+		wlapi_add_timer(pi->phycal_timer, delay, 0);
+		return;
+	}
+
+}
+
+static u32 wlc_phy_get_radio_ver(struct brcms_phy *pi)
+{
+	u32 ver;
+
+	ver = read_radio_id(pi);
+
+	return ver;
+}
+
 struct brcms_phy_pub *
-wlc_phy_attach(struct shared_phy *sh, void *regs, int bandtype,
-	       char *vars, struct wiphy *wiphy)
+wlc_phy_attach(struct shared_phy *sh, struct d11regs __iomem *regs,
+	       int bandtype, struct wiphy *wiphy)
 {
 	struct brcms_phy *pi;
 	u32 sflags = 0;
@@ -522,42 +472,35 @@ wlc_phy_attach(struct shared_phy *sh, void *regs, int bandtype,
 	else
 		sflags = ai_core_sflags(sh->sih, 0, 0);
 
-	if (BAND_5G(bandtype)) {
-		if ((sflags & (SISF_5G_PHY | SISF_DB_PHY)) == 0) {
+	if (bandtype == BRCM_BAND_5G) {
+		if ((sflags & (SISF_5G_PHY | SISF_DB_PHY)) == 0)
 			return NULL;
-		}
 	}
 
 	pi = sh->phy_head;
 	if ((sflags & SISF_DB_PHY) && pi) {
-
 		wlapi_bmac_corereset(pi->sh->physhim, pi->pubpi.coreflags);
 		pi->refcnt++;
 		return &pi->pubpi_ro;
 	}
 
 	pi = kzalloc(sizeof(struct brcms_phy), GFP_ATOMIC);
-	if (pi == NULL) {
+	if (pi == NULL)
 		return NULL;
-	}
 	pi->wiphy = wiphy;
-	pi->regs = (d11regs_t *) regs;
+	pi->regs = regs;
 	pi->sh = sh;
 	pi->phy_init_por = true;
 	pi->phy_wreg_limit = PHY_WREG_LIMIT;
 
-	pi->vars = vars;
-
 	pi->txpwr_percent = 100;
 
 	pi->do_initcal = true;
 
 	pi->phycal_tempdelta = 0;
 
-	if (BAND_2G(bandtype) && (sflags & SISF_2G_PHY)) {
-
+	if (bandtype == BRCM_BAND_2G && (sflags & SISF_2G_PHY))
 		pi->pubpi.coreflags = SICF_GMODE;
-	}
 
 	wlapi_bmac_corereset(pi->sh->physhim, pi->pubpi.coreflags);
 	phyversion = R_REG(&pi->regs->phyversion);
@@ -572,28 +515,26 @@ wlc_phy_attach(struct shared_phy *sh, void *regs, int bandtype,
 	pi->pubpi.phy_corenum = PHY_CORE_NUM_2;
 	pi->pubpi.ana_rev = (phyversion & PV_AV_MASK) >> PV_AV_SHIFT;
 
-	if (!VALID_PHYTYPE(pi->pubpi.phy_type)) {
+	if (!pi->pubpi.phy_type == PHY_TYPE_N &&
+	    !pi->pubpi.phy_type == PHY_TYPE_LCN)
 		goto err;
-	}
-	if (BAND_5G(bandtype)) {
-		if (!ISNPHY(pi)) {
-			goto err;
-		}
-	} else {
-		if (!ISNPHY(pi) && !ISLCNPHY(pi)) {
+
+	if (bandtype == BRCM_BAND_5G) {
+		if (!ISNPHY(pi))
 			goto err;
-		}
+	} else if (!ISNPHY(pi) && !ISLCNPHY(pi)) {
+		goto err;
 	}
 
 	wlc_phy_anacore((struct brcms_phy_pub *) pi, ON);
 
 	idcode = wlc_phy_get_radio_ver(pi);
 	pi->pubpi.radioid =
-	    (idcode & IDCODE_ID_MASK) >> IDCODE_ID_SHIFT;
+		(idcode & IDCODE_ID_MASK) >> IDCODE_ID_SHIFT;
 	pi->pubpi.radiorev =
-	    (idcode & IDCODE_REV_MASK) >> IDCODE_REV_SHIFT;
+		(idcode & IDCODE_REV_MASK) >> IDCODE_REV_SHIFT;
 	pi->pubpi.radiover =
-	    (idcode & IDCODE_VER_MASK) >> IDCODE_VER_SHIFT;
+		(idcode & IDCODE_VER_MASK) >> IDCODE_VER_SHIFT;
 	if (!VALID_RADIO(pi, pi->pubpi.radioid))
 		goto err;
 
@@ -602,8 +543,8 @@ wlc_phy_attach(struct shared_phy *sh, void *regs, int bandtype,
 	wlc_set_phy_uninitted(pi);
 
 	pi->bw = WL_CHANSPEC_BW_20;
-	pi->radio_chanspec =
-	    BAND_2G(bandtype) ? CH20MHZ_CHSPEC(1) : CH20MHZ_CHSPEC(36);
+	pi->radio_chanspec = (bandtype == BRCM_BAND_2G) ?
+			     ch20mhz_chspec(1) : ch20mhz_chspec(36);
 
 	pi->rxiq_samps = PHY_NOISE_SAMPLE_LOG_NUM_NPHY;
 	pi->rxiq_antsel = ANT_RX_DIV_DEF;
@@ -625,7 +566,7 @@ wlc_phy_attach(struct shared_phy *sh, void *regs, int bandtype,
 
 	pi->phy_txcore_disable_temp = PHY_CHAIN_TX_DISABLE_TEMP;
 	pi->phy_txcore_enable_temp =
-	    PHY_CHAIN_TX_DISABLE_TEMP - PHY_HYSTERESIS_DELTATEMP;
+		PHY_CHAIN_TX_DISABLE_TEMP - PHY_HYSTERESIS_DELTATEMP;
 	pi->phy_tempsense_offset = 0;
 	pi->phy_txcore_heatedup = false;
 
@@ -648,11 +589,10 @@ wlc_phy_attach(struct shared_phy *sh, void *regs, int bandtype,
 	if (ISNPHY(pi)) {
 
 		pi->phycal_timer = wlapi_init_timer(pi->sh->physhim,
-							  wlc_phy_timercb_phycal,
-							  pi, "phycal");
-		if (!pi->phycal_timer) {
+						    wlc_phy_timercb_phycal,
+						    pi, "phycal");
+		if (!pi->phycal_timer)
 			goto err;
-		}
 
 		if (!wlc_phy_attach_nphy(pi))
 			goto err;
@@ -661,21 +601,17 @@ wlc_phy_attach(struct shared_phy *sh, void *regs, int bandtype,
 		if (!wlc_phy_attach_lcnphy(pi))
 			goto err;
 
-	} else {
-
 	}
 
 	pi->refcnt++;
 	pi->next = pi->sh->phy_head;
 	sh->phy_head = pi;
 
-	pi->vars = (char *)&pi->vars;
-
 	memcpy(&pi->pubpi_ro, &pi->pubpi, sizeof(struct brcms_phy_pub));
 
 	return &pi->pubpi_ro;
 
- err:
+err:
 	kfree(pi);
 	return NULL;
 }
@@ -685,12 +621,11 @@ void wlc_phy_detach(struct brcms_phy_pub *pih)
 	struct brcms_phy *pi = (struct brcms_phy *) pih;
 
 	if (pih) {
-		if (--pi->refcnt) {
+		if (--pi->refcnt)
 			return;
-		}
 
 		if (pi->phycal_timer) {
-			wlapi_free_timer(pi->sh->physhim, pi->phycal_timer);
+			wlapi_free_timer(pi->phycal_timer);
 			pi->phycal_timer = NULL;
 		}
 
@@ -700,7 +635,7 @@ void wlc_phy_detach(struct brcms_phy_pub *pih)
 			pi->sh->phy_head->next = NULL;
 
 		if (pi->pi_fptr.detach)
-			(pi->pi_fptr.detach) (pi);
+			(pi->pi_fptr.detach)(pi);
 
 		kfree(pi);
 	}
@@ -731,29 +666,6 @@ u32 wlc_phy_get_coreflags(struct brcms_phy_pub *pih)
 	return pi->pubpi.coreflags;
 }
 
-static void wlc_phy_timercb_phycal(void *arg)
-{
-	struct brcms_phy *pi = (struct brcms_phy *) arg;
-	uint delay = 5;
-
-	if (PHY_PERICAL_MPHASE_PENDING(pi)) {
-		if (!pi->sh->up) {
-			wlc_phy_cal_perical_mphase_reset(pi);
-			return;
-		}
-
-		if (SCAN_RM_IN_PROGRESS(pi) || PLT_INPROG_PHY(pi)) {
-
-			delay = 1000;
-			wlc_phy_cal_perical_mphase_restart(pi);
-		} else
-			wlc_phy_cal_perical_nphy_run(pi, PHY_PERICAL_AUTO);
-		wlapi_add_timer(pi->sh->physhim, pi->phycal_timer, delay, 0);
-		return;
-	}
-
-}
-
 void wlc_phy_anacore(struct brcms_phy_pub *pih, bool on)
 {
 	struct brcms_phy *pi = (struct brcms_phy *) pih;
@@ -862,10 +774,10 @@ void wlc_phy_hw_state_upd(struct brcms_phy_pub *pih, bool newstate)
 	pi->sh->up = newstate;
 }
 
-void wlc_phy_init(struct brcms_phy_pub *pih, chanspec_t chanspec)
+void wlc_phy_init(struct brcms_phy_pub *pih, u16 chanspec)
 {
 	u32 mc;
-	initfn_t phy_init = NULL;
+	void (*phy_init)(struct brcms_phy *) = NULL;
 	struct brcms_phy *pi = (struct brcms_phy *) pih;
 
 	if (pi->init_in_progress)
@@ -879,9 +791,8 @@ void wlc_phy_init(struct brcms_phy_pub *pih, chanspec_t chanspec)
 	if (WARN(mc & MCTL_EN_MAC, "HW error MAC running on init"))
 		return;
 
-	if (!(pi->measure_hold & PHY_HOLD_FOR_SCAN)) {
+	if (!(pi->measure_hold & PHY_HOLD_FOR_SCAN))
 		pi->measure_hold |= PHY_HOLD_FOR_NOT_ASSOC;
-	}
 
 	if (WARN(!(ai_core_sflags(pi->sh->sih, 0, 0) & SISF_FCLKA),
 		 "HW error SISF_FCLKA\n"))
@@ -889,9 +800,8 @@ void wlc_phy_init(struct brcms_phy_pub *pih, chanspec_t chanspec)
 
 	phy_init = pi->pi_fptr.init;
 
-	if (phy_init == NULL) {
+	if (phy_init == NULL)
 		return;
-	}
 
 	wlc_phy_anacore(pih, ON);
 
@@ -903,7 +813,7 @@ void wlc_phy_init(struct brcms_phy_pub *pih, chanspec_t chanspec)
 
 	wlc_phy_switch_radio((struct brcms_phy_pub *) pi, ON);
 
-	(*phy_init) (pi);
+	(*phy_init)(pi);
 
 	pi->phy_init_por = false;
 
@@ -921,7 +831,7 @@ void wlc_phy_init(struct brcms_phy_pub *pih, chanspec_t chanspec)
 void wlc_phy_cal_init(struct brcms_phy_pub *pih)
 {
 	struct brcms_phy *pi = (struct brcms_phy *) pih;
-	initfn_t cal_init = NULL;
+	void (*cal_init)(struct brcms_phy *) = NULL;
 
 	if (WARN((R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC) != 0,
 		 "HW error: MAC enabled during phy cal\n"))
@@ -930,7 +840,7 @@ void wlc_phy_cal_init(struct brcms_phy_pub *pih)
 	if (!pi->initialized) {
 		cal_init = pi->pi_fptr.calinit;
 		if (cal_init)
-			(*cal_init) (pi);
+			(*cal_init)(pi);
 
 		pi->initialized = true;
 	}
@@ -942,7 +852,7 @@ int wlc_phy_down(struct brcms_phy_pub *pih)
 	int callbacks = 0;
 
 	if (pi->phycal_timer
-	    && !wlapi_del_timer(pi->sh->physhim, pi->phycal_timer))
+	    && !wlapi_del_timer(pi->phycal_timer))
 		callbacks++;
 
 	pi->nphy_iqcal_chanspec_2G = 0;
@@ -951,15 +861,6 @@ int wlc_phy_down(struct brcms_phy_pub *pih)
 	return callbacks;
 }
 
-static u32 wlc_phy_get_radio_ver(struct brcms_phy *pi)
-{
-	u32 ver;
-
-	ver = read_radio_id(pi);
-
-	return ver;
-}
-
 void
 wlc_phy_table_addr(struct brcms_phy *pi, uint tbl_id, uint tbl_offset,
 		   u16 tblAddr, u16 tblDataHi, u16 tblDataLo)
@@ -990,11 +891,9 @@ void wlc_phy_table_data_write(struct brcms_phy *pi, uint width, u32 val)
 	}
 
 	if (width == 32) {
-
 		write_phy_reg(pi, pi->tbl_data_hi, (u16) (val >> 16));
 		write_phy_reg(pi, pi->tbl_data_lo, (u16) val);
 	} else {
-
 		write_phy_reg(pi, pi->tbl_data_lo, (u16) val);
 	}
 }
@@ -1025,15 +924,12 @@ wlc_phy_write_table(struct brcms_phy *pi, const struct phytbl_info *ptbl_info,
 		}
 
 		if (tbl_width == 32) {
-
 			write_phy_reg(pi, tblDataHi,
 				      (u16) (ptbl_32b[idx] >> 16));
 			write_phy_reg(pi, tblDataLo, (u16) ptbl_32b[idx]);
 		} else if (tbl_width == 16) {
-
 			write_phy_reg(pi, tblDataLo, ptbl_16b[idx]);
 		} else {
-
 			write_phy_reg(pi, tblDataLo, ptbl_8b[idx]);
 		}
 	}
@@ -1064,14 +960,11 @@ wlc_phy_read_table(struct brcms_phy *pi, const struct phytbl_info *ptbl_info,
 		}
 
 		if (tbl_width == 32) {
-
 			ptbl_32b[idx] = read_phy_reg(pi, tblDataLo);
 			ptbl_32b[idx] |= (read_phy_reg(pi, tblDataHi) << 16);
 		} else if (tbl_width == 16) {
-
 			ptbl_16b[idx] = read_phy_reg(pi, tblDataLo);
 		} else {
-
 			ptbl_8b[idx] = (u8) read_phy_reg(pi, tblDataLo);
 		}
 	}
@@ -1084,10 +977,9 @@ wlc_phy_init_radio_regs_allbands(struct brcms_phy *pi,
 	uint i = 0;
 
 	do {
-		if (radioregs[i].do_init) {
+		if (radioregs[i].do_init)
 			write_radio_reg(pi, radioregs[i].address,
 					(u16) radioregs[i].init);
-		}
 
 		i++;
 	} while (radioregs[i].address != 0xffff);
@@ -1096,7 +988,8 @@ wlc_phy_init_radio_regs_allbands(struct brcms_phy *pi,
 }
 
 uint
-wlc_phy_init_radio_regs(struct brcms_phy *pi, struct radio_regs *radioregs,
+wlc_phy_init_radio_regs(struct brcms_phy *pi,
+			const struct radio_regs *radioregs,
 			u16 core_offset)
 {
 	uint i = 0;
@@ -1131,8 +1024,8 @@ wlc_phy_init_radio_regs(struct brcms_phy *pi, struct radio_regs *radioregs,
 
 void wlc_phy_do_dummy_tx(struct brcms_phy *pi, bool ofdm, bool pa_on)
 {
-#define	DUMMY_PKT_LEN	20
-	d11regs_t *regs = pi->regs;
+#define DUMMY_PKT_LEN   20
+	struct d11regs __iomem *regs = pi->regs;
 	int i, count;
 	u8 ofdmpkt[DUMMY_PKT_LEN] = {
 		0xcc, 0x01, 0x02, 0x00, 0x00, 0x00, 0xd4, 0x00, 0x00, 0x00,
@@ -1156,9 +1049,8 @@ void wlc_phy_do_dummy_tx(struct brcms_phy *pi, bool ofdm, bool pa_on)
 		W_REG(&regs->wepctl, 0);
 
 	W_REG(&regs->txe_phyctl, (ofdm ? 1 : 0) | PHY_TXC_ANT_0);
-	if (ISNPHY(pi) || ISLCNPHY(pi)) {
+	if (ISNPHY(pi) || ISLCNPHY(pi))
 		W_REG(&regs->txe_phyctl1, 0x1A02);
-	}
 
 	W_REG(&regs->txe_wm_0, 0);
 	W_REG(&regs->txe_wm_1, 0);
@@ -1185,16 +1077,14 @@ void wlc_phy_do_dummy_tx(struct brcms_phy *pi, bool ofdm, bool pa_on)
 	i = 0;
 	count = ofdm ? 30 : 250;
 	while ((i++ < count)
-	       && (R_REG(&regs->txe_status) & (1 << 7))) {
+	       && (R_REG(&regs->txe_status) & (1 << 7)))
 		udelay(10);
-	}
 
 	i = 0;
 
 	while ((i++ < 10)
-	       && ((R_REG(&regs->txe_status) & (1 << 10)) == 0)) {
+	       && ((R_REG(&regs->txe_status) & (1 << 10)) == 0))
 		udelay(10);
-	}
 
 	i = 0;
 
@@ -1207,28 +1097,26 @@ void wlc_phy_do_dummy_tx(struct brcms_phy *pi, bool ofdm, bool pa_on)
 	}
 }
 
-void wlc_phy_hold_upd(struct brcms_phy_pub *pih, mbool id, bool set)
+void wlc_phy_hold_upd(struct brcms_phy_pub *pih, u32 id, bool set)
 {
 	struct brcms_phy *pi = (struct brcms_phy *) pih;
 
-	if (set) {
+	if (set)
 		mboolset(pi->measure_hold, id);
-	} else {
+	else
 		mboolclr(pi->measure_hold, id);
-	}
 
 	return;
 }
 
-void wlc_phy_mute_upd(struct brcms_phy_pub *pih, bool mute, mbool flags)
+void wlc_phy_mute_upd(struct brcms_phy_pub *pih, bool mute, u32 flags)
 {
 	struct brcms_phy *pi = (struct brcms_phy *) pih;
 
-	if (mute) {
+	if (mute)
 		mboolset(pi->measure_hold, PHY_HOLD_FOR_MUTE);
-	} else {
+	else
 		mboolclr(pi->measure_hold, PHY_HOLD_FOR_MUTE);
-	}
 
 	if (!mute && (flags & PHY_MUTE_FOR_PREISM))
 		pi->nphy_perical_last = pi->sh->now - pi->sh->glacial_timer;
@@ -1257,19 +1145,10 @@ static bool wlc_phy_cal_txpower_recalc_sw(struct brcms_phy *pi)
 void wlc_phy_switch_radio(struct brcms_phy_pub *pih, bool on)
 {
 	struct brcms_phy *pi = (struct brcms_phy *) pih;
-
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
-
-	{
-		uint mc;
-
-		mc = R_REG(&pi->regs->maccontrol);
-	}
+	(void)R_REG(&pi->regs->maccontrol);
 
 	if (ISNPHY(pi)) {
 		wlc_phy_switch_radio_nphy(pi, on);
-
 	} else if (ISLCNPHY(pi)) {
 		if (on) {
 			and_phy_reg(pi, 0x44c,
@@ -1311,26 +1190,25 @@ void wlc_phy_bw_state_set(struct brcms_phy_pub *ppi, u16 bw)
 	pi->bw = bw;
 }
 
-void wlc_phy_chanspec_radio_set(struct brcms_phy_pub *ppi, chanspec_t newch)
+void wlc_phy_chanspec_radio_set(struct brcms_phy_pub *ppi, u16 newch)
 {
 	struct brcms_phy *pi = (struct brcms_phy *) ppi;
 	pi->radio_chanspec = newch;
 
 }
 
-chanspec_t wlc_phy_chanspec_get(struct brcms_phy_pub *ppi)
+u16 wlc_phy_chanspec_get(struct brcms_phy_pub *ppi)
 {
 	struct brcms_phy *pi = (struct brcms_phy *) ppi;
 
 	return pi->radio_chanspec;
 }
 
-void wlc_phy_chanspec_set(struct brcms_phy_pub *ppi, chanspec_t chanspec)
+void wlc_phy_chanspec_set(struct brcms_phy_pub *ppi, u16 chanspec)
 {
 	struct brcms_phy *pi = (struct brcms_phy *) ppi;
 	u16 m_cur_channel;
-	chansetfn_t chanspec_set = NULL;
-
+	void (*chanspec_set)(struct brcms_phy *, u16) = NULL;
 	m_cur_channel = CHSPEC_CHANNEL(chanspec);
 	if (CHSPEC_IS5G(chanspec))
 		m_cur_channel |= D11_CURCHANNEL_5G;
@@ -1340,7 +1218,7 @@ void wlc_phy_chanspec_set(struct brcms_phy_pub *ppi, chanspec_t chanspec)
 
 	chanspec_set = pi->pi_fptr.chanset;
 	if (chanspec_set)
-		(*chanspec_set) (pi, chanspec);
+		(*chanspec_set)(pi, chanspec);
 
 }
 
@@ -1360,17 +1238,16 @@ int wlc_phy_chanspec_freq2bandrange_lpssn(uint freq)
 	return range;
 }
 
-int wlc_phy_chanspec_bandrange_get(struct brcms_phy *pi, chanspec_t chanspec)
+int wlc_phy_chanspec_bandrange_get(struct brcms_phy *pi, u16 chanspec)
 {
 	int range = -1;
 	uint channel = CHSPEC_CHANNEL(chanspec);
 	uint freq = wlc_phy_channel2freq(channel);
 
-	if (ISNPHY(pi)) {
+	if (ISNPHY(pi))
 		range = wlc_phy_get_chan_freq_range_nphy(pi, channel);
-	} else if (ISLCNPHY(pi)) {
+	else if (ISLCNPHY(pi))
 		range = wlc_phy_chanspec_freq2bandrange_lpssn(freq);
-	}
 
 	return range;
 }
@@ -1396,13 +1273,13 @@ int wlc_phy_channel2freq(uint channel)
 
 void
 wlc_phy_chanspec_band_validch(struct brcms_phy_pub *ppi, uint band,
-			      chanvec_t *channels)
+			      struct brcms_chanvec *channels)
 {
 	struct brcms_phy *pi = (struct brcms_phy *) ppi;
 	uint i;
 	uint channel;
 
-	memset(channels, 0, sizeof(chanvec_t));
+	memset(channels, 0, sizeof(struct brcms_chanvec));
 
 	for (i = 0; i < ARRAY_SIZE(chan_info_all); i++) {
 		channel = chan_info_all[i].chan;
@@ -1417,17 +1294,17 @@ wlc_phy_chanspec_band_validch(struct brcms_phy_pub *ppi, uint band,
 	}
 }
 
-chanspec_t wlc_phy_chanspec_band_firstch(struct brcms_phy_pub *ppi, uint band)
+u16 wlc_phy_chanspec_band_firstch(struct brcms_phy_pub *ppi, uint band)
 {
 	struct brcms_phy *pi = (struct brcms_phy *) ppi;
 	uint i;
 	uint channel;
-	chanspec_t chspec;
+	u16 chspec;
 
 	for (i = 0; i < ARRAY_SIZE(chan_info_all); i++) {
 		channel = chan_info_all[i].chan;
 
-		if (ISNPHY(pi) && IS40MHZ(pi)) {
+		if (ISNPHY(pi) && pi->bw == WL_CHANSPEC_BW_40) {
 			uint j;
 
 			for (j = 0; j < ARRAY_SIZE(chan_info_all); j++) {
@@ -1439,16 +1316,15 @@ chanspec_t wlc_phy_chanspec_band_firstch(struct brcms_phy_pub *ppi, uint band)
 			if (j == ARRAY_SIZE(chan_info_all))
 				continue;
 
-			channel = UPPER_20_SB(channel);
-			chspec =
-			    channel | WL_CHANSPEC_BW_40 |
-			    WL_CHANSPEC_CTL_SB_LOWER;
+			channel = upper_20_sb(channel);
+			chspec =  channel | WL_CHANSPEC_BW_40 |
+				  WL_CHANSPEC_CTL_SB_LOWER;
 			if (band == BRCM_BAND_2G)
 				chspec |= WL_CHANSPEC_BAND_2G;
 			else
 				chspec |= WL_CHANSPEC_BAND_5G;
 		} else
-			chspec = CH20MHZ_CHSPEC(channel);
+			chspec = ch20mhz_chspec(channel);
 
 		if ((pi->a_band_high_disable) && (channel >= FIRST_REF5_CHANNUM)
 		    && (channel <= LAST_REF5_CHANNUM))
@@ -1459,7 +1335,7 @@ chanspec_t wlc_phy_chanspec_band_firstch(struct brcms_phy_pub *ppi, uint band)
 			return chspec;
 	}
 
-	return (chanspec_t) INVCHANSPEC;
+	return (u16) INVCHANSPEC;
 }
 
 int wlc_phy_txpower_get(struct brcms_phy_pub *ppi, uint *qdbm, bool *override)
@@ -1528,7 +1404,7 @@ int wlc_phy_txpower_set(struct brcms_phy_pub *ppi, uint qdbm, bool override)
 	int i;
 
 	if (qdbm > 127)
-		return 5;
+		return -EINVAL;
 
 	for (i = 0; i < TXP_NUM_RATES; i++)
 		pi->tx_user_target[i] = (u8) qdbm;
@@ -1539,10 +1415,8 @@ int wlc_phy_txpower_set(struct brcms_phy_pub *ppi, uint qdbm, bool override)
 		if (!SCAN_INPROG_PHY(pi)) {
 			bool suspend;
 
-			suspend =
-			    (0 ==
-			     (R_REG(&pi->regs->maccontrol) &
-			      MCTL_EN_MAC));
+			suspend = (0 == (R_REG(&pi->regs->maccontrol) &
+					 MCTL_EN_MAC));
 
 			if (!suspend)
 				wlapi_suspend_mac_and_wait(pi->sh->physhim);
@@ -1584,9 +1458,8 @@ wlc_phy_txpower_sromlimit(struct brcms_phy_pub *ppi, uint channel, u8 *min_pwr,
 			txp_rate_idx = TXP_FIRST_OFDM;
 
 		for (i = 0; i < ARRAY_SIZE(chan_info_all); i++) {
-			if (channel == chan_info_all[i].chan) {
+			if (channel == chan_info_all[i].chan)
 				break;
-			}
 		}
 
 		if (pi->hwtxpwr) {
@@ -1620,7 +1493,8 @@ wlc_phy_txpower_sromlimit_max_get(struct brcms_phy_pub *ppi, uint chan,
 	pactrl = 0;
 
 	max_num_rate = ISNPHY(pi) ? TXP_NUM_RATES :
-	    ISLCNPHY(pi) ? (TXP_LAST_SISO_MCS_20 + 1) : (TXP_LAST_OFDM + 1);
+		       ISLCNPHY(pi) ? (TXP_LAST_SISO_MCS_20 +
+				       1) : (TXP_LAST_OFDM + 1);
 
 	for (rate = 0; rate < max_num_rate; rate++) {
 
@@ -1659,6 +1533,46 @@ u8 wlc_phy_txpower_get_target_max(struct brcms_phy_pub *ppi)
 	return pi->tx_power_max;
 }
 
+static s8 wlc_phy_env_measure_vbat(struct brcms_phy *pi)
+{
+	if (ISLCNPHY(pi))
+		return wlc_lcnphy_vbatsense(pi, 0);
+	else
+		return 0;
+}
+
+static s8 wlc_phy_env_measure_temperature(struct brcms_phy *pi)
+{
+	if (ISLCNPHY(pi))
+		return wlc_lcnphy_tempsense_degree(pi, 0);
+	else
+		return 0;
+}
+
+static void wlc_phy_upd_env_txpwr_rate_limits(struct brcms_phy *pi, u32 band)
+{
+	u8 i;
+	s8 temp, vbat;
+
+	for (i = 0; i < TXP_NUM_RATES; i++)
+		pi->txpwr_env_limit[i] = BRCMS_TXPWR_MAX;
+
+	vbat = wlc_phy_env_measure_vbat(pi);
+	temp = wlc_phy_env_measure_temperature(pi);
+
+}
+
+static s8
+wlc_user_txpwr_antport_to_rfport(struct brcms_phy *pi, uint chan, u32 band,
+				 u8 rate)
+{
+	s8 offset = 0;
+
+	if (!pi->user_txpwr_at_rfport)
+		return offset;
+	return offset;
+}
+
 void wlc_phy_txpower_recalc_target(struct brcms_phy *pi)
 {
 	u8 maxtxpwr, mintxpwr, rate, pactrl;
@@ -1669,17 +1583,17 @@ void wlc_phy_txpower_recalc_target(struct brcms_phy *pi)
 	u8 tx_pwr_max_rate_ind = 0;
 	u8 max_num_rate;
 	u8 start_rate = 0;
-	chanspec_t chspec;
+	u16 chspec;
 	u32 band = CHSPEC2BAND(pi->radio_chanspec);
-	initfn_t txpwr_recalc_fn = NULL;
+	void (*txpwr_recalc_fn)(struct brcms_phy *) = NULL;
 
 	chspec = pi->radio_chanspec;
 	if (CHSPEC_CTL_SB(chspec) == WL_CHANSPEC_CTL_SB_NONE)
 		target_chan = CHSPEC_CHANNEL(chspec);
 	else if (CHSPEC_CTL_SB(chspec) == WL_CHANSPEC_CTL_SB_UPPER)
-		target_chan = UPPER_20_SB(CHSPEC_CHANNEL(chspec));
+		target_chan = upper_20_sb(CHSPEC_CHANNEL(chspec));
 	else
-		target_chan = LOWER_20_SB(CHSPEC_CHANNEL(chspec));
+		target_chan = lower_20_sb(CHSPEC_CHANNEL(chspec));
 
 	pactrl = 0;
 	if (ISLCNPHY(pi)) {
@@ -1690,8 +1604,8 @@ void wlc_phy_txpower_recalc_target(struct brcms_phy *pi)
 			for (i = TXP_FIRST_SISO_MCS_20;
 			     i <= TXP_LAST_SISO_MCS_20; i++) {
 				pi->tx_srom_max_rate_2g[i - 8] =
-				    pi->tx_srom_max_2g -
-				    ((offset_mcs & 0xf) * 2);
+					pi->tx_srom_max_2g -
+					((offset_mcs & 0xf) * 2);
 				offset_mcs >>= 4;
 			}
 		} else {
@@ -1699,19 +1613,16 @@ void wlc_phy_txpower_recalc_target(struct brcms_phy *pi)
 			for (i = TXP_FIRST_SISO_MCS_20;
 			     i <= TXP_LAST_SISO_MCS_20; i++) {
 				pi->tx_srom_max_rate_2g[i - 8] =
-				    pi->tx_srom_max_2g -
-				    ((offset_mcs & 0xf) * 2);
+					pi->tx_srom_max_2g -
+					((offset_mcs & 0xf) * 2);
 				offset_mcs >>= 4;
 			}
 		}
 	}
-#if WL11N
+
 	max_num_rate = ((ISNPHY(pi)) ? (TXP_NUM_RATES) :
 			((ISLCNPHY(pi)) ?
 			 (TXP_LAST_SISO_MCS_20 + 1) : (TXP_LAST_OFDM + 1)));
-#else
-	max_num_rate = ((ISNPHY(pi)) ? (TXP_NUM_RATES) : (TXP_LAST_OFDM + 1));
-#endif
 
 	wlc_phy_upd_env_txpwr_rate_limits(pi, band);
 
@@ -1719,35 +1630,32 @@ void wlc_phy_txpower_recalc_target(struct brcms_phy *pi)
 
 		tx_pwr_target[rate] = pi->tx_user_target[rate];
 
-		if (pi->user_txpwr_at_rfport) {
+		if (pi->user_txpwr_at_rfport)
 			tx_pwr_target[rate] +=
-			    wlc_user_txpwr_antport_to_rfport(pi, target_chan,
-							     band, rate);
-		}
+				wlc_user_txpwr_antport_to_rfport(pi,
+								 target_chan,
+								 band,
+								 rate);
 
-		{
+		wlc_phy_txpower_sromlimit((struct brcms_phy_pub *) pi,
+					  target_chan,
+					  &mintxpwr, &maxtxpwr, rate);
 
-			wlc_phy_txpower_sromlimit((struct brcms_phy_pub *) pi,
-						  target_chan,
-						  &mintxpwr, &maxtxpwr, rate);
+		maxtxpwr = min(maxtxpwr, pi->txpwr_limit[rate]);
 
-			maxtxpwr = min(maxtxpwr, pi->txpwr_limit[rate]);
-
-			maxtxpwr =
-			    (maxtxpwr > pactrl) ? (maxtxpwr - pactrl) : 0;
+		maxtxpwr = (maxtxpwr > pactrl) ? (maxtxpwr - pactrl) : 0;
 
-			maxtxpwr = (maxtxpwr > 6) ? (maxtxpwr - 6) : 0;
+		maxtxpwr = (maxtxpwr > 6) ? (maxtxpwr - 6) : 0;
 
-			maxtxpwr = min(maxtxpwr, tx_pwr_target[rate]);
+		maxtxpwr = min(maxtxpwr, tx_pwr_target[rate]);
 
-			if (pi->txpwr_percent <= 100)
-				maxtxpwr = (maxtxpwr * pi->txpwr_percent) / 100;
+		if (pi->txpwr_percent <= 100)
+			maxtxpwr = (maxtxpwr * pi->txpwr_percent) / 100;
 
-			tx_pwr_target[rate] = max(maxtxpwr, mintxpwr);
-		}
+		tx_pwr_target[rate] = max(maxtxpwr, mintxpwr);
 
 		tx_pwr_target[rate] =
-		    min(tx_pwr_target[rate], pi->txpwr_env_limit[rate]);
+			min(tx_pwr_target[rate], pi->txpwr_env_limit[rate]);
 
 		if (tx_pwr_target[rate] > tx_pwr_max)
 			tx_pwr_max_rate_ind = rate;
@@ -1764,23 +1672,22 @@ void wlc_phy_txpower_recalc_target(struct brcms_phy *pi)
 
 		pi->tx_power_target[rate] = tx_pwr_target[rate];
 
-		if (!pi->hwpwrctrl || ISNPHY(pi)) {
+		if (!pi->hwpwrctrl || ISNPHY(pi))
 			pi->tx_power_offset[rate] =
-			    pi->tx_power_max - pi->tx_power_target[rate];
-		} else {
+				pi->tx_power_max - pi->tx_power_target[rate];
+		else
 			pi->tx_power_offset[rate] =
-			    pi->tx_power_target[rate] - pi->tx_power_min;
-		}
+				pi->tx_power_target[rate] - pi->tx_power_min;
 	}
 
 	txpwr_recalc_fn = pi->pi_fptr.txpwrrecalc;
 	if (txpwr_recalc_fn)
-		(*txpwr_recalc_fn) (pi);
+		(*txpwr_recalc_fn)(pi);
 }
 
-void
+static void
 wlc_phy_txpower_reg_limit_calc(struct brcms_phy *pi, struct txpwr_limits *txpwr,
-			       chanspec_t chanspec)
+			       u16 chanspec)
 {
 	u8 tmp_txpwr_limit[2 * BRCMS_NUM_RATES_OFDM];
 	u8 *txpwr_ptr1 = NULL, *txpwr_ptr2 = NULL;
@@ -1815,7 +1722,7 @@ wlc_phy_txpower_reg_limit_calc(struct brcms_phy *pi, struct txpwr_limits *txpwr,
 				txpwr_ptr1 = txpwr->mcs_40_siso;
 				txpwr_ptr2 = txpwr->ofdm_40_siso;
 				rate_start_index =
-				    WL_TX_POWER_OFDM40_SISO_FIRST;
+					WL_TX_POWER_OFDM40_SISO_FIRST;
 				break;
 			case 3:
 
@@ -1825,18 +1732,21 @@ wlc_phy_txpower_reg_limit_calc(struct brcms_phy *pi, struct txpwr_limits *txpwr,
 				break;
 			}
 
-			for (rate2 = 0; rate2 < BRCMS_NUM_RATES_OFDM; rate2++) {
+			for (rate2 = 0; rate2 < BRCMS_NUM_RATES_OFDM;
+			     rate2++) {
 				tmp_txpwr_limit[rate2] = 0;
 				tmp_txpwr_limit[BRCMS_NUM_RATES_OFDM + rate2] =
-				    txpwr_ptr1[rate2];
+					txpwr_ptr1[rate2];
 			}
-			wlc_phy_mcs_to_ofdm_powers_nphy(tmp_txpwr_limit, 0,
-				BRCMS_NUM_RATES_OFDM - 1, BRCMS_NUM_RATES_OFDM);
+			wlc_phy_mcs_to_ofdm_powers_nphy(
+				tmp_txpwr_limit, 0,
+				BRCMS_NUM_RATES_OFDM -
+				1, BRCMS_NUM_RATES_OFDM);
 			for (rate1 = rate_start_index, rate2 = 0;
 			     rate2 < BRCMS_NUM_RATES_OFDM; rate1++, rate2++)
 				pi->txpwr_limit[rate1] =
-				    min(txpwr_ptr2[rate2],
-					tmp_txpwr_limit[rate2]);
+					min(txpwr_ptr2[rate2],
+					    tmp_txpwr_limit[rate2]);
 		}
 
 		for (k = 0; k < 4; k++) {
@@ -1866,19 +1776,22 @@ wlc_phy_txpower_reg_limit_calc(struct brcms_phy *pi, struct txpwr_limits *txpwr,
 				rate_start_index = WL_TX_POWER_MCS40_CDD_FIRST;
 				break;
 			}
-			for (rate2 = 0; rate2 < BRCMS_NUM_RATES_OFDM; rate2++) {
+			for (rate2 = 0; rate2 < BRCMS_NUM_RATES_OFDM;
+			     rate2++) {
 				tmp_txpwr_limit[rate2] = 0;
 				tmp_txpwr_limit[BRCMS_NUM_RATES_OFDM + rate2] =
-				    txpwr_ptr1[rate2];
+					txpwr_ptr1[rate2];
 			}
-			wlc_phy_ofdm_to_mcs_powers_nphy(tmp_txpwr_limit, 0,
-				BRCMS_NUM_RATES_OFDM - 1, BRCMS_NUM_RATES_OFDM);
+			wlc_phy_ofdm_to_mcs_powers_nphy(
+				tmp_txpwr_limit, 0,
+				BRCMS_NUM_RATES_OFDM -
+				1, BRCMS_NUM_RATES_OFDM);
 			for (rate1 = rate_start_index, rate2 = 0;
 			     rate2 < BRCMS_NUM_RATES_MCS_1_STREAM;
 			     rate1++, rate2++)
 				pi->txpwr_limit[rate1] =
-				    min(txpwr_ptr2[rate2],
-					tmp_txpwr_limit[rate2]);
+					min(txpwr_ptr2[rate2],
+					    tmp_txpwr_limit[rate2]);
 		}
 
 		for (k = 0; k < 2; k++) {
@@ -1922,10 +1835,10 @@ wlc_phy_txpower_reg_limit_calc(struct brcms_phy *pi, struct txpwr_limits *txpwr,
 		pi->txpwr_limit[WL_TX_POWER_MCS_32] = txpwr->mcs32;
 
 		pi->txpwr_limit[WL_TX_POWER_MCS40_CDD_FIRST] =
-		    min(pi->txpwr_limit[WL_TX_POWER_MCS40_CDD_FIRST],
-			pi->txpwr_limit[WL_TX_POWER_MCS_32]);
+			min(pi->txpwr_limit[WL_TX_POWER_MCS40_CDD_FIRST],
+			    pi->txpwr_limit[WL_TX_POWER_MCS_32]);
 		pi->txpwr_limit[WL_TX_POWER_MCS_32] =
-		    pi->txpwr_limit[WL_TX_POWER_MCS40_CDD_FIRST];
+			pi->txpwr_limit[WL_TX_POWER_MCS40_CDD_FIRST];
 	}
 }
 
@@ -1975,7 +1888,7 @@ void wlc_phy_runbist_config(struct brcms_phy_pub *ppi, bool start_end)
 
 void
 wlc_phy_txpower_limit_set(struct brcms_phy_pub *ppi, struct txpwr_limits *txpwr,
-			  chanspec_t chanspec)
+			  u16 chanspec)
 {
 	struct brcms_phy *pi = (struct brcms_phy *) ppi;
 
@@ -2016,9 +1929,8 @@ void wlc_phy_bf_preempt_enable(struct brcms_phy_pub *pih, bool bf_preempt)
 void wlc_phy_txpower_update_shm(struct brcms_phy *pi)
 {
 	int j;
-	if (ISNPHY(pi)) {
+	if (ISNPHY(pi))
 		return;
-	}
 
 	if (!pi->sh->clk)
 		return;
@@ -2040,9 +1952,9 @@ void wlc_phy_txpower_update_shm(struct brcms_phy *pi)
 			const u8 ucode_ofdm_rates[] = {
 				0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6c
 			};
-			offset = wlapi_bmac_rate_shm_offset(pi->sh->physhim,
-							    ucode_ofdm_rates[j -
-									     TXP_FIRST_OFDM]);
+			offset = wlapi_bmac_rate_shm_offset(
+				pi->sh->physhim,
+				ucode_ofdm_rates[j - TXP_FIRST_OFDM]);
 			wlapi_bmac_write_shm(pi->sh->physhim, offset + 6,
 					     pi->tx_power_offset[j]);
 			wlapi_bmac_write_shm(pi->sh->physhim, offset + 14,
@@ -2056,11 +1968,11 @@ void wlc_phy_txpower_update_shm(struct brcms_phy *pi)
 
 		for (i = TXP_FIRST_OFDM; i <= TXP_LAST_OFDM; i++)
 			pi->tx_power_offset[i] =
-			    (u8) roundup(pi->tx_power_offset[i], 8);
+				(u8) roundup(pi->tx_power_offset[i], 8);
 		wlapi_bmac_write_shm(pi->sh->physhim, M_OFDM_OFFSET,
-				     (u16) ((pi->
-						tx_power_offset[TXP_FIRST_OFDM]
-						+ 7) >> 3));
+				     (u16)
+				     ((pi->tx_power_offset[TXP_FIRST_OFDM]
+				       + 7) >> 3));
 	}
 }
 
@@ -2068,48 +1980,38 @@ bool wlc_phy_txpower_hw_ctrl_get(struct brcms_phy_pub *ppi)
 {
 	struct brcms_phy *pi = (struct brcms_phy *) ppi;
 
-	if (ISNPHY(pi)) {
+	if (ISNPHY(pi))
 		return pi->nphy_txpwrctrl;
-	} else {
+	else
 		return pi->hwpwrctrl;
-	}
 }
 
 void wlc_phy_txpower_hw_ctrl_set(struct brcms_phy_pub *ppi, bool hwpwrctrl)
 {
 	struct brcms_phy *pi = (struct brcms_phy *) ppi;
-	bool cur_hwpwrctrl = pi->hwpwrctrl;
 	bool suspend;
 
-	if (!pi->hwpwrctrl_capable) {
+	if (!pi->hwpwrctrl_capable)
 		return;
-	}
 
 	pi->hwpwrctrl = hwpwrctrl;
 	pi->nphy_txpwrctrl = hwpwrctrl;
 	pi->txpwrctrl = hwpwrctrl;
 
 	if (ISNPHY(pi)) {
-		suspend =
-		    (0 ==
-		     (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
+		suspend = (0 == (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
 		if (!suspend)
 			wlapi_suspend_mac_and_wait(pi->sh->physhim);
 
 		wlc_phy_txpwrctrl_enable_nphy(pi, pi->nphy_txpwrctrl);
-		if (pi->nphy_txpwrctrl == PHY_TPC_HW_OFF) {
+		if (pi->nphy_txpwrctrl == PHY_TPC_HW_OFF)
 			wlc_phy_txpwr_fixpower_nphy(pi);
-		} else {
-
+		else
 			mod_phy_reg(pi, 0x1e7, (0x7f << 0),
 				    pi->saved_txpwr_idx);
-		}
 
 		if (!suspend)
 			wlapi_enable_mac(pi->sh->physhim);
-	} else if (hwpwrctrl != cur_hwpwrctrl) {
-
-		return;
 	}
 }
 
@@ -2125,8 +2027,6 @@ void wlc_phy_txpower_ipa_upd(struct brcms_phy *pi)
 	}
 }
 
-static u32 wlc_phy_txpower_est_power_nphy(struct brcms_phy *pi);
-
 static u32 wlc_phy_txpower_est_power_nphy(struct brcms_phy *pi)
 {
 	s16 tx0_status, tx1_status;
@@ -2137,42 +2037,31 @@ static u32 wlc_phy_txpower_est_power_nphy(struct brcms_phy *pi)
 	estPower1 = read_phy_reg(pi, 0x118);
 	estPower2 = read_phy_reg(pi, 0x119);
 
-	if ((estPower1 & (0x1 << 8))
-	    == (0x1 << 8)) {
-		pwr0 = (u8) (estPower1 & (0xff << 0))
-		    >> 0;
-	} else {
+	if ((estPower1 & (0x1 << 8)) == (0x1 << 8))
+		pwr0 = (u8) (estPower1 & (0xff << 0)) >> 0;
+	else
 		pwr0 = 0x80;
-	}
 
-	if ((estPower2 & (0x1 << 8))
-	    == (0x1 << 8)) {
-		pwr1 = (u8) (estPower2 & (0xff << 0))
-		    >> 0;
-	} else {
+	if ((estPower2 & (0x1 << 8)) == (0x1 << 8))
+		pwr1 = (u8) (estPower2 & (0xff << 0)) >> 0;
+	else
 		pwr1 = 0x80;
-	}
 
 	tx0_status = read_phy_reg(pi, 0x1ed);
 	tx1_status = read_phy_reg(pi, 0x1ee);
 
-	if ((tx0_status & (0x1 << 15))
-	    == (0x1 << 15)) {
-		adj_pwr0 = (u8) (tx0_status & (0xff << 0))
-		    >> 0;
-	} else {
+	if ((tx0_status & (0x1 << 15)) == (0x1 << 15))
+		adj_pwr0 = (u8) (tx0_status & (0xff << 0)) >> 0;
+	else
 		adj_pwr0 = 0x80;
-	}
-	if ((tx1_status & (0x1 << 15))
-	    == (0x1 << 15)) {
-		adj_pwr1 = (u8) (tx1_status & (0xff << 0))
-		    >> 0;
-	} else {
+	if ((tx1_status & (0x1 << 15)) == (0x1 << 15))
+		adj_pwr1 = (u8) (tx1_status & (0xff << 0)) >> 0;
+	else
 		adj_pwr1 = 0x80;
-	}
 
-	est_pwr =
-	    (u32) ((pwr0 << 24) | (pwr1 << 16) | (adj_pwr0 << 8) | adj_pwr1);
+	est_pwr = (u32) ((pwr0 << 24) | (pwr1 << 16) | (adj_pwr0 << 8) |
+			 adj_pwr1);
+
 	return est_pwr;
 }
 
@@ -2193,7 +2082,7 @@ wlc_phy_txpower_get_current(struct brcms_phy_pub *ppi, struct tx_power *power,
 		power->flags |= (WL_TX_POWER_F_MIMO);
 		if (pi->nphy_txpwrctrl == PHY_TPC_HW_ON)
 			power->flags |=
-			    (WL_TX_POWER_F_ENABLED | WL_TX_POWER_F_HW);
+				(WL_TX_POWER_F_ENABLED | WL_TX_POWER_F_HW);
 	} else if (ISLCNPHY(pi)) {
 		power->rf_cores = 1;
 		power->flags |= (WL_TX_POWER_F_SISO);
@@ -2256,16 +2145,18 @@ wlc_phy_txpower_get_current(struct brcms_phy_pub *ppi, struct tx_power *power,
 			power->tx_power_max[1] = pi->tx_power_max;
 
 			power->tx_power_max_rate_ind[0] =
-			    pi->tx_power_max_rate_ind;
+				pi->tx_power_max_rate_ind;
 			power->tx_power_max_rate_ind[1] =
-			    pi->tx_power_max_rate_ind;
+				pi->tx_power_max_rate_ind;
 
 			if (wlc_phy_tpc_isenabled_lcnphy(pi))
 				power->flags |=
-				    (WL_TX_POWER_F_HW | WL_TX_POWER_F_ENABLED);
+					(WL_TX_POWER_F_HW |
+					 WL_TX_POWER_F_ENABLED);
 			else
 				power->flags &=
-				    ~(WL_TX_POWER_F_HW | WL_TX_POWER_F_ENABLED);
+					~(WL_TX_POWER_F_HW |
+					  WL_TX_POWER_F_ENABLED);
 
 			wlc_lcnphy_get_tssi(pi, (s8 *) &power->est_Pout[0],
 					    (s8 *) &power->est_Pout_cck);
@@ -2304,16 +2195,13 @@ void wlc_phy_ant_rxdiv_set(struct brcms_phy_pub *ppi, u8 val)
 				       BRCM_BAND_ALL);
 	}
 
-	if (ISNPHY(pi)) {
-
+	if (ISNPHY(pi))
 		return;
-	}
 
 	if (!pi->sh->clk)
 		return;
 
-	suspend =
-	    (0 == (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
+	suspend = (0 == (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
 	if (!suspend)
 		wlapi_suspend_mac_and_wait(pi->sh->physhim);
 
@@ -2357,10 +2245,127 @@ wlc_phy_noise_calc_phy(struct brcms_phy *pi, u32 *cmplx_pwr, s8 *pwr_ant)
 		pwr_ant[i] = cmplx_pwr_dbm[i];
 	}
 	pi->nphy_noise_index =
-	    MODINC_POW2(pi->nphy_noise_index, PHY_NOISE_WINDOW_SZ);
+		MODINC_POW2(pi->nphy_noise_index, PHY_NOISE_WINDOW_SZ);
 	return true;
 }
 
+static void wlc_phy_noise_cb(struct brcms_phy *pi, u8 channel, s8 noise_dbm)
+{
+	if (!pi->phynoise_state)
+		return;
+
+	if (pi->phynoise_state & PHY_NOISE_STATE_MON) {
+		if (pi->phynoise_chan_watchdog == channel) {
+			pi->sh->phy_noise_window[pi->sh->phy_noise_index] =
+				noise_dbm;
+			pi->sh->phy_noise_index =
+				MODINC(pi->sh->phy_noise_index, MA_WINDOW_SZ);
+		}
+		pi->phynoise_state &= ~PHY_NOISE_STATE_MON;
+	}
+
+	if (pi->phynoise_state & PHY_NOISE_STATE_EXTERNAL)
+		pi->phynoise_state &= ~PHY_NOISE_STATE_EXTERNAL;
+
+}
+
+static s8 wlc_phy_noise_read_shmem(struct brcms_phy *pi)
+{
+	u32 cmplx_pwr[PHY_CORE_MAX];
+	s8 noise_dbm_ant[PHY_CORE_MAX];
+	u16 lo, hi;
+	u32 cmplx_pwr_tot = 0;
+	s8 noise_dbm = PHY_NOISE_FIXED_VAL_NPHY;
+	u8 idx, core;
+
+	memset((u8 *) cmplx_pwr, 0, sizeof(cmplx_pwr));
+	memset((u8 *) noise_dbm_ant, 0, sizeof(noise_dbm_ant));
+
+	for (idx = 0, core = 0; core < pi->pubpi.phy_corenum; idx += 2,
+	     core++) {
+		lo = wlapi_bmac_read_shm(pi->sh->physhim, M_PWRIND_MAP(idx));
+		hi = wlapi_bmac_read_shm(pi->sh->physhim,
+					 M_PWRIND_MAP(idx + 1));
+		cmplx_pwr[core] = (hi << 16) + lo;
+		cmplx_pwr_tot += cmplx_pwr[core];
+		if (cmplx_pwr[core] == 0)
+			noise_dbm_ant[core] = PHY_NOISE_FIXED_VAL_NPHY;
+		else
+			cmplx_pwr[core] >>= PHY_NOISE_SAMPLE_LOG_NUM_UCODE;
+	}
+
+	if (cmplx_pwr_tot != 0)
+		wlc_phy_noise_calc_phy(pi, cmplx_pwr, noise_dbm_ant);
+
+	for (core = 0; core < pi->pubpi.phy_corenum; core++) {
+		pi->nphy_noise_win[core][pi->nphy_noise_index] =
+			noise_dbm_ant[core];
+
+		if (noise_dbm_ant[core] > noise_dbm)
+			noise_dbm = noise_dbm_ant[core];
+	}
+	pi->nphy_noise_index =
+		MODINC_POW2(pi->nphy_noise_index, PHY_NOISE_WINDOW_SZ);
+
+	return noise_dbm;
+
+}
+
+void wlc_phy_noise_sample_intr(struct brcms_phy_pub *pih)
+{
+	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	u16 jssi_aux;
+	u8 channel = 0;
+	s8 noise_dbm = PHY_NOISE_FIXED_VAL_NPHY;
+
+	if (ISLCNPHY(pi)) {
+		u32 cmplx_pwr, cmplx_pwr0, cmplx_pwr1;
+		u16 lo, hi;
+		s32 pwr_offset_dB, gain_dB;
+		u16 status_0, status_1;
+
+		jssi_aux = wlapi_bmac_read_shm(pi->sh->physhim, M_JSSI_AUX);
+		channel = jssi_aux & D11_CURCHANNEL_MAX;
+
+		lo = wlapi_bmac_read_shm(pi->sh->physhim, M_PWRIND_MAP0);
+		hi = wlapi_bmac_read_shm(pi->sh->physhim, M_PWRIND_MAP1);
+		cmplx_pwr0 = (hi << 16) + lo;
+
+		lo = wlapi_bmac_read_shm(pi->sh->physhim, M_PWRIND_MAP2);
+		hi = wlapi_bmac_read_shm(pi->sh->physhim, M_PWRIND_MAP3);
+		cmplx_pwr1 = (hi << 16) + lo;
+		cmplx_pwr = (cmplx_pwr0 + cmplx_pwr1) >> 6;
+
+		status_0 = 0x44;
+		status_1 = wlapi_bmac_read_shm(pi->sh->physhim, M_JSSI_0);
+		if ((cmplx_pwr > 0 && cmplx_pwr < 500)
+		    && ((status_1 & 0xc000) == 0x4000)) {
+
+			wlc_phy_compute_dB(&cmplx_pwr, &noise_dbm,
+					   pi->pubpi.phy_corenum);
+			pwr_offset_dB = (read_phy_reg(pi, 0x434) & 0xFF);
+			if (pwr_offset_dB > 127)
+				pwr_offset_dB -= 256;
+
+			noise_dbm += (s8) (pwr_offset_dB - 30);
+
+			gain_dB = (status_0 & 0x1ff);
+			noise_dbm -= (s8) (gain_dB);
+		} else {
+			noise_dbm = PHY_NOISE_FIXED_VAL_LCNPHY;
+		}
+	} else if (ISNPHY(pi)) {
+
+		jssi_aux = wlapi_bmac_read_shm(pi->sh->physhim, M_JSSI_AUX);
+		channel = jssi_aux & D11_CURCHANNEL_MAX;
+
+		noise_dbm = wlc_phy_noise_read_shmem(pi);
+	}
+
+	wlc_phy_noise_cb(pi, channel, noise_dbm);
+
+}
+
 static void
 wlc_phy_noise_sample_request(struct brcms_phy_pub *pih, u8 reason, u8 ch)
 {
@@ -2369,20 +2374,13 @@ wlc_phy_noise_sample_request(struct brcms_phy_pub *pih, u8 reason, u8 ch)
 	bool sampling_in_progress = (pi->phynoise_state != 0);
 	bool wait_for_intr = true;
 
-	if (NORADIO_ENAB(pi->pubpi)) {
-		return;
-	}
-
 	switch (reason) {
 	case PHY_NOISE_SAMPLE_MON:
-
 		pi->phynoise_chan_watchdog = ch;
 		pi->phynoise_state |= PHY_NOISE_STATE_MON;
-
 		break;
 
 	case PHY_NOISE_SAMPLE_EXTERNAL:
-
 		pi->phynoise_state |= PHY_NOISE_STATE_EXTERNAL;
 		break;
 
@@ -2398,15 +2396,13 @@ wlc_phy_noise_sample_request(struct brcms_phy_pub *pih, u8 reason, u8 ch)
 	if (pi->phy_fixed_noise) {
 		if (ISNPHY(pi)) {
 			pi->nphy_noise_win[WL_ANT_IDX_1][pi->nphy_noise_index] =
-			    PHY_NOISE_FIXED_VAL_NPHY;
+				PHY_NOISE_FIXED_VAL_NPHY;
 			pi->nphy_noise_win[WL_ANT_IDX_2][pi->nphy_noise_index] =
-			    PHY_NOISE_FIXED_VAL_NPHY;
+				PHY_NOISE_FIXED_VAL_NPHY;
 			pi->nphy_noise_index = MODINC_POW2(pi->nphy_noise_index,
 							   PHY_NOISE_WINDOW_SZ);
-
 			noise_dbm = PHY_NOISE_FIXED_VAL_NPHY;
 		} else {
-
 			noise_dbm = PHY_NOISE_FIXED_VAL;
 		}
 
@@ -2469,15 +2465,14 @@ wlc_phy_noise_sample_request(struct brcms_phy_pub *pih, u8 reason, u8 ch)
 			wlapi_enable_mac(pi->sh->physhim);
 
 			for (i = 0; i < pi->pubpi.phy_corenum; i++)
-				cmplx_pwr[i] =
-				    (est[i].i_pwr +
-				     est[i].q_pwr) >> log_num_samps;
+				cmplx_pwr[i] = (est[i].i_pwr + est[i].q_pwr) >>
+					       log_num_samps;
 
 			wlc_phy_noise_calc_phy(pi, cmplx_pwr, noise_dbm_ant);
 
 			for (i = 0; i < pi->pubpi.phy_corenum; i++) {
 				pi->nphy_noise_win[i][pi->nphy_noise_index] =
-				    noise_dbm_ant[i];
+					noise_dbm_ant[i];
 
 				if (noise_dbm_ant[i] > noise_dbm)
 					noise_dbm = noise_dbm_ant[i];
@@ -2489,7 +2484,7 @@ wlc_phy_noise_sample_request(struct brcms_phy_pub *pih, u8 reason, u8 ch)
 		}
 	}
 
- done:
+done:
 
 	if (!wait_for_intr)
 		wlc_phy_noise_cb(pi, ch, noise_dbm);
@@ -2505,124 +2500,7 @@ void wlc_phy_noise_sample_request_external(struct brcms_phy_pub *pih)
 	wlc_phy_noise_sample_request(pih, PHY_NOISE_SAMPLE_EXTERNAL, channel);
 }
 
-static void wlc_phy_noise_cb(struct brcms_phy *pi, u8 channel, s8 noise_dbm)
-{
-	if (!pi->phynoise_state)
-		return;
-
-	if (pi->phynoise_state & PHY_NOISE_STATE_MON) {
-		if (pi->phynoise_chan_watchdog == channel) {
-			pi->sh->phy_noise_window[pi->sh->phy_noise_index] =
-			    noise_dbm;
-			pi->sh->phy_noise_index =
-			    MODINC(pi->sh->phy_noise_index, MA_WINDOW_SZ);
-		}
-		pi->phynoise_state &= ~PHY_NOISE_STATE_MON;
-	}
-
-	if (pi->phynoise_state & PHY_NOISE_STATE_EXTERNAL) {
-		pi->phynoise_state &= ~PHY_NOISE_STATE_EXTERNAL;
-	}
-
-}
-
-static s8 wlc_phy_noise_read_shmem(struct brcms_phy *pi)
-{
-	u32 cmplx_pwr[PHY_CORE_MAX];
-	s8 noise_dbm_ant[PHY_CORE_MAX];
-	u16 lo, hi;
-	u32 cmplx_pwr_tot = 0;
-	s8 noise_dbm = PHY_NOISE_FIXED_VAL_NPHY;
-	u8 idx, core;
-
-	memset((u8 *) cmplx_pwr, 0, sizeof(cmplx_pwr));
-	memset((u8 *) noise_dbm_ant, 0, sizeof(noise_dbm_ant));
-
-	for (idx = 0, core = 0; core < pi->pubpi.phy_corenum; idx += 2, core++) {
-		lo = wlapi_bmac_read_shm(pi->sh->physhim, M_PWRIND_MAP(idx));
-		hi = wlapi_bmac_read_shm(pi->sh->physhim,
-					 M_PWRIND_MAP(idx + 1));
-		cmplx_pwr[core] = (hi << 16) + lo;
-		cmplx_pwr_tot += cmplx_pwr[core];
-		if (cmplx_pwr[core] == 0) {
-			noise_dbm_ant[core] = PHY_NOISE_FIXED_VAL_NPHY;
-		} else
-			cmplx_pwr[core] >>= PHY_NOISE_SAMPLE_LOG_NUM_UCODE;
-	}
-
-	if (cmplx_pwr_tot != 0)
-		wlc_phy_noise_calc_phy(pi, cmplx_pwr, noise_dbm_ant);
-
-	for (core = 0; core < pi->pubpi.phy_corenum; core++) {
-		pi->nphy_noise_win[core][pi->nphy_noise_index] =
-		    noise_dbm_ant[core];
-
-		if (noise_dbm_ant[core] > noise_dbm)
-			noise_dbm = noise_dbm_ant[core];
-	}
-	pi->nphy_noise_index =
-	    MODINC_POW2(pi->nphy_noise_index, PHY_NOISE_WINDOW_SZ);
-
-	return noise_dbm;
-
-}
-
-void wlc_phy_noise_sample_intr(struct brcms_phy_pub *pih)
-{
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
-	u16 jssi_aux;
-	u8 channel = 0;
-	s8 noise_dbm = PHY_NOISE_FIXED_VAL_NPHY;
-
-	if (ISLCNPHY(pi)) {
-		u32 cmplx_pwr, cmplx_pwr0, cmplx_pwr1;
-		u16 lo, hi;
-		s32 pwr_offset_dB, gain_dB;
-		u16 status_0, status_1;
-
-		jssi_aux = wlapi_bmac_read_shm(pi->sh->physhim, M_JSSI_AUX);
-		channel = jssi_aux & D11_CURCHANNEL_MAX;
-
-		lo = wlapi_bmac_read_shm(pi->sh->physhim, M_PWRIND_MAP0);
-		hi = wlapi_bmac_read_shm(pi->sh->physhim, M_PWRIND_MAP1);
-		cmplx_pwr0 = (hi << 16) + lo;
-
-		lo = wlapi_bmac_read_shm(pi->sh->physhim, M_PWRIND_MAP2);
-		hi = wlapi_bmac_read_shm(pi->sh->physhim, M_PWRIND_MAP3);
-		cmplx_pwr1 = (hi << 16) + lo;
-		cmplx_pwr = (cmplx_pwr0 + cmplx_pwr1) >> 6;
-
-		status_0 = 0x44;
-		status_1 = wlapi_bmac_read_shm(pi->sh->physhim, M_JSSI_0);
-		if ((cmplx_pwr > 0 && cmplx_pwr < 500)
-		    && ((status_1 & 0xc000) == 0x4000)) {
-
-			wlc_phy_compute_dB(&cmplx_pwr, &noise_dbm,
-					   pi->pubpi.phy_corenum);
-			pwr_offset_dB = (read_phy_reg(pi, 0x434) & 0xFF);
-			if (pwr_offset_dB > 127)
-				pwr_offset_dB -= 256;
-
-			noise_dbm += (s8) (pwr_offset_dB - 30);
-
-			gain_dB = (status_0 & 0x1ff);
-			noise_dbm -= (s8) (gain_dB);
-		} else {
-			noise_dbm = PHY_NOISE_FIXED_VAL_LCNPHY;
-		}
-	} else if (ISNPHY(pi)) {
-
-		jssi_aux = wlapi_bmac_read_shm(pi->sh->physhim, M_JSSI_AUX);
-		channel = jssi_aux & D11_CURCHANNEL_MAX;
-
-		noise_dbm = wlc_phy_noise_read_shmem(pi);
-	}
-
-	wlc_phy_noise_cb(pi, channel, noise_dbm);
-
-}
-
-s8 lcnphy_gain_index_offset_for_pkt_rssi[] = {
+static const s8 lcnphy_gain_index_offset_for_pkt_rssi[] = {
 	8,
 	8,
 	8,
@@ -2678,27 +2556,21 @@ void wlc_phy_compute_dB(u32 *cmplx_pwr, s8 *p_cmplx_pwr_dB, u8 core)
 	}
 }
 
-void wlc_phy_rssi_compute(struct brcms_phy_pub *pih, void *ctx)
+int wlc_phy_rssi_compute(struct brcms_phy_pub *pih,
+			 struct d11rxhdr *rxh)
 {
-	struct brcms_d11rxhdr *wlc_rxhdr = (struct brcms_d11rxhdr *) ctx;
-	struct d11rxhdr *rxh = &wlc_rxhdr->rxhdr;
-	int rssi = le16_to_cpu(rxh->PhyRxStatus_1) & PRXS1_JSSI_MASK;
+	int rssi = rxh->PhyRxStatus_1 & PRXS1_JSSI_MASK;
 	uint radioid = pih->radioid;
 	struct brcms_phy *pi = (struct brcms_phy *) pih;
 
-	if (NORADIO_ENAB(pi->pubpi)) {
-		rssi = BRCMS_RSSI_INVALID;
-		goto end;
-	}
-
 	if ((pi->sh->corerev >= 11)
-	    && !(le16_to_cpu(rxh->RxStatus2) & RXS_PHYRXST_VALID)) {
+	    && !(rxh->RxStatus2 & RXS_PHYRXST_VALID)) {
 		rssi = BRCMS_RSSI_INVALID;
 		goto end;
 	}
 
 	if (ISLCNPHY(pi)) {
-		u8 gidx = (le16_to_cpu(rxh->PhyRxStatus_2) & 0xFC00) >> 10;
+		u8 gidx = (rxh->PhyRxStatus_2 & 0xFC00) >> 10;
 		struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
 
 		if (rssi > 127)
@@ -2715,16 +2587,15 @@ void wlc_phy_rssi_compute(struct brcms_phy_pub *pih, void *ctx)
 	}
 
 	if (ISLCNPHY(pi)) {
-
 		if (rssi > 127)
 			rssi -= 256;
 	} else if (radioid == BCM2055_ID || radioid == BCM2056_ID
 		   || radioid == BCM2057_ID) {
-		rssi = wlc_phy_rssi_compute_nphy(pi, wlc_rxhdr);
+		rssi = wlc_phy_rssi_compute_nphy(pi, rxh);
 	}
 
- end:
-	wlc_rxhdr->rssi = (s8) rssi;
+end:
+	return rssi;
 }
 
 void wlc_phy_freqtrack_start(struct brcms_phy_pub *pih)
@@ -2757,28 +2628,22 @@ void wlc_phy_watchdog(struct brcms_phy_pub *pih)
 	if (!pi->watchdog_override)
 		return;
 
-	if (!(SCAN_RM_IN_PROGRESS(pi) || PLT_INPROG_PHY(pi))) {
+	if (!(SCAN_RM_IN_PROGRESS(pi) || PLT_INPROG_PHY(pi)))
 		wlc_phy_noise_sample_request((struct brcms_phy_pub *) pi,
 					     PHY_NOISE_SAMPLE_MON,
 					     CHSPEC_CHANNEL(pi->
 							    radio_chanspec));
-	}
 
-	if (pi->phynoise_state && (pi->sh->now - pi->phynoise_now) > 5) {
+	if (pi->phynoise_state && (pi->sh->now - pi->phynoise_now) > 5)
 		pi->phynoise_state = 0;
-	}
 
 	if ((!pi->phycal_txpower) ||
 	    ((pi->sh->now - pi->phycal_txpower) >= pi->sh->fast_timer)) {
 
-		if (!SCAN_INPROG_PHY(pi) && wlc_phy_cal_txpower_recalc_sw(pi)) {
+		if (!SCAN_INPROG_PHY(pi) && wlc_phy_cal_txpower_recalc_sw(pi))
 			pi->phycal_txpower = pi->sh->now;
-		}
 	}
 
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
-
 	if ((SCAN_RM_IN_PROGRESS(pi) || PLT_INPROG_PHY(pi)
 	     || ASSOC_INPROG_PHY(pi)))
 		return;
@@ -2800,8 +2665,9 @@ void wlc_phy_watchdog(struct brcms_phy_pub *pih)
 		    ((pi->sh->now - pi->phy_lastcal) >=
 		     pi->sh->glacial_timer)) {
 			if (!(SCAN_RM_IN_PROGRESS(pi) || ASSOC_INPROG_PHY(pi)))
-				wlc_lcnphy_calib_modes(pi,
-						       LCNPHY_PERICAL_TEMPBASED_TXPWRCTRL);
+				wlc_lcnphy_calib_modes(
+					pi,
+					LCNPHY_PERICAL_TEMPBASED_TXPWRCTRL);
 			if (!
 			    (SCAN_RM_IN_PROGRESS(pi) || PLT_INPROG_PHY(pi)
 			     || ASSOC_INPROG_PHY(pi)
@@ -2819,13 +2685,12 @@ void wlc_phy_BSSinit(struct brcms_phy_pub *pih, bool bonlyap, int rssi)
 	uint i;
 	uint k;
 
-	for (i = 0; i < MA_WINDOW_SZ; i++) {
+	for (i = 0; i < MA_WINDOW_SZ; i++)
 		pi->sh->phy_noise_window[i] = (s8) (rssi & 0xff);
-	}
 	if (ISLCNPHY(pi)) {
 		for (i = 0; i < MA_WINDOW_SZ; i++)
 			pi->sh->phy_noise_window[i] =
-			    PHY_NOISE_FIXED_VAL_LCNPHY;
+				PHY_NOISE_FIXED_VAL_LCNPHY;
 	}
 	pi->sh->phy_noise_index = 0;
 
@@ -2848,72 +2713,9 @@ wlc_phy_papd_decode_epsilon(u32 epsilon, s32 *eps_real, s32 *eps_imag)
 		*eps_real -= 0x2000;
 }
 
-static const fixed AtanTbl[] = {
-	2949120,
-	1740967,
-	919879,
-	466945,
-	234379,
-	117304,
-	58666,
-	29335,
-	14668,
-	7334,
-	3667,
-	1833,
-	917,
-	458,
-	229,
-	115,
-	57,
-	29
-};
-
-void wlc_phy_cordic(fixed theta, cs32 *val)
-{
-	fixed angle, valtmp;
-	unsigned iter;
-	int signx = 1;
-	int signtheta;
-
-	val[0].i = CORDIC_AG;
-	val[0].q = 0;
-	angle = 0;
-
-	signtheta = (theta < 0) ? -1 : 1;
-	theta =
-	    ((theta + FIXED(180) * signtheta) % FIXED(360)) -
-	    FIXED(180) * signtheta;
-
-	if (FLOAT(theta) > 90) {
-		theta -= FIXED(180);
-		signx = -1;
-	} else if (FLOAT(theta) < -90) {
-		theta += FIXED(180);
-		signx = -1;
-	}
-
-	for (iter = 0; iter < CORDIC_NI; iter++) {
-		if (theta > angle) {
-			valtmp = val[0].i - (val[0].q >> iter);
-			val[0].q = (val[0].i >> iter) + val[0].q;
-			val[0].i = valtmp;
-			angle += AtanTbl[iter];
-		} else {
-			valtmp = val[0].i + (val[0].q >> iter);
-			val[0].q = -(val[0].i >> iter) + val[0].q;
-			val[0].i = valtmp;
-			angle -= AtanTbl[iter];
-		}
-	}
-
-	val[0].i = val[0].i * signx;
-	val[0].q = val[0].q * signx;
-}
-
 void wlc_phy_cal_perical_mphase_reset(struct brcms_phy *pi)
 {
-	wlapi_del_timer(pi->sh->physhim, pi->phycal_timer);
+	wlapi_del_timer(pi->phycal_timer);
 
 	pi->cal_type_override = PHY_PERICAL_AUTO;
 	pi->mphase_cal_phase_id = MPHASE_CAL_STATE_IDLE;
@@ -2928,10 +2730,10 @@ wlc_phy_cal_perical_mphase_schedule(struct brcms_phy *pi, uint delay)
 	    (pi->nphy_perical != PHY_PERICAL_MANUAL))
 		return;
 
-	wlapi_del_timer(pi->sh->physhim, pi->phycal_timer);
+	wlapi_del_timer(pi->phycal_timer);
 
 	pi->mphase_cal_phase_id = MPHASE_CAL_STATE_INIT;
-	wlapi_add_timer(pi->sh->physhim, pi->phycal_timer, delay, 0);
+	wlapi_add_timer(pi->phycal_timer, delay, 0);
 }
 
 void wlc_phy_cal_perical(struct brcms_phy_pub *pih, u8 reason)
@@ -2954,11 +2756,12 @@ void wlc_phy_cal_perical(struct brcms_phy_pub *pih, u8 reason)
 
 	case PHY_PERICAL_PHYINIT:
 		if (pi->nphy_perical == PHY_PERICAL_MPHASE) {
-			if (PHY_PERICAL_MPHASE_PENDING(pi)) {
+			if (PHY_PERICAL_MPHASE_PENDING(pi))
 				wlc_phy_cal_perical_mphase_reset(pi);
-			}
-			wlc_phy_cal_perical_mphase_schedule(pi,
-							    PHY_PERICAL_INIT_DELAY);
+
+			wlc_phy_cal_perical_mphase_schedule(
+				pi,
+				PHY_PERICAL_INIT_DELAY);
 		}
 		break;
 
@@ -2966,17 +2769,16 @@ void wlc_phy_cal_perical(struct brcms_phy_pub *pih, u8 reason)
 	case PHY_PERICAL_START_IBSS:
 	case PHY_PERICAL_UP_BSS:
 		if ((pi->nphy_perical == PHY_PERICAL_MPHASE) &&
-		    PHY_PERICAL_MPHASE_PENDING(pi)) {
+		    PHY_PERICAL_MPHASE_PENDING(pi))
 			wlc_phy_cal_perical_mphase_reset(pi);
-		}
 
 		pi->first_cal_after_assoc = true;
 
 		pi->cal_type_override = PHY_PERICAL_FULL;
 
-		if (pi->phycal_tempdelta) {
+		if (pi->phycal_tempdelta)
 			pi->nphy_lastcal_temp = wlc_phy_tempsense_nphy(pi);
-		}
+
 		wlc_phy_cal_perical_nphy_run(pi, PHY_PERICAL_FULL);
 		break;
 
@@ -2984,26 +2786,24 @@ void wlc_phy_cal_perical(struct brcms_phy_pub *pih, u8 reason)
 		if (pi->phycal_tempdelta) {
 			nphy_currtemp = wlc_phy_tempsense_nphy(pi);
 			delta_temp =
-			    (nphy_currtemp > pi->nphy_lastcal_temp) ?
-			    nphy_currtemp - pi->nphy_lastcal_temp :
-			    pi->nphy_lastcal_temp - nphy_currtemp;
+				(nphy_currtemp > pi->nphy_lastcal_temp) ?
+				nphy_currtemp - pi->nphy_lastcal_temp :
+				pi->nphy_lastcal_temp - nphy_currtemp;
 
 			if ((delta_temp < (s16) pi->phycal_tempdelta) &&
 			    (pi->nphy_txiqlocal_chanspec ==
-			     pi->radio_chanspec)) {
+			     pi->radio_chanspec))
 				do_periodic_cal = false;
-			} else {
+			else
 				pi->nphy_lastcal_temp = nphy_currtemp;
-			}
 		}
 
 		if (do_periodic_cal) {
-
 			if (pi->nphy_perical == PHY_PERICAL_MPHASE) {
-
 				if (!PHY_PERICAL_MPHASE_PENDING(pi))
-					wlc_phy_cal_perical_mphase_schedule(pi,
-									    PHY_PERICAL_WDOG_DELAY);
+					wlc_phy_cal_perical_mphase_schedule(
+						pi,
+						PHY_PERICAL_WDOG_DELAY);
 			} else if (pi->nphy_perical == PHY_PERICAL_SPHASE)
 				wlc_phy_cal_perical_nphy_run(pi,
 							     PHY_PERICAL_AUTO);
@@ -3025,7 +2825,7 @@ u8 wlc_phy_nbits(s32 value)
 	s32 abs_val;
 	u8 nbits = 0;
 
-	abs_val = ABS(value);
+	abs_val = abs(value);
 	while ((abs_val >> nbits) > 0)
 		nbits++;
 
@@ -3040,7 +2840,7 @@ void wlc_phy_stf_chain_init(struct brcms_phy_pub *pih, u8 txchain, u8 rxchain)
 	pi->sh->hw_phyrxchain = rxchain;
 	pi->sh->phytxchain = txchain;
 	pi->sh->phyrxchain = rxchain;
-	pi->pubpi.phy_corenum = (u8) PHY_BITSCNT(pi->sh->phyrxchain);
+	pi->pubpi.phy_corenum = (u8)hweight8(pi->sh->phyrxchain);
 }
 
 void wlc_phy_stf_chain_set(struct brcms_phy_pub *pih, u8 txchain, u8 rxchain)
@@ -3049,10 +2849,10 @@ void wlc_phy_stf_chain_set(struct brcms_phy_pub *pih, u8 txchain, u8 rxchain)
 
 	pi->sh->phytxchain = txchain;
 
-	if (ISNPHY(pi)) {
+	if (ISNPHY(pi))
 		wlc_phy_rxcore_setstate_nphy(pih, rxchain);
-	}
-	pi->pubpi.phy_corenum = (u8) PHY_BITSCNT(pi->sh->phyrxchain);
+
+	pi->pubpi.phy_corenum = (u8)hweight8(pi->sh->phyrxchain);
 }
 
 void wlc_phy_stf_chain_get(struct brcms_phy_pub *pih, u8 *txchain, u8 *rxchain)
@@ -3095,17 +2895,17 @@ u8 wlc_phy_stf_chain_active_get(struct brcms_phy_pub *pih)
 	return active_bitmap;
 }
 
-s8 wlc_phy_stf_ssmode_get(struct brcms_phy_pub *pih, chanspec_t chanspec)
+s8 wlc_phy_stf_ssmode_get(struct brcms_phy_pub *pih, u16 chanspec)
 {
 	struct brcms_phy *pi = (struct brcms_phy *) pih;
 	u8 siso_mcs_id, cdd_mcs_id;
 
 	siso_mcs_id =
-	    (CHSPEC_IS40(chanspec)) ? TXP_FIRST_MCS_40_SISO :
-	    TXP_FIRST_MCS_20_SISO;
+		(CHSPEC_IS40(chanspec)) ? TXP_FIRST_MCS_40_SISO :
+		TXP_FIRST_MCS_20_SISO;
 	cdd_mcs_id =
-	    (CHSPEC_IS40(chanspec)) ? TXP_FIRST_MCS_40_CDD :
-	    TXP_FIRST_MCS_20_CDD;
+		(CHSPEC_IS40(chanspec)) ? TXP_FIRST_MCS_40_CDD :
+		TXP_FIRST_MCS_20_CDD;
 
 	if (pi->tx_power_target[siso_mcs_id] >
 	    (pi->tx_power_target[cdd_mcs_id] + 12))
@@ -3133,12 +2933,13 @@ void wlc_lcnphy_epa_switch(struct brcms_phy *pi, bool mode)
 
 			}
 			ai_corereg(pi->sh->sih, SI_CC_IDX,
-				   offsetof(chipcregs_t, gpiocontrol), ~0x0,
-				   0x0);
+				   offsetof(struct chipcregs, gpiocontrol),
+				   ~0x0, 0x0);
 			ai_corereg(pi->sh->sih, SI_CC_IDX,
-				   offsetof(chipcregs_t, gpioout), 0x40, 0x40);
+				   offsetof(struct chipcregs, gpioout), 0x40,
+				   0x40);
 			ai_corereg(pi->sh->sih, SI_CC_IDX,
-				   offsetof(chipcregs_t, gpioouten), 0x40,
+				   offsetof(struct chipcregs, gpioouten), 0x40,
 				   0x40);
 		} else {
 			mod_phy_reg(pi, 0x44c, (0x1 << 2), (0) << 2);
@@ -3146,56 +2947,18 @@ void wlc_lcnphy_epa_switch(struct brcms_phy *pi, bool mode)
 			mod_phy_reg(pi, 0x44d, (0x1 << 2), (0) << 2);
 
 			ai_corereg(pi->sh->sih, SI_CC_IDX,
-				   offsetof(chipcregs_t, gpioout), 0x40, 0x00);
+				   offsetof(struct chipcregs, gpioout), 0x40,
+				   0x00);
 			ai_corereg(pi->sh->sih, SI_CC_IDX,
-				   offsetof(chipcregs_t, gpioouten), 0x40, 0x0);
+				   offsetof(struct chipcregs, gpioouten), 0x40,
+				   0x0);
 			ai_corereg(pi->sh->sih, SI_CC_IDX,
-				   offsetof(chipcregs_t, gpiocontrol), ~0x0,
-				   0x40);
+				   offsetof(struct chipcregs, gpiocontrol),
+				   ~0x0, 0x40);
 		}
 	}
 }
 
-static s8
-wlc_user_txpwr_antport_to_rfport(struct brcms_phy *pi, uint chan, u32 band,
-				 u8 rate)
-{
-	s8 offset = 0;
-
-	if (!pi->user_txpwr_at_rfport)
-		return offset;
-	return offset;
-}
-
-static s8 wlc_phy_env_measure_vbat(struct brcms_phy *pi)
-{
-	if (ISLCNPHY(pi))
-		return wlc_lcnphy_vbatsense(pi, 0);
-	else
-		return 0;
-}
-
-static s8 wlc_phy_env_measure_temperature(struct brcms_phy *pi)
-{
-	if (ISLCNPHY(pi))
-		return wlc_lcnphy_tempsense_degree(pi, 0);
-	else
-		return 0;
-}
-
-static void wlc_phy_upd_env_txpwr_rate_limits(struct brcms_phy *pi, u32 band)
-{
-	u8 i;
-	s8 temp, vbat;
-
-	for (i = 0; i < TXP_NUM_RATES; i++)
-		pi->txpwr_env_limit[i] = BRCMS_TXPWR_MAX;
-
-	vbat = wlc_phy_env_measure_vbat(pi);
-	temp = wlc_phy_env_measure_temperature(pi);
-
-}
-
 void wlc_phy_ldpc_override_set(struct brcms_phy_pub *ppi, bool ldpc)
 {
 	return;
@@ -3208,7 +2971,7 @@ wlc_phy_get_pwrdet_offsets(struct brcms_phy *pi, s8 *cckoffset, s8 *ofdmoffset)
 	*ofdmoffset = 0;
 }
 
-s8 wlc_phy_upd_rssi_offset(struct brcms_phy *pi, s8 rssi, chanspec_t chanspec)
+s8 wlc_phy_upd_rssi_offset(struct brcms_phy *pi, s8 rssi, u16 chanspec)
 {
 
 	return rssi;
diff --git a/drivers/staging/brcm80211/brcmsmac/phy/phy_hal.h b/drivers/staging/brcm80211/brcmsmac/phy/phy_hal.h
index e27d9e95a2dc..96e15163222b 100644
--- a/drivers/staging/brcm80211/brcmsmac/phy/phy_hal.h
+++ b/drivers/staging/brcm80211/brcmsmac/phy/phy_hal.h
@@ -103,6 +103,9 @@
 
 #define	BRCMS_RSSI_INVALID	 0	/* invalid RSSI value */
 
+struct d11regs;
+struct phy_shim_info;
+
 struct txpwr_limits {
 	u8 cck[BRCMS_NUM_RATES_CCK];
 	u8 ofdm[BRCMS_NUM_RATES_OFDM];
@@ -126,42 +129,44 @@ struct txpwr_limits {
 
 struct tx_power {
 	u32 flags;
-	chanspec_t chanspec;	/* txpwr report for this channel */
-	chanspec_t local_chanspec;	/* channel on which we are associated */
-	u8 local_max;	/* local max according to the AP */
-	u8 local_constraint;	/* local constraint according to the AP */
-	s8 antgain[2];	/* Ant gain for each band - from SROM */
-	u8 rf_cores;		/* count of RF Cores being reported */
-	u8 est_Pout[4];	/* Latest tx power out estimate per RF chain */
-	u8 est_Pout_act[4];	/* Latest tx power out estimate per RF chain
-				 * without adjustment
-				 */
-	u8 est_Pout_cck;	/* Latest CCK tx power out estimate */
-	u8 tx_power_max[4];	/* Maximum target power among all rates */
-	u8 tx_power_max_rate_ind[4];	/* Index of the rate with the max target power */
-	u8 user_limit[WL_TX_POWER_RATES];	/* User limit */
-	u8 reg_limit[WL_TX_POWER_RATES];	/* Regulatory power limit */
-	u8 board_limit[WL_TX_POWER_RATES];	/* Max power board can support (SROM) */
-	u8 target[WL_TX_POWER_RATES];	/* Latest target power */
+	u16 chanspec;   /* txpwr report for this channel */
+	u16 local_chanspec;     /* channel on which we are associated */
+	u8 local_max;   /* local max according to the AP */
+	u8 local_constraint;    /* local constraint according to the AP */
+	s8 antgain[2];  /* Ant gain for each band - from SROM */
+	u8 rf_cores;            /* count of RF Cores being reported */
+	u8 est_Pout[4]; /* Latest tx power out estimate per RF chain */
+	u8 est_Pout_act[4];     /* Latest tx power out estimate per RF chain
+				 * without adjustment */
+	u8 est_Pout_cck;        /* Latest CCK tx power out estimate */
+	u8 tx_power_max[4];     /* Maximum target power among all rates */
+	/* Index of the rate with the max target power */
+	u8 tx_power_max_rate_ind[4];
+	/* User limit */
+	u8 user_limit[WL_TX_POWER_RATES];
+	/* Regulatory power limit */
+	u8 reg_limit[WL_TX_POWER_RATES];
+	/* Max power board can support (SROM) */
+	u8 board_limit[WL_TX_POWER_RATES];
+	/* Latest target power */
+	u8 target[WL_TX_POWER_RATES];
 };
 
 struct tx_inst_power {
-	u8 txpwr_est_Pout[2];	/* Latest estimate for 2.4 and 5 Ghz */
-	u8 txpwr_est_Pout_gofdm;	/* Pwr estimate for 2.4 OFDM */
+	u8 txpwr_est_Pout[2];   /* Latest estimate for 2.4 and 5 Ghz */
+	u8 txpwr_est_Pout_gofdm;        /* Pwr estimate for 2.4 OFDM */
 };
 
-struct chanvec {
+struct brcms_chanvec {
 	u8 vec[MAXCHANNEL / NBBY];
 };
 
 struct shared_phy_params {
 	struct si_pub *sih;
-	void *physhim;
+	struct phy_shim_info *physhim;
 	uint unit;
 	uint corerev;
-	uint bustype;
 	uint buscorerev;
-	char *vars;
 	u16 vid;
 	u16 did;
 	uint chip;
@@ -177,8 +182,9 @@ struct shared_phy_params {
 
 
 extern struct shared_phy *wlc_phy_shared_attach(struct shared_phy_params *shp);
-extern struct brcms_phy_pub *wlc_phy_attach(struct shared_phy *sh, void *regs,
-				 int bandtype, char *vars, struct wiphy *wiphy);
+extern struct brcms_phy_pub *wlc_phy_attach(struct shared_phy *sh,
+					    struct d11regs __iomem *regs,
+					    int bandtype, struct wiphy *wiphy);
 extern void wlc_phy_detach(struct brcms_phy_pub *ppi);
 
 extern bool wlc_phy_get_phyversion(struct brcms_phy_pub *pih, u16 *phytype,
@@ -189,7 +195,7 @@ extern u32 wlc_phy_get_coreflags(struct brcms_phy_pub *pih);
 
 extern void wlc_phy_hw_clk_state_upd(struct brcms_phy_pub *ppi, bool newstate);
 extern void wlc_phy_hw_state_upd(struct brcms_phy_pub *ppi, bool newstate);
-extern void wlc_phy_init(struct brcms_phy_pub *ppi, chanspec_t chanspec);
+extern void wlc_phy_init(struct brcms_phy_pub *ppi, u16 chanspec);
 extern void wlc_phy_watchdog(struct brcms_phy_pub *ppi);
 extern int wlc_phy_down(struct brcms_phy_pub *ppi);
 extern u32 wlc_phy_clk_bwbits(struct brcms_phy_pub *pih);
@@ -197,14 +203,15 @@ extern void wlc_phy_cal_init(struct brcms_phy_pub *ppi);
 extern void wlc_phy_antsel_init(struct brcms_phy_pub *ppi, bool lut_init);
 
 extern void wlc_phy_chanspec_set(struct brcms_phy_pub *ppi,
-				 chanspec_t chanspec);
-extern chanspec_t wlc_phy_chanspec_get(struct brcms_phy_pub *ppi);
+				 u16 chanspec);
+extern u16 wlc_phy_chanspec_get(struct brcms_phy_pub *ppi);
 extern void wlc_phy_chanspec_radio_set(struct brcms_phy_pub *ppi,
-				       chanspec_t newch);
+				       u16 newch);
 extern u16 wlc_phy_bw_state_get(struct brcms_phy_pub *ppi);
 extern void wlc_phy_bw_state_set(struct brcms_phy_pub *ppi, u16 bw);
 
-extern void wlc_phy_rssi_compute(struct brcms_phy_pub *pih, void *ctx);
+extern int wlc_phy_rssi_compute(struct brcms_phy_pub *pih,
+				struct d11rxhdr *rxh);
 extern void wlc_phy_por_inform(struct brcms_phy_pub *ppi);
 extern void wlc_phy_noise_sample_intr(struct brcms_phy_pub *ppi);
 extern bool wlc_phy_bist_check_phy(struct brcms_phy_pub *ppi);
@@ -220,9 +227,9 @@ extern void wlc_phy_BSSinit(struct brcms_phy_pub *ppi, bool bonlyap, int rssi);
 extern void wlc_phy_chanspec_ch14_widefilter_set(struct brcms_phy_pub *ppi,
 						 bool wide_filter);
 extern void wlc_phy_chanspec_band_validch(struct brcms_phy_pub *ppi, uint band,
-					  chanvec_t *channels);
-extern chanspec_t wlc_phy_chanspec_band_firstch(struct brcms_phy_pub *ppi,
-						uint band);
+					  struct brcms_chanvec *channels);
+extern u16 wlc_phy_chanspec_band_firstch(struct brcms_phy_pub *ppi,
+					 uint band);
 
 extern void wlc_phy_txpower_sromlimit(struct brcms_phy_pub *ppi, uint chan,
 				      u8 *_min_, u8 *_max_, int rate);
@@ -232,7 +239,7 @@ extern void wlc_phy_txpower_boardlimit_band(struct brcms_phy_pub *ppi,
 					    uint band, s32 *, s32 *, u32 *);
 extern void wlc_phy_txpower_limit_set(struct brcms_phy_pub *ppi,
 				      struct txpwr_limits *,
-				      chanspec_t chanspec);
+				      u16 chanspec);
 extern int wlc_phy_txpower_get(struct brcms_phy_pub *ppi, uint *qdbm,
 			       bool *override);
 extern int wlc_phy_txpower_set(struct brcms_phy_pub *ppi, uint qdbm,
@@ -254,7 +261,7 @@ extern void wlc_phy_stf_chain_get(struct brcms_phy_pub *pih, u8 *txchain,
 				  u8 *rxchain);
 extern u8 wlc_phy_stf_chain_active_get(struct brcms_phy_pub *pih);
 extern s8 wlc_phy_stf_ssmode_get(struct brcms_phy_pub *pih,
-				 chanspec_t chanspec);
+				 u16 chanspec);
 extern void wlc_phy_ldpc_override_set(struct brcms_phy_pub *ppi, bool val);
 
 extern void wlc_phy_cal_perical(struct brcms_phy_pub *ppi, u8 reason);
@@ -264,8 +271,8 @@ extern void wlc_phy_cal_papd_recal(struct brcms_phy_pub *ppi);
 
 extern void wlc_phy_ant_rxdiv_set(struct brcms_phy_pub *ppi, u8 val);
 extern void wlc_phy_clear_tssi(struct brcms_phy_pub *ppi);
-extern void wlc_phy_hold_upd(struct brcms_phy_pub *ppi, mbool id, bool val);
-extern void wlc_phy_mute_upd(struct brcms_phy_pub *ppi, bool val, mbool flags);
+extern void wlc_phy_hold_upd(struct brcms_phy_pub *ppi, u32 id, bool val);
+extern void wlc_phy_mute_upd(struct brcms_phy_pub *ppi, bool val, u32 flags);
 
 extern void wlc_phy_antsel_type_set(struct brcms_phy_pub *ppi, u8 antsel_type);
 
@@ -289,6 +296,6 @@ extern void wlc_phy_freqtrack_end(struct brcms_phy_pub *ppi);
 extern const u8 *wlc_phy_get_ofdm_rate_lookup(void);
 
 extern s8 wlc_phy_get_tx_power_offset_by_mcs(struct brcms_phy_pub *ppi,
-					       u8 mcs_offset);
+					     u8 mcs_offset);
 extern s8 wlc_phy_get_tx_power_offset(struct brcms_phy_pub *ppi, u8 tbl_offset);
-#endif				/* _BRCM_PHY_HAL_H_ */
+#endif                          /* _BRCM_PHY_HAL_H_ */
diff --git a/drivers/staging/brcm80211/brcmsmac/phy/phy_int.h b/drivers/staging/brcm80211/brcmsmac/phy/phy_int.h
index a01b01ccd9ff..bea85241a244 100644
--- a/drivers/staging/brcm80211/brcmsmac/phy/phy_int.h
+++ b/drivers/staging/brcm80211/brcmsmac/phy/phy_int.h
@@ -23,53 +23,26 @@
 
 #define	PHY_VERSION			{ 1, 82, 8, 0 }
 
-#define PHYHAL_ERROR	0x0001
-#define PHYHAL_TRACE	0x0002
-#define PHYHAL_INFORM	0x0004
-
-extern u32 phyhal_msg_level;
-
-#define PHY_INFORM_ON()		(phyhal_msg_level & PHYHAL_INFORM)
-#define PHY_THERMAL_ON()	(phyhal_msg_level & PHYHAL_THERMAL)
-#define PHY_CAL_ON()		(phyhal_msg_level & PHYHAL_CAL)
-
-#ifdef BOARD_TYPE
-#define BOARDTYPE(_type) BOARD_TYPE
-#else
-#define BOARDTYPE(_type) _type
-#endif
-
 #define LCNXN_BASEREV		16
 
+struct phy_shim_info;
+
 struct brcms_phy_srom_fem {
-	u8 tssipos;		/* TSSI positive slope, 1: positive, 0: negative */
-	u8 extpagain;	/* Ext PA gain-type: full-gain: 0, pa-lite: 1, no_pa: 2 */
-	u8 pdetrange;	/* support 32 combinations of different Pdet dynamic ranges */
-	u8 triso;		/* TR switch isolation */
-	u8 antswctrllut;	/* antswctrl lookup table configuration: 32 possible choices */
+	/* TSSI positive slope, 1: positive, 0: negative */
+	u8 tssipos;
+	/* Ext PA gain-type: full-gain: 0, pa-lite: 1, no_pa: 2 */
+	u8 extpagain;
+	/* support 32 combinations of different Pdet dynamic ranges */
+	u8 pdetrange;
+	/* TR switch isolation */
+	u8 triso;
+	/* antswctrl lookup table configuration: 32 possible choices */
+	u8 antswctrllut;
 };
 
-typedef void (*initfn_t) (struct brcms_phy *);
-typedef void (*chansetfn_t) (struct brcms_phy *, chanspec_t);
-typedef int (*longtrnfn_t) (struct brcms_phy *, int);
-typedef void (*txiqccgetfn_t) (struct brcms_phy *, u16 *, u16 *);
-typedef void (*txiqccsetfn_t) (struct brcms_phy *, u16, u16);
-typedef u16(*txloccgetfn_t) (struct brcms_phy *);
-typedef void (*radioloftgetfn_t) (struct brcms_phy *, u8 *, u8 *, u8 *,
-				  u8 *);
-typedef s32(*rxsigpwrfn_t) (struct brcms_phy *, s32);
-typedef void (*detachfn_t) (struct brcms_phy *);
-
-#undef ISNPHY
-#undef ISLCNPHY
 #define ISNPHY(pi)	PHYTYPE_IS((pi)->pubpi.phy_type, PHY_TYPE_N)
 #define ISLCNPHY(pi)	PHYTYPE_IS((pi)->pubpi.phy_type, PHY_TYPE_LCN)
 
-#define ISPHY_11N_CAP(pi)	(ISNPHY(pi) || ISLCNPHY(pi))
-
-#define IS20MHZ(pi)	((pi)->bw == WL_CHANSPEC_BW_20)
-#define IS40MHZ(pi)	((pi)->bw == WL_CHANSPEC_BW_40)
-
 #define PHY_GET_RFATTN(rfgain)	((rfgain) & 0x0f)
 #define PHY_GET_PADMIX(rfgain)	(((rfgain) & 0x10) >> 4)
 #define PHY_GET_RFGAINID(rfattn, padmix, width)	((rfattn) + ((padmix)*(width)))
@@ -203,7 +176,9 @@ typedef void (*detachfn_t) (struct brcms_phy *);
 #define PHY_PERICAL_WDOG_DELAY	5
 
 #define MPHASE_TXCAL_NUMCMDS	2
-#define PHY_PERICAL_MPHASE_PENDING(pi)	(pi->mphase_cal_phase_id > MPHASE_CAL_STATE_IDLE)
+
+#define PHY_PERICAL_MPHASE_PENDING(pi) \
+	(pi->mphase_cal_phase_id > MPHASE_CAL_STATE_IDLE)
 
 enum {
 	MPHASE_CAL_STATE_IDLE = 0,
@@ -248,47 +223,29 @@ enum phy_cal_mode {
 #define CORDIC_AG	39797
 #define	CORDIC_NI	18
 #define	FIXED(X)	((s32)((X) << 16))
-#define	FLOAT(X)	(((X) >= 0) ? ((((X) >> 15) + 1) >> 1) : -((((-(X)) >> 15) + 1) >> 1))
+
+#define	FLOAT(X) \
+	(((X) >= 0) ? ((((X) >> 15) + 1) >> 1) : -((((-(X)) >> 15) + 1) >> 1))
 
 #define PHY_CHAIN_TX_DISABLE_TEMP	115
 #define PHY_HYSTERESIS_DELTATEMP	5
 
-#define PHY_BITSCNT(x)	brcmu_bitcount((u8 *)&(x), sizeof(u8))
-
-#define MOD_PHY_REG(pi, phy_type, reg_name, field, value) \
-	mod_phy_reg(pi, phy_type##_##reg_name, phy_type##_##reg_name##_##field##_MASK, \
-	(value) << phy_type##_##reg_name##_##field##_##SHIFT);
-#define READ_PHY_REG(pi, phy_type, reg_name, field) \
-	((read_phy_reg(pi, phy_type##_##reg_name) & phy_type##_##reg_name##_##field##_##MASK)\
-	>> phy_type##_##reg_name##_##field##_##SHIFT)
+#define SCAN_INPROG_PHY(pi) \
+	(mboolisset(pi->measure_hold, PHY_HOLD_FOR_SCAN))
 
-#define	VALID_PHYTYPE(phytype)	(((uint)phytype == PHY_TYPE_N) || \
-				((uint)phytype == PHY_TYPE_LCN))
+#define PLT_INPROG_PHY(pi)      (mboolisset(pi->measure_hold, PHY_HOLD_FOR_PLT))
 
-#define VALID_N_RADIO(radioid)	((radioid == BCM2055_ID) || (radioid == BCM2056_ID) || \
-				(radioid == BCM2057_ID))
-#define VALID_LCN_RADIO(radioid)	(radioid == BCM2064_ID)
+#define ASSOC_INPROG_PHY(pi) \
+	(mboolisset(pi->measure_hold, PHY_HOLD_FOR_ASSOC))
 
-#define	VALID_RADIO(pi, radioid)	(\
-	(ISNPHY(pi) ? VALID_N_RADIO(radioid) : false) || \
-	(ISLCNPHY(pi) ? VALID_LCN_RADIO(radioid) : false))
+#define SCAN_RM_IN_PROGRESS(pi) \
+	(mboolisset(pi->measure_hold, PHY_HOLD_FOR_SCAN | PHY_HOLD_FOR_RM))
 
-#define SCAN_INPROG_PHY(pi)	(mboolisset(pi->measure_hold, PHY_HOLD_FOR_SCAN))
-#define RM_INPROG_PHY(pi)	(mboolisset(pi->measure_hold, PHY_HOLD_FOR_RM))
-#define PLT_INPROG_PHY(pi)	(mboolisset(pi->measure_hold, PHY_HOLD_FOR_PLT))
-#define ASSOC_INPROG_PHY(pi)	(mboolisset(pi->measure_hold, PHY_HOLD_FOR_ASSOC))
-#define SCAN_RM_IN_PROGRESS(pi) (mboolisset(pi->measure_hold, PHY_HOLD_FOR_SCAN | PHY_HOLD_FOR_RM))
-#define PHY_MUTED(pi)		(mboolisset(pi->measure_hold, PHY_HOLD_FOR_MUTE))
-#define PUB_NOT_ASSOC(pi)	(mboolisset(pi->measure_hold, PHY_HOLD_FOR_NOT_ASSOC))
+#define PHY_MUTED(pi) \
+	(mboolisset(pi->measure_hold, PHY_HOLD_FOR_MUTE))
 
-#if defined(EXT_CBALL)
-#define NORADIO_ENAB(pub) ((pub).radioid == NORADIO_ID)
-#else
-#define NORADIO_ENAB(pub) 0
-#endif
-
-#define PHY_LTRN_LIST_LEN	64
-extern u16 ltrn_list[PHY_LTRN_LIST_LEN];
+#define PUB_NOT_ASSOC(pi) \
+	(mboolisset(pi->measure_hold, PHY_HOLD_FOR_NOT_ASSOC))
 
 struct phy_table_info {
 	uint table;
@@ -532,7 +489,7 @@ struct shared_phy {
 	struct brcms_phy *phy_head;
 	uint unit;
 	struct si_pub *sih;
-	void *physhim;
+	struct phy_shim_info *physhim;
 	uint corerev;
 	u32 machwcap;
 	bool up;
@@ -549,7 +506,6 @@ struct shared_phy {
 	uint boardvendor;
 	u32 boardflags;
 	u32 boardflags2;
-	uint bustype;
 	uint buscorerev;
 	uint fast_timer;
 	uint slow_timer;
@@ -579,41 +535,39 @@ struct brcms_phy_pub {
 };
 
 struct phy_func_ptr {
-	initfn_t init;
-	initfn_t calinit;
-	chansetfn_t chanset;
-	initfn_t txpwrrecalc;
-	longtrnfn_t longtrn;
-	txiqccgetfn_t txiqccget;
-	txiqccsetfn_t txiqccset;
-	txloccgetfn_t txloccget;
-	radioloftgetfn_t radioloftget;
-	initfn_t carrsuppr;
-	rxsigpwrfn_t rxsigpwr;
-	detachfn_t detach;
+	void (*init)(struct brcms_phy *);
+	void (*calinit)(struct brcms_phy *);
+	void (*chanset)(struct brcms_phy *, u16 chanspec);
+	void (*txpwrrecalc)(struct brcms_phy *);
+	int (*longtrn)(struct brcms_phy *, int);
+	void (*txiqccget)(struct brcms_phy *, u16 *, u16 *);
+	void (*txiqccset)(struct brcms_phy *, u16, u16);
+	u16 (*txloccget)(struct brcms_phy *);
+	void (*radioloftget)(struct brcms_phy *, u8 *, u8 *, u8 *, u8 *);
+	void (*carrsuppr)(struct brcms_phy *);
+	s32 (*rxsigpwr)(struct brcms_phy *, s32);
+	void (*detach)(struct brcms_phy *);
 };
 
 struct brcms_phy {
 	struct brcms_phy_pub pubpi_ro;
 	struct shared_phy *sh;
 	struct phy_func_ptr pi_fptr;
-	void *pi_ptr;
 
 	union {
 		struct brcms_phy_lcnphy *pi_lcnphy;
 	} u;
 	bool user_txpwr_at_rfport;
 
-	d11regs_t *regs;
+	struct d11regs __iomem *regs;
 	struct brcms_phy *next;
-	char *vars;
 	struct brcms_phy_pub pubpi;
 
 	bool do_initcal;
 	bool phytest_on;
 	bool ofdm_rateset_war;
 	bool bf_preempt_4306;
-	chanspec_t radio_chanspec;
+	u16 radio_chanspec;
 	u8 antsel_type;
 	u16 bw;
 	u8 txpwr_percent;
@@ -629,7 +583,7 @@ struct brcms_phy {
 	int phynoise_chan_watchdog;
 	bool phynoise_polling;
 	bool disable_percal;
-	mbool measure_hold;
+	u32 measure_hold;
 
 	s16 txpa_2g[PWRTBL_NUM_COEFF];
 	s16 txpa_2g_low_temp[PWRTBL_NUM_COEFF];
@@ -723,7 +677,7 @@ struct brcms_phy {
 	u16 mintxbias;
 	u16 mintxmag;
 	struct lo_complex_abgphy_info gphy_locomp_iq
-					[STATIC_NUM_RF][STATIC_NUM_BB];
+			[STATIC_NUM_RF][STATIC_NUM_BB];
 	s8 stats_11b_txpower[STATIC_NUM_RF][STATIC_NUM_BB];
 	u16 gain_table[TX_GAIN_TABLE_LENGTH];
 	bool loopback_gain;
@@ -840,11 +794,11 @@ struct brcms_phy {
 	u8 mphase_txcal_cmdidx;
 	u8 mphase_txcal_numcmds;
 	u16 mphase_txcal_bestcoeffs[11];
-	chanspec_t nphy_txiqlocal_chanspec;
-	chanspec_t nphy_iqcal_chanspec_2G;
-	chanspec_t nphy_iqcal_chanspec_5G;
-	chanspec_t nphy_rssical_chanspec_2G;
-	chanspec_t nphy_rssical_chanspec_5G;
+	u16 nphy_txiqlocal_chanspec;
+	u16 nphy_iqcal_chanspec_2G;
+	u16 nphy_iqcal_chanspec_5G;
+	u16 nphy_rssical_chanspec_2G;
+	u16 nphy_rssical_chanspec_5G;
 	struct wlapi_timer *phycal_timer;
 	bool use_int_tx_iqlo_cal_nphy;
 	bool internal_tx_iqlo_cal_tapoff_intpa_nphy;
@@ -937,9 +891,9 @@ struct brcms_phy {
 	struct wiphy *wiphy;
 };
 
-struct _cs32 {
-	fixed q;
-	fixed i;
+struct cs32 {
+	s32 q;
+	s32 i;
 };
 
 struct radio_regs {
@@ -964,29 +918,6 @@ struct lcnphy_radio_regs {
 	u8 do_init_g;
 };
 
-extern struct lcnphy_radio_regs lcnphy_radio_regs_2064[];
-extern struct lcnphy_radio_regs lcnphy_radio_regs_2066[];
-extern struct radio_regs regs_2055[], regs_SYN_2056[], regs_TX_2056[],
-	regs_RX_2056[];
-extern struct radio_regs regs_SYN_2056_A1[], regs_TX_2056_A1[],
-	      regs_RX_2056_A1[];
-extern struct radio_regs regs_SYN_2056_rev5[], regs_TX_2056_rev5[],
-	regs_RX_2056_rev5[];
-extern struct radio_regs regs_SYN_2056_rev6[], regs_TX_2056_rev6[],
-	regs_RX_2056_rev6[];
-extern struct radio_regs regs_SYN_2056_rev7[], regs_TX_2056_rev7[],
-	regs_RX_2056_rev7[];
-extern struct radio_regs regs_SYN_2056_rev8[], regs_TX_2056_rev8[],
-	regs_RX_2056_rev8[];
-extern struct radio_20xx_regs regs_2057_rev4[], regs_2057_rev5[],
-	      regs_2057_rev5v1[];
-extern struct radio_20xx_regs regs_2057_rev7[], regs_2057_rev8[];
-
-extern char *phy_getvar(struct brcms_phy *pi, const char *name);
-extern int phy_getintvar(struct brcms_phy *pi, const char *name);
-#define PHY_GETVAR(pi, name)	phy_getvar(pi, name)
-#define PHY_GETINTVAR(pi, name)	phy_getintvar(pi, name)
-
 extern u16 read_phy_reg(struct brcms_phy *pi, u16 addr);
 extern void write_phy_reg(struct brcms_phy *pi, u16 addr, u16 val);
 extern void and_phy_reg(struct brcms_phy *pi, u16 addr, u16 val);
@@ -1022,14 +953,13 @@ extern void wlc_phy_table_data_write(struct brcms_phy *pi, uint width, u32 val);
 extern void write_phy_channel_reg(struct brcms_phy *pi, uint val);
 extern void wlc_phy_txpower_update_shm(struct brcms_phy *pi);
 
-extern void wlc_phy_cordic(fixed theta, cs32 *val);
 extern u8 wlc_phy_nbits(s32 value);
 extern void wlc_phy_compute_dB(u32 *cmplx_pwr, s8 *p_dB, u8 core);
 
 extern uint wlc_phy_init_radio_regs_allbands(struct brcms_phy *pi,
 					     struct radio_20xx_regs *radioregs);
 extern uint wlc_phy_init_radio_regs(struct brcms_phy *pi,
-				    struct radio_regs *radioregs,
+				    const struct radio_regs *radioregs,
 				    u16 core_offset);
 
 extern void wlc_phy_txpower_ipa_upd(struct brcms_phy *pi);
@@ -1053,14 +983,14 @@ extern void wlc_phy_cal_init_nphy(struct brcms_phy *pi);
 extern void wlc_phy_cal_init_lcnphy(struct brcms_phy *pi);
 
 extern void wlc_phy_chanspec_set_nphy(struct brcms_phy *pi,
-				      chanspec_t chanspec);
+				      u16 chanspec);
 extern void wlc_phy_chanspec_set_lcnphy(struct brcms_phy *pi,
-					chanspec_t chanspec);
+					u16 chanspec);
 extern void wlc_phy_chanspec_set_fixup_lcnphy(struct brcms_phy *pi,
-					      chanspec_t chanspec);
+					      u16 chanspec);
 extern int wlc_phy_channel2freq(uint channel);
 extern int wlc_phy_chanspec_freq2bandrange_lpssn(uint);
-extern int wlc_phy_chanspec_bandrange_get(struct brcms_phy *, chanspec_t);
+extern int wlc_phy_chanspec_bandrange_get(struct brcms_phy *, u16 chanspec);
 
 extern void wlc_lcnphy_set_tx_pwr_ctrl(struct brcms_phy *pi, u16 mode);
 extern s8 wlc_lcnphy_get_current_tx_pwr_idx(struct brcms_phy *pi);
@@ -1142,13 +1072,17 @@ extern void wlc_phy_stay_in_carriersearch_nphy(struct brcms_phy *pi,
 					       bool enable);
 extern void wlc_nphy_deaf_mode(struct brcms_phy *pi, bool mode);
 
-#define wlc_phy_write_table_nphy(pi, pti)	wlc_phy_write_table(pi, pti, 0x72, \
-	0x74, 0x73)
-#define wlc_phy_read_table_nphy(pi, pti)	wlc_phy_read_table(pi, pti, 0x72, \
-	0x74, 0x73)
-#define wlc_nphy_table_addr(pi, id, off)	wlc_phy_table_addr((pi), (id), (off), \
-	0x72, 0x74, 0x73)
-#define wlc_nphy_table_data_write(pi, w, v)	wlc_phy_table_data_write((pi), (w), (v))
+#define wlc_phy_write_table_nphy(pi, pti) \
+	wlc_phy_write_table(pi, pti, 0x72, 0x74, 0x73)
+
+#define wlc_phy_read_table_nphy(pi, pti) \
+	wlc_phy_read_table(pi, pti, 0x72, 0x74, 0x73)
+
+#define wlc_nphy_table_addr(pi, id, off) \
+	wlc_phy_table_addr((pi), (id), (off), 0x72, 0x74, 0x73)
+
+#define wlc_nphy_table_data_write(pi, w, v) \
+	wlc_phy_table_data_write((pi), (w), (v))
 
 extern void wlc_phy_table_read_nphy(struct brcms_phy *pi, u32, u32 l, u32 o,
 				    u32 w, void *d);
@@ -1160,7 +1094,7 @@ extern void wlc_phy_table_write_nphy(struct brcms_phy *pi, u32, u32, u32,
 	 (pi->ipa5g_on && CHSPEC_IS5G(pi->radio_chanspec)))
 
 #define BRCMS_PHY_WAR_PR51571(pi) \
-	if (((pi)->sh->bustype == PCI_BUS) && NREV_LT((pi)->pubpi.phy_rev, 3)) \
+	if (NREV_LT((pi)->pubpi.phy_rev, 3)) \
 		(void)R_REG(&(pi)->regs->maccontrol)
 
 extern void wlc_phy_cal_perical_nphy_run(struct brcms_phy *pi, u8 caltype);
@@ -1219,7 +1153,7 @@ extern void wlc_phy_est_tonepwr_nphy(struct brcms_phy *pi, s32 *qdBm_pwrbuf,
 extern void wlc_phy_radio205x_vcocal_nphy(struct brcms_phy *pi);
 
 extern int wlc_phy_rssi_compute_nphy(struct brcms_phy *pi,
-				     struct brcms_d11rxhdr *wlc_rxh);
+				     struct d11rxhdr *rxh);
 
 #define NPHY_TESTPATTERN_BPHY_EVM   0
 #define NPHY_TESTPATTERN_BPHY_RFCS  1
@@ -1229,7 +1163,7 @@ extern void wlc_phy_nphy_tkip_rifs_war(struct brcms_phy *pi, u8 rifs);
 void wlc_phy_get_pwrdet_offsets(struct brcms_phy *pi, s8 *cckoffset,
 				s8 *ofdmoffset);
 extern s8 wlc_phy_upd_rssi_offset(struct brcms_phy *pi, s8 rssi,
-				    chanspec_t chanspec);
+				  u16 chanspec);
 
 extern bool wlc_phy_n_txpower_ipa_ison(struct brcms_phy *pih);
 #endif				/* _BRCM_PHY_INT_H_ */
diff --git a/drivers/staging/brcm80211/brcmsmac/phy/phy_lcn.c b/drivers/staging/brcm80211/brcmsmac/phy/phy_lcn.c
index 6a3fbe67302f..a63aa99d9810 100644
--- a/drivers/staging/brcm80211/brcmsmac/phy/phy_lcn.c
+++ b/drivers/staging/brcm80211/brcmsmac/phy/phy_lcn.c
@@ -14,7 +14,9 @@
  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#include <linux/kernel.h>
 #include <linux/delay.h>
+#include <linux/cordic.h>
 
 #include <pmu.h>
 #include <d11.h>
@@ -67,6 +69,14 @@
 #define LCN_TEMPSENSE_OFFSET  80812
 #define LCN_TEMPSENSE_DEN  2647
 
+#define LCN_BW_LMT	200
+#define LCN_CUR_LMT	1250
+#define LCN_MULT	1
+#define LCN_VCO_DIV	30
+#define LCN_OFFSET	680
+#define LCN_FACT	490
+#define LCN_CUR_DIV	2640
+
 #define LCNPHY_txgainctrlovrval1_pagain_ovr_val1_SHIFT \
 	(0 + 8)
 #define LCNPHY_txgainctrlovrval1_pagain_ovr_val1_MASK \
@@ -79,15 +89,15 @@
 
 #define wlc_lcnphy_enable_tx_gain_override(pi) \
 	wlc_lcnphy_set_tx_gain_override(pi, true)
-#define wlc_lcnphy_disable_tx_gain_override(pi) \
+#define wlc_lcnphy_disable_tx_gain_override(pi)	\
 	wlc_lcnphy_set_tx_gain_override(pi, false)
 
 #define wlc_lcnphy_iqcal_active(pi)	\
 	(read_phy_reg((pi), 0x451) & \
-	((0x1 << 15) | (0x1 << 14)))
+	 ((0x1 << 15) | (0x1 << 14)))
 
 #define txpwrctrl_off(pi) (0x7 != ((read_phy_reg(pi, 0x4a4) & 0xE000) >> 13))
-#define wlc_lcnphy_tempsense_based_pwr_ctrl_enabled(pi) \
+#define wlc_lcnphy_tempsense_based_pwr_ctrl_enabled(pi)	\
 	(pi->temppwrctrl_capable)
 #define wlc_lcnphy_tssi_based_pwr_ctrl_enabled(pi) \
 	(pi->hwpwrctrl_capable)
@@ -132,12 +142,12 @@
 #define LCNPHY_ACI_DETECT_TIMEOUT  2
 #define LCNPHY_ACI_START_DELAY 0
 
-#define wlc_lcnphy_tx_gain_override_enabled(pi) \
+#define wlc_lcnphy_tx_gain_override_enabled(pi)	\
 	(0 != (read_phy_reg((pi), 0x43b) & (0x1 << 6)))
 
 #define wlc_lcnphy_total_tx_frames(pi) \
-	wlapi_bmac_read_shm((pi)->sh->physhim, \
-		M_UCODE_MACSTAT + offsetof(struct macstat, txallfrm))
+	wlapi_bmac_read_shm((pi)->sh->physhim, M_UCODE_MACSTAT + \
+			    offsetof(struct macstat, txallfrm))
 
 struct lcnphy_txgains {
 	u16 gm_gain;
@@ -198,7 +208,7 @@ static const iqcal_gain_params_lcnphy *tbl_iqcal_gainparams_lcnphy[1] = {
 
 static const u16 iqcal_gainparams_numgains_lcnphy[1] = {
 	sizeof(tbl_iqcal_gainparams_lcnphy_2G) /
-	    sizeof(*tbl_iqcal_gainparams_lcnphy_2G),
+	sizeof(*tbl_iqcal_gainparams_lcnphy_2G),
 };
 
 static const struct lcnphy_sfo_cfg lcnphy_sfo_cfg[] = {
@@ -548,7 +558,7 @@ struct chan_info_2064_lcnphy {
 	u8 rxrf_rxrf_spare1;
 };
 
-static struct chan_info_2064_lcnphy chan_info_2064_lcnphy[] = {
+static const struct chan_info_2064_lcnphy chan_info_2064_lcnphy[] = {
 	{1, 2412, 0x0B, 0x0A, 0x00, 0x07, 0x0A, 0x88, 0x88, 0x80},
 	{2, 2417, 0x0B, 0x0A, 0x00, 0x07, 0x0A, 0x88, 0x88, 0x80},
 	{3, 2422, 0x0B, 0x0A, 0x00, 0x07, 0x0A, 0x88, 0x88, 0x80},
@@ -565,7 +575,7 @@ static struct chan_info_2064_lcnphy chan_info_2064_lcnphy[] = {
 	{14, 2484, 0x0B, 0x0A, 0x00, 0x07, 0x0A, 0x88, 0x88, 0x80},
 };
 
-struct lcnphy_radio_regs lcnphy_radio_regs_2064[] = {
+static const struct lcnphy_radio_regs lcnphy_radio_regs_2064[] = {
 	{0x00, 0, 0, 0, 0},
 	{0x01, 0x64, 0x64, 0, 0},
 	{0x02, 0x20, 0x20, 0, 0},
@@ -877,7 +887,7 @@ struct lcnphy_radio_regs lcnphy_radio_regs_2064[] = {
 #define LCNPHY_NUM_DIG_FILT_COEFFS 16
 #define LCNPHY_NUM_TX_DIG_FILTERS_CCK 13
 
-u16 LCNPHY_txdigfiltcoeffs_cck[LCNPHY_NUM_TX_DIG_FILTERS_CCK]
+static const u16 LCNPHY_txdigfiltcoeffs_cck[LCNPHY_NUM_TX_DIG_FILTERS_CCK]
 	[LCNPHY_NUM_DIG_FILT_COEFFS + 1] = {
 	{0, 1, 415, 1874, 64, 128, 64, 792, 1656, 64, 128, 64, 778, 1582, 64,
 	 128, 64,},
@@ -908,7 +918,7 @@ u16 LCNPHY_txdigfiltcoeffs_cck[LCNPHY_NUM_TX_DIG_FILTERS_CCK]
 };
 
 #define LCNPHY_NUM_TX_DIG_FILTERS_OFDM 3
-u16 LCNPHY_txdigfiltcoeffs_ofdm[LCNPHY_NUM_TX_DIG_FILTERS_OFDM]
+static const u16 LCNPHY_txdigfiltcoeffs_ofdm[LCNPHY_NUM_TX_DIG_FILTERS_OFDM]
 	[LCNPHY_NUM_DIG_FILT_COEFFS + 1] = {
 	{0, 0, 0xa2, 0x0, 0x100, 0x100, 0x0, 0x0, 0x0, 0x100, 0x0, 0x0,
 	 0x278, 0xfea0, 0x80, 0x100, 0x80,},
@@ -920,108 +930,50 @@ u16 LCNPHY_txdigfiltcoeffs_ofdm[LCNPHY_NUM_TX_DIG_FILTERS_OFDM]
 
 #define wlc_lcnphy_set_start_tx_pwr_idx(pi, idx) \
 	mod_phy_reg(pi, 0x4a4, \
-		(0x1ff << 0), \
-		(u16)(idx) << 0)
+		    (0x1ff << 0), \
+		    (u16)(idx) << 0)
 
 #define wlc_lcnphy_set_tx_pwr_npt(pi, npt) \
 	mod_phy_reg(pi, 0x4a5, \
-		(0x7 << 8), \
-		(u16)(npt) << 8)
+		    (0x7 << 8),	\
+		    (u16)(npt) << 8)
 
 #define wlc_lcnphy_get_tx_pwr_ctrl(pi) \
 	(read_phy_reg((pi), 0x4a4) & \
-			((0x1 << 15) | \
-			(0x1 << 14) | \
-			(0x1 << 13)))
+	 ((0x1 << 15) |	\
+	  (0x1 << 14) |	\
+	  (0x1 << 13)))
 
 #define wlc_lcnphy_get_tx_pwr_npt(pi) \
 	((read_phy_reg(pi, 0x4a5) & \
-		(0x7 << 8)) >> \
-		8)
+	  (0x7 << 8)) >> \
+	 8)
 
 #define wlc_lcnphy_get_current_tx_pwr_idx_if_pwrctrl_on(pi) \
 	(read_phy_reg(pi, 0x473) & 0x1ff)
 
 #define wlc_lcnphy_get_target_tx_pwr(pi) \
 	((read_phy_reg(pi, 0x4a7) & \
-		(0xff << 0)) >> \
-		0)
+	  (0xff << 0)) >> \
+	 0)
 
 #define wlc_lcnphy_set_target_tx_pwr(pi, target) \
 	mod_phy_reg(pi, 0x4a7, \
-		(0xff << 0), \
-		(u16)(target) << 0)
+		    (0xff << 0), \
+		    (u16)(target) << 0)
+
+#define wlc_radio_2064_rcal_done(pi) \
+	(0 != (read_radio_reg(pi, RADIO_2064_REG05C) & 0x20))
+
+#define tempsense_done(pi) \
+	(0x8000 == (read_phy_reg(pi, 0x476) & 0x8000))
 
-#define wlc_radio_2064_rcal_done(pi) (0 != (read_radio_reg(pi, RADIO_2064_REG05C) & 0x20))
-#define tempsense_done(pi) (0x8000 == (read_phy_reg(pi, 0x476) & 0x8000))
+#define LCNPHY_IQLOCC_READ(val) \
+	((u8)(-(s8)(((val) & 0xf0) >> 4) + (s8)((val) & 0x0f)))
 
-#define LCNPHY_IQLOCC_READ(val) ((u8)(-(s8)(((val) & 0xf0) >> 4) + (s8)((val) & 0x0f)))
 #define FIXED_TXPWR 78
 #define LCNPHY_TEMPSENSE(val) ((s16)((val > 255) ? (val - 512) : val))
 
-static u32 wlc_lcnphy_qdiv_roundup(u32 divident, u32 divisor,
-				      u8 precision);
-static void wlc_lcnphy_set_rx_gain_by_distribution(struct brcms_phy *pi,
-						   u16 ext_lna, u16 trsw,
-						   u16 biq2, u16 biq1,
-						   u16 tia, u16 lna2,
-						   u16 lna1);
-static void wlc_lcnphy_clear_tx_power_offsets(struct brcms_phy *pi);
-static void wlc_lcnphy_set_pa_gain(struct brcms_phy *pi, u16 gain);
-static void wlc_lcnphy_set_trsw_override(struct brcms_phy *pi, bool tx,
-					 bool rx);
-static void wlc_lcnphy_set_bbmult(struct brcms_phy *pi, u8 m0);
-static u8 wlc_lcnphy_get_bbmult(struct brcms_phy *pi);
-static void wlc_lcnphy_get_tx_gain(struct brcms_phy *pi,
-				   struct lcnphy_txgains *gains);
-static void wlc_lcnphy_set_tx_gain_override(struct brcms_phy *pi, bool bEnable);
-static void wlc_lcnphy_toggle_afe_pwdn(struct brcms_phy *pi);
-static void wlc_lcnphy_rx_gain_override_enable(struct brcms_phy *pi,
-					       bool enable);
-static void wlc_lcnphy_set_tx_gain(struct brcms_phy *pi,
-				   struct lcnphy_txgains *target_gains);
-static bool wlc_lcnphy_rx_iq_est(struct brcms_phy *pi, u16 num_samps,
-				 u8 wait_time, struct lcnphy_iq_est *iq_est);
-static bool wlc_lcnphy_calc_rx_iq_comp(struct brcms_phy *pi, u16 num_samps);
-static u16 wlc_lcnphy_get_pa_gain(struct brcms_phy *pi);
-static void wlc_lcnphy_afe_clk_init(struct brcms_phy *pi, u8 mode);
-static void wlc_lcnphy_tx_pwr_ctrl_init(struct brcms_phy_pub *ppi);
-static void wlc_lcnphy_radio_2064_channel_tune_4313(struct brcms_phy *pi,
-						    u8 channel);
-
-static void wlc_lcnphy_load_tx_gain_table(struct brcms_phy *pi,
-				  const struct lcnphy_tx_gain_tbl_entry *g);
-
-static void wlc_lcnphy_samp_cap(struct brcms_phy *pi, int clip_detect_algo,
-				u16 thresh, s16 *ptr, int mode);
-static int wlc_lcnphy_calc_floor(s16 coeff, int type);
-static void wlc_lcnphy_tx_iqlo_loopback(struct brcms_phy *pi,
-					u16 *values_to_save);
-static void wlc_lcnphy_tx_iqlo_loopback_cleanup(struct brcms_phy *pi,
-						u16 *values_to_save);
-static void wlc_lcnphy_set_cc(struct brcms_phy *pi, int cal_type, s16 coeff_x,
-			      s16 coeff_y);
-static struct lcnphy_unsign16_struct wlc_lcnphy_get_cc(struct brcms_phy *pi,
-						       int cal_type);
-static void wlc_lcnphy_a1(struct brcms_phy *pi, int cal_type,
-			  int num_levels, int step_size_lg2);
-static void wlc_lcnphy_tx_iqlo_soft_cal_full(struct brcms_phy *pi);
-
-static void wlc_lcnphy_set_chanspec_tweaks(struct brcms_phy *pi,
-					   chanspec_t chanspec);
-static void wlc_lcnphy_agc_temp_init(struct brcms_phy *pi);
-static void wlc_lcnphy_temp_adj(struct brcms_phy *pi);
-static void wlc_lcnphy_clear_papd_comptable(struct brcms_phy *pi);
-static void wlc_lcnphy_baseband_init(struct brcms_phy *pi);
-static void wlc_lcnphy_radio_init(struct brcms_phy *pi);
-static void wlc_lcnphy_rc_cal(struct brcms_phy *pi);
-static void wlc_lcnphy_rcal(struct brcms_phy *pi);
-static void wlc_lcnphy_txrx_spur_avoidance_mode(struct brcms_phy *pi,
-						bool enable);
-static int wlc_lcnphy_load_tx_iir_filter(struct brcms_phy *pi, bool is_ofdm,
-					 s16 filt_type);
-static void wlc_lcnphy_set_rx_iq_comp(struct brcms_phy *pi, u16 a, u16 b);
-
 void wlc_lcnphy_write_table(struct brcms_phy *pi, const struct phytbl_info *pti)
 {
 	wlc_phy_write_table(pi, pti, 0x455, 0x457, 0x456);
@@ -1034,7 +986,7 @@ void wlc_lcnphy_read_table(struct brcms_phy *pi, struct phytbl_info *pti)
 
 static void
 wlc_lcnphy_common_read_table(struct brcms_phy *pi, u32 tbl_id,
-			     const void *tbl_ptr, u32 tbl_len,
+			     const u16 *tbl_ptr, u32 tbl_len,
 			     u32 tbl_width, u32 tbl_offset)
 {
 	struct phytbl_info tab;
@@ -1048,7 +1000,7 @@ wlc_lcnphy_common_read_table(struct brcms_phy *pi, u32 tbl_id,
 
 static void
 wlc_lcnphy_common_write_table(struct brcms_phy *pi, u32 tbl_id,
-			      const void *tbl_ptr, u32 tbl_len,
+			      const u16 *tbl_ptr, u32 tbl_len,
 			      u32 tbl_width, u32 tbl_offset)
 {
 
@@ -1092,12 +1044,12 @@ static int wlc_lcnphy_calc_floor(s16 coeff_x, int type)
 	int k;
 	k = 0;
 	if (type == 0) {
-		if (coeff_x < 0) {
+		if (coeff_x < 0)
 			k = (coeff_x - 1) / 2;
-		} else {
+		else
 			k = coeff_x / 2;
-		}
 	}
+
 	if (type == 1) {
 		if ((coeff_x + 1) < 0)
 			k = (coeff_x) / 2;
@@ -1107,20 +1059,265 @@ static int wlc_lcnphy_calc_floor(s16 coeff_x, int type)
 	return k;
 }
 
-s8 wlc_lcnphy_get_current_tx_pwr_idx(struct brcms_phy *pi)
+static void
+wlc_lcnphy_get_tx_gain(struct brcms_phy *pi, struct lcnphy_txgains *gains)
 {
-	s8 index;
+	u16 dac_gain, rfgain0, rfgain1;
+
+	dac_gain = read_phy_reg(pi, 0x439) >> 0;
+	gains->dac_gain = (dac_gain & 0x380) >> 7;
+
+	rfgain0 = (read_phy_reg(pi, 0x4b5) & (0xffff << 0)) >> 0;
+	rfgain1 = (read_phy_reg(pi, 0x4fb) & (0x7fff << 0)) >> 0;
+
+	gains->gm_gain = rfgain0 & 0xff;
+	gains->pga_gain = (rfgain0 >> 8) & 0xff;
+	gains->pad_gain = rfgain1 & 0xff;
+}
+
+
+static void wlc_lcnphy_set_dac_gain(struct brcms_phy *pi, u16 dac_gain)
+{
+	u16 dac_ctrl;
+
+	dac_ctrl = (read_phy_reg(pi, 0x439) >> 0);
+	dac_ctrl = dac_ctrl & 0xc7f;
+	dac_ctrl = dac_ctrl | (dac_gain << 7);
+	mod_phy_reg(pi, 0x439, (0xfff << 0), (dac_ctrl) << 0);
+
+}
+
+static void wlc_lcnphy_set_tx_gain_override(struct brcms_phy *pi, bool bEnable)
+{
+	u16 bit = bEnable ? 1 : 0;
+
+	mod_phy_reg(pi, 0x4b0, (0x1 << 7), bit << 7);
+
+	mod_phy_reg(pi, 0x4b0, (0x1 << 14), bit << 14);
+
+	mod_phy_reg(pi, 0x43b, (0x1 << 6), bit << 6);
+}
+
+static void
+wlc_lcnphy_rx_gain_override_enable(struct brcms_phy *pi, bool enable)
+{
+	u16 ebit = enable ? 1 : 0;
+
+	mod_phy_reg(pi, 0x4b0, (0x1 << 8), ebit << 8);
+
+	mod_phy_reg(pi, 0x44c, (0x1 << 0), ebit << 0);
+
+	if (LCNREV_LT(pi->pubpi.phy_rev, 2)) {
+		mod_phy_reg(pi, 0x44c, (0x1 << 4), ebit << 4);
+		mod_phy_reg(pi, 0x44c, (0x1 << 6), ebit << 6);
+		mod_phy_reg(pi, 0x4b0, (0x1 << 5), ebit << 5);
+		mod_phy_reg(pi, 0x4b0, (0x1 << 6), ebit << 6);
+	} else {
+		mod_phy_reg(pi, 0x4b0, (0x1 << 12), ebit << 12);
+		mod_phy_reg(pi, 0x4b0, (0x1 << 13), ebit << 13);
+		mod_phy_reg(pi, 0x4b0, (0x1 << 5), ebit << 5);
+	}
+
+	if (CHSPEC_IS2G(pi->radio_chanspec)) {
+		mod_phy_reg(pi, 0x4b0, (0x1 << 10), ebit << 10);
+		mod_phy_reg(pi, 0x4e5, (0x1 << 3), ebit << 3);
+	}
+}
+
+static void
+wlc_lcnphy_set_rx_gain_by_distribution(struct brcms_phy *pi,
+				       u16 trsw,
+				       u16 ext_lna,
+				       u16 biq2,
+				       u16 biq1,
+				       u16 tia, u16 lna2, u16 lna1)
+{
+	u16 gain0_15, gain16_19;
+
+	gain16_19 = biq2 & 0xf;
+	gain0_15 = ((biq1 & 0xf) << 12) |
+		   ((tia & 0xf) << 8) |
+		   ((lna2 & 0x3) << 6) |
+		   ((lna2 &
+		     0x3) << 4) | ((lna1 & 0x3) << 2) | ((lna1 & 0x3) << 0);
+
+	mod_phy_reg(pi, 0x4b6, (0xffff << 0), gain0_15 << 0);
+	mod_phy_reg(pi, 0x4b7, (0xf << 0), gain16_19 << 0);
+	mod_phy_reg(pi, 0x4b1, (0x3 << 11), lna1 << 11);
+
+	if (LCNREV_LT(pi->pubpi.phy_rev, 2)) {
+		mod_phy_reg(pi, 0x4b1, (0x1 << 9), ext_lna << 9);
+		mod_phy_reg(pi, 0x4b1, (0x1 << 10), ext_lna << 10);
+	} else {
+		mod_phy_reg(pi, 0x4b1, (0x1 << 10), 0 << 10);
+
+		mod_phy_reg(pi, 0x4b1, (0x1 << 15), 0 << 15);
+
+		mod_phy_reg(pi, 0x4b1, (0x1 << 9), ext_lna << 9);
+	}
+
+	mod_phy_reg(pi, 0x44d, (0x1 << 0), (!trsw) << 0);
+
+}
+
+static void wlc_lcnphy_set_trsw_override(struct brcms_phy *pi, bool tx, bool rx)
+{
+
+	mod_phy_reg(pi, 0x44d,
+		    (0x1 << 1) |
+		    (0x1 << 0), (tx ? (0x1 << 1) : 0) | (rx ? (0x1 << 0) : 0));
+
+	or_phy_reg(pi, 0x44c, (0x1 << 1) | (0x1 << 0));
+}
+
+static void wlc_lcnphy_clear_trsw_override(struct brcms_phy *pi)
+{
+
+	and_phy_reg(pi, 0x44c, (u16) ~((0x1 << 1) | (0x1 << 0)));
+}
+
+static void wlc_lcnphy_set_rx_iq_comp(struct brcms_phy *pi, u16 a, u16 b)
+{
+	mod_phy_reg(pi, 0x645, (0x3ff << 0), (a) << 0);
+
+	mod_phy_reg(pi, 0x646, (0x3ff << 0), (b) << 0);
+
+	mod_phy_reg(pi, 0x647, (0x3ff << 0), (a) << 0);
+
+	mod_phy_reg(pi, 0x648, (0x3ff << 0), (b) << 0);
+
+	mod_phy_reg(pi, 0x649, (0x3ff << 0), (a) << 0);
+
+	mod_phy_reg(pi, 0x64a, (0x3ff << 0), (b) << 0);
+
+}
+
+static bool
+wlc_lcnphy_rx_iq_est(struct brcms_phy *pi,
+		     u16 num_samps,
+		     u8 wait_time, struct lcnphy_iq_est *iq_est)
+{
+	int wait_count = 0;
+	bool result = true;
+	u8 phybw40;
+	phybw40 = CHSPEC_IS40(pi->radio_chanspec);
+
+	mod_phy_reg(pi, 0x6da, (0x1 << 5), (1) << 5);
+
+	mod_phy_reg(pi, 0x410, (0x1 << 3), (0) << 3);
+
+	mod_phy_reg(pi, 0x482, (0xffff << 0), (num_samps) << 0);
+
+	mod_phy_reg(pi, 0x481, (0xff << 0), ((u16) wait_time) << 0);
+
+	mod_phy_reg(pi, 0x481, (0x1 << 8), (0) << 8);
+
+	mod_phy_reg(pi, 0x481, (0x1 << 9), (1) << 9);
+
+	while (read_phy_reg(pi, 0x481) & (0x1 << 9)) {
+
+		if (wait_count > (10 * 500)) {
+			result = false;
+			goto cleanup;
+		}
+		udelay(100);
+		wait_count++;
+	}
+
+	iq_est->iq_prod = ((u32) read_phy_reg(pi, 0x483) << 16) |
+			  (u32) read_phy_reg(pi, 0x484);
+	iq_est->i_pwr = ((u32) read_phy_reg(pi, 0x485) << 16) |
+			(u32) read_phy_reg(pi, 0x486);
+	iq_est->q_pwr = ((u32) read_phy_reg(pi, 0x487) << 16) |
+			(u32) read_phy_reg(pi, 0x488);
+
+cleanup:
+	mod_phy_reg(pi, 0x410, (0x1 << 3), (1) << 3);
+
+	mod_phy_reg(pi, 0x6da, (0x1 << 5), (0) << 5);
+
+	return result;
+}
+
+static bool wlc_lcnphy_calc_rx_iq_comp(struct brcms_phy *pi, u16 num_samps)
+{
+#define LCNPHY_MIN_RXIQ_PWR 2
+	bool result;
+	u16 a0_new, b0_new;
+	struct lcnphy_iq_est iq_est = { 0, 0, 0 };
+	s32 a, b, temp;
+	s16 iq_nbits, qq_nbits, arsh, brsh;
+	s32 iq;
+	u32 ii, qq;
 	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
 
-	if (txpwrctrl_off(pi))
-		index = pi_lcn->lcnphy_current_index;
-	else if (wlc_lcnphy_tssi_based_pwr_ctrl_enabled(pi))
-		index =
-		    (s8) (wlc_lcnphy_get_current_tx_pwr_idx_if_pwrctrl_on(pi)
-			    / 2);
-	else
-		index = pi_lcn->lcnphy_current_index;
-	return index;
+	a0_new = ((read_phy_reg(pi, 0x645) & (0x3ff << 0)) >> 0);
+	b0_new = ((read_phy_reg(pi, 0x646) & (0x3ff << 0)) >> 0);
+	mod_phy_reg(pi, 0x6d1, (0x1 << 2), (0) << 2);
+
+	mod_phy_reg(pi, 0x64b, (0x1 << 6), (1) << 6);
+
+	wlc_lcnphy_set_rx_iq_comp(pi, 0, 0);
+
+	result = wlc_lcnphy_rx_iq_est(pi, num_samps, 32, &iq_est);
+	if (!result)
+		goto cleanup;
+
+	iq = (s32) iq_est.iq_prod;
+	ii = iq_est.i_pwr;
+	qq = iq_est.q_pwr;
+
+	if ((ii + qq) < LCNPHY_MIN_RXIQ_PWR) {
+		result = false;
+		goto cleanup;
+	}
+
+	iq_nbits = wlc_phy_nbits(iq);
+	qq_nbits = wlc_phy_nbits(qq);
+
+	arsh = 10 - (30 - iq_nbits);
+	if (arsh >= 0) {
+		a = (-(iq << (30 - iq_nbits)) + (ii >> (1 + arsh)));
+		temp = (s32) (ii >> arsh);
+		if (temp == 0)
+			return false;
+	} else {
+		a = (-(iq << (30 - iq_nbits)) + (ii << (-1 - arsh)));
+		temp = (s32) (ii << -arsh);
+		if (temp == 0)
+			return false;
+	}
+	a /= temp;
+	brsh = qq_nbits - 31 + 20;
+	if (brsh >= 0) {
+		b = (qq << (31 - qq_nbits));
+		temp = (s32) (ii >> brsh);
+		if (temp == 0)
+			return false;
+	} else {
+		b = (qq << (31 - qq_nbits));
+		temp = (s32) (ii << -brsh);
+		if (temp == 0)
+			return false;
+	}
+	b /= temp;
+	b -= a * a;
+	b = (s32) int_sqrt((unsigned long) b);
+	b -= (1 << 10);
+	a0_new = (u16) (a & 0x3ff);
+	b0_new = (u16) (b & 0x3ff);
+cleanup:
+
+	wlc_lcnphy_set_rx_iq_comp(pi, a0_new, b0_new);
+
+	mod_phy_reg(pi, 0x64b, (0x1 << 0), (1) << 0);
+
+	mod_phy_reg(pi, 0x64b, (0x1 << 3), (1) << 3);
+
+	pi_lcn->lcnphy_cal_results.rxiqcal_coeff_a0 = a0_new;
+	pi_lcn->lcnphy_cal_results.rxiqcal_coeff_b0 = b0_new;
+
+	return result;
 }
 
 static u32 wlc_lcnphy_measure_digital_power(struct brcms_phy *pi, u16 nsamples)
@@ -1132,6 +1329,187 @@ static u32 wlc_lcnphy_measure_digital_power(struct brcms_phy *pi, u16 nsamples)
 	return (iq_est.i_pwr + iq_est.q_pwr) / nsamples;
 }
 
+static bool
+wlc_lcnphy_rx_iq_cal(struct brcms_phy *pi,
+		     const struct lcnphy_rx_iqcomp *iqcomp,
+		     int iqcomp_sz, bool tx_switch, bool rx_switch, int module,
+		     int tx_gain_idx)
+{
+	struct lcnphy_txgains old_gains;
+	u16 tx_pwr_ctrl;
+	u8 tx_gain_index_old = 0;
+	bool result = false, tx_gain_override_old = false;
+	u16 i, Core1TxControl_old, RFOverride0_old,
+	    RFOverrideVal0_old, rfoverride2_old, rfoverride2val_old,
+	    rfoverride3_old, rfoverride3val_old, rfoverride4_old,
+	    rfoverride4val_old, afectrlovr_old, afectrlovrval_old;
+	int tia_gain;
+	u32 received_power, rx_pwr_threshold;
+	u16 old_sslpnCalibClkEnCtrl, old_sslpnRxFeClkEnCtrl;
+	u16 values_to_save[11];
+	s16 *ptr;
+	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
+
+	ptr = kmalloc(sizeof(s16) * 131, GFP_ATOMIC);
+	if (NULL == ptr)
+		return false;
+	if (module == 2) {
+		while (iqcomp_sz--) {
+			if (iqcomp[iqcomp_sz].chan ==
+			    CHSPEC_CHANNEL(pi->radio_chanspec)) {
+				wlc_lcnphy_set_rx_iq_comp(pi,
+							  (u16)
+							  iqcomp[iqcomp_sz].a,
+							  (u16)
+							  iqcomp[iqcomp_sz].b);
+				result = true;
+				break;
+			}
+		}
+		goto cal_done;
+	}
+
+	if (module == 1) {
+
+		tx_pwr_ctrl = wlc_lcnphy_get_tx_pwr_ctrl(pi);
+		wlc_lcnphy_set_tx_pwr_ctrl(pi, LCNPHY_TX_PWR_CTRL_OFF);
+
+		for (i = 0; i < 11; i++)
+			values_to_save[i] =
+				read_radio_reg(pi, rxiq_cal_rf_reg[i]);
+		Core1TxControl_old = read_phy_reg(pi, 0x631);
+
+		or_phy_reg(pi, 0x631, 0x0015);
+
+		RFOverride0_old = read_phy_reg(pi, 0x44c);
+		RFOverrideVal0_old = read_phy_reg(pi, 0x44d);
+		rfoverride2_old = read_phy_reg(pi, 0x4b0);
+		rfoverride2val_old = read_phy_reg(pi, 0x4b1);
+		rfoverride3_old = read_phy_reg(pi, 0x4f9);
+		rfoverride3val_old = read_phy_reg(pi, 0x4fa);
+		rfoverride4_old = read_phy_reg(pi, 0x938);
+		rfoverride4val_old = read_phy_reg(pi, 0x939);
+		afectrlovr_old = read_phy_reg(pi, 0x43b);
+		afectrlovrval_old = read_phy_reg(pi, 0x43c);
+		old_sslpnCalibClkEnCtrl = read_phy_reg(pi, 0x6da);
+		old_sslpnRxFeClkEnCtrl = read_phy_reg(pi, 0x6db);
+
+		tx_gain_override_old = wlc_lcnphy_tx_gain_override_enabled(pi);
+		if (tx_gain_override_old) {
+			wlc_lcnphy_get_tx_gain(pi, &old_gains);
+			tx_gain_index_old = pi_lcn->lcnphy_current_index;
+		}
+
+		wlc_lcnphy_set_tx_pwr_by_index(pi, tx_gain_idx);
+
+		mod_phy_reg(pi, 0x4f9, (0x1 << 0), 1 << 0);
+		mod_phy_reg(pi, 0x4fa, (0x1 << 0), 0 << 0);
+
+		mod_phy_reg(pi, 0x43b, (0x1 << 1), 1 << 1);
+		mod_phy_reg(pi, 0x43c, (0x1 << 1), 0 << 1);
+
+		write_radio_reg(pi, RADIO_2064_REG116, 0x06);
+		write_radio_reg(pi, RADIO_2064_REG12C, 0x07);
+		write_radio_reg(pi, RADIO_2064_REG06A, 0xd3);
+		write_radio_reg(pi, RADIO_2064_REG098, 0x03);
+		write_radio_reg(pi, RADIO_2064_REG00B, 0x7);
+		mod_radio_reg(pi, RADIO_2064_REG113, 1 << 4, 1 << 4);
+		write_radio_reg(pi, RADIO_2064_REG01D, 0x01);
+		write_radio_reg(pi, RADIO_2064_REG114, 0x01);
+		write_radio_reg(pi, RADIO_2064_REG02E, 0x10);
+		write_radio_reg(pi, RADIO_2064_REG12A, 0x08);
+
+		mod_phy_reg(pi, 0x938, (0x1 << 0), 1 << 0);
+		mod_phy_reg(pi, 0x939, (0x1 << 0), 0 << 0);
+		mod_phy_reg(pi, 0x938, (0x1 << 1), 1 << 1);
+		mod_phy_reg(pi, 0x939, (0x1 << 1), 1 << 1);
+		mod_phy_reg(pi, 0x938, (0x1 << 2), 1 << 2);
+		mod_phy_reg(pi, 0x939, (0x1 << 2), 1 << 2);
+		mod_phy_reg(pi, 0x938, (0x1 << 3), 1 << 3);
+		mod_phy_reg(pi, 0x939, (0x1 << 3), 1 << 3);
+		mod_phy_reg(pi, 0x938, (0x1 << 5), 1 << 5);
+		mod_phy_reg(pi, 0x939, (0x1 << 5), 0 << 5);
+
+		mod_phy_reg(pi, 0x43b, (0x1 << 0), 1 << 0);
+		mod_phy_reg(pi, 0x43c, (0x1 << 0), 0 << 0);
+
+		wlc_lcnphy_start_tx_tone(pi, 2000, 120, 0);
+		write_phy_reg(pi, 0x6da, 0xffff);
+		or_phy_reg(pi, 0x6db, 0x3);
+		wlc_lcnphy_set_trsw_override(pi, tx_switch, rx_switch);
+		wlc_lcnphy_rx_gain_override_enable(pi, true);
+
+		tia_gain = 8;
+		rx_pwr_threshold = 950;
+		while (tia_gain > 0) {
+			tia_gain -= 1;
+			wlc_lcnphy_set_rx_gain_by_distribution(pi,
+							       0, 0, 2, 2,
+							       (u16)
+							       tia_gain, 1, 0);
+			udelay(500);
+
+			received_power =
+				wlc_lcnphy_measure_digital_power(pi, 2000);
+			if (received_power < rx_pwr_threshold)
+				break;
+		}
+		result = wlc_lcnphy_calc_rx_iq_comp(pi, 0xffff);
+
+		wlc_lcnphy_stop_tx_tone(pi);
+
+		write_phy_reg(pi, 0x631, Core1TxControl_old);
+
+		write_phy_reg(pi, 0x44c, RFOverrideVal0_old);
+		write_phy_reg(pi, 0x44d, RFOverrideVal0_old);
+		write_phy_reg(pi, 0x4b0, rfoverride2_old);
+		write_phy_reg(pi, 0x4b1, rfoverride2val_old);
+		write_phy_reg(pi, 0x4f9, rfoverride3_old);
+		write_phy_reg(pi, 0x4fa, rfoverride3val_old);
+		write_phy_reg(pi, 0x938, rfoverride4_old);
+		write_phy_reg(pi, 0x939, rfoverride4val_old);
+		write_phy_reg(pi, 0x43b, afectrlovr_old);
+		write_phy_reg(pi, 0x43c, afectrlovrval_old);
+		write_phy_reg(pi, 0x6da, old_sslpnCalibClkEnCtrl);
+		write_phy_reg(pi, 0x6db, old_sslpnRxFeClkEnCtrl);
+
+		wlc_lcnphy_clear_trsw_override(pi);
+
+		mod_phy_reg(pi, 0x44c, (0x1 << 2), 0 << 2);
+
+		for (i = 0; i < 11; i++)
+			write_radio_reg(pi, rxiq_cal_rf_reg[i],
+					values_to_save[i]);
+
+		if (tx_gain_override_old)
+			wlc_lcnphy_set_tx_pwr_by_index(pi, tx_gain_index_old);
+		else
+			wlc_lcnphy_disable_tx_gain_override(pi);
+
+		wlc_lcnphy_set_tx_pwr_ctrl(pi, tx_pwr_ctrl);
+		wlc_lcnphy_rx_gain_override_enable(pi, false);
+	}
+
+cal_done:
+	kfree(ptr);
+	return result;
+}
+
+s8 wlc_lcnphy_get_current_tx_pwr_idx(struct brcms_phy *pi)
+{
+	s8 index;
+	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
+
+	if (txpwrctrl_off(pi))
+		index = pi_lcn->lcnphy_current_index;
+	else if (wlc_lcnphy_tssi_based_pwr_ctrl_enabled(pi))
+		index =	(s8) (wlc_lcnphy_get_current_tx_pwr_idx_if_pwrctrl_on(
+			      pi) / 2);
+	else
+		index = pi_lcn->lcnphy_current_index;
+	return index;
+}
+
 void wlc_lcnphy_crsuprs(struct brcms_phy *pi, int channel)
 {
 	u16 afectrlovr, afectrlovrval;
@@ -1200,7 +1578,290 @@ wlc_lcnphy_txrx_spur_avoidance_mode(struct brcms_phy *pi, bool enable)
 	wlapi_switch_macfreq(pi->sh->physhim, enable);
 }
 
-void wlc_phy_chanspec_set_lcnphy(struct brcms_phy *pi, chanspec_t chanspec)
+static void
+wlc_lcnphy_set_chanspec_tweaks(struct brcms_phy *pi, u16 chanspec)
+{
+	u8 channel = CHSPEC_CHANNEL(chanspec);
+	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
+
+	if (channel == 14)
+		mod_phy_reg(pi, 0x448, (0x3 << 8), (2) << 8);
+	else
+		mod_phy_reg(pi, 0x448, (0x3 << 8), (1) << 8);
+
+	pi_lcn->lcnphy_bandedge_corr = 2;
+	if (channel == 1)
+		pi_lcn->lcnphy_bandedge_corr = 4;
+
+	if (channel == 1 || channel == 2 || channel == 3 ||
+	    channel == 4 || channel == 9 ||
+	    channel == 10 || channel == 11 || channel == 12) {
+		si_pmu_pllcontrol(pi->sh->sih, 0x2, 0xffffffff, 0x03000c04);
+		si_pmu_pllcontrol(pi->sh->sih, 0x3, 0xffffff, 0x0);
+		si_pmu_pllcontrol(pi->sh->sih, 0x4, 0xffffffff, 0x200005c0);
+
+		si_pmu_pllupd(pi->sh->sih);
+		write_phy_reg(pi, 0x942, 0);
+		wlc_lcnphy_txrx_spur_avoidance_mode(pi, false);
+		pi_lcn->lcnphy_spurmod = 0;
+		mod_phy_reg(pi, 0x424, (0xff << 8), (0x1b) << 8);
+
+		write_phy_reg(pi, 0x425, 0x5907);
+	} else {
+		si_pmu_pllcontrol(pi->sh->sih, 0x2, 0xffffffff, 0x03140c04);
+		si_pmu_pllcontrol(pi->sh->sih, 0x3, 0xffffff, 0x333333);
+		si_pmu_pllcontrol(pi->sh->sih, 0x4, 0xffffffff, 0x202c2820);
+
+		si_pmu_pllupd(pi->sh->sih);
+		write_phy_reg(pi, 0x942, 0);
+		wlc_lcnphy_txrx_spur_avoidance_mode(pi, true);
+
+		pi_lcn->lcnphy_spurmod = 0;
+		mod_phy_reg(pi, 0x424, (0xff << 8), (0x1f) << 8);
+
+		write_phy_reg(pi, 0x425, 0x590a);
+	}
+
+	or_phy_reg(pi, 0x44a, 0x44);
+	write_phy_reg(pi, 0x44a, 0x80);
+}
+
+static void
+wlc_lcnphy_radio_2064_channel_tune_4313(struct brcms_phy *pi, u8 channel)
+{
+	uint i;
+	const struct chan_info_2064_lcnphy *ci;
+	u8 rfpll_doubler = 0;
+	u8 pll_pwrup, pll_pwrup_ovr;
+	s32 qFxtal, qFref, qFvco, qFcal;
+	u8 d15, d16, f16, e44, e45;
+	u32 div_int, div_frac, fvco3, fpfd, fref3, fcal_div;
+	u16 loop_bw, d30, setCount;
+
+	u8 h29, h28_ten, e30, h30_ten, cp_current;
+	u16 g30, d28;
+
+	ci = &chan_info_2064_lcnphy[0];
+	rfpll_doubler = 1;
+
+	mod_radio_reg(pi, RADIO_2064_REG09D, 0x4, 0x1 << 2);
+
+	write_radio_reg(pi, RADIO_2064_REG09E, 0xf);
+	if (!rfpll_doubler) {
+		loop_bw = PLL_2064_LOOP_BW;
+		d30 = PLL_2064_D30;
+	} else {
+		loop_bw = PLL_2064_LOOP_BW_DOUBLER;
+		d30 = PLL_2064_D30_DOUBLER;
+	}
+
+	if (CHSPEC_IS2G(pi->radio_chanspec)) {
+		for (i = 0; i < ARRAY_SIZE(chan_info_2064_lcnphy); i++)
+			if (chan_info_2064_lcnphy[i].chan == channel)
+				break;
+
+		if (i >= ARRAY_SIZE(chan_info_2064_lcnphy))
+			return;
+
+		ci = &chan_info_2064_lcnphy[i];
+	}
+
+	write_radio_reg(pi, RADIO_2064_REG02A, ci->logen_buftune);
+
+	mod_radio_reg(pi, RADIO_2064_REG030, 0x3, ci->logen_rccr_tx);
+
+	mod_radio_reg(pi, RADIO_2064_REG091, 0x3, ci->txrf_mix_tune_ctrl);
+
+	mod_radio_reg(pi, RADIO_2064_REG038, 0xf, ci->pa_input_tune_g);
+
+	mod_radio_reg(pi, RADIO_2064_REG030, 0x3 << 2,
+		      (ci->logen_rccr_rx) << 2);
+
+	mod_radio_reg(pi, RADIO_2064_REG05E, 0xf, ci->pa_rxrf_lna1_freq_tune);
+
+	mod_radio_reg(pi, RADIO_2064_REG05E, (0xf) << 4,
+		      (ci->pa_rxrf_lna2_freq_tune) << 4);
+
+	write_radio_reg(pi, RADIO_2064_REG06C, ci->rxrf_rxrf_spare1);
+
+	pll_pwrup = (u8) read_radio_reg(pi, RADIO_2064_REG044);
+	pll_pwrup_ovr = (u8) read_radio_reg(pi, RADIO_2064_REG12B);
+
+	or_radio_reg(pi, RADIO_2064_REG044, 0x07);
+
+	or_radio_reg(pi, RADIO_2064_REG12B, (0x07) << 1);
+	e44 = 0;
+	e45 = 0;
+
+	fpfd = rfpll_doubler ? (pi->xtalfreq << 1) : (pi->xtalfreq);
+	if (pi->xtalfreq > 26000000)
+		e44 = 1;
+	if (pi->xtalfreq > 52000000)
+		e45 = 1;
+	if (e44 == 0)
+		fcal_div = 1;
+	else if (e45 == 0)
+		fcal_div = 2;
+	else
+		fcal_div = 4;
+	fvco3 = (ci->freq * 3);
+	fref3 = 2 * fpfd;
+
+	qFxtal = wlc_lcnphy_qdiv_roundup(pi->xtalfreq, PLL_2064_MHZ, 16);
+	qFref = wlc_lcnphy_qdiv_roundup(fpfd, PLL_2064_MHZ, 16);
+	qFcal = pi->xtalfreq * fcal_div / PLL_2064_MHZ;
+	qFvco = wlc_lcnphy_qdiv_roundup(fvco3, 2, 16);
+
+	write_radio_reg(pi, RADIO_2064_REG04F, 0x02);
+
+	d15 = (pi->xtalfreq * fcal_div * 4 / 5) / PLL_2064_MHZ - 1;
+	write_radio_reg(pi, RADIO_2064_REG052, (0x07 & (d15 >> 2)));
+	write_radio_reg(pi, RADIO_2064_REG053, (d15 & 0x3) << 5);
+
+	d16 = (qFcal * 8 / (d15 + 1)) - 1;
+	write_radio_reg(pi, RADIO_2064_REG051, d16);
+
+	f16 = ((d16 + 1) * (d15 + 1)) / qFcal;
+	setCount = f16 * 3 * (ci->freq) / 32 - 1;
+	mod_radio_reg(pi, RADIO_2064_REG053, (0x0f << 0),
+		      (u8) (setCount >> 8));
+
+	or_radio_reg(pi, RADIO_2064_REG053, 0x10);
+	write_radio_reg(pi, RADIO_2064_REG054, (u8) (setCount & 0xff));
+
+	div_int = ((fvco3 * (PLL_2064_MHZ >> 4)) / fref3) << 4;
+
+	div_frac = ((fvco3 * (PLL_2064_MHZ >> 4)) % fref3) << 4;
+	while (div_frac >= fref3) {
+		div_int++;
+		div_frac -= fref3;
+	}
+	div_frac = wlc_lcnphy_qdiv_roundup(div_frac, fref3, 20);
+
+	mod_radio_reg(pi, RADIO_2064_REG045, (0x1f << 0),
+		      (u8) (div_int >> 4));
+	mod_radio_reg(pi, RADIO_2064_REG046, (0x1f << 4),
+		      (u8) (div_int << 4));
+	mod_radio_reg(pi, RADIO_2064_REG046, (0x0f << 0),
+		      (u8) (div_frac >> 16));
+	write_radio_reg(pi, RADIO_2064_REG047, (u8) (div_frac >> 8) & 0xff);
+	write_radio_reg(pi, RADIO_2064_REG048, (u8) div_frac & 0xff);
+
+	write_radio_reg(pi, RADIO_2064_REG040, 0xfb);
+
+	write_radio_reg(pi, RADIO_2064_REG041, 0x9A);
+	write_radio_reg(pi, RADIO_2064_REG042, 0xA3);
+	write_radio_reg(pi, RADIO_2064_REG043, 0x0C);
+
+	h29 = LCN_BW_LMT / loop_bw;
+	d28 = (((PLL_2064_HIGH_END_KVCO - PLL_2064_LOW_END_KVCO) *
+		(fvco3 / 2 - PLL_2064_LOW_END_VCO)) /
+	       (PLL_2064_HIGH_END_VCO - PLL_2064_LOW_END_VCO))
+	      + PLL_2064_LOW_END_KVCO;
+	h28_ten = (d28 * 10) / LCN_VCO_DIV;
+	e30 = (d30 - LCN_OFFSET) / LCN_FACT;
+	g30 = LCN_OFFSET + (e30 * LCN_FACT);
+	h30_ten = (g30 * 10) / LCN_CUR_DIV;
+	cp_current = ((LCN_CUR_LMT * h29 * LCN_MULT * 100) / h28_ten) / h30_ten;
+	mod_radio_reg(pi, RADIO_2064_REG03C, 0x3f, cp_current);
+
+	if (channel >= 1 && channel <= 5)
+		write_radio_reg(pi, RADIO_2064_REG03C, 0x8);
+	else
+		write_radio_reg(pi, RADIO_2064_REG03C, 0x7);
+	write_radio_reg(pi, RADIO_2064_REG03D, 0x3);
+
+	mod_radio_reg(pi, RADIO_2064_REG044, 0x0c, 0x0c);
+	udelay(1);
+
+	wlc_2064_vco_cal(pi);
+
+	write_radio_reg(pi, RADIO_2064_REG044, pll_pwrup);
+	write_radio_reg(pi, RADIO_2064_REG12B, pll_pwrup_ovr);
+	if (LCNREV_IS(pi->pubpi.phy_rev, 1)) {
+		write_radio_reg(pi, RADIO_2064_REG038, 3);
+		write_radio_reg(pi, RADIO_2064_REG091, 7);
+	}
+}
+
+static int
+wlc_lcnphy_load_tx_iir_filter(struct brcms_phy *pi, bool is_ofdm, s16 filt_type)
+{
+	s16 filt_index = -1;
+	int j;
+
+	u16 addr[] = {
+		0x910,
+		0x91e,
+		0x91f,
+		0x924,
+		0x925,
+		0x926,
+		0x920,
+		0x921,
+		0x927,
+		0x928,
+		0x929,
+		0x922,
+		0x923,
+		0x930,
+		0x931,
+		0x932
+	};
+
+	u16 addr_ofdm[] = {
+		0x90f,
+		0x900,
+		0x901,
+		0x906,
+		0x907,
+		0x908,
+		0x902,
+		0x903,
+		0x909,
+		0x90a,
+		0x90b,
+		0x904,
+		0x905,
+		0x90c,
+		0x90d,
+		0x90e
+	};
+
+	if (!is_ofdm) {
+		for (j = 0; j < LCNPHY_NUM_TX_DIG_FILTERS_CCK; j++) {
+			if (filt_type == LCNPHY_txdigfiltcoeffs_cck[j][0]) {
+				filt_index = (s16) j;
+				break;
+			}
+		}
+
+		if (filt_index != -1) {
+			for (j = 0; j < LCNPHY_NUM_DIG_FILT_COEFFS; j++)
+				write_phy_reg(pi, addr[j],
+					      LCNPHY_txdigfiltcoeffs_cck
+					      [filt_index][j + 1]);
+		}
+	} else {
+		for (j = 0; j < LCNPHY_NUM_TX_DIG_FILTERS_OFDM; j++) {
+			if (filt_type == LCNPHY_txdigfiltcoeffs_ofdm[j][0]) {
+				filt_index = (s16) j;
+				break;
+			}
+		}
+
+		if (filt_index != -1) {
+			for (j = 0; j < LCNPHY_NUM_DIG_FILT_COEFFS; j++)
+				write_phy_reg(pi, addr_ofdm[j],
+					      LCNPHY_txdigfiltcoeffs_ofdm
+					      [filt_index][j + 1]);
+		}
+	}
+
+	return (filt_index != -1) ? 0 : -1;
+}
+
+void wlc_phy_chanspec_set_lcnphy(struct brcms_phy *pi, u16 chanspec)
 {
 	u8 channel = CHSPEC_CHANNEL(chanspec);
 
@@ -1211,10 +1872,8 @@ void wlc_phy_chanspec_set_lcnphy(struct brcms_phy *pi, chanspec_t chanspec)
 	or_phy_reg(pi, 0x44a, 0x44);
 	write_phy_reg(pi, 0x44a, 0x80);
 
-	if (!NORADIO_ENAB(pi->pubpi)) {
-		wlc_lcnphy_radio_2064_channel_tune_4313(pi, channel);
-		udelay(1000);
-	}
+	wlc_lcnphy_radio_2064_channel_tune_4313(pi, channel);
+	udelay(1000);
 
 	wlc_lcnphy_toggle_afe_pwdn(pi);
 
@@ -1237,35 +1896,13 @@ void wlc_phy_chanspec_set_lcnphy(struct brcms_phy *pi, chanspec_t chanspec)
 
 }
 
-static void wlc_lcnphy_set_dac_gain(struct brcms_phy *pi, u16 dac_gain)
-{
-	u16 dac_ctrl;
-
-	dac_ctrl = (read_phy_reg(pi, 0x439) >> 0);
-	dac_ctrl = dac_ctrl & 0xc7f;
-	dac_ctrl = dac_ctrl | (dac_gain << 7);
-	mod_phy_reg(pi, 0x439, (0xfff << 0), (dac_ctrl) << 0);
-
-}
-
-static void wlc_lcnphy_set_tx_gain_override(struct brcms_phy *pi, bool bEnable)
-{
-	u16 bit = bEnable ? 1 : 0;
-
-	mod_phy_reg(pi, 0x4b0, (0x1 << 7), bit << 7);
-
-	mod_phy_reg(pi, 0x4b0, (0x1 << 14), bit << 14);
-
-	mod_phy_reg(pi, 0x43b, (0x1 << 6), bit << 6);
-}
-
 static u16 wlc_lcnphy_get_pa_gain(struct brcms_phy *pi)
 {
 	u16 pa_gain;
 
 	pa_gain = (read_phy_reg(pi, 0x4fb) &
 		   LCNPHY_txgainctrlovrval1_pagain_ovr_val1_MASK) >>
-	    LCNPHY_txgainctrlovrval1_pagain_ovr_val1_SHIFT;
+		  LCNPHY_txgainctrlovrval1_pagain_ovr_val1_SHIFT;
 
 	return pa_gain;
 }
@@ -1275,18 +1912,22 @@ static void wlc_lcnphy_set_tx_gain(struct brcms_phy *pi,
 {
 	u16 pa_gain = wlc_lcnphy_get_pa_gain(pi);
 
-	mod_phy_reg(pi, 0x4b5,
-		    (0xffff << 0),
-		    ((target_gains->gm_gain) | (target_gains->pga_gain << 8)) <<
-		    0);
+	mod_phy_reg(
+		pi, 0x4b5,
+		(0xffff << 0),
+		((target_gains->gm_gain) |
+		 (target_gains->pga_gain << 8)) <<
+		0);
 	mod_phy_reg(pi, 0x4fb,
 		    (0x7fff << 0),
 		    ((target_gains->pad_gain) | (pa_gain << 8)) << 0);
 
-	mod_phy_reg(pi, 0x4fc,
-		    (0xffff << 0),
-		    ((target_gains->gm_gain) | (target_gains->pga_gain << 8)) <<
-		    0);
+	mod_phy_reg(
+		pi, 0x4fc,
+		(0xffff << 0),
+		((target_gains->gm_gain) |
+		 (target_gains->pga_gain << 8)) <<
+		0);
 	mod_phy_reg(pi, 0x4fd,
 		    (0x7fff << 0),
 		    ((target_gains->pad_gain) | (pa_gain << 8)) << 0);
@@ -1404,8 +2045,8 @@ static void wlc_lcnphy_pwrctrl_rssiparams(struct brcms_phy *pi)
 	u16 auxpga_vmid, auxpga_vmid_temp, auxpga_gain_temp;
 	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
 
-	auxpga_vmid =
-	    (2 << 8) | (pi_lcn->lcnphy_rssi_vc << 4) | pi_lcn->lcnphy_rssi_vf;
+	auxpga_vmid = (2 << 8) |
+		      (pi_lcn->lcnphy_rssi_vc << 4) | pi_lcn->lcnphy_rssi_vf;
 	auxpga_vmid_temp = (2 << 8) | (8 << 4) | 4;
 	auxpga_gain_temp = 2;
 
@@ -1531,10 +2172,9 @@ static void wlc_lcnphy_tssi_setup(struct brcms_phy *pi)
 
 	mod_radio_reg(pi, RADIO_2064_REG005, 0x8, 1 << 3);
 
-	if (!wlc_lcnphy_tempsense_based_pwr_ctrl_enabled(pi)) {
+	if (!wlc_lcnphy_tempsense_based_pwr_ctrl_enabled(pi))
 		mod_phy_reg(pi, 0x4d7,
 			    (0x1 << 3) | (0x7 << 12), 0 << 3 | 2 << 12);
-	}
 
 	rfseq = wlc_lcnphy_rfseq_tbl_adc_pwrup(pi);
 	tab.tbl_id = LCNPHY_TBL_ID_RFSEQ;
@@ -1601,7 +2241,7 @@ void wlc_lcnphy_txpower_recalc_target(struct brcms_phy *pi)
 {
 	struct phytbl_info tab;
 	u32 rate_table[BRCMS_NUM_RATES_CCK + BRCMS_NUM_RATES_OFDM +
-			  BRCMS_NUM_RATES_MCS_1_STREAM];
+		       BRCMS_NUM_RATES_MCS_1_STREAM];
 	uint i, j;
 	if (wlc_lcnphy_tempsense_based_pwr_ctrl_enabled(pi))
 		return;
@@ -1694,34 +2334,28 @@ static s8 wlc_lcnphy_tempcompensated_txpwrctrl(struct brcms_phy *pi)
 
 	index = FIXED_TXPWR;
 
-	if (NORADIO_ENAB(pi->pubpi))
+	if (pi_lcn->lcnphy_tempsense_slope == 0)
 		return index;
 
-	if (pi_lcn->lcnphy_tempsense_slope == 0) {
-		return index;
-	}
 	temp = (u16) wlc_lcnphy_tempsense(pi, 0);
 	meas_temp = LCNPHY_TEMPSENSE(temp);
 
-	if (pi->tx_power_min != 0) {
+	if (pi->tx_power_min != 0)
 		delta_brd = (pi_lcn->lcnphy_measPower - pi->tx_power_min);
-	} else {
+	else
 		delta_brd = 0;
-	}
 
 	manp = LCNPHY_TEMPSENSE(pi_lcn->lcnphy_rawtempsense);
 	temp_diff = manp - meas_temp;
 	if (temp_diff < 0) {
-
 		neg = 1;
-
 		temp_diff = -temp_diff;
 	}
 
 	delta_temp = (s8) wlc_lcnphy_qdiv_roundup((u32) (temp_diff * 192),
-						    (u32) (pi_lcn->
-							      lcnphy_tempsense_slope
-							      * 10), 0);
+						  (u32) (pi_lcn->
+							 lcnphy_tempsense_slope
+							 * 10), 0);
 	if (neg)
 		delta_temp = -delta_temp;
 
@@ -1735,14 +2369,15 @@ static s8 wlc_lcnphy_tempcompensated_txpwrctrl(struct brcms_phy *pi)
 	if (LCNREV_IS(pi->pubpi.phy_rev, 1))
 		tempcorrx = 4;
 	new_index =
-	    index + delta_brd + delta_temp - pi_lcn->lcnphy_bandedge_corr;
+		index + delta_brd + delta_temp - pi_lcn->lcnphy_bandedge_corr;
 	new_index += tempcorrx;
 
 	if (LCNREV_IS(pi->pubpi.phy_rev, 1))
 		index = 127;
-	if (new_index < 0 || new_index > 126) {
+
+	if (new_index < 0 || new_index > 126)
 		return index;
-	}
+
 	return new_index;
 }
 
@@ -1792,7 +2427,7 @@ void wlc_lcnphy_set_tx_pwr_ctrl(struct brcms_phy *pi, u16 mode)
 			mod_radio_reg(pi, RADIO_2064_REG11F, 0x4, 0);
 
 			pi_lcn->lcnphy_tssi_tx_cnt =
-			    wlc_lcnphy_total_tx_frames(pi);
+				wlc_lcnphy_total_tx_frames(pi);
 
 			wlc_lcnphy_disable_tx_gain_override(pi);
 			pi_lcn->lcnphy_tx_power_idx_override = -1;
@@ -1805,11 +2440,105 @@ void wlc_lcnphy_set_tx_pwr_ctrl(struct brcms_phy *pi, u16 mode)
 			index = wlc_lcnphy_tempcompensated_txpwrctrl(pi);
 			wlc_lcnphy_set_tx_pwr_soft_ctrl(pi, index);
 			pi_lcn->lcnphy_current_index = (s8)
-			    ((read_phy_reg(pi, 0x4a9) & 0xFF) / 2);
+						       ((read_phy_reg(pi,
+								      0x4a9) &
+							 0xFF) / 2);
 		}
 	}
 }
 
+static void
+wlc_lcnphy_tx_iqlo_loopback(struct brcms_phy *pi, u16 *values_to_save)
+{
+	u16 vmid;
+	int i;
+	for (i = 0; i < 20; i++)
+		values_to_save[i] =
+			read_radio_reg(pi, iqlo_loopback_rf_regs[i]);
+
+	mod_phy_reg(pi, 0x44c, (0x1 << 12), 1 << 12);
+	mod_phy_reg(pi, 0x44d, (0x1 << 14), 1 << 14);
+
+	mod_phy_reg(pi, 0x44c, (0x1 << 11), 1 << 11);
+	mod_phy_reg(pi, 0x44d, (0x1 << 13), 0 << 13);
+
+	mod_phy_reg(pi, 0x43b, (0x1 << 1), 1 << 1);
+	mod_phy_reg(pi, 0x43c, (0x1 << 1), 0 << 1);
+
+	mod_phy_reg(pi, 0x43b, (0x1 << 0), 1 << 0);
+	mod_phy_reg(pi, 0x43c, (0x1 << 0), 0 << 0);
+
+	if (LCNREV_IS(pi->pubpi.phy_rev, 2))
+		and_radio_reg(pi, RADIO_2064_REG03A, 0xFD);
+	else
+		and_radio_reg(pi, RADIO_2064_REG03A, 0xF9);
+	or_radio_reg(pi, RADIO_2064_REG11A, 0x1);
+
+	or_radio_reg(pi, RADIO_2064_REG036, 0x01);
+	or_radio_reg(pi, RADIO_2064_REG11A, 0x18);
+	udelay(20);
+
+	if (LCNREV_IS(pi->pubpi.phy_rev, 2)) {
+		if (CHSPEC_IS5G(pi->radio_chanspec))
+			mod_radio_reg(pi, RADIO_2064_REG03A, 1, 0);
+		else
+			or_radio_reg(pi, RADIO_2064_REG03A, 1);
+	} else {
+		if (CHSPEC_IS5G(pi->radio_chanspec))
+			mod_radio_reg(pi, RADIO_2064_REG03A, 3, 1);
+		else
+			or_radio_reg(pi, RADIO_2064_REG03A, 0x3);
+	}
+
+	udelay(20);
+
+	write_radio_reg(pi, RADIO_2064_REG025, 0xF);
+	if (LCNREV_IS(pi->pubpi.phy_rev, 2)) {
+		if (CHSPEC_IS5G(pi->radio_chanspec))
+			mod_radio_reg(pi, RADIO_2064_REG028, 0xF, 0x4);
+		else
+			mod_radio_reg(pi, RADIO_2064_REG028, 0xF, 0x6);
+	} else {
+		if (CHSPEC_IS5G(pi->radio_chanspec))
+			mod_radio_reg(pi, RADIO_2064_REG028, 0x1e, 0x4 << 1);
+		else
+			mod_radio_reg(pi, RADIO_2064_REG028, 0x1e, 0x6 << 1);
+	}
+
+	udelay(20);
+
+	write_radio_reg(pi, RADIO_2064_REG005, 0x8);
+	or_radio_reg(pi, RADIO_2064_REG112, 0x80);
+	udelay(20);
+
+	or_radio_reg(pi, RADIO_2064_REG0FF, 0x10);
+	or_radio_reg(pi, RADIO_2064_REG11F, 0x44);
+	udelay(20);
+
+	or_radio_reg(pi, RADIO_2064_REG00B, 0x7);
+	or_radio_reg(pi, RADIO_2064_REG113, 0x10);
+	udelay(20);
+
+	write_radio_reg(pi, RADIO_2064_REG007, 0x1);
+	udelay(20);
+
+	vmid = 0x2A6;
+	mod_radio_reg(pi, RADIO_2064_REG0FC, 0x3 << 0, (vmid >> 8) & 0x3);
+	write_radio_reg(pi, RADIO_2064_REG0FD, (vmid & 0xff));
+	or_radio_reg(pi, RADIO_2064_REG11F, 0x44);
+	udelay(20);
+
+	or_radio_reg(pi, RADIO_2064_REG0FF, 0x10);
+	udelay(20);
+	write_radio_reg(pi, RADIO_2064_REG012, 0x02);
+	or_radio_reg(pi, RADIO_2064_REG112, 0x06);
+	write_radio_reg(pi, RADIO_2064_REG036, 0x11);
+	write_radio_reg(pi, RADIO_2064_REG059, 0xcc);
+	write_radio_reg(pi, RADIO_2064_REG05C, 0x2e);
+	write_radio_reg(pi, RADIO_2064_REG078, 0xd7);
+	write_radio_reg(pi, RADIO_2064_REG092, 0x15);
+}
+
 static bool wlc_lcnphy_iqcal_wait(struct brcms_phy *pi)
 {
 	uint delay_count = 0;
@@ -1826,6 +2555,20 @@ static bool wlc_lcnphy_iqcal_wait(struct brcms_phy *pi)
 }
 
 static void
+wlc_lcnphy_tx_iqlo_loopback_cleanup(struct brcms_phy *pi, u16 *values_to_save)
+{
+	int i;
+
+	and_phy_reg(pi, 0x44c, 0x0 >> 11);
+
+	and_phy_reg(pi, 0x43b, 0xC);
+
+	for (i = 0; i < 20; i++)
+		write_radio_reg(pi, iqlo_loopback_rf_regs[i],
+				values_to_save[i]);
+}
+
+static void
 wlc_lcnphy_tx_iqlo_cal(struct brcms_phy *pi,
 		       struct lcnphy_txgains *target_gains,
 		       enum lcnphy_cal_mode cal_mode, bool keep_tone)
@@ -1837,20 +2580,23 @@ wlc_lcnphy_tx_iqlo_cal(struct brcms_phy *pi,
 	int j;
 	u16 ncorr_override[5];
 	u16 syst_coeffs[] = { 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
-		0x0000, 0x0000, 0x0000, 0x0000, 0x0000
-	};
+			      0x0000, 0x0000, 0x0000, 0x0000, 0x0000};
 
 	u16 commands_fullcal[] = {
-		0x8434, 0x8334, 0x8084, 0x8267, 0x8056, 0x8234 };
+		0x8434, 0x8334, 0x8084, 0x8267, 0x8056, 0x8234
+	};
 
 	u16 commands_recal[] = {
-		0x8434, 0x8334, 0x8084, 0x8267, 0x8056, 0x8234 };
+		0x8434, 0x8334, 0x8084, 0x8267, 0x8056, 0x8234
+	};
 
 	u16 command_nums_fullcal[] = {
-		0x7a97, 0x7a97, 0x7a97, 0x7a87, 0x7a87, 0x7b97 };
+		0x7a97, 0x7a97, 0x7a97, 0x7a87, 0x7a87, 0x7b97
+	};
 
 	u16 command_nums_recal[] = {
-		0x7a97, 0x7a97, 0x7a97, 0x7a87, 0x7a87, 0x7b97 };
+		0x7a97, 0x7a97, 0x7a97, 0x7a87, 0x7a87, 0x7b97
+	};
 	u16 *command_nums = command_nums_fullcal;
 
 	u16 *start_coeffs = NULL, *cal_cmds = NULL, cal_type, diq_start;
@@ -1862,13 +2608,9 @@ wlc_lcnphy_tx_iqlo_cal(struct brcms_phy *pi,
 	u16 *values_to_save;
 	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
 
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
-
 	values_to_save = kmalloc(sizeof(u16) * 20, GFP_ATOMIC);
-	if (NULL == values_to_save) {
+	if (NULL == values_to_save)
 		return;
-	}
 
 	save_sslpnRxFeClkEnCtrl = read_phy_reg(pi, 0x6db);
 	save_sslpnCalibClkEnCtrl = read_phy_reg(pi, 0x6da);
@@ -1927,7 +2669,7 @@ wlc_lcnphy_tx_iqlo_cal(struct brcms_phy *pi,
 	}
 
 	hash = (target_gains->gm_gain << 8) |
-	    (target_gains->pga_gain << 4) | (target_gains->pad_gain);
+	       (target_gains->pga_gain << 4) | (target_gains->pad_gain);
 
 	band_idx = (CHSPEC_IS5G(pi->radio_chanspec) ? 1 : 0);
 
@@ -1936,11 +2678,11 @@ wlc_lcnphy_tx_iqlo_cal(struct brcms_phy *pi,
 	for (j = 0; j < iqcal_gainparams_numgains_lcnphy[band_idx]; j++) {
 		if (hash == tbl_iqcal_gainparams_lcnphy[band_idx][j][0]) {
 			cal_gains.gm_gain =
-			    tbl_iqcal_gainparams_lcnphy[band_idx][j][1];
+				tbl_iqcal_gainparams_lcnphy[band_idx][j][1];
 			cal_gains.pga_gain =
-			    tbl_iqcal_gainparams_lcnphy[band_idx][j][2];
+				tbl_iqcal_gainparams_lcnphy[band_idx][j][2];
 			cal_gains.pad_gain =
-			    tbl_iqcal_gainparams_lcnphy[band_idx][j][3];
+				tbl_iqcal_gainparams_lcnphy[band_idx][j][3];
 			memcpy(ncorr_override,
 			       &tbl_iqcal_gainparams_lcnphy[band_idx][j][3],
 			       sizeof(ncorr_override));
@@ -1954,14 +2696,14 @@ wlc_lcnphy_tx_iqlo_cal(struct brcms_phy *pi,
 	write_phy_reg(pi, 0x93d, 0xc0);
 
 	wlc_lcnphy_common_write_table(pi, LCNPHY_TBL_ID_IQLOCAL,
-				      (const void *)
 				      lcnphy_iqcal_loft_gainladder,
 				      ARRAY_SIZE(lcnphy_iqcal_loft_gainladder),
 				      16, 0);
 
 	wlc_lcnphy_common_write_table(pi, LCNPHY_TBL_ID_IQLOCAL,
-				      (const void *)lcnphy_iqcal_ir_gainladder,
-				      ARRAY_SIZE(lcnphy_iqcal_ir_gainladder), 16,
+				      lcnphy_iqcal_ir_gainladder,
+				      ARRAY_SIZE(
+					      lcnphy_iqcal_ir_gainladder), 16,
 				      32);
 
 	if (pi->phy_tx_tone_freq) {
@@ -1985,13 +2727,12 @@ wlc_lcnphy_tx_iqlo_cal(struct brcms_phy *pi,
 		command_num = command_nums[i];
 		if (ncorr_override[cal_type])
 			command_num =
-			    ncorr_override[cal_type] << 8 | (command_num &
-							     0xff);
+				ncorr_override[cal_type] << 8 | (command_num &
+								 0xff);
 
 		write_phy_reg(pi, 0x452, command_num);
 
 		if ((cal_type == 3) || (cal_type == 4)) {
-
 			wlc_lcnphy_common_read_table(pi, LCNPHY_TBL_ID_IQLOCAL,
 						     &diq_start, 1, 16, 69);
 
@@ -2001,10 +2742,8 @@ wlc_lcnphy_tx_iqlo_cal(struct brcms_phy *pi,
 
 		write_phy_reg(pi, 0x451, cal_cmds[i]);
 
-		if (!wlc_lcnphy_iqcal_wait(pi)) {
-
+		if (!wlc_lcnphy_iqcal_wait(pi))
 			goto cleanup;
-		}
 
 		wlc_lcnphy_common_read_table(pi, LCNPHY_TBL_ID_IQLOCAL,
 					     best_coeffs,
@@ -2013,16 +2752,15 @@ wlc_lcnphy_tx_iqlo_cal(struct brcms_phy *pi,
 					      best_coeffs,
 					      ARRAY_SIZE(best_coeffs), 16, 64);
 
-		if ((cal_type == 3) || (cal_type == 4)) {
+		if ((cal_type == 3) || (cal_type == 4))
 			wlc_lcnphy_common_write_table(pi, LCNPHY_TBL_ID_IQLOCAL,
 						      &diq_start, 1, 16, 69);
-		}
 		wlc_lcnphy_common_read_table(pi, LCNPHY_TBL_ID_IQLOCAL,
 					     pi_lcn->lcnphy_cal_results.
 					     txiqlocal_bestcoeffs,
 					     ARRAY_SIZE(pi_lcn->
-						       lcnphy_cal_results.
-						       txiqlocal_bestcoeffs),
+							lcnphy_cal_results.
+							txiqlocal_bestcoeffs),
 					     16, 96);
 	}
 
@@ -2030,7 +2768,7 @@ wlc_lcnphy_tx_iqlo_cal(struct brcms_phy *pi,
 				     pi_lcn->lcnphy_cal_results.
 				     txiqlocal_bestcoeffs,
 				     ARRAY_SIZE(pi_lcn->lcnphy_cal_results.
-					       txiqlocal_bestcoeffs), 16, 96);
+						txiqlocal_bestcoeffs), 16, 96);
 	pi_lcn->lcnphy_cal_results.txiqlocal_bestcoeffs_valid = true;
 
 	wlc_lcnphy_common_write_table(pi, LCNPHY_TBL_ID_IQLOCAL,
@@ -2041,7 +2779,7 @@ wlc_lcnphy_tx_iqlo_cal(struct brcms_phy *pi,
 				      &pi_lcn->lcnphy_cal_results.
 				      txiqlocal_bestcoeffs[5], 2, 16, 85);
 
- cleanup:
+cleanup:
 	wlc_lcnphy_tx_iqlo_loopback_cleanup(pi, values_to_save);
 	kfree(values_to_save);
 
@@ -2071,14 +2809,14 @@ static void wlc_lcnphy_idle_tssi_est(struct brcms_phy_pub *ppi)
 	u16 SAVE_txpwrctrl = wlc_lcnphy_get_tx_pwr_ctrl(pi);
 	u16 SAVE_lpfgain = read_radio_reg(pi, RADIO_2064_REG112);
 	u16 SAVE_jtag_bb_afe_switch =
-	    read_radio_reg(pi, RADIO_2064_REG007) & 1;
+		read_radio_reg(pi, RADIO_2064_REG007) & 1;
 	u16 SAVE_jtag_auxpga = read_radio_reg(pi, RADIO_2064_REG0FF) & 0x10;
 	u16 SAVE_iqadc_aux_en = read_radio_reg(pi, RADIO_2064_REG11F) & 4;
 	idleTssi = read_phy_reg(pi, 0x4ab);
 	suspend =
-	    (0 ==
-	     (R_REG(&((struct brcms_phy *) pi)->regs->maccontrol) &
-	      MCTL_EN_MAC));
+		(0 ==
+		 (R_REG(&((struct brcms_phy *) pi)->regs->maccontrol) &
+		  MCTL_EN_MAC));
 	if (!suspend)
 		wlapi_suspend_mac_and_wait(pi->sh->physhim);
 	wlc_lcnphy_set_tx_pwr_ctrl(pi, LCNPHY_TX_PWR_CTRL_OFF);
@@ -2136,7 +2874,7 @@ static void wlc_lcnphy_vbat_temp_sense_setup(struct brcms_phy *pi, u8 mode)
 	struct phytbl_info tab;
 	u32 val;
 	u8 save_reg007, save_reg0FF, save_reg11F, save_reg005, save_reg025,
-	    save_reg112;
+	   save_reg112;
 	u16 values_to_save[14];
 	s8 index;
 	int i;
@@ -2152,8 +2890,7 @@ static void wlc_lcnphy_vbat_temp_sense_setup(struct brcms_phy *pi, u8 mode)
 
 	for (i = 0; i < 14; i++)
 		values_to_save[i] = read_phy_reg(pi, tempsense_phy_regs[i]);
-	suspend =
-	    (0 == (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
+	suspend = (0 == (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
 	if (!suspend)
 		wlapi_suspend_mac_and_wait(pi->sh->physhim);
 	save_txpwrCtrlEn = read_radio_reg(pi, 0x4a4);
@@ -2236,7 +2973,7 @@ static void wlc_lcnphy_vbat_temp_sense_setup(struct brcms_phy *pi, u8 mode)
 		auxpga_gain = 2;
 	}
 	auxpga_vmid =
-	    (u16) ((2 << 8) | (auxpga_vmidcourse << 4) | auxpga_vmidfine);
+		(u16) ((2 << 8) | (auxpga_vmidcourse << 4) | auxpga_vmidfine);
 	mod_phy_reg(pi, 0x4d8, (0x1 << 0), (1) << 0);
 
 	mod_phy_reg(pi, 0x4d8, (0x3ff << 2), (auxpga_vmid) << 2);
@@ -2280,17 +3017,10 @@ static void wlc_lcnphy_tx_pwr_ctrl_init(struct brcms_phy_pub *ppi)
 	struct brcms_phy *pi = (struct brcms_phy *) ppi;
 
 	suspend =
-	    (0 == (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
+		(0 == (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
 	if (!suspend)
 		wlapi_suspend_mac_and_wait(pi->sh->physhim);
 
-	if (NORADIO_ENAB(pi->pubpi)) {
-		wlc_lcnphy_set_bbmult(pi, 0x30);
-		if (!suspend)
-			wlapi_enable_mac(pi->sh->physhim);
-		return;
-	}
-
 	if (!pi->hwpwrctrl_capable) {
 		if (CHSPEC_IS2G(pi->radio_chanspec)) {
 			tx_gains.gm_gain = 4;
@@ -2382,26 +3112,6 @@ wlc_lcnphy_get_radio_loft(struct brcms_phy *pi,
 	*fq0 = LCNPHY_IQLOCC_READ(read_radio_reg(pi, RADIO_2064_REG08C));
 }
 
-static void
-wlc_lcnphy_get_tx_gain(struct brcms_phy *pi, struct lcnphy_txgains *gains)
-{
-	u16 dac_gain;
-
-	dac_gain = read_phy_reg(pi, 0x439) >> 0;
-	gains->dac_gain = (dac_gain & 0x380) >> 7;
-
-	{
-		u16 rfgain0, rfgain1;
-
-		rfgain0 = (read_phy_reg(pi, 0x4b5) & (0xffff << 0)) >> 0;
-		rfgain1 = (read_phy_reg(pi, 0x4fb) & (0x7fff << 0)) >> 0;
-
-		gains->gm_gain = rfgain0 & 0xff;
-		gains->pga_gain = (rfgain0 >> 8) & 0xff;
-		gains->pad_gain = rfgain1 & 0xff;
-	}
-}
-
 void wlc_lcnphy_set_tx_iqcc(struct brcms_phy *pi, u16 a, u16 b)
 {
 	struct phytbl_info tab;
@@ -2489,16 +3199,6 @@ void wlc_lcnphy_set_tx_pwr_by_index(struct brcms_phy *pi, int index)
 	}
 }
 
-static void wlc_lcnphy_set_trsw_override(struct brcms_phy *pi, bool tx, bool rx)
-{
-
-	mod_phy_reg(pi, 0x44d,
-		    (0x1 << 1) |
-		    (0x1 << 0), (tx ? (0x1 << 1) : 0) | (rx ? (0x1 << 0) : 0));
-
-	or_phy_reg(pi, 0x44c, (0x1 << 1) | (0x1 << 0));
-}
-
 static void wlc_lcnphy_clear_papd_comptable(struct brcms_phy *pi)
 {
 	u32 j;
@@ -2518,67 +3218,6 @@ static void wlc_lcnphy_clear_papd_comptable(struct brcms_phy *pi)
 	return;
 }
 
-static void
-wlc_lcnphy_set_rx_gain_by_distribution(struct brcms_phy *pi,
-				       u16 trsw,
-				       u16 ext_lna,
-				       u16 biq2,
-				       u16 biq1,
-				       u16 tia, u16 lna2, u16 lna1)
-{
-	u16 gain0_15, gain16_19;
-
-	gain16_19 = biq2 & 0xf;
-	gain0_15 = ((biq1 & 0xf) << 12) |
-	    ((tia & 0xf) << 8) |
-	    ((lna2 & 0x3) << 6) |
-	    ((lna2 & 0x3) << 4) | ((lna1 & 0x3) << 2) | ((lna1 & 0x3) << 0);
-
-	mod_phy_reg(pi, 0x4b6, (0xffff << 0), gain0_15 << 0);
-	mod_phy_reg(pi, 0x4b7, (0xf << 0), gain16_19 << 0);
-	mod_phy_reg(pi, 0x4b1, (0x3 << 11), lna1 << 11);
-
-	if (LCNREV_LT(pi->pubpi.phy_rev, 2)) {
-		mod_phy_reg(pi, 0x4b1, (0x1 << 9), ext_lna << 9);
-		mod_phy_reg(pi, 0x4b1, (0x1 << 10), ext_lna << 10);
-	} else {
-		mod_phy_reg(pi, 0x4b1, (0x1 << 10), 0 << 10);
-
-		mod_phy_reg(pi, 0x4b1, (0x1 << 15), 0 << 15);
-
-		mod_phy_reg(pi, 0x4b1, (0x1 << 9), ext_lna << 9);
-	}
-
-	mod_phy_reg(pi, 0x44d, (0x1 << 0), (!trsw) << 0);
-
-}
-
-static void
-wlc_lcnphy_rx_gain_override_enable(struct brcms_phy *pi, bool enable)
-{
-	u16 ebit = enable ? 1 : 0;
-
-	mod_phy_reg(pi, 0x4b0, (0x1 << 8), ebit << 8);
-
-	mod_phy_reg(pi, 0x44c, (0x1 << 0), ebit << 0);
-
-	if (LCNREV_LT(pi->pubpi.phy_rev, 2)) {
-		mod_phy_reg(pi, 0x44c, (0x1 << 4), ebit << 4);
-		mod_phy_reg(pi, 0x44c, (0x1 << 6), ebit << 6);
-		mod_phy_reg(pi, 0x4b0, (0x1 << 5), ebit << 5);
-		mod_phy_reg(pi, 0x4b0, (0x1 << 6), ebit << 6);
-	} else {
-		mod_phy_reg(pi, 0x4b0, (0x1 << 12), ebit << 12);
-		mod_phy_reg(pi, 0x4b0, (0x1 << 13), ebit << 13);
-		mod_phy_reg(pi, 0x4b0, (0x1 << 5), ebit << 5);
-	}
-
-	if (CHSPEC_IS2G(pi->radio_chanspec)) {
-		mod_phy_reg(pi, 0x4b0, (0x1 << 10), ebit << 10);
-		mod_phy_reg(pi, 0x4e5, (0x1 << 3), ebit << 3);
-	}
-}
-
 void wlc_lcnphy_tx_pu(struct brcms_phy *pi, bool bEnable)
 {
 	if (!bEnable) {
@@ -2589,9 +3228,9 @@ void wlc_lcnphy_tx_pu(struct brcms_phy *pi, bool bEnable)
 
 		and_phy_reg(pi, 0x44c,
 			    ~(u16) ((0x1 << 3) |
-				       (0x1 << 5) |
-				       (0x1 << 12) |
-				       (0x1 << 0) | (0x1 << 1) | (0x1 << 2)));
+				    (0x1 << 5) |
+				    (0x1 << 12) |
+				    (0x1 << 0) | (0x1 << 1) | (0x1 << 2)));
 
 		and_phy_reg(pi, 0x44d,
 			    ~(u16) ((0x1 << 3) | (0x1 << 5) | (0x1 << 14)));
@@ -2701,7 +3340,8 @@ void wlc_lcnphy_deaf_mode(struct brcms_phy *pi, bool mode)
 		mod_phy_reg((pi), 0x410,
 			    (0x1 << 6) |
 			    (0x1 << 5),
-			    ((CHSPEC_IS2G(pi->radio_chanspec)) ? (!mode) : 0) <<
+			    ((CHSPEC_IS2G(
+				      pi->radio_chanspec)) ? (!mode) : 0) <<
 			    6 | (!mode) << 5);
 		mod_phy_reg(pi, 0x410, (0x1 << 7), (mode) << 7);
 	}
@@ -2714,8 +3354,8 @@ wlc_lcnphy_start_tx_tone(struct brcms_phy *pi, s32 f_kHz, u16 max_val,
 	u8 phy_bw;
 	u16 num_samps, t, k;
 	u32 bw;
-	fixed theta = 0, rot = 0;
-	cs32 tone_samp;
+	s32 theta = 0, rot = 0;
+	struct cordic_iq tone_samp;
 	u32 data_buf[64];
 	u16 i_samp, q_samp;
 	struct phytbl_info tab;
@@ -2737,18 +3377,18 @@ wlc_lcnphy_start_tx_tone(struct brcms_phy *pi, s32 f_kHz, u16 max_val,
 		k = 1;
 		do {
 			bw = phy_bw * 1000 * k;
-			num_samps = bw / ABS(f_kHz);
+			num_samps = bw / abs(f_kHz);
 			k++;
-		} while ((num_samps * (u32) (ABS(f_kHz))) != bw);
+		} while ((num_samps * (u32) (abs(f_kHz))) != bw);
 	} else
 		num_samps = 2;
 
-	rot = FIXED((f_kHz * 36) / phy_bw) / 100;
+	rot = ((f_kHz * 36) / phy_bw) / 100;
 	theta = 0;
 
 	for (t = 0; t < num_samps; t++) {
 
-		wlc_phy_cordic(theta, &tone_samp);
+		tone_samp = cordic_calc_iq(theta);
 
 		theta += rot;
 
@@ -2802,10 +3442,359 @@ void wlc_lcnphy_stop_tx_tone(struct brcms_phy *pi)
 	wlc_lcnphy_deaf_mode(pi, false);
 }
 
-static void wlc_lcnphy_clear_trsw_override(struct brcms_phy *pi)
+static void
+wlc_lcnphy_set_cc(struct brcms_phy *pi, int cal_type, s16 coeff_x, s16 coeff_y)
 {
+	u16 di0dq0;
+	u16 x, y, data_rf;
+	int k;
+	switch (cal_type) {
+	case 0:
+		wlc_lcnphy_set_tx_iqcc(pi, coeff_x, coeff_y);
+		break;
+	case 2:
+		di0dq0 = (coeff_x & 0xff) << 8 | (coeff_y & 0xff);
+		wlc_lcnphy_set_tx_locc(pi, di0dq0);
+		break;
+	case 3:
+		k = wlc_lcnphy_calc_floor(coeff_x, 0);
+		y = 8 + k;
+		k = wlc_lcnphy_calc_floor(coeff_x, 1);
+		x = 8 - k;
+		data_rf = (x * 16 + y);
+		write_radio_reg(pi, RADIO_2064_REG089, data_rf);
+		k = wlc_lcnphy_calc_floor(coeff_y, 0);
+		y = 8 + k;
+		k = wlc_lcnphy_calc_floor(coeff_y, 1);
+		x = 8 - k;
+		data_rf = (x * 16 + y);
+		write_radio_reg(pi, RADIO_2064_REG08A, data_rf);
+		break;
+	case 4:
+		k = wlc_lcnphy_calc_floor(coeff_x, 0);
+		y = 8 + k;
+		k = wlc_lcnphy_calc_floor(coeff_x, 1);
+		x = 8 - k;
+		data_rf = (x * 16 + y);
+		write_radio_reg(pi, RADIO_2064_REG08B, data_rf);
+		k = wlc_lcnphy_calc_floor(coeff_y, 0);
+		y = 8 + k;
+		k = wlc_lcnphy_calc_floor(coeff_y, 1);
+		x = 8 - k;
+		data_rf = (x * 16 + y);
+		write_radio_reg(pi, RADIO_2064_REG08C, data_rf);
+		break;
+	}
+}
 
-	and_phy_reg(pi, 0x44c, (u16) ~((0x1 << 1) | (0x1 << 0)));
+static struct lcnphy_unsign16_struct
+wlc_lcnphy_get_cc(struct brcms_phy *pi, int cal_type)
+{
+	u16 a, b, didq;
+	u8 di0, dq0, ei, eq, fi, fq;
+	struct lcnphy_unsign16_struct cc;
+	cc.re = 0;
+	cc.im = 0;
+	switch (cal_type) {
+	case 0:
+		wlc_lcnphy_get_tx_iqcc(pi, &a, &b);
+		cc.re = a;
+		cc.im = b;
+		break;
+	case 2:
+		didq = wlc_lcnphy_get_tx_locc(pi);
+		di0 = (((didq & 0xff00) << 16) >> 24);
+		dq0 = (((didq & 0x00ff) << 24) >> 24);
+		cc.re = (u16) di0;
+		cc.im = (u16) dq0;
+		break;
+	case 3:
+		wlc_lcnphy_get_radio_loft(pi, &ei, &eq, &fi, &fq);
+		cc.re = (u16) ei;
+		cc.im = (u16) eq;
+		break;
+	case 4:
+		wlc_lcnphy_get_radio_loft(pi, &ei, &eq, &fi, &fq);
+		cc.re = (u16) fi;
+		cc.im = (u16) fq;
+		break;
+	}
+	return cc;
+}
+
+static void
+wlc_lcnphy_samp_cap(struct brcms_phy *pi, int clip_detect_algo, u16 thresh,
+		    s16 *ptr, int mode)
+{
+	u32 curval1, curval2, stpptr, curptr, strptr, val;
+	u16 sslpnCalibClkEnCtrl, timer;
+	u16 old_sslpnCalibClkEnCtrl;
+	s16 imag, real;
+	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
+
+	timer = 0;
+	old_sslpnCalibClkEnCtrl = read_phy_reg(pi, 0x6da);
+
+	curval1 = R_REG(&pi->regs->psm_corectlsts);
+	ptr[130] = 0;
+	W_REG(&pi->regs->psm_corectlsts, ((1 << 6) | curval1));
+
+	W_REG(&pi->regs->smpl_clct_strptr, 0x7E00);
+	W_REG(&pi->regs->smpl_clct_stpptr, 0x8000);
+	udelay(20);
+	curval2 = R_REG(&pi->regs->psm_phy_hdr_param);
+	W_REG(&pi->regs->psm_phy_hdr_param, curval2 | 0x30);
+
+	write_phy_reg(pi, 0x555, 0x0);
+	write_phy_reg(pi, 0x5a6, 0x5);
+
+	write_phy_reg(pi, 0x5a2, (u16) (mode | mode << 6));
+	write_phy_reg(pi, 0x5cf, 3);
+	write_phy_reg(pi, 0x5a5, 0x3);
+	write_phy_reg(pi, 0x583, 0x0);
+	write_phy_reg(pi, 0x584, 0x0);
+	write_phy_reg(pi, 0x585, 0x0fff);
+	write_phy_reg(pi, 0x586, 0x0000);
+
+	write_phy_reg(pi, 0x580, 0x4501);
+
+	sslpnCalibClkEnCtrl = read_phy_reg(pi, 0x6da);
+	write_phy_reg(pi, 0x6da, (u32) (sslpnCalibClkEnCtrl | 0x2008));
+	stpptr = R_REG(&pi->regs->smpl_clct_stpptr);
+	curptr = R_REG(&pi->regs->smpl_clct_curptr);
+	do {
+		udelay(10);
+		curptr = R_REG(&pi->regs->smpl_clct_curptr);
+		timer++;
+	} while ((curptr != stpptr) && (timer < 500));
+
+	W_REG(&pi->regs->psm_phy_hdr_param, 0x2);
+	strptr = 0x7E00;
+	W_REG(&pi->regs->tplatewrptr, strptr);
+	while (strptr < 0x8000) {
+		val = R_REG(&pi->regs->tplatewrdata);
+		imag = ((val >> 16) & 0x3ff);
+		real = ((val) & 0x3ff);
+		if (imag > 511)
+			imag -= 1024;
+
+		if (real > 511)
+			real -= 1024;
+
+		if (pi_lcn->lcnphy_iqcal_swp_dis)
+			ptr[(strptr - 0x7E00) / 4] = real;
+		else
+			ptr[(strptr - 0x7E00) / 4] = imag;
+
+		if (clip_detect_algo) {
+			if (imag > thresh || imag < -thresh) {
+				strptr = 0x8000;
+				ptr[130] = 1;
+			}
+		}
+
+		strptr += 4;
+	}
+
+	write_phy_reg(pi, 0x6da, old_sslpnCalibClkEnCtrl);
+	W_REG(&pi->regs->psm_phy_hdr_param, curval2);
+	W_REG(&pi->regs->psm_corectlsts, curval1);
+}
+
+static void
+wlc_lcnphy_a1(struct brcms_phy *pi, int cal_type, int num_levels,
+	      int step_size_lg2)
+{
+	const struct lcnphy_spb_tone *phy_c1;
+	struct lcnphy_spb_tone phy_c2;
+	struct lcnphy_unsign16_struct phy_c3;
+	int phy_c4, phy_c5, k, l, j, phy_c6;
+	u16 phy_c7, phy_c8, phy_c9;
+	s16 phy_c10, phy_c11, phy_c12, phy_c13, phy_c14, phy_c15, phy_c16;
+	s16 *ptr, phy_c17;
+	s32 phy_c18, phy_c19;
+	u32 phy_c20, phy_c21;
+	bool phy_c22, phy_c23, phy_c24, phy_c25;
+	u16 phy_c26, phy_c27;
+	u16 phy_c28, phy_c29, phy_c30;
+	u16 phy_c31;
+	u16 *phy_c32;
+	phy_c21 = 0;
+	phy_c10 = phy_c13 = phy_c14 = phy_c8 = 0;
+	ptr = kmalloc(sizeof(s16) * 131, GFP_ATOMIC);
+	if (NULL == ptr)
+		return;
+
+	phy_c32 = kmalloc(sizeof(u16) * 20, GFP_ATOMIC);
+	if (NULL == phy_c32) {
+		kfree(ptr);
+		return;
+	}
+	phy_c26 = read_phy_reg(pi, 0x6da);
+	phy_c27 = read_phy_reg(pi, 0x6db);
+	phy_c31 = read_radio_reg(pi, RADIO_2064_REG026);
+	write_phy_reg(pi, 0x93d, 0xC0);
+
+	wlc_lcnphy_start_tx_tone(pi, 3750, 88, 0);
+	write_phy_reg(pi, 0x6da, 0xffff);
+	or_phy_reg(pi, 0x6db, 0x3);
+
+	wlc_lcnphy_tx_iqlo_loopback(pi, phy_c32);
+	udelay(500);
+	phy_c28 = read_phy_reg(pi, 0x938);
+	phy_c29 = read_phy_reg(pi, 0x4d7);
+	phy_c30 = read_phy_reg(pi, 0x4d8);
+	or_phy_reg(pi, 0x938, 0x1 << 2);
+	or_phy_reg(pi, 0x4d7, 0x1 << 2);
+	or_phy_reg(pi, 0x4d7, 0x1 << 3);
+	mod_phy_reg(pi, 0x4d7, (0x7 << 12), 0x2 << 12);
+	or_phy_reg(pi, 0x4d8, 1 << 0);
+	or_phy_reg(pi, 0x4d8, 1 << 1);
+	mod_phy_reg(pi, 0x4d8, (0x3ff << 2), 0x23A << 2);
+	mod_phy_reg(pi, 0x4d8, (0x7 << 12), 0x7 << 12);
+	phy_c1 = &lcnphy_spb_tone_3750[0];
+	phy_c4 = 32;
+
+	if (num_levels == 0) {
+		if (cal_type != 0)
+			num_levels = 4;
+		else
+			num_levels = 9;
+	}
+	if (step_size_lg2 == 0) {
+		if (cal_type != 0)
+			step_size_lg2 = 3;
+		else
+			step_size_lg2 = 8;
+	}
+
+	phy_c7 = (1 << step_size_lg2);
+	phy_c3 = wlc_lcnphy_get_cc(pi, cal_type);
+	phy_c15 = (s16) phy_c3.re;
+	phy_c16 = (s16) phy_c3.im;
+	if (cal_type == 2) {
+		if (phy_c3.re > 127)
+			phy_c15 = phy_c3.re - 256;
+		if (phy_c3.im > 127)
+			phy_c16 = phy_c3.im - 256;
+	}
+	wlc_lcnphy_set_cc(pi, cal_type, phy_c15, phy_c16);
+	udelay(20);
+	for (phy_c8 = 0; phy_c7 != 0 && phy_c8 < num_levels; phy_c8++) {
+		phy_c23 = 1;
+		phy_c22 = 0;
+		switch (cal_type) {
+		case 0:
+			phy_c10 = 511;
+			break;
+		case 2:
+			phy_c10 = 127;
+			break;
+		case 3:
+			phy_c10 = 15;
+			break;
+		case 4:
+			phy_c10 = 15;
+			break;
+		}
+
+		phy_c9 = read_phy_reg(pi, 0x93d);
+		phy_c9 = 2 * phy_c9;
+		phy_c24 = 0;
+		phy_c5 = 7;
+		phy_c25 = 1;
+		while (1) {
+			write_radio_reg(pi, RADIO_2064_REG026,
+					(phy_c5 & 0x7) | ((phy_c5 & 0x7) << 4));
+			udelay(50);
+			phy_c22 = 0;
+			ptr[130] = 0;
+			wlc_lcnphy_samp_cap(pi, 1, phy_c9, &ptr[0], 2);
+			if (ptr[130] == 1)
+				phy_c22 = 1;
+			if (phy_c22)
+				phy_c5 -= 1;
+			if ((phy_c22 != phy_c24) && (!phy_c25))
+				break;
+			if (!phy_c22)
+				phy_c5 += 1;
+			if (phy_c5 <= 0 || phy_c5 >= 7)
+				break;
+			phy_c24 = phy_c22;
+			phy_c25 = 0;
+		}
+
+		if (phy_c5 < 0)
+			phy_c5 = 0;
+		else if (phy_c5 > 7)
+			phy_c5 = 7;
+
+		for (k = -phy_c7; k <= phy_c7; k += phy_c7) {
+			for (l = -phy_c7; l <= phy_c7; l += phy_c7) {
+				phy_c11 = phy_c15 + k;
+				phy_c12 = phy_c16 + l;
+
+				if (phy_c11 < -phy_c10)
+					phy_c11 = -phy_c10;
+				else if (phy_c11 > phy_c10)
+					phy_c11 = phy_c10;
+				if (phy_c12 < -phy_c10)
+					phy_c12 = -phy_c10;
+				else if (phy_c12 > phy_c10)
+					phy_c12 = phy_c10;
+				wlc_lcnphy_set_cc(pi, cal_type, phy_c11,
+						  phy_c12);
+				udelay(20);
+				wlc_lcnphy_samp_cap(pi, 0, 0, ptr, 2);
+
+				phy_c18 = 0;
+				phy_c19 = 0;
+				for (j = 0; j < 128; j++) {
+					if (cal_type != 0)
+						phy_c6 = j % phy_c4;
+					else
+						phy_c6 = (2 * j) % phy_c4;
+
+					phy_c2.re = phy_c1[phy_c6].re;
+					phy_c2.im = phy_c1[phy_c6].im;
+					phy_c17 = ptr[j];
+					phy_c18 = phy_c18 + phy_c17 * phy_c2.re;
+					phy_c19 = phy_c19 + phy_c17 * phy_c2.im;
+				}
+
+				phy_c18 = phy_c18 >> 10;
+				phy_c19 = phy_c19 >> 10;
+				phy_c20 = ((phy_c18 * phy_c18) +
+					   (phy_c19 * phy_c19));
+
+				if (phy_c23 || phy_c20 < phy_c21) {
+					phy_c21 = phy_c20;
+					phy_c13 = phy_c11;
+					phy_c14 = phy_c12;
+				}
+				phy_c23 = 0;
+			}
+		}
+		phy_c23 = 1;
+		phy_c15 = phy_c13;
+		phy_c16 = phy_c14;
+		phy_c7 = phy_c7 >> 1;
+		wlc_lcnphy_set_cc(pi, cal_type, phy_c15, phy_c16);
+		udelay(20);
+	}
+	goto cleanup;
+cleanup:
+	wlc_lcnphy_tx_iqlo_loopback_cleanup(pi, phy_c32);
+	wlc_lcnphy_stop_tx_tone(pi);
+	write_phy_reg(pi, 0x6da, phy_c26);
+	write_phy_reg(pi, 0x6db, phy_c27);
+	write_phy_reg(pi, 0x938, phy_c28);
+	write_phy_reg(pi, 0x4d7, phy_c29);
+	write_phy_reg(pi, 0x4d8, phy_c30);
+	write_radio_reg(pi, RADIO_2064_REG026, phy_c31);
+
+	kfree(phy_c32);
+	kfree(ptr);
 }
 
 void wlc_lcnphy_get_tx_iqcc(struct brcms_phy *pi, u16 *a, u16 *b)
@@ -2824,6 +3813,28 @@ void wlc_lcnphy_get_tx_iqcc(struct brcms_phy *pi, u16 *a, u16 *b)
 	*b = iqcc[1];
 }
 
+static void wlc_lcnphy_tx_iqlo_soft_cal_full(struct brcms_phy *pi)
+{
+	struct lcnphy_unsign16_struct iqcc0, locc2, locc3, locc4;
+
+	wlc_lcnphy_set_cc(pi, 0, 0, 0);
+	wlc_lcnphy_set_cc(pi, 2, 0, 0);
+	wlc_lcnphy_set_cc(pi, 3, 0, 0);
+	wlc_lcnphy_set_cc(pi, 4, 0, 0);
+
+	wlc_lcnphy_a1(pi, 4, 0, 0);
+	wlc_lcnphy_a1(pi, 3, 0, 0);
+	wlc_lcnphy_a1(pi, 2, 3, 2);
+	wlc_lcnphy_a1(pi, 0, 5, 8);
+	wlc_lcnphy_a1(pi, 2, 2, 1);
+	wlc_lcnphy_a1(pi, 0, 4, 3);
+
+	iqcc0 = wlc_lcnphy_get_cc(pi, 0);
+	locc2 = wlc_lcnphy_get_cc(pi, 2);
+	locc3 = wlc_lcnphy_get_cc(pi, 3);
+	locc4 = wlc_lcnphy_get_cc(pi, 4);
+}
+
 u16 wlc_lcnphy_get_tx_locc(struct brcms_phy *pi)
 {
 	struct phytbl_info tab;
@@ -2878,12 +3889,11 @@ static void wlc_lcnphy_txpwrtbl_iqlo_cal(struct brcms_phy *pi)
 					lcnphy_recal ? LCNPHY_CAL_RECAL :
 					LCNPHY_CAL_FULL), false);
 	} else {
-
 		wlc_lcnphy_tx_iqlo_soft_cal_full(pi);
 	}
 
 	wlc_lcnphy_get_radio_loft(pi, &ei0, &eq0, &fi0, &fq0);
-	if ((ABS((s8) fi0) == 15) && (ABS((s8) fq0) == 15)) {
+	if ((abs((s8) fi0) == 15) && (abs((s8) fq0) == 15)) {
 		if (CHSPEC_IS5G(pi->radio_chanspec)) {
 			target_gains.gm_gain = 255;
 			target_gains.pga_gain = 255;
@@ -2905,10 +3915,8 @@ static void wlc_lcnphy_txpwrtbl_iqlo_cal(struct brcms_phy *pi)
 			wlc_lcnphy_tx_iqlo_cal(pi, &target_gains,
 					       LCNPHY_CAL_FULL, false);
 		} else {
-
 			wlc_lcnphy_tx_iqlo_soft_cal_full(pi);
 		}
-
 	}
 
 	wlc_lcnphy_get_tx_iqcc(pi, &a, &b);
@@ -2927,7 +3935,7 @@ static void wlc_lcnphy_txpwrtbl_iqlo_cal(struct brcms_phy *pi)
 
 		wlc_lcnphy_read_table(pi, &tab);
 		val = (val & 0xfff00000) |
-		    ((u32) (a & 0x3FF) << 10) | (b & 0x3ff);
+		      ((u32) (a & 0x3FF) << 10) | (b & 0x3ff);
 		wlc_lcnphy_write_table(pi, &tab);
 
 		val = didq;
@@ -2959,13 +3967,10 @@ s16 wlc_lcnphy_tempsense_new(struct brcms_phy *pi, bool mode)
 	s16 avg = 0;
 	bool suspend = 0;
 
-	if (NORADIO_ENAB(pi->pubpi))
-		return -1;
-
 	if (mode == 1) {
 		suspend =
-		    (0 ==
-		     (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
+			(0 ==
+			 (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
 		if (!suspend)
 			wlapi_suspend_mac_and_wait(pi->sh->physhim);
 		wlc_lcnphy_vbat_temp_sense_setup(pi, TEMPSENSE);
@@ -3006,13 +4011,10 @@ u16 wlc_lcnphy_tempsense(struct brcms_phy *pi, bool mode)
 	u16 SAVE_txpwrctrl = wlc_lcnphy_get_tx_pwr_ctrl(pi);
 	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
 
-	if (NORADIO_ENAB(pi->pubpi))
-		return -1;
-
 	if (mode == 1) {
 		suspend =
-		    (0 ==
-		     (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
+			(0 ==
+			 (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
 		if (!suspend)
 			wlapi_suspend_mac_and_wait(pi->sh->physhim);
 		wlc_lcnphy_vbat_temp_sense_setup(pi, TEMPSENSE);
@@ -3063,8 +4065,9 @@ s8 wlc_lcnphy_tempsense_degree(struct brcms_phy *pi, bool mode)
 {
 	s32 degree = wlc_lcnphy_tempsense_new(pi, mode);
 	degree =
-	    ((degree << 10) + LCN_TEMPSENSE_OFFSET + (LCN_TEMPSENSE_DEN >> 1))
-	    / LCN_TEMPSENSE_DEN;
+		((degree <<
+		  10) + LCN_TEMPSENSE_OFFSET + (LCN_TEMPSENSE_DEN >> 1))
+		/ LCN_TEMPSENSE_DEN;
 	return (s8) degree;
 }
 
@@ -3074,13 +4077,10 @@ s8 wlc_lcnphy_vbatsense(struct brcms_phy *pi, bool mode)
 	s32 avg = 0;
 	bool suspend = 0;
 
-	if (NORADIO_ENAB(pi->pubpi))
-		return -1;
-
 	if (mode == 1) {
 		suspend =
-		    (0 ==
-		     (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
+			(0 ==
+			 (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
 		if (!suspend)
 			wlapi_suspend_mac_and_wait(pi->sh->physhim);
 		wlc_lcnphy_vbat_temp_sense_setup(pi, VBATSENSE);
@@ -3093,9 +4093,8 @@ s8 wlc_lcnphy_vbatsense(struct brcms_phy *pi, bool mode)
 	else
 		avg = (s32) vbatsenseval;
 
-	avg =
-	    (avg * LCN_VBAT_SCALE_NOM +
-	     (LCN_VBAT_SCALE_DEN >> 1)) / LCN_VBAT_SCALE_DEN;
+	avg =	(avg * LCN_VBAT_SCALE_NOM +
+		 (LCN_VBAT_SCALE_DEN >> 1)) / LCN_VBAT_SCALE_DEN;
 
 	if (mode == 1) {
 		if (!suspend)
@@ -3118,312 +4117,8 @@ static void wlc_lcnphy_afe_clk_init(struct brcms_phy *pi, u8 mode)
 	wlc_lcnphy_toggle_afe_pwdn(pi);
 }
 
-static bool
-wlc_lcnphy_rx_iq_est(struct brcms_phy *pi,
-		     u16 num_samps,
-		     u8 wait_time, struct lcnphy_iq_est *iq_est)
-{
-	int wait_count = 0;
-	bool result = true;
-	u8 phybw40;
-	phybw40 = CHSPEC_IS40(pi->radio_chanspec);
-
-	mod_phy_reg(pi, 0x6da, (0x1 << 5), (1) << 5);
-
-	mod_phy_reg(pi, 0x410, (0x1 << 3), (0) << 3);
-
-	mod_phy_reg(pi, 0x482, (0xffff << 0), (num_samps) << 0);
-
-	mod_phy_reg(pi, 0x481, (0xff << 0), ((u16) wait_time) << 0);
-
-	mod_phy_reg(pi, 0x481, (0x1 << 8), (0) << 8);
-
-	mod_phy_reg(pi, 0x481, (0x1 << 9), (1) << 9);
-
-	while (read_phy_reg(pi, 0x481) & (0x1 << 9)) {
-
-		if (wait_count > (10 * 500)) {
-			result = false;
-			goto cleanup;
-		}
-		udelay(100);
-		wait_count++;
-	}
-
-	iq_est->iq_prod = ((u32) read_phy_reg(pi, 0x483) << 16) |
-	    (u32) read_phy_reg(pi, 0x484);
-	iq_est->i_pwr = ((u32) read_phy_reg(pi, 0x485) << 16) |
-	    (u32) read_phy_reg(pi, 0x486);
-	iq_est->q_pwr = ((u32) read_phy_reg(pi, 0x487) << 16) |
-	    (u32) read_phy_reg(pi, 0x488);
-
- cleanup:
-	mod_phy_reg(pi, 0x410, (0x1 << 3), (1) << 3);
-
-	mod_phy_reg(pi, 0x6da, (0x1 << 5), (0) << 5);
-
-	return result;
-}
-
-static bool wlc_lcnphy_calc_rx_iq_comp(struct brcms_phy *pi, u16 num_samps)
-{
-#define LCNPHY_MIN_RXIQ_PWR 2
-	bool result;
-	u16 a0_new, b0_new;
-	struct lcnphy_iq_est iq_est = { 0, 0, 0 };
-	s32 a, b, temp;
-	s16 iq_nbits, qq_nbits, arsh, brsh;
-	s32 iq;
-	u32 ii, qq;
-	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
-
-	a0_new = ((read_phy_reg(pi, 0x645) & (0x3ff << 0)) >> 0);
-	b0_new = ((read_phy_reg(pi, 0x646) & (0x3ff << 0)) >> 0);
-	mod_phy_reg(pi, 0x6d1, (0x1 << 2), (0) << 2);
-
-	mod_phy_reg(pi, 0x64b, (0x1 << 6), (1) << 6);
-
-	wlc_lcnphy_set_rx_iq_comp(pi, 0, 0);
-
-	result = wlc_lcnphy_rx_iq_est(pi, num_samps, 32, &iq_est);
-	if (!result)
-		goto cleanup;
-
-	iq = (s32) iq_est.iq_prod;
-	ii = iq_est.i_pwr;
-	qq = iq_est.q_pwr;
-
-	if ((ii + qq) < LCNPHY_MIN_RXIQ_PWR) {
-		result = false;
-		goto cleanup;
-	}
-
-	iq_nbits = wlc_phy_nbits(iq);
-	qq_nbits = wlc_phy_nbits(qq);
-
-	arsh = 10 - (30 - iq_nbits);
-	if (arsh >= 0) {
-		a = (-(iq << (30 - iq_nbits)) + (ii >> (1 + arsh)));
-		temp = (s32) (ii >> arsh);
-		if (temp == 0) {
-			return false;
-		}
-	} else {
-		a = (-(iq << (30 - iq_nbits)) + (ii << (-1 - arsh)));
-		temp = (s32) (ii << -arsh);
-		if (temp == 0) {
-			return false;
-		}
-	}
-	a /= temp;
-	brsh = qq_nbits - 31 + 20;
-	if (brsh >= 0) {
-		b = (qq << (31 - qq_nbits));
-		temp = (s32) (ii >> brsh);
-		if (temp == 0) {
-			return false;
-		}
-	} else {
-		b = (qq << (31 - qq_nbits));
-		temp = (s32) (ii << -brsh);
-		if (temp == 0) {
-			return false;
-		}
-	}
-	b /= temp;
-	b -= a * a;
-	b = (s32) int_sqrt((unsigned long) b);
-	b -= (1 << 10);
-	a0_new = (u16) (a & 0x3ff);
-	b0_new = (u16) (b & 0x3ff);
- cleanup:
-
-	wlc_lcnphy_set_rx_iq_comp(pi, a0_new, b0_new);
-
-	mod_phy_reg(pi, 0x64b, (0x1 << 0), (1) << 0);
-
-	mod_phy_reg(pi, 0x64b, (0x1 << 3), (1) << 3);
-
-	pi_lcn->lcnphy_cal_results.rxiqcal_coeff_a0 = a0_new;
-	pi_lcn->lcnphy_cal_results.rxiqcal_coeff_b0 = b0_new;
-
-	return result;
-}
-
-static bool
-wlc_lcnphy_rx_iq_cal(struct brcms_phy *pi,
-		     const struct lcnphy_rx_iqcomp *iqcomp,
-		     int iqcomp_sz, bool tx_switch, bool rx_switch, int module,
-		     int tx_gain_idx)
-{
-	struct lcnphy_txgains old_gains;
-	u16 tx_pwr_ctrl;
-	u8 tx_gain_index_old = 0;
-	bool result = false, tx_gain_override_old = false;
-	u16 i, Core1TxControl_old, RFOverride0_old,
-	    RFOverrideVal0_old, rfoverride2_old, rfoverride2val_old,
-	    rfoverride3_old, rfoverride3val_old, rfoverride4_old,
-	    rfoverride4val_old, afectrlovr_old, afectrlovrval_old;
-	int tia_gain;
-	u32 received_power, rx_pwr_threshold;
-	u16 old_sslpnCalibClkEnCtrl, old_sslpnRxFeClkEnCtrl;
-	u16 values_to_save[11];
-	s16 *ptr;
-	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
-
-	ptr = kmalloc(sizeof(s16) * 131, GFP_ATOMIC);
-	if (NULL == ptr) {
-		return false;
-	}
-	if (module == 2) {
-		while (iqcomp_sz--) {
-			if (iqcomp[iqcomp_sz].chan ==
-			    CHSPEC_CHANNEL(pi->radio_chanspec)) {
-
-				wlc_lcnphy_set_rx_iq_comp(pi,
-							  (u16)
-							  iqcomp[iqcomp_sz].a,
-							  (u16)
-							  iqcomp[iqcomp_sz].b);
-				result = true;
-				break;
-			}
-		}
-		goto cal_done;
-	}
-
-	if (module == 1) {
-
-		tx_pwr_ctrl = wlc_lcnphy_get_tx_pwr_ctrl(pi);
-		wlc_lcnphy_set_tx_pwr_ctrl(pi, LCNPHY_TX_PWR_CTRL_OFF);
-
-		for (i = 0; i < 11; i++) {
-			values_to_save[i] =
-			    read_radio_reg(pi, rxiq_cal_rf_reg[i]);
-		}
-		Core1TxControl_old = read_phy_reg(pi, 0x631);
-
-		or_phy_reg(pi, 0x631, 0x0015);
-
-		RFOverride0_old = read_phy_reg(pi, 0x44c);
-		RFOverrideVal0_old = read_phy_reg(pi, 0x44d);
-		rfoverride2_old = read_phy_reg(pi, 0x4b0);
-		rfoverride2val_old = read_phy_reg(pi, 0x4b1);
-		rfoverride3_old = read_phy_reg(pi, 0x4f9);
-		rfoverride3val_old = read_phy_reg(pi, 0x4fa);
-		rfoverride4_old = read_phy_reg(pi, 0x938);
-		rfoverride4val_old = read_phy_reg(pi, 0x939);
-		afectrlovr_old = read_phy_reg(pi, 0x43b);
-		afectrlovrval_old = read_phy_reg(pi, 0x43c);
-		old_sslpnCalibClkEnCtrl = read_phy_reg(pi, 0x6da);
-		old_sslpnRxFeClkEnCtrl = read_phy_reg(pi, 0x6db);
-
-		tx_gain_override_old = wlc_lcnphy_tx_gain_override_enabled(pi);
-		if (tx_gain_override_old) {
-			wlc_lcnphy_get_tx_gain(pi, &old_gains);
-			tx_gain_index_old = pi_lcn->lcnphy_current_index;
-		}
-
-		wlc_lcnphy_set_tx_pwr_by_index(pi, tx_gain_idx);
-
-		mod_phy_reg(pi, 0x4f9, (0x1 << 0), 1 << 0);
-		mod_phy_reg(pi, 0x4fa, (0x1 << 0), 0 << 0);
-
-		mod_phy_reg(pi, 0x43b, (0x1 << 1), 1 << 1);
-		mod_phy_reg(pi, 0x43c, (0x1 << 1), 0 << 1);
-
-		write_radio_reg(pi, RADIO_2064_REG116, 0x06);
-		write_radio_reg(pi, RADIO_2064_REG12C, 0x07);
-		write_radio_reg(pi, RADIO_2064_REG06A, 0xd3);
-		write_radio_reg(pi, RADIO_2064_REG098, 0x03);
-		write_radio_reg(pi, RADIO_2064_REG00B, 0x7);
-		mod_radio_reg(pi, RADIO_2064_REG113, 1 << 4, 1 << 4);
-		write_radio_reg(pi, RADIO_2064_REG01D, 0x01);
-		write_radio_reg(pi, RADIO_2064_REG114, 0x01);
-		write_radio_reg(pi, RADIO_2064_REG02E, 0x10);
-		write_radio_reg(pi, RADIO_2064_REG12A, 0x08);
-
-		mod_phy_reg(pi, 0x938, (0x1 << 0), 1 << 0);
-		mod_phy_reg(pi, 0x939, (0x1 << 0), 0 << 0);
-		mod_phy_reg(pi, 0x938, (0x1 << 1), 1 << 1);
-		mod_phy_reg(pi, 0x939, (0x1 << 1), 1 << 1);
-		mod_phy_reg(pi, 0x938, (0x1 << 2), 1 << 2);
-		mod_phy_reg(pi, 0x939, (0x1 << 2), 1 << 2);
-		mod_phy_reg(pi, 0x938, (0x1 << 3), 1 << 3);
-		mod_phy_reg(pi, 0x939, (0x1 << 3), 1 << 3);
-		mod_phy_reg(pi, 0x938, (0x1 << 5), 1 << 5);
-		mod_phy_reg(pi, 0x939, (0x1 << 5), 0 << 5);
-
-		mod_phy_reg(pi, 0x43b, (0x1 << 0), 1 << 0);
-		mod_phy_reg(pi, 0x43c, (0x1 << 0), 0 << 0);
-
-		wlc_lcnphy_start_tx_tone(pi, 2000, 120, 0);
-		write_phy_reg(pi, 0x6da, 0xffff);
-		or_phy_reg(pi, 0x6db, 0x3);
-		wlc_lcnphy_set_trsw_override(pi, tx_switch, rx_switch);
-		wlc_lcnphy_rx_gain_override_enable(pi, true);
-
-		tia_gain = 8;
-		rx_pwr_threshold = 950;
-		while (tia_gain > 0) {
-			tia_gain -= 1;
-			wlc_lcnphy_set_rx_gain_by_distribution(pi,
-							       0, 0, 2, 2,
-							       (u16)
-							       tia_gain, 1, 0);
-			udelay(500);
-
-			received_power =
-			    wlc_lcnphy_measure_digital_power(pi, 2000);
-			if (received_power < rx_pwr_threshold)
-				break;
-		}
-		result = wlc_lcnphy_calc_rx_iq_comp(pi, 0xffff);
-
-		wlc_lcnphy_stop_tx_tone(pi);
-
-		write_phy_reg(pi, 0x631, Core1TxControl_old);
-
-		write_phy_reg(pi, 0x44c, RFOverrideVal0_old);
-		write_phy_reg(pi, 0x44d, RFOverrideVal0_old);
-		write_phy_reg(pi, 0x4b0, rfoverride2_old);
-		write_phy_reg(pi, 0x4b1, rfoverride2val_old);
-		write_phy_reg(pi, 0x4f9, rfoverride3_old);
-		write_phy_reg(pi, 0x4fa, rfoverride3val_old);
-		write_phy_reg(pi, 0x938, rfoverride4_old);
-		write_phy_reg(pi, 0x939, rfoverride4val_old);
-		write_phy_reg(pi, 0x43b, afectrlovr_old);
-		write_phy_reg(pi, 0x43c, afectrlovrval_old);
-		write_phy_reg(pi, 0x6da, old_sslpnCalibClkEnCtrl);
-		write_phy_reg(pi, 0x6db, old_sslpnRxFeClkEnCtrl);
-
-		wlc_lcnphy_clear_trsw_override(pi);
-
-		mod_phy_reg(pi, 0x44c, (0x1 << 2), 0 << 2);
-
-		for (i = 0; i < 11; i++) {
-			write_radio_reg(pi, rxiq_cal_rf_reg[i],
-					values_to_save[i]);
-		}
-
-		if (tx_gain_override_old) {
-			wlc_lcnphy_set_tx_pwr_by_index(pi, tx_gain_index_old);
-		} else
-			wlc_lcnphy_disable_tx_gain_override(pi);
-		wlc_lcnphy_set_tx_pwr_ctrl(pi, tx_pwr_ctrl);
-
-		wlc_lcnphy_rx_gain_override_enable(pi, false);
-	}
-
- cal_done:
-	kfree(ptr);
-	return result;
-}
-
 static void wlc_lcnphy_temp_adj(struct brcms_phy *pi)
 {
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
 }
 
 static void wlc_lcnphy_glacial_timer_based_cal(struct brcms_phy *pi)
@@ -3433,7 +4128,7 @@ static void wlc_lcnphy_glacial_timer_based_cal(struct brcms_phy *pi)
 	u16 SAVE_pwrctrl = wlc_lcnphy_get_tx_pwr_ctrl(pi);
 	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
 	suspend =
-	    (0 == (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
+		(0 == (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
 	if (!suspend)
 		wlapi_suspend_mac_and_wait(pi->sh->physhim);
 	wlc_lcnphy_deaf_mode(pi, true);
@@ -3463,24 +4158,21 @@ static void wlc_lcnphy_periodic_cal(struct brcms_phy *pi)
 	s32 tssi, pwr, maxtargetpwr, mintargetpwr;
 	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
 
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
-
 	pi->phy_lastcal = pi->sh->now;
 	pi->phy_forcecal = false;
 	full_cal =
-	    (pi_lcn->lcnphy_full_cal_channel !=
-	     CHSPEC_CHANNEL(pi->radio_chanspec));
+		(pi_lcn->lcnphy_full_cal_channel !=
+		 CHSPEC_CHANNEL(pi->radio_chanspec));
 	pi_lcn->lcnphy_full_cal_channel = CHSPEC_CHANNEL(pi->radio_chanspec);
 	index = pi_lcn->lcnphy_current_index;
 
 	suspend =
-	    (0 == (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
+		(0 == (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
 	if (!suspend) {
-
 		wlapi_bmac_write_shm(pi->sh->physhim, M_CTS_DURATION, 10000);
 		wlapi_suspend_mac_and_wait(pi->sh->physhim);
 	}
+
 	wlc_lcnphy_deaf_mode(pi, true);
 
 	wlc_lcnphy_txpwrtbl_iqlo_cal(pi);
@@ -3531,7 +4223,6 @@ void wlc_lcnphy_calib_modes(struct brcms_phy *pi, uint mode)
 
 	switch (mode) {
 	case PHY_PERICAL_CHAN:
-
 		break;
 	case PHY_FULLCAL:
 		wlc_lcnphy_periodic_cal(pi);
@@ -3558,7 +4249,7 @@ void wlc_lcnphy_calib_modes(struct brcms_phy *pi, uint mode)
 	case LCNPHY_PERICAL_TEMPBASED_TXPWRCTRL:
 		if (wlc_lcnphy_tempsense_based_pwr_ctrl_enabled(pi))
 			wlc_lcnphy_tx_power_adjustment(
-						(struct brcms_phy_pub *) pi);
+				(struct brcms_phy_pub *) pi);
 		break;
 	}
 }
@@ -3571,7 +4262,7 @@ void wlc_lcnphy_get_tssi(struct brcms_phy *pi, s8 *ofdm_pwr, s8 *cck_pwr)
 	if (wlc_lcnphy_tssi_based_pwr_ctrl_enabled(pi) &&
 	    (status  & (0x1 << 15))) {
 		*ofdm_pwr = (s8) (((read_phy_reg(pi, 0x4ab) & (0x1ff << 0))
-				     >> 0) >> 1);
+				   >> 0) >> 1);
 
 		if (wlc_phy_tpc_isenabled_lcnphy(pi))
 			cck_offset = pi->tx_power_offset[TXP_FIRST_CCK];
@@ -3591,59 +4282,6 @@ void wlc_phy_cal_init_lcnphy(struct brcms_phy *pi)
 
 }
 
-static void
-wlc_lcnphy_set_chanspec_tweaks(struct brcms_phy *pi, chanspec_t chanspec)
-{
-	u8 channel = CHSPEC_CHANNEL(chanspec);
-	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
-
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
-
-	if (channel == 14) {
-		mod_phy_reg(pi, 0x448, (0x3 << 8), (2) << 8);
-
-	} else {
-		mod_phy_reg(pi, 0x448, (0x3 << 8), (1) << 8);
-
-	}
-	pi_lcn->lcnphy_bandedge_corr = 2;
-	if (channel == 1)
-		pi_lcn->lcnphy_bandedge_corr = 4;
-
-	if (channel == 1 || channel == 2 || channel == 3 ||
-	    channel == 4 || channel == 9 ||
-	    channel == 10 || channel == 11 || channel == 12) {
-		si_pmu_pllcontrol(pi->sh->sih, 0x2, 0xffffffff, 0x03000c04);
-		si_pmu_pllcontrol(pi->sh->sih, 0x3, 0xffffff, 0x0);
-		si_pmu_pllcontrol(pi->sh->sih, 0x4, 0xffffffff, 0x200005c0);
-
-		si_pmu_pllupd(pi->sh->sih);
-		write_phy_reg(pi, 0x942, 0);
-		wlc_lcnphy_txrx_spur_avoidance_mode(pi, false);
-		pi_lcn->lcnphy_spurmod = 0;
-		mod_phy_reg(pi, 0x424, (0xff << 8), (0x1b) << 8);
-
-		write_phy_reg(pi, 0x425, 0x5907);
-	} else {
-		si_pmu_pllcontrol(pi->sh->sih, 0x2, 0xffffffff, 0x03140c04);
-		si_pmu_pllcontrol(pi->sh->sih, 0x3, 0xffffff, 0x333333);
-		si_pmu_pllcontrol(pi->sh->sih, 0x4, 0xffffffff, 0x202c2820);
-
-		si_pmu_pllupd(pi->sh->sih);
-		write_phy_reg(pi, 0x942, 0);
-		wlc_lcnphy_txrx_spur_avoidance_mode(pi, true);
-
-		pi_lcn->lcnphy_spurmod = 0;
-		mod_phy_reg(pi, 0x424, (0xff << 8), (0x1f) << 8);
-
-		write_phy_reg(pi, 0x425, 0x590a);
-	}
-
-	or_phy_reg(pi, 0x44a, 0x44);
-	write_phy_reg(pi, 0x44a, 0x80);
-}
-
 void wlc_lcnphy_tx_power_adjustment(struct brcms_phy_pub *ppi)
 {
 	s8 index;
@@ -3651,570 +4289,21 @@ void wlc_lcnphy_tx_power_adjustment(struct brcms_phy_pub *ppi)
 	struct brcms_phy *pi = (struct brcms_phy *) ppi;
 	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
 	u16 SAVE_txpwrctrl = wlc_lcnphy_get_tx_pwr_ctrl(pi);
-	if (wlc_lcnphy_tempsense_based_pwr_ctrl_enabled(pi) && SAVE_txpwrctrl) {
+	if (wlc_lcnphy_tempsense_based_pwr_ctrl_enabled(pi) &&
+	    SAVE_txpwrctrl) {
 		index = wlc_lcnphy_tempcompensated_txpwrctrl(pi);
 		index2 = (u16) (index * 2);
 		mod_phy_reg(pi, 0x4a9, (0x1ff << 0), (index2) << 0);
 
-		pi_lcn->lcnphy_current_index = (s8)
-		    ((read_phy_reg(pi, 0x4a9) & 0xFF) / 2);
-	}
-}
-
-static void wlc_lcnphy_set_rx_iq_comp(struct brcms_phy *pi, u16 a, u16 b)
-{
-	mod_phy_reg(pi, 0x645, (0x3ff << 0), (a) << 0);
-
-	mod_phy_reg(pi, 0x646, (0x3ff << 0), (b) << 0);
-
-	mod_phy_reg(pi, 0x647, (0x3ff << 0), (a) << 0);
-
-	mod_phy_reg(pi, 0x648, (0x3ff << 0), (b) << 0);
-
-	mod_phy_reg(pi, 0x649, (0x3ff << 0), (a) << 0);
-
-	mod_phy_reg(pi, 0x64a, (0x3ff << 0), (b) << 0);
-
-}
-
-void wlc_phy_init_lcnphy(struct brcms_phy *pi)
-{
-	u8 phybw40;
-	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
-	phybw40 = CHSPEC_IS40(pi->radio_chanspec);
-
-	pi_lcn->lcnphy_cal_counter = 0;
-	pi_lcn->lcnphy_cal_temper = pi_lcn->lcnphy_rawtempsense;
-
-	or_phy_reg(pi, 0x44a, 0x80);
-	and_phy_reg(pi, 0x44a, 0x7f);
-
-	wlc_lcnphy_afe_clk_init(pi, AFE_CLK_INIT_MODE_TXRX2X);
-
-	write_phy_reg(pi, 0x60a, 160);
-
-	write_phy_reg(pi, 0x46a, 25);
-
-	wlc_lcnphy_baseband_init(pi);
-
-	wlc_lcnphy_radio_init(pi);
-
-	if (CHSPEC_IS2G(pi->radio_chanspec))
-		wlc_lcnphy_tx_pwr_ctrl_init((struct brcms_phy_pub *) pi);
-
-	wlc_phy_chanspec_set((struct brcms_phy_pub *) pi, pi->radio_chanspec);
-
-	si_pmu_regcontrol(pi->sh->sih, 0, 0xf, 0x9);
-
-	si_pmu_chipcontrol(pi->sh->sih, 0, 0xffffffff, 0x03CDDDDD);
-
-	if ((pi->sh->boardflags & BFL_FEM)
-	    && wlc_lcnphy_tempsense_based_pwr_ctrl_enabled(pi))
-		wlc_lcnphy_set_tx_pwr_by_index(pi, FIXED_TXPWR);
-
-	wlc_lcnphy_agc_temp_init(pi);
-
-	wlc_lcnphy_temp_adj(pi);
-
-	mod_phy_reg(pi, 0x448, (0x1 << 14), (1) << 14);
-
-	udelay(100);
-	mod_phy_reg(pi, 0x448, (0x1 << 14), (0) << 14);
-
-	wlc_lcnphy_set_tx_pwr_ctrl(pi, LCNPHY_TX_PWR_CTRL_HW);
-	pi_lcn->lcnphy_noise_samples = LCNPHY_NOISE_SAMPLES_DEFAULT;
-	wlc_lcnphy_calib_modes(pi, PHY_PERICAL_PHYINIT);
-}
-
-static void
-wlc_lcnphy_tx_iqlo_loopback(struct brcms_phy *pi, u16 *values_to_save)
-{
-	u16 vmid;
-	int i;
-	for (i = 0; i < 20; i++) {
-		values_to_save[i] =
-		    read_radio_reg(pi, iqlo_loopback_rf_regs[i]);
-	}
-
-	mod_phy_reg(pi, 0x44c, (0x1 << 12), 1 << 12);
-	mod_phy_reg(pi, 0x44d, (0x1 << 14), 1 << 14);
-
-	mod_phy_reg(pi, 0x44c, (0x1 << 11), 1 << 11);
-	mod_phy_reg(pi, 0x44d, (0x1 << 13), 0 << 13);
-
-	mod_phy_reg(pi, 0x43b, (0x1 << 1), 1 << 1);
-	mod_phy_reg(pi, 0x43c, (0x1 << 1), 0 << 1);
-
-	mod_phy_reg(pi, 0x43b, (0x1 << 0), 1 << 0);
-	mod_phy_reg(pi, 0x43c, (0x1 << 0), 0 << 0);
-
-	if (LCNREV_IS(pi->pubpi.phy_rev, 2))
-		and_radio_reg(pi, RADIO_2064_REG03A, 0xFD);
-	else
-		and_radio_reg(pi, RADIO_2064_REG03A, 0xF9);
-	or_radio_reg(pi, RADIO_2064_REG11A, 0x1);
-
-	or_radio_reg(pi, RADIO_2064_REG036, 0x01);
-	or_radio_reg(pi, RADIO_2064_REG11A, 0x18);
-	udelay(20);
-
-	if (LCNREV_IS(pi->pubpi.phy_rev, 2)) {
-		if (CHSPEC_IS5G(pi->radio_chanspec))
-			mod_radio_reg(pi, RADIO_2064_REG03A, 1, 0);
-		else
-			or_radio_reg(pi, RADIO_2064_REG03A, 1);
-	} else {
-		if (CHSPEC_IS5G(pi->radio_chanspec))
-			mod_radio_reg(pi, RADIO_2064_REG03A, 3, 1);
-		else
-			or_radio_reg(pi, RADIO_2064_REG03A, 0x3);
-	}
-
-	udelay(20);
-
-	write_radio_reg(pi, RADIO_2064_REG025, 0xF);
-	if (LCNREV_IS(pi->pubpi.phy_rev, 2)) {
-		if (CHSPEC_IS5G(pi->radio_chanspec))
-			mod_radio_reg(pi, RADIO_2064_REG028, 0xF, 0x4);
-		else
-			mod_radio_reg(pi, RADIO_2064_REG028, 0xF, 0x6);
-	} else {
-		if (CHSPEC_IS5G(pi->radio_chanspec))
-			mod_radio_reg(pi, RADIO_2064_REG028, 0x1e, 0x4 << 1);
-		else
-			mod_radio_reg(pi, RADIO_2064_REG028, 0x1e, 0x6 << 1);
-	}
-
-	udelay(20);
-
-	write_radio_reg(pi, RADIO_2064_REG005, 0x8);
-	or_radio_reg(pi, RADIO_2064_REG112, 0x80);
-	udelay(20);
-
-	or_radio_reg(pi, RADIO_2064_REG0FF, 0x10);
-	or_radio_reg(pi, RADIO_2064_REG11F, 0x44);
-	udelay(20);
-
-	or_radio_reg(pi, RADIO_2064_REG00B, 0x7);
-	or_radio_reg(pi, RADIO_2064_REG113, 0x10);
-	udelay(20);
-
-	write_radio_reg(pi, RADIO_2064_REG007, 0x1);
-	udelay(20);
-
-	vmid = 0x2A6;
-	mod_radio_reg(pi, RADIO_2064_REG0FC, 0x3 << 0, (vmid >> 8) & 0x3);
-	write_radio_reg(pi, RADIO_2064_REG0FD, (vmid & 0xff));
-	or_radio_reg(pi, RADIO_2064_REG11F, 0x44);
-	udelay(20);
-
-	or_radio_reg(pi, RADIO_2064_REG0FF, 0x10);
-	udelay(20);
-	write_radio_reg(pi, RADIO_2064_REG012, 0x02);
-	or_radio_reg(pi, RADIO_2064_REG112, 0x06);
-	write_radio_reg(pi, RADIO_2064_REG036, 0x11);
-	write_radio_reg(pi, RADIO_2064_REG059, 0xcc);
-	write_radio_reg(pi, RADIO_2064_REG05C, 0x2e);
-	write_radio_reg(pi, RADIO_2064_REG078, 0xd7);
-	write_radio_reg(pi, RADIO_2064_REG092, 0x15);
-}
-
-static void
-wlc_lcnphy_samp_cap(struct brcms_phy *pi, int clip_detect_algo, u16 thresh,
-		    s16 *ptr, int mode)
-{
-	u32 curval1, curval2, stpptr, curptr, strptr, val;
-	u16 sslpnCalibClkEnCtrl, timer;
-	u16 old_sslpnCalibClkEnCtrl;
-	s16 imag, real;
-	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
-
-	timer = 0;
-	old_sslpnCalibClkEnCtrl = read_phy_reg(pi, 0x6da);
-
-	curval1 = R_REG(&pi->regs->psm_corectlsts);
-	ptr[130] = 0;
-	W_REG(&pi->regs->psm_corectlsts, ((1 << 6) | curval1));
-
-	W_REG(&pi->regs->smpl_clct_strptr, 0x7E00);
-	W_REG(&pi->regs->smpl_clct_stpptr, 0x8000);
-	udelay(20);
-	curval2 = R_REG(&pi->regs->psm_phy_hdr_param);
-	W_REG(&pi->regs->psm_phy_hdr_param, curval2 | 0x30);
-
-	write_phy_reg(pi, 0x555, 0x0);
-	write_phy_reg(pi, 0x5a6, 0x5);
-
-	write_phy_reg(pi, 0x5a2, (u16) (mode | mode << 6));
-	write_phy_reg(pi, 0x5cf, 3);
-	write_phy_reg(pi, 0x5a5, 0x3);
-	write_phy_reg(pi, 0x583, 0x0);
-	write_phy_reg(pi, 0x584, 0x0);
-	write_phy_reg(pi, 0x585, 0x0fff);
-	write_phy_reg(pi, 0x586, 0x0000);
-
-	write_phy_reg(pi, 0x580, 0x4501);
-
-	sslpnCalibClkEnCtrl = read_phy_reg(pi, 0x6da);
-	write_phy_reg(pi, 0x6da, (u32) (sslpnCalibClkEnCtrl | 0x2008));
-	stpptr = R_REG(&pi->regs->smpl_clct_stpptr);
-	curptr = R_REG(&pi->regs->smpl_clct_curptr);
-	do {
-		udelay(10);
-		curptr = R_REG(&pi->regs->smpl_clct_curptr);
-		timer++;
-	} while ((curptr != stpptr) && (timer < 500));
-
-	W_REG(&pi->regs->psm_phy_hdr_param, 0x2);
-	strptr = 0x7E00;
-	W_REG(&pi->regs->tplatewrptr, strptr);
-	while (strptr < 0x8000) {
-		val = R_REG(&pi->regs->tplatewrdata);
-		imag = ((val >> 16) & 0x3ff);
-		real = ((val) & 0x3ff);
-		if (imag > 511) {
-			imag -= 1024;
-		}
-		if (real > 511) {
-			real -= 1024;
-		}
-		if (pi_lcn->lcnphy_iqcal_swp_dis)
-			ptr[(strptr - 0x7E00) / 4] = real;
-		else
-			ptr[(strptr - 0x7E00) / 4] = imag;
-		if (clip_detect_algo) {
-			if (imag > thresh || imag < -thresh) {
-				strptr = 0x8000;
-				ptr[130] = 1;
-			}
-		}
-		strptr += 4;
-	}
-
-	write_phy_reg(pi, 0x6da, old_sslpnCalibClkEnCtrl);
-	W_REG(&pi->regs->psm_phy_hdr_param, curval2);
-	W_REG(&pi->regs->psm_corectlsts, curval1);
-}
-
-static void wlc_lcnphy_tx_iqlo_soft_cal_full(struct brcms_phy *pi)
-{
-	struct lcnphy_unsign16_struct iqcc0, locc2, locc3, locc4;
-
-	wlc_lcnphy_set_cc(pi, 0, 0, 0);
-	wlc_lcnphy_set_cc(pi, 2, 0, 0);
-	wlc_lcnphy_set_cc(pi, 3, 0, 0);
-	wlc_lcnphy_set_cc(pi, 4, 0, 0);
-
-	wlc_lcnphy_a1(pi, 4, 0, 0);
-	wlc_lcnphy_a1(pi, 3, 0, 0);
-	wlc_lcnphy_a1(pi, 2, 3, 2);
-	wlc_lcnphy_a1(pi, 0, 5, 8);
-	wlc_lcnphy_a1(pi, 2, 2, 1);
-	wlc_lcnphy_a1(pi, 0, 4, 3);
-
-	iqcc0 = wlc_lcnphy_get_cc(pi, 0);
-	locc2 = wlc_lcnphy_get_cc(pi, 2);
-	locc3 = wlc_lcnphy_get_cc(pi, 3);
-	locc4 = wlc_lcnphy_get_cc(pi, 4);
-}
-
-static void
-wlc_lcnphy_set_cc(struct brcms_phy *pi, int cal_type, s16 coeff_x, s16 coeff_y)
-{
-	u16 di0dq0;
-	u16 x, y, data_rf;
-	int k;
-	switch (cal_type) {
-	case 0:
-		wlc_lcnphy_set_tx_iqcc(pi, coeff_x, coeff_y);
-		break;
-	case 2:
-		di0dq0 = (coeff_x & 0xff) << 8 | (coeff_y & 0xff);
-		wlc_lcnphy_set_tx_locc(pi, di0dq0);
-		break;
-	case 3:
-		k = wlc_lcnphy_calc_floor(coeff_x, 0);
-		y = 8 + k;
-		k = wlc_lcnphy_calc_floor(coeff_x, 1);
-		x = 8 - k;
-		data_rf = (x * 16 + y);
-		write_radio_reg(pi, RADIO_2064_REG089, data_rf);
-		k = wlc_lcnphy_calc_floor(coeff_y, 0);
-		y = 8 + k;
-		k = wlc_lcnphy_calc_floor(coeff_y, 1);
-		x = 8 - k;
-		data_rf = (x * 16 + y);
-		write_radio_reg(pi, RADIO_2064_REG08A, data_rf);
-		break;
-	case 4:
-		k = wlc_lcnphy_calc_floor(coeff_x, 0);
-		y = 8 + k;
-		k = wlc_lcnphy_calc_floor(coeff_x, 1);
-		x = 8 - k;
-		data_rf = (x * 16 + y);
-		write_radio_reg(pi, RADIO_2064_REG08B, data_rf);
-		k = wlc_lcnphy_calc_floor(coeff_y, 0);
-		y = 8 + k;
-		k = wlc_lcnphy_calc_floor(coeff_y, 1);
-		x = 8 - k;
-		data_rf = (x * 16 + y);
-		write_radio_reg(pi, RADIO_2064_REG08C, data_rf);
-		break;
-	}
-}
-
-static struct lcnphy_unsign16_struct
-wlc_lcnphy_get_cc(struct brcms_phy *pi, int cal_type)
-{
-	u16 a, b, didq;
-	u8 di0, dq0, ei, eq, fi, fq;
-	struct lcnphy_unsign16_struct cc;
-	cc.re = 0;
-	cc.im = 0;
-	switch (cal_type) {
-	case 0:
-		wlc_lcnphy_get_tx_iqcc(pi, &a, &b);
-		cc.re = a;
-		cc.im = b;
-		break;
-	case 2:
-		didq = wlc_lcnphy_get_tx_locc(pi);
-		di0 = (((didq & 0xff00) << 16) >> 24);
-		dq0 = (((didq & 0x00ff) << 24) >> 24);
-		cc.re = (u16) di0;
-		cc.im = (u16) dq0;
-		break;
-	case 3:
-		wlc_lcnphy_get_radio_loft(pi, &ei, &eq, &fi, &fq);
-		cc.re = (u16) ei;
-		cc.im = (u16) eq;
-		break;
-	case 4:
-		wlc_lcnphy_get_radio_loft(pi, &ei, &eq, &fi, &fq);
-		cc.re = (u16) fi;
-		cc.im = (u16) fq;
-		break;
-	}
-	return cc;
-}
-
-static void
-wlc_lcnphy_a1(struct brcms_phy *pi, int cal_type, int num_levels,
-	      int step_size_lg2)
-{
-	const struct lcnphy_spb_tone *phy_c1;
-	struct lcnphy_spb_tone phy_c2;
-	struct lcnphy_unsign16_struct phy_c3;
-	int phy_c4, phy_c5, k, l, j, phy_c6;
-	u16 phy_c7, phy_c8, phy_c9;
-	s16 phy_c10, phy_c11, phy_c12, phy_c13, phy_c14, phy_c15, phy_c16;
-	s16 *ptr, phy_c17;
-	s32 phy_c18, phy_c19;
-	u32 phy_c20, phy_c21;
-	bool phy_c22, phy_c23, phy_c24, phy_c25;
-	u16 phy_c26, phy_c27;
-	u16 phy_c28, phy_c29, phy_c30;
-	u16 phy_c31;
-	u16 *phy_c32;
-	phy_c21 = 0;
-	phy_c10 = phy_c13 = phy_c14 = phy_c8 = 0;
-	ptr = kmalloc(sizeof(s16) * 131, GFP_ATOMIC);
-	if (NULL == ptr) {
-		return;
-	}
-
-	phy_c32 = kmalloc(sizeof(u16) * 20, GFP_ATOMIC);
-	if (NULL == phy_c32) {
-		kfree(ptr);
-		return;
-	}
-	phy_c26 = read_phy_reg(pi, 0x6da);
-	phy_c27 = read_phy_reg(pi, 0x6db);
-	phy_c31 = read_radio_reg(pi, RADIO_2064_REG026);
-	write_phy_reg(pi, 0x93d, 0xC0);
-
-	wlc_lcnphy_start_tx_tone(pi, 3750, 88, 0);
-	write_phy_reg(pi, 0x6da, 0xffff);
-	or_phy_reg(pi, 0x6db, 0x3);
-
-	wlc_lcnphy_tx_iqlo_loopback(pi, phy_c32);
-	udelay(500);
-	phy_c28 = read_phy_reg(pi, 0x938);
-	phy_c29 = read_phy_reg(pi, 0x4d7);
-	phy_c30 = read_phy_reg(pi, 0x4d8);
-	or_phy_reg(pi, 0x938, 0x1 << 2);
-	or_phy_reg(pi, 0x4d7, 0x1 << 2);
-	or_phy_reg(pi, 0x4d7, 0x1 << 3);
-	mod_phy_reg(pi, 0x4d7, (0x7 << 12), 0x2 << 12);
-	or_phy_reg(pi, 0x4d8, 1 << 0);
-	or_phy_reg(pi, 0x4d8, 1 << 1);
-	mod_phy_reg(pi, 0x4d8, (0x3ff << 2), 0x23A << 2);
-	mod_phy_reg(pi, 0x4d8, (0x7 << 12), 0x7 << 12);
-	phy_c1 = &lcnphy_spb_tone_3750[0];
-	phy_c4 = 32;
-
-	if (num_levels == 0) {
-		if (cal_type != 0) {
-			num_levels = 4;
-		} else {
-			num_levels = 9;
-		}
-	}
-	if (step_size_lg2 == 0) {
-		if (cal_type != 0) {
-			step_size_lg2 = 3;
-		} else {
-			step_size_lg2 = 8;
-		}
-	}
-
-	phy_c7 = (1 << step_size_lg2);
-	phy_c3 = wlc_lcnphy_get_cc(pi, cal_type);
-	phy_c15 = (s16) phy_c3.re;
-	phy_c16 = (s16) phy_c3.im;
-	if (cal_type == 2) {
-		if (phy_c3.re > 127)
-			phy_c15 = phy_c3.re - 256;
-		if (phy_c3.im > 127)
-			phy_c16 = phy_c3.im - 256;
-	}
-	wlc_lcnphy_set_cc(pi, cal_type, phy_c15, phy_c16);
-	udelay(20);
-	for (phy_c8 = 0; phy_c7 != 0 && phy_c8 < num_levels; phy_c8++) {
-		phy_c23 = 1;
-		phy_c22 = 0;
-		switch (cal_type) {
-		case 0:
-			phy_c10 = 511;
-			break;
-		case 2:
-			phy_c10 = 127;
-			break;
-		case 3:
-			phy_c10 = 15;
-			break;
-		case 4:
-			phy_c10 = 15;
-			break;
-		}
-
-		phy_c9 = read_phy_reg(pi, 0x93d);
-		phy_c9 = 2 * phy_c9;
-		phy_c24 = 0;
-		phy_c5 = 7;
-		phy_c25 = 1;
-		while (1) {
-			write_radio_reg(pi, RADIO_2064_REG026,
-					(phy_c5 & 0x7) | ((phy_c5 & 0x7) << 4));
-			udelay(50);
-			phy_c22 = 0;
-			ptr[130] = 0;
-			wlc_lcnphy_samp_cap(pi, 1, phy_c9, &ptr[0], 2);
-			if (ptr[130] == 1)
-				phy_c22 = 1;
-			if (phy_c22)
-				phy_c5 -= 1;
-			if ((phy_c22 != phy_c24) && (!phy_c25))
-				break;
-			if (!phy_c22)
-				phy_c5 += 1;
-			if (phy_c5 <= 0 || phy_c5 >= 7)
-				break;
-			phy_c24 = phy_c22;
-			phy_c25 = 0;
-		}
-
-		if (phy_c5 < 0)
-			phy_c5 = 0;
-		else if (phy_c5 > 7)
-			phy_c5 = 7;
-
-		for (k = -phy_c7; k <= phy_c7; k += phy_c7) {
-			for (l = -phy_c7; l <= phy_c7; l += phy_c7) {
-				phy_c11 = phy_c15 + k;
-				phy_c12 = phy_c16 + l;
-
-				if (phy_c11 < -phy_c10)
-					phy_c11 = -phy_c10;
-				else if (phy_c11 > phy_c10)
-					phy_c11 = phy_c10;
-				if (phy_c12 < -phy_c10)
-					phy_c12 = -phy_c10;
-				else if (phy_c12 > phy_c10)
-					phy_c12 = phy_c10;
-				wlc_lcnphy_set_cc(pi, cal_type, phy_c11,
-						  phy_c12);
-				udelay(20);
-				wlc_lcnphy_samp_cap(pi, 0, 0, ptr, 2);
-
-				phy_c18 = 0;
-				phy_c19 = 0;
-				for (j = 0; j < 128; j++) {
-					if (cal_type != 0) {
-						phy_c6 = j % phy_c4;
-					} else {
-						phy_c6 = (2 * j) % phy_c4;
-					}
-					phy_c2.re = phy_c1[phy_c6].re;
-					phy_c2.im = phy_c1[phy_c6].im;
-					phy_c17 = ptr[j];
-					phy_c18 = phy_c18 + phy_c17 * phy_c2.re;
-					phy_c19 = phy_c19 + phy_c17 * phy_c2.im;
-				}
-
-				phy_c18 = phy_c18 >> 10;
-				phy_c19 = phy_c19 >> 10;
-				phy_c20 =
-				    ((phy_c18 * phy_c18) + (phy_c19 * phy_c19));
-
-				if (phy_c23 || phy_c20 < phy_c21) {
-					phy_c21 = phy_c20;
-					phy_c13 = phy_c11;
-					phy_c14 = phy_c12;
-				}
-				phy_c23 = 0;
-			}
-		}
-		phy_c23 = 1;
-		phy_c15 = phy_c13;
-		phy_c16 = phy_c14;
-		phy_c7 = phy_c7 >> 1;
-		wlc_lcnphy_set_cc(pi, cal_type, phy_c15, phy_c16);
-		udelay(20);
-	}
-	goto cleanup;
- cleanup:
-	wlc_lcnphy_tx_iqlo_loopback_cleanup(pi, phy_c32);
-	wlc_lcnphy_stop_tx_tone(pi);
-	write_phy_reg(pi, 0x6da, phy_c26);
-	write_phy_reg(pi, 0x6db, phy_c27);
-	write_phy_reg(pi, 0x938, phy_c28);
-	write_phy_reg(pi, 0x4d7, phy_c29);
-	write_phy_reg(pi, 0x4d8, phy_c30);
-	write_radio_reg(pi, RADIO_2064_REG026, phy_c31);
-
-	kfree(phy_c32);
-	kfree(ptr);
-}
-
-static void
-wlc_lcnphy_tx_iqlo_loopback_cleanup(struct brcms_phy *pi, u16 *values_to_save)
-{
-	int i;
-
-	and_phy_reg(pi, 0x44c, 0x0 >> 11);
-
-	and_phy_reg(pi, 0x43b, 0xC);
-
-	for (i = 0; i < 20; i++) {
-		write_radio_reg(pi, iqlo_loopback_rf_regs[i],
-				values_to_save[i]);
+		pi_lcn->lcnphy_current_index =
+			(s8)((read_phy_reg(pi, 0x4a9) & 0xFF) / 2);
 	}
 }
 
 static void
 wlc_lcnphy_load_tx_gain_table(struct brcms_phy *pi,
-			const struct lcnphy_tx_gain_tbl_entry *gain_table) {
+			      const struct lcnphy_tx_gain_tbl_entry *gain_table)
+{
 	u32 j;
 	struct phytbl_info tab;
 	u32 val;
@@ -4297,6 +4386,172 @@ static void wlc_lcnphy_load_rfpower(struct brcms_phy *pi)
 	}
 }
 
+static void wlc_lcnphy_bu_tweaks(struct brcms_phy *pi)
+{
+	or_phy_reg(pi, 0x805, 0x1);
+
+	mod_phy_reg(pi, 0x42f, (0x7 << 0), (0x3) << 0);
+
+	mod_phy_reg(pi, 0x030, (0x7 << 0), (0x3) << 0);
+
+	write_phy_reg(pi, 0x414, 0x1e10);
+	write_phy_reg(pi, 0x415, 0x0640);
+
+	mod_phy_reg(pi, 0x4df, (0xff << 8), -9 << 8);
+
+	or_phy_reg(pi, 0x44a, 0x44);
+	write_phy_reg(pi, 0x44a, 0x80);
+	mod_phy_reg(pi, 0x434, (0xff << 0), (0xFD) << 0);
+
+	mod_phy_reg(pi, 0x420, (0xff << 0), (16) << 0);
+
+	if (!(pi->sh->boardrev < 0x1204))
+		mod_radio_reg(pi, RADIO_2064_REG09B, 0xF0, 0xF0);
+
+	write_phy_reg(pi, 0x7d6, 0x0902);
+	mod_phy_reg(pi, 0x429, (0xf << 0), (0x9) << 0);
+
+	mod_phy_reg(pi, 0x429, (0x3f << 4), (0xe) << 4);
+
+	if (LCNREV_IS(pi->pubpi.phy_rev, 1)) {
+		mod_phy_reg(pi, 0x423, (0xff << 0), (0x46) << 0);
+
+		mod_phy_reg(pi, 0x411, (0xff << 0), (1) << 0);
+
+		mod_phy_reg(pi, 0x434, (0xff << 0), (0xFF) << 0);
+
+		mod_phy_reg(pi, 0x656, (0xf << 0), (2) << 0);
+
+		mod_phy_reg(pi, 0x44d, (0x1 << 2), (1) << 2);
+
+		mod_radio_reg(pi, RADIO_2064_REG0F7, 0x4, 0x4);
+		mod_radio_reg(pi, RADIO_2064_REG0F1, 0x3, 0);
+		mod_radio_reg(pi, RADIO_2064_REG0F2, 0xF8, 0x90);
+		mod_radio_reg(pi, RADIO_2064_REG0F3, 0x3, 0x2);
+		mod_radio_reg(pi, RADIO_2064_REG0F3, 0xf0, 0xa0);
+
+		mod_radio_reg(pi, RADIO_2064_REG11F, 0x2, 0x2);
+
+		wlc_lcnphy_clear_tx_power_offsets(pi);
+		mod_phy_reg(pi, 0x4d0, (0x1ff << 6), (10) << 6);
+
+	}
+}
+
+static void wlc_lcnphy_rcal(struct brcms_phy *pi)
+{
+	u8 rcal_value;
+
+	and_radio_reg(pi, RADIO_2064_REG05B, 0xfD);
+
+	or_radio_reg(pi, RADIO_2064_REG004, 0x40);
+	or_radio_reg(pi, RADIO_2064_REG120, 0x10);
+
+	or_radio_reg(pi, RADIO_2064_REG078, 0x80);
+	or_radio_reg(pi, RADIO_2064_REG129, 0x02);
+
+	or_radio_reg(pi, RADIO_2064_REG057, 0x01);
+
+	or_radio_reg(pi, RADIO_2064_REG05B, 0x02);
+	mdelay(5);
+	SPINWAIT(!wlc_radio_2064_rcal_done(pi), 10 * 1000 * 1000);
+
+	if (wlc_radio_2064_rcal_done(pi)) {
+		rcal_value = (u8) read_radio_reg(pi, RADIO_2064_REG05C);
+		rcal_value = rcal_value & 0x1f;
+	}
+
+	and_radio_reg(pi, RADIO_2064_REG05B, 0xfD);
+
+	and_radio_reg(pi, RADIO_2064_REG057, 0xFE);
+}
+
+static void wlc_lcnphy_rc_cal(struct brcms_phy *pi)
+{
+	u8 dflt_rc_cal_val;
+	u16 flt_val;
+
+	dflt_rc_cal_val = 7;
+	if (LCNREV_IS(pi->pubpi.phy_rev, 1))
+		dflt_rc_cal_val = 11;
+	flt_val =
+		(dflt_rc_cal_val << 10) | (dflt_rc_cal_val << 5) |
+		(dflt_rc_cal_val);
+	write_phy_reg(pi, 0x933, flt_val);
+	write_phy_reg(pi, 0x934, flt_val);
+	write_phy_reg(pi, 0x935, flt_val);
+	write_phy_reg(pi, 0x936, flt_val);
+	write_phy_reg(pi, 0x937, (flt_val & 0x1FF));
+
+	return;
+}
+
+static void wlc_radio_2064_init(struct brcms_phy *pi)
+{
+	u32 i;
+	const struct lcnphy_radio_regs *lcnphyregs = NULL;
+
+	lcnphyregs = lcnphy_radio_regs_2064;
+
+	for (i = 0; lcnphyregs[i].address != 0xffff; i++)
+		if (CHSPEC_IS5G(pi->radio_chanspec) && lcnphyregs[i].do_init_a)
+			write_radio_reg(pi,
+					((lcnphyregs[i].address & 0x3fff) |
+					 RADIO_DEFAULT_CORE),
+					(u16) lcnphyregs[i].init_a);
+		else if (lcnphyregs[i].do_init_g)
+			write_radio_reg(pi,
+					((lcnphyregs[i].address & 0x3fff) |
+					 RADIO_DEFAULT_CORE),
+					(u16) lcnphyregs[i].init_g);
+
+	write_radio_reg(pi, RADIO_2064_REG032, 0x62);
+	write_radio_reg(pi, RADIO_2064_REG033, 0x19);
+
+	write_radio_reg(pi, RADIO_2064_REG090, 0x10);
+
+	write_radio_reg(pi, RADIO_2064_REG010, 0x00);
+
+	if (LCNREV_IS(pi->pubpi.phy_rev, 1)) {
+
+		write_radio_reg(pi, RADIO_2064_REG060, 0x7f);
+		write_radio_reg(pi, RADIO_2064_REG061, 0x72);
+		write_radio_reg(pi, RADIO_2064_REG062, 0x7f);
+	}
+
+	write_radio_reg(pi, RADIO_2064_REG01D, 0x02);
+	write_radio_reg(pi, RADIO_2064_REG01E, 0x06);
+
+	mod_phy_reg(pi, 0x4ea, (0x7 << 0), 0 << 0);
+
+	mod_phy_reg(pi, 0x4ea, (0x7 << 3), 1 << 3);
+
+	mod_phy_reg(pi, 0x4ea, (0x7 << 6), 2 << 6);
+
+	mod_phy_reg(pi, 0x4ea, (0x7 << 9), 3 << 9);
+
+	mod_phy_reg(pi, 0x4ea, (0x7 << 12), 4 << 12);
+
+	write_phy_reg(pi, 0x4ea, 0x4688);
+
+	mod_phy_reg(pi, 0x4eb, (0x7 << 0), 2 << 0);
+
+	mod_phy_reg(pi, 0x4eb, (0x7 << 6), 0 << 6);
+
+	mod_phy_reg(pi, 0x46a, (0xffff << 0), 25 << 0);
+
+	wlc_lcnphy_set_tx_locc(pi, 0);
+
+	wlc_lcnphy_rcal(pi);
+
+	wlc_lcnphy_rc_cal(pi);
+}
+
+static void wlc_lcnphy_radio_init(struct brcms_phy *pi)
+{
+	wlc_radio_2064_init(pi);
+}
+
 static void wlc_lcnphy_tbl_init(struct brcms_phy *pi)
 {
 	uint idx;
@@ -4306,9 +4561,8 @@ static void wlc_lcnphy_tbl_init(struct brcms_phy *pi)
 
 	phybw40 = CHSPEC_IS40(pi->radio_chanspec);
 
-	for (idx = 0; idx < dot11lcnphytbl_info_sz_rev0; idx++) {
+	for (idx = 0; idx < dot11lcnphytbl_info_sz_rev0; idx++)
 		wlc_lcnphy_write_table(pi, &dot11lcnphytbl_info_rev0[idx]);
-	}
 
 	if (pi->sh->boardflags & BFL_FEM_BT) {
 		tab.tbl_id = LCNPHY_TBL_ID_RFSEQ;
@@ -4339,39 +4593,35 @@ static void wlc_lcnphy_tbl_init(struct brcms_phy *pi)
 
 	if (CHSPEC_IS2G(pi->radio_chanspec)) {
 		if (pi->sh->boardflags & BFL_FEM)
-			wlc_lcnphy_load_tx_gain_table(pi,
-						      dot11lcnphy_2GHz_extPA_gaintable_rev0);
+			wlc_lcnphy_load_tx_gain_table(
+				pi,
+				dot11lcnphy_2GHz_extPA_gaintable_rev0);
 		else
-			wlc_lcnphy_load_tx_gain_table(pi,
-						      dot11lcnphy_2GHz_gaintable_rev0);
+			wlc_lcnphy_load_tx_gain_table(
+				pi,
+				dot11lcnphy_2GHz_gaintable_rev0);
 	}
 
 	if (LCNREV_IS(pi->pubpi.phy_rev, 2)) {
+		const struct phytbl_info *tb;
+		int l;
+
 		if (CHSPEC_IS2G(pi->radio_chanspec)) {
-			for (idx = 0;
-			     idx < dot11lcnphytbl_rx_gain_info_2G_rev2_sz;
-			     idx++)
-				if (pi->sh->boardflags & BFL_EXTLNA)
-					wlc_lcnphy_write_table(pi,
-							       &dot11lcnphytbl_rx_gain_info_extlna_2G_rev2
-							       [idx]);
-				else
-					wlc_lcnphy_write_table(pi,
-							       &dot11lcnphytbl_rx_gain_info_2G_rev2
-							       [idx]);
+			l = dot11lcnphytbl_rx_gain_info_2G_rev2_sz;
+			if (pi->sh->boardflags & BFL_EXTLNA)
+				tb = dot11lcnphytbl_rx_gain_info_extlna_2G_rev2;
+			else
+				tb = dot11lcnphytbl_rx_gain_info_2G_rev2;
 		} else {
-			for (idx = 0;
-			     idx < dot11lcnphytbl_rx_gain_info_5G_rev2_sz;
-			     idx++)
-				if (pi->sh->boardflags & BFL_EXTLNA_5GHz)
-					wlc_lcnphy_write_table(pi,
-							       &dot11lcnphytbl_rx_gain_info_extlna_5G_rev2
-							       [idx]);
-				else
-					wlc_lcnphy_write_table(pi,
-							       &dot11lcnphytbl_rx_gain_info_5G_rev2
-							       [idx]);
+			l = dot11lcnphytbl_rx_gain_info_5G_rev2_sz;
+			if (pi->sh->boardflags & BFL_EXTLNA_5GHz)
+				tb = dot11lcnphytbl_rx_gain_info_extlna_5G_rev2;
+			else
+				tb = dot11lcnphytbl_rx_gain_info_5G_rev2;
 		}
+
+		for (idx = 0; idx < l; idx++)
+			wlc_lcnphy_write_table(pi, &tb[idx]);
 	}
 
 	if ((pi->sh->boardflags & BFL_FEM)
@@ -4379,11 +4629,13 @@ static void wlc_lcnphy_tbl_init(struct brcms_phy *pi)
 		wlc_lcnphy_write_table(pi, &dot11lcn_sw_ctrl_tbl_info_4313_epa);
 	else if (pi->sh->boardflags & BFL_FEM_BT) {
 		if (pi->sh->boardrev < 0x1250)
-			wlc_lcnphy_write_table(pi,
-					       &dot11lcn_sw_ctrl_tbl_info_4313_bt_epa);
+			wlc_lcnphy_write_table(
+				pi,
+				&dot11lcn_sw_ctrl_tbl_info_4313_bt_epa);
 		else
-			wlc_lcnphy_write_table(pi,
-					       &dot11lcn_sw_ctrl_tbl_info_4313_bt_epa_p250);
+			wlc_lcnphy_write_table(
+				pi,
+				&dot11lcn_sw_ctrl_tbl_info_4313_bt_epa_p250);
 	} else
 		wlc_lcnphy_write_table(pi, &dot11lcn_sw_ctrl_tbl_info_4313);
 
@@ -4420,9 +4672,8 @@ static void wlc_lcnphy_rev0_baseband_init(struct brcms_phy *pi)
 	if (0) {
 		afectrl1 = 0;
 		afectrl1 = (u16) ((pi_lcn->lcnphy_rssi_vf) |
-				     (pi_lcn->lcnphy_rssi_vc << 4) | (pi_lcn->
-								      lcnphy_rssi_gs
-								      << 10));
+				  (pi_lcn->lcnphy_rssi_vc << 4) |
+				  (pi_lcn->lcnphy_rssi_gs << 10));
 		write_phy_reg(pi, 0x43e, afectrl1);
 	}
 
@@ -4443,7 +4694,6 @@ static void wlc_lcnphy_rev2_baseband_init(struct brcms_phy *pi)
 {
 	if (CHSPEC_IS5G(pi->radio_chanspec)) {
 		mod_phy_reg(pi, 0x416, (0xff << 0), 80 << 0);
-
 		mod_phy_reg(pi, 0x416, (0xff << 8), 80 << 8);
 	}
 }
@@ -4455,9 +4705,6 @@ static void wlc_lcnphy_agc_temp_init(struct brcms_phy *pi)
 	u32 tableBuffer[2];
 	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
 
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
-
 	temp = (s16) read_phy_reg(pi, 0x4df);
 	pi_lcn->lcnphy_ofdmgainidxtableoffset = (temp & (0xff << 0)) >> 0;
 
@@ -4484,18 +4731,15 @@ static void wlc_lcnphy_agc_temp_init(struct brcms_phy *pi)
 		tableBuffer[1] -= 128;
 	pi_lcn->lcnphy_tr_T_gain_val = tableBuffer[1];
 
-	temp = (s16) (read_phy_reg(pi, 0x434)
-			& (0xff << 0));
+	temp = (s16) (read_phy_reg(pi, 0x434) & (0xff << 0));
 	if (temp > 127)
 		temp -= 256;
 	pi_lcn->lcnphy_input_pwr_offset_db = (s8) temp;
 
-	pi_lcn->lcnphy_Med_Low_Gain_db = (read_phy_reg(pi, 0x424)
-					  & (0xff << 8))
-	    >> 8;
-	pi_lcn->lcnphy_Very_Low_Gain_db = (read_phy_reg(pi, 0x425)
-					   & (0xff << 0))
-	    >> 0;
+	pi_lcn->lcnphy_Med_Low_Gain_db =
+		(read_phy_reg(pi, 0x424) & (0xff << 8)) >> 8;
+	pi_lcn->lcnphy_Very_Low_Gain_db =
+		(read_phy_reg(pi, 0x425) & (0xff << 0)) >> 0;
 
 	tab.tbl_ptr = tableBuffer;
 	tab.tbl_len = 2;
@@ -4509,61 +4753,6 @@ static void wlc_lcnphy_agc_temp_init(struct brcms_phy *pi)
 
 }
 
-static void wlc_lcnphy_bu_tweaks(struct brcms_phy *pi)
-{
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
-
-	or_phy_reg(pi, 0x805, 0x1);
-
-	mod_phy_reg(pi, 0x42f, (0x7 << 0), (0x3) << 0);
-
-	mod_phy_reg(pi, 0x030, (0x7 << 0), (0x3) << 0);
-
-	write_phy_reg(pi, 0x414, 0x1e10);
-	write_phy_reg(pi, 0x415, 0x0640);
-
-	mod_phy_reg(pi, 0x4df, (0xff << 8), -9 << 8);
-
-	or_phy_reg(pi, 0x44a, 0x44);
-	write_phy_reg(pi, 0x44a, 0x80);
-	mod_phy_reg(pi, 0x434, (0xff << 0), (0xFD) << 0);
-
-	mod_phy_reg(pi, 0x420, (0xff << 0), (16) << 0);
-
-	if (!(pi->sh->boardrev < 0x1204))
-		mod_radio_reg(pi, RADIO_2064_REG09B, 0xF0, 0xF0);
-
-	write_phy_reg(pi, 0x7d6, 0x0902);
-	mod_phy_reg(pi, 0x429, (0xf << 0), (0x9) << 0);
-
-	mod_phy_reg(pi, 0x429, (0x3f << 4), (0xe) << 4);
-
-	if (LCNREV_IS(pi->pubpi.phy_rev, 1)) {
-		mod_phy_reg(pi, 0x423, (0xff << 0), (0x46) << 0);
-
-		mod_phy_reg(pi, 0x411, (0xff << 0), (1) << 0);
-
-		mod_phy_reg(pi, 0x434, (0xff << 0), (0xFF) << 0);
-
-		mod_phy_reg(pi, 0x656, (0xf << 0), (2) << 0);
-
-		mod_phy_reg(pi, 0x44d, (0x1 << 2), (1) << 2);
-
-		mod_radio_reg(pi, RADIO_2064_REG0F7, 0x4, 0x4);
-		mod_radio_reg(pi, RADIO_2064_REG0F1, 0x3, 0);
-		mod_radio_reg(pi, RADIO_2064_REG0F2, 0xF8, 0x90);
-		mod_radio_reg(pi, RADIO_2064_REG0F3, 0x3, 0x2);
-		mod_radio_reg(pi, RADIO_2064_REG0F3, 0xf0, 0xa0);
-
-		mod_radio_reg(pi, RADIO_2064_REG11F, 0x2, 0x2);
-
-		wlc_lcnphy_clear_tx_power_offsets(pi);
-		mod_phy_reg(pi, 0x4d0, (0x1ff << 6), (10) << 6);
-
-	}
-}
-
 static void wlc_lcnphy_baseband_init(struct brcms_phy *pi)
 {
 
@@ -4574,127 +4763,53 @@ static void wlc_lcnphy_baseband_init(struct brcms_phy *pi)
 	wlc_lcnphy_bu_tweaks(pi);
 }
 
-static void wlc_radio_2064_init(struct brcms_phy *pi)
+void wlc_phy_init_lcnphy(struct brcms_phy *pi)
 {
-	u32 i;
-	struct lcnphy_radio_regs *lcnphyregs = NULL;
-
-	lcnphyregs = lcnphy_radio_regs_2064;
-
-	for (i = 0; lcnphyregs[i].address != 0xffff; i++)
-		if (CHSPEC_IS5G(pi->radio_chanspec) && lcnphyregs[i].do_init_a)
-			write_radio_reg(pi,
-					((lcnphyregs[i].address & 0x3fff) |
-					 RADIO_DEFAULT_CORE),
-					(u16) lcnphyregs[i].init_a);
-		else if (lcnphyregs[i].do_init_g)
-			write_radio_reg(pi,
-					((lcnphyregs[i].address & 0x3fff) |
-					 RADIO_DEFAULT_CORE),
-					(u16) lcnphyregs[i].init_g);
-
-	write_radio_reg(pi, RADIO_2064_REG032, 0x62);
-	write_radio_reg(pi, RADIO_2064_REG033, 0x19);
-
-	write_radio_reg(pi, RADIO_2064_REG090, 0x10);
-
-	write_radio_reg(pi, RADIO_2064_REG010, 0x00);
-
-	if (LCNREV_IS(pi->pubpi.phy_rev, 1)) {
-
-		write_radio_reg(pi, RADIO_2064_REG060, 0x7f);
-		write_radio_reg(pi, RADIO_2064_REG061, 0x72);
-		write_radio_reg(pi, RADIO_2064_REG062, 0x7f);
-	}
-
-	write_radio_reg(pi, RADIO_2064_REG01D, 0x02);
-	write_radio_reg(pi, RADIO_2064_REG01E, 0x06);
-
-	mod_phy_reg(pi, 0x4ea, (0x7 << 0), 0 << 0);
-
-	mod_phy_reg(pi, 0x4ea, (0x7 << 3), 1 << 3);
-
-	mod_phy_reg(pi, 0x4ea, (0x7 << 6), 2 << 6);
-
-	mod_phy_reg(pi, 0x4ea, (0x7 << 9), 3 << 9);
-
-	mod_phy_reg(pi, 0x4ea, (0x7 << 12), 4 << 12);
-
-	write_phy_reg(pi, 0x4ea, 0x4688);
-
-	mod_phy_reg(pi, 0x4eb, (0x7 << 0), 2 << 0);
-
-	mod_phy_reg(pi, 0x4eb, (0x7 << 6), 0 << 6);
-
-	mod_phy_reg(pi, 0x46a, (0xffff << 0), 25 << 0);
-
-	wlc_lcnphy_set_tx_locc(pi, 0);
-
-	wlc_lcnphy_rcal(pi);
+	u8 phybw40;
+	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
+	phybw40 = CHSPEC_IS40(pi->radio_chanspec);
 
-	wlc_lcnphy_rc_cal(pi);
-}
+	pi_lcn->lcnphy_cal_counter = 0;
+	pi_lcn->lcnphy_cal_temper = pi_lcn->lcnphy_rawtempsense;
 
-static void wlc_lcnphy_radio_init(struct brcms_phy *pi)
-{
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
+	or_phy_reg(pi, 0x44a, 0x80);
+	and_phy_reg(pi, 0x44a, 0x7f);
 
-	wlc_radio_2064_init(pi);
-}
+	wlc_lcnphy_afe_clk_init(pi, AFE_CLK_INIT_MODE_TXRX2X);
 
-static void wlc_lcnphy_rcal(struct brcms_phy *pi)
-{
-	u8 rcal_value;
+	write_phy_reg(pi, 0x60a, 160);
 
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
+	write_phy_reg(pi, 0x46a, 25);
 
-	and_radio_reg(pi, RADIO_2064_REG05B, 0xfD);
+	wlc_lcnphy_baseband_init(pi);
 
-	or_radio_reg(pi, RADIO_2064_REG004, 0x40);
-	or_radio_reg(pi, RADIO_2064_REG120, 0x10);
+	wlc_lcnphy_radio_init(pi);
 
-	or_radio_reg(pi, RADIO_2064_REG078, 0x80);
-	or_radio_reg(pi, RADIO_2064_REG129, 0x02);
+	if (CHSPEC_IS2G(pi->radio_chanspec))
+		wlc_lcnphy_tx_pwr_ctrl_init((struct brcms_phy_pub *) pi);
 
-	or_radio_reg(pi, RADIO_2064_REG057, 0x01);
+	wlc_phy_chanspec_set((struct brcms_phy_pub *) pi, pi->radio_chanspec);
 
-	or_radio_reg(pi, RADIO_2064_REG05B, 0x02);
-	mdelay(5);
-	SPINWAIT(!wlc_radio_2064_rcal_done(pi), 10 * 1000 * 1000);
+	si_pmu_regcontrol(pi->sh->sih, 0, 0xf, 0x9);
 
-	if (wlc_radio_2064_rcal_done(pi)) {
-		rcal_value = (u8) read_radio_reg(pi, RADIO_2064_REG05C);
-		rcal_value = rcal_value & 0x1f;
-	}
+	si_pmu_chipcontrol(pi->sh->sih, 0, 0xffffffff, 0x03CDDDDD);
 
-	and_radio_reg(pi, RADIO_2064_REG05B, 0xfD);
+	if ((pi->sh->boardflags & BFL_FEM)
+	    && wlc_lcnphy_tempsense_based_pwr_ctrl_enabled(pi))
+		wlc_lcnphy_set_tx_pwr_by_index(pi, FIXED_TXPWR);
 
-	and_radio_reg(pi, RADIO_2064_REG057, 0xFE);
-}
+	wlc_lcnphy_agc_temp_init(pi);
 
-static void wlc_lcnphy_rc_cal(struct brcms_phy *pi)
-{
-	u8 dflt_rc_cal_val;
-	u16 flt_val;
+	wlc_lcnphy_temp_adj(pi);
 
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
+	mod_phy_reg(pi, 0x448, (0x1 << 14), (1) << 14);
 
-	dflt_rc_cal_val = 7;
-	if (LCNREV_IS(pi->pubpi.phy_rev, 1))
-		dflt_rc_cal_val = 11;
-	flt_val =
-	    (dflt_rc_cal_val << 10) | (dflt_rc_cal_val << 5) |
-	    (dflt_rc_cal_val);
-	write_phy_reg(pi, 0x933, flt_val);
-	write_phy_reg(pi, 0x934, flt_val);
-	write_phy_reg(pi, 0x935, flt_val);
-	write_phy_reg(pi, 0x936, flt_val);
-	write_phy_reg(pi, 0x937, (flt_val & 0x1FF));
+	udelay(100);
+	mod_phy_reg(pi, 0x448, (0x1 << 14), (0) << 14);
 
-	return;
+	wlc_lcnphy_set_tx_pwr_ctrl(pi, LCNPHY_TX_PWR_CTRL_HW);
+	pi_lcn->lcnphy_noise_samples = LCNPHY_NOISE_SAMPLES_DEFAULT;
+	wlc_lcnphy_calib_modes(pi, PHY_PERICAL_PHYINIT);
 }
 
 static bool wlc_phy_txpwr_srom_read_lcnphy(struct brcms_phy *pi)
@@ -4702,39 +4817,38 @@ static bool wlc_phy_txpwr_srom_read_lcnphy(struct brcms_phy *pi)
 	s8 txpwr = 0;
 	int i;
 	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
+	struct phy_shim_info *shim = pi->sh->physhim;
 
 	if (CHSPEC_IS2G(pi->radio_chanspec)) {
 		u16 cckpo = 0;
 		u32 offset_ofdm, offset_mcs;
 
 		pi_lcn->lcnphy_tr_isolation_mid =
-		    (u8) PHY_GETINTVAR(pi, "triso2g");
+			(u8)wlapi_getintvar(shim, BRCMS_SROM_TRISO2G);
 
 		pi_lcn->lcnphy_rx_power_offset =
-		    (u8) PHY_GETINTVAR(pi, "rxpo2g");
-
-		pi->txpa_2g[0] = (s16) PHY_GETINTVAR(pi, "pa0b0");
-		pi->txpa_2g[1] = (s16) PHY_GETINTVAR(pi, "pa0b1");
-		pi->txpa_2g[2] = (s16) PHY_GETINTVAR(pi, "pa0b2");
-
-		pi_lcn->lcnphy_rssi_vf = (u8) PHY_GETINTVAR(pi, "rssismf2g");
-		pi_lcn->lcnphy_rssi_vc = (u8) PHY_GETINTVAR(pi, "rssismc2g");
-		pi_lcn->lcnphy_rssi_gs = (u8) PHY_GETINTVAR(pi, "rssisav2g");
-
-		{
-			pi_lcn->lcnphy_rssi_vf_lowtemp = pi_lcn->lcnphy_rssi_vf;
-			pi_lcn->lcnphy_rssi_vc_lowtemp = pi_lcn->lcnphy_rssi_vc;
-			pi_lcn->lcnphy_rssi_gs_lowtemp = pi_lcn->lcnphy_rssi_gs;
-
-			pi_lcn->lcnphy_rssi_vf_hightemp =
-			    pi_lcn->lcnphy_rssi_vf;
-			pi_lcn->lcnphy_rssi_vc_hightemp =
-			    pi_lcn->lcnphy_rssi_vc;
-			pi_lcn->lcnphy_rssi_gs_hightemp =
-			    pi_lcn->lcnphy_rssi_gs;
-		}
+			(u8)wlapi_getintvar(shim, BRCMS_SROM_RXPO2G);
+
+		pi->txpa_2g[0] = (s16)wlapi_getintvar(shim, BRCMS_SROM_PA0B0);
+		pi->txpa_2g[1] = (s16)wlapi_getintvar(shim, BRCMS_SROM_PA0B1);
+		pi->txpa_2g[2] = (s16)wlapi_getintvar(shim, BRCMS_SROM_PA0B2);
 
-		txpwr = (s8) PHY_GETINTVAR(pi, "maxp2ga0");
+		pi_lcn->lcnphy_rssi_vf =
+				(u8)wlapi_getintvar(shim, BRCMS_SROM_RSSISMF2G);
+		pi_lcn->lcnphy_rssi_vc =
+				(u8)wlapi_getintvar(shim, BRCMS_SROM_RSSISMC2G);
+		pi_lcn->lcnphy_rssi_gs =
+				(u8)wlapi_getintvar(shim, BRCMS_SROM_RSSISAV2G);
+
+		pi_lcn->lcnphy_rssi_vf_lowtemp = pi_lcn->lcnphy_rssi_vf;
+		pi_lcn->lcnphy_rssi_vc_lowtemp = pi_lcn->lcnphy_rssi_vc;
+		pi_lcn->lcnphy_rssi_gs_lowtemp = pi_lcn->lcnphy_rssi_gs;
+
+		pi_lcn->lcnphy_rssi_vf_hightemp = pi_lcn->lcnphy_rssi_vf;
+		pi_lcn->lcnphy_rssi_vc_hightemp = pi_lcn->lcnphy_rssi_vc;
+		pi_lcn->lcnphy_rssi_gs_hightemp = pi_lcn->lcnphy_rssi_gs;
+
+		txpwr = (s8)wlapi_getintvar(shim, BRCMS_SROM_MAXP2GA0);
 		pi->tx_srom_max_2g = txpwr;
 
 		for (i = 0; i < PWRTBL_NUM_COEFF; i++) {
@@ -4742,79 +4856,72 @@ static bool wlc_phy_txpwr_srom_read_lcnphy(struct brcms_phy *pi)
 			pi->txpa_2g_high_temp[i] = pi->txpa_2g[i];
 		}
 
-		cckpo = (u16) PHY_GETINTVAR(pi, "cck2gpo");
+		cckpo = (u16)wlapi_getintvar(shim, BRCMS_SROM_CCK2GPO);
+		offset_ofdm = (u32)wlapi_getintvar(shim, BRCMS_SROM_OFDM2GPO);
 		if (cckpo) {
 			uint max_pwr_chan = txpwr;
 
 			for (i = TXP_FIRST_CCK; i <= TXP_LAST_CCK; i++) {
-				pi->tx_srom_max_rate_2g[i] = max_pwr_chan -
-				    ((cckpo & 0xf) * 2);
+				pi->tx_srom_max_rate_2g[i] =
+					max_pwr_chan - ((cckpo & 0xf) * 2);
 				cckpo >>= 4;
 			}
 
-			offset_ofdm = (u32) PHY_GETINTVAR(pi, "ofdm2gpo");
 			for (i = TXP_FIRST_OFDM; i <= TXP_LAST_OFDM; i++) {
-				pi->tx_srom_max_rate_2g[i] = max_pwr_chan -
-				    ((offset_ofdm & 0xf) * 2);
+				pi->tx_srom_max_rate_2g[i] =
+					max_pwr_chan -
+					((offset_ofdm & 0xf) * 2);
 				offset_ofdm >>= 4;
 			}
 		} else {
 			u8 opo = 0;
 
-			opo = (u8) PHY_GETINTVAR(pi, "opo");
+			opo = (u8)wlapi_getintvar(shim, BRCMS_SROM_OPO);
 
-			for (i = TXP_FIRST_CCK; i <= TXP_LAST_CCK; i++) {
+			for (i = TXP_FIRST_CCK; i <= TXP_LAST_CCK; i++)
 				pi->tx_srom_max_rate_2g[i] = txpwr;
-			}
-
-			offset_ofdm = (u32) PHY_GETINTVAR(pi, "ofdm2gpo");
 
 			for (i = TXP_FIRST_OFDM; i <= TXP_LAST_OFDM; i++) {
 				pi->tx_srom_max_rate_2g[i] = txpwr -
-				    ((offset_ofdm & 0xf) * 2);
+						((offset_ofdm & 0xf) * 2);
 				offset_ofdm >>= 4;
 			}
 			offset_mcs =
-			    ((u16) PHY_GETINTVAR(pi, "mcs2gpo1") << 16) |
-			    (u16) PHY_GETINTVAR(pi, "mcs2gpo0");
+				wlapi_getintvar(shim,
+						BRCMS_SROM_MCS2GPO1) << 16;
+			offset_mcs |=
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS2GPO0);
 			pi_lcn->lcnphy_mcs20_po = offset_mcs;
 			for (i = TXP_FIRST_SISO_MCS_20;
 			     i <= TXP_LAST_SISO_MCS_20; i++) {
 				pi->tx_srom_max_rate_2g[i] =
-				    txpwr - ((offset_mcs & 0xf) * 2);
+					txpwr - ((offset_mcs & 0xf) * 2);
 				offset_mcs >>= 4;
 			}
 		}
 
 		pi_lcn->lcnphy_rawtempsense =
-		    (u16) PHY_GETINTVAR(pi, "rawtempsense");
+			(u16)wlapi_getintvar(shim, BRCMS_SROM_RAWTEMPSENSE);
 		pi_lcn->lcnphy_measPower =
-		    (u8) PHY_GETINTVAR(pi, "measpower");
+			(u8)wlapi_getintvar(shim, BRCMS_SROM_MEASPOWER);
 		pi_lcn->lcnphy_tempsense_slope =
-		    (u8) PHY_GETINTVAR(pi, "tempsense_slope");
+			(u8)wlapi_getintvar(shim, BRCMS_SROM_TEMPSENSE_SLOPE);
 		pi_lcn->lcnphy_hw_iqcal_en =
-		    (bool) PHY_GETINTVAR(pi, "hw_iqcal_en");
+			(bool)wlapi_getintvar(shim, BRCMS_SROM_HW_IQCAL_EN);
 		pi_lcn->lcnphy_iqcal_swp_dis =
-		    (bool) PHY_GETINTVAR(pi, "iqcal_swp_dis");
+			(bool)wlapi_getintvar(shim, BRCMS_SROM_IQCAL_SWP_DIS);
 		pi_lcn->lcnphy_tempcorrx =
-		    (u8) PHY_GETINTVAR(pi, "tempcorrx");
+			(u8)wlapi_getintvar(shim, BRCMS_SROM_TEMPCORRX);
 		pi_lcn->lcnphy_tempsense_option =
-		    (u8) PHY_GETINTVAR(pi, "tempsense_option");
+			(u8)wlapi_getintvar(shim, BRCMS_SROM_TEMPSENSE_OPTION);
 		pi_lcn->lcnphy_freqoffset_corr =
-		    (u8) PHY_GETINTVAR(pi, "freqoffset_corr");
-		if ((u8) getintvar(pi->vars, "aa2g") > 1)
+			(u8)wlapi_getintvar(shim, BRCMS_SROM_FREQOFFSET_CORR);
+		if ((u8)wlapi_getintvar(shim, BRCMS_SROM_AA2G) > 1)
 			wlc_phy_ant_rxdiv_set((struct brcms_phy_pub *) pi,
-					      (u8) getintvar(pi->vars,
-								"aa2g"));
+				(u8) wlapi_getintvar(shim, BRCMS_SROM_AA2G));
 	}
 	pi_lcn->lcnphy_cck_dig_filt_type = -1;
-	if (PHY_GETVAR(pi, "cckdigfilttype")) {
-		s16 temp;
-		temp = (s16) PHY_GETINTVAR(pi, "cckdigfilttype");
-		if (temp >= 0) {
-			pi_lcn->lcnphy_cck_dig_filt_type = temp;
-		}
-	}
 
 	return true;
 }
@@ -4834,172 +4941,6 @@ void wlc_2064_vco_cal(struct brcms_phy *pi)
 	mod_radio_reg(pi, RADIO_2064_REG057, 1 << 3, 0);
 }
 
-static void
-wlc_lcnphy_radio_2064_channel_tune_4313(struct brcms_phy *pi, u8 channel)
-{
-	uint i;
-	const struct chan_info_2064_lcnphy *ci;
-	u8 rfpll_doubler = 0;
-	u8 pll_pwrup, pll_pwrup_ovr;
-	fixed qFxtal, qFref, qFvco, qFcal;
-	u8 d15, d16, f16, e44, e45;
-	u32 div_int, div_frac, fvco3, fpfd, fref3, fcal_div;
-	u16 loop_bw, d30, setCount;
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
-	ci = &chan_info_2064_lcnphy[0];
-	rfpll_doubler = 1;
-
-	mod_radio_reg(pi, RADIO_2064_REG09D, 0x4, 0x1 << 2);
-
-	write_radio_reg(pi, RADIO_2064_REG09E, 0xf);
-	if (!rfpll_doubler) {
-		loop_bw = PLL_2064_LOOP_BW;
-		d30 = PLL_2064_D30;
-	} else {
-		loop_bw = PLL_2064_LOOP_BW_DOUBLER;
-		d30 = PLL_2064_D30_DOUBLER;
-	}
-
-	if (CHSPEC_IS2G(pi->radio_chanspec)) {
-		for (i = 0; i < ARRAY_SIZE(chan_info_2064_lcnphy); i++)
-			if (chan_info_2064_lcnphy[i].chan == channel)
-				break;
-
-		if (i >= ARRAY_SIZE(chan_info_2064_lcnphy)) {
-			return;
-		}
-
-		ci = &chan_info_2064_lcnphy[i];
-	}
-
-	write_radio_reg(pi, RADIO_2064_REG02A, ci->logen_buftune);
-
-	mod_radio_reg(pi, RADIO_2064_REG030, 0x3, ci->logen_rccr_tx);
-
-	mod_radio_reg(pi, RADIO_2064_REG091, 0x3, ci->txrf_mix_tune_ctrl);
-
-	mod_radio_reg(pi, RADIO_2064_REG038, 0xf, ci->pa_input_tune_g);
-
-	mod_radio_reg(pi, RADIO_2064_REG030, 0x3 << 2,
-		      (ci->logen_rccr_rx) << 2);
-
-	mod_radio_reg(pi, RADIO_2064_REG05E, 0xf, ci->pa_rxrf_lna1_freq_tune);
-
-	mod_radio_reg(pi, RADIO_2064_REG05E, (0xf) << 4,
-		      (ci->pa_rxrf_lna2_freq_tune) << 4);
-
-	write_radio_reg(pi, RADIO_2064_REG06C, ci->rxrf_rxrf_spare1);
-
-	pll_pwrup = (u8) read_radio_reg(pi, RADIO_2064_REG044);
-	pll_pwrup_ovr = (u8) read_radio_reg(pi, RADIO_2064_REG12B);
-
-	or_radio_reg(pi, RADIO_2064_REG044, 0x07);
-
-	or_radio_reg(pi, RADIO_2064_REG12B, (0x07) << 1);
-	e44 = 0;
-	e45 = 0;
-
-	fpfd = rfpll_doubler ? (pi->xtalfreq << 1) : (pi->xtalfreq);
-	if (pi->xtalfreq > 26000000)
-		e44 = 1;
-	if (pi->xtalfreq > 52000000)
-		e45 = 1;
-	if (e44 == 0)
-		fcal_div = 1;
-	else if (e45 == 0)
-		fcal_div = 2;
-	else
-		fcal_div = 4;
-	fvco3 = (ci->freq * 3);
-	fref3 = 2 * fpfd;
-
-	qFxtal = wlc_lcnphy_qdiv_roundup(pi->xtalfreq, PLL_2064_MHZ, 16);
-	qFref = wlc_lcnphy_qdiv_roundup(fpfd, PLL_2064_MHZ, 16);
-	qFcal = pi->xtalfreq * fcal_div / PLL_2064_MHZ;
-	qFvco = wlc_lcnphy_qdiv_roundup(fvco3, 2, 16);
-
-	write_radio_reg(pi, RADIO_2064_REG04F, 0x02);
-
-	d15 = (pi->xtalfreq * fcal_div * 4 / 5) / PLL_2064_MHZ - 1;
-	write_radio_reg(pi, RADIO_2064_REG052, (0x07 & (d15 >> 2)));
-	write_radio_reg(pi, RADIO_2064_REG053, (d15 & 0x3) << 5);
-
-	d16 = (qFcal * 8 / (d15 + 1)) - 1;
-	write_radio_reg(pi, RADIO_2064_REG051, d16);
-
-	f16 = ((d16 + 1) * (d15 + 1)) / qFcal;
-	setCount = f16 * 3 * (ci->freq) / 32 - 1;
-	mod_radio_reg(pi, RADIO_2064_REG053, (0x0f << 0),
-		      (u8) (setCount >> 8));
-
-	or_radio_reg(pi, RADIO_2064_REG053, 0x10);
-	write_radio_reg(pi, RADIO_2064_REG054, (u8) (setCount & 0xff));
-
-	div_int = ((fvco3 * (PLL_2064_MHZ >> 4)) / fref3) << 4;
-
-	div_frac = ((fvco3 * (PLL_2064_MHZ >> 4)) % fref3) << 4;
-	while (div_frac >= fref3) {
-		div_int++;
-		div_frac -= fref3;
-	}
-	div_frac = wlc_lcnphy_qdiv_roundup(div_frac, fref3, 20);
-
-	mod_radio_reg(pi, RADIO_2064_REG045, (0x1f << 0),
-		      (u8) (div_int >> 4));
-	mod_radio_reg(pi, RADIO_2064_REG046, (0x1f << 4),
-		      (u8) (div_int << 4));
-	mod_radio_reg(pi, RADIO_2064_REG046, (0x0f << 0),
-		      (u8) (div_frac >> 16));
-	write_radio_reg(pi, RADIO_2064_REG047, (u8) (div_frac >> 8) & 0xff);
-	write_radio_reg(pi, RADIO_2064_REG048, (u8) div_frac & 0xff);
-
-	write_radio_reg(pi, RADIO_2064_REG040, 0xfb);
-
-	write_radio_reg(pi, RADIO_2064_REG041, 0x9A);
-	write_radio_reg(pi, RADIO_2064_REG042, 0xA3);
-	write_radio_reg(pi, RADIO_2064_REG043, 0x0C);
-
-	{
-		u8 h29, h23, c28, d29, h28_ten, e30, h30_ten, cp_current;
-		u16 c29, c38, c30, g30, d28;
-		c29 = loop_bw;
-		d29 = 200;
-		c38 = 1250;
-		h29 = d29 / c29;
-		h23 = 1;
-		c28 = 30;
-		d28 = (((PLL_2064_HIGH_END_KVCO - PLL_2064_LOW_END_KVCO) *
-			(fvco3 / 2 - PLL_2064_LOW_END_VCO)) /
-		       (PLL_2064_HIGH_END_VCO - PLL_2064_LOW_END_VCO))
-		    + PLL_2064_LOW_END_KVCO;
-		h28_ten = (d28 * 10) / c28;
-		c30 = 2640;
-		e30 = (d30 - 680) / 490;
-		g30 = 680 + (e30 * 490);
-		h30_ten = (g30 * 10) / c30;
-		cp_current = ((c38 * h29 * h23 * 100) / h28_ten) / h30_ten;
-		mod_radio_reg(pi, RADIO_2064_REG03C, 0x3f, cp_current);
-	}
-	if (channel >= 1 && channel <= 5)
-		write_radio_reg(pi, RADIO_2064_REG03C, 0x8);
-	else
-		write_radio_reg(pi, RADIO_2064_REG03C, 0x7);
-	write_radio_reg(pi, RADIO_2064_REG03D, 0x3);
-
-	mod_radio_reg(pi, RADIO_2064_REG044, 0x0c, 0x0c);
-	udelay(1);
-
-	wlc_2064_vco_cal(pi);
-
-	write_radio_reg(pi, RADIO_2064_REG044, pll_pwrup);
-	write_radio_reg(pi, RADIO_2064_REG12B, pll_pwrup_ovr);
-	if (LCNREV_IS(pi->pubpi.phy_rev, 1)) {
-		write_radio_reg(pi, RADIO_2064_REG038, 3);
-		write_radio_reg(pi, RADIO_2064_REG091, 7);
-	}
-}
-
 bool wlc_phy_tpc_isenabled_lcnphy(struct brcms_phy *pi)
 {
 	if (wlc_lcnphy_tempsense_based_pwr_ctrl_enabled(pi))
@@ -5015,14 +4956,11 @@ void wlc_phy_txpower_recalc_target_lcnphy(struct brcms_phy *pi)
 	if (wlc_lcnphy_tempsense_based_pwr_ctrl_enabled(pi)) {
 		wlc_lcnphy_calib_modes(pi, LCNPHY_PERICAL_TEMPBASED_TXPWRCTRL);
 	} else if (wlc_lcnphy_tssi_based_pwr_ctrl_enabled(pi)) {
-
 		pwr_ctrl = wlc_lcnphy_get_tx_pwr_ctrl(pi);
 		wlc_lcnphy_set_tx_pwr_ctrl(pi, LCNPHY_TX_PWR_CTRL_OFF);
 		wlc_lcnphy_txpower_recalc_target(pi);
-
 		wlc_lcnphy_set_tx_pwr_ctrl(pi, pwr_ctrl);
-	} else
-		return;
+	}
 }
 
 void wlc_phy_detach_lcnphy(struct brcms_phy *pi)
@@ -5035,13 +4973,12 @@ bool wlc_phy_attach_lcnphy(struct brcms_phy *pi)
 	struct brcms_phy_lcnphy *pi_lcn;
 
 	pi->u.pi_lcnphy = kzalloc(sizeof(struct brcms_phy_lcnphy), GFP_ATOMIC);
-	if (pi->u.pi_lcnphy == NULL) {
+	if (pi->u.pi_lcnphy == NULL)
 		return false;
-	}
 
 	pi_lcn = pi->u.pi_lcnphy;
 
-	if ((0 == (pi->sh->boardflags & BFL_NOPA)) && !NORADIO_ENAB(pi->pubpi)) {
+	if (0 == (pi->sh->boardflags & BFL_NOPA)) {
 		pi->hwpwrctrl = true;
 		pi->hwpwrctrl_capable = true;
 	}
@@ -5062,7 +4999,8 @@ bool wlc_phy_attach_lcnphy(struct brcms_phy *pi)
 	if (!wlc_phy_txpwr_srom_read_lcnphy(pi))
 		return false;
 
-	if ((pi->sh->boardflags & BFL_FEM) && (LCNREV_IS(pi->pubpi.phy_rev, 1))) {
+	if ((pi->sh->boardflags & BFL_FEM) &&
+	    (LCNREV_IS(pi->pubpi.phy_rev, 1))) {
 		if (pi_lcn->lcnphy_tempsense_option == 3) {
 			pi->hwpwrctrl = true;
 			pi->hwpwrctrl_capable = true;
@@ -5090,8 +5028,8 @@ static void wlc_lcnphy_set_rx_gain(struct brcms_phy *pi, u32 gain)
 	biq1 = (u16) (gain >> 16) & 0xf;
 
 	gain0_15 = (u16) ((lna1 & 0x3) | ((lna1 & 0x3) << 2) |
-			     ((lna2 & 0x3) << 4) | ((lna2 & 0x3) << 6) |
-			     ((tia & 0xf) << 8) | ((biq0 & 0xf) << 12));
+			  ((lna2 & 0x3) << 4) | ((lna2 & 0x3) << 6) |
+			  ((tia & 0xf) << 8) | ((biq0 & 0xf) << 12));
 	gain16_19 = biq1;
 
 	mod_phy_reg(pi, 0x44d, (0x1 << 0), trsw << 0);
@@ -5126,18 +5064,19 @@ static u32 wlc_lcnphy_get_receive_power(struct brcms_phy *pi, s32 *gain_index)
 					       lcnphy_23bitgaincode_table
 					       [*gain_index]);
 			received_power =
-			    wlc_lcnphy_measure_digital_power(pi,
-							     pi_lcn->
-							     lcnphy_noise_samples);
+				wlc_lcnphy_measure_digital_power(
+					pi,
+					pi_lcn->
+					lcnphy_noise_samples);
 			(*gain_index)++;
 		}
 		(*gain_index)--;
 	} else {
 		wlc_lcnphy_set_rx_gain(pi, gain_code);
 		received_power =
-		    wlc_lcnphy_measure_digital_power(pi,
-						     pi_lcn->
-						     lcnphy_noise_samples);
+			wlc_lcnphy_measure_digital_power(pi,
+							 pi_lcn->
+							 lcnphy_noise_samples);
 	}
 
 	return received_power;
@@ -5150,6 +5089,8 @@ s32 wlc_lcnphy_rx_signal_power(struct brcms_phy *pi, s32 gain_index)
 	s32 log_val, gain_mismatch, desired_gain, input_power_offset_db,
 	    input_power_db;
 	s32 received_power, temperature;
+	u32 power;
+	u32 msb1, msb2, val1, val2, diff1, diff2;
 	uint freq;
 	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
 
@@ -5159,20 +5100,17 @@ s32 wlc_lcnphy_rx_signal_power(struct brcms_phy *pi, s32 gain_index)
 
 	nominal_power_db = read_phy_reg(pi, 0x425) >> 8;
 
-	{
-		u32 power = (received_power * 16);
-		u32 msb1, msb2, val1, val2, diff1, diff2;
-		msb1 = ffs(power) - 1;
-		msb2 = msb1 + 1;
-		val1 = 1 << msb1;
-		val2 = 1 << msb2;
-		diff1 = (power - val1);
-		diff2 = (val2 - power);
-		if (diff1 < diff2)
-			log_val = msb1;
-		else
-			log_val = msb2;
-	}
+	power = (received_power * 16);
+	msb1 = ffs(power) - 1;
+	msb2 = msb1 + 1;
+	val1 = 1 << msb1;
+	val2 = 1 << msb2;
+	diff1 = (power - val1);
+	diff2 = (val2 - power);
+	if (diff1 < diff2)
+		log_val = msb1;
+	else
+		log_val = msb2;
 
 	log_val = log_val * 3;
 
@@ -5188,7 +5126,7 @@ s32 wlc_lcnphy_rx_signal_power(struct brcms_phy *pi, s32 gain_index)
 	input_power_db = input_power_offset_db - desired_gain;
 
 	input_power_db =
-	    input_power_db + lcnphy_gain_index_offset_for_rssi[gain_index];
+		input_power_db + lcnphy_gain_index_offset_for_rssi[gain_index];
 
 	freq = wlc_phy_channel2freq(CHSPEC_CHANNEL(pi->radio_chanspec));
 	if ((freq > 2427) && (freq <= 2467))
@@ -5196,99 +5134,21 @@ s32 wlc_lcnphy_rx_signal_power(struct brcms_phy *pi, s32 gain_index)
 
 	temperature = pi_lcn->lcnphy_lastsensed_temperature;
 
-	if ((temperature - 15) < -30) {
+	if ((temperature - 15) < -30)
 		input_power_db =
-		    input_power_db + (((temperature - 10 - 25) * 286) >> 12) -
-		    7;
-	} else if ((temperature - 15) < 4) {
+			input_power_db +
+			(((temperature - 10 - 25) * 286) >> 12) -
+			7;
+	else if ((temperature - 15) < 4)
 		input_power_db =
-		    input_power_db + (((temperature - 10 - 25) * 286) >> 12) -
-		    3;
-	} else {
-		input_power_db =
-		    input_power_db + (((temperature - 10 - 25) * 286) >> 12);
-	}
+			input_power_db +
+			(((temperature - 10 - 25) * 286) >> 12) -
+			3;
+	else
+		input_power_db = input_power_db +
+					(((temperature - 10 - 25) * 286) >> 12);
 
 	wlc_lcnphy_rx_gain_override_enable(pi, 0);
 
 	return input_power_db;
 }
-
-static int
-wlc_lcnphy_load_tx_iir_filter(struct brcms_phy *pi, bool is_ofdm, s16 filt_type)
-{
-	s16 filt_index = -1;
-	int j;
-
-	u16 addr[] = {
-		0x910,
-		0x91e,
-		0x91f,
-		0x924,
-		0x925,
-		0x926,
-		0x920,
-		0x921,
-		0x927,
-		0x928,
-		0x929,
-		0x922,
-		0x923,
-		0x930,
-		0x931,
-		0x932
-	};
-
-	u16 addr_ofdm[] = {
-		0x90f,
-		0x900,
-		0x901,
-		0x906,
-		0x907,
-		0x908,
-		0x902,
-		0x903,
-		0x909,
-		0x90a,
-		0x90b,
-		0x904,
-		0x905,
-		0x90c,
-		0x90d,
-		0x90e
-	};
-
-	if (!is_ofdm) {
-		for (j = 0; j < LCNPHY_NUM_TX_DIG_FILTERS_CCK; j++) {
-			if (filt_type == LCNPHY_txdigfiltcoeffs_cck[j][0]) {
-				filt_index = (s16) j;
-				break;
-			}
-		}
-
-		if (filt_index != -1) {
-			for (j = 0; j < LCNPHY_NUM_DIG_FILT_COEFFS; j++) {
-				write_phy_reg(pi, addr[j],
-					      LCNPHY_txdigfiltcoeffs_cck
-					      [filt_index][j + 1]);
-			}
-		}
-	} else {
-		for (j = 0; j < LCNPHY_NUM_TX_DIG_FILTERS_OFDM; j++) {
-			if (filt_type == LCNPHY_txdigfiltcoeffs_ofdm[j][0]) {
-				filt_index = (s16) j;
-				break;
-			}
-		}
-
-		if (filt_index != -1) {
-			for (j = 0; j < LCNPHY_NUM_DIG_FILT_COEFFS; j++) {
-				write_phy_reg(pi, addr_ofdm[j],
-					      LCNPHY_txdigfiltcoeffs_ofdm
-					      [filt_index][j + 1]);
-			}
-		}
-	}
-
-	return (filt_index != -1) ? 0 : -1;
-}
diff --git a/drivers/staging/brcm80211/brcmsmac/phy/phy_n.c b/drivers/staging/brcm80211/brcmsmac/phy/phy_n.c
index f8e419239425..cd19c2f7a347 100644
--- a/drivers/staging/brcm80211/brcmsmac/phy/phy_n.c
+++ b/drivers/staging/brcm80211/brcmsmac/phy/phy_n.c
@@ -14,7 +14,9 @@
  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#include <linux/kernel.h>
 #include <linux/delay.h>
+#include <linux/cordic.h>
 
 #include <brcm_hw_ids.h>
 #include <aiutils.h>
@@ -28,27 +30,42 @@
 #include "phyreg_n.h"
 #include "phytbl_n.h"
 
-#define	READ_RADIO_REG2(pi, radio_type, jspace, core, reg_name) \
-	read_radio_reg(pi, radio_type##_##jspace##_##reg_name | \
-	((core == PHY_CORE_0) ? radio_type##_##jspace##0 : radio_type##_##jspace##1))
-#define	WRITE_RADIO_REG2(pi, radio_type, jspace, core, reg_name, value) \
+#define READ_RADIO_REG2(pi, radio_type, jspace, core, reg_name)	\
+	read_radio_reg(pi, radio_type##_##jspace##_##reg_name |	\
+		       ((core == PHY_CORE_0) ? \
+			radio_type##_##jspace##0 : \
+			radio_type##_##jspace##1))
+
+#define WRITE_RADIO_REG2(pi, radio_type, jspace, core, reg_name, value)	\
 	write_radio_reg(pi, radio_type##_##jspace##_##reg_name | \
-	((core == PHY_CORE_0) ? radio_type##_##jspace##0 : radio_type##_##jspace##1), value);
-#define	WRITE_RADIO_SYN(pi, radio_type, reg_name, value) \
-	write_radio_reg(pi, radio_type##_##SYN##_##reg_name, value);
-
-#define	READ_RADIO_REG3(pi, radio_type, jspace, core, reg_name) \
-	read_radio_reg(pi, ((core == PHY_CORE_0) ? radio_type##_##jspace##0##_##reg_name : \
-	radio_type##_##jspace##1##_##reg_name));
-#define	WRITE_RADIO_REG3(pi, radio_type, jspace, core, reg_name, value) \
-	write_radio_reg(pi, ((core == PHY_CORE_0) ? radio_type##_##jspace##0##_##reg_name : \
-	radio_type##_##jspace##1##_##reg_name), value);
-#define	READ_RADIO_REG4(pi, radio_type, jspace, core, reg_name) \
-	read_radio_reg(pi, ((core == PHY_CORE_0) ? radio_type##_##reg_name##_##jspace##0 : \
-	radio_type##_##reg_name##_##jspace##1));
-#define	WRITE_RADIO_REG4(pi, radio_type, jspace, core, reg_name, value) \
-	write_radio_reg(pi, ((core == PHY_CORE_0) ? radio_type##_##reg_name##_##jspace##0 : \
-	radio_type##_##reg_name##_##jspace##1), value);
+			((core ==  PHY_CORE_0) ? \
+			 radio_type##_##jspace##0 : \
+			 radio_type##_##jspace##1), value)
+
+#define WRITE_RADIO_SYN(pi, radio_type, reg_name, value) \
+	write_radio_reg(pi, radio_type##_##SYN##_##reg_name, value)
+
+#define READ_RADIO_REG3(pi, radio_type, jspace, core, reg_name)	\
+	read_radio_reg(pi, ((core == PHY_CORE_0) ? \
+			    radio_type##_##jspace##0##_##reg_name : \
+			    radio_type##_##jspace##1##_##reg_name))
+
+#define WRITE_RADIO_REG3(pi, radio_type, jspace, core, reg_name, value)	\
+	write_radio_reg(pi, ((core ==  PHY_CORE_0) ? \
+			     radio_type##_##jspace##0##_##reg_name : \
+			     radio_type##_##jspace##1##_##reg_name), \
+			value)
+
+#define READ_RADIO_REG4(pi, radio_type, jspace, core, reg_name)	\
+	read_radio_reg(pi, ((core == PHY_CORE_0) ? \
+			     radio_type##_##reg_name##_##jspace##0 : \
+			     radio_type##_##reg_name##_##jspace##1))
+
+#define WRITE_RADIO_REG4(pi, radio_type, jspace, core, reg_name, value)	\
+	write_radio_reg(pi, ((core == PHY_CORE_0) ? \
+			radio_type##_##reg_name##_##jspace##0 : \
+			radio_type##_##reg_name##_##jspace##1), \
+			value)
 
 #define NPHY_ACI_MAX_UNDETECT_WINDOW_SZ 40
 #define NPHY_ACI_CHANNEL_DELTA 5
@@ -103,9 +120,11 @@
 #define NPHY_CALSANITY_RSSI_NB_MAX_POS  9
 #define NPHY_CALSANITY_RSSI_NB_MAX_NEG -9
 #define NPHY_CALSANITY_RSSI_W1_MAX_POS  12
-#define NPHY_CALSANITY_RSSI_W1_MAX_NEG (NPHY_RSSICAL_W1_TARGET - NPHY_RSSICAL_MAXREAD)
+#define NPHY_CALSANITY_RSSI_W1_MAX_NEG (NPHY_RSSICAL_W1_TARGET - \
+					NPHY_RSSICAL_MAXREAD)
 #define NPHY_CALSANITY_RSSI_W2_MAX_POS  NPHY_CALSANITY_RSSI_W1_MAX_POS
-#define NPHY_CALSANITY_RSSI_W2_MAX_NEG (NPHY_RSSICAL_W2_TARGET - NPHY_RSSICAL_MAXREAD)
+#define NPHY_CALSANITY_RSSI_W2_MAX_NEG (NPHY_RSSICAL_W2_TARGET - \
+					NPHY_RSSICAL_MAXREAD)
 #define NPHY_RSSI_SXT(x) ((s8) (-((x) & 0x20) + ((x) & 0x1f)))
 #define NPHY_RSSI_NB_VIOL(x)  (((x) > NPHY_CALSANITY_RSSI_NB_MAX_POS) || \
 			       ((x) < NPHY_CALSANITY_RSSI_NB_MAX_NEG))
@@ -124,11 +143,11 @@
 #define NPHY_PAPD_COMP_OFF 0
 #define NPHY_PAPD_COMP_ON  1
 
-#define NPHY_SROM_TEMPSHIFT		32
-#define NPHY_SROM_MAXTEMPOFFSET		16
-#define NPHY_SROM_MINTEMPOFFSET		-16
+#define NPHY_SROM_TEMPSHIFT             32
+#define NPHY_SROM_MAXTEMPOFFSET         16
+#define NPHY_SROM_MINTEMPOFFSET         -16
 
-#define NPHY_CAL_MAXTEMPDELTA		64
+#define NPHY_CAL_MAXTEMPDELTA           64
 
 #define NPHY_NOISEVAR_TBLLEN40 256
 #define NPHY_NOISEVAR_TBLLEN20 128
@@ -138,8 +157,24 @@
 #define NPHY_ADJUSTED_MINCRSPOWER 0x1e
 
 /* 5357 Chip specific ChipControl register bits */
-#define CCTRL5357_EXTPA                 (1<<14)	/* extPA in ChipControl 1, bit 14 */
-#define CCTRL5357_ANT_MUX_2o3		(1<<15)	/* 2o3 in ChipControl 1, bit 15 */
+#define CCTRL5357_EXTPA            (1<<14) /* extPA in ChipControl 1, bit 14 */
+#define CCTRL5357_ANT_MUX_2o3      (1<<15) /* 2o3 in ChipControl 1, bit 15 */
+
+#define NPHY_CAL_TSSISAMPS      64
+#define NPHY_TEST_TONE_FREQ_40MHz 4000
+#define NPHY_TEST_TONE_FREQ_20MHz 2500
+
+#define MAX_205x_RCAL_WAITLOOPS 10000
+
+#define NPHY_RXCAL_TONEAMP 181
+#define NPHY_RXCAL_TONEFREQ_40MHz 4000
+#define NPHY_RXCAL_TONEFREQ_20MHz 2000
+
+#define TXFILT_SHAPING_OFDM20   0
+#define TXFILT_SHAPING_OFDM40   1
+#define TXFILT_SHAPING_CCK      2
+#define TXFILT_DEFAULT_OFDM20   3
+#define TXFILT_DEFAULT_OFDM40   4
 
 struct nphy_iqcal_params {
 	u16 txlpf;
@@ -184,44 +219,42 @@ struct nphy_ipa_txrxgain {
 
 #define NPHY_IPA_RXCAL_MAXGAININDEX (6 - 1)
 
-struct nphy_ipa_txrxgain nphy_ipa_rxcal_gaintbl_5GHz[] = { {0, 0, 0, 0, 0, 100},
-{0, 0, 0, 0, 0, 50},
-{0, 0, 0, 0, 0, -1},
-{0, 0, 0, 3, 0, -1},
-{0, 0, 3, 3, 0, -1},
-{0, 2, 3, 3, 0, -1}
+static const struct nphy_ipa_txrxgain nphy_ipa_rxcal_gaintbl_5GHz[] = {
+	{0, 0, 0, 0, 0, 100},
+	{0, 0, 0, 0, 0, 50},
+	{0, 0, 0, 0, 0, -1},
+	{0, 0, 0, 3, 0, -1},
+	{0, 0, 3, 3, 0, -1},
+	{0, 2, 3, 3, 0, -1}
 };
 
-struct nphy_ipa_txrxgain nphy_ipa_rxcal_gaintbl_2GHz[] = { {0, 0, 0, 0, 0, 128},
-{0, 0, 0, 0, 0, 70},
-{0, 0, 0, 0, 0, 20},
-{0, 0, 0, 3, 0, 20},
-{0, 0, 3, 3, 0, 20},
-{0, 2, 3, 3, 0, 20}
+static const struct nphy_ipa_txrxgain nphy_ipa_rxcal_gaintbl_2GHz[] = {
+	{0, 0, 0, 0, 0, 128},
+	{0, 0, 0, 0, 0, 70},
+	{0, 0, 0, 0, 0, 20},
+	{0, 0, 0, 3, 0, 20},
+	{0, 0, 3, 3, 0, 20},
+	{0, 2, 3, 3, 0, 20}
 };
 
-struct nphy_ipa_txrxgain nphy_ipa_rxcal_gaintbl_5GHz_rev7[] = {
-{0, 0, 0, 0, 0, 100},
-{0, 0, 0, 0, 0, 50},
-{0, 0, 0, 0, 0, -1},
-{0, 0, 0, 3, 0, -1},
-{0, 0, 3, 3, 0, -1},
-{0, 0, 5, 3, 0, -1}
+static const struct nphy_ipa_txrxgain nphy_ipa_rxcal_gaintbl_5GHz_rev7[] = {
+	{0, 0, 0, 0, 0, 100},
+	{0, 0, 0, 0, 0, 50},
+	{0, 0, 0, 0, 0, -1},
+	{0, 0, 0, 3, 0, -1},
+	{0, 0, 3, 3, 0, -1},
+	{0, 0, 5, 3, 0, -1}
 };
 
-struct nphy_ipa_txrxgain nphy_ipa_rxcal_gaintbl_2GHz_rev7[] = {
-{0, 0, 0, 0, 0, 10},
-{0, 0, 0, 1, 0, 10},
-{0, 0, 1, 2, 0, 10},
-{0, 0, 1, 3, 0, 10},
-{0, 0, 4, 3, 0, 10},
-{0, 0, 6, 3, 0, 10}
+static const struct nphy_ipa_txrxgain nphy_ipa_rxcal_gaintbl_2GHz_rev7[] = {
+	{0, 0, 0, 0, 0, 10},
+	{0, 0, 0, 1, 0, 10},
+	{0, 0, 1, 2, 0, 10},
+	{0, 0, 1, 3, 0, 10},
+	{0, 0, 4, 3, 0, 10},
+	{0, 0, 6, 3, 0, 10}
 };
 
-#define NPHY_RXCAL_TONEAMP 181
-#define NPHY_RXCAL_TONEFREQ_40MHz 4000
-#define NPHY_RXCAL_TONEFREQ_20MHz 2000
-
 enum {
 	NPHY_RXCAL_GAIN_INIT = 0,
 	NPHY_RXCAL_GAIN_UP,
@@ -230,17 +263,11 @@ enum {
 
 #define wlc_phy_get_papd_nphy(pi) \
 	(read_phy_reg((pi), 0x1e7) & \
-			((0x1 << 15) | \
-			(0x1 << 14) | \
-			(0x1 << 13)))
+	 ((0x1 << 15) |	\
+	  (0x1 << 14) |	\
+	  (0x1 << 13)))
 
-#define TXFILT_SHAPING_OFDM20   0
-#define TXFILT_SHAPING_OFDM40   1
-#define TXFILT_SHAPING_CCK      2
-#define TXFILT_DEFAULT_OFDM20   3
-#define TXFILT_DEFAULT_OFDM40   4
-
-u16 NPHY_IPA_REV4_txdigi_filtcoeffs[][NPHY_NUM_DIG_FILT_COEFFS] = {
+static const u16 NPHY_IPA_REV4_txdigi_filtcoeffs[][NPHY_NUM_DIG_FILT_COEFFS] = {
 	{-377, 137, -407, 208, -1527, 956, 93, 186, 93,
 	 230, -44, 230, 201, -191, 201},
 	{-77, 20, -98, 49, -93, 60, 56, 111, 56, 26, -5,
@@ -416,7 +443,7 @@ struct nphy_sfo_cfg {
 	u16 PHY_BW6;
 };
 
-static struct chan_info_nphy_2055 chan_info_nphy_2055[] = {
+static const struct chan_info_nphy_2055 chan_info_nphy_2055[] = {
 	{
 	 184, 4920, 3280, 0x71, 0x01, 0xEC, 0x0F, 0xFF, 0x01, 0x04, 0x0A,
 	 0x00, 0x8F, 0xFF, 0xFF, 0xFF, 0x00, 0x0F, 0x0F, 0x8F, 0xFF, 0x00, 0x0F,
@@ -915,7 +942,7 @@ static struct chan_info_nphy_2055 chan_info_nphy_2055[] = {
 	 0x01, 0x80, 0x3E6, 0x3E2, 0x3DE, 0x41B, 0x41F, 0x424}
 };
 
-static struct chan_info_nphy_radio205x chan_info_nphyrev3_2056[] = {
+static const struct chan_info_nphy_radio205x chan_info_nphyrev3_2056[] = {
 	{
 	 184, 4920, 0xff, 0x01, 0x01, 0x01, 0xec, 0x05, 0x05, 0x04, 0x0c, 0x01,
 	 0x00, 0x00, 0x00, 0x8f, 0x0f, 0x00, 0xff, 0xff, 0x00, 0x08, 0x00, 0x7f,
@@ -1538,7 +1565,7 @@ static struct chan_info_nphy_radio205x chan_info_nphyrev3_2056[] = {
 	 0x0f, 0x00, 0x0d, 0x03e6, 0x03e2, 0x03de, 0x041b, 0x041f, 0x0424}
 };
 
-static struct chan_info_nphy_radio205x chan_info_nphyrev4_2056_A1[] = {
+static const struct chan_info_nphy_radio205x chan_info_nphyrev4_2056_A1[] = {
 	{
 	 184, 4920, 0xff, 0x01, 0x01, 0x01, 0xec, 0x05, 0x05, 0x04, 0x0c, 0x01,
 	 0x00, 0x00, 0x00, 0x8f, 0x0f, 0x00, 0xff, 0xff, 0x00, 0x0e, 0x00, 0x7f,
@@ -2161,7 +2188,7 @@ static struct chan_info_nphy_radio205x chan_info_nphyrev4_2056_A1[] = {
 	 0x0f, 0x00, 0x0e, 0x03e6, 0x03e2, 0x03de, 0x041b, 0x041f, 0x0424}
 };
 
-static struct chan_info_nphy_radio205x chan_info_nphyrev5_2056v5[] = {
+static const struct chan_info_nphy_radio205x chan_info_nphyrev5_2056v5[] = {
 	{
 	 184, 4920, 0xff, 0x01, 0x01, 0x01, 0xec, 0x05, 0x05, 0x04, 0x0c, 0x01,
 	 0x00, 0x00, 0x00, 0x8f, 0x0f, 0x00, 0xff, 0xff, 0x00, 0x0b, 0x00, 0x70,
@@ -2784,7 +2811,7 @@ static struct chan_info_nphy_radio205x chan_info_nphyrev5_2056v5[] = {
 	 0x0d, 0x00, 0x08, 0x03e6, 0x03e2, 0x03de, 0x041b, 0x041f, 0x0424}
 };
 
-static struct chan_info_nphy_radio205x chan_info_nphyrev6_2056v6[] = {
+static const struct chan_info_nphy_radio205x chan_info_nphyrev6_2056v6[] = {
 	{
 	 184, 4920, 0xff, 0x01, 0x01, 0x01, 0xec, 0x05, 0x05, 0x04, 0x0c, 0x01,
 	 0x00, 0x00, 0x00, 0x8f, 0x0f, 0x00, 0xff, 0xfe, 0x00, 0x09, 0x00, 0x77,
@@ -3407,7 +3434,7 @@ static struct chan_info_nphy_radio205x chan_info_nphyrev6_2056v6[] = {
 	 0x09, 0x00, 0x09, 0x03e6, 0x03e2, 0x03de, 0x041b, 0x041f, 0x0424}
 };
 
-static struct chan_info_nphy_radio205x chan_info_nphyrev5n6_2056v7[] = {
+static const struct chan_info_nphy_radio205x chan_info_nphyrev5n6_2056v7[] = {
 	{
 	 184, 4920, 0xff, 0x01, 0x01, 0x01, 0xec, 0x05, 0x05, 0x04, 0x0c, 0x01,
 	 0x00, 0x00, 0x00, 0x8f, 0x0f, 0x00, 0xff, 0xff, 0x00, 0x0b, 0x00, 0x70,
@@ -4030,7 +4057,7 @@ static struct chan_info_nphy_radio205x chan_info_nphyrev5n6_2056v7[] = {
 	 0x0d, 0x00, 0x08, 0x03e6, 0x03e2, 0x03de, 0x041b, 0x041f, 0x0424}
 };
 
-static struct chan_info_nphy_radio205x chan_info_nphyrev6_2056v8[] = {
+static const struct chan_info_nphy_radio205x chan_info_nphyrev6_2056v8[] = {
 	{
 	 184, 4920, 0xff, 0x01, 0x01, 0x01, 0xec, 0x05, 0x05, 0x04, 0x0c, 0x01,
 	 0x00, 0x00, 0x00, 0x8f, 0x0f, 0x00, 0xff, 0xfe, 0x00, 0x09, 0x00, 0x77,
@@ -4653,7 +4680,7 @@ static struct chan_info_nphy_radio205x chan_info_nphyrev6_2056v8[] = {
 	 0x09, 0x00, 0x09, 0x03e6, 0x03e2, 0x03de, 0x041b, 0x041f, 0x0424}
 };
 
-static struct chan_info_nphy_radio205x chan_info_nphyrev6_2056v11[] = {
+static const struct chan_info_nphy_radio205x chan_info_nphyrev6_2056v11[] = {
 	{
 	 184, 4920, 0xff, 0x01, 0x01, 0x01, 0xec, 0x05, 0x05, 0x02, 0x0c, 0x01,
 	 0x00, 0x00, 0x00, 0x8f, 0x0f, 0x00, 0xff, 0xfe, 0x00, 0x09, 0x00, 0x77,
@@ -5276,7 +5303,7 @@ static struct chan_info_nphy_radio205x chan_info_nphyrev6_2056v11[] = {
 	 0x09, 0x00, 0x09, 0x03e6, 0x03e2, 0x03de, 0x041b, 0x041f, 0x0424}
 };
 
-static struct chan_info_nphy_radio2057 chan_info_nphyrev7_2057_rev4[] = {
+static const struct chan_info_nphy_radio2057 chan_info_nphyrev7_2057_rev4[] = {
 	{
 	 184, 4920, 0x68, 0x16, 0x10, 0x0c, 0x0c, 0x0c, 0x30, 0xec, 0x01, 0x0f,
 	 0x00, 0x0f, 0x00, 0xff, 0x00, 0x00, 0x0f, 0x0f, 0xf3, 0x00, 0xef, 0x00,
@@ -6139,7 +6166,8 @@ static struct chan_info_nphy_radio2057 chan_info_nphyrev7_2057_rev4[] = {
 	 0x0424}
 };
 
-static struct chan_info_nphy_radio2057_rev5 chan_info_nphyrev8_2057_rev5[] = {
+static const struct chan_info_nphy_radio2057_rev5
+chan_info_nphyrev8_2057_rev5[] = {
 	{
 	 1, 2412, 0x48, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x6c, 0x09, 0x0d,
 	 0x08, 0x0e, 0x61, 0x03, 0xff, 0x61, 0x03, 0xff, 0x03c9, 0x03c5, 0x03c1,
@@ -6198,7 +6226,8 @@ static struct chan_info_nphy_radio2057_rev5 chan_info_nphyrev8_2057_rev5[] = {
 	 0x041b, 0x041f, 0x0424}
 };
 
-static struct chan_info_nphy_radio2057_rev5 chan_info_nphyrev9_2057_rev5v1[] = {
+static const struct chan_info_nphy_radio2057_rev5
+chan_info_nphyrev9_2057_rev5v1[] = {
 	{
 	 1, 2412, 0x48, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x6c, 0x09, 0x0d,
 	 0x08, 0x0e, 0x61, 0x03, 0xff, 0x61, 0x03, 0xff, 0x03c9, 0x03c5, 0x03c1,
@@ -6257,7 +6286,7 @@ static struct chan_info_nphy_radio2057_rev5 chan_info_nphyrev9_2057_rev5v1[] = {
 	 0x041b, 0x041f, 0x0424}
 };
 
-static struct chan_info_nphy_radio2057 chan_info_nphyrev8_2057_rev7[] = {
+static const struct chan_info_nphy_radio2057 chan_info_nphyrev8_2057_rev7[] = {
 	{
 	 184, 4920, 0x68, 0x16, 0x10, 0x0c, 0x0c, 0x0c, 0x30, 0xec, 0x01, 0x0f,
 	 0x00, 0x0f, 0x00, 0xff, 0x00, 0xd3, 0x0f, 0x0f, 0xd3, 0x00, 0xff, 0x00,
@@ -6998,7 +7027,7 @@ static struct chan_info_nphy_radio2057 chan_info_nphyrev8_2057_rev7[] = {
 	 0x0424}
 };
 
-static struct chan_info_nphy_radio2057 chan_info_nphyrev8_2057_rev8[] = {
+static const struct chan_info_nphy_radio2057 chan_info_nphyrev8_2057_rev8[] = {
 	{
 	 186, 4930, 0x6b, 0x16, 0x10, 0x0c, 0x0c, 0x0c, 0x30, 0xed, 0x01, 0x0f,
 	 0x00, 0x0f, 0x00, 0xff, 0x00, 0xd3, 0x0f, 0x0f, 0xd3, 0x00, 0xff, 0x00,
@@ -7733,7 +7762,7 @@ static struct chan_info_nphy_radio2057 chan_info_nphyrev8_2057_rev8[] = {
 	 0x0424}
 };
 
-struct radio_regs regs_2055[] = {
+static struct radio_regs regs_2055[] = {
 	{0x02, 0x80, 0x80, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0x27, 0x27, 0, 0},
@@ -7962,7 +7991,7 @@ struct radio_regs regs_2055[] = {
 	{0xFFFF, 0, 0, 0, 0},
 };
 
-struct radio_regs regs_SYN_2056[] = {
+static struct radio_regs regs_SYN_2056[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -8147,7 +8176,7 @@ struct radio_regs regs_SYN_2056[] = {
 	{0xFFFF, 0, 0, 0, 0}
 };
 
-struct radio_regs regs_TX_2056[] = {
+static struct radio_regs regs_TX_2056[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -8296,7 +8325,7 @@ struct radio_regs regs_TX_2056[] = {
 	{0xFFFF, 0, 0, 0, 0}
 };
 
-struct radio_regs regs_RX_2056[] = {
+static struct radio_regs regs_RX_2056[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -8447,7 +8476,7 @@ struct radio_regs regs_RX_2056[] = {
 	{0xFFFF, 0, 0, 0, 0}
 };
 
-struct radio_regs regs_SYN_2056_A1[] = {
+static struct radio_regs regs_SYN_2056_A1[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -8632,7 +8661,7 @@ struct radio_regs regs_SYN_2056_A1[] = {
 	{0xFFFF, 0, 0, 0, 0}
 };
 
-struct radio_regs regs_TX_2056_A1[] = {
+static struct radio_regs regs_TX_2056_A1[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -8781,7 +8810,7 @@ struct radio_regs regs_TX_2056_A1[] = {
 	{0xFFFF, 0, 0, 0, 0}
 };
 
-struct radio_regs regs_RX_2056_A1[] = {
+static struct radio_regs regs_RX_2056_A1[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -8932,7 +8961,7 @@ struct radio_regs regs_RX_2056_A1[] = {
 	{0xFFFF, 0, 0, 0, 0}
 };
 
-struct radio_regs regs_SYN_2056_rev5[] = {
+static struct radio_regs regs_SYN_2056_rev5[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -9117,7 +9146,7 @@ struct radio_regs regs_SYN_2056_rev5[] = {
 	{0xFFFF, 0, 0, 0, 0}
 };
 
-struct radio_regs regs_TX_2056_rev5[] = {
+static struct radio_regs regs_TX_2056_rev5[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -9274,7 +9303,7 @@ struct radio_regs regs_TX_2056_rev5[] = {
 	{0xFFFF, 0, 0, 0, 0}
 };
 
-struct radio_regs regs_RX_2056_rev5[] = {
+static struct radio_regs regs_RX_2056_rev5[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -9425,7 +9454,7 @@ struct radio_regs regs_RX_2056_rev5[] = {
 	{0xFFFF, 0, 0, 0, 0}
 };
 
-struct radio_regs regs_SYN_2056_rev6[] = {
+static struct radio_regs regs_SYN_2056_rev6[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -9610,7 +9639,7 @@ struct radio_regs regs_SYN_2056_rev6[] = {
 	{0xFFFF, 0, 0, 0, 0}
 };
 
-struct radio_regs regs_TX_2056_rev6[] = {
+static struct radio_regs regs_TX_2056_rev6[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -9767,7 +9796,7 @@ struct radio_regs regs_TX_2056_rev6[] = {
 	{0xFFFF, 0, 0, 0, 0}
 };
 
-struct radio_regs regs_RX_2056_rev6[] = {
+static struct radio_regs regs_RX_2056_rev6[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -9918,7 +9947,7 @@ struct radio_regs regs_RX_2056_rev6[] = {
 	{0xFFFF, 0, 0, 0, 0}
 };
 
-struct radio_regs regs_SYN_2056_rev7[] = {
+static struct radio_regs regs_SYN_2056_rev7[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -10103,7 +10132,7 @@ struct radio_regs regs_SYN_2056_rev7[] = {
 	{0xFFFF, 0, 0, 0, 0},
 };
 
-struct radio_regs regs_TX_2056_rev7[] = {
+static struct radio_regs regs_TX_2056_rev7[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -10260,7 +10289,7 @@ struct radio_regs regs_TX_2056_rev7[] = {
 	{0xFFFF, 0, 0, 0, 0},
 };
 
-struct radio_regs regs_RX_2056_rev7[] = {
+static struct radio_regs regs_RX_2056_rev7[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -10411,7 +10440,7 @@ struct radio_regs regs_RX_2056_rev7[] = {
 	{0xFFFF, 0, 0, 0, 0},
 };
 
-struct radio_regs regs_SYN_2056_rev8[] = {
+static struct radio_regs regs_SYN_2056_rev8[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -10596,7 +10625,7 @@ struct radio_regs regs_SYN_2056_rev8[] = {
 	{0xFFFF, 0, 0, 0, 0},
 };
 
-struct radio_regs regs_TX_2056_rev8[] = {
+static struct radio_regs regs_TX_2056_rev8[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -10753,7 +10782,7 @@ struct radio_regs regs_TX_2056_rev8[] = {
 	{0xFFFF, 0, 0, 0, 0},
 };
 
-struct radio_regs regs_RX_2056_rev8[] = {
+static struct radio_regs regs_RX_2056_rev8[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -10904,7 +10933,7 @@ struct radio_regs regs_RX_2056_rev8[] = {
 	{0xFFFF, 0, 0, 0, 0},
 };
 
-struct radio_regs regs_SYN_2056_rev11[] = {
+static const struct radio_regs regs_SYN_2056_rev11[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -11089,7 +11118,7 @@ struct radio_regs regs_SYN_2056_rev11[] = {
 	{0xFFFF, 0, 0, 0, 0},
 };
 
-struct radio_regs regs_TX_2056_rev11[] = {
+static const struct radio_regs regs_TX_2056_rev11[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -11246,7 +11275,7 @@ struct radio_regs regs_TX_2056_rev11[] = {
 	{0xFFFF, 0, 0, 0, 0},
 };
 
-struct radio_regs regs_RX_2056_rev11[] = {
+static const struct radio_regs regs_RX_2056_rev11[] = {
 	{0x02, 0, 0, 0, 0},
 	{0x03, 0, 0, 0, 0},
 	{0x04, 0, 0, 0, 0},
@@ -11397,7 +11426,7 @@ struct radio_regs regs_RX_2056_rev11[] = {
 	{0xFFFF, 0, 0, 0, 0},
 };
 
-struct radio_20xx_regs regs_2057_rev4[] = {
+static struct radio_20xx_regs regs_2057_rev4[] = {
 	{0x00, 0x84, 0},
 	{0x01, 0, 0},
 	{0x02, 0x60, 0},
@@ -11787,7 +11816,7 @@ struct radio_20xx_regs regs_2057_rev4[] = {
 	{0xFFFF, 0, 0},
 };
 
-struct radio_20xx_regs regs_2057_rev5[] = {
+static struct radio_20xx_regs regs_2057_rev5[] = {
 	{0x00, 0, 1},
 	{0x01, 0x57, 1},
 	{0x02, 0x20, 1},
@@ -12119,7 +12148,7 @@ struct radio_20xx_regs regs_2057_rev5[] = {
 	{0xFFFF, 0, 0}
 };
 
-struct radio_20xx_regs regs_2057_rev5v1[] = {
+static struct radio_20xx_regs regs_2057_rev5v1[] = {
 	{0x00, 0x15, 1},
 	{0x01, 0x57, 1},
 	{0x02, 0x20, 1},
@@ -12451,7 +12480,7 @@ struct radio_20xx_regs regs_2057_rev5v1[] = {
 	{0xFFFF, 0, 0}
 };
 
-struct radio_20xx_regs regs_2057_rev7[] = {
+static struct radio_20xx_regs regs_2057_rev7[] = {
 	{0x00, 0, 1},
 	{0x01, 0x57, 1},
 	{0x02, 0x20, 1},
@@ -12867,7 +12896,7 @@ struct radio_20xx_regs regs_2057_rev7[] = {
 	{0xFFFF, 0, 0}
 };
 
-struct radio_20xx_regs regs_2057_rev8[] = {
+static struct radio_20xx_regs regs_2057_rev8[] = {
 	{0x00, 0x8, 1},
 	{0x01, 0x57, 1},
 	{0x02, 0x20, 1},
@@ -13290,27 +13319,27 @@ static s32 nphy_lnagain_est1[] = { -224, 23242 };
 
 static const u16 tbl_iqcal_gainparams_nphy[2][NPHY_IQCAL_NUMGAINS][8] = {
 	{
-	 {0x000, 0, 0, 2, 0x69, 0x69, 0x69, 0x69},
-	 {0x700, 7, 0, 0, 0x69, 0x69, 0x69, 0x69},
-	 {0x710, 7, 1, 0, 0x68, 0x68, 0x68, 0x68},
-	 {0x720, 7, 2, 0, 0x67, 0x67, 0x67, 0x67},
-	 {0x730, 7, 3, 0, 0x66, 0x66, 0x66, 0x66},
-	 {0x740, 7, 4, 0, 0x65, 0x65, 0x65, 0x65},
-	 {0x741, 7, 4, 1, 0x65, 0x65, 0x65, 0x65},
-	 {0x742, 7, 4, 2, 0x65, 0x65, 0x65, 0x65},
-	 {0x743, 7, 4, 3, 0x65, 0x65, 0x65, 0x65}
-	 },
-	{
-	 {0x000, 7, 0, 0, 0x79, 0x79, 0x79, 0x79},
-	 {0x700, 7, 0, 0, 0x79, 0x79, 0x79, 0x79},
-	 {0x710, 7, 1, 0, 0x79, 0x79, 0x79, 0x79},
-	 {0x720, 7, 2, 0, 0x78, 0x78, 0x78, 0x78},
-	 {0x730, 7, 3, 0, 0x78, 0x78, 0x78, 0x78},
-	 {0x740, 7, 4, 0, 0x78, 0x78, 0x78, 0x78},
-	 {0x741, 7, 4, 1, 0x78, 0x78, 0x78, 0x78},
-	 {0x742, 7, 4, 2, 0x78, 0x78, 0x78, 0x78},
-	 {0x743, 7, 4, 3, 0x78, 0x78, 0x78, 0x78}
-	 }
+		{0x000, 0, 0, 2, 0x69, 0x69, 0x69, 0x69},
+		{0x700, 7, 0, 0, 0x69, 0x69, 0x69, 0x69},
+		{0x710, 7, 1, 0, 0x68, 0x68, 0x68, 0x68},
+		{0x720, 7, 2, 0, 0x67, 0x67, 0x67, 0x67},
+		{0x730, 7, 3, 0, 0x66, 0x66, 0x66, 0x66},
+		{0x740, 7, 4, 0, 0x65, 0x65, 0x65, 0x65},
+		{0x741, 7, 4, 1, 0x65, 0x65, 0x65, 0x65},
+		{0x742, 7, 4, 2, 0x65, 0x65, 0x65, 0x65},
+		{0x743, 7, 4, 3, 0x65, 0x65, 0x65, 0x65}
+	},
+	{
+		{0x000, 7, 0, 0, 0x79, 0x79, 0x79, 0x79},
+		{0x700, 7, 0, 0, 0x79, 0x79, 0x79, 0x79},
+		{0x710, 7, 1, 0, 0x79, 0x79, 0x79, 0x79},
+		{0x720, 7, 2, 0, 0x78, 0x78, 0x78, 0x78},
+		{0x730, 7, 3, 0, 0x78, 0x78, 0x78, 0x78},
+		{0x740, 7, 4, 0, 0x78, 0x78, 0x78, 0x78},
+		{0x741, 7, 4, 1, 0x78, 0x78, 0x78, 0x78},
+		{0x742, 7, 4, 2, 0x78, 0x78, 0x78, 0x78},
+		{0x743, 7, 4, 3, 0x78, 0x78, 0x78, 0x78}
+	}
 };
 
 static const u32 nphy_tpc_txgain[] = {
@@ -14081,118 +14110,11 @@ static u32 nphy_tpc_5GHz_txgain_HiPwrEPA[] = {
 static u8 ant_sw_ctrl_tbl_rev8_2o3[] = { 0x14, 0x18 };
 static u8 ant_sw_ctrl_tbl_rev8[] = { 0x4, 0x8, 0x4, 0x8, 0x11, 0x12 };
 static u8 ant_sw_ctrl_tbl_rev8_2057v7_core0[] = {
-	0x09, 0x0a, 0x15, 0x16, 0x09, 0x0a };
+	0x09, 0x0a, 0x15, 0x16, 0x09, 0x0a
+};
 static u8 ant_sw_ctrl_tbl_rev8_2057v7_core1[] = {
-	0x09, 0x0a, 0x09, 0x0a, 0x15, 0x16 };
-
-static bool wlc_phy_chan2freq_nphy(struct brcms_phy *pi, uint channel, int *f,
-				   struct chan_info_nphy_radio2057 **t0,
-				   struct chan_info_nphy_radio205x **t1,
-				   struct chan_info_nphy_radio2057_rev5 **t2,
-				   struct chan_info_nphy_2055 **t3);
-static void wlc_phy_chanspec_nphy_setup(struct brcms_phy *pi, chanspec_t chans,
-					const struct nphy_sfo_cfg *c);
-
-static void wlc_phy_adjust_rx_analpfbw_nphy(struct brcms_phy *pi,
-					    u16 reduction_factr);
-static void wlc_phy_adjust_min_noisevar_nphy(struct brcms_phy *pi,
-					     int ntones, int *, u32 *buf);
-static void wlc_phy_adjust_crsminpwr_nphy(struct brcms_phy *pi, u8 minpwr);
-static void wlc_phy_txlpfbw_nphy(struct brcms_phy *pi);
-static void wlc_phy_spurwar_nphy(struct brcms_phy *pi);
-
-static void wlc_phy_radio_preinit_2055(struct brcms_phy *pi);
-static void wlc_phy_radio_init_2055(struct brcms_phy *pi);
-static void wlc_phy_radio_postinit_2055(struct brcms_phy *pi);
-static void wlc_phy_radio_preinit_205x(struct brcms_phy *pi);
-static void wlc_phy_radio_init_2056(struct brcms_phy *pi);
-static void wlc_phy_radio_postinit_2056(struct brcms_phy *pi);
-static void wlc_phy_radio_init_2057(struct brcms_phy *pi);
-static void wlc_phy_radio_postinit_2057(struct brcms_phy *pi);
-static void wlc_phy_workarounds_nphy(struct brcms_phy *pi);
-static void wlc_phy_workarounds_nphy_gainctrl(struct brcms_phy *pi);
-static void wlc_phy_workarounds_nphy_gainctrl_2057_rev5(struct brcms_phy *pi);
-static void wlc_phy_workarounds_nphy_gainctrl_2057_rev6(struct brcms_phy *pi);
-static void wlc_phy_adjust_lnagaintbl_nphy(struct brcms_phy *pi);
-
-static void wlc_phy_restore_rssical_nphy(struct brcms_phy *pi);
-static void wlc_phy_reapply_txcal_coeffs_nphy(struct brcms_phy *pi);
-static void wlc_phy_tx_iq_war_nphy(struct brcms_phy *pi);
-static int wlc_phy_cal_rxiq_nphy_rev3(struct brcms_phy *pi,
-				      struct nphy_txgains tg, u8 type, bool d);
-static void wlc_phy_rxcal_gainctrl_nphy_rev5(struct brcms_phy *pi, u8 rxcore,
-					     u16 *rg, u8 type);
-static void wlc_phy_update_mimoconfig_nphy(struct brcms_phy *pi, s32 preamble);
-static void wlc_phy_savecal_nphy(struct brcms_phy *pi);
-static void wlc_phy_restorecal_nphy(struct brcms_phy *pi);
-static void wlc_phy_resetcca_nphy(struct brcms_phy *pi);
-
-static void wlc_phy_txpwrctrl_config_nphy(struct brcms_phy *pi);
-static void wlc_phy_internal_cal_txgain_nphy(struct brcms_phy *pi);
-static void wlc_phy_precal_txgain_nphy(struct brcms_phy *pi);
-static void wlc_phy_update_txcal_ladder_nphy(struct brcms_phy *pi, u16 core);
-
-static void wlc_phy_extpa_set_tx_digi_filts_nphy(struct brcms_phy *pi);
-static void wlc_phy_ipa_set_tx_digi_filts_nphy(struct brcms_phy *pi);
-static void wlc_phy_ipa_restore_tx_digi_filts_nphy(struct brcms_phy *pi);
-static u16 wlc_phy_ipa_get_bbmult_nphy(struct brcms_phy *pi);
-static void wlc_phy_ipa_set_bbmult_nphy(struct brcms_phy *pi, u8 m0, u8 m1);
-static u32 *wlc_phy_get_ipa_gaintbl_nphy(struct brcms_phy *pi);
-
-static void wlc_phy_a1_nphy(struct brcms_phy *pi, u8 core, u32 winsz, u32,
-			    u32 e);
-static u8 wlc_phy_a3_nphy(struct brcms_phy *pi, u8 start_gain, u8 core);
-static void wlc_phy_a2_nphy(struct brcms_phy *pi, struct nphy_ipa_txcalgains *,
-			    enum phy_cal_mode, u8);
-static void wlc_phy_papd_cal_cleanup_nphy(struct brcms_phy *pi,
-				struct nphy_papd_restore_state *state);
-static void wlc_phy_papd_cal_setup_nphy(struct brcms_phy *pi,
-				struct nphy_papd_restore_state *state, u8);
-
-static void wlc_phy_clip_det_nphy(struct brcms_phy *pi, u8 write, u16 *vals);
-
-static void wlc_phy_set_rfseq_nphy(struct brcms_phy *pi, u8 cmd, u8 *evts,
-				   u8 *dlys, u8 len);
-
-static u16 wlc_phy_read_lpf_bw_ctl_nphy(struct brcms_phy *pi, u16 offset);
-
-static void
-wlc_phy_rfctrl_override_nphy_rev7(struct brcms_phy *pi, u16 field, u16 value,
-				  u8 core_mask, u8 off,
-				  u8 override_id);
-
-static void wlc_phy_rssi_cal_nphy_rev2(struct brcms_phy *pi, u8 rssi_type);
-static void wlc_phy_rssi_cal_nphy_rev3(struct brcms_phy *pi);
-
-static bool wlc_phy_txpwr_srom_read_nphy(struct brcms_phy *pi);
-static void wlc_phy_txpwr_nphy_srom_convert(u8 *srom_max,
-					    u16 *pwr_offset,
-					    u8 tmp_max_pwr, u8 rate_start,
-					    u8 rate_end);
-
-static void wlc_phy_txpwr_limit_to_tbl_nphy(struct brcms_phy *pi);
-static void wlc_phy_txpwrctrl_coeff_setup_nphy(struct brcms_phy *pi);
-static void wlc_phy_txpwrctrl_idle_tssi_nphy(struct brcms_phy *pi);
-static void wlc_phy_txpwrctrl_pwr_setup_nphy(struct brcms_phy *pi);
-
-static bool wlc_phy_txpwr_ison_nphy(struct brcms_phy *pi);
-static u8 wlc_phy_txpwr_idx_cur_get_nphy(struct brcms_phy *pi, u8 core);
-static void wlc_phy_txpwr_idx_cur_set_nphy(struct brcms_phy *pi, u8 idx0,
-					   u8 idx1);
-static void wlc_phy_a4(struct brcms_phy *pi, bool full_cal);
-
-static u16 wlc_phy_radio205x_rcal(struct brcms_phy *pi);
-
-static u16 wlc_phy_radio2057_rccal(struct brcms_phy *pi);
-
-static u16 wlc_phy_gen_load_samples_nphy(struct brcms_phy *pi, u32 f_kHz,
-					    u16 max_val,
-					    u8 dac_test_mode);
-static void wlc_phy_loadsampletable_nphy(struct brcms_phy *pi, cs32 *tone_buf,
-					 u16 num_samps);
-static void wlc_phy_runsamples_nphy(struct brcms_phy *pi, u16 n, u16 lps,
-				    u16 wait, u8 iq, u8 dac_test_mode,
-				    bool modify_bbmult);
+	0x09, 0x0a, 0x09, 0x0a, 0x15, 0x16
+};
 
 bool wlc_phy_bist_check_phy(struct brcms_phy_pub *pih)
 {
@@ -14209,9 +14131,8 @@ bool wlc_phy_bist_check_phy(struct brcms_phy_pub *pih)
 	phybist4 = read_phy_reg(pi, 0x156);
 
 	if ((phybist0 == 0) && (phybist1 == 0x4000) && (phybist2 == 0x1fe0) &&
-	    (phybist3 == 0) && (phybist4 == 0)) {
+	    (phybist3 == 0) && (phybist4 == 0))
 		return true;
-	}
 
 	return false;
 }
@@ -14230,18 +14151,7 @@ static void wlc_phy_bphy_init_nphy(struct brcms_phy *pi)
 			val -= 0x0202;
 	}
 
-	if (NORADIO_ENAB(pi->pubpi)) {
-
-		write_phy_reg(pi, NPHY_TO_BPHY_OFF + BPHY_PHYCRSTH, 0x3206);
-
-		write_phy_reg(pi, NPHY_TO_BPHY_OFF + BPHY_RSSI_TRESH, 0x281e);
-
-		or_phy_reg(pi, NPHY_TO_BPHY_OFF + BPHY_LNA_GAIN_RANGE, 0x1a);
-
-	} else {
-
-		write_phy_reg(pi, NPHY_TO_BPHY_OFF + BPHY_STEP, 0x668);
-	}
+	write_phy_reg(pi, NPHY_TO_BPHY_OFF + BPHY_STEP, 0x668);
 }
 
 void
@@ -14307,7 +14217,8 @@ static void wlc_phy_tbl_init_nphy(struct brcms_phy *pi)
 	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
 
 		antswctrllut = CHSPEC_IS2G(pi->radio_chanspec) ?
-		    pi->srom_fem2g.antswctrllut : pi->srom_fem5g.antswctrllut;
+			       pi->srom_fem2g.antswctrllut : pi->srom_fem5g.
+			       antswctrllut;
 
 		switch (antswctrllut) {
 		case 0:
@@ -14316,20 +14227,20 @@ static void wlc_phy_tbl_init_nphy(struct brcms_phy *pi)
 
 		case 1:
 
-			if (pi->aa2g == 7) {
+			if (pi->aa2g == 7)
+				wlc_phy_table_write_nphy(
+					pi,
+					NPHY_TBL_ID_ANTSWCTRLLUT,
+					2, 0x21, 8,
+					&ant_sw_ctrl_tbl_rev8_2o3[0]);
+			else
+				wlc_phy_table_write_nphy(
+					pi,
+					NPHY_TBL_ID_ANTSWCTRLLUT,
+					2, 0x21, 8,
+					&ant_sw_ctrl_tbl_rev8
+					[0]);
 
-				wlc_phy_table_write_nphy(pi,
-							 NPHY_TBL_ID_ANTSWCTRLLUT,
-							 2, 0x21, 8,
-							 &ant_sw_ctrl_tbl_rev8_2o3
-							 [0]);
-			} else {
-				wlc_phy_table_write_nphy(pi,
-							 NPHY_TBL_ID_ANTSWCTRLLUT,
-							 2, 0x21, 8,
-							 &ant_sw_ctrl_tbl_rev8
-							 [0]);
-			}
 			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_ANTSWCTRLLUT,
 						 2, 0x25, 8,
 						 &ant_sw_ctrl_tbl_rev8[2]);
@@ -14340,31 +14251,31 @@ static void wlc_phy_tbl_init_nphy(struct brcms_phy *pi)
 
 		case 2:
 
-			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_ANTSWCTRLLUT,
-						 2, 0x1, 8,
-						 &ant_sw_ctrl_tbl_rev8_2057v7_core0
-						 [0]);
-			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_ANTSWCTRLLUT,
-						 2, 0x5, 8,
-						 &ant_sw_ctrl_tbl_rev8_2057v7_core0
-						 [2]);
-			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_ANTSWCTRLLUT,
-						 2, 0x9, 8,
-						 &ant_sw_ctrl_tbl_rev8_2057v7_core0
-						 [4]);
-
-			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_ANTSWCTRLLUT,
-						 2, 0x21, 8,
-						 &ant_sw_ctrl_tbl_rev8_2057v7_core1
-						 [0]);
-			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_ANTSWCTRLLUT,
-						 2, 0x25, 8,
-						 &ant_sw_ctrl_tbl_rev8_2057v7_core1
-						 [2]);
-			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_ANTSWCTRLLUT,
-						 2, 0x29, 8,
-						 &ant_sw_ctrl_tbl_rev8_2057v7_core1
-						 [4]);
+			wlc_phy_table_write_nphy(
+				pi, NPHY_TBL_ID_ANTSWCTRLLUT,
+				2, 0x1, 8,
+				&ant_sw_ctrl_tbl_rev8_2057v7_core0[0]);
+			wlc_phy_table_write_nphy(
+				pi, NPHY_TBL_ID_ANTSWCTRLLUT,
+				2, 0x5, 8,
+				&ant_sw_ctrl_tbl_rev8_2057v7_core0[2]);
+			wlc_phy_table_write_nphy(
+				pi, NPHY_TBL_ID_ANTSWCTRLLUT,
+				2, 0x9, 8,
+				&ant_sw_ctrl_tbl_rev8_2057v7_core0[4]);
+
+			wlc_phy_table_write_nphy(
+				pi, NPHY_TBL_ID_ANTSWCTRLLUT,
+				2, 0x21, 8,
+				&ant_sw_ctrl_tbl_rev8_2057v7_core1[0]);
+			wlc_phy_table_write_nphy(
+				pi, NPHY_TBL_ID_ANTSWCTRLLUT,
+				2, 0x25, 8,
+				&ant_sw_ctrl_tbl_rev8_2057v7_core1[2]);
+			wlc_phy_table_write_nphy(
+				pi, NPHY_TBL_ID_ANTSWCTRLLUT,
+				2, 0x29, 8,
+				&ant_sw_ctrl_tbl_rev8_2057v7_core1[4]);
 			break;
 
 		default:
@@ -14375,45 +14286,49 @@ static void wlc_phy_tbl_init_nphy(struct brcms_phy *pi)
 		for (idx = 0; idx < mimophytbl_info_sz_rev3_volatile; idx++) {
 
 			if (idx == ANT_SWCTRL_TBL_REV3_IDX) {
-				antswctrllut = CHSPEC_IS2G(pi->radio_chanspec) ?
-				    pi->srom_fem2g.antswctrllut : pi->
-				    srom_fem5g.antswctrllut;
+				antswctrllut =
+					CHSPEC_IS2G(pi->radio_chanspec) ?
+					pi->srom_fem2g.antswctrllut :
+					pi->srom_fem5g.antswctrllut;
 				switch (antswctrllut) {
 				case 0:
-					wlc_phy_write_table_nphy(pi,
-								 &mimophytbl_info_rev3_volatile
-								 [idx]);
+					wlc_phy_write_table_nphy(
+						pi,
+						&mimophytbl_info_rev3_volatile
+						[idx]);
 					break;
 				case 1:
-					wlc_phy_write_table_nphy(pi,
-								 &mimophytbl_info_rev3_volatile1
-								 [idx]);
+					wlc_phy_write_table_nphy(
+						pi,
+						&mimophytbl_info_rev3_volatile1
+						[idx]);
 					break;
 				case 2:
-					wlc_phy_write_table_nphy(pi,
-								 &mimophytbl_info_rev3_volatile2
-								 [idx]);
+					wlc_phy_write_table_nphy(
+						pi,
+						&mimophytbl_info_rev3_volatile2
+						[idx]);
 					break;
 				case 3:
-					wlc_phy_write_table_nphy(pi,
-								 &mimophytbl_info_rev3_volatile3
-								 [idx]);
+					wlc_phy_write_table_nphy(
+						pi,
+						&mimophytbl_info_rev3_volatile3
+						[idx]);
 					break;
 				default:
 					break;
 				}
 			} else {
-				wlc_phy_write_table_nphy(pi,
-							 &mimophytbl_info_rev3_volatile
-							 [idx]);
+				wlc_phy_write_table_nphy(
+					pi,
+					&mimophytbl_info_rev3_volatile[idx]);
 			}
 		}
 	} else {
-		for (idx = 0; idx < mimophytbl_info_sz_rev0_volatile; idx++) {
+		for (idx = 0; idx < mimophytbl_info_sz_rev0_volatile; idx++)
 			wlc_phy_write_table_nphy(pi,
 						 &mimophytbl_info_rev0_volatile
 						 [idx]);
-		}
 	}
 }
 
@@ -14440,32 +14355,414 @@ void wlc_phy_nphy_tkip_rifs_war(struct brcms_phy *pi, u8 rifs)
 
 	wlc_phy_write_txmacreg_nphy(pi, holdoff, delay);
 
-	if (pi && pi->sh && (pi->sh->_rifs_phy != rifs)) {
+	if (pi && pi->sh && (pi->sh->_rifs_phy != rifs))
 		pi->sh->_rifs_phy = rifs;
+}
+
+static void wlc_phy_txpwrctrl_config_nphy(struct brcms_phy *pi)
+{
+
+	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+		pi->nphy_txpwrctrl = PHY_TPC_HW_ON;
+		pi->phy_5g_pwrgain = true;
+		return;
+	}
+
+	pi->nphy_txpwrctrl = PHY_TPC_HW_OFF;
+	pi->phy_5g_pwrgain = false;
+
+	if ((pi->sh->boardflags2 & BFL2_TXPWRCTRL_EN) &&
+	    NREV_GE(pi->pubpi.phy_rev, 2) && (pi->sh->sromrev >= 4))
+		pi->nphy_txpwrctrl = PHY_TPC_HW_ON;
+	else if ((pi->sh->sromrev >= 4)
+		 && (pi->sh->boardflags2 & BFL2_5G_PWRGAIN))
+		pi->phy_5g_pwrgain = true;
+}
+
+static void wlc_phy_txpwr_srom_read_ppr_nphy(struct brcms_phy *pi)
+{
+	u16 bw40po, cddpo, stbcpo, bwduppo;
+	uint band_num;
+	struct phy_shim_info *shim = pi->sh->physhim;
+
+	if (pi->sh->sromrev >= 9)
+		return;
+
+	bw40po = (u16) wlapi_getintvar(shim, BRCMS_SROM_BW40PO);
+	pi->bw402gpo = bw40po & 0xf;
+	pi->bw405gpo = (bw40po & 0xf0) >> 4;
+	pi->bw405glpo = (bw40po & 0xf00) >> 8;
+	pi->bw405ghpo = (bw40po & 0xf000) >> 12;
+
+	cddpo = (u16) wlapi_getintvar(shim, BRCMS_SROM_CDDPO);
+	pi->cdd2gpo = cddpo & 0xf;
+	pi->cdd5gpo = (cddpo & 0xf0) >> 4;
+	pi->cdd5glpo = (cddpo & 0xf00) >> 8;
+	pi->cdd5ghpo = (cddpo & 0xf000) >> 12;
+
+	stbcpo = (u16) wlapi_getintvar(shim, BRCMS_SROM_STBCPO);
+	pi->stbc2gpo = stbcpo & 0xf;
+	pi->stbc5gpo = (stbcpo & 0xf0) >> 4;
+	pi->stbc5glpo = (stbcpo & 0xf00) >> 8;
+	pi->stbc5ghpo = (stbcpo & 0xf000) >> 12;
+
+	bwduppo = (u16) wlapi_getintvar(shim, BRCMS_SROM_BWDUPPO);
+	pi->bwdup2gpo = bwduppo & 0xf;
+	pi->bwdup5gpo = (bwduppo & 0xf0) >> 4;
+	pi->bwdup5glpo = (bwduppo & 0xf00) >> 8;
+	pi->bwdup5ghpo = (bwduppo & 0xf000) >> 12;
+
+	for (band_num = 0; band_num < (CH_2G_GROUP + CH_5G_GROUP);
+	     band_num++) {
+		switch (band_num) {
+		case 0:
+
+			pi->nphy_txpid2g[PHY_CORE_0] =
+				(u8) wlapi_getintvar(shim,
+						     BRCMS_SROM_TXPID2GA0);
+			pi->nphy_txpid2g[PHY_CORE_1] =
+				(u8) wlapi_getintvar(shim,
+						     BRCMS_SROM_TXPID2GA1);
+			pi->nphy_pwrctrl_info[PHY_CORE_0].max_pwr_2g =
+				(s8) wlapi_getintvar(shim,
+						     BRCMS_SROM_MAXP2GA0);
+			pi->nphy_pwrctrl_info[PHY_CORE_1].max_pwr_2g =
+				(s8) wlapi_getintvar(shim,
+						     BRCMS_SROM_MAXP2GA1);
+			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_2g_a1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA2GW0A0);
+			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_2g_a1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA2GW0A1);
+			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_2g_b0 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA2GW1A0);
+			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_2g_b0 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA2GW1A1);
+			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_2g_b1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA2GW2A0);
+			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_2g_b1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA2GW2A1);
+			pi->nphy_pwrctrl_info[PHY_CORE_0].idle_targ_2g =
+				(s8) wlapi_getintvar(shim, BRCMS_SROM_ITT2GA0);
+			pi->nphy_pwrctrl_info[PHY_CORE_1].idle_targ_2g =
+				(s8) wlapi_getintvar(shim, BRCMS_SROM_ITT2GA1);
+
+			pi->cck2gpo = (u16) wlapi_getintvar(shim,
+							    BRCMS_SROM_CCK2GPO);
+
+			pi->ofdm2gpo =
+				(u32) wlapi_getintvar(shim,
+						      BRCMS_SROM_OFDM2GPO);
+
+			pi->mcs2gpo[0] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS2GPO0);
+			pi->mcs2gpo[1] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS2GPO1);
+			pi->mcs2gpo[2] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS2GPO2);
+			pi->mcs2gpo[3] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS2GPO3);
+			pi->mcs2gpo[4] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS2GPO4);
+			pi->mcs2gpo[5] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS2GPO5);
+			pi->mcs2gpo[6] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS2GPO6);
+			pi->mcs2gpo[7] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS2GPO7);
+			break;
+		case 1:
+
+			pi->nphy_txpid5g[PHY_CORE_0] =
+				(u8) wlapi_getintvar(shim,
+						     BRCMS_SROM_TXPID5GA0);
+			pi->nphy_txpid5g[PHY_CORE_1] =
+				(u8) wlapi_getintvar(shim,
+						     BRCMS_SROM_TXPID5GA1);
+			pi->nphy_pwrctrl_info[PHY_CORE_0].max_pwr_5gm =
+				(s8) wlapi_getintvar(shim, BRCMS_SROM_MAXP5GA0);
+			pi->nphy_pwrctrl_info[PHY_CORE_1].max_pwr_5gm =
+				(s8) wlapi_getintvar(shim,
+						     BRCMS_SROM_MAXP5GA1);
+			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_5gm_a1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GW0A0);
+			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_5gm_a1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GW0A1);
+			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_5gm_b0 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GW1A0);
+			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_5gm_b0 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GW1A1);
+			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_5gm_b1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GW2A0);
+			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_5gm_b1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GW2A1);
+			pi->nphy_pwrctrl_info[PHY_CORE_0].idle_targ_5gm =
+				(s8) wlapi_getintvar(shim, BRCMS_SROM_ITT5GA0);
+			pi->nphy_pwrctrl_info[PHY_CORE_1].idle_targ_5gm =
+				(s8) wlapi_getintvar(shim, BRCMS_SROM_ITT5GA1);
+
+			pi->ofdm5gpo =
+				(u32) wlapi_getintvar(shim,
+						      BRCMS_SROM_OFDM5GPO);
+
+			pi->mcs5gpo[0] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GPO0);
+			pi->mcs5gpo[1] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GPO1);
+			pi->mcs5gpo[2] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GPO2);
+			pi->mcs5gpo[3] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GPO3);
+			pi->mcs5gpo[4] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GPO4);
+			pi->mcs5gpo[5] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GPO5);
+			pi->mcs5gpo[6] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GPO6);
+			pi->mcs5gpo[7] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GPO7);
+			break;
+		case 2:
+
+			pi->nphy_txpid5gl[0] =
+				(u8) wlapi_getintvar(shim,
+						     BRCMS_SROM_TXPID5GLA0);
+			pi->nphy_txpid5gl[1] =
+				(u8) wlapi_getintvar(shim,
+						     BRCMS_SROM_TXPID5GLA1);
+			pi->nphy_pwrctrl_info[0].max_pwr_5gl =
+				(s8) wlapi_getintvar(shim,
+						     BRCMS_SROM_MAXP5GLA0);
+			pi->nphy_pwrctrl_info[1].max_pwr_5gl =
+				(s8) wlapi_getintvar(shim,
+						     BRCMS_SROM_MAXP5GLA1);
+			pi->nphy_pwrctrl_info[0].pwrdet_5gl_a1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GLW0A0);
+			pi->nphy_pwrctrl_info[1].pwrdet_5gl_a1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GLW0A1);
+			pi->nphy_pwrctrl_info[0].pwrdet_5gl_b0 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GLW1A0);
+			pi->nphy_pwrctrl_info[1].pwrdet_5gl_b0 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GLW1A1);
+			pi->nphy_pwrctrl_info[0].pwrdet_5gl_b1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GLW2A0);
+			pi->nphy_pwrctrl_info[1].pwrdet_5gl_b1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GLW2A1);
+			pi->nphy_pwrctrl_info[0].idle_targ_5gl = 0;
+			pi->nphy_pwrctrl_info[1].idle_targ_5gl = 0;
+
+			pi->ofdm5glpo =
+				(u32) wlapi_getintvar(shim,
+						      BRCMS_SROM_OFDM5GLPO);
+
+			pi->mcs5glpo[0] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GLPO0);
+			pi->mcs5glpo[1] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GLPO1);
+			pi->mcs5glpo[2] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GLPO2);
+			pi->mcs5glpo[3] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GLPO3);
+			pi->mcs5glpo[4] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GLPO4);
+			pi->mcs5glpo[5] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GLPO5);
+			pi->mcs5glpo[6] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GLPO6);
+			pi->mcs5glpo[7] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GLPO7);
+			break;
+		case 3:
+
+			pi->nphy_txpid5gh[0] =
+				(u8) wlapi_getintvar(shim,
+						     BRCMS_SROM_TXPID5GHA0);
+			pi->nphy_txpid5gh[1] =
+				(u8) wlapi_getintvar(shim,
+						     BRCMS_SROM_TXPID5GHA1);
+			pi->nphy_pwrctrl_info[0].max_pwr_5gh =
+				(s8) wlapi_getintvar(shim,
+						     BRCMS_SROM_MAXP5GHA0);
+			pi->nphy_pwrctrl_info[1].max_pwr_5gh =
+				(s8) wlapi_getintvar(shim,
+						     BRCMS_SROM_MAXP5GHA1);
+			pi->nphy_pwrctrl_info[0].pwrdet_5gh_a1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GHW0A0);
+			pi->nphy_pwrctrl_info[1].pwrdet_5gh_a1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GHW0A1);
+			pi->nphy_pwrctrl_info[0].pwrdet_5gh_b0 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GHW1A0);
+			pi->nphy_pwrctrl_info[1].pwrdet_5gh_b0 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GHW1A1);
+			pi->nphy_pwrctrl_info[0].pwrdet_5gh_b1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GHW2A0);
+			pi->nphy_pwrctrl_info[1].pwrdet_5gh_b1 =
+				(s16) wlapi_getintvar(shim,
+						      BRCMS_SROM_PA5GHW2A1);
+			pi->nphy_pwrctrl_info[0].idle_targ_5gh = 0;
+			pi->nphy_pwrctrl_info[1].idle_targ_5gh = 0;
+
+			pi->ofdm5ghpo =
+				(u32) wlapi_getintvar(shim,
+						      BRCMS_SROM_OFDM5GHPO);
+
+			pi->mcs5ghpo[0] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GHPO0);
+			pi->mcs5ghpo[1] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GHPO1);
+			pi->mcs5ghpo[2] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GHPO2);
+			pi->mcs5ghpo[3] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GHPO3);
+			pi->mcs5ghpo[4] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GHPO4);
+			pi->mcs5ghpo[5] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GHPO5);
+			pi->mcs5ghpo[6] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GHPO6);
+			pi->mcs5ghpo[7] =
+				(u16) wlapi_getintvar(shim,
+						      BRCMS_SROM_MCS5GHPO7);
+			break;
+		}
 	}
+
+	wlc_phy_txpwr_apply_nphy(pi);
+}
+
+static bool wlc_phy_txpwr_srom_read_nphy(struct brcms_phy *pi)
+{
+	struct phy_shim_info *shim = pi->sh->physhim;
+
+	pi->antswitch = (u8) wlapi_getintvar(shim, BRCMS_SROM_ANTSWITCH);
+	pi->aa2g = (u8) wlapi_getintvar(shim, BRCMS_SROM_AA2G);
+	pi->aa5g = (u8) wlapi_getintvar(shim, BRCMS_SROM_AA5G);
+
+	pi->srom_fem2g.tssipos = (u8) wlapi_getintvar(shim,
+						      BRCMS_SROM_TSSIPOS2G);
+	pi->srom_fem2g.extpagain = (u8) wlapi_getintvar(shim,
+							BRCMS_SROM_EXTPAGAIN2G);
+	pi->srom_fem2g.pdetrange = (u8) wlapi_getintvar(shim,
+							BRCMS_SROM_PDETRANGE2G);
+	pi->srom_fem2g.triso = (u8) wlapi_getintvar(shim, BRCMS_SROM_TRISO2G);
+	pi->srom_fem2g.antswctrllut =
+			(u8) wlapi_getintvar(shim, BRCMS_SROM_ANTSWCTL2G);
+
+	pi->srom_fem5g.tssipos = (u8) wlapi_getintvar(shim,
+						      BRCMS_SROM_TSSIPOS5G);
+	pi->srom_fem5g.extpagain = (u8) wlapi_getintvar(shim,
+							BRCMS_SROM_EXTPAGAIN5G);
+	pi->srom_fem5g.pdetrange = (u8) wlapi_getintvar(shim,
+							BRCMS_SROM_PDETRANGE5G);
+	pi->srom_fem5g.triso = (u8) wlapi_getintvar(shim, BRCMS_SROM_TRISO5G);
+	if (wlapi_getvar(shim, BRCMS_SROM_ANTSWCTL5G))
+		pi->srom_fem5g.antswctrllut =
+			(u8) wlapi_getintvar(shim, BRCMS_SROM_ANTSWCTL5G);
+	else
+		pi->srom_fem5g.antswctrllut =
+			(u8) wlapi_getintvar(shim, BRCMS_SROM_ANTSWCTL2G);
+
+	wlc_phy_txpower_ipa_upd(pi);
+
+	pi->phy_txcore_disable_temp =
+			(s16) wlapi_getintvar(shim, BRCMS_SROM_TEMPTHRESH);
+	if (pi->phy_txcore_disable_temp == 0)
+		pi->phy_txcore_disable_temp = PHY_CHAIN_TX_DISABLE_TEMP;
+
+	pi->phy_tempsense_offset = (s8) wlapi_getintvar(shim,
+							BRCMS_SROM_TEMPOFFSET);
+	if (pi->phy_tempsense_offset != 0) {
+		if (pi->phy_tempsense_offset >
+		    (NPHY_SROM_TEMPSHIFT + NPHY_SROM_MAXTEMPOFFSET))
+			pi->phy_tempsense_offset = NPHY_SROM_MAXTEMPOFFSET;
+		else if (pi->phy_tempsense_offset < (NPHY_SROM_TEMPSHIFT +
+						     NPHY_SROM_MINTEMPOFFSET))
+			pi->phy_tempsense_offset = NPHY_SROM_MINTEMPOFFSET;
+		else
+			pi->phy_tempsense_offset -= NPHY_SROM_TEMPSHIFT;
+	}
+
+	pi->phy_txcore_enable_temp =
+		pi->phy_txcore_disable_temp - PHY_HYSTERESIS_DELTATEMP;
+
+	pi->phycal_tempdelta =
+			(u8) wlapi_getintvar(shim, BRCMS_SROM_PHYCAL_TEMPDELTA);
+	if (pi->phycal_tempdelta > NPHY_CAL_MAXTEMPDELTA)
+		pi->phycal_tempdelta = 0;
+
+	wlc_phy_txpwr_srom_read_ppr_nphy(pi);
+
+	return true;
 }
 
 bool wlc_phy_attach_nphy(struct brcms_phy *pi)
 {
 	uint i;
 
-	if (NREV_GE(pi->pubpi.phy_rev, 3) && NREV_LT(pi->pubpi.phy_rev, 6)) {
+	if (NREV_GE(pi->pubpi.phy_rev, 3) && NREV_LT(pi->pubpi.phy_rev, 6))
 		pi->phyhang_avoid = true;
-	}
 
 	if (NREV_GE(pi->pubpi.phy_rev, 3) && NREV_LT(pi->pubpi.phy_rev, 7)) {
-
 		pi->nphy_gband_spurwar_en = true;
-
-		if (pi->sh->boardflags2 & BFL2_SPUR_WAR) {
+		if (pi->sh->boardflags2 & BFL2_SPUR_WAR)
 			pi->nphy_aband_spurwar_en = true;
-		}
 	}
 	if (NREV_GE(pi->pubpi.phy_rev, 6) && NREV_LT(pi->pubpi.phy_rev, 7)) {
-
-		if (pi->sh->boardflags2 & BFL2_2G_SPUR_WAR) {
+		if (pi->sh->boardflags2 & BFL2_2G_SPUR_WAR)
 			pi->nphy_gband_spurwar2_en = true;
-		}
 	}
 
 	pi->n_preamble_override = AUTO;
@@ -14485,9 +14782,8 @@ bool wlc_phy_attach_nphy(struct brcms_phy *pi)
 	pi->nphy_elna_gain_config = false;
 	pi->radio_is_on = false;
 
-	for (i = 0; i < pi->pubpi.phy_corenum; i++) {
+	for (i = 0; i < pi->pubpi.phy_corenum; i++)
 		pi->nphy_txpwrindex[i].index = AUTO;
-	}
 
 	wlc_phy_txpwrctrl_config_nphy(pi);
 	if (pi->nphy_txpwrctrl == PHY_TPC_HW_ON)
@@ -14504,751 +14800,1405 @@ bool wlc_phy_attach_nphy(struct brcms_phy *pi)
 	return true;
 }
 
-static void wlc_phy_txpwrctrl_config_nphy(struct brcms_phy *pi)
+static s32 get_rf_pwr_offset(struct brcms_phy *pi, s16 pga_gn, s16 pad_gn)
 {
+	s32 rfpwr_offset = 0;
 
-	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-		pi->nphy_txpwrctrl = PHY_TPC_HW_ON;
-		pi->phy_5g_pwrgain = true;
-		return;
-	}
-
-	pi->nphy_txpwrctrl = PHY_TPC_HW_OFF;
-	pi->phy_5g_pwrgain = false;
-
-	if ((pi->sh->boardflags2 & BFL2_TXPWRCTRL_EN) &&
-	    NREV_GE(pi->pubpi.phy_rev, 2) && (pi->sh->sromrev >= 4))
-		pi->nphy_txpwrctrl = PHY_TPC_HW_ON;
-	else if ((pi->sh->sromrev >= 4)
-		 && (pi->sh->boardflags2 & BFL2_5G_PWRGAIN))
-		pi->phy_5g_pwrgain = true;
+	if (CHSPEC_IS2G(pi->radio_chanspec)) {
+		if ((pi->pubpi.radiorev == 3) ||
+		    (pi->pubpi.radiorev == 4) ||
+		    (pi->pubpi.radiorev == 6))
+			rfpwr_offset = (s16)
+				       nphy_papd_padgain_dlt_2g_2057rev3n4
+				       [pad_gn];
+		else if (pi->pubpi.radiorev == 5)
+			rfpwr_offset = (s16)
+				       nphy_papd_padgain_dlt_2g_2057rev5
+				       [pad_gn];
+		else if ((pi->pubpi.radiorev == 7)
+			 || (pi->pubpi.radiorev ==
+			     8))
+			rfpwr_offset = (s16)
+				       nphy_papd_padgain_dlt_2g_2057rev7
+				       [pad_gn];
+	} else {
+		if ((pi->pubpi.radiorev == 3) ||
+		    (pi->pubpi.radiorev == 4) ||
+		    (pi->pubpi.radiorev == 6))
+			rfpwr_offset = (s16)
+				       nphy_papd_pgagain_dlt_5g_2057
+				       [pga_gn];
+		else if ((pi->pubpi.radiorev == 7)
+			 || (pi->pubpi.radiorev ==
+			     8))
+			rfpwr_offset = (s16)
+				       nphy_papd_pgagain_dlt_5g_2057rev7
+				       [pga_gn];
+	}
+	return rfpwr_offset;
 }
 
-void wlc_phy_init_nphy(struct brcms_phy *pi)
+static void wlc_phy_update_mimoconfig_nphy(struct brcms_phy *pi, s32 preamble)
 {
+	bool gf_preamble = false;
 	u16 val;
-	u16 clip1_ths[2];
-	struct nphy_txgains target_gain;
-	u8 tx_pwr_ctrl_state;
-	bool do_nphy_cal = false;
-	uint core;
-	uint origidx, intr_val;
-	d11regs_t *regs;
-	u32 d11_clk_ctl_st;
 
-	core = 0;
-
-	if (!(pi->measure_hold & PHY_HOLD_FOR_SCAN)) {
-		pi->measure_hold |= PHY_HOLD_FOR_NOT_ASSOC;
-	}
+	if (preamble == BRCMS_N_PREAMBLE_GF)
+		gf_preamble = true;
 
-	if ((ISNPHY(pi)) && (NREV_GE(pi->pubpi.phy_rev, 5)) &&
-	    ((pi->sh->chippkg == BCM4717_PKG_ID) ||
-	     (pi->sh->chippkg == BCM4718_PKG_ID))) {
-		if ((pi->sh->boardflags & BFL_EXTLNA) &&
-		    (CHSPEC_IS2G(pi->radio_chanspec))) {
-			ai_corereg(pi->sh->sih, SI_CC_IDX,
-				   offsetof(chipcregs_t, chipcontrol), 0x40,
-				   0x40);
-		}
-	}
+	val = read_phy_reg(pi, 0xed);
 
-	if ((pi->nphy_gband_spurwar2_en) && CHSPEC_IS2G(pi->radio_chanspec) &&
-	    CHSPEC_IS40(pi->radio_chanspec)) {
+	val |= RX_GF_MM_AUTO;
+	val &= ~RX_GF_OR_MM;
+	if (gf_preamble)
+		val |= RX_GF_OR_MM;
 
-		regs = (d11regs_t *) ai_switch_core(pi->sh->sih, D11_CORE_ID,
-						    &origidx, &intr_val);
-		d11_clk_ctl_st = R_REG(&regs->clk_ctl_st);
-		AND_REG(&regs->clk_ctl_st,
-			~(CCS_FORCEHT | CCS_HTAREQ));
+	write_phy_reg(pi, 0xed, val);
+}
 
-		W_REG(&regs->clk_ctl_st, d11_clk_ctl_st);
+static void wlc_phy_ipa_set_tx_digi_filts_nphy(struct brcms_phy *pi)
+{
+	int j, type;
+	u16 addr_offset[] = { 0x186, 0x195, 0x2c5};
 
-		ai_restore_core(pi->sh->sih, origidx, intr_val);
+	for (type = 0; type < 3; type++) {
+		for (j = 0; j < NPHY_NUM_DIG_FILT_COEFFS; j++)
+			write_phy_reg(pi, addr_offset[type] + j,
+				      NPHY_IPA_REV4_txdigi_filtcoeffs[type][j]);
 	}
 
-	pi->use_int_tx_iqlo_cal_nphy =
-	    (PHY_IPA(pi) ||
-	     (NREV_GE(pi->pubpi.phy_rev, 7) ||
-	      (NREV_GE(pi->pubpi.phy_rev, 5)
-	       && pi->sh->boardflags2 & BFL2_INTERNDET_TXIQCAL)));
-
-	pi->internal_tx_iqlo_cal_tapoff_intpa_nphy = false;
-
-	pi->nphy_deaf_count = 0;
-
-	wlc_phy_tbl_init_nphy(pi);
-
-	pi->nphy_crsminpwr_adjusted = false;
-	pi->nphy_noisevars_adjusted = false;
-
-	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-		write_phy_reg(pi, 0xe7, 0);
-		write_phy_reg(pi, 0xec, 0);
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			write_phy_reg(pi, 0x342, 0);
-			write_phy_reg(pi, 0x343, 0);
-			write_phy_reg(pi, 0x346, 0);
-			write_phy_reg(pi, 0x347, 0);
-		}
-		write_phy_reg(pi, 0xe5, 0);
-		write_phy_reg(pi, 0xe6, 0);
+	if (pi->bw == WL_CHANSPEC_BW_40) {
+		for (j = 0; j < NPHY_NUM_DIG_FILT_COEFFS; j++)
+			write_phy_reg(pi, 0x186 + j,
+				      NPHY_IPA_REV4_txdigi_filtcoeffs[3][j]);
 	} else {
-		write_phy_reg(pi, 0xec, 0);
-	}
+		if (CHSPEC_IS5G(pi->radio_chanspec)) {
+			for (j = 0; j < NPHY_NUM_DIG_FILT_COEFFS; j++)
+				write_phy_reg(pi, 0x186 + j,
+					NPHY_IPA_REV4_txdigi_filtcoeffs[5][j]);
+		}
 
-	write_phy_reg(pi, 0x91, 0);
-	write_phy_reg(pi, 0x92, 0);
-	if (NREV_LT(pi->pubpi.phy_rev, 6)) {
-		write_phy_reg(pi, 0x93, 0);
-		write_phy_reg(pi, 0x94, 0);
+		if (CHSPEC_CHANNEL(pi->radio_chanspec) == 14) {
+			for (j = 0; j < NPHY_NUM_DIG_FILT_COEFFS; j++)
+				write_phy_reg(pi, 0x2c5 + j,
+					NPHY_IPA_REV4_txdigi_filtcoeffs[6][j]);
+		}
 	}
+}
 
-	and_phy_reg(pi, 0xa1, ~3);
+static void wlc_phy_ipa_restore_tx_digi_filts_nphy(struct brcms_phy *pi)
+{
+	int j;
 
-	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-		write_phy_reg(pi, 0x8f, 0);
-		write_phy_reg(pi, 0xa5, 0);
+	if (pi->bw == WL_CHANSPEC_BW_40) {
+		for (j = 0; j < NPHY_NUM_DIG_FILT_COEFFS; j++)
+			write_phy_reg(pi, 0x195 + j,
+				      NPHY_IPA_REV4_txdigi_filtcoeffs[4][j]);
 	} else {
-		write_phy_reg(pi, 0xa5, 0);
+		for (j = 0; j < NPHY_NUM_DIG_FILT_COEFFS; j++)
+			write_phy_reg(pi, 0x186 + j,
+				      NPHY_IPA_REV4_txdigi_filtcoeffs[3][j]);
 	}
+}
 
-	if (NREV_IS(pi->pubpi.phy_rev, 2))
-		mod_phy_reg(pi, 0xdc, 0x00ff, 0x3b);
-	else if (NREV_LT(pi->pubpi.phy_rev, 2))
-		mod_phy_reg(pi, 0xdc, 0x00ff, 0x40);
-
-	write_phy_reg(pi, 0x203, 32);
-	write_phy_reg(pi, 0x201, 32);
-
-	if (pi->sh->boardflags2 & BFL2_SKWRKFEM_BRD)
-		write_phy_reg(pi, 0x20d, 160);
-	else
-		write_phy_reg(pi, 0x20d, 184);
-
-	write_phy_reg(pi, 0x13a, 200);
-
-	write_phy_reg(pi, 0x70, 80);
-
-	write_phy_reg(pi, 0x1ff, 48);
+static void
+wlc_phy_set_rfseq_nphy(struct brcms_phy *pi, u8 cmd, u8 *events, u8 *dlys,
+		       u8 len)
+{
+	u32 t1_offset, t2_offset;
+	u8 ctr;
+	u8 end_event =
+		NREV_GE(pi->pubpi.phy_rev,
+			3) ? NPHY_REV3_RFSEQ_CMD_END : NPHY_RFSEQ_CMD_END;
+	u8 end_dly = 1;
 
-	if (NREV_LT(pi->pubpi.phy_rev, 8)) {
-		wlc_phy_update_mimoconfig_nphy(pi, pi->n_preamble_override);
-	}
+	if (pi->phyhang_avoid)
+		wlc_phy_stay_in_carriersearch_nphy(pi, true);
 
-	wlc_phy_stf_chain_upd_nphy(pi);
+	t1_offset = cmd << 4;
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ, len, t1_offset, 8,
+				 events);
+	t2_offset = t1_offset + 0x080;
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ, len, t2_offset, 8,
+				 dlys);
 
-	if (NREV_LT(pi->pubpi.phy_rev, 2)) {
-		write_phy_reg(pi, 0x180, 0xaa8);
-		write_phy_reg(pi, 0x181, 0x9a4);
+	for (ctr = len; ctr < 16; ctr++) {
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ, 1,
+					 t1_offset + ctr, 8, &end_event);
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ, 1,
+					 t2_offset + ctr, 8, &end_dly);
 	}
 
-	if (PHY_IPA(pi)) {
-		for (core = 0; core < pi->pubpi.phy_corenum; core++) {
-
-			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x297 :
-				    0x29b, (0x1 << 0), (1) << 0);
-
-			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x298 :
-				    0x29c, (0x1ff << 7),
-				    (pi->nphy_papd_epsilon_offset[core]) << 7);
+	if (pi->phyhang_avoid)
+		wlc_phy_stay_in_carriersearch_nphy(pi, false);
+}
 
-		}
+static u16 wlc_phy_read_lpf_bw_ctl_nphy(struct brcms_phy *pi, u16 offset)
+{
+	u16 lpf_bw_ctl_val = 0;
+	u16 rx2tx_lpf_rc_lut_offset = 0;
 
-		wlc_phy_ipa_set_tx_digi_filts_nphy(pi);
+	if (offset == 0) {
+		if (CHSPEC_IS40(pi->radio_chanspec))
+			rx2tx_lpf_rc_lut_offset = 0x159;
+		else
+			rx2tx_lpf_rc_lut_offset = 0x154;
 	} else {
-
-		if (NREV_GE(pi->pubpi.phy_rev, 5)) {
-			wlc_phy_extpa_set_tx_digi_filts_nphy(pi);
-		}
+		rx2tx_lpf_rc_lut_offset = offset;
 	}
+	wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_RFSEQ, 1,
+				(u32) rx2tx_lpf_rc_lut_offset, 16,
+				&lpf_bw_ctl_val);
 
-	wlc_phy_workarounds_nphy(pi);
-
-	wlapi_bmac_phyclk_fgc(pi->sh->physhim, ON);
-
-	val = read_phy_reg(pi, 0x01);
-	write_phy_reg(pi, 0x01, val | BBCFG_RESETCCA);
-	write_phy_reg(pi, 0x01, val & (~BBCFG_RESETCCA));
-	wlapi_bmac_phyclk_fgc(pi->sh->physhim, OFF);
-
-	wlapi_bmac_macphyclk_set(pi->sh->physhim, ON);
-
-	wlc_phy_pa_override_nphy(pi, OFF);
-	wlc_phy_force_rfseq_nphy(pi, NPHY_RFSEQ_RX2TX);
-	wlc_phy_force_rfseq_nphy(pi, NPHY_RFSEQ_RESET2RX);
-	wlc_phy_pa_override_nphy(pi, ON);
+	lpf_bw_ctl_val = lpf_bw_ctl_val & 0x7;
 
-	wlc_phy_classifier_nphy(pi, 0, 0);
-	wlc_phy_clip_det_nphy(pi, 0, clip1_ths);
+	return lpf_bw_ctl_val;
+}
 
-	if (CHSPEC_IS2G(pi->radio_chanspec))
-		wlc_phy_bphy_init_nphy(pi);
+static void
+wlc_phy_rfctrl_override_nphy_rev7(struct brcms_phy *pi, u16 field, u16 value,
+				  u8 core_mask, u8 off, u8 override_id)
+{
+	u8 core_num;
+	u16 addr = 0, en_addr = 0, val_addr = 0, en_mask = 0, val_mask = 0;
+	u8 val_shift = 0;
 
-	tx_pwr_ctrl_state = pi->nphy_txpwrctrl;
-	wlc_phy_txpwrctrl_enable_nphy(pi, PHY_TPC_HW_OFF);
+	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+		en_mask = field;
+		for (core_num = 0; core_num < 2; core_num++) {
+			if (override_id == NPHY_REV7_RFCTRLOVERRIDE_ID0) {
 
-	wlc_phy_txpwr_fixpower_nphy(pi);
+				switch (field) {
+				case (0x1 << 2):
+					en_addr = (core_num == 0) ? 0xe7 : 0xec;
+					val_addr = (core_num == 0) ? 0x7a :
+						   0x7d;
+					val_mask = (0x1 << 1);
+					val_shift = 1;
+					break;
+				case (0x1 << 3):
+					en_addr = (core_num == 0) ? 0xe7 : 0xec;
+					val_addr = (core_num == 0) ? 0x7a :
+						   0x7d;
+					val_mask = (0x1 << 2);
+					val_shift = 2;
+					break;
+				case (0x1 << 4):
+					en_addr = (core_num == 0) ? 0xe7 : 0xec;
+					val_addr = (core_num == 0) ? 0x7a :
+						   0x7d;
+					val_mask = (0x1 << 4);
+					val_shift = 4;
+					break;
+				case (0x1 << 5):
+					en_addr = (core_num == 0) ? 0xe7 : 0xec;
+					val_addr = (core_num == 0) ? 0x7a :
+						   0x7d;
+					val_mask = (0x1 << 5);
+					val_shift = 5;
+					break;
+				case (0x1 << 6):
+					en_addr = (core_num == 0) ? 0xe7 : 0xec;
+					val_addr = (core_num == 0) ? 0x7a :
+						   0x7d;
+					val_mask = (0x1 << 6);
+					val_shift = 6;
+					break;
+				case (0x1 << 7):
+					en_addr = (core_num == 0) ? 0xe7 : 0xec;
+					val_addr = (core_num == 0) ? 0x7a :
+						   0x7d;
+					val_mask = (0x1 << 7);
+					val_shift = 7;
+					break;
+				case (0x1 << 10):
+					en_addr = (core_num == 0) ? 0xe7 : 0xec;
+					val_addr = (core_num == 0) ? 0xf8 :
+						   0xfa;
+					val_mask = (0x7 << 4);
+					val_shift = 4;
+					break;
+				case (0x1 << 11):
+					en_addr = (core_num == 0) ? 0xe7 : 0xec;
+					val_addr = (core_num == 0) ? 0x7b :
+						   0x7e;
+					val_mask = (0xffff << 0);
+					val_shift = 0;
+					break;
+				case (0x1 << 12):
+					en_addr = (core_num == 0) ? 0xe7 : 0xec;
+					val_addr = (core_num == 0) ? 0x7c :
+						   0x7f;
+					val_mask = (0xffff << 0);
+					val_shift = 0;
+					break;
+				case (0x3 << 13):
+					en_addr = (core_num == 0) ? 0xe7 : 0xec;
+					val_addr = (core_num == 0) ? 0x348 :
+						   0x349;
+					val_mask = (0xff << 0);
+					val_shift = 0;
+					break;
+				case (0x1 << 13):
+					en_addr = (core_num == 0) ? 0xe7 : 0xec;
+					val_addr = (core_num == 0) ? 0x348 :
+						   0x349;
+					val_mask = (0xf << 0);
+					val_shift = 0;
+					break;
+				default:
+					addr = 0xffff;
+					break;
+				}
+			} else if (override_id ==
+				   NPHY_REV7_RFCTRLOVERRIDE_ID1) {
 
-	wlc_phy_txpwrctrl_idle_tssi_nphy(pi);
+				switch (field) {
+				case (0x1 << 1):
+					en_addr = (core_num == 0) ? 0x342 :
+						  0x343;
+					val_addr = (core_num == 0) ? 0x340 :
+						   0x341;
+					val_mask = (0x1 << 1);
+					val_shift = 1;
+					break;
+				case (0x1 << 3):
+					en_addr = (core_num == 0) ? 0x342 :
+						  0x343;
+					val_addr = (core_num == 0) ? 0x340 :
+						   0x341;
+					val_mask = (0x1 << 3);
+					val_shift = 3;
+					break;
+				case (0x1 << 5):
+					en_addr = (core_num == 0) ? 0x342 :
+						  0x343;
+					val_addr = (core_num == 0) ? 0x340 :
+						   0x341;
+					val_mask = (0x1 << 5);
+					val_shift = 5;
+					break;
+				case (0x1 << 4):
+					en_addr = (core_num == 0) ? 0x342 :
+						  0x343;
+					val_addr = (core_num == 0) ? 0x340 :
+						   0x341;
+					val_mask = (0x1 << 4);
+					val_shift = 4;
+					break;
+				case (0x1 << 2):
 
-	wlc_phy_txpwrctrl_pwr_setup_nphy(pi);
+					en_addr = (core_num == 0) ? 0x342 :
+						  0x343;
+					val_addr = (core_num == 0) ? 0x340 :
+						   0x341;
+					val_mask = (0x1 << 2);
+					val_shift = 2;
+					break;
+				case (0x1 << 7):
 
-	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-		u32 *tx_pwrctrl_tbl = NULL;
-		u16 idx;
-		s16 pga_gn = 0;
-		s16 pad_gn = 0;
-		s32 rfpwr_offset = 0;
+					en_addr = (core_num == 0) ? 0x342 :
+						  0x343;
+					val_addr = (core_num == 0) ? 0x340 :
+						   0x341;
+					val_mask = (0x7 << 8);
+					val_shift = 8;
+					break;
+				case (0x1 << 11):
+					en_addr = (core_num == 0) ? 0x342 :
+						  0x343;
+					val_addr = (core_num == 0) ? 0x340 :
+						   0x341;
+					val_mask = (0x1 << 14);
+					val_shift = 14;
+					break;
+				case (0x1 << 10):
+					en_addr = (core_num == 0) ? 0x342 :
+						  0x343;
+					val_addr = (core_num == 0) ? 0x340 :
+						   0x341;
+					val_mask = (0x1 << 13);
+					val_shift = 13;
+					break;
+				case (0x1 << 9):
+					en_addr = (core_num == 0) ? 0x342 :
+						  0x343;
+					val_addr = (core_num == 0) ? 0x340 :
+						   0x341;
+					val_mask = (0x1 << 12);
+					val_shift = 12;
+					break;
+				case (0x1 << 8):
+					en_addr = (core_num == 0) ? 0x342 :
+						  0x343;
+					val_addr = (core_num == 0) ? 0x340 :
+						   0x341;
+					val_mask = (0x1 << 11);
+					val_shift = 11;
+					break;
+				case (0x1 << 6):
+					en_addr = (core_num == 0) ? 0x342 :
+						  0x343;
+					val_addr = (core_num == 0) ? 0x340 :
+						   0x341;
+					val_mask = (0x1 << 6);
+					val_shift = 6;
+					break;
+				case (0x1 << 0):
+					en_addr = (core_num == 0) ? 0x342 :
+						  0x343;
+					val_addr = (core_num == 0) ? 0x340 :
+						   0x341;
+					val_mask = (0x1 << 0);
+					val_shift = 0;
+					break;
+				default:
+					addr = 0xffff;
+					break;
+				}
+			} else if (override_id ==
+				   NPHY_REV7_RFCTRLOVERRIDE_ID2) {
 
-		if (PHY_IPA(pi)) {
-			tx_pwrctrl_tbl = wlc_phy_get_ipa_gaintbl_nphy(pi);
-		} else {
-			if (CHSPEC_IS5G(pi->radio_chanspec)) {
-				if (NREV_IS(pi->pubpi.phy_rev, 3)) {
-					tx_pwrctrl_tbl =
-					    nphy_tpc_5GHz_txgain_rev3;
-				} else if (NREV_IS(pi->pubpi.phy_rev, 4)) {
-					tx_pwrctrl_tbl =
-					    (pi->srom_fem5g.extpagain == 3) ?
-					    nphy_tpc_5GHz_txgain_HiPwrEPA :
-					    nphy_tpc_5GHz_txgain_rev4;
-				} else {
-					tx_pwrctrl_tbl =
-					    nphy_tpc_5GHz_txgain_rev5;
+				switch (field) {
+				case (0x1 << 3):
+					en_addr = (core_num == 0) ? 0x346 :
+						  0x347;
+					val_addr = (core_num == 0) ? 0x344 :
+						   0x345;
+					val_mask = (0x1 << 3);
+					val_shift = 3;
+					break;
+				case (0x1 << 1):
+					en_addr = (core_num == 0) ? 0x346 :
+						  0x347;
+					val_addr = (core_num == 0) ? 0x344 :
+						   0x345;
+					val_mask = (0x1 << 1);
+					val_shift = 1;
+					break;
+				case (0x1 << 0):
+					en_addr = (core_num == 0) ? 0x346 :
+						  0x347;
+					val_addr = (core_num == 0) ? 0x344 :
+						   0x345;
+					val_mask = (0x1 << 0);
+					val_shift = 0;
+					break;
+				case (0x1 << 2):
+					en_addr = (core_num == 0) ? 0x346 :
+						  0x347;
+					val_addr = (core_num == 0) ? 0x344 :
+						   0x345;
+					val_mask = (0x1 << 2);
+					val_shift = 2;
+					break;
+				case (0x1 << 4):
+					en_addr = (core_num == 0) ? 0x346 :
+						  0x347;
+					val_addr = (core_num == 0) ? 0x344 :
+						   0x345;
+					val_mask = (0x1 << 4);
+					val_shift = 4;
+					break;
+				default:
+					addr = 0xffff;
+					break;
 				}
+			}
 
+			if (off) {
+				and_phy_reg(pi, en_addr, ~en_mask);
+				and_phy_reg(pi, val_addr, ~val_mask);
 			} else {
-				if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-					if (pi->pubpi.radiorev == 5) {
-						tx_pwrctrl_tbl =
-						    nphy_tpc_txgain_epa_2057rev5;
-					} else if (pi->pubpi.radiorev == 3) {
-						tx_pwrctrl_tbl =
-						    nphy_tpc_txgain_epa_2057rev3;
-					}
 
-				} else {
-					if (NREV_GE(pi->pubpi.phy_rev, 5) &&
-					    (pi->srom_fem2g.extpagain == 3)) {
-						tx_pwrctrl_tbl =
-						    nphy_tpc_txgain_HiPwrEPA;
-					} else {
-						tx_pwrctrl_tbl =
-						    nphy_tpc_txgain_rev3;
-					}
+				if ((core_mask == 0)
+				    || (core_mask & (1 << core_num))) {
+					or_phy_reg(pi, en_addr, en_mask);
+
+					if (addr != 0xffff)
+						mod_phy_reg(pi, val_addr,
+							    val_mask,
+							    (value <<
+							     val_shift));
 				}
 			}
 		}
+	}
+}
 
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_CORE1TXPWRCTL, 128,
-					 192, 32, tx_pwrctrl_tbl);
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_CORE2TXPWRCTL, 128,
-					 192, 32, tx_pwrctrl_tbl);
-
-		pi->nphy_gmval = (u16) ((*tx_pwrctrl_tbl >> 16) & 0x7000);
+static void wlc_phy_adjust_lnagaintbl_nphy(struct brcms_phy *pi)
+{
+	uint core;
+	int ctr;
+	s16 gain_delta[2];
+	u8 curr_channel;
+	u16 minmax_gain[2];
+	u16 regval[4];
 
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+	if (pi->phyhang_avoid)
+		wlc_phy_stay_in_carriersearch_nphy(pi, true);
 
-			for (idx = 0; idx < 128; idx++) {
-				pga_gn = (tx_pwrctrl_tbl[idx] >> 24) & 0xf;
-				pad_gn = (tx_pwrctrl_tbl[idx] >> 19) & 0x1f;
+	if (pi->nphy_gain_boost) {
+		if ((CHSPEC_IS2G(pi->radio_chanspec))) {
 
-				if (CHSPEC_IS2G(pi->radio_chanspec)) {
-					if ((pi->pubpi.radiorev == 3) ||
-					    (pi->pubpi.radiorev == 4) ||
-					    (pi->pubpi.radiorev == 6)) {
-						rfpwr_offset = (s16)
-						    nphy_papd_padgain_dlt_2g_2057rev3n4
-						    [pad_gn];
-					} else if (pi->pubpi.radiorev == 5) {
-						rfpwr_offset = (s16)
-						    nphy_papd_padgain_dlt_2g_2057rev5
-						    [pad_gn];
-					} else if ((pi->pubpi.radiorev == 7)
-						   || (pi->pubpi.radiorev ==
-						       8)) {
-						rfpwr_offset = (s16)
-						    nphy_papd_padgain_dlt_2g_2057rev7
-						    [pad_gn];
-					}
-				} else {
-					if ((pi->pubpi.radiorev == 3) ||
-					    (pi->pubpi.radiorev == 4) ||
-					    (pi->pubpi.radiorev == 6)) {
-						rfpwr_offset = (s16)
-						    nphy_papd_pgagain_dlt_5g_2057
-						    [pga_gn];
-					} else if ((pi->pubpi.radiorev == 7)
-						   || (pi->pubpi.radiorev ==
-						       8)) {
-						rfpwr_offset = (s16)
-						    nphy_papd_pgagain_dlt_5g_2057rev7
-						    [pga_gn];
-					}
-				}
-				wlc_phy_table_write_nphy(pi,
-							 NPHY_TBL_ID_CORE1TXPWRCTL,
-							 1, 576 + idx, 32,
-							 &rfpwr_offset);
-				wlc_phy_table_write_nphy(pi,
-							 NPHY_TBL_ID_CORE2TXPWRCTL,
-							 1, 576 + idx, 32,
-							 &rfpwr_offset);
-			}
+			gain_delta[0] = 6;
+			gain_delta[1] = 6;
 		} else {
 
-			for (idx = 0; idx < 128; idx++) {
-				pga_gn = (tx_pwrctrl_tbl[idx] >> 24) & 0xf;
-				if (CHSPEC_IS2G(pi->radio_chanspec)) {
-					rfpwr_offset = (s16)
-					    nphy_papd_pga_gain_delta_ipa_2g
-					    [pga_gn];
-				} else {
-					rfpwr_offset = (s16)
-					    nphy_papd_pga_gain_delta_ipa_5g
-					    [pga_gn];
-				}
-
-				wlc_phy_table_write_nphy(pi,
-							 NPHY_TBL_ID_CORE1TXPWRCTL,
-							 1, 576 + idx, 32,
-							 &rfpwr_offset);
-				wlc_phy_table_write_nphy(pi,
-							 NPHY_TBL_ID_CORE2TXPWRCTL,
-							 1, 576 + idx, 32,
-							 &rfpwr_offset);
-			}
-
+			curr_channel = CHSPEC_CHANNEL(pi->radio_chanspec);
+			gain_delta[0] =
+				(s16)
+				PHY_HW_ROUND(((nphy_lnagain_est0[0] *
+					       curr_channel) +
+					      nphy_lnagain_est0[1]), 13);
+			gain_delta[1] =
+				(s16)
+				PHY_HW_ROUND(((nphy_lnagain_est1[0] *
+					       curr_channel) +
+					      nphy_lnagain_est1[1]), 13);
 		}
 	} else {
 
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_CORE1TXPWRCTL, 128,
-					 192, 32, nphy_tpc_txgain);
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_CORE2TXPWRCTL, 128,
-					 192, 32, nphy_tpc_txgain);
+		gain_delta[0] = 0;
+		gain_delta[1] = 0;
 	}
 
-	if (pi->sh->phyrxchain != 0x3) {
-		wlc_phy_rxcore_setstate_nphy((struct brcms_phy_pub *) pi,
-					     pi->sh->phyrxchain);
+	for (core = 0; core < pi->pubpi.phy_corenum; core++) {
+		if (pi->nphy_elna_gain_config) {
+
+			regval[0] = nphy_def_lnagains[2] + gain_delta[core];
+			regval[1] = nphy_def_lnagains[3] + gain_delta[core];
+			regval[2] = nphy_def_lnagains[3] + gain_delta[core];
+			regval[3] = nphy_def_lnagains[3] + gain_delta[core];
+		} else {
+			for (ctr = 0; ctr < 4; ctr++)
+				regval[ctr] =
+					nphy_def_lnagains[ctr] +
+					gain_delta[core];
+		}
+		wlc_phy_table_write_nphy(pi, core, 4, 8, 16, regval);
+
+		minmax_gain[core] =
+			(u16) (nphy_def_lnagains[2] + gain_delta[core] + 4);
 	}
 
-	if (PHY_PERICAL_MPHASE_PENDING(pi)) {
-		wlc_phy_cal_perical_mphase_restart(pi);
+	mod_phy_reg(pi, 0x1e, (0xff << 0), (minmax_gain[0] << 0));
+	mod_phy_reg(pi, 0x34, (0xff << 0), (minmax_gain[1] << 0));
+
+	if (pi->phyhang_avoid)
+		wlc_phy_stay_in_carriersearch_nphy(pi, false);
+}
+
+static void
+wlc_phy_war_force_trsw_to_R_cliplo_nphy(struct brcms_phy *pi, u8 core)
+{
+	if (core == PHY_CORE_0) {
+		write_phy_reg(pi, 0x38, 0x4);
+		if (CHSPEC_IS2G(pi->radio_chanspec))
+			write_phy_reg(pi, 0x37, 0x0060);
+		else
+			write_phy_reg(pi, 0x37, 0x1080);
+	} else if (core == PHY_CORE_1) {
+		write_phy_reg(pi, 0x2ae, 0x4);
+		if (CHSPEC_IS2G(pi->radio_chanspec))
+			write_phy_reg(pi, 0x2ad, 0x0060);
+		else
+			write_phy_reg(pi, 0x2ad, 0x1080);
 	}
+}
 
-	if (!NORADIO_ENAB(pi->pubpi)) {
-		bool do_rssi_cal = false;
+static void wlc_phy_war_txchain_upd_nphy(struct brcms_phy *pi, u8 txchain)
+{
+	u8 txchain0, txchain1;
 
-		if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-			do_rssi_cal = (CHSPEC_IS2G(pi->radio_chanspec)) ?
-			    (pi->nphy_rssical_chanspec_2G == 0) :
-			    (pi->nphy_rssical_chanspec_5G == 0);
+	txchain0 = txchain & 0x1;
+	txchain1 = (txchain & 0x2) >> 1;
+	if (!txchain0)
+		wlc_phy_war_force_trsw_to_R_cliplo_nphy(pi, PHY_CORE_0);
 
-			if (do_rssi_cal) {
-				wlc_phy_rssi_cal_nphy(pi);
-			} else {
-				wlc_phy_restore_rssical_nphy(pi);
-			}
-		} else {
-			wlc_phy_rssi_cal_nphy(pi);
-		}
+	if (!txchain1)
+		wlc_phy_war_force_trsw_to_R_cliplo_nphy(pi, PHY_CORE_1);
+}
 
-		if (!SCAN_RM_IN_PROGRESS(pi)) {
-			do_nphy_cal = (CHSPEC_IS2G(pi->radio_chanspec)) ?
-			    (pi->nphy_iqcal_chanspec_2G == 0) :
-			    (pi->nphy_iqcal_chanspec_5G == 0);
-		}
+static void wlc_phy_workarounds_nphy_gainctrl_2057_rev5(struct brcms_phy *pi)
+{
+	s8 lna1_gain_db[] = { 8, 13, 17, 22 };
+	s8 lna2_gain_db[] = { -2, 7, 11, 15 };
+	s8 tia_gain_db[] = { -4, -1, 2, 5, 5, 5, 5, 5, 5, 5 };
+	s8 tia_gainbits[] = {
+		0x0, 0x01, 0x02, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 };
 
-		if (!pi->do_initcal)
-			do_nphy_cal = false;
+	mod_phy_reg(pi, 0x1c, (0x1 << 13), (1 << 13));
+	mod_phy_reg(pi, 0x32, (0x1 << 13), (1 << 13));
 
-		if (do_nphy_cal) {
+	mod_phy_reg(pi, 0x289, (0xff << 0), (0x46 << 0));
 
-			target_gain = wlc_phy_get_tx_gain_nphy(pi);
+	mod_phy_reg(pi, 0x283, (0xff << 0), (0x3c << 0));
+	mod_phy_reg(pi, 0x280, (0xff << 0), (0x3c << 0));
 
-			if (pi->antsel_type == ANTSEL_2x3)
-				wlc_phy_antsel_init((struct brcms_phy_pub *) pi,
-						    true);
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 4, 0x8, 8,
+				 lna1_gain_db);
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 4, 0x8, 8,
+				 lna1_gain_db);
 
-			if (pi->nphy_perical != PHY_PERICAL_MPHASE) {
-				wlc_phy_rssi_cal_nphy(pi);
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 4, 0x10, 8,
+				 lna2_gain_db);
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 4, 0x10, 8,
+				 lna2_gain_db);
 
-				if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-					pi->nphy_cal_orig_pwr_idx[0] =
-					    pi->nphy_txpwrindex[PHY_CORE_0].
-					    index_internal;
-					pi->nphy_cal_orig_pwr_idx[1] =
-					    pi->nphy_txpwrindex[PHY_CORE_1].
-					    index_internal;
-
-					wlc_phy_precal_txgain_nphy(pi);
-					target_gain =
-					    wlc_phy_get_tx_gain_nphy(pi);
-				}
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 10, 0x20, 8,
+				 tia_gain_db);
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 10, 0x20, 8,
+				 tia_gain_db);
 
-				if (wlc_phy_cal_txiqlo_nphy
-				    (pi, target_gain, true, false) == 0) {
-					if (wlc_phy_cal_rxiq_nphy
-					    (pi, target_gain, 2,
-					     false) == 0) {
-						wlc_phy_savecal_nphy(pi);
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS1, 10, 0x20, 8,
+				 tia_gainbits);
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS2, 10, 0x20, 8,
+				 tia_gainbits);
 
-					}
-				}
-			} else if (pi->mphase_cal_phase_id ==
-				   MPHASE_CAL_STATE_IDLE) {
+	write_phy_reg(pi, 0x37, 0x74);
+	write_phy_reg(pi, 0x2ad, 0x74);
+	write_phy_reg(pi, 0x38, 0x18);
+	write_phy_reg(pi, 0x2ae, 0x18);
 
-				wlc_phy_cal_perical((struct brcms_phy_pub *) pi,
-						    PHY_PERICAL_PHYINIT);
-			}
-		} else {
-			wlc_phy_restorecal_nphy(pi);
-		}
-	}
+	write_phy_reg(pi, 0x2b, 0xe8);
+	write_phy_reg(pi, 0x41, 0xe8);
 
-	wlc_phy_txpwrctrl_coeff_setup_nphy(pi);
+	if (CHSPEC_IS20(pi->radio_chanspec)) {
 
-	wlc_phy_txpwrctrl_enable_nphy(pi, tx_pwr_ctrl_state);
+		mod_phy_reg(pi, 0x300, (0x3f << 0), (0x12 << 0));
+		mod_phy_reg(pi, 0x301, (0x3f << 0), (0x12 << 0));
+	} else {
 
-	wlc_phy_nphy_tkip_rifs_war(pi, pi->sh->_rifs_phy);
+		mod_phy_reg(pi, 0x300, (0x3f << 0), (0x10 << 0));
+		mod_phy_reg(pi, 0x301, (0x3f << 0), (0x10 << 0));
+	}
+}
 
-	if (NREV_GE(pi->pubpi.phy_rev, 3) && NREV_LE(pi->pubpi.phy_rev, 6))
+static void wlc_phy_workarounds_nphy_gainctrl_2057_rev6(struct brcms_phy *pi)
+{
+	u16 currband;
+	s8 lna1G_gain_db_rev7[] = { 9, 14, 19, 24 };
+	s8 *lna1_gain_db = NULL;
+	s8 *lna1_gain_db_2 = NULL;
+	s8 *lna2_gain_db = NULL;
+	s8 tiaA_gain_db_rev7[] = { -9, -6, -3, 0, 3, 3, 3, 3, 3, 3 };
+	s8 *tia_gain_db;
+	s8 tiaA_gainbits_rev7[] = { 0, 1, 2, 3, 4, 4, 4, 4, 4, 4 };
+	s8 *tia_gainbits;
+	u16 rfseqA_init_gain_rev7[] = { 0x624f, 0x624f };
+	u16 *rfseq_init_gain;
+	u16 init_gaincode;
+	u16 clip1hi_gaincode;
+	u16 clip1md_gaincode = 0;
+	u16 clip1md_gaincode_B;
+	u16 clip1lo_gaincode;
+	u16 clip1lo_gaincode_B;
+	u8 crsminl_th = 0;
+	u8 crsminu_th;
+	u16 nbclip_th = 0;
+	u8 w1clip_th;
+	u16 freq;
+	s8 nvar_baseline_offset0 = 0, nvar_baseline_offset1 = 0;
+	u8 chg_nbclip_th = 0;
 
-		write_phy_reg(pi, 0x70, 50);
+	mod_phy_reg(pi, 0x1c, (0x1 << 13), (1 << 13));
+	mod_phy_reg(pi, 0x32, (0x1 << 13), (1 << 13));
 
-	wlc_phy_txlpfbw_nphy(pi);
+	currband = read_phy_reg(pi, 0x09) & NPHY_BandControl_currentBand;
+	if (currband == 0) {
 
-	wlc_phy_spurwar_nphy(pi);
+		lna1_gain_db = lna1G_gain_db_rev7;
 
-}
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 4, 8, 8,
+					 lna1_gain_db);
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 4, 8, 8,
+					 lna1_gain_db);
 
-static void wlc_phy_update_mimoconfig_nphy(struct brcms_phy *pi, s32 preamble)
-{
-	bool gf_preamble = false;
-	u16 val;
+		mod_phy_reg(pi, 0x283, (0xff << 0), (0x40 << 0));
 
-	if (preamble == BRCMS_N_PREAMBLE_GF)
-		gf_preamble = true;
+		if (CHSPEC_IS40(pi->radio_chanspec)) {
+			mod_phy_reg(pi, 0x280, (0xff << 0), (0x3e << 0));
+			mod_phy_reg(pi, 0x283, (0xff << 0), (0x3e << 0));
+		}
 
-	val = read_phy_reg(pi, 0xed);
+		mod_phy_reg(pi, 0x289, (0xff << 0), (0x46 << 0));
 
-	val |= RX_GF_MM_AUTO;
-	val &= ~RX_GF_OR_MM;
-	if (gf_preamble)
-		val |= RX_GF_OR_MM;
+		if (CHSPEC_IS20(pi->radio_chanspec)) {
+			mod_phy_reg(pi, 0x300, (0x3f << 0), (13 << 0));
+			mod_phy_reg(pi, 0x301, (0x3f << 0), (13 << 0));
+		}
+	} else {
 
-	write_phy_reg(pi, 0xed, val);
-}
+		init_gaincode = 0x9e;
+		clip1hi_gaincode = 0x9e;
+		clip1md_gaincode_B = 0x24;
+		clip1lo_gaincode = 0x8a;
+		clip1lo_gaincode_B = 8;
+		rfseq_init_gain = rfseqA_init_gain_rev7;
 
-static void wlc_phy_resetcca_nphy(struct brcms_phy *pi)
-{
-	u16 val;
+		tia_gain_db = tiaA_gain_db_rev7;
+		tia_gainbits = tiaA_gainbits_rev7;
 
-	wlapi_bmac_phyclk_fgc(pi->sh->physhim, ON);
+		freq = CHAN5G_FREQ(CHSPEC_CHANNEL(pi->radio_chanspec));
+		if (CHSPEC_IS20(pi->radio_chanspec)) {
 
-	val = read_phy_reg(pi, 0x01);
-	write_phy_reg(pi, 0x01, val | BBCFG_RESETCCA);
-	udelay(1);
-	write_phy_reg(pi, 0x01, val & (~BBCFG_RESETCCA));
+			w1clip_th = 25;
+			clip1md_gaincode = 0x82;
 
-	wlapi_bmac_phyclk_fgc(pi->sh->physhim, OFF);
+			if ((freq <= 5080) || (freq == 5825)) {
 
-	wlc_phy_force_rfseq_nphy(pi, NPHY_RFSEQ_RESET2RX);
-}
+				s8 lna1A_gain_db_rev7[] = { 11, 16, 20, 24 };
+				s8 lna1A_gain_db_2_rev7[] = {
+					11, 17, 22, 25};
+				s8 lna2A_gain_db_rev7[] = { -1, 6, 10, 14 };
 
-void wlc_phy_pa_override_nphy(struct brcms_phy *pi, bool en)
-{
-	u16 rfctrlintc_override_val;
+				crsminu_th = 0x3e;
+				lna1_gain_db = lna1A_gain_db_rev7;
+				lna1_gain_db_2 = lna1A_gain_db_2_rev7;
+				lna2_gain_db = lna2A_gain_db_rev7;
+			} else if ((freq >= 5500) && (freq <= 5700)) {
 
-	if (!en) {
+				s8 lna1A_gain_db_rev7[] = { 11, 17, 21, 25 };
+				s8 lna1A_gain_db_2_rev7[] = {
+					12, 18, 22, 26};
+				s8 lna2A_gain_db_rev7[] = { 1, 8, 12, 16 };
 
-		pi->rfctrlIntc1_save = read_phy_reg(pi, 0x91);
-		pi->rfctrlIntc2_save = read_phy_reg(pi, 0x92);
+				crsminu_th = 0x45;
+				clip1md_gaincode_B = 0x14;
+				nbclip_th = 0xff;
+				chg_nbclip_th = 1;
+				lna1_gain_db = lna1A_gain_db_rev7;
+				lna1_gain_db_2 = lna1A_gain_db_2_rev7;
+				lna2_gain_db = lna2A_gain_db_rev7;
+			} else {
 
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			rfctrlintc_override_val = 0x1480;
-		} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-			rfctrlintc_override_val =
-			    CHSPEC_IS5G(pi->radio_chanspec) ? 0x600 : 0x480;
+				s8 lna1A_gain_db_rev7[] = { 12, 18, 22, 26 };
+				s8 lna1A_gain_db_2_rev7[] = {
+					12, 18, 22, 26};
+				s8 lna2A_gain_db_rev7[] = { -1, 6, 10, 14 };
+
+				crsminu_th = 0x41;
+				lna1_gain_db = lna1A_gain_db_rev7;
+				lna1_gain_db_2 = lna1A_gain_db_2_rev7;
+				lna2_gain_db = lna2A_gain_db_rev7;
+			}
+
+			if (freq <= 4920) {
+				nvar_baseline_offset0 = 5;
+				nvar_baseline_offset1 = 5;
+			} else if ((freq > 4920) && (freq <= 5320)) {
+				nvar_baseline_offset0 = 3;
+				nvar_baseline_offset1 = 5;
+			} else if ((freq > 5320) && (freq <= 5700)) {
+				nvar_baseline_offset0 = 3;
+				nvar_baseline_offset1 = 2;
+			} else {
+				nvar_baseline_offset0 = 4;
+				nvar_baseline_offset1 = 0;
+			}
 		} else {
-			rfctrlintc_override_val =
-			    CHSPEC_IS5G(pi->radio_chanspec) ? 0x180 : 0x120;
+
+			crsminu_th = 0x3a;
+			crsminl_th = 0x3a;
+			w1clip_th = 20;
+
+			if ((freq >= 4920) && (freq <= 5320)) {
+				nvar_baseline_offset0 = 4;
+				nvar_baseline_offset1 = 5;
+			} else if ((freq > 5320) && (freq <= 5550)) {
+				nvar_baseline_offset0 = 4;
+				nvar_baseline_offset1 = 2;
+			} else {
+				nvar_baseline_offset0 = 5;
+				nvar_baseline_offset1 = 3;
+			}
 		}
 
-		write_phy_reg(pi, 0x91, rfctrlintc_override_val);
-		write_phy_reg(pi, 0x92, rfctrlintc_override_val);
-	} else {
+		write_phy_reg(pi, 0x20, init_gaincode);
+		write_phy_reg(pi, 0x2a7, init_gaincode);
 
-		write_phy_reg(pi, 0x91, pi->rfctrlIntc1_save);
-		write_phy_reg(pi, 0x92, pi->rfctrlIntc2_save);
-	}
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ,
+					 pi->pubpi.phy_corenum, 0x106, 16,
+					 rfseq_init_gain);
 
-}
+		write_phy_reg(pi, 0x22, clip1hi_gaincode);
+		write_phy_reg(pi, 0x2a9, clip1hi_gaincode);
 
-void wlc_phy_stf_chain_upd_nphy(struct brcms_phy *pi)
-{
+		write_phy_reg(pi, 0x36, clip1md_gaincode_B);
+		write_phy_reg(pi, 0x2ac, clip1md_gaincode_B);
 
-	u16 txrx_chain =
-	    (NPHY_RfseqCoreActv_TxRxChain0 | NPHY_RfseqCoreActv_TxRxChain1);
-	bool CoreActv_override = false;
+		write_phy_reg(pi, 0x37, clip1lo_gaincode);
+		write_phy_reg(pi, 0x2ad, clip1lo_gaincode);
+		write_phy_reg(pi, 0x38, clip1lo_gaincode_B);
+		write_phy_reg(pi, 0x2ae, clip1lo_gaincode_B);
 
-	if (pi->nphy_txrx_chain == BRCMS_N_TXRX_CHAIN0) {
-		txrx_chain = NPHY_RfseqCoreActv_TxRxChain0;
-		CoreActv_override = true;
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 10, 0x20, 8,
+					 tia_gain_db);
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 10, 0x20, 8,
+					 tia_gain_db);
 
-		if (NREV_LE(pi->pubpi.phy_rev, 2)) {
-			and_phy_reg(pi, 0xa0, ~0x20);
-		}
-	} else if (pi->nphy_txrx_chain == BRCMS_N_TXRX_CHAIN1) {
-		txrx_chain = NPHY_RfseqCoreActv_TxRxChain1;
-		CoreActv_override = true;
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS1, 10, 0x20, 8,
+					 tia_gainbits);
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS2, 10, 0x20, 8,
+					 tia_gainbits);
 
-		if (NREV_LE(pi->pubpi.phy_rev, 2)) {
-			or_phy_reg(pi, 0xa0, 0x20);
+		mod_phy_reg(pi, 0x283, (0xff << 0), (crsminu_th << 0));
+
+		if (chg_nbclip_th == 1) {
+			write_phy_reg(pi, 0x2b, nbclip_th);
+			write_phy_reg(pi, 0x41, nbclip_th);
 		}
-	}
 
-	mod_phy_reg(pi, 0xa2, ((0xf << 0) | (0xf << 4)), txrx_chain);
+		mod_phy_reg(pi, 0x300, (0x3f << 0), (w1clip_th << 0));
+		mod_phy_reg(pi, 0x301, (0x3f << 0), (w1clip_th << 0));
 
-	if (CoreActv_override) {
+		mod_phy_reg(pi, 0x2e4,
+			    (0x3f << 0), (nvar_baseline_offset0 << 0));
 
-		pi->nphy_perical = PHY_PERICAL_DISABLE;
-		or_phy_reg(pi, 0xa1, NPHY_RfseqMode_CoreActv_override);
-	} else {
-		pi->nphy_perical = PHY_PERICAL_MPHASE;
-		and_phy_reg(pi, 0xa1, ~NPHY_RfseqMode_CoreActv_override);
+		mod_phy_reg(pi, 0x2e4,
+			    (0x3f << 6), (nvar_baseline_offset1 << 6));
+
+		if (CHSPEC_IS20(pi->radio_chanspec)) {
+
+			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 4, 8, 8,
+						 lna1_gain_db);
+			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 4, 8, 8,
+						 lna1_gain_db_2);
+
+			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 4, 0x10,
+						 8, lna2_gain_db);
+			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 4, 0x10,
+						 8, lna2_gain_db);
+
+			write_phy_reg(pi, 0x24, clip1md_gaincode);
+			write_phy_reg(pi, 0x2ab, clip1md_gaincode);
+		} else {
+			mod_phy_reg(pi, 0x280, (0xff << 0), (crsminl_th << 0));
+		}
 	}
 }
 
-void wlc_phy_rxcore_setstate_nphy(struct brcms_phy_pub *pih, u8 rxcore_bitmask)
+static void wlc_phy_workarounds_nphy_gainctrl(struct brcms_phy *pi)
 {
-	u16 regval;
-	u16 tbl_buf[16];
-	uint i;
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
-	u16 tbl_opcode;
-	bool suspend;
+	u16 w1th, hpf_code, currband;
+	int ctr;
+	u8 rfseq_updategainu_events[] = {
+		NPHY_RFSEQ_CMD_RX_GAIN,
+		NPHY_RFSEQ_CMD_CLR_HIQ_DIS,
+		NPHY_RFSEQ_CMD_SET_HPF_BW
+	};
+	u8 rfseq_updategainu_dlys[] = { 10, 30, 1 };
+	s8 lna1G_gain_db[] = { 7, 11, 16, 23 };
+	s8 lna1G_gain_db_rev4[] = { 8, 12, 17, 25 };
+	s8 lna1G_gain_db_rev5[] = { 9, 13, 18, 26 };
+	s8 lna1G_gain_db_rev6[] = { 8, 13, 18, 25 };
+	s8 lna1G_gain_db_rev6_224B0[] = { 10, 14, 19, 27 };
+	s8 lna1A_gain_db[] = { 7, 11, 17, 23 };
+	s8 lna1A_gain_db_rev4[] = { 8, 12, 18, 23 };
+	s8 lna1A_gain_db_rev5[] = { 6, 10, 16, 21 };
+	s8 lna1A_gain_db_rev6[] = { 6, 10, 16, 21 };
+	s8 *lna1_gain_db = NULL;
+	s8 lna2G_gain_db[] = { -5, 6, 10, 14 };
+	s8 lna2G_gain_db_rev5[] = { -3, 7, 11, 16 };
+	s8 lna2G_gain_db_rev6[] = { -5, 6, 10, 14 };
+	s8 lna2G_gain_db_rev6_224B0[] = { -5, 6, 10, 15 };
+	s8 lna2A_gain_db[] = { -6, 2, 6, 10 };
+	s8 lna2A_gain_db_rev4[] = { -5, 2, 6, 10 };
+	s8 lna2A_gain_db_rev5[] = { -7, 0, 4, 8 };
+	s8 lna2A_gain_db_rev6[] = { -7, 0, 4, 8 };
+	s8 *lna2_gain_db = NULL;
+	s8 tiaG_gain_db[] = {
+		0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A };
+	s8 tiaA_gain_db[] = {
+		0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13 };
+	s8 tiaA_gain_db_rev4[] = {
+		0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d };
+	s8 tiaA_gain_db_rev5[] = {
+		0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d };
+	s8 tiaA_gain_db_rev6[] = {
+		0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d };
+	s8 *tia_gain_db;
+	s8 tiaG_gainbits[] = {
+		0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 };
+	s8 tiaA_gainbits[] = {
+		0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06 };
+	s8 tiaA_gainbits_rev4[] = {
+		0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04 };
+	s8 tiaA_gainbits_rev5[] = {
+		0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04 };
+	s8 tiaA_gainbits_rev6[] = {
+		0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04 };
+	s8 *tia_gainbits;
+	s8 lpf_gain_db[] = { 0x00, 0x06, 0x0c, 0x12, 0x12, 0x12 };
+	s8 lpf_gainbits[] = { 0x00, 0x01, 0x02, 0x03, 0x03, 0x03 };
+	u16 rfseqG_init_gain[] = { 0x613f, 0x613f, 0x613f, 0x613f };
+	u16 rfseqG_init_gain_rev4[] = { 0x513f, 0x513f, 0x513f, 0x513f };
+	u16 rfseqG_init_gain_rev5[] = { 0x413f, 0x413f, 0x413f, 0x413f };
+	u16 rfseqG_init_gain_rev5_elna[] = {
+		0x013f, 0x013f, 0x013f, 0x013f };
+	u16 rfseqG_init_gain_rev6[] = { 0x513f, 0x513f };
+	u16 rfseqG_init_gain_rev6_224B0[] = { 0x413f, 0x413f };
+	u16 rfseqG_init_gain_rev6_elna[] = { 0x113f, 0x113f };
+	u16 rfseqA_init_gain[] = { 0x516f, 0x516f, 0x516f, 0x516f };
+	u16 rfseqA_init_gain_rev4[] = { 0x614f, 0x614f, 0x614f, 0x614f };
+	u16 rfseqA_init_gain_rev4_elna[] = {
+		0x314f, 0x314f, 0x314f, 0x314f };
+	u16 rfseqA_init_gain_rev5[] = { 0x714f, 0x714f, 0x714f, 0x714f };
+	u16 rfseqA_init_gain_rev6[] = { 0x714f, 0x714f };
+	u16 *rfseq_init_gain;
+	u16 initG_gaincode = 0x627e;
+	u16 initG_gaincode_rev4 = 0x527e;
+	u16 initG_gaincode_rev5 = 0x427e;
+	u16 initG_gaincode_rev5_elna = 0x027e;
+	u16 initG_gaincode_rev6 = 0x527e;
+	u16 initG_gaincode_rev6_224B0 = 0x427e;
+	u16 initG_gaincode_rev6_elna = 0x127e;
+	u16 initA_gaincode = 0x52de;
+	u16 initA_gaincode_rev4 = 0x629e;
+	u16 initA_gaincode_rev4_elna = 0x329e;
+	u16 initA_gaincode_rev5 = 0x729e;
+	u16 initA_gaincode_rev6 = 0x729e;
+	u16 init_gaincode;
+	u16 clip1hiG_gaincode = 0x107e;
+	u16 clip1hiG_gaincode_rev4 = 0x007e;
+	u16 clip1hiG_gaincode_rev5 = 0x1076;
+	u16 clip1hiG_gaincode_rev6 = 0x007e;
+	u16 clip1hiA_gaincode = 0x00de;
+	u16 clip1hiA_gaincode_rev4 = 0x029e;
+	u16 clip1hiA_gaincode_rev5 = 0x029e;
+	u16 clip1hiA_gaincode_rev6 = 0x029e;
+	u16 clip1hi_gaincode;
+	u16 clip1mdG_gaincode = 0x0066;
+	u16 clip1mdA_gaincode = 0x00ca;
+	u16 clip1mdA_gaincode_rev4 = 0x1084;
+	u16 clip1mdA_gaincode_rev5 = 0x2084;
+	u16 clip1mdA_gaincode_rev6 = 0x2084;
+	u16 clip1md_gaincode = 0;
+	u16 clip1loG_gaincode = 0x0074;
+	u16 clip1loG_gaincode_rev5[] = {
+		0x0062, 0x0064, 0x006a, 0x106a, 0x106c, 0x1074, 0x107c, 0x207c
+	};
+	u16 clip1loG_gaincode_rev6[] = {
+		0x106a, 0x106c, 0x1074, 0x107c, 0x007e, 0x107e, 0x207e, 0x307e
+	};
+	u16 clip1loG_gaincode_rev6_224B0 = 0x1074;
+	u16 clip1loA_gaincode = 0x00cc;
+	u16 clip1loA_gaincode_rev4 = 0x0086;
+	u16 clip1loA_gaincode_rev5 = 0x2086;
+	u16 clip1loA_gaincode_rev6 = 0x2086;
+	u16 clip1lo_gaincode;
+	u8 crsminG_th = 0x18;
+	u8 crsminG_th_rev5 = 0x18;
+	u8 crsminG_th_rev6 = 0x18;
+	u8 crsminA_th = 0x1e;
+	u8 crsminA_th_rev4 = 0x24;
+	u8 crsminA_th_rev5 = 0x24;
+	u8 crsminA_th_rev6 = 0x24;
+	u8 crsmin_th;
+	u8 crsminlG_th = 0x18;
+	u8 crsminlG_th_rev5 = 0x18;
+	u8 crsminlG_th_rev6 = 0x18;
+	u8 crsminlA_th = 0x1e;
+	u8 crsminlA_th_rev4 = 0x24;
+	u8 crsminlA_th_rev5 = 0x24;
+	u8 crsminlA_th_rev6 = 0x24;
+	u8 crsminl_th = 0;
+	u8 crsminuG_th = 0x18;
+	u8 crsminuG_th_rev5 = 0x18;
+	u8 crsminuG_th_rev6 = 0x18;
+	u8 crsminuA_th = 0x1e;
+	u8 crsminuA_th_rev4 = 0x24;
+	u8 crsminuA_th_rev5 = 0x24;
+	u8 crsminuA_th_rev6 = 0x24;
+	u8 crsminuA_th_rev6_224B0 = 0x2d;
+	u8 crsminu_th;
+	u16 nbclipG_th = 0x20d;
+	u16 nbclipG_th_rev4 = 0x1a1;
+	u16 nbclipG_th_rev5 = 0x1d0;
+	u16 nbclipG_th_rev6 = 0x1d0;
+	u16 nbclipA_th = 0x1a1;
+	u16 nbclipA_th_rev4 = 0x107;
+	u16 nbclipA_th_rev5 = 0x0a9;
+	u16 nbclipA_th_rev6 = 0x0f0;
+	u16 nbclip_th = 0;
+	u8 w1clipG_th = 5;
+	u8 w1clipG_th_rev5 = 9;
+	u8 w1clipG_th_rev6 = 5;
+	u8 w1clipA_th = 25, w1clip_th;
+	u8 rssi_gain_default = 0x50;
+	u8 rssiG_gain_rev6_224B0 = 0x50;
+	u8 rssiA_gain_rev5 = 0x90;
+	u8 rssiA_gain_rev6 = 0x90;
+	u8 rssi_gain;
+	u16 regval[21];
+	u8 triso;
 
-	pi->sh->phyrxchain = rxcore_bitmask;
+	triso = (CHSPEC_IS5G(pi->radio_chanspec)) ? pi->srom_fem5g.triso :
+		pi->srom_fem2g.triso;
 
-	if (!pi->sh->clk)
-		return;
+	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+		if (pi->pubpi.radiorev == 5) {
+			wlc_phy_workarounds_nphy_gainctrl_2057_rev5(pi);
+		} else if (pi->pubpi.radiorev == 7) {
+			wlc_phy_workarounds_nphy_gainctrl_2057_rev6(pi);
 
-	suspend =
-	    (0 == (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
-	if (!suspend)
-		wlapi_suspend_mac_and_wait(pi->sh->physhim);
+			mod_phy_reg(pi, 0x283, (0xff << 0), (0x44 << 0));
+			mod_phy_reg(pi, 0x280, (0xff << 0), (0x44 << 0));
 
-	if (pi->phyhang_avoid)
-		wlc_phy_stay_in_carriersearch_nphy(pi, true);
+		} else if ((pi->pubpi.radiorev == 3)
+			   || (pi->pubpi.radiorev == 8)) {
+			wlc_phy_workarounds_nphy_gainctrl_2057_rev6(pi);
 
-	regval = read_phy_reg(pi, 0xa2);
-	regval &= ~(0xf << 4);
-	regval |= ((u16) (rxcore_bitmask & 0x3)) << 4;
-	write_phy_reg(pi, 0xa2, regval);
+			if (pi->pubpi.radiorev == 8) {
+				mod_phy_reg(pi, 0x283,
+					    (0xff << 0), (0x44 << 0));
+				mod_phy_reg(pi, 0x280,
+					    (0xff << 0), (0x44 << 0));
+			}
+		} else {
+			wlc_phy_workarounds_nphy_gainctrl_2057_rev6(pi);
+		}
+	} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
 
-	if ((rxcore_bitmask & 0x3) != 0x3) {
+		mod_phy_reg(pi, 0xa0, (0x1 << 6), (1 << 6));
 
-		write_phy_reg(pi, 0x20e, 1);
+		mod_phy_reg(pi, 0x1c, (0x1 << 13), (1 << 13));
+		mod_phy_reg(pi, 0x32, (0x1 << 13), (1 << 13));
 
-		if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-			if (pi->rx2tx_biasentry == -1) {
-				wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_RFSEQ,
-							ARRAY_SIZE(tbl_buf), 80,
-							16, tbl_buf);
+		currband =
+			read_phy_reg(pi, 0x09) & NPHY_BandControl_currentBand;
+		if (currband == 0) {
+			if (NREV_GE(pi->pubpi.phy_rev, 6)) {
+				if (pi->pubpi.radiorev == 11) {
+					lna1_gain_db = lna1G_gain_db_rev6_224B0;
+					lna2_gain_db = lna2G_gain_db_rev6_224B0;
+					rfseq_init_gain =
+						rfseqG_init_gain_rev6_224B0;
+					init_gaincode =
+						initG_gaincode_rev6_224B0;
+					clip1hi_gaincode =
+						clip1hiG_gaincode_rev6;
+					clip1lo_gaincode =
+						clip1loG_gaincode_rev6_224B0;
+					nbclip_th = nbclipG_th_rev6;
+					w1clip_th = w1clipG_th_rev6;
+					crsmin_th = crsminG_th_rev6;
+					crsminl_th = crsminlG_th_rev6;
+					crsminu_th = crsminuG_th_rev6;
+					rssi_gain = rssiG_gain_rev6_224B0;
+				} else {
+					lna1_gain_db = lna1G_gain_db_rev6;
+					lna2_gain_db = lna2G_gain_db_rev6;
+					if (pi->sh->boardflags & BFL_EXTLNA) {
 
-				for (i = 0; i < ARRAY_SIZE(tbl_buf); i++) {
-					if (tbl_buf[i] ==
-					    NPHY_REV3_RFSEQ_CMD_CLR_RXRX_BIAS) {
+						rfseq_init_gain =
+						     rfseqG_init_gain_rev6_elna;
+						init_gaincode =
+						       initG_gaincode_rev6_elna;
+					} else {
+						rfseq_init_gain =
+							rfseqG_init_gain_rev6;
+						init_gaincode =
+							initG_gaincode_rev6;
+					}
+					clip1hi_gaincode =
+						clip1hiG_gaincode_rev6;
+					switch (triso) {
+					case 0:
+						clip1lo_gaincode =
+							clip1loG_gaincode_rev6
+							[0];
+						break;
+					case 1:
+						clip1lo_gaincode =
+							clip1loG_gaincode_rev6
+							[1];
+						break;
+					case 2:
+						clip1lo_gaincode =
+							clip1loG_gaincode_rev6
+							[2];
+						break;
+					case 3:
+					default:
 
-						pi->rx2tx_biasentry = (u8) i;
-						tbl_opcode =
-						    NPHY_REV3_RFSEQ_CMD_NOP;
-						wlc_phy_table_write_nphy(pi,
-									 NPHY_TBL_ID_RFSEQ,
-									 1, i,
-									 16,
-									 &tbl_opcode);
+						clip1lo_gaincode =
+							clip1loG_gaincode_rev6
+							[3];
 						break;
-					} else if (tbl_buf[i] ==
-						   NPHY_REV3_RFSEQ_CMD_END) {
+					case 4:
+						clip1lo_gaincode =
+							clip1loG_gaincode_rev6
+							[4];
+						break;
+					case 5:
+						clip1lo_gaincode =
+							clip1loG_gaincode_rev6
+							[5];
+						break;
+					case 6:
+						clip1lo_gaincode =
+							clip1loG_gaincode_rev6
+							[6];
+						break;
+					case 7:
+						clip1lo_gaincode =
+							clip1loG_gaincode_rev6
+							[7];
 						break;
 					}
+					nbclip_th = nbclipG_th_rev6;
+					w1clip_th = w1clipG_th_rev6;
+					crsmin_th = crsminG_th_rev6;
+					crsminl_th = crsminlG_th_rev6;
+					crsminu_th = crsminuG_th_rev6;
+					rssi_gain = rssi_gain_default;
+				}
+			} else if (NREV_IS(pi->pubpi.phy_rev, 5)) {
+				lna1_gain_db = lna1G_gain_db_rev5;
+				lna2_gain_db = lna2G_gain_db_rev5;
+				if (pi->sh->boardflags & BFL_EXTLNA) {
+
+					rfseq_init_gain =
+						rfseqG_init_gain_rev5_elna;
+					init_gaincode =
+						initG_gaincode_rev5_elna;
+				} else {
+					rfseq_init_gain = rfseqG_init_gain_rev5;
+					init_gaincode = initG_gaincode_rev5;
 				}
+				clip1hi_gaincode = clip1hiG_gaincode_rev5;
+				switch (triso) {
+				case 0:
+					clip1lo_gaincode =
+						clip1loG_gaincode_rev5[0];
+					break;
+				case 1:
+					clip1lo_gaincode =
+						clip1loG_gaincode_rev5[1];
+					break;
+				case 2:
+					clip1lo_gaincode =
+						clip1loG_gaincode_rev5[2];
+					break;
+				case 3:
+
+					clip1lo_gaincode =
+						clip1loG_gaincode_rev5[3];
+					break;
+				case 4:
+					clip1lo_gaincode =
+						clip1loG_gaincode_rev5[4];
+					break;
+				case 5:
+					clip1lo_gaincode =
+						clip1loG_gaincode_rev5[5];
+					break;
+				case 6:
+					clip1lo_gaincode =
+						clip1loG_gaincode_rev5[6];
+					break;
+				case 7:
+					clip1lo_gaincode =
+						clip1loG_gaincode_rev5[7];
+					break;
+				default:
+					clip1lo_gaincode =
+						clip1loG_gaincode_rev5[3];
+					break;
+				}
+				nbclip_th = nbclipG_th_rev5;
+				w1clip_th = w1clipG_th_rev5;
+				crsmin_th = crsminG_th_rev5;
+				crsminl_th = crsminlG_th_rev5;
+				crsminu_th = crsminuG_th_rev5;
+				rssi_gain = rssi_gain_default;
+			} else if (NREV_IS(pi->pubpi.phy_rev, 4)) {
+				lna1_gain_db = lna1G_gain_db_rev4;
+				lna2_gain_db = lna2G_gain_db;
+				rfseq_init_gain = rfseqG_init_gain_rev4;
+				init_gaincode = initG_gaincode_rev4;
+				clip1hi_gaincode = clip1hiG_gaincode_rev4;
+				clip1lo_gaincode = clip1loG_gaincode;
+				nbclip_th = nbclipG_th_rev4;
+				w1clip_th = w1clipG_th;
+				crsmin_th = crsminG_th;
+				crsminl_th = crsminlG_th;
+				crsminu_th = crsminuG_th;
+				rssi_gain = rssi_gain_default;
+			} else {
+				lna1_gain_db = lna1G_gain_db;
+				lna2_gain_db = lna2G_gain_db;
+				rfseq_init_gain = rfseqG_init_gain;
+				init_gaincode = initG_gaincode;
+				clip1hi_gaincode = clip1hiG_gaincode;
+				clip1lo_gaincode = clip1loG_gaincode;
+				nbclip_th = nbclipG_th;
+				w1clip_th = w1clipG_th;
+				crsmin_th = crsminG_th;
+				crsminl_th = crsminlG_th;
+				crsminu_th = crsminuG_th;
+				rssi_gain = rssi_gain_default;
 			}
-		}
-	} else {
+			tia_gain_db = tiaG_gain_db;
+			tia_gainbits = tiaG_gainbits;
+			clip1md_gaincode = clip1mdG_gaincode;
+		} else {
+			if (NREV_GE(pi->pubpi.phy_rev, 6)) {
+				lna1_gain_db = lna1A_gain_db_rev6;
+				lna2_gain_db = lna2A_gain_db_rev6;
+				tia_gain_db = tiaA_gain_db_rev6;
+				tia_gainbits = tiaA_gainbits_rev6;
+				rfseq_init_gain = rfseqA_init_gain_rev6;
+				init_gaincode = initA_gaincode_rev6;
+				clip1hi_gaincode = clip1hiA_gaincode_rev6;
+				clip1md_gaincode = clip1mdA_gaincode_rev6;
+				clip1lo_gaincode = clip1loA_gaincode_rev6;
+				crsmin_th = crsminA_th_rev6;
+				crsminl_th = crsminlA_th_rev6;
+				if ((pi->pubpi.radiorev == 11) &&
+				    (CHSPEC_IS40(pi->radio_chanspec) == 0))
+					crsminu_th = crsminuA_th_rev6_224B0;
+				else
+					crsminu_th = crsminuA_th_rev6;
 
-		write_phy_reg(pi, 0x20e, 30);
+				nbclip_th = nbclipA_th_rev6;
+				rssi_gain = rssiA_gain_rev6;
+			} else if (NREV_IS(pi->pubpi.phy_rev, 5)) {
+				lna1_gain_db = lna1A_gain_db_rev5;
+				lna2_gain_db = lna2A_gain_db_rev5;
+				tia_gain_db = tiaA_gain_db_rev5;
+				tia_gainbits = tiaA_gainbits_rev5;
+				rfseq_init_gain = rfseqA_init_gain_rev5;
+				init_gaincode = initA_gaincode_rev5;
+				clip1hi_gaincode = clip1hiA_gaincode_rev5;
+				clip1md_gaincode = clip1mdA_gaincode_rev5;
+				clip1lo_gaincode = clip1loA_gaincode_rev5;
+				crsmin_th = crsminA_th_rev5;
+				crsminl_th = crsminlA_th_rev5;
+				crsminu_th = crsminuA_th_rev5;
+				nbclip_th = nbclipA_th_rev5;
+				rssi_gain = rssiA_gain_rev5;
+			} else if (NREV_IS(pi->pubpi.phy_rev, 4)) {
+				lna1_gain_db = lna1A_gain_db_rev4;
+				lna2_gain_db = lna2A_gain_db_rev4;
+				tia_gain_db = tiaA_gain_db_rev4;
+				tia_gainbits = tiaA_gainbits_rev4;
+				if (pi->sh->boardflags & BFL_EXTLNA_5GHz) {
 
-		if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-			if (pi->rx2tx_biasentry != -1) {
-				tbl_opcode = NPHY_REV3_RFSEQ_CMD_CLR_RXRX_BIAS;
-				wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ,
-							 1, pi->rx2tx_biasentry,
-							 16, &tbl_opcode);
-				pi->rx2tx_biasentry = -1;
+					rfseq_init_gain =
+						rfseqA_init_gain_rev4_elna;
+					init_gaincode =
+						initA_gaincode_rev4_elna;
+				} else {
+					rfseq_init_gain = rfseqA_init_gain_rev4;
+					init_gaincode = initA_gaincode_rev4;
+				}
+				clip1hi_gaincode = clip1hiA_gaincode_rev4;
+				clip1md_gaincode = clip1mdA_gaincode_rev4;
+				clip1lo_gaincode = clip1loA_gaincode_rev4;
+				crsmin_th = crsminA_th_rev4;
+				crsminl_th = crsminlA_th_rev4;
+				crsminu_th = crsminuA_th_rev4;
+				nbclip_th = nbclipA_th_rev4;
+				rssi_gain = rssi_gain_default;
+			} else {
+				lna1_gain_db = lna1A_gain_db;
+				lna2_gain_db = lna2A_gain_db;
+				tia_gain_db = tiaA_gain_db;
+				tia_gainbits = tiaA_gainbits;
+				rfseq_init_gain = rfseqA_init_gain;
+				init_gaincode = initA_gaincode;
+				clip1hi_gaincode = clip1hiA_gaincode;
+				clip1md_gaincode = clip1mdA_gaincode;
+				clip1lo_gaincode = clip1loA_gaincode;
+				crsmin_th = crsminA_th;
+				crsminl_th = crsminlA_th;
+				crsminu_th = crsminuA_th;
+				nbclip_th = nbclipA_th;
+				rssi_gain = rssi_gain_default;
 			}
+			w1clip_th = w1clipA_th;
 		}
-	}
 
-	wlc_phy_force_rfseq_nphy(pi, NPHY_RFSEQ_RESET2RX);
+		write_radio_reg(pi,
+				(RADIO_2056_RX_BIASPOLE_LNAG1_IDAC |
+				 RADIO_2056_RX0), 0x17);
+		write_radio_reg(pi,
+				(RADIO_2056_RX_BIASPOLE_LNAG1_IDAC |
+				 RADIO_2056_RX1), 0x17);
 
-	if (pi->phyhang_avoid)
-		wlc_phy_stay_in_carriersearch_nphy(pi, false);
+		write_radio_reg(pi, (RADIO_2056_RX_LNAG2_IDAC | RADIO_2056_RX0),
+				0xf0);
+		write_radio_reg(pi, (RADIO_2056_RX_LNAG2_IDAC | RADIO_2056_RX1),
+				0xf0);
 
-	if (!suspend)
-		wlapi_enable_mac(pi->sh->physhim);
-}
+		write_radio_reg(pi, (RADIO_2056_RX_RSSI_POLE | RADIO_2056_RX0),
+				0x0);
+		write_radio_reg(pi, (RADIO_2056_RX_RSSI_POLE | RADIO_2056_RX1),
+				0x0);
 
-u8 wlc_phy_rxcore_getstate_nphy(struct brcms_phy_pub *pih)
-{
-	u16 regval, rxen_bits;
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+		write_radio_reg(pi, (RADIO_2056_RX_RSSI_GAIN | RADIO_2056_RX0),
+				rssi_gain);
+		write_radio_reg(pi, (RADIO_2056_RX_RSSI_GAIN | RADIO_2056_RX1),
+				rssi_gain);
 
-	regval = read_phy_reg(pi, 0xa2);
-	rxen_bits = (regval >> 4) & 0xf;
+		write_radio_reg(pi,
+				(RADIO_2056_RX_BIASPOLE_LNAA1_IDAC |
+				 RADIO_2056_RX0), 0x17);
+		write_radio_reg(pi,
+				(RADIO_2056_RX_BIASPOLE_LNAA1_IDAC |
+				 RADIO_2056_RX1), 0x17);
 
-	return (u8) rxen_bits;
-}
+		write_radio_reg(pi, (RADIO_2056_RX_LNAA2_IDAC | RADIO_2056_RX0),
+				0xFF);
+		write_radio_reg(pi, (RADIO_2056_RX_LNAA2_IDAC | RADIO_2056_RX1),
+				0xFF);
 
-bool wlc_phy_n_txpower_ipa_ison(struct brcms_phy *pi)
-{
-	return PHY_IPA(pi);
-}
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 4, 8,
+					 8, lna1_gain_db);
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 4, 8,
+					 8, lna1_gain_db);
 
-static void wlc_phy_txpwr_limit_to_tbl_nphy(struct brcms_phy *pi)
-{
-	u8 idx, idx2, i, delta_ind;
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 4, 0x10,
+					 8, lna2_gain_db);
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 4, 0x10,
+					 8, lna2_gain_db);
 
-	for (idx = TXP_FIRST_CCK; idx <= TXP_LAST_CCK; idx++) {
-		pi->adj_pwr_tbl_nphy[idx] = pi->tx_power_offset[idx];
-	}
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 10, 0x20,
+					 8, tia_gain_db);
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 10, 0x20,
+					 8, tia_gain_db);
 
-	for (i = 0; i < 4; i++) {
-		idx2 = 0;
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS1, 10, 0x20,
+					 8, tia_gainbits);
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS2, 10, 0x20,
+					 8, tia_gainbits);
 
-		delta_ind = 0;
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 6, 0x40,
+					 8, &lpf_gain_db);
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 6, 0x40,
+					 8, &lpf_gain_db);
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS1, 6, 0x40,
+					 8, &lpf_gainbits);
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS2, 6, 0x40,
+					 8, &lpf_gainbits);
 
-		switch (i) {
-		case 0:
+		write_phy_reg(pi, 0x20, init_gaincode);
+		write_phy_reg(pi, 0x2a7, init_gaincode);
 
-			if (CHSPEC_IS40(pi->radio_chanspec)
-			    && NPHY_IS_SROM_REINTERPRET) {
-				idx = TXP_FIRST_MCS_40_SISO;
-			} else {
-				idx = (CHSPEC_IS40(pi->radio_chanspec)) ?
-				    TXP_FIRST_OFDM_40_SISO : TXP_FIRST_OFDM;
-				delta_ind = 1;
-			}
-			break;
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ,
+					 pi->pubpi.phy_corenum, 0x106, 16,
+					 rfseq_init_gain);
 
-		case 1:
+		write_phy_reg(pi, 0x22, clip1hi_gaincode);
+		write_phy_reg(pi, 0x2a9, clip1hi_gaincode);
 
-			idx = (CHSPEC_IS40(pi->radio_chanspec)) ?
-			    TXP_FIRST_MCS_40_CDD : TXP_FIRST_MCS_20_CDD;
-			break;
+		write_phy_reg(pi, 0x24, clip1md_gaincode);
+		write_phy_reg(pi, 0x2ab, clip1md_gaincode);
 
-		case 2:
+		write_phy_reg(pi, 0x37, clip1lo_gaincode);
+		write_phy_reg(pi, 0x2ad, clip1lo_gaincode);
 
-			idx = (CHSPEC_IS40(pi->radio_chanspec)) ?
-			    TXP_FIRST_MCS_40_STBC : TXP_FIRST_MCS_20_STBC;
-			break;
+		mod_phy_reg(pi, 0x27d, (0xff << 0), (crsmin_th << 0));
+		mod_phy_reg(pi, 0x280, (0xff << 0), (crsminl_th << 0));
+		mod_phy_reg(pi, 0x283, (0xff << 0), (crsminu_th << 0));
 
-		case 3:
+		write_phy_reg(pi, 0x2b, nbclip_th);
+		write_phy_reg(pi, 0x41, nbclip_th);
 
-			idx = (CHSPEC_IS40(pi->radio_chanspec)) ?
-			    TXP_FIRST_MCS_40_SDM : TXP_FIRST_MCS_20_SDM;
-			break;
+		mod_phy_reg(pi, 0x27, (0x3f << 0), (w1clip_th << 0));
+		mod_phy_reg(pi, 0x3d, (0x3f << 0), (w1clip_th << 0));
+
+		write_phy_reg(pi, 0x150, 0x809c);
+
+	} else {
+
+		mod_phy_reg(pi, 0x1c, (0x1 << 13), (1 << 13));
+		mod_phy_reg(pi, 0x32, (0x1 << 13), (1 << 13));
+
+		write_phy_reg(pi, 0x2b, 0x84);
+		write_phy_reg(pi, 0x41, 0x84);
+
+		if (CHSPEC_IS20(pi->radio_chanspec)) {
+			write_phy_reg(pi, 0x6b, 0x2b);
+			write_phy_reg(pi, 0x6c, 0x2b);
+			write_phy_reg(pi, 0x6d, 0x9);
+			write_phy_reg(pi, 0x6e, 0x9);
 		}
 
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx];
-		idx = idx + delta_ind;
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx];
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx];
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx++];
+		w1th = NPHY_RSSICAL_W1_TARGET - 4;
+		mod_phy_reg(pi, 0x27, (0x3f << 0), (w1th << 0));
+		mod_phy_reg(pi, 0x3d, (0x3f << 0), (w1th << 0));
 
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx++];
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx];
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx];
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx++];
+		if (CHSPEC_IS20(pi->radio_chanspec)) {
+			mod_phy_reg(pi, 0x1c, (0x1f << 0), (0x1 << 0));
+			mod_phy_reg(pi, 0x32, (0x1f << 0), (0x1 << 0));
 
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx++];
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx];
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx];
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx++];
+			mod_phy_reg(pi, 0x1d, (0x1f << 0), (0x1 << 0));
+			mod_phy_reg(pi, 0x33, (0x1f << 0), (0x1 << 0));
+		}
 
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx];
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx++];
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx];
-		idx = idx + 1 - delta_ind;
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx];
+		write_phy_reg(pi, 0x150, 0x809c);
 
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx];
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx];
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx];
-		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
-		    pi->tx_power_offset[idx];
-	}
-}
+		if (pi->nphy_gain_boost)
+			if ((CHSPEC_IS2G(pi->radio_chanspec)) &&
+			    (CHSPEC_IS40(pi->radio_chanspec)))
+				hpf_code = 4;
+			else
+				hpf_code = 5;
+		else if (CHSPEC_IS40(pi->radio_chanspec))
+			hpf_code = 6;
+		else
+			hpf_code = 7;
 
-void wlc_phy_cal_init_nphy(struct brcms_phy *pi)
-{
-}
+		mod_phy_reg(pi, 0x20, (0x1f << 7), (hpf_code << 7));
+		mod_phy_reg(pi, 0x36, (0x1f << 7), (hpf_code << 7));
 
-static void
-wlc_phy_war_force_trsw_to_R_cliplo_nphy(struct brcms_phy *pi, u8 core)
-{
-	if (core == PHY_CORE_0) {
-		write_phy_reg(pi, 0x38, 0x4);
-		if (CHSPEC_IS2G(pi->radio_chanspec)) {
-			write_phy_reg(pi, 0x37, 0x0060);
-		} else {
-			write_phy_reg(pi, 0x37, 0x1080);
+		for (ctr = 0; ctr < 4; ctr++)
+			regval[ctr] = (hpf_code << 8) | 0x7c;
+		wlc_phy_table_write_nphy(pi, 7, 4, 0x106, 16, regval);
+
+		wlc_phy_adjust_lnagaintbl_nphy(pi);
+
+		if (pi->nphy_elna_gain_config) {
+			regval[0] = 0;
+			regval[1] = 1;
+			regval[2] = 1;
+			regval[3] = 1;
+			wlc_phy_table_write_nphy(pi, 2, 4, 8, 16, regval);
+			wlc_phy_table_write_nphy(pi, 3, 4, 8, 16, regval);
+
+			for (ctr = 0; ctr < 4; ctr++)
+				regval[ctr] = (hpf_code << 8) | 0x74;
+			wlc_phy_table_write_nphy(pi, 7, 4, 0x106, 16, regval);
 		}
-	} else if (core == PHY_CORE_1) {
-		write_phy_reg(pi, 0x2ae, 0x4);
-		if (CHSPEC_IS2G(pi->radio_chanspec)) {
-			write_phy_reg(pi, 0x2ad, 0x0060);
-		} else {
-			write_phy_reg(pi, 0x2ad, 0x1080);
+
+		if (NREV_IS(pi->pubpi.phy_rev, 2)) {
+			for (ctr = 0; ctr < 21; ctr++)
+				regval[ctr] = 3 * ctr;
+			wlc_phy_table_write_nphy(pi, 0, 21, 32, 16, regval);
+			wlc_phy_table_write_nphy(pi, 1, 21, 32, 16, regval);
+
+			for (ctr = 0; ctr < 21; ctr++)
+				regval[ctr] = (u16) ctr;
+			wlc_phy_table_write_nphy(pi, 2, 21, 32, 16, regval);
+			wlc_phy_table_write_nphy(pi, 3, 21, 32, 16, regval);
 		}
-	}
-}
 
-static void wlc_phy_war_txchain_upd_nphy(struct brcms_phy *pi, u8 txchain)
-{
-	u8 txchain0, txchain1;
+		wlc_phy_set_rfseq_nphy(pi, NPHY_RFSEQ_UPDATEGAINU,
+				       rfseq_updategainu_events,
+				       rfseq_updategainu_dlys,
+				       sizeof(rfseq_updategainu_events) /
+				       sizeof(rfseq_updategainu_events[0]));
 
-	txchain0 = txchain & 0x1;
-	txchain1 = (txchain & 0x2) >> 1;
-	if (!txchain0) {
-		wlc_phy_war_force_trsw_to_R_cliplo_nphy(pi, PHY_CORE_0);
-	}
+		mod_phy_reg(pi, 0x153, (0xff << 8), (90 << 8));
 
-	if (!txchain1) {
-		wlc_phy_war_force_trsw_to_R_cliplo_nphy(pi, PHY_CORE_1);
+		if (CHSPEC_IS2G(pi->radio_chanspec))
+			mod_phy_reg(pi,
+				    (NPHY_TO_BPHY_OFF + BPHY_OPTIONAL_MODES),
+				    0x7f, 0x4);
 	}
 }
 
@@ -15362,11 +16312,10 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 	u16 freq;
 	int coreNum;
 
-	if (CHSPEC_IS5G(pi->radio_chanspec)) {
+	if (CHSPEC_IS5G(pi->radio_chanspec))
 		wlc_phy_classifier_nphy(pi, NPHY_ClassifierCtrl_cck_en, 0);
-	} else {
+	else
 		wlc_phy_classifier_nphy(pi, NPHY_ClassifierCtrl_cck_en, 1);
-	}
 
 	if (pi->phyhang_avoid)
 		wlc_phy_stay_in_carriersearch_nphy(pi, true);
@@ -15401,9 +16350,8 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 			write_phy_reg(pi, 0x240, 0x1b0);
 		}
 
-		if (NREV_GE(pi->pubpi.phy_rev, 8)) {
+		if (NREV_GE(pi->pubpi.phy_rev, 8))
 			mod_phy_reg(pi, 0xbd, (0xff << 0), (114 << 0));
-		}
 
 		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_AFECTRL, 1, 0x00, 16,
 					 &dac_control);
@@ -15422,7 +16370,7 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ, 2, 0x16e, 16,
 					 rfseq_rx2tx_dacbufpu_rev7);
 
-		if (PHY_IPA(pi)) {
+		if (PHY_IPA(pi))
 			wlc_phy_set_rfseq_nphy(pi, NPHY_RFSEQ_RX2TX,
 					       rfseq_rx2tx_events_rev3_ipa,
 					       rfseq_rx2tx_dlys_rev3_ipa,
@@ -15431,7 +16379,6 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 					       sizeof
 					       (rfseq_rx2tx_events_rev3_ipa
 						[0]));
-		}
 
 		mod_phy_reg(pi, 0x299, (0x3 << 14), (0x1 << 14));
 		mod_phy_reg(pi, 0x29d, (0x3 << 14), (0x1 << 14));
@@ -15448,11 +16395,13 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 			    || (pi->pubpi.radiorev == 8)) {
 
 				rccal_bcap_val =
-				    read_radio_reg(pi,
-						   RADIO_2057_RCCAL_BCAP_VAL);
+					read_radio_reg(
+						pi,
+						RADIO_2057_RCCAL_BCAP_VAL);
 				rccal_scap_val =
-				    read_radio_reg(pi,
-						   RADIO_2057_RCCAL_SCAP_VAL);
+					read_radio_reg(
+						pi,
+						RADIO_2057_RCCAL_SCAP_VAL);
 
 				rccal_tx20_11b_bcap = rccal_bcap_val;
 				rccal_tx20_11b_scap = rccal_scap_val;
@@ -15497,11 +16446,13 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 				tx_lpf_bw_ofdm_40mhz = 3;
 
 				rccal_bcap_val =
-				    read_radio_reg(pi,
-						   RADIO_2057_RCCAL_BCAP_VAL);
+					read_radio_reg(
+						pi,
+						RADIO_2057_RCCAL_BCAP_VAL);
 				rccal_scap_val =
-				    read_radio_reg(pi,
-						   RADIO_2057_RCCAL_SCAP_VAL);
+					read_radio_reg(
+						pi,
+						RADIO_2057_RCCAL_SCAP_VAL);
 
 				rccal_tx20_11b_bcap = rccal_bcap_val;
 				rccal_tx20_11b_scap = rccal_scap_val;
@@ -15517,70 +16468,83 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 
 		if (rccal_ovrd) {
 
-			rx2tx_lpf_rc_lut_tx20_11b = (rccal_tx20_11b_bcap << 8) |
-			    (rccal_tx20_11b_scap << 3) | tx_lpf_bw_11b;
-			rx2tx_lpf_rc_lut_tx20_11n = (rccal_tx20_11n_bcap << 8) |
-			    (rccal_tx20_11n_scap << 3) | tx_lpf_bw_ofdm_20mhz;
-			rx2tx_lpf_rc_lut_tx40_11n = (rccal_tx40_11n_bcap << 8) |
-			    (rccal_tx40_11n_scap << 3) | tx_lpf_bw_ofdm_40mhz;
+			rx2tx_lpf_rc_lut_tx20_11b =
+				(rccal_tx20_11b_bcap << 8) |
+				(rccal_tx20_11b_scap << 3) |
+				tx_lpf_bw_11b;
+			rx2tx_lpf_rc_lut_tx20_11n =
+				(rccal_tx20_11n_bcap << 8) |
+				(rccal_tx20_11n_scap << 3) |
+				tx_lpf_bw_ofdm_20mhz;
+			rx2tx_lpf_rc_lut_tx40_11n =
+				(rccal_tx40_11n_bcap << 8) |
+				(rccal_tx40_11n_scap << 3) |
+				tx_lpf_bw_ofdm_40mhz;
 
 			for (coreNum = 0; coreNum <= 1; coreNum++) {
-				wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ,
-							 1,
-							 0x152 + coreNum * 0x10,
-							 16,
-							 &rx2tx_lpf_rc_lut_tx20_11b);
-				wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ,
-							 1,
-							 0x153 + coreNum * 0x10,
-							 16,
-							 &rx2tx_lpf_rc_lut_tx20_11n);
-				wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ,
-							 1,
-							 0x154 + coreNum * 0x10,
-							 16,
-							 &rx2tx_lpf_rc_lut_tx20_11n);
-				wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ,
-							 1,
-							 0x155 + coreNum * 0x10,
-							 16,
-							 &rx2tx_lpf_rc_lut_tx40_11n);
-				wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ,
-							 1,
-							 0x156 + coreNum * 0x10,
-							 16,
-							 &rx2tx_lpf_rc_lut_tx40_11n);
-				wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ,
-							 1,
-							 0x157 + coreNum * 0x10,
-							 16,
-							 &rx2tx_lpf_rc_lut_tx40_11n);
-				wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ,
-							 1,
-							 0x158 + coreNum * 0x10,
-							 16,
-							 &rx2tx_lpf_rc_lut_tx40_11n);
-				wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ,
-							 1,
-							 0x159 + coreNum * 0x10,
-							 16,
-							 &rx2tx_lpf_rc_lut_tx40_11n);
+				wlc_phy_table_write_nphy(
+					pi, NPHY_TBL_ID_RFSEQ,
+					1,
+					0x152 + coreNum * 0x10,
+					16,
+					&rx2tx_lpf_rc_lut_tx20_11b);
+				wlc_phy_table_write_nphy(
+					pi, NPHY_TBL_ID_RFSEQ,
+					1,
+					0x153 + coreNum * 0x10,
+					16,
+					&rx2tx_lpf_rc_lut_tx20_11n);
+				wlc_phy_table_write_nphy(
+					pi, NPHY_TBL_ID_RFSEQ,
+					1,
+					0x154 + coreNum * 0x10,
+					16,
+					&rx2tx_lpf_rc_lut_tx20_11n);
+				wlc_phy_table_write_nphy(
+					pi, NPHY_TBL_ID_RFSEQ,
+					1,
+					0x155 + coreNum * 0x10,
+					16,
+					&rx2tx_lpf_rc_lut_tx40_11n);
+				wlc_phy_table_write_nphy(
+					pi, NPHY_TBL_ID_RFSEQ,
+					1,
+					0x156 + coreNum * 0x10,
+					16,
+					&rx2tx_lpf_rc_lut_tx40_11n);
+				wlc_phy_table_write_nphy(
+					pi, NPHY_TBL_ID_RFSEQ,
+					1,
+					0x157 + coreNum * 0x10,
+					16,
+					&rx2tx_lpf_rc_lut_tx40_11n);
+				wlc_phy_table_write_nphy(
+					pi, NPHY_TBL_ID_RFSEQ,
+					1,
+					0x158 + coreNum * 0x10,
+					16,
+					&rx2tx_lpf_rc_lut_tx40_11n);
+				wlc_phy_table_write_nphy(
+					pi, NPHY_TBL_ID_RFSEQ,
+					1,
+					0x159 + coreNum * 0x10,
+					16,
+					&rx2tx_lpf_rc_lut_tx40_11n);
 			}
 
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 4),
-							  1, 0x3, 0,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID2);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 4),
+				1, 0x3, 0,
+				NPHY_REV7_RFCTRLOVERRIDE_ID2);
 		}
 
-		if (!NORADIO_ENAB(pi->pubpi)) {
-			write_phy_reg(pi, 0x32f, 0x3);
-		}
+		write_phy_reg(pi, 0x32f, 0x3);
 
-		if ((pi->pubpi.radiorev == 4) || (pi->pubpi.radiorev == 6)) {
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 2),
-							  1, 0x3, 0,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-		}
+		if ((pi->pubpi.radiorev == 4) || (pi->pubpi.radiorev == 6))
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 2),
+				1, 0x3, 0,
+				NPHY_REV7_RFCTRLOVERRIDE_ID0);
 
 		if ((pi->pubpi.radiorev == 3) || (pi->pubpi.radiorev == 4) ||
 		    (pi->pubpi.radiorev == 6)) {
@@ -15635,17 +16599,16 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 			if (CHSPEC_IS2G(pi->radio_chanspec)) {
 				if ((pi->pubpi.radiorev == 3)
 				    || (pi->pubpi.radiorev == 4)
-				    || (pi->pubpi.radiorev == 6)) {
-
+				    || (pi->pubpi.radiorev == 6))
 					txgm_idac_bleed = 0x7f;
-				}
 
 				for (coreNum = 0; coreNum <= 1; coreNum++) {
 					if (txgm_idac_bleed != 0)
-						WRITE_RADIO_REG4(pi, RADIO_2057,
-								 CORE, coreNum,
-								 TXGM_IDAC_BLEED,
-								 txgm_idac_bleed);
+						WRITE_RADIO_REG4(
+							pi, RADIO_2057,
+							CORE, coreNum,
+							TXGM_IDAC_BLEED,
+							txgm_idac_bleed);
 				}
 
 				if (pi->pubpi.radiorev == 5) {
@@ -15660,18 +16623,20 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 								 CORE, coreNum,
 								 IPA2G_IMAIN,
 								 0x1f);
-						WRITE_RADIO_REG4(pi, RADIO_2057,
-								 CORE, coreNum,
-								 IPA2G_BIAS_FILTER,
-								 0xee);
+						WRITE_RADIO_REG4(
+							pi, RADIO_2057,
+							CORE, coreNum,
+							IPA2G_BIAS_FILTER,
+							0xee);
 						WRITE_RADIO_REG4(pi, RADIO_2057,
 								 CORE, coreNum,
 								 PAD2G_IDACS,
 								 0x8a);
-						WRITE_RADIO_REG4(pi, RADIO_2057,
-								 CORE, coreNum,
-								 PAD_BIAS_FILTER_BWS,
-								 0x3e);
+						WRITE_RADIO_REG4(
+							pi, RADIO_2057,
+							CORE, coreNum,
+							PAD_BIAS_FILTER_BWS,
+							0x3e);
 					}
 
 				} else if ((pi->pubpi.radiorev == 7)
@@ -15700,9 +16665,8 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 				}
 
 			} else {
-				freq =
-				    CHAN5G_FREQ(CHSPEC_CHANNEL
-						(pi->radio_chanspec));
+				freq = CHAN5G_FREQ(CHSPEC_CHANNEL(
+							pi->radio_chanspec));
 				if (((freq >= 5180) && (freq <= 5230))
 				    || ((freq >= 5745) && (freq <= 5805))) {
 					WRITE_RADIO_REG4(pi, RADIO_2057, CORE,
@@ -15809,12 +16773,12 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 		wlc_phy_workarounds_nphy_gainctrl(pi);
 
 		pdetrange =
-		    (CHSPEC_IS5G(pi->radio_chanspec)) ? pi->srom_fem5g.
-		    pdetrange : pi->srom_fem2g.pdetrange;
+			(CHSPEC_IS5G(pi->radio_chanspec)) ? pi->srom_fem5g.
+			pdetrange : pi->srom_fem2g.pdetrange;
 
 		if (pdetrange == 0) {
 			chan_freq_range =
-			    wlc_phy_get_chan_freq_range_nphy(pi, 0);
+				wlc_phy_get_chan_freq_range_nphy(pi, 0);
 			if (chan_freq_range != WL_CHAN_FREQ_RANGE_2G) {
 				aux_adc_vmid_rev7_core0[3] = 0x70;
 				aux_adc_vmid_rev7_core1[3] = 0x70;
@@ -15842,15 +16806,15 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 					if (chan_freq_range ==
 					    WL_CHAN_FREQ_RANGE_2G) {
 						aux_adc_vmid_rev7_core0[3] =
-						    0x8c;
+							0x8c;
 						aux_adc_vmid_rev7_core1[3] =
-						    0x8c;
+							0x8c;
 						aux_adc_gain_rev7[3] = 0;
 					} else {
 						aux_adc_vmid_rev7_core0[3] =
-						    0x96;
+							0x96;
 						aux_adc_vmid_rev7_core1[3] =
-						    0x96;
+							0x96;
 						aux_adc_gain_rev7[3] = 0;
 					}
 				}
@@ -15918,7 +16882,7 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 				       sizeof(rfseq_tx2rx_events_rev3) /
 				       sizeof(rfseq_tx2rx_events_rev3[0]));
 
-		if (PHY_IPA(pi)) {
+		if (PHY_IPA(pi))
 			wlc_phy_set_rfseq_nphy(pi, NPHY_RFSEQ_RX2TX,
 					       rfseq_rx2tx_events_rev3_ipa,
 					       rfseq_rx2tx_dlys_rev3_ipa,
@@ -15927,7 +16891,6 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 					       sizeof
 					       (rfseq_rx2tx_events_rev3_ipa
 						[0]));
-		}
 
 		if ((pi->sh->hw_phyrxchain != 0x3) &&
 		    (pi->sh->hw_phyrxchain != pi->sh->hw_phytxchain)) {
@@ -15936,22 +16899,21 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 				rfseq_rx2tx_dlys_rev3[5] = 59;
 				rfseq_rx2tx_dlys_rev3[6] = 1;
 				rfseq_rx2tx_events_rev3[7] =
-				    NPHY_REV3_RFSEQ_CMD_END;
+					NPHY_REV3_RFSEQ_CMD_END;
 			}
 
-			wlc_phy_set_rfseq_nphy(pi, NPHY_RFSEQ_RX2TX,
-					       rfseq_rx2tx_events_rev3,
-					       rfseq_rx2tx_dlys_rev3,
-					       sizeof(rfseq_rx2tx_events_rev3) /
-					       sizeof(rfseq_rx2tx_events_rev3
-						      [0]));
+			wlc_phy_set_rfseq_nphy(
+				pi, NPHY_RFSEQ_RX2TX,
+				rfseq_rx2tx_events_rev3,
+				rfseq_rx2tx_dlys_rev3,
+				sizeof(rfseq_rx2tx_events_rev3)	/
+				sizeof(rfseq_rx2tx_events_rev3[0]));
 		}
 
-		if (CHSPEC_IS2G(pi->radio_chanspec)) {
+		if (CHSPEC_IS2G(pi->radio_chanspec))
 			write_phy_reg(pi, 0x6a, 0x2);
-		} else {
+		else
 			write_phy_reg(pi, 0x6a, 0x9c40);
-		}
 
 		mod_phy_reg(pi, 0x294, (0xf << 8), (7 << 8));
 
@@ -15978,8 +16940,8 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 					 &dac_control);
 
 		pdetrange =
-		    (CHSPEC_IS5G(pi->radio_chanspec)) ? pi->srom_fem5g.
-		    pdetrange : pi->srom_fem2g.pdetrange;
+			(CHSPEC_IS5G(pi->radio_chanspec)) ? pi->srom_fem5g.
+			pdetrange : pi->srom_fem2g.pdetrange;
 
 		if (pdetrange == 0) {
 			if (NREV_GE(pi->pubpi.phy_rev, 4)) {
@@ -15990,7 +16952,7 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 				aux_adc_gain = aux_adc_gain_rev3;
 			}
 			chan_freq_range =
-			    wlc_phy_get_chan_freq_range_nphy(pi, 0);
+				wlc_phy_get_chan_freq_range_nphy(pi, 0);
 			if (chan_freq_range != WL_CHAN_FREQ_RANGE_2G) {
 				switch (chan_freq_range) {
 				case WL_CHAN_FREQ_RANGE_5GL:
@@ -16033,7 +16995,7 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 
 			if (NREV_GE(pi->pubpi.phy_rev, 6)) {
 				chan_freq_range =
-				    wlc_phy_get_chan_freq_range_nphy(pi, 0);
+					wlc_phy_get_chan_freq_range_nphy(pi, 0);
 				if (chan_freq_range != WL_CHAN_FREQ_RANGE_2G) {
 					bcm_adc_vmid[3] = 0x8e;
 					bcm_adc_gain[3] = 0x03;
@@ -16056,14 +17018,16 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 						 0x1c, 16, bcm_adc_gain);
 		} else if (pdetrange == 3) {
 			chan_freq_range =
-			    wlc_phy_get_chan_freq_range_nphy(pi, 0);
+				wlc_phy_get_chan_freq_range_nphy(pi, 0);
 			if ((NREV_GE(pi->pubpi.phy_rev, 4))
 			    && (chan_freq_range == WL_CHAN_FREQ_RANGE_2G)) {
 
 				u16 auxadc_vmid[] = {
-					0xa2, 0xb4, 0xb4, 0x270 };
+					0xa2, 0xb4, 0xb4, 0x270
+				};
 				u16 auxadc_gain[] = {
-					0x02, 0x02, 0x02, 0x00 };
+					0x02, 0x02, 0x02, 0x00
+				};
 
 				wlc_phy_table_write_nphy(pi,
 							 NPHY_TBL_ID_AFECTRL, 4,
@@ -16084,7 +17048,7 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 			u16 Vmid[2], Av[2];
 
 			chan_freq_range =
-			    wlc_phy_get_chan_freq_range_nphy(pi, 0);
+				wlc_phy_get_chan_freq_range_nphy(pi, 0);
 			if (chan_freq_range != WL_CHAN_FREQ_RANGE_2G) {
 				Vmid[0] = (pdetrange == 4) ? 0x8e : 0x89;
 				Vmid[1] = (pdetrange == 4) ? 0x96 : 0x89;
@@ -16155,8 +17119,8 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 				0x0);
 
 		triso =
-		    (CHSPEC_IS5G(pi->radio_chanspec)) ? pi->srom_fem5g.
-		    triso : pi->srom_fem2g.triso;
+			(CHSPEC_IS5G(pi->radio_chanspec)) ? pi->srom_fem5g.
+			triso : pi->srom_fem2g.triso;
 		if (triso == 7) {
 			wlc_phy_war_force_trsw_to_R_cliplo_nphy(pi, PHY_CORE_0);
 			wlc_phy_war_force_trsw_to_R_cliplo_nphy(pi, PHY_CORE_1);
@@ -16212,11 +17176,10 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 
 		if (NREV_GE(pi->pubpi.phy_rev, 6)) {
 
-			if (pi->sh->boardflags2 & BFL2_SINGLEANT_CCK) {
+			if (pi->sh->boardflags2 & BFL2_SINGLEANT_CCK)
 				wlapi_bmac_mhf(pi->sh->physhim, MHF4,
-					MHF4_BPHY_TXCORE0,
-					MHF4_BPHY_TXCORE0, BRCM_BAND_ALL);
-			}
+					      MHF4_BPHY_TXCORE0,
+					      MHF4_BPHY_TXCORE0, BRCM_BAND_ALL);
 		}
 	} else {
 
@@ -16314,1121 +17277,2720 @@ static void wlc_phy_workarounds_nphy(struct brcms_phy *pi)
 			write_phy_reg(pi, 0x194, 0x0);
 		}
 
-		if (NREV_IS(pi->pubpi.phy_rev, 2)) {
+		if (NREV_IS(pi->pubpi.phy_rev, 2))
 			mod_phy_reg(pi, 0x221,
 				    NPHY_FORCESIG_DECODEGATEDCLKS,
 				    NPHY_FORCESIG_DECODEGATEDCLKS);
-		}
 	}
 
 	if (pi->phyhang_avoid)
 		wlc_phy_stay_in_carriersearch_nphy(pi, false);
 }
 
-static void wlc_phy_workarounds_nphy_gainctrl(struct brcms_phy *pi)
+static void wlc_phy_extpa_set_tx_digi_filts_nphy(struct brcms_phy *pi)
 {
-	u16 w1th, hpf_code, currband;
-	int ctr;
-	u8 rfseq_updategainu_events[] = {
-		NPHY_RFSEQ_CMD_RX_GAIN,
-		NPHY_RFSEQ_CMD_CLR_HIQ_DIS,
-		NPHY_RFSEQ_CMD_SET_HPF_BW
-	};
-	u8 rfseq_updategainu_dlys[] = { 10, 30, 1 };
-	s8 lna1G_gain_db[] = { 7, 11, 16, 23 };
-	s8 lna1G_gain_db_rev4[] = { 8, 12, 17, 25 };
-	s8 lna1G_gain_db_rev5[] = { 9, 13, 18, 26 };
-	s8 lna1G_gain_db_rev6[] = { 8, 13, 18, 25 };
-	s8 lna1G_gain_db_rev6_224B0[] = { 10, 14, 19, 27 };
-	s8 lna1A_gain_db[] = { 7, 11, 17, 23 };
-	s8 lna1A_gain_db_rev4[] = { 8, 12, 18, 23 };
-	s8 lna1A_gain_db_rev5[] = { 6, 10, 16, 21 };
-	s8 lna1A_gain_db_rev6[] = { 6, 10, 16, 21 };
-	s8 *lna1_gain_db = NULL;
-	s8 lna2G_gain_db[] = { -5, 6, 10, 14 };
-	s8 lna2G_gain_db_rev5[] = { -3, 7, 11, 16 };
-	s8 lna2G_gain_db_rev6[] = { -5, 6, 10, 14 };
-	s8 lna2G_gain_db_rev6_224B0[] = { -5, 6, 10, 15 };
-	s8 lna2A_gain_db[] = { -6, 2, 6, 10 };
-	s8 lna2A_gain_db_rev4[] = { -5, 2, 6, 10 };
-	s8 lna2A_gain_db_rev5[] = { -7, 0, 4, 8 };
-	s8 lna2A_gain_db_rev6[] = { -7, 0, 4, 8 };
-	s8 *lna2_gain_db = NULL;
-	s8 tiaG_gain_db[] = {
-		0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A };
-	s8 tiaA_gain_db[] = {
-		0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13, 0x13 };
-	s8 tiaA_gain_db_rev4[] = {
-		0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d };
-	s8 tiaA_gain_db_rev5[] = {
-		0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d };
-	s8 tiaA_gain_db_rev6[] = {
-		0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d, 0x0d };
-	s8 *tia_gain_db;
-	s8 tiaG_gainbits[] = {
-		0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 };
-	s8 tiaA_gainbits[] = {
-		0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06 };
-	s8 tiaA_gainbits_rev4[] = {
-		0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04 };
-	s8 tiaA_gainbits_rev5[] = {
-		0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04 };
-	s8 tiaA_gainbits_rev6[] = {
-		0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04 };
-	s8 *tia_gainbits;
-	s8 lpf_gain_db[] = { 0x00, 0x06, 0x0c, 0x12, 0x12, 0x12 };
-	s8 lpf_gainbits[] = { 0x00, 0x01, 0x02, 0x03, 0x03, 0x03 };
-	u16 rfseqG_init_gain[] = { 0x613f, 0x613f, 0x613f, 0x613f };
-	u16 rfseqG_init_gain_rev4[] = { 0x513f, 0x513f, 0x513f, 0x513f };
-	u16 rfseqG_init_gain_rev5[] = { 0x413f, 0x413f, 0x413f, 0x413f };
-	u16 rfseqG_init_gain_rev5_elna[] = {
-		0x013f, 0x013f, 0x013f, 0x013f };
-	u16 rfseqG_init_gain_rev6[] = { 0x513f, 0x513f };
-	u16 rfseqG_init_gain_rev6_224B0[] = { 0x413f, 0x413f };
-	u16 rfseqG_init_gain_rev6_elna[] = { 0x113f, 0x113f };
-	u16 rfseqA_init_gain[] = { 0x516f, 0x516f, 0x516f, 0x516f };
-	u16 rfseqA_init_gain_rev4[] = { 0x614f, 0x614f, 0x614f, 0x614f };
-	u16 rfseqA_init_gain_rev4_elna[] = {
-		0x314f, 0x314f, 0x314f, 0x314f };
-	u16 rfseqA_init_gain_rev5[] = { 0x714f, 0x714f, 0x714f, 0x714f };
-	u16 rfseqA_init_gain_rev6[] = { 0x714f, 0x714f };
-	u16 *rfseq_init_gain;
-	u16 initG_gaincode = 0x627e;
-	u16 initG_gaincode_rev4 = 0x527e;
-	u16 initG_gaincode_rev5 = 0x427e;
-	u16 initG_gaincode_rev5_elna = 0x027e;
-	u16 initG_gaincode_rev6 = 0x527e;
-	u16 initG_gaincode_rev6_224B0 = 0x427e;
-	u16 initG_gaincode_rev6_elna = 0x127e;
-	u16 initA_gaincode = 0x52de;
-	u16 initA_gaincode_rev4 = 0x629e;
-	u16 initA_gaincode_rev4_elna = 0x329e;
-	u16 initA_gaincode_rev5 = 0x729e;
-	u16 initA_gaincode_rev6 = 0x729e;
-	u16 init_gaincode;
-	u16 clip1hiG_gaincode = 0x107e;
-	u16 clip1hiG_gaincode_rev4 = 0x007e;
-	u16 clip1hiG_gaincode_rev5 = 0x1076;
-	u16 clip1hiG_gaincode_rev6 = 0x007e;
-	u16 clip1hiA_gaincode = 0x00de;
-	u16 clip1hiA_gaincode_rev4 = 0x029e;
-	u16 clip1hiA_gaincode_rev5 = 0x029e;
-	u16 clip1hiA_gaincode_rev6 = 0x029e;
-	u16 clip1hi_gaincode;
-	u16 clip1mdG_gaincode = 0x0066;
-	u16 clip1mdA_gaincode = 0x00ca;
-	u16 clip1mdA_gaincode_rev4 = 0x1084;
-	u16 clip1mdA_gaincode_rev5 = 0x2084;
-	u16 clip1mdA_gaincode_rev6 = 0x2084;
-	u16 clip1md_gaincode = 0;
-	u16 clip1loG_gaincode = 0x0074;
-	u16 clip1loG_gaincode_rev5[] = {
-		0x0062, 0x0064, 0x006a, 0x106a, 0x106c, 0x1074, 0x107c, 0x207c
-	};
-	u16 clip1loG_gaincode_rev6[] = {
-		0x106a, 0x106c, 0x1074, 0x107c, 0x007e, 0x107e, 0x207e, 0x307e
-	};
-	u16 clip1loG_gaincode_rev6_224B0 = 0x1074;
-	u16 clip1loA_gaincode = 0x00cc;
-	u16 clip1loA_gaincode_rev4 = 0x0086;
-	u16 clip1loA_gaincode_rev5 = 0x2086;
-	u16 clip1loA_gaincode_rev6 = 0x2086;
-	u16 clip1lo_gaincode;
-	u8 crsminG_th = 0x18;
-	u8 crsminG_th_rev5 = 0x18;
-	u8 crsminG_th_rev6 = 0x18;
-	u8 crsminA_th = 0x1e;
-	u8 crsminA_th_rev4 = 0x24;
-	u8 crsminA_th_rev5 = 0x24;
-	u8 crsminA_th_rev6 = 0x24;
-	u8 crsmin_th;
-	u8 crsminlG_th = 0x18;
-	u8 crsminlG_th_rev5 = 0x18;
-	u8 crsminlG_th_rev6 = 0x18;
-	u8 crsminlA_th = 0x1e;
-	u8 crsminlA_th_rev4 = 0x24;
-	u8 crsminlA_th_rev5 = 0x24;
-	u8 crsminlA_th_rev6 = 0x24;
-	u8 crsminl_th = 0;
-	u8 crsminuG_th = 0x18;
-	u8 crsminuG_th_rev5 = 0x18;
-	u8 crsminuG_th_rev6 = 0x18;
-	u8 crsminuA_th = 0x1e;
-	u8 crsminuA_th_rev4 = 0x24;
-	u8 crsminuA_th_rev5 = 0x24;
-	u8 crsminuA_th_rev6 = 0x24;
-	u8 crsminuA_th_rev6_224B0 = 0x2d;
-	u8 crsminu_th;
-	u16 nbclipG_th = 0x20d;
-	u16 nbclipG_th_rev4 = 0x1a1;
-	u16 nbclipG_th_rev5 = 0x1d0;
-	u16 nbclipG_th_rev6 = 0x1d0;
-	u16 nbclipA_th = 0x1a1;
-	u16 nbclipA_th_rev4 = 0x107;
-	u16 nbclipA_th_rev5 = 0x0a9;
-	u16 nbclipA_th_rev6 = 0x0f0;
-	u16 nbclip_th = 0;
-	u8 w1clipG_th = 5;
-	u8 w1clipG_th_rev5 = 9;
-	u8 w1clipG_th_rev6 = 5;
-	u8 w1clipA_th = 25, w1clip_th;
-	u8 rssi_gain_default = 0x50;
-	u8 rssiG_gain_rev6_224B0 = 0x50;
-	u8 rssiA_gain_rev5 = 0x90;
-	u8 rssiA_gain_rev6 = 0x90;
-	u8 rssi_gain;
-	u16 regval[21];
-	u8 triso;
+	int j, type = 2;
+	u16 addr_offset = 0x2c5;
 
-	triso = (CHSPEC_IS5G(pi->radio_chanspec)) ? pi->srom_fem5g.triso :
-	    pi->srom_fem2g.triso;
+	for (j = 0; j < NPHY_NUM_DIG_FILT_COEFFS; j++)
+		write_phy_reg(pi, addr_offset + j,
+			      NPHY_IPA_REV4_txdigi_filtcoeffs[type][j]);
+}
+
+static void wlc_phy_clip_det_nphy(struct brcms_phy *pi, u8 write, u16 *vals)
+{
+
+	if (write == 0) {
+		vals[0] = read_phy_reg(pi, 0x2c);
+		vals[1] = read_phy_reg(pi, 0x42);
+	} else {
+		write_phy_reg(pi, 0x2c, vals[0]);
+		write_phy_reg(pi, 0x42, vals[1]);
+	}
+}
+
+static void wlc_phy_ipa_internal_tssi_setup_nphy(struct brcms_phy *pi)
+{
+	u8 core;
 
 	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-		if (pi->pubpi.radiorev == 5) {
+		for (core = 0; core < pi->pubpi.phy_corenum; core++) {
+			if (CHSPEC_IS2G(pi->radio_chanspec)) {
+				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+						 TX_SSI_MASTER, 0x5);
+				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+						 TX_SSI_MUX, 0xe);
 
-			wlc_phy_workarounds_nphy_gainctrl_2057_rev5(pi);
-		} else if (pi->pubpi.radiorev == 7) {
-			wlc_phy_workarounds_nphy_gainctrl_2057_rev6(pi);
+				if (pi->pubpi.radiorev != 5)
+					WRITE_RADIO_REG3(pi, RADIO_2057, TX,
+							 core, TSSIA, 0);
 
-			mod_phy_reg(pi, 0x283, (0xff << 0), (0x44 << 0));
-			mod_phy_reg(pi, 0x280, (0xff << 0), (0x44 << 0));
+				if (!NREV_IS(pi->pubpi.phy_rev, 7))
+					WRITE_RADIO_REG3(pi, RADIO_2057, TX,
+							 core, TSSIG, 0x1);
+				else
+					WRITE_RADIO_REG3(pi, RADIO_2057, TX,
+							 core, TSSIG, 0x31);
+			} else {
+				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+						 TX_SSI_MASTER, 0x9);
+				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+						 TX_SSI_MUX, 0xc);
+				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+						 TSSIG, 0);
 
-		} else if ((pi->pubpi.radiorev == 3)
-			   || (pi->pubpi.radiorev == 8)) {
-			wlc_phy_workarounds_nphy_gainctrl_2057_rev6(pi);
+				if (pi->pubpi.radiorev != 5) {
+					if (!NREV_IS(pi->pubpi.phy_rev, 7))
+						WRITE_RADIO_REG3(pi, RADIO_2057,
+								 TX, core,
+								 TSSIA, 0x1);
+					else
+						WRITE_RADIO_REG3(pi, RADIO_2057,
+								 TX, core,
+								 TSSIA, 0x31);
+				}
+			}
+			WRITE_RADIO_REG3(pi, RADIO_2057, TX, core, IQCAL_VCM_HG,
+					 0);
+			WRITE_RADIO_REG3(pi, RADIO_2057, TX, core, IQCAL_IDAC,
+					 0);
+			WRITE_RADIO_REG3(pi, RADIO_2057, TX, core, TSSI_VCM,
+					 0x3);
+			WRITE_RADIO_REG3(pi, RADIO_2057, TX, core, TSSI_MISC1,
+					 0x0);
+		}
+	} else {
+		WRITE_RADIO_SYN(pi, RADIO_2056, RESERVED_ADDR31,
+				(CHSPEC_IS2G(pi->radio_chanspec)) ? 0x128 :
+				0x80);
+		WRITE_RADIO_SYN(pi, RADIO_2056, RESERVED_ADDR30, 0x0);
+		WRITE_RADIO_SYN(pi, RADIO_2056, GPIO_MASTER1, 0x29);
 
-			if (pi->pubpi.radiorev == 8) {
-				mod_phy_reg(pi, 0x283,
-					    (0xff << 0), (0x44 << 0));
-				mod_phy_reg(pi, 0x280,
-					    (0xff << 0), (0x44 << 0));
+		for (core = 0; core < pi->pubpi.phy_corenum; core++) {
+			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core, IQCAL_VCM_HG,
+					 0x0);
+			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core, IQCAL_IDAC,
+					 0x0);
+			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core, TSSI_VCM,
+					 0x3);
+			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core, TX_AMP_DET,
+					 0x0);
+			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core, TSSI_MISC1,
+					 0x8);
+			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core, TSSI_MISC2,
+					 0x0);
+			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core, TSSI_MISC3,
+					 0x0);
+
+			if (CHSPEC_IS2G(pi->radio_chanspec)) {
+				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
+						 TX_SSI_MASTER, 0x5);
+
+				if (pi->pubpi.radiorev != 5)
+					WRITE_RADIO_REG2(pi, RADIO_2056, TX,
+							 core, TSSIA, 0x0);
+				if (NREV_GE(pi->pubpi.phy_rev, 5))
+					WRITE_RADIO_REG2(pi, RADIO_2056, TX,
+							 core, TSSIG, 0x31);
+				else
+					WRITE_RADIO_REG2(pi, RADIO_2056, TX,
+							 core, TSSIG, 0x11);
+				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
+						 TX_SSI_MUX, 0xe);
+			} else {
+				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
+						 TX_SSI_MASTER, 0x9);
+				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
+						 TSSIA, 0x31);
+				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
+						 TSSIG, 0x0);
+				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
+						 TX_SSI_MUX, 0xc);
 			}
-		} else {
-			wlc_phy_workarounds_nphy_gainctrl_2057_rev6(pi);
 		}
-	} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+	}
+}
 
-		mod_phy_reg(pi, 0xa0, (0x1 << 6), (1 << 6));
+static void
+wlc_phy_rfctrl_override_nphy(struct brcms_phy *pi, u16 field, u16 value,
+			     u8 core_mask, u8 off)
+{
+	u8 core_num;
+	u16 addr = 0, mask = 0, en_addr = 0, val_addr = 0, en_mask =
+		0, val_mask = 0;
+	u8 shift = 0, val_shift = 0;
 
-		mod_phy_reg(pi, 0x1c, (0x1 << 13), (1 << 13));
-		mod_phy_reg(pi, 0x32, (0x1 << 13), (1 << 13));
+	if (NREV_GE(pi->pubpi.phy_rev, 3) && NREV_LT(pi->pubpi.phy_rev, 7)) {
 
-		currband =
-		    read_phy_reg(pi, 0x09) & NPHY_BandControl_currentBand;
-		if (currband == 0) {
-			if (NREV_GE(pi->pubpi.phy_rev, 6)) {
-				if (pi->pubpi.radiorev == 11) {
-					lna1_gain_db = lna1G_gain_db_rev6_224B0;
-					lna2_gain_db = lna2G_gain_db_rev6_224B0;
-					rfseq_init_gain =
-					    rfseqG_init_gain_rev6_224B0;
-					init_gaincode =
-					    initG_gaincode_rev6_224B0;
-					clip1hi_gaincode =
-					    clip1hiG_gaincode_rev6;
-					clip1lo_gaincode =
-					    clip1loG_gaincode_rev6_224B0;
-					nbclip_th = nbclipG_th_rev6;
-					w1clip_th = w1clipG_th_rev6;
-					crsmin_th = crsminG_th_rev6;
-					crsminl_th = crsminlG_th_rev6;
-					crsminu_th = crsminuG_th_rev6;
-					rssi_gain = rssiG_gain_rev6_224B0;
-				} else {
-					lna1_gain_db = lna1G_gain_db_rev6;
-					lna2_gain_db = lna2G_gain_db_rev6;
-					if (pi->sh->boardflags & BFL_EXTLNA) {
+		en_mask = field;
+		for (core_num = 0; core_num < 2; core_num++) {
 
-						rfseq_init_gain =
-						    rfseqG_init_gain_rev6_elna;
-						init_gaincode =
-						    initG_gaincode_rev6_elna;
-					} else {
-						rfseq_init_gain =
-						    rfseqG_init_gain_rev6;
-						init_gaincode =
-						    initG_gaincode_rev6;
-					}
-					clip1hi_gaincode =
-					    clip1hiG_gaincode_rev6;
-					switch (triso) {
-					case 0:
-						clip1lo_gaincode =
-						    clip1loG_gaincode_rev6[0];
-						break;
-					case 1:
-						clip1lo_gaincode =
-						    clip1loG_gaincode_rev6[1];
-						break;
-					case 2:
-						clip1lo_gaincode =
-						    clip1loG_gaincode_rev6[2];
-						break;
-					case 3:
-					default:
+			switch (field) {
+			case (0x1 << 1):
+				en_addr = (core_num == 0) ? 0xe7 : 0xec;
+				val_addr = (core_num == 0) ? 0x7a : 0x7d;
+				val_mask = (0x1 << 0);
+				val_shift = 0;
+				break;
+			case (0x1 << 2):
+				en_addr = (core_num == 0) ? 0xe7 : 0xec;
+				val_addr = (core_num == 0) ? 0x7a : 0x7d;
+				val_mask = (0x1 << 1);
+				val_shift = 1;
+				break;
+			case (0x1 << 3):
+				en_addr = (core_num == 0) ? 0xe7 : 0xec;
+				val_addr = (core_num == 0) ? 0x7a : 0x7d;
+				val_mask = (0x1 << 2);
+				val_shift = 2;
+				break;
+			case (0x1 << 4):
+				en_addr = (core_num == 0) ? 0xe7 : 0xec;
+				val_addr = (core_num == 0) ? 0x7a : 0x7d;
+				val_mask = (0x1 << 4);
+				val_shift = 4;
+				break;
+			case (0x1 << 5):
+				en_addr = (core_num == 0) ? 0xe7 : 0xec;
+				val_addr = (core_num == 0) ? 0x7a : 0x7d;
+				val_mask = (0x1 << 5);
+				val_shift = 5;
+				break;
+			case (0x1 << 6):
+				en_addr = (core_num == 0) ? 0xe7 : 0xec;
+				val_addr = (core_num == 0) ? 0x7a : 0x7d;
+				val_mask = (0x1 << 6);
+				val_shift = 6;
+				break;
+			case (0x1 << 7):
+				en_addr = (core_num == 0) ? 0xe7 : 0xec;
+				val_addr = (core_num == 0) ? 0x7a : 0x7d;
+				val_mask = (0x1 << 7);
+				val_shift = 7;
+				break;
+			case (0x1 << 8):
+				en_addr = (core_num == 0) ? 0xe7 : 0xec;
+				val_addr = (core_num == 0) ? 0x7a : 0x7d;
+				val_mask = (0x7 << 8);
+				val_shift = 8;
+				break;
+			case (0x1 << 11):
+				en_addr = (core_num == 0) ? 0xe7 : 0xec;
+				val_addr = (core_num == 0) ? 0x7a : 0x7d;
+				val_mask = (0x7 << 13);
+				val_shift = 13;
+				break;
 
-						clip1lo_gaincode =
-						    clip1loG_gaincode_rev6[3];
-						break;
-					case 4:
-						clip1lo_gaincode =
-						    clip1loG_gaincode_rev6[4];
-						break;
-					case 5:
-						clip1lo_gaincode =
-						    clip1loG_gaincode_rev6[5];
-						break;
-					case 6:
-						clip1lo_gaincode =
-						    clip1loG_gaincode_rev6[6];
-						break;
-					case 7:
-						clip1lo_gaincode =
-						    clip1loG_gaincode_rev6[7];
-						break;
-					}
-					nbclip_th = nbclipG_th_rev6;
-					w1clip_th = w1clipG_th_rev6;
-					crsmin_th = crsminG_th_rev6;
-					crsminl_th = crsminlG_th_rev6;
-					crsminu_th = crsminuG_th_rev6;
-					rssi_gain = rssi_gain_default;
-				}
-			} else if (NREV_IS(pi->pubpi.phy_rev, 5)) {
-				lna1_gain_db = lna1G_gain_db_rev5;
-				lna2_gain_db = lna2G_gain_db_rev5;
-				if (pi->sh->boardflags & BFL_EXTLNA) {
+			case (0x1 << 9):
+				en_addr = (core_num == 0) ? 0xe7 : 0xec;
+				val_addr = (core_num == 0) ? 0xf8 : 0xfa;
+				val_mask = (0x7 << 0);
+				val_shift = 0;
+				break;
 
-					rfseq_init_gain =
-					    rfseqG_init_gain_rev5_elna;
-					init_gaincode =
-					    initG_gaincode_rev5_elna;
-				} else {
-					rfseq_init_gain = rfseqG_init_gain_rev5;
-					init_gaincode = initG_gaincode_rev5;
-				}
-				clip1hi_gaincode = clip1hiG_gaincode_rev5;
-				switch (triso) {
-				case 0:
-					clip1lo_gaincode =
-					    clip1loG_gaincode_rev5[0];
-					break;
-				case 1:
-					clip1lo_gaincode =
-					    clip1loG_gaincode_rev5[1];
-					break;
-				case 2:
-					clip1lo_gaincode =
-					    clip1loG_gaincode_rev5[2];
-					break;
-				case 3:
+			case (0x1 << 10):
+				en_addr = (core_num == 0) ? 0xe7 : 0xec;
+				val_addr = (core_num == 0) ? 0xf8 : 0xfa;
+				val_mask = (0x7 << 4);
+				val_shift = 4;
+				break;
 
-					clip1lo_gaincode =
-					    clip1loG_gaincode_rev5[3];
-					break;
-				case 4:
-					clip1lo_gaincode =
-					    clip1loG_gaincode_rev5[4];
-					break;
-				case 5:
-					clip1lo_gaincode =
-					    clip1loG_gaincode_rev5[5];
-					break;
-				case 6:
-					clip1lo_gaincode =
-					    clip1loG_gaincode_rev5[6];
-					break;
-				case 7:
-					clip1lo_gaincode =
-					    clip1loG_gaincode_rev5[7];
-					break;
-				default:
-					clip1lo_gaincode =
-					    clip1loG_gaincode_rev5[3];
-					break;
-				}
-				nbclip_th = nbclipG_th_rev5;
-				w1clip_th = w1clipG_th_rev5;
-				crsmin_th = crsminG_th_rev5;
-				crsminl_th = crsminlG_th_rev5;
-				crsminu_th = crsminuG_th_rev5;
-				rssi_gain = rssi_gain_default;
-			} else if (NREV_IS(pi->pubpi.phy_rev, 4)) {
-				lna1_gain_db = lna1G_gain_db_rev4;
-				lna2_gain_db = lna2G_gain_db;
-				rfseq_init_gain = rfseqG_init_gain_rev4;
-				init_gaincode = initG_gaincode_rev4;
-				clip1hi_gaincode = clip1hiG_gaincode_rev4;
-				clip1lo_gaincode = clip1loG_gaincode;
-				nbclip_th = nbclipG_th_rev4;
-				w1clip_th = w1clipG_th;
-				crsmin_th = crsminG_th;
-				crsminl_th = crsminlG_th;
-				crsminu_th = crsminuG_th;
-				rssi_gain = rssi_gain_default;
-			} else {
-				lna1_gain_db = lna1G_gain_db;
-				lna2_gain_db = lna2G_gain_db;
-				rfseq_init_gain = rfseqG_init_gain;
-				init_gaincode = initG_gaincode;
-				clip1hi_gaincode = clip1hiG_gaincode;
-				clip1lo_gaincode = clip1loG_gaincode;
-				nbclip_th = nbclipG_th;
-				w1clip_th = w1clipG_th;
-				crsmin_th = crsminG_th;
-				crsminl_th = crsminlG_th;
-				crsminu_th = crsminuG_th;
-				rssi_gain = rssi_gain_default;
+			case (0x1 << 12):
+				en_addr = (core_num == 0) ? 0xe7 : 0xec;
+				val_addr = (core_num == 0) ? 0x7b : 0x7e;
+				val_mask = (0xffff << 0);
+				val_shift = 0;
+				break;
+			case (0x1 << 13):
+				en_addr = (core_num == 0) ? 0xe7 : 0xec;
+				val_addr = (core_num == 0) ? 0x7c : 0x7f;
+				val_mask = (0xffff << 0);
+				val_shift = 0;
+				break;
+			case (0x1 << 14):
+				en_addr = (core_num == 0) ? 0xe7 : 0xec;
+				val_addr = (core_num == 0) ? 0xf9 : 0xfb;
+				val_mask = (0x3 << 6);
+				val_shift = 6;
+				break;
+			case (0x1 << 0):
+				en_addr = (core_num == 0) ? 0xe5 : 0xe6;
+				val_addr = (core_num == 0) ? 0xf9 : 0xfb;
+				val_mask = (0x1 << 15);
+				val_shift = 15;
+				break;
+			default:
+				addr = 0xffff;
+				break;
 			}
-			tia_gain_db = tiaG_gain_db;
-			tia_gainbits = tiaG_gainbits;
-			clip1md_gaincode = clip1mdG_gaincode;
-		} else {
-			if (NREV_GE(pi->pubpi.phy_rev, 6)) {
-				lna1_gain_db = lna1A_gain_db_rev6;
-				lna2_gain_db = lna2A_gain_db_rev6;
-				tia_gain_db = tiaA_gain_db_rev6;
-				tia_gainbits = tiaA_gainbits_rev6;
-				rfseq_init_gain = rfseqA_init_gain_rev6;
-				init_gaincode = initA_gaincode_rev6;
-				clip1hi_gaincode = clip1hiA_gaincode_rev6;
-				clip1md_gaincode = clip1mdA_gaincode_rev6;
-				clip1lo_gaincode = clip1loA_gaincode_rev6;
-				crsmin_th = crsminA_th_rev6;
-				crsminl_th = crsminlA_th_rev6;
-				if ((pi->pubpi.radiorev == 11) &&
-				    (CHSPEC_IS40(pi->radio_chanspec) == 0)) {
-					crsminu_th = crsminuA_th_rev6_224B0;
-				} else {
-					crsminu_th = crsminuA_th_rev6;
-				}
-				nbclip_th = nbclipA_th_rev6;
-				rssi_gain = rssiA_gain_rev6;
-			} else if (NREV_IS(pi->pubpi.phy_rev, 5)) {
-				lna1_gain_db = lna1A_gain_db_rev5;
-				lna2_gain_db = lna2A_gain_db_rev5;
-				tia_gain_db = tiaA_gain_db_rev5;
-				tia_gainbits = tiaA_gainbits_rev5;
-				rfseq_init_gain = rfseqA_init_gain_rev5;
-				init_gaincode = initA_gaincode_rev5;
-				clip1hi_gaincode = clip1hiA_gaincode_rev5;
-				clip1md_gaincode = clip1mdA_gaincode_rev5;
-				clip1lo_gaincode = clip1loA_gaincode_rev5;
-				crsmin_th = crsminA_th_rev5;
-				crsminl_th = crsminlA_th_rev5;
-				crsminu_th = crsminuA_th_rev5;
-				nbclip_th = nbclipA_th_rev5;
-				rssi_gain = rssiA_gain_rev5;
-			} else if (NREV_IS(pi->pubpi.phy_rev, 4)) {
-				lna1_gain_db = lna1A_gain_db_rev4;
-				lna2_gain_db = lna2A_gain_db_rev4;
-				tia_gain_db = tiaA_gain_db_rev4;
-				tia_gainbits = tiaA_gainbits_rev4;
-				if (pi->sh->boardflags & BFL_EXTLNA_5GHz) {
 
-					rfseq_init_gain =
-					    rfseqA_init_gain_rev4_elna;
-					init_gaincode =
-					    initA_gaincode_rev4_elna;
-				} else {
-					rfseq_init_gain = rfseqA_init_gain_rev4;
-					init_gaincode = initA_gaincode_rev4;
-				}
-				clip1hi_gaincode = clip1hiA_gaincode_rev4;
-				clip1md_gaincode = clip1mdA_gaincode_rev4;
-				clip1lo_gaincode = clip1loA_gaincode_rev4;
-				crsmin_th = crsminA_th_rev4;
-				crsminl_th = crsminlA_th_rev4;
-				crsminu_th = crsminuA_th_rev4;
-				nbclip_th = nbclipA_th_rev4;
-				rssi_gain = rssi_gain_default;
+			if (off) {
+				and_phy_reg(pi, en_addr, ~en_mask);
+				and_phy_reg(pi, val_addr, ~val_mask);
 			} else {
-				lna1_gain_db = lna1A_gain_db;
-				lna2_gain_db = lna2A_gain_db;
-				tia_gain_db = tiaA_gain_db;
-				tia_gainbits = tiaA_gainbits;
-				rfseq_init_gain = rfseqA_init_gain;
-				init_gaincode = initA_gaincode;
-				clip1hi_gaincode = clip1hiA_gaincode;
-				clip1md_gaincode = clip1mdA_gaincode;
-				clip1lo_gaincode = clip1loA_gaincode;
-				crsmin_th = crsminA_th;
-				crsminl_th = crsminlA_th;
-				crsminu_th = crsminuA_th;
-				nbclip_th = nbclipA_th;
-				rssi_gain = rssi_gain_default;
+
+				if ((core_mask == 0)
+				    || (core_mask & (1 << core_num))) {
+					or_phy_reg(pi, en_addr, en_mask);
+
+					if (addr != 0xffff)
+						mod_phy_reg(pi, val_addr,
+							    val_mask,
+							    (value <<
+							     val_shift));
+				}
 			}
-			w1clip_th = w1clipA_th;
 		}
+	} else {
 
-		write_radio_reg(pi,
-				(RADIO_2056_RX_BIASPOLE_LNAG1_IDAC |
-				 RADIO_2056_RX0), 0x17);
-		write_radio_reg(pi,
-				(RADIO_2056_RX_BIASPOLE_LNAG1_IDAC |
-				 RADIO_2056_RX1), 0x17);
+		if (off) {
+			and_phy_reg(pi, 0xec, ~field);
+			value = 0x0;
+		} else {
+			or_phy_reg(pi, 0xec, field);
+		}
 
-		write_radio_reg(pi, (RADIO_2056_RX_LNAG2_IDAC | RADIO_2056_RX0),
-				0xf0);
-		write_radio_reg(pi, (RADIO_2056_RX_LNAG2_IDAC | RADIO_2056_RX1),
-				0xf0);
+		for (core_num = 0; core_num < 2; core_num++) {
 
-		write_radio_reg(pi, (RADIO_2056_RX_RSSI_POLE | RADIO_2056_RX0),
-				0x0);
-		write_radio_reg(pi, (RADIO_2056_RX_RSSI_POLE | RADIO_2056_RX1),
-				0x0);
+			switch (field) {
+			case (0x1 << 1):
+			case (0x1 << 9):
+			case (0x1 << 12):
+			case (0x1 << 13):
+			case (0x1 << 14):
+				addr = 0x78;
 
-		write_radio_reg(pi, (RADIO_2056_RX_RSSI_GAIN | RADIO_2056_RX0),
-				rssi_gain);
-		write_radio_reg(pi, (RADIO_2056_RX_RSSI_GAIN | RADIO_2056_RX1),
-				rssi_gain);
+				core_mask = 0x1;
+				break;
+			case (0x1 << 2):
+			case (0x1 << 3):
+			case (0x1 << 4):
+			case (0x1 << 5):
+			case (0x1 << 6):
+			case (0x1 << 7):
+			case (0x1 << 8):
+				addr = (core_num == 0) ? 0x7a : 0x7d;
+				break;
+			case (0x1 << 10):
+				addr = (core_num == 0) ? 0x7b : 0x7e;
+				break;
+			case (0x1 << 11):
+				addr = (core_num == 0) ? 0x7c : 0x7f;
+				break;
+			default:
+				addr = 0xffff;
+			}
 
-		write_radio_reg(pi,
-				(RADIO_2056_RX_BIASPOLE_LNAA1_IDAC |
-				 RADIO_2056_RX0), 0x17);
-		write_radio_reg(pi,
-				(RADIO_2056_RX_BIASPOLE_LNAA1_IDAC |
-				 RADIO_2056_RX1), 0x17);
+			switch (field) {
+			case (0x1 << 1):
+				mask = (0x7 << 3);
+				shift = 3;
+				break;
+			case (0x1 << 9):
+				mask = (0x1 << 2);
+				shift = 2;
+				break;
+			case (0x1 << 12):
+				mask = (0x1 << 8);
+				shift = 8;
+				break;
+			case (0x1 << 13):
+				mask = (0x1 << 9);
+				shift = 9;
+				break;
+			case (0x1 << 14):
+				mask = (0xf << 12);
+				shift = 12;
+				break;
+			case (0x1 << 2):
+				mask = (0x1 << 0);
+				shift = 0;
+				break;
+			case (0x1 << 3):
+				mask = (0x1 << 1);
+				shift = 1;
+				break;
+			case (0x1 << 4):
+				mask = (0x1 << 2);
+				shift = 2;
+				break;
+			case (0x1 << 5):
+				mask = (0x3 << 4);
+				shift = 4;
+				break;
+			case (0x1 << 6):
+				mask = (0x3 << 6);
+				shift = 6;
+				break;
+			case (0x1 << 7):
+				mask = (0x1 << 8);
+				shift = 8;
+				break;
+			case (0x1 << 8):
+				mask = (0x1 << 9);
+				shift = 9;
+				break;
+			case (0x1 << 10):
+				mask = 0x1fff;
+				shift = 0x0;
+				break;
+			case (0x1 << 11):
+				mask = 0x1fff;
+				shift = 0x0;
+				break;
+			default:
+				mask = 0x0;
+				shift = 0x0;
+				break;
+			}
 
-		write_radio_reg(pi, (RADIO_2056_RX_LNAA2_IDAC | RADIO_2056_RX0),
-				0xFF);
-		write_radio_reg(pi, (RADIO_2056_RX_LNAA2_IDAC | RADIO_2056_RX1),
-				0xFF);
+			if ((addr != 0xffff) && (core_mask & (1 << core_num)))
+				mod_phy_reg(pi, addr, mask, (value << shift));
+		}
 
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 4, 8,
-					 8, lna1_gain_db);
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 4, 8,
-					 8, lna1_gain_db);
+		or_phy_reg(pi, 0xec, (0x1 << 0));
+		or_phy_reg(pi, 0x78, (0x1 << 0));
+		udelay(1);
+		and_phy_reg(pi, 0xec, ~(0x1 << 0));
+	}
+}
 
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 4, 0x10,
-					 8, lna2_gain_db);
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 4, 0x10,
-					 8, lna2_gain_db);
+static void wlc_phy_txpwrctrl_idle_tssi_nphy(struct brcms_phy *pi)
+{
+	s32 rssi_buf[4];
+	s32 int_val;
 
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 10, 0x20,
-					 8, tia_gain_db);
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 10, 0x20,
-					 8, tia_gain_db);
+	if (SCAN_RM_IN_PROGRESS(pi) || PLT_INPROG_PHY(pi) || PHY_MUTED(pi))
 
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS1, 10, 0x20,
-					 8, tia_gainbits);
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS2, 10, 0x20,
-					 8, tia_gainbits);
+		return;
 
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 6, 0x40,
-					 8, &lpf_gain_db);
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 6, 0x40,
-					 8, &lpf_gain_db);
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS1, 6, 0x40,
-					 8, &lpf_gainbits);
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS2, 6, 0x40,
-					 8, &lpf_gainbits);
+	if (PHY_IPA(pi))
+		wlc_phy_ipa_internal_tssi_setup_nphy(pi);
 
-		write_phy_reg(pi, 0x20, init_gaincode);
-		write_phy_reg(pi, 0x2a7, init_gaincode);
+	if (NREV_GE(pi->pubpi.phy_rev, 7))
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 12),
+						  0, 0x3, 0,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+	else if (NREV_GE(pi->pubpi.phy_rev, 3))
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 13), 0, 3, 0);
 
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ,
-					 pi->pubpi.phy_corenum, 0x106, 16,
-					 rfseq_init_gain);
+	wlc_phy_stopplayback_nphy(pi);
 
-		write_phy_reg(pi, 0x22, clip1hi_gaincode);
-		write_phy_reg(pi, 0x2a9, clip1hi_gaincode);
+	wlc_phy_tx_tone_nphy(pi, 4000, 0, 0, 0, false);
 
-		write_phy_reg(pi, 0x24, clip1md_gaincode);
-		write_phy_reg(pi, 0x2ab, clip1md_gaincode);
+	udelay(20);
+	int_val =
+		wlc_phy_poll_rssi_nphy(pi, (u8) NPHY_RSSI_SEL_TSSI_2G, rssi_buf,
+				       1);
+	wlc_phy_stopplayback_nphy(pi);
+	wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_OFF, 0);
 
-		write_phy_reg(pi, 0x37, clip1lo_gaincode);
-		write_phy_reg(pi, 0x2ad, clip1lo_gaincode);
+	if (NREV_GE(pi->pubpi.phy_rev, 7))
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 12),
+						  0, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+	else if (NREV_GE(pi->pubpi.phy_rev, 3))
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 13), 0, 3, 1);
 
-		mod_phy_reg(pi, 0x27d, (0xff << 0), (crsmin_th << 0));
-		mod_phy_reg(pi, 0x280, (0xff << 0), (crsminl_th << 0));
-		mod_phy_reg(pi, 0x283, (0xff << 0), (crsminu_th << 0));
+	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
 
-		write_phy_reg(pi, 0x2b, nbclip_th);
-		write_phy_reg(pi, 0x41, nbclip_th);
+		pi->nphy_pwrctrl_info[PHY_CORE_0].idle_tssi_2g =
+			(u8) ((int_val >> 24) & 0xff);
+		pi->nphy_pwrctrl_info[PHY_CORE_0].idle_tssi_5g =
+			(u8) ((int_val >> 24) & 0xff);
 
-		mod_phy_reg(pi, 0x27, (0x3f << 0), (w1clip_th << 0));
-		mod_phy_reg(pi, 0x3d, (0x3f << 0), (w1clip_th << 0));
+		pi->nphy_pwrctrl_info[PHY_CORE_1].idle_tssi_2g =
+			(u8) ((int_val >> 8) & 0xff);
+		pi->nphy_pwrctrl_info[PHY_CORE_1].idle_tssi_5g =
+			(u8) ((int_val >> 8) & 0xff);
+	} else {
+		pi->nphy_pwrctrl_info[PHY_CORE_0].idle_tssi_2g =
+			(u8) ((int_val >> 24) & 0xff);
 
-		write_phy_reg(pi, 0x150, 0x809c);
+		pi->nphy_pwrctrl_info[PHY_CORE_1].idle_tssi_2g =
+			(u8) ((int_val >> 8) & 0xff);
 
-	} else {
+		pi->nphy_pwrctrl_info[PHY_CORE_0].idle_tssi_5g =
+			(u8) ((int_val >> 16) & 0xff);
+		pi->nphy_pwrctrl_info[PHY_CORE_1].idle_tssi_5g =
+			(u8) ((int_val) & 0xff);
+	}
 
-		mod_phy_reg(pi, 0x1c, (0x1 << 13), (1 << 13));
-		mod_phy_reg(pi, 0x32, (0x1 << 13), (1 << 13));
+}
 
-		write_phy_reg(pi, 0x2b, 0x84);
-		write_phy_reg(pi, 0x41, 0x84);
+static void wlc_phy_txpwr_limit_to_tbl_nphy(struct brcms_phy *pi)
+{
+	u8 idx, idx2, i, delta_ind;
 
-		if (CHSPEC_IS20(pi->radio_chanspec)) {
-			write_phy_reg(pi, 0x6b, 0x2b);
-			write_phy_reg(pi, 0x6c, 0x2b);
-			write_phy_reg(pi, 0x6d, 0x9);
-			write_phy_reg(pi, 0x6e, 0x9);
-		}
+	for (idx = TXP_FIRST_CCK; idx <= TXP_LAST_CCK; idx++)
+		pi->adj_pwr_tbl_nphy[idx] = pi->tx_power_offset[idx];
 
-		w1th = NPHY_RSSICAL_W1_TARGET - 4;
-		mod_phy_reg(pi, 0x27, (0x3f << 0), (w1th << 0));
-		mod_phy_reg(pi, 0x3d, (0x3f << 0), (w1th << 0));
+	for (i = 0; i < 4; i++) {
+		idx2 = 0;
 
-		if (CHSPEC_IS20(pi->radio_chanspec)) {
-			mod_phy_reg(pi, 0x1c, (0x1f << 0), (0x1 << 0));
-			mod_phy_reg(pi, 0x32, (0x1f << 0), (0x1 << 0));
+		delta_ind = 0;
 
-			mod_phy_reg(pi, 0x1d, (0x1f << 0), (0x1 << 0));
-			mod_phy_reg(pi, 0x33, (0x1f << 0), (0x1 << 0));
-		}
+		switch (i) {
+		case 0:
 
-		write_phy_reg(pi, 0x150, 0x809c);
+			if (CHSPEC_IS40(pi->radio_chanspec)
+			    && NPHY_IS_SROM_REINTERPRET) {
+				idx = TXP_FIRST_MCS_40_SISO;
+			} else {
+				idx = (CHSPEC_IS40(pi->radio_chanspec)) ?
+				      TXP_FIRST_OFDM_40_SISO : TXP_FIRST_OFDM;
+				delta_ind = 1;
+			}
+			break;
 
-		if (pi->nphy_gain_boost)
-			if ((CHSPEC_IS2G(pi->radio_chanspec)) &&
-			    (CHSPEC_IS40(pi->radio_chanspec)))
-				hpf_code = 4;
-			else
-				hpf_code = 5;
-		else if (CHSPEC_IS40(pi->radio_chanspec))
-			hpf_code = 6;
-		else
-			hpf_code = 7;
+		case 1:
 
-		mod_phy_reg(pi, 0x20, (0x1f << 7), (hpf_code << 7));
-		mod_phy_reg(pi, 0x36, (0x1f << 7), (hpf_code << 7));
+			idx = (CHSPEC_IS40(pi->radio_chanspec)) ?
+			      TXP_FIRST_MCS_40_CDD : TXP_FIRST_MCS_20_CDD;
+			break;
 
-		for (ctr = 0; ctr < 4; ctr++) {
-			regval[ctr] = (hpf_code << 8) | 0x7c;
-		}
-		wlc_phy_table_write_nphy(pi, 7, 4, 0x106, 16, regval);
+		case 2:
 
-		wlc_phy_adjust_lnagaintbl_nphy(pi);
+			idx = (CHSPEC_IS40(pi->radio_chanspec)) ?
+			      TXP_FIRST_MCS_40_STBC : TXP_FIRST_MCS_20_STBC;
+			break;
 
-		if (pi->nphy_elna_gain_config) {
-			regval[0] = 0;
-			regval[1] = 1;
-			regval[2] = 1;
-			regval[3] = 1;
-			wlc_phy_table_write_nphy(pi, 2, 4, 8, 16, regval);
-			wlc_phy_table_write_nphy(pi, 3, 4, 8, 16, regval);
+		case 3:
 
-			for (ctr = 0; ctr < 4; ctr++) {
-				regval[ctr] = (hpf_code << 8) | 0x74;
-			}
-			wlc_phy_table_write_nphy(pi, 7, 4, 0x106, 16, regval);
+			idx = (CHSPEC_IS40(pi->radio_chanspec)) ?
+			      TXP_FIRST_MCS_40_SDM : TXP_FIRST_MCS_20_SDM;
+			break;
 		}
 
-		if (NREV_IS(pi->pubpi.phy_rev, 2)) {
-			for (ctr = 0; ctr < 21; ctr++) {
-				regval[ctr] = 3 * ctr;
-			}
-			wlc_phy_table_write_nphy(pi, 0, 21, 32, 16, regval);
-			wlc_phy_table_write_nphy(pi, 1, 21, 32, 16, regval);
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx];
+		idx = idx + delta_ind;
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx];
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx];
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx++];
 
-			for (ctr = 0; ctr < 21; ctr++) {
-				regval[ctr] = (u16) ctr;
-			}
-			wlc_phy_table_write_nphy(pi, 2, 21, 32, 16, regval);
-			wlc_phy_table_write_nphy(pi, 3, 21, 32, 16, regval);
-		}
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx++];
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx];
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx];
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx++];
 
-		wlc_phy_set_rfseq_nphy(pi, NPHY_RFSEQ_UPDATEGAINU,
-				       rfseq_updategainu_events,
-				       rfseq_updategainu_dlys,
-				       sizeof(rfseq_updategainu_events) /
-				       sizeof(rfseq_updategainu_events[0]));
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx++];
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx];
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx];
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx++];
 
-		mod_phy_reg(pi, 0x153, (0xff << 8), (90 << 8));
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx];
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx++];
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx];
+		idx = idx + 1 - delta_ind;
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx];
 
-		if (CHSPEC_IS2G(pi->radio_chanspec))
-			mod_phy_reg(pi,
-				    (NPHY_TO_BPHY_OFF + BPHY_OPTIONAL_MODES),
-				    0x7f, 0x4);
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx];
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx];
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx];
+		pi->adj_pwr_tbl_nphy[4 + 4 * (idx2++) + i] =
+			pi->tx_power_offset[idx];
 	}
 }
 
-static void wlc_phy_workarounds_nphy_gainctrl_2057_rev5(struct brcms_phy *pi)
+static void wlc_phy_txpwrctrl_pwr_setup_nphy(struct brcms_phy *pi)
 {
-	s8 lna1_gain_db[] = { 8, 13, 17, 22 };
-	s8 lna2_gain_db[] = { -2, 7, 11, 15 };
-	s8 tia_gain_db[] = { -4, -1, 2, 5, 5, 5, 5, 5, 5, 5 };
-	s8 tia_gainbits[] = {
-		0x0, 0x01, 0x02, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 };
+	u32 idx;
+	s16 a1[2], b0[2], b1[2];
+	s8 target_pwr_qtrdbm[2];
+	s32 num, den, pwr_est;
+	u8 chan_freq_range;
+	u8 idle_tssi[2];
+	u32 tbl_id, tbl_len, tbl_offset;
+	u32 regval[64];
+	u8 core;
 
-	mod_phy_reg(pi, 0x1c, (0x1 << 13), (1 << 13));
-	mod_phy_reg(pi, 0x32, (0x1 << 13), (1 << 13));
+	if (D11REV_IS(pi->sh->corerev, 11) || D11REV_IS(pi->sh->corerev, 12)) {
+		wlapi_bmac_mctrl(pi->sh->physhim, MCTL_PHYLOCK, MCTL_PHYLOCK);
+		(void)R_REG(&pi->regs->maccontrol);
+		udelay(1);
+	}
 
-	mod_phy_reg(pi, 0x289, (0xff << 0), (0x46 << 0));
+	if (pi->phyhang_avoid)
+		wlc_phy_stay_in_carriersearch_nphy(pi, true);
 
-	mod_phy_reg(pi, 0x283, (0xff << 0), (0x3c << 0));
-	mod_phy_reg(pi, 0x280, (0xff << 0), (0x3c << 0));
+	or_phy_reg(pi, 0x122, (0x1 << 0));
 
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 4, 0x8, 8,
-				 lna1_gain_db);
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 4, 0x8, 8,
-				 lna1_gain_db);
+	if (NREV_GE(pi->pubpi.phy_rev, 3))
+		and_phy_reg(pi, 0x1e7, (u16) (~(0x1 << 15)));
+	else
+		or_phy_reg(pi, 0x1e7, (0x1 << 15));
 
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 4, 0x10, 8,
-				 lna2_gain_db);
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 4, 0x10, 8,
-				 lna2_gain_db);
+	if (D11REV_IS(pi->sh->corerev, 11) || D11REV_IS(pi->sh->corerev, 12))
+		wlapi_bmac_mctrl(pi->sh->physhim, MCTL_PHYLOCK, 0);
 
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 10, 0x20, 8,
-				 tia_gain_db);
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 10, 0x20, 8,
-				 tia_gain_db);
+	if (pi->sh->sromrev < 4) {
+		idle_tssi[0] = pi->nphy_pwrctrl_info[0].idle_tssi_2g;
+		idle_tssi[1] = pi->nphy_pwrctrl_info[1].idle_tssi_2g;
+		target_pwr_qtrdbm[0] = 13 * 4;
+		target_pwr_qtrdbm[1] = 13 * 4;
+		a1[0] = -424;
+		a1[1] = -424;
+		b0[0] = 5612;
+		b0[1] = 5612;
+		b1[1] = -1393;
+		b1[0] = -1393;
+	} else {
 
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS1, 10, 0x20, 8,
-				 tia_gainbits);
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS2, 10, 0x20, 8,
-				 tia_gainbits);
+		chan_freq_range = wlc_phy_get_chan_freq_range_nphy(pi, 0);
+		switch (chan_freq_range) {
+		case WL_CHAN_FREQ_RANGE_2G:
+			idle_tssi[0] = pi->nphy_pwrctrl_info[0].idle_tssi_2g;
+			idle_tssi[1] = pi->nphy_pwrctrl_info[1].idle_tssi_2g;
+			target_pwr_qtrdbm[0] =
+				pi->nphy_pwrctrl_info[0].max_pwr_2g;
+			target_pwr_qtrdbm[1] =
+				pi->nphy_pwrctrl_info[1].max_pwr_2g;
+			a1[0] = pi->nphy_pwrctrl_info[0].pwrdet_2g_a1;
+			a1[1] = pi->nphy_pwrctrl_info[1].pwrdet_2g_a1;
+			b0[0] = pi->nphy_pwrctrl_info[0].pwrdet_2g_b0;
+			b0[1] = pi->nphy_pwrctrl_info[1].pwrdet_2g_b0;
+			b1[0] = pi->nphy_pwrctrl_info[0].pwrdet_2g_b1;
+			b1[1] = pi->nphy_pwrctrl_info[1].pwrdet_2g_b1;
+			break;
+		case WL_CHAN_FREQ_RANGE_5GL:
+			idle_tssi[0] = pi->nphy_pwrctrl_info[0].idle_tssi_5g;
+			idle_tssi[1] = pi->nphy_pwrctrl_info[1].idle_tssi_5g;
+			target_pwr_qtrdbm[0] =
+				pi->nphy_pwrctrl_info[0].max_pwr_5gl;
+			target_pwr_qtrdbm[1] =
+				pi->nphy_pwrctrl_info[1].max_pwr_5gl;
+			a1[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gl_a1;
+			a1[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gl_a1;
+			b0[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gl_b0;
+			b0[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gl_b0;
+			b1[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gl_b1;
+			b1[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gl_b1;
+			break;
+		case WL_CHAN_FREQ_RANGE_5GM:
+			idle_tssi[0] = pi->nphy_pwrctrl_info[0].idle_tssi_5g;
+			idle_tssi[1] = pi->nphy_pwrctrl_info[1].idle_tssi_5g;
+			target_pwr_qtrdbm[0] =
+				pi->nphy_pwrctrl_info[0].max_pwr_5gm;
+			target_pwr_qtrdbm[1] =
+				pi->nphy_pwrctrl_info[1].max_pwr_5gm;
+			a1[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gm_a1;
+			a1[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gm_a1;
+			b0[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gm_b0;
+			b0[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gm_b0;
+			b1[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gm_b1;
+			b1[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gm_b1;
+			break;
+		case WL_CHAN_FREQ_RANGE_5GH:
+			idle_tssi[0] = pi->nphy_pwrctrl_info[0].idle_tssi_5g;
+			idle_tssi[1] = pi->nphy_pwrctrl_info[1].idle_tssi_5g;
+			target_pwr_qtrdbm[0] =
+				pi->nphy_pwrctrl_info[0].max_pwr_5gh;
+			target_pwr_qtrdbm[1] =
+				pi->nphy_pwrctrl_info[1].max_pwr_5gh;
+			a1[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gh_a1;
+			a1[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gh_a1;
+			b0[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gh_b0;
+			b0[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gh_b0;
+			b1[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gh_b1;
+			b1[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gh_b1;
+			break;
+		default:
+			idle_tssi[0] = pi->nphy_pwrctrl_info[0].idle_tssi_2g;
+			idle_tssi[1] = pi->nphy_pwrctrl_info[1].idle_tssi_2g;
+			target_pwr_qtrdbm[0] = 13 * 4;
+			target_pwr_qtrdbm[1] = 13 * 4;
+			a1[0] = -424;
+			a1[1] = -424;
+			b0[0] = 5612;
+			b0[1] = 5612;
+			b1[1] = -1393;
+			b1[0] = -1393;
+			break;
+		}
+	}
 
-	write_phy_reg(pi, 0x37, 0x74);
-	write_phy_reg(pi, 0x2ad, 0x74);
-	write_phy_reg(pi, 0x38, 0x18);
-	write_phy_reg(pi, 0x2ae, 0x18);
+	target_pwr_qtrdbm[0] = (s8) pi->tx_power_max;
+	target_pwr_qtrdbm[1] = (s8) pi->tx_power_max;
 
-	write_phy_reg(pi, 0x2b, 0xe8);
-	write_phy_reg(pi, 0x41, 0xe8);
+	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+		if (pi->srom_fem2g.tssipos)
+			or_phy_reg(pi, 0x1e9, (0x1 << 14));
 
-	if (CHSPEC_IS20(pi->radio_chanspec)) {
+		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			for (core = 0; core <= 1; core++) {
+				if (PHY_IPA(pi)) {
+					if (CHSPEC_IS2G(pi->radio_chanspec))
+						WRITE_RADIO_REG3(pi, RADIO_2057,
+								 TX, core,
+								 TX_SSI_MUX,
+								 0xe);
+					else
+						WRITE_RADIO_REG3(pi, RADIO_2057,
+								 TX, core,
+								 TX_SSI_MUX,
+								 0xc);
+				}
+			}
+		} else {
+			if (PHY_IPA(pi)) {
 
-		mod_phy_reg(pi, 0x300, (0x3f << 0), (0x12 << 0));
-		mod_phy_reg(pi, 0x301, (0x3f << 0), (0x12 << 0));
-	} else {
+				write_radio_reg(pi, RADIO_2056_TX_TX_SSI_MUX |
+						RADIO_2056_TX0,
+						(CHSPEC_IS5G
+						 (pi->radio_chanspec)) ?
+						 0xc : 0xe);
+				write_radio_reg(pi,
+						RADIO_2056_TX_TX_SSI_MUX |
+						RADIO_2056_TX1,
+						(CHSPEC_IS5G
+						 (pi->radio_chanspec)) ?
+						 0xc : 0xe);
+			} else {
 
-		mod_phy_reg(pi, 0x300, (0x3f << 0), (0x10 << 0));
-		mod_phy_reg(pi, 0x301, (0x3f << 0), (0x10 << 0));
+				write_radio_reg(pi, RADIO_2056_TX_TX_SSI_MUX |
+						RADIO_2056_TX0, 0x11);
+				write_radio_reg(pi, RADIO_2056_TX_TX_SSI_MUX |
+						RADIO_2056_TX1, 0x11);
+			}
+		}
 	}
-}
 
-static void wlc_phy_workarounds_nphy_gainctrl_2057_rev6(struct brcms_phy *pi)
-{
-	u16 currband;
-	s8 lna1G_gain_db_rev7[] = { 9, 14, 19, 24 };
-	s8 *lna1_gain_db = NULL;
-	s8 *lna1_gain_db_2 = NULL;
-	s8 *lna2_gain_db = NULL;
-	s8 tiaA_gain_db_rev7[] = { -9, -6, -3, 0, 3, 3, 3, 3, 3, 3 };
-	s8 *tia_gain_db;
-	s8 tiaA_gainbits_rev7[] = { 0, 1, 2, 3, 4, 4, 4, 4, 4, 4 };
-	s8 *tia_gainbits;
-	u16 rfseqA_init_gain_rev7[] = { 0x624f, 0x624f };
-	u16 *rfseq_init_gain;
-	u16 init_gaincode;
-	u16 clip1hi_gaincode;
-	u16 clip1md_gaincode = 0;
-	u16 clip1md_gaincode_B;
-	u16 clip1lo_gaincode;
-	u16 clip1lo_gaincode_B;
-	u8 crsminl_th = 0;
-	u8 crsminu_th;
-	u16 nbclip_th = 0;
-	u8 w1clip_th;
-	u16 freq;
-	s8 nvar_baseline_offset0 = 0, nvar_baseline_offset1 = 0;
-	u8 chg_nbclip_th = 0;
+	if (D11REV_IS(pi->sh->corerev, 11) || D11REV_IS(pi->sh->corerev, 12)) {
+		wlapi_bmac_mctrl(pi->sh->physhim, MCTL_PHYLOCK, MCTL_PHYLOCK);
+		(void)R_REG(&pi->regs->maccontrol);
+		udelay(1);
+	}
 
-	mod_phy_reg(pi, 0x1c, (0x1 << 13), (1 << 13));
-	mod_phy_reg(pi, 0x32, (0x1 << 13), (1 << 13));
+	if (NREV_GE(pi->pubpi.phy_rev, 7))
+		mod_phy_reg(pi, 0x1e7, (0x7f << 0),
+			    (NPHY_TxPwrCtrlCmd_pwrIndex_init_rev7 << 0));
+	else
+		mod_phy_reg(pi, 0x1e7, (0x7f << 0),
+			    (NPHY_TxPwrCtrlCmd_pwrIndex_init << 0));
 
-	currband = read_phy_reg(pi, 0x09) & NPHY_BandControl_currentBand;
-	if (currband == 0) {
+	if (NREV_GE(pi->pubpi.phy_rev, 7))
+		mod_phy_reg(pi, 0x222, (0xff << 0),
+			    (NPHY_TxPwrCtrlCmd_pwrIndex_init_rev7 << 0));
+	else if (NREV_GT(pi->pubpi.phy_rev, 1))
+		mod_phy_reg(pi, 0x222, (0xff << 0),
+			    (NPHY_TxPwrCtrlCmd_pwrIndex_init << 0));
 
-		lna1_gain_db = lna1G_gain_db_rev7;
+	if (D11REV_IS(pi->sh->corerev, 11) || D11REV_IS(pi->sh->corerev, 12))
+		wlapi_bmac_mctrl(pi->sh->physhim, MCTL_PHYLOCK, 0);
 
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 4, 8, 8,
-					 lna1_gain_db);
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 4, 8, 8,
-					 lna1_gain_db);
+	write_phy_reg(pi, 0x1e8, (0x3 << 8) | (240 << 0));
 
-		mod_phy_reg(pi, 0x283, (0xff << 0), (0x40 << 0));
+	write_phy_reg(pi, 0x1e9,
+		      (1 << 15) | (idle_tssi[0] << 0) | (idle_tssi[1] << 8));
 
-		if (CHSPEC_IS40(pi->radio_chanspec)) {
-			mod_phy_reg(pi, 0x280, (0xff << 0), (0x3e << 0));
-			mod_phy_reg(pi, 0x283, (0xff << 0), (0x3e << 0));
+	write_phy_reg(pi, 0x1ea,
+		      (target_pwr_qtrdbm[0] << 0) |
+		      (target_pwr_qtrdbm[1] << 8));
+
+	tbl_len = 64;
+	tbl_offset = 0;
+	for (tbl_id = NPHY_TBL_ID_CORE1TXPWRCTL;
+	     tbl_id <= NPHY_TBL_ID_CORE2TXPWRCTL; tbl_id++) {
+
+		for (idx = 0; idx < tbl_len; idx++) {
+			num = 8 *
+			      (16 * b0[tbl_id - 26] + b1[tbl_id - 26] * idx);
+			den = 32768 + a1[tbl_id - 26] * idx;
+			pwr_est = max(((4 * num + den / 2) / den), -8);
+			if (NREV_LT(pi->pubpi.phy_rev, 3)) {
+				if (idx <=
+				    (uint) (31 - idle_tssi[tbl_id - 26] + 1))
+					pwr_est =
+						max(pwr_est,
+						    target_pwr_qtrdbm
+						    [tbl_id - 26] + 1);
+			}
+			regval[idx] = (u32) pwr_est;
 		}
+		wlc_phy_table_write_nphy(pi, tbl_id, tbl_len, tbl_offset, 32,
+					 regval);
+	}
 
-		mod_phy_reg(pi, 0x289, (0xff << 0), (0x46 << 0));
+	wlc_phy_txpwr_limit_to_tbl_nphy(pi);
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_CORE1TXPWRCTL, 84, 64, 8,
+				 pi->adj_pwr_tbl_nphy);
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_CORE2TXPWRCTL, 84, 64, 8,
+				 pi->adj_pwr_tbl_nphy);
 
-		if (CHSPEC_IS20(pi->radio_chanspec)) {
-			mod_phy_reg(pi, 0x300, (0x3f << 0), (13 << 0));
-			mod_phy_reg(pi, 0x301, (0x3f << 0), (13 << 0));
+	if (pi->phyhang_avoid)
+		wlc_phy_stay_in_carriersearch_nphy(pi, false);
+}
+
+static u32 *wlc_phy_get_ipa_gaintbl_nphy(struct brcms_phy *pi)
+{
+	u32 *tx_pwrctrl_tbl = NULL;
+
+	if (CHSPEC_IS2G(pi->radio_chanspec)) {
+		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			if ((pi->pubpi.radiorev == 4)
+			    || (pi->pubpi.radiorev == 6))
+				tx_pwrctrl_tbl =
+					nphy_tpc_txgain_ipa_2g_2057rev4n6;
+			else if (pi->pubpi.radiorev == 3)
+				tx_pwrctrl_tbl =
+					nphy_tpc_txgain_ipa_2g_2057rev3;
+			else if (pi->pubpi.radiorev == 5)
+				tx_pwrctrl_tbl =
+					nphy_tpc_txgain_ipa_2g_2057rev5;
+			else if ((pi->pubpi.radiorev == 7)
+				 || (pi->pubpi.radiorev == 8))
+				tx_pwrctrl_tbl =
+					nphy_tpc_txgain_ipa_2g_2057rev7;
+		} else if (NREV_IS(pi->pubpi.phy_rev, 6)) {
+			tx_pwrctrl_tbl = nphy_tpc_txgain_ipa_rev6;
+		} else if (NREV_IS(pi->pubpi.phy_rev, 5)) {
+			tx_pwrctrl_tbl = nphy_tpc_txgain_ipa_rev5;
+		} else {
+			tx_pwrctrl_tbl = nphy_tpc_txgain_ipa;
 		}
 	} else {
 
-		init_gaincode = 0x9e;
-		clip1hi_gaincode = 0x9e;
-		clip1md_gaincode_B = 0x24;
-		clip1lo_gaincode = 0x8a;
-		clip1lo_gaincode_B = 8;
-		rfseq_init_gain = rfseqA_init_gain_rev7;
+		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			if ((pi->pubpi.radiorev == 3) ||
+			    (pi->pubpi.radiorev == 4) ||
+			    (pi->pubpi.radiorev == 6))
+				tx_pwrctrl_tbl = nphy_tpc_txgain_ipa_5g_2057;
+			else if ((pi->pubpi.radiorev == 7)
+				 || (pi->pubpi.radiorev == 8))
+				tx_pwrctrl_tbl =
+					nphy_tpc_txgain_ipa_5g_2057rev7;
+		} else {
+			tx_pwrctrl_tbl = nphy_tpc_txgain_ipa_5g;
+		}
+	}
 
-		tia_gain_db = tiaA_gain_db_rev7;
-		tia_gainbits = tiaA_gainbits_rev7;
+	return tx_pwrctrl_tbl;
+}
 
-		freq = CHAN5G_FREQ(CHSPEC_CHANNEL(pi->radio_chanspec));
-		if (CHSPEC_IS20(pi->radio_chanspec)) {
+static void wlc_phy_restore_rssical_nphy(struct brcms_phy *pi)
+{
+	if (CHSPEC_IS2G(pi->radio_chanspec)) {
+		if (pi->nphy_rssical_chanspec_2G == 0)
+			return;
 
-			w1clip_th = 25;
-			clip1md_gaincode = 0x82;
+		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			mod_radio_reg(pi, RADIO_2057_NB_MASTER_CORE0,
+				      RADIO_2057_VCM_MASK,
+				      pi->rssical_cache.
+				      rssical_radio_regs_2G[0]);
+			mod_radio_reg(pi, RADIO_2057_NB_MASTER_CORE1,
+				      RADIO_2057_VCM_MASK,
+				      pi->rssical_cache.
+				      rssical_radio_regs_2G[1]);
+		} else {
+			mod_radio_reg(pi,
+				      RADIO_2056_RX_RSSI_MISC | RADIO_2056_RX0,
+				      RADIO_2056_VCM_MASK,
+				      pi->rssical_cache.
+				      rssical_radio_regs_2G[0]);
+			mod_radio_reg(pi,
+				      RADIO_2056_RX_RSSI_MISC | RADIO_2056_RX1,
+				      RADIO_2056_VCM_MASK,
+				      pi->rssical_cache.
+				      rssical_radio_regs_2G[1]);
+		}
 
-			if ((freq <= 5080) || (freq == 5825)) {
+		write_phy_reg(pi, 0x1a6,
+			      pi->rssical_cache.rssical_phyregs_2G[0]);
+		write_phy_reg(pi, 0x1ac,
+			      pi->rssical_cache.rssical_phyregs_2G[1]);
+		write_phy_reg(pi, 0x1b2,
+			      pi->rssical_cache.rssical_phyregs_2G[2]);
+		write_phy_reg(pi, 0x1b8,
+			      pi->rssical_cache.rssical_phyregs_2G[3]);
+		write_phy_reg(pi, 0x1a4,
+			      pi->rssical_cache.rssical_phyregs_2G[4]);
+		write_phy_reg(pi, 0x1aa,
+			      pi->rssical_cache.rssical_phyregs_2G[5]);
+		write_phy_reg(pi, 0x1b0,
+			      pi->rssical_cache.rssical_phyregs_2G[6]);
+		write_phy_reg(pi, 0x1b6,
+			      pi->rssical_cache.rssical_phyregs_2G[7]);
+		write_phy_reg(pi, 0x1a5,
+			      pi->rssical_cache.rssical_phyregs_2G[8]);
+		write_phy_reg(pi, 0x1ab,
+			      pi->rssical_cache.rssical_phyregs_2G[9]);
+		write_phy_reg(pi, 0x1b1,
+			      pi->rssical_cache.rssical_phyregs_2G[10]);
+		write_phy_reg(pi, 0x1b7,
+			      pi->rssical_cache.rssical_phyregs_2G[11]);
 
-				s8 lna1A_gain_db_rev7[] = { 11, 16, 20, 24 };
-				s8 lna1A_gain_db_2_rev7[] = {
-					11, 17, 22, 25 };
-				s8 lna2A_gain_db_rev7[] = { -1, 6, 10, 14 };
+	} else {
+		if (pi->nphy_rssical_chanspec_5G == 0)
+			return;
 
-				crsminu_th = 0x3e;
-				lna1_gain_db = lna1A_gain_db_rev7;
-				lna1_gain_db_2 = lna1A_gain_db_2_rev7;
-				lna2_gain_db = lna2A_gain_db_rev7;
-			} else if ((freq >= 5500) && (freq <= 5700)) {
+		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			mod_radio_reg(pi, RADIO_2057_NB_MASTER_CORE0,
+				      RADIO_2057_VCM_MASK,
+				      pi->rssical_cache.
+				      rssical_radio_regs_5G[0]);
+			mod_radio_reg(pi, RADIO_2057_NB_MASTER_CORE1,
+				      RADIO_2057_VCM_MASK,
+				      pi->rssical_cache.
+				      rssical_radio_regs_5G[1]);
+		} else {
+			mod_radio_reg(pi,
+				      RADIO_2056_RX_RSSI_MISC | RADIO_2056_RX0,
+				      RADIO_2056_VCM_MASK,
+				      pi->rssical_cache.
+				      rssical_radio_regs_5G[0]);
+			mod_radio_reg(pi,
+				      RADIO_2056_RX_RSSI_MISC | RADIO_2056_RX1,
+				      RADIO_2056_VCM_MASK,
+				      pi->rssical_cache.
+				      rssical_radio_regs_5G[1]);
+		}
 
-				s8 lna1A_gain_db_rev7[] = { 11, 17, 21, 25 };
-				s8 lna1A_gain_db_2_rev7[] = {
-					12, 18, 22, 26 };
-				s8 lna2A_gain_db_rev7[] = { 1, 8, 12, 16 };
+		write_phy_reg(pi, 0x1a6,
+			      pi->rssical_cache.rssical_phyregs_5G[0]);
+		write_phy_reg(pi, 0x1ac,
+			      pi->rssical_cache.rssical_phyregs_5G[1]);
+		write_phy_reg(pi, 0x1b2,
+			      pi->rssical_cache.rssical_phyregs_5G[2]);
+		write_phy_reg(pi, 0x1b8,
+			      pi->rssical_cache.rssical_phyregs_5G[3]);
+		write_phy_reg(pi, 0x1a4,
+			      pi->rssical_cache.rssical_phyregs_5G[4]);
+		write_phy_reg(pi, 0x1aa,
+			      pi->rssical_cache.rssical_phyregs_5G[5]);
+		write_phy_reg(pi, 0x1b0,
+			      pi->rssical_cache.rssical_phyregs_5G[6]);
+		write_phy_reg(pi, 0x1b6,
+			      pi->rssical_cache.rssical_phyregs_5G[7]);
+		write_phy_reg(pi, 0x1a5,
+			      pi->rssical_cache.rssical_phyregs_5G[8]);
+		write_phy_reg(pi, 0x1ab,
+			      pi->rssical_cache.rssical_phyregs_5G[9]);
+		write_phy_reg(pi, 0x1b1,
+			      pi->rssical_cache.rssical_phyregs_5G[10]);
+		write_phy_reg(pi, 0x1b7,
+			      pi->rssical_cache.rssical_phyregs_5G[11]);
+	}
+}
 
-				crsminu_th = 0x45;
-				clip1md_gaincode_B = 0x14;
-				nbclip_th = 0xff;
-				chg_nbclip_th = 1;
-				lna1_gain_db = lna1A_gain_db_rev7;
-				lna1_gain_db_2 = lna1A_gain_db_2_rev7;
-				lna2_gain_db = lna2A_gain_db_rev7;
+static void wlc_phy_internal_cal_txgain_nphy(struct brcms_phy *pi)
+{
+	u16 txcal_gain[2];
+
+	pi->nphy_txcal_pwr_idx[0] = pi->nphy_cal_orig_pwr_idx[0];
+	pi->nphy_txcal_pwr_idx[1] = pi->nphy_cal_orig_pwr_idx[0];
+	wlc_phy_txpwr_index_nphy(pi, 1, pi->nphy_cal_orig_pwr_idx[0], true);
+	wlc_phy_txpwr_index_nphy(pi, 2, pi->nphy_cal_orig_pwr_idx[1], true);
+
+	wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_RFSEQ, 2, 0x110, 16,
+				txcal_gain);
+
+	if (CHSPEC_IS2G(pi->radio_chanspec)) {
+		txcal_gain[0] = (txcal_gain[0] & 0xF000) | 0x0F40;
+		txcal_gain[1] = (txcal_gain[1] & 0xF000) | 0x0F40;
+	} else {
+		txcal_gain[0] = (txcal_gain[0] & 0xF000) | 0x0F60;
+		txcal_gain[1] = (txcal_gain[1] & 0xF000) | 0x0F60;
+	}
+
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ, 2, 0x110, 16,
+				 txcal_gain);
+}
+
+static void wlc_phy_precal_txgain_nphy(struct brcms_phy *pi)
+{
+	bool save_bbmult = false;
+	u8 txcal_index_2057_rev5n7 = 0;
+	u8 txcal_index_2057_rev3n4n6 = 10;
+
+	if (pi->use_int_tx_iqlo_cal_nphy) {
+		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			if ((pi->pubpi.radiorev == 3) ||
+			    (pi->pubpi.radiorev == 4) ||
+			    (pi->pubpi.radiorev == 6)) {
+
+				pi->nphy_txcal_pwr_idx[0] =
+					txcal_index_2057_rev3n4n6;
+				pi->nphy_txcal_pwr_idx[1] =
+					txcal_index_2057_rev3n4n6;
+				wlc_phy_txpwr_index_nphy(
+					pi, 3,
+					txcal_index_2057_rev3n4n6,
+					false);
 			} else {
 
-				s8 lna1A_gain_db_rev7[] = { 12, 18, 22, 26 };
-				s8 lna1A_gain_db_2_rev7[] = {
-					12, 18, 22, 26 };
-				s8 lna2A_gain_db_rev7[] = { -1, 6, 10, 14 };
+				pi->nphy_txcal_pwr_idx[0] =
+					txcal_index_2057_rev5n7;
+				pi->nphy_txcal_pwr_idx[1] =
+					txcal_index_2057_rev5n7;
+				wlc_phy_txpwr_index_nphy(
+					pi, 3,
+					txcal_index_2057_rev5n7,
+					false);
+			}
+			save_bbmult = true;
 
-				crsminu_th = 0x41;
-				lna1_gain_db = lna1A_gain_db_rev7;
-				lna1_gain_db_2 = lna1A_gain_db_2_rev7;
-				lna2_gain_db = lna2A_gain_db_rev7;
+		} else if (NREV_LT(pi->pubpi.phy_rev, 5)) {
+			wlc_phy_cal_txgainctrl_nphy(pi, 11, false);
+			if (pi->sh->hw_phytxchain != 3) {
+				pi->nphy_txcal_pwr_idx[1] =
+					pi->nphy_txcal_pwr_idx[0];
+				wlc_phy_txpwr_index_nphy(pi, 3,
+							 pi->
+							 nphy_txcal_pwr_idx[0],
+							 true);
+				save_bbmult = true;
 			}
 
-			if (freq <= 4920) {
-				nvar_baseline_offset0 = 5;
-				nvar_baseline_offset1 = 5;
-			} else if ((freq > 4920) && (freq <= 5320)) {
-				nvar_baseline_offset0 = 3;
-				nvar_baseline_offset1 = 5;
-			} else if ((freq > 5320) && (freq <= 5700)) {
-				nvar_baseline_offset0 = 3;
-				nvar_baseline_offset1 = 2;
+		} else if (NREV_IS(pi->pubpi.phy_rev, 5)) {
+			if (PHY_IPA(pi)) {
+				if (CHSPEC_IS2G(pi->radio_chanspec)) {
+					wlc_phy_cal_txgainctrl_nphy(pi, 12,
+								    false);
+				} else {
+					pi->nphy_txcal_pwr_idx[0] = 80;
+					pi->nphy_txcal_pwr_idx[1] = 80;
+					wlc_phy_txpwr_index_nphy(pi, 3, 80,
+								 false);
+					save_bbmult = true;
+				}
 			} else {
-				nvar_baseline_offset0 = 4;
-				nvar_baseline_offset1 = 0;
+				wlc_phy_internal_cal_txgain_nphy(pi);
+				save_bbmult = true;
 			}
-		} else {
-
-			crsminu_th = 0x3a;
-			crsminl_th = 0x3a;
-			w1clip_th = 20;
 
-			if ((freq >= 4920) && (freq <= 5320)) {
-				nvar_baseline_offset0 = 4;
-				nvar_baseline_offset1 = 5;
-			} else if ((freq > 5320) && (freq <= 5550)) {
-				nvar_baseline_offset0 = 4;
-				nvar_baseline_offset1 = 2;
+		} else if (NREV_IS(pi->pubpi.phy_rev, 6)) {
+			if (PHY_IPA(pi)) {
+				if (CHSPEC_IS2G(pi->radio_chanspec))
+					wlc_phy_cal_txgainctrl_nphy(pi, 12,
+								    false);
+				else
+					wlc_phy_cal_txgainctrl_nphy(pi, 14,
+								    false);
 			} else {
-				nvar_baseline_offset0 = 5;
-				nvar_baseline_offset1 = 3;
+				wlc_phy_internal_cal_txgain_nphy(pi);
+				save_bbmult = true;
 			}
 		}
 
-		write_phy_reg(pi, 0x20, init_gaincode);
-		write_phy_reg(pi, 0x2a7, init_gaincode);
+	} else {
+		wlc_phy_cal_txgainctrl_nphy(pi, 10, false);
+	}
 
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ,
-					 pi->pubpi.phy_corenum, 0x106, 16,
-					 rfseq_init_gain);
+	if (save_bbmult)
+		wlc_phy_table_read_nphy(pi, 15, 1, 87, 16,
+					&pi->nphy_txcal_bbmult);
+}
 
-		write_phy_reg(pi, 0x22, clip1hi_gaincode);
-		write_phy_reg(pi, 0x2a9, clip1hi_gaincode);
+static void
+wlc_phy_rfctrlintc_override_nphy(struct brcms_phy *pi, u8 field, u16 value,
+				 u8 core_code)
+{
+	u16 mask;
+	u16 val;
+	u8 core;
 
-		write_phy_reg(pi, 0x36, clip1md_gaincode_B);
-		write_phy_reg(pi, 0x2ac, clip1md_gaincode_B);
+	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+		for (core = 0; core < pi->pubpi.phy_corenum; core++) {
+			if (core_code == RADIO_MIMO_CORESEL_CORE1
+			    && core == PHY_CORE_1)
+				continue;
+			else if (core_code == RADIO_MIMO_CORESEL_CORE2
+				 && core == PHY_CORE_0)
+				continue;
 
-		write_phy_reg(pi, 0x37, clip1lo_gaincode);
-		write_phy_reg(pi, 0x2ad, clip1lo_gaincode);
-		write_phy_reg(pi, 0x38, clip1lo_gaincode_B);
-		write_phy_reg(pi, 0x2ae, clip1lo_gaincode_B);
+			if (NREV_LT(pi->pubpi.phy_rev, 7)) {
 
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 10, 0x20, 8,
-					 tia_gain_db);
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 10, 0x20, 8,
-					 tia_gain_db);
+				mask = (0x1 << 10);
+				val = 1 << 10;
+				mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x91 :
+					    0x92, mask, val);
+			}
 
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS1, 10, 0x20, 8,
-					 tia_gainbits);
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAINBITS2, 10, 0x20, 8,
-					 tia_gainbits);
+			if (field == NPHY_RfctrlIntc_override_OFF) {
 
-		mod_phy_reg(pi, 0x283, (0xff << 0), (crsminu_th << 0));
+				write_phy_reg(pi, (core == PHY_CORE_0) ? 0x91 :
+					      0x92, 0);
 
-		if (chg_nbclip_th == 1) {
-			write_phy_reg(pi, 0x2b, nbclip_th);
-			write_phy_reg(pi, 0x41, nbclip_th);
-		}
+				wlc_phy_force_rfseq_nphy(pi,
+							 NPHY_RFSEQ_RESET2RX);
+			} else if (field == NPHY_RfctrlIntc_override_TRSW) {
 
-		mod_phy_reg(pi, 0x300, (0x3f << 0), (w1clip_th << 0));
-		mod_phy_reg(pi, 0x301, (0x3f << 0), (w1clip_th << 0));
+				if (NREV_GE(pi->pubpi.phy_rev, 7)) {
 
-		mod_phy_reg(pi, 0x2e4,
-			    (0x3f << 0), (nvar_baseline_offset0 << 0));
+					mask = (0x1 << 6) | (0x1 << 7);
 
-		mod_phy_reg(pi, 0x2e4,
-			    (0x3f << 6), (nvar_baseline_offset1 << 6));
+					val = value << 6;
+					mod_phy_reg(pi,
+						    (core ==
+						     PHY_CORE_0) ? 0x91 : 0x92,
+						    mask, val);
 
-		if (CHSPEC_IS20(pi->radio_chanspec)) {
+					or_phy_reg(pi,
+						   (core ==
+						    PHY_CORE_0) ? 0x91 : 0x92,
+						   (0x1 << 10));
 
-			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 4, 8, 8,
-						 lna1_gain_db);
-			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 4, 8, 8,
-						 lna1_gain_db_2);
+					and_phy_reg(pi, 0x2ff, (u16)
+						    ~(0x3 << 14));
+					or_phy_reg(pi, 0x2ff, (0x1 << 13));
+					or_phy_reg(pi, 0x2ff, (0x1 << 0));
+				} else {
 
-			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN1, 4, 0x10,
-						 8, lna2_gain_db);
-			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_GAIN2, 4, 0x10,
-						 8, lna2_gain_db);
+					mask = (0x1 << 6) |
+					       (0x1 << 7) |
+					       (0x1 << 8) | (0x1 << 9);
+					val = value << 6;
+					mod_phy_reg(pi,
+						    (core ==
+						     PHY_CORE_0) ? 0x91 : 0x92,
+						    mask, val);
 
-			write_phy_reg(pi, 0x24, clip1md_gaincode);
-			write_phy_reg(pi, 0x2ab, clip1md_gaincode);
-		} else {
-			mod_phy_reg(pi, 0x280, (0xff << 0), (crsminl_th << 0));
-		}
+					mask = (0x1 << 0);
+					val = 1 << 0;
+					mod_phy_reg(pi,
+						    (core ==
+						     PHY_CORE_0) ? 0xe7 : 0xec,
+						    mask, val);
 
-	}
+					mask = (core == PHY_CORE_0) ?
+					       (0x1 << 0) : (0x1 << 1);
+					val = 1 << ((core == PHY_CORE_0) ?
+						    0 : 1);
+					mod_phy_reg(pi, 0x78, mask, val);
+
+					SPINWAIT(((read_phy_reg(pi, 0x78) & val)
+						  != 0), 10000);
+					if (WARN(read_phy_reg(pi, 0x78) & val,
+						 "HW error: override failed"))
+						return;
+
+					mask = (0x1 << 0);
+					val = 0 << 0;
+					mod_phy_reg(pi,
+						    (core ==
+						     PHY_CORE_0) ? 0xe7 : 0xec,
+						    mask, val);
+				}
+			} else if (field == NPHY_RfctrlIntc_override_PA) {
+				if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+
+					mask = (0x1 << 4) | (0x1 << 5);
+
+					if (CHSPEC_IS5G(pi->radio_chanspec))
+						val = value << 5;
+					else
+						val = value << 4;
+
+					mod_phy_reg(pi,
+						    (core ==
+						     PHY_CORE_0) ? 0x91 : 0x92,
+						    mask, val);
 
+					or_phy_reg(pi,
+						   (core ==
+						    PHY_CORE_0) ? 0x91 : 0x92,
+						   (0x1 << 12));
+				} else {
+
+					if (CHSPEC_IS5G(pi->radio_chanspec)) {
+						mask = (0x1 << 5);
+						val = value << 5;
+					} else {
+						mask = (0x1 << 4);
+						val = value << 4;
+					}
+					mod_phy_reg(pi,
+						    (core ==
+						     PHY_CORE_0) ? 0x91 : 0x92,
+						    mask, val);
+				}
+			} else if (field ==
+				   NPHY_RfctrlIntc_override_EXT_LNA_PU) {
+				if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+					if (CHSPEC_IS5G(pi->radio_chanspec)) {
+
+						mask = (0x1 << 0);
+						val = value << 0;
+						mod_phy_reg(pi,
+							    (core ==
+							     PHY_CORE_0) ? 0x91
+							    : 0x92, mask, val);
+
+						mask = (0x1 << 2);
+						mod_phy_reg(pi,
+							    (core ==
+							     PHY_CORE_0) ? 0x91
+							    : 0x92, mask, 0);
+					} else {
+
+						mask = (0x1 << 2);
+						val = value << 2;
+						mod_phy_reg(pi,
+							    (core ==
+							     PHY_CORE_0) ? 0x91
+							    : 0x92, mask, val);
+
+						mask = (0x1 << 0);
+						mod_phy_reg(pi,
+							    (core ==
+							     PHY_CORE_0) ? 0x91
+							    : 0x92, mask, 0);
+					}
+
+					mask = (0x1 << 11);
+					val = 1 << 11;
+					mod_phy_reg(pi,
+						    (core ==
+						     PHY_CORE_0) ? 0x91 : 0x92,
+						    mask, val);
+				} else {
+
+					if (CHSPEC_IS5G(pi->radio_chanspec)) {
+						mask = (0x1 << 0);
+						val = value << 0;
+					} else {
+						mask = (0x1 << 2);
+						val = value << 2;
+					}
+					mod_phy_reg(pi,
+						    (core ==
+						     PHY_CORE_0) ? 0x91 : 0x92,
+						    mask, val);
+				}
+			} else if (field ==
+				   NPHY_RfctrlIntc_override_EXT_LNA_GAIN) {
+				if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+					if (CHSPEC_IS5G(pi->radio_chanspec)) {
+
+						mask = (0x1 << 1);
+						val = value << 1;
+						mod_phy_reg(pi,
+							    (core ==
+							     PHY_CORE_0) ? 0x91
+							    : 0x92, mask, val);
+
+						mask = (0x1 << 3);
+						mod_phy_reg(pi,
+							    (core ==
+							     PHY_CORE_0) ? 0x91
+							    : 0x92, mask, 0);
+					} else {
+
+						mask = (0x1 << 3);
+						val = value << 3;
+						mod_phy_reg(pi,
+							    (core ==
+							     PHY_CORE_0) ? 0x91
+							    : 0x92, mask, val);
+
+						mask = (0x1 << 1);
+						mod_phy_reg(pi,
+							    (core ==
+							     PHY_CORE_0) ? 0x91
+							    : 0x92, mask, 0);
+					}
+
+					mask = (0x1 << 11);
+					val = 1 << 11;
+					mod_phy_reg(pi,
+						    (core ==
+						     PHY_CORE_0) ? 0x91 : 0x92,
+						    mask, val);
+				} else {
+
+					if (CHSPEC_IS5G(pi->radio_chanspec)) {
+						mask = (0x1 << 1);
+						val = value << 1;
+					} else {
+						mask = (0x1 << 3);
+						val = value << 3;
+					}
+					mod_phy_reg(pi,
+						    (core ==
+						     PHY_CORE_0) ? 0x91 : 0x92,
+						    mask, val);
+				}
+			}
+		}
+	}
 }
 
-static void wlc_phy_adjust_lnagaintbl_nphy(struct brcms_phy *pi)
+void
+wlc_phy_cal_txgainctrl_nphy(struct brcms_phy *pi, s32 dBm_targetpower,
+			    bool debug)
 {
+	int gainctrl_loopidx;
 	uint core;
-	int ctr;
-	s16 gain_delta[2];
-	u8 curr_channel;
-	u16 minmax_gain[2];
-	u16 regval[4];
+	u16 m0m1, curr_m0m1;
+	s32 delta_power;
+	s32 txpwrindex;
+	s32 qdBm_power[2];
+	u16 orig_BBConfig;
+	u16 phy_saveregs[4];
+	u32 freq_test;
+	u16 ampl_test = 250;
+	uint stepsize;
+	bool phyhang_avoid_state = false;
+
+	if (NREV_GE(pi->pubpi.phy_rev, 7))
+		stepsize = 2;
+	else
+		stepsize = 1;
+
+	if (CHSPEC_IS40(pi->radio_chanspec))
+		freq_test = 5000;
+	else
+		freq_test = 2500;
+
+	wlc_phy_txpwr_index_nphy(pi, 1, pi->nphy_cal_orig_pwr_idx[0], true);
+	wlc_phy_txpwr_index_nphy(pi, 2, pi->nphy_cal_orig_pwr_idx[1], true);
 
 	if (pi->phyhang_avoid)
 		wlc_phy_stay_in_carriersearch_nphy(pi, true);
 
-	if (pi->nphy_gain_boost) {
-		if ((CHSPEC_IS2G(pi->radio_chanspec))) {
+	phyhang_avoid_state = pi->phyhang_avoid;
+	pi->phyhang_avoid = false;
 
-			gain_delta[0] = 6;
-			gain_delta[1] = 6;
-		} else {
+	phy_saveregs[0] = read_phy_reg(pi, 0x91);
+	phy_saveregs[1] = read_phy_reg(pi, 0x92);
+	phy_saveregs[2] = read_phy_reg(pi, 0xe7);
+	phy_saveregs[3] = read_phy_reg(pi, 0xec);
+	wlc_phy_rfctrlintc_override_nphy(pi, NPHY_RfctrlIntc_override_PA, 1,
+					 RADIO_MIMO_CORESEL_CORE1 |
+					 RADIO_MIMO_CORESEL_CORE2);
 
-			curr_channel = CHSPEC_CHANNEL(pi->radio_chanspec);
-			gain_delta[0] =
-			    (s16)
-			    PHY_HW_ROUND(((nphy_lnagain_est0[0] *
-					   curr_channel) +
-					  nphy_lnagain_est0[1]), 13);
-			gain_delta[1] =
-			    (s16)
-			    PHY_HW_ROUND(((nphy_lnagain_est1[0] *
-					   curr_channel) +
-					  nphy_lnagain_est1[1]), 13);
-		}
+	if (!debug) {
+		wlc_phy_rfctrlintc_override_nphy(pi,
+						 NPHY_RfctrlIntc_override_TRSW,
+						 0x2, RADIO_MIMO_CORESEL_CORE1);
+		wlc_phy_rfctrlintc_override_nphy(pi,
+						 NPHY_RfctrlIntc_override_TRSW,
+						 0x8, RADIO_MIMO_CORESEL_CORE2);
 	} else {
-
-		gain_delta[0] = 0;
-		gain_delta[1] = 0;
+		wlc_phy_rfctrlintc_override_nphy(pi,
+						 NPHY_RfctrlIntc_override_TRSW,
+						 0x1, RADIO_MIMO_CORESEL_CORE1);
+		wlc_phy_rfctrlintc_override_nphy(pi,
+						 NPHY_RfctrlIntc_override_TRSW,
+						 0x7, RADIO_MIMO_CORESEL_CORE2);
 	}
 
+	orig_BBConfig = read_phy_reg(pi, 0x01);
+	mod_phy_reg(pi, 0x01, (0x1 << 15), 0);
+
+	wlc_phy_table_read_nphy(pi, 15, 1, 87, 16, &m0m1);
+
 	for (core = 0; core < pi->pubpi.phy_corenum; core++) {
-		if (pi->nphy_elna_gain_config) {
+		txpwrindex = (s32) pi->nphy_cal_orig_pwr_idx[core];
 
-			regval[0] = nphy_def_lnagains[2] + gain_delta[core];
-			regval[1] = nphy_def_lnagains[3] + gain_delta[core];
-			regval[2] = nphy_def_lnagains[3] + gain_delta[core];
-			regval[3] = nphy_def_lnagains[3] + gain_delta[core];
-		} else {
-			for (ctr = 0; ctr < 4; ctr++) {
-				regval[ctr] =
-				    nphy_def_lnagains[ctr] + gain_delta[core];
+		for (gainctrl_loopidx = 0; gainctrl_loopidx < 2;
+		     gainctrl_loopidx++) {
+			wlc_phy_tx_tone_nphy(pi, freq_test, ampl_test, 0, 0,
+					     false);
+
+			if (core == PHY_CORE_0)
+				curr_m0m1 = m0m1 & 0xff00;
+			else
+				curr_m0m1 = m0m1 & 0x00ff;
+
+			wlc_phy_table_write_nphy(pi, 15, 1, 87, 16, &curr_m0m1);
+			wlc_phy_table_write_nphy(pi, 15, 1, 95, 16, &curr_m0m1);
+
+			udelay(50);
+
+			wlc_phy_est_tonepwr_nphy(pi, qdBm_power,
+						 NPHY_CAL_TSSISAMPS);
+
+			pi->nphy_bb_mult_save = 0;
+			wlc_phy_stopplayback_nphy(pi);
+
+			delta_power = (dBm_targetpower * 4) - qdBm_power[core];
+
+			txpwrindex -= stepsize * delta_power;
+			if (txpwrindex < 0)
+				txpwrindex = 0;
+			else if (txpwrindex > 127)
+				txpwrindex = 127;
+
+			if (CHSPEC_IS5G(pi->radio_chanspec)) {
+				if (NREV_IS(pi->pubpi.phy_rev, 4) &&
+				    (pi->srom_fem5g.extpagain == 3)) {
+					if (txpwrindex < 30)
+						txpwrindex = 30;
+				}
+			} else {
+				if (NREV_GE(pi->pubpi.phy_rev, 5) &&
+				    (pi->srom_fem2g.extpagain == 3)) {
+					if (txpwrindex < 50)
+						txpwrindex = 50;
+				}
 			}
+
+			wlc_phy_txpwr_index_nphy(pi, (1 << core),
+						 (u8) txpwrindex, true);
 		}
-		wlc_phy_table_write_nphy(pi, core, 4, 8, 16, regval);
 
-		minmax_gain[core] =
-		    (u16) (nphy_def_lnagains[2] + gain_delta[core] + 4);
+		pi->nphy_txcal_pwr_idx[core] = (u8) txpwrindex;
+
+		if (debug) {
+			u16 radio_gain;
+			u16 dbg_m0m1;
+
+			wlc_phy_table_read_nphy(pi, 15, 1, 87, 16, &dbg_m0m1);
+
+			wlc_phy_tx_tone_nphy(pi, freq_test, ampl_test, 0, 0,
+					     false);
+
+			wlc_phy_table_write_nphy(pi, 15, 1, 87, 16, &dbg_m0m1);
+			wlc_phy_table_write_nphy(pi, 15, 1, 95, 16, &dbg_m0m1);
+
+			udelay(100);
+
+			wlc_phy_est_tonepwr_nphy(pi, qdBm_power,
+						 NPHY_CAL_TSSISAMPS);
+
+			wlc_phy_table_read_nphy(pi, 7, 1, (0x110 + core), 16,
+						&radio_gain);
+
+			mdelay(4000);
+			pi->nphy_bb_mult_save = 0;
+			wlc_phy_stopplayback_nphy(pi);
+		}
 	}
 
-	mod_phy_reg(pi, 0x1e, (0xff << 0), (minmax_gain[0] << 0));
-	mod_phy_reg(pi, 0x34, (0xff << 0), (minmax_gain[1] << 0));
+	wlc_phy_txpwr_index_nphy(pi, 1, pi->nphy_txcal_pwr_idx[0], true);
+	wlc_phy_txpwr_index_nphy(pi, 2, pi->nphy_txcal_pwr_idx[1], true);
+
+	wlc_phy_table_read_nphy(pi, 15, 1, 87, 16, &pi->nphy_txcal_bbmult);
+
+	write_phy_reg(pi, 0x01, orig_BBConfig);
+
+	write_phy_reg(pi, 0x91, phy_saveregs[0]);
+	write_phy_reg(pi, 0x92, phy_saveregs[1]);
+	write_phy_reg(pi, 0xe7, phy_saveregs[2]);
+	write_phy_reg(pi, 0xec, phy_saveregs[3]);
+
+	pi->phyhang_avoid = phyhang_avoid_state;
 
 	if (pi->phyhang_avoid)
 		wlc_phy_stay_in_carriersearch_nphy(pi, false);
 }
 
-void wlc_phy_switch_radio_nphy(struct brcms_phy *pi, bool on)
+static void wlc_phy_savecal_nphy(struct brcms_phy *pi)
 {
-	if (on) {
+	void *tbl_ptr;
+	int coreNum;
+	u16 *txcal_radio_regs = NULL;
+
+	if (pi->phyhang_avoid)
+		wlc_phy_stay_in_carriersearch_nphy(pi, true);
+
+	if (CHSPEC_IS2G(pi->radio_chanspec)) {
+
+		wlc_phy_rx_iq_coeffs_nphy(pi, 0,
+					  &pi->calibration_cache.
+					  rxcal_coeffs_2G);
+
 		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			if (!pi->radio_is_on) {
-				wlc_phy_radio_preinit_205x(pi);
-				wlc_phy_radio_init_2057(pi);
-				wlc_phy_radio_postinit_2057(pi);
-			}
+			txcal_radio_regs =
+				pi->calibration_cache.txcal_radio_regs_2G;
+		} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
 
-			wlc_phy_chanspec_set((struct brcms_phy_pub *) pi,
-					     pi->radio_chanspec);
+			pi->calibration_cache.txcal_radio_regs_2G[0] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_FINE_I |
+					       RADIO_2056_TX0);
+			pi->calibration_cache.txcal_radio_regs_2G[1] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_FINE_Q |
+					       RADIO_2056_TX0);
+			pi->calibration_cache.txcal_radio_regs_2G[2] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_FINE_I |
+					       RADIO_2056_TX1);
+			pi->calibration_cache.txcal_radio_regs_2G[3] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_FINE_Q |
+					       RADIO_2056_TX1);
+
+			pi->calibration_cache.txcal_radio_regs_2G[4] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_COARSE_I |
+					       RADIO_2056_TX0);
+			pi->calibration_cache.txcal_radio_regs_2G[5] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_COARSE_Q |
+					       RADIO_2056_TX0);
+			pi->calibration_cache.txcal_radio_regs_2G[6] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_COARSE_I |
+					       RADIO_2056_TX1);
+			pi->calibration_cache.txcal_radio_regs_2G[7] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_COARSE_Q |
+					       RADIO_2056_TX1);
+		} else {
+			pi->calibration_cache.txcal_radio_regs_2G[0] =
+			       read_radio_reg(pi, RADIO_2055_CORE1_TX_VOS_CNCL);
+			pi->calibration_cache.txcal_radio_regs_2G[1] =
+			       read_radio_reg(pi, RADIO_2055_CORE2_TX_VOS_CNCL);
+			pi->calibration_cache.txcal_radio_regs_2G[2] =
+			       read_radio_reg(pi, RADIO_2055_CORE1_TX_BB_MXGM);
+			pi->calibration_cache.txcal_radio_regs_2G[3] =
+			       read_radio_reg(pi, RADIO_2055_CORE2_TX_BB_MXGM);
+		}
+
+		pi->nphy_iqcal_chanspec_2G = pi->radio_chanspec;
+		tbl_ptr = pi->calibration_cache.txcal_coeffs_2G;
+	} else {
+
+		wlc_phy_rx_iq_coeffs_nphy(pi, 0,
+					  &pi->calibration_cache.
+					  rxcal_coeffs_5G);
+
+		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			txcal_radio_regs =
+				pi->calibration_cache.txcal_radio_regs_5G;
 		} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-			wlc_phy_radio_preinit_205x(pi);
-			wlc_phy_radio_init_2056(pi);
-			wlc_phy_radio_postinit_2056(pi);
 
-			wlc_phy_chanspec_set((struct brcms_phy_pub *) pi,
-					     pi->radio_chanspec);
+			pi->calibration_cache.txcal_radio_regs_5G[0] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_FINE_I |
+					       RADIO_2056_TX0);
+			pi->calibration_cache.txcal_radio_regs_5G[1] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_FINE_Q |
+					       RADIO_2056_TX0);
+			pi->calibration_cache.txcal_radio_regs_5G[2] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_FINE_I |
+					       RADIO_2056_TX1);
+			pi->calibration_cache.txcal_radio_regs_5G[3] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_FINE_Q |
+					       RADIO_2056_TX1);
+
+			pi->calibration_cache.txcal_radio_regs_5G[4] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_COARSE_I |
+					       RADIO_2056_TX0);
+			pi->calibration_cache.txcal_radio_regs_5G[5] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_COARSE_Q |
+					       RADIO_2056_TX0);
+			pi->calibration_cache.txcal_radio_regs_5G[6] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_COARSE_I |
+					       RADIO_2056_TX1);
+			pi->calibration_cache.txcal_radio_regs_5G[7] =
+				read_radio_reg(pi,
+					       RADIO_2056_TX_LOFT_COARSE_Q |
+					       RADIO_2056_TX1);
 		} else {
-			wlc_phy_radio_preinit_2055(pi);
-			wlc_phy_radio_init_2055(pi);
-			wlc_phy_radio_postinit_2055(pi);
+			pi->calibration_cache.txcal_radio_regs_5G[0] =
+			       read_radio_reg(pi, RADIO_2055_CORE1_TX_VOS_CNCL);
+			pi->calibration_cache.txcal_radio_regs_5G[1] =
+			       read_radio_reg(pi, RADIO_2055_CORE2_TX_VOS_CNCL);
+			pi->calibration_cache.txcal_radio_regs_5G[2] =
+			       read_radio_reg(pi, RADIO_2055_CORE1_TX_BB_MXGM);
+			pi->calibration_cache.txcal_radio_regs_5G[3] =
+			       read_radio_reg(pi, RADIO_2055_CORE2_TX_BB_MXGM);
 		}
 
-		pi->radio_is_on = true;
+		pi->nphy_iqcal_chanspec_5G = pi->radio_chanspec;
+		tbl_ptr = pi->calibration_cache.txcal_coeffs_5G;
+	}
+	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+		for (coreNum = 0; coreNum <= 1; coreNum++) {
+
+			txcal_radio_regs[2 * coreNum] =
+				READ_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
+						LOFT_FINE_I);
+			txcal_radio_regs[2 * coreNum + 1] =
+				READ_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
+						LOFT_FINE_Q);
+
+			txcal_radio_regs[2 * coreNum + 4] =
+				READ_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
+						LOFT_COARSE_I);
+			txcal_radio_regs[2 * coreNum + 5] =
+				READ_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
+						LOFT_COARSE_Q);
+		}
+	}
 
+	wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_IQLOCAL, 8, 80, 16, tbl_ptr);
+
+	if (pi->phyhang_avoid)
+		wlc_phy_stay_in_carriersearch_nphy(pi, false);
+}
+
+static void wlc_phy_tx_iq_war_nphy(struct brcms_phy *pi)
+{
+	struct nphy_iq_comp tx_comp;
+
+	wlc_phy_table_read_nphy(pi, 15, 4, 0x50, 16, &tx_comp);
+
+	wlapi_bmac_write_shm(pi->sh->physhim, M_20IN40_IQ, tx_comp.a0);
+	wlapi_bmac_write_shm(pi->sh->physhim, M_20IN40_IQ + 2, tx_comp.b0);
+	wlapi_bmac_write_shm(pi->sh->physhim, M_20IN40_IQ + 4, tx_comp.a1);
+	wlapi_bmac_write_shm(pi->sh->physhim, M_20IN40_IQ + 6, tx_comp.b1);
+}
+
+static void wlc_phy_restorecal_nphy(struct brcms_phy *pi)
+{
+	u16 *loft_comp;
+	u16 txcal_coeffs_bphy[4];
+	u16 *tbl_ptr;
+	int coreNum;
+	u16 *txcal_radio_regs = NULL;
+
+	if (CHSPEC_IS2G(pi->radio_chanspec)) {
+		if (pi->nphy_iqcal_chanspec_2G == 0)
+			return;
+
+		tbl_ptr = pi->calibration_cache.txcal_coeffs_2G;
+		loft_comp = &pi->calibration_cache.txcal_coeffs_2G[5];
 	} else {
+		if (pi->nphy_iqcal_chanspec_5G == 0)
+			return;
 
-		if (NREV_GE(pi->pubpi.phy_rev, 3)
-		    && NREV_LT(pi->pubpi.phy_rev, 7)) {
-			and_phy_reg(pi, 0x78, ~RFCC_CHIP0_PU);
-			mod_radio_reg(pi, RADIO_2056_SYN_COM_PU, 0x2, 0x0);
+		tbl_ptr = pi->calibration_cache.txcal_coeffs_5G;
+		loft_comp = &pi->calibration_cache.txcal_coeffs_5G[5];
+	}
+
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_IQLOCAL, 4, 80, 16, tbl_ptr);
+
+	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+		txcal_coeffs_bphy[0] = tbl_ptr[0];
+		txcal_coeffs_bphy[1] = tbl_ptr[1];
+		txcal_coeffs_bphy[2] = tbl_ptr[2];
+		txcal_coeffs_bphy[3] = tbl_ptr[3];
+	} else {
+		txcal_coeffs_bphy[0] = 0;
+		txcal_coeffs_bphy[1] = 0;
+		txcal_coeffs_bphy[2] = 0;
+		txcal_coeffs_bphy[3] = 0;
+	}
+
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_IQLOCAL, 4, 88, 16,
+				 txcal_coeffs_bphy);
+
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_IQLOCAL, 2, 85, 16, loft_comp);
+
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_IQLOCAL, 2, 93, 16, loft_comp);
+
+	if (NREV_LT(pi->pubpi.phy_rev, 2))
+		wlc_phy_tx_iq_war_nphy(pi);
+
+	if (CHSPEC_IS2G(pi->radio_chanspec)) {
+		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			txcal_radio_regs =
+				pi->calibration_cache.txcal_radio_regs_2G;
+		} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
 
 			write_radio_reg(pi,
-					RADIO_2056_TX_PADA_BOOST_TUNE |
-					RADIO_2056_TX0, 0);
+					RADIO_2056_TX_LOFT_FINE_I |
+					RADIO_2056_TX0,
+					pi->calibration_cache.
+					txcal_radio_regs_2G[0]);
 			write_radio_reg(pi,
-					RADIO_2056_TX_PADG_BOOST_TUNE |
-					RADIO_2056_TX0, 0);
+					RADIO_2056_TX_LOFT_FINE_Q |
+					RADIO_2056_TX0,
+					pi->calibration_cache.
+					txcal_radio_regs_2G[1]);
 			write_radio_reg(pi,
-					RADIO_2056_TX_PGAA_BOOST_TUNE |
-					RADIO_2056_TX0, 0);
+					RADIO_2056_TX_LOFT_FINE_I |
+					RADIO_2056_TX1,
+					pi->calibration_cache.
+					txcal_radio_regs_2G[2]);
 			write_radio_reg(pi,
-					RADIO_2056_TX_PGAG_BOOST_TUNE |
-					RADIO_2056_TX0, 0);
-			mod_radio_reg(pi,
-				      RADIO_2056_TX_MIXA_BOOST_TUNE |
-				      RADIO_2056_TX0, 0xf0, 0);
+					RADIO_2056_TX_LOFT_FINE_Q |
+					RADIO_2056_TX1,
+					pi->calibration_cache.
+					txcal_radio_regs_2G[3]);
+
 			write_radio_reg(pi,
-					RADIO_2056_TX_MIXG_BOOST_TUNE |
-					RADIO_2056_TX0, 0);
+					RADIO_2056_TX_LOFT_COARSE_I |
+					RADIO_2056_TX0,
+					pi->calibration_cache.
+					txcal_radio_regs_2G[4]);
+			write_radio_reg(pi,
+					RADIO_2056_TX_LOFT_COARSE_Q |
+					RADIO_2056_TX0,
+					pi->calibration_cache.
+					txcal_radio_regs_2G[5]);
+			write_radio_reg(pi,
+					RADIO_2056_TX_LOFT_COARSE_I |
+					RADIO_2056_TX1,
+					pi->calibration_cache.
+					txcal_radio_regs_2G[6]);
+			write_radio_reg(pi,
+					RADIO_2056_TX_LOFT_COARSE_Q |
+					RADIO_2056_TX1,
+					pi->calibration_cache.
+					txcal_radio_regs_2G[7]);
+		} else {
+			write_radio_reg(pi, RADIO_2055_CORE1_TX_VOS_CNCL,
+					pi->calibration_cache.
+					txcal_radio_regs_2G[0]);
+			write_radio_reg(pi, RADIO_2055_CORE2_TX_VOS_CNCL,
+					pi->calibration_cache.
+					txcal_radio_regs_2G[1]);
+			write_radio_reg(pi, RADIO_2055_CORE1_TX_BB_MXGM,
+					pi->calibration_cache.
+					txcal_radio_regs_2G[2]);
+			write_radio_reg(pi, RADIO_2055_CORE2_TX_BB_MXGM,
+					pi->calibration_cache.
+					txcal_radio_regs_2G[3]);
+		}
+
+		wlc_phy_rx_iq_coeffs_nphy(pi, 1,
+					  &pi->calibration_cache.
+					  rxcal_coeffs_2G);
+	} else {
+		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			txcal_radio_regs =
+				pi->calibration_cache.txcal_radio_regs_5G;
+		} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
 
 			write_radio_reg(pi,
-					RADIO_2056_TX_PADA_BOOST_TUNE |
-					RADIO_2056_TX1, 0);
+					RADIO_2056_TX_LOFT_FINE_I |
+					RADIO_2056_TX0,
+					pi->calibration_cache.
+					txcal_radio_regs_5G[0]);
 			write_radio_reg(pi,
-					RADIO_2056_TX_PADG_BOOST_TUNE |
-					RADIO_2056_TX1, 0);
+					RADIO_2056_TX_LOFT_FINE_Q |
+					RADIO_2056_TX0,
+					pi->calibration_cache.
+					txcal_radio_regs_5G[1]);
 			write_radio_reg(pi,
-					RADIO_2056_TX_PGAA_BOOST_TUNE |
-					RADIO_2056_TX1, 0);
+					RADIO_2056_TX_LOFT_FINE_I |
+					RADIO_2056_TX1,
+					pi->calibration_cache.
+					txcal_radio_regs_5G[2]);
 			write_radio_reg(pi,
-					RADIO_2056_TX_PGAG_BOOST_TUNE |
-					RADIO_2056_TX1, 0);
-			mod_radio_reg(pi,
-				      RADIO_2056_TX_MIXA_BOOST_TUNE |
-				      RADIO_2056_TX1, 0xf0, 0);
+					RADIO_2056_TX_LOFT_FINE_Q |
+					RADIO_2056_TX1,
+					pi->calibration_cache.
+					txcal_radio_regs_5G[3]);
+
 			write_radio_reg(pi,
-					RADIO_2056_TX_MIXG_BOOST_TUNE |
-					RADIO_2056_TX1, 0);
+					RADIO_2056_TX_LOFT_COARSE_I |
+					RADIO_2056_TX0,
+					pi->calibration_cache.
+					txcal_radio_regs_5G[4]);
+			write_radio_reg(pi,
+					RADIO_2056_TX_LOFT_COARSE_Q |
+					RADIO_2056_TX0,
+					pi->calibration_cache.
+					txcal_radio_regs_5G[5]);
+			write_radio_reg(pi,
+					RADIO_2056_TX_LOFT_COARSE_I |
+					RADIO_2056_TX1,
+					pi->calibration_cache.
+					txcal_radio_regs_5G[6]);
+			write_radio_reg(pi,
+					RADIO_2056_TX_LOFT_COARSE_Q |
+					RADIO_2056_TX1,
+					pi->calibration_cache.
+					txcal_radio_regs_5G[7]);
+		} else {
+			write_radio_reg(pi, RADIO_2055_CORE1_TX_VOS_CNCL,
+					pi->calibration_cache.
+					txcal_radio_regs_5G[0]);
+			write_radio_reg(pi, RADIO_2055_CORE2_TX_VOS_CNCL,
+					pi->calibration_cache.
+					txcal_radio_regs_5G[1]);
+			write_radio_reg(pi, RADIO_2055_CORE1_TX_BB_MXGM,
+					pi->calibration_cache.
+					txcal_radio_regs_5G[2]);
+			write_radio_reg(pi, RADIO_2055_CORE2_TX_BB_MXGM,
+					pi->calibration_cache.
+					txcal_radio_regs_5G[3]);
+		}
 
-			pi->radio_is_on = false;
+		wlc_phy_rx_iq_coeffs_nphy(pi, 1,
+					  &pi->calibration_cache.
+					  rxcal_coeffs_5G);
+	}
+
+	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+		for (coreNum = 0; coreNum <= 1; coreNum++) {
+
+			WRITE_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
+					 LOFT_FINE_I,
+					 txcal_radio_regs[2 * coreNum]);
+			WRITE_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
+					 LOFT_FINE_Q,
+					 txcal_radio_regs[2 * coreNum + 1]);
+
+			WRITE_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
+					 LOFT_COARSE_I,
+					 txcal_radio_regs[2 * coreNum + 4]);
+			WRITE_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
+					 LOFT_COARSE_Q,
+					 txcal_radio_regs[2 * coreNum + 5]);
 		}
+	}
+}
 
-		if (NREV_GE(pi->pubpi.phy_rev, 8)) {
-			and_phy_reg(pi, 0x78, ~RFCC_CHIP0_PU);
-			pi->radio_is_on = false;
+static void wlc_phy_txpwrctrl_coeff_setup_nphy(struct brcms_phy *pi)
+{
+	u32 idx;
+	u16 iqloCalbuf[7];
+	u32 iqcomp, locomp, curr_locomp;
+	s8 locomp_i, locomp_q;
+	s8 curr_locomp_i, curr_locomp_q;
+	u32 tbl_id, tbl_len, tbl_offset;
+	u32 regval[128];
+
+	if (pi->phyhang_avoid)
+		wlc_phy_stay_in_carriersearch_nphy(pi, true);
+
+	wlc_phy_table_read_nphy(pi, 15, 7, 80, 16, iqloCalbuf);
+
+	tbl_len = 128;
+	tbl_offset = 320;
+	for (tbl_id = NPHY_TBL_ID_CORE1TXPWRCTL;
+	     tbl_id <= NPHY_TBL_ID_CORE2TXPWRCTL; tbl_id++) {
+		iqcomp =
+			(tbl_id ==
+			 26) ? (((u32) (iqloCalbuf[0] & 0x3ff)) << 10) |
+			(iqloCalbuf[1] & 0x3ff)
+			: (((u32) (iqloCalbuf[2] & 0x3ff)) << 10) |
+			(iqloCalbuf[3] & 0x3ff);
+
+		for (idx = 0; idx < tbl_len; idx++)
+			regval[idx] = iqcomp;
+		wlc_phy_table_write_nphy(pi, tbl_id, tbl_len, tbl_offset, 32,
+					 regval);
+	}
+
+	tbl_offset = 448;
+	for (tbl_id = NPHY_TBL_ID_CORE1TXPWRCTL;
+	     tbl_id <= NPHY_TBL_ID_CORE2TXPWRCTL; tbl_id++) {
+
+		locomp =
+			(u32) ((tbl_id == 26) ? iqloCalbuf[5] : iqloCalbuf[6]);
+		locomp_i = (s8) ((locomp >> 8) & 0xff);
+		locomp_q = (s8) ((locomp) & 0xff);
+		for (idx = 0; idx < tbl_len; idx++) {
+			if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+				curr_locomp_i = locomp_i;
+				curr_locomp_q = locomp_q;
+			} else {
+				curr_locomp_i = (s8) ((locomp_i *
+						       nphy_tpc_loscale[idx] +
+						       128) >> 8);
+				curr_locomp_q =
+					(s8) ((locomp_q *
+					       nphy_tpc_loscale[idx] +
+					       128) >> 8);
+			}
+			curr_locomp = (u32) ((curr_locomp_i & 0xff) << 8);
+			curr_locomp |= (u32) (curr_locomp_q & 0xff);
+			regval[idx] = curr_locomp;
 		}
+		wlc_phy_table_write_nphy(pi, tbl_id, tbl_len, tbl_offset, 32,
+					 regval);
+	}
+
+	if (NREV_LT(pi->pubpi.phy_rev, 2)) {
 
+		wlapi_bmac_write_shm(pi->sh->physhim, M_CURR_IDX1, 0xFFFF);
+		wlapi_bmac_write_shm(pi->sh->physhim, M_CURR_IDX2, 0xFFFF);
 	}
+
+	if (pi->phyhang_avoid)
+		wlc_phy_stay_in_carriersearch_nphy(pi, false);
 }
 
-static void wlc_phy_radio_preinit_2055(struct brcms_phy *pi)
+static void wlc_phy_txlpfbw_nphy(struct brcms_phy *pi)
 {
+	u8 tx_lpf_bw = 0;
 
-	and_phy_reg(pi, 0x78, ~RFCC_POR_FORCE);
-	or_phy_reg(pi, 0x78, RFCC_CHIP0_PU | RFCC_OE_POR_FORCE);
+	if (NREV_GE(pi->pubpi.phy_rev, 3) && NREV_LT(pi->pubpi.phy_rev, 7)) {
+		if (CHSPEC_IS40(pi->radio_chanspec))
+			tx_lpf_bw = 3;
+		else
+			tx_lpf_bw = 1;
 
-	or_phy_reg(pi, 0x78, RFCC_POR_FORCE);
+		if (PHY_IPA(pi)) {
+			if (CHSPEC_IS40(pi->radio_chanspec))
+				tx_lpf_bw = 5;
+			else
+				tx_lpf_bw = 4;
+		}
+
+		write_phy_reg(pi, 0xe8,
+			      (tx_lpf_bw << 0) |
+			      (tx_lpf_bw << 3) |
+			      (tx_lpf_bw << 6) | (tx_lpf_bw << 9));
+
+		if (PHY_IPA(pi)) {
+
+			if (CHSPEC_IS40(pi->radio_chanspec))
+				tx_lpf_bw = 4;
+			else
+				tx_lpf_bw = 1;
+
+			write_phy_reg(pi, 0xe9,
+				      (tx_lpf_bw << 0) |
+				      (tx_lpf_bw << 3) |
+				      (tx_lpf_bw << 6) | (tx_lpf_bw << 9));
+		}
+	}
 }
 
-static void wlc_phy_radio_init_2055(struct brcms_phy *pi)
+static void
+wlc_phy_adjust_rx_analpfbw_nphy(struct brcms_phy *pi, u16 reduction_factr)
 {
-	wlc_phy_init_radio_regs(pi, regs_2055, RADIO_DEFAULT_CORE);
+	if (NREV_GE(pi->pubpi.phy_rev, 3) && NREV_LT(pi->pubpi.phy_rev, 7)) {
+		if ((CHSPEC_CHANNEL(pi->radio_chanspec) == 11) &&
+		    CHSPEC_IS40(pi->radio_chanspec)) {
+			if (!pi->nphy_anarxlpf_adjusted) {
+				write_radio_reg(pi,
+						(RADIO_2056_RX_RXLPF_RCCAL_LPC |
+						 RADIO_2056_RX0),
+						((pi->nphy_rccal_value +
+						  reduction_factr) | 0x80));
+
+				pi->nphy_anarxlpf_adjusted = true;
+			}
+		} else {
+			if (pi->nphy_anarxlpf_adjusted) {
+				write_radio_reg(pi,
+						(RADIO_2056_RX_RXLPF_RCCAL_LPC |
+						 RADIO_2056_RX0),
+						(pi->nphy_rccal_value | 0x80));
+
+				pi->nphy_anarxlpf_adjusted = false;
+			}
+		}
+	}
 }
 
-static void wlc_phy_radio_postinit_2055(struct brcms_phy *pi)
+static void
+wlc_phy_adjust_min_noisevar_nphy(struct brcms_phy *pi, int ntones,
+				 int *tone_id_buf, u32 *noise_var_buf)
 {
+	int i;
+	u32 offset;
+	int tone_id;
+	int tbllen =
+		CHSPEC_IS40(pi->radio_chanspec) ?
+		NPHY_NOISEVAR_TBLLEN40 : NPHY_NOISEVAR_TBLLEN20;
 
-	and_radio_reg(pi, RADIO_2055_MASTER_CNTRL1,
-		      ~(RADIO_2055_JTAGCTRL_MASK | RADIO_2055_JTAGSYNC_MASK));
+	if (pi->nphy_noisevars_adjusted) {
+		for (i = 0; i < pi->nphy_saved_noisevars.bufcount; i++) {
+			tone_id = pi->nphy_saved_noisevars.tone_id[i];
+			offset = (tone_id >= 0) ?
+				 ((tone_id *
+				   2) + 1) : (tbllen + (tone_id * 2) + 1);
+			wlc_phy_table_write_nphy(
+				pi, NPHY_TBL_ID_NOISEVAR, 1,
+				offset, 32,
+				&pi->nphy_saved_noisevars.min_noise_vars[i]);
+		}
 
-	if (((pi->sh->sromrev >= 4)
-	     && !(pi->sh->boardflags2 & BFL2_RXBB_INT_REG_DIS))
-	    || ((pi->sh->sromrev < 4))) {
-		and_radio_reg(pi, RADIO_2055_CORE1_RXBB_REGULATOR, 0x7F);
-		and_radio_reg(pi, RADIO_2055_CORE2_RXBB_REGULATOR, 0x7F);
+		pi->nphy_saved_noisevars.bufcount = 0;
+		pi->nphy_noisevars_adjusted = false;
 	}
 
-	mod_radio_reg(pi, RADIO_2055_RRCCAL_N_OPT_SEL, 0x3F, 0x2C);
-	write_radio_reg(pi, RADIO_2055_CAL_MISC, 0x3C);
+	if ((noise_var_buf != NULL) && (tone_id_buf != NULL)) {
+		pi->nphy_saved_noisevars.bufcount = 0;
 
-	and_radio_reg(pi, RADIO_2055_CAL_MISC,
-		      ~(RADIO_2055_RRCAL_START | RADIO_2055_RRCAL_RST_N));
+		for (i = 0; i < ntones; i++) {
+			tone_id = tone_id_buf[i];
+			offset = (tone_id >= 0) ?
+				 ((tone_id * 2) + 1) :
+				 (tbllen + (tone_id * 2) + 1);
+			pi->nphy_saved_noisevars.tone_id[i] = tone_id;
+			wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_NOISEVAR, 1,
+						offset, 32,
+						&pi->nphy_saved_noisevars.
+						min_noise_vars[i]);
+			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_NOISEVAR, 1,
+						 offset, 32, &noise_var_buf[i]);
+			pi->nphy_saved_noisevars.bufcount++;
+		}
 
-	or_radio_reg(pi, RADIO_2055_CAL_LPO_CNTRL, RADIO_2055_CAL_LPO_ENABLE);
+		pi->nphy_noisevars_adjusted = true;
+	}
+}
 
-	or_radio_reg(pi, RADIO_2055_CAL_MISC, RADIO_2055_RRCAL_RST_N);
+static void wlc_phy_adjust_crsminpwr_nphy(struct brcms_phy *pi, u8 minpwr)
+{
+	u16 regval;
 
-	udelay(1000);
+	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+		if ((CHSPEC_CHANNEL(pi->radio_chanspec) == 11) &&
+		    CHSPEC_IS40(pi->radio_chanspec)) {
+			if (!pi->nphy_crsminpwr_adjusted) {
+				regval = read_phy_reg(pi, 0x27d);
+				pi->nphy_crsminpwr[0] = regval & 0xff;
+				regval &= 0xff00;
+				regval |= (u16) minpwr;
+				write_phy_reg(pi, 0x27d, regval);
 
-	or_radio_reg(pi, RADIO_2055_CAL_MISC, RADIO_2055_RRCAL_START);
+				regval = read_phy_reg(pi, 0x280);
+				pi->nphy_crsminpwr[1] = regval & 0xff;
+				regval &= 0xff00;
+				regval |= (u16) minpwr;
+				write_phy_reg(pi, 0x280, regval);
 
-	SPINWAIT(((read_radio_reg(pi, RADIO_2055_CAL_COUNTER_OUT2) &
-		   RADIO_2055_RCAL_DONE) != RADIO_2055_RCAL_DONE), 2000);
+				regval = read_phy_reg(pi, 0x283);
+				pi->nphy_crsminpwr[2] = regval & 0xff;
+				regval &= 0xff00;
+				regval |= (u16) minpwr;
+				write_phy_reg(pi, 0x283, regval);
 
-	if (WARN((read_radio_reg(pi, RADIO_2055_CAL_COUNTER_OUT2) &
-		 RADIO_2055_RCAL_DONE) != RADIO_2055_RCAL_DONE,
-		 "HW error: radio calibration1\n"))
+				pi->nphy_crsminpwr_adjusted = true;
+			}
+		} else {
+			if (pi->nphy_crsminpwr_adjusted) {
+				regval = read_phy_reg(pi, 0x27d);
+				regval &= 0xff00;
+				regval |= pi->nphy_crsminpwr[0];
+				write_phy_reg(pi, 0x27d, regval);
+
+				regval = read_phy_reg(pi, 0x280);
+				regval &= 0xff00;
+				regval |= pi->nphy_crsminpwr[1];
+				write_phy_reg(pi, 0x280, regval);
+
+				regval = read_phy_reg(pi, 0x283);
+				regval &= 0xff00;
+				regval |= pi->nphy_crsminpwr[2];
+				write_phy_reg(pi, 0x283, regval);
+
+				pi->nphy_crsminpwr_adjusted = false;
+			}
+		}
+	}
+}
+
+static void wlc_phy_spurwar_nphy(struct brcms_phy *pi)
+{
+	u16 cur_channel = 0;
+	int nphy_adj_tone_id_buf[] = { 57, 58 };
+	u32 nphy_adj_noise_var_buf[] = { 0x3ff, 0x3ff };
+	bool isAdjustNoiseVar = false;
+	uint numTonesAdjust = 0;
+	u32 tempval = 0;
+
+	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+		if (pi->phyhang_avoid)
+			wlc_phy_stay_in_carriersearch_nphy(pi, true);
+
+		cur_channel = CHSPEC_CHANNEL(pi->radio_chanspec);
+
+		if (pi->nphy_gband_spurwar_en) {
+
+			wlc_phy_adjust_rx_analpfbw_nphy(
+				pi,
+				NPHY_ANARXLPFBW_REDUCTIONFACT);
+
+			if (CHSPEC_IS2G(pi->radio_chanspec)) {
+				if ((cur_channel == 11)
+				    && CHSPEC_IS40(pi->radio_chanspec))
+					wlc_phy_adjust_min_noisevar_nphy(
+						pi, 2,
+						nphy_adj_tone_id_buf,
+						nphy_adj_noise_var_buf);
+				else
+					wlc_phy_adjust_min_noisevar_nphy(pi, 0,
+									 NULL,
+									 NULL);
+			}
+
+			wlc_phy_adjust_crsminpwr_nphy(pi,
+						     NPHY_ADJUSTED_MINCRSPOWER);
+		}
+
+		if ((pi->nphy_gband_spurwar2_en)
+		    && CHSPEC_IS2G(pi->radio_chanspec)) {
+
+			if (CHSPEC_IS40(pi->radio_chanspec)) {
+				switch (cur_channel) {
+				case 3:
+					nphy_adj_tone_id_buf[0] = 57;
+					nphy_adj_tone_id_buf[1] = 58;
+					nphy_adj_noise_var_buf[0] = 0x22f;
+					nphy_adj_noise_var_buf[1] = 0x25f;
+					isAdjustNoiseVar = true;
+					break;
+				case 4:
+					nphy_adj_tone_id_buf[0] = 41;
+					nphy_adj_tone_id_buf[1] = 42;
+					nphy_adj_noise_var_buf[0] = 0x22f;
+					nphy_adj_noise_var_buf[1] = 0x25f;
+					isAdjustNoiseVar = true;
+					break;
+				case 5:
+					nphy_adj_tone_id_buf[0] = 25;
+					nphy_adj_tone_id_buf[1] = 26;
+					nphy_adj_noise_var_buf[0] = 0x24f;
+					nphy_adj_noise_var_buf[1] = 0x25f;
+					isAdjustNoiseVar = true;
+					break;
+				case 6:
+					nphy_adj_tone_id_buf[0] = 9;
+					nphy_adj_tone_id_buf[1] = 10;
+					nphy_adj_noise_var_buf[0] = 0x22f;
+					nphy_adj_noise_var_buf[1] = 0x24f;
+					isAdjustNoiseVar = true;
+					break;
+				case 7:
+					nphy_adj_tone_id_buf[0] = 121;
+					nphy_adj_tone_id_buf[1] = 122;
+					nphy_adj_noise_var_buf[0] = 0x18f;
+					nphy_adj_noise_var_buf[1] = 0x24f;
+					isAdjustNoiseVar = true;
+					break;
+				case 8:
+					nphy_adj_tone_id_buf[0] = 105;
+					nphy_adj_tone_id_buf[1] = 106;
+					nphy_adj_noise_var_buf[0] = 0x22f;
+					nphy_adj_noise_var_buf[1] = 0x25f;
+					isAdjustNoiseVar = true;
+					break;
+				case 9:
+					nphy_adj_tone_id_buf[0] = 89;
+					nphy_adj_tone_id_buf[1] = 90;
+					nphy_adj_noise_var_buf[0] = 0x22f;
+					nphy_adj_noise_var_buf[1] = 0x24f;
+					isAdjustNoiseVar = true;
+					break;
+				case 10:
+					nphy_adj_tone_id_buf[0] = 73;
+					nphy_adj_tone_id_buf[1] = 74;
+					nphy_adj_noise_var_buf[0] = 0x22f;
+					nphy_adj_noise_var_buf[1] = 0x24f;
+					isAdjustNoiseVar = true;
+					break;
+				default:
+					isAdjustNoiseVar = false;
+					break;
+				}
+			}
+
+			if (isAdjustNoiseVar) {
+				numTonesAdjust = sizeof(nphy_adj_tone_id_buf) /
+						sizeof(nphy_adj_tone_id_buf[0]);
+
+				wlc_phy_adjust_min_noisevar_nphy(
+					pi,
+					numTonesAdjust,
+					nphy_adj_tone_id_buf,
+					nphy_adj_noise_var_buf);
+
+				tempval = 0;
+
+			} else {
+				wlc_phy_adjust_min_noisevar_nphy(pi, 0, NULL,
+								 NULL);
+			}
+		}
+
+		if ((pi->nphy_aband_spurwar_en) &&
+		    (CHSPEC_IS5G(pi->radio_chanspec))) {
+			switch (cur_channel) {
+			case 54:
+				nphy_adj_tone_id_buf[0] = 32;
+				nphy_adj_noise_var_buf[0] = 0x25f;
+				break;
+			case 38:
+			case 102:
+			case 118:
+				nphy_adj_tone_id_buf[0] = 0;
+				nphy_adj_noise_var_buf[0] = 0x0;
+				break;
+			case 134:
+				nphy_adj_tone_id_buf[0] = 32;
+				nphy_adj_noise_var_buf[0] = 0x21f;
+				break;
+			case 151:
+				nphy_adj_tone_id_buf[0] = 16;
+				nphy_adj_noise_var_buf[0] = 0x23f;
+				break;
+			case 153:
+			case 161:
+				nphy_adj_tone_id_buf[0] = 48;
+				nphy_adj_noise_var_buf[0] = 0x23f;
+				break;
+			default:
+				nphy_adj_tone_id_buf[0] = 0;
+				nphy_adj_noise_var_buf[0] = 0x0;
+				break;
+			}
+
+			if (nphy_adj_tone_id_buf[0]
+			    && nphy_adj_noise_var_buf[0])
+				wlc_phy_adjust_min_noisevar_nphy(
+					pi, 1,
+					nphy_adj_tone_id_buf,
+					nphy_adj_noise_var_buf);
+			else
+				wlc_phy_adjust_min_noisevar_nphy(pi, 0, NULL,
+								 NULL);
+		}
+
+		if (pi->phyhang_avoid)
+			wlc_phy_stay_in_carriersearch_nphy(pi, false);
+	}
+}
+
+void wlc_phy_init_nphy(struct brcms_phy *pi)
+{
+	u16 val;
+	u16 clip1_ths[2];
+	struct nphy_txgains target_gain;
+	u8 tx_pwr_ctrl_state;
+	bool do_nphy_cal = false;
+	uint core;
+	uint origidx, intr_val;
+	struct d11regs __iomem *regs;
+	u32 d11_clk_ctl_st;
+	bool do_rssi_cal = false;
+
+	core = 0;
+
+	if (!(pi->measure_hold & PHY_HOLD_FOR_SCAN))
+		pi->measure_hold |= PHY_HOLD_FOR_NOT_ASSOC;
+
+	if ((ISNPHY(pi)) && (NREV_GE(pi->pubpi.phy_rev, 5)) &&
+	    ((pi->sh->chippkg == BCM4717_PKG_ID) ||
+	     (pi->sh->chippkg == BCM4718_PKG_ID))) {
+		if ((pi->sh->boardflags & BFL_EXTLNA) &&
+		    (CHSPEC_IS2G(pi->radio_chanspec)))
+			ai_corereg(pi->sh->sih, SI_CC_IDX,
+				   offsetof(struct chipcregs, chipcontrol),
+				   0x40, 0x40);
+	}
+
+	if ((pi->nphy_gband_spurwar2_en) && CHSPEC_IS2G(pi->radio_chanspec) &&
+	    CHSPEC_IS40(pi->radio_chanspec)) {
+
+		regs = (struct d11regs __iomem *)
+				ai_switch_core(pi->sh->sih,
+					       D11_CORE_ID, &origidx,
+					       &intr_val);
+		d11_clk_ctl_st = R_REG(&regs->clk_ctl_st);
+		AND_REG(&regs->clk_ctl_st,
+			~(CCS_FORCEHT | CCS_HTAREQ));
+
+		W_REG(&regs->clk_ctl_st, d11_clk_ctl_st);
+
+		ai_restore_core(pi->sh->sih, origidx, intr_val);
+	}
+
+	pi->use_int_tx_iqlo_cal_nphy =
+		(PHY_IPA(pi) ||
+		 (NREV_GE(pi->pubpi.phy_rev, 7) ||
+		  (NREV_GE(pi->pubpi.phy_rev, 5)
+		   && pi->sh->boardflags2 & BFL2_INTERNDET_TXIQCAL)));
+
+	pi->internal_tx_iqlo_cal_tapoff_intpa_nphy = false;
+
+	pi->nphy_deaf_count = 0;
+
+	wlc_phy_tbl_init_nphy(pi);
+
+	pi->nphy_crsminpwr_adjusted = false;
+	pi->nphy_noisevars_adjusted = false;
+
+	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+		write_phy_reg(pi, 0xe7, 0);
+		write_phy_reg(pi, 0xec, 0);
+		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			write_phy_reg(pi, 0x342, 0);
+			write_phy_reg(pi, 0x343, 0);
+			write_phy_reg(pi, 0x346, 0);
+			write_phy_reg(pi, 0x347, 0);
+		}
+		write_phy_reg(pi, 0xe5, 0);
+		write_phy_reg(pi, 0xe6, 0);
+	} else {
+		write_phy_reg(pi, 0xec, 0);
+	}
+
+	write_phy_reg(pi, 0x91, 0);
+	write_phy_reg(pi, 0x92, 0);
+	if (NREV_LT(pi->pubpi.phy_rev, 6)) {
+		write_phy_reg(pi, 0x93, 0);
+		write_phy_reg(pi, 0x94, 0);
+	}
+
+	and_phy_reg(pi, 0xa1, ~3);
+
+	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+		write_phy_reg(pi, 0x8f, 0);
+		write_phy_reg(pi, 0xa5, 0);
+	} else {
+		write_phy_reg(pi, 0xa5, 0);
+	}
+
+	if (NREV_IS(pi->pubpi.phy_rev, 2))
+		mod_phy_reg(pi, 0xdc, 0x00ff, 0x3b);
+	else if (NREV_LT(pi->pubpi.phy_rev, 2))
+		mod_phy_reg(pi, 0xdc, 0x00ff, 0x40);
+
+	write_phy_reg(pi, 0x203, 32);
+	write_phy_reg(pi, 0x201, 32);
+
+	if (pi->sh->boardflags2 & BFL2_SKWRKFEM_BRD)
+		write_phy_reg(pi, 0x20d, 160);
+	else
+		write_phy_reg(pi, 0x20d, 184);
+
+	write_phy_reg(pi, 0x13a, 200);
+
+	write_phy_reg(pi, 0x70, 80);
+
+	write_phy_reg(pi, 0x1ff, 48);
+
+	if (NREV_LT(pi->pubpi.phy_rev, 8))
+		wlc_phy_update_mimoconfig_nphy(pi, pi->n_preamble_override);
+
+	wlc_phy_stf_chain_upd_nphy(pi);
+
+	if (NREV_LT(pi->pubpi.phy_rev, 2)) {
+		write_phy_reg(pi, 0x180, 0xaa8);
+		write_phy_reg(pi, 0x181, 0x9a4);
+	}
+
+	if (PHY_IPA(pi)) {
+		for (core = 0; core < pi->pubpi.phy_corenum; core++) {
+
+			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x297 :
+				    0x29b, (0x1 << 0), (1) << 0);
+
+			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x298 :
+				    0x29c, (0x1ff << 7),
+				    (pi->nphy_papd_epsilon_offset[core]) << 7);
+
+		}
+
+		wlc_phy_ipa_set_tx_digi_filts_nphy(pi);
+	} else if (NREV_GE(pi->pubpi.phy_rev, 5)) {
+		wlc_phy_extpa_set_tx_digi_filts_nphy(pi);
+	}
+
+	wlc_phy_workarounds_nphy(pi);
+
+	wlapi_bmac_phyclk_fgc(pi->sh->physhim, ON);
+
+	val = read_phy_reg(pi, 0x01);
+	write_phy_reg(pi, 0x01, val | BBCFG_RESETCCA);
+	write_phy_reg(pi, 0x01, val & (~BBCFG_RESETCCA));
+	wlapi_bmac_phyclk_fgc(pi->sh->physhim, OFF);
+
+	wlapi_bmac_macphyclk_set(pi->sh->physhim, ON);
+
+	wlc_phy_pa_override_nphy(pi, OFF);
+	wlc_phy_force_rfseq_nphy(pi, NPHY_RFSEQ_RX2TX);
+	wlc_phy_force_rfseq_nphy(pi, NPHY_RFSEQ_RESET2RX);
+	wlc_phy_pa_override_nphy(pi, ON);
+
+	wlc_phy_classifier_nphy(pi, 0, 0);
+	wlc_phy_clip_det_nphy(pi, 0, clip1_ths);
+
+	if (CHSPEC_IS2G(pi->radio_chanspec))
+		wlc_phy_bphy_init_nphy(pi);
+
+	tx_pwr_ctrl_state = pi->nphy_txpwrctrl;
+	wlc_phy_txpwrctrl_enable_nphy(pi, PHY_TPC_HW_OFF);
+
+	wlc_phy_txpwr_fixpower_nphy(pi);
+
+	wlc_phy_txpwrctrl_idle_tssi_nphy(pi);
+
+	wlc_phy_txpwrctrl_pwr_setup_nphy(pi);
+
+	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+		u32 *tx_pwrctrl_tbl = NULL;
+		u16 idx;
+		s16 pga_gn = 0;
+		s16 pad_gn = 0;
+		s32 rfpwr_offset;
+
+		if (PHY_IPA(pi)) {
+			tx_pwrctrl_tbl = wlc_phy_get_ipa_gaintbl_nphy(pi);
+		} else {
+			if (CHSPEC_IS5G(pi->radio_chanspec)) {
+				if (NREV_IS(pi->pubpi.phy_rev, 3))
+					tx_pwrctrl_tbl =
+						nphy_tpc_5GHz_txgain_rev3;
+				else if (NREV_IS(pi->pubpi.phy_rev, 4))
+					tx_pwrctrl_tbl =
+						(pi->srom_fem5g.extpagain ==
+						 3) ?
+						nphy_tpc_5GHz_txgain_HiPwrEPA :
+						nphy_tpc_5GHz_txgain_rev4;
+				else
+					tx_pwrctrl_tbl =
+						nphy_tpc_5GHz_txgain_rev5;
+			} else {
+				if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+					if (pi->pubpi.radiorev == 5)
+						tx_pwrctrl_tbl =
+						   nphy_tpc_txgain_epa_2057rev5;
+					else if (pi->pubpi.radiorev == 3)
+						tx_pwrctrl_tbl =
+						   nphy_tpc_txgain_epa_2057rev3;
+				} else {
+					if (NREV_GE(pi->pubpi.phy_rev, 5) &&
+					    (pi->srom_fem2g.extpagain == 3))
+						tx_pwrctrl_tbl =
+						       nphy_tpc_txgain_HiPwrEPA;
+					else
+						tx_pwrctrl_tbl =
+							nphy_tpc_txgain_rev3;
+				}
+			}
+		}
+
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_CORE1TXPWRCTL, 128,
+					 192, 32, tx_pwrctrl_tbl);
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_CORE2TXPWRCTL, 128,
+					 192, 32, tx_pwrctrl_tbl);
+
+		pi->nphy_gmval = (u16) ((*tx_pwrctrl_tbl >> 16) & 0x7000);
+
+		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+
+			for (idx = 0; idx < 128; idx++) {
+				pga_gn = (tx_pwrctrl_tbl[idx] >> 24) & 0xf;
+				pad_gn = (tx_pwrctrl_tbl[idx] >> 19) & 0x1f;
+				rfpwr_offset = get_rf_pwr_offset(pi, pga_gn,
+								 pad_gn);
+				wlc_phy_table_write_nphy(
+					pi,
+					NPHY_TBL_ID_CORE1TXPWRCTL,
+					1, 576 + idx, 32,
+					&rfpwr_offset);
+				wlc_phy_table_write_nphy(
+					pi,
+					NPHY_TBL_ID_CORE2TXPWRCTL,
+					1, 576 + idx, 32,
+					&rfpwr_offset);
+			}
+		} else {
+
+			for (idx = 0; idx < 128; idx++) {
+				pga_gn = (tx_pwrctrl_tbl[idx] >> 24) & 0xf;
+				if (CHSPEC_IS2G(pi->radio_chanspec))
+					rfpwr_offset = (s16)
+						 nphy_papd_pga_gain_delta_ipa_2g
+								       [pga_gn];
+				else
+					rfpwr_offset = (s16)
+						 nphy_papd_pga_gain_delta_ipa_5g
+								       [pga_gn];
+
+				wlc_phy_table_write_nphy(
+					pi,
+					NPHY_TBL_ID_CORE1TXPWRCTL,
+					1, 576 + idx, 32,
+					&rfpwr_offset);
+				wlc_phy_table_write_nphy(
+					pi,
+					NPHY_TBL_ID_CORE2TXPWRCTL,
+					1, 576 + idx, 32,
+					&rfpwr_offset);
+			}
+
+		}
+	} else {
+
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_CORE1TXPWRCTL, 128,
+					 192, 32, nphy_tpc_txgain);
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_CORE2TXPWRCTL, 128,
+					 192, 32, nphy_tpc_txgain);
+	}
+
+	if (pi->sh->phyrxchain != 0x3)
+		wlc_phy_rxcore_setstate_nphy((struct brcms_phy_pub *) pi,
+					     pi->sh->phyrxchain);
+
+	if (PHY_PERICAL_MPHASE_PENDING(pi))
+		wlc_phy_cal_perical_mphase_restart(pi);
+
+	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+		do_rssi_cal = (CHSPEC_IS2G(pi->radio_chanspec)) ?
+			      (pi->nphy_rssical_chanspec_2G == 0) :
+			      (pi->nphy_rssical_chanspec_5G == 0);
+
+		if (do_rssi_cal)
+			wlc_phy_rssi_cal_nphy(pi);
+		else
+			wlc_phy_restore_rssical_nphy(pi);
+	} else {
+		wlc_phy_rssi_cal_nphy(pi);
+	}
+
+	if (!SCAN_RM_IN_PROGRESS(pi))
+		do_nphy_cal = (CHSPEC_IS2G(pi->radio_chanspec)) ?
+			      (pi->nphy_iqcal_chanspec_2G == 0) :
+			      (pi->nphy_iqcal_chanspec_5G == 0);
+
+	if (!pi->do_initcal)
+		do_nphy_cal = false;
+
+	if (do_nphy_cal) {
+
+		target_gain = wlc_phy_get_tx_gain_nphy(pi);
+
+		if (pi->antsel_type == ANTSEL_2x3)
+			wlc_phy_antsel_init((struct brcms_phy_pub *) pi,
+					    true);
+
+		if (pi->nphy_perical != PHY_PERICAL_MPHASE) {
+			wlc_phy_rssi_cal_nphy(pi);
+
+			if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+				pi->nphy_cal_orig_pwr_idx[0] =
+					pi->nphy_txpwrindex[PHY_CORE_0]
+					.
+					index_internal;
+				pi->nphy_cal_orig_pwr_idx[1] =
+					pi->nphy_txpwrindex[PHY_CORE_1]
+					.
+					index_internal;
+
+				wlc_phy_precal_txgain_nphy(pi);
+				target_gain =
+					wlc_phy_get_tx_gain_nphy(pi);
+			}
+
+			if (wlc_phy_cal_txiqlo_nphy
+				    (pi, target_gain, true,
+				    false) == 0) {
+				if (wlc_phy_cal_rxiq_nphy
+					    (pi, target_gain, 2,
+					    false) == 0)
+					wlc_phy_savecal_nphy(pi);
+
+			}
+		} else if (pi->mphase_cal_phase_id ==
+			   MPHASE_CAL_STATE_IDLE) {
+			wlc_phy_cal_perical((struct brcms_phy_pub *) pi,
+					    PHY_PERICAL_PHYINIT);
+		}
+	} else {
+		wlc_phy_restorecal_nphy(pi);
+	}
+
+	wlc_phy_txpwrctrl_coeff_setup_nphy(pi);
+
+	wlc_phy_txpwrctrl_enable_nphy(pi, tx_pwr_ctrl_state);
+
+	wlc_phy_nphy_tkip_rifs_war(pi, pi->sh->_rifs_phy);
+
+	if (NREV_GE(pi->pubpi.phy_rev, 3) && NREV_LE(pi->pubpi.phy_rev, 6))
+
+		write_phy_reg(pi, 0x70, 50);
+
+	wlc_phy_txlpfbw_nphy(pi);
+
+	wlc_phy_spurwar_nphy(pi);
+
+}
+
+static void wlc_phy_resetcca_nphy(struct brcms_phy *pi)
+{
+	u16 val;
+
+	wlapi_bmac_phyclk_fgc(pi->sh->physhim, ON);
+
+	val = read_phy_reg(pi, 0x01);
+	write_phy_reg(pi, 0x01, val | BBCFG_RESETCCA);
+	udelay(1);
+	write_phy_reg(pi, 0x01, val & (~BBCFG_RESETCCA));
+
+	wlapi_bmac_phyclk_fgc(pi->sh->physhim, OFF);
+
+	wlc_phy_force_rfseq_nphy(pi, NPHY_RFSEQ_RESET2RX);
+}
+
+void wlc_phy_pa_override_nphy(struct brcms_phy *pi, bool en)
+{
+	u16 rfctrlintc_override_val;
+
+	if (!en) {
+
+		pi->rfctrlIntc1_save = read_phy_reg(pi, 0x91);
+		pi->rfctrlIntc2_save = read_phy_reg(pi, 0x92);
+
+		if (NREV_GE(pi->pubpi.phy_rev, 7))
+			rfctrlintc_override_val = 0x1480;
+		else if (NREV_GE(pi->pubpi.phy_rev, 3))
+			rfctrlintc_override_val =
+				CHSPEC_IS5G(pi->radio_chanspec) ? 0x600 : 0x480;
+		else
+			rfctrlintc_override_val =
+				CHSPEC_IS5G(pi->radio_chanspec) ? 0x180 : 0x120;
+
+		write_phy_reg(pi, 0x91, rfctrlintc_override_val);
+		write_phy_reg(pi, 0x92, rfctrlintc_override_val);
+	} else {
+		write_phy_reg(pi, 0x91, pi->rfctrlIntc1_save);
+		write_phy_reg(pi, 0x92, pi->rfctrlIntc2_save);
+	}
+
+}
+
+void wlc_phy_stf_chain_upd_nphy(struct brcms_phy *pi)
+{
+
+	u16 txrx_chain =
+		(NPHY_RfseqCoreActv_TxRxChain0 | NPHY_RfseqCoreActv_TxRxChain1);
+	bool CoreActv_override = false;
+
+	if (pi->nphy_txrx_chain == BRCMS_N_TXRX_CHAIN0) {
+		txrx_chain = NPHY_RfseqCoreActv_TxRxChain0;
+		CoreActv_override = true;
+
+		if (NREV_LE(pi->pubpi.phy_rev, 2))
+			and_phy_reg(pi, 0xa0, ~0x20);
+	} else if (pi->nphy_txrx_chain == BRCMS_N_TXRX_CHAIN1) {
+		txrx_chain = NPHY_RfseqCoreActv_TxRxChain1;
+		CoreActv_override = true;
+
+		if (NREV_LE(pi->pubpi.phy_rev, 2))
+			or_phy_reg(pi, 0xa0, 0x20);
+	}
+
+	mod_phy_reg(pi, 0xa2, ((0xf << 0) | (0xf << 4)), txrx_chain);
+
+	if (CoreActv_override) {
+		pi->nphy_perical = PHY_PERICAL_DISABLE;
+		or_phy_reg(pi, 0xa1, NPHY_RfseqMode_CoreActv_override);
+	} else {
+		pi->nphy_perical = PHY_PERICAL_MPHASE;
+		and_phy_reg(pi, 0xa1, ~NPHY_RfseqMode_CoreActv_override);
+	}
+}
+
+void wlc_phy_rxcore_setstate_nphy(struct brcms_phy_pub *pih, u8 rxcore_bitmask)
+{
+	u16 regval;
+	u16 tbl_buf[16];
+	uint i;
+	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	u16 tbl_opcode;
+	bool suspend;
+
+	pi->sh->phyrxchain = rxcore_bitmask;
+
+	if (!pi->sh->clk)
 		return;
 
-	and_radio_reg(pi, RADIO_2055_CAL_LPO_CNTRL,
-		      ~(RADIO_2055_CAL_LPO_ENABLE));
+	suspend = (0 == (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
+	if (!suspend)
+		wlapi_suspend_mac_and_wait(pi->sh->physhim);
 
-	wlc_phy_chanspec_set((struct brcms_phy_pub *) pi, pi->radio_chanspec);
+	if (pi->phyhang_avoid)
+		wlc_phy_stay_in_carriersearch_nphy(pi, true);
 
-	write_radio_reg(pi, RADIO_2055_CORE1_RXBB_LPF, 9);
-	write_radio_reg(pi, RADIO_2055_CORE2_RXBB_LPF, 9);
+	regval = read_phy_reg(pi, 0xa2);
+	regval &= ~(0xf << 4);
+	regval |= ((u16) (rxcore_bitmask & 0x3)) << 4;
+	write_phy_reg(pi, 0xa2, regval);
 
-	write_radio_reg(pi, RADIO_2055_CORE1_RXBB_MIDAC_HIPAS, 0x83);
-	write_radio_reg(pi, RADIO_2055_CORE2_RXBB_MIDAC_HIPAS, 0x83);
+	if ((rxcore_bitmask & 0x3) != 0x3) {
 
-	mod_radio_reg(pi, RADIO_2055_CORE1_LNA_GAINBST,
-		      RADIO_2055_GAINBST_VAL_MASK, RADIO_2055_GAINBST_CODE);
-	mod_radio_reg(pi, RADIO_2055_CORE2_LNA_GAINBST,
-		      RADIO_2055_GAINBST_VAL_MASK, RADIO_2055_GAINBST_CODE);
-	if (pi->nphy_gain_boost) {
-		and_radio_reg(pi, RADIO_2055_CORE1_RXRF_SPC1,
-			      ~(RADIO_2055_GAINBST_DISABLE));
-		and_radio_reg(pi, RADIO_2055_CORE2_RXRF_SPC1,
-			      ~(RADIO_2055_GAINBST_DISABLE));
+		write_phy_reg(pi, 0x20e, 1);
+
+		if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+			if (pi->rx2tx_biasentry == -1) {
+				wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_RFSEQ,
+							ARRAY_SIZE(tbl_buf), 80,
+							16, tbl_buf);
+
+				for (i = 0; i < ARRAY_SIZE(tbl_buf); i++) {
+					if (tbl_buf[i] ==
+					    NPHY_REV3_RFSEQ_CMD_CLR_RXRX_BIAS) {
+						pi->rx2tx_biasentry = (u8) i;
+						tbl_opcode =
+							NPHY_REV3_RFSEQ_CMD_NOP;
+						wlc_phy_table_write_nphy(
+							pi,
+							NPHY_TBL_ID_RFSEQ,
+							1, i,
+							16,
+							&tbl_opcode);
+						break;
+					} else if (tbl_buf[i] ==
+						   NPHY_REV3_RFSEQ_CMD_END)
+						break;
+				}
+			}
+		}
 	} else {
-		or_radio_reg(pi, RADIO_2055_CORE1_RXRF_SPC1,
-			     RADIO_2055_GAINBST_DISABLE);
-		or_radio_reg(pi, RADIO_2055_CORE2_RXRF_SPC1,
-			     RADIO_2055_GAINBST_DISABLE);
+
+		write_phy_reg(pi, 0x20e, 30);
+
+		if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+			if (pi->rx2tx_biasentry != -1) {
+				tbl_opcode = NPHY_REV3_RFSEQ_CMD_CLR_RXRX_BIAS;
+				wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ,
+							 1, pi->rx2tx_biasentry,
+							 16, &tbl_opcode);
+				pi->rx2tx_biasentry = -1;
+			}
+		}
 	}
 
-	udelay(2);
+	wlc_phy_force_rfseq_nphy(pi, NPHY_RFSEQ_RESET2RX);
+
+	if (pi->phyhang_avoid)
+		wlc_phy_stay_in_carriersearch_nphy(pi, false);
+
+	if (!suspend)
+		wlapi_enable_mac(pi->sh->physhim);
+}
+
+u8 wlc_phy_rxcore_getstate_nphy(struct brcms_phy_pub *pih)
+{
+	u16 regval, rxen_bits;
+	struct brcms_phy *pi = (struct brcms_phy *) pih;
+
+	regval = read_phy_reg(pi, 0xa2);
+	rxen_bits = (regval >> 4) & 0xf;
+
+	return (u8) rxen_bits;
+}
+
+bool wlc_phy_n_txpower_ipa_ison(struct brcms_phy *pi)
+{
+	return PHY_IPA(pi);
+}
+
+void wlc_phy_cal_init_nphy(struct brcms_phy *pi)
+{
 }
 
 static void wlc_phy_radio_preinit_205x(struct brcms_phy *pi)
@@ -17442,11 +20004,257 @@ static void wlc_phy_radio_preinit_205x(struct brcms_phy *pi)
 
 }
 
+static void wlc_phy_radio_init_2057(struct brcms_phy *pi)
+{
+	struct radio_20xx_regs *regs_2057_ptr = NULL;
+
+	if (NREV_IS(pi->pubpi.phy_rev, 7)) {
+		regs_2057_ptr = regs_2057_rev4;
+	} else if (NREV_IS(pi->pubpi.phy_rev, 8)
+		   || NREV_IS(pi->pubpi.phy_rev, 9)) {
+		switch (pi->pubpi.radiorev) {
+		case 5:
+
+			if (pi->pubpi.radiover == 0x0)
+				regs_2057_ptr = regs_2057_rev5;
+			else if (pi->pubpi.radiover == 0x1)
+				regs_2057_ptr = regs_2057_rev5v1;
+			else
+				break;
+
+		case 7:
+
+			regs_2057_ptr = regs_2057_rev7;
+			break;
+
+		case 8:
+
+			regs_2057_ptr = regs_2057_rev8;
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	wlc_phy_init_radio_regs_allbands(pi, regs_2057_ptr);
+}
+
+static u16 wlc_phy_radio205x_rcal(struct brcms_phy *pi)
+{
+	u16 rcal_reg = 0;
+	int i;
+
+	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+
+		if (pi->pubpi.radiorev == 5) {
+
+			and_phy_reg(pi, 0x342, ~(0x1 << 1));
+
+			udelay(10);
+
+			mod_radio_reg(pi, RADIO_2057_IQTEST_SEL_PU, 0x1, 0x1);
+			mod_radio_reg(pi, RADIO_2057v7_IQTEST_SEL_PU2, 0x2,
+				      0x1);
+		}
+		mod_radio_reg(pi, RADIO_2057_RCAL_CONFIG, 0x1, 0x1);
+
+		udelay(10);
+
+		mod_radio_reg(pi, RADIO_2057_RCAL_CONFIG, 0x3, 0x3);
+
+		for (i = 0; i < MAX_205x_RCAL_WAITLOOPS; i++) {
+			rcal_reg = read_radio_reg(pi, RADIO_2057_RCAL_STATUS);
+			if (rcal_reg & 0x1)
+				break;
+
+			udelay(100);
+		}
+
+		if (WARN(i == MAX_205x_RCAL_WAITLOOPS,
+			 "HW error: radio calib2"))
+			return 0;
+
+		mod_radio_reg(pi, RADIO_2057_RCAL_CONFIG, 0x2, 0x0);
+
+		rcal_reg = read_radio_reg(pi, RADIO_2057_RCAL_STATUS) & 0x3e;
+
+		mod_radio_reg(pi, RADIO_2057_RCAL_CONFIG, 0x1, 0x0);
+		if (pi->pubpi.radiorev == 5) {
+
+			mod_radio_reg(pi, RADIO_2057_IQTEST_SEL_PU, 0x1, 0x0);
+			mod_radio_reg(pi, RADIO_2057v7_IQTEST_SEL_PU2, 0x2,
+				      0x0);
+		}
+
+		if ((pi->pubpi.radiorev <= 4) || (pi->pubpi.radiorev == 6)) {
+
+			mod_radio_reg(pi, RADIO_2057_TEMPSENSE_CONFIG, 0x3c,
+				      rcal_reg);
+			mod_radio_reg(pi, RADIO_2057_BANDGAP_RCAL_TRIM, 0xf0,
+				      rcal_reg << 2);
+		}
+
+	} else if (NREV_IS(pi->pubpi.phy_rev, 3)) {
+		u16 savereg;
+
+		savereg =
+			read_radio_reg(
+				pi,
+				RADIO_2056_SYN_PLL_MAST2 |
+				RADIO_2056_SYN);
+		write_radio_reg(pi, RADIO_2056_SYN_PLL_MAST2 | RADIO_2056_SYN,
+				savereg | 0x7);
+		udelay(10);
+
+		write_radio_reg(pi, RADIO_2056_SYN_RCAL_MASTER | RADIO_2056_SYN,
+				0x1);
+		udelay(10);
+
+		write_radio_reg(pi, RADIO_2056_SYN_RCAL_MASTER | RADIO_2056_SYN,
+				0x9);
+
+		for (i = 0; i < MAX_205x_RCAL_WAITLOOPS; i++) {
+			rcal_reg = read_radio_reg(
+				pi,
+				RADIO_2056_SYN_RCAL_CODE_OUT |
+				RADIO_2056_SYN);
+			if (rcal_reg & 0x80)
+				break;
+
+			udelay(100);
+		}
+
+		if (WARN(i == MAX_205x_RCAL_WAITLOOPS,
+			 "HW error: radio calib3"))
+			return 0;
+
+		write_radio_reg(pi, RADIO_2056_SYN_RCAL_MASTER | RADIO_2056_SYN,
+				0x1);
+
+		rcal_reg =
+			read_radio_reg(pi,
+				       RADIO_2056_SYN_RCAL_CODE_OUT |
+				       RADIO_2056_SYN);
+
+		write_radio_reg(pi, RADIO_2056_SYN_RCAL_MASTER | RADIO_2056_SYN,
+				0x0);
+
+		write_radio_reg(pi, RADIO_2056_SYN_PLL_MAST2 | RADIO_2056_SYN,
+				savereg);
+
+		return rcal_reg & 0x1f;
+	}
+	return rcal_reg & 0x3e;
+}
+
+static u16 wlc_phy_radio2057_rccal(struct brcms_phy *pi)
+{
+	u16 rccal_valid;
+	int i;
+	bool chip43226_6362A0;
+
+	chip43226_6362A0 = ((pi->pubpi.radiorev == 3)
+			    || (pi->pubpi.radiorev == 4)
+			    || (pi->pubpi.radiorev == 6));
+
+	rccal_valid = 0;
+	if (chip43226_6362A0) {
+		write_radio_reg(pi, RADIO_2057_RCCAL_MASTER, 0x61);
+		write_radio_reg(pi, RADIO_2057_RCCAL_TRC0, 0xc0);
+	} else {
+		write_radio_reg(pi, RADIO_2057v7_RCCAL_MASTER, 0x61);
+
+		write_radio_reg(pi, RADIO_2057_RCCAL_TRC0, 0xe9);
+	}
+	write_radio_reg(pi, RADIO_2057_RCCAL_X1, 0x6e);
+	write_radio_reg(pi, RADIO_2057_RCCAL_START_R1_Q1_P1, 0x55);
+
+	for (i = 0; i < MAX_205x_RCAL_WAITLOOPS; i++) {
+		rccal_valid = read_radio_reg(pi, RADIO_2057_RCCAL_DONE_OSCCAP);
+		if (rccal_valid & 0x2)
+			break;
+
+		udelay(500);
+	}
+
+	write_radio_reg(pi, RADIO_2057_RCCAL_START_R1_Q1_P1, 0x15);
+
+	rccal_valid = 0;
+	if (chip43226_6362A0) {
+		write_radio_reg(pi, RADIO_2057_RCCAL_MASTER, 0x69);
+		write_radio_reg(pi, RADIO_2057_RCCAL_TRC0, 0xb0);
+	} else {
+		write_radio_reg(pi, RADIO_2057v7_RCCAL_MASTER, 0x69);
+
+		write_radio_reg(pi, RADIO_2057_RCCAL_TRC0, 0xd5);
+	}
+	write_radio_reg(pi, RADIO_2057_RCCAL_X1, 0x6e);
+	write_radio_reg(pi, RADIO_2057_RCCAL_START_R1_Q1_P1, 0x55);
+
+	for (i = 0; i < MAX_205x_RCAL_WAITLOOPS; i++) {
+		rccal_valid = read_radio_reg(pi, RADIO_2057_RCCAL_DONE_OSCCAP);
+		if (rccal_valid & 0x2)
+			break;
+
+		udelay(500);
+	}
+
+	write_radio_reg(pi, RADIO_2057_RCCAL_START_R1_Q1_P1, 0x15);
+
+	rccal_valid = 0;
+	if (chip43226_6362A0) {
+		write_radio_reg(pi, RADIO_2057_RCCAL_MASTER, 0x73);
+
+		write_radio_reg(pi, RADIO_2057_RCCAL_X1, 0x28);
+		write_radio_reg(pi, RADIO_2057_RCCAL_TRC0, 0xb0);
+	} else {
+		write_radio_reg(pi, RADIO_2057v7_RCCAL_MASTER, 0x73);
+		write_radio_reg(pi, RADIO_2057_RCCAL_X1, 0x6e);
+		write_radio_reg(pi, RADIO_2057_RCCAL_TRC0, 0x99);
+	}
+	write_radio_reg(pi, RADIO_2057_RCCAL_START_R1_Q1_P1, 0x55);
+
+	for (i = 0; i < MAX_205x_RCAL_WAITLOOPS; i++) {
+		rccal_valid = read_radio_reg(pi, RADIO_2057_RCCAL_DONE_OSCCAP);
+		if (rccal_valid & 0x2)
+			break;
+
+		udelay(500);
+	}
+
+	if (WARN(!(rccal_valid & 0x2), "HW error: radio calib4"))
+		return 0;
+
+	write_radio_reg(pi, RADIO_2057_RCCAL_START_R1_Q1_P1, 0x15);
+
+	return rccal_valid;
+}
+
+static void wlc_phy_radio_postinit_2057(struct brcms_phy *pi)
+{
+
+	mod_radio_reg(pi, RADIO_2057_XTALPUOVR_PINCTRL, 0x1, 0x1);
+
+	mod_radio_reg(pi, RADIO_2057_RFPLL_MISC_CAL_RESETN, 0x78, 0x78);
+	mod_radio_reg(pi, RADIO_2057_XTAL_CONFIG2, 0x80, 0x80);
+	mdelay(2);
+	mod_radio_reg(pi, RADIO_2057_RFPLL_MISC_CAL_RESETN, 0x78, 0x0);
+	mod_radio_reg(pi, RADIO_2057_XTAL_CONFIG2, 0x80, 0x0);
+
+	if (pi->phy_init_por) {
+		wlc_phy_radio205x_rcal(pi);
+		wlc_phy_radio2057_rccal(pi);
+	}
+
+	mod_radio_reg(pi, RADIO_2057_RFPLL_MASTER, 0x8, 0x0);
+}
+
 static void wlc_phy_radio_init_2056(struct brcms_phy *pi)
 {
-	struct radio_regs *regs_SYN_2056_ptr = NULL;
-	struct radio_regs *regs_TX_2056_ptr = NULL;
-	struct radio_regs *regs_RX_2056_ptr = NULL;
+	const struct radio_regs *regs_SYN_2056_ptr = NULL;
+	const struct radio_regs *regs_TX_2056_ptr = NULL;
+	const struct radio_regs *regs_RX_2056_ptr = NULL;
 
 	if (NREV_IS(pi->pubpi.phy_rev, 3)) {
 		regs_SYN_2056_ptr = regs_SYN_2056;
@@ -17515,92 +20323,190 @@ static void wlc_phy_radio_postinit_2056(struct brcms_phy *pi)
 	mod_radio_reg(pi, RADIO_2056_SYN_COM_RESET, 0x2, 0x0);
 
 	if ((pi->sh->boardflags2 & BFL2_LEGACY)
-	    || (pi->sh->boardflags2 & BFL2_XTALBUFOUTEN)) {
-
+	    || (pi->sh->boardflags2 & BFL2_XTALBUFOUTEN))
 		mod_radio_reg(pi, RADIO_2056_SYN_PLL_MAST2, 0xf4, 0x0);
-	} else {
-
+	else
 		mod_radio_reg(pi, RADIO_2056_SYN_PLL_MAST2, 0xfc, 0x0);
-	}
 
 	mod_radio_reg(pi, RADIO_2056_SYN_RCCAL_CTRL0, 0x1, 0x0);
 
-	if (pi->phy_init_por) {
+	if (pi->phy_init_por)
 		wlc_phy_radio205x_rcal(pi);
-	}
 }
 
-static void wlc_phy_radio_init_2057(struct brcms_phy *pi)
+static void wlc_phy_radio_preinit_2055(struct brcms_phy *pi)
 {
-	struct radio_20xx_regs *regs_2057_ptr = NULL;
 
-	if (NREV_IS(pi->pubpi.phy_rev, 7)) {
+	and_phy_reg(pi, 0x78, ~RFCC_POR_FORCE);
+	or_phy_reg(pi, 0x78, RFCC_CHIP0_PU | RFCC_OE_POR_FORCE);
 
-		regs_2057_ptr = regs_2057_rev4;
-	} else if (NREV_IS(pi->pubpi.phy_rev, 8)
-		   || NREV_IS(pi->pubpi.phy_rev, 9)) {
-		switch (pi->pubpi.radiorev) {
-		case 5:
+	or_phy_reg(pi, 0x78, RFCC_POR_FORCE);
+}
 
-			if (pi->pubpi.radiover == 0x0) {
+static void wlc_phy_radio_init_2055(struct brcms_phy *pi)
+{
+	wlc_phy_init_radio_regs(pi, regs_2055, RADIO_DEFAULT_CORE);
+}
 
-				regs_2057_ptr = regs_2057_rev5;
+static void wlc_phy_radio_postinit_2055(struct brcms_phy *pi)
+{
 
-			} else if (pi->pubpi.radiover == 0x1) {
+	and_radio_reg(pi, RADIO_2055_MASTER_CNTRL1,
+		      ~(RADIO_2055_JTAGCTRL_MASK | RADIO_2055_JTAGSYNC_MASK));
 
-				regs_2057_ptr = regs_2057_rev5v1;
-			} else {
-				break;
-			}
+	if (((pi->sh->sromrev >= 4)
+	     && !(pi->sh->boardflags2 & BFL2_RXBB_INT_REG_DIS))
+	    || ((pi->sh->sromrev < 4))) {
+		and_radio_reg(pi, RADIO_2055_CORE1_RXBB_REGULATOR, 0x7F);
+		and_radio_reg(pi, RADIO_2055_CORE2_RXBB_REGULATOR, 0x7F);
+	}
 
-		case 7:
+	mod_radio_reg(pi, RADIO_2055_RRCCAL_N_OPT_SEL, 0x3F, 0x2C);
+	write_radio_reg(pi, RADIO_2055_CAL_MISC, 0x3C);
 
-			regs_2057_ptr = regs_2057_rev7;
-			break;
+	and_radio_reg(pi, RADIO_2055_CAL_MISC,
+		      ~(RADIO_2055_RRCAL_START | RADIO_2055_RRCAL_RST_N));
 
-		case 8:
+	or_radio_reg(pi, RADIO_2055_CAL_LPO_CNTRL, RADIO_2055_CAL_LPO_ENABLE);
 
-			regs_2057_ptr = regs_2057_rev8;
-			break;
+	or_radio_reg(pi, RADIO_2055_CAL_MISC, RADIO_2055_RRCAL_RST_N);
 
-		default:
-			break;
-		}
+	udelay(1000);
+
+	or_radio_reg(pi, RADIO_2055_CAL_MISC, RADIO_2055_RRCAL_START);
+
+	SPINWAIT(((read_radio_reg(pi, RADIO_2055_CAL_COUNTER_OUT2) &
+		   RADIO_2055_RCAL_DONE) != RADIO_2055_RCAL_DONE), 2000);
+
+	if (WARN((read_radio_reg(pi, RADIO_2055_CAL_COUNTER_OUT2) &
+		  RADIO_2055_RCAL_DONE) != RADIO_2055_RCAL_DONE,
+		 "HW error: radio calibration1\n"))
+		return;
+
+	and_radio_reg(pi, RADIO_2055_CAL_LPO_CNTRL,
+		      ~(RADIO_2055_CAL_LPO_ENABLE));
+
+	wlc_phy_chanspec_set((struct brcms_phy_pub *) pi, pi->radio_chanspec);
+
+	write_radio_reg(pi, RADIO_2055_CORE1_RXBB_LPF, 9);
+	write_radio_reg(pi, RADIO_2055_CORE2_RXBB_LPF, 9);
+
+	write_radio_reg(pi, RADIO_2055_CORE1_RXBB_MIDAC_HIPAS, 0x83);
+	write_radio_reg(pi, RADIO_2055_CORE2_RXBB_MIDAC_HIPAS, 0x83);
+
+	mod_radio_reg(pi, RADIO_2055_CORE1_LNA_GAINBST,
+		      RADIO_2055_GAINBST_VAL_MASK, RADIO_2055_GAINBST_CODE);
+	mod_radio_reg(pi, RADIO_2055_CORE2_LNA_GAINBST,
+		      RADIO_2055_GAINBST_VAL_MASK, RADIO_2055_GAINBST_CODE);
+	if (pi->nphy_gain_boost) {
+		and_radio_reg(pi, RADIO_2055_CORE1_RXRF_SPC1,
+			      ~(RADIO_2055_GAINBST_DISABLE));
+		and_radio_reg(pi, RADIO_2055_CORE2_RXRF_SPC1,
+			      ~(RADIO_2055_GAINBST_DISABLE));
+	} else {
+		or_radio_reg(pi, RADIO_2055_CORE1_RXRF_SPC1,
+			     RADIO_2055_GAINBST_DISABLE);
+		or_radio_reg(pi, RADIO_2055_CORE2_RXRF_SPC1,
+			     RADIO_2055_GAINBST_DISABLE);
 	}
 
-	wlc_phy_init_radio_regs_allbands(pi, regs_2057_ptr);
+	udelay(2);
 }
 
-static void wlc_phy_radio_postinit_2057(struct brcms_phy *pi)
+void wlc_phy_switch_radio_nphy(struct brcms_phy *pi, bool on)
 {
+	if (on) {
+		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			if (!pi->radio_is_on) {
+				wlc_phy_radio_preinit_205x(pi);
+				wlc_phy_radio_init_2057(pi);
+				wlc_phy_radio_postinit_2057(pi);
+			}
 
-	mod_radio_reg(pi, RADIO_2057_XTALPUOVR_PINCTRL, 0x1, 0x1);
+			wlc_phy_chanspec_set((struct brcms_phy_pub *) pi,
+					     pi->radio_chanspec);
+		} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+			wlc_phy_radio_preinit_205x(pi);
+			wlc_phy_radio_init_2056(pi);
+			wlc_phy_radio_postinit_2056(pi);
 
-	mod_radio_reg(pi, RADIO_2057_RFPLL_MISC_CAL_RESETN, 0x78, 0x78);
-	mod_radio_reg(pi, RADIO_2057_XTAL_CONFIG2, 0x80, 0x80);
-	mdelay(2);
-	mod_radio_reg(pi, RADIO_2057_RFPLL_MISC_CAL_RESETN, 0x78, 0x0);
-	mod_radio_reg(pi, RADIO_2057_XTAL_CONFIG2, 0x80, 0x0);
+			wlc_phy_chanspec_set((struct brcms_phy_pub *) pi,
+					     pi->radio_chanspec);
+		} else {
+			wlc_phy_radio_preinit_2055(pi);
+			wlc_phy_radio_init_2055(pi);
+			wlc_phy_radio_postinit_2055(pi);
+		}
 
-	if (pi->phy_init_por) {
-		wlc_phy_radio205x_rcal(pi);
-		wlc_phy_radio2057_rccal(pi);
-	}
+		pi->radio_is_on = true;
 
-	mod_radio_reg(pi, RADIO_2057_RFPLL_MASTER, 0x8, 0x0);
+	} else {
+
+		if (NREV_GE(pi->pubpi.phy_rev, 3)
+		    && NREV_LT(pi->pubpi.phy_rev, 7)) {
+			and_phy_reg(pi, 0x78, ~RFCC_CHIP0_PU);
+			mod_radio_reg(pi, RADIO_2056_SYN_COM_PU, 0x2, 0x0);
+
+			write_radio_reg(pi,
+					RADIO_2056_TX_PADA_BOOST_TUNE |
+					RADIO_2056_TX0, 0);
+			write_radio_reg(pi,
+					RADIO_2056_TX_PADG_BOOST_TUNE |
+					RADIO_2056_TX0, 0);
+			write_radio_reg(pi,
+					RADIO_2056_TX_PGAA_BOOST_TUNE |
+					RADIO_2056_TX0, 0);
+			write_radio_reg(pi,
+					RADIO_2056_TX_PGAG_BOOST_TUNE |
+					RADIO_2056_TX0, 0);
+			mod_radio_reg(pi,
+				      RADIO_2056_TX_MIXA_BOOST_TUNE |
+				      RADIO_2056_TX0, 0xf0, 0);
+			write_radio_reg(pi,
+					RADIO_2056_TX_MIXG_BOOST_TUNE |
+					RADIO_2056_TX0, 0);
+
+			write_radio_reg(pi,
+					RADIO_2056_TX_PADA_BOOST_TUNE |
+					RADIO_2056_TX1, 0);
+			write_radio_reg(pi,
+					RADIO_2056_TX_PADG_BOOST_TUNE |
+					RADIO_2056_TX1, 0);
+			write_radio_reg(pi,
+					RADIO_2056_TX_PGAA_BOOST_TUNE |
+					RADIO_2056_TX1, 0);
+			write_radio_reg(pi,
+					RADIO_2056_TX_PGAG_BOOST_TUNE |
+					RADIO_2056_TX1, 0);
+			mod_radio_reg(pi,
+				      RADIO_2056_TX_MIXA_BOOST_TUNE |
+				      RADIO_2056_TX1, 0xf0, 0);
+			write_radio_reg(pi,
+					RADIO_2056_TX_MIXG_BOOST_TUNE |
+					RADIO_2056_TX1, 0);
+
+			pi->radio_is_on = false;
+		}
+
+		if (NREV_GE(pi->pubpi.phy_rev, 8)) {
+			and_phy_reg(pi, 0x78, ~RFCC_CHIP0_PU);
+			pi->radio_is_on = false;
+		}
+
+	}
 }
 
 static bool
 wlc_phy_chan2freq_nphy(struct brcms_phy *pi, uint channel, int *f,
-		       struct chan_info_nphy_radio2057 **t0,
-		       struct chan_info_nphy_radio205x **t1,
-		       struct chan_info_nphy_radio2057_rev5 **t2,
-		       struct chan_info_nphy_2055 **t3)
+		       const struct chan_info_nphy_radio2057 **t0,
+		       const struct chan_info_nphy_radio205x **t1,
+		       const struct chan_info_nphy_radio2057_rev5 **t2,
+		       const struct chan_info_nphy_2055 **t3)
 {
 	uint i;
-	struct chan_info_nphy_radio2057 *chan_info_tbl_p_0 = NULL;
-	struct chan_info_nphy_radio205x *chan_info_tbl_p_1 = NULL;
-	struct chan_info_nphy_radio2057_rev5 *chan_info_tbl_p_2 = NULL;
+	const struct chan_info_nphy_radio2057 *chan_info_tbl_p_0 = NULL;
+	const struct chan_info_nphy_radio205x *chan_info_tbl_p_1 = NULL;
+	const struct chan_info_nphy_radio2057_rev5 *chan_info_tbl_p_2 = NULL;
 	u32 tbl_len = 0;
 
 	int freq = 0;
@@ -17621,40 +20527,35 @@ wlc_phy_chan2freq_nphy(struct brcms_phy *pi, uint channel, int *f,
 				if (pi->pubpi.radiover == 0x0) {
 
 					chan_info_tbl_p_2 =
-					    chan_info_nphyrev8_2057_rev5;
-					tbl_len =
-					    ARRAY_SIZE
-					    (chan_info_nphyrev8_2057_rev5);
+						chan_info_nphyrev8_2057_rev5;
+					tbl_len = ARRAY_SIZE(
+						  chan_info_nphyrev8_2057_rev5);
 
 				} else if (pi->pubpi.radiover == 0x1) {
 
 					chan_info_tbl_p_2 =
-					    chan_info_nphyrev9_2057_rev5v1;
-					tbl_len =
-					    ARRAY_SIZE
-					    (chan_info_nphyrev9_2057_rev5v1);
+						chan_info_nphyrev9_2057_rev5v1;
+					tbl_len = ARRAY_SIZE(
+						chan_info_nphyrev9_2057_rev5v1);
 
 				}
 				break;
 
 			case 7:
 				chan_info_tbl_p_0 =
-				    chan_info_nphyrev8_2057_rev7;
-				tbl_len =
-				    ARRAY_SIZE(chan_info_nphyrev8_2057_rev7);
+					chan_info_nphyrev8_2057_rev7;
+				tbl_len = ARRAY_SIZE(
+						  chan_info_nphyrev8_2057_rev7);
 				break;
 
 			case 8:
 				chan_info_tbl_p_0 =
-				    chan_info_nphyrev8_2057_rev8;
-				tbl_len =
-				    ARRAY_SIZE(chan_info_nphyrev8_2057_rev8);
+					chan_info_nphyrev8_2057_rev8;
+				tbl_len = ARRAY_SIZE(
+						  chan_info_nphyrev8_2057_rev8);
 				break;
 
 			default:
-				if (NORADIO_ENAB(pi->pubpi)) {
-					goto fail;
-				}
 				break;
 			}
 		} else if (NREV_IS(pi->pubpi.phy_rev, 16)) {
@@ -17677,9 +20578,9 @@ wlc_phy_chan2freq_nphy(struct brcms_phy *pi, uint channel, int *f,
 			}
 		}
 
-		if (i >= tbl_len) {
+		if (i >= tbl_len)
 			goto fail;
-		}
+
 		if (pi->pubpi.radiorev == 5) {
 			*t2 = &chan_info_tbl_p_2[i];
 			freq = chan_info_tbl_p_2[i].freq;
@@ -17710,7 +20611,7 @@ wlc_phy_chan2freq_nphy(struct brcms_phy *pi, uint channel, int *f,
 			case 9:
 				chan_info_tbl_p_1 = chan_info_nphyrev5n6_2056v7;
 				tbl_len =
-				    ARRAY_SIZE(chan_info_nphyrev5n6_2056v7);
+					ARRAY_SIZE(chan_info_nphyrev5n6_2056v7);
 				break;
 			case 8:
 				chan_info_tbl_p_1 = chan_info_nphyrev6_2056v8;
@@ -17718,12 +20619,10 @@ wlc_phy_chan2freq_nphy(struct brcms_phy *pi, uint channel, int *f,
 				break;
 			case 11:
 				chan_info_tbl_p_1 = chan_info_nphyrev6_2056v11;
-				tbl_len = ARRAY_SIZE(chan_info_nphyrev6_2056v11);
+				tbl_len = ARRAY_SIZE(
+						    chan_info_nphyrev6_2056v11);
 				break;
 			default:
-				if (NORADIO_ENAB(pi->pubpi)) {
-					goto fail;
-				}
 				break;
 			}
 		}
@@ -17733,9 +20632,9 @@ wlc_phy_chan2freq_nphy(struct brcms_phy *pi, uint channel, int *f,
 				break;
 		}
 
-		if (i >= tbl_len) {
+		if (i >= tbl_len)
 			goto fail;
-		}
+
 		*t1 = &chan_info_tbl_p_1[i];
 		freq = chan_info_tbl_p_1[i].freq;
 
@@ -17744,9 +20643,9 @@ wlc_phy_chan2freq_nphy(struct brcms_phy *pi, uint channel, int *f,
 			if (chan_info_nphy_2055[i].chan == channel)
 				break;
 
-		if (i >= ARRAY_SIZE(chan_info_nphy_2055)) {
+		if (i >= ARRAY_SIZE(chan_info_nphy_2055))
 			goto fail;
-		}
+
 		*t3 = &chan_info_nphy_2055[i];
 		freq = chan_info_nphy_2055[i].freq;
 	}
@@ -17754,7 +20653,7 @@ wlc_phy_chan2freq_nphy(struct brcms_phy *pi, uint channel, int *f,
 	*f = freq;
 	return true;
 
- fail:
+fail:
 	*f = WL_CHAN_FREQ_RANGE_2G;
 	return false;
 }
@@ -17762,13 +20661,10 @@ wlc_phy_chan2freq_nphy(struct brcms_phy *pi, uint channel, int *f,
 u8 wlc_phy_get_chan_freq_range_nphy(struct brcms_phy *pi, uint channel)
 {
 	int freq;
-	struct chan_info_nphy_radio2057 *t0 = NULL;
-	struct chan_info_nphy_radio205x *t1 = NULL;
-	struct chan_info_nphy_radio2057_rev5 *t2 = NULL;
-	struct chan_info_nphy_2055 *t3 = NULL;
-
-	if (NORADIO_ENAB(pi->pubpi))
-		return WL_CHAN_FREQ_RANGE_2G;
+	const struct chan_info_nphy_radio2057 *t0 = NULL;
+	const struct chan_info_nphy_radio205x *t1 = NULL;
+	const struct chan_info_nphy_radio2057_rev5 *t2 = NULL;
+	const struct chan_info_nphy_2055 *t3 = NULL;
 
 	if (channel == 0)
 		channel = CHSPEC_CHANNEL(pi->radio_chanspec);
@@ -17778,18 +20674,17 @@ u8 wlc_phy_get_chan_freq_range_nphy(struct brcms_phy *pi, uint channel)
 	if (CHSPEC_IS2G(pi->radio_chanspec))
 		return WL_CHAN_FREQ_RANGE_2G;
 
-	if ((freq >= BASE_LOW_5G_CHAN) && (freq < BASE_MID_5G_CHAN)) {
+	if ((freq >= BASE_LOW_5G_CHAN) && (freq < BASE_MID_5G_CHAN))
 		return WL_CHAN_FREQ_RANGE_5GL;
-	} else if ((freq >= BASE_MID_5G_CHAN) && (freq < BASE_HIGH_5G_CHAN)) {
+	else if ((freq >= BASE_MID_5G_CHAN) && (freq < BASE_HIGH_5G_CHAN))
 		return WL_CHAN_FREQ_RANGE_5GM;
-	} else {
+	else
 		return WL_CHAN_FREQ_RANGE_5GH;
-	}
 }
 
 static void
 wlc_phy_chanspec_radio2055_setup(struct brcms_phy *pi,
-				 struct chan_info_nphy_2055 *ci)
+				 const struct chan_info_nphy_2055 *ci)
 {
 
 	write_radio_reg(pi, RADIO_2055_PLL_REF, ci->RF_pll_ref);
@@ -17854,7 +20749,7 @@ static void
 wlc_phy_chanspec_radio2056_setup(struct brcms_phy *pi,
 				 const struct chan_info_nphy_radio205x *ci)
 {
-	struct radio_regs *regs_SYN_2056_ptr = NULL;
+	const struct radio_regs *regs_SYN_2056_ptr = NULL;
 
 	write_radio_reg(pi,
 			RADIO_2056_SYN_PLL_VCOCAL1 | RADIO_2056_SYN,
@@ -17960,15 +20855,14 @@ wlc_phy_chanspec_radio2056_setup(struct brcms_phy *pi,
 			break;
 		}
 	}
-	if (CHSPEC_IS2G(pi->radio_chanspec)) {
+	if (CHSPEC_IS2G(pi->radio_chanspec))
 		write_radio_reg(pi, RADIO_2056_SYN_PLL_CP2 |
 				RADIO_2056_SYN,
 				(u16) regs_SYN_2056_ptr[0x49 - 2].init_g);
-	} else {
+	else
 		write_radio_reg(pi, RADIO_2056_SYN_PLL_CP2 |
 				RADIO_2056_SYN,
 				(u16) regs_SYN_2056_ptr[0x49 - 2].init_a);
-	}
 
 	if (pi->sh->boardflags2 & BFL2_GPLL_WAR) {
 		if (CHSPEC_IS2G(pi->radio_chanspec)) {
@@ -18069,7 +20963,8 @@ wlc_phy_chanspec_radio2056_setup(struct brcms_phy *pi,
 						 mixg_boost_tune);
 			} else {
 
-				bias = IS40MHZ(pi) ? 0x40 : 0x20;
+				bias = (pi->bw == WL_CHANSPEC_BW_40) ?
+				       0x40 : 0x20;
 
 				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
 						 INTPAG_IMAIN_STAT, bias);
@@ -18116,11 +21011,11 @@ wlc_phy_chanspec_radio2056_setup(struct brcms_phy *pi,
 
 			paa_boost_tune = 0x0;
 			pada_boost_tune = 0x77;
-			if (freq != 5825) {
+			if (freq != 5825)
 				pgaa_boost_tune = -(int)(freq - 18) / 36 + 168;
-			} else {
+			else
 				pgaa_boost_tune = 6;
-			}
+
 			mixa_boost_tune = 0xf;
 		}
 
@@ -18146,9 +21041,8 @@ wlc_phy_chanspec_radio2056_setup(struct brcms_phy *pi,
 
 			if ((pi->sh->chip == BCM43224_CHIP_ID) ||
 			    (pi->sh->chip == BCM43225_CHIP_ID)) {
-				if (pi->sh->chippkg == BCM43224_FAB_SMIC) {
+				if (pi->sh->chippkg == BCM43224_FAB_SMIC)
 					cascbias = 0x35;
-				}
 			}
 
 			pabias = (pi->phy_pabias == 0) ? 0x30 : pi->phy_pabias;
@@ -18186,117 +21080,12 @@ void wlc_phy_radio205x_vcocal_nphy(struct brcms_phy *pi)
 	udelay(300);
 }
 
-#define MAX_205x_RCAL_WAITLOOPS 10000
-
-static u16 wlc_phy_radio205x_rcal(struct brcms_phy *pi)
-{
-	u16 rcal_reg = 0;
-	int i;
-
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-
-		if (pi->pubpi.radiorev == 5) {
-
-			and_phy_reg(pi, 0x342, ~(0x1 << 1));
-
-			udelay(10);
-
-			mod_radio_reg(pi, RADIO_2057_IQTEST_SEL_PU, 0x1, 0x1);
-			mod_radio_reg(pi, RADIO_2057v7_IQTEST_SEL_PU2, 0x2,
-				      0x1);
-		}
-		mod_radio_reg(pi, RADIO_2057_RCAL_CONFIG, 0x1, 0x1);
-
-		udelay(10);
-
-		mod_radio_reg(pi, RADIO_2057_RCAL_CONFIG, 0x3, 0x3);
-
-		for (i = 0; i < MAX_205x_RCAL_WAITLOOPS; i++) {
-			rcal_reg = read_radio_reg(pi, RADIO_2057_RCAL_STATUS);
-			if (rcal_reg & 0x1) {
-				break;
-			}
-			udelay(100);
-		}
-
-		if (WARN(i == MAX_205x_RCAL_WAITLOOPS,
-			 "HW error: radio calib2"))
-			return 0;
-
-		mod_radio_reg(pi, RADIO_2057_RCAL_CONFIG, 0x2, 0x0);
-
-		rcal_reg = read_radio_reg(pi, RADIO_2057_RCAL_STATUS) & 0x3e;
-
-		mod_radio_reg(pi, RADIO_2057_RCAL_CONFIG, 0x1, 0x0);
-		if (pi->pubpi.radiorev == 5) {
-
-			mod_radio_reg(pi, RADIO_2057_IQTEST_SEL_PU, 0x1, 0x0);
-			mod_radio_reg(pi, RADIO_2057v7_IQTEST_SEL_PU2, 0x2,
-				      0x0);
-		}
-
-		if ((pi->pubpi.radiorev <= 4) || (pi->pubpi.radiorev == 6)) {
-
-			mod_radio_reg(pi, RADIO_2057_TEMPSENSE_CONFIG, 0x3c,
-				      rcal_reg);
-			mod_radio_reg(pi, RADIO_2057_BANDGAP_RCAL_TRIM, 0xf0,
-				      rcal_reg << 2);
-		}
-
-	} else if (NREV_IS(pi->pubpi.phy_rev, 3)) {
-		u16 savereg;
-
-		savereg =
-		    read_radio_reg(pi,
-				   RADIO_2056_SYN_PLL_MAST2 | RADIO_2056_SYN);
-		write_radio_reg(pi, RADIO_2056_SYN_PLL_MAST2 | RADIO_2056_SYN,
-				savereg | 0x7);
-		udelay(10);
-
-		write_radio_reg(pi, RADIO_2056_SYN_RCAL_MASTER | RADIO_2056_SYN,
-				0x1);
-		udelay(10);
-
-		write_radio_reg(pi, RADIO_2056_SYN_RCAL_MASTER | RADIO_2056_SYN,
-				0x9);
-
-		for (i = 0; i < MAX_205x_RCAL_WAITLOOPS; i++) {
-			rcal_reg = read_radio_reg(pi,
-						  RADIO_2056_SYN_RCAL_CODE_OUT |
-						  RADIO_2056_SYN);
-			if (rcal_reg & 0x80) {
-				break;
-			}
-			udelay(100);
-		}
-
-		if (WARN(i == MAX_205x_RCAL_WAITLOOPS,
-			 "HW error: radio calib3"))
-			return 0;
-
-		write_radio_reg(pi, RADIO_2056_SYN_RCAL_MASTER | RADIO_2056_SYN,
-				0x1);
-
-		rcal_reg =
-		    read_radio_reg(pi,
-				   RADIO_2056_SYN_RCAL_CODE_OUT |
-				   RADIO_2056_SYN);
-
-		write_radio_reg(pi, RADIO_2056_SYN_RCAL_MASTER | RADIO_2056_SYN,
-				0x0);
-
-		write_radio_reg(pi, RADIO_2056_SYN_PLL_MAST2 | RADIO_2056_SYN,
-				savereg);
-
-		return rcal_reg & 0x1f;
-	}
-	return rcal_reg & 0x3e;
-}
-
 static void
-wlc_phy_chanspec_radio2057_setup(struct brcms_phy *pi,
-			const struct chan_info_nphy_radio2057 *ci,
-			const struct chan_info_nphy_radio2057_rev5 *ci2)
+wlc_phy_chanspec_radio2057_setup(
+	struct brcms_phy *pi,
+	const struct chan_info_nphy_radio2057 *ci,
+	const struct chan_info_nphy_radio2057_rev5 *
+	ci2)
 {
 	int coreNum;
 	u16 txmix2g_tune_boost_pu = 0;
@@ -18455,9 +21244,8 @@ wlc_phy_chanspec_radio2057_setup(struct brcms_phy *pi,
 
 	if (CHSPEC_IS2G(pi->radio_chanspec)) {
 		if (PHY_IPA(pi)) {
-			if (pi->pubpi.radiorev == 3) {
+			if (pi->pubpi.radiorev == 3)
 				txmix2g_tune_boost_pu = 0x6b;
-			}
 
 			if (pi->pubpi.radiorev == 5)
 				pad2g_tune_pus = 0x73;
@@ -18489,430 +21277,8 @@ wlc_phy_chanspec_radio2057_setup(struct brcms_phy *pi,
 	wlc_phy_radio205x_vcocal_nphy(pi);
 }
 
-static u16 wlc_phy_radio2057_rccal(struct brcms_phy *pi)
-{
-	u16 rccal_valid;
-	int i;
-	bool chip43226_6362A0;
-
-	chip43226_6362A0 = ((pi->pubpi.radiorev == 3)
-			    || (pi->pubpi.radiorev == 4)
-			    || (pi->pubpi.radiorev == 6));
-
-	rccal_valid = 0;
-	if (chip43226_6362A0) {
-		write_radio_reg(pi, RADIO_2057_RCCAL_MASTER, 0x61);
-		write_radio_reg(pi, RADIO_2057_RCCAL_TRC0, 0xc0);
-	} else {
-		write_radio_reg(pi, RADIO_2057v7_RCCAL_MASTER, 0x61);
-
-		write_radio_reg(pi, RADIO_2057_RCCAL_TRC0, 0xe9);
-	}
-	write_radio_reg(pi, RADIO_2057_RCCAL_X1, 0x6e);
-	write_radio_reg(pi, RADIO_2057_RCCAL_START_R1_Q1_P1, 0x55);
-
-	for (i = 0; i < MAX_205x_RCAL_WAITLOOPS; i++) {
-		rccal_valid = read_radio_reg(pi, RADIO_2057_RCCAL_DONE_OSCCAP);
-		if (rccal_valid & 0x2) {
-			break;
-		}
-		udelay(500);
-	}
-
-	write_radio_reg(pi, RADIO_2057_RCCAL_START_R1_Q1_P1, 0x15);
-
-	rccal_valid = 0;
-	if (chip43226_6362A0) {
-		write_radio_reg(pi, RADIO_2057_RCCAL_MASTER, 0x69);
-		write_radio_reg(pi, RADIO_2057_RCCAL_TRC0, 0xb0);
-	} else {
-		write_radio_reg(pi, RADIO_2057v7_RCCAL_MASTER, 0x69);
-
-		write_radio_reg(pi, RADIO_2057_RCCAL_TRC0, 0xd5);
-	}
-	write_radio_reg(pi, RADIO_2057_RCCAL_X1, 0x6e);
-	write_radio_reg(pi, RADIO_2057_RCCAL_START_R1_Q1_P1, 0x55);
-
-	for (i = 0; i < MAX_205x_RCAL_WAITLOOPS; i++) {
-		rccal_valid = read_radio_reg(pi, RADIO_2057_RCCAL_DONE_OSCCAP);
-		if (rccal_valid & 0x2) {
-			break;
-		}
-		udelay(500);
-	}
-
-	write_radio_reg(pi, RADIO_2057_RCCAL_START_R1_Q1_P1, 0x15);
-
-	rccal_valid = 0;
-	if (chip43226_6362A0) {
-		write_radio_reg(pi, RADIO_2057_RCCAL_MASTER, 0x73);
-
-		write_radio_reg(pi, RADIO_2057_RCCAL_X1, 0x28);
-		write_radio_reg(pi, RADIO_2057_RCCAL_TRC0, 0xb0);
-	} else {
-		write_radio_reg(pi, RADIO_2057v7_RCCAL_MASTER, 0x73);
-		write_radio_reg(pi, RADIO_2057_RCCAL_X1, 0x6e);
-		write_radio_reg(pi, RADIO_2057_RCCAL_TRC0, 0x99);
-	}
-	write_radio_reg(pi, RADIO_2057_RCCAL_START_R1_Q1_P1, 0x55);
-
-	for (i = 0; i < MAX_205x_RCAL_WAITLOOPS; i++) {
-		rccal_valid = read_radio_reg(pi, RADIO_2057_RCCAL_DONE_OSCCAP);
-		if (rccal_valid & 0x2) {
-			break;
-		}
-		udelay(500);
-	}
-
-	if (WARN(!(rccal_valid & 0x2), "HW error: radio calib4"))
-		return 0;
-
-	write_radio_reg(pi, RADIO_2057_RCCAL_START_R1_Q1_P1, 0x15);
-
-	return rccal_valid;
-}
-
-static void
-wlc_phy_adjust_rx_analpfbw_nphy(struct brcms_phy *pi, u16 reduction_factr)
-{
-	if (NREV_GE(pi->pubpi.phy_rev, 3) && NREV_LT(pi->pubpi.phy_rev, 7)) {
-		if ((CHSPEC_CHANNEL(pi->radio_chanspec) == 11) &&
-		    CHSPEC_IS40(pi->radio_chanspec)) {
-			if (!pi->nphy_anarxlpf_adjusted) {
-				write_radio_reg(pi,
-						(RADIO_2056_RX_RXLPF_RCCAL_LPC |
-						 RADIO_2056_RX0),
-						((pi->nphy_rccal_value +
-						  reduction_factr) | 0x80));
-
-				pi->nphy_anarxlpf_adjusted = true;
-			}
-		} else {
-			if (pi->nphy_anarxlpf_adjusted) {
-				write_radio_reg(pi,
-						(RADIO_2056_RX_RXLPF_RCCAL_LPC |
-						 RADIO_2056_RX0),
-						(pi->nphy_rccal_value | 0x80));
-
-				pi->nphy_anarxlpf_adjusted = false;
-			}
-		}
-	}
-}
-
-static void
-wlc_phy_adjust_min_noisevar_nphy(struct brcms_phy *pi, int ntones,
-				 int *tone_id_buf, u32 *noise_var_buf)
-{
-	int i;
-	u32 offset;
-	int tone_id;
-	int tbllen =
-	    CHSPEC_IS40(pi->
-			radio_chanspec) ? NPHY_NOISEVAR_TBLLEN40 :
-	    NPHY_NOISEVAR_TBLLEN20;
-
-	if (pi->nphy_noisevars_adjusted) {
-		for (i = 0; i < pi->nphy_saved_noisevars.bufcount; i++) {
-			tone_id = pi->nphy_saved_noisevars.tone_id[i];
-			offset = (tone_id >= 0) ?
-			    ((tone_id * 2) + 1) : (tbllen + (tone_id * 2) + 1);
-			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_NOISEVAR, 1,
-						 offset, 32,
-						 (void *)&pi->
-						 nphy_saved_noisevars.
-						 min_noise_vars[i]);
-		}
-
-		pi->nphy_saved_noisevars.bufcount = 0;
-		pi->nphy_noisevars_adjusted = false;
-	}
-
-	if ((noise_var_buf != NULL) && (tone_id_buf != NULL)) {
-		pi->nphy_saved_noisevars.bufcount = 0;
-
-		for (i = 0; i < ntones; i++) {
-			tone_id = tone_id_buf[i];
-			offset = (tone_id >= 0) ?
-			    ((tone_id * 2) + 1) : (tbllen + (tone_id * 2) + 1);
-			pi->nphy_saved_noisevars.tone_id[i] = tone_id;
-			wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_NOISEVAR, 1,
-						offset, 32,
-						&pi->nphy_saved_noisevars.
-						min_noise_vars[i]);
-			wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_NOISEVAR, 1,
-						 offset, 32,
-						 (void *)&noise_var_buf[i]);
-			pi->nphy_saved_noisevars.bufcount++;
-		}
-
-		pi->nphy_noisevars_adjusted = true;
-	}
-}
-
-static void wlc_phy_adjust_crsminpwr_nphy(struct brcms_phy *pi, u8 minpwr)
-{
-	u16 regval;
-
-	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-		if ((CHSPEC_CHANNEL(pi->radio_chanspec) == 11) &&
-		    CHSPEC_IS40(pi->radio_chanspec)) {
-			if (!pi->nphy_crsminpwr_adjusted) {
-				regval = read_phy_reg(pi, 0x27d);
-				pi->nphy_crsminpwr[0] = regval & 0xff;
-				regval &= 0xff00;
-				regval |= (u16) minpwr;
-				write_phy_reg(pi, 0x27d, regval);
-
-				regval = read_phy_reg(pi, 0x280);
-				pi->nphy_crsminpwr[1] = regval & 0xff;
-				regval &= 0xff00;
-				regval |= (u16) minpwr;
-				write_phy_reg(pi, 0x280, regval);
-
-				regval = read_phy_reg(pi, 0x283);
-				pi->nphy_crsminpwr[2] = regval & 0xff;
-				regval &= 0xff00;
-				regval |= (u16) minpwr;
-				write_phy_reg(pi, 0x283, regval);
-
-				pi->nphy_crsminpwr_adjusted = true;
-			}
-		} else {
-			if (pi->nphy_crsminpwr_adjusted) {
-				regval = read_phy_reg(pi, 0x27d);
-				regval &= 0xff00;
-				regval |= pi->nphy_crsminpwr[0];
-				write_phy_reg(pi, 0x27d, regval);
-
-				regval = read_phy_reg(pi, 0x280);
-				regval &= 0xff00;
-				regval |= pi->nphy_crsminpwr[1];
-				write_phy_reg(pi, 0x280, regval);
-
-				regval = read_phy_reg(pi, 0x283);
-				regval &= 0xff00;
-				regval |= pi->nphy_crsminpwr[2];
-				write_phy_reg(pi, 0x283, regval);
-
-				pi->nphy_crsminpwr_adjusted = false;
-			}
-		}
-	}
-}
-
-static void wlc_phy_txlpfbw_nphy(struct brcms_phy *pi)
-{
-	u8 tx_lpf_bw = 0;
-
-	if (NREV_GE(pi->pubpi.phy_rev, 3) && NREV_LT(pi->pubpi.phy_rev, 7)) {
-		if (CHSPEC_IS40(pi->radio_chanspec)) {
-			tx_lpf_bw = 3;
-		} else {
-			tx_lpf_bw = 1;
-		}
-
-		if (PHY_IPA(pi)) {
-			if (CHSPEC_IS40(pi->radio_chanspec)) {
-				tx_lpf_bw = 5;
-			} else {
-				tx_lpf_bw = 4;
-			}
-		}
-		write_phy_reg(pi, 0xe8,
-			      (tx_lpf_bw << 0) |
-			      (tx_lpf_bw << 3) |
-			      (tx_lpf_bw << 6) | (tx_lpf_bw << 9));
-
-		if (PHY_IPA(pi)) {
-
-			if (CHSPEC_IS40(pi->radio_chanspec)) {
-				tx_lpf_bw = 4;
-			} else {
-				tx_lpf_bw = 1;
-			}
-
-			write_phy_reg(pi, 0xe9,
-				      (tx_lpf_bw << 0) |
-				      (tx_lpf_bw << 3) |
-				      (tx_lpf_bw << 6) | (tx_lpf_bw << 9));
-		}
-	}
-}
-
-static void wlc_phy_spurwar_nphy(struct brcms_phy *pi)
-{
-	u16 cur_channel = 0;
-	int nphy_adj_tone_id_buf[] = { 57, 58 };
-	u32 nphy_adj_noise_var_buf[] = { 0x3ff, 0x3ff };
-	bool isAdjustNoiseVar = false;
-	uint numTonesAdjust = 0;
-	u32 tempval = 0;
-
-	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-		if (pi->phyhang_avoid)
-			wlc_phy_stay_in_carriersearch_nphy(pi, true);
-
-		cur_channel = CHSPEC_CHANNEL(pi->radio_chanspec);
-
-		if (pi->nphy_gband_spurwar_en) {
-
-			wlc_phy_adjust_rx_analpfbw_nphy(pi,
-							NPHY_ANARXLPFBW_REDUCTIONFACT);
-
-			if (CHSPEC_IS2G(pi->radio_chanspec)) {
-				if ((cur_channel == 11)
-				    && CHSPEC_IS40(pi->radio_chanspec)) {
-
-					wlc_phy_adjust_min_noisevar_nphy(pi, 2,
-									 nphy_adj_tone_id_buf,
-									 nphy_adj_noise_var_buf);
-				} else {
-
-					wlc_phy_adjust_min_noisevar_nphy(pi, 0,
-									 NULL,
-									 NULL);
-				}
-			}
-			wlc_phy_adjust_crsminpwr_nphy(pi,
-						      NPHY_ADJUSTED_MINCRSPOWER);
-		}
-
-		if ((pi->nphy_gband_spurwar2_en)
-		    && CHSPEC_IS2G(pi->radio_chanspec)) {
-
-			if (CHSPEC_IS40(pi->radio_chanspec)) {
-				switch (cur_channel) {
-				case 3:
-					nphy_adj_tone_id_buf[0] = 57;
-					nphy_adj_tone_id_buf[1] = 58;
-					nphy_adj_noise_var_buf[0] = 0x22f;
-					nphy_adj_noise_var_buf[1] = 0x25f;
-					isAdjustNoiseVar = true;
-					break;
-				case 4:
-					nphy_adj_tone_id_buf[0] = 41;
-					nphy_adj_tone_id_buf[1] = 42;
-					nphy_adj_noise_var_buf[0] = 0x22f;
-					nphy_adj_noise_var_buf[1] = 0x25f;
-					isAdjustNoiseVar = true;
-					break;
-				case 5:
-					nphy_adj_tone_id_buf[0] = 25;
-					nphy_adj_tone_id_buf[1] = 26;
-					nphy_adj_noise_var_buf[0] = 0x24f;
-					nphy_adj_noise_var_buf[1] = 0x25f;
-					isAdjustNoiseVar = true;
-					break;
-				case 6:
-					nphy_adj_tone_id_buf[0] = 9;
-					nphy_adj_tone_id_buf[1] = 10;
-					nphy_adj_noise_var_buf[0] = 0x22f;
-					nphy_adj_noise_var_buf[1] = 0x24f;
-					isAdjustNoiseVar = true;
-					break;
-				case 7:
-					nphy_adj_tone_id_buf[0] = 121;
-					nphy_adj_tone_id_buf[1] = 122;
-					nphy_adj_noise_var_buf[0] = 0x18f;
-					nphy_adj_noise_var_buf[1] = 0x24f;
-					isAdjustNoiseVar = true;
-					break;
-				case 8:
-					nphy_adj_tone_id_buf[0] = 105;
-					nphy_adj_tone_id_buf[1] = 106;
-					nphy_adj_noise_var_buf[0] = 0x22f;
-					nphy_adj_noise_var_buf[1] = 0x25f;
-					isAdjustNoiseVar = true;
-					break;
-				case 9:
-					nphy_adj_tone_id_buf[0] = 89;
-					nphy_adj_tone_id_buf[1] = 90;
-					nphy_adj_noise_var_buf[0] = 0x22f;
-					nphy_adj_noise_var_buf[1] = 0x24f;
-					isAdjustNoiseVar = true;
-					break;
-				case 10:
-					nphy_adj_tone_id_buf[0] = 73;
-					nphy_adj_tone_id_buf[1] = 74;
-					nphy_adj_noise_var_buf[0] = 0x22f;
-					nphy_adj_noise_var_buf[1] = 0x24f;
-					isAdjustNoiseVar = true;
-					break;
-				default:
-					isAdjustNoiseVar = false;
-					break;
-				}
-			}
-
-			if (isAdjustNoiseVar) {
-				numTonesAdjust = sizeof(nphy_adj_tone_id_buf) /
-				    sizeof(nphy_adj_tone_id_buf[0]);
-
-				wlc_phy_adjust_min_noisevar_nphy(pi,
-								 numTonesAdjust,
-								 nphy_adj_tone_id_buf,
-								 nphy_adj_noise_var_buf);
-
-				tempval = 0;
-
-			} else {
-
-				wlc_phy_adjust_min_noisevar_nphy(pi, 0, NULL,
-								 NULL);
-			}
-		}
-
-		if ((pi->nphy_aband_spurwar_en) &&
-		    (CHSPEC_IS5G(pi->radio_chanspec))) {
-			switch (cur_channel) {
-			case 54:
-				nphy_adj_tone_id_buf[0] = 32;
-				nphy_adj_noise_var_buf[0] = 0x25f;
-				break;
-			case 38:
-			case 102:
-			case 118:
-				nphy_adj_tone_id_buf[0] = 0;
-				nphy_adj_noise_var_buf[0] = 0x0;
-				break;
-			case 134:
-				nphy_adj_tone_id_buf[0] = 32;
-				nphy_adj_noise_var_buf[0] = 0x21f;
-				break;
-			case 151:
-				nphy_adj_tone_id_buf[0] = 16;
-				nphy_adj_noise_var_buf[0] = 0x23f;
-				break;
-			case 153:
-			case 161:
-				nphy_adj_tone_id_buf[0] = 48;
-				nphy_adj_noise_var_buf[0] = 0x23f;
-				break;
-			default:
-				nphy_adj_tone_id_buf[0] = 0;
-				nphy_adj_noise_var_buf[0] = 0x0;
-				break;
-			}
-
-			if (nphy_adj_tone_id_buf[0]
-			    && nphy_adj_noise_var_buf[0]) {
-				wlc_phy_adjust_min_noisevar_nphy(pi, 1,
-								 nphy_adj_tone_id_buf,
-								 nphy_adj_noise_var_buf);
-			} else {
-				wlc_phy_adjust_min_noisevar_nphy(pi, 0, NULL,
-								 NULL);
-			}
-		}
-
-		if (pi->phyhang_avoid)
-			wlc_phy_stay_in_carriersearch_nphy(pi, false);
-	}
-}
-
 static void
-wlc_phy_chanspec_nphy_setup(struct brcms_phy *pi, chanspec_t chanspec,
+wlc_phy_chanspec_nphy_setup(struct brcms_phy *pi, u16 chanspec,
 			    const struct nphy_sfo_cfg *ci)
 {
 	u16 val;
@@ -18964,14 +21330,11 @@ wlc_phy_chanspec_nphy_setup(struct brcms_phy *pi, chanspec_t chanspec,
 			and_phy_reg(pi, NPHY_TO_BPHY_OFF + BPHY_TEST, ~0x840);
 	}
 
-	if (pi->nphy_txpwrctrl == PHY_TPC_HW_OFF) {
+	if (pi->nphy_txpwrctrl == PHY_TPC_HW_OFF)
 		wlc_phy_txpwr_fixpower_nphy(pi);
-	}
-
-	if (NREV_LT(pi->pubpi.phy_rev, 3)) {
 
+	if (NREV_LT(pi->pubpi.phy_rev, 3))
 		wlc_phy_adjust_lnagaintbl_nphy(pi);
-	}
 
 	wlc_phy_txlpfbw_nphy(pi);
 
@@ -18982,28 +21345,20 @@ wlc_phy_chanspec_nphy_setup(struct brcms_phy *pi, chanspec_t chanspec,
 		val = CHSPEC_CHANNEL(chanspec);
 		if (!CHSPEC_IS40(pi->radio_chanspec)) {
 			if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-				if ((val == 13) || (val == 14) || (val == 153)) {
-					spuravoid = 1;
-				}
-			} else {
-
-				if (((val >= 5) && (val <= 8)) || (val == 13)
-				    || (val == 14)) {
+				if ((val == 13) || (val == 14) || (val == 153))
 					spuravoid = 1;
-				}
+			} else if (((val >= 5) && (val <= 8)) || (val == 13)
+				   || (val == 14)) {
+				spuravoid = 1;
 			}
+		} else if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			if (val == 54)
+				spuravoid = 1;
 		} else {
-			if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-				if (val == 54) {
-					spuravoid = 1;
-				}
-			} else {
-
-				if (pi->nphy_aband_spurwar_en &&
-				    ((val == 38) || (val == 102)
-				     || (val == 118)))
-					spuravoid = 1;
-			}
+			if (pi->nphy_aband_spurwar_en &&
+			    ((val == 38) || (val == 102)
+			     || (val == 118)))
+				spuravoid = 1;
 		}
 
 		if (pi->phy_spuravoid == SPURAVOID_FORCEON)
@@ -19047,20 +21402,16 @@ wlc_phy_chanspec_nphy_setup(struct brcms_phy *pi, chanspec_t chanspec,
 	wlc_phy_spurwar_nphy(pi);
 }
 
-void wlc_phy_chanspec_set_nphy(struct brcms_phy *pi, chanspec_t chanspec)
+void wlc_phy_chanspec_set_nphy(struct brcms_phy *pi, u16 chanspec)
 {
 	int freq;
-	struct chan_info_nphy_radio2057 *t0 = NULL;
-	struct chan_info_nphy_radio205x *t1 = NULL;
-	struct chan_info_nphy_radio2057_rev5 *t2 = NULL;
-	struct chan_info_nphy_2055 *t3 = NULL;
-
-	if (NORADIO_ENAB(pi->pubpi)) {
-		return;
-	}
+	const struct chan_info_nphy_radio2057 *t0 = NULL;
+	const struct chan_info_nphy_radio205x *t1 = NULL;
+	const struct chan_info_nphy_radio2057_rev5 *t2 = NULL;
+	const struct chan_info_nphy_2055 *t3 = NULL;
 
 	if (!wlc_phy_chan2freq_nphy
-	    (pi, CHSPEC_CHANNEL(chanspec), &freq, &t0, &t1, &t2, &t3))
+		    (pi, CHSPEC_CHANNEL(chanspec), &freq, &t0, &t1, &t2, &t3))
 		return;
 
 	wlc_phy_chanspec_radio_set((struct brcms_phy_pub *) pi, chanspec);
@@ -19071,15 +21422,13 @@ void wlc_phy_chanspec_set_nphy(struct brcms_phy *pi, chanspec_t chanspec)
 	if (CHSPEC_IS40(chanspec)) {
 		if (CHSPEC_SB_UPPER(chanspec)) {
 			or_phy_reg(pi, 0xa0, BPHY_BAND_SEL_UP20);
-			if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			if (NREV_GE(pi->pubpi.phy_rev, 7))
 				or_phy_reg(pi, 0x310, PRIM_SEL_UP20);
-			}
 		} else {
 			and_phy_reg(pi, 0xa0, ~BPHY_BAND_SEL_UP20);
-			if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			if (NREV_GE(pi->pubpi.phy_rev, 7))
 				and_phy_reg(pi, 0x310,
 					    (~PRIM_SEL_UP20 & 0xffff));
-			}
 		}
 	}
 
@@ -19124,358 +21473,10 @@ void wlc_phy_chanspec_set_nphy(struct brcms_phy *pi, chanspec_t chanspec)
 
 		wlc_phy_chanspec_radio2055_setup(pi, t3);
 		wlc_phy_chanspec_nphy_setup(pi, chanspec,
-					    (const struct nphy_sfo_cfg *)&(t3->
-								      PHY_BW1a));
-	}
-
-}
-
-static void wlc_phy_savecal_nphy(struct brcms_phy *pi)
-{
-	void *tbl_ptr;
-	int coreNum;
-	u16 *txcal_radio_regs = NULL;
-
-	if (pi->phyhang_avoid)
-		wlc_phy_stay_in_carriersearch_nphy(pi, true);
-
-	if (CHSPEC_IS2G(pi->radio_chanspec)) {
-
-		wlc_phy_rx_iq_coeffs_nphy(pi, 0,
-					  &pi->calibration_cache.
-					  rxcal_coeffs_2G);
-
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			txcal_radio_regs =
-			    pi->calibration_cache.txcal_radio_regs_2G;
-		} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-
-			pi->calibration_cache.txcal_radio_regs_2G[0] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_FINE_I |
-					   RADIO_2056_TX0);
-			pi->calibration_cache.txcal_radio_regs_2G[1] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_FINE_Q |
-					   RADIO_2056_TX0);
-			pi->calibration_cache.txcal_radio_regs_2G[2] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_FINE_I |
-					   RADIO_2056_TX1);
-			pi->calibration_cache.txcal_radio_regs_2G[3] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_FINE_Q |
-					   RADIO_2056_TX1);
-
-			pi->calibration_cache.txcal_radio_regs_2G[4] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_COARSE_I |
-					   RADIO_2056_TX0);
-			pi->calibration_cache.txcal_radio_regs_2G[5] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_COARSE_Q |
-					   RADIO_2056_TX0);
-			pi->calibration_cache.txcal_radio_regs_2G[6] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_COARSE_I |
-					   RADIO_2056_TX1);
-			pi->calibration_cache.txcal_radio_regs_2G[7] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_COARSE_Q |
-					   RADIO_2056_TX1);
-		} else {
-			pi->calibration_cache.txcal_radio_regs_2G[0] =
-			    read_radio_reg(pi, RADIO_2055_CORE1_TX_VOS_CNCL);
-			pi->calibration_cache.txcal_radio_regs_2G[1] =
-			    read_radio_reg(pi, RADIO_2055_CORE2_TX_VOS_CNCL);
-			pi->calibration_cache.txcal_radio_regs_2G[2] =
-			    read_radio_reg(pi, RADIO_2055_CORE1_TX_BB_MXGM);
-			pi->calibration_cache.txcal_radio_regs_2G[3] =
-			    read_radio_reg(pi, RADIO_2055_CORE2_TX_BB_MXGM);
-		}
-
-		pi->nphy_iqcal_chanspec_2G = pi->radio_chanspec;
-		tbl_ptr = pi->calibration_cache.txcal_coeffs_2G;
-	} else {
-
-		wlc_phy_rx_iq_coeffs_nphy(pi, 0,
-					  &pi->calibration_cache.
-					  rxcal_coeffs_5G);
-
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			txcal_radio_regs =
-			    pi->calibration_cache.txcal_radio_regs_5G;
-		} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-
-			pi->calibration_cache.txcal_radio_regs_5G[0] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_FINE_I |
-					   RADIO_2056_TX0);
-			pi->calibration_cache.txcal_radio_regs_5G[1] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_FINE_Q |
-					   RADIO_2056_TX0);
-			pi->calibration_cache.txcal_radio_regs_5G[2] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_FINE_I |
-					   RADIO_2056_TX1);
-			pi->calibration_cache.txcal_radio_regs_5G[3] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_FINE_Q |
-					   RADIO_2056_TX1);
-
-			pi->calibration_cache.txcal_radio_regs_5G[4] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_COARSE_I |
-					   RADIO_2056_TX0);
-			pi->calibration_cache.txcal_radio_regs_5G[5] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_COARSE_Q |
-					   RADIO_2056_TX0);
-			pi->calibration_cache.txcal_radio_regs_5G[6] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_COARSE_I |
-					   RADIO_2056_TX1);
-			pi->calibration_cache.txcal_radio_regs_5G[7] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_LOFT_COARSE_Q |
-					   RADIO_2056_TX1);
-		} else {
-			pi->calibration_cache.txcal_radio_regs_5G[0] =
-			    read_radio_reg(pi, RADIO_2055_CORE1_TX_VOS_CNCL);
-			pi->calibration_cache.txcal_radio_regs_5G[1] =
-			    read_radio_reg(pi, RADIO_2055_CORE2_TX_VOS_CNCL);
-			pi->calibration_cache.txcal_radio_regs_5G[2] =
-			    read_radio_reg(pi, RADIO_2055_CORE1_TX_BB_MXGM);
-			pi->calibration_cache.txcal_radio_regs_5G[3] =
-			    read_radio_reg(pi, RADIO_2055_CORE2_TX_BB_MXGM);
-		}
-
-		pi->nphy_iqcal_chanspec_5G = pi->radio_chanspec;
-		tbl_ptr = pi->calibration_cache.txcal_coeffs_5G;
-	}
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-		for (coreNum = 0; coreNum <= 1; coreNum++) {
-
-			txcal_radio_regs[2 * coreNum] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
-					    LOFT_FINE_I);
-			txcal_radio_regs[2 * coreNum + 1] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
-					    LOFT_FINE_Q);
-
-			txcal_radio_regs[2 * coreNum + 4] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
-					    LOFT_COARSE_I);
-			txcal_radio_regs[2 * coreNum + 5] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
-					    LOFT_COARSE_Q);
-		}
-	}
-
-	wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_IQLOCAL, 8, 80, 16, tbl_ptr);
-
-	if (pi->phyhang_avoid)
-		wlc_phy_stay_in_carriersearch_nphy(pi, false);
-}
-
-static void wlc_phy_restorecal_nphy(struct brcms_phy *pi)
-{
-	u16 *loft_comp;
-	u16 txcal_coeffs_bphy[4];
-	u16 *tbl_ptr;
-	int coreNum;
-	u16 *txcal_radio_regs = NULL;
-
-	if (CHSPEC_IS2G(pi->radio_chanspec)) {
-		if (pi->nphy_iqcal_chanspec_2G == 0)
-			return;
-
-		tbl_ptr = pi->calibration_cache.txcal_coeffs_2G;
-		loft_comp = &pi->calibration_cache.txcal_coeffs_2G[5];
-	} else {
-		if (pi->nphy_iqcal_chanspec_5G == 0)
-			return;
-
-		tbl_ptr = pi->calibration_cache.txcal_coeffs_5G;
-		loft_comp = &pi->calibration_cache.txcal_coeffs_5G[5];
-	}
-
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_IQLOCAL, 4, 80, 16,
-				 (void *)tbl_ptr);
-
-	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-		txcal_coeffs_bphy[0] = tbl_ptr[0];
-		txcal_coeffs_bphy[1] = tbl_ptr[1];
-		txcal_coeffs_bphy[2] = tbl_ptr[2];
-		txcal_coeffs_bphy[3] = tbl_ptr[3];
-	} else {
-		txcal_coeffs_bphy[0] = 0;
-		txcal_coeffs_bphy[1] = 0;
-		txcal_coeffs_bphy[2] = 0;
-		txcal_coeffs_bphy[3] = 0;
-	}
-
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_IQLOCAL, 4, 88, 16,
-				 txcal_coeffs_bphy);
-
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_IQLOCAL, 2, 85, 16, loft_comp);
-
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_IQLOCAL, 2, 93, 16, loft_comp);
-
-	if (NREV_LT(pi->pubpi.phy_rev, 2))
-		wlc_phy_tx_iq_war_nphy(pi);
-
-	if (CHSPEC_IS2G(pi->radio_chanspec)) {
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			txcal_radio_regs =
-			    pi->calibration_cache.txcal_radio_regs_2G;
-		} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_FINE_I |
-					RADIO_2056_TX0,
-					pi->calibration_cache.
-					txcal_radio_regs_2G[0]);
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_FINE_Q |
-					RADIO_2056_TX0,
-					pi->calibration_cache.
-					txcal_radio_regs_2G[1]);
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_FINE_I |
-					RADIO_2056_TX1,
-					pi->calibration_cache.
-					txcal_radio_regs_2G[2]);
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_FINE_Q |
-					RADIO_2056_TX1,
-					pi->calibration_cache.
-					txcal_radio_regs_2G[3]);
-
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_COARSE_I |
-					RADIO_2056_TX0,
-					pi->calibration_cache.
-					txcal_radio_regs_2G[4]);
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_COARSE_Q |
-					RADIO_2056_TX0,
-					pi->calibration_cache.
-					txcal_radio_regs_2G[5]);
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_COARSE_I |
-					RADIO_2056_TX1,
-					pi->calibration_cache.
-					txcal_radio_regs_2G[6]);
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_COARSE_Q |
-					RADIO_2056_TX1,
-					pi->calibration_cache.
-					txcal_radio_regs_2G[7]);
-		} else {
-			write_radio_reg(pi, RADIO_2055_CORE1_TX_VOS_CNCL,
-					pi->calibration_cache.
-					txcal_radio_regs_2G[0]);
-			write_radio_reg(pi, RADIO_2055_CORE2_TX_VOS_CNCL,
-					pi->calibration_cache.
-					txcal_radio_regs_2G[1]);
-			write_radio_reg(pi, RADIO_2055_CORE1_TX_BB_MXGM,
-					pi->calibration_cache.
-					txcal_radio_regs_2G[2]);
-			write_radio_reg(pi, RADIO_2055_CORE2_TX_BB_MXGM,
-					pi->calibration_cache.
-					txcal_radio_regs_2G[3]);
-		}
-
-		wlc_phy_rx_iq_coeffs_nphy(pi, 1,
-					  &pi->calibration_cache.
-					  rxcal_coeffs_2G);
-	} else {
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			txcal_radio_regs =
-			    pi->calibration_cache.txcal_radio_regs_5G;
-		} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_FINE_I |
-					RADIO_2056_TX0,
-					pi->calibration_cache.
-					txcal_radio_regs_5G[0]);
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_FINE_Q |
-					RADIO_2056_TX0,
-					pi->calibration_cache.
-					txcal_radio_regs_5G[1]);
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_FINE_I |
-					RADIO_2056_TX1,
-					pi->calibration_cache.
-					txcal_radio_regs_5G[2]);
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_FINE_Q |
-					RADIO_2056_TX1,
-					pi->calibration_cache.
-					txcal_radio_regs_5G[3]);
-
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_COARSE_I |
-					RADIO_2056_TX0,
-					pi->calibration_cache.
-					txcal_radio_regs_5G[4]);
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_COARSE_Q |
-					RADIO_2056_TX0,
-					pi->calibration_cache.
-					txcal_radio_regs_5G[5]);
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_COARSE_I |
-					RADIO_2056_TX1,
-					pi->calibration_cache.
-					txcal_radio_regs_5G[6]);
-			write_radio_reg(pi,
-					RADIO_2056_TX_LOFT_COARSE_Q |
-					RADIO_2056_TX1,
-					pi->calibration_cache.
-					txcal_radio_regs_5G[7]);
-		} else {
-			write_radio_reg(pi, RADIO_2055_CORE1_TX_VOS_CNCL,
-					pi->calibration_cache.
-					txcal_radio_regs_5G[0]);
-			write_radio_reg(pi, RADIO_2055_CORE2_TX_VOS_CNCL,
-					pi->calibration_cache.
-					txcal_radio_regs_5G[1]);
-			write_radio_reg(pi, RADIO_2055_CORE1_TX_BB_MXGM,
-					pi->calibration_cache.
-					txcal_radio_regs_5G[2]);
-			write_radio_reg(pi, RADIO_2055_CORE2_TX_BB_MXGM,
-					pi->calibration_cache.
-					txcal_radio_regs_5G[3]);
-		}
-
-		wlc_phy_rx_iq_coeffs_nphy(pi, 1,
-					  &pi->calibration_cache.
-					  rxcal_coeffs_5G);
+					    (const struct nphy_sfo_cfg *)
+					     &(t3->PHY_BW1a));
 	}
 
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-		for (coreNum = 0; coreNum <= 1; coreNum++) {
-
-			WRITE_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
-					 LOFT_FINE_I,
-					 txcal_radio_regs[2 * coreNum]);
-			WRITE_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
-					 LOFT_FINE_Q,
-					 txcal_radio_regs[2 * coreNum + 1]);
-
-			WRITE_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
-					 LOFT_COARSE_I,
-					 txcal_radio_regs[2 * coreNum + 4]);
-			WRITE_RADIO_REG3(pi, RADIO_2057, TX, coreNum,
-					 LOFT_COARSE_Q,
-					 txcal_radio_regs[2 * coreNum + 5]);
-		}
-	}
 }
 
 void wlc_phy_antsel_init(struct brcms_phy_pub *ppi, bool lut_init)
@@ -19545,8 +21546,8 @@ u16 wlc_phy_classifier_nphy(struct brcms_phy *pi, u16 mask, u16 val)
 
 	if (D11REV_IS(pi->sh->corerev, 16)) {
 		suspended =
-		    (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC) ?
-		    false : true;
+			(R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC) ?
+			false : true;
 		if (!suspended)
 			wlapi_suspend_mac_and_wait(pi->sh->physhim);
 	}
@@ -19563,18 +21564,6 @@ u16 wlc_phy_classifier_nphy(struct brcms_phy *pi, u16 mask, u16 val)
 	return new_ctl;
 }
 
-static void wlc_phy_clip_det_nphy(struct brcms_phy *pi, u8 write, u16 *vals)
-{
-
-	if (write == 0) {
-		vals[0] = read_phy_reg(pi, 0x2c);
-		vals[1] = read_phy_reg(pi, 0x42);
-	} else {
-		write_phy_reg(pi, 0x2c, vals[0]);
-		write_phy_reg(pi, 0x42, vals[1]);
-	}
-}
-
 void wlc_phy_force_rfseq_nphy(struct brcms_phy *pi, u8 cmd)
 {
 	u16 trigger_mask, status_mask;
@@ -19620,577 +21609,6 @@ void wlc_phy_force_rfseq_nphy(struct brcms_phy *pi, u8 cmd)
 }
 
 static void
-wlc_phy_set_rfseq_nphy(struct brcms_phy *pi, u8 cmd, u8 *events, u8 *dlys,
-		       u8 len)
-{
-	u32 t1_offset, t2_offset;
-	u8 ctr;
-	u8 end_event =
-	    NREV_GE(pi->pubpi.phy_rev,
-		    3) ? NPHY_REV3_RFSEQ_CMD_END : NPHY_RFSEQ_CMD_END;
-	u8 end_dly = 1;
-
-	if (pi->phyhang_avoid)
-		wlc_phy_stay_in_carriersearch_nphy(pi, true);
-
-	t1_offset = cmd << 4;
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ, len, t1_offset, 8,
-				 events);
-	t2_offset = t1_offset + 0x080;
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ, len, t2_offset, 8,
-				 dlys);
-
-	for (ctr = len; ctr < 16; ctr++) {
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ, 1,
-					 t1_offset + ctr, 8, &end_event);
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ, 1,
-					 t2_offset + ctr, 8, &end_dly);
-	}
-
-	if (pi->phyhang_avoid)
-		wlc_phy_stay_in_carriersearch_nphy(pi, false);
-}
-
-static u16 wlc_phy_read_lpf_bw_ctl_nphy(struct brcms_phy *pi, u16 offset)
-{
-	u16 lpf_bw_ctl_val = 0;
-	u16 rx2tx_lpf_rc_lut_offset = 0;
-
-	if (offset == 0) {
-		if (CHSPEC_IS40(pi->radio_chanspec)) {
-			rx2tx_lpf_rc_lut_offset = 0x159;
-		} else {
-			rx2tx_lpf_rc_lut_offset = 0x154;
-		}
-	} else {
-		rx2tx_lpf_rc_lut_offset = offset;
-	}
-	wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_RFSEQ, 1,
-				(u32) rx2tx_lpf_rc_lut_offset, 16,
-				&lpf_bw_ctl_val);
-
-	lpf_bw_ctl_val = lpf_bw_ctl_val & 0x7;
-
-	return lpf_bw_ctl_val;
-}
-
-static void
-wlc_phy_rfctrl_override_nphy_rev7(struct brcms_phy *pi, u16 field, u16 value,
-				  u8 core_mask, u8 off, u8 override_id)
-{
-	u8 core_num;
-	u16 addr = 0, en_addr = 0, val_addr = 0, en_mask = 0, val_mask = 0;
-	u8 val_shift = 0;
-
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-		en_mask = field;
-		for (core_num = 0; core_num < 2; core_num++) {
-			if (override_id == NPHY_REV7_RFCTRLOVERRIDE_ID0) {
-
-				switch (field) {
-				case (0x1 << 2):
-					en_addr = (core_num == 0) ? 0xe7 : 0xec;
-					val_addr = (core_num == 0) ? 0x7a :
-					    0x7d;
-					val_mask = (0x1 << 1);
-					val_shift = 1;
-					break;
-				case (0x1 << 3):
-					en_addr = (core_num == 0) ? 0xe7 : 0xec;
-					val_addr = (core_num == 0) ? 0x7a :
-					    0x7d;
-					val_mask = (0x1 << 2);
-					val_shift = 2;
-					break;
-				case (0x1 << 4):
-					en_addr = (core_num == 0) ? 0xe7 : 0xec;
-					val_addr = (core_num == 0) ? 0x7a :
-					    0x7d;
-					val_mask = (0x1 << 4);
-					val_shift = 4;
-					break;
-				case (0x1 << 5):
-					en_addr = (core_num == 0) ? 0xe7 : 0xec;
-					val_addr = (core_num == 0) ? 0x7a :
-					    0x7d;
-					val_mask = (0x1 << 5);
-					val_shift = 5;
-					break;
-				case (0x1 << 6):
-					en_addr = (core_num == 0) ? 0xe7 : 0xec;
-					val_addr = (core_num == 0) ? 0x7a :
-					    0x7d;
-					val_mask = (0x1 << 6);
-					val_shift = 6;
-					break;
-				case (0x1 << 7):
-					en_addr = (core_num == 0) ? 0xe7 : 0xec;
-					val_addr = (core_num == 0) ? 0x7a :
-					    0x7d;
-					val_mask = (0x1 << 7);
-					val_shift = 7;
-					break;
-				case (0x1 << 10):
-					en_addr = (core_num == 0) ? 0xe7 : 0xec;
-					val_addr = (core_num == 0) ? 0xf8 :
-					    0xfa;
-					val_mask = (0x7 << 4);
-					val_shift = 4;
-					break;
-				case (0x1 << 11):
-					en_addr = (core_num == 0) ? 0xe7 : 0xec;
-					val_addr = (core_num == 0) ? 0x7b :
-					    0x7e;
-					val_mask = (0xffff << 0);
-					val_shift = 0;
-					break;
-				case (0x1 << 12):
-					en_addr = (core_num == 0) ? 0xe7 : 0xec;
-					val_addr = (core_num == 0) ? 0x7c :
-					    0x7f;
-					val_mask = (0xffff << 0);
-					val_shift = 0;
-					break;
-				case (0x3 << 13):
-					en_addr = (core_num == 0) ? 0xe7 : 0xec;
-					val_addr = (core_num == 0) ? 0x348 :
-					    0x349;
-					val_mask = (0xff << 0);
-					val_shift = 0;
-					break;
-				case (0x1 << 13):
-					en_addr = (core_num == 0) ? 0xe7 : 0xec;
-					val_addr = (core_num == 0) ? 0x348 :
-					    0x349;
-					val_mask = (0xf << 0);
-					val_shift = 0;
-					break;
-				default:
-					addr = 0xffff;
-					break;
-				}
-			} else if (override_id == NPHY_REV7_RFCTRLOVERRIDE_ID1) {
-
-				switch (field) {
-				case (0x1 << 1):
-					en_addr = (core_num == 0) ? 0x342 :
-					    0x343;
-					val_addr = (core_num == 0) ? 0x340 :
-					    0x341;
-					val_mask = (0x1 << 1);
-					val_shift = 1;
-					break;
-				case (0x1 << 3):
-					en_addr = (core_num == 0) ? 0x342 :
-					    0x343;
-					val_addr = (core_num == 0) ? 0x340 :
-					    0x341;
-					val_mask = (0x1 << 3);
-					val_shift = 3;
-					break;
-				case (0x1 << 5):
-					en_addr = (core_num == 0) ? 0x342 :
-					    0x343;
-					val_addr = (core_num == 0) ? 0x340 :
-					    0x341;
-					val_mask = (0x1 << 5);
-					val_shift = 5;
-					break;
-				case (0x1 << 4):
-					en_addr = (core_num == 0) ? 0x342 :
-					    0x343;
-					val_addr = (core_num == 0) ? 0x340 :
-					    0x341;
-					val_mask = (0x1 << 4);
-					val_shift = 4;
-					break;
-				case (0x1 << 2):
-
-					en_addr = (core_num == 0) ? 0x342 :
-					    0x343;
-					val_addr = (core_num == 0) ? 0x340 :
-					    0x341;
-					val_mask = (0x1 << 2);
-					val_shift = 2;
-					break;
-				case (0x1 << 7):
-
-					en_addr = (core_num == 0) ? 0x342 :
-					    0x343;
-					val_addr = (core_num == 0) ? 0x340 :
-					    0x341;
-					val_mask = (0x7 << 8);
-					val_shift = 8;
-					break;
-				case (0x1 << 11):
-					en_addr = (core_num == 0) ? 0x342 :
-					    0x343;
-					val_addr = (core_num == 0) ? 0x340 :
-					    0x341;
-					val_mask = (0x1 << 14);
-					val_shift = 14;
-					break;
-				case (0x1 << 10):
-					en_addr = (core_num == 0) ? 0x342 :
-					    0x343;
-					val_addr = (core_num == 0) ? 0x340 :
-					    0x341;
-					val_mask = (0x1 << 13);
-					val_shift = 13;
-					break;
-				case (0x1 << 9):
-					en_addr = (core_num == 0) ? 0x342 :
-					    0x343;
-					val_addr = (core_num == 0) ? 0x340 :
-					    0x341;
-					val_mask = (0x1 << 12);
-					val_shift = 12;
-					break;
-				case (0x1 << 8):
-					en_addr = (core_num == 0) ? 0x342 :
-					    0x343;
-					val_addr = (core_num == 0) ? 0x340 :
-					    0x341;
-					val_mask = (0x1 << 11);
-					val_shift = 11;
-					break;
-				case (0x1 << 6):
-					en_addr = (core_num == 0) ? 0x342 :
-					    0x343;
-					val_addr = (core_num == 0) ? 0x340 :
-					    0x341;
-					val_mask = (0x1 << 6);
-					val_shift = 6;
-					break;
-				case (0x1 << 0):
-					en_addr = (core_num == 0) ? 0x342 :
-					    0x343;
-					val_addr = (core_num == 0) ? 0x340 :
-					    0x341;
-					val_mask = (0x1 << 0);
-					val_shift = 0;
-					break;
-				default:
-					addr = 0xffff;
-					break;
-				}
-			} else if (override_id == NPHY_REV7_RFCTRLOVERRIDE_ID2) {
-
-				switch (field) {
-				case (0x1 << 3):
-					en_addr = (core_num == 0) ? 0x346 :
-					    0x347;
-					val_addr = (core_num == 0) ? 0x344 :
-					    0x345;
-					val_mask = (0x1 << 3);
-					val_shift = 3;
-					break;
-				case (0x1 << 1):
-					en_addr = (core_num == 0) ? 0x346 :
-					    0x347;
-					val_addr = (core_num == 0) ? 0x344 :
-					    0x345;
-					val_mask = (0x1 << 1);
-					val_shift = 1;
-					break;
-				case (0x1 << 0):
-					en_addr = (core_num == 0) ? 0x346 :
-					    0x347;
-					val_addr = (core_num == 0) ? 0x344 :
-					    0x345;
-					val_mask = (0x1 << 0);
-					val_shift = 0;
-					break;
-				case (0x1 << 2):
-					en_addr = (core_num == 0) ? 0x346 :
-					    0x347;
-					val_addr = (core_num == 0) ? 0x344 :
-					    0x345;
-					val_mask = (0x1 << 2);
-					val_shift = 2;
-					break;
-				case (0x1 << 4):
-					en_addr = (core_num == 0) ? 0x346 :
-					    0x347;
-					val_addr = (core_num == 0) ? 0x344 :
-					    0x345;
-					val_mask = (0x1 << 4);
-					val_shift = 4;
-					break;
-				default:
-					addr = 0xffff;
-					break;
-				}
-			}
-
-			if (off) {
-				and_phy_reg(pi, en_addr, ~en_mask);
-				and_phy_reg(pi, val_addr, ~val_mask);
-			} else {
-
-				if ((core_mask == 0)
-				    || (core_mask & (1 << core_num))) {
-					or_phy_reg(pi, en_addr, en_mask);
-
-					if (addr != 0xffff) {
-						mod_phy_reg(pi, val_addr,
-							    val_mask,
-							    (value <<
-							     val_shift));
-					}
-				}
-			}
-		}
-	}
-}
-
-static void
-wlc_phy_rfctrl_override_nphy(struct brcms_phy *pi, u16 field, u16 value,
-			     u8 core_mask, u8 off)
-{
-	u8 core_num;
-	u16 addr = 0, mask = 0, en_addr = 0, val_addr = 0, en_mask =
-	    0, val_mask = 0;
-	u8 shift = 0, val_shift = 0;
-
-	if (NREV_GE(pi->pubpi.phy_rev, 3) && NREV_LT(pi->pubpi.phy_rev, 7)) {
-
-		en_mask = field;
-		for (core_num = 0; core_num < 2; core_num++) {
-
-			switch (field) {
-			case (0x1 << 1):
-				en_addr = (core_num == 0) ? 0xe7 : 0xec;
-				val_addr = (core_num == 0) ? 0x7a : 0x7d;
-				val_mask = (0x1 << 0);
-				val_shift = 0;
-				break;
-			case (0x1 << 2):
-				en_addr = (core_num == 0) ? 0xe7 : 0xec;
-				val_addr = (core_num == 0) ? 0x7a : 0x7d;
-				val_mask = (0x1 << 1);
-				val_shift = 1;
-				break;
-			case (0x1 << 3):
-				en_addr = (core_num == 0) ? 0xe7 : 0xec;
-				val_addr = (core_num == 0) ? 0x7a : 0x7d;
-				val_mask = (0x1 << 2);
-				val_shift = 2;
-				break;
-			case (0x1 << 4):
-				en_addr = (core_num == 0) ? 0xe7 : 0xec;
-				val_addr = (core_num == 0) ? 0x7a : 0x7d;
-				val_mask = (0x1 << 4);
-				val_shift = 4;
-				break;
-			case (0x1 << 5):
-				en_addr = (core_num == 0) ? 0xe7 : 0xec;
-				val_addr = (core_num == 0) ? 0x7a : 0x7d;
-				val_mask = (0x1 << 5);
-				val_shift = 5;
-				break;
-			case (0x1 << 6):
-				en_addr = (core_num == 0) ? 0xe7 : 0xec;
-				val_addr = (core_num == 0) ? 0x7a : 0x7d;
-				val_mask = (0x1 << 6);
-				val_shift = 6;
-				break;
-			case (0x1 << 7):
-				en_addr = (core_num == 0) ? 0xe7 : 0xec;
-				val_addr = (core_num == 0) ? 0x7a : 0x7d;
-				val_mask = (0x1 << 7);
-				val_shift = 7;
-				break;
-			case (0x1 << 8):
-				en_addr = (core_num == 0) ? 0xe7 : 0xec;
-				val_addr = (core_num == 0) ? 0x7a : 0x7d;
-				val_mask = (0x7 << 8);
-				val_shift = 8;
-				break;
-			case (0x1 << 11):
-				en_addr = (core_num == 0) ? 0xe7 : 0xec;
-				val_addr = (core_num == 0) ? 0x7a : 0x7d;
-				val_mask = (0x7 << 13);
-				val_shift = 13;
-				break;
-
-			case (0x1 << 9):
-				en_addr = (core_num == 0) ? 0xe7 : 0xec;
-				val_addr = (core_num == 0) ? 0xf8 : 0xfa;
-				val_mask = (0x7 << 0);
-				val_shift = 0;
-				break;
-
-			case (0x1 << 10):
-				en_addr = (core_num == 0) ? 0xe7 : 0xec;
-				val_addr = (core_num == 0) ? 0xf8 : 0xfa;
-				val_mask = (0x7 << 4);
-				val_shift = 4;
-				break;
-
-			case (0x1 << 12):
-				en_addr = (core_num == 0) ? 0xe7 : 0xec;
-				val_addr = (core_num == 0) ? 0x7b : 0x7e;
-				val_mask = (0xffff << 0);
-				val_shift = 0;
-				break;
-			case (0x1 << 13):
-				en_addr = (core_num == 0) ? 0xe7 : 0xec;
-				val_addr = (core_num == 0) ? 0x7c : 0x7f;
-				val_mask = (0xffff << 0);
-				val_shift = 0;
-				break;
-			case (0x1 << 14):
-				en_addr = (core_num == 0) ? 0xe7 : 0xec;
-				val_addr = (core_num == 0) ? 0xf9 : 0xfb;
-				val_mask = (0x3 << 6);
-				val_shift = 6;
-				break;
-			case (0x1 << 0):
-				en_addr = (core_num == 0) ? 0xe5 : 0xe6;
-				val_addr = (core_num == 0) ? 0xf9 : 0xfb;
-				val_mask = (0x1 << 15);
-				val_shift = 15;
-				break;
-			default:
-				addr = 0xffff;
-				break;
-			}
-
-			if (off) {
-				and_phy_reg(pi, en_addr, ~en_mask);
-				and_phy_reg(pi, val_addr, ~val_mask);
-			} else {
-
-				if ((core_mask == 0)
-				    || (core_mask & (1 << core_num))) {
-					or_phy_reg(pi, en_addr, en_mask);
-
-					if (addr != 0xffff) {
-						mod_phy_reg(pi, val_addr,
-							    val_mask,
-							    (value <<
-							     val_shift));
-					}
-				}
-			}
-		}
-	} else {
-
-		if (off) {
-			and_phy_reg(pi, 0xec, ~field);
-			value = 0x0;
-		} else {
-			or_phy_reg(pi, 0xec, field);
-		}
-
-		for (core_num = 0; core_num < 2; core_num++) {
-
-			switch (field) {
-			case (0x1 << 1):
-			case (0x1 << 9):
-			case (0x1 << 12):
-			case (0x1 << 13):
-			case (0x1 << 14):
-				addr = 0x78;
-
-				core_mask = 0x1;
-				break;
-			case (0x1 << 2):
-			case (0x1 << 3):
-			case (0x1 << 4):
-			case (0x1 << 5):
-			case (0x1 << 6):
-			case (0x1 << 7):
-			case (0x1 << 8):
-				addr = (core_num == 0) ? 0x7a : 0x7d;
-				break;
-			case (0x1 << 10):
-				addr = (core_num == 0) ? 0x7b : 0x7e;
-				break;
-			case (0x1 << 11):
-				addr = (core_num == 0) ? 0x7c : 0x7f;
-				break;
-			default:
-				addr = 0xffff;
-			}
-
-			switch (field) {
-			case (0x1 << 1):
-				mask = (0x7 << 3);
-				shift = 3;
-				break;
-			case (0x1 << 9):
-				mask = (0x1 << 2);
-				shift = 2;
-				break;
-			case (0x1 << 12):
-				mask = (0x1 << 8);
-				shift = 8;
-				break;
-			case (0x1 << 13):
-				mask = (0x1 << 9);
-				shift = 9;
-				break;
-			case (0x1 << 14):
-				mask = (0xf << 12);
-				shift = 12;
-				break;
-			case (0x1 << 2):
-				mask = (0x1 << 0);
-				shift = 0;
-				break;
-			case (0x1 << 3):
-				mask = (0x1 << 1);
-				shift = 1;
-				break;
-			case (0x1 << 4):
-				mask = (0x1 << 2);
-				shift = 2;
-				break;
-			case (0x1 << 5):
-				mask = (0x3 << 4);
-				shift = 4;
-				break;
-			case (0x1 << 6):
-				mask = (0x3 << 6);
-				shift = 6;
-				break;
-			case (0x1 << 7):
-				mask = (0x1 << 8);
-				shift = 8;
-				break;
-			case (0x1 << 8):
-				mask = (0x1 << 9);
-				shift = 9;
-				break;
-			case (0x1 << 10):
-				mask = 0x1fff;
-				shift = 0x0;
-				break;
-			case (0x1 << 11):
-				mask = 0x1fff;
-				shift = 0x0;
-				break;
-			default:
-				mask = 0x0;
-				shift = 0x0;
-				break;
-			}
-
-			if ((addr != 0xffff) && (core_mask & (1 << core_num))) {
-				mod_phy_reg(pi, addr, mask, (value << shift));
-			}
-		}
-
-		or_phy_reg(pi, 0xec, (0x1 << 0));
-		or_phy_reg(pi, 0x78, (0x1 << 0));
-		udelay(1);
-		and_phy_reg(pi, 0xec, ~(0x1 << 0));
-	}
-}
-
-static void
 wlc_phy_rfctrl_override_1tomany_nphy(struct brcms_phy *pi, u16 cmd, u16 value,
 				     u8 core_mask, u8 off)
 {
@@ -20201,76 +21619,93 @@ wlc_phy_rfctrl_override_1tomany_nphy(struct brcms_phy *pi, u16 cmd, u16 value,
 
 		switch (cmd) {
 		case NPHY_REV7_RfctrlOverride_cmd_rxrf_pu:
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 5),
-							  value, core_mask, off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 4), value,
-							  core_mask, off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 3), value,
-							  core_mask, off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 5),
+				value, core_mask, off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 4), value,
+				core_mask, off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 3), value,
+				core_mask, off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
 			break;
 		case NPHY_REV7_RfctrlOverride_cmd_rx_pu:
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 2),
-							  value, core_mask, off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 1), value,
-							  core_mask, off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 0), value,
-							  core_mask, off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 1), value,
-							  core_mask, off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID2);
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 11), 0,
-							  core_mask, off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 2),
+				value, core_mask, off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 1), value,
+				core_mask, off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 0), value,
+				core_mask, off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 1), value,
+				core_mask, off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID2);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 11), 0,
+				core_mask, off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
 			break;
 		case NPHY_REV7_RfctrlOverride_cmd_tx_pu:
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 2),
-							  value, core_mask, off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 1), value,
-							  core_mask, off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 0), value,
-							  core_mask, off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID2);
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 2), value,
-							  core_mask, off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID2);
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 11), 1,
-							  core_mask, off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 2),
+				value, core_mask, off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID0);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 1), value,
+				core_mask, off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 0), value,
+				core_mask, off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID2);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 2), value,
+				core_mask, off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID2);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 11), 1,
+				core_mask, off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
 			break;
 		case NPHY_REV7_RfctrlOverride_cmd_rxgain:
 			rfmxgain = value & 0x000ff;
 			lpfgain = value & 0x0ff00;
 			lpfgain = lpfgain >> 8;
 
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 11),
-							  rfmxgain, core_mask,
-							  off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x3 << 13),
-							  lpfgain, core_mask,
-							  off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 11),
+				rfmxgain, core_mask,
+				off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID0);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x3 << 13),
+				lpfgain, core_mask,
+				off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID0);
 			break;
 		case NPHY_REV7_RfctrlOverride_cmd_txgain:
 			tgain = value & 0x7fff;
 			lpfgain = value & 0x8000;
 			lpfgain = lpfgain >> 14;
 
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 12),
-							  tgain, core_mask, off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 13),
-							  lpfgain, core_mask,
-							  off,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 12),
+				tgain, core_mask, off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID0);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 13),
+				lpfgain, core_mask,
+				off,
+				NPHY_REV7_RFCTRLOVERRIDE_ID0);
 			break;
 		}
 	}
@@ -20283,137 +21718,164 @@ wlc_phy_scale_offset_rssi_nphy(struct brcms_phy *pi, u16 scale, s8 offset,
 	u16 valuetostuff;
 
 	offset = (offset > NPHY_RSSICAL_MAXREAD) ?
-	    NPHY_RSSICAL_MAXREAD : offset;
+		 NPHY_RSSICAL_MAXREAD : offset;
 	offset = (offset < (-NPHY_RSSICAL_MAXREAD - 1)) ?
-	    -NPHY_RSSICAL_MAXREAD - 1 : offset;
+		 -NPHY_RSSICAL_MAXREAD - 1 : offset;
 
 	valuetostuff = ((scale & 0x3f) << 8) | (offset & 0x3f);
 
 	if (((coresel == RADIO_MIMO_CORESEL_CORE1) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_NB)) {
+	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_NB))
 		write_phy_reg(pi, 0x1a6, valuetostuff);
-	}
+
 	if (((coresel == RADIO_MIMO_CORESEL_CORE1) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_NB)) {
+	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_NB))
 		write_phy_reg(pi, 0x1ac, valuetostuff);
-	}
+
 	if (((coresel == RADIO_MIMO_CORESEL_CORE2) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_NB)) {
+	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_NB))
 		write_phy_reg(pi, 0x1b2, valuetostuff);
-	}
+
 	if (((coresel == RADIO_MIMO_CORESEL_CORE2) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_NB)) {
+	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_NB))
 		write_phy_reg(pi, 0x1b8, valuetostuff);
-	}
 
 	if (((coresel == RADIO_MIMO_CORESEL_CORE1) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_W1)) {
+	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_W1))
 		write_phy_reg(pi, 0x1a4, valuetostuff);
-	}
+
 	if (((coresel == RADIO_MIMO_CORESEL_CORE1) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_W1)) {
+	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_W1))
 		write_phy_reg(pi, 0x1aa, valuetostuff);
-	}
+
 	if (((coresel == RADIO_MIMO_CORESEL_CORE2) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_W1)) {
+	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_W1))
 		write_phy_reg(pi, 0x1b0, valuetostuff);
-	}
+
 	if (((coresel == RADIO_MIMO_CORESEL_CORE2) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_W1)) {
+	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_W1))
 		write_phy_reg(pi, 0x1b6, valuetostuff);
-	}
 
 	if (((coresel == RADIO_MIMO_CORESEL_CORE1) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_W2)) {
+	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_W2))
 		write_phy_reg(pi, 0x1a5, valuetostuff);
-	}
 	if (((coresel == RADIO_MIMO_CORESEL_CORE1) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_W2)) {
+	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_W2))
 		write_phy_reg(pi, 0x1ab, valuetostuff);
-	}
+
 	if (((coresel == RADIO_MIMO_CORESEL_CORE2) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_W2)) {
+	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_W2))
 		write_phy_reg(pi, 0x1b1, valuetostuff);
-	}
+
 	if (((coresel == RADIO_MIMO_CORESEL_CORE2) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_W2)) {
+	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_W2))
 		write_phy_reg(pi, 0x1b7, valuetostuff);
-	}
 
 	if (((coresel == RADIO_MIMO_CORESEL_CORE1) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_TBD)) {
+	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_TBD))
 		write_phy_reg(pi, 0x1a7, valuetostuff);
-	}
 	if (((coresel == RADIO_MIMO_CORESEL_CORE1) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_TBD)) {
+	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_TBD))
 		write_phy_reg(pi, 0x1ad, valuetostuff);
-	}
 	if (((coresel == RADIO_MIMO_CORESEL_CORE2) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_TBD)) {
+	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_TBD))
 		write_phy_reg(pi, 0x1b3, valuetostuff);
-	}
 	if (((coresel == RADIO_MIMO_CORESEL_CORE2) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_TBD)) {
+	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_TBD))
 		write_phy_reg(pi, 0x1b9, valuetostuff);
-	}
 
 	if (((coresel == RADIO_MIMO_CORESEL_CORE1) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_IQ)) {
+	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_IQ))
 		write_phy_reg(pi, 0x1a8, valuetostuff);
-	}
+
 	if (((coresel == RADIO_MIMO_CORESEL_CORE1) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_IQ)) {
+	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_IQ))
 		write_phy_reg(pi, 0x1ae, valuetostuff);
-	}
+
 	if (((coresel == RADIO_MIMO_CORESEL_CORE2) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_IQ)) {
+	    (rail == NPHY_RAIL_I) && (rssi_type == NPHY_RSSI_SEL_IQ))
 		write_phy_reg(pi, 0x1b4, valuetostuff);
-	}
+
 	if (((coresel == RADIO_MIMO_CORESEL_CORE2) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_IQ)) {
+	    (rail == NPHY_RAIL_Q) && (rssi_type == NPHY_RSSI_SEL_IQ))
 		write_phy_reg(pi, 0x1ba, valuetostuff);
-	}
 
 	if (((coresel == RADIO_MIMO_CORESEL_CORE1) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rssi_type == NPHY_RSSI_SEL_TSSI_2G)) {
+	    (rssi_type == NPHY_RSSI_SEL_TSSI_2G))
 		write_phy_reg(pi, 0x1a9, valuetostuff);
-	}
 	if (((coresel == RADIO_MIMO_CORESEL_CORE2) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rssi_type == NPHY_RSSI_SEL_TSSI_2G)) {
+	    (rssi_type == NPHY_RSSI_SEL_TSSI_2G))
 		write_phy_reg(pi, 0x1b5, valuetostuff);
-	}
 
 	if (((coresel == RADIO_MIMO_CORESEL_CORE1) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rssi_type == NPHY_RSSI_SEL_TSSI_5G)) {
+	    (rssi_type == NPHY_RSSI_SEL_TSSI_5G))
 		write_phy_reg(pi, 0x1af, valuetostuff);
-	}
+
 	if (((coresel == RADIO_MIMO_CORESEL_CORE2) ||
 	     (coresel == RADIO_MIMO_CORESEL_ALLRX)) &&
-	    (rssi_type == NPHY_RSSI_SEL_TSSI_5G)) {
+	    (rssi_type == NPHY_RSSI_SEL_TSSI_5G))
 		write_phy_reg(pi, 0x1bb, valuetostuff);
+}
+
+static void brcms_phy_wr_tx_mux(struct brcms_phy *pi, u8 core)
+{
+	if (PHY_IPA(pi)) {
+		if (NREV_GE(pi->pubpi.phy_rev, 7))
+			write_radio_reg(pi,
+					((core == PHY_CORE_0) ?
+					 RADIO_2057_TX0_TX_SSI_MUX :
+					 RADIO_2057_TX1_TX_SSI_MUX),
+					(CHSPEC_IS5G(pi->radio_chanspec) ?
+					0xc : 0xe));
+		else
+			write_radio_reg(pi,
+					RADIO_2056_TX_TX_SSI_MUX |
+					((core == PHY_CORE_0) ?
+					 RADIO_2056_TX0 : RADIO_2056_TX1),
+					(CHSPEC_IS5G(pi->radio_chanspec) ?
+					0xc : 0xe));
+	} else {
+		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			write_radio_reg(pi,
+					((core == PHY_CORE_0) ?
+					 RADIO_2057_TX0_TX_SSI_MUX :
+					 RADIO_2057_TX1_TX_SSI_MUX),
+					0x11);
+
+			if (pi->pubpi.radioid == BCM2057_ID)
+				write_radio_reg(pi,
+						RADIO_2057_IQTEST_SEL_PU, 0x1);
+
+		} else {
+			write_radio_reg(pi,
+					RADIO_2056_TX_TX_SSI_MUX |
+					((core == PHY_CORE_0) ?
+					 RADIO_2056_TX0 : RADIO_2056_TX1),
+					0x11);
+		}
 	}
 }
 
@@ -20440,7 +21902,7 @@ void wlc_phy_rssisel_nphy(struct brcms_phy *pi, u8 core_code, u8 rssi_type)
 			mod_phy_reg(pi, 0xe6, (0x1 << 5), 0);
 
 			mask = (0x1 << 2) |
-			    (0x1 << 3) | (0x1 << 4) | (0x1 << 5);
+			       (0x1 << 3) | (0x1 << 4) | (0x1 << 5);
 			mod_phy_reg(pi, 0xf9, mask, 0);
 			mod_phy_reg(pi, 0xfb, mask, 0);
 
@@ -20459,23 +21921,22 @@ void wlc_phy_rssisel_nphy(struct brcms_phy *pi, u8 core_code, u8 rssi_type)
 				if (rssi_type == NPHY_RSSI_SEL_W1 ||
 				    rssi_type == NPHY_RSSI_SEL_W2 ||
 				    rssi_type == NPHY_RSSI_SEL_NB) {
-
 					mod_phy_reg(pi,
 						    (core ==
 						     PHY_CORE_0) ? 0xa6 : 0xa7,
 						    (0x3 << 8), 0);
 
 					mask = (0x1 << 2) |
-					    (0x1 << 3) |
-					    (0x1 << 4) | (0x1 << 5);
+					       (0x1 << 3) |
+					       (0x1 << 4) | (0x1 << 5);
 					mod_phy_reg(pi,
 						    (core ==
 						     PHY_CORE_0) ? 0xf9 : 0xfb,
 						    mask, 0);
 
 					if (rssi_type == NPHY_RSSI_SEL_W1) {
-						if (CHSPEC_IS5G
-						    (pi->radio_chanspec)) {
+						if (CHSPEC_IS5G(
+							  pi->radio_chanspec)) {
 							mask = (0x1 << 2);
 							val = 1 << 2;
 						} else {
@@ -20501,7 +21962,6 @@ void wlc_phy_rssisel_nphy(struct brcms_phy *pi, u8 core_code, u8 rssi_type)
 						    0xe5 : 0xe6, mask, val);
 				} else {
 					if (rssi_type == NPHY_RSSI_SEL_TBD) {
-
 						mask = (0x3 << 8);
 						val = 1 << 8;
 						mod_phy_reg(pi,
@@ -20516,7 +21976,6 @@ void wlc_phy_rssisel_nphy(struct brcms_phy *pi, u8 core_code, u8 rssi_type)
 							    : 0xa7, mask, val);
 					} else if (rssi_type ==
 						   NPHY_RSSI_SEL_IQ) {
-
 						mask = (0x3 << 8);
 						val = 2 << 8;
 						mod_phy_reg(pi,
@@ -20530,7 +21989,6 @@ void wlc_phy_rssisel_nphy(struct brcms_phy *pi, u8 core_code, u8 rssi_type)
 							     PHY_CORE_0) ? 0xa6
 							    : 0xa7, mask, val);
 					} else {
-
 						mask = (0x3 << 8);
 						val = 3 << 8;
 						mod_phy_reg(pi,
@@ -20543,86 +22001,13 @@ void wlc_phy_rssisel_nphy(struct brcms_phy *pi, u8 core_code, u8 rssi_type)
 							    (core ==
 							     PHY_CORE_0) ? 0xa6
 							    : 0xa7, mask, val);
-
-						if (PHY_IPA(pi)) {
-							if (NREV_GE
-							    (pi->pubpi.phy_rev,
-							     7)) {
-
-								write_radio_reg
-								    (pi,
-								     ((core ==
-								       PHY_CORE_0)
-								      ?
-								      RADIO_2057_TX0_TX_SSI_MUX
-								      :
-								      RADIO_2057_TX1_TX_SSI_MUX),
-								     (CHSPEC_IS5G
-								      (pi->
-								       radio_chanspec)
-								      ? 0xc :
-								      0xe));
-							} else {
-								write_radio_reg
-								    (pi,
-								     RADIO_2056_TX_TX_SSI_MUX
-								     |
-								     ((core ==
-								       PHY_CORE_0)
-								      ?
-								      RADIO_2056_TX0
-								      :
-								      RADIO_2056_TX1),
-								     (CHSPEC_IS5G
-								      (pi->
-								       radio_chanspec)
-								      ? 0xc :
-								      0xe));
-							}
-						} else {
-
-							if (NREV_GE
-							    (pi->pubpi.phy_rev,
-							     7)) {
-								write_radio_reg
-								    (pi,
-								     ((core ==
-								       PHY_CORE_0)
-								      ?
-								      RADIO_2057_TX0_TX_SSI_MUX
-								      :
-								      RADIO_2057_TX1_TX_SSI_MUX),
-								     0x11);
-
-								if (pi->pubpi.
-								    radioid ==
-								    BCM2057_ID)
-									write_radio_reg
-									    (pi,
-									     RADIO_2057_IQTEST_SEL_PU,
-									     0x1);
-
-							} else {
-								write_radio_reg
-								    (pi,
-								     RADIO_2056_TX_TX_SSI_MUX
-								     |
-								     ((core ==
-								       PHY_CORE_0)
-								      ?
-								      RADIO_2056_TX0
-								      :
-								      RADIO_2056_TX1),
-								     0x11);
-							}
-						}
-
+						brcms_phy_wr_tx_mux(pi, core);
 						afectrlovr_rssi_val = 1 << 9;
 						mod_phy_reg(pi,
-							    (core ==
-							     PHY_CORE_0) ? 0x8f
-							    : 0xa5, (0x1 << 9),
-							    afectrlovr_rssi_val);
+							   (core ==
+							    PHY_CORE_0) ? 0x8f
+							   : 0xa5, (0x1 << 9),
+							   afectrlovr_rssi_val);
 					}
 				}
 			}
@@ -20631,19 +22016,15 @@ void wlc_phy_rssisel_nphy(struct brcms_phy *pi, u8 core_code, u8 rssi_type)
 
 		if ((rssi_type == NPHY_RSSI_SEL_W1) ||
 		    (rssi_type == NPHY_RSSI_SEL_W2) ||
-		    (rssi_type == NPHY_RSSI_SEL_NB)) {
-
+		    (rssi_type == NPHY_RSSI_SEL_NB))
 			val = 0x0;
-		} else if (rssi_type == NPHY_RSSI_SEL_TBD) {
-
+		else if (rssi_type == NPHY_RSSI_SEL_TBD)
 			val = 0x1;
-		} else if (rssi_type == NPHY_RSSI_SEL_IQ) {
-
+		else if (rssi_type == NPHY_RSSI_SEL_IQ)
 			val = 0x2;
-		} else {
-
+		else
 			val = 0x3;
-		}
+
 		mask = ((0x3 << 12) | (0x3 << 14));
 		val = (val << 12) | (val << 14);
 		mod_phy_reg(pi, 0xa6, mask, val);
@@ -20652,15 +22033,13 @@ void wlc_phy_rssisel_nphy(struct brcms_phy *pi, u8 core_code, u8 rssi_type)
 		if ((rssi_type == NPHY_RSSI_SEL_W1) ||
 		    (rssi_type == NPHY_RSSI_SEL_W2) ||
 		    (rssi_type == NPHY_RSSI_SEL_NB)) {
-			if (rssi_type == NPHY_RSSI_SEL_W1) {
+			if (rssi_type == NPHY_RSSI_SEL_W1)
 				val = 0x1;
-			}
-			if (rssi_type == NPHY_RSSI_SEL_W2) {
+			if (rssi_type == NPHY_RSSI_SEL_W2)
 				val = 0x2;
-			}
-			if (rssi_type == NPHY_RSSI_SEL_NB) {
+			if (rssi_type == NPHY_RSSI_SEL_NB)
 				val = 0x3;
-			}
+
 			mask = (0x3 << 4);
 			val = (val << 4);
 			mod_phy_reg(pi, 0x7a, mask, val);
@@ -20698,16 +22077,16 @@ void wlc_phy_rssisel_nphy(struct brcms_phy *pi, u8 core_code, u8 rssi_type)
 		    (rssi_type == NPHY_RSSI_SEL_NB)) {
 			rfctrlcmd_mask = ((0x1 << 8) | (0x7 << 3));
 			rfctrlcmd_val = (rfctrlcmd_rxen_val << 8) |
-			    (rfctrlcmd_coresel_val << 3);
+					(rfctrlcmd_coresel_val << 3);
 
 			rfctrlovr_mask = ((0x1 << 5) |
 					  (0x1 << 12) |
 					  (0x1 << 1) | (0x1 << 0));
 			rfctrlovr_val = (rfctrlovr_rssi_val <<
 					 5) |
-			    (rfctrlovr_rxen_val << 12) |
-			    (rfctrlovr_coresel_val << 1) |
-			    (rfctrlovr_trigger_val << 0);
+					(rfctrlovr_rxen_val << 12) |
+					(rfctrlovr_coresel_val << 1) |
+					(rfctrlovr_trigger_val << 0);
 
 			mod_phy_reg(pi, 0x78, rfctrlcmd_mask, rfctrlcmd_val);
 			mod_phy_reg(pi, 0xec, rfctrlovr_mask, rfctrlovr_val);
@@ -20762,13 +22141,11 @@ wlc_phy_poll_rssi_nphy(struct brcms_phy *pi, u8 rssi_type, s32 *rssi_buf,
 	wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_ALLRX, rssi_type);
 
 	gpiosel_orig = read_phy_reg(pi, 0xca);
-	if (NREV_LT(pi->pubpi.phy_rev, 2)) {
+	if (NREV_LT(pi->pubpi.phy_rev, 2))
 		write_phy_reg(pi, 0xca, 5);
-	}
 
-	for (ctr = 0; ctr < 4; ctr++) {
+	for (ctr = 0; ctr < 4; ctr++)
 		rssi_buf[ctr] = 0;
-	}
 
 	for (samp = 0; samp < nsamps; samp++) {
 		if (NREV_LT(pi->pubpi.phy_rev, 2)) {
@@ -20785,9 +22162,8 @@ wlc_phy_poll_rssi_nphy(struct brcms_phy *pi, u8 rssi_type, s32 *rssi_buf,
 		tmp_buf[ctr++] = ((s8) ((rssi1 & 0x3f) << 2)) >> 2;
 		tmp_buf[ctr++] = ((s8) (((rssi1 >> 8) & 0x3f) << 2)) >> 2;
 
-		for (ctr = 0; ctr < 4; ctr++) {
+		for (ctr = 0; ctr < 4; ctr++)
 			rssi_buf[ctr] += tmp_buf[ctr];
-		}
 
 	}
 
@@ -20796,9 +22172,8 @@ wlc_phy_poll_rssi_nphy(struct brcms_phy *pi, u8 rssi_type, s32 *rssi_buf,
 	rssi_out_val |= (rssi_buf[1] & 0xff) << 16;
 	rssi_out_val |= (rssi_buf[0] & 0xff) << 24;
 
-	if (NREV_LT(pi->pubpi.phy_rev, 2)) {
+	if (NREV_LT(pi->pubpi.phy_rev, 2))
 		write_phy_reg(pi, 0xca, gpiosel_orig);
-	}
 
 	write_phy_reg(pi, 0xa6, afectrlCore1_save);
 	write_phy_reg(pi, 0xa7, afectrlCore2_save);
@@ -20848,7 +22223,7 @@ s16 wlc_phy_tempsense_nphy(struct brcms_phy *pi)
 		u16 tempsense_Rcal;
 
 		syn_tempprocsense_save =
-		    read_radio_reg(pi, RADIO_2057_TEMPSENSE_CONFIG);
+			read_radio_reg(pi, RADIO_2057_TEMPSENSE_CONFIG);
 
 		afectrlCore1_save = read_phy_reg(pi, 0xa6);
 		afectrlCore2_save = read_phy_reg(pi, 0xa7);
@@ -20982,7 +22357,7 @@ s16 wlc_phy_tempsense_nphy(struct brcms_phy *pi)
 
 	} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
 		syn_tempprocsense_save =
-		    read_radio_reg(pi, RADIO_2056_SYN_TEMPPROCSENSE);
+			read_radio_reg(pi, RADIO_2056_SYN_TEMPPROCSENSE);
 
 		afectrlCore1_save = read_phy_reg(pi, 0xa6);
 		afectrlCore2_save = read_phy_reg(pi, 0xa7);
@@ -20997,14 +22372,13 @@ s16 wlc_phy_tempsense_nphy(struct brcms_phy *pi)
 			write_radio_reg(pi, RADIO_2056_SYN_TEMPPROCSENSE, 0x05);
 
 		wlc_phy_poll_rssi_nphy(pi, NPHY_RSSI_SEL_IQ, radio_temp2, 1);
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+		if (NREV_GE(pi->pubpi.phy_rev, 7))
 			write_radio_reg(pi, RADIO_2057_TEMPSENSE_CONFIG, 0x01);
-		} else {
+		else
 			write_radio_reg(pi, RADIO_2056_SYN_TEMPPROCSENSE, 0x01);
-		}
 
 		radio_temp[0] =
-		    (126 * (radio_temp[1] + radio_temp2[1]) + 3987) / 64;
+			(126 * (radio_temp[1] + radio_temp2[1]) + 3987) / 64;
 
 		write_radio_reg(pi, RADIO_2056_SYN_TEMPPROCSENSE,
 				syn_tempprocsense_save);
@@ -21019,17 +22393,17 @@ s16 wlc_phy_tempsense_nphy(struct brcms_phy *pi)
 	} else {
 
 		pwrdet_rxtx_core1_save =
-		    read_radio_reg(pi, RADIO_2055_PWRDET_RXTX_CORE1);
+			read_radio_reg(pi, RADIO_2055_PWRDET_RXTX_CORE1);
 		pwrdet_rxtx_core2_save =
-		    read_radio_reg(pi, RADIO_2055_PWRDET_RXTX_CORE2);
+			read_radio_reg(pi, RADIO_2055_PWRDET_RXTX_CORE2);
 		core1_txrf_iqcal1_save =
-		    read_radio_reg(pi, RADIO_2055_CORE1_TXRF_IQCAL1);
+			read_radio_reg(pi, RADIO_2055_CORE1_TXRF_IQCAL1);
 		core1_txrf_iqcal2_save =
-		    read_radio_reg(pi, RADIO_2055_CORE1_TXRF_IQCAL2);
+			read_radio_reg(pi, RADIO_2055_CORE1_TXRF_IQCAL2);
 		core2_txrf_iqcal1_save =
-		    read_radio_reg(pi, RADIO_2055_CORE2_TXRF_IQCAL1);
+			read_radio_reg(pi, RADIO_2055_CORE2_TXRF_IQCAL1);
 		core2_txrf_iqcal2_save =
-		    read_radio_reg(pi, RADIO_2055_CORE2_TXRF_IQCAL2);
+			read_radio_reg(pi, RADIO_2055_CORE2_TXRF_IQCAL2);
 		pd_pll_ts_save = read_radio_reg(pi, RADIO_2055_PD_PLL_TS);
 
 		afectrlCore1_save = read_phy_reg(pi, 0xa6);
@@ -21060,11 +22434,12 @@ s16 wlc_phy_tempsense_nphy(struct brcms_phy *pi)
 		radio_temp[3] = (radio_temp[3] + radio_temp2[3]);
 
 		radio_temp[0] =
-		    (radio_temp[0] + radio_temp[1] + radio_temp[2] +
-		     radio_temp[3]);
+			(radio_temp[0] + radio_temp[1] + radio_temp[2] +
+			 radio_temp[3]);
 
 		radio_temp[0] =
-		    (radio_temp[0] + (8 * 32)) * (950 - 350) / 63 + (350 * 8);
+			(radio_temp[0] +
+			 (8 * 32)) * (950 - 350) / 63 + (350 * 8);
 
 		radio_temp[0] = (radio_temp[0] - (8 * 420)) / 38;
 
@@ -21126,521 +22501,24 @@ wlc_phy_set_rssi_2055_vcm(struct brcms_phy *pi, u8 rssi_type, u8 *vcm_buf)
 					      RADIO_2055_NBRSSI_VCM_Q_SHIFT);
 			}
 		} else {
-
-			if (core == PHY_CORE_0) {
+			if (core == PHY_CORE_0)
 				mod_radio_reg(pi,
 					      RADIO_2055_CORE1_RXBB_RSSI_CTRL5,
 					      RADIO_2055_WBRSSI_VCM_IQ_MASK,
 					      vcm_buf[2 *
 						      core] <<
 					      RADIO_2055_WBRSSI_VCM_IQ_SHIFT);
-			} else {
+			else
 				mod_radio_reg(pi,
 					      RADIO_2055_CORE2_RXBB_RSSI_CTRL5,
 					      RADIO_2055_WBRSSI_VCM_IQ_MASK,
 					      vcm_buf[2 *
 						      core] <<
 					      RADIO_2055_WBRSSI_VCM_IQ_SHIFT);
-			}
 		}
 	}
 }
 
-void wlc_phy_rssi_cal_nphy(struct brcms_phy *pi)
-{
-	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-
-		wlc_phy_rssi_cal_nphy_rev3(pi);
-	} else {
-		wlc_phy_rssi_cal_nphy_rev2(pi, NPHY_RSSI_SEL_NB);
-		wlc_phy_rssi_cal_nphy_rev2(pi, NPHY_RSSI_SEL_W1);
-		wlc_phy_rssi_cal_nphy_rev2(pi, NPHY_RSSI_SEL_W2);
-	}
-}
-
-static void wlc_phy_rssi_cal_nphy_rev2(struct brcms_phy *pi, u8 rssi_type)
-{
-	s32 target_code;
-	u16 classif_state;
-	u16 clip_state[2];
-	u16 rssi_ctrl_state[2], pd_state[2];
-	u16 rfctrlintc_state[2], rfpdcorerxtx_state[2];
-	u16 rfctrlintc_override_val;
-	u16 clip_off[] = { 0xffff, 0xffff };
-	u16 rf_pd_val, pd_mask, rssi_ctrl_mask;
-	u8 vcm, min_vcm, vcm_tmp[4];
-	u8 vcm_final[4] = { 0, 0, 0, 0 };
-	u8 result_idx, ctr;
-	s32 poll_results[4][4] = {
-		{0, 0, 0, 0},
-		{0, 0, 0, 0},
-		{0, 0, 0, 0},
-		{0, 0, 0, 0}
-	};
-	s32 poll_miniq[4][2] = {
-		{0, 0},
-		{0, 0},
-		{0, 0},
-		{0, 0}
-	};
-	s32 min_d, curr_d;
-	s32 fine_digital_offset[4];
-	s32 poll_results_min[4] = { 0, 0, 0, 0 };
-	s32 min_poll;
-
-	switch (rssi_type) {
-	case NPHY_RSSI_SEL_NB:
-		target_code = NPHY_RSSICAL_NB_TARGET;
-		break;
-	case NPHY_RSSI_SEL_W1:
-		target_code = NPHY_RSSICAL_W1_TARGET;
-		break;
-	case NPHY_RSSI_SEL_W2:
-		target_code = NPHY_RSSICAL_W2_TARGET;
-		break;
-	default:
-		return;
-		break;
-	}
-
-	classif_state = wlc_phy_classifier_nphy(pi, 0, 0);
-	wlc_phy_classifier_nphy(pi, (0x7 << 0), 4);
-	wlc_phy_clip_det_nphy(pi, 0, clip_state);
-	wlc_phy_clip_det_nphy(pi, 1, clip_off);
-
-	rf_pd_val = (rssi_type == NPHY_RSSI_SEL_NB) ? 0x6 : 0x4;
-	rfctrlintc_override_val =
-	    CHSPEC_IS5G(pi->radio_chanspec) ? 0x140 : 0x110;
-
-	rfctrlintc_state[0] = read_phy_reg(pi, 0x91);
-	rfpdcorerxtx_state[0] = read_radio_reg(pi, RADIO_2055_PD_CORE1_RXTX);
-	write_phy_reg(pi, 0x91, rfctrlintc_override_val);
-	write_radio_reg(pi, RADIO_2055_PD_CORE1_RXTX, rf_pd_val);
-
-	rfctrlintc_state[1] = read_phy_reg(pi, 0x92);
-	rfpdcorerxtx_state[1] = read_radio_reg(pi, RADIO_2055_PD_CORE2_RXTX);
-	write_phy_reg(pi, 0x92, rfctrlintc_override_val);
-	write_radio_reg(pi, RADIO_2055_PD_CORE2_RXTX, rf_pd_val);
-
-	pd_mask = RADIO_2055_NBRSSI_PD | RADIO_2055_WBRSSI_G1_PD |
-	    RADIO_2055_WBRSSI_G2_PD;
-	pd_state[0] =
-	    read_radio_reg(pi, RADIO_2055_PD_CORE1_RSSI_MISC) & pd_mask;
-	pd_state[1] =
-	    read_radio_reg(pi, RADIO_2055_PD_CORE2_RSSI_MISC) & pd_mask;
-	mod_radio_reg(pi, RADIO_2055_PD_CORE1_RSSI_MISC, pd_mask, 0);
-	mod_radio_reg(pi, RADIO_2055_PD_CORE2_RSSI_MISC, pd_mask, 0);
-	rssi_ctrl_mask = RADIO_2055_NBRSSI_SEL | RADIO_2055_WBRSSI_G1_SEL |
-	    RADIO_2055_WBRSSI_G2_SEL;
-	rssi_ctrl_state[0] =
-	    read_radio_reg(pi, RADIO_2055_SP_RSSI_CORE1) & rssi_ctrl_mask;
-	rssi_ctrl_state[1] =
-	    read_radio_reg(pi, RADIO_2055_SP_RSSI_CORE2) & rssi_ctrl_mask;
-	wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_ALLRX, rssi_type);
-
-	wlc_phy_scale_offset_rssi_nphy(pi, 0x0, 0x0, RADIO_MIMO_CORESEL_ALLRX,
-				       NPHY_RAIL_I, rssi_type);
-	wlc_phy_scale_offset_rssi_nphy(pi, 0x0, 0x0, RADIO_MIMO_CORESEL_ALLRX,
-				       NPHY_RAIL_Q, rssi_type);
-
-	for (vcm = 0; vcm < 4; vcm++) {
-
-		vcm_tmp[0] = vcm_tmp[1] = vcm_tmp[2] = vcm_tmp[3] = vcm;
-		if (rssi_type != NPHY_RSSI_SEL_W2) {
-			wlc_phy_set_rssi_2055_vcm(pi, rssi_type, vcm_tmp);
-		}
-
-		wlc_phy_poll_rssi_nphy(pi, rssi_type, &poll_results[vcm][0],
-				       NPHY_RSSICAL_NPOLL);
-
-		if ((rssi_type == NPHY_RSSI_SEL_W1)
-		    || (rssi_type == NPHY_RSSI_SEL_W2)) {
-			for (ctr = 0; ctr < 2; ctr++) {
-				poll_miniq[vcm][ctr] =
-				    min(poll_results[vcm][ctr * 2 + 0],
-					poll_results[vcm][ctr * 2 + 1]);
-			}
-		}
-	}
-
-	for (result_idx = 0; result_idx < 4; result_idx++) {
-		min_d = NPHY_RSSICAL_MAXD;
-		min_vcm = 0;
-		min_poll = NPHY_RSSICAL_MAXREAD * NPHY_RSSICAL_NPOLL + 1;
-		for (vcm = 0; vcm < 4; vcm++) {
-			curr_d = ABS(((rssi_type == NPHY_RSSI_SEL_NB) ?
-				      poll_results[vcm][result_idx] :
-				      poll_miniq[vcm][result_idx / 2]) -
-				     (target_code * NPHY_RSSICAL_NPOLL));
-			if (curr_d < min_d) {
-				min_d = curr_d;
-				min_vcm = vcm;
-			}
-			if (poll_results[vcm][result_idx] < min_poll) {
-				min_poll = poll_results[vcm][result_idx];
-			}
-		}
-		vcm_final[result_idx] = min_vcm;
-		poll_results_min[result_idx] = min_poll;
-	}
-
-	if (rssi_type != NPHY_RSSI_SEL_W2) {
-		wlc_phy_set_rssi_2055_vcm(pi, rssi_type, vcm_final);
-	}
-
-	for (result_idx = 0; result_idx < 4; result_idx++) {
-		fine_digital_offset[result_idx] =
-		    (target_code * NPHY_RSSICAL_NPOLL) -
-		    poll_results[vcm_final[result_idx]][result_idx];
-		if (fine_digital_offset[result_idx] < 0) {
-			fine_digital_offset[result_idx] =
-			    ABS(fine_digital_offset[result_idx]);
-			fine_digital_offset[result_idx] +=
-			    (NPHY_RSSICAL_NPOLL / 2);
-			fine_digital_offset[result_idx] /= NPHY_RSSICAL_NPOLL;
-			fine_digital_offset[result_idx] =
-			    -fine_digital_offset[result_idx];
-		} else {
-			fine_digital_offset[result_idx] +=
-			    (NPHY_RSSICAL_NPOLL / 2);
-			fine_digital_offset[result_idx] /= NPHY_RSSICAL_NPOLL;
-		}
-
-		if (poll_results_min[result_idx] ==
-		    NPHY_RSSICAL_MAXREAD * NPHY_RSSICAL_NPOLL) {
-			fine_digital_offset[result_idx] =
-			    (target_code - NPHY_RSSICAL_MAXREAD - 1);
-		}
-
-		wlc_phy_scale_offset_rssi_nphy(pi, 0x0,
-					       (s8)
-					       fine_digital_offset[result_idx],
-					       (result_idx / 2 ==
-						0) ? RADIO_MIMO_CORESEL_CORE1 :
-					       RADIO_MIMO_CORESEL_CORE2,
-					       (result_idx % 2 ==
-						0) ? NPHY_RAIL_I : NPHY_RAIL_Q,
-					       rssi_type);
-	}
-
-	mod_radio_reg(pi, RADIO_2055_PD_CORE1_RSSI_MISC, pd_mask, pd_state[0]);
-	mod_radio_reg(pi, RADIO_2055_PD_CORE2_RSSI_MISC, pd_mask, pd_state[1]);
-	if (rssi_ctrl_state[0] == RADIO_2055_NBRSSI_SEL) {
-		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE1,
-				     NPHY_RSSI_SEL_NB);
-	} else if (rssi_ctrl_state[0] == RADIO_2055_WBRSSI_G1_SEL) {
-		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE1,
-				     NPHY_RSSI_SEL_W1);
-	} else if (rssi_ctrl_state[0] == RADIO_2055_WBRSSI_G2_SEL) {
-		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE1,
-				     NPHY_RSSI_SEL_W2);
-	} else {
-		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE1,
-				     NPHY_RSSI_SEL_W2);
-	}
-	if (rssi_ctrl_state[1] == RADIO_2055_NBRSSI_SEL) {
-		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE2,
-				     NPHY_RSSI_SEL_NB);
-	} else if (rssi_ctrl_state[1] == RADIO_2055_WBRSSI_G1_SEL) {
-		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE2,
-				     NPHY_RSSI_SEL_W1);
-	} else if (rssi_ctrl_state[1] == RADIO_2055_WBRSSI_G2_SEL) {
-		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE2,
-				     NPHY_RSSI_SEL_W2);
-	} else {
-		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE2,
-				     NPHY_RSSI_SEL_W2);
-	}
-
-	wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_OFF, rssi_type);
-
-	write_phy_reg(pi, 0x91, rfctrlintc_state[0]);
-	write_radio_reg(pi, RADIO_2055_PD_CORE1_RXTX, rfpdcorerxtx_state[0]);
-	write_phy_reg(pi, 0x92, rfctrlintc_state[1]);
-	write_radio_reg(pi, RADIO_2055_PD_CORE2_RXTX, rfpdcorerxtx_state[1]);
-
-	wlc_phy_classifier_nphy(pi, (0x7 << 0), classif_state);
-	wlc_phy_clip_det_nphy(pi, 1, clip_state);
-
-	wlc_phy_resetcca_nphy(pi);
-}
-
-int
-wlc_phy_rssi_compute_nphy(struct brcms_phy *pi, struct brcms_d11rxhdr *wlc_rxh)
-{
-	struct d11rxhdr *rxh = &wlc_rxh->rxhdr;
-	s16 rxpwr, rxpwr0, rxpwr1;
-	s16 phyRx0_l, phyRx2_l;
-
-	rxpwr = 0;
-	rxpwr0 = le16_to_cpu(rxh->PhyRxStatus_1) & PRXS1_nphy_PWR0_MASK;
-	rxpwr1 = (le16_to_cpu(rxh->PhyRxStatus_1) & PRXS1_nphy_PWR1_MASK) >> 8;
-
-	if (rxpwr0 > 127)
-		rxpwr0 -= 256;
-	if (rxpwr1 > 127)
-		rxpwr1 -= 256;
-
-	phyRx0_l = le16_to_cpu(rxh->PhyRxStatus_0) & 0x00ff;
-	phyRx2_l = le16_to_cpu(rxh->PhyRxStatus_2) & 0x00ff;
-	if (phyRx2_l > 127)
-		phyRx2_l -= 256;
-
-	if (((rxpwr0 == 16) || (rxpwr0 == 32))) {
-		rxpwr0 = rxpwr1;
-		rxpwr1 = phyRx2_l;
-	}
-
-	wlc_rxh->rxpwr[0] = (s8) rxpwr0;
-	wlc_rxh->rxpwr[1] = (s8) rxpwr1;
-	wlc_rxh->do_rssi_ma = 0;
-
-	if (pi->sh->rssi_mode == RSSI_ANT_MERGE_MAX)
-		rxpwr = (rxpwr0 > rxpwr1) ? rxpwr0 : rxpwr1;
-	else if (pi->sh->rssi_mode == RSSI_ANT_MERGE_MIN)
-		rxpwr = (rxpwr0 < rxpwr1) ? rxpwr0 : rxpwr1;
-	else if (pi->sh->rssi_mode == RSSI_ANT_MERGE_AVG)
-		rxpwr = (rxpwr0 + rxpwr1) >> 1;
-
-	return rxpwr;
-}
-
-static void
-wlc_phy_rfctrlintc_override_nphy(struct brcms_phy *pi, u8 field, u16 value,
-				 u8 core_code)
-{
-	u16 mask;
-	u16 val;
-	u8 core;
-
-	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-		for (core = 0; core < pi->pubpi.phy_corenum; core++) {
-			if (core_code == RADIO_MIMO_CORESEL_CORE1
-			    && core == PHY_CORE_1)
-				continue;
-			else if (core_code == RADIO_MIMO_CORESEL_CORE2
-				 && core == PHY_CORE_0)
-				continue;
-
-			if (NREV_LT(pi->pubpi.phy_rev, 7)) {
-
-				mask = (0x1 << 10);
-				val = 1 << 10;
-				mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x91 :
-					    0x92, mask, val);
-			}
-
-			if (field == NPHY_RfctrlIntc_override_OFF) {
-
-				write_phy_reg(pi, (core == PHY_CORE_0) ? 0x91 :
-					      0x92, 0);
-
-				wlc_phy_force_rfseq_nphy(pi,
-							 NPHY_RFSEQ_RESET2RX);
-			} else if (field == NPHY_RfctrlIntc_override_TRSW) {
-
-				if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-
-					mask = (0x1 << 6) | (0x1 << 7);
-
-					val = value << 6;
-					mod_phy_reg(pi,
-						    (core ==
-						     PHY_CORE_0) ? 0x91 : 0x92,
-						    mask, val);
-
-					or_phy_reg(pi,
-						   (core ==
-						    PHY_CORE_0) ? 0x91 : 0x92,
-						   (0x1 << 10));
-
-					and_phy_reg(pi, 0x2ff, (u16)
-						    ~(0x3 << 14));
-					or_phy_reg(pi, 0x2ff, (0x1 << 13));
-					or_phy_reg(pi, 0x2ff, (0x1 << 0));
-				} else {
-
-					mask = (0x1 << 6) |
-					    (0x1 << 7) |
-					    (0x1 << 8) | (0x1 << 9);
-					val = value << 6;
-					mod_phy_reg(pi,
-						    (core ==
-						     PHY_CORE_0) ? 0x91 : 0x92,
-						    mask, val);
-
-					mask = (0x1 << 0);
-					val = 1 << 0;
-					mod_phy_reg(pi,
-						    (core ==
-						     PHY_CORE_0) ? 0xe7 : 0xec,
-						    mask, val);
-
-					mask = (core == PHY_CORE_0) ? (0x1 << 0)
-					    : (0x1 << 1);
-					val = 1 << ((core == PHY_CORE_0) ?
-						    0 : 1);
-					mod_phy_reg(pi, 0x78, mask, val);
-
-					SPINWAIT(((read_phy_reg(pi, 0x78) & val)
-						  != 0), 10000);
-					if (WARN(read_phy_reg(pi, 0x78) & val,
-						"HW error: override failed"))
-						return;
-
-					mask = (0x1 << 0);
-					val = 0 << 0;
-					mod_phy_reg(pi,
-						    (core ==
-						     PHY_CORE_0) ? 0xe7 : 0xec,
-						    mask, val);
-				}
-			} else if (field == NPHY_RfctrlIntc_override_PA) {
-				if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-
-					mask = (0x1 << 4) | (0x1 << 5);
-
-					if (CHSPEC_IS5G(pi->radio_chanspec)) {
-						val = value << 5;
-					} else {
-						val = value << 4;
-					}
-
-					mod_phy_reg(pi,
-						    (core ==
-						     PHY_CORE_0) ? 0x91 : 0x92,
-						    mask, val);
-
-					or_phy_reg(pi,
-						   (core ==
-						    PHY_CORE_0) ? 0x91 : 0x92,
-						   (0x1 << 12));
-				} else {
-
-					if (CHSPEC_IS5G(pi->radio_chanspec)) {
-						mask = (0x1 << 5);
-						val = value << 5;
-					} else {
-						mask = (0x1 << 4);
-						val = value << 4;
-					}
-					mod_phy_reg(pi,
-						    (core ==
-						     PHY_CORE_0) ? 0x91 : 0x92,
-						    mask, val);
-				}
-			} else if (field == NPHY_RfctrlIntc_override_EXT_LNA_PU) {
-				if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-					if (CHSPEC_IS5G(pi->radio_chanspec)) {
-
-						mask = (0x1 << 0);
-						val = value << 0;
-						mod_phy_reg(pi,
-							    (core ==
-							     PHY_CORE_0) ? 0x91
-							    : 0x92, mask, val);
-
-						mask = (0x1 << 2);
-						mod_phy_reg(pi,
-							    (core ==
-							     PHY_CORE_0) ? 0x91
-							    : 0x92, mask, 0);
-					} else {
-
-						mask = (0x1 << 2);
-						val = value << 2;
-						mod_phy_reg(pi,
-							    (core ==
-							     PHY_CORE_0) ? 0x91
-							    : 0x92, mask, val);
-
-						mask = (0x1 << 0);
-						mod_phy_reg(pi,
-							    (core ==
-							     PHY_CORE_0) ? 0x91
-							    : 0x92, mask, 0);
-					}
-
-					mask = (0x1 << 11);
-					val = 1 << 11;
-					mod_phy_reg(pi,
-						    (core ==
-						     PHY_CORE_0) ? 0x91 : 0x92,
-						    mask, val);
-				} else {
-
-					if (CHSPEC_IS5G(pi->radio_chanspec)) {
-						mask = (0x1 << 0);
-						val = value << 0;
-					} else {
-						mask = (0x1 << 2);
-						val = value << 2;
-					}
-					mod_phy_reg(pi,
-						    (core ==
-						     PHY_CORE_0) ? 0x91 : 0x92,
-						    mask, val);
-				}
-			} else if (field ==
-				   NPHY_RfctrlIntc_override_EXT_LNA_GAIN) {
-				if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-					if (CHSPEC_IS5G(pi->radio_chanspec)) {
-
-						mask = (0x1 << 1);
-						val = value << 1;
-						mod_phy_reg(pi,
-							    (core ==
-							     PHY_CORE_0) ? 0x91
-							    : 0x92, mask, val);
-
-						mask = (0x1 << 3);
-						mod_phy_reg(pi,
-							    (core ==
-							     PHY_CORE_0) ? 0x91
-							    : 0x92, mask, 0);
-					} else {
-
-						mask = (0x1 << 3);
-						val = value << 3;
-						mod_phy_reg(pi,
-							    (core ==
-							     PHY_CORE_0) ? 0x91
-							    : 0x92, mask, val);
-
-						mask = (0x1 << 1);
-						mod_phy_reg(pi,
-							    (core ==
-							     PHY_CORE_0) ? 0x91
-							    : 0x92, mask, 0);
-					}
-
-					mask = (0x1 << 11);
-					val = 1 << 11;
-					mod_phy_reg(pi,
-						    (core ==
-						     PHY_CORE_0) ? 0x91 : 0x92,
-						    mask, val);
-				} else {
-
-					if (CHSPEC_IS5G(pi->radio_chanspec)) {
-						mask = (0x1 << 1);
-						val = value << 1;
-					} else {
-						mask = (0x1 << 3);
-						val = value << 3;
-					}
-					mod_phy_reg(pi,
-						    (core ==
-						     PHY_CORE_0) ? 0x91 : 0x92,
-						    mask, val);
-				}
-			}
-		}
-	} else {
-		return;
-	}
-}
-
 static void wlc_phy_rssi_cal_nphy_rev3(struct brcms_phy *pi)
 {
 	u16 classif_state;
@@ -21683,10 +22561,14 @@ static void wlc_phy_rssi_cal_nphy_rev3(struct brcms_phy *pi)
 	u16 NPHY_REV7_RfctrlMiscReg3_save, NPHY_REV7_RfctrlMiscReg4_save;
 	u16 NPHY_REV7_RfctrlMiscReg5_save, NPHY_REV7_RfctrlMiscReg6_save;
 
-	NPHY_REV7_RfctrlOverride3_save = NPHY_REV7_RfctrlOverride4_save =
-	    NPHY_REV7_RfctrlOverride5_save = NPHY_REV7_RfctrlOverride6_save =
-	    NPHY_REV7_RfctrlMiscReg3_save = NPHY_REV7_RfctrlMiscReg4_save =
-	    NPHY_REV7_RfctrlMiscReg5_save = NPHY_REV7_RfctrlMiscReg6_save = 0;
+	NPHY_REV7_RfctrlOverride3_save =
+		NPHY_REV7_RfctrlOverride4_save =
+		NPHY_REV7_RfctrlOverride5_save =
+		NPHY_REV7_RfctrlOverride6_save =
+		NPHY_REV7_RfctrlMiscReg3_save =
+		NPHY_REV7_RfctrlMiscReg4_save =
+		NPHY_REV7_RfctrlMiscReg5_save =
+		NPHY_REV7_RfctrlMiscReg6_save = 0;
 
 	classif_state = wlc_phy_classifier_nphy(pi, 0, 0);
 	wlc_phy_classifier_nphy(pi, (0x7 << 0), 4);
@@ -21726,21 +22608,21 @@ static void wlc_phy_rssi_cal_nphy_rev3(struct brcms_phy *pi)
 	wlc_phy_rfctrlintc_override_nphy(pi, NPHY_RfctrlIntc_override_TRSW, 1,
 					 RADIO_MIMO_CORESEL_ALLRXTX);
 
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-		wlc_phy_rfctrl_override_1tomany_nphy(pi,
-						     NPHY_REV7_RfctrlOverride_cmd_rxrf_pu,
-						     0, 0, 0);
-	} else {
+	if (NREV_GE(pi->pubpi.phy_rev, 7))
+		wlc_phy_rfctrl_override_1tomany_nphy(
+			pi,
+			NPHY_REV7_RfctrlOverride_cmd_rxrf_pu,
+			0, 0, 0);
+	else
 		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 0), 0, 0, 0);
-	}
 
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-		wlc_phy_rfctrl_override_1tomany_nphy(pi,
-						     NPHY_REV7_RfctrlOverride_cmd_rx_pu,
-						     1, 0, 0);
-	} else {
+	if (NREV_GE(pi->pubpi.phy_rev, 7))
+		wlc_phy_rfctrl_override_1tomany_nphy(
+			pi,
+			NPHY_REV7_RfctrlOverride_cmd_rx_pu,
+			1, 0, 0);
+	else
 		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 1), 1, 0, 0);
-	}
 
 	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
 		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 7),
@@ -21755,12 +22637,14 @@ static void wlc_phy_rssi_cal_nphy_rev3(struct brcms_phy *pi)
 
 	if (CHSPEC_IS5G(pi->radio_chanspec)) {
 		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 5),
-							  0, 0, 0,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 4), 1, 0,
-							  0,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 5),
+				0, 0, 0,
+				NPHY_REV7_RFCTRLOVERRIDE_ID0);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 4), 1, 0,
+				0,
+				NPHY_REV7_RFCTRLOVERRIDE_ID0);
 		} else {
 			wlc_phy_rfctrl_override_nphy(pi, (0x1 << 5), 0, 0, 0);
 			wlc_phy_rfctrl_override_nphy(pi, (0x1 << 4), 1, 0, 0);
@@ -21768,12 +22652,14 @@ static void wlc_phy_rssi_cal_nphy_rev3(struct brcms_phy *pi)
 
 	} else {
 		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 4),
-							  0, 0, 0,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 5), 1, 0,
-							  0,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 4),
+				0, 0, 0,
+				NPHY_REV7_RFCTRLOVERRIDE_ID0);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 5), 1, 0,
+				0,
+				NPHY_REV7_RFCTRLOVERRIDE_ID0);
 		} else {
 			wlc_phy_rfctrl_override_nphy(pi, (0x1 << 4), 0, 0, 0);
 			wlc_phy_rfctrl_override_nphy(pi, (0x1 << 5), 1, 0, 0);
@@ -21781,7 +22667,7 @@ static void wlc_phy_rssi_cal_nphy_rev3(struct brcms_phy *pi)
 	}
 
 	rxcore_state = wlc_phy_rxcore_getstate_nphy(
-						(struct brcms_phy_pub *) pi);
+		(struct brcms_phy_pub *) pi);
 
 	vcm_level_max = 8;
 
@@ -21804,21 +22690,18 @@ static void wlc_phy_rssi_cal_nphy_rev3(struct brcms_phy *pi)
 					       NPHY_RAIL_Q, NPHY_RSSI_SEL_NB);
 
 		for (vcm = 0; vcm < vcm_level_max; vcm++) {
-			if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-
+			if (NREV_GE(pi->pubpi.phy_rev, 7))
 				mod_radio_reg(pi, (core == PHY_CORE_0) ?
 					      RADIO_2057_NB_MASTER_CORE0 :
 					      RADIO_2057_NB_MASTER_CORE1,
 					      RADIO_2057_VCM_MASK, vcm);
-			} else {
-
+			else
 				mod_radio_reg(pi, RADIO_2056_RX_RSSI_MISC |
 					      ((core ==
 						PHY_CORE_0) ? RADIO_2056_RX0 :
 					       RADIO_2056_RX1),
 					      RADIO_2056_VCM_MASK,
 					      vcm << RADIO_2056_RSSI_VCM_SHIFT);
-			}
 
 			wlc_phy_poll_rssi_nphy(pi, NPHY_RSSI_SEL_NB,
 					       &poll_results[vcm][0],
@@ -21826,92 +22709,90 @@ static void wlc_phy_rssi_cal_nphy_rev3(struct brcms_phy *pi)
 		}
 
 		for (result_idx = 0; result_idx < 4; result_idx++) {
-			if ((core == result_idx / 2) && (result_idx % 2 == 0)) {
+			if ((core == result_idx / 2) &&
+			    (result_idx % 2 == 0)) {
 
 				min_d = NPHY_RSSICAL_MAXD;
 				min_vcm = 0;
 				min_poll =
-				    NPHY_RSSICAL_MAXREAD * NPHY_RSSICAL_NPOLL +
-				    1;
+					NPHY_RSSICAL_MAXREAD *
+					NPHY_RSSICAL_NPOLL + 1;
 				for (vcm = 0; vcm < vcm_level_max; vcm++) {
-					curr_d = poll_results[vcm][result_idx] *
-					    poll_results[vcm][result_idx] +
-					    poll_results[vcm][result_idx + 1] *
-					    poll_results[vcm][result_idx + 1];
+					curr_d =
+						poll_results[vcm][result_idx] *
+						poll_results[vcm][result_idx] +
+						poll_results[vcm][result_idx +
+								  1] *
+						poll_results[vcm][result_idx +
+								  1];
 					if (curr_d < min_d) {
 						min_d = curr_d;
 						min_vcm = vcm;
 					}
 					if (poll_results[vcm][result_idx] <
-					    min_poll) {
+					    min_poll)
 						min_poll =
-						    poll_results[vcm]
-						    [result_idx];
-					}
+							poll_results[vcm]
+							[result_idx];
 				}
 				vcm_final = min_vcm;
 				poll_results_min[result_idx] = min_poll;
 			}
 		}
 
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+		if (NREV_GE(pi->pubpi.phy_rev, 7))
 			mod_radio_reg(pi, (core == PHY_CORE_0) ?
 				      RADIO_2057_NB_MASTER_CORE0 :
 				      RADIO_2057_NB_MASTER_CORE1,
 				      RADIO_2057_VCM_MASK, vcm_final);
-		} else {
+		else
 			mod_radio_reg(pi, RADIO_2056_RX_RSSI_MISC |
 				      ((core ==
 					PHY_CORE_0) ? RADIO_2056_RX0 :
 				       RADIO_2056_RX1), RADIO_2056_VCM_MASK,
 				      vcm_final << RADIO_2056_RSSI_VCM_SHIFT);
-		}
 
 		for (result_idx = 0; result_idx < 4; result_idx++) {
 			if (core == result_idx / 2) {
 				fine_digital_offset[result_idx] =
-				    (NPHY_RSSICAL_NB_TARGET *
-				     NPHY_RSSICAL_NPOLL) -
-				    poll_results[vcm_final][result_idx];
+					(NPHY_RSSICAL_NB_TARGET *
+					 NPHY_RSSICAL_NPOLL) -
+					poll_results[vcm_final][result_idx];
 				if (fine_digital_offset[result_idx] < 0) {
 					fine_digital_offset[result_idx] =
-					    ABS(fine_digital_offset
-						[result_idx]);
+						abs(fine_digital_offset
+						    [result_idx]);
 					fine_digital_offset[result_idx] +=
-					    (NPHY_RSSICAL_NPOLL / 2);
+						(NPHY_RSSICAL_NPOLL / 2);
 					fine_digital_offset[result_idx] /=
-					    NPHY_RSSICAL_NPOLL;
+						NPHY_RSSICAL_NPOLL;
 					fine_digital_offset[result_idx] =
-					    -fine_digital_offset[result_idx];
+						-fine_digital_offset[
+								    result_idx];
 				} else {
 					fine_digital_offset[result_idx] +=
-					    (NPHY_RSSICAL_NPOLL / 2);
+						(NPHY_RSSICAL_NPOLL / 2);
 					fine_digital_offset[result_idx] /=
-					    NPHY_RSSICAL_NPOLL;
+						NPHY_RSSICAL_NPOLL;
 				}
 
 				if (poll_results_min[result_idx] ==
-				    NPHY_RSSICAL_MAXREAD * NPHY_RSSICAL_NPOLL) {
+				    NPHY_RSSICAL_MAXREAD * NPHY_RSSICAL_NPOLL)
 					fine_digital_offset[result_idx] =
-					    (NPHY_RSSICAL_NB_TARGET -
-					     NPHY_RSSICAL_MAXREAD - 1);
-				}
-
-				wlc_phy_scale_offset_rssi_nphy(pi, 0x0,
-							       (s8)
-							       fine_digital_offset
-							       [result_idx],
-							       (result_idx /
-								2 ==
-								0) ?
-							       RADIO_MIMO_CORESEL_CORE1
-							       :
-							       RADIO_MIMO_CORESEL_CORE2,
-							       (result_idx %
-								2 ==
-								0) ? NPHY_RAIL_I
-							       : NPHY_RAIL_Q,
-							       NPHY_RSSI_SEL_NB);
+						(NPHY_RSSICAL_NB_TARGET -
+						 NPHY_RSSICAL_MAXREAD - 1);
+
+				wlc_phy_scale_offset_rssi_nphy(
+					pi, 0x0,
+					(s8)
+					fine_digital_offset
+					[result_idx],
+					(result_idx / 2 == 0) ?
+					RADIO_MIMO_CORESEL_CORE1 :
+					RADIO_MIMO_CORESEL_CORE2,
+					(result_idx % 2 == 0) ?
+					NPHY_RAIL_I : NPHY_RAIL_Q,
+					NPHY_RSSI_SEL_NB);
 			}
 		}
 
@@ -21952,46 +22833,44 @@ static void wlc_phy_rssi_cal_nphy_rev3(struct brcms_phy *pi)
 			for (result_idx = 0; result_idx < 4; result_idx++) {
 				if (core == result_idx / 2) {
 					fine_digital_offset[result_idx] =
-					    (target_code * NPHY_RSSICAL_NPOLL) -
-					    poll_result_core[result_idx];
-					if (fine_digital_offset[result_idx] < 0) {
+						(target_code *
+						 NPHY_RSSICAL_NPOLL) -
+						poll_result_core[result_idx];
+					if (fine_digital_offset[result_idx] <
+					    0) {
 						fine_digital_offset[result_idx]
-						    =
-						    ABS(fine_digital_offset
-							[result_idx]);
+							= abs(
+							    fine_digital_offset
+							    [result_idx]);
 						fine_digital_offset[result_idx]
-						    += (NPHY_RSSICAL_NPOLL / 2);
+							+= (NPHY_RSSICAL_NPOLL
+							    / 2);
 						fine_digital_offset[result_idx]
-						    /= NPHY_RSSICAL_NPOLL;
+							/= NPHY_RSSICAL_NPOLL;
 						fine_digital_offset[result_idx]
-						    =
-						    -fine_digital_offset
-						    [result_idx];
+							= -fine_digital_offset
+								[result_idx];
 					} else {
 						fine_digital_offset[result_idx]
-						    += (NPHY_RSSICAL_NPOLL / 2);
+							+= (NPHY_RSSICAL_NPOLL
+							    / 2);
 						fine_digital_offset[result_idx]
-						    /= NPHY_RSSICAL_NPOLL;
+							/= NPHY_RSSICAL_NPOLL;
 					}
 
-					wlc_phy_scale_offset_rssi_nphy(pi, 0x0,
-								       (s8)
-								       fine_digital_offset
-								       [core *
-									2],
-								       (core ==
-									PHY_CORE_0)
-								       ?
-								       RADIO_MIMO_CORESEL_CORE1
-								       :
-								       RADIO_MIMO_CORESEL_CORE2,
-								       (result_idx
-									% 2 ==
-									0) ?
-								       NPHY_RAIL_I
-								       :
-								       NPHY_RAIL_Q,
-								       rssi_type);
+					wlc_phy_scale_offset_rssi_nphy(
+						pi, 0x0,
+						(s8)
+						fine_digital_offset
+						[core *
+						 2],
+						(core == PHY_CORE_0) ?
+						RADIO_MIMO_CORESEL_CORE1 :
+						RADIO_MIMO_CORESEL_CORE2,
+						(result_idx % 2 == 0) ?
+						NPHY_RAIL_I :
+						NPHY_RAIL_Q,
+						rssi_type);
 				}
 			}
 
@@ -22040,87 +22919,87 @@ static void wlc_phy_rssi_cal_nphy_rev3(struct brcms_phy *pi)
 	if (CHSPEC_IS2G(pi->radio_chanspec)) {
 		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
 			pi->rssical_cache.rssical_radio_regs_2G[0] =
-			    read_radio_reg(pi, RADIO_2057_NB_MASTER_CORE0);
+				read_radio_reg(pi, RADIO_2057_NB_MASTER_CORE0);
 			pi->rssical_cache.rssical_radio_regs_2G[1] =
-			    read_radio_reg(pi, RADIO_2057_NB_MASTER_CORE1);
+				read_radio_reg(pi, RADIO_2057_NB_MASTER_CORE1);
 		} else {
 			pi->rssical_cache.rssical_radio_regs_2G[0] =
-			    read_radio_reg(pi,
-					   RADIO_2056_RX_RSSI_MISC |
-					   RADIO_2056_RX0);
+				read_radio_reg(pi,
+					       RADIO_2056_RX_RSSI_MISC |
+					       RADIO_2056_RX0);
 			pi->rssical_cache.rssical_radio_regs_2G[1] =
-			    read_radio_reg(pi,
-					   RADIO_2056_RX_RSSI_MISC |
-					   RADIO_2056_RX1);
+				read_radio_reg(pi,
+					       RADIO_2056_RX_RSSI_MISC |
+					       RADIO_2056_RX1);
 		}
 
 		pi->rssical_cache.rssical_phyregs_2G[0] =
-		    read_phy_reg(pi, 0x1a6);
+			read_phy_reg(pi, 0x1a6);
 		pi->rssical_cache.rssical_phyregs_2G[1] =
-		    read_phy_reg(pi, 0x1ac);
+			read_phy_reg(pi, 0x1ac);
 		pi->rssical_cache.rssical_phyregs_2G[2] =
-		    read_phy_reg(pi, 0x1b2);
+			read_phy_reg(pi, 0x1b2);
 		pi->rssical_cache.rssical_phyregs_2G[3] =
-		    read_phy_reg(pi, 0x1b8);
+			read_phy_reg(pi, 0x1b8);
 		pi->rssical_cache.rssical_phyregs_2G[4] =
-		    read_phy_reg(pi, 0x1a4);
+			read_phy_reg(pi, 0x1a4);
 		pi->rssical_cache.rssical_phyregs_2G[5] =
-		    read_phy_reg(pi, 0x1aa);
+			read_phy_reg(pi, 0x1aa);
 		pi->rssical_cache.rssical_phyregs_2G[6] =
-		    read_phy_reg(pi, 0x1b0);
+			read_phy_reg(pi, 0x1b0);
 		pi->rssical_cache.rssical_phyregs_2G[7] =
-		    read_phy_reg(pi, 0x1b6);
+			read_phy_reg(pi, 0x1b6);
 		pi->rssical_cache.rssical_phyregs_2G[8] =
-		    read_phy_reg(pi, 0x1a5);
+			read_phy_reg(pi, 0x1a5);
 		pi->rssical_cache.rssical_phyregs_2G[9] =
-		    read_phy_reg(pi, 0x1ab);
+			read_phy_reg(pi, 0x1ab);
 		pi->rssical_cache.rssical_phyregs_2G[10] =
-		    read_phy_reg(pi, 0x1b1);
+			read_phy_reg(pi, 0x1b1);
 		pi->rssical_cache.rssical_phyregs_2G[11] =
-		    read_phy_reg(pi, 0x1b7);
+			read_phy_reg(pi, 0x1b7);
 
 		pi->nphy_rssical_chanspec_2G = pi->radio_chanspec;
 	} else {
 		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
 			pi->rssical_cache.rssical_radio_regs_5G[0] =
-			    read_radio_reg(pi, RADIO_2057_NB_MASTER_CORE0);
+				read_radio_reg(pi, RADIO_2057_NB_MASTER_CORE0);
 			pi->rssical_cache.rssical_radio_regs_5G[1] =
-			    read_radio_reg(pi, RADIO_2057_NB_MASTER_CORE1);
+				read_radio_reg(pi, RADIO_2057_NB_MASTER_CORE1);
 		} else {
 			pi->rssical_cache.rssical_radio_regs_5G[0] =
-			    read_radio_reg(pi,
-					   RADIO_2056_RX_RSSI_MISC |
-					   RADIO_2056_RX0);
+				read_radio_reg(pi,
+					       RADIO_2056_RX_RSSI_MISC |
+					       RADIO_2056_RX0);
 			pi->rssical_cache.rssical_radio_regs_5G[1] =
-			    read_radio_reg(pi,
-					   RADIO_2056_RX_RSSI_MISC |
-					   RADIO_2056_RX1);
+				read_radio_reg(pi,
+					       RADIO_2056_RX_RSSI_MISC |
+					       RADIO_2056_RX1);
 		}
 
 		pi->rssical_cache.rssical_phyregs_5G[0] =
-		    read_phy_reg(pi, 0x1a6);
+			read_phy_reg(pi, 0x1a6);
 		pi->rssical_cache.rssical_phyregs_5G[1] =
-		    read_phy_reg(pi, 0x1ac);
+			read_phy_reg(pi, 0x1ac);
 		pi->rssical_cache.rssical_phyregs_5G[2] =
-		    read_phy_reg(pi, 0x1b2);
+			read_phy_reg(pi, 0x1b2);
 		pi->rssical_cache.rssical_phyregs_5G[3] =
-		    read_phy_reg(pi, 0x1b8);
+			read_phy_reg(pi, 0x1b8);
 		pi->rssical_cache.rssical_phyregs_5G[4] =
-		    read_phy_reg(pi, 0x1a4);
+			read_phy_reg(pi, 0x1a4);
 		pi->rssical_cache.rssical_phyregs_5G[5] =
-		    read_phy_reg(pi, 0x1aa);
+			read_phy_reg(pi, 0x1aa);
 		pi->rssical_cache.rssical_phyregs_5G[6] =
-		    read_phy_reg(pi, 0x1b0);
+			read_phy_reg(pi, 0x1b0);
 		pi->rssical_cache.rssical_phyregs_5G[7] =
-		    read_phy_reg(pi, 0x1b6);
+			read_phy_reg(pi, 0x1b6);
 		pi->rssical_cache.rssical_phyregs_5G[8] =
-		    read_phy_reg(pi, 0x1a5);
+			read_phy_reg(pi, 0x1a5);
 		pi->rssical_cache.rssical_phyregs_5G[9] =
-		    read_phy_reg(pi, 0x1ab);
+			read_phy_reg(pi, 0x1ab);
 		pi->rssical_cache.rssical_phyregs_5G[10] =
-		    read_phy_reg(pi, 0x1b1);
+			read_phy_reg(pi, 0x1b1);
 		pi->rssical_cache.rssical_phyregs_5G[11] =
-		    read_phy_reg(pi, 0x1b7);
+			read_phy_reg(pi, 0x1b7);
 
 		pi->nphy_rssical_chanspec_5G = pi->radio_chanspec;
 	}
@@ -22129,121 +23008,287 @@ static void wlc_phy_rssi_cal_nphy_rev3(struct brcms_phy *pi)
 	wlc_phy_clip_det_nphy(pi, 1, clip_state);
 }
 
-static void wlc_phy_restore_rssical_nphy(struct brcms_phy *pi)
+static void wlc_phy_rssi_cal_nphy_rev2(struct brcms_phy *pi, u8 rssi_type)
 {
-	if (CHSPEC_IS2G(pi->radio_chanspec)) {
-		if (pi->nphy_rssical_chanspec_2G == 0)
-			return;
+	s32 target_code;
+	u16 classif_state;
+	u16 clip_state[2];
+	u16 rssi_ctrl_state[2], pd_state[2];
+	u16 rfctrlintc_state[2], rfpdcorerxtx_state[2];
+	u16 rfctrlintc_override_val;
+	u16 clip_off[] = { 0xffff, 0xffff };
+	u16 rf_pd_val, pd_mask, rssi_ctrl_mask;
+	u8 vcm, min_vcm, vcm_tmp[4];
+	u8 vcm_final[4] = { 0, 0, 0, 0 };
+	u8 result_idx, ctr;
+	s32 poll_results[4][4] = {
+		{0, 0, 0, 0},
+		{0, 0, 0, 0},
+		{0, 0, 0, 0},
+		{0, 0, 0, 0}
+	};
+	s32 poll_miniq[4][2] = {
+		{0, 0},
+		{0, 0},
+		{0, 0},
+		{0, 0}
+	};
+	s32 min_d, curr_d;
+	s32 fine_digital_offset[4];
+	s32 poll_results_min[4] = { 0, 0, 0, 0 };
+	s32 min_poll;
 
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			mod_radio_reg(pi, RADIO_2057_NB_MASTER_CORE0,
-				      RADIO_2057_VCM_MASK,
-				      pi->rssical_cache.
-				      rssical_radio_regs_2G[0]);
-			mod_radio_reg(pi, RADIO_2057_NB_MASTER_CORE1,
-				      RADIO_2057_VCM_MASK,
-				      pi->rssical_cache.
-				      rssical_radio_regs_2G[1]);
-		} else {
-			mod_radio_reg(pi,
-				      RADIO_2056_RX_RSSI_MISC | RADIO_2056_RX0,
-				      RADIO_2056_VCM_MASK,
-				      pi->rssical_cache.
-				      rssical_radio_regs_2G[0]);
-			mod_radio_reg(pi,
-				      RADIO_2056_RX_RSSI_MISC | RADIO_2056_RX1,
-				      RADIO_2056_VCM_MASK,
-				      pi->rssical_cache.
-				      rssical_radio_regs_2G[1]);
+	switch (rssi_type) {
+	case NPHY_RSSI_SEL_NB:
+		target_code = NPHY_RSSICAL_NB_TARGET;
+		break;
+	case NPHY_RSSI_SEL_W1:
+		target_code = NPHY_RSSICAL_W1_TARGET;
+		break;
+	case NPHY_RSSI_SEL_W2:
+		target_code = NPHY_RSSICAL_W2_TARGET;
+		break;
+	default:
+		return;
+		break;
+	}
+
+	classif_state = wlc_phy_classifier_nphy(pi, 0, 0);
+	wlc_phy_classifier_nphy(pi, (0x7 << 0), 4);
+	wlc_phy_clip_det_nphy(pi, 0, clip_state);
+	wlc_phy_clip_det_nphy(pi, 1, clip_off);
+
+	rf_pd_val = (rssi_type == NPHY_RSSI_SEL_NB) ? 0x6 : 0x4;
+	rfctrlintc_override_val =
+		CHSPEC_IS5G(pi->radio_chanspec) ? 0x140 : 0x110;
+
+	rfctrlintc_state[0] = read_phy_reg(pi, 0x91);
+	rfpdcorerxtx_state[0] = read_radio_reg(pi, RADIO_2055_PD_CORE1_RXTX);
+	write_phy_reg(pi, 0x91, rfctrlintc_override_val);
+	write_radio_reg(pi, RADIO_2055_PD_CORE1_RXTX, rf_pd_val);
+
+	rfctrlintc_state[1] = read_phy_reg(pi, 0x92);
+	rfpdcorerxtx_state[1] = read_radio_reg(pi, RADIO_2055_PD_CORE2_RXTX);
+	write_phy_reg(pi, 0x92, rfctrlintc_override_val);
+	write_radio_reg(pi, RADIO_2055_PD_CORE2_RXTX, rf_pd_val);
+
+	pd_mask = RADIO_2055_NBRSSI_PD | RADIO_2055_WBRSSI_G1_PD |
+		  RADIO_2055_WBRSSI_G2_PD;
+	pd_state[0] =
+		read_radio_reg(pi, RADIO_2055_PD_CORE1_RSSI_MISC) & pd_mask;
+	pd_state[1] =
+		read_radio_reg(pi, RADIO_2055_PD_CORE2_RSSI_MISC) & pd_mask;
+	mod_radio_reg(pi, RADIO_2055_PD_CORE1_RSSI_MISC, pd_mask, 0);
+	mod_radio_reg(pi, RADIO_2055_PD_CORE2_RSSI_MISC, pd_mask, 0);
+	rssi_ctrl_mask = RADIO_2055_NBRSSI_SEL | RADIO_2055_WBRSSI_G1_SEL |
+			 RADIO_2055_WBRSSI_G2_SEL;
+	rssi_ctrl_state[0] =
+		read_radio_reg(pi, RADIO_2055_SP_RSSI_CORE1) & rssi_ctrl_mask;
+	rssi_ctrl_state[1] =
+		read_radio_reg(pi, RADIO_2055_SP_RSSI_CORE2) & rssi_ctrl_mask;
+	wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_ALLRX, rssi_type);
+
+	wlc_phy_scale_offset_rssi_nphy(pi, 0x0, 0x0, RADIO_MIMO_CORESEL_ALLRX,
+				       NPHY_RAIL_I, rssi_type);
+	wlc_phy_scale_offset_rssi_nphy(pi, 0x0, 0x0, RADIO_MIMO_CORESEL_ALLRX,
+				       NPHY_RAIL_Q, rssi_type);
+
+	for (vcm = 0; vcm < 4; vcm++) {
+
+		vcm_tmp[0] = vcm_tmp[1] = vcm_tmp[2] = vcm_tmp[3] = vcm;
+		if (rssi_type != NPHY_RSSI_SEL_W2)
+			wlc_phy_set_rssi_2055_vcm(pi, rssi_type, vcm_tmp);
+
+		wlc_phy_poll_rssi_nphy(pi, rssi_type, &poll_results[vcm][0],
+				       NPHY_RSSICAL_NPOLL);
+
+		if ((rssi_type == NPHY_RSSI_SEL_W1)
+		    || (rssi_type == NPHY_RSSI_SEL_W2)) {
+			for (ctr = 0; ctr < 2; ctr++)
+				poll_miniq[vcm][ctr] =
+					min(poll_results[vcm][ctr * 2 + 0],
+					    poll_results[vcm][ctr * 2 + 1]);
 		}
+	}
 
-		write_phy_reg(pi, 0x1a6,
-			      pi->rssical_cache.rssical_phyregs_2G[0]);
-		write_phy_reg(pi, 0x1ac,
-			      pi->rssical_cache.rssical_phyregs_2G[1]);
-		write_phy_reg(pi, 0x1b2,
-			      pi->rssical_cache.rssical_phyregs_2G[2]);
-		write_phy_reg(pi, 0x1b8,
-			      pi->rssical_cache.rssical_phyregs_2G[3]);
-		write_phy_reg(pi, 0x1a4,
-			      pi->rssical_cache.rssical_phyregs_2G[4]);
-		write_phy_reg(pi, 0x1aa,
-			      pi->rssical_cache.rssical_phyregs_2G[5]);
-		write_phy_reg(pi, 0x1b0,
-			      pi->rssical_cache.rssical_phyregs_2G[6]);
-		write_phy_reg(pi, 0x1b6,
-			      pi->rssical_cache.rssical_phyregs_2G[7]);
-		write_phy_reg(pi, 0x1a5,
-			      pi->rssical_cache.rssical_phyregs_2G[8]);
-		write_phy_reg(pi, 0x1ab,
-			      pi->rssical_cache.rssical_phyregs_2G[9]);
-		write_phy_reg(pi, 0x1b1,
-			      pi->rssical_cache.rssical_phyregs_2G[10]);
-		write_phy_reg(pi, 0x1b7,
-			      pi->rssical_cache.rssical_phyregs_2G[11]);
+	for (result_idx = 0; result_idx < 4; result_idx++) {
+		min_d = NPHY_RSSICAL_MAXD;
+		min_vcm = 0;
+		min_poll = NPHY_RSSICAL_MAXREAD * NPHY_RSSICAL_NPOLL + 1;
+		for (vcm = 0; vcm < 4; vcm++) {
+			curr_d = abs(((rssi_type == NPHY_RSSI_SEL_NB) ?
+				      poll_results[vcm][result_idx] :
+				      poll_miniq[vcm][result_idx / 2]) -
+				     (target_code * NPHY_RSSICAL_NPOLL));
+			if (curr_d < min_d) {
+				min_d = curr_d;
+				min_vcm = vcm;
+			}
+			if (poll_results[vcm][result_idx] < min_poll)
+				min_poll = poll_results[vcm][result_idx];
+		}
+		vcm_final[result_idx] = min_vcm;
+		poll_results_min[result_idx] = min_poll;
+	}
 
-	} else {
-		if (pi->nphy_rssical_chanspec_5G == 0)
-			return;
+	if (rssi_type != NPHY_RSSI_SEL_W2)
+		wlc_phy_set_rssi_2055_vcm(pi, rssi_type, vcm_final);
 
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			mod_radio_reg(pi, RADIO_2057_NB_MASTER_CORE0,
-				      RADIO_2057_VCM_MASK,
-				      pi->rssical_cache.
-				      rssical_radio_regs_5G[0]);
-			mod_radio_reg(pi, RADIO_2057_NB_MASTER_CORE1,
-				      RADIO_2057_VCM_MASK,
-				      pi->rssical_cache.
-				      rssical_radio_regs_5G[1]);
+	for (result_idx = 0; result_idx < 4; result_idx++) {
+		fine_digital_offset[result_idx] =
+			(target_code * NPHY_RSSICAL_NPOLL) -
+			poll_results[vcm_final[result_idx]][result_idx];
+		if (fine_digital_offset[result_idx] < 0) {
+			fine_digital_offset[result_idx] =
+				abs(fine_digital_offset[result_idx]);
+			fine_digital_offset[result_idx] +=
+				(NPHY_RSSICAL_NPOLL / 2);
+			fine_digital_offset[result_idx] /= NPHY_RSSICAL_NPOLL;
+			fine_digital_offset[result_idx] =
+				-fine_digital_offset[result_idx];
 		} else {
-			mod_radio_reg(pi,
-				      RADIO_2056_RX_RSSI_MISC | RADIO_2056_RX0,
-				      RADIO_2056_VCM_MASK,
-				      pi->rssical_cache.
-				      rssical_radio_regs_5G[0]);
-			mod_radio_reg(pi,
-				      RADIO_2056_RX_RSSI_MISC | RADIO_2056_RX1,
-				      RADIO_2056_VCM_MASK,
-				      pi->rssical_cache.
-				      rssical_radio_regs_5G[1]);
+			fine_digital_offset[result_idx] +=
+				(NPHY_RSSICAL_NPOLL / 2);
+			fine_digital_offset[result_idx] /= NPHY_RSSICAL_NPOLL;
 		}
 
-		write_phy_reg(pi, 0x1a6,
-			      pi->rssical_cache.rssical_phyregs_5G[0]);
-		write_phy_reg(pi, 0x1ac,
-			      pi->rssical_cache.rssical_phyregs_5G[1]);
-		write_phy_reg(pi, 0x1b2,
-			      pi->rssical_cache.rssical_phyregs_5G[2]);
-		write_phy_reg(pi, 0x1b8,
-			      pi->rssical_cache.rssical_phyregs_5G[3]);
-		write_phy_reg(pi, 0x1a4,
-			      pi->rssical_cache.rssical_phyregs_5G[4]);
-		write_phy_reg(pi, 0x1aa,
-			      pi->rssical_cache.rssical_phyregs_5G[5]);
-		write_phy_reg(pi, 0x1b0,
-			      pi->rssical_cache.rssical_phyregs_5G[6]);
-		write_phy_reg(pi, 0x1b6,
-			      pi->rssical_cache.rssical_phyregs_5G[7]);
-		write_phy_reg(pi, 0x1a5,
-			      pi->rssical_cache.rssical_phyregs_5G[8]);
-		write_phy_reg(pi, 0x1ab,
-			      pi->rssical_cache.rssical_phyregs_5G[9]);
-		write_phy_reg(pi, 0x1b1,
-			      pi->rssical_cache.rssical_phyregs_5G[10]);
-		write_phy_reg(pi, 0x1b7,
-			      pi->rssical_cache.rssical_phyregs_5G[11]);
+		if (poll_results_min[result_idx] ==
+		    NPHY_RSSICAL_MAXREAD * NPHY_RSSICAL_NPOLL)
+			fine_digital_offset[result_idx] =
+				(target_code - NPHY_RSSICAL_MAXREAD - 1);
+
+		wlc_phy_scale_offset_rssi_nphy(pi, 0x0,
+					       (s8)
+					       fine_digital_offset[result_idx],
+					       (result_idx / 2 ==
+						0) ? RADIO_MIMO_CORESEL_CORE1 :
+					       RADIO_MIMO_CORESEL_CORE2,
+					       (result_idx % 2 ==
+						0) ? NPHY_RAIL_I : NPHY_RAIL_Q,
+					       rssi_type);
+	}
+
+	mod_radio_reg(pi, RADIO_2055_PD_CORE1_RSSI_MISC, pd_mask, pd_state[0]);
+	mod_radio_reg(pi, RADIO_2055_PD_CORE2_RSSI_MISC, pd_mask, pd_state[1]);
+	if (rssi_ctrl_state[0] == RADIO_2055_NBRSSI_SEL)
+		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE1,
+				     NPHY_RSSI_SEL_NB);
+	else if (rssi_ctrl_state[0] == RADIO_2055_WBRSSI_G1_SEL)
+		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE1,
+				     NPHY_RSSI_SEL_W1);
+	else if (rssi_ctrl_state[0] == RADIO_2055_WBRSSI_G2_SEL)
+		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE1,
+				     NPHY_RSSI_SEL_W2);
+	else
+		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE1,
+				     NPHY_RSSI_SEL_W2);
+	if (rssi_ctrl_state[1] == RADIO_2055_NBRSSI_SEL)
+		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE2,
+				     NPHY_RSSI_SEL_NB);
+	else if (rssi_ctrl_state[1] == RADIO_2055_WBRSSI_G1_SEL)
+		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE2,
+				     NPHY_RSSI_SEL_W1);
+	else if (rssi_ctrl_state[1] == RADIO_2055_WBRSSI_G2_SEL)
+		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE2,
+				     NPHY_RSSI_SEL_W2);
+	else
+		wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_CORE2,
+				     NPHY_RSSI_SEL_W2);
+
+	wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_OFF, rssi_type);
+
+	write_phy_reg(pi, 0x91, rfctrlintc_state[0]);
+	write_radio_reg(pi, RADIO_2055_PD_CORE1_RXTX, rfpdcorerxtx_state[0]);
+	write_phy_reg(pi, 0x92, rfctrlintc_state[1]);
+	write_radio_reg(pi, RADIO_2055_PD_CORE2_RXTX, rfpdcorerxtx_state[1]);
+
+	wlc_phy_classifier_nphy(pi, (0x7 << 0), classif_state);
+	wlc_phy_clip_det_nphy(pi, 1, clip_state);
+
+	wlc_phy_resetcca_nphy(pi);
+}
+
+void wlc_phy_rssi_cal_nphy(struct brcms_phy *pi)
+{
+	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+		wlc_phy_rssi_cal_nphy_rev3(pi);
+	} else {
+		wlc_phy_rssi_cal_nphy_rev2(pi, NPHY_RSSI_SEL_NB);
+		wlc_phy_rssi_cal_nphy_rev2(pi, NPHY_RSSI_SEL_W1);
+		wlc_phy_rssi_cal_nphy_rev2(pi, NPHY_RSSI_SEL_W2);
 	}
 }
 
+int
+wlc_phy_rssi_compute_nphy(struct brcms_phy *pi, struct d11rxhdr *rxh)
+{
+	s16 rxpwr, rxpwr0, rxpwr1;
+	s16 phyRx0_l, phyRx2_l;
+
+	rxpwr = 0;
+	rxpwr0 = rxh->PhyRxStatus_1 & PRXS1_nphy_PWR0_MASK;
+	rxpwr1 = (rxh->PhyRxStatus_1 & PRXS1_nphy_PWR1_MASK) >> 8;
+
+	if (rxpwr0 > 127)
+		rxpwr0 -= 256;
+	if (rxpwr1 > 127)
+		rxpwr1 -= 256;
+
+	phyRx0_l = rxh->PhyRxStatus_0 & 0x00ff;
+	phyRx2_l = rxh->PhyRxStatus_2 & 0x00ff;
+	if (phyRx2_l > 127)
+		phyRx2_l -= 256;
+
+	if (((rxpwr0 == 16) || (rxpwr0 == 32))) {
+		rxpwr0 = rxpwr1;
+		rxpwr1 = phyRx2_l;
+	}
+
+	if (pi->sh->rssi_mode == RSSI_ANT_MERGE_MAX)
+		rxpwr = (rxpwr0 > rxpwr1) ? rxpwr0 : rxpwr1;
+	else if (pi->sh->rssi_mode == RSSI_ANT_MERGE_MIN)
+		rxpwr = (rxpwr0 < rxpwr1) ? rxpwr0 : rxpwr1;
+	else if (pi->sh->rssi_mode == RSSI_ANT_MERGE_AVG)
+		rxpwr = (rxpwr0 + rxpwr1) >> 1;
+
+	return rxpwr;
+}
+
+static void
+wlc_phy_loadsampletable_nphy(struct brcms_phy *pi, struct cordic_iq *tone_buf,
+			     u16 num_samps)
+{
+	u16 t;
+	u32 *data_buf = NULL;
+
+	data_buf = kmalloc(sizeof(u32) * num_samps, GFP_ATOMIC);
+	if (data_buf == NULL)
+		return;
+
+	if (pi->phyhang_avoid)
+		wlc_phy_stay_in_carriersearch_nphy(pi, true);
+
+	for (t = 0; t < num_samps; t++)
+		data_buf[t] = ((((unsigned int)tone_buf[t].i) & 0x3ff) << 10) |
+			      (((unsigned int)tone_buf[t].q) & 0x3ff);
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_SAMPLEPLAY, num_samps, 0, 32,
+				 data_buf);
+
+	kfree(data_buf);
+
+	if (pi->phyhang_avoid)
+		wlc_phy_stay_in_carriersearch_nphy(pi, false);
+}
+
 static u16
 wlc_phy_gen_load_samples_nphy(struct brcms_phy *pi, u32 f_kHz, u16 max_val,
 			      u8 dac_test_mode)
 {
 	u8 phy_bw, is_phybw40;
 	u16 num_samps, t, spur;
-	fixed theta = 0, rot = 0;
+	s32 theta = 0, rot = 0;
 	u32 tbl_len;
-	cs32 *tone_buf = NULL;
+	struct cordic_iq *tone_buf = NULL;
 
 	is_phybw40 = CHSPEC_IS40(pi->radio_chanspec);
 	phy_bw = (is_phybw40 == 1) ? 40 : 20;
@@ -22258,18 +23303,17 @@ wlc_phy_gen_load_samples_nphy(struct brcms_phy *pi, u32 f_kHz, u16 max_val,
 		tbl_len = (phy_bw << 1);
 	}
 
-	tone_buf = kmalloc(sizeof(cs32) * tbl_len, GFP_ATOMIC);
-	if (tone_buf == NULL) {
+	tone_buf = kmalloc(sizeof(struct cordic_iq) * tbl_len, GFP_ATOMIC);
+	if (tone_buf == NULL)
 		return 0;
-	}
 
 	num_samps = (u16) tbl_len;
-	rot = FIXED((f_kHz * 36) / phy_bw) / 100;
+	rot = ((f_kHz * 36) / phy_bw) / 100;
 	theta = 0;
 
 	for (t = 0; t < num_samps; t++) {
 
-		wlc_phy_cordic(theta, &tone_buf[t]);
+		tone_buf[t] = cordic_calc_iq(theta);
 
 		theta += rot;
 
@@ -22284,54 +23328,6 @@ wlc_phy_gen_load_samples_nphy(struct brcms_phy *pi, u32 f_kHz, u16 max_val,
 	return num_samps;
 }
 
-int
-wlc_phy_tx_tone_nphy(struct brcms_phy *pi, u32 f_kHz, u16 max_val,
-		     u8 iqmode, u8 dac_test_mode, bool modify_bbmult)
-{
-	u16 num_samps;
-	u16 loops = 0xffff;
-	u16 wait = 0;
-
-	num_samps =
-		wlc_phy_gen_load_samples_nphy(pi, f_kHz, max_val, dac_test_mode);
-	if (num_samps == 0) {
-		return -EBADE;
-	}
-
-	wlc_phy_runsamples_nphy(pi, num_samps, loops, wait, iqmode,
-				dac_test_mode, modify_bbmult);
-
-	return 0;
-}
-
-static void
-wlc_phy_loadsampletable_nphy(struct brcms_phy *pi, cs32 *tone_buf,
-			     u16 num_samps)
-{
-	u16 t;
-	u32 *data_buf = NULL;
-
-	data_buf = kmalloc(sizeof(u32) * num_samps, GFP_ATOMIC);
-	if (data_buf == NULL) {
-		return;
-	}
-
-	if (pi->phyhang_avoid)
-		wlc_phy_stay_in_carriersearch_nphy(pi, true);
-
-	for (t = 0; t < num_samps; t++) {
-		data_buf[t] = ((((unsigned int)tone_buf[t].i) & 0x3ff) << 10) |
-		    (((unsigned int)tone_buf[t].q) & 0x3ff);
-	}
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_SAMPLEPLAY, num_samps, 0, 32,
-				 data_buf);
-
-	kfree(data_buf);
-
-	if (pi->phyhang_avoid)
-		wlc_phy_stay_in_carriersearch_nphy(pi, false);
-}
-
 static void
 wlc_phy_runsamples_nphy(struct brcms_phy *pi, u16 num_samps, u16 loops,
 			u16 wait, u8 iqmode, u8 dac_test_mode,
@@ -22356,22 +23352,24 @@ wlc_phy_runsamples_nphy(struct brcms_phy *pi, u16 num_samps, u16 loops,
 		lpf_bw_ctl_override4 = read_phy_reg(pi, 0x343) & (0x1 << 7);
 		if (lpf_bw_ctl_override3 | lpf_bw_ctl_override4) {
 			lpf_bw_ctl_miscreg3 = read_phy_reg(pi, 0x340) &
-			    (0x7 << 8);
+					      (0x7 << 8);
 			lpf_bw_ctl_miscreg4 = read_phy_reg(pi, 0x341) &
-			    (0x7 << 8);
+					      (0x7 << 8);
 		} else {
-			wlc_phy_rfctrl_override_nphy_rev7(pi,
-							  (0x1 << 7),
-							  wlc_phy_read_lpf_bw_ctl_nphy
-							  (pi, 0), 0, 0,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi,
+				(0x1 << 7),
+				wlc_phy_read_lpf_bw_ctl_nphy
+					(pi,
+					0), 0, 0,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
 
 			pi->nphy_sample_play_lpf_bw_ctl_ovr = true;
 
 			lpf_bw_ctl_miscreg3 = read_phy_reg(pi, 0x340) &
-			    (0x7 << 8);
+					      (0x7 << 8);
 			lpf_bw_ctl_miscreg4 = read_phy_reg(pi, 0x341) &
-			    (0x7 << 8);
+					      (0x7 << 8);
 		}
 	}
 
@@ -22380,7 +23378,7 @@ wlc_phy_runsamples_nphy(struct brcms_phy *pi, u16 num_samps, u16 loops,
 		wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_IQLOCAL, 1, 87, 16,
 					&bb_mult);
 		pi->nphy_bb_mult_save =
-		    BB_MULT_VALID_MASK | (bb_mult & BB_MULT_MASK);
+			BB_MULT_VALID_MASK | (bb_mult & BB_MULT_MASK);
 	}
 
 	if (modify_bbmult) {
@@ -22395,11 +23393,11 @@ wlc_phy_runsamples_nphy(struct brcms_phy *pi, u16 num_samps, u16 loops,
 
 	write_phy_reg(pi, 0xc6, num_samps - 1);
 
-	if (loops != 0xffff) {
+	if (loops != 0xffff)
 		write_phy_reg(pi, 0xc4, loops - 1);
-	} else {
+	else
 		write_phy_reg(pi, 0xc4, loops);
-	}
+
 	write_phy_reg(pi, 0xc5, wait);
 
 	orig_RfseqCoreActv = read_phy_reg(pi, 0xa1);
@@ -22420,6 +23418,25 @@ wlc_phy_runsamples_nphy(struct brcms_phy *pi, u16 num_samps, u16 loops,
 	write_phy_reg(pi, 0xa1, orig_RfseqCoreActv);
 }
 
+int
+wlc_phy_tx_tone_nphy(struct brcms_phy *pi, u32 f_kHz, u16 max_val,
+		     u8 iqmode, u8 dac_test_mode, bool modify_bbmult)
+{
+	u16 num_samps;
+	u16 loops = 0xffff;
+	u16 wait = 0;
+
+	num_samps = wlc_phy_gen_load_samples_nphy(pi, f_kHz, max_val,
+						  dac_test_mode);
+	if (num_samps == 0)
+		return -EBADE;
+
+	wlc_phy_runsamples_nphy(pi, num_samps, loops, wait, iqmode,
+				dac_test_mode, modify_bbmult);
+
+	return 0;
+}
+
 void wlc_phy_stopplayback_nphy(struct brcms_phy *pi)
 {
 	u16 playback_status;
@@ -22429,13 +23446,11 @@ void wlc_phy_stopplayback_nphy(struct brcms_phy *pi)
 		wlc_phy_stay_in_carriersearch_nphy(pi, true);
 
 	playback_status = read_phy_reg(pi, 0xc7);
-	if (playback_status & 0x1) {
+	if (playback_status & 0x1)
 		or_phy_reg(pi, 0xc3, NPHY_sampleCmd_STOP);
-	} else if (playback_status & 0x2) {
-
+	else if (playback_status & 0x2)
 		and_phy_reg(pi, 0xc2,
 			    (u16) ~NPHY_iqloCalCmdGctl_IQLO_CAL_EN);
-	}
 
 	and_phy_reg(pi, 0xc3, (u16) ~(0x1 << 2));
 
@@ -22450,10 +23465,11 @@ void wlc_phy_stopplayback_nphy(struct brcms_phy *pi)
 
 	if (NREV_IS(pi->pubpi.phy_rev, 7) || NREV_GE(pi->pubpi.phy_rev, 8)) {
 		if (pi->nphy_sample_play_lpf_bw_ctl_ovr) {
-			wlc_phy_rfctrl_override_nphy_rev7(pi,
-							  (0x1 << 7),
-							  0, 0, 1,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi,
+				(0x1 << 7),
+				0, 0, 1,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
 			pi->nphy_sample_play_lpf_bw_ctl_ovr = false;
 		}
 	}
@@ -22462,6 +23478,47 @@ void wlc_phy_stopplayback_nphy(struct brcms_phy *pi)
 		wlc_phy_stay_in_carriersearch_nphy(pi, false);
 }
 
+static u32 *brcms_phy_get_tx_pwrctrl_tbl(struct brcms_phy *pi)
+{
+	u32 *tx_pwrctrl_tbl = NULL;
+	uint phyrev = pi->pubpi.phy_rev;
+
+	if (PHY_IPA(pi)) {
+		tx_pwrctrl_tbl =
+			wlc_phy_get_ipa_gaintbl_nphy(pi);
+	} else {
+		if (CHSPEC_IS5G(pi->radio_chanspec)) {
+			if (NREV_IS(phyrev, 3))
+				tx_pwrctrl_tbl = nphy_tpc_5GHz_txgain_rev3;
+			else if (NREV_IS(phyrev, 4))
+				tx_pwrctrl_tbl =
+					(pi->srom_fem5g.extpagain == 3) ?
+					nphy_tpc_5GHz_txgain_HiPwrEPA :
+					nphy_tpc_5GHz_txgain_rev4;
+			else
+				tx_pwrctrl_tbl = nphy_tpc_5GHz_txgain_rev5;
+		} else {
+			if (NREV_GE(phyrev, 7)) {
+				if (pi->pubpi.radiorev == 3)
+					tx_pwrctrl_tbl =
+						nphy_tpc_txgain_epa_2057rev3;
+				else if (pi->pubpi.radiorev == 5)
+					tx_pwrctrl_tbl =
+						nphy_tpc_txgain_epa_2057rev5;
+			} else {
+				if (NREV_GE(phyrev, 5) &&
+				   (pi->srom_fem2g.extpagain ==	3))
+					tx_pwrctrl_tbl =
+						nphy_tpc_txgain_HiPwrEPA;
+				else
+					tx_pwrctrl_tbl =
+						nphy_tpc_txgain_rev3;
+			}
+		}
+	}
+	return tx_pwrctrl_tbl;
+}
+
 struct nphy_txgains wlc_phy_get_tx_gain_nphy(struct brcms_phy *pi)
 {
 	u16 base_idx[2], curr_gain[2];
@@ -22482,33 +23539,33 @@ struct nphy_txgains wlc_phy_get_tx_gain_nphy(struct brcms_phy *pi)
 		for (core_no = 0; core_no < 2; core_no++) {
 			if (NREV_GE(pi->pubpi.phy_rev, 7)) {
 				target_gain.ipa[core_no] =
-				    curr_gain[core_no] & 0x0007;
+					curr_gain[core_no] & 0x0007;
 				target_gain.pad[core_no] =
-				    ((curr_gain[core_no] & 0x00F8) >> 3);
+					((curr_gain[core_no] & 0x00F8) >> 3);
 				target_gain.pga[core_no] =
-				    ((curr_gain[core_no] & 0x0F00) >> 8);
+					((curr_gain[core_no] & 0x0F00) >> 8);
 				target_gain.txgm[core_no] =
-				    ((curr_gain[core_no] & 0x7000) >> 12);
+					((curr_gain[core_no] & 0x7000) >> 12);
 				target_gain.txlpf[core_no] =
-				    ((curr_gain[core_no] & 0x8000) >> 15);
+					((curr_gain[core_no] & 0x8000) >> 15);
 			} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
 				target_gain.ipa[core_no] =
-				    curr_gain[core_no] & 0x000F;
+					curr_gain[core_no] & 0x000F;
 				target_gain.pad[core_no] =
-				    ((curr_gain[core_no] & 0x00F0) >> 4);
+					((curr_gain[core_no] & 0x00F0) >> 4);
 				target_gain.pga[core_no] =
-				    ((curr_gain[core_no] & 0x0F00) >> 8);
+					((curr_gain[core_no] & 0x0F00) >> 8);
 				target_gain.txgm[core_no] =
-				    ((curr_gain[core_no] & 0x7000) >> 12);
+					((curr_gain[core_no] & 0x7000) >> 12);
 			} else {
 				target_gain.ipa[core_no] =
-				    curr_gain[core_no] & 0x0003;
+					curr_gain[core_no] & 0x0003;
 				target_gain.pad[core_no] =
-				    ((curr_gain[core_no] & 0x000C) >> 2);
+					((curr_gain[core_no] & 0x000C) >> 2);
 				target_gain.pga[core_no] =
-				    ((curr_gain[core_no] & 0x0070) >> 4);
+					((curr_gain[core_no] & 0x0070) >> 4);
 				target_gain.txgm[core_no] =
-				    ((curr_gain[core_no] & 0x0380) >> 7);
+					((curr_gain[core_no] & 0x0380) >> 7);
 			}
 		}
 	} else {
@@ -22518,96 +23575,60 @@ struct nphy_txgains wlc_phy_get_tx_gain_nphy(struct brcms_phy *pi)
 		base_idx[1] = (read_phy_reg(pi, 0x1ee) >> 8) & 0x7f;
 		for (core_no = 0; core_no < 2; core_no++) {
 			if (NREV_GE(phyrev, 3)) {
-				if (PHY_IPA(pi)) {
-					tx_pwrctrl_tbl =
-					    wlc_phy_get_ipa_gaintbl_nphy(pi);
-				} else {
-					if (CHSPEC_IS5G(pi->radio_chanspec)) {
-						if (NREV_IS(phyrev, 3)) {
-							tx_pwrctrl_tbl =
-							    nphy_tpc_5GHz_txgain_rev3;
-						} else if (NREV_IS(phyrev, 4)) {
-							tx_pwrctrl_tbl =
-							    (pi->srom_fem5g.
-							     extpagain ==
-							     3) ?
-							    nphy_tpc_5GHz_txgain_HiPwrEPA
-							    :
-							    nphy_tpc_5GHz_txgain_rev4;
-						} else {
-							tx_pwrctrl_tbl =
-							    nphy_tpc_5GHz_txgain_rev5;
-						}
-					} else {
-						if (NREV_GE(phyrev, 7)) {
-							if (pi->pubpi.
-							    radiorev == 3) {
-								tx_pwrctrl_tbl =
-								    nphy_tpc_txgain_epa_2057rev3;
-							} else if (pi->pubpi.
-								   radiorev ==
-								   5) {
-								tx_pwrctrl_tbl =
-								    nphy_tpc_txgain_epa_2057rev5;
-							}
-
-						} else {
-							if (NREV_GE(phyrev, 5)
-							    && (pi->srom_fem2g.
-								extpagain ==
-								3)) {
-								tx_pwrctrl_tbl =
-								    nphy_tpc_txgain_HiPwrEPA;
-							} else {
-								tx_pwrctrl_tbl =
-								    nphy_tpc_txgain_rev3;
-							}
-						}
-					}
-				}
+				tx_pwrctrl_tbl =
+					brcms_phy_get_tx_pwrctrl_tbl(pi);
 				if (NREV_GE(phyrev, 7)) {
 					target_gain.ipa[core_no] =
-					    (tx_pwrctrl_tbl[base_idx[core_no]]
-					     >> 16) & 0x7;
+						(tx_pwrctrl_tbl
+						 [base_idx[core_no]]
+						 >> 16) & 0x7;
 					target_gain.pad[core_no] =
-					    (tx_pwrctrl_tbl[base_idx[core_no]]
-					     >> 19) & 0x1f;
+						(tx_pwrctrl_tbl
+						 [base_idx[core_no]]
+						 >> 19) & 0x1f;
 					target_gain.pga[core_no] =
-					    (tx_pwrctrl_tbl[base_idx[core_no]]
-					     >> 24) & 0xf;
+						(tx_pwrctrl_tbl
+						 [base_idx[core_no]]
+						 >> 24) & 0xf;
 					target_gain.txgm[core_no] =
-					    (tx_pwrctrl_tbl[base_idx[core_no]]
-					     >> 28) & 0x7;
+						(tx_pwrctrl_tbl
+						 [base_idx[core_no]]
+						 >> 28) & 0x7;
 					target_gain.txlpf[core_no] =
-					    (tx_pwrctrl_tbl[base_idx[core_no]]
-					     >> 31) & 0x1;
+						(tx_pwrctrl_tbl
+						 [base_idx[core_no]]
+						 >> 31) & 0x1;
 				} else {
 					target_gain.ipa[core_no] =
-					    (tx_pwrctrl_tbl[base_idx[core_no]]
-					     >> 16) & 0xf;
+						(tx_pwrctrl_tbl
+						 [base_idx[core_no]]
+						 >> 16) & 0xf;
 					target_gain.pad[core_no] =
-					    (tx_pwrctrl_tbl[base_idx[core_no]]
-					     >> 20) & 0xf;
+						(tx_pwrctrl_tbl
+						 [base_idx[core_no]]
+						 >> 20) & 0xf;
 					target_gain.pga[core_no] =
-					    (tx_pwrctrl_tbl[base_idx[core_no]]
-					     >> 24) & 0xf;
+						(tx_pwrctrl_tbl
+						 [base_idx[core_no]]
+						 >> 24) & 0xf;
 					target_gain.txgm[core_no] =
-					    (tx_pwrctrl_tbl[base_idx[core_no]]
-					     >> 28) & 0x7;
+						(tx_pwrctrl_tbl
+						[base_idx[core_no]]
+						 >> 28) & 0x7;
 				}
 			} else {
 				target_gain.ipa[core_no] =
-				    (nphy_tpc_txgain[base_idx[core_no]] >> 16) &
-				    0x3;
+					(nphy_tpc_txgain[base_idx[core_no]] >>
+					 16) & 0x3;
 				target_gain.pad[core_no] =
-				    (nphy_tpc_txgain[base_idx[core_no]] >> 18) &
-				    0x3;
+					(nphy_tpc_txgain[base_idx[core_no]] >>
+					 18) & 0x3;
 				target_gain.pga[core_no] =
-				    (nphy_tpc_txgain[base_idx[core_no]] >> 20) &
-				    0x7;
+					(nphy_tpc_txgain[base_idx[core_no]] >>
+					 20) & 0x7;
 				target_gain.txgm[core_no] =
-				    (nphy_tpc_txgain[base_idx[core_no]] >> 23) &
-				    0x7;
+					(nphy_tpc_txgain[base_idx[core_no]] >>
+					 23) & 0x7;
 			}
 		}
 	}
@@ -22626,26 +23647,23 @@ wlc_phy_iqcal_gainparams_nphy(struct brcms_phy *pi, u16 core_no,
 	u8 band_idx = (CHSPEC_IS5G(pi->radio_chanspec) ? 1 : 0);
 
 	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+		if (NREV_GE(pi->pubpi.phy_rev, 7))
 			params->txlpf = target_gain.txlpf[core_no];
-		}
+
 		params->txgm = target_gain.txgm[core_no];
 		params->pga = target_gain.pga[core_no];
 		params->pad = target_gain.pad[core_no];
 		params->ipa = target_gain.ipa[core_no];
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+		if (NREV_GE(pi->pubpi.phy_rev, 7))
 			params->cal_gain =
-			    ((params->txlpf << 15) | (params->
-						      txgm << 12) | (params->
-								     pga << 8) |
-			     (params->pad << 3) | (params->ipa));
-		} else {
+				((params->txlpf << 15) | (params->txgm << 12) |
+				 (params->pga << 8) |
+				 (params->pad << 3) | (params->ipa));
+		else
 			params->cal_gain =
-			    ((params->txgm << 12) | (params->
-						     pga << 8) | (params->
-								  pad << 4) |
-			     (params->ipa));
-		}
+				((params->txgm << 12) | (params->pga << 8) |
+				 (params->pad << 4) | (params->ipa));
+
 		params->ncorr[0] = 0x79;
 		params->ncorr[1] = 0x79;
 		params->ncorr[2] = 0x79;
@@ -22654,9 +23672,8 @@ wlc_phy_iqcal_gainparams_nphy(struct brcms_phy *pi, u16 core_no,
 	} else {
 
 		gain_index = ((target_gain.pad[core_no] << 0) |
-			      (target_gain.pga[core_no] << 4) | (target_gain.
-								 txgm[core_no]
-								 << 8));
+			      (target_gain.pga[core_no] << 4) |
+			      (target_gain.txgm[core_no] << 8));
 
 		idx = -1;
 		for (k = 0; k < NPHY_IQCAL_NUMGAINS; k++) {
@@ -22688,37 +23705,39 @@ static void wlc_phy_txcal_radio_setup_nphy(struct brcms_phy *pi)
 		for (core = 0; core <= 1; core++) {
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 0] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, core,
-					    TX_SSI_MASTER);
+				READ_RADIO_REG3(pi, RADIO_2057, TX, core,
+						TX_SSI_MASTER);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 1] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, core,
-					    IQCAL_VCM_HG);
+				READ_RADIO_REG3(pi, RADIO_2057, TX, core,
+						IQCAL_VCM_HG);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 2] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, core,
-					    IQCAL_IDAC);
+				READ_RADIO_REG3(pi, RADIO_2057, TX, core,
+						IQCAL_IDAC);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 3] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, core, TSSI_VCM);
+				READ_RADIO_REG3(pi, RADIO_2057, TX, core,
+						TSSI_VCM);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 4] = 0;
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 5] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, core,
-					    TX_SSI_MUX);
+				READ_RADIO_REG3(pi, RADIO_2057, TX, core,
+						TX_SSI_MUX);
 
 			if (pi->pubpi.radiorev != 5)
 				pi->tx_rx_cal_radio_saveregs[(core * 11) + 6] =
-				    READ_RADIO_REG3(pi, RADIO_2057, TX, core,
-						    TSSIA);
+					READ_RADIO_REG3(pi, RADIO_2057, TX,
+							core,
+							TSSIA);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 7] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, core, TSSIG);
+			       READ_RADIO_REG3(pi, RADIO_2057, TX, core, TSSIG);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 8] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, core,
-					    TSSI_MISC1);
+				READ_RADIO_REG3(pi, RADIO_2057, TX, core,
+						TSSI_MISC1);
 
 			if (CHSPEC_IS5G(pi->radio_chanspec)) {
 				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
@@ -22734,19 +23753,15 @@ static void wlc_phy_txcal_radio_setup_nphy(struct brcms_phy *pi)
 				if (pi->use_int_tx_iqlo_cal_nphy) {
 					WRITE_RADIO_REG3(pi, RADIO_2057, TX,
 							 core, TX_SSI_MUX, 0x4);
-					if (!
-					    (pi->
-					     internal_tx_iqlo_cal_tapoff_intpa_nphy)) {
-
+					if (!(pi->
+					internal_tx_iqlo_cal_tapoff_intpa_nphy))
 						WRITE_RADIO_REG3(pi, RADIO_2057,
 								 TX, core,
 								 TSSIA, 0x31);
-					} else {
-
+					else
 						WRITE_RADIO_REG3(pi, RADIO_2057,
 								 TX, core,
 								 TSSIA, 0x21);
-					}
 				}
 				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
 						 TSSI_MISC1, 0x00);
@@ -22767,19 +23782,15 @@ static void wlc_phy_txcal_radio_setup_nphy(struct brcms_phy *pi)
 					WRITE_RADIO_REG3(pi, RADIO_2057, TX,
 							 core, TX_SSI_MUX,
 							 0x06);
-					if (!
-					    (pi->
-					     internal_tx_iqlo_cal_tapoff_intpa_nphy)) {
-
+					if (!(pi->
+					internal_tx_iqlo_cal_tapoff_intpa_nphy))
 						WRITE_RADIO_REG3(pi, RADIO_2057,
 								 TX, core,
 								 TSSIG, 0x31);
-					} else {
-
+					else
 						WRITE_RADIO_REG3(pi, RADIO_2057,
 								 TX, core,
 								 TSSIG, 0x21);
-					}
 				}
 				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
 						 TSSI_MISC1, 0x00);
@@ -22789,58 +23800,62 @@ static void wlc_phy_txcal_radio_setup_nphy(struct brcms_phy *pi)
 
 		for (core = 0; core <= 1; core++) {
 			jtag_core =
-			    (core ==
-			     PHY_CORE_0) ? RADIO_2056_TX0 : RADIO_2056_TX1;
+				(core ==
+				 PHY_CORE_0) ? RADIO_2056_TX0 : RADIO_2056_TX1;
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 0] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_TX_SSI_MASTER |
-					   jtag_core);
+				read_radio_reg(pi,
+					       RADIO_2056_TX_TX_SSI_MASTER |
+					       jtag_core);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 1] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_IQCAL_VCM_HG |
-					   jtag_core);
+				read_radio_reg(pi,
+					       RADIO_2056_TX_IQCAL_VCM_HG |
+					       jtag_core);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 2] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_IQCAL_IDAC |
-					   jtag_core);
+				read_radio_reg(pi,
+					       RADIO_2056_TX_IQCAL_IDAC |
+					       jtag_core);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 3] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_TSSI_VCM | jtag_core);
+				read_radio_reg(
+					pi,
+					RADIO_2056_TX_TSSI_VCM |
+					jtag_core);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 4] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_TX_AMP_DET |
-					   jtag_core);
+				read_radio_reg(pi,
+					       RADIO_2056_TX_TX_AMP_DET |
+					       jtag_core);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 5] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_TX_SSI_MUX |
-					   jtag_core);
+				read_radio_reg(pi,
+					       RADIO_2056_TX_TX_SSI_MUX |
+					       jtag_core);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 6] =
-			    read_radio_reg(pi, RADIO_2056_TX_TSSIA | jtag_core);
+				read_radio_reg(pi,
+					       RADIO_2056_TX_TSSIA | jtag_core);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 7] =
-			    read_radio_reg(pi, RADIO_2056_TX_TSSIG | jtag_core);
+				read_radio_reg(pi,
+					       RADIO_2056_TX_TSSIG | jtag_core);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 8] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_TSSI_MISC1 |
-					   jtag_core);
+				read_radio_reg(pi,
+					       RADIO_2056_TX_TSSI_MISC1 |
+					       jtag_core);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 9] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_TSSI_MISC2 |
-					   jtag_core);
+				read_radio_reg(pi,
+					       RADIO_2056_TX_TSSI_MISC2 |
+					       jtag_core);
 
 			pi->tx_rx_cal_radio_saveregs[(core * 11) + 10] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_TSSI_MISC3 |
-					   jtag_core);
+				read_radio_reg(pi,
+					       RADIO_2056_TX_TSSI_MISC3 |
+					       jtag_core);
 
 			if (CHSPEC_IS5G(pi->radio_chanspec)) {
 				write_radio_reg(pi,
@@ -22860,16 +23875,18 @@ static void wlc_phy_txcal_radio_setup_nphy(struct brcms_phy *pi)
 						jtag_core, 0x00);
 
 				if (PHY_IPA(pi)) {
-					write_radio_reg(pi,
-							RADIO_2056_TX_TX_SSI_MUX
-							| jtag_core, 0x4);
+					write_radio_reg(
+						pi,
+						RADIO_2056_TX_TX_SSI_MUX
+						| jtag_core, 0x4);
 					write_radio_reg(pi,
 							RADIO_2056_TX_TSSIA |
 							jtag_core, 0x1);
 				} else {
-					write_radio_reg(pi,
-							RADIO_2056_TX_TX_SSI_MUX
-							| jtag_core, 0x00);
+					write_radio_reg(
+						pi,
+						RADIO_2056_TX_TX_SSI_MUX
+						| jtag_core, 0x00);
 					write_radio_reg(pi,
 							RADIO_2056_TX_TSSIA |
 							jtag_core, 0x2f);
@@ -22909,26 +23926,27 @@ static void wlc_phy_txcal_radio_setup_nphy(struct brcms_phy *pi)
 
 				if (PHY_IPA(pi)) {
 
-					write_radio_reg(pi,
-							RADIO_2056_TX_TX_SSI_MUX
-							| jtag_core, 0x06);
-					if (NREV_LT(pi->pubpi.phy_rev, 5)) {
-
-						write_radio_reg(pi,
-								RADIO_2056_TX_TSSIG
-								| jtag_core,
-								0x11);
-					} else {
-
-						write_radio_reg(pi,
-								RADIO_2056_TX_TSSIG
-								| jtag_core,
-								0x1);
-					}
+					write_radio_reg(
+						pi,
+						RADIO_2056_TX_TX_SSI_MUX
+						| jtag_core, 0x06);
+					if (NREV_LT(pi->pubpi.phy_rev, 5))
+						write_radio_reg(
+							pi,
+							RADIO_2056_TX_TSSIG
+							| jtag_core,
+							0x11);
+					else
+						write_radio_reg(
+							pi,
+							RADIO_2056_TX_TSSIG
+							| jtag_core,
+							0x1);
 				} else {
-					write_radio_reg(pi,
-							RADIO_2056_TX_TX_SSI_MUX
-							| jtag_core, 0x00);
+					write_radio_reg(
+						pi,
+						RADIO_2056_TX_TX_SSI_MUX
+						| jtag_core, 0x00);
 					write_radio_reg(pi,
 							RADIO_2056_TX_TSSIG |
 							jtag_core, 0x20);
@@ -22948,23 +23966,23 @@ static void wlc_phy_txcal_radio_setup_nphy(struct brcms_phy *pi)
 	} else {
 
 		pi->tx_rx_cal_radio_saveregs[0] =
-		    read_radio_reg(pi, RADIO_2055_CORE1_TXRF_IQCAL1);
+			read_radio_reg(pi, RADIO_2055_CORE1_TXRF_IQCAL1);
 		write_radio_reg(pi, RADIO_2055_CORE1_TXRF_IQCAL1, 0x29);
 		pi->tx_rx_cal_radio_saveregs[1] =
-		    read_radio_reg(pi, RADIO_2055_CORE1_TXRF_IQCAL2);
+			read_radio_reg(pi, RADIO_2055_CORE1_TXRF_IQCAL2);
 		write_radio_reg(pi, RADIO_2055_CORE1_TXRF_IQCAL2, 0x54);
 
 		pi->tx_rx_cal_radio_saveregs[2] =
-		    read_radio_reg(pi, RADIO_2055_CORE2_TXRF_IQCAL1);
+			read_radio_reg(pi, RADIO_2055_CORE2_TXRF_IQCAL1);
 		write_radio_reg(pi, RADIO_2055_CORE2_TXRF_IQCAL1, 0x29);
 		pi->tx_rx_cal_radio_saveregs[3] =
-		    read_radio_reg(pi, RADIO_2055_CORE2_TXRF_IQCAL2);
+			read_radio_reg(pi, RADIO_2055_CORE2_TXRF_IQCAL2);
 		write_radio_reg(pi, RADIO_2055_CORE2_TXRF_IQCAL2, 0x54);
 
 		pi->tx_rx_cal_radio_saveregs[4] =
-		    read_radio_reg(pi, RADIO_2055_PWRDET_RXTX_CORE1);
+			read_radio_reg(pi, RADIO_2055_PWRDET_RXTX_CORE1);
 		pi->tx_rx_cal_radio_saveregs[5] =
-		    read_radio_reg(pi, RADIO_2055_PWRDET_RXTX_CORE2);
+			read_radio_reg(pi, RADIO_2055_PWRDET_RXTX_CORE2);
 
 		if ((read_phy_reg(pi, 0x09) & NPHY_BandControl_currentBand) ==
 		    0) {
@@ -23025,11 +24043,8 @@ static void wlc_phy_txcal_radio_cleanup_nphy(struct brcms_phy *pi)
 			if (pi->pubpi.radiorev != 5)
 				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
 						 TSSIA,
-						 pi->
-						 tx_rx_cal_radio_saveregs[(core
-									   *
-									   11) +
-									  6]);
+						 pi->tx_rx_cal_radio_saveregs
+							     [(core * 11) + 6]);
 
 			WRITE_RADIO_REG3(pi, RADIO_2057, TX, core, TSSIG,
 					 pi->
@@ -23043,9 +24058,8 @@ static void wlc_phy_txcal_radio_cleanup_nphy(struct brcms_phy *pi)
 		}
 	} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
 		for (core = 0; core <= 1; core++) {
-			jtag_core =
-			    (core ==
-			     PHY_CORE_0) ? RADIO_2056_TX0 : RADIO_2056_TX1;
+			jtag_core = (core == PHY_CORE_0) ?
+				     RADIO_2056_TX0 : RADIO_2056_TX1;
 
 			write_radio_reg(pi,
 					RADIO_2056_TX_TX_SSI_MASTER | jtag_core,
@@ -23171,23 +24185,22 @@ static void wlc_phy_txcal_physetup_nphy(struct brcms_phy *pi)
 		pi->tx_rx_cal_phy_saveregs[7] = read_phy_reg(pi, 0x91);
 		pi->tx_rx_cal_phy_saveregs[8] = read_phy_reg(pi, 0x92);
 
-		if (!(pi->use_int_tx_iqlo_cal_nphy)) {
-
-			wlc_phy_rfctrlintc_override_nphy(pi,
-							 NPHY_RfctrlIntc_override_PA,
-							 1,
-							 RADIO_MIMO_CORESEL_CORE1
-							 |
-							 RADIO_MIMO_CORESEL_CORE2);
-		} else {
-
-			wlc_phy_rfctrlintc_override_nphy(pi,
-							 NPHY_RfctrlIntc_override_PA,
-							 0,
-							 RADIO_MIMO_CORESEL_CORE1
-							 |
-							 RADIO_MIMO_CORESEL_CORE2);
-		}
+		if (!(pi->use_int_tx_iqlo_cal_nphy))
+			wlc_phy_rfctrlintc_override_nphy(
+				pi,
+				NPHY_RfctrlIntc_override_PA,
+				1,
+				RADIO_MIMO_CORESEL_CORE1
+				|
+				RADIO_MIMO_CORESEL_CORE2);
+		else
+			wlc_phy_rfctrlintc_override_nphy(
+				pi,
+				NPHY_RfctrlIntc_override_PA,
+				0,
+				RADIO_MIMO_CORESEL_CORE1
+				|
+				RADIO_MIMO_CORESEL_CORE2);
 
 		wlc_phy_rfctrlintc_override_nphy(pi,
 						 NPHY_RfctrlIntc_override_TRSW,
@@ -23205,12 +24218,13 @@ static void wlc_phy_txcal_physetup_nphy(struct brcms_phy *pi)
 			    0x29b, (0x1 << 0), (0) << 0);
 
 		if (NREV_IS(pi->pubpi.phy_rev, 7)
-		    || NREV_GE(pi->pubpi.phy_rev, 8)) {
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 7),
-							  wlc_phy_read_lpf_bw_ctl_nphy
-							  (pi, 0), 0, 0,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		}
+		    || NREV_GE(pi->pubpi.phy_rev, 8))
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 7),
+				wlc_phy_read_lpf_bw_ctl_nphy
+					(pi,
+					0), 0, 0,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
 
 		if (pi->use_int_tx_iqlo_cal_nphy
 		    && !(pi->internal_tx_iqlo_cal_tapoff_intpa_nphy)) {
@@ -23221,25 +24235,30 @@ static void wlc_phy_txcal_physetup_nphy(struct brcms_phy *pi)
 					      1 << 4);
 
 				if (CHSPEC_IS2G(pi->radio_chanspec)) {
-					mod_radio_reg(pi,
-						      RADIO_2057_PAD2G_TUNE_PUS_CORE0,
-						      1, 0);
-					mod_radio_reg(pi,
-						      RADIO_2057_PAD2G_TUNE_PUS_CORE1,
-						      1, 0);
+					mod_radio_reg(
+						pi,
+						RADIO_2057_PAD2G_TUNE_PUS_CORE0,
+						1, 0);
+					mod_radio_reg(
+						pi,
+						RADIO_2057_PAD2G_TUNE_PUS_CORE1,
+						1, 0);
 				} else {
-					mod_radio_reg(pi,
-						      RADIO_2057_IPA5G_CASCOFFV_PU_CORE0,
-						      1, 0);
-					mod_radio_reg(pi,
-						      RADIO_2057_IPA5G_CASCOFFV_PU_CORE1,
-						      1, 0);
+					mod_radio_reg(
+					     pi,
+					     RADIO_2057_IPA5G_CASCOFFV_PU_CORE0,
+					     1, 0);
+					mod_radio_reg(
+					     pi,
+					     RADIO_2057_IPA5G_CASCOFFV_PU_CORE1,
+					     1, 0);
 				}
 			} else if (NREV_GE(pi->pubpi.phy_rev, 8)) {
-				wlc_phy_rfctrl_override_nphy_rev7(pi,
-								  (0x1 << 3), 0,
-								  0x3, 0,
-								  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+				wlc_phy_rfctrl_override_nphy_rev7(
+					pi,
+					(0x1 << 3), 0,
+					0x3, 0,
+					NPHY_REV7_RFCTRLOVERRIDE_ID0);
 			}
 		}
 	} else {
@@ -23302,11 +24321,11 @@ static void wlc_phy_txcal_phycleanup_nphy(struct brcms_phy *pi)
 		write_phy_reg(pi, 0x29b, pi->tx_rx_cal_phy_saveregs[10]);
 
 		if (NREV_IS(pi->pubpi.phy_rev, 7)
-		    || NREV_GE(pi->pubpi.phy_rev, 8)) {
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 7), 0, 0,
-							  1,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		}
+		    || NREV_GE(pi->pubpi.phy_rev, 8))
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 7), 0, 0,
+				1,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
 
 		wlc_phy_resetcca_nphy(pi);
 
@@ -23315,28 +24334,33 @@ static void wlc_phy_txcal_phycleanup_nphy(struct brcms_phy *pi)
 
 			if (NREV_IS(pi->pubpi.phy_rev, 7)) {
 				if (CHSPEC_IS2G(pi->radio_chanspec)) {
-					mod_radio_reg(pi,
-						      RADIO_2057_PAD2G_TUNE_PUS_CORE0,
-						      1, 1);
-					mod_radio_reg(pi,
-						      RADIO_2057_PAD2G_TUNE_PUS_CORE1,
-						      1, 1);
+					mod_radio_reg(
+						pi,
+						RADIO_2057_PAD2G_TUNE_PUS_CORE0,
+						1, 1);
+					mod_radio_reg(
+						pi,
+						RADIO_2057_PAD2G_TUNE_PUS_CORE1,
+						1, 1);
 				} else {
-					mod_radio_reg(pi,
-						      RADIO_2057_IPA5G_CASCOFFV_PU_CORE0,
-						      1, 1);
-					mod_radio_reg(pi,
-						      RADIO_2057_IPA5G_CASCOFFV_PU_CORE1,
-						      1, 1);
+					mod_radio_reg(
+					     pi,
+					     RADIO_2057_IPA5G_CASCOFFV_PU_CORE0,
+					     1, 1);
+					mod_radio_reg(
+					     pi,
+					     RADIO_2057_IPA5G_CASCOFFV_PU_CORE1,
+					     1, 1);
 				}
 
 				mod_radio_reg(pi, RADIO_2057_OVR_REG0, 1 << 4,
 					      0);
 			} else if (NREV_GE(pi->pubpi.phy_rev, 8)) {
-				wlc_phy_rfctrl_override_nphy_rev7(pi,
-								  (0x1 << 3), 0,
-								  0x3, 1,
-								  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+				wlc_phy_rfctrl_override_nphy_rev7(
+					pi,
+					(0x1 << 3), 0,
+					0x3, 1,
+					NPHY_REV7_RFCTRLOVERRIDE_ID0);
 			}
 		}
 	} else {
@@ -23356,10 +24380,6 @@ static void wlc_phy_txcal_phycleanup_nphy(struct brcms_phy *pi)
 	}
 }
 
-#define NPHY_CAL_TSSISAMPS	64
-#define NPHY_TEST_TONE_FREQ_40MHz 4000
-#define NPHY_TEST_TONE_FREQ_20MHz 2500
-
 void
 wlc_phy_est_tonepwr_nphy(struct brcms_phy *pi, s32 *qdBm_pwrbuf, u8 num_samps)
 {
@@ -23379,8 +24399,8 @@ wlc_phy_est_tonepwr_nphy(struct brcms_phy *pi, s32 *qdBm_pwrbuf, u8 num_samps)
 	idle_tssi[1] = (temp <= 31) ? temp : (temp - 64);
 
 	tssi_type =
-	    CHSPEC_IS5G(pi->radio_chanspec) ?
-	    (u8)NPHY_RSSI_SEL_TSSI_5G : (u8)NPHY_RSSI_SEL_TSSI_2G;
+		CHSPEC_IS5G(pi->radio_chanspec) ?
+		(u8)NPHY_RSSI_SEL_TSSI_5G : (u8)NPHY_RSSI_SEL_TSSI_2G;
 
 	wlc_phy_poll_rssi_nphy(pi, tssi_type, rssi_buf, num_samps);
 
@@ -23390,17 +24410,15 @@ wlc_phy_est_tonepwr_nphy(struct brcms_phy *pi, s32 *qdBm_pwrbuf, u8 num_samps)
 	pwrindex[0] = idle_tssi[0] - tssival[0] + 64;
 	pwrindex[1] = idle_tssi[1] - tssival[1] + 64;
 
-	if (pwrindex[0] < 0) {
+	if (pwrindex[0] < 0)
 		pwrindex[0] = 0;
-	} else if (pwrindex[0] > 63) {
+	else if (pwrindex[0] > 63)
 		pwrindex[0] = 63;
-	}
 
-	if (pwrindex[1] < 0) {
+	if (pwrindex[1] < 0)
 		pwrindex[1] = 0;
-	} else if (pwrindex[1] > 63) {
+	else if (pwrindex[1] > 63)
 		pwrindex[1] = 63;
-	}
 
 	wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_CORE1TXPWRCTL, 1,
 				(u32) pwrindex[0], 32, &qdBm_pwrbuf[0]);
@@ -23408,326 +24426,1241 @@ wlc_phy_est_tonepwr_nphy(struct brcms_phy *pi, s32 *qdBm_pwrbuf, u8 num_samps)
 				(u32) pwrindex[1], 32, &qdBm_pwrbuf[1]);
 }
 
-static void wlc_phy_internal_cal_txgain_nphy(struct brcms_phy *pi)
+static void wlc_phy_update_txcal_ladder_nphy(struct brcms_phy *pi, u16 core)
 {
-	u16 txcal_gain[2];
+	int index;
+	u32 bbmult_scale;
+	u16 bbmult;
+	u16 tblentry;
 
-	pi->nphy_txcal_pwr_idx[0] = pi->nphy_cal_orig_pwr_idx[0];
-	pi->nphy_txcal_pwr_idx[1] = pi->nphy_cal_orig_pwr_idx[0];
-	wlc_phy_txpwr_index_nphy(pi, 1, pi->nphy_cal_orig_pwr_idx[0], true);
-	wlc_phy_txpwr_index_nphy(pi, 2, pi->nphy_cal_orig_pwr_idx[1], true);
+	struct nphy_txiqcal_ladder ladder_lo[] = {
+		{3, 0}, {4, 0}, {6, 0}, {9, 0}, {13, 0}, {18, 0},
+		{25, 0}, {25, 1}, {25, 2}, {25, 3}, {25, 4}, {25, 5},
+		{25, 6}, {25, 7}, {35, 7}, {50, 7}, {71, 7}, {100, 7}
+	};
 
-	wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_RFSEQ, 2, 0x110, 16,
-				txcal_gain);
+	struct nphy_txiqcal_ladder ladder_iq[] = {
+		{3, 0}, {4, 0}, {6, 0}, {9, 0}, {13, 0}, {18, 0},
+		{25, 0}, {35, 0}, {50, 0}, {71, 0}, {100, 0}, {100, 1},
+		{100, 2}, {100, 3}, {100, 4}, {100, 5}, {100, 6}, {100, 7}
+	};
 
-	if (CHSPEC_IS2G(pi->radio_chanspec)) {
-		txcal_gain[0] = (txcal_gain[0] & 0xF000) | 0x0F40;
-		txcal_gain[1] = (txcal_gain[1] & 0xF000) | 0x0F40;
-	} else {
-		txcal_gain[0] = (txcal_gain[0] & 0xF000) | 0x0F60;
-		txcal_gain[1] = (txcal_gain[1] & 0xF000) | 0x0F60;
+	bbmult = (core == PHY_CORE_0) ?
+		 ((pi->nphy_txcal_bbmult >> 8) & 0xff) :
+		 (pi->nphy_txcal_bbmult & 0xff);
+
+	for (index = 0; index < 18; index++) {
+		bbmult_scale = ladder_lo[index].percent * bbmult;
+		bbmult_scale /= 100;
+
+		tblentry =
+			((bbmult_scale & 0xff) << 8) | ladder_lo[index].g_env;
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_IQLOCAL, 1, index, 16,
+					 &tblentry);
+
+		bbmult_scale = ladder_iq[index].percent * bbmult;
+		bbmult_scale /= 100;
+
+		tblentry =
+			((bbmult_scale & 0xff) << 8) | ladder_iq[index].g_env;
+		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_IQLOCAL, 1, index + 32,
+					 16, &tblentry);
 	}
+}
 
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ, 2, 0x110, 16,
-				 txcal_gain);
+static u8 wlc_phy_txpwr_idx_cur_get_nphy(struct brcms_phy *pi, u8 core)
+{
+	u16 tmp;
+	tmp = read_phy_reg(pi, ((core == PHY_CORE_0) ? 0x1ed : 0x1ee));
+
+	tmp = (tmp & (0x7f << 8)) >> 8;
+	return (u8) tmp;
 }
 
-static void wlc_phy_precal_txgain_nphy(struct brcms_phy *pi)
+static void
+wlc_phy_txpwr_idx_cur_set_nphy(struct brcms_phy *pi, u8 idx0, u8 idx1)
 {
-	bool save_bbmult = false;
-	u8 txcal_index_2057_rev5n7 = 0;
-	u8 txcal_index_2057_rev3n4n6 = 10;
+	mod_phy_reg(pi, 0x1e7, (0x7f << 0), idx0);
+
+	if (NREV_GT(pi->pubpi.phy_rev, 1))
+		mod_phy_reg(pi, 0x222, (0xff << 0), idx1);
+}
+
+static u16 wlc_phy_ipa_get_bbmult_nphy(struct brcms_phy *pi)
+{
+	u16 m0m1;
+
+	wlc_phy_table_read_nphy(pi, 15, 1, 87, 16, &m0m1);
+
+	return m0m1;
+}
+
+static void wlc_phy_ipa_set_bbmult_nphy(struct brcms_phy *pi, u8 m0, u8 m1)
+{
+	u16 m0m1 = (u16) ((m0 << 8) | m1);
+
+	wlc_phy_table_write_nphy(pi, 15, 1, 87, 16, &m0m1);
+	wlc_phy_table_write_nphy(pi, 15, 1, 95, 16, &m0m1);
+}
+
+static void
+wlc_phy_papd_cal_setup_nphy(struct brcms_phy *pi,
+			    struct nphy_papd_restore_state *state, u8 core)
+{
+	s32 tone_freq;
+	u8 off_core;
+	u16 mixgain = 0;
+
+	off_core = core ^ 0x1;
+	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+
+		if (NREV_IS(pi->pubpi.phy_rev, 7)
+		    || NREV_GE(pi->pubpi.phy_rev, 8))
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 7),
+				wlc_phy_read_lpf_bw_ctl_nphy
+					(pi,
+					0), 0, 0,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
+
+		if (CHSPEC_IS2G(pi->radio_chanspec)) {
+			if (pi->pubpi.radiorev == 5)
+				mixgain = (core == 0) ? 0x20 : 0x00;
+			else if ((pi->pubpi.radiorev == 7)
+				 || (pi->pubpi.radiorev == 8))
+				mixgain = 0x00;
+			else if ((pi->pubpi.radiorev <= 4)
+				 || (pi->pubpi.radiorev == 6))
+				mixgain = 0x00;
+		} else {
+			if ((pi->pubpi.radiorev == 4) ||
+			    (pi->pubpi.radiorev == 6))
+				mixgain = 0x50;
+			else if ((pi->pubpi.radiorev == 3)
+				 || (pi->pubpi.radiorev == 7)
+				 || (pi->pubpi.radiorev == 8))
+				mixgain = 0x0;
+		}
+
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 11),
+						  mixgain, (1 << core), 0,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+
+		wlc_phy_rfctrl_override_1tomany_nphy(
+			pi,
+			NPHY_REV7_RfctrlOverride_cmd_tx_pu,
+			1, (1 << core), 0);
+		wlc_phy_rfctrl_override_1tomany_nphy(
+			pi,
+			NPHY_REV7_RfctrlOverride_cmd_tx_pu,
+			0, (1 << off_core), 0);
+
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 3),
+						  0, 0x3, 0,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 2), 1,
+						  (1 << core), 0,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 0), 0,
+						  (1 << core), 0,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 1), 1,
+						  (1 << core), 0,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID2);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 8), 0,
+						  (1 << core), 0,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 9), 1,
+						  (1 << core), 0,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 10), 0,
+						  (1 << core), 0,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 3), 1,
+						  (1 << core), 0,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 5),
+						  0, (1 << core), 0,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 4), 0,
+						  (1 << core), 0,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+
+		state->afectrl[core] = read_phy_reg(pi, (core == PHY_CORE_0) ?
+						    0xa6 : 0xa7);
+		state->afeoverride[core] =
+			read_phy_reg(pi, (core == PHY_CORE_0) ? 0x8f : 0xa5);
+		state->afectrl[off_core] =
+			read_phy_reg(pi, (core == PHY_CORE_0) ? 0xa7 : 0xa6);
+		state->afeoverride[off_core] =
+			read_phy_reg(pi, (core == PHY_CORE_0) ? 0xa5 : 0x8f);
+
+		mod_phy_reg(pi, ((core == PHY_CORE_0) ? 0xa6 : 0xa7),
+			    (0x1 << 2), 0);
+		mod_phy_reg(pi, ((core == PHY_CORE_0) ? 0x8f :
+				 0xa5), (0x1 << 2), (0x1 << 2));
+
+		mod_phy_reg(pi, ((core == PHY_CORE_0) ? 0xa7 : 0xa6),
+			    (0x1 << 2), (0x1 << 2));
+		mod_phy_reg(pi, ((core == PHY_CORE_0) ? 0xa5 :
+				 0x8f), (0x1 << 2), (0x1 << 2));
+
+		if (CHSPEC_IS2G(pi->radio_chanspec)) {
+			state->pwrup[core] =
+				READ_RADIO_REG3(pi, RADIO_2057, TX, core,
+						TXRXCOUPLE_2G_PWRUP);
+			state->atten[core] =
+				READ_RADIO_REG3(pi, RADIO_2057, TX, core,
+						TXRXCOUPLE_2G_ATTEN);
+			state->pwrup[off_core] =
+				READ_RADIO_REG3(pi, RADIO_2057, TX, off_core,
+						TXRXCOUPLE_2G_PWRUP);
+			state->atten[off_core] =
+				READ_RADIO_REG3(pi, RADIO_2057, TX, off_core,
+						TXRXCOUPLE_2G_ATTEN);
+
+			WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+					 TXRXCOUPLE_2G_PWRUP, 0xc);
 
-	if (pi->use_int_tx_iqlo_cal_nphy) {
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
 			if ((pi->pubpi.radiorev == 3) ||
 			    (pi->pubpi.radiorev == 4) ||
-			    (pi->pubpi.radiorev == 6)) {
+			    (pi->pubpi.radiorev == 6))
+				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+						 TXRXCOUPLE_2G_ATTEN, 0xf0);
+			else if (pi->pubpi.radiorev == 5)
+				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+						 TXRXCOUPLE_2G_ATTEN,
+						 (core == 0) ? 0xf7 : 0xf2);
+			else if ((pi->pubpi.radiorev == 7)
+				 || (pi->pubpi.radiorev == 8))
+				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+						 TXRXCOUPLE_2G_ATTEN, 0xf0);
 
-				pi->nphy_txcal_pwr_idx[0] =
-				    txcal_index_2057_rev3n4n6;
-				pi->nphy_txcal_pwr_idx[1] =
-				    txcal_index_2057_rev3n4n6;
-				wlc_phy_txpwr_index_nphy(pi, 3,
-							 txcal_index_2057_rev3n4n6,
-							 false);
-			} else {
+			WRITE_RADIO_REG3(pi, RADIO_2057, TX, off_core,
+					 TXRXCOUPLE_2G_PWRUP, 0x0);
+			WRITE_RADIO_REG3(pi, RADIO_2057, TX, off_core,
+					 TXRXCOUPLE_2G_ATTEN, 0xff);
+		} else {
+			state->pwrup[core] =
+				READ_RADIO_REG3(pi, RADIO_2057, TX, core,
+						TXRXCOUPLE_5G_PWRUP);
+			state->atten[core] =
+				READ_RADIO_REG3(pi, RADIO_2057, TX, core,
+						TXRXCOUPLE_5G_ATTEN);
+			state->pwrup[off_core] =
+				READ_RADIO_REG3(pi, RADIO_2057, TX, off_core,
+						TXRXCOUPLE_5G_PWRUP);
+			state->atten[off_core] =
+				READ_RADIO_REG3(pi, RADIO_2057, TX, off_core,
+						TXRXCOUPLE_5G_ATTEN);
 
-				pi->nphy_txcal_pwr_idx[0] =
-				    txcal_index_2057_rev5n7;
-				pi->nphy_txcal_pwr_idx[1] =
-				    txcal_index_2057_rev5n7;
-				wlc_phy_txpwr_index_nphy(pi, 3,
-							 txcal_index_2057_rev5n7,
-							 false);
-			}
-			save_bbmult = true;
+			WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+					 TXRXCOUPLE_5G_PWRUP, 0xc);
 
-		} else if (NREV_LT(pi->pubpi.phy_rev, 5)) {
-			wlc_phy_cal_txgainctrl_nphy(pi, 11, false);
-			if (pi->sh->hw_phytxchain != 3) {
-				pi->nphy_txcal_pwr_idx[1] =
-				    pi->nphy_txcal_pwr_idx[0];
-				wlc_phy_txpwr_index_nphy(pi, 3,
-							 pi->
-							 nphy_txcal_pwr_idx[0],
-							 true);
-				save_bbmult = true;
+			if ((pi->pubpi.radiorev == 7)
+			    || (pi->pubpi.radiorev == 8))
+				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+						 TXRXCOUPLE_5G_ATTEN, 0xf4);
+
+			else
+				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+						 TXRXCOUPLE_5G_ATTEN, 0xf0);
+
+			WRITE_RADIO_REG3(pi, RADIO_2057, TX, off_core,
+					 TXRXCOUPLE_5G_PWRUP, 0x0);
+			WRITE_RADIO_REG3(pi, RADIO_2057, TX, off_core,
+					 TXRXCOUPLE_5G_ATTEN, 0xff);
+		}
+
+		tone_freq = 4000;
+
+		wlc_phy_tx_tone_nphy(pi, tone_freq, 181, 0, 0, false);
+
+		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x297 :
+			    0x29b, (0x1 << 0), (NPHY_PAPD_COMP_ON) << 0);
+
+		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
+			    0x2a4, (0x1 << 13), (1) << 13);
+
+		mod_phy_reg(pi, (off_core == PHY_CORE_0) ? 0x297 :
+			    0x29b, (0x1 << 0), (NPHY_PAPD_COMP_OFF) << 0);
+
+		mod_phy_reg(pi, (off_core == PHY_CORE_0) ? 0x2a3 :
+			    0x2a4, (0x1 << 13), (0) << 13);
+
+	} else {
+
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 12), 0, 0x3, 0);
+
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 3), 1, 0, 0);
+
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 0), 0, 0x3, 0);
+
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 2), 1, 0x3, 0);
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 1), 1, 0x3, 0);
+
+		state->afectrl[core] = read_phy_reg(pi, (core == PHY_CORE_0) ?
+						    0xa6 : 0xa7);
+		state->afeoverride[core] =
+			read_phy_reg(pi, (core == PHY_CORE_0) ? 0x8f : 0xa5);
+
+		mod_phy_reg(pi, ((core == PHY_CORE_0) ? 0xa6 : 0xa7),
+			    (0x1 << 0) | (0x1 << 1) | (0x1 << 2), 0);
+		mod_phy_reg(pi, ((core == PHY_CORE_0) ? 0x8f :
+				 0xa5),
+			    (0x1 << 0) |
+			    (0x1 << 1) |
+			    (0x1 << 2), (0x1 << 0) | (0x1 << 1) | (0x1 << 2));
+
+		state->vga_master[core] =
+			READ_RADIO_REG2(pi, RADIO_2056, RX, core, VGA_MASTER);
+		WRITE_RADIO_REG2(pi, RADIO_2056, RX, core, VGA_MASTER, 0x2b);
+		if (CHSPEC_IS2G(pi->radio_chanspec)) {
+			state->fbmix[core] =
+				READ_RADIO_REG2(pi, RADIO_2056, RX, core,
+						TXFBMIX_G);
+			state->intpa_master[core] =
+				READ_RADIO_REG2(pi, RADIO_2056, TX, core,
+						INTPAG_MASTER);
+
+			WRITE_RADIO_REG2(pi, RADIO_2056, RX, core, TXFBMIX_G,
+					 0x03);
+			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
+					 INTPAG_MASTER, 0x04);
+		} else {
+			state->fbmix[core] =
+				READ_RADIO_REG2(pi, RADIO_2056, RX, core,
+						TXFBMIX_A);
+			state->intpa_master[core] =
+				READ_RADIO_REG2(pi, RADIO_2056, TX, core,
+						INTPAA_MASTER);
+
+			WRITE_RADIO_REG2(pi, RADIO_2056, RX, core, TXFBMIX_A,
+					 0x03);
+			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
+					 INTPAA_MASTER, 0x04);
+
+		}
+
+		tone_freq = 4000;
+
+		wlc_phy_tx_tone_nphy(pi, tone_freq, 181, 0, 0, false);
+
+		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x297 :
+			    0x29b, (0x1 << 0), (1) << 0);
+
+		mod_phy_reg(pi, (off_core == PHY_CORE_0) ? 0x297 :
+			    0x29b, (0x1 << 0), (0) << 0);
+
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 3), 0, 0x3, 0);
+	}
+}
+
+static void
+wlc_phy_papd_cal_cleanup_nphy(struct brcms_phy *pi,
+			      struct nphy_papd_restore_state *state)
+{
+	u8 core;
+
+	wlc_phy_stopplayback_nphy(pi);
+
+	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+
+		for (core = 0; core < pi->pubpi.phy_corenum; core++) {
+
+			if (CHSPEC_IS2G(pi->radio_chanspec)) {
+				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+						 TXRXCOUPLE_2G_PWRUP, 0);
+				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+						 TXRXCOUPLE_2G_ATTEN,
+						 state->atten[core]);
+			} else {
+				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+						 TXRXCOUPLE_5G_PWRUP, 0);
+				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
+						 TXRXCOUPLE_5G_ATTEN,
+						 state->atten[core]);
 			}
+		}
 
-		} else if (NREV_IS(pi->pubpi.phy_rev, 5)) {
-			if (PHY_IPA(pi)) {
-				if (CHSPEC_IS2G(pi->radio_chanspec)) {
-					wlc_phy_cal_txgainctrl_nphy(pi, 12,
-								    false);
-				} else {
-					pi->nphy_txcal_pwr_idx[0] = 80;
-					pi->nphy_txcal_pwr_idx[1] = 80;
-					wlc_phy_txpwr_index_nphy(pi, 3, 80,
-								 false);
-					save_bbmult = true;
-				}
+		if ((pi->pubpi.radiorev == 4) || (pi->pubpi.radiorev == 6))
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 2),
+				1, 0x3, 0,
+				NPHY_REV7_RFCTRLOVERRIDE_ID0);
+		else
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 2),
+				0, 0x3, 1,
+				NPHY_REV7_RFCTRLOVERRIDE_ID0);
+
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 1),
+						  0, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 0), 0, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID2);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 2), 0, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID2);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 11), 1, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 3), 0, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 11), 0, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 12), 0, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 2), 1, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 0), 0, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 1), 1, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID2);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 8), 0, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 9), 1, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 10), 0, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 3), 1, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 5), 0, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 4), 0, 0x3, 1,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
+
+		for (core = 0; core < pi->pubpi.phy_corenum; core++) {
+
+			write_phy_reg(pi, (core == PHY_CORE_0) ?
+				      0xa6 : 0xa7, state->afectrl[core]);
+			write_phy_reg(pi, (core == PHY_CORE_0) ? 0x8f :
+				      0xa5, state->afeoverride[core]);
+		}
+
+		wlc_phy_ipa_set_bbmult_nphy(pi, (state->mm >> 8) & 0xff,
+					    (state->mm & 0xff));
+
+		if (NREV_IS(pi->pubpi.phy_rev, 7)
+		    || NREV_GE(pi->pubpi.phy_rev, 8))
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi, (0x1 << 7), 0, 0,
+				1,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
+	} else {
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 12), 0, 0x3, 1);
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 13), 0, 0x3, 1);
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 0), 0, 0x3, 1);
+
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 2), 0, 0x3, 1);
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 1), 0, 0x3, 1);
+
+		for (core = 0; core < pi->pubpi.phy_corenum; core++) {
+
+			WRITE_RADIO_REG2(pi, RADIO_2056, RX, core, VGA_MASTER,
+					 state->vga_master[core]);
+			if (CHSPEC_IS2G(pi->radio_chanspec)) {
+				WRITE_RADIO_REG2(pi, RADIO_2056, RX, core,
+						 TXFBMIX_G, state->fbmix[core]);
+				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
+						 INTPAG_MASTER,
+						 state->intpa_master[core]);
 			} else {
+				WRITE_RADIO_REG2(pi, RADIO_2056, RX, core,
+						 TXFBMIX_A, state->fbmix[core]);
+				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
+						 INTPAA_MASTER,
+						 state->intpa_master[core]);
+			}
 
-				wlc_phy_internal_cal_txgain_nphy(pi);
-				save_bbmult = true;
+			write_phy_reg(pi, (core == PHY_CORE_0) ?
+				      0xa6 : 0xa7, state->afectrl[core]);
+			write_phy_reg(pi, (core == PHY_CORE_0) ? 0x8f :
+				      0xa5, state->afeoverride[core]);
+		}
+
+		wlc_phy_ipa_set_bbmult_nphy(pi, (state->mm >> 8) & 0xff,
+					    (state->mm & 0xff));
+
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 3), 0, 0x3, 1);
+	}
+}
+
+static void
+wlc_phy_a1_nphy(struct brcms_phy *pi, u8 core, u32 winsz, u32 start,
+		u32 end)
+{
+	u32 *buf, *src, *dst, sz;
+
+	sz = end - start + 1;
+
+	buf = kmalloc(2 * sizeof(u32) * NPHY_PAPD_EPS_TBL_SIZE, GFP_ATOMIC);
+	if (NULL == buf)
+		return;
+
+	src = buf;
+	dst = buf + NPHY_PAPD_EPS_TBL_SIZE;
+
+	wlc_phy_table_read_nphy(pi,
+				(core ==
+				 PHY_CORE_0 ? NPHY_TBL_ID_EPSILONTBL0 :
+				 NPHY_TBL_ID_EPSILONTBL1),
+				NPHY_PAPD_EPS_TBL_SIZE, 0, 32, src);
+
+	do {
+		u32 phy_a1, phy_a2;
+		s32 phy_a3, phy_a4, phy_a5, phy_a6, phy_a7;
+
+		phy_a1 = end - min(end, (winsz >> 1));
+		phy_a2 = min_t(u32, NPHY_PAPD_EPS_TBL_SIZE - 1,
+			       end + (winsz >> 1));
+		phy_a3 = phy_a2 - phy_a1 + 1;
+		phy_a6 = 0;
+		phy_a7 = 0;
+
+		do {
+			wlc_phy_papd_decode_epsilon(src[phy_a2], &phy_a4,
+						    &phy_a5);
+			phy_a6 += phy_a4;
+			phy_a7 += phy_a5;
+		} while (phy_a2-- != phy_a1);
+
+		phy_a6 /= phy_a3;
+		phy_a7 /= phy_a3;
+		dst[end] = ((u32) phy_a7 << 13) | ((u32) phy_a6 & 0x1fff);
+	} while (end-- != start);
+
+	wlc_phy_table_write_nphy(pi,
+				 (core ==
+				  PHY_CORE_0) ? NPHY_TBL_ID_EPSILONTBL0 :
+				 NPHY_TBL_ID_EPSILONTBL1, sz, start, 32, dst);
+
+	kfree(buf);
+}
+
+static void
+wlc_phy_a2_nphy(struct brcms_phy *pi, struct nphy_ipa_txcalgains *txgains,
+		enum phy_cal_mode cal_mode, u8 core)
+{
+	u16 phy_a1, phy_a2, phy_a3;
+	u16 phy_a4, phy_a5;
+	bool phy_a6;
+	u8 phy_a7, m[2];
+	u32 phy_a8 = 0;
+	struct nphy_txgains phy_a9;
+
+	if (NREV_LT(pi->pubpi.phy_rev, 3))
+		return;
+
+	phy_a7 = (core == PHY_CORE_0) ? 1 : 0;
+
+	phy_a6 = ((cal_mode == CAL_GCTRL)
+		  || (cal_mode == CAL_SOFT)) ? true : false;
+
+	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+
+		phy_a9 = wlc_phy_get_tx_gain_nphy(pi);
+
+		if (CHSPEC_IS2G(pi->radio_chanspec))
+			phy_a5 = ((phy_a9.txlpf[core] << 15) |
+				  (phy_a9.txgm[core] << 12) |
+				  (phy_a9.pga[core] << 8) |
+				  (txgains->gains.pad[core] << 3) |
+				  (phy_a9.ipa[core]));
+		else
+			phy_a5 = ((phy_a9.txlpf[core] << 15) |
+				  (phy_a9.txgm[core] << 12) |
+				  (txgains->gains.pga[core] << 8) |
+				  (phy_a9.pad[core] << 3) | (phy_a9.ipa[core]));
+
+		wlc_phy_rfctrl_override_1tomany_nphy(
+			pi,
+			NPHY_REV7_RfctrlOverride_cmd_txgain,
+			phy_a5, (1 << core), 0);
+
+		if (CHSPEC_IS2G(pi->radio_chanspec)) {
+			if ((pi->pubpi.radiorev <= 4)
+			    || (pi->pubpi.radiorev == 6))
+				m[core] = (pi->bw == WL_CHANSPEC_BW_40) ?
+					  60 : 79;
+			else
+				m[core] = (pi->bw == WL_CHANSPEC_BW_40) ?
+					  45 : 64;
+		} else {
+			m[core] = (pi->bw == WL_CHANSPEC_BW_40) ? 75 : 107;
+		}
+
+		m[phy_a7] = 0;
+		wlc_phy_ipa_set_bbmult_nphy(pi, m[0], m[1]);
+
+		phy_a2 = 63;
+
+		if (CHSPEC_IS2G(pi->radio_chanspec)) {
+			if ((pi->pubpi.radiorev == 4)
+			    || (pi->pubpi.radiorev == 6)) {
+				phy_a1 = 30;
+				phy_a3 = 30;
+			} else {
+				phy_a1 = 25;
+				phy_a3 = 25;
+			}
+		} else {
+			if ((pi->pubpi.radiorev == 5)
+			    || (pi->pubpi.radiorev == 7)
+			    || (pi->pubpi.radiorev == 8)) {
+				phy_a1 = 25;
+				phy_a3 = 25;
+			} else {
+				phy_a1 = 35;
+				phy_a3 = 35;
 			}
+		}
 
-		} else if (NREV_IS(pi->pubpi.phy_rev, 6)) {
-			if (PHY_IPA(pi)) {
+		if (cal_mode == CAL_GCTRL) {
+			if ((pi->pubpi.radiorev == 5)
+			    && (CHSPEC_IS2G(pi->radio_chanspec)))
+				phy_a1 = 55;
+			else if (((pi->pubpi.radiorev == 7) &&
+				  (CHSPEC_IS2G(pi->radio_chanspec))) ||
+				 ((pi->pubpi.radiorev == 8) &&
+				  (CHSPEC_IS2G(pi->radio_chanspec))))
+				phy_a1 = 60;
+			else
+				phy_a1 = 63;
+
+		} else if ((cal_mode != CAL_FULL) && (cal_mode != CAL_SOFT)) {
+
+			phy_a1 = 35;
+			phy_a3 = 35;
+		}
+
+		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x297 :
+			    0x29b, (0x1 << 0), (1) << 0);
+
+		mod_phy_reg(pi, (phy_a7 == PHY_CORE_0) ? 0x297 :
+			    0x29b, (0x1 << 0), (0) << 0);
+
+		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
+			    0x2a4, (0x1 << 13), (1) << 13);
+
+		mod_phy_reg(pi, (phy_a7 == PHY_CORE_0) ? 0x2a3 :
+			    0x2a4, (0x1 << 13), (0) << 13);
+
+		write_phy_reg(pi, 0x2a1, 0x80);
+		write_phy_reg(pi, 0x2a2, 0x100);
+
+		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
+			    0x2a4, (0x7 << 4), (11) << 4);
+
+		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
+			    0x2a4, (0x7 << 8), (11) << 8);
+
+		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
+			    0x2a4, (0x7 << 0), (0x3) << 0);
+
+		write_phy_reg(pi, 0x2e5, 0x20);
+
+		mod_phy_reg(pi, 0x2a0, (0x3f << 0), (phy_a3) << 0);
+
+		mod_phy_reg(pi, 0x29f, (0x3f << 0), (phy_a1) << 0);
+
+		mod_phy_reg(pi, 0x29f, (0x3f << 8), (phy_a2) << 8);
+
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 3),
+						  1, ((core == 0) ? 1 : 2), 0,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 3),
+						  0, ((core == 0) ? 2 : 1), 0,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+
+		write_phy_reg(pi, 0x2be, 1);
+		SPINWAIT(read_phy_reg(pi, 0x2be), 10 * 1000 * 1000);
+
+		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 3),
+						  0, 0x3, 0,
+						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
+
+		wlc_phy_table_write_nphy(pi,
+					 (core ==
+					  PHY_CORE_0) ? NPHY_TBL_ID_EPSILONTBL0
+					 : NPHY_TBL_ID_EPSILONTBL1, 1, phy_a3,
+					 32, &phy_a8);
+
+		if (cal_mode != CAL_GCTRL) {
+			if (CHSPEC_IS5G(pi->radio_chanspec))
+				wlc_phy_a1_nphy(pi, core, 5, 0, 35);
+		}
+
+		wlc_phy_rfctrl_override_1tomany_nphy(
+			pi,
+			NPHY_REV7_RfctrlOverride_cmd_txgain,
+			phy_a5, (1 << core), 1);
+
+	} else {
+
+		if (txgains) {
+			if (txgains->useindex) {
+				phy_a4 = 15 - ((txgains->index) >> 3);
 				if (CHSPEC_IS2G(pi->radio_chanspec)) {
-					wlc_phy_cal_txgainctrl_nphy(pi, 12,
-								    false);
+					if (NREV_GE(pi->pubpi.phy_rev, 6))
+						phy_a5 = 0x00f7 | (phy_a4 << 8);
+
+					else
+					if (NREV_IS(pi->pubpi.phy_rev, 5))
+						phy_a5 = 0x10f7 | (phy_a4 << 8);
+					else
+						phy_a5 = 0x50f7 | (phy_a4 << 8);
 				} else {
-					wlc_phy_cal_txgainctrl_nphy(pi, 14,
-								    false);
+					phy_a5 = 0x70f7 | (phy_a4 << 8);
 				}
+				wlc_phy_rfctrl_override_nphy(pi,
+							     (0x1 << 13),
+							     phy_a5,
+							     (1 << core), 0);
 			} else {
-
-				wlc_phy_internal_cal_txgain_nphy(pi);
-				save_bbmult = true;
+				wlc_phy_rfctrl_override_nphy(pi,
+							     (0x1 << 13),
+							     0x5bf7,
+							     (1 << core), 0);
 			}
 		}
 
-	} else {
-		wlc_phy_cal_txgainctrl_nphy(pi, 10, false);
-	}
+		if (CHSPEC_IS2G(pi->radio_chanspec))
+			m[core] = (pi->bw == WL_CHANSPEC_BW_40) ? 45 : 64;
+		else
+			m[core] = (pi->bw == WL_CHANSPEC_BW_40) ? 75 : 107;
 
-	if (save_bbmult) {
-		wlc_phy_table_read_nphy(pi, 15, 1, 87, 16,
-					&pi->nphy_txcal_bbmult);
+		m[phy_a7] = 0;
+		wlc_phy_ipa_set_bbmult_nphy(pi, m[0], m[1]);
+
+		phy_a2 = 63;
+
+		if (cal_mode == CAL_FULL) {
+			phy_a1 = 25;
+			phy_a3 = 25;
+		} else if (cal_mode == CAL_SOFT) {
+			phy_a1 = 25;
+			phy_a3 = 25;
+		} else if (cal_mode == CAL_GCTRL) {
+			phy_a1 = 63;
+			phy_a3 = 25;
+		} else {
+
+			phy_a1 = 25;
+			phy_a3 = 25;
+		}
+
+		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x297 :
+			    0x29b, (0x1 << 0), (1) << 0);
+
+		mod_phy_reg(pi, (phy_a7 == PHY_CORE_0) ? 0x297 :
+			    0x29b, (0x1 << 0), (0) << 0);
+
+		if (NREV_GE(pi->pubpi.phy_rev, 6)) {
+			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
+				    0x2a4, (0x1 << 13), (1) << 13);
+
+			mod_phy_reg(pi, (phy_a7 == PHY_CORE_0) ? 0x2a3 :
+				    0x2a4, (0x1 << 13), (0) << 13);
+
+			write_phy_reg(pi, 0x2a1, 0x20);
+			write_phy_reg(pi, 0x2a2, 0x60);
+
+			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
+				    0x2a4, (0xf << 4), (9) << 4);
+
+			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
+				    0x2a4, (0xf << 8), (9) << 8);
+
+			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
+				    0x2a4, (0xf << 0), (0x2) << 0);
+
+			write_phy_reg(pi, 0x2e5, 0x20);
+		} else {
+			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
+				    0x2a4, (0x1 << 11), (1) << 11);
+
+			mod_phy_reg(pi, (phy_a7 == PHY_CORE_0) ? 0x2a3 :
+				    0x2a4, (0x1 << 11), (0) << 11);
+
+			write_phy_reg(pi, 0x2a1, 0x80);
+			write_phy_reg(pi, 0x2a2, 0x600);
+
+			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
+				    0x2a4, (0x7 << 4), (0) << 4);
+
+			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
+				    0x2a4, (0x7 << 8), (0) << 8);
+
+			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
+				    0x2a4, (0x7 << 0), (0x3) << 0);
+
+			mod_phy_reg(pi, 0x2a0, (0x3f << 8), (0x20) << 8);
+
+		}
+
+		mod_phy_reg(pi, 0x2a0, (0x3f << 0), (phy_a3) << 0);
+
+		mod_phy_reg(pi, 0x29f, (0x3f << 0), (phy_a1) << 0);
+
+		mod_phy_reg(pi, 0x29f, (0x3f << 8), (phy_a2) << 8);
+
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 3), 1, 0x3, 0);
+
+		write_phy_reg(pi, 0x2be, 1);
+		SPINWAIT(read_phy_reg(pi, 0x2be), 10 * 1000 * 1000);
+
+		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 3), 0, 0x3, 0);
+
+		wlc_phy_table_write_nphy(pi,
+					 (core ==
+					  PHY_CORE_0) ? NPHY_TBL_ID_EPSILONTBL0
+					 : NPHY_TBL_ID_EPSILONTBL1, 1, phy_a3,
+					 32, &phy_a8);
+
+		if (cal_mode != CAL_GCTRL)
+			wlc_phy_a1_nphy(pi, core, 5, 0, 40);
 	}
 }
 
-void
-wlc_phy_cal_txgainctrl_nphy(struct brcms_phy *pi, s32 dBm_targetpower,
-			    bool debug)
+static u8 wlc_phy_a3_nphy(struct brcms_phy *pi, u8 start_gain, u8 core)
 {
-	int gainctrl_loopidx;
-	uint core;
-	u16 m0m1, curr_m0m1;
-	s32 delta_power;
-	s32 txpwrindex;
-	s32 qdBm_power[2];
-	u16 orig_BBConfig;
-	u16 phy_saveregs[4];
-	u32 freq_test;
-	u16 ampl_test = 250;
-	uint stepsize;
-	bool phyhang_avoid_state = false;
+	int phy_a1;
+	int phy_a2;
+	bool phy_a3;
+	struct nphy_ipa_txcalgains phy_a4;
+	bool phy_a5 = false;
+	bool phy_a6 = true;
+	s32 phy_a7, phy_a8;
+	u32 phy_a9;
+	int phy_a10;
+	bool phy_a11 = false;
+	int phy_a12;
+	u8 phy_a13 = 0;
+	u8 phy_a14;
+	u8 *phy_a15 = NULL;
+
+	phy_a4.useindex = true;
+	phy_a12 = start_gain;
 
 	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
 
-		stepsize = 2;
-	} else {
+		phy_a2 = 20;
+		phy_a1 = 1;
 
-		stepsize = 1;
-	}
+		if (CHSPEC_IS2G(pi->radio_chanspec)) {
+			if (pi->pubpi.radiorev == 5) {
 
-	if (CHSPEC_IS40(pi->radio_chanspec)) {
-		freq_test = 5000;
-	} else {
-		freq_test = 2500;
-	}
+				phy_a15 = pad_gain_codes_used_2057rev5;
+				phy_a13 =
+					sizeof(pad_gain_codes_used_2057rev5) /
+					sizeof(pad_gain_codes_used_2057rev5
+						[0]) - 1;
 
-	wlc_phy_txpwr_index_nphy(pi, 1, pi->nphy_cal_orig_pwr_idx[0], true);
-	wlc_phy_txpwr_index_nphy(pi, 2, pi->nphy_cal_orig_pwr_idx[1], true);
+			} else if ((pi->pubpi.radiorev == 7)
+				   || (pi->pubpi.radiorev == 8)) {
 
-	if (pi->phyhang_avoid)
-		wlc_phy_stay_in_carriersearch_nphy(pi, true);
+				phy_a15 = pad_gain_codes_used_2057rev7;
+				phy_a13 =
+					sizeof(pad_gain_codes_used_2057rev7) /
+					sizeof(pad_gain_codes_used_2057rev7
+						[0]) - 1;
 
-	phyhang_avoid_state = pi->phyhang_avoid;
-	pi->phyhang_avoid = false;
+			} else {
 
-	phy_saveregs[0] = read_phy_reg(pi, 0x91);
-	phy_saveregs[1] = read_phy_reg(pi, 0x92);
-	phy_saveregs[2] = read_phy_reg(pi, 0xe7);
-	phy_saveregs[3] = read_phy_reg(pi, 0xec);
-	wlc_phy_rfctrlintc_override_nphy(pi, NPHY_RfctrlIntc_override_PA, 1,
-					 RADIO_MIMO_CORESEL_CORE1 |
-					 RADIO_MIMO_CORESEL_CORE2);
+				phy_a15 = pad_all_gain_codes_2057;
+				phy_a13 = sizeof(pad_all_gain_codes_2057) /
+					  sizeof(pad_all_gain_codes_2057[0]) -
+					  1;
+			}
 
-	if (!debug) {
-		wlc_phy_rfctrlintc_override_nphy(pi,
-						 NPHY_RfctrlIntc_override_TRSW,
-						 0x2, RADIO_MIMO_CORESEL_CORE1);
-		wlc_phy_rfctrlintc_override_nphy(pi,
-						 NPHY_RfctrlIntc_override_TRSW,
-						 0x8, RADIO_MIMO_CORESEL_CORE2);
-	} else {
-		wlc_phy_rfctrlintc_override_nphy(pi,
-						 NPHY_RfctrlIntc_override_TRSW,
-						 0x1, RADIO_MIMO_CORESEL_CORE1);
-		wlc_phy_rfctrlintc_override_nphy(pi,
-						 NPHY_RfctrlIntc_override_TRSW,
-						 0x7, RADIO_MIMO_CORESEL_CORE2);
-	}
+		} else {
 
-	orig_BBConfig = read_phy_reg(pi, 0x01);
-	mod_phy_reg(pi, 0x01, (0x1 << 15), 0);
+			phy_a15 = pga_all_gain_codes_2057;
+			phy_a13 = sizeof(pga_all_gain_codes_2057) /
+				  sizeof(pga_all_gain_codes_2057[0]) - 1;
+		}
 
-	wlc_phy_table_read_nphy(pi, 15, 1, 87, 16, &m0m1);
+		phy_a14 = 0;
 
-	for (core = 0; core < pi->pubpi.phy_corenum; core++) {
-		txpwrindex = (s32) pi->nphy_cal_orig_pwr_idx[core];
+		for (phy_a10 = 0; phy_a10 < phy_a2; phy_a10++) {
+			if (CHSPEC_IS2G(pi->radio_chanspec))
+				phy_a4.gains.pad[core] =
+					(u16) phy_a15[phy_a12];
+			else
+				phy_a4.gains.pga[core] =
+					(u16) phy_a15[phy_a12];
 
-		for (gainctrl_loopidx = 0; gainctrl_loopidx < 2;
-		     gainctrl_loopidx++) {
-			wlc_phy_tx_tone_nphy(pi, freq_test, ampl_test, 0, 0,
-					     false);
+			wlc_phy_a2_nphy(pi, &phy_a4, CAL_GCTRL, core);
 
-			if (core == PHY_CORE_0) {
-				curr_m0m1 = m0m1 & 0xff00;
-			} else {
-				curr_m0m1 = m0m1 & 0x00ff;
+			wlc_phy_table_read_nphy(pi,
+						(core ==
+						 PHY_CORE_0 ?
+						 NPHY_TBL_ID_EPSILONTBL0 :
+						 NPHY_TBL_ID_EPSILONTBL1), 1,
+						63, 32, &phy_a9);
+
+			wlc_phy_papd_decode_epsilon(phy_a9, &phy_a7, &phy_a8);
+
+			phy_a3 = ((phy_a7 == 4095) || (phy_a7 == -4096) ||
+				  (phy_a8 == 4095) || (phy_a8 == -4096));
+
+			if (!phy_a6 && (phy_a3 != phy_a5)) {
+				if (!phy_a3)
+					phy_a12 -= (u8) phy_a1;
+
+				phy_a11 = true;
+				break;
 			}
 
-			wlc_phy_table_write_nphy(pi, 15, 1, 87, 16, &curr_m0m1);
-			wlc_phy_table_write_nphy(pi, 15, 1, 95, 16, &curr_m0m1);
+			if (phy_a3)
+				phy_a12 += (u8) phy_a1;
+			else
+				phy_a12 -= (u8) phy_a1;
 
-			udelay(50);
+			if ((phy_a12 < phy_a14) || (phy_a12 > phy_a13)) {
+				if (phy_a12 < phy_a14)
+					phy_a12 = phy_a14;
+				else
+					phy_a12 = phy_a13;
 
-			wlc_phy_est_tonepwr_nphy(pi, qdBm_power,
-						 NPHY_CAL_TSSISAMPS);
+				phy_a11 = true;
+				break;
+			}
 
-			pi->nphy_bb_mult_save = 0;
-			wlc_phy_stopplayback_nphy(pi);
+			phy_a6 = false;
+			phy_a5 = phy_a3;
+		}
 
-			delta_power = (dBm_targetpower * 4) - qdBm_power[core];
+	} else {
+		phy_a2 = 10;
+		phy_a1 = 8;
+		for (phy_a10 = 0; phy_a10 < phy_a2; phy_a10++) {
+			phy_a4.index = (u8) phy_a12;
+			wlc_phy_a2_nphy(pi, &phy_a4, CAL_GCTRL, core);
 
-			txpwrindex -= stepsize * delta_power;
-			if (txpwrindex < 0) {
-				txpwrindex = 0;
-			} else if (txpwrindex > 127) {
-				txpwrindex = 127;
+			wlc_phy_table_read_nphy(pi,
+						(core ==
+						 PHY_CORE_0 ?
+						 NPHY_TBL_ID_EPSILONTBL0 :
+						 NPHY_TBL_ID_EPSILONTBL1), 1,
+						63, 32, &phy_a9);
+
+			wlc_phy_papd_decode_epsilon(phy_a9, &phy_a7, &phy_a8);
+
+			phy_a3 = ((phy_a7 == 4095) || (phy_a7 == -4096) ||
+				  (phy_a8 == 4095) || (phy_a8 == -4096));
+
+			if (!phy_a6 && (phy_a3 != phy_a5)) {
+				if (!phy_a3)
+					phy_a12 -= (u8) phy_a1;
+
+				phy_a11 = true;
+				break;
 			}
 
-			if (CHSPEC_IS5G(pi->radio_chanspec)) {
-				if (NREV_IS(pi->pubpi.phy_rev, 4) &&
-				    (pi->srom_fem5g.extpagain == 3)) {
-					if (txpwrindex < 30) {
-						txpwrindex = 30;
-					}
-				}
-			} else {
-				if (NREV_GE(pi->pubpi.phy_rev, 5) &&
-				    (pi->srom_fem2g.extpagain == 3)) {
-					if (txpwrindex < 50) {
-						txpwrindex = 50;
-					}
-				}
+			if (phy_a3)
+				phy_a12 += (u8) phy_a1;
+			else
+				phy_a12 -= (u8) phy_a1;
+
+			if ((phy_a12 < 0) || (phy_a12 > 127)) {
+				if (phy_a12 < 0)
+					phy_a12 = 0;
+				else
+					phy_a12 = 127;
+
+				phy_a11 = true;
+				break;
 			}
 
-			wlc_phy_txpwr_index_nphy(pi, (1 << core),
-						 (u8) txpwrindex, true);
+			phy_a6 = false;
+			phy_a5 = phy_a3;
 		}
 
-		pi->nphy_txcal_pwr_idx[core] = (u8) txpwrindex;
+	}
 
-		if (debug) {
-			u16 radio_gain;
-			u16 dbg_m0m1;
+	if (NREV_GE(pi->pubpi.phy_rev, 7))
+		return (u8) phy_a15[phy_a12];
+	else
+		return (u8) phy_a12;
 
-			wlc_phy_table_read_nphy(pi, 15, 1, 87, 16, &dbg_m0m1);
+}
 
-			wlc_phy_tx_tone_nphy(pi, freq_test, ampl_test, 0, 0,
-					     false);
+static void wlc_phy_a4(struct brcms_phy *pi, bool full_cal)
+{
+	struct nphy_ipa_txcalgains phy_b1[2];
+	struct nphy_papd_restore_state phy_b2;
+	bool phy_b3;
+	u8 phy_b4;
+	u8 phy_b5;
+	s16 phy_b6, phy_b7, phy_b8;
+	u16 phy_b9;
+	s16 phy_b10, phy_b11, phy_b12;
 
-			wlc_phy_table_write_nphy(pi, 15, 1, 87, 16, &dbg_m0m1);
-			wlc_phy_table_write_nphy(pi, 15, 1, 95, 16, &dbg_m0m1);
+	phy_b11 = 0;
+	phy_b12 = 0;
+	phy_b7 = 0;
+	phy_b8 = 0;
+	phy_b6 = 0;
 
-			udelay(100);
+	if (pi->nphy_papd_skip == 1)
+		return;
 
-			wlc_phy_est_tonepwr_nphy(pi, qdBm_power,
-						 NPHY_CAL_TSSISAMPS);
+	phy_b3 = (0 == (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
+	if (!phy_b3)
+		wlapi_suspend_mac_and_wait(pi->sh->physhim);
 
-			wlc_phy_table_read_nphy(pi, 7, 1, (0x110 + core), 16,
-						&radio_gain);
+	wlc_phy_stay_in_carriersearch_nphy(pi, true);
 
-			mdelay(4000);
-			pi->nphy_bb_mult_save = 0;
-			wlc_phy_stopplayback_nphy(pi);
+	pi->nphy_force_papd_cal = false;
+
+	for (phy_b5 = 0; phy_b5 < pi->pubpi.phy_corenum; phy_b5++)
+		pi->nphy_papd_tx_gain_at_last_cal[phy_b5] =
+			wlc_phy_txpwr_idx_cur_get_nphy(pi, phy_b5);
+
+	pi->nphy_papd_last_cal = pi->sh->now;
+	pi->nphy_papd_recal_counter++;
+
+	phy_b4 = pi->nphy_txpwrctrl;
+	wlc_phy_txpwrctrl_enable_nphy(pi, PHY_TPC_HW_OFF);
+
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_SCALARTBL0, 64, 0, 32,
+				 nphy_papd_scaltbl);
+	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_SCALARTBL1, 64, 0, 32,
+				 nphy_papd_scaltbl);
+
+	phy_b9 = read_phy_reg(pi, 0x01);
+	mod_phy_reg(pi, 0x01, (0x1 << 15), 0);
+
+	for (phy_b5 = 0; phy_b5 < pi->pubpi.phy_corenum; phy_b5++) {
+		s32 i, val = 0;
+		for (i = 0; i < 64; i++)
+			wlc_phy_table_write_nphy(pi,
+						 ((phy_b5 ==
+						   PHY_CORE_0) ?
+						  NPHY_TBL_ID_EPSILONTBL0 :
+						  NPHY_TBL_ID_EPSILONTBL1), 1,
+						 i, 32, &val);
+	}
+
+	wlc_phy_ipa_restore_tx_digi_filts_nphy(pi);
+
+	phy_b2.mm = wlc_phy_ipa_get_bbmult_nphy(pi);
+	for (phy_b5 = 0; phy_b5 < pi->pubpi.phy_corenum; phy_b5++) {
+		wlc_phy_papd_cal_setup_nphy(pi, &phy_b2, phy_b5);
+
+		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			if (CHSPEC_IS2G(pi->radio_chanspec)) {
+				if ((pi->pubpi.radiorev == 3)
+				    || (pi->pubpi.radiorev == 4)
+				    || (pi->pubpi.radiorev == 6)) {
+					pi->nphy_papd_cal_gain_index[phy_b5] =
+						23;
+				} else if (pi->pubpi.radiorev == 5) {
+					pi->nphy_papd_cal_gain_index[phy_b5] =
+						0;
+					pi->nphy_papd_cal_gain_index[phy_b5] =
+						wlc_phy_a3_nphy(
+							pi,
+							pi->
+							nphy_papd_cal_gain_index
+							[phy_b5],
+							phy_b5);
+
+				} else if ((pi->pubpi.radiorev == 7)
+					   || (pi->pubpi.radiorev == 8)) {
+
+					pi->nphy_papd_cal_gain_index[phy_b5] =
+						0;
+					pi->nphy_papd_cal_gain_index[phy_b5] =
+						wlc_phy_a3_nphy(
+							pi,
+							pi->
+							nphy_papd_cal_gain_index
+							[phy_b5],
+							phy_b5);
+
+				}
+
+				phy_b1[phy_b5].gains.pad[phy_b5] =
+					pi->nphy_papd_cal_gain_index[phy_b5];
+
+			} else {
+				pi->nphy_papd_cal_gain_index[phy_b5] = 0;
+				pi->nphy_papd_cal_gain_index[phy_b5] =
+					wlc_phy_a3_nphy(
+						pi,
+						pi->
+						nphy_papd_cal_gain_index
+						[phy_b5], phy_b5);
+				phy_b1[phy_b5].gains.pga[phy_b5] =
+					pi->nphy_papd_cal_gain_index[phy_b5];
+			}
+		} else {
+			phy_b1[phy_b5].useindex = true;
+			phy_b1[phy_b5].index = 16;
+			phy_b1[phy_b5].index =
+				wlc_phy_a3_nphy(pi, phy_b1[phy_b5].index,
+						phy_b5);
+
+			pi->nphy_papd_cal_gain_index[phy_b5] =
+				15 - ((phy_b1[phy_b5].index) >> 3);
 		}
+
+		switch (pi->nphy_papd_cal_type) {
+		case 0:
+			wlc_phy_a2_nphy(pi, &phy_b1[phy_b5], CAL_FULL, phy_b5);
+			break;
+		case 1:
+			wlc_phy_a2_nphy(pi, &phy_b1[phy_b5], CAL_SOFT, phy_b5);
+			break;
+		}
+
+		if (NREV_GE(pi->pubpi.phy_rev, 7))
+			wlc_phy_papd_cal_cleanup_nphy(pi, &phy_b2);
 	}
 
-	wlc_phy_txpwr_index_nphy(pi, 1, pi->nphy_txcal_pwr_idx[0], true);
-	wlc_phy_txpwr_index_nphy(pi, 2, pi->nphy_txcal_pwr_idx[1], true);
+	if (NREV_LT(pi->pubpi.phy_rev, 7))
+		wlc_phy_papd_cal_cleanup_nphy(pi, &phy_b2);
 
-	wlc_phy_table_read_nphy(pi, 15, 1, 87, 16, &pi->nphy_txcal_bbmult);
+	for (phy_b5 = 0; phy_b5 < pi->pubpi.phy_corenum; phy_b5++) {
+		int eps_offset = 0;
 
-	write_phy_reg(pi, 0x01, orig_BBConfig);
+		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+			if (CHSPEC_IS2G(pi->radio_chanspec)) {
+				if (pi->pubpi.radiorev == 3)
+					eps_offset = -2;
+				else if (pi->pubpi.radiorev == 5)
+					eps_offset = 3;
+				else
+					eps_offset = -1;
+			} else {
+				eps_offset = 2;
+			}
 
-	write_phy_reg(pi, 0x91, phy_saveregs[0]);
-	write_phy_reg(pi, 0x92, phy_saveregs[1]);
-	write_phy_reg(pi, 0xe7, phy_saveregs[2]);
-	write_phy_reg(pi, 0xec, phy_saveregs[3]);
+			if (CHSPEC_IS2G(pi->radio_chanspec)) {
+				phy_b8 = phy_b1[phy_b5].gains.pad[phy_b5];
+				phy_b10 = 0;
+				if ((pi->pubpi.radiorev == 3) ||
+				    (pi->pubpi.radiorev == 4) ||
+				    (pi->pubpi.radiorev == 6)) {
+					phy_b12 = -(
+					    nphy_papd_padgain_dlt_2g_2057rev3n4
+							     [phy_b8] + 1) / 2;
+					phy_b10 = -1;
+				} else if (pi->pubpi.radiorev == 5) {
+					phy_b12 = -(
+					    nphy_papd_padgain_dlt_2g_2057rev5
+							     [phy_b8] + 1) / 2;
+				} else if ((pi->pubpi.radiorev == 7) ||
+					   (pi->pubpi.radiorev == 8)) {
+					phy_b12 = -(
+					    nphy_papd_padgain_dlt_2g_2057rev7
+							     [phy_b8] + 1) / 2;
+				}
+			} else {
+				phy_b7 = phy_b1[phy_b5].gains.pga[phy_b5];
+				if ((pi->pubpi.radiorev == 3) ||
+				    (pi->pubpi.radiorev == 4) ||
+				    (pi->pubpi.radiorev == 6))
+					phy_b11 =
+						-(nphy_papd_pgagain_dlt_5g_2057
+						  [phy_b7]
+						  + 1) / 2;
+				else if ((pi->pubpi.radiorev == 7)
+					 || (pi->pubpi.radiorev == 8))
+					phy_b11 = -(
+					      nphy_papd_pgagain_dlt_5g_2057rev7
+							     [phy_b7] + 1) / 2;
 
-	pi->phyhang_avoid = phyhang_avoid_state;
+				phy_b10 = -9;
+			}
 
-	if (pi->phyhang_avoid)
-		wlc_phy_stay_in_carriersearch_nphy(pi, false);
-}
+			if (CHSPEC_IS2G(pi->radio_chanspec))
+				phy_b6 =
+					-60 + 27 + eps_offset + phy_b12 +
+					phy_b10;
+			else
+				phy_b6 =
+					-60 + 27 + eps_offset + phy_b11 +
+					phy_b10;
 
-static void wlc_phy_update_txcal_ladder_nphy(struct brcms_phy *pi, u16 core)
-{
-	int index;
-	u32 bbmult_scale;
-	u16 bbmult;
-	u16 tblentry;
+			mod_phy_reg(pi, (phy_b5 == PHY_CORE_0) ? 0x298 :
+				    0x29c, (0x1ff << 7), (phy_b6) << 7);
 
-	struct nphy_txiqcal_ladder ladder_lo[] = {
-		{3, 0}, {4, 0}, {6, 0}, {9, 0}, {13, 0}, {18, 0},
-		{25, 0}, {25, 1}, {25, 2}, {25, 3}, {25, 4}, {25, 5},
-		{25, 6}, {25, 7}, {35, 7}, {50, 7}, {71, 7}, {100, 7}
-	};
+			pi->nphy_papd_epsilon_offset[phy_b5] = phy_b6;
+		} else {
+			if (NREV_LT(pi->pubpi.phy_rev, 5))
+				eps_offset = 4;
+			else
+				eps_offset = 2;
 
-	struct nphy_txiqcal_ladder ladder_iq[] = {
-		{3, 0}, {4, 0}, {6, 0}, {9, 0}, {13, 0}, {18, 0},
-		{25, 0}, {35, 0}, {50, 0}, {71, 0}, {100, 0}, {100, 1},
-		{100, 2}, {100, 3}, {100, 4}, {100, 5}, {100, 6}, {100, 7}
-	};
+			phy_b7 = 15 - ((phy_b1[phy_b5].index) >> 3);
 
-	bbmult = (core == PHY_CORE_0) ?
-	    ((pi->nphy_txcal_bbmult >> 8) & 0xff) : (pi->
-						     nphy_txcal_bbmult & 0xff);
+			if (CHSPEC_IS2G(pi->radio_chanspec)) {
+				phy_b11 =
+					-(nphy_papd_pga_gain_delta_ipa_2g[
+						  phy_b7] +
+					  1) / 2;
+				phy_b10 = 0;
+			} else {
+				phy_b11 =
+					-(nphy_papd_pga_gain_delta_ipa_5g[
+						  phy_b7] +
+					  1) / 2;
+				phy_b10 = -9;
+			}
 
-	for (index = 0; index < 18; index++) {
-		bbmult_scale = ladder_lo[index].percent * bbmult;
-		bbmult_scale /= 100;
+			phy_b6 = -60 + 27 + eps_offset + phy_b11 + phy_b10;
 
-		tblentry =
-		    ((bbmult_scale & 0xff) << 8) | ladder_lo[index].g_env;
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_IQLOCAL, 1, index, 16,
-					 &tblentry);
+			mod_phy_reg(pi, (phy_b5 == PHY_CORE_0) ? 0x298 :
+				    0x29c, (0x1ff << 7), (phy_b6) << 7);
 
-		bbmult_scale = ladder_iq[index].percent * bbmult;
-		bbmult_scale /= 100;
+			pi->nphy_papd_epsilon_offset[phy_b5] = phy_b6;
+		}
+	}
+
+	mod_phy_reg(pi, (0 == PHY_CORE_0) ? 0x297 :
+		    0x29b, (0x1 << 0), (NPHY_PAPD_COMP_ON) << 0);
+
+	mod_phy_reg(pi, (1 == PHY_CORE_0) ? 0x297 :
+		    0x29b, (0x1 << 0), (NPHY_PAPD_COMP_ON) << 0);
+
+	if (NREV_GE(pi->pubpi.phy_rev, 6)) {
+		mod_phy_reg(pi, (0 == PHY_CORE_0) ? 0x2a3 :
+			    0x2a4, (0x1 << 13), (0) << 13);
+
+		mod_phy_reg(pi, (1 == PHY_CORE_0) ? 0x2a3 :
+			    0x2a4, (0x1 << 13), (0) << 13);
+
+	} else {
+		mod_phy_reg(pi, (0 == PHY_CORE_0) ? 0x2a3 :
+			    0x2a4, (0x1 << 11), (0) << 11);
+
+		mod_phy_reg(pi, (1 == PHY_CORE_0) ? 0x2a3 :
+			    0x2a4, (0x1 << 11), (0) << 11);
 
-		tblentry =
-		    ((bbmult_scale & 0xff) << 8) | ladder_iq[index].g_env;
-		wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_IQLOCAL, 1, index + 32,
-					 16, &tblentry);
 	}
+	pi->nphy_papdcomp = NPHY_PAPD_COMP_ON;
+
+	write_phy_reg(pi, 0x01, phy_b9);
+
+	wlc_phy_ipa_set_tx_digi_filts_nphy(pi);
+
+	wlc_phy_txpwrctrl_enable_nphy(pi, phy_b4);
+	if (phy_b4 == PHY_TPC_HW_OFF) {
+		wlc_phy_txpwr_index_nphy(pi, (1 << 0),
+					 (s8) (pi->nphy_txpwrindex[0].
+					       index_internal), false);
+		wlc_phy_txpwr_index_nphy(pi, (1 << 1),
+					 (s8) (pi->nphy_txpwrindex[1].
+					       index_internal), false);
+	}
+
+	wlc_phy_stay_in_carriersearch_nphy(pi, false);
+
+	if (!phy_b3)
+		wlapi_enable_mac(pi->sh->physhim);
 }
 
 void wlc_phy_cal_perical_nphy_run(struct brcms_phy *pi, u8 caltype)
@@ -23738,11 +25671,6 @@ void wlc_phy_cal_perical_nphy_run(struct brcms_phy *pi, u8 caltype)
 	bool restore_tx_gain = false;
 	bool mphase;
 
-	if (NORADIO_ENAB(pi->pubpi)) {
-		wlc_phy_cal_perical_mphase_reset(pi);
-		return;
-	}
-
 	if (PHY_MUTED(pi))
 		return;
 
@@ -23751,19 +25679,18 @@ void wlc_phy_cal_perical_nphy_run(struct brcms_phy *pi, u8 caltype)
 	else if (caltype == PHY_PERICAL_PARTIAL)
 		fullcal = false;
 
-	if (pi->cal_type_override != PHY_PERICAL_AUTO) {
+	if (pi->cal_type_override != PHY_PERICAL_AUTO)
 		fullcal =
-		    (pi->cal_type_override == PHY_PERICAL_FULL) ? true : false;
-	}
+			(pi->cal_type_override ==
+			 PHY_PERICAL_FULL) ? true : false;
 
 	if ((pi->mphase_cal_phase_id > MPHASE_CAL_STATE_INIT)) {
 		if (pi->nphy_txiqlocal_chanspec != pi->radio_chanspec)
 			wlc_phy_cal_perical_mphase_restart(pi);
 	}
 
-	if ((pi->mphase_cal_phase_id == MPHASE_CAL_STATE_RXCAL)) {
+	if ((pi->mphase_cal_phase_id == MPHASE_CAL_STATE_RXCAL))
 		wlapi_bmac_write_shm(pi->sh->physhim, M_CTS_DURATION, 10000);
-	}
 
 	wlapi_suspend_mac_and_wait(pi->sh->physhim);
 
@@ -23772,9 +25699,9 @@ void wlc_phy_cal_perical_nphy_run(struct brcms_phy *pi, u8 caltype)
 	if ((pi->mphase_cal_phase_id == MPHASE_CAL_STATE_IDLE) ||
 	    (pi->mphase_cal_phase_id == MPHASE_CAL_STATE_INIT)) {
 		pi->nphy_cal_orig_pwr_idx[0] =
-		    (u8) ((read_phy_reg(pi, 0x1ed) >> 8) & 0x7f);
+			(u8) ((read_phy_reg(pi, 0x1ed) >> 8) & 0x7f);
 		pi->nphy_cal_orig_pwr_idx[1] =
-		    (u8) ((read_phy_reg(pi, 0x1ee) >> 8) & 0x7f);
+			(u8) ((read_phy_reg(pi, 0x1ee) >> 8) & 0x7f);
 
 		if (pi->nphy_txpwrctrl != PHY_TPC_HW_OFF) {
 			wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_RFSEQ, 2,
@@ -23803,7 +25730,8 @@ void wlc_phy_cal_perical_nphy_run(struct brcms_phy *pi, u8 caltype)
 			target_gain = pi->nphy_cal_target_gain;
 		}
 		if (0 ==
-		    wlc_phy_cal_txiqlo_nphy(pi, target_gain, fullcal, mphase)) {
+		    wlc_phy_cal_txiqlo_nphy(pi, target_gain, fullcal,
+					    mphase)) {
 			if (PHY_IPA(pi))
 				wlc_phy_a4(pi, true);
 
@@ -23815,13 +25743,9 @@ void wlc_phy_cal_perical_nphy_run(struct brcms_phy *pi, u8 caltype)
 			wlc_phyreg_enter((struct brcms_phy_pub *) pi);
 
 			if (0 == wlc_phy_cal_rxiq_nphy(pi, target_gain,
-							     (pi->
-							      first_cal_after_assoc
-							      || (pi->
-								  cal_type_override
-								  ==
-								  PHY_PERICAL_FULL))
-							     ? 2 : 0, false)) {
+					(pi->first_cal_after_assoc ||
+					(pi->cal_type_override ==
+					 PHY_PERICAL_FULL)) ? 2 : 0, false)) {
 				wlc_phy_savecal_nphy(pi);
 
 				wlc_phy_txpwrctrl_coeff_setup_nphy(pi);
@@ -23829,9 +25753,8 @@ void wlc_phy_cal_perical_nphy_run(struct brcms_phy *pi, u8 caltype)
 				pi->nphy_perical_last = pi->sh->now;
 			}
 		}
-		if (caltype != PHY_PERICAL_AUTO) {
+		if (caltype != PHY_PERICAL_AUTO)
 			wlc_phy_rssi_cal_nphy(pi);
-		}
 
 		if (pi->first_cal_after_assoc
 		    || (pi->cal_type_override == PHY_PERICAL_FULL)) {
@@ -23840,18 +25763,17 @@ void wlc_phy_cal_perical_nphy_run(struct brcms_phy *pi, u8 caltype)
 			wlc_phy_txpwrctrl_pwr_setup_nphy(pi);
 		}
 
-		if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+		if (NREV_GE(pi->pubpi.phy_rev, 3))
 			wlc_phy_radio205x_vcocal_nphy(pi);
-		}
 	} else {
 		switch (pi->mphase_cal_phase_id) {
 		case MPHASE_CAL_STATE_INIT:
 			pi->nphy_perical_last = pi->sh->now;
 			pi->nphy_txiqlocal_chanspec = pi->radio_chanspec;
 
-			if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+			if (NREV_GE(pi->pubpi.phy_rev, 3))
 				wlc_phy_precal_txgain_nphy(pi);
-			}
+
 			pi->nphy_cal_target_gain = wlc_phy_get_tx_gain_nphy(pi);
 			pi->mphase_cal_phase_id++;
 			break;
@@ -23866,8 +25788,8 @@ void wlc_phy_cal_perical_nphy_run(struct brcms_phy *pi, u8 caltype)
 				pi->nphy_rxcal_active = true;
 
 			if (wlc_phy_cal_txiqlo_nphy
-			    (pi, pi->nphy_cal_target_gain, fullcal,
-			     true) != 0) {
+				    (pi, pi->nphy_cal_target_gain, fullcal,
+				    true) != 0) {
 
 				wlc_phy_cal_perical_mphase_reset(pi);
 				break;
@@ -23875,20 +25797,19 @@ void wlc_phy_cal_perical_nphy_run(struct brcms_phy *pi, u8 caltype)
 
 			if (NREV_LE(pi->pubpi.phy_rev, 2) &&
 			    (pi->mphase_cal_phase_id ==
-			     MPHASE_CAL_STATE_TXPHASE4)) {
+			     MPHASE_CAL_STATE_TXPHASE4))
 				pi->mphase_cal_phase_id += 2;
-			} else {
+			else
 				pi->mphase_cal_phase_id++;
-			}
 			break;
 
 		case MPHASE_CAL_STATE_PAPDCAL:
 			if ((pi->radar_percal_mask & 0x2) != 0)
 				pi->nphy_rxcal_active = true;
 
-			if (PHY_IPA(pi)) {
+			if (PHY_IPA(pi))
 				wlc_phy_a4(pi, true);
-			}
+
 			pi->mphase_cal_phase_id++;
 			break;
 
@@ -23899,9 +25820,8 @@ void wlc_phy_cal_perical_nphy_run(struct brcms_phy *pi, u8 caltype)
 						  (pi->first_cal_after_assoc ||
 						   (pi->cal_type_override ==
 						    PHY_PERICAL_FULL)) ? 2 : 0,
-						  false) == 0) {
+						  false) == 0)
 				wlc_phy_savecal_nphy(pi);
-			}
 
 			pi->mphase_cal_phase_id++;
 			break;
@@ -23912,16 +25832,15 @@ void wlc_phy_cal_perical_nphy_run(struct brcms_phy *pi, u8 caltype)
 			wlc_phy_txpwrctrl_coeff_setup_nphy(pi);
 			wlc_phy_rssi_cal_nphy(pi);
 
-			if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+			if (NREV_GE(pi->pubpi.phy_rev, 3))
 				wlc_phy_radio205x_vcocal_nphy(pi);
-			}
+
 			restore_tx_gain = true;
 
-			if (pi->first_cal_after_assoc) {
+			if (pi->first_cal_after_assoc)
 				pi->mphase_cal_phase_id++;
-			} else {
+			else
 				wlc_phy_cal_perical_mphase_reset(pi);
-			}
 
 			break;
 
@@ -23962,15 +25881,15 @@ void wlc_phy_cal_perical_nphy_run(struct brcms_phy *pi, u8 caltype)
 			} else {
 				wlc_phy_txpwr_index_nphy(pi, (1 << 0),
 							 (s8) (pi->
-								 nphy_txpwrindex
-								 [0].
-								 index_internal),
+							       nphy_txpwrindex
+							       [0].
+							       index_internal),
 							 false);
 				wlc_phy_txpwr_index_nphy(pi, (1 << 1),
 							 (s8) (pi->
-								 nphy_txpwrindex
-								 [1].
-								 index_internal),
+							       nphy_txpwrindex
+							       [1].
+							       index_internal),
 							 false);
 			}
 		}
@@ -24007,35 +25926,35 @@ wlc_phy_cal_txiqlo_nphy(struct brcms_phy *pi, struct nphy_txgains target_gain,
 
 	u16 tbl_tx_iqlo_cal_loft_ladder_20[] = {
 		0x0300, 0x0500, 0x0700, 0x0900, 0x0d00, 0x1100, 0x1900, 0x1901,
-		    0x1902,
+		0x1902,
 		0x1903, 0x1904, 0x1905, 0x1906, 0x1907, 0x2407, 0x3207, 0x4607,
-		    0x6407
+		0x6407
 	};
 
 	u16 tbl_tx_iqlo_cal_iqimb_ladder_20[] = {
 		0x0200, 0x0300, 0x0600, 0x0900, 0x0d00, 0x1100, 0x1900, 0x2400,
-		    0x3200,
+		0x3200,
 		0x4600, 0x6400, 0x6401, 0x6402, 0x6403, 0x6404, 0x6405, 0x6406,
-		    0x6407
+		0x6407
 	};
 
 	u16 tbl_tx_iqlo_cal_loft_ladder_40[] = {
 		0x0200, 0x0300, 0x0400, 0x0700, 0x0900, 0x0c00, 0x1200, 0x1201,
-		    0x1202,
+		0x1202,
 		0x1203, 0x1204, 0x1205, 0x1206, 0x1207, 0x1907, 0x2307, 0x3207,
-		    0x4707
+		0x4707
 	};
 
 	u16 tbl_tx_iqlo_cal_iqimb_ladder_40[] = {
 		0x0100, 0x0200, 0x0400, 0x0700, 0x0900, 0x0c00, 0x1200, 0x1900,
-		    0x2300,
+		0x2300,
 		0x3200, 0x4700, 0x4701, 0x4702, 0x4703, 0x4704, 0x4705, 0x4706,
-		    0x4707
+		0x4707
 	};
 
 	u16 tbl_tx_iqlo_cal_startcoefs[] = {
 		0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
-		    0x0000
+		0x0000
 	};
 
 	u16 tbl_tx_iqlo_cal_cmds_fullcal[] = {
@@ -24071,11 +25990,10 @@ wlc_phy_cal_txiqlo_nphy(struct brcms_phy *pi, struct nphy_txgains target_gain,
 		pi->phyhang_avoid = false;
 	}
 
-	if (CHSPEC_IS40(pi->radio_chanspec)) {
+	if (CHSPEC_IS40(pi->radio_chanspec))
 		phy_bw = 40;
-	} else {
+	else
 		phy_bw = 20;
-	}
 
 	wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_RFSEQ, 2, 0x110, 16, gain_save);
 
@@ -24117,11 +26035,10 @@ wlc_phy_cal_txiqlo_nphy(struct brcms_phy *pi, struct nphy_txgains target_gain,
 					 16, tbl_ptr);
 	}
 
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+	if (NREV_GE(pi->pubpi.phy_rev, 7))
 		write_phy_reg(pi, 0xc2, 0x8ad9);
-	} else {
+	else
 		write_phy_reg(pi, 0xc2, 0x8aa9);
-	}
 
 	max_val = 250;
 	tone_freq = (phy_bw == 20) ? 2500 : 5000;
@@ -24131,7 +26048,8 @@ wlc_phy_cal_txiqlo_nphy(struct brcms_phy *pi, struct nphy_txgains target_gain,
 		bcmerror = 0;
 	} else {
 		bcmerror =
-		    wlc_phy_tx_tone_nphy(pi, tone_freq, max_val, 1, 0, false);
+			wlc_phy_tx_tone_nphy(pi, tone_freq, max_val, 1, 0,
+					     false);
 	}
 
 	if (bcmerror == 0) {
@@ -24139,19 +26057,15 @@ wlc_phy_cal_txiqlo_nphy(struct brcms_phy *pi, struct nphy_txgains target_gain,
 		if (pi->mphase_cal_phase_id > MPHASE_CAL_STATE_TXPHASE0) {
 			tbl_ptr = pi->mphase_txcal_bestcoeffs;
 			tbl_len = ARRAY_SIZE(pi->mphase_txcal_bestcoeffs);
-			if (NREV_LT(pi->pubpi.phy_rev, 3)) {
-
+			if (NREV_LT(pi->pubpi.phy_rev, 3))
 				tbl_len -= 2;
-			}
 		} else {
 			if ((!fullcal) && (pi->nphy_txiqlocal_coeffsvalid)) {
 
 				tbl_ptr = pi->nphy_txiqlocal_bestc;
 				tbl_len = ARRAY_SIZE(pi->nphy_txiqlocal_bestc);
-				if (NREV_LT(pi->pubpi.phy_rev, 3)) {
-
+				if (NREV_LT(pi->pubpi.phy_rev, 3))
 					tbl_len -= 2;
-				}
 			} else {
 
 				fullcal = true;
@@ -24159,14 +26073,12 @@ wlc_phy_cal_txiqlo_nphy(struct brcms_phy *pi, struct nphy_txgains target_gain,
 				if (NREV_GE(pi->pubpi.phy_rev, 3)) {
 					tbl_ptr =
 					    tbl_tx_iqlo_cal_startcoefs_nphyrev3;
-					tbl_len =
-					    ARRAY_SIZE
-					    (tbl_tx_iqlo_cal_startcoefs_nphyrev3);
+					tbl_len = ARRAY_SIZE(
+					   tbl_tx_iqlo_cal_startcoefs_nphyrev3);
 				} else {
 					tbl_ptr = tbl_tx_iqlo_cal_startcoefs;
-					tbl_len =
-					    ARRAY_SIZE
-					    (tbl_tx_iqlo_cal_startcoefs);
+					tbl_len = ARRAY_SIZE(
+						    tbl_tx_iqlo_cal_startcoefs);
 				}
 			}
 		}
@@ -24175,21 +26087,22 @@ wlc_phy_cal_txiqlo_nphy(struct brcms_phy *pi, struct nphy_txgains target_gain,
 
 		if (fullcal) {
 			max_cal_cmds = (NREV_GE(pi->pubpi.phy_rev, 3)) ?
-			    ARRAY_SIZE(tbl_tx_iqlo_cal_cmds_fullcal_nphyrev3) :
-			    ARRAY_SIZE(tbl_tx_iqlo_cal_cmds_fullcal);
+				       ARRAY_SIZE(
+				tbl_tx_iqlo_cal_cmds_fullcal_nphyrev3) :
+				       ARRAY_SIZE(tbl_tx_iqlo_cal_cmds_fullcal);
 		} else {
 			max_cal_cmds = (NREV_GE(pi->pubpi.phy_rev, 3)) ?
-			    ARRAY_SIZE(tbl_tx_iqlo_cal_cmds_recal_nphyrev3) :
-			    ARRAY_SIZE(tbl_tx_iqlo_cal_cmds_recal);
+				       ARRAY_SIZE(
+				tbl_tx_iqlo_cal_cmds_recal_nphyrev3) :
+				       ARRAY_SIZE(tbl_tx_iqlo_cal_cmds_recal);
 		}
 
 		if (mphase) {
 			cal_cnt = pi->mphase_txcal_cmdidx;
-			if ((cal_cnt + pi->mphase_txcal_numcmds) < max_cal_cmds) {
+			if ((cal_cnt + pi->mphase_txcal_numcmds) < max_cal_cmds)
 				num_cals = cal_cnt + pi->mphase_txcal_numcmds;
-			} else {
+			else
 				num_cals = max_cal_cmds;
-			}
 		} else {
 			cal_cnt = 0;
 			num_cals = max_cal_cmds;
@@ -24199,13 +26112,14 @@ wlc_phy_cal_txiqlo_nphy(struct brcms_phy *pi, struct nphy_txgains target_gain,
 
 			if (fullcal) {
 				cal_cmd = (NREV_GE(pi->pubpi.phy_rev, 3)) ?
-				    tbl_tx_iqlo_cal_cmds_fullcal_nphyrev3
-				    [cal_cnt] :
-				    tbl_tx_iqlo_cal_cmds_fullcal[cal_cnt];
+					  tbl_tx_iqlo_cal_cmds_fullcal_nphyrev3
+					  [cal_cnt] :
+					  tbl_tx_iqlo_cal_cmds_fullcal[cal_cnt];
 			} else {
 				cal_cmd = (NREV_GE(pi->pubpi.phy_rev, 3)) ?
-				    tbl_tx_iqlo_cal_cmds_recal_nphyrev3[cal_cnt]
-				    : tbl_tx_iqlo_cal_cmds_recal[cal_cnt];
+					  tbl_tx_iqlo_cal_cmds_recal_nphyrev3[
+					cal_cnt]
+					  : tbl_tx_iqlo_cal_cmds_recal[cal_cnt];
 			}
 
 			core_no = ((cal_cmd & 0x3000) >> 12);
@@ -24216,15 +26130,16 @@ wlc_phy_cal_txiqlo_nphy(struct brcms_phy *pi, struct nphy_txgains target_gain,
 			     PHY_IPA(pi)
 			     && (CHSPEC_IS2G(pi->radio_chanspec)))) {
 				if (!ladder_updated[core_no]) {
-					wlc_phy_update_txcal_ladder_nphy(pi,
-									 core_no);
+					wlc_phy_update_txcal_ladder_nphy(
+						pi,
+						core_no);
 					ladder_updated[core_no] = true;
 				}
 			}
 
 			val =
-			    (cal_params[core_no].
-			     ncorr[cal_type] << 8) | NPHY_N_GCTL;
+				(cal_params[core_no].
+				 ncorr[cal_type] << 8) | NPHY_N_GCTL;
 			write_phy_reg(pi, 0xc1, val);
 
 			if ((cal_type == 1) || (cal_type == 3)
@@ -24272,8 +26187,8 @@ wlc_phy_cal_txiqlo_nphy(struct brcms_phy *pi, struct nphy_txgains target_gain,
 		}
 
 		mphase_cal_lastphase =
-		    (NREV_LE(pi->pubpi.phy_rev, 2)) ?
-		    MPHASE_CAL_STATE_TXPHASE4 : MPHASE_CAL_STATE_TXPHASE5;
+			(NREV_LE(pi->pubpi.phy_rev, 2)) ?
+			MPHASE_CAL_STATE_TXPHASE4 : MPHASE_CAL_STATE_TXPHASE5;
 
 		if (!mphase
 		    || (pi->mphase_cal_phase_id == mphase_cal_lastphase)) {
@@ -24303,10 +26218,9 @@ wlc_phy_cal_txiqlo_nphy(struct brcms_phy *pi, struct nphy_txgains target_gain,
 						 16, tbl_buf);
 
 			tbl_len = ARRAY_SIZE(pi->nphy_txiqlocal_bestc);
-			if (NREV_LT(pi->pubpi.phy_rev, 3)) {
-
+			if (NREV_LT(pi->pubpi.phy_rev, 3))
 				tbl_len -= 2;
-			}
+
 			wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_IQLOCAL,
 						tbl_len, 96, 16,
 						pi->nphy_txiqlocal_bestc);
@@ -24315,10 +26229,9 @@ wlc_phy_cal_txiqlo_nphy(struct brcms_phy *pi, struct nphy_txgains target_gain,
 			pi->nphy_txiqlocal_chanspec = pi->radio_chanspec;
 		} else {
 			tbl_len = ARRAY_SIZE(pi->mphase_txcal_bestcoeffs);
-			if (NREV_LT(pi->pubpi.phy_rev, 3)) {
-
+			if (NREV_LT(pi->pubpi.phy_rev, 3))
 				tbl_len -= 2;
-			}
+
 			wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_IQLOCAL,
 						tbl_len, 96, 16,
 						pi->mphase_txcal_bestcoeffs);
@@ -24343,9 +26256,8 @@ wlc_phy_cal_txiqlo_nphy(struct brcms_phy *pi, struct nphy_txgains target_gain,
 			wlc_phy_tx_iq_war_nphy(pi);
 	}
 
-	if (NREV_GE(pi->pubpi.phy_rev, 4)) {
+	if (NREV_GE(pi->pubpi.phy_rev, 4))
 		pi->phyhang_avoid = phyhang_avoid_state;
-	}
 
 	wlc_phy_stay_in_carriersearch_nphy(pi, false);
 
@@ -24387,18 +26299,6 @@ static void wlc_phy_reapply_txcal_coeffs_nphy(struct brcms_phy *pi)
 	}
 }
 
-static void wlc_phy_tx_iq_war_nphy(struct brcms_phy *pi)
-{
-	struct nphy_iq_comp tx_comp;
-
-	wlc_phy_table_read_nphy(pi, 15, 4, 0x50, 16, (void *)&tx_comp);
-
-	wlapi_bmac_write_shm(pi->sh->physhim, M_20IN40_IQ, tx_comp.a0);
-	wlapi_bmac_write_shm(pi->sh->physhim, M_20IN40_IQ + 2, tx_comp.b0);
-	wlapi_bmac_write_shm(pi->sh->physhim, M_20IN40_IQ + 4, tx_comp.a1);
-	wlapi_bmac_write_shm(pi->sh->physhim, M_20IN40_IQ + 6, tx_comp.b1);
-}
-
 void
 wlc_phy_rx_iq_coeffs_nphy(struct brcms_phy *pi, u8 write,
 			  struct nphy_iq_comp *pcomp)
@@ -24438,14 +26338,17 @@ wlc_phy_rx_iq_est_nphy(struct brcms_phy *pi, struct phy_iq_est *est,
 	if ((read_phy_reg(pi, 0x129) & NPHY_IqestCmd_iqstart) == 0) {
 		for (core = 0; core < pi->pubpi.phy_corenum; core++) {
 			est[core].i_pwr =
-			    (read_phy_reg(pi, NPHY_IqestipwrAccHi(core)) << 16)
-			    | read_phy_reg(pi, NPHY_IqestipwrAccLo(core));
+				(read_phy_reg(pi,
+					      NPHY_IqestipwrAccHi(core)) << 16)
+				| read_phy_reg(pi, NPHY_IqestipwrAccLo(core));
 			est[core].q_pwr =
-			    (read_phy_reg(pi, NPHY_IqestqpwrAccHi(core)) << 16)
-			    | read_phy_reg(pi, NPHY_IqestqpwrAccLo(core));
+				(read_phy_reg(pi,
+					      NPHY_IqestqpwrAccHi(core)) << 16)
+				| read_phy_reg(pi, NPHY_IqestqpwrAccLo(core));
 			est[core].iq_prod =
-			    (read_phy_reg(pi, NPHY_IqestIqAccHi(core)) << 16) |
-			    read_phy_reg(pi, NPHY_IqestIqAccLo(core));
+				(read_phy_reg(pi,
+					      NPHY_IqestIqAccHi(core)) << 16) |
+				read_phy_reg(pi, NPHY_IqestIqAccLo(core));
 		}
 	}
 }
@@ -24470,7 +26373,7 @@ static void wlc_phy_calc_rx_iq_comp_nphy(struct brcms_phy *pi, u8 core_mask)
 	new_comp.a0 = new_comp.b0 = new_comp.a1 = new_comp.b1 = 0x0;
 	wlc_phy_rx_iq_coeffs_nphy(pi, 1, &new_comp);
 
- cal_try:
+cal_try:
 	wlc_phy_rx_iq_est_nphy(pi, est, 0x4000, 32, 0);
 
 	new_comp = old_comp;
@@ -24561,7 +26464,7 @@ static void wlc_phy_calc_rx_iq_comp_nphy(struct brcms_phy *pi, u8 core_mask)
 
 	if (bcmerror != 0) {
 		printk(KERN_DEBUG "%s: Failed, cnt = %d\n", __func__,
-			cal_retry);
+		       cal_retry);
 
 		if (cal_retry < CAL_RETRY_CNT) {
 			cal_retry++;
@@ -24584,121 +26487,125 @@ static void wlc_phy_rxcal_radio_setup_nphy(struct brcms_phy *pi, u8 rx_core)
 		if (rx_core == PHY_CORE_0) {
 			if (CHSPEC_IS5G(pi->radio_chanspec)) {
 				pi->tx_rx_cal_radio_saveregs[0] =
-				    read_radio_reg(pi,
-						   RADIO_2057_TX0_TXRXCOUPLE_5G_PWRUP);
+					read_radio_reg(pi,
+					    RADIO_2057_TX0_TXRXCOUPLE_5G_PWRUP);
 				pi->tx_rx_cal_radio_saveregs[1] =
-				    read_radio_reg(pi,
-						   RADIO_2057_TX0_TXRXCOUPLE_5G_ATTEN);
+					read_radio_reg(pi,
+					    RADIO_2057_TX0_TXRXCOUPLE_5G_ATTEN);
 
 				write_radio_reg(pi,
-						RADIO_2057_TX0_TXRXCOUPLE_5G_PWRUP,
-						0x3);
+					RADIO_2057_TX0_TXRXCOUPLE_5G_PWRUP,
+					0x3);
 				write_radio_reg(pi,
-						RADIO_2057_TX0_TXRXCOUPLE_5G_ATTEN,
-						0xaf);
+					RADIO_2057_TX0_TXRXCOUPLE_5G_ATTEN,
+					0xaf);
 
 			} else {
 				pi->tx_rx_cal_radio_saveregs[0] =
-				    read_radio_reg(pi,
-						   RADIO_2057_TX0_TXRXCOUPLE_2G_PWRUP);
+					read_radio_reg(pi,
+					    RADIO_2057_TX0_TXRXCOUPLE_2G_PWRUP);
 				pi->tx_rx_cal_radio_saveregs[1] =
-				    read_radio_reg(pi,
-						   RADIO_2057_TX0_TXRXCOUPLE_2G_ATTEN);
-
-				write_radio_reg(pi,
-						RADIO_2057_TX0_TXRXCOUPLE_2G_PWRUP,
-						0x3);
-				write_radio_reg(pi,
-						RADIO_2057_TX0_TXRXCOUPLE_2G_ATTEN,
-						0x7f);
+					read_radio_reg(pi,
+					    RADIO_2057_TX0_TXRXCOUPLE_2G_ATTEN);
+
+				write_radio_reg(
+					pi,
+					RADIO_2057_TX0_TXRXCOUPLE_2G_PWRUP,
+					0x3);
+				write_radio_reg(
+					pi,
+					RADIO_2057_TX0_TXRXCOUPLE_2G_ATTEN,
+					0x7f);
 			}
 
 		} else {
 			if (CHSPEC_IS5G(pi->radio_chanspec)) {
 				pi->tx_rx_cal_radio_saveregs[0] =
-				    read_radio_reg(pi,
-						   RADIO_2057_TX1_TXRXCOUPLE_5G_PWRUP);
+					read_radio_reg(pi,
+					    RADIO_2057_TX1_TXRXCOUPLE_5G_PWRUP);
 				pi->tx_rx_cal_radio_saveregs[1] =
-				    read_radio_reg(pi,
-						   RADIO_2057_TX1_TXRXCOUPLE_5G_ATTEN);
-
-				write_radio_reg(pi,
-						RADIO_2057_TX1_TXRXCOUPLE_5G_PWRUP,
-						0x3);
-				write_radio_reg(pi,
-						RADIO_2057_TX1_TXRXCOUPLE_5G_ATTEN,
-						0xaf);
+					read_radio_reg(pi,
+					    RADIO_2057_TX1_TXRXCOUPLE_5G_ATTEN);
+
+				write_radio_reg(
+					pi,
+					RADIO_2057_TX1_TXRXCOUPLE_5G_PWRUP,
+					0x3);
+				write_radio_reg(
+					pi,
+					RADIO_2057_TX1_TXRXCOUPLE_5G_ATTEN,
+					0xaf);
 
 			} else {
 				pi->tx_rx_cal_radio_saveregs[0] =
-				    read_radio_reg(pi,
-						   RADIO_2057_TX1_TXRXCOUPLE_2G_PWRUP);
+					read_radio_reg(pi,
+					    RADIO_2057_TX1_TXRXCOUPLE_2G_PWRUP);
 				pi->tx_rx_cal_radio_saveregs[1] =
-				    read_radio_reg(pi,
-						   RADIO_2057_TX1_TXRXCOUPLE_2G_ATTEN);
+					read_radio_reg(pi,
+					    RADIO_2057_TX1_TXRXCOUPLE_2G_ATTEN);
 
 				write_radio_reg(pi,
-						RADIO_2057_TX1_TXRXCOUPLE_2G_PWRUP,
-						0x3);
+					RADIO_2057_TX1_TXRXCOUPLE_2G_PWRUP,
+					0x3);
 				write_radio_reg(pi,
-						RADIO_2057_TX1_TXRXCOUPLE_2G_ATTEN,
-						0x7f);
+					RADIO_2057_TX1_TXRXCOUPLE_2G_ATTEN,
+					0x7f);
 			}
 		}
 
 	} else {
 		if (rx_core == PHY_CORE_0) {
 			pi->tx_rx_cal_radio_saveregs[0] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_RXIQCAL_TXMUX |
-					   RADIO_2056_TX1);
+				read_radio_reg(pi,
+					       RADIO_2056_TX_RXIQCAL_TXMUX |
+					       RADIO_2056_TX1);
 			pi->tx_rx_cal_radio_saveregs[1] =
-			    read_radio_reg(pi,
-					   RADIO_2056_RX_RXIQCAL_RXMUX |
-					   RADIO_2056_RX0);
+				read_radio_reg(pi,
+					       RADIO_2056_RX_RXIQCAL_RXMUX |
+					       RADIO_2056_RX0);
 
 			if (pi->pubpi.radiorev >= 5) {
 				pi->tx_rx_cal_radio_saveregs[2] =
-				    read_radio_reg(pi,
-						   RADIO_2056_RX_RXSPARE2 |
-						   RADIO_2056_RX0);
+					read_radio_reg(pi,
+						       RADIO_2056_RX_RXSPARE2 |
+						       RADIO_2056_RX0);
 				pi->tx_rx_cal_radio_saveregs[3] =
-				    read_radio_reg(pi,
-						   RADIO_2056_TX_TXSPARE2 |
-						   RADIO_2056_TX1);
+					read_radio_reg(pi,
+						       RADIO_2056_TX_TXSPARE2 |
+						       RADIO_2056_TX1);
 			}
 
 			if (CHSPEC_IS5G(pi->radio_chanspec)) {
 
 				if (pi->pubpi.radiorev >= 5) {
 					pi->tx_rx_cal_radio_saveregs[4] =
-					    read_radio_reg(pi,
-							   RADIO_2056_RX_LNAA_MASTER
-							   | RADIO_2056_RX0);
+						read_radio_reg(pi,
+						      RADIO_2056_RX_LNAA_MASTER
+						      | RADIO_2056_RX0);
 
-					write_radio_reg(pi,
-							RADIO_2056_RX_LNAA_MASTER
-							| RADIO_2056_RX0, 0x40);
+					write_radio_reg(
+						pi,
+						RADIO_2056_RX_LNAA_MASTER
+						| RADIO_2056_RX0, 0x40);
 
 					write_radio_reg(pi,
-							RADIO_2056_TX_TXSPARE2 |
-							RADIO_2056_TX1, bias_a);
+						RADIO_2056_TX_TXSPARE2 |
+						RADIO_2056_TX1, bias_a);
 
 					write_radio_reg(pi,
-							RADIO_2056_RX_RXSPARE2 |
-							RADIO_2056_RX0, bias_a);
+						RADIO_2056_RX_RXSPARE2 |
+						RADIO_2056_RX0, bias_a);
 				} else {
 					pi->tx_rx_cal_radio_saveregs[4] =
-					    read_radio_reg(pi,
-							   RADIO_2056_RX_LNAA_TUNE
-							   | RADIO_2056_RX0);
+						read_radio_reg(pi,
+							RADIO_2056_RX_LNAA_TUNE
+							| RADIO_2056_RX0);
 
 					offtune_val =
-					    (pi->
-					     tx_rx_cal_radio_saveregs[2] & 0xF0)
-					    >> 8;
+						(pi->tx_rx_cal_radio_saveregs
+						 [2] & 0xF0) >> 8;
 					offtune_val =
-					    (offtune_val <= 0x7) ? 0xF : 0;
+						(offtune_val <= 0x7) ? 0xF : 0;
 
 					mod_radio_reg(pi,
 						      RADIO_2056_RX_LNAA_TUNE |
@@ -24715,34 +26622,41 @@ static void wlc_phy_rxcal_radio_setup_nphy(struct brcms_phy *pi, u8 rx_core)
 			} else {
 				if (pi->pubpi.radiorev >= 5) {
 					pi->tx_rx_cal_radio_saveregs[4] =
-					    read_radio_reg(pi,
-							   RADIO_2056_RX_LNAG_MASTER
-							   | RADIO_2056_RX0);
-
-					write_radio_reg(pi,
-							RADIO_2056_RX_LNAG_MASTER
-							| RADIO_2056_RX0, 0x40);
-
-					write_radio_reg(pi,
-							RADIO_2056_TX_TXSPARE2 |
-							RADIO_2056_TX1, bias_g);
-
-					write_radio_reg(pi,
-							RADIO_2056_RX_RXSPARE2 |
-							RADIO_2056_RX0, bias_g);
+						read_radio_reg(
+						      pi,
+						      RADIO_2056_RX_LNAG_MASTER
+						    | RADIO_2056_RX0);
+
+					write_radio_reg(
+						pi,
+						RADIO_2056_RX_LNAG_MASTER
+						| RADIO_2056_RX0, 0x40);
+
+					write_radio_reg(
+						pi,
+						RADIO_2056_TX_TXSPARE2
+						|
+						RADIO_2056_TX1, bias_g);
+
+					write_radio_reg(
+						pi,
+						RADIO_2056_RX_RXSPARE2
+						|
+						RADIO_2056_RX0, bias_g);
 
 				} else {
 					pi->tx_rx_cal_radio_saveregs[4] =
-					    read_radio_reg(pi,
-							   RADIO_2056_RX_LNAG_TUNE
-							   | RADIO_2056_RX0);
+						read_radio_reg(
+							pi,
+							RADIO_2056_RX_LNAG_TUNE
+							| RADIO_2056_RX0);
 
 					offtune_val =
-					    (pi->
-					     tx_rx_cal_radio_saveregs[2] & 0xF0)
-					    >> 8;
+						(pi->
+						 tx_rx_cal_radio_saveregs[2] &
+						 0xF0) >> 8;
 					offtune_val =
-					    (offtune_val <= 0x7) ? 0xF : 0;
+						(offtune_val <= 0x7) ? 0xF : 0;
 
 					mod_radio_reg(pi,
 						      RADIO_2056_RX_LNAG_TUNE |
@@ -24760,56 +26674,63 @@ static void wlc_phy_rxcal_radio_setup_nphy(struct brcms_phy *pi, u8 rx_core)
 
 		} else {
 			pi->tx_rx_cal_radio_saveregs[0] =
-			    read_radio_reg(pi,
-					   RADIO_2056_TX_RXIQCAL_TXMUX |
-					   RADIO_2056_TX0);
+				read_radio_reg(pi,
+					       RADIO_2056_TX_RXIQCAL_TXMUX |
+					       RADIO_2056_TX0);
 			pi->tx_rx_cal_radio_saveregs[1] =
-			    read_radio_reg(pi,
-					   RADIO_2056_RX_RXIQCAL_RXMUX |
-					   RADIO_2056_RX1);
+				read_radio_reg(pi,
+					       RADIO_2056_RX_RXIQCAL_RXMUX |
+					       RADIO_2056_RX1);
 
 			if (pi->pubpi.radiorev >= 5) {
 				pi->tx_rx_cal_radio_saveregs[2] =
-				    read_radio_reg(pi,
-						   RADIO_2056_RX_RXSPARE2 |
-						   RADIO_2056_RX1);
+					read_radio_reg(pi,
+						       RADIO_2056_RX_RXSPARE2 |
+						       RADIO_2056_RX1);
 				pi->tx_rx_cal_radio_saveregs[3] =
-				    read_radio_reg(pi,
-						   RADIO_2056_TX_TXSPARE2 |
-						   RADIO_2056_TX0);
+					read_radio_reg(pi,
+						       RADIO_2056_TX_TXSPARE2 |
+						       RADIO_2056_TX0);
 			}
 
 			if (CHSPEC_IS5G(pi->radio_chanspec)) {
 
 				if (pi->pubpi.radiorev >= 5) {
 					pi->tx_rx_cal_radio_saveregs[4] =
-					    read_radio_reg(pi,
-							   RADIO_2056_RX_LNAA_MASTER
-							   | RADIO_2056_RX1);
-
-					write_radio_reg(pi,
-							RADIO_2056_RX_LNAA_MASTER
-							| RADIO_2056_RX1, 0x40);
-
-					write_radio_reg(pi,
-							RADIO_2056_TX_TXSPARE2 |
-							RADIO_2056_TX0, bias_a);
-
-					write_radio_reg(pi,
-							RADIO_2056_RX_RXSPARE2 |
-							RADIO_2056_RX1, bias_a);
+						read_radio_reg(
+						       pi,
+						       RADIO_2056_RX_LNAA_MASTER
+						       | RADIO_2056_RX1);
+
+					write_radio_reg(
+						pi,
+						RADIO_2056_RX_LNAA_MASTER |
+						RADIO_2056_RX1, 0x40);
+
+					write_radio_reg(
+						pi,
+						RADIO_2056_TX_TXSPARE2
+						|
+						RADIO_2056_TX0, bias_a);
+
+					write_radio_reg(
+						pi,
+						RADIO_2056_RX_RXSPARE2
+						|
+						RADIO_2056_RX1, bias_a);
 				} else {
 					pi->tx_rx_cal_radio_saveregs[4] =
-					    read_radio_reg(pi,
-							   RADIO_2056_RX_LNAA_TUNE
-							   | RADIO_2056_RX1);
+						read_radio_reg(
+							pi,
+							RADIO_2056_RX_LNAA_TUNE
+							| RADIO_2056_RX1);
 
 					offtune_val =
-					    (pi->
-					     tx_rx_cal_radio_saveregs[2] & 0xF0)
-					    >> 8;
+						(pi->
+						 tx_rx_cal_radio_saveregs[2] &
+						 0xF0) >> 8;
 					offtune_val =
-					    (offtune_val <= 0x7) ? 0xF : 0;
+						(offtune_val <= 0x7) ? 0xF : 0;
 
 					mod_radio_reg(pi,
 						      RADIO_2056_RX_LNAA_TUNE |
@@ -24826,33 +26747,40 @@ static void wlc_phy_rxcal_radio_setup_nphy(struct brcms_phy *pi, u8 rx_core)
 			} else {
 				if (pi->pubpi.radiorev >= 5) {
 					pi->tx_rx_cal_radio_saveregs[4] =
-					    read_radio_reg(pi,
-							   RADIO_2056_RX_LNAG_MASTER
-							   | RADIO_2056_RX1);
-
-					write_radio_reg(pi,
-							RADIO_2056_RX_LNAG_MASTER
-							| RADIO_2056_RX1, 0x40);
-
-					write_radio_reg(pi,
-							RADIO_2056_TX_TXSPARE2 |
-							RADIO_2056_TX0, bias_g);
-
-					write_radio_reg(pi,
-							RADIO_2056_RX_RXSPARE2 |
-							RADIO_2056_RX1, bias_g);
+						read_radio_reg(
+						      pi,
+						      RADIO_2056_RX_LNAG_MASTER
+						    | RADIO_2056_RX1);
+
+					write_radio_reg(
+						pi,
+						RADIO_2056_RX_LNAG_MASTER
+						| RADIO_2056_RX1, 0x40);
+
+					write_radio_reg(
+						pi,
+						RADIO_2056_TX_TXSPARE2
+						|
+						RADIO_2056_TX0, bias_g);
+
+					write_radio_reg(
+						pi,
+						RADIO_2056_RX_RXSPARE2
+						|
+						RADIO_2056_RX1, bias_g);
 				} else {
 					pi->tx_rx_cal_radio_saveregs[4] =
-					    read_radio_reg(pi,
-							   RADIO_2056_RX_LNAG_TUNE
-							   | RADIO_2056_RX1);
+						read_radio_reg(
+							pi,
+							RADIO_2056_RX_LNAG_TUNE
+							| RADIO_2056_RX1);
 
 					offtune_val =
-					    (pi->
-					     tx_rx_cal_radio_saveregs[2] & 0xF0)
-					    >> 8;
+						(pi->
+						 tx_rx_cal_radio_saveregs[2] &
+						 0xF0) >> 8;
 					offtune_val =
-					    (offtune_val <= 0x7) ? 0xF : 0;
+						(offtune_val <= 0x7) ? 0xF : 0;
 
 					mod_radio_reg(pi,
 						      RADIO_2056_RX_LNAG_TUNE |
@@ -24876,46 +26804,54 @@ static void wlc_phy_rxcal_radio_cleanup_nphy(struct brcms_phy *pi, u8 rx_core)
 	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
 		if (rx_core == PHY_CORE_0) {
 			if (CHSPEC_IS5G(pi->radio_chanspec)) {
-				write_radio_reg(pi,
-						RADIO_2057_TX0_TXRXCOUPLE_5G_PWRUP,
-						pi->
-						tx_rx_cal_radio_saveregs[0]);
-				write_radio_reg(pi,
-						RADIO_2057_TX0_TXRXCOUPLE_5G_ATTEN,
-						pi->
-						tx_rx_cal_radio_saveregs[1]);
+				write_radio_reg(
+					pi,
+					RADIO_2057_TX0_TXRXCOUPLE_5G_PWRUP,
+					pi->
+					tx_rx_cal_radio_saveregs[0]);
+				write_radio_reg(
+					pi,
+					RADIO_2057_TX0_TXRXCOUPLE_5G_ATTEN,
+					pi->
+					tx_rx_cal_radio_saveregs[1]);
 
 			} else {
-				write_radio_reg(pi,
-						RADIO_2057_TX0_TXRXCOUPLE_2G_PWRUP,
-						pi->
-						tx_rx_cal_radio_saveregs[0]);
-				write_radio_reg(pi,
-						RADIO_2057_TX0_TXRXCOUPLE_2G_ATTEN,
-						pi->
-						tx_rx_cal_radio_saveregs[1]);
+				write_radio_reg(
+					pi,
+					RADIO_2057_TX0_TXRXCOUPLE_2G_PWRUP,
+					pi->
+					tx_rx_cal_radio_saveregs[0]);
+				write_radio_reg(
+					pi,
+					RADIO_2057_TX0_TXRXCOUPLE_2G_ATTEN,
+					pi->
+					tx_rx_cal_radio_saveregs[1]);
 			}
 
 		} else {
 			if (CHSPEC_IS5G(pi->radio_chanspec)) {
-				write_radio_reg(pi,
-						RADIO_2057_TX1_TXRXCOUPLE_5G_PWRUP,
-						pi->
-						tx_rx_cal_radio_saveregs[0]);
-				write_radio_reg(pi,
-						RADIO_2057_TX1_TXRXCOUPLE_5G_ATTEN,
-						pi->
-						tx_rx_cal_radio_saveregs[1]);
+				write_radio_reg(
+					pi,
+					RADIO_2057_TX1_TXRXCOUPLE_5G_PWRUP,
+					pi->
+					tx_rx_cal_radio_saveregs[0]);
+				write_radio_reg(
+					pi,
+					RADIO_2057_TX1_TXRXCOUPLE_5G_ATTEN,
+					pi->
+					tx_rx_cal_radio_saveregs[1]);
 
 			} else {
-				write_radio_reg(pi,
-						RADIO_2057_TX1_TXRXCOUPLE_2G_PWRUP,
-						pi->
-						tx_rx_cal_radio_saveregs[0]);
-				write_radio_reg(pi,
-						RADIO_2057_TX1_TXRXCOUPLE_2G_ATTEN,
-						pi->
-						tx_rx_cal_radio_saveregs[1]);
+				write_radio_reg(
+					pi,
+					RADIO_2057_TX1_TXRXCOUPLE_2G_PWRUP,
+					pi->
+					tx_rx_cal_radio_saveregs[0]);
+				write_radio_reg(
+					pi,
+					RADIO_2057_TX1_TXRXCOUPLE_2G_ATTEN,
+					pi->
+					tx_rx_cal_radio_saveregs[1]);
 			}
 		}
 
@@ -24946,37 +26882,39 @@ static void wlc_phy_rxcal_radio_cleanup_nphy(struct brcms_phy *pi, u8 rx_core)
 			}
 
 			if (CHSPEC_IS5G(pi->radio_chanspec)) {
-				if (pi->pubpi.radiorev >= 5) {
-					write_radio_reg(pi,
-							RADIO_2056_RX_LNAA_MASTER
-							| RADIO_2056_RX0,
-							pi->
-							tx_rx_cal_radio_saveregs
-							[4]);
-				} else {
-					write_radio_reg(pi,
-							RADIO_2056_RX_LNAA_TUNE
-							| RADIO_2056_RX0,
-							pi->
-							tx_rx_cal_radio_saveregs
-							[4]);
-				}
+				if (pi->pubpi.radiorev >= 5)
+					write_radio_reg(
+						pi,
+						RADIO_2056_RX_LNAA_MASTER
+						| RADIO_2056_RX0,
+						pi->
+						tx_rx_cal_radio_saveregs
+						[4]);
+				else
+					write_radio_reg(
+						pi,
+						RADIO_2056_RX_LNAA_TUNE
+						| RADIO_2056_RX0,
+						pi->
+						tx_rx_cal_radio_saveregs
+						[4]);
 			} else {
-				if (pi->pubpi.radiorev >= 5) {
-					write_radio_reg(pi,
-							RADIO_2056_RX_LNAG_MASTER
-							| RADIO_2056_RX0,
-							pi->
-							tx_rx_cal_radio_saveregs
-							[4]);
-				} else {
-					write_radio_reg(pi,
-							RADIO_2056_RX_LNAG_TUNE
-							| RADIO_2056_RX0,
-							pi->
-							tx_rx_cal_radio_saveregs
-							[4]);
-				}
+				if (pi->pubpi.radiorev >= 5)
+					write_radio_reg(
+						pi,
+						RADIO_2056_RX_LNAG_MASTER
+						| RADIO_2056_RX0,
+						pi->
+						tx_rx_cal_radio_saveregs
+						[4]);
+				else
+					write_radio_reg(
+						pi,
+						RADIO_2056_RX_LNAG_TUNE
+						| RADIO_2056_RX0,
+						pi->
+						tx_rx_cal_radio_saveregs
+						[4]);
 			}
 
 		} else {
@@ -25005,37 +26943,39 @@ static void wlc_phy_rxcal_radio_cleanup_nphy(struct brcms_phy *pi, u8 rx_core)
 			}
 
 			if (CHSPEC_IS5G(pi->radio_chanspec)) {
-				if (pi->pubpi.radiorev >= 5) {
-					write_radio_reg(pi,
-							RADIO_2056_RX_LNAA_MASTER
-							| RADIO_2056_RX1,
-							pi->
-							tx_rx_cal_radio_saveregs
-							[4]);
-				} else {
-					write_radio_reg(pi,
-							RADIO_2056_RX_LNAA_TUNE
-							| RADIO_2056_RX1,
-							pi->
-							tx_rx_cal_radio_saveregs
-							[4]);
-				}
+				if (pi->pubpi.radiorev >= 5)
+					write_radio_reg(
+						pi,
+						RADIO_2056_RX_LNAA_MASTER
+						| RADIO_2056_RX1,
+						pi->
+						tx_rx_cal_radio_saveregs
+						[4]);
+				else
+					write_radio_reg(
+						pi,
+						RADIO_2056_RX_LNAA_TUNE
+						| RADIO_2056_RX1,
+						pi->
+						tx_rx_cal_radio_saveregs
+						[4]);
 			} else {
-				if (pi->pubpi.radiorev >= 5) {
-					write_radio_reg(pi,
-							RADIO_2056_RX_LNAG_MASTER
-							| RADIO_2056_RX1,
-							pi->
-							tx_rx_cal_radio_saveregs
-							[4]);
-				} else {
-					write_radio_reg(pi,
-							RADIO_2056_RX_LNAG_TUNE
-							| RADIO_2056_RX1,
-							pi->
-							tx_rx_cal_radio_saveregs
-							[4]);
-				}
+				if (pi->pubpi.radiorev >= 5)
+					write_radio_reg(
+						pi,
+						RADIO_2056_RX_LNAG_MASTER
+						| RADIO_2056_RX1,
+						pi->
+						tx_rx_cal_radio_saveregs
+						[4]);
+				else
+					write_radio_reg(
+						pi,
+						RADIO_2056_RX_LNAG_TUNE
+						| RADIO_2056_RX1,
+						pi->
+						tx_rx_cal_radio_saveregs
+						[4]);
 			}
 		}
 	}
@@ -25046,18 +26986,16 @@ static void wlc_phy_rxcal_physetup_nphy(struct brcms_phy *pi, u8 rx_core)
 	u8 tx_core;
 	u16 rx_antval, tx_antval;
 
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-
+	if (NREV_GE(pi->pubpi.phy_rev, 7))
 		tx_core = rx_core;
-	} else {
+	else
 		tx_core = (rx_core == PHY_CORE_0) ? 1 : 0;
-	}
 
 	pi->tx_rx_cal_phy_saveregs[0] = read_phy_reg(pi, 0xa2);
 	pi->tx_rx_cal_phy_saveregs[1] =
-	    read_phy_reg(pi, (rx_core == PHY_CORE_0) ? 0xa6 : 0xa7);
+		read_phy_reg(pi, (rx_core == PHY_CORE_0) ? 0xa6 : 0xa7);
 	pi->tx_rx_cal_phy_saveregs[2] =
-	    read_phy_reg(pi, (rx_core == PHY_CORE_0) ? 0x8f : 0xa5);
+		read_phy_reg(pi, (rx_core == PHY_CORE_0) ? 0x8f : 0xa5);
 	pi->tx_rx_cal_phy_saveregs[3] = read_phy_reg(pi, 0x91);
 	pi->tx_rx_cal_phy_saveregs[4] = read_phy_reg(pi, 0x92);
 	pi->tx_rx_cal_phy_saveregs[5] = read_phy_reg(pi, 0x7a);
@@ -25122,17 +27060,19 @@ static void wlc_phy_rxcal_physetup_nphy(struct brcms_phy *pi, u8 rx_core)
 						  NPHY_REV7_RFCTRLOVERRIDE_ID2);
 		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 11), 0, 0, 0,
 						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		if (CHSPEC_IS40(pi->radio_chanspec)) {
-			wlc_phy_rfctrl_override_nphy_rev7(pi,
-							  (0x1 << 7),
-							  2, 0, 0,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		} else {
-			wlc_phy_rfctrl_override_nphy_rev7(pi,
-							  (0x1 << 7),
-							  0, 0, 0,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		}
+		if (CHSPEC_IS40(pi->radio_chanspec))
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi,
+				(0x1 << 7),
+				2, 0, 0,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
+		else
+			wlc_phy_rfctrl_override_nphy_rev7(
+				pi,
+				(0x1 << 7),
+				0, 0, 0,
+				NPHY_REV7_RFCTRLOVERRIDE_ID1);
+
 		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 7),
 						  0, 0, 0,
 						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
@@ -25203,26 +27143,24 @@ wlc_phy_rxcal_gainctrl_nphy_rev5(struct brcms_phy *pi, u8 rx_core,
 	struct phy_iq_est est[PHY_CORE_MAX];
 	u8 tx_core;
 	struct nphy_iq_comp save_comp, zero_comp;
-	u32 i_pwr, q_pwr, curr_pwr, optim_pwr = 0, prev_pwr = 0, thresh_pwr =
-	    10000;
+	u32 i_pwr, q_pwr, curr_pwr, optim_pwr = 0, prev_pwr = 0,
+	    thresh_pwr = 10000;
 	s16 desired_log2_pwr, actual_log2_pwr, delta_pwr;
 	bool gainctrl_done = false;
 	u8 mix_tia_gain = 3;
 	s8 optim_gaintbl_index = 0, prev_gaintbl_index = 0;
 	s8 curr_gaintbl_index = 3;
 	u8 gainctrl_dirn = NPHY_RXCAL_GAIN_INIT;
-	struct nphy_ipa_txrxgain *nphy_rxcal_gaintbl;
+	const struct nphy_ipa_txrxgain *nphy_rxcal_gaintbl;
 	u16 hpvga, lpf_biq1, lpf_biq0, lna2, lna1;
 	int fine_gain_idx;
 	s8 txpwrindex;
 	u16 nphy_rxcal_txgain[2];
 
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-
+	if (NREV_GE(pi->pubpi.phy_rev, 7))
 		tx_core = rx_core;
-	} else {
+	else
 		tx_core = 1 - rx_core;
-	}
 
 	num_samps = 1024;
 	desired_log2_pwr = (cal_type == 0) ? 13 : 13;
@@ -25232,45 +27170,42 @@ wlc_phy_rxcal_gainctrl_nphy_rev5(struct brcms_phy *pi, u8 rx_core,
 	wlc_phy_rx_iq_coeffs_nphy(pi, 1, &zero_comp);
 
 	if (CHSPEC_IS5G(pi->radio_chanspec)) {
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+		if (NREV_GE(pi->pubpi.phy_rev, 7))
 			mix_tia_gain = 3;
-		} else if (NREV_GE(pi->pubpi.phy_rev, 4)) {
+		else if (NREV_GE(pi->pubpi.phy_rev, 4))
 			mix_tia_gain = 4;
-		} else {
+		else
 			mix_tia_gain = 6;
-		}
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+		if (NREV_GE(pi->pubpi.phy_rev, 7))
 			nphy_rxcal_gaintbl = nphy_ipa_rxcal_gaintbl_5GHz_rev7;
-		} else {
+		else
 			nphy_rxcal_gaintbl = nphy_ipa_rxcal_gaintbl_5GHz;
-		}
 	} else {
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+		if (NREV_GE(pi->pubpi.phy_rev, 7))
 			nphy_rxcal_gaintbl = nphy_ipa_rxcal_gaintbl_2GHz_rev7;
-		} else {
+		else
 			nphy_rxcal_gaintbl = nphy_ipa_rxcal_gaintbl_2GHz;
-		}
 	}
 
 	do {
 
 		hpvga = (NREV_GE(pi->pubpi.phy_rev, 7)) ?
-		    0 : nphy_rxcal_gaintbl[curr_gaintbl_index].hpvga;
+			0 : nphy_rxcal_gaintbl[curr_gaintbl_index].hpvga;
 		lpf_biq1 = nphy_rxcal_gaintbl[curr_gaintbl_index].lpf_biq1;
 		lpf_biq0 = nphy_rxcal_gaintbl[curr_gaintbl_index].lpf_biq0;
 		lna2 = nphy_rxcal_gaintbl[curr_gaintbl_index].lna2;
 		lna1 = nphy_rxcal_gaintbl[curr_gaintbl_index].lna1;
 		txpwrindex = nphy_rxcal_gaintbl[curr_gaintbl_index].txpwrindex;
 
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			wlc_phy_rfctrl_override_1tomany_nphy(pi,
-							     NPHY_REV7_RfctrlOverride_cmd_rxgain,
-							     ((lpf_biq1 << 12) |
-							      (lpf_biq0 << 8) |
-							      (mix_tia_gain <<
-							       4) | (lna2 << 2)
-							      | lna1), 0x3, 0);
-		} else {
+		if (NREV_GE(pi->pubpi.phy_rev, 7))
+			wlc_phy_rfctrl_override_1tomany_nphy(
+				pi,
+				NPHY_REV7_RfctrlOverride_cmd_rxgain,
+				((lpf_biq1 << 12) |
+				 (lpf_biq0 << 8) |
+				 (mix_tia_gain << 4) | (lna2 << 2)
+				 | lna1), 0x3, 0);
+		else
 			wlc_phy_rfctrl_override_nphy(pi, (0x1 << 12),
 						     ((hpvga << 12) |
 						      (lpf_biq1 << 10) |
@@ -25278,7 +27213,6 @@ wlc_phy_rxcal_gainctrl_nphy_rev5(struct brcms_phy *pi, u8 rx_core,
 						      (mix_tia_gain << 4) |
 						      (lna2 << 2) | lna1), 0x3,
 						     0);
-		}
 
 		pi->nphy_rxcal_pwr_idx[tx_core] = txpwrindex;
 
@@ -25367,27 +27301,28 @@ wlc_phy_rxcal_gainctrl_nphy_rev5(struct brcms_phy *pi, u8 rx_core,
 	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
 		fine_gain_idx = (int)lpf_biq1 + delta_pwr;
 
-		if (fine_gain_idx + (int)lpf_biq0 > 10) {
+		if (fine_gain_idx + (int)lpf_biq0 > 10)
 			lpf_biq1 = 10 - lpf_biq0;
-		} else {
+		else
 			lpf_biq1 = (u16) max(fine_gain_idx, 0);
-		}
-		wlc_phy_rfctrl_override_1tomany_nphy(pi,
-						     NPHY_REV7_RfctrlOverride_cmd_rxgain,
-						     ((lpf_biq1 << 12) |
-						      (lpf_biq0 << 8) |
-						      (mix_tia_gain << 4) |
-						      (lna2 << 2) | lna1), 0x3,
-						     0);
+
+		wlc_phy_rfctrl_override_1tomany_nphy(
+			pi,
+			NPHY_REV7_RfctrlOverride_cmd_rxgain,
+			((lpf_biq1 << 12) |
+			 (lpf_biq0 << 8) |
+			 (mix_tia_gain << 4) |
+			 (lna2 << 2) | lna1), 0x3,
+			0);
 	} else {
 		hpvga = (u16) max(min(((int)hpvga) + delta_pwr, 10), 0);
 		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 12),
-					     ((hpvga << 12) | (lpf_biq1 << 10) |
-					      (lpf_biq0 << 8) | (mix_tia_gain <<
-								 4) | (lna2 <<
-								       2) |
+					     ((hpvga << 12) |
+					      (lpf_biq1 << 10) |
+					      (lpf_biq0 << 8) |
+					      (mix_tia_gain << 4) |
+					      (lna2 << 2) |
 					      lna1), 0x3, 0);
-
 	}
 
 	if (rxgain != NULL) {
@@ -25444,9 +27379,8 @@ wlc_phy_rc_sweep_nphy(struct brcms_phy *pi, u8 core_idx, u8 loopback_type)
 	u16 num_samps, log_num_samps = 10;
 	struct phy_iq_est est[PHY_CORE_MAX];
 
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+	if (NREV_GE(pi->pubpi.phy_rev, 7))
 		return 0;
-	}
 
 	num_samps = (1 << log_num_samps);
 
@@ -25465,20 +27399,21 @@ wlc_phy_rc_sweep_nphy(struct brcms_phy *pi, u8 core_idx, u8 loopback_type)
 	if (core_idx == 0) {
 		radio_addr_offset_rx = RADIO_2056_RX0;
 		radio_addr_offset_tx =
-		    (loopback_type == 0) ? RADIO_2056_TX0 : RADIO_2056_TX1;
+			(loopback_type == 0) ? RADIO_2056_TX0 : RADIO_2056_TX1;
 	} else {
 		radio_addr_offset_rx = RADIO_2056_RX1;
 		radio_addr_offset_tx =
-		    (loopback_type == 0) ? RADIO_2056_TX1 : RADIO_2056_TX0;
+			(loopback_type == 0) ? RADIO_2056_TX1 : RADIO_2056_TX0;
 	}
 
 	orig_txlpf_rccal_lpc_ovr_val =
-	    read_radio_reg(pi,
-			   (RADIO_2056_TX_TXLPF_RCCAL | radio_addr_offset_tx));
+		read_radio_reg(pi,
+			       (RADIO_2056_TX_TXLPF_RCCAL |
+				radio_addr_offset_tx));
 	orig_rxlpf_rccal_hpc_ovr_val =
-	    read_radio_reg(pi,
-			   (RADIO_2056_RX_RXLPF_RCCAL_HPC |
-			    radio_addr_offset_rx));
+		read_radio_reg(pi,
+			       (RADIO_2056_RX_RXLPF_RCCAL_HPC |
+				radio_addr_offset_rx));
 
 	orig_dcBypass = ((read_phy_reg(pi, 0x48) >> 8) & 1);
 
@@ -25549,17 +27484,18 @@ wlc_phy_rc_sweep_nphy(struct brcms_phy *pi, u8 core_idx, u8 loopback_type)
 
 			wlc_phy_rx_iq_est_nphy(pi, est, num_samps, 32, 0);
 
-			if (core_idx == 0) {
+			if (core_idx == 0)
 				ref_iq_vals =
-				    max_t(u32, (est[0].i_pwr +
-					 est[0].q_pwr) >> (log_num_samps + 1),
-					1);
-			} else {
+					max_t(u32, (est[0].i_pwr +
+						    est[0].q_pwr) >>
+					      (log_num_samps + 1),
+					      1);
+			else
 				ref_iq_vals =
-				    max_t(u32, (est[1].i_pwr +
-					 est[1].q_pwr) >> (log_num_samps + 1),
-					1);
-			}
+					max_t(u32, (est[1].i_pwr +
+						    est[1].q_pwr) >>
+					      (log_num_samps + 1),
+					      1);
 
 			wlc_phy_tx_tone_nphy(pi, target_bw, NPHY_RXCAL_TONEAMP,
 					     0, 1, false);
@@ -25568,20 +27504,19 @@ wlc_phy_rc_sweep_nphy(struct brcms_phy *pi, u8 core_idx, u8 loopback_type)
 
 		wlc_phy_rx_iq_est_nphy(pi, est, num_samps, 32, 0);
 
-		if (core_idx == 0) {
-			target_iq_vals =
-			    (est[0].i_pwr + est[0].q_pwr) >> (log_num_samps +
-							      1);
-		} else {
+		if (core_idx == 0)
+			target_iq_vals = (est[0].i_pwr + est[0].q_pwr) >>
+					 (log_num_samps + 1);
+		else
 			target_iq_vals =
-			    (est[1].i_pwr + est[1].q_pwr) >> (log_num_samps +
-							      1);
-		}
+				(est[1].i_pwr +
+				 est[1].q_pwr) >> (log_num_samps + 1);
+
 		pwr_ratio = (uint) ((target_iq_vals << 16) / ref_iq_vals);
 
-		if (rccal_stepsize == 0) {
+		if (rccal_stepsize == 0)
 			rccal_stepsize--;
-		} else if (rccal_stepsize == 1) {
+		else if (rccal_stepsize == 1) {
 			last_rccal_val = rccal_val;
 			rccal_val += (pwr_ratio > target_pwr_ratio) ? 1 : -1;
 			last_pwr_ratio = pwr_ratio;
@@ -25594,21 +27529,20 @@ wlc_phy_rc_sweep_nphy(struct brcms_phy *pi, u8 core_idx, u8 loopback_type)
 
 		if (rccal_stepsize == -1) {
 			best_rccal_val =
-			    (ABS((int)last_pwr_ratio - (int)target_pwr_ratio) <
-			     ABS((int)pwr_ratio -
-				 (int)target_pwr_ratio)) ? last_rccal_val :
-			    rccal_val;
+				(abs((int)last_pwr_ratio -
+				     (int)target_pwr_ratio) <
+				 abs((int)pwr_ratio -
+				     (int)target_pwr_ratio)) ? last_rccal_val :
+				rccal_val;
 
 			if (CHSPEC_IS40(pi->radio_chanspec)) {
 				if ((best_rccal_val > 140)
-				    || (best_rccal_val < 135)) {
+				    || (best_rccal_val < 135))
 					best_rccal_val = 138;
-				}
 			} else {
 				if ((best_rccal_val > 142)
-				    || (best_rccal_val < 137)) {
+				    || (best_rccal_val < 137))
 					best_rccal_val = 140;
-				}
 			}
 
 			write_radio_reg(pi,
@@ -25649,7 +27583,7 @@ wlc_phy_rc_sweep_nphy(struct brcms_phy *pi, u8 core_idx, u8 loopback_type)
 	return best_rccal_val - 0x80;
 }
 
-#define WAIT_FOR_SCOPE	4000
+#define WAIT_FOR_SCOPE  4000
 static int wlc_phy_cal_rxiq_nphy_rev3(struct brcms_phy *pi,
 				      struct nphy_txgains target_gain,
 				      u8 cal_type, bool debug)
@@ -25687,12 +27621,12 @@ static int wlc_phy_cal_rxiq_nphy_rev3(struct brcms_phy *pi,
 	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ, 2, 0x110, 16, cal_gain);
 
 	rxcore_state = wlc_phy_rxcore_getstate_nphy(
-						(struct brcms_phy_pub *) pi);
+		(struct brcms_phy_pub *) pi);
 
 	for (rx_core = 0; rx_core < pi->pubpi.phy_corenum; rx_core++) {
 
 		skip_rxiqcal =
-		    ((rxcore_state & (1 << rx_core)) == 0) ? true : false;
+			((rxcore_state & (1 << rx_core)) == 0) ? true : false;
 
 		wlc_phy_rxcal_physetup_nphy(pi, rx_core);
 
@@ -25703,7 +27637,8 @@ static int wlc_phy_cal_rxiq_nphy_rev3(struct brcms_phy *pi,
 			wlc_phy_rxcal_gainctrl_nphy(pi, rx_core, NULL, 0);
 
 			wlc_phy_tx_tone_nphy(pi,
-					     (CHSPEC_IS40(pi->radio_chanspec)) ?
+					     (CHSPEC_IS40(
+						      pi->radio_chanspec)) ?
 					     NPHY_RXCAL_TONEFREQ_40MHz :
 					     NPHY_RXCAL_TONEFREQ_20MHz,
 					     NPHY_RXCAL_TONEAMP, 0, cal_type,
@@ -25721,23 +27656,21 @@ static int wlc_phy_cal_rxiq_nphy_rev3(struct brcms_phy *pi,
 
 			if (rx_core == PHY_CORE_1) {
 
-				if (rxcore_state == 1) {
+				if (rxcore_state == 1)
 					wlc_phy_rxcore_setstate_nphy(
 						(struct brcms_phy_pub *) pi, 3);
-				}
 
 				wlc_phy_rxcal_gainctrl_nphy(pi, rx_core, NULL,
 							    1);
 
 				best_rccal[rx_core] =
-				    wlc_phy_rc_sweep_nphy(pi, rx_core, 1);
+					wlc_phy_rc_sweep_nphy(pi, rx_core, 1);
 				pi->nphy_rccal_value = best_rccal[rx_core];
 
-				if (rxcore_state == 1) {
+				if (rxcore_state == 1)
 					wlc_phy_rxcore_setstate_nphy(
 						(struct brcms_phy_pub *) pi,
 						rxcore_state);
-				}
 			}
 		}
 
@@ -25756,18 +27689,22 @@ static int wlc_phy_cal_rxiq_nphy_rev3(struct brcms_phy *pi,
 
 		for (rx_core = 0; rx_core < pi->pubpi.phy_corenum; rx_core++) {
 			rxlpf_rccal_hpc =
-			    (((int)best_rccal[rx_core] - 12) >> 1) + 10;
+				(((int)best_rccal[rx_core] - 12) >> 1) + 10;
 			txlpf_rccal_lpc = ((int)best_rccal[rx_core] - 12) + 10;
 
 			if (PHY_IPA(pi)) {
-				txlpf_rccal_lpc += IS40MHZ(pi) ? 24 : 12;
-				txlpf_idac = IS40MHZ(pi) ? 0x0e : 0x13;
+				txlpf_rccal_lpc +=
+					(pi->bw == WL_CHANSPEC_BW_40) ? 24 : 12;
+				txlpf_idac = (pi->bw == WL_CHANSPEC_BW_40) ?
+					     0x0e : 0x13;
 				WRITE_RADIO_REG2(pi, RADIO_2056, TX, rx_core,
 						 TXLPF_IDAC_4, txlpf_idac);
 			}
 
-			rxlpf_rccal_hpc = max(min_t(u8, rxlpf_rccal_hpc, 31), 0);
-			txlpf_rccal_lpc = max(min_t(u8, txlpf_rccal_lpc, 31), 0);
+			rxlpf_rccal_hpc = max(min_t(u8, rxlpf_rccal_hpc, 31),
+					      0);
+			txlpf_rccal_lpc = max(min_t(u8, txlpf_rccal_lpc, 31),
+					      0);
 
 			write_radio_reg(pi, (RADIO_2056_RX_RXLPF_RCCAL_HPC |
 					     ((rx_core ==
@@ -25787,21 +27724,21 @@ static int wlc_phy_cal_rxiq_nphy_rev3(struct brcms_phy *pi,
 
 	wlc_phy_resetcca_nphy(pi);
 
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-		wlc_phy_rfctrl_override_1tomany_nphy(pi,
-						     NPHY_REV7_RfctrlOverride_cmd_rxgain,
-						     0, 0x3, 1);
-	} else {
+	if (NREV_GE(pi->pubpi.phy_rev, 7))
+		wlc_phy_rfctrl_override_1tomany_nphy(
+			pi,
+			NPHY_REV7_RfctrlOverride_cmd_rxgain,
+			0, 0x3, 1);
+	else
 		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 12), 0, 0x3, 1);
-	}
+
 	wlc_phy_force_rfseq_nphy(pi, NPHY_RFSEQ_RESET2RX);
 
 	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_RFSEQ, 2, 0x110, 16,
 				 gain_save);
 
-	if (NREV_GE(pi->pubpi.phy_rev, 4)) {
+	if (NREV_GE(pi->pubpi.phy_rev, 4))
 		pi->phyhang_avoid = phyhang_avoid_state;
-	}
 
 	wlc_phy_stay_in_carriersearch_nphy(pi, false);
 
@@ -25835,10 +27772,8 @@ wlc_phy_cal_rxiq_nphy_rev2(struct brcms_phy *pi,
 
 	wlc_phy_stay_in_carriersearch_nphy(pi, true);
 
-	if (NREV_LT(pi->pubpi.phy_rev, 2)) {
-
+	if (NREV_LT(pi->pubpi.phy_rev, 2))
 		wlc_phy_reapply_txcal_coeffs_nphy(pi);
-	}
 
 	wlc_phy_table_read_nphy(pi, NPHY_TBL_ID_RFSEQ, 2, 0x110, 16, gain_save);
 
@@ -25874,19 +27809,16 @@ wlc_phy_cal_rxiq_nphy_rev2(struct brcms_phy *pi,
 			   ((0x1 << 1) | (0x1 << 2)));
 		or_phy_reg(pi, 0xa5, ((0x1 << 1) | (0x1 << 2)));
 
-		if (((pi->nphy_rxcalparams) & 0xff000000)) {
-
+		if (((pi->nphy_rxcalparams) & 0xff000000))
 			write_phy_reg(pi,
 				      (rx_core == PHY_CORE_0) ? 0x91 : 0x92,
-				      (CHSPEC_IS5G(pi->radio_chanspec) ? 0x140 :
-				       0x110));
-		} else {
-
+				      (CHSPEC_IS5G(pi->radio_chanspec) ?
+					0x140 : 0x110));
+		else
 			write_phy_reg(pi,
 				      (rx_core == PHY_CORE_0) ? 0x91 : 0x92,
-				      (CHSPEC_IS5G(pi->radio_chanspec) ? 0x180 :
-				       0x120));
-		}
+				      (CHSPEC_IS5G(pi->radio_chanspec) ?
+				       0x180 : 0x120));
 
 		write_phy_reg(pi, (tx_core == PHY_CORE_0) ? 0x91 : 0x92,
 			      (CHSPEC_IS5G(pi->radio_chanspec) ? 0x148 :
@@ -25926,7 +27858,7 @@ wlc_phy_cal_rxiq_nphy_rev2(struct brcms_phy *pi,
 					use_hpf_num = 1;
 					curr_hpf = curr_hpf1;
 					actual_log2_pwr =
-					    wlc_phy_nbits(tot_pwr[2]);
+						wlc_phy_nbits(tot_pwr[2]);
 				} else {
 					if (tot_pwr[0] > 10000) {
 						curr_lna = lna_vals[1];
@@ -25935,7 +27867,8 @@ wlc_phy_cal_rxiq_nphy_rev2(struct brcms_phy *pi,
 						use_hpf_num = 1;
 						curr_hpf = curr_hpf1;
 						actual_log2_pwr =
-						    wlc_phy_nbits(tot_pwr[1]);
+							wlc_phy_nbits(
+								tot_pwr[1]);
 					} else {
 						curr_lna = lna_vals[0];
 						curr_hpf1 = hpf1_vals[0];
@@ -25943,18 +27876,18 @@ wlc_phy_cal_rxiq_nphy_rev2(struct brcms_phy *pi,
 						use_hpf_num = 2;
 						curr_hpf = curr_hpf2;
 						actual_log2_pwr =
-						    wlc_phy_nbits(tot_pwr[0]);
+							wlc_phy_nbits(
+								tot_pwr[0]);
 					}
 				}
 
 				hpf_change = desired_log2_pwr - actual_log2_pwr;
 				curr_hpf += hpf_change;
 				curr_hpf = max(min_t(u16, curr_hpf, 10), 0);
-				if (use_hpf_num == 1) {
+				if (use_hpf_num == 1)
 					curr_hpf1 = curr_hpf;
-				} else {
+				else
 					curr_hpf2 = curr_hpf;
-				}
 			}
 
 			wlc_phy_rfctrl_override_nphy(pi, (0x1 << 10),
@@ -25967,15 +27900,12 @@ wlc_phy_cal_rxiq_nphy_rev2(struct brcms_phy *pi,
 
 			if (first_playtone) {
 				bcmerror = wlc_phy_tx_tone_nphy(pi, 4000,
-								(u16) (pi->
-									  nphy_rxcalparams
-									  &
-									  0xffff),
-								0, 0, true);
+						(u16) (pi->nphy_rxcalparams &
+						       0xffff), 0, 0, true);
 				first_playtone = false;
 			} else {
-				phy_bw =
-				    (CHSPEC_IS40(pi->radio_chanspec)) ? 40 : 20;
+				phy_bw = (CHSPEC_IS40(pi->radio_chanspec)) ?
+					  40 : 20;
 				wlc_phy_runsamples_nphy(pi, phy_bw * 8, 0xffff,
 							0, 0, 0, true);
 			}
@@ -25986,12 +27916,10 @@ wlc_phy_cal_rxiq_nphy_rev2(struct brcms_phy *pi,
 					wlc_phy_rx_iq_est_nphy(pi, est,
 							       num_samps, 32,
 							       0);
-					i_pwr =
-					    (est[rx_core].i_pwr +
-					     num_samps / 2) / num_samps;
-					q_pwr =
-					    (est[rx_core].q_pwr +
-					     num_samps / 2) / num_samps;
+					i_pwr =	(est[rx_core].i_pwr +
+						 num_samps / 2) / num_samps;
+					q_pwr =	(est[rx_core].q_pwr +
+						 num_samps / 2) / num_samps;
 					tot_pwr[gain_pass] = i_pwr + q_pwr;
 				} else {
 
@@ -26038,1350 +27966,14 @@ int
 wlc_phy_cal_rxiq_nphy(struct brcms_phy *pi, struct nphy_txgains target_gain,
 		      u8 cal_type, bool debug)
 {
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-
+	if (NREV_GE(pi->pubpi.phy_rev, 7))
 		cal_type = 0;
-	}
-	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+
+	if (NREV_GE(pi->pubpi.phy_rev, 3))
 		return wlc_phy_cal_rxiq_nphy_rev3(pi, target_gain, cal_type,
 						  debug);
-	} else {
+	else
 		return wlc_phy_cal_rxiq_nphy_rev2(pi, target_gain, debug);
-	}
-}
-
-static void wlc_phy_extpa_set_tx_digi_filts_nphy(struct brcms_phy *pi)
-{
-	int j, type = 2;
-	u16 addr_offset = 0x2c5;
-
-	for (j = 0; j < NPHY_NUM_DIG_FILT_COEFFS; j++) {
-		write_phy_reg(pi, addr_offset + j,
-			      NPHY_IPA_REV4_txdigi_filtcoeffs[type][j]);
-	}
-}
-
-static void wlc_phy_ipa_set_tx_digi_filts_nphy(struct brcms_phy *pi)
-{
-	int j, type;
-	u16 addr_offset[] = { 0x186, 0x195,
-		0x2c5
-	};
-
-	for (type = 0; type < 3; type++) {
-		for (j = 0; j < NPHY_NUM_DIG_FILT_COEFFS; j++) {
-			write_phy_reg(pi, addr_offset[type] + j,
-				      NPHY_IPA_REV4_txdigi_filtcoeffs[type][j]);
-		}
-	}
-
-	if (IS40MHZ(pi)) {
-		for (j = 0; j < NPHY_NUM_DIG_FILT_COEFFS; j++) {
-			write_phy_reg(pi, 0x186 + j,
-				      NPHY_IPA_REV4_txdigi_filtcoeffs[3][j]);
-		}
-	} else {
-		if (CHSPEC_IS5G(pi->radio_chanspec)) {
-			for (j = 0; j < NPHY_NUM_DIG_FILT_COEFFS; j++) {
-				write_phy_reg(pi, 0x186 + j,
-					      NPHY_IPA_REV4_txdigi_filtcoeffs[5]
-					      [j]);
-			}
-		}
-
-		if (CHSPEC_CHANNEL(pi->radio_chanspec) == 14) {
-			for (j = 0; j < NPHY_NUM_DIG_FILT_COEFFS; j++) {
-				write_phy_reg(pi, 0x2c5 + j,
-					      NPHY_IPA_REV4_txdigi_filtcoeffs[6]
-					      [j]);
-			}
-		}
-	}
-}
-
-static void wlc_phy_ipa_restore_tx_digi_filts_nphy(struct brcms_phy *pi)
-{
-	int j;
-
-	if (IS40MHZ(pi)) {
-		for (j = 0; j < NPHY_NUM_DIG_FILT_COEFFS; j++) {
-			write_phy_reg(pi, 0x195 + j,
-				      NPHY_IPA_REV4_txdigi_filtcoeffs[4][j]);
-		}
-	} else {
-		for (j = 0; j < NPHY_NUM_DIG_FILT_COEFFS; j++) {
-			write_phy_reg(pi, 0x186 + j,
-				      NPHY_IPA_REV4_txdigi_filtcoeffs[3][j]);
-		}
-	}
-}
-
-static u16 wlc_phy_ipa_get_bbmult_nphy(struct brcms_phy *pi)
-{
-	u16 m0m1;
-
-	wlc_phy_table_read_nphy(pi, 15, 1, 87, 16, &m0m1);
-
-	return m0m1;
-}
-
-static void wlc_phy_ipa_set_bbmult_nphy(struct brcms_phy *pi, u8 m0, u8 m1)
-{
-	u16 m0m1 = (u16) ((m0 << 8) | m1);
-
-	wlc_phy_table_write_nphy(pi, 15, 1, 87, 16, &m0m1);
-	wlc_phy_table_write_nphy(pi, 15, 1, 95, 16, &m0m1);
-}
-
-static u32 *wlc_phy_get_ipa_gaintbl_nphy(struct brcms_phy *pi)
-{
-	u32 *tx_pwrctrl_tbl = NULL;
-
-	if (CHSPEC_IS2G(pi->radio_chanspec)) {
-
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-
-			if ((pi->pubpi.radiorev == 4)
-			    || (pi->pubpi.radiorev == 6)) {
-
-				tx_pwrctrl_tbl =
-				    nphy_tpc_txgain_ipa_2g_2057rev4n6;
-			} else if (pi->pubpi.radiorev == 3) {
-
-				tx_pwrctrl_tbl =
-				    nphy_tpc_txgain_ipa_2g_2057rev3;
-			} else if (pi->pubpi.radiorev == 5) {
-
-				tx_pwrctrl_tbl =
-				    nphy_tpc_txgain_ipa_2g_2057rev5;
-			} else if ((pi->pubpi.radiorev == 7)
-				   || (pi->pubpi.radiorev == 8)) {
-
-				tx_pwrctrl_tbl =
-				    nphy_tpc_txgain_ipa_2g_2057rev7;
-			}
-
-		} else if (NREV_IS(pi->pubpi.phy_rev, 6)) {
-
-			tx_pwrctrl_tbl = nphy_tpc_txgain_ipa_rev6;
-		} else if (NREV_IS(pi->pubpi.phy_rev, 5)) {
-
-			tx_pwrctrl_tbl = nphy_tpc_txgain_ipa_rev5;
-		} else {
-
-			tx_pwrctrl_tbl = nphy_tpc_txgain_ipa;
-		}
-
-	} else {
-
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			if ((pi->pubpi.radiorev == 3) ||
-			    (pi->pubpi.radiorev == 4) ||
-			    (pi->pubpi.radiorev == 6)) {
-
-				tx_pwrctrl_tbl = nphy_tpc_txgain_ipa_5g_2057;
-			} else if ((pi->pubpi.radiorev == 7)
-				   || (pi->pubpi.radiorev == 8)) {
-
-				tx_pwrctrl_tbl =
-				    nphy_tpc_txgain_ipa_5g_2057rev7;
-			}
-
-		} else {
-			tx_pwrctrl_tbl = nphy_tpc_txgain_ipa_5g;
-		}
-	}
-
-	return tx_pwrctrl_tbl;
-}
-
-static void
-wlc_phy_papd_cal_setup_nphy(struct brcms_phy *pi,
-			    struct nphy_papd_restore_state *state, u8 core)
-{
-	s32 tone_freq;
-	u8 off_core;
-	u16 mixgain = 0;
-
-	off_core = core ^ 0x1;
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-
-		if (NREV_IS(pi->pubpi.phy_rev, 7)
-		    || NREV_GE(pi->pubpi.phy_rev, 8)) {
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 7),
-							  wlc_phy_read_lpf_bw_ctl_nphy
-							  (pi, 0), 0, 0,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		}
-
-		if (CHSPEC_IS2G(pi->radio_chanspec)) {
-			if (pi->pubpi.radiorev == 5) {
-				mixgain = (core == 0) ? 0x20 : 0x00;
-
-			} else if ((pi->pubpi.radiorev == 7)
-				   || (pi->pubpi.radiorev == 8)) {
-
-				mixgain = 0x00;
-
-			} else if ((pi->pubpi.radiorev <= 4)
-				   || (pi->pubpi.radiorev == 6)) {
-
-				mixgain = 0x00;
-			}
-
-		} else {
-			if ((pi->pubpi.radiorev == 4) ||
-			    (pi->pubpi.radiorev == 6)) {
-
-				mixgain = 0x50;
-			} else if ((pi->pubpi.radiorev == 3)
-				   || (pi->pubpi.radiorev == 7)
-				   || (pi->pubpi.radiorev == 8)) {
-
-				mixgain = 0x0;
-			}
-		}
-
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 11),
-						  mixgain, (1 << core), 0,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-
-		wlc_phy_rfctrl_override_1tomany_nphy(pi,
-						     NPHY_REV7_RfctrlOverride_cmd_tx_pu,
-						     1, (1 << core), 0);
-		wlc_phy_rfctrl_override_1tomany_nphy(pi,
-						     NPHY_REV7_RfctrlOverride_cmd_tx_pu,
-						     0, (1 << off_core), 0);
-
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 3),
-						  0, 0x3, 0,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 2), 1,
-						  (1 << core), 0,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 0), 0,
-						  (1 << core), 0,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 1), 1,
-						  (1 << core), 0,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID2);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 8), 0,
-						  (1 << core), 0,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 9), 1,
-						  (1 << core), 0,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 10), 0,
-						  (1 << core), 0,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 3), 1,
-						  (1 << core), 0,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 5),
-						  0, (1 << core), 0,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 4), 0,
-						  (1 << core), 0,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-
-		state->afectrl[core] = read_phy_reg(pi, (core == PHY_CORE_0) ?
-						    0xa6 : 0xa7);
-		state->afeoverride[core] =
-		    read_phy_reg(pi, (core == PHY_CORE_0) ? 0x8f : 0xa5);
-		state->afectrl[off_core] =
-		    read_phy_reg(pi, (core == PHY_CORE_0) ? 0xa7 : 0xa6);
-		state->afeoverride[off_core] =
-		    read_phy_reg(pi, (core == PHY_CORE_0) ? 0xa5 : 0x8f);
-
-		mod_phy_reg(pi, ((core == PHY_CORE_0) ? 0xa6 : 0xa7),
-			    (0x1 << 2), 0);
-		mod_phy_reg(pi, ((core == PHY_CORE_0) ? 0x8f :
-				 0xa5), (0x1 << 2), (0x1 << 2));
-
-		mod_phy_reg(pi, ((core == PHY_CORE_0) ? 0xa7 : 0xa6),
-			    (0x1 << 2), (0x1 << 2));
-		mod_phy_reg(pi, ((core == PHY_CORE_0) ? 0xa5 :
-				 0x8f), (0x1 << 2), (0x1 << 2));
-
-		if (CHSPEC_IS2G(pi->radio_chanspec)) {
-			state->pwrup[core] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, core,
-					    TXRXCOUPLE_2G_PWRUP);
-			state->atten[core] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, core,
-					    TXRXCOUPLE_2G_ATTEN);
-			state->pwrup[off_core] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, off_core,
-					    TXRXCOUPLE_2G_PWRUP);
-			state->atten[off_core] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, off_core,
-					    TXRXCOUPLE_2G_ATTEN);
-
-			WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-					 TXRXCOUPLE_2G_PWRUP, 0xc);
-
-			if ((pi->pubpi.radiorev == 3) ||
-			    (pi->pubpi.radiorev == 4) ||
-			    (pi->pubpi.radiorev == 6)) {
-
-				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-						 TXRXCOUPLE_2G_ATTEN, 0xf0);
-
-			} else if (pi->pubpi.radiorev == 5) {
-
-				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-						 TXRXCOUPLE_2G_ATTEN,
-						 (core == 0) ? 0xf7 : 0xf2);
-
-			} else if ((pi->pubpi.radiorev == 7)
-				   || (pi->pubpi.radiorev == 8)) {
-
-				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-						 TXRXCOUPLE_2G_ATTEN, 0xf0);
-
-			}
-
-			WRITE_RADIO_REG3(pi, RADIO_2057, TX, off_core,
-					 TXRXCOUPLE_2G_PWRUP, 0x0);
-			WRITE_RADIO_REG3(pi, RADIO_2057, TX, off_core,
-					 TXRXCOUPLE_2G_ATTEN, 0xff);
-
-		} else {
-			state->pwrup[core] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, core,
-					    TXRXCOUPLE_5G_PWRUP);
-			state->atten[core] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, core,
-					    TXRXCOUPLE_5G_ATTEN);
-			state->pwrup[off_core] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, off_core,
-					    TXRXCOUPLE_5G_PWRUP);
-			state->atten[off_core] =
-			    READ_RADIO_REG3(pi, RADIO_2057, TX, off_core,
-					    TXRXCOUPLE_5G_ATTEN);
-
-			WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-					 TXRXCOUPLE_5G_PWRUP, 0xc);
-
-			if ((pi->pubpi.radiorev == 7)
-			    || (pi->pubpi.radiorev == 8)) {
-
-				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-						 TXRXCOUPLE_5G_ATTEN, 0xf4);
-
-			} else {
-				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-						 TXRXCOUPLE_5G_ATTEN, 0xf0);
-			}
-
-			WRITE_RADIO_REG3(pi, RADIO_2057, TX, off_core,
-					 TXRXCOUPLE_5G_PWRUP, 0x0);
-			WRITE_RADIO_REG3(pi, RADIO_2057, TX, off_core,
-					 TXRXCOUPLE_5G_ATTEN, 0xff);
-		}
-
-		tone_freq = 4000;
-
-		wlc_phy_tx_tone_nphy(pi, tone_freq, 181, 0, 0, false);
-
-		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x297 :
-			    0x29b, (0x1 << 0), (NPHY_PAPD_COMP_ON) << 0);
-
-		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
-			    0x2a4, (0x1 << 13), (1) << 13);
-
-		mod_phy_reg(pi, (off_core == PHY_CORE_0) ? 0x297 :
-			    0x29b, (0x1 << 0), (NPHY_PAPD_COMP_OFF) << 0);
-
-		mod_phy_reg(pi, (off_core == PHY_CORE_0) ? 0x2a3 :
-			    0x2a4, (0x1 << 13), (0) << 13);
-
-	} else {
-
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 12), 0, 0x3, 0);
-
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 3), 1, 0, 0);
-
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 0), 0, 0x3, 0);
-
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 2), 1, 0x3, 0);
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 1), 1, 0x3, 0);
-
-		state->afectrl[core] = read_phy_reg(pi, (core == PHY_CORE_0) ?
-						    0xa6 : 0xa7);
-		state->afeoverride[core] =
-		    read_phy_reg(pi, (core == PHY_CORE_0) ? 0x8f : 0xa5);
-
-		mod_phy_reg(pi, ((core == PHY_CORE_0) ? 0xa6 : 0xa7),
-			    (0x1 << 0) | (0x1 << 1) | (0x1 << 2), 0);
-		mod_phy_reg(pi, ((core == PHY_CORE_0) ? 0x8f :
-				 0xa5),
-			    (0x1 << 0) |
-			    (0x1 << 1) |
-			    (0x1 << 2), (0x1 << 0) | (0x1 << 1) | (0x1 << 2));
-
-		state->vga_master[core] =
-		    READ_RADIO_REG2(pi, RADIO_2056, RX, core, VGA_MASTER);
-		WRITE_RADIO_REG2(pi, RADIO_2056, RX, core, VGA_MASTER, 0x2b);
-		if (CHSPEC_IS2G(pi->radio_chanspec)) {
-			state->fbmix[core] =
-			    READ_RADIO_REG2(pi, RADIO_2056, RX, core,
-					    TXFBMIX_G);
-			state->intpa_master[core] =
-			    READ_RADIO_REG2(pi, RADIO_2056, TX, core,
-					    INTPAG_MASTER);
-
-			WRITE_RADIO_REG2(pi, RADIO_2056, RX, core, TXFBMIX_G,
-					 0x03);
-			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
-					 INTPAG_MASTER, 0x04);
-		} else {
-			state->fbmix[core] =
-			    READ_RADIO_REG2(pi, RADIO_2056, RX, core,
-					    TXFBMIX_A);
-			state->intpa_master[core] =
-			    READ_RADIO_REG2(pi, RADIO_2056, TX, core,
-					    INTPAA_MASTER);
-
-			WRITE_RADIO_REG2(pi, RADIO_2056, RX, core, TXFBMIX_A,
-					 0x03);
-			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
-					 INTPAA_MASTER, 0x04);
-
-		}
-
-		tone_freq = 4000;
-
-		wlc_phy_tx_tone_nphy(pi, tone_freq, 181, 0, 0, false);
-
-		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x297 :
-			    0x29b, (0x1 << 0), (1) << 0);
-
-		mod_phy_reg(pi, (off_core == PHY_CORE_0) ? 0x297 :
-			    0x29b, (0x1 << 0), (0) << 0);
-
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 3), 0, 0x3, 0);
-	}
-}
-
-static void
-wlc_phy_papd_cal_cleanup_nphy(struct brcms_phy *pi,
-			      struct nphy_papd_restore_state *state)
-{
-	u8 core;
-
-	wlc_phy_stopplayback_nphy(pi);
-
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-
-		for (core = 0; core < pi->pubpi.phy_corenum; core++) {
-
-			if (CHSPEC_IS2G(pi->radio_chanspec)) {
-				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-						 TXRXCOUPLE_2G_PWRUP, 0);
-				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-						 TXRXCOUPLE_2G_ATTEN,
-						 state->atten[core]);
-			} else {
-				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-						 TXRXCOUPLE_5G_PWRUP, 0);
-				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-						 TXRXCOUPLE_5G_ATTEN,
-						 state->atten[core]);
-			}
-		}
-
-		if ((pi->pubpi.radiorev == 4) || (pi->pubpi.radiorev == 6)) {
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 2),
-							  1, 0x3, 0,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-		} else {
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 2),
-							  0, 0x3, 1,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-		}
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 1),
-						  0, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 0), 0, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID2);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 2), 0, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID2);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 11), 1, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 3), 0, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 11), 0, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 12), 0, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 2), 1, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 0), 0, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 1), 1, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID2);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 8), 0, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 9), 1, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 10), 0, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 3), 1, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 5), 0, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 4), 0, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-
-		for (core = 0; core < pi->pubpi.phy_corenum; core++) {
-
-			write_phy_reg(pi, (core == PHY_CORE_0) ?
-				      0xa6 : 0xa7, state->afectrl[core]);
-			write_phy_reg(pi, (core == PHY_CORE_0) ? 0x8f :
-				      0xa5, state->afeoverride[core]);
-		}
-
-		wlc_phy_ipa_set_bbmult_nphy(pi, (state->mm >> 8) & 0xff,
-					    (state->mm & 0xff));
-
-		if (NREV_IS(pi->pubpi.phy_rev, 7)
-		    || NREV_GE(pi->pubpi.phy_rev, 8)) {
-			wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 7), 0, 0,
-							  1,
-							  NPHY_REV7_RFCTRLOVERRIDE_ID1);
-		}
-	} else {
-
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 12), 0, 0x3, 1);
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 13), 0, 0x3, 1);
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 0), 0, 0x3, 1);
-
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 2), 0, 0x3, 1);
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 1), 0, 0x3, 1);
-
-		for (core = 0; core < pi->pubpi.phy_corenum; core++) {
-
-			WRITE_RADIO_REG2(pi, RADIO_2056, RX, core, VGA_MASTER,
-					 state->vga_master[core]);
-			if (CHSPEC_IS2G(pi->radio_chanspec)) {
-				WRITE_RADIO_REG2(pi, RADIO_2056, RX, core,
-						 TXFBMIX_G, state->fbmix[core]);
-				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
-						 INTPAG_MASTER,
-						 state->intpa_master[core]);
-			} else {
-				WRITE_RADIO_REG2(pi, RADIO_2056, RX, core,
-						 TXFBMIX_A, state->fbmix[core]);
-				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
-						 INTPAA_MASTER,
-						 state->intpa_master[core]);
-			}
-
-			write_phy_reg(pi, (core == PHY_CORE_0) ?
-				      0xa6 : 0xa7, state->afectrl[core]);
-			write_phy_reg(pi, (core == PHY_CORE_0) ? 0x8f :
-				      0xa5, state->afeoverride[core]);
-		}
-
-		wlc_phy_ipa_set_bbmult_nphy(pi, (state->mm >> 8) & 0xff,
-					    (state->mm & 0xff));
-
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 3), 0, 0x3, 1);
-	}
-}
-
-static void
-wlc_phy_a1_nphy(struct brcms_phy *pi, u8 core, u32 winsz, u32 start,
-		u32 end)
-{
-	u32 *buf, *src, *dst, sz;
-
-	sz = end - start + 1;
-
-	buf = kmalloc(2 * sizeof(u32) * NPHY_PAPD_EPS_TBL_SIZE, GFP_ATOMIC);
-	if (NULL == buf) {
-		return;
-	}
-
-	src = buf;
-	dst = buf + NPHY_PAPD_EPS_TBL_SIZE;
-
-	wlc_phy_table_read_nphy(pi,
-				(core ==
-				 PHY_CORE_0 ? NPHY_TBL_ID_EPSILONTBL0 :
-				 NPHY_TBL_ID_EPSILONTBL1),
-				NPHY_PAPD_EPS_TBL_SIZE, 0, 32, src);
-
-	do {
-		u32 phy_a1, phy_a2;
-		s32 phy_a3, phy_a4, phy_a5, phy_a6, phy_a7;
-
-		phy_a1 = end - min(end, (winsz >> 1));
-		phy_a2 = min_t(u32, NPHY_PAPD_EPS_TBL_SIZE - 1, end + (winsz >> 1));
-		phy_a3 = phy_a2 - phy_a1 + 1;
-		phy_a6 = 0;
-		phy_a7 = 0;
-
-		do {
-			wlc_phy_papd_decode_epsilon(src[phy_a2], &phy_a4,
-						    &phy_a5);
-			phy_a6 += phy_a4;
-			phy_a7 += phy_a5;
-		} while (phy_a2-- != phy_a1);
-
-		phy_a6 /= phy_a3;
-		phy_a7 /= phy_a3;
-		dst[end] = ((u32) phy_a7 << 13) | ((u32) phy_a6 & 0x1fff);
-	} while (end-- != start);
-
-	wlc_phy_table_write_nphy(pi,
-				 (core ==
-				  PHY_CORE_0) ? NPHY_TBL_ID_EPSILONTBL0 :
-				 NPHY_TBL_ID_EPSILONTBL1, sz, start, 32, dst);
-
-	kfree(buf);
-}
-
-static void
-wlc_phy_a2_nphy(struct brcms_phy *pi, struct nphy_ipa_txcalgains *txgains,
-		enum phy_cal_mode cal_mode, u8 core)
-{
-	u16 phy_a1, phy_a2, phy_a3;
-	u16 phy_a4, phy_a5;
-	bool phy_a6;
-	u8 phy_a7, m[2];
-	u32 phy_a8 = 0;
-	struct nphy_txgains phy_a9;
-
-	if (NREV_LT(pi->pubpi.phy_rev, 3))
-		return;
-
-	phy_a7 = (core == PHY_CORE_0) ? 1 : 0;
-
-	phy_a6 = ((cal_mode == CAL_GCTRL)
-		  || (cal_mode == CAL_SOFT)) ? true : false;
-
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-
-		phy_a9 = wlc_phy_get_tx_gain_nphy(pi);
-
-		if (CHSPEC_IS2G(pi->radio_chanspec)) {
-			phy_a5 = ((phy_a9.txlpf[core] << 15) |
-				  (phy_a9.txgm[core] << 12) |
-				  (phy_a9.pga[core] << 8) |
-				  (txgains->gains.pad[core] << 3) |
-				  (phy_a9.ipa[core]));
-		} else {
-			phy_a5 = ((phy_a9.txlpf[core] << 15) |
-				  (phy_a9.txgm[core] << 12) |
-				  (txgains->gains.pga[core] << 8) |
-				  (phy_a9.pad[core] << 3) | (phy_a9.ipa[core]));
-		}
-
-		wlc_phy_rfctrl_override_1tomany_nphy(pi,
-						     NPHY_REV7_RfctrlOverride_cmd_txgain,
-						     phy_a5, (1 << core), 0);
-
-		if (CHSPEC_IS2G(pi->radio_chanspec)) {
-			if ((pi->pubpi.radiorev <= 4)
-			    || (pi->pubpi.radiorev == 6)) {
-
-				m[core] = IS40MHZ(pi) ? 60 : 79;
-			} else {
-
-				m[core] = IS40MHZ(pi) ? 45 : 64;
-			}
-
-		} else {
-			m[core] = IS40MHZ(pi) ? 75 : 107;
-		}
-
-		m[phy_a7] = 0;
-		wlc_phy_ipa_set_bbmult_nphy(pi, m[0], m[1]);
-
-		phy_a2 = 63;
-
-		if (CHSPEC_IS2G(pi->radio_chanspec)) {
-			if ((pi->pubpi.radiorev == 4)
-			    || (pi->pubpi.radiorev == 6)) {
-				phy_a1 = 30;
-				phy_a3 = 30;
-			} else {
-				phy_a1 = 25;
-				phy_a3 = 25;
-			}
-		} else {
-			if ((pi->pubpi.radiorev == 5)
-			    || (pi->pubpi.radiorev == 7)
-			    || (pi->pubpi.radiorev == 8)) {
-				phy_a1 = 25;
-				phy_a3 = 25;
-			} else {
-				phy_a1 = 35;
-				phy_a3 = 35;
-			}
-		}
-
-		if (cal_mode == CAL_GCTRL) {
-			if ((pi->pubpi.radiorev == 5)
-			    && (CHSPEC_IS2G(pi->radio_chanspec))) {
-				phy_a1 = 55;
-			} else if (((pi->pubpi.radiorev == 7) &&
-				    (CHSPEC_IS2G(pi->radio_chanspec))) ||
-				   ((pi->pubpi.radiorev == 8) &&
-				    (CHSPEC_IS2G(pi->radio_chanspec)))) {
-				phy_a1 = 60;
-			} else {
-				phy_a1 = 63;
-			}
-
-		} else if ((cal_mode != CAL_FULL) && (cal_mode != CAL_SOFT)) {
-
-			phy_a1 = 35;
-			phy_a3 = 35;
-		}
-
-		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x297 :
-			    0x29b, (0x1 << 0), (1) << 0);
-
-		mod_phy_reg(pi, (phy_a7 == PHY_CORE_0) ? 0x297 :
-			    0x29b, (0x1 << 0), (0) << 0);
-
-		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
-			    0x2a4, (0x1 << 13), (1) << 13);
-
-		mod_phy_reg(pi, (phy_a7 == PHY_CORE_0) ? 0x2a3 :
-			    0x2a4, (0x1 << 13), (0) << 13);
-
-		write_phy_reg(pi, 0x2a1, 0x80);
-		write_phy_reg(pi, 0x2a2, 0x100);
-
-		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
-			    0x2a4, (0x7 << 4), (11) << 4);
-
-		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
-			    0x2a4, (0x7 << 8), (11) << 8);
-
-		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
-			    0x2a4, (0x7 << 0), (0x3) << 0);
-
-		write_phy_reg(pi, 0x2e5, 0x20);
-
-		mod_phy_reg(pi, 0x2a0, (0x3f << 0), (phy_a3) << 0);
-
-		mod_phy_reg(pi, 0x29f, (0x3f << 0), (phy_a1) << 0);
-
-		mod_phy_reg(pi, 0x29f, (0x3f << 8), (phy_a2) << 8);
-
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 3),
-						  1, ((core == 0) ? 1 : 2), 0,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 3),
-						  0, ((core == 0) ? 2 : 1), 0,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-
-		write_phy_reg(pi, 0x2be, 1);
-		SPINWAIT(read_phy_reg(pi, 0x2be), 10 * 1000 * 1000);
-
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 3),
-						  0, 0x3, 0,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-
-		wlc_phy_table_write_nphy(pi,
-					 (core ==
-					  PHY_CORE_0) ? NPHY_TBL_ID_EPSILONTBL0
-					 : NPHY_TBL_ID_EPSILONTBL1, 1, phy_a3,
-					 32, &phy_a8);
-
-		if (cal_mode != CAL_GCTRL) {
-			if (CHSPEC_IS5G(pi->radio_chanspec)) {
-				wlc_phy_a1_nphy(pi, core, 5, 0, 35);
-			}
-		}
-
-		wlc_phy_rfctrl_override_1tomany_nphy(pi,
-						     NPHY_REV7_RfctrlOverride_cmd_txgain,
-						     phy_a5, (1 << core), 1);
-
-	} else {
-
-		if (txgains) {
-			if (txgains->useindex) {
-				phy_a4 = 15 - ((txgains->index) >> 3);
-				if (CHSPEC_IS2G(pi->radio_chanspec)) {
-					if (NREV_GE(pi->pubpi.phy_rev, 6))
-						phy_a5 = 0x00f7 | (phy_a4 << 8);
-
-					else
-					    if (NREV_IS(pi->pubpi.phy_rev, 5))
-						phy_a5 = 0x10f7 | (phy_a4 << 8);
-					else
-						phy_a5 = 0x50f7 | (phy_a4 << 8);
-				} else {
-					phy_a5 = 0x70f7 | (phy_a4 << 8);
-				}
-				wlc_phy_rfctrl_override_nphy(pi,
-							     (0x1 << 13),
-							     phy_a5,
-							     (1 << core), 0);
-			} else {
-				wlc_phy_rfctrl_override_nphy(pi,
-							     (0x1 << 13),
-							     0x5bf7,
-							     (1 << core), 0);
-			}
-		}
-
-		if (CHSPEC_IS2G(pi->radio_chanspec)) {
-			m[core] = IS40MHZ(pi) ? 45 : 64;
-		} else {
-			m[core] = IS40MHZ(pi) ? 75 : 107;
-		}
-
-		m[phy_a7] = 0;
-		wlc_phy_ipa_set_bbmult_nphy(pi, m[0], m[1]);
-
-		phy_a2 = 63;
-
-		if (cal_mode == CAL_FULL) {
-			phy_a1 = 25;
-			phy_a3 = 25;
-		} else if (cal_mode == CAL_SOFT) {
-			phy_a1 = 25;
-			phy_a3 = 25;
-		} else if (cal_mode == CAL_GCTRL) {
-			phy_a1 = 63;
-			phy_a3 = 25;
-		} else {
-
-			phy_a1 = 25;
-			phy_a3 = 25;
-		}
-
-		mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x297 :
-			    0x29b, (0x1 << 0), (1) << 0);
-
-		mod_phy_reg(pi, (phy_a7 == PHY_CORE_0) ? 0x297 :
-			    0x29b, (0x1 << 0), (0) << 0);
-
-		if (NREV_GE(pi->pubpi.phy_rev, 6)) {
-			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
-				    0x2a4, (0x1 << 13), (1) << 13);
-
-			mod_phy_reg(pi, (phy_a7 == PHY_CORE_0) ? 0x2a3 :
-				    0x2a4, (0x1 << 13), (0) << 13);
-
-			write_phy_reg(pi, 0x2a1, 0x20);
-			write_phy_reg(pi, 0x2a2, 0x60);
-
-			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
-				    0x2a4, (0xf << 4), (9) << 4);
-
-			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
-				    0x2a4, (0xf << 8), (9) << 8);
-
-			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
-				    0x2a4, (0xf << 0), (0x2) << 0);
-
-			write_phy_reg(pi, 0x2e5, 0x20);
-		} else {
-			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
-				    0x2a4, (0x1 << 11), (1) << 11);
-
-			mod_phy_reg(pi, (phy_a7 == PHY_CORE_0) ? 0x2a3 :
-				    0x2a4, (0x1 << 11), (0) << 11);
-
-			write_phy_reg(pi, 0x2a1, 0x80);
-			write_phy_reg(pi, 0x2a2, 0x600);
-
-			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
-				    0x2a4, (0x7 << 4), (0) << 4);
-
-			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
-				    0x2a4, (0x7 << 8), (0) << 8);
-
-			mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x2a3 :
-				    0x2a4, (0x7 << 0), (0x3) << 0);
-
-			mod_phy_reg(pi, 0x2a0, (0x3f << 8), (0x20) << 8);
-
-		}
-
-		mod_phy_reg(pi, 0x2a0, (0x3f << 0), (phy_a3) << 0);
-
-		mod_phy_reg(pi, 0x29f, (0x3f << 0), (phy_a1) << 0);
-
-		mod_phy_reg(pi, 0x29f, (0x3f << 8), (phy_a2) << 8);
-
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 3), 1, 0x3, 0);
-
-		write_phy_reg(pi, 0x2be, 1);
-		SPINWAIT(read_phy_reg(pi, 0x2be), 10 * 1000 * 1000);
-
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 3), 0, 0x3, 0);
-
-		wlc_phy_table_write_nphy(pi,
-					 (core ==
-					  PHY_CORE_0) ? NPHY_TBL_ID_EPSILONTBL0
-					 : NPHY_TBL_ID_EPSILONTBL1, 1, phy_a3,
-					 32, &phy_a8);
-
-		if (cal_mode != CAL_GCTRL) {
-			wlc_phy_a1_nphy(pi, core, 5, 0, 40);
-		}
-	}
-}
-
-static u8 wlc_phy_a3_nphy(struct brcms_phy *pi, u8 start_gain, u8 core)
-{
-	int phy_a1;
-	int phy_a2;
-	bool phy_a3;
-	struct nphy_ipa_txcalgains phy_a4;
-	bool phy_a5 = false;
-	bool phy_a6 = true;
-	s32 phy_a7, phy_a8;
-	u32 phy_a9;
-	int phy_a10;
-	bool phy_a11 = false;
-	int phy_a12;
-	u8 phy_a13 = 0;
-	u8 phy_a14;
-	u8 *phy_a15 = NULL;
-
-	phy_a4.useindex = true;
-	phy_a12 = start_gain;
-
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-
-		phy_a2 = 20;
-		phy_a1 = 1;
-
-		if (CHSPEC_IS2G(pi->radio_chanspec)) {
-			if (pi->pubpi.radiorev == 5) {
-
-				phy_a15 = pad_gain_codes_used_2057rev5;
-				phy_a13 = sizeof(pad_gain_codes_used_2057rev5) /
-				    sizeof(pad_gain_codes_used_2057rev5[0]) - 1;
-
-			} else if ((pi->pubpi.radiorev == 7)
-				   || (pi->pubpi.radiorev == 8)) {
-
-				phy_a15 = pad_gain_codes_used_2057rev7;
-				phy_a13 = sizeof(pad_gain_codes_used_2057rev7) /
-				    sizeof(pad_gain_codes_used_2057rev7[0]) - 1;
-
-			} else {
-
-				phy_a15 = pad_all_gain_codes_2057;
-				phy_a13 = sizeof(pad_all_gain_codes_2057) /
-				    sizeof(pad_all_gain_codes_2057[0]) - 1;
-			}
-
-		} else {
-
-			phy_a15 = pga_all_gain_codes_2057;
-			phy_a13 = sizeof(pga_all_gain_codes_2057) /
-			    sizeof(pga_all_gain_codes_2057[0]) - 1;
-		}
-
-		phy_a14 = 0;
-
-		for (phy_a10 = 0; phy_a10 < phy_a2; phy_a10++) {
-			if (CHSPEC_IS2G(pi->radio_chanspec)) {
-				phy_a4.gains.pad[core] =
-				    (u16) phy_a15[phy_a12];
-			} else {
-				phy_a4.gains.pga[core] =
-				    (u16) phy_a15[phy_a12];
-			}
-
-			wlc_phy_a2_nphy(pi, &phy_a4, CAL_GCTRL, core);
-
-			wlc_phy_table_read_nphy(pi,
-						(core ==
-						 PHY_CORE_0 ?
-						 NPHY_TBL_ID_EPSILONTBL0 :
-						 NPHY_TBL_ID_EPSILONTBL1), 1,
-						63, 32, &phy_a9);
-
-			wlc_phy_papd_decode_epsilon(phy_a9, &phy_a7, &phy_a8);
-
-			phy_a3 = ((phy_a7 == 4095) || (phy_a7 == -4096) ||
-				  (phy_a8 == 4095) || (phy_a8 == -4096));
-
-			if (!phy_a6 && (phy_a3 != phy_a5)) {
-				if (!phy_a3) {
-					phy_a12 -= (u8) phy_a1;
-				}
-				phy_a11 = true;
-				break;
-			}
-
-			if (phy_a3)
-				phy_a12 += (u8) phy_a1;
-			else
-				phy_a12 -= (u8) phy_a1;
-
-			if ((phy_a12 < phy_a14) || (phy_a12 > phy_a13)) {
-				if (phy_a12 < phy_a14) {
-					phy_a12 = phy_a14;
-				} else {
-					phy_a12 = phy_a13;
-				}
-				phy_a11 = true;
-				break;
-			}
-
-			phy_a6 = false;
-			phy_a5 = phy_a3;
-		}
-
-	} else {
-		phy_a2 = 10;
-		phy_a1 = 8;
-		for (phy_a10 = 0; phy_a10 < phy_a2; phy_a10++) {
-			phy_a4.index = (u8) phy_a12;
-			wlc_phy_a2_nphy(pi, &phy_a4, CAL_GCTRL, core);
-
-			wlc_phy_table_read_nphy(pi,
-						(core ==
-						 PHY_CORE_0 ?
-						 NPHY_TBL_ID_EPSILONTBL0 :
-						 NPHY_TBL_ID_EPSILONTBL1), 1,
-						63, 32, &phy_a9);
-
-			wlc_phy_papd_decode_epsilon(phy_a9, &phy_a7, &phy_a8);
-
-			phy_a3 = ((phy_a7 == 4095) || (phy_a7 == -4096) ||
-				  (phy_a8 == 4095) || (phy_a8 == -4096));
-
-			if (!phy_a6 && (phy_a3 != phy_a5)) {
-				if (!phy_a3) {
-					phy_a12 -= (u8) phy_a1;
-				}
-				phy_a11 = true;
-				break;
-			}
-
-			if (phy_a3)
-				phy_a12 += (u8) phy_a1;
-			else
-				phy_a12 -= (u8) phy_a1;
-
-			if ((phy_a12 < 0) || (phy_a12 > 127)) {
-				if (phy_a12 < 0) {
-					phy_a12 = 0;
-				} else {
-					phy_a12 = 127;
-				}
-				phy_a11 = true;
-				break;
-			}
-
-			phy_a6 = false;
-			phy_a5 = phy_a3;
-		}
-
-	}
-
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-		return (u8) phy_a15[phy_a12];
-	} else {
-		return (u8) phy_a12;
-	}
-
-}
-
-static void wlc_phy_a4(struct brcms_phy *pi, bool full_cal)
-{
-	struct nphy_ipa_txcalgains phy_b1[2];
-	struct nphy_papd_restore_state phy_b2;
-	bool phy_b3;
-	u8 phy_b4;
-	u8 phy_b5;
-	s16 phy_b6, phy_b7, phy_b8;
-	u16 phy_b9;
-	s16 phy_b10, phy_b11, phy_b12;
-
-	phy_b11 = 0;
-	phy_b12 = 0;
-	phy_b7 = 0;
-	phy_b8 = 0;
-	phy_b6 = 0;
-
-	if (pi->nphy_papd_skip == 1)
-		return;
-
-	phy_b3 =
-	    (0 == (R_REG(&pi->regs->maccontrol) & MCTL_EN_MAC));
-	if (!phy_b3) {
-		wlapi_suspend_mac_and_wait(pi->sh->physhim);
-	}
-
-	wlc_phy_stay_in_carriersearch_nphy(pi, true);
-
-	pi->nphy_force_papd_cal = false;
-
-	for (phy_b5 = 0; phy_b5 < pi->pubpi.phy_corenum; phy_b5++)
-		pi->nphy_papd_tx_gain_at_last_cal[phy_b5] =
-		    wlc_phy_txpwr_idx_cur_get_nphy(pi, phy_b5);
-
-	pi->nphy_papd_last_cal = pi->sh->now;
-	pi->nphy_papd_recal_counter++;
-
-	if (NORADIO_ENAB(pi->pubpi))
-		return;
-
-	phy_b4 = pi->nphy_txpwrctrl;
-	wlc_phy_txpwrctrl_enable_nphy(pi, PHY_TPC_HW_OFF);
-
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_SCALARTBL0, 64, 0, 32,
-				 nphy_papd_scaltbl);
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_SCALARTBL1, 64, 0, 32,
-				 nphy_papd_scaltbl);
-
-	phy_b9 = read_phy_reg(pi, 0x01);
-	mod_phy_reg(pi, 0x01, (0x1 << 15), 0);
-
-	for (phy_b5 = 0; phy_b5 < pi->pubpi.phy_corenum; phy_b5++) {
-		s32 i, val = 0;
-		for (i = 0; i < 64; i++) {
-			wlc_phy_table_write_nphy(pi,
-						 ((phy_b5 ==
-						   PHY_CORE_0) ?
-						  NPHY_TBL_ID_EPSILONTBL0 :
-						  NPHY_TBL_ID_EPSILONTBL1), 1,
-						 i, 32, &val);
-		}
-	}
-
-	wlc_phy_ipa_restore_tx_digi_filts_nphy(pi);
-
-	phy_b2.mm = wlc_phy_ipa_get_bbmult_nphy(pi);
-	for (phy_b5 = 0; phy_b5 < pi->pubpi.phy_corenum; phy_b5++) {
-		wlc_phy_papd_cal_setup_nphy(pi, &phy_b2, phy_b5);
-
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			if (CHSPEC_IS2G(pi->radio_chanspec)) {
-
-				if ((pi->pubpi.radiorev == 3)
-				    || (pi->pubpi.radiorev == 4)
-				    || (pi->pubpi.radiorev == 6)) {
-
-					pi->nphy_papd_cal_gain_index[phy_b5] =
-					    23;
-
-				} else if (pi->pubpi.radiorev == 5) {
-
-					pi->nphy_papd_cal_gain_index[phy_b5] =
-					    0;
-					pi->nphy_papd_cal_gain_index[phy_b5] =
-					    wlc_phy_a3_nphy(pi,
-							    pi->
-							    nphy_papd_cal_gain_index
-							    [phy_b5], phy_b5);
-
-				} else if ((pi->pubpi.radiorev == 7)
-					   || (pi->pubpi.radiorev == 8)) {
-
-					pi->nphy_papd_cal_gain_index[phy_b5] =
-					    0;
-					pi->nphy_papd_cal_gain_index[phy_b5] =
-					    wlc_phy_a3_nphy(pi,
-							    pi->
-							    nphy_papd_cal_gain_index
-							    [phy_b5], phy_b5);
-
-				}
-
-				phy_b1[phy_b5].gains.pad[phy_b5] =
-				    pi->nphy_papd_cal_gain_index[phy_b5];
-
-			} else {
-				pi->nphy_papd_cal_gain_index[phy_b5] = 0;
-				pi->nphy_papd_cal_gain_index[phy_b5] =
-				    wlc_phy_a3_nphy(pi,
-						    pi->
-						    nphy_papd_cal_gain_index
-						    [phy_b5], phy_b5);
-				phy_b1[phy_b5].gains.pga[phy_b5] =
-				    pi->nphy_papd_cal_gain_index[phy_b5];
-			}
-		} else {
-			phy_b1[phy_b5].useindex = true;
-			phy_b1[phy_b5].index = 16;
-			phy_b1[phy_b5].index =
-			    wlc_phy_a3_nphy(pi, phy_b1[phy_b5].index, phy_b5);
-
-			pi->nphy_papd_cal_gain_index[phy_b5] =
-			    15 - ((phy_b1[phy_b5].index) >> 3);
-		}
-
-		switch (pi->nphy_papd_cal_type) {
-		case 0:
-			wlc_phy_a2_nphy(pi, &phy_b1[phy_b5], CAL_FULL, phy_b5);
-			break;
-		case 1:
-			wlc_phy_a2_nphy(pi, &phy_b1[phy_b5], CAL_SOFT, phy_b5);
-			break;
-		}
-
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			wlc_phy_papd_cal_cleanup_nphy(pi, &phy_b2);
-		}
-	}
-
-	if (NREV_LT(pi->pubpi.phy_rev, 7)) {
-		wlc_phy_papd_cal_cleanup_nphy(pi, &phy_b2);
-	}
-
-	for (phy_b5 = 0; phy_b5 < pi->pubpi.phy_corenum; phy_b5++) {
-		int eps_offset = 0;
-
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			if (CHSPEC_IS2G(pi->radio_chanspec)) {
-				if (pi->pubpi.radiorev == 3) {
-					eps_offset = -2;
-				} else if (pi->pubpi.radiorev == 5) {
-					eps_offset = 3;
-				} else {
-					eps_offset = -1;
-				}
-			} else {
-				eps_offset = 2;
-			}
-
-			if (CHSPEC_IS2G(pi->radio_chanspec)) {
-				phy_b8 = phy_b1[phy_b5].gains.pad[phy_b5];
-				phy_b10 = 0;
-				if ((pi->pubpi.radiorev == 3) ||
-				    (pi->pubpi.radiorev == 4) ||
-				    (pi->pubpi.radiorev == 6)) {
-					phy_b12 =
-					    -
-					    (nphy_papd_padgain_dlt_2g_2057rev3n4
-					     [phy_b8]
-					     + 1) / 2;
-					phy_b10 = -1;
-				} else if (pi->pubpi.radiorev == 5) {
-					phy_b12 =
-					    -(nphy_papd_padgain_dlt_2g_2057rev5
-					      [phy_b8]
-					      + 1) / 2;
-				} else if ((pi->pubpi.radiorev == 7) ||
-					   (pi->pubpi.radiorev == 8)) {
-					phy_b12 =
-					    -(nphy_papd_padgain_dlt_2g_2057rev7
-					      [phy_b8]
-					      + 1) / 2;
-				}
-			} else {
-				phy_b7 = phy_b1[phy_b5].gains.pga[phy_b5];
-				if ((pi->pubpi.radiorev == 3) ||
-				    (pi->pubpi.radiorev == 4) ||
-				    (pi->pubpi.radiorev == 6)) {
-					phy_b11 =
-					    -(nphy_papd_pgagain_dlt_5g_2057
-					      [phy_b7]
-					      + 1) / 2;
-				} else if ((pi->pubpi.radiorev == 7)
-					   || (pi->pubpi.radiorev == 8)) {
-					phy_b11 =
-					    -(nphy_papd_pgagain_dlt_5g_2057rev7
-					      [phy_b7]
-					      + 1) / 2;
-				}
-
-				phy_b10 = -9;
-			}
-
-			if (CHSPEC_IS2G(pi->radio_chanspec)) {
-				phy_b6 =
-				    -60 + 27 + eps_offset + phy_b12 + phy_b10;
-			} else {
-				phy_b6 =
-				    -60 + 27 + eps_offset + phy_b11 + phy_b10;
-			}
-
-			mod_phy_reg(pi, (phy_b5 == PHY_CORE_0) ? 0x298 :
-				    0x29c, (0x1ff << 7), (phy_b6) << 7);
-
-			pi->nphy_papd_epsilon_offset[phy_b5] = phy_b6;
-		} else {
-			if (NREV_LT(pi->pubpi.phy_rev, 5)) {
-				eps_offset = 4;
-			} else {
-				eps_offset = 2;
-			}
-
-			phy_b7 = 15 - ((phy_b1[phy_b5].index) >> 3);
-
-			if (CHSPEC_IS2G(pi->radio_chanspec)) {
-				phy_b11 =
-				    -(nphy_papd_pga_gain_delta_ipa_2g[phy_b7] +
-				      1) / 2;
-				phy_b10 = 0;
-			} else {
-				phy_b11 =
-				    -(nphy_papd_pga_gain_delta_ipa_5g[phy_b7] +
-				      1) / 2;
-				phy_b10 = -9;
-			}
-
-			phy_b6 = -60 + 27 + eps_offset + phy_b11 + phy_b10;
-
-			mod_phy_reg(pi, (phy_b5 == PHY_CORE_0) ? 0x298 :
-				    0x29c, (0x1ff << 7), (phy_b6) << 7);
-
-			pi->nphy_papd_epsilon_offset[phy_b5] = phy_b6;
-		}
-	}
-
-	mod_phy_reg(pi, (0 == PHY_CORE_0) ? 0x297 :
-		    0x29b, (0x1 << 0), (NPHY_PAPD_COMP_ON) << 0);
-
-	mod_phy_reg(pi, (1 == PHY_CORE_0) ? 0x297 :
-		    0x29b, (0x1 << 0), (NPHY_PAPD_COMP_ON) << 0);
-
-	if (NREV_GE(pi->pubpi.phy_rev, 6)) {
-		mod_phy_reg(pi, (0 == PHY_CORE_0) ? 0x2a3 :
-			    0x2a4, (0x1 << 13), (0) << 13);
-
-		mod_phy_reg(pi, (1 == PHY_CORE_0) ? 0x2a3 :
-			    0x2a4, (0x1 << 13), (0) << 13);
-
-	} else {
-		mod_phy_reg(pi, (0 == PHY_CORE_0) ? 0x2a3 :
-			    0x2a4, (0x1 << 11), (0) << 11);
-
-		mod_phy_reg(pi, (1 == PHY_CORE_0) ? 0x2a3 :
-			    0x2a4, (0x1 << 11), (0) << 11);
-
-	}
-	pi->nphy_papdcomp = NPHY_PAPD_COMP_ON;
-
-	write_phy_reg(pi, 0x01, phy_b9);
-
-	wlc_phy_ipa_set_tx_digi_filts_nphy(pi);
-
-	wlc_phy_txpwrctrl_enable_nphy(pi, phy_b4);
-	if (phy_b4 == PHY_TPC_HW_OFF) {
-		wlc_phy_txpwr_index_nphy(pi, (1 << 0),
-					 (s8) (pi->nphy_txpwrindex[0].
-						 index_internal), false);
-		wlc_phy_txpwr_index_nphy(pi, (1 << 1),
-					 (s8) (pi->nphy_txpwrindex[1].
-						 index_internal), false);
-	}
-
-	wlc_phy_stay_in_carriersearch_nphy(pi, false);
-
-	if (!phy_b3) {
-		wlapi_enable_mac(pi->sh->physhim);
-	}
 }
 
 void wlc_phy_txpwr_fixpower_nphy(struct brcms_phy *pi)
@@ -27423,11 +28015,10 @@ void wlc_phy_txpwr_fixpower_nphy(struct brcms_phy *pi)
 		}
 	}
 
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
+	if (NREV_GE(pi->pubpi.phy_rev, 7))
 		txpi[0] = txpi[1] = 30;
-	} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+	else if (NREV_GE(pi->pubpi.phy_rev, 3))
 		txpi[0] = txpi[1] = 40;
-	}
 
 	if (NREV_LT(pi->pubpi.phy_rev, 7)) {
 
@@ -27447,37 +28038,36 @@ void wlc_phy_txpwr_fixpower_nphy(struct brcms_phy *pi)
 		if (NREV_GE(phyrev, 3)) {
 			if (PHY_IPA(pi)) {
 				u32 *tx_gaintbl =
-				    wlc_phy_get_ipa_gaintbl_nphy(pi);
+					wlc_phy_get_ipa_gaintbl_nphy(pi);
 				txgain = tx_gaintbl[txpi[core]];
 			} else {
 				if (CHSPEC_IS5G(pi->radio_chanspec)) {
 					if (NREV_IS(phyrev, 3)) {
 						txgain =
-						    nphy_tpc_5GHz_txgain_rev3
-						    [txpi[core]];
+						      nphy_tpc_5GHz_txgain_rev3
+								   [txpi[core]];
 					} else if (NREV_IS(phyrev, 4)) {
-						txgain =
-						    (pi->srom_fem5g.extpagain ==
-						     3) ?
-						    nphy_tpc_5GHz_txgain_HiPwrEPA
-						    [txpi[core]] :
-						    nphy_tpc_5GHz_txgain_rev4
-						    [txpi[core]];
+						txgain = (
+						  pi->srom_fem5g.extpagain ==
+						  3) ?
+						  nphy_tpc_5GHz_txgain_HiPwrEPA
+						 [txpi[core]] :
+						 nphy_tpc_5GHz_txgain_rev4
+						 [txpi[core]];
 					} else {
 						txgain =
-						    nphy_tpc_5GHz_txgain_rev5
-						    [txpi[core]];
+						      nphy_tpc_5GHz_txgain_rev5
+								   [txpi[core]];
 					}
 				} else {
 					if (NREV_GE(phyrev, 5) &&
 					    (pi->srom_fem2g.extpagain == 3)) {
 						txgain =
-						    nphy_tpc_txgain_HiPwrEPA
-						    [txpi[core]];
+							nphy_tpc_txgain_HiPwrEPA
+							[txpi[core]];
 					} else {
-						txgain =
-						    nphy_tpc_txgain_rev3[txpi
-									 [core]];
+						txgain = nphy_tpc_txgain_rev3
+							 [txpi[core]];
 					}
 				}
 			}
@@ -27497,12 +28087,12 @@ void wlc_phy_txpwr_fixpower_nphy(struct brcms_phy *pi)
 
 		bbmult = (txgain >> 0) & ((1 << (7 - 0 + 1)) - 1);
 
-		if (NREV_GE(phyrev, 3)) {
+		if (NREV_GE(phyrev, 3))
 			mod_phy_reg(pi, ((core == PHY_CORE_0) ? 0x8f :
 					 0xa5), (0x1 << 8), (0x1 << 8));
-		} else {
+		else
 			mod_phy_reg(pi, 0xa5, (0x1 << 14), (0x1 << 14));
-		}
+
 		write_phy_reg(pi, (core == PHY_CORE_0) ? 0xaa : 0xab, dac_gain);
 
 		wlc_phy_table_write_nphy(pi, 7, 1, (0x110 + core), 16,
@@ -27562,9 +28152,8 @@ wlc_phy_txpwr_nphy_po_apply(u8 *srom_max, u8 pwr_offset,
 {
 	u8 rate;
 
-	for (rate = rate_start; rate <= rate_end; rate++) {
+	for (rate = rate_start; rate <= rate_end; rate++)
 		srom_max[rate] -= 2 * pwr_offset;
-	}
 }
 
 void
@@ -27603,7 +28192,8 @@ void wlc_phy_txpwr_apply_nphy(struct brcms_phy *pi)
 	u16 pwr_offsets1[2], *pwr_offsets2 = NULL;
 	u8 *tx_srom_max_rate = NULL;
 
-	for (band_num = 0; band_num < (CH_2G_GROUP + CH_5G_GROUP); band_num++) {
+	for (band_num = 0; band_num < (CH_2G_GROUP + CH_5G_GROUP);
+	     band_num++) {
 		switch (band_num) {
 		case 0:
 
@@ -27619,7 +28209,7 @@ void wlc_phy_txpwr_apply_nphy(struct brcms_phy *pi)
 
 			pwr_offsets1[0] = (u16) (pi->ofdm2gpo & 0xffff);
 			pwr_offsets1[1] =
-			    (u16) (pi->ofdm2gpo >> 16) & 0xffff;
+				(u16) (pi->ofdm2gpo >> 16) & 0xffff;
 
 			pwr_offsets2 = pi->mcs2gpo;
 
@@ -27636,7 +28226,7 @@ void wlc_phy_txpwr_apply_nphy(struct brcms_phy *pi)
 
 			pwr_offsets1[0] = (u16) (pi->ofdm5gpo & 0xffff);
 			pwr_offsets1[1] =
-			    (u16) (pi->ofdm5gpo >> 16) & 0xffff;
+				(u16) (pi->ofdm5gpo >> 16) & 0xffff;
 
 			pwr_offsets2 = pi->mcs5gpo;
 
@@ -27653,7 +28243,7 @@ void wlc_phy_txpwr_apply_nphy(struct brcms_phy *pi)
 
 			pwr_offsets1[0] = (u16) (pi->ofdm5glpo & 0xffff);
 			pwr_offsets1[1] =
-			    (u16) (pi->ofdm5glpo >> 16) & 0xffff;
+				(u16) (pi->ofdm5glpo >> 16) & 0xffff;
 
 			pwr_offsets2 = pi->mcs5glpo;
 
@@ -27670,7 +28260,7 @@ void wlc_phy_txpwr_apply_nphy(struct brcms_phy *pi)
 
 			pwr_offsets1[0] = (u16) (pi->ofdm5ghpo & 0xffff);
 			pwr_offsets1[1] =
-			    (u16) (pi->ofdm5ghpo >> 16) & 0xffff;
+				(u16) (pi->ofdm5ghpo >> 16) & 0xffff;
 
 			pwr_offsets2 = pi->mcs5ghpo;
 
@@ -27696,12 +28286,10 @@ void wlc_phy_txpwr_apply_nphy(struct brcms_phy *pi)
 						TXP_FIRST_MCS_20_CDD,
 						TXP_LAST_MCS_20_CDD);
 
-		if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-
+		if (NREV_GE(pi->pubpi.phy_rev, 3))
 			wlc_phy_txpwr_nphy_po_apply(tx_srom_max_rate, tmp_cddpo,
 						    TXP_FIRST_MCS_20_CDD,
 						    TXP_LAST_MCS_20_CDD);
-		}
 
 		wlc_phy_mcs_to_ofdm_powers_nphy(tx_srom_max_rate,
 						TXP_FIRST_OFDM_20_CDD,
@@ -27713,13 +28301,11 @@ void wlc_phy_txpwr_apply_nphy(struct brcms_phy *pi)
 						TXP_FIRST_MCS_20_STBC,
 						TXP_LAST_MCS_20_STBC);
 
-		if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-
+		if (NREV_GE(pi->pubpi.phy_rev, 3))
 			wlc_phy_txpwr_nphy_po_apply(tx_srom_max_rate,
 						    tmp_stbcpo,
 						    TXP_FIRST_MCS_20_STBC,
 						    TXP_LAST_MCS_20_STBC);
-		}
 
 		wlc_phy_txpwr_nphy_srom_convert(tx_srom_max_rate,
 						&pwr_offsets2[2], tmp_max_pwr,
@@ -27773,294 +28359,26 @@ void wlc_phy_txpwr_apply_nphy(struct brcms_phy *pi)
 		} else {
 
 			for (rate1 = TXP_FIRST_OFDM_40_SISO, rate2 =
-			     TXP_FIRST_OFDM; rate1 <= TXP_LAST_MCS_40_SDM;
+				     TXP_FIRST_OFDM;
+			     rate1 <= TXP_LAST_MCS_40_SDM;
 			     rate1++, rate2++)
 				tx_srom_max_rate[rate1] =
-				    tx_srom_max_rate[rate2];
+					tx_srom_max_rate[rate2];
 		}
 
-		if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+		if (NREV_GE(pi->pubpi.phy_rev, 3))
 			wlc_phy_txpwr_nphy_po_apply(tx_srom_max_rate,
 						    tmp_bw40po,
 						    TXP_FIRST_OFDM_40_SISO,
 						    TXP_LAST_MCS_40_SDM);
-		}
 
 		tx_srom_max_rate[TXP_MCS_32] =
-		    tx_srom_max_rate[TXP_FIRST_MCS_40_CDD];
+			tx_srom_max_rate[TXP_FIRST_MCS_40_CDD];
 	}
 
 	return;
 }
 
-static void wlc_phy_txpwr_srom_read_ppr_nphy(struct brcms_phy *pi)
-{
-	u16 bw40po, cddpo, stbcpo, bwduppo;
-	uint band_num;
-
-	if (pi->sh->sromrev >= 9) {
-
-		return;
-	}
-
-	bw40po = (u16) PHY_GETINTVAR(pi, "bw40po");
-	pi->bw402gpo = bw40po & 0xf;
-	pi->bw405gpo = (bw40po & 0xf0) >> 4;
-	pi->bw405glpo = (bw40po & 0xf00) >> 8;
-	pi->bw405ghpo = (bw40po & 0xf000) >> 12;
-
-	cddpo = (u16) PHY_GETINTVAR(pi, "cddpo");
-	pi->cdd2gpo = cddpo & 0xf;
-	pi->cdd5gpo = (cddpo & 0xf0) >> 4;
-	pi->cdd5glpo = (cddpo & 0xf00) >> 8;
-	pi->cdd5ghpo = (cddpo & 0xf000) >> 12;
-
-	stbcpo = (u16) PHY_GETINTVAR(pi, "stbcpo");
-	pi->stbc2gpo = stbcpo & 0xf;
-	pi->stbc5gpo = (stbcpo & 0xf0) >> 4;
-	pi->stbc5glpo = (stbcpo & 0xf00) >> 8;
-	pi->stbc5ghpo = (stbcpo & 0xf000) >> 12;
-
-	bwduppo = (u16) PHY_GETINTVAR(pi, "bwduppo");
-	pi->bwdup2gpo = bwduppo & 0xf;
-	pi->bwdup5gpo = (bwduppo & 0xf0) >> 4;
-	pi->bwdup5glpo = (bwduppo & 0xf00) >> 8;
-	pi->bwdup5ghpo = (bwduppo & 0xf000) >> 12;
-
-	for (band_num = 0; band_num < (CH_2G_GROUP + CH_5G_GROUP); band_num++) {
-		switch (band_num) {
-		case 0:
-
-			pi->nphy_txpid2g[PHY_CORE_0] =
-			    (u8) PHY_GETINTVAR(pi, "txpid2ga0");
-			pi->nphy_txpid2g[PHY_CORE_1] =
-			    (u8) PHY_GETINTVAR(pi, "txpid2ga1");
-			pi->nphy_pwrctrl_info[PHY_CORE_0].max_pwr_2g =
-			    (s8) PHY_GETINTVAR(pi, "maxp2ga0");
-			pi->nphy_pwrctrl_info[PHY_CORE_1].max_pwr_2g =
-			    (s8) PHY_GETINTVAR(pi, "maxp2ga1");
-			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_2g_a1 =
-			    (s16) PHY_GETINTVAR(pi, "pa2gw0a0");
-			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_2g_a1 =
-			    (s16) PHY_GETINTVAR(pi, "pa2gw0a1");
-			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_2g_b0 =
-			    (s16) PHY_GETINTVAR(pi, "pa2gw1a0");
-			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_2g_b0 =
-			    (s16) PHY_GETINTVAR(pi, "pa2gw1a1");
-			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_2g_b1 =
-			    (s16) PHY_GETINTVAR(pi, "pa2gw2a0");
-			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_2g_b1 =
-			    (s16) PHY_GETINTVAR(pi, "pa2gw2a1");
-			pi->nphy_pwrctrl_info[PHY_CORE_0].idle_targ_2g =
-			    (s8) PHY_GETINTVAR(pi, "itt2ga0");
-			pi->nphy_pwrctrl_info[PHY_CORE_1].idle_targ_2g =
-			    (s8) PHY_GETINTVAR(pi, "itt2ga1");
-
-			pi->cck2gpo = (u16) PHY_GETINTVAR(pi, "cck2gpo");
-
-			pi->ofdm2gpo = (u32) PHY_GETINTVAR(pi, "ofdm2gpo");
-
-			pi->mcs2gpo[0] = (u16) PHY_GETINTVAR(pi, "mcs2gpo0");
-			pi->mcs2gpo[1] = (u16) PHY_GETINTVAR(pi, "mcs2gpo1");
-			pi->mcs2gpo[2] = (u16) PHY_GETINTVAR(pi, "mcs2gpo2");
-			pi->mcs2gpo[3] = (u16) PHY_GETINTVAR(pi, "mcs2gpo3");
-			pi->mcs2gpo[4] = (u16) PHY_GETINTVAR(pi, "mcs2gpo4");
-			pi->mcs2gpo[5] = (u16) PHY_GETINTVAR(pi, "mcs2gpo5");
-			pi->mcs2gpo[6] = (u16) PHY_GETINTVAR(pi, "mcs2gpo6");
-			pi->mcs2gpo[7] = (u16) PHY_GETINTVAR(pi, "mcs2gpo7");
-			break;
-		case 1:
-
-			pi->nphy_txpid5g[PHY_CORE_0] =
-			    (u8) PHY_GETINTVAR(pi, "txpid5ga0");
-			pi->nphy_txpid5g[PHY_CORE_1] =
-			    (u8) PHY_GETINTVAR(pi, "txpid5ga1");
-			pi->nphy_pwrctrl_info[PHY_CORE_0].max_pwr_5gm =
-			    (s8) PHY_GETINTVAR(pi, "maxp5ga0");
-			pi->nphy_pwrctrl_info[PHY_CORE_1].max_pwr_5gm =
-			    (s8) PHY_GETINTVAR(pi, "maxp5ga1");
-			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_5gm_a1 =
-			    (s16) PHY_GETINTVAR(pi, "pa5gw0a0");
-			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_5gm_a1 =
-			    (s16) PHY_GETINTVAR(pi, "pa5gw0a1");
-			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_5gm_b0 =
-			    (s16) PHY_GETINTVAR(pi, "pa5gw1a0");
-			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_5gm_b0 =
-			    (s16) PHY_GETINTVAR(pi, "pa5gw1a1");
-			pi->nphy_pwrctrl_info[PHY_CORE_0].pwrdet_5gm_b1 =
-			    (s16) PHY_GETINTVAR(pi, "pa5gw2a0");
-			pi->nphy_pwrctrl_info[PHY_CORE_1].pwrdet_5gm_b1 =
-			    (s16) PHY_GETINTVAR(pi, "pa5gw2a1");
-			pi->nphy_pwrctrl_info[PHY_CORE_0].idle_targ_5gm =
-			    (s8) PHY_GETINTVAR(pi, "itt5ga0");
-			pi->nphy_pwrctrl_info[PHY_CORE_1].idle_targ_5gm =
-			    (s8) PHY_GETINTVAR(pi, "itt5ga1");
-
-			pi->ofdm5gpo = (u32) PHY_GETINTVAR(pi, "ofdm5gpo");
-
-			pi->mcs5gpo[0] = (u16) PHY_GETINTVAR(pi, "mcs5gpo0");
-			pi->mcs5gpo[1] = (u16) PHY_GETINTVAR(pi, "mcs5gpo1");
-			pi->mcs5gpo[2] = (u16) PHY_GETINTVAR(pi, "mcs5gpo2");
-			pi->mcs5gpo[3] = (u16) PHY_GETINTVAR(pi, "mcs5gpo3");
-			pi->mcs5gpo[4] = (u16) PHY_GETINTVAR(pi, "mcs5gpo4");
-			pi->mcs5gpo[5] = (u16) PHY_GETINTVAR(pi, "mcs5gpo5");
-			pi->mcs5gpo[6] = (u16) PHY_GETINTVAR(pi, "mcs5gpo6");
-			pi->mcs5gpo[7] = (u16) PHY_GETINTVAR(pi, "mcs5gpo7");
-			break;
-		case 2:
-
-			pi->nphy_txpid5gl[0] =
-			    (u8) PHY_GETINTVAR(pi, "txpid5gla0");
-			pi->nphy_txpid5gl[1] =
-			    (u8) PHY_GETINTVAR(pi, "txpid5gla1");
-			pi->nphy_pwrctrl_info[0].max_pwr_5gl =
-			    (s8) PHY_GETINTVAR(pi, "maxp5gla0");
-			pi->nphy_pwrctrl_info[1].max_pwr_5gl =
-			    (s8) PHY_GETINTVAR(pi, "maxp5gla1");
-			pi->nphy_pwrctrl_info[0].pwrdet_5gl_a1 =
-			    (s16) PHY_GETINTVAR(pi, "pa5glw0a0");
-			pi->nphy_pwrctrl_info[1].pwrdet_5gl_a1 =
-			    (s16) PHY_GETINTVAR(pi, "pa5glw0a1");
-			pi->nphy_pwrctrl_info[0].pwrdet_5gl_b0 =
-			    (s16) PHY_GETINTVAR(pi, "pa5glw1a0");
-			pi->nphy_pwrctrl_info[1].pwrdet_5gl_b0 =
-			    (s16) PHY_GETINTVAR(pi, "pa5glw1a1");
-			pi->nphy_pwrctrl_info[0].pwrdet_5gl_b1 =
-			    (s16) PHY_GETINTVAR(pi, "pa5glw2a0");
-			pi->nphy_pwrctrl_info[1].pwrdet_5gl_b1 =
-			    (s16) PHY_GETINTVAR(pi, "pa5glw2a1");
-			pi->nphy_pwrctrl_info[0].idle_targ_5gl = 0;
-			pi->nphy_pwrctrl_info[1].idle_targ_5gl = 0;
-
-			pi->ofdm5glpo = (u32) PHY_GETINTVAR(pi, "ofdm5glpo");
-
-			pi->mcs5glpo[0] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5glpo0");
-			pi->mcs5glpo[1] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5glpo1");
-			pi->mcs5glpo[2] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5glpo2");
-			pi->mcs5glpo[3] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5glpo3");
-			pi->mcs5glpo[4] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5glpo4");
-			pi->mcs5glpo[5] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5glpo5");
-			pi->mcs5glpo[6] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5glpo6");
-			pi->mcs5glpo[7] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5glpo7");
-			break;
-		case 3:
-
-			pi->nphy_txpid5gh[0] =
-			    (u8) PHY_GETINTVAR(pi, "txpid5gha0");
-			pi->nphy_txpid5gh[1] =
-			    (u8) PHY_GETINTVAR(pi, "txpid5gha1");
-			pi->nphy_pwrctrl_info[0].max_pwr_5gh =
-			    (s8) PHY_GETINTVAR(pi, "maxp5gha0");
-			pi->nphy_pwrctrl_info[1].max_pwr_5gh =
-			    (s8) PHY_GETINTVAR(pi, "maxp5gha1");
-			pi->nphy_pwrctrl_info[0].pwrdet_5gh_a1 =
-			    (s16) PHY_GETINTVAR(pi, "pa5ghw0a0");
-			pi->nphy_pwrctrl_info[1].pwrdet_5gh_a1 =
-			    (s16) PHY_GETINTVAR(pi, "pa5ghw0a1");
-			pi->nphy_pwrctrl_info[0].pwrdet_5gh_b0 =
-			    (s16) PHY_GETINTVAR(pi, "pa5ghw1a0");
-			pi->nphy_pwrctrl_info[1].pwrdet_5gh_b0 =
-			    (s16) PHY_GETINTVAR(pi, "pa5ghw1a1");
-			pi->nphy_pwrctrl_info[0].pwrdet_5gh_b1 =
-			    (s16) PHY_GETINTVAR(pi, "pa5ghw2a0");
-			pi->nphy_pwrctrl_info[1].pwrdet_5gh_b1 =
-			    (s16) PHY_GETINTVAR(pi, "pa5ghw2a1");
-			pi->nphy_pwrctrl_info[0].idle_targ_5gh = 0;
-			pi->nphy_pwrctrl_info[1].idle_targ_5gh = 0;
-
-			pi->ofdm5ghpo = (u32) PHY_GETINTVAR(pi, "ofdm5ghpo");
-
-			pi->mcs5ghpo[0] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5ghpo0");
-			pi->mcs5ghpo[1] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5ghpo1");
-			pi->mcs5ghpo[2] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5ghpo2");
-			pi->mcs5ghpo[3] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5ghpo3");
-			pi->mcs5ghpo[4] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5ghpo4");
-			pi->mcs5ghpo[5] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5ghpo5");
-			pi->mcs5ghpo[6] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5ghpo6");
-			pi->mcs5ghpo[7] =
-			    (u16) PHY_GETINTVAR(pi, "mcs5ghpo7");
-			break;
-		}
-	}
-
-	wlc_phy_txpwr_apply_nphy(pi);
-}
-
-static bool wlc_phy_txpwr_srom_read_nphy(struct brcms_phy *pi)
-{
-
-	pi->antswitch = (u8) PHY_GETINTVAR(pi, "antswitch");
-	pi->aa2g = (u8) PHY_GETINTVAR(pi, "aa2g");
-	pi->aa5g = (u8) PHY_GETINTVAR(pi, "aa5g");
-
-	pi->srom_fem2g.tssipos = (u8) PHY_GETINTVAR(pi, "tssipos2g");
-	pi->srom_fem2g.extpagain = (u8) PHY_GETINTVAR(pi, "extpagain2g");
-	pi->srom_fem2g.pdetrange = (u8) PHY_GETINTVAR(pi, "pdetrange2g");
-	pi->srom_fem2g.triso = (u8) PHY_GETINTVAR(pi, "triso2g");
-	pi->srom_fem2g.antswctrllut = (u8) PHY_GETINTVAR(pi, "antswctl2g");
-
-	pi->srom_fem5g.tssipos = (u8) PHY_GETINTVAR(pi, "tssipos5g");
-	pi->srom_fem5g.extpagain = (u8) PHY_GETINTVAR(pi, "extpagain5g");
-	pi->srom_fem5g.pdetrange = (u8) PHY_GETINTVAR(pi, "pdetrange5g");
-	pi->srom_fem5g.triso = (u8) PHY_GETINTVAR(pi, "triso5g");
-	if (PHY_GETVAR(pi, "antswctl5g")) {
-
-		pi->srom_fem5g.antswctrllut =
-		    (u8) PHY_GETINTVAR(pi, "antswctl5g");
-	} else {
-
-		pi->srom_fem5g.antswctrllut =
-		    (u8) PHY_GETINTVAR(pi, "antswctl2g");
-	}
-
-	wlc_phy_txpower_ipa_upd(pi);
-
-	pi->phy_txcore_disable_temp = (s16) PHY_GETINTVAR(pi, "tempthresh");
-	if (pi->phy_txcore_disable_temp == 0) {
-		pi->phy_txcore_disable_temp = PHY_CHAIN_TX_DISABLE_TEMP;
-	}
-
-	pi->phy_tempsense_offset = (s8) PHY_GETINTVAR(pi, "tempoffset");
-	if (pi->phy_tempsense_offset != 0) {
-		if (pi->phy_tempsense_offset >
-		    (NPHY_SROM_TEMPSHIFT + NPHY_SROM_MAXTEMPOFFSET)) {
-			pi->phy_tempsense_offset = NPHY_SROM_MAXTEMPOFFSET;
-		} else if (pi->phy_tempsense_offset < (NPHY_SROM_TEMPSHIFT +
-						    NPHY_SROM_MINTEMPOFFSET)) {
-			pi->phy_tempsense_offset = NPHY_SROM_MINTEMPOFFSET;
-		} else {
-			pi->phy_tempsense_offset -= NPHY_SROM_TEMPSHIFT;
-		}
-	}
-
-	pi->phy_txcore_enable_temp =
-	    pi->phy_txcore_disable_temp - PHY_HYSTERESIS_DELTATEMP;
-
-	pi->phycal_tempdelta = (u8) PHY_GETINTVAR(pi, "phycal_tempdelta");
-	if (pi->phycal_tempdelta > NPHY_CAL_MAXTEMPDELTA) {
-		pi->phycal_tempdelta = 0;
-	}
-
-	wlc_phy_txpwr_srom_read_ppr_nphy(pi);
-
-	return true;
-}
-
 void wlc_phy_txpower_recalc_target_nphy(struct brcms_phy *pi)
 {
 	u8 tx_pwr_ctrl_state;
@@ -28081,512 +28399,10 @@ void wlc_phy_txpower_recalc_target_nphy(struct brcms_phy *pi)
 		wlapi_bmac_mctrl(pi->sh->physhim, MCTL_PHYLOCK, 0);
 }
 
-static void wlc_phy_txpwrctrl_coeff_setup_nphy(struct brcms_phy *pi)
-{
-	u32 idx;
-	u16 iqloCalbuf[7];
-	u32 iqcomp, locomp, curr_locomp;
-	s8 locomp_i, locomp_q;
-	s8 curr_locomp_i, curr_locomp_q;
-	u32 tbl_id, tbl_len, tbl_offset;
-	u32 regval[128];
-
-	if (pi->phyhang_avoid)
-		wlc_phy_stay_in_carriersearch_nphy(pi, true);
-
-	wlc_phy_table_read_nphy(pi, 15, 7, 80, 16, iqloCalbuf);
-
-	tbl_len = 128;
-	tbl_offset = 320;
-	for (tbl_id = NPHY_TBL_ID_CORE1TXPWRCTL;
-	     tbl_id <= NPHY_TBL_ID_CORE2TXPWRCTL; tbl_id++) {
-		iqcomp =
-		    (tbl_id ==
-		     26) ? (((u32) (iqloCalbuf[0] & 0x3ff)) << 10) |
-		    (iqloCalbuf[1] & 0x3ff)
-		    : (((u32) (iqloCalbuf[2] & 0x3ff)) << 10) |
-		    (iqloCalbuf[3] & 0x3ff);
-
-		for (idx = 0; idx < tbl_len; idx++) {
-			regval[idx] = iqcomp;
-		}
-		wlc_phy_table_write_nphy(pi, tbl_id, tbl_len, tbl_offset, 32,
-					 regval);
-	}
-
-	tbl_offset = 448;
-	for (tbl_id = NPHY_TBL_ID_CORE1TXPWRCTL;
-	     tbl_id <= NPHY_TBL_ID_CORE2TXPWRCTL; tbl_id++) {
-
-		locomp =
-		    (u32) ((tbl_id == 26) ? iqloCalbuf[5] : iqloCalbuf[6]);
-		locomp_i = (s8) ((locomp >> 8) & 0xff);
-		locomp_q = (s8) ((locomp) & 0xff);
-		for (idx = 0; idx < tbl_len; idx++) {
-			if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-				curr_locomp_i = locomp_i;
-				curr_locomp_q = locomp_q;
-			} else {
-				curr_locomp_i = (s8) ((locomp_i *
-							 nphy_tpc_loscale[idx] +
-							 128) >> 8);
-				curr_locomp_q =
-				    (s8) ((locomp_q * nphy_tpc_loscale[idx] +
-					     128) >> 8);
-			}
-			curr_locomp = (u32) ((curr_locomp_i & 0xff) << 8);
-			curr_locomp |= (u32) (curr_locomp_q & 0xff);
-			regval[idx] = curr_locomp;
-		}
-		wlc_phy_table_write_nphy(pi, tbl_id, tbl_len, tbl_offset, 32,
-					 regval);
-	}
-
-	if (NREV_LT(pi->pubpi.phy_rev, 2)) {
-
-		wlapi_bmac_write_shm(pi->sh->physhim, M_CURR_IDX1, 0xFFFF);
-		wlapi_bmac_write_shm(pi->sh->physhim, M_CURR_IDX2, 0xFFFF);
-	}
-
-	if (pi->phyhang_avoid)
-		wlc_phy_stay_in_carriersearch_nphy(pi, false);
-}
-
-static void wlc_phy_ipa_internal_tssi_setup_nphy(struct brcms_phy *pi)
-{
-	u8 core;
-
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-		for (core = 0; core < pi->pubpi.phy_corenum; core++) {
-			if (CHSPEC_IS2G(pi->radio_chanspec)) {
-				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-						 TX_SSI_MASTER, 0x5);
-				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-						 TX_SSI_MUX, 0xe);
-
-				if (pi->pubpi.radiorev != 5)
-					WRITE_RADIO_REG3(pi, RADIO_2057, TX,
-							 core, TSSIA, 0);
-
-				if (!NREV_IS(pi->pubpi.phy_rev, 7)) {
-
-					WRITE_RADIO_REG3(pi, RADIO_2057, TX,
-							 core, TSSIG, 0x1);
-				} else {
-
-					WRITE_RADIO_REG3(pi, RADIO_2057, TX,
-							 core, TSSIG, 0x31);
-				}
-			} else {
-				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-						 TX_SSI_MASTER, 0x9);
-				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-						 TX_SSI_MUX, 0xc);
-				WRITE_RADIO_REG3(pi, RADIO_2057, TX, core,
-						 TSSIG, 0);
-
-				if (pi->pubpi.radiorev != 5) {
-					if (!NREV_IS(pi->pubpi.phy_rev, 7)) {
-
-						WRITE_RADIO_REG3(pi, RADIO_2057,
-								 TX, core,
-								 TSSIA, 0x1);
-					} else {
-
-						WRITE_RADIO_REG3(pi, RADIO_2057,
-								 TX, core,
-								 TSSIA, 0x31);
-					}
-				}
-			}
-			WRITE_RADIO_REG3(pi, RADIO_2057, TX, core, IQCAL_VCM_HG,
-					 0);
-			WRITE_RADIO_REG3(pi, RADIO_2057, TX, core, IQCAL_IDAC,
-					 0);
-			WRITE_RADIO_REG3(pi, RADIO_2057, TX, core, TSSI_VCM,
-					 0x3);
-			WRITE_RADIO_REG3(pi, RADIO_2057, TX, core, TSSI_MISC1,
-					 0x0);
-		}
-	} else {
-		WRITE_RADIO_SYN(pi, RADIO_2056, RESERVED_ADDR31,
-				(CHSPEC_IS2G(pi->radio_chanspec)) ? 0x128 :
-				0x80);
-		WRITE_RADIO_SYN(pi, RADIO_2056, RESERVED_ADDR30, 0x0);
-		WRITE_RADIO_SYN(pi, RADIO_2056, GPIO_MASTER1, 0x29);
-
-		for (core = 0; core < pi->pubpi.phy_corenum; core++) {
-			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core, IQCAL_VCM_HG,
-					 0x0);
-			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core, IQCAL_IDAC,
-					 0x0);
-			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core, TSSI_VCM,
-					 0x3);
-			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core, TX_AMP_DET,
-					 0x0);
-			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core, TSSI_MISC1,
-					 0x8);
-			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core, TSSI_MISC2,
-					 0x0);
-			WRITE_RADIO_REG2(pi, RADIO_2056, TX, core, TSSI_MISC3,
-					 0x0);
-
-			if (CHSPEC_IS2G(pi->radio_chanspec)) {
-				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
-						 TX_SSI_MASTER, 0x5);
-
-				if (pi->pubpi.radiorev != 5)
-					WRITE_RADIO_REG2(pi, RADIO_2056, TX,
-							 core, TSSIA, 0x0);
-				if (NREV_GE(pi->pubpi.phy_rev, 5)) {
-
-					WRITE_RADIO_REG2(pi, RADIO_2056, TX,
-							 core, TSSIG, 0x31);
-				} else {
-					WRITE_RADIO_REG2(pi, RADIO_2056, TX,
-							 core, TSSIG, 0x11);
-				}
-				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
-						 TX_SSI_MUX, 0xe);
-			} else {
-				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
-						 TX_SSI_MASTER, 0x9);
-				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
-						 TSSIA, 0x31);
-				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
-						 TSSIG, 0x0);
-				WRITE_RADIO_REG2(pi, RADIO_2056, TX, core,
-						 TX_SSI_MUX, 0xc);
-			}
-		}
-	}
-}
-
-static void wlc_phy_txpwrctrl_idle_tssi_nphy(struct brcms_phy *pi)
-{
-	s32 rssi_buf[4];
-	s32 int_val;
-
-	if (SCAN_RM_IN_PROGRESS(pi) || PLT_INPROG_PHY(pi) || PHY_MUTED(pi))
-
-		return;
-
-	if (PHY_IPA(pi)) {
-		wlc_phy_ipa_internal_tssi_setup_nphy(pi);
-	}
-
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 12),
-						  0, 0x3, 0,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-	} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 13), 0, 3, 0);
-	}
-
-	wlc_phy_stopplayback_nphy(pi);
-
-	wlc_phy_tx_tone_nphy(pi, 4000, 0, 0, 0, false);
-
-	udelay(20);
-	int_val =
-	    wlc_phy_poll_rssi_nphy(pi, (u8) NPHY_RSSI_SEL_TSSI_2G, rssi_buf,
-				   1);
-	wlc_phy_stopplayback_nphy(pi);
-	wlc_phy_rssisel_nphy(pi, RADIO_MIMO_CORESEL_OFF, 0);
-
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-		wlc_phy_rfctrl_override_nphy_rev7(pi, (0x1 << 12),
-						  0, 0x3, 1,
-						  NPHY_REV7_RFCTRLOVERRIDE_ID0);
-	} else if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-		wlc_phy_rfctrl_override_nphy(pi, (0x1 << 13), 0, 3, 1);
-	}
-
-	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-
-		pi->nphy_pwrctrl_info[PHY_CORE_0].idle_tssi_2g =
-		    (u8) ((int_val >> 24) & 0xff);
-		pi->nphy_pwrctrl_info[PHY_CORE_0].idle_tssi_5g =
-		    (u8) ((int_val >> 24) & 0xff);
-
-		pi->nphy_pwrctrl_info[PHY_CORE_1].idle_tssi_2g =
-		    (u8) ((int_val >> 8) & 0xff);
-		pi->nphy_pwrctrl_info[PHY_CORE_1].idle_tssi_5g =
-		    (u8) ((int_val >> 8) & 0xff);
-	} else {
-		pi->nphy_pwrctrl_info[PHY_CORE_0].idle_tssi_2g =
-		    (u8) ((int_val >> 24) & 0xff);
-
-		pi->nphy_pwrctrl_info[PHY_CORE_1].idle_tssi_2g =
-		    (u8) ((int_val >> 8) & 0xff);
-
-		pi->nphy_pwrctrl_info[PHY_CORE_0].idle_tssi_5g =
-		    (u8) ((int_val >> 16) & 0xff);
-		pi->nphy_pwrctrl_info[PHY_CORE_1].idle_tssi_5g =
-		    (u8) ((int_val) & 0xff);
-	}
-
-}
-
-static void wlc_phy_txpwrctrl_pwr_setup_nphy(struct brcms_phy *pi)
-{
-	u32 idx;
-	s16 a1[2], b0[2], b1[2];
-	s8 target_pwr_qtrdbm[2];
-	s32 num, den, pwr_est;
-	u8 chan_freq_range;
-	u8 idle_tssi[2];
-	u32 tbl_id, tbl_len, tbl_offset;
-	u32 regval[64];
-	u8 core;
-
-	if (D11REV_IS(pi->sh->corerev, 11) || D11REV_IS(pi->sh->corerev, 12)) {
-		wlapi_bmac_mctrl(pi->sh->physhim, MCTL_PHYLOCK, MCTL_PHYLOCK);
-		(void)R_REG(&pi->regs->maccontrol);
-		udelay(1);
-	}
-
-	if (pi->phyhang_avoid)
-		wlc_phy_stay_in_carriersearch_nphy(pi, true);
-
-	or_phy_reg(pi, 0x122, (0x1 << 0));
-
-	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-		and_phy_reg(pi, 0x1e7, (u16) (~(0x1 << 15)));
-	} else {
-
-		or_phy_reg(pi, 0x1e7, (0x1 << 15));
-	}
-
-	if (D11REV_IS(pi->sh->corerev, 11) || D11REV_IS(pi->sh->corerev, 12))
-		wlapi_bmac_mctrl(pi->sh->physhim, MCTL_PHYLOCK, 0);
-
-	if (pi->sh->sromrev < 4) {
-		idle_tssi[0] = pi->nphy_pwrctrl_info[0].idle_tssi_2g;
-		idle_tssi[1] = pi->nphy_pwrctrl_info[1].idle_tssi_2g;
-		target_pwr_qtrdbm[0] = 13 * 4;
-		target_pwr_qtrdbm[1] = 13 * 4;
-		a1[0] = -424;
-		a1[1] = -424;
-		b0[0] = 5612;
-		b0[1] = 5612;
-		b1[1] = -1393;
-		b1[0] = -1393;
-	} else {
-
-		chan_freq_range = wlc_phy_get_chan_freq_range_nphy(pi, 0);
-		switch (chan_freq_range) {
-		case WL_CHAN_FREQ_RANGE_2G:
-			idle_tssi[0] = pi->nphy_pwrctrl_info[0].idle_tssi_2g;
-			idle_tssi[1] = pi->nphy_pwrctrl_info[1].idle_tssi_2g;
-			target_pwr_qtrdbm[0] =
-			    pi->nphy_pwrctrl_info[0].max_pwr_2g;
-			target_pwr_qtrdbm[1] =
-			    pi->nphy_pwrctrl_info[1].max_pwr_2g;
-			a1[0] = pi->nphy_pwrctrl_info[0].pwrdet_2g_a1;
-			a1[1] = pi->nphy_pwrctrl_info[1].pwrdet_2g_a1;
-			b0[0] = pi->nphy_pwrctrl_info[0].pwrdet_2g_b0;
-			b0[1] = pi->nphy_pwrctrl_info[1].pwrdet_2g_b0;
-			b1[0] = pi->nphy_pwrctrl_info[0].pwrdet_2g_b1;
-			b1[1] = pi->nphy_pwrctrl_info[1].pwrdet_2g_b1;
-			break;
-		case WL_CHAN_FREQ_RANGE_5GL:
-			idle_tssi[0] = pi->nphy_pwrctrl_info[0].idle_tssi_5g;
-			idle_tssi[1] = pi->nphy_pwrctrl_info[1].idle_tssi_5g;
-			target_pwr_qtrdbm[0] =
-			    pi->nphy_pwrctrl_info[0].max_pwr_5gl;
-			target_pwr_qtrdbm[1] =
-			    pi->nphy_pwrctrl_info[1].max_pwr_5gl;
-			a1[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gl_a1;
-			a1[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gl_a1;
-			b0[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gl_b0;
-			b0[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gl_b0;
-			b1[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gl_b1;
-			b1[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gl_b1;
-			break;
-		case WL_CHAN_FREQ_RANGE_5GM:
-			idle_tssi[0] = pi->nphy_pwrctrl_info[0].idle_tssi_5g;
-			idle_tssi[1] = pi->nphy_pwrctrl_info[1].idle_tssi_5g;
-			target_pwr_qtrdbm[0] =
-			    pi->nphy_pwrctrl_info[0].max_pwr_5gm;
-			target_pwr_qtrdbm[1] =
-			    pi->nphy_pwrctrl_info[1].max_pwr_5gm;
-			a1[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gm_a1;
-			a1[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gm_a1;
-			b0[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gm_b0;
-			b0[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gm_b0;
-			b1[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gm_b1;
-			b1[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gm_b1;
-			break;
-		case WL_CHAN_FREQ_RANGE_5GH:
-			idle_tssi[0] = pi->nphy_pwrctrl_info[0].idle_tssi_5g;
-			idle_tssi[1] = pi->nphy_pwrctrl_info[1].idle_tssi_5g;
-			target_pwr_qtrdbm[0] =
-			    pi->nphy_pwrctrl_info[0].max_pwr_5gh;
-			target_pwr_qtrdbm[1] =
-			    pi->nphy_pwrctrl_info[1].max_pwr_5gh;
-			a1[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gh_a1;
-			a1[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gh_a1;
-			b0[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gh_b0;
-			b0[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gh_b0;
-			b1[0] = pi->nphy_pwrctrl_info[0].pwrdet_5gh_b1;
-			b1[1] = pi->nphy_pwrctrl_info[1].pwrdet_5gh_b1;
-			break;
-		default:
-			idle_tssi[0] = pi->nphy_pwrctrl_info[0].idle_tssi_2g;
-			idle_tssi[1] = pi->nphy_pwrctrl_info[1].idle_tssi_2g;
-			target_pwr_qtrdbm[0] = 13 * 4;
-			target_pwr_qtrdbm[1] = 13 * 4;
-			a1[0] = -424;
-			a1[1] = -424;
-			b0[0] = 5612;
-			b0[1] = 5612;
-			b1[1] = -1393;
-			b1[0] = -1393;
-			break;
-		}
-	}
-
-	target_pwr_qtrdbm[0] = (s8) pi->tx_power_max;
-	target_pwr_qtrdbm[1] = (s8) pi->tx_power_max;
-
-	if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-		if (pi->srom_fem2g.tssipos) {
-			or_phy_reg(pi, 0x1e9, (0x1 << 14));
-		}
-
-		if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-			for (core = 0; core <= 1; core++) {
-				if (PHY_IPA(pi)) {
-
-					if (CHSPEC_IS2G(pi->radio_chanspec)) {
-						WRITE_RADIO_REG3(pi, RADIO_2057,
-								 TX, core,
-								 TX_SSI_MUX,
-								 0xe);
-					} else {
-						WRITE_RADIO_REG3(pi, RADIO_2057,
-								 TX, core,
-								 TX_SSI_MUX,
-								 0xc);
-					}
-				} else {
-				}
-			}
-		} else {
-			if (PHY_IPA(pi)) {
-
-				write_radio_reg(pi, RADIO_2056_TX_TX_SSI_MUX |
-						RADIO_2056_TX0,
-						(CHSPEC_IS5G
-						 (pi->
-						  radio_chanspec)) ? 0xc : 0xe);
-				write_radio_reg(pi,
-						RADIO_2056_TX_TX_SSI_MUX |
-						RADIO_2056_TX1,
-						(CHSPEC_IS5G
-						 (pi->
-						  radio_chanspec)) ? 0xc : 0xe);
-			} else {
-
-				write_radio_reg(pi, RADIO_2056_TX_TX_SSI_MUX |
-						RADIO_2056_TX0, 0x11);
-				write_radio_reg(pi, RADIO_2056_TX_TX_SSI_MUX |
-						RADIO_2056_TX1, 0x11);
-			}
-		}
-	}
-
-	if (D11REV_IS(pi->sh->corerev, 11) || D11REV_IS(pi->sh->corerev, 12)) {
-		wlapi_bmac_mctrl(pi->sh->physhim, MCTL_PHYLOCK, MCTL_PHYLOCK);
-		(void)R_REG(&pi->regs->maccontrol);
-		udelay(1);
-	}
-
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-		mod_phy_reg(pi, 0x1e7, (0x7f << 0),
-			    (NPHY_TxPwrCtrlCmd_pwrIndex_init_rev7 << 0));
-	} else {
-		mod_phy_reg(pi, 0x1e7, (0x7f << 0),
-			    (NPHY_TxPwrCtrlCmd_pwrIndex_init << 0));
-	}
-
-	if (NREV_GE(pi->pubpi.phy_rev, 7)) {
-		mod_phy_reg(pi, 0x222, (0xff << 0),
-			    (NPHY_TxPwrCtrlCmd_pwrIndex_init_rev7 << 0));
-	} else if (NREV_GT(pi->pubpi.phy_rev, 1)) {
-		mod_phy_reg(pi, 0x222, (0xff << 0),
-			    (NPHY_TxPwrCtrlCmd_pwrIndex_init << 0));
-	}
-
-	if (D11REV_IS(pi->sh->corerev, 11) || D11REV_IS(pi->sh->corerev, 12))
-		wlapi_bmac_mctrl(pi->sh->physhim, MCTL_PHYLOCK, 0);
-
-	write_phy_reg(pi, 0x1e8, (0x3 << 8) | (240 << 0));
-
-	write_phy_reg(pi, 0x1e9,
-		      (1 << 15) | (idle_tssi[0] << 0) | (idle_tssi[1] << 8));
-
-	write_phy_reg(pi, 0x1ea,
-		      (target_pwr_qtrdbm[0] << 0) |
-		      (target_pwr_qtrdbm[1] << 8));
-
-	tbl_len = 64;
-	tbl_offset = 0;
-	for (tbl_id = NPHY_TBL_ID_CORE1TXPWRCTL;
-	     tbl_id <= NPHY_TBL_ID_CORE2TXPWRCTL; tbl_id++) {
-
-		for (idx = 0; idx < tbl_len; idx++) {
-			num =
-			    8 * (16 * b0[tbl_id - 26] + b1[tbl_id - 26] * idx);
-			den = 32768 + a1[tbl_id - 26] * idx;
-			pwr_est = max(((4 * num + den / 2) / den), -8);
-			if (NREV_LT(pi->pubpi.phy_rev, 3)) {
-				if (idx <=
-				    (uint) (31 - idle_tssi[tbl_id - 26] + 1))
-					pwr_est =
-					    max(pwr_est,
-						target_pwr_qtrdbm[tbl_id - 26] +
-						1);
-			}
-			regval[idx] = (u32) pwr_est;
-		}
-		wlc_phy_table_write_nphy(pi, tbl_id, tbl_len, tbl_offset, 32,
-					 regval);
-	}
-
-	wlc_phy_txpwr_limit_to_tbl_nphy(pi);
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_CORE1TXPWRCTL, 84, 64, 8,
-				 pi->adj_pwr_tbl_nphy);
-	wlc_phy_table_write_nphy(pi, NPHY_TBL_ID_CORE2TXPWRCTL, 84, 64, 8,
-				 pi->adj_pwr_tbl_nphy);
-
-	if (pi->phyhang_avoid)
-		wlc_phy_stay_in_carriersearch_nphy(pi, false);
-}
-
 static bool wlc_phy_txpwr_ison_nphy(struct brcms_phy *pi)
 {
 	return read_phy_reg((pi), 0x1e7) & ((0x1 << 15) |
-					     (0x1 << 14) | (0x1 << 13));
-}
-
-static u8 wlc_phy_txpwr_idx_cur_get_nphy(struct brcms_phy *pi, u8 core)
-{
-	u16 tmp;
-	tmp = read_phy_reg(pi, ((core == PHY_CORE_0) ? 0x1ed : 0x1ee));
-
-	tmp = (tmp & (0x7f << 8)) >> 8;
-	return (u8) tmp;
-}
-
-static void
-wlc_phy_txpwr_idx_cur_set_nphy(struct brcms_phy *pi, u8 idx0, u8 idx1)
-{
-	mod_phy_reg(pi, 0x1e7, (0x7f << 0), idx0);
-
-	if (NREV_GT(pi->pubpi.phy_rev, 1))
-		mod_phy_reg(pi, 0x222, (0xff << 0), idx1);
+					    (0x1 << 14) | (0x1 << 13));
 }
 
 u16 wlc_phy_txpwr_idx_get_nphy(struct brcms_phy *pi)
@@ -28600,12 +28416,9 @@ u16 wlc_phy_txpwr_idx_get_nphy(struct brcms_phy *pi)
 
 		tmp = (pwr_idx[0] << 8) | pwr_idx[1];
 	} else {
-		tmp =
-		    ((pi->nphy_txpwrindex[PHY_CORE_0].
-		      index_internal & 0xff) << 8) | (pi->
-						      nphy_txpwrindex
-						      [PHY_CORE_1].
-						      index_internal & 0xff);
+		tmp = ((pi->nphy_txpwrindex[PHY_CORE_0].index_internal & 0xff)
+			<< 8) |
+			(pi->nphy_txpwrindex[PHY_CORE_1].index_internal & 0xff);
 	}
 
 	return tmp;
@@ -28618,13 +28431,12 @@ void wlc_phy_txpwr_papd_cal_nphy(struct brcms_phy *pi)
 		|| (wlc_phy_txpwr_ison_nphy(pi)
 		    &&
 		    (((u32)
-		      ABS(wlc_phy_txpwr_idx_cur_get_nphy(pi, 0) -
+		      abs(wlc_phy_txpwr_idx_cur_get_nphy(pi, 0) -
 			  pi->nphy_papd_tx_gain_at_last_cal[0]) >= 4)
 		     || ((u32)
-			 ABS(wlc_phy_txpwr_idx_cur_get_nphy(pi, 1) -
-			     pi->nphy_papd_tx_gain_at_last_cal[1]) >= 4))))) {
+			 abs(wlc_phy_txpwr_idx_cur_get_nphy(pi, 1) -
+			     pi->nphy_papd_tx_gain_at_last_cal[1]) >= 4)))))
 		wlc_phy_a4(pi, true);
-	}
 }
 
 void wlc_phy_txpwrctrl_enable_nphy(struct brcms_phy *pi, u8 ctrl_type)
@@ -28655,32 +28467,29 @@ void wlc_phy_txpwrctrl_enable_nphy(struct brcms_phy *pi, u8 ctrl_type)
 				for (core = 0; core < pi->pubpi.phy_corenum;
 				     core++)
 					pi->nphy_txpwr_idx[core] =
-					    wlc_phy_txpwr_idx_cur_get_nphy(pi,
-									   (u8)
-									   core);
+						wlc_phy_txpwr_idx_cur_get_nphy(
+							pi,
+							(u8) core);
 			}
 
 		}
 
 		tbl_len = 84;
 		tbl_offset = 64;
-		for (ctr = 0; ctr < tbl_len; ctr++) {
+		for (ctr = 0; ctr < tbl_len; ctr++)
 			regval[ctr] = 0;
-		}
 		wlc_phy_table_write_nphy(pi, 26, tbl_len, tbl_offset, 16,
 					 regval);
 		wlc_phy_table_write_nphy(pi, 27, tbl_len, tbl_offset, 16,
 					 regval);
 
-		if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-
+		if (NREV_GE(pi->pubpi.phy_rev, 3))
 			and_phy_reg(pi, 0x1e7,
 				    (u16) (~((0x1 << 15) |
-						(0x1 << 14) | (0x1 << 13))));
-		} else {
+					     (0x1 << 14) | (0x1 << 13))));
+		else
 			and_phy_reg(pi, 0x1e7,
 				    (u16) (~((0x1 << 14) | (0x1 << 13))));
-		}
 
 		if (NREV_GE(pi->pubpi.phy_rev, 3)) {
 			or_phy_reg(pi, 0x8f, (0x1 << 8));
@@ -28694,7 +28503,8 @@ void wlc_phy_txpwrctrl_enable_nphy(struct brcms_phy *pi, u8 ctrl_type)
 		else if (NREV_LT(pi->pubpi.phy_rev, 2))
 			mod_phy_reg(pi, 0xdc, 0x00ff, 0x5a);
 
-		if (NREV_LT(pi->pubpi.phy_rev, 2) && IS40MHZ(pi))
+		if (NREV_LT(pi->pubpi.phy_rev, 2) &&
+		    pi->bw == WL_CHANSPEC_BW_40)
 			wlapi_bmac_mhf(pi->sh->physhim, MHF1, MHF1_IQSWAP_WAR,
 				       MHF1_IQSWAP_WAR, BRCM_BAND_ALL);
 
@@ -28730,7 +28540,7 @@ void wlc_phy_txpwrctrl_enable_nphy(struct brcms_phy *pi, u8 ctrl_type)
 
 		if (NREV_GE(pi->pubpi.phy_rev, 3)) {
 			if ((pi->nphy_txpwr_idx[0] != 128)
-			    && (pi->nphy_txpwr_idx[1] != 128)) {
+			    && (pi->nphy_txpwr_idx[1] != 128))
 				wlc_phy_txpwr_idx_cur_set_nphy(pi,
 							       pi->
 							       nphy_txpwr_idx
@@ -28738,7 +28548,6 @@ void wlc_phy_txpwrctrl_enable_nphy(struct brcms_phy *pi, u8 ctrl_type)
 							       pi->
 							       nphy_txpwr_idx
 							       [1]);
-			}
 		}
 
 		if (NREV_GE(pi->pubpi.phy_rev, 3)) {
@@ -28753,7 +28562,8 @@ void wlc_phy_txpwrctrl_enable_nphy(struct brcms_phy *pi, u8 ctrl_type)
 		else if (NREV_LT(pi->pubpi.phy_rev, 2))
 			mod_phy_reg(pi, 0xdc, 0x00ff, 0x40);
 
-		if (NREV_LT(pi->pubpi.phy_rev, 2) && IS40MHZ(pi))
+		if (NREV_LT(pi->pubpi.phy_rev, 2) &&
+		    pi->bw == WL_CHANSPEC_BW_40)
 			wlapi_bmac_mhf(pi->sh->physhim, MHF1, MHF1_IQSWAP_WAR,
 				       0x0, BRCM_BAND_ALL);
 
@@ -28799,17 +28609,14 @@ wlc_phy_txpwr_index_nphy(struct brcms_phy *pi, u8 core_mask, s8 txpwrindex,
 
 	for (core = 0; core < pi->pubpi.phy_corenum; core++) {
 
-		if ((core_mask & (1 << core)) == 0) {
+		if ((core_mask & (1 << core)) == 0)
 			continue;
-		}
 
 		txpwrctl_tbl = (core == PHY_CORE_0) ? 26 : 27;
 
 		if (txpwrindex < 0) {
-			if (pi->nphy_txpwrindex[core].index < 0) {
-
+			if (pi->nphy_txpwrindex[core].index < 0)
 				continue;
-			}
 
 			if (NREV_GE(pi->pubpi.phy_rev, 3)) {
 				mod_phy_reg(pi, 0x8f,
@@ -28842,29 +28649,21 @@ wlc_phy_txpwr_index_nphy(struct brcms_phy *pi, u8 core_mask, s8 txpwrindex,
 			wlc_phy_table_write_nphy(pi, 15, 1, 87, 16, &m1m2);
 
 			if (restore_cals) {
-
-				wlc_phy_table_write_nphy(pi, 15, 2,
-							 (80 + 2 * core), 16,
-							 (void *)&pi->
-							 nphy_txpwrindex[core].
-							 iqcomp_a);
-
-				wlc_phy_table_write_nphy(pi, 15, 1, (85 + core),
-							 16,
-							 &pi->
-							 nphy_txpwrindex[core].
-							 locomp);
-				wlc_phy_table_write_nphy(pi, 15, 1, (93 + core),
-							 16,
-							 (void *)&pi->
-							 nphy_txpwrindex[core].
-							 locomp);
+				wlc_phy_table_write_nphy(
+					pi, 15, 2, (80 + 2 * core), 16,
+					&pi->nphy_txpwrindex[core].iqcomp_a);
+				wlc_phy_table_write_nphy(
+					pi, 15, 1, (85 + core), 16,
+					&pi->nphy_txpwrindex[core].locomp);
+				wlc_phy_table_write_nphy(
+					pi, 15, 1, (93 + core), 16,
+					&pi->nphy_txpwrindex[core].locomp);
 			}
 
 			wlc_phy_txpwrctrl_enable_nphy(pi, pi->nphy_txpwrctrl);
 
 			pi->nphy_txpwrindex[core].index_internal =
-			    pi->nphy_txpwrindex[core].index_internal_save;
+				pi->nphy_txpwrindex[core].index_internal_save;
 		} else {
 
 			if (pi->nphy_txpwrindex[core].index < 0) {
@@ -28879,14 +28678,13 @@ wlc_phy_txpwr_index_nphy(struct brcms_phy *pi, u8 core_mask, s8 txpwrindex,
 						    AfectrlOverride);
 				} else {
 					pi->nphy_txpwrindex[core].
-					    AfectrlOverride =
-					    read_phy_reg(pi, 0xa5);
+					AfectrlOverride =
+						read_phy_reg(pi, 0xa5);
 				}
 
 				pi->nphy_txpwrindex[core].AfeCtrlDacGain =
-				    read_phy_reg(pi,
-						 (core ==
-						  PHY_CORE_0) ? 0xaa : 0xab);
+					read_phy_reg(pi, (core == PHY_CORE_0) ?
+							 0xaa : 0xab);
 
 				wlc_phy_table_read_nphy(pi, 7, 1,
 							(0x110 + core), 16,
@@ -28898,23 +28696,23 @@ wlc_phy_txpwr_index_nphy(struct brcms_phy *pi, u8 core_mask, s8 txpwrindex,
 							&tmpval);
 				tmpval >>= ((core == PHY_CORE_0) ? 8 : 0);
 				tmpval &= 0xff;
-				pi->nphy_txpwrindex[core].bbmult =
-				    (u8) tmpval;
+				pi->nphy_txpwrindex[core].bbmult = (u8) tmpval;
 
 				wlc_phy_table_read_nphy(pi, 15, 2,
 							(80 + 2 * core), 16,
-							(void *)&pi->
+							&pi->
 							nphy_txpwrindex[core].
 							iqcomp_a);
 
 				wlc_phy_table_read_nphy(pi, 15, 1, (85 + core),
 							16,
-							(void *)&pi->
+							&pi->
 							nphy_txpwrindex[core].
 							locomp);
 
 				pi->nphy_txpwrindex[core].index_internal_save =
-				    pi->nphy_txpwrindex[core].index_internal;
+					pi->nphy_txpwrindex[core].
+					index_internal;
 			}
 
 			tx_pwr_ctrl_state = pi->nphy_txpwrctrl;
@@ -28927,22 +28725,22 @@ wlc_phy_txpwr_index_nphy(struct brcms_phy *pi, u8 core_mask, s8 txpwrindex,
 						(tx_ind0 + txpwrindex), 32,
 						&txgain);
 
-			if (NREV_GE(pi->pubpi.phy_rev, 3)) {
-				rad_gain =
-				    (txgain >> 16) & ((1 << (32 - 16 + 1)) - 1);
-			} else {
-				rad_gain =
-				    (txgain >> 16) & ((1 << (28 - 16 + 1)) - 1);
-			}
+			if (NREV_GE(pi->pubpi.phy_rev, 3))
+				rad_gain = (txgain >> 16) &
+					   ((1 << (32 - 16 + 1)) - 1);
+			else
+				rad_gain = (txgain >> 16) &
+					   ((1 << (28 - 16 + 1)) - 1);
+
 			dac_gain = (txgain >> 8) & ((1 << (13 - 8 + 1)) - 1);
 			bbmult = (txgain >> 0) & ((1 << (7 - 0 + 1)) - 1);
 
-			if (NREV_GE(pi->pubpi.phy_rev, 3)) {
+			if (NREV_GE(pi->pubpi.phy_rev, 3))
 				mod_phy_reg(pi, ((core == PHY_CORE_0) ? 0x8f :
 						 0xa5), (0x1 << 8), (0x1 << 8));
-			} else {
+			else
 				mod_phy_reg(pi, 0xa5, (0x1 << 14), (0x1 << 14));
-			}
+
 			write_phy_reg(pi, (core == PHY_CORE_0) ?
 				      0xaa : 0xab, dac_gain);
 
@@ -28951,9 +28749,8 @@ wlc_phy_txpwr_index_nphy(struct brcms_phy *pi, u8 core_mask, s8 txpwrindex,
 
 			wlc_phy_table_read_nphy(pi, 15, 1, 87, 16, &m1m2);
 			m1m2 &= ((core == PHY_CORE_0) ? 0x00ff : 0xff00);
-			m1m2 |=
-			    ((core ==
-			      PHY_CORE_0) ? (bbmult << 8) : (bbmult << 0));
+			m1m2 |= ((core == PHY_CORE_0) ?
+				(bbmult << 8) : (bbmult << 0));
 
 			wlc_phy_table_write_nphy(pi, 15, 1, 87, 16, &m1m2);
 
@@ -28974,23 +28771,20 @@ wlc_phy_txpwr_index_nphy(struct brcms_phy *pi, u8 core_mask, s8 txpwrindex,
 			wlc_phy_table_read_nphy(pi, txpwrctl_tbl, 1,
 						(lo_ind0 + txpwrindex), 32,
 						&locomp);
-			if (restore_cals) {
+			if (restore_cals)
 				wlc_phy_table_write_nphy(pi, 15, 1, (85 + core),
 							 16, &locomp);
-			}
 
 			if (NREV_IS(pi->pubpi.phy_rev, 1))
 				wlapi_bmac_phyclk_fgc(pi->sh->physhim, OFF);
 
 			if (PHY_IPA(pi)) {
 				wlc_phy_table_read_nphy(pi,
-							(core ==
-							 PHY_CORE_0 ?
-							 NPHY_TBL_ID_CORE1TXPWRCTL
-							 :
-							 NPHY_TBL_ID_CORE2TXPWRCTL),
-							1, 576 + txpwrindex, 32,
-							&rfpwr_offset);
+						(core == PHY_CORE_0 ?
+						 NPHY_TBL_ID_CORE1TXPWRCTL :
+						 NPHY_TBL_ID_CORE2TXPWRCTL),
+						1, 576 + txpwrindex, 32,
+						&rfpwr_offset);
 
 				mod_phy_reg(pi, (core == PHY_CORE_0) ? 0x297 :
 					    0x29b, (0x1ff << 4),
@@ -29046,7 +28840,7 @@ void wlc_phy_stay_in_carriersearch_nphy(struct brcms_phy *pi, bool enable)
 	if (enable) {
 		if (pi->nphy_deaf_count == 0) {
 			pi->classifier_state =
-			    wlc_phy_classifier_nphy(pi, 0, 0);
+				wlc_phy_classifier_nphy(pi, 0, 0);
 			wlc_phy_classifier_nphy(pi, (0x7 << 0), 4);
 			wlc_phy_clip_det_nphy(pi, 0, pi->clip_state);
 			wlc_phy_clip_det_nphy(pi, 1, clip_off);
@@ -29074,9 +28868,9 @@ void wlc_nphy_deaf_mode(struct brcms_phy *pi, bool mode)
 	if (mode) {
 		if (pi->nphy_deaf_count == 0)
 			wlc_phy_stay_in_carriersearch_nphy(pi, true);
-	} else {
-		if (pi->nphy_deaf_count > 0)
-			wlc_phy_stay_in_carriersearch_nphy(pi, false);
+	} else if (pi->nphy_deaf_count > 0) {
+		wlc_phy_stay_in_carriersearch_nphy(pi, false);
 	}
+
 	wlapi_enable_mac(pi->sh->physhim);
 }
diff --git a/drivers/staging/brcm80211/brcmsmac/phy/phy_qmath.c b/drivers/staging/brcm80211/brcmsmac/phy/phy_qmath.c
index 01ff0c8eb4b9..faf1ebe76068 100644
--- a/drivers/staging/brcm80211/brcmsmac/phy/phy_qmath.c
+++ b/drivers/staging/brcm80211/brcmsmac/phy/phy_qmath.c
@@ -17,89 +17,95 @@
 #include "phy_qmath.h"
 
 /*
-Description: This function make 16 bit unsigned multiplication. To fit the output into
-16 bits the 32 bit multiplication result is right shifted by 16 bits.
-*/
+ * Description: This function make 16 bit unsigned multiplication.
+ * To fit the output into 16 bits the 32 bit multiplication result is right
+ * shifted by 16 bits.
+ */
 u16 qm_mulu16(u16 op1, u16 op2)
 {
 	return (u16) (((u32) op1 * (u32) op2) >> 16);
 }
 
 /*
-Description: This function make 16 bit multiplication and return the result in 16 bits.
-To fit the multiplication result into 16 bits the multiplication result is right shifted by
-15 bits. Right shifting 15 bits instead of 16 bits is done to remove the extra sign bit formed
-due to the multiplication.
-When both the 16bit inputs are 0x8000 then the output is saturated to 0x7fffffff.
-*/
+ * Description: This function make 16 bit multiplication and return the result
+ * in 16 bits. To fit the multiplication result into 16 bits the multiplication
+ * result is right shifted by 15 bits. Right shifting 15 bits instead of 16 bits
+ * is done to remove the extra sign bit formed due to the multiplication.
+ * When both the 16bit inputs are 0x8000 then the output is saturated to
+ * 0x7fffffff.
+ */
 s16 qm_muls16(s16 op1, s16 op2)
 {
 	s32 result;
-	if (op1 == (s16) 0x8000 && op2 == (s16) 0x8000) {
+	if (op1 == (s16) 0x8000 && op2 == (s16) 0x8000)
 		result = 0x7fffffff;
-	} else {
+	else
 		result = ((s32) (op1) * (s32) (op2));
-	}
+
 	return (s16) (result >> 15);
 }
 
 /*
-Description: This function add two 32 bit numbers and return the 32bit result.
-If the result overflow 32 bits, the output will be saturated to 32bits.
-*/
+ * Description: This function add two 32 bit numbers and return the 32bit
+ * result. If the result overflow 32 bits, the output will be saturated to
+ * 32bits.
+ */
 s32 qm_add32(s32 op1, s32 op2)
 {
 	s32 result;
 	result = op1 + op2;
-	if (op1 < 0 && op2 < 0 && result > 0) {
+	if (op1 < 0 && op2 < 0 && result > 0)
 		result = 0x80000000;
-	} else if (op1 > 0 && op2 > 0 && result < 0) {
+	else if (op1 > 0 && op2 > 0 && result < 0)
 		result = 0x7fffffff;
-	}
+
 	return result;
 }
 
 /*
-Description: This function add two 16 bit numbers and return the 16bit result.
-If the result overflow 16 bits, the output will be saturated to 16bits.
-*/
+ * Description: This function add two 16 bit numbers and return the 16bit
+ * result. If the result overflow 16 bits, the output will be saturated to
+ * 16bits.
+ */
 s16 qm_add16(s16 op1, s16 op2)
 {
 	s16 result;
 	s32 temp = (s32) op1 + (s32) op2;
-	if (temp > (s32) 0x7fff) {
+	if (temp > (s32) 0x7fff)
 		result = (s16) 0x7fff;
-	} else if (temp < (s32) 0xffff8000) {
+	else if (temp < (s32) 0xffff8000)
 		result = (s16) 0xffff8000;
-	} else {
+	else
 		result = (s16) temp;
-	}
+
 	return result;
 }
 
 /*
-Description: This function make 16 bit subtraction and return the 16bit result.
-If the result overflow 16 bits, the output will be saturated to 16bits.
-*/
+ * Description: This function make 16 bit subtraction and return the 16bit
+ * result. If the result overflow 16 bits, the output will be saturated to
+ * 16bits.
+ */
 s16 qm_sub16(s16 op1, s16 op2)
 {
 	s16 result;
 	s32 temp = (s32) op1 - (s32) op2;
-	if (temp > (s32) 0x7fff) {
+	if (temp > (s32) 0x7fff)
 		result = (s16) 0x7fff;
-	} else if (temp < (s32) 0xffff8000) {
+	else if (temp < (s32) 0xffff8000)
 		result = (s16) 0xffff8000;
-	} else {
+	else
 		result = (s16) temp;
-	}
+
 	return result;
 }
 
 /*
-Description: This function make a 32 bit saturated left shift when the specified shift
-is +ve. This function will make a 32 bit right shift when the specified shift is -ve.
-This function return the result after shifting operation.
-*/
+ * Description: This function make a 32 bit saturated left shift when the
+ * specified shift is +ve. This function will make a 32 bit right shift when
+ * the specified shift is -ve. This function return the result after shifting
+ * operation.
+ */
 s32 qm_shl32(s32 op, int shift)
 {
 	int i;
@@ -110,20 +116,21 @@ s32 qm_shl32(s32 op, int shift)
 	else if (shift < -31)
 		shift = -31;
 	if (shift >= 0) {
-		for (i = 0; i < shift; i++) {
+		for (i = 0; i < shift; i++)
 			result = qm_add32(result, result);
-		}
 	} else {
 		result = result >> (-shift);
 	}
+
 	return result;
 }
 
 /*
-Description: This function make a 16 bit saturated left shift when the specified shift
-is +ve. This function will make a 16 bit right shift when the specified shift is -ve.
-This function return the result after shifting operation.
-*/
+ * Description: This function make a 16 bit saturated left shift when the
+ * specified shift is +ve. This function will make a 16 bit right shift when
+ * the specified shift is -ve. This function return the result after shifting
+ * operation.
+ */
 s16 qm_shl16(s16 op, int shift)
 {
 	int i;
@@ -134,29 +141,29 @@ s16 qm_shl16(s16 op, int shift)
 	else if (shift < -15)
 		shift = -15;
 	if (shift > 0) {
-		for (i = 0; i < shift; i++) {
+		for (i = 0; i < shift; i++)
 			result = qm_add16(result, result);
-		}
 	} else {
 		result = result >> (-shift);
 	}
+
 	return result;
 }
 
 /*
-Description: This function make a 16 bit right shift when shift is +ve.
-This function make a 16 bit saturated left shift when shift is -ve. This function
-return the result of the shift operation.
-*/
+ * Description: This function make a 16 bit right shift when shift is +ve.
+ * This function make a 16 bit saturated left shift when shift is -ve. This
+ * function return the result of the shift operation.
+ */
 s16 qm_shr16(s16 op, int shift)
 {
 	return qm_shl16(op, -shift);
 }
 
 /*
-Description: This function return the number of redundant sign bits in a 32 bit number.
-Example: qm_norm32(0x00000080) = 23
-*/
+ * Description: This function return the number of redundant sign bits in a
+ * 32 bit number. Example: qm_norm32(0x00000080) = 23
+ */
 s16 qm_norm32(s32 op)
 {
 	u16 u16extraSignBits;
@@ -208,28 +215,30 @@ static const s16 log_table[] = {
 	32024
 };
 
-#define LOG_TABLE_SIZE 32	/* log_table size */
-#define LOG2_LOG_TABLE_SIZE 5	/* log2(log_table size) */
-#define Q_LOG_TABLE 15		/* qformat of log_table */
-#define LOG10_2		19728	/* log10(2) in q.16 */
+#define LOG_TABLE_SIZE 32       /* log_table size */
+#define LOG2_LOG_TABLE_SIZE 5   /* log2(log_table size) */
+#define Q_LOG_TABLE 15          /* qformat of log_table */
+#define LOG10_2         19728   /* log10(2) in q.16 */
 
 /*
-Description:
-This routine takes the input number N and its q format qN and compute
-the log10(N). This routine first normalizes the input no N.	Then N is in mag*(2^x) format.
-mag is any number in the range 2^30-(2^31 - 1). Then log2(mag * 2^x) = log2(mag) + x is computed.
-From that log10(mag * 2^x) = log2(mag * 2^x) * log10(2) is computed.
-This routine looks the log2 value in the table considering LOG2_LOG_TABLE_SIZE+1 MSBs.
-As the MSB is always 1, only next LOG2_OF_LOG_TABLE_SIZE MSBs are used for table lookup.
-Next 16 MSBs are used for interpolation.
-Inputs:
-N - number to which log10 has to be found.
-qN - q format of N
-log10N - address where log10(N) will be written.
-qLog10N - address where log10N qformat will be written.
-Note/Problem:
-For accurate results input should be in normalized or near normalized form.
-*/
+ * Description:
+ * This routine takes the input number N and its q format qN and compute
+ * the log10(N). This routine first normalizes the input no N.	Then N is in
+ * mag*(2^x) format. mag is any number in the range 2^30-(2^31 - 1).
+ * Then log2(mag * 2^x) = log2(mag) + x is computed. From that
+ * log10(mag * 2^x) = log2(mag * 2^x) * log10(2) is computed.
+ * This routine looks the log2 value in the table considering
+ * LOG2_LOG_TABLE_SIZE+1 MSBs. As the MSB is always 1, only next
+ * LOG2_OF_LOG_TABLE_SIZE MSBs are used for table lookup. Next 16 MSBs are used
+ * for interpolation.
+ * Inputs:
+ * N - number to which log10 has to be found.
+ * qN - q format of N
+ * log10N - address where log10(N) will be written.
+ * qLog10N - address where log10N qformat will be written.
+ * Note/Problem:
+ * For accurate results input should be in normalized or near normalized form.
+ */
 void qm_log10(s32 N, s16 qN, s16 *log10N, s16 *qLog10N)
 {
 	s16 s16norm, s16tableIndex, s16errorApproximation;
@@ -248,12 +257,13 @@ void qm_log10(s32 N, s16 qN, s16 *log10N, s16 *qLog10N)
 	 */
 	qN = qN + s16norm - 30;
 
-	/* take the table index as the LOG2_OF_LOG_TABLE_SIZE bits right of the MSB */
+	/* take the table index as the LOG2_OF_LOG_TABLE_SIZE bits right of the
+	 * MSB */
 	s16tableIndex = (s16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE)));
 
 	/* remove the MSB. the MSB is always 1 after normalization. */
 	s16tableIndex =
-	    s16tableIndex & (s16) ((1 << LOG2_LOG_TABLE_SIZE) - 1);
+		s16tableIndex & (s16) ((1 << LOG2_LOG_TABLE_SIZE) - 1);
 
 	/* remove the (1+LOG2_OF_LOG_TABLE_SIZE) MSBs in the N. */
 	N = N & ((1 << (32 - (2 + LOG2_LOG_TABLE_SIZE))) - 1);
@@ -263,23 +273,27 @@ void qm_log10(s32 N, s16 qN, s16 *log10N, s16 *qLog10N)
 	u16offset = (u16) (N >> (32 - (2 + LOG2_LOG_TABLE_SIZE + 16)));
 
 	/* look the log value in the table. */
-	s32log = log_table[s16tableIndex];	/* q.15 format */
+	s32log = log_table[s16tableIndex];      /* q.15 format */
 
-	/* interpolate using the offset. */
-	s16errorApproximation = (s16) qm_mulu16(u16offset, (u16) (log_table[s16tableIndex + 1] - log_table[s16tableIndex]));	/* q.15 */
+	/* interpolate using the offset. q.15 format. */
+	s16errorApproximation = (s16) qm_mulu16(u16offset,
+				(u16) (log_table[s16tableIndex + 1] -
+				       log_table[s16tableIndex]));
 
-	s32log = qm_add16((s16) s32log, s16errorApproximation);	/* q.15 format */
+	 /* q.15 format */
+	s32log = qm_add16((s16) s32log, s16errorApproximation);
 
 	/* adjust for the qformat of the N as
 	 * log2(mag * 2^x) = log2(mag) + x
 	 */
-	s32log = qm_add32(s32log, ((s32) -qN) << 15);	/* q.15 format */
+	s32log = qm_add32(s32log, ((s32) -qN) << 15);   /* q.15 format */
 
 	/* normalize the result. */
 	s16norm = qm_norm32(s32log);
 
 	/* bring all the important bits into lower 16 bits */
-	s32log = qm_shl32(s32log, s16norm - 16);	/* q.15+s16norm-16 format */
+	/* q.15+s16norm-16 format */
+	s32log = qm_shl32(s32log, s16norm - 16);
 
 	/* compute the log10(N) by multiplying log2(N) with log10(2).
 	 * as log10(mag * 2^x) = log2(mag * 2^x) * log10(2)
diff --git a/drivers/staging/brcm80211/brcmsmac/phy/phytbl_lcn.c b/drivers/staging/brcm80211/brcmsmac/phy/phytbl_lcn.c
index 023d05aa97ad..622c01ca72c5 100644
--- a/drivers/staging/brcm80211/brcmsmac/phy/phytbl_lcn.c
+++ b/drivers/staging/brcm80211/brcmsmac/phy/phytbl_lcn.c
@@ -17,7 +17,7 @@
 #include <types.h>
 #include "phytbl_lcn.h"
 
-const u32 dot11lcn_gain_tbl_rev0[] = {
+static const u32 dot11lcn_gain_tbl_rev0[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -116,7 +116,7 @@ const u32 dot11lcn_gain_tbl_rev0[] = {
 	0x00000000,
 };
 
-const u32 dot11lcn_gain_tbl_rev1[] = {
+static const u32 dot11lcn_gain_tbl_rev1[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -215,7 +215,7 @@ const u32 dot11lcn_gain_tbl_rev1[] = {
 	0x00000000,
 };
 
-const u16 dot11lcn_aux_gain_idx_tbl_rev0[] = {
+static const u16 dot11lcn_aux_gain_idx_tbl_rev0[] = {
 	0x0401,
 	0x0402,
 	0x0403,
@@ -256,7 +256,7 @@ const u16 dot11lcn_aux_gain_idx_tbl_rev0[] = {
 	0x0000,
 };
 
-const u32 dot11lcn_gain_idx_tbl_rev0[] = {
+static const u32 dot11lcn_gain_idx_tbl_rev0[] = {
 	0x00000000,
 	0x00000000,
 	0x10000000,
@@ -407,7 +407,7 @@ const u32 dot11lcn_gain_idx_tbl_rev0[] = {
 	0x0000001c,
 };
 
-const u16 dot11lcn_aux_gain_idx_tbl_2G[] = {
+static const u16 dot11lcn_aux_gain_idx_tbl_2G[] = {
 	0x0000,
 	0x0000,
 	0x0000,
@@ -448,7 +448,7 @@ const u16 dot11lcn_aux_gain_idx_tbl_2G[] = {
 	0x0000
 };
 
-const u8 dot11lcn_gain_val_tbl_2G[] = {
+static const u8 dot11lcn_gain_val_tbl_2G[] = {
 	0xfc,
 	0x02,
 	0x08,
@@ -519,7 +519,7 @@ const u8 dot11lcn_gain_val_tbl_2G[] = {
 	0x00
 };
 
-const u32 dot11lcn_gain_idx_tbl_2G[] = {
+static const u32 dot11lcn_gain_idx_tbl_2G[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -674,7 +674,7 @@ const u32 dot11lcn_gain_idx_tbl_2G[] = {
 	0x00000000
 };
 
-const u32 dot11lcn_gain_tbl_2G[] = {
+static const u32 dot11lcn_gain_tbl_2G[] = {
 	0x00000000,
 	0x00000004,
 	0x00000008,
@@ -773,7 +773,7 @@ const u32 dot11lcn_gain_tbl_2G[] = {
 	0x00000000
 };
 
-const u32 dot11lcn_gain_tbl_extlna_2G[] = {
+static const u32 dot11lcn_gain_tbl_extlna_2G[] = {
 	0x00000000,
 	0x00000004,
 	0x00000008,
@@ -872,7 +872,7 @@ const u32 dot11lcn_gain_tbl_extlna_2G[] = {
 	0x00000000
 };
 
-const u16 dot11lcn_aux_gain_idx_tbl_extlna_2G[] = {
+static const u16 dot11lcn_aux_gain_idx_tbl_extlna_2G[] = {
 	0x0400,
 	0x0400,
 	0x0400,
@@ -913,7 +913,7 @@ const u16 dot11lcn_aux_gain_idx_tbl_extlna_2G[] = {
 	0x0000
 };
 
-const u8 dot11lcn_gain_val_tbl_extlna_2G[] = {
+static const u8 dot11lcn_gain_val_tbl_extlna_2G[] = {
 	0xfc,
 	0x02,
 	0x08,
@@ -984,7 +984,7 @@ const u8 dot11lcn_gain_val_tbl_extlna_2G[] = {
 	0x00
 };
 
-const u32 dot11lcn_gain_idx_tbl_extlna_2G[] = {
+static const u32 dot11lcn_gain_idx_tbl_extlna_2G[] = {
 	0x00000000,
 	0x00000040,
 	0x00000000,
@@ -1139,7 +1139,7 @@ const u32 dot11lcn_gain_idx_tbl_extlna_2G[] = {
 	0x00000000
 };
 
-const u32 dot11lcn_aux_gain_idx_tbl_5G[] = {
+static const u32 dot11lcn_aux_gain_idx_tbl_5G[] = {
 	0x0000,
 	0x0000,
 	0x0000,
@@ -1180,7 +1180,7 @@ const u32 dot11lcn_aux_gain_idx_tbl_5G[] = {
 	0x0000
 };
 
-const u32 dot11lcn_gain_val_tbl_5G[] = {
+static const u32 dot11lcn_gain_val_tbl_5G[] = {
 	0xf7,
 	0xfd,
 	0x00,
@@ -1251,7 +1251,7 @@ const u32 dot11lcn_gain_val_tbl_5G[] = {
 	0x00
 };
 
-const u32 dot11lcn_gain_idx_tbl_5G[] = {
+static const u32 dot11lcn_gain_idx_tbl_5G[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -1406,7 +1406,7 @@ const u32 dot11lcn_gain_idx_tbl_5G[] = {
 	0x00000000
 };
 
-const u32 dot11lcn_gain_tbl_5G[] = {
+static const u32 dot11lcn_gain_tbl_5G[] = {
 	0x00000000,
 	0x00000040,
 	0x00000080,
@@ -1520,7 +1520,7 @@ const struct phytbl_info dot11lcnphytbl_rx_gain_info_rev0[] = {
 	,
 };
 
-const struct phytbl_info dot11lcnphytbl_rx_gain_info_rev1[] = {
+static const struct phytbl_info dot11lcnphytbl_rx_gain_info_rev1[] = {
 	{&dot11lcn_gain_tbl_rev1,
 	 sizeof(dot11lcn_gain_tbl_rev1) / sizeof(dot11lcn_gain_tbl_rev1[0]), 18,
 	 0, 32}
@@ -1611,10 +1611,6 @@ const u32 dot11lcnphytbl_rx_gain_info_sz_rev0 =
 	sizeof(dot11lcnphytbl_rx_gain_info_rev0) /
 	sizeof(dot11lcnphytbl_rx_gain_info_rev0[0]);
 
-const u32 dot11lcnphytbl_rx_gain_info_sz_rev1 =
-	sizeof(dot11lcnphytbl_rx_gain_info_rev1) /
-	sizeof(dot11lcnphytbl_rx_gain_info_rev1[0]);
-
 const u32 dot11lcnphytbl_rx_gain_info_2G_rev2_sz =
 	sizeof(dot11lcnphytbl_rx_gain_info_2G_rev2) /
 	sizeof(dot11lcnphytbl_rx_gain_info_2G_rev2[0]);
@@ -1623,7 +1619,7 @@ const u32 dot11lcnphytbl_rx_gain_info_5G_rev2_sz =
 	sizeof(dot11lcnphytbl_rx_gain_info_5G_rev2) /
 	sizeof(dot11lcnphytbl_rx_gain_info_5G_rev2[0]);
 
-const u16 dot11lcn_min_sig_sq_tbl_rev0[] = {
+static const u16 dot11lcn_min_sig_sq_tbl_rev0[] = {
 	0x014d,
 	0x014d,
 	0x014d,
@@ -1690,7 +1686,7 @@ const u16 dot11lcn_min_sig_sq_tbl_rev0[] = {
 	0x014d,
 };
 
-const u16 dot11lcn_noise_scale_tbl_rev0[] = {
+static const u16 dot11lcn_noise_scale_tbl_rev0[] = {
 	0x0000,
 	0x0000,
 	0x0000,
@@ -1757,7 +1753,7 @@ const u16 dot11lcn_noise_scale_tbl_rev0[] = {
 	0x0000,
 };
 
-const u32 dot11lcn_fltr_ctrl_tbl_rev0[] = {
+static const u32 dot11lcn_fltr_ctrl_tbl_rev0[] = {
 	0x000141f8,
 	0x000021f8,
 	0x000021fb,
@@ -1770,7 +1766,7 @@ const u32 dot11lcn_fltr_ctrl_tbl_rev0[] = {
 	0x0000024b,
 };
 
-const u32 dot11lcn_ps_ctrl_tbl_rev0[] = {
+static const u32 dot11lcn_ps_ctrl_tbl_rev0[] = {
 	0x00100001,
 	0x00200010,
 	0x00300001,
@@ -1793,7 +1789,7 @@ const u32 dot11lcn_ps_ctrl_tbl_rev0[] = {
 	0x00600f22,
 };
 
-const u16 dot11lcn_sw_ctrl_tbl_4313_epa_rev0_combo[] = {
+static const u16 dot11lcn_sw_ctrl_tbl_4313_epa_rev0_combo[] = {
 	0x0007,
 	0x0005,
 	0x0006,
@@ -1861,7 +1857,7 @@ const u16 dot11lcn_sw_ctrl_tbl_4313_epa_rev0_combo[] = {
 
 };
 
-const u16 dot11lcn_sw_ctrl_tbl_4313_bt_epa_p250_rev0[] = {
+static const u16 dot11lcn_sw_ctrl_tbl_4313_bt_epa_p250_rev0[] = {
 	0x0007,
 	0x0005,
 	0x0002,
@@ -1928,7 +1924,7 @@ const u16 dot11lcn_sw_ctrl_tbl_4313_bt_epa_p250_rev0[] = {
 	0x0002,
 };
 
-const u16 dot11lcn_sw_ctrl_tbl_4313_epa_rev0[] = {
+static const u16 dot11lcn_sw_ctrl_tbl_4313_epa_rev0[] = {
 	0x0002,
 	0x0008,
 	0x0004,
@@ -1995,7 +1991,7 @@ const u16 dot11lcn_sw_ctrl_tbl_4313_epa_rev0[] = {
 	0x0001,
 };
 
-const u16 dot11lcn_sw_ctrl_tbl_4313_rev0[] = {
+static const u16 dot11lcn_sw_ctrl_tbl_4313_rev0[] = {
 	0x000a,
 	0x0009,
 	0x0006,
@@ -2062,7 +2058,7 @@ const u16 dot11lcn_sw_ctrl_tbl_4313_rev0[] = {
 	0x0005,
 };
 
-const u16 dot11lcn_sw_ctrl_tbl_rev0[] = {
+static const u16 dot11lcn_sw_ctrl_tbl_rev0[] = {
 	0x0004,
 	0x0004,
 	0x0002,
@@ -2129,7 +2125,7 @@ const u16 dot11lcn_sw_ctrl_tbl_rev0[] = {
 	0x0002,
 };
 
-const u8 dot11lcn_nf_table_rev0[] = {
+static const u8 dot11lcn_nf_table_rev0[] = {
 	0x5f,
 	0x36,
 	0x29,
@@ -2148,7 +2144,7 @@ const u8 dot11lcn_nf_table_rev0[] = {
 	0x1f,
 };
 
-const u8 dot11lcn_gain_val_tbl_rev0[] = {
+static const u8 dot11lcn_gain_val_tbl_rev0[] = {
 	0x09,
 	0x0f,
 	0x14,
@@ -2211,7 +2207,7 @@ const u8 dot11lcn_gain_val_tbl_rev0[] = {
 	0x00,
 };
 
-const u8 dot11lcn_spur_tbl_rev0[] = {
+static const u8 dot11lcn_spur_tbl_rev0[] = {
 	0x01,
 	0x01,
 	0x01,
@@ -2342,7 +2338,7 @@ const u8 dot11lcn_spur_tbl_rev0[] = {
 	0x01,
 };
 
-const u16 dot11lcn_unsup_mcs_tbl_rev0[] = {
+static const u16 dot11lcn_unsup_mcs_tbl_rev0[] = {
 	0x001a,
 	0x0034,
 	0x004e,
@@ -2499,7 +2495,7 @@ const u16 dot11lcn_unsup_mcs_tbl_rev0[] = {
 	0x06f6,
 };
 
-const u16 dot11lcn_iq_local_tbl_rev0[] = {
+static const u16 dot11lcn_iq_local_tbl_rev0[] = {
 	0x0200,
 	0x0300,
 	0x0400,
@@ -2610,7 +2606,7 @@ const u16 dot11lcn_iq_local_tbl_rev0[] = {
 	0x0000,
 };
 
-const u32 dot11lcn_papd_compdelta_tbl_rev0[] = {
+static const u32 dot11lcn_papd_compdelta_tbl_rev0[] = {
 	0x00080000,
 	0x00080000,
 	0x00080000,
@@ -2834,26 +2830,26 @@ const struct phytbl_info dot11lcnphytbl_info_rev0[] = {
 
 const struct phytbl_info dot11lcn_sw_ctrl_tbl_info_4313 = {
 	&dot11lcn_sw_ctrl_tbl_4313_rev0,
-	    sizeof(dot11lcn_sw_ctrl_tbl_4313_rev0) /
-	    sizeof(dot11lcn_sw_ctrl_tbl_4313_rev0[0]), 15, 0, 16
+	sizeof(dot11lcn_sw_ctrl_tbl_4313_rev0) /
+	sizeof(dot11lcn_sw_ctrl_tbl_4313_rev0[0]), 15, 0, 16
 };
 
 const struct phytbl_info dot11lcn_sw_ctrl_tbl_info_4313_epa = {
 	&dot11lcn_sw_ctrl_tbl_4313_epa_rev0,
-	    sizeof(dot11lcn_sw_ctrl_tbl_4313_epa_rev0) /
-	    sizeof(dot11lcn_sw_ctrl_tbl_4313_epa_rev0[0]), 15, 0, 16
+	sizeof(dot11lcn_sw_ctrl_tbl_4313_epa_rev0) /
+	sizeof(dot11lcn_sw_ctrl_tbl_4313_epa_rev0[0]), 15, 0, 16
 };
 
 const struct phytbl_info dot11lcn_sw_ctrl_tbl_info_4313_bt_epa = {
 	&dot11lcn_sw_ctrl_tbl_4313_epa_rev0_combo,
-	    sizeof(dot11lcn_sw_ctrl_tbl_4313_epa_rev0_combo) /
-	    sizeof(dot11lcn_sw_ctrl_tbl_4313_epa_rev0_combo[0]), 15, 0, 16
+	sizeof(dot11lcn_sw_ctrl_tbl_4313_epa_rev0_combo) /
+	sizeof(dot11lcn_sw_ctrl_tbl_4313_epa_rev0_combo[0]), 15, 0, 16
 };
 
 const struct phytbl_info dot11lcn_sw_ctrl_tbl_info_4313_bt_epa_p250 = {
 	&dot11lcn_sw_ctrl_tbl_4313_bt_epa_p250_rev0,
-	    sizeof(dot11lcn_sw_ctrl_tbl_4313_bt_epa_p250_rev0) /
-	    sizeof(dot11lcn_sw_ctrl_tbl_4313_bt_epa_p250_rev0[0]), 15, 0, 16
+	sizeof(dot11lcn_sw_ctrl_tbl_4313_bt_epa_p250_rev0) /
+	sizeof(dot11lcn_sw_ctrl_tbl_4313_bt_epa_p250_rev0[0]), 15, 0, 16
 };
 
 const u32 dot11lcnphytbl_info_sz_rev0 =
@@ -2861,778 +2857,394 @@ const u32 dot11lcnphytbl_info_sz_rev0 =
 
 const struct lcnphy_tx_gain_tbl_entry
 dot11lcnphy_2GHz_extPA_gaintable_rev0[128] = {
-	{3, 0, 31, 0, 72,}
-	,
-	{3, 0, 31, 0, 70,}
-	,
-	{3, 0, 31, 0, 68,}
-	,
-	{3, 0, 30, 0, 67,}
-	,
-	{3, 0, 29, 0, 68,}
-	,
-	{3, 0, 28, 0, 68,}
-	,
-	{3, 0, 27, 0, 69,}
-	,
-	{3, 0, 26, 0, 70,}
-	,
-	{3, 0, 25, 0, 70,}
-	,
-	{3, 0, 24, 0, 71,}
-	,
-	{3, 0, 23, 0, 72,}
-	,
-	{3, 0, 23, 0, 70,}
-	,
-	{3, 0, 22, 0, 71,}
-	,
-	{3, 0, 21, 0, 72,}
-	,
-	{3, 0, 21, 0, 70,}
-	,
-	{3, 0, 21, 0, 68,}
-	,
-	{3, 0, 21, 0, 66,}
-	,
-	{3, 0, 21, 0, 64,}
-	,
-	{3, 0, 21, 0, 63,}
-	,
-	{3, 0, 20, 0, 64,}
-	,
-	{3, 0, 19, 0, 65,}
-	,
-	{3, 0, 19, 0, 64,}
-	,
-	{3, 0, 18, 0, 65,}
-	,
-	{3, 0, 18, 0, 64,}
-	,
-	{3, 0, 17, 0, 65,}
-	,
-	{3, 0, 17, 0, 64,}
-	,
-	{3, 0, 16, 0, 65,}
-	,
-	{3, 0, 16, 0, 64,}
-	,
-	{3, 0, 16, 0, 62,}
-	,
-	{3, 0, 16, 0, 60,}
-	,
-	{3, 0, 16, 0, 58,}
-	,
-	{3, 0, 15, 0, 61,}
-	,
-	{3, 0, 15, 0, 59,}
-	,
-	{3, 0, 14, 0, 61,}
-	,
-	{3, 0, 14, 0, 60,}
-	,
-	{3, 0, 14, 0, 58,}
-	,
-	{3, 0, 13, 0, 60,}
-	,
-	{3, 0, 13, 0, 59,}
-	,
-	{3, 0, 12, 0, 62,}
-	,
-	{3, 0, 12, 0, 60,}
-	,
-	{3, 0, 12, 0, 58,}
-	,
-	{3, 0, 11, 0, 62,}
-	,
-	{3, 0, 11, 0, 60,}
-	,
-	{3, 0, 11, 0, 59,}
-	,
-	{3, 0, 11, 0, 57,}
-	,
-	{3, 0, 10, 0, 61,}
-	,
-	{3, 0, 10, 0, 59,}
-	,
-	{3, 0, 10, 0, 57,}
-	,
-	{3, 0, 9, 0, 62,}
-	,
-	{3, 0, 9, 0, 60,}
-	,
-	{3, 0, 9, 0, 58,}
-	,
-	{3, 0, 9, 0, 57,}
-	,
-	{3, 0, 8, 0, 62,}
-	,
-	{3, 0, 8, 0, 60,}
-	,
-	{3, 0, 8, 0, 58,}
-	,
-	{3, 0, 8, 0, 57,}
-	,
-	{3, 0, 8, 0, 55,}
-	,
-	{3, 0, 7, 0, 61,}
-	,
-	{3, 0, 7, 0, 60,}
-	,
-	{3, 0, 7, 0, 58,}
-	,
-	{3, 0, 7, 0, 56,}
-	,
-	{3, 0, 7, 0, 55,}
-	,
-	{3, 0, 6, 0, 62,}
-	,
-	{3, 0, 6, 0, 60,}
-	,
-	{3, 0, 6, 0, 58,}
-	,
-	{3, 0, 6, 0, 57,}
-	,
-	{3, 0, 6, 0, 55,}
-	,
-	{3, 0, 6, 0, 54,}
-	,
-	{3, 0, 6, 0, 52,}
-	,
-	{3, 0, 5, 0, 61,}
-	,
-	{3, 0, 5, 0, 59,}
-	,
-	{3, 0, 5, 0, 57,}
-	,
-	{3, 0, 5, 0, 56,}
-	,
-	{3, 0, 5, 0, 54,}
-	,
-	{3, 0, 5, 0, 53,}
-	,
-	{3, 0, 5, 0, 51,}
-	,
-	{3, 0, 4, 0, 62,}
-	,
-	{3, 0, 4, 0, 60,}
-	,
-	{3, 0, 4, 0, 58,}
-	,
-	{3, 0, 4, 0, 57,}
-	,
-	{3, 0, 4, 0, 55,}
-	,
-	{3, 0, 4, 0, 54,}
-	,
-	{3, 0, 4, 0, 52,}
-	,
-	{3, 0, 4, 0, 51,}
-	,
-	{3, 0, 4, 0, 49,}
-	,
-	{3, 0, 4, 0, 48,}
-	,
-	{3, 0, 4, 0, 46,}
-	,
-	{3, 0, 3, 0, 60,}
-	,
-	{3, 0, 3, 0, 58,}
-	,
-	{3, 0, 3, 0, 57,}
-	,
-	{3, 0, 3, 0, 55,}
-	,
-	{3, 0, 3, 0, 54,}
-	,
-	{3, 0, 3, 0, 52,}
-	,
-	{3, 0, 3, 0, 51,}
-	,
-	{3, 0, 3, 0, 49,}
-	,
-	{3, 0, 3, 0, 48,}
-	,
-	{3, 0, 3, 0, 46,}
-	,
-	{3, 0, 3, 0, 45,}
-	,
-	{3, 0, 3, 0, 44,}
-	,
-	{3, 0, 3, 0, 43,}
-	,
-	{3, 0, 3, 0, 41,}
-	,
-	{3, 0, 2, 0, 61,}
-	,
-	{3, 0, 2, 0, 59,}
-	,
-	{3, 0, 2, 0, 57,}
-	,
-	{3, 0, 2, 0, 56,}
-	,
-	{3, 0, 2, 0, 54,}
-	,
-	{3, 0, 2, 0, 53,}
-	,
-	{3, 0, 2, 0, 51,}
-	,
-	{3, 0, 2, 0, 50,}
-	,
-	{3, 0, 2, 0, 48,}
-	,
-	{3, 0, 2, 0, 47,}
-	,
-	{3, 0, 2, 0, 46,}
-	,
-	{3, 0, 2, 0, 44,}
-	,
-	{3, 0, 2, 0, 43,}
-	,
-	{3, 0, 2, 0, 42,}
-	,
-	{3, 0, 2, 0, 41,}
-	,
-	{3, 0, 2, 0, 39,}
-	,
-	{3, 0, 2, 0, 38,}
-	,
-	{3, 0, 2, 0, 37,}
-	,
-	{3, 0, 2, 0, 36,}
-	,
-	{3, 0, 2, 0, 35,}
-	,
-	{3, 0, 2, 0, 34,}
-	,
-	{3, 0, 2, 0, 33,}
-	,
-	{3, 0, 2, 0, 32,}
-	,
-	{3, 0, 1, 0, 63,}
-	,
-	{3, 0, 1, 0, 61,}
-	,
-	{3, 0, 1, 0, 59,}
-	,
-	{3, 0, 1, 0, 57,}
-	,
+	{3, 0, 31, 0, 72},
+	{3, 0, 31, 0, 70},
+	{3, 0, 31, 0, 68},
+	{3, 0, 30, 0, 67},
+	{3, 0, 29, 0, 68},
+	{3, 0, 28, 0, 68},
+	{3, 0, 27, 0, 69},
+	{3, 0, 26, 0, 70},
+	{3, 0, 25, 0, 70},
+	{3, 0, 24, 0, 71},
+	{3, 0, 23, 0, 72},
+	{3, 0, 23, 0, 70},
+	{3, 0, 22, 0, 71},
+	{3, 0, 21, 0, 72},
+	{3, 0, 21, 0, 70},
+	{3, 0, 21, 0, 68},
+	{3, 0, 21, 0, 66},
+	{3, 0, 21, 0, 64},
+	{3, 0, 21, 0, 63},
+	{3, 0, 20, 0, 64},
+	{3, 0, 19, 0, 65},
+	{3, 0, 19, 0, 64},
+	{3, 0, 18, 0, 65},
+	{3, 0, 18, 0, 64},
+	{3, 0, 17, 0, 65},
+	{3, 0, 17, 0, 64},
+	{3, 0, 16, 0, 65},
+	{3, 0, 16, 0, 64},
+	{3, 0, 16, 0, 62},
+	{3, 0, 16, 0, 60},
+	{3, 0, 16, 0, 58},
+	{3, 0, 15, 0, 61},
+	{3, 0, 15, 0, 59},
+	{3, 0, 14, 0, 61},
+	{3, 0, 14, 0, 60},
+	{3, 0, 14, 0, 58},
+	{3, 0, 13, 0, 60},
+	{3, 0, 13, 0, 59},
+	{3, 0, 12, 0, 62},
+	{3, 0, 12, 0, 60},
+	{3, 0, 12, 0, 58},
+	{3, 0, 11, 0, 62},
+	{3, 0, 11, 0, 60},
+	{3, 0, 11, 0, 59},
+	{3, 0, 11, 0, 57},
+	{3, 0, 10, 0, 61},
+	{3, 0, 10, 0, 59},
+	{3, 0, 10, 0, 57},
+	{3, 0, 9, 0, 62},
+	{3, 0, 9, 0, 60},
+	{3, 0, 9, 0, 58},
+	{3, 0, 9, 0, 57},
+	{3, 0, 8, 0, 62},
+	{3, 0, 8, 0, 60},
+	{3, 0, 8, 0, 58},
+	{3, 0, 8, 0, 57},
+	{3, 0, 8, 0, 55},
+	{3, 0, 7, 0, 61},
+	{3, 0, 7, 0, 60},
+	{3, 0, 7, 0, 58},
+	{3, 0, 7, 0, 56},
+	{3, 0, 7, 0, 55},
+	{3, 0, 6, 0, 62},
+	{3, 0, 6, 0, 60},
+	{3, 0, 6, 0, 58},
+	{3, 0, 6, 0, 57},
+	{3, 0, 6, 0, 55},
+	{3, 0, 6, 0, 54},
+	{3, 0, 6, 0, 52},
+	{3, 0, 5, 0, 61},
+	{3, 0, 5, 0, 59},
+	{3, 0, 5, 0, 57},
+	{3, 0, 5, 0, 56},
+	{3, 0, 5, 0, 54},
+	{3, 0, 5, 0, 53},
+	{3, 0, 5, 0, 51},
+	{3, 0, 4, 0, 62},
+	{3, 0, 4, 0, 60},
+	{3, 0, 4, 0, 58},
+	{3, 0, 4, 0, 57},
+	{3, 0, 4, 0, 55},
+	{3, 0, 4, 0, 54},
+	{3, 0, 4, 0, 52},
+	{3, 0, 4, 0, 51},
+	{3, 0, 4, 0, 49},
+	{3, 0, 4, 0, 48},
+	{3, 0, 4, 0, 46},
+	{3, 0, 3, 0, 60},
+	{3, 0, 3, 0, 58},
+	{3, 0, 3, 0, 57},
+	{3, 0, 3, 0, 55},
+	{3, 0, 3, 0, 54},
+	{3, 0, 3, 0, 52},
+	{3, 0, 3, 0, 51},
+	{3, 0, 3, 0, 49},
+	{3, 0, 3, 0, 48},
+	{3, 0, 3, 0, 46},
+	{3, 0, 3, 0, 45},
+	{3, 0, 3, 0, 44},
+	{3, 0, 3, 0, 43},
+	{3, 0, 3, 0, 41},
+	{3, 0, 2, 0, 61},
+	{3, 0, 2, 0, 59},
+	{3, 0, 2, 0, 57},
+	{3, 0, 2, 0, 56},
+	{3, 0, 2, 0, 54},
+	{3, 0, 2, 0, 53},
+	{3, 0, 2, 0, 51},
+	{3, 0, 2, 0, 50},
+	{3, 0, 2, 0, 48},
+	{3, 0, 2, 0, 47},
+	{3, 0, 2, 0, 46},
+	{3, 0, 2, 0, 44},
+	{3, 0, 2, 0, 43},
+	{3, 0, 2, 0, 42},
+	{3, 0, 2, 0, 41},
+	{3, 0, 2, 0, 39},
+	{3, 0, 2, 0, 38},
+	{3, 0, 2, 0, 37},
+	{3, 0, 2, 0, 36},
+	{3, 0, 2, 0, 35},
+	{3, 0, 2, 0, 34},
+	{3, 0, 2, 0, 33},
+	{3, 0, 2, 0, 32},
+	{3, 0, 1, 0, 63},
+	{3, 0, 1, 0, 61},
+	{3, 0, 1, 0, 59},
+	{3, 0, 1, 0, 57},
 };
 
 const struct lcnphy_tx_gain_tbl_entry dot11lcnphy_2GHz_gaintable_rev0[128] = {
-	{7, 0, 31, 0, 72,}
-	,
-	{7, 0, 31, 0, 70,}
-	,
-	{7, 0, 31, 0, 68,}
-	,
-	{7, 0, 30, 0, 67,}
-	,
-	{7, 0, 29, 0, 68,}
-	,
-	{7, 0, 28, 0, 68,}
-	,
-	{7, 0, 27, 0, 69,}
-	,
-	{7, 0, 26, 0, 70,}
-	,
-	{7, 0, 25, 0, 70,}
-	,
-	{7, 0, 24, 0, 71,}
-	,
-	{7, 0, 23, 0, 72,}
-	,
-	{7, 0, 23, 0, 70,}
-	,
-	{7, 0, 22, 0, 71,}
-	,
-	{7, 0, 21, 0, 72,}
-	,
-	{7, 0, 21, 0, 70,}
-	,
-	{7, 0, 21, 0, 68,}
-	,
-	{7, 0, 21, 0, 66,}
-	,
-	{7, 0, 21, 0, 64,}
-	,
-	{7, 0, 21, 0, 63,}
-	,
-	{7, 0, 20, 0, 64,}
-	,
-	{7, 0, 19, 0, 65,}
-	,
-	{7, 0, 19, 0, 64,}
-	,
-	{7, 0, 18, 0, 65,}
-	,
-	{7, 0, 18, 0, 64,}
-	,
-	{7, 0, 17, 0, 65,}
-	,
-	{7, 0, 17, 0, 64,}
-	,
-	{7, 0, 16, 0, 65,}
-	,
-	{7, 0, 16, 0, 64,}
-	,
-	{7, 0, 16, 0, 62,}
-	,
-	{7, 0, 16, 0, 60,}
-	,
-	{7, 0, 16, 0, 58,}
-	,
-	{7, 0, 15, 0, 61,}
-	,
-	{7, 0, 15, 0, 59,}
-	,
-	{7, 0, 14, 0, 61,}
-	,
-	{7, 0, 14, 0, 60,}
-	,
-	{7, 0, 14, 0, 58,}
-	,
-	{7, 0, 13, 0, 60,}
-	,
-	{7, 0, 13, 0, 59,}
-	,
-	{7, 0, 12, 0, 62,}
-	,
-	{7, 0, 12, 0, 60,}
-	,
-	{7, 0, 12, 0, 58,}
-	,
-	{7, 0, 11, 0, 62,}
-	,
-	{7, 0, 11, 0, 60,}
-	,
-	{7, 0, 11, 0, 59,}
-	,
-	{7, 0, 11, 0, 57,}
-	,
-	{7, 0, 10, 0, 61,}
-	,
-	{7, 0, 10, 0, 59,}
-	,
-	{7, 0, 10, 0, 57,}
-	,
-	{7, 0, 9, 0, 62,}
-	,
-	{7, 0, 9, 0, 60,}
-	,
-	{7, 0, 9, 0, 58,}
-	,
-	{7, 0, 9, 0, 57,}
-	,
-	{7, 0, 8, 0, 62,}
-	,
-	{7, 0, 8, 0, 60,}
-	,
-	{7, 0, 8, 0, 58,}
-	,
-	{7, 0, 8, 0, 57,}
-	,
-	{7, 0, 8, 0, 55,}
-	,
-	{7, 0, 7, 0, 61,}
-	,
-	{7, 0, 7, 0, 60,}
-	,
-	{7, 0, 7, 0, 58,}
-	,
-	{7, 0, 7, 0, 56,}
-	,
-	{7, 0, 7, 0, 55,}
-	,
-	{7, 0, 6, 0, 62,}
-	,
-	{7, 0, 6, 0, 60,}
-	,
-	{7, 0, 6, 0, 58,}
-	,
-	{7, 0, 6, 0, 57,}
-	,
-	{7, 0, 6, 0, 55,}
-	,
-	{7, 0, 6, 0, 54,}
-	,
-	{7, 0, 6, 0, 52,}
-	,
-	{7, 0, 5, 0, 61,}
-	,
-	{7, 0, 5, 0, 59,}
-	,
-	{7, 0, 5, 0, 57,}
-	,
-	{7, 0, 5, 0, 56,}
-	,
-	{7, 0, 5, 0, 54,}
-	,
-	{7, 0, 5, 0, 53,}
-	,
-	{7, 0, 5, 0, 51,}
-	,
-	{7, 0, 4, 0, 62,}
-	,
-	{7, 0, 4, 0, 60,}
-	,
-	{7, 0, 4, 0, 58,}
-	,
-	{7, 0, 4, 0, 57,}
-	,
-	{7, 0, 4, 0, 55,}
-	,
-	{7, 0, 4, 0, 54,}
-	,
-	{7, 0, 4, 0, 52,}
-	,
-	{7, 0, 4, 0, 51,}
-	,
-	{7, 0, 4, 0, 49,}
-	,
-	{7, 0, 4, 0, 48,}
-	,
-	{7, 0, 4, 0, 46,}
-	,
-	{7, 0, 3, 0, 60,}
-	,
-	{7, 0, 3, 0, 58,}
-	,
-	{7, 0, 3, 0, 57,}
-	,
-	{7, 0, 3, 0, 55,}
-	,
-	{7, 0, 3, 0, 54,}
-	,
-	{7, 0, 3, 0, 52,}
-	,
-	{7, 0, 3, 0, 51,}
-	,
-	{7, 0, 3, 0, 49,}
-	,
-	{7, 0, 3, 0, 48,}
-	,
-	{7, 0, 3, 0, 46,}
-	,
-	{7, 0, 3, 0, 45,}
-	,
-	{7, 0, 3, 0, 44,}
-	,
-	{7, 0, 3, 0, 43,}
-	,
-	{7, 0, 3, 0, 41,}
-	,
-	{7, 0, 2, 0, 61,}
-	,
-	{7, 0, 2, 0, 59,}
-	,
-	{7, 0, 2, 0, 57,}
-	,
-	{7, 0, 2, 0, 56,}
-	,
-	{7, 0, 2, 0, 54,}
-	,
-	{7, 0, 2, 0, 53,}
-	,
-	{7, 0, 2, 0, 51,}
-	,
-	{7, 0, 2, 0, 50,}
-	,
-	{7, 0, 2, 0, 48,}
-	,
-	{7, 0, 2, 0, 47,}
-	,
-	{7, 0, 2, 0, 46,}
-	,
-	{7, 0, 2, 0, 44,}
-	,
-	{7, 0, 2, 0, 43,}
-	,
-	{7, 0, 2, 0, 42,}
-	,
-	{7, 0, 2, 0, 41,}
-	,
-	{7, 0, 2, 0, 39,}
-	,
-	{7, 0, 2, 0, 38,}
-	,
-	{7, 0, 2, 0, 37,}
-	,
-	{7, 0, 2, 0, 36,}
-	,
-	{7, 0, 2, 0, 35,}
-	,
-	{7, 0, 2, 0, 34,}
-	,
-	{7, 0, 2, 0, 33,}
-	,
-	{7, 0, 2, 0, 32,}
-	,
-	{7, 0, 1, 0, 63,}
-	,
-	{7, 0, 1, 0, 61,}
-	,
-	{7, 0, 1, 0, 59,}
-	,
-	{7, 0, 1, 0, 57,}
-	,
+	{7, 0, 31, 0, 72},
+	{7, 0, 31, 0, 70},
+	{7, 0, 31, 0, 68},
+	{7, 0, 30, 0, 67},
+	{7, 0, 29, 0, 68},
+	{7, 0, 28, 0, 68},
+	{7, 0, 27, 0, 69},
+	{7, 0, 26, 0, 70},
+	{7, 0, 25, 0, 70},
+	{7, 0, 24, 0, 71},
+	{7, 0, 23, 0, 72},
+	{7, 0, 23, 0, 70},
+	{7, 0, 22, 0, 71},
+	{7, 0, 21, 0, 72},
+	{7, 0, 21, 0, 70},
+	{7, 0, 21, 0, 68},
+	{7, 0, 21, 0, 66},
+	{7, 0, 21, 0, 64},
+	{7, 0, 21, 0, 63},
+	{7, 0, 20, 0, 64},
+	{7, 0, 19, 0, 65},
+	{7, 0, 19, 0, 64},
+	{7, 0, 18, 0, 65},
+	{7, 0, 18, 0, 64},
+	{7, 0, 17, 0, 65},
+	{7, 0, 17, 0, 64},
+	{7, 0, 16, 0, 65},
+	{7, 0, 16, 0, 64},
+	{7, 0, 16, 0, 62},
+	{7, 0, 16, 0, 60},
+	{7, 0, 16, 0, 58},
+	{7, 0, 15, 0, 61},
+	{7, 0, 15, 0, 59},
+	{7, 0, 14, 0, 61},
+	{7, 0, 14, 0, 60},
+	{7, 0, 14, 0, 58},
+	{7, 0, 13, 0, 60},
+	{7, 0, 13, 0, 59},
+	{7, 0, 12, 0, 62},
+	{7, 0, 12, 0, 60},
+	{7, 0, 12, 0, 58},
+	{7, 0, 11, 0, 62},
+	{7, 0, 11, 0, 60},
+	{7, 0, 11, 0, 59},
+	{7, 0, 11, 0, 57},
+	{7, 0, 10, 0, 61},
+	{7, 0, 10, 0, 59},
+	{7, 0, 10, 0, 57},
+	{7, 0, 9, 0, 62},
+	{7, 0, 9, 0, 60},
+	{7, 0, 9, 0, 58},
+	{7, 0, 9, 0, 57},
+	{7, 0, 8, 0, 62},
+	{7, 0, 8, 0, 60},
+	{7, 0, 8, 0, 58},
+	{7, 0, 8, 0, 57},
+	{7, 0, 8, 0, 55},
+	{7, 0, 7, 0, 61},
+	{7, 0, 7, 0, 60},
+	{7, 0, 7, 0, 58},
+	{7, 0, 7, 0, 56},
+	{7, 0, 7, 0, 55},
+	{7, 0, 6, 0, 62},
+	{7, 0, 6, 0, 60},
+	{7, 0, 6, 0, 58},
+	{7, 0, 6, 0, 57},
+	{7, 0, 6, 0, 55},
+	{7, 0, 6, 0, 54},
+	{7, 0, 6, 0, 52},
+	{7, 0, 5, 0, 61},
+	{7, 0, 5, 0, 59},
+	{7, 0, 5, 0, 57},
+	{7, 0, 5, 0, 56},
+	{7, 0, 5, 0, 54},
+	{7, 0, 5, 0, 53},
+	{7, 0, 5, 0, 51},
+	{7, 0, 4, 0, 62},
+	{7, 0, 4, 0, 60},
+	{7, 0, 4, 0, 58},
+	{7, 0, 4, 0, 57},
+	{7, 0, 4, 0, 55},
+	{7, 0, 4, 0, 54},
+	{7, 0, 4, 0, 52},
+	{7, 0, 4, 0, 51},
+	{7, 0, 4, 0, 49},
+	{7, 0, 4, 0, 48},
+	{7, 0, 4, 0, 46},
+	{7, 0, 3, 0, 60},
+	{7, 0, 3, 0, 58},
+	{7, 0, 3, 0, 57},
+	{7, 0, 3, 0, 55},
+	{7, 0, 3, 0, 54},
+	{7, 0, 3, 0, 52},
+	{7, 0, 3, 0, 51},
+	{7, 0, 3, 0, 49},
+	{7, 0, 3, 0, 48},
+	{7, 0, 3, 0, 46},
+	{7, 0, 3, 0, 45},
+	{7, 0, 3, 0, 44},
+	{7, 0, 3, 0, 43},
+	{7, 0, 3, 0, 41},
+	{7, 0, 2, 0, 61},
+	{7, 0, 2, 0, 59},
+	{7, 0, 2, 0, 57},
+	{7, 0, 2, 0, 56},
+	{7, 0, 2, 0, 54},
+	{7, 0, 2, 0, 53},
+	{7, 0, 2, 0, 51},
+	{7, 0, 2, 0, 50},
+	{7, 0, 2, 0, 48},
+	{7, 0, 2, 0, 47},
+	{7, 0, 2, 0, 46},
+	{7, 0, 2, 0, 44},
+	{7, 0, 2, 0, 43},
+	{7, 0, 2, 0, 42},
+	{7, 0, 2, 0, 41},
+	{7, 0, 2, 0, 39},
+	{7, 0, 2, 0, 38},
+	{7, 0, 2, 0, 37},
+	{7, 0, 2, 0, 36},
+	{7, 0, 2, 0, 35},
+	{7, 0, 2, 0, 34},
+	{7, 0, 2, 0, 33},
+	{7, 0, 2, 0, 32},
+	{7, 0, 1, 0, 63},
+	{7, 0, 1, 0, 61},
+	{7, 0, 1, 0, 59},
+	{7, 0, 1, 0, 57},
 };
 
 const struct lcnphy_tx_gain_tbl_entry dot11lcnphy_5GHz_gaintable_rev0[128] = {
-	{255, 255, 0xf0, 0, 152,}
-	,
-	{255, 255, 0xf0, 0, 147,}
-	,
-	{255, 255, 0xf0, 0, 143,}
-	,
-	{255, 255, 0xf0, 0, 139,}
-	,
-	{255, 255, 0xf0, 0, 135,}
-	,
-	{255, 255, 0xf0, 0, 131,}
-	,
-	{255, 255, 0xf0, 0, 128,}
-	,
-	{255, 255, 0xf0, 0, 124,}
-	,
-	{255, 255, 0xf0, 0, 121,}
-	,
-	{255, 255, 0xf0, 0, 117,}
-	,
-	{255, 255, 0xf0, 0, 114,}
-	,
-	{255, 255, 0xf0, 0, 111,}
-	,
-	{255, 255, 0xf0, 0, 107,}
-	,
-	{255, 255, 0xf0, 0, 104,}
-	,
-	{255, 255, 0xf0, 0, 101,}
-	,
-	{255, 255, 0xf0, 0, 99,}
-	,
-	{255, 255, 0xf0, 0, 96,}
-	,
-	{255, 255, 0xf0, 0, 93,}
-	,
-	{255, 255, 0xf0, 0, 90,}
-	,
-	{255, 255, 0xf0, 0, 88,}
-	,
-	{255, 255, 0xf0, 0, 85,}
-	,
-	{255, 255, 0xf0, 0, 83,}
-	,
-	{255, 255, 0xf0, 0, 81,}
-	,
-	{255, 255, 0xf0, 0, 78,}
-	,
-	{255, 255, 0xf0, 0, 76,}
-	,
-	{255, 255, 0xf0, 0, 74,}
-	,
-	{255, 255, 0xf0, 0, 72,}
-	,
-	{255, 255, 0xf0, 0, 70,}
-	,
-	{255, 255, 0xf0, 0, 68,}
-	,
-	{255, 255, 0xf0, 0, 66,}
-	,
-	{255, 255, 0xf0, 0, 64,}
-	,
-	{255, 248, 0xf0, 0, 64,}
-	,
-	{255, 241, 0xf0, 0, 64,}
-	,
-	{255, 251, 0xe0, 0, 64,}
-	,
-	{255, 244, 0xe0, 0, 64,}
-	,
-	{255, 254, 0xd0, 0, 64,}
-	,
-	{255, 246, 0xd0, 0, 64,}
-	,
-	{255, 239, 0xd0, 0, 64,}
-	,
-	{255, 249, 0xc0, 0, 64,}
-	,
-	{255, 242, 0xc0, 0, 64,}
-	,
-	{255, 255, 0xb0, 0, 64,}
-	,
-	{255, 248, 0xb0, 0, 64,}
-	,
-	{255, 241, 0xb0, 0, 64,}
-	,
-	{255, 254, 0xa0, 0, 64,}
-	,
-	{255, 246, 0xa0, 0, 64,}
-	,
-	{255, 239, 0xa0, 0, 64,}
-	,
-	{255, 255, 0x90, 0, 64,}
-	,
-	{255, 248, 0x90, 0, 64,}
-	,
-	{255, 241, 0x90, 0, 64,}
-	,
-	{255, 234, 0x90, 0, 64,}
-	,
-	{255, 255, 0x80, 0, 64,}
-	,
-	{255, 248, 0x80, 0, 64,}
-	,
-	{255, 241, 0x80, 0, 64,}
-	,
-	{255, 234, 0x80, 0, 64,}
-	,
-	{255, 255, 0x70, 0, 64,}
-	,
-	{255, 248, 0x70, 0, 64,}
-	,
-	{255, 241, 0x70, 0, 64,}
-	,
-	{255, 234, 0x70, 0, 64,}
-	,
-	{255, 227, 0x70, 0, 64,}
-	,
-	{255, 221, 0x70, 0, 64,}
-	,
-	{255, 215, 0x70, 0, 64,}
-	,
-	{255, 208, 0x70, 0, 64,}
-	,
-	{255, 203, 0x70, 0, 64,}
-	,
-	{255, 197, 0x70, 0, 64,}
-	,
-	{255, 255, 0x60, 0, 64,}
-	,
-	{255, 248, 0x60, 0, 64,}
-	,
-	{255, 241, 0x60, 0, 64,}
-	,
-	{255, 234, 0x60, 0, 64,}
-	,
-	{255, 227, 0x60, 0, 64,}
-	,
-	{255, 221, 0x60, 0, 64,}
-	,
-	{255, 255, 0x50, 0, 64,}
-	,
-	{255, 248, 0x50, 0, 64,}
-	,
-	{255, 241, 0x50, 0, 64,}
-	,
-	{255, 234, 0x50, 0, 64,}
-	,
-	{255, 227, 0x50, 0, 64,}
-	,
-	{255, 221, 0x50, 0, 64,}
-	,
-	{255, 215, 0x50, 0, 64,}
-	,
-	{255, 208, 0x50, 0, 64,}
-	,
-	{255, 255, 0x40, 0, 64,}
-	,
-	{255, 248, 0x40, 0, 64,}
-	,
-	{255, 241, 0x40, 0, 64,}
-	,
-	{255, 234, 0x40, 0, 64,}
-	,
-	{255, 227, 0x40, 0, 64,}
-	,
-	{255, 221, 0x40, 0, 64,}
-	,
-	{255, 215, 0x40, 0, 64,}
-	,
-	{255, 208, 0x40, 0, 64,}
-	,
-	{255, 203, 0x40, 0, 64,}
-	,
-	{255, 197, 0x40, 0, 64,}
-	,
-	{255, 255, 0x30, 0, 64,}
-	,
-	{255, 248, 0x30, 0, 64,}
-	,
-	{255, 241, 0x30, 0, 64,}
-	,
-	{255, 234, 0x30, 0, 64,}
-	,
-	{255, 227, 0x30, 0, 64,}
-	,
-	{255, 221, 0x30, 0, 64,}
-	,
-	{255, 215, 0x30, 0, 64,}
-	,
-	{255, 208, 0x30, 0, 64,}
-	,
-	{255, 203, 0x30, 0, 64,}
-	,
-	{255, 197, 0x30, 0, 64,}
-	,
-	{255, 191, 0x30, 0, 64,}
-	,
-	{255, 186, 0x30, 0, 64,}
-	,
-	{255, 181, 0x30, 0, 64,}
-	,
-	{255, 175, 0x30, 0, 64,}
-	,
-	{255, 255, 0x20, 0, 64,}
-	,
-	{255, 248, 0x20, 0, 64,}
-	,
-	{255, 241, 0x20, 0, 64,}
-	,
-	{255, 234, 0x20, 0, 64,}
-	,
-	{255, 227, 0x20, 0, 64,}
-	,
-	{255, 221, 0x20, 0, 64,}
-	,
-	{255, 215, 0x20, 0, 64,}
-	,
-	{255, 208, 0x20, 0, 64,}
-	,
-	{255, 203, 0x20, 0, 64,}
-	,
-	{255, 197, 0x20, 0, 64,}
-	,
-	{255, 191, 0x20, 0, 64,}
-	,
-	{255, 186, 0x20, 0, 64,}
-	,
-	{255, 181, 0x20, 0, 64,}
-	,
-	{255, 175, 0x20, 0, 64,}
-	,
-	{255, 170, 0x20, 0, 64,}
-	,
-	{255, 166, 0x20, 0, 64,}
-	,
-	{255, 161, 0x20, 0, 64,}
-	,
-	{255, 156, 0x20, 0, 64,}
-	,
-	{255, 152, 0x20, 0, 64,}
-	,
-	{255, 148, 0x20, 0, 64,}
-	,
-	{255, 143, 0x20, 0, 64,}
-	,
-	{255, 139, 0x20, 0, 64,}
-	,
-	{255, 135, 0x20, 0, 64,}
-	,
-	{255, 132, 0x20, 0, 64,}
-	,
-	{255, 255, 0x10, 0, 64,}
-	,
-	{255, 248, 0x10, 0, 64,}
-	,
+	{255, 255, 0xf0, 0, 152},
+	{255, 255, 0xf0, 0, 147},
+	{255, 255, 0xf0, 0, 143},
+	{255, 255, 0xf0, 0, 139},
+	{255, 255, 0xf0, 0, 135},
+	{255, 255, 0xf0, 0, 131},
+	{255, 255, 0xf0, 0, 128},
+	{255, 255, 0xf0, 0, 124},
+	{255, 255, 0xf0, 0, 121},
+	{255, 255, 0xf0, 0, 117},
+	{255, 255, 0xf0, 0, 114},
+	{255, 255, 0xf0, 0, 111},
+	{255, 255, 0xf0, 0, 107},
+	{255, 255, 0xf0, 0, 104},
+	{255, 255, 0xf0, 0, 101},
+	{255, 255, 0xf0, 0, 99},
+	{255, 255, 0xf0, 0, 96},
+	{255, 255, 0xf0, 0, 93},
+	{255, 255, 0xf0, 0, 90},
+	{255, 255, 0xf0, 0, 88},
+	{255, 255, 0xf0, 0, 85},
+	{255, 255, 0xf0, 0, 83},
+	{255, 255, 0xf0, 0, 81},
+	{255, 255, 0xf0, 0, 78},
+	{255, 255, 0xf0, 0, 76},
+	{255, 255, 0xf0, 0, 74},
+	{255, 255, 0xf0, 0, 72},
+	{255, 255, 0xf0, 0, 70},
+	{255, 255, 0xf0, 0, 68},
+	{255, 255, 0xf0, 0, 66},
+	{255, 255, 0xf0, 0, 64},
+	{255, 248, 0xf0, 0, 64},
+	{255, 241, 0xf0, 0, 64},
+	{255, 251, 0xe0, 0, 64},
+	{255, 244, 0xe0, 0, 64},
+	{255, 254, 0xd0, 0, 64},
+	{255, 246, 0xd0, 0, 64},
+	{255, 239, 0xd0, 0, 64},
+	{255, 249, 0xc0, 0, 64},
+	{255, 242, 0xc0, 0, 64},
+	{255, 255, 0xb0, 0, 64},
+	{255, 248, 0xb0, 0, 64},
+	{255, 241, 0xb0, 0, 64},
+	{255, 254, 0xa0, 0, 64},
+	{255, 246, 0xa0, 0, 64},
+	{255, 239, 0xa0, 0, 64},
+	{255, 255, 0x90, 0, 64},
+	{255, 248, 0x90, 0, 64},
+	{255, 241, 0x90, 0, 64},
+	{255, 234, 0x90, 0, 64},
+	{255, 255, 0x80, 0, 64},
+	{255, 248, 0x80, 0, 64},
+	{255, 241, 0x80, 0, 64},
+	{255, 234, 0x80, 0, 64},
+	{255, 255, 0x70, 0, 64},
+	{255, 248, 0x70, 0, 64},
+	{255, 241, 0x70, 0, 64},
+	{255, 234, 0x70, 0, 64},
+	{255, 227, 0x70, 0, 64},
+	{255, 221, 0x70, 0, 64},
+	{255, 215, 0x70, 0, 64},
+	{255, 208, 0x70, 0, 64},
+	{255, 203, 0x70, 0, 64},
+	{255, 197, 0x70, 0, 64},
+	{255, 255, 0x60, 0, 64},
+	{255, 248, 0x60, 0, 64},
+	{255, 241, 0x60, 0, 64},
+	{255, 234, 0x60, 0, 64},
+	{255, 227, 0x60, 0, 64},
+	{255, 221, 0x60, 0, 64},
+	{255, 255, 0x50, 0, 64},
+	{255, 248, 0x50, 0, 64},
+	{255, 241, 0x50, 0, 64},
+	{255, 234, 0x50, 0, 64},
+	{255, 227, 0x50, 0, 64},
+	{255, 221, 0x50, 0, 64},
+	{255, 215, 0x50, 0, 64},
+	{255, 208, 0x50, 0, 64},
+	{255, 255, 0x40, 0, 64},
+	{255, 248, 0x40, 0, 64},
+	{255, 241, 0x40, 0, 64},
+	{255, 234, 0x40, 0, 64},
+	{255, 227, 0x40, 0, 64},
+	{255, 221, 0x40, 0, 64},
+	{255, 215, 0x40, 0, 64},
+	{255, 208, 0x40, 0, 64},
+	{255, 203, 0x40, 0, 64},
+	{255, 197, 0x40, 0, 64},
+	{255, 255, 0x30, 0, 64},
+	{255, 248, 0x30, 0, 64},
+	{255, 241, 0x30, 0, 64},
+	{255, 234, 0x30, 0, 64},
+	{255, 227, 0x30, 0, 64},
+	{255, 221, 0x30, 0, 64},
+	{255, 215, 0x30, 0, 64},
+	{255, 208, 0x30, 0, 64},
+	{255, 203, 0x30, 0, 64},
+	{255, 197, 0x30, 0, 64},
+	{255, 191, 0x30, 0, 64},
+	{255, 186, 0x30, 0, 64},
+	{255, 181, 0x30, 0, 64},
+	{255, 175, 0x30, 0, 64},
+	{255, 255, 0x20, 0, 64},
+	{255, 248, 0x20, 0, 64},
+	{255, 241, 0x20, 0, 64},
+	{255, 234, 0x20, 0, 64},
+	{255, 227, 0x20, 0, 64},
+	{255, 221, 0x20, 0, 64},
+	{255, 215, 0x20, 0, 64},
+	{255, 208, 0x20, 0, 64},
+	{255, 203, 0x20, 0, 64},
+	{255, 197, 0x20, 0, 64},
+	{255, 191, 0x20, 0, 64},
+	{255, 186, 0x20, 0, 64},
+	{255, 181, 0x20, 0, 64},
+	{255, 175, 0x20, 0, 64},
+	{255, 170, 0x20, 0, 64},
+	{255, 166, 0x20, 0, 64},
+	{255, 161, 0x20, 0, 64},
+	{255, 156, 0x20, 0, 64},
+	{255, 152, 0x20, 0, 64},
+	{255, 148, 0x20, 0, 64},
+	{255, 143, 0x20, 0, 64},
+	{255, 139, 0x20, 0, 64},
+	{255, 135, 0x20, 0, 64},
+	{255, 132, 0x20, 0, 64},
+	{255, 255, 0x10, 0, 64},
+	{255, 248, 0x10, 0, 64},
 };
diff --git a/drivers/staging/brcm80211/brcmsmac/phy/phytbl_n.c b/drivers/staging/brcm80211/brcmsmac/phy/phytbl_n.c
index 7f741f4868a6..dbf50ef6cd75 100644
--- a/drivers/staging/brcm80211/brcmsmac/phy/phytbl_n.c
+++ b/drivers/staging/brcm80211/brcmsmac/phy/phytbl_n.c
@@ -17,7 +17,7 @@
 #include <types.h>
 #include "phytbl_n.h"
 
-const u32 frame_struct_rev0[] = {
+static const u32 frame_struct_rev0[] = {
 	0x08004a04,
 	0x00100000,
 	0x01000a05,
@@ -852,7 +852,7 @@ const u32 frame_struct_rev0[] = {
 	0x00000000,
 };
 
-const u8 frame_lut_rev0[] = {
+static const u8 frame_lut_rev0[] = {
 	0x02,
 	0x04,
 	0x14,
@@ -887,7 +887,7 @@ const u8 frame_lut_rev0[] = {
 	0x2a,
 };
 
-const u32 tmap_tbl_rev0[] = {
+static const u32 tmap_tbl_rev0[] = {
 	0x8a88aa80,
 	0x8aaaaa8a,
 	0x8a8a8aa8,
@@ -1338,7 +1338,7 @@ const u32 tmap_tbl_rev0[] = {
 	0x00000000,
 };
 
-const u32 tdtrn_tbl_rev0[] = {
+static const u32 tdtrn_tbl_rev0[] = {
 	0x061c061c,
 	0x0050ee68,
 	0xf592fe36,
@@ -2045,7 +2045,7 @@ const u32 tdtrn_tbl_rev0[] = {
 	0x00f006be,
 };
 
-const u32 intlv_tbl_rev0[] = {
+static const u32 intlv_tbl_rev0[] = {
 	0x00802070,
 	0x0671188d,
 	0x0a60192c,
@@ -2055,7 +2055,7 @@ const u32 intlv_tbl_rev0[] = {
 	0x00000070,
 };
 
-const u16 pilot_tbl_rev0[] = {
+static const u16 pilot_tbl_rev0[] = {
 	0xff08,
 	0xff08,
 	0xff08,
@@ -2146,7 +2146,7 @@ const u16 pilot_tbl_rev0[] = {
 	0xffff,
 };
 
-const u32 pltlut_tbl_rev0[] = {
+static const u32 pltlut_tbl_rev0[] = {
 	0x76540123,
 	0x62407351,
 	0x76543201,
@@ -2155,7 +2155,7 @@ const u32 pltlut_tbl_rev0[] = {
 	0x76430521,
 };
 
-const u32 tdi_tbl20_ant0_rev0[] = {
+static const u32 tdi_tbl20_ant0_rev0[] = {
 	0x00091226,
 	0x000a1429,
 	0x000b56ad,
@@ -2213,7 +2213,7 @@ const u32 tdi_tbl20_ant0_rev0[] = {
 	0x00000000,
 };
 
-const u32 tdi_tbl20_ant1_rev0[] = {
+static const u32 tdi_tbl20_ant1_rev0[] = {
 	0x00014b26,
 	0x00028d29,
 	0x000393ad,
@@ -2271,7 +2271,7 @@ const u32 tdi_tbl20_ant1_rev0[] = {
 	0x00000000,
 };
 
-const u32 tdi_tbl40_ant0_rev0[] = {
+static const u32 tdi_tbl40_ant0_rev0[] = {
 	0x0011a346,
 	0x00136ccf,
 	0x0014f5d9,
@@ -2384,7 +2384,7 @@ const u32 tdi_tbl40_ant0_rev0[] = {
 	0x00000000,
 };
 
-const u32 tdi_tbl40_ant1_rev0[] = {
+static const u32 tdi_tbl40_ant1_rev0[] = {
 	0x001edb36,
 	0x000129ca,
 	0x0002b353,
@@ -2497,7 +2497,7 @@ const u32 tdi_tbl40_ant1_rev0[] = {
 	0x00000000,
 };
 
-const u16 bdi_tbl_rev0[] = {
+static const u16 bdi_tbl_rev0[] = {
 	0x0070,
 	0x0126,
 	0x012c,
@@ -2506,7 +2506,7 @@ const u16 bdi_tbl_rev0[] = {
 	0x04d2,
 };
 
-const u32 chanest_tbl_rev0[] = {
+static const u32 chanest_tbl_rev0[] = {
 	0x44444444,
 	0x44444444,
 	0x44444444,
@@ -2605,7 +2605,7 @@ const u32 chanest_tbl_rev0[] = {
 	0x10101010,
 };
 
-const u8 mcs_tbl_rev0[] = {
+static const u8 mcs_tbl_rev0[] = {
 	0x00,
 	0x08,
 	0x0a,
@@ -2736,7 +2736,7 @@ const u8 mcs_tbl_rev0[] = {
 	0x00,
 };
 
-const u32 noise_var_tbl0_rev0[] = {
+static const u32 noise_var_tbl0_rev0[] = {
 	0x020c020c,
 	0x0000014d,
 	0x020c020c,
@@ -2995,7 +2995,7 @@ const u32 noise_var_tbl0_rev0[] = {
 	0x0000014d,
 };
 
-const u32 noise_var_tbl1_rev0[] = {
+static const u32 noise_var_tbl1_rev0[] = {
 	0x020c020c,
 	0x0000014d,
 	0x020c020c,
@@ -3254,7 +3254,7 @@ const u32 noise_var_tbl1_rev0[] = {
 	0x0000014d,
 };
 
-const u8 est_pwr_lut_core0_rev0[] = {
+static const u8 est_pwr_lut_core0_rev0[] = {
 	0x50,
 	0x4f,
 	0x4e,
@@ -3321,7 +3321,7 @@ const u8 est_pwr_lut_core0_rev0[] = {
 	0x11,
 };
 
-const u8 est_pwr_lut_core1_rev0[] = {
+static const u8 est_pwr_lut_core1_rev0[] = {
 	0x50,
 	0x4f,
 	0x4e,
@@ -3388,7 +3388,7 @@ const u8 est_pwr_lut_core1_rev0[] = {
 	0x11,
 };
 
-const u8 adj_pwr_lut_core0_rev0[] = {
+static const u8 adj_pwr_lut_core0_rev0[] = {
 	0x00,
 	0x00,
 	0x00,
@@ -3519,7 +3519,7 @@ const u8 adj_pwr_lut_core0_rev0[] = {
 	0x00,
 };
 
-const u8 adj_pwr_lut_core1_rev0[] = {
+static const u8 adj_pwr_lut_core1_rev0[] = {
 	0x00,
 	0x00,
 	0x00,
@@ -3650,7 +3650,7 @@ const u8 adj_pwr_lut_core1_rev0[] = {
 	0x00,
 };
 
-const u32 gainctrl_lut_core0_rev0[] = {
+static const u32 gainctrl_lut_core0_rev0[] = {
 	0x03cc2b44,
 	0x03cc2b42,
 	0x03cc2b40,
@@ -3781,7 +3781,7 @@ const u32 gainctrl_lut_core0_rev0[] = {
 	0x00002b00,
 };
 
-const u32 gainctrl_lut_core1_rev0[] = {
+static const u32 gainctrl_lut_core1_rev0[] = {
 	0x03cc2b44,
 	0x03cc2b42,
 	0x03cc2b40,
@@ -3912,7 +3912,7 @@ const u32 gainctrl_lut_core1_rev0[] = {
 	0x00002b00,
 };
 
-const u32 iq_lut_core0_rev0[] = {
+static const u32 iq_lut_core0_rev0[] = {
 	0x0000007f,
 	0x0000007f,
 	0x0000007f,
@@ -4043,7 +4043,7 @@ const u32 iq_lut_core0_rev0[] = {
 	0x0000007f,
 };
 
-const u32 iq_lut_core1_rev0[] = {
+static const u32 iq_lut_core1_rev0[] = {
 	0x0000007f,
 	0x0000007f,
 	0x0000007f,
@@ -4174,7 +4174,7 @@ const u32 iq_lut_core1_rev0[] = {
 	0x0000007f,
 };
 
-const u16 loft_lut_core0_rev0[] = {
+static const u16 loft_lut_core0_rev0[] = {
 	0x0000,
 	0x0101,
 	0x0002,
@@ -4305,7 +4305,7 @@ const u16 loft_lut_core0_rev0[] = {
 	0x0103,
 };
 
-const u16 loft_lut_core1_rev0[] = {
+static const u16 loft_lut_core1_rev0[] = {
 	0x0000,
 	0x0101,
 	0x0002,
@@ -4522,7 +4522,8 @@ const struct phytbl_info mimophytbl_info_rev0[] = {
 	{&chanest_tbl_rev0,
 	 sizeof(chanest_tbl_rev0) / sizeof(chanest_tbl_rev0[0]), 22, 0, 32}
 	,
-	{&mcs_tbl_rev0, sizeof(mcs_tbl_rev0) / sizeof(mcs_tbl_rev0[0]), 18, 0, 8}
+	{&mcs_tbl_rev0, sizeof(mcs_tbl_rev0) / sizeof(mcs_tbl_rev0[0]), 18, 0,
+	 8}
 	,
 	{&noise_var_tbl0_rev0,
 	 sizeof(noise_var_tbl0_rev0) / sizeof(noise_var_tbl0_rev0[0]), 16, 0,
@@ -4540,7 +4541,7 @@ const u32 mimophytbl_info_sz_rev0_volatile =
 	sizeof(mimophytbl_info_rev0_volatile) /
 	sizeof(mimophytbl_info_rev0_volatile[0]);
 
-const u16 ant_swctrl_tbl_rev3[] = {
+static const u16 ant_swctrl_tbl_rev3[] = {
 	0x0082,
 	0x0082,
 	0x0211,
@@ -4575,7 +4576,7 @@ const u16 ant_swctrl_tbl_rev3[] = {
 	0x0000,
 };
 
-const u16 ant_swctrl_tbl_rev3_1[] = {
+static const u16 ant_swctrl_tbl_rev3_1[] = {
 	0x0022,
 	0x0022,
 	0x0011,
@@ -4610,7 +4611,7 @@ const u16 ant_swctrl_tbl_rev3_1[] = {
 	0x0000,
 };
 
-const u16 ant_swctrl_tbl_rev3_2[] = {
+static const u16 ant_swctrl_tbl_rev3_2[] = {
 	0x0088,
 	0x0088,
 	0x0044,
@@ -4645,7 +4646,7 @@ const u16 ant_swctrl_tbl_rev3_2[] = {
 	0x0000,
 };
 
-const u16 ant_swctrl_tbl_rev3_3[] = {
+static const u16 ant_swctrl_tbl_rev3_3[] = {
 	0x022,
 	0x022,
 	0x011,
@@ -4680,7 +4681,7 @@ const u16 ant_swctrl_tbl_rev3_3[] = {
 	0x3cc
 };
 
-const u32 frame_struct_rev3[] = {
+static const u32 frame_struct_rev3[] = {
 	0x08004a04,
 	0x00100000,
 	0x01000a05,
@@ -5515,7 +5516,7 @@ const u32 frame_struct_rev3[] = {
 	0x00000000,
 };
 
-const u16 pilot_tbl_rev3[] = {
+static const u16 pilot_tbl_rev3[] = {
 	0xff08,
 	0xff08,
 	0xff08,
@@ -5606,7 +5607,7 @@ const u16 pilot_tbl_rev3[] = {
 	0xffff,
 };
 
-const u32 tmap_tbl_rev3[] = {
+static const u32 tmap_tbl_rev3[] = {
 	0x8a88aa80,
 	0x8aaaaa8a,
 	0x8a8a8aa8,
@@ -6057,7 +6058,7 @@ const u32 tmap_tbl_rev3[] = {
 	0x00000000,
 };
 
-const u32 intlv_tbl_rev3[] = {
+static const u32 intlv_tbl_rev3[] = {
 	0x00802070,
 	0x0671188d,
 	0x0a60192c,
@@ -6067,7 +6068,7 @@ const u32 intlv_tbl_rev3[] = {
 	0x00000070,
 };
 
-const u32 tdtrn_tbl_rev3[] = {
+static const u32 tdtrn_tbl_rev3[] = {
 	0x061c061c,
 	0x0050ee68,
 	0xf592fe36,
@@ -7033,7 +7034,7 @@ const u32 noise_var_tbl_rev3[] = {
 	0x0000014d,
 };
 
-const u16 mcs_tbl_rev3[] = {
+static const u16 mcs_tbl_rev3[] = {
 	0x0000,
 	0x0008,
 	0x000a,
@@ -7164,7 +7165,7 @@ const u16 mcs_tbl_rev3[] = {
 	0x0007,
 };
 
-const u32 tdi_tbl20_ant0_rev3[] = {
+static const u32 tdi_tbl20_ant0_rev3[] = {
 	0x00091226,
 	0x000a1429,
 	0x000b56ad,
@@ -7222,7 +7223,7 @@ const u32 tdi_tbl20_ant0_rev3[] = {
 	0x00000000,
 };
 
-const u32 tdi_tbl20_ant1_rev3[] = {
+static const u32 tdi_tbl20_ant1_rev3[] = {
 	0x00014b26,
 	0x00028d29,
 	0x000393ad,
@@ -7280,7 +7281,7 @@ const u32 tdi_tbl20_ant1_rev3[] = {
 	0x00000000,
 };
 
-const u32 tdi_tbl40_ant0_rev3[] = {
+static const u32 tdi_tbl40_ant0_rev3[] = {
 	0x0011a346,
 	0x00136ccf,
 	0x0014f5d9,
@@ -7393,7 +7394,7 @@ const u32 tdi_tbl40_ant0_rev3[] = {
 	0x00000000,
 };
 
-const u32 tdi_tbl40_ant1_rev3[] = {
+static const u32 tdi_tbl40_ant1_rev3[] = {
 	0x001edb36,
 	0x000129ca,
 	0x0002b353,
@@ -7506,7 +7507,7 @@ const u32 tdi_tbl40_ant1_rev3[] = {
 	0x00000000,
 };
 
-const u32 pltlut_tbl_rev3[] = {
+static const u32 pltlut_tbl_rev3[] = {
 	0x76540213,
 	0x62407351,
 	0x76543210,
@@ -7515,7 +7516,7 @@ const u32 pltlut_tbl_rev3[] = {
 	0x76430521,
 };
 
-const u32 chanest_tbl_rev3[] = {
+static const u32 chanest_tbl_rev3[] = {
 	0x44444444,
 	0x44444444,
 	0x44444444,
@@ -7614,7 +7615,7 @@ const u32 chanest_tbl_rev3[] = {
 	0x10101010,
 };
 
-const u8 frame_lut_rev3[] = {
+static const u8 frame_lut_rev3[] = {
 	0x02,
 	0x04,
 	0x14,
@@ -7649,7 +7650,7 @@ const u8 frame_lut_rev3[] = {
 	0x2a,
 };
 
-const u8 est_pwr_lut_core0_rev3[] = {
+static const u8 est_pwr_lut_core0_rev3[] = {
 	0x55,
 	0x54,
 	0x54,
@@ -7716,7 +7717,7 @@ const u8 est_pwr_lut_core0_rev3[] = {
 	0xfd,
 };
 
-const u8 est_pwr_lut_core1_rev3[] = {
+static const u8 est_pwr_lut_core1_rev3[] = {
 	0x55,
 	0x54,
 	0x54,
@@ -7783,7 +7784,7 @@ const u8 est_pwr_lut_core1_rev3[] = {
 	0xfd,
 };
 
-const u8 adj_pwr_lut_core0_rev3[] = {
+static const u8 adj_pwr_lut_core0_rev3[] = {
 	0x00,
 	0x00,
 	0x00,
@@ -7914,7 +7915,7 @@ const u8 adj_pwr_lut_core0_rev3[] = {
 	0x00,
 };
 
-const u8 adj_pwr_lut_core1_rev3[] = {
+static const u8 adj_pwr_lut_core1_rev3[] = {
 	0x00,
 	0x00,
 	0x00,
@@ -8045,7 +8046,7 @@ const u8 adj_pwr_lut_core1_rev3[] = {
 	0x00,
 };
 
-const u32 gainctrl_lut_core0_rev3[] = {
+static const u32 gainctrl_lut_core0_rev3[] = {
 	0x5bf70044,
 	0x5bf70042,
 	0x5bf70040,
@@ -8176,7 +8177,7 @@ const u32 gainctrl_lut_core0_rev3[] = {
 	0x5b07001c,
 };
 
-const u32 gainctrl_lut_core1_rev3[] = {
+static const u32 gainctrl_lut_core1_rev3[] = {
 	0x5bf70044,
 	0x5bf70042,
 	0x5bf70040,
@@ -8307,7 +8308,7 @@ const u32 gainctrl_lut_core1_rev3[] = {
 	0x5b07001c,
 };
 
-const u32 iq_lut_core0_rev3[] = {
+static const u32 iq_lut_core0_rev3[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -8438,7 +8439,7 @@ const u32 iq_lut_core0_rev3[] = {
 	0x00000000,
 };
 
-const u32 iq_lut_core1_rev3[] = {
+static const u32 iq_lut_core1_rev3[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -8569,7 +8570,7 @@ const u32 iq_lut_core1_rev3[] = {
 	0x00000000,
 };
 
-const u16 loft_lut_core0_rev3[] = {
+static const u16 loft_lut_core0_rev3[] = {
 	0x0000,
 	0x0000,
 	0x0000,
@@ -8700,7 +8701,7 @@ const u16 loft_lut_core0_rev3[] = {
 	0x0000,
 };
 
-const u16 loft_lut_core1_rev3[] = {
+static const u16 loft_lut_core1_rev3[] = {
 	0x0000,
 	0x0000,
 	0x0000,
@@ -8831,7 +8832,7 @@ const u16 loft_lut_core1_rev3[] = {
 	0x0000,
 };
 
-const u16 papd_comp_rfpwr_tbl_core0_rev3[] = {
+static const u16 papd_comp_rfpwr_tbl_core0_rev3[] = {
 	0x0036,
 	0x0036,
 	0x0036,
@@ -8962,7 +8963,7 @@ const u16 papd_comp_rfpwr_tbl_core0_rev3[] = {
 	0x01d6,
 };
 
-const u16 papd_comp_rfpwr_tbl_core1_rev3[] = {
+static const u16 papd_comp_rfpwr_tbl_core1_rev3[] = {
 	0x0036,
 	0x0036,
 	0x0036,
@@ -9093,7 +9094,7 @@ const u16 papd_comp_rfpwr_tbl_core1_rev3[] = {
 	0x01d6,
 };
 
-const u32 papd_comp_epsilon_tbl_core0_rev3[] = {
+static const u32 papd_comp_epsilon_tbl_core0_rev3[] = {
 	0x00000000,
 	0x00001fa0,
 	0x00019f78,
@@ -9160,7 +9161,7 @@ const u32 papd_comp_epsilon_tbl_core0_rev3[] = {
 	0x03e38ffe,
 };
 
-const u32 papd_cal_scalars_tbl_core0_rev3[] = {
+static const u32 papd_cal_scalars_tbl_core0_rev3[] = {
 	0x05af005a,
 	0x0571005e,
 	0x05040066,
@@ -9227,7 +9228,7 @@ const u32 papd_cal_scalars_tbl_core0_rev3[] = {
 	0x002606a4,
 };
 
-const u32 papd_comp_epsilon_tbl_core1_rev3[] = {
+static const u32 papd_comp_epsilon_tbl_core1_rev3[] = {
 	0x00000000,
 	0x00001fa0,
 	0x00019f78,
@@ -9294,7 +9295,7 @@ const u32 papd_comp_epsilon_tbl_core1_rev3[] = {
 	0x03e38ffe,
 };
 
-const u32 papd_cal_scalars_tbl_core1_rev3[] = {
+static const u32 papd_cal_scalars_tbl_core1_rev3[] = {
 	0x05af005a,
 	0x0571005e,
 	0x05040066,
@@ -9489,7 +9490,7 @@ const u32 mimophytbl_info_sz_rev3_volatile3 =
 	sizeof(mimophytbl_info_rev3_volatile3) /
 	sizeof(mimophytbl_info_rev3_volatile3[0]);
 
-const u32 tmap_tbl_rev7[] = {
+static const u32 tmap_tbl_rev7[] = {
 	0x8a88aa80,
 	0x8aaaaa8a,
 	0x8a8a8aa8,
@@ -10199,7 +10200,7 @@ const u32 noise_var_tbl_rev7[] = {
 	0x0000014d,
 };
 
-const u32 papd_comp_epsilon_tbl_core0_rev7[] = {
+static const u32 papd_comp_epsilon_tbl_core0_rev7[] = {
 	0x00000000,
 	0x00000000,
 	0x00016023,
@@ -10266,7 +10267,7 @@ const u32 papd_comp_epsilon_tbl_core0_rev7[] = {
 	0x0156cfff,
 };
 
-const u32 papd_cal_scalars_tbl_core0_rev7[] = {
+static const u32 papd_cal_scalars_tbl_core0_rev7[] = {
 	0x0b5e002d,
 	0x0ae2002f,
 	0x0a3b0032,
@@ -10333,7 +10334,7 @@ const u32 papd_cal_scalars_tbl_core0_rev7[] = {
 	0x004e068c,
 };
 
-const u32 papd_comp_epsilon_tbl_core1_rev7[] = {
+static const u32 papd_comp_epsilon_tbl_core1_rev7[] = {
 	0x00000000,
 	0x00000000,
 	0x00016023,
@@ -10400,7 +10401,7 @@ const u32 papd_comp_epsilon_tbl_core1_rev7[] = {
 	0x0156cfff,
 };
 
-const u32 papd_cal_scalars_tbl_core1_rev7[] = {
+static const u32 papd_cal_scalars_tbl_core1_rev7[] = {
 	0x0b5e002d,
 	0x0ae2002f,
 	0x0a3b0032,
diff --git a/drivers/staging/brcm80211/brcmsmac/phy/phytbl_n.h b/drivers/staging/brcm80211/brcmsmac/phy/phytbl_n.h
index c5266cf23725..dc8a84e85117 100644
--- a/drivers/staging/brcm80211/brcmsmac/phy/phytbl_n.h
+++ b/drivers/staging/brcm80211/brcmsmac/phy/phytbl_n.h
@@ -20,21 +20,31 @@
 #include "phy_int.h"
 
 extern const struct phytbl_info mimophytbl_info_rev0[],
-	mimophytbl_info_rev0_volatile[];
-extern const u32 mimophytbl_info_sz_rev0, mimophytbl_info_sz_rev0_volatile;
+				mimophytbl_info_rev0_volatile[];
+
+extern const u32 mimophytbl_info_sz_rev0,
+		 mimophytbl_info_sz_rev0_volatile;
 
 extern const struct phytbl_info mimophytbl_info_rev3[],
-	mimophytbl_info_rev3_volatile[], mimophytbl_info_rev3_volatile1[],
-	mimophytbl_info_rev3_volatile2[], mimophytbl_info_rev3_volatile3[];
-extern const u32 mimophytbl_info_sz_rev3, mimophytbl_info_sz_rev3_volatile,
-	mimophytbl_info_sz_rev3_volatile1, mimophytbl_info_sz_rev3_volatile2,
-	mimophytbl_info_sz_rev3_volatile3;
+				mimophytbl_info_rev3_volatile[],
+				mimophytbl_info_rev3_volatile1[],
+				mimophytbl_info_rev3_volatile2[],
+				mimophytbl_info_rev3_volatile3[];
+
+extern const u32 mimophytbl_info_sz_rev3,
+		 mimophytbl_info_sz_rev3_volatile,
+		 mimophytbl_info_sz_rev3_volatile1,
+		 mimophytbl_info_sz_rev3_volatile2,
+		 mimophytbl_info_sz_rev3_volatile3;
 
 extern const u32 noise_var_tbl_rev3[];
 
 extern const struct phytbl_info mimophytbl_info_rev7[];
+
 extern const u32 mimophytbl_info_sz_rev7;
+
 extern const u32 noise_var_tbl_rev7[];
 
 extern const struct phytbl_info mimophytbl_info_rev16[];
+
 extern const u32 mimophytbl_info_sz_rev16;
diff --git a/drivers/staging/brcm80211/brcmsmac/phy_shim.c b/drivers/staging/brcm80211/brcmsmac/phy_shim.c
index 82ecdcda271f..5926854f62e2 100644
--- a/drivers/staging/brcm80211/brcmsmac/phy_shim.c
+++ b/drivers/staging/brcm80211/brcmsmac/phy_shim.c
@@ -15,15 +15,15 @@
  */
 
 /*
- * This is "two-way" interface, acting as the SHIM layer between WL and PHY layer.
- *   WL driver can optinally call this translation layer to do some preprocessing, then reach PHY.
- *   On the PHY->WL driver direction, all calls go through this layer since PHY doesn't have the
- *   access to wlc_hw pointer.
+ * This is "two-way" interface, acting as the SHIM layer between driver
+ * and PHY layer. The driver can optionally call this translation layer
+ * to do some preprocessing, then reach PHY. On the PHY->driver direction,
+ * all calls go through this layer since PHY doesn't have access to the
+ * driver's brcms_hardware pointer.
  */
 #include <linux/slab.h>
 #include <net/mac80211.h>
 
-#include "bmac.h"
 #include "main.h"
 #include "mac80211_if.h"
 #include "phy_shim.h"
@@ -31,21 +31,19 @@
 /* PHY SHIM module specific state */
 struct phy_shim_info {
 	struct brcms_hardware *wlc_hw;	/* pointer to main wlc_hw structure */
-	void *wlc;		/* pointer to main wlc structure */
-	void *wl;		/* pointer to os-specific private state */
+	struct brcms_c_info *wlc;	/* pointer to main wlc structure */
+	struct brcms_info *wl; /* pointer to os-specific private state */
 };
 
 struct phy_shim_info *wlc_phy_shim_attach(struct brcms_hardware *wlc_hw,
-						       void *wl, void *wlc) {
+					  struct brcms_info *wl,
+					  struct brcms_c_info *wlc) {
 	struct phy_shim_info *physhim = NULL;
 
 	physhim = kzalloc(sizeof(struct phy_shim_info), GFP_ATOMIC);
-	if (!physhim) {
-		wiphy_err(wlc_hw->wlc->wiphy,
-			  "wl%d: wlc_phy_shim_attach: out of mem\n",
-			  wlc_hw->unit);
+	if (!physhim)
 		return NULL;
-	}
+
 	physhim->wlc_hw = wlc_hw;
 	physhim->wlc = wlc;
 	physhim->wl = wl;
@@ -59,28 +57,28 @@ void wlc_phy_shim_detach(struct phy_shim_info *physhim)
 }
 
 struct wlapi_timer *wlapi_init_timer(struct phy_shim_info *physhim,
-				     void (*fn) (void *arg), void *arg,
-				     const char *name)
+				     void (*fn)(struct brcms_phy *pi),
+				     void *arg, const char *name)
 {
 	return (struct wlapi_timer *)
-			brcms_init_timer(physhim->wl, fn, arg, name);
+			brcms_init_timer(physhim->wl, (void (*)(void *))fn,
+					 arg, name);
 }
 
-void wlapi_free_timer(struct phy_shim_info *physhim, struct wlapi_timer *t)
+void wlapi_free_timer(struct wlapi_timer *t)
 {
-	brcms_free_timer(physhim->wl, (struct brcms_timer *)t);
+	brcms_free_timer((struct brcms_timer *)t);
 }
 
 void
-wlapi_add_timer(struct phy_shim_info *physhim, struct wlapi_timer *t, uint ms,
-		int periodic)
+wlapi_add_timer(struct wlapi_timer *t, uint ms, int periodic)
 {
-	brcms_add_timer(physhim->wl, (struct brcms_timer *)t, ms, periodic);
+	brcms_add_timer((struct brcms_timer *)t, ms, periodic);
 }
 
-bool wlapi_del_timer(struct phy_shim_info *physhim, struct wlapi_timer *t)
+bool wlapi_del_timer(struct wlapi_timer *t)
 {
-	return brcms_del_timer(physhim->wl, (struct brcms_timer *)t);
+	return brcms_del_timer((struct brcms_timer *)t);
 }
 
 void wlapi_intrson(struct phy_shim_info *physhim)
@@ -216,3 +214,12 @@ wlapi_copyto_objmem(struct phy_shim_info *physhim, uint offset, const void *buf,
 {
 	brcms_b_copyto_objmem(physhim->wlc_hw, offset, buf, l, sel);
 }
+
+char *wlapi_getvar(struct phy_shim_info *physhim, enum brcms_srom_id id)
+{
+	return getvar(physhim->wlc_hw->sih, id);
+}
+int wlapi_getintvar(struct phy_shim_info *physhim, enum brcms_srom_id id)
+{
+	return getintvar(physhim->wlc_hw->sih, id);
+}
diff --git a/drivers/staging/brcm80211/brcmsmac/phy_shim.h b/drivers/staging/brcm80211/brcmsmac/phy_shim.h
index 2d12bb4400f1..9168c459b185 100644
--- a/drivers/staging/brcm80211/brcmsmac/phy_shim.h
+++ b/drivers/staging/brcm80211/brcmsmac/phy_shim.h
@@ -45,17 +45,17 @@
 #define FRA_ERR_20MHZ	60
 #define FRA_ERR_40MHZ	120
 
-#define ANTSEL_NA		0	/* No boardlevel selection available */
-#define ANTSEL_2x4		1	/* 2x4 boardlevel selection available */
-#define ANTSEL_2x3		2	/* 2x3 CB2 boardlevel selection available */
+#define ANTSEL_NA		0 /* No boardlevel selection available */
+#define ANTSEL_2x4		1 /* 2x4 boardlevel selection available */
+#define ANTSEL_2x3		2 /* 2x3 CB2 boardlevel selection available */
 
 /* Rx Antenna diversity control values */
-#define	ANT_RX_DIV_FORCE_0		0	/* Use antenna 0 */
-#define	ANT_RX_DIV_FORCE_1		1	/* Use antenna 1 */
-#define	ANT_RX_DIV_START_1		2	/* Choose starting with 1 */
-#define	ANT_RX_DIV_START_0		3	/* Choose starting with 0 */
-#define	ANT_RX_DIV_ENABLE		3	/* APHY bbConfig Enable RX Diversity */
-#define ANT_RX_DIV_DEF		ANT_RX_DIV_START_0	/* default antdiv setting */
+#define	ANT_RX_DIV_FORCE_0	0	/* Use antenna 0 */
+#define	ANT_RX_DIV_FORCE_1	1	/* Use antenna 1 */
+#define	ANT_RX_DIV_START_1	2	/* Choose starting with 1 */
+#define	ANT_RX_DIV_START_0	3	/* Choose starting with 0 */
+#define	ANT_RX_DIV_ENABLE	3	/* APHY bbConfig Enable RX Diversity */
+#define ANT_RX_DIV_DEF		ANT_RX_DIV_START_0 /* default antdiv setting */
 
 #define WL_ANT_RX_MAX		2	/* max 2 receive antennas */
 #define WL_ANT_HT_RX_MAX	3	/* max 3 receive antennas/cores */
@@ -77,26 +77,40 @@
 #define WL_TX_POWER_RATES	       101
 #define WL_TX_POWER_CCK_FIRST	       0
 #define WL_TX_POWER_CCK_NUM	       4
-#define WL_TX_POWER_OFDM_FIRST	       4	/* Index for first 20MHz OFDM SISO rate */
-#define WL_TX_POWER_OFDM20_CDD_FIRST   12	/* Index for first 20MHz OFDM CDD rate */
-#define WL_TX_POWER_OFDM40_SISO_FIRST  52	/* Index for first 40MHz OFDM SISO rate */
-#define WL_TX_POWER_OFDM40_CDD_FIRST   60	/* Index for first 40MHz OFDM CDD rate */
+/* Index for first 20MHz OFDM SISO rate */
+#define WL_TX_POWER_OFDM_FIRST	       4
+/* Index for first 20MHz OFDM CDD rate */
+#define WL_TX_POWER_OFDM20_CDD_FIRST   12
+/* Index for first 40MHz OFDM SISO rate */
+#define WL_TX_POWER_OFDM40_SISO_FIRST  52
+/* Index for first 40MHz OFDM CDD rate */
+#define WL_TX_POWER_OFDM40_CDD_FIRST   60
 #define WL_TX_POWER_OFDM_NUM	       8
-#define WL_TX_POWER_MCS20_SISO_FIRST   20	/* Index for first 20MHz MCS SISO rate */
-#define WL_TX_POWER_MCS20_CDD_FIRST    28	/* Index for first 20MHz MCS CDD rate */
-#define WL_TX_POWER_MCS20_STBC_FIRST   36	/* Index for first 20MHz MCS STBC rate */
-#define WL_TX_POWER_MCS20_SDM_FIRST    44	/* Index for first 20MHz MCS SDM rate */
-#define WL_TX_POWER_MCS40_SISO_FIRST   68	/* Index for first 40MHz MCS SISO rate */
-#define WL_TX_POWER_MCS40_CDD_FIRST    76	/* Index for first 40MHz MCS CDD rate */
-#define WL_TX_POWER_MCS40_STBC_FIRST   84	/* Index for first 40MHz MCS STBC rate */
-#define WL_TX_POWER_MCS40_SDM_FIRST    92	/* Index for first 40MHz MCS SDM rate */
+/* Index for first 20MHz MCS SISO rate */
+#define WL_TX_POWER_MCS20_SISO_FIRST   20
+/* Index for first 20MHz MCS CDD rate */
+#define WL_TX_POWER_MCS20_CDD_FIRST    28
+/* Index for first 20MHz MCS STBC rate */
+#define WL_TX_POWER_MCS20_STBC_FIRST   36
+/* Index for first 20MHz MCS SDM rate */
+#define WL_TX_POWER_MCS20_SDM_FIRST    44
+/* Index for first 40MHz MCS SISO rate */
+#define WL_TX_POWER_MCS40_SISO_FIRST   68
+/* Index for first 40MHz MCS CDD rate */
+#define WL_TX_POWER_MCS40_CDD_FIRST    76
+/* Index for first 40MHz MCS STBC rate */
+#define WL_TX_POWER_MCS40_STBC_FIRST   84
+/* Index for first 40MHz MCS SDM rate */
+#define WL_TX_POWER_MCS40_SDM_FIRST    92
 #define WL_TX_POWER_MCS_1_STREAM_NUM   8
 #define WL_TX_POWER_MCS_2_STREAM_NUM   8
-#define WL_TX_POWER_MCS_32	       100	/* Index for 40MHz rate MCS 32 */
+/* Index for 40MHz rate MCS 32 */
+#define WL_TX_POWER_MCS_32	       100
 #define WL_TX_POWER_MCS_32_NUM	       1
 
 /* sslpnphy specifics */
-#define WL_TX_POWER_MCS20_SISO_FIRST_SSN   12	/* Index for first 20MHz MCS SISO rate */
+/* Index for first 20MHz MCS SISO rate */
+#define WL_TX_POWER_MCS20_SISO_FIRST_SSN   12
 
 /* struct tx_power::flags bits */
 #define WL_TX_POWER_F_ENABLED	1
@@ -108,20 +122,20 @@
 #define BRCMS_N_TXRX_CHAIN0		0
 #define BRCMS_N_TXRX_CHAIN1		1
 
+struct brcms_phy;
+
 extern struct phy_shim_info *wlc_phy_shim_attach(struct brcms_hardware *wlc_hw,
-						void *wl, void *wlc);
+						 struct brcms_info *wl,
+						 struct brcms_c_info *wlc);
 extern void wlc_phy_shim_detach(struct phy_shim_info *physhim);
 
 /* PHY to WL utility functions */
 extern struct wlapi_timer *wlapi_init_timer(struct phy_shim_info *physhim,
-					    void (*fn) (void *arg), void *arg,
-					    const char *name);
-extern void wlapi_free_timer(struct phy_shim_info *physhim,
-			     struct wlapi_timer *t);
-extern void wlapi_add_timer(struct phy_shim_info *physhim,
-			    struct wlapi_timer *t, uint ms, int periodic);
-extern bool wlapi_del_timer(struct phy_shim_info *physhim,
-			    struct wlapi_timer *t);
+					    void (*fn) (struct brcms_phy *pi),
+					    void *arg, const char *name);
+extern void wlapi_free_timer(struct wlapi_timer *t);
+extern void wlapi_add_timer(struct wlapi_timer *t, uint ms, int periodic);
+extern bool wlapi_del_timer(struct wlapi_timer *t);
 extern void wlapi_intrson(struct phy_shim_info *physhim);
 extern u32 wlapi_intrsoff(struct phy_shim_info *physhim);
 extern void wlapi_intrsrestore(struct phy_shim_info *physhim,
@@ -161,4 +175,8 @@ extern void wlapi_copyto_objmem(struct phy_shim_info *physhim, uint,
 extern void wlapi_high_update_phy_mode(struct phy_shim_info *physhim,
 				       u32 phy_mode);
 extern u16 wlapi_bmac_get_txant(struct phy_shim_info *physhim);
+extern char *wlapi_getvar(struct phy_shim_info *physhim, enum brcms_srom_id id);
+extern int wlapi_getintvar(struct phy_shim_info *physhim,
+			   enum brcms_srom_id id);
+
 #endif				/* _BRCM_PHY_SHIM_H_ */
diff --git a/drivers/staging/brcm80211/brcmsmac/pmu.c b/drivers/staging/brcm80211/brcmsmac/pmu.c
index e8b2b81d2d04..3b36e3acfd74 100644
--- a/drivers/staging/brcm80211/brcmsmac/pmu.c
+++ b/drivers/staging/brcm80211/brcmsmac/pmu.c
@@ -70,11 +70,13 @@
 #define PMURES_BIT(bit)	(1 << (bit))
 
 /* PMU corerev and chip specific PLL controls.
- * PMU<rev>_PLL<num>_XX where <rev> is PMU corerev and <num> is an arbitrary number
- * to differentiate different PLLs controlled by the same PMU rev.
+ * PMU<rev>_PLL<num>_XX where <rev> is PMU corerev and <num> is an arbitrary
+ * number to differentiate different PLLs controlled by the same PMU rev.
+ */
+/* pllcontrol registers:
+ * ndiv_pwrdn, pwrdn_ch<x>, refcomp_pwrdn, dly_ch<x>,
+ * p1div, p2div, _bypass_sdmod
  */
-/* pllcontrol registers */
-/* ndiv_pwrdn, pwrdn_ch<x>, refcomp_pwrdn, dly_ch<x>, p1div, p2div, _bypass_sdmod */
 #define PMU1_PLL0_PLLCTL0		0
 #define PMU1_PLL0_PLLCTL1		1
 #define PMU1_PLL0_PLLCTL2		2
@@ -137,7 +139,8 @@ static void si_pmu_res_masks(struct si_pub *sih, u32 * pmin, u32 * pmax)
 }
 
 static void
-si_pmu_spuravoid_pllupdate(struct si_pub *sih, chipcregs_t *cc, u8 spuravoid)
+si_pmu_spuravoid_pllupdate(struct si_pub *sih, struct chipcregs __iomem *cc,
+			   u8 spuravoid)
 {
 	u32 tmp = 0;
 
@@ -199,28 +202,6 @@ si_pmu_spuravoid_pllupdate(struct si_pub *sih, chipcregs_t *cc, u8 spuravoid)
 	W_REG(&cc->pmucontrol, tmp);
 }
 
-u32 si_pmu_ilp_clock(struct si_pub *sih)
-{
-	static u32 ilpcycles_per_sec;
-
-	if (!PMUCTL_ENAB(sih))
-		return ILP_CLOCK;
-
-	if (ilpcycles_per_sec == 0) {
-		u32 start, end, delta;
-		u32 origidx = ai_coreidx(sih);
-		chipcregs_t *cc = ai_setcoreidx(sih, SI_CC_IDX);
-		start = R_REG(&cc->pmutimer);
-		mdelay(ILP_CALC_DUR);
-		end = R_REG(&cc->pmutimer);
-		delta = end - start;
-		ilpcycles_per_sec = delta * (1000 / ILP_CALC_DUR);
-		ai_setcoreidx(sih, origidx);
-	}
-
-	return ilpcycles_per_sec;
-}
-
 u16 si_pmu_fast_pwrup_delay(struct si_pub *sih)
 {
 	uint delay = PMU_MAX_TRANSITION_DLY;
@@ -240,7 +221,7 @@ u16 si_pmu_fast_pwrup_delay(struct si_pub *sih)
 
 void si_pmu_sprom_enable(struct si_pub *sih, bool enable)
 {
-	chipcregs_t *cc;
+	struct chipcregs __iomem *cc;
 	uint origidx;
 
 	/* Remember original core before switch to chipc */
@@ -254,34 +235,37 @@ void si_pmu_sprom_enable(struct si_pub *sih, bool enable)
 /* Read/write a chipcontrol reg */
 u32 si_pmu_chipcontrol(struct si_pub *sih, uint reg, u32 mask, u32 val)
 {
-	ai_corereg(sih, SI_CC_IDX, offsetof(chipcregs_t, chipcontrol_addr), ~0,
-		   reg);
+	ai_corereg(sih, SI_CC_IDX, offsetof(struct chipcregs, chipcontrol_addr),
+		   ~0, reg);
 	return ai_corereg(sih, SI_CC_IDX,
-			  offsetof(chipcregs_t, chipcontrol_data), mask, val);
+			  offsetof(struct chipcregs, chipcontrol_data), mask,
+			  val);
 }
 
 /* Read/write a regcontrol reg */
 u32 si_pmu_regcontrol(struct si_pub *sih, uint reg, u32 mask, u32 val)
 {
-	ai_corereg(sih, SI_CC_IDX, offsetof(chipcregs_t, regcontrol_addr), ~0,
-		   reg);
+	ai_corereg(sih, SI_CC_IDX, offsetof(struct chipcregs, regcontrol_addr),
+		   ~0, reg);
 	return ai_corereg(sih, SI_CC_IDX,
-			  offsetof(chipcregs_t, regcontrol_data), mask, val);
+			  offsetof(struct chipcregs, regcontrol_data), mask,
+			  val);
 }
 
 /* Read/write a pllcontrol reg */
 u32 si_pmu_pllcontrol(struct si_pub *sih, uint reg, u32 mask, u32 val)
 {
-	ai_corereg(sih, SI_CC_IDX, offsetof(chipcregs_t, pllcontrol_addr), ~0,
-		   reg);
+	ai_corereg(sih, SI_CC_IDX, offsetof(struct chipcregs, pllcontrol_addr),
+		   ~0, reg);
 	return ai_corereg(sih, SI_CC_IDX,
-			  offsetof(chipcregs_t, pllcontrol_data), mask, val);
+			  offsetof(struct chipcregs, pllcontrol_data), mask,
+			  val);
 }
 
 /* PMU PLL update */
 void si_pmu_pllupd(struct si_pub *sih)
 {
-	ai_corereg(sih, SI_CC_IDX, offsetof(chipcregs_t, pmucontrol),
+	ai_corereg(sih, SI_CC_IDX, offsetof(struct chipcregs, pmucontrol),
 		   PCTL_PLL_PLLCTL_UPD, PCTL_PLL_PLLCTL_UPD);
 }
 
@@ -291,7 +275,7 @@ u32 si_pmu_alp_clock(struct si_pub *sih)
 	u32 clock = ALP_CLOCK;
 
 	/* bail out with default */
-	if (!PMUCTL_ENAB(sih))
+	if (!(sih->cccaps & CC_CAP_PMU))
 		return clock;
 
 	switch (sih->chip) {
@@ -310,12 +294,12 @@ u32 si_pmu_alp_clock(struct si_pub *sih)
 
 void si_pmu_spuravoid(struct si_pub *sih, u8 spuravoid)
 {
-	chipcregs_t *cc;
+	struct chipcregs __iomem *cc;
 	uint origidx, intr_val;
 
 	/* Remember original core before switch to chipc */
-	cc = (chipcregs_t *) ai_switch_core(sih, CC_CORE_ID, &origidx,
-					    &intr_val);
+	cc = (struct chipcregs __iomem *)
+			ai_switch_core(sih, CC_CORE_ID, &origidx, &intr_val);
 
 	/* update the pll changes */
 	si_pmu_spuravoid_pllupdate(sih, cc, spuravoid);
@@ -327,7 +311,7 @@ void si_pmu_spuravoid(struct si_pub *sih, u8 spuravoid)
 /* initialize PMU */
 void si_pmu_init(struct si_pub *sih)
 {
-	chipcregs_t *cc;
+	struct chipcregs __iomem *cc;
 	uint origidx;
 
 	/* Remember original core before switch to chipc */
@@ -366,7 +350,7 @@ void si_pmu_swreg_init(struct si_pub *sih)
 /* initialize PLL */
 void si_pmu_pll_init(struct si_pub *sih, uint xtalfreq)
 {
-	chipcregs_t *cc;
+	struct chipcregs __iomem *cc;
 	uint origidx;
 
 	/* Remember original core before switch to chipc */
@@ -390,7 +374,7 @@ void si_pmu_pll_init(struct si_pub *sih, uint xtalfreq)
 /* initialize PMU resources */
 void si_pmu_res_init(struct si_pub *sih)
 {
-	chipcregs_t *cc;
+	struct chipcregs __iomem *cc;
 	uint origidx;
 	u32 min_mask = 0, max_mask = 0;
 
@@ -422,7 +406,7 @@ void si_pmu_res_init(struct si_pub *sih)
 
 u32 si_pmu_measure_alpclk(struct si_pub *sih)
 {
-	chipcregs_t *cc;
+	struct chipcregs __iomem *cc;
 	uint origidx;
 	u32 alp_khz;
 
diff --git a/drivers/staging/brcm80211/brcmsmac/pmu.h b/drivers/staging/brcm80211/brcmsmac/pmu.h
index 0c7e48c4bcd8..3a08c620640e 100644
--- a/drivers/staging/brcm80211/brcmsmac/pmu.h
+++ b/drivers/staging/brcm80211/brcmsmac/pmu.h
@@ -19,25 +19,11 @@
 #define _BRCM_PMU_H_
 
 #include "types.h"
-/*
- * LDO selections used in si_pmu_set_ldo_voltage
- */
-#define SET_LDO_VOLTAGE_LDO1	1
-#define SET_LDO_VOLTAGE_LDO2	2
-#define SET_LDO_VOLTAGE_LDO3	3
-#define SET_LDO_VOLTAGE_PAREF	4
-#define SET_LDO_VOLTAGE_CLDO_PWM	5
-#define SET_LDO_VOLTAGE_CLDO_BURST	6
-#define SET_LDO_VOLTAGE_CBUCK_PWM	7
-#define SET_LDO_VOLTAGE_CBUCK_BURST	8
-#define SET_LDO_VOLTAGE_LNLDO1	9
-#define SET_LDO_VOLTAGE_LNLDO2_SEL	10
 
 extern u16 si_pmu_fast_pwrup_delay(struct si_pub *sih);
 extern void si_pmu_sprom_enable(struct si_pub *sih, bool enable);
 extern u32 si_pmu_chipcontrol(struct si_pub *sih, uint reg, u32 mask, u32 val);
 extern u32 si_pmu_regcontrol(struct si_pub *sih, uint reg, u32 mask, u32 val);
-extern u32 si_pmu_ilp_clock(struct si_pub *sih);
 extern u32 si_pmu_alp_clock(struct si_pub *sih);
 extern void si_pmu_pllupd(struct si_pub *sih);
 extern void si_pmu_spuravoid(struct si_pub *sih, u8 spuravoid);
diff --git a/drivers/staging/brcm80211/brcmsmac/pub.h b/drivers/staging/brcm80211/brcmsmac/pub.h
index 01d74609560f..37bb2dcc113f 100644
--- a/drivers/staging/brcm80211/brcmsmac/pub.h
+++ b/drivers/staging/brcm80211/brcmsmac/pub.h
@@ -21,8 +21,265 @@
 #include "types.h"
 #include "defs.h"
 
+enum brcms_srom_id {
+	BRCMS_SROM_NULL,
+	BRCMS_SROM_CONT,
+	BRCMS_SROM_AA2G,
+	BRCMS_SROM_AA5G,
+	BRCMS_SROM_AG0,
+	BRCMS_SROM_AG1,
+	BRCMS_SROM_AG2,
+	BRCMS_SROM_AG3,
+	BRCMS_SROM_ANTSWCTL2G,
+	BRCMS_SROM_ANTSWCTL5G,
+	BRCMS_SROM_ANTSWITCH,
+	BRCMS_SROM_BOARDFLAGS2,
+	BRCMS_SROM_BOARDFLAGS,
+	BRCMS_SROM_BOARDNUM,
+	BRCMS_SROM_BOARDREV,
+	BRCMS_SROM_BOARDTYPE,
+	BRCMS_SROM_BW40PO,
+	BRCMS_SROM_BWDUPPO,
+	BRCMS_SROM_BXA2G,
+	BRCMS_SROM_BXA5G,
+	BRCMS_SROM_CC,
+	BRCMS_SROM_CCK2GPO,
+	BRCMS_SROM_CCKBW202GPO,
+	BRCMS_SROM_CCKBW20UL2GPO,
+	BRCMS_SROM_CCODE,
+	BRCMS_SROM_CDDPO,
+	BRCMS_SROM_DEVID,
+	BRCMS_SROM_ET1MACADDR,
+	BRCMS_SROM_EXTPAGAIN2G,
+	BRCMS_SROM_EXTPAGAIN5G,
+	BRCMS_SROM_FREQOFFSET_CORR,
+	BRCMS_SROM_HW_IQCAL_EN,
+	BRCMS_SROM_IL0MACADDR,
+	BRCMS_SROM_IQCAL_SWP_DIS,
+	BRCMS_SROM_LEDBH0,
+	BRCMS_SROM_LEDBH1,
+	BRCMS_SROM_LEDBH2,
+	BRCMS_SROM_LEDBH3,
+	BRCMS_SROM_LEDDC,
+	BRCMS_SROM_LEGOFDM40DUPPO,
+	BRCMS_SROM_LEGOFDMBW202GPO,
+	BRCMS_SROM_LEGOFDMBW205GHPO,
+	BRCMS_SROM_LEGOFDMBW205GLPO,
+	BRCMS_SROM_LEGOFDMBW205GMPO,
+	BRCMS_SROM_LEGOFDMBW20UL2GPO,
+	BRCMS_SROM_LEGOFDMBW20UL5GHPO,
+	BRCMS_SROM_LEGOFDMBW20UL5GLPO,
+	BRCMS_SROM_LEGOFDMBW20UL5GMPO,
+	BRCMS_SROM_MACADDR,
+	BRCMS_SROM_MCS2GPO0,
+	BRCMS_SROM_MCS2GPO1,
+	BRCMS_SROM_MCS2GPO2,
+	BRCMS_SROM_MCS2GPO3,
+	BRCMS_SROM_MCS2GPO4,
+	BRCMS_SROM_MCS2GPO5,
+	BRCMS_SROM_MCS2GPO6,
+	BRCMS_SROM_MCS2GPO7,
+	BRCMS_SROM_MCS32PO,
+	BRCMS_SROM_MCS5GHPO0,
+	BRCMS_SROM_MCS5GHPO1,
+	BRCMS_SROM_MCS5GHPO2,
+	BRCMS_SROM_MCS5GHPO3,
+	BRCMS_SROM_MCS5GHPO4,
+	BRCMS_SROM_MCS5GHPO5,
+	BRCMS_SROM_MCS5GHPO6,
+	BRCMS_SROM_MCS5GHPO7,
+	BRCMS_SROM_MCS5GLPO0,
+	BRCMS_SROM_MCS5GLPO1,
+	BRCMS_SROM_MCS5GLPO2,
+	BRCMS_SROM_MCS5GLPO3,
+	BRCMS_SROM_MCS5GLPO4,
+	BRCMS_SROM_MCS5GLPO5,
+	BRCMS_SROM_MCS5GLPO6,
+	BRCMS_SROM_MCS5GLPO7,
+	BRCMS_SROM_MCS5GPO0,
+	BRCMS_SROM_MCS5GPO1,
+	BRCMS_SROM_MCS5GPO2,
+	BRCMS_SROM_MCS5GPO3,
+	BRCMS_SROM_MCS5GPO4,
+	BRCMS_SROM_MCS5GPO5,
+	BRCMS_SROM_MCS5GPO6,
+	BRCMS_SROM_MCS5GPO7,
+	BRCMS_SROM_MCSBW202GPO,
+	BRCMS_SROM_MCSBW205GHPO,
+	BRCMS_SROM_MCSBW205GLPO,
+	BRCMS_SROM_MCSBW205GMPO,
+	BRCMS_SROM_MCSBW20UL2GPO,
+	BRCMS_SROM_MCSBW20UL5GHPO,
+	BRCMS_SROM_MCSBW20UL5GLPO,
+	BRCMS_SROM_MCSBW20UL5GMPO,
+	BRCMS_SROM_MCSBW402GPO,
+	BRCMS_SROM_MCSBW405GHPO,
+	BRCMS_SROM_MCSBW405GLPO,
+	BRCMS_SROM_MCSBW405GMPO,
+	BRCMS_SROM_MEASPOWER,
+	BRCMS_SROM_OFDM2GPO,
+	BRCMS_SROM_OFDM5GHPO,
+	BRCMS_SROM_OFDM5GLPO,
+	BRCMS_SROM_OFDM5GPO,
+	BRCMS_SROM_OPO,
+	BRCMS_SROM_PA0B0,
+	BRCMS_SROM_PA0B1,
+	BRCMS_SROM_PA0B2,
+	BRCMS_SROM_PA0ITSSIT,
+	BRCMS_SROM_PA0MAXPWR,
+	BRCMS_SROM_PA1B0,
+	BRCMS_SROM_PA1B1,
+	BRCMS_SROM_PA1B2,
+	BRCMS_SROM_PA1HIB0,
+	BRCMS_SROM_PA1HIB1,
+	BRCMS_SROM_PA1HIB2,
+	BRCMS_SROM_PA1HIMAXPWR,
+	BRCMS_SROM_PA1ITSSIT,
+	BRCMS_SROM_PA1LOB0,
+	BRCMS_SROM_PA1LOB1,
+	BRCMS_SROM_PA1LOB2,
+	BRCMS_SROM_PA1LOMAXPWR,
+	BRCMS_SROM_PA1MAXPWR,
+	BRCMS_SROM_PDETRANGE2G,
+	BRCMS_SROM_PDETRANGE5G,
+	BRCMS_SROM_PHYCAL_TEMPDELTA,
+	BRCMS_SROM_RAWTEMPSENSE,
+	BRCMS_SROM_REGREV,
+	BRCMS_SROM_REV,
+	BRCMS_SROM_RSSISAV2G,
+	BRCMS_SROM_RSSISAV5G,
+	BRCMS_SROM_RSSISMC2G,
+	BRCMS_SROM_RSSISMC5G,
+	BRCMS_SROM_RSSISMF2G,
+	BRCMS_SROM_RSSISMF5G,
+	BRCMS_SROM_RXCHAIN,
+	BRCMS_SROM_RXPO2G,
+	BRCMS_SROM_RXPO5G,
+	BRCMS_SROM_STBCPO,
+	BRCMS_SROM_TEMPCORRX,
+	BRCMS_SROM_TEMPOFFSET,
+	BRCMS_SROM_TEMPSENSE_OPTION,
+	BRCMS_SROM_TEMPSENSE_SLOPE,
+	BRCMS_SROM_TEMPTHRESH,
+	BRCMS_SROM_TRI2G,
+	BRCMS_SROM_TRI5GH,
+	BRCMS_SROM_TRI5GL,
+	BRCMS_SROM_TRI5G,
+	BRCMS_SROM_TRISO2G,
+	BRCMS_SROM_TRISO5G,
+	BRCMS_SROM_TSSIPOS2G,
+	BRCMS_SROM_TSSIPOS5G,
+	BRCMS_SROM_TXCHAIN,
+	BRCMS_SROM_TXPID2GA0,
+	BRCMS_SROM_TXPID2GA1,
+	BRCMS_SROM_TXPID2GA2,
+	BRCMS_SROM_TXPID2GA3,
+	BRCMS_SROM_TXPID5GA0,
+	BRCMS_SROM_TXPID5GA1,
+	BRCMS_SROM_TXPID5GA2,
+	BRCMS_SROM_TXPID5GA3,
+	BRCMS_SROM_TXPID5GHA0,
+	BRCMS_SROM_TXPID5GHA1,
+	BRCMS_SROM_TXPID5GHA2,
+	BRCMS_SROM_TXPID5GHA3,
+	BRCMS_SROM_TXPID5GLA0,
+	BRCMS_SROM_TXPID5GLA1,
+	BRCMS_SROM_TXPID5GLA2,
+	BRCMS_SROM_TXPID5GLA3,
+	/*
+	 * per-path identifiers (see srom.c)
+	 */
+	BRCMS_SROM_ITT2GA0,
+	BRCMS_SROM_ITT2GA1,
+	BRCMS_SROM_ITT2GA2,
+	BRCMS_SROM_ITT2GA3,
+	BRCMS_SROM_ITT5GA0,
+	BRCMS_SROM_ITT5GA1,
+	BRCMS_SROM_ITT5GA2,
+	BRCMS_SROM_ITT5GA3,
+	BRCMS_SROM_MAXP2GA0,
+	BRCMS_SROM_MAXP2GA1,
+	BRCMS_SROM_MAXP2GA2,
+	BRCMS_SROM_MAXP2GA3,
+	BRCMS_SROM_MAXP5GA0,
+	BRCMS_SROM_MAXP5GA1,
+	BRCMS_SROM_MAXP5GA2,
+	BRCMS_SROM_MAXP5GA3,
+	BRCMS_SROM_MAXP5GHA0,
+	BRCMS_SROM_MAXP5GHA1,
+	BRCMS_SROM_MAXP5GHA2,
+	BRCMS_SROM_MAXP5GHA3,
+	BRCMS_SROM_MAXP5GLA0,
+	BRCMS_SROM_MAXP5GLA1,
+	BRCMS_SROM_MAXP5GLA2,
+	BRCMS_SROM_MAXP5GLA3,
+	BRCMS_SROM_PA2GW0A0,
+	BRCMS_SROM_PA2GW0A1,
+	BRCMS_SROM_PA2GW0A2,
+	BRCMS_SROM_PA2GW0A3,
+	BRCMS_SROM_PA2GW1A0,
+	BRCMS_SROM_PA2GW1A1,
+	BRCMS_SROM_PA2GW1A2,
+	BRCMS_SROM_PA2GW1A3,
+	BRCMS_SROM_PA2GW2A0,
+	BRCMS_SROM_PA2GW2A1,
+	BRCMS_SROM_PA2GW2A2,
+	BRCMS_SROM_PA2GW2A3,
+	BRCMS_SROM_PA2GW3A0,
+	BRCMS_SROM_PA2GW3A1,
+	BRCMS_SROM_PA2GW3A2,
+	BRCMS_SROM_PA2GW3A3,
+	BRCMS_SROM_PA5GHW0A0,
+	BRCMS_SROM_PA5GHW0A1,
+	BRCMS_SROM_PA5GHW0A2,
+	BRCMS_SROM_PA5GHW0A3,
+	BRCMS_SROM_PA5GHW1A0,
+	BRCMS_SROM_PA5GHW1A1,
+	BRCMS_SROM_PA5GHW1A2,
+	BRCMS_SROM_PA5GHW1A3,
+	BRCMS_SROM_PA5GHW2A0,
+	BRCMS_SROM_PA5GHW2A1,
+	BRCMS_SROM_PA5GHW2A2,
+	BRCMS_SROM_PA5GHW2A3,
+	BRCMS_SROM_PA5GHW3A0,
+	BRCMS_SROM_PA5GHW3A1,
+	BRCMS_SROM_PA5GHW3A2,
+	BRCMS_SROM_PA5GHW3A3,
+	BRCMS_SROM_PA5GLW0A0,
+	BRCMS_SROM_PA5GLW0A1,
+	BRCMS_SROM_PA5GLW0A2,
+	BRCMS_SROM_PA5GLW0A3,
+	BRCMS_SROM_PA5GLW1A0,
+	BRCMS_SROM_PA5GLW1A1,
+	BRCMS_SROM_PA5GLW1A2,
+	BRCMS_SROM_PA5GLW1A3,
+	BRCMS_SROM_PA5GLW2A0,
+	BRCMS_SROM_PA5GLW2A1,
+	BRCMS_SROM_PA5GLW2A2,
+	BRCMS_SROM_PA5GLW2A3,
+	BRCMS_SROM_PA5GLW3A0,
+	BRCMS_SROM_PA5GLW3A1,
+	BRCMS_SROM_PA5GLW3A2,
+	BRCMS_SROM_PA5GLW3A3,
+	BRCMS_SROM_PA5GW0A0,
+	BRCMS_SROM_PA5GW0A1,
+	BRCMS_SROM_PA5GW0A2,
+	BRCMS_SROM_PA5GW0A3,
+	BRCMS_SROM_PA5GW1A0,
+	BRCMS_SROM_PA5GW1A1,
+	BRCMS_SROM_PA5GW1A2,
+	BRCMS_SROM_PA5GW1A3,
+	BRCMS_SROM_PA5GW2A0,
+	BRCMS_SROM_PA5GW2A1,
+	BRCMS_SROM_PA5GW2A2,
+	BRCMS_SROM_PA5GW2A3,
+	BRCMS_SROM_PA5GW3A0,
+	BRCMS_SROM_PA5GW3A1,
+	BRCMS_SROM_PA5GW3A2,
+	BRCMS_SROM_PA5GW3A3,
+};
+
 #define	BRCMS_NUMRATES	16	/* max # of rates in a rateset */
-#define	MAXMULTILIST	32	/* max # multicast addresses */
 #define	D11_PHY_HDR_LEN	6	/* Phy header length - 6 bytes */
 
 /* phy types */
@@ -40,10 +297,6 @@
 #define BRCMS_20_MHZ	20	/* 20Mhz nphy channel bandwidth */
 #define BRCMS_40_MHZ	40	/* 40Mhz nphy channel bandwidth */
 
-#define CHSPEC_WLC_BW(chanspec)	(CHSPEC_IS40(chanspec) ? BRCMS_40_MHZ : \
-				 CHSPEC_IS20(chanspec) ? BRCMS_20_MHZ : \
-							 BRCMS_10_MHZ)
-
 #define	BRCMS_RSSI_MINVAL	-200	/* Low value, e.g. for forcing roam */
 #define	BRCMS_RSSI_NO_SIGNAL	-91	/* NDIS RSSI link quality cutoffs */
 #define	BRCMS_RSSI_VERY_LOW	-80	/* Very low quality cutoffs */
@@ -52,13 +305,6 @@
 #define	BRCMS_RSSI_VERY_GOOD	-58	/* Very good quality cutoffs */
 #define	BRCMS_RSSI_EXCELLENT	-57	/* Excellent quality cutoffs */
 
-/* macro to perform PHY -> D11 PHY TYPE, currently 1:1 */
-#define BRCMS_PHYTYPE(_x) (_x)
-
-#define MA_WINDOW_SZ		8	/* moving average window size */
-
-#define BRCMS_SNR_INVALID		0	/* invalid SNR value */
-
 /* a large TX Power as an init value to factor out of min() calculations,
  * keep low enough to fit in an s8, units are .25 dBm
  */
@@ -69,63 +315,58 @@
 #define	BRCMS_RATE_MASK	0x7f	/* Rate value mask w/o basic rate flag */
 
 /* legacy rx Antenna diversity for SISO rates */
-#define	ANT_RX_DIV_FORCE_0		0	/* Use antenna 0 */
-#define	ANT_RX_DIV_FORCE_1		1	/* Use antenna 1 */
-#define	ANT_RX_DIV_START_1		2	/* Choose starting with 1 */
-#define	ANT_RX_DIV_START_0		3	/* Choose starting with 0 */
-#define	ANT_RX_DIV_ENABLE		3	/* APHY bbConfig Enable RX Diversity */
-#define ANT_RX_DIV_DEF		ANT_RX_DIV_START_0	/* default antdiv setting */
+#define	ANT_RX_DIV_FORCE_0	0	/* Use antenna 0 */
+#define	ANT_RX_DIV_FORCE_1	1	/* Use antenna 1 */
+#define	ANT_RX_DIV_START_1	2	/* Choose starting with 1 */
+#define	ANT_RX_DIV_START_0	3	/* Choose starting with 0 */
+#define	ANT_RX_DIV_ENABLE	3	/* APHY bbConfig Enable RX Diversity */
+/* default antdiv setting */
+#define ANT_RX_DIV_DEF		ANT_RX_DIV_START_0
 
 /* legacy rx Antenna diversity for SISO rates */
-#define ANT_TX_FORCE_0		0	/* Tx on antenna 0, "legacy term Main" */
-#define ANT_TX_FORCE_1		1	/* Tx on antenna 1, "legacy term Aux" */
-#define ANT_TX_LAST_RX		3	/* Tx on phy's last good Rx antenna */
-#define ANT_TX_DEF			3	/* driver's default tx antenna setting */
-
-#define TXCORE_POLICY_ALL	0x1	/* use all available core for transmit */
+/* Tx on antenna 0, "legacy term Main" */
+#define ANT_TX_FORCE_0		0
+/* Tx on antenna 1, "legacy term Aux" */
+#define ANT_TX_FORCE_1		1
+/* Tx on phy's last good Rx antenna */
+#define ANT_TX_LAST_RX		3
+/* driver's default tx antenna setting */
+#define ANT_TX_DEF		3
 
 /* Tx Chain values */
-#define TXCHAIN_DEF		0x1	/* def bitmap of txchain */
-#define TXCHAIN_DEF_NPHY	0x3	/* default bitmap of tx chains for nphy */
-#define TXCHAIN_DEF_HTPHY	0x7	/* default bitmap of tx chains for nphy */
-#define RXCHAIN_DEF		0x1	/* def bitmap of rxchain */
-#define RXCHAIN_DEF_NPHY	0x3	/* default bitmap of rx chains for nphy */
-#define RXCHAIN_DEF_HTPHY	0x7	/* default bitmap of rx chains for nphy */
-#define ANTSWITCH_NONE		0	/* no antenna switch */
-#define ANTSWITCH_TYPE_1	1	/* antenna switch on 4321CB2, 2of3 */
-#define ANTSWITCH_TYPE_2	2	/* antenna switch on 4321MPCI, 2of3 */
-#define ANTSWITCH_TYPE_3	3	/* antenna switch on 4322, 2of3 */
+/* def bitmap of txchain */
+#define TXCHAIN_DEF		0x1
+/* default bitmap of tx chains for nphy */
+#define TXCHAIN_DEF_NPHY	0x3
+/* default bitmap of tx chains for nphy */
+#define TXCHAIN_DEF_HTPHY	0x7
+/* def bitmap of rxchain */
+#define RXCHAIN_DEF		0x1
+/* default bitmap of rx chains for nphy */
+#define RXCHAIN_DEF_NPHY	0x3
+/* default bitmap of rx chains for nphy */
+#define RXCHAIN_DEF_HTPHY	0x7
+/* no antenna switch */
+#define ANTSWITCH_NONE		0
+/* antenna switch on 4321CB2, 2of3 */
+#define ANTSWITCH_TYPE_1	1
+/* antenna switch on 4321MPCI, 2of3 */
+#define ANTSWITCH_TYPE_2	2
+/* antenna switch on 4322, 2of3 */
+#define ANTSWITCH_TYPE_3	3
 
 #define RXBUFSZ		PKTBUFSZ
-#ifndef AIDMAPSZ
-#define AIDMAPSZ	(roundup(MAXSCB, NBBY)/NBBY)	/* aid bitmap size in bytes */
-#endif				/* AIDMAPSZ */
 
 #define MAX_STREAMS_SUPPORTED	4	/* max number of streams supported */
 
-#define	WL_SPURAVOID_OFF	0
-#define	WL_SPURAVOID_ON1	1
-#define	WL_SPURAVOID_ON2	2
-
-struct brcms_tunables {
-	int ntxd;		/* size of tx descriptor table */
-	int nrxd;		/* size of rx descriptor table */
-	int rxbufsz;		/* size of rx buffers to post */
-	int nrxbufpost;		/* # of rx buffers to post */
-	int maxscb;		/* # of SCBs supported */
-	int ampdunummpdu;	/* max number of mpdu in an ampdu */
-	int maxpktcb;		/* max # of packet callbacks */
-	int maxucodebss;	/* max # of BSS handled in ucode bcn/prb */
-	int maxucodebss4;	/* max # of BSS handled in sw bcn/prb */
-	int maxbss;		/* max # of bss info elements in scan list */
-	int datahiwat;		/* data msg txq hiwat mark */
-	int ampdudatahiwat;	/* AMPDU msg txq hiwat mark */
-	int rxbnd;		/* max # of rx bufs to process before deferring to dpc */
-	int txsbnd;		/* max # tx status to process in wlc_txstatus() */
-	int memreserved;	/* memory reserved for BMAC's USB dma rx */
+struct brcm_rateset {
+	/* # rates in this set */
+	u32 count;
+	/* rates in 500kbps units w/hi bit set if basic */
+	u8 rates[WL_NUMRATES];
 };
 
-struct brcms_rateset {
+struct brcms_c_rateset {
 	uint count;		/* number of rates in rates[] */
 	 /* rates in 500kbps units w/hi bit set if basic */
 	u8 rates[BRCMS_NUMRATES];
@@ -133,34 +374,11 @@ struct brcms_rateset {
 	u8 mcs[MCSSET_LEN];	/* supported mcs index bit map */
 };
 
-struct rsn_parms {
-	u8 flags;		/* misc booleans (e.g., supported) */
-	u8 multicast;	/* multicast cipher */
-	u8 ucount;		/* count of unicast ciphers */
-	u8 unicast[4];	/* unicast ciphers */
-	u8 acount;		/* count of auth modes */
-	u8 auth[4];		/* Authentication modes */
-	u8 PAD[4];		/* padding for future growth */
-};
-
-/*
- * 32 SSID chars, max of 4 chars for each SSID char "\xFF", plus NULL.
- */
-#define SSID_FMT_BUF_LEN	((4 * IEEE80211_MAX_SSID_LEN) + 1)
-
-#define RSN_FLAGS_SUPPORTED		0x1	/* Flag for rsn_params */
-#define RSN_FLAGS_PREAUTH		0x2	/* Flag for WPA2 rsn_params */
-
 /* All the HT-specific default advertised capabilities (including AMPDU)
  * should be grouped here at one place
  */
 #define AMPDU_DEF_MPDU_DENSITY	6	/* default mpdu density (110 ==> 4us) */
 
-/* defaults for the HT (MIMO) bss */
-#define HT_CAP	(IEEE80211_HT_CAP_SM_PS |\
-	IEEE80211_HT_CAP_SUP_WIDTH_20_40 | IEEE80211_HT_CAP_GRN_FLD |\
-	IEEE80211_HT_CAP_MAX_AMSDU | IEEE80211_HT_CAP_DSSSCCK40)
-
 /* wlc internal bss_info */
 struct brcms_bss_info {
 	u8 BSSID[ETH_ALEN];	/* network BSSID */
@@ -170,200 +388,55 @@ struct brcms_bss_info {
 	s16 RSSI;		/* receive signal strength (in dBm) */
 	s16 SNR;		/* receive signal SNR in dB */
 	u16 beacon_period;	/* units are Kusec */
-	u16 atim_window;	/* units are Kusec */
-	chanspec_t chanspec;	/* Channel num, bw, ctrl_sb and band */
-	s8 infra;		/* 0=IBSS, 1=infrastructure, 2=unknown */
-	wlc_rateset_t rateset;	/* supported rates */
-	u8 dtim_period;	/* DTIM period */
-	s8 phy_noise;		/* noise right after tx (in dBm) */
-	u16 capability;	/* Capability information */
-	u8 wme_qosinfo;	/* QoS Info from WME IE; valid if BSS_WME flag set */
-	struct rsn_parms wpa;
-	struct rsn_parms wpa2;
-	u16 qbss_load_aac;	/* qbss load available admission capacity */
-	/* qbss_load_chan_free <- (0xff - channel_utilization of qbss_load_ie_t) */
-	u8 qbss_load_chan_free;	/* indicates how free the channel is */
-	u8 mcipher;		/* multicast cipher */
-	u8 wpacfg;		/* wpa config index */
+	u16 chanspec;	/* Channel num, bw, ctrl_sb and band */
+	struct brcms_c_rateset rateset;	/* supported rates */
 };
 
-/* IOVar flags for common error checks */
-#define IOVF_MFG	(1<<3)	/* flag for mfgtest iovars */
-#define IOVF_WHL	(1<<4)	/* value must be whole (0-max) */
-#define IOVF_NTRL	(1<<5)	/* value must be natural (1-max) */
-
-#define IOVF_SET_UP	(1<<6)	/* set requires driver be up */
-#define IOVF_SET_DOWN	(1<<7)	/* set requires driver be down */
-#define IOVF_SET_CLK	(1<<8)	/* set requires core clock */
-#define IOVF_SET_BAND	(1<<9)	/* set requires fixed band */
-
-#define IOVF_GET_UP	(1<<10)	/* get requires driver be up */
-#define IOVF_GET_DOWN	(1<<11)	/* get requires driver be down */
-#define IOVF_GET_CLK	(1<<12)	/* get requires core clock */
-#define IOVF_GET_BAND	(1<<13)	/* get requires fixed band */
-#define IOVF_OPEN_ALLOW	(1<<14)	/* set allowed iovar for opensrc */
-
-/* watchdog down and dump callback function proto's */
-typedef int (*watchdog_fn_t) (void *handle);
-typedef int (*down_fn_t) (void *handle);
-typedef int (*dump_fn_t) (void *handle, struct brcmu_strbuf *b);
-
-/* IOVar handler
- *
- * handle - a pointer value registered with the function
- * vi - iovar_info that was looked up
- * actionid - action ID, calculated by IOV_GVAL() and IOV_SVAL() based on varid.
- * name - the actual iovar name
- * params/plen - parameters and length for a get, input only.
- * arg/len - buffer and length for value to be set or retrieved, input or output.
- * vsize - value size, valid for integer type only.
- * wlcif - interface context (brcms_c_if pointer)
- *
- * All pointers may point into the same buffer.
- */
-typedef int (*iovar_fn_t) (void *handle, const struct brcmu_iovar *vi,
-			   u32 actionid, const char *name, void *params,
-			   uint plen, void *arg, int alen, int vsize,
-			   struct brcms_c_if *wlcif);
-
 #define MAC80211_PROMISC_BCNS	(1 << 0)
 #define MAC80211_SCAN		(1 << 1)
 
 /*
- * Public portion of "common" os-independent state structure.
+ * Public portion of common driver state structure.
  * The wlc handle points at this.
  */
 struct brcms_pub {
-	void *wlc;
-
+	struct brcms_c_info *wlc;
 	struct ieee80211_hw *ieee_hw;
-	struct scb *global_scb;
 	struct scb_ampdu *global_ampdu;
 	uint mac80211_state;
 	uint unit;		/* device instance number */
 	uint corerev;		/* core revision */
 	struct si_pub *sih;	/* SI handle (cookie for siutils calls) */
-	char *vars;		/* "environment" name=value */
 	bool up;		/* interface up and running */
 	bool hw_off;		/* HW is off */
-	/* tunables: ntxd, nrxd, maxscb, etc. */
-	struct brcms_tunables *tunables;
-	bool hw_up;		/* one time hw up/down(from boot or hibernation) */
-	bool _piomode;		/* true if pio mode *//* BMAC_NOTE: NEED In both */
+	bool hw_up;		/* one time hw up/down */
+	bool _piomode;		/* true if pio mode */
 	uint _nbands;		/* # bands supported */
 	uint now;		/* # elapsed seconds */
 
 	bool promisc;		/* promiscuous destination address */
 	bool delayed_down;	/* down delayed */
-	bool _ap;		/* AP mode enabled */
-	bool _apsta;		/* simultaneous AP/STA mode enabled */
-	bool _assoc_recreate;	/* association recreation on up transitions */
-	int _wme;		/* WME QoS mode */
-	u8 _mbss;		/* MBSS mode on */
-	bool allmulti;		/* enable all multicasts */
 	bool associated;	/* true:part of [I]BSS, false: not */
 	/* (union of stas_associated, aps_associated) */
-	bool phytest_on;	/* whether a PHY test is running */
-	bool bf_preempt_4306;	/* True to enable 'darwin' mode */
 	bool _ampdu;		/* ampdu enabled or not */
-	bool _cac;		/* 802.11e CAC enabled */
 	u8 _n_enab;		/* bitmap of 11N + HT support */
-	bool _n_reqd;		/* N support required for clients */
-
-	s8 _coex;		/* 20/40 MHz BSS Management AUTO, ENAB, DISABLE */
-	bool _priofc;		/* Priority-based flowcontrol */
 
 	u8 cur_etheraddr[ETH_ALEN];	/* our local ethernet address */
 
-	u8 *multicast;	/* ptr to list of multicast addresses */
-	uint nmulticast;	/* # enabled multicast addresses */
-
-	u32 wlfeatureflag;	/* Flags to control sw features from registry */
-	int psq_pkts_total;	/* total num of ps pkts */
-
-	u16 txmaxpkts;	/* max number of large pkts allowed to be pending */
-
-	/* s/w decryption counters */
-	u32 swdecrypt;	/* s/w decrypt attempts */
-
 	int bcmerror;		/* last bcm error */
 
-	mbool radio_disabled;	/* bit vector for radio disabled reasons */
-	bool radio_active;	/* radio on/off state */
-	u16 roam_time_thresh;	/* Max. # secs. of not hearing beacons
-					 * before roaming.
-					 */
-	bool align_wd_tbtt;	/* Align watchdog with tbtt indication
-				 * handling. This flag is cleared by default
-				 * and is set by per port code explicitly and
-				 * you need to make sure the OSL_SYSUPTIME()
-				 * is implemented properly in osl of that port
-				 * when it enables this Power Save feature.
-				 */
+	u32 radio_disabled;	/* bit vector for radio disabled reasons */
 
 	u16 boardrev;	/* version # of particular board */
 	u8 sromrev;		/* version # of the srom */
 	char srom_ccode[BRCM_CNTRY_BUF_SZ];	/* Country Code in SROM */
 	u32 boardflags;	/* Board specific flags from srom */
 	u32 boardflags2;	/* More board flags if sromrev >= 4 */
-	bool tempsense_disable;	/* disable periodic tempsense check */
 	bool phy_11ncapable;	/* the PHY/HW is capable of 802.11N */
-	bool _ampdumac;		/* mac assist ampdu enabled or not */
 
 	struct wl_cnt *_cnt;	/* low-level counters in driver */
 };
 
-/* wl_monitor rx status per packet */
-struct wl_rxsts {
-	uint pkterror;		/* error flags per pkt */
-	uint phytype;		/* 802.11 A/B/G ... */
-	uint channel;		/* channel */
-	uint datarate;		/* rate in 500kbps */
-	uint antenna;		/* antenna pkts received on */
-	uint pktlength;		/* pkt length minus bcm phy hdr */
-	u32 mactime;		/* time stamp from mac, count per 1us */
-	uint sq;		/* signal quality */
-	s32 signal;		/* in dbm */
-	s32 noise;		/* in dbm */
-	uint preamble;		/* Unknown, short, long */
-	uint encoding;		/* Unknown, CCK, PBCC, OFDM */
-	uint nfrmtype;		/* special 802.11n frames(AMPDU, AMSDU) */
-	struct brcms_if *wlif;	/* wl interface */
-};
-
-/* status per error RX pkt */
-#define WL_RXS_CRC_ERROR		0x00000001	/* CRC Error in packet */
-#define WL_RXS_RUNT_ERROR		0x00000002	/* Runt packet */
-#define WL_RXS_ALIGN_ERROR		0x00000004	/* Misaligned packet */
-#define WL_RXS_OVERSIZE_ERROR		0x00000008	/* packet bigger than RX_LENGTH (usually 1518) */
-#define WL_RXS_WEP_ICV_ERROR		0x00000010	/* Integrity Check Value error */
-#define WL_RXS_WEP_ENCRYPTED		0x00000020	/* Encrypted with WEP */
-#define WL_RXS_PLCP_SHORT		0x00000040	/* Short PLCP error */
-#define WL_RXS_DECRYPT_ERR		0x00000080	/* Decryption error */
-#define WL_RXS_OTHER_ERR		0x80000000	/* Other errors */
-
-/* phy type */
-#define WL_RXS_PHY_A			0x00000000	/* A phy type */
-#define WL_RXS_PHY_B			0x00000001	/* B phy type */
-#define WL_RXS_PHY_G			0x00000002	/* G phy type */
-#define WL_RXS_PHY_N			0x00000004	/* N phy type */
-
-/* encoding */
-#define WL_RXS_ENCODING_CCK		0x00000000	/* CCK encoding */
-#define WL_RXS_ENCODING_OFDM		0x00000001	/* OFDM encoding */
-
-/* preamble */
-#define WL_RXS_UNUSED_STUB		0x0	/* stub to match with wlc_ethereal.h */
-#define WL_RXS_PREAMBLE_SHORT		0x00000001	/* Short preamble */
-#define WL_RXS_PREAMBLE_LONG		0x00000002	/* Long preamble */
-#define WL_RXS_PREAMBLE_MIMO_MM		0x00000003	/* MIMO mixed mode preamble */
-#define WL_RXS_PREAMBLE_MIMO_GF		0x00000004	/* MIMO green field preamble */
-
-#define WL_RXS_NFRM_AMPDU_FIRST		0x00000001	/* first MPDU in A-MPDU */
-#define WL_RXS_NFRM_AMPDU_SUB		0x00000002	/* subsequent MPDU(s) in A-MPDU */
-#define WL_RXS_NFRM_AMSDU_FIRST		0x00000004	/* first MSDU in A-MSDU */
-#define WL_RXS_NFRM_AMSDU_SUB		0x00000008	/* subsequent MSDU(s) in A-MSDU */
-
 enum wlc_par_id {
 	IOV_MPC = 1,
 	IOV_RTSTHRESH,
@@ -376,49 +449,15 @@ enum wlc_par_id {
  * *********************************************
  */
 
-/* AP Support (versus STA) */
-#define	AP_ENAB(pub)	(0)
-
-/* Macro to check if APSTA mode enabled */
-#define APSTA_ENAB(pub)	(0)
-
-/* Some useful combinations */
-#define STA_ONLY(pub)	(!AP_ENAB(pub))
-#define AP_ONLY(pub)	(AP_ENAB(pub) && !APSTA_ENAB(pub))
-
 #define ENAB_1x1	0x01
 #define ENAB_2x2	0x02
 #define ENAB_3x3	0x04
 #define ENAB_4x4	0x08
 #define SUPPORT_11N	(ENAB_1x1|ENAB_2x2)
 #define SUPPORT_HT	(ENAB_1x1|ENAB_2x2|ENAB_3x3)
-/* WL11N Support */
-#if ((defined(NCONF) && (NCONF != 0)) || (defined(LCNCONF) && (LCNCONF != 0)) || \
-	(defined(HTCONF) && (HTCONF != 0)) || (defined(SSLPNCONF) && (SSLPNCONF != 0)))
-#define N_ENAB(pub) ((pub)->_n_enab & SUPPORT_11N)
-#define N_REQD(pub) ((pub)->_n_reqd)
-#else
-#define N_ENAB(pub)	0
-#define N_REQD(pub)	0
-#endif
-
-#if (defined(HTCONF) && (HTCONF != 0))
-#define HT_ENAB(pub) (((pub)->_n_enab & SUPPORT_HT) == SUPPORT_HT)
-#else
-#define HT_ENAB(pub) 0
-#endif
 
+/* WL11N Support */
 #define AMPDU_AGG_HOST	1
-#define AMPDU_ENAB(pub) ((pub)->_ampdu)
-
-#define EDCF_ENAB(pub) (WME_ENAB(pub))
-#define QOS_ENAB(pub) (WME_ENAB(pub) || N_ENAB(pub))
-
-#define MONITOR_ENAB(wlc)	((wlc)->monitor)
-
-#define PROMISC_ENAB(wlc)	((wlc)->promisc)
-
-#define	BRCMS_PREC_COUNT	16	/* Max precedence level implemented */
 
 /* pri is priority encoded in the packet. This maps the Packet priority to
  * enqueue precedence as defined in wlc_prec_map
@@ -426,18 +465,19 @@ enum wlc_par_id {
 extern const u8 wlc_prio2prec_map[];
 #define BRCMS_PRIO_TO_PREC(pri)	wlc_prio2prec_map[(pri) & 7]
 
-/* This maps priority to one precedence higher - Used by PS-Poll response packets to
- * simulate enqueue-at-head operation, but still maintain the order on the queue
- */
-#define BRCMS_PRIO_TO_HI_PREC(pri)	min(BRCMS_PRIO_TO_PREC(pri) + 1,\
-					    BRCMS_PREC_COUNT - 1)
-
-extern const u8 wme_fifo2ac[];
-#define WME_PRIO2AC(prio)	wme_fifo2ac[prio2fifo[(prio)]]
+#define	BRCMS_PREC_COUNT	16	/* Max precedence level implemented */
 
 /* Mask to describe all precedence levels */
 #define BRCMS_PREC_BMP_ALL		MAXBITVAL(BRCMS_PREC_COUNT)
 
+/*
+ * This maps priority to one precedence higher - Used by PS-Poll response
+ * packets to simulate enqueue-at-head operation, but still maintain the
+ * order on the queue
+ */
+#define BRCMS_PRIO_TO_HI_PREC(pri)	min(BRCMS_PRIO_TO_PREC(pri) + 1,\
+					    BRCMS_PREC_COUNT - 1)
+
 /* Define a bitmap of precedences comprised by each AC */
 #define BRCMS_PREC_BMP_AC_BE	(NBITVAL(BRCMS_PRIO_TO_PREC(PRIO_8021D_BE)) | \
 			NBITVAL(BRCMS_PRIO_TO_HI_PREC(PRIO_8021D_BE)) |	\
@@ -456,14 +496,6 @@ extern const u8 wme_fifo2ac[];
 			NBITVAL(BRCMS_PRIO_TO_PREC(PRIO_8021D_NC)) |	\
 			NBITVAL(BRCMS_PRIO_TO_HI_PREC(PRIO_8021D_NC)))
 
-/* WME Support */
-#define WME_ENAB(pub) ((pub)->_wme != OFF)
-#define WME_AUTO(wlc) ((wlc)->pub->_wme == AUTO)
-
-/* invalid core flags, use the saved coreflags */
-#define BRCMS_USE_COREFLAGS	0xffffffff
-
-
 /* network protection config */
 #define	BRCMS_PROT_G_SPEC		1	/* SPEC g protection */
 #define	BRCMS_PROT_G_OVR		2	/* SPEC g prot override */
@@ -480,28 +512,34 @@ extern const u8 wme_fifo2ac[];
 /*
  * 54g modes (basic bits may still be overridden)
  *
- * GMODE_LEGACY_B			Rateset: 1b, 2b, 5.5, 11
- *					Preamble: Long
- *					Shortslot: Off
- * GMODE_AUTO				Rateset: 1b, 2b, 5.5b, 11b, 18, 24, 36, 54
- *					Extended Rateset: 6, 9, 12, 48
- *					Preamble: Long
- *					Shortslot: Auto
- * GMODE_ONLY				Rateset: 1b, 2b, 5.5b, 11b, 18, 24b, 36, 54
- *					Extended Rateset: 6b, 9, 12b, 48
- *					Preamble: Short required
- *					Shortslot: Auto
- * GMODE_B_DEFERRED			Rateset: 1b, 2b, 5.5b, 11b, 18, 24, 36, 54
- *					Extended Rateset: 6, 9, 12, 48
- *					Preamble: Long
- *					Shortslot: On
- * GMODE_PERFORMANCE			Rateset: 1b, 2b, 5.5b, 6b, 9, 11b, 12b, 18, 24b, 36, 48, 54
- *					Preamble: Short required
- *					Shortslot: On and required
- * GMODE_LRS				Rateset: 1b, 2b, 5.5b, 11b
- *					Extended Rateset: 6, 9, 12, 18, 24, 36, 48, 54
- *					Preamble: Long
- *					Shortslot: Auto
+ * GMODE_LEGACY_B
+ *	Rateset: 1b, 2b, 5.5, 11
+ *	Preamble: Long
+ *	Shortslot: Off
+ * GMODE_AUTO
+ *	Rateset: 1b, 2b, 5.5b, 11b, 18, 24, 36, 54
+ *	Extended Rateset: 6, 9, 12, 48
+ *	Preamble: Long
+ *	Shortslot: Auto
+ * GMODE_ONLY
+ *	Rateset: 1b, 2b, 5.5b, 11b, 18, 24b, 36, 54
+ *	Extended Rateset: 6b, 9, 12b, 48
+ *	Preamble: Short required
+ *	Shortslot: Auto
+ * GMODE_B_DEFERRED
+ *	Rateset: 1b, 2b, 5.5b, 11b, 18, 24, 36, 54
+ *	Extended Rateset: 6, 9, 12, 48
+ *	Preamble: Long
+ *	Shortslot: On
+ * GMODE_PERFORMANCE
+ *	Rateset: 1b, 2b, 5.5b, 6b, 9, 11b, 12b, 18, 24b, 36, 48, 54
+ *	Preamble: Short required
+ *	Shortslot: On and required
+ * GMODE_LRS
+ *	Rateset: 1b, 2b, 5.5b, 11b
+ *	Extended Rateset: 6, 9, 12, 18, 24, 36, 48, 54
+ *	Preamble: Long
+ *	Shortslot: Auto
  */
 #define GMODE_LEGACY_B		0
 #define GMODE_AUTO		1
@@ -511,58 +549,13 @@ extern const u8 wme_fifo2ac[];
 #define GMODE_LRS		5
 #define GMODE_MAX		6
 
-/* values for PLCPHdr_override */
-#define BRCMS_PLCP_AUTO	-1
-#define BRCMS_PLCP_SHORT	0
-#define BRCMS_PLCP_LONG	1
-
-/* values for g_protection_override and n_protection_override */
-#define BRCMS_PROTECTION_AUTO		-1
-#define BRCMS_PROTECTION_OFF		0
-#define BRCMS_PROTECTION_ON		1
-#define BRCMS_PROTECTION_MMHDR_ONLY	2
-#define BRCMS_PROTECTION_CTS_ONLY		3
-
-/* values for g_protection_control and n_protection_control */
-#define BRCMS_PROTECTION_CTL_OFF		0
-#define BRCMS_PROTECTION_CTL_LOCAL	1
-#define BRCMS_PROTECTION_CTL_OVERLAP	2
-
-/* values for n_protection */
-#define BRCMS_N_PROTECTION_OFF		0
-#define BRCMS_N_PROTECTION_OPTIONAL	1
-#define BRCMS_N_PROTECTION_20IN40		2
-#define BRCMS_N_PROTECTION_MIXEDMODE	3
-
-/* values for band specific 40MHz capabilities */
-#define BRCMS_N_BW_20ALL			0
-#define BRCMS_N_BW_40ALL			1
-#define BRCMS_N_BW_20IN2G_40IN5G		2
-
-/* bitflags for SGI support (sgi_rx iovar) */
-#define BRCMS_N_SGI_20			0x01
-#define BRCMS_N_SGI_40			0x02
-
-/* defines used by the nrate iovar */
-#define NRATE_MCS_INUSE	0x00000080	/* MSC in use,indicates b0-6 holds an mcs */
-#define NRATE_RATE_MASK 0x0000007f	/* rate/mcs value */
-#define NRATE_STF_MASK	0x0000ff00	/* stf mode mask: siso, cdd, stbc, sdm */
-#define NRATE_STF_SHIFT	8	/* stf mode shift */
-#define NRATE_OVERRIDE	0x80000000	/* bit indicates override both rate & mode */
-#define NRATE_OVERRIDE_MCS_ONLY 0x40000000	/* bit indicate to override mcs only */
-#define NRATE_SGI_MASK  0x00800000	/* sgi mode */
-#define NRATE_SGI_SHIFT 23	/* sgi mode */
-#define NRATE_LDPC_CODING 0x00400000	/* bit indicates adv coding in use */
-#define NRATE_LDPC_SHIFT 22	/* ldpc shift */
-
-#define NRATE_STF_SISO	0	/* stf mode SISO */
-#define NRATE_STF_CDD	1	/* stf mode CDD */
-#define NRATE_STF_STBC	2	/* stf mode STBC */
-#define NRATE_STF_SDM	3	/* stf mode SDM */
-
-#define ANT_SELCFG_MAX		4	/* max number of antenna configurations */
-
-#define HIGHEST_SINGLE_STREAM_MCS	7	/* MCS values greater than this enable multiple streams */
+/* MCS values greater than this enable multiple streams */
+#define HIGHEST_SINGLE_STREAM_MCS	7
+
+#define	MAXBANDS		2	/* Maximum #of bands */
+
+/* max number of antenna configurations */
+#define ANT_SELCFG_MAX		4
 
 struct brcms_antselcfg {
 	u8 ant_config[ANT_SELCFG_MAX];	/* antenna configuration */
@@ -570,15 +563,14 @@ struct brcms_antselcfg {
 };
 
 /* common functions for every port */
-extern void *brcms_c_attach(struct brcms_info *wl, u16 vendor, u16 device,
-			uint unit, bool piomode, void *regsva, uint bustype,
-			void *btparam, uint *perr);
+extern struct brcms_c_info *
+brcms_c_attach(struct brcms_info *wl, u16 vendor, u16 device, uint unit,
+	       bool piomode, void __iomem *regsva, struct pci_dev *btparam,
+	       uint *perr);
 extern uint brcms_c_detach(struct brcms_c_info *wlc);
 extern int brcms_c_up(struct brcms_c_info *wlc);
 extern uint brcms_c_down(struct brcms_c_info *wlc);
 
-extern int brcms_c_set(struct brcms_c_info *wlc, int cmd, int arg);
-extern int brcms_c_get(struct brcms_c_info *wlc, int cmd, int *arg);
 extern bool brcms_c_chipmatch(u16 vendor, u16 device);
 extern void brcms_c_init(struct brcms_c_info *wlc);
 extern void brcms_c_reset(struct brcms_c_info *wlc);
@@ -589,55 +581,31 @@ extern void brcms_c_intrsrestore(struct brcms_c_info *wlc, u32 macintmask);
 extern bool brcms_c_intrsupd(struct brcms_c_info *wlc);
 extern bool brcms_c_isr(struct brcms_c_info *wlc, bool *wantdpc);
 extern bool brcms_c_dpc(struct brcms_c_info *wlc, bool bounded);
-extern bool brcms_c_sendpkt_mac80211(struct brcms_c_info *wlc,
+extern void brcms_c_sendpkt_mac80211(struct brcms_c_info *wlc,
 				     struct sk_buff *sdu,
 				     struct ieee80211_hw *hw);
-extern int brcms_c_ioctl(struct brcms_c_info *wlc, int cmd, void *arg, int len,
-			 struct brcms_c_if *wlcif);
 extern bool brcms_c_aggregatable(struct brcms_c_info *wlc, u8 tid);
-
-/* helper functions */
-extern void brcms_c_statsupd(struct brcms_c_info *wlc);
 extern void brcms_c_protection_upd(struct brcms_c_info *wlc, uint idx,
 				   int val);
 extern int brcms_c_get_header_len(void);
-extern void brcms_c_mac_bcn_promisc_change(struct brcms_c_info *wlc,
-					   bool promisc);
 extern void brcms_c_set_addrmatch(struct brcms_c_info *wlc,
 				  int match_reg_offset,
 				  const u8 *addr);
 extern void brcms_c_wme_setparams(struct brcms_c_info *wlc, u16 aci,
 			      const struct ieee80211_tx_queue_params *arg,
 			      bool suspend);
-extern struct brcms_pub *brcms_c_pub(void *wlc);
-
-/* common functions for every port */
-extern void brcms_c_mhf(struct brcms_c_info *wlc, u8 idx, u16 mask, u16 val,
-		    int bands);
-extern void brcms_c_rate_lookup_init(struct brcms_c_info *wlc,
-				     wlc_rateset_t *rateset);
-extern void brcms_default_rateset(struct brcms_c_info *wlc, wlc_rateset_t *rs);
-
+extern struct brcms_pub *brcms_c_pub(struct brcms_c_info *wlc);
 extern void brcms_c_ampdu_flush(struct brcms_c_info *wlc,
 			    struct ieee80211_sta *sta, u16 tid);
 extern void brcms_c_ampdu_tx_operational(struct brcms_c_info *wlc, u8 tid,
 					 u8 ba_wsize, uint max_rx_ampdu_bytes);
-extern int brcms_c_set_par(struct brcms_c_info *wlc, enum wlc_par_id par_id,
-			   int val);
-extern int brcms_c_get_par(struct brcms_c_info *wlc, enum wlc_par_id par_id,
-			   int *ret_int_ptr);
-extern char *getvar(char *vars, const char *name);
-extern int getintvar(char *vars, const char *name);
-
-/* wlc_phy.c helper functions */
-extern void brcms_c_set_ps_ctrl(struct brcms_c_info *wlc);
-extern void brcms_c_mctrl(struct brcms_c_info *wlc, u32 mask, u32 val);
-
+extern char *getvar(struct si_pub *sih, enum brcms_srom_id id);
+extern int getintvar(struct si_pub *sih, enum brcms_srom_id id);
 extern int brcms_c_module_register(struct brcms_pub *pub,
-			       const char *name, void *hdl,
-			       watchdog_fn_t watchdog_fn, down_fn_t down_fn);
+				   const char *name, struct brcms_info *hdl,
+				   int (*down_fn)(void *handle));
 extern int brcms_c_module_unregister(struct brcms_pub *pub, const char *name,
-				 void *hdl);
+				     struct brcms_info *hdl);
 extern void brcms_c_suspend_mac_and_wait(struct brcms_c_info *wlc);
 extern void brcms_c_enable_mac(struct brcms_c_info *wlc);
 extern void brcms_c_associate_upd(struct brcms_c_info *wlc, bool state);
@@ -646,20 +614,21 @@ extern void brcms_c_scan_stop(struct brcms_c_info *wlc);
 extern int brcms_c_get_curband(struct brcms_c_info *wlc);
 extern void brcms_c_wait_for_tx_completion(struct brcms_c_info *wlc,
 					   bool drop);
-
-/* helper functions */
+extern int brcms_c_set_channel(struct brcms_c_info *wlc, u16 channel);
+extern int brcms_c_set_rate_limit(struct brcms_c_info *wlc, u16 srl, u16 lrl);
+extern void brcms_c_get_current_rateset(struct brcms_c_info *wlc,
+				 struct brcm_rateset *currs);
+extern int brcms_c_set_rateset(struct brcms_c_info *wlc,
+					struct brcm_rateset *rs);
+extern int brcms_c_set_beacon_period(struct brcms_c_info *wlc, u16 period);
+extern u16 brcms_c_get_phy_type(struct brcms_c_info *wlc, int phyidx);
+extern void brcms_c_set_shortslot_override(struct brcms_c_info *wlc,
+				    s8 sslot_override);
+extern void brcms_c_set_beacon_listen_interval(struct brcms_c_info *wlc,
+					u8 interval);
+extern int brcms_c_set_tx_power(struct brcms_c_info *wlc, int txpwr);
+extern int brcms_c_get_tx_power(struct brcms_c_info *wlc);
+extern void brcms_c_set_radio_mpc(struct brcms_c_info *wlc, bool mpc);
 extern bool brcms_c_check_radio_disabled(struct brcms_c_info *wlc);
-extern bool brcms_c_radio_monitor_stop(struct brcms_c_info *wlc);
-
-#define	MAXBANDS		2	/* Maximum #of bands */
-/* bandstate array indices */
-#define BAND_2G_INDEX		0	/* wlc->bandstate[x] index */
-#define BAND_5G_INDEX		1	/* wlc->bandstate[x] index */
-
-#define BAND_2G_NAME		"2.4G"
-#define BAND_5G_NAME		"5G"
-
-/* BMAC RPC: 7 u32 params: pkttotlen, fifo, commit, fid, txpktpend, pktflag, rpc_id */
-#define BRCMS_RPCTX_PARAMS		32
 
 #endif				/* _BRCM_PUB_H_ */
diff --git a/drivers/staging/brcm80211/brcmsmac/rate.c b/drivers/staging/brcm80211/brcmsmac/rate.c
index f0e4b99c2566..0a0c0ad4f96f 100644
--- a/drivers/staging/brcm80211/brcmsmac/rate.c
+++ b/drivers/staging/brcm80211/brcmsmac/rate.c
@@ -21,7 +21,10 @@
 #include "pub.h"
 #include "rate.h"
 
-/* Rate info per rate: It tells whether a rate is ofdm or not and its phy_rate value */
+/*
+ * Rate info per rate: It tells whether a rate is ofdm or not and its phy_rate
+ * value
+ */
 const u8 rate_info[BRCM_MAXRATE + 1] = {
 	/*  0     1     2     3     4     5     6     7     8     9 */
 /*   0 */ 0x00, 0x00, 0x0a, 0x00, 0x14, 0x00, 0x00, 0x00, 0x00, 0x00,
@@ -139,20 +142,25 @@ const struct brcms_mcs_info mcs_table[MCS_TABLE_SIZE] = {
 	{0, 6000, 0, CEIL(6000 * 10, 9), 0x00, BRCM_RATE_6M},
 };
 
-/* phycfg for legacy OFDM frames: code rate, modulation scheme, spatial streams
- *   Number of spatial streams: always 1
- *   other fields: refer to table 78 of section 17.3.2.2 of the original .11a standard
+/*
+ * phycfg for legacy OFDM frames: code rate, modulation scheme, spatial streams
+ * Number of spatial streams: always 1 other fields: refer to table 78 of
+ * section 17.3.2.2 of the original .11a standard
  */
 struct legacy_phycfg {
 	u32 rate_ofdm;	/* ofdm mac rate */
-	u8 tx_phy_ctl3;	/* phy ctl byte 3, code rate, modulation type, # of streams */
+	/* phy ctl byte 3, code rate, modulation type, # of streams */
+	u8 tx_phy_ctl3;
 };
 
-#define LEGACY_PHYCFG_TABLE_SIZE	12	/* Number of legacy_rate_cfg entries in the table */
+/* Number of legacy_rate_cfg entries in the table */
+#define LEGACY_PHYCFG_TABLE_SIZE	12
 
-/* In CCK mode LPPHY overloads OFDM Modulation bits with CCK Data Rate */
-/* Eventually MIMOPHY would also be converted to this format */
-/* 0 = 1Mbps; 1 = 2Mbps; 2 = 5.5Mbps; 3 = 11Mbps */
+/*
+ * In CCK mode LPPHY overloads OFDM Modulation bits with CCK Data Rate
+ * Eventually MIMOPHY would also be converted to this format
+ * 0 = 1Mbps; 1 = 2Mbps; 2 = 5.5Mbps; 3 = 11Mbps
+ */
 static const struct
 legacy_phycfg legacy_phycfg_table[LEGACY_PHYCFG_TABLE_SIZE] = {
 	{BRCM_RATE_1M, 0x00},	/* CCK  1Mbps,  data rate  0 */
@@ -179,86 +187,89 @@ legacy_phycfg legacy_phycfg_table[LEGACY_PHYCFG_TABLE_SIZE] = {
 
 /* Hardware rates (also encodes default basic rates) */
 
-const wlc_rateset_t cck_ofdm_mimo_rates = {
+const struct brcms_c_rateset cck_ofdm_mimo_rates = {
 	12,
-	{			/*    1b,   2b,   5.5b, 6,    9,    11b,  12,   18,   24,   36,   48,   54 Mbps */
-	 0x82, 0x84, 0x8b, 0x0c, 0x12, 0x96, 0x18, 0x24, 0x30, 0x48, 0x60,
-	 0x6c},
+	/*  1b,   2b,   5.5b, 6,    9,    11b,  12,   18,   24,   36,   48, */
+	{ 0x82, 0x84, 0x8b, 0x0c, 0x12, 0x96, 0x18, 0x24, 0x30, 0x48, 0x60,
+	/* 54 Mbps */
+	  0x6c},
 	0x00,
-	{0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
+	{ 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	  0x00, 0x00, 0x00, 0x00, 0x00}
 };
 
-const wlc_rateset_t ofdm_mimo_rates = {
+const struct brcms_c_rateset ofdm_mimo_rates = {
 	8,
-	{			/*    6b,   9,    12b,  18,   24b,  36,   48,   54 Mbps */
-	 0x8c, 0x12, 0x98, 0x24, 0xb0, 0x48, 0x60, 0x6c},
+	/*  6b,   9,    12b,  18,   24b,  36,   48,   54 Mbps */
+	{ 0x8c, 0x12, 0x98, 0x24, 0xb0, 0x48, 0x60, 0x6c},
 	0x00,
-	{0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
+	{ 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	  0x00, 0x00, 0x00, 0x00, 0x00}
 };
 
 /* Default ratesets that include MCS32 for 40BW channels */
-const wlc_rateset_t cck_ofdm_40bw_mimo_rates = {
+static const struct brcms_c_rateset cck_ofdm_40bw_mimo_rates = {
 	12,
-	{			/*    1b,   2b,   5.5b, 6,    9,    11b,  12,   18,   24,   36,   48,   54 Mbps */
-	 0x82, 0x84, 0x8b, 0x0c, 0x12, 0x96, 0x18, 0x24, 0x30, 0x48, 0x60,
-	 0x6c},
+	/*  1b,   2b,   5.5b, 6,    9,    11b,  12,   18,   24,   36,   48 */
+	{ 0x82, 0x84, 0x8b, 0x0c, 0x12, 0x96, 0x18, 0x24, 0x30, 0x48, 0x60,
+	/* 54 Mbps */
+	  0x6c},
 	0x00,
-	{0xff, 0xff, 0xff, 0xff, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
+	{ 0xff, 0xff, 0xff, 0xff, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	  0x00, 0x00, 0x00, 0x00, 0x00}
 };
 
-const wlc_rateset_t ofdm_40bw_mimo_rates = {
+static const struct brcms_c_rateset ofdm_40bw_mimo_rates = {
 	8,
-	{			/*    6b,   9,    12b,  18,   24b,  36,   48,   54 Mbps */
-	 0x8c, 0x12, 0x98, 0x24, 0xb0, 0x48, 0x60, 0x6c},
+	/*  6b,   9,    12b,  18,   24b,  36,   48,   54 Mbps */
+	{ 0x8c, 0x12, 0x98, 0x24, 0xb0, 0x48, 0x60, 0x6c},
 	0x00,
-	{0xff, 0xff, 0xff, 0xff, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
+	{ 0xff, 0xff, 0xff, 0xff, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	  0x00, 0x00, 0x00, 0x00, 0x00}
 };
 
-const wlc_rateset_t cck_ofdm_rates = {
+const struct brcms_c_rateset cck_ofdm_rates = {
 	12,
-	{			/*    1b,   2b,   5.5b, 6,    9,    11b,  12,   18,   24,   36,   48,   54 Mbps */
-	 0x82, 0x84, 0x8b, 0x0c, 0x12, 0x96, 0x18, 0x24, 0x30, 0x48, 0x60,
-	 0x6c},
+	/*  1b,   2b, 5.5b, 6,    9,    11b,  12,   18,   24,   36,   48,*/
+	{ 0x82, 0x84, 0x8b, 0x0c, 0x12, 0x96, 0x18, 0x24, 0x30, 0x48, 0x60,
+	/*54 Mbps */
+	  0x6c},
 	0x00,
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
+	{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	  0x00, 0x00, 0x00, 0x00, 0x00}
 };
 
-const wlc_rateset_t gphy_legacy_rates = {
+const struct brcms_c_rateset gphy_legacy_rates = {
 	4,
-	{			/*    1b,   2b,   5.5b,  11b Mbps */
-	 0x82, 0x84, 0x8b, 0x96},
+	/*  1b,   2b,   5.5b, 11b Mbps */
+	{ 0x82, 0x84, 0x8b, 0x96},
 	0x00,
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
+	{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	  0x00, 0x00, 0x00, 0x00, 0x00}
 };
 
-const wlc_rateset_t ofdm_rates = {
+const struct brcms_c_rateset ofdm_rates = {
 	8,
-	{			/*    6b,   9,    12b,  18,   24b,  36,   48,   54 Mbps */
-	 0x8c, 0x12, 0x98, 0x24, 0xb0, 0x48, 0x60, 0x6c},
+	/*  6b,   9,    12b,  18,   24b,  36,   48,   54 Mbps */
+	{ 0x8c, 0x12, 0x98, 0x24, 0xb0, 0x48, 0x60, 0x6c},
 	0x00,
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
+	{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	  0x00, 0x00, 0x00, 0x00, 0x00}
 };
 
-const wlc_rateset_t cck_rates = {
+const struct brcms_c_rateset cck_rates = {
 	4,
-	{			/*    1b,   2b,   5.5,  11 Mbps */
-	 0x82, 0x84, 0x0b, 0x16},
+	/*  1b,   2b,   5.5,  11 Mbps */
+	{ 0x82, 0x84, 0x0b, 0x16},
 	0x00,
-	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	 0x00, 0x00, 0x00, 0x00}
+	{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	  0x00, 0x00, 0x00, 0x00, 0x00}
 };
 
 /* check if rateset is valid.
  * if check_brate is true, rateset without a basic rate is considered NOT valid.
  */
-static bool brcms_c_rateset_valid(wlc_rateset_t *rs, bool check_brate)
+static bool brcms_c_rateset_valid(struct brcms_c_rateset *rs, bool check_brate)
 {
 	uint idx;
 
@@ -276,19 +287,20 @@ static bool brcms_c_rateset_valid(wlc_rateset_t *rs, bool check_brate)
 	return false;
 }
 
-void brcms_c_rateset_mcs_upd(wlc_rateset_t *rs, u8 txstreams)
+void brcms_c_rateset_mcs_upd(struct brcms_c_rateset *rs, u8 txstreams)
 {
 	int i;
 	for (i = txstreams; i < MAX_STREAMS_SUPPORTED; i++)
 		rs->mcs[i] = 0;
 }
 
-/* filter based on hardware rateset, and sort filtered rateset with basic bit(s) preserved,
- * and check if resulting rateset is valid.
+/*
+ * filter based on hardware rateset, and sort filtered rateset with basic
+ * bit(s) preserved, and check if resulting rateset is valid.
 */
 bool
-brcms_c_rate_hwrs_filter_sort_validate(wlc_rateset_t *rs,
-				   const wlc_rateset_t *hw_rs,
+brcms_c_rate_hwrs_filter_sort_validate(struct brcms_c_rateset *rs,
+				   const struct brcms_c_rateset *hw_rs,
 				   bool check_brate, u8 txstreams)
 {
 	u8 rateset[BRCM_MAXRATE + 1];
@@ -328,10 +340,10 @@ brcms_c_rate_hwrs_filter_sort_validate(wlc_rateset_t *rs,
 }
 
 /* calculate the rate of a rx'd frame and return it as a ratespec */
-ratespec_t brcms_c_compute_rspec(struct d11rxhdr *rxh, u8 *plcp)
+u32 brcms_c_compute_rspec(struct d11rxhdr *rxh, u8 *plcp)
 {
 	int phy_type;
-	ratespec_t rspec = PHY_TXC1_BW_20MHZ << RSPEC_BW_SHIFT;
+	u32 rspec = PHY_TXC1_BW_20MHZ << RSPEC_BW_SHIFT;
 
 	phy_type =
 	    ((rxh->RxChan & RXS_CHAN_PHYTYPE_MASK) >> RXS_CHAN_PHYTYPE_SHIFT);
@@ -341,12 +353,12 @@ ratespec_t brcms_c_compute_rspec(struct d11rxhdr *rxh, u8 *plcp)
 		switch (rxh->PhyRxStatus_0 & PRXS0_FT_MASK) {
 		case PRXS0_CCK:
 			rspec =
-			    CCK_PHY2MAC_RATE(
+				cck_phy2mac_rate(
 				((struct cck_phy_hdr *) plcp)->signal);
 			break;
 		case PRXS0_OFDM:
 			rspec =
-			    OFDM_PHY2MAC_RATE(
+			    ofdm_phy2mac_rate(
 				((struct ofdm_phy_hdr *) plcp)->rlpt[0]);
 			break;
 		case PRXS0_PREN:
@@ -363,23 +375,24 @@ ratespec_t brcms_c_compute_rspec(struct d11rxhdr *rxh, u8 *plcp)
 			/* not supported, error condition */
 			break;
 		}
-		if (PLCP3_ISSGI(plcp[3]))
+		if (plcp3_issgi(plcp[3]))
 			rspec |= RSPEC_SHORT_GI;
 	} else
 	    if ((phy_type == PHY_TYPE_A) || (rxh->PhyRxStatus_0 & PRXS0_OFDM))
-		rspec = OFDM_PHY2MAC_RATE(
+		rspec = ofdm_phy2mac_rate(
 				((struct ofdm_phy_hdr *) plcp)->rlpt[0]);
 	else
-		rspec = CCK_PHY2MAC_RATE(
+		rspec = cck_phy2mac_rate(
 				((struct cck_phy_hdr *) plcp)->signal);
 
 	return rspec;
 }
 
 /* copy rateset src to dst as-is (no masking or sorting) */
-void brcms_c_rateset_copy(const wlc_rateset_t *src, wlc_rateset_t *dst)
+void brcms_c_rateset_copy(const struct brcms_c_rateset *src,
+			  struct brcms_c_rateset *dst)
 {
-	memcpy(dst, src, sizeof(wlc_rateset_t));
+	memcpy(dst, src, sizeof(struct brcms_c_rateset));
 }
 
 /*
@@ -392,8 +405,8 @@ void brcms_c_rateset_copy(const wlc_rateset_t *src, wlc_rateset_t *dst)
  * 'xmask' is the copy mask (typically 0x7f or 0xff).
  */
 void
-brcms_c_rateset_filter(wlc_rateset_t *src, wlc_rateset_t *dst, bool basic_only,
-		   u8 rates, uint xmask, bool mcsallow)
+brcms_c_rateset_filter(struct brcms_c_rateset *src, struct brcms_c_rateset *dst,
+		       bool basic_only, u8 rates, uint xmask, bool mcsallow)
 {
 	uint i;
 	uint r;
@@ -404,9 +417,11 @@ brcms_c_rateset_filter(wlc_rateset_t *src, wlc_rateset_t *dst, bool basic_only,
 		r = src->rates[i];
 		if (basic_only && !(r & BRCMS_RATE_FLAG))
 			continue;
-		if (rates == BRCMS_RATES_CCK && IS_OFDM((r & BRCMS_RATE_MASK)))
+		if (rates == BRCMS_RATES_CCK &&
+		    is_ofdm_rate((r & BRCMS_RATE_MASK)))
 			continue;
-		if (rates == BRCMS_RATES_OFDM && IS_CCK((r & BRCMS_RATE_MASK)))
+		if (rates == BRCMS_RATES_OFDM &&
+		    is_cck_rate((r & BRCMS_RATE_MASK)))
 			continue;
 		dst->rates[count++] = r & xmask;
 	}
@@ -423,26 +438,27 @@ brcms_c_rateset_filter(wlc_rateset_t *src, wlc_rateset_t *dst, bool basic_only,
  * and fill rs_tgt with result
  */
 void
-brcms_c_rateset_default(wlc_rateset_t *rs_tgt, const wlc_rateset_t *rs_hw,
-		    uint phy_type, int bandtype, bool cck_only, uint rate_mask,
-		    bool mcsallow, u8 bw, u8 txstreams)
+brcms_c_rateset_default(struct brcms_c_rateset *rs_tgt,
+			const struct brcms_c_rateset *rs_hw,
+			uint phy_type, int bandtype, bool cck_only,
+			uint rate_mask, bool mcsallow, u8 bw, u8 txstreams)
 {
-	const wlc_rateset_t *rs_dflt;
-	wlc_rateset_t rs_sel;
+	const struct brcms_c_rateset *rs_dflt;
+	struct brcms_c_rateset rs_sel;
 	if ((PHYTYPE_IS(phy_type, PHY_TYPE_HT)) ||
 	    (PHYTYPE_IS(phy_type, PHY_TYPE_N)) ||
 	    (PHYTYPE_IS(phy_type, PHY_TYPE_LCN)) ||
 	    (PHYTYPE_IS(phy_type, PHY_TYPE_SSN))) {
-		if (BAND_5G(bandtype)) {
+		if (bandtype == BRCM_BAND_5G)
 			rs_dflt = (bw == BRCMS_20_MHZ ?
 				   &ofdm_mimo_rates : &ofdm_40bw_mimo_rates);
-		} else {
+		else
 			rs_dflt = (bw == BRCMS_20_MHZ ?
 				   &cck_ofdm_mimo_rates :
 				   &cck_ofdm_40bw_mimo_rates);
-		}
 	} else if (PHYTYPE_IS(phy_type, PHY_TYPE_LP)) {
-		rs_dflt = (BAND_5G(bandtype)) ? &ofdm_rates : &cck_ofdm_rates;
+		rs_dflt = (bandtype == BRCM_BAND_5G) ?
+			  &ofdm_rates : &cck_ofdm_rates;
 	} else if (PHYTYPE_IS(phy_type, PHY_TYPE_A)) {
 		rs_dflt = &ofdm_rates;
 	} else if (PHYTYPE_IS(phy_type, PHY_TYPE_G)) {
@@ -475,21 +491,21 @@ s16 brcms_c_rate_legacy_phyctl(uint rate)
 	return -1;
 }
 
-void brcms_c_rateset_mcs_clear(wlc_rateset_t *rateset)
+void brcms_c_rateset_mcs_clear(struct brcms_c_rateset *rateset)
 {
 	uint i;
 	for (i = 0; i < MCSSET_LEN; i++)
 		rateset->mcs[i] = 0;
 }
 
-void brcms_c_rateset_mcs_build(wlc_rateset_t *rateset, u8 txstreams)
+void brcms_c_rateset_mcs_build(struct brcms_c_rateset *rateset, u8 txstreams)
 {
 	memcpy(&rateset->mcs[0], &cck_ofdm_mimo_rates.mcs[0], MCSSET_LEN);
 	brcms_c_rateset_mcs_upd(rateset, txstreams);
 }
 
 /* Based on bandwidth passed, allow/disallow MCS 32 in the rateset */
-void brcms_c_rateset_bw_mcs_filter(wlc_rateset_t *rateset, u8 bw)
+void brcms_c_rateset_bw_mcs_filter(struct brcms_c_rateset *rateset, u8 bw)
 {
 	if (bw == BRCMS_40_MHZ)
 		setbit(rateset->mcs, 32);
diff --git a/drivers/staging/brcm80211/brcmsmac/rate.h b/drivers/staging/brcm80211/brcmsmac/rate.h
index dbfd3e5816d4..e7b9dc2f2731 100644
--- a/drivers/staging/brcm80211/brcmsmac/rate.h
+++ b/drivers/staging/brcm80211/brcmsmac/rate.h
@@ -18,156 +18,233 @@
 #define _BRCM_RATE_H_
 
 #include "types.h"
+#include "d11.h"
 
 extern const u8 rate_info[];
-extern const struct brcms_rateset cck_ofdm_mimo_rates;
-extern const struct brcms_rateset ofdm_mimo_rates;
-extern const struct brcms_rateset cck_ofdm_rates;
-extern const struct brcms_rateset ofdm_rates;
-extern const struct brcms_rateset cck_rates;
-extern const struct brcms_rateset gphy_legacy_rates;
-extern const struct brcms_rateset wlc_lrs_rates;
-extern const struct brcms_rateset rate_limit_1_2;
+extern const struct brcms_c_rateset cck_ofdm_mimo_rates;
+extern const struct brcms_c_rateset ofdm_mimo_rates;
+extern const struct brcms_c_rateset cck_ofdm_rates;
+extern const struct brcms_c_rateset ofdm_rates;
+extern const struct brcms_c_rateset cck_rates;
+extern const struct brcms_c_rateset gphy_legacy_rates;
+extern const struct brcms_c_rateset rate_limit_1_2;
 
 struct brcms_mcs_info {
-	u32 phy_rate_20;	/* phy rate in kbps [20Mhz] */
-	u32 phy_rate_40;	/* phy rate in kbps [40Mhz] */
-	u32 phy_rate_20_sgi;	/* phy rate in kbps [20Mhz] with SGI */
-	u32 phy_rate_40_sgi;	/* phy rate in kbps [40Mhz] with SGI */
-	u8 tx_phy_ctl3;	/* phy ctl byte 3, code rate, modulation type, # of streams */
-	u8 leg_ofdm;		/* matching legacy ofdm rate in 500bkps */
+	/* phy rate in kbps [20Mhz] */
+	u32 phy_rate_20;
+	/* phy rate in kbps [40Mhz] */
+	u32 phy_rate_40;
+	/* phy rate in kbps [20Mhz] with SGI */
+	u32 phy_rate_20_sgi;
+	/* phy rate in kbps [40Mhz] with SGI */
+	u32 phy_rate_40_sgi;
+	/* phy ctl byte 3, code rate, modulation type, # of streams */
+	u8 tx_phy_ctl3;
+	/* matching legacy ofdm rate in 500bkps */
+	u8 leg_ofdm;
 };
 
 #define BRCMS_MAXMCS	32	/* max valid mcs index */
 #define MCS_TABLE_SIZE	33	/* Number of mcs entries in the table */
 extern const struct brcms_mcs_info mcs_table[];
 
-#define MCS_INVALID	0xFF
-#define MCS_CR_MASK	0x07	/* Code Rate bit mask */
-#define MCS_MOD_MASK	0x38	/* Modulation bit shift */
-#define MCS_MOD_SHIFT	3	/* MOdulation bit shift */
 #define MCS_TXS_MASK	0xc0	/* num tx streams - 1 bit mask */
 #define MCS_TXS_SHIFT	6	/* num tx streams - 1 bit shift */
-#define MCS_CR(_mcs)	(mcs_table[_mcs].tx_phy_ctl3 & MCS_CR_MASK)
-#define MCS_MOD(_mcs)	((mcs_table[_mcs].tx_phy_ctl3 & MCS_MOD_MASK) >> MCS_MOD_SHIFT)
-#define MCS_TXS(_mcs)	((mcs_table[_mcs].tx_phy_ctl3 & MCS_TXS_MASK) >> MCS_TXS_SHIFT)
-#define MCS_RATE(_mcs, _is40, _sgi)	(_sgi ? \
-	(_is40 ? mcs_table[_mcs].phy_rate_40_sgi : mcs_table[_mcs].phy_rate_20_sgi) : \
-	(_is40 ? mcs_table[_mcs].phy_rate_40 : mcs_table[_mcs].phy_rate_20))
-#define VALID_MCS(_mcs)	((_mcs < MCS_TABLE_SIZE))
+
+/* returns num tx streams - 1 */
+static inline u8 mcs_2_txstreams(u8 mcs)
+{
+	return (mcs_table[mcs].tx_phy_ctl3 & MCS_TXS_MASK) >> MCS_TXS_SHIFT;
+}
+
+static inline uint mcs_2_rate(u8 mcs, bool is40, bool sgi)
+{
+	if (sgi) {
+		if (is40)
+			return mcs_table[mcs].phy_rate_40_sgi;
+		return mcs_table[mcs].phy_rate_20_sgi;
+	}
+	if (is40)
+		return mcs_table[mcs].phy_rate_40;
+
+	return mcs_table[mcs].phy_rate_20;
+}
 
 /* Macro to use the rate_info table */
 #define	BRCMS_RATE_MASK_FULL 0xff /* Rate value mask with basic rate flag */
 
-/* convert 500kbps to bps */
-#define BRCMS_RATE_500K_TO_BPS(rate)	((rate) * 500000)
-
-/* rate spec : holds rate and mode specific information required to generate a tx frame. */
-/* Legacy CCK and OFDM information is held in the same manner as was done in the past    */
-/* (in the lower byte) the upper 3 bytes primarily hold MIMO specific information        */
+/*
+ * rate spec : holds rate and mode specific information required to generate a
+ * tx frame. Legacy CCK and OFDM information is held in the same manner as was
+ * done in the past (in the lower byte) the upper 3 bytes primarily hold MIMO
+ * specific information
+ */
 
 /* rate spec bit fields */
-#define RSPEC_RATE_MASK		0x0000007F	/* Either 500Kbps units or MIMO MCS idx */
-#define RSPEC_MIMORATE		0x08000000	/* mimo MCS is stored in RSPEC_RATE_MASK */
-#define RSPEC_BW_MASK		0x00000700	/* mimo bw mask */
-#define RSPEC_BW_SHIFT		8	/* mimo bw shift */
-#define RSPEC_STF_MASK		0x00003800	/* mimo Space/Time/Frequency mode mask */
-#define RSPEC_STF_SHIFT		11	/* mimo Space/Time/Frequency mode shift */
-#define RSPEC_CT_MASK		0x0000C000	/* mimo coding type mask */
-#define RSPEC_CT_SHIFT		14	/* mimo coding type shift */
-#define RSPEC_STC_MASK		0x00300000	/* mimo num STC streams per PLCP defn. */
-#define RSPEC_STC_SHIFT		20	/* mimo num STC streams per PLCP defn. */
-#define RSPEC_LDPC_CODING	0x00400000	/* mimo bit indicates adv coding in use */
-#define RSPEC_SHORT_GI		0x00800000	/* mimo bit indicates short GI in use */
-#define RSPEC_OVERRIDE		0x80000000	/* bit indicates override both rate & mode */
-#define RSPEC_OVERRIDE_MCS_ONLY 0x40000000	/* bit indicates override rate only */
-
-#define BRCMS_HTPHY		127	/* HT PHY Membership */
-
-#define RSPEC_ACTIVE(rspec)	(rspec & (RSPEC_RATE_MASK | RSPEC_MIMORATE))
-#define RSPEC2RATE(rspec)	((rspec & RSPEC_MIMORATE) ? \
-	MCS_RATE((rspec & RSPEC_RATE_MASK), RSPEC_IS40MHZ(rspec), RSPEC_ISSGI(rspec)) : \
-	(rspec & RSPEC_RATE_MASK))
-/* return rate in unit of 500Kbps -- for internal use in wlc_rate_sel.c */
-#define RSPEC2RATE500K(rspec)	((rspec & RSPEC_MIMORATE) ? \
-		MCS_RATE((rspec & RSPEC_RATE_MASK), state->is40bw, RSPEC_ISSGI(rspec))/500 : \
-		(rspec & RSPEC_RATE_MASK))
-#define CRSPEC2RATE500K(rspec)	((rspec & RSPEC_MIMORATE) ? \
-		MCS_RATE((rspec & RSPEC_RATE_MASK), RSPEC_IS40MHZ(rspec), RSPEC_ISSGI(rspec))/500 :\
-		(rspec & RSPEC_RATE_MASK))
-
-#define RSPEC2KBPS(rspec)	(IS_MCS(rspec) ? RSPEC2RATE(rspec) : RSPEC2RATE(rspec)*500)
-#define RSPEC_PHYTXBYTE2(rspec)	((rspec & 0xff00) >> 8)
-#define RSPEC_GET_BW(rspec)	((rspec & RSPEC_BW_MASK) >> RSPEC_BW_SHIFT)
-#define RSPEC_IS40MHZ(rspec)	((((rspec & RSPEC_BW_MASK) >> RSPEC_BW_SHIFT) == \
-				PHY_TXC1_BW_40MHZ) || (((rspec & RSPEC_BW_MASK) >> \
-				RSPEC_BW_SHIFT) == PHY_TXC1_BW_40MHZ_DUP))
-#define RSPEC_ISSGI(rspec)	((rspec & RSPEC_SHORT_GI) == RSPEC_SHORT_GI)
-#define RSPEC_MIMOPLCP3(rspec)	((rspec & 0xf00000) >> 16)
-#define PLCP3_ISSGI(plcp)	(plcp & (RSPEC_SHORT_GI >> 16))
-#define RSPEC_STC(rspec)	((rspec & RSPEC_STC_MASK) >> RSPEC_STC_SHIFT)
-#define RSPEC_STF(rspec)	((rspec & RSPEC_STF_MASK) >> RSPEC_STF_SHIFT)
-#define PLCP3_ISSTBC(plcp)	((plcp & (RSPEC_STC_MASK) >> 16) == 0x10)
-#define PLCP3_STC_MASK          0x30
-#define PLCP3_STC_SHIFT         4
-
-/* Rate info table; takes a legacy rate or ratespec_t */
-#define	IS_MCS(r)	(r & RSPEC_MIMORATE)
-#define	IS_OFDM(r)	(!IS_MCS(r) && (rate_info[(r) & RSPEC_RATE_MASK] & \
-					BRCMS_RATE_FLAG))
-#define	IS_CCK(r)	(!IS_MCS(r) && ( \
-			 ((r) & BRCMS_RATE_MASK) == BRCM_RATE_1M || \
-			 ((r) & BRCMS_RATE_MASK) == BRCM_RATE_2M || \
-			 ((r) & BRCMS_RATE_MASK) == BRCM_RATE_5M5 || \
-			 ((r) & BRCMS_RATE_MASK) == BRCM_RATE_11M))
-#define IS_SINGLE_STREAM(mcs)	(((mcs) <= HIGHEST_SINGLE_STREAM_MCS) || ((mcs) == 32))
-#define CCK_RSPEC(cck)		((cck) & RSPEC_RATE_MASK)
-#define OFDM_RSPEC(ofdm)	(((ofdm) & RSPEC_RATE_MASK) |\
-	(PHY_TXC1_MODE_CDD << RSPEC_STF_SHIFT))
-#define LEGACY_RSPEC(rate)	(IS_CCK(rate) ? CCK_RSPEC(rate) : OFDM_RSPEC(rate))
-
-#define MCS_RSPEC(mcs)		(((mcs) & RSPEC_RATE_MASK) | RSPEC_MIMORATE | \
-	(IS_SINGLE_STREAM(mcs) ? (PHY_TXC1_MODE_CDD << RSPEC_STF_SHIFT) : \
-	(PHY_TXC1_MODE_SDM << RSPEC_STF_SHIFT)))
-
-/* Convert encoded rate value in plcp header to numerical rates in 500 KHz increments */
+
+/* Either 500Kbps units or MIMO MCS idx */
+#define RSPEC_RATE_MASK		0x0000007F
+/* mimo MCS is stored in RSPEC_RATE_MASK */
+#define RSPEC_MIMORATE		0x08000000
+/* mimo bw mask */
+#define RSPEC_BW_MASK		0x00000700
+/* mimo bw shift */
+#define RSPEC_BW_SHIFT		8
+/* mimo Space/Time/Frequency mode mask */
+#define RSPEC_STF_MASK		0x00003800
+/* mimo Space/Time/Frequency mode shift */
+#define RSPEC_STF_SHIFT		11
+/* mimo coding type mask */
+#define RSPEC_CT_MASK		0x0000C000
+/* mimo coding type shift */
+#define RSPEC_CT_SHIFT		14
+/* mimo num STC streams per PLCP defn. */
+#define RSPEC_STC_MASK		0x00300000
+/* mimo num STC streams per PLCP defn. */
+#define RSPEC_STC_SHIFT		20
+/* mimo bit indicates adv coding in use */
+#define RSPEC_LDPC_CODING	0x00400000
+/* mimo bit indicates short GI in use */
+#define RSPEC_SHORT_GI		0x00800000
+/* bit indicates override both rate & mode */
+#define RSPEC_OVERRIDE		0x80000000
+/* bit indicates override rate only */
+#define RSPEC_OVERRIDE_MCS_ONLY 0x40000000
+
+static inline bool rspec_active(u32 rspec)
+{
+	return rspec & (RSPEC_RATE_MASK | RSPEC_MIMORATE);
+}
+
+static inline u8 rspec_phytxbyte2(u32 rspec)
+{
+	return (rspec & 0xff00) >> 8;
+}
+
+static inline u32 rspec_get_bw(u32 rspec)
+{
+	return (rspec & RSPEC_BW_MASK) >> RSPEC_BW_SHIFT;
+}
+
+static inline bool rspec_issgi(u32 rspec)
+{
+	return (rspec & RSPEC_SHORT_GI) == RSPEC_SHORT_GI;
+}
+
+static inline bool rspec_is40mhz(u32 rspec)
+{
+	u32 bw = rspec_get_bw(rspec);
+
+	return bw == PHY_TXC1_BW_40MHZ || bw == PHY_TXC1_BW_40MHZ_DUP;
+}
+
+static inline uint rspec2rate(u32 rspec)
+{
+	if (rspec & RSPEC_MIMORATE)
+		return mcs_2_rate(rspec & RSPEC_RATE_MASK, rspec_is40mhz(rspec),
+				  rspec_issgi(rspec));
+	return rspec & RSPEC_RATE_MASK;
+}
+
+static inline u8 rspec_mimoplcp3(u32 rspec)
+{
+	return (rspec & 0xf00000) >> 16;
+}
+
+static inline bool plcp3_issgi(u8 plcp)
+{
+	return (plcp & (RSPEC_SHORT_GI >> 16)) != 0;
+}
+
+static inline uint rspec_stc(u32 rspec)
+{
+	return (rspec & RSPEC_STC_MASK) >> RSPEC_STC_SHIFT;
+}
+
+static inline uint rspec_stf(u32 rspec)
+{
+	return (rspec & RSPEC_STF_MASK) >> RSPEC_STF_SHIFT;
+}
+
+static inline bool is_mcs_rate(u32 ratespec)
+{
+	return (ratespec & RSPEC_MIMORATE) != 0;
+}
+
+static inline bool is_ofdm_rate(u32 ratespec)
+{
+	return !is_mcs_rate(ratespec) &&
+	       (rate_info[ratespec & RSPEC_RATE_MASK] & BRCMS_RATE_FLAG);
+}
+
+static inline bool is_cck_rate(u32 ratespec)
+{
+	u32 rate = (ratespec & BRCMS_RATE_MASK);
+
+	return !is_mcs_rate(ratespec) && (
+			rate == BRCM_RATE_1M || rate == BRCM_RATE_2M ||
+			rate == BRCM_RATE_5M5 || rate == BRCM_RATE_11M);
+}
+
+static inline bool is_single_stream(u8 mcs)
+{
+	return mcs <= HIGHEST_SINGLE_STREAM_MCS || mcs == 32;
+}
+
+static inline u8 cck_rspec(u8 cck)
+{
+	return cck & RSPEC_RATE_MASK;
+}
+
+/* Convert encoded rate value in plcp header to numerical rates in 500 KHz
+ * increments */
 extern const u8 ofdm_rate_lookup[];
-#define OFDM_PHY2MAC_RATE(rlpt)		(ofdm_rate_lookup[rlpt & 0x7])
-#define CCK_PHY2MAC_RATE(signal)	(signal/5)
+
+static inline u8 ofdm_phy2mac_rate(u8 rlpt)
+{
+	return ofdm_rate_lookup[rlpt & 0x7];
+}
+
+static inline u8 cck_phy2mac_rate(u8 signal)
+{
+	return signal/5;
+}
 
 /* Rates specified in brcms_c_rateset_filter() */
 #define BRCMS_RATES_CCK_OFDM	0
 #define BRCMS_RATES_CCK		1
 #define BRCMS_RATES_OFDM		2
 
-/* sanitize, and sort a rateset with the basic bit(s) preserved, validate rateset */
+/* sanitize, and sort a rateset with the basic bit(s) preserved, validate
+ * rateset */
 extern bool
-brcms_c_rate_hwrs_filter_sort_validate(struct brcms_rateset *rs,
-				       const struct brcms_rateset *hw_rs,
+brcms_c_rate_hwrs_filter_sort_validate(struct brcms_c_rateset *rs,
+				       const struct brcms_c_rateset *hw_rs,
 				       bool check_brate, u8 txstreams);
 /* copy rateset src to dst as-is (no masking or sorting) */
-extern void brcms_c_rateset_copy(const struct brcms_rateset *src,
-			     struct brcms_rateset *dst);
+extern void brcms_c_rateset_copy(const struct brcms_c_rateset *src,
+			     struct brcms_c_rateset *dst);
 
 /* would be nice to have these documented ... */
-extern ratespec_t brcms_c_compute_rspec(struct d11rxhdr *rxh, u8 *plcp);
+extern u32 brcms_c_compute_rspec(struct d11rxhdr *rxh, u8 *plcp);
 
-extern void brcms_c_rateset_filter(struct brcms_rateset *src,
-	struct brcms_rateset *dst, bool basic_only, u8 rates, uint xmask,
+extern void brcms_c_rateset_filter(struct brcms_c_rateset *src,
+	struct brcms_c_rateset *dst, bool basic_only, u8 rates, uint xmask,
 	bool mcsallow);
 
 extern void
-brcms_c_rateset_default(struct brcms_rateset *rs_tgt,
-			const struct brcms_rateset *rs_hw, uint phy_type,
+brcms_c_rateset_default(struct brcms_c_rateset *rs_tgt,
+			const struct brcms_c_rateset *rs_hw, uint phy_type,
 			int bandtype, bool cck_only, uint rate_mask,
 			bool mcsallow, u8 bw, u8 txstreams);
 
 extern s16 brcms_c_rate_legacy_phyctl(uint rate);
 
-extern void brcms_c_rateset_mcs_upd(struct brcms_rateset *rs, u8 txstreams);
-extern void brcms_c_rateset_mcs_clear(struct brcms_rateset *rateset);
-extern void brcms_c_rateset_mcs_build(struct brcms_rateset *rateset,
+extern void brcms_c_rateset_mcs_upd(struct brcms_c_rateset *rs, u8 txstreams);
+extern void brcms_c_rateset_mcs_clear(struct brcms_c_rateset *rateset);
+extern void brcms_c_rateset_mcs_build(struct brcms_c_rateset *rateset,
 				      u8 txstreams);
-extern void brcms_c_rateset_bw_mcs_filter(struct brcms_rateset *rateset, u8 bw);
+extern void brcms_c_rateset_bw_mcs_filter(struct brcms_c_rateset *rateset,
+					  u8 bw);
 
 #endif				/* _BRCM_RATE_H_ */
diff --git a/drivers/staging/brcm80211/brcmsmac/scb.h b/drivers/staging/brcm80211/brcmsmac/scb.h
index d6c8328554d0..51c79c7239b7 100644
--- a/drivers/staging/brcm80211/brcmsmac/scb.h
+++ b/drivers/staging/brcm80211/brcmsmac/scb.h
@@ -23,17 +23,27 @@
 #include "types.h"
 
 #define AMPDU_TX_BA_MAX_WSIZE	64	/* max Tx ba window size (in pdu) */
+
+#define AMPDU_MAX_SCB_TID	NUMPRIO
+
+/* scb flags */
+#define SCB_WMECAP		0x0040
+#define SCB_HTCAP		0x10000	/* HT (MIMO) capable device */
+#define SCB_IS40		0x80000	/* 40MHz capable */
+#define SCB_STBCCAP		0x40000000	/* STBC Capable */
+
+#define SCB_MAGIC	0xbeefcafe
+
 /* structure to store per-tid state for the ampdu initiator */
 struct scb_ampdu_tid_ini {
-	u8 tx_in_transit;	/* number of pending mpdus in transit in driver */
-	u8 tid;		/* initiator tid for easy lookup */
-	u8 txretry[AMPDU_TX_BA_MAX_WSIZE];	/* tx retry count; indexed by seq modulo */
-	struct scb *scb;	/* backptr for easy lookup */
-	u8 ba_wsize;		/* negotiated ba window size (in pdu) */
+	u8 tx_in_transit; /* number of pending mpdus in transit in driver */
+	u8 tid;		  /* initiator tid for easy lookup */
+	/* tx retry count; indexed by seq modulo */
+	u8 txretry[AMPDU_TX_BA_MAX_WSIZE];
+	struct scb *scb;  /* backptr for easy lookup */
+	u8 ba_wsize;	  /* negotiated ba window size (in pdu) */
 };
 
-#define AMPDU_MAX_SCB_TID	NUMPRIO
-
 struct scb_ampdu {
 	struct scb *scb;	/* back pointer for easy reference */
 	u8 mpdu_density;	/* mpdu density */
@@ -41,45 +51,32 @@ struct scb_ampdu {
 	u8 release;		/* # of mpdus released at a time */
 	u16 min_len;		/* min mpdu len to support the density */
 	u32 max_rx_ampdu_bytes;	/* max ampdu rcv length; 8k, 16k, 32k, 64k */
-	struct pktq txq;	/* sdu transmit queue pending aggregation */
 
-	/* This could easily be a ini[] pointer and we keep this info in wl itself instead
-	 * of having mac80211 hold it for us.  Also could be made dynamic per tid instead of
-	 * static.
+	/*
+	 * This could easily be a ini[] pointer and we keep this info in wl
+	 * itself instead of having mac80211 hold it for us. Also could be made
+	 * dynamic per tid instead of static.
 	 */
 	/* initiator info - per tid (NUMPRIO): */
 	struct scb_ampdu_tid_ini ini[AMPDU_MAX_SCB_TID];
 };
 
-#define SCB_MAGIC	0xbeefcafe
-
 /* station control block - one per remote MAC address */
 struct scb {
 	u32 magic;
-	u32 flags;		/* various bit flags as defined below */
-	u32 flags2;		/* various bit flags2 as defined below */
-	u8 state;		/* current state bitfield of auth/assoc process */
+	u32 flags;	/* various bit flags as defined below */
+	u32 flags2;	/* various bit flags2 as defined below */
+	u8 state;	/* current state bitfield of auth/assoc process */
 	u8 ea[ETH_ALEN];	/* station address */
-	void *fragbuf[NUMPRIO];	/* defragmentation buffer per prio */
-	uint fragresid[NUMPRIO];	/* #bytes unused in frag buffer per prio */
+	uint fragresid[NUMPRIO];/* #bytes unused in frag buffer per prio */
 
 	u16 seqctl[NUMPRIO];	/* seqctl of last received frame (for dups) */
-	u16 seqctl_nonqos;	/* seqctl of last received frame (for dups) for
-				 * non-QoS data and management
-				 */
-	u16 seqnum[NUMPRIO];	/* WME: driver maintained sw seqnum per priority */
+	/* seqctl of last received frame (for dups) for non-QoS data and
+	 * management */
+	u16 seqctl_nonqos;
+	u16 seqnum[NUMPRIO];/* WME: driver maintained sw seqnum per priority */
 
 	struct scb_ampdu scb_ampdu;	/* AMPDU state including per tid info */
 };
 
-/* scb flags */
-#define SCB_WMECAP		0x0040	/* may ONLY be set if WME_ENAB(wlc) */
-#define SCB_HTCAP		0x10000	/* HT (MIMO) capable device */
-#define SCB_IS40		0x80000	/* 40MHz capable */
-#define SCB_STBCCAP		0x40000000	/* STBC Capable */
-#define SCB_WME(a)		((a)->flags & SCB_WMECAP)/* implies WME_ENAB */
-#define SCB_SEQNUM(scb, prio)	((scb)->seqnum[(prio)])
-#define SCB_PS(a)		NULL
-#define SCB_STBC_CAP(a)		((a)->flags & SCB_STBCCAP)
-#define SCB_AMPDU(a)		true
 #endif				/* _BRCM_SCB_H_ */
diff --git a/drivers/staging/brcm80211/brcmsmac/srom.c b/drivers/staging/brcm80211/brcmsmac/srom.c
index f39442ed4ce7..99f791048e84 100644
--- a/drivers/staging/brcm80211/brcmsmac/srom.c
+++ b/drivers/staging/brcm80211/brcmsmac/srom.c
@@ -18,24 +18,23 @@
 #include <linux/string.h>
 #include <linux/io.h>
 #include <linux/etherdevice.h>
+#include <linux/crc8.h>
 #include <stdarg.h>
 
 #include <chipcommon.h>
 #include <brcmu_utils.h>
+#include "pub.h"
 #include "nicpci.h"
 #include "aiutils.h"
 #include "otp.h"
 #include "srom.h"
 
-#define SROM_OFFSET(sih) ((sih->ccrev > 31) ? \
-	(((sih->cccaps & CC_CAP_SROM) == 0) ? NULL : \
-	 ((u8 *)curmap + PCI_16KB0_CCREGS_OFFSET + CC_SROM_OTP)) : \
-	((u8 *)curmap + PCI_BAR0_SPROM_OFFSET))
-
-#if defined(BCMDBG)
-#define WRITE_ENABLE_DELAY	500	/* 500 ms after write enable/disable toggle */
-#define WRITE_WORD_DELAY	20	/* 20 ms between each word write */
-#endif
+/*
+ * SROM CRC8 polynomial value:
+ *
+ * x^8 + x^7 +x^6 + x^4 + x^2 + 1
+ */
+#define SROM_CRC8_POLY		0xAB
 
 /* Maximum srom: 6 Kilobits == 768 bytes */
 #define	SROM_MAX		768
@@ -260,7 +259,8 @@
 /* Temp sense related entries */
 #define SROM8_MPWR_RAWTS		90
 #define SROM8_TS_SLP_OPT_CORRX	91
-/* FOC: freiquency offset correction, HWIQ: H/W IOCAL enable, IQSWP: IQ CAL swap disable */
+/* FOC: freiquency offset correction, HWIQ: H/W IOCAL enable,
+ * IQSWP: IQ CAL swap disable */
 #define SROM8_FOC_HWIQ_IQSWP	92
 
 /* Temperature delta for PHY calibration */
@@ -349,20 +349,53 @@
 #define SROM9_PO_LOFDM40DUP	203
 
 /* SROM flags (see sromvar_t) */
-#define SRFL_MORE	1	/* value continues as described by the next entry */
+
+/* value continues as described by the next entry */
+#define SRFL_MORE	1
 #define	SRFL_NOFFS	2	/* value bits can't be all one's */
 #define	SRFL_PRHEX	4	/* value is in hexdecimal format */
 #define	SRFL_PRSIGN	8	/* value is in signed decimal format */
 #define	SRFL_CCODE	0x10	/* value is in country code format */
 #define	SRFL_ETHADDR	0x20	/* value is an Ethernet address */
 #define SRFL_LEDDC	0x40	/* value is an LED duty cycle */
-#define SRFL_NOVAR	0x80	/* do not generate a nvram param, entry is for mfgc */
+/* do not generate a nvram param, entry is for mfgc */
+#define SRFL_NOVAR	0x80
 
 /* Max. nvram variable table size */
 #define	MAXSZ_NVRAM_VARS	4096
 
+/*
+ * indicates type of value.
+ */
+enum brcms_srom_var_type {
+	BRCMS_SROM_STRING,
+	BRCMS_SROM_SNUMBER,
+	BRCMS_SROM_UNUMBER
+};
+
+/*
+ * storage type for srom variable.
+ *
+ * var_list: for linked list operations.
+ * varid: identifier of the variable.
+ * var_type: type of variable.
+ * buf: variable value when var_type == BRCMS_SROM_STRING.
+ * uval: unsigned variable value when var_type == BRCMS_SROM_UNUMBER.
+ * sval: signed variable value when var_type == BRCMS_SROM_SNUMBER.
+ */
+struct brcms_srom_list_head {
+	struct list_head var_list;
+	enum brcms_srom_id varid;
+	enum brcms_srom_var_type var_type;
+	union {
+		char buf[0];
+		u32 uval;
+		s32 sval;
+	};
+};
+
 struct brcms_sromvar {
-	const char *name;
+	enum brcms_srom_id varid;
 	u32 revmask;
 	u32 flags;
 	u16 off;
@@ -375,615 +408,476 @@ struct brcms_varbuf {
 	unsigned int size;	/* current (residual) size in bytes */
 };
 
-/* Assumptions:
- * - Ethernet address spans across 3 consective words
+/*
+ * Assumptions:
+ * - Ethernet address spans across 3 consecutive words
  *
  * Table rules:
- * - Add multiple entries next to each other if a value spans across multiple words
- *   (even multiple fields in the same word) with each entry except the last having
- *   it's SRFL_MORE bit set.
- * - Ethernet address entry does not follow above rule and must not have SRFL_MORE
- *   bit set. Its SRFL_ETHADDR bit implies it takes multiple words.
- * - The last entry's name field must be NULL to indicate the end of the table. Other
- *   entries must have non-NULL name.
+ * - Add multiple entries next to each other if a value spans across multiple
+ *   words (even multiple fields in the same word) with each entry except the
+ *   last having it's SRFL_MORE bit set.
+ * - Ethernet address entry does not follow above rule and must not have
+ *   SRFL_MORE bit set. Its SRFL_ETHADDR bit implies it takes multiple words.
+ * - The last entry's name field must be NULL to indicate the end of the table.
+ *   Other entries must have non-NULL name.
  */
 static const struct brcms_sromvar pci_sromvars[] = {
-	{"devid", 0xffffff00, SRFL_PRHEX | SRFL_NOVAR, PCI_F0DEVID, 0xffff},
-	{"boardrev", 0x0000000e, SRFL_PRHEX, SROM_AABREV, SROM_BR_MASK},
-	{"boardrev", 0x000000f0, SRFL_PRHEX, SROM4_BREV, 0xffff},
-	{"boardrev", 0xffffff00, SRFL_PRHEX, SROM8_BREV, 0xffff},
-	{"boardflags", 0x00000002, SRFL_PRHEX, SROM_BFL, 0xffff},
-	{"boardflags", 0x00000004, SRFL_PRHEX | SRFL_MORE, SROM_BFL, 0xffff},
-	{"", 0, 0, SROM_BFL2, 0xffff},
-	{"boardflags", 0x00000008, SRFL_PRHEX | SRFL_MORE, SROM_BFL, 0xffff},
-	{"", 0, 0, SROM3_BFL2, 0xffff},
-	{"boardflags", 0x00000010, SRFL_PRHEX | SRFL_MORE, SROM4_BFL0, 0xffff},
-	{"", 0, 0, SROM4_BFL1, 0xffff},
-	{"boardflags", 0x000000e0, SRFL_PRHEX | SRFL_MORE, SROM5_BFL0, 0xffff},
-	{"", 0, 0, SROM5_BFL1, 0xffff},
-	{"boardflags", 0xffffff00, SRFL_PRHEX | SRFL_MORE, SROM8_BFL0, 0xffff},
-	{"", 0, 0, SROM8_BFL1, 0xffff},
-	{"boardflags2", 0x00000010, SRFL_PRHEX | SRFL_MORE, SROM4_BFL2, 0xffff},
-	{"", 0, 0, SROM4_BFL3, 0xffff},
-	{"boardflags2", 0x000000e0, SRFL_PRHEX | SRFL_MORE, SROM5_BFL2, 0xffff},
-	{"", 0, 0, SROM5_BFL3, 0xffff},
-	{"boardflags2", 0xffffff00, SRFL_PRHEX | SRFL_MORE, SROM8_BFL2, 0xffff},
-	{"", 0, 0, SROM8_BFL3, 0xffff},
-	{"boardtype", 0xfffffffc, SRFL_PRHEX, SROM_SSID, 0xffff},
-	{"boardnum", 0x00000006, 0, SROM_MACLO_IL0, 0xffff},
-	{"boardnum", 0x00000008, 0, SROM3_MACLO, 0xffff},
-	{"boardnum", 0x00000010, 0, SROM4_MACLO, 0xffff},
-	{"boardnum", 0x000000e0, 0, SROM5_MACLO, 0xffff},
-	{"boardnum", 0xffffff00, 0, SROM8_MACLO, 0xffff},
-	{"cc", 0x00000002, 0, SROM_AABREV, SROM_CC_MASK},
-	{"regrev", 0x00000008, 0, SROM_OPO, 0xff00},
-	{"regrev", 0x00000010, 0, SROM4_REGREV, 0x00ff},
-	{"regrev", 0x000000e0, 0, SROM5_REGREV, 0x00ff},
-	{"regrev", 0xffffff00, 0, SROM8_REGREV, 0x00ff},
-	{"ledbh0", 0x0000000e, SRFL_NOFFS, SROM_LEDBH10, 0x00ff},
-	{"ledbh1", 0x0000000e, SRFL_NOFFS, SROM_LEDBH10, 0xff00},
-	{"ledbh2", 0x0000000e, SRFL_NOFFS, SROM_LEDBH32, 0x00ff},
-	{"ledbh3", 0x0000000e, SRFL_NOFFS, SROM_LEDBH32, 0xff00},
-	{"ledbh0", 0x00000010, SRFL_NOFFS, SROM4_LEDBH10, 0x00ff},
-	{"ledbh1", 0x00000010, SRFL_NOFFS, SROM4_LEDBH10, 0xff00},
-	{"ledbh2", 0x00000010, SRFL_NOFFS, SROM4_LEDBH32, 0x00ff},
-	{"ledbh3", 0x00000010, SRFL_NOFFS, SROM4_LEDBH32, 0xff00},
-	{"ledbh0", 0x000000e0, SRFL_NOFFS, SROM5_LEDBH10, 0x00ff},
-	{"ledbh1", 0x000000e0, SRFL_NOFFS, SROM5_LEDBH10, 0xff00},
-	{"ledbh2", 0x000000e0, SRFL_NOFFS, SROM5_LEDBH32, 0x00ff},
-	{"ledbh3", 0x000000e0, SRFL_NOFFS, SROM5_LEDBH32, 0xff00},
-	{"ledbh0", 0xffffff00, SRFL_NOFFS, SROM8_LEDBH10, 0x00ff},
-	{"ledbh1", 0xffffff00, SRFL_NOFFS, SROM8_LEDBH10, 0xff00},
-	{"ledbh2", 0xffffff00, SRFL_NOFFS, SROM8_LEDBH32, 0x00ff},
-	{"ledbh3", 0xffffff00, SRFL_NOFFS, SROM8_LEDBH32, 0xff00},
-	{"pa0b0", 0x0000000e, SRFL_PRHEX, SROM_WL0PAB0, 0xffff},
-	{"pa0b1", 0x0000000e, SRFL_PRHEX, SROM_WL0PAB1, 0xffff},
-	{"pa0b2", 0x0000000e, SRFL_PRHEX, SROM_WL0PAB2, 0xffff},
-	{"pa0itssit", 0x0000000e, 0, SROM_ITT, 0x00ff},
-	{"pa0maxpwr", 0x0000000e, 0, SROM_WL10MAXP, 0x00ff},
-	{"pa0b0", 0xffffff00, SRFL_PRHEX, SROM8_W0_PAB0, 0xffff},
-	{"pa0b1", 0xffffff00, SRFL_PRHEX, SROM8_W0_PAB1, 0xffff},
-	{"pa0b2", 0xffffff00, SRFL_PRHEX, SROM8_W0_PAB2, 0xffff},
-	{"pa0itssit", 0xffffff00, 0, SROM8_W0_ITTMAXP, 0xff00},
-	{"pa0maxpwr", 0xffffff00, 0, SROM8_W0_ITTMAXP, 0x00ff},
-	{"opo", 0x0000000c, 0, SROM_OPO, 0x00ff},
-	{"opo", 0xffffff00, 0, SROM8_2G_OFDMPO, 0x00ff},
-	{"aa2g", 0x0000000e, 0, SROM_AABREV, SROM_AA0_MASK},
-	{"aa2g", 0x000000f0, 0, SROM4_AA, 0x00ff},
-	{"aa2g", 0xffffff00, 0, SROM8_AA, 0x00ff},
-	{"aa5g", 0x0000000e, 0, SROM_AABREV, SROM_AA1_MASK},
-	{"aa5g", 0x000000f0, 0, SROM4_AA, 0xff00},
-	{"aa5g", 0xffffff00, 0, SROM8_AA, 0xff00},
-	{"ag0", 0x0000000e, 0, SROM_AG10, 0x00ff},
-	{"ag1", 0x0000000e, 0, SROM_AG10, 0xff00},
-	{"ag0", 0x000000f0, 0, SROM4_AG10, 0x00ff},
-	{"ag1", 0x000000f0, 0, SROM4_AG10, 0xff00},
-	{"ag2", 0x000000f0, 0, SROM4_AG32, 0x00ff},
-	{"ag3", 0x000000f0, 0, SROM4_AG32, 0xff00},
-	{"ag0", 0xffffff00, 0, SROM8_AG10, 0x00ff},
-	{"ag1", 0xffffff00, 0, SROM8_AG10, 0xff00},
-	{"ag2", 0xffffff00, 0, SROM8_AG32, 0x00ff},
-	{"ag3", 0xffffff00, 0, SROM8_AG32, 0xff00},
-	{"pa1b0", 0x0000000e, SRFL_PRHEX, SROM_WL1PAB0, 0xffff},
-	{"pa1b1", 0x0000000e, SRFL_PRHEX, SROM_WL1PAB1, 0xffff},
-	{"pa1b2", 0x0000000e, SRFL_PRHEX, SROM_WL1PAB2, 0xffff},
-	{"pa1lob0", 0x0000000c, SRFL_PRHEX, SROM_WL1LPAB0, 0xffff},
-	{"pa1lob1", 0x0000000c, SRFL_PRHEX, SROM_WL1LPAB1, 0xffff},
-	{"pa1lob2", 0x0000000c, SRFL_PRHEX, SROM_WL1LPAB2, 0xffff},
-	{"pa1hib0", 0x0000000c, SRFL_PRHEX, SROM_WL1HPAB0, 0xffff},
-	{"pa1hib1", 0x0000000c, SRFL_PRHEX, SROM_WL1HPAB1, 0xffff},
-	{"pa1hib2", 0x0000000c, SRFL_PRHEX, SROM_WL1HPAB2, 0xffff},
-	{"pa1itssit", 0x0000000e, 0, SROM_ITT, 0xff00},
-	{"pa1maxpwr", 0x0000000e, 0, SROM_WL10MAXP, 0xff00},
-	{"pa1lomaxpwr", 0x0000000c, 0, SROM_WL1LHMAXP, 0xff00},
-	{"pa1himaxpwr", 0x0000000c, 0, SROM_WL1LHMAXP, 0x00ff},
-	{"pa1b0", 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB0, 0xffff},
-	{"pa1b1", 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB1, 0xffff},
-	{"pa1b2", 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB2, 0xffff},
-	{"pa1lob0", 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB0_LC, 0xffff},
-	{"pa1lob1", 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB1_LC, 0xffff},
-	{"pa1lob2", 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB2_LC, 0xffff},
-	{"pa1hib0", 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB0_HC, 0xffff},
-	{"pa1hib1", 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB1_HC, 0xffff},
-	{"pa1hib2", 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB2_HC, 0xffff},
-	{"pa1itssit", 0xffffff00, 0, SROM8_W1_ITTMAXP, 0xff00},
-	{"pa1maxpwr", 0xffffff00, 0, SROM8_W1_ITTMAXP, 0x00ff},
-	{"pa1lomaxpwr", 0xffffff00, 0, SROM8_W1_MAXP_LCHC, 0xff00},
-	{"pa1himaxpwr", 0xffffff00, 0, SROM8_W1_MAXP_LCHC, 0x00ff},
-	{"bxa2g", 0x00000008, 0, SROM_BXARSSI2G, 0x1800},
-	{"rssisav2g", 0x00000008, 0, SROM_BXARSSI2G, 0x0700},
-	{"rssismc2g", 0x00000008, 0, SROM_BXARSSI2G, 0x00f0},
-	{"rssismf2g", 0x00000008, 0, SROM_BXARSSI2G, 0x000f},
-	{"bxa2g", 0xffffff00, 0, SROM8_BXARSSI2G, 0x1800},
-	{"rssisav2g", 0xffffff00, 0, SROM8_BXARSSI2G, 0x0700},
-	{"rssismc2g", 0xffffff00, 0, SROM8_BXARSSI2G, 0x00f0},
-	{"rssismf2g", 0xffffff00, 0, SROM8_BXARSSI2G, 0x000f},
-	{"bxa5g", 0x00000008, 0, SROM_BXARSSI5G, 0x1800},
-	{"rssisav5g", 0x00000008, 0, SROM_BXARSSI5G, 0x0700},
-	{"rssismc5g", 0x00000008, 0, SROM_BXARSSI5G, 0x00f0},
-	{"rssismf5g", 0x00000008, 0, SROM_BXARSSI5G, 0x000f},
-	{"bxa5g", 0xffffff00, 0, SROM8_BXARSSI5G, 0x1800},
-	{"rssisav5g", 0xffffff00, 0, SROM8_BXARSSI5G, 0x0700},
-	{"rssismc5g", 0xffffff00, 0, SROM8_BXARSSI5G, 0x00f0},
-	{"rssismf5g", 0xffffff00, 0, SROM8_BXARSSI5G, 0x000f},
-	{"tri2g", 0x00000008, 0, SROM_TRI52G, 0x00ff},
-	{"tri5g", 0x00000008, 0, SROM_TRI52G, 0xff00},
-	{"tri5gl", 0x00000008, 0, SROM_TRI5GHL, 0x00ff},
-	{"tri5gh", 0x00000008, 0, SROM_TRI5GHL, 0xff00},
-	{"tri2g", 0xffffff00, 0, SROM8_TRI52G, 0x00ff},
-	{"tri5g", 0xffffff00, 0, SROM8_TRI52G, 0xff00},
-	{"tri5gl", 0xffffff00, 0, SROM8_TRI5GHL, 0x00ff},
-	{"tri5gh", 0xffffff00, 0, SROM8_TRI5GHL, 0xff00},
-	{"rxpo2g", 0x00000008, SRFL_PRSIGN, SROM_RXPO52G, 0x00ff},
-	{"rxpo5g", 0x00000008, SRFL_PRSIGN, SROM_RXPO52G, 0xff00},
-	{"rxpo2g", 0xffffff00, SRFL_PRSIGN, SROM8_RXPO52G, 0x00ff},
-	{"rxpo5g", 0xffffff00, SRFL_PRSIGN, SROM8_RXPO52G, 0xff00},
-	{"txchain", 0x000000f0, SRFL_NOFFS, SROM4_TXRXC, SROM4_TXCHAIN_MASK},
-	{"rxchain", 0x000000f0, SRFL_NOFFS, SROM4_TXRXC, SROM4_RXCHAIN_MASK},
-	{"antswitch", 0x000000f0, SRFL_NOFFS, SROM4_TXRXC, SROM4_SWITCH_MASK},
-	{"txchain", 0xffffff00, SRFL_NOFFS, SROM8_TXRXC, SROM4_TXCHAIN_MASK},
-	{"rxchain", 0xffffff00, SRFL_NOFFS, SROM8_TXRXC, SROM4_RXCHAIN_MASK},
-	{"antswitch", 0xffffff00, SRFL_NOFFS, SROM8_TXRXC, SROM4_SWITCH_MASK},
-	{"tssipos2g", 0xffffff00, 0, SROM8_FEM2G, SROM8_FEM_TSSIPOS_MASK},
-	{"extpagain2g", 0xffffff00, 0, SROM8_FEM2G, SROM8_FEM_EXTPA_GAIN_MASK},
-	{"pdetrange2g", 0xffffff00, 0, SROM8_FEM2G, SROM8_FEM_PDET_RANGE_MASK},
-	{"triso2g", 0xffffff00, 0, SROM8_FEM2G, SROM8_FEM_TR_ISO_MASK},
-	{"antswctl2g", 0xffffff00, 0, SROM8_FEM2G, SROM8_FEM_ANTSWLUT_MASK},
-	{"tssipos5g", 0xffffff00, 0, SROM8_FEM5G, SROM8_FEM_TSSIPOS_MASK},
-	{"extpagain5g", 0xffffff00, 0, SROM8_FEM5G, SROM8_FEM_EXTPA_GAIN_MASK},
-	{"pdetrange5g", 0xffffff00, 0, SROM8_FEM5G, SROM8_FEM_PDET_RANGE_MASK},
-	{"triso5g", 0xffffff00, 0, SROM8_FEM5G, SROM8_FEM_TR_ISO_MASK},
-	{"antswctl5g", 0xffffff00, 0, SROM8_FEM5G, SROM8_FEM_ANTSWLUT_MASK},
-	{"tempthresh", 0xffffff00, 0, SROM8_THERMAL, 0xff00},
-	{"tempoffset", 0xffffff00, 0, SROM8_THERMAL, 0x00ff},
-	{"txpid2ga0", 0x000000f0, 0, SROM4_TXPID2G, 0x00ff},
-	{"txpid2ga1", 0x000000f0, 0, SROM4_TXPID2G, 0xff00},
-	{"txpid2ga2", 0x000000f0, 0, SROM4_TXPID2G + 1, 0x00ff},
-	{"txpid2ga3", 0x000000f0, 0, SROM4_TXPID2G + 1, 0xff00},
-	{"txpid5ga0", 0x000000f0, 0, SROM4_TXPID5G, 0x00ff},
-	{"txpid5ga1", 0x000000f0, 0, SROM4_TXPID5G, 0xff00},
-	{"txpid5ga2", 0x000000f0, 0, SROM4_TXPID5G + 1, 0x00ff},
-	{"txpid5ga3", 0x000000f0, 0, SROM4_TXPID5G + 1, 0xff00},
-	{"txpid5gla0", 0x000000f0, 0, SROM4_TXPID5GL, 0x00ff},
-	{"txpid5gla1", 0x000000f0, 0, SROM4_TXPID5GL, 0xff00},
-	{"txpid5gla2", 0x000000f0, 0, SROM4_TXPID5GL + 1, 0x00ff},
-	{"txpid5gla3", 0x000000f0, 0, SROM4_TXPID5GL + 1, 0xff00},
-	{"txpid5gha0", 0x000000f0, 0, SROM4_TXPID5GH, 0x00ff},
-	{"txpid5gha1", 0x000000f0, 0, SROM4_TXPID5GH, 0xff00},
-	{"txpid5gha2", 0x000000f0, 0, SROM4_TXPID5GH + 1, 0x00ff},
-	{"txpid5gha3", 0x000000f0, 0, SROM4_TXPID5GH + 1, 0xff00},
-
-	{"ccode", 0x0000000f, SRFL_CCODE, SROM_CCODE, 0xffff},
-	{"ccode", 0x00000010, SRFL_CCODE, SROM4_CCODE, 0xffff},
-	{"ccode", 0x000000e0, SRFL_CCODE, SROM5_CCODE, 0xffff},
-	{"ccode", 0xffffff00, SRFL_CCODE, SROM8_CCODE, 0xffff},
-	{"macaddr", 0xffffff00, SRFL_ETHADDR, SROM8_MACHI, 0xffff},
-	{"macaddr", 0x000000e0, SRFL_ETHADDR, SROM5_MACHI, 0xffff},
-	{"macaddr", 0x00000010, SRFL_ETHADDR, SROM4_MACHI, 0xffff},
-	{"macaddr", 0x00000008, SRFL_ETHADDR, SROM3_MACHI, 0xffff},
-	{"il0macaddr", 0x00000007, SRFL_ETHADDR, SROM_MACHI_IL0, 0xffff},
-	{"et1macaddr", 0x00000007, SRFL_ETHADDR, SROM_MACHI_ET1, 0xffff},
-	{"leddc", 0xffffff00, SRFL_NOFFS | SRFL_LEDDC, SROM8_LEDDC, 0xffff},
-	{"leddc", 0x000000e0, SRFL_NOFFS | SRFL_LEDDC, SROM5_LEDDC, 0xffff},
-	{"leddc", 0x00000010, SRFL_NOFFS | SRFL_LEDDC, SROM4_LEDDC, 0xffff},
-	{"leddc", 0x00000008, SRFL_NOFFS | SRFL_LEDDC, SROM3_LEDDC, 0xffff},
-	{"rawtempsense", 0xffffff00, SRFL_PRHEX, SROM8_MPWR_RAWTS, 0x01ff},
-	{"measpower", 0xffffff00, SRFL_PRHEX, SROM8_MPWR_RAWTS, 0xfe00},
-	{"tempsense_slope", 0xffffff00, SRFL_PRHEX, SROM8_TS_SLP_OPT_CORRX,
+	{BRCMS_SROM_DEVID, 0xffffff00, SRFL_PRHEX | SRFL_NOVAR, PCI_F0DEVID,
+	 0xffff},
+	{BRCMS_SROM_BOARDREV, 0x0000000e, SRFL_PRHEX, SROM_AABREV,
+	 SROM_BR_MASK},
+	{BRCMS_SROM_BOARDREV, 0x000000f0, SRFL_PRHEX, SROM4_BREV, 0xffff},
+	{BRCMS_SROM_BOARDREV, 0xffffff00, SRFL_PRHEX, SROM8_BREV, 0xffff},
+	{BRCMS_SROM_BOARDFLAGS, 0x00000002, SRFL_PRHEX, SROM_BFL, 0xffff},
+	{BRCMS_SROM_BOARDFLAGS, 0x00000004, SRFL_PRHEX | SRFL_MORE, SROM_BFL,
+	 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM_BFL2, 0xffff},
+	{BRCMS_SROM_BOARDFLAGS, 0x00000008, SRFL_PRHEX | SRFL_MORE, SROM_BFL,
+	 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM3_BFL2, 0xffff},
+	{BRCMS_SROM_BOARDFLAGS, 0x00000010, SRFL_PRHEX | SRFL_MORE, SROM4_BFL0,
+	 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM4_BFL1, 0xffff},
+	{BRCMS_SROM_BOARDFLAGS, 0x000000e0, SRFL_PRHEX | SRFL_MORE, SROM5_BFL0,
+	 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM5_BFL1, 0xffff},
+	{BRCMS_SROM_BOARDFLAGS, 0xffffff00, SRFL_PRHEX | SRFL_MORE, SROM8_BFL0,
+	 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM8_BFL1, 0xffff},
+	{BRCMS_SROM_BOARDFLAGS2, 0x00000010, SRFL_PRHEX | SRFL_MORE, SROM4_BFL2,
+	 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM4_BFL3, 0xffff},
+	{BRCMS_SROM_BOARDFLAGS2, 0x000000e0, SRFL_PRHEX | SRFL_MORE, SROM5_BFL2,
+	 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM5_BFL3, 0xffff},
+	{BRCMS_SROM_BOARDFLAGS2, 0xffffff00, SRFL_PRHEX | SRFL_MORE, SROM8_BFL2,
+	 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM8_BFL3, 0xffff},
+	{BRCMS_SROM_BOARDTYPE, 0xfffffffc, SRFL_PRHEX, SROM_SSID, 0xffff},
+	{BRCMS_SROM_BOARDNUM, 0x00000006, 0, SROM_MACLO_IL0, 0xffff},
+	{BRCMS_SROM_BOARDNUM, 0x00000008, 0, SROM3_MACLO, 0xffff},
+	{BRCMS_SROM_BOARDNUM, 0x00000010, 0, SROM4_MACLO, 0xffff},
+	{BRCMS_SROM_BOARDNUM, 0x000000e0, 0, SROM5_MACLO, 0xffff},
+	{BRCMS_SROM_BOARDNUM, 0xffffff00, 0, SROM8_MACLO, 0xffff},
+	{BRCMS_SROM_CC, 0x00000002, 0, SROM_AABREV, SROM_CC_MASK},
+	{BRCMS_SROM_REGREV, 0x00000008, 0, SROM_OPO, 0xff00},
+	{BRCMS_SROM_REGREV, 0x00000010, 0, SROM4_REGREV, 0x00ff},
+	{BRCMS_SROM_REGREV, 0x000000e0, 0, SROM5_REGREV, 0x00ff},
+	{BRCMS_SROM_REGREV, 0xffffff00, 0, SROM8_REGREV, 0x00ff},
+	{BRCMS_SROM_LEDBH0, 0x0000000e, SRFL_NOFFS, SROM_LEDBH10, 0x00ff},
+	{BRCMS_SROM_LEDBH1, 0x0000000e, SRFL_NOFFS, SROM_LEDBH10, 0xff00},
+	{BRCMS_SROM_LEDBH2, 0x0000000e, SRFL_NOFFS, SROM_LEDBH32, 0x00ff},
+	{BRCMS_SROM_LEDBH3, 0x0000000e, SRFL_NOFFS, SROM_LEDBH32, 0xff00},
+	{BRCMS_SROM_LEDBH0, 0x00000010, SRFL_NOFFS, SROM4_LEDBH10, 0x00ff},
+	{BRCMS_SROM_LEDBH1, 0x00000010, SRFL_NOFFS, SROM4_LEDBH10, 0xff00},
+	{BRCMS_SROM_LEDBH2, 0x00000010, SRFL_NOFFS, SROM4_LEDBH32, 0x00ff},
+	{BRCMS_SROM_LEDBH3, 0x00000010, SRFL_NOFFS, SROM4_LEDBH32, 0xff00},
+	{BRCMS_SROM_LEDBH0, 0x000000e0, SRFL_NOFFS, SROM5_LEDBH10, 0x00ff},
+	{BRCMS_SROM_LEDBH1, 0x000000e0, SRFL_NOFFS, SROM5_LEDBH10, 0xff00},
+	{BRCMS_SROM_LEDBH2, 0x000000e0, SRFL_NOFFS, SROM5_LEDBH32, 0x00ff},
+	{BRCMS_SROM_LEDBH3, 0x000000e0, SRFL_NOFFS, SROM5_LEDBH32, 0xff00},
+	{BRCMS_SROM_LEDBH0, 0xffffff00, SRFL_NOFFS, SROM8_LEDBH10, 0x00ff},
+	{BRCMS_SROM_LEDBH1, 0xffffff00, SRFL_NOFFS, SROM8_LEDBH10, 0xff00},
+	{BRCMS_SROM_LEDBH2, 0xffffff00, SRFL_NOFFS, SROM8_LEDBH32, 0x00ff},
+	{BRCMS_SROM_LEDBH3, 0xffffff00, SRFL_NOFFS, SROM8_LEDBH32, 0xff00},
+	{BRCMS_SROM_PA0B0, 0x0000000e, SRFL_PRHEX, SROM_WL0PAB0, 0xffff},
+	{BRCMS_SROM_PA0B1, 0x0000000e, SRFL_PRHEX, SROM_WL0PAB1, 0xffff},
+	{BRCMS_SROM_PA0B2, 0x0000000e, SRFL_PRHEX, SROM_WL0PAB2, 0xffff},
+	{BRCMS_SROM_PA0ITSSIT, 0x0000000e, 0, SROM_ITT, 0x00ff},
+	{BRCMS_SROM_PA0MAXPWR, 0x0000000e, 0, SROM_WL10MAXP, 0x00ff},
+	{BRCMS_SROM_PA0B0, 0xffffff00, SRFL_PRHEX, SROM8_W0_PAB0, 0xffff},
+	{BRCMS_SROM_PA0B1, 0xffffff00, SRFL_PRHEX, SROM8_W0_PAB1, 0xffff},
+	{BRCMS_SROM_PA0B2, 0xffffff00, SRFL_PRHEX, SROM8_W0_PAB2, 0xffff},
+	{BRCMS_SROM_PA0ITSSIT, 0xffffff00, 0, SROM8_W0_ITTMAXP, 0xff00},
+	{BRCMS_SROM_PA0MAXPWR, 0xffffff00, 0, SROM8_W0_ITTMAXP, 0x00ff},
+	{BRCMS_SROM_OPO, 0x0000000c, 0, SROM_OPO, 0x00ff},
+	{BRCMS_SROM_OPO, 0xffffff00, 0, SROM8_2G_OFDMPO, 0x00ff},
+	{BRCMS_SROM_AA2G, 0x0000000e, 0, SROM_AABREV, SROM_AA0_MASK},
+	{BRCMS_SROM_AA2G, 0x000000f0, 0, SROM4_AA, 0x00ff},
+	{BRCMS_SROM_AA2G, 0xffffff00, 0, SROM8_AA, 0x00ff},
+	{BRCMS_SROM_AA5G, 0x0000000e, 0, SROM_AABREV, SROM_AA1_MASK},
+	{BRCMS_SROM_AA5G, 0x000000f0, 0, SROM4_AA, 0xff00},
+	{BRCMS_SROM_AA5G, 0xffffff00, 0, SROM8_AA, 0xff00},
+	{BRCMS_SROM_AG0, 0x0000000e, 0, SROM_AG10, 0x00ff},
+	{BRCMS_SROM_AG1, 0x0000000e, 0, SROM_AG10, 0xff00},
+	{BRCMS_SROM_AG0, 0x000000f0, 0, SROM4_AG10, 0x00ff},
+	{BRCMS_SROM_AG1, 0x000000f0, 0, SROM4_AG10, 0xff00},
+	{BRCMS_SROM_AG2, 0x000000f0, 0, SROM4_AG32, 0x00ff},
+	{BRCMS_SROM_AG3, 0x000000f0, 0, SROM4_AG32, 0xff00},
+	{BRCMS_SROM_AG0, 0xffffff00, 0, SROM8_AG10, 0x00ff},
+	{BRCMS_SROM_AG1, 0xffffff00, 0, SROM8_AG10, 0xff00},
+	{BRCMS_SROM_AG2, 0xffffff00, 0, SROM8_AG32, 0x00ff},
+	{BRCMS_SROM_AG3, 0xffffff00, 0, SROM8_AG32, 0xff00},
+	{BRCMS_SROM_PA1B0, 0x0000000e, SRFL_PRHEX, SROM_WL1PAB0, 0xffff},
+	{BRCMS_SROM_PA1B1, 0x0000000e, SRFL_PRHEX, SROM_WL1PAB1, 0xffff},
+	{BRCMS_SROM_PA1B2, 0x0000000e, SRFL_PRHEX, SROM_WL1PAB2, 0xffff},
+	{BRCMS_SROM_PA1LOB0, 0x0000000c, SRFL_PRHEX, SROM_WL1LPAB0, 0xffff},
+	{BRCMS_SROM_PA1LOB1, 0x0000000c, SRFL_PRHEX, SROM_WL1LPAB1, 0xffff},
+	{BRCMS_SROM_PA1LOB2, 0x0000000c, SRFL_PRHEX, SROM_WL1LPAB2, 0xffff},
+	{BRCMS_SROM_PA1HIB0, 0x0000000c, SRFL_PRHEX, SROM_WL1HPAB0, 0xffff},
+	{BRCMS_SROM_PA1HIB1, 0x0000000c, SRFL_PRHEX, SROM_WL1HPAB1, 0xffff},
+	{BRCMS_SROM_PA1HIB2, 0x0000000c, SRFL_PRHEX, SROM_WL1HPAB2, 0xffff},
+	{BRCMS_SROM_PA1ITSSIT, 0x0000000e, 0, SROM_ITT, 0xff00},
+	{BRCMS_SROM_PA1MAXPWR, 0x0000000e, 0, SROM_WL10MAXP, 0xff00},
+	{BRCMS_SROM_PA1LOMAXPWR, 0x0000000c, 0, SROM_WL1LHMAXP, 0xff00},
+	{BRCMS_SROM_PA1HIMAXPWR, 0x0000000c, 0, SROM_WL1LHMAXP, 0x00ff},
+	{BRCMS_SROM_PA1B0, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB0, 0xffff},
+	{BRCMS_SROM_PA1B1, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB1, 0xffff},
+	{BRCMS_SROM_PA1B2, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB2, 0xffff},
+	{BRCMS_SROM_PA1LOB0, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB0_LC, 0xffff},
+	{BRCMS_SROM_PA1LOB1, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB1_LC, 0xffff},
+	{BRCMS_SROM_PA1LOB2, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB2_LC, 0xffff},
+	{BRCMS_SROM_PA1HIB0, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB0_HC, 0xffff},
+	{BRCMS_SROM_PA1HIB1, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB1_HC, 0xffff},
+	{BRCMS_SROM_PA1HIB2, 0xffffff00, SRFL_PRHEX, SROM8_W1_PAB2_HC, 0xffff},
+	{BRCMS_SROM_PA1ITSSIT, 0xffffff00, 0, SROM8_W1_ITTMAXP, 0xff00},
+	{BRCMS_SROM_PA1MAXPWR, 0xffffff00, 0, SROM8_W1_ITTMAXP, 0x00ff},
+	{BRCMS_SROM_PA1LOMAXPWR, 0xffffff00, 0, SROM8_W1_MAXP_LCHC, 0xff00},
+	{BRCMS_SROM_PA1HIMAXPWR, 0xffffff00, 0, SROM8_W1_MAXP_LCHC, 0x00ff},
+	{BRCMS_SROM_BXA2G, 0x00000008, 0, SROM_BXARSSI2G, 0x1800},
+	{BRCMS_SROM_RSSISAV2G, 0x00000008, 0, SROM_BXARSSI2G, 0x0700},
+	{BRCMS_SROM_RSSISMC2G, 0x00000008, 0, SROM_BXARSSI2G, 0x00f0},
+	{BRCMS_SROM_RSSISMF2G, 0x00000008, 0, SROM_BXARSSI2G, 0x000f},
+	{BRCMS_SROM_BXA2G, 0xffffff00, 0, SROM8_BXARSSI2G, 0x1800},
+	{BRCMS_SROM_RSSISAV2G, 0xffffff00, 0, SROM8_BXARSSI2G, 0x0700},
+	{BRCMS_SROM_RSSISMC2G, 0xffffff00, 0, SROM8_BXARSSI2G, 0x00f0},
+	{BRCMS_SROM_RSSISMF2G, 0xffffff00, 0, SROM8_BXARSSI2G, 0x000f},
+	{BRCMS_SROM_BXA5G, 0x00000008, 0, SROM_BXARSSI5G, 0x1800},
+	{BRCMS_SROM_RSSISAV5G, 0x00000008, 0, SROM_BXARSSI5G, 0x0700},
+	{BRCMS_SROM_RSSISMC5G, 0x00000008, 0, SROM_BXARSSI5G, 0x00f0},
+	{BRCMS_SROM_RSSISMF5G, 0x00000008, 0, SROM_BXARSSI5G, 0x000f},
+	{BRCMS_SROM_BXA5G, 0xffffff00, 0, SROM8_BXARSSI5G, 0x1800},
+	{BRCMS_SROM_RSSISAV5G, 0xffffff00, 0, SROM8_BXARSSI5G, 0x0700},
+	{BRCMS_SROM_RSSISMC5G, 0xffffff00, 0, SROM8_BXARSSI5G, 0x00f0},
+	{BRCMS_SROM_RSSISMF5G, 0xffffff00, 0, SROM8_BXARSSI5G, 0x000f},
+	{BRCMS_SROM_TRI2G, 0x00000008, 0, SROM_TRI52G, 0x00ff},
+	{BRCMS_SROM_TRI5G, 0x00000008, 0, SROM_TRI52G, 0xff00},
+	{BRCMS_SROM_TRI5GL, 0x00000008, 0, SROM_TRI5GHL, 0x00ff},
+	{BRCMS_SROM_TRI5GH, 0x00000008, 0, SROM_TRI5GHL, 0xff00},
+	{BRCMS_SROM_TRI2G, 0xffffff00, 0, SROM8_TRI52G, 0x00ff},
+	{BRCMS_SROM_TRI5G, 0xffffff00, 0, SROM8_TRI52G, 0xff00},
+	{BRCMS_SROM_TRI5GL, 0xffffff00, 0, SROM8_TRI5GHL, 0x00ff},
+	{BRCMS_SROM_TRI5GH, 0xffffff00, 0, SROM8_TRI5GHL, 0xff00},
+	{BRCMS_SROM_RXPO2G, 0x00000008, SRFL_PRSIGN, SROM_RXPO52G, 0x00ff},
+	{BRCMS_SROM_RXPO5G, 0x00000008, SRFL_PRSIGN, SROM_RXPO52G, 0xff00},
+	{BRCMS_SROM_RXPO2G, 0xffffff00, SRFL_PRSIGN, SROM8_RXPO52G, 0x00ff},
+	{BRCMS_SROM_RXPO5G, 0xffffff00, SRFL_PRSIGN, SROM8_RXPO52G, 0xff00},
+	{BRCMS_SROM_TXCHAIN, 0x000000f0, SRFL_NOFFS, SROM4_TXRXC,
+	 SROM4_TXCHAIN_MASK},
+	{BRCMS_SROM_RXCHAIN, 0x000000f0, SRFL_NOFFS, SROM4_TXRXC,
+	 SROM4_RXCHAIN_MASK},
+	{BRCMS_SROM_ANTSWITCH, 0x000000f0, SRFL_NOFFS, SROM4_TXRXC,
+	 SROM4_SWITCH_MASK},
+	{BRCMS_SROM_TXCHAIN, 0xffffff00, SRFL_NOFFS, SROM8_TXRXC,
+	 SROM4_TXCHAIN_MASK},
+	{BRCMS_SROM_RXCHAIN, 0xffffff00, SRFL_NOFFS, SROM8_TXRXC,
+	 SROM4_RXCHAIN_MASK},
+	{BRCMS_SROM_ANTSWITCH, 0xffffff00, SRFL_NOFFS, SROM8_TXRXC,
+	 SROM4_SWITCH_MASK},
+	{BRCMS_SROM_TSSIPOS2G, 0xffffff00, 0, SROM8_FEM2G,
+	 SROM8_FEM_TSSIPOS_MASK},
+	{BRCMS_SROM_EXTPAGAIN2G, 0xffffff00, 0, SROM8_FEM2G,
+	 SROM8_FEM_EXTPA_GAIN_MASK},
+	{BRCMS_SROM_PDETRANGE2G, 0xffffff00, 0, SROM8_FEM2G,
+	 SROM8_FEM_PDET_RANGE_MASK},
+	{BRCMS_SROM_TRISO2G, 0xffffff00, 0, SROM8_FEM2G, SROM8_FEM_TR_ISO_MASK},
+	{BRCMS_SROM_ANTSWCTL2G, 0xffffff00, 0, SROM8_FEM2G,
+	 SROM8_FEM_ANTSWLUT_MASK},
+	{BRCMS_SROM_TSSIPOS5G, 0xffffff00, 0, SROM8_FEM5G,
+	 SROM8_FEM_TSSIPOS_MASK},
+	{BRCMS_SROM_EXTPAGAIN5G, 0xffffff00, 0, SROM8_FEM5G,
+	 SROM8_FEM_EXTPA_GAIN_MASK},
+	{BRCMS_SROM_PDETRANGE5G, 0xffffff00, 0, SROM8_FEM5G,
+	 SROM8_FEM_PDET_RANGE_MASK},
+	{BRCMS_SROM_TRISO5G, 0xffffff00, 0, SROM8_FEM5G, SROM8_FEM_TR_ISO_MASK},
+	{BRCMS_SROM_ANTSWCTL5G, 0xffffff00, 0, SROM8_FEM5G,
+	 SROM8_FEM_ANTSWLUT_MASK},
+	{BRCMS_SROM_TEMPTHRESH, 0xffffff00, 0, SROM8_THERMAL, 0xff00},
+	{BRCMS_SROM_TEMPOFFSET, 0xffffff00, 0, SROM8_THERMAL, 0x00ff},
+	{BRCMS_SROM_TXPID2GA0, 0x000000f0, 0, SROM4_TXPID2G, 0x00ff},
+	{BRCMS_SROM_TXPID2GA1, 0x000000f0, 0, SROM4_TXPID2G, 0xff00},
+	{BRCMS_SROM_TXPID2GA2, 0x000000f0, 0, SROM4_TXPID2G + 1, 0x00ff},
+	{BRCMS_SROM_TXPID2GA3, 0x000000f0, 0, SROM4_TXPID2G + 1, 0xff00},
+	{BRCMS_SROM_TXPID5GA0, 0x000000f0, 0, SROM4_TXPID5G, 0x00ff},
+	{BRCMS_SROM_TXPID5GA1, 0x000000f0, 0, SROM4_TXPID5G, 0xff00},
+	{BRCMS_SROM_TXPID5GA2, 0x000000f0, 0, SROM4_TXPID5G + 1, 0x00ff},
+	{BRCMS_SROM_TXPID5GA3, 0x000000f0, 0, SROM4_TXPID5G + 1, 0xff00},
+	{BRCMS_SROM_TXPID5GLA0, 0x000000f0, 0, SROM4_TXPID5GL, 0x00ff},
+	{BRCMS_SROM_TXPID5GLA1, 0x000000f0, 0, SROM4_TXPID5GL, 0xff00},
+	{BRCMS_SROM_TXPID5GLA2, 0x000000f0, 0, SROM4_TXPID5GL + 1, 0x00ff},
+	{BRCMS_SROM_TXPID5GLA3, 0x000000f0, 0, SROM4_TXPID5GL + 1, 0xff00},
+	{BRCMS_SROM_TXPID5GHA0, 0x000000f0, 0, SROM4_TXPID5GH, 0x00ff},
+	{BRCMS_SROM_TXPID5GHA1, 0x000000f0, 0, SROM4_TXPID5GH, 0xff00},
+	{BRCMS_SROM_TXPID5GHA2, 0x000000f0, 0, SROM4_TXPID5GH + 1, 0x00ff},
+	{BRCMS_SROM_TXPID5GHA3, 0x000000f0, 0, SROM4_TXPID5GH + 1, 0xff00},
+
+	{BRCMS_SROM_CCODE, 0x0000000f, SRFL_CCODE, SROM_CCODE, 0xffff},
+	{BRCMS_SROM_CCODE, 0x00000010, SRFL_CCODE, SROM4_CCODE, 0xffff},
+	{BRCMS_SROM_CCODE, 0x000000e0, SRFL_CCODE, SROM5_CCODE, 0xffff},
+	{BRCMS_SROM_CCODE, 0xffffff00, SRFL_CCODE, SROM8_CCODE, 0xffff},
+	{BRCMS_SROM_MACADDR, 0xffffff00, SRFL_ETHADDR, SROM8_MACHI, 0xffff},
+	{BRCMS_SROM_MACADDR, 0x000000e0, SRFL_ETHADDR, SROM5_MACHI, 0xffff},
+	{BRCMS_SROM_MACADDR, 0x00000010, SRFL_ETHADDR, SROM4_MACHI, 0xffff},
+	{BRCMS_SROM_MACADDR, 0x00000008, SRFL_ETHADDR, SROM3_MACHI, 0xffff},
+	{BRCMS_SROM_IL0MACADDR, 0x00000007, SRFL_ETHADDR, SROM_MACHI_IL0,
+	 0xffff},
+	{BRCMS_SROM_ET1MACADDR, 0x00000007, SRFL_ETHADDR, SROM_MACHI_ET1,
+	 0xffff},
+	{BRCMS_SROM_LEDDC, 0xffffff00, SRFL_NOFFS | SRFL_LEDDC, SROM8_LEDDC,
+	 0xffff},
+	{BRCMS_SROM_LEDDC, 0x000000e0, SRFL_NOFFS | SRFL_LEDDC, SROM5_LEDDC,
+	 0xffff},
+	{BRCMS_SROM_LEDDC, 0x00000010, SRFL_NOFFS | SRFL_LEDDC, SROM4_LEDDC,
+	 0xffff},
+	{BRCMS_SROM_LEDDC, 0x00000008, SRFL_NOFFS | SRFL_LEDDC, SROM3_LEDDC,
+	 0xffff},
+	{BRCMS_SROM_RAWTEMPSENSE, 0xffffff00, SRFL_PRHEX, SROM8_MPWR_RAWTS,
+	 0x01ff},
+	{BRCMS_SROM_MEASPOWER, 0xffffff00, SRFL_PRHEX, SROM8_MPWR_RAWTS,
+	 0xfe00},
+	{BRCMS_SROM_TEMPSENSE_SLOPE, 0xffffff00, SRFL_PRHEX,
+	 SROM8_TS_SLP_OPT_CORRX, 0x00ff},
+	{BRCMS_SROM_TEMPCORRX, 0xffffff00, SRFL_PRHEX, SROM8_TS_SLP_OPT_CORRX,
+	 0xfc00},
+	{BRCMS_SROM_TEMPSENSE_OPTION, 0xffffff00, SRFL_PRHEX,
+	 SROM8_TS_SLP_OPT_CORRX, 0x0300},
+	{BRCMS_SROM_FREQOFFSET_CORR, 0xffffff00, SRFL_PRHEX,
+	 SROM8_FOC_HWIQ_IQSWP, 0x000f},
+	{BRCMS_SROM_IQCAL_SWP_DIS, 0xffffff00, SRFL_PRHEX, SROM8_FOC_HWIQ_IQSWP,
+	 0x0010},
+	{BRCMS_SROM_HW_IQCAL_EN, 0xffffff00, SRFL_PRHEX, SROM8_FOC_HWIQ_IQSWP,
+	 0x0020},
+	{BRCMS_SROM_PHYCAL_TEMPDELTA, 0xffffff00, 0, SROM8_PHYCAL_TEMPDELTA,
 	 0x00ff},
-	{"tempcorrx", 0xffffff00, SRFL_PRHEX, SROM8_TS_SLP_OPT_CORRX, 0xfc00},
-	{"tempsense_option", 0xffffff00, SRFL_PRHEX, SROM8_TS_SLP_OPT_CORRX,
-	 0x0300},
-	{"freqoffset_corr", 0xffffff00, SRFL_PRHEX, SROM8_FOC_HWIQ_IQSWP,
-	 0x000f},
-	{"iqcal_swp_dis", 0xffffff00, SRFL_PRHEX, SROM8_FOC_HWIQ_IQSWP, 0x0010},
-	{"hw_iqcal_en", 0xffffff00, SRFL_PRHEX, SROM8_FOC_HWIQ_IQSWP, 0x0020},
-	{"phycal_tempdelta", 0xffffff00, 0, SROM8_PHYCAL_TEMPDELTA, 0x00ff},
-
-	{"cck2gpo", 0x000000f0, 0, SROM4_2G_CCKPO, 0xffff},
-	{"cck2gpo", 0x00000100, 0, SROM8_2G_CCKPO, 0xffff},
-	{"ofdm2gpo", 0x000000f0, SRFL_MORE, SROM4_2G_OFDMPO, 0xffff},
-	{"", 0, 0, SROM4_2G_OFDMPO + 1, 0xffff},
-	{"ofdm5gpo", 0x000000f0, SRFL_MORE, SROM4_5G_OFDMPO, 0xffff},
-	{"", 0, 0, SROM4_5G_OFDMPO + 1, 0xffff},
-	{"ofdm5glpo", 0x000000f0, SRFL_MORE, SROM4_5GL_OFDMPO, 0xffff},
-	{"", 0, 0, SROM4_5GL_OFDMPO + 1, 0xffff},
-	{"ofdm5ghpo", 0x000000f0, SRFL_MORE, SROM4_5GH_OFDMPO, 0xffff},
-	{"", 0, 0, SROM4_5GH_OFDMPO + 1, 0xffff},
-	{"ofdm2gpo", 0x00000100, SRFL_MORE, SROM8_2G_OFDMPO, 0xffff},
-	{"", 0, 0, SROM8_2G_OFDMPO + 1, 0xffff},
-	{"ofdm5gpo", 0x00000100, SRFL_MORE, SROM8_5G_OFDMPO, 0xffff},
-	{"", 0, 0, SROM8_5G_OFDMPO + 1, 0xffff},
-	{"ofdm5glpo", 0x00000100, SRFL_MORE, SROM8_5GL_OFDMPO, 0xffff},
-	{"", 0, 0, SROM8_5GL_OFDMPO + 1, 0xffff},
-	{"ofdm5ghpo", 0x00000100, SRFL_MORE, SROM8_5GH_OFDMPO, 0xffff},
-	{"", 0, 0, SROM8_5GH_OFDMPO + 1, 0xffff},
-	{"mcs2gpo0", 0x000000f0, 0, SROM4_2G_MCSPO, 0xffff},
-	{"mcs2gpo1", 0x000000f0, 0, SROM4_2G_MCSPO + 1, 0xffff},
-	{"mcs2gpo2", 0x000000f0, 0, SROM4_2G_MCSPO + 2, 0xffff},
-	{"mcs2gpo3", 0x000000f0, 0, SROM4_2G_MCSPO + 3, 0xffff},
-	{"mcs2gpo4", 0x000000f0, 0, SROM4_2G_MCSPO + 4, 0xffff},
-	{"mcs2gpo5", 0x000000f0, 0, SROM4_2G_MCSPO + 5, 0xffff},
-	{"mcs2gpo6", 0x000000f0, 0, SROM4_2G_MCSPO + 6, 0xffff},
-	{"mcs2gpo7", 0x000000f0, 0, SROM4_2G_MCSPO + 7, 0xffff},
-	{"mcs5gpo0", 0x000000f0, 0, SROM4_5G_MCSPO, 0xffff},
-	{"mcs5gpo1", 0x000000f0, 0, SROM4_5G_MCSPO + 1, 0xffff},
-	{"mcs5gpo2", 0x000000f0, 0, SROM4_5G_MCSPO + 2, 0xffff},
-	{"mcs5gpo3", 0x000000f0, 0, SROM4_5G_MCSPO + 3, 0xffff},
-	{"mcs5gpo4", 0x000000f0, 0, SROM4_5G_MCSPO + 4, 0xffff},
-	{"mcs5gpo5", 0x000000f0, 0, SROM4_5G_MCSPO + 5, 0xffff},
-	{"mcs5gpo6", 0x000000f0, 0, SROM4_5G_MCSPO + 6, 0xffff},
-	{"mcs5gpo7", 0x000000f0, 0, SROM4_5G_MCSPO + 7, 0xffff},
-	{"mcs5glpo0", 0x000000f0, 0, SROM4_5GL_MCSPO, 0xffff},
-	{"mcs5glpo1", 0x000000f0, 0, SROM4_5GL_MCSPO + 1, 0xffff},
-	{"mcs5glpo2", 0x000000f0, 0, SROM4_5GL_MCSPO + 2, 0xffff},
-	{"mcs5glpo3", 0x000000f0, 0, SROM4_5GL_MCSPO + 3, 0xffff},
-	{"mcs5glpo4", 0x000000f0, 0, SROM4_5GL_MCSPO + 4, 0xffff},
-	{"mcs5glpo5", 0x000000f0, 0, SROM4_5GL_MCSPO + 5, 0xffff},
-	{"mcs5glpo6", 0x000000f0, 0, SROM4_5GL_MCSPO + 6, 0xffff},
-	{"mcs5glpo7", 0x000000f0, 0, SROM4_5GL_MCSPO + 7, 0xffff},
-	{"mcs5ghpo0", 0x000000f0, 0, SROM4_5GH_MCSPO, 0xffff},
-	{"mcs5ghpo1", 0x000000f0, 0, SROM4_5GH_MCSPO + 1, 0xffff},
-	{"mcs5ghpo2", 0x000000f0, 0, SROM4_5GH_MCSPO + 2, 0xffff},
-	{"mcs5ghpo3", 0x000000f0, 0, SROM4_5GH_MCSPO + 3, 0xffff},
-	{"mcs5ghpo4", 0x000000f0, 0, SROM4_5GH_MCSPO + 4, 0xffff},
-	{"mcs5ghpo5", 0x000000f0, 0, SROM4_5GH_MCSPO + 5, 0xffff},
-	{"mcs5ghpo6", 0x000000f0, 0, SROM4_5GH_MCSPO + 6, 0xffff},
-	{"mcs5ghpo7", 0x000000f0, 0, SROM4_5GH_MCSPO + 7, 0xffff},
-	{"mcs2gpo0", 0x00000100, 0, SROM8_2G_MCSPO, 0xffff},
-	{"mcs2gpo1", 0x00000100, 0, SROM8_2G_MCSPO + 1, 0xffff},
-	{"mcs2gpo2", 0x00000100, 0, SROM8_2G_MCSPO + 2, 0xffff},
-	{"mcs2gpo3", 0x00000100, 0, SROM8_2G_MCSPO + 3, 0xffff},
-	{"mcs2gpo4", 0x00000100, 0, SROM8_2G_MCSPO + 4, 0xffff},
-	{"mcs2gpo5", 0x00000100, 0, SROM8_2G_MCSPO + 5, 0xffff},
-	{"mcs2gpo6", 0x00000100, 0, SROM8_2G_MCSPO + 6, 0xffff},
-	{"mcs2gpo7", 0x00000100, 0, SROM8_2G_MCSPO + 7, 0xffff},
-	{"mcs5gpo0", 0x00000100, 0, SROM8_5G_MCSPO, 0xffff},
-	{"mcs5gpo1", 0x00000100, 0, SROM8_5G_MCSPO + 1, 0xffff},
-	{"mcs5gpo2", 0x00000100, 0, SROM8_5G_MCSPO + 2, 0xffff},
-	{"mcs5gpo3", 0x00000100, 0, SROM8_5G_MCSPO + 3, 0xffff},
-	{"mcs5gpo4", 0x00000100, 0, SROM8_5G_MCSPO + 4, 0xffff},
-	{"mcs5gpo5", 0x00000100, 0, SROM8_5G_MCSPO + 5, 0xffff},
-	{"mcs5gpo6", 0x00000100, 0, SROM8_5G_MCSPO + 6, 0xffff},
-	{"mcs5gpo7", 0x00000100, 0, SROM8_5G_MCSPO + 7, 0xffff},
-	{"mcs5glpo0", 0x00000100, 0, SROM8_5GL_MCSPO, 0xffff},
-	{"mcs5glpo1", 0x00000100, 0, SROM8_5GL_MCSPO + 1, 0xffff},
-	{"mcs5glpo2", 0x00000100, 0, SROM8_5GL_MCSPO + 2, 0xffff},
-	{"mcs5glpo3", 0x00000100, 0, SROM8_5GL_MCSPO + 3, 0xffff},
-	{"mcs5glpo4", 0x00000100, 0, SROM8_5GL_MCSPO + 4, 0xffff},
-	{"mcs5glpo5", 0x00000100, 0, SROM8_5GL_MCSPO + 5, 0xffff},
-	{"mcs5glpo6", 0x00000100, 0, SROM8_5GL_MCSPO + 6, 0xffff},
-	{"mcs5glpo7", 0x00000100, 0, SROM8_5GL_MCSPO + 7, 0xffff},
-	{"mcs5ghpo0", 0x00000100, 0, SROM8_5GH_MCSPO, 0xffff},
-	{"mcs5ghpo1", 0x00000100, 0, SROM8_5GH_MCSPO + 1, 0xffff},
-	{"mcs5ghpo2", 0x00000100, 0, SROM8_5GH_MCSPO + 2, 0xffff},
-	{"mcs5ghpo3", 0x00000100, 0, SROM8_5GH_MCSPO + 3, 0xffff},
-	{"mcs5ghpo4", 0x00000100, 0, SROM8_5GH_MCSPO + 4, 0xffff},
-	{"mcs5ghpo5", 0x00000100, 0, SROM8_5GH_MCSPO + 5, 0xffff},
-	{"mcs5ghpo6", 0x00000100, 0, SROM8_5GH_MCSPO + 6, 0xffff},
-	{"mcs5ghpo7", 0x00000100, 0, SROM8_5GH_MCSPO + 7, 0xffff},
-	{"cddpo", 0x000000f0, 0, SROM4_CDDPO, 0xffff},
-	{"stbcpo", 0x000000f0, 0, SROM4_STBCPO, 0xffff},
-	{"bw40po", 0x000000f0, 0, SROM4_BW40PO, 0xffff},
-	{"bwduppo", 0x000000f0, 0, SROM4_BWDUPPO, 0xffff},
-	{"cddpo", 0x00000100, 0, SROM8_CDDPO, 0xffff},
-	{"stbcpo", 0x00000100, 0, SROM8_STBCPO, 0xffff},
-	{"bw40po", 0x00000100, 0, SROM8_BW40PO, 0xffff},
-	{"bwduppo", 0x00000100, 0, SROM8_BWDUPPO, 0xffff},
+
+	{BRCMS_SROM_CCK2GPO, 0x000000f0, 0, SROM4_2G_CCKPO, 0xffff},
+	{BRCMS_SROM_CCK2GPO, 0x00000100, 0, SROM8_2G_CCKPO, 0xffff},
+	{BRCMS_SROM_OFDM2GPO, 0x000000f0, SRFL_MORE, SROM4_2G_OFDMPO, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM4_2G_OFDMPO + 1, 0xffff},
+	{BRCMS_SROM_OFDM5GPO, 0x000000f0, SRFL_MORE, SROM4_5G_OFDMPO, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM4_5G_OFDMPO + 1, 0xffff},
+	{BRCMS_SROM_OFDM5GLPO, 0x000000f0, SRFL_MORE, SROM4_5GL_OFDMPO, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM4_5GL_OFDMPO + 1, 0xffff},
+	{BRCMS_SROM_OFDM5GHPO, 0x000000f0, SRFL_MORE, SROM4_5GH_OFDMPO, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM4_5GH_OFDMPO + 1, 0xffff},
+	{BRCMS_SROM_OFDM2GPO, 0x00000100, SRFL_MORE, SROM8_2G_OFDMPO, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM8_2G_OFDMPO + 1, 0xffff},
+	{BRCMS_SROM_OFDM5GPO, 0x00000100, SRFL_MORE, SROM8_5G_OFDMPO, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM8_5G_OFDMPO + 1, 0xffff},
+	{BRCMS_SROM_OFDM5GLPO, 0x00000100, SRFL_MORE, SROM8_5GL_OFDMPO, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM8_5GL_OFDMPO + 1, 0xffff},
+	{BRCMS_SROM_OFDM5GHPO, 0x00000100, SRFL_MORE, SROM8_5GH_OFDMPO, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM8_5GH_OFDMPO + 1, 0xffff},
+	{BRCMS_SROM_MCS2GPO0, 0x000000f0, 0, SROM4_2G_MCSPO, 0xffff},
+	{BRCMS_SROM_MCS2GPO1, 0x000000f0, 0, SROM4_2G_MCSPO + 1, 0xffff},
+	{BRCMS_SROM_MCS2GPO2, 0x000000f0, 0, SROM4_2G_MCSPO + 2, 0xffff},
+	{BRCMS_SROM_MCS2GPO3, 0x000000f0, 0, SROM4_2G_MCSPO + 3, 0xffff},
+	{BRCMS_SROM_MCS2GPO4, 0x000000f0, 0, SROM4_2G_MCSPO + 4, 0xffff},
+	{BRCMS_SROM_MCS2GPO5, 0x000000f0, 0, SROM4_2G_MCSPO + 5, 0xffff},
+	{BRCMS_SROM_MCS2GPO6, 0x000000f0, 0, SROM4_2G_MCSPO + 6, 0xffff},
+	{BRCMS_SROM_MCS2GPO7, 0x000000f0, 0, SROM4_2G_MCSPO + 7, 0xffff},
+	{BRCMS_SROM_MCS5GPO0, 0x000000f0, 0, SROM4_5G_MCSPO, 0xffff},
+	{BRCMS_SROM_MCS5GPO1, 0x000000f0, 0, SROM4_5G_MCSPO + 1, 0xffff},
+	{BRCMS_SROM_MCS5GPO2, 0x000000f0, 0, SROM4_5G_MCSPO + 2, 0xffff},
+	{BRCMS_SROM_MCS5GPO3, 0x000000f0, 0, SROM4_5G_MCSPO + 3, 0xffff},
+	{BRCMS_SROM_MCS5GPO4, 0x000000f0, 0, SROM4_5G_MCSPO + 4, 0xffff},
+	{BRCMS_SROM_MCS5GPO5, 0x000000f0, 0, SROM4_5G_MCSPO + 5, 0xffff},
+	{BRCMS_SROM_MCS5GPO6, 0x000000f0, 0, SROM4_5G_MCSPO + 6, 0xffff},
+	{BRCMS_SROM_MCS5GPO7, 0x000000f0, 0, SROM4_5G_MCSPO + 7, 0xffff},
+	{BRCMS_SROM_MCS5GLPO0, 0x000000f0, 0, SROM4_5GL_MCSPO, 0xffff},
+	{BRCMS_SROM_MCS5GLPO1, 0x000000f0, 0, SROM4_5GL_MCSPO + 1, 0xffff},
+	{BRCMS_SROM_MCS5GLPO2, 0x000000f0, 0, SROM4_5GL_MCSPO + 2, 0xffff},
+	{BRCMS_SROM_MCS5GLPO3, 0x000000f0, 0, SROM4_5GL_MCSPO + 3, 0xffff},
+	{BRCMS_SROM_MCS5GLPO4, 0x000000f0, 0, SROM4_5GL_MCSPO + 4, 0xffff},
+	{BRCMS_SROM_MCS5GLPO5, 0x000000f0, 0, SROM4_5GL_MCSPO + 5, 0xffff},
+	{BRCMS_SROM_MCS5GLPO6, 0x000000f0, 0, SROM4_5GL_MCSPO + 6, 0xffff},
+	{BRCMS_SROM_MCS5GLPO7, 0x000000f0, 0, SROM4_5GL_MCSPO + 7, 0xffff},
+	{BRCMS_SROM_MCS5GHPO0, 0x000000f0, 0, SROM4_5GH_MCSPO, 0xffff},
+	{BRCMS_SROM_MCS5GHPO1, 0x000000f0, 0, SROM4_5GH_MCSPO + 1, 0xffff},
+	{BRCMS_SROM_MCS5GHPO2, 0x000000f0, 0, SROM4_5GH_MCSPO + 2, 0xffff},
+	{BRCMS_SROM_MCS5GHPO3, 0x000000f0, 0, SROM4_5GH_MCSPO + 3, 0xffff},
+	{BRCMS_SROM_MCS5GHPO4, 0x000000f0, 0, SROM4_5GH_MCSPO + 4, 0xffff},
+	{BRCMS_SROM_MCS5GHPO5, 0x000000f0, 0, SROM4_5GH_MCSPO + 5, 0xffff},
+	{BRCMS_SROM_MCS5GHPO6, 0x000000f0, 0, SROM4_5GH_MCSPO + 6, 0xffff},
+	{BRCMS_SROM_MCS5GHPO7, 0x000000f0, 0, SROM4_5GH_MCSPO + 7, 0xffff},
+	{BRCMS_SROM_MCS2GPO0, 0x00000100, 0, SROM8_2G_MCSPO, 0xffff},
+	{BRCMS_SROM_MCS2GPO1, 0x00000100, 0, SROM8_2G_MCSPO + 1, 0xffff},
+	{BRCMS_SROM_MCS2GPO2, 0x00000100, 0, SROM8_2G_MCSPO + 2, 0xffff},
+	{BRCMS_SROM_MCS2GPO3, 0x00000100, 0, SROM8_2G_MCSPO + 3, 0xffff},
+	{BRCMS_SROM_MCS2GPO4, 0x00000100, 0, SROM8_2G_MCSPO + 4, 0xffff},
+	{BRCMS_SROM_MCS2GPO5, 0x00000100, 0, SROM8_2G_MCSPO + 5, 0xffff},
+	{BRCMS_SROM_MCS2GPO6, 0x00000100, 0, SROM8_2G_MCSPO + 6, 0xffff},
+	{BRCMS_SROM_MCS2GPO7, 0x00000100, 0, SROM8_2G_MCSPO + 7, 0xffff},
+	{BRCMS_SROM_MCS5GPO0, 0x00000100, 0, SROM8_5G_MCSPO, 0xffff},
+	{BRCMS_SROM_MCS5GPO1, 0x00000100, 0, SROM8_5G_MCSPO + 1, 0xffff},
+	{BRCMS_SROM_MCS5GPO2, 0x00000100, 0, SROM8_5G_MCSPO + 2, 0xffff},
+	{BRCMS_SROM_MCS5GPO3, 0x00000100, 0, SROM8_5G_MCSPO + 3, 0xffff},
+	{BRCMS_SROM_MCS5GPO4, 0x00000100, 0, SROM8_5G_MCSPO + 4, 0xffff},
+	{BRCMS_SROM_MCS5GPO5, 0x00000100, 0, SROM8_5G_MCSPO + 5, 0xffff},
+	{BRCMS_SROM_MCS5GPO6, 0x00000100, 0, SROM8_5G_MCSPO + 6, 0xffff},
+	{BRCMS_SROM_MCS5GPO7, 0x00000100, 0, SROM8_5G_MCSPO + 7, 0xffff},
+	{BRCMS_SROM_MCS5GLPO0, 0x00000100, 0, SROM8_5GL_MCSPO, 0xffff},
+	{BRCMS_SROM_MCS5GLPO1, 0x00000100, 0, SROM8_5GL_MCSPO + 1, 0xffff},
+	{BRCMS_SROM_MCS5GLPO2, 0x00000100, 0, SROM8_5GL_MCSPO + 2, 0xffff},
+	{BRCMS_SROM_MCS5GLPO3, 0x00000100, 0, SROM8_5GL_MCSPO + 3, 0xffff},
+	{BRCMS_SROM_MCS5GLPO4, 0x00000100, 0, SROM8_5GL_MCSPO + 4, 0xffff},
+	{BRCMS_SROM_MCS5GLPO5, 0x00000100, 0, SROM8_5GL_MCSPO + 5, 0xffff},
+	{BRCMS_SROM_MCS5GLPO6, 0x00000100, 0, SROM8_5GL_MCSPO + 6, 0xffff},
+	{BRCMS_SROM_MCS5GLPO7, 0x00000100, 0, SROM8_5GL_MCSPO + 7, 0xffff},
+	{BRCMS_SROM_MCS5GHPO0, 0x00000100, 0, SROM8_5GH_MCSPO, 0xffff},
+	{BRCMS_SROM_MCS5GHPO1, 0x00000100, 0, SROM8_5GH_MCSPO + 1, 0xffff},
+	{BRCMS_SROM_MCS5GHPO2, 0x00000100, 0, SROM8_5GH_MCSPO + 2, 0xffff},
+	{BRCMS_SROM_MCS5GHPO3, 0x00000100, 0, SROM8_5GH_MCSPO + 3, 0xffff},
+	{BRCMS_SROM_MCS5GHPO4, 0x00000100, 0, SROM8_5GH_MCSPO + 4, 0xffff},
+	{BRCMS_SROM_MCS5GHPO5, 0x00000100, 0, SROM8_5GH_MCSPO + 5, 0xffff},
+	{BRCMS_SROM_MCS5GHPO6, 0x00000100, 0, SROM8_5GH_MCSPO + 6, 0xffff},
+	{BRCMS_SROM_MCS5GHPO7, 0x00000100, 0, SROM8_5GH_MCSPO + 7, 0xffff},
+	{BRCMS_SROM_CDDPO, 0x000000f0, 0, SROM4_CDDPO, 0xffff},
+	{BRCMS_SROM_STBCPO, 0x000000f0, 0, SROM4_STBCPO, 0xffff},
+	{BRCMS_SROM_BW40PO, 0x000000f0, 0, SROM4_BW40PO, 0xffff},
+	{BRCMS_SROM_BWDUPPO, 0x000000f0, 0, SROM4_BWDUPPO, 0xffff},
+	{BRCMS_SROM_CDDPO, 0x00000100, 0, SROM8_CDDPO, 0xffff},
+	{BRCMS_SROM_STBCPO, 0x00000100, 0, SROM8_STBCPO, 0xffff},
+	{BRCMS_SROM_BW40PO, 0x00000100, 0, SROM8_BW40PO, 0xffff},
+	{BRCMS_SROM_BWDUPPO, 0x00000100, 0, SROM8_BWDUPPO, 0xffff},
 
 	/* power per rate from sromrev 9 */
-	{"cckbw202gpo", 0xfffffe00, 0, SROM9_2GPO_CCKBW20, 0xffff},
-	{"cckbw20ul2gpo", 0xfffffe00, 0, SROM9_2GPO_CCKBW20UL, 0xffff},
-	{"legofdmbw202gpo", 0xfffffe00, SRFL_MORE, SROM9_2GPO_LOFDMBW20,
+	{BRCMS_SROM_CCKBW202GPO, 0xfffffe00, 0, SROM9_2GPO_CCKBW20, 0xffff},
+	{BRCMS_SROM_CCKBW20UL2GPO, 0xfffffe00, 0, SROM9_2GPO_CCKBW20UL, 0xffff},
+	{BRCMS_SROM_LEGOFDMBW202GPO, 0xfffffe00, SRFL_MORE,
+	 SROM9_2GPO_LOFDMBW20, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM9_2GPO_LOFDMBW20 + 1, 0xffff},
+	{BRCMS_SROM_LEGOFDMBW20UL2GPO, 0xfffffe00, SRFL_MORE,
+	 SROM9_2GPO_LOFDMBW20UL, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM9_2GPO_LOFDMBW20UL + 1, 0xffff},
+	{BRCMS_SROM_LEGOFDMBW205GLPO, 0xfffffe00, SRFL_MORE,
+	 SROM9_5GLPO_LOFDMBW20, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM9_5GLPO_LOFDMBW20 + 1, 0xffff},
+	{BRCMS_SROM_LEGOFDMBW20UL5GLPO, 0xfffffe00, SRFL_MORE,
+	 SROM9_5GLPO_LOFDMBW20UL, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM9_5GLPO_LOFDMBW20UL + 1, 0xffff},
+	{BRCMS_SROM_LEGOFDMBW205GMPO, 0xfffffe00, SRFL_MORE,
+	 SROM9_5GMPO_LOFDMBW20, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM9_5GMPO_LOFDMBW20 + 1, 0xffff},
+	{BRCMS_SROM_LEGOFDMBW20UL5GMPO, 0xfffffe00, SRFL_MORE,
+	 SROM9_5GMPO_LOFDMBW20UL, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM9_5GMPO_LOFDMBW20UL + 1, 0xffff},
+	{BRCMS_SROM_LEGOFDMBW205GHPO, 0xfffffe00, SRFL_MORE,
+	 SROM9_5GHPO_LOFDMBW20, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM9_5GHPO_LOFDMBW20 + 1, 0xffff},
+	{BRCMS_SROM_LEGOFDMBW20UL5GHPO, 0xfffffe00, SRFL_MORE,
+	 SROM9_5GHPO_LOFDMBW20UL, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM9_5GHPO_LOFDMBW20UL + 1, 0xffff},
+	{BRCMS_SROM_MCSBW202GPO, 0xfffffe00, SRFL_MORE, SROM9_2GPO_MCSBW20,
 	 0xffff},
-	{"", 0, 0, SROM9_2GPO_LOFDMBW20 + 1, 0xffff},
-	{"legofdmbw20ul2gpo", 0xfffffe00, SRFL_MORE, SROM9_2GPO_LOFDMBW20UL,
+	{BRCMS_SROM_CONT, 0, 0, SROM9_2GPO_MCSBW20 + 1, 0xffff},
+	{BRCMS_SROM_MCSBW20UL2GPO, 0xfffffe00, SRFL_MORE, SROM9_2GPO_MCSBW20UL,
 	 0xffff},
-	{"", 0, 0, SROM9_2GPO_LOFDMBW20UL + 1, 0xffff},
-	{"legofdmbw205glpo", 0xfffffe00, SRFL_MORE, SROM9_5GLPO_LOFDMBW20,
+	{BRCMS_SROM_CONT, 0, 0, SROM9_2GPO_MCSBW20UL + 1, 0xffff},
+	{BRCMS_SROM_MCSBW402GPO, 0xfffffe00, SRFL_MORE, SROM9_2GPO_MCSBW40,
 	 0xffff},
-	{"", 0, 0, SROM9_5GLPO_LOFDMBW20 + 1, 0xffff},
-	{"legofdmbw20ul5glpo", 0xfffffe00, SRFL_MORE, SROM9_5GLPO_LOFDMBW20UL,
+	{BRCMS_SROM_CONT, 0, 0, SROM9_2GPO_MCSBW40 + 1, 0xffff},
+	{BRCMS_SROM_MCSBW205GLPO, 0xfffffe00, SRFL_MORE, SROM9_5GLPO_MCSBW20,
 	 0xffff},
-	{"", 0, 0, SROM9_5GLPO_LOFDMBW20UL + 1, 0xffff},
-	{"legofdmbw205gmpo", 0xfffffe00, SRFL_MORE, SROM9_5GMPO_LOFDMBW20,
+	{BRCMS_SROM_CONT, 0, 0, SROM9_5GLPO_MCSBW20 + 1, 0xffff},
+	{BRCMS_SROM_MCSBW20UL5GLPO, 0xfffffe00, SRFL_MORE,
+	 SROM9_5GLPO_MCSBW20UL, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM9_5GLPO_MCSBW20UL + 1, 0xffff},
+	{BRCMS_SROM_MCSBW405GLPO, 0xfffffe00, SRFL_MORE, SROM9_5GLPO_MCSBW40,
 	 0xffff},
-	{"", 0, 0, SROM9_5GMPO_LOFDMBW20 + 1, 0xffff},
-	{"legofdmbw20ul5gmpo", 0xfffffe00, SRFL_MORE, SROM9_5GMPO_LOFDMBW20UL,
+	{BRCMS_SROM_CONT, 0, 0, SROM9_5GLPO_MCSBW40 + 1, 0xffff},
+	{BRCMS_SROM_MCSBW205GMPO, 0xfffffe00, SRFL_MORE, SROM9_5GMPO_MCSBW20,
 	 0xffff},
-	{"", 0, 0, SROM9_5GMPO_LOFDMBW20UL + 1, 0xffff},
-	{"legofdmbw205ghpo", 0xfffffe00, SRFL_MORE, SROM9_5GHPO_LOFDMBW20,
+	{BRCMS_SROM_CONT, 0, 0, SROM9_5GMPO_MCSBW20 + 1, 0xffff},
+	{BRCMS_SROM_MCSBW20UL5GMPO, 0xfffffe00, SRFL_MORE,
+	 SROM9_5GMPO_MCSBW20UL, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM9_5GMPO_MCSBW20UL + 1, 0xffff},
+	{BRCMS_SROM_MCSBW405GMPO, 0xfffffe00, SRFL_MORE, SROM9_5GMPO_MCSBW40,
 	 0xffff},
-	{"", 0, 0, SROM9_5GHPO_LOFDMBW20 + 1, 0xffff},
-	{"legofdmbw20ul5ghpo", 0xfffffe00, SRFL_MORE, SROM9_5GHPO_LOFDMBW20UL,
+	{BRCMS_SROM_CONT, 0, 0, SROM9_5GMPO_MCSBW40 + 1, 0xffff},
+	{BRCMS_SROM_MCSBW205GHPO, 0xfffffe00, SRFL_MORE, SROM9_5GHPO_MCSBW20,
 	 0xffff},
-	{"", 0, 0, SROM9_5GHPO_LOFDMBW20UL + 1, 0xffff},
-	{"mcsbw202gpo", 0xfffffe00, SRFL_MORE, SROM9_2GPO_MCSBW20, 0xffff},
-	{"", 0, 0, SROM9_2GPO_MCSBW20 + 1, 0xffff},
-	{"mcsbw20ul2gpo", 0xfffffe00, SRFL_MORE, SROM9_2GPO_MCSBW20UL, 0xffff},
-	{"", 0, 0, SROM9_2GPO_MCSBW20UL + 1, 0xffff},
-	{"mcsbw402gpo", 0xfffffe00, SRFL_MORE, SROM9_2GPO_MCSBW40, 0xffff},
-	{"", 0, 0, SROM9_2GPO_MCSBW40 + 1, 0xffff},
-	{"mcsbw205glpo", 0xfffffe00, SRFL_MORE, SROM9_5GLPO_MCSBW20, 0xffff},
-	{"", 0, 0, SROM9_5GLPO_MCSBW20 + 1, 0xffff},
-	{"mcsbw20ul5glpo", 0xfffffe00, SRFL_MORE, SROM9_5GLPO_MCSBW20UL,
+	{BRCMS_SROM_CONT, 0, 0, SROM9_5GHPO_MCSBW20 + 1, 0xffff},
+	{BRCMS_SROM_MCSBW20UL5GHPO, 0xfffffe00, SRFL_MORE,
+	 SROM9_5GHPO_MCSBW20UL, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM9_5GHPO_MCSBW20UL + 1, 0xffff},
+	{BRCMS_SROM_MCSBW405GHPO, 0xfffffe00, SRFL_MORE, SROM9_5GHPO_MCSBW40,
 	 0xffff},
-	{"", 0, 0, SROM9_5GLPO_MCSBW20UL + 1, 0xffff},
-	{"mcsbw405glpo", 0xfffffe00, SRFL_MORE, SROM9_5GLPO_MCSBW40, 0xffff},
-	{"", 0, 0, SROM9_5GLPO_MCSBW40 + 1, 0xffff},
-	{"mcsbw205gmpo", 0xfffffe00, SRFL_MORE, SROM9_5GMPO_MCSBW20, 0xffff},
-	{"", 0, 0, SROM9_5GMPO_MCSBW20 + 1, 0xffff},
-	{"mcsbw20ul5gmpo", 0xfffffe00, SRFL_MORE, SROM9_5GMPO_MCSBW20UL,
-	 0xffff},
-	{"", 0, 0, SROM9_5GMPO_MCSBW20UL + 1, 0xffff},
-	{"mcsbw405gmpo", 0xfffffe00, SRFL_MORE, SROM9_5GMPO_MCSBW40, 0xffff},
-	{"", 0, 0, SROM9_5GMPO_MCSBW40 + 1, 0xffff},
-	{"mcsbw205ghpo", 0xfffffe00, SRFL_MORE, SROM9_5GHPO_MCSBW20, 0xffff},
-	{"", 0, 0, SROM9_5GHPO_MCSBW20 + 1, 0xffff},
-	{"mcsbw20ul5ghpo", 0xfffffe00, SRFL_MORE, SROM9_5GHPO_MCSBW20UL,
-	 0xffff},
-	{"", 0, 0, SROM9_5GHPO_MCSBW20UL + 1, 0xffff},
-	{"mcsbw405ghpo", 0xfffffe00, SRFL_MORE, SROM9_5GHPO_MCSBW40, 0xffff},
-	{"", 0, 0, SROM9_5GHPO_MCSBW40 + 1, 0xffff},
-	{"mcs32po", 0xfffffe00, 0, SROM9_PO_MCS32, 0xffff},
-	{"legofdm40duppo", 0xfffffe00, 0, SROM9_PO_LOFDM40DUP, 0xffff},
+	{BRCMS_SROM_CONT, 0, 0, SROM9_5GHPO_MCSBW40 + 1, 0xffff},
+	{BRCMS_SROM_MCS32PO, 0xfffffe00, 0, SROM9_PO_MCS32, 0xffff},
+	{BRCMS_SROM_LEGOFDM40DUPPO, 0xfffffe00, 0, SROM9_PO_LOFDM40DUP, 0xffff},
 
-	{NULL, 0, 0, 0, 0}
+	{BRCMS_SROM_NULL, 0, 0, 0, 0}
 };
 
 static const struct brcms_sromvar perpath_pci_sromvars[] = {
-	{"maxp2ga", 0x000000f0, 0, SROM4_2G_ITT_MAXP, 0x00ff},
-	{"itt2ga", 0x000000f0, 0, SROM4_2G_ITT_MAXP, 0xff00},
-	{"itt5ga", 0x000000f0, 0, SROM4_5G_ITT_MAXP, 0xff00},
-	{"pa2gw0a", 0x000000f0, SRFL_PRHEX, SROM4_2G_PA, 0xffff},
-	{"pa2gw1a", 0x000000f0, SRFL_PRHEX, SROM4_2G_PA + 1, 0xffff},
-	{"pa2gw2a", 0x000000f0, SRFL_PRHEX, SROM4_2G_PA + 2, 0xffff},
-	{"pa2gw3a", 0x000000f0, SRFL_PRHEX, SROM4_2G_PA + 3, 0xffff},
-	{"maxp5ga", 0x000000f0, 0, SROM4_5G_ITT_MAXP, 0x00ff},
-	{"maxp5gha", 0x000000f0, 0, SROM4_5GLH_MAXP, 0x00ff},
-	{"maxp5gla", 0x000000f0, 0, SROM4_5GLH_MAXP, 0xff00},
-	{"pa5gw0a", 0x000000f0, SRFL_PRHEX, SROM4_5G_PA, 0xffff},
-	{"pa5gw1a", 0x000000f0, SRFL_PRHEX, SROM4_5G_PA + 1, 0xffff},
-	{"pa5gw2a", 0x000000f0, SRFL_PRHEX, SROM4_5G_PA + 2, 0xffff},
-	{"pa5gw3a", 0x000000f0, SRFL_PRHEX, SROM4_5G_PA + 3, 0xffff},
-	{"pa5glw0a", 0x000000f0, SRFL_PRHEX, SROM4_5GL_PA, 0xffff},
-	{"pa5glw1a", 0x000000f0, SRFL_PRHEX, SROM4_5GL_PA + 1, 0xffff},
-	{"pa5glw2a", 0x000000f0, SRFL_PRHEX, SROM4_5GL_PA + 2, 0xffff},
-	{"pa5glw3a", 0x000000f0, SRFL_PRHEX, SROM4_5GL_PA + 3, 0xffff},
-	{"pa5ghw0a", 0x000000f0, SRFL_PRHEX, SROM4_5GH_PA, 0xffff},
-	{"pa5ghw1a", 0x000000f0, SRFL_PRHEX, SROM4_5GH_PA + 1, 0xffff},
-	{"pa5ghw2a", 0x000000f0, SRFL_PRHEX, SROM4_5GH_PA + 2, 0xffff},
-	{"pa5ghw3a", 0x000000f0, SRFL_PRHEX, SROM4_5GH_PA + 3, 0xffff},
-	{"maxp2ga", 0xffffff00, 0, SROM8_2G_ITT_MAXP, 0x00ff},
-	{"itt2ga", 0xffffff00, 0, SROM8_2G_ITT_MAXP, 0xff00},
-	{"itt5ga", 0xffffff00, 0, SROM8_5G_ITT_MAXP, 0xff00},
-	{"pa2gw0a", 0xffffff00, SRFL_PRHEX, SROM8_2G_PA, 0xffff},
-	{"pa2gw1a", 0xffffff00, SRFL_PRHEX, SROM8_2G_PA + 1, 0xffff},
-	{"pa2gw2a", 0xffffff00, SRFL_PRHEX, SROM8_2G_PA + 2, 0xffff},
-	{"maxp5ga", 0xffffff00, 0, SROM8_5G_ITT_MAXP, 0x00ff},
-	{"maxp5gha", 0xffffff00, 0, SROM8_5GLH_MAXP, 0x00ff},
-	{"maxp5gla", 0xffffff00, 0, SROM8_5GLH_MAXP, 0xff00},
-	{"pa5gw0a", 0xffffff00, SRFL_PRHEX, SROM8_5G_PA, 0xffff},
-	{"pa5gw1a", 0xffffff00, SRFL_PRHEX, SROM8_5G_PA + 1, 0xffff},
-	{"pa5gw2a", 0xffffff00, SRFL_PRHEX, SROM8_5G_PA + 2, 0xffff},
-	{"pa5glw0a", 0xffffff00, SRFL_PRHEX, SROM8_5GL_PA, 0xffff},
-	{"pa5glw1a", 0xffffff00, SRFL_PRHEX, SROM8_5GL_PA + 1, 0xffff},
-	{"pa5glw2a", 0xffffff00, SRFL_PRHEX, SROM8_5GL_PA + 2, 0xffff},
-	{"pa5ghw0a", 0xffffff00, SRFL_PRHEX, SROM8_5GH_PA, 0xffff},
-	{"pa5ghw1a", 0xffffff00, SRFL_PRHEX, SROM8_5GH_PA + 1, 0xffff},
-	{"pa5ghw2a", 0xffffff00, SRFL_PRHEX, SROM8_5GH_PA + 2, 0xffff},
-	{NULL, 0, 0, 0, 0}
+	{BRCMS_SROM_MAXP2GA0, 0x000000f0, 0, SROM4_2G_ITT_MAXP, 0x00ff},
+	{BRCMS_SROM_ITT2GA0, 0x000000f0, 0, SROM4_2G_ITT_MAXP, 0xff00},
+	{BRCMS_SROM_ITT5GA0, 0x000000f0, 0, SROM4_5G_ITT_MAXP, 0xff00},
+	{BRCMS_SROM_PA2GW0A0, 0x000000f0, SRFL_PRHEX, SROM4_2G_PA, 0xffff},
+	{BRCMS_SROM_PA2GW1A0, 0x000000f0, SRFL_PRHEX, SROM4_2G_PA + 1, 0xffff},
+	{BRCMS_SROM_PA2GW2A0, 0x000000f0, SRFL_PRHEX, SROM4_2G_PA + 2, 0xffff},
+	{BRCMS_SROM_PA2GW3A0, 0x000000f0, SRFL_PRHEX, SROM4_2G_PA + 3, 0xffff},
+	{BRCMS_SROM_MAXP5GA0, 0x000000f0, 0, SROM4_5G_ITT_MAXP, 0x00ff},
+	{BRCMS_SROM_MAXP5GHA0, 0x000000f0, 0, SROM4_5GLH_MAXP, 0x00ff},
+	{BRCMS_SROM_MAXP5GLA0, 0x000000f0, 0, SROM4_5GLH_MAXP, 0xff00},
+	{BRCMS_SROM_PA5GW0A0, 0x000000f0, SRFL_PRHEX, SROM4_5G_PA, 0xffff},
+	{BRCMS_SROM_PA5GW1A0, 0x000000f0, SRFL_PRHEX, SROM4_5G_PA + 1, 0xffff},
+	{BRCMS_SROM_PA5GW2A0, 0x000000f0, SRFL_PRHEX, SROM4_5G_PA + 2, 0xffff},
+	{BRCMS_SROM_PA5GW3A0, 0x000000f0, SRFL_PRHEX, SROM4_5G_PA + 3, 0xffff},
+	{BRCMS_SROM_PA5GLW0A0, 0x000000f0, SRFL_PRHEX, SROM4_5GL_PA, 0xffff},
+	{BRCMS_SROM_PA5GLW1A0, 0x000000f0, SRFL_PRHEX, SROM4_5GL_PA + 1,
+	 0xffff},
+	{BRCMS_SROM_PA5GLW2A0, 0x000000f0, SRFL_PRHEX, SROM4_5GL_PA + 2,
+	 0xffff},
+	{BRCMS_SROM_PA5GLW3A0, 0x000000f0, SRFL_PRHEX, SROM4_5GL_PA + 3,
+	 0xffff},
+	{BRCMS_SROM_PA5GHW0A0, 0x000000f0, SRFL_PRHEX, SROM4_5GH_PA, 0xffff},
+	{BRCMS_SROM_PA5GHW1A0, 0x000000f0, SRFL_PRHEX, SROM4_5GH_PA + 1,
+	 0xffff},
+	{BRCMS_SROM_PA5GHW2A0, 0x000000f0, SRFL_PRHEX, SROM4_5GH_PA + 2,
+	 0xffff},
+	{BRCMS_SROM_PA5GHW3A0, 0x000000f0, SRFL_PRHEX, SROM4_5GH_PA + 3,
+	 0xffff},
+	{BRCMS_SROM_MAXP2GA0, 0xffffff00, 0, SROM8_2G_ITT_MAXP, 0x00ff},
+	{BRCMS_SROM_ITT2GA0, 0xffffff00, 0, SROM8_2G_ITT_MAXP, 0xff00},
+	{BRCMS_SROM_ITT5GA0, 0xffffff00, 0, SROM8_5G_ITT_MAXP, 0xff00},
+	{BRCMS_SROM_PA2GW0A0, 0xffffff00, SRFL_PRHEX, SROM8_2G_PA, 0xffff},
+	{BRCMS_SROM_PA2GW1A0, 0xffffff00, SRFL_PRHEX, SROM8_2G_PA + 1, 0xffff},
+	{BRCMS_SROM_PA2GW2A0, 0xffffff00, SRFL_PRHEX, SROM8_2G_PA + 2, 0xffff},
+	{BRCMS_SROM_MAXP5GA0, 0xffffff00, 0, SROM8_5G_ITT_MAXP, 0x00ff},
+	{BRCMS_SROM_MAXP5GHA0, 0xffffff00, 0, SROM8_5GLH_MAXP, 0x00ff},
+	{BRCMS_SROM_MAXP5GLA0, 0xffffff00, 0, SROM8_5GLH_MAXP, 0xff00},
+	{BRCMS_SROM_PA5GW0A0, 0xffffff00, SRFL_PRHEX, SROM8_5G_PA, 0xffff},
+	{BRCMS_SROM_PA5GW1A0, 0xffffff00, SRFL_PRHEX, SROM8_5G_PA + 1, 0xffff},
+	{BRCMS_SROM_PA5GW2A0, 0xffffff00, SRFL_PRHEX, SROM8_5G_PA + 2, 0xffff},
+	{BRCMS_SROM_PA5GLW0A0, 0xffffff00, SRFL_PRHEX, SROM8_5GL_PA, 0xffff},
+	{BRCMS_SROM_PA5GLW1A0, 0xffffff00, SRFL_PRHEX, SROM8_5GL_PA + 1,
+	 0xffff},
+	{BRCMS_SROM_PA5GLW2A0, 0xffffff00, SRFL_PRHEX, SROM8_5GL_PA + 2,
+	 0xffff},
+	{BRCMS_SROM_PA5GHW0A0, 0xffffff00, SRFL_PRHEX, SROM8_5GH_PA, 0xffff},
+	{BRCMS_SROM_PA5GHW1A0, 0xffffff00, SRFL_PRHEX, SROM8_5GH_PA + 1,
+	 0xffff},
+	{BRCMS_SROM_PA5GHW2A0, 0xffffff00, SRFL_PRHEX, SROM8_5GH_PA + 2,
+	 0xffff},
+	{BRCMS_SROM_NULL, 0, 0, 0, 0}
 };
 
-static void _initvars_srom_pci(u8 sromrev, u16 *srom, uint off,
-			       struct brcms_varbuf *b);
-static int initvars_srom_pci(struct si_pub *sih, void *curmap, char **vars,
-			     uint *count);
-static int sprom_read_pci(struct si_pub *sih, u16 *sprom,
-			  uint wordoff, u16 *buf, uint nwords, bool check_crc);
-#if defined(BCMNVRAMR)
-static int otp_read_pci(struct si_pub *sih, u16 *buf, uint bufsz);
-#endif
-
-static int initvars_table(char *start, char *end,
-			  char **vars, uint *count);
-
-/* Initialization of varbuf structure */
-static void varbuf_init(struct brcms_varbuf *b, char *buf, uint size)
-{
-	b->size = size;
-	b->base = b->buf = buf;
-}
-
-/* append a null terminated var=value string */
-static int varbuf_append(struct brcms_varbuf *b, const char *fmt, ...)
-{
-	va_list ap;
-	int r;
-	size_t len;
-	char *s;
-
-	if (b->size < 2)
-		return 0;
-
-	va_start(ap, fmt);
-	r = vsnprintf(b->buf, b->size, fmt, ap);
-	va_end(ap);
-
-	/* C99 snprintf behavior returns r >= size on overflow,
-	 * others return -1 on overflow.
-	 * All return -1 on format error.
-	 * We need to leave room for 2 null terminations, one for the current var
-	 * string, and one for final null of the var table. So check that the
-	 * strlen written, r, leaves room for 2 chars.
-	 */
-	if ((r == -1) || (r > (int)(b->size - 2))) {
-		b->size = 0;
-		return 0;
-	}
-
-	/* Remove any earlier occurrence of the same variable */
-	s = strchr(b->buf, '=');
-	if (s != NULL) {
-		len = (size_t) (s - b->buf);
-		for (s = b->base; s < b->buf;) {
-			if ((memcmp(s, b->buf, len) == 0) && s[len] == '=') {
-				len = strlen(s) + 1;
-				memmove(s, (s + len),
-					((b->buf + r + 1) - (s + len)));
-				b->buf -= len;
-				b->size += (unsigned int)len;
-				break;
-			}
-
-			while (*s++)
-				;
-		}
-	}
-
-	/* skip over this string's null termination */
-	r++;
-	b->size -= r;
-	b->buf += r;
-
-	return r;
-}
-
-/*
- * Initialize local vars from the right source for this platform.
- * Return 0 on success, nonzero on error.
- */
-int srom_var_init(struct si_pub *sih, uint bustype, void *curmap,
-		  char **vars, uint *count)
-{
-	uint len;
-
-	len = 0;
-
-	if (vars == NULL || count == NULL)
-		return 0;
-
-	*vars = NULL;
-	*count = 0;
-
-	if (curmap != NULL && bustype == PCI_BUS)
-		return initvars_srom_pci(sih, curmap, vars, count);
-
-	return -EINVAL;
-}
-
-static inline void ltoh16_buf(u16 *buf, unsigned int size)
-{
-	for (size /= 2; size; size--)
-		*(buf + size) = le16_to_cpu(*(buf + size));
-}
-
-static inline void htol16_buf(u16 *buf, unsigned int size)
-{
-	for (size /= 2; size; size--)
-		*(buf + size) = cpu_to_le16(*(buf + size));
-}
-
-/*
- * Read in and validate sprom.
- * Return 0 on success, nonzero on error.
- */
-static int
-sprom_read_pci(struct si_pub *sih, u16 *sprom, uint wordoff,
-	       u16 *buf, uint nwords, bool check_crc)
-{
-	int err = 0;
-	uint i;
-
-	/* read the sprom */
-	for (i = 0; i < nwords; i++)
-		buf[i] = R_REG(&sprom[wordoff + i]);
-
-	if (check_crc) {
-
-		if (buf[0] == 0xffff) {
-			/* The hardware thinks that an srom that starts with 0xffff
-			 * is blank, regardless of the rest of the content, so declare
-			 * it bad.
-			 */
-			return -ENODATA;
-		}
-
-		/* fixup the endianness so crc8 will pass */
-		htol16_buf(buf, nwords * 2);
-		if (brcmu_crc8((u8 *) buf, nwords * 2, CRC8_INIT_VALUE) !=
-		    CRC8_GOOD_VALUE) {
-			/* DBG only pci always read srom4 first, then srom8/9 */
-			err = -EIO;
-		}
-		/* now correct the endianness of the byte array */
-		ltoh16_buf(buf, nwords * 2);
-	}
-	return err;
-}
-
-#if defined(BCMNVRAMR)
-static int otp_read_pci(struct si_pub *sih, u16 *buf, uint bufsz)
-{
-	u8 *otp;
-	uint sz = OTP_SZ_MAX / 2;	/* size in words */
-	int err = 0;
-
-	otp = kzalloc(OTP_SZ_MAX, GFP_ATOMIC);
-	if (otp == NULL) {
-		return -ENOMEM;
-	}
-
-	err = otp_read_region(sih, OTP_HW_RGN, (u16 *) otp, &sz);
-
-	memcpy(buf, otp, bufsz);
-
-	kfree(otp);
-
-	/* Check CRC */
-	if (buf[0] == 0xffff) {
-		/* The hardware thinks that an srom that starts with 0xffff
-		 * is blank, regardless of the rest of the content, so declare
-		 * it bad.
-		 */
-		return -ENODATA;
-	}
-
-	/* fixup the endianness so crc8 will pass */
-	htol16_buf(buf, bufsz);
-	if (brcmu_crc8((u8 *) buf, SROM4_WORDS * 2, CRC8_INIT_VALUE) !=
-	    CRC8_GOOD_VALUE) {
-		err = -EIO;
-	}
-	/* now correct the endianness of the byte array */
-	ltoh16_buf(buf, bufsz);
+/* crc table has the same contents for every device instance, so it can be
+ * shared between devices. */
+static u8 brcms_srom_crc8_table[CRC8_TABLE_SIZE];
 
-	return err;
-}
-#endif				/* defined(BCMNVRAMR) */
-/*
-* Create variable table from memory.
-* Return 0 on success, nonzero on error.
-*/
-static int initvars_table(char *start, char *end,
-			  char **vars, uint *count)
+static u16 __iomem *
+srom_window_address(struct si_pub *sih, u8 __iomem *curmap)
 {
-	int c = (int)(end - start);
-
-	/* do it only when there is more than just the null string */
-	if (c > 1) {
-		char *vp = kmalloc(c, GFP_ATOMIC);
-		if (!vp)
-			return -ENOMEM;
-		memcpy(vp, start, c);
-		*vars = vp;
-		*count = c;
-	} else {
-		*vars = NULL;
-		*count = 0;
-	}
+	if (sih->ccrev < 32)
+		return (u16 __iomem *)(curmap + PCI_BAR0_SPROM_OFFSET);
+	if (sih->cccaps & CC_CAP_SROM)
+		return (u16 __iomem *)
+		       (curmap + PCI_16KB0_CCREGS_OFFSET + CC_SROM_OTP);
 
-	return 0;
+	return NULL;
 }
 
 /* Parse SROM and create name=value pairs. 'srom' points to
@@ -1012,9 +906,28 @@ static uint mask_width(u16 mask)
 	return 0;
 }
 
+static inline void ltoh16_buf(u16 *buf, unsigned int size)
+{
+	size /= 2;
+	while (size--)
+		*(buf + size) = le16_to_cpu(*(__le16 *)(buf + size));
+}
+
+static inline void htol16_buf(u16 *buf, unsigned int size)
+{
+	size /= 2;
+	while (size--)
+		*(__le16 *)(buf + size) = cpu_to_le16(*(buf + size));
+}
+
+/*
+ * convert binary srom data into linked list of srom variable items.
+ */
 static void
-_initvars_srom_pci(u8 sromrev, u16 *srom, uint off, struct brcms_varbuf *b)
+_initvars_srom_pci(u8 sromrev, u16 *srom, struct list_head *var_list)
 {
+	struct brcms_srom_list_head *entry;
+	enum brcms_srom_id id;
 	u16 w;
 	u32 val;
 	const struct brcms_sromvar *srv;
@@ -1022,46 +935,52 @@ _initvars_srom_pci(u8 sromrev, u16 *srom, uint off, struct brcms_varbuf *b)
 	uint flags;
 	u32 sr = (1 << sromrev);
 
-	varbuf_append(b, "sromrev=%d", sromrev);
+	/* first store the srom revision */
+	entry = kzalloc(sizeof(struct brcms_srom_list_head), GFP_KERNEL);
+	entry->varid = BRCMS_SROM_REV;
+	entry->var_type = BRCMS_SROM_UNUMBER;
+	entry->uval = sromrev;
+	list_add(&entry->var_list, var_list);
 
-	for (srv = pci_sromvars; srv->name != NULL; srv++) {
-		const char *name;
+	for (srv = pci_sromvars; srv->varid != BRCMS_SROM_NULL; srv++) {
+		enum brcms_srom_var_type type;
+		u8 ea[ETH_ALEN];
+		u8 extra_space = 0;
 
 		if ((srv->revmask & sr) == 0)
 			continue;
 
-		if (srv->off < off)
-			continue;
-
 		flags = srv->flags;
-		name = srv->name;
+		id = srv->varid;
 
 		/* This entry is for mfgc only. Don't generate param for it, */
 		if (flags & SRFL_NOVAR)
 			continue;
 
 		if (flags & SRFL_ETHADDR) {
-			u8 ea[ETH_ALEN];
-
-			ea[0] = (srom[srv->off - off] >> 8) & 0xff;
-			ea[1] = srom[srv->off - off] & 0xff;
-			ea[2] = (srom[srv->off + 1 - off] >> 8) & 0xff;
-			ea[3] = srom[srv->off + 1 - off] & 0xff;
-			ea[4] = (srom[srv->off + 2 - off] >> 8) & 0xff;
-			ea[5] = srom[srv->off + 2 - off] & 0xff;
-
-			varbuf_append(b, "%s=%pM", name, ea);
+			/*
+			 * stored in string format XX:XX:XX:XX:XX:XX (17 chars)
+			 */
+			ea[0] = (srom[srv->off] >> 8) & 0xff;
+			ea[1] = srom[srv->off] & 0xff;
+			ea[2] = (srom[srv->off + 1] >> 8) & 0xff;
+			ea[3] = srom[srv->off + 1] & 0xff;
+			ea[4] = (srom[srv->off + 2] >> 8) & 0xff;
+			ea[5] = srom[srv->off + 2] & 0xff;
+			/* 17 characters + string terminator - union size */
+			extra_space = 18 - sizeof(s32);
+			type = BRCMS_SROM_STRING;
 		} else {
-			w = srom[srv->off - off];
+			w = srom[srv->off];
 			val = (w & srv->mask) >> mask_shift(srv->mask);
 			width = mask_width(srv->mask);
 
 			while (srv->flags & SRFL_MORE) {
 				srv++;
-				if (srv->off == 0 || srv->off < off)
+				if (srv->off == 0)
 					continue;
 
-				w = srom[srv->off - off];
+				w = srom[srv->off];
 				val +=
 				    ((w & srv->mask) >> mask_shift(srv->
 								   mask)) <<
@@ -1074,28 +993,42 @@ _initvars_srom_pci(u8 sromrev, u16 *srom, uint off, struct brcms_varbuf *b)
 				continue;
 
 			if (flags & SRFL_CCODE) {
-				if (val == 0)
-					varbuf_append(b, "ccode=");
-				else
-					varbuf_append(b, "ccode=%c%c",
-						      (val >> 8), (val & 0xff));
-			}
-			/* LED Powersave duty cycle has to be scaled:
-			 *(oncount >> 24) (offcount >> 8)
-			 */
-			else if (flags & SRFL_LEDDC) {
-				u32 w32 = (((val >> 8) & 0xff) << 24) |	/* oncount */
-				    (((val & 0xff)) << 8);	/* offcount */
-				varbuf_append(b, "leddc=%d", w32);
-			} else if (flags & SRFL_PRHEX)
-				varbuf_append(b, "%s=0x%x", name, val);
-			else if ((flags & SRFL_PRSIGN)
-				 && (val & (1 << (width - 1))))
-				varbuf_append(b, "%s=%d", name,
-					      (int)(val | (~0 << width)));
+				type = BRCMS_SROM_STRING;
+			} else if (flags & SRFL_LEDDC) {
+				/* LED Powersave duty cycle has to be scaled:
+				 *(oncount >> 24) (offcount >> 8)
+				 */
+				u32 w32 = /* oncount */
+					  (((val >> 8) & 0xff) << 24) |
+					  /* offcount */
+					  (((val & 0xff)) << 8);
+				type = BRCMS_SROM_UNUMBER;
+				val = w32;
+			} else if ((flags & SRFL_PRSIGN)
+				 && (val & (1 << (width - 1)))) {
+				type = BRCMS_SROM_SNUMBER;
+				val |= ~0 << width;
+			} else
+				type = BRCMS_SROM_UNUMBER;
+		}
+
+		entry = kzalloc(sizeof(struct brcms_srom_list_head) +
+				extra_space, GFP_KERNEL);
+		entry->varid = id;
+		entry->var_type = type;
+		if (flags & SRFL_ETHADDR) {
+			snprintf(entry->buf, 18, "%pM", ea);
+		} else if (flags & SRFL_CCODE) {
+			if (val == 0)
+				entry->buf[0] = '\0';
 			else
-				varbuf_append(b, "%s=%u", name, val);
+				snprintf(entry->buf, 3, "%c%c",
+					 (val >> 8), (val & 0xff));
+		} else {
+			entry->uval = val;
 		}
+
+		list_add(&entry->var_list, var_list);
 	}
 
 	if (sromrev >= 4) {
@@ -1111,34 +1044,31 @@ _initvars_srom_pci(u8 sromrev, u16 *srom, uint off, struct brcms_varbuf *b)
 		}
 
 		for (p = 0; p < MAX_PATH_SROM; p++) {
-			for (srv = perpath_pci_sromvars; srv->name != NULL;
-			     srv++) {
+			for (srv = perpath_pci_sromvars;
+			     srv->varid != BRCMS_SROM_NULL; srv++) {
 				if ((srv->revmask & sr) == 0)
 					continue;
 
-				if (pb + srv->off < off)
-					continue;
-
-				/* This entry is for mfgc only. Don't generate param for it, */
 				if (srv->flags & SRFL_NOVAR)
 					continue;
 
-				w = srom[pb + srv->off - off];
+				w = srom[pb + srv->off];
 				val = (w & srv->mask) >> mask_shift(srv->mask);
 				width = mask_width(srv->mask);
 
-				/* Cheating: no per-path var is more than 1 word */
-
+				/* Cheating: no per-path var is more than
+				 * 1 word */
 				if ((srv->flags & SRFL_NOFFS)
 				    && ((int)val == (1 << width) - 1))
 					continue;
 
-				if (srv->flags & SRFL_PRHEX)
-					varbuf_append(b, "%s%d=0x%x", srv->name,
-						      p, val);
-				else
-					varbuf_append(b, "%s%d=%d", srv->name,
-						      p, val);
+				entry =
+				    kzalloc(sizeof(struct brcms_srom_list_head),
+					    GFP_KERNEL);
+				entry->varid = srv->varid+p;
+				entry->var_type = BRCMS_SROM_UNUMBER;
+				entry->uval = val;
+				list_add(&entry->var_list, var_list);
 			}
 			pb += psz;
 		}
@@ -1146,17 +1076,90 @@ _initvars_srom_pci(u8 sromrev, u16 *srom, uint off, struct brcms_varbuf *b)
 }
 
 /*
+ * Read in and validate sprom.
+ * Return 0 on success, nonzero on error.
+ */
+static int
+sprom_read_pci(struct si_pub *sih, u16 __iomem *sprom, uint wordoff,
+	       u16 *buf, uint nwords, bool check_crc)
+{
+	int err = 0;
+	uint i;
+
+	/* read the sprom */
+	for (i = 0; i < nwords; i++)
+		buf[i] = R_REG(&sprom[wordoff + i]);
+
+	if (check_crc) {
+
+		if (buf[0] == 0xffff)
+			/*
+			 * The hardware thinks that an srom that starts with
+			 * 0xffff is blank, regardless of the rest of the
+			 * content, so declare it bad.
+			 */
+			return -ENODATA;
+
+		/* fixup the endianness so crc8 will pass */
+		htol16_buf(buf, nwords * 2);
+		if (crc8(brcms_srom_crc8_table, (u8 *) buf, nwords * 2,
+			 CRC8_INIT_VALUE) !=
+			 CRC8_GOOD_VALUE(brcms_srom_crc8_table))
+			/* DBG only pci always read srom4 first, then srom8/9 */
+			err = -EIO;
+
+		/* now correct the endianness of the byte array */
+		ltoh16_buf(buf, nwords * 2);
+	}
+	return err;
+}
+
+static int otp_read_pci(struct si_pub *sih, u16 *buf, uint bufsz)
+{
+	u8 *otp;
+	uint sz = OTP_SZ_MAX / 2;	/* size in words */
+	int err = 0;
+
+	otp = kzalloc(OTP_SZ_MAX, GFP_ATOMIC);
+	if (otp == NULL)
+		return -ENOMEM;
+
+	err = otp_read_region(sih, OTP_HW_RGN, (u16 *) otp, &sz);
+
+	memcpy(buf, otp, bufsz);
+
+	kfree(otp);
+
+	/* Check CRC */
+	if (buf[0] == 0xffff)
+		/* The hardware thinks that an srom that starts with 0xffff
+		 * is blank, regardless of the rest of the content, so declare
+		 * it bad.
+		 */
+		return -ENODATA;
+
+	/* fixup the endianness so crc8 will pass */
+	htol16_buf(buf, bufsz);
+	if (crc8(brcms_srom_crc8_table, (u8 *) buf, SROM4_WORDS * 2,
+		 CRC8_INIT_VALUE) != CRC8_GOOD_VALUE(brcms_srom_crc8_table))
+		err = -EIO;
+
+	/* now correct the endianness of the byte array */
+	ltoh16_buf(buf, bufsz);
+
+	return err;
+}
+
+/*
  * Initialize nonvolatile variable table from sprom.
  * Return 0 on success, nonzero on error.
  */
-static int initvars_srom_pci(struct si_pub *sih, void *curmap, char **vars,
-			     uint *count)
+static int initvars_srom_pci(struct si_pub *sih, void __iomem *curmap)
 {
-	u16 *srom, *sromwindow;
+	u16 *srom;
+	u16 __iomem *sromwindow;
 	u8 sromrev = 0;
 	u32 sr;
-	struct brcms_varbuf b;
-	char *vp, *base = NULL;
 	int err = 0;
 
 	/*
@@ -1166,7 +1169,9 @@ static int initvars_srom_pci(struct si_pub *sih, void *curmap, char **vars,
 	if (!srom)
 		return -ENOMEM;
 
-	sromwindow = (u16 *) SROM_OFFSET(sih);
+	sromwindow = srom_window_address(sih, curmap);
+
+	crc8_populate_lsb(brcms_srom_crc8_table, SROM_CRC8_POLY);
 	if (ai_is_sprom_available(sih)) {
 		err = sprom_read_pci(sih, sromwindow, 0, srom, SROM_WORDS,
 				     true);
@@ -1188,50 +1193,106 @@ static int initvars_srom_pci(struct si_pub *sih, void *curmap, char **vars,
 			if (sromrev == 0x10)
 				sromrev = 1;
 		}
-	}
-#if defined(BCMNVRAMR)
-	/* Use OTP if SPROM not available */
-	else {
+	} else {
+		/* Use OTP if SPROM not available */
 		err = otp_read_pci(sih, srom, SROM_MAX);
 		if (err == 0)
 			/* OTP only contain SROM rev8/rev9 for now */
 			sromrev = srom[SROM4_CRCREV] & 0xff;
 	}
-#else
-	else
-		err = -ENODEV;
-#endif
 
 	if (!err) {
+		struct si_info *sii = (struct si_info *)sih;
+
 		/* Bitmask for the sromrev */
 		sr = 1 << sromrev;
 
-		/* srom version check: Current valid versions: 1, 2, 3, 4, 5, 8, 9 */
+		/*
+		 * srom version check: Current valid versions: 1, 2, 3, 4, 5, 8,
+		 * 9
+		 */
 		if ((sr & 0x33e) == 0) {
 			err = -EINVAL;
 			goto errout;
 		}
 
-		base = kmalloc(MAXSZ_NVRAM_VARS, GFP_ATOMIC);
-		if (!base) {
-			err = -ENOMEM;
-			goto errout;
-		}
-
-		varbuf_init(&b, base, MAXSZ_NVRAM_VARS);
+		INIT_LIST_HEAD(&sii->var_list);
 
 		/* parse SROM into name=value pairs. */
-		_initvars_srom_pci(sromrev, srom, 0, &b);
-
-		/* final nullbyte terminator */
-		vp = b.buf;
-		*vp++ = '\0';
-
-		err = initvars_table(base, vp, vars, count);
-		kfree(base);
+		_initvars_srom_pci(sromrev, srom, &sii->var_list);
 	}
 
 errout:
 	kfree(srom);
 	return err;
 }
+
+void srom_free_vars(struct si_pub *sih)
+{
+	struct si_info *sii;
+	struct brcms_srom_list_head *entry, *next;
+
+	sii = (struct si_info *)sih;
+	list_for_each_entry_safe(entry, next, &sii->var_list, var_list) {
+		list_del(&entry->var_list);
+		kfree(entry);
+	}
+}
+/*
+ * Initialize local vars from the right source for this platform.
+ * Return 0 on success, nonzero on error.
+ */
+int srom_var_init(struct si_pub *sih, void __iomem *curmap)
+{
+	uint len;
+
+	len = 0;
+
+	if (curmap != NULL)
+		return initvars_srom_pci(sih, curmap);
+
+	return -EINVAL;
+}
+
+/*
+ * Search the name=value vars for a specific one and return its value.
+ * Returns NULL if not found.
+ */
+char *getvar(struct si_pub *sih, enum brcms_srom_id id)
+{
+	struct si_info *sii;
+	struct brcms_srom_list_head *entry;
+
+	sii = (struct si_info *)sih;
+
+	list_for_each_entry(entry, &sii->var_list, var_list)
+		if (entry->varid == id)
+			return &entry->buf[0];
+
+	/* nothing found */
+	return NULL;
+}
+
+/*
+ * Search the vars for a specific one and return its value as
+ * an integer. Returns 0 if not found.-
+ */
+int getintvar(struct si_pub *sih, enum brcms_srom_id id)
+{
+	struct si_info *sii;
+	struct brcms_srom_list_head *entry;
+	unsigned long res;
+
+	sii = (struct si_info *)sih;
+
+	list_for_each_entry(entry, &sii->var_list, var_list)
+		if (entry->varid == id) {
+			if (entry->var_type == BRCMS_SROM_SNUMBER ||
+			    entry->var_type == BRCMS_SROM_UNUMBER)
+				return (int)entry->sval;
+			else if (!kstrtoul(&entry->buf[0], 0, &res))
+				return (int)res;
+		}
+
+	return 0;
+}
diff --git a/drivers/staging/brcm80211/brcmsmac/srom.h b/drivers/staging/brcm80211/brcmsmac/srom.h
index efc4d1edd86d..708c43ff51cc 100644
--- a/drivers/staging/brcm80211/brcmsmac/srom.h
+++ b/drivers/staging/brcm80211/brcmsmac/srom.h
@@ -20,8 +20,8 @@
 #include "types.h"
 
 /* Prototypes */
-extern int srom_var_init(struct si_pub *sih, uint bus, void *curmap,
-			 char **vars, uint *count);
+extern int srom_var_init(struct si_pub *sih, void __iomem *curmap);
+extern void srom_free_vars(struct si_pub *sih);
 
 extern int srom_read(struct si_pub *sih, uint bus, void *curmap,
 		     uint byteoff, uint nbytes, u16 *buf, bool check_crc);
diff --git a/drivers/staging/brcm80211/brcmsmac/stf.c b/drivers/staging/brcm80211/brcmsmac/stf.c
index a55ff0101782..d8f528eb180c 100644
--- a/drivers/staging/brcm80211/brcmsmac/stf.c
+++ b/drivers/staging/brcm80211/brcmsmac/stf.c
@@ -22,7 +22,6 @@
 #include "phy/phy_hal.h"
 #include "channel.h"
 #include "main.h"
-#include "bmac.h"
 #include "stf.h"
 
 #define MIN_SPATIAL_EXPANSION	0
@@ -31,20 +30,12 @@
 #define BRCMS_STF_SS_STBC_RX(wlc) (BRCMS_ISNPHY(wlc->band) && \
 	NREV_GT(wlc->band->phyrev, 3) && NREV_LE(wlc->band->phyrev, 6))
 
-static bool brcms_c_stf_stbc_tx_set(struct brcms_c_info *wlc, s32 int_val);
-static int brcms_c_stf_txcore_set(struct brcms_c_info *wlc, u8 Nsts, u8 val);
-static int brcms_c_stf_spatial_policy_set(struct brcms_c_info *wlc, int val);
-static void brcms_c_stf_stbc_rx_ht_update(struct brcms_c_info *wlc, int val);
-
-static void _brcms_c_stf_phy_txant_upd(struct brcms_c_info *wlc);
-static u16 _brcms_c_stf_phytxchain_sel(struct brcms_c_info *wlc,
-				       ratespec_t rspec);
-
 #define NSTS_1	1
 #define NSTS_2	2
 #define NSTS_3	3
 #define NSTS_4	4
-const u8 txcore_default[5] = {
+
+static const u8 txcore_default[5] = {
 	(0),			/* bitmap of the core enabled */
 	(0x01),			/* For Nsts = 1, enable core 1 */
 	(0x03),			/* For Nsts = 2, enable core 1 & 2 */
@@ -60,16 +51,16 @@ static void brcms_c_stf_stbc_rx_ht_update(struct brcms_c_info *wlc, int val)
 			return;
 	}
 
-	wlc->ht_cap.cap_info &= ~IEEE80211_HT_CAP_RX_STBC;
-	wlc->ht_cap.cap_info |= (val << IEEE80211_HT_CAP_RX_STBC_SHIFT);
-
 	if (wlc->pub->up) {
 		brcms_c_update_beacon(wlc);
 		brcms_c_update_probe_resp(wlc, true);
 	}
 }
 
-/* every WLC_TEMPSENSE_PERIOD seconds temperature check to decide whether to turn on/off txchain */
+/*
+ * every WLC_TEMPSENSE_PERIOD seconds temperature check to decide whether to
+ * turn on/off txchain.
+ */
 void brcms_c_tempsense_upd(struct brcms_c_info *wlc)
 {
 	struct brcms_phy_pub *pi = wlc->band->pi;
@@ -81,21 +72,19 @@ void brcms_c_tempsense_upd(struct brcms_c_info *wlc)
 	txchain = active_chains & 0xf;
 
 	if (wlc->stf->txchain == wlc->stf->hw_txchain) {
-		if (txchain && (txchain < wlc->stf->hw_txchain)) {
+		if (txchain && (txchain < wlc->stf->hw_txchain))
 			/* turn off 1 tx chain */
 			brcms_c_stf_txchain_set(wlc, txchain, true);
-		}
 	} else if (wlc->stf->txchain < wlc->stf->hw_txchain) {
-		if (txchain == wlc->stf->hw_txchain) {
+		if (txchain == wlc->stf->hw_txchain)
 			/* turn back on txchain */
 			brcms_c_stf_txchain_set(wlc, txchain, true);
-		}
 	}
 }
 
 void
 brcms_c_stf_ss_algo_channel_get(struct brcms_c_info *wlc, u16 *ss_algo_channel,
-			    chanspec_t chanspec)
+			    u16 chanspec)
 {
 	struct tx_power power;
 	u8 siso_mcs_id, cdd_mcs_id, stbc_mcs_id;
@@ -120,16 +109,19 @@ brcms_c_stf_ss_algo_channel_get(struct brcms_c_info *wlc, u16 *ss_algo_channel,
 
 	/* criteria to choose stf mode */
 
-	/* the "+3dbm (12 0.25db units)" is to account for the fact that with CDD, tx occurs
-	 * on both chains
+	/*
+	 * the "+3dbm (12 0.25db units)" is to account for the fact that with
+	 * CDD, tx occurs on both chains
 	 */
 	if (power.target[siso_mcs_id] > (power.target[cdd_mcs_id] + 12))
 		setbit(ss_algo_channel, PHY_TXC1_MODE_SISO);
 	else
 		setbit(ss_algo_channel, PHY_TXC1_MODE_CDD);
 
-	/* STBC is ORed into to algo channel as STBC requires per-packet SCB capability check
-	 * so cannot be default mode of operation. One of SISO, CDD have to be set
+	/*
+	 * STBC is ORed into to algo channel as STBC requires per-packet SCB
+	 * capability check so cannot be default mode of operation. One of
+	 * SISO, CDD have to be set
 	 */
 	if (power.target[siso_mcs_id] <= (power.target[stbc_mcs_id] + 12))
 		setbit(ss_algo_channel, PHY_TXC1_MODE_STBC);
@@ -137,19 +129,12 @@ brcms_c_stf_ss_algo_channel_get(struct brcms_c_info *wlc, u16 *ss_algo_channel,
 
 static bool brcms_c_stf_stbc_tx_set(struct brcms_c_info *wlc, s32 int_val)
 {
-	if ((int_val != AUTO) && (int_val != OFF) && (int_val != ON)) {
+	if ((int_val != AUTO) && (int_val != OFF) && (int_val != ON))
 		return false;
-	}
 
 	if ((int_val == ON) && (wlc->stf->txstreams == 1))
 		return false;
 
-	if ((int_val == OFF) || (wlc->stf->txstreams == 1)
-	    || !BRCMS_STBC_CAP_PHY(wlc))
-		wlc->ht_cap.cap_info &= ~IEEE80211_HT_CAP_TX_STBC;
-	else
-		wlc->ht_cap.cap_info |= IEEE80211_HT_CAP_TX_STBC;
-
 	wlc->bandstate[BAND_2G_INDEX]->band_stf_stbc_tx = (s8) int_val;
 	wlc->bandstate[BAND_5G_INDEX]->band_stf_stbc_tx = (s8) int_val;
 
@@ -159,9 +144,8 @@ static bool brcms_c_stf_stbc_tx_set(struct brcms_c_info *wlc, s32 int_val)
 bool brcms_c_stf_stbc_rx_set(struct brcms_c_info *wlc, s32 int_val)
 {
 	if ((int_val != HT_CAP_RX_STBC_NO)
-	    && (int_val != HT_CAP_RX_STBC_ONE_STREAM)) {
+	    && (int_val != HT_CAP_RX_STBC_ONE_STREAM))
 		return false;
-	}
 
 	if (BRCMS_STF_SS_STBC_RX(wlc)) {
 		if ((int_val != HT_CAP_RX_STBC_NO)
@@ -179,15 +163,13 @@ static int brcms_c_stf_txcore_set(struct brcms_c_info *wlc, u8 Nsts,
 	BCMMSG(wlc->wiphy, "wl%d: Nsts %d core_mask %x\n",
 		 wlc->pub->unit, Nsts, core_mask);
 
-	if (BRCMS_BITSCNT(core_mask) > wlc->stf->txstreams) {
+	if (hweight8(core_mask) > wlc->stf->txstreams)
 		core_mask = 0;
-	}
 
-	if ((BRCMS_BITSCNT(core_mask) == wlc->stf->txstreams) &&
+	if ((hweight8(core_mask) == wlc->stf->txstreams) &&
 	    ((core_mask & ~wlc->stf->txchain)
-	     || !(core_mask & wlc->stf->txchain))) {
+	     || !(core_mask & wlc->stf->txchain)))
 		core_mask = wlc->stf->txchain;
-	}
 
 	wlc->stf->txcore[Nsts] = core_mask;
 	/* Nsts = 1..4, txcore index = 1..4 */
@@ -223,6 +205,60 @@ static int brcms_c_stf_spatial_policy_set(struct brcms_c_info *wlc, int val)
 	return 0;
 }
 
+/*
+ * Centralized txant update function. call it whenever wlc->stf->txant and/or
+ * wlc->stf->txchain change.
+ *
+ * Antennas are controlled by ucode indirectly, which drives PHY or GPIO to
+ * achieve various tx/rx antenna selection schemes
+ *
+ * legacy phy, bit 6 and bit 7 means antenna 0 and 1 respectively, bit6+bit7
+ * means auto(last rx).
+ * for NREV<3, bit 6 and bit 7 means antenna 0 and 1 respectively, bit6+bit7
+ * means last rx and do tx-antenna selection for SISO transmissions
+ * for NREV=3, bit 6 and bit _8_ means antenna 0 and 1 respectively, bit6+bit7
+ * means last rx and do tx-antenna selection for SISO transmissions
+ * for NREV>=7, bit 6 and bit 7 mean antenna 0 and 1 respectively, nit6+bit7
+ * means both cores active
+*/
+static void _brcms_c_stf_phy_txant_upd(struct brcms_c_info *wlc)
+{
+	s8 txant;
+
+	txant = (s8) wlc->stf->txant;
+	if (BRCMS_PHY_11N_CAP(wlc->band)) {
+		if (txant == ANT_TX_FORCE_0) {
+			wlc->stf->phytxant = PHY_TXC_ANT_0;
+		} else if (txant == ANT_TX_FORCE_1) {
+			wlc->stf->phytxant = PHY_TXC_ANT_1;
+
+			if (BRCMS_ISNPHY(wlc->band) &&
+			    NREV_GE(wlc->band->phyrev, 3)
+			    && NREV_LT(wlc->band->phyrev, 7))
+				wlc->stf->phytxant = PHY_TXC_ANT_2;
+		} else {
+			if (BRCMS_ISLCNPHY(wlc->band) ||
+			    BRCMS_ISSSLPNPHY(wlc->band))
+				wlc->stf->phytxant = PHY_TXC_LCNPHY_ANT_LAST;
+			else {
+				/* catch out of sync wlc->stf->txcore */
+				WARN_ON(wlc->stf->txchain <= 0);
+				wlc->stf->phytxant =
+				    wlc->stf->txchain << PHY_TXC_ANT_SHIFT;
+			}
+		}
+	} else {
+		if (txant == ANT_TX_FORCE_0)
+			wlc->stf->phytxant = PHY_TXC_OLD_ANT_0;
+		else if (txant == ANT_TX_FORCE_1)
+			wlc->stf->phytxant = PHY_TXC_OLD_ANT_1;
+		else
+			wlc->stf->phytxant = PHY_TXC_OLD_ANT_LAST;
+	}
+
+	brcms_b_txant_set(wlc->hw, wlc->stf->phytxant);
+}
+
 int brcms_c_stf_txchain_set(struct brcms_c_info *wlc, s32 int_val, bool force)
 {
 	u8 txchain = (u8) int_val;
@@ -236,41 +272,14 @@ int brcms_c_stf_txchain_set(struct brcms_c_info *wlc, s32 int_val, bool force)
 	    || !(txchain & wlc->stf->hw_txchain))
 		return -EINVAL;
 
-	/* if nrate override is configured to be non-SISO STF mode, reject reducing txchain to 1 */
-	txstreams = (u8) BRCMS_BITSCNT(txchain);
+	/*
+	 * if nrate override is configured to be non-SISO STF mode, reject
+	 * reducing txchain to 1
+	 */
+	txstreams = (u8) hweight8(txchain);
 	if (txstreams > MAX_STREAMS_SUPPORTED)
 		return -EINVAL;
 
-	if (txstreams == 1) {
-		for (i = 0; i < NBANDS(wlc); i++)
-			if ((RSPEC_STF(wlc->bandstate[i]->rspec_override) !=
-			     PHY_TXC1_MODE_SISO)
-			    || (RSPEC_STF(wlc->bandstate[i]->mrspec_override) !=
-				PHY_TXC1_MODE_SISO)) {
-				if (!force)
-					return -EBADE;
-
-				/* over-write the override rspec */
-				if (RSPEC_STF(wlc->bandstate[i]->rspec_override)
-				    != PHY_TXC1_MODE_SISO) {
-					wlc->bandstate[i]->rspec_override = 0;
-					wiphy_err(wlc->wiphy, "%s(): temp "
-						  "sense override non-SISO "
-						  "rspec_override\n",
-						  __func__);
-				}
-				if (RSPEC_STF
-				    (wlc->bandstate[i]->mrspec_override) !=
-				    PHY_TXC1_MODE_SISO) {
-					wlc->bandstate[i]->mrspec_override = 0;
-					wiphy_err(wlc->wiphy, "%s(): temp "
-						  "sense override non-SISO "
-						  "mrspec_override\n",
-						  __func__);
-				}
-			}
-	}
-
 	wlc->stf->txchain = txchain;
 	wlc->stf->txstreams = txstreams;
 	brcms_c_stf_stbc_tx_set(wlc, wlc->band->band_stf_stbc_tx);
@@ -289,7 +298,10 @@ int brcms_c_stf_txchain_set(struct brcms_c_info *wlc, s32 int_val, bool force)
 	return 0;
 }
 
-/* update wlc->stf->ss_opmode which represents the operational stf_ss mode we're using */
+/*
+ * update wlc->stf->ss_opmode which represents the operational stf_ss mode
+ * we're using
+ */
 int brcms_c_stf_ss_update(struct brcms_c_info *wlc, struct brcms_band *band)
 {
 	int ret_code = 0;
@@ -298,21 +310,22 @@ int brcms_c_stf_ss_update(struct brcms_c_info *wlc, struct brcms_band *band)
 
 	prev_stf_ss = wlc->stf->ss_opmode;
 
-	/* NOTE: opmode can only be SISO or CDD as STBC is decided on a per-packet basis */
+	/*
+	 * NOTE: opmode can only be SISO or CDD as STBC is decided on a
+	 * per-packet basis
+	 */
 	if (BRCMS_STBC_CAP_PHY(wlc) &&
 	    wlc->stf->ss_algosel_auto
 	    && (wlc->stf->ss_algo_channel != (u16) -1)) {
-		upd_stf_ss = (wlc->stf->no_cddstbc || (wlc->stf->txstreams == 1)
-			      || isset(&wlc->stf->ss_algo_channel,
-				       PHY_TXC1_MODE_SISO)) ? PHY_TXC1_MODE_SISO
-		    : PHY_TXC1_MODE_CDD;
+		upd_stf_ss = (wlc->stf->txstreams == 1 ||
+			      isset(&wlc->stf->ss_algo_channel,
+				    PHY_TXC1_MODE_SISO)) ?
+				    PHY_TXC1_MODE_SISO : PHY_TXC1_MODE_CDD;
 	} else {
 		if (wlc->band != band)
 			return ret_code;
-		upd_stf_ss = (wlc->stf->no_cddstbc
-			      || (wlc->stf->txstreams ==
-				  1)) ? PHY_TXC1_MODE_SISO : band->
-		    band_stf_ss_mode;
+		upd_stf_ss = (wlc->stf->txstreams == 1) ?
+				PHY_TXC1_MODE_SISO : band->band_stf_ss_mode;
 	}
 	if (prev_stf_ss != upd_stf_ss) {
 		wlc->stf->ss_opmode = upd_stf_ss;
@@ -340,7 +353,8 @@ int brcms_c_stf_attach(struct brcms_c_info *wlc)
 
 	if (BRCMS_STBC_CAP_PHY(wlc)) {
 		wlc->stf->ss_algosel_auto = true;
-		wlc->stf->ss_algo_channel = (u16) -1;	/* Init the default value */
+		/* Init the default value */
+		wlc->stf->ss_algo_channel = (u16) -1;
 	}
 	return 0;
 }
@@ -349,59 +363,6 @@ void brcms_c_stf_detach(struct brcms_c_info *wlc)
 {
 }
 
-/*
- * Centralized txant update function. call it whenever wlc->stf->txant and/or wlc->stf->txchain
- *  change
- *
- * Antennas are controlled by ucode indirectly, which drives PHY or GPIO to
- *   achieve various tx/rx antenna selection schemes
- *
- * legacy phy, bit 6 and bit 7 means antenna 0 and 1 respectively, bit6+bit7 means auto(last rx)
- * for NREV<3, bit 6 and bit 7 means antenna 0 and 1 respectively, bit6+bit7 means last rx and
- *    do tx-antenna selection for SISO transmissions
- * for NREV=3, bit 6 and bit _8_ means antenna 0 and 1 respectively, bit6+bit7 means last rx and
- *    do tx-antenna selection for SISO transmissions
- * for NREV>=7, bit 6 and bit 7 mean antenna 0 and 1 respectively, nit6+bit7 means both cores active
-*/
-static void _brcms_c_stf_phy_txant_upd(struct brcms_c_info *wlc)
-{
-	s8 txant;
-
-	txant = (s8) wlc->stf->txant;
-	if (BRCMS_PHY_11N_CAP(wlc->band)) {
-		if (txant == ANT_TX_FORCE_0) {
-			wlc->stf->phytxant = PHY_TXC_ANT_0;
-		} else if (txant == ANT_TX_FORCE_1) {
-			wlc->stf->phytxant = PHY_TXC_ANT_1;
-
-			if (BRCMS_ISNPHY(wlc->band) &&
-			    NREV_GE(wlc->band->phyrev, 3)
-			    && NREV_LT(wlc->band->phyrev, 7)) {
-				wlc->stf->phytxant = PHY_TXC_ANT_2;
-			}
-		} else {
-			if (BRCMS_ISLCNPHY(wlc->band) ||
-			    BRCMS_ISSSLPNPHY(wlc->band))
-				wlc->stf->phytxant = PHY_TXC_LCNPHY_ANT_LAST;
-			else {
-				/* catch out of sync wlc->stf->txcore */
-				WARN_ON(wlc->stf->txchain <= 0);
-				wlc->stf->phytxant =
-				    wlc->stf->txchain << PHY_TXC_ANT_SHIFT;
-			}
-		}
-	} else {
-		if (txant == ANT_TX_FORCE_0)
-			wlc->stf->phytxant = PHY_TXC_OLD_ANT_0;
-		else if (txant == ANT_TX_FORCE_1)
-			wlc->stf->phytxant = PHY_TXC_OLD_ANT_1;
-		else
-			wlc->stf->phytxant = PHY_TXC_OLD_ANT_LAST;
-	}
-
-	brcms_b_txant_set(wlc->hw, wlc->stf->phytxant);
-}
-
 void brcms_c_stf_phy_txant_upd(struct brcms_c_info *wlc)
 {
 	_brcms_c_stf_phy_txant_upd(wlc);
@@ -410,31 +371,29 @@ void brcms_c_stf_phy_txant_upd(struct brcms_c_info *wlc)
 void brcms_c_stf_phy_chain_calc(struct brcms_c_info *wlc)
 {
 	/* get available rx/tx chains */
-	wlc->stf->hw_txchain = (u8) getintvar(wlc->pub->vars, "txchain");
-	wlc->stf->hw_rxchain = (u8) getintvar(wlc->pub->vars, "rxchain");
+	wlc->stf->hw_txchain = (u8) getintvar(wlc->hw->sih, BRCMS_SROM_TXCHAIN);
+	wlc->stf->hw_rxchain = (u8) getintvar(wlc->hw->sih, BRCMS_SROM_RXCHAIN);
 
 	/* these parameter are intended to be used for all PHY types */
 	if (wlc->stf->hw_txchain == 0 || wlc->stf->hw_txchain == 0xf) {
-		if (BRCMS_ISNPHY(wlc->band)) {
+		if (BRCMS_ISNPHY(wlc->band))
 			wlc->stf->hw_txchain = TXCHAIN_DEF_NPHY;
-		} else {
+		else
 			wlc->stf->hw_txchain = TXCHAIN_DEF;
-		}
 	}
 
 	wlc->stf->txchain = wlc->stf->hw_txchain;
-	wlc->stf->txstreams = (u8) BRCMS_BITSCNT(wlc->stf->hw_txchain);
+	wlc->stf->txstreams = (u8) hweight8(wlc->stf->hw_txchain);
 
 	if (wlc->stf->hw_rxchain == 0 || wlc->stf->hw_rxchain == 0xf) {
-		if (BRCMS_ISNPHY(wlc->band)) {
+		if (BRCMS_ISNPHY(wlc->band))
 			wlc->stf->hw_rxchain = RXCHAIN_DEF_NPHY;
-		} else {
+		else
 			wlc->stf->hw_rxchain = RXCHAIN_DEF;
-		}
 	}
 
 	wlc->stf->rxchain = wlc->stf->hw_rxchain;
-	wlc->stf->rxstreams = (u8) BRCMS_BITSCNT(wlc->stf->hw_rxchain);
+	wlc->stf->rxstreams = (u8) hweight8(wlc->stf->hw_rxchain);
 
 	/* initialize the txcore table */
 	memcpy(wlc->stf->txcore, txcore_default, sizeof(wlc->stf->txcore));
@@ -445,24 +404,24 @@ void brcms_c_stf_phy_chain_calc(struct brcms_c_info *wlc)
 }
 
 static u16 _brcms_c_stf_phytxchain_sel(struct brcms_c_info *wlc,
-				       ratespec_t rspec)
+				       u32 rspec)
 {
 	u16 phytxant = wlc->stf->phytxant;
 
-	if (RSPEC_STF(rspec) != PHY_TXC1_MODE_SISO) {
+	if (rspec_stf(rspec) != PHY_TXC1_MODE_SISO)
 		phytxant = wlc->stf->txchain << PHY_TXC_ANT_SHIFT;
-	} else if (wlc->stf->txant == ANT_TX_DEF)
+	else if (wlc->stf->txant == ANT_TX_DEF)
 		phytxant = wlc->stf->txchain << PHY_TXC_ANT_SHIFT;
 	phytxant &= PHY_TXC_ANT_MASK;
 	return phytxant;
 }
 
-u16 brcms_c_stf_phytxchain_sel(struct brcms_c_info *wlc, ratespec_t rspec)
+u16 brcms_c_stf_phytxchain_sel(struct brcms_c_info *wlc, u32 rspec)
 {
 	return _brcms_c_stf_phytxchain_sel(wlc, rspec);
 }
 
-u16 brcms_c_stf_d11hdrs_phyctl_txant(struct brcms_c_info *wlc, ratespec_t rspec)
+u16 brcms_c_stf_d11hdrs_phyctl_txant(struct brcms_c_info *wlc, u32 rspec)
 {
 	u16 phytxant = wlc->stf->phytxant;
 	u16 mask = PHY_TXC_ANT_MASK;
diff --git a/drivers/staging/brcm80211/brcmsmac/stf.h b/drivers/staging/brcm80211/brcmsmac/stf.h
index 06c2a399649e..19f6580f69be 100644
--- a/drivers/staging/brcm80211/brcmsmac/stf.h
+++ b/drivers/staging/brcm80211/brcmsmac/stf.h
@@ -25,7 +25,7 @@ extern void brcms_c_stf_detach(struct brcms_c_info *wlc);
 extern void brcms_c_tempsense_upd(struct brcms_c_info *wlc);
 extern void brcms_c_stf_ss_algo_channel_get(struct brcms_c_info *wlc,
 					u16 *ss_algo_channel,
-					chanspec_t chanspec);
+					u16 chanspec);
 extern int brcms_c_stf_ss_update(struct brcms_c_info *wlc,
 			     struct brcms_band *band);
 extern void brcms_c_stf_phy_txant_upd(struct brcms_c_info *wlc);
@@ -35,8 +35,8 @@ extern bool brcms_c_stf_stbc_rx_set(struct brcms_c_info *wlc, s32 int_val);
 extern void brcms_c_stf_phy_txant_upd(struct brcms_c_info *wlc);
 extern void brcms_c_stf_phy_chain_calc(struct brcms_c_info *wlc);
 extern u16 brcms_c_stf_phytxchain_sel(struct brcms_c_info *wlc,
-				      ratespec_t rspec);
+				      u32 rspec);
 extern u16 brcms_c_stf_d11hdrs_phyctl_txant(struct brcms_c_info *wlc,
-					ratespec_t rspec);
+					u32 rspec);
 
 #endif				/* _BRCM_STF_H_ */
diff --git a/drivers/staging/brcm80211/brcmsmac/types.h b/drivers/staging/brcm80211/brcmsmac/types.h
index 823b5e4672e2..27a814b07462 100644
--- a/drivers/staging/brcm80211/brcmsmac/types.h
+++ b/drivers/staging/brcm80211/brcmsmac/types.h
@@ -20,59 +20,77 @@
 #include <linux/types.h>
 #include <linux/io.h>
 
-/* Bus types */
-#define	SI_BUS			0	/* SOC Interconnect */
-#define	PCI_BUS			1	/* PCI target */
-#define SDIO_BUS		3	/* SDIO target */
-#define JTAG_BUS		4	/* JTAG */
-#define USB_BUS			5	/* USB (does not support R/W REG) */
-#define SPI_BUS			6	/* gSPI target */
-#define RPC_BUS			7	/* RPC target */
-
 #define WL_CHAN_FREQ_RANGE_2G      0
 #define WL_CHAN_FREQ_RANGE_5GL     1
 #define WL_CHAN_FREQ_RANGE_5GM     2
 #define WL_CHAN_FREQ_RANGE_5GH     3
 
-#define MAX_DMA_SEGS 4
-
 /* boardflags */
-#define	BFL_PACTRL		0x00000002	/* Board has gpio 9 controlling the PA */
-#define	BFL_NOPLLDOWN		0x00000020	/* Not ok to power down the chip pll and oscillator */
-#define BFL_FEM			0x00000800	/* Board supports the Front End Module */
-#define BFL_EXTLNA		0x00001000	/* Board has an external LNA in 2.4GHz band */
-#define BFL_NOPA		0x00010000	/* Board has no PA */
-#define BFL_BUCKBOOST		0x00200000	/* Power topology uses BUCKBOOST */
-#define BFL_FEM_BT		0x00400000	/* Board has FEM and switch to share antenna w/ BT */
-#define BFL_NOCBUCK		0x00800000	/* Power topology doesn't use CBUCK */
-#define BFL_PALDO		0x02000000	/* Power topology uses PALDO */
-#define BFL_EXTLNA_5GHz		0x10000000	/* Board has an external LNA in 5GHz band */
+
+/* Board has gpio 9 controlling the PA */
+#define	BFL_PACTRL		0x00000002
+/* Not ok to power down the chip pll and oscillator */
+#define	BFL_NOPLLDOWN		0x00000020
+/* Board supports the Front End Module */
+#define BFL_FEM			0x00000800
+/* Board has an external LNA in 2.4GHz band */
+#define BFL_EXTLNA		0x00001000
+/* Board has no PA */
+#define BFL_NOPA		0x00010000
+/* Power topology uses BUCKBOOST */
+#define BFL_BUCKBOOST		0x00200000
+/* Board has FEM and switch to share antenna w/ BT */
+#define BFL_FEM_BT		0x00400000
+/* Power topology doesn't use CBUCK */
+#define BFL_NOCBUCK		0x00800000
+/* Power topology uses PALDO */
+#define BFL_PALDO		0x02000000
+/* Board has an external LNA in 5GHz band */
+#define BFL_EXTLNA_5GHz		0x10000000
 
 /* boardflags2 */
-#define BFL2_RXBB_INT_REG_DIS	0x00000001	/* Board has an external rxbb regulator */
-#define BFL2_APLL_WAR		0x00000002	/* Flag to implement alternative A-band PLL settings */
-#define BFL2_TXPWRCTRL_EN	0x00000004	/* Board permits enabling TX Power Control */
-#define BFL2_2X4_DIV		0x00000008	/* Board supports the 2X4 diversity switch */
-#define BFL2_5G_PWRGAIN		0x00000010	/* Board supports 5G band power gain */
-#define BFL2_PCIEWAR_OVR	0x00000020	/* Board overrides ASPM and Clkreq settings */
+
+/* Board has an external rxbb regulator */
+#define BFL2_RXBB_INT_REG_DIS	0x00000001
+/* Flag to implement alternative A-band PLL settings */
+#define BFL2_APLL_WAR		0x00000002
+/* Board permits enabling TX Power Control */
+#define BFL2_TXPWRCTRL_EN	0x00000004
+/* Board supports the 2X4 diversity switch */
+#define BFL2_2X4_DIV		0x00000008
+/* Board supports 5G band power gain */
+#define BFL2_5G_PWRGAIN		0x00000010
+/* Board overrides ASPM and Clkreq settings */
+#define BFL2_PCIEWAR_OVR	0x00000020
 #define BFL2_LEGACY		0x00000080
-#define BFL2_SKWRKFEM_BRD	0x00000100	/* 4321mcm93 board uses Skyworks FEM */
-#define BFL2_SPUR_WAR		0x00000200	/* Board has a WAR for clock-harmonic spurs */
-#define BFL2_GPLL_WAR		0x00000400	/* Flag to narrow G-band PLL loop b/w */
-#define BFL2_SINGLEANT_CCK	0x00001000	/* Tx CCK pkts on Ant 0 only */
-#define BFL2_2G_SPUR_WAR	0x00002000	/* WAR to reduce and avoid clock-harmonic spurs in 2G */
-#define BFL2_GPLL_WAR2	        0x00010000	/* Flag to widen G-band PLL loop b/w */
+/* 4321mcm93 board uses Skyworks FEM */
+#define BFL2_SKWRKFEM_BRD	0x00000100
+/* Board has a WAR for clock-harmonic spurs */
+#define BFL2_SPUR_WAR		0x00000200
+/* Flag to narrow G-band PLL loop b/w */
+#define BFL2_GPLL_WAR		0x00000400
+/* Tx CCK pkts on Ant 0 only */
+#define BFL2_SINGLEANT_CCK	0x00001000
+/* WAR to reduce and avoid clock-harmonic spurs in 2G */
+#define BFL2_2G_SPUR_WAR	0x00002000
+/* Flag to widen G-band PLL loop b/w */
+#define BFL2_GPLL_WAR2	        0x00010000
 #define BFL2_IPALVLSHIFT_3P3    0x00020000
-#define BFL2_INTERNDET_TXIQCAL  0x00040000	/* Use internal envelope detector for TX IQCAL */
-#define BFL2_XTALBUFOUTEN       0x00080000	/* Keep the buffered Xtal output from radio "ON"
-						 * Most drivers will turn it off without this flag
-						 * to save power.
-						 */
+/* Use internal envelope detector for TX IQCAL */
+#define BFL2_INTERNDET_TXIQCAL  0x00040000
+/* Keep the buffered Xtal output from radio "ON". Most drivers will turn it
+ * off without this flag to save power. */
+#define BFL2_XTALBUFOUTEN       0x00080000
 
-/* board specific GPIO assignment, gpio 0-3 are also customer-configurable led */
-#define	BOARD_GPIO_PACTRL	0x200	/* bit 9 controls the PA on new 4306 boards */
-#define BOARD_GPIO_12		0x1000	/* gpio 12 */
-#define BOARD_GPIO_13		0x2000	/* gpio 13 */
+/*
+ * board specific GPIO assignment, gpio 0-3 are also customer-configurable
+ * led
+ */
+
+/* bit 9 controls the PA on new 4306 boards */
+#define	BOARD_GPIO_PACTRL	0x200
+#define BOARD_GPIO_12		0x1000
+#define BOARD_GPIO_13		0x2000
 
 /* **** Core type/rev defaults **** */
 #define D11CONF		0x0fffffb0	/* Supported  D11 revs: 4, 5, 7-27
@@ -152,30 +170,53 @@
 #define PHYCONF_HAS(val) CONF_HAS(PHYTYPE, val)
 #define PHYCONF_IS(val)	CONF_IS(PHYTYPE, val)
 
-#define NREV_IS(var, val)	(NCONF_HAS(val) && (NCONF_IS(val) || ((var) == (val))))
-#define NREV_GE(var, val)	(NCONF_GE(val) && (!NCONF_LT(val) || ((var) >= (val))))
-#define NREV_GT(var, val)	(NCONF_GT(val) && (!NCONF_LE(val) || ((var) > (val))))
-#define NREV_LT(var, val)	(NCONF_LT(val) && (!NCONF_GE(val) || ((var) < (val))))
-#define NREV_LE(var, val)	(NCONF_LE(val) && (!NCONF_GT(val) || ((var) <= (val))))
+#define NREV_IS(var, val) \
+	(NCONF_HAS(val) && (NCONF_IS(val) || ((var) == (val))))
+
+#define NREV_GE(var, val) \
+	(NCONF_GE(val) && (!NCONF_LT(val) || ((var) >= (val))))
+
+#define NREV_GT(var, val) \
+	(NCONF_GT(val) && (!NCONF_LE(val) || ((var) > (val))))
+
+#define NREV_LT(var, val) \
+	(NCONF_LT(val) && (!NCONF_GE(val) || ((var) < (val))))
+
+#define NREV_LE(var, val) \
+	(NCONF_LE(val) && (!NCONF_GT(val) || ((var) <= (val))))
+
+#define LCNREV_IS(var, val) \
+	(LCNCONF_HAS(val) && (LCNCONF_IS(val) || ((var) == (val))))
+
+#define LCNREV_GE(var, val) \
+	(LCNCONF_GE(val) && (!LCNCONF_LT(val) || ((var) >= (val))))
+
+#define LCNREV_GT(var, val) \
+	(LCNCONF_GT(val) && (!LCNCONF_LE(val) || ((var) > (val))))
+
+#define LCNREV_LT(var, val) \
+	(LCNCONF_LT(val) && (!LCNCONF_GE(val) || ((var) < (val))))
 
-#define LCNREV_IS(var, val)	(LCNCONF_HAS(val) && (LCNCONF_IS(val) || ((var) == (val))))
-#define LCNREV_GE(var, val)	(LCNCONF_GE(val) && (!LCNCONF_LT(val) || ((var) >= (val))))
-#define LCNREV_GT(var, val)	(LCNCONF_GT(val) && (!LCNCONF_LE(val) || ((var) > (val))))
-#define LCNREV_LT(var, val)	(LCNCONF_LT(val) && (!LCNCONF_GE(val) || ((var) < (val))))
-#define LCNREV_LE(var, val)	(LCNCONF_LE(val) && (!LCNCONF_GT(val) || ((var) <= (val))))
+#define LCNREV_LE(var, val) \
+	(LCNCONF_LE(val) && (!LCNCONF_GT(val) || ((var) <= (val))))
 
-#define D11REV_IS(var, val)	(D11CONF_HAS(val) && (D11CONF_IS(val) || ((var) == (val))))
-#define D11REV_GE(var, val)	(D11CONF_GE(val) && (!D11CONF_LT(val) || ((var) >= (val))))
-#define D11REV_GT(var, val)	(D11CONF_GT(val) && (!D11CONF_LE(val) || ((var) > (val))))
-#define D11REV_LT(var, val)	(D11CONF_LT(val) && (!D11CONF_GE(val) || ((var) < (val))))
-#define D11REV_LE(var, val)	(D11CONF_LE(val) && (!D11CONF_GT(val) || ((var) <= (val))))
+#define D11REV_IS(var, val) \
+	(D11CONF_HAS(val) && (D11CONF_IS(val) || ((var) == (val))))
 
-#define PHYTYPE_IS(var, val)	(PHYCONF_HAS(val) && (PHYCONF_IS(val) || ((var) == (val))))
+#define D11REV_GE(var, val) \
+	(D11CONF_GE(val) && (!D11CONF_LT(val) || ((var) >= (val))))
 
-/* Finally, early-exit from switch case if anyone wants it... */
+#define D11REV_GT(var, val) \
+	(D11CONF_GT(val) && (!D11CONF_LE(val) || ((var) > (val))))
 
-#define CASECHECK(config, val)	if (!(CONF_HAS(config, val))) break
-#define CASEMSK(config, mask)	if (!(CONF_MSK(config, mask))) break
+#define D11REV_LT(var, val) \
+	(D11CONF_LT(val) && (!D11CONF_GE(val) || ((var) < (val))))
+
+#define D11REV_LE(var, val) \
+	(D11CONF_LE(val) && (!D11CONF_GT(val) || ((var) <= (val))))
+
+#define PHYTYPE_IS(var, val)\
+	(PHYCONF_HAS(val) && (PHYCONF_IS(val) || ((var) == (val))))
 
 /* Set up PHYTYPE automatically: (depends on PHY_TYPE_X, from d11.h) */
 
@@ -203,116 +244,36 @@
  * ********************************************************************
  */
 
-/*************************************************
- * Defaults for tunables (e.g. sizing constants)
- *
- * For each new tunable, add a member to the end
- * of struct brcms_tunables in brcms_c_pub.h to enable
- * runtime checks of tunable values. (Directly
- * using the macros in code invalidates ROM code)
- *
- * ***********************************************
- */
-#define NTXD		256	/* Max # of entries in Tx FIFO based on 4kb page size */
-#define NRXD		256	/* Max # of entries in Rx FIFO based on 4kb page size */
-#define	NRXBUFPOST	32	/* try to keep this # rbufs posted to the chip */
-#define MAXSCB		32	/* Maximum SCBs in cache for STA */
-#define AMPDU_NUM_MPDU		16	/* max allowed number of mpdus in an ampdu (2 streams) */
-
-/* Count of packet callback structures. either of following
- * 1. Set to the number of SCBs since a STA
- * can queue up a rate callback for each IBSS STA it knows about, and an AP can
- * queue up an "are you there?" Null Data callback for each associated STA
- * 2. controlled by tunable config file
- */
-#define MAXPKTCB	MAXSCB	/* Max number of packet callbacks */
-
-/* NetBSD also needs to keep track of this */
-
-/* Number of BSS handled in ucode bcn/prb */
-#define BRCMS_MAX_UCODE_BSS	(16)
-/* Number of BSS handled in sw bcn/prb */
-#define BRCMS_MAX_UCODE_BSS4	(4)
-/* max # BSS configs */
-#define BRCMS_MAXBSSCFG		(1)
-/* max # available networks */
-#define MAXBSS		64
-/* data msg txq hiwat mark */
-#define BRCMS_DATAHIWAT		50
-#define BRCMS_AMPDUDATAHIWAT 255
-
-/* bounded rx loops */
-#define RXBND		8	/* max # frames to process in brcms_c_recv() */
-#define TXSBND		8	/* max # tx status to process in wlc_txstatus() */
-
-#define BAND_5G(bt)	((bt) == BRCM_BAND_5G)
-#define BAND_2G(bt)	((bt) == BRCM_BAND_2G)
-
 #define BCMMSG(dev, fmt, args...)		\
 do {						\
 	if (brcm_msg_level & LOG_TRACE_VAL)	\
 		wiphy_err(dev, "%s: " fmt, __func__, ##args);	\
 } while (0)
 
-#define WL_ERROR_ON()		(brcm_msg_level & LOG_ERROR_VAL)
-
-/* register access macros */
-#ifndef __BIG_ENDIAN
-#ifndef __mips__
-#define R_REG(r) \
-	({\
-		sizeof(*(r)) == sizeof(u8) ? \
-		readb((u8 *)(r)) : \
-		sizeof(*(r)) == sizeof(u16) ? readw((u16 *)(r)) : \
-		readl((u32 *)(r)); \
-	})
-#else				/* __mips__ */
-#define R_REG(r) \
-	({ \
-		__typeof(*(r)) __osl_v; \
-		__asm__ __volatile__("sync"); \
-		switch (sizeof(*(r))) { \
-		case sizeof(u8): \
-			__osl_v = readb((u8 *)(r)); \
-			break; \
-		case sizeof(u16): \
-			__osl_v = readw((u16 *)(r)); \
-			break; \
-		case sizeof(u32): \
-			__osl_v = \
-			readl((u32 *)(r)); \
-			break; \
-		} \
-		__asm__ __volatile__("sync"); \
-		__osl_v; \
-	})
-#endif				/* __mips__ */
+/*
+ * Register access macros.
+ *
+ * These macro's take a pointer to the address to read as one of their
+ * arguments. The macro itself deduces the size of the IO transaction (u8, u16
+ * or u32). Advantage of this approach in combination with using a struct to
+ * define the registers in a register block, is that access size and access
+ * location are defined in only one spot. This reduces the risk of the
+ * programmer trying to use an unsupported transaction size on a register.
+ *
+ */
 
-#define W_REG(r, v) do { \
-		switch (sizeof(*(r))) { \
-		case sizeof(u8): \
-			writeb((u8)(v), (u8 *)(r)); break; \
-		case sizeof(u16): \
-			writew((u16)(v), (u16 *)(r)); break; \
-		case sizeof(u32): \
-			writel((u32)(v), (u32 *)(r)); break; \
-		}; \
-	} while (0)
-#else				/* __BIG_ENDIAN */
 #define R_REG(r) \
 	({ \
 		__typeof(*(r)) __osl_v; \
 		switch (sizeof(*(r))) { \
 		case sizeof(u8): \
-			__osl_v = \
-			readb((u8 *)((r)^3)); \
+			__osl_v = readb((u8 __iomem *)(r)); \
 			break; \
 		case sizeof(u16): \
-			__osl_v = \
-			readw((u16 *)((r)^2)); \
+			__osl_v = readw((u16 __iomem *)(r)); \
 			break; \
 		case sizeof(u32): \
-			__osl_v = readl((u32 *)(r)); \
+			__osl_v = readl((u32 __iomem *)(r)); \
 			break; \
 		} \
 		__osl_v; \
@@ -321,19 +282,18 @@ do {						\
 #define W_REG(r, v) do { \
 		switch (sizeof(*(r))) { \
 		case sizeof(u8):	\
-			writeb((u8)(v), \
-			(u8 *)((r)^3)); break; \
+			writeb((u8)((v) & 0xFF), (u8 __iomem *)(r)); \
+			break; \
 		case sizeof(u16):	\
-			writew((u16)(v), \
-			(u16 *)((r)^2)); break; \
+			writew((u16)((v) & 0xFFFF), (u16 __iomem *)(r)); \
+			break; \
 		case sizeof(u32):	\
-			writel((u32)(v), \
-			(u32 *)(r)); break; \
+			writel((u32)(v), (u32 __iomem *)(r)); \
+			break; \
 		} \
 	} while (0)
-#endif				/* __BIG_ENDIAN */
 
-#ifdef __mips__
+#ifdef CONFIG_BCM47XX
 /*
  * bcm4716 (which includes 4717 & 4718), plus 4706 on PCIe can reorder
  * transactions. As a fix, a read after write is performed on certain places
@@ -342,7 +302,7 @@ do {						\
 #define W_REG_FLUSH(r, v)	({ W_REG((r), (v)); (void)R_REG(r); })
 #else
 #define W_REG_FLUSH(r, v)	W_REG((r), (v))
-#endif				/* __mips__ */
+#endif				/* CONFIG_BCM47XX */
 
 #define AND_REG(r, v)	W_REG((r), R_REG(r) & (v))
 #define OR_REG(r, v)	W_REG((r), R_REG(r) | (v))
@@ -351,12 +311,17 @@ do {						\
 		W_REG((r), ((R_REG(r) & ~(mask)) | (val)))
 
 /* multi-bool data type: set of bools, mbool is true if any is set */
-typedef u32 mbool;
-#define mboolset(mb, bit)		((mb) |= (bit))	/* set one bool */
-#define mboolclr(mb, bit)		((mb) &= ~(bit))	/* clear one bool */
-#define mboolisset(mb, bit)		(((mb) & (bit)) != 0)	/* true if one bool is set */
+
+/* set one bool */
+#define mboolset(mb, bit)		((mb) |= (bit))
+/* clear one bool */
+#define mboolclr(mb, bit)		((mb) &= ~(bit))
+/* true if one bool is set */
+#define mboolisset(mb, bit)		(((mb) & (bit)) != 0)
 #define	mboolmaskset(mb, mask, val)	((mb) = (((mb) & ~(mask)) | (val)))
 
+#define CEIL(x, y)		(((x) + ((y)-1)) / (y))
+
 /* forward declarations */
 struct wiphy;
 struct ieee80211_sta;
@@ -364,34 +329,22 @@ struct ieee80211_tx_queue_params;
 struct brcms_info;
 struct brcms_c_info;
 struct brcms_hardware;
-struct brcms_c_if;
-struct brcmu_iovar;
-struct brcmu_strbuf;
 struct brcms_txq_info;
 struct brcms_band;
 struct dma_pub;
 struct si_pub;
 struct tx_status;
 struct d11rxhdr;
-struct brcms_d11rxhdr;
 struct txpwr_limits;
-struct brcms_phy;
-
-typedef volatile struct intctrlregs intctrlregs_t;
-typedef volatile struct pio2regs pio2regs_t;
-typedef volatile struct pio2regp pio2regp_t;
-typedef volatile struct pio4regs pio4regs_t;
-typedef volatile struct pio4regp pio4regp_t;
-typedef volatile struct fifo64 fifo64_t;
-typedef volatile struct d11regs d11regs_t;
-typedef volatile struct dma32diag dma32diag_t;
-typedef volatile struct dma64regs dma64regs_t;
-typedef struct brcms_rateset wlc_rateset_t;
-typedef u32 ratespec_t;
-typedef struct chanvec chanvec_t;
-typedef s32 fixed;
-typedef struct _cs32 cs32;
-typedef volatile union pmqreg pmqreg_t;
+
+/* iovar structure */
+struct brcmu_iovar {
+	const char *name;	/* name for lookup and display */
+	u16 varid;	/* id for switch */
+	u16 flags;	/* driver-specific flag bits */
+	u16 type;	/* base type of argument */
+	u16 minlen;	/* min length for buffer vars */
+};
 
 /* brcm_msg_level is a bit vector with defs in defs.h */
 extern u32 brcm_msg_level;
diff --git a/drivers/staging/brcm80211/brcmsmac/ucode_loader.c b/drivers/staging/brcm80211/brcmsmac/ucode_loader.c
index bf733fb18ce1..80e3ccf865e3 100644
--- a/drivers/staging/brcm80211/brcmsmac/ucode_loader.c
+++ b/drivers/staging/brcm80211/brcmsmac/ucode_loader.c
@@ -37,79 +37,73 @@ enum {
 	D11UCODE_OVERSIGHT_BOMMINOR
 };
 
-struct d11init *d11lcn0bsinitvals24;
-struct d11init *d11lcn0initvals24;
-struct d11init *d11lcn1bsinitvals24;
-struct d11init *d11lcn1initvals24;
-struct d11init *d11lcn2bsinitvals24;
-struct d11init *d11lcn2initvals24;
-struct d11init *d11n0absinitvals16;
-struct d11init *d11n0bsinitvals16;
-struct d11init *d11n0initvals16;
-u32 *bcm43xx_16_mimo;
-u32 bcm43xx_16_mimosz;
-u32 *bcm43xx_24_lcn;
-u32 bcm43xx_24_lcnsz;
-u32 *bcm43xx_bommajor;
-u32 *bcm43xx_bomminor;
-
-int brcms_ucode_data_init(struct brcms_info *wl)
+int brcms_ucode_data_init(struct brcms_info *wl, struct brcms_ucode *ucode)
 {
 	int rc;
+
 	rc = brcms_check_firmwares(wl);
 
 	rc = rc < 0 ? rc :
-		brcms_ucode_init_buf(wl, (void **)&d11lcn0bsinitvals24,
+		brcms_ucode_init_buf(wl, (void **)&ucode->d11lcn0bsinitvals24,
 				     D11LCN0BSINITVALS24);
-	rc = rc < 0 ? rc : brcms_ucode_init_buf(wl, (void **)&d11lcn0initvals24,
-					     D11LCN0INITVALS24);
-	rc = rc < 0 ? rc :
-		brcms_ucode_init_buf(wl, (void **)&d11lcn1bsinitvals24,
-				     D11LCN1BSINITVALS24);
-	rc = rc < 0 ? rc : brcms_ucode_init_buf(wl, (void **)&d11lcn1initvals24,
-					     D11LCN1INITVALS24);
+	rc = rc < 0 ?
+	     rc : brcms_ucode_init_buf(wl, (void **)&ucode->d11lcn0initvals24,
+				       D11LCN0INITVALS24);
+	rc = rc < 0 ?
+	     rc : brcms_ucode_init_buf(wl, (void **)&ucode->d11lcn1bsinitvals24,
+				       D11LCN1BSINITVALS24);
+	rc = rc < 0 ?
+	     rc : brcms_ucode_init_buf(wl, (void **)&ucode->d11lcn1initvals24,
+				       D11LCN1INITVALS24);
 	rc = rc < 0 ? rc :
-		brcms_ucode_init_buf(wl, (void **)&d11lcn2bsinitvals24,
+		brcms_ucode_init_buf(wl, (void **)&ucode->d11lcn2bsinitvals24,
 				     D11LCN2BSINITVALS24);
-	rc = rc < 0 ? rc : brcms_ucode_init_buf(wl, (void **)&d11lcn2initvals24,
-					     D11LCN2INITVALS24);
-	rc = rc < 0 ? rc :
-		brcms_ucode_init_buf(wl, (void **)&d11n0absinitvals16,
-				     D11N0ABSINITVALS16);
-	rc = rc < 0 ? rc : brcms_ucode_init_buf(wl, (void **)&d11n0bsinitvals16,
-					     D11N0BSINITVALS16);
-	rc = rc < 0 ? rc : brcms_ucode_init_buf(wl, (void **)&d11n0initvals16,
-					     D11N0INITVALS16);
-	rc = rc < 0 ? rc : brcms_ucode_init_buf(wl, (void **)&bcm43xx_16_mimo,
-					     D11UCODE_OVERSIGHT16_MIMO);
-	rc = rc < 0 ? rc : brcms_ucode_init_uint(wl, &bcm43xx_16_mimosz,
-					      D11UCODE_OVERSIGHT16_MIMOSZ);
-	rc = rc < 0 ? rc : brcms_ucode_init_buf(wl, (void **)&bcm43xx_24_lcn,
-					     D11UCODE_OVERSIGHT24_LCN);
-	rc = rc < 0 ? rc : brcms_ucode_init_uint(wl, &bcm43xx_24_lcnsz,
-					      D11UCODE_OVERSIGHT24_LCNSZ);
-	rc = rc < 0 ? rc : brcms_ucode_init_buf(wl, (void **)&bcm43xx_bommajor,
-					     D11UCODE_OVERSIGHT_BOMMAJOR);
-	rc = rc < 0 ? rc : brcms_ucode_init_buf(wl, (void **)&bcm43xx_bomminor,
-					     D11UCODE_OVERSIGHT_BOMMINOR);
+	rc = rc < 0 ?
+	     rc : brcms_ucode_init_buf(wl, (void **)&ucode->d11lcn2initvals24,
+				       D11LCN2INITVALS24);
+	rc = rc < 0 ?
+	     rc : brcms_ucode_init_buf(wl, (void **)&ucode->d11n0absinitvals16,
+				       D11N0ABSINITVALS16);
+	rc = rc < 0 ?
+	     rc : brcms_ucode_init_buf(wl, (void **)&ucode->d11n0bsinitvals16,
+				       D11N0BSINITVALS16);
+	rc = rc < 0 ?
+	     rc : brcms_ucode_init_buf(wl, (void **)&ucode->d11n0initvals16,
+				       D11N0INITVALS16);
+	rc = rc < 0 ?
+	     rc : brcms_ucode_init_buf(wl, (void **)&ucode->bcm43xx_16_mimo,
+				       D11UCODE_OVERSIGHT16_MIMO);
+	rc = rc < 0 ?
+	     rc : brcms_ucode_init_uint(wl, &ucode->bcm43xx_16_mimosz,
+					D11UCODE_OVERSIGHT16_MIMOSZ);
+	rc = rc < 0 ?
+	     rc : brcms_ucode_init_buf(wl, (void **)&ucode->bcm43xx_24_lcn,
+				       D11UCODE_OVERSIGHT24_LCN);
+	rc = rc < 0 ?
+	     rc : brcms_ucode_init_uint(wl, &ucode->bcm43xx_24_lcnsz,
+					D11UCODE_OVERSIGHT24_LCNSZ);
+	rc = rc < 0 ?
+	     rc : brcms_ucode_init_buf(wl, (void **)&ucode->bcm43xx_bommajor,
+				       D11UCODE_OVERSIGHT_BOMMAJOR);
+	rc = rc < 0 ?
+	     rc : brcms_ucode_init_buf(wl, (void **)&ucode->bcm43xx_bomminor,
+				       D11UCODE_OVERSIGHT_BOMMINOR);
 	return rc;
 }
 
-void brcms_ucode_data_free(void)
+void brcms_ucode_data_free(struct brcms_ucode *ucode)
 {
-	brcms_ucode_free_buf((void *)d11lcn0bsinitvals24);
-	brcms_ucode_free_buf((void *)d11lcn0initvals24);
-	brcms_ucode_free_buf((void *)d11lcn1bsinitvals24);
-	brcms_ucode_free_buf((void *)d11lcn1initvals24);
-	brcms_ucode_free_buf((void *)d11lcn2bsinitvals24);
-	brcms_ucode_free_buf((void *)d11lcn2initvals24);
-	brcms_ucode_free_buf((void *)d11n0absinitvals16);
-	brcms_ucode_free_buf((void *)d11n0bsinitvals16);
-	brcms_ucode_free_buf((void *)d11n0initvals16);
-	brcms_ucode_free_buf((void *)bcm43xx_16_mimo);
-	brcms_ucode_free_buf((void *)bcm43xx_24_lcn);
-	brcms_ucode_free_buf((void *)bcm43xx_bommajor);
-	brcms_ucode_free_buf((void *)bcm43xx_bomminor);
-
-	return;
+	brcms_ucode_free_buf((void *)ucode->d11lcn0bsinitvals24);
+	brcms_ucode_free_buf((void *)ucode->d11lcn0initvals24);
+	brcms_ucode_free_buf((void *)ucode->d11lcn1bsinitvals24);
+	brcms_ucode_free_buf((void *)ucode->d11lcn1initvals24);
+	brcms_ucode_free_buf((void *)ucode->d11lcn2bsinitvals24);
+	brcms_ucode_free_buf((void *)ucode->d11lcn2initvals24);
+	brcms_ucode_free_buf((void *)ucode->d11n0absinitvals16);
+	brcms_ucode_free_buf((void *)ucode->d11n0bsinitvals16);
+	brcms_ucode_free_buf((void *)ucode->d11n0initvals16);
+	brcms_ucode_free_buf((void *)ucode->bcm43xx_16_mimo);
+	brcms_ucode_free_buf((void *)ucode->bcm43xx_24_lcn);
+	brcms_ucode_free_buf((void *)ucode->bcm43xx_bommajor);
+	brcms_ucode_free_buf((void *)ucode->bcm43xx_bomminor);
 }
diff --git a/drivers/staging/brcm80211/brcmsmac/ucode_loader.h b/drivers/staging/brcm80211/brcmsmac/ucode_loader.h
index ca53deced7bf..18750a814b4f 100644
--- a/drivers/staging/brcm80211/brcmsmac/ucode_loader.h
+++ b/drivers/staging/brcm80211/brcmsmac/ucode_loader.h
@@ -13,6 +13,8 @@
  * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
+#ifndef	_BRCM_UCODE_H_
+#define	_BRCM_UCODE_H_
 
 #include "types.h"		/* forward structure declarations */
 
@@ -21,32 +23,36 @@
 
 #define UCODE_LOADER_API_VER 0
 
-struct d11init {
-	u16 addr;
-	u16 size;
-	u32 value;
+struct d11init;
+
+struct brcms_ucode {
+	struct d11init *d11lcn0bsinitvals24;
+	struct d11init *d11lcn0initvals24;
+	struct d11init *d11lcn1bsinitvals24;
+	struct d11init *d11lcn1initvals24;
+	struct d11init *d11lcn2bsinitvals24;
+	struct d11init *d11lcn2initvals24;
+	struct d11init *d11n0absinitvals16;
+	struct d11init *d11n0bsinitvals16;
+	struct d11init *d11n0initvals16;
+	__le32 *bcm43xx_16_mimo;
+	size_t bcm43xx_16_mimosz;
+	__le32 *bcm43xx_24_lcn;
+	size_t bcm43xx_24_lcnsz;
+	u32 *bcm43xx_bommajor;
+	u32 *bcm43xx_bomminor;
 };
 
-extern struct d11init *d11lcn0bsinitvals24;
-extern struct d11init *d11lcn0initvals24;
-extern struct d11init *d11lcn1bsinitvals24;
-extern struct d11init *d11lcn1initvals24;
-extern struct d11init *d11lcn2bsinitvals24;
-extern struct d11init *d11lcn2initvals24;
-extern struct d11init *d11n0absinitvals16;
-extern struct d11init *d11n0bsinitvals16;
-extern struct d11init *d11n0initvals16;
-extern u32 *bcm43xx_16_mimo;
-extern u32 bcm43xx_16_mimosz;
-extern u32 *bcm43xx_24_lcn;
-extern u32 bcm43xx_24_lcnsz;
-
-extern int brcms_ucode_data_init(struct brcms_info *wl);
-extern void brcms_ucode_data_free(void);
+extern int
+brcms_ucode_data_init(struct brcms_info *wl, struct brcms_ucode *ucode);
+
+extern void brcms_ucode_data_free(struct brcms_ucode *ucode);
 
 extern int brcms_ucode_init_buf(struct brcms_info *wl, void **pbuf,
 				unsigned int idx);
-extern int brcms_ucode_init_uint(struct brcms_info *wl, unsigned *data,
-			      unsigned int idx);
+extern int brcms_ucode_init_uint(struct brcms_info *wl, size_t *n_bytes,
+				 unsigned int idx);
 extern void brcms_ucode_free_buf(void *);
 extern int  brcms_check_firmwares(struct brcms_info *wl);
+
+#endif	/* _BRCM_UCODE_H_ */
diff --git a/drivers/staging/brcm80211/brcmutil/Makefile b/drivers/staging/brcm80211/brcmutil/Makefile
index 6403423c0212..45b808d747e2 100644
--- a/drivers/staging/brcm80211/brcmutil/Makefile
+++ b/drivers/staging/brcm80211/brcmutil/Makefile
@@ -20,8 +20,7 @@ ccflags-y :=				\
 	-Idrivers/staging/brcm80211/include
 
 BRCMUTIL_OFILES := \
-	utils.o \
-	wifi.o
+	utils.o
 
 MODULEPFX := brcmutil
 
diff --git a/drivers/staging/brcm80211/brcmutil/utils.c b/drivers/staging/brcm80211/brcmutil/utils.c
index 37b6b7797793..f27c48910827 100644
--- a/drivers/staging/brcm80211/brcmutil/utils.c
+++ b/drivers/staging/brcm80211/brcmutil/utils.c
@@ -15,6 +15,7 @@
  */
 
 #include <linux/netdevice.h>
+#include <linux/module.h>
 #include <brcmu_utils.h>
 
 MODULE_AUTHOR("Broadcom Corporation");
@@ -219,7 +220,7 @@ EXPORT_SYMBOL(brcmu_pktq_pdeq_tail);
 
 void
 brcmu_pktq_pflush(struct pktq *pq, int prec, bool dir,
-	    ifpkt_cb_t fn, void *arg)
+		  bool (*fn)(struct sk_buff *, void *), void *arg)
 {
 	struct pktq_prec *q;
 	struct sk_buff *p, *prev = NULL;
@@ -244,14 +245,13 @@ brcmu_pktq_pflush(struct pktq *pq, int prec, bool dir,
 		}
 	}
 
-	if (q->head == NULL) {
+	if (q->head == NULL)
 		q->tail = NULL;
-	}
 }
 EXPORT_SYMBOL(brcmu_pktq_pflush);
 
 void brcmu_pktq_flush(struct pktq *pq, bool dir,
-		ifpkt_cb_t fn, void *arg)
+		      bool (*fn)(struct sk_buff *, void *), void *arg)
 {
 	int prec;
 	for (prec = 0; prec < pq->num_prec; prec++)
@@ -350,21 +350,6 @@ struct sk_buff *brcmu_pktq_mdeq(struct pktq *pq, uint prec_bmp,
 }
 EXPORT_SYMBOL(brcmu_pktq_mdeq);
 
-/* parse a xx:xx:xx:xx:xx:xx format ethernet address */
-int brcmu_ether_atoe(char *p, u8 *ea)
-{
-	int i = 0;
-
-	for (;;) {
-		ea[i++] = (char)simple_strtoul(p, &p, 16);
-		if (!*p++ || i == 6)
-			break;
-	}
-
-	return i == 6;
-}
-EXPORT_SYMBOL(brcmu_ether_atoe);
-
 #if defined(BCMDBG)
 /* pretty hex print a pkt buffer chain */
 void brcmu_prpkt(const char *msg, struct sk_buff *p0)
@@ -380,232 +365,11 @@ void brcmu_prpkt(const char *msg, struct sk_buff *p0)
 EXPORT_SYMBOL(brcmu_prpkt);
 #endif				/* defined(BCMDBG) */
 
-/* iovar table lookup */
-const struct brcmu_iovar *brcmu_iovar_lookup(const struct brcmu_iovar *table,
-					const char *name)
-{
-	const struct brcmu_iovar *vi;
-	const char *lookup_name;
-
-	/* skip any ':' delimited option prefixes */
-	lookup_name = strrchr(name, ':');
-	if (lookup_name != NULL)
-		lookup_name++;
-	else
-		lookup_name = name;
-
-	for (vi = table; vi->name; vi++) {
-		if (!strcmp(vi->name, lookup_name))
-			return vi;
-	}
-	/* ran to end of table */
-
-	return NULL;		/* var name not found */
-}
-EXPORT_SYMBOL(brcmu_iovar_lookup);
-
-int brcmu_iovar_lencheck(const struct brcmu_iovar *vi, void *arg, int len,
-			 bool set)
-{
-	int bcmerror = 0;
-
-	/* length check on io buf */
-	switch (vi->type) {
-	case IOVT_BOOL:
-	case IOVT_INT8:
-	case IOVT_INT16:
-	case IOVT_INT32:
-	case IOVT_UINT8:
-	case IOVT_UINT16:
-	case IOVT_UINT32:
-		/* all integers are s32 sized args at the ioctl interface */
-		if (len < (int)sizeof(int)) {
-			bcmerror = -EOVERFLOW;
-		}
-		break;
-
-	case IOVT_BUFFER:
-		/* buffer must meet minimum length requirement */
-		if (len < vi->minlen) {
-			bcmerror = -EOVERFLOW;
-		}
-		break;
-
-	case IOVT_VOID:
-		if (!set) {
-			/* Cannot return nil... */
-			bcmerror = -ENOTSUPP;
-		} else if (len) {
-			/* Set is an action w/o parameters */
-			bcmerror = -ENOBUFS;
-		}
-		break;
-
-	default:
-		/* unknown type for length check in iovar info */
-		bcmerror = -ENOTSUPP;
-	}
-
-	return bcmerror;
-}
-EXPORT_SYMBOL(brcmu_iovar_lencheck);
-
-/*******************************************************************************
- * crc8
- *
- * Computes a crc8 over the input data using the polynomial:
- *
- *       x^8 + x^7 +x^6 + x^4 + x^2 + 1
- *
- * The caller provides the initial value (either CRC8_INIT_VALUE
- * or the previous returned value) to allow for processing of
- * discontiguous blocks of data.  When generating the CRC the
- * caller is responsible for complementing the final return value
- * and inserting it into the byte stream.  When checking, a final
- * return value of CRC8_GOOD_VALUE indicates a valid CRC.
- *
- * Reference: Dallas Semiconductor Application Note 27
- *   Williams, Ross N., "A Painless Guide to CRC Error Detection Algorithms",
- *     ver 3, Aug 1993, ross@guest.adelaide.edu.au, Rocksoft Pty Ltd.,
- *     ftp://ftp.rocksoft.com/clients/rocksoft/papers/crc_v3.txt
- *
- * ****************************************************************************
- */
-
-static const u8 crc8_table[256] = {
-	0x00, 0xF7, 0xB9, 0x4E, 0x25, 0xD2, 0x9C, 0x6B,
-	0x4A, 0xBD, 0xF3, 0x04, 0x6F, 0x98, 0xD6, 0x21,
-	0x94, 0x63, 0x2D, 0xDA, 0xB1, 0x46, 0x08, 0xFF,
-	0xDE, 0x29, 0x67, 0x90, 0xFB, 0x0C, 0x42, 0xB5,
-	0x7F, 0x88, 0xC6, 0x31, 0x5A, 0xAD, 0xE3, 0x14,
-	0x35, 0xC2, 0x8C, 0x7B, 0x10, 0xE7, 0xA9, 0x5E,
-	0xEB, 0x1C, 0x52, 0xA5, 0xCE, 0x39, 0x77, 0x80,
-	0xA1, 0x56, 0x18, 0xEF, 0x84, 0x73, 0x3D, 0xCA,
-	0xFE, 0x09, 0x47, 0xB0, 0xDB, 0x2C, 0x62, 0x95,
-	0xB4, 0x43, 0x0D, 0xFA, 0x91, 0x66, 0x28, 0xDF,
-	0x6A, 0x9D, 0xD3, 0x24, 0x4F, 0xB8, 0xF6, 0x01,
-	0x20, 0xD7, 0x99, 0x6E, 0x05, 0xF2, 0xBC, 0x4B,
-	0x81, 0x76, 0x38, 0xCF, 0xA4, 0x53, 0x1D, 0xEA,
-	0xCB, 0x3C, 0x72, 0x85, 0xEE, 0x19, 0x57, 0xA0,
-	0x15, 0xE2, 0xAC, 0x5B, 0x30, 0xC7, 0x89, 0x7E,
-	0x5F, 0xA8, 0xE6, 0x11, 0x7A, 0x8D, 0xC3, 0x34,
-	0xAB, 0x5C, 0x12, 0xE5, 0x8E, 0x79, 0x37, 0xC0,
-	0xE1, 0x16, 0x58, 0xAF, 0xC4, 0x33, 0x7D, 0x8A,
-	0x3F, 0xC8, 0x86, 0x71, 0x1A, 0xED, 0xA3, 0x54,
-	0x75, 0x82, 0xCC, 0x3B, 0x50, 0xA7, 0xE9, 0x1E,
-	0xD4, 0x23, 0x6D, 0x9A, 0xF1, 0x06, 0x48, 0xBF,
-	0x9E, 0x69, 0x27, 0xD0, 0xBB, 0x4C, 0x02, 0xF5,
-	0x40, 0xB7, 0xF9, 0x0E, 0x65, 0x92, 0xDC, 0x2B,
-	0x0A, 0xFD, 0xB3, 0x44, 0x2F, 0xD8, 0x96, 0x61,
-	0x55, 0xA2, 0xEC, 0x1B, 0x70, 0x87, 0xC9, 0x3E,
-	0x1F, 0xE8, 0xA6, 0x51, 0x3A, 0xCD, 0x83, 0x74,
-	0xC1, 0x36, 0x78, 0x8F, 0xE4, 0x13, 0x5D, 0xAA,
-	0x8B, 0x7C, 0x32, 0xC5, 0xAE, 0x59, 0x17, 0xE0,
-	0x2A, 0xDD, 0x93, 0x64, 0x0F, 0xF8, 0xB6, 0x41,
-	0x60, 0x97, 0xD9, 0x2E, 0x45, 0xB2, 0xFC, 0x0B,
-	0xBE, 0x49, 0x07, 0xF0, 0x9B, 0x6C, 0x22, 0xD5,
-	0xF4, 0x03, 0x4D, 0xBA, 0xD1, 0x26, 0x68, 0x9F
-};
-
-u8 brcmu_crc8(u8 *pdata,	/* pointer to array of data to process */
-			 uint nbytes,	/* number of input data bytes to process */
-			 u8 crc	/* either CRC8_INIT_VALUE or previous return value */
-	) {
-	/* loop over the buffer data */
-	while (nbytes-- > 0)
-		crc = crc8_table[(crc ^ *pdata++) & 0xff];
-
-	return crc;
-}
-EXPORT_SYMBOL(brcmu_crc8);
-
+#if defined(BCMDBG)
 /*
- * Traverse a string of 1-byte tag/1-byte length/variable-length value
- * triples, returning a pointer to the substring whose first element
- * matches tag
+ * print bytes formatted as hex to a string. return the resulting
+ * string length
  */
-struct brcmu_tlv *brcmu_parse_tlvs(void *buf, int buflen, uint key)
-{
-	struct brcmu_tlv *elt;
-	int totlen;
-
-	elt = (struct brcmu_tlv *) buf;
-	totlen = buflen;
-
-	/* find tagged parameter */
-	while (totlen >= 2) {
-		int len = elt->len;
-
-		/* validate remaining totlen */
-		if ((elt->id == key) && (totlen >= (len + 2)))
-			return elt;
-
-		elt = (struct brcmu_tlv *) ((u8 *) elt + (len + 2));
-		totlen -= (len + 2);
-	}
-
-	return NULL;
-}
-EXPORT_SYMBOL(brcmu_parse_tlvs);
-
-
-#if defined(BCMDBG)
-int
-brcmu_format_flags(const struct brcmu_bit_desc *bd, u32 flags, char *buf,
-		   int len)
-{
-	int i;
-	char *p = buf;
-	char hexstr[16];
-	int slen = 0, nlen = 0;
-	u32 bit;
-	const char *name;
-
-	if (len < 2 || !buf)
-		return 0;
-
-	buf[0] = '\0';
-
-	for (i = 0; flags != 0; i++) {
-		bit = bd[i].bit;
-		name = bd[i].name;
-		if (bit == 0 && flags != 0) {
-			/* print any unnamed bits */
-			snprintf(hexstr, 16, "0x%X", flags);
-			name = hexstr;
-			flags = 0;	/* exit loop */
-		} else if ((flags & bit) == 0)
-			continue;
-		flags &= ~bit;
-		nlen = strlen(name);
-		slen += nlen;
-		/* count btwn flag space */
-		if (flags != 0)
-			slen += 1;
-		/* need NULL char as well */
-		if (len <= slen)
-			break;
-		/* copy NULL char but don't count it */
-		strncpy(p, name, nlen + 1);
-		p += nlen;
-		/* copy btwn flag space and NULL char */
-		if (flags != 0)
-			p += snprintf(p, 2, " ");
-		len -= slen;
-	}
-
-	/* indicate the str was too short */
-	if (flags != 0) {
-		if (len < 2)
-			p -= 2 - len;	/* overwrite last char */
-		p += snprintf(p, 2, ">");
-	}
-
-	return (int)(p - buf);
-}
-EXPORT_SYMBOL(brcmu_format_flags);
-
-/* print bytes formatted as hex to a string. return the resulting string length */
 int brcmu_format_hex(char *str, const void *bytes, int len)
 {
 	int i;
@@ -620,168 +384,3 @@ int brcmu_format_hex(char *str, const void *bytes, int len)
 }
 EXPORT_SYMBOL(brcmu_format_hex);
 #endif				/* defined(BCMDBG) */
-
-char *brcmu_chipname(uint chipid, char *buf, uint len)
-{
-	const char *fmt;
-
-	fmt = ((chipid > 0xa000) || (chipid < 0x4000)) ? "%d" : "%x";
-	snprintf(buf, len, fmt, chipid);
-	return buf;
-}
-EXPORT_SYMBOL(brcmu_chipname);
-
-uint brcmu_mkiovar(char *name, char *data, uint datalen, char *buf, uint buflen)
-{
-	uint len;
-
-	len = strlen(name) + 1;
-
-	if ((len + datalen) > buflen)
-		return 0;
-
-	strncpy(buf, name, buflen);
-
-	/* append data onto the end of the name string */
-	memcpy(&buf[len], data, datalen);
-	len += datalen;
-
-	return len;
-}
-EXPORT_SYMBOL(brcmu_mkiovar);
-
-/* Quarter dBm units to mW
- * Table starts at QDBM_OFFSET, so the first entry is mW for qdBm=153
- * Table is offset so the last entry is largest mW value that fits in
- * a u16.
- */
-
-#define QDBM_OFFSET 153		/* Offset for first entry */
-#define QDBM_TABLE_LEN 40	/* Table size */
-
-/* Smallest mW value that will round up to the first table entry, QDBM_OFFSET.
- * Value is ( mW(QDBM_OFFSET - 1) + mW(QDBM_OFFSET) ) / 2
- */
-#define QDBM_TABLE_LOW_BOUND 6493	/* Low bound */
-
-/* Largest mW value that will round down to the last table entry,
- * QDBM_OFFSET + QDBM_TABLE_LEN-1.
- * Value is ( mW(QDBM_OFFSET + QDBM_TABLE_LEN - 1) +
- * mW(QDBM_OFFSET + QDBM_TABLE_LEN) ) / 2.
- */
-#define QDBM_TABLE_HIGH_BOUND 64938	/* High bound */
-
-static const u16 nqdBm_to_mW_map[QDBM_TABLE_LEN] = {
-/* qdBm:	+0	+1	+2	+3	+4	+5	+6	+7 */
-/* 153: */ 6683, 7079, 7499, 7943, 8414, 8913, 9441, 10000,
-/* 161: */ 10593, 11220, 11885, 12589, 13335, 14125, 14962, 15849,
-/* 169: */ 16788, 17783, 18836, 19953, 21135, 22387, 23714, 25119,
-/* 177: */ 26607, 28184, 29854, 31623, 33497, 35481, 37584, 39811,
-/* 185: */ 42170, 44668, 47315, 50119, 53088, 56234, 59566, 63096
-};
-
-u16 brcmu_qdbm_to_mw(u8 qdbm)
-{
-	uint factor = 1;
-	int idx = qdbm - QDBM_OFFSET;
-
-	if (idx >= QDBM_TABLE_LEN) {
-		/* clamp to max u16 mW value */
-		return 0xFFFF;
-	}
-
-	/* scale the qdBm index up to the range of the table 0-40
-	 * where an offset of 40 qdBm equals a factor of 10 mW.
-	 */
-	while (idx < 0) {
-		idx += 40;
-		factor *= 10;
-	}
-
-	/* return the mW value scaled down to the correct factor of 10,
-	 * adding in factor/2 to get proper rounding.
-	 */
-	return (nqdBm_to_mW_map[idx] + factor / 2) / factor;
-}
-EXPORT_SYMBOL(brcmu_qdbm_to_mw);
-
-u8 brcmu_mw_to_qdbm(u16 mw)
-{
-	u8 qdbm;
-	int offset;
-	uint mw_uint = mw;
-	uint boundary;
-
-	/* handle boundary case */
-	if (mw_uint <= 1)
-		return 0;
-
-	offset = QDBM_OFFSET;
-
-	/* move mw into the range of the table */
-	while (mw_uint < QDBM_TABLE_LOW_BOUND) {
-		mw_uint *= 10;
-		offset -= 40;
-	}
-
-	for (qdbm = 0; qdbm < QDBM_TABLE_LEN - 1; qdbm++) {
-		boundary = nqdBm_to_mW_map[qdbm] + (nqdBm_to_mW_map[qdbm + 1] -
-						    nqdBm_to_mW_map[qdbm]) / 2;
-		if (mw_uint < boundary)
-			break;
-	}
-
-	qdbm += (u8) offset;
-
-	return qdbm;
-}
-EXPORT_SYMBOL(brcmu_mw_to_qdbm);
-
-uint brcmu_bitcount(u8 *bitmap, uint length)
-{
-	uint bitcount = 0, i;
-	u8 tmp;
-	for (i = 0; i < length; i++) {
-		tmp = bitmap[i];
-		while (tmp) {
-			bitcount++;
-			tmp &= (tmp - 1);
-		}
-	}
-	return bitcount;
-}
-EXPORT_SYMBOL(brcmu_bitcount);
-
-/* Initialization of brcmu_strbuf structure */
-void brcmu_binit(struct brcmu_strbuf *b, char *buf, uint size)
-{
-	b->origsize = b->size = size;
-	b->origbuf = b->buf = buf;
-}
-EXPORT_SYMBOL(brcmu_binit);
-
-/* Buffer sprintf wrapper to guard against buffer overflow */
-int brcmu_bprintf(struct brcmu_strbuf *b, const char *fmt, ...)
-{
-	va_list ap;
-	int r;
-
-	va_start(ap, fmt);
-	r = vsnprintf(b->buf, b->size, fmt, ap);
-
-	/* Non Ansi C99 compliant returns -1,
-	 * Ansi compliant return r >= b->size,
-	 * stdlib returns 0, handle all
-	 */
-	if ((r == -1) || (r >= (int)b->size) || (r == 0)) {
-		b->size = 0;
-	} else {
-		b->size -= r;
-		b->buf += r;
-	}
-
-	va_end(ap);
-
-	return r;
-}
-EXPORT_SYMBOL(brcmu_bprintf);
diff --git a/drivers/staging/brcm80211/brcmutil/wifi.c b/drivers/staging/brcm80211/brcmutil/wifi.c
deleted file mode 100644
index b9ffe8682a27..000000000000
--- a/drivers/staging/brcm80211/brcmutil/wifi.c
+++ /dev/null
@@ -1,131 +0,0 @@
-/*
- * Copyright (c) 2010 Broadcom Corporation
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
- * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
- * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
- * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-#include <brcmu_wifi.h>
-
-/*
- * Verify the chanspec is using a legal set of parameters, i.e. that the
- * chanspec specified a band, bw, ctl_sb and channel and that the
- * combination could be legal given any set of circumstances.
- * RETURNS: true is the chanspec is malformed, false if it looks good.
- */
-bool brcmu_chspec_malformed(chanspec_t chanspec)
-{
-	/* must be 2G or 5G band */
-	if (!CHSPEC_IS5G(chanspec) && !CHSPEC_IS2G(chanspec))
-		return true;
-	/* must be 20 or 40 bandwidth */
-	if (!CHSPEC_IS40(chanspec) && !CHSPEC_IS20(chanspec))
-		return true;
-
-	/* 20MHZ b/w must have no ctl sb, 40 must have a ctl sb */
-	if (CHSPEC_IS20(chanspec)) {
-		if (!CHSPEC_SB_NONE(chanspec))
-			return true;
-	} else {
-		if (!CHSPEC_SB_UPPER(chanspec) && !CHSPEC_SB_LOWER(chanspec))
-			return true;
-	}
-
-	return false;
-}
-EXPORT_SYMBOL(brcmu_chspec_malformed);
-
-/*
- * This function returns the channel number that control traffic is being sent on, for legacy
- * channels this is just the channel number, for 40MHZ channels it is the upper or lowre 20MHZ
- * sideband depending on the chanspec selected
- */
-u8 brcmu_chspec_ctlchan(chanspec_t chspec)
-{
-	u8 ctl_chan;
-
-	/* Is there a sideband ? */
-	if (CHSPEC_CTL_SB(chspec) == WL_CHANSPEC_CTL_SB_NONE) {
-		return CHSPEC_CHANNEL(chspec);
-	} else {
-		/* we only support 40MHZ with sidebands */
-		/* chanspec channel holds the centre frequency, use that and the
-		 * side band information to reconstruct the control channel number
-		 */
-		if (CHSPEC_CTL_SB(chspec) == WL_CHANSPEC_CTL_SB_UPPER) {
-			/* control chan is the upper 20 MHZ SB of the 40MHZ channel */
-			ctl_chan = UPPER_20_SB(CHSPEC_CHANNEL(chspec));
-		} else {
-			/* control chan is the lower 20 MHZ SB of the 40MHZ channel */
-			ctl_chan = LOWER_20_SB(CHSPEC_CHANNEL(chspec));
-		}
-	}
-
-	return ctl_chan;
-}
-EXPORT_SYMBOL(brcmu_chspec_ctlchan);
-
-/*
- * Return the channel number for a given frequency and base frequency.
- * The returned channel number is relative to the given base frequency.
- * If the given base frequency is zero, a base frequency of 5 GHz is assumed for
- * frequencies from 5 - 6 GHz, and 2.407 GHz is assumed for 2.4 - 2.5 GHz.
- *
- * Frequency is specified in MHz.
- * The base frequency is specified as (start_factor * 500 kHz).
- * Constants WF_CHAN_FACTOR_2_4_G, WF_CHAN_FACTOR_5_G are defined for
- * 2.4 GHz and 5 GHz bands.
- *
- * The returned channel will be in the range [1, 14] in the 2.4 GHz band
- * and [0, 200] otherwise.
- * -1 is returned if the start_factor is WF_CHAN_FACTOR_2_4_G and the
- * frequency is not a 2.4 GHz channel, or if the frequency is not and even
- * multiple of 5 MHz from the base frequency to the base plus 1 GHz.
- *
- * Reference 802.11 REVma, section 17.3.8.3, and 802.11B section 18.4.6.2
- */
-int brcmu_mhz2channel(uint freq, uint start_factor)
-{
-	int ch = -1;
-	uint base;
-	int offset;
-
-	/* take the default channel start frequency */
-	if (start_factor == 0) {
-		if (freq >= 2400 && freq <= 2500)
-			start_factor = WF_CHAN_FACTOR_2_4_G;
-		else if (freq >= 5000 && freq <= 6000)
-			start_factor = WF_CHAN_FACTOR_5_G;
-	}
-
-	if (freq == 2484 && start_factor == WF_CHAN_FACTOR_2_4_G)
-		return 14;
-
-	base = start_factor / 2;
-
-	/* check that the frequency is in 1GHz range of the base */
-	if ((freq < base) || (freq > base + 1000))
-		return -1;
-
-	offset = freq - base;
-	ch = offset / 5;
-
-	/* check that frequency is a 5MHz multiple from the base */
-	if (offset != (ch * 5))
-		return -1;
-
-	/* restricted channel range check for 2.4G */
-	if (start_factor == WF_CHAN_FACTOR_2_4_G && (ch < 1 || ch > 13))
-		return -1;
-
-	return ch;
-}
-EXPORT_SYMBOL(brcmu_mhz2channel);
diff --git a/drivers/staging/brcm80211/include/brcmu_utils.h b/drivers/staging/brcm80211/include/brcmu_utils.h
index 2d54cc5f4b11..7d0f46e0eb95 100644
--- a/drivers/staging/brcm80211/include/brcmu_utils.h
+++ b/drivers/staging/brcm80211/include/brcmu_utils.h
@@ -19,18 +19,6 @@
 
 #include <linux/skbuff.h>
 
-/* Buffer structure for collecting string-formatted data
-* using brcmu_bprintf() API.
-* Use brcmu_binit() to initialize before use
-*/
-
-struct brcmu_strbuf {
-	char *buf;	/* pointer to current position in origbuf */
-	unsigned int size;	/* current (residual) size in bytes */
-	char *origbuf;	/* unmodified pointer to orignal buffer */
-	unsigned int origsize;	/* unmodified orignal buffer size in bytes */
-};
-
 /*
  * Spin at most 'us' microseconds while 'exp' is true.
  * Caller should explicitly test 'exp' when this completes
@@ -45,12 +33,36 @@ struct brcmu_strbuf {
 }
 
 /* osl multi-precedence packet queue */
-#ifndef PKTQ_LEN_DEFAULT
 #define PKTQ_LEN_DEFAULT        128	/* Max 128 packets */
-#endif
-#ifndef PKTQ_MAX_PREC
 #define PKTQ_MAX_PREC           16	/* Maximum precedence levels */
-#endif
+
+#define BCME_STRLEN		64	/* Max string length for BCM errors */
+
+/* the largest reasonable packet buffer driver uses for ethernet MTU in bytes */
+#define	PKTBUFSZ	2048
+
+#ifndef setbit
+#ifndef NBBY			/* the BSD family defines NBBY */
+#define	NBBY	8		/* 8 bits per byte */
+#endif				/* #ifndef NBBY */
+#define	setbit(a, i)	(((u8 *)a)[(i)/NBBY] |= 1<<((i)%NBBY))
+#define	clrbit(a, i)	(((u8 *)a)[(i)/NBBY] &= ~(1<<((i)%NBBY)))
+#define	isset(a, i)	(((const u8 *)a)[(i)/NBBY] & (1<<((i)%NBBY)))
+#define	isclr(a, i)	((((const u8 *)a)[(i)/NBBY] & (1<<((i)%NBBY))) == 0)
+#endif				/* setbit */
+
+#define	NBITS(type)	(sizeof(type) * 8)
+#define NBITVAL(nbits)	(1 << (nbits))
+#define MAXBITVAL(nbits)	((1 << (nbits)) - 1)
+#define	NBITMASK(nbits)	MAXBITVAL(nbits)
+#define MAXNBVAL(nbyte)	MAXBITVAL((nbyte) * 8)
+
+/* crc defines */
+#define CRC16_INIT_VALUE 0xffff	/* Initial CRC16 checksum value */
+#define CRC16_GOOD_VALUE 0xf0b8	/* Good final CRC16 checksum value */
+
+/* 18-bytes of Ethernet address buffer length */
+#define ETHER_ADDR_STR_LEN	18
 
 struct pktq_prec {
 	struct sk_buff *head;	/* first packet to dequeue */
@@ -72,19 +84,37 @@ struct pktq {
 	struct pktq_prec q[PKTQ_MAX_PREC];
 };
 
-/* fn(pkt, arg).  return true if pkt belongs to if */
-typedef bool(*ifpkt_cb_t) (struct sk_buff *, void *);
-
 /* operations on a specific precedence in packet queue */
 
-#define pktq_psetmax(pq, prec, _max)    ((pq)->q[prec].max = (_max))
-#define pktq_plen(pq, prec)             ((pq)->q[prec].len)
-#define pktq_pavail(pq, prec)           ((pq)->q[prec].max - (pq)->q[prec].len)
-#define pktq_pfull(pq, prec)            ((pq)->q[prec].len >= (pq)->q[prec].max)
-#define pktq_pempty(pq, prec)           ((pq)->q[prec].len == 0)
+static inline int pktq_plen(struct pktq *pq, int prec)
+{
+	return pq->q[prec].len;
+}
 
-#define pktq_ppeek(pq, prec)            ((pq)->q[prec].head)
-#define pktq_ppeek_tail(pq, prec)       ((pq)->q[prec].tail)
+static inline int pktq_pavail(struct pktq *pq, int prec)
+{
+	return pq->q[prec].max - pq->q[prec].len;
+}
+
+static inline bool pktq_pfull(struct pktq *pq, int prec)
+{
+	return pq->q[prec].len >= pq->q[prec].max;
+}
+
+static inline bool pktq_pempty(struct pktq *pq, int prec)
+{
+	return pq->q[prec].len == 0;
+}
+
+static inline struct sk_buff *pktq_ppeek(struct pktq *pq, int prec)
+{
+	return pq->q[prec].head;
+}
+
+static inline struct sk_buff *pktq_ppeek_tail(struct pktq *pq, int prec)
+{
+	return pq->q[prec].tail;
+}
 
 extern struct sk_buff *brcmu_pktq_penq(struct pktq *pq, int prec,
 				 struct sk_buff *p);
@@ -98,8 +128,9 @@ extern struct sk_buff *brcmu_pkt_buf_get_skb(uint len);
 extern void brcmu_pkt_buf_free_skb(struct sk_buff *skb);
 
 /* Empty the queue at particular precedence level */
+/* callback function fn(pkt, arg) returns true if pkt belongs to if */
 extern void brcmu_pktq_pflush(struct pktq *pq, int prec,
-	bool dir, ifpkt_cb_t fn, void *arg);
+	bool dir, bool (*fn)(struct sk_buff *, void *), void *arg);
 
 /* operations on a set of precedences in packet queue */
 
@@ -109,25 +140,36 @@ extern struct sk_buff *brcmu_pktq_mdeq(struct pktq *pq, uint prec_bmp,
 
 /* operations on packet queue as a whole */
 
-#define pktq_len(pq)                    ((int)(pq)->len)
-#define pktq_max(pq)                    ((int)(pq)->max)
-#define pktq_avail(pq)                  ((int)((pq)->max - (pq)->len))
-#define pktq_full(pq)                   ((pq)->len >= (pq)->max)
-#define pktq_empty(pq)                  ((pq)->len == 0)
+static inline int pktq_len(struct pktq *pq)
+{
+	return (int)pq->len;
+}
+
+static inline int pktq_max(struct pktq *pq)
+{
+	return (int)pq->max;
+}
+
+static inline int pktq_avail(struct pktq *pq)
+{
+	return (int)(pq->max - pq->len);
+}
+
+static inline bool pktq_full(struct pktq *pq)
+{
+	return pq->len >= pq->max;
+}
 
-/* operations for single precedence queues */
-#define pktenq(pq, p)		brcmu_pktq_penq(((struct pktq *)pq), 0, (p))
-#define pktenq_head(pq, p)\
-	brcmu_pktq_penq_head(((struct pktq *)pq), 0, (p))
-#define pktdeq(pq)		brcmu_pktq_pdeq(((struct pktq *)pq), 0)
-#define pktdeq_tail(pq)		brcmu_pktq_pdeq_tail(((struct pktq *)pq), 0)
-#define pktqinit(pq, len)	brcmu_pktq_init(((struct pktq *)pq), 1, len)
+static inline bool pktq_empty(struct pktq *pq)
+{
+	return pq->len == 0;
+}
 
 extern void brcmu_pktq_init(struct pktq *pq, int num_prec, int max_len);
 /* prec_out may be NULL if caller is not interested in return value */
 extern struct sk_buff *brcmu_pktq_peek_tail(struct pktq *pq, int *prec_out);
 extern void brcmu_pktq_flush(struct pktq *pq, bool dir,
-	ifpkt_cb_t fn, void *arg);
+		bool (*fn)(struct sk_buff *, void *), void *arg);
 
 /* externs */
 /* packet */
@@ -135,9 +177,6 @@ extern uint brcmu_pktfrombuf(struct sk_buff *p,
 	uint offset, int len, unsigned char *buf);
 extern uint brcmu_pkttotlen(struct sk_buff *p);
 
-/* ethernet address */
-extern int brcmu_ether_atoe(char *p, u8 *ea);
-
 /* ip address */
 struct ipv4_addr;
 
@@ -147,155 +186,10 @@ extern void brcmu_prpkt(const char *msg, struct sk_buff *p0);
 #define brcmu_prpkt(a, b)
 #endif				/* BCMDBG */
 
-/* Support for sharing code across in-driver iovar implementations.
- * The intent is that a driver use this structure to map iovar names
- * to its (private) iovar identifiers, and the lookup function to
- * find the entry.  Macros are provided to map ids and get/set actions
- * into a single number space for a switch statement.
- */
-
-/* iovar structure */
-struct brcmu_iovar {
-	const char *name;	/* name for lookup and display */
-	u16 varid;	/* id for switch */
-	u16 flags;	/* driver-specific flag bits */
-	u16 type;	/* base type of argument */
-	u16 minlen;	/* min length for buffer vars */
-};
-
-/* varid definitions are per-driver, may use these get/set bits */
-
-/* IOVar action bits for id mapping */
-#define IOV_GET 0		/* Get an iovar */
-#define IOV_SET 1		/* Set an iovar */
-
-/* Varid to actionid mapping */
-#define IOV_GVAL(id)		((id)*2)
-#define IOV_SVAL(id)		(((id)*2)+IOV_SET)
-#define IOV_ISSET(actionid)	((actionid & IOV_SET) == IOV_SET)
-#define IOV_ID(actionid)	(actionid >> 1)
-
-extern const struct
-brcmu_iovar *brcmu_iovar_lookup(const struct brcmu_iovar *table,
-				const char *name);
-extern int brcmu_iovar_lencheck(const struct brcmu_iovar *table, void *arg,
-				int len, bool set);
-
-/* Base type definitions */
-#define IOVT_VOID	0	/* no value (implictly set only) */
-#define IOVT_BOOL	1	/* any value ok (zero/nonzero) */
-#define IOVT_INT8	2	/* integer values are range-checked */
-#define IOVT_UINT8	3	/* unsigned int 8 bits */
-#define IOVT_INT16	4	/* int 16 bits */
-#define IOVT_UINT16	5	/* unsigned int 16 bits */
-#define IOVT_INT32	6	/* int 32 bits */
-#define IOVT_UINT32	7	/* unsigned int 32 bits */
-#define IOVT_BUFFER	8	/* buffer is size-checked as per minlen */
-#define BCM_IOVT_VALID(type) (((unsigned int)(type)) <= IOVT_BUFFER)
-
-/* ** driver/apps-shared section ** */
-
-#define BCME_STRLEN		64	/* Max string length for BCM errors */
-
-#ifndef ABS
-#define	ABS(a)			(((a) < 0) ? -(a) : (a))
-#endif				/* ABS */
-
-#define CEIL(x, y)		(((x) + ((y)-1)) / (y))
-#define	ISPOWEROF2(x)		((((x)-1)&(x)) == 0)
-
-/* map physical to virtual I/O */
-#define REG_MAP(pa, size)       ioremap_nocache((unsigned long)(pa), \
-					(unsigned long)(size))
-
-/* the largest reasonable packet buffer driver uses for ethernet MTU in bytes */
-#define	PKTBUFSZ	2048
-
-#define OSL_SYSUPTIME()		((u32)jiffies * (1000 / HZ))
-
-#ifndef setbit
-#ifndef NBBY			/* the BSD family defines NBBY */
-#define	NBBY	8		/* 8 bits per byte */
-#endif				/* #ifndef NBBY */
-#define	setbit(a, i)	(((u8 *)a)[(i)/NBBY] |= 1<<((i)%NBBY))
-#define	clrbit(a, i)	(((u8 *)a)[(i)/NBBY] &= ~(1<<((i)%NBBY)))
-#define	isset(a, i)	(((const u8 *)a)[(i)/NBBY] & (1<<((i)%NBBY)))
-#define	isclr(a, i)	((((const u8 *)a)[(i)/NBBY] & (1<<((i)%NBBY))) == 0)
-#endif				/* setbit */
-
-#define	NBITS(type)	(sizeof(type) * 8)
-#define NBITVAL(nbits)	(1 << (nbits))
-#define MAXBITVAL(nbits)	((1 << (nbits)) - 1)
-#define	NBITMASK(nbits)	MAXBITVAL(nbits)
-#define MAXNBVAL(nbyte)	MAXBITVAL((nbyte) * 8)
-
-/* basic mux operation - can be optimized on several architectures */
-#define MUX(pred, true, false) ((pred) ? (true) : (false))
-
-/* modulo inc/dec - assumes x E [0, bound - 1] */
-#define MODDEC(x, bound) MUX((x) == 0, (bound) - 1, (x) - 1)
-#define MODINC(x, bound) MUX((x) == (bound) - 1, 0, (x) + 1)
-
-/* modulo inc/dec, bound = 2^k */
-#define MODDEC_POW2(x, bound) (((x) - 1) & ((bound) - 1))
-#define MODINC_POW2(x, bound) (((x) + 1) & ((bound) - 1))
-
-/* modulo add/sub - assumes x, y E [0, bound - 1] */
-#define MODADD(x, y, bound) \
-	MUX((x) + (y) >= (bound), (x) + (y) - (bound), (x) + (y))
-#define MODSUB(x, y, bound) \
-	MUX(((int)(x)) - ((int)(y)) < 0, (x) - (y) + (bound), (x) - (y))
-
-/* module add/sub, bound = 2^k */
-#define MODADD_POW2(x, y, bound) (((x) + (y)) & ((bound) - 1))
-#define MODSUB_POW2(x, y, bound) (((x) - (y)) & ((bound) - 1))
-
-/* crc defines */
-#define CRC8_INIT_VALUE  0xff	/* Initial CRC8 checksum value */
-#define CRC8_GOOD_VALUE  0x9f	/* Good final CRC8 checksum value */
-#define CRC16_INIT_VALUE 0xffff	/* Initial CRC16 checksum value */
-#define CRC16_GOOD_VALUE 0xf0b8	/* Good final CRC16 checksum value */
-
-/* brcmu_format_flags() bit description structure */
-struct brcmu_bit_desc {
-	u32 bit;
-	const char *name;
-};
-
-/* tag_ID/length/value_buffer tuple */
-struct brcmu_tlv {
-	u8 id;
-	u8 len;
-	u8 data[1];
-};
-
-#define ETHER_ADDR_STR_LEN	18	/* 18-bytes of Ethernet address buffer length */
-
 /* externs */
-/* crc */
-extern u8 brcmu_crc8(u8 *p, uint nbytes, u8 crc);
-
 /* format/print */
 #if defined(BCMDBG)
-extern int brcmu_format_flags(const struct brcmu_bit_desc *bd, u32 flags,
-			      char *buf, int len);
 extern int brcmu_format_hex(char *str, const void *bytes, int len);
 #endif
 
-extern char *brcmu_chipname(uint chipid, char *buf, uint len);
-
-extern struct brcmu_tlv *brcmu_parse_tlvs(void *buf, int buflen,
-					  uint key);
-
-/* power conversion */
-extern u16 brcmu_qdbm_to_mw(u8 qdbm);
-extern u8 brcmu_mw_to_qdbm(u16 mw);
-
-extern void brcmu_binit(struct brcmu_strbuf *b, char *buf, uint size);
-extern int brcmu_bprintf(struct brcmu_strbuf *b, const char *fmt, ...);
-
-extern uint brcmu_mkiovar(char *name, char *data, uint datalen,
-			  char *buf, uint len);
-extern uint brcmu_bitcount(u8 *bitmap, uint bytelength);
-
 #endif				/* _BRCMU_UTILS_H_ */
diff --git a/drivers/staging/brcm80211/include/brcmu_wifi.h b/drivers/staging/brcm80211/include/brcmu_wifi.h
index fde592bd9177..f10d30274c23 100644
--- a/drivers/staging/brcm80211/include/brcmu_wifi.h
+++ b/drivers/staging/brcm80211/include/brcmu_wifi.h
@@ -20,8 +20,10 @@
 #include <linux/if_ether.h>		/* for ETH_ALEN */
 #include <linux/ieee80211.h>		/* for WLAN_PMKID_LEN */
 
-/* A chanspec holds the channel number, band, bandwidth and control sideband */
-typedef u16 chanspec_t;
+/*
+ * A chanspec (u16) holds the channel number, band, bandwidth and control
+ * sideband
+ */
 
 /* channel defines */
 #define CH_UPPER_SB			0x01
@@ -29,13 +31,20 @@ typedef u16 chanspec_t;
 #define CH_EWA_VALID			0x04
 #define CH_20MHZ_APART			4
 #define CH_10MHZ_APART			2
-#define CH_5MHZ_APART			1	/* 2G band channels are 5 Mhz apart */
+#define CH_5MHZ_APART			1 /* 2G band channels are 5 Mhz apart */
 #define CH_MAX_2G_CHANNEL		14	/* Max channel in 2G band */
 #define BRCM_MAX_2G_CHANNEL	CH_MAX_2G_CHANNEL	/* legacy define */
-#define	MAXCHANNEL		224	/* max # supported channels. The max channel no is 216,
-					 * this is that + 1 rounded up to a multiple of NBBY (8).
-					 * DO NOT MAKE it > 255: channels are u8's all over
-					 */
+
+/* bandstate array indices */
+#define BAND_2G_INDEX		0	/* wlc->bandstate[x] index */
+#define BAND_5G_INDEX		1	/* wlc->bandstate[x] index */
+
+/*
+ * max # supported channels. The max channel no is 216, this is that + 1
+ * rounded up to a multiple of NBBY (8). DO NOT MAKE it > 255: channels are
+ * u8's all over
+*/
+#define	MAXCHANNEL		224
 
 #define WL_CHANSPEC_CHAN_MASK		0x00ff
 #define WL_CHANSPEC_CHAN_SHIFT		0
@@ -63,58 +72,78 @@ typedef u16 chanspec_t;
 #define WF_CHAN_FACTOR_5_G		10000	/* 5   GHz band, 5000 MHz */
 #define WF_CHAN_FACTOR_4_G		8000	/* 4.9 GHz band for Japan */
 
-/* channel defines */
-#define LOWER_20_SB(channel)	(((channel) > CH_10MHZ_APART) ? ((channel) - CH_10MHZ_APART) : 0)
-#define UPPER_20_SB(channel)	(((channel) < (MAXCHANNEL - CH_10MHZ_APART)) ? \
-				((channel) + CH_10MHZ_APART) : 0)
-#define CHSPEC_BANDUNIT(chspec)	(CHSPEC_IS5G(chspec) ? BAND_5G_INDEX : \
-						       BAND_2G_INDEX)
-#define CH20MHZ_CHSPEC(channel)	(chanspec_t)((chanspec_t)(channel) | WL_CHANSPEC_BW_20 | \
-				WL_CHANSPEC_CTL_SB_NONE | (((channel) <= CH_MAX_2G_CHANNEL) ? \
-				WL_CHANSPEC_BAND_2G : WL_CHANSPEC_BAND_5G))
-#define NEXT_20MHZ_CHAN(channel)	(((channel) < (MAXCHANNEL - CH_20MHZ_APART)) ? \
-					((channel) + CH_20MHZ_APART) : 0)
-#define CH40MHZ_CHSPEC(channel, ctlsb)	(chanspec_t) \
-					((channel) | (ctlsb) | WL_CHANSPEC_BW_40 | \
-					((channel) <= CH_MAX_2G_CHANNEL ? WL_CHANSPEC_BAND_2G : \
-					WL_CHANSPEC_BAND_5G))
 #define CHSPEC_CHANNEL(chspec)	((u8)((chspec) & WL_CHANSPEC_CHAN_MASK))
 #define CHSPEC_BAND(chspec)	((chspec) & WL_CHANSPEC_BAND_MASK)
 
-#ifdef WL11N_20MHZONLY
+#define CHSPEC_CTL_SB(chspec)	((chspec) & WL_CHANSPEC_CTL_SB_MASK)
+#define CHSPEC_BW(chspec)	((chspec) & WL_CHANSPEC_BW_MASK)
 
-#define CHSPEC_CTL_SB(chspec)	WL_CHANSPEC_CTL_SB_NONE
-#define CHSPEC_BW(chspec)	WL_CHANSPEC_BW_20
-#define CHSPEC_IS10(chspec)	0
-#define CHSPEC_IS20(chspec)	1
-#ifndef CHSPEC_IS40
-#define CHSPEC_IS40(chspec)	0
-#endif
+#define CHSPEC_IS10(chspec) \
+	(((chspec) & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_10)
 
-#else				/* !WL11N_20MHZONLY */
+#define CHSPEC_IS20(chspec) \
+	(((chspec) & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_20)
 
-#define CHSPEC_CTL_SB(chspec)	((chspec) & WL_CHANSPEC_CTL_SB_MASK)
-#define CHSPEC_BW(chspec)	((chspec) & WL_CHANSPEC_BW_MASK)
-#define CHSPEC_IS10(chspec)	(((chspec) & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_10)
-#define CHSPEC_IS20(chspec)	(((chspec) & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_20)
 #ifndef CHSPEC_IS40
-#define CHSPEC_IS40(chspec)	(((chspec) & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_40)
+#define CHSPEC_IS40(chspec) \
+	(((chspec) & WL_CHANSPEC_BW_MASK) == WL_CHANSPEC_BW_40)
 #endif
 
-#endif				/* !WL11N_20MHZONLY */
+#define CHSPEC_IS5G(chspec) \
+	(((chspec) & WL_CHANSPEC_BAND_MASK) == WL_CHANSPEC_BAND_5G)
+
+#define CHSPEC_IS2G(chspec) \
+	(((chspec) & WL_CHANSPEC_BAND_MASK) == WL_CHANSPEC_BAND_2G)
+
+#define CHSPEC_SB_NONE(chspec) \
+	(((chspec) & WL_CHANSPEC_CTL_SB_MASK) == WL_CHANSPEC_CTL_SB_NONE)
+
+#define CHSPEC_SB_UPPER(chspec) \
+	(((chspec) & WL_CHANSPEC_CTL_SB_MASK) == WL_CHANSPEC_CTL_SB_UPPER)
+
+#define CHSPEC_SB_LOWER(chspec) \
+	(((chspec) & WL_CHANSPEC_CTL_SB_MASK) == WL_CHANSPEC_CTL_SB_LOWER)
+
+#define CHSPEC_CTL_CHAN(chspec) \
+	((CHSPEC_SB_LOWER(chspec)) ? \
+	(lower_20_sb(((chspec) & WL_CHANSPEC_CHAN_MASK))) : \
+	(upper_20_sb(((chspec) & WL_CHANSPEC_CHAN_MASK))))
 
-#define CHSPEC_IS5G(chspec)	(((chspec) & WL_CHANSPEC_BAND_MASK) == WL_CHANSPEC_BAND_5G)
-#define CHSPEC_IS2G(chspec)	(((chspec) & WL_CHANSPEC_BAND_MASK) == WL_CHANSPEC_BAND_2G)
-#define CHSPEC_SB_NONE(chspec)	(((chspec) & WL_CHANSPEC_CTL_SB_MASK) == WL_CHANSPEC_CTL_SB_NONE)
-#define CHSPEC_SB_UPPER(chspec)	(((chspec) & WL_CHANSPEC_CTL_SB_MASK) == WL_CHANSPEC_CTL_SB_UPPER)
-#define CHSPEC_SB_LOWER(chspec)	(((chspec) & WL_CHANSPEC_CTL_SB_MASK) == WL_CHANSPEC_CTL_SB_LOWER)
-#define CHSPEC_CTL_CHAN(chspec)  ((CHSPEC_SB_LOWER(chspec)) ? \
-				  (LOWER_20_SB(((chspec) & WL_CHANSPEC_CHAN_MASK))) : \
-				  (UPPER_20_SB(((chspec) & WL_CHANSPEC_CHAN_MASK))))
 #define CHSPEC2BAND(chspec) (CHSPEC_IS5G(chspec) ? BRCM_BAND_5G : BRCM_BAND_2G)
 
 #define CHANSPEC_STR_LEN    8
 
+static inline int lower_20_sb(int channel)
+{
+	return channel > CH_10MHZ_APART ? (channel - CH_10MHZ_APART) : 0;
+}
+
+static inline int upper_20_sb(int channel)
+{
+	return (channel < (MAXCHANNEL - CH_10MHZ_APART)) ?
+	       channel + CH_10MHZ_APART : 0;
+}
+
+static inline int chspec_bandunit(u16 chspec)
+{
+	return CHSPEC_IS5G(chspec) ? BAND_5G_INDEX : BAND_2G_INDEX;
+}
+
+static inline u16 ch20mhz_chspec(int channel)
+{
+	u16 rc = channel <= CH_MAX_2G_CHANNEL ?
+		 WL_CHANSPEC_BAND_2G : WL_CHANSPEC_BAND_5G;
+
+	return	(u16)((u16)channel | WL_CHANSPEC_BW_20 |
+		      WL_CHANSPEC_CTL_SB_NONE | rc);
+}
+
+static inline int next_20mhz_chan(int channel)
+{
+	return channel < (MAXCHANNEL - CH_20MHZ_APART) ?
+	       channel + CH_20MHZ_APART : 0;
+}
+
 /* defined rate in 500kbps */
 #define BRCM_MAXRATE	108	/* in 500kbps units */
 #define BRCM_RATE_1M	2	/* in 500kbps units */
@@ -134,43 +163,10 @@ typedef u16 chanspec_t;
 
 #define MCSSET_LEN	16
 
-#define AC_BITMAP_TST(ab, ac)	(((ab) & (1 << (ac))) != 0)
-
-/*
- * Verify the chanspec is using a legal set of parameters, i.e. that the
- * chanspec specified a band, bw, ctl_sb and channel and that the
- * combination could be legal given any set of circumstances.
- * RETURNS: true is the chanspec is malformed, false if it looks good.
- */
-extern bool brcmu_chspec_malformed(chanspec_t chanspec);
-
-/*
- * This function returns the channel number that control traffic is being sent on, for legacy
- * channels this is just the channel number, for 40MHZ channels it is the upper or lowre 20MHZ
- * sideband depending on the chanspec selected
- */
-extern u8 brcmu_chspec_ctlchan(chanspec_t chspec);
-
-/*
- * Return the channel number for a given frequency and base frequency.
- * The returned channel number is relative to the given base frequency.
- * If the given base frequency is zero, a base frequency of 5 GHz is assumed for
- * frequencies from 5 - 6 GHz, and 2.407 GHz is assumed for 2.4 - 2.5 GHz.
- *
- * Frequency is specified in MHz.
- * The base frequency is specified as (start_factor * 500 kHz).
- * Constants WF_CHAN_FACTOR_2_4_G, WF_CHAN_FACTOR_5_G are defined for
- * 2.4 GHz and 5 GHz bands.
- *
- * The returned channel will be in the range [1, 14] in the 2.4 GHz band
- * and [0, 200] otherwise.
- * -1 is returned if the start_factor is WF_CHAN_FACTOR_2_4_G and the
- * frequency is not a 2.4 GHz channel, or if the frequency is not and even
- * multiple of 5 MHz from the base frequency to the base plus 1 GHz.
- *
- * Reference 802.11 REVma, section 17.3.8.3, and 802.11B section 18.4.6.2
- */
-extern int brcmu_mhz2channel(uint freq, uint start_factor);
+static inline bool ac_bitmap_tst(u8 bitmap, int prec)
+{
+	return (bitmap & (1 << (prec))) != 0;
+}
 
 /* Enumerate crypto algorithms */
 #define	CRYPTO_ALGO_OFF			0
@@ -183,11 +179,13 @@ extern int brcmu_mhz2channel(uint freq, uint start_factor);
 #define CRYPTO_ALGO_NALG		7
 
 /* wireless security bitvec */
+
 #define WEP_ENABLED		0x0001
 #define TKIP_ENABLED		0x0002
 #define AES_ENABLED		0x0004
 #define WSEC_SWFLAG		0x0008
-#define SES_OW_ENABLED		0x0040	/* to go into transition mode without setting wep */
+/* to go into transition mode without setting wep */
+#define SES_OW_ENABLED		0x0040
 
 /* WPA authentication mode bitvec */
 #define WPA_AUTH_DISABLED	0x0000	/* Legacy (i.e., non-WPA) */
@@ -196,7 +194,7 @@ extern int brcmu_mhz2channel(uint freq, uint start_factor);
 #define WPA_AUTH_PSK		0x0004	/* Pre-shared key */
 #define WPA_AUTH_RESERVED1	0x0008
 #define WPA_AUTH_RESERVED2	0x0010
-					/* #define WPA_AUTH_8021X 0x0020 *//* 802.1x, reserved */
+
 #define WPA2_AUTH_RESERVED1	0x0020
 #define WPA2_AUTH_UNSPECIFIED	0x0040	/* over 802.1x */
 #define WPA2_AUTH_PSK		0x0080	/* Pre-shared key */
@@ -218,26 +216,24 @@ extern int brcmu_mhz2channel(uint freq, uint start_factor);
 #define HT_CAP_RX_STBC_NO		0x0
 #define HT_CAP_RX_STBC_ONE_STREAM	0x1
 
-typedef struct _pmkid {
+struct pmkid {
 	u8 BSSID[ETH_ALEN];
 	u8 PMKID[WLAN_PMKID_LEN];
-} pmkid_t;
+};
 
-typedef struct _pmkid_list {
-	u32 npmkid;
-	pmkid_t pmkid[1];
-} pmkid_list_t;
+struct pmkid_list {
+	__le32 npmkid;
+	struct pmkid pmkid[1];
+};
 
-typedef struct _pmkid_cand {
+struct pmkid_cand {
 	u8 BSSID[ETH_ALEN];
 	u8 preauth;
-} pmkid_cand_t;
+};
 
-typedef struct _pmkid_cand_list {
+struct pmkid_cand_list {
 	u32 npmkid_cand;
-	pmkid_cand_t pmkid_cand[1];
-} pmkid_cand_list_t;
-
-typedef u8 ac_bitmap_t;
+	struct pmkid_cand pmkid_cand[1];
+};
 
 #endif				/* _BRCMU_WIFI_H_ */
diff --git a/drivers/staging/brcm80211/include/chipcommon.h b/drivers/staging/brcm80211/include/chipcommon.h
index 296582aced69..fefabc39e646 100644
--- a/drivers/staging/brcm80211/include/chipcommon.h
+++ b/drivers/staging/brcm80211/include/chipcommon.h
@@ -19,7 +19,7 @@
 
 #include "defs.h"		/* for PAD macro */
 
-typedef volatile struct {
+struct chipcregs {
 	u32 chipid;		/* 0x0 */
 	u32 capabilities;
 	u32 corecontrol;	/* corerev >= 1 */
@@ -214,7 +214,7 @@ typedef volatile struct {
 	u32 pmu_xtalfreq;	/* 0x66C, pmurev >= 10 */
 	u32 PAD[100];
 	u16 sromotp[768];
-} chipcregs_t;
+};
 
 /* chipid */
 #define	CID_ID_MASK		0x0000ffff	/* Chip Id mask */
@@ -231,7 +231,8 @@ typedef volatile struct {
 #define	CC_CAP_UARTS_MASK	0x00000003	/* Number of UARTs */
 #define CC_CAP_MIPSEB		0x00000004	/* MIPS is in big-endian mode */
 #define CC_CAP_UCLKSEL		0x00000018	/* UARTs clock select */
-#define CC_CAP_UINTCLK		0x00000008	/* UARTs are driven by internal divided clock */
+/* UARTs are driven by internal divided clock */
+#define CC_CAP_UINTCLK		0x00000008
 #define CC_CAP_UARTGPIO		0x00000020	/* UARTs own GPIOs 15:12 */
 #define CC_CAP_EXTBUS_MASK	0x000000c0	/* External bus mask */
 #define CC_CAP_EXTBUS_NONE	0x00000000	/* No ExtBus present */
@@ -248,10 +249,12 @@ typedef volatile struct {
 #define CC_CAP_BKPLN64		0x08000000	/* 64-bit backplane */
 #define	CC_CAP_PMU		0x10000000	/* PMU Present, rev >= 20 */
 #define	CC_CAP_SROM		0x40000000	/* Srom Present, rev >= 32 */
-#define	CC_CAP_NFLASH		0x80000000	/* Nand flash present, rev >= 35 */
+/* Nand flash present, rev >= 35 */
+#define	CC_CAP_NFLASH		0x80000000
 
 #define	CC_CAP2_SECI		0x00000001	/* SECI Present, rev >= 36 */
-#define	CC_CAP2_GSIO		0x00000002	/* GSIO (spi/i2c) present, rev >= 37 */
+/* GSIO (spi/i2c) present, rev >= 37 */
+#define	CC_CAP2_GSIO		0x00000002
 
 /* pmucapabilities */
 #define PCAP_REV_MASK	0x000000ff
diff --git a/drivers/staging/brcm80211/include/defs.h b/drivers/staging/brcm80211/include/defs.h
index 8b3e17dec150..1e5f310af1e7 100644
--- a/drivers/staging/brcm80211/include/defs.h
+++ b/drivers/staging/brcm80211/include/defs.h
@@ -27,14 +27,8 @@
 #define USB_BUS			5
 #define SPI_BUS			6
 
-#ifndef OFF
 #define	OFF	0
-#endif
-
-#ifndef ON
 #define	ON	1		/* ON = 1 */
-#endif
-
 #define	AUTO	(-1)		/* Auto = -1 */
 
 /*
@@ -54,27 +48,23 @@
 
 #define WL_NUMRATES		16	/* max # of rates in a rateset */
 
-typedef struct wl_rateset {
-	u32 count;		/* # rates in this set */
-	u8 rates[WL_NUMRATES];	/* rates in 500kbps units w/hi bit set if basic */
-} wl_rateset_t;
-
 #define BRCM_CNTRY_BUF_SZ	4	/* Country string is 3 bytes + NUL */
 
-#define BRCM_SET_CHANNEL				30
-#define BRCM_SET_SRL				32
-#define BRCM_SET_LRL				34
+#define BRCM_SET_CHANNEL	30
+#define BRCM_SET_SRL		32
+#define BRCM_SET_LRL		34
+#define BRCM_SET_BCNPRD		76
 
-#define BRCM_SET_RATESET				72
-#define BRCM_SET_BCNPRD				76
-#define BRCM_GET_CURR_RATESET			114	/* current rateset */
-#define BRCM_GET_PHYLIST				180
+#define BRCM_GET_CURR_RATESET	114	/* current rateset */
+#define BRCM_GET_PHYLIST	180
 
 /* Bit masks for radio disabled status - returned by WL_GET_RADIO */
+
 #define WL_RADIO_SW_DISABLE		(1<<0)
 #define WL_RADIO_HW_DISABLE		(1<<1)
 #define WL_RADIO_MPC_DISABLE		(1<<2)
-#define WL_RADIO_COUNTRY_DISABLE	(1<<3)	/* some countries don't support any channel */
+/* some countries don't support any channel */
+#define WL_RADIO_COUNTRY_DISABLE	(1<<3)
 
 /* Override bit for SET_TXPWR.  if set, ignore other level limits */
 #define WL_TXPWR_OVERRIDE	(1U<<31)
@@ -100,7 +90,9 @@ typedef struct wl_rateset {
 /*
  * Sonics Configuration Space Registers.
  */
-#define	SBCONFIGOFF		0xf00	/* core sbconfig regs are top 256bytes of regs */
+
+/* core sbconfig regs are top 256bytes of regs */
+#define	SBCONFIGOFF		0xf00
 
 /* cpp contortions to concatenate w/arg prescan */
 #ifndef	PAD
diff --git a/drivers/staging/brcm80211/include/soc.h b/drivers/staging/brcm80211/include/soc.h
index 6e5a705c4937..4fcb956ad9e0 100644
--- a/drivers/staging/brcm80211/include/soc.h
+++ b/drivers/staging/brcm80211/include/soc.h
@@ -17,11 +17,7 @@
 #ifndef	_BRCM_SOC_H
 #define	_BRCM_SOC_H
 
-#ifdef SI_ENUM_BASE_VARIABLE
-#define SI_ENUM_BASE		(sii->pub.si_enum_base)
-#else
 #define SI_ENUM_BASE		0x18000000	/* Enumeration space base */
-#endif				/* SI_ENUM_BASE_VARIABLE */
 
 /* core codes */
 #define	NODEV_CORE_ID		0x700	/* Invalid coreid */
@@ -81,9 +77,8 @@
 #define	DMEMS_CORE_ID		0x835	/* SDR/DDR1 memory controller core */
 #define	DEF_SHIM_COMP		0x837	/* SHIM component in ubus/6362 */
 #define OOB_ROUTER_CORE_ID	0x367	/* OOB router core ID */
-#define	DEF_AI_COMP		0xfff	/* Default component, in ai chips it maps all
-					 * unused address ranges
-					 */
+/* Default component, in ai chips it maps all unused address ranges */
+#define	DEF_AI_COMP		0xfff
 
 /* Common core control flags */
 #define	SICF_BIST_EN		0x8000
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index e90e3cceb5f8..b347cda3ca9e 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -1432,7 +1432,21 @@ static int do_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 	return ret;
 }
 
-static void comedi_unmap(struct vm_area_struct *area)
+
+static void comedi_vm_open(struct vm_area_struct *area)
+{
+	struct comedi_async *async;
+	struct comedi_device *dev;
+
+	async = area->vm_private_data;
+	dev = async->subdevice->device;
+
+	mutex_lock(&dev->mutex);
+	async->mmap_count++;
+	mutex_unlock(&dev->mutex);
+}
+
+static void comedi_vm_close(struct vm_area_struct *area)
 {
 	struct comedi_async *async;
 	struct comedi_device *dev;
@@ -1446,15 +1460,13 @@ static void comedi_unmap(struct vm_area_struct *area)
 }
 
 static struct vm_operations_struct comedi_vm_ops = {
-	.close = comedi_unmap,
+	.open = comedi_vm_open,
+	.close = comedi_vm_close,
 };
 
 static int comedi_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	const unsigned minor = iminor(file->f_dentry->d_inode);
-	struct comedi_device_file_info *dev_file_info =
-	    comedi_get_device_file_info(minor);
-	struct comedi_device *dev = dev_file_info->device;
 	struct comedi_async *async = NULL;
 	unsigned long start = vma->vm_start;
 	unsigned long size;
@@ -1462,6 +1474,15 @@ static int comedi_mmap(struct file *file, struct vm_area_struct *vma)
 	int i;
 	int retval;
 	struct comedi_subdevice *s;
+	struct comedi_device_file_info *dev_file_info;
+	struct comedi_device *dev;
+
+	dev_file_info = comedi_get_device_file_info(minor);
+	if (dev_file_info == NULL)
+	        return -ENODEV;
+	dev = dev_file_info->device;
+	if (dev == NULL)
+	        return -ENODEV;
 
 	mutex_lock(&dev->mutex);
 	if (!dev->attached) {
@@ -1528,11 +1549,17 @@ static unsigned int comedi_poll(struct file *file, poll_table * wait)
 {
 	unsigned int mask = 0;
 	const unsigned minor = iminor(file->f_dentry->d_inode);
-	struct comedi_device_file_info *dev_file_info =
-	    comedi_get_device_file_info(minor);
-	struct comedi_device *dev = dev_file_info->device;
 	struct comedi_subdevice *read_subdev;
 	struct comedi_subdevice *write_subdev;
+	struct comedi_device_file_info *dev_file_info;
+	struct comedi_device *dev;
+	dev_file_info = comedi_get_device_file_info(minor);
+
+	if (dev_file_info == NULL)
+	        return -ENODEV;
+	dev = dev_file_info->device;
+	if (dev == NULL)
+	        return -ENODEV;
 
 	mutex_lock(&dev->mutex);
 	if (!dev->attached) {
@@ -1578,9 +1605,15 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 	int n, m, count = 0, retval = 0;
 	DECLARE_WAITQUEUE(wait, current);
 	const unsigned minor = iminor(file->f_dentry->d_inode);
-	struct comedi_device_file_info *dev_file_info =
-	    comedi_get_device_file_info(minor);
-	struct comedi_device *dev = dev_file_info->device;
+	struct comedi_device_file_info *dev_file_info;
+	struct comedi_device *dev;
+	dev_file_info = comedi_get_device_file_info(minor);
+
+	if (dev_file_info == NULL)
+	        return -ENODEV;
+	dev = dev_file_info->device;
+	if (dev == NULL)
+	        return -ENODEV;
 
 	if (!dev->attached) {
 		DPRINTK("no driver configured on comedi%i\n", dev->minor);
@@ -1640,11 +1673,11 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 				retval = -EAGAIN;
 				break;
 			}
+			schedule();
 			if (signal_pending(current)) {
 				retval = -ERESTARTSYS;
 				break;
 			}
-			schedule();
 			if (!s->busy)
 				break;
 			if (s->busy != file) {
@@ -1683,9 +1716,15 @@ static ssize_t comedi_read(struct file *file, char __user *buf, size_t nbytes,
 	int n, m, count = 0, retval = 0;
 	DECLARE_WAITQUEUE(wait, current);
 	const unsigned minor = iminor(file->f_dentry->d_inode);
-	struct comedi_device_file_info *dev_file_info =
-	    comedi_get_device_file_info(minor);
-	struct comedi_device *dev = dev_file_info->device;
+	struct comedi_device_file_info *dev_file_info;
+	struct comedi_device *dev;
+	dev_file_info = comedi_get_device_file_info(minor);
+
+	if (dev_file_info == NULL)
+	        return -ENODEV;
+	dev = dev_file_info->device;
+	if (dev == NULL)
+	        return -ENODEV;
 
 	if (!dev->attached) {
 		DPRINTK("no driver configured on comedi%i\n", dev->minor);
@@ -1741,11 +1780,11 @@ static ssize_t comedi_read(struct file *file, char __user *buf, size_t nbytes,
 				retval = -EAGAIN;
 				break;
 			}
+			schedule();
 			if (signal_pending(current)) {
 				retval = -ERESTARTSYS;
 				break;
 			}
-			schedule();
 			if (!s->busy) {
 				retval = 0;
 				break;
@@ -1885,11 +1924,17 @@ ok:
 static int comedi_close(struct inode *inode, struct file *file)
 {
 	const unsigned minor = iminor(inode);
-	struct comedi_device_file_info *dev_file_info =
-	    comedi_get_device_file_info(minor);
-	struct comedi_device *dev = dev_file_info->device;
 	struct comedi_subdevice *s = NULL;
 	int i;
+	struct comedi_device_file_info *dev_file_info;
+	struct comedi_device *dev;
+	dev_file_info = comedi_get_device_file_info(minor);
+
+	if (dev_file_info == NULL)
+	        return -ENODEV;
+	dev = dev_file_info->device;
+	if (dev == NULL)
+	        return -ENODEV;
 
 	mutex_lock(&dev->mutex);
 
@@ -1923,10 +1968,15 @@ static int comedi_close(struct inode *inode, struct file *file)
 static int comedi_fasync(int fd, struct file *file, int on)
 {
 	const unsigned minor = iminor(file->f_dentry->d_inode);
-	struct comedi_device_file_info *dev_file_info =
-	    comedi_get_device_file_info(minor);
+	struct comedi_device_file_info *dev_file_info;
+	struct comedi_device *dev;
+	dev_file_info = comedi_get_device_file_info(minor);
 
-	struct comedi_device *dev = dev_file_info->device;
+	if (dev_file_info == NULL)
+	        return -ENODEV;
+	dev = dev_file_info->device;
+	if (dev == NULL)
+	        return -ENODEV;
 
 	return fasync_helper(fd, file, on, &dev->async_queue);
 }
diff --git a/drivers/staging/hv/hyperv_storage.h b/drivers/staging/hv/hyperv_storage.h
index a01f9a07c988..5af82f4235bd 100644
--- a/drivers/staging/hv/hyperv_storage.h
+++ b/drivers/staging/hv/hyperv_storage.h
@@ -218,6 +218,7 @@ struct vstor_packet {
 #define STORVSC_MAX_LUNS_PER_TARGET			64
 #define STORVSC_MAX_TARGETS				1
 #define STORVSC_MAX_CHANNELS				1
+#define STORVSC_MAX_CMD_LEN				16
 
 struct hv_storvsc_request;
 
diff --git a/drivers/staging/hv/netvsc_drv.c b/drivers/staging/hv/netvsc_drv.c
index 61989f0d9f0d..88d519359e6b 100644
--- a/drivers/staging/hv/netvsc_drv.c
+++ b/drivers/staging/hv/netvsc_drv.c
@@ -217,8 +217,8 @@ void netvsc_linkstatus_callback(struct hv_device *device_obj,
 	if (status == 1) {
 		netif_carrier_on(net);
 		netif_wake_queue(net);
-		netif_notify_peers(net);
 		ndev_ctx = netdev_priv(net);
+		schedule_delayed_work(&ndev_ctx->dwork, 0);
 		schedule_delayed_work(&ndev_ctx->dwork, msecs_to_jiffies(20));
 	} else {
 		netif_carrier_off(net);
diff --git a/drivers/staging/hv/storvsc_drv.c b/drivers/staging/hv/storvsc_drv.c
index 7effaf32e25f..26983ac1ae42 100644
--- a/drivers/staging/hv/storvsc_drv.c
+++ b/drivers/staging/hv/storvsc_drv.c
@@ -701,6 +701,8 @@ static int storvsc_probe(struct hv_device *device)
 	host->max_id = STORVSC_MAX_TARGETS;
 	/* max # of channels */
 	host->max_channel = STORVSC_MAX_CHANNELS - 1;
+	/* max cmd length */
+	host->max_cmd_len = STORVSC_MAX_CMD_LEN;
 
 	/* Register the HBA and start the scsi bus scan */
 	ret = scsi_add_host(host, &device->device);
diff --git a/drivers/staging/iio/adc/Kconfig b/drivers/staging/iio/adc/Kconfig
index b39f2e1c1fe6..ce9761c79c51 100644
--- a/drivers/staging/iio/adc/Kconfig
+++ b/drivers/staging/iio/adc/Kconfig
@@ -205,3 +205,10 @@ config MAX1363_RING_BUFFER
 	help
 	  Say yes here to include ring buffer support in the MAX1363
 	  ADC driver.
+
+config STMPE_ADC
+	bool "ST Microelectronics STMPE ADC driver"
+	depends on MFD_STMPE
+	help
+	  Say yes here to build support for ST Microelectronics STMPE
+	  built in ADC block (stmpe811).
diff --git a/drivers/staging/iio/adc/Makefile b/drivers/staging/iio/adc/Makefile
index f02035139979..e54101c03121 100644
--- a/drivers/staging/iio/adc/Makefile
+++ b/drivers/staging/iio/adc/Makefile
@@ -40,3 +40,5 @@ obj-$(CONFIG_AD7816) += ad7816.o
 obj-$(CONFIG_ADT75) += adt75.o
 obj-$(CONFIG_ADT7310) += adt7310.o
 obj-$(CONFIG_ADT7410) += adt7410.o
+
+obj-$(CONFIG_STMPE_ADC) += stmpe-adc.o
diff --git a/drivers/staging/iio/adc/stmpe-adc.c b/drivers/staging/iio/adc/stmpe-adc.c
new file mode 100644
index 000000000000..3a9d6bee558d
--- /dev/null
+++ b/drivers/staging/iio/adc/stmpe-adc.c
@@ -0,0 +1,334 @@
+/*
+ *  stmpe.c - STMicroelectronics STMPE811 IIO ADC Driver
+ *
+ *  4 channel, 10/12-bit ADC
+ *
+ *  Copyright (C) 2013 Toradex AG <stefan.agner@toradex.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/regulator/consumer.h>
+#include <linux/of_platform.h>
+#include <linux/err.h>
+
+#include <linux/mfd/stmpe.h>
+
+#include "../iio.h"
+#include "adc.h"
+
+#define STMPE_REG_INT_STA	0x0B
+#define STMPE_REG_ADC_INT_EN	0x0E
+#define STMPE_REG_ADC_INT_STA	0x0F
+
+#define STMPE_REG_ADC_CTRL1	0x20
+#define STMPE_REG_ADC_CTRL2	0x21
+#define STMPE_REG_ADC_CAPT	0x22
+#define STMPE_REG_ADC_DATA_CH(channel)	(0x30 + 2*channel)
+
+#define	STMPE_ADC_CH(channel)	((1 << channel) & 0xff)
+
+#define STMPE_ADC_TIMEOUT	(msecs_to_jiffies(1000))
+
+struct stmpe_adc {
+	struct stmpe *stmpe;
+	struct clk		*clk;
+	struct device		*dev;
+	unsigned int		irq;
+
+	struct completion	completion;
+
+	u8			channel;
+	u32			value;
+	u8			sample_time;
+	u8			mod_12b;
+	u8			ref_sel;
+	u8			adc_freq;
+};
+
+static int stmpe_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int *val,
+				int *val2,
+				long mask)
+{
+	struct stmpe_adc *info = iio_priv(indio_dev);
+	unsigned long timeout;
+
+	if (mask > 0)
+		return -EINVAL;
+
+	mutex_lock(&indio_dev->mlock);
+
+	info->channel = (u8)chan->scan_index;
+	stmpe_reg_write(info->stmpe, STMPE_REG_ADC_INT_EN,
+			STMPE_ADC_CH(info->channel));
+
+	stmpe_reg_write(info->stmpe, STMPE_REG_ADC_CAPT,
+			STMPE_ADC_CH(info->channel));
+
+	timeout = wait_for_completion_interruptible_timeout
+		(&info->completion, STMPE_ADC_TIMEOUT);
+
+	*val = info->value;
+
+	mutex_unlock(&indio_dev->mlock);
+
+	if (timeout == -ERESTARTSYS)
+		return -EINTR;
+
+	if (timeout == 0)
+		return -ETIMEDOUT;
+
+	return IIO_VAL_INT;
+}
+
+static irqreturn_t stmpe_adc_isr(int irq, void *dev_id)
+{
+	struct stmpe_adc *info = (struct stmpe_adc *)dev_id;
+	u8 data[2];
+	int int_sta;
+
+	int_sta = stmpe_reg_read(info->stmpe, STMPE_REG_ADC_INT_STA);
+
+	/* Is the interrupt relevant */
+	if (!(int_sta & STMPE_ADC_CH(info->channel)))
+		return IRQ_NONE;
+
+	/* Read value */
+	stmpe_block_read(info->stmpe, STMPE_REG_ADC_DATA_CH(info->channel), 2,
+			data);
+	info->value = ((u32)data[0] << 8) + data[1];
+
+	stmpe_reg_write(info->stmpe, STMPE_REG_ADC_INT_STA, int_sta);
+
+	complete(&info->completion);
+
+	return IRQ_HANDLED;
+}
+
+static const struct iio_info stmpe_adc_iio_info = {
+	.num_interrupt_lines = 1,
+	.read_raw = &stmpe_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+#define STMPE_EV_M                                              \
+	(IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING)      \
+	| IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_FALLING))
+#define STMPE_INFO_MASK (1 << IIO_CHAN_INFO_SCALE_SHARED)
+
+static const struct iio_chan_spec stmpe_adc_all_iio_channels[] = {
+	IIO_CHAN(IIO_IN, 0, 1, 0, NULL, 0, 0, STMPE_INFO_MASK,
+			0, 0, IIO_ST('u', 12, 16, 0), STMPE_EV_M),
+	IIO_CHAN(IIO_IN, 0, 1, 0, NULL, 1, 0, STMPE_INFO_MASK,
+			0, 1, IIO_ST('u', 12, 16, 0), STMPE_EV_M),
+	IIO_CHAN(IIO_IN, 0, 1, 0, NULL, 2, 0, STMPE_INFO_MASK,
+			0, 2, IIO_ST('u', 12, 16, 0), STMPE_EV_M),
+	IIO_CHAN(IIO_IN, 0, 1, 0, NULL, 3, 0, STMPE_INFO_MASK,
+			0, 3, IIO_ST('u', 12, 16, 0), STMPE_EV_M),
+	IIO_CHAN(IIO_IN, 0, 1, 0, NULL, 4, 0, STMPE_INFO_MASK,
+			0, 4, IIO_ST('u', 12, 16, 0), STMPE_EV_M),
+	IIO_CHAN(IIO_IN, 0, 1, 0, NULL, 5, 0, STMPE_INFO_MASK,
+			0, 5, IIO_ST('u', 12, 16, 0), STMPE_EV_M),
+	IIO_CHAN(IIO_IN, 0, 1, 0, NULL, 6, 0, STMPE_INFO_MASK,
+			0, 6, IIO_ST('u', 12, 16, 0), STMPE_EV_M),
+	IIO_CHAN(IIO_IN, 0, 1, 0, NULL, 7, 0, STMPE_INFO_MASK,
+			0, 7, IIO_ST('u', 12, 16, 0), STMPE_EV_M),
+};
+
+static const struct iio_chan_spec stmpe_adc_iio_channels[] = {
+	IIO_CHAN(IIO_IN, 0, 1, 0, NULL, 4, 0, STMPE_INFO_MASK,
+			0, 4, IIO_ST('u', 12, 16, 0), STMPE_EV_M),
+	IIO_CHAN(IIO_IN, 0, 1, 0, NULL, 5, 0, STMPE_INFO_MASK,
+			0, 5, IIO_ST('u', 12, 16, 0), STMPE_EV_M),
+	IIO_CHAN(IIO_IN, 0, 1, 0, NULL, 6, 0, STMPE_INFO_MASK,
+			0, 6, IIO_ST('u', 12, 16, 0), STMPE_EV_M),
+	IIO_CHAN(IIO_IN, 0, 1, 0, NULL, 7, 0, STMPE_INFO_MASK,
+			0, 7, IIO_ST('u', 12, 16, 0), STMPE_EV_M),
+};
+
+static int stmpe_adc_remove_devices(struct device *dev, void *c)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	platform_device_unregister(pdev);
+
+	return 0;
+}
+
+static int __devinit stmpe_adc_init_hw(struct stmpe_adc *adc)
+{
+	int ret;
+	u8 adc_ctrl1, adc_ctrl1_mask;
+	struct stmpe *stmpe = adc->stmpe;
+	struct device *dev = adc->dev;
+
+	ret = stmpe_enable(stmpe, STMPE_BLOCK_ADC);
+	if (ret) {
+		dev_err(dev, "Could not enable clock for ADC\n");
+		return ret;
+	}
+
+	adc_ctrl1 = SAMPLE_TIME(adc->sample_time) | MOD_12B(adc->mod_12b) |
+		REF_SEL(adc->ref_sel);
+	adc_ctrl1_mask = SAMPLE_TIME(0xff) | MOD_12B(0xff) | REF_SEL(0xff);
+
+	ret = stmpe_set_bits(stmpe, STMPE_REG_ADC_CTRL1,
+			adc_ctrl1_mask, adc_ctrl1);
+	if (ret) {
+		dev_err(dev, "Could not setup ADC\n");
+		return ret;
+	}
+
+	ret = stmpe_set_bits(stmpe, STMPE_REG_ADC_CTRL2,
+			ADC_FREQ(0xff), ADC_FREQ(adc->adc_freq));
+	if (ret) {
+		dev_err(dev, "Could not setup ADC\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+
+static int __devinit stmpe_adc_probe(struct platform_device *pdev)
+{
+	struct stmpe *stmpe = dev_get_drvdata(pdev->dev.parent);
+	struct stmpe_platform_data *pdata = stmpe->pdata;
+	struct stmpe_adc_platform_data *adc_pdata = NULL;
+	struct stmpe_adc *info = NULL;
+	struct iio_dev *indio_dev = NULL;
+	int ret = -ENODEV;
+	int irq;
+
+	irq = platform_get_irq_byname(pdev, "STMPE_ADC");
+	if (irq < 0)
+		return irq;
+
+	indio_dev = iio_allocate_device(sizeof(struct stmpe_adc));
+	if (!indio_dev) {
+		dev_err(&pdev->dev, "failed allocating iio device\n");
+		return -ENOMEM;
+	}
+
+	info = iio_priv(indio_dev);
+	info->irq = irq;
+	info->stmpe = stmpe;
+
+	init_completion(&info->completion);
+	ret = request_threaded_irq(info->irq, NULL, stmpe_adc_isr, IRQF_ONESHOT,
+					"stmpe-adc", info);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed requesting irq, irq = %d\n",
+							info->irq);
+		goto err_free;
+	}
+	platform_set_drvdata(pdev, indio_dev);
+
+	indio_dev->name = dev_name(&pdev->dev);
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->info = &stmpe_adc_iio_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	/* Register TS-Channels only if they are available */
+	if (stmpe->pdata->blocks & STMPE_BLOCK_TOUCHSCREEN)
+		indio_dev->channels = stmpe_adc_iio_channels;
+	else
+		indio_dev->channels = stmpe_adc_all_iio_channels;
+	indio_dev->num_channels = ARRAY_SIZE(stmpe_adc_iio_channels);
+
+	if (pdata)
+		adc_pdata = pdata->adc;
+
+	if (adc_pdata) {
+		info->sample_time = adc_pdata->sample_time;
+		info->mod_12b = adc_pdata->mod_12b;
+		info->ref_sel = adc_pdata->ref_sel;
+		info->adc_freq = adc_pdata->adc_freq;
+	}
+
+	ret = stmpe_adc_init_hw(info);
+	if (ret)
+		goto err_irq;
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto err_irq;
+
+
+	dev_info(&pdev->dev, "Initialized\n");
+
+	return 0;
+
+err_irq:
+	free_irq(info->irq, info);
+err_free:
+	kfree(indio_dev);
+	return ret;
+}
+
+static int stmpe_adc_remove(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct stmpe_adc *info = iio_priv(indio_dev);
+
+	device_for_each_child(&pdev->dev, NULL,
+				stmpe_adc_remove_devices);
+	iio_device_unregister(indio_dev);
+	free_irq(info->irq, info);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int stmpe_adc_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct stmpe_adc *info = iio_priv(indio_dev);
+
+	stmpe_adc_init_hw(info);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(stmpe_adc_pm_ops, NULL, stmpe_adc_resume);
+
+static struct platform_driver stmpe_adc_driver = {
+	.probe		= stmpe_adc_probe,
+	.remove		= stmpe_adc_remove,
+	.driver		= {
+		.name	= "stmpe-adc",
+		.owner	= THIS_MODULE,
+		.pm	= &stmpe_adc_pm_ops,
+	},
+};
+
+module_platform_driver(stmpe_adc_driver);
+
+MODULE_AUTHOR("Stefan Agner <stefan.agner@toradex.com>");
+MODULE_DESCRIPTION("STMPEXXX ADC driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:stmpe-adc");
diff --git a/drivers/staging/iio/industrialio-core.c b/drivers/staging/iio/industrialio-core.c
index 19819e7578c6..fd812c343e81 100644
--- a/drivers/staging/iio/industrialio-core.c
+++ b/drivers/staging/iio/industrialio-core.c
@@ -860,7 +860,10 @@ static ssize_t iio_ev_state_show(struct device *dev,
 {
 	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	int val = indio_dev->info->read_event_config(indio_dev,
+	int val = -ENODEV;
+
+	if (indio_dev->info != NULL && indio_dev->info->read_event_config != NULL)
+		val = indio_dev->info->read_event_config(indio_dev,
 						     this_attr->address);
 
 	if (val < 0)
@@ -875,9 +878,10 @@ static ssize_t iio_ev_value_show(struct device *dev,
 {
 	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
-	int val, ret;
+	int val, ret = -ENODEV;
 
-	ret = indio_dev->info->read_event_value(indio_dev,
+	if (indio_dev->info != NULL && indio_dev->info->read_event_config != NULL)
+		ret = indio_dev->info->read_event_value(indio_dev,
 						this_attr->address, &val);
 	if (ret < 0)
 		return ret;
diff --git a/drivers/staging/iio/light/Kconfig b/drivers/staging/iio/light/Kconfig
index 1ad2d56c8ba8..5e5d9c50f219 100644
--- a/drivers/staging/iio/light/Kconfig
+++ b/drivers/staging/iio/light/Kconfig
@@ -4,15 +4,26 @@
 comment "Light sensors"
 
 config SENSORS_ISL29018
-        tristate "ISL 29018 light and proximity sensor"
-        depends on I2C
-        default n
-        help
-         If you say yes here you get support for ambient light sensing and
-         proximity infrared sensing from Intersil ISL29018.
-         This driver will provide the measurements of ambient light intensity
-         in lux, proximity infrared sensing and normal infrared sensing.
-         Data from sensor is accessible via sysfs.
+	tristate "ISL 29018 light and proximity sensor"
+	depends on I2C
+	default n
+	help
+	  If you say yes here you get support for ambient light sensing and
+	  proximity infrared sensing from Intersil ISL29018.
+	  This driver will provide the measurements of ambient light intensity
+	  in lux, proximity infrared sensing and normal infrared sensing.
+	  Data from sensor is accessible via sysfs.
+
+config SENSORS_ISL29028
+	tristate "ISL 29028 light and proximity sensor"
+	depends on I2C
+	default n
+	help
+	  If you say yes here you get support for ambient light sensing and
+	  proximity ir sensing from intersil ISL29028.
+	  This driver will provide the measurements of ambient light intensity
+	  in lux, proximity infrared sensing and normal infrared sensing.
+	  Data from sensor is accessible via sysfs.
 
 config SENSORS_TSL2563
 	tristate "TAOS TSL2560, TSL2561, TSL2562 and TSL2563 ambient light sensors"
@@ -30,3 +41,11 @@ config TSL2583
 	help
 	 Provides support for the TAOS tsl2580, tsl2581 and tsl2583 devices.
 	 Access ALS data via iio, sysfs.
+
+config SENSORS_LTR558
+	tristate "LTR558 Ambient light and proximity sensor"
+	depends on I2C
+	default n
+	help
+	  If you say yes here you get support for ambient light sensing and
+	  proximity ir sensing from Lite On Technology LTR558.
diff --git a/drivers/staging/iio/light/Makefile b/drivers/staging/iio/light/Makefile
index 3011fbfa8dc2..0590ac51fd2a 100644
--- a/drivers/staging/iio/light/Makefile
+++ b/drivers/staging/iio/light/Makefile
@@ -1,7 +1,10 @@
 #
 # Makefile for industrial I/O Light sensors
 #
+GCOV_PROFILE := y
 
 obj-$(CONFIG_SENSORS_TSL2563)	+= tsl2563.o
 obj-$(CONFIG_SENSORS_ISL29018)	+= isl29018.o
 obj-$(CONFIG_TSL2583)	+= tsl2583.o
+obj-$(CONFIG_SENSORS_ISL29028)	+= isl29028.o
+obj-$(CONFIG_SENSORS_LTR558)	+= ltr558als.o
diff --git a/drivers/staging/iio/light/isl29028.c b/drivers/staging/iio/light/isl29028.c
new file mode 100644
index 000000000000..5874c32958d9
--- /dev/null
+++ b/drivers/staging/iio/light/isl29028.c
@@ -0,0 +1,1269 @@
+/*
+ * A iio driver for the light sensor ISL 29028.
+ *
+ * IIO Light driver for monitoring ambient light intensity in lux and proximity
+ * ir.
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA	02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/completion.h>
+#include <linux/interrupt.h>
+#include "../iio.h"
+
+#define CONVERSION_TIME_MS		100
+
+#define ISL29028_REG_ADD_CONFIGURE	0x01
+
+#define CONFIGURE_PROX_EN_MASK		(1 << 7)
+#define CONFIGURE_PROX_EN_SH		7
+
+#define CONFIGURE_PROX_SLP_SH		4
+#define CONFIGURE_PROX_SLP_MASK		(7 << CONFIGURE_PROX_SLP_SH)
+
+#define CONFIGURE_PROX_DRIVE		(1 << 3)
+
+#define CONFIGURE_ALS_EN		1
+#define CONFIGURE_ALS_DIS		0
+#define CONFIGURE_ALS_EN_SH		2
+#define CONFIGURE_ALS_EN_MASK		(1 << CONFIGURE_ALS_EN_SH)
+
+
+#define CONFIGURE_ALS_RANGE_LOW_LUX	0
+#define CONFIGURE_ALS_RANGE_HIGH_LUX	1
+#define CONFIGURE_ALS_RANGE_SH		1
+#define CONFIGURE_ALS_RANGE_MASK	(1 << CONFIGURE_ALS_RANGE_SH)
+
+#define CONFIGURE_ALS_IR_MODE_MASK	1
+#define CONFIGURE_ALS_IR_MODE_SH	0
+#define CONFIGURE_ALS_IR_MODE_IR	1
+#define CONFIGURE_ALS_IR_MODE_ALS	0
+
+#define ISL29028_REG_ADD_INTERRUPT	0x02
+#define INTERRUPT_PROX_FLAG_MASK	(1 << 7)
+#define INTERRUPT_PROX_FLAG_SH		7
+#define INTERRUPT_PROX_FLAG_EN		1
+#define INTERRUPT_PROX_FLAG_DIS		0
+
+#define INTERRUPT_PROX_PERSIST_SH	5
+#define INTERRUPT_PROX_PERSIST_MASK	(3 << 5)
+
+#define INTERRUPT_ALS_FLAG_MASK		(1 << 3)
+#define INTERRUPT_ALS_FLAG_SH		3
+#define INTERRUPT_ALS_FLAG_EN		1
+#define INTERRUPT_ALS_FLAG_DIS		0
+
+#define INTERRUPT_ALS_PERSIST_SH	1
+#define INTERRUPT_ALS_PERSIST_MASK	(3 << 1)
+
+#define ISL29028_REG_ADD_PROX_LOW_THRES		0x03
+#define ISL29028_REG_ADD_PROX_HIGH_THRES	0x04
+
+#define ISL29028_REG_ADD_ALSIR_LOW_THRES	0x05
+#define ISL29028_REG_ADD_ALSIR_LH_THRES		0x06
+#define ISL29028_REG_ADD_ALSIR_LH_THRES_L_SH	0
+#define ISL29028_REG_ADD_ALSIR_LH_THRES_H_SH	4
+#define ISL29028_REG_ADD_ALSIR_HIGH_THRES	0x07
+
+#define ISL29028_REG_ADD_PROX_DATA		0x08
+#define ISL29028_REG_ADD_ALSIR_L		0x09
+#define ISL29028_REG_ADD_ALSIR_U		0x0A
+
+#define ISL29028_REG_ADD_TEST1_MODE		0x0E
+#define ISL29028_REG_ADD_TEST2_MODE		0x0F
+
+#define ISL29028_MAX_REGS		ISL29028_REG_ADD_TEST2_MODE
+
+enum {
+	MODE_NONE = 0,
+	MODE_ALS,
+	MODE_IR
+};
+
+struct isl29028_chip {
+	struct iio_dev		*indio_dev;
+	struct i2c_client	*client;
+	struct mutex		lock;
+	int			irq;
+
+	int			prox_period;
+	int			prox_low_thres;
+	int			prox_high_thres;
+	int			prox_persist;
+	bool			is_prox_enable;
+	int			prox_reading;
+
+	int			als_high_thres;
+	int			als_low_thres;
+	int			als_persist;
+	int			als_range;
+	int			als_reading;
+	int			als_ir_mode;
+
+	int			ir_high_thres;
+	int			ir_low_thres;
+	int			ir_reading;
+
+	bool			is_int_enable;
+	bool			is_proxim_int_waiting;
+	bool			is_als_int_waiting;
+	struct completion	prox_completion;
+	struct completion	als_completion;
+	u8			reg_cache[ISL29028_MAX_REGS];
+};
+
+static bool isl29028_write_data(struct i2c_client *client, u8 reg,
+	u8 val, u8 mask, u8 shift)
+{
+	u8 regval;
+	int ret = 0;
+	struct isl29028_chip *chip = i2c_get_clientdata(client);
+
+	regval = chip->reg_cache[reg];
+	regval &= ~mask;
+	regval |= val << shift;
+
+	ret = i2c_smbus_write_byte_data(client, reg, regval);
+	if (ret) {
+		dev_err(&client->dev, "Write to device reg %d fails status "
+				"%x\n", reg, ret);
+		return false;
+	}
+	chip->reg_cache[reg] = regval;
+	return true;
+}
+
+static bool isl29018_set_proxim_period(struct i2c_client *client,
+		bool is_enable,	int period)
+{
+	int prox_period[] = {0, 12, 50, 75, 100, 200, 400, 800};
+	int i;
+	int sel;
+	bool st;
+	if (period < 12)
+		sel = 7;
+	else {
+		for (i = 1; i < ARRAY_SIZE(prox_period) - 1; ++i) {
+			if ((prox_period[i] <= period) &&
+						period < prox_period[i + 1])
+				break;
+		}
+		sel = 7 - i;
+	}
+
+	if (!is_enable) {
+		dev_dbg(&client->dev, "Disabling proximity sensing\n");
+		st = isl29028_write_data(client, ISL29028_REG_ADD_CONFIGURE,
+			0, CONFIGURE_PROX_EN_MASK, CONFIGURE_PROX_EN_SH);
+	} else {
+		dev_dbg(&client->dev, "Enabling proximity sensing with period "
+			"of %d ms sel %d period %d\n", prox_period[7 - sel],
+			sel, period);
+		st = isl29028_write_data(client, ISL29028_REG_ADD_CONFIGURE,
+			sel, CONFIGURE_PROX_SLP_MASK, CONFIGURE_PROX_SLP_SH);
+		if (st)
+			st = isl29028_write_data(client,
+				ISL29028_REG_ADD_CONFIGURE, 1,
+				CONFIGURE_PROX_EN_MASK, CONFIGURE_PROX_EN_SH);
+	}
+	return st;
+}
+
+static bool isl29018_set_proxim_persist(struct i2c_client *client,
+		bool is_enable,	int persist)
+{
+	int prox_perstant[] = {1, 4, 8, 16};
+	int i;
+	int sel;
+	bool st;
+	if (is_enable) {
+		for (i = 0; i < ARRAY_SIZE(prox_perstant) - 1; ++i) {
+			if ((prox_perstant[i] <= persist) &&
+						persist < prox_perstant[i+1])
+				break;
+		}
+		sel = i;
+	}
+
+	if (is_enable) {
+		dev_dbg(&client->dev, "Enabling proximity threshold interrupt\n");
+		st = isl29028_write_data(client, ISL29028_REG_ADD_INTERRUPT,
+			sel, INTERRUPT_PROX_PERSIST_MASK,
+			INTERRUPT_PROX_PERSIST_SH);
+		if (st)
+			st = isl29028_write_data(client,
+				ISL29028_REG_ADD_INTERRUPT,
+				INTERRUPT_PROX_FLAG_EN,
+				INTERRUPT_PROX_FLAG_MASK,
+				INTERRUPT_PROX_FLAG_SH);
+	} else {
+		st = isl29028_write_data(client,
+			ISL29028_REG_ADD_INTERRUPT, INTERRUPT_PROX_FLAG_DIS,
+			INTERRUPT_PROX_FLAG_MASK, INTERRUPT_PROX_FLAG_SH);
+	}
+	return st;
+}
+
+static bool isl29018_set_als_persist(struct i2c_client *client, bool is_enable,
+			int persist)
+{
+	int prox_perstant[] = {1, 4, 8, 16};
+	int i;
+	int sel;
+	bool st;
+	if (is_enable) {
+		for (i = 0; i < ARRAY_SIZE(prox_perstant) - 1; ++i) {
+			if ((prox_perstant[i] <= persist) &&
+						persist < prox_perstant[i+1])
+				break;
+		}
+		sel = i;
+	}
+
+	if (is_enable) {
+		dev_dbg(&client->dev, "Enabling als threshold interrupt\n");
+		st = isl29028_write_data(client, ISL29028_REG_ADD_INTERRUPT,
+			sel, INTERRUPT_ALS_PERSIST_MASK,
+			INTERRUPT_ALS_PERSIST_SH);
+		if (st)
+			st = isl29028_write_data(client,
+				ISL29028_REG_ADD_INTERRUPT,
+				INTERRUPT_ALS_FLAG_EN,
+				INTERRUPT_ALS_FLAG_MASK,
+				INTERRUPT_ALS_FLAG_SH);
+	} else {
+		st = isl29028_write_data(client,
+			ISL29028_REG_ADD_INTERRUPT, INTERRUPT_ALS_FLAG_DIS,
+			INTERRUPT_ALS_FLAG_MASK, INTERRUPT_ALS_FLAG_SH);
+	}
+	return st;
+}
+
+static bool isl29018_set_proxim_high_threshold(struct i2c_client *client, u8 th)
+{
+	return isl29028_write_data(client, ISL29028_REG_ADD_PROX_HIGH_THRES,
+		th, 0xFF, 0);
+}
+
+static bool isl29018_set_proxim_low_threshold(struct i2c_client *client, u8 th)
+{
+	return isl29028_write_data(client, ISL29028_REG_ADD_PROX_LOW_THRES,
+		th, 0xFF, 0);
+}
+
+static bool isl29018_set_irals_high_threshold(struct i2c_client *client,
+				u32 als)
+{
+	bool st;
+	st = isl29028_write_data(client, ISL29028_REG_ADD_ALSIR_HIGH_THRES,
+		(als >> 4) & 0xFF, 0xFF, 0);
+	if (st)
+		st = isl29028_write_data(client,
+			ISL29028_REG_ADD_ALSIR_LH_THRES, als & 0xF,
+			0xF << ISL29028_REG_ADD_ALSIR_LH_THRES_H_SH,
+			ISL29028_REG_ADD_ALSIR_LH_THRES_H_SH);
+	return st;
+}
+
+static bool isl29018_set_irals_low_threshold(struct i2c_client *client, u32 als)
+{
+	bool st;
+	st = isl29028_write_data(client,
+		ISL29028_REG_ADD_ALSIR_LH_THRES, (als >> 8) & 0xF,
+		0xF << ISL29028_REG_ADD_ALSIR_LH_THRES_L_SH,
+		ISL29028_REG_ADD_ALSIR_LH_THRES_L_SH);
+	if (st)
+		st = isl29028_write_data(client,
+			ISL29028_REG_ADD_ALSIR_LOW_THRES,
+			als & 0xFF, 0xFF, 0);
+	return st;
+}
+
+static bool isl29018_set_als_ir_mode(struct i2c_client *client, bool is_enable,
+	bool is_als)
+{
+	struct isl29028_chip *chip = i2c_get_clientdata(client);
+	bool st;
+	if (is_enable) {
+		if (is_als) {
+			dev_dbg(&client->dev, "Enabling ALS mode\n");
+			st = isl29028_write_data(client,
+				ISL29028_REG_ADD_CONFIGURE,
+				CONFIGURE_ALS_IR_MODE_ALS,
+				CONFIGURE_ALS_IR_MODE_MASK,
+				CONFIGURE_ALS_IR_MODE_SH);
+			if (st)
+				st = isl29028_write_data(client,
+					ISL29028_REG_ADD_CONFIGURE,
+					CONFIGURE_ALS_RANGE_HIGH_LUX,
+					CONFIGURE_ALS_RANGE_MASK,
+					CONFIGURE_ALS_RANGE_SH);
+			if (st)
+				st = isl29018_set_irals_high_threshold(client,
+					chip->als_high_thres);
+			if (st)
+				st = isl29018_set_irals_low_threshold(client,
+					chip->als_low_thres);
+		} else {
+			dev_dbg(&client->dev, "Enabling IR mode\n");
+			st = isl29028_write_data(client,
+				ISL29028_REG_ADD_CONFIGURE,
+				CONFIGURE_ALS_IR_MODE_IR,
+				CONFIGURE_ALS_IR_MODE_MASK,
+				CONFIGURE_ALS_IR_MODE_SH);
+			if (st)
+				st = isl29018_set_irals_high_threshold(client,
+					chip->ir_high_thres);
+			if (st)
+				st = isl29018_set_irals_low_threshold(client,
+					chip->ir_low_thres);
+		}
+		if (st)
+			st = isl29028_write_data(client,
+				ISL29028_REG_ADD_CONFIGURE,
+				CONFIGURE_ALS_EN,
+				CONFIGURE_ALS_EN_MASK,
+				CONFIGURE_ALS_EN_SH);
+	} else {
+		st = isl29028_write_data(client,
+			ISL29028_REG_ADD_CONFIGURE,
+			CONFIGURE_ALS_DIS,
+			CONFIGURE_ALS_EN_MASK,
+			CONFIGURE_ALS_EN_SH);
+	}
+	return st;
+}
+
+static bool isl29028_read_als_ir(struct i2c_client *client, int *als_ir)
+{
+	s32 lsb;
+	s32 msb;
+
+	lsb = i2c_smbus_read_byte_data(client, ISL29028_REG_ADD_ALSIR_L);
+	if (lsb < 0) {
+		dev_err(&client->dev, "Error in reading register %d, error %d\n",
+				ISL29028_REG_ADD_ALSIR_L, lsb);
+		return false;
+	}
+
+	msb = i2c_smbus_read_byte_data(client, ISL29028_REG_ADD_ALSIR_U);
+	if (msb < 0) {
+		dev_err(&client->dev, "Error in reading register %d, error %d\n",
+				ISL29028_REG_ADD_ALSIR_U, lsb);
+		return false;
+	}
+	*als_ir = ((msb & 0xF) << 8) | (lsb & 0xFF);
+	return true;
+}
+
+static bool isl29028_read_proxim(struct i2c_client *client, int *prox)
+{
+	s32 data;
+
+	data = i2c_smbus_read_byte_data(client, ISL29028_REG_ADD_PROX_DATA);
+	if (data < 0) {
+		dev_err(&client->dev, "Error in reading register %d, error %d\n",
+				ISL29028_REG_ADD_PROX_DATA, data);
+		return false;
+	}
+	*prox = (int)data;
+	return true;
+}
+
+/* Sysfs interface */
+/* proximity period  */
+static ssize_t show_prox_period(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "%d\n", chip->prox_period);
+}
+
+static ssize_t store_prox_period(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	bool st;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+
+	mutex_lock(&chip->lock);
+	st = isl29018_set_proxim_period(client, chip->is_prox_enable,
+				(int)lval);
+	if (st)
+		chip->prox_period = (int)lval;
+	else
+		dev_err(dev, "Error in setting the proximity period\n");
+
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* proximity enable/disable  */
+static ssize_t show_prox_enable(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	if (chip->is_prox_enable)
+		return sprintf(buf, "1\n");
+	else
+		return sprintf(buf, "0\n");
+}
+
+static ssize_t store_prox_enable(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	bool st;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+	if ((lval != 1) && (lval != 0)) {
+		dev_err(dev, "illegal value %lu\n", lval);
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	if (lval == 1)
+		st = isl29018_set_proxim_period(client, true,
+							chip->prox_period);
+	else
+		st = isl29018_set_proxim_period(client, false,
+							chip->prox_period);
+	if (st)
+		chip->is_prox_enable = (lval) ? true : false;
+	else
+		dev_err(dev, "Error in enabling proximity\n");
+
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* als/ir enable/disable  */
+static ssize_t show_als_ir_mode(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "Current Mode: %d [0:None, 1:ALS, 2:IR]\n",
+			chip->als_ir_mode);
+}
+
+static ssize_t store_als_ir_mode(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	bool st;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+	if (lval > 2) {
+		dev_err(dev, "illegal value %lu\n", lval);
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	if (lval == 0)
+		st = isl29018_set_als_ir_mode(client, false, false);
+	else if (lval == 1)
+		st = isl29018_set_als_ir_mode(client, true, true);
+	else
+		st = isl29018_set_als_ir_mode(client, true, false);
+	if (st)
+		chip->als_ir_mode = (int)lval;
+	else
+		dev_err(dev, "Error in enabling als/ir mode\n");
+
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* Proximity low thresholds  */
+static ssize_t show_proxim_low_threshold(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "%d\n", chip->prox_low_thres);
+}
+
+static ssize_t store_proxim_low_threshold(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	bool st;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+
+	if ((lval > 0xFF) || (lval < 0x0)) {
+		dev_err(dev, "The threshold is not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	st = isl29018_set_proxim_low_threshold(client, (u8)lval);
+	if (st)
+		chip->prox_low_thres = (int)lval;
+	else
+		dev_err(dev, "Error in setting proximity low threshold\n");
+
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* Proximity high thresholds  */
+static ssize_t show_proxim_high_threshold(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "%d\n", chip->prox_high_thres);
+}
+
+static ssize_t store_proxim_high_threshold(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	bool st;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+
+	if ((lval > 0xFF) || (lval < 0x0)) {
+		dev_err(dev, "The threshold is not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	st = isl29018_set_proxim_high_threshold(client, (u8)lval);
+	if (st)
+		chip->prox_high_thres = (int)lval;
+	else
+		dev_err(dev, "Error in setting proximity high threshold\n");
+
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* als low thresholds  */
+static ssize_t show_als_low_threshold(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "%d\n", chip->als_low_thres);
+}
+
+static ssize_t store_als_low_threshold(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	bool st;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+
+	if ((lval > 0xFFFF) || (lval < 0x0)) {
+		dev_err(dev, "The ALS threshold is not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	if (chip->als_ir_mode == MODE_ALS) {
+		st = isl29018_set_irals_low_threshold(client, (int)lval);
+		if (st)
+			chip->als_low_thres = (int)lval;
+		else
+			dev_err(dev, "Error in setting als low threshold\n");
+	} else
+		chip->als_low_thres = (int)lval;
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* Als high thresholds  */
+static ssize_t show_als_high_threshold(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "%d\n", chip->als_high_thres);
+}
+
+static ssize_t store_als_high_threshold(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	bool st;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+
+	if ((lval > 0xFFFF) || (lval < 0x0)) {
+		dev_err(dev, "The als threshold is not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	if (chip->als_ir_mode == MODE_ALS) {
+		st = isl29018_set_irals_high_threshold(client, (int)lval);
+		if (st)
+			chip->als_high_thres = (int)lval;
+		else
+			dev_err(dev, "Error in setting als high threshold\n");
+	} else
+		chip->als_high_thres = (int)lval;
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* IR low thresholds  */
+static ssize_t show_ir_low_threshold(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "%d\n", chip->ir_low_thres);
+}
+
+static ssize_t store_ir_low_threshold(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	bool st;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+
+	if ((lval > 0xFFFF) || (lval < 0x0)) {
+		dev_err(dev, "The IR threshold is not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	if (chip->als_ir_mode == MODE_IR) {
+		st = isl29018_set_irals_low_threshold(client, (int)lval);
+		if (st)
+			chip->ir_low_thres = (int)lval;
+		else
+			dev_err(dev, "Error in setting als low threshold\n");
+	} else
+		chip->ir_low_thres = (int)lval;
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* IR high thresholds  */
+static ssize_t show_ir_high_threshold(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "%d\n", chip->ir_high_thres);
+}
+
+static ssize_t store_ir_high_threshold(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	bool st;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+
+	if ((lval > 0xFFFF) || (lval < 0x0)) {
+		dev_err(dev, "The als threshold is not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	if (chip->als_ir_mode == MODE_IR) {
+		st = isl29018_set_irals_high_threshold(client, (int)lval);
+		if (st)
+			chip->ir_high_thres = (int)lval;
+		else
+			dev_err(dev, "Error in setting als high threshold\n");
+	} else
+		chip->ir_high_thres = (int)lval;
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* Proximity persist  */
+static ssize_t show_proxim_persist(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "%d\n", chip->prox_persist);
+}
+
+static ssize_t store_proxim_persist(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+
+	if ((lval > 16) || (lval < 0x0)) {
+		dev_err(dev, "The proximity persist is not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	chip->prox_persist = (int)lval;
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* als/ir persist  */
+static ssize_t show_als_persist(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "%d\n", chip->als_persist);
+}
+
+static ssize_t store_als_persist(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+
+	if ((lval > 16) || (lval < 0x0)) {
+		dev_err(dev, "The als persist is not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	chip->als_persist = (int)lval;
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* Display proxim data  */
+static ssize_t show_proxim_data(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	int prox_data;
+	bool st;
+	ssize_t buf_count = 0;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	mutex_lock(&chip->lock);
+
+	if (chip->is_prox_enable) {
+		st = isl29028_read_proxim(chip->client, &prox_data);
+		if (st) {
+			buf_count = sprintf(buf, "%d\n", prox_data);
+			chip->prox_reading = prox_data;
+		}
+	} else
+		buf_count = sprintf(buf, "%d\n", chip->prox_reading);
+
+	mutex_unlock(&chip->lock);
+	return buf_count;
+}
+
+/* Display als data  */
+static ssize_t show_als_data(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	int als_ir_data;
+	bool st;
+	ssize_t buf_count = 0;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	mutex_lock(&chip->lock);
+
+	if (chip->als_ir_mode == MODE_ALS) {
+		st = isl29028_read_als_ir(chip->client, &als_ir_data);
+		if (st) {
+			/* convert als data count to lux */
+			/* if als_range = 0, lux = count * 0.0326 */
+			/* if als_range = 1, lux = count * 0.522 */
+			if (!chip->als_range)
+				als_ir_data = (als_ir_data * 326) / 10000;
+			else
+				als_ir_data = (als_ir_data * 522) / 1000;
+
+			buf_count = sprintf(buf, "%d\n", als_ir_data);
+			chip->als_reading = als_ir_data;
+		}
+	} else
+		buf_count = sprintf(buf, "%d\n", chip->als_reading);
+	mutex_unlock(&chip->lock);
+	return buf_count;
+}
+
+/* Display IR data  */
+static ssize_t show_ir_data(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	int als_ir_data;
+	bool st;
+	ssize_t buf_count = 0;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	mutex_lock(&chip->lock);
+
+	if (chip->als_ir_mode == MODE_IR) {
+		st = isl29028_read_als_ir(chip->client, &als_ir_data);
+		if (st) {
+			buf_count = sprintf(buf, "%d\n", als_ir_data);
+			chip->ir_reading = als_ir_data;
+		}
+	} else
+		buf_count = sprintf(buf, "%d\n", chip->ir_reading);
+	mutex_unlock(&chip->lock);
+	return buf_count;
+}
+
+/* Wait for the proximity threshold interrupt*/
+static ssize_t show_wait_proxim_int(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	bool st;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	if (!chip->is_int_enable) {
+		dev_err(dev, "%s() Interrupt mode not supported\n", __func__);
+		return sprintf(buf, "error\n");
+	}
+
+	mutex_lock(&chip->lock);
+	st = isl29018_set_proxim_persist(client, true, chip->prox_persist);
+	if (!st) {
+		dev_err(dev, "%s() Error in configuration\n", __func__);
+		mutex_unlock(&chip->lock);
+		return sprintf(buf, "error\n");
+	}
+
+	chip->is_proxim_int_waiting =  true;
+	mutex_unlock(&chip->lock);
+	wait_for_completion(&chip->prox_completion);
+	mutex_lock(&chip->lock);
+	chip->is_proxim_int_waiting =  false;
+	isl29018_set_proxim_persist(client, false, chip->prox_persist);
+	mutex_unlock(&chip->lock);
+	return sprintf(buf, "done\n");
+}
+
+/* Wait for the als/ir interrupt*/
+static ssize_t show_wait_als_ir_int(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	bool st;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	if (!chip->is_int_enable) {
+		dev_err(dev, "%s() Interrupt mode not supported\n", __func__);
+		return sprintf(buf, "error\n");
+	}
+
+	mutex_lock(&chip->lock);
+
+	st = isl29018_set_als_persist(client, true, chip->als_persist);
+	if (!st) {
+		dev_err(dev, "%s() Error in als ir int configuration\n",
+					 __func__);
+		mutex_unlock(&chip->lock);
+		return sprintf(buf, "error\n");
+	}
+
+	chip->is_als_int_waiting =  true;
+	mutex_unlock(&chip->lock);
+	wait_for_completion(&chip->als_completion);
+	mutex_lock(&chip->lock);
+	chip->is_als_int_waiting =  false;
+	st = isl29018_set_als_persist(client, false, chip->als_persist);
+	mutex_unlock(&chip->lock);
+	return sprintf(buf, "done\n");
+}
+
+/* Read name */
+static ssize_t show_name(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct isl29028_chip *chip = indio_dev->dev_data;
+	return sprintf(buf, "%s\n", chip->client->name);
+}
+
+static IIO_DEVICE_ATTR(proximity_low_threshold, S_IRUGO | S_IWUSR,
+		show_proxim_low_threshold, store_proxim_low_threshold, 0);
+static IIO_DEVICE_ATTR(proximity_high_threshold, S_IRUGO | S_IWUSR,
+		show_proxim_high_threshold, store_proxim_high_threshold, 0);
+static IIO_DEVICE_ATTR(proximity_persist, S_IRUGO | S_IWUSR,
+		show_proxim_persist, store_proxim_persist, 0);
+static IIO_DEVICE_ATTR(proximity_period, S_IRUGO | S_IWUSR,
+		show_prox_period, store_prox_period, 0);
+static IIO_DEVICE_ATTR(proximity_enable, S_IRUGO | S_IWUSR,
+		show_prox_enable, store_prox_enable, 0);
+static IIO_DEVICE_ATTR(wait_proxim_thres, S_IRUGO,
+		show_wait_proxim_int, NULL, 0);
+static IIO_DEVICE_ATTR(proximity_value, S_IRUGO,
+		show_proxim_data, NULL, 0);
+
+static IIO_DEVICE_ATTR(als_low_threshold, S_IRUGO | S_IWUSR,
+		show_als_low_threshold, store_als_low_threshold, 0);
+static IIO_DEVICE_ATTR(als_high_threshold, S_IRUGO | S_IWUSR,
+		show_als_high_threshold, store_als_high_threshold, 0);
+static IIO_DEVICE_ATTR(als_persist, S_IRUGO | S_IWUSR,
+		show_als_persist, store_als_persist, 0);
+static IIO_DEVICE_ATTR(als_ir_mode, S_IRUGO | S_IWUSR,
+		show_als_ir_mode, store_als_ir_mode, 0);
+static IIO_DEVICE_ATTR(als_value, S_IRUGO,
+		show_als_data, NULL, 0);
+static IIO_DEVICE_ATTR(wait_als_ir_thres, S_IRUGO,
+		show_wait_als_ir_int, NULL, 0);
+
+static IIO_DEVICE_ATTR(ir_value, S_IRUGO,
+		show_ir_data, NULL, 0);
+static IIO_DEVICE_ATTR(ir_low_threshold, S_IRUGO | S_IWUSR,
+		show_ir_low_threshold, store_ir_low_threshold, 0);
+static IIO_DEVICE_ATTR(ir_high_threshold, S_IRUGO | S_IWUSR,
+		show_ir_high_threshold, store_ir_high_threshold, 0);
+
+static IIO_DEVICE_ATTR(name, S_IRUGO, show_name, NULL, 0);
+
+static struct attribute *isl29028_attributes[] = {
+	&iio_dev_attr_name.dev_attr.attr,
+
+	&iio_dev_attr_ir_value.dev_attr.attr,
+
+	&iio_dev_attr_als_low_threshold.dev_attr.attr,
+	&iio_dev_attr_als_high_threshold.dev_attr.attr,
+	&iio_dev_attr_als_persist.dev_attr.attr,
+	&iio_dev_attr_als_ir_mode.dev_attr.attr,
+	&iio_dev_attr_als_value.dev_attr.attr,
+	&iio_dev_attr_wait_als_ir_thres.dev_attr.attr,
+	&iio_dev_attr_ir_low_threshold.dev_attr.attr,
+	&iio_dev_attr_ir_high_threshold.dev_attr.attr,
+
+	&iio_dev_attr_proximity_low_threshold.dev_attr.attr,
+	&iio_dev_attr_proximity_high_threshold.dev_attr.attr,
+	&iio_dev_attr_proximity_enable.dev_attr.attr,
+	&iio_dev_attr_proximity_period.dev_attr.attr,
+	&iio_dev_attr_proximity_persist.dev_attr.attr,
+	&iio_dev_attr_proximity_value.dev_attr.attr,
+	&iio_dev_attr_wait_proxim_thres.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group isl29108_group = {
+	.attrs = isl29028_attributes,
+};
+
+static int isl29028_chip_init(struct i2c_client *client)
+{
+	struct isl29028_chip *chip = i2c_get_clientdata(client);
+	int i;
+	bool st;
+
+	for (i = 0; i < ARRAY_SIZE(chip->reg_cache); i++)
+		chip->reg_cache[i] = 0;
+
+	chip->is_prox_enable  = 0;
+	chip->prox_low_thres = 0;
+	chip->prox_high_thres = 0xFF;
+	chip->prox_period = 0;
+	chip->prox_reading = 0;
+
+	chip->als_low_thres = 0;
+	chip->als_high_thres = 0xFFF;
+	chip->als_range = 1;
+	chip->als_reading = 0;
+	chip->als_ir_mode = 0;
+
+	chip->ir_high_thres = 0xFFF;
+	chip->ir_low_thres = 0;
+	chip->ir_reading = 0;
+
+	chip->is_int_enable = false;
+	chip->prox_persist = 1;
+	chip->als_persist = 1;
+	chip->is_proxim_int_waiting = false;
+	chip->is_als_int_waiting = false;
+
+	st = isl29028_write_data(client, ISL29028_REG_ADD_TEST1_MODE,
+					0x0, 0xFF, 0);
+	if (st)
+		st = isl29028_write_data(client, ISL29028_REG_ADD_TEST2_MODE,
+					0x0, 0xFF, 0);
+	if (st)
+		st = isl29028_write_data(client, ISL29028_REG_ADD_CONFIGURE,
+					0x0, 0xFF, 0);
+	if (st)
+		msleep(1);
+	if (!st) {
+		dev_err(&client->dev, "%s(): fails\n", __func__);
+		return -ENODEV;
+	}
+	return 0;
+}
+
+static irqreturn_t threshold_isr(int irq, void *irq_data)
+{
+	struct isl29028_chip *chip = (struct isl29028_chip *)irq_data;
+	s32 int_reg;
+	struct i2c_client *client = chip->client;
+
+	int_reg = i2c_smbus_read_byte_data(client, ISL29028_REG_ADD_INTERRUPT);
+	if (int_reg < 0) {
+		dev_err(&client->dev, "Error in reading register %d, error %d\n",
+				ISL29028_REG_ADD_INTERRUPT, int_reg);
+		return IRQ_HANDLED;
+	}
+
+	if (int_reg & INTERRUPT_PROX_FLAG_MASK) {
+		/* Write 0 to clear */
+		isl29028_write_data(client,
+			ISL29028_REG_ADD_INTERRUPT, INTERRUPT_PROX_FLAG_DIS,
+			INTERRUPT_PROX_FLAG_MASK, INTERRUPT_PROX_FLAG_SH);
+		if (chip->is_proxim_int_waiting)
+			complete(&chip->prox_completion);
+	}
+
+	if (int_reg & INTERRUPT_ALS_FLAG_MASK) {
+		/* Write 0 to clear */
+		isl29028_write_data(client,
+			ISL29028_REG_ADD_INTERRUPT, INTERRUPT_ALS_FLAG_DIS,
+			INTERRUPT_ALS_FLAG_MASK, INTERRUPT_ALS_FLAG_SH);
+		if (chip->is_als_int_waiting)
+			complete(&chip->als_completion);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static const struct iio_info isl29028_info = {
+	.attrs = &isl29108_group,
+	.driver_module = THIS_MODULE,
+};
+
+static int __devinit isl29028_probe(struct i2c_client *client,
+	const struct i2c_device_id *id)
+{
+	struct isl29028_chip *chip;
+	int err;
+
+	dev_dbg(&client->dev, "%s() called\n", __func__);
+
+	chip = kzalloc(sizeof(struct isl29028_chip), GFP_KERNEL);
+	if (!chip) {
+		dev_err(&client->dev, "Memory allocation fails\n");
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	i2c_set_clientdata(client, chip);
+	chip->client = client;
+	chip->irq = client->irq;
+
+	mutex_init(&chip->lock);
+
+	err = isl29028_chip_init(client);
+	if (err)
+		goto exit_free;
+
+	init_completion(&chip->prox_completion);
+	init_completion(&chip->als_completion);
+
+	if (chip->irq > 0) {
+		err = request_threaded_irq(chip->irq, NULL, threshold_isr,
+						IRQF_SHARED, "ISL29028", chip);
+		if (err) {
+			dev_err(&client->dev, "Unable to register irq %d; "
+				"error %d\n", chip->irq, err);
+			goto exit_free;
+		}
+		chip->is_int_enable = true;
+	}
+
+	chip->indio_dev = iio_allocate_device(0);
+	if (!chip->indio_dev) {
+		dev_err(&client->dev, "iio allocation fails\n");
+		goto exit_irq;
+	}
+
+	chip->indio_dev->info = &isl29028_info;
+	chip->indio_dev->dev.parent = &client->dev;
+	chip->indio_dev->dev_data = (void *)(chip);
+	chip->indio_dev->modes = INDIO_DIRECT_MODE;
+	err = iio_device_register(chip->indio_dev);
+	if (err) {
+		dev_err(&client->dev, "iio registration fails\n");
+		goto exit_iio_free;
+	}
+	dev_dbg(&client->dev, "%s() success\n", __func__);
+	return 0;
+
+exit_iio_free:
+	iio_free_device(chip->indio_dev);
+exit_irq:
+	if (chip->irq > 0)
+		free_irq(chip->irq, chip);
+exit_free:
+	kfree(chip);
+exit:
+	return err;
+}
+
+static int __devexit isl29028_remove(struct i2c_client *client)
+{
+	struct isl29028_chip *chip = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "%s()\n", __func__);
+	iio_device_unregister(chip->indio_dev);
+	if (chip->irq > 0)
+		free_irq(chip->irq, chip);
+	kfree(chip);
+	return 0;
+}
+
+static const struct i2c_device_id isl29028_id[] = {
+	{"isl29028", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, isl29028_id);
+
+static struct i2c_driver isl29028_driver = {
+	.class	= I2C_CLASS_HWMON,
+	.driver  = {
+		.name = "isl29028",
+		.owner = THIS_MODULE,
+	},
+	.probe	 = isl29028_probe,
+	.remove  = __devexit_p(isl29028_remove),
+	.id_table = isl29028_id,
+};
+
+static int __init isl29028_init(void)
+{
+	return i2c_add_driver(&isl29028_driver);
+}
+
+static void __exit isl29028_exit(void)
+{
+	i2c_del_driver(&isl29028_driver);
+}
+
+module_init(isl29028_init);
+module_exit(isl29028_exit);
diff --git a/drivers/staging/iio/light/ltr558als.c b/drivers/staging/iio/light/ltr558als.c
new file mode 100644
index 000000000000..4fef5d4cc034
--- /dev/null
+++ b/drivers/staging/iio/light/ltr558als.c
@@ -0,0 +1,883 @@
+/* Lite-On LTR-558ALS Linux Driver
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * 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.
+ */
+
+#include <linux/delay.h>
+#include <linux/earlysuspend.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/input.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/pm.h>
+#include <linux/wakelock.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <asm/gpio.h>
+#include <asm/uaccess.h>
+
+#include "ltr558als.h"
+#include "../iio.h"
+
+#define DRIVER_VERSION "1.0"
+#define DEVICE_NAME "LTR_558ALS"
+
+struct ltr558_chip {
+	struct i2c_client	*client;
+	struct mutex	lock;
+	struct iio_dev	*indio_dev;
+	int		irq;
+
+	bool	is_als_enable;
+	bool	als_enabled_before_suspend;
+	int		als_gainrange;
+	int		als_persist;
+	int		als_reading;
+	int		als_high_thres;
+	int		als_low_thres;
+
+	bool	is_prox_enable;
+	bool	prox_enabled_before_suspend;
+	int		ps_gainrange;
+	int		prox_persist;
+	int		prox_reading;
+	int		prox_low_thres;
+	int		prox_high_thres;
+};
+
+static int ltr558_i2c_read_reg(struct i2c_client *client, u8 regnum)
+{
+	return i2c_smbus_read_byte_data(client, regnum);
+}
+
+static int ltr558_i2c_write_reg(struct i2c_client *client, u8 regnum, u8 value)
+{
+	int writeerror;
+
+	writeerror = i2c_smbus_write_byte_data(client, regnum, value);
+	if (writeerror < 0)
+		return writeerror;
+	else
+		return 0;
+}
+
+static int ltr558_ps_enable(struct i2c_client *client, int gainrange)
+{
+	int error;
+	int setgain;
+
+	switch (gainrange) {
+		case PS_RANGE4:
+			setgain = MODE_PS_ON_Gain4;
+			break;
+
+		case PS_RANGE8:
+			setgain = MODE_PS_ON_Gain8;
+			break;
+
+		case PS_RANGE16:
+			setgain = MODE_PS_ON_Gain16;
+			break;
+
+		case PS_RANGE1:
+		default:
+			setgain = MODE_PS_ON_Gain1;
+			break;
+	}
+
+	error = ltr558_i2c_write_reg(client, LTR558_PS_CONTR, setgain);
+	mdelay(WAKEUP_DELAY);
+	return error;
+}
+
+static int ltr558_ps_disable(struct i2c_client *client)
+{
+	return ltr558_i2c_write_reg(client, LTR558_PS_CONTR, MODE_PS_StdBy);
+}
+
+static int ltr558_ps_read(struct i2c_client *client)
+{
+	int psval_lo = 0, psval_hi = 0, psdata = 0;
+	psval_lo = ltr558_i2c_read_reg(client, LTR558_PS_DATA_0);
+	if (psval_lo < 0){
+		psdata = psval_lo;
+		goto out;
+	}
+
+	psval_hi = ltr558_i2c_read_reg(client, LTR558_PS_DATA_1);
+	if (psval_hi < 0){
+		psdata = psval_hi;
+		goto out;
+	}
+
+	psdata = ((psval_hi & 0x07) * 256) + psval_lo;
+out:
+	return psdata;
+}
+
+static int ltr558_als_enable(struct i2c_client *client, int gainrange)
+{
+	int error = -1;
+
+	if (gainrange == ALS_RANGE1_320)
+		error = ltr558_i2c_write_reg(client, LTR558_ALS_CONTR,
+				MODE_ALS_ON_Range1);
+	else if (gainrange == ALS_RANGE2_64K)
+		error = ltr558_i2c_write_reg(client, LTR558_ALS_CONTR,
+				MODE_ALS_ON_Range2);
+
+	mdelay(WAKEUP_DELAY);
+	return error;
+}
+
+static int ltr558_als_disable(struct i2c_client *client)
+{
+	return ltr558_i2c_write_reg(client, LTR558_ALS_CONTR, MODE_ALS_StdBy);
+}
+
+static int ltr558_als_read(struct i2c_client *client)
+{
+	int alsval_ch0_lo, alsval_ch0_hi;
+	int alsval_ch1_lo, alsval_ch1_hi;
+	unsigned int alsval_ch0 = 0, alsval_ch1 = 0;
+	int luxdata = 0, ratio = 0;
+	long ch0_coeff = 0, ch1_coeff = 0;
+
+	alsval_ch1_lo = ltr558_i2c_read_reg(client, LTR558_ALS_DATA_CH1_0);
+	alsval_ch1_hi = ltr558_i2c_read_reg(client, LTR558_ALS_DATA_CH1_1);
+	alsval_ch1 = (alsval_ch1_hi * 256) + alsval_ch1_lo;
+
+	alsval_ch0_lo = ltr558_i2c_read_reg(client, LTR558_ALS_DATA_CH0_0);
+	alsval_ch0_hi = ltr558_i2c_read_reg(client, LTR558_ALS_DATA_CH0_1);
+	alsval_ch0 = (alsval_ch0_hi * 256) + alsval_ch0_lo;
+
+	if (alsval_ch0 == 0 && alsval_ch1 == 0)
+		return 0;
+
+	/* lux formula */
+	ratio = (100 * alsval_ch1)/(alsval_ch1 + alsval_ch0);
+
+	if (ratio < 45) {
+		ch0_coeff = 17743;
+		ch1_coeff = -11059;
+	}
+	else if ((ratio >= 45) && (ratio < 64)) {
+		ch0_coeff = 37725;
+		ch1_coeff = 13363;
+	}
+	else if ((ratio >= 64) && (ratio < 85)) {
+		ch0_coeff = 16900;
+		ch1_coeff = 1690;
+	}
+	else if (ratio >= 85) {
+		ch0_coeff = 0;
+		ch1_coeff = 0;
+	}
+
+	luxdata = ((alsval_ch0 * ch0_coeff) - (alsval_ch1 * ch1_coeff))/10000;
+	return luxdata;
+}
+
+static bool ltr558_set_proxim_high_threshold(struct i2c_client *client,
+		u32 thresh)
+{
+	bool st;
+	st = ltr558_i2c_write_reg(client, LTR558_PS_THRES_UP_0,
+			thresh & 0xFF);
+	if (!st)
+		st = ltr558_i2c_write_reg(client, LTR558_PS_THRES_UP_1,
+				(thresh >> 8) & 0x07);
+	return st;
+}
+
+static bool ltr558_set_proxim_low_threshold(struct i2c_client *client,
+		u32 thresh)
+{
+	bool st;
+	st = ltr558_i2c_write_reg(client, LTR558_PS_THRES_LOW_0,
+			thresh & 0xFF);
+	if (!st)
+		st = ltr558_i2c_write_reg(client, LTR558_PS_THRES_LOW_1,
+				(thresh >> 8) & 0x07);
+	return st;
+}
+
+static bool ltr558_set_als_high_threshold(struct i2c_client *client, u32 thresh)
+{
+	bool st;
+	st = ltr558_i2c_write_reg(client, LTR558_ALS_THRES_UP_0,
+			thresh & 0xFF);
+	if (!st)
+		st = ltr558_i2c_write_reg(client, LTR558_ALS_THRES_UP_1,
+				(thresh >> 8) & 0xFF);
+	return st;
+}
+
+static bool ltr558_set_als_low_threshold(struct i2c_client *client, u32 thresh)
+{
+	bool st;
+	st = ltr558_i2c_write_reg(client, LTR558_ALS_THRES_LOW_0,
+			thresh & 0xFF);
+	if (!st)
+		st = ltr558_i2c_write_reg(client, LTR558_ALS_THRES_LOW_1,
+				((thresh >> 8) & 0xFF));
+	return st;
+}
+
+/* Sysfs interface */
+
+/* proximity enable/disable  */
+static ssize_t show_prox_enable(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	if (chip->is_prox_enable)
+		return sprintf(buf, "1\n");
+	else
+		return sprintf(buf, "0\n");
+}
+
+static ssize_t store_prox_enable(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	int err = 0;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+	if ((lval != 1) && (lval != 0)) {
+		dev_err(dev, "illegal value %lu\n", lval);
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	if (lval == 1)
+		err = ltr558_ps_enable(client, PS_RANGE1);
+	else
+		err = ltr558_ps_disable(client);
+
+	if (err < 0)
+		dev_err(dev, "Error in enabling proximity\n");
+	else
+		chip->is_prox_enable = (lval) ? true : false;
+
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* Proximity low thresholds  */
+static ssize_t show_proxim_low_threshold(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "%d\n", chip->prox_low_thres);
+}
+
+static ssize_t store_proxim_low_threshold(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	bool st;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+
+	if ((lval > 0x7FF) || (lval < 0x0)) {
+		dev_err(dev, "The threshold is not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	st = ltr558_set_proxim_low_threshold(client, (u8)lval);
+	if (!st)
+		chip->prox_low_thres = (int)lval;
+	else
+		dev_err(dev, "Error in setting proximity low threshold\n");
+
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* Proximity high thresholds  */
+static ssize_t show_proxim_high_threshold(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "%d\n", chip->prox_high_thres);
+}
+
+static ssize_t store_proxim_high_threshold(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	bool st;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+
+	if ((lval > 0x7FF) || (lval < 0x0)) {
+		dev_err(dev, "The threshold is not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	st = ltr558_set_proxim_high_threshold(client, lval);
+	if (!st)
+		chip->prox_high_thres = (int)lval;
+	else
+		dev_err(dev, "Error in setting proximity high threshold\n");
+
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* als enable/disable  */
+static ssize_t show_als_enable(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	if (chip->is_als_enable)
+		return sprintf(buf, "1\n");
+	else
+		return sprintf(buf, "0\n");
+}
+
+static ssize_t store_als_enable(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	int err = 0;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+	if ((lval != 1) && (lval != 0)) {
+		dev_err(dev, "illegal value %lu\n", lval);
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	if (lval == 1)
+		err = ltr558_als_enable(client, chip->als_gainrange);
+	else
+		err = ltr558_als_disable(client);
+
+	if (err < 0)
+		dev_err(dev, "Error in enabling ALS\n");
+	else
+		chip->is_als_enable = (lval) ? true : false;
+
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* als low thresholds  */
+static ssize_t show_als_low_threshold(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "%d\n", chip->als_low_thres);
+}
+
+static ssize_t store_als_low_threshold(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	bool st;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+
+	if ((lval > 0xFFFF) || (lval < 0x0)) {
+		dev_err(dev, "The ALS threshold is not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	st = ltr558_set_als_low_threshold(client, (int)lval);
+	if (!st)
+		chip->als_low_thres = (int)lval;
+	else
+		dev_err(dev, "Error in setting als low threshold\n");
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* Als high thresholds  */
+static ssize_t show_als_high_threshold(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "%d\n", chip->als_high_thres);
+}
+
+static ssize_t store_als_high_threshold(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+	struct i2c_client *client = chip->client;
+	bool st;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+
+	if ((lval > 0xFFFF) || (lval < 0x0)) {
+		dev_err(dev, "The als threshold is not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	st = ltr558_set_als_high_threshold(client, (int)lval);
+	if (!st)
+		chip->als_high_thres = (int)lval;
+	else
+		dev_err(dev, "Error in setting als high threshold\n");
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* Proximity persist  */
+static ssize_t show_proxim_persist(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "%d\n", chip->prox_persist);
+}
+
+static ssize_t store_proxim_persist(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+
+	if ((lval > 16) || (lval < 0x0)) {
+		dev_err(dev, "The proximity persist is not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	chip->prox_persist = (int)lval;
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* als/ir persist  */
+static ssize_t show_als_persist(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	return sprintf(buf, "%d\n", chip->als_persist);
+}
+
+static ssize_t store_als_persist(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+	unsigned long lval;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	if (strict_strtoul(buf, 10, &lval))
+		return -EINVAL;
+
+	if ((lval > 16) || (lval < 0x0)) {
+		dev_err(dev, "The als persist is not supported\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&chip->lock);
+	chip->als_persist = (int)lval;
+	mutex_unlock(&chip->lock);
+	return count;
+}
+
+/* Display proxim data  */
+static ssize_t show_proxim_data(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+	int prox_data = 0;
+	ssize_t buf_count = 0;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+	mutex_lock(&chip->lock);
+
+	if (chip->is_prox_enable) {
+		prox_data = ltr558_ps_read(chip->client);
+		chip->prox_reading = prox_data;
+		buf_count = sprintf(buf, "%d\n", prox_data);
+	}
+	else
+		buf_count = sprintf(buf, "%d\n", chip->prox_reading);
+	mutex_unlock(&chip->lock);
+	return buf_count;
+}
+
+/* Display als data  */
+static ssize_t show_als_data(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+	ssize_t buf_count = 0;
+	int als_data = 0;
+
+	dev_vdbg(dev, "%s()\n", __func__);
+
+	mutex_lock(&chip->lock);
+	if (chip->is_als_enable) {
+		als_data = ltr558_als_read(chip->client);
+		buf_count = sprintf(buf, "%d\n", als_data);
+		chip->als_reading = als_data;
+	}
+	else
+		buf_count = sprintf(buf, "%d\n", chip->als_reading);
+	mutex_unlock(&chip->lock);
+
+	return buf_count;
+}
+
+/* Read name */
+static ssize_t show_name(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct ltr558_chip *chip = indio_dev->dev_data;
+	return sprintf(buf, "%s\n", chip->client->name);
+}
+
+static IIO_DEVICE_ATTR(proximity_low_threshold, S_IRUGO | S_IWUSR,
+		show_proxim_low_threshold, store_proxim_low_threshold, 0);
+static IIO_DEVICE_ATTR(proximity_high_threshold, S_IRUGO | S_IWUSR,
+		show_proxim_high_threshold, store_proxim_high_threshold, 0);
+static IIO_DEVICE_ATTR(proximity_persist, S_IRUGO | S_IWUSR,
+		show_proxim_persist, store_proxim_persist, 0);
+static IIO_DEVICE_ATTR(proximity_enable, S_IRUGO | S_IWUSR,
+		show_prox_enable, store_prox_enable, 0);
+static IIO_DEVICE_ATTR(proximity_value, S_IRUGO,
+		show_proxim_data, NULL, 0);
+
+static IIO_DEVICE_ATTR(als_low_threshold, S_IRUGO | S_IWUSR,
+		show_als_low_threshold, store_als_low_threshold, 0);
+static IIO_DEVICE_ATTR(als_high_threshold, S_IRUGO | S_IWUSR,
+		show_als_high_threshold, store_als_high_threshold, 0);
+static IIO_DEVICE_ATTR(als_persist, S_IRUGO | S_IWUSR,
+		show_als_persist, store_als_persist, 0);
+static IIO_DEVICE_ATTR(als_enable, S_IRUGO | S_IWUSR,
+		show_als_enable, store_als_enable, 0);
+static IIO_DEVICE_ATTR(als_value, S_IRUGO,
+		show_als_data, NULL, 0);
+
+static IIO_DEVICE_ATTR(name, S_IRUGO, show_name, NULL, 0);
+
+static struct attribute *ltr558_attributes[] = {
+	&iio_dev_attr_name.dev_attr.attr,
+
+	&iio_dev_attr_als_low_threshold.dev_attr.attr,
+	&iio_dev_attr_als_high_threshold.dev_attr.attr,
+	&iio_dev_attr_als_enable.dev_attr.attr,
+	&iio_dev_attr_als_persist.dev_attr.attr,
+	&iio_dev_attr_als_value.dev_attr.attr,
+
+	&iio_dev_attr_proximity_low_threshold.dev_attr.attr,
+	&iio_dev_attr_proximity_high_threshold.dev_attr.attr,
+	&iio_dev_attr_proximity_enable.dev_attr.attr,
+	&iio_dev_attr_proximity_persist.dev_attr.attr,
+	&iio_dev_attr_proximity_value.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group ltr558_group = {
+	.attrs = ltr558_attributes,
+};
+
+static int ltr558_chip_init(struct i2c_client *client)
+{
+	struct ltr558_chip *chip = i2c_get_clientdata(client);
+	int error = 0;
+
+	mdelay(PON_DELAY);
+
+	chip->is_prox_enable  = 0;
+	chip->prox_low_thres = 0;
+	chip->prox_high_thres = 0x7FF;
+	chip->prox_reading = 0;
+
+	chip->als_low_thres = 0;
+	chip->als_high_thres = 0xFFFF;
+	chip->als_reading = 0;
+
+	chip->is_als_enable = 0;
+	chip->prox_persist = 0;
+	chip->als_persist = 0;
+
+	/* Enable PS to Gain1 at startup */
+	chip->ps_gainrange = PS_RANGE1;
+	error = ltr558_ps_enable(client, chip->ps_gainrange);
+	if (error < 0)
+		goto out;
+
+
+	/* Enable ALS to Full Range at startup */
+	chip->als_gainrange = ALS_RANGE2_64K;
+	error = ltr558_als_enable(client, chip->als_gainrange);
+	if (error < 0)
+		goto out;
+
+out:
+	return error;
+}
+
+static const struct iio_info ltr558_info = {
+	.attrs = &ltr558_group,
+	.driver_module = THIS_MODULE,
+};
+
+static irqreturn_t threshold_isr(int irq, void *irq_data)
+{
+	struct ltr558_chip *chip = (struct ltr558_chip *)irq_data;
+	s32 int_reg;
+	struct i2c_client *client = chip->client;
+
+	int_reg = i2c_smbus_read_byte_data(client, LTR558_ALS_PS_STATUS);
+	if (int_reg < 0) {
+		dev_err(&client->dev, "Error in reading register %d, error %d\n",
+				LTR558_ALS_PS_STATUS, int_reg);
+		return IRQ_HANDLED;
+	}
+
+	if (int_reg & STATUS_ALS_INT_TRIGGER) {
+		if (int_reg & STATUS_ALS_NEW_DATA)
+			chip->als_reading = ltr558_als_read(client);
+	}
+
+	if (int_reg & STATUS_PS_INT_TRIGGER) {
+		if (int_reg & STATUS_PS_NEW_DATA)
+			chip->prox_reading = ltr558_ps_read(client);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int ltr558_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
+{
+	int ret = 0;
+	struct ltr558_chip *chip;
+
+	/* data memory allocation */
+	chip = kzalloc(sizeof(struct ltr558_chip), GFP_KERNEL);
+	if (chip == NULL) {
+		printk(KERN_ALERT "%s: LTR-558ALS kzalloc failed.\n", __func__);
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	i2c_set_clientdata(client, chip);
+	chip->client = client;
+	chip->irq = client->irq;
+
+	if (chip->irq > 0) {
+		ret = request_threaded_irq(chip->irq, NULL, threshold_isr,
+						IRQF_SHARED, "LTR558_ALS", chip);
+		if (ret) {
+			dev_err(&client->dev, "Unable to register irq %d; "
+				"ret %d\n", chip->irq, ret);
+			goto exit_free;
+		}
+	}
+
+	mutex_init(&chip->lock);
+
+	ret = ltr558_chip_init(client);
+	if (ret)
+		goto exit_free;
+
+	chip->indio_dev = iio_allocate_device(0);
+	if (!chip->indio_dev) {
+		dev_err(&client->dev, "iio allocation fails\n");
+		goto exit_irq;
+	}
+
+	chip->indio_dev->info = &ltr558_info;
+	chip->indio_dev->dev.parent = &client->dev;
+	chip->indio_dev->dev_data = (void *)(chip);
+	chip->indio_dev->modes = INDIO_DIRECT_MODE;
+	ret = iio_device_register(chip->indio_dev);
+	if (ret) {
+		dev_err(&client->dev, "iio registration fails\n");
+		goto exit_iio_free;
+	}
+
+	dev_dbg(&client->dev, "%s() success\n", __func__);
+	return 0;
+
+exit_iio_free:
+	iio_free_device(chip->indio_dev);
+exit_irq:
+	if (chip->irq > 0)
+		free_irq(chip->irq, chip);
+exit_free:
+	kfree(chip);
+exit:
+	return ret;
+}
+
+
+static int ltr558_remove(struct i2c_client *client)
+{
+	struct ltr558_chip *chip = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "%s()\n", __func__);
+	iio_device_unregister(chip->indio_dev);
+	if (chip->irq > 0)
+		free_irq(chip->irq, chip);
+	ltr558_ps_disable(client);
+	ltr558_als_disable(client);
+	kfree(chip);
+	return 0;
+}
+
+
+static int ltr558_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+	struct ltr558_chip *chip = i2c_get_clientdata(client);
+	int ret;
+
+	if (chip->is_als_enable == 1)
+		chip->als_enabled_before_suspend = 1;
+	if (chip->is_prox_enable == 1)
+		chip->prox_enabled_before_suspend = 1;
+
+	ret = ltr558_ps_disable(client);
+	if (ret == 0)
+		ret = ltr558_als_disable(client);
+
+	return ret;
+}
+
+
+static int ltr558_resume(struct i2c_client *client)
+{
+	struct ltr558_chip *chip = i2c_get_clientdata(client);
+	int error = 0;
+
+	mdelay(PON_DELAY);
+
+	if (chip->prox_enabled_before_suspend == 1) {
+		error = ltr558_ps_enable(client, chip->ps_gainrange);
+		if (error < 0)
+			goto out;
+	}
+
+	if (chip->als_enabled_before_suspend == 1) {
+		error = ltr558_als_enable(client, chip->als_gainrange);
+	}
+out:
+	return error;
+}
+
+
+static const struct i2c_device_id ltr558_id[] = {
+	{ DEVICE_NAME, 0 },
+	{}
+};
+
+
+static struct i2c_driver ltr558_driver = {
+	.class	= I2C_CLASS_HWMON,
+	.probe = ltr558_probe,
+	.remove = ltr558_remove,
+	.id_table = ltr558_id,
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = DEVICE_NAME,
+	},
+	.suspend = ltr558_suspend,
+	.resume = ltr558_resume,
+};
+
+
+static int __init ltr558_driverinit(void)
+{
+	return i2c_add_driver(&ltr558_driver);
+}
+
+
+static void __exit ltr558_driverexit(void)
+{
+	i2c_del_driver(&ltr558_driver);
+}
+
+
+module_init(ltr558_driverinit)
+module_exit(ltr558_driverexit)
diff --git a/drivers/staging/iio/light/ltr558als.h b/drivers/staging/iio/light/ltr558als.h
new file mode 100644
index 000000000000..17e1b903f661
--- /dev/null
+++ b/drivers/staging/iio/light/ltr558als.h
@@ -0,0 +1,76 @@
+/* Lite-On LTR-558ALS Linux Driver
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * 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.
+ */
+
+#ifndef _LTR558_H
+#define _LTR558_H
+
+
+/* LTR-558 Registers */
+#define LTR558_ALS_CONTR	0x80
+#define LTR558_PS_CONTR		0x81
+#define LTR558_PS_LED		0x82
+#define LTR558_PS_N_PULSES	0x83
+#define LTR558_PS_MEAS_RATE	0x84
+#define LTR558_ALS_MEAS_RATE	0x85
+#define LTR558_MANUFACTURER_ID	0x87
+
+#define LTR558_INTERRUPT	0x8F
+#define LTR558_PS_THRES_UP_0	0x90
+#define LTR558_PS_THRES_UP_1	0x91
+#define LTR558_PS_THRES_LOW_0	0x92
+#define LTR558_PS_THRES_LOW_1	0x93
+
+#define LTR558_ALS_THRES_UP_0	0x97
+#define LTR558_ALS_THRES_UP_1	0x98
+#define LTR558_ALS_THRES_LOW_0	0x99
+#define LTR558_ALS_THRES_LOW_1	0x9A
+
+#define LTR558_INTERRUPT_PERSIST 0x9E
+
+/* 558's Read Only Registers */
+#define LTR558_ALS_DATA_CH1_0	0x88
+#define LTR558_ALS_DATA_CH1_1	0x89
+#define LTR558_ALS_DATA_CH0_0	0x8A
+#define LTR558_ALS_DATA_CH0_1	0x8B
+#define LTR558_ALS_PS_STATUS	0x8C
+#define LTR558_PS_DATA_0	0x8D
+#define LTR558_PS_DATA_1	0x8E
+
+/* ALS PS STATUS 0x8C */
+#define STATUS_ALS_GAIN_RANGE1	0x10
+#define STATUS_ALS_INT_TRIGGER	0x08
+#define STATUS_ALS_NEW_DATA		0x04
+#define STATUS_PS_INT_TRIGGER	0x02
+#define STATUS_PS_NEW_DATA		0x01
+
+/* Basic Operating Modes */
+#define MODE_ALS_ON_Range1	0x0B
+#define MODE_ALS_ON_Range2	0x03
+#define MODE_ALS_StdBy		0x00
+
+#define MODE_PS_ON_Gain1	0x03
+#define MODE_PS_ON_Gain4	0x07
+#define MODE_PS_ON_Gain8	0x0B
+#define MODE_PS_ON_Gain16	0x0F
+#define MODE_PS_StdBy		0x00
+
+#define PS_RANGE1 	1
+#define PS_RANGE4	2
+#define PS_RANGE8 	4
+#define PS_RANGE16	8
+
+#define ALS_RANGE1_320	1
+#define ALS_RANGE2_64K 	2
+
+/* Power On response time in ms */
+#define PON_DELAY	600
+#define WAKEUP_DELAY	10
+
+#endif
diff --git a/drivers/staging/iio/magnetometer/ak8975.c b/drivers/staging/iio/magnetometer/ak8975.c
index 33919e87e7ce..ba923fc46656 100644
--- a/drivers/staging/iio/magnetometer/ak8975.c
+++ b/drivers/staging/iio/magnetometer/ak8975.c
@@ -109,8 +109,8 @@ static int ak8975_write_data(struct i2c_client *client,
 	struct i2c_msg msg;
 	u8 w_data[2];
 	int ret = 0;
-
-	struct ak8975_data *data = i2c_get_clientdata(client);
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct ak8975_data *data = iio_priv(indio_dev);
 
 	regval = data->reg_cache[reg];
 	regval &= ~mask;
@@ -171,7 +171,8 @@ static int ak8975_read_data(struct i2c_client *client,
  */
 static int ak8975_setup(struct i2c_client *client)
 {
-	struct ak8975_data *data = i2c_get_clientdata(client);
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct ak8975_data *data = iio_priv(indio_dev);
 	u8 device_id;
 	int ret;
 
@@ -403,7 +404,7 @@ static ssize_t show_raw(struct device *dev, struct device_attribute *devattr,
 	}
 
 	/* Wait for the conversion to complete. */
-	if (data->eoc_gpio)
+	if (gpio_is_valid(data->eoc_gpio))
 		ret = wait_conversion_complete_gpio(data);
 	else
 		ret = wait_conversion_complete_polled(data);
@@ -487,11 +488,14 @@ static int ak8975_probe(struct i2c_client *client,
 	int err;
 
 	/* Grab and set up the supplied GPIO. */
-	eoc_gpio = irq_to_gpio(client->irq);
+	if (client->dev.platform_data == NULL)
+		eoc_gpio = -1;
+	else
+		eoc_gpio = *(int *)(client->dev.platform_data);
 
 	/* We may not have a GPIO based IRQ to scan, that is fine, we will
 	   poll if so */
-	if (eoc_gpio > 0) {
+	if (gpio_is_valid(eoc_gpio)) {
 		err = gpio_request(eoc_gpio, "ak_8975");
 		if (err < 0) {
 			dev_err(&client->dev,
@@ -507,8 +511,7 @@ static int ak8975_probe(struct i2c_client *client,
 						eoc_gpio, err);
 			goto exit_gpio;
 		}
-	} else
-		eoc_gpio = 0;	/* No GPIO available */
+	}
 
 	/* Register with IIO */
 	indio_dev = iio_allocate_device(sizeof(*data));
@@ -517,12 +520,6 @@ static int ak8975_probe(struct i2c_client *client,
 		goto exit_gpio;
 	}
 	data = iio_priv(indio_dev);
-	/* Perform some basic start-of-day setup of the device. */
-	err = ak8975_setup(client);
-	if (err < 0) {
-		dev_err(&client->dev, "AK8975 initialization fails\n");
-		goto exit_gpio;
-	}
 
 	i2c_set_clientdata(client, indio_dev);
 	data->client = client;
@@ -533,6 +530,13 @@ static int ak8975_probe(struct i2c_client *client,
 	indio_dev->info = &ak8975_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
+	/* Perform some basic start-of-day setup of the device. */
+	err = ak8975_setup(client);
+	if (err < 0) {
+		dev_err(&client->dev, "AK8975 initialization fails\n");
+		goto exit_free_iio;
+	}
+
 	err = iio_device_register(indio_dev);
 	if (err < 0)
 		goto exit_free_iio;
@@ -542,7 +546,7 @@ static int ak8975_probe(struct i2c_client *client,
 exit_free_iio:
 	iio_free_device(indio_dev);
 exit_gpio:
-	if (eoc_gpio)
+	if (gpio_is_valid(eoc_gpio))
 		gpio_free(eoc_gpio);
 exit:
 	return err;
@@ -557,7 +561,7 @@ static int ak8975_remove(struct i2c_client *client)
 	iio_device_unregister(indio_dev);
 	iio_free_device(indio_dev);
 
-	if (eoc_gpio)
+	if (gpio_is_valid(eoc_gpio))
 		gpio_free(eoc_gpio);
 
 	return 0;
diff --git a/drivers/staging/nvec/nvec.c b/drivers/staging/nvec/nvec.c
index 72258e8c64ca..be3c392d83b3 100644
--- a/drivers/staging/nvec/nvec.c
+++ b/drivers/staging/nvec/nvec.c
@@ -353,7 +353,6 @@ static int __devinit tegra_nvec_probe(struct platform_device *pdev)
 	if(err < 0)
 		dev_err(nvec->dev, "couldn't request gpio\n");
 
-	tegra_gpio_enable(nvec->gpio);
 	gpio_direction_output(nvec->gpio, 1);
 	gpio_set_value(nvec->gpio, 1);
 
diff --git a/drivers/staging/quatech_usb2/quatech_usb2.c b/drivers/staging/quatech_usb2/quatech_usb2.c
index ca098cabc2bc..02fafecd4773 100644
--- a/drivers/staging/quatech_usb2/quatech_usb2.c
+++ b/drivers/staging/quatech_usb2/quatech_usb2.c
@@ -916,9 +916,10 @@ static int qt2_ioctl(struct tty_struct *tty,
 		dbg("%s() port %d, cmd == TIOCMIWAIT enter",
 			__func__, port->number);
 		prev_msr_value = port_extra->shadowMSR  & QT2_SERIAL_MSR_MASK;
+		barrier();
+		__set_current_state(TASK_INTERRUPTIBLE);
 		while (1) {
 			add_wait_queue(&port_extra->wait, &wait);
-			set_current_state(TASK_INTERRUPTIBLE);
 			schedule();
 			dbg("%s(): port %d, cmd == TIOCMIWAIT here\n",
 				__func__, port->number);
@@ -926,9 +927,12 @@ static int qt2_ioctl(struct tty_struct *tty,
 			/* see if a signal woke us up */
 			if (signal_pending(current))
 				return -ERESTARTSYS;
+			set_current_state(TASK_INTERRUPTIBLE);
 			msr_value = port_extra->shadowMSR & QT2_SERIAL_MSR_MASK;
-			if (msr_value == prev_msr_value)
+			if (msr_value == prev_msr_value) {
+				__set_current_state(TASK_RUNNING);
 				return -EIO;  /* no change - error */
+			}
 			if ((arg & TIOCM_RNG &&
 				((prev_msr_value & QT2_SERIAL_MSR_RI) ==
 					(msr_value & QT2_SERIAL_MSR_RI))) ||
@@ -941,6 +945,7 @@ static int qt2_ioctl(struct tty_struct *tty,
 				(arg & TIOCM_CTS &&
 				((prev_msr_value & QT2_SERIAL_MSR_CTS) ==
 					(msr_value & QT2_SERIAL_MSR_CTS)))) {
+				__set_current_state(TASK_RUNNING);
 				return 0;
 			}
 		} /* end inifinite while */
diff --git a/drivers/staging/rtl8712/usb_intf.c b/drivers/staging/rtl8712/usb_intf.c
index 21ce2af447b5..6cb7e28c99a6 100644
--- a/drivers/staging/rtl8712/usb_intf.c
+++ b/drivers/staging/rtl8712/usb_intf.c
@@ -86,6 +86,7 @@ static struct usb_device_id rtl871x_usb_id_tbl[] = {
 	{USB_DEVICE(0x0DF6, 0x0045)},
 	{USB_DEVICE(0x0DF6, 0x0059)}, /* 11n mode disable */
 	{USB_DEVICE(0x0DF6, 0x004B)},
+	{USB_DEVICE(0x0DF6, 0x005D)},
 	{USB_DEVICE(0x0DF6, 0x0063)},
 	/* Sweex */
 	{USB_DEVICE(0x177F, 0x0154)},
diff --git a/drivers/staging/rts_pstor/rtsx.c b/drivers/staging/rts_pstor/rtsx.c
index 16c73fbff51f..890e6cce6d3d 100644
--- a/drivers/staging/rts_pstor/rtsx.c
+++ b/drivers/staging/rts_pstor/rtsx.c
@@ -1015,6 +1015,7 @@ static int __devinit rtsx_probe(struct pci_dev *pci, const struct pci_device_id
 	th = kthread_create(rtsx_scan_thread, dev, "rtsx-scan");
 	if (IS_ERR(th)) {
 		printk(KERN_ERR "Unable to start the device-scanning thread\n");
+		complete(&dev->scanning_done);
 		quiesce_and_remove_host(dev);
 		err = PTR_ERR(th);
 		goto errout;
diff --git a/drivers/staging/serqt_usb2/serqt_usb2.c b/drivers/staging/serqt_usb2/serqt_usb2.c
index 12f5eba0355c..48aa61eb9c74 100644
--- a/drivers/staging/serqt_usb2/serqt_usb2.c
+++ b/drivers/staging/serqt_usb2/serqt_usb2.c
@@ -24,7 +24,6 @@ static int debug;
 #define DRIVER_DESC "Quatech USB to Serial Driver"
 
 #define	USB_VENDOR_ID_QUATECH			0x061d	/* Quatech VID */
-#define QUATECH_SSU100	0xC020	/* SSU100 */
 #define QUATECH_SSU200	0xC030	/* SSU200 */
 #define QUATECH_DSU100	0xC040	/* DSU100 */
 #define QUATECH_DSU200	0xC050	/* DSU200 */
@@ -127,7 +126,6 @@ static int debug;
 #define RS232_MODE          0x00
 
 static const struct usb_device_id serqt_id_table[] = {
-	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_SSU100)},
 	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_SSU200)},
 	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_DSU100)},
 	{USB_DEVICE(USB_VENDOR_ID_QUATECH, QUATECH_DSU200)},
@@ -775,7 +773,6 @@ static int qt_startup(struct usb_serial *serial)
 	}
 
 	switch (serial->dev->descriptor.idProduct) {
-	case QUATECH_SSU100:
 	case QUATECH_DSU100:
 	case QUATECH_QSU100:
 	case QUATECH_ESU100A:
diff --git a/drivers/staging/usbip/usbip_common.h b/drivers/staging/usbip/usbip_common.h
index 074ac4267d3b..be216175ae87 100644
--- a/drivers/staging/usbip/usbip_common.h
+++ b/drivers/staging/usbip/usbip_common.h
@@ -126,12 +126,12 @@ extern struct device_attribute dev_attr_usbip_debug;
  *
  */
 #define USBIP_CMD_SUBMIT	0x0001
-#define USBIP_RET_SUBMIT	0x0002
-#define USBIP_CMD_UNLINK	0x0003
+#define USBIP_CMD_UNLINK	0x0002
+#define USBIP_RET_SUBMIT	0x0003
 #define USBIP_RET_UNLINK	0x0004
 
-#define USBIP_DIR_IN	0x00
-#define USBIP_DIR_OUT	0x01
+#define USBIP_DIR_OUT	0x00
+#define USBIP_DIR_IN	0x01
 
 /**
  * struct usbip_header_basic - data pertinent to every request
diff --git a/drivers/staging/usbip/vhci_rx.c b/drivers/staging/usbip/vhci_rx.c
index 09c44abb89e8..3872b8cccdcf 100644
--- a/drivers/staging/usbip/vhci_rx.c
+++ b/drivers/staging/usbip/vhci_rx.c
@@ -68,6 +68,7 @@ static void vhci_recv_ret_submit(struct vhci_device *vdev,
 {
 	struct usbip_device *ud = &vdev->ud;
 	struct urb *urb;
+	unsigned long flags;
 
 	spin_lock(&vdev->priv_lock);
 	urb = pickup_urb_and_free_priv(vdev, pdu->base.seqnum);
@@ -101,9 +102,9 @@ static void vhci_recv_ret_submit(struct vhci_device *vdev,
 
 	usbip_dbg_vhci_rx("now giveback urb %p\n", urb);
 
-	spin_lock(&the_controller->lock);
+	spin_lock_irqsave(&the_controller->lock, flags);
 	usb_hcd_unlink_urb_from_ep(vhci_to_hcd(the_controller), urb);
-	spin_unlock(&the_controller->lock);
+	spin_unlock_irqrestore(&the_controller->lock, flags);
 
 	usb_hcd_giveback_urb(vhci_to_hcd(the_controller), urb, urb->status);
 
@@ -141,6 +142,7 @@ static void vhci_recv_ret_unlink(struct vhci_device *vdev,
 {
 	struct vhci_unlink *unlink;
 	struct urb *urb;
+	unsigned long flags;
 
 	usbip_dump_header(pdu);
 
@@ -170,9 +172,9 @@ static void vhci_recv_ret_unlink(struct vhci_device *vdev,
 		urb->status = pdu->u.ret_unlink.status;
 		pr_info("urb->status %d\n", urb->status);
 
-		spin_lock(&the_controller->lock);
+		spin_lock_irqsave(&the_controller->lock, flags);
 		usb_hcd_unlink_urb_from_ep(vhci_to_hcd(the_controller), urb);
-		spin_unlock(&the_controller->lock);
+		spin_unlock_irqrestore(&the_controller->lock, flags);
 
 		usb_hcd_giveback_urb(vhci_to_hcd(the_controller), urb,
 				     urb->status);
diff --git a/drivers/switch/Kconfig b/drivers/switch/Kconfig
new file mode 100644
index 000000000000..52385914b9ae
--- /dev/null
+++ b/drivers/switch/Kconfig
@@ -0,0 +1,15 @@
+menuconfig SWITCH
+	tristate "Switch class support"
+	help
+	  Say Y here to enable switch class support. This allows
+	  monitoring switches by userspace via sysfs and uevent.
+
+if SWITCH
+
+config SWITCH_GPIO
+	tristate "GPIO Swith support"
+	depends on GENERIC_GPIO
+	help
+	  Say Y here to enable GPIO based switch support.
+
+endif # SWITCH
diff --git a/drivers/switch/Makefile b/drivers/switch/Makefile
new file mode 100644
index 000000000000..f7606ed4a719
--- /dev/null
+++ b/drivers/switch/Makefile
@@ -0,0 +1,4 @@
+# Switch Class Driver
+obj-$(CONFIG_SWITCH)		+= switch_class.o
+obj-$(CONFIG_SWITCH_GPIO)	+= switch_gpio.o
+
diff --git a/drivers/switch/switch_class.c b/drivers/switch/switch_class.c
new file mode 100644
index 000000000000..2c94752f6459
--- /dev/null
+++ b/drivers/switch/switch_class.c
@@ -0,0 +1,175 @@
+/*
+ *  drivers/switch/switch_class.c
+ *
+ * Copyright (C) 2008 Google, Inc.
+ * Copyright (C) 2012 - NVIDIA, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/module.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/err.h>
+#include <linux/switch.h>
+
+struct class *switch_class;
+static atomic_t device_count;
+
+static ssize_t state_show(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct switch_dev *sdev = (struct switch_dev *)
+		dev_get_drvdata(dev);
+
+	if (sdev->print_state) {
+		int ret = sdev->print_state(sdev, buf);
+		if (ret >= 0)
+			return ret;
+	}
+	return sprintf(buf, "%d\n", sdev->state);
+}
+
+static ssize_t name_show(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct switch_dev *sdev = (struct switch_dev *)
+		dev_get_drvdata(dev);
+
+	if (sdev->print_name) {
+		int ret = sdev->print_name(sdev, buf);
+		if (ret >= 0)
+			return ret;
+	}
+	return sprintf(buf, "%s\n", sdev->name);
+}
+
+static DEVICE_ATTR(state, S_IRUGO | S_IWUSR, state_show, NULL);
+static DEVICE_ATTR(name, S_IRUGO | S_IWUSR, name_show, NULL);
+
+void switch_set_state(struct switch_dev *sdev, int state)
+{
+	char name_buf[120];
+	char state_buf[120];
+	char *prop_buf;
+	char *envp[3];
+	int env_offset = 0;
+	int length;
+
+	if (sdev->state != state) {
+		sdev->state = state;
+
+		prop_buf = (char *)get_zeroed_page(GFP_KERNEL);
+		if (prop_buf) {
+			length = name_show(sdev->dev, NULL, prop_buf);
+			if (length > 0) {
+				if (prop_buf[length - 1] == '\n')
+					prop_buf[length - 1] = 0;
+				snprintf(name_buf, sizeof(name_buf),
+					"SWITCH_NAME=%s", prop_buf);
+				envp[env_offset++] = name_buf;
+			}
+			length = state_show(sdev->dev, NULL, prop_buf);
+			if (length > 0) {
+				if (prop_buf[length - 1] == '\n')
+					prop_buf[length - 1] = 0;
+				snprintf(state_buf, sizeof(state_buf),
+					"SWITCH_STATE=%s", prop_buf);
+				envp[env_offset++] = state_buf;
+			}
+			envp[env_offset] = NULL;
+			kobject_uevent_env(&sdev->dev->kobj, KOBJ_CHANGE, envp);
+			free_page((unsigned long)prop_buf);
+		} else {
+			printk(KERN_ERR "out of memory in switch_set_state\n");
+			kobject_uevent(&sdev->dev->kobj, KOBJ_CHANGE);
+		}
+	}
+}
+EXPORT_SYMBOL_GPL(switch_set_state);
+
+static int create_switch_class(void)
+{
+	if (!switch_class) {
+		switch_class = class_create(THIS_MODULE, "switch");
+		if (IS_ERR(switch_class))
+			return PTR_ERR(switch_class);
+		atomic_set(&device_count, 0);
+	}
+
+	return 0;
+}
+
+int switch_dev_register(struct switch_dev *sdev)
+{
+	int ret;
+
+	if (!switch_class) {
+		ret = create_switch_class();
+		if (ret < 0)
+			return ret;
+	}
+
+	sdev->index = atomic_inc_return(&device_count);
+	sdev->dev = device_create(switch_class, NULL,
+		MKDEV(0, sdev->index), NULL, sdev->name);
+	if (IS_ERR(sdev->dev))
+		return PTR_ERR(sdev->dev);
+
+	ret = device_create_file(sdev->dev, &dev_attr_state);
+	if (ret < 0)
+		goto err_create_file_1;
+	ret = device_create_file(sdev->dev, &dev_attr_name);
+	if (ret < 0)
+		goto err_create_file_2;
+
+	dev_set_drvdata(sdev->dev, sdev);
+	sdev->state = 0;
+	return 0;
+
+err_create_file_2:
+	device_remove_file(sdev->dev, &dev_attr_state);
+err_create_file_1:
+	device_destroy(switch_class, MKDEV(0, sdev->index));
+	printk(KERN_ERR "switch: Failed to register driver %s\n", sdev->name);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(switch_dev_register);
+
+void switch_dev_unregister(struct switch_dev *sdev)
+{
+	device_remove_file(sdev->dev, &dev_attr_name);
+	device_remove_file(sdev->dev, &dev_attr_state);
+	dev_set_drvdata(sdev->dev, NULL);
+	device_destroy(switch_class, MKDEV(0, sdev->index));
+}
+EXPORT_SYMBOL_GPL(switch_dev_unregister);
+
+static int __init switch_class_init(void)
+{
+	return create_switch_class();
+}
+
+static void __exit switch_class_exit(void)
+{
+	class_destroy(switch_class);
+}
+
+module_init(switch_class_init);
+module_exit(switch_class_exit);
+
+MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
+MODULE_DESCRIPTION("Switch class driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/switch/switch_gpio.c b/drivers/switch/switch_gpio.c
new file mode 100644
index 000000000000..7e9faa211e48
--- /dev/null
+++ b/drivers/switch/switch_gpio.c
@@ -0,0 +1,172 @@
+/*
+ *  drivers/switch/switch_gpio.c
+ *
+ * Copyright (C) 2008 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/switch.h>
+#include <linux/workqueue.h>
+#include <linux/gpio.h>
+
+struct gpio_switch_data {
+	struct switch_dev sdev;
+	unsigned gpio;
+	const char *name_on;
+	const char *name_off;
+	const char *state_on;
+	const char *state_off;
+	int irq;
+	struct work_struct work;
+};
+
+static void gpio_switch_work(struct work_struct *work)
+{
+	int state;
+	struct gpio_switch_data	*data =
+		container_of(work, struct gpio_switch_data, work);
+
+	state = gpio_get_value(data->gpio);
+	switch_set_state(&data->sdev, state);
+}
+
+static irqreturn_t gpio_irq_handler(int irq, void *dev_id)
+{
+	struct gpio_switch_data *switch_data =
+	    (struct gpio_switch_data *)dev_id;
+
+	schedule_work(&switch_data->work);
+	return IRQ_HANDLED;
+}
+
+static ssize_t switch_gpio_print_state(struct switch_dev *sdev, char *buf)
+{
+	struct gpio_switch_data	*switch_data =
+		container_of(sdev, struct gpio_switch_data, sdev);
+	const char *state;
+	if (switch_get_state(sdev))
+		state = switch_data->state_on;
+	else
+		state = switch_data->state_off;
+
+	if (state)
+		return sprintf(buf, "%s\n", state);
+	return -1;
+}
+
+static int gpio_switch_probe(struct platform_device *pdev)
+{
+	struct gpio_switch_platform_data *pdata = pdev->dev.platform_data;
+	struct gpio_switch_data *switch_data;
+	int ret = 0;
+
+	if (!pdata)
+		return -EBUSY;
+
+	switch_data = kzalloc(sizeof(struct gpio_switch_data), GFP_KERNEL);
+	if (!switch_data)
+		return -ENOMEM;
+
+	switch_data->sdev.name = pdata->name;
+	switch_data->gpio = pdata->gpio;
+	switch_data->name_on = pdata->name_on;
+	switch_data->name_off = pdata->name_off;
+	switch_data->state_on = pdata->state_on;
+	switch_data->state_off = pdata->state_off;
+	switch_data->sdev.print_state = switch_gpio_print_state;
+
+    ret = switch_dev_register(&switch_data->sdev);
+	if (ret < 0)
+		goto err_switch_dev_register;
+
+	ret = gpio_request(switch_data->gpio, pdev->name);
+	if (ret < 0)
+		goto err_request_gpio;
+
+	ret = gpio_direction_input(switch_data->gpio);
+	if (ret < 0)
+		goto err_set_gpio_input;
+
+	INIT_WORK(&switch_data->work, gpio_switch_work);
+
+	switch_data->irq = gpio_to_irq(switch_data->gpio);
+	if (switch_data->irq < 0) {
+		ret = switch_data->irq;
+		goto err_detect_irq_num_failed;
+	}
+
+	ret = request_irq(switch_data->irq, gpio_irq_handler,
+			  IRQF_TRIGGER_LOW, pdev->name, switch_data);
+	if (ret < 0)
+		goto err_request_irq;
+
+	/* Perform initial detection */
+	gpio_switch_work(&switch_data->work);
+
+	return 0;
+
+err_request_irq:
+err_detect_irq_num_failed:
+err_set_gpio_input:
+	gpio_free(switch_data->gpio);
+err_request_gpio:
+    switch_dev_unregister(&switch_data->sdev);
+err_switch_dev_register:
+	kfree(switch_data);
+
+	return ret;
+}
+
+static int __devexit gpio_switch_remove(struct platform_device *pdev)
+{
+	struct gpio_switch_data *switch_data = platform_get_drvdata(pdev);
+
+	cancel_work_sync(&switch_data->work);
+	gpio_free(switch_data->gpio);
+    switch_dev_unregister(&switch_data->sdev);
+	kfree(switch_data);
+
+	return 0;
+}
+
+static struct platform_driver gpio_switch_driver = {
+	.probe		= gpio_switch_probe,
+	.remove		= __devexit_p(gpio_switch_remove),
+	.driver		= {
+		.name	= "switch-gpio",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init gpio_switch_init(void)
+{
+	return platform_driver_register(&gpio_switch_driver);
+}
+
+static void __exit gpio_switch_exit(void)
+{
+	platform_driver_unregister(&gpio_switch_driver);
+}
+
+module_init(gpio_switch_init);
+module_exit(gpio_switch_exit);
+
+MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
+MODULE_DESCRIPTION("GPIO Switch driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index 6a4ea29c2f36..c4ac6f6ed1ea 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -1043,6 +1043,8 @@ done:
 		 */
 		send_check_condition = 1;
 	} else {
+		cmd->data_length = cmd->se_cmd.data_length;
+
 		if (iscsit_decide_list_to_build(cmd, payload_length) < 0)
 			return iscsit_add_reject_from_cmd(
 				ISCSI_REASON_BOOKMARK_NO_RESOURCES,
@@ -1079,7 +1081,9 @@ attach_cmd:
 	 */
 	if (!cmd->immediate_data) {
 		cmdsn_ret = iscsit_sequence_cmd(conn, cmd, hdr->cmdsn);
-		if (cmdsn_ret == CMDSN_ERROR_CANNOT_RECOVER)
+		if (cmdsn_ret == CMDSN_LOWER_THAN_EXP)
+			return 0;
+		else if (cmdsn_ret == CMDSN_ERROR_CANNOT_RECOVER)
 			return iscsit_add_reject_from_cmd(
 				ISCSI_REASON_PROTOCOL_ERROR,
 				1, 0, buf, cmd);
@@ -1819,17 +1823,16 @@ attach:
 		int cmdsn_ret = iscsit_sequence_cmd(conn, cmd, hdr->cmdsn);
 		if (cmdsn_ret == CMDSN_HIGHER_THAN_EXP)
 			out_of_order_cmdsn = 1;
-		else if (cmdsn_ret == CMDSN_LOWER_THAN_EXP) {
+		else if (cmdsn_ret == CMDSN_LOWER_THAN_EXP)
 			return 0;
-		} else { /* (cmdsn_ret == CMDSN_ERROR_CANNOT_RECOVER) */
+		else if (cmdsn_ret == CMDSN_ERROR_CANNOT_RECOVER)
 			return iscsit_add_reject_from_cmd(
 					ISCSI_REASON_PROTOCOL_ERROR,
 					1, 0, buf, cmd);
-		}
 	}
 	iscsit_ack_from_expstatsn(conn, hdr->exp_statsn);
 
-	if (out_of_order_cmdsn)
+	if (out_of_order_cmdsn || !(hdr->opcode & ISCSI_OP_IMMEDIATE))
 		return 0;
 	/*
 	 * Found the referenced task, send to transport for processing.
@@ -2511,10 +2514,10 @@ static int iscsit_send_data_in(
 	if (hdr->flags & ISCSI_FLAG_DATA_STATUS) {
 		if (cmd->se_cmd.se_cmd_flags & SCF_OVERFLOW_BIT) {
 			hdr->flags |= ISCSI_FLAG_DATA_OVERFLOW;
-			hdr->residual_count = cpu_to_be32(cmd->residual_count);
+			hdr->residual_count = cpu_to_be32(cmd->se_cmd.residual_count);
 		} else if (cmd->se_cmd.se_cmd_flags & SCF_UNDERFLOW_BIT) {
 			hdr->flags |= ISCSI_FLAG_DATA_UNDERFLOW;
-			hdr->residual_count = cpu_to_be32(cmd->residual_count);
+			hdr->residual_count = cpu_to_be32(cmd->se_cmd.residual_count);
 		}
 	}
 	hton24(hdr->dlength, datain.length);
@@ -3016,10 +3019,10 @@ static int iscsit_send_status(
 	hdr->flags		|= ISCSI_FLAG_CMD_FINAL;
 	if (cmd->se_cmd.se_cmd_flags & SCF_OVERFLOW_BIT) {
 		hdr->flags |= ISCSI_FLAG_CMD_OVERFLOW;
-		hdr->residual_count = cpu_to_be32(cmd->residual_count);
+		hdr->residual_count = cpu_to_be32(cmd->se_cmd.residual_count);
 	} else if (cmd->se_cmd.se_cmd_flags & SCF_UNDERFLOW_BIT) {
 		hdr->flags |= ISCSI_FLAG_CMD_UNDERFLOW;
-		hdr->residual_count = cpu_to_be32(cmd->residual_count);
+		hdr->residual_count = cpu_to_be32(cmd->se_cmd.residual_count);
 	}
 	hdr->response		= cmd->iscsi_response;
 	hdr->cmd_status		= cmd->se_cmd.scsi_status;
@@ -3131,6 +3134,7 @@ static int iscsit_send_task_mgt_rsp(
 	hdr			= (struct iscsi_tm_rsp *) cmd->pdu;
 	memset(hdr, 0, ISCSI_HDR_LEN);
 	hdr->opcode		= ISCSI_OP_SCSI_TMFUNC_RSP;
+	hdr->flags		= ISCSI_FLAG_CMD_FINAL;
 	hdr->response		= iscsit_convert_tcm_tmr_rsp(se_tmr);
 	hdr->itt		= cpu_to_be32(cmd->init_task_tag);
 	cmd->stat_sn		= conn->stat_sn++;
diff --git a/drivers/target/iscsi/iscsi_target_core.h b/drivers/target/iscsi/iscsi_target_core.h
index 470ed551eeb5..8d18b2cfafec 100644
--- a/drivers/target/iscsi/iscsi_target_core.h
+++ b/drivers/target/iscsi/iscsi_target_core.h
@@ -395,7 +395,6 @@ struct iscsi_cmd {
 	u32			pdu_send_order;
 	/* Current struct iscsi_pdu in struct iscsi_cmd->pdu_list */
 	u32			pdu_start;
-	u32			residual_count;
 	/* Next struct iscsi_seq to send in struct iscsi_cmd->seq_list */
 	u32			seq_send_order;
 	/* Number of struct iscsi_seq in struct iscsi_cmd->seq_list */
diff --git a/drivers/target/loopback/tcm_loop.c b/drivers/target/loopback/tcm_loop.c
index aa2d67997235..fb85b35bd2f7 100644
--- a/drivers/target/loopback/tcm_loop.c
+++ b/drivers/target/loopback/tcm_loop.c
@@ -174,6 +174,24 @@ static int tcm_loop_new_cmd_map(struct se_cmd *se_cmd)
 		sgl_bidi = sdb->table.sgl;
 		sgl_bidi_count = sdb->table.nents;
 	}
+	/*
+	 * Because some userspace code via scsi-generic do not memset their
+	 * associated read buffers, go ahead and do that here for type
+	 * SCF_SCSI_CONTROL_SG_IO_CDB.  Also note that this is currently
+	 * guaranteed to be a single SGL for SCF_SCSI_CONTROL_SG_IO_CDB
+	 * by target core in transport_generic_allocate_tasks() ->
+	 * transport_generic_cmd_sequencer().
+	 */
+	if (se_cmd->se_cmd_flags & SCF_SCSI_CONTROL_SG_IO_CDB &&
+	    se_cmd->data_direction == DMA_FROM_DEVICE) {
+		struct scatterlist *sg = scsi_sglist(sc);
+		unsigned char *buf = kmap(sg_page(sg)) + sg->offset;
+
+		if (buf != NULL) {
+			memset(buf, 0, sg->length);
+			kunmap(sg_page(sg));
+		}
+	}
 
 	/* Tell the core about our preallocated memory */
 	ret = transport_generic_map_mem_to_cmd(se_cmd, scsi_sglist(sc),
diff --git a/drivers/target/target_core_alua.c b/drivers/target/target_core_alua.c
index 98c98a3a0250..8badcb469431 100644
--- a/drivers/target/target_core_alua.c
+++ b/drivers/target/target_core_alua.c
@@ -68,6 +68,15 @@ int core_emulate_report_target_port_groups(struct se_cmd *cmd)
 	unsigned char *buf;
 	u32 rd_len = 0, off = 4; /* Skip over RESERVED area to first
 				    Target port group descriptor */
+	/*
+	 * Need at least 4 bytes of response data or else we can't
+	 * even fit the return data length.
+	 */
+	if (cmd->data_length < 4) {
+		pr_warn("REPORT TARGET PORT GROUPS allocation length %u"
+			" too small\n", cmd->data_length);
+		return -EINVAL;
+	}
 
 	buf = transport_kmap_first_data_page(cmd);
 
@@ -75,6 +84,17 @@ int core_emulate_report_target_port_groups(struct se_cmd *cmd)
 	list_for_each_entry(tg_pt_gp, &su_dev->t10_alua.tg_pt_gps_list,
 			tg_pt_gp_list) {
 		/*
+		 * Check if the Target port group and Target port descriptor list
+		 * based on tg_pt_gp_members count will fit into the response payload.
+		 * Otherwise, bump rd_len to let the initiator know we have exceeded
+		 * the allocation length and the response is truncated.
+		 */
+		if ((off + 8 + (tg_pt_gp->tg_pt_gp_members * 4)) >
+		     cmd->data_length) {
+			rd_len += 8 + (tg_pt_gp->tg_pt_gp_members * 4);
+			continue;
+		}
+		/*
 		 * PREF: Preferred target port bit, determine if this
 		 * bit should be set for port group.
 		 */
diff --git a/drivers/target/target_core_cdb.c b/drivers/target/target_core_cdb.c
index f04d4ef99dca..5f91397f6017 100644
--- a/drivers/target/target_core_cdb.c
+++ b/drivers/target/target_core_cdb.c
@@ -477,7 +477,7 @@ target_emulate_evpd_86(struct se_cmd *cmd, unsigned char *buf)
 	if (cmd->data_length < 60)
 		return 0;
 
-	buf[2] = 0x3c;
+	buf[3] = 0x3c;
 	/* Set HEADSUP, ORDSUP, SIMPSUP */
 	buf[5] = 0x07;
 
diff --git a/drivers/target/target_core_file.c b/drivers/target/target_core_file.c
index bc1b33639b8d..ceb2a28513e8 100644
--- a/drivers/target/target_core_file.c
+++ b/drivers/target/target_core_file.c
@@ -288,9 +288,9 @@ static int fd_do_readv(struct se_task *task)
 		return -ENOMEM;
 	}
 
-	for (i = 0; i < task->task_sg_nents; i++) {
-		iov[i].iov_len = sg[i].length;
-		iov[i].iov_base = sg_virt(&sg[i]);
+	for_each_sg(task->task_sg, sg, task->task_sg_nents, i) {
+		iov[i].iov_len = sg->length;
+		iov[i].iov_base = sg_virt(sg);
 	}
 
 	old_fs = get_fs();
@@ -340,9 +340,9 @@ static int fd_do_writev(struct se_task *task)
 		return -ENOMEM;
 	}
 
-	for (i = 0; i < task->task_sg_nents; i++) {
-		iov[i].iov_len = sg[i].length;
-		iov[i].iov_base = sg_virt(&sg[i]);
+	for_each_sg(task->task_sg, sg, task->task_sg_nents, i) {
+		iov[i].iov_len = sg->length;
+		iov[i].iov_base = sg_virt(sg);
 	}
 
 	old_fs = get_fs();
diff --git a/drivers/target/target_core_tmr.c b/drivers/target/target_core_tmr.c
index 27d4925e51c3..5c1b8c599f6d 100644
--- a/drivers/target/target_core_tmr.c
+++ b/drivers/target/target_core_tmr.c
@@ -67,15 +67,16 @@ void core_tmr_release_req(
 	struct se_tmr_req *tmr)
 {
 	struct se_device *dev = tmr->tmr_dev;
+	unsigned long flags;
 
 	if (!dev) {
 		kmem_cache_free(se_tmr_req_cache, tmr);
 		return;
 	}
 
-	spin_lock_irq(&dev->se_tmr_lock);
+	spin_lock_irqsave(&dev->se_tmr_lock, flags);
 	list_del(&tmr->tmr_list);
-	spin_unlock_irq(&dev->se_tmr_lock);
+	spin_unlock_irqrestore(&dev->se_tmr_lock, flags);
 
 	kmem_cache_free(se_tmr_req_cache, tmr);
 }
@@ -100,54 +101,20 @@ static void core_tmr_handle_tas_abort(
 	transport_cmd_finish_abort(cmd, 0);
 }
 
-int core_tmr_lun_reset(
+static void core_tmr_drain_tmr_list(
 	struct se_device *dev,
 	struct se_tmr_req *tmr,
-	struct list_head *preempt_and_abort_list,
-	struct se_cmd *prout_cmd)
+	struct list_head *preempt_and_abort_list)
 {
-	struct se_cmd *cmd, *tcmd;
-	struct se_node_acl *tmr_nacl = NULL;
-	struct se_portal_group *tmr_tpg = NULL;
-	struct se_queue_obj *qobj = &dev->dev_queue_obj;
+	LIST_HEAD(drain_tmr_list);
 	struct se_tmr_req *tmr_p, *tmr_pp;
-	struct se_task *task, *task_tmp;
+	struct se_cmd *cmd;
 	unsigned long flags;
-	int fe_count, tas;
-	/*
-	 * TASK_ABORTED status bit, this is configurable via ConfigFS
-	 * struct se_device attributes.  spc4r17 section 7.4.6 Control mode page
-	 *
-	 * A task aborted status (TAS) bit set to zero specifies that aborted
-	 * tasks shall be terminated by the device server without any response
-	 * to the application client. A TAS bit set to one specifies that tasks
-	 * aborted by the actions of an I_T nexus other than the I_T nexus on
-	 * which the command was received shall be completed with TASK ABORTED
-	 * status (see SAM-4).
-	 */
-	tas = dev->se_sub_dev->se_dev_attrib.emulate_tas;
-	/*
-	 * Determine if this se_tmr is coming from a $FABRIC_MOD
-	 * or struct se_device passthrough..
-	 */
-	if (tmr && tmr->task_cmd && tmr->task_cmd->se_sess) {
-		tmr_nacl = tmr->task_cmd->se_sess->se_node_acl;
-		tmr_tpg = tmr->task_cmd->se_sess->se_tpg;
-		if (tmr_nacl && tmr_tpg) {
-			pr_debug("LUN_RESET: TMR caller fabric: %s"
-				" initiator port %s\n",
-				tmr_tpg->se_tpg_tfo->get_fabric_name(),
-				tmr_nacl->initiatorname);
-		}
-	}
-	pr_debug("LUN_RESET: %s starting for [%s], tas: %d\n",
-		(preempt_and_abort_list) ? "Preempt" : "TMR",
-		dev->transport->name, tas);
 	/*
 	 * Release all pending and outgoing TMRs aside from the received
 	 * LUN_RESET tmr..
 	 */
-	spin_lock_irq(&dev->se_tmr_lock);
+	spin_lock_irqsave(&dev->se_tmr_lock, flags);
 	list_for_each_entry_safe(tmr_p, tmr_pp, &dev->dev_tmr_list, tmr_list) {
 		/*
 		 * Allow the received TMR to return with FUNCTION_COMPLETE.
@@ -169,29 +136,48 @@ int core_tmr_lun_reset(
 		    (core_scsi3_check_cdb_abort_and_preempt(
 					preempt_and_abort_list, cmd) != 0))
 			continue;
-		spin_unlock_irq(&dev->se_tmr_lock);
 
-		spin_lock_irqsave(&cmd->t_state_lock, flags);
+		spin_lock(&cmd->t_state_lock);
 		if (!atomic_read(&cmd->t_transport_active)) {
-			spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-			spin_lock_irq(&dev->se_tmr_lock);
+			spin_unlock(&cmd->t_state_lock);
 			continue;
 		}
 		if (cmd->t_state == TRANSPORT_ISTATE_PROCESSING) {
-			spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-			spin_lock_irq(&dev->se_tmr_lock);
+			spin_unlock(&cmd->t_state_lock);
 			continue;
 		}
+		spin_unlock(&cmd->t_state_lock);
+
+		list_move_tail(&tmr_p->tmr_list, &drain_tmr_list);
+	}
+	spin_unlock_irqrestore(&dev->se_tmr_lock, flags);
+
+	while (!list_empty(&drain_tmr_list)) {
+		tmr = list_entry(drain_tmr_list.next, struct se_tmr_req, tmr_list);
+		list_del(&tmr->tmr_list);
+		cmd = tmr->task_cmd;
+
 		pr_debug("LUN_RESET: %s releasing TMR %p Function: 0x%02x,"
 			" Response: 0x%02x, t_state: %d\n",
-			(preempt_and_abort_list) ? "Preempt" : "", tmr_p,
-			tmr_p->function, tmr_p->response, cmd->t_state);
-		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+			(preempt_and_abort_list) ? "Preempt" : "", tmr,
+			tmr->function, tmr->response, cmd->t_state);
 
 		transport_cmd_finish_abort_tmr(cmd);
-		spin_lock_irq(&dev->se_tmr_lock);
 	}
-	spin_unlock_irq(&dev->se_tmr_lock);
+}
+
+static void core_tmr_drain_task_list(
+	struct se_device *dev,
+	struct se_cmd *prout_cmd,
+	struct se_node_acl *tmr_nacl,
+	int tas,
+	struct list_head *preempt_and_abort_list)
+{
+	LIST_HEAD(drain_task_list);
+	struct se_cmd *cmd;
+	struct se_task *task, *task_tmp;
+	unsigned long flags;
+	int fe_count;
 	/*
 	 * Complete outstanding struct se_task CDBs with TASK_ABORTED SAM status.
 	 * This is following sam4r17, section 5.6 Aborting commands, Table 38
@@ -236,9 +222,23 @@ int core_tmr_lun_reset(
 		if (prout_cmd == cmd)
 			continue;
 
-		list_del(&task->t_state_list);
+		list_move_tail(&task->t_state_list, &drain_task_list);
 		atomic_set(&task->task_state_active, 0);
-		spin_unlock_irqrestore(&dev->execute_task_lock, flags);
+		/*
+		 * Remove from task execute list before processing drain_task_list
+		 */
+		if (atomic_read(&task->task_execute_queue) != 0) {
+			list_del(&task->t_execute_list);
+			atomic_set(&task->task_execute_queue, 0);
+			atomic_dec(&dev->execute_tasks);
+		}
+	}
+	spin_unlock_irqrestore(&dev->execute_task_lock, flags);
+
+	while (!list_empty(&drain_task_list)) {
+		task = list_entry(drain_task_list.next, struct se_task, t_state_list);
+		list_del(&task->t_state_list);
+		cmd = task->task_se_cmd;
 
 		spin_lock_irqsave(&cmd->t_state_lock, flags);
 		pr_debug("LUN_RESET: %s cmd: %p task: %p"
@@ -275,20 +275,14 @@ int core_tmr_lun_reset(
 
 			atomic_set(&task->task_active, 0);
 			atomic_set(&task->task_stop, 0);
-		} else {
-			if (atomic_read(&task->task_execute_queue) != 0)
-				transport_remove_task_from_execute_queue(task, dev);
 		}
 		__transport_stop_task_timer(task, &flags);
 
 		if (!atomic_dec_and_test(&cmd->t_task_cdbs_ex_left)) {
-			spin_unlock_irqrestore(
-					&cmd->t_state_lock, flags);
+			spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 			pr_debug("LUN_RESET: Skipping task: %p, dev: %p for"
 				" t_task_cdbs_ex_left: %d\n", task, dev,
 				atomic_read(&cmd->t_task_cdbs_ex_left));
-
-			spin_lock_irqsave(&dev->execute_task_lock, flags);
 			continue;
 		}
 		fe_count = atomic_read(&cmd->t_fe_count);
@@ -298,22 +292,31 @@ int core_tmr_lun_reset(
 				" task: %p, t_fe_count: %d dev: %p\n", task,
 				fe_count, dev);
 			atomic_set(&cmd->t_transport_aborted, 1);
-			spin_unlock_irqrestore(&cmd->t_state_lock,
-						flags);
-			core_tmr_handle_tas_abort(tmr_nacl, cmd, tas, fe_count);
+			spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
-			spin_lock_irqsave(&dev->execute_task_lock, flags);
+			core_tmr_handle_tas_abort(tmr_nacl, cmd, tas, fe_count);
 			continue;
 		}
 		pr_debug("LUN_RESET: Got t_transport_active = 0 for task: %p,"
 			" t_fe_count: %d dev: %p\n", task, fe_count, dev);
 		atomic_set(&cmd->t_transport_aborted, 1);
 		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-		core_tmr_handle_tas_abort(tmr_nacl, cmd, tas, fe_count);
 
-		spin_lock_irqsave(&dev->execute_task_lock, flags);
+		core_tmr_handle_tas_abort(tmr_nacl, cmd, tas, fe_count);
 	}
-	spin_unlock_irqrestore(&dev->execute_task_lock, flags);
+}
+
+static void core_tmr_drain_cmd_list(
+	struct se_device *dev,
+	struct se_cmd *prout_cmd,
+	struct se_node_acl *tmr_nacl,
+	int tas,
+	struct list_head *preempt_and_abort_list)
+{
+	LIST_HEAD(drain_cmd_list);
+	struct se_queue_obj *qobj = &dev->dev_queue_obj;
+	struct se_cmd *cmd, *tcmd;
+	unsigned long flags;
 	/*
 	 * Release all commands remaining in the struct se_device cmd queue.
 	 *
@@ -337,11 +340,26 @@ int core_tmr_lun_reset(
 		 */
 		if (prout_cmd == cmd)
 			continue;
+		/*
+		 * Skip direct processing of TRANSPORT_FREE_CMD_INTR for
+		 * HW target mode fabrics.
+		 */
+		spin_lock(&cmd->t_state_lock);
+		if (cmd->t_state == TRANSPORT_FREE_CMD_INTR) {
+			spin_unlock(&cmd->t_state_lock);
+			continue;
+		}
+		spin_unlock(&cmd->t_state_lock);
 
-		atomic_dec(&cmd->t_transport_queue_active);
+		atomic_set(&cmd->t_transport_queue_active, 0);
 		atomic_dec(&qobj->queue_cnt);
-		list_del(&cmd->se_queue_node);
-		spin_unlock_irqrestore(&qobj->cmd_queue_lock, flags);
+		list_move_tail(&cmd->se_queue_node, &drain_cmd_list);
+	}
+	spin_unlock_irqrestore(&qobj->cmd_queue_lock, flags);
+
+	while (!list_empty(&drain_cmd_list)) {
+		cmd = list_entry(drain_cmd_list.next, struct se_cmd, se_queue_node);
+		list_del_init(&cmd->se_queue_node);
 
 		pr_debug("LUN_RESET: %s from Device Queue: cmd: %p t_state:"
 			" %d t_fe_count: %d\n", (preempt_and_abort_list) ?
@@ -354,9 +372,53 @@ int core_tmr_lun_reset(
 
 		core_tmr_handle_tas_abort(tmr_nacl, cmd, tas,
 				atomic_read(&cmd->t_fe_count));
-		spin_lock_irqsave(&qobj->cmd_queue_lock, flags);
 	}
-	spin_unlock_irqrestore(&qobj->cmd_queue_lock, flags);
+}
+
+int core_tmr_lun_reset(
+        struct se_device *dev,
+        struct se_tmr_req *tmr,
+        struct list_head *preempt_and_abort_list,
+        struct se_cmd *prout_cmd)
+{
+	struct se_node_acl *tmr_nacl = NULL;
+	struct se_portal_group *tmr_tpg = NULL;
+	int tas;
+        /*
+	 * TASK_ABORTED status bit, this is configurable via ConfigFS
+	 * struct se_device attributes.  spc4r17 section 7.4.6 Control mode page
+	 *
+	 * A task aborted status (TAS) bit set to zero specifies that aborted
+	 * tasks shall be terminated by the device server without any response
+	 * to the application client. A TAS bit set to one specifies that tasks
+	 * aborted by the actions of an I_T nexus other than the I_T nexus on
+	 * which the command was received shall be completed with TASK ABORTED
+	 * status (see SAM-4).
+	 */
+	tas = dev->se_sub_dev->se_dev_attrib.emulate_tas;
+	/*
+	 * Determine if this se_tmr is coming from a $FABRIC_MOD
+	 * or struct se_device passthrough..
+	 */
+	if (tmr && tmr->task_cmd && tmr->task_cmd->se_sess) {
+		tmr_nacl = tmr->task_cmd->se_sess->se_node_acl;
+		tmr_tpg = tmr->task_cmd->se_sess->se_tpg;
+		if (tmr_nacl && tmr_tpg) {
+			pr_debug("LUN_RESET: TMR caller fabric: %s"
+				" initiator port %s\n",
+				tmr_tpg->se_tpg_tfo->get_fabric_name(),
+				tmr_nacl->initiatorname);
+		}
+	}
+	pr_debug("LUN_RESET: %s starting for [%s], tas: %d\n",
+		(preempt_and_abort_list) ? "Preempt" : "TMR",
+		dev->transport->name, tas);
+
+	core_tmr_drain_tmr_list(dev, tmr, preempt_and_abort_list);
+	core_tmr_drain_task_list(dev, prout_cmd, tmr_nacl, tas,
+				preempt_and_abort_list);
+	core_tmr_drain_cmd_list(dev, prout_cmd, tmr_nacl, tas,
+				preempt_and_abort_list);
 	/*
 	 * Clear any legacy SPC-2 reservation when called during
 	 * LOGICAL UNIT RESET
@@ -379,3 +441,4 @@ int core_tmr_lun_reset(
 			dev->transport->name);
 	return 0;
 }
+
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index a4b0a8d27f25..e2added477b9 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -594,13 +594,14 @@ check_lun:
 
 void transport_cmd_finish_abort(struct se_cmd *cmd, int remove)
 {
-	transport_remove_cmd_from_queue(cmd, &cmd->se_dev->dev_queue_obj);
 	transport_lun_remove_cmd(cmd);
 
 	if (transport_cmd_check_stop_to_fabric(cmd))
 		return;
-	if (remove)
+	if (remove) {
+		transport_remove_cmd_from_queue(cmd, &cmd->se_dev->dev_queue_obj);
 		transport_generic_remove(cmd, 0);
+	}
 }
 
 void transport_cmd_finish_abort_tmr(struct se_cmd *cmd)
@@ -621,8 +622,6 @@ static void transport_add_cmd_to_queue(
 	struct se_queue_obj *qobj = &dev->dev_queue_obj;
 	unsigned long flags;
 
-	INIT_LIST_HEAD(&cmd->se_queue_node);
-
 	if (t_state) {
 		spin_lock_irqsave(&cmd->t_state_lock, flags);
 		cmd->t_state = t_state;
@@ -631,15 +630,21 @@ static void transport_add_cmd_to_queue(
 	}
 
 	spin_lock_irqsave(&qobj->cmd_queue_lock, flags);
+
+	/* If the cmd is already on the list, remove it before we add it */
+	if (!list_empty(&cmd->se_queue_node))
+		list_del(&cmd->se_queue_node);
+	else
+		atomic_inc(&qobj->queue_cnt);
+
 	if (cmd->se_cmd_flags & SCF_EMULATE_QUEUE_FULL) {
 		cmd->se_cmd_flags &= ~SCF_EMULATE_QUEUE_FULL;
 		list_add(&cmd->se_queue_node, &qobj->qobj_list);
 	} else
 		list_add_tail(&cmd->se_queue_node, &qobj->qobj_list);
-	atomic_inc(&cmd->t_transport_queue_active);
+	atomic_set(&cmd->t_transport_queue_active, 1);
 	spin_unlock_irqrestore(&qobj->cmd_queue_lock, flags);
 
-	atomic_inc(&qobj->queue_cnt);
 	wake_up_interruptible(&qobj->thread_wq);
 }
 
@@ -656,9 +661,9 @@ transport_get_cmd_from_queue(struct se_queue_obj *qobj)
 	}
 	cmd = list_first_entry(&qobj->qobj_list, struct se_cmd, se_queue_node);
 
-	atomic_dec(&cmd->t_transport_queue_active);
+	atomic_set(&cmd->t_transport_queue_active, 0);
 
-	list_del(&cmd->se_queue_node);
+	list_del_init(&cmd->se_queue_node);
 	atomic_dec(&qobj->queue_cnt);
 	spin_unlock_irqrestore(&qobj->cmd_queue_lock, flags);
 
@@ -668,7 +673,6 @@ transport_get_cmd_from_queue(struct se_queue_obj *qobj)
 static void transport_remove_cmd_from_queue(struct se_cmd *cmd,
 		struct se_queue_obj *qobj)
 {
-	struct se_cmd *t;
 	unsigned long flags;
 
 	spin_lock_irqsave(&qobj->cmd_queue_lock, flags);
@@ -676,14 +680,9 @@ static void transport_remove_cmd_from_queue(struct se_cmd *cmd,
 		spin_unlock_irqrestore(&qobj->cmd_queue_lock, flags);
 		return;
 	}
-
-	list_for_each_entry(t, &qobj->qobj_list, se_queue_node)
-		if (t == cmd) {
-			atomic_dec(&cmd->t_transport_queue_active);
-			atomic_dec(&qobj->queue_cnt);
-			list_del(&cmd->se_queue_node);
-			break;
-		}
+	atomic_set(&cmd->t_transport_queue_active, 0);
+	atomic_dec(&qobj->queue_cnt);
+	list_del_init(&cmd->se_queue_node);
 	spin_unlock_irqrestore(&qobj->cmd_queue_lock, flags);
 
 	if (atomic_read(&cmd->t_transport_queue_active)) {
@@ -1067,7 +1066,7 @@ static void transport_release_all_cmds(struct se_device *dev)
 	list_for_each_entry_safe(cmd, tcmd, &dev->dev_queue_obj.qobj_list,
 				se_queue_node) {
 		t_state = cmd->t_state;
-		list_del(&cmd->se_queue_node);
+		list_del_init(&cmd->se_queue_node);
 		spin_unlock_irqrestore(&dev->dev_queue_obj.cmd_queue_lock,
 				flags);
 
@@ -1598,6 +1597,7 @@ void transport_init_se_cmd(
 	INIT_LIST_HEAD(&cmd->se_delayed_node);
 	INIT_LIST_HEAD(&cmd->se_ordered_node);
 	INIT_LIST_HEAD(&cmd->se_qf_node);
+	INIT_LIST_HEAD(&cmd->se_queue_node);
 
 	INIT_LIST_HEAD(&cmd->t_task_list);
 	init_completion(&cmd->transport_lun_fe_stop_comp);
@@ -2562,10 +2562,15 @@ static inline u32 transport_get_sectors_6(
 
 	/*
 	 * Everything else assume TYPE_DISK Sector CDB location.
-	 * Use 8-bit sector value.
+	 * Use 8-bit sector value.  SBC-3 says:
+	 *
+	 *   A TRANSFER LENGTH field set to zero specifies that 256
+	 *   logical blocks shall be written.  Any other value
+	 *   specifies the number of logical blocks that shall be
+	 *   written.
 	 */
 type_disk:
-	return (u32)cdb[4];
+	return cdb[4] ? : 256;
 }
 
 static inline u32 transport_get_sectors_10(
@@ -3873,6 +3878,18 @@ int transport_generic_map_mem_to_cmd(
 
 	if ((cmd->se_cmd_flags & SCF_SCSI_DATA_SG_IO_CDB) ||
 	    (cmd->se_cmd_flags & SCF_SCSI_CONTROL_SG_IO_CDB)) {
+		/*
+		 * Reject SCSI data overflow with map_mem_to_cmd() as incoming
+		 * scatterlists already have been set to follow what the fabric
+		 * passes for the original expected data transfer length.
+		 */
+		if (cmd->se_cmd_flags & SCF_OVERFLOW_BIT) {
+			pr_warn("Rejecting SCSI DATA overflow for fabric using"
+				" SCF_PASSTHROUGH_SG_TO_MEM_NOALLOC\n");
+			cmd->se_cmd_flags |= SCF_SCSI_CDB_EXCEPTION;
+			cmd->scsi_sense_reason = TCM_INVALID_CDB_FIELD;
+			return -EINVAL;
+		}
 
 		cmd->t_data_sg = sgl;
 		cmd->t_data_nents = sgl_count;
@@ -4920,6 +4937,15 @@ EXPORT_SYMBOL(transport_check_aborted_status);
 
 void transport_send_task_abort(struct se_cmd *cmd)
 {
+	unsigned long flags;
+
+	spin_lock_irqsave(&cmd->t_state_lock, flags);
+	if (cmd->se_cmd_flags & SCF_SENT_CHECK_CONDITION) {
+		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+		return;
+	}
+	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+
 	/*
 	 * If there are still expected incoming fabric WRITEs, we wait
 	 * until until they have completed before sending a TASK_ABORTED
diff --git a/drivers/tty/hvc/hvc_dcc.c b/drivers/tty/hvc/hvc_dcc.c
index 435f6facbc23..44fbebab5075 100644
--- a/drivers/tty/hvc/hvc_dcc.c
+++ b/drivers/tty/hvc/hvc_dcc.c
@@ -46,6 +46,7 @@ static inline char __dcc_getchar(void)
 
 	asm volatile("mrc p14, 0, %0, c0, c5, 0	@ read comms data reg"
 		: "=r" (__c));
+	isb();
 
 	return __c;
 }
@@ -55,6 +56,7 @@ static inline void __dcc_putchar(char c)
 	asm volatile("mcr p14, 0, %0, c0, c5, 0	@ write a char"
 		: /* no output register */
 		: "r" (c));
+	isb();
 }
 
 static int hvc_dcc_put_chars(uint32_t vt, const char *buf, int count)
diff --git a/drivers/tty/pty.c b/drivers/tty/pty.c
index e809e9d4683c..e18604b3fc7d 100644
--- a/drivers/tty/pty.c
+++ b/drivers/tty/pty.c
@@ -670,12 +670,18 @@ static int ptmx_open(struct inode *inode, struct file *filp)
 
 	nonseekable_open(inode, filp);
 
+	retval = tty_alloc_file(filp);
+	if (retval)
+		return retval;
+
 	/* find a device that is not in use. */
 	tty_lock();
 	index = devpts_new_index(inode);
 	tty_unlock();
-	if (index < 0)
-		return index;
+	if (index < 0) {
+		retval = index;
+		goto err_file;
+	}
 
 	mutex_lock(&tty_mutex);
 	tty_lock();
@@ -689,27 +695,27 @@ static int ptmx_open(struct inode *inode, struct file *filp)
 
 	set_bit(TTY_PTY_LOCK, &tty->flags); /* LOCK THE SLAVE */
 
-	retval = tty_add_file(tty, filp);
-	if (retval)
-		goto out;
+	tty_add_file(tty, filp);
 
 	retval = devpts_pty_new(inode, tty->link);
 	if (retval)
-		goto out1;
+		goto err_release;
 
 	retval = ptm_driver->ops->open(tty, filp);
 	if (retval)
-		goto out2;
-out1:
+		goto err_release;
+
 	tty_unlock();
-	return retval;
-out2:
+	return 0;
+err_release:
 	tty_unlock();
 	tty_release(inode, filp);
 	return retval;
 out:
 	devpts_kill_index(inode, index);
 	tty_unlock();
+err_file:
+	tty_free_file(filp);
 	return retval;
 }
 
diff --git a/drivers/tty/serial/8250.c b/drivers/tty/serial/8250.c
index 7f50999eebc2..295fd2d7c713 100644
--- a/drivers/tty/serial/8250.c
+++ b/drivers/tty/serial/8250.c
@@ -307,7 +307,8 @@ static const struct serial8250_config uart_config[] = {
 		.tx_loadsz	= 8,
 		.fcr		= UART_FCR_ENABLE_FIFO | UART_FCR_R_TRIG_01 |
 				  UART_FCR_T_TRIG_01,
-		.flags		= UART_CAP_FIFO | UART_CAP_RTOIE,
+		.flags		= UART_CAP_FIFO | UART_CAP_RTOIE |
+				  UART_CAP_HW_CTSRTS,
 	},
 };
 
@@ -1640,6 +1641,7 @@ static irqreturn_t serial8250_interrupt(int irq, void *dev_id)
 	struct irq_info *i = dev_id;
 	struct list_head *l, *end = NULL;
 	int pass_counter = 0, handled = 0;
+	static unsigned int tegra_spurious = 0;
 
 	DEBUG_INTR("serial8250_interrupt(%d)...", irq);
 
@@ -1673,6 +1675,26 @@ static irqreturn_t serial8250_interrupt(int irq, void *dev_id)
 			handled = 1;
 
 			end = NULL;
+		} else if ((up->port.type == PORT_TEGRA) && !handled) {
+			/* irq 68: nobody cared workaround */
+
+			/* The irq request flag sometimes does not get reset or is
+			 * asserted immediately, but iir does not indicated this, if
+			 * so we get here with iir set to 0xc1, i.e. no irq pending. */
+
+			/* Try enabling the transmit register empty interrupt,
+			 * iir becomes 0xc2, irq gets de-asserted and ier is reverted
+			 * by the regular code flow in the ISR. */
+			tegra_spurious++;
+			/* Try this every 4096 spurious irq. */
+			if ((tegra_spurious % 0x1000) == 0xfff) {
+				up->ier |= UART_IER_THRI;
+				serial_out(up, UART_IER, up->ier);
+				udelay(1);
+				handled = 1;
+				end = NULL;
+			} else if (end == NULL)
+				end = l;
 		} else if (end == NULL)
 			end = l;
 
@@ -1902,8 +1924,13 @@ static void serial8250_set_mctrl(struct uart_port *port, unsigned int mctrl)
 		container_of(port, struct uart_8250_port, port);
 	unsigned char mcr = 0;
 
-	if (mctrl & TIOCM_RTS)
-		mcr |= UART_MCR_RTS;
+	if (up->port.type == PORT_TEGRA) {
+		if (mctrl & TIOCM_RTS)
+			mcr |= UART_MCR_HW_RTS;
+	} else {
+		if (mctrl & TIOCM_RTS)
+			mcr |= UART_MCR_RTS;
+	}
 	if (mctrl & TIOCM_DTR)
 		mcr |= UART_MCR_DTR;
 	if (mctrl & TIOCM_OUT1)
@@ -2461,6 +2488,19 @@ serial8250_do_set_termios(struct uart_port *port, struct ktermios *termios,
 		serial_outp(up, UART_EFR, efr);
 	}
 
+	if (up->capabilities & UART_CAP_HW_CTSRTS) {
+		unsigned char mcr = serial_inp(up, UART_MCR);
+		/*
+		 * TEGRA UART core support the auto control of the RTS and CTS
+		 * flow control.
+		 */
+		if (termios->c_cflag & CRTSCTS)
+			mcr |= UART_MCR_HW_CTS;
+		else
+			mcr &= ~UART_MCR_HW_CTS;
+		serial_outp(up, UART_MCR, mcr);
+	}
+
 #ifdef CONFIG_ARCH_OMAP
 	/* Workaround to enable 115200 baud on OMAP1510 internal ports */
 	if (cpu_is_omap1510() && is_omap_port(up)) {
@@ -2716,6 +2756,9 @@ static void serial8250_config_port(struct uart_port *port, int flags)
 	if (up->port.type == PORT_16550A && up->port.iotype == UPIO_AU)
 		up->bugs |= UART_BUG_NOMSR;
 
+	if (up->port.type == PORT_TEGRA)
+		up->bugs |= UART_BUG_NOMSR;
+
 	if (up->port.type != PORT_UNKNOWN && flags & UART_CONFIG_IRQ)
 		autoconfig_irq(up);
 
@@ -3379,6 +3422,7 @@ out:
 	return ret;
 }
 
+#ifdef MODULE
 static void __exit serial8250_exit(void)
 {
 	struct platform_device *isa_dev = serial8250_isa_devs;
@@ -3402,6 +3446,9 @@ static void __exit serial8250_exit(void)
 
 module_init(serial8250_init);
 module_exit(serial8250_exit);
+#else
+postcore_initcall(serial8250_init);
+#endif
 
 EXPORT_SYMBOL(serial8250_suspend_port);
 EXPORT_SYMBOL(serial8250_resume_port);
diff --git a/drivers/tty/serial/8250.h b/drivers/tty/serial/8250.h
index 6edf4a6a22d4..66a0c936b937 100644
--- a/drivers/tty/serial/8250.h
+++ b/drivers/tty/serial/8250.h
@@ -43,6 +43,7 @@ struct serial8250_config {
 #define UART_CAP_AFE	(1 << 11)	/* MCR-based hw flow control */
 #define UART_CAP_UUE	(1 << 12)	/* UART needs IER bit 6 set (Xscale) */
 #define UART_CAP_RTOIE	(1 << 13)	/* UART needs IER bit 4 set (Xscale, Tegra) */
+#define UART_CAP_HW_CTSRTS	(1 << 14)	/* UART core support hw control of RTS and CTS */
 
 #define UART_BUG_QUOT	(1 << 0)	/* UART has buggy quot LSB */
 #define UART_BUG_TXEN	(1 << 1)	/* UART has buggy TX IIR status */
diff --git a/drivers/tty/serial/Kconfig b/drivers/tty/serial/Kconfig
index 4dcb37bbdf92..5c9d9890c9a5 100644
--- a/drivers/tty/serial/Kconfig
+++ b/drivers/tty/serial/Kconfig
@@ -528,6 +528,14 @@ config SERIAL_S5PV210
 	help
 	  Serial port support for Samsung's S5P Family of SoC's
 
+config SERIAL_TEGRA
+	boolean "High speed serial support for NVIDIA Tegra SoCs"
+	depends on ARCH_TEGRA && TEGRA_SYSTEM_DMA
+	select SERIAL_CORE
+	help
+	  Support for the on-chip UARTs on NVIDIA Tegra SoC, providing
+	  /dev/ttyHSx, where x is determined by the number of UARTs on the
+	  platform
 
 config SERIAL_MAX3100
 	tristate "MAX3100 support"
@@ -1570,7 +1578,7 @@ config SERIAL_IFX6X60
 	  Support for the IFX6x60 modem devices on Intel MID platforms.
 
 config SERIAL_PCH_UART
-	tristate "Intel EG20T PCH / OKI SEMICONDUCTOR IOH(ML7213/ML7223) UART"
+	tristate "Intel EG20T PCH/LAPIS Semicon IOH(ML7213/ML7223/ML7831) UART"
 	depends on PCI
 	select SERIAL_CORE
 	help
@@ -1578,12 +1586,12 @@ config SERIAL_PCH_UART
 	  which is an IOH(Input/Output Hub) for x86 embedded processor.
 	  Enabling PCH_DMA, this PCH UART works as DMA mode.
 
-	  This driver also can be used for OKI SEMICONDUCTOR IOH(Input/
-	  Output Hub), ML7213 and ML7223.
-	  ML7213 IOH is for IVI(In-Vehicle Infotainment) use and ML7223 IOH is
-	  for MP(Media Phone) use.
-	  ML7213/ML7223 is companion chip for Intel Atom E6xx series.
-	  ML7213/ML7223 is completely compatible for Intel EG20T PCH.
+	  This driver also can be used for LAPIS Semiconductor IOH(Input/
+	  Output Hub), ML7213, ML7223 and ML7831.
+	  ML7213 IOH is for IVI(In-Vehicle Infotainment) use, ML7223 IOH is
+	  for MP(Media Phone) use and ML7831 IOH is for general purpose use.
+	  ML7213/ML7223/ML7831 is companion chip for Intel Atom E6xx series.
+	  ML7213/ML7223/ML7831 is completely compatible for Intel EG20T PCH.
 
 config SERIAL_MSM_SMD
 	bool "Enable tty device interface for some SMD ports"
diff --git a/drivers/tty/serial/Makefile b/drivers/tty/serial/Makefile
index 83b4da6a1062..efb9c27efbf3 100644
--- a/drivers/tty/serial/Makefile
+++ b/drivers/tty/serial/Makefile
@@ -1,6 +1,7 @@
 #
 # Makefile for the kernel serial device drivers.
 #
+GCOV_PROFILE_tegra_hsuart.o := y
 
 obj-$(CONFIG_SERIAL_CORE) += serial_core.o
 obj-$(CONFIG_SERIAL_21285) += 21285.o
@@ -43,6 +44,8 @@ obj-$(CONFIG_SERIAL_S3C2412) += s3c2412.o
 obj-$(CONFIG_SERIAL_S3C2440) += s3c2440.o
 obj-$(CONFIG_SERIAL_S3C6400) += s3c6400.o
 obj-$(CONFIG_SERIAL_S5PV210) += s5pv210.o
+CFLAGS_tegra_hsuart.o		= -Werror
+obj-$(CONFIG_SERIAL_TEGRA) += tegra_hsuart.o
 obj-$(CONFIG_SERIAL_MAX3100) += max3100.o
 obj-$(CONFIG_SERIAL_MAX3107) += max3107.o
 obj-$(CONFIG_SERIAL_MAX3107_AAVA) += max3107-aava.o
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index f5f6831b0a64..f20c82aa6d28 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -480,7 +480,7 @@ static int pl011_dma_tx_refill(struct uart_amba_port *uap)
 		return -EBUSY;
 	}
 
-	desc = dma_dev->device_prep_slave_sg(chan, &dmatx->sg, 1, DMA_TO_DEVICE,
+	desc = dmaengine_prep_slave_sg(chan, &dmatx->sg, 1, DMA_MEM_TO_DEV,
 					     DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	if (!desc) {
 		dma_unmap_sg(dma_dev->dev, &dmatx->sg, 1, DMA_TO_DEVICE);
@@ -663,7 +663,6 @@ static void pl011_dma_rx_callback(void *data);
 static int pl011_dma_rx_trigger_dma(struct uart_amba_port *uap)
 {
 	struct dma_chan *rxchan = uap->dmarx.chan;
-	struct dma_device *dma_dev;
 	struct pl011_dmarx_data *dmarx = &uap->dmarx;
 	struct dma_async_tx_descriptor *desc;
 	struct pl011_sgbuf *sgbuf;
@@ -675,8 +674,8 @@ static int pl011_dma_rx_trigger_dma(struct uart_amba_port *uap)
 	sgbuf = uap->dmarx.use_buf_b ?
 		&uap->dmarx.sgbuf_b : &uap->dmarx.sgbuf_a;
 	dma_dev = rxchan->device;
-	desc = rxchan->device->device_prep_slave_sg(rxchan, &sgbuf->sg, 1,
-					DMA_FROM_DEVICE,
+	desc = dmaengine_prep_slave_sg(rxchan, &sgbuf->sg, 1,
+					DMA_DEV_TO_MEM,
 					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	/*
 	 * If the DMA engine is busy and cannot prepare a
@@ -1733,9 +1732,19 @@ pl011_console_write(struct console *co, const char *s, unsigned int count)
 {
 	struct uart_amba_port *uap = amba_ports[co->index];
 	unsigned int status, old_cr, new_cr;
+	unsigned long flags;
+	int locked = 1;
 
 	clk_enable(uap->clk);
 
+	local_irq_save(flags);
+	if (uap->port.sysrq)
+		locked = 0;
+	else if (oops_in_progress)
+		locked = spin_trylock(&uap->port.lock);
+	else
+		spin_lock(&uap->port.lock);
+
 	/*
 	 *	First save the CR then disable the interrupts
 	 */
@@ -1755,6 +1764,10 @@ pl011_console_write(struct console *co, const char *s, unsigned int count)
 	} while (status & UART01x_FR_BUSY);
 	writew(old_cr, uap->port.membase + UART011_CR);
 
+	if (locked)
+		spin_unlock(&uap->port.lock);
+	local_irq_restore(flags);
+
 	clk_disable(uap->clk);
 }
 
diff --git a/drivers/tty/serial/atmel_serial.c b/drivers/tty/serial/atmel_serial.c
index b922f5d2e61e..948fc77b647a 100644
--- a/drivers/tty/serial/atmel_serial.c
+++ b/drivers/tty/serial/atmel_serial.c
@@ -199,8 +199,9 @@ void atmel_config_rs485(struct uart_port *port, struct serial_rs485 *rs485conf)
 {
 	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
 	unsigned int mode;
+	unsigned long flags;
 
-	spin_lock(&port->lock);
+	spin_lock_irqsave(&port->lock, flags);
 
 	/* Disable interrupts */
 	UART_PUT_IDR(port, atmel_port->tx_done_mask);
@@ -231,7 +232,7 @@ void atmel_config_rs485(struct uart_port *port, struct serial_rs485 *rs485conf)
 	/* Enable interrupts */
 	UART_PUT_IER(port, atmel_port->tx_done_mask);
 
-	spin_unlock(&port->lock);
+	spin_unlock_irqrestore(&port->lock, flags);
 
 }
 
diff --git a/drivers/tty/serial/jsm/jsm.h b/drivers/tty/serial/jsm/jsm.h
index b704c8ce0d71..5b837e749c16 100644
--- a/drivers/tty/serial/jsm/jsm.h
+++ b/drivers/tty/serial/jsm/jsm.h
@@ -183,10 +183,8 @@ struct jsm_board
 /* Our Read/Error/Write queue sizes */
 #define RQUEUEMASK	0x1FFF		/* 8 K - 1 */
 #define EQUEUEMASK	0x1FFF		/* 8 K - 1 */
-#define WQUEUEMASK	0x0FFF		/* 4 K - 1 */
 #define RQUEUESIZE	(RQUEUEMASK + 1)
 #define EQUEUESIZE	RQUEUESIZE
-#define WQUEUESIZE	(WQUEUEMASK + 1)
 
 
 /************************************************************************
@@ -226,10 +224,6 @@ struct jsm_channel {
 	u16		ch_e_head;	/* Head location of the error queue */
 	u16		ch_e_tail;	/* Tail location of the error queue */
 
-	u8		*ch_wqueue;	/* Our write queue buffer - malloc'ed */
-	u16		ch_w_head;	/* Head location of the write queue */
-	u16		ch_w_tail;	/* Tail location of the write queue */
-
 	u64		ch_rxcount;	/* total of data received so far */
 	u64		ch_txcount;	/* total of data transmitted so far */
 
@@ -378,7 +372,6 @@ extern int	jsm_debug;
  * Prototypes for non-static functions used in more than one module
  *
  *************************************************************************/
-int jsm_tty_write(struct uart_port *port);
 int jsm_tty_init(struct jsm_board *);
 int jsm_uart_port_init(struct jsm_board *);
 int jsm_remove_uart_port(struct jsm_board *);
diff --git a/drivers/tty/serial/jsm/jsm_driver.c b/drivers/tty/serial/jsm/jsm_driver.c
index 96da17868cf3..6c12d94e6d3f 100644
--- a/drivers/tty/serial/jsm/jsm_driver.c
+++ b/drivers/tty/serial/jsm/jsm_driver.c
@@ -211,7 +211,6 @@ static void __devexit jsm_remove_one(struct pci_dev *pdev)
 		if (brd->channels[i]) {
 			kfree(brd->channels[i]->ch_rqueue);
 			kfree(brd->channels[i]->ch_equeue);
-			kfree(brd->channels[i]->ch_wqueue);
 			kfree(brd->channels[i]);
 		}
 	}
@@ -270,6 +269,7 @@ static void jsm_io_resume(struct pci_dev *pdev)
 	struct jsm_board *brd = pci_get_drvdata(pdev);
 
 	pci_restore_state(pdev);
+	pci_save_state(pdev);
 
 	jsm_uart_port_init(brd);
 }
diff --git a/drivers/tty/serial/jsm/jsm_neo.c b/drivers/tty/serial/jsm/jsm_neo.c
index 4538c3e3646e..bd6e84699e11 100644
--- a/drivers/tty/serial/jsm/jsm_neo.c
+++ b/drivers/tty/serial/jsm/jsm_neo.c
@@ -496,12 +496,15 @@ static void neo_copy_data_from_queue_to_uart(struct jsm_channel *ch)
 	int s;
 	int qlen;
 	u32 len_written = 0;
+	struct circ_buf *circ;
 
 	if (!ch)
 		return;
 
+	circ = &ch->uart_port.state->xmit;
+
 	/* No data to write to the UART */
-	if (ch->ch_w_tail == ch->ch_w_head)
+	if (uart_circ_empty(circ))
 		return;
 
 	/* If port is "stopped", don't send any data to the UART */
@@ -517,11 +520,10 @@ static void neo_copy_data_from_queue_to_uart(struct jsm_channel *ch)
 		if (ch->ch_cached_lsr & UART_LSR_THRE) {
 			ch->ch_cached_lsr &= ~(UART_LSR_THRE);
 
-			writeb(ch->ch_wqueue[ch->ch_w_tail], &ch->ch_neo_uart->txrx);
+			writeb(circ->buf[circ->tail], &ch->ch_neo_uart->txrx);
 			jsm_printk(WRITE, INFO, &ch->ch_bd->pci_dev,
-					"Tx data: %x\n", ch->ch_wqueue[ch->ch_w_head]);
-			ch->ch_w_tail++;
-			ch->ch_w_tail &= WQUEUEMASK;
+					"Tx data: %x\n", circ->buf[circ->head]);
+			circ->tail = (circ->tail + 1) & (UART_XMIT_SIZE - 1);
 			ch->ch_txcount++;
 		}
 		return;
@@ -536,36 +538,36 @@ static void neo_copy_data_from_queue_to_uart(struct jsm_channel *ch)
 	n = UART_17158_TX_FIFOSIZE - ch->ch_t_tlevel;
 
 	/* cache head and tail of queue */
-	head = ch->ch_w_head & WQUEUEMASK;
-	tail = ch->ch_w_tail & WQUEUEMASK;
-	qlen = (head - tail) & WQUEUEMASK;
+	head = circ->head & (UART_XMIT_SIZE - 1);
+	tail = circ->tail & (UART_XMIT_SIZE - 1);
+	qlen = uart_circ_chars_pending(circ);
 
 	/* Find minimum of the FIFO space, versus queue length */
 	n = min(n, qlen);
 
 	while (n > 0) {
 
-		s = ((head >= tail) ? head : WQUEUESIZE) - tail;
+		s = ((head >= tail) ? head : UART_XMIT_SIZE) - tail;
 		s = min(s, n);
 
 		if (s <= 0)
 			break;
 
-		memcpy_toio(&ch->ch_neo_uart->txrxburst, ch->ch_wqueue + tail, s);
+		memcpy_toio(&ch->ch_neo_uart->txrxburst, circ->buf + tail, s);
 		/* Add and flip queue if needed */
-		tail = (tail + s) & WQUEUEMASK;
+		tail = (tail + s) & (UART_XMIT_SIZE - 1);
 		n -= s;
 		ch->ch_txcount += s;
 		len_written += s;
 	}
 
 	/* Update the final tail */
-	ch->ch_w_tail = tail & WQUEUEMASK;
+	circ->tail = tail & (UART_XMIT_SIZE - 1);
 
 	if (len_written >= ch->ch_t_tlevel)
 		ch->ch_flags &= ~(CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
 
-	if (!jsm_tty_write(&ch->uart_port))
+	if (uart_circ_empty(circ))
 		uart_write_wakeup(&ch->uart_port);
 }
 
@@ -946,7 +948,6 @@ static void neo_param(struct jsm_channel *ch)
 	if ((ch->ch_c_cflag & (CBAUD)) == 0) {
 		ch->ch_r_head = ch->ch_r_tail = 0;
 		ch->ch_e_head = ch->ch_e_tail = 0;
-		ch->ch_w_head = ch->ch_w_tail = 0;
 
 		neo_flush_uart_write(ch);
 		neo_flush_uart_read(ch);
diff --git a/drivers/tty/serial/jsm/jsm_tty.c b/drivers/tty/serial/jsm/jsm_tty.c
index 7a4a914ecff0..434bd881fcae 100644
--- a/drivers/tty/serial/jsm/jsm_tty.c
+++ b/drivers/tty/serial/jsm/jsm_tty.c
@@ -118,6 +118,19 @@ static void jsm_tty_set_mctrl(struct uart_port *port, unsigned int mctrl)
 	udelay(10);
 }
 
+/*
+ * jsm_tty_write()
+ *
+ * Take data from the user or kernel and send it out to the FEP.
+ * In here exists all the Transparent Print magic as well.
+ */
+static void jsm_tty_write(struct uart_port *port)
+{
+	struct jsm_channel *channel;
+	channel = container_of(port, struct jsm_channel, uart_port);
+	channel->ch_bd->bd_ops->copy_data_from_queue_to_uart(channel);
+}
+
 static void jsm_tty_start_tx(struct uart_port *port)
 {
 	struct jsm_channel *channel = (struct jsm_channel *)port;
@@ -216,14 +229,6 @@ static int jsm_tty_open(struct uart_port *port)
 			return -ENOMEM;
 		}
 	}
-	if (!channel->ch_wqueue) {
-		channel->ch_wqueue = kzalloc(WQUEUESIZE, GFP_KERNEL);
-		if (!channel->ch_wqueue) {
-			jsm_printk(INIT, ERR, &channel->ch_bd->pci_dev,
-				"unable to allocate write queue buf");
-			return -ENOMEM;
-		}
-	}
 
 	channel->ch_flags &= ~(CH_OPENING);
 	/*
@@ -237,7 +242,6 @@ static int jsm_tty_open(struct uart_port *port)
 	 */
 	channel->ch_r_head = channel->ch_r_tail = 0;
 	channel->ch_e_head = channel->ch_e_tail = 0;
-	channel->ch_w_head = channel->ch_w_tail = 0;
 
 	brd->bd_ops->flush_uart_write(channel);
 	brd->bd_ops->flush_uart_read(channel);
@@ -836,75 +840,3 @@ void jsm_check_queue_flow_control(struct jsm_channel *ch)
 		}
 	}
 }
-
-/*
- * jsm_tty_write()
- *
- * Take data from the user or kernel and send it out to the FEP.
- * In here exists all the Transparent Print magic as well.
- */
-int jsm_tty_write(struct uart_port *port)
-{
-	int bufcount;
-	int data_count = 0,data_count1 =0;
-	u16 head;
-	u16 tail;
-	u16 tmask;
-	u32 remain;
-	int temp_tail = port->state->xmit.tail;
-	struct jsm_channel *channel = (struct jsm_channel *)port;
-
-	tmask = WQUEUEMASK;
-	head = (channel->ch_w_head) & tmask;
-	tail = (channel->ch_w_tail) & tmask;
-
-	if ((bufcount = tail - head - 1) < 0)
-		bufcount += WQUEUESIZE;
-
-	bufcount = min(bufcount, 56);
-	remain = WQUEUESIZE - head;
-
-	data_count = 0;
-	if (bufcount >= remain) {
-		bufcount -= remain;
-		while ((port->state->xmit.head != temp_tail) &&
-		(data_count < remain)) {
-			channel->ch_wqueue[head++] =
-			port->state->xmit.buf[temp_tail];
-
-			temp_tail++;
-			temp_tail &= (UART_XMIT_SIZE - 1);
-			data_count++;
-		}
-		if (data_count == remain) head = 0;
-	}
-
-	data_count1 = 0;
-	if (bufcount > 0) {
-		remain = bufcount;
-		while ((port->state->xmit.head != temp_tail) &&
-			(data_count1 < remain)) {
-			channel->ch_wqueue[head++] =
-				port->state->xmit.buf[temp_tail];
-
-			temp_tail++;
-			temp_tail &= (UART_XMIT_SIZE - 1);
-			data_count1++;
-
-		}
-	}
-
-	port->state->xmit.tail = temp_tail;
-
-	data_count += data_count1;
-	if (data_count) {
-		head &= tmask;
-		channel->ch_w_head = head;
-	}
-
-	if (data_count) {
-		channel->ch_bd->bd_ops->copy_data_from_queue_to_uart(channel);
-	}
-
-	return data_count;
-}
diff --git a/drivers/tty/serial/pch_uart.c b/drivers/tty/serial/pch_uart.c
index b46218d679e2..9748681679eb 100644
--- a/drivers/tty/serial/pch_uart.c
+++ b/drivers/tty/serial/pch_uart.c
@@ -256,6 +256,8 @@ enum pch_uart_num_t {
 	pch_ml7213_uart2,
 	pch_ml7223_uart0,
 	pch_ml7223_uart1,
+	pch_ml7831_uart0,
+	pch_ml7831_uart1,
 };
 
 static struct pch_uart_driver_data drv_dat[] = {
@@ -268,6 +270,8 @@ static struct pch_uart_driver_data drv_dat[] = {
 	[pch_ml7213_uart2] = {PCH_UART_2LINE, 2},
 	[pch_ml7223_uart0] = {PCH_UART_8LINE, 0},
 	[pch_ml7223_uart1] = {PCH_UART_2LINE, 1},
+	[pch_ml7831_uart0] = {PCH_UART_8LINE, 0},
+	[pch_ml7831_uart1] = {PCH_UART_2LINE, 1},
 };
 
 static unsigned int default_baud = 9600;
@@ -626,6 +630,7 @@ static void pch_request_dma(struct uart_port *port)
 		dev_err(priv->port.dev, "%s:dma_request_channel FAILS(Rx)\n",
 			__func__);
 		dma_release_channel(priv->chan_tx);
+		priv->chan_tx = NULL;
 		return;
 	}
 
@@ -746,8 +751,8 @@ static int dma_handle_rx(struct eg20t_port *priv)
 
 	sg_dma_address(sg) = priv->rx_buf_dma;
 
-	desc = priv->chan_rx->device->device_prep_slave_sg(priv->chan_rx,
-			sg, 1, DMA_FROM_DEVICE,
+	desc = dmaengine_prep_slave_sg(priv->chan_rx,
+			sg, 1, DMA_DEV_TO_MEM,
 			DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 
 	if (!desc)
@@ -905,8 +910,8 @@ static unsigned int dma_handle_tx(struct eg20t_port *priv)
 			sg_dma_len(sg) = size;
 	}
 
-	desc = priv->chan_tx->device->device_prep_slave_sg(priv->chan_tx,
-					priv->sg_tx_p, nent, DMA_TO_DEVICE,
+	desc = dmaengine_prep_slave_sg(priv->chan_tx,
+					priv->sg_tx_p, nent, DMA_MEM_TO_DEV,
 					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	if (!desc) {
 		dev_err(priv->port.dev, "%s:device_prep_slave_sg Failed\n",
@@ -1213,8 +1218,7 @@ static void pch_uart_shutdown(struct uart_port *port)
 		dev_err(priv->port.dev,
 			"pch_uart_hal_set_fifo Failed(ret=%d)\n", ret);
 
-	if (priv->use_dma_flag)
-		pch_free_dma(port);
+	pch_free_dma(port);
 
 	free_irq(priv->port.irq, priv);
 }
@@ -1278,6 +1282,7 @@ static void pch_uart_set_termios(struct uart_port *port,
 	if (rtn)
 		goto out;
 
+	pch_uart_set_mctrl(&priv->port, priv->port.mctrl);
 	/* Don't rewrite B0 */
 	if (tty_termios_baud_rate(termios))
 		tty_termios_encode_baud_rate(termios, baud, baud);
@@ -1550,6 +1555,10 @@ static DEFINE_PCI_DEVICE_TABLE(pch_uart_pci_id) = {
 	 .driver_data = pch_ml7223_uart0},
 	{PCI_DEVICE(PCI_VENDOR_ID_ROHM, 0x800D),
 	 .driver_data = pch_ml7223_uart1},
+	{PCI_DEVICE(PCI_VENDOR_ID_ROHM, 0x8811),
+	 .driver_data = pch_ml7831_uart0},
+	{PCI_DEVICE(PCI_VENDOR_ID_ROHM, 0x8812),
+	 .driver_data = pch_ml7831_uart1},
 	{0,},
 };
 
diff --git a/drivers/tty/serial/pxa.c b/drivers/tty/serial/pxa.c
index 531931c1b250..5c8e3bba6c84 100644
--- a/drivers/tty/serial/pxa.c
+++ b/drivers/tty/serial/pxa.c
@@ -100,6 +100,16 @@ static inline void receive_chars(struct uart_pxa_port *up, int *status)
 	int max_count = 256;
 
 	do {
+		/* work around Errata #20 according to
+		 * Intel(R) PXA27x Processor Family
+		 * Specification Update (May 2005)
+		 *
+		 * Step 2
+		 * Disable the Reciever Time Out Interrupt via IER[RTOEI]
+		 */
+		up->ier &= ~UART_IER_RTOIE;
+		serial_out(up, UART_IER, up->ier);
+
 		ch = serial_in(up, UART_RX);
 		flag = TTY_NORMAL;
 		up->port.icount.rx++;
@@ -156,6 +166,16 @@ static inline void receive_chars(struct uart_pxa_port *up, int *status)
 		*status = serial_in(up, UART_LSR);
 	} while ((*status & UART_LSR_DR) && (max_count-- > 0));
 	tty_flip_buffer_push(tty);
+
+	/* work around Errata #20 according to
+	 * Intel(R) PXA27x Processor Family
+	 * Specification Update (May 2005)
+	 *
+	 * Step 6:
+	 * No more data in FIFO: Re-enable RTO interrupt via IER[RTOIE]
+	 */
+	up->ier |= UART_IER_RTOIE;
+	serial_out(up, UART_IER, up->ier);
 }
 
 static void transmit_chars(struct uart_pxa_port *up)
diff --git a/drivers/tty/serial/serial_core.c b/drivers/tty/serial/serial_core.c
index a3efbea5dbba..681955ac3284 100644
--- a/drivers/tty/serial/serial_core.c
+++ b/drivers/tty/serial/serial_core.c
@@ -91,6 +91,9 @@ static void __uart_start(struct tty_struct *tty)
 	struct uart_state *state = tty->driver_data;
 	struct uart_port *port = state->uart_port;
 
+	if (port->ops->wake_peer)
+		port->ops->wake_peer(port);
+
 	if (!uart_circ_empty(&state->xmit) && state->xmit.buf &&
 	    !tty->stopped && !tty->hw_stopped)
 		port->ops->start_tx(port);
@@ -2008,7 +2011,10 @@ int uart_resume_port(struct uart_driver *drv, struct uart_port *uport)
 		if (port->tty && port->tty->termios && termios.c_cflag == 0)
 			termios = *(port->tty->termios);
 
-		uport->ops->set_termios(uport, &termios, NULL);
+		if (console_suspend_enabled)
+			uart_change_pm(state, 0);
+		if (uport->type != PORT_TEGRA)
+			uport->ops->set_termios(uport, &termios, NULL);
 		if (console_suspend_enabled)
 			console_start(uport->cons);
 	}
diff --git a/drivers/tty/serial/sh-sci.c b/drivers/tty/serial/sh-sci.c
index 5ea6ec3442e6..ef49f0a08364 100644
--- a/drivers/tty/serial/sh-sci.c
+++ b/drivers/tty/serial/sh-sci.c
@@ -1213,8 +1213,8 @@ static void sci_submit_rx(struct sci_port *s)
 		struct scatterlist *sg = &s->sg_rx[i];
 		struct dma_async_tx_descriptor *desc;
 
-		desc = chan->device->device_prep_slave_sg(chan,
-			sg, 1, DMA_FROM_DEVICE, DMA_PREP_INTERRUPT);
+		desc = dmaengine_prep_slave_sg(chan,
+			sg, 1, DMA_DEV_TO_MEM, DMA_PREP_INTERRUPT);
 
 		if (desc) {
 			s->desc_rx[i] = desc;
@@ -1328,8 +1328,8 @@ static void work_fn_tx(struct work_struct *work)
 
 	BUG_ON(!sg_dma_len(sg));
 
-	desc = chan->device->device_prep_slave_sg(chan,
-			sg, s->sg_len_tx, DMA_TO_DEVICE,
+	desc = dmaengine_prep_slave_sg(chan,
+			sg, s->sg_len_tx, DMA_MEM_TO_DEV,
 			DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	if (!desc) {
 		/* switch to PIO */
diff --git a/drivers/tty/serial/tegra_hsuart.c b/drivers/tty/serial/tegra_hsuart.c
new file mode 100644
index 000000000000..3f38cca3d6ec
--- /dev/null
+++ b/drivers/tty/serial/tegra_hsuart.c
@@ -0,0 +1,1792 @@
+/*
+ * drivers/serial/tegra_hsuart.c
+ *
+ * High-speed serial driver for NVIDIA Tegra SoCs
+ *
+ * Copyright (C) 2009-2012 NVIDIA Corporation
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/serial.h>
+#include <linux/serial_core.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+#include <linux/termios.h>
+#include <linux/irq.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/string.h>
+#include <linux/pagemap.h>
+#include <linux/serial_reg.h>
+#include <linux/serial_8250.h>
+#include <linux/debugfs.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/tegra_uart.h>
+
+#include <mach/dma.h>
+#include <mach/clk.h>
+
+#define TEGRA_UART_TYPE "TEGRA_UART"
+
+#define TX_EMPTY_STATUS (UART_LSR_TEMT | UART_LSR_THRE)
+
+#define BYTES_TO_ALIGN(x) ((unsigned long)(ALIGN((x), sizeof(u32))) - \
+	(unsigned long)(x))
+
+#define UART_RX_DMA_BUFFER_SIZE    (2048*8)
+
+#define UART_LSR_FIFOE		0x80
+#define UART_LSR_TXFIFO_FULL	0x100
+#define UART_IER_EORD		0x20
+#define UART_MCR_RTS_EN		0x40
+#define UART_MCR_CTS_EN		0x20
+#define UART_LSR_ANY		(UART_LSR_OE | UART_LSR_BI | \
+				UART_LSR_PE | UART_LSR_FE)
+#define TEGRA_UART_IRDA_CSR	0x8
+#define TEGRA_UART_SIR_ENABLED	0x80
+
+#define TX_FORCE_PIO 0
+#define RX_FORCE_PIO 0
+
+const int dma_req_sel[] = {
+	TEGRA_DMA_REQ_SEL_UARTA,
+	TEGRA_DMA_REQ_SEL_UARTB,
+	TEGRA_DMA_REQ_SEL_UARTC,
+	TEGRA_DMA_REQ_SEL_UARTD,
+	TEGRA_DMA_REQ_SEL_UARTE,
+};
+
+#define TEGRA_TX_PIO			1
+#define TEGRA_TX_DMA			2
+
+#define TEGRA_UART_MIN_DMA		16
+#define TEGRA_UART_FIFO_SIZE		8
+
+#define TEGRA_UART_CLOSED    0
+#define TEGRA_UART_OPENED    1
+#define TEGRA_UART_CLOCK_OFF 2
+#define TEGRA_UART_SUSPEND   3
+
+/* Tx fifo trigger level setting in tegra uart is in
+ * reverse way then conventional uart */
+#define TEGRA_UART_TX_TRIG_16B 0x00
+#define TEGRA_UART_TX_TRIG_8B  0x10
+#define TEGRA_UART_TX_TRIG_4B  0x20
+#define TEGRA_UART_TX_TRIG_1B  0x30
+
+struct tegra_uart_port {
+	struct uart_port	uport;
+	char			port_name[32];
+	bool			is_irda;
+	int			(*irda_init)(void);
+	void			(*irda_start)(void);
+	int			(*irda_mode_switch)(int);
+	void			(*irda_shutdown)(void);
+	void			(*irda_remove)(void);
+
+	/* Module info */
+	unsigned long		size;
+	struct clk		*clk;
+	unsigned int		baud;
+
+	/* Register shadow */
+	unsigned char		fcr_shadow;
+	unsigned char		mcr_shadow;
+	unsigned char		lcr_shadow;
+	unsigned char		ier_shadow;
+	bool			use_cts_control;
+	bool			rts_active;
+
+	int			tx_in_progress;
+	unsigned int		tx_bytes;
+
+	dma_addr_t		xmit_dma_addr;
+
+	/* TX DMA */
+	struct tegra_dma_req	tx_dma_req;
+	struct tegra_dma_channel *tx_dma;
+
+	/* RX DMA */
+	struct tegra_dma_req	rx_dma_req;
+	struct tegra_dma_channel *rx_dma;
+
+	bool			use_rx_dma;
+	bool			use_tx_dma;
+	int			uart_state;
+	bool			rx_timeout;
+	int			rx_in_progress;
+};
+
+static void tegra_set_baudrate(struct tegra_uart_port *t, unsigned int baud);
+static void do_handle_rx_pio(struct tegra_uart_port *t);
+static void do_handle_rx_dma(struct tegra_uart_port *t);
+static void set_rts(struct tegra_uart_port *t, bool active);
+static void tegra_start_rx(struct uart_port *u);
+static void tegra_stop_rx(struct uart_port *u);
+
+static inline u8 uart_readb(struct tegra_uart_port *t, unsigned long reg)
+{
+	u8 val = readb(t->uport.membase + (reg << t->uport.regshift));
+	dev_vdbg(t->uport.dev, "%s: %p %03lx = %02x\n", __func__,
+		t->uport.membase, reg << t->uport.regshift, val);
+	return val;
+}
+
+static inline u32 uart_readl(struct tegra_uart_port *t, unsigned long reg)
+{
+	u32 val = readl(t->uport.membase + (reg << t->uport.regshift));
+	dev_vdbg(t->uport.dev, "%s: %p %03lx = %02x\n", __func__,
+		t->uport.membase, reg << t->uport.regshift, val);
+	return val;
+}
+
+static inline void uart_writeb(struct tegra_uart_port *t, u8 val,
+	unsigned long reg)
+{
+	dev_vdbg(t->uport.dev, "%s: %p %03lx %02x\n",
+		__func__, t->uport.membase, reg << t->uport.regshift, val);
+	writeb(val, t->uport.membase + (reg << t->uport.regshift));
+}
+
+static inline void uart_writel(struct tegra_uart_port *t, u32 val,
+	unsigned long reg)
+{
+	dev_vdbg(t->uport.dev, "%s: %p %03lx %08x\n",
+		__func__, t->uport.membase, reg << t->uport.regshift, val);
+	writel(val, t->uport.membase + (reg << t->uport.regshift));
+}
+
+static void fill_tx_fifo(struct tegra_uart_port *t, int max_bytes)
+{
+	int i;
+	struct circ_buf *xmit = &t->uport.state->xmit;
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+	unsigned long lsr;
+#endif
+
+	for (i = 0; i < max_bytes; i++) {
+		BUG_ON(uart_circ_empty(xmit));
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+		lsr = uart_readl(t, UART_LSR);
+		if ((lsr & UART_LSR_TXFIFO_FULL))
+			break;
+#endif
+		uart_writeb(t, xmit->buf[xmit->tail], UART_TX);
+		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+		t->uport.icount.tx++;
+	}
+}
+
+static void tegra_start_pio_tx(struct tegra_uart_port *t, unsigned int bytes)
+{
+	if (bytes > TEGRA_UART_FIFO_SIZE)
+		bytes = TEGRA_UART_FIFO_SIZE;
+
+	t->fcr_shadow &= ~UART_FCR_T_TRIG_11;
+	t->fcr_shadow |= TEGRA_UART_TX_TRIG_8B;
+	uart_writeb(t, t->fcr_shadow, UART_FCR);
+	t->tx_in_progress = TEGRA_TX_PIO;
+	t->tx_bytes = bytes;
+	t->ier_shadow |= UART_IER_THRI;
+	uart_writeb(t, t->ier_shadow, UART_IER);
+}
+
+static void tegra_start_dma_tx(struct tegra_uart_port *t, unsigned long bytes)
+{
+	struct circ_buf *xmit;
+	xmit = &t->uport.state->xmit;
+
+	dma_sync_single_for_device(t->uport.dev, t->xmit_dma_addr,
+		UART_XMIT_SIZE, DMA_TO_DEVICE);
+
+	t->fcr_shadow &= ~UART_FCR_T_TRIG_11;
+	t->fcr_shadow |= TEGRA_UART_TX_TRIG_4B;
+	uart_writeb(t, t->fcr_shadow, UART_FCR);
+
+	t->tx_bytes = bytes & ~(sizeof(u32)-1);
+	t->tx_dma_req.source_addr = t->xmit_dma_addr + xmit->tail;
+	t->tx_dma_req.size = t->tx_bytes;
+
+	t->tx_in_progress = TEGRA_TX_DMA;
+
+	tegra_dma_enqueue_req(t->tx_dma, &t->tx_dma_req);
+}
+
+/* Called with u->lock taken */
+static void tegra_start_next_tx(struct tegra_uart_port *t)
+{
+	unsigned long tail;
+	unsigned long count;
+	unsigned long lsr;
+	struct uart_port *u = &t->uport;
+
+	struct circ_buf *xmit;
+
+	xmit = &t->uport.state->xmit;
+	tail = (unsigned long)&xmit->buf[xmit->tail];
+	count = CIRC_CNT_TO_END(xmit->head, xmit->tail, UART_XMIT_SIZE);
+
+
+	dev_vdbg(t->uport.dev, "+%s %lu %d\n", __func__, count,
+		t->tx_in_progress);
+
+	if (count == 0) {
+		if (t->is_irda) {
+			do {
+				lsr = uart_readb(t, UART_LSR);
+				if (lsr & UART_LSR_TEMT)
+					break;
+			} while (1);
+			tegra_start_rx(u);
+		}
+		goto out;
+	}
+
+	if (t->is_irda) {
+		if (t->rx_in_progress)
+			tegra_stop_rx(u);
+	}
+
+	if (!t->use_tx_dma || count < TEGRA_UART_MIN_DMA)
+		tegra_start_pio_tx(t, count);
+	else if (BYTES_TO_ALIGN(tail) > 0)
+		tegra_start_pio_tx(t, BYTES_TO_ALIGN(tail));
+	else
+		tegra_start_dma_tx(t, count);
+
+out:
+	dev_vdbg(t->uport.dev, "-%s", __func__);
+}
+
+/* Called by serial core driver with u->lock taken. */
+static void tegra_start_tx(struct uart_port *u)
+{
+	struct tegra_uart_port *t;
+	struct circ_buf *xmit;
+
+	t = container_of(u, struct tegra_uart_port, uport);
+	xmit = &u->state->xmit;
+
+	if (!uart_circ_empty(xmit) && !t->tx_in_progress)
+		tegra_start_next_tx(t);
+}
+
+static int tegra_start_dma_rx(struct tegra_uart_port *t)
+{
+	wmb();
+	dma_sync_single_for_device(t->uport.dev, t->rx_dma_req.dest_addr,
+			t->rx_dma_req.size, DMA_TO_DEVICE);
+	if (tegra_dma_enqueue_req(t->rx_dma, &t->rx_dma_req)) {
+		dev_err(t->uport.dev, "Could not enqueue Rx DMA req\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void tegra_rx_dma_threshold_callback(struct tegra_dma_req *req)
+{
+	struct tegra_uart_port *t = req->dev;
+	struct uart_port *u = &t->uport;
+	unsigned long flags;
+
+	spin_lock_irqsave(&u->lock, flags);
+
+	do_handle_rx_dma(t);
+
+	spin_unlock_irqrestore(&u->lock, flags);
+}
+
+/*
+ * It is expected that the callers take the UART lock when this API is called.
+ *
+ * There are 2 contexts when this function is called:
+ *
+ * 1. DMA ISR - DMA ISR triggers the threshold complete calback, which calls the
+ * dequue API which in-turn calls this callback. UART lock is taken during
+ * the call to the threshold callback.
+ *
+ * 2. UART ISR - UART calls the dequue API which in-turn will call this API.
+ * In this case, UART ISR takes the UART lock.
+ */
+static void tegra_rx_dma_complete_callback(struct tegra_dma_req *req)
+{
+	struct tegra_uart_port *t = req->dev;
+	struct uart_port *u = &t->uport;
+	struct tty_struct *tty = u->state->port.tty;
+	int copied;
+
+	/* If we are here, DMA is stopped */
+
+	dev_dbg(t->uport.dev, "%s: %d %d\n", __func__, req->bytes_transferred,
+		req->status);
+	if (req->bytes_transferred) {
+		t->uport.icount.rx += req->bytes_transferred;
+		dma_sync_single_for_cpu(t->uport.dev, req->dest_addr,
+				req->size, DMA_FROM_DEVICE);
+		copied = tty_insert_flip_string(tty,
+			((unsigned char *)(req->virt_addr)),
+			req->bytes_transferred);
+		if (copied != req->bytes_transferred) {
+			WARN_ON(1);
+			dev_err(t->uport.dev, "Not able to copy uart data "
+				"to tty layer Req %d and coped %d\n",
+				req->bytes_transferred, copied);
+		}
+		dma_sync_single_for_device(t->uport.dev, req->dest_addr,
+				req->size, DMA_TO_DEVICE);
+	}
+
+	do_handle_rx_pio(t);
+
+	/* Push the read data later in caller place. */
+	if (req->status == -TEGRA_DMA_REQ_ERROR_ABORTED)
+		return;
+
+	spin_unlock(&u->lock);
+	tty_flip_buffer_push(u->state->port.tty);
+	spin_lock(&u->lock);
+}
+
+/* Lock already taken */
+static void do_handle_rx_dma(struct tegra_uart_port *t)
+{
+	struct uart_port *u = &t->uport;
+	if (t->rts_active)
+		set_rts(t, false);
+	tegra_dma_dequeue_req(t->rx_dma, &t->rx_dma_req);
+	tty_flip_buffer_push(u->state->port.tty);
+	/* enqueue the request again */
+	tegra_start_dma_rx(t);
+	if (t->rts_active)
+		set_rts(t, true);
+}
+
+/* Wait for a symbol-time. */
+static void wait_sym_time(struct tegra_uart_port *t, unsigned int syms)
+{
+
+	/* Definitely have a start bit. */
+	unsigned int bits = 1;
+	switch (t->lcr_shadow & 3) {
+	case UART_LCR_WLEN5:
+		bits += 5;
+		break;
+	case UART_LCR_WLEN6:
+		bits += 6;
+		break;
+	case UART_LCR_WLEN7:
+		bits += 7;
+		break;
+	default:
+		bits += 8;
+		break;
+	}
+
+	/* Technically 5 bits gets 1.5 bits of stop... */
+	if (t->lcr_shadow & UART_LCR_STOP)
+		bits += 2;
+	else
+		bits++;
+
+	if (t->lcr_shadow & UART_LCR_PARITY)
+		bits++;
+
+	if (likely(t->baud))
+		udelay(DIV_ROUND_UP(syms * bits * 1000000, t->baud));
+}
+
+/* Flush desired FIFO. */
+static void tegra_fifo_reset(struct tegra_uart_port *t, u8 fcr_bits)
+{
+	unsigned char fcr = t->fcr_shadow;
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	fcr |= fcr_bits & (UART_FCR_CLEAR_RCVR | UART_FCR_CLEAR_XMIT);
+	uart_writeb(t, fcr, UART_FCR);
+#else
+	/*Hw issue: Resetting tx fifo with non-fifo
+	mode to avoid any extra character to be sent*/
+	fcr &= ~UART_FCR_ENABLE_FIFO;
+	uart_writeb(t, fcr, UART_FCR);
+	udelay(60);
+	fcr |= fcr_bits & (UART_FCR_CLEAR_RCVR | UART_FCR_CLEAR_XMIT);
+	uart_writeb(t, fcr, UART_FCR);
+	fcr |= UART_FCR_ENABLE_FIFO;
+	uart_writeb(t, fcr, UART_FCR);
+#endif
+	uart_readb(t, UART_SCR); /* Dummy read to ensure the write is posted */
+	wait_sym_time(t, 1); /* Wait for the flush to propagate. */
+}
+
+static char do_decode_rx_error(struct tegra_uart_port *t, u8 lsr)
+{
+	char flag = TTY_NORMAL;
+
+	if (unlikely(lsr & UART_LSR_ANY)) {
+		if (lsr & UART_LSR_OE) {
+			/* Overrrun error  */
+			flag |= TTY_OVERRUN;
+			t->uport.icount.overrun++;
+			dev_err(t->uport.dev, "Got overrun errors\n");
+		} else if (lsr & UART_LSR_PE) {
+			/* Parity error */
+			flag |= TTY_PARITY;
+			t->uport.icount.parity++;
+			dev_err(t->uport.dev, "Got Parity errors\n");
+		} else if (lsr & UART_LSR_FE) {
+			flag |= TTY_FRAME;
+			t->uport.icount.frame++;
+			dev_err(t->uport.dev, "Got frame errors\n");
+		} else if (lsr & UART_LSR_BI) {
+			dev_err(t->uport.dev, "Got Break\n");
+			t->uport.icount.brk++;
+			/* If FIFO read error without any data, reset Rx FIFO */
+			if (!(lsr & UART_LSR_DR) && (lsr & UART_LSR_FIFOE))
+				tegra_fifo_reset(t, UART_FCR_CLEAR_RCVR);
+		}
+	}
+	return flag;
+}
+
+static void do_handle_rx_pio(struct tegra_uart_port *t)
+{
+	int count = 0;
+	do {
+		char flag = TTY_NORMAL;
+		unsigned char lsr = 0;
+		unsigned char ch;
+
+
+		lsr = uart_readb(t, UART_LSR);
+		if (!(lsr & UART_LSR_DR))
+			break;
+
+		flag =  do_decode_rx_error(t, lsr);
+		ch = uart_readb(t, UART_RX);
+		t->uport.icount.rx++;
+		count++;
+
+		if (!uart_handle_sysrq_char(&t->uport, c))
+			uart_insert_char(&t->uport, lsr, UART_LSR_OE, ch, flag);
+	} while (1);
+
+	dev_dbg(t->uport.dev, "PIO received %d bytes\n", count);
+	return;
+}
+
+static void do_handle_modem_signal(struct uart_port *u)
+{
+	unsigned char msr;
+	struct tegra_uart_port *t;
+
+	t = container_of(u, struct tegra_uart_port, uport);
+	msr = uart_readb(t, UART_MSR);
+	if (msr & UART_MSR_CTS)
+		dev_dbg(u->dev, "CTS triggered\n");
+	if (msr & UART_MSR_DSR)
+		dev_dbg(u->dev, "DSR enabled\n");
+	if (msr & UART_MSR_DCD)
+		dev_dbg(u->dev, "CD enabled\n");
+	if (msr & UART_MSR_RI)
+		dev_dbg(u->dev, "RI enabled\n");
+	return;
+}
+
+static void do_handle_tx_pio(struct tegra_uart_port *t)
+{
+	struct circ_buf *xmit = &t->uport.state->xmit;
+
+	fill_tx_fifo(t, t->tx_bytes);
+
+	t->tx_in_progress = 0;
+
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(&t->uport);
+
+	tegra_start_next_tx(t);
+	return;
+}
+
+static void tegra_tx_dma_complete_callback(struct tegra_dma_req *req)
+{
+	struct tegra_uart_port *t = req->dev;
+	struct circ_buf *xmit = &t->uport.state->xmit;
+	int count = req->bytes_transferred;
+	unsigned long flags;
+
+	dev_vdbg(t->uport.dev, "%s: %d\n", __func__, count);
+
+	/* Update xmit pointers without lock if dma aborted. */
+	if (req->status == -TEGRA_DMA_REQ_ERROR_ABORTED) {
+		xmit->tail = (xmit->tail + count) & (UART_XMIT_SIZE - 1);
+		t->tx_in_progress = 0;
+		return;
+	}
+
+	spin_lock_irqsave(&t->uport.lock, flags);
+	xmit->tail = (xmit->tail + count) & (UART_XMIT_SIZE - 1);
+	t->tx_in_progress = 0;
+
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(&t->uport);
+
+	tegra_start_next_tx(t);
+
+	spin_unlock_irqrestore(&t->uport.lock, flags);
+}
+
+static irqreturn_t tegra_uart_isr(int irq, void *data)
+{
+	struct tegra_uart_port *t = data;
+	struct uart_port *u = &t->uport;
+	unsigned char iir;
+	unsigned char ier;
+	bool is_rx_int = false;
+	unsigned long flags;
+
+	spin_lock_irqsave(&u->lock, flags);
+	t  = container_of(u, struct tegra_uart_port, uport);
+	while (1) {
+		iir = uart_readb(t, UART_IIR);
+		if (iir & UART_IIR_NO_INT) {
+			if (likely(t->use_rx_dma) && is_rx_int) {
+				do_handle_rx_dma(t);
+
+				if (t->rx_in_progress) {
+					ier = t->ier_shadow;
+					ier |= (UART_IER_RLSI | UART_IER_RTOIE |
+								UART_IER_EORD);
+					t->ier_shadow = ier;
+					uart_writeb(t, ier, UART_IER);
+				}
+			}
+			spin_unlock_irqrestore(&u->lock, flags);
+			return IRQ_HANDLED;
+		}
+
+		dev_dbg(u->dev, "tegra_uart_isr iir = 0x%x (%d)\n", iir,
+			(iir >> 1) & 0x7);
+		switch ((iir >> 1) & 0x7) {
+		case 0: /* Modem signal change interrupt */
+			do_handle_modem_signal(u);
+			break;
+		case 1: /* Transmit interrupt only triggered when using PIO */
+			t->ier_shadow &= ~UART_IER_THRI;
+			uart_writeb(t, t->ier_shadow, UART_IER);
+			do_handle_tx_pio(t);
+			break;
+		case 4: /* End of data */
+		case 6: /* Rx timeout */
+		case 2: /* Receive */
+			if (likely(t->use_rx_dma)) {
+				if (!is_rx_int) {
+					is_rx_int = true;
+					/* Disable interrups */
+					ier = t->ier_shadow;
+					ier |= UART_IER_RDI;
+					uart_writeb(t, ier, UART_IER);
+					ier &= ~(UART_IER_RDI | UART_IER_RLSI |
+						UART_IER_RTOIE | UART_IER_EORD);
+					t->ier_shadow = ier;
+					uart_writeb(t, ier, UART_IER);
+				}
+			} else {
+				do_handle_rx_pio(t);
+
+				spin_unlock_irqrestore(&u->lock, flags);
+				tty_flip_buffer_push(u->state->port.tty);
+				spin_lock_irqsave(&u->lock, flags);
+			}
+			break;
+		case 3: /* Receive error */
+			/* FIXME how to handle this? Why do we get here */
+			do_decode_rx_error(t, uart_readb(t, UART_LSR));
+			break;
+		case 5: /* break nothing to handle */
+		case 7: /* break nothing to handle */
+			break;
+		}
+	}
+}
+
+static void tegra_start_rx(struct uart_port *u)
+{
+	struct tegra_uart_port *t;
+	unsigned char ier;
+
+	t = container_of(u, struct tegra_uart_port, uport);
+
+	if (t->rts_active)
+		set_rts(t, true);
+
+	if (!t->rx_in_progress) {
+		wait_sym_time(t, 1); /* wait a character interval */
+
+		/* Clear the received Bytes from FIFO */
+		tegra_fifo_reset(t, UART_FCR_CLEAR_RCVR);
+		uart_readb(t, UART_LSR);
+		ier = 0;
+		ier |= (UART_IER_RLSI | UART_IER_RTOIE);
+		if (t->use_rx_dma)
+			ier |= UART_IER_EORD;
+		else
+			ier |= UART_IER_RDI;
+		t->ier_shadow |= ier;
+		uart_writeb(t, t->ier_shadow, UART_IER);
+
+		t->rx_in_progress = 1;
+
+		if (t->use_rx_dma && t->rx_dma)
+			tegra_dma_enqueue_req(t->rx_dma, &t->rx_dma_req);
+
+		tty_flip_buffer_push(u->state->port.tty);
+	}
+
+	return;
+}
+
+static void tegra_stop_rx(struct uart_port *u)
+{
+	struct tegra_uart_port *t;
+	unsigned char ier;
+
+	t = container_of(u, struct tegra_uart_port, uport);
+
+	if (t->rts_active)
+		set_rts(t, false);
+
+	if (t->rx_in_progress) {
+		wait_sym_time(t, 1); /* wait a character interval */
+
+		ier = t->ier_shadow;
+		ier &= ~(UART_IER_RDI | UART_IER_RLSI | UART_IER_RTOIE |
+					UART_IER_EORD);
+		t->ier_shadow = ier;
+		uart_writeb(t, ier, UART_IER);
+		t->rx_in_progress = 0;
+
+		if (t->use_rx_dma && t->rx_dma)
+			tegra_dma_dequeue_req(t->rx_dma, &t->rx_dma_req);
+		else
+			do_handle_rx_pio(t);
+
+		tty_flip_buffer_push(u->state->port.tty);
+	}
+
+	return;
+}
+
+static void tegra_uart_hw_deinit(struct tegra_uart_port *t)
+{
+	unsigned long flags;
+	unsigned long char_time = DIV_ROUND_UP(10000000, t->baud);
+	unsigned long fifo_empty_time = t->uport.fifosize * char_time;
+	unsigned long wait_time;
+	unsigned char lsr;
+	unsigned char msr;
+	unsigned char mcr;
+
+	/* Disable interrupts */
+	uart_writeb(t, 0, UART_IER);
+
+	lsr = uart_readb(t, UART_LSR);
+	if ((lsr & UART_LSR_TEMT) != UART_LSR_TEMT) {
+		msr = uart_readb(t, UART_MSR);
+		mcr = uart_readb(t, UART_MCR);
+		if ((mcr & UART_MCR_CTS_EN) && (msr & UART_MSR_CTS))
+			dev_err(t->uport.dev, "%s: Tx fifo not empty and "
+				"slave disabled CTS, Waiting for slave to"
+				" be ready\n", __func__);
+
+		/* Wait for Tx fifo to be empty */
+		while ((lsr & UART_LSR_TEMT) != UART_LSR_TEMT) {
+			wait_time = min(fifo_empty_time, 100lu);
+			udelay(wait_time);
+			fifo_empty_time -= wait_time;
+			if (!fifo_empty_time) {
+				msr = uart_readb(t, UART_MSR);
+				mcr = uart_readb(t, UART_MCR);
+				if ((mcr & UART_MCR_CTS_EN) &&
+					(msr & UART_MSR_CTS))
+					dev_err(t->uport.dev, "%s: Slave is "
+					"still not ready!\n", __func__);
+				break;
+			}
+			lsr = uart_readb(t, UART_LSR);
+		}
+	}
+
+	spin_lock_irqsave(&t->uport.lock, flags);
+
+	/* Reset the Rx and Tx FIFOs */
+	tegra_fifo_reset(t, UART_FCR_CLEAR_XMIT | UART_FCR_CLEAR_RCVR);
+
+	t->baud = 0;
+	t->uart_state = TEGRA_UART_CLOSED;
+
+	spin_unlock_irqrestore(&t->uport.lock, flags);
+
+	clk_disable(t->clk);
+}
+
+static void tegra_uart_free_rx_dma_buffer(struct tegra_uart_port *t)
+{
+	if (likely(t->rx_dma_req.dest_addr))
+		dma_free_coherent(t->uport.dev, t->rx_dma_req.size,
+			t->rx_dma_req.virt_addr, t->rx_dma_req.dest_addr);
+	t->rx_dma_req.dest_addr = 0;
+	t->rx_dma_req.virt_addr = NULL;
+}
+
+static void tegra_uart_free_rx_dma(struct tegra_uart_port *t)
+{
+	if (!t->use_rx_dma)
+		return;
+
+	tegra_dma_free_channel(t->rx_dma);
+	t->rx_dma = NULL;
+	t->use_rx_dma = false;
+}
+
+static int tegra_uart_hw_init(struct tegra_uart_port *t)
+{
+	unsigned char ier;
+	unsigned char sir;
+
+	dev_vdbg(t->uport.dev, "+tegra_uart_hw_init\n");
+
+	t->fcr_shadow = 0;
+	t->mcr_shadow = 0;
+	t->lcr_shadow = 0;
+	t->ier_shadow = 0;
+	t->baud = 0;
+
+	clk_enable(t->clk);
+
+	/* Reset the UART controller to clear all previous status.*/
+	tegra_periph_reset_assert(t->clk);
+	udelay(100);
+	tegra_periph_reset_deassert(t->clk);
+	udelay(100);
+
+	t->rx_in_progress = 0;
+
+	/*
+	 * Set the trigger level
+	 *
+	 * For PIO mode:
+	 *
+	 * For receive, this will interrupt the CPU after that many number of
+	 * bytes are received, for the remaining bytes the receive timeout
+	 * interrupt is received.
+	 *
+	 *  Rx high watermark is set to 4.
+	 *
+	 * For transmit, if the trasnmit interrupt is enabled, this will
+	 * interrupt the CPU when the number of entries in the FIFO reaches the
+	 * low watermark.
+	 *
+	 *  Tx low watermark is set to 8.
+	 *
+	 *  For DMA mode:
+	 *
+	 *  Set the Tx trigger to 4. This should match the DMA burst size that
+	 *  programmed in the DMA registers.
+	 */
+	t->fcr_shadow = UART_FCR_ENABLE_FIFO;
+	t->fcr_shadow |= UART_FCR_R_TRIG_01;
+	t->fcr_shadow |= TEGRA_UART_TX_TRIG_8B;
+	uart_writeb(t, t->fcr_shadow, UART_FCR);
+
+	if (t->use_rx_dma) {
+		/*
+		 * Initialize the UART for a simple default configuration
+		 * so that the receive DMA buffer may be enqueued */
+		t->lcr_shadow = 3;  /* no parity, stop, 8 data bits */
+		tegra_set_baudrate(t, 9600);
+		t->fcr_shadow |= UART_FCR_DMA_SELECT;
+		uart_writeb(t, t->fcr_shadow, UART_FCR);
+		if (tegra_start_dma_rx(t)) {
+			dev_err(t->uport.dev, "Rx DMA enqueue failed\n");
+			tegra_uart_free_rx_dma(t);
+			t->fcr_shadow &= ~UART_FCR_DMA_SELECT;
+			uart_writeb(t, t->fcr_shadow, UART_FCR);
+		}
+	} else {
+		uart_writeb(t, t->fcr_shadow, UART_FCR);
+	}
+
+	t->rx_in_progress = 1;
+
+	/*
+	 *  Enable IE_RXS for the receive status interrupts like line errros.
+	 *  Enable IE_RX_TIMEOUT to get the bytes which cannot be DMA'd.
+	 *
+	 *  If using DMA mode, enable EORD instead of receive interrupt which
+	 *  will interrupt after the UART is done with the receive instead of
+	 *  the interrupt when the FIFO "threshold" is reached.
+	 *
+	 *  EORD is different interrupt than RX_TIMEOUT - RX_TIMEOUT occurs when
+	 *  the DATA is sitting in the FIFO and couldn't be transferred to the
+	 *  DMA as the DMA size alignment(4 bytes) is not met. EORD will be
+	 *  triggered when there is a pause of the incomming data stream for 4
+	 *  characters long.
+	 *
+	 *  For pauses in the data which is not aligned to 4 bytes, we get
+	 *  both the EORD as well as RX_TIMEOUT - SW sees RX_TIMEOUT first
+	 *  then the EORD.
+	 *
+	 *  Don't get confused, believe in the magic of nvidia hw...:-)
+	 */
+	ier = 0;
+	ier |= UART_IER_RLSI | UART_IER_RTOIE;
+	if (t->use_rx_dma)
+		ier |= UART_IER_EORD;
+	else
+		ier |= UART_IER_RDI;
+	t->ier_shadow = ier;
+	uart_writeb(t, ier, UART_IER);
+
+	/*
+	 * Tegra UART controller also support SIR PHY
+	 * Enabled IRDA will transmit each zero bit as a short IR pulse.
+	 */
+	if (t->is_irda) {
+		dev_vdbg(t->uport.dev, "SIR enabled\n");
+		sir = TEGRA_UART_SIR_ENABLED;
+		uart_writeb(t, sir, TEGRA_UART_IRDA_CSR);
+	}
+
+	t->uart_state = TEGRA_UART_OPENED;
+	dev_vdbg(t->uport.dev, "-tegra_uart_hw_init\n");
+	return 0;
+}
+
+static int tegra_uart_init_rx_dma_buffer(struct tegra_uart_port *t)
+{
+	dma_addr_t rx_dma_phys;
+	void *rx_dma_virt;
+
+	t->rx_dma_req.size = UART_RX_DMA_BUFFER_SIZE;
+	rx_dma_virt = dma_alloc_coherent(t->uport.dev,
+		t->rx_dma_req.size, &rx_dma_phys, GFP_KERNEL);
+	if (!rx_dma_virt) {
+		dev_err(t->uport.dev, "DMA buffers allocate failed\n");
+		return -ENOMEM;
+	}
+	t->rx_dma_req.dest_addr = rx_dma_phys;
+	t->rx_dma_req.virt_addr = rx_dma_virt;
+
+	t->rx_dma_req.source_addr = (unsigned long)t->uport.mapbase;
+	t->rx_dma_req.source_wrap = 4;
+	t->rx_dma_req.dest_wrap = 0;
+	t->rx_dma_req.to_memory = 1;
+	t->rx_dma_req.source_bus_width = 8;
+	t->rx_dma_req.dest_bus_width = 32;
+	t->rx_dma_req.req_sel = dma_req_sel[t->uport.line];
+	t->rx_dma_req.complete = tegra_rx_dma_complete_callback;
+	t->rx_dma_req.threshold = tegra_rx_dma_threshold_callback;
+	t->rx_dma_req.dev = t;
+
+	return 0;
+}
+
+static int tegra_uart_init_rx_dma(struct tegra_uart_port *t)
+{
+	t->rx_dma = tegra_dma_allocate_channel(TEGRA_DMA_MODE_CONTINUOUS,
+					"uart_rx_%d", t->uport.line);
+	if (!t->rx_dma) {
+		dev_err(t->uport.dev, "%s: failed to allocate RX DMA.\n",
+				__func__);
+		return -ENODEV;
+	}
+	return 0;
+}
+
+static int tegra_startup(struct uart_port *u)
+{
+	struct tegra_uart_port *t = container_of(u,
+		struct tegra_uart_port, uport);
+	int ret = 0;
+	struct tegra_uart_platform_data *pdata;
+
+	t = container_of(u, struct tegra_uart_port, uport);
+	sprintf(t->port_name, "tegra_uart_%d", u->line);
+
+	t->use_tx_dma = false;
+	if (!TX_FORCE_PIO) {
+		t->tx_dma = tegra_dma_allocate_channel(TEGRA_DMA_MODE_ONESHOT,
+					"uart_tx_%d", u->line);
+		if (t->tx_dma)
+			t->use_tx_dma = true;
+		else
+			pr_err("%s: failed to allocate TX DMA.\n", __func__);
+	}
+	if (t->use_tx_dma) {
+		t->tx_dma_req.instance = u->line;
+		t->tx_dma_req.complete = tegra_tx_dma_complete_callback;
+		t->tx_dma_req.to_memory = 0;
+
+		t->tx_dma_req.dest_addr = (unsigned long)t->uport.mapbase;
+		t->tx_dma_req.dest_wrap = 4;
+		t->tx_dma_req.source_wrap = 0;
+		t->tx_dma_req.source_bus_width = 32;
+		t->tx_dma_req.dest_bus_width = 8;
+		t->tx_dma_req.req_sel = dma_req_sel[t->uport.line];
+		t->tx_dma_req.dev = t;
+		t->tx_dma_req.size = 0;
+		t->xmit_dma_addr = dma_map_single(t->uport.dev,
+			t->uport.state->xmit.buf, UART_XMIT_SIZE,
+			DMA_TO_DEVICE);
+	}
+	t->tx_in_progress = 0;
+
+	t->use_rx_dma = false;
+	if (!RX_FORCE_PIO && t->rx_dma_req.virt_addr) {
+		if (!tegra_uart_init_rx_dma(t))
+			t->use_rx_dma = true;
+	}
+
+	ret = tegra_uart_hw_init(t);
+	if (ret)
+		goto fail;
+
+	if (t->is_irda && t->irda_start)
+		t->irda_start();
+
+	pdata = u->dev->platform_data;
+	if (pdata && pdata->is_loopback)
+		t->mcr_shadow |= UART_MCR_LOOP;
+
+	dev_dbg(u->dev, "Requesting IRQ %d\n", u->irq);
+	ret = request_irq(u->irq, tegra_uart_isr, IRQF_DISABLED,
+				t->port_name, t);
+	if (ret) {
+		dev_err(u->dev, "Failed to register ISR for IRQ %d\n", u->irq);
+		goto fail;
+	}
+
+	dev_dbg(u->dev, "Started UART port %d\n", u->line);
+	return 0;
+fail:
+	dev_err(u->dev, "Tegra UART startup failed\n");
+	return ret;
+}
+
+static void tegra_shutdown(struct uart_port *u)
+{
+	struct tegra_uart_port *t;
+
+	t = container_of(u, struct tegra_uart_port, uport);
+	dev_vdbg(u->dev, "+tegra_shutdown\n");
+
+	if (t->is_irda && t->irda_shutdown)
+		t->irda_shutdown();
+
+	tegra_uart_hw_deinit(t);
+
+	t->rx_in_progress = 0;
+	t->tx_in_progress = 0;
+
+	tegra_uart_free_rx_dma(t);
+	if (t->use_tx_dma) {
+		tegra_dma_free_channel(t->tx_dma);
+		t->tx_dma = NULL;
+		t->use_tx_dma = false;
+		dma_unmap_single(t->uport.dev, t->xmit_dma_addr, UART_XMIT_SIZE,
+				DMA_TO_DEVICE);
+		t->xmit_dma_addr = 0;
+	}
+
+	free_irq(u->irq, t);
+	dev_vdbg(u->dev, "-tegra_shutdown\n");
+}
+
+static void tegra_wake_peer(struct uart_port *u)
+{
+	struct tegra_uart_platform_data *pdata = u->dev->platform_data;
+
+	if (pdata && pdata->wake_peer)
+		pdata->wake_peer(u);
+}
+
+static unsigned int tegra_get_mctrl(struct uart_port *u)
+{
+	/*
+	 * RI - Ring detector is active
+	 * CD/DCD/CAR - Carrier detect is always active. For some reason
+	 *			  linux has different names for carrier detect.
+	 * DSR - Data Set ready is active as the hardware doesn't support it.
+	 *	   Don't know if the linux support this yet?
+	 * CTS - Clear to send. Always set to active, as the hardware handles
+	 *	   CTS automatically.
+	 */
+	return TIOCM_RI | TIOCM_CD | TIOCM_DSR | TIOCM_CTS;
+}
+
+static void set_rts(struct tegra_uart_port *t, bool active)
+{
+	unsigned char mcr;
+	mcr = t->mcr_shadow;
+	if (active)
+		mcr |= UART_MCR_RTS_EN;
+	else
+		mcr &= ~UART_MCR_RTS_EN;
+	if (mcr != t->mcr_shadow) {
+		uart_writeb(t, mcr, UART_MCR);
+		t->mcr_shadow = mcr;
+	}
+	return;
+}
+
+static void set_dtr(struct tegra_uart_port *t, bool active)
+{
+	unsigned char mcr;
+	mcr = t->mcr_shadow;
+	if (active)
+		mcr |= UART_MCR_DTR;
+	else
+		mcr &= ~UART_MCR_DTR;
+	if (mcr != t->mcr_shadow) {
+		uart_writeb(t, mcr, UART_MCR);
+		t->mcr_shadow = mcr;
+	}
+	return;
+}
+
+static void tegra_set_mctrl(struct uart_port *u, unsigned int mctrl)
+{
+	unsigned char mcr;
+	struct tegra_uart_port *t;
+
+	dev_dbg(u->dev, "tegra_set_mctrl called with %d\n", mctrl);
+	t = container_of(u, struct tegra_uart_port, uport);
+
+	mcr = t->mcr_shadow;
+	if (mctrl & TIOCM_RTS) {
+		t->rts_active = true;
+		set_rts(t, true);
+	} else {
+		t->rts_active = false;
+		set_rts(t, false);
+	}
+
+	if (mctrl & TIOCM_DTR)
+		set_dtr(t, true);
+	else
+		set_dtr(t, false);
+	return;
+}
+
+static void tegra_break_ctl(struct uart_port *u, int break_ctl)
+{
+	struct tegra_uart_port *t;
+	unsigned char lcr;
+
+	t = container_of(u, struct tegra_uart_port, uport);
+	lcr = t->lcr_shadow;
+	if (break_ctl)
+		lcr |= UART_LCR_SBC;
+	else
+		lcr &= ~UART_LCR_SBC;
+	uart_writeb(t, lcr, UART_LCR);
+	t->lcr_shadow = lcr;
+}
+
+static int tegra_request_port(struct uart_port *u)
+{
+	return 0;
+}
+
+static void tegra_release_port(struct uart_port *u)
+{
+	/* Nothing to do here */
+}
+
+static unsigned int tegra_tx_empty(struct uart_port *u)
+{
+	struct tegra_uart_port *t;
+	unsigned int ret = 0;
+	unsigned long flags;
+	unsigned char lsr;
+
+	t = container_of(u, struct tegra_uart_port, uport);
+	dev_vdbg(u->dev, "+tegra_tx_empty\n");
+
+	spin_lock_irqsave(&u->lock, flags);
+	if (!t->tx_in_progress) {
+		lsr = uart_readb(t, UART_LSR);
+		if ((lsr & TX_EMPTY_STATUS) == TX_EMPTY_STATUS)
+			ret = TIOCSER_TEMT;
+	}
+	spin_unlock_irqrestore(&u->lock, flags);
+
+	dev_vdbg(u->dev, "-tegra_tx_empty\n");
+	return ret;
+}
+
+static void tegra_stop_tx(struct uart_port *u)
+{
+	struct tegra_uart_port *t;
+
+	t = container_of(u, struct tegra_uart_port, uport);
+
+	if (t->use_tx_dma)
+		tegra_dma_dequeue_req(t->tx_dma, &t->tx_dma_req);
+
+	return;
+}
+
+static void tegra_enable_ms(struct uart_port *u)
+{
+}
+
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+static int clk_div71_get_divider(unsigned long parent_rate,
+		unsigned long rate)
+{
+	s64 divider_u71 = parent_rate;
+	if (!rate)
+		return -EINVAL;
+
+	divider_u71 *= 2;
+	divider_u71 += rate - 1;
+	do_div(divider_u71, rate);
+
+	if ((divider_u71 - 2) < 0)
+		return 0;
+
+	if ((divider_u71 - 2) > 255)
+		return -EINVAL;
+
+	return divider_u71 - 2;
+}
+#endif
+
+static int clk_div16_get_divider(unsigned long parent_rate, unsigned long rate)
+{
+	s64 divider_u16;
+
+	divider_u16 = parent_rate;
+	if (!rate)
+		return -EINVAL;
+	divider_u16 += rate - 1;
+	do_div(divider_u16, rate);
+
+	if (divider_u16 > 0xFFFF)
+		return -EINVAL;
+
+	return divider_u16;
+}
+
+static unsigned long find_best_clock_source(struct tegra_uart_port *t,
+		unsigned long rate)
+{
+	struct uart_port *u = &t->uport;
+	struct tegra_uart_platform_data *pdata;
+	int i;
+	int divider;
+	unsigned long parent_rate;
+	unsigned long new_rate;
+	unsigned long err_rate;
+	unsigned int fin_err = rate;
+	unsigned long fin_rate = rate;
+	int final_index = -1;
+	int count;
+	unsigned long error_2perc;
+
+	pdata = u->dev->platform_data;
+	if (!pdata || !pdata->parent_clk_count)
+		return fin_rate;
+
+	error_2perc = (rate / 50);
+
+	for (count = 0; count < pdata->parent_clk_count; ++count) {
+		parent_rate = pdata->parent_clk_list[count].fixed_clk_rate;
+
+		if (parent_rate < rate)
+			continue;
+
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+		divider = clk_div71_get_divider(parent_rate, rate);
+
+		/* Get the best divider around calculated value */
+		if (divider > 2) {
+			for (i = divider - 2; i < (divider + 2); ++i) {
+				new_rate = ((parent_rate << 1) + i + 1) /
+								(i + 2);
+				err_rate = abs(new_rate - rate);
+				if (err_rate < fin_err) {
+					final_index = count;
+					fin_err = err_rate;
+					fin_rate = new_rate;
+					if (fin_err < error_2perc)
+						break;
+				}
+			}
+			if (fin_err < error_2perc)
+				break;
+		}
+#endif
+		/* Get the divisor by uart controller dll/dlm */
+		divider = clk_div16_get_divider(parent_rate, rate);
+
+		/* Get the best divider around calculated value */
+		if (divider > 2) {
+			for (i = divider - 2; i < (divider + 2); ++i) {
+				new_rate = parent_rate/i;
+				err_rate = abs(new_rate - rate);
+				if (err_rate < fin_err) {
+					final_index = count;
+					fin_err = err_rate;
+					fin_rate = parent_rate;
+					if (fin_err < error_2perc)
+						break;
+				}
+			}
+			if (fin_err < error_2perc)
+				break;
+		}
+	}
+
+	if (final_index >= 0) {
+		dev_info(t->uport.dev, "Setting clk_src %s\n",
+				pdata->parent_clk_list[final_index].name);
+		clk_set_parent(t->clk,
+			pdata->parent_clk_list[final_index].parent_clk);
+	}
+	return fin_rate;
+}
+
+#define UART_CLOCK_ACCURACY 5
+static void tegra_set_baudrate(struct tegra_uart_port *t, unsigned int baud)
+{
+	unsigned long rate;
+	unsigned int divisor;
+	unsigned char lcr;
+	unsigned int baud_actual;
+	unsigned int baud_delta;
+	unsigned long best_rate;
+
+	if (t->baud == baud)
+		return;
+
+	rate = baud * 16;
+	best_rate = find_best_clock_source(t, rate);
+	clk_set_rate(t->clk, best_rate);
+
+	rate = clk_get_rate(t->clk);
+
+	divisor = rate;
+	do_div(divisor, 16);
+	divisor += baud/2;
+	do_div(divisor, baud);
+
+	/* The allowable baudrate error from desired baudrate is 5% */
+	baud_actual = divisor ? rate / (16 * divisor) : 0;
+	baud_delta = abs(baud_actual - baud);
+	if (WARN_ON(baud_delta * 20 > baud)) {
+		dev_err(t->uport.dev, "requested baud %u, actual %u\n",
+				baud, baud_actual);
+	}
+
+	lcr = t->lcr_shadow;
+	lcr |= UART_LCR_DLAB;
+	uart_writeb(t, lcr, UART_LCR);
+
+	uart_writel(t, divisor & 0xFF, UART_TX);
+	uart_writel(t, ((divisor >> 8) & 0xFF), UART_IER);
+
+	lcr &= ~UART_LCR_DLAB;
+	uart_writeb(t, lcr, UART_LCR);
+	uart_readb(t, UART_SCR); /* Dummy read to ensure the write is posted */
+
+	t->baud = baud;
+	wait_sym_time(t, 2); /* wait two character intervals at new rate */
+	dev_dbg(t->uport.dev, "Baud %u clock freq %lu and divisor of %u\n",
+		baud, rate, divisor);
+}
+
+static void tegra_set_termios(struct uart_port *u, struct ktermios *termios,
+					   struct ktermios *oldtermios)
+{
+	struct tegra_uart_port *t;
+	unsigned int baud;
+	unsigned long flags;
+	unsigned int lcr;
+	unsigned int c_cflag = termios->c_cflag;
+	unsigned char mcr;
+
+	t = container_of(u, struct tegra_uart_port, uport);
+	dev_vdbg(t->uport.dev, "+tegra_set_termios\n");
+
+	spin_lock_irqsave(&u->lock, flags);
+
+	/* Changing configuration, it is safe to stop any rx now */
+	if (t->rts_active)
+		set_rts(t, false);
+
+	/* Clear all interrupts as configuration is going to be change */
+	uart_writeb(t, t->ier_shadow | UART_IER_RDI, UART_IER);
+	uart_readb(t, UART_IER);
+	uart_writeb(t, 0, UART_IER);
+	uart_readb(t, UART_IER);
+
+	/* Parity */
+	lcr = t->lcr_shadow;
+	lcr &= ~UART_LCR_PARITY;
+	if (PARENB == (c_cflag & PARENB)) {
+		if (CMSPAR == (c_cflag & CMSPAR)) {
+			/* FIXME What is space parity? */
+			/* data |= SPACE_PARITY; */
+		} else if (c_cflag & PARODD) {
+			lcr |= UART_LCR_PARITY;
+			lcr &= ~UART_LCR_EPAR;
+			lcr &= ~UART_LCR_SPAR;
+		} else {
+			lcr |= UART_LCR_PARITY;
+			lcr |= UART_LCR_EPAR;
+			lcr &= ~UART_LCR_SPAR;
+		}
+	}
+
+	lcr &= ~UART_LCR_WLEN8;
+	switch (c_cflag & CSIZE) {
+	case CS5:
+		lcr |= UART_LCR_WLEN5;
+		break;
+	case CS6:
+		lcr |= UART_LCR_WLEN6;
+		break;
+	case CS7:
+		lcr |= UART_LCR_WLEN7;
+		break;
+	default:
+		lcr |= UART_LCR_WLEN8;
+		break;
+	}
+
+	/* Stop bits */
+	if (termios->c_cflag & CSTOPB)
+		lcr |= UART_LCR_STOP;
+	else
+		lcr &= ~UART_LCR_STOP;
+
+	uart_writeb(t, lcr, UART_LCR);
+	t->lcr_shadow = lcr;
+
+	/* Baud rate. */
+	baud = uart_get_baud_rate(u, termios, oldtermios, 200, 4000000);
+	spin_unlock_irqrestore(&u->lock, flags);
+	tegra_set_baudrate(t, baud);
+	spin_lock_irqsave(&u->lock, flags);
+
+	/* Flow control */
+	if (termios->c_cflag & CRTSCTS)	{
+		mcr = t->mcr_shadow;
+		mcr |= UART_MCR_CTS_EN;
+		mcr &= ~UART_MCR_RTS_EN;
+		t->mcr_shadow = mcr;
+		uart_writeb(t, mcr, UART_MCR);
+		t->use_cts_control = true;
+		/* if top layer has asked to set rts active then do so here */
+		if (t->rts_active)
+			set_rts(t, true);
+	} else {
+		mcr = t->mcr_shadow;
+		mcr &= ~UART_MCR_CTS_EN;
+		mcr &= ~UART_MCR_RTS_EN;
+		t->mcr_shadow = mcr;
+		uart_writeb(t, mcr, UART_MCR);
+		t->use_cts_control = false;
+	}
+
+	/* update the port timeout based on new settings */
+	uart_update_timeout(u, termios->c_cflag, baud);
+
+	/* Make sure all write has completed */
+	uart_readb(t, UART_IER);
+
+	/* Reenable interrupt */
+	uart_writeb(t, t->ier_shadow, UART_IER);
+	uart_readb(t, UART_IER);
+
+	spin_unlock_irqrestore(&u->lock, flags);
+	dev_vdbg(t->uport.dev, "-tegra_set_termios\n");
+	return;
+}
+
+/*
+ * Flush any TX data submitted for DMA and PIO. Called when the
+ * TX circular buffer is reset.
+ */
+static void tegra_flush_buffer(struct uart_port *u)
+{
+	struct tegra_uart_port *t;
+
+	dev_vdbg(u->dev, "%s called", __func__);
+
+	t = container_of(u, struct tegra_uart_port, uport);
+
+	t->tx_bytes = 0;
+
+	if (t->use_tx_dma) {
+		tegra_dma_dequeue_req(t->tx_dma, &t->tx_dma_req);
+		t->tx_dma_req.size = 0;
+	}
+	return;
+}
+
+
+static void tegra_pm(struct uart_port *u, unsigned int state,
+	unsigned int oldstate)
+{
+
+}
+
+static const char *tegra_type(struct uart_port *u)
+{
+	return TEGRA_UART_TYPE;
+}
+
+static struct uart_ops tegra_uart_ops = {
+	.tx_empty	= tegra_tx_empty,
+	.set_mctrl	= tegra_set_mctrl,
+	.get_mctrl	= tegra_get_mctrl,
+	.stop_tx	= tegra_stop_tx,
+	.start_tx	= tegra_start_tx,
+	.stop_rx	= tegra_stop_rx,
+	.flush_buffer	= tegra_flush_buffer,
+	.enable_ms	= tegra_enable_ms,
+	.break_ctl	= tegra_break_ctl,
+	.startup	= tegra_startup,
+	.shutdown	= tegra_shutdown,
+	.wake_peer	= tegra_wake_peer,
+	.set_termios	= tegra_set_termios,
+	.pm		= tegra_pm,
+	.type		= tegra_type,
+	.request_port	= tegra_request_port,
+	.release_port	= tegra_release_port,
+};
+
+static struct uart_driver tegra_uart_driver = {
+	.owner		= THIS_MODULE,
+	.driver_name	= "tegra_uart",
+	.dev_name	= "ttyHS",
+	.cons		= 0,
+	.nr		= 5,
+};
+
+static int __init tegra_uart_probe(struct platform_device *pdev)
+{
+	struct tegra_uart_port *t;
+	struct uart_port *u;
+	struct tegra_uart_platform_data *pdata;
+	struct resource *resource;
+	int ret;
+	char name[64];
+	if (pdev->id < 0 || pdev->id > tegra_uart_driver.nr) {
+		pr_err("Invalid Uart instance (%d)\n", pdev->id);
+		return -ENODEV;
+	}
+
+	t = kzalloc(sizeof(struct tegra_uart_port), GFP_KERNEL);
+	if (!t) {
+		pr_err("%s: Failed to allocate memory\n", __func__);
+		return -ENOMEM;
+	}
+	u = &t->uport;
+	u->dev = &pdev->dev;
+	platform_set_drvdata(pdev, u);
+	u->line = pdev->id;
+	u->ops = &tegra_uart_ops;
+	u->type = PORT_TEGRA;
+	u->fifosize = 32;
+
+	pdata = u->dev->platform_data;
+	if (pdata && pdata->is_irda) {
+		dev_info(&pdev->dev, "Initialized UART %d as SIR PHY\n",
+								u->line);
+		t->is_irda = pdata->is_irda;
+		t->irda_init = pdata->irda_init;
+		t->irda_start = pdata->irda_start;
+		t->irda_mode_switch = pdata->irda_mode_switch;
+		t->irda_shutdown = pdata->irda_shutdown;
+		t->irda_remove = pdata->irda_remove;
+		if (t->irda_init)
+			t->irda_init();
+	}
+
+	resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (unlikely(!resource)) {
+		ret = -ENXIO;
+		goto fail;
+	}
+
+	u->mapbase = resource->start;
+	u->membase = IO_ADDRESS(u->mapbase);
+	if (unlikely(!u->membase)) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+	u->iotype = UPIO_MEM32;
+
+	u->irq = platform_get_irq(pdev, 0);
+	if (unlikely(u->irq < 0)) {
+		ret = -ENXIO;
+		goto fail;
+	}
+
+	u->regshift = 2;
+
+	t->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR_OR_NULL(t->clk)) {
+		dev_err(&pdev->dev, "Couldn't get the clock\n");
+		ret = -ENODEV;
+		goto fail;
+	}
+
+	ret = uart_add_one_port(&tegra_uart_driver, u);
+	if (ret) {
+		pr_err("%s: Failed(%d) to add uart port %s%d\n",
+			__func__, ret, tegra_uart_driver.dev_name, u->line);
+		goto fail;
+	}
+
+	snprintf(name, sizeof(name), "tegra_hsuart_%d", u->line);
+	pr_info("Registered UART port %s%d\n",
+		tegra_uart_driver.dev_name, u->line);
+	t->uart_state = TEGRA_UART_CLOSED;
+
+	if (!RX_FORCE_PIO) {
+		ret = tegra_uart_init_rx_dma_buffer(t);
+		if (ret < 0) {
+			pr_err("%s: Failed(%d) to allocate rx dma buffer "
+				"%s%d\n", __func__, ret,
+				tegra_uart_driver.dev_name, u->line);
+			goto rx_dma_buff_fail;
+		}
+	}
+	return ret;
+
+rx_dma_buff_fail:
+	uart_remove_one_port(&tegra_uart_driver, u);
+fail:
+	if (t->clk)
+		clk_put(t->clk);
+	platform_set_drvdata(pdev, NULL);
+	kfree(t);
+	return ret;
+}
+
+static int __devexit tegra_uart_remove(struct platform_device *pdev)
+{
+	struct tegra_uart_port *t = platform_get_drvdata(pdev);
+	struct uart_port *u;
+
+	if (pdev->id < 0 || pdev->id > tegra_uart_driver.nr)
+		pr_err("Invalid Uart instance (%d)\n", pdev->id);
+
+	if (t->is_irda && t->irda_remove)
+		t->irda_remove();
+
+	u = &t->uport;
+	uart_remove_one_port(&tegra_uart_driver, u);
+
+	tegra_uart_free_rx_dma_buffer(t);
+
+	platform_set_drvdata(pdev, NULL);
+
+	pr_info("Unregistered UART port %s%d\n",
+		tegra_uart_driver.dev_name, u->line);
+	kfree(t);
+	return 0;
+}
+
+static int tegra_uart_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct tegra_uart_port *t = platform_get_drvdata(pdev);
+	struct uart_port *u;
+
+	if (pdev->id < 0 || pdev->id > tegra_uart_driver.nr)
+		pr_err("Invalid Uart instance (%d)\n", pdev->id);
+
+	u = &t->uport;
+	dev_dbg(t->uport.dev, "tegra_uart_suspend called\n");
+
+	/* enable clock before calling suspend so that controller
+	   register can be accessible */
+	if (t->uart_state == TEGRA_UART_CLOCK_OFF) {
+		clk_enable(t->clk);
+		t->uart_state = TEGRA_UART_OPENED;
+	}
+
+	uart_suspend_port(&tegra_uart_driver, u);
+	t->uart_state = TEGRA_UART_SUSPEND;
+
+	return 0;
+}
+
+static int tegra_uart_resume(struct platform_device *pdev)
+{
+	struct tegra_uart_port *t = platform_get_drvdata(pdev);
+	struct uart_port *u;
+
+	if (pdev->id < 0 || pdev->id > tegra_uart_driver.nr)
+		pr_err("Invalid Uart instance (%d)\n", pdev->id);
+
+	u = &t->uport;
+	dev_dbg(t->uport.dev, "tegra_uart_resume called\n");
+
+	if (t->uart_state == TEGRA_UART_SUSPEND)
+		uart_resume_port(&tegra_uart_driver, u);
+	return 0;
+}
+
+/* Switch off the clock of the uart controller. */
+void tegra_uart_request_clock_off(struct uart_port *uport)
+{
+	unsigned long flags;
+	struct tegra_uart_port *t;
+	bool is_clk_disable = false;
+
+	if (IS_ERR_OR_NULL(uport))
+		BUG();
+
+	dev_vdbg(uport->dev, "tegra_uart_request_clock_off");
+
+	t = container_of(uport, struct tegra_uart_port, uport);
+	spin_lock_irqsave(&uport->lock, flags);
+	if (t->uart_state == TEGRA_UART_OPENED) {
+		is_clk_disable = true;
+		t->uart_state = TEGRA_UART_CLOCK_OFF;
+	}
+	spin_unlock_irqrestore(&uport->lock, flags);
+
+	if (is_clk_disable)
+		clk_disable(t->clk);
+
+	return;
+}
+
+/* Switch on the clock of the uart controller */
+void tegra_uart_request_clock_on(struct uart_port *uport)
+{
+	unsigned long flags;
+	struct tegra_uart_port *t;
+	bool is_clk_enable = false;
+
+	if (IS_ERR_OR_NULL(uport))
+		BUG();
+
+	t = container_of(uport, struct tegra_uart_port, uport);
+	spin_lock_irqsave(&uport->lock, flags);
+	if (t->uart_state == TEGRA_UART_CLOCK_OFF) {
+		is_clk_enable = true;
+		t->uart_state = TEGRA_UART_OPENED;
+	}
+	spin_unlock_irqrestore(&uport->lock, flags);
+
+	if (is_clk_enable)
+		clk_enable(t->clk);
+
+	return;
+}
+
+/* Set the modem control signals state of uart controller. */
+void tegra_uart_set_mctrl(struct uart_port *uport, unsigned int mctrl)
+{
+	unsigned long flags;
+	struct tegra_uart_port *t;
+
+	t = container_of(uport, struct tegra_uart_port, uport);
+	if (t->uart_state != TEGRA_UART_OPENED) {
+		dev_err(t->uport.dev, "Uart is in invalid state\n");
+		return;
+	}
+
+	spin_lock_irqsave(&uport->lock, flags);
+	if (mctrl & TIOCM_RTS) {
+		t->rts_active = true;
+		set_rts(t, true);
+	} else {
+		t->rts_active = false;
+		set_rts(t, false);
+	}
+
+	if (mctrl & TIOCM_DTR)
+		set_dtr(t, true);
+	else
+		set_dtr(t, false);
+	spin_unlock_irqrestore(&uport->lock, flags);
+	return;
+}
+
+/*
+ * Return the status of the transmit fifo whether empty or not.
+ * Return 0 if tx fifo is not empty.
+ * Return TIOCSER_TEMT if tx fifo is empty.
+ */
+int tegra_uart_is_tx_empty(struct uart_port *uport)
+{
+	return tegra_tx_empty(uport);
+}
+
+static struct platform_driver tegra_uart_platform_driver __refdata= {
+	.probe		= tegra_uart_probe,
+	.remove		= __devexit_p(tegra_uart_remove),
+	.suspend	= tegra_uart_suspend,
+	.resume		= tegra_uart_resume,
+	.driver		= {
+		.name	= "tegra_uart"
+	}
+};
+
+static int __init tegra_uart_init(void)
+{
+	int ret;
+
+	ret = uart_register_driver(&tegra_uart_driver);
+	if (unlikely(ret)) {
+		pr_err("Could not register %s driver\n",
+			tegra_uart_driver.driver_name);
+		return ret;
+	}
+
+	ret = platform_driver_register(&tegra_uart_platform_driver);
+	if (unlikely(ret)) {
+		pr_err("Could not register the UART platfrom driver\n");
+		uart_unregister_driver(&tegra_uart_driver);
+		return ret;
+	}
+
+	pr_info("Initialized tegra uart driver\n");
+	return 0;
+}
+
+static void __exit tegra_uart_exit(void)
+{
+	pr_info("Unloading tegra uart driver\n");
+	platform_driver_unregister(&tegra_uart_platform_driver);
+	uart_unregister_driver(&tegra_uart_driver);
+}
+
+module_init(tegra_uart_init);
+module_exit(tegra_uart_exit);
+MODULE_DESCRIPTION("High speed UART driver for tegra chipset");
diff --git a/drivers/tty/tty_io.c b/drivers/tty/tty_io.c
index 4f1fc81112e6..1a890e270be0 100644
--- a/drivers/tty/tty_io.c
+++ b/drivers/tty/tty_io.c
@@ -194,8 +194,7 @@ static inline struct tty_struct *file_tty(struct file *file)
 	return ((struct tty_file_private *)file->private_data)->tty;
 }
 
-/* Associate a new file with the tty structure */
-int tty_add_file(struct tty_struct *tty, struct file *file)
+int tty_alloc_file(struct file *file)
 {
 	struct tty_file_private *priv;
 
@@ -203,15 +202,36 @@ int tty_add_file(struct tty_struct *tty, struct file *file)
 	if (!priv)
 		return -ENOMEM;
 
+	file->private_data = priv;
+
+	return 0;
+}
+
+/* Associate a new file with the tty structure */
+void tty_add_file(struct tty_struct *tty, struct file *file)
+{
+	struct tty_file_private *priv = file->private_data;
+
 	priv->tty = tty;
 	priv->file = file;
-	file->private_data = priv;
 
 	spin_lock(&tty_files_lock);
 	list_add(&priv->list, &tty->tty_files);
 	spin_unlock(&tty_files_lock);
+}
 
-	return 0;
+/**
+ * tty_free_file - free file->private_data
+ *
+ * This shall be used only for fail path handling when tty_add_file was not
+ * called yet.
+ */
+void tty_free_file(struct file *file)
+{
+	struct tty_file_private *priv = file->private_data;
+
+	file->private_data = NULL;
+	kfree(priv);
 }
 
 /* Delete file from its tty */
@@ -222,8 +242,7 @@ void tty_del_file(struct file *file)
 	spin_lock(&tty_files_lock);
 	list_del(&priv->list);
 	spin_unlock(&tty_files_lock);
-	file->private_data = NULL;
-	kfree(priv);
+	tty_free_file(file);
 }
 
 
@@ -1811,6 +1830,10 @@ static int tty_open(struct inode *inode, struct file *filp)
 	nonseekable_open(inode, filp);
 
 retry_open:
+	retval = tty_alloc_file(filp);
+	if (retval)
+		return -ENOMEM;
+
 	noctty = filp->f_flags & O_NOCTTY;
 	index  = -1;
 	retval = 0;
@@ -1823,6 +1846,7 @@ retry_open:
 		if (!tty) {
 			tty_unlock();
 			mutex_unlock(&tty_mutex);
+			tty_free_file(filp);
 			return -ENXIO;
 		}
 		driver = tty_driver_kref_get(tty->driver);
@@ -1855,6 +1879,7 @@ retry_open:
 		}
 		tty_unlock();
 		mutex_unlock(&tty_mutex);
+		tty_free_file(filp);
 		return -ENODEV;
 	}
 
@@ -1862,6 +1887,7 @@ retry_open:
 	if (!driver) {
 		tty_unlock();
 		mutex_unlock(&tty_mutex);
+		tty_free_file(filp);
 		return -ENODEV;
 	}
 got_driver:
@@ -1872,6 +1898,8 @@ got_driver:
 		if (IS_ERR(tty)) {
 			tty_unlock();
 			mutex_unlock(&tty_mutex);
+			tty_driver_kref_put(driver);
+			tty_free_file(filp);
 			return PTR_ERR(tty);
 		}
 	}
@@ -1887,15 +1915,11 @@ got_driver:
 	tty_driver_kref_put(driver);
 	if (IS_ERR(tty)) {
 		tty_unlock();
+		tty_free_file(filp);
 		return PTR_ERR(tty);
 	}
 
-	retval = tty_add_file(tty, filp);
-	if (retval) {
-		tty_unlock();
-		tty_release(inode, filp);
-		return retval;
-	}
+	tty_add_file(tty, filp);
 
 	check_tty_count(tty, "tty_open");
 	if (tty->driver->type == TTY_DRIVER_TYPE_PTY &&
diff --git a/drivers/tty/tty_ldisc.c b/drivers/tty/tty_ldisc.c
index ef925d581713..a76c808afadc 100644
--- a/drivers/tty/tty_ldisc.c
+++ b/drivers/tty/tty_ldisc.c
@@ -36,6 +36,7 @@
 
 #include <linux/kmod.h>
 #include <linux/nsproxy.h>
+#include <linux/ratelimit.h>
 
 /*
  *	This guards the refcounted line discipline lists. The lock
@@ -548,15 +549,16 @@ static void tty_ldisc_flush_works(struct tty_struct *tty)
 /**
  *	tty_ldisc_wait_idle	-	wait for the ldisc to become idle
  *	@tty: tty to wait for
+ *	@timeout: for how long to wait at most
  *
  *	Wait for the line discipline to become idle. The discipline must
  *	have been halted for this to guarantee it remains idle.
  */
-static int tty_ldisc_wait_idle(struct tty_struct *tty)
+static int tty_ldisc_wait_idle(struct tty_struct *tty, long timeout)
 {
-	int ret;
+	long ret;
 	ret = wait_event_timeout(tty_ldisc_idle,
-			atomic_read(&tty->ldisc->users) == 1, 5 * HZ);
+			atomic_read(&tty->ldisc->users) == 1, timeout);
 	if (ret < 0)
 		return ret;
 	return ret > 0 ? 0 : -EBUSY;
@@ -666,7 +668,7 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 
 	tty_ldisc_flush_works(tty);
 
-	retval = tty_ldisc_wait_idle(tty);
+	retval = tty_ldisc_wait_idle(tty, 5 * HZ);
 
 	tty_lock();
 	mutex_lock(&tty->ldisc_mutex);
@@ -763,8 +765,6 @@ static int tty_ldisc_reinit(struct tty_struct *tty, int ldisc)
 	if (IS_ERR(ld))
 		return -1;
 
-	WARN_ON_ONCE(tty_ldisc_wait_idle(tty));
-
 	tty_ldisc_close(tty, tty->ldisc);
 	tty_ldisc_put(tty->ldisc);
 	tty->ldisc = NULL;
@@ -839,7 +839,7 @@ void tty_ldisc_hangup(struct tty_struct *tty)
 	tty_unlock();
 	cancel_work_sync(&tty->buf.work);
 	mutex_unlock(&tty->ldisc_mutex);
-
+retry:
 	tty_lock();
 	mutex_lock(&tty->ldisc_mutex);
 
@@ -848,6 +848,22 @@ void tty_ldisc_hangup(struct tty_struct *tty)
 	   it means auditing a lot of other paths so this is
 	   a FIXME */
 	if (tty->ldisc) {	/* Not yet closed */
+		if (atomic_read(&tty->ldisc->users) != 1) {
+			char cur_n[TASK_COMM_LEN], tty_n[64];
+			long timeout = 3 * HZ;
+			tty_unlock();
+
+			while (tty_ldisc_wait_idle(tty, timeout) == -EBUSY) {
+				timeout = MAX_SCHEDULE_TIMEOUT;
+				printk_ratelimited(KERN_WARNING
+					"%s: waiting (%s) for %s took too long, but we keep waiting...\n",
+					__func__, get_task_comm(cur_n, current),
+					tty_name(tty, tty_n));
+			}
+			mutex_unlock(&tty->ldisc_mutex);
+			goto retry;
+		}
+
 		if (reset == 0) {
 
 			if (!tty_ldisc_reinit(tty, tty->termios->c_line))
diff --git a/drivers/tty/tty_port.c b/drivers/tty/tty_port.c
index 33d37d230f8f..a4aaca0e014d 100644
--- a/drivers/tty/tty_port.c
+++ b/drivers/tty/tty_port.c
@@ -227,7 +227,6 @@ int tty_port_block_til_ready(struct tty_port *port,
 	int do_clocal = 0, retval;
 	unsigned long flags;
 	DEFINE_WAIT(wait);
-	int cd;
 
 	/* block if port is in the process of being closed */
 	if (tty_hung_up_p(filp) || port->flags & ASYNC_CLOSING) {
@@ -284,11 +283,14 @@ int tty_port_block_til_ready(struct tty_port *port,
 				retval = -ERESTARTSYS;
 			break;
 		}
-		/* Probe the carrier. For devices with no carrier detect this
-		   will always return true */
-		cd = tty_port_carrier_raised(port);
+		/*
+		 * Probe the carrier. For devices with no carrier detect
+		 * tty_port_carrier_raised will always return true.
+		 * Never ask drivers if CLOCAL is set, this causes troubles
+		 * on some hardware.
+		 */
 		if (!(port->flags & ASYNC_CLOSING) &&
-				(do_clocal || cd))
+				(do_clocal || tty_port_carrier_raised(port)))
 			break;
 		if (signal_pending(current)) {
 			retval = -ERESTARTSYS;
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index dac7676ce21b..cd16e13f61c9 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -155,7 +155,6 @@ static int acm_start_wb(struct acm *acm, struct acm_wb *wb)
 	wb->urb->transfer_dma = wb->dmah;
 	wb->urb->transfer_buffer_length = wb->len;
 	wb->urb->dev = acm->dev;
-
 	rc = usb_submit_urb(wb->urb, GFP_ATOMIC);
 	if (rc < 0) {
 		dev_err(&acm->data->dev,
@@ -171,6 +170,9 @@ static int acm_write_start(struct acm *acm, int wbn)
 	unsigned long flags;
 	struct acm_wb *wb = &acm->wb[wbn];
 	int rc;
+#ifdef CONFIG_PM
+	struct urb *res;
+#endif
 
 	spin_lock_irqsave(&acm->write_lock, flags);
 	if (!acm->dev) {
@@ -183,15 +185,35 @@ static int acm_write_start(struct acm *acm, int wbn)
 							acm->susp_count);
 	usb_autopm_get_interface_async(acm->control);
 	if (acm->susp_count) {
+#ifdef CONFIG_PM
+		acm->transmitting++;
+		wb->urb->transfer_buffer = wb->buf;
+		wb->urb->transfer_dma = wb->dmah;
+		wb->urb->transfer_buffer_length = wb->len;
+		wb->urb->dev = acm->dev;
+		usb_anchor_urb(wb->urb, &acm->deferred);
+#else
 		if (!acm->delayed_wb)
 			acm->delayed_wb = wb;
 		else
 			usb_autopm_put_interface_async(acm->control);
+#endif
 		spin_unlock_irqrestore(&acm->write_lock, flags);
 		return 0;	/* A white lie */
 	}
 	usb_mark_last_busy(acm->dev);
-
+#ifdef CONFIG_PM
+	while ((res = usb_get_from_anchor(&acm->deferred))) {
+		/* decrement ref count*/
+		usb_put_urb(res);
+		rc = usb_submit_urb(res, GFP_ATOMIC);
+		if (rc < 0) {
+			dbg("usb_submit_urb(pending request) failed: %d", rc);
+			usb_unanchor_urb(res);
+			acm_write_done(acm, res->context);
+		}
+	}
+#endif
 	rc = acm_start_wb(acm, wb);
 	spin_unlock_irqrestore(&acm->write_lock, flags);
 
@@ -394,7 +416,7 @@ static void acm_read_bulk_callback(struct urb *urb)
 	}
 	usb_mark_last_busy(acm->dev);
 
-	if (urb->status) {
+	if (urb->status && !urb->actual_length) {
 		dev_dbg(&acm->data->dev, "%s - non-zero urb status: %d\n",
 							__func__, urb->status);
 		return;
@@ -476,7 +498,7 @@ static int acm_tty_open(struct tty_struct *tty, struct file *filp)
 	if (usb_autopm_get_interface(acm->control) < 0)
 		goto early_bail;
 	else
-		acm->control->needs_remote_wakeup = 1;
+		acm->control->needs_remote_wakeup = 0;
 
 	mutex_lock(&acm->mutex);
 	if (acm->port.count++) {
@@ -485,6 +507,9 @@ static int acm_tty_open(struct tty_struct *tty, struct file *filp)
 		goto out;
 	}
 
+	if (acm_submit_read_urbs(acm, GFP_KERNEL))
+		goto bail_out;
+
 	acm->ctrlurb->dev = acm->dev;
 	if (usb_submit_urb(acm->ctrlurb, GFP_KERNEL)) {
 		dev_err(&acm->control->dev,
@@ -498,9 +523,6 @@ static int acm_tty_open(struct tty_struct *tty, struct file *filp)
 
 	usb_autopm_put_interface(acm->control);
 
-	if (acm_submit_read_urbs(acm, GFP_KERNEL))
-		goto bail_out;
-
 	set_bit(ASYNCB_INITIALIZED, &acm->port.flags);
 	rv = tty_port_block_til_ready(&acm->port, tty, filp);
 
@@ -539,7 +561,6 @@ static void acm_port_down(struct acm *acm)
 {
 	int i;
 
-	mutex_lock(&open_mutex);
 	if (acm->dev) {
 		usb_autopm_get_interface(acm->control);
 		acm_set_control(acm, acm->ctrlout = 0);
@@ -551,14 +572,23 @@ static void acm_port_down(struct acm *acm)
 		acm->control->needs_remote_wakeup = 0;
 		usb_autopm_put_interface(acm->control);
 	}
-	mutex_unlock(&open_mutex);
 }
 
 static void acm_tty_hangup(struct tty_struct *tty)
 {
-	struct acm *acm = tty->driver_data;
+	struct acm *acm;
+
+	mutex_lock(&open_mutex);
+	acm = tty->driver_data;
+
+	if (!acm)
+		goto out;
+
 	tty_port_hangup(&acm->port);
 	acm_port_down(acm);
+
+out:
+	mutex_unlock(&open_mutex);
 }
 
 static void acm_tty_close(struct tty_struct *tty, struct file *filp)
@@ -569,8 +599,9 @@ static void acm_tty_close(struct tty_struct *tty, struct file *filp)
 	   shutdown */
 	if (!acm)
 		return;
+
+	mutex_lock(&open_mutex);
 	if (tty_port_close_start(&acm->port, tty, filp) == 0) {
-		mutex_lock(&open_mutex);
 		if (!acm->dev) {
 			tty_port_tty_set(&acm->port, NULL);
 			acm_tty_unregister(acm);
@@ -582,6 +613,7 @@ static void acm_tty_close(struct tty_struct *tty, struct file *filp)
 	acm_port_down(acm);
 	tty_port_close_end(&acm->port, tty);
 	tty_port_tty_set(&acm->port, NULL);
+	mutex_unlock(&open_mutex);
 }
 
 static int acm_tty_write(struct tty_struct *tty,
@@ -863,8 +895,12 @@ static int acm_probe(struct usb_interface *intf,
 	quirks = (unsigned long)id->driver_info;
 	num_rx_buf = (quirks == SINGLE_RX_URB) ? 1 : ACM_NR;
 
+	/* not a real CDC ACM device */
+	if (quirks & NOT_REAL_ACM)
+		return -ENODEV;
+
 	/* handle quirks deadly to normal probing*/
-	if (quirks == NO_UNION_NORMAL) {
+	if (quirks & NO_UNION_NORMAL) {
 		data_interface = usb_ifnum_to_if(usb_dev, 1);
 		control_interface = usb_ifnum_to_if(usb_dev, 0);
 		goto skip_normal_probe;
@@ -1074,6 +1110,7 @@ made_compressed_probe:
 	acm->readsize = readsize;
 	acm->rx_buflimit = num_rx_buf;
 	INIT_WORK(&acm->work, acm_softint);
+	init_usb_anchor(&acm->deferred);
 	spin_lock_init(&acm->write_lock);
 	spin_lock_init(&acm->read_lock);
 	mutex_init(&acm->mutex);
@@ -1081,6 +1118,8 @@ made_compressed_probe:
 	acm->is_int_ep = usb_endpoint_xfer_int(epread);
 	if (acm->is_int_ep)
 		acm->bInterval = epread->bInterval;
+	if (quirks & NO_HANGUP_IN_RESET_RESUME)
+		acm->no_hangup_in_reset_resume = 1;
 	tty_port_init(&acm->port);
 	acm->port.ops = &acm_port_ops;
 
@@ -1181,6 +1220,8 @@ made_compressed_probe:
 		i = device_create_file(&intf->dev, &dev_attr_wCountryCodes);
 		if (i < 0) {
 			kfree(acm->country_codes);
+			acm->country_codes = NULL;
+			acm->country_code_size = 0;
 			goto skip_countries;
 		}
 
@@ -1189,6 +1230,8 @@ made_compressed_probe:
 		if (i < 0) {
 			device_remove_file(&intf->dev, &dev_attr_wCountryCodes);
 			kfree(acm->country_codes);
+			acm->country_codes = NULL;
+			acm->country_code_size = 0;
 			goto skip_countries;
 		}
 	}
@@ -1241,6 +1284,11 @@ static void stop_data_traffic(struct acm *acm)
 {
 	int i;
 
+	if (!acm) {
+		pr_err("%s: !acm\n", __func__);
+		return;
+	}
+
 	dev_dbg(&acm->control->dev, "%s\n", __func__);
 
 	usb_kill_urb(acm->ctrlurb);
@@ -1257,6 +1305,7 @@ static void acm_disconnect(struct usb_interface *intf)
 	struct acm *acm = usb_get_intfdata(intf);
 	struct usb_device *usb_dev = interface_to_usbdev(intf);
 	struct tty_struct *tty;
+	struct urb *res;
 
 	/* sibling interface is already cleaning up */
 	if (!acm)
@@ -1276,6 +1325,10 @@ static void acm_disconnect(struct usb_interface *intf)
 
 	stop_data_traffic(acm);
 
+	/* decrement ref count of anchored urbs */
+	while ((res = usb_get_from_anchor(&acm->deferred)))
+		usb_put_urb(res);
+
 	acm_write_buffers_free(acm);
 	usb_free_coherent(usb_dev, acm->ctrlsize, acm->ctrl_buffer,
 			  acm->ctrl_dma);
@@ -1305,6 +1358,11 @@ static int acm_suspend(struct usb_interface *intf, pm_message_t message)
 	struct acm *acm = usb_get_intfdata(intf);
 	int cnt;
 
+	if (!acm) {
+		pr_err("%s: !acm\n", __func__);
+		return -ENODEV;
+	}
+
 	if (message.event & PM_EVENT_AUTO) {
 		int b;
 
@@ -1339,23 +1397,52 @@ static int acm_suspend(struct usb_interface *intf, pm_message_t message)
 static int acm_resume(struct usb_interface *intf)
 {
 	struct acm *acm = usb_get_intfdata(intf);
-	struct acm_wb *wb;
 	int rv = 0;
 	int cnt;
+#ifdef CONFIG_PM
+	struct urb *res;
+#else
+	struct acm_wb *wb;
+#endif
+
+	if (!acm) {
+		pr_err("%s: !acm\n", __func__);
+		return -ENODEV;
+	}
 
 	spin_lock_irq(&acm->read_lock);
-	acm->susp_count -= 1;
-	cnt = acm->susp_count;
+	if (acm->susp_count > 0) {
+		acm->susp_count -= 1;
+		cnt = acm->susp_count;
+	} else {
+		spin_unlock_irq(&acm->read_lock);
+		return 0;
+	}
 	spin_unlock_irq(&acm->read_lock);
 
 	if (cnt)
 		return 0;
 
+
 	mutex_lock(&acm->mutex);
+
 	if (acm->port.count) {
 		rv = usb_submit_urb(acm->ctrlurb, GFP_NOIO);
-
 		spin_lock_irq(&acm->write_lock);
+#ifdef CONFIG_PM
+		while ((res = usb_get_from_anchor(&acm->deferred))) {
+			/* decrement ref count*/
+			usb_put_urb(res);
+			rv = usb_submit_urb(res, GFP_ATOMIC);
+			if (rv < 0) {
+				dbg("usb_submit_urb(pending request)"
+					" failed: %d", rv);
+				usb_unanchor_urb(res);
+				acm_write_done(acm, res->context);
+			}
+		}
+		spin_unlock_irq(&acm->write_lock);
+#else
 		if (acm->delayed_wb) {
 			wb = acm->delayed_wb;
 			acm->delayed_wb = NULL;
@@ -1364,6 +1451,7 @@ static int acm_resume(struct usb_interface *intf)
 		} else {
 			spin_unlock_irq(&acm->write_lock);
 		}
+#endif
 
 		/*
 		 * delayed error checking because we must
@@ -1385,11 +1473,17 @@ static int acm_reset_resume(struct usb_interface *intf)
 	struct acm *acm = usb_get_intfdata(intf);
 	struct tty_struct *tty;
 
+	if (!acm) {
+		pr_err("%s: !acm\n", __func__);
+		return -ENODEV;
+	}
+
 	mutex_lock(&acm->mutex);
 	if (acm->port.count) {
 		tty = tty_port_tty_get(&acm->port);
 		if (tty) {
-			tty_hangup(tty);
+			if (!acm->no_hangup_in_reset_resume)
+				tty_hangup(tty);
 			tty_kref_put(tty);
 		}
 	}
@@ -1456,6 +1550,16 @@ static const struct usb_device_id acm_ids[] = {
 	},
 	{ USB_DEVICE(0x22b8, 0x6425), /* Motorola MOTOMAGX phones */
 	},
+	/* Motorola H24 HSPA module: */
+	{ USB_DEVICE(0x22b8, 0x2d91) }, /* modem                                */
+	{ USB_DEVICE(0x22b8, 0x2d92) }, /* modem           + diagnostics        */
+	{ USB_DEVICE(0x22b8, 0x2d93) }, /* modem + AT port                      */
+	{ USB_DEVICE(0x22b8, 0x2d95) }, /* modem + AT port + diagnostics        */
+	{ USB_DEVICE(0x22b8, 0x2d96) }, /* modem                         + NMEA */
+	{ USB_DEVICE(0x22b8, 0x2d97) }, /* modem           + diagnostics + NMEA */
+	{ USB_DEVICE(0x22b8, 0x2d99) }, /* modem + AT port               + NMEA */
+	{ USB_DEVICE(0x22b8, 0x2d9a) }, /* modem + AT port + diagnostics + NMEA */
+
 	{ USB_DEVICE(0x0572, 0x1329), /* Hummingbird huc56s (Conexant) */
 	.driver_info = NO_UNION_NORMAL, /* union descriptor misplaced on
 					   data interface instead of
@@ -1469,6 +1573,9 @@ static const struct usb_device_id acm_ids[] = {
 	{ USB_DEVICE(0x1576, 0x03b1), /* Maretron USB100 */
 	.driver_info = NO_UNION_NORMAL, /* reports zero length descriptor */
 	},
+	{ USB_DEVICE(0x1519, 0x0020),
+	.driver_info = NO_UNION_NORMAL | NO_HANGUP_IN_RESET_RESUME, /* has no union descriptor */
+	},
 
 	/* Nokia S60 phones expose two ACM channels. The first is
 	 * a modem and is picked up by the standard AT-command
@@ -1534,6 +1641,9 @@ static const struct usb_device_id acm_ids[] = {
 	{ NOKIA_PCSUITE_ACM_INFO(0x03cd), }, /* Nokia C7 */
 	{ SAMSUNG_PCSUITE_ACM_INFO(0x6651), }, /* Samsung GTi8510 (INNOV8) */
 
+	/* Support for Owen devices */
+	{ USB_DEVICE(0x03eb, 0x0030), }, /* Owen SI30 */
+
 	/* NOTE: non-Nokia COMM/ACM/0xff is likely MSFT RNDIS... NOT a modem! */
 
 	/* Support Lego NXT using pbLua firmware */
@@ -1546,6 +1656,16 @@ static const struct usb_device_id acm_ids[] = {
 	.driver_info = NO_DATA_INTERFACE,
 	},
 
+	/* Exclude XMM6260 boot rom (not running modem software yet) */
+	{ USB_DEVICE(0x058b, 0x0041),
+	.driver_info = NOT_REAL_ACM,
+	},
+
+	/* Icera 450 */
+	{ USB_DEVICE(0x1983, 0x0321),
+	.driver_info = NO_HANGUP_IN_RESET_RESUME,
+	},
+
 	/* control interfaces without any protocol set */
 	{ USB_INTERFACE_INFO(USB_CLASS_COMM, USB_CDC_SUBCLASS_ACM,
 		USB_CDC_PROTO_NONE) },
diff --git a/drivers/usb/class/cdc-acm.h b/drivers/usb/class/cdc-acm.h
index ca7937f26e27..ec59fda787fe 100644
--- a/drivers/usb/class/cdc-acm.h
+++ b/drivers/usb/class/cdc-acm.h
@@ -115,8 +115,10 @@ struct acm {
 	unsigned int is_int_ep:1;			/* interrupt endpoints contrary to spec used */
 	unsigned int throttled:1;			/* actually throttled */
 	unsigned int throttle_req:1;			/* throttle requested */
+	unsigned int no_hangup_in_reset_resume:1;	/* do not call tty_hangup in acm_reset_resume */
 	u8 bInterval;
 	struct acm_wb *delayed_wb;			/* write queued for a device about to be woken */
+	struct usb_anchor	deferred;
 };
 
 #define CDC_DATA_INTERFACE_TYPE	0x0a
@@ -127,3 +129,5 @@ struct acm {
 #define NO_CAP_LINE			4
 #define NOT_A_MODEM			8
 #define NO_DATA_INTERFACE		16
+#define NOT_REAL_ACM			32
+#define NO_HANGUP_IN_RESET_RESUME	64
diff --git a/drivers/usb/class/cdc-wdm.c b/drivers/usb/class/cdc-wdm.c
index 2b9ff518b509..90581a85134a 100644
--- a/drivers/usb/class/cdc-wdm.c
+++ b/drivers/usb/class/cdc-wdm.c
@@ -57,6 +57,8 @@ MODULE_DEVICE_TABLE (usb, wdm_ids);
 
 #define WDM_MAX			16
 
+/* CDC-WMC r1.1 requires wMaxCommand to be "at least 256 decimal (0x100)" */
+#define WDM_DEFAULT_BUFSIZE	256
 
 static DEFINE_MUTEX(wdm_mutex);
 
@@ -88,7 +90,8 @@ struct wdm_device {
 	int			count;
 	dma_addr_t		shandle;
 	dma_addr_t		ihandle;
-	struct mutex		lock;
+	struct mutex		wlock;
+	struct mutex		rlock;
 	wait_queue_head_t	wait;
 	struct work_struct	rxwork;
 	int			werr;
@@ -323,7 +326,7 @@ static ssize_t wdm_write
 	}
 
 	/* concurrent writes and disconnect */
-	r = mutex_lock_interruptible(&desc->lock);
+	r = mutex_lock_interruptible(&desc->wlock);
 	rv = -ERESTARTSYS;
 	if (r) {
 		kfree(buf);
@@ -386,7 +389,7 @@ static ssize_t wdm_write
 out:
 	usb_autopm_put_interface(desc->intf);
 outnp:
-	mutex_unlock(&desc->lock);
+	mutex_unlock(&desc->wlock);
 outnl:
 	return rv < 0 ? rv : count;
 }
@@ -399,7 +402,7 @@ static ssize_t wdm_read
 	struct wdm_device *desc = file->private_data;
 
 
-	rv = mutex_lock_interruptible(&desc->lock); /*concurrent reads */
+	rv = mutex_lock_interruptible(&desc->rlock); /*concurrent reads */
 	if (rv < 0)
 		return -ERESTARTSYS;
 
@@ -467,14 +470,16 @@ retry:
 	for (i = 0; i < desc->length - cntr; i++)
 		desc->ubuf[i] = desc->ubuf[i + cntr];
 
+	spin_lock_irq(&desc->iuspin);
 	desc->length -= cntr;
+	spin_unlock_irq(&desc->iuspin);
 	/* in case we had outstanding data */
 	if (!desc->length)
 		clear_bit(WDM_READ, &desc->flags);
 	rv = cntr;
 
 err:
-	mutex_unlock(&desc->lock);
+	mutex_unlock(&desc->rlock);
 	return rv;
 }
 
@@ -540,7 +545,8 @@ static int wdm_open(struct inode *inode, struct file *file)
 	}
 	intf->needs_remote_wakeup = 1;
 
-	mutex_lock(&desc->lock);
+	/* using write lock to protect desc->count */
+	mutex_lock(&desc->wlock);
 	if (!desc->count++) {
 		desc->werr = 0;
 		desc->rerr = 0;
@@ -553,7 +559,7 @@ static int wdm_open(struct inode *inode, struct file *file)
 	} else {
 		rv = 0;
 	}
-	mutex_unlock(&desc->lock);
+	mutex_unlock(&desc->wlock);
 	usb_autopm_put_interface(desc->intf);
 out:
 	mutex_unlock(&wdm_mutex);
@@ -565,9 +571,11 @@ static int wdm_release(struct inode *inode, struct file *file)
 	struct wdm_device *desc = file->private_data;
 
 	mutex_lock(&wdm_mutex);
-	mutex_lock(&desc->lock);
+
+	/* using write lock to protect desc->count */
+	mutex_lock(&desc->wlock);
 	desc->count--;
-	mutex_unlock(&desc->lock);
+	mutex_unlock(&desc->wlock);
 
 	if (!desc->count) {
 		dev_dbg(&desc->intf->dev, "wdm_release: cleanup");
@@ -630,7 +638,7 @@ static int wdm_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	struct usb_cdc_dmm_desc *dmhd;
 	u8 *buffer = intf->altsetting->extra;
 	int buflen = intf->altsetting->extralen;
-	u16 maxcom = 0;
+	u16 maxcom = WDM_DEFAULT_BUFSIZE;
 
 	if (!buffer)
 		goto out;
@@ -665,7 +673,8 @@ next_desc:
 	desc = kzalloc(sizeof(struct wdm_device), GFP_KERNEL);
 	if (!desc)
 		goto out;
-	mutex_init(&desc->lock);
+	mutex_init(&desc->rlock);
+	mutex_init(&desc->wlock);
 	spin_lock_init(&desc->iuspin);
 	init_waitqueue_head(&desc->wait);
 	desc->wMaxCommand = maxcom;
@@ -716,7 +725,7 @@ next_desc:
 		goto err;
 
 	desc->inbuf = usb_alloc_coherent(interface_to_usbdev(intf),
-					 desc->bMaxPacketSize0,
+					 desc->wMaxCommand,
 					 GFP_KERNEL,
 					 &desc->response->transfer_dma);
 	if (!desc->inbuf)
@@ -779,11 +788,13 @@ static void wdm_disconnect(struct usb_interface *intf)
 	/* to terminate pending flushes */
 	clear_bit(WDM_IN_USE, &desc->flags);
 	spin_unlock_irqrestore(&desc->iuspin, flags);
-	mutex_lock(&desc->lock);
+	wake_up_all(&desc->wait);
+	mutex_lock(&desc->rlock);
+	mutex_lock(&desc->wlock);
 	kill_urbs(desc);
 	cancel_work_sync(&desc->rxwork);
-	mutex_unlock(&desc->lock);
-	wake_up_all(&desc->wait);
+	mutex_unlock(&desc->wlock);
+	mutex_unlock(&desc->rlock);
 	if (!desc->count)
 		cleanup(desc);
 	mutex_unlock(&wdm_mutex);
@@ -798,8 +809,10 @@ static int wdm_suspend(struct usb_interface *intf, pm_message_t message)
 	dev_dbg(&desc->intf->dev, "wdm%d_suspend\n", intf->minor);
 
 	/* if this is an autosuspend the caller does the locking */
-	if (!(message.event & PM_EVENT_AUTO))
-		mutex_lock(&desc->lock);
+	if (!(message.event & PM_EVENT_AUTO)) {
+		mutex_lock(&desc->rlock);
+		mutex_lock(&desc->wlock);
+	}
 	spin_lock_irq(&desc->iuspin);
 
 	if ((message.event & PM_EVENT_AUTO) &&
@@ -815,8 +828,10 @@ static int wdm_suspend(struct usb_interface *intf, pm_message_t message)
 		kill_urbs(desc);
 		cancel_work_sync(&desc->rxwork);
 	}
-	if (!(message.event & PM_EVENT_AUTO))
-		mutex_unlock(&desc->lock);
+	if (!(message.event & PM_EVENT_AUTO)) {
+		mutex_unlock(&desc->wlock);
+		mutex_unlock(&desc->rlock);
+	}
 
 	return rv;
 }
@@ -854,7 +869,8 @@ static int wdm_pre_reset(struct usb_interface *intf)
 {
 	struct wdm_device *desc = usb_get_intfdata(intf);
 
-	mutex_lock(&desc->lock);
+	mutex_lock(&desc->rlock);
+	mutex_lock(&desc->wlock);
 	kill_urbs(desc);
 
 	/*
@@ -876,7 +892,8 @@ static int wdm_post_reset(struct usb_interface *intf)
 	int rv;
 
 	rv = recover_from_urb_loss(desc);
-	mutex_unlock(&desc->lock);
+	mutex_unlock(&desc->wlock);
+	mutex_unlock(&desc->rlock);
 	return 0;
 }
 
diff --git a/drivers/usb/core/devio.c b/drivers/usb/core/devio.c
index 37518dfdeb98..0ca54e22d319 100644
--- a/drivers/usb/core/devio.c
+++ b/drivers/usb/core/devio.c
@@ -407,7 +407,7 @@ static void async_completed(struct urb *urb)
 		sinfo.si_errno = as->status;
 		sinfo.si_code = SI_ASYNCIO;
 		sinfo.si_addr = as->userurb;
-		pid = as->pid;
+		pid = get_pid(as->pid);
 		uid = as->uid;
 		euid = as->euid;
 		secid = as->secid;
@@ -422,9 +422,11 @@ static void async_completed(struct urb *urb)
 		cancel_bulk_urbs(ps, as->bulk_addr);
 	spin_unlock(&ps->lock);
 
-	if (signr)
+	if (signr) {
 		kill_pid_info_as_uid(sinfo.si_signo, &sinfo, pid, uid,
 				      euid, secid);
+		put_pid(pid);
+	}
 
 	wake_up(&ps->wait);
 }
@@ -607,9 +609,10 @@ static int findintfep(struct usb_device *dev, unsigned int ep)
 }
 
 static int check_ctrlrecip(struct dev_state *ps, unsigned int requesttype,
-			   unsigned int index)
+			   unsigned int request, unsigned int index)
 {
 	int ret = 0;
+	struct usb_host_interface *alt_setting;
 
 	if (ps->dev->state != USB_STATE_UNAUTHENTICATED
 	 && ps->dev->state != USB_STATE_ADDRESS
@@ -618,6 +621,19 @@ static int check_ctrlrecip(struct dev_state *ps, unsigned int requesttype,
 	if (USB_TYPE_VENDOR == (USB_TYPE_MASK & requesttype))
 		return 0;
 
+	/*
+	 * check for the special corner case 'get_device_id' in the printer
+	 * class specification, where wIndex is (interface << 8 | altsetting)
+	 * instead of just interface
+	 */
+	if (requesttype == 0xa1 && request == 0) {
+		alt_setting = usb_find_alt_setting(ps->dev->actconfig,
+						   index >> 8, index & 0xff);
+		if (alt_setting
+		 && alt_setting->desc.bInterfaceClass == USB_CLASS_PRINTER)
+			index >>= 8;
+	}
+
 	index &= 0xff;
 	switch (requesttype & USB_RECIP_MASK) {
 	case USB_RECIP_ENDPOINT:
@@ -770,7 +786,8 @@ static int proc_control(struct dev_state *ps, void __user *arg)
 
 	if (copy_from_user(&ctrl, arg, sizeof(ctrl)))
 		return -EFAULT;
-	ret = check_ctrlrecip(ps, ctrl.bRequestType, ctrl.wIndex);
+	ret = check_ctrlrecip(ps, ctrl.bRequestType, ctrl.bRequest,
+			      ctrl.wIndex);
 	if (ret)
 		return ret;
 	wLength = ctrl.wLength;		/* To suppress 64k PAGE_SIZE warning */
@@ -1100,7 +1117,7 @@ static int proc_do_submiturb(struct dev_state *ps, struct usbdevfs_urb *uurb,
 			kfree(dr);
 			return -EINVAL;
 		}
-		ret = check_ctrlrecip(ps, dr->bRequestType,
+		ret = check_ctrlrecip(ps, dr->bRequestType, dr->bRequest,
 				      le16_to_cpup(&dr->wIndex));
 		if (ret) {
 			kfree(dr);
diff --git a/drivers/usb/core/driver.c b/drivers/usb/core/driver.c
index 34e3da5aa72a..75b4bc03e2e1 100644
--- a/drivers/usb/core/driver.c
+++ b/drivers/usb/core/driver.c
@@ -1583,7 +1583,7 @@ int usb_autopm_get_interface_async(struct usb_interface *intf)
 	dev_vdbg(&intf->dev, "%s: cnt %d -> %d\n",
 			__func__, atomic_read(&intf->dev.power.usage_count),
 			status);
-	if (status > 0)
+	if (status > 0 || status == -EINPROGRESS)
 		status = 0;
 	return status;
 }
@@ -1668,6 +1668,11 @@ int usb_runtime_suspend(struct device *dev)
 		return -EAGAIN;
 
 	status = usb_suspend_both(udev, PMSG_AUTO_SUSPEND);
+
+	/* Allow a retry if autosuspend failed temporarily */
+	if (status == -EAGAIN || status == -EBUSY)
+		usb_mark_last_busy(udev);
+
 	/* The PM core reacts badly unless the return code is 0,
 	 * -EAGAIN, or -EBUSY, so always return -EBUSY on an error.
 	 */
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index 73cbbd85219f..ea47c13b902f 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -1398,11 +1398,10 @@ int usb_hcd_map_urb_for_dma(struct usb_hcd *hcd, struct urb *urb,
 					ret = -EAGAIN;
 				else
 					urb->transfer_flags |= URB_DMA_MAP_SG;
-				if (n != urb->num_sgs) {
-					urb->num_sgs = n;
+				urb->num_mapped_sgs = n;
+				if (n != urb->num_sgs)
 					urb->transfer_flags |=
 							URB_DMA_SG_COMBINED;
-				}
 			} else if (urb->sg) {
 				struct scatterlist *sg = urb->sg;
 				urb->transfer_dma = dma_map_page(
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index a428aa080a36..210e3597091a 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -813,6 +813,12 @@ static void hub_activate(struct usb_hub *hub, enum hub_activation_type type)
 					USB_PORT_FEAT_C_PORT_LINK_STATE);
 		}
 
+		if ((portchange & USB_PORT_STAT_C_BH_RESET) &&
+				hub_is_superspeed(hub->hdev)) {
+			need_debounce_delay = true;
+			clear_port_feature(hub->hdev, port1,
+					USB_PORT_FEAT_C_BH_PORT_RESET);
+		}
 		/* We can forget about a "removed" device when there's a
 		 * physical disconnect or the connect status changes.
 		 */
diff --git a/drivers/usb/core/quirks.c b/drivers/usb/core/quirks.c
index 81ce6a8e1d94..4c65eb6a867a 100644
--- a/drivers/usb/core/quirks.c
+++ b/drivers/usb/core/quirks.c
@@ -38,6 +38,54 @@ static const struct usb_device_id usb_quirk_list[] = {
 	/* Creative SB Audigy 2 NX */
 	{ USB_DEVICE(0x041e, 0x3020), .driver_info = USB_QUIRK_RESET_RESUME },
 
+	/* Logitech Webcam C200 */
+	{ USB_DEVICE(0x046d, 0x0802), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Logitech Webcam C250 */
+	{ USB_DEVICE(0x046d, 0x0804), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Logitech Webcam C300 */
+	{ USB_DEVICE(0x046d, 0x0805), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Logitech Webcam B/C500 */
+	{ USB_DEVICE(0x046d, 0x0807), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Logitech Webcam C600 */
+	{ USB_DEVICE(0x046d, 0x0808), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Logitech Webcam Pro 9000 */
+	{ USB_DEVICE(0x046d, 0x0809), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Logitech Webcam C905 */
+	{ USB_DEVICE(0x046d, 0x080a), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Logitech Webcam C210 */
+	{ USB_DEVICE(0x046d, 0x0819), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Logitech Webcam C260 */
+	{ USB_DEVICE(0x046d, 0x081a), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Logitech Webcam C310 */
+	{ USB_DEVICE(0x046d, 0x081b), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Logitech Webcam C910 */
+	{ USB_DEVICE(0x046d, 0x0821), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Logitech Webcam C160 */
+	{ USB_DEVICE(0x046d, 0x0824), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Logitech Webcam C270 */
+	{ USB_DEVICE(0x046d, 0x0825), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Logitech Quickcam Pro 9000 */
+	{ USB_DEVICE(0x046d, 0x0990), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Logitech Quickcam E3500 */
+	{ USB_DEVICE(0x046d, 0x09a4), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Logitech Quickcam Vision Pro */
+	{ USB_DEVICE(0x046d, 0x09a6), .driver_info = USB_QUIRK_RESET_RESUME },
+
 	/* Logitech Harmony 700-series */
 	{ USB_DEVICE(0x046d, 0xc122), .driver_info = USB_QUIRK_DELAY_INIT },
 
@@ -69,6 +117,12 @@ static const struct usb_device_id usb_quirk_list[] = {
 	{ USB_DEVICE(0x06a3, 0x0006), .driver_info =
 			USB_QUIRK_CONFIG_INTF_STRINGS },
 
+	/* Guillemot Webcam Hercules Dualpix Exchange (2nd ID) */
+	{ USB_DEVICE(0x06f8, 0x0804), .driver_info = USB_QUIRK_RESET_RESUME },
+
+	/* Guillemot Webcam Hercules Dualpix Exchange*/
+	{ USB_DEVICE(0x06f8, 0x3005), .driver_info = USB_QUIRK_RESET_RESUME },
+
 	/* M-Systems Flash Disk Pioneers */
 	{ USB_DEVICE(0x08ec, 0x1000), .driver_info = USB_QUIRK_RESET_RESUME },
 
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index 5a084b9cfa3c..e7c6ca49ab8e 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -141,9 +141,9 @@ config USB_ATMEL_USBA
 
 config USB_FSL_USB2
 	tristate "Freescale Highspeed USB DR Peripheral Controller"
-	depends on FSL_SOC || ARCH_MXC
+	depends on FSL_SOC || ARCH_MXC || ARCH_TEGRA
 	select USB_GADGET_DUALSPEED
-	select USB_FSL_MPH_DR_OF if OF
+	select USB_FSL_MPH_DR_OF if OF && FSL_SOC
 	help
 	   Some of Freescale PowerPC processors have a High Speed
 	   Dual-Role(DR) USB controller, which supports device mode.
@@ -178,6 +178,17 @@ config USB_OMAP
 	   dynamically linked module called "omap_udc" and force all
 	   gadget drivers to also be dynamically linked.
 
+config USB_TEGRA
+	tristate "Nvidia Highspeed USB 2.0 device controller"
+	depends on ARCH_TEGRA
+	select USB_GADGET_DUALSPEED
+	help
+	   Enables TEGRA USB 2.0 pheripheral driver.
+
+	   Say "y" to link the driver statically, or "m" to build a
+	   dynamically linked module called "tegra_udc" and force all
+	   gadget drivers to also be dynamically linked.
+
 config USB_PXA25X
 	tristate "PXA 25x or IXP 4xx"
 	depends on (ARCH_PXA && PXA25x) || ARCH_IXP4XX
@@ -442,7 +453,7 @@ config USB_LANGWELL
 	   gadget drivers to also be dynamically linked.
 
 config USB_EG20T
-	tristate "Intel EG20T PCH/OKI SEMICONDUCTOR ML7213 IOH UDC"
+	tristate "Intel EG20T PCH/LAPIS Semiconductor IOH(ML7213/ML7831) UDC"
 	depends on PCI
 	select USB_GADGET_DUALSPEED
 	help
@@ -458,10 +469,11 @@ config USB_EG20T
 	  This driver dose not support interrupt transfer or isochronous
 	  transfer modes.
 
-	  This driver also can be used for OKI SEMICONDUCTOR's ML7213 which is
+	  This driver also can be used for LAPIS Semiconductor's ML7213 which is
 	  for IVI(In-Vehicle Infotainment) use.
-	  ML7213 is companion chip for Intel Atom E6xx series.
-	  ML7213 is completely compatible for Intel EG20T PCH.
+	  ML7831 is for general purpose use.
+	  ML7213/ML7831 is companion chip for Intel Atom E6xx series.
+	  ML7213/ML7831 is completely compatible for Intel EG20T PCH.
 
 config USB_CI13XXX_MSM
 	tristate "MIPS USB CI13xxx for MSM"
@@ -822,6 +834,14 @@ config USB_G_PRINTER
 	  For more information, see Documentation/usb/gadget_printer.txt
 	  which includes sample code for accessing the device file.
 
+config USB_G_ANDROID
+	boolean "Android Gadget"
+	depends on SWITCH
+	help
+	  The Android gadget driver supports multiple USB functions.
+	  The functions can be configured via a board file and may be
+	  enabled and disabled dynamically.
+
 config USB_CDC_COMPOSITE
 	tristate "CDC Composite Device (Ethernet and ACM)"
 	depends on NET
diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index 9ba725af4a08..13f24548cee1 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -1,6 +1,8 @@
 #
 # USB peripheral controller drivers
 #
+GCOV_PROFILE_tegra_udc.o := y
+
 ccflags-$(CONFIG_USB_GADGET_DEBUG) := -DDEBUG
 
 obj-$(CONFIG_USB_GADGET)	+= udc-core.o
@@ -19,6 +21,8 @@ obj-$(CONFIG_USB_ATMEL_USBA)	+= atmel_usba_udc.o
 obj-$(CONFIG_USB_FSL_USB2)	+= fsl_usb2_udc.o
 fsl_usb2_udc-y			:= fsl_udc_core.o
 fsl_usb2_udc-$(CONFIG_ARCH_MXC)	+= fsl_mxc_udc.o
+CFLAGS_tegra_udc.o		= -Werror
+obj-$(CONFIG_USB_TEGRA)		+= tegra_udc.o
 obj-$(CONFIG_USB_M66592)	+= m66592-udc.o
 obj-$(CONFIG_USB_R8A66597)	+= r8a66597-udc.o
 obj-$(CONFIG_USB_FSL_QE)	+= fsl_qe_udc.o
@@ -51,6 +55,7 @@ g_dbgp-y			:= dbgp.o
 g_nokia-y			:= nokia.o
 g_webcam-y			:= webcam.o
 g_ncm-y				:= ncm.o
+g_android-y			:= android.o
 
 obj-$(CONFIG_USB_ZERO)		+= g_zero.o
 obj-$(CONFIG_USB_AUDIO)		+= g_audio.o
@@ -69,3 +74,4 @@ obj-$(CONFIG_USB_G_MULTI)	+= g_multi.o
 obj-$(CONFIG_USB_G_NOKIA)	+= g_nokia.o
 obj-$(CONFIG_USB_G_WEBCAM)	+= g_webcam.o
 obj-$(CONFIG_USB_G_NCM)		+= g_ncm.o
+obj-$(CONFIG_USB_G_ANDROID)	+= g_android.o
diff --git a/drivers/usb/gadget/android.c b/drivers/usb/gadget/android.c
new file mode 100644
index 000000000000..9f79c267890e
--- /dev/null
+++ b/drivers/usb/gadget/android.c
@@ -0,0 +1,1257 @@
+/*
+ * Gadget Driver for Android
+ *
+ * Copyright (C) 2008 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+/* #define DEBUG */
+/* #define VERBOSE_DEBUG */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/utsname.h>
+#include <linux/platform_device.h>
+
+#include <linux/usb/ch9.h>
+#include <linux/usb/composite.h>
+#include <linux/usb/gadget.h>
+
+#include "gadget_chips.h"
+
+/*
+ * Kbuild is not very cooperative with respect to linking separately
+ * compiled library objects into one module.  So for now we won't use
+ * separate compilation ... ensuring init/exit sections work to shrink
+ * the runtime footprint, and giving us at least some parts of what
+ * a "gcc --combine ... part1.c part2.c part3.c ... " build would.
+ */
+#include "usbstring.c"
+#include "config.c"
+#include "epautoconf.c"
+#include "composite.c"
+
+#include "f_audio_source.c"
+#include "f_mass_storage.c"
+#include "u_serial.c"
+#include "f_acm.c"
+#include "f_adb.c"
+#include "f_mtp.c"
+#include "f_accessory.c"
+#define USB_ETH_RNDIS y
+#include "f_rndis.c"
+#include "rndis.c"
+#include "u_ether.c"
+
+MODULE_AUTHOR("Mike Lockwood");
+MODULE_DESCRIPTION("Android Composite USB Driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("1.0");
+
+static const char longname[] = "Gadget Android";
+
+/* Default vendor and product IDs, overridden by userspace */
+#define VENDOR_ID		0x18D1
+#define PRODUCT_ID		0x0001
+
+struct android_usb_function {
+	char *name;
+	void *config;
+
+	struct device *dev;
+	char *dev_name;
+	struct device_attribute **attributes;
+
+	/* for android_dev.enabled_functions */
+	struct list_head enabled_list;
+
+	/* Optional: initialization during gadget bind */
+	int (*init)(struct android_usb_function *, struct usb_composite_dev *);
+	/* Optional: cleanup during gadget unbind */
+	void (*cleanup)(struct android_usb_function *);
+
+	int (*bind_config)(struct android_usb_function *, struct usb_configuration *);
+
+	/* Optional: called when the configuration is removed */
+	void (*unbind_config)(struct android_usb_function *, struct usb_configuration *);
+	/* Optional: handle ctrl requests before the device is configured */
+	int (*ctrlrequest)(struct android_usb_function *,
+					struct usb_composite_dev *,
+					const struct usb_ctrlrequest *);
+};
+
+struct android_dev {
+	struct android_usb_function **functions;
+	struct list_head enabled_functions;
+	struct usb_composite_dev *cdev;
+	struct device *dev;
+
+	bool enabled;
+	struct mutex mutex;
+	bool connected;
+	bool sw_connected;
+	struct work_struct work;
+};
+
+static struct class *android_class;
+static struct android_dev *_android_dev;
+static int android_bind_config(struct usb_configuration *c);
+static void android_unbind_config(struct usb_configuration *c);
+
+/* string IDs are assigned dynamically */
+#define STRING_MANUFACTURER_IDX		0
+#define STRING_PRODUCT_IDX		1
+#define STRING_SERIAL_IDX		2
+
+static char manufacturer_string[256];
+static char product_string[256];
+static char serial_string[256];
+
+/* String Table */
+static struct usb_string strings_dev[] = {
+	[STRING_MANUFACTURER_IDX].s = manufacturer_string,
+	[STRING_PRODUCT_IDX].s = product_string,
+	[STRING_SERIAL_IDX].s = serial_string,
+	{  }			/* end of list */
+};
+
+static struct usb_gadget_strings stringtab_dev = {
+	.language	= 0x0409,	/* en-us */
+	.strings	= strings_dev,
+};
+
+static struct usb_gadget_strings *dev_strings[] = {
+	&stringtab_dev,
+	NULL,
+};
+
+static struct usb_device_descriptor device_desc = {
+	.bLength              = sizeof(device_desc),
+	.bDescriptorType      = USB_DT_DEVICE,
+	.bcdUSB               = __constant_cpu_to_le16(0x0200),
+	.bDeviceClass         = USB_CLASS_PER_INTERFACE,
+	.idVendor             = __constant_cpu_to_le16(VENDOR_ID),
+	.idProduct            = __constant_cpu_to_le16(PRODUCT_ID),
+	.bcdDevice            = __constant_cpu_to_le16(0xffff),
+	.bNumConfigurations   = 1,
+};
+
+static struct usb_configuration android_config_driver = {
+	.label		= "android",
+	.unbind		= android_unbind_config,
+	.bConfigurationValue = 1,
+};
+
+static void android_work(struct work_struct *data)
+{
+	struct android_dev *dev = container_of(data, struct android_dev, work);
+	struct usb_composite_dev *cdev = dev->cdev;
+	char *disconnected[2] = { "USB_STATE=DISCONNECTED", NULL };
+	char *connected[2]    = { "USB_STATE=CONNECTED", NULL };
+	char *configured[2]   = { "USB_STATE=CONFIGURED", NULL };
+	char **uevent_envp = NULL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&cdev->lock, flags);
+        if (cdev->config)
+		uevent_envp = configured;
+	else if (dev->connected != dev->sw_connected)
+		uevent_envp = dev->connected ? connected : disconnected;
+	dev->sw_connected = dev->connected;
+	spin_unlock_irqrestore(&cdev->lock, flags);
+
+	if (uevent_envp) {
+		kobject_uevent_env(&dev->dev->kobj, KOBJ_CHANGE, uevent_envp);
+		pr_info("%s: sent uevent %s\n", __func__, uevent_envp[0]);
+	} else {
+		pr_info("%s: did not send uevent (%d %d %p)\n", __func__,
+			 dev->connected, dev->sw_connected, cdev->config);
+	}
+}
+
+
+/*-------------------------------------------------------------------------*/
+/* Supported functions initialization */
+
+static int adb_function_init(struct android_usb_function *f, struct usb_composite_dev *cdev)
+{
+	return adb_setup();
+}
+
+static void adb_function_cleanup(struct android_usb_function *f)
+{
+	adb_cleanup();
+}
+
+static int adb_function_bind_config(struct android_usb_function *f, struct usb_configuration *c)
+{
+	return adb_bind_config(c);
+}
+
+static struct android_usb_function adb_function = {
+	.name		= "adb",
+	.init		= adb_function_init,
+	.cleanup	= adb_function_cleanup,
+	.bind_config	= adb_function_bind_config,
+};
+
+
+#define MAX_ACM_INSTANCES 4
+struct acm_function_config {
+	int instances;
+};
+
+static int acm_function_init(struct android_usb_function *f, struct usb_composite_dev *cdev)
+{
+	f->config = kzalloc(sizeof(struct acm_function_config), GFP_KERNEL);
+	if (!f->config)
+		return -ENOMEM;
+
+	return gserial_setup(cdev->gadget, MAX_ACM_INSTANCES);
+}
+
+static void acm_function_cleanup(struct android_usb_function *f)
+{
+	gserial_cleanup();
+	kfree(f->config);
+	f->config = NULL;
+}
+
+static int acm_function_bind_config(struct android_usb_function *f, struct usb_configuration *c)
+{
+	int i;
+	int ret = 0;
+	struct acm_function_config *config = f->config;
+
+	for (i = 0; i < config->instances; i++) {
+		ret = acm_bind_config(c, i);
+		if (ret) {
+			pr_err("Could not bind acm%u config\n", i);
+			break;
+		}
+	}
+
+	return ret;
+}
+
+static ssize_t acm_instances_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct android_usb_function *f = dev_get_drvdata(dev);
+	struct acm_function_config *config = f->config;
+	return sprintf(buf, "%d\n", config->instances);
+}
+
+static ssize_t acm_instances_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct android_usb_function *f = dev_get_drvdata(dev);
+	struct acm_function_config *config = f->config;
+	int value;
+
+	sscanf(buf, "%d", &value);
+	if (value > MAX_ACM_INSTANCES)
+		value = MAX_ACM_INSTANCES;
+	config->instances = value;
+	return size;
+}
+
+static DEVICE_ATTR(instances, S_IRUGO | S_IWUSR, acm_instances_show, acm_instances_store);
+static struct device_attribute *acm_function_attributes[] = { &dev_attr_instances, NULL };
+
+static struct android_usb_function acm_function = {
+	.name		= "acm",
+	.init		= acm_function_init,
+	.cleanup	= acm_function_cleanup,
+	.bind_config	= acm_function_bind_config,
+	.attributes	= acm_function_attributes,
+};
+
+
+static int mtp_function_init(struct android_usb_function *f, struct usb_composite_dev *cdev)
+{
+	return mtp_setup();
+}
+
+static void mtp_function_cleanup(struct android_usb_function *f)
+{
+	mtp_cleanup();
+}
+
+static int mtp_function_bind_config(struct android_usb_function *f, struct usb_configuration *c)
+{
+	return mtp_bind_config(c, false);
+}
+
+static int ptp_function_init(struct android_usb_function *f, struct usb_composite_dev *cdev)
+{
+	/* nothing to do - initialization is handled by mtp_function_init */
+	return 0;
+}
+
+static void ptp_function_cleanup(struct android_usb_function *f)
+{
+	/* nothing to do - cleanup is handled by mtp_function_cleanup */
+}
+
+static int ptp_function_bind_config(struct android_usb_function *f, struct usb_configuration *c)
+{
+	return mtp_bind_config(c, true);
+}
+
+static int mtp_function_ctrlrequest(struct android_usb_function *f,
+						struct usb_composite_dev *cdev,
+						const struct usb_ctrlrequest *c)
+{
+	return mtp_ctrlrequest(cdev, c);
+}
+
+static int ptp_function_ctrlrequest(struct android_usb_function *f,
+						struct usb_composite_dev *cdev,
+						const struct usb_ctrlrequest *c)
+{
+	return mtp_ctrlrequest(cdev, c);
+}
+
+static struct android_usb_function mtp_function = {
+	.name		= "mtp",
+	.init		= mtp_function_init,
+	.cleanup	= mtp_function_cleanup,
+	.bind_config	= mtp_function_bind_config,
+	.ctrlrequest	= mtp_function_ctrlrequest,
+};
+
+/* PTP function is same as MTP with slightly different interface descriptor */
+static struct android_usb_function ptp_function = {
+	.name		= "ptp",
+	.init		= ptp_function_init,
+	.cleanup	= ptp_function_cleanup,
+	.bind_config	= ptp_function_bind_config,
+	.ctrlrequest	= ptp_function_ctrlrequest,
+};
+
+
+struct rndis_function_config {
+	u8      ethaddr[ETH_ALEN];
+	u32     vendorID;
+	char	manufacturer[256];
+	bool	wceis;
+};
+
+static int rndis_function_init(struct android_usb_function *f, struct usb_composite_dev *cdev)
+{
+	f->config = kzalloc(sizeof(struct rndis_function_config), GFP_KERNEL);
+	if (!f->config)
+		return -ENOMEM;
+	return 0;
+}
+
+static void rndis_function_cleanup(struct android_usb_function *f)
+{
+	kfree(f->config);
+	f->config = NULL;
+}
+
+static int rndis_function_bind_config(struct android_usb_function *f,
+					struct usb_configuration *c)
+{
+	int ret;
+	struct rndis_function_config *rndis = f->config;
+
+	if (!rndis) {
+		pr_err("%s: rndis_pdata\n", __func__);
+		return -1;
+	}
+
+	pr_info("%s MAC: %02X:%02X:%02X:%02X:%02X:%02X\n", __func__,
+		rndis->ethaddr[0], rndis->ethaddr[1], rndis->ethaddr[2],
+		rndis->ethaddr[3], rndis->ethaddr[4], rndis->ethaddr[5]);
+
+	ret = gether_setup_name(c->cdev->gadget, rndis->ethaddr, "rndis");
+	if (ret) {
+		pr_err("%s: gether_setup failed\n", __func__);
+		return ret;
+	}
+
+	if (rndis->wceis) {
+		/* "Wireless" RNDIS; auto-detected by Windows */
+		rndis_iad_descriptor.bFunctionClass =
+						USB_CLASS_WIRELESS_CONTROLLER;
+		rndis_iad_descriptor.bFunctionSubClass = 0x01;
+		rndis_iad_descriptor.bFunctionProtocol = 0x03;
+		rndis_control_intf.bInterfaceClass =
+						USB_CLASS_WIRELESS_CONTROLLER;
+		rndis_control_intf.bInterfaceSubClass =	 0x01;
+		rndis_control_intf.bInterfaceProtocol =	 0x03;
+	}
+
+	return rndis_bind_config_vendor(c, rndis->ethaddr, rndis->vendorID,
+					rndis->manufacturer);
+}
+
+static void rndis_function_unbind_config(struct android_usb_function *f,
+						struct usb_configuration *c)
+{
+	gether_cleanup();
+}
+
+static ssize_t rndis_manufacturer_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct android_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *config = f->config;
+	return sprintf(buf, "%s\n", config->manufacturer);
+}
+
+static ssize_t rndis_manufacturer_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct android_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *config = f->config;
+
+	if (size >= sizeof(config->manufacturer))
+		return -EINVAL;
+	if (sscanf(buf, "%s", config->manufacturer) == 1)
+		return size;
+	return -1;
+}
+
+static DEVICE_ATTR(manufacturer, S_IRUGO | S_IWUSR, rndis_manufacturer_show,
+						    rndis_manufacturer_store);
+
+static ssize_t rndis_wceis_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct android_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *config = f->config;
+	return sprintf(buf, "%d\n", config->wceis);
+}
+
+static ssize_t rndis_wceis_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct android_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *config = f->config;
+	int value;
+
+	if (sscanf(buf, "%d", &value) == 1) {
+		config->wceis = value;
+		return size;
+	}
+	return -EINVAL;
+}
+
+static DEVICE_ATTR(wceis, S_IRUGO | S_IWUSR, rndis_wceis_show,
+					     rndis_wceis_store);
+
+static ssize_t rndis_ethaddr_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct android_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *rndis = f->config;
+	return sprintf(buf, "%02x:%02x:%02x:%02x:%02x:%02x\n",
+		rndis->ethaddr[0], rndis->ethaddr[1], rndis->ethaddr[2],
+		rndis->ethaddr[3], rndis->ethaddr[4], rndis->ethaddr[5]);
+}
+
+static ssize_t rndis_ethaddr_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct android_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *rndis = f->config;
+
+	if (sscanf(buf, "%02x:%02x:%02x:%02x:%02x:%02x\n",
+		    (int *)&rndis->ethaddr[0], (int *)&rndis->ethaddr[1],
+		    (int *)&rndis->ethaddr[2], (int *)&rndis->ethaddr[3],
+		    (int *)&rndis->ethaddr[4], (int *)&rndis->ethaddr[5]) == 6)
+		return size;
+	return -EINVAL;
+}
+
+static DEVICE_ATTR(ethaddr, S_IRUGO | S_IWUSR, rndis_ethaddr_show,
+					       rndis_ethaddr_store);
+
+static ssize_t rndis_vendorID_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct android_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *config = f->config;
+	return sprintf(buf, "%04x\n", config->vendorID);
+}
+
+static ssize_t rndis_vendorID_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct android_usb_function *f = dev_get_drvdata(dev);
+	struct rndis_function_config *config = f->config;
+	int value;
+
+	if (sscanf(buf, "%04x", &value) == 1) {
+		config->vendorID = value;
+		return size;
+	}
+	return -EINVAL;
+}
+
+static DEVICE_ATTR(vendorID, S_IRUGO | S_IWUSR, rndis_vendorID_show,
+						rndis_vendorID_store);
+
+static struct device_attribute *rndis_function_attributes[] = {
+	&dev_attr_manufacturer,
+	&dev_attr_wceis,
+	&dev_attr_ethaddr,
+	&dev_attr_vendorID,
+	NULL
+};
+
+static struct android_usb_function rndis_function = {
+	.name		= "rndis",
+	.init		= rndis_function_init,
+	.cleanup	= rndis_function_cleanup,
+	.bind_config	= rndis_function_bind_config,
+	.unbind_config	= rndis_function_unbind_config,
+	.attributes	= rndis_function_attributes,
+};
+
+
+struct mass_storage_function_config {
+	struct fsg_config fsg;
+	struct fsg_common *common;
+};
+
+static int mass_storage_function_init(struct android_usb_function *f,
+					struct usb_composite_dev *cdev)
+{
+	struct mass_storage_function_config *config;
+	struct fsg_common *common;
+	int err;
+
+	config = kzalloc(sizeof(struct mass_storage_function_config),
+								GFP_KERNEL);
+	if (!config)
+		return -ENOMEM;
+
+	config->fsg.nluns = 1;
+	config->fsg.luns[0].removable = 1;
+
+	common = fsg_common_init(NULL, cdev, &config->fsg);
+	if (IS_ERR(common)) {
+		kfree(config);
+		return PTR_ERR(common);
+	}
+
+	err = sysfs_create_link(&f->dev->kobj,
+				&common->luns[0].dev.kobj,
+				"lun");
+	if (err) {
+		kfree(config);
+		return err;
+	}
+
+	config->common = common;
+	f->config = config;
+	return 0;
+}
+
+static void mass_storage_function_cleanup(struct android_usb_function *f)
+{
+	kfree(f->config);
+	f->config = NULL;
+}
+
+static int mass_storage_function_bind_config(struct android_usb_function *f,
+						struct usb_configuration *c)
+{
+	struct mass_storage_function_config *config = f->config;
+	return fsg_bind_config(c->cdev, c, config->common);
+}
+
+static ssize_t mass_storage_inquiry_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct android_usb_function *f = dev_get_drvdata(dev);
+	struct mass_storage_function_config *config = f->config;
+	return sprintf(buf, "%s\n", config->common->inquiry_string);
+}
+
+static ssize_t mass_storage_inquiry_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct android_usb_function *f = dev_get_drvdata(dev);
+	struct mass_storage_function_config *config = f->config;
+	if (size >= sizeof(config->common->inquiry_string))
+		return -EINVAL;
+	if (sscanf(buf, "%s", config->common->inquiry_string) != 1)
+		return -EINVAL;
+	return size;
+}
+
+static DEVICE_ATTR(inquiry_string, S_IRUGO | S_IWUSR,
+					mass_storage_inquiry_show,
+					mass_storage_inquiry_store);
+
+static struct device_attribute *mass_storage_function_attributes[] = {
+	&dev_attr_inquiry_string,
+	NULL
+};
+
+static struct android_usb_function mass_storage_function = {
+	.name		= "mass_storage",
+	.init		= mass_storage_function_init,
+	.cleanup	= mass_storage_function_cleanup,
+	.bind_config	= mass_storage_function_bind_config,
+	.attributes	= mass_storage_function_attributes,
+};
+
+
+static int accessory_function_init(struct android_usb_function *f,
+					struct usb_composite_dev *cdev)
+{
+	return acc_setup();
+}
+
+static void accessory_function_cleanup(struct android_usb_function *f)
+{
+	acc_cleanup();
+}
+
+static int accessory_function_bind_config(struct android_usb_function *f,
+						struct usb_configuration *c)
+{
+	return acc_bind_config(c);
+}
+
+static int accessory_function_ctrlrequest(struct android_usb_function *f,
+						struct usb_composite_dev *cdev,
+						const struct usb_ctrlrequest *c)
+{
+	return acc_ctrlrequest(cdev, c);
+}
+
+static struct android_usb_function accessory_function = {
+	.name		= "accessory",
+	.init		= accessory_function_init,
+	.cleanup	= accessory_function_cleanup,
+	.bind_config	= accessory_function_bind_config,
+	.ctrlrequest	= accessory_function_ctrlrequest,
+};
+
+static int audio_source_function_init(struct android_usb_function *f,
+			struct usb_composite_dev *cdev)
+{
+	struct audio_source_config *config;
+
+	config = kzalloc(sizeof(struct audio_source_config), GFP_KERNEL);
+	if (!config)
+		return -ENOMEM;
+	config->card = -1;
+	config->device = -1;
+	f->config = config;
+	return 0;
+}
+
+static void audio_source_function_cleanup(struct android_usb_function *f)
+{
+	kfree(f->config);
+}
+
+static int audio_source_function_bind_config(struct android_usb_function *f,
+						struct usb_configuration *c)
+{
+	struct audio_source_config *config = f->config;
+
+	return audio_source_bind_config(c, config);
+}
+
+static void audio_source_function_unbind_config(struct android_usb_function *f,
+						struct usb_configuration *c)
+{
+	struct audio_source_config *config = f->config;
+
+	config->card = -1;
+	config->device = -1;
+}
+
+static ssize_t audio_source_pcm_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct android_usb_function *f = dev_get_drvdata(dev);
+	struct audio_source_config *config = f->config;
+
+	/* print PCM card and device numbers */
+	return sprintf(buf, "%d %d\n", config->card, config->device);
+}
+
+static DEVICE_ATTR(pcm, S_IRUGO | S_IWUSR, audio_source_pcm_show, NULL);
+
+static struct device_attribute *audio_source_function_attributes[] = {
+	&dev_attr_pcm,
+	NULL
+};
+
+static struct android_usb_function audio_source_function = {
+	.name		= "audio_source",
+	.init		= audio_source_function_init,
+	.cleanup	= audio_source_function_cleanup,
+	.bind_config	= audio_source_function_bind_config,
+	.unbind_config	= audio_source_function_unbind_config,
+	.attributes	= audio_source_function_attributes,
+};
+
+static struct android_usb_function *supported_functions[] = {
+	&adb_function,
+	&acm_function,
+	&mtp_function,
+	&ptp_function,
+	&rndis_function,
+	&mass_storage_function,
+	&accessory_function,
+	&audio_source_function,
+	NULL
+};
+
+
+static int android_init_functions(struct android_usb_function **functions,
+				  struct usb_composite_dev *cdev)
+{
+	struct android_dev *dev = _android_dev;
+	struct android_usb_function *f;
+	struct device_attribute **attrs;
+	struct device_attribute *attr;
+	int err;
+	int index = 0;
+
+	for (; (f = *functions++); index++) {
+		f->dev_name = kasprintf(GFP_KERNEL, "f_%s", f->name);
+		f->dev = device_create(android_class, dev->dev,
+				MKDEV(0, index), f, f->dev_name);
+		if (IS_ERR(f->dev)) {
+			pr_err("%s: Failed to create dev %s", __func__,
+							f->dev_name);
+			err = PTR_ERR(f->dev);
+			goto err_create;
+		}
+
+		if (f->init) {
+			err = f->init(f, cdev);
+			if (err) {
+				pr_err("%s: Failed to init %s", __func__,
+								f->name);
+				goto err_out;
+			}
+		}
+
+		attrs = f->attributes;
+		if (attrs) {
+			while ((attr = *attrs++) && !err)
+				err = device_create_file(f->dev, attr);
+		}
+		if (err) {
+			pr_err("%s: Failed to create function %s attributes",
+					__func__, f->name);
+			goto err_out;
+		}
+	}
+	return 0;
+
+err_out:
+	device_destroy(android_class, f->dev->devt);
+err_create:
+	kfree(f->dev_name);
+	return err;
+}
+
+static void android_cleanup_functions(struct android_usb_function **functions)
+{
+	struct android_usb_function *f;
+
+	while (*functions) {
+		f = *functions++;
+
+		if (f->dev) {
+			device_destroy(android_class, f->dev->devt);
+			kfree(f->dev_name);
+		}
+
+		if (f->cleanup)
+			f->cleanup(f);
+	}
+}
+
+static int
+android_bind_enabled_functions(struct android_dev *dev,
+			       struct usb_configuration *c)
+{
+	struct android_usb_function *f;
+	int ret;
+
+	list_for_each_entry(f, &dev->enabled_functions, enabled_list) {
+		ret = f->bind_config(f, c);
+		if (ret) {
+			pr_err("%s: %s failed", __func__, f->name);
+			return ret;
+		}
+	}
+	return 0;
+}
+
+static void
+android_unbind_enabled_functions(struct android_dev *dev,
+			       struct usb_configuration *c)
+{
+	struct android_usb_function *f;
+
+	list_for_each_entry(f, &dev->enabled_functions, enabled_list) {
+		if (f->unbind_config)
+			f->unbind_config(f, c);
+	}
+}
+
+static int android_enable_function(struct android_dev *dev, char *name)
+{
+	struct android_usb_function **functions = dev->functions;
+	struct android_usb_function *f;
+	while ((f = *functions++)) {
+		if (!strcmp(name, f->name)) {
+			list_add_tail(&f->enabled_list, &dev->enabled_functions);
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+/*-------------------------------------------------------------------------*/
+/* /sys/class/android_usb/android%d/ interface */
+
+static ssize_t
+functions_show(struct device *pdev, struct device_attribute *attr, char *buf)
+{
+	struct android_dev *dev = dev_get_drvdata(pdev);
+	struct android_usb_function *f;
+	char *buff = buf;
+
+	mutex_lock(&dev->mutex);
+
+	list_for_each_entry(f, &dev->enabled_functions, enabled_list)
+		buff += sprintf(buff, "%s,", f->name);
+
+	mutex_unlock(&dev->mutex);
+
+	if (buff != buf)
+		*(buff-1) = '\n';
+	return buff - buf;
+}
+
+static ssize_t
+functions_store(struct device *pdev, struct device_attribute *attr,
+			       const char *buff, size_t size)
+{
+	struct android_dev *dev = dev_get_drvdata(pdev);
+	char *name;
+	char buf[256], *b;
+	int err;
+
+	mutex_lock(&dev->mutex);
+
+	if (dev->enabled) {
+		mutex_unlock(&dev->mutex);
+		return -EBUSY;
+	}
+
+	INIT_LIST_HEAD(&dev->enabled_functions);
+
+	strncpy(buf, buff, sizeof(buf));
+	b = strim(buf);
+
+	while (b) {
+		name = strsep(&b, ",");
+		if (name) {
+			err = android_enable_function(dev, name);
+			if (err)
+				pr_err("android_usb: Cannot enable '%s'", name);
+		}
+	}
+
+	mutex_unlock(&dev->mutex);
+
+	return size;
+}
+
+static ssize_t enable_show(struct device *pdev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct android_dev *dev = dev_get_drvdata(pdev);
+	return sprintf(buf, "%d\n", dev->enabled);
+}
+
+static ssize_t enable_store(struct device *pdev, struct device_attribute *attr,
+			    const char *buff, size_t size)
+{
+	struct android_dev *dev = dev_get_drvdata(pdev);
+	struct usb_composite_dev *cdev = dev->cdev;
+	int enabled = 0;
+
+	mutex_lock(&dev->mutex);
+
+	sscanf(buff, "%d", &enabled);
+	if (enabled && !dev->enabled) {
+		/* update values in composite driver's copy of device descriptor */
+		cdev->desc.idVendor = device_desc.idVendor;
+		cdev->desc.idProduct = device_desc.idProduct;
+		cdev->desc.bcdDevice = device_desc.bcdDevice;
+		cdev->desc.bDeviceClass = device_desc.bDeviceClass;
+		cdev->desc.bDeviceSubClass = device_desc.bDeviceSubClass;
+		cdev->desc.bDeviceProtocol = device_desc.bDeviceProtocol;
+		usb_add_config(cdev, &android_config_driver,
+					android_bind_config);
+		usb_gadget_connect(cdev->gadget);
+		dev->enabled = true;
+	} else if (!enabled && dev->enabled) {
+		usb_gadget_disconnect(cdev->gadget);
+		/* Cancel pending control requests */
+		usb_ep_dequeue(cdev->gadget->ep0, cdev->req);
+		usb_remove_config(cdev, &android_config_driver);
+		dev->enabled = false;
+	} else {
+		pr_err("android_usb: already %s\n",
+				dev->enabled ? "enabled" : "disabled");
+	}
+
+	mutex_unlock(&dev->mutex);
+	return size;
+}
+
+static ssize_t state_show(struct device *pdev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct android_dev *dev = dev_get_drvdata(pdev);
+	struct usb_composite_dev *cdev = dev->cdev;
+	char *state = "DISCONNECTED";
+	unsigned long flags;
+
+	if (!cdev)
+		goto out;
+
+	spin_lock_irqsave(&cdev->lock, flags);
+        if (cdev->config)
+		state = "CONFIGURED";
+	else if (dev->connected)
+		state = "CONNECTED";
+	spin_unlock_irqrestore(&cdev->lock, flags);
+out:
+	return sprintf(buf, "%s\n", state);
+}
+
+#define DESCRIPTOR_ATTR(field, format_string)				\
+static ssize_t								\
+field ## _show(struct device *dev, struct device_attribute *attr,	\
+		char *buf)						\
+{									\
+	return sprintf(buf, format_string, device_desc.field);		\
+}									\
+static ssize_t								\
+field ## _store(struct device *dev, struct device_attribute *attr,	\
+		const char *buf, size_t size)				\
+{									\
+	int value;							\
+	if (sscanf(buf, format_string, &value) == 1) {			\
+		device_desc.field = value;				\
+		return size;						\
+	}								\
+	return -1;							\
+}									\
+static DEVICE_ATTR(field, S_IRUGO | S_IWUSR, field ## _show, field ## _store);
+
+#define DESCRIPTOR_STRING_ATTR(field, buffer)				\
+static ssize_t								\
+field ## _show(struct device *dev, struct device_attribute *attr,	\
+		char *buf)						\
+{									\
+	return sprintf(buf, "%s", buffer);				\
+}									\
+static ssize_t								\
+field ## _store(struct device *dev, struct device_attribute *attr,	\
+		const char *buf, size_t size)				\
+{									\
+	if (size >= sizeof(buffer)) return -EINVAL;			\
+	if (sscanf(buf, "%s", buffer) == 1) {				\
+		return size;						\
+	}								\
+	return -1;							\
+}									\
+static DEVICE_ATTR(field, S_IRUGO | S_IWUSR, field ## _show, field ## _store);
+
+
+DESCRIPTOR_ATTR(idVendor, "%04x\n")
+DESCRIPTOR_ATTR(idProduct, "%04x\n")
+DESCRIPTOR_ATTR(bcdDevice, "%04x\n")
+DESCRIPTOR_ATTR(bDeviceClass, "%d\n")
+DESCRIPTOR_ATTR(bDeviceSubClass, "%d\n")
+DESCRIPTOR_ATTR(bDeviceProtocol, "%d\n")
+DESCRIPTOR_STRING_ATTR(iManufacturer, manufacturer_string)
+DESCRIPTOR_STRING_ATTR(iProduct, product_string)
+DESCRIPTOR_STRING_ATTR(iSerial, serial_string)
+
+static DEVICE_ATTR(functions, S_IRUGO | S_IWUSR, functions_show, functions_store);
+static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR, enable_show, enable_store);
+static DEVICE_ATTR(state, S_IRUGO, state_show, NULL);
+
+static struct device_attribute *android_usb_attributes[] = {
+	&dev_attr_idVendor,
+	&dev_attr_idProduct,
+	&dev_attr_bcdDevice,
+	&dev_attr_bDeviceClass,
+	&dev_attr_bDeviceSubClass,
+	&dev_attr_bDeviceProtocol,
+	&dev_attr_iManufacturer,
+	&dev_attr_iProduct,
+	&dev_attr_iSerial,
+	&dev_attr_functions,
+	&dev_attr_enable,
+	&dev_attr_state,
+	NULL
+};
+
+/*-------------------------------------------------------------------------*/
+/* Composite driver */
+
+static int android_bind_config(struct usb_configuration *c)
+{
+	struct android_dev *dev = _android_dev;
+	int ret = 0;
+
+	ret = android_bind_enabled_functions(dev, c);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static void android_unbind_config(struct usb_configuration *c)
+{
+	struct android_dev *dev = _android_dev;
+
+	android_unbind_enabled_functions(dev, c);
+}
+
+static int android_bind(struct usb_composite_dev *cdev)
+{
+	struct android_dev *dev = _android_dev;
+	struct usb_gadget	*gadget = cdev->gadget;
+	int			gcnum, id, ret;
+
+	usb_gadget_disconnect(gadget);
+
+	ret = android_init_functions(dev->functions, cdev);
+	if (ret)
+		return ret;
+
+	/* Allocate string descriptor numbers ... note that string
+	 * contents can be overridden by the composite_dev glue.
+	 */
+	id = usb_string_id(cdev);
+	if (id < 0)
+		return id;
+	strings_dev[STRING_MANUFACTURER_IDX].id = id;
+	device_desc.iManufacturer = id;
+
+	id = usb_string_id(cdev);
+	if (id < 0)
+		return id;
+	strings_dev[STRING_PRODUCT_IDX].id = id;
+	device_desc.iProduct = id;
+
+	/* Default strings - should be updated by userspace */
+	strncpy(manufacturer_string, "Android", sizeof(manufacturer_string) - 1);
+	strncpy(product_string, "Android", sizeof(product_string) - 1);
+	strncpy(serial_string, "0123456789ABCDEF", sizeof(serial_string) - 1);
+
+	id = usb_string_id(cdev);
+	if (id < 0)
+		return id;
+	strings_dev[STRING_SERIAL_IDX].id = id;
+	device_desc.iSerialNumber = id;
+
+	gcnum = usb_gadget_controller_number(gadget);
+	if (gcnum >= 0)
+		device_desc.bcdDevice = cpu_to_le16(0x0200 + gcnum);
+	else {
+		/* gadget zero is so simple (for now, no altsettings) that
+		 * it SHOULD NOT have problems with bulk-capable hardware.
+		 * so just warn about unrcognized controllers -- don't panic.
+		 *
+		 * things like configuration and altsetting numbering
+		 * can need hardware-specific attention though.
+		 */
+		pr_warning("%s: controller '%s' not recognized\n",
+			longname, gadget->name);
+		device_desc.bcdDevice = __constant_cpu_to_le16(0x9999);
+	}
+
+	dev->cdev = cdev;
+
+	return 0;
+}
+
+static int android_usb_unbind(struct usb_composite_dev *cdev)
+{
+	struct android_dev *dev = _android_dev;
+
+	cancel_work_sync(&dev->work);
+	android_cleanup_functions(dev->functions);
+	return 0;
+}
+
+static struct usb_composite_driver android_usb_driver = {
+	.name		= "android_usb",
+	.dev		= &device_desc,
+	.strings	= dev_strings,
+	.unbind		= android_usb_unbind,
+	.max_speed	= USB_SPEED_HIGH,
+};
+
+static int
+android_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *c)
+{
+	struct android_dev		*dev = _android_dev;
+	struct usb_composite_dev	*cdev = get_gadget_data(gadget);
+	struct usb_request		*req = cdev->req;
+	struct android_usb_function	*f;
+	int value = -EOPNOTSUPP;
+	unsigned long flags;
+
+	req->zero = 0;
+	req->complete = composite_setup_complete;
+	req->length = 0;
+	gadget->ep0->driver_data = cdev;
+
+	list_for_each_entry(f, &dev->enabled_functions, enabled_list) {
+		if (f->ctrlrequest) {
+			value = f->ctrlrequest(f, cdev, c);
+			if (value >= 0)
+				break;
+		}
+	}
+
+	/* Special case the accessory function.
+	 * It needs to handle control requests before it is enabled.
+	 */
+	if (value < 0)
+		value = acc_ctrlrequest(cdev, c);
+
+	if (value < 0)
+		value = composite_setup(gadget, c);
+
+	spin_lock_irqsave(&cdev->lock, flags);
+	if (!dev->connected) {
+		dev->connected = 1;
+		schedule_work(&dev->work);
+	}
+	else if (c->bRequest == USB_REQ_SET_CONFIGURATION && cdev->config) {
+		schedule_work(&dev->work);
+	}
+	spin_unlock_irqrestore(&cdev->lock, flags);
+
+	return value;
+}
+
+static void android_disconnect(struct usb_gadget *gadget)
+{
+	struct android_dev *dev = _android_dev;
+	struct usb_composite_dev *cdev = get_gadget_data(gadget);
+	unsigned long flags;
+
+	composite_disconnect(gadget);
+	/* accessory HID support can be active while the
+	   accessory function is not actually enabled,
+	   so we need to inform it when we are disconnected.
+	 */
+	acc_disconnect();
+
+	spin_lock_irqsave(&cdev->lock, flags);
+	dev->connected = 0;
+	schedule_work(&dev->work);
+	spin_unlock_irqrestore(&cdev->lock, flags);
+}
+
+static int android_create_device(struct android_dev *dev)
+{
+	struct device_attribute **attrs = android_usb_attributes;
+	struct device_attribute *attr;
+	int err;
+
+	dev->dev = device_create(android_class, NULL,
+					MKDEV(0, 0), NULL, "android0");
+	if (IS_ERR(dev->dev))
+		return PTR_ERR(dev->dev);
+
+	dev_set_drvdata(dev->dev, dev);
+
+	while ((attr = *attrs++)) {
+		err = device_create_file(dev->dev, attr);
+		if (err) {
+			device_destroy(android_class, dev->dev->devt);
+			return err;
+		}
+	}
+	return 0;
+}
+
+
+static int __init init(void)
+{
+	struct android_dev *dev;
+	int err;
+
+	android_class = class_create(THIS_MODULE, "android_usb");
+	if (IS_ERR(android_class))
+		return PTR_ERR(android_class);
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	dev->functions = supported_functions;
+	INIT_LIST_HEAD(&dev->enabled_functions);
+	INIT_WORK(&dev->work, android_work);
+	mutex_init(&dev->mutex);
+
+	err = android_create_device(dev);
+	if (err) {
+		class_destroy(android_class);
+		kfree(dev);
+		return err;
+	}
+
+	_android_dev = dev;
+
+	/* Override composite driver functions */
+	composite_driver.setup = android_setup;
+	composite_driver.disconnect = android_disconnect;
+
+	return usb_composite_probe(&android_usb_driver, android_bind);
+}
+module_init(init);
+
+static void __exit cleanup(void)
+{
+	usb_composite_unregister(&android_usb_driver);
+	class_destroy(android_class);
+	kfree(_android_dev);
+	_android_dev = NULL;
+}
+module_exit(cleanup);
diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index aef47414f5d5..9dac72cff73d 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -712,6 +712,7 @@ static int set_config(struct usb_composite_dev *cdev,
 	power = c->bMaxPower ? (2 * c->bMaxPower) : CONFIG_USB_GADGET_VBUS_DRAW;
 done:
 	usb_gadget_vbus_draw(gadget, power);
+
 	if (result >= 0 && cdev->delayed_status)
 		result = USB_GADGET_DELAYED_STATUS;
 	return result;
@@ -759,6 +760,7 @@ int usb_add_config(struct usb_composite_dev *cdev,
 
 	INIT_LIST_HEAD(&config->functions);
 	config->next_interface_id = 0;
+	memset(config->interface, '\0', sizeof(config->interface));
 
 	status = bind(config);
 	if (status < 0) {
@@ -799,6 +801,45 @@ done:
 	return status;
 }
 
+static int remove_config(struct usb_composite_dev *cdev,
+			      struct usb_configuration *config)
+{
+	while (!list_empty(&config->functions)) {
+		struct usb_function		*f;
+
+		f = list_first_entry(&config->functions,
+				struct usb_function, list);
+		list_del(&f->list);
+		if (f->unbind) {
+			DBG(cdev, "unbind function '%s'/%p\n", f->name, f);
+			f->unbind(config, f);
+			/* may free memory for "f" */
+		}
+	}
+	list_del(&config->list);
+	if (config->unbind) {
+		DBG(cdev, "unbind config '%s'/%p\n", config->label, config);
+		config->unbind(config);
+			/* may free memory for "c" */
+	}
+	return 0;
+}
+
+int usb_remove_config(struct usb_composite_dev *cdev,
+		      struct usb_configuration *config)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&cdev->lock, flags);
+
+	if (cdev->config == config)
+		reset_config(cdev);
+
+	spin_unlock_irqrestore(&cdev->lock, flags);
+
+	return remove_config(cdev, config);
+}
+
 /*-------------------------------------------------------------------------*/
 
 /* We support strings in multiple languages ... string descriptor zero
@@ -1353,28 +1394,9 @@ composite_unbind(struct usb_gadget *gadget)
 
 	while (!list_empty(&cdev->configs)) {
 		struct usb_configuration	*c;
-
 		c = list_first_entry(&cdev->configs,
 				struct usb_configuration, list);
-		while (!list_empty(&c->functions)) {
-			struct usb_function		*f;
-
-			f = list_first_entry(&c->functions,
-					struct usb_function, list);
-			list_del(&f->list);
-			if (f->unbind) {
-				DBG(cdev, "unbind function '%s'/%p\n",
-						f->name, f);
-				f->unbind(c, f);
-				/* may free memory for "f" */
-			}
-		}
-		list_del(&c->list);
-		if (c->unbind) {
-			DBG(cdev, "unbind config '%s'/%p\n", c->label, c);
-			c->unbind(c);
-			/* may free memory for "c" */
-		}
+		remove_config(cdev, c);
 	}
 	if (composite->unbind)
 		composite->unbind(cdev);
diff --git a/drivers/usb/gadget/f_accessory.c b/drivers/usb/gadget/f_accessory.c
new file mode 100644
index 000000000000..d248ef98b532
--- /dev/null
+++ b/drivers/usb/gadget/f_accessory.c
@@ -0,0 +1,1172 @@
+/*
+ * Gadget Function Driver for Android USB accessories
+ *
+ * Copyright (C) 2011 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+/* #define DEBUG */
+/* #define VERBOSE_DEBUG */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/poll.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+
+#include <linux/types.h>
+#include <linux/file.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+
+#include <linux/hid.h>
+#include <linux/hiddev.h>
+#include <linux/usb.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/f_accessory.h>
+
+#define BULK_BUFFER_SIZE    16384
+#define ACC_STRING_SIZE     256
+
+#define PROTOCOL_VERSION    2
+
+/* String IDs */
+#define INTERFACE_STRING_INDEX	0
+
+/* number of tx and rx requests to allocate */
+#define TX_REQ_MAX 4
+#define RX_REQ_MAX 2
+
+struct acc_hid_dev {
+	struct list_head	list;
+	struct hid_device *hid;
+	struct acc_dev *dev;
+	/* accessory defined ID */
+	int id;
+	/* HID report descriptor */
+	u8 *report_desc;
+	/* length of HID report descriptor */
+	int report_desc_len;
+	/* number of bytes of report_desc we have received so far */
+	int report_desc_offset;
+};
+
+struct acc_dev {
+	struct usb_function function;
+	struct usb_composite_dev *cdev;
+	spinlock_t lock;
+
+	struct usb_ep *ep_in;
+	struct usb_ep *ep_out;
+
+	/* set to 1 when we connect */
+	int online:1;
+	/* Set to 1 when we disconnect.
+	 * Not cleared until our file is closed.
+	 */
+	int disconnected:1;
+
+	/* strings sent by the host */
+	char manufacturer[ACC_STRING_SIZE];
+	char model[ACC_STRING_SIZE];
+	char description[ACC_STRING_SIZE];
+	char version[ACC_STRING_SIZE];
+	char uri[ACC_STRING_SIZE];
+	char serial[ACC_STRING_SIZE];
+
+	/* for acc_complete_set_string */
+	int string_index;
+
+	/* set to 1 if we have a pending start request */
+	int start_requested;
+
+	int audio_mode;
+
+	/* synchronize access to our device file */
+	atomic_t open_excl;
+
+	struct list_head tx_idle;
+
+	wait_queue_head_t read_wq;
+	wait_queue_head_t write_wq;
+	struct usb_request *rx_req[RX_REQ_MAX];
+	int rx_done;
+
+	/* delayed work for handling ACCESSORY_START */
+	struct delayed_work start_work;
+
+	/* worker for registering and unregistering hid devices */
+	struct work_struct hid_work;
+
+	/* list of active HID devices */
+	struct list_head	hid_list;
+
+	/* list of new HID devices to register */
+	struct list_head	new_hid_list;
+
+	/* list of dead HID devices to unregister */
+	struct list_head	dead_hid_list;
+};
+
+static struct usb_interface_descriptor acc_interface_desc = {
+	.bLength                = USB_DT_INTERFACE_SIZE,
+	.bDescriptorType        = USB_DT_INTERFACE,
+	.bInterfaceNumber       = 0,
+	.bNumEndpoints          = 2,
+	.bInterfaceClass        = USB_CLASS_VENDOR_SPEC,
+	.bInterfaceSubClass     = USB_SUBCLASS_VENDOR_SPEC,
+	.bInterfaceProtocol     = 0,
+};
+
+static struct usb_endpoint_descriptor acc_highspeed_in_desc = {
+	.bLength                = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType        = USB_DT_ENDPOINT,
+	.bEndpointAddress       = USB_DIR_IN,
+	.bmAttributes           = USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize         = __constant_cpu_to_le16(512),
+};
+
+static struct usb_endpoint_descriptor acc_highspeed_out_desc = {
+	.bLength                = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType        = USB_DT_ENDPOINT,
+	.bEndpointAddress       = USB_DIR_OUT,
+	.bmAttributes           = USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize         = __constant_cpu_to_le16(512),
+};
+
+static struct usb_endpoint_descriptor acc_fullspeed_in_desc = {
+	.bLength                = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType        = USB_DT_ENDPOINT,
+	.bEndpointAddress       = USB_DIR_IN,
+	.bmAttributes           = USB_ENDPOINT_XFER_BULK,
+};
+
+static struct usb_endpoint_descriptor acc_fullspeed_out_desc = {
+	.bLength                = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType        = USB_DT_ENDPOINT,
+	.bEndpointAddress       = USB_DIR_OUT,
+	.bmAttributes           = USB_ENDPOINT_XFER_BULK,
+};
+
+static struct usb_descriptor_header *fs_acc_descs[] = {
+	(struct usb_descriptor_header *) &acc_interface_desc,
+	(struct usb_descriptor_header *) &acc_fullspeed_in_desc,
+	(struct usb_descriptor_header *) &acc_fullspeed_out_desc,
+	NULL,
+};
+
+static struct usb_descriptor_header *hs_acc_descs[] = {
+	(struct usb_descriptor_header *) &acc_interface_desc,
+	(struct usb_descriptor_header *) &acc_highspeed_in_desc,
+	(struct usb_descriptor_header *) &acc_highspeed_out_desc,
+	NULL,
+};
+
+static struct usb_string acc_string_defs[] = {
+	[INTERFACE_STRING_INDEX].s	= "Android Accessory Interface",
+	{  },	/* end of list */
+};
+
+static struct usb_gadget_strings acc_string_table = {
+	.language		= 0x0409,	/* en-US */
+	.strings		= acc_string_defs,
+};
+
+static struct usb_gadget_strings *acc_strings[] = {
+	&acc_string_table,
+	NULL,
+};
+
+/* temporary variable used between acc_open() and acc_gadget_bind() */
+static struct acc_dev *_acc_dev;
+
+static inline struct acc_dev *func_to_dev(struct usb_function *f)
+{
+	return container_of(f, struct acc_dev, function);
+}
+
+static struct usb_request *acc_request_new(struct usb_ep *ep, int buffer_size)
+{
+	struct usb_request *req = usb_ep_alloc_request(ep, GFP_KERNEL);
+	if (!req)
+		return NULL;
+
+	/* now allocate buffers for the requests */
+	req->buf = kmalloc(buffer_size, GFP_KERNEL);
+	if (!req->buf) {
+		usb_ep_free_request(ep, req);
+		return NULL;
+	}
+
+	return req;
+}
+
+static void acc_request_free(struct usb_request *req, struct usb_ep *ep)
+{
+	if (req) {
+		kfree(req->buf);
+		usb_ep_free_request(ep, req);
+	}
+}
+
+/* add a request to the tail of a list */
+static void req_put(struct acc_dev *dev, struct list_head *head,
+		struct usb_request *req)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	list_add_tail(&req->list, head);
+	spin_unlock_irqrestore(&dev->lock, flags);
+}
+
+/* remove a request from the head of a list */
+static struct usb_request *req_get(struct acc_dev *dev, struct list_head *head)
+{
+	unsigned long flags;
+	struct usb_request *req;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	if (list_empty(head)) {
+		req = 0;
+	} else {
+		req = list_first_entry(head, struct usb_request, list);
+		list_del(&req->list);
+	}
+	spin_unlock_irqrestore(&dev->lock, flags);
+	return req;
+}
+
+static void acc_set_disconnected(struct acc_dev *dev)
+{
+	dev->online = 0;
+	dev->disconnected = 1;
+}
+
+static void acc_complete_in(struct usb_ep *ep, struct usb_request *req)
+{
+	struct acc_dev *dev = _acc_dev;
+
+	if (req->status != 0)
+		acc_set_disconnected(dev);
+
+	req_put(dev, &dev->tx_idle, req);
+
+	wake_up(&dev->write_wq);
+}
+
+static void acc_complete_out(struct usb_ep *ep, struct usb_request *req)
+{
+	struct acc_dev *dev = _acc_dev;
+
+	dev->rx_done = 1;
+	if (req->status != 0)
+		acc_set_disconnected(dev);
+
+	wake_up(&dev->read_wq);
+}
+
+static void acc_complete_set_string(struct usb_ep *ep, struct usb_request *req)
+{
+	struct acc_dev	*dev = ep->driver_data;
+	char *string_dest = NULL;
+	int length = req->actual;
+
+	if (req->status != 0) {
+		pr_err("acc_complete_set_string, err %d\n", req->status);
+		return;
+	}
+
+	switch (dev->string_index) {
+	case ACCESSORY_STRING_MANUFACTURER:
+		string_dest = dev->manufacturer;
+		break;
+	case ACCESSORY_STRING_MODEL:
+		string_dest = dev->model;
+		break;
+	case ACCESSORY_STRING_DESCRIPTION:
+		string_dest = dev->description;
+		break;
+	case ACCESSORY_STRING_VERSION:
+		string_dest = dev->version;
+		break;
+	case ACCESSORY_STRING_URI:
+		string_dest = dev->uri;
+		break;
+	case ACCESSORY_STRING_SERIAL:
+		string_dest = dev->serial;
+		break;
+	}
+	if (string_dest) {
+		unsigned long flags;
+
+		if (length >= ACC_STRING_SIZE)
+			length = ACC_STRING_SIZE - 1;
+
+		spin_lock_irqsave(&dev->lock, flags);
+		memcpy(string_dest, req->buf, length);
+		/* ensure zero termination */
+		string_dest[length] = 0;
+		spin_unlock_irqrestore(&dev->lock, flags);
+	} else {
+		pr_err("unknown accessory string index %d\n",
+			dev->string_index);
+	}
+}
+
+static void acc_complete_set_hid_report_desc(struct usb_ep *ep,
+		struct usb_request *req)
+{
+	struct acc_hid_dev *hid = req->context;
+	struct acc_dev *dev = hid->dev;
+	int length = req->actual;
+
+	if (req->status != 0) {
+		pr_err("acc_complete_set_hid_report_desc, err %d\n",
+			req->status);
+		return;
+	}
+
+	memcpy(hid->report_desc + hid->report_desc_offset, req->buf, length);
+	hid->report_desc_offset += length;
+	if (hid->report_desc_offset == hid->report_desc_len) {
+		/* After we have received the entire report descriptor
+		 * we schedule work to initialize the HID device
+		 */
+		schedule_work(&dev->hid_work);
+	}
+}
+
+static void acc_complete_send_hid_event(struct usb_ep *ep,
+		struct usb_request *req)
+{
+	struct acc_hid_dev *hid = req->context;
+	int length = req->actual;
+
+	if (req->status != 0) {
+		pr_err("acc_complete_send_hid_event, err %d\n", req->status);
+		return;
+	}
+
+	hid_report_raw_event(hid->hid, HID_INPUT_REPORT, req->buf, length, 1);
+}
+
+static int acc_hid_parse(struct hid_device *hid)
+{
+	struct acc_hid_dev *hdev = hid->driver_data;
+
+	hid_parse_report(hid, hdev->report_desc, hdev->report_desc_len);
+	return 0;
+}
+
+static int acc_hid_start(struct hid_device *hid)
+{
+	return 0;
+}
+
+static void acc_hid_stop(struct hid_device *hid)
+{
+}
+
+static int acc_hid_open(struct hid_device *hid)
+{
+	return 0;
+}
+
+static void acc_hid_close(struct hid_device *hid)
+{
+}
+
+static struct hid_ll_driver acc_hid_ll_driver = {
+	.parse = acc_hid_parse,
+	.start = acc_hid_start,
+	.stop = acc_hid_stop,
+	.open = acc_hid_open,
+	.close = acc_hid_close,
+};
+
+static struct acc_hid_dev *acc_hid_new(struct acc_dev *dev,
+		int id, int desc_len)
+{
+	struct acc_hid_dev *hdev;
+
+	hdev = kzalloc(sizeof(*hdev), GFP_ATOMIC);
+	if (!hdev)
+		return NULL;
+	hdev->report_desc = kzalloc(desc_len, GFP_ATOMIC);
+	if (!hdev->report_desc) {
+		kfree(hdev);
+		return NULL;
+	}
+	hdev->dev = dev;
+	hdev->id = id;
+	hdev->report_desc_len = desc_len;
+
+	return hdev;
+}
+
+static struct acc_hid_dev *acc_hid_get(struct list_head *list, int id)
+{
+	struct acc_hid_dev *hid;
+
+	list_for_each_entry(hid, list, list) {
+		if (hid->id == id)
+			return hid;
+	}
+	return NULL;
+}
+
+static int acc_register_hid(struct acc_dev *dev, int id, int desc_length)
+{
+	struct acc_hid_dev *hid;
+	unsigned long flags;
+
+	/* report descriptor length must be > 0 */
+	if (desc_length <= 0)
+		return -EINVAL;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	/* replace HID if one already exists with this ID */
+	hid = acc_hid_get(&dev->hid_list, id);
+	if (!hid)
+		hid = acc_hid_get(&dev->new_hid_list, id);
+	if (hid)
+		list_move(&hid->list, &dev->dead_hid_list);
+
+	hid = acc_hid_new(dev, id, desc_length);
+	if (!hid) {
+		spin_unlock_irqrestore(&dev->lock, flags);
+		return -ENOMEM;
+	}
+
+	list_add(&hid->list, &dev->new_hid_list);
+	spin_unlock_irqrestore(&dev->lock, flags);
+
+	/* schedule work to register the HID device */
+	schedule_work(&dev->hid_work);
+	return 0;
+}
+
+static int acc_unregister_hid(struct acc_dev *dev, int id)
+{
+	struct acc_hid_dev *hid;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	hid = acc_hid_get(&dev->hid_list, id);
+	if (!hid)
+		hid = acc_hid_get(&dev->new_hid_list, id);
+	if (!hid) {
+		spin_unlock_irqrestore(&dev->lock, flags);
+		return -EINVAL;
+	}
+
+	list_move(&hid->list, &dev->dead_hid_list);
+	spin_unlock_irqrestore(&dev->lock, flags);
+
+	schedule_work(&dev->hid_work);
+	return 0;
+}
+
+static int create_bulk_endpoints(struct acc_dev *dev,
+				struct usb_endpoint_descriptor *in_desc,
+				struct usb_endpoint_descriptor *out_desc)
+{
+	struct usb_composite_dev *cdev = dev->cdev;
+	struct usb_request *req;
+	struct usb_ep *ep;
+	int i;
+
+	DBG(cdev, "create_bulk_endpoints dev: %p\n", dev);
+
+	ep = usb_ep_autoconfig(cdev->gadget, in_desc);
+	if (!ep) {
+		DBG(cdev, "usb_ep_autoconfig for ep_in failed\n");
+		return -ENODEV;
+	}
+	DBG(cdev, "usb_ep_autoconfig for ep_in got %s\n", ep->name);
+	ep->driver_data = dev;		/* claim the endpoint */
+	dev->ep_in = ep;
+
+	ep = usb_ep_autoconfig(cdev->gadget, out_desc);
+	if (!ep) {
+		DBG(cdev, "usb_ep_autoconfig for ep_out failed\n");
+		return -ENODEV;
+	}
+	DBG(cdev, "usb_ep_autoconfig for ep_out got %s\n", ep->name);
+	ep->driver_data = dev;		/* claim the endpoint */
+	dev->ep_out = ep;
+
+	ep = usb_ep_autoconfig(cdev->gadget, out_desc);
+	if (!ep) {
+		DBG(cdev, "usb_ep_autoconfig for ep_out failed\n");
+		return -ENODEV;
+	}
+	DBG(cdev, "usb_ep_autoconfig for ep_out got %s\n", ep->name);
+	ep->driver_data = dev;		/* claim the endpoint */
+	dev->ep_out = ep;
+
+	/* now allocate requests for our endpoints */
+	for (i = 0; i < TX_REQ_MAX; i++) {
+		req = acc_request_new(dev->ep_in, BULK_BUFFER_SIZE);
+		if (!req)
+			goto fail;
+		req->complete = acc_complete_in;
+		req_put(dev, &dev->tx_idle, req);
+	}
+	for (i = 0; i < RX_REQ_MAX; i++) {
+		req = acc_request_new(dev->ep_out, BULK_BUFFER_SIZE);
+		if (!req)
+			goto fail;
+		req->complete = acc_complete_out;
+		dev->rx_req[i] = req;
+	}
+
+	return 0;
+
+fail:
+	pr_err("acc_bind() could not allocate requests\n");
+	while ((req = req_get(dev, &dev->tx_idle)))
+		acc_request_free(req, dev->ep_in);
+	for (i = 0; i < RX_REQ_MAX; i++)
+		acc_request_free(dev->rx_req[i], dev->ep_out);
+	return -1;
+}
+
+static ssize_t acc_read(struct file *fp, char __user *buf,
+	size_t count, loff_t *pos)
+{
+	struct acc_dev *dev = fp->private_data;
+	struct usb_request *req;
+	int r = count, xfer;
+	int ret = 0;
+
+	pr_debug("acc_read(%d)\n", count);
+
+	if (dev->disconnected)
+		return -ENODEV;
+
+	if (count > BULK_BUFFER_SIZE)
+		count = BULK_BUFFER_SIZE;
+
+	/* we will block until we're online */
+	pr_debug("acc_read: waiting for online\n");
+	ret = wait_event_interruptible(dev->read_wq, dev->online);
+	if (ret < 0) {
+		r = ret;
+		goto done;
+	}
+
+requeue_req:
+	/* queue a request */
+	req = dev->rx_req[0];
+	req->length = count;
+	dev->rx_done = 0;
+	ret = usb_ep_queue(dev->ep_out, req, GFP_KERNEL);
+	if (ret < 0) {
+		r = -EIO;
+		goto done;
+	} else {
+		pr_debug("rx %p queue\n", req);
+	}
+
+	/* wait for a request to complete */
+	ret = wait_event_interruptible(dev->read_wq, dev->rx_done);
+	if (ret < 0) {
+		r = ret;
+		usb_ep_dequeue(dev->ep_out, req);
+		goto done;
+	}
+	if (dev->online) {
+		/* If we got a 0-len packet, throw it back and try again. */
+		if (req->actual == 0)
+			goto requeue_req;
+
+		pr_debug("rx %p %d\n", req, req->actual);
+		xfer = (req->actual < count) ? req->actual : count;
+		r = xfer;
+		if (copy_to_user(buf, req->buf, xfer))
+			r = -EFAULT;
+	} else
+		r = -EIO;
+
+done:
+	pr_debug("acc_read returning %d\n", r);
+	return r;
+}
+
+static ssize_t acc_write(struct file *fp, const char __user *buf,
+	size_t count, loff_t *pos)
+{
+	struct acc_dev *dev = fp->private_data;
+	struct usb_request *req = 0;
+	int r = count, xfer;
+	int ret;
+
+	pr_debug("acc_write(%d)\n", count);
+
+	if (!dev->online || dev->disconnected)
+		return -ENODEV;
+
+	while (count > 0) {
+		if (!dev->online) {
+			pr_debug("acc_write dev->error\n");
+			r = -EIO;
+			break;
+		}
+
+		/* get an idle tx request to use */
+		req = 0;
+		ret = wait_event_interruptible(dev->write_wq,
+			((req = req_get(dev, &dev->tx_idle)) || !dev->online));
+		if (!req) {
+			r = ret;
+			break;
+		}
+
+		if (count > BULK_BUFFER_SIZE)
+			xfer = BULK_BUFFER_SIZE;
+		else
+			xfer = count;
+		if (copy_from_user(req->buf, buf, xfer)) {
+			r = -EFAULT;
+			break;
+		}
+
+		req->length = xfer;
+		ret = usb_ep_queue(dev->ep_in, req, GFP_KERNEL);
+		if (ret < 0) {
+			pr_debug("acc_write: xfer error %d\n", ret);
+			r = -EIO;
+			break;
+		}
+
+		buf += xfer;
+		count -= xfer;
+
+		/* zero this so we don't try to free it on error exit */
+		req = 0;
+	}
+
+	if (req)
+		req_put(dev, &dev->tx_idle, req);
+
+	pr_debug("acc_write returning %d\n", r);
+	return r;
+}
+
+static long acc_ioctl(struct file *fp, unsigned code, unsigned long value)
+{
+	struct acc_dev *dev = fp->private_data;
+	char *src = NULL;
+	int ret;
+
+	switch (code) {
+	case ACCESSORY_GET_STRING_MANUFACTURER:
+		src = dev->manufacturer;
+		break;
+	case ACCESSORY_GET_STRING_MODEL:
+		src = dev->model;
+		break;
+	case ACCESSORY_GET_STRING_DESCRIPTION:
+		src = dev->description;
+		break;
+	case ACCESSORY_GET_STRING_VERSION:
+		src = dev->version;
+		break;
+	case ACCESSORY_GET_STRING_URI:
+		src = dev->uri;
+		break;
+	case ACCESSORY_GET_STRING_SERIAL:
+		src = dev->serial;
+		break;
+	case ACCESSORY_IS_START_REQUESTED:
+		return dev->start_requested;
+	case ACCESSORY_GET_AUDIO_MODE:
+		return dev->audio_mode;
+	}
+	if (!src)
+		return -EINVAL;
+
+	ret = strlen(src) + 1;
+	if (copy_to_user((void __user *)value, src, ret))
+		ret = -EFAULT;
+	return ret;
+}
+
+static int acc_open(struct inode *ip, struct file *fp)
+{
+	printk(KERN_INFO "acc_open\n");
+	if (atomic_xchg(&_acc_dev->open_excl, 1))
+		return -EBUSY;
+
+	_acc_dev->disconnected = 0;
+	fp->private_data = _acc_dev;
+	return 0;
+}
+
+static int acc_release(struct inode *ip, struct file *fp)
+{
+	printk(KERN_INFO "acc_release\n");
+
+	WARN_ON(!atomic_xchg(&_acc_dev->open_excl, 0));
+	_acc_dev->disconnected = 0;
+	return 0;
+}
+
+/* file operations for /dev/usb_accessory */
+static const struct file_operations acc_fops = {
+	.owner = THIS_MODULE,
+	.read = acc_read,
+	.write = acc_write,
+	.unlocked_ioctl = acc_ioctl,
+	.open = acc_open,
+	.release = acc_release,
+};
+
+static int acc_hid_probe(struct hid_device *hdev,
+		const struct hid_device_id *id)
+{
+	int ret;
+
+	ret = hid_parse(hdev);
+	if (ret)
+		return ret;
+	return hid_hw_start(hdev, HID_CONNECT_DEFAULT);
+}
+
+static struct miscdevice acc_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "usb_accessory",
+	.fops = &acc_fops,
+};
+
+static const struct hid_device_id acc_hid_table[] = {
+	{ HID_USB_DEVICE(HID_ANY_ID, HID_ANY_ID) },
+	{ }
+};
+
+static struct hid_driver acc_hid_driver = {
+	.name = "USB accessory",
+	.id_table = acc_hid_table,
+	.probe = acc_hid_probe,
+};
+
+static int acc_ctrlrequest(struct usb_composite_dev *cdev,
+				const struct usb_ctrlrequest *ctrl)
+{
+	struct acc_dev	*dev = _acc_dev;
+	int	value = -EOPNOTSUPP;
+	struct acc_hid_dev *hid;
+	int offset;
+	u8 b_requestType = ctrl->bRequestType;
+	u8 b_request = ctrl->bRequest;
+	u16	w_index = le16_to_cpu(ctrl->wIndex);
+	u16	w_value = le16_to_cpu(ctrl->wValue);
+	u16	w_length = le16_to_cpu(ctrl->wLength);
+	unsigned long flags;
+
+/*
+	printk(KERN_INFO "acc_ctrlrequest "
+			"%02x.%02x v%04x i%04x l%u\n",
+			b_requestType, b_request,
+			w_value, w_index, w_length);
+*/
+
+	if (b_requestType == (USB_DIR_OUT | USB_TYPE_VENDOR)) {
+		if (b_request == ACCESSORY_START) {
+			dev->start_requested = 1;
+			schedule_delayed_work(
+				&dev->start_work, msecs_to_jiffies(10));
+			value = 0;
+		} else if (b_request == ACCESSORY_SEND_STRING) {
+			dev->string_index = w_index;
+			cdev->gadget->ep0->driver_data = dev;
+			cdev->req->complete = acc_complete_set_string;
+			value = w_length;
+		} else if (b_request == ACCESSORY_SET_AUDIO_MODE &&
+				w_index == 0 && w_length == 0) {
+			dev->audio_mode = w_value;
+			value = 0;
+		} else if (b_request == ACCESSORY_REGISTER_HID) {
+			value = acc_register_hid(dev, w_value, w_index);
+		} else if (b_request == ACCESSORY_UNREGISTER_HID) {
+			value = acc_unregister_hid(dev, w_value);
+		} else if (b_request == ACCESSORY_SET_HID_REPORT_DESC) {
+			spin_lock_irqsave(&dev->lock, flags);
+			hid = acc_hid_get(&dev->new_hid_list, w_value);
+			spin_unlock_irqrestore(&dev->lock, flags);
+			if (!hid) {
+				value = -EINVAL;
+				goto err;
+			}
+			offset = w_index;
+			if (offset != hid->report_desc_offset
+				|| offset + w_length > hid->report_desc_len) {
+				value = -EINVAL;
+				goto err;
+			}
+			cdev->req->context = hid;
+			cdev->req->complete = acc_complete_set_hid_report_desc;
+			value = w_length;
+		} else if (b_request == ACCESSORY_SEND_HID_EVENT) {
+			spin_lock_irqsave(&dev->lock, flags);
+			hid = acc_hid_get(&dev->hid_list, w_value);
+			spin_unlock_irqrestore(&dev->lock, flags);
+			if (!hid) {
+				value = -EINVAL;
+				goto err;
+			}
+			cdev->req->context = hid;
+			cdev->req->complete = acc_complete_send_hid_event;
+			value = w_length;
+		}
+	} else if (b_requestType == (USB_DIR_IN | USB_TYPE_VENDOR)) {
+		if (b_request == ACCESSORY_GET_PROTOCOL) {
+			*((u16 *)cdev->req->buf) = PROTOCOL_VERSION;
+			value = sizeof(u16);
+
+			/* clear strings left over from a previous session */
+			memset(dev->manufacturer, 0, sizeof(dev->manufacturer));
+			memset(dev->model, 0, sizeof(dev->model));
+			memset(dev->description, 0, sizeof(dev->description));
+			memset(dev->version, 0, sizeof(dev->version));
+			memset(dev->uri, 0, sizeof(dev->uri));
+			memset(dev->serial, 0, sizeof(dev->serial));
+			dev->start_requested = 0;
+			dev->audio_mode = 0;
+		}
+	}
+
+	if (value >= 0) {
+		cdev->req->zero = 0;
+		cdev->req->length = value;
+		value = usb_ep_queue(cdev->gadget->ep0, cdev->req, GFP_ATOMIC);
+		if (value < 0)
+			ERROR(cdev, "%s setup response queue error\n",
+				__func__);
+	}
+
+err:
+	if (value == -EOPNOTSUPP)
+		VDBG(cdev,
+			"unknown class-specific control req "
+			"%02x.%02x v%04x i%04x l%u\n",
+			ctrl->bRequestType, ctrl->bRequest,
+			w_value, w_index, w_length);
+	return value;
+}
+
+static int
+acc_function_bind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct usb_composite_dev *cdev = c->cdev;
+	struct acc_dev	*dev = func_to_dev(f);
+	int			id;
+	int			ret;
+
+	DBG(cdev, "acc_function_bind dev: %p\n", dev);
+
+	ret = hid_register_driver(&acc_hid_driver);
+	if (ret)
+		return ret;
+
+	dev->start_requested = 0;
+
+	/* allocate interface ID(s) */
+	id = usb_interface_id(c, f);
+	if (id < 0)
+		return id;
+	acc_interface_desc.bInterfaceNumber = id;
+
+	/* allocate endpoints */
+	ret = create_bulk_endpoints(dev, &acc_fullspeed_in_desc,
+			&acc_fullspeed_out_desc);
+	if (ret)
+		return ret;
+
+	/* support high speed hardware */
+	if (gadget_is_dualspeed(c->cdev->gadget)) {
+		acc_highspeed_in_desc.bEndpointAddress =
+			acc_fullspeed_in_desc.bEndpointAddress;
+		acc_highspeed_out_desc.bEndpointAddress =
+			acc_fullspeed_out_desc.bEndpointAddress;
+	}
+
+	DBG(cdev, "%s speed %s: IN/%s, OUT/%s\n",
+			gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full",
+			f->name, dev->ep_in->name, dev->ep_out->name);
+	return 0;
+}
+
+static void
+kill_all_hid_devices(struct acc_dev *dev)
+{
+	struct acc_hid_dev *hid;
+	struct list_head *entry, *temp;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	list_for_each_safe(entry, temp, &dev->hid_list) {
+		hid = list_entry(entry, struct acc_hid_dev, list);
+		list_del(&hid->list);
+		list_add(&hid->list, &dev->dead_hid_list);
+	}
+	list_for_each_safe(entry, temp, &dev->new_hid_list) {
+		hid = list_entry(entry, struct acc_hid_dev, list);
+		list_del(&hid->list);
+		list_add(&hid->list, &dev->dead_hid_list);
+	}
+	spin_unlock_irqrestore(&dev->lock, flags);
+
+	schedule_work(&dev->hid_work);
+}
+
+static void
+acc_hid_unbind(struct acc_dev *dev)
+{
+	hid_unregister_driver(&acc_hid_driver);
+	kill_all_hid_devices(dev);
+}
+
+static void
+acc_function_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct acc_dev	*dev = func_to_dev(f);
+	struct usb_request *req;
+	int i;
+
+	while ((req = req_get(dev, &dev->tx_idle)))
+		acc_request_free(req, dev->ep_in);
+	for (i = 0; i < RX_REQ_MAX; i++)
+		acc_request_free(dev->rx_req[i], dev->ep_out);
+
+	acc_hid_unbind(dev);
+}
+
+static void acc_start_work(struct work_struct *data)
+{
+	char *envp[2] = { "ACCESSORY=START", NULL };
+	kobject_uevent_env(&acc_device.this_device->kobj, KOBJ_CHANGE, envp);
+}
+
+static int acc_hid_init(struct acc_hid_dev *hdev)
+{
+	struct hid_device *hid;
+	int ret;
+
+	hid = hid_allocate_device();
+	if (IS_ERR(hid))
+		return PTR_ERR(hid);
+
+	hid->ll_driver = &acc_hid_ll_driver;
+	hid->dev.parent = acc_device.this_device;
+
+	hid->bus = BUS_USB;
+	hid->vendor = HID_ANY_ID;
+	hid->product = HID_ANY_ID;
+	hid->driver_data = hdev;
+	ret = hid_add_device(hid);
+	if (ret) {
+		pr_err("can't add hid device: %d\n", ret);
+		hid_destroy_device(hid);
+		return ret;
+	}
+
+	hdev->hid = hid;
+	return 0;
+}
+
+static void acc_hid_delete(struct acc_hid_dev *hid)
+{
+	kfree(hid->report_desc);
+	kfree(hid);
+}
+
+static void acc_hid_work(struct work_struct *data)
+{
+	struct acc_dev *dev = _acc_dev;
+	struct list_head	*entry, *temp;
+	struct acc_hid_dev *hid;
+	struct list_head	new_list, dead_list;
+	unsigned long flags;
+
+	INIT_LIST_HEAD(&new_list);
+
+	spin_lock_irqsave(&dev->lock, flags);
+
+	/* copy hids that are ready for initialization to new_list */
+	list_for_each_safe(entry, temp, &dev->new_hid_list) {
+		hid = list_entry(entry, struct acc_hid_dev, list);
+		if (hid->report_desc_offset == hid->report_desc_len)
+			list_move(&hid->list, &new_list);
+	}
+
+	if (list_empty(&dev->dead_hid_list)) {
+		INIT_LIST_HEAD(&dead_list);
+	} else {
+		/* move all of dev->dead_hid_list to dead_list */
+		dead_list.prev = dev->dead_hid_list.prev;
+		dead_list.next = dev->dead_hid_list.next;
+		dead_list.next->prev = &dead_list;
+		dead_list.prev->next = &dead_list;
+		INIT_LIST_HEAD(&dev->dead_hid_list);
+	}
+
+	spin_unlock_irqrestore(&dev->lock, flags);
+
+	/* register new HID devices */
+	list_for_each_safe(entry, temp, &new_list) {
+		hid = list_entry(entry, struct acc_hid_dev, list);
+		if (acc_hid_init(hid)) {
+			pr_err("can't add HID device %p\n", hid);
+			acc_hid_delete(hid);
+		} else {
+			spin_lock_irqsave(&dev->lock, flags);
+			list_move(&hid->list, &dev->hid_list);
+			spin_unlock_irqrestore(&dev->lock, flags);
+		}
+	}
+
+	/* remove dead HID devices */
+	list_for_each_safe(entry, temp, &dead_list) {
+		hid = list_entry(entry, struct acc_hid_dev, list);
+		list_del(&hid->list);
+		if (hid->hid)
+			hid_destroy_device(hid->hid);
+		acc_hid_delete(hid);
+	}
+}
+
+static int acc_function_set_alt(struct usb_function *f,
+		unsigned intf, unsigned alt)
+{
+	struct acc_dev	*dev = func_to_dev(f);
+	struct usb_composite_dev *cdev = f->config->cdev;
+	int ret;
+
+	DBG(cdev, "acc_function_set_alt intf: %d alt: %d\n", intf, alt);
+	config_ep_by_speed(cdev->gadget, f, dev->ep_in);
+	ret = usb_ep_enable(dev->ep_in);
+	if (ret)
+		return ret;
+	config_ep_by_speed(cdev->gadget, f, dev->ep_out);
+	ret = usb_ep_enable(dev->ep_out);
+	if (ret) {
+		usb_ep_disable(dev->ep_in);
+		return ret;
+	}
+
+	dev->online = 1;
+
+	/* readers may be blocked waiting for us to go online */
+	wake_up(&dev->read_wq);
+	return 0;
+}
+
+static void acc_function_disable(struct usb_function *f)
+{
+	struct acc_dev	*dev = func_to_dev(f);
+	struct usb_composite_dev	*cdev = dev->cdev;
+
+	DBG(cdev, "acc_function_disable\n");
+	acc_set_disconnected(dev);
+	usb_ep_disable(dev->ep_in);
+	usb_ep_disable(dev->ep_out);
+
+	/* readers may be blocked waiting for us to go online */
+	wake_up(&dev->read_wq);
+
+	VDBG(cdev, "%s disabled\n", dev->function.name);
+}
+
+static int acc_bind_config(struct usb_configuration *c)
+{
+	struct acc_dev *dev = _acc_dev;
+	int ret;
+
+	printk(KERN_INFO "acc_bind_config\n");
+
+	/* allocate a string ID for our interface */
+	if (acc_string_defs[INTERFACE_STRING_INDEX].id == 0) {
+		ret = usb_string_id(c->cdev);
+		if (ret < 0)
+			return ret;
+		acc_string_defs[INTERFACE_STRING_INDEX].id = ret;
+		acc_interface_desc.iInterface = ret;
+	}
+
+	dev->cdev = c->cdev;
+	dev->function.name = "accessory";
+	dev->function.strings = acc_strings,
+	dev->function.descriptors = fs_acc_descs;
+	dev->function.hs_descriptors = hs_acc_descs;
+	dev->function.bind = acc_function_bind;
+	dev->function.unbind = acc_function_unbind;
+	dev->function.set_alt = acc_function_set_alt;
+	dev->function.disable = acc_function_disable;
+
+	return usb_add_function(c, &dev->function);
+}
+
+static int acc_setup(void)
+{
+	struct acc_dev *dev;
+	int ret;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	spin_lock_init(&dev->lock);
+	init_waitqueue_head(&dev->read_wq);
+	init_waitqueue_head(&dev->write_wq);
+	atomic_set(&dev->open_excl, 0);
+	INIT_LIST_HEAD(&dev->tx_idle);
+	INIT_LIST_HEAD(&dev->hid_list);
+	INIT_LIST_HEAD(&dev->new_hid_list);
+	INIT_LIST_HEAD(&dev->dead_hid_list);
+	INIT_DELAYED_WORK(&dev->start_work, acc_start_work);
+	INIT_WORK(&dev->hid_work, acc_hid_work);
+
+	/* _acc_dev must be set before calling usb_gadget_register_driver */
+	_acc_dev = dev;
+
+	ret = misc_register(&acc_device);
+	if (ret)
+		goto err;
+
+	return 0;
+
+err:
+	kfree(dev);
+	pr_err("USB accessory gadget driver failed to initialize\n");
+	return ret;
+}
+
+static void acc_disconnect(void)
+{
+	/* unregister all HID devices if USB is disconnected */
+	kill_all_hid_devices(_acc_dev);
+}
+
+static void acc_cleanup(void)
+{
+	misc_deregister(&acc_device);
+	kfree(_acc_dev);
+	_acc_dev = NULL;
+}
diff --git a/drivers/usb/gadget/f_acm.c b/drivers/usb/gadget/f_acm.c
index 3f8849339ade..d84debc42215 100644
--- a/drivers/usb/gadget/f_acm.c
+++ b/drivers/usb/gadget/f_acm.c
@@ -677,6 +677,7 @@ acm_unbind(struct usb_configuration *c, struct usb_function *f)
 		usb_free_descriptors(f->hs_descriptors);
 	usb_free_descriptors(f->descriptors);
 	gs_free_req(acm->notify, acm->notify_req);
+	kfree(acm->port.func.name);
 	kfree(acm);
 }
 
@@ -748,7 +749,11 @@ int acm_bind_config(struct usb_configuration *c, u8 port_num)
 	acm->port.disconnect = acm_disconnect;
 	acm->port.send_break = acm_send_break;
 
-	acm->port.func.name = "acm";
+	acm->port.func.name = kasprintf(GFP_KERNEL, "acm%u", port_num);
+	if (!acm->port.func.name) {
+		kfree(acm);
+		return -ENOMEM;
+	}
 	acm->port.func.strings = acm_strings;
 	/* descriptors are per-instance copies */
 	acm->port.func.bind = acm_bind;
diff --git a/drivers/usb/gadget/f_adb.c b/drivers/usb/gadget/f_adb.c
new file mode 100644
index 000000000000..94a793f43901
--- /dev/null
+++ b/drivers/usb/gadget/f_adb.c
@@ -0,0 +1,635 @@
+/*
+ * Gadget Driver for Android ADB
+ *
+ * Copyright (C) 2008 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/module.h>
+#include <linux/init.h>
+#include <linux/poll.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+
+#define ADB_BULK_BUFFER_SIZE           4096
+
+/* number of tx requests to allocate */
+#define TX_REQ_MAX 4
+
+static const char adb_shortname[] = "android_adb";
+
+struct adb_dev {
+	struct usb_function function;
+	struct usb_composite_dev *cdev;
+	spinlock_t lock;
+
+	struct usb_ep *ep_in;
+	struct usb_ep *ep_out;
+
+	int online;
+	int error;
+
+	atomic_t read_excl;
+	atomic_t write_excl;
+	atomic_t open_excl;
+
+	struct list_head tx_idle;
+
+	wait_queue_head_t read_wq;
+	wait_queue_head_t write_wq;
+	struct usb_request *rx_req;
+	int rx_done;
+};
+
+static struct usb_interface_descriptor adb_interface_desc = {
+	.bLength                = USB_DT_INTERFACE_SIZE,
+	.bDescriptorType        = USB_DT_INTERFACE,
+	.bInterfaceNumber       = 0,
+	.bNumEndpoints          = 2,
+	.bInterfaceClass        = 0xFF,
+	.bInterfaceSubClass     = 0x42,
+	.bInterfaceProtocol     = 1,
+};
+
+static struct usb_endpoint_descriptor adb_highspeed_in_desc = {
+	.bLength                = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType        = USB_DT_ENDPOINT,
+	.bEndpointAddress       = USB_DIR_IN,
+	.bmAttributes           = USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize         = __constant_cpu_to_le16(512),
+};
+
+static struct usb_endpoint_descriptor adb_highspeed_out_desc = {
+	.bLength                = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType        = USB_DT_ENDPOINT,
+	.bEndpointAddress       = USB_DIR_OUT,
+	.bmAttributes           = USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize         = __constant_cpu_to_le16(512),
+};
+
+static struct usb_endpoint_descriptor adb_fullspeed_in_desc = {
+	.bLength                = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType        = USB_DT_ENDPOINT,
+	.bEndpointAddress       = USB_DIR_IN,
+	.bmAttributes           = USB_ENDPOINT_XFER_BULK,
+};
+
+static struct usb_endpoint_descriptor adb_fullspeed_out_desc = {
+	.bLength                = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType        = USB_DT_ENDPOINT,
+	.bEndpointAddress       = USB_DIR_OUT,
+	.bmAttributes           = USB_ENDPOINT_XFER_BULK,
+};
+
+static struct usb_descriptor_header *fs_adb_descs[] = {
+	(struct usb_descriptor_header *) &adb_interface_desc,
+	(struct usb_descriptor_header *) &adb_fullspeed_in_desc,
+	(struct usb_descriptor_header *) &adb_fullspeed_out_desc,
+	NULL,
+};
+
+static struct usb_descriptor_header *hs_adb_descs[] = {
+	(struct usb_descriptor_header *) &adb_interface_desc,
+	(struct usb_descriptor_header *) &adb_highspeed_in_desc,
+	(struct usb_descriptor_header *) &adb_highspeed_out_desc,
+	NULL,
+};
+
+
+/* temporary variable used between adb_open() and adb_gadget_bind() */
+static struct adb_dev *_adb_dev;
+
+static inline struct adb_dev *func_to_adb(struct usb_function *f)
+{
+	return container_of(f, struct adb_dev, function);
+}
+
+
+static struct usb_request *adb_request_new(struct usb_ep *ep, int buffer_size)
+{
+	struct usb_request *req = usb_ep_alloc_request(ep, GFP_KERNEL);
+	if (!req)
+		return NULL;
+
+	/* now allocate buffers for the requests */
+	req->buf = kmalloc(buffer_size, GFP_KERNEL);
+	if (!req->buf) {
+		usb_ep_free_request(ep, req);
+		return NULL;
+	}
+
+	return req;
+}
+
+static void adb_request_free(struct usb_request *req, struct usb_ep *ep)
+{
+	if (req) {
+		kfree(req->buf);
+		usb_ep_free_request(ep, req);
+	}
+}
+
+static inline int adb_lock(atomic_t *excl)
+{
+	int ret = -1;
+
+	preempt_disable();
+	if (atomic_inc_return(excl) == 1) {
+		ret = 0;
+	} else
+		atomic_dec(excl);
+
+	preempt_enable();
+	return ret;
+}
+
+static inline void adb_unlock(atomic_t *excl)
+{
+	atomic_dec(excl);
+}
+
+/* add a request to the tail of a list */
+void adb_req_put(struct adb_dev *dev, struct list_head *head,
+		struct usb_request *req)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	list_add_tail(&req->list, head);
+	spin_unlock_irqrestore(&dev->lock, flags);
+}
+
+/* remove a request from the head of a list */
+struct usb_request *adb_req_get(struct adb_dev *dev, struct list_head *head)
+{
+	unsigned long flags;
+	struct usb_request *req;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	if (list_empty(head)) {
+		req = 0;
+	} else {
+		req = list_first_entry(head, struct usb_request, list);
+		list_del(&req->list);
+	}
+	spin_unlock_irqrestore(&dev->lock, flags);
+	return req;
+}
+
+static void adb_complete_in(struct usb_ep *ep, struct usb_request *req)
+{
+	struct adb_dev *dev = _adb_dev;
+
+	if (req->status != 0)
+		dev->error = 1;
+
+	adb_req_put(dev, &dev->tx_idle, req);
+
+	wake_up(&dev->write_wq);
+}
+
+static void adb_complete_out(struct usb_ep *ep, struct usb_request *req)
+{
+	struct adb_dev *dev = _adb_dev;
+
+	dev->rx_done = 1;
+	if (req->status != 0)
+		dev->error = 1;
+
+	wake_up(&dev->read_wq);
+}
+
+static int adb_create_bulk_endpoints(struct adb_dev *dev,
+				struct usb_endpoint_descriptor *in_desc,
+				struct usb_endpoint_descriptor *out_desc)
+{
+	struct usb_composite_dev *cdev = dev->cdev;
+	struct usb_request *req;
+	struct usb_ep *ep;
+	int i;
+
+	DBG(cdev, "create_bulk_endpoints dev: %p\n", dev);
+
+	ep = usb_ep_autoconfig(cdev->gadget, in_desc);
+	if (!ep) {
+		DBG(cdev, "usb_ep_autoconfig for ep_in failed\n");
+		return -ENODEV;
+	}
+	DBG(cdev, "usb_ep_autoconfig for ep_in got %s\n", ep->name);
+	ep->driver_data = dev;		/* claim the endpoint */
+	dev->ep_in = ep;
+
+	ep = usb_ep_autoconfig(cdev->gadget, out_desc);
+	if (!ep) {
+		DBG(cdev, "usb_ep_autoconfig for ep_out failed\n");
+		return -ENODEV;
+	}
+	DBG(cdev, "usb_ep_autoconfig for adb ep_out got %s\n", ep->name);
+	ep->driver_data = dev;		/* claim the endpoint */
+	dev->ep_out = ep;
+
+	/* now allocate requests for our endpoints */
+	req = adb_request_new(dev->ep_out, ADB_BULK_BUFFER_SIZE);
+	if (!req)
+		goto fail;
+	req->complete = adb_complete_out;
+	dev->rx_req = req;
+
+	for (i = 0; i < TX_REQ_MAX; i++) {
+		req = adb_request_new(dev->ep_in, ADB_BULK_BUFFER_SIZE);
+		if (!req)
+			goto fail;
+		req->complete = adb_complete_in;
+		adb_req_put(dev, &dev->tx_idle, req);
+	}
+
+	return 0;
+
+fail:
+	printk(KERN_ERR "adb_bind() could not allocate requests\n");
+	return -1;
+}
+
+static ssize_t adb_read(struct file *fp, char __user *buf,
+				size_t count, loff_t *pos)
+{
+	struct adb_dev *dev = fp->private_data;
+	struct usb_request *req;
+	int r = count, xfer;
+	int ret;
+
+	pr_debug("adb_read(%d)\n", count);
+	if (!_adb_dev)
+		return -ENODEV;
+
+	if (count > ADB_BULK_BUFFER_SIZE)
+		return -EINVAL;
+
+	if (adb_lock(&dev->read_excl))
+		return -EBUSY;
+
+	/* we will block until we're online */
+	while (!(dev->online || dev->error)) {
+		pr_debug("adb_read: waiting for online state\n");
+		ret = wait_event_interruptible(dev->read_wq,
+				(dev->online || dev->error));
+		if (ret < 0) {
+			adb_unlock(&dev->read_excl);
+			return ret;
+		}
+	}
+	if (dev->error) {
+		r = -EIO;
+		goto done;
+	}
+
+requeue_req:
+	/* queue a request */
+	req = dev->rx_req;
+	req->length = count;
+	dev->rx_done = 0;
+	ret = usb_ep_queue(dev->ep_out, req, GFP_ATOMIC);
+	if (ret < 0) {
+		pr_debug("adb_read: failed to queue req %p (%d)\n", req, ret);
+		r = -EIO;
+		dev->error = 1;
+		goto done;
+	} else {
+		pr_debug("rx %p queue\n", req);
+	}
+
+	/* wait for a request to complete */
+	ret = wait_event_interruptible(dev->read_wq, dev->rx_done);
+	if (ret < 0) {
+		dev->error = 1;
+		r = ret;
+		usb_ep_dequeue(dev->ep_out, req);
+		goto done;
+	}
+	if (!dev->error) {
+		/* If we got a 0-len packet, throw it back and try again. */
+		if (req->actual == 0)
+			goto requeue_req;
+
+		pr_debug("rx %p %d\n", req, req->actual);
+		xfer = (req->actual < count) ? req->actual : count;
+		if (copy_to_user(buf, req->buf, xfer))
+			r = -EFAULT;
+
+	} else
+		r = -EIO;
+
+done:
+	adb_unlock(&dev->read_excl);
+	pr_debug("adb_read returning %d\n", r);
+	return r;
+}
+
+static ssize_t adb_write(struct file *fp, const char __user *buf,
+				 size_t count, loff_t *pos)
+{
+	struct adb_dev *dev = fp->private_data;
+	struct usb_request *req = 0;
+	int r = count, xfer;
+	int ret;
+
+	if (!_adb_dev)
+		return -ENODEV;
+	pr_debug("adb_write(%d)\n", count);
+
+	if (adb_lock(&dev->write_excl))
+		return -EBUSY;
+
+	while (count > 0) {
+		if (dev->error) {
+			pr_debug("adb_write dev->error\n");
+			r = -EIO;
+			break;
+		}
+
+		/* get an idle tx request to use */
+		req = 0;
+		ret = wait_event_interruptible(dev->write_wq,
+			(req = adb_req_get(dev, &dev->tx_idle)) || dev->error);
+
+		if (ret < 0) {
+			r = ret;
+			break;
+		}
+
+		if (req != 0) {
+			if (count > ADB_BULK_BUFFER_SIZE)
+				xfer = ADB_BULK_BUFFER_SIZE;
+			else
+				xfer = count;
+			if (copy_from_user(req->buf, buf, xfer)) {
+				r = -EFAULT;
+				break;
+			}
+
+			req->length = xfer;
+			ret = usb_ep_queue(dev->ep_in, req, GFP_ATOMIC);
+			if (ret < 0) {
+				pr_debug("adb_write: xfer error %d\n", ret);
+				dev->error = 1;
+				r = -EIO;
+				break;
+			}
+
+			buf += xfer;
+			count -= xfer;
+
+			/* zero this so we don't try to free it on error exit */
+			req = 0;
+		}
+	}
+
+	if (req)
+		adb_req_put(dev, &dev->tx_idle, req);
+
+	adb_unlock(&dev->write_excl);
+	pr_debug("adb_write returning %d\n", r);
+	return r;
+}
+
+static int adb_open(struct inode *ip, struct file *fp)
+{
+	static unsigned long last_print;
+	static unsigned long count = 0;
+
+	if (!_adb_dev)
+		return -ENODEV;
+
+	if (++count == 1)
+		last_print = jiffies;
+	else {
+		if (!time_before(jiffies, last_print + HZ/2))
+			count = 0;
+		last_print = jiffies;
+	}
+
+	if (adb_lock(&_adb_dev->open_excl)) {
+		cpu_relax();
+		return -EBUSY;
+	}
+
+	if (count < 5)
+		printk(KERN_INFO "adb_open(%s)\n", current->comm);
+
+
+	fp->private_data = _adb_dev;
+
+	/* clear the error latch */
+	_adb_dev->error = 0;
+
+	return 0;
+}
+
+static int adb_release(struct inode *ip, struct file *fp)
+{
+	static unsigned long last_print;
+	static unsigned long count = 0;
+
+	if (++count == 1)
+		last_print = jiffies;
+	else {
+		if (!time_before(jiffies, last_print + HZ/2))
+			count = 0;
+		last_print = jiffies;
+	}
+
+	if (count < 5)
+		printk(KERN_INFO "adb_release\n");
+	adb_unlock(&_adb_dev->open_excl);
+	return 0;
+}
+
+/* file operations for ADB device /dev/android_adb */
+static struct file_operations adb_fops = {
+	.owner = THIS_MODULE,
+	.read = adb_read,
+	.write = adb_write,
+	.open = adb_open,
+	.release = adb_release,
+};
+
+static struct miscdevice adb_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = adb_shortname,
+	.fops = &adb_fops,
+};
+
+
+
+
+static int
+adb_function_bind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct usb_composite_dev *cdev = c->cdev;
+	struct adb_dev	*dev = func_to_adb(f);
+	int			id;
+	int			ret;
+
+	dev->cdev = cdev;
+	DBG(cdev, "adb_function_bind dev: %p\n", dev);
+
+	/* allocate interface ID(s) */
+	id = usb_interface_id(c, f);
+	if (id < 0)
+		return id;
+	adb_interface_desc.bInterfaceNumber = id;
+
+	/* allocate endpoints */
+	ret = adb_create_bulk_endpoints(dev, &adb_fullspeed_in_desc,
+			&adb_fullspeed_out_desc);
+	if (ret)
+		return ret;
+
+	/* support high speed hardware */
+	if (gadget_is_dualspeed(c->cdev->gadget)) {
+		adb_highspeed_in_desc.bEndpointAddress =
+			adb_fullspeed_in_desc.bEndpointAddress;
+		adb_highspeed_out_desc.bEndpointAddress =
+			adb_fullspeed_out_desc.bEndpointAddress;
+	}
+
+	DBG(cdev, "%s speed %s: IN/%s, OUT/%s\n",
+			gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full",
+			f->name, dev->ep_in->name, dev->ep_out->name);
+	return 0;
+}
+
+static void
+adb_function_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct adb_dev	*dev = func_to_adb(f);
+	struct usb_request *req;
+
+
+	dev->online = 0;
+	dev->error = 1;
+
+	wake_up(&dev->read_wq);
+
+	adb_request_free(dev->rx_req, dev->ep_out);
+	while ((req = adb_req_get(dev, &dev->tx_idle)))
+		adb_request_free(req, dev->ep_in);
+}
+
+static int adb_function_set_alt(struct usb_function *f,
+		unsigned intf, unsigned alt)
+{
+	struct adb_dev	*dev = func_to_adb(f);
+	struct usb_composite_dev *cdev = f->config->cdev;
+	int ret;
+
+	DBG(cdev, "adb_function_set_alt intf: %d alt: %d\n", intf, alt);
+	config_ep_by_speed(cdev->gadget, f, dev->ep_in);
+	ret = usb_ep_enable(dev->ep_in);
+	if (ret)
+		return ret;
+	config_ep_by_speed(cdev->gadget, f, dev->ep_out);
+	ret = usb_ep_enable(dev->ep_out);
+	if (ret) {
+		usb_ep_disable(dev->ep_in);
+		return ret;
+	}
+	dev->online = 1;
+
+	/* readers may be blocked waiting for us to go online */
+	wake_up(&dev->read_wq);
+	return 0;
+}
+
+static void adb_function_disable(struct usb_function *f)
+{
+	struct adb_dev	*dev = func_to_adb(f);
+	struct usb_composite_dev	*cdev = dev->cdev;
+
+	DBG(cdev, "adb_function_disable cdev %p\n", cdev);
+	dev->online = 0;
+	dev->error = 1;
+	usb_ep_disable(dev->ep_in);
+	usb_ep_disable(dev->ep_out);
+
+	/* readers may be blocked waiting for us to go online */
+	wake_up(&dev->read_wq);
+
+	VDBG(cdev, "%s disabled\n", dev->function.name);
+}
+
+static int adb_bind_config(struct usb_configuration *c)
+{
+	struct adb_dev *dev = _adb_dev;
+
+	printk(KERN_INFO "adb_bind_config\n");
+
+	dev->cdev = c->cdev;
+	dev->function.name = "adb";
+	dev->function.descriptors = fs_adb_descs;
+	dev->function.hs_descriptors = hs_adb_descs;
+	dev->function.bind = adb_function_bind;
+	dev->function.unbind = adb_function_unbind;
+	dev->function.set_alt = adb_function_set_alt;
+	dev->function.disable = adb_function_disable;
+
+	return usb_add_function(c, &dev->function);
+}
+
+static int adb_setup(void)
+{
+	struct adb_dev *dev;
+	int ret;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	spin_lock_init(&dev->lock);
+
+	init_waitqueue_head(&dev->read_wq);
+	init_waitqueue_head(&dev->write_wq);
+
+	atomic_set(&dev->open_excl, 0);
+	atomic_set(&dev->read_excl, 0);
+	atomic_set(&dev->write_excl, 0);
+
+	INIT_LIST_HEAD(&dev->tx_idle);
+
+	_adb_dev = dev;
+
+	ret = misc_register(&adb_device);
+	if (ret)
+		goto err;
+
+	return 0;
+
+err:
+	kfree(dev);
+	printk(KERN_ERR "adb gadget driver failed to initialize\n");
+	return ret;
+}
+
+static void adb_cleanup(void)
+{
+	misc_deregister(&adb_device);
+
+	kfree(_adb_dev);
+	_adb_dev = NULL;
+}
diff --git a/drivers/usb/gadget/f_audio_source.c b/drivers/usb/gadget/f_audio_source.c
new file mode 100644
index 000000000000..3ba7d7569b12
--- /dev/null
+++ b/drivers/usb/gadget/f_audio_source.c
@@ -0,0 +1,827 @@
+/*
+ * Gadget Function Driver for USB audio source device
+ *
+ * Copyright (C) 2012 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/device.h>
+#include <linux/usb/audio.h>
+#include <linux/wait.h>
+#include <sound/core.h>
+#include <sound/initval.h>
+#include <sound/pcm.h>
+
+#define SAMPLE_RATE 44100
+/* Each frame is two 16 bit integers (one per channel) */
+#define BYTES_PER_FRAME 4
+#define FRAMES_PER_MSEC (SAMPLE_RATE / 1000)
+
+#define IN_EP_MAX_PACKET_SIZE 256
+
+/* Number of requests to allocate */
+#define IN_EP_REQ_COUNT 4
+
+#define AUDIO_AC_INTERFACE	0
+#define AUDIO_AS_INTERFACE	1
+#define AUDIO_NUM_INTERFACES	2
+
+/* B.3.1  Standard AC Interface Descriptor */
+static struct usb_interface_descriptor ac_interface_desc = {
+	.bLength =		USB_DT_INTERFACE_SIZE,
+	.bDescriptorType =	USB_DT_INTERFACE,
+	.bNumEndpoints =	0,
+	.bInterfaceClass =	USB_CLASS_AUDIO,
+	.bInterfaceSubClass =	USB_SUBCLASS_AUDIOCONTROL,
+};
+
+DECLARE_UAC_AC_HEADER_DESCRIPTOR(2);
+
+#define UAC_DT_AC_HEADER_LENGTH	UAC_DT_AC_HEADER_SIZE(AUDIO_NUM_INTERFACES)
+/* 1 input terminal, 1 output terminal and 1 feature unit */
+#define UAC_DT_TOTAL_LENGTH (UAC_DT_AC_HEADER_LENGTH \
+	+ UAC_DT_INPUT_TERMINAL_SIZE + UAC_DT_OUTPUT_TERMINAL_SIZE \
+	+ UAC_DT_FEATURE_UNIT_SIZE(0))
+/* B.3.2  Class-Specific AC Interface Descriptor */
+static struct uac1_ac_header_descriptor_2 ac_header_desc = {
+	.bLength =		UAC_DT_AC_HEADER_LENGTH,
+	.bDescriptorType =	USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype =	UAC_HEADER,
+	.bcdADC =		__constant_cpu_to_le16(0x0100),
+	.wTotalLength =		__constant_cpu_to_le16(UAC_DT_TOTAL_LENGTH),
+	.bInCollection =	AUDIO_NUM_INTERFACES,
+	.baInterfaceNr = {
+		[0] =		AUDIO_AC_INTERFACE,
+		[1] =		AUDIO_AS_INTERFACE,
+	}
+};
+
+#define INPUT_TERMINAL_ID	1
+static struct uac_input_terminal_descriptor input_terminal_desc = {
+	.bLength =		UAC_DT_INPUT_TERMINAL_SIZE,
+	.bDescriptorType =	USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype =	UAC_INPUT_TERMINAL,
+	.bTerminalID =		INPUT_TERMINAL_ID,
+	.wTerminalType =	UAC_INPUT_TERMINAL_MICROPHONE,
+	.bAssocTerminal =	0,
+	.wChannelConfig =	0x3,
+};
+
+DECLARE_UAC_FEATURE_UNIT_DESCRIPTOR(0);
+
+#define FEATURE_UNIT_ID		2
+static struct uac_feature_unit_descriptor_0 feature_unit_desc = {
+	.bLength		= UAC_DT_FEATURE_UNIT_SIZE(0),
+	.bDescriptorType	= USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype	= UAC_FEATURE_UNIT,
+	.bUnitID		= FEATURE_UNIT_ID,
+	.bSourceID		= INPUT_TERMINAL_ID,
+	.bControlSize		= 2,
+};
+
+#define OUTPUT_TERMINAL_ID	3
+static struct uac1_output_terminal_descriptor output_terminal_desc = {
+	.bLength		= UAC_DT_OUTPUT_TERMINAL_SIZE,
+	.bDescriptorType	= USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype	= UAC_OUTPUT_TERMINAL,
+	.bTerminalID		= OUTPUT_TERMINAL_ID,
+	.wTerminalType		= UAC_TERMINAL_STREAMING,
+	.bAssocTerminal		= FEATURE_UNIT_ID,
+	.bSourceID		= FEATURE_UNIT_ID,
+};
+
+/* B.4.1  Standard AS Interface Descriptor */
+static struct usb_interface_descriptor as_interface_alt_0_desc = {
+	.bLength =		USB_DT_INTERFACE_SIZE,
+	.bDescriptorType =	USB_DT_INTERFACE,
+	.bAlternateSetting =	0,
+	.bNumEndpoints =	0,
+	.bInterfaceClass =	USB_CLASS_AUDIO,
+	.bInterfaceSubClass =	USB_SUBCLASS_AUDIOSTREAMING,
+};
+
+static struct usb_interface_descriptor as_interface_alt_1_desc = {
+	.bLength =		USB_DT_INTERFACE_SIZE,
+	.bDescriptorType =	USB_DT_INTERFACE,
+	.bAlternateSetting =	1,
+	.bNumEndpoints =	1,
+	.bInterfaceClass =	USB_CLASS_AUDIO,
+	.bInterfaceSubClass =	USB_SUBCLASS_AUDIOSTREAMING,
+};
+
+/* B.4.2  Class-Specific AS Interface Descriptor */
+static struct uac1_as_header_descriptor as_header_desc = {
+	.bLength =		UAC_DT_AS_HEADER_SIZE,
+	.bDescriptorType =	USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype =	UAC_AS_GENERAL,
+	.bTerminalLink =	INPUT_TERMINAL_ID,
+	.bDelay =		1,
+	.wFormatTag =		UAC_FORMAT_TYPE_I_PCM,
+};
+
+DECLARE_UAC_FORMAT_TYPE_I_DISCRETE_DESC(1);
+
+static struct uac_format_type_i_discrete_descriptor_1 as_type_i_desc = {
+	.bLength =		UAC_FORMAT_TYPE_I_DISCRETE_DESC_SIZE(1),
+	.bDescriptorType =	USB_DT_CS_INTERFACE,
+	.bDescriptorSubtype =	UAC_FORMAT_TYPE,
+	.bFormatType =		UAC_FORMAT_TYPE_I,
+	.bSubframeSize =	2,
+	.bBitResolution =	16,
+	.bSamFreqType =		1,
+};
+
+/* Standard ISO IN Endpoint Descriptor for highspeed */
+static struct usb_endpoint_descriptor hs_as_in_ep_desc  = {
+	.bLength =		USB_DT_ENDPOINT_AUDIO_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_SYNC_SYNC
+				| USB_ENDPOINT_XFER_ISOC,
+	.wMaxPacketSize =	__constant_cpu_to_le16(IN_EP_MAX_PACKET_SIZE),
+	.bInterval =		4, /* poll 1 per millisecond */
+};
+
+/* Standard ISO IN Endpoint Descriptor for highspeed */
+static struct usb_endpoint_descriptor fs_as_in_ep_desc  = {
+	.bLength =		USB_DT_ENDPOINT_AUDIO_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_SYNC_SYNC
+				| USB_ENDPOINT_XFER_ISOC,
+	.wMaxPacketSize =	__constant_cpu_to_le16(IN_EP_MAX_PACKET_SIZE),
+	.bInterval =		1, /* poll 1 per millisecond */
+};
+
+/* Class-specific AS ISO OUT Endpoint Descriptor */
+static struct uac_iso_endpoint_descriptor as_iso_in_desc = {
+	.bLength =		UAC_ISO_ENDPOINT_DESC_SIZE,
+	.bDescriptorType =	USB_DT_CS_ENDPOINT,
+	.bDescriptorSubtype =	UAC_EP_GENERAL,
+	.bmAttributes =		1,
+	.bLockDelayUnits =	1,
+	.wLockDelay =		__constant_cpu_to_le16(1),
+};
+
+static struct usb_descriptor_header *hs_audio_desc[] = {
+	(struct usb_descriptor_header *)&ac_interface_desc,
+	(struct usb_descriptor_header *)&ac_header_desc,
+
+	(struct usb_descriptor_header *)&input_terminal_desc,
+	(struct usb_descriptor_header *)&output_terminal_desc,
+	(struct usb_descriptor_header *)&feature_unit_desc,
+
+	(struct usb_descriptor_header *)&as_interface_alt_0_desc,
+	(struct usb_descriptor_header *)&as_interface_alt_1_desc,
+	(struct usb_descriptor_header *)&as_header_desc,
+
+	(struct usb_descriptor_header *)&as_type_i_desc,
+
+	(struct usb_descriptor_header *)&hs_as_in_ep_desc,
+	(struct usb_descriptor_header *)&as_iso_in_desc,
+	NULL,
+};
+
+static struct usb_descriptor_header *fs_audio_desc[] = {
+	(struct usb_descriptor_header *)&ac_interface_desc,
+	(struct usb_descriptor_header *)&ac_header_desc,
+
+	(struct usb_descriptor_header *)&input_terminal_desc,
+	(struct usb_descriptor_header *)&output_terminal_desc,
+	(struct usb_descriptor_header *)&feature_unit_desc,
+
+	(struct usb_descriptor_header *)&as_interface_alt_0_desc,
+	(struct usb_descriptor_header *)&as_interface_alt_1_desc,
+	(struct usb_descriptor_header *)&as_header_desc,
+
+	(struct usb_descriptor_header *)&as_type_i_desc,
+
+	(struct usb_descriptor_header *)&fs_as_in_ep_desc,
+	(struct usb_descriptor_header *)&as_iso_in_desc,
+	NULL,
+};
+
+static struct snd_pcm_hardware audio_hw_info = {
+	.info =			SNDRV_PCM_INFO_MMAP |
+				SNDRV_PCM_INFO_MMAP_VALID |
+				SNDRV_PCM_INFO_BATCH |
+				SNDRV_PCM_INFO_INTERLEAVED |
+				SNDRV_PCM_INFO_BLOCK_TRANSFER,
+
+	.formats		= SNDRV_PCM_FMTBIT_S16_LE,
+	.channels_min		= 2,
+	.channels_max		= 2,
+	.rate_min		= SAMPLE_RATE,
+	.rate_max		= SAMPLE_RATE,
+
+	.buffer_bytes_max =	1024 * 1024,
+	.period_bytes_min =	64,
+	.period_bytes_max =	512 * 1024,
+	.periods_min =		2,
+	.periods_max =		1024,
+};
+
+/*-------------------------------------------------------------------------*/
+
+struct audio_source_config {
+	int	card;
+	int	device;
+};
+
+struct audio_dev {
+	struct usb_function		func;
+	struct snd_card			*card;
+	struct snd_pcm			*pcm;
+	struct snd_pcm_substream *substream;
+
+	struct list_head		idle_reqs;
+	struct usb_ep			*in_ep;
+	struct usb_endpoint_descriptor	*in_desc;
+
+	spinlock_t			lock;
+
+	/* beginning, end and current position in our buffer */
+	void				*buffer_start;
+	void				*buffer_end;
+	void				*buffer_pos;
+
+	/* byte size of a "period" */
+	unsigned int			period;
+	/* bytes sent since last call to snd_pcm_period_elapsed */
+	unsigned int			period_offset;
+	/* time we started playing */
+	ktime_t				start_time;
+	/* number of frames sent since start_time */
+	s64				frames_sent;
+};
+
+static inline struct audio_dev *func_to_audio(struct usb_function *f)
+{
+	return container_of(f, struct audio_dev, func);
+}
+
+/*-------------------------------------------------------------------------*/
+
+static struct usb_request *audio_request_new(struct usb_ep *ep, int buffer_size)
+{
+	struct usb_request *req = usb_ep_alloc_request(ep, GFP_KERNEL);
+	if (!req)
+		return NULL;
+
+	req->buf = kmalloc(buffer_size, GFP_KERNEL);
+	if (!req->buf) {
+		usb_ep_free_request(ep, req);
+		return NULL;
+	}
+	req->length = buffer_size;
+	return req;
+}
+
+static void audio_request_free(struct usb_request *req, struct usb_ep *ep)
+{
+	if (req) {
+		kfree(req->buf);
+		usb_ep_free_request(ep, req);
+	}
+}
+
+static void audio_req_put(struct audio_dev *audio, struct usb_request *req)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&audio->lock, flags);
+	list_add_tail(&req->list, &audio->idle_reqs);
+	spin_unlock_irqrestore(&audio->lock, flags);
+}
+
+static struct usb_request *audio_req_get(struct audio_dev *audio)
+{
+	unsigned long flags;
+	struct usb_request *req;
+
+	spin_lock_irqsave(&audio->lock, flags);
+	if (list_empty(&audio->idle_reqs)) {
+		req = 0;
+	} else {
+		req = list_first_entry(&audio->idle_reqs, struct usb_request,
+				list);
+		list_del(&req->list);
+	}
+	spin_unlock_irqrestore(&audio->lock, flags);
+	return req;
+}
+
+/* send the appropriate number of packets to match our bitrate */
+static void audio_send(struct audio_dev *audio)
+{
+	struct snd_pcm_runtime *runtime;
+	struct usb_request *req;
+	int length, length1, length2, ret;
+	s64 msecs;
+	s64 frames;
+	ktime_t now;
+
+	/* audio->substream will be null if we have been closed */
+	if (!audio->substream)
+		return;
+	/* audio->buffer_pos will be null if we have been stopped */
+	if (!audio->buffer_pos)
+		return;
+
+	runtime = audio->substream->runtime;
+
+	/* compute number of frames to send */
+	now = ktime_get();
+	msecs = ktime_to_ns(now) - ktime_to_ns(audio->start_time);
+	do_div(msecs, 1000000);
+	frames = msecs * SAMPLE_RATE;
+	do_div(frames, 1000);
+
+	/* Readjust our frames_sent if we fall too far behind.
+	 * If we get too far behind it is better to drop some frames than
+	 * to keep sending data too fast in an attempt to catch up.
+	 */
+	if (frames - audio->frames_sent > 10 * FRAMES_PER_MSEC)
+		audio->frames_sent = frames - FRAMES_PER_MSEC;
+
+	frames -= audio->frames_sent;
+
+	/* We need to send something to keep the pipeline going */
+	if (frames <= 0)
+		frames = FRAMES_PER_MSEC;
+
+	while (frames > 0) {
+		req = audio_req_get(audio);
+		if (!req)
+			break;
+
+		length = frames_to_bytes(runtime, frames);
+		if (length > IN_EP_MAX_PACKET_SIZE)
+			length = IN_EP_MAX_PACKET_SIZE;
+
+		if (audio->buffer_pos + length > audio->buffer_end)
+			length1 = audio->buffer_end - audio->buffer_pos;
+		else
+			length1 = length;
+		memcpy(req->buf, audio->buffer_pos, length1);
+		if (length1 < length) {
+			/* Wrap around and copy remaining length
+			 * at beginning of buffer.
+			 */
+			length2 = length - length1;
+			memcpy(req->buf + length1, audio->buffer_start,
+					length2);
+			audio->buffer_pos = audio->buffer_start + length2;
+		} else {
+			audio->buffer_pos += length1;
+			if (audio->buffer_pos >= audio->buffer_end)
+				audio->buffer_pos = audio->buffer_start;
+		}
+
+		req->length = length;
+		ret = usb_ep_queue(audio->in_ep, req, GFP_ATOMIC);
+		if (ret < 0) {
+			pr_err("usb_ep_queue failed ret: %d\n", ret);
+			audio_req_put(audio, req);
+			break;
+		}
+
+		frames -= bytes_to_frames(runtime, length);
+		audio->frames_sent += bytes_to_frames(runtime, length);
+	}
+}
+
+static void audio_control_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	/* nothing to do here */
+}
+
+static void audio_data_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	struct audio_dev *audio = req->context;
+
+	pr_debug("audio_data_complete req->status %d req->actual %d\n",
+		req->status, req->actual);
+
+	audio_req_put(audio, req);
+
+	if (!audio->buffer_start)
+		return;
+
+	audio->period_offset += req->actual;
+	if (audio->period_offset >= audio->period) {
+		snd_pcm_period_elapsed(audio->substream);
+		audio->period_offset = 0;
+	}
+	audio_send(audio);
+}
+
+static int audio_set_endpoint_req(struct usb_function *f,
+		const struct usb_ctrlrequest *ctrl)
+{
+	int value = -EOPNOTSUPP;
+	u16 ep = le16_to_cpu(ctrl->wIndex);
+	u16 len = le16_to_cpu(ctrl->wLength);
+	u16 w_value = le16_to_cpu(ctrl->wValue);
+
+	pr_debug("bRequest 0x%x, w_value 0x%04x, len %d, endpoint %d\n",
+			ctrl->bRequest, w_value, len, ep);
+
+	switch (ctrl->bRequest) {
+	case UAC_SET_CUR:
+	case UAC_SET_MIN:
+	case UAC_SET_MAX:
+	case UAC_SET_RES:
+		value = len;
+		break;
+	default:
+		break;
+	}
+
+	return value;
+}
+
+static int audio_get_endpoint_req(struct usb_function *f,
+		const struct usb_ctrlrequest *ctrl)
+{
+	struct usb_composite_dev *cdev = f->config->cdev;
+	int value = -EOPNOTSUPP;
+	u8 ep = ((le16_to_cpu(ctrl->wIndex) >> 8) & 0xFF);
+	u16 len = le16_to_cpu(ctrl->wLength);
+	u16 w_value = le16_to_cpu(ctrl->wValue);
+	u8 *buf = cdev->req->buf;
+
+	pr_debug("bRequest 0x%x, w_value 0x%04x, len %d, endpoint %d\n",
+			ctrl->bRequest, w_value, len, ep);
+
+	if (w_value == UAC_EP_CS_ATTR_SAMPLE_RATE << 8) {
+		switch (ctrl->bRequest) {
+		case UAC_GET_CUR:
+		case UAC_GET_MIN:
+		case UAC_GET_MAX:
+		case UAC_GET_RES:
+			/* return our sample rate */
+			buf[0] = (u8)SAMPLE_RATE;
+			buf[1] = (u8)(SAMPLE_RATE >> 8);
+			buf[2] = (u8)(SAMPLE_RATE >> 16);
+			value = 3;
+			break;
+		default:
+			break;
+		}
+	}
+
+	return value;
+}
+
+static int
+audio_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
+{
+	struct usb_composite_dev *cdev = f->config->cdev;
+	struct usb_request *req = cdev->req;
+	int value = -EOPNOTSUPP;
+	u16 w_index = le16_to_cpu(ctrl->wIndex);
+	u16 w_value = le16_to_cpu(ctrl->wValue);
+	u16 w_length = le16_to_cpu(ctrl->wLength);
+
+	/* composite driver infrastructure handles everything; interface
+	 * activation uses set_alt().
+	 */
+	switch (ctrl->bRequestType) {
+	case USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_ENDPOINT:
+		value = audio_set_endpoint_req(f, ctrl);
+		break;
+
+	case USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_ENDPOINT:
+		value = audio_get_endpoint_req(f, ctrl);
+		break;
+	}
+
+	/* respond with data transfer or status phase? */
+	if (value >= 0) {
+		pr_debug("audio req%02x.%02x v%04x i%04x l%d\n",
+			ctrl->bRequestType, ctrl->bRequest,
+			w_value, w_index, w_length);
+		req->zero = 0;
+		req->length = value;
+		req->complete = audio_control_complete;
+		value = usb_ep_queue(cdev->gadget->ep0, req, GFP_ATOMIC);
+		if (value < 0)
+			pr_err("audio response on err %d\n", value);
+	}
+
+	/* device either stalls (value < 0) or reports success */
+	return value;
+}
+
+static int audio_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
+{
+	struct audio_dev *audio = func_to_audio(f);
+	struct usb_composite_dev *cdev = f->config->cdev;
+
+	pr_debug("audio_set_alt intf %d, alt %d\n", intf, alt);
+	config_ep_by_speed(cdev->gadget, f, audio->in_ep);
+	usb_ep_enable(audio->in_ep);
+	return 0;
+}
+
+static void audio_disable(struct usb_function *f)
+{
+	struct audio_dev	*audio = func_to_audio(f);
+
+	pr_debug("audio_disable\n");
+	usb_ep_disable(audio->in_ep);
+}
+
+/*-------------------------------------------------------------------------*/
+
+static void audio_build_desc(struct audio_dev *audio)
+{
+	u8 *sam_freq;
+	int rate;
+
+	/* Set channel numbers */
+	input_terminal_desc.bNrChannels = 2;
+	as_type_i_desc.bNrChannels = 2;
+
+	/* Set sample rates */
+	rate = SAMPLE_RATE;
+	sam_freq = as_type_i_desc.tSamFreq[0];
+	memcpy(sam_freq, &rate, 3);
+}
+
+/* audio function driver setup/binding */
+static int
+audio_bind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct usb_composite_dev *cdev = c->cdev;
+	struct audio_dev *audio = func_to_audio(f);
+	int status;
+	struct usb_ep *ep;
+	struct usb_request *req;
+	int i;
+
+	audio_build_desc(audio);
+
+	/* allocate instance-specific interface IDs, and patch descriptors */
+	status = usb_interface_id(c, f);
+	if (status < 0)
+		goto fail;
+	ac_interface_desc.bInterfaceNumber = status;
+
+	status = usb_interface_id(c, f);
+	if (status < 0)
+		goto fail;
+	as_interface_alt_0_desc.bInterfaceNumber = status;
+	as_interface_alt_1_desc.bInterfaceNumber = status;
+
+	status = -ENODEV;
+
+	/* allocate our endpoint */
+	ep = usb_ep_autoconfig(cdev->gadget, &fs_as_in_ep_desc);
+	if (!ep)
+		goto fail;
+	audio->in_ep = ep;
+	ep->driver_data = audio; /* claim */
+
+	if (gadget_is_dualspeed(c->cdev->gadget))
+		hs_as_in_ep_desc.bEndpointAddress =
+			fs_as_in_ep_desc.bEndpointAddress;
+
+	f->descriptors = fs_audio_desc;
+	f->hs_descriptors = hs_audio_desc;
+
+	for (i = 0, status = 0; i < IN_EP_REQ_COUNT && status == 0; i++) {
+		req = audio_request_new(ep, IN_EP_MAX_PACKET_SIZE);
+		if (req) {
+			req->context = audio;
+			req->complete = audio_data_complete;
+			audio_req_put(audio, req);
+		} else
+			status = -ENOMEM;
+	}
+
+fail:
+	return status;
+}
+
+static void
+audio_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct audio_dev *audio = func_to_audio(f);
+	struct usb_request *req;
+
+	while ((req = audio_req_get(audio)))
+		audio_request_free(req, audio->in_ep);
+
+	snd_card_free_when_closed(audio->card);
+	kfree(audio);
+}
+
+static void audio_pcm_playback_start(struct audio_dev *audio)
+{
+	audio->start_time = ktime_get();
+	audio->frames_sent = 0;
+	audio_send(audio);
+}
+
+static void audio_pcm_playback_stop(struct audio_dev *audio)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&audio->lock, flags);
+	audio->buffer_start = 0;
+	audio->buffer_end = 0;
+	audio->buffer_pos = 0;
+	spin_unlock_irqrestore(&audio->lock, flags);
+}
+
+static int audio_pcm_open(struct snd_pcm_substream *substream)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct audio_dev *audio = substream->private_data;
+
+	runtime->private_data = audio;
+	runtime->hw = audio_hw_info;
+	snd_pcm_limit_hw_rates(runtime);
+	runtime->hw.channels_max = 2;
+
+	audio->substream = substream;
+	return 0;
+}
+
+static int audio_pcm_close(struct snd_pcm_substream *substream)
+{
+	struct audio_dev *audio = substream->private_data;
+	unsigned long flags;
+
+	spin_lock_irqsave(&audio->lock, flags);
+	audio->substream = NULL;
+	spin_unlock_irqrestore(&audio->lock, flags);
+
+	return 0;
+}
+
+static int audio_pcm_hw_params(struct snd_pcm_substream *substream,
+				struct snd_pcm_hw_params *params)
+{
+	unsigned int channels = params_channels(params);
+	unsigned int rate = params_rate(params);
+
+	if (rate != SAMPLE_RATE)
+		return -EINVAL;
+	if (channels != 2)
+		return -EINVAL;
+
+	return snd_pcm_lib_alloc_vmalloc_buffer(substream,
+		params_buffer_bytes(params));
+}
+
+static int audio_pcm_hw_free(struct snd_pcm_substream *substream)
+{
+	return snd_pcm_lib_free_vmalloc_buffer(substream);
+}
+
+static int audio_pcm_prepare(struct snd_pcm_substream *substream)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct audio_dev *audio = runtime->private_data;
+
+	audio->period = snd_pcm_lib_period_bytes(substream);
+	audio->period_offset = 0;
+	audio->buffer_start = runtime->dma_area;
+	audio->buffer_end = audio->buffer_start
+		+ snd_pcm_lib_buffer_bytes(substream);
+	audio->buffer_pos = audio->buffer_start;
+
+	return 0;
+}
+
+static snd_pcm_uframes_t audio_pcm_pointer(struct snd_pcm_substream *substream)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct audio_dev *audio = runtime->private_data;
+	ssize_t bytes = audio->buffer_pos - audio->buffer_start;
+
+	/* return offset of next frame to fill in our buffer */
+	return bytes_to_frames(runtime, bytes);
+}
+
+static int audio_pcm_playback_trigger(struct snd_pcm_substream *substream,
+					int cmd)
+{
+	struct audio_dev *audio = substream->runtime->private_data;
+	int ret = 0;
+
+	switch (cmd) {
+	case SNDRV_PCM_TRIGGER_START:
+	case SNDRV_PCM_TRIGGER_RESUME:
+		audio_pcm_playback_start(audio);
+		break;
+
+	case SNDRV_PCM_TRIGGER_STOP:
+	case SNDRV_PCM_TRIGGER_SUSPEND:
+		audio_pcm_playback_stop(audio);
+		break;
+
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static struct snd_pcm_ops audio_playback_ops = {
+	.open		= audio_pcm_open,
+	.close		= audio_pcm_close,
+	.ioctl		= snd_pcm_lib_ioctl,
+	.hw_params	= audio_pcm_hw_params,
+	.hw_free	= audio_pcm_hw_free,
+	.prepare	= audio_pcm_prepare,
+	.trigger	= audio_pcm_playback_trigger,
+	.pointer	= audio_pcm_pointer,
+};
+
+int audio_source_bind_config(struct usb_configuration *c,
+		struct audio_source_config *config)
+{
+	struct audio_dev *audio;
+	struct snd_card *card;
+	struct snd_pcm *pcm;
+	int err;
+
+	config->card = -1;
+	config->device = -1;
+
+	audio = kzalloc(sizeof *audio, GFP_KERNEL);
+	if (!audio)
+		return -ENOMEM;
+
+	audio->func.name = "audio_source";
+
+	spin_lock_init(&audio->lock);
+
+	audio->func.bind = audio_bind;
+	audio->func.unbind = audio_unbind;
+	audio->func.set_alt = audio_set_alt;
+	audio->func.setup = audio_setup;
+	audio->func.disable = audio_disable;
+	audio->in_desc = &fs_as_in_ep_desc;
+
+	INIT_LIST_HEAD(&audio->idle_reqs);
+
+	err = snd_card_create(SNDRV_DEFAULT_IDX1, SNDRV_DEFAULT_STR1,
+			THIS_MODULE, 0, &card);
+	if (err)
+		goto snd_card_fail;
+
+	snd_card_set_dev(card, &c->cdev->gadget->dev);
+
+	err = snd_pcm_new(card, "USB audio source", 0, 1, 0, &pcm);
+	if (err)
+		goto pcm_fail;
+	pcm->private_data = audio;
+	pcm->info_flags = 0;
+	audio->pcm = pcm;
+
+	strlcpy(pcm->name, "USB gadget audio", sizeof(pcm->name));
+
+	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_PLAYBACK, &audio_playback_ops);
+	snd_pcm_lib_preallocate_pages_for_all(pcm, SNDRV_DMA_TYPE_DEV,
+				NULL, 0, 64 * 1024);
+
+	strlcpy(card->driver, "audio_source", sizeof(card->driver));
+	strlcpy(card->shortname, card->driver, sizeof(card->shortname));
+	strlcpy(card->longname, "USB accessory audio source",
+		sizeof(card->longname));
+
+	err = snd_card_register(card);
+	if (err)
+		goto register_fail;
+
+	err = usb_add_function(c, &audio->func);
+	if (err)
+		goto add_fail;
+
+	config->card = pcm->card->number;
+	config->device = pcm->device;
+	audio->card = card;
+	return 0;
+
+add_fail:
+register_fail:
+pcm_fail:
+	snd_card_free(audio->card);
+snd_card_fail:
+	kfree(audio);
+	return err;
+}
diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index 5b9339582007..497a216188ad 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -3045,7 +3045,7 @@ static int fsg_bind_config(struct usb_composite_dev *cdev,
 	if (unlikely(!fsg))
 		return -ENOMEM;
 
-	fsg->function.name        = FSG_DRIVER_DESC;
+	fsg->function.name        = "mass_storage";
 	fsg->function.strings     = fsg_strings_array;
 	fsg->function.bind        = fsg_bind;
 	fsg->function.unbind      = fsg_unbind;
diff --git a/drivers/usb/gadget/f_mtp.c b/drivers/usb/gadget/f_mtp.c
new file mode 100644
index 000000000000..5aa5214297d1
--- /dev/null
+++ b/drivers/usb/gadget/f_mtp.c
@@ -0,0 +1,1264 @@
+/*
+ * Gadget Function Driver for MTP
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+/* #define DEBUG */
+/* #define VERBOSE_DEBUG */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/poll.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+
+#include <linux/types.h>
+#include <linux/file.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+
+#include <linux/usb.h>
+#include <linux/usb_usual.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/f_mtp.h>
+
+#define MTP_BULK_BUFFER_SIZE       16384
+#define INTR_BUFFER_SIZE           28
+
+/* String IDs */
+#define INTERFACE_STRING_INDEX	0
+
+/* values for mtp_dev.state */
+#define STATE_OFFLINE               0   /* initial state, disconnected */
+#define STATE_READY                 1   /* ready for userspace calls */
+#define STATE_BUSY                  2   /* processing userspace calls */
+#define STATE_CANCELED              3   /* transaction canceled by host */
+#define STATE_ERROR                 4   /* error from completion routine */
+
+/* number of tx and rx requests to allocate */
+#define TX_REQ_MAX 4
+#define RX_REQ_MAX 2
+#define INTR_REQ_MAX 5
+
+/* ID for Microsoft MTP OS String */
+#define MTP_OS_STRING_ID   0xEE
+
+/* MTP class reqeusts */
+#define MTP_REQ_CANCEL              0x64
+#define MTP_REQ_GET_EXT_EVENT_DATA  0x65
+#define MTP_REQ_RESET               0x66
+#define MTP_REQ_GET_DEVICE_STATUS   0x67
+
+/* constants for device status */
+#define MTP_RESPONSE_OK             0x2001
+#define MTP_RESPONSE_DEVICE_BUSY    0x2019
+
+static const char mtp_shortname[] = "mtp_usb";
+
+struct mtp_dev {
+	struct usb_function function;
+	struct usb_composite_dev *cdev;
+	spinlock_t lock;
+
+	struct usb_ep *ep_in;
+	struct usb_ep *ep_out;
+	struct usb_ep *ep_intr;
+
+	int state;
+
+	/* synchronize access to our device file */
+	atomic_t open_excl;
+	/* to enforce only one ioctl at a time */
+	atomic_t ioctl_excl;
+
+	struct list_head tx_idle;
+	struct list_head intr_idle;
+
+	wait_queue_head_t read_wq;
+	wait_queue_head_t write_wq;
+	wait_queue_head_t intr_wq;
+	struct usb_request *rx_req[RX_REQ_MAX];
+	int rx_done;
+
+	/* for processing MTP_SEND_FILE, MTP_RECEIVE_FILE and
+	 * MTP_SEND_FILE_WITH_HEADER ioctls on a work queue
+	 */
+	struct workqueue_struct *wq;
+	struct work_struct send_file_work;
+	struct work_struct receive_file_work;
+	struct file *xfer_file;
+	loff_t xfer_file_offset;
+	int64_t xfer_file_length;
+	unsigned xfer_send_header;
+	uint16_t xfer_command;
+	uint32_t xfer_transaction_id;
+	int xfer_result;
+};
+
+static struct usb_interface_descriptor mtp_interface_desc = {
+	.bLength                = USB_DT_INTERFACE_SIZE,
+	.bDescriptorType        = USB_DT_INTERFACE,
+	.bInterfaceNumber       = 0,
+	.bNumEndpoints          = 3,
+	.bInterfaceClass        = USB_CLASS_VENDOR_SPEC,
+	.bInterfaceSubClass     = USB_SUBCLASS_VENDOR_SPEC,
+	.bInterfaceProtocol     = 0,
+};
+
+static struct usb_interface_descriptor ptp_interface_desc = {
+	.bLength                = USB_DT_INTERFACE_SIZE,
+	.bDescriptorType        = USB_DT_INTERFACE,
+	.bInterfaceNumber       = 0,
+	.bNumEndpoints          = 3,
+	.bInterfaceClass        = USB_CLASS_STILL_IMAGE,
+	.bInterfaceSubClass     = 1,
+	.bInterfaceProtocol     = 1,
+};
+
+static struct usb_endpoint_descriptor mtp_highspeed_in_desc = {
+	.bLength                = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType        = USB_DT_ENDPOINT,
+	.bEndpointAddress       = USB_DIR_IN,
+	.bmAttributes           = USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize         = __constant_cpu_to_le16(512),
+};
+
+static struct usb_endpoint_descriptor mtp_highspeed_out_desc = {
+	.bLength                = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType        = USB_DT_ENDPOINT,
+	.bEndpointAddress       = USB_DIR_OUT,
+	.bmAttributes           = USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize         = __constant_cpu_to_le16(512),
+};
+
+static struct usb_endpoint_descriptor mtp_fullspeed_in_desc = {
+	.bLength                = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType        = USB_DT_ENDPOINT,
+	.bEndpointAddress       = USB_DIR_IN,
+	.bmAttributes           = USB_ENDPOINT_XFER_BULK,
+};
+
+static struct usb_endpoint_descriptor mtp_fullspeed_out_desc = {
+	.bLength                = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType        = USB_DT_ENDPOINT,
+	.bEndpointAddress       = USB_DIR_OUT,
+	.bmAttributes           = USB_ENDPOINT_XFER_BULK,
+};
+
+static struct usb_endpoint_descriptor mtp_intr_desc = {
+	.bLength                = USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType        = USB_DT_ENDPOINT,
+	.bEndpointAddress       = USB_DIR_IN,
+	.bmAttributes           = USB_ENDPOINT_XFER_INT,
+	.wMaxPacketSize         = __constant_cpu_to_le16(INTR_BUFFER_SIZE),
+	.bInterval              = 6,
+};
+
+static struct usb_descriptor_header *fs_mtp_descs[] = {
+	(struct usb_descriptor_header *) &mtp_interface_desc,
+	(struct usb_descriptor_header *) &mtp_fullspeed_in_desc,
+	(struct usb_descriptor_header *) &mtp_fullspeed_out_desc,
+	(struct usb_descriptor_header *) &mtp_intr_desc,
+	NULL,
+};
+
+static struct usb_descriptor_header *hs_mtp_descs[] = {
+	(struct usb_descriptor_header *) &mtp_interface_desc,
+	(struct usb_descriptor_header *) &mtp_highspeed_in_desc,
+	(struct usb_descriptor_header *) &mtp_highspeed_out_desc,
+	(struct usb_descriptor_header *) &mtp_intr_desc,
+	NULL,
+};
+
+static struct usb_descriptor_header *fs_ptp_descs[] = {
+	(struct usb_descriptor_header *) &ptp_interface_desc,
+	(struct usb_descriptor_header *) &mtp_fullspeed_in_desc,
+	(struct usb_descriptor_header *) &mtp_fullspeed_out_desc,
+	(struct usb_descriptor_header *) &mtp_intr_desc,
+	NULL,
+};
+
+static struct usb_descriptor_header *hs_ptp_descs[] = {
+	(struct usb_descriptor_header *) &ptp_interface_desc,
+	(struct usb_descriptor_header *) &mtp_highspeed_in_desc,
+	(struct usb_descriptor_header *) &mtp_highspeed_out_desc,
+	(struct usb_descriptor_header *) &mtp_intr_desc,
+	NULL,
+};
+
+static struct usb_string mtp_string_defs[] = {
+	/* Naming interface "MTP" so libmtp will recognize us */
+	[INTERFACE_STRING_INDEX].s	= "MTP",
+	{  },	/* end of list */
+};
+
+static struct usb_gadget_strings mtp_string_table = {
+	.language		= 0x0409,	/* en-US */
+	.strings		= mtp_string_defs,
+};
+
+static struct usb_gadget_strings *mtp_strings[] = {
+	&mtp_string_table,
+	NULL,
+};
+
+/* Microsoft MTP OS String */
+static u8 mtp_os_string[] = {
+	18, /* sizeof(mtp_os_string) */
+	USB_DT_STRING,
+	/* Signature field: "MSFT100" */
+	'M', 0, 'S', 0, 'F', 0, 'T', 0, '1', 0, '0', 0, '0', 0,
+	/* vendor code */
+	1,
+	/* padding */
+	0
+};
+
+/* Microsoft Extended Configuration Descriptor Header Section */
+struct mtp_ext_config_desc_header {
+	__le32	dwLength;
+	__u16	bcdVersion;
+	__le16	wIndex;
+	__u8	bCount;
+	__u8	reserved[7];
+};
+
+/* Microsoft Extended Configuration Descriptor Function Section */
+struct mtp_ext_config_desc_function {
+	__u8	bFirstInterfaceNumber;
+	__u8	bInterfaceCount;
+	__u8	compatibleID[8];
+	__u8	subCompatibleID[8];
+	__u8	reserved[6];
+};
+
+/* MTP Extended Configuration Descriptor */
+struct {
+	struct mtp_ext_config_desc_header	header;
+	struct mtp_ext_config_desc_function    function;
+} mtp_ext_config_desc = {
+	.header = {
+		.dwLength = __constant_cpu_to_le32(sizeof(mtp_ext_config_desc)),
+		.bcdVersion = __constant_cpu_to_le16(0x0100),
+		.wIndex = __constant_cpu_to_le16(4),
+		.bCount = __constant_cpu_to_le16(1),
+	},
+	.function = {
+		.bFirstInterfaceNumber = 0,
+		.bInterfaceCount = 1,
+		.compatibleID = { 'M', 'T', 'P' },
+	},
+};
+
+struct mtp_device_status {
+	__le16	wLength;
+	__le16	wCode;
+};
+
+/* temporary variable used between mtp_open() and mtp_gadget_bind() */
+static struct mtp_dev *_mtp_dev;
+
+static inline struct mtp_dev *func_to_mtp(struct usb_function *f)
+{
+	return container_of(f, struct mtp_dev, function);
+}
+
+static struct usb_request *mtp_request_new(struct usb_ep *ep, int buffer_size)
+{
+	struct usb_request *req = usb_ep_alloc_request(ep, GFP_KERNEL);
+	if (!req)
+		return NULL;
+
+	/* now allocate buffers for the requests */
+	req->buf = kmalloc(buffer_size, GFP_KERNEL);
+	if (!req->buf) {
+		usb_ep_free_request(ep, req);
+		return NULL;
+	}
+
+	return req;
+}
+
+static void mtp_request_free(struct usb_request *req, struct usb_ep *ep)
+{
+	if (req) {
+		kfree(req->buf);
+		usb_ep_free_request(ep, req);
+	}
+}
+
+static inline int mtp_lock(atomic_t *excl)
+{
+	if (atomic_inc_return(excl) == 1) {
+		return 0;
+	} else {
+		atomic_dec(excl);
+		return -1;
+	}
+}
+
+static inline void mtp_unlock(atomic_t *excl)
+{
+	atomic_dec(excl);
+}
+
+/* add a request to the tail of a list */
+static void mtp_req_put(struct mtp_dev *dev, struct list_head *head,
+		struct usb_request *req)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	list_add_tail(&req->list, head);
+	spin_unlock_irqrestore(&dev->lock, flags);
+}
+
+/* remove a request from the head of a list */
+static struct usb_request
+*mtp_req_get(struct mtp_dev *dev, struct list_head *head)
+{
+	unsigned long flags;
+	struct usb_request *req;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	if (list_empty(head)) {
+		req = 0;
+	} else {
+		req = list_first_entry(head, struct usb_request, list);
+		list_del(&req->list);
+	}
+	spin_unlock_irqrestore(&dev->lock, flags);
+	return req;
+}
+
+static void mtp_complete_in(struct usb_ep *ep, struct usb_request *req)
+{
+	struct mtp_dev *dev = _mtp_dev;
+
+	if (req->status != 0)
+		dev->state = STATE_ERROR;
+
+	mtp_req_put(dev, &dev->tx_idle, req);
+
+	wake_up(&dev->write_wq);
+}
+
+static void mtp_complete_out(struct usb_ep *ep, struct usb_request *req)
+{
+	struct mtp_dev *dev = _mtp_dev;
+
+	dev->rx_done = 1;
+	if (req->status != 0)
+		dev->state = STATE_ERROR;
+
+	wake_up(&dev->read_wq);
+}
+
+static void mtp_complete_intr(struct usb_ep *ep, struct usb_request *req)
+{
+	struct mtp_dev *dev = _mtp_dev;
+
+	if (req->status != 0)
+		dev->state = STATE_ERROR;
+
+	mtp_req_put(dev, &dev->intr_idle, req);
+
+	wake_up(&dev->intr_wq);
+}
+
+static int mtp_create_bulk_endpoints(struct mtp_dev *dev,
+				struct usb_endpoint_descriptor *in_desc,
+				struct usb_endpoint_descriptor *out_desc,
+				struct usb_endpoint_descriptor *intr_desc)
+{
+	struct usb_composite_dev *cdev = dev->cdev;
+	struct usb_request *req;
+	struct usb_ep *ep;
+	int i;
+
+	DBG(cdev, "create_bulk_endpoints dev: %p\n", dev);
+
+	ep = usb_ep_autoconfig(cdev->gadget, in_desc);
+	if (!ep) {
+		DBG(cdev, "usb_ep_autoconfig for ep_in failed\n");
+		return -ENODEV;
+	}
+	DBG(cdev, "usb_ep_autoconfig for ep_in got %s\n", ep->name);
+	ep->driver_data = dev;		/* claim the endpoint */
+	dev->ep_in = ep;
+
+	ep = usb_ep_autoconfig(cdev->gadget, out_desc);
+	if (!ep) {
+		DBG(cdev, "usb_ep_autoconfig for ep_out failed\n");
+		return -ENODEV;
+	}
+	DBG(cdev, "usb_ep_autoconfig for mtp ep_out got %s\n", ep->name);
+	ep->driver_data = dev;		/* claim the endpoint */
+	dev->ep_out = ep;
+
+	ep = usb_ep_autoconfig(cdev->gadget, out_desc);
+	if (!ep) {
+		DBG(cdev, "usb_ep_autoconfig for ep_out failed\n");
+		return -ENODEV;
+	}
+	DBG(cdev, "usb_ep_autoconfig for mtp ep_out got %s\n", ep->name);
+	ep->driver_data = dev;		/* claim the endpoint */
+	dev->ep_out = ep;
+
+	ep = usb_ep_autoconfig(cdev->gadget, intr_desc);
+	if (!ep) {
+		DBG(cdev, "usb_ep_autoconfig for ep_intr failed\n");
+		return -ENODEV;
+	}
+	DBG(cdev, "usb_ep_autoconfig for mtp ep_intr got %s\n", ep->name);
+	ep->driver_data = dev;		/* claim the endpoint */
+	dev->ep_intr = ep;
+
+	/* now allocate requests for our endpoints */
+	for (i = 0; i < TX_REQ_MAX; i++) {
+		req = mtp_request_new(dev->ep_in, MTP_BULK_BUFFER_SIZE);
+		if (!req)
+			goto fail;
+		req->complete = mtp_complete_in;
+		mtp_req_put(dev, &dev->tx_idle, req);
+	}
+	for (i = 0; i < RX_REQ_MAX; i++) {
+		req = mtp_request_new(dev->ep_out, MTP_BULK_BUFFER_SIZE);
+		if (!req)
+			goto fail;
+		req->complete = mtp_complete_out;
+		dev->rx_req[i] = req;
+	}
+	for (i = 0; i < INTR_REQ_MAX; i++) {
+		req = mtp_request_new(dev->ep_intr, INTR_BUFFER_SIZE);
+		if (!req)
+			goto fail;
+		req->complete = mtp_complete_intr;
+		mtp_req_put(dev, &dev->intr_idle, req);
+	}
+
+	return 0;
+
+fail:
+	printk(KERN_ERR "mtp_bind() could not allocate requests\n");
+	return -1;
+}
+
+static ssize_t mtp_read(struct file *fp, char __user *buf,
+	size_t count, loff_t *pos)
+{
+	struct mtp_dev *dev = fp->private_data;
+	struct usb_composite_dev *cdev = dev->cdev;
+	struct usb_request *req;
+	int r = count, xfer;
+	int ret = 0;
+
+	DBG(cdev, "mtp_read(%d)\n", count);
+
+	if (count > MTP_BULK_BUFFER_SIZE)
+		return -EINVAL;
+
+	/* we will block until we're online */
+	DBG(cdev, "mtp_read: waiting for online state\n");
+	ret = wait_event_interruptible(dev->read_wq,
+		dev->state != STATE_OFFLINE);
+	if (ret < 0) {
+		r = ret;
+		goto done;
+	}
+	spin_lock_irq(&dev->lock);
+	if (dev->state == STATE_CANCELED) {
+		/* report cancelation to userspace */
+		dev->state = STATE_READY;
+		spin_unlock_irq(&dev->lock);
+		return -ECANCELED;
+	}
+	dev->state = STATE_BUSY;
+	spin_unlock_irq(&dev->lock);
+
+requeue_req:
+	/* queue a request */
+	req = dev->rx_req[0];
+	req->length = count;
+	dev->rx_done = 0;
+	ret = usb_ep_queue(dev->ep_out, req, GFP_KERNEL);
+	if (ret < 0) {
+		r = -EIO;
+		goto done;
+	} else {
+		DBG(cdev, "rx %p queue\n", req);
+	}
+
+	/* wait for a request to complete */
+	ret = wait_event_interruptible(dev->read_wq, dev->rx_done);
+	if (ret < 0) {
+		r = ret;
+		usb_ep_dequeue(dev->ep_out, req);
+		goto done;
+	}
+	if (dev->state == STATE_BUSY) {
+		/* If we got a 0-len packet, throw it back and try again. */
+		if (req->actual == 0)
+			goto requeue_req;
+
+		DBG(cdev, "rx %p %d\n", req, req->actual);
+		xfer = (req->actual < count) ? req->actual : count;
+		r = xfer;
+		if (copy_to_user(buf, req->buf, xfer))
+			r = -EFAULT;
+	} else
+		r = -EIO;
+
+done:
+	spin_lock_irq(&dev->lock);
+	if (dev->state == STATE_CANCELED)
+		r = -ECANCELED;
+	else if (dev->state != STATE_OFFLINE)
+		dev->state = STATE_READY;
+	spin_unlock_irq(&dev->lock);
+
+	DBG(cdev, "mtp_read returning %d\n", r);
+	return r;
+}
+
+static ssize_t mtp_write(struct file *fp, const char __user *buf,
+	size_t count, loff_t *pos)
+{
+	struct mtp_dev *dev = fp->private_data;
+	struct usb_composite_dev *cdev = dev->cdev;
+	struct usb_request *req = 0;
+	int r = count, xfer;
+	int sendZLP = 0;
+	int ret;
+
+	DBG(cdev, "mtp_write(%d)\n", count);
+
+	spin_lock_irq(&dev->lock);
+	if (dev->state == STATE_CANCELED) {
+		/* report cancelation to userspace */
+		dev->state = STATE_READY;
+		spin_unlock_irq(&dev->lock);
+		return -ECANCELED;
+	}
+	if (dev->state == STATE_OFFLINE) {
+		spin_unlock_irq(&dev->lock);
+		return -ENODEV;
+	}
+	dev->state = STATE_BUSY;
+	spin_unlock_irq(&dev->lock);
+
+	/* we need to send a zero length packet to signal the end of transfer
+	 * if the transfer size is aligned to a packet boundary.
+	 */
+	if ((count & (dev->ep_in->maxpacket - 1)) == 0) {
+		sendZLP = 1;
+	}
+
+	while (count > 0 || sendZLP) {
+		/* so we exit after sending ZLP */
+		if (count == 0)
+			sendZLP = 0;
+
+		if (dev->state != STATE_BUSY) {
+			DBG(cdev, "mtp_write dev->error\n");
+			r = -EIO;
+			break;
+		}
+
+		/* get an idle tx request to use */
+		req = 0;
+		ret = wait_event_interruptible(dev->write_wq,
+			((req = mtp_req_get(dev, &dev->tx_idle))
+				|| dev->state != STATE_BUSY));
+		if (!req) {
+			r = ret;
+			break;
+		}
+
+		if (count > MTP_BULK_BUFFER_SIZE)
+			xfer = MTP_BULK_BUFFER_SIZE;
+		else
+			xfer = count;
+		if (xfer && copy_from_user(req->buf, buf, xfer)) {
+			r = -EFAULT;
+			break;
+		}
+
+		req->length = xfer;
+		ret = usb_ep_queue(dev->ep_in, req, GFP_KERNEL);
+		if (ret < 0) {
+			DBG(cdev, "mtp_write: xfer error %d\n", ret);
+			r = -EIO;
+			break;
+		}
+
+		buf += xfer;
+		count -= xfer;
+
+		/* zero this so we don't try to free it on error exit */
+		req = 0;
+	}
+
+	if (req)
+		mtp_req_put(dev, &dev->tx_idle, req);
+
+	spin_lock_irq(&dev->lock);
+	if (dev->state == STATE_CANCELED)
+		r = -ECANCELED;
+	else if (dev->state != STATE_OFFLINE)
+		dev->state = STATE_READY;
+	spin_unlock_irq(&dev->lock);
+
+	DBG(cdev, "mtp_write returning %d\n", r);
+	return r;
+}
+
+/* read from a local file and write to USB */
+static void send_file_work(struct work_struct *data) {
+	struct mtp_dev	*dev = container_of(data, struct mtp_dev, send_file_work);
+	struct usb_composite_dev *cdev = dev->cdev;
+	struct usb_request *req = 0;
+	struct mtp_data_header *header;
+	struct file *filp;
+	loff_t offset;
+	int64_t count;
+	int xfer, ret, hdr_size;
+	int r = 0;
+	int sendZLP = 0;
+
+	/* read our parameters */
+	smp_rmb();
+	filp = dev->xfer_file;
+	offset = dev->xfer_file_offset;
+	count = dev->xfer_file_length;
+
+	DBG(cdev, "send_file_work(%lld %lld)\n", offset, count);
+
+	if (dev->xfer_send_header) {
+		hdr_size = sizeof(struct mtp_data_header);
+		count += hdr_size;
+	} else {
+		hdr_size = 0;
+	}
+
+	/* we need to send a zero length packet to signal the end of transfer
+	 * if the transfer size is aligned to a packet boundary.
+	 */
+	if ((count & (dev->ep_in->maxpacket - 1)) == 0) {
+		sendZLP = 1;
+	}
+
+	while (count > 0 || sendZLP) {
+		/* so we exit after sending ZLP */
+		if (count == 0)
+			sendZLP = 0;
+
+		/* get an idle tx request to use */
+		req = 0;
+		ret = wait_event_interruptible(dev->write_wq,
+			(req = mtp_req_get(dev, &dev->tx_idle))
+			|| dev->state != STATE_BUSY);
+		if (dev->state == STATE_CANCELED) {
+			r = -ECANCELED;
+			break;
+		}
+		if (!req) {
+			r = ret;
+			break;
+		}
+
+		if (count > MTP_BULK_BUFFER_SIZE)
+			xfer = MTP_BULK_BUFFER_SIZE;
+		else
+			xfer = count;
+
+		if (hdr_size) {
+			/* prepend MTP data header */
+			header = (struct mtp_data_header *)req->buf;
+			header->length = __cpu_to_le32(count);
+			header->type = __cpu_to_le16(2); /* data packet */
+			header->command = __cpu_to_le16(dev->xfer_command);
+			header->transaction_id = __cpu_to_le32(dev->xfer_transaction_id);
+		}
+
+		ret = vfs_read(filp, req->buf + hdr_size, xfer - hdr_size, &offset);
+		if (ret < 0) {
+			r = ret;
+			break;
+		}
+		xfer = ret + hdr_size;
+		hdr_size = 0;
+
+		req->length = xfer;
+		ret = usb_ep_queue(dev->ep_in, req, GFP_KERNEL);
+		if (ret < 0) {
+			DBG(cdev, "send_file_work: xfer error %d\n", ret);
+			dev->state = STATE_ERROR;
+			r = -EIO;
+			break;
+		}
+
+		count -= xfer;
+
+		/* zero this so we don't try to free it on error exit */
+		req = 0;
+	}
+
+	if (req)
+		mtp_req_put(dev, &dev->tx_idle, req);
+
+	DBG(cdev, "send_file_work returning %d\n", r);
+	/* write the result */
+	dev->xfer_result = r;
+	smp_wmb();
+}
+
+/* read from USB and write to a local file */
+static void receive_file_work(struct work_struct *data)
+{
+	struct mtp_dev	*dev = container_of(data, struct mtp_dev, receive_file_work);
+	struct usb_composite_dev *cdev = dev->cdev;
+	struct usb_request *read_req = NULL, *write_req = NULL;
+	struct file *filp;
+	loff_t offset;
+	int64_t count;
+	int ret, cur_buf = 0;
+	int r = 0;
+
+	/* read our parameters */
+	smp_rmb();
+	filp = dev->xfer_file;
+	offset = dev->xfer_file_offset;
+	count = dev->xfer_file_length;
+
+	DBG(cdev, "receive_file_work(%lld)\n", count);
+
+	while (count > 0 || write_req) {
+		if (count > 0) {
+			/* queue a request */
+			read_req = dev->rx_req[cur_buf];
+			cur_buf = (cur_buf + 1) % RX_REQ_MAX;
+
+			read_req->length = (count > MTP_BULK_BUFFER_SIZE
+					? MTP_BULK_BUFFER_SIZE : count);
+			dev->rx_done = 0;
+			ret = usb_ep_queue(dev->ep_out, read_req, GFP_KERNEL);
+			if (ret < 0) {
+				r = -EIO;
+				dev->state = STATE_ERROR;
+				break;
+			}
+		}
+
+		if (write_req) {
+			DBG(cdev, "rx %p %d\n", write_req, write_req->actual);
+			ret = vfs_write(filp, write_req->buf, write_req->actual,
+				&offset);
+			DBG(cdev, "vfs_write %d\n", ret);
+			if (ret != write_req->actual) {
+				r = -EIO;
+				dev->state = STATE_ERROR;
+				break;
+			}
+			write_req = NULL;
+		}
+
+		if (read_req) {
+			/* wait for our last read to complete */
+			ret = wait_event_interruptible(dev->read_wq,
+				dev->rx_done || dev->state != STATE_BUSY);
+			if (dev->state == STATE_CANCELED) {
+				r = -ECANCELED;
+				if (!dev->rx_done)
+					usb_ep_dequeue(dev->ep_out, read_req);
+				break;
+			}
+			/* if xfer_file_length is 0xFFFFFFFF, then we read until
+			 * we get a zero length packet
+			 */
+			if (count != 0xFFFFFFFF)
+				count -= read_req->actual;
+			if (read_req->actual < read_req->length) {
+				/* short packet is used to signal EOF for sizes > 4 gig */
+				DBG(cdev, "got short packet\n");
+				count = 0;
+			}
+
+			write_req = read_req;
+			read_req = NULL;
+		}
+	}
+
+	DBG(cdev, "receive_file_work returning %d\n", r);
+	/* write the result */
+	dev->xfer_result = r;
+	smp_wmb();
+}
+
+static int mtp_send_event(struct mtp_dev *dev, struct mtp_event *event)
+{
+	struct usb_request *req= NULL;
+	int ret;
+	int length = event->length;
+
+	DBG(dev->cdev, "mtp_send_event(%d)\n", event->length);
+
+	if (length < 0 || length > INTR_BUFFER_SIZE)
+		return -EINVAL;
+	if (dev->state == STATE_OFFLINE)
+		return -ENODEV;
+
+	ret = wait_event_interruptible_timeout(dev->intr_wq,
+		(req = mtp_req_get(dev, &dev->intr_idle)), msecs_to_jiffies(1000));
+	if (!req)
+	    return -ETIME;
+
+	if (copy_from_user(req->buf, (void __user *)event->data, length)) {
+		mtp_req_put(dev, &dev->intr_idle, req);
+		return -EFAULT;
+	}
+	req->length = length;
+	ret = usb_ep_queue(dev->ep_intr, req, GFP_KERNEL);
+	if (ret)
+		mtp_req_put(dev, &dev->intr_idle, req);
+
+	return ret;
+}
+
+static long mtp_ioctl(struct file *fp, unsigned code, unsigned long value)
+{
+	struct mtp_dev *dev = fp->private_data;
+	struct file *filp = NULL;
+	int ret = -EINVAL;
+
+	if (mtp_lock(&dev->ioctl_excl))
+		return -EBUSY;
+
+	switch (code) {
+	case MTP_SEND_FILE:
+	case MTP_RECEIVE_FILE:
+	case MTP_SEND_FILE_WITH_HEADER:
+	{
+		struct mtp_file_range	mfr;
+		struct work_struct *work;
+
+		spin_lock_irq(&dev->lock);
+		if (dev->state == STATE_CANCELED) {
+			/* report cancelation to userspace */
+			dev->state = STATE_READY;
+			spin_unlock_irq(&dev->lock);
+			ret = -ECANCELED;
+			goto out;
+		}
+		if (dev->state == STATE_OFFLINE) {
+			spin_unlock_irq(&dev->lock);
+			ret = -ENODEV;
+			goto out;
+		}
+		dev->state = STATE_BUSY;
+		spin_unlock_irq(&dev->lock);
+
+		if (copy_from_user(&mfr, (void __user *)value, sizeof(mfr))) {
+			ret = -EFAULT;
+			goto fail;
+		}
+		/* hold a reference to the file while we are working with it */
+		filp = fget(mfr.fd);
+		if (!filp) {
+			ret = -EBADF;
+			goto fail;
+		}
+
+		/* write the parameters */
+		dev->xfer_file = filp;
+		dev->xfer_file_offset = mfr.offset;
+		dev->xfer_file_length = mfr.length;
+		smp_wmb();
+
+		if (code == MTP_SEND_FILE_WITH_HEADER) {
+			work = &dev->send_file_work;
+			dev->xfer_send_header = 1;
+			dev->xfer_command = mfr.command;
+			dev->xfer_transaction_id = mfr.transaction_id;
+		} else if (code == MTP_SEND_FILE) {
+			work = &dev->send_file_work;
+			dev->xfer_send_header = 0;
+		} else {
+			work = &dev->receive_file_work;
+		}
+
+		/* We do the file transfer on a work queue so it will run
+		 * in kernel context, which is necessary for vfs_read and
+		 * vfs_write to use our buffers in the kernel address space.
+		 */
+		queue_work(dev->wq, work);
+		/* wait for operation to complete */
+		flush_workqueue(dev->wq);
+		fput(filp);
+
+		/* read the result */
+		smp_rmb();
+		ret = dev->xfer_result;
+		break;
+	}
+	case MTP_SEND_EVENT:
+	{
+		struct mtp_event	event;
+		/* return here so we don't change dev->state below,
+		 * which would interfere with bulk transfer state.
+		 */
+		if (copy_from_user(&event, (void __user *)value, sizeof(event)))
+			ret = -EFAULT;
+		else
+			ret = mtp_send_event(dev, &event);
+		goto out;
+	}
+	}
+
+fail:
+	spin_lock_irq(&dev->lock);
+	if (dev->state == STATE_CANCELED)
+		ret = -ECANCELED;
+	else if (dev->state != STATE_OFFLINE)
+		dev->state = STATE_READY;
+	spin_unlock_irq(&dev->lock);
+out:
+	mtp_unlock(&dev->ioctl_excl);
+	DBG(dev->cdev, "ioctl returning %d\n", ret);
+	return ret;
+}
+
+static int mtp_open(struct inode *ip, struct file *fp)
+{
+	printk(KERN_INFO "mtp_open\n");
+	if (mtp_lock(&_mtp_dev->open_excl))
+		return -EBUSY;
+
+	/* clear any error condition */
+	if (_mtp_dev->state != STATE_OFFLINE)
+		_mtp_dev->state = STATE_READY;
+
+	fp->private_data = _mtp_dev;
+	return 0;
+}
+
+static int mtp_release(struct inode *ip, struct file *fp)
+{
+	printk(KERN_INFO "mtp_release\n");
+
+	mtp_unlock(&_mtp_dev->open_excl);
+	return 0;
+}
+
+/* file operations for /dev/mtp_usb */
+static const struct file_operations mtp_fops = {
+	.owner = THIS_MODULE,
+	.read = mtp_read,
+	.write = mtp_write,
+	.unlocked_ioctl = mtp_ioctl,
+	.open = mtp_open,
+	.release = mtp_release,
+};
+
+static struct miscdevice mtp_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = mtp_shortname,
+	.fops = &mtp_fops,
+};
+
+static int mtp_ctrlrequest(struct usb_composite_dev *cdev,
+				const struct usb_ctrlrequest *ctrl)
+{
+	struct mtp_dev *dev = _mtp_dev;
+	int	value = -EOPNOTSUPP;
+	u16	w_index = le16_to_cpu(ctrl->wIndex);
+	u16	w_value = le16_to_cpu(ctrl->wValue);
+	u16	w_length = le16_to_cpu(ctrl->wLength);
+	unsigned long	flags;
+
+	VDBG(cdev, "mtp_ctrlrequest "
+			"%02x.%02x v%04x i%04x l%u\n",
+			ctrl->bRequestType, ctrl->bRequest,
+			w_value, w_index, w_length);
+
+	/* Handle MTP OS string */
+	if (ctrl->bRequestType ==
+			(USB_DIR_IN | USB_TYPE_STANDARD | USB_RECIP_DEVICE)
+			&& ctrl->bRequest == USB_REQ_GET_DESCRIPTOR
+			&& (w_value >> 8) == USB_DT_STRING
+			&& (w_value & 0xFF) == MTP_OS_STRING_ID) {
+		value = (w_length < sizeof(mtp_os_string)
+				? w_length : sizeof(mtp_os_string));
+		memcpy(cdev->req->buf, mtp_os_string, value);
+	} else if ((ctrl->bRequestType & USB_TYPE_MASK) == USB_TYPE_VENDOR) {
+		/* Handle MTP OS descriptor */
+		DBG(cdev, "vendor request: %d index: %d value: %d length: %d\n",
+			ctrl->bRequest, w_index, w_value, w_length);
+
+		if (ctrl->bRequest == 1
+				&& (ctrl->bRequestType & USB_DIR_IN)
+				&& (w_index == 4 || w_index == 5)) {
+			value = (w_length < sizeof(mtp_ext_config_desc) ?
+					w_length : sizeof(mtp_ext_config_desc));
+			memcpy(cdev->req->buf, &mtp_ext_config_desc, value);
+		}
+	} else if ((ctrl->bRequestType & USB_TYPE_MASK) == USB_TYPE_CLASS) {
+		DBG(cdev, "class request: %d index: %d value: %d length: %d\n",
+			ctrl->bRequest, w_index, w_value, w_length);
+
+		if (ctrl->bRequest == MTP_REQ_CANCEL && w_index == 0
+				&& w_value == 0) {
+			DBG(cdev, "MTP_REQ_CANCEL\n");
+
+			spin_lock_irqsave(&dev->lock, flags);
+			if (dev->state == STATE_BUSY) {
+				dev->state = STATE_CANCELED;
+				wake_up(&dev->read_wq);
+				wake_up(&dev->write_wq);
+			}
+			spin_unlock_irqrestore(&dev->lock, flags);
+
+			/* We need to queue a request to read the remaining
+			 *  bytes, but we don't actually need to look at
+			 * the contents.
+			 */
+			value = w_length;
+		} else if (ctrl->bRequest == MTP_REQ_GET_DEVICE_STATUS
+				&& w_index == 0 && w_value == 0) {
+			struct mtp_device_status *status = cdev->req->buf;
+			status->wLength =
+				__constant_cpu_to_le16(sizeof(*status));
+
+			DBG(cdev, "MTP_REQ_GET_DEVICE_STATUS\n");
+			spin_lock_irqsave(&dev->lock, flags);
+			/* device status is "busy" until we report
+			 * the cancelation to userspace
+			 */
+			if (dev->state == STATE_CANCELED)
+				status->wCode =
+					__cpu_to_le16(MTP_RESPONSE_DEVICE_BUSY);
+			else
+				status->wCode =
+					__cpu_to_le16(MTP_RESPONSE_OK);
+			spin_unlock_irqrestore(&dev->lock, flags);
+			value = sizeof(*status);
+		}
+	}
+
+	/* respond with data transfer or status phase? */
+	if (value >= 0) {
+		int rc;
+		cdev->req->zero = value < w_length;
+		cdev->req->length = value;
+		rc = usb_ep_queue(cdev->gadget->ep0, cdev->req, GFP_ATOMIC);
+		if (rc < 0)
+			ERROR(cdev, "%s setup response queue error\n", __func__);
+	}
+	return value;
+}
+
+static int
+mtp_function_bind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct usb_composite_dev *cdev = c->cdev;
+	struct mtp_dev	*dev = func_to_mtp(f);
+	int			id;
+	int			ret;
+
+	dev->cdev = cdev;
+	DBG(cdev, "mtp_function_bind dev: %p\n", dev);
+
+	/* allocate interface ID(s) */
+	id = usb_interface_id(c, f);
+	if (id < 0)
+		return id;
+	mtp_interface_desc.bInterfaceNumber = id;
+
+	/* allocate endpoints */
+	ret = mtp_create_bulk_endpoints(dev, &mtp_fullspeed_in_desc,
+			&mtp_fullspeed_out_desc, &mtp_intr_desc);
+	if (ret)
+		return ret;
+
+	/* support high speed hardware */
+	if (gadget_is_dualspeed(c->cdev->gadget)) {
+		mtp_highspeed_in_desc.bEndpointAddress =
+			mtp_fullspeed_in_desc.bEndpointAddress;
+		mtp_highspeed_out_desc.bEndpointAddress =
+			mtp_fullspeed_out_desc.bEndpointAddress;
+	}
+
+	DBG(cdev, "%s speed %s: IN/%s, OUT/%s\n",
+			gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full",
+			f->name, dev->ep_in->name, dev->ep_out->name);
+	return 0;
+}
+
+static void
+mtp_function_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct mtp_dev	*dev = func_to_mtp(f);
+	struct usb_request *req;
+	int i;
+
+	while ((req = mtp_req_get(dev, &dev->tx_idle)))
+		mtp_request_free(req, dev->ep_in);
+	for (i = 0; i < RX_REQ_MAX; i++)
+		mtp_request_free(dev->rx_req[i], dev->ep_out);
+	while ((req = mtp_req_get(dev, &dev->intr_idle)))
+		mtp_request_free(req, dev->ep_intr);
+	dev->state = STATE_OFFLINE;
+}
+
+static int mtp_function_set_alt(struct usb_function *f,
+		unsigned intf, unsigned alt)
+{
+	struct mtp_dev	*dev = func_to_mtp(f);
+	struct usb_composite_dev *cdev = f->config->cdev;
+	int ret;
+
+	DBG(cdev, "mtp_function_set_alt intf: %d alt: %d\n", intf, alt);
+	config_ep_by_speed(cdev->gadget, f, dev->ep_in);
+	ret = usb_ep_enable(dev->ep_in);
+	if (ret)
+		return ret;
+	config_ep_by_speed(cdev->gadget, f, dev->ep_out);
+	ret = usb_ep_enable(dev->ep_out);
+	if (ret) {
+		usb_ep_disable(dev->ep_in);
+		return ret;
+	}
+	dev->ep_intr->desc = &mtp_intr_desc;
+	ret = usb_ep_enable(dev->ep_intr);
+	if (ret) {
+		usb_ep_disable(dev->ep_out);
+		usb_ep_disable(dev->ep_in);
+		return ret;
+	}
+	dev->state = STATE_READY;
+
+	/* readers may be blocked waiting for us to go online */
+	wake_up(&dev->read_wq);
+	return 0;
+}
+
+static void mtp_function_disable(struct usb_function *f)
+{
+	struct mtp_dev	*dev = func_to_mtp(f);
+	struct usb_composite_dev	*cdev = dev->cdev;
+
+	DBG(cdev, "mtp_function_disable\n");
+	dev->state = STATE_OFFLINE;
+	usb_ep_disable(dev->ep_in);
+	usb_ep_disable(dev->ep_out);
+	usb_ep_disable(dev->ep_intr);
+
+	/* readers may be blocked waiting for us to go online */
+	wake_up(&dev->read_wq);
+
+	VDBG(cdev, "%s disabled\n", dev->function.name);
+}
+
+static int mtp_bind_config(struct usb_configuration *c, bool ptp_config)
+{
+	struct mtp_dev *dev = _mtp_dev;
+	int ret = 0;
+
+	printk(KERN_INFO "mtp_bind_config\n");
+
+	/* allocate a string ID for our interface */
+	if (mtp_string_defs[INTERFACE_STRING_INDEX].id == 0) {
+		ret = usb_string_id(c->cdev);
+		if (ret < 0)
+			return ret;
+		mtp_string_defs[INTERFACE_STRING_INDEX].id = ret;
+		mtp_interface_desc.iInterface = ret;
+	}
+
+	dev->cdev = c->cdev;
+	dev->function.name = "mtp";
+	dev->function.strings = mtp_strings;
+	if (ptp_config) {
+		dev->function.descriptors = fs_ptp_descs;
+		dev->function.hs_descriptors = hs_ptp_descs;
+	} else {
+		dev->function.descriptors = fs_mtp_descs;
+		dev->function.hs_descriptors = hs_mtp_descs;
+	}
+	dev->function.bind = mtp_function_bind;
+	dev->function.unbind = mtp_function_unbind;
+	dev->function.set_alt = mtp_function_set_alt;
+	dev->function.disable = mtp_function_disable;
+
+	return usb_add_function(c, &dev->function);
+}
+
+static int mtp_setup(void)
+{
+	struct mtp_dev *dev;
+	int ret;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	spin_lock_init(&dev->lock);
+	init_waitqueue_head(&dev->read_wq);
+	init_waitqueue_head(&dev->write_wq);
+	init_waitqueue_head(&dev->intr_wq);
+	atomic_set(&dev->open_excl, 0);
+	atomic_set(&dev->ioctl_excl, 0);
+	INIT_LIST_HEAD(&dev->tx_idle);
+	INIT_LIST_HEAD(&dev->intr_idle);
+
+	dev->wq = create_singlethread_workqueue("f_mtp");
+	if (!dev->wq) {
+		ret = -ENOMEM;
+		goto err1;
+	}
+	INIT_WORK(&dev->send_file_work, send_file_work);
+	INIT_WORK(&dev->receive_file_work, receive_file_work);
+
+	_mtp_dev = dev;
+
+	ret = misc_register(&mtp_device);
+	if (ret)
+		goto err2;
+
+	return 0;
+
+err2:
+	destroy_workqueue(dev->wq);
+err1:
+	_mtp_dev = NULL;
+	kfree(dev);
+	printk(KERN_ERR "mtp gadget driver failed to initialize\n");
+	return ret;
+}
+
+static void mtp_cleanup(void)
+{
+	struct mtp_dev *dev = _mtp_dev;
+
+	if (!dev)
+		return;
+
+	misc_deregister(&mtp_device);
+	destroy_workqueue(dev->wq);
+	_mtp_dev = NULL;
+	kfree(dev);
+}
diff --git a/drivers/usb/gadget/f_rndis.c b/drivers/usb/gadget/f_rndis.c
index 3ea4666be3d0..25cb5fc8b263 100644
--- a/drivers/usb/gadget/f_rndis.c
+++ b/drivers/usb/gadget/f_rndis.c
@@ -26,7 +26,7 @@
 
 #include <linux/slab.h>
 #include <linux/kernel.h>
-#include <linux/device.h>
+#include <linux/platform_device.h>
 #include <linux/etherdevice.h>
 
 #include <linux/atomic.h>
@@ -80,6 +80,8 @@ struct f_rndis {
 	struct gether			port;
 	u8				ctrl_id, data_id;
 	u8				ethaddr[ETH_ALEN];
+	u32				vendorID;
+	const char			*manufacturer;
 	int				config;
 
 	struct usb_ep			*notify;
@@ -179,12 +181,11 @@ static struct usb_interface_assoc_descriptor
 rndis_iad_descriptor = {
 	.bLength =		sizeof rndis_iad_descriptor,
 	.bDescriptorType =	USB_DT_INTERFACE_ASSOCIATION,
-
 	.bFirstInterface =	0, /* XXX, hardcoded */
 	.bInterfaceCount = 	2,	// control + data
 	.bFunctionClass =	USB_CLASS_COMM,
 	.bFunctionSubClass =	USB_CDC_SUBCLASS_ETHERNET,
-	.bFunctionProtocol =	USB_CDC_PROTO_NONE,
+	.bFunctionProtocol =	USB_CDC_ACM_PROTO_VENDOR,
 	/* .iFunction = DYNAMIC */
 };
 
@@ -390,7 +391,6 @@ static void rndis_response_available(void *_rndis)
 {
 	struct f_rndis			*rndis = _rndis;
 	struct usb_request		*req = rndis->notify_req;
-	struct usb_composite_dev	*cdev = rndis->port.func.config->cdev;
 	__le32				*data = req->buf;
 	int				status;
 
@@ -408,14 +408,13 @@ static void rndis_response_available(void *_rndis)
 	status = usb_ep_queue(rndis->notify, req, GFP_ATOMIC);
 	if (status) {
 		atomic_dec(&rndis->notify_count);
-		DBG(cdev, "notify/0 --> %d\n", status);
+		pr_err("notify/0 --> %d\n", status);
 	}
 }
 
 static void rndis_response_complete(struct usb_ep *ep, struct usb_request *req)
 {
 	struct f_rndis			*rndis = req->context;
-	struct usb_composite_dev	*cdev = rndis->port.func.config->cdev;
 	int				status = req->status;
 
 	/* after TX:
@@ -429,7 +428,7 @@ static void rndis_response_complete(struct usb_ep *ep, struct usb_request *req)
 		atomic_set(&rndis->notify_count, 0);
 		break;
 	default:
-		DBG(cdev, "RNDIS %s response error %d, %d/%d\n",
+		pr_err("RNDIS %s response error %d, %d/%d\n",
 			ep->name, status,
 			req->actual, req->length);
 		/* FALLTHROUGH */
@@ -445,7 +444,7 @@ static void rndis_response_complete(struct usb_ep *ep, struct usb_request *req)
 		status = usb_ep_queue(rndis->notify, req, GFP_ATOMIC);
 		if (status) {
 			atomic_dec(&rndis->notify_count);
-			DBG(cdev, "notify/1 --> %d\n", status);
+			pr_err("notify/1 --> %d\n", status);
 		}
 		break;
 	}
@@ -454,14 +453,13 @@ static void rndis_response_complete(struct usb_ep *ep, struct usb_request *req)
 static void rndis_command_complete(struct usb_ep *ep, struct usb_request *req)
 {
 	struct f_rndis			*rndis = req->context;
-	struct usb_composite_dev	*cdev = rndis->port.func.config->cdev;
 	int				status;
 
 	/* received RNDIS command from USB_CDC_SEND_ENCAPSULATED_COMMAND */
 //	spin_lock(&dev->lock);
 	status = rndis_msg_parser(rndis->config, (u8 *) req->buf);
 	if (status < 0)
-		ERROR(cdev, "RNDIS command error %d, %d/%d\n",
+		pr_err("RNDIS command error %d, %d/%d\n",
 			status, req->actual, req->length);
 //	spin_unlock(&dev->lock);
 }
@@ -776,12 +774,10 @@ rndis_bind(struct usb_configuration *c, struct usb_function *f)
 	rndis_set_param_medium(rndis->config, NDIS_MEDIUM_802_3, 0);
 	rndis_set_host_mac(rndis->config, rndis->ethaddr);
 
-#if 0
-// FIXME
-	if (rndis_set_param_vendor(rndis->config, vendorID,
-				manufacturer))
-		goto fail0;
-#endif
+	if (rndis->manufacturer && rndis->vendorID &&
+			rndis_set_param_vendor(rndis->config, rndis->vendorID,
+					       rndis->manufacturer))
+		goto fail;
 
 	/* NOTE:  all that is done without knowing or caring about
 	 * the network link ... which is unavailable to this code
@@ -831,6 +827,7 @@ rndis_unbind(struct usb_configuration *c, struct usb_function *f)
 
 	if (gadget_is_superspeed(c->cdev->gadget))
 		usb_free_descriptors(f->ss_descriptors);
+
 	if (gadget_is_dualspeed(c->cdev->gadget))
 		usb_free_descriptors(f->hs_descriptors);
 	usb_free_descriptors(f->descriptors);
@@ -848,20 +845,9 @@ static inline bool can_support_rndis(struct usb_configuration *c)
 	return true;
 }
 
-/**
- * rndis_bind_config - add RNDIS network link to a configuration
- * @c: the configuration to support the network link
- * @ethaddr: a buffer in which the ethernet address of the host side
- *	side of the link was recorded
- * Context: single threaded during gadget setup
- *
- * Returns zero on success, else negative errno.
- *
- * Caller must have called @gether_setup().  Caller is also responsible
- * for calling @gether_cleanup() before module unload.
- */
 int
-rndis_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN])
+rndis_bind_config_vendor(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
+				u32 vendorID, const char *manufacturer)
 {
 	struct f_rndis	*rndis;
 	int		status;
@@ -869,14 +855,14 @@ rndis_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN])
 	if (!can_support_rndis(c) || !ethaddr)
 		return -EINVAL;
 
+	/* setup RNDIS itself */
+	status = rndis_init();
+	if (status < 0)
+		return status;
+
 	/* maybe allocate device-global string IDs */
 	if (rndis_string_defs[0].id == 0) {
 
-		/* ... and setup RNDIS itself */
-		status = rndis_init();
-		if (status < 0)
-			return status;
-
 		/* control interface label */
 		status = usb_string_id(c->cdev);
 		if (status < 0)
@@ -906,6 +892,8 @@ rndis_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN])
 		goto fail;
 
 	memcpy(rndis->ethaddr, ethaddr, ETH_ALEN);
+	rndis->vendorID = vendorID;
+	rndis->manufacturer = manufacturer;
 
 	/* RNDIS activates when the host changes this filter */
 	rndis->port.cdc_filter = 0;
diff --git a/drivers/usb/gadget/fsl_udc_core.c b/drivers/usb/gadget/fsl_udc_core.c
index de24a4233c25..e3196ba2833c 100644
--- a/drivers/usb/gadget/fsl_udc_core.c
+++ b/drivers/usb/gadget/fsl_udc_core.c
@@ -5,6 +5,8 @@
  * Author: Li Yang <leoli@freescale.com>
  *         Jiang Bo <tanya.jiang@freescale.com>
  *
+ * Copyright (C) 2010-2012 NVIDIA Corporation
+ *
  * Description:
  * Freescale high-speed USB SOC DR module device controller driver.
  * This can be found on MPC8349E/MPC8313E/MPC5121E cpus.
@@ -40,6 +42,8 @@
 #include <linux/fsl_devices.h>
 #include <linux/dmapool.h>
 #include <linux/delay.h>
+#include <linux/regulator/consumer.h>
+#include <linux/workqueue.h>
 
 #include <asm/byteorder.h>
 #include <asm/io.h>
@@ -47,15 +51,26 @@
 #include <asm/unaligned.h>
 #include <asm/dma.h>
 
+#include <mach/iomap.h>
+
 #include "fsl_usb2_udc.h"
 
+#ifdef CONFIG_ARCH_TEGRA
+#define	DRIVER_DESC	"NVidia Tegra High-Speed USB SOC Device Controller driver"
+#else
 #define	DRIVER_DESC	"Freescale High-Speed USB SOC Device Controller driver"
+#endif
 #define	DRIVER_AUTHOR	"Li Yang/Jiang Bo"
 #define	DRIVER_VERSION	"Apr 20, 2007"
 
 #define	DMA_ADDR_INVALID	(~(dma_addr_t)0)
+#define USB1_PREFETCH_ID	6
 
+#ifdef CONFIG_ARCH_TEGRA
+static const char driver_name[] = "fsl-tegra-udc";
+#else
 static const char driver_name[] = "fsl-usb2-udc";
+#endif
 static const char driver_desc[] = DRIVER_DESC;
 
 static struct usb_dr_device *dr_regs;
@@ -63,9 +78,21 @@ static struct usb_dr_device *dr_regs;
 static struct usb_sys_interface *usb_sys_regs;
 #endif
 
+/* Charger current limit=1800mA, as per the USB charger spec */
+#define USB_CHARGING_CURRENT_LIMIT_MA 1800
+/* 1 sec wait time for charger detection after vbus is detected */
+#define USB_CHARGER_DETECTION_WAIT_TIME_MS 1000
+#define BOOST_TRIGGER_SIZE 4096
+
 /* it is initialized in probe()  */
 static struct fsl_udc *udc_controller = NULL;
 
+#ifdef CONFIG_TEGRA_GADGET_BOOST_CPU_FREQ
+static struct pm_qos_request_list boost_cpu_freq_req;
+static u32 ep_queue_request_count;
+static u8 boost_cpufreq_work_flag;
+#endif
+
 static const struct usb_endpoint_descriptor
 fsl_ep0_desc = {
 	.bLength =		USB_DT_ENDPOINT_SIZE,
@@ -75,7 +102,9 @@ fsl_ep0_desc = {
 	.wMaxPacketSize =	USB_MAX_CTRL_PAYLOAD,
 };
 
+static u32 *control_reg = NULL;
 static void fsl_ep_fifo_flush(struct usb_ep *_ep);
+static int reset_queues(struct fsl_udc *udc);
 
 #ifdef CONFIG_PPC32
 /*
@@ -150,10 +179,44 @@ static inline void fsl_set_accessors(struct fsl_usb2_platform_data *pdata) {}
 #define hc32_to_cpu(x)		le32_to_cpu(x)
 #endif /* CONFIG_PPC32 */
 
+/*
+ * High speed test mode packet(53 bytes).
+ * See USB 2.0 spec, section 7.1.20.
+ */
+static const u8 fsl_udc_test_packet[53] = {
+	/* JKJKJKJK x9 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	/* JJKKJJKK x8 */
+	0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa,
+	/* JJJJKKKK x8 */
+	0xee, 0xee, 0xee, 0xee, 0xee, 0xee, 0xee, 0xee,
+	/* JJJJJJJKKKKKKK x8 */
+	0xfe, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	/* JJJJJJJK x8 */
+	0x7f, 0xbf, 0xdf, 0xef, 0xf7, 0xfb, 0xfd,
+	/* JKKKKKKK x10, JK */
+	0xfc, 0x7e, 0xbf, 0xdf, 0xef, 0xf7, 0xfb, 0xfd, 0x7e
+};
+
 /********************************************************************
  *	Internal Used Function
 ********************************************************************/
 /*-----------------------------------------------------------------
+ * vbus_enabled() - checks vbus status
+ *--------------------------------------------------------------*/
+static inline bool vbus_enabled(void)
+{
+	bool status = false;
+#ifdef CONFIG_TEGRA_SILICON_PLATFORM
+	status = (fsl_readl(&usb_sys_regs->vbus_wakeup) & USB_SYS_VBUS_STATUS);
+#else
+	/*On FPGA VBUS is detected through VBUS A Session instead of VBUS status. */
+	status = (fsl_readl(&usb_sys_regs->vbus_sensors) & USB_SYS_VBUS_ASESSION);
+#endif
+	return status;
+}
+
+/*-----------------------------------------------------------------
  * done() - retire a request; caller blocked irqs
  * @status : request status to be set, only works when
  *	request is still in progress.
@@ -206,7 +269,13 @@ static void done(struct fsl_ep *ep, struct fsl_req *req, int status)
 			req->req.actual, req->req.length);
 
 	ep->stopped = 1;
-
+#ifdef CONFIG_TEGRA_GADGET_BOOST_CPU_FREQ
+	if (req->req.complete && req->req.length >= BOOST_TRIGGER_SIZE) {
+		ep_queue_request_count--;
+		if (!ep_queue_request_count)
+			schedule_work(&udc->boost_cpufreq_work);
+	}
+#endif
 	spin_unlock(&ep->udc->lock);
 	/* complete() is from gadget layer,
 	 * eg fsg->bulk_in_complete() */
@@ -237,22 +306,56 @@ static void nuke(struct fsl_ep *ep, int status)
 	}
 }
 
+static int can_pullup(struct fsl_udc *udc)
+{
+	return udc->driver && udc->softconnect && udc->vbus_active;
+}
+
 /*------------------------------------------------------------------
 	Internal Hardware related function
  ------------------------------------------------------------------*/
 
+#define FSL_UDC_RESET_TIMEOUT 1000
+static int dr_controller_reset(struct fsl_udc *udc)
+{
+	unsigned int tmp;
+	unsigned long timeout;
+
+	/* Stop and reset the usb controller */
+	tmp = fsl_readl(&dr_regs->usbcmd);
+	tmp &= ~USB_CMD_RUN_STOP;
+	fsl_writel(tmp, &dr_regs->usbcmd);
+
+	tmp = fsl_readl(&dr_regs->usbcmd);
+	tmp |= USB_CMD_CTRL_RESET;
+	fsl_writel(tmp, &dr_regs->usbcmd);
+
+	/* Wait for reset to complete */
+	timeout = jiffies + FSL_UDC_RESET_TIMEOUT;
+	while (fsl_readl(&dr_regs->usbcmd) & USB_CMD_CTRL_RESET) {
+		if (time_after(jiffies, timeout)) {
+			ERR("udc reset timeout!\n");
+			return -ETIMEDOUT;
+		}
+		cpu_relax();
+	}
+	return 0;
+}
+
 static int dr_controller_setup(struct fsl_udc *udc)
 {
 	unsigned int tmp, portctrl, ep_num;
 	unsigned int max_no_of_ep;
-#ifndef CONFIG_ARCH_MXC
+#if !defined(CONFIG_ARCH_MXC) && !defined(CONFIG_ARCH_TEGRA)
 	unsigned int ctrl;
 #endif
+#ifdef CONFIG_ARCH_TEGRA
 	unsigned long timeout;
-#define FSL_UDC_RESET_TIMEOUT 1000
+#endif
+	int status;
 
 	/* Config PHY interface */
-	portctrl = fsl_readl(&dr_regs->portsc1);
+	portctrl = fsl_readl(control_reg);
 	portctrl &= ~(PORTSCX_PHY_TYPE_SEL | PORTSCX_PORT_WIDTH);
 	switch (udc->phy_mode) {
 	case FSL_USB2_PHY_ULPI:
@@ -270,26 +373,11 @@ static int dr_controller_setup(struct fsl_udc *udc)
 	default:
 		return -EINVAL;
 	}
-	fsl_writel(portctrl, &dr_regs->portsc1);
-
-	/* Stop and reset the usb controller */
-	tmp = fsl_readl(&dr_regs->usbcmd);
-	tmp &= ~USB_CMD_RUN_STOP;
-	fsl_writel(tmp, &dr_regs->usbcmd);
-
-	tmp = fsl_readl(&dr_regs->usbcmd);
-	tmp |= USB_CMD_CTRL_RESET;
-	fsl_writel(tmp, &dr_regs->usbcmd);
+	fsl_writel(portctrl, control_reg);
 
-	/* Wait for reset to complete */
-	timeout = jiffies + FSL_UDC_RESET_TIMEOUT;
-	while (fsl_readl(&dr_regs->usbcmd) & USB_CMD_CTRL_RESET) {
-		if (time_after(jiffies, timeout)) {
-			ERR("udc reset timeout!\n");
-			return -ETIMEDOUT;
-		}
-		cpu_relax();
-	}
+	status = dr_controller_reset(udc);
+	if (status)
+		return status;
 
 	/* Set the controller as device mode */
 	tmp = fsl_readl(&dr_regs->usbmode);
@@ -301,6 +389,19 @@ static int dr_controller_setup(struct fsl_udc *udc)
 		tmp |= USB_MODE_ES;
 	fsl_writel(tmp, &dr_regs->usbmode);
 
+#ifdef CONFIG_ARCH_TEGRA
+	/* Wait for controller to switch to device mode */
+	timeout = jiffies + FSL_UDC_RESET_TIMEOUT;
+	while ((fsl_readl(&dr_regs->usbmode) & USB_MODE_CTRL_MODE_DEVICE) !=
+	       USB_MODE_CTRL_MODE_DEVICE) {
+		if (time_after(jiffies, timeout)) {
+			ERR("udc device mode setup timeout!\n");
+			return -ETIMEDOUT;
+		}
+		cpu_relax();
+	}
+#endif
+
 	/* Clear the setup status */
 	fsl_writel(0, &dr_regs->usbsts);
 
@@ -321,7 +422,7 @@ static int dr_controller_setup(struct fsl_udc *udc)
 		fsl_writel(tmp, &dr_regs->endptctrl[ep_num]);
 	}
 	/* Config control enable i/o output, cpu endian register */
-#ifndef CONFIG_ARCH_MXC
+#if !defined(CONFIG_ARCH_MXC) && !defined(CONFIG_ARCH_TEGRA)
 	if (udc->pdata->have_sysif_regs) {
 		ctrl = __raw_readl(&usb_sys_regs->control);
 		ctrl |= USB_CTRL_IOENB;
@@ -349,7 +450,32 @@ static int dr_controller_setup(struct fsl_udc *udc)
 static void dr_controller_run(struct fsl_udc *udc)
 {
 	u32 temp;
+#ifdef CONFIG_ARCH_TEGRA
+	unsigned long timeout;
+#define FSL_UDC_RUN_TIMEOUT 1000
+#endif
+	/* Clear stopped bit */
+	udc->stopped = 0;
 
+/* If OTG transceiver is available, then it handles the VBUS detection */
+	if (!udc_controller->transceiver) {
+#ifdef CONFIG_TEGRA_SILICON_PLATFORM
+		/* Enable cable detection interrupt, without setting the
+		 * USB_SYS_VBUS_WAKEUP_INT bit. USB_SYS_VBUS_WAKEUP_INT is
+		 * clear on write */
+		temp = fsl_readl(&usb_sys_regs->vbus_wakeup);
+		temp |= (USB_SYS_VBUS_WAKEUP_INT_ENABLE | USB_SYS_VBUS_WAKEUP_ENABLE);
+		temp &= ~USB_SYS_VBUS_WAKEUP_INT_STATUS;
+		fsl_writel(temp, &usb_sys_regs->vbus_wakeup);
+#else
+		/*On FPGA VBUS is detected through VBUS A Session instead of VBUS
+		 * status. */
+		temp = fsl_readl(&usb_sys_regs->vbus_sensors);
+		temp |= USB_SYS_VBUS_ASESSION_INT_EN;
+		temp &= ~USB_SYS_VBUS_ASESSION_CHANGED;
+		fsl_writel(temp, &usb_sys_regs->vbus_sensors);
+#endif
+	}
 	/* Enable DR irq reg */
 	temp = USB_INTR_INT_EN | USB_INTR_ERR_INT_EN
 		| USB_INTR_PTC_DETECT_EN | USB_INTR_RESET_EN
@@ -357,9 +483,6 @@ static void dr_controller_run(struct fsl_udc *udc)
 
 	fsl_writel(temp, &dr_regs->usbintr);
 
-	/* Clear stopped bit */
-	udc->stopped = 0;
-
 	/* Set the controller as device mode */
 	temp = fsl_readl(&dr_regs->usbmode);
 	temp |= USB_MODE_CTRL_MODE_DEVICE;
@@ -367,26 +490,37 @@ static void dr_controller_run(struct fsl_udc *udc)
 
 	/* Set controller to Run */
 	temp = fsl_readl(&dr_regs->usbcmd);
-	temp |= USB_CMD_RUN_STOP;
+	if (can_pullup(udc))
+		temp |= USB_CMD_RUN_STOP;
+	else
+		temp &= ~USB_CMD_RUN_STOP;
 	fsl_writel(temp, &dr_regs->usbcmd);
+
+#ifdef CONFIG_ARCH_TEGRA
+	if (can_pullup(udc)) {
+		/* Wait for controller to start */
+		timeout = jiffies + FSL_UDC_RUN_TIMEOUT;
+		while ((fsl_readl(&dr_regs->usbcmd) & USB_CMD_RUN_STOP) !=
+		       USB_CMD_RUN_STOP) {
+			if (time_after(jiffies, timeout)) {
+				ERR("udc start timeout!\n");
+				return;
+			}
+			cpu_relax();
+		}
+	}
+#endif
+
+	return;
 }
 
 static void dr_controller_stop(struct fsl_udc *udc)
 {
 	unsigned int tmp;
 
-	pr_debug("%s\n", __func__);
-
-	/* if we're in OTG mode, and the Host is currently using the port,
-	 * stop now and don't rip the controller out from under the
-	 * ehci driver
-	 */
-	if (udc->gadget.is_otg) {
-		if (!(fsl_readl(&dr_regs->otgsc) & OTGSC_STS_USB_ID)) {
-			pr_debug("udc: Leaving early\n");
-			return;
-		}
-	}
+	/* Clear pending interrupt status bits */
+	tmp = fsl_readl(&dr_regs->usbsts);
+	fsl_writel(tmp, &dr_regs->usbsts);
 
 	/* disable all INTR */
 	fsl_writel(0, &dr_regs->usbintr);
@@ -517,9 +651,9 @@ static void ep0_setup(struct fsl_udc *udc)
 	/* the intialization of an ep includes: fields in QH, Regs,
 	 * fsl_ep struct */
 	struct_ep_qh_setup(udc, 0, USB_RECV, USB_ENDPOINT_XFER_CONTROL,
-			USB_MAX_CTRL_PAYLOAD, 0, 0);
+			USB_MAX_CTRL_PAYLOAD, 1, 0);
 	struct_ep_qh_setup(udc, 0, USB_SEND, USB_ENDPOINT_XFER_CONTROL,
-			USB_MAX_CTRL_PAYLOAD, 0, 0);
+			USB_MAX_CTRL_PAYLOAD, 1, 0);
 	dr_ep_setup(0, USB_RECV, USB_ENDPOINT_XFER_CONTROL);
 	dr_ep_setup(0, USB_SEND, USB_ENDPOINT_XFER_CONTROL);
 
@@ -642,15 +776,21 @@ static int fsl_ep_disable(struct usb_ep *_ep)
 
 	/* disable ep on controller */
 	ep_num = ep_index(ep);
-	epctrl = fsl_readl(&dr_regs->endptctrl[ep_num]);
-	if (ep_is_in(ep)) {
-		epctrl &= ~(EPCTRL_TX_ENABLE | EPCTRL_TX_TYPE);
-		epctrl |= EPCTRL_EP_TYPE_BULK << EPCTRL_TX_EP_TYPE_SHIFT;
-	} else {
-		epctrl &= ~(EPCTRL_RX_ENABLE | EPCTRL_TX_TYPE);
-		epctrl |= EPCTRL_EP_TYPE_BULK << EPCTRL_RX_EP_TYPE_SHIFT;
+#if defined(CONFIG_ARCH_TEGRA)
+	/* Touch the registers if cable is connected and phy is on */
+	if (vbus_enabled())
+#endif
+	{
+		epctrl = fsl_readl(&dr_regs->endptctrl[ep_num]);
+		if (ep_is_in(ep)) {
+			epctrl &= ~(EPCTRL_TX_ENABLE | EPCTRL_TX_TYPE);
+			epctrl |= EPCTRL_EP_TYPE_BULK << EPCTRL_TX_EP_TYPE_SHIFT;
+		} else {
+			epctrl &= ~(EPCTRL_RX_ENABLE | EPCTRL_TX_TYPE);
+			epctrl |= EPCTRL_EP_TYPE_BULK << EPCTRL_RX_EP_TYPE_SHIFT;
+		}
+		fsl_writel(epctrl, &dr_regs->endptctrl[ep_num]);
 	}
-	fsl_writel(epctrl, &dr_regs->endptctrl[ep_num]);
 
 	udc = (struct fsl_udc *)ep->udc;
 	spin_lock_irqsave(&udc->lock, flags);
@@ -710,6 +850,9 @@ static void fsl_queue_td(struct fsl_ep *ep, struct fsl_req *req)
 		? (1 << (ep_index(ep) + 16))
 		: (1 << (ep_index(ep)));
 
+	/* Flush all the dTD structs out to memory */
+	wmb();
+
 	/* check if the pipe is empty */
 	if (!(list_empty(&ep->queue))) {
 		/* Add td to the end */
@@ -717,6 +860,7 @@ static void fsl_queue_td(struct fsl_ep *ep, struct fsl_req *req)
 		lastreq = list_entry(ep->queue.prev, struct fsl_req, queue);
 		lastreq->tail->next_td_ptr =
 			cpu_to_hc32(req->head->td_dma & DTD_ADDR_MASK);
+		wmb();
 		/* Read prime bit, if 1 goto done */
 		if (fsl_readl(&dr_regs->endpointprime) & bitmask)
 			goto out;
@@ -748,6 +892,10 @@ static void fsl_queue_td(struct fsl_ep *ep, struct fsl_req *req)
 			| EP_QUEUE_HEAD_STATUS_HALT));
 	dQH->size_ioc_int_sts &= temp;
 
+#if defined(CONFIG_ARCH_TEGRA)
+	fsl_udc_ep_barrier();
+#endif
+
 	/* Ensure that updates to the QH will occur before priming. */
 	wmb();
 
@@ -767,7 +915,7 @@ out:
  * @is_last: return flag if it is the last dTD of the request
  * return: pointer to the built dTD */
 static struct ep_td_struct *fsl_build_dtd(struct fsl_req *req, unsigned *length,
-		dma_addr_t *dma, int *is_last)
+		dma_addr_t *dma, int *is_last, gfp_t gfp_flags)
 {
 	u32 swap_temp;
 	struct ep_td_struct *dtd;
@@ -776,7 +924,7 @@ static struct ep_td_struct *fsl_build_dtd(struct fsl_req *req, unsigned *length,
 	*length = min(req->req.length - req->req.actual,
 			(unsigned)EP_MAX_LENGTH_TRANSFER);
 
-	dtd = dma_pool_alloc(udc_controller->td_pool, GFP_KERNEL, dma);
+	dtd = dma_pool_alloc(udc_controller->td_pool, gfp_flags, dma);
 	if (dtd == NULL)
 		return dtd;
 
@@ -826,7 +974,7 @@ static struct ep_td_struct *fsl_build_dtd(struct fsl_req *req, unsigned *length,
 }
 
 /* Generate dtd chain for a request */
-static int fsl_req_to_dtd(struct fsl_req *req)
+static int fsl_req_to_dtd(struct fsl_req *req, gfp_t gfp_flags)
 {
 	unsigned	count;
 	int		is_last;
@@ -834,8 +982,12 @@ static int fsl_req_to_dtd(struct fsl_req *req)
 	struct ep_td_struct	*last_dtd = NULL, *dtd;
 	dma_addr_t dma;
 
+#if defined(CONFIG_ARCH_TEGRA)
+	fsl_udc_dtd_prepare();
+#endif
+
 	do {
-		dtd = fsl_build_dtd(req, &count, &dma, &is_last);
+		dtd = fsl_build_dtd(req, &count, &dma, &is_last, gfp_flags);
 		if (dtd == NULL)
 			return -ENOMEM;
 
@@ -864,8 +1016,10 @@ fsl_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 {
 	struct fsl_ep *ep = container_of(_ep, struct fsl_ep, ep);
 	struct fsl_req *req = container_of(_req, struct fsl_req, req);
-	struct fsl_udc *udc;
+	struct fsl_udc *udc = ep->udc;
 	unsigned long flags;
+	enum dma_data_direction dir;
+	int status;
 
 	/* catch various bogus parameters */
 	if (!_req || !req->req.complete || !req->req.buf
@@ -873,16 +1027,32 @@ fsl_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 		VDBG("%s, bad params", __func__);
 		return -EINVAL;
 	}
-	if (unlikely(!_ep || !ep->desc)) {
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	if (unlikely(!ep->desc)) {
 		VDBG("%s, bad ep", __func__);
+		spin_unlock_irqrestore(&udc->lock, flags);
 		return -EINVAL;
 	}
+
 	if (ep->desc->bmAttributes == USB_ENDPOINT_XFER_ISOC) {
-		if (req->req.length > ep->ep.maxpacket)
+		if (req->req.length > ep->ep.maxpacket) {
+			spin_unlock_irqrestore(&udc->lock, flags);
 			return -EMSGSIZE;
+		}
+	}
+#ifdef CONFIG_TEGRA_GADGET_BOOST_CPU_FREQ
+	if (req->req.length >= BOOST_TRIGGER_SIZE) {
+		ep_queue_request_count++;
+		if (ep_queue_request_count && boost_cpufreq_work_flag)
+			schedule_work(&udc->boost_cpufreq_work);
 	}
+#endif
+	dir = ep_is_in(ep) ? DMA_TO_DEVICE : DMA_FROM_DEVICE;
+
+	spin_unlock_irqrestore(&udc->lock, flags);
 
-	udc = ep->udc;
 	if (!udc->driver || udc->gadget.speed == USB_SPEED_UNKNOWN)
 		return -ESHUTDOWN;
 
@@ -890,18 +1060,12 @@ fsl_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 
 	/* map virtual address to hardware */
 	if (req->req.dma == DMA_ADDR_INVALID) {
-		req->req.dma = dma_map_single(ep->udc->gadget.dev.parent,
-					req->req.buf,
-					req->req.length, ep_is_in(ep)
-						? DMA_TO_DEVICE
-						: DMA_FROM_DEVICE);
+		req->req.dma = dma_map_single(udc->gadget.dev.parent,
+					req->req.buf, req->req.length, dir);
 		req->mapped = 1;
 	} else {
-		dma_sync_single_for_device(ep->udc->gadget.dev.parent,
-					req->req.dma, req->req.length,
-					ep_is_in(ep)
-						? DMA_TO_DEVICE
-						: DMA_FROM_DEVICE);
+		dma_sync_single_for_device(udc->gadget.dev.parent,
+					req->req.dma, req->req.length, dir);
 		req->mapped = 0;
 	}
 
@@ -909,16 +1073,23 @@ fsl_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 	req->req.actual = 0;
 	req->dtd_count = 0;
 
-	spin_lock_irqsave(&udc->lock, flags);
 
 	/* build dtds and push them to device queue */
-	if (!fsl_req_to_dtd(req)) {
-		fsl_queue_td(ep, req);
-	} else {
+	status = fsl_req_to_dtd(req, gfp_flags);
+	if (status)
+		goto err_unmap;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/* re-check if the ep has not been disabled */
+	if (unlikely(!ep->desc)) {
 		spin_unlock_irqrestore(&udc->lock, flags);
-		return -ENOMEM;
+		status = -EINVAL;
+		goto err_unmap;
 	}
 
+	fsl_queue_td(ep, req);
+
 	/* Update ep0 state */
 	if ((ep_index(ep) == 0))
 		udc->ep0_state = DATA_STATE_XMIT;
@@ -929,6 +1100,15 @@ fsl_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
 	spin_unlock_irqrestore(&udc->lock, flags);
 
 	return 0;
+
+err_unmap:
+	if (req->mapped) {
+		dma_unmap_single(udc->gadget.dev.parent,
+			req->req.dma, req->req.length, dir);
+		req->req.dma = DMA_ADDR_INVALID;
+		req->mapped = 0;
+	}
+	return status;
 }
 
 /* dequeues (cancels, unlinks) an I/O request from an endpoint */
@@ -949,12 +1129,19 @@ static int fsl_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
 	/* Stop the ep before we deal with the queue */
 	ep->stopped = 1;
 	ep_num = ep_index(ep);
-	epctrl = fsl_readl(&dr_regs->endptctrl[ep_num]);
-	if (ep_is_in(ep))
-		epctrl &= ~EPCTRL_TX_ENABLE;
-	else
-		epctrl &= ~EPCTRL_RX_ENABLE;
-	fsl_writel(epctrl, &dr_regs->endptctrl[ep_num]);
+
+#if defined(CONFIG_ARCH_TEGRA)
+	/* Touch the registers if cable is connected and phy is on */
+	if(vbus_enabled())
+#endif
+	{
+		epctrl = fsl_readl(&dr_regs->endptctrl[ep_num]);
+		if (ep_is_in(ep))
+			epctrl &= ~EPCTRL_TX_ENABLE;
+		else
+			epctrl &= ~EPCTRL_RX_ENABLE;
+		fsl_writel(epctrl, &dr_regs->endptctrl[ep_num]);
+	}
 
 	/* make sure it's actually queued on this endpoint */
 	list_for_each_entry(req, &ep->queue, queue) {
@@ -997,12 +1184,19 @@ static int fsl_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
 	done(ep, req, -ECONNRESET);
 
 	/* Enable EP */
-out:	epctrl = fsl_readl(&dr_regs->endptctrl[ep_num]);
-	if (ep_is_in(ep))
-		epctrl |= EPCTRL_TX_ENABLE;
-	else
-		epctrl |= EPCTRL_RX_ENABLE;
-	fsl_writel(epctrl, &dr_regs->endptctrl[ep_num]);
+out:
+#if defined(CONFIG_ARCH_TEGRA)
+	/* Touch the registers if cable is connected and phy is on */
+	if(vbus_enabled())
+#endif
+	{
+		epctrl = fsl_readl(&dr_regs->endptctrl[ep_num]);
+		if (ep_is_in(ep))
+			epctrl |= EPCTRL_TX_ENABLE;
+		else
+			epctrl |= EPCTRL_RX_ENABLE;
+		fsl_writel(epctrl, &dr_regs->endptctrl[ep_num]);
+	}
 	ep->stopped = stopped;
 
 	spin_unlock_irqrestore(&ep->udc->lock, flags);
@@ -1100,6 +1294,12 @@ static void fsl_ep_fifo_flush(struct usb_ep *_ep)
 	unsigned long timeout;
 #define FSL_UDC_FLUSH_TIMEOUT 1000
 
+#if defined(CONFIG_ARCH_TEGRA)
+	/* Touch the registers if cable is connected and phy is on */
+	if (!vbus_enabled())
+		return;
+#endif
+
 	if (!_ep) {
 		return;
 	} else {
@@ -1163,6 +1363,7 @@ static int fsl_get_frame(struct usb_gadget *gadget)
 /*-----------------------------------------------------------------------
  * Tries to wake up the host connected to this gadget
  -----------------------------------------------------------------------*/
+#ifndef CONFIG_USB_ANDROID
 static int fsl_wakeup(struct usb_gadget *gadget)
 {
 	struct fsl_udc *udc = container_of(gadget, struct fsl_udc, gadget);
@@ -1181,10 +1382,15 @@ static int fsl_wakeup(struct usb_gadget *gadget)
 	fsl_writel(portsc, &dr_regs->portsc1);
 	return 0;
 }
+#endif
 
-static int can_pullup(struct fsl_udc *udc)
+
+static int fsl_set_selfpowered(struct usb_gadget * gadget, int is_on)
 {
-	return udc->driver && udc->softconnect && udc->vbus_active;
+	struct fsl_udc *udc;
+	udc = container_of(gadget, struct fsl_udc, gadget);
+	udc->selfpowered = (is_on != 0);
+	return 0;
 }
 
 /* Notify controller that VBUS is powered, Called by whatever
@@ -1195,16 +1401,49 @@ static int fsl_vbus_session(struct usb_gadget *gadget, int is_active)
 	unsigned long	flags;
 
 	udc = container_of(gadget, struct fsl_udc, gadget);
-	spin_lock_irqsave(&udc->lock, flags);
+
 	VDBG("VBUS %s", is_active ? "on" : "off");
-	udc->vbus_active = (is_active != 0);
-	if (can_pullup(udc))
-		fsl_writel((fsl_readl(&dr_regs->usbcmd) | USB_CMD_RUN_STOP),
-				&dr_regs->usbcmd);
-	else
-		fsl_writel((fsl_readl(&dr_regs->usbcmd) & ~USB_CMD_RUN_STOP),
-				&dr_regs->usbcmd);
-	spin_unlock_irqrestore(&udc->lock, flags);
+	if (udc->vbus_active && !is_active) {
+		/* If cable disconnected, cancel any delayed work */
+		cancel_delayed_work(&udc->work);
+		spin_lock_irqsave(&udc->lock, flags);
+		/* reset all internal Queues and inform client driver */
+		reset_queues(udc);
+		/* stop the controller and turn off the clocks */
+		dr_controller_stop(udc);
+		dr_controller_reset(udc);
+		udc->vbus_active = 0;
+		udc->usb_state = USB_STATE_DEFAULT;
+		spin_unlock_irqrestore(&udc->lock, flags);
+		fsl_udc_clk_suspend(false);
+		if (udc->vbus_regulator) {
+			/* set the current limit to 0mA */
+			regulator_set_current_limit(
+				udc->vbus_regulator, 0, 0);
+		}
+	} else if (!udc->vbus_active && is_active) {
+		fsl_udc_clk_resume(false);
+		/* setup the controller in the device mode */
+		dr_controller_setup(udc);
+		/* setup EP0 for setup packet */
+		ep0_setup(udc);
+		/* initialize the USB and EP states */
+		udc->usb_state = USB_STATE_ATTACHED;
+		udc->ep0_state = WAIT_FOR_SETUP;
+		udc->ep0_dir = 0;
+		udc->vbus_active = 1;
+		/* start the controller */
+		dr_controller_run(udc);
+		if (udc->vbus_regulator) {
+			/* set the current limit to 100mA */
+			regulator_set_current_limit(
+				udc->vbus_regulator, 0, 100);
+		}
+		/* Schedule work to wait for 1000 msec and check for
+		 * charger if setup packet is not received */
+		schedule_delayed_work(&udc->work,
+			USB_CHARGER_DETECTION_WAIT_TIME_MS);
+	}
 	return 0;
 }
 
@@ -1220,6 +1459,12 @@ static int fsl_vbus_draw(struct usb_gadget *gadget, unsigned mA)
 	struct fsl_udc *udc;
 
 	udc = container_of(gadget, struct fsl_udc, gadget);
+	/* check udc regulator is available for drawing the vbus current */
+	if (udc->vbus_regulator) {
+		udc->current_limit = mA;
+		schedule_work(&udc->charger_work);
+	}
+
 	if (udc->transceiver)
 		return otg_set_power(udc->transceiver, mA);
 	return -ENOTSUPP;
@@ -1234,13 +1479,16 @@ static int fsl_pullup(struct usb_gadget *gadget, int is_on)
 
 	udc = container_of(gadget, struct fsl_udc, gadget);
 	udc->softconnect = (is_on != 0);
+	if (udc_controller->transceiver &&
+		udc_controller->transceiver->state != OTG_STATE_B_PERIPHERAL)
+		return 0;
+
 	if (can_pullup(udc))
 		fsl_writel((fsl_readl(&dr_regs->usbcmd) | USB_CMD_RUN_STOP),
 				&dr_regs->usbcmd);
 	else
 		fsl_writel((fsl_readl(&dr_regs->usbcmd) & ~USB_CMD_RUN_STOP),
 				&dr_regs->usbcmd);
-
 	return 0;
 }
 
@@ -1250,8 +1498,10 @@ static int fsl_stop(struct usb_gadget_driver *driver);
 /* defined in gadget.h */
 static struct usb_gadget_ops fsl_gadget_ops = {
 	.get_frame = fsl_get_frame,
+#ifndef CONFIG_USB_ANDROID
 	.wakeup = fsl_wakeup,
-/*	.set_selfpowered = fsl_set_selfpowered,	*/ /* Always selfpowered */
+#endif
+	.set_selfpowered = fsl_set_selfpowered,
 	.vbus_session = fsl_vbus_session,
 	.vbus_draw = fsl_vbus_draw,
 	.pullup = fsl_pullup,
@@ -1299,7 +1549,7 @@ static int ep0_prime_status(struct fsl_udc *udc, int direction)
 			ep_is_in(ep) ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
 	req->mapped = 1;
 
-	if (fsl_req_to_dtd(req) == 0)
+	if (fsl_req_to_dtd(req, GFP_ATOMIC) == 0)
 		fsl_queue_td(ep, req);
 	else
 		return -ENOMEM;
@@ -1345,7 +1595,8 @@ static void ch9getstatus(struct fsl_udc *udc, u8 request_type, u16 value,
 
 	if ((request_type & USB_RECIP_MASK) == USB_RECIP_DEVICE) {
 		/* Get device status */
-		tmp = 1 << USB_DEVICE_SELF_POWERED;
+		if (udc->selfpowered)
+			tmp = 1 << USB_DEVICE_SELF_POWERED;
 		tmp |= udc->remote_wakeup << USB_DEVICE_REMOTE_WAKEUP;
 	} else if ((request_type & USB_RECIP_MASK) == USB_RECIP_INTERFACE) {
 		/* Get interface status */
@@ -1377,13 +1628,25 @@ static void ch9getstatus(struct fsl_udc *udc, u8 request_type, u16 value,
 	req->req.complete = NULL;
 	req->dtd_count = 0;
 
-	req->req.dma = dma_map_single(ep->udc->gadget.dev.parent,
-				req->req.buf, req->req.length,
-				ep_is_in(ep) ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
-	req->mapped = 1;
+	/* map virtual address to hardware */
+	if (req->req.dma == DMA_ADDR_INVALID) {
+		req->req.dma = dma_map_single(ep->udc->gadget.dev.parent,
+					req->req.buf,
+					req->req.length, ep_is_in(ep)
+						? DMA_TO_DEVICE
+						: DMA_FROM_DEVICE);
+		req->mapped = 1;
+	} else {
+		dma_sync_single_for_device(ep->udc->gadget.dev.parent,
+					req->req.dma, req->req.length,
+					ep_is_in(ep)
+						? DMA_TO_DEVICE
+						: DMA_FROM_DEVICE);
+		req->mapped = 0;
+	}
 
 	/* prime the data phase */
-	if ((fsl_req_to_dtd(req) == 0))
+	if ((fsl_req_to_dtd(req, GFP_ATOMIC) == 0))
 		fsl_queue_td(ep, req);
 	else			/* no mem */
 		goto stall;
@@ -1395,6 +1658,107 @@ stall:
 	ep0stall(udc);
 }
 
+static void udc_test_mode(struct fsl_udc *udc, u32 test_mode)
+{
+	struct fsl_req *req;
+	struct fsl_ep *ep;
+	u32 portsc, bitmask;
+	unsigned long timeout;
+
+	/* Ack the ep0 IN */
+	if (ep0_prime_status(udc, EP_DIR_IN))
+		ep0stall(udc);
+
+	/* get the ep0 */
+	ep = &udc->eps[0];
+	bitmask = ep_is_in(ep)
+		? (1 << (ep_index(ep) + 16))
+		: (1 << (ep_index(ep)));
+
+	timeout = jiffies + HZ;
+	/* Wait until ep0 IN endpoint txfr is complete */
+	while (!(fsl_readl(&dr_regs->endptcomplete) & bitmask)) {
+		if (time_after(jiffies, timeout)) {
+			pr_err("Timeout for Ep0 IN Ack\n");
+			break;
+		}
+		cpu_relax();
+	}
+
+	switch (test_mode << PORTSCX_PTC_BIT_POS) {
+	case PORTSCX_PTC_JSTATE:
+		VDBG("TEST_J\n");
+		break;
+	case PORTSCX_PTC_KSTATE:
+		VDBG("TEST_K\n");
+		break;
+	case PORTSCX_PTC_SEQNAK:
+		VDBG("TEST_SE0_NAK\n");
+		break;
+	case PORTSCX_PTC_PACKET:
+		VDBG("TEST_PACKET\n");
+
+		/* get the ep and configure for IN direction */
+		ep = &udc->eps[0];
+		udc->ep0_dir = USB_DIR_IN;
+
+		/* Initialize ep0 status request structure */
+		req = container_of(fsl_alloc_request(NULL, GFP_ATOMIC),
+				struct fsl_req, req);
+		/* allocate a small amount of memory to get valid address */
+		req->req.buf = kmalloc(sizeof(fsl_udc_test_packet), GFP_ATOMIC);
+		req->req.dma = virt_to_phys(req->req.buf);
+
+		/* Fill in the reqest structure */
+		memcpy(req->req.buf, fsl_udc_test_packet, sizeof(fsl_udc_test_packet));
+		req->ep = ep;
+		req->req.length = sizeof(fsl_udc_test_packet);
+		req->req.status = -EINPROGRESS;
+		req->req.actual = 0;
+		req->req.complete = NULL;
+		req->dtd_count = 0;
+		req->mapped = 0;
+
+		dma_sync_single_for_device(ep->udc->gadget.dev.parent,
+					req->req.dma, req->req.length,
+					ep_is_in(ep)
+						? DMA_TO_DEVICE
+						: DMA_FROM_DEVICE);
+
+		/* prime the data phase */
+		if ((fsl_req_to_dtd(req, GFP_ATOMIC) == 0))
+			fsl_queue_td(ep, req);
+		else			/* no mem */
+			goto stall;
+
+		list_add_tail(&req->queue, &ep->queue);
+		udc->ep0_state = DATA_STATE_XMIT;
+		break;
+	case PORTSCX_PTC_FORCE_EN:
+		VDBG("TEST_FORCE_EN\n");
+		break;
+	default:
+		ERR("udc unknown test mode[%d]!\n", test_mode);
+		goto stall;
+	}
+
+	/* read the portsc register */
+	portsc = fsl_readl(&dr_regs->portsc1);
+	/* set the test mode selector */
+	portsc |= test_mode << PORTSCX_PTC_BIT_POS;
+	fsl_writel(portsc, &dr_regs->portsc1);
+
+	/*
+	 * The device must have its power cycled to exit test mode.
+	 * See USB 2.0 spec, section 9.4.9 for test modes operation in "Set Feature"
+	 * See USB 2.0 spec, section 7.1.20 for test modes.
+	 */
+	pr_info("udc entering the test mode, power cycle to exit test mode\n");
+	return;
+stall:
+	ep0stall(udc);
+}
+
 static void setup_received_irq(struct fsl_udc *udc,
 		struct usb_ctrlrequest *setup)
 {
@@ -1419,6 +1783,11 @@ static void setup_received_irq(struct fsl_udc *udc,
 		if (setup->bRequestType != (USB_DIR_OUT | USB_TYPE_STANDARD
 						| USB_RECIP_DEVICE))
 			break;
+#ifdef CONFIG_ARCH_TEGRA
+		/* This delay is necessary for some windows drivers to
+		 * properly recognize the device */
+		mdelay(1);
+#endif
 		ch9setaddress(udc, wValue, wIndex, wLength);
 		return;
 
@@ -1429,7 +1798,17 @@ static void setup_received_irq(struct fsl_udc *udc,
 		int rc = -EOPNOTSUPP;
 		u16 ptc = 0;
 
-		if ((setup->bRequestType & (USB_RECIP_MASK | USB_TYPE_MASK))
+		if (setup->bRequestType == USB_RECIP_DEVICE &&
+				 wValue == USB_DEVICE_TEST_MODE) {
+			/*
+			 * If the feature selector is TEST_MODE, then the most
+			 * significant byte of wIndex is used to specify the specific
+			 * test mode and the lower byte of wIndex must be zero.
+			 */
+			udc_test_mode(udc, wIndex >> 8);
+			return;
+
+		} else if ((setup->bRequestType & (USB_RECIP_MASK | USB_TYPE_MASK))
 				== (USB_RECIP_ENDPOINT | USB_TYPE_STANDARD)) {
 			int pipe = get_pipe_by_windex(wIndex);
 			struct fsl_ep *ep;
@@ -1492,7 +1871,7 @@ static void setup_received_irq(struct fsl_udc *udc,
 		udc->ep0_dir = (setup->bRequestType & USB_DIR_IN)
 				?  USB_DIR_IN : USB_DIR_OUT;
 		spin_unlock(&udc->lock);
-		if (udc->driver->setup(&udc->gadget,
+		if (udc->driver && udc->driver->setup(&udc->gadget,
 				&udc->local_setup_buff) < 0)
 			ep0stall(udc);
 		spin_lock(&udc->lock);
@@ -1502,7 +1881,7 @@ static void setup_received_irq(struct fsl_udc *udc,
 		/* No data phase, IN status from gadget */
 		udc->ep0_dir = USB_DIR_IN;
 		spin_unlock(&udc->lock);
-		if (udc->driver->setup(&udc->gadget,
+		if (udc->driver && udc->driver->setup(&udc->gadget,
 				&udc->local_setup_buff) < 0)
 			ep0stall(udc);
 		spin_lock(&udc->lock);
@@ -1601,6 +1980,13 @@ static int process_ep_req(struct fsl_udc *udc, int pipe,
 	actual = curr_req->req.length;
 
 	for (j = 0; j < curr_req->dtd_count; j++) {
+#ifdef CONFIG_ARCH_TEGRA
+		/* Fence read for coherency of AHB master intiated writes */
+		readb(IO_ADDRESS(IO_PPCS_PHYS + USB1_PREFETCH_ID));
+#endif
+		dma_sync_single_for_cpu(udc->gadget.dev.parent, curr_td->td_dma,
+			sizeof(struct ep_td_struct), DMA_FROM_DEVICE);
+
 		remaining_length = (hc32_to_cpu(curr_td->size_ioc_sts)
 					& DTD_PACKET_SIZE)
 				>> DTD_LENGTH_BIT_POS;
@@ -1677,7 +2063,12 @@ static void dtd_complete_irq(struct fsl_udc *udc)
 	if (!bit_pos)
 		return;
 
+#ifdef CONFIG_ARCH_TEGRA
+	/* XXX what's going on here */
+	for (i = 0; i < udc->max_ep; i++) {
+#else
 	for (i = 0; i < udc->max_ep * 2; i++) {
+#endif
 		ep_num = i >> 1;
 		direction = i % 2;
 
@@ -1720,13 +2111,10 @@ static void port_change_irq(struct fsl_udc *udc)
 {
 	u32 speed;
 
-	if (udc->bus_reset)
-		udc->bus_reset = 0;
-
 	/* Bus resetting is finished */
 	if (!(fsl_readl(&dr_regs->portsc1) & PORTSCX_PORT_RESET)) {
 		/* Get the speed */
-		speed = (fsl_readl(&dr_regs->portsc1)
+		speed = (fsl_readl(control_reg)
 				& PORTSCX_PORT_SPEED_MASK);
 		switch (speed) {
 		case PORTSCX_PORT_SPEED_HIGH:
@@ -1756,7 +2144,7 @@ static void suspend_irq(struct fsl_udc *udc)
 	udc->usb_state = USB_STATE_SUSPENDED;
 
 	/* report suspend to the driver, serial.c does not support this */
-	if (udc->driver->suspend)
+	if (udc->driver && udc->driver->suspend)
 		udc->driver->suspend(&udc->gadget);
 }
 
@@ -1766,7 +2154,7 @@ static void bus_resume(struct fsl_udc *udc)
 	udc->resume_state = 0;
 
 	/* report resume to the driver, serial.c does not support this */
-	if (udc->driver->resume)
+	if (udc->driver && udc->driver->resume)
 		udc->driver->resume(&udc->gadget);
 }
 
@@ -1780,7 +2168,8 @@ static int reset_queues(struct fsl_udc *udc)
 
 	/* report disconnect; the driver is already quiesced */
 	spin_unlock(&udc->lock);
-	udc->driver->disconnect(&udc->gadget);
+	if (udc->driver && udc->driver->disconnect)
+		udc->driver->disconnect(&udc->gadget);
 	spin_lock(&udc->lock);
 
 	return 0;
@@ -1828,10 +2217,17 @@ static void reset_irq(struct fsl_udc *udc)
 	/* Write 1s to the flush register */
 	fsl_writel(0xffffffff, &dr_regs->endptflush);
 
+#if defined(CONFIG_ARCH_TEGRA)
+	/* When the bus reset is seen on Tegra, the PORTSCX_PORT_RESET bit
+	 * is not set */
+	VDBG("Bus reset");
+	/* Reset all the queues, include XD, dTD, EP queue
+	 * head and TR Queue */
+	reset_queues(udc);
+	udc->usb_state = USB_STATE_DEFAULT;
+#else
 	if (fsl_readl(&dr_regs->portsc1) & PORTSCX_PORT_RESET) {
 		VDBG("Bus reset");
-		/* Bus is reseting */
-		udc->bus_reset = 1;
 		/* Reset all the queues, include XD, dTD, EP queue
 		 * head and TR Queue */
 		reset_queues(udc);
@@ -1851,8 +2247,90 @@ static void reset_irq(struct fsl_udc *udc)
 		dr_controller_run(udc);
 		udc->usb_state = USB_STATE_ATTACHED;
 	}
+#endif
 }
 
+static void fsl_udc_set_current_limit_work(struct work_struct* work)
+{
+	struct fsl_udc *udc = container_of (work, struct fsl_udc, charger_work);
+
+	/* check udc regulator is available for drawing vbus current*/
+	if (udc->vbus_regulator) {
+		/* set the current limit in uA */
+		regulator_set_current_limit(
+			udc->vbus_regulator, 0,
+			udc->current_limit *1000);
+	}
+}
+
+#ifdef CONFIG_TEGRA_GADGET_BOOST_CPU_FREQ
+static void fsl_udc_boost_cpu_frequency_work(struct work_struct* work)
+{
+	if (ep_queue_request_count && boost_cpufreq_work_flag) {
+		pm_qos_update_request(&boost_cpu_freq_req, (s32)CONFIG_TEGRA_GADGET_BOOST_CPU_FREQ * 1000);
+		boost_cpufreq_work_flag = 0;
+	} else if (!ep_queue_request_count && !boost_cpufreq_work_flag) {
+		pm_qos_update_request(&boost_cpu_freq_req, PM_QOS_DEFAULT_VALUE);
+		boost_cpufreq_work_flag = 1;
+	}
+}
+#endif
+
+static void fsl_udc_irq_work(struct work_struct* irq_work)
+{
+	struct fsl_udc *udc = container_of (irq_work, struct fsl_udc, irq_work);
+	bool cable_connected = false;
+	u32 temp = 0;
+
+	temp = fsl_readl(&usb_sys_regs->vbus_wakeup);
+	/* Check whether cable is connected*/
+	if (temp & USB_SYS_VBUS_STATUS)
+		fsl_vbus_session(&udc->gadget, 1);
+	else
+		fsl_vbus_session(&udc->gadget, 0);
+}
+
+/*
+ * If VBUS is detected and setup packet is not received in 100ms then
+ * work thread starts and checks for the USB charger detection.
+ */
+static void fsl_udc_charger_detect_work(struct work_struct* work)
+{
+	struct fsl_udc *udc = container_of (work, struct fsl_udc, work.work);
+
+	/* check for the platform charger detection */
+	if (fsl_udc_charger_detect()) {
+		printk(KERN_INFO "USB compliant charger detected\n");
+		/* check udc regulator is available for drawing vbus current*/
+		if (udc->vbus_regulator) {
+			/* set the current limit in uA */
+			regulator_set_current_limit(
+				udc->vbus_regulator, 0,
+				USB_CHARGING_CURRENT_LIMIT_MA*1000);
+		}
+	}
+}
+
+#if defined(CONFIG_ARCH_TEGRA)
+/*
+ * Restart device controller in the OTG mode on VBUS detection
+ */
+static void fsl_udc_restart(struct fsl_udc *udc)
+{
+	/* setup the controller in the device mode */
+	dr_controller_setup(udc);
+	/* setup EP0 for setup packet */
+	ep0_setup(udc);
+	/* start the controller */
+	udc->vbus_active = 1;
+	dr_controller_run(udc);
+	/* initialize the USB and EP states */
+	udc->usb_state = USB_STATE_ATTACHED;
+	udc->ep0_state = WAIT_FOR_SETUP;
+	udc->ep0_dir = 0;
+}
+#endif
+
 /*
  * USB device controller interrupt handler
  */
@@ -1863,10 +2341,37 @@ static irqreturn_t fsl_udc_irq(int irq, void *_udc)
 	irqreturn_t status = IRQ_NONE;
 	unsigned long flags;
 
-	/* Disable ISR for OTG host mode */
-	if (udc->stopped)
-		return IRQ_NONE;
 	spin_lock_irqsave(&udc->lock, flags);
+
+	if (!udc->transceiver)
+	{
+		u32 temp = fsl_readl(&usb_sys_regs->vbus_wakeup);
+		/* write back the register to clear the interrupt */
+		fsl_writel(temp, &usb_sys_regs->vbus_wakeup);
+		if (temp & USB_SYS_VBUS_WAKEUP_INT_STATUS) {
+				schedule_work(&udc->irq_work);
+		}
+		status = IRQ_HANDLED;
+	}
+
+	/* Disable ISR for OTG host mode */
+	if (udc->stopped) {
+		spin_unlock_irqrestore(&udc->lock, flags);
+		return status;
+	}
+
+#ifdef CONFIG_ARCH_TEGRA
+	/* Fence read for coherency of AHB master intiated writes */
+	readb(IO_ADDRESS(IO_PPCS_PHYS + USB1_PREFETCH_ID));
+#endif
+#ifndef CONFIG_TEGRA_SILICON_PLATFORM
+	{
+		u32 temp = fsl_readl(&usb_sys_regs->vbus_sensors);
+		udc->vbus_active = (temp & USB_SYS_VBUS_ASESSION) ? true : false;
+		/* write back the register to clear the interrupt */
+		fsl_writel(temp, &usb_sys_regs->vbus_sensors);
+	}
+#endif
 	irq_src = fsl_readl(&dr_regs->usbsts) & fsl_readl(&dr_regs->usbintr);
 	/* Clear notification bits */
 	fsl_writel(irq_src, &dr_regs->usbsts);
@@ -1883,6 +2388,9 @@ static irqreturn_t fsl_udc_irq(int irq, void *_udc)
 		VDBG("Packet int");
 		/* Setup package, we only support ep0 as control ep */
 		if (fsl_readl(&dr_regs->endptsetupstat) & EP_SETUP_STATUS_EP0) {
+			/* Setup packet received, we are connected to host and
+			 * not charger. Cancel any delayed work */
+			__cancel_delayed_work(&udc->work);
 			tripwire_handler(udc, 0,
 					(u8 *) (&udc->local_setup_buff));
 			setup_received_irq(udc, &udc->local_setup_buff);
@@ -1909,7 +2417,6 @@ static irqreturn_t fsl_udc_irq(int irq, void *_udc)
 
 	/* Reset Received */
 	if (irq_src & USB_STS_RESET) {
-		VDBG("reset int");
 		reset_irq(udc);
 		status = IRQ_HANDLED;
 	}
@@ -1967,30 +2474,15 @@ static int fsl_start(struct usb_gadget_driver *driver,
 		goto out;
 	}
 
-	if (udc_controller->transceiver) {
-		/* Suspend the controller until OTG enable it */
-		udc_controller->stopped = 1;
-		printk(KERN_INFO "Suspend udc for OTG auto detect\n");
-
-		/* connect to bus through transceiver */
-		if (udc_controller->transceiver) {
-			retval = otg_set_peripheral(udc_controller->transceiver,
-						    &udc_controller->gadget);
-			if (retval < 0) {
-				ERR("can't bind to transceiver\n");
-				driver->unbind(&udc_controller->gadget);
-				udc_controller->gadget.dev.driver = 0;
-				udc_controller->driver = 0;
-				return retval;
-			}
-		}
-	} else {
-		/* Enable DR IRQ reg and set USBCMD reg Run bit */
+	/* Enable DR IRQ reg and Set usbcmd reg  Run bit */
+	if (!udc_controller->transceiver) {
 		dr_controller_run(udc_controller);
 		udc_controller->usb_state = USB_STATE_ATTACHED;
 		udc_controller->ep0_state = WAIT_FOR_SETUP;
 		udc_controller->ep0_dir = 0;
+		udc_controller->vbus_active = vbus_enabled();
 	}
+
 	printk(KERN_INFO "%s: bind to driver %s\n",
 			udc_controller->gadget.name, driver->driver.name);
 
@@ -2013,9 +2505,6 @@ static int fsl_stop(struct usb_gadget_driver *driver)
 	if (!driver || driver != udc_controller->driver || !driver->unbind)
 		return -EINVAL;
 
-	if (udc_controller->transceiver)
-		otg_set_peripheral(udc_controller->transceiver, NULL);
-
 	/* stop DR, disable intr */
 	dr_controller_stop(udc_controller);
 
@@ -2053,7 +2542,11 @@ static int fsl_stop(struct usb_gadget_driver *driver)
 
 #include <linux/seq_file.h>
 
+#ifdef CONFIG_ARCH_TEGRA
+static const char proc_filename[] = "driver/fsl_tegra_udc";
+#else
 static const char proc_filename[] = "driver/fsl_usb2_udc";
+#endif
 
 static int fsl_proc_read(char *page, char **start, off_t off, int count,
 		int *eof, void *_dev)
@@ -2064,6 +2557,7 @@ static int fsl_proc_read(char *page, char **start, off_t off, int count,
 	unsigned long flags;
 	int t, i;
 	u32 tmp_reg;
+	u32 tmp_reg2;
 	struct fsl_ep *ep = NULL;
 	struct fsl_req *req;
 
@@ -2147,6 +2641,13 @@ static int fsl_proc_read(char *page, char **start, off_t off, int count,
 	next += t;
 
 	tmp_reg = fsl_readl(&dr_regs->portsc1);
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	tmp_reg2 = tmp_reg;
+#else
+	/* In Tegra3 the Phy Type Select(PTS) and Port Speed fields are specified in
+	 * hostpc1devlc register instead of portsc1 register. */
+	tmp_reg2 = fsl_readl(&dr_regs->hostpc1devlc);
+#endif
 	t = scnprintf(next, size,
 		"USB Port Status&Control Reg:\n"
 		"Port Transceiver Type : %s Port Speed: %s\n"
@@ -2157,7 +2658,7 @@ static int fsl_proc_read(char *page, char **start, off_t off, int count,
 		"Port Enabled/Disabled: %s "
 		"Current Connect Status: %s\n\n", ( {
 			char *s;
-			switch (tmp_reg & PORTSCX_PTS_FSLS) {
+			switch (tmp_reg2 & PORTSCX_PTS_FSLS) {
 			case PORTSCX_PTS_UTMI:
 				s = "UTMI"; break;
 			case PORTSCX_PTS_ULPI:
@@ -2169,7 +2670,7 @@ static int fsl_proc_read(char *page, char **start, off_t off, int count,
 			}
 			s;} ), ( {
 			char *s;
-			switch (tmp_reg & PORTSCX_PORT_SPEED_UNDEF) {
+			switch (tmp_reg2 & PORTSCX_PORT_SPEED_UNDEF) {
 			case PORTSCX_PORT_SPEED_FULL:
 				s = "Full Speed"; break;
 			case PORTSCX_PORT_SPEED_LOW:
@@ -2235,7 +2736,7 @@ static int fsl_proc_read(char *page, char **start, off_t off, int count,
 	size -= t;
 	next += t;
 
-#ifndef CONFIG_ARCH_MXC
+#if !defined(CONFIG_ARCH_MXC) && !defined(CONFIG_ARCH_TEGRA)
 	if (udc->pdata->have_sysif_regs) {
 		tmp_reg = usb_sys_regs->snoop1;
 		t = scnprintf(next, size, "Snoop1 Reg : = [0x%x]\n\n", tmp_reg);
@@ -2352,6 +2853,13 @@ static int __init struct_udc_setup(struct fsl_udc *udc,
 		return -1;
 	}
 
+#ifdef CONFIG_ARCH_TEGRA
+	/* Tegra uses hardware queue heads */
+	size = udc->max_ep * sizeof(struct ep_queue_head);
+	udc->ep_qh = (struct ep_queue_head *)((u8 *)dr_regs + QH_OFFSET);
+	udc->ep_qh_dma = platform_get_resource(pdev, IORESOURCE_MEM, 0)->start +
+		QH_OFFSET;
+#else
 	/* initialized QHs, take care of alignment */
 	size = udc->max_ep * sizeof(struct ep_queue_head);
 	if (size < QH_ALIGNMENT)
@@ -2367,6 +2875,7 @@ static int __init struct_udc_setup(struct fsl_udc *udc,
 		kfree(udc->eps);
 		return -1;
 	}
+#endif
 
 	udc->ep_qh_size = size;
 
@@ -2431,6 +2940,9 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 	int ret = -ENODEV;
 	unsigned int i;
 	u32 dccparams;
+#if defined(CONFIG_ARCH_TEGRA)
+	struct resource *res_sys = NULL;
+#endif
 
 	if (strcmp(pdev->name, driver_name)) {
 		VDBG("Wrong device");
@@ -2448,30 +2960,17 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 	spin_lock_init(&udc_controller->lock);
 	udc_controller->stopped = 1;
 
-#ifdef CONFIG_USB_OTG
-	if (pdata->operating_mode == FSL_USB2_DR_OTG) {
-		udc_controller->transceiver = otg_get_transceiver();
-		if (!udc_controller->transceiver) {
-			ERR("Can't find OTG driver!\n");
-			ret = -ENODEV;
-			goto err_kfree;
-		}
-	}
-#endif
-
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		ret = -ENXIO;
 		goto err_kfree;
 	}
 
-	if (pdata->operating_mode == FSL_USB2_DR_DEVICE) {
-		if (!request_mem_region(res->start, resource_size(res),
-					driver_name)) {
-			ERR("request mem region for %s failed\n", pdev->name);
-			ret = -EBUSY;
-			goto err_kfree;
-		}
+	if (!request_mem_region(res->start, resource_size(res),
+				driver_name)) {
+		ERR("request mem region for %s failed\n", pdev->name);
+		ret = -EBUSY;
+		goto err_kfree;
 	}
 
 	dr_regs = ioremap(res->start, resource_size(res));
@@ -2493,7 +2992,26 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 	/* Set accessors only after pdata->init() ! */
 	fsl_set_accessors(pdata);
 
-#ifndef CONFIG_ARCH_MXC
+#if defined(CONFIG_ARCH_TEGRA)
+	/* If the PHY registers are NOT provided as a seperate aperture, then
+	 * we should be using the registers inside the controller aperture. */
+	res_sys = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (res_sys)  {
+		usb_sys_regs = ioremap(res_sys->start, resource_size(res_sys));
+		if (!usb_sys_regs)
+			goto err_release_mem_region;
+	} else {
+		usb_sys_regs = (struct usb_sys_interface *)
+			((u32)dr_regs + USB_DR_SYS_OFFSET);
+	}
+#endif
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	control_reg = &dr_regs->portsc1;
+#else
+	control_reg = &dr_regs->hostpc1devlc;
+#endif
+#if !defined(CONFIG_ARCH_MXC) && !defined(CONFIG_ARCH_TEGRA)
 	if (pdata->have_sysif_regs)
 		usb_sys_regs = (struct usb_sys_interface *)
 				((u32)dr_regs + USB_DR_SYS_OFFSET);
@@ -2536,11 +3054,9 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 		goto err_free_irq;
 	}
 
-	if (!udc_controller->transceiver) {
-		/* initialize usb hw reg except for regs for EP,
-		 * leave usbintr reg untouched */
-		dr_controller_setup(udc_controller);
-	}
+	/* initialize usb hw reg except for regs for EP,
+	 * leave usbintr reg untouched */
+	dr_controller_setup(udc_controller);
 
 	fsl_udc_clk_finalize(pdev);
 
@@ -2560,9 +3076,6 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto err_free_irq;
 
-	if (udc_controller->transceiver)
-		udc_controller->gadget.is_otg = 1;
-
 	/* setup QH and epctrl for ep0 */
 	ep0_setup(udc_controller);
 
@@ -2599,6 +3112,51 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 		goto err_del_udc;
 
 	create_proc_file();
+#ifdef CONFIG_TEGRA_GADGET_BOOST_CPU_FREQ
+	boost_cpufreq_work_flag = 1;
+	ep_queue_request_count = 0;
+#endif
+	/* create a delayed work for detecting the USB charger */
+	INIT_DELAYED_WORK(&udc_controller->work, fsl_udc_charger_detect_work);
+	INIT_WORK(&udc_controller->charger_work, fsl_udc_set_current_limit_work);
+#ifdef CONFIG_TEGRA_GADGET_BOOST_CPU_FREQ
+	INIT_WORK(&udc_controller->boost_cpufreq_work, fsl_udc_boost_cpu_frequency_work);
+	pm_qos_add_request(&boost_cpu_freq_req, PM_QOS_CPU_FREQ_MIN, PM_QOS_DEFAULT_VALUE);
+#endif
+	/* create a work for controlling the clocks to the phy if otg is disabled */
+	INIT_WORK(&udc_controller->irq_work, fsl_udc_irq_work);
+
+	/* Get the regulator for drawing the vbus current in udc driver */
+	udc_controller->vbus_regulator = regulator_get(NULL, "usb_bat_chg");
+	if (IS_ERR(udc_controller->vbus_regulator)) {
+		dev_info(&pdev->dev,
+			"usb_bat_chg regulator not registered:"
+				" USB charging will not be enabled\n");
+		udc_controller->vbus_regulator = NULL;
+	}
+
+#ifdef CONFIG_USB_OTG_UTILS
+	udc_controller->transceiver = otg_get_transceiver();
+	if (udc_controller->transceiver) {
+		dr_controller_stop(udc_controller);
+		dr_controller_reset(udc_controller);
+		fsl_udc_clk_suspend(false);
+		udc_controller->vbus_active = 0;
+		udc_controller->usb_state = USB_STATE_DEFAULT;
+		otg_set_peripheral(udc_controller->transceiver, &udc_controller->gadget);
+	}
+#else
+#ifdef CONFIG_ARCH_TEGRA
+	/* Power down the phy if cable is not connected */
+	if(!vbus_enabled())
+		fsl_udc_clk_suspend(false);
+#endif
+#endif
+
+	/* This should done ideally if board does not have pmu interrupt */
+	if (!udc_controller->transceiver)
+		fsl_udc_clk_enable();
+
 	return 0;
 
 err_del_udc:
@@ -2614,8 +3172,7 @@ err_iounmap:
 err_iounmap_noclk:
 	iounmap(dr_regs);
 err_release_mem_region:
-	if (pdata->operating_mode == FSL_USB2_DR_DEVICE)
-		release_mem_region(res->start, resource_size(res));
+	release_mem_region(res->start, resource_size(res));
 err_kfree:
 	kfree(udc_controller);
 	udc_controller = NULL;
@@ -2638,6 +3195,16 @@ static int __exit fsl_udc_remove(struct platform_device *pdev)
 	usb_del_gadget_udc(&udc_controller->gadget);
 	udc_controller->done = &done;
 
+	cancel_delayed_work(&udc_controller->work);
+#ifdef CONFIG_TEGRA_GADGET_BOOST_CPU_FREQ
+	cancel_work_sync(&udc_controller->boost_cpufreq_work);
+#endif
+	if (udc_controller->vbus_regulator)
+		regulator_put(udc_controller->vbus_regulator);
+
+	if (udc_controller->transceiver)
+		otg_set_peripheral(udc_controller->transceiver, NULL);
+
 	fsl_udc_clk_release();
 
 	/* DR has been stopped in usb_gadget_unregister_driver() */
@@ -2651,8 +3218,7 @@ static int __exit fsl_udc_remove(struct platform_device *pdev)
 	dma_pool_destroy(udc_controller->td_pool);
 	free_irq(udc_controller->irq, udc_controller);
 	iounmap(dr_regs);
-	if (pdata->operating_mode == FSL_USB2_DR_DEVICE)
-		release_mem_region(res->start, resource_size(res));
+	release_mem_region(res->start, resource_size(res));
 
 	device_unregister(&udc_controller->gadget.dev);
 	/* free udc --wait for the release() finished */
@@ -2674,8 +3240,27 @@ static int __exit fsl_udc_remove(struct platform_device *pdev)
  -----------------------------------------------------------------*/
 static int fsl_udc_suspend(struct platform_device *pdev, pm_message_t state)
 {
-	dr_controller_stop(udc_controller);
-	return 0;
+    if (udc_controller->transceiver) {
+        if (udc_controller->transceiver->state != OTG_STATE_B_PERIPHERAL) {
+            /* we are not in device mode, return */
+            return 0;
+        }
+    }
+    if (udc_controller->vbus_active) {
+        spin_lock(&udc_controller->lock);
+        /* Reset all internal Queues and inform client driver */
+        reset_queues(udc_controller);
+        udc_controller->vbus_active = 0;
+        udc_controller->usb_state = USB_STATE_DEFAULT;
+        spin_unlock(&udc_controller->lock);
+    }
+    /* stop the controller and turn off the clocks */
+    dr_controller_stop(udc_controller);
+    if (udc_controller->transceiver) {
+        udc_controller->transceiver->state = OTG_STATE_UNDEFINED;
+    }
+    fsl_udc_clk_suspend(true);
+    return 0;
 }
 
 /*-----------------------------------------------------------------
@@ -2684,73 +3269,39 @@ static int fsl_udc_suspend(struct platform_device *pdev, pm_message_t state)
  *-----------------------------------------------------------------*/
 static int fsl_udc_resume(struct platform_device *pdev)
 {
-	/* Enable DR irq reg and set controller Run */
-	if (udc_controller->stopped) {
-		dr_controller_setup(udc_controller);
-		dr_controller_run(udc_controller);
-	}
-	udc_controller->usb_state = USB_STATE_ATTACHED;
-	udc_controller->ep0_state = WAIT_FOR_SETUP;
-	udc_controller->ep0_dir = 0;
-	return 0;
-}
-
-static int fsl_udc_otg_suspend(struct device *dev, pm_message_t state)
-{
-	struct fsl_udc *udc = udc_controller;
-	u32 mode, usbcmd;
-
-	mode = fsl_readl(&dr_regs->usbmode) & USB_MODE_CTRL_MODE_MASK;
-
-	pr_debug("%s(): mode 0x%x stopped %d\n", __func__, mode, udc->stopped);
-
-	/*
-	 * If the controller is already stopped, then this must be a
-	 * PM suspend.  Remember this fact, so that we will leave the
-	 * controller stopped at PM resume time.
-	 */
-	if (udc->stopped) {
-		pr_debug("gadget already stopped, leaving early\n");
-		udc->already_stopped = 1;
-		return 0;
-	}
-
-	if (mode != USB_MODE_CTRL_MODE_DEVICE) {
-		pr_debug("gadget not in device mode, leaving early\n");
-		return 0;
+	if (udc_controller->transceiver) {
+		fsl_udc_clk_resume(true);
+		if (!(fsl_readl(&usb_sys_regs->vbus_wakeup) & USB_SYS_ID_PIN_STATUS)) {
+			/* If ID status is low means host is connected, return */
+			fsl_udc_clk_suspend(false);
+			return 0;
+		}
+		/* check for VBUS */
+		if (!(fsl_readl(&usb_sys_regs->vbus_wakeup) & USB_SYS_VBUS_STATUS)) {
+			/* if there is no VBUS then power down the clocks and return */
+			fsl_udc_clk_suspend(false);
+			return 0;
+		} else {
+			fsl_udc_clk_suspend(false);
+			if (udc_controller->transceiver->state == OTG_STATE_A_HOST)
+				return 0;
+			/* Detected VBUS set the transceiver state to device mode */
+			udc_controller->transceiver->state = OTG_STATE_B_PERIPHERAL;
+		}
+	} else {
+		fsl_udc_clk_resume(true);
 	}
+	fsl_udc_clk_resume(true);
 
-	/* stop the controller */
-	usbcmd = fsl_readl(&dr_regs->usbcmd) & ~USB_CMD_RUN_STOP;
-	fsl_writel(usbcmd, &dr_regs->usbcmd);
-
-	udc->stopped = 1;
-
-	pr_info("USB Gadget suspended\n");
+	/* Power down the phy if cable is not connected */
+	if (!(fsl_readl(&usb_sys_regs->vbus_wakeup) & USB_SYS_VBUS_STATUS))
+		fsl_udc_clk_suspend(false);
+	else
+		fsl_udc_restart(udc_controller);
 
 	return 0;
 }
 
-static int fsl_udc_otg_resume(struct device *dev)
-{
-	pr_debug("%s(): stopped %d  already_stopped %d\n", __func__,
-		 udc_controller->stopped, udc_controller->already_stopped);
-
-	/*
-	 * If the controller was stopped at suspend time, then
-	 * don't resume it now.
-	 */
-	if (udc_controller->already_stopped) {
-		udc_controller->already_stopped = 0;
-		pr_debug("gadget was already stopped, leaving early\n");
-		return 0;
-	}
-
-	pr_info("USB Gadget resume\n");
-
-	return fsl_udc_resume(NULL);
-}
-
 /*-------------------------------------------------------------------------
 	Register entry point for the peripheral controller driver
 --------------------------------------------------------------------------*/
@@ -2763,9 +3314,6 @@ static struct platform_driver udc_driver = {
 	.driver  = {
 		.name = (char *)driver_name,
 		.owner = THIS_MODULE,
-		/* udc suspend/resume called from OTG driver */
-		.suspend = fsl_udc_otg_suspend,
-		.resume  = fsl_udc_otg_resume,
 	},
 };
 
diff --git a/drivers/usb/gadget/fsl_usb2_udc.h b/drivers/usb/gadget/fsl_usb2_udc.h
index 1d51be83fda8..814214f1c648 100644
--- a/drivers/usb/gadget/fsl_usb2_udc.h
+++ b/drivers/usb/gadget/fsl_usb2_udc.h
@@ -9,6 +9,7 @@
 #define USB_MAX_CTRL_PAYLOAD		64
 #define USB_DR_SYS_OFFSET		0x400
 
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
  /* USB DR device mode registers (Little Endian) */
 struct usb_dr_device {
 	/* Capability register */
@@ -82,8 +83,106 @@ struct usb_dr_host {
 	u32 endptcomplete;	/* Endpoint Complete Register */
 	u32 endptctrl[6];	/* Endpoint Control Registers */
 };
+#else
+/* Tegra3 support.
+ * Following changes have been done in tegra3 USB regs:
+ * 1. Registers usbcmd to portsc1 have been shifted up by 16 bytes.
+ * 2. Registers otgsc and usbmode have been shifted down by 80 bytes.
+ * 3. hostpc1devlc register has been added at offset 0x1b4(436).
+ * 4. Registers endptsetupstat to endptctrl have shifted down by 92 bytes.
+ */
+
+/* USB DR device mode registers (Little Endian) */
+struct usb_dr_device {
+	/* Capability register */
+	u8 res1[256];
+	u16 caplength;		/* Capability Register Length */
+	u16 hciversion;		/* Host Controller Interface Version */
+	u32 hcsparams;		/* Host Controller Structual Parameters */
+	u32 hccparams;		/* Host Controller Capability Parameters */
+	u8 res2[20];
+	u32 dciversion;		/* Device Controller Interface Version */
+	u32 dccparams;		/* Device Controller Capability Parameters */
+	u8 res3[8];
+	/* Operation register */
+	u32 usbcmd;		/* USB Command Register */
+	u32 usbsts;		/* USB Status Register */
+	u32 usbintr;		/* USB Interrupt Enable Register */
+	u32 frindex;		/* Frame Index Register */
+	u8 res4[4];
+	u32 deviceaddr;		/* Device Address */
+	u32 endpointlistaddr;	/* Endpoint List Address Register */
+	u8 res5[4];
+	u32 burstsize;		/* Master Interface Data Burst Size Register */
+	u32 txttfilltuning;	/* Transmit FIFO Tuning Controls Register */
+	u8 res6[24];
+	u32 configflag;		/* Configure Flag Register */
+	u32 portsc1;		/* Port 1 Status and Control Register */
+	u8 res7[60];
+	u32 hostpc1devlc;      /* Usb LPM Behavior and Control Register */
+	u8 res8[60];
+	u32 otgsc;		/* On-The-Go Status and Control */
+	u32 usbmode;		/* USB Mode Register */
+	u8 res9[12];
+	u32 endptsetupstat;	/* Endpoint Setup Status Register */
+	u32 endpointprime;	/* Endpoint Initialization Register */
+	u32 endptflush;		/* Endpoint Flush Register */
+	u32 endptstatus;	/* Endpoint Status Register */
+	u32 endptcomplete;	/* Endpoint Complete Register */
+	u32 endptctrl[6];	/* Endpoint Control Registers */
+};
+
+ /* USB DR host mode registers (Little Endian) */
+struct usb_dr_host {
+	/* Capability register */
+	u8 res1[256];
+	u16 caplength;		/* Capability Register Length */
+	u16 hciversion;		/* Host Controller Interface Version */
+	u32 hcsparams;		/* Host Controller Structual Parameters */
+	u32 hccparams;		/* Host Controller Capability Parameters */
+	u8 res2[20];
+	u32 dciversion;		/* Device Controller Interface Version */
+	u32 dccparams;		/* Device Controller Capability Parameters */
+	u8 res3[8];
+	/* Operation register */
+	u32 usbcmd;		/* USB Command Register */
+	u32 usbsts;		/* USB Status Register */
+	u32 usbintr;		/* USB Interrupt Enable Register */
+	u32 frindex;		/* Frame Index Register */
+	u8 res4[4];
+	u32 periodiclistbase;	/* Periodic Frame List Base Address Register */
+	u32 asynclistaddr;	/* Current Asynchronous List Address Register */
+	u8 res5[4];
+	u32 burstsize;		/* Master Interface Data Burst Size Register */
+	u32 txttfilltuning;	/* Transmit FIFO Tuning Controls Register */
+	u8 res6[24];
+	u32 configflag;		/* Configure Flag Register */
+	u32 portsc1;		/* Port 1 Status and Control Register */
+	u8 res7[60];
+	u32 hostpc1devlc;      /* Usb LPM Behavior and Control Register */
+	u8 res8[60];
+	u32 otgsc;		/* On-The-Go Status and Control */
+	u32 usbmode;		/* USB Mode Register */
+	u8 res9[12];
+	u32 endptsetupstat;	/* Endpoint Setup Status Register */
+	u32 endpointprime;	/* Endpoint Initialization Register */
+	u32 endptflush;		/* Endpoint Flush Register */
+	u32 endptstatus;	/* Endpoint Status Register */
+	u32 endptcomplete;	/* Endpoint Complete Register */
+	u32 endptctrl[6];	/* Endpoint Control Registers */
+};
+#endif // ifdef CONFIG_ARCH_TEGRA_2x_SOC
 
  /* non-EHCI USB system interface registers (Big Endian) */
+#ifdef CONFIG_ARCH_TEGRA
+struct usb_sys_interface {
+	u32 suspend_ctrl;
+	u32 vbus_sensors;
+	u32 vbus_wakeup;
+	u32 vbus_alt_status;
+	u32 legacy_ctrl;
+};
+#else
 struct usb_sys_interface {
 	u32 snoop1;
 	u32 snoop2;
@@ -93,6 +192,7 @@ struct usb_sys_interface {
 	u8 res[236];
 	u32 control;		/* General Purpose Control Register */
 };
+#endif
 
 /* ep0 transfer state */
 #define WAIT_FOR_SETUP          0
@@ -198,11 +298,59 @@ struct usb_sys_interface {
 #define  PORTSCX_WAKE_ON_CONNECT_DIS          0x00200000
 #define  PORTSCX_WAKE_ON_OVER_CURRENT         0x00400000
 #define  PORTSCX_PHY_LOW_POWER_SPD            0x00800000
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
 #define  PORTSCX_PORT_FORCE_FULL_SPEED        0x01000000
 #define  PORTSCX_PORT_SPEED_MASK              0x0C000000
 #define  PORTSCX_PORT_WIDTH                   0x10000000
 #define  PORTSCX_PHY_TYPE_SEL                 0xC0000000
 
+/* bit 27-26 are port speed */
+#define  PORTSCX_PORT_SPEED_FULL              0x00000000
+#define  PORTSCX_PORT_SPEED_LOW               0x04000000
+#define  PORTSCX_PORT_SPEED_HIGH              0x08000000
+#define  PORTSCX_PORT_SPEED_UNDEF             0x0C000000
+#define  PORTSCX_SPEED_BIT_POS                26
+
+/* bit 28 is parallel transceiver width for UTMI interface */
+#define  PORTSCX_PTW                          0x10000000
+#define  PORTSCX_PTW_8BIT                     0x00000000
+#define  PORTSCX_PTW_16BIT                    0x10000000
+
+/* bit 31-30 are port transceiver select */
+#define  PORTSCX_PTS_UTMI                     0x00000000
+#define  PORTSCX_PTS_ULPI                     0x80000000
+#define  PORTSCX_PTS_FSLS                     0xC0000000
+#define  PORTSCX_PTS_BIT_POS                  30
+#else
+/* In tegra3 the following fields have moved to new HOSTPC1_DEVLC reg and
+ * their offsets have changed.
+ * Keeping the name of bit masks same as before (PORTSCX_*) to have
+ * minimum changes to code */
+#define  PORTSCX_PORT_FORCE_FULL_SPEED    0x00800000
+#define  PORTSCX_PORT_SPEED_MASK          0x06000000
+#define  PORTSCX_PORT_WIDTH               0x08000000
+#define  PORTSCX_PHY_TYPE_SEL             0xE0000000
+
+/* bit 26-25 are port speed */
+#define  PORTSCX_PORT_SPEED_FULL          0x00000000
+#define  PORTSCX_PORT_SPEED_LOW           0x02000000
+#define  PORTSCX_PORT_SPEED_HIGH          0x04000000
+#define  PORTSCX_PORT_SPEED_UNDEF         0x06000000
+#define  PORTSCX_SPEED_BIT_POS            25
+
+/* bit 27 is parallel transceiver width for UTMI interface */
+#define  PORTSCX_PTW                      0x08000000
+#define  PORTSCX_PTW_8BIT                 0x00000000
+#define  PORTSCX_PTW_16BIT                0x08000000
+
+/* bit 31-29 are port transceiver select */
+#define  PORTSCX_PTS_UTMI                 0x00000000
+#define  PORTSCX_PTS_ULPI                 0x40000000
+#define  PORTSCX_PTS_FSLS                 0x60000000
+#define  PORTSCX_PTS_BIT_POS              29
+#endif
+
 /* bit 11-10 are line status */
 #define  PORTSCX_LINE_STATUS_SE0              0x00000000
 #define  PORTSCX_LINE_STATUS_JSTATE           0x00000400
@@ -226,24 +374,6 @@ struct usb_sys_interface {
 #define  PORTSCX_PTC_FORCE_EN                 0x00050000
 #define  PORTSCX_PTC_BIT_POS                  16
 
-/* bit 27-26 are port speed */
-#define  PORTSCX_PORT_SPEED_FULL              0x00000000
-#define  PORTSCX_PORT_SPEED_LOW               0x04000000
-#define  PORTSCX_PORT_SPEED_HIGH              0x08000000
-#define  PORTSCX_PORT_SPEED_UNDEF             0x0C000000
-#define  PORTSCX_SPEED_BIT_POS                26
-
-/* bit 28 is parallel transceiver width for UTMI interface */
-#define  PORTSCX_PTW                          0x10000000
-#define  PORTSCX_PTW_8BIT                     0x00000000
-#define  PORTSCX_PTW_16BIT                    0x10000000
-
-/* bit 31-30 are port transceiver select */
-#define  PORTSCX_PTS_UTMI                     0x00000000
-#define  PORTSCX_PTS_ULPI                     0x80000000
-#define  PORTSCX_PTS_FSLS                     0xC0000000
-#define  PORTSCX_PTS_BIT_POS                  30
-
 /* otgsc Register Bit Masks */
 #define  OTGSC_CTRL_VUSB_DISCHARGE            0x00000001
 #define  OTGSC_CTRL_VUSB_CHARGE               0x00000002
@@ -420,12 +550,25 @@ struct ep_td_struct {
                                                DTD_STATUS_DATA_BUFF_ERR | \
                                                DTD_STATUS_TRANSACTION_ERR)
 /* Alignment requirements; must be a power of two */
+#if defined(CONFIG_ARCH_TEGRA)
+#define DTD_ALIGNMENT				0x80
+#else
 #define DTD_ALIGNMENT				0x20
+#endif
 #define QH_ALIGNMENT				2048
+#define QH_OFFSET				0x1000
 
 /* Controller dma boundary */
 #define UDC_DMA_BOUNDARY			0x1000
 
+#define USB_SYS_VBUS_ASESSION_INT_EN		0x10000
+#define USB_SYS_VBUS_ASESSION_CHANGED		0x20000
+#define USB_SYS_VBUS_ASESSION			0x40000
+#define USB_SYS_VBUS_WAKEUP_ENABLE		0x40000000
+#define USB_SYS_VBUS_WAKEUP_INT_ENABLE		0x100
+#define USB_SYS_VBUS_WAKEUP_INT_STATUS		0x200
+#define USB_SYS_VBUS_STATUS			0x400
+#define USB_SYS_ID_PIN_STATUS       (0x4)
 /*-------------------------------------------------------------------------*/
 
 /* ### driver private data
@@ -476,8 +619,8 @@ struct fsl_udc {
 	unsigned vbus_active:1;
 	unsigned stopped:1;
 	unsigned remote_wakeup:1;
-	unsigned already_stopped:1;
 	unsigned big_endian_desc:1;
+	unsigned selfpowered:1;
 
 	struct ep_queue_head *ep_qh;	/* Endpoints Queue-Head */
 	struct fsl_req *status_req;	/* ep0 status request */
@@ -488,13 +631,18 @@ struct fsl_udc {
 	dma_addr_t ep_qh_dma;		/* dma address of QH */
 
 	u32 max_pipes;          /* Device max pipes */
-	u32 bus_reset;		/* Device is bus resetting */
 	u32 resume_state;	/* USB state to resume */
 	u32 usb_state;		/* USB current state */
 	u32 ep0_state;		/* Endpoint zero state */
 	u32 ep0_dir;		/* Endpoint zero direction: can be
 				   USB_DIR_IN or USB_DIR_OUT */
 	u8 device_address;	/* Device USB address */
+	struct delayed_work work;	/* delayed work for charger detection */
+	struct regulator *vbus_regulator;	/* regulator for drawing VBUS */
+	u32 current_limit;
+	struct work_struct charger_work; /* work for setting regulator current limit */
+	struct work_struct boost_cpufreq_work; /* work for boosting cpu frequency */
+	struct work_struct irq_work; /* irq work for controlling the usb power*/
 };
 
 /*-------------------------------------------------------------------------*/
@@ -570,10 +718,17 @@ static void dump_msg(const char *label, const u8 * buf, unsigned int length)
 #define get_pipe_by_ep(EP)	(ep_index(EP) * 2 + ep_is_in(EP))
 
 struct platform_device;
-#ifdef CONFIG_ARCH_MXC
+#if defined(CONFIG_ARCH_MXC) || defined(CONFIG_ARCH_TEGRA)
 int fsl_udc_clk_init(struct platform_device *pdev);
 void fsl_udc_clk_finalize(struct platform_device *pdev);
 void fsl_udc_clk_release(void);
+void fsl_udc_clk_suspend(bool is_dpd);
+void fsl_udc_clk_resume(bool is_dpd);
+void fsl_udc_clk_enable(void);
+void fsl_udc_clk_disable(void);
+bool fsl_udc_charger_detect(void);
+void fsl_udc_dtd_prepare(void);
+void fsl_udc_ep_barrier(void);
 #else
 static inline int fsl_udc_clk_init(struct platform_device *pdev)
 {
@@ -585,6 +740,28 @@ static inline void fsl_udc_clk_finalize(struct platform_device *pdev)
 static inline void fsl_udc_clk_release(void)
 {
 }
+static inline void fsl_udc_clk_suspend(bool is_dpd)
+{
+}
+static inline void fsl_udc_clk_resume(bool is_dpd)
+{
+}
+void fsl_udc_clk_enable(void)
+{
+}
+void fsl_udc_clk_disable(void)
+{
+}
+static inline bool fsl_udc_charger_detect(void)
+{
+	return false;
+}
+void fsl_udc_dtd_prepare(void)
+{
+}
+void fsl_udc_ep_barrier(void)
+{
+}
 #endif
 
 #endif
diff --git a/drivers/usb/gadget/gadget_chips.h b/drivers/usb/gadget/gadget_chips.h
index f3a83cd0ef50..db7f521e5d0a 100644
--- a/drivers/usb/gadget/gadget_chips.h
+++ b/drivers/usb/gadget/gadget_chips.h
@@ -49,6 +49,7 @@
 #define gadget_is_s3c2410(g)		(!strcmp("s3c2410_udc", (g)->name))
 #define gadget_is_s3c_hsotg(g)		(!strcmp("s3c-hsotg", (g)->name))
 #define gadget_is_s3c_hsudc(g)		(!strcmp("s3c-hsudc", (g)->name))
+#define gadget_is_tegra(g)		(!strcmp("tegra-udc", (g)->name))
 
 /**
  * usb_gadget_controller_number - support bcdDevice id convention
@@ -115,7 +116,8 @@ static inline int usb_gadget_controller_number(struct usb_gadget *gadget)
 		return 0x30;
 	else if (gadget_is_net2272(gadget))
 		return 0x31;
-
+	else if (gadget_is_tegra(gadget))
+		return 0x32;
 	return -ENOENT;
 }
 
diff --git a/drivers/usb/gadget/pch_udc.c b/drivers/usb/gadget/pch_udc.c
index f96615ab6b77..06c40b164b2d 100644
--- a/drivers/usb/gadget/pch_udc.c
+++ b/drivers/usb/gadget/pch_udc.c
@@ -363,6 +363,7 @@ struct pch_udc_dev {
 #define PCI_DEVICE_ID_INTEL_EG20T_UDC	0x8808
 #define PCI_VENDOR_ID_ROHM		0x10DB
 #define PCI_DEVICE_ID_ML7213_IOH_UDC	0x801D
+#define PCI_DEVICE_ID_ML7831_IOH_UDC	0x8808
 
 static const char	ep0_string[] = "ep0in";
 static DEFINE_SPINLOCK(udc_stall_spinlock);	/* stall spin lock */
@@ -2979,6 +2980,11 @@ static DEFINE_PCI_DEVICE_TABLE(pch_udc_pcidev_id) = {
 		.class = (PCI_CLASS_SERIAL_USB << 8) | 0xfe,
 		.class_mask = 0xffffffff,
 	},
+	{
+		PCI_DEVICE(PCI_VENDOR_ID_ROHM, PCI_DEVICE_ID_ML7831_IOH_UDC),
+		.class = (PCI_CLASS_SERIAL_USB << 8) | 0xfe,
+		.class_mask = 0xffffffff,
+	},
 	{ 0 },
 };
 
diff --git a/drivers/usb/gadget/printer.c b/drivers/usb/gadget/printer.c
index a341dde6f9c3..0c06d8000ef1 100644
--- a/drivers/usb/gadget/printer.c
+++ b/drivers/usb/gadget/printer.c
@@ -1611,7 +1611,7 @@ cleanup(void)
 	if (status)
 		ERROR(dev, "usb_gadget_unregister_driver %x\n", status);
 
-	unregister_chrdev_region(g_printer_devno, 2);
+	unregister_chrdev_region(g_printer_devno, 1);
 	class_destroy(usb_gadget_class);
 	mutex_unlock(&usb_printer_gadget.lock_printer_io);
 }
diff --git a/drivers/usb/gadget/rndis.c b/drivers/usb/gadget/rndis.c
index d3cdffea9c8a..bbfbde741595 100644
--- a/drivers/usb/gadget/rndis.c
+++ b/drivers/usb/gadget/rndis.c
@@ -1147,11 +1147,15 @@ static struct proc_dir_entry *rndis_connect_state [RNDIS_MAX_CONFIGS];
 
 #endif /* CONFIG_USB_GADGET_DEBUG_FILES */
 
+static bool rndis_initialized;
 
 int rndis_init(void)
 {
 	u8 i;
 
+	if (rndis_initialized)
+		return 0;
+
 	for (i = 0; i < RNDIS_MAX_CONFIGS; i++) {
 #ifdef	CONFIG_USB_GADGET_DEBUG_FILES
 		char name [20];
@@ -1178,6 +1182,7 @@ int rndis_init(void)
 		INIT_LIST_HEAD(&(rndis_per_dev_params[i].resp_queue));
 	}
 
+	rndis_initialized = true;
 	return 0;
 }
 
@@ -1186,7 +1191,13 @@ void rndis_exit(void)
 #ifdef CONFIG_USB_GADGET_DEBUG_FILES
 	u8 i;
 	char name[20];
+#endif
 
+	if (!rndis_initialized)
+		return;
+	rndis_initialized = false;
+
+#ifdef CONFIG_USB_GADGET_DEBUG_FILES
 	for (i = 0; i < RNDIS_MAX_CONFIGS; i++) {
 		sprintf(name, NAME_TEMPLATE, i);
 		remove_proc_entry(name, NULL);
diff --git a/drivers/usb/gadget/storage_common.c b/drivers/usb/gadget/storage_common.c
index d3dd227a2bfc..cdd407a252d1 100644
--- a/drivers/usb/gadget/storage_common.c
+++ b/drivers/usb/gadget/storage_common.c
@@ -763,10 +763,16 @@ static ssize_t fsg_store_file(struct device *dev, struct device_attribute *attr,
 	struct rw_semaphore	*filesem = dev_get_drvdata(dev);
 	int		rc = 0;
 
+
+#ifndef CONFIG_USB_ANDROID_MASS_STORAGE
+	/* disabled in android because we need to allow closing the backing file
+	 * if the media was removed
+	 */
 	if (curlun->prevent_medium_removal && fsg_lun_is_open(curlun)) {
 		LDBG(curlun, "eject attempt prevented\n");
 		return -EBUSY;				/* "Door is locked" */
 	}
+#endif
 
 	/* Remove a trailing newline */
 	if (count > 0 && buf[count-1] == '\n')
diff --git a/drivers/usb/gadget/tegra_udc.c b/drivers/usb/gadget/tegra_udc.c
new file mode 100644
index 000000000000..074a9b1ce6d9
--- /dev/null
+++ b/drivers/usb/gadget/tegra_udc.c
@@ -0,0 +1,2900 @@
+/*
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Description:
+ * High-speed USB device controller driver.
+ * The driver is based on Freescale driver code from Li Yang and Jiang Bo.
+ *
+ * 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.
+ */
+
+#undef VERBOSE
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/ioport.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/interrupt.h>
+#include <linux/proc_fs.h>
+#include <linux/mm.h>
+#include <linux/moduleparam.h>
+#include <linux/device.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/otg.h>
+#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <linux/dmapool.h>
+#include <linux/delay.h>
+#include <linux/regulator/consumer.h>
+#include <linux/workqueue.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/pm_qos_params.h>
+#include <linux/platform_data/tegra_usb.h>
+
+#include <asm/byteorder.h>
+#include <asm/io.h>
+#include <asm/system.h>
+#include <asm/unaligned.h>
+#include <asm/dma.h>
+
+#include <mach/usb_phy.h>
+#include <mach/iomap.h>
+
+#include "tegra_udc.h"
+
+
+#define	DRIVER_DESC	"Nvidia Tegra High-Speed USB SOC " \
+					"Device Controller driver"
+
+#define	DRIVER_AUTHOR	"Venkat Moganty/Rakesh Bodla"
+#define	DRIVER_VERSION	"Apr 30, 2012"
+#define USB1_PREFETCH_ID	6
+
+#define get_ep_by_pipe(udc, pipe)	((pipe == 1) ? &udc->eps[0] : \
+						&udc->eps[pipe])
+#define get_pipe_by_windex(windex)	((windex & USB_ENDPOINT_NUMBER_MASK) \
+					* 2 + ((windex & USB_DIR_IN) ? 1 : 0))
+
+#define ep_index(EP)	((EP)->desc->bEndpointAddress&0xF)
+#define ep_is_in(EP)	((ep_index(EP) == 0) ? (EP->udc->ep0_dir == \
+				USB_DIR_IN) : ((EP)->desc->bEndpointAddress \
+				& USB_DIR_IN) == USB_DIR_IN)
+
+
+static const char driver_name[] = "tegra-udc";
+static const char driver_desc[] = DRIVER_DESC;
+
+static void tegra_ep_fifo_flush(struct usb_ep *_ep);
+static int reset_queues(struct tegra_udc *udc);
+
+/*
+ * High speed test mode packet(53 bytes).
+ * See USB 2.0 spec, section 7.1.20.
+ */
+static const u8 tegra_udc_test_packet[53] = {
+	/* JKJKJKJK x9 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	/* JJKKJJKK x8 */
+	0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa,
+	/* JJJJKKKK x8 */
+	0xee, 0xee, 0xee, 0xee, 0xee, 0xee, 0xee, 0xee,
+	/* JJJJJJJKKKKKKK x8 */
+	0xfe, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	/* JJJJJJJK x8 */
+	0x7f, 0xbf, 0xdf, 0xef, 0xf7, 0xfb, 0xfd,
+	/* JKKKKKKK x10, JK */
+	0xfc, 0x7e, 0xbf, 0xdf, 0xef, 0xf7, 0xfb, 0xfd, 0x7e
+};
+
+#ifdef CONFIG_MACH_COLIBRI_T20
+/* To limit the speed of USB to full speed */
+extern int g_usb_high_speed;
+#endif /* CONFIG_MACH_COLIBRI_T20 */
+
+static struct tegra_udc *the_udc;
+
+#ifdef CONFIG_TEGRA_GADGET_BOOST_CPU_FREQ
+	static struct pm_qos_request_list boost_cpu_freq_req;
+	static u32 ep_queue_request_count;
+	static u8 boost_cpufreq_work_flag;
+#endif
+
+static inline void udc_writel(struct tegra_udc *udc, u32 val, u32 offset)
+{
+	writel(val, udc->regs + offset);
+}
+
+static inline unsigned int udc_readl(struct tegra_udc *udc, u32 offset)
+{
+	return readl(udc->regs + offset);
+}
+
+/* checks vbus status */
+static inline bool vbus_enabled(struct tegra_udc *udc)
+{
+	bool status = false;
+	status = (udc_readl(udc, VBUS_WAKEUP_REG_OFFSET) & USB_SYS_VBUS_STATUS);
+	return status;
+}
+
+/**
+ * done() - retire a request; caller blocked irqs
+ * @status : request status to be set, only works when
+ * request is still in progress.
+ */
+static void done(struct tegra_ep *ep, struct tegra_req *req, int status)
+{
+	struct tegra_udc *udc = NULL;
+	unsigned char stopped = ep->stopped;
+	struct ep_td_struct *curr_td, *next_td = 0;
+	int j;
+	int count;
+	BUG_ON(!(in_irq() || irqs_disabled()));
+	udc = (struct tegra_udc *)ep->udc;
+	/* Removed the req from tegra_ep->queue */
+	list_del_init(&req->queue);
+
+	/* req.status should be set as -EINPROGRESS in ep_queue() */
+	if (req->req.status == -EINPROGRESS)
+		req->req.status = status;
+	else
+		status = req->req.status;
+
+	/* Free dtd for the request */
+	count = 0;
+	if (ep->last_td) {
+		next_td = ep->last_td;
+		count = ep->last_dtd_count;
+	}
+	ep->last_td = req->head;
+	ep->last_dtd_count = req->dtd_count;
+
+	for (j = 0; j < count; j++) {
+		curr_td = next_td;
+		if (j != count - 1) {
+			next_td = curr_td->next_td_virt;
+		}
+		dma_pool_free(udc->td_pool, curr_td, curr_td->td_dma);
+	}
+
+	if (req->mapped) {
+		dma_unmap_single(ep->udc->gadget.dev.parent,
+			req->req.dma, req->req.length,
+			ep_is_in(ep)
+				? DMA_TO_DEVICE
+				: DMA_FROM_DEVICE);
+		req->req.dma = DMA_ADDR_INVALID;
+		req->mapped = 0;
+	} else
+		dma_sync_single_for_cpu(ep->udc->gadget.dev.parent,
+			req->req.dma, req->req.length,
+			ep_is_in(ep)
+				? DMA_TO_DEVICE
+				: DMA_FROM_DEVICE);
+
+	if (status && (status != -ESHUTDOWN))
+		VDBG("complete %s req %p stat %d len %u/%u",
+			ep->ep.name, &req->req, status,
+			req->req.actual, req->req.length);
+
+	ep->stopped = 1;
+#ifdef CONFIG_TEGRA_GADGET_BOOST_CPU_FREQ
+	if (req->req.complete && req->req.length >= BOOST_TRIGGER_SIZE) {
+		ep_queue_request_count--;
+		if (!ep_queue_request_count)
+			schedule_work(&udc->boost_cpufreq_work);
+	}
+#endif
+
+	/* complete() is from gadget layer,
+	 * eg fsg->bulk_in_complete() */
+	if (req->req.complete) {
+		spin_unlock(&ep->udc->lock);
+		req->req.complete(&ep->ep, &req->req);
+		spin_lock(&ep->udc->lock);
+	}
+
+	ep->stopped = stopped;
+}
+
+/*
+ * nuke(): delete all requests related to this ep
+ * Must be called with spinlock held and interrupt disabled
+ */
+static void nuke(struct tegra_ep *ep, int status)
+{
+	ep->stopped = 1;
+
+	/* Flush fifo */
+	tegra_ep_fifo_flush(&ep->ep);
+
+	/* Whether this eq has request linked */
+	while (!list_empty(&ep->queue)) {
+		struct tegra_req *req = NULL;
+
+		req = list_entry(ep->queue.next, struct tegra_req, queue);
+		done(ep, req, status);
+	}
+}
+
+static int can_pullup(struct tegra_udc *udc)
+{
+	DBG("%s(%d) udc->softconnect = %d udc->vbus_active = %d\n",
+		__func__, __LINE__, udc->softconnect, udc->vbus_active);
+	return udc->driver && udc->softconnect && udc->vbus_active;
+}
+
+static int dr_controller_reset(struct tegra_udc *udc)
+{
+	unsigned int tmp;
+	unsigned long timeout;
+	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
+
+	/* Stop and reset the usb controller */
+	tmp = udc_readl(udc, USB_CMD_REG_OFFSET);
+	tmp &= ~USB_CMD_RUN_STOP;
+	udc_writel(udc, tmp, USB_CMD_REG_OFFSET);
+
+	tmp = udc_readl(udc, USB_CMD_REG_OFFSET);
+	tmp |= USB_CMD_CTRL_RESET;
+	udc_writel(udc, tmp, USB_CMD_REG_OFFSET);
+
+	/* Wait for reset to complete */
+	timeout = jiffies + UDC_RESET_TIMEOUT_MS;
+	while (udc_readl(udc, USB_CMD_REG_OFFSET) & USB_CMD_CTRL_RESET) {
+		if (time_after(jiffies, timeout)) {
+			ERR("udc reset timeout!\n");
+			return -ETIMEDOUT;
+		}
+		cpu_relax();
+	}
+
+#ifdef CONFIG_MACH_COLIBRI_T20
+	/* To limit the speed of USB to full speed */
+	if (!g_usb_high_speed) {
+		tmp = udc_readl(udc, PORTSCX_REG_OFFSET);
+		tmp |= PORTSCX_PORT_FORCE_FULL_SPEED;
+		udc_writel(udc, tmp, PORTSCX_REG_OFFSET);
+		tmp = udc_readl(udc, PORTSCX_REG_OFFSET);
+	}
+#endif /* CONFIG_MACH_COLIBRI_T20 */
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+	return 0;
+}
+
+static int dr_controller_setup(struct tegra_udc *udc)
+{
+	unsigned int tmp, portctrl;
+	unsigned long timeout;
+	int status;
+	unsigned int port_control_reg_offset;
+	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
+
+	if (udc->has_hostpc)
+		port_control_reg_offset = USB_HOSTPCX_DEVLC_REG_OFFSET;
+	else
+		port_control_reg_offset = PORTSCX_REG_OFFSET;
+
+	/* Config PHY interface */
+	portctrl = udc_readl(udc, port_control_reg_offset);
+	portctrl &= ~(PORTSCX_PHY_TYPE_SEL | PORTSCX_PORT_WIDTH);
+	portctrl |= PORTSCX_PTS_UTMI;
+	udc_writel(udc, portctrl, port_control_reg_offset);
+
+	status = dr_controller_reset(udc);
+	if (status)
+		return status;
+
+	/* Set the controller as device mode */
+	tmp = udc_readl(udc, USB_MODE_REG_OFFSET);
+	tmp |= USB_MODE_CTRL_MODE_DEVICE;
+	/* Disable Setup Lockout */
+	tmp |= USB_MODE_SETUP_LOCK_OFF;
+	udc_writel(udc, tmp, USB_MODE_REG_OFFSET);
+
+	/* Wait for controller to switch to device mode */
+	timeout = jiffies + UDC_RESET_TIMEOUT_MS;
+	while ((udc_readl(udc, USB_MODE_REG_OFFSET) &
+		USB_MODE_CTRL_MODE_DEVICE) != USB_MODE_CTRL_MODE_DEVICE) {
+		if (time_after(jiffies, timeout)) {
+			ERR("udc device mode setup timeout!\n");
+			return -ETIMEDOUT;
+		}
+		cpu_relax();
+	}
+
+	/* Clear the setup status */
+	udc_writel(udc, 0, USB_STS_REG_OFFSET);
+
+	tmp = udc->ep_qh_dma;
+	tmp &= USB_EP_LIST_ADDRESS_MASK;
+	udc_writel(udc, tmp, USB_EP_LIST_ADDRESS_REG_OFFSET);
+
+	VDBG("vir[qh_base] is %p phy[qh_base] is 0x%8x reg is 0x%8x",
+		udc->ep_qh, (int)tmp,
+		udc_readl(udc, USB_EP_LIST_ADDRESS_REG_OFFSET));
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+	return 0;
+}
+
+/* Enable DR irq and set controller to run state */
+static void dr_controller_run(struct tegra_udc *udc)
+{
+	u32 temp;
+	unsigned long timeout;
+	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
+
+	/* Clear stopped bit */
+	udc->stopped = 0;
+
+	/* If OTG transceiver is available, then it handles the VBUS detection*/
+	if (!udc->transceiver) {
+		/* Enable cable detection interrupt, without setting the
+		 * USB_SYS_VBUS_WAKEUP_INT bit. USB_SYS_VBUS_WAKEUP_INT is
+		 * clear on write */
+		temp = udc_readl(udc, VBUS_WAKEUP_REG_OFFSET);
+		temp |= (USB_SYS_VBUS_WAKEUP_INT_ENABLE
+				| USB_SYS_VBUS_WAKEUP_ENABLE);
+		temp &= ~USB_SYS_VBUS_WAKEUP_INT_STATUS;
+		udc_writel(udc, temp, VBUS_WAKEUP_REG_OFFSET);
+	}
+	/* Enable DR irq reg */
+	temp = USB_INTR_INT_EN | USB_INTR_ERR_INT_EN
+		| USB_INTR_PTC_DETECT_EN | USB_INTR_RESET_EN
+		| USB_INTR_DEVICE_SUSPEND | USB_INTR_SYS_ERR_EN;
+
+	udc_writel(udc, temp, USB_INTR_REG_OFFSET);
+
+	/* Set the controller as device mode */
+	temp = udc_readl(udc, USB_MODE_REG_OFFSET);
+	temp |= USB_MODE_CTRL_MODE_DEVICE;
+	udc_writel(udc, temp, USB_MODE_REG_OFFSET);
+
+	/* Set controller to Run */
+	temp = udc_readl(udc, USB_CMD_REG_OFFSET);
+	if (can_pullup(udc))
+		temp |= USB_CMD_RUN_STOP;
+	else
+		temp &= ~USB_CMD_RUN_STOP;
+	udc_writel(udc, temp, USB_CMD_REG_OFFSET);
+
+	if (can_pullup(udc)) {
+		/* Wait for controller to start */
+		timeout = jiffies + UDC_RUN_TIMEOUT_MS;
+		while ((udc_readl(udc, USB_CMD_REG_OFFSET) & USB_CMD_RUN_STOP)
+			!= USB_CMD_RUN_STOP) {
+			if (time_after(jiffies, timeout)) {
+				ERR("udc start timeout!\n");
+				return;
+			}
+			cpu_relax();
+		}
+	}
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+	return;
+}
+
+static void dr_controller_stop(struct tegra_udc *udc)
+{
+	unsigned int tmp;
+	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
+
+	/* Clear pending interrupt status bits */
+	tmp = udc_readl(udc, USB_STS_REG_OFFSET);
+	udc_writel(udc, tmp, USB_STS_REG_OFFSET);
+
+	/* disable all INTR */
+	udc_writel(udc, 0, USB_INTR_REG_OFFSET);
+
+	/* Set stopped bit for isr */
+	udc->stopped = 1;
+
+	/* set controller to Stop */
+	tmp = udc_readl(udc, USB_CMD_REG_OFFSET);
+	tmp &= ~USB_CMD_RUN_STOP;
+	udc_writel(udc, tmp, USB_CMD_REG_OFFSET);
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+	return;
+}
+
+static void dr_ep_setup(struct tegra_udc *udc, unsigned char ep_num,
+			unsigned char dir, unsigned char ep_type)
+{
+	unsigned int tmp_epctrl = 0;
+
+	tmp_epctrl = udc_readl(udc, EP_CONTROL_REG_OFFSET + (ep_num * 4));
+	if (dir) {
+		if (ep_num)
+			tmp_epctrl |= EPCTRL_TX_DATA_TOGGLE_RST;
+		tmp_epctrl |= EPCTRL_TX_ENABLE;
+		tmp_epctrl |= ((unsigned int)(ep_type)
+				<< EPCTRL_TX_EP_TYPE_SHIFT);
+	} else {
+		if (ep_num)
+			tmp_epctrl |= EPCTRL_RX_DATA_TOGGLE_RST;
+		tmp_epctrl |= EPCTRL_RX_ENABLE;
+		tmp_epctrl |= ((unsigned int)(ep_type)
+				<< EPCTRL_RX_EP_TYPE_SHIFT);
+	}
+
+	udc_writel(udc, tmp_epctrl, EP_CONTROL_REG_OFFSET + (ep_num * 4));
+}
+
+static void dr_ep_change_stall(struct tegra_udc *udc, unsigned char ep_num,
+			unsigned char dir, int value)
+{
+	u32 tmp_epctrl = 0;
+
+	tmp_epctrl = udc_readl(udc, EP_CONTROL_REG_OFFSET + (ep_num * 4));
+	if (value) {
+		/* set the stall bit */
+		if (dir)
+			tmp_epctrl |= EPCTRL_TX_EP_STALL;
+		else
+			tmp_epctrl |= EPCTRL_RX_EP_STALL;
+	} else {
+		/* clear the stall bit and reset data toggle */
+		if (dir) {
+			tmp_epctrl &= ~EPCTRL_TX_EP_STALL;
+			tmp_epctrl |= EPCTRL_TX_DATA_TOGGLE_RST;
+		} else {
+			tmp_epctrl &= ~EPCTRL_RX_EP_STALL;
+			tmp_epctrl |= EPCTRL_RX_DATA_TOGGLE_RST;
+		}
+	}
+	udc_writel(udc, tmp_epctrl, EP_CONTROL_REG_OFFSET + (ep_num * 4));
+}
+
+/* Get stall status of a specific ep
+   Return: 0: not stalled; 1:stalled */
+static int dr_ep_get_stall(struct tegra_udc *udc, unsigned char ep_num,
+		unsigned char dir)
+{
+	u32 epctrl;
+
+	epctrl = udc_readl(udc, EP_CONTROL_REG_OFFSET + (ep_num * 4));
+	if (dir)
+		return (epctrl & EPCTRL_TX_EP_STALL) ? 1 : 0;
+	else
+		return (epctrl & EPCTRL_RX_EP_STALL) ? 1 : 0;
+}
+
+/**
+ * struct_ep_qh_setup(): set the Endpoint Capabilites field of QH
+ * @zlt: Zero Length Termination Select (1: disable; 0: enable)
+ * @mult: Mult field
+ */
+static void struct_ep_qh_setup(struct tegra_udc *udc, unsigned char ep_num,
+		unsigned char dir, unsigned char ep_type,
+		unsigned int max_pkt_len, unsigned int zlt, unsigned char mult)
+{
+	struct ep_queue_head *p_QH = &udc->ep_qh[2 * ep_num + dir];
+	unsigned int tmp = 0;
+
+	/* set the Endpoint Capabilites in QH */
+	switch (ep_type) {
+	case USB_ENDPOINT_XFER_CONTROL:
+		/* Interrupt On Setup (IOS). for control ep  */
+		tmp = (max_pkt_len << EP_QUEUE_HEAD_MAX_PKT_LEN_POS)
+			| EP_QUEUE_HEAD_IOS;
+		break;
+	case USB_ENDPOINT_XFER_ISOC:
+		tmp = (max_pkt_len << EP_QUEUE_HEAD_MAX_PKT_LEN_POS)
+			| (mult << EP_QUEUE_HEAD_MULT_POS);
+		break;
+	case USB_ENDPOINT_XFER_BULK:
+	case USB_ENDPOINT_XFER_INT:
+		tmp = max_pkt_len << EP_QUEUE_HEAD_MAX_PKT_LEN_POS;
+		break;
+	default:
+		VDBG("error ep type is %d", ep_type);
+		return;
+	}
+	if (zlt)
+		tmp |= EP_QUEUE_HEAD_ZLT_SEL;
+
+	p_QH->max_pkt_length = cpu_to_le32(tmp);
+	p_QH->next_dtd_ptr = 1;
+	p_QH->size_ioc_int_sts = 0;
+
+	return;
+}
+
+/* Setup qh structure and ep register for ep0. */
+static void ep0_setup(struct tegra_udc *udc)
+{
+	/* the intialization of an ep includes: fields in QH, Regs,
+	 * tegra_ep struct */
+	struct_ep_qh_setup(udc, 0, USB_RECV, USB_ENDPOINT_XFER_CONTROL,
+			USB_MAX_CTRL_PAYLOAD, 1, 0);
+	struct_ep_qh_setup(udc, 0, USB_SEND, USB_ENDPOINT_XFER_CONTROL,
+			USB_MAX_CTRL_PAYLOAD, 1, 0);
+	dr_ep_setup(udc, 0, USB_RECV, USB_ENDPOINT_XFER_CONTROL);
+	dr_ep_setup(udc, 0, USB_SEND, USB_ENDPOINT_XFER_CONTROL);
+
+	return;
+
+}
+
+/**
+ * when configurations are set, or when interface settings change
+ * for example the do_set_interface() in gadget layer,
+ * the driver will enable or disable the relevant endpoints
+ * ep0 doesn't use this routine. It is always enabled.
+ */
+static int tegra_ep_enable(struct usb_ep *_ep,
+		const struct usb_endpoint_descriptor *desc)
+{
+	struct tegra_udc *udc = NULL;
+	struct tegra_ep *ep = NULL;
+	unsigned short max = 0;
+	unsigned char mult = 0, zlt;
+	int retval = -EINVAL;
+	unsigned long flags = 0;
+
+	ep = container_of(_ep, struct tegra_ep, ep);
+
+	/* catch various bogus parameters */
+	if (!_ep || !desc || ep->desc
+			|| (desc->bDescriptorType != USB_DT_ENDPOINT))
+		return -EINVAL;
+
+	udc = ep->udc;
+
+	if (!udc->driver || (udc->gadget.speed == USB_SPEED_UNKNOWN))
+		return -ESHUTDOWN;
+
+	max = le16_to_cpu(desc->wMaxPacketSize);
+
+	/* Disable automatic zlp generation.  Driver is responsible to indicate
+	 * explicitly through req->req.zero.  This is needed to enable multi-td
+	 * request.
+	 */
+	zlt = 1;
+
+	/* Assume the max packet size from gadget is always correct */
+	switch (desc->bmAttributes & 0x03) {
+	case USB_ENDPOINT_XFER_CONTROL:
+	case USB_ENDPOINT_XFER_BULK:
+	case USB_ENDPOINT_XFER_INT:
+		/* mult = 0.  Execute N Transactions as demonstrated by
+		 * the USB variable length packet protocol where N is
+		 * computed using the Maximum Packet Length (dQH) and
+		 * the Total Bytes field (dTD) */
+		mult = 0;
+		break;
+	case USB_ENDPOINT_XFER_ISOC:
+		/* Calculate transactions needed for high bandwidth iso */
+		mult = (unsigned char)(1 + ((max >> 11) & 0x03));
+		max = max & 0x7ff;	/* bit 0~10 */
+		/* 3 transactions at most */
+		if (mult > 3)
+			goto en_done;
+		break;
+	default:
+		goto en_done;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+	ep->ep.maxpacket = max;
+	ep->desc = desc;
+	ep->stopped = 0;
+	ep->last_td = 0;
+	ep->last_dtd_count = 0;
+
+	/* Controller related setup
+	 * Init EPx Queue Head (Ep Capabilites field in QH
+	 * according to max, zlt, mult)
+	 */
+	struct_ep_qh_setup(udc, (unsigned char) ep_index(ep),
+			(unsigned char) ((desc->bEndpointAddress & USB_DIR_IN)
+					?  USB_SEND : USB_RECV),
+			(unsigned char) (desc->bmAttributes
+					& USB_ENDPOINT_XFERTYPE_MASK),
+			max, zlt, mult);
+
+	/* Init endpoint ctrl register */
+	dr_ep_setup(udc, (unsigned char) ep_index(ep),
+			(unsigned char) ((desc->bEndpointAddress & USB_DIR_IN)
+					? USB_SEND : USB_RECV),
+			(unsigned char) (desc->bmAttributes
+					& USB_ENDPOINT_XFERTYPE_MASK));
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+	retval = 0;
+
+	VDBG("enabled %s (ep%d%s) maxpacket %d", ep->ep.name,
+			ep->desc->bEndpointAddress & 0x0f,
+			(desc->bEndpointAddress & USB_DIR_IN)
+				? "in" : "out", max);
+en_done:
+	return retval;
+}
+
+/**
+ * @ep : the ep being unconfigured. May not be ep0
+ * Any pending and uncomplete req will complete with status (-ESHUTDOWN)
+ */
+static int tegra_ep_disable(struct usb_ep *_ep)
+{
+	struct tegra_udc *udc = NULL;
+	struct tegra_ep *ep = NULL;
+
+	unsigned long flags = 0;
+	u32 epctrl;
+	int ep_num;
+	struct ep_td_struct *curr_td, *next_td;
+	int j;
+
+	ep = container_of(_ep, struct tegra_ep, ep);
+	if (!_ep || !ep->desc) {
+		VDBG("%s not enabled", _ep ? ep->ep.name : NULL);
+		return -EINVAL;
+	}
+	udc = (struct tegra_udc *)ep->udc;
+
+	/* disable ep on controller */
+	ep_num = ep_index(ep);
+
+	/* Touch the registers if cable is connected and phy is on */
+	if (udc->vbus_active) {
+		epctrl = udc_readl(udc, EP_CONTROL_REG_OFFSET + (ep_num * 4));
+		if (ep_is_in(ep))
+			epctrl &= ~EPCTRL_TX_ENABLE;
+		else
+			epctrl &= ~EPCTRL_RX_ENABLE;
+		udc_writel(udc, epctrl, EP_CONTROL_REG_OFFSET + (ep_num * 4));
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/* nuke all pending requests (does flush) */
+	nuke(ep, -ESHUTDOWN);
+
+	ep->desc = NULL;
+	ep->stopped = 1;
+	if (ep->last_td) {
+		next_td = ep->last_td;
+		for (j = 0; j < ep->last_dtd_count; j++) {
+			curr_td = next_td;
+			dma_pool_free(udc->td_pool, curr_td, curr_td->td_dma);
+			if (j != ep->last_dtd_count - 1) {
+				next_td = curr_td->next_td_virt;
+			}
+		}
+	}
+	ep->last_td =0;
+	ep->last_dtd_count = 0;
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	VDBG("disabled %s OK", _ep->name);
+	return 0;
+}
+
+/**
+ * Allocate a request object used by this endpoint
+ * the main operation is to insert the req->queue to the eq->queue
+ * Returns the request, or null if one could not be allocated
+ */
+static struct usb_request *
+tegra_alloc_request(struct usb_ep *_ep, gfp_t gfp_flags)
+{
+	struct tegra_req *req = NULL;
+
+	req = kzalloc(sizeof *req, gfp_flags);
+	if (!req)
+		return NULL;
+
+	req->req.dma = DMA_ADDR_INVALID;
+	INIT_LIST_HEAD(&req->queue);
+
+	return &req->req;
+}
+
+static void tegra_free_request(struct usb_ep *_ep, struct usb_request *_req)
+{
+	struct tegra_req *req = NULL;
+
+	req = container_of(_req, struct tegra_req, req);
+
+	if (_req)
+		kfree(req);
+}
+
+static void tegra_queue_td(struct tegra_ep *ep, struct tegra_req *req)
+{
+	int i = ep_index(ep) * 2 + ep_is_in(ep);
+	u32 temp, bitmask, tmp_stat;
+	struct ep_queue_head *dQH = &ep->udc->ep_qh[i];
+	struct tegra_udc *udc = ep->udc;
+
+	bitmask = ep_is_in(ep)
+		? (1 << (ep_index(ep) + 16))
+		: (1 << (ep_index(ep)));
+
+	/* Flush all the dTD structs out to memory */
+	wmb();
+
+	/* check if the pipe is empty */
+	if (!(list_empty(&ep->queue))) {
+		/* Add td to the end */
+		struct tegra_req *lastreq;
+		lastreq = list_entry(ep->queue.prev, struct tegra_req, queue);
+		lastreq->tail->next_td_ptr =
+			cpu_to_le32(req->head->td_dma & DTD_ADDR_MASK);
+		wmb();
+		/* Read prime bit, if 1 goto done */
+		if (udc_readl(udc, EP_PRIME_REG_OFFSET) & bitmask)
+			goto out;
+
+		do {
+			/* Set ATDTW bit in USBCMD */
+			temp = udc_readl(udc, USB_CMD_REG_OFFSET);
+			temp |= USB_CMD_ATDTW;
+			udc_writel(udc, temp, USB_CMD_REG_OFFSET);
+
+			/* Read correct status bit */
+			tmp_stat = udc_readl(udc, EP_STATUS_REG_OFFSET)
+						& bitmask;
+
+		} while (!(udc_readl(udc, USB_CMD_REG_OFFSET) & USB_CMD_ATDTW));
+
+		/* Write ATDTW bit to 0 */
+		temp = udc_readl(udc, USB_CMD_REG_OFFSET);
+		udc_writel(udc, temp & ~USB_CMD_ATDTW, USB_CMD_REG_OFFSET);
+
+		if (tmp_stat)
+			goto out;
+	}
+
+	/* Write dQH next pointer and terminate bit to 0 */
+	temp = req->head->td_dma & EP_QUEUE_HEAD_NEXT_POINTER_MASK;
+	dQH->next_dtd_ptr = cpu_to_le32(temp);
+
+	/* Clear active and halt bit */
+	temp = cpu_to_le32(~(EP_QUEUE_HEAD_STATUS_ACTIVE
+			| EP_QUEUE_HEAD_STATUS_HALT));
+	dQH->size_ioc_int_sts &= temp;
+
+	tegra_usb_phy_memory_prefetch_on(udc->phy);
+
+	/* Ensure that updates to the QH will occur before priming. */
+	wmb();
+
+	/* Prime endpoint by writing 1 to ENDPTPRIME */
+	temp = ep_is_in(ep)
+		? (1 << (ep_index(ep) + 16))
+		: (1 << (ep_index(ep)));
+	udc_writel(udc, temp, EP_PRIME_REG_OFFSET);
+out:
+	return;
+}
+
+/**
+ * Fill in the dTD structure
+ * @req     : request that the transfer belongs to
+ * @length  : return actually data length of the dTD
+ * @dma     : return dma address of the dTD
+ * @is_last : return flag if it is the last dTD of the request
+ * return   : pointer to the built dTD
+ */
+static struct ep_td_struct *tegra_build_dtd(struct tegra_req *req,
+	unsigned *length, dma_addr_t *dma, int *is_last, gfp_t gfp_flags)
+{
+	u32 swap_temp;
+	struct ep_td_struct *dtd;
+
+	/* how big will this transfer be? */
+	*length = min(req->req.length - req->req.actual,
+			(unsigned)EP_MAX_LENGTH_TRANSFER);
+
+	dtd = dma_pool_alloc(the_udc->td_pool, gfp_flags, dma);
+	if (dtd == NULL)
+		return dtd;
+
+	dtd->td_dma = *dma;
+	/* Clear reserved field */
+	swap_temp = cpu_to_le32(dtd->size_ioc_sts);
+	swap_temp &= ~DTD_RESERVED_FIELDS;
+	dtd->size_ioc_sts = cpu_to_le32(swap_temp);
+
+	/* Init all of buffer page pointers */
+	swap_temp = (u32) (req->req.dma + req->req.actual);
+	dtd->buff_ptr0 = cpu_to_le32(swap_temp);
+	dtd->buff_ptr1 = cpu_to_le32(swap_temp + 0x1000);
+	dtd->buff_ptr2 = cpu_to_le32(swap_temp + 0x2000);
+	dtd->buff_ptr3 = cpu_to_le32(swap_temp + 0x3000);
+	dtd->buff_ptr4 = cpu_to_le32(swap_temp + 0x4000);
+
+	req->req.actual += *length;
+
+	/* zlp is needed if req->req.zero is set */
+	if (req->req.zero) {
+		if (*length == 0 || (*length % req->ep->ep.maxpacket) != 0)
+			*is_last = 1;
+		else
+			*is_last = 0;
+	} else if (req->req.length == req->req.actual)
+		*is_last = 1;
+	else
+		*is_last = 0;
+
+	if ((*is_last) == 0)
+		VDBG("multi-dtd request!");
+
+	/* Fill in the transfer size; set active bit */
+	swap_temp = ((*length << DTD_LENGTH_BIT_POS) | DTD_STATUS_ACTIVE);
+
+	/* Enable interrupt for the last dtd of a request */
+	if (*is_last && !req->req.no_interrupt)
+		swap_temp |= DTD_IOC;
+
+	dtd->size_ioc_sts = cpu_to_le32(swap_temp);
+
+	mb();
+
+	VDBG("length = %d address= 0x%x", *length, (int)*dma);
+
+	return dtd;
+}
+
+/* Generate dtd chain for a request */
+static int tegra_req_to_dtd(struct tegra_req *req, gfp_t gfp_flags)
+{
+	unsigned	count;
+	int		is_last;
+	int		is_first = 1;
+	struct ep_td_struct	*last_dtd = NULL, *dtd;
+	dma_addr_t dma;
+
+	tegra_usb_phy_memory_prefetch_off(the_udc->phy);
+
+	do {
+		dtd = tegra_build_dtd(req, &count, &dma, &is_last, gfp_flags);
+		if (dtd == NULL)
+			return -ENOMEM;
+
+		if (is_first) {
+			is_first = 0;
+			req->head = dtd;
+		} else {
+			last_dtd->next_td_ptr = cpu_to_le32(dma);
+			last_dtd->next_td_virt = dtd;
+		}
+		last_dtd = dtd;
+
+		req->dtd_count++;
+	} while (!is_last);
+
+	dtd->next_td_ptr = cpu_to_le32(DTD_NEXT_TERMINATE);
+
+	req->tail = dtd;
+
+	return 0;
+}
+
+/* queues (submits) an I/O request to an endpoint */
+static int
+tegra_ep_queue(struct usb_ep *_ep, struct usb_request *_req, gfp_t gfp_flags)
+{
+	struct tegra_ep *ep = container_of(_ep, struct tegra_ep, ep);
+	struct tegra_req *req = container_of(_req, struct tegra_req, req);
+	struct tegra_udc *udc = ep->udc;
+	unsigned long flags;
+	enum dma_data_direction dir;
+	int status;
+
+	/* catch various bogus parameters */
+	if (!_req || !req->req.complete || !req->req.buf
+			|| !list_empty(&req->queue)) {
+		VDBG("%s, bad params", __func__);
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	if (unlikely(!ep->desc)) {
+		VDBG("%s, bad ep", __func__);
+		spin_unlock_irqrestore(&udc->lock, flags);
+		return -EINVAL;
+	}
+
+	if (ep->desc->bmAttributes == USB_ENDPOINT_XFER_ISOC) {
+		if (req->req.length > ep->ep.maxpacket) {
+			spin_unlock_irqrestore(&udc->lock, flags);
+			return -EMSGSIZE;
+		}
+	}
+
+#ifdef CONFIG_TEGRA_GADGET_BOOST_CPU_FREQ
+	if (req->req.length >= BOOST_TRIGGER_SIZE) {
+		ep_queue_request_count++;
+		if (ep_queue_request_count && boost_cpufreq_work_flag)
+			schedule_work(&udc->boost_cpufreq_work);
+	}
+#endif
+
+	dir = ep_is_in(ep) ? DMA_TO_DEVICE : DMA_FROM_DEVICE;
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	if (!udc->driver || udc->gadget.speed == USB_SPEED_UNKNOWN)
+		return -ESHUTDOWN;
+
+	req->ep = ep;
+
+	/* map virtual address to hardware */
+	if (req->req.dma == DMA_ADDR_INVALID) {
+		req->req.dma = dma_map_single(udc->gadget.dev.parent,
+					req->req.buf, req->req.length, dir);
+		req->mapped = 1;
+	} else {
+		dma_sync_single_for_device(udc->gadget.dev.parent,
+					req->req.dma, req->req.length, dir);
+		req->mapped = 0;
+	}
+
+	req->req.status = -EINPROGRESS;
+	req->req.actual = 0;
+	req->dtd_count = 0;
+
+
+	/* build dtds and push them to device queue */
+	status = tegra_req_to_dtd(req, gfp_flags);
+	if (status)
+		goto err_unmap;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/* re-check if the ep has not been disabled */
+	if (unlikely(!ep->desc)) {
+		spin_unlock_irqrestore(&udc->lock, flags);
+		status = -EINVAL;
+		goto err_unmap;
+	}
+
+	tegra_queue_td(ep, req);
+
+	/* Update ep0 state */
+	if ((ep_index(ep) == 0))
+		udc->ep0_state = DATA_STATE_XMIT;
+
+	/* irq handler advances the queue */
+	if (req != NULL)
+		list_add_tail(&req->queue, &ep->queue);
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+
+err_unmap:
+	if (req->mapped) {
+		dma_unmap_single(udc->gadget.dev.parent,
+			req->req.dma, req->req.length, dir);
+		req->req.dma = DMA_ADDR_INVALID;
+		req->mapped = 0;
+	}
+	return status;
+}
+
+/* dequeues (cancels, unlinks) an I/O request from an endpoint */
+static int tegra_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
+{
+	struct tegra_ep *ep = container_of(_ep, struct tegra_ep, ep);
+	struct tegra_req *req;
+	struct tegra_udc *udc = ep->udc;
+	unsigned long flags;
+	int ep_num, stopped, ret = 0;
+	u32 epctrl;
+
+	if (!_ep || !_req)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ep->udc->lock, flags);
+	stopped = ep->stopped;
+
+	/* Stop the ep before we deal with the queue */
+	ep->stopped = 1;
+	ep_num = ep_index(ep);
+
+	/* Touch the registers if cable is connected and phy is on */
+	if (udc->vbus_active) {
+		epctrl = udc_readl(udc, EP_CONTROL_REG_OFFSET + (ep_num * 4));
+		if (ep_is_in(ep))
+			epctrl &= ~EPCTRL_TX_ENABLE;
+		else
+			epctrl &= ~EPCTRL_RX_ENABLE;
+		udc_writel(udc, epctrl, EP_CONTROL_REG_OFFSET + (ep_num * 4));
+	}
+
+	/* make sure it's actually queued on this endpoint */
+	list_for_each_entry(req, &ep->queue, queue) {
+		if (&req->req == _req)
+			break;
+	}
+	if (&req->req != _req) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* The request is in progress, or completed but not dequeued */
+	if (ep->queue.next == &req->queue) {
+		_req->status = -ECONNRESET;
+		tegra_ep_fifo_flush(_ep);	/* flush current transfer */
+
+		/* The request isn't the last request in this ep queue */
+		if (req->queue.next != &ep->queue) {
+			struct ep_queue_head *qh;
+			struct tegra_req *next_req;
+
+			qh = ep->qh;
+			next_req = list_entry(req->queue.next, struct tegra_req,
+					queue);
+
+			/* Point the QH to the first TD of next request */
+			writel((u32) next_req->head, &qh->curr_dtd_ptr);
+		}
+
+		/* The request hasn't been processed, patch up the TD chain */
+	} else {
+		struct tegra_req *prev_req;
+
+		prev_req = list_entry(req->queue.prev, struct tegra_req, queue);
+		writel(readl(&req->tail->next_td_ptr),
+				&prev_req->tail->next_td_ptr);
+	}
+
+	done(ep, req, -ECONNRESET);
+
+	/* Enable EP */
+out:
+	/* Touch the registers if cable is connected and phy is on */
+	if (udc->vbus_active) {
+		epctrl = udc_readl(udc, EP_CONTROL_REG_OFFSET + (ep_num * 4));
+		if (ep_is_in(ep))
+			epctrl |= EPCTRL_TX_ENABLE;
+		else
+			epctrl |= EPCTRL_RX_ENABLE;
+		udc_writel(udc, epctrl, EP_CONTROL_REG_OFFSET + (ep_num * 4));
+	}
+	ep->stopped = stopped;
+
+	spin_unlock_irqrestore(&ep->udc->lock, flags);
+	return ret;
+}
+
+/**
+ * modify the endpoint halt feature
+ * @ep: the non-isochronous endpoint being stalled
+ * @value: 1--set halt  0--clear halt
+ * Returns zero, or a negative error code.
+ */
+static int tegra_ep_set_halt(struct usb_ep *_ep, int value)
+{
+	struct tegra_ep *ep = NULL;
+	unsigned long flags = 0;
+	int status = -EOPNOTSUPP;	/* operation not supported */
+	unsigned char ep_dir = 0, ep_num = 0;
+	struct tegra_udc *udc = NULL;
+
+	ep = container_of(_ep, struct tegra_ep, ep);
+	udc = ep->udc;
+	if (!_ep || !ep->desc) {
+		status = -EINVAL;
+		goto out;
+	}
+
+	if (ep->desc->bmAttributes == USB_ENDPOINT_XFER_ISOC) {
+		status = -EOPNOTSUPP;
+		goto out;
+	}
+
+	/* Attempt to halt IN ep will fail if any transfer requests
+	 * are still queue */
+	if (value && ep_is_in(ep) && !list_empty(&ep->queue)) {
+		status = -EAGAIN;
+		goto out;
+	}
+
+	status = 0;
+	ep_dir = ep_is_in(ep) ? USB_SEND : USB_RECV;
+	ep_num = (unsigned char)(ep_index(ep));
+	spin_lock_irqsave(&ep->udc->lock, flags);
+	dr_ep_change_stall(udc, ep_num, ep_dir, value);
+	spin_unlock_irqrestore(&ep->udc->lock, flags);
+
+	if (ep_index(ep) == 0) {
+		udc->ep0_state = WAIT_FOR_SETUP;
+		udc->ep0_dir = 0;
+	}
+out:
+	VDBG(" %s %s halt stat %d", ep->ep.name,
+			value ?  "set" : "clear", status);
+
+	return status;
+}
+
+static int tegra_ep_fifo_status(struct usb_ep *_ep)
+{
+	struct tegra_ep *ep;
+	struct tegra_udc *udc;
+	int size = 0;
+	u32 bitmask;
+	struct ep_queue_head *d_qh;
+
+	ep = container_of(_ep, struct tegra_ep, ep);
+	if (!_ep || (!ep->desc && ep_index(ep) != 0))
+		return -ENODEV;
+
+	udc = (struct tegra_udc *)ep->udc;
+
+	if (!udc->driver || udc->gadget.speed == USB_SPEED_UNKNOWN)
+		return -ESHUTDOWN;
+
+	d_qh = &ep->udc->ep_qh[ep_index(ep) * 2 + ep_is_in(ep)];
+
+	bitmask = (ep_is_in(ep)) ? (1 << (ep_index(ep) + 16)) :
+	    (1 << (ep_index(ep)));
+
+	if (udc_readl(udc, EP_STATUS_REG_OFFSET) & bitmask)
+		size = (d_qh->size_ioc_int_sts & DTD_PACKET_SIZE)
+		    >> DTD_LENGTH_BIT_POS;
+
+	pr_debug("%s %u\n", __func__, size);
+	return size;
+}
+
+static void tegra_ep_fifo_flush(struct usb_ep *_ep)
+{
+	struct tegra_ep *ep;
+	struct tegra_udc *udc;
+	int ep_num, ep_dir;
+	u32 bits;
+	unsigned long timeout;
+
+	if (!_ep) {
+		return;
+	} else {
+		ep = container_of(_ep, struct tegra_ep, ep);
+		if (!ep->desc)
+			return;
+	}
+	ep_num = ep_index(ep);
+	ep_dir = ep_is_in(ep) ? USB_SEND : USB_RECV;
+	udc = ep->udc;
+
+	if (ep_num == 0)
+		bits = (1 << 16) | 1;
+	else if (ep_dir == USB_SEND)
+		bits = 1 << (16 + ep_num);
+	else
+		bits = 1 << ep_num;
+
+	/* Touch the registers if cable is connected and phy is on */
+	if (!udc->vbus_active)
+		return;
+
+	timeout = jiffies + UDC_FLUSH_TIMEOUT_MS;
+	do {
+		udc_writel(udc, bits, EPFLUSH_REG_OFFSET);
+
+		/* Wait until flush complete */
+		while (udc_readl(udc, EPFLUSH_REG_OFFSET)) {
+			if (time_after(jiffies, timeout)) {
+				ERR("ep flush timeout\n");
+				return;
+			}
+			cpu_relax();
+		}
+		/* See if we need to flush again */
+	} while (udc_readl(udc, EP_STATUS_REG_OFFSET) & bits);
+}
+
+static struct usb_ep_ops tegra_ep_ops = {
+	.enable = tegra_ep_enable,
+	.disable = tegra_ep_disable,
+
+	.alloc_request = tegra_alloc_request,
+	.free_request = tegra_free_request,
+
+	.queue = tegra_ep_queue,
+	.dequeue = tegra_ep_dequeue,
+
+	.set_halt = tegra_ep_set_halt,
+	.fifo_status = tegra_ep_fifo_status,
+	.fifo_flush = tegra_ep_fifo_flush,	/* flush fifo */
+};
+
+/* Get the current frame number (from DR frame_index Reg ) */
+static int tegra_get_frame(struct usb_gadget *gadget)
+{
+	struct tegra_udc *udc = container_of(gadget, struct tegra_udc, gadget);
+	return (int)(udc_readl(udc, USB_FRINDEX_REG_OFFSET)
+						& USB_FRINDEX_MASKS);
+}
+
+#ifndef CONFIG_USB_ANDROID
+/* Tries to wake up the host connected to this gadget */
+static int tegra_wakeup(struct usb_gadget *gadget)
+{
+	struct tegra_udc *udc = container_of(gadget, struct tegra_udc, gadget);
+	u32 portsc;
+
+	/* Remote wakeup feature not enabled by host */
+	if (!udc->remote_wakeup)
+		return -ENOTSUPP;
+
+	portsc = udc_readl(udc, PORTSCX_REG_OFFSET);
+	/* not suspended? */
+	if (!(portsc & PORTSCX_PORT_SUSPEND))
+		return 0;
+
+	/* trigger force resume */
+	portsc |= PORTSCX_PORT_FORCE_RESUME;
+	udc_writel(udc, portsc, PORTSCX_REG_OFFSET);
+	return 0;
+}
+#endif
+
+static int tegra_set_selfpowered(struct usb_gadget *gadget, int is_on)
+{
+	struct tegra_udc *udc;
+	udc = container_of(gadget, struct tegra_udc, gadget);
+	udc->selfpowered = (is_on != 0);
+	return 0;
+}
+
+static int tegra_usb_set_charging_current(struct tegra_udc *udc)
+{
+	int max_ua;
+
+	if (NULL == udc->vbus_reg)
+		return 0;
+
+	switch (udc->connect_type) {
+	case CONNECT_TYPE_NONE:
+		pr_debug("detected USB charging is disabled");
+		max_ua = 0;
+		break;
+	case CONNECT_TYPE_SDP:
+		pr_debug("detected SDP port");
+		max_ua = USB_CHARGING_SDP_CURRENT_LIMIT_UA;
+		break;
+	case CONNECT_TYPE_DCP:
+		pr_debug("detected DCP port(wall charger)");
+		max_ua = USB_CHARGING_DCP_CURRENT_LIMIT_UA;
+		break;
+	case CONNECT_TYPE_CDP:
+		pr_debug("detected CDP port(1A USB port)");
+		max_ua = USB_CHARGING_CDP_CURRENT_LIMIT_UA;
+		break;
+	case CONNECT_TYPE_NON_STANDARD_CHARGER:
+		pr_debug("detected non-standard charging port");
+		max_ua = USB_CHARGING_NON_STANDARD_CHARGER_CURRENT_LIMIT_UA;
+		break;
+	default:
+		pr_debug("detected USB charging type is unknown");
+		max_ua = 0;
+	}
+
+	return regulator_set_current_limit(udc->vbus_reg, 0, max_ua);
+}
+
+static void tegra_detect_charging_type_is_cdp_or_dcp(struct tegra_udc *udc)
+{
+	u32 portsc;
+	u32 temp;
+	unsigned long flags;
+
+	/* use spinlock to prevent kernel preemption here */
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/* set controller to run which cause D+ pull high */
+	temp = udc_readl(udc, USB_CMD_REG_OFFSET);
+	temp |= USB_CMD_RUN_STOP;
+	udc_writel(udc, temp, USB_CMD_REG_OFFSET);
+
+	udelay(10);
+
+	/* use D+ and D- status to check it is CDP or DCP */
+	portsc = udc_readl(udc, PORTSCX_REG_OFFSET) & PORTSCX_LINE_STATUS_BITS;
+	if (portsc == (PORTSCX_LINE_STATUS_DP_BIT | PORTSCX_LINE_STATUS_DM_BIT))
+		udc->connect_type = CONNECT_TYPE_DCP;
+	else if (portsc == PORTSCX_LINE_STATUS_DP_BIT)
+		udc->connect_type = CONNECT_TYPE_CDP;
+	else
+		/*
+		 * If it take more 100mS between D+ pull high and read Line
+		 * Status, host might initiate the RESET, then we see both
+		 * line status as 0 (SE0). This really should not happen as we
+		 * disabled the kernel preemption before reaching here.
+		 */
+		BUG();
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+}
+
+/**
+ * Notify controller that VBUS is powered, called by whatever
+ * detects VBUS sessions
+ */
+static int tegra_vbus_session(struct usb_gadget *gadget, int is_active)
+{
+	struct tegra_udc *udc = container_of(gadget, struct tegra_udc, gadget);
+	unsigned long flags;
+	DBG("%s(%d) turn VBUS state from %s to %s", __func__, __LINE__,
+		udc->vbus_active ? "on" : "off", is_active ? "on" : "off");
+
+	if (udc->vbus_active && !is_active) {
+		/* If cable disconnected, cancel any delayed work */
+		cancel_delayed_work(&udc->work);
+		spin_lock_irqsave(&udc->lock, flags);
+		/* reset all internal Queues and inform client driver */
+		reset_queues(udc);
+		/* stop the controller and turn off the clocks */
+		dr_controller_stop(udc);
+		dr_controller_reset(udc);
+		udc->vbus_active = 0;
+		udc->usb_state = USB_STATE_DEFAULT;
+		udc->connect_type = CONNECT_TYPE_NONE;
+		spin_unlock_irqrestore(&udc->lock,flags);
+		tegra_usb_phy_power_off(udc->phy);
+		tegra_usb_set_charging_current(udc);
+	} else if (!udc->vbus_active && is_active) {
+		tegra_usb_phy_power_on(udc->phy);
+		/* setup the controller in the device mode */
+		dr_controller_setup(udc);
+		/* setup EP0 for setup packet */
+		ep0_setup(udc);
+		/* initialize the USB and EP states */
+		udc->usb_state = USB_STATE_ATTACHED;
+		udc->ep0_state = WAIT_FOR_SETUP;
+		udc->ep0_dir = 0;
+		udc->vbus_active = 1;
+		if (tegra_usb_phy_charger_detected(udc->phy)) {
+			tegra_detect_charging_type_is_cdp_or_dcp(udc);
+		} else {
+			udc->connect_type = CONNECT_TYPE_SDP;
+			/*
+			 * Schedule work to wait for 1000 msec and check for
+			 * a non-standard charger if setup packet is not
+			 * received.
+			 */
+			schedule_delayed_work(&udc->work, msecs_to_jiffies(
+					USB_CHARGER_DETECTION_WAIT_TIME_MS));
+		}
+		/* start the controller */
+		dr_controller_run(udc);
+		tegra_usb_set_charging_current(udc);
+	}
+
+	return 0;
+}
+
+/**
+ * Constrain controller's VBUS power usage.
+ * This call is used by gadget drivers during SET_CONFIGURATION calls,
+ * reporting how much power the device may consume. For example, this
+ * could affect how quickly batteries are recharged.
+ *
+ * Returns zero on success, else negative errno.
+ */
+static int tegra_vbus_draw(struct usb_gadget *gadget, unsigned mA)
+{
+	struct tegra_udc *udc;
+
+	udc = container_of(gadget, struct tegra_udc, gadget);
+	/* check udc regulator is available for drawing the vbus current */
+	if (udc->vbus_reg) {
+		udc->current_limit = mA;
+		schedule_work(&udc->charger_work);
+	}
+
+	if (udc->transceiver)
+		return otg_set_power(udc->transceiver, mA);
+	return -ENOTSUPP;
+}
+
+/**
+ * Change Data+ pullup status
+ * this func is used by usb_gadget_connect/disconnect
+ */
+static int tegra_pullup(struct usb_gadget *gadget, int is_on)
+{
+	struct tegra_udc *udc;
+	u32 tmp;
+
+	udc = container_of(gadget, struct tegra_udc, gadget);
+	udc->softconnect = (is_on != 0);
+	if (udc->transceiver && udc->transceiver->state !=
+			OTG_STATE_B_PERIPHERAL)
+			return 0;
+
+	tmp = udc_readl(udc, USB_CMD_REG_OFFSET);
+	if (can_pullup(udc))
+		udc_writel(udc, tmp | USB_CMD_RUN_STOP,
+				USB_CMD_REG_OFFSET);
+	else
+		udc_writel(udc, (tmp & ~USB_CMD_RUN_STOP),
+				USB_CMD_REG_OFFSET);
+
+	return 0;
+}
+
+/* Release udc structures */
+static void tegra_udc_release(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct tegra_udc *udc = platform_get_drvdata(pdev);
+
+	complete(udc->done);
+	tegra_usb_phy_close(udc->phy);
+	kfree(udc);
+}
+
+static int tegra_udc_start(struct usb_gadget_driver *driver,
+		int (*bind)(struct usb_gadget *));
+static int tegra_udc_stop(struct usb_gadget_driver *driver);
+/* defined in gadget.h */
+static struct usb_gadget_ops tegra_gadget_ops = {
+	.get_frame = tegra_get_frame,
+#ifndef CONFIG_USB_ANDROID
+	.wakeup = tegra_wakeup,
+#endif
+	.set_selfpowered = tegra_set_selfpowered,
+	.vbus_session = tegra_vbus_session,
+	.vbus_draw = tegra_vbus_draw,
+	.pullup = tegra_pullup,
+	.start = tegra_udc_start,
+	.stop = tegra_udc_stop,
+};
+
+static int tegra_udc_setup_gadget_dev(struct tegra_udc *udc)
+{
+	/* Setup gadget structure */
+	udc->gadget.ops = &tegra_gadget_ops;
+	udc->gadget.is_dualspeed = 1;
+	udc->gadget.ep0 = &udc->eps[0].ep;
+	INIT_LIST_HEAD(&udc->gadget.ep_list);
+	udc->gadget.speed = USB_SPEED_UNKNOWN;
+	udc->gadget.name = driver_name;
+
+	/* Setup gadget.dev and register with kernel */
+	dev_set_name(&udc->gadget.dev, "gadget");
+	udc->gadget.dev.release = tegra_udc_release;
+	udc->gadget.dev.parent = &udc->pdev->dev;
+
+	return device_register(&udc->gadget.dev);
+}
+
+
+/**
+ * Set protocol stall on ep0, protocol stall will automatically be cleared
+ * on new transaction.
+ */
+static void ep0stall(struct tegra_udc *udc)
+{
+	u32 tmp;
+
+	/* must set tx and rx to stall at the same time */
+	tmp = udc_readl(udc, EP_CONTROL_REG_OFFSET);
+	tmp |= EPCTRL_TX_EP_STALL | EPCTRL_RX_EP_STALL;
+	udc_writel(udc, tmp, EP_CONTROL_REG_OFFSET);
+	udc->ep0_state = WAIT_FOR_SETUP;
+	udc->ep0_dir = 0;
+}
+
+/* Prime a status phase for ep0 */
+static int ep0_prime_status(struct tegra_udc *udc, int direction)
+{
+	struct tegra_req *req = udc->status_req;
+	struct tegra_ep *ep;
+
+	if (direction == EP_DIR_IN)
+		udc->ep0_dir = USB_DIR_IN;
+	else
+		udc->ep0_dir = USB_DIR_OUT;
+
+	ep = &udc->eps[0];
+	udc->ep0_state = WAIT_FOR_OUT_STATUS;
+
+	req->ep = ep;
+	req->req.length = 0;
+	req->req.status = -EINPROGRESS;
+	req->req.actual = 0;
+	req->req.complete = NULL;
+	req->dtd_count = 0;
+
+	if (tegra_req_to_dtd(req, GFP_ATOMIC) == 0)
+		tegra_queue_td(ep, req);
+	else
+		return -ENOMEM;
+
+	list_add_tail(&req->queue, &ep->queue);
+
+	return 0;
+}
+
+static void udc_reset_ep_queue(struct tegra_udc *udc, u8 pipe)
+{
+	struct tegra_ep *ep = get_ep_by_pipe(udc, pipe);
+
+	if (ep->name)
+		nuke(ep, -ESHUTDOWN);
+}
+
+/* ch9 Set address */
+static void ch9setaddress(struct tegra_udc *udc, u16 value, u16 index,
+		u16 length)
+{
+	/* Save the new address to device struct */
+	udc->device_address = (u8) value;
+	/* Update usb state */
+	udc->usb_state = USB_STATE_ADDRESS;
+	/* Status phase */
+	if (ep0_prime_status(udc, EP_DIR_IN))
+		ep0stall(udc);
+}
+
+/* ch9 Get status */
+static void ch9getstatus(struct tegra_udc *udc, u8 request_type, u16 value,
+		u16 index, u16 length)
+{
+	u16 tmp = 0;		/* Status, cpu endian */
+	struct tegra_req *req;
+	struct tegra_ep *ep;
+
+	ep = &udc->eps[0];
+
+	if ((request_type & USB_RECIP_MASK) == USB_RECIP_DEVICE) {
+		/* Get device status */
+		if (udc->selfpowered)
+			tmp = 1 << USB_DEVICE_SELF_POWERED;
+		tmp |= udc->remote_wakeup << USB_DEVICE_REMOTE_WAKEUP;
+	} else if ((request_type & USB_RECIP_MASK) == USB_RECIP_INTERFACE) {
+		/* Get interface status
+		 * We don't have interface information in udc driver */
+		tmp = 0;
+	} else if ((request_type & USB_RECIP_MASK) == USB_RECIP_ENDPOINT) {
+		/* Get endpoint status */
+		struct tegra_ep *target_ep;
+
+		target_ep = get_ep_by_pipe(udc, get_pipe_by_windex(index));
+
+		/* stall if endpoint doesn't exist */
+		if (!target_ep->desc)
+			goto stall;
+		tmp = dr_ep_get_stall(udc, ep_index(target_ep),
+				ep_is_in(target_ep)) << USB_ENDPOINT_HALT;
+	}
+
+	udc->ep0_dir = USB_DIR_IN;
+	/* Borrow the per device status_req */
+	req = udc->status_req;
+	/* Fill in the reqest structure */
+	*((u16 *) req->req.buf) = cpu_to_le16(tmp);
+	req->ep = ep;
+	req->req.length = 2;
+	req->req.status = -EINPROGRESS;
+	req->req.actual = 0;
+	req->req.complete = NULL;
+	req->dtd_count = 0;
+
+	/* map virtual address to hardware */
+	if (req->req.dma == DMA_ADDR_INVALID) {
+		req->req.dma = dma_map_single(ep->udc->gadget.dev.parent,
+					req->req.buf,
+					req->req.length, ep_is_in(ep)
+						? DMA_TO_DEVICE
+						: DMA_FROM_DEVICE);
+		req->mapped = 1;
+	} else {
+		dma_sync_single_for_device(ep->udc->gadget.dev.parent,
+					req->req.dma, req->req.length,
+					ep_is_in(ep)
+						? DMA_TO_DEVICE
+						: DMA_FROM_DEVICE);
+		req->mapped = 0;
+	}
+
+	/* prime the data phase */
+	if ((tegra_req_to_dtd(req, GFP_ATOMIC) == 0))
+		tegra_queue_td(ep, req);
+	else			/* no mem */
+		goto stall;
+
+	list_add_tail(&req->queue, &ep->queue);
+	udc->ep0_state = DATA_STATE_XMIT;
+	return;
+stall:
+	ep0stall(udc);
+}
+
+static void udc_test_mode(struct tegra_udc *udc, u32 test_mode)
+{
+	struct tegra_req *req;
+	struct tegra_ep *ep;
+	u32 portsc, bitmask;
+	unsigned long timeout;
+
+	/* Ack the ep0 IN */
+	if (ep0_prime_status(udc, EP_DIR_IN))
+		ep0stall(udc);
+
+	/* get the ep0 */
+	ep = &udc->eps[0];
+	bitmask = ep_is_in(ep)
+		? (1 << (ep_index(ep) + 16))
+		: (1 << (ep_index(ep)));
+
+	timeout = jiffies + HZ;
+	/* Wait until ep0 IN endpoint txfr is complete */
+	while (!(udc_readl(udc, EP_COMPLETE_REG_OFFSET) & bitmask)) {
+		if (time_after(jiffies, timeout)) {
+			pr_err("Timeout for Ep0 IN Ack\n");
+			break;
+		}
+		cpu_relax();
+	}
+
+	switch (test_mode << PORTSCX_PTC_BIT_POS) {
+	case PORTSCX_PTC_JSTATE:
+		VDBG("TEST_J\n");
+		break;
+	case PORTSCX_PTC_KSTATE:
+		VDBG("TEST_K\n");
+		break;
+	case PORTSCX_PTC_SEQNAK:
+		VDBG("TEST_SE0_NAK\n");
+		break;
+	case PORTSCX_PTC_PACKET:
+		VDBG("TEST_PACKET\n");
+
+		/* get the ep and configure for IN direction */
+		ep = &udc->eps[0];
+		udc->ep0_dir = USB_DIR_IN;
+
+		/* Initialize ep0 status request structure */
+		req = container_of(tegra_alloc_request(NULL, GFP_ATOMIC),
+				struct tegra_req, req);
+		/* allocate a small amount of memory to get valid address */
+		req->req.buf = kmalloc(sizeof(tegra_udc_test_packet),
+					GFP_ATOMIC);
+		req->req.dma = virt_to_phys(req->req.buf);
+
+		/* Fill in the reqest structure */
+		memcpy(req->req.buf, tegra_udc_test_packet,
+					sizeof(tegra_udc_test_packet));
+		req->ep = ep;
+		req->req.length = sizeof(tegra_udc_test_packet);
+		req->req.status = -EINPROGRESS;
+		req->req.actual = 0;
+		req->req.complete = NULL;
+		req->dtd_count = 0;
+		req->mapped = 0;
+
+		dma_sync_single_for_device(ep->udc->gadget.dev.parent,
+					req->req.dma, req->req.length,
+					ep_is_in(ep)
+						? DMA_TO_DEVICE
+						: DMA_FROM_DEVICE);
+
+		/* prime the data phase */
+		if ((tegra_req_to_dtd(req, GFP_ATOMIC) == 0))
+			tegra_queue_td(ep, req);
+		else			/* no mem */
+			goto stall;
+
+		list_add_tail(&req->queue, &ep->queue);
+		udc->ep0_state = DATA_STATE_XMIT;
+		break;
+	case PORTSCX_PTC_FORCE_EN:
+		VDBG("TEST_FORCE_EN\n");
+		break;
+	default:
+		ERR("udc unknown test mode[%d]!\n", test_mode);
+		goto stall;
+	}
+
+	/* read the portsc register */
+	portsc = udc_readl(udc, PORTSCX_REG_OFFSET);
+	/* set the test mode selector */
+	portsc |= test_mode << PORTSCX_PTC_BIT_POS;
+	udc_writel(udc, portsc, PORTSCX_REG_OFFSET);
+
+	/*
+	 * The device must have its power cycled to exit test mode.
+	 * See USB 2.0 spec, section 9.4.9 for test modes operation
+	 * in "Set Feature".
+	 * See USB 2.0 spec, section 7.1.20 for test modes.
+	 */
+	pr_info("udc entering the test mode, power cycle to exit test mode\n");
+	return;
+stall:
+	ep0stall(udc);
+}
+
+static void setup_received_irq(struct tegra_udc *udc,
+		struct usb_ctrlrequest *setup)
+{
+	u16 wValue = le16_to_cpu(setup->wValue);
+	u16 wIndex = le16_to_cpu(setup->wIndex);
+	u16 wLength = le16_to_cpu(setup->wLength);
+
+	udc_reset_ep_queue(udc, 0);
+
+	/* We process some stardard setup requests here */
+	switch (setup->bRequest) {
+	case USB_REQ_GET_STATUS:
+		/* Data+Status phase from udc */
+		if ((setup->bRequestType & (USB_DIR_IN | USB_TYPE_MASK))
+					!= (USB_DIR_IN | USB_TYPE_STANDARD))
+			break;
+		ch9getstatus(udc, setup->bRequestType, wValue, wIndex, wLength);
+		return;
+
+	case USB_REQ_SET_ADDRESS:
+		/* Status phase from udc */
+		if (setup->bRequestType != (USB_DIR_OUT | USB_TYPE_STANDARD
+						| USB_RECIP_DEVICE))
+			break;
+		/* This delay is necessary for some windows drivers to
+		 * properly recognize the device */
+		mdelay(1);
+		ch9setaddress(udc, wValue, wIndex, wLength);
+		return;
+
+	case USB_REQ_CLEAR_FEATURE:
+	case USB_REQ_SET_FEATURE:
+		/* Status phase from udc */
+	{
+		int rc = -EOPNOTSUPP;
+
+		if (setup->bRequestType == USB_RECIP_DEVICE &&
+				 wValue == USB_DEVICE_TEST_MODE) {
+			/*
+			 * If the feature selector is TEST_MODE, then the most
+			 * significant byte of wIndex is used to specify the
+			 * specific test mode and the lower byte of wIndex must
+			 * be zero.
+			 */
+			udc_test_mode(udc, wIndex >> 8);
+			return;
+
+		} else if ((setup->bRequestType &
+				(USB_RECIP_MASK | USB_TYPE_MASK)) ==
+				(USB_RECIP_ENDPOINT | USB_TYPE_STANDARD)) {
+			int pipe = get_pipe_by_windex(wIndex);
+			struct tegra_ep *ep;
+
+			if (wValue != 0 || wLength != 0 || pipe > udc->max_ep)
+				break;
+			ep = get_ep_by_pipe(udc, pipe);
+
+			spin_unlock(&udc->lock);
+			rc = tegra_ep_set_halt(&ep->ep,
+					(setup->bRequest == USB_REQ_SET_FEATURE)
+						? 1 : 0);
+			spin_lock(&udc->lock);
+
+		} else if ((setup->bRequestType & (USB_RECIP_MASK
+				| USB_TYPE_MASK)) == (USB_RECIP_DEVICE
+				| USB_TYPE_STANDARD)) {
+			/* Note: The driver has not include OTG support yet.
+			 * This will be set when OTG support is added */
+			if (!gadget_is_otg(&udc->gadget))
+				break;
+			else if (setup->bRequest == USB_DEVICE_B_HNP_ENABLE)
+				udc->gadget.b_hnp_enable = 1;
+			else if (setup->bRequest == USB_DEVICE_A_HNP_SUPPORT)
+				udc->gadget.a_hnp_support = 1;
+			else if (setup->bRequest ==
+					USB_DEVICE_A_ALT_HNP_SUPPORT)
+				udc->gadget.a_alt_hnp_support = 1;
+			else
+				break;
+			rc = 0;
+		} else
+			break;
+
+		if (rc == 0) {
+			if (ep0_prime_status(udc, EP_DIR_IN))
+				ep0stall(udc);
+		}
+		return;
+	}
+
+	default:
+		break;
+	}
+
+	/* Requests handled by gadget */
+	if (wLength) {
+		/* Data phase from gadget, status phase from udc */
+		udc->ep0_dir = (setup->bRequestType & USB_DIR_IN)
+				?  USB_DIR_IN : USB_DIR_OUT;
+		spin_unlock(&udc->lock);
+		if (udc->driver && udc->driver->setup(&udc->gadget,
+				&udc->local_setup_buff) < 0)
+			ep0stall(udc);
+		spin_lock(&udc->lock);
+		udc->ep0_state = (setup->bRequestType & USB_DIR_IN)
+				?  DATA_STATE_XMIT : DATA_STATE_RECV;
+	} else {
+		/* No data phase, IN status from gadget */
+		udc->ep0_dir = USB_DIR_IN;
+		spin_unlock(&udc->lock);
+		if (udc->driver && udc->driver->setup(&udc->gadget,
+				&udc->local_setup_buff) < 0)
+			ep0stall(udc);
+		spin_lock(&udc->lock);
+		udc->ep0_state = WAIT_FOR_OUT_STATUS;
+	}
+}
+
+/* Process request for Data or Status phase of ep0
+ * prime status phase if needed */
+static void ep0_req_complete(struct tegra_udc *udc, struct tegra_ep *ep0,
+		struct tegra_req *req)
+{
+	if (udc->usb_state == USB_STATE_ADDRESS) {
+		/* Set the new address */
+		u32 new_address = (u32) udc->device_address;
+		udc_writel(udc, new_address << USB_DEVICE_ADDRESS_BIT_POS,
+				USB_DEVICE_ADDR_REG_OFFSET);
+	}
+
+	done(ep0, req, 0);
+
+	switch (udc->ep0_state) {
+	case DATA_STATE_XMIT:
+		/* receive status phase */
+		if (ep0_prime_status(udc, EP_DIR_OUT))
+			ep0stall(udc);
+		break;
+	case DATA_STATE_RECV:
+		/* send status phase */
+		if (ep0_prime_status(udc, EP_DIR_IN))
+			ep0stall(udc);
+		break;
+	case WAIT_FOR_OUT_STATUS:
+		udc->ep0_state = WAIT_FOR_SETUP;
+		break;
+	case WAIT_FOR_SETUP:
+		ERR("Unexpect ep0 packets\n");
+		break;
+	default:
+		ep0stall(udc);
+		break;
+	}
+}
+
+/* Tripwire mechanism to ensure a setup packet payload is extracted without
+ * being corrupted by another incoming setup packet */
+static void tripwire_handler(struct tegra_udc *udc, u8 ep_num, u8 *buffer_ptr)
+{
+	u32 temp;
+	struct ep_queue_head *qh;
+
+	qh = &udc->ep_qh[ep_num * 2 + EP_DIR_OUT];
+
+	/* Clear bit in ENDPTSETUPSTAT */
+	temp = udc_readl(udc, EP_SETUP_STATUS_REG_OFFSET);
+	udc_writel(udc, temp | (1 << ep_num), EP_SETUP_STATUS_REG_OFFSET);
+
+	/* while a hazard exists when setup package arrives */
+	do {
+		/* Set Setup Tripwire */
+		temp = udc_readl(udc, USB_CMD_REG_OFFSET);
+		udc_writel(udc, temp | USB_CMD_SUTW, USB_CMD_REG_OFFSET);
+
+		/* Copy the setup packet to local buffer */
+		memcpy(buffer_ptr, (u8 *) qh->setup_buffer, 8);
+	} while (!(udc_readl(udc, USB_CMD_REG_OFFSET) & USB_CMD_SUTW));
+
+	/* Clear Setup Tripwire */
+	temp = udc_readl(udc, USB_CMD_REG_OFFSET);
+	udc_writel(udc, temp & ~USB_CMD_SUTW, USB_CMD_REG_OFFSET);
+}
+
+/* process-ep_req(): free the completed Tds for this req */
+static int process_ep_req(struct tegra_udc *udc, int pipe,
+		struct tegra_req *curr_req)
+{
+	struct ep_td_struct *curr_td;
+	int	td_complete, actual, remaining_length, j, tmp;
+	int	status = 0;
+	int	errors = 0;
+	struct  ep_queue_head *curr_qh = &udc->ep_qh[pipe];
+	int direction = pipe % 2;
+
+	curr_td = curr_req->head;
+	td_complete = 0;
+	actual = curr_req->req.length;
+
+	for (j = 0; j < curr_req->dtd_count; j++) {
+		/* Fence read for coherency of AHB master intiated writes */
+		readb(IO_ADDRESS(IO_PPCS_PHYS + USB1_PREFETCH_ID));
+
+		dma_sync_single_for_cpu(udc->gadget.dev.parent, curr_td->td_dma,
+				sizeof(struct ep_td_struct), DMA_FROM_DEVICE);
+
+		remaining_length = (le32_to_cpu(curr_td->size_ioc_sts)
+					& DTD_PACKET_SIZE)
+				>> DTD_LENGTH_BIT_POS;
+		actual -= remaining_length;
+		errors = le32_to_cpu(curr_td->size_ioc_sts);
+		if (errors & DTD_ERROR_MASK) {
+			if (errors & DTD_STATUS_HALTED) {
+				ERR("dTD error %08x QH=%d\n", errors, pipe);
+				/* Clear the errors and Halt condition */
+				tmp = le32_to_cpu(curr_qh->size_ioc_int_sts);
+				tmp &= ~errors;
+				curr_qh->size_ioc_int_sts = cpu_to_le32(tmp);
+				status = -EPIPE;
+				/* FIXME: continue with next queued TD? */
+
+				break;
+			}
+			if (errors & DTD_STATUS_DATA_BUFF_ERR) {
+				VDBG("Transfer overflow");
+				status = -EPROTO;
+				break;
+			} else if (errors & DTD_STATUS_TRANSACTION_ERR) {
+				VDBG("ISO error");
+				status = -EILSEQ;
+				break;
+			} else
+				ERR("Unknown error has occurred (0x%x)!\n",
+					errors);
+
+		} else if (le32_to_cpu(curr_td->size_ioc_sts)
+				& DTD_STATUS_ACTIVE) {
+			VDBG("Request not complete");
+			status = REQ_UNCOMPLETE;
+			return status;
+		} else if (remaining_length) {
+			if (direction) {
+				VDBG("Transmit dTD remaining length not zero");
+				status = -EPROTO;
+				break;
+			} else {
+				td_complete++;
+				break;
+			}
+		} else {
+			td_complete++;
+			VDBG("dTD transmitted successful");
+		}
+
+		if (j != curr_req->dtd_count - 1)
+			curr_td = (struct ep_td_struct *)curr_td->next_td_virt;
+	}
+
+	if (status)
+		return status;
+
+	curr_req->req.actual = actual;
+
+	return 0;
+}
+
+/* Process a DTD completion interrupt */
+static void dtd_complete_irq(struct tegra_udc *udc)
+{
+	u32 bit_pos;
+	int i, ep_num, direction, bit_mask, status;
+	struct tegra_ep *curr_ep;
+	struct tegra_req *curr_req, *temp_req;
+
+	/* Clear the bits in the register */
+	bit_pos = udc_readl(udc, EP_COMPLETE_REG_OFFSET);
+	udc_writel(udc, bit_pos, EP_COMPLETE_REG_OFFSET);
+
+	if (!bit_pos)
+		return;
+
+	for (i = 0; i < udc->max_ep; i++) {
+		ep_num = i >> 1;
+		direction = i % 2;
+
+		bit_mask = 1 << (ep_num + 16 * direction);
+
+		if (!(bit_pos & bit_mask))
+			continue;
+
+		curr_ep = get_ep_by_pipe(udc, i);
+
+		/* If the ep is configured */
+		if (curr_ep->name == NULL) {
+			WARNING("Invalid EP?");
+			continue;
+		}
+
+		/* process the req queue until an uncomplete request */
+		list_for_each_entry_safe(curr_req, temp_req, &curr_ep->queue,
+				queue) {
+			status = process_ep_req(udc, i, curr_req);
+
+			VDBG("status of process_ep_req= %d, ep = %d",
+					status, ep_num);
+			if (status == REQ_UNCOMPLETE)
+				break;
+			/* write back status to req */
+			curr_req->req.status = status;
+
+			if (ep_num == 0) {
+				ep0_req_complete(udc, curr_ep, curr_req);
+				break;
+			} else
+				done(curr_ep, curr_req, status);
+		}
+	}
+}
+
+/* Process a port change interrupt */
+static void port_change_irq(struct tegra_udc *udc)
+{
+	u32 speed;
+	unsigned int port_control_reg_offset;
+
+	if (udc->has_hostpc)
+		port_control_reg_offset = USB_HOSTPCX_DEVLC_REG_OFFSET;
+	else
+		port_control_reg_offset = PORTSCX_REG_OFFSET;
+
+	/* Bus resetting is finished */
+	if (!(udc_readl(udc, port_control_reg_offset) & PORTSCX_PORT_RESET)) {
+		/* Get the speed */
+		speed = (udc_readl(udc, port_control_reg_offset)
+				& PORTSCX_PORT_SPEED_MASK);
+		if (speed == PORTSCX_PORT_SPEED_HIGH)
+			udc->gadget.speed = USB_SPEED_HIGH;
+		else if (speed == PORTSCX_PORT_SPEED_FULL)
+			udc->gadget.speed = USB_SPEED_FULL;
+		else if (speed == PORTSCX_PORT_SPEED_LOW)
+			udc->gadget.speed = USB_SPEED_LOW;
+		else
+			udc->gadget.speed = USB_SPEED_UNKNOWN;
+	}
+
+	/* Update USB state */
+	if (!udc->resume_state)
+		udc->usb_state = USB_STATE_DEFAULT;
+}
+
+/* Process suspend interrupt */
+static void suspend_irq(struct tegra_udc *udc)
+{
+	udc->resume_state = udc->usb_state;
+	udc->usb_state = USB_STATE_SUSPENDED;
+
+	/* report suspend to the driver, serial.c does not support this */
+	if (udc->driver && udc->driver->suspend)
+		udc->driver->suspend(&udc->gadget);
+}
+
+static void bus_resume(struct tegra_udc *udc)
+{
+	udc->usb_state = udc->resume_state;
+	udc->resume_state = 0;
+
+	/* report resume to the driver, serial.c does not support this */
+	if (udc->driver && udc->driver->resume)
+		udc->driver->resume(&udc->gadget);
+}
+
+/* Clear up all ep queues */
+static int reset_queues(struct tegra_udc *udc)
+{
+	u8 pipe;
+
+	for (pipe = 0; pipe < udc->max_pipes; pipe++)
+		udc_reset_ep_queue(udc, pipe);
+
+	/* report disconnect; the driver is already quiesced */
+	spin_unlock(&udc->lock);
+	if (udc->driver && udc->driver->disconnect)
+		udc->driver->disconnect(&udc->gadget);
+	spin_lock(&udc->lock);
+
+	return 0;
+}
+
+/* Process reset interrupt */
+static void reset_irq(struct tegra_udc *udc)
+{
+	u32 temp;
+	unsigned long timeout;
+
+	/* Clear the device address */
+	temp = udc_readl(udc, USB_DEVICE_ADDR_REG_OFFSET);
+	udc_writel(udc, temp & ~USB_DEVICE_ADDRESS_MASK,
+				USB_DEVICE_ADDR_REG_OFFSET);
+
+	udc->device_address = 0;
+
+	/* Clear usb state */
+	udc->resume_state = 0;
+	udc->ep0_dir = 0;
+	udc->ep0_state = WAIT_FOR_SETUP;
+	udc->remote_wakeup = 0;	/* default to 0 on reset */
+	udc->gadget.b_hnp_enable = 0;
+	udc->gadget.a_hnp_support = 0;
+	udc->gadget.a_alt_hnp_support = 0;
+
+	/* Clear all the setup token semaphores */
+	temp = udc_readl(udc, EP_SETUP_STATUS_REG_OFFSET);
+	udc_writel(udc, temp, EP_SETUP_STATUS_REG_OFFSET);
+
+	/* Clear all the endpoint complete status bits */
+	temp = udc_readl(udc, EP_COMPLETE_REG_OFFSET);
+	udc_writel(udc, temp, EP_COMPLETE_REG_OFFSET);
+
+	timeout = jiffies + 100;
+	while (udc_readl(udc, EP_PRIME_REG_OFFSET)) {
+		/* Wait until all endptprime bits cleared */
+		if (time_after(jiffies, timeout)) {
+			ERR("Timeout for reset\n");
+			break;
+		}
+		cpu_relax();
+	}
+
+	/* Write 1s to the flush register */
+	udc_writel(udc, 0xffffffff, EPFLUSH_REG_OFFSET);
+
+	/* When the bus reset is seen on Tegra, the PORTSCX_PORT_RESET bit
+	 * is not set. Reset all the queues, include XD, dTD, EP queue
+	 * head and TR Queue */
+	VDBG("Bus reset");
+	reset_queues(udc);
+	udc->usb_state = USB_STATE_DEFAULT;
+}
+
+static void tegra_udc_set_current_limit_work(struct work_struct *work)
+{
+	struct tegra_udc *udc = container_of(work, struct tegra_udc,
+						charger_work);
+	/* check udc regulator is available for drawing vbus current*/
+	if (udc->vbus_reg) {
+		/* set the current limit in uA */
+		regulator_set_current_limit(
+			udc->vbus_reg, 0,
+			udc->current_limit * 1000);
+	}
+}
+
+#ifdef CONFIG_TEGRA_GADGET_BOOST_CPU_FREQ
+static void tegra_udc_boost_cpu_frequency_work(struct work_struct *work)
+{
+	if (ep_queue_request_count && boost_cpufreq_work_flag) {
+		pm_qos_update_request(&boost_cpu_freq_req,
+			(s32)CONFIG_TEGRA_GADGET_BOOST_CPU_FREQ * 1000);
+		boost_cpufreq_work_flag = 0;
+	} else if (!ep_queue_request_count && !boost_cpufreq_work_flag) {
+		pm_qos_update_request(&boost_cpu_freq_req,
+			PM_QOS_DEFAULT_VALUE);
+		boost_cpufreq_work_flag = 1;
+	}
+}
+#endif
+
+static void tegra_udc_irq_work(struct work_struct *irq_work)
+{
+	struct tegra_udc *udc = container_of(irq_work, struct tegra_udc,
+						 irq_work);
+	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
+
+	/* Check whether cable is connected*/
+	if (vbus_enabled(udc))
+		tegra_vbus_session(&udc->gadget, 1);
+	else
+		tegra_vbus_session(&udc->gadget, 0);
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+}
+
+/*
+ * When VBUS is detected we already know it is DCP/SDP/CDP devices if it is a
+ * standard device; If we did not receive EP0 setup packet, we can assuming it
+ * is a charger capable of 1.8A charging.
+ */
+static void tegra_udc_charger_detect_work(struct work_struct *work)
+{
+	struct tegra_udc *udc = container_of(work, struct tegra_udc, work.work);
+	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
+
+	udc->connect_type = CONNECT_TYPE_NON_STANDARD_CHARGER;
+	tegra_usb_set_charging_current(udc);
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+}
+
+/* Restart device controller in the OTG mode on VBUS detection */
+static void tegra_udc_restart(struct tegra_udc *udc)
+{
+	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
+
+	/* setup the controller in the device mode */
+	dr_controller_setup(udc);
+	/* setup EP0 for setup packet */
+	ep0_setup(udc);
+	udc->vbus_active = 1;
+	/* start the controller */
+	dr_controller_run(udc);
+	/* initialize the USB and EP states */
+	udc->usb_state = USB_STATE_ATTACHED;
+	udc->ep0_state = WAIT_FOR_SETUP;
+	udc->ep0_dir = 0;
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+}
+
+/* USB device controller interrupt handler */
+static irqreturn_t tegra_udc_irq(int irq, void *_udc)
+{
+	struct tegra_udc *udc = _udc;
+	u32 irq_src, temp;
+	irqreturn_t status = IRQ_NONE;
+	unsigned long flags;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	if (!udc->transceiver) {
+		temp = udc_readl(udc, VBUS_WAKEUP_REG_OFFSET);
+		/* write back the register to clear the interrupt */
+		udc_writel(udc, temp, VBUS_WAKEUP_REG_OFFSET);
+		if (temp & USB_SYS_VBUS_WAKEUP_INT_STATUS)
+			schedule_work(&udc->irq_work);
+		status = IRQ_HANDLED;
+	}
+
+	/* Disable ISR for OTG host mode */
+	if (udc->stopped)
+		goto done;
+
+	/* Fence read for coherency of AHB master intiated writes */
+	readb(IO_ADDRESS(IO_PPCS_PHYS + USB1_PREFETCH_ID));
+
+	irq_src = udc_readl(udc, USB_STS_REG_OFFSET) &
+				udc_readl(udc, USB_INTR_REG_OFFSET);
+
+	if (irq_src == 0)
+		goto done;
+
+	/* Clear notification bits */
+	udc_writel(udc, irq_src, USB_STS_REG_OFFSET);
+
+	/* Need to resume? */
+	if (udc->usb_state == USB_STATE_SUSPENDED)
+		if (!(udc_readl(udc, PORTSCX_REG_OFFSET)
+				& PORTSCX_PORT_SUSPEND))
+			bus_resume(udc);
+
+	/* USB Interrupt */
+	if (irq_src & USB_STS_INT) {
+		VDBG("Packet int");
+		/* Setup package, we only support ep0 as control ep */
+		if (udc_readl(udc, EP_SETUP_STATUS_REG_OFFSET) &
+				EP_SETUP_STATUS_EP0) {
+			/* Setup packet received, we are connected to host
+			 * and not to charger. Cancel any delayed work */
+			__cancel_delayed_work(&udc->work);
+			tripwire_handler(udc, 0,
+					(u8 *) (&udc->local_setup_buff));
+			setup_received_irq(udc, &udc->local_setup_buff);
+			status = IRQ_HANDLED;
+		}
+
+		/* completion of dtd */
+		if (udc_readl(udc, EP_COMPLETE_REG_OFFSET)) {
+			dtd_complete_irq(udc);
+			status = IRQ_HANDLED;
+		}
+	}
+
+	/* SOF (for ISO transfer) */
+	if (irq_src & USB_STS_SOF)
+		status = IRQ_HANDLED;
+
+	/* Port Change */
+	if (irq_src & USB_STS_PORT_CHANGE) {
+		port_change_irq(udc);
+		status = IRQ_HANDLED;
+	}
+
+	/* Reset Received */
+	if (irq_src & USB_STS_RESET) {
+		reset_irq(udc);
+		status = IRQ_HANDLED;
+	}
+
+	/* Sleep Enable (Suspend) */
+	if (irq_src & USB_STS_SUSPEND) {
+		suspend_irq(udc);
+		status = IRQ_HANDLED;
+	}
+
+	if (irq_src & (USB_STS_ERR | USB_STS_SYS_ERR))
+		VDBG("Error IRQ %x", irq_src);
+
+done:
+	spin_unlock_irqrestore(&udc->lock, flags);
+	return status;
+}
+
+/**
+ * Hook to gadget drivers
+ * Called by initialization code of gadget drivers
+ */
+static int tegra_udc_start(struct usb_gadget_driver *driver,
+		int (*bind)(struct usb_gadget *))
+{
+	struct tegra_udc *udc = the_udc;
+	int retval = -ENODEV;
+	unsigned long flags = 0;
+	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
+
+	if (!udc)
+		return -ENODEV;
+
+	if (!driver || (driver->speed != USB_SPEED_FULL
+				&& driver->speed != USB_SPEED_HIGH)
+			|| !bind || !driver->disconnect
+			|| !driver->setup)
+		return -EINVAL;
+
+	if (udc->driver)
+		return -EBUSY;
+
+	/* lock is needed but whether should use this lock or another */
+	spin_lock_irqsave(&udc->lock, flags);
+
+	driver->driver.bus = NULL;
+	/* hook up the driver */
+	udc->driver = driver;
+	udc->gadget.dev.driver = &driver->driver;
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	/* bind udc driver to gadget driver */
+	retval = bind(&udc->gadget);
+	if (retval) {
+		VDBG("bind to %s --> %d", driver->driver.name, retval);
+		udc->gadget.dev.driver = NULL;
+		udc->driver = NULL;
+		goto out;
+	}
+
+	/* Enable DR IRQ reg and Set usbcmd reg  Run bit */
+	if (vbus_enabled(udc)) {
+		dr_controller_run(udc);
+		udc->usb_state = USB_STATE_ATTACHED;
+		udc->ep0_state = WAIT_FOR_SETUP;
+		udc->ep0_dir = 0;
+		udc->vbus_active = 1;
+	}
+
+	printk(KERN_INFO "%s: bind to driver %s\n",
+			udc->gadget.name, driver->driver.name);
+
+out:
+	if (retval)
+		printk(KERN_WARNING "gadget driver register failed %d\n",
+								retval);
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+	return retval;
+}
+
+/* Disconnect from gadget driver */
+static int tegra_udc_stop(struct usb_gadget_driver *driver)
+{
+	struct tegra_udc *udc = the_udc;
+	struct tegra_ep *loop_ep;
+	unsigned long flags;
+
+	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
+	if (!udc)
+		return -ENODEV;
+
+	if (!driver || driver != udc->driver || !driver->unbind)
+		return -EINVAL;
+
+	/* stop DR, disable intr */
+	dr_controller_stop(udc);
+
+	/* in fact, no needed */
+	udc->usb_state = USB_STATE_ATTACHED;
+	udc->ep0_state = WAIT_FOR_SETUP;
+	udc->ep0_dir = 0;
+
+	/* stand operation */
+	spin_lock_irqsave(&udc->lock, flags);
+	udc->gadget.speed = USB_SPEED_UNKNOWN;
+	nuke(&udc->eps[0], -ESHUTDOWN);
+	list_for_each_entry(loop_ep, &udc->gadget.ep_list,
+			ep.ep_list)
+		nuke(loop_ep, -ESHUTDOWN);
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	/* report disconnect; the controller is already quiesced */
+	driver->disconnect(&udc->gadget);
+
+	/* unbind gadget and unhook driver. */
+	driver->unbind(&udc->gadget);
+	udc->gadget.dev.driver = NULL;
+	udc->driver = NULL;
+
+	printk(KERN_WARNING "unregistered gadget driver '%s'\n",
+	       driver->driver.name);
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+
+	return 0;
+}
+
+
+/* Internal structure setup functions */
+static int tegra_udc_setup_qh(struct tegra_udc *udc)
+{
+	u32 dccparams;
+	size_t size;
+
+	/* Read Device Controller Capability Parameters register */
+	dccparams = udc_readl(udc, DCCPARAMS_REG_OFFSET);
+	if (!(dccparams & DCCPARAMS_DC)) {
+		ERR("This SOC doesn't support device role\n");
+		return -ENODEV;
+	}
+
+	/* Get max device endpoints */
+	/* DEN is bidirectional ep number, max_ep doubles the number */
+	udc->max_ep = (dccparams & DCCPARAMS_DEN_MASK) * 2;
+
+	udc->eps = kzalloc(sizeof(struct tegra_ep) * udc->max_ep, GFP_KERNEL);
+	if (!udc->eps) {
+		ERR("malloc tegra_ep failed\n");
+		return -1;
+	}
+
+	/* Setup hardware queue heads */
+	size = udc->max_ep * sizeof(struct ep_queue_head);
+	udc->ep_qh = (struct ep_queue_head *)((u8 *)(udc->regs) + QH_OFFSET);
+	udc->ep_qh_dma = platform_get_resource(udc->pdev, IORESOURCE_MEM
+				, 0)->start + QH_OFFSET;
+	udc->ep_qh_size = size;
+
+	/* Initialize ep0 status request structure */
+	/* FIXME: tegra_alloc_request() ignores ep argument */
+	udc->status_req = container_of(tegra_alloc_request(NULL, GFP_KERNEL),
+			struct tegra_req, req);
+	/* Allocate a small amount of memory to get valid address */
+	udc->status_req->req.buf = dma_alloc_coherent(&udc->pdev->dev,
+				STATUS_BUFFER_SIZE, &udc->status_req->req.dma,
+				GFP_KERNEL);
+	if (!udc->status_req->req.buf) {
+		ERR("alloc status_req buffer failed\n");
+		kfree(udc->eps);
+		return -ENOMEM;
+	}
+
+	udc->resume_state = USB_STATE_NOTATTACHED;
+	udc->usb_state = USB_STATE_POWERED;
+	udc->ep0_dir = 0;
+	udc->remote_wakeup = 0;	/* default to 0 on reset */
+
+	return 0;
+}
+
+/**
+ * Setup the tegra_ep struct for eps
+ * Link tegra_ep->ep to gadget->ep_list
+ * ep0out is not used so do nothing here
+ * ep0in should be taken care
+ */
+static int __init struct_ep_setup(struct tegra_udc *udc, unsigned char index,
+		char *name, int link)
+{
+	struct tegra_ep *ep = &udc->eps[index];
+
+	ep->udc = udc;
+	strcpy(ep->name, name);
+	ep->ep.name = ep->name;
+
+	ep->ep.ops = &tegra_ep_ops;
+	ep->stopped = 0;
+
+	/* for ep0: maxP defined in desc
+	 * for other eps, maxP is set by epautoconfig() called by gadget layer
+	 */
+	ep->ep.maxpacket = (unsigned short) ~0;
+
+	/* the queue lists any req for this ep */
+	INIT_LIST_HEAD(&ep->queue);
+
+	/* gagdet.ep_list used for ep_autoconfig so no ep0 */
+	if (link)
+		list_add_tail(&ep->ep.ep_list, &udc->gadget.ep_list);
+	ep->gadget = &udc->gadget;
+	ep->qh = &udc->ep_qh[index];
+
+	return 0;
+}
+
+static int tegra_udc_ep_setup(struct tegra_udc *udc)
+{
+	/* initialize EP0 descriptor */
+	static const struct usb_endpoint_descriptor tegra_ep0_desc = {
+		.bLength =		USB_DT_ENDPOINT_SIZE,
+		.bDescriptorType =	USB_DT_ENDPOINT,
+		.bEndpointAddress = 0,
+		.bmAttributes = USB_ENDPOINT_XFER_CONTROL,
+		.wMaxPacketSize =	USB_MAX_CTRL_PAYLOAD,
+	};
+	int i;
+
+	/* setup QH and epctrl for ep0 */
+	ep0_setup(udc);
+
+	/* setup udc->eps[] for ep0 */
+	struct_ep_setup(udc, 0, "ep0", 0);
+	/* for ep0: the desc defined here;
+	 * for other eps, gadget layer called ep_enable with defined desc
+	 */
+	udc->eps[0].desc = &tegra_ep0_desc;
+	udc->eps[0].ep.maxpacket = USB_MAX_CTRL_PAYLOAD;
+
+	/* setup the udc->eps[] for non-control endpoints and link
+	 * to gadget.ep_list */
+	for (i = 1; i < (int)(udc->max_ep / 2); i++) {
+		char name[14];
+
+		sprintf(name, "ep%dout", i);
+		struct_ep_setup(udc, i * 2, name, 1);
+		sprintf(name, "ep%din", i);
+		struct_ep_setup(udc, i * 2 + 1, name, 1);
+	}
+
+	return 0;
+}
+
+
+/* Driver probe function
+ * all intialization operations implemented here except enabling usb_intr reg
+ * board setup should have been done in the platform code
+ */
+static int __init tegra_udc_probe(struct platform_device *pdev)
+{
+	struct tegra_udc *udc;
+	struct resource *res;
+	struct tegra_usb_platform_data *pdata = dev_get_platdata(&pdev->dev);
+	int err = -ENODEV;
+	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
+
+	if (strcmp(pdev->name, driver_name)) {
+		VDBG("Wrong device");
+		return -ENODEV;
+	}
+
+	the_udc = udc = kzalloc(sizeof(struct tegra_udc), GFP_KERNEL);
+	if (udc == NULL) {
+		ERR("malloc udc failed\n");
+		return -ENOMEM;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		err = -ENXIO;
+		ERR("failed to get platform resources\n");
+		goto err_kfree;
+	}
+
+	if (!request_mem_region(res->start, res->end - res->start + 1,
+				driver_name)) {
+		ERR("request mem region failed\n");
+		err = -EBUSY;
+		goto err_kfree;
+	}
+
+	udc->regs = ioremap(res->start, resource_size(res));
+	if (!udc->regs) {
+		err = -ENOMEM;
+		ERR("failed to map mem region\n");
+		goto err_rel_mem_region;
+	}
+
+	udc->irq = platform_get_irq(pdev, 0);
+	if (!udc->irq) {
+		err = -ENODEV;
+		ERR("failed to get platform irq resources\n");
+		goto err_iounmap;
+	}
+
+	err = request_irq(udc->irq, tegra_udc_irq,
+				IRQF_SHARED | IRQF_TRIGGER_HIGH,
+				driver_name, udc);
+	if (err) {
+		ERR("cannot request irq %d err %d\n", udc->irq, err);
+		goto err_iounmap;
+	}
+
+	if (pdata->u_data.dev.remote_wakeup_supported) {
+		err = enable_irq_wake(udc->irq);
+		if (err < 0) {
+			dev_warn(&pdev->dev,
+				"Couldn't enable USB udc mode wakeup, irq=%d,"
+				" error=%d\n", udc->irq, err);
+			err = 0;
+		}
+	}
+
+	udc->phy = tegra_usb_phy_open(pdev);
+	if (IS_ERR(udc->phy)) {
+		dev_err(&pdev->dev, "failed to open USB phy\n");
+		err = -ENXIO;
+		goto err_irq;
+	}
+
+	err = tegra_usb_phy_power_on(udc->phy);
+	if (err) {
+		dev_err(&pdev->dev, "failed to power on the phy\n");
+		goto err_phy;
+	}
+
+	err = tegra_usb_phy_init(udc->phy);
+	if (err) {
+		dev_err(&pdev->dev, "failed to init the phy\n");
+		goto err_phy;
+	}
+	spin_lock_init(&udc->lock);
+	udc->stopped = 1;
+	udc->pdev = pdev;
+	udc->has_hostpc = tegra_usb_phy_has_hostpc(udc->phy) ? 1 : 0;
+	platform_set_drvdata(pdev, udc);
+
+	/* Initialize the udc structure including QH members */
+	err = tegra_udc_setup_qh(udc);
+	if (err) {
+		dev_err(&pdev->dev, "failed to setup udc QH\n");
+		goto err_phy;
+	}
+
+	/* Initialize usb hw reg except for regs for EP,
+	 * leave usbintr reg untouched */
+	err = dr_controller_setup(udc);
+	if (err) {
+		dev_err(&pdev->dev, "failed to setup udc controller\n");
+		goto err_phy;
+	}
+
+	err = tegra_udc_setup_gadget_dev(udc);
+	if (err) {
+		dev_err(&pdev->dev, "failed to setup udc gadget device\n");
+		goto err_phy;
+	}
+
+	err = tegra_udc_ep_setup(udc);
+	if (err) {
+		dev_err(&pdev->dev, "failed to setup end points\n");
+		goto err_unregister;
+	}
+
+	/* Use dma_pool for TD management */
+	udc->td_pool = dma_pool_create("udc_td", &pdev->dev,
+			sizeof(struct ep_td_struct),
+			DTD_ALIGNMENT, UDC_DMA_BOUNDARY);
+	if (!udc->td_pool) {
+		err = -ENOMEM;
+		goto err_unregister;
+	}
+
+	err = usb_add_gadget_udc(&pdev->dev, &udc->gadget);
+	if (err)
+		goto err_del_udc;
+#ifdef CONFIG_TEGRA_GADGET_BOOST_CPU_FREQ
+	boost_cpufreq_work_flag = 1;
+	ep_queue_request_count = 0;
+	INIT_WORK(&udc->boost_cpufreq_work,
+					tegra_udc_boost_cpu_frequency_work);
+	pm_qos_add_request(&boost_cpu_freq_req, PM_QOS_CPU_FREQ_MIN,
+					PM_QOS_DEFAULT_VALUE);
+#endif
+
+	/* Create work for controlling clocks to the phy if otg is disabled */
+	INIT_WORK(&udc->irq_work, tegra_udc_irq_work);
+	/* Create a delayed work for detecting the USB charger */
+	INIT_DELAYED_WORK(&udc->work, tegra_udc_charger_detect_work);
+	INIT_WORK(&udc->charger_work, tegra_udc_set_current_limit_work);
+
+	/* Get the regulator for drawing the vbus current in udc driver */
+	udc->vbus_reg = regulator_get(NULL, "usb_bat_chg");
+	if (IS_ERR(udc->vbus_reg)) {
+		dev_info(&pdev->dev,
+			"usb_bat_chg regulator not registered:"
+				" USB charging will not be enabled\n");
+		udc->vbus_reg = NULL;
+	}
+
+#ifdef CONFIG_USB_OTG_UTILS
+	if (tegra_usb_phy_otg_supported(udc->phy))
+		udc->transceiver = otg_get_transceiver();
+
+	if (udc->transceiver) {
+		dr_controller_stop(udc);
+		dr_controller_reset(udc);
+		tegra_usb_phy_power_off(udc->phy);
+		udc->vbus_active = 0;
+		udc->usb_state = USB_STATE_DEFAULT;
+		otg_set_peripheral(udc->transceiver, &udc->gadget);
+	}
+#else
+	/* Power down the phy if cable is not connected */
+	if (!vbus_enabled(udc))
+		tegra_usb_phy_power_off(udc->phy);
+#endif
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+	return 0;
+
+err_del_udc:
+	dma_pool_destroy(udc->td_pool);
+
+err_unregister:
+	device_unregister(&udc->gadget.dev);
+
+err_phy:
+	tegra_usb_phy_close(udc->phy);
+
+err_irq:
+	free_irq(udc->irq, udc);
+
+err_iounmap:
+	iounmap(udc->regs);
+
+err_rel_mem_region:
+	release_mem_region(res->start, res->end - res->start + 1);
+
+err_kfree:
+	kfree(udc);
+
+	return err;
+}
+
+/* Driver removal function
+ * Free resources and finish pending transactions
+ */
+static int __exit tegra_udc_remove(struct platform_device *pdev)
+{
+	struct tegra_udc *udc = platform_get_drvdata(pdev);
+	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	DECLARE_COMPLETION(done);
+
+	if (!udc)
+		return -ENODEV;
+
+	usb_del_gadget_udc(&udc->gadget);
+	udc->done = &done;
+
+	cancel_delayed_work(&udc->work);
+#ifdef CONFIG_TEGRA_GADGET_BOOST_CPU_FREQ
+	cancel_work_sync(&udc->boost_cpufreq_work);
+#endif
+
+	if (udc->vbus_reg)
+		regulator_put(udc->vbus_reg);
+
+	if (udc->transceiver)
+		otg_set_peripheral(udc->transceiver, NULL);
+
+
+	/* Free allocated memory */
+	dma_free_coherent(&pdev->dev, STATUS_BUFFER_SIZE,
+				udc->status_req->req.buf,
+				udc->status_req->req.dma);
+	kfree(udc->status_req);
+	kfree(udc->eps);
+
+	dma_pool_destroy(udc->td_pool);
+	free_irq(udc->irq, udc);
+	iounmap(udc->regs);
+	release_mem_region(res->start, res->end - res->start + 1);
+
+	device_unregister(&udc->gadget.dev);
+	/* Free udc -- wait for the release() finished */
+	wait_for_completion(&done);
+
+	return 0;
+}
+
+static int tegra_udc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct tegra_udc *udc = platform_get_drvdata(pdev);
+	unsigned long flags;
+	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
+
+	/* If the controller is in otg mode, return */
+	if (udc->transceiver)
+			return 0;
+
+	if (udc->vbus_active) {
+		spin_lock_irqsave(&udc->lock, flags);
+		/* Reset all internal Queues and inform client driver */
+		reset_queues(udc);
+		udc->vbus_active = 0;
+		udc->usb_state = USB_STATE_DEFAULT;
+		spin_unlock_irqrestore(&udc->lock, flags);
+	}
+	/* Stop the controller and turn off the clocks */
+	dr_controller_stop(udc);
+	if (udc->transceiver)
+		udc->transceiver->state = OTG_STATE_UNDEFINED;
+
+	tegra_usb_phy_power_off(udc->phy);
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+	return 0;
+}
+
+static int tegra_udc_resume(struct platform_device *pdev)
+{
+	struct tegra_udc *udc = platform_get_drvdata(pdev);
+	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
+
+	if (udc->transceiver)
+		return 0;
+
+	tegra_usb_phy_power_on(udc->phy);
+	tegra_udc_restart(udc);
+
+	/* Power down the phy if cable is not connected */
+	if (!vbus_enabled(udc)) {
+		udc->vbus_active = 0;
+		tegra_usb_phy_power_off(udc->phy);
+	}
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+	return 0;
+}
+
+
+static struct platform_driver tegra_udc_driver = {
+	.remove  = __exit_p(tegra_udc_remove),
+	.suspend = tegra_udc_suspend,
+	.resume  = tegra_udc_resume,
+	.driver  = {
+		.name = (char *)driver_name,
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init udc_init(void)
+{
+	printk(KERN_INFO "%s (%s)\n", driver_desc, DRIVER_VERSION);
+	return platform_driver_probe(&tegra_udc_driver, tegra_udc_probe);
+}
+module_init(udc_init);
+static void __exit udc_exit(void)
+{
+	platform_driver_unregister(&tegra_udc_driver);
+	printk(KERN_WARNING "%s unregistered\n", driver_desc);
+}
+module_exit(udc_exit);
+
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:tegra-udc");
diff --git a/drivers/usb/gadget/tegra_udc.h b/drivers/usb/gadget/tegra_udc.h
new file mode 100644
index 000000000000..094e3eb0e458
--- /dev/null
+++ b/drivers/usb/gadget/tegra_udc.h
@@ -0,0 +1,457 @@
+/*
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ *
+ * Description:
+ * High-speed USB device controller driver.
+ * USB device/endpoint management registers.
+ * The driver is previously named as fsl_udc_core. Based on Freescale driver
+ * code from Li Yang and Jiang Bo.
+ *
+ * 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.
+ */
+#ifndef __TEGRA_UDC_H
+#define __TEGRA_UDC_H
+
+#ifdef VERBOSE
+#define VDBG(fmt, args...)	printk(KERN_DEBUG "[%s]  " fmt "\n", \
+				__func__, ## args)
+#else
+#define VDBG(fmt, args...)	do {} while (0)
+#endif
+
+
+#ifdef DEBUG
+#define DBG(stuff...)	pr_info("tegra_udc: " stuff)
+#else
+#define DBG(stuff...)	do {} while (0)
+#endif
+
+#define ERR(stuff...)		pr_err("tegra_udc: " stuff)
+#define WARNING(stuff...)	pr_warning("tegra_udc: " stuff)
+
+#define DMA_ADDR_INVALID	(~(dma_addr_t)0)
+#define STATUS_BUFFER_SIZE	8
+
+#define USB_MAX_CTRL_PAYLOAD		64
+
+ /* Charger current limit=1800mA, as per the USB charger spec */
+#define USB_CHARGING_DCP_CURRENT_LIMIT_UA 1800000
+#define USB_CHARGING_CDP_CURRENT_LIMIT_UA 900000
+#define USB_CHARGING_SDP_CURRENT_LIMIT_UA 100000
+#define USB_CHARGING_NON_STANDARD_CHARGER_CURRENT_LIMIT_UA 1800000
+
+ /* 1 sec wait time for charger detection after vbus is detected */
+#define USB_CHARGER_DETECTION_WAIT_TIME_MS 1000
+#define BOOST_TRIGGER_SIZE 4096
+
+#define UDC_RESET_TIMEOUT_MS 1000
+#define UDC_RUN_TIMEOUT_MS 1000
+#define UDC_FLUSH_TIMEOUT_MS 1000
+
+/* ep0 transfer state */
+#define WAIT_FOR_SETUP			0
+#define DATA_STATE_XMIT			1
+#define DATA_STATE_NEED_ZLP		2
+#define WAIT_FOR_OUT_STATUS		3
+#define DATA_STATE_RECV			4
+
+/*
+ * ### pipe direction macro from device view
+ */
+#define USB_RECV	0	/* OUT EP */
+#define USB_SEND	1	/* IN EP */
+
+/* Device Controller Capability Parameter register */
+#define DCCPARAMS_REG_OFFSET		0x124
+#define DCCPARAMS_DC				0x00000080
+#define DCCPARAMS_DEN_MASK			0x0000001f
+
+/* USB CMD  Register Bit Masks */
+#define USB_CMD_REG_OFFSET		((udc->has_hostpc) ? 0x130 : 0x140)
+#define  USB_CMD_RUN_STOP		      0x00000001
+#define  USB_CMD_CTRL_RESET                   0x00000002
+#define  USB_CMD_PERIODIC_SCHEDULE_EN	      0x00000010
+#define  USB_CMD_ASYNC_SCHEDULE_EN            0x00000020
+#define  USB_CMD_INT_AA_DOORBELL              0x00000040
+#define  USB_CMD_ASP                          0x00000300
+#define  USB_CMD_ASYNC_SCH_PARK_EN            0x00000800
+#define  USB_CMD_SUTW                         0x00002000
+#define  USB_CMD_ATDTW                        0x00004000
+#define  USB_CMD_ITC                          0x00FF0000
+/* bit 15,3,2 are frame list size */
+#define  USB_CMD_FRAME_SIZE_1024              0x00000000
+#define  USB_CMD_FRAME_SIZE_512               0x00000004
+#define  USB_CMD_FRAME_SIZE_256               0x00000008
+#define  USB_CMD_FRAME_SIZE_128               0x0000000C
+#define  USB_CMD_FRAME_SIZE_64                0x00008000
+#define  USB_CMD_FRAME_SIZE_32                0x00008004
+#define  USB_CMD_FRAME_SIZE_16                0x00008008
+#define  USB_CMD_FRAME_SIZE_8                 0x0000800C
+/* bit 9-8 are async schedule park mode count */
+#define  USB_CMD_ASP_00                       0x00000000
+#define  USB_CMD_ASP_01                       0x00000100
+#define  USB_CMD_ASP_10                       0x00000200
+#define  USB_CMD_ASP_11                       0x00000300
+#define  USB_CMD_ASP_BIT_POS                  8
+/* bit 23-16 are interrupt threshold control */
+#define  USB_CMD_ITC_NO_THRESHOLD             0x00000000
+#define  USB_CMD_ITC_1_MICRO_FRM              0x00010000
+#define  USB_CMD_ITC_2_MICRO_FRM              0x00020000
+#define  USB_CMD_ITC_4_MICRO_FRM              0x00040000
+#define  USB_CMD_ITC_8_MICRO_FRM              0x00080000
+#define  USB_CMD_ITC_16_MICRO_FRM             0x00100000
+#define  USB_CMD_ITC_32_MICRO_FRM             0x00200000
+#define  USB_CMD_ITC_64_MICRO_FRM             0x00400000
+#define  USB_CMD_ITC_BIT_POS                  16
+
+/* USB STS Register Bit Masks */
+#define USB_STS_REG_OFFSET		((udc->has_hostpc) ? 0x134 : 0x144)
+#define  USB_STS_INT                          0x00000001
+#define  USB_STS_ERR                          0x00000002
+#define  USB_STS_PORT_CHANGE                  0x00000004
+#define  USB_STS_FRM_LST_ROLL                 0x00000008
+#define  USB_STS_SYS_ERR                      0x00000010
+#define  USB_STS_IAA                          0x00000020
+#define  USB_STS_RESET                        0x00000040
+#define  USB_STS_SOF                          0x00000080
+#define  USB_STS_SUSPEND                      0x00000100
+#define  USB_STS_HC_HALTED                    0x00001000
+#define  USB_STS_RCL                          0x00002000
+#define  USB_STS_PERIODIC_SCHEDULE            0x00004000
+#define  USB_STS_ASYNC_SCHEDULE               0x00008000
+
+/* USB INTR Register Bit Masks */
+#define USB_INTR_REG_OFFSET		((udc->has_hostpc) ? 0x138 : 0x148)
+#define  USB_INTR_INT_EN                      0x00000001
+#define  USB_INTR_ERR_INT_EN                  0x00000002
+#define  USB_INTR_PTC_DETECT_EN               0x00000004
+#define  USB_INTR_FRM_LST_ROLL_EN             0x00000008
+#define  USB_INTR_SYS_ERR_EN                  0x00000010
+#define  USB_INTR_ASYN_ADV_EN                 0x00000020
+#define  USB_INTR_RESET_EN                    0x00000040
+#define  USB_INTR_SOF_EN                      0x00000080
+#define  USB_INTR_DEVICE_SUSPEND              0x00000100
+
+/* Frame Index Register Bit Masks */
+#define USB_FRINDEX_REG_OFFSET		((udc->has_hostpc) ? 0x13c : 0x14c)
+#define  USB_FRINDEX_MASKS			0x3fff
+
+/* Device Address bit masks */
+#define USB_DEVICE_ADDR_REG_OFFSET	((udc->has_hostpc) ? 0x144 : 0x154)
+#define  USB_DEVICE_ADDRESS_MASK              0xFE000000
+#define  USB_DEVICE_ADDRESS_BIT_POS           25
+
+/* endpoint list address bit masks */
+#define USB_EP_LIST_ADDRESS_REG_OFFSET	((udc->has_hostpc) ? 0x148 : 0x158)
+#define  USB_EP_LIST_ADDRESS_MASK			0xfffff800
+
+/* PORTSCX  Register Bit Masks */
+#define PORTSCX_REG_OFFSET		((udc->has_hostpc) ? 0x174 : 0x184)
+#define  PORTSCX_CURRENT_CONNECT_STATUS       0x00000001
+#define  PORTSCX_CONNECT_STATUS_CHANGE        0x00000002
+#define  PORTSCX_PORT_ENABLE                  0x00000004
+#define  PORTSCX_PORT_EN_DIS_CHANGE           0x00000008
+#define  PORTSCX_OVER_CURRENT_ACT             0x00000010
+#define  PORTSCX_OVER_CURRENT_CHG             0x00000020
+#define  PORTSCX_PORT_FORCE_RESUME            0x00000040
+#define  PORTSCX_PORT_SUSPEND                 0x00000080
+#define  PORTSCX_PORT_RESET                   0x00000100
+#define  PORTSCX_LINE_STATUS_BITS             0x00000C00
+#define  PORTSCX_LINE_STATUS_DP_BIT           0x00000800
+#define  PORTSCX_LINE_STATUS_DM_BIT           0x00000400
+#define  PORTSCX_PORT_POWER                   0x00001000
+#define  PORTSCX_PORT_INDICTOR_CTRL           0x0000C000
+#define  PORTSCX_PORT_TEST_CTRL               0x000F0000
+#define  PORTSCX_WAKE_ON_CONNECT_EN           0x00100000
+#define  PORTSCX_WAKE_ON_CONNECT_DIS          0x00200000
+#define  PORTSCX_WAKE_ON_OVER_CURRENT         0x00400000
+#define  PORTSCX_PHY_LOW_POWER_SPD            0x00800000
+
+/* In tegra3 the following fields have moved to new HOSTPC1_DEVLC reg and
+ * their offsets have changed.
+ * Keeping the name of bit masks same as before (PORTSCX_*) to have
+ * minimum changes to code */
+#define USB_HOSTPCX_DEVLC_REG_OFFSET			0x1b4
+
+#define  PORTSCX_PORT_FORCE_FULL_SPEED ((udc->has_hostpc) ? 0x00800000 \
+						: 0x01000000)
+#define  PORTSCX_PORT_SPEED_MASK  ((udc->has_hostpc) ? 0x06000000 : 0x0C000000)
+#define  PORTSCX_PORT_WIDTH	  ((udc->has_hostpc) ? 0x08000000 : 0x10000000)
+#define  PORTSCX_PHY_TYPE_SEL	  ((udc->has_hostpc) ? 0xE0000000 : 0xC0000000)
+
+/* bits for port speed */
+#define  PORTSCX_PORT_SPEED_FULL  ((udc->has_hostpc) ? 0x00000000 : 0x00000000)
+#define  PORTSCX_PORT_SPEED_LOW	  ((udc->has_hostpc) ? 0x02000000 : 0x04000000)
+#define  PORTSCX_PORT_SPEED_HIGH  ((udc->has_hostpc) ? 0x04000000 : 0x08000000)
+#define  PORTSCX_PORT_SPEED_UNDEF ((udc->has_hostpc) ? 0x06000000 : 0x0C000000)
+#define  PORTSCX_SPEED_BIT_POS	  ((udc->has_hostpc) ? 25 : 26)
+
+/* bits for parallel transceiver width for UTMI interface */
+#define  PORTSCX_PTW		((udc->has_hostpc) ? 0x08000000 : 0x10000000)
+#define  PORTSCX_PTW_8BIT	((udc->has_hostpc) ? 0x00000000 : 0x00000000)
+#define  PORTSCX_PTW_16BIT	((udc->has_hostpc) ? 0x08000000 : 0x10000000)
+
+/* bits for port transceiver select */
+#define  PORTSCX_PTS_UTMI	((udc->has_hostpc) ? 0x00000000 : 0x00000000)
+#define  PORTSCX_PTS_ULPI	((udc->has_hostpc) ? 0x40000000 : 0x80000000)
+#define  PORTSCX_PTS_FSLS	((udc->has_hostpc) ? 0x60000000 : 0xC0000000)
+#define  PORTSCX_PTS_BIT_POS		((udc->has_hostpc) ? 29 : 30)
+
+/* bit 11-10 are line status */
+#define  PORTSCX_LINE_STATUS_SE0              0x00000000
+#define  PORTSCX_LINE_STATUS_JSTATE           0x00000400
+#define  PORTSCX_LINE_STATUS_KSTATE           0x00000800
+#define  PORTSCX_LINE_STATUS_UNDEF            0x00000C00
+#define  PORTSCX_LINE_STATUS_BIT_POS          10
+
+/* bit 15-14 are port indicator control */
+#define  PORTSCX_PIC_OFF                      0x00000000
+#define  PORTSCX_PIC_AMBER                    0x00004000
+#define  PORTSCX_PIC_GREEN                    0x00008000
+#define  PORTSCX_PIC_UNDEF                    0x0000C000
+#define  PORTSCX_PIC_BIT_POS                  14
+
+/* bit 19-16 are port test control */
+#define  PORTSCX_PTC_DISABLE                  0x00000000
+#define  PORTSCX_PTC_JSTATE                   0x00010000
+#define  PORTSCX_PTC_KSTATE                   0x00020000
+#define  PORTSCX_PTC_SEQNAK                   0x00030000
+#define  PORTSCX_PTC_PACKET                   0x00040000
+#define  PORTSCX_PTC_FORCE_EN                 0x00050000
+#define  PORTSCX_PTC_BIT_POS                  16
+
+
+/* USB MODE Register Bit Masks */
+#define USB_MODE_REG_OFFSET		((udc->has_hostpc) ? 0x1f8 : 0x1a8)
+#define  USB_MODE_CTRL_MODE_IDLE		0x00000000
+#define  USB_MODE_CTRL_MODE_DEVICE		0x00000002
+#define  USB_MODE_CTRL_MODE_HOST		0x00000003
+#define  USB_MODE_CTRL_MODE_RSV			0x00000001
+#define  USB_MODE_SETUP_LOCK_OFF		0x00000008
+#define  USB_MODE_STREAM_DISABLE		0x00000010
+
+/* Endpoint Setup Status bit masks */
+#define EP_SETUP_STATUS_REG_OFFSET	((udc->has_hostpc) ? 0x208 : 0x1ac)
+#define  EP_SETUP_STATUS_MASK			0x0000003F
+#define  EP_SETUP_STATUS_EP0			0x00000001
+
+/* Endpoint Prime Register */
+#define EP_PRIME_REG_OFFSET		((udc->has_hostpc) ? 0x20c : 0x1b0)
+
+/* Endpoint Flush Register */
+#define EPFLUSH_REG_OFFSET		((udc->has_hostpc) ? 0x210 : 0x1b4)
+#define  EPFLUSH_TX_OFFSET		0x00010000
+#define  EPFLUSH_RX_OFFSET		0x00000000
+
+/* Endpoint Status Register */
+#define EP_STATUS_REG_OFFSET		((udc->has_hostpc) ? 0x214 : 0x1b8)
+
+/* Endpoint Complete Register */
+#define EP_COMPLETE_REG_OFFSET		((udc->has_hostpc) ? 0x218 : 0x1bc)
+
+/* Endpoint Control Registers */
+#define EP_CONTROL_REG_OFFSET		((udc->has_hostpc) ? 0x21c : 0x1c0)
+
+/* ENDPOINTCTRLx  Register Bit Masks */
+#define  EPCTRL_TX_ENABLE                     0x00800000
+#define  EPCTRL_TX_DATA_TOGGLE_RST            0x00400000	/* Not EP0 */
+#define  EPCTRL_TX_DATA_TOGGLE_INH            0x00200000	/* Not EP0 */
+#define  EPCTRL_TX_TYPE                       0x000C0000
+#define  EPCTRL_TX_DATA_SOURCE                0x00020000	/* Not EP0 */
+#define  EPCTRL_TX_EP_STALL                   0x00010000
+#define  EPCTRL_RX_ENABLE                     0x00000080
+#define  EPCTRL_RX_DATA_TOGGLE_RST            0x00000040	/* Not EP0 */
+#define  EPCTRL_RX_DATA_TOGGLE_INH            0x00000020	/* Not EP0 */
+#define  EPCTRL_RX_TYPE                       0x0000000C
+#define  EPCTRL_RX_DATA_SINK                  0x00000002	/* Not EP0 */
+#define  EPCTRL_RX_EP_STALL                   0x00000001
+
+/* bit 19-18 and 3-2 are endpoint type */
+#define  EPCTRL_EP_TYPE_CONTROL               0
+#define  EPCTRL_EP_TYPE_ISO                   1
+#define  EPCTRL_EP_TYPE_BULK                  2
+#define  EPCTRL_EP_TYPE_INTERRUPT             3
+#define  EPCTRL_TX_EP_TYPE_SHIFT              18
+#define  EPCTRL_RX_EP_TYPE_SHIFT              2
+
+#define VBUS_SENSOR_REG_OFFSET			0x404
+#define VBUS_WAKEUP_REG_OFFSET			0x408
+
+#define  USB_SYS_VBUS_ASESSION_INT_EN		0x10000
+#define  USB_SYS_VBUS_ASESSION_CHANGED		0x20000
+#define  USB_SYS_VBUS_ASESSION			0x40000
+#define  USB_SYS_VBUS_WAKEUP_ENABLE		0x40000000
+#define  USB_SYS_VBUS_WAKEUP_INT_ENABLE		0x100
+#define  USB_SYS_VBUS_WAKEUP_INT_STATUS		0x200
+#define  USB_SYS_VBUS_STATUS			0x400
+#define  USB_SYS_ID_PIN_STATUS			0x4
+
+
+/* Endpoint Queue Head Bit Masks */
+#define  EP_QUEUE_HEAD_MULT_POS               30
+#define  EP_QUEUE_HEAD_ZLT_SEL                0x20000000
+#define  EP_QUEUE_HEAD_MAX_PKT_LEN_POS        16
+#define  EP_QUEUE_HEAD_MAX_PKT_LEN(ep_info)   (((ep_info)>>16)&0x07ff)
+#define  EP_QUEUE_HEAD_IOS                    0x00008000
+#define  EP_QUEUE_HEAD_NEXT_TERMINATE         0x00000001
+#define  EP_QUEUE_HEAD_IOC                    0x00008000
+#define  EP_QUEUE_HEAD_MULTO                  0x00000C00
+#define  EP_QUEUE_HEAD_STATUS_HALT	      0x00000040
+#define  EP_QUEUE_HEAD_STATUS_ACTIVE          0x00000080
+#define  EP_QUEUE_CURRENT_OFFSET_MASK         0x00000FFF
+#define  EP_QUEUE_HEAD_NEXT_POINTER_MASK      0xFFFFFFE0
+#define  EP_QUEUE_FRINDEX_MASK                0x000007FF
+#define  EP_MAX_LENGTH_TRANSFER               0x4000
+
+
+
+/* Endpoint Transfer Descriptor bit Masks */
+#define  DTD_NEXT_TERMINATE                   0x00000001
+#define  DTD_IOC                              0x00008000
+#define  DTD_STATUS_ACTIVE                    0x00000080
+#define  DTD_STATUS_HALTED                    0x00000040
+#define  DTD_STATUS_DATA_BUFF_ERR             0x00000020
+#define  DTD_STATUS_TRANSACTION_ERR           0x00000008
+#define  DTD_RESERVED_FIELDS                  0x80007300
+#define  DTD_ADDR_MASK                        0xFFFFFFE0
+#define  DTD_PACKET_SIZE                      0x7FFF0000
+#define  DTD_LENGTH_BIT_POS                   16
+#define  DTD_ERROR_MASK                       (DTD_STATUS_HALTED | \
+						DTD_STATUS_DATA_BUFF_ERR | \
+						DTD_STATUS_TRANSACTION_ERR)
+/* Alignment requirements; must be a power of two */
+#define DTD_ALIGNMENT				0x80
+#define QH_ALIGNMENT				2048
+#define QH_OFFSET				0x1000
+
+/* Controller dma boundary */
+#define UDC_DMA_BOUNDARY			0x1000
+
+#define REQ_UNCOMPLETE			1
+
+#define EP_DIR_IN	1
+#define EP_DIR_OUT	0
+
+/*
+ * Endpoint Queue Head data struct
+ * Rem: all the variables of qh are LittleEndian Mode
+ * and NEXT_POINTER_MASK should operate on a LittleEndian, Phy Addr
+ */
+struct ep_queue_head {
+	u32 max_pkt_length; /* Mult(31-30), Zlt(29), Max Pkt len and IOS(15) */
+	u32 curr_dtd_ptr;	/* Current dTD Pointer(31-5) */
+	u32 next_dtd_ptr;	/* Next dTD Pointer(31-5), T(0) */
+	u32 size_ioc_int_sts;	/* Total bytes (30-16), IOC (15),
+				   MultO(11-10), STS (7-0)	*/
+	u32 buff_ptr0;		/* Buffer pointer Page 0 (31-12) */
+	u32 buff_ptr1;		/* Buffer pointer Page 1 (31-12) */
+	u32 buff_ptr2;		/* Buffer pointer Page 2 (31-12) */
+	u32 buff_ptr3;		/* Buffer pointer Page 3 (31-12) */
+	u32 buff_ptr4;		/* Buffer pointer Page 4 (31-12) */
+	u32 res1;
+	u8 setup_buffer[8]; /* Setup data 8 bytes */
+	u32 res2[4];
+};
+
+/* Endpoint Transfer Descriptor data struct */
+/* Rem: all the variables of td are LittleEndian Mode */
+struct ep_td_struct {
+	u32 next_td_ptr;	/* Next TD pointer(31-5), T(0) set
+				   indicate invalid */
+	u32 size_ioc_sts;	/* Total bytes (30-16), IOC (15),
+				   MultO(11-10), STS (7-0)	*/
+	u32 buff_ptr0;		/* Buffer pointer Page 0 */
+	u32 buff_ptr1;		/* Buffer pointer Page 1 */
+	u32 buff_ptr2;		/* Buffer pointer Page 2 */
+	u32 buff_ptr3;		/* Buffer pointer Page 3 */
+	u32 buff_ptr4;		/* Buffer pointer Page 4 */
+	u32 res;
+	/* 32 bytes */
+	dma_addr_t td_dma;	/* dma address for this td */
+	/* virtual address of next td specified in next_td_ptr */
+	struct ep_td_struct *next_td_virt;
+};
+
+
+struct tegra_req {
+	struct usb_request req;
+	struct list_head queue;
+	/* ep_queue() func will add
+	   a request->queue into a udc_ep->queue 'd tail */
+	struct tegra_ep *ep;
+	unsigned mapped:1;
+
+	struct ep_td_struct *head, *tail;	/* For dTD List
+						   cpu endian Virtual addr */
+	unsigned int dtd_count;
+};
+
+struct tegra_ep {
+	struct usb_ep ep;
+	struct list_head queue;
+	struct tegra_udc *udc;
+	struct ep_queue_head *qh;
+	const struct usb_endpoint_descriptor *desc;
+	struct usb_gadget *gadget;
+	struct ep_td_struct *last_td;
+	int last_dtd_count;
+
+	char name[14];
+	unsigned stopped:1;
+};
+
+enum tegra_connect_type {
+	CONNECT_TYPE_NONE,
+	CONNECT_TYPE_SDP,
+	CONNECT_TYPE_DCP,
+	CONNECT_TYPE_CDP,
+	CONNECT_TYPE_NON_STANDARD_CHARGER
+};
+
+struct tegra_udc {
+	struct usb_gadget gadget;
+	struct usb_gadget_driver *driver;
+	struct completion *done;	/* to make sure release() is done */
+	struct tegra_ep *eps;
+	struct platform_device *pdev;
+	struct tegra_usb_phy *phy;
+	struct usb_ctrlrequest local_setup_buff;
+	struct otg_transceiver *transceiver;
+	struct ep_queue_head *ep_qh;	/* Endpoints Queue-Head */
+	struct tegra_req *status_req;	/* ep0 status request */
+	struct dma_pool *td_pool;	/* dma pool for DTD */
+	struct delayed_work work;       /* delayed work for charger detection */
+	struct regulator *vbus_reg;	/* regulator for drawing VBUS */
+	/* work for setting regulator current limit */
+	struct work_struct charger_work;
+	/* work for boosting cpu frequency */
+	struct work_struct boost_cpufreq_work;
+	/* irq work for controlling the usb power */
+	struct work_struct irq_work;
+	enum tegra_connect_type connect_type;
+	void __iomem *regs;
+	size_t ep_qh_size;		/* size after alignment adjustment*/
+	dma_addr_t ep_qh_dma;		/* dma address of QH */
+	unsigned int max_ep;
+	unsigned int irq;
+	u32 max_pipes;		/* Device max pipes */
+	u32 resume_state;	/* USB state to resume */
+	u32 usb_state;		/* USB current state */
+	u32 ep0_state;		/* Endpoint zero state */
+	u32 ep0_dir;	/* Endpoint zero direction: USB_DIR_IN/USB_DIR_OUT */
+	u8 device_address;	/* Device USB address */
+	u32 current_limit;
+	spinlock_t lock;
+	unsigned softconnect:1;
+	unsigned vbus_active:1;
+	unsigned stopped:1;
+	unsigned remote_wakeup:1;
+	unsigned selfpowered:1;
+	bool has_hostpc;
+};
+
+
+#endif /* __TEGRA_UDC_H */
diff --git a/drivers/usb/gadget/u_ether.c b/drivers/usb/gadget/u_ether.c
index dfed4c1d96c0..4f0f71116cfe 100644
--- a/drivers/usb/gadget/u_ether.c
+++ b/drivers/usb/gadget/u_ether.c
@@ -767,6 +767,26 @@ static struct device_type gadget_type = {
  */
 int gether_setup(struct usb_gadget *g, u8 ethaddr[ETH_ALEN])
 {
+	return gether_setup_name(g, ethaddr, "usb");
+}
+
+/**
+ * gether_setup_name - initialize one ethernet-over-usb link
+ * @g: gadget to associated with these links
+ * @ethaddr: NULL, or a buffer in which the ethernet address of the
+ *	host side of the link is recorded
+ * @netname: name for network device (for example, "usb")
+ * Context: may sleep
+ *
+ * This sets up the single network link that may be exported by a
+ * gadget driver using this framework.  The link layer addresses are
+ * set up using module parameters.
+ *
+ * Returns negative errno, or zero on success
+ */
+int gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
+		const char *netname)
+{
 	struct eth_dev		*dev;
 	struct net_device	*net;
 	int			status;
@@ -789,7 +809,7 @@ int gether_setup(struct usb_gadget *g, u8 ethaddr[ETH_ALEN])
 
 	/* network device setup */
 	dev->net = net;
-	strcpy(net->name, "usb%d");
+	snprintf(net->name, sizeof(net->name), "%s%%d", netname);
 
 	if (get_ether_addr(dev_addr, net->dev_addr))
 		dev_warn(&g->dev,
@@ -945,7 +965,6 @@ void gether_disconnect(struct gether *link)
 	struct eth_dev		*dev = link->ioport;
 	struct usb_request	*req;
 
-	WARN_ON(!dev);
 	if (!dev)
 		return;
 
diff --git a/drivers/usb/gadget/u_ether.h b/drivers/usb/gadget/u_ether.h
index c966440ddd70..c32227ebac0b 100644
--- a/drivers/usb/gadget/u_ether.h
+++ b/drivers/usb/gadget/u_ether.h
@@ -82,6 +82,9 @@ struct gether {
 /* netdev setup/teardown as directed by the gadget driver */
 int gether_setup(struct usb_gadget *g, u8 ethaddr[ETH_ALEN]);
 void gether_cleanup(void);
+/* variant of gether_setup that allows customizing network device name */
+int gether_setup_name(struct usb_gadget *g, u8 ethaddr[ETH_ALEN],
+		const char *netname);
 
 /* connect/disconnect is handled by individual functions */
 struct net_device *gether_connect(struct gether *);
@@ -108,16 +111,37 @@ int eem_bind_config(struct usb_configuration *c);
 
 #ifdef USB_ETH_RNDIS
 
-int rndis_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN]);
+int rndis_bind_config_vendor(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
+				u32 vendorID, const char *manufacturer);
 
 #else
 
 static inline int
-rndis_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN])
+rndis_bind_config_vendor(struct usb_configuration *c, u8 ethaddr[ETH_ALEN],
+				u32 vendorID, const char *manufacturer)
 {
 	return 0;
 }
 
 #endif
 
+/**
+ * rndis_bind_config - add RNDIS network link to a configuration
+ * @c: the configuration to support the network link
+ * @ethaddr: a buffer in which the ethernet address of the host side
+ *	side of the link was recorded
+ * Context: single threaded during gadget setup
+ *
+ * Returns zero on success, else negative errno.
+ *
+ * Caller must have called @gether_setup().  Caller is also responsible
+ * for calling @gether_cleanup() before module unload.
+ */
+static inline int rndis_bind_config(struct usb_configuration *c,
+				    u8 ethaddr[ETH_ALEN])
+{
+	return rndis_bind_config_vendor(c, ethaddr, 0, NULL);
+}
+
+
 #endif /* __U_ETHER_H */
diff --git a/drivers/usb/gadget/u_serial.c b/drivers/usb/gadget/u_serial.c
index a8aa46962d81..ac0ef4b7deeb 100644
--- a/drivers/usb/gadget/u_serial.c
+++ b/drivers/usb/gadget/u_serial.c
@@ -122,7 +122,7 @@ struct gs_port {
 };
 
 /* increase N_PORTS if you need more */
-#define N_PORTS		4
+#define N_PORTS		8
 static struct portmaster {
 	struct mutex	lock;			/* protect open/close */
 	struct gs_port	*port;
@@ -1028,7 +1028,7 @@ static const struct tty_operations gs_tty_ops = {
 
 static struct tty_driver *gs_tty_driver;
 
-static int __init
+static int
 gs_port_alloc(unsigned port_num, struct usb_cdc_line_coding *coding)
 {
 	struct gs_port	*port;
@@ -1074,7 +1074,7 @@ gs_port_alloc(unsigned port_num, struct usb_cdc_line_coding *coding)
  *
  * Returns negative errno or zero.
  */
-int __init gserial_setup(struct usb_gadget *g, unsigned count)
+int gserial_setup(struct usb_gadget *g, unsigned count)
 {
 	unsigned			i;
 	struct usb_cdc_line_coding	coding;
diff --git a/drivers/usb/gadget/udc-core.c b/drivers/usb/gadget/udc-core.c
index 05ba47214361..df5ad21f2379 100644
--- a/drivers/usb/gadget/udc-core.c
+++ b/drivers/usb/gadget/udc-core.c
@@ -344,7 +344,7 @@ EXPORT_SYMBOL_GPL(usb_gadget_unregister_driver);
 static ssize_t usb_udc_srp_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t n)
 {
-	struct usb_udc		*udc = dev_get_drvdata(dev);
+	struct usb_udc		*udc =  container_of(dev, struct usb_udc, dev);
 
 	if (sysfs_streq(buf, "1"))
 		usb_gadget_wakeup(udc->gadget);
@@ -356,7 +356,7 @@ static DEVICE_ATTR(srp, S_IWUSR, NULL, usb_udc_srp_store);
 static ssize_t usb_udc_softconn_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t n)
 {
-	struct usb_udc		*udc = dev_get_drvdata(dev);
+	struct usb_udc		*udc =  container_of(dev, struct usb_udc, dev);
 
 	if (sysfs_streq(buf, "connect")) {
 		usb_gadget_connect(udc->gadget);
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index ab085f12d570..9179076801c7 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -578,3 +578,9 @@ config USB_OCTEON_OHCI
 config USB_OCTEON2_COMMON
 	bool
 	default y if USB_OCTEON_EHCI || USB_OCTEON_OHCI
+
+config USB_EHCI_ONOFF_FEATURE
+        boolean "EHCI ON/OFF Feature"
+        depends on USB && USB_EHCI_HCD
+        help
+          Enable support for turning EHCI Off and On
diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index 624a362f2fee..ea6d2e8f6596 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -1,6 +1,7 @@
 #
 # Makefile for USB Host Controller Drivers
 #
+GCOV_PROFILE := y
 
 ccflags-$(CONFIG_USB_DEBUG) := -DDEBUG
 
diff --git a/drivers/usb/host/ehci-dbg.c b/drivers/usb/host/ehci-dbg.c
index 40a844c1dbb4..3e2ccb0dd255 100644
--- a/drivers/usb/host/ehci-dbg.c
+++ b/drivers/usb/host/ehci-dbg.c
@@ -808,7 +808,7 @@ static ssize_t fill_registers_buffer(struct debug_buffer *buf)
 	next += temp;
 
 	temp = scnprintf (next, size, "uframe %04x\n",
-			ehci_readl(ehci, &ehci->regs->frame_index));
+			ehci_read_frame_index(ehci));
 	size -= temp;
 	next += temp;
 
diff --git a/drivers/usb/host/ehci-fsl.c b/drivers/usb/host/ehci-fsl.c
index 34a3140d1e5f..53ca216657d9 100644
--- a/drivers/usb/host/ehci-fsl.c
+++ b/drivers/usb/host/ehci-fsl.c
@@ -117,15 +117,12 @@ static int usb_hcd_fsl_probe(const struct hc_driver *driver,
 
 	pdata->regs = hcd->regs;
 
-	if (pdata->power_budget)
-		hcd->power_budget = pdata->power_budget;
-
 	/*
 	 * do platform specific init: check the clock, grab/config pins, etc.
 	 */
 	if (pdata->init && pdata->init(pdev)) {
 		retval = -ENODEV;
-		goto err3;
+		goto err4;
 	}
 
 	/* Enable USB controller, 83xx or 8536 */
@@ -137,30 +134,6 @@ static int usb_hcd_fsl_probe(const struct hc_driver *driver,
 	retval = usb_add_hcd(hcd, irq, IRQF_DISABLED | IRQF_SHARED);
 	if (retval != 0)
 		goto err4;
-
-#ifdef CONFIG_USB_OTG
-	if (pdata->operating_mode == FSL_USB2_DR_OTG) {
-		struct ehci_hcd *ehci = hcd_to_ehci(hcd);
-
-		ehci->transceiver = otg_get_transceiver();
-		dev_dbg(&pdev->dev, "hcd=0x%p  ehci=0x%p, transceiver=0x%p\n",
-			hcd, ehci, ehci->transceiver);
-
-		if (ehci->transceiver) {
-			retval = otg_set_host(ehci->transceiver,
-					      &ehci_to_hcd(ehci)->self);
-			if (retval) {
-				if (ehci->transceiver)
-					put_device(ehci->transceiver->dev);
-				goto err4;
-			}
-		} else {
-			dev_err(&pdev->dev, "can't find transceiver\n");
-			retval = -ENODEV;
-			goto err4;
-		}
-	}
-#endif
 	return retval;
 
       err4:
@@ -191,12 +164,6 @@ static void usb_hcd_fsl_remove(struct usb_hcd *hcd,
 			       struct platform_device *pdev)
 {
 	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
-	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
-
-	if (ehci->transceiver) {
-		otg_set_host(ehci->transceiver, NULL);
-		put_device(ehci->transceiver->dev);
-	}
 
 	usb_remove_hcd(hcd);
 
@@ -577,38 +544,6 @@ static struct dev_pm_ops ehci_fsl_pm_ops = {
 #define EHCI_FSL_PM_OPS		NULL
 #endif /* CONFIG_PM */
 
-#ifdef CONFIG_USB_OTG
-static int ehci_start_port_reset(struct usb_hcd *hcd, unsigned port)
-{
-	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
-	u32 status;
-
-	if (!port)
-		return -EINVAL;
-
-	port--;
-
-	/* start port reset before HNP protocol time out */
-	status = readl(&ehci->regs->port_status[port]);
-	if (!(status & PORT_CONNECT))
-		return -ENODEV;
-
-	/* khubd will finish the reset later */
-	if (ehci_is_TDI(ehci)) {
-		writel(PORT_RESET |
-		       (status & ~(PORT_CSC | PORT_PEC | PORT_OCC)),
-		       &ehci->regs->port_status[port]);
-	} else {
-		writel(PORT_RESET, &ehci->regs->port_status[port]);
-	}
-
-	return 0;
-}
-#else
-#define ehci_start_port_reset	NULL
-#endif /* CONFIG_USB_OTG */
-
-
 static const struct hc_driver ehci_fsl_hc_driver = {
 	.description = hcd_name,
 	.product_desc = "Freescale On-Chip EHCI Host Controller",
@@ -648,7 +583,6 @@ static const struct hc_driver ehci_fsl_hc_driver = {
 	.hub_control = ehci_hub_control,
 	.bus_suspend = ehci_bus_suspend,
 	.bus_resume = ehci_bus_resume,
-	.start_port_reset = ehci_start_port_reset,
 	.relinquish_port = ehci_relinquish_port,
 	.port_handed_over = ehci_port_handed_over,
 
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index f72ae0b6ee7f..1a534c70ea6e 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -203,7 +203,10 @@ static int tdi_in_host_mode (struct ehci_hcd *ehci)
 	u32 __iomem	*reg_ptr;
 	u32		tmp;
 
-	reg_ptr = (u32 __iomem *)(((u8 __iomem *)ehci->regs) + USBMODE);
+	if (ehci->has_hostpc)
+		reg_ptr = (u32 __iomem *)(((u8 __iomem *)ehci->regs) + USBMODE_EX);
+	else
+		reg_ptr = (u32 __iomem *)(((u8 __iomem *)ehci->regs) + USBMODE);
 	tmp = ehci_readl(ehci, reg_ptr);
 	return (tmp & 3) == USBMODE_CM_HC;
 }
@@ -277,7 +280,10 @@ static int ehci_reset (struct ehci_hcd *ehci)
 
 	command |= CMD_RESET;
 	dbg_cmd (ehci, "reset", command);
-	ehci_writel(ehci, command, &ehci->regs->command);
+#ifdef CONFIG_USB_EHCI_TEGRA
+	if (!ehci->controller_resets_phy)
+#endif
+		ehci_writel(ehci, command, &ehci->regs->command);
 	ehci_to_hcd(ehci)->state = HC_STATE_HALT;
 	ehci->next_statechange = jiffies;
 	retval = handshake (ehci, &ehci->regs->command,
@@ -768,6 +774,35 @@ static int ehci_run (struct usb_hcd *hcd)
 	return 0;
 }
 
+static int __maybe_unused ehci_setup (struct usb_hcd *hcd)
+{
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	int retval;
+
+	ehci->regs = (void __iomem *)ehci->caps +
+	    HC_LENGTH(ehci, ehci_readl(ehci, &ehci->caps->hc_capbase));
+	dbg_hcs_params(ehci, "reset");
+	dbg_hcc_params(ehci, "reset");
+
+	/* cache this readonly data; minimize chip reads */
+	ehci->hcs_params = ehci_readl(ehci, &ehci->caps->hcs_params);
+
+	ehci->sbrn = HCD_USB2;
+
+	retval = ehci_halt(ehci);
+	if (retval)
+		return retval;
+
+	/* data structure init */
+	retval = ehci_init(hcd);
+	if (retval)
+		return retval;
+
+	ehci_reset(ehci);
+
+	return 0;
+}
+
 /*-------------------------------------------------------------------------*/
 
 static irqreturn_t ehci_irq (struct usb_hcd *hcd)
@@ -871,6 +906,9 @@ static irqreturn_t ehci_irq (struct usb_hcd *hcd)
 			ehci->reset_done[i] = jiffies + msecs_to_jiffies(25);
 			ehci_dbg (ehci, "port %d remote wakeup\n", i + 1);
 			mod_timer(&hcd->rh_timer, ehci->reset_done[i]);
+#ifdef CONFIG_USB_EHCI_TEGRA
+			ehci->controller_remote_wakeup = true;
+#endif
 		}
 	}
 
@@ -1166,8 +1204,7 @@ ehci_endpoint_reset(struct usb_hcd *hcd, struct usb_host_endpoint *ep)
 static int ehci_get_frame (struct usb_hcd *hcd)
 {
 	struct ehci_hcd		*ehci = hcd_to_ehci (hcd);
-	return (ehci_readl(ehci, &ehci->regs->frame_index) >> 3) %
-		ehci->periodic_size;
+	return (ehci_read_frame_index(ehci) >> 3) % ehci->periodic_size;
 }
 
 /*-------------------------------------------------------------------------*/
diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index 4c32cb19b405..bf171c180987 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -27,7 +27,6 @@
  */
 
 /*-------------------------------------------------------------------------*/
-#include <linux/usb/otg.h>
 
 #define	PORT_WAKE_BITS	(PORT_WKOC_E|PORT_WKDISC_E|PORT_WKCONN_E)
 
@@ -146,7 +145,7 @@ static __maybe_unused void ehci_adjust_port_wakeup_flags(struct ehci_hcd *ehci,
 	spin_lock_irqsave(&ehci->lock, flags);
 
 	/* clear phy low-power mode before changing wakeup flags */
-	if (ehci->has_hostpc) {
+	if (ehci->has_hostpc && !ehci->broken_hostpc_phcd) {
 		port = HCS_N_PORTS(ehci->hcs_params);
 		while (port--) {
 			u32 __iomem	*hostpc_reg;
@@ -182,7 +181,7 @@ static __maybe_unused void ehci_adjust_port_wakeup_flags(struct ehci_hcd *ehci,
 	}
 
 	/* enter phy low-power mode again */
-	if (ehci->has_hostpc) {
+	if (ehci->has_hostpc && !ehci->broken_hostpc_phcd) {
 		port = HCS_N_PORTS(ehci->hcs_params);
 		while (port--) {
 			u32 __iomem	*hostpc_reg;
@@ -285,8 +284,8 @@ static int ehci_bus_suspend (struct usb_hcd *hcd)
 			changed = 1;
 		}
 	}
-
-	if (changed && ehci->has_hostpc) {
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	if (changed && ehci->has_hostpc && !ehci->broken_hostpc_phcd) {
 		spin_unlock_irq(&ehci->lock);
 		msleep(5);	/* 5 ms for HCD to enter low-power mode */
 		spin_lock_irq(&ehci->lock);
@@ -306,7 +305,7 @@ static int ehci_bus_suspend (struct usb_hcd *hcd)
 					"succeeded" : "failed");
 		}
 	}
-
+#endif
 	/* Apparently some devices need a >= 1-uframe delay here */
 	if (ehci->bus_suspended)
 		udelay(150);
@@ -390,7 +389,7 @@ static int ehci_bus_resume (struct usb_hcd *hcd)
 	spin_lock_irq(&ehci->lock);
 
 	/* clear phy low-power mode before resume */
-	if (ehci->bus_suspended && ehci->has_hostpc) {
+	if (ehci->bus_suspended && ehci->has_hostpc && !ehci->broken_hostpc_phcd) {
 		i = HCS_N_PORTS(ehci->hcs_params);
 		while (i--) {
 			if (test_bit(i, &ehci->bus_suspended)) {
@@ -726,20 +725,13 @@ static int ehci_hub_control (
 				goto error;
 			if (ehci->no_selective_suspend)
 				break;
-#ifdef CONFIG_USB_OTG
-			if ((hcd->self.otg_port == (wIndex + 1))
-			    && hcd->self.b_hnp_enable) {
-				otg_start_hnp(ehci->transceiver);
-				break;
-			}
-#endif
 			if (!(temp & PORT_SUSPEND))
 				break;
 			if ((temp & PORT_PE) == 0)
 				goto error;
 
 			/* clear phy low-power mode before resume */
-			if (hostpc_reg) {
+			if (hostpc_reg && !ehci->broken_hostpc_phcd) {
 				temp1 = ehci_readl(ehci, hostpc_reg);
 				ehci_writel(ehci, temp1 & ~HOSTPC_PHCD,
 						hostpc_reg);
@@ -987,7 +979,7 @@ static int ehci_hub_control (
 			temp &= ~PORT_WKCONN_E;
 			temp |= PORT_WKDISC_E | PORT_WKOC_E;
 			ehci_writel(ehci, temp | PORT_SUSPEND, status_reg);
-			if (hostpc_reg) {
+			if (hostpc_reg && !ehci->broken_hostpc_phcd) {
 				spin_unlock_irqrestore(&ehci->lock, flags);
 				msleep(5);/* 5ms for HCD enter low pwr mode */
 				spin_lock_irqsave(&ehci->lock, flags);
diff --git a/drivers/usb/host/ehci-pci.c b/drivers/usb/host/ehci-pci.c
index 1102ce65a3a9..1d1caa6a33fc 100644
--- a/drivers/usb/host/ehci-pci.c
+++ b/drivers/usb/host/ehci-pci.c
@@ -224,6 +224,11 @@ static int ehci_pci_setup(struct usb_hcd *hcd)
 			pci_dev_put(p_smbus);
 		}
 		break;
+	case PCI_VENDOR_ID_NETMOS:
+		/* MosChip frame-index-register bug */
+		ehci_info(ehci, "applying MosChip frame-index workaround\n");
+		ehci->frame_index_bug = 1;
+		break;
 	}
 
 	/* optional debug port, normally in the first BAR */
diff --git a/drivers/usb/host/ehci-q.c b/drivers/usb/host/ehci-q.c
index 0917e3a32465..0eb20989d67e 100644
--- a/drivers/usb/host/ehci-q.c
+++ b/drivers/usb/host/ehci-q.c
@@ -334,6 +334,7 @@ qh_completions (struct ehci_hcd *ehci, struct ehci_qh *qh)
 	qh->qh_state = QH_STATE_COMPLETING;
 	stopped = (state == QH_STATE_IDLE);
 
+	ehci_sync_qh(ehci, qh);
  rescan:
 	last = NULL;
 	last_status = -EINPROGRESS;
@@ -367,6 +368,7 @@ qh_completions (struct ehci_hcd *ehci, struct ehci_qh *qh)
 		if (qtd == end)
 			break;
 
+		ehci_sync_qtd(ehci, qtd);
 		/* hardware copies qtd out of qh overlay */
 		rmb ();
 		token = hc32_to_cpu(ehci, qtd->hw_token);
@@ -649,7 +651,7 @@ qh_urb_transaction (
 	/*
 	 * data transfer stage:  buffer setup
 	 */
-	i = urb->num_sgs;
+	i = urb->num_mapped_sgs;
 	if (len > 0 && i > 0) {
 		sg = urb->sg;
 		buf = sg_dma_address(sg);
@@ -995,9 +997,16 @@ static void qh_link_async (struct ehci_hcd *ehci, struct ehci_qh *qh)
 	head->qh_next.qh = qh;
 	head->hw->hw_next = dma;
 
+	/*
+	 * flush qh descriptor into memory immediately,
+	 * see comments in qh_append_tds.
+	 * */
+	ehci_sync_mem();
+
 	qh_get(qh);
 	qh->xacterrs = 0;
 	qh->qh_state = QH_STATE_LINKED;
+	wmb();
 	/* qtd completions reported later by interrupt */
 }
 
@@ -1082,6 +1091,18 @@ static struct ehci_qh *qh_append_tds (
 			wmb ();
 			dummy->hw_token = token;
 
+			/*
+			 * Writing to dma coherent buffer on ARM may
+			 * be delayed to reach memory, so HC may not see
+			 * hw_token of dummy qtd in time, which can cause
+			 * the qtd transaction to be executed very late,
+			 * and degrade performance a lot. ehci_sync_mem
+			 * is added to flush 'token' immediatelly into
+			 * memory, so that ehci can execute the transaction
+			 * ASAP.
+			 * */
+			ehci_sync_mem();
+
 			urb->hcpriv = qh_get (qh);
 		}
 	}
diff --git a/drivers/usb/host/ehci-sched.c b/drivers/usb/host/ehci-sched.c
index 2abf8543f083..c787af2ba4be 100644
--- a/drivers/usb/host/ehci-sched.c
+++ b/drivers/usb/host/ehci-sched.c
@@ -36,6 +36,27 @@
 
 static int ehci_get_frame (struct usb_hcd *hcd);
 
+#ifdef CONFIG_PCI
+
+static unsigned ehci_read_frame_index(struct ehci_hcd *ehci)
+{
+	unsigned uf;
+
+	/*
+	 * The MosChip MCS9990 controller updates its microframe counter
+	 * a little before the frame counter, and occasionally we will read
+	 * the invalid intermediate value.  Avoid problems by checking the
+	 * microframe number (the low-order 3 bits); if they are 0 then
+	 * re-read the register to get the correct value.
+	 */
+	uf = ehci_readl(ehci, &ehci->regs->frame_index);
+	if (unlikely(ehci->frame_index_bug && ((uf & 7) == 0)))
+		uf = ehci_readl(ehci, &ehci->regs->frame_index);
+	return uf;
+}
+
+#endif
+
 /*-------------------------------------------------------------------------*/
 
 /*
@@ -482,7 +503,7 @@ static int enable_periodic (struct ehci_hcd *ehci)
 	ehci_to_hcd(ehci)->state = HC_STATE_RUNNING;
 
 	/* make sure ehci_work scans these */
-	ehci->next_uframe = ehci_readl(ehci, &ehci->regs->frame_index)
+	ehci->next_uframe = ehci_read_frame_index(ehci)
 		% (ehci->periodic_size << 3);
 	if (unlikely(ehci->broken_periodic))
 		ehci->last_periodic_enable = ktime_get_real();
@@ -1409,7 +1430,7 @@ iso_stream_schedule (
 		goto fail;
 	}
 
-	now = ehci_readl(ehci, &ehci->regs->frame_index) & (mod - 1);
+	now = ehci_read_frame_index(ehci) & (mod - 1);
 
 	/* Typical case: reuse current schedule, stream is still active.
 	 * Hopefully there are no gaps from the host falling behind
@@ -1455,30 +1476,36 @@ iso_stream_schedule (
 	 * jump until after the queue is primed.
 	 */
 	else {
+		int done = 0;
 		start = SCHEDULE_SLOP + (now & ~0x07);
 
 		/* NOTE:  assumes URB_ISO_ASAP, to limit complexity/bugs */
 
-		/* find a uframe slot with enough bandwidth */
-		next = start + period;
-		for (; start < next; start++) {
-
+		/* find a uframe slot with enough bandwidth.
+		 * Early uframes are more precious because full-speed
+		 * iso IN transfers can't use late uframes,
+		 * and therefore they should be allocated last.
+		 */
+		next = start;
+		start += period;
+		do {
+			start--;
 			/* check schedule: enough space? */
 			if (stream->highspeed) {
 				if (itd_slot_ok(ehci, mod, start,
 						stream->usecs, period))
-					break;
+					done = 1;
 			} else {
 				if ((start % 8) >= 6)
 					continue;
 				if (sitd_slot_ok(ehci, mod, stream,
 						start, sched, period))
-					break;
+					done = 1;
 			}
-		}
+		} while (start > next && !done);
 
 		/* no room in the schedule */
-		if (start == next) {
+		if (!done) {
 			ehci_dbg(ehci, "iso resched full %p (now %d max %d)\n",
 				urb, now, now + mod);
 			status = -ENOSPC;
@@ -2276,7 +2303,7 @@ scan_periodic (struct ehci_hcd *ehci)
 	 */
 	now_uframe = ehci->next_uframe;
 	if (HC_IS_RUNNING(ehci_to_hcd(ehci)->state)) {
-		clock = ehci_readl(ehci, &ehci->regs->frame_index);
+		clock = ehci_read_frame_index(ehci);
 		clock_frame = (clock >> 3) & (ehci->periodic_size - 1);
 	} else  {
 		clock = now_uframe + mod - 1;
@@ -2455,8 +2482,7 @@ restart:
 					|| ehci->periodic_sched == 0)
 				break;
 			ehci->next_uframe = now_uframe;
-			now = ehci_readl(ehci, &ehci->regs->frame_index) &
-					(mod - 1);
+			now = ehci_read_frame_index(ehci) & (mod - 1);
 			if (now_uframe == now)
 				break;
 
diff --git a/drivers/usb/host/ehci-tegra.c b/drivers/usb/host/ehci-tegra.c
index 02b2bfd49a10..095447d9e3a2 100644
--- a/drivers/usb/host/ehci-tegra.c
+++ b/drivers/usb/host/ehci-tegra.c
@@ -1,8 +1,8 @@
 /*
  * EHCI-compliant USB host controller driver for NVIDIA Tegra SoCs
  *
- * Copyright (C) 2010 Google, Inc.
- * Copyright (C) 2009 NVIDIA Corporation
+ * Copyright (c) 2010 Google, Inc.
+ * Copyright (c) 2009-2012 NVIDIA 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
@@ -16,46 +16,194 @@
  *
  */
 
-#include <linux/clk.h>
 #include <linux/platform_device.h>
 #include <linux/platform_data/tegra_usb.h>
 #include <linux/irq.h>
 #include <linux/usb/otg.h>
 #include <mach/usb_phy.h>
+#include <mach/iomap.h>
+
+#include "../../../arch/arm/mach-tegra/tegra_usb_phy.h"
+
+#if 0
+#define EHCI_DBG(stuff...)	pr_info("ehci-tegra: " stuff)
+#else
+#define EHCI_DBG(stuff...)	do {} while (0)
+#endif
+
+#define TEGRA_USB_PORTSC1_PFSC	(1 << 24)
+
+static const char driver_name[] = "tegra-ehci";
 
 #define TEGRA_USB_DMA_ALIGN 32
 
 struct tegra_ehci_hcd {
 	struct ehci_hcd *ehci;
 	struct tegra_usb_phy *phy;
-	struct clk *clk;
-	struct clk *emc_clk;
+#ifdef CONFIG_USB_OTG_UTILS
 	struct otg_transceiver *transceiver;
-	int host_resumed;
-	int bus_suspended;
-	int port_resuming;
-	int power_down_on_bus_suspend;
-	enum tegra_usb_phy_port_speed port_speed;
+#endif
+	struct mutex sync_lock;
+	bool port_resuming;
+	unsigned int irq;
+	bool bus_suspended_fail;
+};
+
+struct dma_align_buffer {
+	void *kmalloc_ptr;
+	void *old_xfer_buffer;
+	u8 data[0];
 };
 
-static void tegra_ehci_power_up(struct usb_hcd *hcd)
+#ifdef CONFIG_MACH_COLIBRI_T20
+/* To limit the speed of USB to full speed */
+int g_usb_high_speed = 0;
+
+/* To limit the speed of USB to full speed */
+static int __init enable_usb_high_speed(char *s)
 {
-	struct tegra_ehci_hcd *tegra = dev_get_drvdata(hcd->self.controller);
+	if (!(*s) || !strcmp(s, "1"))
+		g_usb_high_speed = 1;
 
-	clk_enable(tegra->emc_clk);
-	clk_enable(tegra->clk);
-	tegra_usb_phy_power_on(tegra->phy);
-	tegra->host_resumed = 1;
+	return 0;
 }
+__setup("usb_high_speed=", enable_usb_high_speed);
+EXPORT_SYMBOL_GPL(g_usb_high_speed);
+#endif /* CONFIG_MACH_COLIBRI_T20 */
 
-static void tegra_ehci_power_down(struct usb_hcd *hcd)
+static void free_align_buffer(struct urb *urb)
+{
+	struct dma_align_buffer *temp = container_of(urb->transfer_buffer,
+						struct dma_align_buffer, data);
+
+	if (!(urb->transfer_flags & URB_ALIGNED_TEMP_BUFFER))
+		return;
+
+	/* In transaction, DMA from Device */
+	if (usb_urb_dir_in(urb))
+		memcpy(temp->old_xfer_buffer, temp->data,
+					urb->transfer_buffer_length);
+
+	urb->transfer_buffer = temp->old_xfer_buffer;
+	urb->transfer_flags &= ~URB_ALIGNED_TEMP_BUFFER;
+	kfree(temp->kmalloc_ptr);
+}
+
+static int alloc_align_buffer(struct urb *urb, gfp_t mem_flags)
+{
+	struct dma_align_buffer *temp, *kmalloc_ptr;
+	size_t kmalloc_size;
+
+	if (urb->num_sgs || urb->sg ||
+		urb->transfer_buffer_length == 0 ||
+		!((uintptr_t)urb->transfer_buffer & (TEGRA_USB_DMA_ALIGN - 1)))
+		return 0;
+
+	/* Allocate a buffer with enough padding for alignment */
+	kmalloc_size = urb->transfer_buffer_length +
+		sizeof(struct dma_align_buffer) + TEGRA_USB_DMA_ALIGN - 1;
+	kmalloc_ptr = kmalloc(kmalloc_size, mem_flags);
+
+	if (!kmalloc_ptr)
+		return -ENOMEM;
+
+	/* Position our struct dma_align_buffer such that data is aligned */
+	temp = PTR_ALIGN(kmalloc_ptr + 1, TEGRA_USB_DMA_ALIGN) - 1;
+	temp->kmalloc_ptr = kmalloc_ptr;
+	temp->old_xfer_buffer = urb->transfer_buffer;
+	/* OUT transaction, DMA to Device */
+	if (!usb_urb_dir_in(urb))
+		memcpy(temp->data, urb->transfer_buffer,
+				urb->transfer_buffer_length);
+
+	urb->transfer_buffer = temp->data;
+	urb->transfer_flags |= URB_ALIGNED_TEMP_BUFFER;
+
+	return 0;
+}
+
+static int tegra_ehci_map_urb_for_dma(struct usb_hcd *hcd,
+	struct urb *urb, gfp_t mem_flags)
+{
+	int ret;
+
+	ret = alloc_align_buffer(urb, mem_flags);
+	if (ret)
+		return ret;
+
+	ret = usb_hcd_map_urb_for_dma(hcd, urb, mem_flags);
+
+	/* Control packets over dma */
+	if (urb->setup_dma)
+		dma_sync_single_for_device(hcd->self.controller,
+			urb->setup_dma, sizeof(struct usb_ctrlrequest),
+			DMA_TO_DEVICE);
+
+	/* urb buffers over dma */
+	if (urb->transfer_dma) {
+		enum dma_data_direction dir;
+		dir = usb_urb_dir_in(urb) ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
+
+		dma_sync_single_for_device(hcd->self.controller,
+			urb->transfer_dma, urb->transfer_buffer_length, dir);
+	}
+
+	if (ret)
+		free_align_buffer(urb);
+
+	return ret;
+}
+
+static void tegra_ehci_unmap_urb_for_dma(struct usb_hcd *hcd,
+	struct urb *urb)
+{
+
+	if (urb->transfer_dma) {
+		enum dma_data_direction dir;
+		dir = usb_urb_dir_in(urb) ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
+		if (dir == DMA_FROM_DEVICE)
+			dma_sync_single_for_cpu(hcd->self.controller,
+				urb->transfer_dma, urb->transfer_buffer_length,
+									   DMA_FROM_DEVICE);
+	}
+
+	usb_hcd_unmap_urb_for_dma(hcd, urb);
+	free_align_buffer(urb);
+}
+
+static irqreturn_t tegra_ehci_irq(struct usb_hcd *hcd)
 {
 	struct tegra_ehci_hcd *tegra = dev_get_drvdata(hcd->self.controller);
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	irqreturn_t irq_status;
 
-	tegra->host_resumed = 0;
-	tegra_usb_phy_power_off(tegra->phy);
-	clk_disable(tegra->clk);
-	clk_disable(tegra->emc_clk);
+	spin_lock(&ehci->lock);
+	irq_status = tegra_usb_phy_irq(tegra->phy);
+	if (irq_status == IRQ_NONE) {
+		spin_unlock(&ehci->lock);
+		return irq_status;
+	}
+	if (tegra_usb_phy_remote_wakeup(tegra->phy)) {
+		ehci_info(ehci, "remote wakeup detected\n");
+		usb_hcd_resume_root_hub(hcd);
+		spin_unlock(&ehci->lock);
+		return irq_status;
+	}
+	spin_unlock(&ehci->lock);
+
+	EHCI_DBG("%s() cmd = 0x%x, int_sts = 0x%x, portsc = 0x%x\n", __func__,
+		ehci_readl(ehci, &ehci->regs->command),
+		ehci_readl(ehci, &ehci->regs->status),
+		ehci_readl(ehci, &ehci->regs->port_status[0]));
+
+	irq_status = ehci_irq(hcd);
+
+	if (ehci->controller_remote_wakeup) {
+		ehci->controller_remote_wakeup = false;
+		tegra_usb_phy_pre_resume(tegra->phy, true);
+		tegra->port_resuming = 1;
+	}
+	return irq_status;
 }
 
 static int tegra_ehci_internal_port_reset(
@@ -120,297 +268,168 @@ static int tegra_ehci_internal_port_reset(
 
 	/* restore original interrupt enable bits */
 	ehci_writel(ehci, saved_usbintr, &ehci->regs->intr_enable);
+
 	return retval;
 }
 
 static int tegra_ehci_hub_control(
 	struct usb_hcd	*hcd,
-	u16		typeReq,
-	u16		wValue,
-	u16		wIndex,
-	char		*buf,
-	u16		wLength
+	u16	typeReq,
+	u16	wValue,
+	u16	wIndex,
+	char	*buf,
+	u16	wLength
 )
 {
-	struct ehci_hcd	*ehci = hcd_to_ehci(hcd);
 	struct tegra_ehci_hcd *tegra = dev_get_drvdata(hcd->self.controller);
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	int	retval = 0;
 	u32 __iomem	*status_reg;
-	u32		temp;
-	unsigned long	flags;
-	int		retval = 0;
-
-	status_reg = &ehci->regs->port_status[(wIndex & 0xff) - 1];
 
-	spin_lock_irqsave(&ehci->lock, flags);
+#ifdef CONFIG_MACH_COLIBRI_T20
+	u32 temp;
+
+	/* To limit the speed of USB to full speed */
+	if (!g_usb_high_speed) {
+		/* Check whether port is not 2nd one internally connected to
+		   ASIX Ethernet chip, set PFSC (Port Force Full Speed) only
+		   for externally accessible OTG and host port */
+		if (tegra->phy->inst != 1) {
+			status_reg = &ehci->regs->port_status[(wIndex & 0xff)
+							      - 1];
+			temp = ehci_readl(ehci, status_reg);
+			/* Check whether PFSC bit is already set or not */
+			if (!(temp & TEGRA_USB_PORTSC1_PFSC)) {
+				ehci_writel(ehci, (temp |
+						   TEGRA_USB_PORTSC1_PFSC),
+					    status_reg);
+				temp = ehci_readl(ehci, status_reg);
+			}
+		}
+	}
+#endif /* CONFIG_MACH_COLIBRI_T20 */
 
-	/*
-	 * In ehci_hub_control() for USB_PORT_FEAT_ENABLE clears the other bits
-	 * that are write on clear, by writing back the register read value, so
-	 * USB_PORT_FEAT_ENABLE is handled by masking the set on clear bits
-	 */
-	if (typeReq == ClearPortFeature && wValue == USB_PORT_FEAT_ENABLE) {
-		temp = ehci_readl(ehci, status_reg) & ~PORT_RWC_BITS;
-		ehci_writel(ehci, temp & ~PORT_PE, status_reg);
-		goto done;
+	if (!tegra_usb_phy_hw_accessible(tegra->phy)) {
+		if (buf)
+			memset(buf, 0, wLength);
+		return retval;
 	}
 
-	else if (typeReq == GetPortStatus) {
-		temp = ehci_readl(ehci, status_reg);
-		if (tegra->port_resuming && !(temp & PORT_SUSPEND)) {
-			/* Resume completed, re-enable disconnect detection */
+	/* Do tegra phy specific actions based on the type request */
+	switch (typeReq) {
+	case GetPortStatus:
+		if (tegra->port_resuming) {
+			u32 cmd;
+			int delay = ehci->reset_done[wIndex-1] - jiffies;
+			/* Sometimes it seems we get called too soon... In that case, wait.*/
+			if (delay > 0) {
+				ehci_dbg(ehci, "GetPortStatus called too soon, waiting %dms...\n", delay);
+				mdelay(jiffies_to_msecs(delay));
+			}
+			status_reg = &ehci->regs->port_status[(wIndex & 0xff) - 1];
+			/* Ensure the port PORT_SUSPEND and PORT_RESUME has cleared */
+			if (handshake(ehci, status_reg, (PORT_SUSPEND | PORT_RESUME), 0, 25000)) {
+				EHCI_DBG("%s: timeout waiting for SUSPEND to clear\n", __func__);
+			}
+			tegra_usb_phy_post_resume(tegra->phy);
 			tegra->port_resuming = 0;
-			tegra_usb_phy_postresume(tegra->phy);
+			/* If run bit is not set by now enable it */
+			cmd = ehci_readl(ehci, &ehci->regs->command);
+			if (!(cmd & CMD_RUN)) {
+				cmd |= CMD_RUN;
+				ehci->command |= CMD_RUN;
+				ehci_writel(ehci, cmd, &ehci->regs->command);
+			}
+			/* Now we can safely re-enable irqs */
+			ehci_writel(ehci, INTR_MASK, &ehci->regs->intr_enable);
 		}
-	}
-
-	else if (typeReq == SetPortFeature && wValue == USB_PORT_FEAT_SUSPEND) {
-		temp = ehci_readl(ehci, status_reg);
-		if ((temp & PORT_PE) == 0 || (temp & PORT_RESET) != 0) {
-			retval = -EPIPE;
-			goto done;
+		break;
+	case ClearPortFeature:
+		if (wValue == USB_PORT_FEAT_SUSPEND) {
+			tegra_usb_phy_pre_resume(tegra->phy, false);
+			tegra->port_resuming = 1;
+		} else if (wValue == USB_PORT_FEAT_ENABLE) {
+			u32 temp;
+			temp = ehci_readl(ehci, &ehci->regs->port_status[0]) & ~PORT_RWC_BITS;
+			ehci_writel(ehci, temp & ~PORT_PE, &ehci->regs->port_status[0]);
+			return retval;
 		}
-
-		temp &= ~PORT_WKCONN_E;
-		temp |= PORT_WKDISC_E | PORT_WKOC_E;
-		ehci_writel(ehci, temp | PORT_SUSPEND, status_reg);
-
-		/*
-		 * If a transaction is in progress, there may be a delay in
-		 * suspending the port. Poll until the port is suspended.
-		 */
-		if (handshake(ehci, status_reg, PORT_SUSPEND,
-						PORT_SUSPEND, 5000))
-			pr_err("%s: timeout waiting for SUSPEND\n", __func__);
-
-		set_bit((wIndex & 0xff) - 1, &ehci->suspended_ports);
-		goto done;
+		break;
 	}
 
 	/* For USB1 port we need to issue Port Reset twice internally */
-	if (tegra->phy->instance == 0 &&
+	if (tegra->phy->inst == 0 &&
 	   (typeReq == SetPortFeature && wValue == USB_PORT_FEAT_RESET)) {
-		spin_unlock_irqrestore(&ehci->lock, flags);
+		status_reg = &ehci->regs->port_status[(wIndex & 0xff) - 1];
 		return tegra_ehci_internal_port_reset(ehci, status_reg);
 	}
 
-	/*
-	 * Tegra host controller will time the resume operation to clear the bit
-	 * when the port control state switches to HS or FS Idle. This behavior
-	 * is different from EHCI where the host controller driver is required
-	 * to set this bit to a zero after the resume duration is timed in the
-	 * driver.
-	 */
-	else if (typeReq == ClearPortFeature &&
-					wValue == USB_PORT_FEAT_SUSPEND) {
-		temp = ehci_readl(ehci, status_reg);
-		if ((temp & PORT_RESET) || !(temp & PORT_PE)) {
-			retval = -EPIPE;
-			goto done;
+	/* handle ehci hub control request */
+	retval = ehci_hub_control(hcd, typeReq, wValue, wIndex, buf, wLength);
+
+	/* do tegra phy specific actions based on the type request */
+	if (!retval) {
+		switch (typeReq) {
+		case SetPortFeature:
+			if (wValue == USB_PORT_FEAT_SUSPEND) {
+				/* Need a 4ms delay for controller to suspend */
+				mdelay(4);
+				tegra_usb_phy_post_suspend(tegra->phy);
+			} else if (wValue == USB_PORT_FEAT_RESET) {
+				if (wIndex == 1)
+					tegra_usb_phy_bus_reset(tegra->phy);
+			} else if (wValue == USB_PORT_FEAT_POWER) {
+				if (wIndex == 1)
+					tegra_usb_phy_port_power(tegra->phy);
+			}
+			break;
+		case ClearPortFeature:
+			if (wValue == USB_PORT_FEAT_SUSPEND) {
+				/* tegra USB controller needs 25 ms to resume the port */
+				ehci->reset_done[wIndex-1] = jiffies + msecs_to_jiffies(25);
+			}
+			break;
 		}
-
-		if (!(temp & PORT_SUSPEND))
-			goto done;
-
-		/* Disable disconnect detection during port resume */
-		tegra_usb_phy_preresume(tegra->phy);
-
-		ehci->reset_done[wIndex-1] = jiffies + msecs_to_jiffies(25);
-
-		temp &= ~(PORT_RWC_BITS | PORT_WAKE_BITS);
-		/* start resume signalling */
-		ehci_writel(ehci, temp | PORT_RESUME, status_reg);
-
-		spin_unlock_irqrestore(&ehci->lock, flags);
-		msleep(20);
-		spin_lock_irqsave(&ehci->lock, flags);
-
-		/* Poll until the controller clears RESUME and SUSPEND */
-		if (handshake(ehci, status_reg, PORT_RESUME, 0, 2000))
-			pr_err("%s: timeout waiting for RESUME\n", __func__);
-		if (handshake(ehci, status_reg, PORT_SUSPEND, 0, 2000))
-			pr_err("%s: timeout waiting for SUSPEND\n", __func__);
-
-		ehci->reset_done[wIndex-1] = 0;
-
-		tegra->port_resuming = 1;
-		goto done;
 	}
 
-	spin_unlock_irqrestore(&ehci->lock, flags);
-
-	/* Handle the hub control events here */
-	return ehci_hub_control(hcd, typeReq, wValue, wIndex, buf, wLength);
-done:
-	spin_unlock_irqrestore(&ehci->lock, flags);
 	return retval;
 }
 
-static void tegra_ehci_restart(struct usb_hcd *hcd)
+static void tegra_ehci_shutdown(struct usb_hcd *hcd)
 {
 	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
-
-	ehci_reset(ehci);
-
-	/* setup the frame list and Async q heads */
-	ehci_writel(ehci, ehci->periodic_dma, &ehci->regs->frame_list);
-	ehci_writel(ehci, (u32)ehci->async->qh_dma, &ehci->regs->async_next);
-	/* setup the command register and set the controller in RUN mode */
-	ehci->command &= ~(CMD_LRESET|CMD_IAAD|CMD_PSE|CMD_ASE|CMD_RESET);
-	ehci->command |= CMD_RUN;
-	ehci_writel(ehci, ehci->command, &ehci->regs->command);
-
-	down_write(&ehci_cf_port_reset_rwsem);
-	ehci_writel(ehci, FLAG_CF, &ehci->regs->configured_flag);
-	/* flush posted writes */
-	ehci_readl(ehci, &ehci->regs->command);
-	up_write(&ehci_cf_port_reset_rwsem);
-}
-
-static int tegra_usb_suspend(struct usb_hcd *hcd)
-{
 	struct tegra_ehci_hcd *tegra = dev_get_drvdata(hcd->self.controller);
-	struct ehci_regs __iomem *hw = tegra->ehci->regs;
-	unsigned long flags;
-
-	spin_lock_irqsave(&tegra->ehci->lock, flags);
-
-	tegra->port_speed = (readl(&hw->port_status[0]) >> 26) & 0x3;
-	ehci_halt(tegra->ehci);
-	clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-
-	spin_unlock_irqrestore(&tegra->ehci->lock, flags);
-
-	tegra_ehci_power_down(hcd);
-	return 0;
-}
-
-static int tegra_usb_resume(struct usb_hcd *hcd)
-{
-	struct tegra_ehci_hcd *tegra = dev_get_drvdata(hcd->self.controller);
-	struct ehci_hcd	*ehci = hcd_to_ehci(hcd);
-	struct ehci_regs __iomem *hw = ehci->regs;
-	unsigned long val;
-
-	set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-	tegra_ehci_power_up(hcd);
-
-	if (tegra->port_speed > TEGRA_USB_PHY_PORT_SPEED_HIGH) {
-		/* Wait for the phy to detect new devices
-		 * before we restart the controller */
-		msleep(10);
-		goto restart;
-	}
-
-	/* Force the phy to keep data lines in suspend state */
-	tegra_ehci_phy_restore_start(tegra->phy, tegra->port_speed);
-
-	/* Enable host mode */
-	tdi_reset(ehci);
-
-	/* Enable Port Power */
-	val = readl(&hw->port_status[0]);
-	val |= PORT_POWER;
-	writel(val, &hw->port_status[0]);
-	udelay(10);
-
-	/* Check if the phy resume from LP0. When the phy resume from LP0
-	 * USB register will be reset. */
-	if (!readl(&hw->async_next)) {
-		/* Program the field PTC based on the saved speed mode */
-		val = readl(&hw->port_status[0]);
-		val &= ~PORT_TEST(~0);
-		if (tegra->port_speed == TEGRA_USB_PHY_PORT_SPEED_HIGH)
-			val |= PORT_TEST_FORCE;
-		else if (tegra->port_speed == TEGRA_USB_PHY_PORT_SPEED_FULL)
-			val |= PORT_TEST(6);
-		else if (tegra->port_speed == TEGRA_USB_PHY_PORT_SPEED_LOW)
-			val |= PORT_TEST(7);
-		writel(val, &hw->port_status[0]);
-		udelay(10);
-
-		/* Disable test mode by setting PTC field to NORMAL_OP */
-		val = readl(&hw->port_status[0]);
-		val &= ~PORT_TEST(~0);
-		writel(val, &hw->port_status[0]);
-		udelay(10);
+	mutex_lock(&tegra->sync_lock);
+	del_timer_sync(&ehci->watchdog);
+	del_timer_sync(&ehci->iaa_watchdog);
+	if (tegra_usb_phy_hw_accessible(tegra->phy)) {
+		spin_lock_irq(&ehci->lock);
+		ehci_silence_controller(ehci);
+		spin_unlock_irq(&ehci->lock);
 	}
-
-	/* Poll until CCS is enabled */
-	if (handshake(ehci, &hw->port_status[0], PORT_CONNECT,
-						 PORT_CONNECT, 2000)) {
-		pr_err("%s: timeout waiting for PORT_CONNECT\n", __func__);
-		goto restart;
-	}
-
-	/* Poll until PE is enabled */
-	if (handshake(ehci, &hw->port_status[0], PORT_PE,
-						 PORT_PE, 2000)) {
-		pr_err("%s: timeout waiting for USB_PORTSC1_PE\n", __func__);
-		goto restart;
-	}
-
-	/* Clear the PCI status, to avoid an interrupt taken upon resume */
-	val = readl(&hw->status);
-	val |= STS_PCD;
-	writel(val, &hw->status);
-
-	/* Put controller in suspend mode by writing 1 to SUSP bit of PORTSC */
-	val = readl(&hw->port_status[0]);
-	if ((val & PORT_POWER) && (val & PORT_PE)) {
-		val |= PORT_SUSPEND;
-		writel(val, &hw->port_status[0]);
-
-		/* Wait until port suspend completes */
-		if (handshake(ehci, &hw->port_status[0], PORT_SUSPEND,
-							 PORT_SUSPEND, 1000)) {
-			pr_err("%s: timeout waiting for PORT_SUSPEND\n",
-								__func__);
-			goto restart;
-		}
-	}
-
-	tegra_ehci_phy_restore_end(tegra->phy);
-	return 0;
-
-restart:
-	if (tegra->port_speed <= TEGRA_USB_PHY_PORT_SPEED_HIGH)
-		tegra_ehci_phy_restore_end(tegra->phy);
-
-	tegra_ehci_restart(hcd);
-	return 0;
-}
-
-static void tegra_ehci_shutdown(struct usb_hcd *hcd)
-{
-	struct tegra_ehci_hcd *tegra = dev_get_drvdata(hcd->self.controller);
-
-	/* ehci_shutdown touches the USB controller registers, make sure
-	 * controller has clocks to it */
-	if (!tegra->host_resumed)
-		tegra_ehci_power_up(hcd);
-
-	ehci_shutdown(hcd);
+	mutex_unlock(&tegra->sync_lock);
 }
 
 static int tegra_ehci_setup(struct usb_hcd *hcd)
 {
 	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	struct tegra_ehci_hcd *tegra = dev_get_drvdata(hcd->self.controller);
 	int retval;
 
 	/* EHCI registers start at offset 0x100 */
 	ehci->caps = hcd->regs + 0x100;
 	ehci->regs = hcd->regs + 0x100 +
-		HC_LENGTH(ehci, readl(&ehci->caps->hc_capbase));
+				HC_LENGTH(ehci, readl(&ehci->caps->hc_capbase));
 
 	dbg_hcs_params(ehci, "reset");
 	dbg_hcc_params(ehci, "reset");
 
 	/* cache this readonly data; minimize chip reads */
 	ehci->hcs_params = readl(&ehci->caps->hcs_params);
+	ehci->has_hostpc = tegra_usb_phy_has_hostpc(tegra->phy) ? 1 : 0;
+	ehci->broken_hostpc_phcd = true;
 
-	/* switch to host mode */
 	hcd->has_tt = 1;
-	ehci_reset(ehci);
 
 	retval = ehci_halt(ehci);
 	if (retval)
@@ -422,156 +441,80 @@ static int tegra_ehci_setup(struct usb_hcd *hcd)
 		return retval;
 
 	ehci->sbrn = 0x20;
+	ehci->controller_remote_wakeup = false;
+	ehci_reset(ehci);
+	tegra_usb_phy_reset(tegra->phy);
 
 	ehci_port_power(ehci, 1);
 	return retval;
 }
 
+
 #ifdef CONFIG_PM
 static int tegra_ehci_bus_suspend(struct usb_hcd *hcd)
 {
 	struct tegra_ehci_hcd *tegra = dev_get_drvdata(hcd->self.controller);
-	int error_status = 0;
-
-	error_status = ehci_bus_suspend(hcd);
-	if (!error_status && tegra->power_down_on_bus_suspend) {
-		tegra_usb_suspend(hcd);
-		tegra->bus_suspended = 1;
-	}
+	int err = 0;
+	EHCI_DBG("%s() BEGIN\n", __func__);
+	mutex_lock(&tegra->sync_lock);
+	tegra->bus_suspended_fail = false;
+	err = ehci_bus_suspend(hcd);
+	if (err)
+		tegra->bus_suspended_fail = true;
+	else
+		tegra_usb_phy_suspend(tegra->phy);
+	mutex_unlock(&tegra->sync_lock);
+	EHCI_DBG("%s() END\n", __func__);
 
-	return error_status;
+	return err;
 }
 
 static int tegra_ehci_bus_resume(struct usb_hcd *hcd)
 {
 	struct tegra_ehci_hcd *tegra = dev_get_drvdata(hcd->self.controller);
+	int err = 0;
+	EHCI_DBG("%s() BEGIN\n", __func__);
 
-	if (tegra->bus_suspended && tegra->power_down_on_bus_suspend) {
-		tegra_usb_resume(hcd);
-		tegra->bus_suspended = 0;
-	}
+	mutex_lock(&tegra->sync_lock);
+	tegra_usb_phy_resume(tegra->phy);
+	err = ehci_bus_resume(hcd);
+	mutex_unlock(&tegra->sync_lock);
+	EHCI_DBG("%s() END\n", __func__);
 
-	tegra_usb_phy_preresume(tegra->phy);
-	tegra->port_resuming = 1;
-	return ehci_bus_resume(hcd);
+	return err;
 }
 #endif
 
-struct temp_buffer {
-	void *kmalloc_ptr;
-	void *old_xfer_buffer;
-	u8 data[0];
-};
-
-static void free_temp_buffer(struct urb *urb)
-{
-	enum dma_data_direction dir;
-	struct temp_buffer *temp;
-
-	if (!(urb->transfer_flags & URB_ALIGNED_TEMP_BUFFER))
-		return;
-
-	dir = usb_urb_dir_in(urb) ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
-
-	temp = container_of(urb->transfer_buffer, struct temp_buffer,
-			    data);
-
-	if (dir == DMA_FROM_DEVICE)
-		memcpy(temp->old_xfer_buffer, temp->data,
-		       urb->transfer_buffer_length);
-	urb->transfer_buffer = temp->old_xfer_buffer;
-	kfree(temp->kmalloc_ptr);
-
-	urb->transfer_flags &= ~URB_ALIGNED_TEMP_BUFFER;
-}
-
-static int alloc_temp_buffer(struct urb *urb, gfp_t mem_flags)
-{
-	enum dma_data_direction dir;
-	struct temp_buffer *temp, *kmalloc_ptr;
-	size_t kmalloc_size;
-
-	if (urb->num_sgs || urb->sg ||
-	    urb->transfer_buffer_length == 0 ||
-	    !((uintptr_t)urb->transfer_buffer & (TEGRA_USB_DMA_ALIGN - 1)))
-		return 0;
-
-	dir = usb_urb_dir_in(urb) ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
-
-	/* Allocate a buffer with enough padding for alignment */
-	kmalloc_size = urb->transfer_buffer_length +
-		sizeof(struct temp_buffer) + TEGRA_USB_DMA_ALIGN - 1;
-
-	kmalloc_ptr = kmalloc(kmalloc_size, mem_flags);
-	if (!kmalloc_ptr)
-		return -ENOMEM;
-
-	/* Position our struct temp_buffer such that data is aligned */
-	temp = PTR_ALIGN(kmalloc_ptr + 1, TEGRA_USB_DMA_ALIGN) - 1;
-
-	temp->kmalloc_ptr = kmalloc_ptr;
-	temp->old_xfer_buffer = urb->transfer_buffer;
-	if (dir == DMA_TO_DEVICE)
-		memcpy(temp->data, urb->transfer_buffer,
-		       urb->transfer_buffer_length);
-	urb->transfer_buffer = temp->data;
-
-	urb->transfer_flags |= URB_ALIGNED_TEMP_BUFFER;
-
-	return 0;
-}
-
-static int tegra_ehci_map_urb_for_dma(struct usb_hcd *hcd, struct urb *urb,
-				      gfp_t mem_flags)
-{
-	int ret;
-
-	ret = alloc_temp_buffer(urb, mem_flags);
-	if (ret)
-		return ret;
-
-	ret = usb_hcd_map_urb_for_dma(hcd, urb, mem_flags);
-	if (ret)
-		free_temp_buffer(urb);
-
-	return ret;
-}
-
-static void tegra_ehci_unmap_urb_for_dma(struct usb_hcd *hcd, struct urb *urb)
-{
-	usb_hcd_unmap_urb_for_dma(hcd, urb);
-	free_temp_buffer(urb);
-}
-
 static const struct hc_driver tegra_ehci_hc_driver = {
 	.description		= hcd_name,
 	.product_desc		= "Tegra EHCI Host Controller",
 	.hcd_priv_size		= sizeof(struct ehci_hcd),
-
 	.flags			= HCD_USB2 | HCD_MEMORY,
 
-	.reset			= tegra_ehci_setup,
-	.irq			= ehci_irq,
-
+	/* standard ehci functions */
 	.start			= ehci_run,
 	.stop			= ehci_stop,
-	.shutdown		= tegra_ehci_shutdown,
 	.urb_enqueue		= ehci_urb_enqueue,
 	.urb_dequeue		= ehci_urb_dequeue,
-	.map_urb_for_dma	= tegra_ehci_map_urb_for_dma,
-	.unmap_urb_for_dma	= tegra_ehci_unmap_urb_for_dma,
 	.endpoint_disable	= ehci_endpoint_disable,
-	.endpoint_reset		= ehci_endpoint_reset,
+	.endpoint_reset	= ehci_endpoint_reset,
 	.get_frame_number	= ehci_get_frame,
 	.hub_status_data	= ehci_hub_status_data,
-	.hub_control		= tegra_ehci_hub_control,
 	.clear_tt_buffer_complete = ehci_clear_tt_buffer_complete,
-#ifdef CONFIG_PM
-	.bus_suspend		= tegra_ehci_bus_suspend,
-	.bus_resume		= tegra_ehci_bus_resume,
-#endif
 	.relinquish_port	= ehci_relinquish_port,
 	.port_handed_over	= ehci_port_handed_over,
+
+	/* modified ehci functions for tegra */
+	.reset			= tegra_ehci_setup,
+	.irq			= tegra_ehci_irq,
+	.shutdown		= tegra_ehci_shutdown,
+	.map_urb_for_dma	= tegra_ehci_map_urb_for_dma,
+	.unmap_urb_for_dma	= tegra_ehci_unmap_urb_for_dma,
+	.hub_control		= tegra_ehci_hub_control,
+#ifdef CONFIG_PM
+	.bus_suspend	= tegra_ehci_bus_suspend,
+	.bus_resume	= tegra_ehci_bus_resume,
+#endif
 };
 
 static int tegra_ehci_probe(struct platform_device *pdev)
@@ -579,55 +522,31 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 	struct resource *res;
 	struct usb_hcd *hcd;
 	struct tegra_ehci_hcd *tegra;
-	struct tegra_ehci_platform_data *pdata;
+	struct tegra_usb_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	int err = 0;
 	int irq;
-	int instance = pdev->id;
 
-	pdata = pdev->dev.platform_data;
-	if (!pdata) {
-		dev_err(&pdev->dev, "Platform data missing\n");
-		return -EINVAL;
+	tegra = devm_kzalloc(&pdev->dev, sizeof(struct tegra_ehci_hcd),
+		GFP_KERNEL);
+	if (!tegra) {
+		dev_err(&pdev->dev, "memory alloc failed\n");
+		return -ENOMEM;
 	}
 
-	tegra = kzalloc(sizeof(struct tegra_ehci_hcd), GFP_KERNEL);
-	if (!tegra)
-		return -ENOMEM;
+	mutex_init(&tegra->sync_lock);
 
 	hcd = usb_create_hcd(&tegra_ehci_hc_driver, &pdev->dev,
 					dev_name(&pdev->dev));
 	if (!hcd) {
-		dev_err(&pdev->dev, "Unable to create HCD\n");
-		err = -ENOMEM;
-		goto fail_hcd;
+		dev_err(&pdev->dev, "unable to create HCD\n");
+		return -ENOMEM;
 	}
 
 	platform_set_drvdata(pdev, tegra);
 
-	tegra->clk = clk_get(&pdev->dev, NULL);
-	if (IS_ERR(tegra->clk)) {
-		dev_err(&pdev->dev, "Can't get ehci clock\n");
-		err = PTR_ERR(tegra->clk);
-		goto fail_clk;
-	}
-
-	err = clk_enable(tegra->clk);
-	if (err)
-		goto fail_clken;
-
-	tegra->emc_clk = clk_get(&pdev->dev, "emc");
-	if (IS_ERR(tegra->emc_clk)) {
-		dev_err(&pdev->dev, "Can't get emc clock\n");
-		err = PTR_ERR(tegra->emc_clk);
-		goto fail_emc_clk;
-	}
-
-	clk_enable(tegra->emc_clk);
-	clk_set_rate(tegra->emc_clk, 400000000);
-
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
-		dev_err(&pdev->dev, "Failed to get I/O memory\n");
+		dev_err(&pdev->dev, "failed to get I/O memory\n");
 		err = -ENXIO;
 		goto fail_io;
 	}
@@ -635,107 +554,102 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 	hcd->rsrc_len = resource_size(res);
 	hcd->regs = ioremap(res->start, resource_size(res));
 	if (!hcd->regs) {
-		dev_err(&pdev->dev, "Failed to remap I/O memory\n");
+		dev_err(&pdev->dev, "failed to remap I/O memory\n");
 		err = -ENOMEM;
 		goto fail_io;
 	}
 
-	tegra->phy = tegra_usb_phy_open(instance, hcd->regs, pdata->phy_config,
-						TEGRA_USB_PHY_MODE_HOST);
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "failed to get IRQ\n");
+		err = -ENODEV;
+		goto fail_irq;
+	}
+	set_irq_flags(irq, IRQF_VALID);
+	tegra->irq = irq;
+
+	tegra->phy = tegra_usb_phy_open(pdev);
 	if (IS_ERR(tegra->phy)) {
-		dev_err(&pdev->dev, "Failed to open USB phy\n");
+		dev_err(&pdev->dev, "failed to open USB phy\n");
 		err = -ENXIO;
-		goto fail_phy;
+		goto fail_irq;
 	}
 
 	err = tegra_usb_phy_power_on(tegra->phy);
 	if (err) {
-		dev_err(&pdev->dev, "Failed to power on the phy\n");
-		goto fail;
+		dev_err(&pdev->dev, "failed to power on the phy\n");
+		goto fail_phy;
 	}
 
-	tegra->host_resumed = 1;
-	tegra->power_down_on_bus_suspend = pdata->power_down_on_bus_suspend;
-	tegra->ehci = hcd_to_ehci(hcd);
+	err = tegra_usb_phy_init(tegra->phy);
+	if (err) {
+		dev_err(&pdev->dev, "failed to init the phy\n");
+		goto fail_phy;
+	}
 
-	irq = platform_get_irq(pdev, 0);
-	if (!irq) {
-		dev_err(&pdev->dev, "Failed to get IRQ\n");
-		err = -ENODEV;
-		goto fail;
+	err = usb_add_hcd(hcd, irq, IRQF_SHARED | IRQF_TRIGGER_HIGH);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to add USB HCD, error=%d\n", err);
+		goto fail_phy;
+	}
+
+	if (pdata->u_data.host.remote_wakeup_supported) {
+		err = enable_irq_wake(tegra->irq);
+		if (err < 0) {
+			dev_warn(&pdev->dev,
+					"Couldn't enable USB host mode wakeup,"
+					" irq=%d error=%d\n", irq, err);
+			err = 0;
+			tegra->irq = 0;
+		}
 	}
-	set_irq_flags(irq, IRQF_VALID);
+
+	tegra->ehci = hcd_to_ehci(hcd);
 
 #ifdef CONFIG_USB_OTG_UTILS
-	if (pdata->operating_mode == TEGRA_USB_OTG) {
+	if (tegra_usb_phy_otg_supported(tegra->phy)) {
 		tegra->transceiver = otg_get_transceiver();
 		if (tegra->transceiver)
 			otg_set_host(tegra->transceiver, &hcd->self);
 	}
 #endif
-
-	err = usb_add_hcd(hcd, irq, IRQF_DISABLED | IRQF_SHARED);
-	if (err) {
-		dev_err(&pdev->dev, "Failed to add USB HCD\n");
-		goto fail;
-	}
-
 	return err;
 
-fail:
-#ifdef CONFIG_USB_OTG_UTILS
-	if (tegra->transceiver) {
-		otg_set_host(tegra->transceiver, NULL);
-		otg_put_transceiver(tegra->transceiver);
-	}
-#endif
-	tegra_usb_phy_close(tegra->phy);
 fail_phy:
+	tegra_usb_phy_close(tegra->phy);
+fail_irq:
 	iounmap(hcd->regs);
 fail_io:
-	clk_disable(tegra->emc_clk);
-	clk_put(tegra->emc_clk);
-fail_emc_clk:
-	clk_disable(tegra->clk);
-fail_clken:
-	clk_put(tegra->clk);
-fail_clk:
 	usb_put_hcd(hcd);
-fail_hcd:
-	kfree(tegra);
+
 	return err;
 }
 
+
 #ifdef CONFIG_PM
 static int tegra_ehci_resume(struct platform_device *pdev)
 {
 	struct tegra_ehci_hcd *tegra = platform_get_drvdata(pdev);
-	struct usb_hcd *hcd = ehci_to_hcd(tegra->ehci);
-
-	if (tegra->bus_suspended)
-		return 0;
 
-	return tegra_usb_resume(hcd);
+	return tegra_usb_phy_power_on(tegra->phy);
 }
 
 static int tegra_ehci_suspend(struct platform_device *pdev, pm_message_t state)
 {
 	struct tegra_ehci_hcd *tegra = platform_get_drvdata(pdev);
-	struct usb_hcd *hcd = ehci_to_hcd(tegra->ehci);
-
-	if (tegra->bus_suspended)
-		return 0;
 
-	if (time_before(jiffies, tegra->ehci->next_statechange))
-		msleep(10);
-
-	return tegra_usb_suspend(hcd);
+	/* bus suspend could have failed because of remote wakeup resume */
+	if (tegra->bus_suspended_fail)
+		return -EBUSY;
+	else
+		return tegra_usb_phy_power_off(tegra->phy);
 }
 #endif
 
 static int tegra_ehci_remove(struct platform_device *pdev)
 {
 	struct tegra_ehci_hcd *tegra = platform_get_drvdata(pdev);
+	struct tegra_usb_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	struct usb_hcd *hcd = ehci_to_hcd(tegra->ehci);
 
 	if (tegra == NULL || hcd == NULL)
@@ -748,19 +662,19 @@ static int tegra_ehci_remove(struct platform_device *pdev)
 	}
 #endif
 
-	usb_remove_hcd(hcd);
-	usb_put_hcd(hcd);
+	if (tegra->irq && pdata->u_data.host.remote_wakeup_supported)
+		disable_irq_wake(tegra->irq);
+
+	/* Make sure phy is powered ON to access USB register */
+	if(!tegra_usb_phy_hw_accessible(tegra->phy))
+		tegra_usb_phy_power_on(tegra->phy);
 
+	usb_remove_hcd(hcd);
+	tegra_usb_phy_power_off(tegra->phy);
 	tegra_usb_phy_close(tegra->phy);
 	iounmap(hcd->regs);
+	usb_put_hcd(hcd);
 
-	clk_disable(tegra->clk);
-	clk_put(tegra->clk);
-
-	clk_disable(tegra->emc_clk);
-	clk_put(tegra->emc_clk);
-
-	kfree(tegra);
 	return 0;
 }
 
@@ -775,13 +689,13 @@ static void tegra_ehci_hcd_shutdown(struct platform_device *pdev)
 
 static struct platform_driver tegra_ehci_driver = {
 	.probe		= tegra_ehci_probe,
-	.remove		= tegra_ehci_remove,
+	.remove	= tegra_ehci_remove,
+	.shutdown	= tegra_ehci_hcd_shutdown,
 #ifdef CONFIG_PM
-	.suspend	= tegra_ehci_suspend,
-	.resume		= tegra_ehci_resume,
+	.suspend = tegra_ehci_suspend,
+	.resume  = tegra_ehci_resume,
 #endif
-	.shutdown	= tegra_ehci_hcd_shutdown,
-	.driver		= {
-		.name	= "tegra-ehci",
+	.driver	= {
+		.name	= driver_name,
 	}
 };
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index cc7d337ec355..569cdf5a4c03 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -139,6 +139,12 @@ struct ehci_hcd {			/* one per controller */
 	unsigned		fs_i_thresh:1;	/* Intel iso scheduling */
 	unsigned		use_dummy_qh:1;	/* AMD Frame List table quirk*/
 	unsigned		has_synopsys_hc_bug:1; /* Synopsys HC */
+	unsigned		frame_index_bug:1; /* MosChip (AKA NetMos) */
+#ifdef CONFIG_USB_EHCI_TEGRA
+	unsigned		controller_resets_phy:1;
+	unsigned		controller_remote_wakeup:1;
+	unsigned		broken_hostpc_phcd:1;
+#endif
 
 	/* required for usb32 quirk */
 	#define OHCI_CTRL_HCFS          (3 << 6)
@@ -165,10 +171,6 @@ struct ehci_hcd {			/* one per controller */
 #ifdef DEBUG
 	struct dentry		*debug_dir;
 #endif
-	/*
-	 * OTG controllers and transceivers need software interaction
-	 */
-	struct otg_transceiver	*transceiver;
 };
 
 /* convert between an HCD pointer and the corresponding EHCI_HCD */
@@ -740,6 +742,61 @@ static inline u32 hc32_to_cpup (const struct ehci_hcd *ehci, const __hc32 *x)
 
 /*-------------------------------------------------------------------------*/
 
+#ifdef CONFIG_PCI
+
+/* For working around the MosChip frame-index-register bug */
+static unsigned ehci_read_frame_index(struct ehci_hcd *ehci);
+
+#else
+
+static inline unsigned ehci_read_frame_index(struct ehci_hcd *ehci)
+{
+	return ehci_readl(ehci, &ehci->regs->frame_index);
+}
+
+#endif
+
+/*
+ * Writing to dma coherent memory on ARM may be delayed via L2
+ * writing buffer, so introduce the helper which can flush L2 writing
+ * buffer into memory immediately, especially used to flush ehci
+ * descriptor to memory.
+ * */
+#ifdef	CONFIG_ARM_DMA_MEM_BUFFERABLE
+static inline void ehci_sync_mem(void)
+{
+	mb();
+}
+
+/*
+ * DMA coherent memory on ARM which features speculative prefetcher doesn't
+ * guarantee coherency, so introduce the helpers which can invalidate QH and
+ * QTD in L1/L2 cache. It enforces CPU reads from memory directly.
+ */
+static inline void ehci_sync_qh(struct ehci_hcd *ehci, struct ehci_qh *qh)
+{
+	dma_sync_single_for_cpu(ehci_to_hcd(ehci)->self.controller, qh->qh_dma,
+		sizeof(struct ehci_qh_hw), DMA_FROM_DEVICE);
+}
+static inline void ehci_sync_qtd(struct ehci_hcd *ehci, struct ehci_qtd *qtd)
+{
+	dma_sync_single_for_cpu(ehci_to_hcd(ehci)->self.controller,
+		qtd->qtd_dma, sizeof(struct ehci_qtd), DMA_FROM_DEVICE);
+}
+#else
+static inline void ehci_sync_mem()
+{
+}
+static inline void ehci_sync_qh(struct ehci_hcd *ehci, struct ehci_qh *qh)
+{
+}
+static inline void ehci_sync_qtd(struct ehci_hcd *ehci, struct ehci_qtd *qtd)
+{
+}
+#endif
+
+/*-------------------------------------------------------------------------*/
+
 #ifndef DEBUG
 #define STUB_DEBUG_FILES
 #endif	/* DEBUG */
diff --git a/drivers/usb/host/fhci-sched.c b/drivers/usb/host/fhci-sched.c
index a42ef380e917..2df851b4bc7c 100644
--- a/drivers/usb/host/fhci-sched.c
+++ b/drivers/usb/host/fhci-sched.c
@@ -1,7 +1,7 @@
 /*
  * Freescale QUICC Engine USB Host Controller Driver
  *
- * Copyright (c) Freescale Semicondutor, Inc. 2006.
+ * Copyright (c) Freescale Semicondutor, Inc. 2006, 2011.
  *               Shlomi Gridish <gridish@freescale.com>
  *               Jerry Huang <Chang-Ming.Huang@freescale.com>
  * Copyright (c) Logic Product Development, Inc. 2007
@@ -810,9 +810,11 @@ void fhci_queue_urb(struct fhci_hcd *fhci, struct urb *urb)
 		ed->dev_addr = usb_pipedevice(urb->pipe);
 		ed->max_pkt_size = usb_maxpacket(urb->dev, urb->pipe,
 			usb_pipeout(urb->pipe));
+		/* setup stage */
 		td = fhci_td_fill(fhci, urb, urb_priv, ed, cnt++, FHCI_TA_SETUP,
 			USB_TD_TOGGLE_DATA0, urb->setup_packet, 8, 0, 0, true);
 
+		/* data stage */
 		if (data_len > 0) {
 			td = fhci_td_fill(fhci, urb, urb_priv, ed, cnt++,
 				usb_pipeout(urb->pipe) ? FHCI_TA_OUT :
@@ -820,9 +822,18 @@ void fhci_queue_urb(struct fhci_hcd *fhci, struct urb *urb)
 				USB_TD_TOGGLE_DATA1, data, data_len, 0, 0,
 				true);
 		}
-		td = fhci_td_fill(fhci, urb, urb_priv, ed, cnt++,
-			usb_pipeout(urb->pipe) ? FHCI_TA_IN : FHCI_TA_OUT,
-			USB_TD_TOGGLE_DATA1, data, 0, 0, 0, true);
+
+		/* status stage */
+		if (data_len > 0)
+			td = fhci_td_fill(fhci, urb, urb_priv, ed, cnt++,
+				(usb_pipeout(urb->pipe) ? FHCI_TA_IN :
+							  FHCI_TA_OUT),
+				USB_TD_TOGGLE_DATA1, data, 0, 0, 0, true);
+		else
+			 td = fhci_td_fill(fhci, urb, urb_priv, ed, cnt++,
+				FHCI_TA_IN,
+				USB_TD_TOGGLE_DATA1, data, 0, 0, 0, true);
+
 		urb_state = US_CTRL_SETUP;
 		break;
 	case FHCI_TF_ISO:
diff --git a/drivers/usb/host/ohci-hcd.c b/drivers/usb/host/ohci-hcd.c
index f9cf3f04b742..23107e230530 100644
--- a/drivers/usb/host/ohci-hcd.c
+++ b/drivers/usb/host/ohci-hcd.c
@@ -389,17 +389,14 @@ ohci_shutdown (struct usb_hcd *hcd)
 	struct ohci_hcd *ohci;
 
 	ohci = hcd_to_ohci (hcd);
-	ohci_writel (ohci, OHCI_INTR_MIE, &ohci->regs->intrdisable);
-	ohci->hc_control = ohci_readl(ohci, &ohci->regs->control);
+	ohci_writel(ohci, (u32) ~0, &ohci->regs->intrdisable);
 
-	/* If the SHUTDOWN quirk is set, don't put the controller in RESET */
-	ohci->hc_control &= (ohci->flags & OHCI_QUIRK_SHUTDOWN ?
-			OHCI_CTRL_RWC | OHCI_CTRL_HCFS :
-			OHCI_CTRL_RWC);
-	ohci_writel(ohci, ohci->hc_control, &ohci->regs->control);
+	/* Software reset, after which the controller goes into SUSPEND */
+	ohci_writel(ohci, OHCI_HCR, &ohci->regs->cmdstatus);
+	ohci_readl(ohci, &ohci->regs->cmdstatus);	/* flush the writes */
+	udelay(10);
 
-	/* flush the writes */
-	(void) ohci_readl (ohci, &ohci->regs->control);
+	ohci_writel(ohci, ohci->fminterval, &ohci->regs->fminterval);
 }
 
 static int check_ed(struct ohci_hcd *ohci, struct ed *ed)
diff --git a/drivers/usb/host/ohci-hub.c b/drivers/usb/host/ohci-hub.c
index 9154615292db..2f00040fc408 100644
--- a/drivers/usb/host/ohci-hub.c
+++ b/drivers/usb/host/ohci-hub.c
@@ -356,10 +356,7 @@ static void ohci_finish_controller_resume(struct usb_hcd *hcd)
 		msleep(20);
 	}
 
-	/* Does the root hub have a port wakeup pending? */
-	if (ohci_readl(ohci, &ohci->regs->intrstatus) &
-			(OHCI_INTR_RD | OHCI_INTR_RHSC))
-		usb_hcd_resume_root_hub(hcd);
+	usb_hcd_resume_root_hub(hcd);
 }
 
 /* Carry out polling-, autostop-, and autoresume-related state changes */
diff --git a/drivers/usb/host/ohci-pci.c b/drivers/usb/host/ohci-pci.c
index ad8166c681e2..bc01b064585a 100644
--- a/drivers/usb/host/ohci-pci.c
+++ b/drivers/usb/host/ohci-pci.c
@@ -175,28 +175,6 @@ static int ohci_quirk_amd700(struct usb_hcd *hcd)
 	return 0;
 }
 
-/* nVidia controllers continue to drive Reset signalling on the bus
- * even after system shutdown, wasting power.  This flag tells the
- * shutdown routine to leave the controller OPERATIONAL instead of RESET.
- */
-static int ohci_quirk_nvidia_shutdown(struct usb_hcd *hcd)
-{
-	struct pci_dev *pdev = to_pci_dev(hcd->self.controller);
-	struct ohci_hcd	*ohci = hcd_to_ohci(hcd);
-
-	/* Evidently nVidia fixed their later hardware; this is a guess at
-	 * the changeover point.
-	 */
-#define PCI_DEVICE_ID_NVIDIA_NFORCE_MCP51_USB		0x026d
-
-	if (pdev->device < PCI_DEVICE_ID_NVIDIA_NFORCE_MCP51_USB) {
-		ohci->flags |= OHCI_QUIRK_SHUTDOWN;
-		ohci_dbg(ohci, "enabled nVidia shutdown quirk\n");
-	}
-
-	return 0;
-}
-
 static void sb800_prefetch(struct ohci_hcd *ohci, int on)
 {
 	struct pci_dev *pdev;
@@ -260,10 +238,6 @@ static const struct pci_device_id ohci_pci_quirks[] = {
 		PCI_DEVICE(PCI_VENDOR_ID_ATI, 0x4399),
 		.driver_data = (unsigned long)ohci_quirk_amd700,
 	},
-	{
-		PCI_DEVICE(PCI_VENDOR_ID_NVIDIA, PCI_ANY_ID),
-		.driver_data = (unsigned long) ohci_quirk_nvidia_shutdown,
-	},
 
 	/* FIXME for some of the early AMD 760 southbridges, OHCI
 	 * won't work at all.  blacklist them.
diff --git a/drivers/usb/host/ohci.h b/drivers/usb/host/ohci.h
index 35e5fd640ce7..0795b934d00c 100644
--- a/drivers/usb/host/ohci.h
+++ b/drivers/usb/host/ohci.h
@@ -403,7 +403,6 @@ struct ohci_hcd {
 #define	OHCI_QUIRK_HUB_POWER	0x100			/* distrust firmware power/oc setup */
 #define	OHCI_QUIRK_AMD_PLL	0x200			/* AMD PLL quirk*/
 #define	OHCI_QUIRK_AMD_PREFETCH	0x400			/* pre-fetch for ISO transfer */
-#define	OHCI_QUIRK_SHUTDOWN	0x800			/* nVidia power bug */
 	// there are also chip quirks/bugs in init logic
 
 	struct work_struct	nec_work;	/* Worker for NEC quirk */
diff --git a/drivers/usb/host/pci-quirks.c b/drivers/usb/host/pci-quirks.c
index 629a96813fd6..23e04fb038ba 100644
--- a/drivers/usb/host/pci-quirks.c
+++ b/drivers/usb/host/pci-quirks.c
@@ -36,6 +36,7 @@
 #define OHCI_INTRENABLE		0x10
 #define OHCI_INTRDISABLE	0x14
 #define OHCI_FMINTERVAL		0x34
+#define OHCI_HCFS		(3 << 6)	/* hc functional state */
 #define OHCI_HCR		(1 << 0)	/* host controller reset */
 #define OHCI_OCR		(1 << 3)	/* ownership change request */
 #define OHCI_CTRL_RWC		(1 << 9)	/* remote wakeup connected */
@@ -465,6 +466,8 @@ static void __devinit quirk_usb_handoff_ohci(struct pci_dev *pdev)
 {
 	void __iomem *base;
 	u32 control;
+	u32 fminterval;
+	int cnt;
 
 	if (!mmio_resource_enabled(pdev, 0))
 		return;
@@ -497,41 +500,32 @@ static void __devinit quirk_usb_handoff_ohci(struct pci_dev *pdev)
 	}
 #endif
 
-	/* reset controller, preserving RWC (and possibly IR) */
-	writel(control & OHCI_CTRL_MASK, base + OHCI_CONTROL);
-	readl(base + OHCI_CONTROL);
+	/* disable interrupts */
+	writel((u32) ~0, base + OHCI_INTRDISABLE);
 
-	/* Some NVIDIA controllers stop working if kept in RESET for too long */
-	if (pdev->vendor == PCI_VENDOR_ID_NVIDIA) {
-		u32 fminterval;
-		int cnt;
+	/* Reset the USB bus, if the controller isn't already in RESET */
+	if (control & OHCI_HCFS) {
+		/* Go into RESET, preserving RWC (and possibly IR) */
+		writel(control & OHCI_CTRL_MASK, base + OHCI_CONTROL);
+		readl(base + OHCI_CONTROL);
 
-		/* drive reset for at least 50 ms (7.1.7.5) */
+		/* drive bus reset for at least 50 ms (7.1.7.5) */
 		msleep(50);
+	}
 
-		/* software reset of the controller, preserving HcFmInterval */
-		fminterval = readl(base + OHCI_FMINTERVAL);
-		writel(OHCI_HCR, base + OHCI_CMDSTATUS);
+	/* software reset of the controller, preserving HcFmInterval */
+	fminterval = readl(base + OHCI_FMINTERVAL);
+	writel(OHCI_HCR, base + OHCI_CMDSTATUS);
 
-		/* reset requires max 10 us delay */
-		for (cnt = 30; cnt > 0; --cnt) {	/* ... allow extra time */
-			if ((readl(base + OHCI_CMDSTATUS) & OHCI_HCR) == 0)
-				break;
-			udelay(1);
-		}
-		writel(fminterval, base + OHCI_FMINTERVAL);
-
-		/* Now we're in the SUSPEND state with all devices reset
-		 * and wakeups and interrupts disabled
-		 */
+	/* reset requires max 10 us delay */
+	for (cnt = 30; cnt > 0; --cnt) {	/* ... allow extra time */
+		if ((readl(base + OHCI_CMDSTATUS) & OHCI_HCR) == 0)
+			break;
+		udelay(1);
 	}
+	writel(fminterval, base + OHCI_FMINTERVAL);
 
-	/*
-	 * disable interrupts
-	 */
-	writel(~(u32)0, base + OHCI_INTRDISABLE);
-	writel(~(u32)0, base + OHCI_INTRSTATUS);
-
+	/* Now the controller is safely in SUSPEND and nothing can wake it up */
 	iounmap(base);
 }
 
@@ -626,7 +620,7 @@ static void __devinit quirk_usb_disable_ehci(struct pci_dev *pdev)
 	void __iomem *base, *op_reg_base;
 	u32	hcc_params, cap, val;
 	u8	offset, cap_length;
-	int	wait_time, delta, count = 256/4;
+	int	wait_time, count = 256/4;
 
 	if (!mmio_resource_enabled(pdev, 0))
 		return;
@@ -672,11 +666,10 @@ static void __devinit quirk_usb_disable_ehci(struct pci_dev *pdev)
 		writel(val, op_reg_base + EHCI_USBCMD);
 
 		wait_time = 2000;
-		delta = 100;
 		do {
 			writel(0x3f, op_reg_base + EHCI_USBSTS);
-			udelay(delta);
-			wait_time -= delta;
+			udelay(100);
+			wait_time -= 100;
 			val = readl(op_reg_base + EHCI_USBSTS);
 			if ((val == ~(u32)0) || (val & EHCI_USBSTS_HALTED)) {
 				break;
diff --git a/drivers/usb/host/uhci-q.c b/drivers/usb/host/uhci-q.c
index 84ed28b34f93..82539913ad84 100644
--- a/drivers/usb/host/uhci-q.c
+++ b/drivers/usb/host/uhci-q.c
@@ -943,7 +943,7 @@ static int uhci_submit_common(struct uhci_hcd *uhci, struct urb *urb,
 	if (usb_pipein(urb->pipe))
 		status |= TD_CTRL_SPD;
 
-	i = urb->num_sgs;
+	i = urb->num_mapped_sgs;
 	if (len > 0 && i > 0) {
 		sg = urb->sg;
 		data = sg_dma_address(sg);
diff --git a/drivers/usb/host/whci/qset.c b/drivers/usb/host/whci/qset.c
index d6e175428618..76083ae92138 100644
--- a/drivers/usb/host/whci/qset.c
+++ b/drivers/usb/host/whci/qset.c
@@ -124,7 +124,7 @@ void qset_clear(struct whc *whc, struct whc_qset *qset)
 {
 	qset->td_start = qset->td_end = qset->ntds = 0;
 
-	qset->qh.link = cpu_to_le32(QH_LINK_NTDS(8) | QH_LINK_T);
+	qset->qh.link = cpu_to_le64(QH_LINK_NTDS(8) | QH_LINK_T);
 	qset->qh.status = qset->qh.status & QH_STATUS_SEQ_MASK;
 	qset->qh.err_count = 0;
 	qset->qh.scratch[0] = 0;
@@ -443,7 +443,7 @@ static int qset_add_urb_sg(struct whc *whc, struct whc_qset *qset, struct urb *u
 
 	remaining = urb->transfer_buffer_length;
 
-	for_each_sg(urb->sg, sg, urb->num_sgs, i) {
+	for_each_sg(urb->sg, sg, urb->num_mapped_sgs, i) {
 		dma_addr_t dma_addr;
 		size_t dma_remaining;
 		dma_addr_t sp, ep;
@@ -561,7 +561,7 @@ static int qset_add_urb_sg_linearize(struct whc *whc, struct whc_qset *qset,
 
 	remaining = urb->transfer_buffer_length;
 
-	for_each_sg(urb->sg, sg, urb->num_sgs, i) {
+	for_each_sg(urb->sg, sg, urb->num_mapped_sgs, i) {
 		size_t len;
 		size_t sg_remaining;
 		void *orig;
diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index 723f8231193d..ce9f974dac0f 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -392,6 +392,20 @@ static int xhci_get_ports(struct usb_hcd *hcd, __le32 __iomem ***port_array)
 	return max_ports;
 }
 
+/* Test and clear port RWC bit */
+void xhci_test_and_clear_bit(struct xhci_hcd *xhci, __le32 __iomem **port_array,
+				int port_id, u32 port_bit)
+{
+	u32 temp;
+
+	temp = xhci_readl(xhci, port_array[port_id]);
+	if (temp & port_bit) {
+		temp = xhci_port_state_to_neutral(temp);
+		temp |= port_bit;
+		xhci_writel(xhci, temp, port_array[port_id]);
+	}
+}
+
 int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 		u16 wIndex, char *buf, u16 wLength)
 {
@@ -938,12 +952,8 @@ int xhci_bus_resume(struct usb_hcd *hcd)
 			spin_lock_irqsave(&xhci->lock, flags);
 
 			/* Clear PLC */
-			temp = xhci_readl(xhci, port_array[port_index]);
-			if (temp & PORT_PLC) {
-				temp = xhci_port_state_to_neutral(temp);
-				temp |= PORT_PLC;
-				xhci_writel(xhci, temp, port_array[port_index]);
-			}
+			xhci_test_and_clear_bit(xhci, port_array, port_index,
+						PORT_PLC);
 
 			slot_id = xhci_find_slot_id_by_port(hcd,
 					xhci, port_index + 1);
diff --git a/drivers/usb/host/xhci-mem.c b/drivers/usb/host/xhci-mem.c
index d446886b22b0..d718033dc531 100644
--- a/drivers/usb/host/xhci-mem.c
+++ b/drivers/usb/host/xhci-mem.c
@@ -81,7 +81,7 @@ static void xhci_segment_free(struct xhci_hcd *xhci, struct xhci_segment *seg)
  * related flags, such as End TRB, Toggle Cycle, and no snoop.
  */
 static void xhci_link_segments(struct xhci_hcd *xhci, struct xhci_segment *prev,
-		struct xhci_segment *next, bool link_trbs)
+		struct xhci_segment *next, bool link_trbs, bool isoc)
 {
 	u32 val;
 
@@ -97,7 +97,9 @@ static void xhci_link_segments(struct xhci_hcd *xhci, struct xhci_segment *prev,
 		val &= ~TRB_TYPE_BITMASK;
 		val |= TRB_TYPE(TRB_LINK);
 		/* Always set the chain bit with 0.95 hardware */
-		if (xhci_link_trb_quirk(xhci))
+		/* Set chain bit for isoc rings on AMD 0.96 host */
+		if (xhci_link_trb_quirk(xhci) ||
+				(isoc && (xhci->quirks & XHCI_AMD_0x96_HOST)))
 			val |= TRB_CHAIN;
 		prev->trbs[TRBS_PER_SEGMENT-1].link.control = cpu_to_le32(val);
 	}
@@ -112,18 +114,20 @@ void xhci_ring_free(struct xhci_hcd *xhci, struct xhci_ring *ring)
 	struct xhci_segment *seg;
 	struct xhci_segment *first_seg;
 
-	if (!ring || !ring->first_seg)
+	if (!ring)
 		return;
-	first_seg = ring->first_seg;
-	seg = first_seg->next;
-	xhci_dbg(xhci, "Freeing ring at %p\n", ring);
-	while (seg != first_seg) {
-		struct xhci_segment *next = seg->next;
-		xhci_segment_free(xhci, seg);
-		seg = next;
+	if (ring->first_seg) {
+		first_seg = ring->first_seg;
+		seg = first_seg->next;
+		xhci_dbg(xhci, "Freeing ring at %p\n", ring);
+		while (seg != first_seg) {
+			struct xhci_segment *next = seg->next;
+			xhci_segment_free(xhci, seg);
+			seg = next;
+		}
+		xhci_segment_free(xhci, first_seg);
+		ring->first_seg = NULL;
 	}
-	xhci_segment_free(xhci, first_seg);
-	ring->first_seg = NULL;
 	kfree(ring);
 }
 
@@ -152,7 +156,7 @@ static void xhci_initialize_ring_info(struct xhci_ring *ring)
  * See section 4.9.1 and figures 15 and 16.
  */
 static struct xhci_ring *xhci_ring_alloc(struct xhci_hcd *xhci,
-		unsigned int num_segs, bool link_trbs, gfp_t flags)
+		unsigned int num_segs, bool link_trbs, bool isoc, gfp_t flags)
 {
 	struct xhci_ring	*ring;
 	struct xhci_segment	*prev;
@@ -178,12 +182,12 @@ static struct xhci_ring *xhci_ring_alloc(struct xhci_hcd *xhci,
 		next = xhci_segment_alloc(xhci, flags);
 		if (!next)
 			goto fail;
-		xhci_link_segments(xhci, prev, next, link_trbs);
+		xhci_link_segments(xhci, prev, next, link_trbs, isoc);
 
 		prev = next;
 		num_segs--;
 	}
-	xhci_link_segments(xhci, prev, ring->first_seg, link_trbs);
+	xhci_link_segments(xhci, prev, ring->first_seg, link_trbs, isoc);
 
 	if (link_trbs) {
 		/* See section 4.9.2.1 and 6.4.4.1 */
@@ -229,14 +233,14 @@ void xhci_free_or_cache_endpoint_ring(struct xhci_hcd *xhci,
  * pointers to the beginning of the ring.
  */
 static void xhci_reinit_cached_ring(struct xhci_hcd *xhci,
-		struct xhci_ring *ring)
+		struct xhci_ring *ring, bool isoc)
 {
 	struct xhci_segment	*seg = ring->first_seg;
 	do {
 		memset(seg->trbs, 0,
 				sizeof(union xhci_trb)*TRBS_PER_SEGMENT);
 		/* All endpoint rings have link TRBs */
-		xhci_link_segments(xhci, seg, seg->next, 1);
+		xhci_link_segments(xhci, seg, seg->next, 1, isoc);
 		seg = seg->next;
 	} while (seg != ring->first_seg);
 	xhci_initialize_ring_info(ring);
@@ -540,7 +544,7 @@ struct xhci_stream_info *xhci_alloc_stream_info(struct xhci_hcd *xhci,
 	 */
 	for (cur_stream = 1; cur_stream < num_streams; cur_stream++) {
 		stream_info->stream_rings[cur_stream] =
-			xhci_ring_alloc(xhci, 1, true, mem_flags);
+			xhci_ring_alloc(xhci, 1, true, false, mem_flags);
 		cur_ring = stream_info->stream_rings[cur_stream];
 		if (!cur_ring)
 			goto cleanup_rings;
@@ -765,7 +769,7 @@ int xhci_alloc_virt_device(struct xhci_hcd *xhci, int slot_id,
 	}
 
 	/* Allocate endpoint 0 ring */
-	dev->eps[0].ring = xhci_ring_alloc(xhci, 1, true, flags);
+	dev->eps[0].ring = xhci_ring_alloc(xhci, 1, true, false, flags);
 	if (!dev->eps[0].ring)
 		goto fail;
 
@@ -871,7 +875,6 @@ int xhci_setup_addressable_virt_dev(struct xhci_hcd *xhci, struct usb_device *ud
 	struct xhci_virt_device *dev;
 	struct xhci_ep_ctx	*ep0_ctx;
 	struct xhci_slot_ctx    *slot_ctx;
-	struct xhci_input_control_ctx *ctrl_ctx;
 	u32			port_num;
 	struct usb_device *top_dev;
 
@@ -883,12 +886,8 @@ int xhci_setup_addressable_virt_dev(struct xhci_hcd *xhci, struct usb_device *ud
 		return -EINVAL;
 	}
 	ep0_ctx = xhci_get_ep_ctx(xhci, dev->in_ctx, 0);
-	ctrl_ctx = xhci_get_input_control_ctx(xhci, dev->in_ctx);
 	slot_ctx = xhci_get_slot_ctx(xhci, dev->in_ctx);
 
-	/* 2) New slot context and endpoint 0 context are valid*/
-	ctrl_ctx->add_flags = cpu_to_le32(SLOT_FLAG | EP0_FLAG);
-
 	/* 3) Only the control endpoint is valid - one endpoint context */
 	slot_ctx->dev_info |= cpu_to_le32(LAST_CTX(1) | udev->route);
 	switch (udev->speed) {
@@ -1175,10 +1174,10 @@ int xhci_endpoint_init(struct xhci_hcd *xhci,
 	 */
 	if (usb_endpoint_xfer_isoc(&ep->desc))
 		virt_dev->eps[ep_index].new_ring =
-			xhci_ring_alloc(xhci, 8, true, mem_flags);
+			xhci_ring_alloc(xhci, 8, true, true, mem_flags);
 	else
 		virt_dev->eps[ep_index].new_ring =
-			xhci_ring_alloc(xhci, 1, true, mem_flags);
+			xhci_ring_alloc(xhci, 1, true, false, mem_flags);
 	if (!virt_dev->eps[ep_index].new_ring) {
 		/* Attempt to use the ring cache */
 		if (virt_dev->num_rings_cached == 0)
@@ -1187,7 +1186,8 @@ int xhci_endpoint_init(struct xhci_hcd *xhci,
 			virt_dev->ring_cache[virt_dev->num_rings_cached];
 		virt_dev->ring_cache[virt_dev->num_rings_cached] = NULL;
 		virt_dev->num_rings_cached--;
-		xhci_reinit_cached_ring(xhci, virt_dev->eps[ep_index].new_ring);
+		xhci_reinit_cached_ring(xhci, virt_dev->eps[ep_index].new_ring,
+			usb_endpoint_xfer_isoc(&ep->desc) ? true : false);
 	}
 	virt_dev->eps[ep_index].skip = false;
 	ep_ring = virt_dev->eps[ep_index].new_ring;
@@ -2001,7 +2001,7 @@ int xhci_mem_init(struct xhci_hcd *xhci, gfp_t flags)
 		goto fail;
 
 	/* Set up the command ring to have one segments for now. */
-	xhci->cmd_ring = xhci_ring_alloc(xhci, 1, true, flags);
+	xhci->cmd_ring = xhci_ring_alloc(xhci, 1, true, false, flags);
 	if (!xhci->cmd_ring)
 		goto fail;
 	xhci_dbg(xhci, "Allocated command ring at %p\n", xhci->cmd_ring);
@@ -2032,7 +2032,8 @@ int xhci_mem_init(struct xhci_hcd *xhci, gfp_t flags)
 	 * the event ring segment table (ERST).  Section 4.9.3.
 	 */
 	xhci_dbg(xhci, "// Allocating event ring\n");
-	xhci->event_ring = xhci_ring_alloc(xhci, ERST_NUM_SEGS, false, flags);
+	xhci->event_ring = xhci_ring_alloc(xhci, ERST_NUM_SEGS, false, false,
+						flags);
 	if (!xhci->event_ring)
 		goto fail;
 	if (xhci_check_trb_in_td_math(xhci, flags) < 0)
diff --git a/drivers/usb/host/xhci-pci.c b/drivers/usb/host/xhci-pci.c
index cb16de213f64..50e7156a7d81 100644
--- a/drivers/usb/host/xhci-pci.c
+++ b/drivers/usb/host/xhci-pci.c
@@ -128,6 +128,9 @@ static int xhci_pci_setup(struct usb_hcd *hcd)
 	if (pdev->vendor == PCI_VENDOR_ID_NEC)
 		xhci->quirks |= XHCI_NEC_HOST;
 
+	if (pdev->vendor == PCI_VENDOR_ID_AMD && xhci->hci_version == 0x96)
+		xhci->quirks |= XHCI_AMD_0x96_HOST;
+
 	/* AMD PLL quirk */
 	if (pdev->vendor == PCI_VENDOR_ID_AMD && usb_amd_find_chipset_info())
 		xhci->quirks |= XHCI_AMD_PLL_FIX;
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 952e2ded61af..ae99e3b32fb7 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -185,7 +185,7 @@ static void inc_deq(struct xhci_hcd *xhci, struct xhci_ring *ring, bool consumer
  *			prepare_transfer()?
  */
 static void inc_enq(struct xhci_hcd *xhci, struct xhci_ring *ring,
-		bool consumer, bool more_trbs_coming)
+		bool consumer, bool more_trbs_coming, bool isoc)
 {
 	u32 chain;
 	union xhci_trb *next;
@@ -212,11 +212,13 @@ static void inc_enq(struct xhci_hcd *xhci, struct xhci_ring *ring,
 				if (!chain && !more_trbs_coming)
 					break;
 
-				/* If we're not dealing with 0.95 hardware,
+				/* If we're not dealing with 0.95 hardware or
+				 * isoc rings on AMD 0.96 host,
 				 * carry over the chain bit of the previous TRB
 				 * (which may mean the chain bit is cleared).
 				 */
-				if (!xhci_link_trb_quirk(xhci)) {
+				if (!(isoc && (xhci->quirks & XHCI_AMD_0x96_HOST))
+						&& !xhci_link_trb_quirk(xhci)) {
 					next->link.control &=
 						cpu_to_le32(~TRB_CHAIN);
 					next->link.control |=
@@ -814,23 +816,24 @@ void xhci_stop_endpoint_command_watchdog(unsigned long arg)
 	struct xhci_ring *ring;
 	struct xhci_td *cur_td;
 	int ret, i, j;
+	unsigned long flags;
 
 	ep = (struct xhci_virt_ep *) arg;
 	xhci = ep->xhci;
 
-	spin_lock(&xhci->lock);
+	spin_lock_irqsave(&xhci->lock, flags);
 
 	ep->stop_cmds_pending--;
 	if (xhci->xhc_state & XHCI_STATE_DYING) {
 		xhci_dbg(xhci, "Stop EP timer ran, but another timer marked "
 				"xHCI as DYING, exiting.\n");
-		spin_unlock(&xhci->lock);
+		spin_unlock_irqrestore(&xhci->lock, flags);
 		return;
 	}
 	if (!(ep->stop_cmds_pending == 0 && (ep->ep_state & EP_HALT_PENDING))) {
 		xhci_dbg(xhci, "Stop EP timer ran, but no command pending, "
 				"exiting.\n");
-		spin_unlock(&xhci->lock);
+		spin_unlock_irqrestore(&xhci->lock, flags);
 		return;
 	}
 
@@ -842,11 +845,11 @@ void xhci_stop_endpoint_command_watchdog(unsigned long arg)
 	xhci->xhc_state |= XHCI_STATE_DYING;
 	/* Disable interrupts from the host controller and start halting it */
 	xhci_quiesce(xhci);
-	spin_unlock(&xhci->lock);
+	spin_unlock_irqrestore(&xhci->lock, flags);
 
 	ret = xhci_halt(xhci);
 
-	spin_lock(&xhci->lock);
+	spin_lock_irqsave(&xhci->lock, flags);
 	if (ret < 0) {
 		/* This is bad; the host is not responding to commands and it's
 		 * not allowing itself to be halted.  At least interrupts are
@@ -894,7 +897,7 @@ void xhci_stop_endpoint_command_watchdog(unsigned long arg)
 			}
 		}
 	}
-	spin_unlock(&xhci->lock);
+	spin_unlock_irqrestore(&xhci->lock, flags);
 	xhci_dbg(xhci, "Calling usb_hc_died()\n");
 	usb_hc_died(xhci_to_hcd(xhci)->primary_hcd);
 	xhci_dbg(xhci, "xHCI host controller is dead.\n");
@@ -1212,6 +1215,7 @@ static void handle_vendor_event(struct xhci_hcd *xhci,
  *
  * Returns a zero-based port number, which is suitable for indexing into each of
  * the split roothubs' port arrays and bus state arrays.
+ * Add one to it in order to call xhci_find_slot_id_by_port.
  */
 static unsigned int find_faked_portnum_from_hw_portnum(struct usb_hcd *hcd,
 		struct xhci_hcd *xhci, u32 port_id)
@@ -1334,7 +1338,7 @@ static void handle_port_status(struct xhci_hcd *xhci,
 			temp |= PORT_LINK_STROBE | XDEV_U0;
 			xhci_writel(xhci, temp, port_array[faked_port_index]);
 			slot_id = xhci_find_slot_id_by_port(hcd, xhci,
-					faked_port_index);
+					faked_port_index + 1);
 			if (!slot_id) {
 				xhci_dbg(xhci, "slot_id is zero\n");
 				goto cleanup;
@@ -1342,10 +1346,8 @@ static void handle_port_status(struct xhci_hcd *xhci,
 			xhci_ring_device(xhci, slot_id);
 			xhci_dbg(xhci, "resume SS port %d finished\n", port_id);
 			/* Clear PORT_PLC */
-			temp = xhci_readl(xhci, port_array[faked_port_index]);
-			temp = xhci_port_state_to_neutral(temp);
-			temp |= PORT_PLC;
-			xhci_writel(xhci, temp, port_array[faked_port_index]);
+			xhci_test_and_clear_bit(xhci, port_array,
+						faked_port_index, PORT_PLC);
 		} else {
 			xhci_dbg(xhci, "resume HS port %d\n", port_id);
 			bus_state->resume_done[faked_port_index] = jiffies +
@@ -1356,6 +1358,10 @@ static void handle_port_status(struct xhci_hcd *xhci,
 		}
 	}
 
+	if (hcd->speed != HCD_USB3)
+		xhci_test_and_clear_bit(xhci, port_array, faked_port_index,
+					PORT_PLC);
+
 cleanup:
 	/* Update event ring dequeue pointer before dropping the lock */
 	inc_deq(xhci, xhci->event_ring, true);
@@ -2409,7 +2415,7 @@ irqreturn_t xhci_msi_irq(int irq, struct usb_hcd *hcd)
  *			prepare_transfer()?
  */
 static void queue_trb(struct xhci_hcd *xhci, struct xhci_ring *ring,
-		bool consumer, bool more_trbs_coming,
+		bool consumer, bool more_trbs_coming, bool isoc,
 		u32 field1, u32 field2, u32 field3, u32 field4)
 {
 	struct xhci_generic_trb *trb;
@@ -2419,7 +2425,7 @@ static void queue_trb(struct xhci_hcd *xhci, struct xhci_ring *ring,
 	trb->field[1] = cpu_to_le32(field2);
 	trb->field[2] = cpu_to_le32(field3);
 	trb->field[3] = cpu_to_le32(field4);
-	inc_enq(xhci, ring, consumer, more_trbs_coming);
+	inc_enq(xhci, ring, consumer, more_trbs_coming, isoc);
 }
 
 /*
@@ -2427,7 +2433,7 @@ static void queue_trb(struct xhci_hcd *xhci, struct xhci_ring *ring,
  * FIXME allocate segments if the ring is full.
  */
 static int prepare_ring(struct xhci_hcd *xhci, struct xhci_ring *ep_ring,
-		u32 ep_state, unsigned int num_trbs, gfp_t mem_flags)
+		u32 ep_state, unsigned int num_trbs, bool isoc, gfp_t mem_flags)
 {
 	/* Make sure the endpoint has been added to xHC schedule */
 	switch (ep_state) {
@@ -2469,10 +2475,11 @@ static int prepare_ring(struct xhci_hcd *xhci, struct xhci_ring *ep_ring,
 		next = ring->enqueue;
 
 		while (last_trb(xhci, ring, ring->enq_seg, next)) {
-			/* If we're not dealing with 0.95 hardware,
-			 * clear the chain bit.
+			/* If we're not dealing with 0.95 hardware or isoc rings
+			 * on AMD 0.96 host, clear the chain bit.
 			 */
-			if (!xhci_link_trb_quirk(xhci))
+			if (!xhci_link_trb_quirk(xhci) && !(isoc &&
+					(xhci->quirks & XHCI_AMD_0x96_HOST)))
 				next->link.control &= cpu_to_le32(~TRB_CHAIN);
 			else
 				next->link.control |= cpu_to_le32(TRB_CHAIN);
@@ -2505,6 +2512,7 @@ static int prepare_transfer(struct xhci_hcd *xhci,
 		unsigned int num_trbs,
 		struct urb *urb,
 		unsigned int td_index,
+		bool isoc,
 		gfp_t mem_flags)
 {
 	int ret;
@@ -2522,7 +2530,7 @@ static int prepare_transfer(struct xhci_hcd *xhci,
 
 	ret = prepare_ring(xhci, ep_ring,
 			   le32_to_cpu(ep_ctx->ep_info) & EP_STATE_MASK,
-			   num_trbs, mem_flags);
+			   num_trbs, isoc, mem_flags);
 	if (ret)
 		return ret;
 
@@ -2555,7 +2563,7 @@ static unsigned int count_sg_trbs_needed(struct xhci_hcd *xhci, struct urb *urb)
 	struct scatterlist *sg;
 
 	sg = NULL;
-	num_sgs = urb->num_sgs;
+	num_sgs = urb->num_mapped_sgs;
 	temp = urb->transfer_buffer_length;
 
 	xhci_dbg(xhci, "count sg list trbs: \n");
@@ -2739,13 +2747,13 @@ static int queue_bulk_sg_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 		return -EINVAL;
 
 	num_trbs = count_sg_trbs_needed(xhci, urb);
-	num_sgs = urb->num_sgs;
+	num_sgs = urb->num_mapped_sgs;
 	total_packet_count = roundup(urb->transfer_buffer_length,
 			le16_to_cpu(urb->ep->desc.wMaxPacketSize));
 
 	trb_buff_len = prepare_transfer(xhci, xhci->devs[slot_id],
 			ep_index, urb->stream_id,
-			num_trbs, urb, 0, mem_flags);
+			num_trbs, urb, 0, false, mem_flags);
 	if (trb_buff_len < 0)
 		return trb_buff_len;
 
@@ -2840,7 +2848,7 @@ static int queue_bulk_sg_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 			more_trbs_coming = true;
 		else
 			more_trbs_coming = false;
-		queue_trb(xhci, ep_ring, false, more_trbs_coming,
+		queue_trb(xhci, ep_ring, false, more_trbs_coming, false,
 				lower_32_bits(addr),
 				upper_32_bits(addr),
 				length_field,
@@ -2931,7 +2939,7 @@ int xhci_queue_bulk_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 
 	ret = prepare_transfer(xhci, xhci->devs[slot_id],
 			ep_index, urb->stream_id,
-			num_trbs, urb, 0, mem_flags);
+			num_trbs, urb, 0, false, mem_flags);
 	if (ret < 0)
 		return ret;
 
@@ -3003,7 +3011,7 @@ int xhci_queue_bulk_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 			more_trbs_coming = true;
 		else
 			more_trbs_coming = false;
-		queue_trb(xhci, ep_ring, false, more_trbs_coming,
+		queue_trb(xhci, ep_ring, false, more_trbs_coming, false,
 				lower_32_bits(addr),
 				upper_32_bits(addr),
 				length_field,
@@ -3063,7 +3071,7 @@ int xhci_queue_ctrl_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 		num_trbs++;
 	ret = prepare_transfer(xhci, xhci->devs[slot_id],
 			ep_index, urb->stream_id,
-			num_trbs, urb, 0, mem_flags);
+			num_trbs, urb, 0, false, mem_flags);
 	if (ret < 0)
 		return ret;
 
@@ -3096,7 +3104,7 @@ int xhci_queue_ctrl_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 		}
 	}
 
-	queue_trb(xhci, ep_ring, false, true,
+	queue_trb(xhci, ep_ring, false, true, false,
 		  setup->bRequestType | setup->bRequest << 8 | le16_to_cpu(setup->wValue) << 16,
 		  le16_to_cpu(setup->wIndex) | le16_to_cpu(setup->wLength) << 16,
 		  TRB_LEN(8) | TRB_INTR_TARGET(0),
@@ -3116,7 +3124,7 @@ int xhci_queue_ctrl_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 	if (urb->transfer_buffer_length > 0) {
 		if (setup->bRequestType & USB_DIR_IN)
 			field |= TRB_DIR_IN;
-		queue_trb(xhci, ep_ring, false, true,
+		queue_trb(xhci, ep_ring, false, true, false,
 				lower_32_bits(urb->transfer_dma),
 				upper_32_bits(urb->transfer_dma),
 				length_field,
@@ -3132,7 +3140,7 @@ int xhci_queue_ctrl_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 		field = 0;
 	else
 		field = TRB_DIR_IN;
-	queue_trb(xhci, ep_ring, false, false,
+	queue_trb(xhci, ep_ring, false, false, false,
 			0,
 			0,
 			TRB_INTR_TARGET(0),
@@ -3281,7 +3289,8 @@ static int xhci_queue_isoc_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 		trbs_per_td = count_isoc_trbs_needed(xhci, urb, i);
 
 		ret = prepare_transfer(xhci, xhci->devs[slot_id], ep_index,
-				urb->stream_id, trbs_per_td, urb, i, mem_flags);
+				urb->stream_id, trbs_per_td, urb, i, true,
+				mem_flags);
 		if (ret < 0) {
 			if (i == 0)
 				return ret;
@@ -3351,7 +3360,7 @@ static int xhci_queue_isoc_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 				remainder |
 				TRB_INTR_TARGET(0);
 
-			queue_trb(xhci, ep_ring, false, more_trbs_coming,
+			queue_trb(xhci, ep_ring, false, more_trbs_coming, true,
 				lower_32_bits(addr),
 				upper_32_bits(addr),
 				length_field,
@@ -3365,7 +3374,8 @@ static int xhci_queue_isoc_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 		/* Check TD length */
 		if (running_total != td_len) {
 			xhci_err(xhci, "ISOC TD length unmatch\n");
-			return -EINVAL;
+			ret = -EINVAL;
+			goto cleanup;
 		}
 	}
 
@@ -3433,7 +3443,7 @@ int xhci_queue_isoc_tx_prepare(struct xhci_hcd *xhci, gfp_t mem_flags,
 	 * Do not insert any td of the urb to the ring if the check failed.
 	 */
 	ret = prepare_ring(xhci, ep_ring, le32_to_cpu(ep_ctx->ep_info) & EP_STATE_MASK,
-			   num_trbs, mem_flags);
+			   num_trbs, true, mem_flags);
 	if (ret)
 		return ret;
 
@@ -3492,7 +3502,7 @@ static int queue_command(struct xhci_hcd *xhci, u32 field1, u32 field2,
 		reserved_trbs++;
 
 	ret = prepare_ring(xhci, xhci->cmd_ring, EP_STATE_RUNNING,
-			reserved_trbs, GFP_ATOMIC);
+			reserved_trbs, false, GFP_ATOMIC);
 	if (ret < 0) {
 		xhci_err(xhci, "ERR: No room for command on command ring\n");
 		if (command_must_succeed)
@@ -3500,8 +3510,8 @@ static int queue_command(struct xhci_hcd *xhci, u32 field1, u32 field2,
 					"unfailable commands failed.\n");
 		return ret;
 	}
-	queue_trb(xhci, xhci->cmd_ring, false, false, field1, field2, field3,
-			field4 | xhci->cmd_ring->cycle_state);
+	queue_trb(xhci, xhci->cmd_ring, false, false, false, field1, field2,
+			field3,	field4 | xhci->cmd_ring->cycle_state);
 	return 0;
 }
 
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index 3a0f695138f4..5fc595f01b31 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -657,7 +657,10 @@ static void xhci_clear_command_ring(struct xhci_hcd *xhci)
 	ring = xhci->cmd_ring;
 	seg = ring->deq_seg;
 	do {
-		memset(seg->trbs, 0, SEGMENT_SIZE);
+		memset(seg->trbs, 0,
+			sizeof(union xhci_trb) * (TRBS_PER_SEGMENT - 1));
+		seg->trbs[TRBS_PER_SEGMENT - 1].link.control &=
+			cpu_to_le32(~TRB_CYCLE);
 		seg = seg->next;
 	} while (seg != ring->deq_seg);
 
@@ -749,7 +752,7 @@ int xhci_resume(struct xhci_hcd *xhci, bool hibernated)
 	u32			command, temp = 0;
 	struct usb_hcd		*hcd = xhci_to_hcd(xhci);
 	struct usb_hcd		*secondary_hcd;
-	int			retval;
+	int			retval = 0;
 
 	/* Wait a bit if either of the roothubs need to settle from the
 	 * transition into bus suspend.
@@ -759,6 +762,9 @@ int xhci_resume(struct xhci_hcd *xhci, bool hibernated)
 				xhci->bus_state[1].next_statechange))
 		msleep(100);
 
+	set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+	set_bit(HCD_FLAG_HW_ACCESSIBLE, &xhci->shared_hcd->flags);
+
 	spin_lock_irq(&xhci->lock);
 	if (xhci->quirks & XHCI_RESET_ON_RESUME)
 		hibernated = true;
@@ -828,20 +834,13 @@ int xhci_resume(struct xhci_hcd *xhci, bool hibernated)
 			return retval;
 		xhci_dbg(xhci, "Start the primary HCD\n");
 		retval = xhci_run(hcd->primary_hcd);
-		if (retval)
-			goto failed_restart;
-
-		xhci_dbg(xhci, "Start the secondary HCD\n");
-		retval = xhci_run(secondary_hcd);
 		if (!retval) {
-			set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-			set_bit(HCD_FLAG_HW_ACCESSIBLE,
-					&xhci->shared_hcd->flags);
+			xhci_dbg(xhci, "Start the secondary HCD\n");
+			retval = xhci_run(secondary_hcd);
 		}
-failed_restart:
 		hcd->state = HC_STATE_SUSPENDED;
 		xhci->shared_hcd->state = HC_STATE_SUSPENDED;
-		return retval;
+		goto done;
 	}
 
 	/* step 4: set Run/Stop bit */
@@ -860,11 +859,14 @@ failed_restart:
 	 * Running endpoints by ringing their doorbells
 	 */
 
-	set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-	set_bit(HCD_FLAG_HW_ACCESSIBLE, &xhci->shared_hcd->flags);
-
 	spin_unlock_irq(&xhci->lock);
-	return 0;
+
+ done:
+	if (retval == 0) {
+		usb_hcd_resume_root_hub(hcd);
+		usb_hcd_resume_root_hub(xhci->shared_hcd);
+	}
+	return retval;
 }
 #endif	/* CONFIG_PM */
 
@@ -1566,6 +1568,7 @@ static int xhci_configure_endpoint_result(struct xhci_hcd *xhci,
 		/* FIXME: can we allocate more resources for the HC? */
 		break;
 	case COMP_BW_ERR:
+	case COMP_2ND_BW_ERR:
 		dev_warn(&udev->dev, "Not enough bandwidth "
 				"for new device state.\n");
 		ret = -ENOSPC;
@@ -1888,6 +1891,12 @@ int xhci_check_bandwidth(struct usb_hcd *hcd, struct usb_device *udev)
 	ctrl_ctx->add_flags |= cpu_to_le32(SLOT_FLAG);
 	ctrl_ctx->add_flags &= cpu_to_le32(~EP0_FLAG);
 	ctrl_ctx->drop_flags &= cpu_to_le32(~(SLOT_FLAG | EP0_FLAG));
+
+	/* Don't issue the command if there's no endpoints to update. */
+	if (ctrl_ctx->add_flags == cpu_to_le32(SLOT_FLAG) &&
+			ctrl_ctx->drop_flags == 0)
+		return 0;
+
 	xhci_dbg(xhci, "New Input Control Context:\n");
 	slot_ctx = xhci_get_slot_ctx(xhci, virt_dev->in_ctx);
 	xhci_dbg_ctx(xhci, virt_dev->in_ctx,
@@ -2174,8 +2183,7 @@ static int xhci_calculate_streams_and_bitmask(struct xhci_hcd *xhci,
 		if (ret < 0)
 			return ret;
 
-		max_streams = USB_SS_MAX_STREAMS(
-				eps[i]->ss_ep_comp.bmAttributes);
+		max_streams = usb_ss_max_streams(&eps[i]->ss_ep_comp);
 		if (max_streams < (*num_streams - 1)) {
 			xhci_dbg(xhci, "Ep 0x%x only supports %u stream IDs.\n",
 					eps[i]->desc.bEndpointAddress,
@@ -2867,6 +2875,10 @@ int xhci_address_device(struct usb_hcd *hcd, struct usb_device *udev)
 	/* Otherwise, update the control endpoint ring enqueue pointer. */
 	else
 		xhci_copy_ep0_dequeue_into_input_ctx(xhci, udev);
+	ctrl_ctx = xhci_get_input_control_ctx(xhci, virt_dev->in_ctx);
+	ctrl_ctx->add_flags = cpu_to_le32(SLOT_FLAG | EP0_FLAG);
+	ctrl_ctx->drop_flags = 0;
+
 	xhci_dbg(xhci, "Slot ID %d Input Context:\n", udev->slot_id);
 	xhci_dbg_ctx(xhci, virt_dev->in_ctx, 2);
 
@@ -2948,7 +2960,6 @@ int xhci_address_device(struct usb_hcd *hcd, struct usb_device *udev)
 	virt_dev->address = (le32_to_cpu(slot_ctx->dev_state) & DEV_ADDR_MASK)
 		+ 1;
 	/* Zero the input context control for later use */
-	ctrl_ctx = xhci_get_input_control_ctx(xhci, virt_dev->in_ctx);
 	ctrl_ctx->add_flags = 0;
 	ctrl_ctx->drop_flags = 0;
 
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index cae8e23308bf..c149d200415e 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -900,7 +900,6 @@ struct xhci_transfer_event {
 /* Invalid Stream ID Error */
 #define COMP_STRID_ERR	34
 /* Secondary Bandwidth Error - may be returned by a Configure Endpoint cmd */
-/* FIXME - check for this */
 #define COMP_2ND_BW_ERR	35
 /* Split Transaction Error */
 #define	COMP_SPLIT_ERR	36
@@ -1318,6 +1317,7 @@ struct xhci_hcd {
 #define XHCI_EP_LIMIT_QUIRK	(1 << 5)
 #define XHCI_BROKEN_MSI		(1 << 6)
 #define XHCI_RESET_ON_RESUME	(1 << 7)
+#define XHCI_AMD_0x96_HOST	(1 << 9)
 	unsigned int		num_active_eps;
 	unsigned int		limit_active_eps;
 	/* There are two roothubs to keep track of bus suspend info for */
@@ -1572,6 +1572,8 @@ void xhci_ring_ep_doorbell(struct xhci_hcd *xhci, unsigned int slot_id,
 		unsigned int ep_index, unsigned int stream_id);
 
 /* xHCI roothub code */
+void xhci_test_and_clear_bit(struct xhci_hcd *xhci, __le32 __iomem **port_array,
+				int port_id, u32 port_bit);
 int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue, u16 wIndex,
 		char *buf, u16 wLength);
 int xhci_hub_status_data(struct usb_hcd *hcd, char *buf);
diff --git a/drivers/usb/misc/isight_firmware.c b/drivers/usb/misc/isight_firmware.c
index fe1d44319d0a..8f725f651915 100644
--- a/drivers/usb/misc/isight_firmware.c
+++ b/drivers/usb/misc/isight_firmware.c
@@ -55,8 +55,9 @@ static int isight_firmware_load(struct usb_interface *intf,
 
 	ptr = firmware->data;
 
+	buf[0] = 0x01;
 	if (usb_control_msg
-	    (dev, usb_sndctrlpipe(dev, 0), 0xa0, 0x40, 0xe600, 0, "\1", 1,
+	    (dev, usb_sndctrlpipe(dev, 0), 0xa0, 0x40, 0xe600, 0, buf, 1,
 	     300) != 1) {
 		printk(KERN_ERR
 		       "Failed to initialise isight firmware loader\n");
@@ -100,8 +101,9 @@ static int isight_firmware_load(struct usb_interface *intf,
 		}
 	}
 
+	buf[0] = 0x00;
 	if (usb_control_msg
-	    (dev, usb_sndctrlpipe(dev, 0), 0xa0, 0x40, 0xe600, 0, "\0", 1,
+	    (dev, usb_sndctrlpipe(dev, 0), 0xa0, 0x40, 0xe600, 0, buf, 1,
 	     300) != 1) {
 		printk(KERN_ERR "isight firmware loading completion failed\n");
 		ret = -ENODEV;
diff --git a/drivers/usb/misc/usbsevseg.c b/drivers/usb/misc/usbsevseg.c
index 417b8f207e8b..59689fa2f7c1 100644
--- a/drivers/usb/misc/usbsevseg.c
+++ b/drivers/usb/misc/usbsevseg.c
@@ -24,7 +24,7 @@
 
 #define VENDOR_ID	0x0fc5
 #define PRODUCT_ID	0x1227
-#define MAXLEN		6
+#define MAXLEN		8
 
 /* table of devices that work with this driver */
 static const struct usb_device_id id_table[] = {
diff --git a/drivers/usb/mon/mon_bin.c b/drivers/usb/mon/mon_bin.c
index a09dbd243eb3..a04b2ff9dd83 100644
--- a/drivers/usb/mon/mon_bin.c
+++ b/drivers/usb/mon/mon_bin.c
@@ -1101,7 +1101,7 @@ static long mon_bin_ioctl(struct file *file, unsigned int cmd, unsigned long arg
 		nevents = mon_bin_queued(rp);
 
 		sp = (struct mon_bin_stats __user *)arg;
-		if (put_user(rp->cnt_lost, &sp->dropped))
+		if (put_user(ndropped, &sp->dropped))
 			return -EFAULT;
 		if (put_user(nevents, &sp->queued))
 			return -EFAULT;
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index 20a28731c338..96c5dbd1e0ee 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -2013,8 +2013,6 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 	if (status < 0)
 		goto fail3;
 
-	pm_runtime_put(musb->controller);
-
 	status = musb_init_debugfs(musb);
 	if (status < 0)
 		goto fail4;
@@ -2302,18 +2300,12 @@ static int musb_suspend(struct device *dev)
 		 */
 	}
 
-	musb_save_context(musb);
-
 	spin_unlock_irqrestore(&musb->lock, flags);
 	return 0;
 }
 
 static int musb_resume_noirq(struct device *dev)
 {
-	struct musb	*musb = dev_to_musb(dev);
-
-	musb_restore_context(musb);
-
 	/* for static cmos like DaVinci, register values were preserved
 	 * unless for some reason the whole soc powered down or the USB
 	 * module got reset through the PSC (vs just being disabled).
diff --git a/drivers/usb/musb/ux500_dma.c b/drivers/usb/musb/ux500_dma.c
index ef4333f4bbe0..6be1697182f9 100644
--- a/drivers/usb/musb/ux500_dma.c
+++ b/drivers/usb/musb/ux500_dma.c
@@ -147,8 +147,7 @@ static bool ux500_configure_channel(struct dma_channel *channel,
 	dma_chan->device->device_control(dma_chan, DMA_SLAVE_CONFIG,
 					     (unsigned long) &slave_conf);
 
-	dma_desc = dma_chan->device->
-			device_prep_slave_sg(dma_chan, &sg, 1, direction,
+	dma_desc = dmaengine_prep_slave_sg(dma_chan, &sg, 1, direction,
 					     DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	if (!dma_desc)
 		return false;
diff --git a/drivers/usb/otg/Kconfig b/drivers/usb/otg/Kconfig
index c66481ad98d7..e38d2b4ca134 100644
--- a/drivers/usb/otg/Kconfig
+++ b/drivers/usb/otg/Kconfig
@@ -12,11 +12,27 @@ config USB_OTG_UTILS
 	  Select this to make sure the build includes objects from
 	  the OTG infrastructure directory.
 
+config USB_OTG_WAKELOCK
+	bool "Hold a wakelock when USB connected"
+	depends on WAKELOCK
+	select USB_OTG_UTILS
+	help
+	  Select this to automatically hold a wakelock when USB is
+	  connected, preventing suspend.
+
 if USB || USB_GADGET
 
 #
 # USB Transceiver Drivers
 #
+config USB_COLIBRI_OTG
+	boolean "Colibri OTG Driver"
+	depends on USB && ARCH_TEGRA && !USB_TEGRA_OTG
+	select USB_OTG_UTILS
+	help
+	  Enable this driver on boards which use a regular GPIO for VBUS
+	  detection.
+
 config USB_GPIO_VBUS
 	tristate "GPIO based peripheral-only VBUS sensing 'transceiver'"
 	depends on GENERIC_GPIO
@@ -113,6 +129,14 @@ config USB_MSM_OTG
 	  This driver is not supported on boards like trout which
 	  has an external PHY.
 
+config USB_TEGRA_OTG
+	boolean "Tegra OTG Driver"
+	depends on USB && ARCH_TEGRA
+	select USB_OTG_UTILS
+	help
+	  Enable this driver on boards which use the internal VBUS and ID
+	  sensing of the Tegra USB PHY.
+
 config AB8500_USB
         tristate "AB8500 USB Transceiver Driver"
         depends on AB8500_CORE
diff --git a/drivers/usb/otg/Makefile b/drivers/usb/otg/Makefile
index 566655c53331..cbedff4e254f 100644
--- a/drivers/usb/otg/Makefile
+++ b/drivers/usb/otg/Makefile
@@ -1,15 +1,21 @@
 #
 # OTG infrastructure and transceiver drivers
 #
+GCOV_PROFILE_tegra-otg.o := y
 
 ccflags-$(CONFIG_USB_DEBUG)		:= -DDEBUG
 ccflags-$(CONFIG_USB_GADGET_DEBUG)	+= -DDEBUG
 
 # infrastructure
 obj-$(CONFIG_USB_OTG_UTILS)	+= otg.o
+obj-$(CONFIG_USB_OTG_WAKELOCK)	+= otg-wakelock.o
+obj-$(CONFIG_USB_OTG_UTILS)	+= otg_id.o
 
 # transceiver drivers
 obj-$(CONFIG_USB_GPIO_VBUS)	+= gpio_vbus.o
+obj-$(CONFIG_USB_COLIBRI_OTG)	+= colibri-otg.o
+CFLAGS_tegra-otg.o		= -Werror
+obj-$(CONFIG_USB_TEGRA_OTG)	+= tegra-otg.o
 obj-$(CONFIG_ISP1301_OMAP)	+= isp1301_omap.o
 obj-$(CONFIG_TWL4030_USB)	+= twl4030-usb.o
 obj-$(CONFIG_TWL6030_USB)	+= twl6030-usb.o
diff --git a/drivers/usb/otg/colibri-otg.c b/drivers/usb/otg/colibri-otg.c
new file mode 100644
index 000000000000..54d792536363
--- /dev/null
+++ b/drivers/usb/otg/colibri-otg.c
@@ -0,0 +1,268 @@
+/*
+ * drivers/usb/otg/colibri-otg.c
+ *
+ * OTG transceiver driver for Tegra UTMI phy with GPIO VBUS detection
+ *
+ * Copyright (C) 2010 NVIDIA Corp.
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (C) 2012 Toradex, Inc.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/colibri_usb.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/usb.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/hcd.h>
+#include <linux/usb/otg.h>
+
+#include <mach/gpio.h>
+
+struct colibri_otg_data {
+	struct otg_transceiver otg;
+	spinlock_t lock;
+	int irq;
+	struct platform_device *host;
+	struct platform_device *pdev;
+	struct work_struct work;
+};
+
+static const char *tegra_state_name(enum usb_otg_state state)
+{
+	if (state == OTG_STATE_A_HOST)
+		return "HOST";
+	if (state == OTG_STATE_B_PERIPHERAL)
+		return "PERIPHERAL";
+	if (state == OTG_STATE_A_SUSPEND)
+		return "SUSPEND";
+	return "INVALID";
+}
+
+void tegra_start_host(struct colibri_otg_data *tegra)
+{
+	struct colibri_otg_platform_data *pdata = tegra->otg.dev->platform_data;
+	if (!tegra->pdev) {
+		tegra->pdev = pdata->host_register();
+	}
+}
+
+void tegra_stop_host(struct colibri_otg_data *tegra)
+{
+	struct colibri_otg_platform_data *pdata = tegra->otg.dev->platform_data;
+	if (tegra->pdev) {
+		pdata->host_unregister(tegra->pdev);
+		tegra->pdev = NULL;
+	}
+}
+
+static void tegra_otg_notify_event(struct otg_transceiver *otg,
+					enum usb_xceiv_events event)
+{
+	otg->last_event = event;
+	atomic_notifier_call_chain(&otg->notifier, event, NULL);
+}
+
+static void irq_work(struct work_struct *work)
+{
+	struct colibri_otg_data *tegra =
+		container_of(work, struct colibri_otg_data, work);
+	struct otg_transceiver *otg = &tegra->otg;
+	enum usb_otg_state from = otg->state;
+	enum usb_otg_state to = OTG_STATE_UNDEFINED;
+	unsigned long flags;
+
+	spin_lock_irqsave(&tegra->lock, flags);
+
+	/* Check client detect (High active) */
+	mdelay(100);
+	if (gpio_get_value(irq_to_gpio(tegra->irq)))
+		to = OTG_STATE_B_PERIPHERAL;
+	else
+		to = OTG_STATE_A_HOST;
+
+	spin_unlock_irqrestore(&tegra->lock, flags);
+
+	if (to != OTG_STATE_UNDEFINED) {
+		otg->state = to;
+
+		if (from != to) {
+			dev_info(tegra->otg.dev, "%s --> %s\n",
+				 tegra_state_name(from), tegra_state_name(to));
+
+			if (to == OTG_STATE_B_PERIPHERAL) {
+				if (from == OTG_STATE_A_HOST) {
+					tegra_stop_host(tegra);
+					tegra_otg_notify_event(otg, USB_EVENT_NONE);
+				}
+				if (otg->gadget) {
+					usb_gadget_vbus_connect(otg->gadget);
+					tegra_otg_notify_event(otg, USB_EVENT_VBUS);
+				}
+			} else if (to == OTG_STATE_A_HOST) {
+				if (otg->gadget && (from == OTG_STATE_B_PERIPHERAL)) {
+					usb_gadget_vbus_disconnect(otg->gadget);
+					tegra_otg_notify_event(otg, USB_EVENT_NONE);
+				}
+				tegra_start_host(tegra);
+				tegra_otg_notify_event(otg, USB_EVENT_ID);
+			}
+		}
+	}
+}
+
+static irqreturn_t colibri_otg_irq(int irq, void *data)
+{
+	struct colibri_otg_data *tegra = data;
+
+	schedule_work(&tegra->work);
+
+	return IRQ_HANDLED;
+}
+
+static int colibri_otg_set_peripheral(struct otg_transceiver *otg,
+				struct usb_gadget *gadget)
+{
+	struct colibri_otg_data *tegra;
+
+	tegra = container_of(otg, struct colibri_otg_data, otg);
+	otg->gadget = gadget;
+
+	/* Set initial state */
+	schedule_work(&tegra->work);
+
+	return 0;
+}
+
+static int colibri_otg_set_host(struct otg_transceiver *otg,
+				struct usb_bus *host)
+{
+	struct colibri_otg_data *tegra;
+
+	tegra = container_of(otg, struct colibri_otg_data, otg);
+	otg->host = host;
+
+	return 0;
+}
+
+static int colibri_otg_set_power(struct otg_transceiver *otg, unsigned mA)
+{
+	return 0;
+}
+
+static int colibri_otg_set_suspend(struct otg_transceiver *otg, int suspend)
+{
+	return 0;
+}
+
+static int colibri_otg_probe(struct platform_device *pdev)
+{
+	struct colibri_otg_data *tegra;
+	struct colibri_otg_platform_data *plat = pdev->dev.platform_data;
+	int err, gpio_cd;
+
+	if (!plat) {
+		dev_err(&pdev->dev, "no platform data?\n");
+		return -ENODEV;
+	}
+
+	tegra = kzalloc(sizeof(struct colibri_otg_data), GFP_KERNEL);
+	if (!tegra)
+		return -ENOMEM;
+
+	tegra->otg.dev = &pdev->dev;
+	tegra->otg.label = "colibri-otg";
+	tegra->otg.state = OTG_STATE_UNDEFINED;
+	tegra->otg.set_host = colibri_otg_set_host;
+	tegra->otg.set_peripheral = colibri_otg_set_peripheral;
+	tegra->otg.set_suspend = colibri_otg_set_suspend;
+	tegra->otg.set_power = colibri_otg_set_power;
+	spin_lock_init(&tegra->lock);
+
+	platform_set_drvdata(pdev, tegra);
+
+	tegra->otg.state = OTG_STATE_A_SUSPEND;
+
+	err = otg_set_transceiver(&tegra->otg);
+	if (err) {
+		dev_err(&pdev->dev, "can't register transceiver (%d)\n", err);
+		goto err_otg;
+	}
+
+	gpio_cd = plat->cable_detect_gpio;
+	err = gpio_request(gpio_cd, "USBC_DET");
+	if (err)
+		goto err_gpio;
+	gpio_direction_input(gpio_cd);
+
+	tegra->irq = gpio_to_irq(gpio_cd);
+	err = request_threaded_irq(tegra->irq, colibri_otg_irq, NULL,
+				   IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+				   "colibri-otg USBC_DET", tegra);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to register USB client detect IRQ\n");
+		goto err_irq;
+	}
+
+	INIT_WORK (&tegra->work, irq_work);
+
+	dev_info(&pdev->dev, "otg transceiver registered\n");
+
+	return 0;
+
+err_irq:
+	gpio_free(gpio_cd);
+err_gpio:
+	otg_set_transceiver(NULL);
+err_otg:
+	platform_set_drvdata(pdev, NULL);
+	kfree(tegra);
+	return err;
+}
+
+static int __exit colibri_otg_remove(struct platform_device *pdev)
+{
+	struct colibri_otg_data *tegra = platform_get_drvdata(pdev);
+
+	free_irq(tegra->irq, tegra);
+	gpio_free(irq_to_gpio(tegra->irq));
+	otg_set_transceiver(NULL);
+	platform_set_drvdata(pdev, NULL);
+	kfree(tegra);
+
+	return 0;
+}
+
+static struct platform_driver colibri_otg_driver = {
+	.driver = {
+		.name  = "colibri-otg",
+	},
+	.remove  = __exit_p(colibri_otg_remove),
+	.probe   = colibri_otg_probe,
+};
+
+static int __init colibri_otg_init(void)
+{
+	return platform_driver_register(&colibri_otg_driver);
+}
+subsys_initcall(colibri_otg_init);
+
+static void __exit colibri_otg_exit(void)
+{
+	platform_driver_unregister(&colibri_otg_driver);
+}
+module_exit(colibri_otg_exit);
diff --git a/drivers/usb/otg/otg-wakelock.c b/drivers/usb/otg/otg-wakelock.c
new file mode 100644
index 000000000000..2f11472dd2b3
--- /dev/null
+++ b/drivers/usb/otg/otg-wakelock.c
@@ -0,0 +1,169 @@
+/*
+ * otg-wakelock.c
+ *
+ * Copyright (C) 2011 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/notifier.h>
+#include <linux/wakelock.h>
+#include <linux/spinlock.h>
+#include <linux/usb/otg.h>
+
+#define TEMPORARY_HOLD_TIME	2000
+
+static bool enabled = true;
+static struct otg_transceiver *otgwl_xceiv;
+static struct notifier_block otgwl_nb;
+
+/*
+ * otgwl_spinlock is held while the VBUS lock is grabbed or dropped and the
+ * held field is updated to match.
+ */
+
+static DEFINE_SPINLOCK(otgwl_spinlock);
+
+/*
+ * Only one lock, but since these 3 fields are associated with each other...
+ */
+
+struct otgwl_lock {
+	char name[40];
+	struct wake_lock wakelock;
+	bool held;
+};
+
+/*
+ * VBUS present lock.  Also used as a timed lock on charger
+ * connect/disconnect and USB host disconnect, to allow the system
+ * to react to the change in power.
+ */
+
+static struct otgwl_lock vbus_lock;
+
+static void otgwl_hold(struct otgwl_lock *lock)
+{
+	if (!lock->held) {
+		wake_lock(&lock->wakelock);
+		lock->held = true;
+	}
+}
+
+static void otgwl_temporary_hold(struct otgwl_lock *lock)
+{
+	wake_lock_timeout(&lock->wakelock,
+			  msecs_to_jiffies(TEMPORARY_HOLD_TIME));
+	lock->held = false;
+}
+
+static void otgwl_drop(struct otgwl_lock *lock)
+{
+	if (lock->held) {
+		wake_unlock(&lock->wakelock);
+		lock->held = false;
+	}
+}
+
+static void otgwl_handle_event(unsigned long event)
+{
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&otgwl_spinlock, irqflags);
+
+	if (!enabled) {
+		otgwl_drop(&vbus_lock);
+		spin_unlock_irqrestore(&otgwl_spinlock, irqflags);
+		return;
+	}
+
+	switch (event) {
+	case USB_EVENT_VBUS:
+	case USB_EVENT_ENUMERATED:
+		otgwl_hold(&vbus_lock);
+		break;
+
+	case USB_EVENT_NONE:
+	case USB_EVENT_ID:
+	case USB_EVENT_CHARGER:
+		otgwl_temporary_hold(&vbus_lock);
+		break;
+
+	default:
+		break;
+	}
+
+	spin_unlock_irqrestore(&otgwl_spinlock, irqflags);
+}
+
+static int otgwl_otg_notifications(struct notifier_block *nb,
+				   unsigned long event, void *unused)
+{
+	otgwl_handle_event(event);
+	return NOTIFY_OK;
+}
+
+static int set_enabled(const char *val, const struct kernel_param *kp)
+{
+	int rv = param_set_bool(val, kp);
+
+	if (rv)
+		return rv;
+
+	if (otgwl_xceiv)
+		otgwl_handle_event(otgwl_xceiv->last_event);
+
+	return 0;
+}
+
+static struct kernel_param_ops enabled_param_ops = {
+	.set = set_enabled,
+	.get = param_get_bool,
+};
+
+module_param_cb(enabled, &enabled_param_ops, &enabled, 0644);
+MODULE_PARM_DESC(enabled, "enable wakelock when VBUS present");
+
+static int __init otg_wakelock_init(void)
+{
+	int ret;
+
+	otgwl_xceiv = otg_get_transceiver();
+
+	if (!otgwl_xceiv) {
+		pr_err("%s: No OTG transceiver found\n", __func__);
+		return -ENODEV;
+	}
+
+	snprintf(vbus_lock.name, sizeof(vbus_lock.name), "vbus-%s",
+		 dev_name(otgwl_xceiv->dev));
+	wake_lock_init(&vbus_lock.wakelock, WAKE_LOCK_SUSPEND,
+		       vbus_lock.name);
+
+	otgwl_nb.notifier_call = otgwl_otg_notifications;
+	ret = otg_register_notifier(otgwl_xceiv, &otgwl_nb);
+
+	if (ret) {
+		pr_err("%s: otg_register_notifier on transceiver %s"
+		       " failed\n", __func__,
+		       dev_name(otgwl_xceiv->dev));
+		otgwl_xceiv = NULL;
+		wake_lock_destroy(&vbus_lock.wakelock);
+		return ret;
+	}
+
+	otgwl_handle_event(otgwl_xceiv->last_event);
+	return ret;
+}
+
+late_initcall(otg_wakelock_init);
diff --git a/drivers/usb/otg/otg_id.c b/drivers/usb/otg/otg_id.c
new file mode 100644
index 000000000000..8037edbf3141
--- /dev/null
+++ b/drivers/usb/otg/otg_id.c
@@ -0,0 +1,205 @@
+/*
+ * Copyright (C) 2011 Google, Inc.
+ *
+ * Author:
+ *	Colin Cross <ccross@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/kernel.h>
+#include <linux/mutex.h>
+#include <linux/notifier.h>
+#include <linux/usb/otg_id.h>
+
+static DEFINE_MUTEX(otg_id_lock);
+static struct plist_head otg_id_plist =
+	PLIST_HEAD_INIT(otg_id_plist);
+static struct otg_id_notifier_block *otg_id_active;
+static bool otg_id_cancelling;
+static bool otg_id_inited;
+static int otg_id_suspended;
+static bool otg_id_pending;
+
+static void otg_id_cancel(void)
+{
+	if (otg_id_active) {
+		otg_id_cancelling = true;
+		mutex_unlock(&otg_id_lock);
+
+		otg_id_active->cancel(otg_id_active);
+
+		mutex_lock(&otg_id_lock);
+		otg_id_cancelling = false;
+	}
+}
+
+static void __otg_id_notify(void)
+{
+	int ret;
+	struct otg_id_notifier_block *otg_id_nb;
+	bool proxy_wait = false;
+	if (plist_head_empty(&otg_id_plist))
+		return;
+
+	plist_for_each_entry(otg_id_nb, &otg_id_plist, p) {
+		if (proxy_wait) {
+			if (otg_id_nb->proxy_wait)
+				ret = otg_id_nb->proxy_wait(otg_id_nb);
+		} else {
+			ret = otg_id_nb->detect(otg_id_nb);
+		}
+		if (ret == OTG_ID_HANDLED) {
+			otg_id_active = otg_id_nb;
+			return;
+		}
+		if (ret == OTG_ID_PROXY_WAIT)
+			proxy_wait = true;
+
+	}
+
+	WARN(1, "otg id event not handled");
+	otg_id_active = NULL;
+}
+
+int otg_id_init(void)
+{
+	mutex_lock(&otg_id_lock);
+
+	otg_id_inited = true;
+	__otg_id_notify();
+
+	mutex_unlock(&otg_id_lock);
+	return 0;
+}
+late_initcall(otg_id_init);
+
+/**
+ * otg_id_register_notifier
+ * @otg_id_nb: notifier block containing priority and callback function
+ *
+ * Register a notifier that will be called on any USB cable state change.
+ * The priority determines the order the callback will be called in, a higher
+ * number will be called first.  A callback function needs to determine the
+ * type of USB cable that is connected.  If it can determine the type, it
+ * should notify the appropriate drivers (for example, call an otg notifier
+ * with USB_EVENT_VBUS), and return OTG_ID_HANDLED.  Once a callback has
+ * returned OTG_ID_HANDLED, it is responsible for calling otg_id_notify() when
+ * the detected USB cable is disconnected.
+ */
+int otg_id_register_notifier(struct otg_id_notifier_block *otg_id_nb)
+{
+	plist_node_init(&otg_id_nb->p, otg_id_nb->priority);
+
+	mutex_lock(&otg_id_lock);
+	plist_add(&otg_id_nb->p, &otg_id_plist);
+
+	if (otg_id_inited) {
+		otg_id_cancel();
+		__otg_id_notify();
+	}
+
+	mutex_unlock(&otg_id_lock);
+
+	return 0;
+}
+
+void otg_id_unregister_notifier(struct otg_id_notifier_block *otg_id_nb)
+{
+	mutex_lock(&otg_id_lock);
+
+	plist_del(&otg_id_nb->p, &otg_id_plist);
+
+	if (otg_id_inited && (otg_id_active == otg_id_nb)) {
+		otg_id_cancel();
+		__otg_id_notify();
+	}
+
+	mutex_unlock(&otg_id_lock);
+}
+
+/**
+ * otg_id_notify
+ *
+ * Notify listeners on any USB cable state change.
+ *
+ * A driver may only call otg_id_notify if it returned OTG_ID_HANDLED the last
+ * time it's notifier was called, and it's cancel function has not been called.
+ */
+void otg_id_notify(void)
+{
+	mutex_lock(&otg_id_lock);
+
+	if (otg_id_cancelling)
+		goto out;
+
+	if (otg_id_suspended != 0) {
+		otg_id_pending = true;
+		goto out;
+	}
+
+	__otg_id_notify();
+out:
+	mutex_unlock(&otg_id_lock);
+}
+
+/**
+ * otg_id_suspend
+ *
+ * Mark the otg_id subsystem as going into suspend. From here on out,
+ * any notifications will be deferred until the last otg_id client resumes.
+ * If there is a pending notification when calling this function, it will
+ * return a negative errno and expects that the caller will abort suspend.
+ * Returs 0 on success.
+ */
+int otg_id_suspend(void)
+{
+	int ret = 0;
+
+	mutex_lock(&otg_id_lock);
+
+	/*
+	 * if there's a pending notification, tell the caller to abort suspend
+	 */
+	if (otg_id_suspended != 0 && otg_id_pending) {
+		pr_info("otg_id: pending notification, should abort suspend\n");
+		ret = -EBUSY;
+		goto out;
+	}
+
+	otg_id_suspended++;
+out:
+	mutex_unlock(&otg_id_lock);
+	return ret;
+}
+
+/**
+ * otg_id_resume
+ *
+ * Inform the otg_id subsystem that a client is resuming. If this is the
+ * last client to be resumed and there's a pending notification,
+ * otg_id_notify() is called.
+ */
+void otg_id_resume(void)
+{
+	mutex_lock(&otg_id_lock);
+	if (WARN(!otg_id_suspended, "unbalanced otg_id_resume\n"))
+		goto out;
+	if (--otg_id_suspended == 0) {
+		if (otg_id_pending) {
+			pr_info("otg_id: had pending notification\n");
+			otg_id_pending = false;
+			__otg_id_notify();
+		}
+	}
+out:
+	mutex_unlock(&otg_id_lock);
+}
diff --git a/drivers/usb/otg/tegra-otg.c b/drivers/usb/otg/tegra-otg.c
new file mode 100644
index 000000000000..12c359bcbe46
--- /dev/null
+++ b/drivers/usb/otg/tegra-otg.c
@@ -0,0 +1,622 @@
+/*
+ * drivers/usb/otg/tegra-otg.c
+ *
+ * OTG transceiver driver for Tegra UTMI phy
+ *
+ * Copyright (C) 2010-2012 NVIDIA CORPORATION. All rights reserved.
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/usb.h>
+#include <linux/usb/otg.h>
+#include <linux/usb/gadget.h>
+#include <linux/platform_device.h>
+#include <linux/platform_data/tegra_usb.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+
+#define USB_PHY_WAKEUP		0x408
+#define  USB_ID_INT_EN		(1 << 0)
+#define  USB_ID_INT_STATUS	(1 << 1)
+#define  USB_ID_STATUS		(1 << 2)
+#define  USB_ID_PIN_WAKEUP_EN	(1 << 6)
+#define  USB_VBUS_WAKEUP_EN	(1 << 30)
+#define  USB_VBUS_INT_EN	(1 << 8)
+#define  USB_VBUS_INT_STATUS	(1 << 9)
+#define  USB_VBUS_STATUS	(1 << 10)
+#define  USB_INT_EN		(USB_VBUS_INT_EN | USB_ID_INT_EN | \
+						USB_VBUS_WAKEUP_EN | USB_ID_PIN_WAKEUP_EN)
+
+#ifdef DEBUG
+#define DBG(stuff...)	pr_info("tegra-otg: " stuff)
+#else
+#define DBG(stuff...)	do {} while (0)
+#endif
+
+struct tegra_otg_data {
+	struct otg_transceiver otg;
+	unsigned long int_status;
+	spinlock_t lock;
+	struct mutex irq_work_mutex;
+	void __iomem *regs;
+	struct clk *clk;
+	int irq;
+	struct platform_device *pdev;
+	struct work_struct work;
+	unsigned int intr_reg_data;
+	bool clk_enabled;
+	bool interrupt_mode;
+	bool builtin_host;
+	bool suspended;
+};
+
+static struct tegra_otg_data *tegra_clone;
+
+static inline unsigned long otg_readl(struct tegra_otg_data *tegra,
+				      unsigned int offset)
+{
+	return readl(tegra->regs + offset);
+}
+
+static inline void otg_writel(struct tegra_otg_data *tegra, unsigned long val,
+			      unsigned int offset)
+{
+	writel(val, tegra->regs + offset);
+}
+
+static const char *tegra_state_name(enum usb_otg_state state)
+{
+	switch (state) {
+		case OTG_STATE_A_HOST:
+			return "HOST";
+		case OTG_STATE_B_PERIPHERAL:
+			return "PERIPHERAL";
+		case OTG_STATE_A_SUSPEND:
+			return "SUSPEND";
+		case OTG_STATE_UNDEFINED:
+			return "UNDEFINED";
+		default:
+			return "INVALID";
+	}
+}
+
+static unsigned long enable_interrupt(struct tegra_otg_data *tegra, bool en)
+{
+	unsigned long val;
+
+	clk_enable(tegra->clk);
+	val = otg_readl(tegra, USB_PHY_WAKEUP);
+	if (en) {
+		if (tegra->builtin_host)
+			val |= USB_INT_EN;
+		else
+			val |= USB_VBUS_INT_EN | USB_VBUS_WAKEUP_EN | USB_ID_PIN_WAKEUP_EN;
+	}
+	else
+		val &= ~USB_INT_EN;
+	otg_writel(tegra, val, USB_PHY_WAKEUP);
+	/* Add delay to make sure register is updated */
+	udelay(1);
+	clk_disable(tegra->clk);
+
+	return val;
+}
+
+static void tegra_start_host(struct tegra_otg_data *tegra)
+{
+	struct tegra_usb_otg_data *pdata = tegra->otg.dev->platform_data;
+	struct platform_device *pdev, *ehci_device = pdata->ehci_device;
+	void *platform_data;
+	int val;
+	DBG("%s(%d) Begin\n", __func__, __LINE__);
+
+	if (tegra->pdev)
+		return ;
+
+	/* prepare device structure for registering host*/
+	pdev = platform_device_alloc(ehci_device->name, ehci_device->id);
+	if (!pdev)
+		return ;
+
+	val = platform_device_add_resources(pdev, ehci_device->resource,
+					    ehci_device->num_resources);
+	if (val)
+		goto error;
+
+	pdev->dev.dma_mask = ehci_device->dev.dma_mask;
+	pdev->dev.coherent_dma_mask = ehci_device->dev.coherent_dma_mask;
+
+	platform_data = kmalloc(sizeof(struct tegra_usb_platform_data), GFP_KERNEL);
+	if (!platform_data)
+		goto error;
+
+	memcpy(platform_data, pdata->ehci_pdata,
+					sizeof(struct tegra_usb_platform_data));
+	pdev->dev.platform_data = platform_data;
+	val = platform_device_add(pdev);
+	if (val)
+		goto error_add;
+
+	tegra->pdev = pdev;
+	DBG("%s(%d) End\n", __func__, __LINE__);
+	return ;
+
+error_add:
+	kfree(platform_data);
+error:
+	pr_err("%s: failed to add the host controller device\n", __func__);
+	platform_device_put(pdev);
+	tegra->pdev = NULL;
+}
+
+static void tegra_stop_host(struct tegra_otg_data *tegra)
+{
+	struct platform_device *pdev = tegra->pdev;
+
+	DBG("%s(%d) Begin\n", __func__, __LINE__);
+
+	if (pdev) {
+		/* unregister host from otg */
+		platform_device_unregister(pdev);
+		tegra->pdev = NULL;
+	}
+
+	DBG("%s(%d) End\n", __func__, __LINE__);
+}
+
+static void tegra_otg_notify_event(struct otg_transceiver *otg,
+					enum usb_xceiv_events event)
+{
+	otg->last_event = event;
+	atomic_notifier_call_chain(&otg->notifier, event, NULL);
+}
+
+static void tegra_change_otg_state(struct tegra_otg_data *tegra,
+				enum usb_otg_state to)
+{
+	struct otg_transceiver *otg = &tegra->otg;
+	enum usb_otg_state from = otg->state;
+
+	if(!tegra->interrupt_mode){
+		DBG("OTG: Vbus detection is disabled");
+		return;
+	}
+
+	DBG("%s(%d) requested otg state %s-->%s\n", __func__,
+		__LINE__, tegra_state_name(from), tegra_state_name(to));
+
+	if (to != OTG_STATE_UNDEFINED && from != to) {
+		otg->state = to;
+		dev_info(tegra->otg.dev, "%s --> %s\n", tegra_state_name(from),
+					      tegra_state_name(to));
+
+		if (from == OTG_STATE_A_SUSPEND) {
+			if (to == OTG_STATE_B_PERIPHERAL && otg->gadget) {
+				usb_gadget_vbus_connect(otg->gadget);
+				tegra_otg_notify_event(otg, USB_EVENT_VBUS);
+			}
+			else if (to == OTG_STATE_A_HOST) {
+				tegra_start_host(tegra);
+				tegra_otg_notify_event(otg, USB_EVENT_ID);
+			}
+		} else if (from == OTG_STATE_A_HOST) {
+			if (to == OTG_STATE_A_SUSPEND) {
+				tegra_stop_host(tegra);
+				tegra_otg_notify_event(otg, USB_EVENT_NONE);
+			}
+		} else if (from == OTG_STATE_B_PERIPHERAL && otg->gadget) {
+			if (to == OTG_STATE_A_SUSPEND) {
+				usb_gadget_vbus_disconnect(otg->gadget);
+				tegra_otg_notify_event(otg, USB_EVENT_NONE);
+			}
+		}
+	}
+}
+
+static void irq_work(struct work_struct *work)
+{
+	struct tegra_otg_data *tegra =
+		container_of(work, struct tegra_otg_data, work);
+	struct otg_transceiver *otg = &tegra->otg;
+	enum usb_otg_state from;
+	enum usb_otg_state to = OTG_STATE_UNDEFINED;
+	unsigned long flags;
+	unsigned long status;
+
+	mutex_lock(&tegra->irq_work_mutex);
+
+	spin_lock_irqsave(&tegra->lock, flags);
+	from = otg->state;
+	status = tegra->int_status;
+
+	/* Debug prints */
+	DBG("%s(%d) status = 0x%lx\n", __func__, __LINE__, status);
+	if ((status & USB_ID_INT_STATUS) &&
+			(status & USB_VBUS_INT_STATUS))
+		DBG("%s(%d) got vbus & id interrupt\n", __func__, __LINE__);
+	else {
+		if (status & USB_ID_INT_STATUS)
+			DBG("%s(%d) got id interrupt\n", __func__, __LINE__);
+		if (status & USB_VBUS_INT_STATUS)
+			DBG("%s(%d) got vbus interrupt\n", __func__, __LINE__);
+	}
+
+	if (!(status & USB_ID_STATUS) && (status & USB_ID_INT_EN))
+		to = OTG_STATE_A_HOST;
+	else if (status & USB_VBUS_STATUS && from != OTG_STATE_A_HOST)
+		to = OTG_STATE_B_PERIPHERAL;
+	else
+		to = OTG_STATE_A_SUSPEND;
+
+	spin_unlock_irqrestore(&tegra->lock, flags);
+	tegra_change_otg_state(tegra, to);
+	mutex_unlock(&tegra->irq_work_mutex);
+}
+
+static irqreturn_t tegra_otg_irq(int irq, void *data)
+{
+	struct tegra_otg_data *tegra = data;
+	unsigned long flags;
+	unsigned long val;
+
+	spin_lock_irqsave(&tegra->lock, flags);
+	val = otg_readl(tegra, USB_PHY_WAKEUP);
+	DBG("%s(%d) interrupt val = 0x%lx\n", __func__, __LINE__, val);
+
+	if (val & (USB_VBUS_INT_EN | USB_ID_INT_EN)) {
+		DBG("%s(%d) PHY_WAKEUP = 0x%lx\n", __func__, __LINE__, val);
+		otg_writel(tegra, val, USB_PHY_WAKEUP);
+		if ((val & USB_ID_INT_STATUS) || (val & USB_VBUS_INT_STATUS)) {
+			tegra->int_status = val;
+			schedule_work(&tegra->work);
+		}
+	}
+	spin_unlock_irqrestore(&tegra->lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+
+static int tegra_otg_set_peripheral(struct otg_transceiver *otg,
+				struct usb_gadget *gadget)
+{
+	struct tegra_otg_data *tegra;
+	unsigned long val;
+	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
+
+	tegra = container_of(otg, struct tegra_otg_data, otg);
+	otg->gadget = gadget;
+
+	val = enable_interrupt(tegra, true);
+
+	if ((val & USB_ID_STATUS) && (val & USB_VBUS_STATUS))
+		val |= USB_VBUS_INT_STATUS;
+	else if (!(val & USB_ID_STATUS)) {
+		if(!tegra->builtin_host)
+			val &= ~USB_ID_INT_STATUS;
+		else
+			val |= USB_ID_INT_STATUS;
+	}
+	else
+		val &= ~(USB_ID_INT_STATUS | USB_VBUS_INT_STATUS);
+
+	if ((val & USB_ID_INT_STATUS) || (val & USB_VBUS_INT_STATUS)) {
+		tegra->int_status = val;
+		schedule_work(&tegra->work);
+	}
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+	return 0;
+}
+
+static int tegra_otg_set_host(struct otg_transceiver *otg,
+				struct usb_bus *host)
+{
+	struct tegra_otg_data *tegra;
+	unsigned long val;
+	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
+
+	tegra = container_of(otg, struct tegra_otg_data, otg);
+	otg->host = host;
+
+	clk_enable(tegra->clk);
+	val = otg_readl(tegra, USB_PHY_WAKEUP);
+	val &= ~(USB_VBUS_INT_STATUS | USB_ID_INT_STATUS);
+	val |= (USB_ID_INT_EN | USB_ID_PIN_WAKEUP_EN);
+	otg_writel(tegra, val, USB_PHY_WAKEUP);
+	clk_disable(tegra->clk);
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+	return 0;
+}
+
+static int tegra_otg_set_power(struct otg_transceiver *otg, unsigned mA)
+{
+	return 0;
+}
+
+static int tegra_otg_set_suspend(struct otg_transceiver *otg, int suspend)
+{
+	return 0;
+}
+
+static ssize_t show_host_en(struct device *dev, struct device_attribute *attr,
+				char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct tegra_otg_data *tegra = platform_get_drvdata(pdev);
+
+	*buf = tegra->interrupt_mode ? '0': '1';
+	strcat(buf, "\n");
+	return strlen(buf);
+}
+
+static ssize_t store_host_en(struct device *dev, struct device_attribute *attr,
+				const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct tegra_otg_data *tegra = platform_get_drvdata(pdev);
+	unsigned int host;
+
+	if (sscanf(buf, "%d", &host) != 1 || host < 0 || host > 1)
+		return -EINVAL;
+
+	if (host) {
+		enable_interrupt(tegra, false);
+		tegra_change_otg_state(tegra, OTG_STATE_A_SUSPEND);
+		tegra_change_otg_state(tegra, OTG_STATE_A_HOST);
+		tegra->interrupt_mode = false;
+	} else {
+		tegra->interrupt_mode = true;
+		tegra_change_otg_state(tegra, OTG_STATE_A_SUSPEND);
+		enable_interrupt(tegra, true);
+	}
+
+	return count;
+}
+
+static DEVICE_ATTR(enable_host, 0644, show_host_en, store_host_en);
+
+static int tegra_otg_probe(struct platform_device *pdev)
+{
+	struct tegra_otg_data *tegra;
+	struct resource *res;
+	struct tegra_usb_otg_data *pdata = dev_get_platdata(&pdev->dev);
+	int err;
+
+	tegra = kzalloc(sizeof(struct tegra_otg_data), GFP_KERNEL);
+	if (!tegra)
+		return -ENOMEM;
+
+	tegra->otg.dev = &pdev->dev;
+	tegra->otg.label = "tegra-otg";
+	tegra->otg.state = OTG_STATE_UNDEFINED;
+	tegra->otg.set_host = tegra_otg_set_host;
+	tegra->otg.set_peripheral = tegra_otg_set_peripheral;
+	tegra->otg.set_suspend = tegra_otg_set_suspend;
+	tegra->otg.set_power = tegra_otg_set_power;
+	spin_lock_init(&tegra->lock);
+	mutex_init(&tegra->irq_work_mutex);
+
+	if (pdata) {
+		tegra->builtin_host = !pdata->ehci_pdata->builtin_host_disabled;
+	}
+
+	platform_set_drvdata(pdev, tegra);
+	tegra_clone = tegra;
+	tegra->interrupt_mode = true;
+	tegra->suspended = false;
+
+	tegra->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(tegra->clk)) {
+		dev_err(&pdev->dev, "Can't get otg clock\n");
+		err = PTR_ERR(tegra->clk);
+		goto err_clk;
+	}
+
+	err = clk_enable(tegra->clk);
+	if (err)
+		goto err_clken;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get I/O memory\n");
+		err = -ENXIO;
+		goto err_io;
+	}
+	tegra->regs = ioremap(res->start, resource_size(res));
+	if (!tegra->regs) {
+		err = -ENOMEM;
+		goto err_io;
+	}
+
+	tegra->otg.state = OTG_STATE_A_SUSPEND;
+
+	err = otg_set_transceiver(&tegra->otg);
+	if (err) {
+		dev_err(&pdev->dev, "can't register transceiver (%d)\n", err);
+		goto err_otg;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get IRQ\n");
+		err = -ENXIO;
+		goto err_irq;
+	}
+	tegra->irq = res->start;
+	err = request_threaded_irq(tegra->irq, tegra_otg_irq,
+				   NULL,
+				   IRQF_SHARED | IRQF_TRIGGER_HIGH,
+				   "tegra-otg", tegra);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to register IRQ\n");
+		goto err_irq;
+	}
+
+	if (pdata->ehci_pdata->u_data.host.remote_wakeup_supported) {
+		err = enable_irq_wake(tegra->irq);
+		if (err < 0) {
+			dev_warn(&pdev->dev,
+				"Couldn't enable USB otg mode wakeup,"
+				" irq=%d, error=%d\n", tegra->irq, err);
+			err = 0;
+		}
+	}
+
+	INIT_WORK(&tegra->work, irq_work);
+
+	dev_info(&pdev->dev, "otg transceiver registered\n");
+
+	err = device_create_file(&pdev->dev, &dev_attr_enable_host);
+	if (err) {
+		dev_warn(&pdev->dev, "Can't register sysfs attribute\n");
+		goto err_irq;
+	}
+
+	clk_disable(tegra->clk);
+
+	return 0;
+
+err_irq:
+	otg_set_transceiver(NULL);
+err_otg:
+	iounmap(tegra->regs);
+err_io:
+	clk_disable(tegra->clk);
+err_clken:
+	clk_put(tegra->clk);
+err_clk:
+	platform_set_drvdata(pdev, NULL);
+	kfree(tegra);
+	return err;
+}
+
+static int __exit tegra_otg_remove(struct platform_device *pdev)
+{
+	struct tegra_otg_data *tegra = platform_get_drvdata(pdev);
+
+	free_irq(tegra->irq, tegra);
+	otg_set_transceiver(NULL);
+	iounmap(tegra->regs);
+	clk_disable(tegra->clk);
+	clk_put(tegra->clk);
+	platform_set_drvdata(pdev, NULL);
+	mutex_destroy(&tegra->irq_work_mutex);
+	kfree(tegra);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int tegra_otg_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct tegra_otg_data *tegra = platform_get_drvdata(pdev);
+	struct otg_transceiver *otg = &tegra->otg;
+	int val;
+
+	mutex_lock(&tegra->irq_work_mutex);
+	DBG("%s(%d) BEGIN state : %s\n", __func__, __LINE__,
+					tegra_state_name(otg->state));
+
+	clk_enable(tegra->clk);
+	val = otg_readl(tegra, USB_PHY_WAKEUP);
+	val &= ~(USB_ID_INT_EN | USB_VBUS_INT_EN);
+	otg_writel(tegra, val, USB_PHY_WAKEUP);
+	clk_disable(tegra->clk);
+
+	/* Suspend peripheral mode, host mode is taken care by host driver */
+	if (otg->state == OTG_STATE_B_PERIPHERAL)
+		tegra_change_otg_state(tegra, OTG_STATE_A_SUSPEND);
+
+	tegra->suspended = true;
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+	mutex_unlock(&tegra->irq_work_mutex);
+	return 0;
+}
+
+static void tegra_otg_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct tegra_otg_data *tegra = platform_get_drvdata(pdev);
+	int val;
+	unsigned long flags;
+	DBG("%s(%d) BEGIN\n", __func__, __LINE__);
+
+	mutex_lock(&tegra->irq_work_mutex);
+	if (!tegra->suspended) {
+		mutex_unlock(&tegra->irq_work_mutex);
+		return;
+	}
+
+	/* Clear pending interrupts */
+	clk_enable(tegra->clk);
+	val = otg_readl(tegra, USB_PHY_WAKEUP);
+	otg_writel(tegra, val, USB_PHY_WAKEUP);
+	DBG("%s(%d) PHY WAKEUP register : 0x%x\n", __func__, __LINE__, val);
+	clk_disable(tegra->clk);
+
+	/* Enable interrupt and call work to set to appropriate state */
+	spin_lock_irqsave(&tegra->lock, flags);
+	if (tegra->builtin_host)
+		tegra->int_status = val | USB_INT_EN;
+	else
+		tegra->int_status = val | USB_VBUS_INT_EN | USB_VBUS_WAKEUP_EN |
+			USB_ID_PIN_WAKEUP_EN;
+
+	spin_unlock_irqrestore(&tegra->lock, flags);
+	schedule_work(&tegra->work);
+	enable_interrupt(tegra, true);
+
+	tegra->suspended = false;
+
+	DBG("%s(%d) END\n", __func__, __LINE__);
+	mutex_unlock(&tegra->irq_work_mutex);
+}
+
+static const struct dev_pm_ops tegra_otg_pm_ops = {
+	.complete = tegra_otg_resume,
+	.suspend = tegra_otg_suspend,
+};
+#endif
+
+static struct platform_driver tegra_otg_driver = {
+	.driver = {
+		.name  = "tegra-otg",
+#ifdef CONFIG_PM
+		.pm    = &tegra_otg_pm_ops,
+#endif
+	},
+	.remove  = __exit_p(tegra_otg_remove),
+	.probe   = tegra_otg_probe,
+};
+
+static int __init tegra_otg_init(void)
+{
+	return platform_driver_register(&tegra_otg_driver);
+}
+subsys_initcall(tegra_otg_init);
+
+static void __exit tegra_otg_exit(void)
+{
+	platform_driver_unregister(&tegra_otg_driver);
+}
+module_exit(tegra_otg_exit);
diff --git a/drivers/usb/renesas_usbhs/fifo.c b/drivers/usb/renesas_usbhs/fifo.c
index a34430f55fb7..18d909eebae6 100644
--- a/drivers/usb/renesas_usbhs/fifo.c
+++ b/drivers/usb/renesas_usbhs/fifo.c
@@ -602,9 +602,8 @@ static void usbhsf_dma_prepare_tasklet(unsigned long data)
 	sg_dma_address(&sg) = pkt->dma + pkt->actual;
 	sg_dma_len(&sg) = pkt->trans;
 
-	desc = chan->device->device_prep_slave_sg(chan, &sg, 1, dir,
-						  DMA_PREP_INTERRUPT |
-						  DMA_CTRL_ACK);
+	desc = dmaengine_prep_slave_sg(chan, &sg, 1, dir,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	if (!desc)
 		return;
 
diff --git a/drivers/usb/serial/Kconfig b/drivers/usb/serial/Kconfig
index b71e309116a3..f6a54356c29d 100644
--- a/drivers/usb/serial/Kconfig
+++ b/drivers/usb/serial/Kconfig
@@ -661,4 +661,17 @@ config USB_SERIAL_DEBUG
 	  To compile this driver as a module, choose M here: the
 	  module will be called usb-debug.
 
+config USB_SERIAL_BASEBAND
+	tristate "USB Baseband Driver"
+	help
+	  Say Y here if you want to use USB baseband character driver.
+
+	  This driver may be used as a raw character device driver for
+	  USB modems.  For example, downloading modem firmware upon
+	  modem boot up (for flashless modems), or sending modem
+	  AT commands after modem software is running on device.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called baseband_chr.
+
 endif # USB_SERIAL
diff --git a/drivers/usb/serial/Makefile b/drivers/usb/serial/Makefile
index 9e536eefb32c..2654c327aaf2 100644
--- a/drivers/usb/serial/Makefile
+++ b/drivers/usb/serial/Makefile
@@ -13,6 +13,7 @@ usbserial-$(CONFIG_USB_EZUSB)		+= ezusb.o
 
 obj-$(CONFIG_USB_SERIAL_AIRCABLE)		+= aircable.o
 obj-$(CONFIG_USB_SERIAL_ARK3116)		+= ark3116.o
+obj-$(CONFIG_USB_SERIAL_BASEBAND)		+= baseband_usb_chr.o
 obj-$(CONFIG_USB_SERIAL_BELKIN)			+= belkin_sa.o
 obj-$(CONFIG_USB_SERIAL_CH341)			+= ch341.o
 obj-$(CONFIG_USB_SERIAL_CP210X)			+= cp210x.o
diff --git a/drivers/usb/serial/ark3116.c b/drivers/usb/serial/ark3116.c
index 5cdb9d912275..18e875b92e00 100644
--- a/drivers/usb/serial/ark3116.c
+++ b/drivers/usb/serial/ark3116.c
@@ -42,7 +42,7 @@ static int debug;
  * Version information
  */
 
-#define DRIVER_VERSION "v0.6"
+#define DRIVER_VERSION "v0.7"
 #define DRIVER_AUTHOR "Bart Hartgers <bart.hartgers+ark3116@gmail.com>"
 #define DRIVER_DESC "USB ARK3116 serial/IrDA driver"
 #define DRIVER_DEV_DESC "ARK3116 RS232/IrDA"
@@ -380,10 +380,6 @@ static int ark3116_open(struct tty_struct *tty, struct usb_serial_port *port)
 		goto err_out;
 	}
 
-	/* setup termios */
-	if (tty)
-		ark3116_set_termios(tty, port, NULL);
-
 	/* remove any data still left: also clears error state */
 	ark3116_read_reg(serial, UART_RX, buf);
 
@@ -406,6 +402,10 @@ static int ark3116_open(struct tty_struct *tty, struct usb_serial_port *port)
 	/* enable DMA */
 	ark3116_write_reg(port->serial, UART_FCR, UART_FCR_DMA_SELECT);
 
+	/* setup termios */
+	if (tty)
+		ark3116_set_termios(tty, port, NULL);
+
 err_out:
 	kfree(buf);
 	return result;
diff --git a/drivers/usb/serial/baseband_usb_chr.c b/drivers/usb/serial/baseband_usb_chr.c
new file mode 100644
index 000000000000..c4eda3fe1ab7
--- /dev/null
+++ b/drivers/usb/serial/baseband_usb_chr.c
@@ -0,0 +1,1302 @@
+/*
+ * baseband_usb_chr.c
+ *
+ * USB character driver to communicate with baseband modems.
+ *
+ * Copyright (c) 2012, NVIDIA 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/cdev.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/list.h>
+#include <linux/errno.h>
+#include <linux/usb.h>
+#include <linux/workqueue.h>
+#include <linux/uaccess.h>
+#include <linux/vmalloc.h>
+#include <asm/ioctls.h>
+#include "baseband_usb_chr.h"
+
+MODULE_LICENSE("GPL");
+
+/* To add new usb devices, update
+ * (1) baseband_usb_driver_id_table
+ *     - usb vendor id / product id
+ * (2) baseband_usb_driver_intf_table
+ *     - usb interface number
+ */
+
+static struct usb_device_id baseband_usb_driver_id_table[] = {
+	/* XMM modem #1 BOOT ROM */
+	{ USB_DEVICE(0x058b, 0x0041), },
+	/* XMM modem #2 BOOT ROM */
+	{ USB_DEVICE(0x8087, 0x0716), },
+	/* empty entry required to terminate list */
+	{ },
+};
+
+static unsigned int baseband_usb_driver_intf_table[] = {
+	/* XMM modem #1 BOOT ROM */
+	0x01,
+	/* XMM modem #2 BOOT ROM */
+	0x00,
+	/* empty entry required to terminate list */
+	0x00,
+};
+
+MODULE_DEVICE_TABLE(usb, baseband_usb_driver_id_table);
+
+static struct baseband_usb *baseband_usb_chr;
+static struct usb_interface *probe_usb_intf;
+static bool usb_device_connection;
+static struct workqueue_struct *chr_ipc_wq;
+
+static atomic_t g_rx_count = ATOMIC_INIT(0);
+
+struct usb_chr_resource {
+	unsigned char *ipc_rx;
+	unsigned char *ipc_tx;
+	unsigned char *rx_buf;
+};
+static struct usb_chr_resource usb_chr_res;
+
+/* baseband ipc functions */
+
+static void baseband_ipc_dump(const char *prefix, unsigned long int offset,
+	const void *buf, size_t bufsiz)
+{
+	size_t i;
+
+	for (i = 0; i < bufsiz; i += 16) {
+		pr_debug("%s"
+			"[%lx+%x] %p "
+			"%02x %02x %02x %02x "
+			"%02x %02x %02x %02x "
+			"%02x %02x %02x %02x "
+			"%02x %02x %02x %02x\n",
+			prefix,
+			offset,
+			i,
+			((const unsigned char *) buf) + i,
+			(i + 0 < bufsiz) ? ((const unsigned char *) buf)[i+0]
+				: 0xff,
+			(i + 1 < bufsiz) ? ((const unsigned char *) buf)[i+1]
+				: 0xff,
+			(i + 2 < bufsiz) ? ((const unsigned char *) buf)[i+2]
+				: 0xff,
+			(i + 3 < bufsiz) ? ((const unsigned char *) buf)[i+3]
+				: 0xff,
+			(i + 4 < bufsiz) ? ((const unsigned char *) buf)[i+4]
+				: 0xff,
+			(i + 5 < bufsiz) ? ((const unsigned char *) buf)[i+5]
+				: 0xff,
+			(i + 6 < bufsiz) ? ((const unsigned char *) buf)[i+6]
+				: 0xff,
+			(i + 7 < bufsiz) ? ((const unsigned char *) buf)[i+7]
+				: 0xff,
+			(i + 8 < bufsiz) ? ((const unsigned char *) buf)[i+8]
+				: 0xff,
+			(i + 9 < bufsiz) ? ((const unsigned char *) buf)[i+9]
+				: 0xff,
+			(i + 10 < bufsiz) ? ((const unsigned char *) buf)[i+10]
+				: 0xff,
+			(i + 11 < bufsiz) ? ((const unsigned char *) buf)[i+11]
+				: 0xff,
+			(i + 12 < bufsiz) ? ((const unsigned char *) buf)[i+12]
+				: 0xff,
+			(i + 13 < bufsiz) ? ((const unsigned char *) buf)[i+13]
+				: 0xff,
+			(i + 14 < bufsiz) ? ((const unsigned char *) buf)[i+14]
+				: 0xff,
+			(i + 15 < bufsiz) ? ((const unsigned char *) buf)[i+15]
+				: 0xff);
+	}
+
+}
+
+static size_t peek_ipc_tx_bufsiz(struct baseband_ipc *ipc,
+	size_t bufsiz)
+{
+	struct baseband_ipc_buf *ipc_buf, *ipc_buf_next;
+	size_t tx_bufsiz;
+
+	pr_debug("peek_ipc_tx_bufsiz\n");
+
+	/* check input */
+	if (!ipc) {
+		pr_err("%s: !ipc\n", __func__);
+		return 0;
+	}
+
+	/* acquire tx buffer semaphores */
+	if (down_interruptible(&ipc->buf_sem)) {
+		pr_err("peek_ipc_tx_bufsiz - "
+			"cannot acquire buffer semaphore\n");
+		return -ERESTARTSYS;
+	}
+
+	/* calculate maximum number of tx buffers which can be sent */
+	tx_bufsiz = 0;
+	list_for_each_entry_safe(ipc_buf, ipc_buf_next, &ipc->tx.buf, list)
+	{
+		pr_debug("peek_ipc_tx_bufsiz - "
+			"ipc_buf %p ipc_buf->offset %x ipc_buf->count %x\n",
+			ipc_buf, ipc_buf->offset, ipc_buf->count);
+		if (ipc_buf->count > bufsiz - tx_bufsiz)
+			break;
+		else
+			tx_bufsiz += ipc_buf->count;
+	}
+
+	/* release tx buffer semaphores */
+	up(&ipc->buf_sem);
+
+	return tx_bufsiz;
+}
+
+static size_t get_ipc_tx_buf(struct baseband_ipc *ipc,
+	void *buf, size_t bufsiz)
+{
+	struct baseband_ipc_buf *ipc_buf, *ipc_buf_next;
+	size_t tx_bufsiz;
+
+	pr_debug("get_ipc_tx_buf\n");
+
+	/* check input */
+	if (!ipc || !buf) {
+		pr_err("%s: !ipc || !buf\n", __func__);
+		return 0;
+	}
+	if (!bufsiz)
+		return 0;
+
+	/* acquire tx buffer semaphores */
+	if (down_interruptible(&ipc->buf_sem)) {
+		pr_err("get_ipc_tx_buf - "
+			"cannot acquire buffer semaphore\n");
+		return -ERESTARTSYS;
+	}
+
+	/* get tx data from tx linked list */
+	tx_bufsiz = 0;
+	list_for_each_entry_safe(ipc_buf, ipc_buf_next, &ipc->tx.buf, list)
+	{
+		pr_debug("get_ipc_tx_buf - "
+			"ipc_buf %p ipc_buf->offset %x ipc_buf->count %x\n",
+			ipc_buf, ipc_buf->offset, ipc_buf->count);
+		pr_debug("get_ipc_tx_buf - "
+			"ipc_buf->data [0] %x [1] %x [2] %x [3] %x\n",
+			ipc_buf->data[0],
+			ipc_buf->data[1],
+			ipc_buf->data[2],
+			ipc_buf->data[3]);
+		if (ipc_buf->count > bufsiz - tx_bufsiz) {
+			/* copy part of tx buffer */
+			memcpy(buf + tx_bufsiz,
+				ipc_buf->data + ipc_buf->offset,
+				bufsiz - tx_bufsiz);
+			ipc_buf->offset += bufsiz - tx_bufsiz;
+			ipc_buf->count -= bufsiz - tx_bufsiz;
+			tx_bufsiz = bufsiz;
+		} else {
+			/* copy all data from tx buffer */
+			memcpy(buf + tx_bufsiz,
+				ipc_buf->data + ipc_buf->offset,
+				ipc_buf->count);
+			tx_bufsiz += ipc_buf->count;
+			ipc_buf->offset = 0;
+			ipc_buf->count = 0;
+			/* add tx buffer to tx free list */
+			list_move_tail(&ipc_buf->list, &ipc->tx_free.buf);
+			wake_up(&ipc->tx_free.wait);
+		}
+		/* check if done */
+		if (tx_bufsiz == bufsiz)
+			break;
+	}
+
+	/* release tx buffer semaphores */
+	up(&ipc->buf_sem);
+
+	return tx_bufsiz;
+}
+
+static size_t put_ipc_rx_buf(struct baseband_ipc *ipc,
+	const void *buf, size_t bufsiz)
+{
+	struct baseband_ipc_buf *ipc_buf, *ipc_buf_next;
+	size_t rx_bufsiz;
+	int ret;
+
+	pr_debug("put_ipc_rx_buf\n");
+
+	/* check input */
+	if (!ipc || !buf) {
+		pr_err("%s: !ipc || !buf\n", __func__);
+		return 0;
+	}
+	if (!bufsiz)
+		return 0;
+
+	/* acquire rx buffer semaphores */
+retry:
+	if (down_interruptible(&ipc->buf_sem)) {
+		pr_err("put_ipc_rx_buf - "
+			"cannot acquire buffer semaphore\n");
+		return -ERESTARTSYS;
+	}
+
+	/* put rx data in rx linked list */
+	rx_bufsiz = 0;
+	list_for_each_entry_safe(ipc_buf, ipc_buf_next, &ipc->rx_free.buf, list)
+	{
+		pr_debug("put_ipc_rx_buf - "
+			"ipc_buf %p ipc_buf->offset %x ipc_buf->count %x\n",
+			ipc_buf, ipc_buf->offset, ipc_buf->count);
+		if (sizeof(ipc_buf->data) > bufsiz - rx_bufsiz) {
+			/* partially fill rx free buffer */
+			memcpy(ipc_buf->data,
+				buf + rx_bufsiz,
+				bufsiz - rx_bufsiz);
+			ipc_buf->offset = 0;
+			ipc_buf->count = bufsiz - rx_bufsiz;
+			rx_bufsiz = bufsiz;
+		} else {
+			/* fill entire rx free buffer */
+			memcpy(ipc_buf->data,
+				buf + rx_bufsiz,
+				sizeof(ipc_buf->data));
+			ipc_buf->offset = 0;
+			ipc_buf->count = sizeof(ipc_buf->data);
+			rx_bufsiz += sizeof(ipc_buf->data);
+		}
+		/* add filled rx free buffer to rx linked list */
+		list_move_tail(&ipc_buf->list, &ipc->rx.buf);
+		wake_up(&ipc->rx.wait);
+		/* check if done */
+		if (rx_bufsiz == bufsiz)
+			break;
+	}
+
+	/* release rx buffer semaphores */
+	up(&ipc->buf_sem);
+
+	/* wait for rx free buffer available */
+	if (!rx_bufsiz) {
+		ret = wait_event_interruptible_timeout(ipc->rx_free.wait,
+			!list_empty(&ipc->rx_free.buf), HZ*2);
+		if (ret == 0) {
+			pr_err("%s timeout occured no wait\n", __func__);
+			return -ETIMEDOUT;
+		}
+		if (ret == -ERESTARTSYS) {
+			pr_err("put_ipc_rx_buf - "
+				"interrupted wait\n");
+			return -ERESTARTSYS;
+		}
+		goto retry;
+	}
+
+	return rx_bufsiz;
+
+}
+
+static ssize_t baseband_ipc_file_read(struct baseband_ipc *ipc,
+	struct file *file, char *buf, size_t count, loff_t *pos)
+{
+	struct baseband_ipc_buf *ipc_buf, *ipc_buf_next;
+	size_t read_count;
+
+	pr_debug("baseband_ipc_file_read\n");
+
+	/* check input */
+	if (!ipc || !buf) {
+		pr_err("%s: !ipc || !buf\n", __func__);
+		return -EIO;
+	}
+
+	/* acquire rx buffer semaphores */
+retry:
+	if (down_interruptible(&ipc->buf_sem)) {
+		pr_err("baseband_ipc_file_read - "
+			"cannot acquire buffer semaphore\n");
+		return -ERESTARTSYS;
+	}
+
+	/* get read data from rx linked list */
+	read_count = 0;
+	list_for_each_entry_safe(ipc_buf, ipc_buf_next, &ipc->rx.buf, list)
+	{
+		pr_debug("baseband_ipc_file_read - "
+			"ipc_buf %p ipc_buf->offset %x ipc_buf->count %x\n",
+			ipc_buf, ipc_buf->offset, ipc_buf->count);
+		pr_debug("baseband_ipc_file_read - "
+			"ipc_buf->data [0] %x [1] %x [2] %x [3] %x\n",
+			ipc_buf->data[0],
+			ipc_buf->data[1],
+			ipc_buf->data[2],
+			ipc_buf->data[3]);
+		if (ipc_buf->count > count - read_count) {
+			/* copy part of rx buffer */
+			if (copy_to_user(buf + read_count,
+				ipc_buf->data + ipc_buf->offset,
+				count - read_count)) {
+				pr_err("copy_to_user failed\n");
+				up(&ipc->buf_sem);
+				return -EFAULT;
+			}
+			ipc_buf->offset += count - read_count;
+			ipc_buf->count -= count - read_count;
+			read_count = count;
+		} else {
+			/* copy all data from rx buffer */
+			if (copy_to_user(buf + read_count,
+				ipc_buf->data + ipc_buf->offset,
+				ipc_buf->count)) {
+				pr_err("copy_to_user failed\n");
+				up(&ipc->buf_sem);
+				return -EFAULT;
+			}
+			read_count += ipc_buf->count;
+			ipc_buf->offset = 0;
+			ipc_buf->count = 0;
+			/* add rx buffer to rx free list */
+			list_move_tail(&ipc_buf->list, &ipc->rx_free.buf);
+			wake_up(&ipc->rx_free.wait);
+		}
+		/* check if done */
+		if (read_count == count)
+			break;
+	}
+
+	/* release rx buffer semaphores */
+	up(&ipc->buf_sem);
+
+	/* wait for rx buffer available */
+	if (!read_count) {
+		if (wait_event_interruptible(ipc->rx.wait,
+			!list_empty(&ipc->rx.buf))) {
+			pr_err("baseband_ipc_file_read - "
+				"interrupted wait\n");
+			return -ERESTARTSYS;
+		}
+		goto retry;
+	}
+
+	return read_count;
+}
+
+static ssize_t baseband_ipc_file_write(struct baseband_ipc *ipc,
+	struct file *file, const char *buf, size_t count, loff_t *pos)
+{
+	struct baseband_ipc_buf *ipc_buf, *ipc_buf_next;
+	size_t write_count;
+
+	pr_debug("baseband_ipc_file_write\n");
+
+	/* check input */
+	if (!ipc || !buf) {
+		pr_err("%s: !ipc || !buf\n", __func__);
+		return -EIO;
+	}
+
+	/* do not accept write if previous tx not finished */
+	if (peek_ipc_tx_bufsiz(ipc, USB_CHR_TX_BUFSIZ) != 0) {
+		pr_debug("%s: not accepting write of %u bytes"
+			" - previous tx not finished\n",
+			__func__, count);
+		return 0;
+	}
+
+	/* acquire tx buffer semaphores */
+retry:
+	if (down_interruptible(&ipc->buf_sem)) {
+		pr_err("baseband_ipc_file_write - "
+			"cannot acquire buffer semaphore\n");
+		return -ERESTARTSYS;
+	}
+
+	/* put write data in tx linked list */
+	write_count = 0;
+	list_for_each_entry_safe(ipc_buf, ipc_buf_next, &ipc->tx_free.buf, list)
+	{
+		pr_debug("baseband_ipc_file_write - "
+			"ipc_buf %p ipc_buf->offset %x ipc_buf->count %x\n",
+			ipc_buf, ipc_buf->offset, ipc_buf->count);
+		if (sizeof(ipc_buf->data) > count - write_count) {
+			/* partially fill tx free buffer */
+			if (copy_from_user(ipc_buf->data,
+				buf + write_count,
+				count - write_count)) {
+				pr_err("copy_from_user failed\n");
+				up(&ipc->buf_sem);
+				return -EFAULT;
+			}
+			ipc_buf->offset = 0;
+			ipc_buf->count = count - write_count;
+			write_count = count;
+		} else {
+			/* fill entire tx free buffer */
+			if (copy_from_user(ipc_buf->data,
+				buf + write_count,
+				sizeof(ipc_buf->data))) {
+				pr_err("copy_from_user failed\n");
+				up(&ipc->buf_sem);
+				return -EFAULT;
+			}
+			ipc_buf->offset = 0;
+			ipc_buf->count = sizeof(ipc_buf->data);
+			write_count += sizeof(ipc_buf->data);
+		}
+		/* add filled tx free buffer to tx linked list */
+		pr_debug("baseband_ipc_file_write - "
+			"ipc_buf->data [0] %x [1] %x [2] %x [3] %x\n",
+			ipc_buf->data[0],
+			ipc_buf->data[1],
+			ipc_buf->data[2],
+			ipc_buf->data[3]);
+		list_move_tail(&ipc_buf->list, &ipc->tx.buf);
+		wake_up(&ipc->tx.wait);
+		/* check if done */
+		if (write_count == count)
+			break;
+	}
+
+	/* release tx buffer semaphores */
+	up(&ipc->buf_sem);
+
+	/* wait for tx buffer available */
+	if (!write_count) {
+		if (wait_event_interruptible(ipc->tx_free.wait,
+			!list_empty(&ipc->tx_free.buf))) {
+			pr_err("baseband_ipc_file_write - "
+				"interrupted wait\n");
+			return -ERESTARTSYS;
+		}
+		goto retry;
+	}
+
+	/* queue ipc transaction work */
+	queue_work(ipc->workqueue, &ipc->work);
+
+	return write_count;
+}
+
+static void baseband_ipc_close(struct baseband_ipc *ipc)
+{
+	struct baseband_ipc_buf *ipc_buf, *ipc_buf_next;
+
+	pr_debug("baseband_ipc_close {\n");
+
+	/* check input */
+	if (!ipc)
+		return;
+
+	/* cancel work queue */
+	if (ipc->workqueue) {
+		pr_debug("destroy workqueue {\n");
+		cancel_work_sync(&ipc->work);
+		pr_debug("destroy workqueue }\n");
+	}
+	memset(&ipc->work, 0, sizeof(ipc->work));
+
+	/* destroy wait queues */
+	memset(&ipc->tx_free.wait, 0, sizeof(ipc->tx_free.wait));
+	memset(&ipc->rx_free.wait, 0, sizeof(ipc->rx_free.wait));
+	memset(&ipc->tx.wait, 0, sizeof(ipc->tx.wait));
+	memset(&ipc->rx.wait, 0, sizeof(ipc->rx.wait));
+
+	/* destroy data buffers */
+	ipc->ipc_tx = (unsigned char *) 0;
+	ipc->ipc_rx = (unsigned char *) 0;
+	list_for_each_entry_safe(ipc_buf, ipc_buf_next, &ipc->tx_free.buf, list)
+	{
+		vfree(ipc_buf);
+	}
+	list_for_each_entry_safe(ipc_buf, ipc_buf_next, &ipc->rx_free.buf, list)
+	{
+		vfree(ipc_buf);
+	}
+	list_for_each_entry_safe(ipc_buf, ipc_buf_next, &ipc->tx.buf, list)
+	{
+		vfree(ipc_buf);
+	}
+	list_for_each_entry_safe(ipc_buf, ipc_buf_next, &ipc->rx.buf, list)
+	{
+		vfree(ipc_buf);
+	}
+
+	/* destroy semaphores */
+	memset(&ipc->buf_sem, 0, sizeof(ipc->buf_sem));
+
+	/* free baseband ipc structure */
+	vfree(ipc);
+
+	pr_debug("baseband_ipc_close }\n");
+}
+
+static struct baseband_ipc *baseband_ipc_open(work_func_t work_func,
+	work_func_t rx_work_func,
+	work_func_t tx_work_func)
+{
+	struct baseband_ipc *ipc;
+	struct baseband_ipc_buf *ipc_buf;
+	int i;
+
+	pr_debug("baseband_ipc_open {\n");
+
+	/* allocate baseband ipc structure */
+	ipc = vmalloc(sizeof(struct baseband_ipc));
+	if (!ipc)
+		return (struct baseband_ipc *) 0;
+
+	memset(ipc, 0 , sizeof(struct baseband_ipc));
+	/* create semaphores */
+	sema_init(&ipc->buf_sem, 1);
+
+	/* create data buffers */
+	INIT_LIST_HEAD(&ipc->rx.buf);
+	INIT_LIST_HEAD(&ipc->tx.buf);
+	INIT_LIST_HEAD(&ipc->rx_free.buf);
+	INIT_LIST_HEAD(&ipc->tx_free.buf);
+	for (i = 0; i < BASEBAND_IPC_NUM_RX_BUF; i++) {
+		ipc_buf = (struct baseband_ipc_buf *)
+			vmalloc(sizeof(struct baseband_ipc_buf));
+		if (!ipc_buf) {
+			pr_err("cannot allocate baseband ipc rx buffer #%d\n",
+				i);
+			goto error_exit;
+		}
+		pr_debug("baseband_ipc_open - "
+			"rx_free: ipc_buf %p\n",
+			ipc_buf);
+		list_add_tail(&ipc_buf->list, &ipc->rx_free.buf);
+	}
+	for (i = 0; i < BASEBAND_IPC_NUM_TX_BUF; i++) {
+		ipc_buf = (struct baseband_ipc_buf *)
+			vmalloc(sizeof(struct baseband_ipc_buf));
+		if (!ipc_buf) {
+			pr_err("cannot allocate baseband ipc tx buffer #%d\n",
+				i);
+			goto error_exit;
+		}
+		pr_debug("baseband_ipc_open - "
+			"tx_free: ipc_buf %p\n",
+			ipc_buf);
+		list_add_tail(&ipc_buf->list, &ipc->tx_free.buf);
+	}
+	ipc->ipc_rx = usb_chr_res.ipc_rx;
+	if (!ipc->ipc_rx) {
+		pr_err("%s: cannot find ipc->ipc_rx\n", __func__);
+		goto error_exit;
+	}
+	ipc->ipc_tx = usb_chr_res.ipc_tx;
+	if (!ipc->ipc_tx) {
+		pr_err("%s: cannot find ipc->ipc_tx\n", __func__);
+		goto error_exit;
+	}
+
+	/* create wait queues */
+	init_waitqueue_head(&ipc->rx.wait);
+	init_waitqueue_head(&ipc->tx.wait);
+	init_waitqueue_head(&ipc->rx_free.wait);
+	init_waitqueue_head(&ipc->tx_free.wait);
+
+	/* init work queue */
+	if (chr_ipc_wq)
+		ipc->workqueue = chr_ipc_wq;
+	else {
+		pr_err("%s: no workqueue found\n", __func__);
+		goto error_exit;
+	}
+	if (work_func)
+		INIT_WORK(&ipc->work, work_func);
+	if (rx_work_func)
+		INIT_WORK(&ipc->rx_work, rx_work_func);
+	if (tx_work_func)
+		INIT_WORK(&ipc->tx_work, tx_work_func);
+
+	pr_debug("baseband_ipc_open }\n");
+	return ipc;
+
+error_exit:
+	baseband_ipc_close(ipc);
+	return (struct baseband_ipc *) 0;
+}
+
+/* usb rx */
+
+static void baseband_usb_chr_rx_urb_comp(struct urb *urb)
+{
+	struct baseband_usb *usb = (struct baseband_usb *) urb->context;
+
+	pr_debug("baseband_usb_chr_rx_urb_comp { urb %p\n", urb);
+
+	/* check input */
+	if (!usb) {
+		pr_err("%s: !usb\n", __func__);
+		return;
+	}
+	if (!usb->ipc) {
+		pr_err("%s: !usb->ipc\n", __func__);
+		return;
+	}
+	if (!usb->ipc->workqueue) {
+		pr_err("%s: !usb->ipc->rx_work\n", __func__);
+		return;
+	}
+
+	switch (urb->status) {
+	case 0:
+		/* success */
+		break;
+	case -ENOENT:
+	case -ESHUTDOWN:
+	case -EPROTO:
+		pr_info("%s: link down\n", __func__);
+	default:
+		pr_err("%s: urb error status %d\n", __func__, urb->status);
+		return;
+	}
+
+	/* queue rx urb completion work */
+	queue_work(usb->ipc->workqueue, &usb->ipc->rx_work);
+
+	pr_debug("baseband_usb_chr_rx_urb_comp }\n");
+}
+
+static int baseband_usb_chr_rx_urb_submit(struct baseband_usb *usb)
+{
+	struct urb *urb;
+	void *buf;
+	int err;
+
+	pr_debug("baseband_usb_chr_rx_urb_submit { usb %p\n", usb);
+
+	/* check input */
+	if (!usb_device_connection) {
+		pr_err("%s: no usb device connection\n", __func__);
+		return -1;
+	}
+	if (!usb->usb.rx_urb) {
+		pr_err("%s: no rx urb!\n", __func__);
+		return -1;
+	}
+
+	/* fill rx urb */
+	urb = usb->usb.rx_urb;
+	buf = usb->usb.rx_urb->transfer_buffer;
+	usb_fill_bulk_urb(urb, usb->usb.device, usb->usb.pipe.bulk.in,
+		buf, USB_CHR_RX_BUFSIZ,
+		baseband_usb_chr_rx_urb_comp,
+		usb);
+	urb->transfer_flags = 0;
+
+	/* submit rx urb */
+	usb->usb.rx_urb = urb;
+	err = usb_submit_urb(urb, GFP_KERNEL);
+	if (err < 0) {
+		pr_err("usb_submit_urb() failed - err %d\n", err);
+		return err;
+	}
+
+	pr_debug("baseband_usb_chr_rx_urb_submit }\n");
+	return err;
+}
+
+static void baseband_usb_chr_rx_urb_comp_work(struct work_struct *work)
+{
+	struct baseband_usb *usb = baseband_usb_chr;
+	struct urb *urb = usb->usb.rx_urb;
+	size_t len;
+
+	pr_debug("baseband_usb_chr_rx_urb_comp_work { work %p\n", work);
+
+	if (usb_device_connection == false) {
+		/* device is closing or disconnect - nothing to read */
+		pr_info("%s: device is disconnected\n", __func__);
+		return;
+	}
+	/* put rx urb data in rx buffer */
+	if (urb->actual_length) {
+		pr_debug("baseband_usb_chr_rx_urb_comp_work - "
+			"urb->actual_length %d\n", urb->actual_length);
+		len = put_ipc_rx_buf(usb->ipc,
+			urb->transfer_buffer, urb->actual_length);
+		if (len == -ETIMEDOUT) {
+			/* device closed */
+			pr_info("%s: device closed\n", __func__);
+			return;
+		}
+		baseband_ipc_dump("baseband_usb_chr_rx_urb_comp_work"
+			" - rx buf ", 0,
+			urb->transfer_buffer, len > 16 ? 16 : len);
+		if (len != urb->actual_length) {
+			pr_err("baseband_usb_chr_rx_urb_comp_work - "
+				"put_ipx_rx_buf() only put %d/%d bytes\n",
+				len, urb->actual_length);
+		}
+		/* increment count of available rx bytes */
+		atomic_add(len, &g_rx_count);
+	}
+
+	/* submit next rx urb */
+	baseband_usb_chr_rx_urb_submit(usb);
+
+	pr_debug("baseband_usb_chr_rx_urb_comp_work }\n");
+}
+
+/* usb functions */
+
+static void find_usb_pipe(struct baseband_usb *usb)
+{
+	struct usb_device *usbdev = usb->usb.device;
+	struct usb_interface *intf = usb->usb.interface;
+	unsigned char numendpoint = intf->cur_altsetting->desc.bNumEndpoints;
+	struct usb_host_endpoint *endpoint = intf->cur_altsetting->endpoint;
+	unsigned char n;
+
+	for (n = 0; n < numendpoint; n++) {
+		if (usb_endpoint_is_isoc_in(&endpoint[n].desc)) {
+			pr_debug("endpoint[%d] isochronous in\n", n);
+			usb->usb.pipe.isoch.in = usb_rcvisocpipe(usbdev,
+				endpoint[n].desc.bEndpointAddress);
+		} else if (usb_endpoint_is_isoc_out(&endpoint[n].desc)) {
+			pr_debug("endpoint[%d] isochronous out\n", n);
+			usb->usb.pipe.isoch.out = usb_sndisocpipe(usbdev,
+				endpoint[n].desc.bEndpointAddress);
+		} else if (usb_endpoint_is_bulk_in(&endpoint[n].desc)) {
+			pr_debug("endpoint[%d] bulk in\n", n);
+			usb->usb.pipe.bulk.in = usb_rcvbulkpipe(usbdev,
+				endpoint[n].desc.bEndpointAddress);
+		} else if (usb_endpoint_is_bulk_out(&endpoint[n].desc)) {
+			pr_debug("endpoint[%d] bulk out\n", n);
+			usb->usb.pipe.bulk.out = usb_sndbulkpipe(usbdev,
+				endpoint[n].desc.bEndpointAddress);
+		} else if (usb_endpoint_is_int_in(&endpoint[n].desc)) {
+			pr_debug("endpoint[%d] interrupt in\n", n);
+			usb->usb.pipe.interrupt.in = usb_rcvintpipe(usbdev,
+				endpoint[n].desc.bEndpointAddress);
+		} else if (usb_endpoint_is_int_out(&endpoint[n].desc)) {
+			pr_debug("endpoint[%d] interrupt out\n", n);
+			usb->usb.pipe.interrupt.out = usb_sndintpipe(usbdev,
+				endpoint[n].desc.bEndpointAddress);
+		} else {
+			pr_debug("endpoint[%d] skipped\n", n);
+		}
+	}
+}
+
+static int baseband_usb_driver_probe(struct usb_interface *intf,
+	const struct usb_device_id *id)
+{
+	int i;
+
+	pr_debug("%s(%d) { intf %p id %p\n", __func__, __LINE__, intf, id);
+
+	pr_debug("intf->cur_altsetting->desc.bInterfaceNumber %02x\n",
+		intf->cur_altsetting->desc.bInterfaceNumber);
+	pr_debug("intf->cur_altsetting->desc.bAlternateSetting %02x\n",
+		intf->cur_altsetting->desc.bAlternateSetting);
+	pr_debug("intf->cur_altsetting->desc.bNumEndpoints %02x\n",
+		intf->cur_altsetting->desc.bNumEndpoints);
+	pr_debug("intf->cur_altsetting->desc.bInterfaceClass %02x\n",
+		intf->cur_altsetting->desc.bInterfaceClass);
+	pr_debug("intf->cur_altsetting->desc.bInterfaceSubClass %02x\n",
+		intf->cur_altsetting->desc.bInterfaceSubClass);
+	pr_debug("intf->cur_altsetting->desc.bInterfaceProtocol %02x\n",
+		intf->cur_altsetting->desc.bInterfaceProtocol);
+	pr_debug("intf->cur_altsetting->desc.iInterface %02x\n",
+		intf->cur_altsetting->desc.iInterface);
+
+	/* usb interface mismatch */
+	for (i = 0; baseband_usb_driver_id_table[i].match_flags; i++) {
+		if (id == &baseband_usb_driver_id_table[i]) {
+			if (baseband_usb_driver_intf_table[i] !=
+				intf->cur_altsetting->desc.bInterfaceNumber) {
+				pr_debug("%s(%d) } -ENODEV\n", __func__, __LINE__);
+				return -ENODEV;
+			}
+		}
+	}
+
+	/* usb interface match */
+	probe_usb_intf = intf;
+	usb_device_connection = true;
+
+	pr_debug("%s(%d) }\n", __func__, __LINE__);
+	return 0;
+}
+
+static void baseband_usb_driver_disconnect(struct usb_interface *intf)
+{
+	struct usb_device *usb_dev = interface_to_usbdev(intf);
+	pr_debug("%s(%d) { intf %p\n", __func__, __LINE__, intf);
+
+	if (!baseband_usb_chr) {
+		pr_err("%s: no baseband_usb_chr\n", __func__);
+		return;
+	}
+
+	if (baseband_usb_chr->usb.interface != intf) {
+		pr_info("%s(%d) -ENODEV\n", __func__, __LINE__);
+		return;
+	}
+	if (baseband_usb_chr->usb.device == usb_dev) {
+		pr_info("%s: Matching usb device: Flush workqueue\n", __func__);
+		/* flush queued ipc transaction work */
+		if (baseband_usb_chr && baseband_usb_chr->ipc
+			&& baseband_usb_chr->ipc->workqueue)
+			flush_workqueue(baseband_usb_chr->ipc->workqueue);
+		usb_device_connection = false;
+		probe_usb_intf = NULL;
+	}
+	pr_debug("%s(%d) }\n", __func__, __LINE__);
+}
+
+static struct usb_driver baseband_usb_driver = {
+	.name = "bb_usb_chr",
+	.probe = baseband_usb_driver_probe,
+	.disconnect = baseband_usb_driver_disconnect,
+	.id_table = baseband_usb_driver_id_table,
+};
+
+static void baseband_usb_chr_work(struct work_struct *work)
+{
+	struct baseband_usb *usb = baseband_usb_chr;
+	struct {
+		unsigned char *buf;
+		unsigned int bufsiz_byte;
+	} rx, tx;
+	int ipc_tx_byte;
+	int err;
+
+	pr_debug("baseband_usb_chr_work {\n");
+
+	/* check input */
+	if (!usb || !usb->ipc) {
+		pr_err("baseband_usb_chr_work - "
+			"usb not open\n");
+		return;
+	}
+	if (!usb->usb.device) {
+		pr_err("baseband_usb_chr_work - "
+			"usb device not probed yet\n");
+		mdelay(10);
+		queue_work(usb->ipc->workqueue, &usb->ipc->work);
+		return;
+	}
+	if (!usb->ipc->ipc_rx) {
+		pr_err("baseband_usb_chr_work - "
+			"null usb->ipc->ipc_rx\n");
+		return;
+	}
+	if (!usb->ipc->ipc_tx) {
+		pr_err("baseband_usb_chr_work - "
+			"null usb->ipc->ipc_tx\n");
+		return;
+	}
+
+	/* usb transaction loop */
+	rx.buf = usb->ipc->ipc_rx;
+	tx.buf = usb->ipc->ipc_tx;
+	while ((tx.bufsiz_byte = peek_ipc_tx_bufsiz(usb->ipc,
+		USB_CHR_TX_BUFSIZ)) != 0) {
+		get_ipc_tx_buf(usb->ipc, tx.buf, tx.bufsiz_byte);
+		err = usb_bulk_msg(usb->usb.device, usb->usb.pipe.bulk.out,
+			tx.buf, tx.bufsiz_byte, &ipc_tx_byte, USB_CHR_TIMEOUT);
+		if (err < 0) {
+			pr_err("baseband_usb_chr_work - "
+				"usb_bulk_msg err %d\n", err);
+			continue;
+		}
+		if (tx.bufsiz_byte != ipc_tx_byte) {
+			pr_err("tx.bufsiz_byte %d != ipc_tx_byte %d\n",
+				tx.bufsiz_byte, ipc_tx_byte);
+			continue;
+		}
+	}
+
+	pr_debug("baseband_usb_chr_work }\n");
+}
+
+/* usb device driver functions */
+
+static void baseband_usb_close(struct baseband_usb *usb)
+{
+	pr_debug("baseband_usb_close {\n");
+
+	/* check input */
+	if (!usb)
+		return;
+
+	/* we need proper lock, maybe...*/
+	usb_device_connection = false;
+
+	/* free re-usable rx urb + rx urb transfer buffer */
+	if (usb->usb.rx_urb) {
+		pr_debug("%s: free rx urb\n", __func__);
+		usb_kill_urb(usb->usb.rx_urb);
+		if (usb->usb.rx_urb->transfer_buffer) {
+			pr_debug("%s: free rx urb transfer buffer\n", __func__);
+			usb->usb.rx_urb->transfer_buffer = (void *) 0;
+		}
+	}
+
+	if (usb->ipc) {
+		flush_work_sync(&usb->ipc->work);
+		flush_work_sync(&usb->ipc->rx_work);
+	}
+
+
+	/* close baseband ipc */
+	if (usb->ipc) {
+		baseband_ipc_close(usb->ipc);
+		usb_free_urb(usb->usb.rx_urb);
+		usb->usb.rx_urb = NULL;
+		usb->ipc = NULL;
+	}
+
+	/* free baseband usb structure */
+	vfree(usb);
+
+	pr_debug("baseband_usb_close }\n");
+}
+
+static struct baseband_usb *baseband_usb_open(work_func_t work_func,
+	work_func_t rx_work_func,
+	work_func_t tx_work_func)
+{
+	struct baseband_usb *usb;
+	int err, i;
+	struct urb *urb;
+	void *buf;
+
+	pr_debug("baseband_usb_open {\n");
+
+	/* allocate baseband usb structure */
+	usb = vmalloc(sizeof(struct baseband_usb));
+	if (!usb)
+		return (struct baseband_usb *) 0;
+	memset(usb, 0, sizeof(struct baseband_usb));
+
+	/* open baseband ipc */
+	usb->ipc = baseband_ipc_open(work_func,
+		rx_work_func,
+		tx_work_func);
+	if (!usb->ipc) {
+		pr_err("open baseband ipc failed\n");
+		goto error_exit;
+	}
+
+	usb->usb.driver = &baseband_usb_driver;
+
+	for (i = 0; i < 5 * 50; i++) {
+		if (probe_usb_intf && usb_device_connection)
+			break;
+		/* wait for probe */
+		pr_debug("%s: waiting for usb probe...\n", __func__);
+		msleep(20);
+	}
+	if (!probe_usb_intf || !usb_device_connection) {
+		pr_err("%s: probe timed out!\n", __func__);
+		goto error_exit;
+	}
+
+	/* get probed usb device information */
+	usb->usb.device = interface_to_usbdev(probe_usb_intf);
+	usb->usb.interface = probe_usb_intf;
+	find_usb_pipe(usb);
+	usb->usb.rx_urb = (struct urb *) 0;
+	usb->usb.tx_urb = (struct urb *) 0;
+	pr_debug("usb->usb.driver->name %s\n",
+		usb->usb.driver->name);
+	pr_debug("usb->usb.device %p\n",
+		usb->usb.device);
+	pr_debug("usb->usb.interface %p\n",
+		usb->usb.interface);
+	pr_debug("usb->usb.pipe.isoch.in %x\n",
+		usb->usb.pipe.isoch.in);
+	pr_debug("usb->usb.pipe.isoch.out %x\n",
+		usb->usb.pipe.isoch.out);
+	pr_debug("usb->usb.pipe.bulk.in %x\n",
+		usb->usb.pipe.bulk.in);
+	pr_debug("usb->usb.pipe.bulk.out %x\n",
+		usb->usb.pipe.bulk.out);
+	pr_debug("usb->usb.pipe.interrupt.in %x\n",
+		usb->usb.pipe.interrupt.in);
+	pr_debug("usb->usb.pipe.interrupt.out %x\n",
+		usb->usb.pipe.interrupt.out);
+
+	/* allocate re-usable rx urb + rx urb transfer buffer */
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!urb) {
+		pr_err("usb_alloc_urb() failed\n");
+		goto error_exit;
+	}
+	buf = usb_chr_res.rx_buf;
+	if (!buf) {
+		pr_err("%s: usb rx buffer not found\n", __func__);
+		usb_free_urb(urb);
+		goto error_exit;
+	}
+	urb->transfer_buffer = buf;
+	usb->usb.rx_urb = urb;
+
+	/* start usb rx */
+	err = baseband_usb_chr_rx_urb_submit(usb);
+	if (err < 0) {
+		pr_err("submit rx failed - err %d\n", err);
+		goto error_exit;
+	}
+
+	pr_debug("baseband_usb_open }\n");
+	return usb;
+
+error_exit:
+	baseband_usb_close(usb);
+	baseband_usb_chr = (struct baseband_usb *) 0;
+	return (struct baseband_usb *) 0;
+}
+
+
+/* usb character file operations */
+
+static int baseband_usb_chr_open(struct inode *inode, struct file *file)
+{
+	pr_debug("baseband_usb_chr_open {\n");
+
+	if (baseband_usb_chr) {
+		pr_err("%s: device is already open\n", __func__);
+		/* application uses two fd opens for download*/
+		baseband_usb_chr->ref++;
+		return 0;
+	}
+
+	/* open baseband usb */
+	baseband_usb_chr = baseband_usb_open(baseband_usb_chr_work,
+				baseband_usb_chr_rx_urb_comp_work,
+				(work_func_t) 0);
+	if (!baseband_usb_chr) {
+		pr_err("cannot open baseband usb chr\n");
+		return -ENODEV;
+	}
+	baseband_usb_chr->ref++;
+
+	if (!try_module_get(THIS_MODULE))
+		return -ENODEV;
+
+	pr_debug("baseband_usb_chr_open }\n");
+	return 0;
+}
+
+static int baseband_usb_chr_release(struct inode *inode, struct file *file)
+{
+	pr_debug("baseband_usb_chr_release\n");
+	pr_info("baseband_usb_chr_release {\n");
+
+	if (baseband_usb_chr) {
+		baseband_usb_chr->ref--;
+		if (baseband_usb_chr->ref)
+			return 0;
+
+		/* close baseband usb */
+		baseband_usb_close(baseband_usb_chr);
+		baseband_usb_chr = (struct baseband_usb *) 0;
+	}
+
+	module_put(THIS_MODULE);
+	pr_info("baseband_usb_chr_release }\n");
+
+	return 0;
+}
+
+static ssize_t baseband_usb_chr_read(struct file *file, char *buf,
+	size_t count, loff_t *pos)
+{
+	ssize_t ret;
+
+	pr_debug("baseband_usb_chr_read\n");
+
+	if (!baseband_usb_chr || !baseband_usb_chr->ipc) {
+		pr_err("%s: -ENODEV\n", __func__);
+		return -ENODEV;
+	}
+	ret = baseband_ipc_file_read(baseband_usb_chr->ipc,
+		file, buf, count, pos);
+	if (ret > 0) {
+		/* decrement count of available rx bytes */
+		int val = atomic_read(&g_rx_count);
+		pr_debug("baseband_usb_chr_read - read %d unread %d\n",
+			ret, val - ret);
+		atomic_sub(ret, &g_rx_count);
+	}
+	return ret;
+}
+
+static ssize_t baseband_usb_chr_write(struct file *file, const char *buf,
+	size_t count, loff_t *pos)
+{
+	pr_debug("baseband_usb_chr_write\n");
+	if (!baseband_usb_chr || !baseband_usb_chr->ipc) {
+		pr_err("%s: -ENODEV\n", __func__);
+		return -ENODEV;
+	}
+	return baseband_ipc_file_write(baseband_usb_chr->ipc,
+		file, buf, count, pos);
+}
+
+static long baseband_usb_chr_ioctl(struct file *file, unsigned int cmd,
+	unsigned long arg)
+{
+	pr_debug("baseband_usb_chr_ioctl\n");
+	switch (cmd) {
+	case TCFLSH:
+		pr_debug("TCFLSH\n");
+		/* flush queued ipc transaction work */
+		if (!baseband_usb_chr || !baseband_usb_chr->ipc
+			|| !baseband_usb_chr->ipc->workqueue) {
+			pr_err("%s: no workqueue!\n", __func__);
+			return -ENODEV;
+		}
+		flush_workqueue(baseband_usb_chr->ipc->workqueue);
+		return 0;
+	case FIONREAD:
+		pr_debug("FIONREAD\n");
+		/* return count of available rx bytes */
+		{
+			int __user *p = (int __user *) arg;
+			int val = atomic_read(&g_rx_count);
+			if (put_user(val, p))
+				break;
+		}
+		return 0;
+	default:
+		pr_err("unsupported ioctl cmd %x\n", cmd);
+		return 0;
+	}
+	return -ENODEV;
+}
+
+static const struct file_operations baseband_usb_chr_fops = {
+	.open = baseband_usb_chr_open,
+	.release = baseband_usb_chr_release,
+	.read = baseband_usb_chr_read,
+	.write = baseband_usb_chr_write,
+	.unlocked_ioctl = baseband_usb_chr_ioctl,
+};
+
+/* module init / exit functions */
+
+static int baseband_usb_chr_init(void)
+{
+	int err = -ENOMEM;
+
+	pr_debug("baseband_usb_chr_init {\n");
+
+	usb_chr_res.ipc_rx = kmalloc(USB_CHR_RX_BUFSIZ, GFP_KERNEL);
+	if (!usb_chr_res.ipc_rx) {
+		pr_err("cannot allocate ipc_rx\n");
+		goto error;
+	}
+	usb_chr_res.ipc_tx = kmalloc(USB_CHR_TX_BUFSIZ, GFP_KERNEL);
+	if (!usb_chr_res.ipc_tx) {
+		pr_err("cannot allocate ipc_tx\n");
+		goto error;
+	}
+	usb_chr_res.rx_buf = kmalloc(USB_CHR_RX_BUFSIZ, GFP_KERNEL);
+	if (!usb_chr_res.rx_buf) {
+		pr_err("%s: usb buffer kmalloc() failed\n", __func__);
+		goto error;
+	}
+
+	/* register character device */
+	err = register_chrdev(BASEBAND_USB_CHR_DEV_MAJOR,
+		BASEBAND_USB_CHR_DEV_NAME,
+		&baseband_usb_chr_fops);
+	if (err < 0) {
+		pr_err("cannot register character device - %d\n", err);
+		goto error;
+	}
+	pr_debug("registered baseband usb character device - major %d\n",
+		BASEBAND_USB_CHR_DEV_MAJOR);
+
+	/* create workqueue thread */
+	chr_ipc_wq = create_singlethread_workqueue("baseband_chr_wq");
+	if (chr_ipc_wq == NULL) {
+		pr_err("cannot create workqueue\n");
+		unregister_chrdev(BASEBAND_USB_CHR_DEV_MAJOR,
+			BASEBAND_USB_CHR_DEV_NAME);
+		err = -ENODEV;
+		goto error;
+	}
+
+	/* register usb driver */
+	err = usb_register(&baseband_usb_driver);
+	if (err < 0) {
+		pr_err("%s: cannot register usb driver %d\n", __func__, err);
+		goto error2;
+	}
+
+	pr_debug("baseband_usb_chr_init }\n");
+	return 0;
+
+error2:
+	unregister_chrdev(BASEBAND_USB_CHR_DEV_MAJOR,
+			BASEBAND_USB_CHR_DEV_NAME);
+	destroy_workqueue(chr_ipc_wq);
+	chr_ipc_wq = NULL;
+error:
+	kfree(usb_chr_res.ipc_rx);
+	kfree(usb_chr_res.ipc_tx);
+	kfree(usb_chr_res.rx_buf);
+	return err;
+}
+
+static void baseband_usb_chr_exit(void)
+{
+	pr_debug("baseband_usb_chr_exit {\n");
+
+	/* unregister character device */
+	unregister_chrdev(BASEBAND_USB_CHR_DEV_MAJOR,
+		BASEBAND_USB_CHR_DEV_NAME);
+
+	if (chr_ipc_wq) {
+		destroy_workqueue(chr_ipc_wq);
+		chr_ipc_wq = NULL;
+	}
+
+	/* close usb driver */
+	usb_deregister(&baseband_usb_driver);
+
+	kfree(usb_chr_res.ipc_rx);
+	kfree(usb_chr_res.ipc_tx);
+	kfree(usb_chr_res.rx_buf);
+
+	pr_debug("baseband_usb_chr_exit }\n");
+}
+
+module_init(baseband_usb_chr_init)
+module_exit(baseband_usb_chr_exit)
+
diff --git a/drivers/usb/serial/baseband_usb_chr.h b/drivers/usb/serial/baseband_usb_chr.h
new file mode 100644
index 000000000000..e7553f76093e
--- /dev/null
+++ b/drivers/usb/serial/baseband_usb_chr.h
@@ -0,0 +1,107 @@
+/*
+ * baseband_usb_chr.h
+ *
+ * USB character driver to communicate with baseband modems.
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __BASEBAND_USB_CHR_H__
+#define __BASEBAND_USB_CHR_H__
+
+#define BASEBAND_USB_CHR_DEV_NAME		"baseband_usb_chr"
+#define BASEBAND_USB_CHR_DEV_MAJOR		66
+
+#ifndef USB_CHR_RX_BUFSIZ
+#define USB_CHR_RX_BUFSIZ			(32*1024)
+#endif  /* USB_CHR_RX_BUFSIZ */
+
+#ifndef USB_CHR_TX_BUFSIZ
+#define USB_CHR_TX_BUFSIZ			(32*1024)
+#endif  /* USB_CHR_TX_BUFSIZ */
+
+#ifndef USB_CHR_TIMEOUT
+#define USB_CHR_TIMEOUT				5000 /* ms */
+#endif  /* USB_CHR_TIMEOUT */
+
+#ifndef BASEBAND_IPC_NUM_RX_BUF
+#define BASEBAND_IPC_NUM_RX_BUF			1
+#endif  /* BASEBAND_IPC_NUM_RX_BUF */
+
+#ifndef BASEBAND_IPC_NUM_TX_BUF
+#define BASEBAND_IPC_NUM_TX_BUF			1
+#endif  /* BASEBAND_IPC_NUM_TX_BUF */
+
+#ifndef BASEBAND_IPC_BUFSIZ
+#define BASEBAND_IPC_BUFSIZ			65536
+#endif  /* BASEBAND_IPC_BUFSIZ */
+
+struct baseband_ipc {
+	/* rx / tx data */
+	struct semaphore buf_sem;
+	struct {
+		/* linked list of data buffers */
+		struct list_head buf;
+		/* wait queue of processes trying to access data buffers */
+		wait_queue_head_t wait;
+	} rx, tx, rx_free, tx_free;
+	unsigned char *ipc_rx;
+	unsigned char *ipc_tx;
+	/* work queue
+	 * - queued per ipc transaction
+	 * - initiated by either:
+	 *   = interrupt on gpio line (rx data available)
+	 *   = tx data packet being added to tx linked list
+	 */
+	struct workqueue_struct *workqueue;
+	struct work_struct work;
+	struct work_struct rx_work;
+	struct work_struct tx_work;
+};
+
+struct baseband_ipc_buf {
+	struct list_head list;
+	/* data buffer */
+	unsigned char data[BASEBAND_IPC_BUFSIZ];
+	/* offset of first data byte */
+	size_t offset;
+	/* number of valid data bytes */
+	size_t count;
+};
+
+struct baseband_usb {
+	struct baseband_ipc *ipc;
+	unsigned int ref;
+	struct {
+		struct usb_driver *driver;
+		struct usb_device *device;
+		struct usb_interface *interface;
+		struct {
+			struct {
+				unsigned int in;
+				unsigned int out;
+			} isoch, bulk, interrupt;
+		} pipe;
+		/* currently active rx urb */
+		struct urb *rx_urb;
+		/* currently active tx urb */
+		struct urb *tx_urb;
+	} usb;
+};
+
+#endif  /* __BASEBAND_USB_CHR_H__ */
+
diff --git a/drivers/usb/serial/cp210x.c b/drivers/usb/serial/cp210x.c
index fd67cc53545b..a5152379cb4c 100644
--- a/drivers/usb/serial/cp210x.c
+++ b/drivers/usb/serial/cp210x.c
@@ -39,6 +39,8 @@ static void cp210x_get_termios(struct tty_struct *,
 	struct usb_serial_port *port);
 static void cp210x_get_termios_port(struct usb_serial_port *port,
 	unsigned int *cflagp, unsigned int *baudp);
+static void cp210x_change_speed(struct tty_struct *, struct usb_serial_port *,
+							struct ktermios *);
 static void cp210x_set_termios(struct tty_struct *, struct usb_serial_port *,
 							struct ktermios*);
 static int cp210x_tiocmget(struct tty_struct *);
@@ -92,6 +94,7 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(0x10C4, 0x818B) }, /* AVIT Research USB to TTL */
 	{ USB_DEVICE(0x10C4, 0x819F) }, /* MJS USB Toslink Switcher */
 	{ USB_DEVICE(0x10C4, 0x81A6) }, /* ThinkOptics WavIt */
+	{ USB_DEVICE(0x10C4, 0x81A9) }, /* Multiplex RC Interface */
 	{ USB_DEVICE(0x10C4, 0x81AC) }, /* MSD Dash Hawk */
 	{ USB_DEVICE(0x10C4, 0x81AD) }, /* INSYS USB Modem */
 	{ USB_DEVICE(0x10C4, 0x81C8) }, /* Lipowsky Industrie Elektronik GmbH, Baby-JTAG */
@@ -137,6 +140,7 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(0x1843, 0x0200) }, /* Vaisala USB Instrument Cable */
 	{ USB_DEVICE(0x18EF, 0xE00F) }, /* ELV USB-I2C-Interface */
 	{ USB_DEVICE(0x1BE3, 0x07A6) }, /* WAGO 750-923 USB Service Cable */
+	{ USB_DEVICE(0x3195, 0xF190) }, /* Link Instruments MSO-19 */
 	{ USB_DEVICE(0x413C, 0x9500) }, /* DW700 GPS USB interface */
 	{ } /* Terminating Entry */
 };
@@ -200,6 +204,8 @@ static struct usb_serial_driver cp210x_device = {
 #define CP210X_EMBED_EVENTS	0x15
 #define CP210X_GET_EVENTSTATE	0x16
 #define CP210X_SET_CHARS	0x19
+#define CP210X_GET_BAUDRATE	0x1D
+#define CP210X_SET_BAUDRATE	0x1E
 
 /* CP210X_IFC_ENABLE */
 #define UART_ENABLE		0x0001
@@ -353,8 +359,8 @@ static inline int cp210x_set_config_single(struct usb_serial_port *port,
  * Quantises the baud rate as per AN205 Table 1
  */
 static unsigned int cp210x_quantise_baudrate(unsigned int baud) {
-	if      (baud <= 56)       baud = 0;
-	else if (baud <= 300)      baud = 300;
+	if (baud <= 300)
+		baud = 300;
 	else if (baud <= 600)      baud = 600;
 	else if (baud <= 1200)     baud = 1200;
 	else if (baud <= 1800)     baud = 1800;
@@ -382,17 +388,15 @@ static unsigned int cp210x_quantise_baudrate(unsigned int baud) {
 	else if (baud <= 491520)   baud = 460800;
 	else if (baud <= 567138)   baud = 500000;
 	else if (baud <= 670254)   baud = 576000;
-	else if (baud <= 1053257)  baud = 921600;
-	else if (baud <= 1474560)  baud = 1228800;
-	else if (baud <= 2457600)  baud = 1843200;
-	else                       baud = 3686400;
+	else if (baud < 1000000)
+		baud = 921600;
+	else if (baud > 2000000)
+		baud = 2000000;
 	return baud;
 }
 
 static int cp210x_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
-	int result;
-
 	dbg("%s - port %d", __func__, port->number);
 
 	if (cp210x_set_config_single(port, CP210X_IFC_ENABLE, UART_ENABLE)) {
@@ -401,13 +405,14 @@ static int cp210x_open(struct tty_struct *tty, struct usb_serial_port *port)
 		return -EPROTO;
 	}
 
-	result = usb_serial_generic_open(tty, port);
-	if (result)
-		return result;
-
 	/* Configure the termios structure */
 	cp210x_get_termios(tty, port);
-	return 0;
+
+	/* The baud rate must be initialised on cp2104 */
+	if (tty)
+		cp210x_change_speed(tty, port, NULL);
+
+	return usb_serial_generic_open(tty, port);
 }
 
 static void cp210x_close(struct usb_serial_port *port)
@@ -459,10 +464,7 @@ static void cp210x_get_termios_port(struct usb_serial_port *port,
 
 	dbg("%s - port %d", __func__, port->number);
 
-	cp210x_get_config(port, CP210X_GET_BAUDDIV, &baud, 2);
-	/* Convert to baudrate */
-	if (baud)
-		baud = cp210x_quantise_baudrate((BAUD_RATE_GEN_FREQ + baud/2)/ baud);
+	cp210x_get_config(port, CP210X_GET_BAUDRATE, &baud, 4);
 
 	dbg("%s - baud rate = %d", __func__, baud);
 	*baudp = baud;
@@ -576,11 +578,64 @@ static void cp210x_get_termios_port(struct usb_serial_port *port,
 	*cflagp = cflag;
 }
 
+/*
+ * CP2101 supports the following baud rates:
+ *
+ *	300, 600, 1200, 1800, 2400, 4800, 7200, 9600, 14400, 19200, 28800,
+ *	38400, 56000, 57600, 115200, 128000, 230400, 460800, 921600
+ *
+ * CP2102 and CP2103 support the following additional rates:
+ *
+ *	4000, 16000, 51200, 64000, 76800, 153600, 250000, 256000, 500000,
+ *	576000
+ *
+ * The device will map a requested rate to a supported one, but the result
+ * of requests for rates greater than 1053257 is undefined (see AN205).
+ *
+ * CP2104, CP2105 and CP2110 support most rates up to 2M, 921k and 1M baud,
+ * respectively, with an error less than 1%. The actual rates are determined
+ * by
+ *
+ *	div = round(freq / (2 x prescale x request))
+ *	actual = freq / (2 x prescale x div)
+ *
+ * For CP2104 and CP2105 freq is 48Mhz and prescale is 4 for request <= 365bps
+ * or 1 otherwise.
+ * For CP2110 freq is 24Mhz and prescale is 4 for request <= 300bps or 1
+ * otherwise.
+ */
+static void cp210x_change_speed(struct tty_struct *tty,
+		struct usb_serial_port *port, struct ktermios *old_termios)
+{
+	u32 baud;
+
+	baud = tty->termios->c_ospeed;
+
+	/* This maps the requested rate to a rate valid on cp2102 or cp2103,
+	 * or to an arbitrary rate in [1M,2M].
+	 *
+	 * NOTE: B0 is not implemented.
+	 */
+	baud = cp210x_quantise_baudrate(baud);
+
+	dbg("%s - setting baud rate to %u", __func__, baud);
+	if (cp210x_set_config(port, CP210X_SET_BAUDRATE, &baud,
+							sizeof(baud))) {
+		dev_warn(&port->dev, "failed to set baud rate to %u\n", baud);
+		if (old_termios)
+			baud = old_termios->c_ospeed;
+		else
+			baud = 9600;
+	}
+
+	tty_encode_baud_rate(tty, baud, baud);
+}
+
 static void cp210x_set_termios(struct tty_struct *tty,
 		struct usb_serial_port *port, struct ktermios *old_termios)
 {
 	unsigned int cflag, old_cflag;
-	unsigned int baud = 0, bits;
+	unsigned int bits;
 	unsigned int modem_ctl[4];
 
 	dbg("%s - port %d", __func__, port->number);
@@ -591,20 +646,9 @@ static void cp210x_set_termios(struct tty_struct *tty,
 	tty->termios->c_cflag &= ~CMSPAR;
 	cflag = tty->termios->c_cflag;
 	old_cflag = old_termios->c_cflag;
-	baud = cp210x_quantise_baudrate(tty_get_baud_rate(tty));
-
-	/* If the baud rate is to be updated*/
-	if (baud != tty_termios_baud_rate(old_termios) && baud != 0) {
-		dbg("%s - Setting baud rate to %d baud", __func__,
-				baud);
-		if (cp210x_set_config_single(port, CP210X_SET_BAUDDIV,
-					((BAUD_RATE_GEN_FREQ + baud/2) / baud))) {
-			dbg("Baud rate requested not supported by device");
-			baud = tty_termios_baud_rate(old_termios);
-		}
-	}
-	/* Report back the resulting baud rate */
-	tty_encode_baud_rate(tty, baud, baud);
+
+	if (tty->termios->c_ospeed != old_termios->c_ospeed)
+		cp210x_change_speed(tty, port, old_termios);
 
 	/* If the number of data bits is to be updated */
 	if ((cflag & CSIZE) != (old_cflag & CSIZE)) {
diff --git a/drivers/usb/serial/ftdi_sio.c b/drivers/usb/serial/ftdi_sio.c
index 5fc13e717911..1a1710a91e0e 100644
--- a/drivers/usb/serial/ftdi_sio.c
+++ b/drivers/usb/serial/ftdi_sio.c
@@ -207,6 +207,8 @@ static struct usb_device_id id_table_combined [] = {
 	{ USB_DEVICE(FTDI_VID, FTDI_XF_640_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_XF_642_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_DSS20_PID) },
+	{ USB_DEVICE(FTDI_VID, FTDI_URBAN_0_PID) },
+	{ USB_DEVICE(FTDI_VID, FTDI_URBAN_1_PID) },
 	{ USB_DEVICE(FTDI_NF_RIC_VID, FTDI_NF_RIC_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_VNHCPCUSB_D_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_MTXORB_0_PID) },
@@ -733,6 +735,7 @@ static struct usb_device_id id_table_combined [] = {
 	{ USB_DEVICE(TML_VID, TML_USB_SERIAL_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_ELSTER_UNICOM_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_PROPOX_JTAGCABLEII_PID) },
+	{ USB_DEVICE(FTDI_VID, FTDI_PROPOX_ISPCABLEIII_PID) },
 	{ USB_DEVICE(OLIMEX_VID, OLIMEX_ARM_USB_OCD_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
 	{ USB_DEVICE(OLIMEX_VID, OLIMEX_ARM_USB_OCD_H_PID),
@@ -745,6 +748,8 @@ static struct usb_device_id id_table_combined [] = {
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
 	{ USB_DEVICE(FTDI_VID, LMI_LM3S_EVAL_BOARD_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
+	{ USB_DEVICE(FTDI_VID, LMI_LM3S_ICDI_BOARD_PID),
+		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
 	{ USB_DEVICE(FTDI_VID, FTDI_TURTELIZER_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
 	{ USB_DEVICE(RATOC_VENDOR_ID, RATOC_PRODUCT_ID_USB60F) },
@@ -791,6 +796,7 @@ static struct usb_device_id id_table_combined [] = {
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
 	{ USB_DEVICE(ADI_VID, ADI_GNICEPLUS_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
+	{ USB_DEVICE(HORNBY_VID, HORNBY_ELITE_PID) },
 	{ USB_DEVICE(JETI_VID, JETI_SPC1201_PID) },
 	{ USB_DEVICE(MARVELL_VID, MARVELL_SHEEVAPLUG_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
@@ -799,6 +805,8 @@ static struct usb_device_id id_table_combined [] = {
 	{ USB_DEVICE(BAYER_VID, BAYER_CONTOUR_CABLE_PID) },
 	{ USB_DEVICE(FTDI_VID, MARVELL_OPENRD_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
+	{ USB_DEVICE(FTDI_VID, TI_XDS100V2_PID),
+		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
 	{ USB_DEVICE(FTDI_VID, HAMEG_HO820_PID) },
 	{ USB_DEVICE(FTDI_VID, HAMEG_HO720_PID) },
 	{ USB_DEVICE(FTDI_VID, HAMEG_HO730_PID) },
@@ -835,6 +843,7 @@ static struct usb_device_id id_table_combined [] = {
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
 	{ USB_DEVICE(ST_VID, ST_STMCLT1030_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_stmclite_quirk },
+	{ USB_DEVICE(FTDI_VID, FTDI_RF_R106) },
 	{ },					/* Optional parameter entry */
 	{ }					/* Terminating entry */
 };
@@ -1324,8 +1333,7 @@ static int set_serial_info(struct tty_struct *tty,
 		goto check_and_exit;
 	}
 
-	if ((new_serial.baud_base != priv->baud_base) &&
-	    (new_serial.baud_base < 9600)) {
+	if (new_serial.baud_base != priv->baud_base) {
 		mutex_unlock(&priv->cfg_lock);
 		return -EINVAL;
 	}
@@ -1814,6 +1822,7 @@ static int ftdi_sio_port_remove(struct usb_serial_port *port)
 
 static int ftdi_open(struct tty_struct *tty, struct usb_serial_port *port)
 {
+	struct ktermios dummy;
 	struct usb_device *dev = port->serial->dev;
 	struct ftdi_private *priv = usb_get_serial_port_data(port);
 	int result;
@@ -1832,8 +1841,10 @@ static int ftdi_open(struct tty_struct *tty, struct usb_serial_port *port)
 	   This is same behaviour as serial.c/rs_open() - Kuba */
 
 	/* ftdi_set_termios  will send usb control messages */
-	if (tty)
-		ftdi_set_termios(tty, port, tty->termios);
+	if (tty) {
+		memset(&dummy, 0, sizeof(dummy));
+		ftdi_set_termios(tty, port, &dummy);
+	}
 
 	/* Start reading from the device */
 	result = usb_serial_generic_open(tty, port);
@@ -2079,13 +2090,19 @@ static void ftdi_set_termios(struct tty_struct *tty,
 
 	cflag = termios->c_cflag;
 
-	/* FIXME -For this cut I don't care if the line is really changing or
-	   not  - so just do the change regardless  - should be able to
-	   compare old_termios and tty->termios */
+	if (old_termios->c_cflag == termios->c_cflag
+	    && old_termios->c_ispeed == termios->c_ispeed
+	    && old_termios->c_ospeed == termios->c_ospeed)
+		goto no_c_cflag_changes;
+
 	/* NOTE These routines can get interrupted by
 	   ftdi_sio_read_bulk_callback  - need to examine what this means -
 	   don't see any problems yet */
 
+	if ((old_termios->c_cflag & (CSIZE|PARODD|PARENB|CMSPAR|CSTOPB)) ==
+	    (termios->c_cflag & (CSIZE|PARODD|PARENB|CMSPAR|CSTOPB)))
+		goto no_data_parity_stop_changes;
+
 	/* Set number of data bits, parity, stop bits */
 
 	urb_value = 0;
@@ -2126,6 +2143,7 @@ static void ftdi_set_termios(struct tty_struct *tty,
 	}
 
 	/* Now do the baudrate */
+no_data_parity_stop_changes:
 	if ((cflag & CBAUD) == B0) {
 		/* Disable flow control */
 		if (usb_control_msg(dev, usb_sndctrlpipe(dev, 0),
@@ -2153,6 +2171,7 @@ static void ftdi_set_termios(struct tty_struct *tty,
 
 	/* Set flow control */
 	/* Note device also supports DTR/CD (ugh) and Xon/Xoff in hardware */
+no_c_cflag_changes:
 	if (cflag & CRTSCTS) {
 		dbg("%s Setting to CRTSCTS flow control", __func__);
 		if (usb_control_msg(dev,
diff --git a/drivers/usb/serial/ftdi_sio_ids.h b/drivers/usb/serial/ftdi_sio_ids.h
index bf5227ad3ef7..76d4f31b38c5 100644
--- a/drivers/usb/serial/ftdi_sio_ids.h
+++ b/drivers/usb/serial/ftdi_sio_ids.h
@@ -39,6 +39,13 @@
 /* www.candapter.com Ewert Energy Systems CANdapter device */
 #define FTDI_CANDAPTER_PID 0x9F80 /* Product Id */
 
+/*
+ * Texas Instruments XDS100v2 JTAG / BeagleBone A3
+ * http://processors.wiki.ti.com/index.php/XDS100
+ * http://beagleboard.org/bone
+ */
+#define TI_XDS100V2_PID		0xa6d0
+
 #define FTDI_NXTCAM_PID		0xABB8 /* NXTCam for Mindstorms NXT */
 
 /* US Interface Navigator (http://www.usinterface.com/) */
@@ -54,6 +61,7 @@
 /* FTDI 2332C Dual channel device, side A=245 FIFO (JTAG), Side B=RS232 UART */
 #define LMI_LM3S_DEVEL_BOARD_PID	0xbcd8
 #define LMI_LM3S_EVAL_BOARD_PID		0xbcd9
+#define LMI_LM3S_ICDI_BOARD_PID		0xbcda
 
 #define FTDI_TURTELIZER_PID	0xBDC8 /* JTAG/RS-232 adapter by egnite GmbH */
 
@@ -111,6 +119,7 @@
 
 /* Propox devices */
 #define FTDI_PROPOX_JTAGCABLEII_PID	0xD738
+#define FTDI_PROPOX_ISPCABLEIII_PID	0xD739
 
 /* Lenz LI-USB Computer Interface. */
 #define FTDI_LENZ_LIUSB_PID	0xD780
@@ -420,9 +429,11 @@
 #define PROTEGO_SPECIAL_4	0xFC73	/* special/unknown device */
 
 /*
- * DSS-20 Sync Station for Sony Ericsson P800
+ * Sony Ericsson product ids
  */
-#define FTDI_DSS20_PID          0xFC82
+#define FTDI_DSS20_PID		0xFC82	/* DSS-20 Sync Station for Sony Ericsson P800 */
+#define FTDI_URBAN_0_PID	0xFC8A	/* Sony Ericsson Urban, uart #0 */
+#define FTDI_URBAN_1_PID	0xFC8B	/* Sony Ericsson Urban, uart #1 */
 
 /* www.irtrans.de device */
 #define FTDI_IRTRANS_PID 0xFC60 /* Product Id */
@@ -521,6 +532,12 @@
 #define ADI_GNICEPLUS_PID	0xF001
 
 /*
+ * Hornby Elite
+ */
+#define HORNBY_VID		0x04D8
+#define HORNBY_ELITE_PID	0x000A
+
+/*
  * RATOC REX-USB60F
  */
 #define RATOC_VENDOR_ID		0x0584
@@ -1164,3 +1181,9 @@
  */
 /* TagTracer MIFARE*/
 #define FTDI_ZEITCONTROL_TAGTRACE_MIFARE_PID	0xF7C0
+
+/*
+ * Rainforest Automation
+ */
+/* ZigBee controller */
+#define FTDI_RF_R106		0x8A28
diff --git a/drivers/usb/serial/io_ti.c b/drivers/usb/serial/io_ti.c
index 0aac00afb5c8..8a90d58ee96d 100644
--- a/drivers/usb/serial/io_ti.c
+++ b/drivers/usb/serial/io_ti.c
@@ -2677,15 +2677,7 @@ cleanup:
 
 static void edge_disconnect(struct usb_serial *serial)
 {
-	int i;
-	struct edgeport_port *edge_port;
-
 	dbg("%s", __func__);
-
-	for (i = 0; i < serial->num_ports; ++i) {
-		edge_port = usb_get_serial_port_data(serial->port[i]);
-		edge_remove_sysfs_attrs(edge_port->port);
-	}
 }
 
 static void edge_release(struct usb_serial *serial)
@@ -2764,6 +2756,7 @@ static struct usb_serial_driver edgeport_1port_device = {
 	.disconnect		= edge_disconnect,
 	.release		= edge_release,
 	.port_probe		= edge_create_sysfs_attrs,
+	.port_remove		= edge_remove_sysfs_attrs,
 	.ioctl			= edge_ioctl,
 	.set_termios		= edge_set_termios,
 	.tiocmget		= edge_tiocmget,
@@ -2795,6 +2788,7 @@ static struct usb_serial_driver edgeport_2port_device = {
 	.disconnect		= edge_disconnect,
 	.release		= edge_release,
 	.port_probe		= edge_create_sysfs_attrs,
+	.port_remove		= edge_remove_sysfs_attrs,
 	.ioctl			= edge_ioctl,
 	.set_termios		= edge_set_termios,
 	.tiocmget		= edge_tiocmget,
diff --git a/drivers/usb/serial/omninet.c b/drivers/usb/serial/omninet.c
index 60f38d5e64fc..0a8c1e64b247 100644
--- a/drivers/usb/serial/omninet.c
+++ b/drivers/usb/serial/omninet.c
@@ -315,7 +315,7 @@ static int omninet_write_room(struct tty_struct *tty)
 	int room = 0; /* Default: no room */
 
 	/* FIXME: no consistent locking for write_urb_busy */
-	if (wport->write_urb_busy)
+	if (!wport->write_urb_busy)
 		room = wport->bulk_out_size - OMNINET_HEADERLEN;
 
 	dbg("%s - returns %d", __func__, room);
diff --git a/drivers/usb/serial/option.c b/drivers/usb/serial/option.c
index fe22e90bc879..ea4e5ebdd35a 100644
--- a/drivers/usb/serial/option.c
+++ b/drivers/usb/serial/option.c
@@ -156,6 +156,7 @@ static void option_instat_callback(struct urb *urb);
 #define HUAWEI_PRODUCT_K4511			0x14CC
 #define HUAWEI_PRODUCT_ETS1220			0x1803
 #define HUAWEI_PRODUCT_E353			0x1506
+#define HUAWEI_PRODUCT_E173S			0x1C05
 
 #define QUANTA_VENDOR_ID			0x0408
 #define QUANTA_PRODUCT_Q101			0xEA02
@@ -230,9 +231,11 @@ static void option_instat_callback(struct urb *urb);
 #define NOVATELWIRELESS_PRODUCT_EVDO_EMBEDDED_HIGHSPEED	0x8001
 #define NOVATELWIRELESS_PRODUCT_HSPA_EMBEDDED_FULLSPEED	0x9000
 #define NOVATELWIRELESS_PRODUCT_HSPA_EMBEDDED_HIGHSPEED	0x9001
+#define NOVATELWIRELESS_PRODUCT_E362		0x9010
 #define NOVATELWIRELESS_PRODUCT_G1		0xA001
 #define NOVATELWIRELESS_PRODUCT_G1_M		0xA002
 #define NOVATELWIRELESS_PRODUCT_G2		0xA010
+#define NOVATELWIRELESS_PRODUCT_MC551		0xB001
 
 /* AMOI PRODUCTS */
 #define AMOI_VENDOR_ID				0x1614
@@ -264,6 +267,9 @@ static void option_instat_callback(struct urb *urb);
 #define DELL_PRODUCT_5730_MINICARD_TELUS	0x8181
 #define DELL_PRODUCT_5730_MINICARD_VZW		0x8182
 
+#define DELL_PRODUCT_5800_MINICARD_VZW		0x8195  /* Novatel E362 */
+#define DELL_PRODUCT_5800_V2_MINICARD_VZW	0x8196  /* Novatel E362 */
+
 #define KYOCERA_VENDOR_ID			0x0c88
 #define KYOCERA_PRODUCT_KPC650			0x17da
 #define KYOCERA_PRODUCT_KPC680			0x180a
@@ -316,6 +322,9 @@ static void option_instat_callback(struct urb *urb);
 #define ZTE_PRODUCT_AC8710			0xfff1
 #define ZTE_PRODUCT_AC2726			0xfff5
 #define ZTE_PRODUCT_AC8710T			0xffff
+#define ZTE_PRODUCT_MC2718			0xffe8
+#define ZTE_PRODUCT_AD3812			0xffeb
+#define ZTE_PRODUCT_MC2716			0xffed
 
 #define BENQ_VENDOR_ID				0x04a5
 #define BENQ_PRODUCT_H10			0x4068
@@ -468,6 +477,18 @@ static void option_instat_callback(struct urb *urb);
 #define YUGA_PRODUCT_CLU528			0x260D
 #define YUGA_PRODUCT_CLU526			0x260F
 
+/* Viettel products */
+#define VIETTEL_VENDOR_ID			0x2262
+#define VIETTEL_PRODUCT_VT1000			0x0002
+
+/* ZD Incorporated */
+#define ZD_VENDOR_ID				0x0685
+#define ZD_PRODUCT_7000				0x7000
+
+/* LG products */
+#define LG_VENDOR_ID				0x1004
+#define LG_PRODUCT_L02C				0x618f
+
 /* some devices interfaces need special handling due to a number of reasons */
 enum option_blacklist_reason {
 		OPTION_BLACKLIST_NONE = 0,
@@ -475,31 +496,66 @@ enum option_blacklist_reason {
 		OPTION_BLACKLIST_RESERVED_IF = 2
 };
 
+#define MAX_BL_NUM  8
 struct option_blacklist_info {
-	const u32 infolen;	/* number of interface numbers on blacklist */
-	const u8  *ifaceinfo;	/* pointer to the array holding the numbers */
-	enum option_blacklist_reason reason;
+	/* bitfield of interface numbers for OPTION_BLACKLIST_SENDSETUP */
+	const unsigned long sendsetup;
+	/* bitfield of interface numbers for OPTION_BLACKLIST_RESERVED_IF */
+	const unsigned long reserved;
 };
 
-static const u8 four_g_w14_no_sendsetup[] = { 0, 1 };
 static const struct option_blacklist_info four_g_w14_blacklist = {
-	.infolen = ARRAY_SIZE(four_g_w14_no_sendsetup),
-	.ifaceinfo = four_g_w14_no_sendsetup,
-	.reason = OPTION_BLACKLIST_SENDSETUP
+	.sendsetup = BIT(0) | BIT(1),
 };
 
-static const u8 alcatel_x200_no_sendsetup[] = { 0, 1 };
 static const struct option_blacklist_info alcatel_x200_blacklist = {
-	.infolen = ARRAY_SIZE(alcatel_x200_no_sendsetup),
-	.ifaceinfo = alcatel_x200_no_sendsetup,
-	.reason = OPTION_BLACKLIST_SENDSETUP
+	.sendsetup = BIT(0) | BIT(1),
+};
+
+static const struct option_blacklist_info zte_0037_blacklist = {
+	.sendsetup = BIT(0) | BIT(1),
 };
 
-static const u8 zte_k3765_z_no_sendsetup[] = { 0, 1, 2 };
 static const struct option_blacklist_info zte_k3765_z_blacklist = {
-	.infolen = ARRAY_SIZE(zte_k3765_z_no_sendsetup),
-	.ifaceinfo = zte_k3765_z_no_sendsetup,
-	.reason = OPTION_BLACKLIST_SENDSETUP
+	.sendsetup = BIT(0) | BIT(1) | BIT(2),
+	.reserved = BIT(4),
+};
+
+static const struct option_blacklist_info zte_ad3812_z_blacklist = {
+	.sendsetup = BIT(0) | BIT(1) | BIT(2),
+};
+
+static const struct option_blacklist_info zte_mc2718_z_blacklist = {
+	.sendsetup = BIT(1) | BIT(2) | BIT(3) | BIT(4),
+};
+
+static const struct option_blacklist_info zte_mc2716_z_blacklist = {
+	.sendsetup = BIT(1) | BIT(2) | BIT(3),
+};
+
+static const struct option_blacklist_info huawei_cdc12_blacklist = {
+	.reserved = BIT(1) | BIT(2),
+};
+
+static const struct option_blacklist_info net_intf1_blacklist = {
+	.reserved = BIT(1),
+};
+
+static const struct option_blacklist_info net_intf3_blacklist = {
+	.reserved = BIT(3),
+};
+
+static const struct option_blacklist_info net_intf4_blacklist = {
+	.reserved = BIT(4),
+};
+
+static const struct option_blacklist_info net_intf5_blacklist = {
+	.reserved = BIT(5),
+};
+
+static const struct option_blacklist_info zte_mf626_blacklist = {
+	.sendsetup = BIT(0) | BIT(1),
+	.reserved = BIT(4),
 };
 
 static const struct usb_device_id option_ids[] = {
@@ -599,12 +655,16 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E143D, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E143E, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E143F, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_K4505, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_K3765, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E173S, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_K4505, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t) &huawei_cdc12_blacklist },
+	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_K3765, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t) &huawei_cdc12_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_ETS1220, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E14AC, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_K3806, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_K4605, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_K4605, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t) &huawei_cdc12_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_K3770, 0xff, 0x02, 0x31) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_K3770, 0xff, 0x02, 0x32) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_K3771, 0xff, 0x02, 0x31) },
@@ -614,6 +674,14 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_K4511, 0xff, 0x01, 0x31) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_K4511, 0xff, 0x01, 0x32) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E353, 0xff, 0x01, 0x01) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E353, 0xff, 0x01, 0x02) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E353, 0xff, 0x01, 0x03) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E353, 0xff, 0x01, 0x10) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E353, 0xff, 0x01, 0x12) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E353, 0xff, 0x01, 0x13) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E353, 0xff, 0x02, 0x01) },  /* E398 3G Modem */
+	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E353, 0xff, 0x02, 0x02) },  /* E398 3G PC UI Interface */
+	{ USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E353, 0xff, 0x02, 0x03) },  /* E398 3G Application Interface */
 	{ USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_V640) },
 	{ USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_V620) },
 	{ USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_V740) },
@@ -652,6 +720,9 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_G1) },
 	{ USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_G1_M) },
 	{ USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_G2) },
+	/* Novatel Ovation MC551 a.k.a. Verizon USB551L */
+	{ USB_DEVICE_AND_INTERFACE_INFO(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_MC551, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_E362, 0xff, 0xff, 0xff) },
 
 	{ USB_DEVICE(AMOI_VENDOR_ID, AMOI_PRODUCT_H01) },
 	{ USB_DEVICE(AMOI_VENDOR_ID, AMOI_PRODUCT_H01A) },
@@ -674,6 +745,8 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5730_MINICARD_SPRINT) },	/* Dell Wireless 5730 Mobile Broadband EVDO/HSPA Mini-Card */
 	{ USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5730_MINICARD_TELUS) },	/* Dell Wireless 5730 Mobile Broadband EVDO/HSPA Mini-Card */
 	{ USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5730_MINICARD_VZW) }, 	/* Dell Wireless 5730 Mobile Broadband EVDO/HSPA Mini-Card */
+	{ USB_DEVICE_AND_INTERFACE_INFO(DELL_VENDOR_ID, DELL_PRODUCT_5800_MINICARD_VZW, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(DELL_VENDOR_ID, DELL_PRODUCT_5800_V2_MINICARD_VZW, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE(ANYDATA_VENDOR_ID, ANYDATA_PRODUCT_ADU_E100A) },	/* ADU-E100, ADU-310 */
 	{ USB_DEVICE(ANYDATA_VENDOR_ID, ANYDATA_PRODUCT_ADU_500A) },
 	{ USB_DEVICE(ANYDATA_VENDOR_ID, ANYDATA_PRODUCT_ADU_620UW) },
@@ -700,12 +773,14 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE(KYOCERA_VENDOR_ID, KYOCERA_PRODUCT_KPC680) },
 	{ USB_DEVICE(QUALCOMM_VENDOR_ID, 0x6000)}, /* ZTE AC8700 */
 	{ USB_DEVICE(QUALCOMM_VENDOR_ID, 0x6613)}, /* Onda H600/ZTE MF330 */
+	{ USB_DEVICE(QUALCOMM_VENDOR_ID, 0x9000)}, /* SIMCom SIM5218 */
 	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_6280) }, /* BP3-USB & BP3-EXT HSDPA */
 	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_6008) },
 	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_UC864E) },
 	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_UC864G) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_MF622, 0xff, 0xff, 0xff) }, /* ZTE WCDMA products */
-	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0002, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0002, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t)&net_intf1_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0003, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0004, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0005, 0xff, 0xff, 0xff) },
@@ -720,51 +795,62 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x000f, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0010, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0011, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0012, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0012, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t)&net_intf1_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0013, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0014, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_MF628, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0016, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0017, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0017, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t)&net_intf3_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0018, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0019, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0020, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0021, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0021, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t)&net_intf4_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0022, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0023, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0024, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0025, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0025, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t)&net_intf1_blacklist },
 	/* { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0026, 0xff, 0xff, 0xff) }, */
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0028, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0029, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0030, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_MF626, 0xff,
-	  0xff, 0xff), .driver_info = (kernel_ulong_t)&four_g_w14_blacklist },
+	  0xff, 0xff), .driver_info = (kernel_ulong_t)&zte_mf626_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0032, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0033, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0034, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0037, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0037, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t)&zte_0037_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0038, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0039, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0040, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0042, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0042, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t)&net_intf4_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0043, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0044, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0048, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0049, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0049, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t)&net_intf5_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0050, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0051, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0052, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0052, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t)&net_intf4_blacklist },
 	/* { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0053, 0xff, 0xff, 0xff) }, */
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0054, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0055, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0055, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t)&net_intf1_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0056, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0057, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0058, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0058, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t)&net_intf4_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0059, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0061, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0062, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0063, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0063, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t)&net_intf4_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0064, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0065, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0066, 0xff, 0xff, 0xff) },
@@ -779,11 +865,13 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0083, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0086, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0087, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0104, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0104, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t)&net_intf4_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0105, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0106, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0108, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0113, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0113, 0xff, 0xff, 0xff),
+		.driver_info = (kernel_ulong_t)&net_intf5_blacklist },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0117, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0118, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0121, 0xff, 0xff, 0xff) },
@@ -1003,6 +1091,12 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_AC8710, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_AC2726, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_AC8710T, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_MC2718, 0xff, 0xff, 0xff),
+	 .driver_info = (kernel_ulong_t)&zte_mc2718_z_blacklist },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_AD3812, 0xff, 0xff, 0xff),
+	 .driver_info = (kernel_ulong_t)&zte_ad3812_z_blacklist },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_MC2716, 0xff, 0xff, 0xff),
+	 .driver_info = (kernel_ulong_t)&zte_mc2716_z_blacklist },
 	{ USB_DEVICE(BENQ_VENDOR_ID, BENQ_PRODUCT_H10) },
 	{ USB_DEVICE(DLINK_VENDOR_ID, DLINK_PRODUCT_DWM_652) },
 	{ USB_DEVICE(ALINK_VENDOR_ID, DLINK_PRODUCT_DWM_652_U5) }, /* Yes, ALINK_VENDOR_ID */
@@ -1101,6 +1195,9 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLU516) },
 	{ USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLU528) },
 	{ USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLU526) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(VIETTEL_VENDOR_ID, VIETTEL_PRODUCT_VT1000, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZD_VENDOR_ID, ZD_PRODUCT_7000, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE(LG_VENDOR_ID, LG_PRODUCT_L02C) }, /* docomo L-02C modem */
 	{ } /* Terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, option_ids);
@@ -1214,10 +1311,35 @@ static void __exit option_exit(void)
 module_init(option_init);
 module_exit(option_exit);
 
+static bool is_blacklisted(const u8 ifnum, enum option_blacklist_reason reason,
+			   const struct option_blacklist_info *blacklist)
+{
+	unsigned long num;
+	const unsigned long *intf_list;
+
+	if (blacklist) {
+		if (reason == OPTION_BLACKLIST_SENDSETUP)
+			intf_list = &blacklist->sendsetup;
+		else if (reason == OPTION_BLACKLIST_RESERVED_IF)
+			intf_list = &blacklist->reserved;
+		else {
+			BUG_ON(reason);
+			return false;
+		}
+
+		for_each_set_bit(num, intf_list, MAX_BL_NUM + 1) {
+			if (num == ifnum)
+				return true;
+		}
+	}
+	return false;
+}
+
 static int option_probe(struct usb_serial *serial,
 			const struct usb_device_id *id)
 {
 	struct usb_wwan_intf_private *data;
+
 	/* D-Link DWM 652 still exposes CD-Rom emulation interface in modem mode */
 	if (serial->dev->descriptor.idVendor == DLINK_VENDOR_ID &&
 		serial->dev->descriptor.idProduct == DLINK_PRODUCT_DWM_652 &&
@@ -1230,14 +1352,14 @@ static int option_probe(struct usb_serial *serial,
 		serial->interface->cur_altsetting->desc.bInterfaceClass != 0xff)
 		return -ENODEV;
 
-	/* Don't bind network interfaces on Huawei K3765, K4505 & K4605 */
-	if (serial->dev->descriptor.idVendor == HUAWEI_VENDOR_ID &&
-		(serial->dev->descriptor.idProduct == HUAWEI_PRODUCT_K3765 ||
-			serial->dev->descriptor.idProduct == HUAWEI_PRODUCT_K4505 ||
-			serial->dev->descriptor.idProduct == HUAWEI_PRODUCT_K4605) &&
-		(serial->interface->cur_altsetting->desc.bInterfaceNumber == 1 ||
-			serial->interface->cur_altsetting->desc.bInterfaceNumber == 2))
-		return -ENODEV;
+	/* Don't bind reserved interfaces (like network ones) which often have
+	 * the same class/subclass/protocol as the serial interfaces.  Look at
+	 * the Windows driver .INF files for reserved interface numbers.
+	 */
+	if (is_blacklisted(
+		serial->interface->cur_altsetting->desc.bInterfaceNumber,
+		OPTION_BLACKLIST_RESERVED_IF,
+		(const struct option_blacklist_info *) id->driver_info))
 
 	/* Don't bind network interface on Samsung GT-B3730, it is handled by a separate module */
 	if (serial->dev->descriptor.idVendor == SAMSUNG_VENDOR_ID &&
@@ -1246,7 +1368,6 @@ static int option_probe(struct usb_serial *serial,
 		return -ENODEV;
 
 	data = serial->private = kzalloc(sizeof(struct usb_wwan_intf_private), GFP_KERNEL);
-
 	if (!data)
 		return -ENOMEM;
 	data->send_setup = option_send_setup;
@@ -1255,23 +1376,6 @@ static int option_probe(struct usb_serial *serial,
 	return 0;
 }
 
-static enum option_blacklist_reason is_blacklisted(const u8 ifnum,
-				const struct option_blacklist_info *blacklist)
-{
-	const u8  *info;
-	int i;
-
-	if (blacklist) {
-		info = blacklist->ifaceinfo;
-
-		for (i = 0; i < blacklist->infolen; i++) {
-			if (info[i] == ifnum)
-				return blacklist->reason;
-		}
-	}
-	return OPTION_BLACKLIST_NONE;
-}
-
 static void option_instat_callback(struct urb *urb)
 {
 	int err;
@@ -1343,9 +1447,8 @@ static int option_send_setup(struct usb_serial_port *port)
 	int val = 0;
 	dbg("%s", __func__);
 
-	if (is_blacklisted(ifNum,
-			   (struct option_blacklist_info *) intfdata->private)
-	    == OPTION_BLACKLIST_SENDSETUP) {
+	if (is_blacklisted(ifNum, OPTION_BLACKLIST_SENDSETUP,
+			(struct option_blacklist_info *) intfdata->private)) {
 		dbg("No send_setup on blacklisted interface #%d\n", ifNum);
 		return -EIO;
 	}
diff --git a/drivers/usb/serial/pl2303.c b/drivers/usb/serial/pl2303.c
index 1d33260de014..d44c669cc483 100644
--- a/drivers/usb/serial/pl2303.c
+++ b/drivers/usb/serial/pl2303.c
@@ -91,7 +91,7 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(SONY_VENDOR_ID, SONY_QN3USB_PRODUCT_ID) },
 	{ USB_DEVICE(SANWA_VENDOR_ID, SANWA_PRODUCT_ID) },
 	{ USB_DEVICE(ADLINK_VENDOR_ID, ADLINK_ND6530_PRODUCT_ID) },
-	{ USB_DEVICE(WINCHIPHEAD_VENDOR_ID, WINCHIPHEAD_USBSER_PRODUCT_ID) },
+	{ USB_DEVICE(SMART_VENDOR_ID, SMART_PRODUCT_ID) },
 	{ }					/* Terminating entry */
 };
 
diff --git a/drivers/usb/serial/pl2303.h b/drivers/usb/serial/pl2303.h
index ca0d237683b3..c38b8c00c06f 100644
--- a/drivers/usb/serial/pl2303.h
+++ b/drivers/usb/serial/pl2303.h
@@ -145,6 +145,7 @@
 #define ADLINK_VENDOR_ID		0x0b63
 #define ADLINK_ND6530_PRODUCT_ID	0x6530
 
-/* WinChipHead USB->RS 232 adapter */
-#define WINCHIPHEAD_VENDOR_ID		0x4348
-#define WINCHIPHEAD_USBSER_PRODUCT_ID	0x5523
+/* SMART USB Serial Adapter */
+#define SMART_VENDOR_ID	0x0b8c
+#define SMART_PRODUCT_ID	0x2303
+
diff --git a/drivers/usb/serial/qcaux.c b/drivers/usb/serial/qcaux.c
index 30b73e68a904..a34819884c1a 100644
--- a/drivers/usb/serial/qcaux.c
+++ b/drivers/usb/serial/qcaux.c
@@ -36,6 +36,7 @@
 #define UTSTARCOM_PRODUCT_UM175_V1		0x3712
 #define UTSTARCOM_PRODUCT_UM175_V2		0x3714
 #define UTSTARCOM_PRODUCT_UM175_ALLTEL		0x3715
+#define PANTECH_PRODUCT_UML190_VZW		0x3716
 #define PANTECH_PRODUCT_UML290_VZW		0x3718
 
 /* CMOTECH devices */
@@ -67,7 +68,11 @@ static struct usb_device_id id_table[] = {
 	{ USB_DEVICE_AND_INTERFACE_INFO(LG_VENDOR_ID, LG_PRODUCT_VX4400_6000, 0xff, 0xff, 0x00) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(SANYO_VENDOR_ID, SANYO_PRODUCT_KATANA_LX, 0xff, 0xff, 0x00) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(SAMSUNG_VENDOR_ID, SAMSUNG_PRODUCT_U520, 0xff, 0x00, 0x00) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(UTSTARCOM_VENDOR_ID, PANTECH_PRODUCT_UML290_VZW, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(UTSTARCOM_VENDOR_ID, PANTECH_PRODUCT_UML190_VZW, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(UTSTARCOM_VENDOR_ID, PANTECH_PRODUCT_UML190_VZW, 0xff, 0xfe, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(UTSTARCOM_VENDOR_ID, PANTECH_PRODUCT_UML290_VZW, 0xff, 0xfd, 0xff) },  /* NMEA */
+	{ USB_DEVICE_AND_INTERFACE_INFO(UTSTARCOM_VENDOR_ID, PANTECH_PRODUCT_UML290_VZW, 0xff, 0xfe, 0xff) },  /* WMC */
+	{ USB_DEVICE_AND_INTERFACE_INFO(UTSTARCOM_VENDOR_ID, PANTECH_PRODUCT_UML290_VZW, 0xff, 0xff, 0xff) },  /* DIAG */
 	{ },
 };
 MODULE_DEVICE_TABLE(usb, id_table);
diff --git a/drivers/usb/serial/qcserial.c b/drivers/usb/serial/qcserial.c
index aeccc7f0a93c..b9bb24729c99 100644
--- a/drivers/usb/serial/qcserial.c
+++ b/drivers/usb/serial/qcserial.c
@@ -28,6 +28,7 @@ static const struct usb_device_id id_table[] = {
 	{USB_DEVICE(0x05c6, 0x9212)},	/* Acer Gobi Modem Device */
 	{USB_DEVICE(0x03f0, 0x1f1d)},	/* HP un2400 Gobi Modem Device */
 	{USB_DEVICE(0x03f0, 0x201d)},	/* HP un2400 Gobi QDL Device */
+	{USB_DEVICE(0x03f0, 0x371d)},	/* HP un2430 Mobile Broadband Module */
 	{USB_DEVICE(0x04da, 0x250d)},	/* Panasonic Gobi Modem device */
 	{USB_DEVICE(0x04da, 0x250c)},	/* Panasonic Gobi QDL device */
 	{USB_DEVICE(0x413c, 0x8172)},	/* Dell Gobi Modem device */
@@ -84,6 +85,7 @@ static const struct usb_device_id id_table[] = {
 	{USB_DEVICE(0x16d8, 0x8002)},	/* CMDTech Gobi 2000 Modem device (VU922) */
 	{USB_DEVICE(0x05c6, 0x9204)},	/* Gobi 2000 QDL device */
 	{USB_DEVICE(0x05c6, 0x9205)},	/* Gobi 2000 Modem device */
+	{USB_DEVICE(0x1199, 0x9013)},	/* Sierra Wireless Gobi 3000 Modem device (MC8355) */
 	{ }				/* Terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, id_table);
diff --git a/drivers/usb/storage/Kconfig b/drivers/usb/storage/Kconfig
index bedc4b9f2ac4..fe2d803a6347 100644
--- a/drivers/usb/storage/Kconfig
+++ b/drivers/usb/storage/Kconfig
@@ -42,7 +42,7 @@ config USB_STORAGE_REALTEK
 
 config REALTEK_AUTOPM
 	bool "Realtek Card Reader autosuspend support"
-	depends on USB_STORAGE_REALTEK && CONFIG_PM_RUNTIME
+	depends on USB_STORAGE_REALTEK && PM_RUNTIME
 	default y
 
 config USB_STORAGE_DATAFAB
diff --git a/drivers/usb/storage/protocol.c b/drivers/usb/storage/protocol.c
index fc310f75eada..0fded39e3b3e 100644
--- a/drivers/usb/storage/protocol.c
+++ b/drivers/usb/storage/protocol.c
@@ -58,7 +58,9 @@
 
 void usb_stor_pad12_command(struct scsi_cmnd *srb, struct us_data *us)
 {
-	/* Pad the SCSI command with zeros out to 12 bytes
+	/*
+	 * Pad the SCSI command with zeros out to 12 bytes.  If the
+	 * command already is 12 bytes or longer, leave it alone.
 	 *
 	 * NOTE: This only works because a scsi_cmnd struct field contains
 	 * a unsigned char cmnd[16], so we know we have storage available
@@ -66,9 +68,6 @@ void usb_stor_pad12_command(struct scsi_cmnd *srb, struct us_data *us)
 	for (; srb->cmd_len<12; srb->cmd_len++)
 		srb->cmnd[srb->cmd_len] = 0;
 
-	/* set command length to 12 bytes */
-	srb->cmd_len = 12;
-
 	/* send the command to the transport layer */
 	usb_stor_invoke_transport(srb, us);
 }
diff --git a/drivers/usb/storage/realtek_cr.c b/drivers/usb/storage/realtek_cr.c
index 34adc4b42ceb..232167ad4781 100644
--- a/drivers/usb/storage/realtek_cr.c
+++ b/drivers/usb/storage/realtek_cr.c
@@ -320,6 +320,11 @@ static int rts51x_read_mem(struct us_data *us, u16 addr, u8 *data, u16 len)
 {
 	int retval;
 	u8 cmnd[12] = { 0 };
+	u8 *buf;
+
+	buf = kmalloc(len, GFP_NOIO);
+	if (buf == NULL)
+		return USB_STOR_TRANSPORT_ERROR;
 
 	US_DEBUGP("%s, addr = 0x%x, len = %d\n", __func__, addr, len);
 
@@ -331,10 +336,14 @@ static int rts51x_read_mem(struct us_data *us, u16 addr, u8 *data, u16 len)
 	cmnd[5] = (u8) len;
 
 	retval = rts51x_bulk_transport(us, 0, cmnd, 12,
-				       data, len, DMA_FROM_DEVICE, NULL);
-	if (retval != USB_STOR_TRANSPORT_GOOD)
+				       buf, len, DMA_FROM_DEVICE, NULL);
+	if (retval != USB_STOR_TRANSPORT_GOOD) {
+		kfree(buf);
 		return -EIO;
+	}
 
+	memcpy(data, buf, len);
+	kfree(buf);
 	return 0;
 }
 
@@ -342,6 +351,12 @@ static int rts51x_write_mem(struct us_data *us, u16 addr, u8 *data, u16 len)
 {
 	int retval;
 	u8 cmnd[12] = { 0 };
+	u8 *buf;
+
+	buf = kmalloc(len, GFP_NOIO);
+	if (buf == NULL)
+		return USB_STOR_TRANSPORT_ERROR;
+	memcpy(buf, data, len);
 
 	US_DEBUGP("%s, addr = 0x%x, len = %d\n", __func__, addr, len);
 
@@ -353,7 +368,8 @@ static int rts51x_write_mem(struct us_data *us, u16 addr, u8 *data, u16 len)
 	cmnd[5] = (u8) len;
 
 	retval = rts51x_bulk_transport(us, 0, cmnd, 12,
-				       data, len, DMA_TO_DEVICE, NULL);
+				       buf, len, DMA_TO_DEVICE, NULL);
+	kfree(buf);
 	if (retval != USB_STOR_TRANSPORT_GOOD)
 		return -EIO;
 
@@ -365,6 +381,11 @@ static int rts51x_read_status(struct us_data *us,
 {
 	int retval;
 	u8 cmnd[12] = { 0 };
+	u8 *buf;
+
+	buf = kmalloc(len, GFP_NOIO);
+	if (buf == NULL)
+		return USB_STOR_TRANSPORT_ERROR;
 
 	US_DEBUGP("%s, lun = %d\n", __func__, lun);
 
@@ -372,10 +393,14 @@ static int rts51x_read_status(struct us_data *us,
 	cmnd[1] = 0x09;
 
 	retval = rts51x_bulk_transport(us, lun, cmnd, 12,
-				       status, len, DMA_FROM_DEVICE, actlen);
-	if (retval != USB_STOR_TRANSPORT_GOOD)
+				       buf, len, DMA_FROM_DEVICE, actlen);
+	if (retval != USB_STOR_TRANSPORT_GOOD) {
+		kfree(buf);
 		return -EIO;
+	}
 
+	memcpy(status, buf, len);
+	kfree(buf);
 	return 0;
 }
 
diff --git a/drivers/usb/storage/transport.c b/drivers/usb/storage/transport.c
index e8ae21b2d387..ff32390d61e5 100644
--- a/drivers/usb/storage/transport.c
+++ b/drivers/usb/storage/transport.c
@@ -691,6 +691,9 @@ void usb_stor_invoke_transport(struct scsi_cmnd *srb, struct us_data *us)
 		int temp_result;
 		struct scsi_eh_save ses;
 		int sense_size = US_SENSE_SIZE;
+		struct scsi_sense_hdr sshdr;
+		const u8 *scdd;
+		u8 fm_ili;
 
 		/* device supports and needs bigger sense buffer */
 		if (us->fflags & US_FL_SANE_SENSE)
@@ -774,32 +777,30 @@ Retry_Sense:
 			srb->sense_buffer[7] = (US_SENSE_SIZE - 8);
 		}
 
+		scsi_normalize_sense(srb->sense_buffer, SCSI_SENSE_BUFFERSIZE,
+				     &sshdr);
+
 		US_DEBUGP("-- Result from auto-sense is %d\n", temp_result);
 		US_DEBUGP("-- code: 0x%x, key: 0x%x, ASC: 0x%x, ASCQ: 0x%x\n",
-			  srb->sense_buffer[0],
-			  srb->sense_buffer[2] & 0xf,
-			  srb->sense_buffer[12], 
-			  srb->sense_buffer[13]);
+			  sshdr.response_code, sshdr.sense_key,
+			  sshdr.asc, sshdr.ascq);
 #ifdef CONFIG_USB_STORAGE_DEBUG
-		usb_stor_show_sense(
-			  srb->sense_buffer[2] & 0xf,
-			  srb->sense_buffer[12], 
-			  srb->sense_buffer[13]);
+		usb_stor_show_sense(sshdr.sense_key, sshdr.asc, sshdr.ascq);
 #endif
 
 		/* set the result so the higher layers expect this data */
 		srb->result = SAM_STAT_CHECK_CONDITION;
 
+		scdd = scsi_sense_desc_find(srb->sense_buffer,
+					    SCSI_SENSE_BUFFERSIZE, 4);
+		fm_ili = (scdd ? scdd[3] : srb->sense_buffer[2]) & 0xA0;
+
 		/* We often get empty sense data.  This could indicate that
 		 * everything worked or that there was an unspecified
 		 * problem.  We have to decide which.
 		 */
-		if (	/* Filemark 0, ignore EOM, ILI 0, no sense */
-				(srb->sense_buffer[2] & 0xaf) == 0 &&
-			/* No ASC or ASCQ */
-				srb->sense_buffer[12] == 0 &&
-				srb->sense_buffer[13] == 0) {
-
+		if (sshdr.sense_key == 0 && sshdr.asc == 0 && sshdr.ascq == 0 &&
+		    fm_ili == 0) {
 			/* If things are really okay, then let's show that.
 			 * Zero out the sense buffer so the higher layers
 			 * won't realize we did an unsolicited auto-sense.
@@ -814,7 +815,10 @@ Retry_Sense:
 			 */
 			} else {
 				srb->result = DID_ERROR << 16;
-				srb->sense_buffer[2] = HARDWARE_ERROR;
+				if ((sshdr.response_code & 0x72) == 0x72)
+					srb->sense_buffer[1] = HARDWARE_ERROR;
+				else
+					srb->sense_buffer[2] = HARDWARE_ERROR;
 			}
 		}
 	}
diff --git a/drivers/usb/storage/unusual_devs.h b/drivers/usb/storage/unusual_devs.h
index 3041a974faf3..f5dcc4f214fa 100644
--- a/drivers/usb/storage/unusual_devs.h
+++ b/drivers/usb/storage/unusual_devs.h
@@ -1840,6 +1840,12 @@ UNUSUAL_DEV(  0x12d1, 0x143F, 0x0000, 0x0000,
 		USB_SC_DEVICE, USB_PR_DEVICE, usb_stor_huawei_e220_init,
 		0),
 
+UNUSUAL_DEV(  0x12d1, 0x1446, 0x0000, 0x0000,
+		"HUAWEI MOBILE",
+		"Mass Storage",
+		USB_SC_DEVICE, USB_PR_DEVICE, usb_stor_huawei_e220_init,
+		0),
+
 /* Reported by Vilius Bilinkevicius <vilisas AT xxx DOT lt) */
 UNUSUAL_DEV(  0x132b, 0x000b, 0x0001, 0x0001,
 		"Minolta",
@@ -1854,6 +1860,13 @@ UNUSUAL_DEV(  0x1370, 0x6828, 0x0110, 0x0110,
 		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
 		US_FL_IGNORE_RESIDUE ),
 
+/* Reported by Qinglin Ye <yestyle@gmail.com> */
+UNUSUAL_DEV(  0x13fe, 0x3600, 0x0100, 0x0100,
+		"Kingston",
+		"DT 101 G2",
+		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
+		US_FL_BULK_IGNORE_TAG ),
+
 /* Reported by Francesco Foresti <frafore@tiscali.it> */
 UNUSUAL_DEV(  0x14cd, 0x6600, 0x0201, 0x0201,
 		"Super Top",
diff --git a/drivers/usb/storage/usb.c b/drivers/usb/storage/usb.c
index 0ca095820f3e..9e069efeefee 100644
--- a/drivers/usb/storage/usb.c
+++ b/drivers/usb/storage/usb.c
@@ -831,12 +831,22 @@ static int usb_stor_scan_thread(void * __us)
 
 	dev_dbg(dev, "device found\n");
 
-	set_freezable();
-	/* Wait for the timeout to expire or for a disconnect */
+	set_freezable_with_signal();
+	/*
+	 * Wait for the timeout to expire or for a disconnect
+	 *
+	 * We can't freeze in this thread or we risk causing khubd to
+	 * fail to freeze, but we can't be non-freezable either. Nor can
+	 * khubd freeze while waiting for scanning to complete as it may
+	 * hold the device lock, causing a hang when suspending devices.
+	 * So we request a fake signal when freezing and use
+	 * interruptible sleep to kick us out of our wait early when
+	 * freezing happens.
+	 */
 	if (delay_use > 0) {
 		dev_dbg(dev, "waiting for device to settle "
 				"before scanning\n");
-		wait_event_freezable_timeout(us->delay_wait,
+		wait_event_interruptible_timeout(us->delay_wait,
 				test_bit(US_FLIDX_DONT_SCAN, &us->dflags),
 				delay_use * HZ);
 	}
@@ -1063,6 +1073,7 @@ static struct usb_driver usb_storage_driver = {
 	.id_table =	usb_storage_usb_ids,
 	.supports_autosuspend = 1,
 	.soft_unbind =	1,
+	.no_dynamic_id = 1,
 };
 
 static int __init usb_stor_init(void)
diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 549b960667c8..97e8171020ad 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -23,6 +23,8 @@ source "drivers/gpu/drm/Kconfig"
 
 source "drivers/gpu/stub/Kconfig"
 
+source "drivers/gpu/ion/Kconfig"
+
 config VGASTATE
        tristate
        default n
@@ -2385,6 +2387,7 @@ config FB_PUV3_UNIGFX
 
 source "drivers/video/omap/Kconfig"
 source "drivers/video/omap2/Kconfig"
+source "drivers/video/tegra/Kconfig"
 
 source "drivers/video/backlight/Kconfig"
 source "drivers/video/display/Kconfig"
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 8b83129e209c..6ba964094fd4 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -141,6 +141,7 @@ obj-$(CONFIG_FB_MSM)              += msm/
 obj-$(CONFIG_FB_NUC900)           += nuc900fb.o
 obj-$(CONFIG_FB_JZ4740)		  += jz4740_fb.o
 obj-$(CONFIG_FB_PUV3_UNIGFX)      += fb-puv3.o
+obj-y                             += tegra/
 
 # Platform or fallback drivers go here
 obj-$(CONFIG_FB_UVESA)            += uvesafb.o
diff --git a/drivers/video/backlight/Kconfig b/drivers/video/backlight/Kconfig
index 278aeaa92505..19db75bc1097 100644
--- a/drivers/video/backlight/Kconfig
+++ b/drivers/video/backlight/Kconfig
@@ -238,6 +238,15 @@ config BACKLIGHT_PWM
 	  If you have a LCD backlight adjustable by PWM, say Y to enable
 	  this driver.
 
+config BACKLIGHT_TEGRA_PWM
+	bool "Tegra PMx based PWM Backlight Driver"
+	depends on TEGRA_DC
+	help
+	  Enable support for Tegra2 DC pwm backlight.
+
+	  If you have a pwm backlight adjustable by the DC PM0 or PM1 signal
+	  control on tegra, say Y to enable this driver.
+
 config BACKLIGHT_DA903X
 	tristate "Backlight Driver for DA9030/DA9034 using WLED"
 	depends on PMIC_DA903X
@@ -338,6 +347,7 @@ config BACKLIGHT_PCF50633
 config BACKLIGHT_AAT2870
 	tristate "AnalogicTech AAT2870 Backlight"
 	depends on BACKLIGHT_CLASS_DEVICE && MFD_AAT2870_CORE
+	default n
 	help
 	  If you have a AnalogicTech AAT2870 say Y to enable the
 	  backlight driver.
diff --git a/drivers/video/backlight/Makefile b/drivers/video/backlight/Makefile
index fdd1fc4b2770..400cf43c5972 100644
--- a/drivers/video/backlight/Makefile
+++ b/drivers/video/backlight/Makefile
@@ -26,6 +26,8 @@ obj-$(CONFIG_BACKLIGHT_OMAP1)	+= omap1_bl.o
 obj-$(CONFIG_BACKLIGHT_PROGEAR) += progear_bl.o
 obj-$(CONFIG_BACKLIGHT_CARILLO_RANCH) += cr_bllcd.o
 obj-$(CONFIG_BACKLIGHT_PWM)	+= pwm_bl.o
+CFLAGS_tegra_pwm_bl.o	= -Werror
+obj-$(CONFIG_BACKLIGHT_TEGRA_PWM) += tegra_pwm_bl.o
 obj-$(CONFIG_BACKLIGHT_DA903X)	+= da903x_bl.o
 obj-$(CONFIG_BACKLIGHT_MAX8925)	+= max8925_bl.o
 obj-$(CONFIG_BACKLIGHT_APPLE)	+= apple_bl.o
diff --git a/drivers/video/backlight/aat2870_bl.c b/drivers/video/backlight/aat2870_bl.c
index 331f1ef1dad5..203eaa65717e 100644
--- a/drivers/video/backlight/aat2870_bl.c
+++ b/drivers/video/backlight/aat2870_bl.c
@@ -27,6 +27,7 @@
 #include <linux/delay.h>
 #include <linux/fb.h>
 #include <linux/backlight.h>
+#include <linux/mfd/core.h>
 #include <linux/mfd/aat2870.h>
 
 struct aat2870_bl_driver_data {
@@ -127,7 +128,7 @@ static const struct backlight_ops aat2870_bl_ops = {
 
 static int aat2870_bl_probe(struct platform_device *pdev)
 {
-	struct aat2870_bl_platform_data *pdata = pdev->dev.platform_data;
+	struct aat2870_bl_platform_data *pdata = mfd_get_data(pdev);
 	struct aat2870_bl_driver_data *aat2870_bl;
 	struct backlight_device *bd;
 	struct backlight_properties props;
diff --git a/drivers/video/backlight/tegra_pwm_bl.c b/drivers/video/backlight/tegra_pwm_bl.c
new file mode 100644
index 000000000000..a9a429e5c837
--- /dev/null
+++ b/drivers/video/backlight/tegra_pwm_bl.c
@@ -0,0 +1,187 @@
+/*
+ * linux/drivers/video/backlight/tegra_pwm_bl.c
+ *
+ * Tegra pwm backlight driver
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ * Author: Renuka Apte <rapte@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/fb.h>
+#include <linux/backlight.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/tegra_pwm_bl.h>
+#include <linux/gpio.h>
+#include <mach/dc.h>
+
+struct tegra_pwm_bl_data {
+	struct device *dev;
+	int which_dc;
+	int (*notify)(struct device *, int brightness);
+	struct tegra_dc_pwm_params params;
+	int (*check_fb)(struct device *dev, struct fb_info *info);
+};
+
+static int tegra_pwm_backlight_update_status(struct backlight_device *bl)
+{
+	struct tegra_pwm_bl_data *tbl = dev_get_drvdata(&bl->dev);
+	int brightness = bl->props.brightness;
+	int max = bl->props.max_brightness;
+	struct tegra_dc *dc;
+
+	if (bl->props.power != FB_BLANK_UNBLANK)
+		brightness = 0;
+
+	if (bl->props.fb_blank != FB_BLANK_UNBLANK)
+		brightness = 0;
+
+	if (tbl->notify)
+		brightness = tbl->notify(tbl->dev, brightness);
+
+	if (brightness > max)
+		dev_err(&bl->dev, "Invalid brightness value: %d max: %d\n",
+		brightness, max);
+
+#if defined(CONFIG_ARCH_TEGRA_2x_SOC)
+	/* map API brightness range from (0~255) to hw range (0~128) */
+	tbl->params.duty_cycle = (brightness * 128) / 255;
+#else
+	tbl->params.duty_cycle = brightness & 0xFF;
+#endif
+
+	/* Call tegra display controller function to update backlight */
+	dc = tegra_dc_get_dc(tbl->which_dc);
+	if (dc)
+		tegra_dc_config_pwm(dc, &tbl->params);
+	else
+		dev_err(&bl->dev, "tegra display controller not available\n");
+
+	return 0;
+}
+
+static int tegra_pwm_backlight_get_brightness(struct backlight_device *bl)
+{
+	return bl->props.brightness;
+}
+
+static int tegra_pwm_backlight_check_fb(struct backlight_device *bl,
+					struct fb_info *info)
+{
+	struct tegra_pwm_bl_data *tbl = dev_get_drvdata(&bl->dev);
+	return !tbl->check_fb || tbl->check_fb(tbl->dev, info);
+}
+
+static const struct backlight_ops tegra_pwm_backlight_ops = {
+	.update_status	= tegra_pwm_backlight_update_status,
+	.get_brightness	= tegra_pwm_backlight_get_brightness,
+	.check_fb	= tegra_pwm_backlight_check_fb,
+};
+
+static int tegra_pwm_backlight_probe(struct platform_device *pdev)
+{
+	struct backlight_properties props;
+	struct platform_tegra_pwm_backlight_data *data;
+	struct backlight_device *bl;
+	struct tegra_pwm_bl_data *tbl;
+	int ret;
+
+	data = pdev->dev.platform_data;
+	if (!data) {
+		dev_err(&pdev->dev, "failed to find platform data\n");
+		return -EINVAL;
+	}
+
+	tbl = kzalloc(sizeof(*tbl), GFP_KERNEL);
+	if (!tbl) {
+		dev_err(&pdev->dev, "no memory for state\n");
+		ret = -ENOMEM;
+		goto err_alloc;
+	}
+
+	tbl->dev = &pdev->dev;
+	tbl->which_dc = data->which_dc;
+	tbl->notify = data->notify;
+	tbl->check_fb = data->check_fb;
+	tbl->params.which_pwm = data->which_pwm;
+	tbl->params.gpio_conf_to_sfio = data->gpio_conf_to_sfio;
+	tbl->params.period = data->period;
+	tbl->params.clk_div = data->clk_div;
+	tbl->params.clk_select = data->clk_select;
+
+	/* If backlight pin is sfio, request for it */
+	if (gpio_is_valid(tbl->params.gpio_conf_to_sfio)) {
+		ret = gpio_request(tbl->params.gpio_conf_to_sfio, "disp_bl");
+		if (ret)
+			dev_err(&pdev->dev, "backlight gpio request failed\n");
+	}
+
+	memset(&props, 0, sizeof(struct backlight_properties));
+	props.type = BACKLIGHT_RAW;
+	props.max_brightness = data->max_brightness;
+	bl = backlight_device_register(dev_name(&pdev->dev), &pdev->dev, tbl,
+			&tegra_pwm_backlight_ops, &props);
+	if (IS_ERR(bl)) {
+		dev_err(&pdev->dev, "failed to register backlight\n");
+		ret = PTR_ERR(bl);
+		goto err_bl;
+	}
+
+	bl->props.brightness = data->dft_brightness;
+
+	if (gpio_is_valid(tbl->params.gpio_conf_to_sfio))
+		gpio_free(tbl->params.gpio_conf_to_sfio);
+	backlight_update_status(bl);
+
+	platform_set_drvdata(pdev, bl);
+	return 0;
+
+err_bl:
+	kfree(tbl);
+err_alloc:
+	return ret;
+}
+
+static int tegra_pwm_backlight_remove(struct platform_device *pdev)
+{
+	struct backlight_device *bl = platform_get_drvdata(pdev);
+	struct tegra_pwm_bl_data *tbl = dev_get_drvdata(&bl->dev);
+
+	backlight_device_unregister(bl);
+	kfree(tbl);
+	return 0;
+}
+
+static struct platform_driver tegra_pwm_backlight_driver = {
+	.driver		= {
+		.name	= "tegra-pwm-bl",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= tegra_pwm_backlight_probe,
+	.remove		= tegra_pwm_backlight_remove,
+};
+
+static int __init tegra_pwm_backlight_init(void)
+{
+	return platform_driver_register(&tegra_pwm_backlight_driver);
+}
+late_initcall(tegra_pwm_backlight_init);
+
+static void __exit tegra_pwm_backlight_exit(void)
+{
+	platform_driver_unregister(&tegra_pwm_backlight_driver);
+}
+module_exit(tegra_pwm_backlight_exit);
+
+MODULE_DESCRIPTION("Tegra PWM Backlight Driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:tegra-pwm-backlight");
+
diff --git a/drivers/video/carminefb.c b/drivers/video/carminefb.c
index caaa27d4a46a..cb09aa1fa138 100644
--- a/drivers/video/carminefb.c
+++ b/drivers/video/carminefb.c
@@ -32,11 +32,11 @@
 #define CARMINEFB_DEFAULT_VIDEO_MODE	1
 
 static unsigned int fb_mode = CARMINEFB_DEFAULT_VIDEO_MODE;
-module_param(fb_mode, uint, 444);
+module_param(fb_mode, uint, 0444);
 MODULE_PARM_DESC(fb_mode, "Initial video mode as integer.");
 
 static char *fb_mode_str;
-module_param(fb_mode_str, charp, 444);
+module_param(fb_mode_str, charp, 0444);
 MODULE_PARM_DESC(fb_mode_str, "Initial video mode in characters.");
 
 /*
@@ -46,7 +46,7 @@ MODULE_PARM_DESC(fb_mode_str, "Initial video mode in characters.");
  * 0b010 Display 1
  */
 static int fb_displays = CARMINE_USE_DISPLAY0 | CARMINE_USE_DISPLAY1;
-module_param(fb_displays, int, 444);
+module_param(fb_displays, int, 0444);
 MODULE_PARM_DESC(fb_displays, "Bit mode, which displays are used");
 
 struct carmine_hw {
diff --git a/drivers/video/fbmem.c b/drivers/video/fbmem.c
index 5aac00eb1830..ad936295d8f4 100644
--- a/drivers/video/fbmem.c
+++ b/drivers/video/fbmem.c
@@ -1738,8 +1738,6 @@ void fb_set_suspend(struct fb_info *info, int state)
 {
 	struct fb_event event;
 
-	if (!lock_fb_info(info))
-		return;
 	event.info = info;
 	if (state) {
 		fb_notifier_call_chain(FB_EVENT_SUSPEND, &event);
@@ -1748,7 +1746,6 @@ void fb_set_suspend(struct fb_info *info, int state)
 		info->state = FBINFO_STATE_RUNNING;
 		fb_notifier_call_chain(FB_EVENT_RESUME, &event);
 	}
-	unlock_fb_info(info);
 }
 
 /**
diff --git a/drivers/video/fbmon.c b/drivers/video/fbmon.c
index 4f57485f8c54..62dd32b39aac 100644
--- a/drivers/video/fbmon.c
+++ b/drivers/video/fbmon.c
@@ -546,6 +546,9 @@ static int get_dst_timing(unsigned char *block,
 static void get_detailed_timing(unsigned char *block,
 				struct fb_videomode *mode)
 {
+	int v_size = V_SIZE;
+	int h_size = H_SIZE;
+
 	mode->xres = H_ACTIVE;
 	mode->yres = V_ACTIVE;
 	mode->pixclock = PIXEL_CLOCK;
@@ -574,11 +577,18 @@ static void get_detailed_timing(unsigned char *block,
 	}
 	mode->flag = FB_MODE_IS_DETAILED;
 
+	/* get aspect ratio */
+	if (h_size * 18 > v_size * 31 && h_size * 18 < v_size * 33)
+		mode->flag |= FB_FLAG_RATIO_16_9;
+	if (h_size * 18 > v_size * 23 && h_size * 18 < v_size * 25)
+		mode->flag |= FB_FLAG_RATIO_4_3;
+
 	DPRINTK("      %d MHz ",  PIXEL_CLOCK/1000000);
 	DPRINTK("%d %d %d %d ", H_ACTIVE, H_ACTIVE + H_SYNC_OFFSET,
 	       H_ACTIVE + H_SYNC_OFFSET + H_SYNC_WIDTH, H_ACTIVE + H_BLANKING);
 	DPRINTK("%d %d %d %d ", V_ACTIVE, V_ACTIVE + V_SYNC_OFFSET,
 	       V_ACTIVE + V_SYNC_OFFSET + V_SYNC_WIDTH, V_ACTIVE + V_BLANKING);
+	DPRINTK("%dmm %dmm ", H_SIZE, V_SIZE);
 	DPRINTK("%sHSync %sVSync\n\n", (HSYNC_POSITIVE) ? "+" : "-",
 	       (VSYNC_POSITIVE) ? "+" : "-");
 }
@@ -976,7 +986,7 @@ void fb_edid_to_monspecs(unsigned char *edid, struct fb_monspecs *specs)
 /**
  * fb_edid_add_monspecs() - add monitor video modes from E-EDID data
  * @edid:	128 byte array with an E-EDID block
- * @spacs:	monitor specs to be extended
+ * @specs:	monitor specs to be extended
  */
 void fb_edid_add_monspecs(unsigned char *edid, struct fb_monspecs *specs)
 {
@@ -993,7 +1003,7 @@ void fb_edid_add_monspecs(unsigned char *edid, struct fb_monspecs *specs)
 		return;
 
 	if (edid[0] != 0x2 ||
-	    edid[2] < 4 || edid[2] > 128 - DETAILED_TIMING_DESCRIPTION_SIZE)
+	    edid[2] < 4 || edid[2] > 128)
 		return;
 
 	DPRINTK("  Short Video Descriptors\n");
@@ -1001,14 +1011,23 @@ void fb_edid_add_monspecs(unsigned char *edid, struct fb_monspecs *specs)
 	while (pos < edid[2]) {
 		u8 len = edid[pos] & 0x1f, type = (edid[pos] >> 5) & 7;
 		pr_debug("Data block %u of %u bytes\n", type, len);
-		if (type == 2)
+
+		pos++;
+		if (type == 2) {
 			for (i = pos; i < pos + len; i++) {
-				u8 idx = edid[pos + i] & 0x7f;
+				u8 idx = edid[i] & 0x7f;
 				svd[svd_n++] = idx;
 				pr_debug("N%sative mode #%d\n",
-					 edid[pos + i] & 0x80 ? "" : "on-n", idx);
+					 edid[i] & 0x80 ? "" : "on-n", idx);
 			}
-		pos += len + 1;
+		} else if (type == 3 && len >= 3) {
+			u32 ieee_reg = edid[pos] | (edid[pos + 1] << 8) |
+				(edid[pos + 2] << 16);
+			if (ieee_reg == 0x000c03)
+				specs->misc |= FB_MISC_HDMI;
+		}
+
+		pos += len;
 	}
 
 	block = edid + edid[2];
@@ -1041,10 +1060,8 @@ void fb_edid_add_monspecs(unsigned char *edid, struct fb_monspecs *specs)
 
 	for (i = specs->modedb_len + num; i < specs->modedb_len + num + svd_n; i++) {
 		int idx = svd[i - specs->modedb_len - num];
-		if (!idx || idx > 63) {
+		if (!idx || idx > (CEA_MODEDB_SIZE - 1)) {
 			pr_warning("Reserved SVD code %d\n", idx);
-		} else if (idx > ARRAY_SIZE(cea_modes) || !cea_modes[idx].xres) {
-			pr_warning("Unimplemented SVD code %d\n", idx);
 		} else {
 			memcpy(&m[i], cea_modes + idx, sizeof(m[i]));
 			pr_debug("Adding SVD #%d: %ux%u@%u\n", idx,
diff --git a/drivers/video/fbsysfs.c b/drivers/video/fbsysfs.c
index 04251ce89184..67afa9c2289d 100644
--- a/drivers/video/fbsysfs.c
+++ b/drivers/video/fbsysfs.c
@@ -399,9 +399,12 @@ static ssize_t store_fbstate(struct device *device,
 
 	state = simple_strtoul(buf, &last, 0);
 
+	if (!lock_fb_info(fb_info))
+		return -ENODEV;
 	console_lock();
 	fb_set_suspend(fb_info, (int)state);
 	console_unlock();
+	unlock_fb_info(fb_info);
 
 	return count;
 }
diff --git a/drivers/video/logo/Kconfig b/drivers/video/logo/Kconfig
index 39ac49e0682c..7fce1987ddc7 100644
--- a/drivers/video/logo/Kconfig
+++ b/drivers/video/logo/Kconfig
@@ -82,4 +82,8 @@ config LOGO_M32R_CLUT224
 	depends on M32R
 	default y
 
+config LOGO_CUSTOM_CLUT224
+	bool "Custom 224-color Linux logo"
+	default n
+
 endif # LOGO
diff --git a/drivers/video/logo/Makefile b/drivers/video/logo/Makefile
index 3b437813584c..45d4b5346d07 100644
--- a/drivers/video/logo/Makefile
+++ b/drivers/video/logo/Makefile
@@ -18,6 +18,8 @@ obj-$(CONFIG_LOGO_M32R_CLUT224)		+= logo_m32r_clut224.o
 
 obj-$(CONFIG_SPU_BASE)			+= logo_spe_clut224.o
 
+obj-$(CONFIG_LOGO_CUSTOM_CLUT224)	+= logo_custom_clut224.o
+
 # How to generate logo's
 
 # Use logo-cfiles to retrieve list of .c files to be built
diff --git a/drivers/video/logo/logo.c b/drivers/video/logo/logo.c
index ea7a8ccc830c..6e29415c9790 100644
--- a/drivers/video/logo/logo.c
+++ b/drivers/video/logo/logo.c
@@ -100,6 +100,10 @@ const struct linux_logo * __init_refok fb_find_logo(int depth)
 		/* M32R Linux logo */
 		logo = &logo_m32r_clut224;
 #endif
+#ifdef CONFIG_LOGO_CUSTOM_CLUT224
+		/* Custom Linux logo */
+		logo = &logo_custom_clut224;
+#endif
 	}
 	return logo;
 }
diff --git a/drivers/video/modedb.c b/drivers/video/modedb.c
index cb175fe7abc0..34048e2b124a 100644
--- a/drivers/video/modedb.c
+++ b/drivers/video/modedb.c
@@ -292,64 +292,524 @@ static const struct fb_videomode modedb[] = {
 };
 
 #ifdef CONFIG_FB_MODE_HELPERS
-const struct fb_videomode cea_modes[64] = {
-	/* #1: 640x480p@59.94/60Hz */
-	[1] = {
-		NULL, 60, 640, 480, 39722, 48, 16, 33, 10, 96, 2, 0,
-		FB_VMODE_NONINTERLACED, 0,
-	},
-	/* #3: 720x480p@59.94/60Hz */
-	[3] = {
-		NULL, 60, 720, 480, 37037, 60, 16, 30, 9, 62, 6, 0,
-		FB_VMODE_NONINTERLACED, 0,
-	},
-	/* #5: 1920x1080i@59.94/60Hz */
-	[5] = {
-		NULL, 60, 1920, 1080, 13763, 148, 88, 15, 2, 44, 5,
-		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_INTERLACED, 0,
-	},
-	/* #7: 720(1440)x480iH@59.94/60Hz */
-	[7] = {
-		NULL, 60, 1440, 480, 18554/*37108*/, 114, 38, 15, 4, 124, 3, 0,
-		FB_VMODE_INTERLACED, 0,
-	},
-	/* #9: 720(1440)x240pH@59.94/60Hz */
-	[9] = {
-		NULL, 60, 1440, 240, 18554, 114, 38, 16, 4, 124, 3, 0,
-		FB_VMODE_NONINTERLACED, 0,
-	},
-	/* #18: 720x576pH@50Hz */
-	[18] = {
-		NULL, 50, 720, 576, 37037, 68, 12, 39, 5, 64, 5, 0,
-		FB_VMODE_NONINTERLACED, 0,
-	},
-	/* #19: 1280x720p@50Hz */
-	[19] = {
-		NULL, 50, 1280, 720, 13468, 220, 440, 20, 5, 40, 5,
-		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_NONINTERLACED, 0,
-	},
-	/* #20: 1920x1080i@50Hz */
-	[20] = {
-		NULL, 50, 1920, 1080, 13480, 148, 528, 15, 5, 528, 5,
-		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_INTERLACED, 0,
-	},
-	/* #32: 1920x1080p@23.98/24Hz */
-	[32] = {
-		NULL, 24, 1920, 1080, 13468, 148, 638, 36, 4, 44, 5,
-		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_NONINTERLACED, 0,
-	},
-	/* #35: (2880)x480p4x@59.94/60Hz */
-	[35] = {
-		NULL, 60, 2880, 480, 9250, 240, 64, 30, 9, 248, 6, 0,
-		FB_VMODE_NONINTERLACED, 0,
-	},
+const struct fb_videomode cea_modes[CEA_MODEDB_SIZE] = {
+	{},
+	/* 1: 640x480p @ 59.94Hz/60Hz */
+	{.refresh = 59, .xres = 640, .yres = 480, .pixclock = 39721,
+	 .left_margin = 48, .right_margin = 16,
+	 .upper_margin = 33, .lower_margin = 1,
+	 .hsync_len = 96, .vsync_len = 2,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 2: 720x480p @ 59.94Hz/60Hz */
+	{.refresh = 59, .xres = 720, .yres = 480, .pixclock = 37037,
+	 .left_margin = 60, .right_margin = 16,
+	 .upper_margin = 30, .lower_margin = 9,
+	 .hsync_len = 62, .vsync_len = 6,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 3: 720x480p @ 59.94Hz/60Hz */
+	{.refresh = 59, .xres = 720, .yres = 480, .pixclock = 37037,
+	 .left_margin = 60, .right_margin = 16,
+	 .upper_margin = 30, .lower_margin = 9,
+	 .hsync_len = 62, .vsync_len = 6,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 4: 1280x720p @ 59.94Hz/60Hz */
+	{.refresh = 60, .xres = 1280, .yres = 720, .pixclock = 13468,
+	 .left_margin = 220, .right_margin = 110,
+	 .upper_margin = 20, .lower_margin = 5,
+	 .hsync_len = 40, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 5: 1920x1080i @ 59.94Hz/60Hz */
+	{.refresh = 60, .xres = 1920, .yres = 1080, .pixclock = 13468,
+	 .left_margin = 148, .right_margin = 88,
+	 .upper_margin = 15, .lower_margin = 2,
+	 .hsync_len = 44, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 6: 720(1440)x480i @ 59.94Hz/60Hz */
+	{.refresh = 59, .xres = 1440, .yres = 480, .pixclock = 37037,
+	 .left_margin = 114, .right_margin = 38,
+	 .upper_margin = 15, .lower_margin = 4,
+	 .hsync_len = 124, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 7: 720(1440)x480i @ 59.94Hz/60Hz */
+	{.refresh = 59, .xres = 1440, .yres = 480, .pixclock = 37037,
+	 .left_margin = 114, .right_margin = 38,
+	 .upper_margin = 15, .lower_margin = 4,
+	 .hsync_len = 124, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 8: 720(1440)x240p @ 59.94Hz/60Hz */
+	{.refresh = 59, .xres = 1440, .yres = 240, .pixclock = 37037,
+	 .left_margin = 114, .right_margin = 38,
+	 .upper_margin = 15, .lower_margin = 5,
+	 .hsync_len = 124, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 9: 720(1440)x240p @ 59.94Hz/60Hz */
+	{.refresh = 59, .xres = 1440, .yres = 240, .pixclock = 37037,
+	 .left_margin = 114, .right_margin = 38,
+	 .upper_margin = 15, .lower_margin = 5,
+	 .hsync_len = 124, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 10: 2880x480i @ 59.94Hz/60Hz */
+	{.refresh = 59, .xres = 2880, .yres = 480, .pixclock = 18518,
+	 .left_margin = 228, .right_margin = 76,
+	 .upper_margin = 15, .lower_margin = 4,
+	 .hsync_len = 248, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 11: 2880x480i @ 59.94Hz/60Hz */
+	{.refresh = 59, .xres = 2880, .yres = 480, .pixclock = 18518,
+	 .left_margin = 228, .right_margin = 76,
+	 .upper_margin = 15, .lower_margin = 4,
+	 .hsync_len = 248, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 12: 2880x240p @ 59.94Hz/60Hz */
+	{.refresh = 59, .xres = 2880, .yres = 240, .pixclock = 18518,
+	 .left_margin = 228, .right_margin = 76,
+	 .upper_margin = 15, .lower_margin = 5,
+	 .hsync_len = 248, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 13: 2880x240p @ 59.94Hz/60Hz */
+	{.refresh = 59, .xres = 2880, .yres = 240, .pixclock = 18518,
+	 .left_margin = 228, .right_margin = 76,
+	 .upper_margin = 15, .lower_margin = 5,
+	 .hsync_len = 248, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 14: 1440x480p @ 59.94Hz/60Hz */
+	{.refresh = 59, .xres = 1440, .yres = 480, .pixclock = 18518,
+	 .left_margin = 120, .right_margin = 32,
+	 .upper_margin = 30, .lower_margin = 9,
+	 .hsync_len = 124, .vsync_len = 6,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 15: 1440x480p @ 59.94Hz/60Hz */
+	{.refresh = 59, .xres = 1440, .yres = 480, .pixclock = 18518,
+	 .left_margin = 120, .right_margin = 32,
+	 .upper_margin = 30, .lower_margin = 9,
+	 .hsync_len = 124, .vsync_len = 6,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 16: 1920x1080p @ 59.94Hz/60Hz */
+	{.refresh = 60, .xres = 1920, .yres = 1080, .pixclock = 6734,
+	 .left_margin = 148, .right_margin = 88,
+	 .upper_margin = 36, .lower_margin = 4,
+	 .hsync_len = 44, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 17: 720x576p @ 50Hz */
+	{.refresh = 50, .xres = 720, .yres = 576, .pixclock = 37037,
+	 .left_margin = 68, .right_margin = 12,
+	 .upper_margin = 39, .lower_margin = 5,
+	 .hsync_len = 64, .vsync_len = 5,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 18: 720x576p @ 50Hz */
+	{.refresh = 50, .xres = 720, .yres = 576, .pixclock = 37037,
+	 .left_margin = 68, .right_margin = 12,
+	 .upper_margin = 39, .lower_margin = 5,
+	 .hsync_len = 64, .vsync_len = 5,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 19: 1280x720p @ 50Hz */
+	{.refresh = 50, .xres = 1280, .yres = 720, .pixclock = 13468,
+	 .left_margin = 220, .right_margin = 440,
+	 .upper_margin = 20, .lower_margin = 5,
+	 .hsync_len = 40, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 20: 1920x1080i @ 50Hz */
+	{.refresh = 50, .xres = 1920, .yres = 1080, .pixclock = 13468,
+	 .left_margin = 148, .right_margin = 528,
+	 .upper_margin = 15, .lower_margin = 2,
+	 .hsync_len = 44, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 21: 720(1440)x576i @ 50Hz */
+	{.refresh = 50, .xres = 1440, .yres = 576, .pixclock = 37037,
+	 .left_margin = 138, .right_margin = 24,
+	 .upper_margin = 19, .lower_margin = 2,
+	 .hsync_len = 126, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 22: 720(1440)x576i @ 50Hz */
+	{.refresh = 50, .xres = 1440, .yres = 576, .pixclock = 37037,
+	 .left_margin = 138, .right_margin = 24,
+	 .upper_margin = 19, .lower_margin = 2,
+	 .hsync_len = 126, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 23: 720(1440)x288p @ 50Hz */
+	{.refresh = 49, .xres = 1440, .yres = 288, .pixclock = 37037,
+	 .left_margin = 138, .right_margin = 24,
+	 .upper_margin = 19, .lower_margin = 4,
+	 .hsync_len = 126, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 24: 720(1440)x288p @ 50Hz */
+	{.refresh = 49, .xres = 1440, .yres = 288, .pixclock = 37037,
+	 .left_margin = 138, .right_margin = 24,
+	 .upper_margin = 19, .lower_margin = 4,
+	 .hsync_len = 126, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 25: 2880x576i @ 50Hz */
+	{.refresh = 50, .xres = 2880, .yres = 576, .pixclock = 18518,
+	 .left_margin = 276, .right_margin = 48,
+	 .upper_margin = 19, .lower_margin = 2,
+	 .hsync_len = 252, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 26: 2880x576i @ 50Hz */
+	{.refresh = 50, .xres = 2880, .yres = 576, .pixclock = 18518,
+	 .left_margin = 276, .right_margin = 48,
+	 .upper_margin = 19, .lower_margin = 2,
+	 .hsync_len = 252, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 27: 2880x288p @ 50Hz */
+	{.refresh = 49, .xres = 2880, .yres = 288, .pixclock = 18518,
+	 .left_margin = 276, .right_margin = 48,
+	 .upper_margin = 19, .lower_margin = 4,
+	 .hsync_len = 252, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 28: 2880x288p @ 50Hz */
+	{.refresh = 49, .xres = 2880, .yres = 288, .pixclock = 18518,
+	 .left_margin = 276, .right_margin = 48,
+	 .upper_margin = 19, .lower_margin = 4,
+	 .hsync_len = 252, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 29: 1440x576p @ 50Hz */
+	{.refresh = 50, .xres = 1440, .yres = 576, .pixclock = 18518,
+	 .left_margin = 136, .right_margin = 24,
+	 .upper_margin = 39, .lower_margin = 5,
+	 .hsync_len = 128, .vsync_len = 5,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 30: 1440x576p @ 50Hz */
+	{.refresh = 50, .xres = 1440, .yres = 576, .pixclock = 18518,
+	 .left_margin = 136, .right_margin = 24,
+	 .upper_margin = 39, .lower_margin = 5,
+	 .hsync_len = 128, .vsync_len = 5,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 31: 1920x1080p @ 50Hz */
+	{.refresh = 50, .xres = 1920, .yres = 1080, .pixclock = 6734,
+	 .left_margin = 148, .right_margin = 528,
+	 .upper_margin = 36, .lower_margin = 4,
+	 .hsync_len = 44, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 32: 1920x1080p @ 23.97Hz/24Hz */
+	{.refresh = 24, .xres = 1920, .yres = 1080, .pixclock = 13468,
+	 .left_margin = 148, .right_margin = 638,
+	 .upper_margin = 36, .lower_margin = 4,
+	 .hsync_len = 44, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 33: 1920x1080p @ 25Hz */
+	{.refresh = 25, .xres = 1920, .yres = 1080, .pixclock = 13468,
+	 .left_margin = 148, .right_margin = 528,
+	 .upper_margin = 36, .lower_margin = 4,
+	 .hsync_len = 44, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 34: 1920x1080p @ 29.97Hz/30Hz */
+	{.refresh = 30, .xres = 1920, .yres = 1080, .pixclock = 13468,
+	 .left_margin = 148, .right_margin = 88,
+	 .upper_margin = 36, .lower_margin = 4,
+	 .hsync_len = 44, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 35: 2880x480p @ 59.94Hz/60Hz */
+	{.refresh = 59, .xres = 2880, .yres = 480, .pixclock = 9259,
+	 .left_margin = 240, .right_margin = 64,
+	 .upper_margin = 30, .lower_margin = 9,
+	 .hsync_len = 248, .vsync_len = 6,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 36: 2880x480p @ 59.94Hz/60Hz */
+	{.refresh = 59, .xres = 2880, .yres = 480, .pixclock = 9259,
+	 .left_margin = 240, .right_margin = 64,
+	 .upper_margin = 30, .lower_margin = 9,
+	 .hsync_len = 248, .vsync_len = 6,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 37: 2880x576p @ 50Hz */
+	{.refresh = 50, .xres = 2880, .yres = 576, .pixclock = 9259,
+	 .left_margin = 272, .right_margin = 48,
+	 .upper_margin = 39, .lower_margin = 5,
+	 .hsync_len = 256, .vsync_len = 5,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 38: 2880x576p @ 50Hz */
+	{.refresh = 50, .xres = 2880, .yres = 576, .pixclock = 9259,
+	 .left_margin = 272, .right_margin = 48,
+	 .upper_margin = 39, .lower_margin = 5,
+	 .hsync_len = 256, .vsync_len = 5,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 39: 1920x1080i @ 50Hz */
+	{.refresh = 50, .xres = 1920, .yres = 1080, .pixclock = 13888,
+	 .left_margin = 184, .right_margin = 32,
+	 .upper_margin = 57, .lower_margin = 2,
+	 .hsync_len = 168, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 40: 1920x1080i @ 100Hz */
+	{.refresh = 100, .xres = 1920, .yres = 1080, .pixclock = 6734,
+	 .left_margin = 148, .right_margin = 528,
+	 .upper_margin = 15, .lower_margin = 2,
+	 .hsync_len = 44, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 41: 1280x720p @ 100Hz */
+	{.refresh = 100, .xres = 1280, .yres = 720, .pixclock = 6734,
+	 .left_margin = 220, .right_margin = 440,
+	 .upper_margin = 20, .lower_margin = 5,
+	 .hsync_len = 40, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 42: 720x576p @ 100Hz */
+	{.refresh = 100, .xres = 720, .yres = 576, .pixclock = 18518,
+	 .left_margin = 68, .right_margin = 12,
+	 .upper_margin = 39, .lower_margin = 5,
+	 .hsync_len = 64, .vsync_len = 5,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 43: 720x576p @ 100Hz */
+	{.refresh = 100, .xres = 720, .yres = 576, .pixclock = 18518,
+	 .left_margin = 68, .right_margin = 12,
+	 .upper_margin = 39, .lower_margin = 5,
+	 .hsync_len = 64, .vsync_len = 5,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 44: 720(1440)x576i @ 100Hz */
+	{.refresh = 100, .xres = 1440, .yres = 576, .pixclock = 18518,
+	 .left_margin = 138, .right_margin = 24,
+	 .upper_margin = 19, .lower_margin = 2,
+	 .hsync_len = 126, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 45: 720(1440)x576i @ 100Hz */
+	{.refresh = 100, .xres = 1440, .yres = 576, .pixclock = 18518,
+	 .left_margin = 138, .right_margin = 24,
+	 .upper_margin = 19, .lower_margin = 2,
+	 .hsync_len = 126, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 46: 1920x1080i @ 119.88/120Hz */
+	{.refresh = 120, .xres = 1920, .yres = 1080, .pixclock = 6734,
+	 .left_margin = 148, .right_margin = 88,
+	 .upper_margin = 15, .lower_margin = 2,
+	 .hsync_len = 44, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 47: 1280x720p @ 119.88/120Hz */
+	{.refresh = 120, .xres = 1280, .yres = 720, .pixclock = 6734,
+	 .left_margin = 220, .right_margin = 110,
+	 .upper_margin = 20, .lower_margin = 5,
+	 .hsync_len = 40, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 48: 720x480p @ 119.88/120Hz */
+	{.refresh = 119, .xres = 720, .yres = 480, .pixclock = 18518,
+	 .left_margin = 60, .right_margin = 16,
+	 .upper_margin = 30, .lower_margin = 9,
+	 .hsync_len = 62, .vsync_len = 6,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 49: 720x480p @ 119.88/120Hz */
+	{.refresh = 119, .xres = 720, .yres = 480, .pixclock = 18518,
+	 .left_margin = 60, .right_margin = 16,
+	 .upper_margin = 30, .lower_margin = 9,
+	 .hsync_len = 62, .vsync_len = 6,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 50: 720(1440)x480i @ 119.88/120Hz */
+	{.refresh = 119, .xres = 1440, .yres = 480, .pixclock = 18518,
+	 .left_margin = 114, .right_margin = 38,
+	 .upper_margin = 15, .lower_margin = 4,
+	 .hsync_len = 124, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 51: 720(1440)x480i @ 119.88/120Hz */
+	{.refresh = 119, .xres = 1440, .yres = 480, .pixclock = 18518,
+	 .left_margin = 114, .right_margin = 38,
+	 .upper_margin = 15, .lower_margin = 4,
+	 .hsync_len = 124, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 52: 720x576p @ 200Hz */
+	{.refresh = 200, .xres = 720, .yres = 576, .pixclock = 9259,
+	 .left_margin = 68, .right_margin = 12,
+	 .upper_margin = 39, .lower_margin = 5,
+	 .hsync_len = 64, .vsync_len = 5,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 53: 720x576p @ 200Hz */
+	{.refresh = 200, .xres = 720, .yres = 576, .pixclock = 9259,
+	 .left_margin = 68, .right_margin = 12,
+	 .upper_margin = 39, .lower_margin = 5,
+	 .hsync_len = 64, .vsync_len = 5,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 54: 720(1440)x576i @ 200Hz */
+	{.refresh = 200, .xres = 1440, .yres = 576, .pixclock = 9259,
+	 .left_margin = 138, .right_margin = 24,
+	 .upper_margin = 19, .lower_margin = 2,
+	 .hsync_len = 126, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 55: 720(1440)x576i @ 200Hz */
+	{.refresh = 200, .xres = 1440, .yres = 576, .pixclock = 9259,
+	 .left_margin = 138, .right_margin = 24,
+	 .upper_margin = 19, .lower_margin = 2,
+	 .hsync_len = 126, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 56: 720x480p @ 239.76/240Hz */
+	{.refresh = 239, .xres = 720, .yres = 480, .pixclock = 9259,
+	 .left_margin = 60, .right_margin = 16,
+	 .upper_margin = 30, .lower_margin = 9,
+	 .hsync_len = 62, .vsync_len = 6,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 57: 720x480p @ 239.76/240Hz */
+	{.refresh = 239, .xres = 720, .yres = 480, .pixclock = 9259,
+	 .left_margin = 60, .right_margin = 16,
+	 .upper_margin = 30, .lower_margin = 9,
+	 .hsync_len = 62, .vsync_len = 6,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 58: 720(1440)x480i @ 239.76/240Hz */
+	{.refresh = 239, .xres = 1440, .yres = 480, .pixclock = 9259,
+	 .left_margin = 114, .right_margin = 38,
+	 .upper_margin = 15, .lower_margin = 4,
+	 .hsync_len = 124, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_4_3 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 59: 720(1440)x480i @ 239.76/240Hz */
+	{.refresh = 239, .xres = 1440, .yres = 480, .pixclock = 9259,
+	 .left_margin = 114, .right_margin = 38,
+	 .upper_margin = 15, .lower_margin = 4,
+	 .hsync_len = 124, .vsync_len = 3,
+	 .sync = 0,
+	 .flag = FB_FLAG_RATIO_16_9 | FB_FLAG_PIXEL_REPEAT,
+	 .vmode = FB_VMODE_INTERLACED},
+	/* 60: 1280x720p @ 23.97Hz/24Hz */
+	{.refresh = 24, .xres = 1280, .yres = 720, .pixclock = 16835,
+	 .left_margin = 220, .right_margin = 1760,
+	 .upper_margin = 20, .lower_margin = 5,
+	 .hsync_len = 40, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 61: 1280x720p @ 25Hz */
+	{.refresh = 25, .xres = 1280, .yres = 720, .pixclock = 13468,
+	 .left_margin = 220, .right_margin = 2420,
+	 .upper_margin = 20, .lower_margin = 5,
+	 .hsync_len = 40, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 62: 1280x720p @ 29.97Hz/30Hz */
+	{.refresh = 30, .xres = 1280, .yres = 720, .pixclock = 13468,
+	 .left_margin = 220, .right_margin = 1760,
+	 .upper_margin = 20, .lower_margin = 5,
+	 .hsync_len = 40, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 63: 1920x1080p @ 119.88/120Hz */
+	{.refresh = 120, .xres = 1920, .yres = 1080, .pixclock = 3367,
+	 .left_margin = 148, .right_margin = 88,
+	 .upper_margin = 36, .lower_margin = 4,
+	 .hsync_len = 44, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
+	/* 64: 1920x1080p @ 100Hz */
+	{.refresh = 100, .xres = 1920, .yres = 1080, .pixclock = 3367,
+	 .left_margin = 148, .right_margin = 528,
+	 .upper_margin = 36, .lower_margin = 4,
+	 .hsync_len = 44, .vsync_len = 5,
+	 .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	 .flag = FB_FLAG_RATIO_16_9,
+	 .vmode = FB_VMODE_NONINTERLACED},
 };
+EXPORT_SYMBOL(cea_modes);
 
-const struct fb_videomode vesa_modes[] = {
+const struct fb_videomode vesa_modes[VESA_MODEDB_SIZE] = {
 	/* 0 640x350-85 VESA */
 	{ NULL, 85, 640, 350, 31746,  96, 32, 60, 32, 64, 3,
 	  FB_SYNC_HOR_HIGH_ACT, FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA},
@@ -781,7 +1241,7 @@ void fb_var_to_videomode(struct fb_videomode *mode,
 	mode->upper_margin = var->upper_margin;
 	mode->lower_margin = var->lower_margin;
 	mode->sync = var->sync;
-	mode->vmode = var->vmode & FB_VMODE_MASK;
+	mode->vmode = var->vmode & (FB_VMODE_MASK | FB_VMODE_STEREO_MASK);
 	mode->flag = FB_MODE_IS_FROM_VAR;
 	mode->refresh = 0;
 
@@ -826,7 +1286,7 @@ void fb_videomode_to_var(struct fb_var_screeninfo *var,
 	var->hsync_len = mode->hsync_len;
 	var->vsync_len = mode->vsync_len;
 	var->sync = mode->sync;
-	var->vmode = mode->vmode & FB_VMODE_MASK;
+	var->vmode = mode->vmode & (FB_VMODE_MASK | FB_VMODE_STEREO_MASK);
 }
 
 /**
@@ -995,7 +1455,7 @@ int fb_add_videomode(const struct fb_videomode *mode, struct list_head *head)
 		if (!modelist)
 			return -ENOMEM;
 		modelist->mode = *mode;
-		list_add(&modelist->list, head);
+		list_add_tail(&modelist->list, head);
 	}
 	return 0;
 }
diff --git a/drivers/video/mx3fb.c b/drivers/video/mx3fb.c
index 7e3a490e8d76..5e15a92609b0 100644
--- a/drivers/video/mx3fb.c
+++ b/drivers/video/mx3fb.c
@@ -333,8 +333,8 @@ static void sdc_enable_channel(struct mx3fb_info *mx3_fbi)
 
 	/* This enables the channel */
 	if (mx3_fbi->cookie < 0) {
-		mx3_fbi->txd = dma_chan->device->device_prep_slave_sg(dma_chan,
-		      &mx3_fbi->sg[0], 1, DMA_TO_DEVICE, DMA_PREP_INTERRUPT);
+		mx3_fbi->txd = dmaengine_prep_slave_sg(dma_chan,
+		      &mx3_fbi->sg[0], 1, DMA_MEM_TO_DEV, DMA_PREP_INTERRUPT);
 		if (!mx3_fbi->txd) {
 			dev_err(mx3fb->dev, "Cannot allocate descriptor on %d\n",
 				dma_chan->chan_id);
@@ -1103,8 +1103,8 @@ static int mx3fb_pan_display(struct fb_var_screeninfo *var,
 	if (mx3_fbi->txd)
 		async_tx_ack(mx3_fbi->txd);
 
-	txd = dma_chan->device->device_prep_slave_sg(dma_chan, sg +
-		mx3_fbi->cur_ipu_buf, 1, DMA_TO_DEVICE, DMA_PREP_INTERRUPT);
+	txd = dmaengine_prep_slave_sg(dma_chan, sg +
+		mx3_fbi->cur_ipu_buf, 1, DMA_MEM_TO_DEV, DMA_PREP_INTERRUPT);
 	if (!txd) {
 		dev_err(fbi->device,
 			"Error preparing a DMA transaction descriptor.\n");
diff --git a/drivers/video/offb.c b/drivers/video/offb.c
index cb163a5397be..3251a0236d56 100644
--- a/drivers/video/offb.c
+++ b/drivers/video/offb.c
@@ -100,36 +100,32 @@ static int offb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 			  u_int transp, struct fb_info *info)
 {
 	struct offb_par *par = (struct offb_par *) info->par;
-	int i, depth;
-	u32 *pal = info->pseudo_palette;
-
-	depth = info->var.bits_per_pixel;
-	if (depth == 16)
-		depth = (info->var.green.length == 5) ? 15 : 16;
-
-	if (regno > 255 ||
-	    (depth == 16 && regno > 63) ||
-	    (depth == 15 && regno > 31))
-		return 1;
-
-	if (regno < 16) {
-		switch (depth) {
-		case 15:
-			pal[regno] = (regno << 10) | (regno << 5) | regno;
-			break;
-		case 16:
-			pal[regno] = (regno << 11) | (regno << 5) | regno;
-			break;
-		case 24:
-			pal[regno] = (regno << 16) | (regno << 8) | regno;
-			break;
-		case 32:
-			i = (regno << 8) | regno;
-			pal[regno] = (i << 16) | i;
-			break;
+
+	if (info->fix.visual == FB_VISUAL_TRUECOLOR) {
+		u32 *pal = info->pseudo_palette;
+		u32 cr = red >> (16 - info->var.red.length);
+		u32 cg = green >> (16 - info->var.green.length);
+		u32 cb = blue >> (16 - info->var.blue.length);
+		u32 value;
+
+		if (regno >= 16)
+			return -EINVAL;
+
+		value = (cr << info->var.red.offset) |
+			(cg << info->var.green.offset) |
+			(cb << info->var.blue.offset);
+		if (info->var.transp.length > 0) {
+			u32 mask = (1 << info->var.transp.length) - 1;
+			mask <<= info->var.transp.offset;
+			value |= mask;
 		}
+		pal[regno] = value;
+		return 0;
 	}
 
+	if (regno > 255)
+		return -EINVAL;
+
 	red >>= 8;
 	green >>= 8;
 	blue >>= 8;
@@ -381,7 +377,7 @@ static void __init offb_init_fb(const char *name, const char *full_name,
 				int pitch, unsigned long address,
 				int foreign_endian, struct device_node *dp)
 {
-	unsigned long res_size = pitch * height * (depth + 7) / 8;
+	unsigned long res_size = pitch * height;
 	struct offb_par *par = &default_par;
 	unsigned long res_start = address;
 	struct fb_fix_screeninfo *fix;
diff --git a/drivers/video/sh_mobile_hdmi.c b/drivers/video/sh_mobile_hdmi.c
index 7d54e2c612f7..647ba984f00f 100644
--- a/drivers/video/sh_mobile_hdmi.c
+++ b/drivers/video/sh_mobile_hdmi.c
@@ -1111,6 +1111,7 @@ static long sh_hdmi_clk_configure(struct sh_hdmi *hdmi, unsigned long hdmi_rate,
 static void sh_hdmi_edid_work_fn(struct work_struct *work)
 {
 	struct sh_hdmi *hdmi = container_of(work, struct sh_hdmi, edid_work.work);
+	struct fb_info *info;
 	struct sh_mobile_hdmi_info *pdata = hdmi->dev->platform_data;
 	struct sh_mobile_lcdc_chan *ch;
 	int ret;
@@ -1123,8 +1124,9 @@ static void sh_hdmi_edid_work_fn(struct work_struct *work)
 
 	mutex_lock(&hdmi->mutex);
 
+	info = hdmi->info;
+
 	if (hdmi->hp_state == HDMI_HOTPLUG_CONNECTED) {
-		struct fb_info *info = hdmi->info;
 		unsigned long parent_rate = 0, hdmi_rate;
 
 		ret = sh_hdmi_read_edid(hdmi, &hdmi_rate, &parent_rate);
@@ -1148,42 +1150,45 @@ static void sh_hdmi_edid_work_fn(struct work_struct *work)
 
 		ch = info->par;
 
-		console_lock();
+		if (lock_fb_info(info)) {
+			console_lock();
 
-		/* HDMI plug in */
-		if (!sh_hdmi_must_reconfigure(hdmi) &&
-		    info->state == FBINFO_STATE_RUNNING) {
-			/*
-			 * First activation with the default monitor - just turn
-			 * on, if we run a resume here, the logo disappears
-			 */
-			if (lock_fb_info(info)) {
+			/* HDMI plug in */
+			if (!sh_hdmi_must_reconfigure(hdmi) &&
+			    info->state == FBINFO_STATE_RUNNING) {
+				/*
+				 * First activation with the default monitor - just turn
+				 * on, if we run a resume here, the logo disappears
+				 */
 				info->var.width = hdmi->var.width;
 				info->var.height = hdmi->var.height;
 				sh_hdmi_display_on(hdmi, info);
-				unlock_fb_info(info);
+			} else {
+				/* New monitor or have to wake up */
+				fb_set_suspend(info, 0);
 			}
-		} else {
-			/* New monitor or have to wake up */
-			fb_set_suspend(info, 0);
-		}
 
-		console_unlock();
+			console_unlock();
+			unlock_fb_info(info);
+		}
 	} else {
 		ret = 0;
-		if (!hdmi->info)
+		if (!info)
 			goto out;
 
 		hdmi->monspec.modedb_len = 0;
 		fb_destroy_modedb(hdmi->monspec.modedb);
 		hdmi->monspec.modedb = NULL;
 
-		console_lock();
+		if (lock_fb_info(info)) {
+			console_lock();
 
-		/* HDMI disconnect */
-		fb_set_suspend(hdmi->info, 1);
+			/* HDMI disconnect */
+			fb_set_suspend(info, 1);
 
-		console_unlock();
+			console_unlock();
+			unlock_fb_info(info);
+		}
 	}
 
 out:
diff --git a/drivers/video/tegra/Kconfig b/drivers/video/tegra/Kconfig
new file mode 100644
index 000000000000..68cf54264403
--- /dev/null
+++ b/drivers/video/tegra/Kconfig
@@ -0,0 +1,184 @@
+if ARCH_TEGRA
+
+comment "NVIDIA Tegra Display Driver options"
+
+config TEGRA_GRHOST
+	tristate "Tegra graphics host driver"
+	help
+	  Driver for the Tegra graphics host hardware.
+
+config TEGRA_GRHOST_USE_NVMAP
+	bool "Use nvmap as graphics memory manager"
+	default y
+	help
+	  Use nvmap as the graphics memory manager. This is the only
+	  choice at the moment.
+
+config TEGRA_GRHOST_DEFAULT_TIMEOUT
+	depends on TEGRA_GRHOST
+	int "Default timeout for submits"
+	default 0 if TEGRA_SIMULATION_PLATFORM
+	default 30000
+	help
+	  Default timeout for jobs in milliseconds. Set to zero for no timeout.
+
+config TEGRA_DC
+	tristate "Tegra Display Contoller"
+	depends on ARCH_TEGRA && TEGRA_GRHOST
+	select FB_MODE_HELPERS
+	select I2C
+	help
+	  Tegra display controller support.
+
+config FB_TEGRA
+	tristate "Tegra Framebuffer driver"
+	depends on TEGRA_DC && FB = y
+	select FB_CFB_FILLRECT
+	select FB_CFB_COPYAREA
+	select FB_CFB_IMAGEBLIT
+	default FB
+	help
+	  Framebuffer device support for the Tegra display controller.
+
+config TEGRA_DC_EXTENSIONS
+	bool "Tegra Display Controller Extensions"
+	depends on TEGRA_DC
+	default y
+	help
+	  This exposes support for extended capabilities of the Tegra display
+	  controller to userspace drivers.
+
+config TEGRA_NVMAP
+	bool "Tegra GPU memory management driver (nvmap)"
+	select ARM_DMA_USE_IOMMU if IOMMU_API
+	default y
+	help
+	  Say Y here to include the memory management driver for the Tegra
+	  GPU, multimedia and display subsystems
+
+config NVMAP_RECLAIM_UNPINNED_VM
+	bool "Virtualize IOVMM memory in nvmap"
+	depends on TEGRA_NVMAP && (TEGRA_IOVMM || IOMMU_API)
+	default y
+	help
+	  Say Y here to enable nvmap to reclaim I/O virtual memory after
+	  it has been unpinned, and re-use it for other handles. This can
+	  allow a larger virtual I/O VM space than would normally be
+	  supported by the hardware, at a slight cost in performance.
+
+config NVMAP_ALLOW_SYSMEM
+	bool "Allow physical system memory to be used by nvmap"
+	depends on TEGRA_NVMAP
+	default n
+	help
+	  Say Y here to allow nvmap to use physical system memory (i.e.,
+	  shared with the operating system but not translated through
+	  an IOVMM device) for allocations.
+
+config NVMAP_HIGHMEM_ONLY
+	bool "Use only HIGHMEM for nvmap"
+	depends on TEGRA_NVMAP && (NVMAP_ALLOW_SYSMEM || TEGRA_IOVMM || IOMMU_API) && HIGHMEM
+	default n
+	help
+	  Say Y here to restrict nvmap system memory allocations (both
+	  physical system memory and IOVMM) to just HIGHMEM pages.
+
+config NVMAP_CARVEOUT_KILLER
+	bool "Reclaim nvmap carveout by killing processes"
+	depends on TEGRA_NVMAP
+	default n
+	help
+	  Say Y here to allow the system to reclaim carveout space by killing
+	  processes. This will kill the largest consumers of lowest priority
+	  first.
+
+config NVMAP_CARVEOUT_COMPACTOR
+	bool "Compact carveout when it gets fragmented"
+	depends on TEGRA_NVMAP
+	default y
+	help
+	  When carveout allocation attempt fails, compactor defragements
+	  heap and retries the failed allocation.
+	  Say Y here to let nvmap to keep carveout fragmentation under control.
+
+config NVMAP_PAGE_POOLS
+	bool "Use page pools to reduce allocation overhead"
+	depends on TEGRA_NVMAP
+	default y
+	help
+	  say Y here to reduce the alloction overhead, which is significant
+	  for uncached, writecombine and inner cacheable memories as it
+	  involves changing page attributes during every allocation per page
+	  and flushing cache. Alloc time is reduced by allcoating the pages
+	  ahead and keeping them aside. The reserved pages would be released
+	  when system is low on memory and acquired back during release of
+	  memory.
+
+config NVMAP_PAGE_POOL_SIZE
+	hex
+	default 0x0
+
+config NVMAP_CACHE_MAINT_BY_SET_WAYS
+	bool "Enable cache maintenance by set/ways"
+	depends on TEGRA_NVMAP
+	help
+	 Say Y here to reduce cache maintenance overhead by MVA.
+	 This helps in reducing cache maintenance overhead in the systems,
+	 where inner cache includes only L1. For the systems, where inner cache
+	 includes L1 and L2, keep this option disabled.
+
+config NVMAP_OUTER_CACHE_MAINT_BY_SET_WAYS
+	bool "Enable outer cache maintenance by set/ways"
+	depends on TEGRA_NVMAP
+	help
+	  Say Y here if you want to optimize cache maintenance for ranges
+	  bigger than size of outer cache. This option has no effect on
+	  system without outer cache.
+
+config NVMAP_VPR
+	bool "Enable VPR Heap."
+	depends on TEGRA_NVMAP
+	default n
+	help
+	  Say Y here to enable Video Protection Region(VPR) heap.
+	  if unsure, say N.
+
+config NVMAP_FORCE_ZEROED_USER_PAGES
+	bool "Only alloc zeroed pages for user space"
+	depends on TEGRA_NVMAP
+	help
+	  Say Y here to force zeroing of pages allocated for user space. This
+	  avoids leaking kernel secure data to user space. This can add
+	  significant overhead to allocation operations depending on the
+	  allocation size requested.
+
+config TEGRA_DSI
+	bool "Enable DSI panel."
+	default n
+	help
+	  Say Y here to enable the DSI panel.
+
+config NVMAP_CONVERT_CARVEOUT_TO_IOVMM
+	bool "Convert carveout to IOVMM"
+	depends on TEGRA_NVMAP &&  (TEGRA_IOVMM_SMMU || IOMMU_API)
+	default y
+	help
+	  Say Y here to force to convert carveout memory requests to
+	  I/O virtual memory requests.
+
+config TEGRA_NVHDCP
+	bool "Support NVHDCP content protection on HDMI"
+	default n
+	help
+	  Say Y here to support NVHDCP upstream and downstream protocols, this
+	  requires a correctly fused chip to negotiate keys.
+
+config TEGRA_HDMI_74MHZ_LIMIT
+	bool "Support only up to 74.25 MHz HDMI pixel frequency"
+	default n
+	help
+	  Say Y here to make kernel report only low bandwidth modes.
+	  Useful only for boards which can't deliver 148.50 MHz.
+
+endif
+
diff --git a/drivers/video/tegra/Makefile b/drivers/video/tegra/Makefile
new file mode 100644
index 000000000000..ce581d2c81a3
--- /dev/null
+++ b/drivers/video/tegra/Makefile
@@ -0,0 +1,7 @@
+GCOV_PROFILE := y
+subdir-ccflags-y := -Werror
+EXTRA_CFLAGS += -Idrivers/video/tegra/host
+obj-$(CONFIG_TEGRA_GRHOST) += host/
+obj-$(CONFIG_TEGRA_DC) += dc/
+obj-$(CONFIG_FB_TEGRA) += fb.o
+obj-$(CONFIG_TEGRA_NVMAP) += nvmap/
diff --git a/drivers/video/tegra/dc/Makefile b/drivers/video/tegra/dc/Makefile
new file mode 100644
index 000000000000..59104c681bae
--- /dev/null
+++ b/drivers/video/tegra/dc/Makefile
@@ -0,0 +1,12 @@
+GCOV_PROFILE := y
+EXTRA_CFLAGS += -Idrivers/video/tegra/host
+obj-y += dc.o bandwidth.o mode.o clock.o lut.o csc.o window.o
+obj-y += rgb.o
+obj-y += hdmi.o
+obj-$(CONFIG_TEGRA_NVHDCP) += nvhdcp.o
+obj-y += edid.o
+obj-y += nvsd.o
+obj-y += dsi.o
+obj-y += dc_sysfs.o
+obj-y += dc_config.o
+obj-$(CONFIG_TEGRA_DC_EXTENSIONS) += ext/
diff --git a/drivers/video/tegra/dc/bandwidth.c b/drivers/video/tegra/dc/bandwidth.c
new file mode 100644
index 000000000000..0b307f4bc4a2
--- /dev/null
+++ b/drivers/video/tegra/dc/bandwidth.c
@@ -0,0 +1,284 @@
+/*
+ * drivers/video/tegra/dc/bandwidth.c
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/clk.h>
+
+#include <mach/clk.h>
+#include <mach/dc.h>
+#include <mach/fb.h>
+#include <mach/mc.h>
+#include <linux/nvhost.h>
+#include <mach/latency_allowance.h>
+
+#include "dc_reg.h"
+#include "dc_config.h"
+#include "dc_priv.h"
+
+static int use_dynamic_emc = 1;
+
+module_param_named(use_dynamic_emc, use_dynamic_emc, int, S_IRUGO | S_IWUSR);
+
+/* uses the larger of w->bandwidth or w->new_bandwidth */
+static void tegra_dc_set_latency_allowance(struct tegra_dc *dc,
+	struct tegra_dc_win *w)
+{
+	/* windows A, B, C for first and second display */
+	static const enum tegra_la_id la_id_tab[2][3] = {
+		/* first display */
+		{ TEGRA_LA_DISPLAY_0A, TEGRA_LA_DISPLAY_0B,
+			TEGRA_LA_DISPLAY_0C },
+		/* second display */
+		{ TEGRA_LA_DISPLAY_0AB, TEGRA_LA_DISPLAY_0BB,
+			TEGRA_LA_DISPLAY_0CB },
+	};
+	/* window B V-filter tap for first and second display. */
+	static const enum tegra_la_id vfilter_tab[2] = {
+		TEGRA_LA_DISPLAY_1B, TEGRA_LA_DISPLAY_1BB,
+	};
+	unsigned long bw;
+
+	BUG_ON(dc->ndev->id >= ARRAY_SIZE(la_id_tab));
+	BUG_ON(dc->ndev->id >= ARRAY_SIZE(vfilter_tab));
+	BUG_ON(w->idx >= ARRAY_SIZE(*la_id_tab));
+
+	bw = max(w->bandwidth, w->new_bandwidth);
+
+	/* tegra_dc_get_bandwidth() treats V filter windows as double
+	 * bandwidth, but LA has a seperate client for V filter */
+	if (w->idx == 1 && win_use_v_filter(dc, w))
+		bw /= 2;
+
+	/* our bandwidth is in kbytes/sec, but LA takes MBps.
+	 * round up bandwidth to next 1MBps */
+	bw = bw / 1000 + 1;
+
+#ifdef CONFIG_TEGRA_SILICON_PLATFORM
+	tegra_set_latency_allowance(la_id_tab[dc->ndev->id][w->idx], bw);
+	/* if window B, also set the 1B client for the 2-tap V filter. */
+	if (w->idx == 1)
+		tegra_set_latency_allowance(vfilter_tab[dc->ndev->id], bw);
+#endif
+}
+
+static unsigned int tegra_dc_windows_is_overlapped(struct tegra_dc_win *a,
+						   struct tegra_dc_win *b)
+{
+	if (!WIN_IS_ENABLED(a) || !WIN_IS_ENABLED(b))
+		return 0;
+
+	/* because memory access to load the fifo can overlap, only care
+	 * if windows overlap vertically */
+	return ((a->out_y + a->out_h > b->out_y) && (a->out_y <= b->out_y)) ||
+		((b->out_y + b->out_h > a->out_y) && (b->out_y <= a->out_y));
+}
+
+static unsigned long tegra_dc_find_max_bandwidth(struct tegra_dc_win *wins[],
+						 int n)
+{
+	unsigned i;
+	unsigned j;
+	unsigned overlap_count;
+	unsigned max_bw = 0;
+
+	WARN_ONCE(n > 3, "Code assumes at most 3 windows, bandwidth is likely"
+			 "inaccurate.\n");
+
+	/* If we had a large number of windows, we would compute adjacency
+	 * graph representing 2 window overlaps, find all cliques in the graph,
+	 * assign bandwidth to each clique, and then select the clique with
+	 * maximum bandwidth. But because we have at most 3 windows,
+	 * implementing proper Bron-Kerbosh algorithm would be an overkill,
+	 * brute force will suffice.
+	 *
+	 * Thus: find maximum bandwidth for either single or a pair of windows
+	 * and count number of window pair overlaps. If there are three
+	 * pairs, all 3 window overlap.
+	 */
+
+	overlap_count = 0;
+	for (i = 0; i < n; i++) {
+		unsigned int bw1;
+
+		if (wins[i] == NULL)
+			continue;
+		bw1 = wins[i]->new_bandwidth;
+		if (bw1 > max_bw)
+			/* Single window */
+			max_bw = bw1;
+
+		for (j = i + 1; j < n; j++) {
+			if (wins[j] == NULL)
+				continue;
+			if (tegra_dc_windows_is_overlapped(wins[i], wins[j])) {
+				unsigned int bw2 = wins[j]->new_bandwidth;
+				if (bw1 + bw2 > max_bw)
+					/* Window pair overlaps */
+					max_bw = bw1 + bw2;
+				overlap_count++;
+			}
+		}
+	}
+
+	if (overlap_count == 3)
+		/* All three windows overlap */
+		max_bw = wins[0]->new_bandwidth + wins[1]->new_bandwidth +
+			 wins[2]->new_bandwidth;
+
+	return max_bw;
+}
+
+/*
+ * Calculate peak EMC bandwidth for each enabled window =
+ * pixel_clock * win_bpp * (use_v_filter ? 2 : 1)) * H_scale_factor *
+ * (windows_tiling ? 2 : 1)
+ *
+ * note:
+ * (*) We use 2 tap V filter, so need double BW if use V filter
+ * (*) Tiling mode on T30 and DDR3 requires double BW
+ *
+ * return:
+ * bandwidth in kBps
+ */
+static unsigned long tegra_dc_calc_win_bandwidth(struct tegra_dc *dc,
+	struct tegra_dc_win *w)
+{
+	unsigned long ret;
+	int tiled_windows_bw_multiplier;
+	unsigned long bpp;
+
+	if (!WIN_IS_ENABLED(w))
+		return 0;
+
+	if (dfixed_trunc(w->w) == 0 || dfixed_trunc(w->h) == 0 ||
+	    w->out_w == 0 || w->out_h == 0)
+		return 0;
+
+	tiled_windows_bw_multiplier =
+		tegra_mc_get_tiled_memory_bandwidth_multiplier();
+
+	/* all of tegra's YUV formats(420 and 422) fetch 2 bytes per pixel,
+	 * but the size reported by tegra_dc_fmt_bpp for the planar version
+	 * is of the luma plane's size only. */
+	bpp = tegra_dc_is_yuv_planar(w->fmt) ?
+		2 * tegra_dc_fmt_bpp(w->fmt) : tegra_dc_fmt_bpp(w->fmt);
+	ret = dc->mode.pclk / 1000UL * bpp / 8 * (
+		win_use_v_filter(dc, w) ? 2 : 1) *
+		dfixed_trunc(w->w) / w->out_w * (WIN_IS_TILED(w) ?
+		tiled_windows_bw_multiplier : 1);
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	/*
+	 * Assuming 60% efficiency: i.e. if we calculate we need 70MBps, we
+	 * will request 117MBps from EMC.
+	 */
+	ret = ret + (17 * ret / 25);
+#endif
+	return ret;
+}
+
+static unsigned long tegra_dc_get_bandwidth(
+	struct tegra_dc_win *windows[], int n)
+{
+	int i;
+
+	BUG_ON(n > DC_N_WINDOWS);
+
+	/* emc rate and latency allowance both need to know per window
+	 * bandwidths */
+	for (i = 0; i < n; i++) {
+		struct tegra_dc_win *w = windows[i];
+
+		if (w)
+			w->new_bandwidth =
+				tegra_dc_calc_win_bandwidth(w->dc, w);
+	}
+
+	return tegra_dc_find_max_bandwidth(windows, n);
+}
+
+/* to save power, call when display memory clients would be idle */
+void tegra_dc_clear_bandwidth(struct tegra_dc *dc)
+{
+	trace_printk("%s:%s rate=%d\n", dc->ndev->name, __func__,
+		dc->emc_clk_rate);
+	if (tegra_is_clk_enabled(dc->emc_clk))
+		clk_disable(dc->emc_clk);
+	dc->emc_clk_rate = 0;
+}
+
+/* use the larger of dc->emc_clk_rate or dc->new_emc_clk_rate, and copies
+ * dc->new_emc_clk_rate into dc->emc_clk_rate.
+ * calling this function both before and after a flip is sufficient to select
+ * the best possible frequency and latency allowance.
+ * set use_new to true to force dc->new_emc_clk_rate programming.
+ */
+void tegra_dc_program_bandwidth(struct tegra_dc *dc, bool use_new)
+{
+	unsigned i;
+
+	if (use_new || dc->emc_clk_rate != dc->new_emc_clk_rate) {
+		/* going from 0 to non-zero */
+		if (!dc->emc_clk_rate && !tegra_is_clk_enabled(dc->emc_clk))
+			clk_enable(dc->emc_clk);
+
+		clk_set_rate(dc->emc_clk,
+			max(dc->emc_clk_rate, dc->new_emc_clk_rate));
+		dc->emc_clk_rate = dc->new_emc_clk_rate;
+
+		/* going from non-zero to 0 */
+		if (!dc->new_emc_clk_rate && tegra_is_clk_enabled(dc->emc_clk))
+			clk_disable(dc->emc_clk);
+	}
+
+	for (i = 0; i < DC_N_WINDOWS; i++) {
+		struct tegra_dc_win *w = &dc->windows[i];
+
+		if ((use_new || w->bandwidth != w->new_bandwidth) &&
+			w->new_bandwidth != 0)
+			tegra_dc_set_latency_allowance(dc, w);
+		w->bandwidth = w->new_bandwidth;
+		trace_printk("%s:win%u bandwidth=%d\n", dc->ndev->name, w->idx,
+			w->bandwidth);
+	}
+}
+
+int tegra_dc_set_dynamic_emc(struct tegra_dc_win *windows[], int n)
+{
+	unsigned long new_rate;
+	struct tegra_dc *dc;
+
+	if (!use_dynamic_emc)
+		return 0;
+
+	dc = windows[0]->dc;
+
+	/* calculate the new rate based on this POST */
+	new_rate = tegra_dc_get_bandwidth(windows, n);
+	if (WARN_ONCE(new_rate > (ULONG_MAX / 1000), "bandwidth maxed out\n"))
+		new_rate = ULONG_MAX;
+	else
+		new_rate = EMC_BW_TO_FREQ(new_rate * 1000);
+
+	if (tegra_dc_has_multiple_dc())
+		new_rate = ULONG_MAX;
+
+	trace_printk("%s:new_emc_clk_rate=%ld\n", dc->ndev->name, new_rate);
+	dc->new_emc_clk_rate = new_rate;
+
+	return 0;
+}
diff --git a/drivers/video/tegra/dc/clock.c b/drivers/video/tegra/dc/clock.c
new file mode 100644
index 000000000000..c785282693e6
--- /dev/null
+++ b/drivers/video/tegra/dc/clock.c
@@ -0,0 +1,150 @@
+/*
+ * drivers/video/tegra/dc/clock.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/err.h>
+#include <linux/types.h>
+#include <linux/clk.h>
+
+#include <mach/clk.h>
+#include <mach/dc.h>
+
+#include "dc_reg.h"
+#include "dc_priv.h"
+
+unsigned long tegra_dc_pclk_round_rate(struct tegra_dc *dc, int pclk)
+{
+	unsigned long rate;
+	unsigned long div;
+
+	rate = tegra_dc_clk_get_rate(dc);
+
+	div = DIV_ROUND_CLOSEST(rate * 2, pclk);
+
+	if (div < 2)
+		return 0;
+
+	return rate * 2 / div;
+}
+
+static unsigned long tegra_dc_pclk_predict_rate(struct clk *parent, int pclk)
+{
+	unsigned long rate;
+	unsigned long div;
+
+	rate = clk_get_rate(parent);
+
+	div = DIV_ROUND_CLOSEST(rate * 2, pclk);
+
+	if (div < 2)
+		return 0;
+
+	return rate * 2 / div;
+}
+
+void tegra_dc_setup_clk(struct tegra_dc *dc, struct clk *clk)
+{
+	int pclk;
+
+	if (dc->out->type == TEGRA_DC_OUT_RGB) {
+		unsigned long rate;
+		struct clk *parent_clk =
+			clk_get_sys(NULL, dc->out->parent_clk ? : "pll_p");
+
+		if (dc->out->parent_clk_backup &&
+		    (parent_clk == clk_get_sys(NULL, "pll_p"))) {
+			rate = tegra_dc_pclk_predict_rate(
+				parent_clk, dc->mode.pclk);
+			/* use pll_d as last resort */
+			if (rate < (dc->mode.pclk / 100 * 99) ||
+			    rate > (dc->mode.pclk / 100 * 109))
+				parent_clk = clk_get_sys(
+					NULL, dc->out->parent_clk_backup);
+		}
+
+		if (clk_get_parent(clk) != parent_clk)
+			clk_set_parent(clk, parent_clk);
+
+		if (parent_clk != clk_get_sys(NULL, "pll_p")) {
+			struct clk *base_clk = clk_get_parent(parent_clk);
+
+			/* Assuming either pll_d or pll_d2 is used */
+			rate = dc->mode.pclk * 2;
+
+			if (rate != clk_get_rate(base_clk))
+				clk_set_rate(base_clk, rate);
+		}
+	}
+
+	if (dc->out->type == TEGRA_DC_OUT_HDMI) {
+		unsigned long rate;
+		struct clk *parent_clk = clk_get_sys(NULL,
+			dc->out->parent_clk ? : "pll_d_out0");
+		struct clk *base_clk = clk_get_parent(parent_clk);
+
+		/*
+		 * Providing dynamic frequency rate setting for T20/T30 HDMI.
+		 * The required rate needs to be setup at 4x multiplier,
+		 * as out0 is 1/2 of the actual PLL output.
+		 */
+
+		rate = dc->mode.pclk * 4;
+		if (rate != clk_get_rate(base_clk))
+			clk_set_rate(base_clk, rate);
+
+		if (clk_get_parent(clk) != parent_clk)
+			clk_set_parent(clk, parent_clk);
+	}
+
+	if (dc->out->type == TEGRA_DC_OUT_DSI) {
+		unsigned long rate;
+		struct clk *parent_clk;
+		struct clk *base_clk;
+
+		if (clk == dc->clk) {
+			parent_clk = clk_get_sys(NULL,
+					dc->out->parent_clk ? : "pll_d_out0");
+			base_clk = clk_get_parent(parent_clk);
+			tegra_clk_cfg_ex(base_clk,
+					TEGRA_CLK_PLLD_DSI_OUT_ENB, 1);
+		} else {
+			if (dc->pdata->default_out->dsi->dsi_instance) {
+				parent_clk = clk_get_sys(NULL,
+					dc->out->parent_clk ? : "pll_d2_out0");
+				base_clk = clk_get_parent(parent_clk);
+				tegra_clk_cfg_ex(base_clk,
+						TEGRA_CLK_PLLD_CSI_OUT_ENB, 1);
+			} else {
+				parent_clk = clk_get_sys(NULL,
+					dc->out->parent_clk ? : "pll_d_out0");
+				base_clk = clk_get_parent(parent_clk);
+				tegra_clk_cfg_ex(base_clk,
+						TEGRA_CLK_PLLD_DSI_OUT_ENB, 1);
+			}
+		}
+
+		rate = dc->mode.pclk * dc->shift_clk_div * 2;
+		if (rate != clk_get_rate(base_clk))
+			clk_set_rate(base_clk, rate);
+
+		if (clk_get_parent(clk) != parent_clk)
+			clk_set_parent(clk, parent_clk);
+	}
+
+	pclk = tegra_dc_pclk_round_rate(dc, dc->mode.pclk);
+	tegra_dvfs_set_rate(clk, pclk);
+}
diff --git a/drivers/video/tegra/dc/csc.c b/drivers/video/tegra/dc/csc.c
new file mode 100644
index 000000000000..09db5fee4c6f
--- /dev/null
+++ b/drivers/video/tegra/dc/csc.c
@@ -0,0 +1,69 @@
+/*
+ * drivers/video/tegra/dc/csc.c
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/err.h>
+#include <linux/types.h>
+#include <mach/dc.h>
+
+#include "dc_reg.h"
+#include "dc_priv.h"
+
+void tegra_dc_init_csc_defaults(struct tegra_dc_csc *csc)
+{
+	csc->yof   = 0x00f0;
+	csc->kyrgb = 0x012a;
+	csc->kur   = 0x0000;
+	csc->kvr   = 0x0198;
+	csc->kug   = 0x039b;
+	csc->kvg   = 0x032f;
+	csc->kub   = 0x0204;
+	csc->kvb   = 0x0000;
+}
+
+void tegra_dc_set_csc(struct tegra_dc *dc, struct tegra_dc_csc *csc)
+{
+	tegra_dc_writel(dc, csc->yof,	DC_WIN_CSC_YOF);
+	tegra_dc_writel(dc, csc->kyrgb,	DC_WIN_CSC_KYRGB);
+	tegra_dc_writel(dc, csc->kur,	DC_WIN_CSC_KUR);
+	tegra_dc_writel(dc, csc->kvr,	DC_WIN_CSC_KVR);
+	tegra_dc_writel(dc, csc->kug,	DC_WIN_CSC_KUG);
+	tegra_dc_writel(dc, csc->kvg,	DC_WIN_CSC_KVG);
+	tegra_dc_writel(dc, csc->kub,	DC_WIN_CSC_KUB);
+	tegra_dc_writel(dc, csc->kvb,	DC_WIN_CSC_KVB);
+}
+
+int tegra_dc_update_csc(struct tegra_dc *dc, int win_idx)
+{
+	mutex_lock(&dc->lock);
+
+	if (!dc->enabled) {
+		mutex_unlock(&dc->lock);
+		return -EFAULT;
+	}
+
+	tegra_dc_hold_dc_out(dc);
+	tegra_dc_writel(dc, WINDOW_A_SELECT << win_idx,
+			DC_CMD_DISPLAY_WINDOW_HEADER);
+
+	tegra_dc_set_csc(dc, &dc->windows[win_idx].csc);
+	tegra_dc_release_dc_out(dc);
+
+	mutex_unlock(&dc->lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(tegra_dc_update_csc);
+
diff --git a/drivers/video/tegra/dc/dc.c b/drivers/video/tegra/dc/dc.c
new file mode 100644
index 000000000000..d8e731999b88
--- /dev/null
+++ b/drivers/video/tegra/dc/dc.c
@@ -0,0 +1,2148 @@
+/*
+ * drivers/video/tegra/dc/dc.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/workqueue.h>
+#include <linux/ktime.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/backlight.h>
+#include <linux/gpio.h>
+#include <linux/nvhost.h>
+#include <video/tegrafb.h>
+#include <drm/drm_fixed.h>
+#ifdef CONFIG_SWITCH
+#include <linux/switch.h>
+#endif
+
+#include <mach/clk.h>
+#include <mach/dc.h>
+#include <mach/fb.h>
+#include <mach/mc.h>
+#include <linux/nvhost.h>
+#include <mach/latency_allowance.h>
+
+#include "dc_reg.h"
+#include "dc_config.h"
+#include "dc_priv.h"
+#include "nvsd.h"
+
+#define TEGRA_CRC_LATCHED_DELAY		34
+
+#define DC_COM_PIN_OUTPUT_POLARITY1_INIT_VAL	0x01000000
+#define DC_COM_PIN_OUTPUT_POLARITY3_INIT_VAL	0x0
+
+static struct tegra_dc_mode override_disp_mode[3];
+
+static void _tegra_dc_controller_disable(struct tegra_dc *dc);
+
+struct tegra_dc *tegra_dcs[TEGRA_MAX_DC];
+
+DEFINE_MUTEX(tegra_dc_lock);
+DEFINE_MUTEX(shared_lock);
+
+void tegra_dc_clk_enable(struct tegra_dc *dc)
+{
+	if (!tegra_is_clk_enabled(dc->clk)) {
+		clk_enable(dc->clk);
+		tegra_dvfs_set_rate(dc->clk, dc->mode.pclk);
+	}
+}
+
+void tegra_dc_clk_disable(struct tegra_dc *dc)
+{
+	if (tegra_is_clk_enabled(dc->clk)) {
+		clk_disable(dc->clk);
+		tegra_dvfs_set_rate(dc->clk, 0);
+	}
+}
+
+void tegra_dc_hold_dc_out(struct tegra_dc *dc)
+{
+	if (dc->out_ops->hold)
+		dc->out_ops->hold(dc);
+}
+
+void tegra_dc_release_dc_out(struct tegra_dc *dc)
+{
+	if (dc->out_ops->release)
+		dc->out_ops->release(dc);
+}
+
+#define DUMP_REG(a) do {			\
+	snprintf(buff, sizeof(buff), "%-32s\t%03x\t%08lx\n", \
+		 #a, a, tegra_dc_readl(dc, a));		      \
+	print(data, buff);				      \
+	} while (0)
+
+#define print_underflow_info(dc) do {                 \
+	trace_printk("%s:Underflow stats: underflows : %llu, "      \
+			"undeflows_a : %llu, "                          \
+			"underflows_b : %llu, "                         \
+			"underflows_c : %llu\n",                        \
+			dc->ndev->name,                                 \
+			dc->stats.underflows,                           \
+			dc->stats.underflows_a, dc->stats.underflows_b, \
+			dc->stats.underflows_c);                        \
+	} while (0)
+
+static void _dump_regs(struct tegra_dc *dc, void *data,
+		       void (* print)(void *data, const char *str))
+{
+	int i;
+	char buff[256];
+
+	mutex_lock(&dc->lock);
+	tegra_dc_hold_dc_out(dc);
+	tegra_dc_io_start(dc);
+
+	DUMP_REG(DC_CMD_DISPLAY_COMMAND_OPTION0);
+	DUMP_REG(DC_CMD_DISPLAY_COMMAND);
+	DUMP_REG(DC_CMD_SIGNAL_RAISE);
+	DUMP_REG(DC_CMD_INT_STATUS);
+	DUMP_REG(DC_CMD_INT_MASK);
+	DUMP_REG(DC_CMD_INT_ENABLE);
+	DUMP_REG(DC_CMD_INT_TYPE);
+	DUMP_REG(DC_CMD_INT_POLARITY);
+	DUMP_REG(DC_CMD_SIGNAL_RAISE1);
+	DUMP_REG(DC_CMD_SIGNAL_RAISE2);
+	DUMP_REG(DC_CMD_SIGNAL_RAISE3);
+	DUMP_REG(DC_CMD_STATE_ACCESS);
+	DUMP_REG(DC_CMD_STATE_CONTROL);
+	DUMP_REG(DC_CMD_DISPLAY_WINDOW_HEADER);
+	DUMP_REG(DC_CMD_REG_ACT_CONTROL);
+
+	DUMP_REG(DC_DISP_DISP_SIGNAL_OPTIONS0);
+	DUMP_REG(DC_DISP_DISP_SIGNAL_OPTIONS1);
+	DUMP_REG(DC_DISP_DISP_WIN_OPTIONS);
+	DUMP_REG(DC_DISP_MEM_HIGH_PRIORITY);
+	DUMP_REG(DC_DISP_MEM_HIGH_PRIORITY_TIMER);
+	DUMP_REG(DC_DISP_DISP_TIMING_OPTIONS);
+	DUMP_REG(DC_DISP_REF_TO_SYNC);
+	DUMP_REG(DC_DISP_SYNC_WIDTH);
+	DUMP_REG(DC_DISP_BACK_PORCH);
+	DUMP_REG(DC_DISP_DISP_ACTIVE);
+	DUMP_REG(DC_DISP_FRONT_PORCH);
+	DUMP_REG(DC_DISP_H_PULSE0_CONTROL);
+	DUMP_REG(DC_DISP_H_PULSE0_POSITION_A);
+	DUMP_REG(DC_DISP_H_PULSE0_POSITION_B);
+	DUMP_REG(DC_DISP_H_PULSE0_POSITION_C);
+	DUMP_REG(DC_DISP_H_PULSE0_POSITION_D);
+	DUMP_REG(DC_DISP_H_PULSE1_CONTROL);
+	DUMP_REG(DC_DISP_H_PULSE1_POSITION_A);
+	DUMP_REG(DC_DISP_H_PULSE1_POSITION_B);
+	DUMP_REG(DC_DISP_H_PULSE1_POSITION_C);
+	DUMP_REG(DC_DISP_H_PULSE1_POSITION_D);
+	DUMP_REG(DC_DISP_H_PULSE2_CONTROL);
+	DUMP_REG(DC_DISP_H_PULSE2_POSITION_A);
+	DUMP_REG(DC_DISP_H_PULSE2_POSITION_B);
+	DUMP_REG(DC_DISP_H_PULSE2_POSITION_C);
+	DUMP_REG(DC_DISP_H_PULSE2_POSITION_D);
+	DUMP_REG(DC_DISP_V_PULSE0_CONTROL);
+	DUMP_REG(DC_DISP_V_PULSE0_POSITION_A);
+	DUMP_REG(DC_DISP_V_PULSE0_POSITION_B);
+	DUMP_REG(DC_DISP_V_PULSE0_POSITION_C);
+	DUMP_REG(DC_DISP_V_PULSE1_CONTROL);
+	DUMP_REG(DC_DISP_V_PULSE1_POSITION_A);
+	DUMP_REG(DC_DISP_V_PULSE1_POSITION_B);
+	DUMP_REG(DC_DISP_V_PULSE1_POSITION_C);
+	DUMP_REG(DC_DISP_V_PULSE2_CONTROL);
+	DUMP_REG(DC_DISP_V_PULSE2_POSITION_A);
+	DUMP_REG(DC_DISP_V_PULSE3_CONTROL);
+	DUMP_REG(DC_DISP_V_PULSE3_POSITION_A);
+	DUMP_REG(DC_DISP_M0_CONTROL);
+	DUMP_REG(DC_DISP_M1_CONTROL);
+	DUMP_REG(DC_DISP_DI_CONTROL);
+	DUMP_REG(DC_DISP_PP_CONTROL);
+	DUMP_REG(DC_DISP_PP_SELECT_A);
+	DUMP_REG(DC_DISP_PP_SELECT_B);
+	DUMP_REG(DC_DISP_PP_SELECT_C);
+	DUMP_REG(DC_DISP_PP_SELECT_D);
+	DUMP_REG(DC_DISP_DISP_CLOCK_CONTROL);
+	DUMP_REG(DC_DISP_DISP_INTERFACE_CONTROL);
+	DUMP_REG(DC_DISP_DISP_COLOR_CONTROL);
+	DUMP_REG(DC_DISP_SHIFT_CLOCK_OPTIONS);
+	DUMP_REG(DC_DISP_DATA_ENABLE_OPTIONS);
+	DUMP_REG(DC_DISP_SERIAL_INTERFACE_OPTIONS);
+	DUMP_REG(DC_DISP_LCD_SPI_OPTIONS);
+	DUMP_REG(DC_DISP_BORDER_COLOR);
+	DUMP_REG(DC_DISP_COLOR_KEY0_LOWER);
+	DUMP_REG(DC_DISP_COLOR_KEY0_UPPER);
+	DUMP_REG(DC_DISP_COLOR_KEY1_LOWER);
+	DUMP_REG(DC_DISP_COLOR_KEY1_UPPER);
+	DUMP_REG(DC_DISP_CURSOR_FOREGROUND);
+	DUMP_REG(DC_DISP_CURSOR_BACKGROUND);
+	DUMP_REG(DC_DISP_CURSOR_START_ADDR);
+	DUMP_REG(DC_DISP_CURSOR_START_ADDR_NS);
+	DUMP_REG(DC_DISP_CURSOR_POSITION);
+	DUMP_REG(DC_DISP_CURSOR_POSITION_NS);
+	DUMP_REG(DC_DISP_INIT_SEQ_CONTROL);
+	DUMP_REG(DC_DISP_SPI_INIT_SEQ_DATA_A);
+	DUMP_REG(DC_DISP_SPI_INIT_SEQ_DATA_B);
+	DUMP_REG(DC_DISP_SPI_INIT_SEQ_DATA_C);
+	DUMP_REG(DC_DISP_SPI_INIT_SEQ_DATA_D);
+	DUMP_REG(DC_DISP_DC_MCCIF_FIFOCTRL);
+	DUMP_REG(DC_DISP_MCCIF_DISPLAY0A_HYST);
+	DUMP_REG(DC_DISP_MCCIF_DISPLAY0B_HYST);
+	DUMP_REG(DC_DISP_MCCIF_DISPLAY0C_HYST);
+	DUMP_REG(DC_DISP_MCCIF_DISPLAY1B_HYST);
+	DUMP_REG(DC_DISP_DAC_CRT_CTRL);
+	DUMP_REG(DC_DISP_DISP_MISC_CONTROL);
+
+
+	for (i = 0; i < 3; i++) {
+		print(data, "\n");
+		snprintf(buff, sizeof(buff), "WINDOW %c:\n", 'A' + i);
+		print(data, buff);
+
+		tegra_dc_writel(dc, WINDOW_A_SELECT << i,
+				DC_CMD_DISPLAY_WINDOW_HEADER);
+		DUMP_REG(DC_CMD_DISPLAY_WINDOW_HEADER);
+		DUMP_REG(DC_WIN_WIN_OPTIONS);
+		DUMP_REG(DC_WIN_BYTE_SWAP);
+		DUMP_REG(DC_WIN_BUFFER_CONTROL);
+		DUMP_REG(DC_WIN_COLOR_DEPTH);
+		DUMP_REG(DC_WIN_POSITION);
+		DUMP_REG(DC_WIN_SIZE);
+		DUMP_REG(DC_WIN_PRESCALED_SIZE);
+		DUMP_REG(DC_WIN_H_INITIAL_DDA);
+		DUMP_REG(DC_WIN_V_INITIAL_DDA);
+		DUMP_REG(DC_WIN_DDA_INCREMENT);
+		DUMP_REG(DC_WIN_LINE_STRIDE);
+		DUMP_REG(DC_WIN_BUF_STRIDE);
+		DUMP_REG(DC_WIN_UV_BUF_STRIDE);
+		DUMP_REG(DC_WIN_BLEND_NOKEY);
+		DUMP_REG(DC_WIN_BLEND_1WIN);
+		DUMP_REG(DC_WIN_BLEND_2WIN_X);
+		DUMP_REG(DC_WIN_BLEND_2WIN_Y);
+		DUMP_REG(DC_WIN_BLEND_3WIN_XY);
+		DUMP_REG(DC_WINBUF_START_ADDR);
+		DUMP_REG(DC_WINBUF_START_ADDR_U);
+		DUMP_REG(DC_WINBUF_START_ADDR_V);
+		DUMP_REG(DC_WINBUF_ADDR_H_OFFSET);
+		DUMP_REG(DC_WINBUF_ADDR_V_OFFSET);
+		DUMP_REG(DC_WINBUF_UFLOW_STATUS);
+		DUMP_REG(DC_WIN_CSC_YOF);
+		DUMP_REG(DC_WIN_CSC_KYRGB);
+		DUMP_REG(DC_WIN_CSC_KUR);
+		DUMP_REG(DC_WIN_CSC_KVR);
+		DUMP_REG(DC_WIN_CSC_KUG);
+		DUMP_REG(DC_WIN_CSC_KVG);
+		DUMP_REG(DC_WIN_CSC_KUB);
+		DUMP_REG(DC_WIN_CSC_KVB);
+	}
+
+	DUMP_REG(DC_CMD_DISPLAY_POWER_CONTROL);
+	DUMP_REG(DC_COM_PIN_OUTPUT_ENABLE2);
+	DUMP_REG(DC_COM_PIN_OUTPUT_POLARITY2);
+	DUMP_REG(DC_COM_PIN_OUTPUT_DATA2);
+	DUMP_REG(DC_COM_PIN_INPUT_ENABLE2);
+	DUMP_REG(DC_COM_PIN_OUTPUT_SELECT5);
+	DUMP_REG(DC_DISP_DISP_SIGNAL_OPTIONS0);
+	DUMP_REG(DC_DISP_M1_CONTROL);
+	DUMP_REG(DC_COM_PM1_CONTROL);
+	DUMP_REG(DC_COM_PM1_DUTY_CYCLE);
+	DUMP_REG(DC_DISP_SD_CONTROL);
+
+	tegra_dc_io_end(dc);
+	tegra_dc_release_dc_out(dc);
+	mutex_unlock(&dc->lock);
+}
+
+#undef DUMP_REG
+
+#ifdef DEBUG
+static void dump_regs_print(void *data, const char *str)
+{
+	struct tegra_dc *dc = data;
+	dev_dbg(&dc->ndev->dev, "%s", str);
+}
+
+static void dump_regs(struct tegra_dc *dc)
+{
+	_dump_regs(dc, dc, dump_regs_print);
+}
+#else /* !DEBUG */
+
+static void dump_regs(struct tegra_dc *dc) {}
+
+#endif /* DEBUG */
+
+#ifdef CONFIG_DEBUG_FS
+
+static void dbg_regs_print(void *data, const char *str)
+{
+	struct seq_file *s = data;
+
+	seq_printf(s, "%s", str);
+}
+
+#undef DUMP_REG
+
+static int dbg_dc_show(struct seq_file *s, void *unused)
+{
+	struct tegra_dc *dc = s->private;
+
+	_dump_regs(dc, s, dbg_regs_print);
+
+	return 0;
+}
+
+
+static int dbg_dc_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dbg_dc_show, inode->i_private);
+}
+
+static const struct file_operations regs_fops = {
+	.open		= dbg_dc_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int dbg_dc_mode_show(struct seq_file *s, void *unused)
+{
+	struct tegra_dc *dc = s->private;
+	struct tegra_dc_mode *m;
+
+	mutex_lock(&dc->lock);
+	m = &dc->mode;
+	seq_printf(s,
+		"pclk: %d\n"
+		"h_ref_to_sync: %d\n"
+		"v_ref_to_sync: %d\n"
+		"h_sync_width: %d\n"
+		"v_sync_width: %d\n"
+		"h_back_porch: %d\n"
+		"v_back_porch: %d\n"
+		"h_active: %d\n"
+		"v_active: %d\n"
+		"h_front_porch: %d\n"
+		"v_front_porch: %d\n"
+		"stereo_mode: %d\n",
+		m->pclk, m->h_ref_to_sync, m->v_ref_to_sync,
+		m->h_sync_width, m->v_sync_width,
+		m->h_back_porch, m->v_back_porch,
+		m->h_active, m->v_active,
+		m->h_front_porch, m->v_front_porch,
+		m->stereo_mode);
+	mutex_unlock(&dc->lock);
+	return 0;
+}
+
+static int dbg_dc_mode_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dbg_dc_mode_show, inode->i_private);
+}
+
+static const struct file_operations mode_fops = {
+	.open		= dbg_dc_mode_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int dbg_dc_stats_show(struct seq_file *s, void *unused)
+{
+	struct tegra_dc *dc = s->private;
+
+	mutex_lock(&dc->lock);
+	seq_printf(s,
+		"underflows: %llu\n"
+		"underflows_a: %llu\n"
+		"underflows_b: %llu\n"
+		"underflows_c: %llu\n",
+		dc->stats.underflows,
+		dc->stats.underflows_a,
+		dc->stats.underflows_b,
+		dc->stats.underflows_c);
+	mutex_unlock(&dc->lock);
+
+	return 0;
+}
+
+static int dbg_dc_stats_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dbg_dc_stats_show, inode->i_private);
+}
+
+static const struct file_operations stats_fops = {
+	.open		= dbg_dc_stats_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static void __devexit tegra_dc_remove_debugfs(struct tegra_dc *dc)
+{
+	if (dc->debugdir)
+		debugfs_remove_recursive(dc->debugdir);
+	dc->debugdir = NULL;
+}
+
+static void tegra_dc_create_debugfs(struct tegra_dc *dc)
+{
+	struct dentry *retval;
+
+	dc->debugdir = debugfs_create_dir(dev_name(&dc->ndev->dev), NULL);
+	if (!dc->debugdir)
+		goto remove_out;
+
+	retval = debugfs_create_file("regs", S_IRUGO, dc->debugdir, dc,
+		&regs_fops);
+	if (!retval)
+		goto remove_out;
+
+	retval = debugfs_create_file("mode", S_IRUGO, dc->debugdir, dc,
+		&mode_fops);
+	if (!retval)
+		goto remove_out;
+
+	retval = debugfs_create_file("stats", S_IRUGO, dc->debugdir, dc,
+		&stats_fops);
+	if (!retval)
+		goto remove_out;
+
+	return;
+remove_out:
+	dev_err(&dc->ndev->dev, "could not create debugfs\n");
+	tegra_dc_remove_debugfs(dc);
+}
+
+#else /* !CONFIG_DEBUGFS */
+static inline void tegra_dc_create_debugfs(struct tegra_dc *dc) { };
+static inline void __devexit tegra_dc_remove_debugfs(struct tegra_dc *dc) { };
+#endif /* CONFIG_DEBUGFS */
+
+static int tegra_dc_set(struct tegra_dc *dc, int index)
+{
+	int ret = 0;
+
+	mutex_lock(&tegra_dc_lock);
+	if (index >= TEGRA_MAX_DC) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (dc != NULL && tegra_dcs[index] != NULL) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	tegra_dcs[index] = dc;
+
+out:
+	mutex_unlock(&tegra_dc_lock);
+
+	return ret;
+}
+
+unsigned int tegra_dc_has_multiple_dc(void)
+{
+	unsigned int idx;
+	unsigned int cnt = 0;
+	struct tegra_dc *dc;
+
+	mutex_lock(&tegra_dc_lock);
+	for (idx = 0; idx < TEGRA_MAX_DC; idx++)
+		cnt += ((dc = tegra_dcs[idx]) != NULL && dc->enabled) ? 1 : 0;
+	mutex_unlock(&tegra_dc_lock);
+
+	return (cnt > 1);
+}
+
+/* get the stride size of a window.
+ * return: stride size in bytes for window win. or 0 if unavailble. */
+int tegra_dc_get_stride(struct tegra_dc *dc, unsigned win)
+{
+	u32 stride;
+
+	if (!dc->enabled)
+		return 0;
+	BUG_ON(win > DC_N_WINDOWS);
+	mutex_lock(&dc->lock);
+	tegra_dc_hold_dc_out(dc);
+	tegra_dc_writel(dc, WINDOW_A_SELECT << win,
+		DC_CMD_DISPLAY_WINDOW_HEADER);
+	stride = tegra_dc_readl(dc, DC_WIN_LINE_STRIDE);
+	tegra_dc_release_dc_out(dc);
+	mutex_unlock(&dc->lock);
+	return GET_LINE_STRIDE(stride);
+}
+EXPORT_SYMBOL(tegra_dc_get_stride);
+
+struct tegra_dc *tegra_dc_get_dc(unsigned idx)
+{
+	if (idx < TEGRA_MAX_DC)
+		return tegra_dcs[idx];
+	else
+		return NULL;
+}
+EXPORT_SYMBOL(tegra_dc_get_dc);
+
+struct tegra_dc_win *tegra_dc_get_window(struct tegra_dc *dc, unsigned win)
+{
+	if (win >= dc->n_windows)
+		return NULL;
+
+	return &dc->windows[win];
+}
+EXPORT_SYMBOL(tegra_dc_get_window);
+
+bool tegra_dc_get_connected(struct tegra_dc *dc)
+{
+	return dc->connected;
+}
+EXPORT_SYMBOL(tegra_dc_get_connected);
+
+bool tegra_dc_hpd(struct tegra_dc *dc)
+{
+	int sense;
+	int level;
+
+	level = gpio_get_value(dc->out->hotplug_gpio);
+
+	sense = dc->out->flags & TEGRA_DC_OUT_HOTPLUG_MASK;
+
+	return (sense == TEGRA_DC_OUT_HOTPLUG_HIGH && level) ||
+		(sense == TEGRA_DC_OUT_HOTPLUG_LOW && !level);
+}
+EXPORT_SYMBOL(tegra_dc_hpd);
+
+static void tegra_dc_set_scaling_filter(struct tegra_dc *dc)
+{
+	unsigned i;
+	unsigned v0 = 128;
+	unsigned v1 = 0;
+	/* linear horizontal and vertical filters */
+	for (i = 0; i < 16; i++) {
+		tegra_dc_writel(dc, (v1 << 16) | (v0 << 8),
+				DC_WIN_H_FILTER_P(i));
+
+		tegra_dc_writel(dc, v0,
+				DC_WIN_V_FILTER_P(i));
+		v0 -= 8;
+		v1 += 8;
+	}
+}
+
+static inline void disable_dc_irq(unsigned int irq)
+{
+	disable_irq(irq);
+}
+
+u32 tegra_dc_get_syncpt_id(const struct tegra_dc *dc, int i)
+{
+	return dc->syncpt[i].id;
+}
+EXPORT_SYMBOL(tegra_dc_get_syncpt_id);
+
+u32 tegra_dc_incr_syncpt_max(struct tegra_dc *dc, int i)
+{
+	u32 max;
+
+	mutex_lock(&dc->lock);
+	tegra_dc_hold_dc_out(dc);
+	max = nvhost_syncpt_incr_max_ext(dc->ndev,
+		dc->syncpt[i].id, ((dc->enabled) ? 1 : 0));
+	dc->syncpt[i].max = max;
+	tegra_dc_release_dc_out(dc);
+	mutex_unlock(&dc->lock);
+
+	return max;
+}
+
+void tegra_dc_incr_syncpt_min(struct tegra_dc *dc, int i, u32 val)
+{
+	mutex_lock(&dc->lock);
+	if (dc->enabled) {
+		tegra_dc_hold_dc_out(dc);
+		while (dc->syncpt[i].min < val) {
+			dc->syncpt[i].min++;
+			nvhost_syncpt_cpu_incr_ext(dc->ndev, dc->syncpt[i].id);
+		}
+		tegra_dc_release_dc_out(dc);
+	}
+	mutex_unlock(&dc->lock);
+}
+
+void
+tegra_dc_config_pwm(struct tegra_dc *dc, struct tegra_dc_pwm_params *cfg)
+{
+	unsigned int ctrl;
+	unsigned long out_sel;
+	unsigned long cmd_state;
+
+	mutex_lock(&dc->lock);
+	if (!dc->enabled) {
+		mutex_unlock(&dc->lock);
+		return;
+	}
+
+	tegra_dc_hold_dc_out(dc);
+
+	ctrl = ((cfg->period << PM_PERIOD_SHIFT) |
+		(cfg->clk_div << PM_CLK_DIVIDER_SHIFT) |
+		cfg->clk_select);
+
+	/* The new value should be effected immediately */
+	cmd_state = tegra_dc_readl(dc, DC_CMD_STATE_ACCESS);
+	tegra_dc_writel(dc, (cmd_state | (1 << 2)), DC_CMD_STATE_ACCESS);
+
+	switch (cfg->which_pwm) {
+	case TEGRA_PWM_PM0:
+		/* Select the LM0 on PM0 */
+		out_sel = tegra_dc_readl(dc, DC_COM_PIN_OUTPUT_SELECT5);
+		out_sel &= ~(7 << 0);
+		out_sel |= (3 << 0);
+		tegra_dc_writel(dc, out_sel, DC_COM_PIN_OUTPUT_SELECT5);
+		tegra_dc_writel(dc, ctrl, DC_COM_PM0_CONTROL);
+		tegra_dc_writel(dc, cfg->duty_cycle, DC_COM_PM0_DUTY_CYCLE);
+		break;
+	case TEGRA_PWM_PM1:
+		/* Select the LM1 on PM1 */
+		out_sel = tegra_dc_readl(dc, DC_COM_PIN_OUTPUT_SELECT5);
+		out_sel &= ~(7 << 4);
+		out_sel |= (3 << 4);
+		tegra_dc_writel(dc, out_sel, DC_COM_PIN_OUTPUT_SELECT5);
+		tegra_dc_writel(dc, ctrl, DC_COM_PM1_CONTROL);
+		tegra_dc_writel(dc, cfg->duty_cycle, DC_COM_PM1_DUTY_CYCLE);
+		break;
+	default:
+		dev_err(&dc->ndev->dev, "Error: Need which_pwm\n");
+		break;
+	}
+	tegra_dc_writel(dc, cmd_state, DC_CMD_STATE_ACCESS);
+	tegra_dc_release_dc_out(dc);
+	mutex_unlock(&dc->lock);
+}
+EXPORT_SYMBOL(tegra_dc_config_pwm);
+
+void tegra_dc_set_out_pin_polars(struct tegra_dc *dc,
+				const struct tegra_dc_out_pin *pins,
+				const unsigned int n_pins)
+{
+	unsigned int i;
+
+	int name;
+	int pol;
+
+	u32 pol1, pol3;
+
+	u32 set1, unset1;
+	u32 set3, unset3;
+
+	set1 = set3 = unset1 = unset3 = 0;
+
+	for (i = 0; i < n_pins; i++) {
+		name = (pins + i)->name;
+		pol  = (pins + i)->pol;
+
+		/* set polarity by name */
+		switch (name) {
+		case TEGRA_DC_OUT_PIN_DATA_ENABLE:
+			if (pol == TEGRA_DC_OUT_PIN_POL_LOW)
+				set3 |= LSPI_OUTPUT_POLARITY_LOW;
+			else
+				unset3 |= LSPI_OUTPUT_POLARITY_LOW;
+			break;
+		case TEGRA_DC_OUT_PIN_H_SYNC:
+			if (pol == TEGRA_DC_OUT_PIN_POL_LOW)
+				set1 |= LHS_OUTPUT_POLARITY_LOW;
+			else
+				unset1 |= LHS_OUTPUT_POLARITY_LOW;
+			break;
+		case TEGRA_DC_OUT_PIN_V_SYNC:
+			if (pol == TEGRA_DC_OUT_PIN_POL_LOW)
+				set1 |= LVS_OUTPUT_POLARITY_LOW;
+			else
+				unset1 |= LVS_OUTPUT_POLARITY_LOW;
+			break;
+		case TEGRA_DC_OUT_PIN_PIXEL_CLOCK:
+			if (pol == TEGRA_DC_OUT_PIN_POL_LOW)
+				set1 |= LSC0_OUTPUT_POLARITY_LOW;
+			else
+				unset1 |= LSC0_OUTPUT_POLARITY_LOW;
+			break;
+		default:
+			printk("Invalid argument in function %s\n",
+			       __FUNCTION__);
+			break;
+		}
+	}
+
+	pol1 = DC_COM_PIN_OUTPUT_POLARITY1_INIT_VAL;
+	pol3 = DC_COM_PIN_OUTPUT_POLARITY3_INIT_VAL;
+
+	pol1 |= set1;
+	pol1 &= ~unset1;
+
+	pol3 |= set3;
+	pol3 &= ~unset3;
+
+	tegra_dc_writel(dc, pol1, DC_COM_PIN_OUTPUT_POLARITY1);
+	tegra_dc_writel(dc, pol3, DC_COM_PIN_OUTPUT_POLARITY3);
+}
+
+static struct tegra_dc_mode *tegra_dc_get_override_mode(struct tegra_dc *dc)
+{
+	if (dc->out->type == TEGRA_DC_OUT_RGB ||
+		dc->out->type == TEGRA_DC_OUT_HDMI ||
+		dc->out->type == TEGRA_DC_OUT_DSI)
+		return override_disp_mode[dc->out->type].pclk ?
+			&override_disp_mode[dc->out->type] : NULL;
+	else
+		return NULL;
+}
+
+static void tegra_dc_set_out(struct tegra_dc *dc, struct tegra_dc_out *out)
+{
+	struct tegra_dc_mode *mode;
+
+	dc->out = out;
+	mode = tegra_dc_get_override_mode(dc);
+
+	if (mode)
+		tegra_dc_set_mode(dc, mode);
+	else if (out->n_modes > 0)
+		tegra_dc_set_mode(dc, &dc->out->modes[0]);
+
+	switch (out->type) {
+	case TEGRA_DC_OUT_RGB:
+		dc->out_ops = &tegra_dc_rgb_ops;
+		break;
+
+	case TEGRA_DC_OUT_HDMI:
+		dc->out_ops = &tegra_dc_hdmi_ops;
+		break;
+
+	case TEGRA_DC_OUT_DSI:
+		dc->out_ops = &tegra_dc_dsi_ops;
+		break;
+
+	default:
+		dc->out_ops = NULL;
+		break;
+	}
+
+	if (dc->out_ops && dc->out_ops->init)
+		dc->out_ops->init(dc);
+
+}
+
+unsigned tegra_dc_get_out_height(const struct tegra_dc *dc)
+{
+	if (dc->out)
+		return dc->out->height;
+	else
+		return 0;
+}
+EXPORT_SYMBOL(tegra_dc_get_out_height);
+
+unsigned tegra_dc_get_out_width(const struct tegra_dc *dc)
+{
+	if (dc->out)
+		return dc->out->width;
+	else
+		return 0;
+}
+EXPORT_SYMBOL(tegra_dc_get_out_width);
+
+unsigned tegra_dc_get_out_max_pixclock(const struct tegra_dc *dc)
+{
+	if (dc->out && dc->out->max_pixclock)
+		return dc->out->max_pixclock;
+	else
+		return 0;
+}
+EXPORT_SYMBOL(tegra_dc_get_out_max_pixclock);
+
+void tegra_dc_enable_crc(struct tegra_dc *dc)
+{
+	u32 val;
+
+	mutex_lock(&dc->lock);
+	tegra_dc_hold_dc_out(dc);
+	tegra_dc_io_start(dc);
+
+	val = CRC_ALWAYS_ENABLE | CRC_INPUT_DATA_ACTIVE_DATA |
+		CRC_ENABLE_ENABLE;
+	tegra_dc_writel(dc, val, DC_COM_CRC_CONTROL);
+	tegra_dc_writel(dc, GENERAL_UPDATE, DC_CMD_STATE_CONTROL);
+	tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
+	tegra_dc_release_dc_out(dc);
+	mutex_unlock(&dc->lock);
+}
+
+void tegra_dc_disable_crc(struct tegra_dc *dc)
+{
+	mutex_lock(&dc->lock);
+	tegra_dc_hold_dc_out(dc);
+	tegra_dc_writel(dc, 0x0, DC_COM_CRC_CONTROL);
+	tegra_dc_writel(dc, GENERAL_UPDATE, DC_CMD_STATE_CONTROL);
+	tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
+
+	tegra_dc_io_end(dc);
+	tegra_dc_release_dc_out(dc);
+	mutex_unlock(&dc->lock);
+}
+
+u32 tegra_dc_read_checksum_latched(struct tegra_dc *dc)
+{
+	int crc = 0;
+
+	if (!dc) {
+		dev_err(&dc->ndev->dev, "Failed to get dc.\n");
+		goto crc_error;
+	}
+
+	/* TODO: Replace mdelay with code to sync VBlANK, since
+	 * DC_COM_CRC_CHECKSUM_LATCHED is available after VBLANK */
+	mdelay(TEGRA_CRC_LATCHED_DELAY);
+
+	mutex_lock(&dc->lock);
+	tegra_dc_hold_dc_out(dc);
+	crc = tegra_dc_readl(dc, DC_COM_CRC_CHECKSUM_LATCHED);
+	tegra_dc_release_dc_out(dc);
+	mutex_unlock(&dc->lock);
+crc_error:
+	return crc;
+}
+
+static bool tegra_dc_windows_are_dirty(struct tegra_dc *dc)
+{
+#ifndef CONFIG_TEGRA_SIMULATION_PLATFORM
+	u32 val;
+
+	val = tegra_dc_readl(dc, DC_CMD_STATE_CONTROL);
+	if (val & (WIN_A_ACT_REQ | WIN_B_ACT_REQ | WIN_C_ACT_REQ))
+	    return true;
+#endif
+	return false;
+}
+
+static inline void enable_dc_irq(unsigned int irq)
+{
+#ifndef CONFIG_TEGRA_FPGA_PLATFORM
+	enable_irq(irq);
+#else
+	/* Always disable DC interrupts on FPGA. */
+	disable_irq(irq);
+#endif
+}
+
+void tegra_dc_get_fbvblank(struct tegra_dc *dc, struct fb_vblank *vblank)
+{
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
+		vblank->flags = FB_VBLANK_HAVE_VSYNC;
+}
+
+int tegra_dc_wait_for_vsync(struct tegra_dc *dc)
+{
+	int ret = -ENOTTY;
+
+	if (!(dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE) || !dc->enabled)
+		return ret;
+
+	/*
+	 * Logic is as follows
+	 * a) Indicate we need a vblank.
+	 * b) Wait for completion to be signalled from isr.
+	 * c) Initialize completion for next iteration.
+	 */
+
+	tegra_dc_hold_dc_out(dc);
+	dc->out->user_needs_vblank = true;
+
+	ret = wait_for_completion_interruptible(&dc->out->user_vblank_comp);
+	init_completion(&dc->out->user_vblank_comp);
+	tegra_dc_release_dc_out(dc);
+
+	return ret;
+}
+
+static void tegra_dc_vblank(struct work_struct *work)
+{
+	struct tegra_dc *dc = container_of(work, struct tegra_dc, vblank_work);
+	bool nvsd_updated = false;
+
+	mutex_lock(&dc->lock);
+
+	if (!dc->enabled) {
+		mutex_unlock(&dc->lock);
+		return;
+	}
+
+	tegra_dc_hold_dc_out(dc);
+	/* use the new frame's bandwidth setting instead of max(current, new),
+	 * skip this if we're using tegra_dc_one_shot_worker() */
+	if (!(dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE))
+		tegra_dc_program_bandwidth(dc, true);
+
+	/* Clear the V_BLANK_FLIP bit of vblank ref-count if update is clean. */
+	if (!tegra_dc_windows_are_dirty(dc))
+		clear_bit(V_BLANK_FLIP, &dc->vblank_ref_count);
+
+	/* Update the SD brightness */
+	if (dc->enabled && dc->out->sd_settings) {
+		nvsd_updated = nvsd_update_brightness(dc);
+		/* Ref-count vblank if nvsd is on-going. Otherwise, clean the
+		 * V_BLANK_NVSD bit of vblank ref-count. */
+		if (nvsd_updated) {
+			set_bit(V_BLANK_NVSD, &dc->vblank_ref_count);
+			tegra_dc_unmask_interrupt(dc, V_BLANK_INT);
+		} else {
+			clear_bit(V_BLANK_NVSD, &dc->vblank_ref_count);
+		}
+	}
+
+	/* Mask vblank interrupt if ref-count is zero. */
+	if (!dc->vblank_ref_count)
+		tegra_dc_mask_interrupt(dc, V_BLANK_INT);
+
+	tegra_dc_release_dc_out(dc);
+	mutex_unlock(&dc->lock);
+
+	/* Do the actual brightness update outside of the mutex */
+	if (nvsd_updated && dc->out->sd_settings &&
+	    dc->out->sd_settings->bl_device) {
+
+		struct platform_device *pdev = dc->out->sd_settings->bl_device;
+		struct backlight_device *bl = platform_get_drvdata(pdev);
+		if (bl)
+			backlight_update_status(bl);
+	}
+}
+
+static void tegra_dc_one_shot_worker(struct work_struct *work)
+{
+	struct tegra_dc *dc = container_of(
+		to_delayed_work(work), struct tegra_dc, one_shot_work);
+	mutex_lock(&dc->lock);
+
+	/* memory client has gone idle */
+	tegra_dc_clear_bandwidth(dc);
+
+	if (dc->out_ops->idle)
+		dc->out_ops->idle(dc);
+
+	mutex_unlock(&dc->lock);
+}
+
+/* return an arbitrarily large number if count overflow occurs.
+ * make it a nice base-10 number to show up in stats output */
+static u64 tegra_dc_underflow_count(struct tegra_dc *dc, unsigned reg)
+{
+	unsigned count = tegra_dc_readl(dc, reg);
+	tegra_dc_writel(dc, 0, reg);
+	return ((count & 0x80000000) == 0) ? count : 10000000000ll;
+}
+
+static void tegra_dc_underflow_handler(struct tegra_dc *dc)
+{
+	u32 val;
+	int i;
+
+	dc->stats.underflows++;
+	if (dc->underflow_mask & WIN_A_UF_INT) {
+		dc->stats.underflows_a += tegra_dc_underflow_count(dc,
+			DC_WINBUF_AD_UFLOW_STATUS);
+		trace_printk("%s:Window A Underflow\n", dc->ndev->name);
+	}
+	if (dc->underflow_mask & WIN_B_UF_INT) {
+		dc->stats.underflows_b += tegra_dc_underflow_count(dc,
+			DC_WINBUF_BD_UFLOW_STATUS);
+		trace_printk("%s:Window B Underflow\n", dc->ndev->name);
+	}
+	if (dc->underflow_mask & WIN_C_UF_INT) {
+		dc->stats.underflows_c += tegra_dc_underflow_count(dc,
+			DC_WINBUF_CD_UFLOW_STATUS);
+		trace_printk("%s:Window C Underflow\n", dc->ndev->name);
+	}
+
+	/* Check for any underflow reset conditions */
+	for (i = 0; i < DC_N_WINDOWS; i++) {
+		if (dc->underflow_mask & (WIN_A_UF_INT << i)) {
+			dc->windows[i].underflows++;
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+			if (dc->windows[i].underflows > 4) {
+				schedule_work(&dc->reset_work);
+				/* reset counter */
+				dc->windows[i].underflows = 0;
+				trace_printk("%s:Reset work scheduled for "
+						"window %c\n",
+						dc->ndev->name, (65 + i));
+			}
+#endif
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+			if (dc->windows[i].underflows > 4) {
+				trace_printk("%s:window %c in underflow state."
+					" enable UF_LINE_FLUSH to clear up\n",
+					dc->ndev->name, (65 + i));
+				tegra_dc_writel(dc, UF_LINE_FLUSH,
+						DC_DISP_DISP_MISC_CONTROL);
+				tegra_dc_writel(dc, GENERAL_UPDATE,
+						DC_CMD_STATE_CONTROL);
+				tegra_dc_writel(dc, GENERAL_ACT_REQ,
+						DC_CMD_STATE_CONTROL);
+
+				tegra_dc_writel(dc, 0,
+						DC_DISP_DISP_MISC_CONTROL);
+				tegra_dc_writel(dc, GENERAL_UPDATE,
+						DC_CMD_STATE_CONTROL);
+				tegra_dc_writel(dc, GENERAL_ACT_REQ,
+						DC_CMD_STATE_CONTROL);
+			}
+#endif
+		} else {
+			dc->windows[i].underflows = 0;
+		}
+	}
+
+	/* Clear the underflow mask now that we've checked it. */
+	tegra_dc_writel(dc, dc->underflow_mask, DC_CMD_INT_STATUS);
+	dc->underflow_mask = 0;
+	val = tegra_dc_readl(dc, DC_CMD_INT_MASK);
+	tegra_dc_writel(dc, val | ALL_UF_INT, DC_CMD_INT_MASK);
+	print_underflow_info(dc);
+}
+
+#ifndef CONFIG_TEGRA_FPGA_PLATFORM
+static void tegra_dc_one_shot_irq(struct tegra_dc *dc, unsigned long status)
+{
+	/* pending user vblank, so wakeup */
+	if ((status & (V_BLANK_INT | MSF_INT)) &&
+	    (dc->out->user_needs_vblank)) {
+		dc->out->user_needs_vblank = false;
+		complete(&dc->out->user_vblank_comp);
+	}
+
+	if (status & V_BLANK_INT) {
+		/* Sync up windows. */
+		tegra_dc_trigger_windows(dc);
+
+		/* Schedule any additional bottom-half vblank actvities. */
+		queue_work(system_freezable_wq, &dc->vblank_work);
+	}
+
+	if (status & FRAME_END_INT) {
+		/* Mark the frame_end as complete. */
+		if (!completion_done(&dc->frame_end_complete))
+			complete(&dc->frame_end_complete);
+	}
+}
+
+static void tegra_dc_continuous_irq(struct tegra_dc *dc, unsigned long status)
+{
+	/* Schedule any additional bottom-half vblank actvities. */
+	if (status & V_BLANK_INT)
+		queue_work(system_freezable_wq, &dc->vblank_work);
+
+	if (status & FRAME_END_INT) {
+#ifndef CONFIG_ANDROID
+		struct timespec tm = CURRENT_TIME;
+		dc->frame_end_timestamp = timespec_to_ns(&tm);
+		wake_up(&dc->timestamp_wq);
+#endif /* !CONFIG_ANDROID */
+
+		/* Mark the frame_end as complete. */
+		if (!completion_done(&dc->frame_end_complete))
+			complete(&dc->frame_end_complete);
+
+		tegra_dc_trigger_windows(dc);
+	}
+}
+
+#ifndef CONFIG_ANDROID
+/* XXX: Not sure if we limit look ahead to 1 frame */
+bool tegra_dc_is_within_n_vsync(struct tegra_dc *dc, s64 ts)
+{
+	BUG_ON(!dc->frametime_ns);
+	return ((ts - dc->frame_end_timestamp) < dc->frametime_ns);
+}
+
+bool tegra_dc_does_vsync_separate(struct tegra_dc *dc, s64 new_ts, s64 old_ts)
+{
+	BUG_ON(!dc->frametime_ns);
+	return (((new_ts - old_ts) > dc->frametime_ns)
+		|| (div_s64((new_ts - dc->frame_end_timestamp), dc->frametime_ns)
+			!= div_s64((old_ts - dc->frame_end_timestamp),
+				dc->frametime_ns)));
+}
+#endif /* !CONFIG_ANDROID */
+#endif
+
+static irqreturn_t tegra_dc_irq(int irq, void *ptr)
+{
+#ifndef CONFIG_TEGRA_FPGA_PLATFORM
+	struct tegra_dc *dc = ptr;
+	unsigned long status;
+	unsigned long underflow_mask;
+	u32 val;
+
+	if (!nvhost_module_powered_ext(nvhost_get_parent(dc->ndev))) {
+		WARN(1, "IRQ when DC not powered!\n");
+		tegra_dc_io_start(dc);
+		status = tegra_dc_readl(dc, DC_CMD_INT_STATUS);
+		tegra_dc_writel(dc, status, DC_CMD_INT_STATUS);
+		tegra_dc_io_end(dc);
+		return IRQ_HANDLED;
+	}
+
+	/* clear all status flags except underflow, save those for the worker */
+	status = tegra_dc_readl(dc, DC_CMD_INT_STATUS);
+	tegra_dc_writel(dc, status & ~ALL_UF_INT, DC_CMD_INT_STATUS);
+	val = tegra_dc_readl(dc, DC_CMD_INT_MASK);
+	tegra_dc_writel(dc, val & ~ALL_UF_INT, DC_CMD_INT_MASK);
+
+	/*
+	 * Overlays can get thier internal state corrupted during and underflow
+	 * condition.  The only way to fix this state is to reset the DC.
+	 * if we get 4 consecutive frames with underflows, assume we're
+	 * hosed and reset.
+	 */
+	underflow_mask = status & ALL_UF_INT;
+
+	/* Check underflow */
+	if (underflow_mask) {
+		dc->underflow_mask |= underflow_mask;
+		schedule_delayed_work(&dc->underflow_work,
+			msecs_to_jiffies(1));
+	}
+
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
+		tegra_dc_one_shot_irq(dc, status);
+	else
+		tegra_dc_continuous_irq(dc, status);
+
+	return IRQ_HANDLED;
+#else /* CONFIG_TEGRA_FPGA_PLATFORM */
+	return IRQ_NONE;
+#endif /* !CONFIG_TEGRA_FPGA_PLATFORM */
+}
+
+static void tegra_dc_set_color_control(struct tegra_dc *dc)
+{
+	u32 color_control;
+
+	switch (dc->out->depth) {
+	case 3:
+		color_control = BASE_COLOR_SIZE111;
+		break;
+
+	case 6:
+		color_control = BASE_COLOR_SIZE222;
+		break;
+
+	case 8:
+		color_control = BASE_COLOR_SIZE332;
+		break;
+
+	case 9:
+		color_control = BASE_COLOR_SIZE333;
+		break;
+
+	case 12:
+		color_control = BASE_COLOR_SIZE444;
+		break;
+
+	case 15:
+		color_control = BASE_COLOR_SIZE555;
+		break;
+
+	case 16:
+		color_control = BASE_COLOR_SIZE565;
+		break;
+
+	case 18:
+		color_control = BASE_COLOR_SIZE666;
+		break;
+
+	default:
+		color_control = BASE_COLOR_SIZE888;
+		break;
+	}
+
+	switch (dc->out->dither) {
+	case TEGRA_DC_DISABLE_DITHER:
+		color_control |= DITHER_CONTROL_DISABLE;
+		break;
+	case TEGRA_DC_ORDERED_DITHER:
+		color_control |= DITHER_CONTROL_ORDERED;
+		break;
+	case TEGRA_DC_ERRDIFF_DITHER:
+		/* The line buffer for error-diffusion dither is limited
+		 * to 1280 pixels per line. This limits the maximum
+		 * horizontal active area size to 1280 pixels when error
+		 * diffusion is enabled.
+		 */
+		BUG_ON(dc->mode.h_active > 1280);
+		color_control |= DITHER_CONTROL_ERRDIFF;
+		break;
+	}
+
+	tegra_dc_writel(dc, color_control, DC_DISP_DISP_COLOR_CONTROL);
+}
+
+static u32 get_syncpt(struct tegra_dc *dc, int idx)
+{
+	u32 syncpt_id;
+
+	switch (dc->ndev->id) {
+	case 0:
+		switch (idx) {
+		case 0:
+			syncpt_id = NVSYNCPT_DISP0_A;
+			break;
+		case 1:
+			syncpt_id = NVSYNCPT_DISP0_B;
+			break;
+		case 2:
+			syncpt_id = NVSYNCPT_DISP0_C;
+			break;
+		default:
+			BUG();
+			break;
+		}
+		break;
+	case 1:
+		switch (idx) {
+		case 0:
+			syncpt_id = NVSYNCPT_DISP1_A;
+			break;
+		case 1:
+			syncpt_id = NVSYNCPT_DISP1_B;
+			break;
+		case 2:
+			syncpt_id = NVSYNCPT_DISP1_C;
+			break;
+		default:
+			BUG();
+			break;
+		}
+		break;
+	default:
+		BUG();
+		break;
+	}
+
+	return syncpt_id;
+}
+
+static int tegra_dc_init(struct tegra_dc *dc)
+{
+	int i;
+	int int_enable;
+
+	tegra_dc_writel(dc, 0x00000100, DC_CMD_GENERAL_INCR_SYNCPT_CNTRL);
+	if (dc->ndev->id == 0) {
+		tegra_mc_set_priority(TEGRA_MC_CLIENT_DISPLAY0A,
+				      TEGRA_MC_PRIO_MED);
+		tegra_mc_set_priority(TEGRA_MC_CLIENT_DISPLAY0B,
+				      TEGRA_MC_PRIO_MED);
+		tegra_mc_set_priority(TEGRA_MC_CLIENT_DISPLAY0C,
+				      TEGRA_MC_PRIO_MED);
+		tegra_mc_set_priority(TEGRA_MC_CLIENT_DISPLAY1B,
+				      TEGRA_MC_PRIO_MED);
+		tegra_mc_set_priority(TEGRA_MC_CLIENT_DISPLAYHC,
+				      TEGRA_MC_PRIO_HIGH);
+	} else if (dc->ndev->id == 1) {
+		tegra_mc_set_priority(TEGRA_MC_CLIENT_DISPLAY0AB,
+				      TEGRA_MC_PRIO_MED);
+		tegra_mc_set_priority(TEGRA_MC_CLIENT_DISPLAY0BB,
+				      TEGRA_MC_PRIO_MED);
+		tegra_mc_set_priority(TEGRA_MC_CLIENT_DISPLAY0CB,
+				      TEGRA_MC_PRIO_MED);
+		tegra_mc_set_priority(TEGRA_MC_CLIENT_DISPLAY1BB,
+				      TEGRA_MC_PRIO_MED);
+		tegra_mc_set_priority(TEGRA_MC_CLIENT_DISPLAYHCB,
+				      TEGRA_MC_PRIO_HIGH);
+	}
+	tegra_dc_writel(dc, 0x00000100 | dc->vblank_syncpt,
+			DC_CMD_CONT_SYNCPT_VSYNC);
+	tegra_dc_writel(dc, 0x00004700, DC_CMD_INT_TYPE);
+	tegra_dc_writel(dc, 0x0001c700, DC_CMD_INT_POLARITY);
+	tegra_dc_writel(dc, 0x00202020, DC_DISP_MEM_HIGH_PRIORITY);
+	tegra_dc_writel(dc, 0x00010101, DC_DISP_MEM_HIGH_PRIORITY_TIMER);
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+	tegra_dc_writel(dc, 0x00000000, DC_DISP_DISP_MISC_CONTROL);
+#endif
+	/* enable interrupts for vblank, frame_end and underflows */
+	int_enable = (FRAME_END_INT | V_BLANK_INT | ALL_UF_INT);
+	/* for panels with one-shot mode enable tearing effect interrupt */
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
+		int_enable |= MSF_INT;
+
+	tegra_dc_writel(dc, int_enable, DC_CMD_INT_ENABLE);
+	tegra_dc_writel(dc, ALL_UF_INT, DC_CMD_INT_MASK);
+
+	tegra_dc_writel(dc, 0x00000000, DC_DISP_BORDER_COLOR);
+
+	tegra_dc_set_color_control(dc);
+	for (i = 0; i < DC_N_WINDOWS; i++) {
+		struct tegra_dc_win *win = &dc->windows[i];
+		tegra_dc_writel(dc, WINDOW_A_SELECT << i,
+				DC_CMD_DISPLAY_WINDOW_HEADER);
+		tegra_dc_set_csc(dc, &win->csc);
+		tegra_dc_set_lut(dc, win);
+		tegra_dc_set_scaling_filter(dc);
+	}
+
+
+	for (i = 0; i < dc->n_windows; i++) {
+		u32 syncpt = get_syncpt(dc, i);
+
+		dc->syncpt[i].id = syncpt;
+
+		dc->syncpt[i].min = dc->syncpt[i].max =
+			nvhost_syncpt_read_ext(dc->ndev, syncpt);
+	}
+
+	print_mode_info(dc, dc->mode);
+
+	if (dc->mode.pclk)
+		if (tegra_dc_program_mode(dc, &dc->mode))
+			return -EINVAL;
+
+	/* Initialize SD AFTER the modeset.
+	   nvsd_init handles the sd_settings = NULL case. */
+	nvsd_init(dc, dc->out->sd_settings);
+
+	return 0;
+}
+
+static bool _tegra_dc_controller_enable(struct tegra_dc *dc)
+{
+	int failed_init = 0;
+
+	if (dc->out->enable)
+		dc->out->enable();
+
+	tegra_dc_setup_clk(dc, dc->clk);
+	tegra_dc_clk_enable(dc);
+
+	/* do not accept interrupts during initialization */
+	tegra_dc_writel(dc, 0, DC_CMD_INT_MASK);
+
+	enable_dc_irq(dc->irq);
+
+	failed_init = tegra_dc_init(dc);
+	if (failed_init) {
+		tegra_dc_writel(dc, 0, DC_CMD_INT_MASK);
+		disable_irq(dc->irq);
+		tegra_dc_clear_bandwidth(dc);
+		tegra_dc_clk_disable(dc);
+		if (dc->out && dc->out->disable)
+			dc->out->disable();
+		return false;
+	}
+
+	if (dc->out_ops && dc->out_ops->enable)
+		dc->out_ops->enable(dc);
+
+	/* force a full blending update */
+	dc->blend.z[0] = -1;
+
+	tegra_dc_ext_enable(dc->ext);
+
+	trace_printk("%s:enable\n", dc->ndev->name);
+
+	tegra_dc_writel(dc, GENERAL_UPDATE, DC_CMD_STATE_CONTROL);
+	tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
+
+	if (dc->out->postpoweron)
+		dc->out->postpoweron();
+
+	return true;
+}
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+static bool _tegra_dc_controller_reset_enable(struct tegra_dc *dc)
+{
+	bool ret = true;
+
+	if (dc->out->enable)
+		dc->out->enable();
+
+	tegra_dc_setup_clk(dc, dc->clk);
+	tegra_dc_clk_enable(dc);
+
+	if (dc->ndev->id == 0 && tegra_dcs[1] != NULL) {
+		mutex_lock(&tegra_dcs[1]->lock);
+		disable_irq(tegra_dcs[1]->irq);
+	} else if (dc->ndev->id == 1 && tegra_dcs[0] != NULL) {
+		mutex_lock(&tegra_dcs[0]->lock);
+		disable_irq(tegra_dcs[0]->irq);
+	}
+
+	msleep(5);
+	tegra_periph_reset_assert(dc->clk);
+	msleep(2);
+#ifdef CONFIG_TEGRA_SILICON_PLATFORM
+	tegra_periph_reset_deassert(dc->clk);
+	msleep(1);
+#endif
+
+	if (dc->ndev->id == 0 && tegra_dcs[1] != NULL) {
+		enable_dc_irq(tegra_dcs[1]->irq);
+		mutex_unlock(&tegra_dcs[1]->lock);
+	} else if (dc->ndev->id == 1 && tegra_dcs[0] != NULL) {
+		enable_dc_irq(tegra_dcs[0]->irq);
+		mutex_unlock(&tegra_dcs[0]->lock);
+	}
+
+	enable_dc_irq(dc->irq);
+
+	if (tegra_dc_init(dc)) {
+		dev_err(&dc->ndev->dev, "cannot initialize\n");
+		ret = false;
+	}
+
+	if (dc->out_ops && dc->out_ops->enable)
+		dc->out_ops->enable(dc);
+
+	if (dc->out->postpoweron)
+		dc->out->postpoweron();
+
+	/* force a full blending update */
+	dc->blend.z[0] = -1;
+
+	tegra_dc_ext_enable(dc->ext);
+
+	if (!ret) {
+		dev_err(&dc->ndev->dev, "initialization failed,disabling");
+		_tegra_dc_controller_disable(dc);
+	}
+
+	trace_printk("%s:reset enable\n", dc->ndev->name);
+	return ret;
+}
+#endif
+
+static bool _tegra_dc_enable(struct tegra_dc *dc)
+{
+	if (dc->mode.pclk == 0)
+		return false;
+
+	if (!dc->out)
+		return false;
+
+	tegra_dc_io_start(dc);
+
+	if (!_tegra_dc_controller_enable(dc)) {
+		tegra_dc_io_end(dc);
+		return false;
+	}
+	return true;
+}
+
+void tegra_dc_enable(struct tegra_dc *dc)
+{
+	mutex_lock(&dc->lock);
+
+	if (!dc->enabled)
+		dc->enabled = _tegra_dc_enable(dc);
+
+	mutex_unlock(&dc->lock);
+	print_mode_info(dc, dc->mode);
+}
+
+static void _tegra_dc_controller_disable(struct tegra_dc *dc)
+{
+	unsigned i;
+
+	if (dc->out && dc->out->prepoweroff)
+		dc->out->prepoweroff();
+
+	if (dc->out_ops && dc->out_ops->disable)
+		dc->out_ops->disable(dc);
+
+	tegra_dc_writel(dc, 0, DC_CMD_INT_MASK);
+	tegra_dc_writel(dc, 0, DC_CMD_INT_ENABLE);
+	disable_irq(dc->irq);
+
+	tegra_dc_clear_bandwidth(dc);
+	tegra_dc_clk_disable(dc);
+
+	if (dc->out && dc->out->disable)
+		dc->out->disable();
+
+	for (i = 0; i < dc->n_windows; i++) {
+		struct tegra_dc_win *w = &dc->windows[i];
+
+		/* reset window bandwidth */
+		w->bandwidth = 0;
+		w->new_bandwidth = 0;
+
+		/* disable windows */
+		w->flags &= ~TEGRA_WIN_FLAG_ENABLED;
+
+		/* flush any pending syncpt waits */
+		while (dc->syncpt[i].min < dc->syncpt[i].max) {
+			trace_printk("%s:syncpt flush id=%d\n", dc->ndev->name,
+				dc->syncpt[i].id);
+			dc->syncpt[i].min++;
+			nvhost_syncpt_cpu_incr_ext(dc->ndev, dc->syncpt[i].id);
+		}
+	}
+	trace_printk("%s:disabled\n", dc->ndev->name);
+}
+
+void tegra_dc_stats_enable(struct tegra_dc *dc, bool enable)
+{
+#if 0 /* underflow interrupt is already enabled by dc reset worker */
+	u32 val;
+	if (dc->enabled)  {
+		val = tegra_dc_readl(dc, DC_CMD_INT_ENABLE);
+		if (enable)
+			val |= (WIN_A_UF_INT | WIN_B_UF_INT | WIN_C_UF_INT);
+		else
+			val &= ~(WIN_A_UF_INT | WIN_B_UF_INT | WIN_C_UF_INT);
+		tegra_dc_writel(dc, val, DC_CMD_INT_ENABLE);
+	}
+#endif
+}
+
+bool tegra_dc_stats_get(struct tegra_dc *dc)
+{
+#if 0 /* right now it is always enabled */
+	u32 val;
+	bool res;
+
+	if (dc->enabled)  {
+		val = tegra_dc_readl(dc, DC_CMD_INT_ENABLE);
+		res = !!(val & (WIN_A_UF_INT | WIN_B_UF_INT | WIN_C_UF_INT));
+	} else {
+		res = false;
+	}
+
+	return res;
+#endif
+	return true;
+}
+
+/* make the screen blank by disabling all windows */
+void tegra_dc_blank(struct tegra_dc *dc)
+{
+	struct tegra_dc_win *dcwins[DC_N_WINDOWS];
+	unsigned i;
+
+	for (i = 0; i < DC_N_WINDOWS; i++) {
+		dcwins[i] = tegra_dc_get_window(dc, i);
+		dcwins[i]->flags &= ~TEGRA_WIN_FLAG_ENABLED;
+	}
+
+	tegra_dc_update_windows(dcwins, DC_N_WINDOWS);
+	tegra_dc_sync_windows(dcwins, DC_N_WINDOWS);
+}
+
+static void _tegra_dc_disable(struct tegra_dc *dc)
+{
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE) {
+		mutex_lock(&dc->one_shot_lock);
+		cancel_delayed_work_sync(&dc->one_shot_work);
+	}
+
+	tegra_dc_hold_dc_out(dc);
+
+	_tegra_dc_controller_disable(dc);
+	tegra_dc_io_end(dc);
+
+	tegra_dc_release_dc_out(dc);
+
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
+		mutex_unlock(&dc->one_shot_lock);
+}
+
+void tegra_dc_disable(struct tegra_dc *dc)
+{
+	tegra_dc_ext_disable(dc->ext);
+
+	/* it's important that new underflow work isn't scheduled before the
+	 * lock is acquired. */
+	cancel_delayed_work_sync(&dc->underflow_work);
+
+	mutex_lock(&dc->lock);
+
+	if (dc->enabled) {
+		dc->enabled = false;
+
+		if (!dc->suspended)
+			_tegra_dc_disable(dc);
+	}
+
+#ifdef CONFIG_SWITCH
+	switch_set_state(&dc->modeset_switch, 0);
+#endif
+
+	mutex_unlock(&dc->lock);
+	print_mode_info(dc, dc->mode);
+}
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+static void tegra_dc_reset_worker(struct work_struct *work)
+{
+	struct tegra_dc *dc =
+		container_of(work, struct tegra_dc, reset_work);
+
+	unsigned long val = 0;
+
+	mutex_lock(&shared_lock);
+
+	dev_warn(&dc->ndev->dev,
+		"overlay stuck in underflow state.  resetting.\n");
+
+	tegra_dc_ext_disable(dc->ext);
+
+	mutex_lock(&dc->lock);
+
+	if (dc->enabled == false)
+		goto unlock;
+
+	dc->enabled = false;
+
+	/*
+	 * off host read bus
+	 */
+	val = tegra_dc_readl(dc, DC_CMD_CONT_SYNCPT_VSYNC);
+	val &= ~(0x00000100);
+	tegra_dc_writel(dc, val, DC_CMD_CONT_SYNCPT_VSYNC);
+
+	/*
+	 * set DC to STOP mode
+	 */
+	tegra_dc_writel(dc, DISP_CTRL_MODE_STOP, DC_CMD_DISPLAY_COMMAND);
+
+	msleep(10);
+
+	_tegra_dc_controller_disable(dc);
+
+	/* _tegra_dc_controller_reset_enable deasserts reset */
+	_tegra_dc_controller_reset_enable(dc);
+
+	dc->enabled = true;
+
+	/* reopen host read bus */
+	val = tegra_dc_readl(dc, DC_CMD_CONT_SYNCPT_VSYNC);
+	val &= ~(0x00000100);
+	val |= 0x100;
+	tegra_dc_writel(dc, val, DC_CMD_CONT_SYNCPT_VSYNC);
+
+unlock:
+	mutex_unlock(&dc->lock);
+	mutex_unlock(&shared_lock);
+	trace_printk("%s:reset complete\n", dc->ndev->name);
+}
+#endif
+
+static void tegra_dc_underflow_worker(struct work_struct *work)
+{
+	struct tegra_dc *dc = container_of(
+		to_delayed_work(work), struct tegra_dc, underflow_work);
+
+	mutex_lock(&dc->lock);
+	tegra_dc_hold_dc_out(dc);
+
+	if (dc->enabled) {
+		tegra_dc_underflow_handler(dc);
+	}
+	tegra_dc_release_dc_out(dc);
+	mutex_unlock(&dc->lock);
+}
+
+#ifdef CONFIG_SWITCH
+static ssize_t switch_modeset_print_mode(struct switch_dev *sdev, char *buf)
+{
+	struct tegra_dc *dc =
+		container_of(sdev, struct tegra_dc, modeset_switch);
+
+	if (!sdev->state)
+		return sprintf(buf, "offline\n");
+
+	return sprintf(buf, "%dx%d\n", dc->mode.h_active, dc->mode.v_active);
+}
+#endif
+
+static int tegra_dc_probe(struct nvhost_device *ndev,
+	struct nvhost_device_id *id_table)
+{
+	struct tegra_dc *dc;
+	struct tegra_dc_mode *mode;
+	struct clk *clk;
+	struct clk *emc_clk;
+	struct resource	*res;
+	struct resource *base_res;
+	struct resource *fb_mem = NULL;
+	int ret = 0;
+	void __iomem *base;
+	char *option = NULL;
+	char driver[10];
+	int irq;
+	int i;
+
+	/* try to use kernel cmd line specified mode */
+	sprintf(driver, "tegrafb%d", ndev->id);
+	fb_get_options(driver, &option);
+
+	if (option != NULL && !strcmp(option, "off"))
+		return -ENOENT;
+
+	if (!ndev->dev.platform_data) {
+		dev_err(&ndev->dev, "no platform data\n");
+		return -ENOENT;
+	}
+
+	dc = kzalloc(sizeof(struct tegra_dc), GFP_KERNEL);
+	if (!dc) {
+		dev_err(&ndev->dev, "can't allocate memory for tegra_dc\n");
+		return -ENOMEM;
+	}
+
+	irq = nvhost_get_irq_byname(ndev, "irq");
+	if (irq <= 0) {
+		dev_err(&ndev->dev, "no irq\n");
+		ret = -ENOENT;
+		goto err_free;
+	}
+
+	res = nvhost_get_resource_byname(ndev, IORESOURCE_MEM, "regs");
+	if (!res) {
+		dev_err(&ndev->dev, "no mem resource\n");
+		ret = -ENOENT;
+		goto err_free;
+	}
+
+	base_res = request_mem_region(res->start, resource_size(res),
+		ndev->name);
+	if (!base_res) {
+		dev_err(&ndev->dev, "request_mem_region failed\n");
+		ret = -EBUSY;
+		goto err_free;
+	}
+
+	base = ioremap(res->start, resource_size(res));
+	if (!base) {
+		dev_err(&ndev->dev, "registers can't be mapped\n");
+		ret = -EBUSY;
+		goto err_release_resource_reg;
+	}
+
+	fb_mem = nvhost_get_resource_byname(ndev, IORESOURCE_MEM, "fbmem");
+
+	clk = clk_get(&ndev->dev, NULL);
+	if (IS_ERR_OR_NULL(clk)) {
+		dev_err(&ndev->dev, "can't get clock\n");
+		ret = -ENOENT;
+		goto err_iounmap_reg;
+	}
+
+	emc_clk = clk_get(&ndev->dev, "emc");
+	if (IS_ERR_OR_NULL(emc_clk)) {
+		dev_err(&ndev->dev, "can't get emc clock\n");
+		ret = -ENOENT;
+		goto err_put_clk;
+	}
+
+	dc->clk = clk;
+	dc->emc_clk = emc_clk;
+	dc->shift_clk_div = 1;
+	/* Initialize one shot work delay, it will be assigned by dsi
+	 * according to refresh rate later. */
+	dc->one_shot_delay_ms = 40;
+
+	dc->base_res = base_res;
+	dc->base = base;
+	dc->irq = irq;
+	dc->ndev = ndev;
+	dc->pdata = ndev->dev.platform_data;
+
+	/*
+	 * The emc is a shared clock, it will be set based on
+	 * the requirements for each user on the bus.
+	 */
+	dc->emc_clk_rate = 0;
+
+	mutex_init(&dc->lock);
+	mutex_init(&dc->one_shot_lock);
+	init_completion(&dc->frame_end_complete);
+	init_waitqueue_head(&dc->wq);
+#ifndef CONFIG_ANDROID
+	init_waitqueue_head(&dc->timestamp_wq);
+#endif /* !CONFIG_ANDROID */
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	INIT_WORK(&dc->reset_work, tegra_dc_reset_worker);
+#endif
+	INIT_WORK(&dc->vblank_work, tegra_dc_vblank);
+	dc->vblank_ref_count = 0;
+	INIT_DELAYED_WORK(&dc->underflow_work, tegra_dc_underflow_worker);
+	INIT_DELAYED_WORK(&dc->one_shot_work, tegra_dc_one_shot_worker);
+
+	tegra_dc_init_lut_defaults(&dc->fb_lut);
+
+	dc->n_windows = DC_N_WINDOWS;
+	for (i = 0; i < dc->n_windows; i++) {
+		struct tegra_dc_win *win = &dc->windows[i];
+		win->idx = i;
+		win->dc = dc;
+		tegra_dc_init_csc_defaults(&win->csc);
+		tegra_dc_init_lut_defaults(&win->lut);
+	}
+
+	ret = tegra_dc_set(dc, ndev->id);
+	if (ret < 0) {
+		dev_err(&ndev->dev, "can't add dc\n");
+		goto err_free_irq;
+	}
+
+	nvhost_set_drvdata(ndev, dc);
+
+#ifdef CONFIG_SWITCH
+	dc->modeset_switch.name = dev_name(&ndev->dev);
+	dc->modeset_switch.state = 0;
+	dc->modeset_switch.print_state = switch_modeset_print_mode;
+	switch_dev_register(&dc->modeset_switch);
+#endif
+
+	tegra_dc_feature_register(dc);
+
+	if (dc->pdata->default_out)
+		tegra_dc_set_out(dc, dc->pdata->default_out);
+	else
+		dev_err(&ndev->dev, "No default output specified.  Leaving output disabled.\n");
+
+	dc->vblank_syncpt = (dc->ndev->id == 0) ?
+		NVSYNCPT_VBLANK0 : NVSYNCPT_VBLANK1;
+
+	dc->ext = tegra_dc_ext_register(ndev, dc);
+	if (IS_ERR_OR_NULL(dc->ext)) {
+		dev_warn(&ndev->dev, "Failed to enable Tegra DC extensions.\n");
+		dc->ext = NULL;
+	}
+
+	/* interrupt handler must be registered before tegra_fb_register() */
+	if (request_irq(irq, tegra_dc_irq, 0,
+			dev_name(&ndev->dev), dc)) {
+		dev_err(&ndev->dev, "request_irq %d failed\n", irq);
+		ret = -EBUSY;
+		goto err_put_emc_clk;
+	}
+	disable_dc_irq(irq);
+
+	mutex_lock(&dc->lock);
+	if (dc->pdata->flags & TEGRA_DC_FLAG_ENABLED)
+		dc->enabled = _tegra_dc_enable(dc);
+	mutex_unlock(&dc->lock);
+
+	tegra_dc_create_debugfs(dc);
+
+	dev_info(&ndev->dev, "probed\n");
+
+	if (dc->pdata->fb) {
+		if (dc->enabled && dc->pdata->fb->bits_per_pixel == -1) {
+			unsigned long fmt;
+			tegra_dc_writel(dc,
+					WINDOW_A_SELECT << dc->pdata->fb->win,
+					DC_CMD_DISPLAY_WINDOW_HEADER);
+
+			fmt = tegra_dc_readl(dc, DC_WIN_COLOR_DEPTH);
+			dc->pdata->fb->bits_per_pixel =
+				tegra_dc_fmt_bpp(fmt);
+		}
+
+		mode = tegra_dc_get_override_mode(dc);
+		if (mode) {
+			dc->pdata->fb->xres = mode->h_active;
+			dc->pdata->fb->yres = mode->v_active;
+		}
+
+		dc->fb = tegra_fb_register(ndev, dc, dc->pdata->fb, fb_mem);
+
+		if (IS_ERR_OR_NULL(dc->fb))
+			dc->fb = NULL;
+	}
+
+	if (dc->out && dc->out->hotplug_init)
+		dc->out->hotplug_init();
+
+	if (dc->out_ops && dc->out_ops->detect)
+		dc->out_ops->detect(dc);
+	else
+		dc->connected = true;
+
+	tegra_dc_create_sysfs(&dc->ndev->dev);
+
+	return 0;
+
+err_free_irq:
+	free_irq(irq, dc);
+err_put_emc_clk:
+	clk_put(emc_clk);
+err_put_clk:
+	clk_put(clk);
+err_iounmap_reg:
+	iounmap(base);
+	if (fb_mem)
+		release_resource(fb_mem);
+err_release_resource_reg:
+	release_resource(base_res);
+err_free:
+	kfree(dc);
+
+	return ret;
+}
+
+static int tegra_dc_remove(struct nvhost_device *ndev)
+{
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+
+	tegra_dc_remove_sysfs(&dc->ndev->dev);
+	tegra_dc_remove_debugfs(dc);
+
+	if (dc->fb) {
+		tegra_fb_unregister(dc->fb);
+		if (dc->fb_mem)
+			release_resource(dc->fb_mem);
+	}
+
+	tegra_dc_ext_disable(dc->ext);
+
+	if (dc->ext)
+		tegra_dc_ext_unregister(dc->ext);
+
+	if (dc->enabled)
+		_tegra_dc_disable(dc);
+
+#ifdef CONFIG_SWITCH
+	switch_dev_unregister(&dc->modeset_switch);
+#endif
+	free_irq(dc->irq, dc);
+	clk_put(dc->emc_clk);
+	clk_put(dc->clk);
+	iounmap(dc->base);
+	if (dc->fb_mem)
+		release_resource(dc->base_res);
+	kfree(dc);
+	tegra_dc_set(NULL, ndev->id);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int tegra_dc_suspend(struct nvhost_device *ndev, pm_message_t state)
+{
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+
+	trace_printk("%s:suspend\n", dc->ndev->name);
+	dev_info(&ndev->dev, "suspend\n");
+
+	tegra_dc_ext_disable(dc->ext);
+
+	mutex_lock(&dc->lock);
+
+	if (dc->out_ops && dc->out_ops->suspend)
+		dc->out_ops->suspend(dc);
+
+	if (dc->enabled) {
+		_tegra_dc_disable(dc);
+
+		dc->suspended = true;
+	}
+
+	if (dc->out && dc->out->postsuspend) {
+		dc->out->postsuspend();
+		if (dc->out->type && dc->out->type == TEGRA_DC_OUT_HDMI)
+			/*
+			 * avoid resume event due to voltage falling
+			 */
+			msleep(100);
+	}
+
+	mutex_unlock(&dc->lock);
+
+	return 0;
+}
+
+static int tegra_dc_resume(struct nvhost_device *ndev)
+{
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+
+	trace_printk("%s:resume\n", dc->ndev->name);
+	dev_info(&ndev->dev, "resume\n");
+
+	mutex_lock(&dc->lock);
+	dc->suspended = false;
+
+	if (dc->enabled)
+		_tegra_dc_enable(dc);
+
+	if (dc->out && dc->out->hotplug_init)
+		dc->out->hotplug_init();
+
+	if (dc->out_ops && dc->out_ops->resume)
+		dc->out_ops->resume(dc);
+	mutex_unlock(&dc->lock);
+
+	return 0;
+}
+
+#endif /* CONFIG_PM */
+
+static void tegra_dc_shutdown(struct nvhost_device *ndev)
+{
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+
+	if (!dc || !dc->enabled)
+		return;
+
+	tegra_dc_blank(dc);
+	tegra_dc_disable(dc);
+}
+
+extern int suspend_set(const char *val, struct kernel_param *kp)
+{
+	if (!strcmp(val, "dump"))
+		dump_regs(tegra_dcs[0]);
+#ifdef CONFIG_PM
+	else if (!strcmp(val, "suspend"))
+		tegra_dc_suspend(tegra_dcs[0]->ndev, PMSG_SUSPEND);
+	else if (!strcmp(val, "resume"))
+		tegra_dc_resume(tegra_dcs[0]->ndev);
+#endif
+
+	return 0;
+}
+
+extern int suspend_get(char *buffer, struct kernel_param *kp)
+{
+	return 0;
+}
+
+int suspend;
+
+module_param_call(suspend, suspend_set, suspend_get, &suspend, 0644);
+
+struct nvhost_driver tegra_dc_driver = {
+	.driver = {
+		.name = "tegradc",
+		.owner = THIS_MODULE,
+	},
+	.probe = tegra_dc_probe,
+	.remove = tegra_dc_remove,
+#ifdef CONFIG_PM
+	.suspend = tegra_dc_suspend,
+	.resume = tegra_dc_resume,
+#endif
+	.shutdown = tegra_dc_shutdown,
+};
+
+#ifndef MODULE
+static int __init parse_disp_params(char *options, struct tegra_dc_mode *mode)
+{
+	int i, params[11];
+	char *p;
+
+	for (i = 0; i < ARRAY_SIZE(params); i++) {
+		if ((p = strsep(&options, ",")) != NULL) {
+			if (*p)
+				params[i] = simple_strtoul(p, &p, 10);
+		} else
+			return -EINVAL;
+	}
+
+	if ((mode->pclk = params[0]) == 0)
+		return -EINVAL;
+
+	mode->h_active      = params[1];
+	mode->v_active      = params[2];
+	mode->h_ref_to_sync = params[3];
+	mode->v_ref_to_sync = params[4];
+	mode->h_sync_width  = params[5];
+	mode->v_sync_width  = params[6];
+	mode->h_back_porch  = params[7];
+	mode->v_back_porch  = params[8];
+	mode->h_front_porch = params[9];
+	mode->v_front_porch = params[10];
+
+	return 0;
+}
+
+static int __init tegra_dc_mode_override(char *str)
+{
+	char *p = str, *options;
+
+	if (!p || !*p)
+		return -EINVAL;
+
+	p = strstr(str, "hdmi:");
+	if (p) {
+		p += 5;
+		options = strsep(&p, ";");
+		if (parse_disp_params(options, &override_disp_mode[TEGRA_DC_OUT_HDMI]))
+			return -EINVAL;
+	}
+
+	p = strstr(str, "rgb:");
+	if (p) {
+		p += 4;
+		options = strsep(&p, ";");
+		if (parse_disp_params(options, &override_disp_mode[TEGRA_DC_OUT_RGB]))
+			return -EINVAL;
+	}
+
+	p = strstr(str, "dsi:");
+	if (p) {
+		p += 4;
+		options = strsep(&p, ";");
+		if (parse_disp_params(options, &override_disp_mode[TEGRA_DC_OUT_DSI]))
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
+__setup("disp_params=", tegra_dc_mode_override);
+#endif
+
+static int __init tegra_dc_module_init(void)
+{
+	int ret = tegra_dc_ext_module_init();
+	if (ret)
+		return ret;
+	return nvhost_driver_register(&tegra_dc_driver);
+}
+
+static void __exit tegra_dc_module_exit(void)
+{
+	nvhost_driver_unregister(&tegra_dc_driver);
+	tegra_dc_ext_module_exit();
+}
+
+module_exit(tegra_dc_module_exit);
+module_init(tegra_dc_module_init);
diff --git a/drivers/video/tegra/dc/dc_config.c b/drivers/video/tegra/dc/dc_config.c
new file mode 100644
index 000000000000..90f505bba28a
--- /dev/null
+++ b/drivers/video/tegra/dc/dc_config.c
@@ -0,0 +1,247 @@
+/*
+ * drivers/video/tegra/dc/dc_config.c
+ *
+ * Copyright (c) 2012, NVIDIA 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "dc_config.h"
+
+static struct tegra_dc_feature_entry t20_feature_entries_a[] = {
+	{ 0, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_A,} },
+	{ 0, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
+	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
+	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
+	{ 0, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 0,} },
+	{ 0, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
+	{ 0, TEGRA_DC_FEATURE_INVERT_TYPE, {1, 1, 0,} },
+
+	{ 1, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_B,} },
+	{ 1, TEGRA_DC_FEATURE_PREFERRED_FORMATS, {TEGRA_WIN_PREF_FMT_WIN_B,} },
+	{ 1, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
+	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
+	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
+	{ 1, TEGRA_DC_FEATURE_FILTER_TYPE, {1, 1,} },
+	{ 1, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
+	{ 1, TEGRA_DC_FEATURE_INVERT_TYPE, {1, 1, 0,} },
+
+	{ 2, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_C,} },
+	{ 2, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
+	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
+	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
+	{ 2, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 1,} },
+	{ 2, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
+	{ 2, TEGRA_DC_FEATURE_INVERT_TYPE, {1, 1, 0,} },
+};
+
+static struct tegra_dc_feature_entry t20_feature_entries_b[] = {
+	{ 0, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_A,} },
+	{ 0, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
+	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
+	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
+	{ 0, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 0,} },
+	{ 0, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
+	{ 0, TEGRA_DC_FEATURE_INVERT_TYPE, {1, 1, 0,} },
+
+	{ 1, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_B,} },
+	{ 1, TEGRA_DC_FEATURE_PREFERRED_FORMATS, {TEGRA_WIN_PREF_FMT_WIN_B,} },
+	{ 1, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
+	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
+	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
+	{ 1, TEGRA_DC_FEATURE_FILTER_TYPE, {1, 1,} },
+	{ 1, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
+	{ 1, TEGRA_DC_FEATURE_INVERT_TYPE, {1, 1, 0,} },
+
+	{ 2, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_C,} },
+	{ 2, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
+	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
+	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
+	{ 2, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 1,} },
+	{ 2, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
+	{ 2, TEGRA_DC_FEATURE_INVERT_TYPE, {1, 1, 0,} },
+};
+
+struct tegra_dc_feature t20_feature_table_a = {
+	ARRAY_SIZE(t20_feature_entries_a), t20_feature_entries_a,
+};
+
+struct tegra_dc_feature t20_feature_table_b = {
+	ARRAY_SIZE(t20_feature_entries_b), t20_feature_entries_b,
+};
+
+static struct tegra_dc_feature_entry t30_feature_entries_a[] = {
+	{ 0, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_A,} },
+	{ 0, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
+	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
+	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
+	{ 0, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 0,} },
+	{ 0, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1} },
+	{ 0, TEGRA_DC_FEATURE_INVERT_TYPE, {1, 1, 0,} },
+
+	{ 1, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_B,} },
+	{ 1, TEGRA_DC_FEATURE_PREFERRED_FORMATS, {TEGRA_WIN_PREF_FMT_WIN_B,} },
+	{ 1, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
+	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
+	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
+	{ 1, TEGRA_DC_FEATURE_FILTER_TYPE, {1, 1,} },
+	{ 1, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1} },
+	{ 1, TEGRA_DC_FEATURE_INVERT_TYPE, {1, 1, 0,} },
+
+	{ 2, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_C,} },
+	{ 2, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
+	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
+	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
+	{ 2, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 1,} },
+	{ 2, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1} },
+	{ 2, TEGRA_DC_FEATURE_INVERT_TYPE, {1, 1, 0,} },
+};
+
+static struct tegra_dc_feature_entry t30_feature_entries_b[] = {
+	{ 0, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_A,} },
+	{ 0, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
+	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
+	{ 0, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
+	{ 0, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 0,} },
+	{ 0, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
+	{ 0, TEGRA_DC_FEATURE_INVERT_TYPE, {1, 1, 0,} },
+
+	{ 1, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_B,} },
+	{ 1, TEGRA_DC_FEATURE_PREFERRED_FORMATS, {TEGRA_WIN_PREF_FMT_WIN_B,} },
+	{ 1, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
+	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
+	{ 1, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
+	{ 1, TEGRA_DC_FEATURE_FILTER_TYPE, {1, 1,} },
+	{ 1, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
+	{ 1, TEGRA_DC_FEATURE_INVERT_TYPE, {1, 1, 0,} },
+
+	{ 2, TEGRA_DC_FEATURE_FORMATS, {TEGRA_WIN_FMT_WIN_C,} },
+	{ 2, TEGRA_DC_FEATURE_BLEND_TYPE, {1,} },
+	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SIZE, {4095, 1, 4095, 1,} },
+	{ 2, TEGRA_DC_FEATURE_MAXIMUM_SCALE, {2, 2, 2, 2,} },
+	{ 2, TEGRA_DC_FEATURE_FILTER_TYPE, {0, 1,} },
+	{ 2, TEGRA_DC_FEATURE_LAYOUT_TYPE, {1, 1,} },
+	{ 2, TEGRA_DC_FEATURE_INVERT_TYPE, {1, 1, 0,} },
+};
+
+struct tegra_dc_feature t30_feature_table_a = {
+	ARRAY_SIZE(t30_feature_entries_a), t30_feature_entries_a,
+};
+
+struct tegra_dc_feature t30_feature_table_b = {
+	ARRAY_SIZE(t30_feature_entries_b), t30_feature_entries_b,
+};
+
+int tegra_dc_get_feature(struct tegra_dc_feature *feature, int win_idx,
+					enum tegra_dc_feature_option option)
+{
+	int i;
+	struct tegra_dc_feature_entry *entry;
+
+	if (!feature)
+		return -EINVAL;
+
+	for (i = 0; i < feature->num_entries; i++) {
+		entry = &feature->entries[i];
+		if (entry->window_index == win_idx && entry->option == option)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+long *tegra_dc_parse_feature(struct tegra_dc *dc, int win_idx, int operation)
+{
+	int idx;
+	struct tegra_dc_feature_entry *entry;
+	enum tegra_dc_feature_option option;
+	struct tegra_dc_feature *feature = dc->feature;
+
+	switch (operation) {
+	case GET_WIN_FORMATS:
+		option = TEGRA_DC_FEATURE_FORMATS;
+		break;
+	case GET_WIN_SIZE:
+		option = TEGRA_DC_FEATURE_MAXIMUM_SIZE;
+		break;
+	case HAS_SCALE:
+		option = TEGRA_DC_FEATURE_MAXIMUM_SCALE;
+		break;
+	case HAS_TILED:
+		option = TEGRA_DC_FEATURE_LAYOUT_TYPE;
+		break;
+	case HAS_V_FILTER:
+		option = TEGRA_DC_FEATURE_FILTER_TYPE;
+		break;
+	case HAS_H_FILTER:
+		option = TEGRA_DC_FEATURE_FILTER_TYPE;
+		break;
+	case HAS_GEN2_BLEND:
+		option = TEGRA_DC_FEATURE_BLEND_TYPE;
+		break;
+	default:
+		return NULL;
+	}
+
+	idx = tegra_dc_get_feature(feature, win_idx, option);
+	if (IS_ERR_VALUE(idx))
+		return NULL;
+	entry = &feature->entries[idx];
+
+	return entry->arg;
+}
+
+int tegra_dc_feature_has_scaling(struct tegra_dc *dc, int win_idx)
+{
+	int i;
+	long *addr = tegra_dc_parse_feature(dc, win_idx, HAS_SCALE);
+
+	for (i = 0; i < ENTRY_SIZE; i++)
+		if (addr[i] != 1)
+			return 1;
+	return 0;
+}
+
+int tegra_dc_feature_has_tiling(struct tegra_dc *dc, int win_idx)
+{
+	long *addr = tegra_dc_parse_feature(dc, win_idx, HAS_TILED);
+
+	return addr[TILED_LAYOUT];
+}
+
+int tegra_dc_feature_has_filter(struct tegra_dc *dc, int win_idx, int operation)
+{
+	long *addr = tegra_dc_parse_feature(dc, win_idx, operation);
+
+	if (operation == HAS_V_FILTER)
+		return addr[V_FILTER];
+	else
+		return addr[H_FILTER];
+}
+
+void tegra_dc_feature_register(struct tegra_dc *dc)
+{
+#if defined(CONFIG_ARCH_TEGRA_2x_SOC)
+	if (!dc->ndev->id)
+		dc->feature = &t20_feature_table_a;
+	else
+		dc->feature = &t20_feature_table_b;
+#elif defined(CONFIG_ARCH_TEGRA_3x_SOC)
+	if (!dc->ndev->id)
+		dc->feature = &t30_feature_table_a;
+	else
+		dc->feature = &t30_feature_table_b;
+#endif
+}
diff --git a/drivers/video/tegra/dc/dc_config.h b/drivers/video/tegra/dc/dc_config.h
new file mode 100644
index 000000000000..6f5d08ccd82a
--- /dev/null
+++ b/drivers/video/tegra/dc/dc_config.h
@@ -0,0 +1,162 @@
+/*
+ * drivers/video/tegra/dc/dc_config.c
+ * Copyright (c) 2010-2012, NVIDIA 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __DRIVERS_VIDEO_TEGRA_DC_DC_CONFIG_H
+#define __DRIVERS_VIDEO_TEGRA_DC_DC_CONFIG_H
+
+#include <linux/errno.h>
+#include <mach/dc.h>
+
+#include "dc_priv.h"
+
+#define ENTRY_SIZE	4	/* Size of feature entry args */
+
+/* Define the supported formats. TEGRA_WIN_FMT_WIN_x macros are defined
+ * based on T20/T30 formats. */
+#define TEGRA_WIN_FMT_BASE_CNT	(TEGRA_WIN_FMT_YUV422RA + 1)
+#define TEGRA_WIN_FMT_BASE	((1 << TEGRA_WIN_FMT_P8) | \
+				(1 << TEGRA_WIN_FMT_B4G4R4A4) | \
+				(1 << TEGRA_WIN_FMT_B5G5R5A) | \
+				(1 << TEGRA_WIN_FMT_B5G6R5) | \
+				(1 << TEGRA_WIN_FMT_AB5G5R5) | \
+				(1 << TEGRA_WIN_FMT_B8G8R8A8) | \
+				(1 << TEGRA_WIN_FMT_R8G8B8A8) | \
+				(1 << TEGRA_WIN_FMT_YCbCr422) | \
+				(1 << TEGRA_WIN_FMT_YUV422) | \
+				(1 << TEGRA_WIN_FMT_YCbCr420P) | \
+				(1 << TEGRA_WIN_FMT_YUV420P) | \
+				(1 << TEGRA_WIN_FMT_YCbCr422P) | \
+				(1 << TEGRA_WIN_FMT_YUV422P) | \
+				(1 << TEGRA_WIN_FMT_YCbCr422R) | \
+				(1 << TEGRA_WIN_FMT_YUV422R))
+
+#define TEGRA_WIN_FMT_WIN_A	((1 << TEGRA_WIN_FMT_P1) | \
+				(1 << TEGRA_WIN_FMT_P2) | \
+				(1 << TEGRA_WIN_FMT_P4) | \
+				(1 << TEGRA_WIN_FMT_P8) | \
+				(1 << TEGRA_WIN_FMT_B4G4R4A4) | \
+				(1 << TEGRA_WIN_FMT_B5G5R5A) | \
+				(1 << TEGRA_WIN_FMT_B5G6R5) | \
+				(1 << TEGRA_WIN_FMT_AB5G5R5) | \
+				(1 << TEGRA_WIN_FMT_B8G8R8A8) | \
+				(1 << TEGRA_WIN_FMT_R8G8B8A8) | \
+				(1 << TEGRA_WIN_FMT_B6x2G6x2R6x2A8) | \
+				(1 << TEGRA_WIN_FMT_R6x2G6x2B6x2A8))
+
+#define TEGRA_WIN_FMT_WIN_B	(TEGRA_WIN_FMT_BASE | \
+				(1 << TEGRA_WIN_FMT_B6x2G6x2R6x2A8) | \
+				(1 << TEGRA_WIN_FMT_R6x2G6x2B6x2A8) | \
+				(1 << TEGRA_WIN_FMT_YCbCr422RA) | \
+				(1 << TEGRA_WIN_FMT_YUV422RA))
+
+#define TEGRA_WIN_FMT_WIN_C	(TEGRA_WIN_FMT_BASE | \
+				(1 << TEGRA_WIN_FMT_B6x2G6x2R6x2A8) | \
+				(1 << TEGRA_WIN_FMT_R6x2G6x2B6x2A8) | \
+				(1 << TEGRA_WIN_FMT_YCbCr422RA) | \
+				(1 << TEGRA_WIN_FMT_YUV422RA))
+
+/* preferred formats do not include 32-bpp formats */
+#define TEGRA_WIN_PREF_FMT_WIN_B	(TEGRA_WIN_FMT_WIN_B & \
+				~(1 << TEGRA_WIN_FMT_B8G8R8A8) & \
+				~(1 << TEGRA_WIN_FMT_R8G8B8A8))
+
+
+
+/* For each entry, we define the offset to read specific feature. Define the
+ * offset for TEGRA_DC_FEATURE_MAXIMUM_SCALE */
+#define H_SCALE_UP	0
+#define V_SCALE_UP	1
+#define H_FILTER_DOWN	2
+#define V_FILTER_DOWN	3
+
+/* Define the offset for TEGRA_DC_FEATURE_MAXIMUM_SIZE */
+#define MAX_WIDTH	0
+#define MIN_WIDTH	1
+#define MAX_HEIGHT	2
+#define MIN_HEIGHT	3
+#define CHECK_SIZE(val, min, max)	( \
+		((val) < (min) || (val) > (max)) ? -EINVAL : 0)
+
+/* Define the offset for TEGRA_DC_FEATURE_FILTER_TYPE */
+#define V_FILTER	0
+#define H_FILTER	1
+
+/* Define the offset for TEGRA_DC_FEATURE_INVERT_TYPE */
+#define H_INVERT	0
+#define V_INVERT	1
+#define SCAN_COLUMN	2
+
+/* Define the offset for TEGRA_DC_FEATURE_LAYOUT_TYPE. */
+#define PITCHED_LAYOUT	0
+#define TILED_LAYOUT	1
+
+/* Available operations on feature table. */
+enum {
+	HAS_SCALE,
+	HAS_TILED,
+	HAS_V_FILTER,
+	HAS_H_FILTER,
+	HAS_GEN2_BLEND,
+	GET_WIN_FORMATS,
+	GET_WIN_SIZE,
+};
+
+enum tegra_dc_feature_option {
+	TEGRA_DC_FEATURE_FORMATS,
+	TEGRA_DC_FEATURE_BLEND_TYPE,
+	TEGRA_DC_FEATURE_MAXIMUM_SIZE,
+	TEGRA_DC_FEATURE_MAXIMUM_SCALE,
+	TEGRA_DC_FEATURE_FILTER_TYPE,
+	TEGRA_DC_FEATURE_LAYOUT_TYPE,
+	TEGRA_DC_FEATURE_INVERT_TYPE,
+	TEGRA_DC_FEATURE_PREFERRED_FORMATS,
+};
+
+struct tegra_dc_feature_entry {
+	u32 window_index;
+	u32 option;
+	long arg[ENTRY_SIZE];
+};
+
+struct tegra_dc_feature {
+	u32 num_entries;
+	struct tegra_dc_feature_entry *entries;
+};
+
+int tegra_dc_feature_has_scaling(struct tegra_dc *dc, int win_idx);
+int tegra_dc_feature_has_tiling(struct tegra_dc *dc, int win_idx);
+int tegra_dc_feature_has_filter(struct tegra_dc *dc, int win_idx, int operation);
+
+long *tegra_dc_parse_feature(struct tegra_dc *dc, int win_idx, int operation);
+void tegra_dc_feature_register(struct tegra_dc *dc);
+
+static inline bool win_use_v_filter(struct tegra_dc *dc,
+	const struct tegra_dc_win *win)
+{
+	return tegra_dc_feature_has_filter(dc, win->idx, HAS_V_FILTER) &&
+		win->h.full != dfixed_const(win->out_h);
+}
+static inline bool win_use_h_filter(struct tegra_dc *dc,
+	const struct tegra_dc_win *win)
+{
+	return tegra_dc_feature_has_filter(dc, win->idx, HAS_H_FILTER) &&
+		win->w.full != dfixed_const(win->out_w);
+}
+
+#endif
diff --git a/drivers/video/tegra/dc/dc_priv.h b/drivers/video/tegra/dc/dc_priv.h
new file mode 100644
index 000000000000..e91071f70ddc
--- /dev/null
+++ b/drivers/video/tegra/dc/dc_priv.h
@@ -0,0 +1,419 @@
+/*
+ * drivers/video/tegra/dc/dc_priv.h
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 __DRIVERS_VIDEO_TEGRA_DC_DC_PRIV_H
+#define __DRIVERS_VIDEO_TEGRA_DC_DC_PRIV_H
+
+#include <linux/io.h>
+#include <linux/mutex.h>
+#include <linux/wait.h>
+#include <linux/fb.h>
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/switch.h>
+#include <linux/nvhost.h>
+
+#include <mach/dc.h>
+
+#include <mach/tegra_dc_ext.h>
+#include <mach/clk.h>
+
+#include "dc_reg.h"
+
+#define WIN_IS_TILED(win)	((win)->flags & TEGRA_WIN_FLAG_TILED)
+#define WIN_IS_ENABLED(win)	((win)->flags & TEGRA_WIN_FLAG_ENABLED)
+
+#define NEED_UPDATE_EMC_ON_EVERY_FRAME (windows_idle_detection_time == 0)
+
+/* DDR: 8 bytes transfer per clock */
+#define DDR_BW_TO_FREQ(bw) ((bw) / 8)
+
+#if defined(CONFIG_TEGRA_EMC_TO_DDR_CLOCK)
+#define EMC_BW_TO_FREQ(bw) (DDR_BW_TO_FREQ(bw) * CONFIG_TEGRA_EMC_TO_DDR_CLOCK)
+#else
+#define EMC_BW_TO_FREQ(bw) (DDR_BW_TO_FREQ(bw) * 2)
+#endif
+
+#ifndef CONFIG_TEGRA_FPGA_PLATFORM
+#define ALL_UF_INT (WIN_A_UF_INT | WIN_B_UF_INT | WIN_C_UF_INT)
+#else
+/* ignore underflows when on simulation and fpga platform */
+#define ALL_UF_INT (0)
+#endif
+
+struct tegra_dc;
+
+struct tegra_dc_blend {
+	unsigned z[DC_N_WINDOWS];
+	unsigned flags[DC_N_WINDOWS];
+};
+
+struct tegra_dc_out_ops {
+	/* initialize output.  dc clocks are not on at this point */
+	int (*init)(struct tegra_dc *dc);
+	/* destroy output.  dc clocks are not on at this point */
+	void (*destroy)(struct tegra_dc *dc);
+	/* detect connected display.  can sleep.*/
+	bool (*detect)(struct tegra_dc *dc);
+	/* enable output.  dc clocks are on at this point */
+	void (*enable)(struct tegra_dc *dc);
+	/* disable output.  dc clocks are on at this point */
+	void (*disable)(struct tegra_dc *dc);
+	/* hold output.  keeps dc clocks on. */
+	void (*hold)(struct tegra_dc *dc);
+	/* release output.  dc clocks may turn off after this. */
+	void (*release)(struct tegra_dc *dc);
+	/* idle routine of output.  dc clocks may turn off after this. */
+	void (*idle)(struct tegra_dc *dc);
+	/* suspend output.  dc clocks are on at this point */
+	void (*suspend)(struct tegra_dc *dc);
+	/* resume output.  dc clocks are on at this point */
+	void (*resume)(struct tegra_dc *dc);
+	/* mode filter. to provide a list of supported modes*/
+	bool (*mode_filter)(const struct tegra_dc *dc,
+			struct fb_videomode *mode);
+};
+
+struct tegra_dc {
+	struct nvhost_device		*ndev;
+	struct tegra_dc_platform_data	*pdata;
+
+	struct resource			*base_res;
+	void __iomem			*base;
+	int				irq;
+
+	struct clk			*clk;
+	struct clk			*emc_clk;
+	int				emc_clk_rate;
+	int				new_emc_clk_rate;
+	u32				shift_clk_div;
+
+	bool				connected;
+	bool				enabled;
+	bool				suspended;
+
+	struct tegra_dc_out		*out;
+	struct tegra_dc_out_ops		*out_ops;
+	void				*out_data;
+
+	struct tegra_dc_mode		mode;
+#ifndef CONFIG_ANDROID
+	s64				frametime_ns;
+#endif /* !CONFIG_ANDROID */
+
+	struct tegra_dc_win		windows[DC_N_WINDOWS];
+	struct tegra_dc_blend		blend;
+	int				n_windows;
+
+	wait_queue_head_t		wq;
+#ifndef CONFIG_ANDROID
+	wait_queue_head_t		timestamp_wq;
+#endif /* !CONFIG_ANDROID */
+
+	struct mutex			lock;
+	struct mutex			one_shot_lock;
+
+	struct resource			*fb_mem;
+	struct tegra_fb_info		*fb;
+
+	struct {
+		u32			id;
+		u32			min;
+		u32			max;
+	} syncpt[DC_N_WINDOWS];
+	u32				vblank_syncpt;
+
+	unsigned long			underflow_mask;
+	struct work_struct		reset_work;
+
+#ifdef CONFIG_SWITCH
+	struct switch_dev		modeset_switch;
+#endif
+
+	struct completion		frame_end_complete;
+
+	struct work_struct		vblank_work;
+	long				vblank_ref_count;
+
+	struct {
+		u64			underflows;
+		u64			underflows_a;
+		u64			underflows_b;
+		u64			underflows_c;
+	} stats;
+
+	struct tegra_dc_ext		*ext;
+
+	struct tegra_dc_feature		*feature;
+
+#ifdef CONFIG_DEBUG_FS
+	struct dentry			*debugdir;
+#endif
+	struct tegra_dc_lut		fb_lut;
+	struct delayed_work		underflow_work;
+	u32				one_shot_delay_ms;
+	struct delayed_work		one_shot_work;
+#ifndef CONFIG_ANDROID
+	s64				frame_end_timestamp;
+#endif /* !CONFIG_ANDROID */
+};
+
+#define print_mode_info(dc, mode) do {					\
+	trace_printk("%s:Mode settings: "				\
+			"ref_to_sync: H = %d V = %d, "			\
+			"sync_width: H = %d V = %d, "			\
+			"back_porch: H = %d V = %d, "			\
+			"active: H = %d V = %d, "			\
+			"front_porch: H = %d V = %d, "			\
+			"pclk = %d, stereo mode = %d\n",		\
+			dc->ndev->name,					\
+			mode.h_ref_to_sync, mode.v_ref_to_sync,		\
+			mode.h_sync_width, mode.v_sync_width,		\
+			mode.h_back_porch, mode.v_back_porch,		\
+			mode.h_active, mode.v_active,			\
+			mode.h_front_porch, mode.v_front_porch,		\
+			mode.pclk, mode.stereo_mode);			\
+	} while (0)
+
+static inline void tegra_dc_io_start(struct tegra_dc *dc)
+{
+	nvhost_module_busy_ext(nvhost_get_parent(dc->ndev));
+}
+
+static inline void tegra_dc_io_end(struct tegra_dc *dc)
+{
+	nvhost_module_idle_ext(nvhost_get_parent(dc->ndev));
+}
+
+static inline unsigned long tegra_dc_readl(struct tegra_dc *dc,
+					   unsigned long reg)
+{
+	unsigned long ret;
+
+	BUG_ON(!nvhost_module_powered_ext(nvhost_get_parent(dc->ndev)));
+	if (!tegra_is_clk_enabled(dc->clk))
+		WARN(1, "DC is clock-gated.\n");
+
+	ret = readl(dc->base + reg * 4);
+	trace_printk("readl %p=%#08lx\n", dc->base + reg * 4, ret);
+	return ret;
+}
+
+static inline void tegra_dc_writel(struct tegra_dc *dc, unsigned long val,
+				   unsigned long reg)
+{
+	BUG_ON(!nvhost_module_powered_ext(nvhost_get_parent(dc->ndev)));
+	if (!tegra_is_clk_enabled(dc->clk))
+		WARN(1, "DC is clock-gated.\n");
+
+	trace_printk("writel %p=%#08lx\n", dc->base + reg * 4, val);
+	writel(val, dc->base + reg * 4);
+}
+
+static inline void _tegra_dc_write_table(struct tegra_dc *dc, const u32 *table,
+					 unsigned len)
+{
+	int i;
+
+	for (i = 0; i < len; i++)
+		tegra_dc_writel(dc, table[i * 2 + 1], table[i * 2]);
+}
+
+#define tegra_dc_write_table(dc, table)		\
+	_tegra_dc_write_table(dc, table, ARRAY_SIZE(table) / 2)
+
+static inline void tegra_dc_set_outdata(struct tegra_dc *dc, void *data)
+{
+	dc->out_data = data;
+}
+
+static inline void *tegra_dc_get_outdata(struct tegra_dc *dc)
+{
+	return dc->out_data;
+}
+
+static inline unsigned long tegra_dc_get_default_emc_clk_rate(
+	struct tegra_dc *dc)
+{
+	return dc->pdata->emc_clk_rate ? dc->pdata->emc_clk_rate : ULONG_MAX;
+}
+
+static inline int tegra_dc_fmt_bpp(int fmt)
+{
+	switch (fmt) {
+	case TEGRA_WIN_FMT_P1:
+		return 1;
+
+	case TEGRA_WIN_FMT_P2:
+		return 2;
+
+	case TEGRA_WIN_FMT_P4:
+		return 4;
+
+	case TEGRA_WIN_FMT_P8:
+		return 8;
+
+	case TEGRA_WIN_FMT_B4G4R4A4:
+	case TEGRA_WIN_FMT_B5G5R5A:
+	case TEGRA_WIN_FMT_B5G6R5:
+	case TEGRA_WIN_FMT_AB5G5R5:
+		return 16;
+
+	case TEGRA_WIN_FMT_B8G8R8A8:
+	case TEGRA_WIN_FMT_R8G8B8A8:
+	case TEGRA_WIN_FMT_B6x2G6x2R6x2A8:
+	case TEGRA_WIN_FMT_R6x2G6x2B6x2A8:
+		return 32;
+
+	/* for planar formats, size of the Y plane, 8bit */
+	case TEGRA_WIN_FMT_YCbCr420P:
+	case TEGRA_WIN_FMT_YUV420P:
+	case TEGRA_WIN_FMT_YCbCr422P:
+	case TEGRA_WIN_FMT_YUV422P:
+	case TEGRA_WIN_FMT_YCbCr422R:
+	case TEGRA_WIN_FMT_YUV422R:
+	case TEGRA_WIN_FMT_YCbCr422RA:
+	case TEGRA_WIN_FMT_YUV422RA:
+		return 8;
+
+	/* YUYV packed into 32-bits */
+	case TEGRA_WIN_FMT_YCbCr422:
+	case TEGRA_WIN_FMT_YUV422:
+		return 16;
+	}
+	return 0;
+}
+
+static inline bool tegra_dc_is_yuv(int fmt)
+{
+	switch (fmt) {
+	case TEGRA_WIN_FMT_YUV420P:
+	case TEGRA_WIN_FMT_YCbCr420P:
+	case TEGRA_WIN_FMT_YCbCr422P:
+	case TEGRA_WIN_FMT_YUV422P:
+	case TEGRA_WIN_FMT_YCbCr422:
+	case TEGRA_WIN_FMT_YUV422:
+	case TEGRA_WIN_FMT_YCbCr422R:
+	case TEGRA_WIN_FMT_YUV422R:
+	case TEGRA_WIN_FMT_YCbCr422RA:
+	case TEGRA_WIN_FMT_YUV422RA:
+		return true;
+	}
+	return false;
+}
+
+static inline bool tegra_dc_is_yuv_planar(int fmt)
+{
+	switch (fmt) {
+	case TEGRA_WIN_FMT_YUV420P:
+	case TEGRA_WIN_FMT_YCbCr420P:
+	case TEGRA_WIN_FMT_YCbCr422P:
+	case TEGRA_WIN_FMT_YUV422P:
+	case TEGRA_WIN_FMT_YCbCr422R:
+	case TEGRA_WIN_FMT_YUV422R:
+	case TEGRA_WIN_FMT_YCbCr422RA:
+	case TEGRA_WIN_FMT_YUV422RA:
+		return true;
+	}
+	return false;
+}
+
+static inline void tegra_dc_unmask_interrupt(struct tegra_dc *dc, u32 int_val)
+{
+	u32 val;
+
+	val = tegra_dc_readl(dc, DC_CMD_INT_MASK);
+	val |= int_val;
+	tegra_dc_writel(dc, val, DC_CMD_INT_MASK);
+}
+
+static inline void tegra_dc_mask_interrupt(struct tegra_dc *dc, u32 int_val)
+{
+	u32 val;
+
+	val = tegra_dc_readl(dc, DC_CMD_INT_MASK);
+	val &= ~int_val;
+	tegra_dc_writel(dc, val, DC_CMD_INT_MASK);
+}
+
+static inline unsigned long tegra_dc_clk_get_rate(struct tegra_dc *dc)
+{
+#ifdef CONFIG_TEGRA_SILICON_PLATFORM
+	return clk_get_rate(dc->clk);
+#else
+	return dc->mode.pclk;
+#endif
+}
+
+extern struct tegra_dc_out_ops tegra_dc_rgb_ops;
+extern struct tegra_dc_out_ops tegra_dc_hdmi_ops;
+extern struct tegra_dc_out_ops tegra_dc_dsi_ops;
+
+/* defined in dc_sysfs.c, used by dc.c */
+void __devexit tegra_dc_remove_sysfs(struct device *dev);
+void tegra_dc_create_sysfs(struct device *dev);
+
+/* defined in dc.c, used by dc_sysfs.c */
+void tegra_dc_stats_enable(struct tegra_dc *dc, bool enable);
+bool tegra_dc_stats_get(struct tegra_dc *dc);
+
+/* defined in dc.c, used by dc_sysfs.c */
+u32 tegra_dc_read_checksum_latched(struct tegra_dc *dc);
+void tegra_dc_enable_crc(struct tegra_dc *dc);
+void tegra_dc_disable_crc(struct tegra_dc *dc);
+
+void tegra_dc_set_out_pin_polars(struct tegra_dc *dc,
+				const struct tegra_dc_out_pin *pins,
+				const unsigned int n_pins);
+/* defined in dc.c, used in bandwidth.c and ext/dev.c */
+unsigned int tegra_dc_has_multiple_dc(void);
+
+/* defined in dc.c, used in dsi.c */
+void tegra_dc_clk_enable(struct tegra_dc *dc);
+void tegra_dc_clk_disable(struct tegra_dc *dc);
+
+/* defined in dc.c, used in nvsd.c and dsi.c */
+void tegra_dc_hold_dc_out(struct tegra_dc *dc);
+void tegra_dc_release_dc_out(struct tegra_dc *dc);
+
+/* defined in bandwidth.c, used in dc.c */
+void tegra_dc_clear_bandwidth(struct tegra_dc *dc);
+void tegra_dc_program_bandwidth(struct tegra_dc *dc, bool use_new);
+int tegra_dc_set_dynamic_emc(struct tegra_dc_win *windows[], int n);
+
+/* defined in mode.c, used in dc.c */
+int tegra_dc_program_mode(struct tegra_dc *dc, struct tegra_dc_mode *mode);
+int tegra_dc_calc_refresh(const struct tegra_dc_mode *m);
+
+/* defined in clock.c, used in dc.c, dsi.c and hdmi.c */
+void tegra_dc_setup_clk(struct tegra_dc *dc, struct clk *clk);
+unsigned long tegra_dc_pclk_round_rate(struct tegra_dc *dc, int pclk);
+
+/* defined in lut.c, used in dc.c */
+void tegra_dc_init_lut_defaults(struct tegra_dc_lut *lut);
+void tegra_dc_set_lut(struct tegra_dc *dc, struct tegra_dc_win *win);
+
+/* defined in csc.c, used in dc.c */
+void tegra_dc_init_csc_defaults(struct tegra_dc_csc *csc);
+void tegra_dc_set_csc(struct tegra_dc *dc, struct tegra_dc_csc *csc);
+
+/* defined in window.c, used in dc.c */
+void tegra_dc_trigger_windows(struct tegra_dc *dc);
+
+#endif
diff --git a/drivers/video/tegra/dc/dc_reg.h b/drivers/video/tegra/dc/dc_reg.h
new file mode 100644
index 000000000000..86b1029d3bba
--- /dev/null
+++ b/drivers/video/tegra/dc/dc_reg.h
@@ -0,0 +1,564 @@
+/*
+ * drivers/video/tegra/dc/dc_reg.h
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 __DRIVERS_VIDEO_TEGRA_DC_DC_REG_H
+#define __DRIVERS_VIDEO_TEGRA_DC_DC_REG_H
+
+#define DC_CMD_GENERAL_INCR_SYNCPT		0x000
+#define DC_CMD_GENERAL_INCR_SYNCPT_CNTRL	0x001
+#define DC_CMD_GENERAL_INCR_SYNCPT_ERROR	0x002
+#define DC_CMD_WIN_A_INCR_SYNCPT		0x008
+#define DC_CMD_WIN_A_INCR_SYNCPT_CNTRL		0x009
+#define DC_CMD_WIN_A_INCR_SYNCPT_ERROR		0x00a
+#define DC_CMD_WIN_B_INCR_SYNCPT		0x010
+#define DC_CMD_WIN_B_INCR_SYNCPT_CNTRL		0x011
+#define DC_CMD_WIN_B_INCR_SYNCPT_ERROR		0x012
+#define DC_CMD_WIN_C_INCR_SYNCPT		0x018
+#define DC_CMD_WIN_C_INCR_SYNCPT_CNTRL		0x019
+#define DC_CMD_WIN_C_INCR_SYNCPT_ERROR		0x01a
+#define DC_CMD_CONT_SYNCPT_VSYNC		0x028
+#define DC_CMD_DISPLAY_COMMAND_OPTION0		0x031
+#define  MSF_POLARITY_HIGH			(0 << 0)
+#define  MSF_POLARITY_LOW			(1 << 0)
+#define  MSF_DISABLE				(0 << 1)
+#define  MSF_ENABLE				(1 << 1)
+#define  MSF_LSPI				(0 << 2)
+#define  MSF_LDC				(1 << 2)
+#define  MSF_LSDI				(2 << 2)
+
+#define DC_CMD_DISPLAY_COMMAND			0x032
+#define  DISP_COMMAND_RAISE		(1 << 0)
+#define  DISP_CTRL_MODE_STOP		(0 << 5)
+#define  DISP_CTRL_MODE_C_DISPLAY	(1 << 5)
+#define  DISP_CTRL_MODE_NC_DISPLAY	(2 << 5)
+#define  DISP_COMMAND_RAISE_VECTOR(x)	(((x) & 0x1f) << 22)
+#define  DISP_COMMAND_RAISE_CHANNEL_ID(x)	(((x) & 0xf) << 27)
+
+#define DC_CMD_SIGNAL_RAISE			0x033
+#define DC_CMD_DISPLAY_POWER_CONTROL		0x036
+#define  PW0_ENABLE		(1 << 0)
+#define  PW1_ENABLE		(1 << 2)
+#define  PW2_ENABLE		(1 << 4)
+#define  PW3_ENABLE		(1 << 6)
+#define  PW4_ENABLE		(1 << 8)
+#define  PM0_ENABLE		(1 << 16)
+#define  PM1_ENABLE		(1 << 18)
+#define  SPI_ENABLE		(1 << 24)
+#define  HSPI_ENABLE		(1 << 25)
+
+#define DC_CMD_INT_STATUS			0x037
+#define DC_CMD_INT_MASK				0x038
+#define DC_CMD_INT_ENABLE			0x039
+#define DC_CMD_INT_TYPE				0x03a
+#define DC_CMD_INT_POLARITY			0x03b
+#define  CTXSW_INT		(1 << 0)
+#define  FRAME_END_INT		(1 << 1)
+#define  V_BLANK_INT		(1 << 2)
+#define  H_BLANK_INT		(1 << 3)
+#define  V_PULSE3_INT		(1 << 4)
+#define  SPI_BUSY_INT		(1 << 7)
+#define  WIN_A_UF_INT		(1 << 8)
+#define  WIN_B_UF_INT		(1 << 9)
+#define  WIN_C_UF_INT		(1 << 10)
+#define  MSF_INT		(1 << 12)
+#define  SSF_INT		(1 << 13)
+#define  WIN_A_OF_INT		(1 << 14)
+#define  WIN_B_OF_INT		(1 << 15)
+#define  WIN_C_OF_INT		(1 << 16)
+#define  GPIO_0_INT		(1 << 18)
+#define  GPIO_1_INT		(1 << 19)
+#define  GPIO_2_INT		(1 << 20)
+
+#define DC_CMD_SIGNAL_RAISE1			0x03c
+#define DC_CMD_SIGNAL_RAISE2			0x03d
+#define DC_CMD_SIGNAL_RAISE3			0x03e
+#define DC_CMD_STATE_ACCESS			0x040
+#define  READ_MUX_ASSEMBLY	(0 << 0)
+#define  READ_MUX_ACTIVE	(1 << 0)
+#define  WRITE_MUX_ASSEMBLY	(0 << 2)
+#define  WRITE_MUX_ACTIVE	(1 << 2)
+
+#define DC_CMD_STATE_CONTROL			0x041
+#define  GENERAL_ACT_REQ	(1 << 0)
+#define  WIN_A_ACT_REQ		(1 << 1)
+#define  WIN_B_ACT_REQ		(1 << 2)
+#define  WIN_C_ACT_REQ		(1 << 3)
+#define  GENERAL_UPDATE		(1 << 8)
+#define  WIN_A_UPDATE		(1 << 9)
+#define  WIN_B_UPDATE		(1 << 10)
+#define  WIN_C_UPDATE		(1 << 11)
+#define  NC_HOST_TRIG		(1 << 24)
+
+#define DC_CMD_DISPLAY_WINDOW_HEADER		0x042
+#define  WINDOW_A_SELECT		(1 << 4)
+#define  WINDOW_B_SELECT		(1 << 5)
+#define  WINDOW_C_SELECT		(1 << 6)
+
+#define DC_CMD_REG_ACT_CONTROL			0x043
+
+#define DC_COM_CRC_CONTROL			0x300
+#define  CRC_ALWAYS_ENABLE		(1 << 3)
+#define  CRC_ALWAYS_DISABLE		(0 << 3)
+#define  CRC_INPUT_DATA_ACTIVE_DATA	(1 << 2)
+#define  CRC_INPUT_DATA_FULL_FRAME	(0 << 2)
+#define  CRC_WAIT_TWO_VSYNC		(1 << 1)
+#define  CRC_WAIT_ONE_VSYNC		(0 << 1)
+#define  CRC_ENABLE_ENABLE		(1 << 0)
+#define  CRC_ENABLE_DISABLE		(0 << 0)
+#define DC_COM_CRC_CHECKSUM			0x301
+#define DC_COM_PIN_OUTPUT_ENABLE0		0x302
+#define DC_COM_PIN_OUTPUT_ENABLE1		0x303
+#define DC_COM_PIN_OUTPUT_ENABLE2		0x304
+#define DC_COM_PIN_OUTPUT_ENABLE3		0x305
+#define  PIN_OUTPUT_LSPI_OUTPUT_EN		(1 << 8)
+#define  PIN_OUTPUT_LSPI_OUTPUT_DIS		(1 << 8)
+#define DC_COM_PIN_OUTPUT_POLARITY0		0x306
+
+#define DC_COM_PIN_OUTPUT_POLARITY1		0x307
+#define  LHS_OUTPUT_POLARITY_LOW	(1 << 30)
+#define  LVS_OUTPUT_POLARITY_LOW	(1 << 28)
+#define  LSC0_OUTPUT_POLARITY_LOW	(1 << 24)
+
+#define DC_COM_PIN_OUTPUT_POLARITY2		0x308
+
+#define DC_COM_PIN_OUTPUT_POLARITY3		0x309
+#define  LSPI_OUTPUT_POLARITY_LOW	(1 << 8)
+
+#define DC_COM_PIN_OUTPUT_DATA0			0x30a
+#define DC_COM_PIN_OUTPUT_DATA1			0x30b
+#define DC_COM_PIN_OUTPUT_DATA2			0x30c
+#define DC_COM_PIN_OUTPUT_DATA3			0x30d
+#define DC_COM_PIN_INPUT_ENABLE0		0x30e
+#define DC_COM_PIN_INPUT_ENABLE1		0x30f
+#define DC_COM_PIN_INPUT_ENABLE2		0x310
+#define DC_COM_PIN_INPUT_ENABLE3		0x311
+#define  PIN_INPUT_LSPI_INPUT_EN		(1 << 8)
+#define  PIN_INPUT_LSPI_INPUT_DIS		(1 << 8)
+#define DC_COM_PIN_INPUT_DATA0			0x312
+#define DC_COM_PIN_INPUT_DATA1			0x313
+#define DC_COM_PIN_OUTPUT_SELECT0		0x314
+#define DC_COM_PIN_OUTPUT_SELECT1		0x315
+#define DC_COM_PIN_OUTPUT_SELECT2		0x316
+#define DC_COM_PIN_OUTPUT_SELECT3		0x317
+#define DC_COM_PIN_OUTPUT_SELECT4		0x318
+#define DC_COM_PIN_OUTPUT_SELECT5		0x319
+#define DC_COM_PIN_OUTPUT_SELECT6		0x31a
+
+#define PIN5_LM1_LCD_M1_OUTPUT_MASK	(7 << 4)
+#define PIN5_LM1_LCD_M1_OUTPUT_M1	(0 << 4)
+#define PIN5_LM1_LCD_M1_OUTPUT_LD21	(2 << 4)
+#define PIN5_LM1_LCD_M1_OUTPUT_PM1	(3 << 4)
+
+#define  PIN1_LHS_OUTPUT		(1 << 30)
+#define  PIN1_LVS_OUTPUT		(1 << 28)
+
+#define DC_COM_PIN_MISC_CONTROL			0x31b
+#define DC_COM_PM0_CONTROL			0x31c
+#define DC_COM_PM0_DUTY_CYCLE			0x31d
+#define DC_COM_PM1_CONTROL			0x31e
+#define DC_COM_PM1_DUTY_CYCLE			0x31f
+
+#define PM_PERIOD_SHIFT                 18
+#define PM_CLK_DIVIDER_SHIFT		4
+
+#define DC_COM_SPI_CONTROL			0x320
+#define DC_COM_SPI_START_BYTE			0x321
+#define DC_COM_HSPI_WRITE_DATA_AB		0x322
+#define DC_COM_HSPI_WRITE_DATA_CD		0x323
+#define DC_COM_HSPI_CS_DC			0x324
+#define DC_COM_SCRATCH_REGISTER_A		0x325
+#define DC_COM_SCRATCH_REGISTER_B		0x326
+#define DC_COM_GPIO_CTRL			0x327
+#define DC_COM_GPIO_DEBOUNCE_COUNTER		0x328
+#define DC_COM_CRC_CHECKSUM_LATCHED		0x329
+
+#define DC_DISP_DISP_SIGNAL_OPTIONS0		0x400
+#define  H_PULSE_0_ENABLE		(1 << 8)
+#define  H_PULSE_1_ENABLE		(1 << 10)
+#define  H_PULSE_2_ENABLE		(1 << 12)
+#define  V_PULSE_0_ENABLE		(1 << 16)
+#define  V_PULSE_1_ENABLE		(1 << 18)
+#define  V_PULSE_2_ENABLE		(1 << 19)
+#define  V_PULSE_3_ENABLE		(1 << 20)
+#define  M0_ENABLE			(1 << 24)
+#define  M1_ENABLE			(1 << 26)
+
+#define DC_DISP_DISP_SIGNAL_OPTIONS1		0x401
+#define  DI_ENABLE			(1 << 16)
+#define  PP_ENABLE			(1 << 18)
+
+#define DC_DISP_DISP_WIN_OPTIONS		0x402
+#define  CURSOR_ENABLE			(1 << 16)
+#define  TVO_ENABLE			(1 << 28)
+#define  DSI_ENABLE			(1 << 29)
+#define  HDMI_ENABLE			(1 << 30)
+
+#define DC_DISP_MEM_HIGH_PRIORITY		0x403
+#define DC_DISP_MEM_HIGH_PRIORITY_TIMER		0x404
+#define DC_DISP_DISP_TIMING_OPTIONS		0x405
+#define  VSYNC_H_POSITION(x)		((x) & 0xfff)
+
+#define DC_DISP_REF_TO_SYNC			0x406
+#define DC_DISP_SYNC_WIDTH			0x407
+#define DC_DISP_BACK_PORCH			0x408
+#define DC_DISP_DISP_ACTIVE			0x409
+#define DC_DISP_FRONT_PORCH			0x40a
+#define DC_DISP_H_PULSE0_CONTROL		0x40b
+#define DC_DISP_H_PULSE0_POSITION_A		0x40c
+#define DC_DISP_H_PULSE0_POSITION_B		0x40d
+#define DC_DISP_H_PULSE0_POSITION_C		0x40e
+#define DC_DISP_H_PULSE0_POSITION_D		0x40f
+#define DC_DISP_H_PULSE1_CONTROL		0x410
+#define DC_DISP_H_PULSE1_POSITION_A		0x411
+#define DC_DISP_H_PULSE1_POSITION_B		0x412
+#define DC_DISP_H_PULSE1_POSITION_C		0x413
+#define DC_DISP_H_PULSE1_POSITION_D		0x414
+#define DC_DISP_H_PULSE2_CONTROL		0x415
+#define DC_DISP_H_PULSE2_POSITION_A		0x416
+#define DC_DISP_H_PULSE2_POSITION_B		0x417
+#define DC_DISP_H_PULSE2_POSITION_C		0x418
+#define DC_DISP_H_PULSE2_POSITION_D		0x419
+#define DC_DISP_V_PULSE0_CONTROL		0x41a
+#define DC_DISP_V_PULSE0_POSITION_A		0x41b
+#define DC_DISP_V_PULSE0_POSITION_B		0x41c
+#define DC_DISP_V_PULSE0_POSITION_C		0x41d
+#define DC_DISP_V_PULSE1_CONTROL		0x41e
+#define DC_DISP_V_PULSE1_POSITION_A		0x41f
+#define DC_DISP_V_PULSE1_POSITION_B		0x420
+#define DC_DISP_V_PULSE1_POSITION_C		0x421
+#define DC_DISP_V_PULSE2_CONTROL		0x422
+#define DC_DISP_V_PULSE2_POSITION_A		0x423
+#define DC_DISP_V_PULSE3_CONTROL		0x424
+#define DC_DISP_V_PULSE3_POSITION_A		0x425
+#define DC_DISP_M0_CONTROL			0x426
+#define DC_DISP_M1_CONTROL			0x427
+#define DC_DISP_DI_CONTROL			0x428
+#define DC_DISP_PP_CONTROL			0x429
+#define DC_DISP_PP_SELECT_A			0x42a
+#define DC_DISP_PP_SELECT_B			0x42b
+#define DC_DISP_PP_SELECT_C			0x42c
+#define DC_DISP_PP_SELECT_D			0x42d
+
+#define  PULSE_MODE_NORMAL		(0 << 3)
+#define  PULSE_MODE_ONE_CLOCK		(1 << 3)
+#define  PULSE_POLARITY_HIGH		(0 << 4)
+#define  PULSE_POLARITY_LOW		(1 << 4)
+#define  PULSE_QUAL_ALWAYS		(0 << 6)
+#define  PULSE_QUAL_VACTIVE		(2 << 6)
+#define  PULSE_QUAL_VACTIVE1		(3 << 6)
+#define  PULSE_LAST_START_A		(0 << 8)
+#define  PULSE_LAST_END_A		(1 << 8)
+#define  PULSE_LAST_START_B		(2 << 8)
+#define  PULSE_LAST_END_B		(3 << 8)
+#define  PULSE_LAST_START_C		(4 << 8)
+#define  PULSE_LAST_END_C		(5 << 8)
+#define  PULSE_LAST_START_D		(6 << 8)
+#define  PULSE_LAST_END_D		(7 << 8)
+
+#define  PULSE_START(x)			((x) & 0xfff)
+#define  PULSE_END(x)			(((x) & 0xfff) << 16)
+
+#define DC_DISP_DISP_CLOCK_CONTROL		0x42e
+#define  PIXEL_CLK_DIVIDER_PCD1		(0 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD1H	(1 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD2		(2 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD3		(3 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD4		(4 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD6		(5 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD8		(6 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD9		(7 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD12	(8 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD16	(9 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD18	(10 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD24	(11 << 8)
+#define  PIXEL_CLK_DIVIDER_PCD13	(12 << 8)
+#define  SHIFT_CLK_DIVIDER(x)		((x) & 0xff)
+
+#define DC_DISP_DISP_INTERFACE_CONTROL		0x42f
+#define  DISP_DATA_FORMAT_DF1P1C	(0 << 0)
+#define  DISP_DATA_FORMAT_DF1P2C24B	(1 << 0)
+#define  DISP_DATA_FORMAT_DF1P2C18B	(2 << 0)
+#define  DISP_DATA_FORMAT_DF1P2C16B	(3 << 0)
+#define  DISP_DATA_FORMAT_DF2S		(5 << 0)
+#define  DISP_DATA_FORMAT_DF3S		(6 << 0)
+#define  DISP_DATA_FORMAT_DFSPI		(7 << 0)
+#define  DISP_DATA_FORMAT_DF1P3C24B	(8 << 0)
+#define  DISP_DATA_FORMAT_DF1P3C18B	(9 << 0)
+#define  DISP_DATA_ALIGNMENT_MSB	(0 << 8)
+#define  DISP_DATA_ALIGNMENT_LSB	(1 << 8)
+#define  DISP_DATA_ORDER_RED_BLUE	(0 << 9)
+#define  DISP_DATA_ORDER_BLUE_RED	(1 << 9)
+
+#define DC_DISP_DISP_COLOR_CONTROL		0x430
+#define  BASE_COLOR_SIZE666		(0 << 0)
+#define  BASE_COLOR_SIZE111		(1 << 0)
+#define  BASE_COLOR_SIZE222		(2 << 0)
+#define  BASE_COLOR_SIZE333		(3 << 0)
+#define  BASE_COLOR_SIZE444		(4 << 0)
+#define  BASE_COLOR_SIZE555		(5 << 0)
+#define  BASE_COLOR_SIZE565		(6 << 0)
+#define  BASE_COLOR_SIZE332		(7 << 0)
+#define  BASE_COLOR_SIZE888		(8 << 0)
+
+#define  DITHER_CONTROL_DISABLE		(0 << 8)
+#define  DITHER_CONTROL_ORDERED		(2 << 8)
+#define  DITHER_CONTROL_ERRDIFF		(3 << 8)
+
+#define DC_DISP_SHIFT_CLOCK_OPTIONS		0x431
+#define DC_DISP_DATA_ENABLE_OPTIONS		0x432
+#define   DE_SELECT_ACTIVE_BLANK	0x0
+#define   DE_SELECT_ACTIVE		0x1
+#define   DE_SELECT_ACTIVE_IS		0x2
+#define   DE_CONTROL_ONECLK		(0 << 2)
+#define   DE_CONTROL_NORMAL		(1 << 2)
+#define   DE_CONTROL_EARLY_EXT		(2 << 2)
+#define   DE_CONTROL_EARLY		(3 << 2)
+#define   DE_CONTROL_ACTIVE_BLANK	(4 << 2)
+
+#define DC_DISP_SERIAL_INTERFACE_OPTIONS	0x433
+#define DC_DISP_LCD_SPI_OPTIONS			0x434
+#define DC_DISP_BORDER_COLOR			0x435
+#define DC_DISP_COLOR_KEY0_LOWER		0x436
+#define DC_DISP_COLOR_KEY0_UPPER		0x437
+#define DC_DISP_COLOR_KEY1_LOWER		0x438
+#define DC_DISP_COLOR_KEY1_UPPER		0x439
+
+#define DC_DISP_CURSOR_FOREGROUND		0x43c
+#define DC_DISP_CURSOR_BACKGROUND		0x43d
+#define   CURSOR_COLOR(_r, _g, _b) ((_r) | ((_g) << 8) | ((_b) << 16))
+
+#define DC_DISP_CURSOR_START_ADDR		0x43e
+#define DC_DISP_CURSOR_START_ADDR_NS		0x43f
+#define   CURSOR_START_ADDR_MASK	(((1 << 22) - 1) << 10)
+#define   CURSOR_START_ADDR(_addr)	((_addr) >> 10)
+#define	  CURSOR_SIZE_64		(1 << 24)
+
+#define DC_DISP_CURSOR_POSITION			0x440
+#define   CURSOR_POSITION(_x, _y)		\
+	(((_x) & ((1 << 16) - 1)) |		\
+	(((_y) & ((1 << 16) - 1)) << 16))
+
+#define DC_DISP_CURSOR_POSITION_NS		0x441
+#define DC_DISP_INIT_SEQ_CONTROL		0x442
+#define DC_DISP_SPI_INIT_SEQ_DATA_A		0x443
+#define DC_DISP_SPI_INIT_SEQ_DATA_B		0x444
+#define DC_DISP_SPI_INIT_SEQ_DATA_C		0x445
+#define DC_DISP_SPI_INIT_SEQ_DATA_D		0x446
+#define DC_DISP_DC_MCCIF_FIFOCTRL		0x480
+#define DC_DISP_MCCIF_DISPLAY0A_HYST		0x481
+#define DC_DISP_MCCIF_DISPLAY0B_HYST		0x482
+#define DC_DISP_MCCIF_DISPLAY0C_HYST		0x483
+#define DC_DISP_MCCIF_DISPLAY1B_HYST		0x484
+#define DC_DISP_DAC_CRT_CTRL			0x4c0
+#define DC_DISP_DISP_MISC_CONTROL		0x4c1
+#define   UF_LINE_FLUSH                         (1 << 1)
+
+#define DC_WIN_COLOR_PALETTE(x)			(0x500 + (x))
+
+#define DC_WIN_PALETTE_COLOR_EXT		0x600
+#define DC_WIN_H_FILTER_P(x)			(0x601 + (x))
+#define DC_WIN_CSC_YOF				0x611
+#define DC_WIN_CSC_KYRGB			0x612
+#define DC_WIN_CSC_KUR				0x613
+#define DC_WIN_CSC_KVR				0x614
+#define DC_WIN_CSC_KUG				0x615
+#define DC_WIN_CSC_KVG				0x616
+#define DC_WIN_CSC_KUB				0x617
+#define DC_WIN_CSC_KVB				0x618
+#define DC_WIN_V_FILTER_P(x)			(0x619 + (x))
+#define DC_WIN_WIN_OPTIONS			0x700
+#define  H_DIRECTION_INCREMENT		(0 << 0)
+#define  H_DIRECTION_DECREMENT		(1 << 0)
+#define  V_DIRECTION_INCREMENT		(0 << 2)
+#define  V_DIRECTION_DECREMENT		(1 << 2)
+#define  COLOR_EXPAND			(1 << 6)
+#define  H_FILTER_ENABLE		(1 << 8)
+#define  V_FILTER_ENABLE		(1 << 10)
+#define  CP_ENABLE			(1 << 16)
+#define  CSC_ENABLE			(1 << 18)
+#define  DV_ENABLE			(1 << 20)
+#define  WIN_ENABLE			(1 << 30)
+
+#define DC_WIN_BYTE_SWAP			0x701
+#define  BYTE_SWAP_NOSWAP		0
+#define  BYTE_SWAP_SWAP2		1
+#define  BYTE_SWAP_SWAP4		2
+#define  BYTE_SWAP_SWAP4HW		3
+
+#define DC_WIN_BUFFER_CONTROL			0x702
+#define  BUFFER_CONTROL_HOST		0
+#define  BUFFER_CONTROL_VI		1
+#define  BUFFER_CONTROL_EPP		2
+#define  BUFFER_CONTROL_MPEGE		3
+#define  BUFFER_CONTROL_SB2D		4
+
+#define DC_WIN_COLOR_DEPTH			0x703
+
+#define DC_WIN_POSITION				0x704
+#define  H_POSITION(x)		(((x) & 0xfff) << 0)
+#define  V_POSITION(x)		(((x) & 0xfff) << 16)
+
+#define DC_WIN_SIZE				0x705
+#define  H_SIZE(x)		(((x) & 0xfff) << 0)
+#define  V_SIZE(x)		(((x) & 0xfff) << 16)
+
+#define DC_WIN_PRESCALED_SIZE			0x706
+#define  H_PRESCALED_SIZE(x)	(((x) & 0x3fff) << 0)
+#define  V_PRESCALED_SIZE(x)	(((x) & 0xfff) << 16)
+
+#define DC_WIN_H_INITIAL_DDA			0x707
+#define DC_WIN_V_INITIAL_DDA			0x708
+#define DC_WIN_DDA_INCREMENT			0x709
+#define  H_DDA_INC(x)		(((x) & 0xffff) << 0)
+#define  V_DDA_INC(x)		(((x) & 0xffff) << 16)
+
+#define DC_WIN_LINE_STRIDE			0x70a
+#define  LINE_STRIDE(x)		(x)
+#define  UV_LINE_STRIDE(x)	(((x) & 0xffff) << 16)
+#define  GET_LINE_STRIDE(x)	((x) & 0xffff)
+#define  GET_UV_LINE_STRIDE(x)	(((x) >> 16) & 0xffff)
+#define DC_WIN_BUF_STRIDE			0x70b
+#define DC_WIN_UV_BUF_STRIDE			0x70c
+#define DC_WIN_BUFFER_ADDR_MODE			0x70d
+#define  DC_WIN_BUFFER_ADDR_MODE_LINEAR		(0 << 0)
+#define  DC_WIN_BUFFER_ADDR_MODE_LINEAR_UV	(0 << 16)
+#define  DC_WIN_BUFFER_ADDR_MODE_TILE		(1 << 0)
+#define  DC_WIN_BUFFER_ADDR_MODE_TILE_UV	(1 << 16)
+#define DC_WIN_DV_CONTROL			0x70e
+#define DC_WIN_BLEND_NOKEY			0x70f
+#define DC_WIN_BLEND_1WIN			0x710
+#define DC_WIN_BLEND_2WIN_X			0x711
+#define DC_WIN_BLEND_2WIN_Y			0x712
+#define DC_WIN_BLEND_3WIN_XY			0x713
+#define  CKEY_NOKEY			(0 << 0)
+#define  CKEY_KEY0			(1 << 0)
+#define  CKEY_KEY1			(2 << 0)
+#define  CKEY_KEY01			(3 << 0)
+#define  BLEND_CONTROL_FIX		(0 << 2)
+#define  BLEND_CONTROL_ALPHA		(1 << 2)
+#define  BLEND_CONTROL_DEPENDANT	(2 << 2)
+#define  BLEND_CONTROL_PREMULT		(3 << 2)
+#define  BLEND_WEIGHT0(x)		(((x) & 0xff) << 8)
+#define  BLEND_WEIGHT1(x)		(((x) & 0xff) << 16)
+#define  BLEND(key, control, weight0, weight1)			\
+	  (CKEY_ ## key | BLEND_CONTROL_ ## control |		\
+	   BLEND_WEIGHT0(weight0) | BLEND_WEIGHT1(weight1))
+
+
+#define DC_WIN_HP_FETCH_CONTROL			0x714
+
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+#define DC_WIN_GLOBAL_ALPHA			0x715
+#define  GLOBAL_ALPHA_ENABLE		0x10000
+#endif
+
+#define DC_WINBUF_START_ADDR			0x800
+#define DC_WINBUF_START_ADDR_NS			0x801
+#define DC_WINBUF_START_ADDR_U			0x802
+#define DC_WINBUF_START_ADDR_U_NS		0x803
+#define DC_WINBUF_START_ADDR_V			0x804
+#define DC_WINBUF_START_ADDR_V_NS		0x805
+#define DC_WINBUF_ADDR_H_OFFSET			0x806
+#define DC_WINBUF_ADDR_H_OFFSET_NS		0x807
+#define DC_WINBUF_ADDR_V_OFFSET			0x808
+#define DC_WINBUF_ADDR_V_OFFSET_NS		0x809
+#define DC_WINBUF_UFLOW_STATUS			0x80a
+
+/* direct versions of DC_WINBUF_UFLOW_STATUS */
+#define DC_WINBUF_AD_UFLOW_STATUS		0xbca
+#define DC_WINBUF_BD_UFLOW_STATUS		0xdca
+#define DC_WINBUF_CD_UFLOW_STATUS		0xfca
+
+#define DC_DISP_SD_CONTROL			0x4c2
+#define  SD_ENABLE_NORMAL		(1 << 0)
+#define  SD_ENABLE_ONESHOT		(2 << 0)
+#define  SD_USE_VID_LUMA		(1 << 2)
+#define  SD_BIN_WIDTH_ONE		(0 << 3)
+#define  SD_BIN_WIDTH_TWO		(1 << 3)
+#define  SD_BIN_WIDTH_FOUR		(2 << 3)
+#define  SD_BIN_WIDTH_EIGHT		(3 << 3)
+#define  SD_BIN_WIDTH_MASK		(3 << 3)
+#define  SD_AGGRESSIVENESS(x)	   	(((x) & 0x7) << 5)
+#define  SD_HW_UPDATE_DLY(x)		(((x) & 0x3) << 8)
+#define  SD_ONESHOT_ENABLE		(1 << 10)
+#define  SD_CORRECTION_MODE_AUTO	(0 << 11)
+#define  SD_CORRECTION_MODE_MAN		(1 << 11)
+
+#define NUM_BIN_WIDTHS 4
+#define STEPS_PER_AGG_LVL 64
+#define STEPS_PER_AGG_CHG_LOG2 5
+#define STEPS_PER_AGG_CHG (1<<STEPS_PER_AGG_CHG_LOG2)
+#define ADJ_PHASE_STEP 8
+#define K_STEP 4
+
+#define DC_DISP_SD_CSC_COEFF			0x4c3
+#define  SD_CSC_COEFF_R(x)		(((x) & 0xf) << 4)
+#define  SD_CSC_COEFF_G(x)		(((x) & 0xf) << 12)
+#define  SD_CSC_COEFF_B(x)		(((x) & 0xf) << 20)
+
+#define DC_DISP_SD_LUT(i)			(0x4c4 + i)
+#define DC_DISP_SD_LUT_NUM			9
+#define  SD_LUT_R(x)			(((x) & 0xff) << 0)
+#define  SD_LUT_G(x)			(((x) & 0xff) << 8)
+#define  SD_LUT_B(x)			(((x) & 0xff) << 16)
+
+#define DC_DISP_SD_FLICKER_CONTROL		0x4cd
+#define  SD_FC_TIME_LIMIT(x)		(((x) & 0xff) << 0)
+#define  SD_FC_THRESHOLD(x)		(((x) & 0xff) << 8)
+
+#define DC_DISP_SD_PIXEL_COUNT			0x4ce
+
+#define DC_DISP_SD_HISTOGRAM(i)			(0x4cf + i)
+#define DC_DISP_SD_HISTOGRAM_NUM		8
+#define  SD_HISTOGRAM_BIN_0(val)	(((val) & (0xff << 0)) >> 0)
+#define  SD_HISTOGRAM_BIN_1(val)	(((val) & (0xff << 8)) >> 8)
+#define  SD_HISTOGRAM_BIN_2(val)	(((val) & (0xff << 16)) >> 16)
+#define  SD_HISTOGRAM_BIN_3(val)	(((val) & (0xff << 24)) >> 24)
+
+#define DC_DISP_SD_BL_PARAMETERS		0x4d7
+#define  SD_BLP_TIME_CONSTANT(x)	(((x) & 0x7ff) << 0)
+#define  SD_BLP_STEP(x)			(((x) & 0xff) << 16)
+
+#define DC_DISP_SD_BL_TF(i)			(0x4d8 + i)
+#define DC_DISP_SD_BL_TF_NUM			4
+#define  SD_BL_TF_POINT_0(x)		(((x) & 0xff) << 0)
+#define  SD_BL_TF_POINT_1(x)		(((x) & 0xff) << 8)
+#define  SD_BL_TF_POINT_2(x)		(((x) & 0xff) << 16)
+#define  SD_BL_TF_POINT_3(x)		(((x) & 0xff) << 24)
+
+#define DC_DISP_SD_BL_CONTROL			0x4dc
+#define  SD_BLC_MODE_MAN		(0 << 0)
+#define  SD_BLC_MODE_AUTO		(1 << 1)
+#define  SD_BLC_BRIGHTNESS(val)	 	(((val) & (0xff << 8)) >> 8)
+
+#define DC_DISP_SD_HW_K_VALUES			0x4dd
+#define  SD_HW_K_R(val)			(((val) & (0x3ff << 0)) >> 0)
+#define  SD_HW_K_G(val)			(((val) & (0x3ff << 10)) >> 10)
+#define  SD_HW_K_B(val)			(((val) & (0x3ff << 20)) >> 20)
+
+#define DC_DISP_SD_MAN_K_VALUES			0x4de
+#define  SD_MAN_K_R(x)			(((x) & 0x3ff) << 0)
+#define  SD_MAN_K_G(x)			(((x) & 0x3ff) << 10)
+#define  SD_MAN_K_B(x)			(((x) & 0x3ff) << 20)
+
+#define  NUM_AGG_PRI_LVLS		4
+#define  SD_AGG_PRI_LVL(x)		((x) >> 3)
+#define  SD_GET_AGG(x)			((x) & 0x7)
+
+#endif
diff --git a/drivers/video/tegra/dc/dc_sysfs.c b/drivers/video/tegra/dc/dc_sysfs.c
new file mode 100644
index 000000000000..09a8e2dbb5b1
--- /dev/null
+++ b/drivers/video/tegra/dc/dc_sysfs.c
@@ -0,0 +1,374 @@
+/*
+ * drivers/video/tegra/dc/dc_sysfs.c
+ *
+ * Copyright (c) 2011-2012, NVIDIA 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/fb.h>
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+
+#include <mach/dc.h>
+#include <mach/fb.h>
+
+#include "dc_reg.h"
+#include "dc_priv.h"
+#include "nvsd.h"
+
+static ssize_t mode_show(struct device *device,
+	struct device_attribute *attr, char *buf)
+{
+	struct nvhost_device *ndev = to_nvhost_device(device);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	struct tegra_dc_mode *m;
+	ssize_t res;
+
+	mutex_lock(&dc->lock);
+	m = &dc->mode;
+	res = snprintf(buf, PAGE_SIZE,
+		"pclk: %d\n"
+		"h_ref_to_sync: %d\n"
+		"v_ref_to_sync: %d\n"
+		"h_sync_width: %d\n"
+		"v_sync_width: %d\n"
+		"h_back_porch: %d\n"
+		"v_back_porch: %d\n"
+		"h_active: %d\n"
+		"v_active: %d\n"
+		"h_front_porch: %d\n"
+		"v_front_porch: %d\n"
+		"stereo_mode: %d\n",
+		m->pclk, m->h_ref_to_sync, m->v_ref_to_sync,
+		m->h_sync_width, m->v_sync_width,
+		m->h_back_porch, m->v_back_porch,
+		m->h_active, m->v_active,
+		m->h_front_porch, m->v_front_porch,
+		m->stereo_mode);
+	mutex_unlock(&dc->lock);
+
+	return res;
+}
+
+static DEVICE_ATTR(mode, S_IRUGO, mode_show, NULL);
+
+static ssize_t stats_enable_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	bool enabled;
+
+	if (mutex_lock_killable(&dc->lock))
+		return -EINTR;
+	enabled = tegra_dc_stats_get(dc);
+	mutex_unlock(&dc->lock);
+
+	return snprintf(buf, PAGE_SIZE, "%d", enabled);
+}
+
+static ssize_t stats_enable_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	unsigned long val = 0;
+
+	if (strict_strtoul(buf, 10, &val) < 0)
+		return -EINVAL;
+
+	if (mutex_lock_killable(&dc->lock))
+		return -EINTR;
+	tegra_dc_stats_enable(dc, !!val);
+	mutex_unlock(&dc->lock);
+
+	return count;
+}
+
+static DEVICE_ATTR(stats_enable, S_IRUGO|S_IWUSR,
+	stats_enable_show, stats_enable_store);
+
+static ssize_t enable_show(struct device *device,
+	struct device_attribute *attr, char *buf)
+{
+	struct nvhost_device *ndev = to_nvhost_device(device);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	ssize_t res;
+
+	mutex_lock(&dc->lock);
+	res = snprintf(buf, PAGE_SIZE, "%d\n", dc->enabled);
+	mutex_unlock(&dc->lock);
+	return res;
+}
+
+static ssize_t enable_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	unsigned long val = 0;
+
+	if (strict_strtoul(buf, 10, &val) < 0)
+		return -EINVAL;
+
+	if (val) {
+		tegra_dc_enable(dc);
+	} else {
+		tegra_dc_disable(dc);
+	}
+
+	return count;
+}
+
+static DEVICE_ATTR(enable, S_IRUGO|S_IWUSR, enable_show, enable_store);
+
+static ssize_t crc_checksum_latched_show(struct device *device,
+	struct device_attribute *attr, char *buf)
+{
+	struct nvhost_device *ndev = to_nvhost_device(device);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+
+	u32 crc;
+
+	if (!dc->enabled) {
+		dev_err(&dc->ndev->dev, "Failed to get dc.\n");
+		return -EFAULT;
+	}
+
+	crc = tegra_dc_read_checksum_latched(dc);
+
+	return snprintf(buf, PAGE_SIZE, "%u", crc);
+}
+
+static ssize_t crc_checksum_latched_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	unsigned long val = 0;
+
+	if (!dc->enabled) {
+		dev_err(&dc->ndev->dev, "Failed to get dc.\n");
+		return -EFAULT;
+	}
+
+	if (strict_strtoul(buf, 10, &val) < 0)
+		return -EINVAL;
+
+	if (val == 1) {
+		tegra_dc_enable_crc(dc);
+		dev_err(&dc->ndev->dev, "crc is enabled.\n");
+	} else if (val == 0) {
+		tegra_dc_disable_crc(dc);
+		dev_err(&dc->ndev->dev, "crc is disabled.\n");
+	} else
+		dev_err(&dc->ndev->dev, "Invalid input.\n");
+
+	return count;
+}
+static DEVICE_ATTR(crc_checksum_latched, S_IRUGO|S_IWUSR,
+		crc_checksum_latched_show, crc_checksum_latched_store);
+
+#define ORIENTATION_PORTRAIT	"portrait"
+#define ORIENTATION_LANDSCAPE	"landscape"
+
+static ssize_t orientation_3d_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	struct tegra_dc_out *dc_out = dc->out;
+	const char *orientation;
+	switch (dc_out->stereo->orientation) {
+	case TEGRA_DC_STEREO_LANDSCAPE:
+		orientation = ORIENTATION_LANDSCAPE;
+		break;
+	case TEGRA_DC_STEREO_PORTRAIT:
+		orientation = ORIENTATION_PORTRAIT;
+		break;
+	default:
+		pr_err("Invalid value is stored for stereo_orientation.\n");
+		return -EINVAL;
+	}
+	return snprintf(buf, PAGE_SIZE, "%s\n", orientation);
+}
+
+static ssize_t orientation_3d_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t cnt)
+{
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	struct tegra_dc_out *dc_out = dc->out;
+	struct tegra_stereo_out *stereo = dc_out->stereo;
+	int orientation;
+
+	if (0 == strncmp(buf, ORIENTATION_PORTRAIT,
+			min(cnt, ARRAY_SIZE(ORIENTATION_PORTRAIT) - 1))) {
+		orientation = TEGRA_DC_STEREO_PORTRAIT;
+	} else if (0 == strncmp(buf, ORIENTATION_LANDSCAPE,
+			min(cnt, ARRAY_SIZE(ORIENTATION_LANDSCAPE) - 1))) {
+		orientation = TEGRA_DC_STEREO_LANDSCAPE;
+	} else {
+		pr_err("Invalid property value for stereo_orientation.\n");
+		return -EINVAL;
+	}
+	stereo->orientation = orientation;
+	stereo->set_orientation(orientation);
+	return cnt;
+}
+
+static DEVICE_ATTR(stereo_orientation,
+	S_IRUGO|S_IWUSR, orientation_3d_show, orientation_3d_store);
+
+#define MODE_2D		"2d"
+#define MODE_3D		"3d"
+
+static ssize_t mode_3d_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	struct tegra_dc_out *dc_out = dc->out;
+	const char *mode;
+	switch (dc_out->stereo->mode_2d_3d) {
+	case TEGRA_DC_STEREO_MODE_2D:
+		mode = MODE_2D;
+		break;
+	case TEGRA_DC_STEREO_MODE_3D:
+		mode = MODE_3D;
+		break;
+	default:
+		pr_err("Invalid value is stored for stereo_mode.\n");
+		return -EINVAL;
+	}
+	return snprintf(buf, PAGE_SIZE, "%s\n", mode);
+}
+
+static ssize_t mode_3d_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t cnt)
+{
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	struct tegra_dc_out *dc_out = dc->out;
+	struct tegra_stereo_out *stereo = dc_out->stereo;
+	int mode;
+
+	if (0 == strncmp(buf, MODE_2D, min(cnt, ARRAY_SIZE(MODE_2D) - 1))) {
+		mode = TEGRA_DC_STEREO_MODE_2D;
+	} else if (0 == strncmp(buf, MODE_3D,
+			min(cnt, ARRAY_SIZE(MODE_3D) - 1))) {
+		mode = TEGRA_DC_STEREO_MODE_3D;
+	} else {
+		pr_err("Invalid property value for stereo_mode.\n");
+		return -EINVAL;
+	}
+	stereo->mode_2d_3d = mode;
+	stereo->set_mode(mode);
+	return cnt;
+}
+
+static DEVICE_ATTR(stereo_mode,
+	S_IRUGO|S_IWUSR, mode_3d_show, mode_3d_store);
+
+static ssize_t nvdps_show(struct device *device,
+	struct device_attribute *attr, char *buf)
+{
+	int refresh_rate;
+	struct nvhost_device *ndev = to_nvhost_device(device);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+
+	refresh_rate = tegra_fb_get_mode(dc);
+	return snprintf(buf, PAGE_SIZE, "%d\n", refresh_rate);
+}
+
+
+static ssize_t nvdps_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	int refresh_rate;
+	int e;
+
+	e = kstrtoint(buf, 10, &refresh_rate);
+	if (e)
+		return e;
+	e = tegra_fb_set_mode(dc, refresh_rate);
+
+	return count;
+}
+
+static DEVICE_ATTR(nvdps, S_IRUGO|S_IWUSR, nvdps_show, nvdps_store);
+
+static ssize_t smart_panel_show(struct device *device,
+	struct device_attribute *attr, char  *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "1\n");
+}
+
+static DEVICE_ATTR(smart_panel, S_IRUGO, smart_panel_show, NULL);
+
+void __devexit tegra_dc_remove_sysfs(struct device *dev)
+{
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	struct tegra_dc_sd_settings *sd_settings = dc->out->sd_settings;
+
+	device_remove_file(dev, &dev_attr_mode);
+	device_remove_file(dev, &dev_attr_nvdps);
+	device_remove_file(dev, &dev_attr_enable);
+	device_remove_file(dev, &dev_attr_stats_enable);
+	device_remove_file(dev, &dev_attr_crc_checksum_latched);
+
+	if (dc->out->stereo) {
+		device_remove_file(dev, &dev_attr_stereo_orientation);
+		device_remove_file(dev, &dev_attr_stereo_mode);
+	}
+
+	if (sd_settings)
+		nvsd_remove_sysfs(dev);
+
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
+		device_remove_file(dev, &dev_attr_smart_panel);
+}
+
+void tegra_dc_create_sysfs(struct device *dev)
+{
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	struct tegra_dc_sd_settings *sd_settings = dc->out->sd_settings;
+	int error = 0;
+
+	error |= device_create_file(dev, &dev_attr_mode);
+	error |= device_create_file(dev, &dev_attr_nvdps);
+	error |= device_create_file(dev, &dev_attr_enable);
+	error |= device_create_file(dev, &dev_attr_stats_enable);
+	error |= device_create_file(dev, &dev_attr_crc_checksum_latched);
+
+	if (dc->out->stereo) {
+		error |= device_create_file(dev, &dev_attr_stereo_orientation);
+		error |= device_create_file(dev, &dev_attr_stereo_mode);
+	}
+
+	if (sd_settings)
+		error |= nvsd_create_sysfs(dev);
+
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
+		error |= device_create_file(dev, &dev_attr_smart_panel);
+
+	if (error)
+		dev_err(&ndev->dev, "Failed to create sysfs attributes!\n");
+}
diff --git a/drivers/video/tegra/dc/dsi.c b/drivers/video/tegra/dc/dsi.c
new file mode 100644
index 000000000000..d3b1d40d535b
--- /dev/null
+++ b/drivers/video/tegra/dc/dsi.c
@@ -0,0 +1,3510 @@
+/*
+ * drivers/video/tegra/dc/dsi.c
+ *
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/fb.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/workqueue.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/nvhost.h>
+
+#include <mach/clk.h>
+#include <mach/dc.h>
+#include <mach/fb.h>
+#include <mach/csi.h>
+#include <mach/iomap.h>
+#include <linux/nvhost.h>
+
+#include "dc_reg.h"
+#include "dc_priv.h"
+#include "dsi_regs.h"
+#include "dsi.h"
+
+#define APB_MISC_GP_MIPI_PAD_CTRL_0	(TEGRA_APB_MISC_BASE + 0x820)
+#define DSIB_MODE_ENABLE		0x2
+
+#define DSI_USE_SYNC_POINTS		1
+#define S_TO_MS(x)			(1000 * (x))
+
+#define DSI_MODULE_NOT_INIT		0x0
+#define DSI_MODULE_INIT			0x1
+
+#define DSI_LPHS_NOT_INIT		0x0
+#define DSI_LPHS_IN_LP_MODE		0x1
+#define DSI_LPHS_IN_HS_MODE		0x2
+
+#define DSI_VIDEO_TYPE_NOT_INIT		0x0
+#define DSI_VIDEO_TYPE_VIDEO_MODE	0x1
+#define DSI_VIDEO_TYPE_CMD_MODE		0x2
+
+#define DSI_DRIVEN_MODE_NOT_INIT	0x0
+#define DSI_DRIVEN_MODE_DC		0x1
+#define DSI_DRIVEN_MODE_HOST		0x2
+
+#define DSI_PHYCLK_OUT_DIS		0x0
+#define DSI_PHYCLK_OUT_EN		0x1
+
+#define DSI_PHYCLK_NOT_INIT		0x0
+#define DSI_PHYCLK_CONTINUOUS		0x1
+#define DSI_PHYCLK_TX_ONLY		0x2
+
+#define DSI_CLK_BURST_NOT_INIT		0x0
+#define DSI_CLK_BURST_NONE_BURST	0x1
+#define DSI_CLK_BURST_BURST_MODE	0x2
+
+#define DSI_DC_STREAM_DISABLE		0x0
+#define DSI_DC_STREAM_ENABLE		0x1
+
+#define DSI_LP_OP_NOT_INIT		0x0
+#define DSI_LP_OP_WRITE			0x1
+#define DSI_LP_OP_READ			0x2
+
+#define DSI_HOST_IDLE_PERIOD		1000
+
+static bool enable_read_debug;
+module_param(enable_read_debug, bool, 0644);
+MODULE_PARM_DESC(enable_read_debug,
+		"Enable to print read fifo and return packet type");
+
+struct dsi_status {
+	unsigned init:2;
+
+	unsigned lphs:2;
+
+	unsigned vtype:2;
+	unsigned driven:2;
+
+	unsigned clk_out:2;
+	unsigned clk_mode:2;
+	unsigned clk_burst:2;
+
+	unsigned lp_op:2;
+
+	unsigned dc_stream:1;
+};
+
+/* source of video data */
+enum {
+	TEGRA_DSI_DRIVEN_BY_DC,
+	TEGRA_DSI_DRIVEN_BY_HOST,
+};
+
+struct tegra_dc_dsi_data {
+	struct tegra_dc *dc;
+	void __iomem *base;
+	struct resource *base_res;
+
+	struct clk *dc_clk;
+	struct clk *dsi_clk;
+	struct clk *dsi_fixed_clk;
+	bool clk_ref;
+
+	struct mutex lock;
+
+	/* data from board info */
+	struct tegra_dsi_out info;
+
+	struct dsi_status status;
+
+	struct dsi_phy_timing_inclk phy_timing;
+
+	bool ulpm;
+	bool enabled;
+	bool host_suspended;
+	struct mutex host_resume_lock;
+	struct delayed_work idle_work;
+	unsigned long idle_delay;
+	spinlock_t host_ref_lock;
+	u8 host_ref;
+
+	u8 driven_mode;
+	u8 controller_index;
+
+	u8 pixel_scaler_mul;
+	u8 pixel_scaler_div;
+
+	u32 default_shift_clk_div;
+	u32 default_pixel_clk_khz;
+	u32 default_hs_clk_khz;
+
+	u32 shift_clk_div;
+	u32 target_hs_clk_khz;
+	u32 target_lp_clk_khz;
+
+	u32 syncpt_id;
+	u32 syncpt_val;
+
+	u16 current_bit_clk_ns;
+	u32 current_dsi_clk_khz;
+
+	u32 dsi_control_val;
+};
+
+const u32 dsi_pkt_seq_reg[NUMOF_PKT_SEQ] = {
+	DSI_PKT_SEQ_0_LO,
+	DSI_PKT_SEQ_0_HI,
+	DSI_PKT_SEQ_1_LO,
+	DSI_PKT_SEQ_1_HI,
+	DSI_PKT_SEQ_2_LO,
+	DSI_PKT_SEQ_2_HI,
+	DSI_PKT_SEQ_3_LO,
+	DSI_PKT_SEQ_3_HI,
+	DSI_PKT_SEQ_4_LO,
+	DSI_PKT_SEQ_4_HI,
+	DSI_PKT_SEQ_5_LO,
+	DSI_PKT_SEQ_5_HI,
+};
+
+const u32 dsi_pkt_seq_video_non_burst_syne[NUMOF_PKT_SEQ] = {
+	PKT_ID0(CMD_VS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
+	0,
+	PKT_ID0(CMD_VE) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
+	0,
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
+	0,
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_BLNK) | PKT_LEN1(1) |
+	PKT_ID2(CMD_HE) | PKT_LEN2(0),
+	PKT_ID3(CMD_BLNK) | PKT_LEN3(2) | PKT_ID4(CMD_RGB) | PKT_LEN4(3) |
+	PKT_ID5(CMD_BLNK) | PKT_LEN5(4),
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
+	0,
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_BLNK) | PKT_LEN1(1) |
+	PKT_ID2(CMD_HE) | PKT_LEN2(0),
+	PKT_ID3(CMD_BLNK) | PKT_LEN3(2) | PKT_ID4(CMD_RGB) | PKT_LEN4(3) |
+	PKT_ID5(CMD_BLNK) | PKT_LEN5(4),
+};
+
+const u32 dsi_pkt_seq_video_non_burst[NUMOF_PKT_SEQ] = {
+	PKT_ID0(CMD_VS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
+	0,
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
+	0,
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
+	0,
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_BLNK) | PKT_LEN1(2) |
+	PKT_ID2(CMD_RGB) | PKT_LEN2(3),
+	PKT_ID3(CMD_BLNK) | PKT_LEN3(4),
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
+	0,
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_BLNK) | PKT_LEN1(2) |
+	PKT_ID2(CMD_RGB) | PKT_LEN2(3),
+	PKT_ID3(CMD_BLNK) | PKT_LEN3(4),
+};
+
+static const u32 dsi_pkt_seq_video_burst[NUMOF_PKT_SEQ] = {
+	PKT_ID0(CMD_VS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(7) | PKT_LP,
+	0,
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(7) | PKT_LP,
+	0,
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(7) | PKT_LP,
+	0,
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_BLNK) | PKT_LEN1(2)|
+	PKT_ID2(CMD_RGB) | PKT_LEN2(3) | PKT_LP,
+	PKT_ID0(CMD_EOT) | PKT_LEN0(7),
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(7) | PKT_LP,
+	0,
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_BLNK) | PKT_LEN1(2)|
+	PKT_ID2(CMD_RGB) | PKT_LEN2(3) | PKT_LP,
+	PKT_ID0(CMD_EOT) | PKT_LEN0(7),
+};
+
+static const u32 dsi_pkt_seq_video_burst_no_eot[NUMOF_PKT_SEQ] = {
+	PKT_ID0(CMD_VS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
+	0,
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
+	0,
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
+	0,
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_BLNK) | PKT_LEN1(2)|
+	PKT_ID2(CMD_RGB) | PKT_LEN2(3) | PKT_LP,
+	PKT_ID0(CMD_EOT) | PKT_LEN0(0),
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_EOT) | PKT_LEN1(0) | PKT_LP,
+	0,
+	PKT_ID0(CMD_HS) | PKT_LEN0(0) | PKT_ID1(CMD_BLNK) | PKT_LEN1(2)|
+	PKT_ID2(CMD_RGB) | PKT_LEN2(3) | PKT_LP,
+	PKT_ID0(CMD_EOT) | PKT_LEN0(0),
+};
+
+/* TODO: verify with hw about this format */
+const u32 dsi_pkt_seq_cmd_mode[NUMOF_PKT_SEQ] = {
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	PKT_ID0(CMD_LONGW) | PKT_LEN0(3) | PKT_ID1(CMD_EOT) | PKT_LEN1(7),
+	0,
+	0,
+	0,
+	PKT_ID0(CMD_LONGW) | PKT_LEN0(3) | PKT_ID1(CMD_EOT) | PKT_LEN1(7),
+	0,
+};
+
+const u32 init_reg[] = {
+	DSI_INT_ENABLE,
+	DSI_INT_STATUS,
+	DSI_INT_MASK,
+	DSI_INIT_SEQ_DATA_0,
+	DSI_INIT_SEQ_DATA_1,
+	DSI_INIT_SEQ_DATA_2,
+	DSI_INIT_SEQ_DATA_3,
+	DSI_INIT_SEQ_DATA_4,
+	DSI_INIT_SEQ_DATA_5,
+	DSI_INIT_SEQ_DATA_6,
+	DSI_INIT_SEQ_DATA_7,
+	DSI_DCS_CMDS,
+	DSI_PKT_SEQ_0_LO,
+	DSI_PKT_SEQ_1_LO,
+	DSI_PKT_SEQ_2_LO,
+	DSI_PKT_SEQ_3_LO,
+	DSI_PKT_SEQ_4_LO,
+	DSI_PKT_SEQ_5_LO,
+	DSI_PKT_SEQ_0_HI,
+	DSI_PKT_SEQ_1_HI,
+	DSI_PKT_SEQ_2_HI,
+	DSI_PKT_SEQ_3_HI,
+	DSI_PKT_SEQ_4_HI,
+	DSI_PKT_SEQ_5_HI,
+	DSI_CONTROL,
+	DSI_HOST_DSI_CONTROL,
+	DSI_PAD_CONTROL,
+	DSI_PAD_CONTROL_CD,
+	DSI_SOL_DELAY,
+	DSI_MAX_THRESHOLD,
+	DSI_TRIGGER,
+	DSI_TX_CRC,
+	DSI_INIT_SEQ_CONTROL,
+	DSI_PKT_LEN_0_1,
+	DSI_PKT_LEN_2_3,
+	DSI_PKT_LEN_4_5,
+	DSI_PKT_LEN_6_7,
+};
+
+static int tegra_dsi_host_suspend(struct tegra_dc *dc);
+static int tegra_dsi_host_resume(struct tegra_dc *dc);
+static void tegra_dc_dsi_idle_work(struct work_struct *work);
+
+inline unsigned long tegra_dsi_readl(struct tegra_dc_dsi_data *dsi, u32 reg)
+{
+	unsigned long ret;
+
+	BUG_ON(!nvhost_module_powered_ext(nvhost_get_parent(dsi->dc->ndev)));
+	ret = readl(dsi->base + reg * 4);
+	trace_printk("readl %p=%#08lx\n", dsi->base + reg * 4, ret);
+	return ret;
+}
+EXPORT_SYMBOL(tegra_dsi_readl);
+
+inline void tegra_dsi_writel(struct tegra_dc_dsi_data *dsi, u32 val, u32 reg)
+{
+	BUG_ON(!nvhost_module_powered_ext(nvhost_get_parent(dsi->dc->ndev)));
+	trace_printk("writel %p=%#08x\n", dsi->base + reg * 4, val);
+	writel(val, dsi->base + reg * 4);
+}
+EXPORT_SYMBOL(tegra_dsi_writel);
+
+#ifdef CONFIG_DEBUG_FS
+static int dbg_dsi_show(struct seq_file *s, void *unused)
+{
+	struct tegra_dc_dsi_data *dsi = s->private;
+
+#define DUMP_REG(a) do {						\
+		seq_printf(s, "%-32s\t%03x\t%08lx\n",			\
+		       #a, a, tegra_dsi_readl(dsi, a));		\
+	} while (0)
+
+	tegra_dc_io_start(dsi->dc);
+	clk_enable(dsi->dsi_clk);
+
+	DUMP_REG(DSI_INCR_SYNCPT_CNTRL);
+	DUMP_REG(DSI_INCR_SYNCPT_ERROR);
+	DUMP_REG(DSI_CTXSW);
+	DUMP_REG(DSI_POWER_CONTROL);
+	DUMP_REG(DSI_INT_ENABLE);
+	DUMP_REG(DSI_HOST_DSI_CONTROL);
+	DUMP_REG(DSI_CONTROL);
+	DUMP_REG(DSI_SOL_DELAY);
+	DUMP_REG(DSI_MAX_THRESHOLD);
+	DUMP_REG(DSI_TRIGGER);
+	DUMP_REG(DSI_TX_CRC);
+	DUMP_REG(DSI_STATUS);
+	DUMP_REG(DSI_INIT_SEQ_CONTROL);
+	DUMP_REG(DSI_INIT_SEQ_DATA_0);
+	DUMP_REG(DSI_INIT_SEQ_DATA_1);
+	DUMP_REG(DSI_INIT_SEQ_DATA_2);
+	DUMP_REG(DSI_INIT_SEQ_DATA_3);
+	DUMP_REG(DSI_INIT_SEQ_DATA_4);
+	DUMP_REG(DSI_INIT_SEQ_DATA_5);
+	DUMP_REG(DSI_INIT_SEQ_DATA_6);
+	DUMP_REG(DSI_INIT_SEQ_DATA_7);
+	DUMP_REG(DSI_PKT_SEQ_0_LO);
+	DUMP_REG(DSI_PKT_SEQ_0_HI);
+	DUMP_REG(DSI_PKT_SEQ_1_LO);
+	DUMP_REG(DSI_PKT_SEQ_1_HI);
+	DUMP_REG(DSI_PKT_SEQ_2_LO);
+	DUMP_REG(DSI_PKT_SEQ_2_HI);
+	DUMP_REG(DSI_PKT_SEQ_3_LO);
+	DUMP_REG(DSI_PKT_SEQ_3_HI);
+	DUMP_REG(DSI_PKT_SEQ_4_LO);
+	DUMP_REG(DSI_PKT_SEQ_4_HI);
+	DUMP_REG(DSI_PKT_SEQ_5_LO);
+	DUMP_REG(DSI_PKT_SEQ_5_HI);
+	DUMP_REG(DSI_DCS_CMDS);
+	DUMP_REG(DSI_PKT_LEN_0_1);
+	DUMP_REG(DSI_PKT_LEN_2_3);
+	DUMP_REG(DSI_PKT_LEN_4_5);
+	DUMP_REG(DSI_PKT_LEN_6_7);
+	DUMP_REG(DSI_PHY_TIMING_0);
+	DUMP_REG(DSI_PHY_TIMING_1);
+	DUMP_REG(DSI_PHY_TIMING_2);
+	DUMP_REG(DSI_BTA_TIMING);
+	DUMP_REG(DSI_TIMEOUT_0);
+	DUMP_REG(DSI_TIMEOUT_1);
+	DUMP_REG(DSI_TO_TALLY);
+	DUMP_REG(DSI_PAD_CONTROL);
+	DUMP_REG(DSI_PAD_CONTROL_CD);
+	DUMP_REG(DSI_PAD_CD_STATUS);
+	DUMP_REG(DSI_VID_MODE_CONTROL);
+#undef DUMP_REG
+
+	clk_disable(dsi->dsi_clk);
+	tegra_dc_io_end(dsi->dc);
+
+	return 0;
+}
+
+static int dbg_dsi_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dbg_dsi_show, inode->i_private);
+}
+
+static const struct file_operations dbg_fops = {
+	.open		= dbg_dsi_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static struct dentry *dsidir;
+
+static void tegra_dc_dsi_debug_create(struct tegra_dc_dsi_data *dsi)
+{
+	struct dentry *retval;
+
+	dsidir = debugfs_create_dir("tegra_dsi", NULL);
+	if (!dsidir)
+		return;
+	retval = debugfs_create_file("regs", S_IRUGO, dsidir, dsi,
+		&dbg_fops);
+	if (!retval)
+		goto free_out;
+	return;
+free_out:
+	debugfs_remove_recursive(dsidir);
+	dsidir = NULL;
+	return;
+}
+#else
+static inline void tegra_dc_dsi_debug_create(struct tegra_dc_dsi_data *dsi)
+{ }
+#endif
+
+static inline void tegra_dsi_clk_enable(struct tegra_dc_dsi_data *dsi)
+{
+	if (!tegra_is_clk_enabled(dsi->dsi_clk)) {
+		clk_enable(dsi->dsi_clk);
+		clk_enable(dsi->dsi_fixed_clk);
+	}
+}
+
+static inline void tegra_dsi_clk_disable(struct tegra_dc_dsi_data *dsi)
+{
+	if (tegra_is_clk_enabled(dsi->dsi_clk)) {
+		clk_disable(dsi->dsi_clk);
+		clk_disable(dsi->dsi_fixed_clk);
+	}
+}
+
+static int tegra_dsi_syncpt(struct tegra_dc_dsi_data *dsi)
+{
+	u32 val;
+	int ret;
+
+	ret = 0;
+
+	dsi->syncpt_val = nvhost_syncpt_read_ext(dsi->dc->ndev, dsi->syncpt_id);
+
+	val = DSI_INCR_SYNCPT_COND(OP_DONE) |
+		DSI_INCR_SYNCPT_INDX(dsi->syncpt_id);
+	tegra_dsi_writel(dsi, val, DSI_INCR_SYNCPT);
+
+	/* TODO: Use interrupt rather than polling */
+	ret = nvhost_syncpt_wait_timeout_ext(dsi->dc->ndev, dsi->syncpt_id,
+		dsi->syncpt_val + 1, MAX_SCHEDULE_TIMEOUT, NULL);
+	if (ret < 0) {
+		dev_err(&dsi->dc->ndev->dev, "DSI sync point failure\n");
+		goto fail;
+	}
+
+	(dsi->syncpt_val)++;
+	return 0;
+fail:
+	return ret;
+}
+
+static u32 tegra_dsi_get_hs_clk_rate(struct tegra_dc_dsi_data *dsi)
+{
+	u32 dsi_clock_rate_khz;
+
+	switch (dsi->info.video_burst_mode) {
+	case TEGRA_DSI_VIDEO_BURST_MODE_LOW_SPEED:
+	case TEGRA_DSI_VIDEO_BURST_MODE_MEDIUM_SPEED:
+	case TEGRA_DSI_VIDEO_BURST_MODE_FAST_SPEED:
+	case TEGRA_DSI_VIDEO_BURST_MODE_FASTEST_SPEED:
+		/* Calculate DSI HS clock rate for DSI burst mode */
+		dsi_clock_rate_khz = dsi->default_pixel_clk_khz *
+							dsi->shift_clk_div;
+		break;
+	case TEGRA_DSI_VIDEO_NONE_BURST_MODE:
+	case TEGRA_DSI_VIDEO_NONE_BURST_MODE_WITH_SYNC_END:
+	case TEGRA_DSI_VIDEO_BURST_MODE_LOWEST_SPEED:
+	default:
+		/* Clock rate is default DSI clock rate for non-burst mode */
+		dsi_clock_rate_khz = dsi->default_hs_clk_khz;
+		break;
+	}
+
+	return dsi_clock_rate_khz;
+}
+
+static u32 tegra_dsi_get_lp_clk_rate(struct tegra_dc_dsi_data *dsi, u8 lp_op)
+{
+	u32 dsi_clock_rate_khz;
+
+	if (dsi->info.enable_hs_clock_on_lp_cmd_mode)
+		if (dsi->info.hs_clk_in_lp_cmd_mode_freq_khz)
+			dsi_clock_rate_khz =
+				dsi->info.hs_clk_in_lp_cmd_mode_freq_khz;
+		else
+			dsi_clock_rate_khz = tegra_dsi_get_hs_clk_rate(dsi);
+	else
+		if (lp_op == DSI_LP_OP_READ)
+			dsi_clock_rate_khz =
+				dsi->info.lp_read_cmd_mode_freq_khz;
+		else
+			dsi_clock_rate_khz =
+				dsi->info.lp_cmd_mode_freq_khz;
+
+	return dsi_clock_rate_khz;
+}
+
+static u32 tegra_dsi_get_shift_clk_div(struct tegra_dc_dsi_data *dsi)
+{
+	u32 shift_clk_div;
+	u32 max_shift_clk_div;
+	u32 burst_width;
+	u32 burst_width_max;
+
+	/* Get the real value of default shift_clk_div. default_shift_clk_div
+	 * holds the real value of shift_clk_div.
+	 */
+	shift_clk_div = dsi->default_shift_clk_div;
+
+	/* Calculate shift_clk_div which can matche the video_burst_mode. */
+	if (dsi->info.video_burst_mode >=
+			TEGRA_DSI_VIDEO_BURST_MODE_LOWEST_SPEED) {
+		/* The max_shift_clk_div is multiplied by 10 to save the
+		 * fraction
+		 */
+		if (dsi->info.max_panel_freq_khz >= dsi->default_hs_clk_khz)
+			max_shift_clk_div = dsi->info.max_panel_freq_khz
+				* shift_clk_div * 10 / dsi->default_hs_clk_khz;
+		else
+			max_shift_clk_div = shift_clk_div * 10;
+
+		burst_width = dsi->info.video_burst_mode
+				- TEGRA_DSI_VIDEO_BURST_MODE_LOWEST_SPEED;
+		burst_width_max = TEGRA_DSI_VIDEO_BURST_MODE_FASTEST_SPEED
+				- TEGRA_DSI_VIDEO_BURST_MODE_LOWEST_SPEED;
+
+		shift_clk_div = (max_shift_clk_div - shift_clk_div * 10) *
+			burst_width / (burst_width_max * 10) + shift_clk_div;
+	}
+
+	return shift_clk_div;
+}
+
+static void tegra_dsi_init_sw(struct tegra_dc *dc,
+			struct tegra_dc_dsi_data *dsi)
+{
+	u32 h_width_pixels;
+	u32 v_width_lines;
+	u32 pixel_clk_hz;
+	u32 byte_clk_hz;
+	u32 plld_clk_mhz;
+
+	switch (dsi->info.pixel_format) {
+	case TEGRA_DSI_PIXEL_FORMAT_16BIT_P:
+		/* 2 bytes per pixel */
+		dsi->pixel_scaler_mul = 2;
+		dsi->pixel_scaler_div = 1;
+		break;
+	case TEGRA_DSI_PIXEL_FORMAT_18BIT_P:
+		/* 2.25 bytes per pixel */
+		dsi->pixel_scaler_mul = 9;
+		dsi->pixel_scaler_div = 4;
+		break;
+	case TEGRA_DSI_PIXEL_FORMAT_18BIT_NP:
+	case TEGRA_DSI_PIXEL_FORMAT_24BIT_P:
+		/* 3 bytes per pixel */
+		dsi->pixel_scaler_mul = 3;
+		dsi->pixel_scaler_div = 1;
+		break;
+	default:
+		break;
+	}
+
+	dsi->controller_index = dc->ndev->id;
+	dsi->ulpm = false;
+	dsi->enabled = false;
+	dsi->clk_ref = false;
+
+	dsi->dsi_control_val =
+			DSI_CONTROL_VIRTUAL_CHANNEL(dsi->info.virtual_channel) |
+			DSI_CONTROL_NUM_DATA_LANES(dsi->info.n_data_lanes - 1) |
+			DSI_CONTROL_VID_SOURCE(dsi->controller_index) |
+			DSI_CONTROL_DATA_FORMAT(dsi->info.pixel_format);
+
+	/* Below we are going to calculate dsi and dc clock rate.
+	 * Calcuate the horizontal and vertical width.
+	 */
+	h_width_pixels = dc->mode.h_back_porch + dc->mode.h_front_porch +
+			dc->mode.h_sync_width + dc->mode.h_active;
+	v_width_lines = dc->mode.v_back_porch + dc->mode.v_front_porch +
+			dc->mode.v_sync_width + dc->mode.v_active;
+
+	/* Calculate minimum required pixel rate. */
+	pixel_clk_hz = h_width_pixels * v_width_lines * dsi->info.refresh_rate;
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE) {
+		if (dsi->info.rated_refresh_rate >= dsi->info.refresh_rate)
+			dev_info(&dc->ndev->dev, "DSI: measured refresh rate "
+				"should be larger than rated refresh rate.\n");
+		dc->mode.rated_pclk = h_width_pixels * v_width_lines *
+						dsi->info.rated_refresh_rate;
+	}
+
+	/* Calculate minimum byte rate on DSI interface. */
+	byte_clk_hz = (pixel_clk_hz * dsi->pixel_scaler_mul) /
+			(dsi->pixel_scaler_div * dsi->info.n_data_lanes);
+
+	/* Round up to multiple of mega hz. */
+	plld_clk_mhz = DIV_ROUND_UP((byte_clk_hz * NUMOF_BIT_PER_BYTE),
+								1000000);
+
+	/* Calculate default real shift_clk_div. */
+	dsi->default_shift_clk_div = (NUMOF_BIT_PER_BYTE / 2) *
+		dsi->pixel_scaler_mul / (dsi->pixel_scaler_div *
+		dsi->info.n_data_lanes);
+	/* Calculate default DSI hs clock. DSI interface is double data rate.
+	 * Data is transferred on both rising and falling edge of clk, div by 2
+	 * to get the actual clock rate.
+	 */
+	dsi->default_hs_clk_khz = plld_clk_mhz * 1000 / 2;
+	dsi->default_pixel_clk_khz = plld_clk_mhz * 1000 / 2
+						/ dsi->default_shift_clk_div;
+
+	/* Get the actual shift_clk_div and clock rates. */
+	dsi->shift_clk_div = tegra_dsi_get_shift_clk_div(dsi);
+	dsi->target_lp_clk_khz =
+			tegra_dsi_get_lp_clk_rate(dsi, DSI_LP_OP_WRITE);
+	dsi->target_hs_clk_khz = tegra_dsi_get_hs_clk_rate(dsi);
+
+	dev_info(&dc->ndev->dev, "DSI: HS clock rate is %d\n",
+							dsi->target_hs_clk_khz);
+
+	dsi->controller_index = dc->ndev->id;
+
+#if DSI_USE_SYNC_POINTS
+	dsi->syncpt_id = NVSYNCPT_DSI;
+#endif
+
+	/*
+	 * Force video clock to be continuous mode if
+	 * enable_hs_clock_on_lp_cmd_mode is set
+	 */
+	if (dsi->info.enable_hs_clock_on_lp_cmd_mode) {
+		if (dsi->info.video_clock_mode !=
+					TEGRA_DSI_VIDEO_CLOCK_CONTINUOUS)
+			dev_warn(&dc->ndev->dev,
+				"Force clock continuous mode\n");
+
+		dsi->info.video_clock_mode = TEGRA_DSI_VIDEO_CLOCK_CONTINUOUS;
+	}
+
+	dsi->host_ref = 0;
+	dsi->host_suspended = false;
+	spin_lock_init(&dsi->host_ref_lock);
+	mutex_init(&dsi->host_resume_lock);
+	init_completion(&dc->out->user_vblank_comp);
+	INIT_DELAYED_WORK(&dsi->idle_work, tegra_dc_dsi_idle_work);
+	dsi->idle_delay = msecs_to_jiffies(DSI_HOST_IDLE_PERIOD);
+}
+
+#define SELECT_T_PHY(platform_t_phy_ns, default_phy, clk_ns, hw_inc) ( \
+(platform_t_phy_ns) ? ( \
+((DSI_CONVERT_T_PHY_NS_TO_T_PHY(platform_t_phy_ns, clk_ns, hw_inc)) < 0 ? 0 : \
+(DSI_CONVERT_T_PHY_NS_TO_T_PHY(platform_t_phy_ns, clk_ns, hw_inc)))) : \
+((default_phy) < 0 ? 0 : (default_phy)))
+
+static void tegra_dsi_get_clk_phy_timing(struct tegra_dc_dsi_data *dsi,
+		struct dsi_phy_timing_inclk *phy_timing_clk, u32 clk_ns)
+{
+	phy_timing_clk->t_tlpx = SELECT_T_PHY(
+		dsi->info.phy_timing.t_tlpx_ns,
+		T_TLPX_DEFAULT(clk_ns), clk_ns, T_TLPX_HW_INC);
+
+	phy_timing_clk->t_clktrail = SELECT_T_PHY(
+		dsi->info.phy_timing.t_clktrail_ns,
+		T_CLKTRAIL_DEFAULT(clk_ns), clk_ns, T_CLKTRAIL_HW_INC);
+
+	phy_timing_clk->t_clkpost = SELECT_T_PHY(
+		dsi->info.phy_timing.t_clkpost_ns,
+		T_CLKPOST_DEFAULT(clk_ns), clk_ns, T_CLKPOST_HW_INC);
+
+	phy_timing_clk->t_clkzero = SELECT_T_PHY(
+		dsi->info.phy_timing.t_clkzero_ns,
+		T_CLKZERO_DEFAULT(clk_ns), clk_ns, T_CLKZERO_HW_INC);
+
+	phy_timing_clk->t_clkprepare = SELECT_T_PHY(
+		dsi->info.phy_timing.t_clkprepare_ns,
+		T_CLKPREPARE_DEFAULT(clk_ns), clk_ns, T_CLKPREPARE_HW_INC);
+
+	phy_timing_clk->t_clkpre = SELECT_T_PHY(
+		dsi->info.phy_timing.t_clkpre_ns,
+		T_CLKPRE_DEFAULT, clk_ns, T_CLKPRE_HW_INC);
+}
+
+static void tegra_dsi_get_hs_phy_timing(struct tegra_dc_dsi_data *dsi,
+		struct dsi_phy_timing_inclk *phy_timing_clk, u32 clk_ns)
+{
+	phy_timing_clk->t_tlpx = SELECT_T_PHY(
+		dsi->info.phy_timing.t_tlpx_ns,
+		T_TLPX_DEFAULT(clk_ns), clk_ns, T_TLPX_HW_INC);
+
+	phy_timing_clk->t_hsdexit = SELECT_T_PHY(
+		dsi->info.phy_timing.t_hsdexit_ns,
+		T_HSEXIT_DEFAULT(clk_ns), clk_ns, T_HSEXIT_HW_INC);
+
+	phy_timing_clk->t_hstrail = SELECT_T_PHY(
+		dsi->info.phy_timing.t_hstrail_ns,
+		T_HSTRAIL_DEFAULT(clk_ns), clk_ns, T_HSTRAIL_HW_INC);
+
+	phy_timing_clk->t_datzero = SELECT_T_PHY(
+		dsi->info.phy_timing.t_datzero_ns,
+		T_DATZERO_DEFAULT(clk_ns), clk_ns, T_DATZERO_HW_INC);
+
+	phy_timing_clk->t_hsprepare = SELECT_T_PHY(
+		dsi->info.phy_timing.t_hsprepare_ns,
+		T_HSPREPARE_DEFAULT(clk_ns), clk_ns, T_HSPREPARE_HW_INC);
+}
+
+static void tegra_dsi_get_escape_phy_timing(struct tegra_dc_dsi_data *dsi,
+		struct dsi_phy_timing_inclk *phy_timing_clk, u32 clk_ns)
+{
+	phy_timing_clk->t_tlpx = SELECT_T_PHY(
+		dsi->info.phy_timing.t_tlpx_ns,
+		T_TLPX_DEFAULT(clk_ns), clk_ns, T_TLPX_HW_INC);
+}
+
+static void tegra_dsi_get_bta_phy_timing(struct tegra_dc_dsi_data *dsi,
+		struct dsi_phy_timing_inclk *phy_timing_clk, u32 clk_ns)
+{
+	phy_timing_clk->t_tlpx = SELECT_T_PHY(
+		dsi->info.phy_timing.t_tlpx_ns,
+		T_TLPX_DEFAULT(clk_ns), clk_ns, T_TLPX_HW_INC);
+
+	phy_timing_clk->t_taget = SELECT_T_PHY(
+		dsi->info.phy_timing.t_taget_ns,
+		T_TAGET_DEFAULT(clk_ns), clk_ns, T_TAGET_HW_INC);
+
+	phy_timing_clk->t_tasure = SELECT_T_PHY(
+		dsi->info.phy_timing.t_tasure_ns,
+		T_TASURE_DEFAULT(clk_ns), clk_ns, T_TASURE_HW_INC);
+
+	phy_timing_clk->t_tago = SELECT_T_PHY(
+		dsi->info.phy_timing.t_tago_ns,
+		T_TAGO_DEFAULT(clk_ns), clk_ns, T_TAGO_HW_INC);
+}
+
+static void tegra_dsi_get_ulps_phy_timing(struct tegra_dc_dsi_data *dsi,
+		struct dsi_phy_timing_inclk *phy_timing_clk, u32 clk_ns)
+{
+	phy_timing_clk->t_tlpx = SELECT_T_PHY(
+		dsi->info.phy_timing.t_tlpx_ns,
+		T_TLPX_DEFAULT(clk_ns), clk_ns, T_TLPX_HW_INC);
+
+	phy_timing_clk->t_wakeup = SELECT_T_PHY(
+		dsi->info.phy_timing.t_wakeup_ns,
+		T_WAKEUP_DEFAULT, clk_ns, T_WAKEUP_HW_INC);
+}
+
+#undef SELECT_T_PHY
+
+static void tegra_dsi_get_phy_timing(struct tegra_dc_dsi_data *dsi,
+				struct dsi_phy_timing_inclk *phy_timing_clk,
+				u32 clk_ns, u8 lphs)
+{
+	if (lphs == DSI_LPHS_IN_HS_MODE) {
+		tegra_dsi_get_clk_phy_timing(dsi, phy_timing_clk, clk_ns);
+		tegra_dsi_get_hs_phy_timing(dsi, phy_timing_clk, clk_ns);
+	} else {
+		/* default is LP mode */
+		tegra_dsi_get_escape_phy_timing(dsi, phy_timing_clk, clk_ns);
+		tegra_dsi_get_bta_phy_timing(dsi, phy_timing_clk, clk_ns);
+		tegra_dsi_get_ulps_phy_timing(dsi, phy_timing_clk, clk_ns);
+		if (dsi->info.enable_hs_clock_on_lp_cmd_mode)
+			tegra_dsi_get_clk_phy_timing
+				(dsi, phy_timing_clk, clk_ns);
+	}
+}
+
+static int tegra_dsi_mipi_phy_timing_range(struct tegra_dc_dsi_data *dsi,
+				struct dsi_phy_timing_inclk *phy_timing,
+				u32 clk_ns, u8 lphs)
+{
+#define CHECK_RANGE(val, min, max) ( \
+		((min) == NOT_DEFINED ? 0 : (val) < (min)) || \
+		((max) == NOT_DEFINED ? 0 : (val) > (max)) ? -EINVAL : 0)
+
+	int err = 0;
+
+	err = CHECK_RANGE(
+	DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_tlpx, clk_ns, T_TLPX_HW_INC),
+			MIPI_T_TLPX_NS_MIN, MIPI_T_TLPX_NS_MAX);
+	if (err < 0) {
+		dev_warn(&dsi->dc->ndev->dev,
+			"dsi: Tlpx mipi range violated\n");
+		goto fail;
+	}
+
+	if (lphs == DSI_LPHS_IN_HS_MODE) {
+		err = CHECK_RANGE(
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_hsdexit, clk_ns, T_HSEXIT_HW_INC),
+			MIPI_T_HSEXIT_NS_MIN, MIPI_T_HSEXIT_NS_MAX);
+		if (err < 0) {
+			dev_warn(&dsi->dc->ndev->dev,
+				"dsi: HsExit mipi range violated\n");
+			goto fail;
+		}
+
+		err = CHECK_RANGE(
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_hstrail, clk_ns, T_HSTRAIL_HW_INC),
+			MIPI_T_HSTRAIL_NS_MIN(clk_ns), MIPI_T_HSTRAIL_NS_MAX);
+		if (err < 0) {
+			dev_warn(&dsi->dc->ndev->dev,
+				"dsi: HsTrail mipi range violated\n");
+			goto fail;
+		}
+
+		err = CHECK_RANGE(
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_datzero, clk_ns, T_DATZERO_HW_INC),
+			MIPI_T_HSZERO_NS_MIN, MIPI_T_HSZERO_NS_MAX);
+		if (err < 0) {
+			dev_warn(&dsi->dc->ndev->dev,
+				"dsi: HsZero mipi range violated\n");
+			goto fail;
+		}
+
+		err = CHECK_RANGE(
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_hsprepare, clk_ns, T_HSPREPARE_HW_INC),
+			MIPI_T_HSPREPARE_NS_MIN(clk_ns),
+			MIPI_T_HSPREPARE_NS_MAX(clk_ns));
+		if (err < 0) {
+			dev_warn(&dsi->dc->ndev->dev,
+				"dsi: HsPrepare mipi range violated\n");
+			goto fail;
+		}
+
+		err = CHECK_RANGE(
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+		phy_timing->t_hsprepare, clk_ns, T_HSPREPARE_HW_INC) +
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_datzero, clk_ns, T_DATZERO_HW_INC),
+			MIPI_T_HSPREPARE_ADD_HSZERO_NS_MIN(clk_ns),
+			MIPI_T_HSPREPARE_ADD_HSZERO_NS_MAX);
+		if (err < 0) {
+			dev_warn(&dsi->dc->ndev->dev,
+			"dsi: HsPrepare + HsZero mipi range violated\n");
+			goto fail;
+		}
+	} else {
+		/* default is LP mode */
+		err = CHECK_RANGE(
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_wakeup, clk_ns, T_WAKEUP_HW_INC),
+			MIPI_T_WAKEUP_NS_MIN, MIPI_T_WAKEUP_NS_MAX);
+		if (err < 0) {
+			dev_warn(&dsi->dc->ndev->dev,
+				"dsi: WakeUp mipi range violated\n");
+			goto fail;
+		}
+
+		err = CHECK_RANGE(
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_tasure, clk_ns, T_TASURE_HW_INC),
+			MIPI_T_TASURE_NS_MIN(DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_tlpx, clk_ns, T_TLPX_HW_INC)),
+			MIPI_T_TASURE_NS_MAX(DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_tlpx, clk_ns, T_TLPX_HW_INC)));
+		if (err < 0) {
+			dev_warn(&dsi->dc->ndev->dev,
+				"dsi: TaSure mipi range violated\n");
+			goto fail;
+		}
+	}
+
+	if (lphs == DSI_LPHS_IN_HS_MODE ||
+		dsi->info.enable_hs_clock_on_lp_cmd_mode) {
+		err = CHECK_RANGE(
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_clktrail, clk_ns, T_CLKTRAIL_HW_INC),
+			MIPI_T_CLKTRAIL_NS_MIN, MIPI_T_CLKTRAIL_NS_MAX);
+		if (err < 0) {
+			dev_warn(&dsi->dc->ndev->dev,
+				"dsi: ClkTrail mipi range violated\n");
+			goto fail;
+		}
+
+		err = CHECK_RANGE(
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_clkpost, clk_ns, T_CLKPOST_HW_INC),
+			MIPI_T_CLKPOST_NS_MIN(clk_ns), MIPI_T_CLKPOST_NS_MAX);
+		if (err < 0) {
+			dev_warn(&dsi->dc->ndev->dev,
+				"dsi: ClkPost mipi range violated\n");
+			goto fail;
+		}
+
+		err = CHECK_RANGE(
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_clkzero, clk_ns, T_CLKZERO_HW_INC),
+			MIPI_T_CLKZERO_NS_MIN, MIPI_T_CLKZERO_NS_MAX);
+		if (err < 0) {
+			dev_warn(&dsi->dc->ndev->dev,
+				"dsi: ClkZero mipi range violated\n");
+			goto fail;
+		}
+
+		err = CHECK_RANGE(
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_clkprepare, clk_ns, T_CLKPREPARE_HW_INC),
+			MIPI_T_CLKPREPARE_NS_MIN, MIPI_T_CLKPREPARE_NS_MAX);
+		if (err < 0) {
+			dev_warn(&dsi->dc->ndev->dev,
+				"dsi: ClkPrepare mipi range violated\n");
+			goto fail;
+		}
+
+		err = CHECK_RANGE(
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_clkpre, clk_ns, T_CLKPRE_HW_INC),
+			MIPI_T_CLKPRE_NS_MIN, MIPI_T_CLKPRE_NS_MAX);
+		if (err < 0) {
+			dev_warn(&dsi->dc->ndev->dev,
+				"dsi: ClkPre mipi range violated\n");
+			goto fail;
+		}
+
+		err = CHECK_RANGE(
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+		phy_timing->t_clkprepare, clk_ns, T_CLKPREPARE_HW_INC) +
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+			phy_timing->t_clkzero, clk_ns, T_CLKZERO_HW_INC),
+			MIPI_T_CLKPREPARE_ADD_CLKZERO_NS_MIN,
+			MIPI_T_CLKPREPARE_ADD_CLKZERO_NS_MAX);
+		if (err < 0) {
+			dev_warn(&dsi->dc->ndev->dev,
+			"dsi: ClkPrepare + ClkZero mipi range violated\n");
+			goto fail;
+		}
+	}
+fail:
+#undef CHECK_RANGE
+	return err;
+}
+
+static int tegra_dsi_hs_phy_len(struct tegra_dc_dsi_data *dsi,
+				struct dsi_phy_timing_inclk *phy_timing,
+				u32 clk_ns, u8 lphs)
+{
+	u32 hs_t_phy_ns;
+	u32 clk_t_phy_ns;
+	u32 t_phy_ns;
+	u32 h_blank_ns;
+	struct tegra_dc_mode *modes;
+	u32 t_pix_ns;
+	int err = 0;
+
+	if (!(lphs == DSI_LPHS_IN_HS_MODE))
+		goto fail;
+
+	modes = dsi->dc->out->modes;
+	t_pix_ns = clk_ns * BITS_PER_BYTE *
+		dsi->pixel_scaler_mul / dsi->pixel_scaler_div;
+
+	hs_t_phy_ns =
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+		phy_timing->t_tlpx, clk_ns, T_TLPX_HW_INC) +
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+		phy_timing->t_hsprepare, clk_ns, T_HSPREPARE_HW_INC) +
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+		phy_timing->t_datzero, clk_ns, T_DATZERO_HW_INC) +
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+		phy_timing->t_hstrail, clk_ns, T_HSTRAIL_HW_INC) +
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+		phy_timing->t_hsdexit, clk_ns, T_HSEXIT_HW_INC);
+
+	clk_t_phy_ns =
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+		phy_timing->t_clkpost, clk_ns, T_CLKPOST_HW_INC) +
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+		phy_timing->t_clktrail, clk_ns, T_CLKTRAIL_HW_INC) +
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+		phy_timing->t_hsdexit, clk_ns, T_HSEXIT_HW_INC) +
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+		phy_timing->t_tlpx, clk_ns, T_TLPX_HW_INC) +
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+		phy_timing->t_clkprepare, clk_ns, T_CLKPREPARE_HW_INC) +
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+		phy_timing->t_clkzero, clk_ns, T_CLKZERO_HW_INC) +
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+		phy_timing->t_clkpre, clk_ns, T_CLKPRE_HW_INC);
+
+	h_blank_ns = t_pix_ns * (modes->h_sync_width + modes->h_back_porch +
+						modes->h_front_porch);
+
+	/* Extra tlpx and byte cycle required by dsi HW */
+	t_phy_ns = dsi->info.n_data_lanes * (hs_t_phy_ns + clk_t_phy_ns +
+		DSI_CONVERT_T_PHY_TO_T_PHY_NS(
+		phy_timing->t_tlpx, clk_ns, T_TLPX_HW_INC) +
+		clk_ns * BITS_PER_BYTE);
+
+	if (h_blank_ns < t_phy_ns) {
+		err = -EINVAL;
+		dev_err(&dsi->dc->ndev->dev,
+			"dsi: Hblank is smaller than HS trans phy timing\n");
+		goto fail;
+	}
+
+	return 0;
+fail:
+	return err;
+}
+
+static int tegra_dsi_constraint_phy_timing(struct tegra_dc_dsi_data *dsi,
+				struct dsi_phy_timing_inclk *phy_timing,
+				u32 clk_ns, u8 lphs)
+{
+	int err = 0;
+
+	err = tegra_dsi_mipi_phy_timing_range(dsi, phy_timing, clk_ns, lphs);
+	if (err < 0) {
+		dev_warn(&dsi->dc->ndev->dev, "dsi: mipi range violated\n");
+		goto fail;
+	}
+
+	err = tegra_dsi_hs_phy_len(dsi, phy_timing, clk_ns, lphs);
+	if (err < 0) {
+		dev_err(&dsi->dc->ndev->dev, "dsi: Hblank too short\n");
+		goto fail;
+	}
+
+	/* TODO: add more contraints */
+fail:
+	return err;
+}
+
+static void tegra_dsi_set_phy_timing(struct tegra_dc_dsi_data *dsi, u8 lphs)
+{
+	u32 val;
+	struct dsi_phy_timing_inclk phy_timing = dsi->phy_timing;
+
+	tegra_dsi_get_phy_timing
+		(dsi, &phy_timing, dsi->current_bit_clk_ns, lphs);
+
+	tegra_dsi_constraint_phy_timing(dsi, &phy_timing,
+					dsi->current_bit_clk_ns, lphs);
+
+	val = DSI_PHY_TIMING_0_THSDEXIT(phy_timing.t_hsdexit) |
+			DSI_PHY_TIMING_0_THSTRAIL(phy_timing.t_hstrail) |
+			DSI_PHY_TIMING_0_TDATZERO(phy_timing.t_datzero) |
+			DSI_PHY_TIMING_0_THSPREPR(phy_timing.t_hsprepare);
+	tegra_dsi_writel(dsi, val, DSI_PHY_TIMING_0);
+
+	val = DSI_PHY_TIMING_1_TCLKTRAIL(phy_timing.t_clktrail) |
+			DSI_PHY_TIMING_1_TCLKPOST(phy_timing.t_clkpost) |
+			DSI_PHY_TIMING_1_TCLKZERO(phy_timing.t_clkzero) |
+			DSI_PHY_TIMING_1_TTLPX(phy_timing.t_tlpx);
+	tegra_dsi_writel(dsi, val, DSI_PHY_TIMING_1);
+
+	val = DSI_PHY_TIMING_2_TCLKPREPARE(phy_timing.t_clkprepare) |
+		DSI_PHY_TIMING_2_TCLKPRE(phy_timing.t_clkpre) |
+			DSI_PHY_TIMING_2_TWAKEUP(phy_timing.t_wakeup);
+	tegra_dsi_writel(dsi, val, DSI_PHY_TIMING_2);
+
+	val = DSI_BTA_TIMING_TTAGET(phy_timing.t_taget) |
+			DSI_BTA_TIMING_TTASURE(phy_timing.t_tasure) |
+			DSI_BTA_TIMING_TTAGO(phy_timing.t_tago);
+	tegra_dsi_writel(dsi, val, DSI_BTA_TIMING);
+
+	dsi->phy_timing = phy_timing;
+}
+
+static u32 tegra_dsi_sol_delay_burst(struct tegra_dc *dc,
+				struct tegra_dc_dsi_data *dsi)
+{
+	u32 dsi_to_pixel_clk_ratio;
+	u32 temp;
+	u32 temp1;
+	u32 mipi_clk_adj_kHz = 0;
+	u32 sol_delay;
+	struct tegra_dc_mode *dc_modes = &dc->mode;
+
+	/* Get Fdsi/Fpixel ration (note: Fdsi is in bit format) */
+	dsi_to_pixel_clk_ratio = (dsi->current_dsi_clk_khz * 2 +
+		dsi->default_pixel_clk_khz - 1) / dsi->default_pixel_clk_khz;
+
+	/* Convert Fdsi to byte format */
+	dsi_to_pixel_clk_ratio *= 1000/8;
+
+	/* Multiplying by 1000 so that we don't loose the fraction part */
+	temp = dc_modes->h_active * 1000;
+	temp1 = dc_modes->h_active + dc_modes->h_back_porch +
+			dc_modes->h_sync_width;
+
+	sol_delay = temp1 * dsi_to_pixel_clk_ratio -
+			temp * dsi->pixel_scaler_mul /
+			(dsi->pixel_scaler_div * dsi->info.n_data_lanes);
+
+	/* Do rounding on sol delay */
+	sol_delay = (sol_delay + 1000 - 1)/1000;
+
+	/* TODO:
+	 * 1. find out the correct sol fifo depth to use
+	 * 2. verify with hw about the clamping function
+	 */
+	if (sol_delay > (480 * 4)) {
+		sol_delay = (480 * 4);
+		mipi_clk_adj_kHz = sol_delay +
+			(dc_modes->h_active * dsi->pixel_scaler_mul) /
+			(dsi->info.n_data_lanes * dsi->pixel_scaler_div);
+
+		mipi_clk_adj_kHz *= (dsi->default_pixel_clk_khz / temp1);
+
+		mipi_clk_adj_kHz *= 4;
+	}
+
+	dsi->target_hs_clk_khz = mipi_clk_adj_kHz;
+
+	return sol_delay;
+}
+
+static void tegra_dsi_set_sol_delay(struct tegra_dc *dc,
+				struct tegra_dc_dsi_data *dsi)
+{
+	u32 sol_delay;
+
+	if (dsi->info.video_burst_mode == TEGRA_DSI_VIDEO_NONE_BURST_MODE ||
+		dsi->info.video_burst_mode ==
+				TEGRA_DSI_VIDEO_NONE_BURST_MODE_WITH_SYNC_END) {
+#define VIDEO_FIFO_LATENCY_PIXEL_CLK 8
+		sol_delay = VIDEO_FIFO_LATENCY_PIXEL_CLK *
+			dsi->pixel_scaler_mul / dsi->pixel_scaler_div;
+#undef VIDEO_FIFO_LATENCY_PIXEL_CLK
+		dsi->status.clk_burst = DSI_CLK_BURST_NONE_BURST;
+	} else {
+		sol_delay = tegra_dsi_sol_delay_burst(dc, dsi);
+		dsi->status.clk_burst = DSI_CLK_BURST_BURST_MODE;
+	}
+
+	tegra_dsi_writel(dsi, DSI_SOL_DELAY_SOL_DELAY(sol_delay),
+								DSI_SOL_DELAY);
+}
+
+static void tegra_dsi_set_timeout(struct tegra_dc_dsi_data *dsi)
+{
+	u32 val;
+	u32 bytes_per_frame;
+	u32 timeout = 0;
+
+	/* TODO: verify the following equation */
+	bytes_per_frame = dsi->current_dsi_clk_khz * 1000 * 2 /
+						(dsi->info.refresh_rate * 8);
+	timeout = bytes_per_frame / DSI_CYCLE_COUNTER_VALUE;
+	timeout = (timeout + DSI_HTX_TO_MARGIN) & 0xffff;
+
+	val = DSI_TIMEOUT_0_LRXH_TO(DSI_LRXH_TO_VALUE) |
+			DSI_TIMEOUT_0_HTX_TO(timeout);
+	tegra_dsi_writel(dsi, val, DSI_TIMEOUT_0);
+
+	if (dsi->info.panel_reset_timeout_msec)
+		timeout = (dsi->info.panel_reset_timeout_msec * 1000*1000)
+					/ dsi->current_bit_clk_ns;
+	else
+		timeout = DSI_PR_TO_VALUE;
+
+	val = DSI_TIMEOUT_1_PR_TO(timeout) |
+		DSI_TIMEOUT_1_TA_TO(DSI_TA_TO_VALUE);
+	tegra_dsi_writel(dsi, val, DSI_TIMEOUT_1);
+
+	val = DSI_TO_TALLY_P_RESET_STATUS(IN_RESET) |
+		DSI_TO_TALLY_TA_TALLY(DSI_TA_TALLY_VALUE)|
+		DSI_TO_TALLY_LRXH_TALLY(DSI_LRXH_TALLY_VALUE)|
+		DSI_TO_TALLY_HTX_TALLY(DSI_HTX_TALLY_VALUE);
+	tegra_dsi_writel(dsi, val, DSI_TO_TALLY);
+}
+
+static void tegra_dsi_setup_video_mode_pkt_length(struct tegra_dc *dc,
+						struct tegra_dc_dsi_data *dsi)
+{
+	u32 val;
+	u32 hact_pkt_len;
+	u32 hsa_pkt_len;
+	u32 hbp_pkt_len;
+	u32 hfp_pkt_len;
+
+	hact_pkt_len = dc->mode.h_active * dsi->pixel_scaler_mul /
+							dsi->pixel_scaler_div;
+	hsa_pkt_len = dc->mode.h_sync_width * dsi->pixel_scaler_mul /
+							dsi->pixel_scaler_div;
+	hbp_pkt_len = dc->mode.h_back_porch * dsi->pixel_scaler_mul /
+							dsi->pixel_scaler_div;
+	hfp_pkt_len = dc->mode.h_front_porch * dsi->pixel_scaler_mul /
+							dsi->pixel_scaler_div;
+
+	if (dsi->info.video_burst_mode !=
+				TEGRA_DSI_VIDEO_NONE_BURST_MODE_WITH_SYNC_END)
+		hbp_pkt_len += hsa_pkt_len;
+
+	hsa_pkt_len -= DSI_HSYNC_BLNK_PKT_OVERHEAD;
+	hbp_pkt_len -= DSI_HBACK_PORCH_PKT_OVERHEAD;
+	hfp_pkt_len -= DSI_HFRONT_PORCH_PKT_OVERHEAD;
+
+	val = DSI_PKT_LEN_0_1_LENGTH_0(0) |
+			DSI_PKT_LEN_0_1_LENGTH_1(hsa_pkt_len);
+	tegra_dsi_writel(dsi, val, DSI_PKT_LEN_0_1);
+
+	val = DSI_PKT_LEN_2_3_LENGTH_2(hbp_pkt_len) |
+			DSI_PKT_LEN_2_3_LENGTH_3(hact_pkt_len);
+	tegra_dsi_writel(dsi, val, DSI_PKT_LEN_2_3);
+
+	val = DSI_PKT_LEN_4_5_LENGTH_4(hfp_pkt_len) |
+			DSI_PKT_LEN_4_5_LENGTH_5(0);
+	tegra_dsi_writel(dsi, val, DSI_PKT_LEN_4_5);
+
+	val = DSI_PKT_LEN_6_7_LENGTH_6(0) | DSI_PKT_LEN_6_7_LENGTH_7(0);
+	tegra_dsi_writel(dsi, val, DSI_PKT_LEN_6_7);
+}
+
+static void tegra_dsi_setup_cmd_mode_pkt_length(struct tegra_dc *dc,
+						struct tegra_dc_dsi_data *dsi)
+{
+	unsigned long	val;
+	unsigned long	act_bytes;
+
+	act_bytes = dc->mode.h_active * dsi->pixel_scaler_mul /
+			dsi->pixel_scaler_div + 1;
+
+	val = DSI_PKT_LEN_0_1_LENGTH_0(0) | DSI_PKT_LEN_0_1_LENGTH_1(0);
+	tegra_dsi_writel(dsi, val, DSI_PKT_LEN_0_1);
+
+	val = DSI_PKT_LEN_2_3_LENGTH_2(0) | DSI_PKT_LEN_2_3_LENGTH_3(act_bytes);
+	tegra_dsi_writel(dsi, val, DSI_PKT_LEN_2_3);
+
+	val = DSI_PKT_LEN_4_5_LENGTH_4(0) | DSI_PKT_LEN_4_5_LENGTH_5(act_bytes);
+	tegra_dsi_writel(dsi, val, DSI_PKT_LEN_4_5);
+
+	val = DSI_PKT_LEN_6_7_LENGTH_6(0) | DSI_PKT_LEN_6_7_LENGTH_7(0x0f0f);
+	tegra_dsi_writel(dsi, val, DSI_PKT_LEN_6_7);
+}
+
+static void tegra_dsi_set_pkt_length(struct tegra_dc *dc,
+				struct tegra_dc_dsi_data *dsi)
+{
+	if (dsi->driven_mode == TEGRA_DSI_DRIVEN_BY_HOST)
+		return;
+
+	if (dsi->info.video_data_type == TEGRA_DSI_VIDEO_TYPE_VIDEO_MODE)
+		tegra_dsi_setup_video_mode_pkt_length(dc, dsi);
+	else
+		tegra_dsi_setup_cmd_mode_pkt_length(dc, dsi);
+}
+
+static void tegra_dsi_set_pkt_seq(struct tegra_dc *dc,
+				struct tegra_dc_dsi_data *dsi)
+{
+	const u32 *pkt_seq;
+	u32 rgb_info;
+	u32 pkt_seq_3_5_rgb_lo;
+	u32 pkt_seq_3_5_rgb_hi;
+	u32	val;
+	u32 reg;
+	u8  i;
+
+	if (dsi->driven_mode == TEGRA_DSI_DRIVEN_BY_HOST)
+		return;
+
+	switch (dsi->info.pixel_format) {
+	case TEGRA_DSI_PIXEL_FORMAT_16BIT_P:
+		rgb_info = CMD_RGB_16BPP;
+		break;
+	case TEGRA_DSI_PIXEL_FORMAT_18BIT_P:
+		rgb_info = CMD_RGB_18BPP;
+		break;
+	case TEGRA_DSI_PIXEL_FORMAT_18BIT_NP:
+		rgb_info = CMD_RGB_18BPPNP;
+		break;
+	case TEGRA_DSI_PIXEL_FORMAT_24BIT_P:
+	default:
+		rgb_info = CMD_RGB_24BPP;
+		break;
+	}
+
+	pkt_seq_3_5_rgb_lo = 0;
+	pkt_seq_3_5_rgb_hi = 0;
+	if (dsi->info.video_data_type == TEGRA_DSI_VIDEO_TYPE_COMMAND_MODE)
+		pkt_seq = dsi_pkt_seq_cmd_mode;
+	else {
+		switch (dsi->info.video_burst_mode) {
+		case TEGRA_DSI_VIDEO_BURST_MODE_LOWEST_SPEED:
+		case TEGRA_DSI_VIDEO_BURST_MODE_LOW_SPEED:
+		case TEGRA_DSI_VIDEO_BURST_MODE_MEDIUM_SPEED:
+		case TEGRA_DSI_VIDEO_BURST_MODE_FAST_SPEED:
+		case TEGRA_DSI_VIDEO_BURST_MODE_FASTEST_SPEED:
+			pkt_seq_3_5_rgb_lo =
+					DSI_PKT_SEQ_3_LO_PKT_32_ID(rgb_info);
+			if (!dsi->info.no_pkt_seq_eot)
+				pkt_seq = dsi_pkt_seq_video_burst;
+			else
+				pkt_seq = dsi_pkt_seq_video_burst_no_eot;
+			break;
+		case TEGRA_DSI_VIDEO_NONE_BURST_MODE_WITH_SYNC_END:
+			pkt_seq_3_5_rgb_hi =
+					DSI_PKT_SEQ_3_HI_PKT_34_ID(rgb_info);
+			pkt_seq = dsi_pkt_seq_video_non_burst_syne;
+			break;
+		case TEGRA_DSI_VIDEO_NONE_BURST_MODE:
+		default:
+			pkt_seq_3_5_rgb_lo =
+					DSI_PKT_SEQ_3_LO_PKT_32_ID(rgb_info);
+			pkt_seq = dsi_pkt_seq_video_non_burst;
+			break;
+		}
+	}
+
+	for (i = 0; i < NUMOF_PKT_SEQ; i++) {
+		val = pkt_seq[i];
+		reg = dsi_pkt_seq_reg[i];
+		if ((reg == DSI_PKT_SEQ_3_LO) || (reg == DSI_PKT_SEQ_5_LO))
+			val |= pkt_seq_3_5_rgb_lo;
+		if ((reg == DSI_PKT_SEQ_3_HI) || (reg == DSI_PKT_SEQ_5_HI))
+			val |= pkt_seq_3_5_rgb_hi;
+		tegra_dsi_writel(dsi, val, reg);
+	}
+}
+
+static void tegra_dsi_reset_underflow_overflow
+				(struct tegra_dc_dsi_data *dsi)
+{
+	u32 val;
+
+	val = tegra_dsi_readl(dsi, DSI_STATUS);
+	val &= (DSI_STATUS_LB_OVERFLOW(0x1) | DSI_STATUS_LB_UNDERFLOW(0x1));
+	if (val) {
+		if (val & DSI_STATUS_LB_OVERFLOW(0x1))
+			dev_warn(&dsi->dc->ndev->dev,
+				"dsi: video fifo overflow. Resetting flag\n");
+		if (val & DSI_STATUS_LB_UNDERFLOW(0x1))
+			dev_warn(&dsi->dc->ndev->dev,
+				"dsi: video fifo underflow. Resetting flag\n");
+		val = tegra_dsi_readl(dsi, DSI_HOST_DSI_CONTROL);
+		val |= DSI_HOST_CONTROL_FIFO_STAT_RESET(0x1);
+		tegra_dsi_writel(dsi, val, DSI_HOST_DSI_CONTROL);
+		udelay(5);
+	}
+}
+
+static void tegra_dsi_soft_reset(struct tegra_dc_dsi_data *dsi)
+{
+	u32 trigger;
+
+	tegra_dsi_writel(dsi,
+		DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_DISABLE),
+		DSI_POWER_CONTROL);
+	/* stabilization delay */
+	udelay(300);
+
+	tegra_dsi_writel(dsi,
+		DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_ENABLE),
+		DSI_POWER_CONTROL);
+	/* stabilization delay */
+	udelay(300);
+
+	/* dsi HW does not clear host trigger bit automatically
+	 * on dsi interface disable if host fifo is empty or in mid
+	 * of host transmission
+	 */
+	trigger = tegra_dsi_readl(dsi, DSI_TRIGGER);
+	if (trigger)
+		tegra_dsi_writel(dsi, 0x0, DSI_TRIGGER);
+}
+
+static void tegra_dsi_stop_dc_stream(struct tegra_dc *dc,
+					struct tegra_dc_dsi_data *dsi)
+{
+	/* Mask the MSF interrupt. */
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
+		tegra_dc_mask_interrupt(dc, MSF_INT);
+
+	tegra_dc_writel(dc, DISP_CTRL_MODE_STOP, DC_CMD_DISPLAY_COMMAND);
+	tegra_dc_writel(dc, 0, DC_DISP_DISP_WIN_OPTIONS);
+	tegra_dc_writel(dc, GENERAL_UPDATE, DC_CMD_STATE_CONTROL);
+	tegra_dc_writel(dc, GENERAL_ACT_REQ , DC_CMD_STATE_CONTROL);
+
+	dsi->status.dc_stream = DSI_DC_STREAM_DISABLE;
+}
+
+static void tegra_dsi_stop_dc_stream_at_frame_end(struct tegra_dc *dc,
+						struct tegra_dc_dsi_data *dsi)
+{
+	int val;
+	long timeout;
+	u32 frame_period = DIV_ROUND_UP(S_TO_MS(1), dsi->info.refresh_rate);
+
+	INIT_COMPLETION(dc->frame_end_complete);
+
+	/* unmask frame end interrupt */
+	val = tegra_dc_readl(dc, DC_CMD_INT_MASK);
+	tegra_dc_writel(dc, val | FRAME_END_INT, DC_CMD_INT_MASK);
+
+	tegra_dsi_stop_dc_stream(dc, dsi);
+
+	/* wait for frame_end completion.
+	 * timeout is 2 frame duration to accomodate for
+	 * internal delay.
+	 */
+	timeout = wait_for_completion_interruptible_timeout(
+			&dc->frame_end_complete,
+			msecs_to_jiffies(2 * frame_period));
+
+	/* give 2 line time to dsi HW to catch up
+	 * with pixels sent by dc
+	 */
+	udelay(50);
+
+	tegra_dsi_soft_reset(dsi);
+
+	/* reinstate interrupt mask */
+	tegra_dc_writel(dc, val, DC_CMD_INT_MASK);
+
+	if (timeout == 0)
+		dev_warn(&dc->ndev->dev,
+			"DC doesn't stop at end of frame.\n");
+
+	tegra_dsi_reset_underflow_overflow(dsi);
+}
+
+static void tegra_dsi_start_dc_stream(struct tegra_dc *dc,
+					struct tegra_dc_dsi_data *dsi)
+{
+	u32 val;
+
+	tegra_dc_writel(dc, DSI_ENABLE, DC_DISP_DISP_WIN_OPTIONS);
+
+	/* TODO: clean up */
+	tegra_dc_writel(dc, PW0_ENABLE | PW1_ENABLE | PW2_ENABLE | PW3_ENABLE |
+			PW4_ENABLE | PM0_ENABLE | PM1_ENABLE,
+			DC_CMD_DISPLAY_POWER_CONTROL);
+
+	/* Configure one-shot mode or continuous mode */
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE) {
+		/* disable LSPI/LCD_DE output */
+		val = PIN_OUTPUT_LSPI_OUTPUT_DIS;
+		tegra_dc_writel(dc, val, DC_COM_PIN_OUTPUT_ENABLE3);
+
+		/* enable MSF & set MSF polarity */
+		val = MSF_ENABLE | MSF_LSPI;
+		if (!dsi->info.te_polarity_low)
+			val |= MSF_POLARITY_HIGH;
+		else
+			val |= MSF_POLARITY_LOW;
+		tegra_dc_writel(dc, val, DC_CMD_DISPLAY_COMMAND_OPTION0);
+
+		/* set non-continuous mode */
+		tegra_dc_writel(dc, DISP_CTRL_MODE_NC_DISPLAY,
+						DC_CMD_DISPLAY_COMMAND);
+		tegra_dc_writel(dc, GENERAL_UPDATE, DC_CMD_STATE_CONTROL);
+		tegra_dc_writel(dc, GENERAL_ACT_REQ | NC_HOST_TRIG,
+						DC_CMD_STATE_CONTROL);
+
+		/* Unmask the MSF interrupt. */
+		tegra_dc_unmask_interrupt(dc, MSF_INT);
+	} else {
+		/* set continuous mode */
+		tegra_dc_writel(dc, DISP_CTRL_MODE_C_DISPLAY,
+						DC_CMD_DISPLAY_COMMAND);
+		tegra_dc_writel(dc, GENERAL_UPDATE, DC_CMD_STATE_CONTROL);
+		tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
+	}
+
+	dsi->status.dc_stream = DSI_DC_STREAM_ENABLE;
+}
+
+static void tegra_dsi_set_dc_clk(struct tegra_dc *dc,
+				struct tegra_dc_dsi_data *dsi)
+{
+	u32 shift_clk_div_register;
+	u32 val;
+
+	/* Get the corresponding register value of shift_clk_div. */
+	shift_clk_div_register = dsi->shift_clk_div * 2 - 2;
+
+#ifndef CONFIG_TEGRA_SILICON_PLATFORM
+	shift_clk_div_register = 1;
+#endif
+
+	/* TODO: find out if PCD3 option is required */
+	val = PIXEL_CLK_DIVIDER_PCD1 |
+		SHIFT_CLK_DIVIDER(shift_clk_div_register);
+	tegra_dc_writel(dc, val, DC_DISP_DISP_CLOCK_CONTROL);
+}
+
+static void tegra_dsi_set_dsi_clk(struct tegra_dc *dc,
+			struct tegra_dc_dsi_data *dsi, u32 clk)
+{
+	u32 rm;
+
+	/* Round up to MHz */
+	rm = clk % 1000;
+	if (rm != 0)
+		clk -= rm;
+
+	/* Set up pixel clock */
+	dc->shift_clk_div = dsi->shift_clk_div;
+	dc->mode.pclk = (clk * 1000) / dsi->shift_clk_div;
+	/* TODO: Define one shot work delay in board file. */
+	/* Since for one-shot mode, refresh rate is usually set larger than
+	 * expected refresh rate, it needs at least 3 frame period. Less
+	 * delay one shot work is, more powering saving we have. */
+	dc->one_shot_delay_ms = 4 *
+			DIV_ROUND_UP(S_TO_MS(1), dsi->info.refresh_rate);
+
+	/* Enable DSI clock */
+	tegra_dc_setup_clk(dc, dsi->dsi_clk);
+	tegra_dsi_clk_enable(dsi);
+	tegra_periph_reset_deassert(dsi->dsi_clk);
+
+	dsi->current_dsi_clk_khz = clk_get_rate(dsi->dsi_clk) / 1000;
+	dsi->current_bit_clk_ns =  1000*1000 / (dsi->current_dsi_clk_khz * 2);
+}
+
+static void tegra_dsi_hs_clk_out_enable(struct tegra_dc_dsi_data *dsi)
+{
+	u32 val;
+
+	val = tegra_dsi_readl(dsi, DSI_CONTROL);
+	val &= ~DSI_CONTROL_HS_CLK_CTRL(1);
+
+	if (dsi->info.video_clock_mode == TEGRA_DSI_VIDEO_CLOCK_CONTINUOUS) {
+		val |= DSI_CONTROL_HS_CLK_CTRL(CONTINUOUS);
+		dsi->status.clk_mode = DSI_PHYCLK_CONTINUOUS;
+	} else {
+		val |= DSI_CONTROL_HS_CLK_CTRL(TX_ONLY);
+		dsi->status.clk_mode = DSI_PHYCLK_TX_ONLY;
+	}
+	tegra_dsi_writel(dsi, val, DSI_CONTROL);
+
+	val = tegra_dsi_readl(dsi, DSI_HOST_DSI_CONTROL);
+	val &= ~DSI_HOST_DSI_CONTROL_HIGH_SPEED_TRANS(1);
+	val |= DSI_HOST_DSI_CONTROL_HIGH_SPEED_TRANS(TEGRA_DSI_HIGH);
+	tegra_dsi_writel(dsi, val, DSI_HOST_DSI_CONTROL);
+
+	dsi->status.clk_out = DSI_PHYCLK_OUT_EN;
+}
+
+static void tegra_dsi_hs_clk_out_enable_in_lp(struct tegra_dc_dsi_data *dsi)
+{
+	u32 val;
+	tegra_dsi_hs_clk_out_enable(dsi);
+
+	val = tegra_dsi_readl(dsi, DSI_HOST_DSI_CONTROL);
+	val &= ~DSI_HOST_DSI_CONTROL_HIGH_SPEED_TRANS(1);
+	val |= DSI_HOST_DSI_CONTROL_HIGH_SPEED_TRANS(TEGRA_DSI_LOW);
+	tegra_dsi_writel(dsi, val, DSI_HOST_DSI_CONTROL);
+}
+
+static void tegra_dsi_hs_clk_out_disable(struct tegra_dc *dc,
+						struct tegra_dc_dsi_data *dsi)
+{
+	u32 val;
+
+	if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
+		tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
+
+	tegra_dsi_writel(dsi, TEGRA_DSI_DISABLE, DSI_POWER_CONTROL);
+	/* stabilization delay */
+	udelay(300);
+
+	val = tegra_dsi_readl(dsi, DSI_HOST_DSI_CONTROL);
+	val &= ~DSI_HOST_DSI_CONTROL_HIGH_SPEED_TRANS(1);
+	val |= DSI_HOST_DSI_CONTROL_HIGH_SPEED_TRANS(TEGRA_DSI_LOW);
+	tegra_dsi_writel(dsi, val, DSI_HOST_DSI_CONTROL);
+
+	tegra_dsi_writel(dsi, TEGRA_DSI_ENABLE, DSI_POWER_CONTROL);
+	/* stabilization delay */
+	udelay(300);
+
+	dsi->status.clk_mode = DSI_PHYCLK_NOT_INIT;
+	dsi->status.clk_out = DSI_PHYCLK_OUT_DIS;
+}
+
+static void tegra_dsi_set_control_reg_lp(struct tegra_dc_dsi_data *dsi)
+{
+	u32 dsi_control;
+	u32 host_dsi_control;
+	u32 max_threshold;
+
+	dsi_control = dsi->dsi_control_val | DSI_CTRL_HOST_DRIVEN;
+	host_dsi_control = HOST_DSI_CTRL_COMMON |
+			HOST_DSI_CTRL_HOST_DRIVEN |
+			DSI_HOST_DSI_CONTROL_HIGH_SPEED_TRANS(TEGRA_DSI_LOW);
+	max_threshold = DSI_MAX_THRESHOLD_MAX_THRESHOLD(DSI_HOST_FIFO_DEPTH);
+
+	tegra_dsi_writel(dsi, max_threshold, DSI_MAX_THRESHOLD);
+	tegra_dsi_writel(dsi, dsi_control, DSI_CONTROL);
+	tegra_dsi_writel(dsi, host_dsi_control, DSI_HOST_DSI_CONTROL);
+
+	dsi->status.driven = DSI_DRIVEN_MODE_HOST;
+	dsi->status.clk_burst = DSI_CLK_BURST_NOT_INIT;
+	dsi->status.vtype = DSI_VIDEO_TYPE_NOT_INIT;
+}
+
+static void tegra_dsi_set_control_reg_hs(struct tegra_dc_dsi_data *dsi,
+						u8 driven_mode)
+{
+	u32 dsi_control;
+	u32 host_dsi_control;
+	u32 max_threshold;
+	u32 dcs_cmd;
+
+	dsi_control = dsi->dsi_control_val;
+	host_dsi_control = HOST_DSI_CTRL_COMMON;
+	max_threshold = 0;
+	dcs_cmd = 0;
+
+	if (driven_mode == TEGRA_DSI_DRIVEN_BY_HOST) {
+		dsi_control |= DSI_CTRL_HOST_DRIVEN;
+		host_dsi_control |= HOST_DSI_CTRL_HOST_DRIVEN;
+		max_threshold =
+			DSI_MAX_THRESHOLD_MAX_THRESHOLD(DSI_HOST_FIFO_DEPTH);
+		dsi->status.driven = DSI_DRIVEN_MODE_HOST;
+	} else {
+		dsi_control |= DSI_CTRL_DC_DRIVEN;
+		host_dsi_control |= HOST_DSI_CTRL_DC_DRIVEN;
+		max_threshold =
+			DSI_MAX_THRESHOLD_MAX_THRESHOLD(DSI_VIDEO_FIFO_DEPTH);
+		dsi->status.driven = DSI_DRIVEN_MODE_DC;
+
+		if (dsi->info.video_data_type ==
+			TEGRA_DSI_VIDEO_TYPE_COMMAND_MODE) {
+			dsi_control |= DSI_CTRL_CMD_MODE;
+			dcs_cmd = DSI_DCS_CMDS_LT5_DCS_CMD(
+				DSI_WRITE_MEMORY_START)|
+				DSI_DCS_CMDS_LT3_DCS_CMD(
+				DSI_WRITE_MEMORY_CONTINUE);
+			dsi->status.vtype = DSI_VIDEO_TYPE_CMD_MODE;
+		} else {
+			dsi_control |= DSI_CTRL_VIDEO_MODE;
+			dsi->status.vtype = DSI_VIDEO_TYPE_VIDEO_MODE;
+		}
+	}
+
+	tegra_dsi_writel(dsi, max_threshold, DSI_MAX_THRESHOLD);
+	tegra_dsi_writel(dsi, dcs_cmd, DSI_DCS_CMDS);
+	tegra_dsi_writel(dsi, dsi_control, DSI_CONTROL);
+	tegra_dsi_writel(dsi, host_dsi_control, DSI_HOST_DSI_CONTROL);
+}
+
+static void tegra_dsi_pad_calibration(struct tegra_dc_dsi_data *dsi)
+{
+	u32 val;
+
+	val =	DSI_PAD_CONTROL_PAD_LPUPADJ(0x1) |
+		DSI_PAD_CONTROL_PAD_LPDNADJ(0x1) |
+		DSI_PAD_CONTROL_PAD_PREEMP_EN(0x1) |
+		DSI_PAD_CONTROL_PAD_SLEWDNADJ(0x6) |
+		DSI_PAD_CONTROL_PAD_SLEWUPADJ(0x6);
+	if (!dsi->ulpm) {
+		val |=	DSI_PAD_CONTROL_PAD_PDIO(0) |
+			DSI_PAD_CONTROL_PAD_PDIO_CLK(0) |
+			DSI_PAD_CONTROL_PAD_PULLDN_ENAB(TEGRA_DSI_DISABLE);
+	} else {
+		val |=	DSI_PAD_CONTROL_PAD_PDIO(0x3) |
+			DSI_PAD_CONTROL_PAD_PDIO_CLK(0x1) |
+			DSI_PAD_CONTROL_PAD_PULLDN_ENAB(TEGRA_DSI_ENABLE);
+	}
+	tegra_dsi_writel(dsi, val, DSI_PAD_CONTROL);
+
+	val = MIPI_CAL_TERMOSA(0x4);
+	tegra_vi_csi_writel(val, CSI_CILA_MIPI_CAL_CONFIG_0);
+
+	val = MIPI_CAL_TERMOSB(0x4);
+	tegra_vi_csi_writel(val, CSI_CILB_MIPI_CAL_CONFIG_0);
+
+	val = MIPI_CAL_HSPUOSD(0x3) | MIPI_CAL_HSPDOSD(0x4);
+	tegra_vi_csi_writel(val, CSI_DSI_MIPI_CAL_CONFIG);
+
+	val = PAD_DRIV_DN_REF(0x5) | PAD_DRIV_UP_REF(0x7);
+	tegra_vi_csi_writel(val, CSI_MIPIBIAS_PAD_CONFIG);
+
+	val = PAD_CIL_PDVREG(0x0);
+	tegra_vi_csi_writel(val, CSI_CIL_PAD_CONFIG);
+}
+
+static void tegra_dsi_panelB_enable(void)
+{
+	unsigned int val;
+
+	val = readl(IO_ADDRESS(APB_MISC_GP_MIPI_PAD_CTRL_0));
+	val |= DSIB_MODE_ENABLE;
+	writel(val, (IO_ADDRESS(APB_MISC_GP_MIPI_PAD_CTRL_0)));
+}
+
+static int tegra_dsi_init_hw(struct tegra_dc *dc,
+						struct tegra_dc_dsi_data *dsi)
+{
+	u32 i;
+
+	tegra_dsi_writel(dsi,
+		DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_DISABLE),
+		DSI_POWER_CONTROL);
+	/* stabilization delay */
+	udelay(300);
+
+	tegra_dsi_set_dsi_clk(dc, dsi, dsi->target_lp_clk_khz);
+	if (dsi->info.dsi_instance) {
+		tegra_dsi_panelB_enable();
+	}
+
+	/* TODO: only need to change the timing for bta */
+	tegra_dsi_set_phy_timing(dsi, DSI_LPHS_IN_LP_MODE);
+
+	if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
+		tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
+
+	/* Initializing DSI registers */
+	for (i = 0; i < ARRAY_SIZE(init_reg); i++)
+		tegra_dsi_writel(dsi, 0, init_reg[i]);
+
+	tegra_dsi_writel(dsi, dsi->dsi_control_val, DSI_CONTROL);
+
+	tegra_dsi_pad_calibration(dsi);
+
+	tegra_dsi_writel(dsi,
+		DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_ENABLE),
+		DSI_POWER_CONTROL);
+	/* stabilization delay */
+	udelay(300);
+
+	dsi->status.init = DSI_MODULE_INIT;
+	dsi->status.lphs = DSI_LPHS_NOT_INIT;
+	dsi->status.vtype = DSI_VIDEO_TYPE_NOT_INIT;
+	dsi->status.driven = DSI_DRIVEN_MODE_NOT_INIT;
+	dsi->status.clk_out = DSI_PHYCLK_OUT_DIS;
+	dsi->status.clk_mode = DSI_PHYCLK_NOT_INIT;
+	dsi->status.clk_burst = DSI_CLK_BURST_NOT_INIT;
+	dsi->status.dc_stream = DSI_DC_STREAM_DISABLE;
+	dsi->status.lp_op = DSI_LP_OP_NOT_INIT;
+
+	return 0;
+}
+
+static int tegra_dsi_set_to_lp_mode(struct tegra_dc *dc,
+			struct tegra_dc_dsi_data *dsi, u8 lp_op)
+{
+	int err;
+
+	if (dsi->status.init != DSI_MODULE_INIT) {
+		err = -EPERM;
+		goto fail;
+	}
+
+	if (dsi->status.lphs == DSI_LPHS_IN_LP_MODE &&
+			dsi->status.lp_op == lp_op)
+		goto success;
+
+	if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
+		tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
+
+	/* disable/enable hs clk according to enable_hs_clock_on_lp_cmd_mode */
+	if ((dsi->status.clk_out == DSI_PHYCLK_OUT_EN) &&
+		(!dsi->info.enable_hs_clock_on_lp_cmd_mode))
+		tegra_dsi_hs_clk_out_disable(dc, dsi);
+
+	dsi->target_lp_clk_khz = tegra_dsi_get_lp_clk_rate(dsi, lp_op);
+	if (dsi->current_dsi_clk_khz != dsi->target_lp_clk_khz) {
+		tegra_dsi_set_dsi_clk(dc, dsi, dsi->target_lp_clk_khz);
+		tegra_dsi_set_timeout(dsi);
+	}
+
+	tegra_dsi_set_phy_timing(dsi, DSI_LPHS_IN_LP_MODE);
+
+	tegra_dsi_set_control_reg_lp(dsi);
+
+	if ((dsi->status.clk_out == DSI_PHYCLK_OUT_DIS) &&
+		(dsi->info.enable_hs_clock_on_lp_cmd_mode))
+		tegra_dsi_hs_clk_out_enable_in_lp(dsi);
+
+	dsi->status.lphs = DSI_LPHS_IN_LP_MODE;
+	dsi->status.lp_op = lp_op;
+	dsi->driven_mode = TEGRA_DSI_DRIVEN_BY_HOST;
+success:
+	err = 0;
+fail:
+	return err;
+}
+
+static int tegra_dsi_set_to_hs_mode(struct tegra_dc *dc,
+					struct tegra_dc_dsi_data *dsi,
+					u8 driven_mode)
+{
+	int err;
+
+	if (dsi->status.init != DSI_MODULE_INIT) {
+		err = -EPERM;
+		goto fail;
+	}
+
+	if (dsi->status.lphs == DSI_LPHS_IN_HS_MODE &&
+		dsi->driven_mode == driven_mode)
+		goto success;
+
+	dsi->driven_mode = driven_mode;
+
+	if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
+		tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
+
+	if ((dsi->status.clk_out == DSI_PHYCLK_OUT_EN) &&
+		(!dsi->info.enable_hs_clock_on_lp_cmd_mode))
+		tegra_dsi_hs_clk_out_disable(dc, dsi);
+
+	if (dsi->current_dsi_clk_khz != dsi->target_hs_clk_khz) {
+		tegra_dsi_set_dsi_clk(dc, dsi, dsi->target_hs_clk_khz);
+		tegra_dsi_set_timeout(dsi);
+	}
+
+	tegra_dsi_set_phy_timing(dsi, DSI_LPHS_IN_HS_MODE);
+
+	if (driven_mode == TEGRA_DSI_DRIVEN_BY_DC) {
+		tegra_dsi_set_pkt_seq(dc, dsi);
+		tegra_dsi_set_pkt_length(dc, dsi);
+		tegra_dsi_set_sol_delay(dc, dsi);
+		tegra_dsi_set_dc_clk(dc, dsi);
+	}
+
+	tegra_dsi_set_control_reg_hs(dsi, driven_mode);
+
+	if (dsi->status.clk_out == DSI_PHYCLK_OUT_DIS ||
+		dsi->info.enable_hs_clock_on_lp_cmd_mode)
+		tegra_dsi_hs_clk_out_enable(dsi);
+
+	dsi->status.lphs = DSI_LPHS_IN_HS_MODE;
+success:
+	dsi->status.lp_op = DSI_LP_OP_NOT_INIT;
+	err = 0;
+fail:
+	return err;
+}
+
+static bool tegra_dsi_write_busy(struct tegra_dc_dsi_data *dsi)
+{
+	u32 timeout = 0;
+	bool retVal = true;
+
+	while (timeout <= DSI_MAX_COMMAND_DELAY_USEC) {
+		if (!(DSI_TRIGGER_HOST_TRIGGER(0x1) &
+			tegra_dsi_readl(dsi, DSI_TRIGGER))) {
+			retVal = false;
+			break;
+		}
+		udelay(DSI_COMMAND_DELAY_STEPS_USEC);
+		timeout += DSI_COMMAND_DELAY_STEPS_USEC;
+	}
+
+	return retVal;
+}
+
+static bool tegra_dsi_read_busy(struct tegra_dc_dsi_data *dsi)
+{
+	u32 timeout = 0;
+	bool retVal = true;
+
+	while (timeout <  DSI_STATUS_POLLING_DURATION_USEC) {
+		if (!(DSI_HOST_DSI_CONTROL_IMM_BTA(0x1) &
+			tegra_dsi_readl(dsi, DSI_HOST_DSI_CONTROL))) {
+			retVal = false;
+			break;
+		}
+		udelay(DSI_STATUS_POLLING_DELAY_USEC);
+		timeout += DSI_STATUS_POLLING_DELAY_USEC;
+	}
+
+	return retVal;
+}
+
+static bool tegra_dsi_host_busy(struct tegra_dc_dsi_data *dsi)
+{
+	int err = 0;
+
+	if (tegra_dsi_write_busy(dsi)) {
+		err = -EBUSY;
+		dev_err(&dsi->dc->ndev->dev,
+			"DSI trigger bit already set\n");
+		goto fail;
+	}
+
+	if (tegra_dsi_read_busy(dsi)) {
+		err = -EBUSY;
+		dev_err(&dsi->dc->ndev->dev,
+			"DSI immediate bta bit already set\n");
+		goto fail;
+	}
+fail:
+	return (err < 0 ? true : false);
+}
+
+static void tegra_dsi_reset_read_count(struct tegra_dc_dsi_data *dsi)
+{
+	u32 val;
+
+	val = tegra_dsi_readl(dsi, DSI_STATUS);
+	val &= DSI_STATUS_RD_FIFO_COUNT(0x1f);
+	if (val) {
+		dev_warn(&dsi->dc->ndev->dev,
+			"DSI read count not zero, resetting\n");
+		tegra_dsi_soft_reset(dsi);
+	}
+}
+
+static struct dsi_status *tegra_dsi_save_state_switch_to_host_cmd_mode(
+						struct tegra_dc_dsi_data *dsi,
+						struct tegra_dc *dc,
+						u8 lp_op)
+{
+	struct dsi_status *init_status = NULL;
+	int err;
+
+	if (dsi->status.init != DSI_MODULE_INIT ||
+		dsi->status.lphs == DSI_LPHS_NOT_INIT) {
+		err = -EPERM;
+		goto fail;
+	}
+
+	init_status = kzalloc(sizeof(*init_status), GFP_KERNEL);
+	if (!init_status)
+		return ERR_PTR(-ENOMEM);
+
+	*init_status = dsi->status;
+
+	if (dsi->info.hs_cmd_mode_supported) {
+		err = tegra_dsi_set_to_hs_mode(dc, dsi,
+				TEGRA_DSI_DRIVEN_BY_HOST);
+		if (err < 0) {
+			dev_err(&dc->ndev->dev,
+			"Switch to HS host mode failed\n");
+			goto fail;
+		}
+
+		goto success;
+	}
+
+	if (dsi->status.lp_op != lp_op) {
+		err = tegra_dsi_set_to_lp_mode(dc, dsi, lp_op);
+		if (err < 0) {
+			dev_err(&dc->ndev->dev,
+			"DSI failed to go to LP mode\n");
+			goto fail;
+		}
+	}
+success:
+	return init_status;
+fail:
+	kfree(init_status);
+	return ERR_PTR(err);
+}
+
+static struct dsi_status *tegra_dsi_prepare_host_transmission(
+				struct tegra_dc *dc,
+				struct tegra_dc_dsi_data *dsi,
+				u8 lp_op)
+{
+	int err = 0;
+	struct dsi_status *init_status;
+	bool restart_dc_stream = false;
+
+	if (dsi->status.init != DSI_MODULE_INIT ||
+		dsi->ulpm) {
+		err = -EPERM;
+		goto fail;
+	}
+
+	if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE) {
+		restart_dc_stream = true;
+		tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
+	}
+
+	if (tegra_dsi_host_busy(dsi)) {
+		tegra_dsi_soft_reset(dsi);
+		if (tegra_dsi_host_busy(dsi)) {
+			err = -EBUSY;
+			dev_err(&dc->ndev->dev, "DSI host busy\n");
+			goto fail;
+		}
+	}
+
+	if (lp_op == DSI_LP_OP_READ)
+		tegra_dsi_reset_read_count(dsi);
+
+	if (dsi->status.lphs == DSI_LPHS_NOT_INIT) {
+		err = tegra_dsi_set_to_lp_mode(dc, dsi, lp_op);
+		if (err < 0) {
+			dev_err(&dc->ndev->dev, "Failed to config LP write\n");
+			goto fail;
+		}
+	}
+
+	init_status = tegra_dsi_save_state_switch_to_host_cmd_mode
+					(dsi, dc, lp_op);
+	if (IS_ERR_OR_NULL(init_status)) {
+		err = PTR_ERR(init_status);
+		dev_err(&dc->ndev->dev, "DSI state saving failed\n");
+		goto fail;
+	}
+
+	if (restart_dc_stream)
+		init_status->dc_stream = DSI_DC_STREAM_ENABLE;
+
+	return init_status;
+fail:
+	return ERR_PTR(err);
+}
+
+static int tegra_dsi_restore_state(struct tegra_dc *dc,
+				struct tegra_dc_dsi_data *dsi,
+				struct dsi_status *init_status)
+{
+	int err = 0;
+
+	if (init_status->lphs == DSI_LPHS_IN_LP_MODE) {
+		err = tegra_dsi_set_to_lp_mode(dc, dsi, init_status->lp_op);
+		if (err < 0) {
+			dev_err(&dc->ndev->dev,
+				"Failed to config LP mode\n");
+			goto fail;
+		}
+		goto success;
+	}
+
+	if (init_status->lphs == DSI_LPHS_IN_HS_MODE) {
+		u8 driven = (init_status->driven == DSI_DRIVEN_MODE_DC) ?
+			TEGRA_DSI_DRIVEN_BY_DC : TEGRA_DSI_DRIVEN_BY_HOST;
+		err = tegra_dsi_set_to_hs_mode(dc, dsi, driven);
+		if (err < 0) {
+			dev_err(&dc->ndev->dev, "Failed to config HS mode\n");
+			goto fail;
+		}
+	}
+
+	if (init_status->dc_stream == DSI_DC_STREAM_ENABLE)
+		tegra_dsi_start_dc_stream(dc, dsi);
+success:
+fail:
+	kfree(init_status);
+	return err;
+}
+
+static int tegra_dsi_host_trigger(struct tegra_dc_dsi_data *dsi)
+{
+	int status = 0;
+
+	if (tegra_dsi_readl(dsi, DSI_TRIGGER)) {
+		status = -EBUSY;
+		goto fail;
+	}
+
+	tegra_dsi_writel(dsi,
+		DSI_TRIGGER_HOST_TRIGGER(TEGRA_DSI_ENABLE), DSI_TRIGGER);
+
+#if DSI_USE_SYNC_POINTS
+	status = tegra_dsi_syncpt(dsi);
+	if (status < 0) {
+		dev_err(&dsi->dc->ndev->dev,
+			"DSI syncpt for host trigger failed\n");
+		goto fail;
+	}
+#else
+	if (tegra_dsi_write_busy(dsi)) {
+		status = -EBUSY;
+		dev_err(&dsi->dc->ndev->dev,
+			"Timeout waiting on write completion\n");
+	}
+#endif
+
+fail:
+	return status;
+}
+
+static int _tegra_dsi_write_data(struct tegra_dc_dsi_data *dsi,
+					u8 *pdata, u8 data_id, u16 data_len)
+{
+	u8 virtual_channel;
+	u32 val;
+	int err;
+
+	err = 0;
+
+	virtual_channel = dsi->info.virtual_channel <<
+						DSI_VIR_CHANNEL_BIT_POSITION;
+
+	/* always use hw for ecc */
+	val = (virtual_channel | data_id) << 0 |
+			data_len << 8;
+	tegra_dsi_writel(dsi, val, DSI_WR_DATA);
+
+	/* if pdata != NULL, pkt type is long pkt */
+	if (pdata != NULL) {
+		while (data_len) {
+			if (data_len >= 4) {
+				val = ((u32 *) pdata)[0];
+				data_len -= 4;
+				pdata += 4;
+			} else {
+				val = 0;
+				memcpy(&val, pdata, data_len);
+				pdata += data_len;
+				data_len = 0;
+			}
+			tegra_dsi_writel(dsi, val, DSI_WR_DATA);
+		}
+	}
+
+	err = tegra_dsi_host_trigger(dsi);
+	if (err < 0)
+		dev_err(&dsi->dc->ndev->dev, "DSI host trigger failed\n");
+
+	return err;
+}
+
+static void tegra_dc_dsi_hold_host(struct tegra_dc *dc)
+{
+	struct tegra_dc_dsi_data *dsi = tegra_dc_get_outdata(dc);
+
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_LP_MODE) {
+		spin_lock(&dsi->host_ref_lock);
+		dsi->host_ref++;
+		spin_unlock(&dsi->host_ref_lock);
+		tegra_dsi_host_resume(dc);
+
+		/*
+		 * Take an extra refrence to count for the clk_disable in
+		 * tegra_dc_release_host.
+		 */
+		clk_enable(dc->clk);
+	}
+}
+
+static void tegra_dc_dsi_release_host(struct tegra_dc *dc)
+{
+	struct tegra_dc_dsi_data *dsi = tegra_dc_get_outdata(dc);
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_LP_MODE) {
+		clk_disable(dc->clk);
+		spin_lock(&dsi->host_ref_lock);
+		dsi->host_ref--;
+
+		if (!dsi->host_ref &&
+		    (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE))
+			schedule_delayed_work(&dsi->idle_work, dsi->idle_delay);
+
+		spin_unlock(&dsi->host_ref_lock);
+	}
+}
+
+static void tegra_dc_dsi_idle_work(struct work_struct *work)
+{
+	struct tegra_dc_dsi_data *dsi = container_of(
+		to_delayed_work(work), struct tegra_dc_dsi_data, idle_work);
+
+	if (dsi->dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_LP_MODE)
+		tegra_dsi_host_suspend(dsi->dc);
+}
+
+int tegra_dsi_write_data(struct tegra_dc *dc,
+			struct tegra_dc_dsi_data *dsi,
+			u8 *pdata, u8 data_id, u16 data_len)
+{
+	int err = 0;
+	struct dsi_status *init_status;
+
+	tegra_dc_io_start(dc);
+
+	init_status = tegra_dsi_prepare_host_transmission(
+				dc, dsi, DSI_LP_OP_WRITE);
+	if (IS_ERR_OR_NULL(init_status)) {
+		err = PTR_ERR(init_status);
+		dev_err(&dc->ndev->dev, "DSI host config failed\n");
+		goto fail;
+	}
+
+	err = _tegra_dsi_write_data(dsi, pdata, data_id, data_len);
+fail:
+	err = tegra_dsi_restore_state(dc, dsi, init_status);
+	if (err < 0)
+		dev_err(&dc->ndev->dev, "Failed to restore prev state\n");
+	tegra_dc_io_end(dc);
+
+	return err;
+}
+EXPORT_SYMBOL(tegra_dsi_write_data);
+
+static int tegra_dsi_send_panel_cmd(struct tegra_dc *dc,
+					struct tegra_dc_dsi_data *dsi,
+					struct tegra_dsi_cmd *cmd,
+					u32 n_cmd)
+{
+	u32 i;
+	int err;
+
+	err = 0;
+	for (i = 0; i < n_cmd; i++) {
+		struct tegra_dsi_cmd *cur_cmd;
+		cur_cmd = &cmd[i];
+
+		if (cur_cmd->cmd_type == TEGRA_DSI_DELAY_MS)
+			mdelay(cur_cmd->sp_len_dly.delay_ms);
+		else {
+			err = tegra_dsi_write_data(dc, dsi,
+						cur_cmd->pdata,
+						cur_cmd->data_id,
+						cur_cmd->sp_len_dly.data_len);
+			if (err < 0)
+				break;
+		}
+	}
+	return err;
+}
+
+static u8 tegra_dsi_ecc(u32 header)
+{
+	char ecc_parity[24] = {
+		0x07, 0x0b, 0x0d, 0x0e, 0x13, 0x15, 0x16, 0x19,
+		0x1a, 0x1c, 0x23, 0x25, 0x26, 0x29, 0x2a, 0x2c,
+		0x31, 0x32, 0x34, 0x38, 0x1f, 0x2f, 0x37, 0x3b
+	};
+	u8 ecc_byte;
+	int i;
+
+	ecc_byte = 0;
+	for (i = 0; i < 24; i++)
+		ecc_byte ^= ((header >> i) & 1) ? ecc_parity[i] : 0x00;
+
+	return ecc_byte;
+}
+
+static u16 tegra_dsi_cs(char *pdata, u16 data_len)
+{
+	u16 byte_cnt;
+	u8 bit_cnt;
+	char curr_byte;
+	u16 crc = 0xFFFF;
+	u16 poly = 0x8408;
+
+	if (data_len > 0) {
+		for (byte_cnt = 0; byte_cnt < data_len; byte_cnt++) {
+			curr_byte = pdata[byte_cnt];
+			for (bit_cnt = 0; bit_cnt < 8; bit_cnt++) {
+				if (((crc & 0x0001 ) ^
+					(curr_byte & 0x0001)) > 0)
+					crc = ((crc >> 1) & 0x7FFF) ^ poly;
+				else
+					crc = (crc >> 1) & 0x7FFF;
+
+				curr_byte = (curr_byte >> 1 ) & 0x7F;
+			}
+		}
+	}
+	return crc;
+}
+
+static int tegra_dsi_dcs_pkt_seq_ctrl_init(struct tegra_dc_dsi_data *dsi,
+						struct tegra_dsi_cmd *cmd)
+{
+	u8 virtual_channel;
+	u32 val;
+	u16 data_len = cmd->sp_len_dly.data_len;
+	u8 seq_ctrl_reg = 0;
+
+	virtual_channel = dsi->info.virtual_channel <<
+				DSI_VIR_CHANNEL_BIT_POSITION;
+
+	val = (virtual_channel | cmd->data_id) << 0 |
+		data_len << 8;
+
+	val |= tegra_dsi_ecc(val) << 24;
+
+	tegra_dsi_writel(dsi, val, DSI_INIT_SEQ_DATA_0 + seq_ctrl_reg++);
+
+	/* if pdata != NULL, pkt type is long pkt */
+	if (cmd->pdata != NULL) {
+		u8 *pdata;
+		u8 *pdata_mem;
+		/*  allocate memory for pdata + 2 bytes checksum */
+		pdata_mem = kzalloc(sizeof(u8) * data_len + 2, GFP_KERNEL);
+		if (!pdata_mem) {
+			dev_err(&dsi->dc->ndev->dev, "dsi: memory err\n");
+			tegra_dsi_soft_reset(dsi);
+			return -ENOMEM;
+		}
+
+		memcpy(pdata_mem, cmd->pdata, data_len);
+		pdata = pdata_mem;
+		*((u16 *)(pdata + data_len)) = tegra_dsi_cs(pdata, data_len);
+
+		/* data_len = length of pdata + 2 byte checksum */
+		data_len += 2;
+
+		while (data_len) {
+			if (data_len >= 4) {
+				val = ((u32 *) pdata)[0];
+				data_len -= 4;
+				pdata += 4;
+			} else {
+				val = 0;
+				memcpy(&val, pdata, data_len);
+				pdata += data_len;
+				data_len = 0;
+			}
+			tegra_dsi_writel(dsi, val, DSI_INIT_SEQ_DATA_0 +
+							seq_ctrl_reg++);
+		}
+		kfree(pdata_mem);
+	}
+
+	return 0;
+}
+
+int tegra_dsi_start_host_cmd_v_blank_dcs(struct tegra_dc_dsi_data * dsi,
+						struct tegra_dsi_cmd *cmd)
+{
+#define PKT_HEADER_LEN_BYTE	4
+#define CHECKSUM_LEN_BYTE	2
+
+	int err = 0;
+	u32 val;
+	u16 tot_pkt_len = PKT_HEADER_LEN_BYTE;
+	struct tegra_dc *dc = dsi->dc;
+
+	if (cmd->cmd_type != TEGRA_DSI_PACKET_CMD)
+		return -EINVAL;
+
+	mutex_lock(&dsi->lock);
+	tegra_dc_dsi_hold_host(dc);
+
+	tegra_dc_io_start(dc);
+
+
+	err = tegra_dsi_dcs_pkt_seq_ctrl_init(dsi, cmd);
+	if (err < 0) {
+		dev_err(&dsi->dc->ndev->dev,
+			"dsi: dcs pkt seq ctrl init failed\n");
+		goto fail;
+	}
+
+	if (cmd->pdata) {
+		u16 data_len = cmd->sp_len_dly.data_len;
+		tot_pkt_len += data_len + CHECKSUM_LEN_BYTE;
+	}
+
+	val = DSI_INIT_SEQ_CONTROL_DSI_FRAME_INIT_BYTE_COUNT(tot_pkt_len) |
+		DSI_INIT_SEQ_CONTROL_DSI_SEND_INIT_SEQUENCE(
+						TEGRA_DSI_ENABLE);
+	tegra_dsi_writel(dsi, val, DSI_INIT_SEQ_CONTROL);
+
+fail:
+	tegra_dc_io_end(dc);
+	tegra_dc_dsi_release_host(dc);
+	mutex_unlock(&dsi->lock);
+	return err;
+
+#undef PKT_HEADER_LEN_BYTE
+#undef CHECKSUM_LEN_BYTE
+}
+EXPORT_SYMBOL(tegra_dsi_start_host_cmd_v_blank_dcs);
+
+void tegra_dsi_stop_host_cmd_v_blank_dcs(struct tegra_dc_dsi_data * dsi)
+{
+	struct tegra_dc *dc = dsi->dc;
+	u32 cnt;
+
+	mutex_lock(&dsi->lock);
+	tegra_dc_dsi_hold_host(dc);
+
+	tegra_dc_io_start(dc);
+
+	tegra_dsi_writel(dsi, TEGRA_DSI_DISABLE, DSI_INIT_SEQ_CONTROL);
+
+	/* clear seq data registers */
+	for (cnt = 0; cnt < 8; cnt++)
+		tegra_dsi_writel(dsi, 0, DSI_INIT_SEQ_DATA_0 + cnt);
+
+	tegra_dc_io_end(dc);
+
+	tegra_dc_dsi_release_host(dc);
+	mutex_unlock(&dsi->lock);
+}
+EXPORT_SYMBOL(tegra_dsi_stop_host_cmd_v_blank_dcs);
+
+static int tegra_dsi_bta(struct tegra_dc_dsi_data *dsi)
+{
+	u32 val;
+	u32 poll_time;
+	int err;
+
+	poll_time = 0;
+	err = 0;
+
+	val = tegra_dsi_readl(dsi, DSI_HOST_DSI_CONTROL);
+	val |= DSI_HOST_DSI_CONTROL_IMM_BTA(TEGRA_DSI_ENABLE);
+	tegra_dsi_writel(dsi, val, DSI_HOST_DSI_CONTROL);
+
+#if DSI_USE_SYNC_POINTS
+	dsi->syncpt_val = nvhost_syncpt_read_ext(dsi->dc->ndev,
+			dsi->syncpt_id);
+
+	val = DSI_INCR_SYNCPT_COND(OP_DONE) |
+		DSI_INCR_SYNCPT_INDX(dsi->syncpt_id);
+	tegra_dsi_writel(dsi, val, DSI_INCR_SYNCPT);
+
+	/* TODO: Use interrupt rather than polling */
+	err = nvhost_syncpt_wait_timeout_ext(dsi->dc->ndev, dsi->syncpt_id,
+		dsi->syncpt_val + 1, MAX_SCHEDULE_TIMEOUT, NULL);
+	if (err < 0)
+		dev_err(&dsi->dc->ndev->dev,
+			"DSI sync point failure\n");
+	else
+		(dsi->syncpt_val)++;
+#else
+	if (tegra_dsi_read_busy(dsi)) {
+		err = -EBUSY;
+		dev_err(&dsi->dc->ndev->dev,
+			"Timeout wating on read completion\n");
+	}
+#endif
+
+	return err;
+}
+
+static int tegra_dsi_parse_read_response(struct tegra_dc *dc,
+					u32 rd_fifo_cnt, u8 *read_fifo)
+{
+	int err;
+	u32 payload_size;
+
+	payload_size = 0;
+	err = 0;
+
+	switch (read_fifo[0]) {
+	case DSI_ESCAPE_CMD:
+		dev_info(&dc->ndev->dev, "escape cmd[0x%x]\n", read_fifo[0]);
+		break;
+	case DSI_ACK_NO_ERR:
+		dev_info(&dc->ndev->dev,
+			"Panel ack, no err[0x%x]\n", read_fifo[0]);
+		return err;
+	default:
+		dev_info(&dc->ndev->dev, "Invalid read response\n");
+		break;
+	}
+
+	switch (read_fifo[4] & 0xff) {
+	case GEN_LONG_RD_RES:
+		/* Fall through */
+	case DCS_LONG_RD_RES:
+		payload_size = (read_fifo[5] |
+				(read_fifo[6] << 8)) & 0xFFFF;
+		dev_info(&dc->ndev->dev, "Long read response Packet\n"
+				"payload_size[0x%x]\n", payload_size);
+		break;
+	case GEN_1_BYTE_SHORT_RD_RES:
+		/* Fall through */
+	case DCS_1_BYTE_SHORT_RD_RES:
+		payload_size = 1;
+		dev_info(&dc->ndev->dev, "Short read response Packet\n"
+			"payload_size[0x%x]\n", payload_size);
+		break;
+	case GEN_2_BYTE_SHORT_RD_RES:
+		/* Fall through */
+	case DCS_2_BYTE_SHORT_RD_RES:
+		payload_size = 2;
+		dev_info(&dc->ndev->dev, "Short read response Packet\n"
+			"payload_size[0x%x]\n", payload_size);
+		break;
+	case ACK_ERR_RES:
+		payload_size = 2;
+		dev_info(&dc->ndev->dev, "Acknowledge error report response\n"
+			"Packet payload_size[0x%x]\n", payload_size);
+		break;
+	default:
+		dev_info(&dc->ndev->dev, "Invalid response packet\n");
+		err = -EINVAL;
+		break;
+	}
+	return err;
+}
+
+static int tegra_dsi_read_fifo(struct tegra_dc *dc,
+			struct tegra_dc_dsi_data *dsi,
+			u8 *read_fifo)
+{
+	u32 val;
+	u32 i;
+	u32 poll_time = 0;
+	u32 rd_fifo_cnt;
+	int err = 0;
+	u8 *read_fifo_cp = read_fifo;
+
+	while (poll_time <  DSI_DELAY_FOR_READ_FIFO) {
+		mdelay(1);
+		val = tegra_dsi_readl(dsi, DSI_STATUS);
+		rd_fifo_cnt = val & DSI_STATUS_RD_FIFO_COUNT(0x1f);
+		if (rd_fifo_cnt << 2 > DSI_READ_FIFO_DEPTH)
+			dev_err(&dc->ndev->dev,
+			"DSI RD_FIFO_CNT is greater than RD_FIFO_DEPTH\n");
+			break;
+		poll_time++;
+	}
+
+	if (rd_fifo_cnt == 0) {
+		dev_info(&dc->ndev->dev,
+			"DSI RD_FIFO_CNT is zero\n");
+		err = -EINVAL;
+		goto fail;
+	}
+
+	if (val & (DSI_STATUS_LB_UNDERFLOW(0x1) |
+		DSI_STATUS_LB_OVERFLOW(0x1))) {
+		dev_warn(&dc->ndev->dev,
+			"DSI overflow/underflow error\n");
+	}
+
+	/* Read data from FIFO */
+	for (i = 0; i < rd_fifo_cnt; i++) {
+		val = tegra_dsi_readl(dsi, DSI_RD_DATA);
+		if (enable_read_debug)
+			dev_info(&dc->ndev->dev,
+			"Read data[%d]: 0x%x\n", i, val);
+		memcpy(read_fifo, &val, 4);
+		read_fifo += 4;
+	}
+
+	/* Make sure all the data is read from the FIFO */
+	val = tegra_dsi_readl(dsi, DSI_STATUS);
+	val &= DSI_STATUS_RD_FIFO_COUNT(0x1f);
+	if (val)
+		dev_err(&dc->ndev->dev, "DSI FIFO_RD_CNT not zero"
+		" even after reading FIFO_RD_CNT words from read fifo\n");
+
+	if (enable_read_debug) {
+		err =
+		tegra_dsi_parse_read_response(dc, rd_fifo_cnt, read_fifo_cp);
+		if (err < 0)
+			dev_warn(&dc->ndev->dev, "Unexpected read data\n");
+	}
+fail:
+	return err;
+}
+
+int tegra_dsi_read_data(struct tegra_dc *dc,
+				struct tegra_dc_dsi_data *dsi,
+				u32 max_ret_payload_size,
+				u32 panel_reg_addr, u8 *read_data)
+{
+	int err = 0;
+	struct dsi_status *init_status;
+
+	mutex_lock(&dsi->lock);
+	tegra_dc_io_start(dc);
+
+	init_status = tegra_dsi_prepare_host_transmission(
+				dc, dsi, DSI_LP_OP_WRITE);
+	if (IS_ERR_OR_NULL(init_status)) {
+		err = PTR_ERR(init_status);
+		dev_err(&dc->ndev->dev, "DSI host config failed\n");
+		goto fail;
+	}
+
+	/* Set max return payload size in words */
+	err = _tegra_dsi_write_data(dsi, NULL,
+		dsi_command_max_return_pkt_size,
+		max_ret_payload_size);
+	if (err < 0) {
+		dev_err(&dc->ndev->dev,
+				"DSI write failed\n");
+		goto fail;
+	}
+
+	/* DCS to read given panel register */
+	err = _tegra_dsi_write_data(dsi, NULL,
+		dsi_command_dcs_read_with_no_params,
+		panel_reg_addr);
+	if (err < 0) {
+		dev_err(&dc->ndev->dev,
+				"DSI write failed\n");
+		goto fail;
+	}
+
+	tegra_dsi_reset_read_count(dsi);
+
+	if (dsi->status.lp_op == DSI_LP_OP_WRITE) {
+		err = tegra_dsi_set_to_lp_mode(dc, dsi, DSI_LP_OP_READ);
+		if (err < 0) {
+			dev_err(&dc->ndev->dev,
+			"DSI failed to go to LP read mode\n");
+			goto fail;
+		}
+	}
+
+	err = tegra_dsi_bta(dsi);
+	if (err < 0) {
+		dev_err(&dc->ndev->dev,
+			"DSI IMM BTA timeout\n");
+		goto fail;
+	}
+
+	err = tegra_dsi_read_fifo(dc, dsi, read_data);
+	if (err < 0) {
+		dev_err(&dc->ndev->dev, "DSI read fifo failure\n");
+		goto fail;
+	}
+fail:
+	err = tegra_dsi_restore_state(dc, dsi, init_status);
+	if (err < 0)
+		dev_err(&dc->ndev->dev, "Failed to restore prev state\n");
+	tegra_dc_io_end(dc);
+	mutex_unlock(&dsi->lock);
+	return err;
+}
+EXPORT_SYMBOL(tegra_dsi_read_data);
+
+int tegra_dsi_panel_sanity_check(struct tegra_dc *dc,
+				struct tegra_dc_dsi_data *dsi)
+{
+	int err = 0;
+	u8 read_fifo[DSI_READ_FIFO_DEPTH];
+	struct dsi_status *init_status;
+	static struct tegra_dsi_cmd dsi_nop_cmd =
+			DSI_CMD_SHORT(0x05, 0x0, 0x0);
+
+	tegra_dc_io_start(dc);
+
+	init_status = tegra_dsi_prepare_host_transmission(
+					dc, dsi, DSI_LP_OP_WRITE);
+	if (IS_ERR_OR_NULL(init_status)) {
+		err = PTR_ERR(init_status);
+		dev_err(&dc->ndev->dev, "DSI host config failed\n");
+		goto fail;
+	}
+
+	err = _tegra_dsi_write_data(dsi, NULL, dsi_nop_cmd.data_id, 0x0);
+	if (err < 0) {
+		dev_err(&dc->ndev->dev, "DSI nop write failed\n");
+		goto fail;
+	}
+
+	tegra_dsi_reset_read_count(dsi);
+
+	if (dsi->status.lp_op == DSI_LP_OP_WRITE) {
+		err = tegra_dsi_set_to_lp_mode(dc, dsi, DSI_LP_OP_READ);
+		if (err < 0) {
+			dev_err(&dc->ndev->dev,
+			"DSI failed to go to LP read mode\n");
+			goto fail;
+		}
+	}
+
+	err = tegra_dsi_bta(dsi);
+	if (err < 0) {
+		dev_err(&dc->ndev->dev, "DSI BTA failed\n");
+		goto fail;
+	}
+
+	err = tegra_dsi_read_fifo(dc, dsi, read_fifo);
+	if (err < 0) {
+		dev_err(&dc->ndev->dev, "DSI read fifo failure\n");
+		goto fail;
+	}
+
+	if (read_fifo[0] != DSI_ACK_NO_ERR) {
+		dev_warn(&dc->ndev->dev,
+			"Ack no error trigger message not received\n");
+		err = -EAGAIN;
+	}
+fail:
+	err = tegra_dsi_restore_state(dc, dsi, init_status);
+	if (err < 0)
+		dev_err(&dc->ndev->dev, "Failed to restore prev state\n");
+	tegra_dc_io_end(dc);
+	return err;
+}
+EXPORT_SYMBOL(tegra_dsi_panel_sanity_check);
+
+static int tegra_dsi_enter_ulpm(struct tegra_dc_dsi_data *dsi)
+{
+	u32 val;
+	int ret;
+
+	ret = 0;
+
+	val = tegra_dsi_readl(dsi, DSI_HOST_DSI_CONTROL);
+	val &= ~DSI_HOST_DSI_CONTROL_ULTRA_LOW_POWER(3);
+	val |= DSI_HOST_DSI_CONTROL_ULTRA_LOW_POWER(ENTER_ULPM);
+	tegra_dsi_writel(dsi, val, DSI_HOST_DSI_CONTROL);
+
+#if DSI_USE_SYNC_POINTS
+	ret = tegra_dsi_syncpt(dsi);
+	if (ret < 0) {
+		dev_err(&dsi->dc->ndev->dev,
+			"DSI syncpt for ulpm enter failed\n");
+		goto fail;
+	}
+#else
+	/* TODO: Find exact delay required */
+	mdelay(10);
+#endif
+	dsi->ulpm = true;
+fail:
+	return ret;
+}
+
+static int tegra_dsi_exit_ulpm(struct tegra_dc_dsi_data *dsi)
+{
+	u32 val;
+	int ret;
+
+	ret = 0;
+
+	val = tegra_dsi_readl(dsi, DSI_HOST_DSI_CONTROL);
+	val &= ~DSI_HOST_DSI_CONTROL_ULTRA_LOW_POWER(3);
+	val |= DSI_HOST_DSI_CONTROL_ULTRA_LOW_POWER(EXIT_ULPM);
+	tegra_dsi_writel(dsi, val, DSI_HOST_DSI_CONTROL);
+
+#if DSI_USE_SYNC_POINTS
+	ret = tegra_dsi_syncpt(dsi);
+	if (ret < 0) {
+		dev_err(&dsi->dc->ndev->dev,
+			"DSI syncpt for ulpm exit failed\n");
+		goto fail;
+	}
+#else
+	/* TODO: Find exact delay required */
+	mdelay(10);
+#endif
+	dsi->ulpm = false;
+
+	val = tegra_dsi_readl(dsi, DSI_HOST_DSI_CONTROL);
+	val &= ~DSI_HOST_DSI_CONTROL_ULTRA_LOW_POWER(0x3);
+	val |= DSI_HOST_DSI_CONTROL_ULTRA_LOW_POWER(NORMAL);
+	tegra_dsi_writel(dsi, val, DSI_HOST_DSI_CONTROL);
+fail:
+	return ret;
+
+}
+
+static void tegra_dsi_send_dc_frames(struct tegra_dc *dc,
+				     struct tegra_dc_dsi_data *dsi,
+				     int no_of_frames)
+{
+	int err;
+	u32 frame_period = DIV_ROUND_UP(S_TO_MS(1), dsi->info.refresh_rate);
+	u8 lp_op = dsi->status.lp_op;
+	bool switch_to_lp = (dsi->status.lphs == DSI_LPHS_IN_LP_MODE);
+
+	if (dsi->status.lphs != DSI_LPHS_IN_HS_MODE) {
+		err = tegra_dsi_set_to_hs_mode(dc, dsi,
+				TEGRA_DSI_DRIVEN_BY_DC);
+		if (err < 0) {
+			dev_err(&dc->ndev->dev,
+				"Switch to HS host mode failed\n");
+			return;
+		}
+	}
+
+	/*
+	 * Some panels need DC frames be sent under certain
+	 * conditions. We are working on the right fix for this
+	 * requirement, while using this current fix.
+	 */
+	tegra_dsi_start_dc_stream(dc, dsi);
+
+	/*
+	 * Send frames in Continuous or One-shot mode.
+	 */
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE) {
+		while (no_of_frames--) {
+			tegra_dc_writel(dc, GENERAL_ACT_REQ | NC_HOST_TRIG,
+					DC_CMD_STATE_CONTROL);
+			mdelay(frame_period);
+		}
+	} else
+		mdelay(no_of_frames * frame_period);
+
+	tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
+
+	if (switch_to_lp) {
+		err = tegra_dsi_set_to_lp_mode(dc, dsi, lp_op);
+		if (err < 0)
+			dev_err(&dc->ndev->dev,
+				"DSI failed to go to LP mode\n");
+	}
+}
+
+static void tegra_dc_dsi_enable(struct tegra_dc *dc)
+{
+	struct tegra_dc_dsi_data *dsi = tegra_dc_get_outdata(dc);
+	int err;
+	u32 val;
+
+	mutex_lock(&dsi->lock);
+	tegra_dc_dsi_hold_host(dc);
+
+	tegra_dc_io_start(dc);
+	/* Stop DC stream before configuring DSI registers
+	 * to avoid visible glitches on panel during transition
+	 * from bootloader to kernel driver
+	 */
+	tegra_dsi_stop_dc_stream(dc, dsi);
+
+	if (dsi->enabled) {
+		if (dsi->ulpm) {
+			if (tegra_dsi_exit_ulpm(dsi) < 0) {
+				dev_err(&dc->ndev->dev,
+					"DSI failed to exit ulpm\n");
+				goto fail;
+			}
+		}
+
+		if (dsi->info.panel_reset) {
+			/*
+			 * Certain panels need dc frames be sent before
+			 * waking panel.
+			 */
+			if (dsi->info.panel_send_dc_frames)
+				tegra_dsi_send_dc_frames(dc, dsi, 2);
+
+			err = tegra_dsi_send_panel_cmd(dc, dsi,
+							dsi->info.dsi_init_cmd,
+							dsi->info.n_init_cmd);
+			if (err < 0) {
+				dev_err(&dc->ndev->dev,
+				"dsi: error sending dsi init cmd\n");
+				goto fail;
+			}
+		} else if (dsi->info.dsi_late_resume_cmd) {
+			err = tegra_dsi_send_panel_cmd(dc, dsi,
+						dsi->info.dsi_late_resume_cmd,
+						dsi->info.n_late_resume_cmd);
+			if (err < 0) {
+				dev_err(&dc->ndev->dev,
+				"dsi: error sending late resume cmd\n");
+				goto fail;
+			}
+		}
+	} else {
+		err = tegra_dsi_init_hw(dc, dsi);
+		if (err < 0) {
+			dev_err(&dc->ndev->dev,
+				"dsi: not able to init dsi hardware\n");
+			goto fail;
+		}
+
+		if (dsi->ulpm) {
+			if (tegra_dsi_enter_ulpm(dsi) < 0) {
+				dev_err(&dc->ndev->dev,
+					"DSI failed to enter ulpm\n");
+				goto fail;
+			}
+
+			val = tegra_dsi_readl(dsi, DSI_PAD_CONTROL);
+
+			/* erase bits we're about to set */
+			val &= ~(DSI_PAD_CONTROL_PAD_PDIO(0x3) |
+				DSI_PAD_CONTROL_PAD_PDIO_CLK(0x1) |
+				DSI_PAD_CONTROL_PAD_PULLDN_ENAB(0x1));
+
+			val |= (DSI_PAD_CONTROL_PAD_PDIO(0) |
+				DSI_PAD_CONTROL_PAD_PDIO_CLK(0) |
+				DSI_PAD_CONTROL_PAD_PULLDN_ENAB
+						(TEGRA_DSI_DISABLE));
+
+			tegra_dsi_writel(dsi, val, DSI_PAD_CONTROL);
+			if (tegra_dsi_exit_ulpm(dsi) < 0) {
+				dev_err(&dc->ndev->dev,
+					"DSI failed to exit ulpm\n");
+				goto fail;
+			}
+		}
+
+		/*
+		 * Certain panels need dc frames be sent before
+		 * waking panel.
+		 */
+		if (dsi->info.panel_send_dc_frames)
+			tegra_dsi_send_dc_frames(dc, dsi, 2);
+
+		err = tegra_dsi_set_to_lp_mode(dc, dsi, DSI_LP_OP_WRITE);
+		if (err < 0) {
+			dev_err(&dc->ndev->dev,
+				"dsi: not able to set to lp mode\n");
+			goto fail;
+		}
+
+		err = tegra_dsi_send_panel_cmd(dc, dsi, dsi->info.dsi_init_cmd,
+						dsi->info.n_init_cmd);
+		if (err < 0) {
+			dev_err(&dc->ndev->dev,
+				"dsi: error while sending dsi init cmd\n");
+			goto fail;
+		}
+
+		err = tegra_dsi_set_to_hs_mode(dc, dsi,
+				TEGRA_DSI_DRIVEN_BY_DC);
+		if (err < 0) {
+			dev_err(&dc->ndev->dev,
+				"dsi: not able to set to hs mode\n");
+			goto fail;
+		}
+
+		dsi->enabled = true;
+	}
+
+	if (dsi->status.driven == DSI_DRIVEN_MODE_DC)
+		tegra_dsi_start_dc_stream(dc, dsi);
+fail:
+	tegra_dc_io_end(dc);
+	tegra_dc_dsi_release_host(dc);
+	mutex_unlock(&dsi->lock);
+}
+
+static void _tegra_dc_dsi_init(struct tegra_dc *dc)
+{
+	struct tegra_dc_dsi_data *dsi = tegra_dc_get_outdata(dc);
+
+	tegra_dc_dsi_debug_create(dsi);
+	tegra_dsi_init_sw(dc, dsi);
+	/* TODO: Configure the CSI pad configuration */
+}
+
+static int tegra_dc_dsi_cp_p_cmd(struct tegra_dsi_cmd *src,
+					struct tegra_dsi_cmd *dst, u16 n_cmd)
+{
+	u16 i;
+	u16 len;
+
+	memcpy(dst, src, sizeof(*dst) * n_cmd);
+
+	for (i = 0; i < n_cmd; i++)
+		if (src[i].pdata) {
+			len = sizeof(*src[i].pdata) *
+					src[i].sp_len_dly.data_len;
+			dst[i].pdata = kzalloc(len, GFP_KERNEL);
+			if (!dst[i].pdata)
+				goto free_cmd_pdata;
+			memcpy(dst[i].pdata, src[i].pdata, len);
+		}
+
+	return 0;
+
+free_cmd_pdata:
+	for (--i; i >= 0; i--)
+		if (dst[i].pdata)
+			kfree(dst[i].pdata);
+	return -ENOMEM;
+}
+
+static int tegra_dc_dsi_cp_info(struct tegra_dc_dsi_data *dsi,
+					struct tegra_dsi_out *p_dsi)
+{
+	struct tegra_dsi_cmd *p_init_cmd;
+	struct tegra_dsi_cmd *p_early_suspend_cmd = NULL;
+	struct tegra_dsi_cmd *p_late_resume_cmd = NULL;
+	struct tegra_dsi_cmd *p_suspend_cmd;
+	int err;
+
+	if (p_dsi->n_data_lanes > MAX_DSI_DATA_LANES)
+		return -EINVAL;
+
+	p_init_cmd = kzalloc(sizeof(*p_init_cmd) *
+				p_dsi->n_init_cmd, GFP_KERNEL);
+	if (!p_init_cmd)
+		return -ENOMEM;
+
+	if (p_dsi->dsi_early_suspend_cmd) {
+		p_early_suspend_cmd = kzalloc(sizeof(*p_early_suspend_cmd) *
+					p_dsi->n_early_suspend_cmd,
+					GFP_KERNEL);
+		if (!p_early_suspend_cmd) {
+			err = -ENOMEM;
+			goto err_free_init_cmd;
+		}
+	}
+
+	if (p_dsi->dsi_late_resume_cmd) {
+		p_late_resume_cmd = kzalloc(sizeof(*p_late_resume_cmd) *
+					p_dsi->n_late_resume_cmd,
+					GFP_KERNEL);
+		if (!p_late_resume_cmd) {
+			err = -ENOMEM;
+			goto err_free_p_early_suspend_cmd;
+		}
+	}
+
+	p_suspend_cmd = kzalloc(sizeof(*p_suspend_cmd) * p_dsi->n_suspend_cmd,
+				GFP_KERNEL);
+	if (!p_suspend_cmd) {
+		err = -ENOMEM;
+		goto err_free_p_late_resume_cmd;
+	}
+
+	memcpy(&dsi->info, p_dsi, sizeof(dsi->info));
+
+	/* Copy panel init cmd */
+	err = tegra_dc_dsi_cp_p_cmd(p_dsi->dsi_init_cmd,
+						p_init_cmd, p_dsi->n_init_cmd);
+	if (err < 0)
+		goto err_free;
+	dsi->info.dsi_init_cmd = p_init_cmd;
+
+	/* Copy panel early suspend cmd */
+	if (p_dsi->dsi_early_suspend_cmd) {
+		err = tegra_dc_dsi_cp_p_cmd(p_dsi->dsi_early_suspend_cmd,
+					p_early_suspend_cmd,
+					p_dsi->n_early_suspend_cmd);
+		if (err < 0)
+			goto err_free;
+		dsi->info.dsi_early_suspend_cmd = p_early_suspend_cmd;
+	}
+
+	/* Copy panel late resume cmd */
+	if (p_dsi->dsi_late_resume_cmd) {
+		err = tegra_dc_dsi_cp_p_cmd(p_dsi->dsi_late_resume_cmd,
+						p_late_resume_cmd,
+						p_dsi->n_late_resume_cmd);
+		if (err < 0)
+			goto err_free;
+		dsi->info.dsi_late_resume_cmd = p_late_resume_cmd;
+	}
+
+	/* Copy panel suspend cmd */
+	err = tegra_dc_dsi_cp_p_cmd(p_dsi->dsi_suspend_cmd, p_suspend_cmd,
+					p_dsi->n_suspend_cmd);
+	if (err < 0)
+		goto err_free;
+	dsi->info.dsi_suspend_cmd = p_suspend_cmd;
+
+	if (!dsi->info.panel_reset_timeout_msec)
+		dsi->info.panel_reset_timeout_msec =
+						DEFAULT_PANEL_RESET_TIMEOUT;
+
+	if (!dsi->info.panel_buffer_size_byte)
+		dsi->info.panel_buffer_size_byte = DEFAULT_PANEL_BUFFER_BYTE;
+
+	if (!dsi->info.max_panel_freq_khz) {
+		dsi->info.max_panel_freq_khz = DEFAULT_MAX_DSI_PHY_CLK_KHZ;
+
+		if (dsi->info.video_burst_mode >
+				TEGRA_DSI_VIDEO_NONE_BURST_MODE_WITH_SYNC_END){
+			dev_err(&dsi->dc->ndev->dev, "DSI: max_panel_freq_khz"
+					"is not set for DSI burst mode.\n");
+			dsi->info.video_burst_mode =
+				TEGRA_DSI_VIDEO_BURST_MODE_LOWEST_SPEED;
+		}
+	}
+
+	if (!dsi->info.lp_cmd_mode_freq_khz)
+		dsi->info.lp_cmd_mode_freq_khz = DEFAULT_LP_CMD_MODE_CLK_KHZ;
+
+	if (!dsi->info.chip_id || !dsi->info.chip_rev)
+		dev_warn(&dsi->dc->ndev->dev,
+			"DSI: Failed to get chip info\n");
+
+	if (!dsi->info.lp_read_cmd_mode_freq_khz)
+		dsi->info.lp_read_cmd_mode_freq_khz =
+			dsi->info.lp_cmd_mode_freq_khz;
+
+	/* host mode is for testing only */
+	dsi->driven_mode = TEGRA_DSI_DRIVEN_BY_DC;
+	return 0;
+
+err_free:
+	kfree(p_suspend_cmd);
+err_free_p_late_resume_cmd:
+	kfree(p_late_resume_cmd);
+err_free_p_early_suspend_cmd:
+	kfree(p_early_suspend_cmd);
+err_free_init_cmd:
+	kfree(p_init_cmd);
+	return err;
+}
+
+static int tegra_dc_dsi_init(struct tegra_dc *dc)
+{
+	struct tegra_dc_dsi_data *dsi;
+	struct resource *res;
+	struct resource *base_res;
+	void __iomem *base;
+	struct clk *dc_clk = NULL;
+	struct clk *dsi_clk = NULL;
+	struct clk *dsi_fixed_clk = NULL;
+	struct tegra_dsi_out *dsi_pdata;
+	int err;
+
+	err = 0;
+
+	dsi = kzalloc(sizeof(*dsi), GFP_KERNEL);
+	if (!dsi)
+		return -ENOMEM;
+
+	res = nvhost_get_resource_byname(dc->ndev, IORESOURCE_MEM,
+					"dsi_regs");
+	if (!res) {
+		dev_err(&dc->ndev->dev, "dsi: no mem resource\n");
+		err = -ENOENT;
+		goto err_free_dsi;
+	}
+
+	base_res = request_mem_region(res->start, resource_size(res),
+				dc->ndev->name);
+	if (!base_res) {
+		dev_err(&dc->ndev->dev, "dsi: request_mem_region failed\n");
+		err = -EBUSY;
+		goto err_free_dsi;
+	}
+
+	base = ioremap(res->start, resource_size(res));
+	if (!base) {
+		dev_err(&dc->ndev->dev, "dsi: registers can't be mapped\n");
+		err = -EBUSY;
+		goto err_release_regs;
+	}
+
+	dsi_pdata = dc->pdata->default_out->dsi;
+	if (!dsi_pdata) {
+		dev_err(&dc->ndev->dev, "dsi: dsi data not available\n");
+		goto err_release_regs;
+	}
+
+	if (dsi_pdata->dsi_instance)
+		dsi_clk = clk_get(&dc->ndev->dev, "dsib");
+	else
+		dsi_clk = clk_get(&dc->ndev->dev, "dsia");
+	dsi_fixed_clk = clk_get(&dc->ndev->dev, "dsi-fixed");
+
+	if (IS_ERR_OR_NULL(dsi_clk) || IS_ERR_OR_NULL(dsi_fixed_clk)) {
+		dev_err(&dc->ndev->dev, "dsi: can't get clock\n");
+		err = -EBUSY;
+		goto err_release_regs;
+	}
+
+	dc_clk = clk_get_sys(dev_name(&dc->ndev->dev), NULL);
+	if (IS_ERR_OR_NULL(dc_clk)) {
+		dev_err(&dc->ndev->dev, "dsi: dc clock %s unavailable\n",
+			dev_name(&dc->ndev->dev));
+		err = -EBUSY;
+		goto err_clk_put;
+	}
+
+	mutex_init(&dsi->lock);
+	dsi->dc = dc;
+	dsi->base = base;
+	dsi->base_res = base_res;
+	dsi->dc_clk = dc_clk;
+	dsi->dsi_clk = dsi_clk;
+	dsi->dsi_fixed_clk = dsi_fixed_clk;
+
+	err = tegra_dc_dsi_cp_info(dsi, dsi_pdata);
+	if (err < 0)
+		goto err_dsi_data;
+
+	tegra_dc_set_outdata(dc, dsi);
+	_tegra_dc_dsi_init(dc);
+
+	return 0;
+
+err_dsi_data:
+err_clk_put:
+	clk_put(dsi_clk);
+	clk_put(dsi_fixed_clk);
+err_release_regs:
+	release_resource(base_res);
+err_free_dsi:
+	kfree(dsi);
+
+	return err;
+}
+
+static void tegra_dc_dsi_destroy(struct tegra_dc *dc)
+{
+	struct tegra_dc_dsi_data *dsi = tegra_dc_get_outdata(dc);
+	u16 i;
+	u32 val;
+
+	mutex_lock(&dsi->lock);
+
+	/* free up the pdata */
+	for (i = 0; i < dsi->info.n_init_cmd; i++) {
+		if (dsi->info.dsi_init_cmd[i].pdata)
+			kfree(dsi->info.dsi_init_cmd[i].pdata);
+	}
+	kfree(dsi->info.dsi_init_cmd);
+
+	/* Disable dc stream */
+	if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
+		tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
+
+	/* Disable dsi phy clock */
+	if (dsi->status.clk_out == DSI_PHYCLK_OUT_EN)
+		tegra_dsi_hs_clk_out_disable(dc, dsi);
+
+	val = DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_DISABLE);
+	tegra_dsi_writel(dsi, val, DSI_POWER_CONTROL);
+
+	iounmap(dsi->base);
+	release_resource(dsi->base_res);
+
+	clk_put(dsi->dc_clk);
+	clk_put(dsi->dsi_clk);
+
+	mutex_unlock(&dsi->lock);
+
+	mutex_destroy(&dsi->lock);
+	kfree(dsi);
+}
+
+static void tegra_dsi_config_phy_clk(struct tegra_dc_dsi_data *dsi,
+							u32 settings)
+{
+	struct clk *parent_clk = NULL;
+	struct clk *base_clk = NULL;
+
+	/* Disable dsi fast and slow clock */
+	parent_clk = clk_get_parent(dsi->dsi_clk);
+	base_clk = clk_get_parent(parent_clk);
+	if (dsi->info.dsi_instance)
+		tegra_clk_cfg_ex(base_clk,
+				TEGRA_CLK_PLLD_CSI_OUT_ENB,
+				settings);
+	else
+		tegra_clk_cfg_ex(base_clk,
+				TEGRA_CLK_PLLD_DSI_OUT_ENB,
+				settings);
+}
+
+static int tegra_dsi_deep_sleep(struct tegra_dc *dc,
+				struct tegra_dc_dsi_data *dsi, u32 suspend_aggr)
+{
+	int val = 0;
+	int err = 0;
+
+	if (!dsi->enabled) {
+		err = -EPERM;
+		goto fail;
+	}
+
+	switch (suspend_aggr) {
+	case DSI_SUSPEND_FULL:
+		/* Suspend DSI panel */
+		err = tegra_dsi_set_to_lp_mode(dc, dsi, DSI_LP_OP_WRITE);
+		if (err < 0) {
+			dev_err(&dc->ndev->dev,
+			"DSI failed to go to LP mode\n");
+			goto fail;
+		}
+
+		err = tegra_dsi_send_panel_cmd(dc, dsi,
+				dsi->info.dsi_suspend_cmd,
+				dsi->info.n_suspend_cmd);
+		/*
+		 * Certain panels need dc frames be sent after
+		 * putting panel to sleep.
+		 */
+		if (dsi->info.panel_send_dc_frames)
+			tegra_dsi_send_dc_frames(dc, dsi, 2);
+
+		if (err < 0) {
+			dev_err(&dc->ndev->dev,
+				"dsi: Error sending suspend cmd\n");
+			goto fail;
+		}
+	case DSI_HOST_SUSPEND_LV2:
+		/* Set DSI to ULPM and suspend pads. DSI will be set to the
+		 * lowest power state in this level. */
+		if (!dsi->ulpm) {
+			err = tegra_dsi_enter_ulpm(dsi);
+			if (err < 0) {
+				dev_err(&dc->ndev->dev,
+					"DSI failed to enter ulpm\n");
+				goto fail;
+			}
+		}
+
+		val = DSI_PAD_CONTROL_PAD_PDIO(0x3) |
+			DSI_PAD_CONTROL_PAD_PDIO_CLK(0x1) |
+			DSI_PAD_CONTROL_PAD_PULLDN_ENAB(TEGRA_DSI_ENABLE);
+		tegra_dsi_writel(dsi, val, DSI_PAD_CONTROL);
+
+		/* Suspend core-logic */
+		val = DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_DISABLE);
+		tegra_dsi_writel(dsi, val, DSI_POWER_CONTROL);
+	case DSI_HOST_SUSPEND_LV1:
+		/* Disable dsi fast and slow clock */
+		tegra_dsi_config_phy_clk(dsi, TEGRA_DSI_DISABLE);
+	case DSI_HOST_SUSPEND_LV0:
+		/* Disable dsi source clock */
+		tegra_dsi_clk_disable(dsi);
+		break;
+	case DSI_NO_SUSPEND:
+		break;
+	default:
+		dev_err(&dc->ndev->dev, "DSI suspend aggressiveness"
+						"is not supported.\n");
+	}
+
+	dsi->enabled = false;
+
+	return 0;
+fail:
+	return err;
+}
+
+static int tegra_dsi_host_suspend(struct tegra_dc *dc)
+{
+	int err = 0;
+	struct tegra_dc_dsi_data *dsi = tegra_dc_get_outdata(dc);
+
+	if (dsi->host_suspended)
+		return 0;
+
+	BUG_ON(!tegra_is_clk_enabled(dc->clk));
+	tegra_dc_io_start(dc);
+	dsi->host_suspended = true;
+
+	tegra_dsi_stop_dc_stream(dc, dsi);
+
+	err = tegra_dsi_deep_sleep(dc, dsi, dsi->info.suspend_aggr);
+	if (err < 0)
+		dev_err(&dc->ndev->dev,
+			"DSI failed to enter deep sleep\n");
+
+	tegra_dc_clk_disable(dc);
+
+	tegra_dc_io_end(dc);
+	return err;
+}
+
+static int tegra_dsi_host_resume(struct tegra_dc *dc)
+{
+	int val = 0;
+	int err = 0;
+	struct tegra_dc_dsi_data *dsi = tegra_dc_get_outdata(dc);
+
+	mutex_lock(&dsi->host_resume_lock);
+	cancel_delayed_work_sync(&dsi->idle_work);
+	if (!dsi->host_suspended) {
+		mutex_unlock(&dsi->host_resume_lock);
+		return 0;
+	}
+
+	tegra_dc_clk_enable(dc);
+	switch (dsi->info.suspend_aggr) {
+	case DSI_HOST_SUSPEND_LV0:
+		tegra_dsi_clk_enable(dsi);
+		break;
+	case DSI_HOST_SUSPEND_LV1:
+		tegra_dsi_config_phy_clk(dsi, TEGRA_DSI_ENABLE);
+		tegra_dsi_clk_enable(dsi);
+		break;
+	case DSI_HOST_SUSPEND_LV2:
+		tegra_dsi_config_phy_clk(dsi, TEGRA_DSI_ENABLE);
+		tegra_dsi_clk_enable(dsi);
+
+		tegra_dsi_writel(dsi,
+			DSI_POWER_CONTROL_LEG_DSI_ENABLE(TEGRA_DSI_ENABLE),
+			DSI_POWER_CONTROL);
+
+		if (dsi->ulpm) {
+			err = tegra_dsi_enter_ulpm(dsi);
+			if (err < 0) {
+				dev_err(&dc->ndev->dev,
+					"DSI failed to enter ulpm\n");
+				goto fail;
+			}
+
+			val = tegra_dsi_readl(dsi, DSI_PAD_CONTROL);
+			val &= ~(DSI_PAD_CONTROL_PAD_PDIO(0x3) |
+				DSI_PAD_CONTROL_PAD_PDIO_CLK(0x1) |
+				DSI_PAD_CONTROL_PAD_PULLDN_ENAB(0x1));
+			tegra_dsi_writel(dsi, val, DSI_PAD_CONTROL);
+
+			if (tegra_dsi_exit_ulpm(dsi) < 0) {
+				dev_err(&dc->ndev->dev,
+					"DSI failed to exit ulpm\n");
+				goto fail;
+			}
+		}
+		break;
+	case DSI_NO_SUSPEND:
+		break;
+	default:
+		dev_err(&dc->ndev->dev, "DSI suspend aggressivenes"
+						"is not supported.\n");
+	}
+
+	tegra_dsi_start_dc_stream(dc, dsi);
+
+	dsi->enabled = true;
+	dsi->host_suspended = false;
+	mutex_unlock(&dsi->host_resume_lock);
+fail:
+	return err;
+}
+
+static void tegra_dc_dsi_disable(struct tegra_dc *dc)
+{
+	int err;
+	struct tegra_dc_dsi_data *dsi = tegra_dc_get_outdata(dc);
+
+	tegra_dc_io_start(dc);
+	mutex_lock(&dsi->lock);
+
+	if (dsi->status.dc_stream == DSI_DC_STREAM_ENABLE)
+		tegra_dsi_stop_dc_stream_at_frame_end(dc, dsi);
+
+	if (dsi->info.power_saving_suspend) {
+		if (tegra_dsi_deep_sleep(dc, dsi, DSI_SUSPEND_FULL) < 0) {
+			dev_err(&dc->ndev->dev,
+				"DSI failed to enter deep sleep\n");
+			goto fail;
+		}
+	} else {
+		if (dsi->info.dsi_early_suspend_cmd) {
+			err = tegra_dsi_send_panel_cmd(dc, dsi,
+				dsi->info.dsi_early_suspend_cmd,
+				dsi->info.n_early_suspend_cmd);
+			if (err < 0) {
+				dev_err(&dc->ndev->dev,
+				"dsi: Error sending early suspend cmd\n");
+				goto fail;
+			}
+		}
+
+		if (!dsi->ulpm) {
+			if (tegra_dsi_enter_ulpm(dsi) < 0) {
+				dev_err(&dc->ndev->dev,
+					"DSI failed to enter ulpm\n");
+				goto fail;
+			}
+		}
+	}
+fail:
+	mutex_unlock(&dsi->lock);
+	tegra_dc_io_end(dc);
+}
+
+#ifdef CONFIG_PM
+static void tegra_dc_dsi_suspend(struct tegra_dc *dc)
+{
+	struct tegra_dc_dsi_data *dsi;
+
+	dsi = tegra_dc_get_outdata(dc);
+
+	if (!dsi->enabled)
+		return;
+
+	tegra_dc_io_start(dc);
+	mutex_lock(&dsi->lock);
+
+	if (!dsi->info.power_saving_suspend) {
+		if (dsi->ulpm) {
+			if (tegra_dsi_exit_ulpm(dsi) < 0) {
+				dev_err(&dc->ndev->dev,
+					"DSI failed to exit ulpm");
+				goto fail;
+			}
+		}
+
+		if (tegra_dsi_deep_sleep(dc, dsi, DSI_SUSPEND_FULL) < 0) {
+			dev_err(&dc->ndev->dev,
+				"DSI failed to enter deep sleep\n");
+			goto fail;
+		}
+	}
+fail:
+	mutex_unlock(&dsi->lock);
+	tegra_dc_io_end(dc);
+}
+
+static void tegra_dc_dsi_resume(struct tegra_dc *dc)
+{
+	/* Not required since tegra_dc_dsi_enable
+	 * will reconfigure the controller from scratch
+	 */
+}
+#endif
+
+struct tegra_dc_out_ops tegra_dc_dsi_ops = {
+	.init = tegra_dc_dsi_init,
+	.destroy = tegra_dc_dsi_destroy,
+	.enable = tegra_dc_dsi_enable,
+	.disable = tegra_dc_dsi_disable,
+	.hold = tegra_dc_dsi_hold_host,
+	.release = tegra_dc_dsi_release_host,
+#ifdef CONFIG_PM
+	.suspend = tegra_dc_dsi_suspend,
+	.resume = tegra_dc_dsi_resume,
+#endif
+};
diff --git a/drivers/video/tegra/dc/dsi.h b/drivers/video/tegra/dc/dsi.h
new file mode 100644
index 000000000000..bf54913a1794
--- /dev/null
+++ b/drivers/video/tegra/dc/dsi.h
@@ -0,0 +1,375 @@
+/*
+ * drivers/video/tegra/dc/dsi.h
+ *
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 __DRIVERS_VIDEO_TEGRA_DC_DSI_H__
+#define __DRIVERS_VIDEO_TEGRA_DC_DSI_H__
+
+/* source of video data */
+enum {
+	TEGRA_DSI_VIDEO_DRIVEN_BY_DC,
+	TEGRA_DSI_VIDEO_DRIVEN_BY_HOST,
+};
+
+/* Max number of data lanes supported */
+#define MAX_DSI_DATA_LANES	2
+/* Default Peripheral reset timeout */
+#define DSI_PR_TO_VALUE		0x2000
+
+/* DCS commands for command mode */
+#define DSI_ENTER_PARTIAL_MODE	0x12
+#define DSI_SET_PIXEL_FORMAT	0x3A
+#define DSI_AREA_COLOR_MODE	0x4C
+#define DSI_SET_PARTIAL_AREA	0x30
+#define DSI_SET_PAGE_ADDRESS	0x2B
+#define DSI_SET_ADDRESS_MODE	0x36
+#define DSI_SET_COLUMN_ADDRESS	0x2A
+#define DSI_WRITE_MEMORY_START	0x2C
+#define DSI_WRITE_MEMORY_CONTINUE	0x3C
+#define DSI_MAX_COMMAND_DELAY_USEC	250000
+#define DSI_COMMAND_DELAY_STEPS_USEC	10
+
+/* Trigger message */
+#define DSI_ESCAPE_CMD	0x87
+#define DSI_ACK_NO_ERR	0x84
+
+/* DSI return packet types */
+#define GEN_LONG_RD_RES 0x1A
+#define DCS_LONG_RD_RES 0x1C
+#define GEN_1_BYTE_SHORT_RD_RES 0x11
+#define DCS_1_BYTE_SHORT_RD_RES 0x21
+#define GEN_2_BYTE_SHORT_RD_RES 0x12
+#define DCS_2_BYTE_SHORT_RD_RES 0x22
+#define ACK_ERR_RES 0x02
+
+/* End of Transmit command for HS mode */
+#define DSI_CMD_HS_EOT_PACKAGE          0x000F0F08
+
+/* Delay required after issuing the trigger*/
+#define DSI_COMMAND_COMPLETION_DELAY_USEC   5
+
+#define DSI_DELAY_FOR_READ_FIFO 5
+
+/* Dsi virtual channel bit position, refer to the DSI specs */
+#define DSI_VIR_CHANNEL_BIT_POSITION	6
+
+/* DSI packet commands from Host to peripherals */
+enum {
+	dsi_command_v_sync_start = 0x01,
+	dsi_command_v_sync_end = 0x11,
+	dsi_command_h_sync_start = 0x21,
+	dsi_command_h_sync_end = 0x31,
+	dsi_command_end_of_transaction = 0x08,
+	dsi_command_blanking = 0x19,
+	dsi_command_null_packet = 0x09,
+	dsi_command_h_active_length_16bpp = 0x0E,
+	dsi_command_h_active_length_18bpp = 0x1E,
+	dsi_command_h_active_length_18bpp_np = 0x2E,
+	dsi_command_h_active_length_24bpp = 0x3E,
+	dsi_command_h_sync_active = dsi_command_blanking,
+	dsi_command_h_back_porch = dsi_command_blanking,
+	dsi_command_h_front_porch = dsi_command_blanking,
+	dsi_command_writ_no_param = 0x05,
+	dsi_command_long_write = 0x39,
+	dsi_command_max_return_pkt_size = 0x37,
+	dsi_command_generic_read_request_with_2_param = 0x24,
+	dsi_command_dcs_read_with_no_params = 0x06,
+};
+
+/* Maximum polling time for reading the dsi status register */
+#define DSI_STATUS_POLLING_DURATION_USEC    100000
+#define DSI_STATUS_POLLING_DELAY_USEC       100
+
+/*
+  * Horizontal Sync Blank Packet Over head
+  * DSI_overhead = size_of(HS packet header)
+  *             + size_of(BLANK packet header) + size_of(checksum)
+  * DSI_overhead = 4 + 4 + 2 = 10
+  */
+#define DSI_HSYNC_BLNK_PKT_OVERHEAD  10
+
+/*
+ * Horizontal Front Porch Packet Overhead
+ * DSI_overhead = size_of(checksum)
+ *            + size_of(BLANK packet header) + size_of(checksum)
+ * DSI_overhead = 2 + 4 + 2 = 8
+ */
+#define DSI_HFRONT_PORCH_PKT_OVERHEAD 8
+
+/*
+ * Horizontal Back Porch Packet
+ * DSI_overhead = size_of(HE packet header)
+ *            + size_of(BLANK packet header) + size_of(checksum)
+ *            + size_of(RGB packet header)
+ * DSI_overhead = 4 + 4 + 2 + 4 = 14
+ */
+#define DSI_HBACK_PORCH_PKT_OVERHEAD  14
+
+/* Additional Hs TX timeout margin */
+#define DSI_HTX_TO_MARGIN   720
+
+#define DSI_CYCLE_COUNTER_VALUE     512
+
+#define DSI_LRXH_TO_VALUE   0x2000
+
+/* Turn around timeout terminal count */
+#define DSI_TA_TO_VALUE     0x2000
+
+/* Turn around timeout tally */
+#define DSI_TA_TALLY_VALUE      0x0
+/* LP Rx timeout tally */
+#define DSI_LRXH_TALLY_VALUE    0x0
+/* HS Tx Timeout tally */
+#define DSI_HTX_TALLY_VALUE     0x0
+
+/* DSI Power control settle time 10 micro seconds */
+#define DSI_POWER_CONTROL_SETTLE_TIME_US    10
+
+#define DSI_HOST_FIFO_DEPTH     64
+#define DSI_VIDEO_FIFO_DEPTH    480
+#define DSI_READ_FIFO_DEPTH	(32 << 2)
+
+#define NUMOF_BIT_PER_BYTE			8
+#define DEFAULT_LP_CMD_MODE_CLK_KHZ		10000
+#define DEFAULT_MAX_DSI_PHY_CLK_KHZ		(500*1000)
+#define DEFAULT_PANEL_RESET_TIMEOUT		2
+#define DEFAULT_PANEL_BUFFER_BYTE		512
+
+/*
+ * TODO: are DSI_HOST_DSI_CONTROL_CRC_RESET(RESET_CRC) and
+ * DSI_HOST_DSI_CONTROL_HOST_TX_TRIG_SRC(IMMEDIATE) required for everyone?
+ */
+#define HOST_DSI_CTRL_COMMON \
+			(DSI_HOST_DSI_CONTROL_PHY_CLK_DIV(DSI_PHY_CLK_DIV1) | \
+			DSI_HOST_DSI_CONTROL_ULTRA_LOW_POWER(NORMAL) | \
+			DSI_HOST_DSI_CONTROL_PERIPH_RESET(TEGRA_DSI_DISABLE) | \
+			DSI_HOST_DSI_CONTROL_RAW_DATA(TEGRA_DSI_DISABLE) | \
+			DSI_HOST_DSI_CONTROL_IMM_BTA(TEGRA_DSI_DISABLE) | \
+			DSI_HOST_DSI_CONTROL_PKT_BTA(TEGRA_DSI_DISABLE) | \
+			DSI_HOST_DSI_CONTROL_CS_ENABLE(TEGRA_DSI_ENABLE) | \
+			DSI_HOST_DSI_CONTROL_ECC_ENABLE(TEGRA_DSI_ENABLE) | \
+			DSI_HOST_DSI_CONTROL_PKT_WR_FIFO_SEL(HOST_ONLY))
+
+#define HOST_DSI_CTRL_HOST_DRIVEN \
+			(DSI_HOST_DSI_CONTROL_CRC_RESET(RESET_CRC) | \
+			DSI_HOST_DSI_CONTROL_HOST_TX_TRIG_SRC(IMMEDIATE))
+
+#define HOST_DSI_CTRL_DC_DRIVEN 0
+
+#define DSI_CTRL_HOST_DRIVEN	(DSI_CONTROL_VID_ENABLE(TEGRA_DSI_DISABLE) | \
+				DSI_CONTROL_HOST_ENABLE(TEGRA_DSI_ENABLE))
+
+#define DSI_CTRL_DC_DRIVEN	(DSI_CONTROL_VID_TX_TRIG_SRC(SOL) | \
+				DSI_CONTROL_VID_ENABLE(TEGRA_DSI_ENABLE) | \
+				DSI_CONTROL_HOST_ENABLE(TEGRA_DSI_DISABLE))
+
+#define DSI_CTRL_CMD_MODE	(DSI_CONTROL_VID_DCS_ENABLE(TEGRA_DSI_ENABLE))
+
+#define DSI_CTRL_VIDEO_MODE	(DSI_CONTROL_VID_DCS_ENABLE(TEGRA_DSI_DISABLE))
+
+
+enum {
+	CMD_VS		= 0x01,
+	CMD_VE		= 0x11,
+
+	CMD_HS		= 0x21,
+	CMD_HE		= 0x31,
+
+	CMD_EOT		= 0x08,
+	CMD_NULL	= 0x09,
+	CMD_SHORTW	= 0x15,
+	CMD_BLNK	= 0x19,
+	CMD_LONGW	= 0x39,
+
+	CMD_RGB	= 0x00,
+	CMD_RGB_16BPP	= 0x0E,
+	CMD_RGB_18BPP	= 0x1E,
+	CMD_RGB_18BPPNP = 0x2E,
+	CMD_RGB_24BPP	= 0x3E,
+};
+
+#define PKT_ID0(id)		(DSI_PKT_SEQ_0_LO_PKT_00_ID(id) | \
+				DSI_PKT_SEQ_1_LO_PKT_10_EN(TEGRA_DSI_ENABLE))
+#define PKT_LEN0(len)	(DSI_PKT_SEQ_0_LO_PKT_00_SIZE(len))
+
+#define PKT_ID1(id)		(DSI_PKT_SEQ_0_LO_PKT_01_ID(id) | \
+				DSI_PKT_SEQ_1_LO_PKT_11_EN(TEGRA_DSI_ENABLE))
+#define PKT_LEN1(len)	(DSI_PKT_SEQ_0_LO_PKT_01_SIZE(len))
+
+#define PKT_ID2(id)		(DSI_PKT_SEQ_0_LO_PKT_02_ID(id) | \
+				DSI_PKT_SEQ_1_LO_PKT_12_EN(TEGRA_DSI_ENABLE))
+#define PKT_LEN2(len)	(DSI_PKT_SEQ_0_LO_PKT_02_SIZE(len))
+
+#define PKT_ID3(id)		(DSI_PKT_SEQ_0_HI_PKT_03_ID(id) | \
+				DSI_PKT_SEQ_1_HI_PKT_13_EN(TEGRA_DSI_ENABLE))
+#define PKT_LEN3(len)	(DSI_PKT_SEQ_0_HI_PKT_03_SIZE(len))
+
+#define PKT_ID4(id)		(DSI_PKT_SEQ_0_HI_PKT_04_ID(id) | \
+				DSI_PKT_SEQ_1_HI_PKT_14_EN(TEGRA_DSI_ENABLE))
+#define PKT_LEN4(len)	(DSI_PKT_SEQ_0_HI_PKT_04_SIZE(len))
+
+#define PKT_ID5(id)		(DSI_PKT_SEQ_0_HI_PKT_05_ID(id) | \
+				DSI_PKT_SEQ_1_HI_PKT_15_EN(TEGRA_DSI_ENABLE))
+#define PKT_LEN5(len)	(DSI_PKT_SEQ_0_HI_PKT_05_SIZE(len))
+
+#define PKT_LP		(DSI_PKT_SEQ_0_LO_SEQ_0_FORCE_LP(TEGRA_DSI_ENABLE))
+
+#define NUMOF_PKT_SEQ	12
+
+/* Mipi v1.00.00 phy timing range */
+#define NOT_DEFINED			-1
+#define MIPI_T_HSEXIT_NS_MIN		100
+#define MIPI_T_HSEXIT_NS_MAX		NOT_DEFINED
+#define	MIPI_T_HSTRAIL_NS_MIN(clk_ns)	max((8 * (clk_ns)), (60 + 4 * (clk_ns)))
+#define MIPI_T_HSTRAIL_NS_MAX		NOT_DEFINED
+#define MIPI_T_HSZERO_NS_MIN		NOT_DEFINED
+#define MIPI_T_HSZERO_NS_MAX		NOT_DEFINED
+#define MIPI_T_HSPREPARE_NS_MIN(clk_ns)	(40 + 4 * (clk_ns))
+#define MIPI_T_HSPREPARE_NS_MAX(clk_ns)	(85 + 6 * (clk_ns))
+#define MIPI_T_CLKTRAIL_NS_MIN		60
+#define MIPI_T_CLKTRAIL_NS_MAX		NOT_DEFINED
+#define	MIPI_T_CLKPOST_NS_MIN(clk_ns)	(60 + 52 * (clk_ns))
+#define MIPI_T_CLKPOST_NS_MAX		NOT_DEFINED
+#define	MIPI_T_CLKZERO_NS_MIN		NOT_DEFINED
+#define MIPI_T_CLKZERO_NS_MAX		NOT_DEFINED
+#define MIPI_T_TLPX_NS_MIN		50
+#define MIPI_T_TLPX_NS_MAX		NOT_DEFINED
+#define MIPI_T_CLKPREPARE_NS_MIN	38
+#define MIPI_T_CLKPREPARE_NS_MAX	95
+#define MIPI_T_CLKPRE_NS_MIN		8
+#define MIPI_T_CLKPRE_NS_MAX		NOT_DEFINED
+#define	MIPI_T_WAKEUP_NS_MIN		1
+#define MIPI_T_WAKEUP_NS_MAX		NOT_DEFINED
+#define MIPI_T_TASURE_NS_MIN(tlpx_ns)	(tlpx_ns)
+#define MIPI_T_TASURE_NS_MAX(tlpx_ns)	(2 * (tlpx_ns))
+#define MIPI_T_HSPREPARE_ADD_HSZERO_NS_MIN(clk_ns)	(145 + 10 * (clk_ns))
+#define MIPI_T_HSPREPARE_ADD_HSZERO_NS_MAX		NOT_DEFINED
+#define	MIPI_T_CLKPREPARE_ADD_CLKZERO_NS_MIN		300
+#define MIPI_T_CLKPREPARE_ADD_CLKZERO_NS_MAX		NOT_DEFINED
+
+#define DSI_TBYTE(clk_ns)	((clk_ns) * (BITS_PER_BYTE))
+#define DSI_CONVERT_T_PHY_NS_TO_T_PHY(t_phy_ns, clk_ns, hw_inc) \
+				((int)((DIV_ROUND_CLOSEST((t_phy_ns), \
+				(DSI_TBYTE(clk_ns)))) - (hw_inc)))
+
+#define DSI_CONVERT_T_PHY_TO_T_PHY_NS(t_phy, clk_ns, hw_inc) \
+				(((t_phy) + (hw_inc)) * (DSI_TBYTE(clk_ns)))
+
+/* Default phy timing in ns */
+#define T_HSEXIT_NS_DEFAULT		120
+#define T_HSTRAIL_NS_DEFAULT(clk_ns) \
+			max((8 * (clk_ns)), (60 + 4 * (clk_ns)))
+
+#define T_DATZERO_NS_DEFAULT(clk_ns)	(145 + 5 * (clk_ns))
+#define T_HSPREPARE_NS_DEFAULT(clk_ns)	(65 + 5 * (clk_ns))
+#define T_CLKTRAIL_NS_DEFAULT		80
+#define T_CLKPOST_NS_DEFAULT(clk_ns)	(70 + 52 * (clk_ns))
+#define T_CLKZERO_NS_DEFAULT		260
+#define T_TLPX_NS_DEFAULT		60
+#define T_CLKPREPARE_NS_DEFAULT		65
+#define T_TAGO_NS_DEFAULT		(4 * (T_TLPX_NS_DEFAULT))
+#define T_TASURE_NS_DEFAULT		(2 * (T_TLPX_NS_DEFAULT))
+#define T_TAGET_NS_DEFAULT		(5 * (T_TLPX_NS_DEFAULT))
+
+/* HW increment to phy register values */
+#define T_HSEXIT_HW_INC		1
+#define T_HSTRAIL_HW_INC	0
+#define T_DATZERO_HW_INC	3
+#define T_HSPREPARE_HW_INC	1
+#define T_CLKTRAIL_HW_INC	1
+#define T_CLKPOST_HW_INC	1
+#define T_CLKZERO_HW_INC	1
+#define T_TLPX_HW_INC		1
+#define T_CLKPREPARE_HW_INC	1
+#define T_TAGO_HW_INC		1
+#define T_TASURE_HW_INC		1
+#define T_TAGET_HW_INC		1
+#define T_CLKPRE_HW_INC		1
+#define T_WAKEUP_HW_INC		1
+
+/* Default phy timing reg values */
+#define T_HSEXIT_DEFAULT(clk_ns) \
+(DSI_CONVERT_T_PHY_NS_TO_T_PHY( \
+T_HSEXIT_NS_DEFAULT, clk_ns, T_HSEXIT_HW_INC))
+
+#define T_HSTRAIL_DEFAULT(clk_ns) \
+(3 + (DSI_CONVERT_T_PHY_NS_TO_T_PHY( \
+T_HSTRAIL_NS_DEFAULT(clk_ns), clk_ns, T_HSTRAIL_HW_INC)))
+
+#define T_DATZERO_DEFAULT(clk_ns) \
+(DSI_CONVERT_T_PHY_NS_TO_T_PHY( \
+T_DATZERO_NS_DEFAULT(clk_ns), clk_ns, T_DATZERO_HW_INC))
+
+#define T_HSPREPARE_DEFAULT(clk_ns) \
+(DSI_CONVERT_T_PHY_NS_TO_T_PHY( \
+T_HSPREPARE_NS_DEFAULT(clk_ns), clk_ns, T_HSPREPARE_HW_INC))
+
+#define T_CLKTRAIL_DEFAULT(clk_ns) \
+(DSI_CONVERT_T_PHY_NS_TO_T_PHY( \
+T_CLKTRAIL_NS_DEFAULT, clk_ns, T_CLKTRAIL_HW_INC))
+
+#define T_CLKPOST_DEFAULT(clk_ns) \
+(DSI_CONVERT_T_PHY_NS_TO_T_PHY( \
+T_CLKPOST_NS_DEFAULT(clk_ns), clk_ns, T_CLKPOST_HW_INC))
+
+#define T_CLKZERO_DEFAULT(clk_ns) \
+(DSI_CONVERT_T_PHY_NS_TO_T_PHY( \
+T_CLKZERO_NS_DEFAULT, clk_ns, T_CLKZERO_HW_INC))
+
+#define T_TLPX_DEFAULT(clk_ns) \
+(DSI_CONVERT_T_PHY_NS_TO_T_PHY( \
+T_TLPX_NS_DEFAULT, clk_ns, T_TLPX_HW_INC))
+
+#define T_CLKPREPARE_DEFAULT(clk_ns) \
+(DSI_CONVERT_T_PHY_NS_TO_T_PHY( \
+T_CLKPREPARE_NS_DEFAULT, clk_ns, T_CLKPREPARE_HW_INC))
+
+#define T_CLKPRE_DEFAULT	0x1
+#define T_WAKEUP_DEFAULT	0x7f
+
+#define T_TAGO_DEFAULT(clk_ns) \
+(DSI_CONVERT_T_PHY_NS_TO_T_PHY( \
+T_TAGO_NS_DEFAULT, clk_ns, T_TAGO_HW_INC))
+
+#define T_TASURE_DEFAULT(clk_ns) \
+(DSI_CONVERT_T_PHY_NS_TO_T_PHY( \
+T_TASURE_NS_DEFAULT, clk_ns, T_TASURE_HW_INC))
+
+#define T_TAGET_DEFAULT(clk_ns) \
+(DSI_CONVERT_T_PHY_NS_TO_T_PHY( \
+T_TAGET_NS_DEFAULT, clk_ns, T_TAGET_HW_INC))
+
+/* Defines the DSI phy timing parameters */
+struct dsi_phy_timing_inclk {
+	unsigned	t_hsdexit;
+	unsigned	t_hstrail;
+	unsigned	t_hsprepare;
+	unsigned	t_datzero;
+
+	unsigned	t_clktrail;
+	unsigned	t_clkpost;
+	unsigned	t_clkzero;
+	unsigned	t_tlpx;
+
+	unsigned	t_clkpre;
+	unsigned	t_clkprepare;
+	unsigned	t_wakeup;
+
+	unsigned	t_taget;
+	unsigned	t_tasure;
+	unsigned	t_tago;
+};
+
+#endif
diff --git a/drivers/video/tegra/dc/dsi_regs.h b/drivers/video/tegra/dc/dsi_regs.h
new file mode 100644
index 000000000000..71045fcec29e
--- /dev/null
+++ b/drivers/video/tegra/dc/dsi_regs.h
@@ -0,0 +1,351 @@
+/*
+ * drivers/video/tegra/dc/dsi_regs.h
+ *
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 __DRIVERS_VIDEO_TEGRA_DC_DSI_REG_H__
+#define __DRIVERS_VIDEO_TEGRA_DC_DSI_REG_H__
+
+enum {
+	TEGRA_DSI_DISABLE,
+	TEGRA_DSI_ENABLE,
+};
+
+/* These are word offsets from base (not byte offsets) */
+enum {
+	OP_DONE = 1,
+};
+#define DSI_INCR_SYNCPT 0x00
+#define DSI_INCR_SYNCPT_COND(x)		(((x) & 0xff) << 8)
+#define DSI_INCR_SYNCPT_INDX(x)		(((x) & 0xff) << 0)
+
+#define DSI_INCR_SYNCPT_CNTRL 0x01
+#define DSI_INCR_SYNCPT_ERROR 0x02
+#define DSI_CTXSW 0x08
+#define DSI_RD_DATA 0x09
+#define DSI_WR_DATA 0x0a
+
+#define DSI_POWER_CONTROL 0x0b
+#define   DSI_POWER_CONTROL_LEG_DSI_ENABLE(x)		(((x) & 0x1) << 0)
+
+#define DSI_INT_ENABLE 0x0c
+#define DSI_INT_STATUS 0x0d
+#define DSI_INT_MASK 0x0e
+
+#define DSI_HOST_DSI_CONTROL 0x0f
+enum {
+	RESET_CRC = 1,
+};
+#define   DSI_HOST_CONTROL_FIFO_STAT_RESET(x)		(((x) & 0x1) << 21)
+#define   DSI_HOST_DSI_CONTROL_CRC_RESET(x)		(((x) & 0x1) << 20)
+enum {
+	DSI_PHY_CLK_DIV1,
+	DSI_PHY_CLK_DIV2,
+};
+#define   DSI_HOST_DSI_CONTROL_PHY_CLK_DIV(x)		(((x) & 0x7) << 16)
+enum {
+	SOL,
+	FIFO_LEVEL,
+	IMMEDIATE,
+};
+#define   DSI_HOST_DSI_CONTROL_HOST_TX_TRIG_SRC(x)	(((x) & 0x3) << 12)
+enum {
+	NORMAL,
+	ENTER_ULPM,
+	EXIT_ULPM,
+};
+#define   DSI_HOST_DSI_CONTROL_ULTRA_LOW_POWER(x)	(((x) & 0x3) << 8)
+#define   DSI_HOST_DSI_CONTROL_PERIPH_RESET(x)		(((x) & 0x1) << 7)
+#define   DSI_HOST_DSI_CONTROL_RAW_DATA(x)		(((x) & 0x1) << 6)
+enum {
+	TEGRA_DSI_LOW,
+	TEGRA_DSI_HIGH,
+};
+#define   DSI_HOST_DSI_CONTROL_HIGH_SPEED_TRANS(x)	(((x) & 0x1) << 5)
+enum {
+	HOST_ONLY,
+	VIDEO_HOST,
+};
+#define   DSI_HOST_DSI_CONTROL_PKT_WR_FIFO_SEL(x)	(((x) & 0x1) << 4)
+#define   DSI_HOST_DSI_CONTROL_IMM_BTA(x)		(((x) & 0x1) << 3)
+#define   DSI_HOST_DSI_CONTROL_PKT_BTA(x)		(((x) & 0x1) << 2)
+#define   DSI_HOST_DSI_CONTROL_CS_ENABLE(x)		(((x) & 0x1) << 1)
+#define   DSI_HOST_DSI_CONTROL_ECC_ENABLE(x)		(((x) & 0x1) << 0)
+
+#define DSI_CONTROL 0x10
+#define   DSI_CONTROL_DBG_ENABLE(x)			(((x) & 0x1) << 31)
+enum {
+	CONTINUOUS,
+	TX_ONLY,
+};
+#define   DSI_CONTROL_HS_CLK_CTRL(x)			(((x) & 0x1) << 20)
+#define   DSI_CONTROL_VIRTUAL_CHANNEL(x)		(((x) & 0x3) << 16)
+#define   DSI_CONTROL_DATA_FORMAT(x)			(((x) & 0x3) << 12)
+#define   DSI_CONTROL_VID_TX_TRIG_SRC(x)		(((x) & 0x3) << 8)
+#define   DSI_CONTROL_NUM_DATA_LANES(x)			(((x) & 0x3) << 4)
+#define   DSI_CONTROL_VID_DCS_ENABLE(x)			(((x) & 0x1) << 3)
+#define   DSI_CONTROL_VID_SOURCE(x)			(((x) & 0x1) << 2)
+#define   DSI_CONTROL_VID_ENABLE(x)			(((x) & 0x1) << 1)
+#define   DSI_CONTROL_HOST_ENABLE(x)			(((x) & 0x1) << 0)
+
+#define DSI_SOL_DELAY 0x11
+#define DSI_SOL_DELAY_SOL_DELAY(x)			(((x) & 0xffff) << 0)
+
+#define DSI_MAX_THRESHOLD 0x12
+#define DSI_MAX_THRESHOLD_MAX_THRESHOLD(x)		(((x) & 0xffff) << 0)
+
+#define DSI_TRIGGER 0x13
+#define DSI_TRIGGER_HOST_TRIGGER(x)			(((x) & 0x1) << 1)
+#define DSI_TRIGGER_VID_TRIGGER(x)			(((x) & 0x1) << 0)
+
+#define DSI_TX_CRC 0x14
+#define DSI_TX_CRC_TX_CRC(x)			(((x) & 0xffffffff) << 0)
+
+#define DSI_STATUS 0x15
+#define DSI_STATUS_IDLE(x)			(((x) & 0x1) << 10)
+#define DSI_STATUS_LB_UNDERFLOW(x)		(((x) & 0x1) << 9)
+#define DSI_STATUS_LB_OVERFLOW(x)		(((x) & 0x1) << 8)
+#define DSI_STATUS_RD_FIFO_COUNT(x)		(((x) & 0x1f) << 0)
+
+#define DSI_INIT_SEQ_CONTROL 0x1a
+#define   DSI_INIT_SEQ_CONTROL_DSI_FRAME_INIT_BYTE_COUNT(x) \
+				(((x) & 0x3f) << 8)
+#define   DSI_INIT_SEQ_CONTROL_DSI_SEND_INIT_SEQUENCE(x) \
+				(((x) & 0xff) << 0)
+
+#define DSI_INIT_SEQ_DATA_0 0x1b
+#define DSI_INIT_SEQ_DATA_1 0x1c
+#define DSI_INIT_SEQ_DATA_2 0x1d
+#define DSI_INIT_SEQ_DATA_3 0x1e
+#define DSI_INIT_SEQ_DATA_4 0x1f
+#define DSI_INIT_SEQ_DATA_5 0x20
+#define DSI_INIT_SEQ_DATA_6 0x21
+#define DSI_INIT_SEQ_DATA_7 0x22
+
+#define DSI_PKT_SEQ_0_LO 0x23
+#define   DSI_PKT_SEQ_0_LO_SEQ_0_FORCE_LP(x)	(((x) & 0x1) << 30)
+#define   DSI_PKT_SEQ_0_LO_PKT_02_EN(x)		(((x) & 0x1) << 29)
+#define   DSI_PKT_SEQ_0_LO_PKT_02_ID(x)		(((x) & 0x3f) << 23)
+#define   DSI_PKT_SEQ_0_LO_PKT_02_SIZE(x)	(((x) & 0x7) << 20)
+#define   DSI_PKT_SEQ_0_LO_PKT_01_EN(x)		(((x) & 0x1) << 19)
+#define   DSI_PKT_SEQ_0_LO_PKT_01_ID(x)		(((x) & 0x3f) << 13)
+#define   DSI_PKT_SEQ_0_LO_PKT_01_SIZE(x)	(((x) & 0x7) << 10)
+#define   DSI_PKT_SEQ_0_LO_PKT_00_EN(x)		(((x) & 0x1) << 9)
+#define   DSI_PKT_SEQ_0_LO_PKT_00_ID(x)		(((x) & 0x3f) << 3)
+#define   DSI_PKT_SEQ_0_LO_PKT_00_SIZE(x)	(((x) & 0x7) << 0)
+
+#define DSI_PKT_SEQ_0_HI 0x24
+#define   DSI_PKT_SEQ_0_HI_PKT_05_EN(x)		(((x) & 0x1) << 29)
+#define   DSI_PKT_SEQ_0_HI_PKT_05_ID(x)		(((x) & 0x3f) << 23)
+#define   DSI_PKT_SEQ_0_HI_PKT_05_SIZE(x)	(((x) & 0x7) << 20)
+#define   DSI_PKT_SEQ_0_HI_PKT_04_EN(x)		(((x) & 0x1) << 19)
+#define   DSI_PKT_SEQ_0_HI_PKT_04_ID(x)		(((x) & 0x3f) << 13)
+#define   DSI_PKT_SEQ_0_HI_PKT_04_SIZE(x)	(((x) & 0x7) << 10)
+#define   DSI_PKT_SEQ_0_HI_PKT_03_EN(x)		(((x) & 0x1) << 9)
+#define   DSI_PKT_SEQ_0_HI_PKT_03_ID(x)		(((x) & 0x3f) << 3)
+#define   DSI_PKT_SEQ_0_HI_PKT_03_SIZE(x)	(((x) & 0x7) << 0)
+
+#define DSI_PKT_SEQ_1_LO 0x25
+#define   DSI_PKT_SEQ_1_LO_SEQ_1_FORCE_LP(x)	(((x) & 0x1) << 30)
+#define   DSI_PKT_SEQ_1_LO_PKT_12_EN(x)		(((x) & 0x1) << 29)
+#define   DSI_PKT_SEQ_1_LO_PKT_12_ID(x)		(((x) & 0x3f) << 23)
+#define   DSI_PKT_SEQ_1_LO_PKT_12_SIZE(x)	(((x) & 0x7) << 20)
+#define   DSI_PKT_SEQ_1_LO_PKT_11_EN(x)		(((x) & 0x1) << 19)
+#define   DSI_PKT_SEQ_1_LO_PKT_11_ID(x)		(((x) & 0x3f) << 13)
+#define   DSI_PKT_SEQ_1_LO_PKT_11_SIZE(x)	(((x) & 0x7) << 10)
+#define   DSI_PKT_SEQ_1_LO_PKT_10_EN(x)		(((x) & 0x1) << 9)
+#define   DSI_PKT_SEQ_1_LO_PKT_10_ID(x)		(((x) & 0x3f) << 3)
+#define   DSI_PKT_SEQ_1_LO_PKT_10_SIZE(x)	(((x) & 0x7) << 0)
+
+#define DSI_PKT_SEQ_1_HI 0x26
+#define   DSI_PKT_SEQ_1_HI_PKT_15_EN(x)		(((x) & 0x1) << 29)
+#define   DSI_PKT_SEQ_1_HI_PKT_15_ID(x)		(((x) & 0x3f) << 23)
+#define   DSI_PKT_SEQ_1_HI_PKT_15_SIZE(x)	(((x) & 0x7) << 20)
+#define   DSI_PKT_SEQ_1_HI_PKT_14_EN(x)		(((x) & 0x1) << 19)
+#define   DSI_PKT_SEQ_1_HI_PKT_14_ID(x)		(((x) & 0x3f) << 13)
+#define   DSI_PKT_SEQ_1_HI_PKT_14_SIZE(x)	(((x) & 0x7) << 10)
+#define   DSI_PKT_SEQ_1_HI_PKT_13_EN(x)		(((x) & 0x1) << 9)
+#define   DSI_PKT_SEQ_1_HI_PKT_13_ID(x)		(((x) & 0x3f) << 3)
+#define   DSI_PKT_SEQ_1_HI_PKT_13_SIZE(x)	(((x) & 0x7) << 0)
+
+#define DSI_PKT_SEQ_2_LO 0x27
+#define   DSI_PKT_SEQ_2_LO_SEQ_2_FORCE_LP(x)	(((x) & 0x1) << 30)
+#define   DSI_PKT_SEQ_2_LO_PKT_22_EN(x)		(((x) & 0x1) << 29)
+#define   DSI_PKT_SEQ_2_LO_PKT_22_ID(x)		(((x) & 0x3f) << 23)
+#define   DSI_PKT_SEQ_2_LO_PKT_22_SIZE(x)	(((x) & 0x7) << 20)
+#define   DSI_PKT_SEQ_2_LO_PKT_21_EN(x)		(((x) & 0x1) << 19)
+#define   DSI_PKT_SEQ_2_LO_PKT_21_ID(x)		(((x) & 0x3f) << 13)
+#define   DSI_PKT_SEQ_2_LO_PKT_21_SIZE(x)	(((x) & 0x7) << 10)
+#define   DSI_PKT_SEQ_2_LO_PKT_20_EN(x)		(((x) & 0x1) << 9)
+#define   DSI_PKT_SEQ_2_LO_PKT_20_ID(x)		(((x) & 0x3f) << 3)
+#define   DSI_PKT_SEQ_2_LO_PKT_20_SIZE(x)	(((x) & 0x7) << 0)
+
+#define DSI_PKT_SEQ_2_HI 0x28
+#define   DSI_PKT_SEQ_2_HI_PKT_25_EN(x)		(((x) & 0x1) << 29)
+#define   DSI_PKT_SEQ_2_HI_PKT_25_ID(x)		(((x) & 0x3f) << 23)
+#define   DSI_PKT_SEQ_2_HI_PKT_25_SIZE(x)	(((x) & 0x7) << 20)
+#define   DSI_PKT_SEQ_2_HI_PKT_24_EN(x)		(((x) & 0x1) << 19)
+#define   DSI_PKT_SEQ_2_HI_PKT_24_ID(x)		(((x) & 0x3f) << 13)
+#define   DSI_PKT_SEQ_2_HI_PKT_24_SIZE(x)	(((x) & 0x7) << 10)
+#define   DSI_PKT_SEQ_2_HI_PKT_23_EN(x)		(((x) & 0x1) << 9)
+#define   DSI_PKT_SEQ_2_HI_PKT_23_ID(x)		(((x) & 0x3f) << 3)
+#define   DSI_PKT_SEQ_2_HI_PKT_23_SIZE(x)	(((x) & 0x7) << 0)
+
+#define DSI_PKT_SEQ_3_LO 0x29
+#define   DSI_PKT_SEQ_3_LO_SEQ_3_FORCE_LP(x)	(((x) & 0x1) << 30)
+#define   DSI_PKT_SEQ_3_LO_PKT_32_EN(x)		(((x) & 0x1) << 29)
+#define   DSI_PKT_SEQ_3_LO_PKT_32_ID(x)		(((x) & 0x3f) << 23)
+#define   DSI_PKT_SEQ_3_LO_PKT_32_SIZE(x)	(((x) & 0x7) << 20)
+#define   DSI_PKT_SEQ_3_LO_PKT_31_EN(x)		(((x) & 0x1) << 19)
+#define   DSI_PKT_SEQ_3_LO_PKT_31_ID(x)		(((x) & 0x3f) << 13)
+#define   DSI_PKT_SEQ_3_LO_PKT_31_SIZE(x)	(((x) & 0x7) << 10)
+#define   DSI_PKT_SEQ_3_LO_PKT_30_EN(x)		(((x) & 0x1) << 9)
+#define   DSI_PKT_SEQ_3_LO_PKT_30_ID(x)		(((x) & 0x3f) << 3)
+#define   DSI_PKT_SEQ_3_LO_PKT_30_SIZE(x)	(((x) & 0x7) << 0)
+
+#define DSI_PKT_SEQ_3_HI 0x2a
+#define   DSI_PKT_SEQ_3_HI_PKT_35_EN(x)		(((x) & 0x1) << 29)
+#define   DSI_PKT_SEQ_3_HI_PKT_35_ID(x)		(((x) & 0x3f) << 23)
+#define   DSI_PKT_SEQ_3_HI_PKT_35_SIZE(x)	(((x) & 0x7) << 20)
+#define   DSI_PKT_SEQ_3_HI_PKT_34_EN(x)		(((x) & 0x1) << 19)
+#define   DSI_PKT_SEQ_3_HI_PKT_34_ID(x)		(((x) & 0x3f) << 13)
+#define   DSI_PKT_SEQ_3_HI_PKT_34_SIZE(x)	(((x) & 0x7) << 10)
+#define   DSI_PKT_SEQ_3_HI_PKT_33_EN(x)		(((x) & 0x1) << 9)
+#define   DSI_PKT_SEQ_3_HI_PKT_33_ID(x)		(((x) & 0x3f) << 3)
+#define   DSI_PKT_SEQ_3_HI_PKT_33_SIZE(x)	(((x) & 0x7) << 0)
+
+#define DSI_PKT_SEQ_4_LO 0x2b
+#define   DSI_PKT_SEQ_4_LO_SEQ_4_FORCE_LP(x)	(((x) & 0x1) << 30)
+#define   DSI_PKT_SEQ_4_LO_PKT_42_EN(x)		(((x) & 0x1) << 29)
+#define   DSI_PKT_SEQ_4_LO_PKT_42_ID(x)		(((x) & 0x3f) << 23)
+#define   DSI_PKT_SEQ_4_LO_PKT_42_SIZE(x)	(((x) & 0x7) << 20)
+#define   DSI_PKT_SEQ_4_LO_PKT_41_EN(x)		(((x) & 0x1) << 19)
+#define   DSI_PKT_SEQ_4_LO_PKT_41_ID(x)		(((x) & 0x3f) << 13)
+#define   DSI_PKT_SEQ_4_LO_PKT_41_SIZE(x)	(((x) & 0x7) << 10)
+#define   DSI_PKT_SEQ_4_LO_PKT_40_EN(x)		(((x) & 0x1) << 9)
+#define   DSI_PKT_SEQ_4_LO_PKT_40_ID(x)		(((x) & 0x3f) << 3)
+#define   DSI_PKT_SEQ_4_LO_PKT_40_SIZE(x)	(((x) & 0x7) << 0)
+
+#define DSI_PKT_SEQ_4_HI 0x2c
+#define   DSI_PKT_SEQ_4_HI_PKT_45_EN(x)		(((x) & 0x1) << 29)
+#define   DSI_PKT_SEQ_4_HI_PKT_45_ID(x)		(((x) & 0x3f) << 23)
+#define   DSI_PKT_SEQ_4_HI_PKT_45_SIZE(x)	(((x) & 0x7) << 20)
+#define   DSI_PKT_SEQ_4_HI_PKT_44_EN(x)		(((x) & 0x1) << 19)
+#define   DSI_PKT_SEQ_4_HI_PKT_44_ID(x)		(((x) & 0x3f) << 13)
+#define   DSI_PKT_SEQ_4_HI_PKT_44_SIZE(x)	(((x) & 0x7) << 10)
+#define   DSI_PKT_SEQ_4_HI_PKT_43_EN(x)		(((x) & 0x1) << 9)
+#define   DSI_PKT_SEQ_4_HI_PKT_43_ID(x)		(((x) & 0x3f) << 3)
+#define   DSI_PKT_SEQ_4_HI_PKT_43_SIZE(x)	(((x) & 0x7) << 0)
+
+#define DSI_PKT_SEQ_5_LO 0x2d
+#define   DSI_PKT_SEQ_5_LO_SEQ_5_FORCE_LP(x)	(((x) & 0x1) << 30)
+#define   DSI_PKT_SEQ_5_LO_PKT_52_EN(x)		(((x) & 0x1) << 29)
+#define   DSI_PKT_SEQ_5_LO_PKT_52_ID(x)		(((x) & 0x3f) << 23)
+#define   DSI_PKT_SEQ_5_LO_PKT_52_SIZE(x)	(((x) & 0x7) << 20)
+#define   DSI_PKT_SEQ_5_LO_PKT_51_EN(x)		(((x) & 0x1) << 19)
+#define   DSI_PKT_SEQ_5_LO_PKT_51_ID(x)		(((x) & 0x3f) << 13)
+#define   DSI_PKT_SEQ_5_LO_PKT_51_SIZE(x)	(((x) & 0x7) << 10)
+#define   DSI_PKT_SEQ_5_LO_PKT_50_EN(x)		(((x) & 0x1) << 9)
+#define   DSI_PKT_SEQ_5_LO_PKT_50_ID(x)		(((x) & 0x3f) << 3)
+#define   DSI_PKT_SEQ_5_LO_PKT_50_SIZE(x)	(((x) & 0x7) << 0)
+
+#define DSI_PKT_SEQ_5_HI 0x2e
+#define   DSI_PKT_SEQ_5_HI_PKT_55_EN(x)		(((x) & 0x1) << 29)
+#define   DSI_PKT_SEQ_5_HI_PKT_55_ID(x)		(((x) & 0x3f) << 23)
+#define   DSI_PKT_SEQ_5_HI_PKT_55_SIZE(x)	(((x) & 0x7) << 20)
+#define   DSI_PKT_SEQ_5_HI_PKT_54_EN(x)		(((x) & 0x1) << 19)
+#define   DSI_PKT_SEQ_5_HI_PKT_54_ID(x)		(((x) & 0x3f) << 13)
+#define   DSI_PKT_SEQ_5_HI_PKT_54_SIZE(x)	(((x) & 0x7) << 10)
+#define   DSI_PKT_SEQ_5_HI_PKT_53_EN(x)		(((x) & 0x1) << 9)
+#define   DSI_PKT_SEQ_5_HI_PKT_53_ID(x)		(((x) & 0x3f) << 3)
+#define   DSI_PKT_SEQ_5_HI_PKT_53_SIZE(x)	(((x) & 0x7) << 0)
+
+#define DSI_DCS_CMDS 0x33
+#define   DSI_DCS_CMDS_LT5_DCS_CMD(x)		(((x) & 0xff) << 8)
+#define   DSI_DCS_CMDS_LT3_DCS_CMD(x)		(((x) & 0xff) << 0)
+
+#define DSI_PKT_LEN_0_1 0x34
+#define   DSI_PKT_LEN_0_1_LENGTH_1(x)		(((x) & 0xffff) << 16)
+#define   DSI_PKT_LEN_0_1_LENGTH_0(x)		(((x) & 0xffff) << 0)
+
+#define DSI_PKT_LEN_2_3 0x35
+#define   DSI_PKT_LEN_2_3_LENGTH_3(x)		(((x) & 0xffff) << 16)
+#define   DSI_PKT_LEN_2_3_LENGTH_2(x)		(((x) & 0xffff) << 0)
+
+
+#define DSI_PKT_LEN_4_5 0x36
+#define   DSI_PKT_LEN_4_5_LENGTH_5(x)		(((x) & 0xffff) << 16)
+#define   DSI_PKT_LEN_4_5_LENGTH_4(x)		(((x) & 0xffff) << 0)
+
+#define DSI_PKT_LEN_6_7 0x37
+#define   DSI_PKT_LEN_6_7_LENGTH_7(x)		(((x) & 0xffff) << 16)
+#define   DSI_PKT_LEN_6_7_LENGTH_6(x)		(((x) & 0xffff) << 0)
+
+#define DSI_PHY_TIMING_0 0x3c
+#define   DSI_PHY_TIMING_0_THSDEXIT(x)		(((x) & 0xff) << 24)
+#define   DSI_PHY_TIMING_0_THSTRAIL(x)		(((x) & 0xff) << 16)
+#define   DSI_PHY_TIMING_0_TDATZERO(x)		(((x) & 0xff) << 8)
+#define   DSI_PHY_TIMING_0_THSPREPR(x)		(((x) & 0xff) << 0)
+
+#define DSI_PHY_TIMING_1 0x3d
+#define   DSI_PHY_TIMING_1_TCLKTRAIL(x)		(((x) & 0xff) << 24)
+#define	  DSI_PHY_TIMING_1_TCLKPOST(x)		(((x) & 0xff) << 16)
+#define   DSI_PHY_TIMING_1_TCLKZERO(x)		(((x) & 0xff) << 8)
+#define   DSI_PHY_TIMING_1_TTLPX(x)		(((x) & 0xff) << 0)
+
+#define DSI_PHY_TIMING_2 0x3e
+#define   DSI_PHY_TIMING_2_TCLKPREPARE(x)	(((x) & 0xff) << 16)
+#define	  DSI_PHY_TIMING_2_TCLKPRE(x)		(((x) & 0xff) << 8)
+#define   DSI_PHY_TIMING_2_TWAKEUP(x)		(((x) & 0xff) << 0)
+
+#define DSI_BTA_TIMING 0x3f
+#define   DSI_BTA_TIMING_TTAGET(x)		(((x) & 0xff) << 16)
+#define	  DSI_BTA_TIMING_TTASURE(x)		(((x) & 0xff) << 8)
+#define   DSI_BTA_TIMING_TTAGO(x)		(((x) & 0xff) << 0)
+
+
+#define DSI_TIMEOUT_0 0x44
+#define	  DSI_TIMEOUT_0_LRXH_TO(x)		(((x) & 0xffff) << 16)
+#define   DSI_TIMEOUT_0_HTX_TO(x)		(((x) & 0xffff) << 0)
+
+#define DSI_TIMEOUT_1 0x45
+#define	  DSI_TIMEOUT_1_PR_TO(x)		(((x) & 0xffff) << 16)
+#define   DSI_TIMEOUT_1_TA_TO(x)		(((x) & 0xffff) << 0)
+
+#define DSI_TO_TALLY 0x46
+enum {
+	IN_RESET,
+	READY,
+};
+#define DSI_TO_TALLY_P_RESET_STATUS(x)		(((x) & 0x1) << 24)
+#define DSI_TO_TALLY_TA_TALLY(x)		(((x) & 0xff) << 16)
+#define DSI_TO_TALLY_LRXH_TALLY(x)		(((x) & 0xff) << 8)
+#define DSI_TO_TALLY_HTX_TALLY(x)		(((x) & 0xff) << 0)
+
+#define DSI_PAD_CONTROL 0x4b
+#define DSI_PAD_CONTROL_PAD_PULLDN_ENAB(x)	(((x) & 0x1) << 28)
+#define DSI_PAD_CONTROL_PAD_SLEWUPADJ(x)	(((x) & 0x7) << 24)
+#define DSI_PAD_CONTROL_PAD_SLEWDNADJ(x)	(((x) & 0x7) << 20)
+#define DSI_PAD_CONTROL_PAD_PREEMP_EN(x)	(((x) & 0x1) << 19)
+#define DSI_PAD_CONTROL_PAD_PDIO_CLK(x)		(((x) & 0x1) << 18)
+#define DSI_PAD_CONTROL_PAD_PDIO(x)		(((x) & 0x3) << 16)
+#define DSI_PAD_CONTROL_PAD_LPUPADJ(x)		(((x) & 0x3) << 14)
+#define DSI_PAD_CONTROL_PAD_LPDNADJ(x)		(((x) & 0x3) << 12)
+
+#define DSI_PAD_CONTROL_CD 0x4c
+#define DSI_PAD_CD_STATUS 0x4d
+#define DSI_VID_MODE_CONTROL 0x4e
+
+#endif
+
diff --git a/drivers/video/tegra/dc/edid.c b/drivers/video/tegra/dc/edid.c
new file mode 100644
index 000000000000..b1de7ab1dfb5
--- /dev/null
+++ b/drivers/video/tegra/dc/edid.c
@@ -0,0 +1,617 @@
+/*
+ * drivers/video/tegra/dc/edid.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/debugfs.h>
+#include <linux/fb.h>
+#include <linux/i2c.h>
+#include <linux/seq_file.h>
+#include <linux/vmalloc.h>
+
+#include "edid.h"
+
+struct tegra_edid_pvt {
+	struct kref			refcnt;
+	struct tegra_edid_hdmi_eld	eld;
+	bool				support_stereo;
+	bool				support_underscan;
+	/* Note: dc_edid must remain the last member */
+	struct tegra_dc_edid		dc_edid;
+};
+
+struct tegra_edid {
+	struct i2c_client	*client;
+	struct i2c_board_info	info;
+	int			bus;
+
+	struct tegra_edid_pvt	*data;
+
+	struct mutex		lock;
+};
+
+#if defined(DEBUG) || defined(CONFIG_DEBUG_FS)
+static int tegra_edid_show(struct seq_file *s, void *unused)
+{
+	struct tegra_edid *edid = s->private;
+	struct tegra_dc_edid *data;
+	u8 *buf;
+	int i;
+
+	data = tegra_edid_get_data(edid);
+	if (!data) {
+		seq_printf(s, "No EDID\n");
+		return 0;
+	}
+
+	buf = data->buf;
+
+	for (i = 0; i < data->len; i++) {
+		if (i % 16 == 0)
+			seq_printf(s, "edid[%03x] =", i);
+
+		seq_printf(s, " %02x", buf[i]);
+
+		if (i % 16 == 15)
+			seq_printf(s, "\n");
+	}
+
+	tegra_edid_put_data(data);
+
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_DEBUG_FS
+static int tegra_edid_debug_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, tegra_edid_show, inode->i_private);
+}
+
+static const struct file_operations tegra_edid_debug_fops = {
+	.open		= tegra_edid_debug_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+void tegra_edid_debug_add(struct tegra_edid *edid)
+{
+	char name[] = "edidX";
+
+	snprintf(name, sizeof(name), "edid%1d", edid->bus);
+	debugfs_create_file(name, S_IRUGO, NULL, edid, &tegra_edid_debug_fops);
+}
+#else
+void tegra_edid_debug_add(struct tegra_edid *edid)
+{
+}
+#endif
+
+#ifdef DEBUG
+static char tegra_edid_dump_buff[16 * 1024];
+
+static void tegra_edid_dump(struct tegra_edid *edid)
+{
+	struct seq_file s;
+	int i;
+	char c;
+
+	memset(&s, 0x0, sizeof(s));
+
+	s.buf = tegra_edid_dump_buff;
+	s.size = sizeof(tegra_edid_dump_buff);
+	s.private = edid;
+
+	tegra_edid_show(&s, NULL);
+
+	i = 0;
+	while (i < s.count ) {
+		if ((s.count - i) > 256) {
+			c = s.buf[i + 256];
+			s.buf[i + 256] = 0;
+			printk("%s", s.buf + i);
+			s.buf[i + 256] = c;
+		} else {
+			printk("%s", s.buf + i);
+		}
+		i += 256;
+	}
+}
+#else
+static void tegra_edid_dump(struct tegra_edid *edid)
+{
+}
+#endif
+
+int tegra_edid_read_block(struct tegra_edid *edid, int block, u8 *data)
+{
+	u8 block_buf[] = {block >> 1};
+	u8 cmd_buf[] = {(block & 0x1) * 128};
+	int status;
+	struct i2c_msg msg[] = {
+		{
+			.addr = 0x30,
+			.flags = 0,
+			.len = 1,
+			.buf = block_buf,
+		},
+		{
+			.addr = 0x50,
+			.flags = 0,
+			.len = 1,
+			.buf = cmd_buf,
+		},
+		{
+			.addr = 0x50,
+			.flags = I2C_M_RD,
+			.len = 128,
+			.buf = data,
+		}};
+	struct i2c_msg *m;
+	int msg_len;
+
+	if (block > 1) {
+		msg_len = 3;
+		m = msg;
+	} else {
+		msg_len = 2;
+		m = &msg[1];
+	}
+
+	status = i2c_transfer(edid->client->adapter, m, msg_len);
+
+	if (status < 0)
+		return status;
+
+	if (status != msg_len)
+		return -EIO;
+
+	return 0;
+}
+
+int tegra_edid_parse_ext_block(const u8 *raw, int idx,
+			       struct tegra_edid_pvt *edid)
+{
+	const u8 *ptr;
+	u8 tmp;
+	u8 code;
+	int len;
+	int i;
+	bool basic_audio = false;
+
+	ptr = &raw[0];
+
+	/* If CEA 861 block get info for eld struct */
+	if (edid && ptr) {
+		if (*ptr <= 3)
+			edid->eld.eld_ver = 0x02;
+		edid->eld.cea_edid_ver = ptr[1];
+
+		/* check for basic audio support in CEA 861 block */
+		if(raw[3] & (1<<6)) {
+			/* For basic audio, set spk_alloc to Left+Right.
+			 * If there is a Speaker Alloc block this will
+			 * get over written with that value */
+			basic_audio = true;
+		}
+	}
+
+	if (raw[3] & 0x80)
+		edid->support_underscan = 1;
+	else
+		edid->support_underscan = 0;
+
+	ptr = &raw[4];
+
+	while (ptr < &raw[idx]) {
+		tmp = *ptr;
+		len = tmp & 0x1f;
+
+		/* HDMI Specification v1.4a, section 8.3.2:
+		 * see Table 8-16 for HDMI VSDB format.
+		 * data blocks have tags in top 3 bits:
+		 * tag code 2: video data block
+		 * tag code 3: vendor specific data block
+		 */
+		code = (tmp >> 5) & 0x7;
+		switch (code) {
+		case 1:
+		{
+			edid->eld.sad_count = len;
+			edid->eld.conn_type = 0x00;
+			edid->eld.support_hdcp = 0x00;
+			for (i = 0; (i < len) && (i < ELD_MAX_SAD); i ++)
+				edid->eld.sad[i] = ptr[i + 1];
+			len++;
+			ptr += len; /* adding the header */
+			/* Got an audio data block so enable audio */
+			if(basic_audio == true)
+				edid->eld.spk_alloc = 1;
+			break;
+		}
+		/* case 2 is commented out for now */
+		case 3:
+		{
+			int j = 0;
+
+			if ((ptr[1] == 0x03) &&
+				(ptr[2] == 0x0c) &&
+				(ptr[3] == 0)) {
+				edid->eld.port_id[0] = ptr[4];
+				edid->eld.port_id[1] = ptr[5];
+			}
+			if ((len >= 8) &&
+				(ptr[1] == 0x03) &&
+				(ptr[2] == 0x0c) &&
+				(ptr[3] == 0)) {
+				j = 8;
+				tmp = ptr[j++];
+				/* HDMI_Video_present? */
+				if (tmp & 0x20) {
+					/* Latency_Fields_present? */
+					if (tmp & 0x80)
+						j += 2;
+					/* I_Latency_Fields_present? */
+					if (tmp & 0x40)
+						j += 2;
+					/* 3D_present? */
+					if (j <= len && (ptr[j] & 0x80))
+						edid->support_stereo = 1;
+				}
+			}
+			if ((len > 5) &&
+				(ptr[1] == 0x03) &&
+				(ptr[2] == 0x0c) &&
+				(ptr[3] == 0)) {
+
+				edid->eld.support_ai = (ptr[6] & 0x80);
+			}
+
+			if ((len > 9) &&
+				(ptr[1] == 0x03) &&
+				(ptr[2] == 0x0c) &&
+				(ptr[3] == 0)) {
+
+				edid->eld.aud_synch_delay = ptr[10];
+			}
+			len++;
+			ptr += len; /* adding the header */
+			break;
+		}
+		case 4:
+		{
+			edid->eld.spk_alloc = ptr[1];
+			len++;
+			ptr += len; /* adding the header */
+			break;
+		}
+		default:
+			len++; /* len does not include header */
+			ptr += len;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+int tegra_edid_mode_support_stereo(struct fb_videomode *mode)
+{
+	if (!mode)
+		return 0;
+
+	if (mode->xres == 1280 &&
+		mode->yres == 720 &&
+		((mode->refresh == 60) || (mode->refresh == 50)))
+		return 1;
+
+	if (mode->xres == 1920 && mode->yres == 1080 && mode->refresh == 24)
+		return 1;
+
+	return 0;
+}
+
+static void data_release(struct kref *ref)
+{
+	struct tegra_edid_pvt *data =
+		container_of(ref, struct tegra_edid_pvt, refcnt);
+	vfree(data);
+}
+
+int tegra_edid_get_monspecs_test(struct tegra_edid *edid,
+			struct fb_monspecs *specs, unsigned char *edid_ptr)
+{
+	int i, j, ret;
+	int extension_blocks;
+	struct tegra_edid_pvt *new_data, *old_data;
+	u8 *data;
+
+	new_data = vmalloc(SZ_32K + sizeof(struct tegra_edid_pvt));
+	if (!new_data)
+		return -ENOMEM;
+
+	kref_init(&new_data->refcnt);
+
+	new_data->support_stereo = 0;
+	new_data->support_underscan = 0;
+
+	data = new_data->dc_edid.buf;
+	memcpy(data, edid_ptr, 128);
+
+	memset(specs, 0x0, sizeof(struct fb_monspecs));
+	memset(&new_data->eld, 0x0, sizeof(new_data->eld));
+	fb_edid_to_monspecs(data, specs);
+	if (specs->modedb == NULL) {
+		ret = -EINVAL;
+		goto fail;
+	}
+
+	memcpy(new_data->eld.monitor_name, specs->monitor,
+					sizeof(specs->monitor));
+
+	new_data->eld.mnl = strlen(new_data->eld.monitor_name) + 1;
+	new_data->eld.product_id[0] = data[0x8];
+	new_data->eld.product_id[1] = data[0x9];
+	new_data->eld.manufacture_id[0] = data[0xA];
+	new_data->eld.manufacture_id[1] = data[0xB];
+
+	extension_blocks = data[0x7e];
+	for (i = 1; i <= extension_blocks; i++) {
+		memcpy(data+128, edid_ptr+128, 128);
+
+		if (data[i * 128] == 0x2) {
+			fb_edid_add_monspecs(data + i * 128, specs);
+
+			tegra_edid_parse_ext_block(data + i * 128,
+					data[i * 128 + 2], new_data);
+
+			if (new_data->support_stereo) {
+				for (j = 0; j < specs->modedb_len; j++) {
+					if (tegra_edid_mode_support_stereo(
+						&specs->modedb[j]))
+						specs->modedb[j].vmode |=
+#ifndef CONFIG_TEGRA_HDMI_74MHZ_LIMIT
+						FB_VMODE_STEREO_FRAME_PACK;
+#else
+						FB_VMODE_STEREO_LEFT_RIGHT;
+#endif
+				}
+			}
+		}
+	}
+
+	new_data->dc_edid.len = i * 128;
+
+	mutex_lock(&edid->lock);
+	old_data = edid->data;
+	edid->data = new_data;
+	mutex_unlock(&edid->lock);
+
+	if (old_data)
+		kref_put(&old_data->refcnt, data_release);
+
+	tegra_edid_dump(edid);
+	return 0;
+fail:
+	vfree(new_data);
+	return ret;
+}
+
+int tegra_edid_get_monspecs(struct tegra_edid *edid, struct fb_monspecs *specs)
+{
+	int i;
+	int j;
+	int ret;
+	int extension_blocks;
+	struct tegra_edid_pvt *new_data, *old_data;
+	u8 *data;
+
+	new_data = vmalloc(SZ_32K + sizeof(struct tegra_edid_pvt));
+	if (!new_data)
+		return -ENOMEM;
+
+	kref_init(&new_data->refcnt);
+
+	new_data->support_stereo = 0;
+
+	data = new_data->dc_edid.buf;
+
+	ret = tegra_edid_read_block(edid, 0, data);
+	if (ret)
+		goto fail;
+
+	memset(specs, 0x0, sizeof(struct fb_monspecs));
+	memset(&new_data->eld, 0x0, sizeof(new_data->eld));
+	fb_edid_to_monspecs(data, specs);
+	if (specs->modedb == NULL) {
+		ret = -EINVAL;
+		goto fail;
+	}
+	memcpy(new_data->eld.monitor_name, specs->monitor, sizeof(specs->monitor));
+	new_data->eld.mnl = strlen(new_data->eld.monitor_name) + 1;
+	new_data->eld.product_id[0] = data[0x8];
+	new_data->eld.product_id[1] = data[0x9];
+	new_data->eld.manufacture_id[0] = data[0xA];
+	new_data->eld.manufacture_id[1] = data[0xB];
+
+	extension_blocks = data[0x7e];
+
+	for (i = 1; i <= extension_blocks; i++) {
+		ret = tegra_edid_read_block(edid, i, data + i * 128);
+		if (ret < 0)
+			break;
+
+		if (data[i * 128] == 0x2) {
+			fb_edid_add_monspecs(data + i * 128, specs);
+
+			tegra_edid_parse_ext_block(data + i * 128,
+					data[i * 128 + 2], new_data);
+
+			if (new_data->support_stereo) {
+				for (j = 0; j < specs->modedb_len; j++) {
+					if (tegra_edid_mode_support_stereo(
+						&specs->modedb[j]))
+						specs->modedb[j].vmode |=
+#ifndef CONFIG_TEGRA_HDMI_74MHZ_LIMIT
+						FB_VMODE_STEREO_FRAME_PACK;
+#else
+						FB_VMODE_STEREO_LEFT_RIGHT;
+#endif
+				}
+			}
+		}
+	}
+
+	new_data->dc_edid.len = i * 128;
+
+	mutex_lock(&edid->lock);
+	old_data = edid->data;
+	edid->data = new_data;
+	mutex_unlock(&edid->lock);
+
+	if (old_data)
+		kref_put(&old_data->refcnt, data_release);
+
+	tegra_edid_dump(edid);
+	return 0;
+
+fail:
+	vfree(new_data);
+	return ret;
+}
+
+int tegra_edid_underscan_supported(struct tegra_edid *edid)
+{
+	if ((!edid) || (!edid->data))
+		return 0;
+
+	return edid->data->support_underscan;
+}
+
+int tegra_edid_get_eld(struct tegra_edid *edid, struct tegra_edid_hdmi_eld *elddata)
+{
+	if (!elddata || !edid->data)
+		return -EFAULT;
+
+	memcpy(elddata,&edid->data->eld,sizeof(struct tegra_edid_hdmi_eld));
+
+	return 0;
+}
+
+struct tegra_edid *tegra_edid_create(int bus)
+{
+	struct tegra_edid *edid;
+	struct i2c_adapter *adapter;
+	int err;
+
+	edid = kzalloc(sizeof(struct tegra_edid), GFP_KERNEL);
+	if (!edid)
+		return ERR_PTR(-ENOMEM);
+
+	mutex_init(&edid->lock);
+	strlcpy(edid->info.type, "tegra_edid", sizeof(edid->info.type));
+	edid->bus = bus;
+	edid->info.addr = 0x50;
+	edid->info.platform_data = edid;
+
+	adapter = i2c_get_adapter(bus);
+	if (!adapter) {
+		pr_err("can't get adpater for bus %d\n", bus);
+		err = -EBUSY;
+		goto free_edid;
+	}
+
+	edid->client = i2c_new_device(adapter, &edid->info);
+	i2c_put_adapter(adapter);
+
+	if (!edid->client) {
+		pr_err("can't create new device\n");
+		err = -EBUSY;
+		goto free_edid;
+	}
+
+	tegra_edid_debug_add(edid);
+
+	return edid;
+
+free_edid:
+	kfree(edid);
+
+	return ERR_PTR(err);
+}
+
+void tegra_edid_destroy(struct tegra_edid *edid)
+{
+	i2c_release_client(edid->client);
+	if (edid->data)
+		kref_put(&edid->data->refcnt, data_release);
+	kfree(edid);
+}
+
+struct tegra_dc_edid *tegra_edid_get_data(struct tegra_edid *edid)
+{
+	struct tegra_edid_pvt *data;
+
+	mutex_lock(&edid->lock);
+	data = edid->data;
+	if (data)
+		kref_get(&data->refcnt);
+	mutex_unlock(&edid->lock);
+
+	return data ? &data->dc_edid : NULL;
+}
+
+void tegra_edid_put_data(struct tegra_dc_edid *data)
+{
+	struct tegra_edid_pvt *pvt;
+
+	if (!data)
+		return;
+
+	pvt = container_of(data, struct tegra_edid_pvt, dc_edid);
+
+	kref_put(&pvt->refcnt, data_release);
+}
+
+static const struct i2c_device_id tegra_edid_id[] = {
+        { "tegra_edid", 0 },
+        { }
+};
+
+MODULE_DEVICE_TABLE(i2c, tegra_edid_id);
+
+static struct i2c_driver tegra_edid_driver = {
+        .id_table = tegra_edid_id,
+        .driver = {
+                .name = "tegra_edid",
+        },
+};
+
+static int __init tegra_edid_init(void)
+{
+        return i2c_add_driver(&tegra_edid_driver);
+}
+
+static void __exit tegra_edid_exit(void)
+{
+        i2c_del_driver(&tegra_edid_driver);
+}
+
+module_init(tegra_edid_init);
+module_exit(tegra_edid_exit);
diff --git a/drivers/video/tegra/dc/edid.h b/drivers/video/tegra/dc/edid.h
new file mode 100644
index 000000000000..77db36f4adbf
--- /dev/null
+++ b/drivers/video/tegra/dc/edid.h
@@ -0,0 +1,62 @@
+/*
+ * drivers/video/tegra/dc/edid.h
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 __DRIVERS_VIDEO_TEGRA_DC_EDID_H
+#define __DRIVERS_VIDEO_TEGRA_DC_EDID_H
+
+#include <linux/i2c.h>
+#include <linux/wait.h>
+#include <mach/dc.h>
+
+#define ELD_MAX_MNL	16
+#define ELD_MAX_SAD	16
+struct tegra_edid;
+
+/*
+ * ELD: EDID Like Data
+ */
+struct tegra_edid_hdmi_eld {
+	u8	baseline_len;
+	u8	eld_ver;
+	u8	cea_edid_ver;
+	char	monitor_name[ELD_MAX_MNL + 1];
+	u8	mnl;
+	u8	manufacture_id[2];
+	u8	product_id[2];
+	u8	port_id[8];
+	u8	support_hdcp;
+	u8	support_ai;
+	u8	conn_type;
+	u8	aud_synch_delay;
+	u8	spk_alloc;
+	u8	sad_count;
+	u8	sad[ELD_MAX_SAD];
+};
+
+struct tegra_edid *tegra_edid_create(int bus);
+void tegra_edid_destroy(struct tegra_edid *edid);
+
+int tegra_edid_get_monspecs_test(struct tegra_edid *edid,
+				struct fb_monspecs *specs, u8 *edid_ptr);
+int tegra_edid_get_monspecs(struct tegra_edid *edid, struct fb_monspecs *specs);
+int tegra_edid_get_eld(struct tegra_edid *edid, struct tegra_edid_hdmi_eld *elddata);
+
+struct tegra_dc_edid *tegra_edid_get_data(struct tegra_edid *edid);
+void tegra_edid_put_data(struct tegra_dc_edid *data);
+
+int tegra_edid_underscan_supported(struct tegra_edid *edid);
+#endif
diff --git a/drivers/video/tegra/dc/ext/Makefile b/drivers/video/tegra/dc/ext/Makefile
new file mode 100644
index 000000000000..16e4cdf43ebb
--- /dev/null
+++ b/drivers/video/tegra/dc/ext/Makefile
@@ -0,0 +1,7 @@
+GCOV_PROFILE := y
+EXTRA_CFLAGS += -Idrivers/video/tegra/host
+obj-y += dev.o
+obj-y += util.o
+obj-y += cursor.o
+obj-y += events.o
+obj-y += control.o
diff --git a/drivers/video/tegra/dc/ext/control.c b/drivers/video/tegra/dc/ext/control.c
new file mode 100644
index 000000000000..ed812be2eab5
--- /dev/null
+++ b/drivers/video/tegra/dc/ext/control.c
@@ -0,0 +1,276 @@
+/*
+ * drivers/video/tegra/dc/ext/control.c
+ *
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * Author: Robert Morell <rmorell@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * 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.
+ */
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/file.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/uaccess.h>
+
+#include "tegra_dc_ext_priv.h"
+
+static struct tegra_dc_ext_control g_control;
+
+int tegra_dc_ext_process_hotplug(int output)
+{
+	return tegra_dc_ext_queue_hotplug(&g_control, output);
+}
+
+static int
+get_output_properties(struct tegra_dc_ext_control_output_properties *properties)
+{
+	struct tegra_dc *dc;
+
+	/* TODO: this should be more dynamic */
+	if (properties->handle > 2)
+		return -EINVAL;
+
+	switch (properties->handle) {
+	case 0:
+		properties->type = TEGRA_DC_EXT_LVDS;
+		break;
+	case 1:
+		properties->type = TEGRA_DC_EXT_HDMI;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	properties->associated_head = properties->handle;
+	properties->head_mask = (1 << properties->associated_head);
+
+	dc = tegra_dc_get_dc(properties->associated_head);
+	properties->connected = tegra_dc_get_connected(dc);
+
+	return 0;
+}
+
+static int get_output_edid(struct tegra_dc_ext_control_output_edid *edid)
+{
+	struct tegra_dc *dc;
+	size_t user_size = edid->size;
+	struct tegra_dc_edid *dc_edid = NULL;
+	int ret = 0;
+
+	/* TODO: this should be more dynamic */
+	if (edid->handle > 2)
+		return -EINVAL;
+
+	dc = tegra_dc_get_dc(edid->handle);
+
+	dc_edid = tegra_dc_get_edid(dc);
+	if (IS_ERR(dc_edid))
+		return PTR_ERR(dc_edid);
+
+	if (!dc_edid) {
+		edid->size = 0;
+	} else {
+		edid->size = dc_edid->len;
+
+		if (user_size < edid->size) {
+			ret = -EFBIG;
+			goto done;
+		}
+
+		if (copy_to_user(edid->data, dc_edid->buf, edid->size)) {
+			ret = -EFAULT;
+			goto done;
+		}
+
+	}
+
+done:
+	if (dc_edid)
+		tegra_dc_put_edid(dc_edid);
+
+	return ret;
+}
+
+static int set_event_mask(struct tegra_dc_ext_control_user *user, u32 mask)
+{
+	struct list_head *list, *tmp;
+
+	if (mask & ~TEGRA_DC_EXT_EVENT_MASK_ALL)
+		return -EINVAL;
+
+	mutex_lock(&user->lock);
+
+	user->event_mask = mask;
+
+	list_for_each_safe(list, tmp, &user->event_list) {
+		struct tegra_dc_ext_event_list *ev_list;
+		ev_list = list_entry(list, struct tegra_dc_ext_event_list,
+			list);
+		if (!(mask & ev_list->event.type)) {
+			list_del(list);
+			kfree(ev_list);
+		}
+	}
+	mutex_unlock(&user->lock);
+
+	return 0;
+}
+
+static int get_capabilities(struct tegra_dc_ext_control_capabilities *caps)
+{
+	caps->caps = TEGRA_DC_EXT_CAPABILITIES;
+	return 0;
+}
+
+static long tegra_dc_ext_control_ioctl(struct file *filp, unsigned int cmd,
+				       unsigned long arg)
+{
+	void __user *user_arg = (void __user *)arg;
+	struct tegra_dc_ext_control_user *user = filp->private_data;
+
+	switch (cmd) {
+	case TEGRA_DC_EXT_CONTROL_GET_NUM_OUTPUTS:
+	{
+		u32 num = tegra_dc_ext_get_num_outputs();
+
+		if (copy_to_user(user_arg, &num, sizeof(num)))
+			return -EFAULT;
+
+		return 0;
+	}
+	case TEGRA_DC_EXT_CONTROL_GET_OUTPUT_PROPERTIES:
+	{
+		struct tegra_dc_ext_control_output_properties args;
+		int ret;
+
+		if (copy_from_user(&args, user_arg, sizeof(args)))
+			return -EFAULT;
+
+		ret = get_output_properties(&args);
+
+		if (copy_to_user(user_arg, &args, sizeof(args)))
+			return -EFAULT;
+
+		return ret;
+	}
+	case TEGRA_DC_EXT_CONTROL_GET_OUTPUT_EDID:
+	{
+		struct tegra_dc_ext_control_output_edid args;
+		int ret;
+
+		if (copy_from_user(&args, user_arg, sizeof(args)))
+			return -EFAULT;
+
+		ret = get_output_edid(&args);
+
+		if (copy_to_user(user_arg, &args, sizeof(args)))
+			return -EFAULT;
+
+		return ret;
+	}
+	case TEGRA_DC_EXT_CONTROL_SET_EVENT_MASK:
+		return set_event_mask(user, (u32) arg);
+	case TEGRA_DC_EXT_CONTROL_GET_CAPABILITIES:
+	{
+		struct tegra_dc_ext_control_capabilities args;
+		int ret;
+
+		ret = get_capabilities(&args);
+
+		if (copy_to_user(user_arg, &args, sizeof(args)))
+			return -EFAULT;
+
+		return ret;
+	}
+	default:
+		return -EINVAL;
+	}
+}
+
+static int tegra_dc_ext_control_open(struct inode *inode, struct file *filp)
+{
+	struct tegra_dc_ext_control_user *user;
+	struct tegra_dc_ext_control *control;
+
+	user = kzalloc(sizeof(*user), GFP_KERNEL);
+	if (!user)
+		return -ENOMEM;
+
+	control = container_of(inode->i_cdev, struct tegra_dc_ext_control,
+		cdev);
+	user->control = control;;
+
+	INIT_LIST_HEAD(&user->event_list);
+	mutex_init(&user->lock);
+
+	filp->private_data = user;
+
+	mutex_lock(&control->lock);
+	list_add(&user->list, &control->users);
+	mutex_unlock(&control->lock);
+
+	return 0;
+}
+
+static int tegra_dc_ext_control_release(struct inode *inode, struct file *filp)
+{
+	struct tegra_dc_ext_control_user *user = filp->private_data;
+	struct tegra_dc_ext_control *control = user->control;
+
+	/* This will free any pending events for this user */
+	set_event_mask(user, 0);
+
+	mutex_lock(&control->lock);
+	list_del(&user->list);
+	mutex_unlock(&control->lock);
+
+	kfree(user);
+
+	return 0;
+}
+
+static const struct file_operations tegra_dc_ext_event_devops = {
+	.owner =		THIS_MODULE,
+	.open =			tegra_dc_ext_control_open,
+	.release =		tegra_dc_ext_control_release,
+	.read =			tegra_dc_ext_event_read,
+	.poll =			tegra_dc_ext_event_poll,
+	.unlocked_ioctl =	tegra_dc_ext_control_ioctl,
+};
+
+int tegra_dc_ext_control_init(void)
+{
+	struct tegra_dc_ext_control *control = &g_control;
+	int ret;
+
+	cdev_init(&control->cdev, &tegra_dc_ext_event_devops);
+	control->cdev.owner = THIS_MODULE;
+	ret = cdev_add(&control->cdev, tegra_dc_ext_devno, 1);
+	if (ret)
+		return ret;
+
+	control->dev = device_create(tegra_dc_ext_class,
+	     NULL, tegra_dc_ext_devno, NULL, "tegra_dc_ctrl");
+	if (IS_ERR(control->dev)) {
+		ret = PTR_ERR(control->dev);
+		cdev_del(&control->cdev);
+	}
+
+	mutex_init(&control->lock);
+
+	INIT_LIST_HEAD(&control->users);
+
+	return ret;
+}
diff --git a/drivers/video/tegra/dc/ext/cursor.c b/drivers/video/tegra/dc/ext/cursor.c
new file mode 100644
index 000000000000..970f38f5ac09
--- /dev/null
+++ b/drivers/video/tegra/dc/ext/cursor.c
@@ -0,0 +1,203 @@
+/*
+ * drivers/video/tegra/dc/ext/cursor.c
+ *
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * Author: Robert Morell <rmorell@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * 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.
+ */
+
+#include <video/tegra_dc_ext.h>
+
+#include "tegra_dc_ext_priv.h"
+
+/* ugh */
+#include "../dc_priv.h"
+#include "../dc_reg.h"
+
+int tegra_dc_ext_get_cursor(struct tegra_dc_ext_user *user)
+{
+	struct tegra_dc_ext *ext = user->ext;
+	int ret = 0;
+
+	mutex_lock(&ext->cursor.lock);
+
+	if (!ext->cursor.user)
+		ext->cursor.user = user;
+	else if (ext->cursor.user != user)
+		ret = -EBUSY;
+
+	mutex_unlock(&ext->cursor.lock);
+
+	return ret;
+}
+
+int tegra_dc_ext_put_cursor(struct tegra_dc_ext_user *user)
+{
+	struct tegra_dc_ext *ext = user->ext;
+	int ret = 0;
+
+	mutex_lock(&ext->cursor.lock);
+
+	if (ext->cursor.user == user)
+		ext->cursor.user = 0;
+	else
+		ret = -EACCES;
+
+	mutex_unlock(&ext->cursor.lock);
+
+	return ret;
+}
+
+static void set_cursor_image_hw(struct tegra_dc *dc,
+				struct tegra_dc_ext_cursor_image *args,
+				dma_addr_t phys_addr)
+{
+	tegra_dc_writel(dc,
+		CURSOR_COLOR(args->foreground.r,
+			     args->foreground.g,
+			     args->foreground.b),
+		DC_DISP_CURSOR_FOREGROUND);
+	tegra_dc_writel(dc,
+		CURSOR_COLOR(args->background.r,
+			     args->background.g,
+			     args->background.b),
+		DC_DISP_CURSOR_BACKGROUND);
+
+	BUG_ON(phys_addr & ~CURSOR_START_ADDR_MASK);
+
+	tegra_dc_writel(dc,
+		CURSOR_START_ADDR(((unsigned long) phys_addr)) |
+		((args->flags & TEGRA_DC_EXT_CURSOR_IMAGE_FLAGS_SIZE_64x64) ?
+			CURSOR_SIZE_64 : 0),
+		DC_DISP_CURSOR_START_ADDR);
+}
+
+int tegra_dc_ext_set_cursor_image(struct tegra_dc_ext_user *user,
+				  struct tegra_dc_ext_cursor_image *args)
+{
+	struct tegra_dc_ext *ext = user->ext;
+	struct tegra_dc *dc = ext->dc;
+	struct nvmap_handle_ref *handle, *old_handle;
+	dma_addr_t phys_addr;
+	u32 size;
+	int ret;
+
+	if (!user->nvmap)
+		return -EFAULT;
+
+	size = args->flags & (TEGRA_DC_EXT_CURSOR_IMAGE_FLAGS_SIZE_32x32 |
+			      TEGRA_DC_EXT_CURSOR_IMAGE_FLAGS_SIZE_64x64);
+
+	if (size != TEGRA_DC_EXT_CURSOR_IMAGE_FLAGS_SIZE_32x32 &&
+	    size !=  TEGRA_DC_EXT_CURSOR_IMAGE_FLAGS_SIZE_64x64)
+		return -EINVAL;
+
+	mutex_lock(&ext->cursor.lock);
+
+	if (ext->cursor.user != user) {
+		ret = -EACCES;
+		goto unlock;
+	}
+
+	if (!ext->enabled) {
+		ret = -ENXIO;
+		goto unlock;
+	}
+
+	old_handle = ext->cursor.cur_handle;
+
+	ret = tegra_dc_ext_pin_window(user, args->buff_id, &handle, &phys_addr);
+	if (ret)
+		goto unlock;
+
+	ext->cursor.cur_handle = handle;
+
+	mutex_lock(&dc->lock);
+
+	set_cursor_image_hw(dc, args, phys_addr);
+
+	tegra_dc_writel(dc, GENERAL_ACT_REQ << 8, DC_CMD_STATE_CONTROL);
+	tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
+
+	/* XXX sync here? */
+
+	mutex_unlock(&dc->lock);
+
+	mutex_unlock(&ext->cursor.lock);
+
+	if (old_handle) {
+		nvmap_unpin(ext->nvmap, old_handle);
+		nvmap_free(ext->nvmap, old_handle);
+	}
+
+	return 0;
+
+unlock:
+	mutex_unlock(&ext->cursor.lock);
+
+	return ret;
+}
+
+int tegra_dc_ext_set_cursor(struct tegra_dc_ext_user *user,
+			    struct tegra_dc_ext_cursor *args)
+{
+	struct tegra_dc_ext *ext = user->ext;
+	struct tegra_dc *dc = ext->dc;
+	u32 win_options;
+	bool enable;
+	int ret;
+
+	mutex_lock(&ext->cursor.lock);
+
+	if (ext->cursor.user != user) {
+		ret = -EACCES;
+		goto unlock;
+	}
+
+	if (!ext->enabled) {
+		ret = -ENXIO;
+		goto unlock;
+	}
+
+	enable = !!(args->flags & TEGRA_DC_EXT_CURSOR_FLAGS_VISIBLE);
+
+	mutex_lock(&dc->lock);
+
+	win_options = tegra_dc_readl(dc, DC_DISP_DISP_WIN_OPTIONS);
+	if (!!(win_options & CURSOR_ENABLE) != enable) {
+		win_options &= ~CURSOR_ENABLE;
+		if (enable)
+			win_options |= CURSOR_ENABLE;
+		tegra_dc_writel(dc, win_options, DC_DISP_DISP_WIN_OPTIONS);
+	}
+
+	tegra_dc_writel(dc, CURSOR_POSITION(args->x, args->y),
+		DC_DISP_CURSOR_POSITION);
+
+	tegra_dc_writel(dc, GENERAL_ACT_REQ << 8, DC_CMD_STATE_CONTROL);
+	tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
+
+	/* TODO: need to sync here?  hopefully can avoid this, but need to
+	 * figure out interaction w/ rest of GENERAL_ACT_REQ */
+
+	mutex_unlock(&dc->lock);
+
+	mutex_unlock(&ext->cursor.lock);
+
+	return 0;
+
+unlock:
+	mutex_unlock(&ext->cursor.lock);
+
+	return ret;
+}
diff --git a/drivers/video/tegra/dc/ext/dev.c b/drivers/video/tegra/dc/ext/dev.c
new file mode 100644
index 000000000000..37a6d9bd3f80
--- /dev/null
+++ b/drivers/video/tegra/dc/ext/dev.c
@@ -0,0 +1,1167 @@
+/*
+ * drivers/video/tegra/dc/dev.c
+ *
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * Author: Robert Morell <rmorell@nvidia.com>
+ * Some code based on fbdev extensions written by:
+ *	Erik Gilling <konkers@android.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * 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.
+ */
+
+#include <linux/file.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+
+#include <video/tegra_dc_ext.h>
+
+#include <mach/dc.h>
+#include <linux/nvmap.h>
+#include <mach/tegra_dc_ext.h>
+
+/* XXX ew */
+#include "../dc_priv.h"
+#include "../dc_config.h"
+/* XXX ew 2 */
+#include "../../host/dev.h"
+/* XXX ew 3 */
+#include "../../nvmap/nvmap.h"
+#include "tegra_dc_ext_priv.h"
+
+int tegra_dc_ext_devno;
+struct class *tegra_dc_ext_class;
+static int head_count;
+
+struct tegra_dc_ext_flip_win {
+	struct tegra_dc_ext_flip_windowattr	attr;
+	struct nvmap_handle_ref			*handle[TEGRA_DC_NUM_PLANES];
+	dma_addr_t				phys_addr;
+	dma_addr_t				phys_addr_u;
+	dma_addr_t				phys_addr_v;
+	u32					syncpt_max;
+};
+
+struct tegra_dc_ext_flip_data {
+	struct tegra_dc_ext		*ext;
+	struct work_struct		work;
+	struct tegra_dc_ext_flip_win	win[DC_N_WINDOWS];
+#ifndef CONFIG_ANDROID
+	struct list_head		timestamp_node;
+#endif /* !CONFIG_ANDROID */
+};
+
+int tegra_dc_ext_get_num_outputs(void)
+{
+	/* TODO: decouple output count from head count */
+	return head_count;
+}
+
+static int tegra_dc_ext_set_nvmap_fd(struct tegra_dc_ext_user *user,
+				     int fd)
+{
+	struct nvmap_client *nvmap = NULL;
+
+	if (fd >= 0) {
+		nvmap = nvmap_client_get_file(fd);
+		if (IS_ERR(nvmap))
+			return PTR_ERR(nvmap);
+	}
+
+	if (user->nvmap)
+		nvmap_client_put(user->nvmap);
+
+	user->nvmap = nvmap;
+
+	return 0;
+}
+
+static int tegra_dc_ext_get_window(struct tegra_dc_ext_user *user,
+				   unsigned int n)
+{
+	struct tegra_dc_ext *ext = user->ext;
+	struct tegra_dc_ext_win *win;
+	int ret = 0;
+
+	if (n >= DC_N_WINDOWS)
+		return -EINVAL;
+
+	win = &ext->win[n];
+
+	mutex_lock(&win->lock);
+
+	if (!win->user)
+		win->user = user;
+	else if (win->user != user)
+		ret = -EBUSY;
+
+	mutex_unlock(&win->lock);
+
+	return ret;
+}
+
+static int tegra_dc_ext_put_window(struct tegra_dc_ext_user *user,
+				   unsigned int n)
+{
+	struct tegra_dc_ext *ext = user->ext;
+	struct tegra_dc_ext_win *win;
+	int ret = 0;
+
+	if (n >= DC_N_WINDOWS)
+		return -EINVAL;
+
+	win = &ext->win[n];
+
+	mutex_lock(&win->lock);
+
+	if (win->user == user) {
+		flush_workqueue(win->flip_wq);
+		win->user = 0;
+	} else {
+		ret = -EACCES;
+	}
+
+	mutex_unlock(&win->lock);
+
+	return ret;
+}
+
+static void set_enable(struct tegra_dc_ext *ext, bool en)
+{
+	int i;
+
+	/*
+	 * Take all locks to make sure any flip requests or cursor moves are
+	 * out of their critical sections
+	 */
+	for (i = 0; i < ext->dc->n_windows; i++)
+		mutex_lock(&ext->win[i].lock);
+	mutex_lock(&ext->cursor.lock);
+
+	ext->enabled = en;
+
+	mutex_unlock(&ext->cursor.lock);
+	for (i = ext->dc->n_windows - 1; i >= 0 ; i--)
+		mutex_unlock(&ext->win[i].lock);
+}
+
+void tegra_dc_ext_enable(struct tegra_dc_ext *ext)
+{
+	set_enable(ext, true);
+}
+
+void tegra_dc_ext_disable(struct tegra_dc_ext *ext)
+{
+	int i;
+	set_enable(ext, false);
+
+	/*
+	 * Flush the flip queue -- note that this must be called with dc->lock
+	 * unlocked or else it will hang.
+	 */
+	for (i = 0; i < ext->dc->n_windows; i++) {
+		struct tegra_dc_ext_win *win = &ext->win[i];
+
+		flush_workqueue(win->flip_wq);
+	}
+}
+
+int tegra_dc_ext_check_windowattr(struct tegra_dc_ext *ext,
+						struct tegra_dc_win *win)
+{
+	long *addr;
+	struct tegra_dc *dc = ext->dc;
+
+	/* Check the window format */
+	addr = tegra_dc_parse_feature(dc, win->idx, GET_WIN_FORMATS);
+	if (!test_bit(win->fmt, addr)) {
+		dev_err(&dc->ndev->dev, "Color format of window %d is"
+						" invalid.\n", win->idx);
+		goto fail;
+	}
+
+	/* Check window size */
+	addr = tegra_dc_parse_feature(dc, win->idx, GET_WIN_SIZE);
+	if (CHECK_SIZE(win->out_w, addr[MIN_WIDTH], addr[MAX_WIDTH]) ||
+		CHECK_SIZE(win->out_h, addr[MIN_HEIGHT], addr[MAX_HEIGHT])) {
+		dev_err(&dc->ndev->dev, "Size of window %d is"
+						" invalid.\n", win->idx);
+		goto fail;
+	}
+
+	return 0;
+fail:
+	return -EINVAL;
+}
+
+static int tegra_dc_ext_set_windowattr(struct tegra_dc_ext *ext,
+			       struct tegra_dc_win *win,
+			       const struct tegra_dc_ext_flip_win *flip_win)
+{
+	int err = 0;
+	struct tegra_dc_ext_win *ext_win = &ext->win[win->idx];
+#ifndef CONFIG_ANDROID
+	s64 timestamp_ns;
+#endif /* !CONFIG_ANDROID */
+
+	if (flip_win->handle[TEGRA_DC_Y] == NULL) {
+		win->flags = 0;
+		memset(ext_win->cur_handle, 0, sizeof(ext_win->cur_handle));
+		return 0;
+	}
+
+	win->flags = TEGRA_WIN_FLAG_ENABLED;
+	if (flip_win->attr.blend == TEGRA_DC_EXT_BLEND_PREMULT)
+		win->flags |= TEGRA_WIN_FLAG_BLEND_PREMULT;
+	else if (flip_win->attr.blend == TEGRA_DC_EXT_BLEND_COVERAGE)
+		win->flags |= TEGRA_WIN_FLAG_BLEND_COVERAGE;
+	if (flip_win->attr.flags & TEGRA_DC_EXT_FLIP_FLAG_TILED)
+		win->flags |= TEGRA_WIN_FLAG_TILED;
+	if (flip_win->attr.flags & TEGRA_DC_EXT_FLIP_FLAG_INVERT_H)
+		win->flags |= TEGRA_WIN_FLAG_INVERT_H;
+	if (flip_win->attr.flags & TEGRA_DC_EXT_FLIP_FLAG_INVERT_V)
+		win->flags |= TEGRA_WIN_FLAG_INVERT_V;
+	if (flip_win->attr.flags & TEGRA_DC_EXT_FLIP_FLAG_GLOBAL_ALPHA)
+		win->global_alpha = flip_win->attr.global_alpha;
+	else
+		win->global_alpha = 255;
+	win->fmt = flip_win->attr.pixformat;
+	win->x.full = flip_win->attr.x;
+	win->y.full = flip_win->attr.y;
+	win->w.full = flip_win->attr.w;
+	win->h.full = flip_win->attr.h;
+	/* XXX verify that this doesn't go outside display's active region */
+	win->out_x = flip_win->attr.out_x;
+	win->out_y = flip_win->attr.out_y;
+	win->out_w = flip_win->attr.out_w;
+	win->out_h = flip_win->attr.out_h;
+	win->z = flip_win->attr.z;
+	memcpy(ext_win->cur_handle, flip_win->handle,
+	       sizeof(ext_win->cur_handle));
+
+	/* XXX verify that this won't read outside of the surface */
+	win->phys_addr = flip_win->phys_addr + flip_win->attr.offset;
+
+	win->phys_addr_u = flip_win->handle[TEGRA_DC_U] ?
+		flip_win->phys_addr_u : flip_win->phys_addr;
+	win->phys_addr_u += flip_win->attr.offset_u;
+
+	win->phys_addr_v = flip_win->handle[TEGRA_DC_V] ?
+		flip_win->phys_addr_v : flip_win->phys_addr;
+	win->phys_addr_v += flip_win->attr.offset_v;
+
+	win->stride = flip_win->attr.stride;
+	win->stride_uv = flip_win->attr.stride_uv;
+
+	err = tegra_dc_ext_check_windowattr(ext, win);
+	if (err < 0)
+		dev_err(&ext->dc->ndev->dev,
+				"Window atrributes are invalid.\n");
+
+	if ((s32)flip_win->attr.pre_syncpt_id >= 0) {
+		nvhost_syncpt_wait_timeout(
+				&nvhost_get_host(ext->dc->ndev)->syncpt,
+				flip_win->attr.pre_syncpt_id,
+				flip_win->attr.pre_syncpt_val,
+				msecs_to_jiffies(500), NULL);
+	}
+
+#ifndef CONFIG_ANDROID
+#ifndef CONFIG_TEGRA_SIMULATION_PLATFORM
+	timestamp_ns = timespec_to_ns(&flip_win->attr.timestamp);
+
+	if (timestamp_ns) {
+		/* XXX: Should timestamping be overridden by "no_vsync" flag */
+		tegra_dc_config_frame_end_intr(win->dc, true);
+		trace_printk("%s:Before timestamp wait\n", win->dc->ndev->name);
+		err = wait_event_interruptible(win->dc->timestamp_wq,
+				tegra_dc_is_within_n_vsync(win->dc, timestamp_ns));
+		trace_printk("%s:After timestamp wait\n", win->dc->ndev->name);
+		tegra_dc_config_frame_end_intr(win->dc, false);
+	}
+#endif
+	return err;
+#else /* !CONFIG_ANDROID */
+	return 0;
+#endif /* !CONFIG_ANDROID */
+}
+
+static int (*flip_callback)(void);
+static spinlock_t flip_callback_lock;
+static bool init_tegra_dc_flip_callback_called;
+
+static int __init init_tegra_dc_flip_callback(void)
+{
+	spin_lock_init(&flip_callback_lock);
+	init_tegra_dc_flip_callback_called = true;
+	return 0;
+}
+
+pure_initcall(init_tegra_dc_flip_callback);
+
+int tegra_dc_set_flip_callback(int (*callback)(void))
+{
+	WARN_ON(!init_tegra_dc_flip_callback_called);
+
+	spin_lock(&flip_callback_lock);
+	flip_callback = callback;
+	spin_unlock(&flip_callback_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(tegra_dc_set_flip_callback);
+
+int tegra_dc_unset_flip_callback()
+{
+	spin_lock(&flip_callback_lock);
+	flip_callback = NULL;
+	spin_unlock(&flip_callback_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(tegra_dc_unset_flip_callback);
+
+static void tegra_dc_ext_flip_worker(struct work_struct *work)
+{
+	struct tegra_dc_ext_flip_data *data =
+		container_of(work, struct tegra_dc_ext_flip_data, work);
+	struct tegra_dc_ext *ext = data->ext;
+	struct tegra_dc_win *wins[DC_N_WINDOWS];
+	struct nvmap_handle_ref *unpin_handles[DC_N_WINDOWS *
+					       TEGRA_DC_NUM_PLANES];
+	struct nvmap_handle_ref *old_handle;
+	int i, nr_unpin = 0, nr_win = 0;
+	bool skip_flip = false;
+
+	for (i = 0; i < DC_N_WINDOWS; i++) {
+		struct tegra_dc_ext_flip_win *flip_win = &data->win[i];
+		int index = flip_win->attr.index;
+		struct tegra_dc_win *win;
+		struct tegra_dc_ext_win *ext_win;
+#ifndef CONFIG_ANDROID
+		int j = 0;
+		struct tegra_dc_ext_flip_data *temp = NULL;
+		s64 head_timestamp = 0;
+#endif /* !CONFIG_ANDROID */
+
+		if (index < 0)
+			continue;
+
+		win = tegra_dc_get_window(ext->dc, index);
+		ext_win = &ext->win[index];
+
+		if (!(atomic_dec_and_test(&ext_win->nr_pending_flips)) &&
+			(flip_win->attr.flags & TEGRA_DC_EXT_FLIP_FLAG_CURSOR))
+			skip_flip = true;
+
+#ifndef CONFIG_ANDROID
+		mutex_lock(&ext_win->queue_lock);
+		list_for_each_entry(temp, &ext_win->timestamp_queue,
+				timestamp_node) {
+			if (j == 0) {
+				if (unlikely(temp != data))
+					dev_err(&win->dc->ndev->dev,
+							"work queue did NOT dequeue head!!!");
+				else
+					head_timestamp =
+						timespec_to_ns(&flip_win->attr.timestamp);
+			} else {
+				s64 timestamp =
+					timespec_to_ns(&temp->win[i].attr.timestamp);
+
+				skip_flip = !tegra_dc_does_vsync_separate(ext->dc,
+						timestamp, head_timestamp);
+				/* Look ahead only one flip */
+				break;
+			}
+			j++;
+		}
+		if (!list_empty(&ext_win->timestamp_queue))
+			list_del(&data->timestamp_node);
+		mutex_unlock(&ext_win->queue_lock);
+#endif /* !CONFIG_ANDROID */
+
+		if (win->flags & TEGRA_WIN_FLAG_ENABLED) {
+			int j;
+			for (j = 0; j < TEGRA_DC_NUM_PLANES; j++) {
+				if (skip_flip)
+					old_handle = flip_win->handle[j];
+				else
+					old_handle = ext_win->cur_handle[j];
+
+				if (!old_handle)
+					continue;
+
+				unpin_handles[nr_unpin++] = old_handle;
+			}
+		}
+
+		if (!skip_flip)
+			tegra_dc_ext_set_windowattr(ext, win, &data->win[i]);
+
+		wins[nr_win++] = win;
+	}
+
+	if (!skip_flip) {
+		tegra_dc_update_windows(wins, nr_win);
+		/* TODO: implement swapinterval here */
+		tegra_dc_sync_windows(wins, nr_win);
+		if (!tegra_dc_has_multiple_dc()) {
+			spin_lock(&flip_callback_lock);
+			if (flip_callback)
+				flip_callback();
+			spin_unlock(&flip_callback_lock);
+		}
+#ifdef CONFIG_ANDROID
+	}
+#endif /* CONFIG_ANDROID */
+
+		for (i = 0; i < DC_N_WINDOWS; i++) {
+			struct tegra_dc_ext_flip_win *flip_win = &data->win[i];
+			int index = flip_win->attr.index;
+
+			if (index < 0)
+				continue;
+
+			tegra_dc_incr_syncpt_min(ext->dc, index,
+					flip_win->syncpt_max);
+		}
+#ifndef CONFIG_ANDROID
+	}
+#endif /* !CONFIG_ANDROID */
+
+	/* unpin and deref previous front buffers */
+	for (i = 0; i < nr_unpin; i++) {
+		nvmap_unpin(ext->nvmap, unpin_handles[i]);
+		nvmap_free(ext->nvmap, unpin_handles[i]);
+	}
+
+	kfree(data);
+}
+
+static int lock_windows_for_flip(struct tegra_dc_ext_user *user,
+				 struct tegra_dc_ext_flip *args)
+{
+	struct tegra_dc_ext *ext = user->ext;
+	u8 idx_mask = 0;
+	int i;
+
+	for (i = 0; i < DC_N_WINDOWS; i++) {
+		int index = args->win[i].index;
+
+		if (index < 0)
+			continue;
+
+		idx_mask |= BIT(index);
+	}
+
+	for (i = 0; i < DC_N_WINDOWS; i++) {
+		struct tegra_dc_ext_win *win;
+
+		if (!(idx_mask & BIT(i)))
+			continue;
+
+		win = &ext->win[i];
+
+		mutex_lock(&win->lock);
+
+		if (win->user != user)
+			goto fail_unlock;
+	}
+
+	return 0;
+
+fail_unlock:
+	do {
+		if (!(idx_mask & BIT(i)))
+			continue;
+
+		mutex_unlock(&ext->win[i].lock);
+	} while (i--);
+
+	return -EACCES;
+}
+
+static void unlock_windows_for_flip(struct tegra_dc_ext_user *user,
+				    struct tegra_dc_ext_flip *args)
+{
+	struct tegra_dc_ext *ext = user->ext;
+	u8 idx_mask = 0;
+	int i;
+
+	for (i = 0; i < DC_N_WINDOWS; i++) {
+		int index = args->win[i].index;
+
+		if (index < 0)
+			continue;
+
+		idx_mask |= BIT(index);
+	}
+
+	for (i = DC_N_WINDOWS - 1; i >= 0; i--) {
+		if (!(idx_mask & BIT(i)))
+			continue;
+
+		mutex_unlock(&ext->win[i].lock);
+	}
+}
+
+static int sanitize_flip_args(struct tegra_dc_ext_user *user,
+			      struct tegra_dc_ext_flip *args)
+{
+	int i, used_windows = 0;
+
+	for (i = 0; i < DC_N_WINDOWS; i++) {
+		int index = args->win[i].index;
+
+		if (index < 0)
+			continue;
+
+		if (index >= DC_N_WINDOWS)
+			return -EINVAL;
+
+		if (used_windows & BIT(index))
+			return -EINVAL;
+
+		used_windows |= BIT(index);
+	}
+
+	if (!used_windows)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int tegra_dc_ext_flip(struct tegra_dc_ext_user *user,
+			     struct tegra_dc_ext_flip *args)
+{
+	struct tegra_dc_ext *ext = user->ext;
+	struct tegra_dc_ext_flip_data *data;
+	int work_index = -1;
+	int i, ret = 0;
+#ifndef CONFIG_ANDROID
+	bool has_timestamp = false;
+#endif /* !CONFIG_ANDROID */
+
+#ifdef CONFIG_ANDROID
+	int index_check[DC_N_WINDOWS] = {0, };
+	int zero_index_id = 0;
+#endif
+
+	if (!user->nvmap)
+		return -EFAULT;
+
+	ret = sanitize_flip_args(user, args);
+	if (ret)
+		return ret;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	INIT_WORK(&data->work, tegra_dc_ext_flip_worker);
+	data->ext = ext;
+
+#ifdef CONFIG_ANDROID
+	for (i = 0; i < DC_N_WINDOWS; i++) {
+		index_check[i] = args->win[i].index;
+		if (index_check[i] == 0)
+			zero_index_id = i;
+	}
+
+	if (index_check[DC_N_WINDOWS - 1] != 0) {
+		struct tegra_dc_ext_flip_windowattr win_temp;
+		win_temp = args->win[DC_N_WINDOWS - 1];
+		args->win[DC_N_WINDOWS - 1] = args->win[zero_index_id];
+		args->win[zero_index_id] = win_temp;
+	}
+#endif
+
+	for (i = 0; i < DC_N_WINDOWS; i++) {
+		struct tegra_dc_ext_flip_win *flip_win = &data->win[i];
+		int index = args->win[i].index;
+
+		memcpy(&flip_win->attr, &args->win[i], sizeof(flip_win->attr));
+#ifndef CONFIG_ANDROID
+		if (timespec_to_ns(&flip_win->attr.timestamp))
+			has_timestamp = true;
+#endif /* !CONFIG_ANDROID */
+
+		if (index < 0)
+			continue;
+
+		ret = tegra_dc_ext_pin_window(user, flip_win->attr.buff_id,
+					      &flip_win->handle[TEGRA_DC_Y],
+					      &flip_win->phys_addr);
+		if (ret)
+			goto fail_pin;
+
+		if (flip_win->attr.buff_id_u) {
+			ret = tegra_dc_ext_pin_window(user,
+					      flip_win->attr.buff_id_u,
+					      &flip_win->handle[TEGRA_DC_U],
+					      &flip_win->phys_addr_u);
+			if (ret)
+				goto fail_pin;
+		} else {
+			flip_win->handle[TEGRA_DC_U] = NULL;
+			flip_win->phys_addr_u = 0;
+		}
+
+		if (flip_win->attr.buff_id_v) {
+			ret = tegra_dc_ext_pin_window(user,
+					      flip_win->attr.buff_id_v,
+					      &flip_win->handle[TEGRA_DC_V],
+					      &flip_win->phys_addr_v);
+			if (ret)
+				goto fail_pin;
+		} else {
+			flip_win->handle[TEGRA_DC_V] = NULL;
+			flip_win->phys_addr_v = 0;
+		}
+	}
+
+	ret = lock_windows_for_flip(user, args);
+	if (ret)
+		goto fail_pin;
+
+	if (!ext->enabled) {
+		ret = -ENXIO;
+		goto unlock;
+	}
+
+	for (i = 0; i < DC_N_WINDOWS; i++) {
+		u32 syncpt_max;
+		int index = args->win[i].index;
+		struct tegra_dc_win *win;
+		struct tegra_dc_ext_win *ext_win;
+
+		if (index < 0)
+			continue;
+
+		win = tegra_dc_get_window(ext->dc, index);
+		ext_win = &ext->win[index];
+
+		syncpt_max = tegra_dc_incr_syncpt_max(ext->dc, index);
+
+		data->win[i].syncpt_max = syncpt_max;
+
+		/*
+		 * Any of these windows' syncpoints should be equivalent for
+		 * the client, so we just send back an arbitrary one of them
+		 */
+		args->post_syncpt_val = syncpt_max;
+		args->post_syncpt_id = tegra_dc_get_syncpt_id(ext->dc, index);
+		work_index = index;
+
+		atomic_inc(&ext->win[work_index].nr_pending_flips);
+	}
+	if (work_index < 0) {
+		ret = -EINVAL;
+		goto unlock;
+	}
+#ifndef CONFIG_ANDROID
+	if (has_timestamp) {
+		mutex_lock(&ext->win[work_index].queue_lock);
+		list_add_tail(&data->timestamp_node, &ext->win[work_index].timestamp_queue);
+		mutex_unlock(&ext->win[work_index].queue_lock);
+	}
+#endif /* !CONFIG_ANDROID */
+	queue_work(ext->win[work_index].flip_wq, &data->work);
+
+	unlock_windows_for_flip(user, args);
+
+	return 0;
+
+unlock:
+	unlock_windows_for_flip(user, args);
+
+fail_pin:
+	for (i = 0; i < DC_N_WINDOWS; i++) {
+		int j;
+		for (j = 0; j < TEGRA_DC_NUM_PLANES; j++) {
+			if (!data->win[i].handle[j])
+				continue;
+
+			nvmap_unpin(ext->nvmap, data->win[i].handle[j]);
+			nvmap_free(ext->nvmap, data->win[i].handle[j]);
+		}
+	}
+	kfree(data);
+
+	return ret;
+}
+
+static int tegra_dc_ext_set_csc(struct tegra_dc_ext_user *user,
+				struct tegra_dc_ext_csc *new_csc)
+{
+	unsigned int index = new_csc->win_index;
+	struct tegra_dc *dc = user->ext->dc;
+	struct tegra_dc_ext_win *ext_win;
+	struct tegra_dc_csc *csc;
+
+	if (index >= DC_N_WINDOWS)
+		return -EINVAL;
+
+	ext_win = &user->ext->win[index];
+	csc = &dc->windows[index].csc;
+
+	mutex_lock(&ext_win->lock);
+
+	if (ext_win->user != user) {
+		mutex_unlock(&ext_win->lock);
+		return -EACCES;
+	}
+
+	csc->yof =   new_csc->yof;
+	csc->kyrgb = new_csc->kyrgb;
+	csc->kur =   new_csc->kur;
+	csc->kvr =   new_csc->kvr;
+	csc->kug =   new_csc->kug;
+	csc->kvg =   new_csc->kvg;
+	csc->kub =   new_csc->kub;
+	csc->kvb =   new_csc->kvb;
+
+	tegra_dc_update_csc(dc, index);
+
+	mutex_unlock(&ext_win->lock);
+
+	return 0;
+}
+
+static int set_lut_channel(u16 *channel_from_user,
+			   u8 *channel_to,
+			   u32 start,
+			   u32 len)
+{
+	int i;
+	u16 lut16bpp[256];
+
+	if (channel_from_user) {
+		if (copy_from_user(lut16bpp, channel_from_user, len<<1))
+			return 1;
+
+		for (i = 0; i < len; i++)
+			channel_to[start+i] = lut16bpp[i]>>8;
+	} else {
+		for (i = 0; i < len; i++)
+			channel_to[start+i] = start+i;
+	}
+
+	return 0;
+}
+
+static int tegra_dc_ext_set_lut(struct tegra_dc_ext_user *user,
+				struct tegra_dc_ext_lut *new_lut)
+{
+	int err;
+	unsigned int index = new_lut->win_index;
+	u32 start = new_lut->start;
+	u32 len = new_lut->len;
+
+	struct tegra_dc *dc = user->ext->dc;
+	struct tegra_dc_ext_win *ext_win;
+	struct tegra_dc_lut *lut;
+
+	if (index >= DC_N_WINDOWS)
+		return -EINVAL;
+
+	if ((start >= 256) || (len > 256) || ((start + len) > 256))
+		return -EINVAL;
+
+	ext_win = &user->ext->win[index];
+	lut = &dc->windows[index].lut;
+
+	mutex_lock(&ext_win->lock);
+
+	if (ext_win->user != user) {
+		mutex_unlock(&ext_win->lock);
+		return -EACCES;
+	}
+
+	err = set_lut_channel(new_lut->r, lut->r, start, len) |
+	      set_lut_channel(new_lut->g, lut->g, start, len) |
+	      set_lut_channel(new_lut->b, lut->b, start, len);
+
+	if (err) {
+		mutex_unlock(&ext_win->lock);
+		return -EFAULT;
+	}
+
+	tegra_dc_update_lut(dc, index,
+			new_lut->flags & TEGRA_DC_EXT_LUT_FLAGS_FBOVERRIDE);
+
+	mutex_unlock(&ext_win->lock);
+
+	return 0;
+}
+
+static u32 tegra_dc_ext_get_vblank_syncpt(struct tegra_dc_ext_user *user)
+{
+	struct tegra_dc *dc = user->ext->dc;
+
+	return dc->vblank_syncpt;
+}
+
+static int tegra_dc_ext_get_status(struct tegra_dc_ext_user *user,
+				   struct tegra_dc_ext_status *status)
+{
+	struct tegra_dc *dc = user->ext->dc;
+
+	memset(status, 0, sizeof(*status));
+
+	if (dc->enabled)
+		status->flags |= TEGRA_DC_EXT_FLAGS_ENABLED;
+
+	return 0;
+}
+
+static int tegra_dc_ext_get_feature(struct tegra_dc_ext_user *user,
+				   struct tegra_dc_ext_feature *feature)
+{
+	struct tegra_dc *dc = user->ext->dc;
+	struct tegra_dc_feature *table = dc->feature;
+
+	if (dc->enabled && feature->entries) {
+		feature->length = table->num_entries;
+		memcpy(feature->entries, table->entries, table->num_entries *
+					sizeof(struct tegra_dc_feature_entry));
+	}
+
+	return 0;
+}
+
+static long tegra_dc_ioctl(struct file *filp, unsigned int cmd,
+			   unsigned long arg)
+{
+	void __user *user_arg = (void __user *)arg;
+	struct tegra_dc_ext_user *user = filp->private_data;
+
+	switch (cmd) {
+	case TEGRA_DC_EXT_SET_NVMAP_FD:
+		return tegra_dc_ext_set_nvmap_fd(user, arg);
+
+	case TEGRA_DC_EXT_GET_WINDOW:
+		return tegra_dc_ext_get_window(user, arg);
+	case TEGRA_DC_EXT_PUT_WINDOW:
+		return tegra_dc_ext_put_window(user, arg);
+
+	case TEGRA_DC_EXT_FLIP:
+	{
+		struct tegra_dc_ext_flip args;
+		int ret;
+
+		if (copy_from_user(&args, user_arg, sizeof(args)))
+			return -EFAULT;
+
+		ret = tegra_dc_ext_flip(user, &args);
+
+		if (copy_to_user(user_arg, &args, sizeof(args)))
+			return -EFAULT;
+
+		return ret;
+	}
+
+	case TEGRA_DC_EXT_GET_CURSOR:
+		return tegra_dc_ext_get_cursor(user);
+	case TEGRA_DC_EXT_PUT_CURSOR:
+		return tegra_dc_ext_put_cursor(user);
+	case TEGRA_DC_EXT_SET_CURSOR_IMAGE:
+	{
+		struct tegra_dc_ext_cursor_image args;
+
+		if (copy_from_user(&args, user_arg, sizeof(args)))
+			return -EFAULT;
+
+		return tegra_dc_ext_set_cursor_image(user, &args);
+	}
+	case TEGRA_DC_EXT_SET_CURSOR:
+	{
+		struct tegra_dc_ext_cursor args;
+
+		if (copy_from_user(&args, user_arg, sizeof(args)))
+			return -EFAULT;
+
+		return tegra_dc_ext_set_cursor(user, &args);
+	}
+
+	case TEGRA_DC_EXT_SET_CSC:
+	{
+		struct tegra_dc_ext_csc args;
+
+		if (copy_from_user(&args, user_arg, sizeof(args)))
+			return -EFAULT;
+
+		return tegra_dc_ext_set_csc(user, &args);
+	}
+
+	case TEGRA_DC_EXT_GET_VBLANK_SYNCPT:
+	{
+		u32 syncpt = tegra_dc_ext_get_vblank_syncpt(user);
+
+		if (copy_to_user(user_arg, &syncpt, sizeof(syncpt)))
+			return -EFAULT;
+
+		return 0;
+	}
+
+	case TEGRA_DC_EXT_GET_STATUS:
+	{
+		struct tegra_dc_ext_status args;
+		int ret;
+
+		ret = tegra_dc_ext_get_status(user, &args);
+
+		if (copy_to_user(user_arg, &args, sizeof(args)))
+			return -EFAULT;
+
+		return ret;
+	}
+
+	case TEGRA_DC_EXT_SET_LUT:
+	{
+		struct tegra_dc_ext_lut args;
+
+		if (copy_from_user(&args, user_arg, sizeof(args)))
+			return -EFAULT;
+
+		return tegra_dc_ext_set_lut(user, &args);
+	}
+
+	case TEGRA_DC_EXT_GET_FEATURES:
+	{
+		struct tegra_dc_ext_feature args;
+		int ret;
+
+		if (copy_from_user(&args, user_arg, sizeof(args)))
+			return -EFAULT;
+
+		ret = tegra_dc_ext_get_feature(user, &args);
+
+		if (copy_to_user(user_arg, &args, sizeof(args)))
+			return -EFAULT;
+
+		return ret;
+	}
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int tegra_dc_open(struct inode *inode, struct file *filp)
+{
+	struct tegra_dc_ext_user *user;
+	struct tegra_dc_ext *ext;
+
+	user = kzalloc(sizeof(*user), GFP_KERNEL);
+	if (!user)
+		return -ENOMEM;
+
+	ext = container_of(inode->i_cdev, struct tegra_dc_ext, cdev);
+	user->ext = ext;
+
+	filp->private_data = user;
+
+	return 0;
+}
+
+static int tegra_dc_release(struct inode *inode, struct file *filp)
+{
+	struct tegra_dc_ext_user *user = filp->private_data;
+	struct tegra_dc_ext *ext = user->ext;
+	unsigned int i;
+
+	for (i = 0; i < DC_N_WINDOWS; i++) {
+		if (ext->win[i].user == user)
+			tegra_dc_ext_put_window(user, i);
+	}
+	if (ext->cursor.user == user)
+		tegra_dc_ext_put_cursor(user);
+
+	if (user->nvmap)
+		nvmap_client_put(user->nvmap);
+
+	kfree(user);
+
+	return 0;
+}
+
+static int tegra_dc_ext_setup_windows(struct tegra_dc_ext *ext)
+{
+	int i, ret;
+
+	for (i = 0; i < ext->dc->n_windows; i++) {
+		struct tegra_dc_ext_win *win = &ext->win[i];
+		char name[32];
+
+		win->ext = ext;
+		win->idx = i;
+
+		snprintf(name, sizeof(name), "tegradc.%d/%c",
+			 ext->dc->ndev->id, 'a' + i);
+		win->flip_wq = create_singlethread_workqueue(name);
+		if (!win->flip_wq) {
+			ret = -ENOMEM;
+			goto cleanup;
+		}
+
+		mutex_init(&win->lock);
+#ifndef CONFIG_ANDROID
+		mutex_init(&win->queue_lock);
+		INIT_LIST_HEAD(&win->timestamp_queue);
+#endif /* !CONFIG_ANDROID */
+	}
+
+	return 0;
+
+cleanup:
+	while (i--) {
+		struct tegra_dc_ext_win *win = &ext->win[i];
+		destroy_workqueue(win->flip_wq);
+	}
+
+	return ret;
+}
+
+static const struct file_operations tegra_dc_devops = {
+	.owner =		THIS_MODULE,
+	.open =			tegra_dc_open,
+	.release =		tegra_dc_release,
+	.unlocked_ioctl =	tegra_dc_ioctl,
+};
+
+struct tegra_dc_ext *tegra_dc_ext_register(struct nvhost_device *ndev,
+					   struct tegra_dc *dc)
+{
+	int ret;
+	struct tegra_dc_ext *ext;
+	int devno;
+
+	ext = kzalloc(sizeof(*ext), GFP_KERNEL);
+	if (!ext)
+		return ERR_PTR(-ENOMEM);
+
+	BUG_ON(!tegra_dc_ext_devno);
+	devno = tegra_dc_ext_devno + head_count + 1;
+
+	cdev_init(&ext->cdev, &tegra_dc_devops);
+	ext->cdev.owner = THIS_MODULE;
+	ret = cdev_add(&ext->cdev, devno, 1);
+	if (ret) {
+		dev_err(&ndev->dev, "Failed to create character device\n");
+		goto cleanup_alloc;
+	}
+
+	ext->dev = device_create(tegra_dc_ext_class,
+				 &ndev->dev,
+				 devno,
+				 NULL,
+				 "tegra_dc_%d",
+				 ndev->id);
+
+	if (IS_ERR(ext->dev)) {
+		ret = PTR_ERR(ext->dev);
+		goto cleanup_cdev;
+	}
+
+	ext->dc = dc;
+
+	ext->nvmap = nvmap_create_client(nvmap_dev, "tegra_dc_ext");
+	if (!ext->nvmap) {
+		ret = -ENOMEM;
+		goto cleanup_device;
+	}
+
+	ret = tegra_dc_ext_setup_windows(ext);
+	if (ret)
+		goto cleanup_nvmap;
+
+	mutex_init(&ext->cursor.lock);
+
+	head_count++;
+
+	return ext;
+
+cleanup_nvmap:
+	nvmap_client_put(ext->nvmap);
+
+cleanup_device:
+	device_del(ext->dev);
+
+cleanup_cdev:
+	cdev_del(&ext->cdev);
+
+cleanup_alloc:
+	kfree(ext);
+
+	return ERR_PTR(ret);
+}
+
+void tegra_dc_ext_unregister(struct tegra_dc_ext *ext)
+{
+	int i;
+
+	for (i = 0; i < ext->dc->n_windows; i++) {
+		struct tegra_dc_ext_win *win = &ext->win[i];
+
+		flush_workqueue(win->flip_wq);
+		destroy_workqueue(win->flip_wq);
+	}
+
+	nvmap_client_put(ext->nvmap);
+	device_del(ext->dev);
+	cdev_del(&ext->cdev);
+
+	kfree(ext);
+
+	head_count--;
+}
+
+int __init tegra_dc_ext_module_init(void)
+{
+	int ret;
+
+	tegra_dc_ext_class = class_create(THIS_MODULE, "tegra_dc_ext");
+	if (!tegra_dc_ext_class) {
+		printk(KERN_ERR "tegra_dc_ext: failed to create class\n");
+		return -ENOMEM;
+	}
+
+	/* Reserve one character device per head, plus the control device */
+	ret = alloc_chrdev_region(&tegra_dc_ext_devno,
+				  0, TEGRA_MAX_DC + 1,
+				  "tegra_dc_ext");
+	if (ret)
+		goto cleanup_class;
+
+	ret = tegra_dc_ext_control_init();
+	if (ret)
+		goto cleanup_region;
+
+	return 0;
+
+cleanup_region:
+	unregister_chrdev_region(tegra_dc_ext_devno, TEGRA_MAX_DC);
+
+cleanup_class:
+	class_destroy(tegra_dc_ext_class);
+
+	return ret;
+}
+
+void __exit tegra_dc_ext_module_exit(void)
+{
+	unregister_chrdev_region(tegra_dc_ext_devno, TEGRA_MAX_DC);
+	class_destroy(tegra_dc_ext_class);
+}
diff --git a/drivers/video/tegra/dc/ext/events.c b/drivers/video/tegra/dc/ext/events.c
new file mode 100644
index 000000000000..577d056e2436
--- /dev/null
+++ b/drivers/video/tegra/dc/ext/events.c
@@ -0,0 +1,197 @@
+/*
+ * drivers/video/tegra/dc/ext/events.c
+ *
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * Author: Robert Morell <rmorell@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * 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.
+ */
+
+#include <linux/err.h>
+#include <linux/fs.h>
+#include <linux/list.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+
+#include "tegra_dc_ext_priv.h"
+
+static DECLARE_WAIT_QUEUE_HEAD(event_wait);
+
+unsigned int tegra_dc_ext_event_poll(struct file *filp, poll_table *wait)
+{
+	struct tegra_dc_ext_control_user *user = filp->private_data;
+	unsigned int mask = 0;
+
+	poll_wait(filp, &event_wait, wait);
+
+	if (atomic_read(&user->num_events))
+		mask |= POLLIN;
+
+	return mask;
+}
+
+static int get_next_event(struct tegra_dc_ext_control_user *user,
+			  struct tegra_dc_ext_event_list *event,
+			  bool block)
+{
+	struct list_head *list = &user->event_list;
+	struct tegra_dc_ext_event_list *next_event;
+	int ret;
+
+	if (block) {
+		ret = wait_event_interruptible(event_wait,
+			atomic_read(&user->num_events));
+
+		if (unlikely(ret)) {
+			if (ret == -ERESTARTSYS)
+				return -EAGAIN;
+			return ret;
+		}
+	} else {
+		if (!atomic_read(&user->num_events))
+			return 0;
+	}
+
+	mutex_lock(&user->lock);
+
+	BUG_ON(list_empty(list));
+	next_event = list_first_entry(list, struct tegra_dc_ext_event_list,
+			list);
+	*event = *next_event;
+	list_del(&next_event->list);
+	kfree(next_event);
+
+	atomic_dec(&user->num_events);
+
+	mutex_unlock(&user->lock);
+
+	return 1;
+}
+
+ssize_t tegra_dc_ext_event_read(struct file *filp, char __user *buf,
+				size_t size, loff_t *ppos)
+{
+	struct tegra_dc_ext_control_user *user = filp->private_data;
+	struct tegra_dc_ext_event_list event_elem;
+	struct tegra_dc_ext_event *event = &event_elem.event;
+	ssize_t retval = 0, to_copy, event_size, pending;
+	loff_t previously_copied = 0;
+	char *to_copy_ptr;
+
+	if (size == 0)
+		return 0;
+
+	if (user->partial_copy) {
+		/*
+		 * We didn't transfer the entire event last time, need to
+		 * finish it up
+		 */
+		event_elem = user->event_to_copy;
+		previously_copied = user->partial_copy;
+	} else {
+		/* Get the next event, if any */
+		pending = get_next_event(user, &event_elem,
+			!(filp->f_flags & O_NONBLOCK));
+		if (pending <= 0)
+			return pending;
+	}
+
+	/* Write the event to the user */
+	event_size = sizeof(*event) + event->data_size;
+	BUG_ON(event_size <= previously_copied);
+	event_size -= previously_copied;
+
+	to_copy_ptr = (char *)event + previously_copied;
+	to_copy = min_t(ssize_t, size, event_size);
+	if (copy_to_user(buf, to_copy_ptr, to_copy)) {
+		retval = -EFAULT;
+		to_copy = 0;
+	}
+
+	/* Note that we currently only deliver one event at a time */
+
+	if (event_size > to_copy) {
+		/*
+		 * We were only able to copy part of this event.  Stash it for
+		 * next time.
+		 */
+		user->event_to_copy = event_elem;
+		user->partial_copy = previously_copied + to_copy;
+	} else {
+		user->partial_copy = 0;
+	}
+
+	return to_copy ? to_copy : retval;
+}
+
+static int tegra_dc_ext_queue_event(struct tegra_dc_ext_control *control,
+				    struct tegra_dc_ext_event *event)
+{
+	struct list_head *cur;
+	int retval = 0;
+
+	mutex_lock(&control->lock);
+	list_for_each(cur, &control->users) {
+		struct tegra_dc_ext_control_user *user;
+		struct tegra_dc_ext_event_list *ev_list;
+
+		user = container_of(cur, struct tegra_dc_ext_control_user,
+			list);
+		mutex_lock(&user->lock);
+
+		if (!(user->event_mask & event->type)) {
+			mutex_unlock(&user->lock);
+			continue;
+		}
+
+		ev_list = kmalloc(sizeof(*ev_list), GFP_KERNEL);
+		if (!ev_list) {
+			retval = -ENOMEM;
+			mutex_unlock(&user->lock);
+			continue;
+		}
+
+		memcpy(&ev_list->event, event,
+			sizeof(*event) + event->data_size);
+
+		list_add_tail(&ev_list->list, &user->event_list);
+
+		atomic_inc(&user->num_events);
+
+		mutex_unlock(&user->lock);
+	}
+	mutex_unlock(&control->lock);
+
+	/* Is it worth it to track waiters with more granularity? */
+	wake_up(&event_wait);
+
+	return retval;
+}
+
+int tegra_dc_ext_queue_hotplug(struct tegra_dc_ext_control *control, int output)
+{
+	struct {
+		struct tegra_dc_ext_event event;
+		struct tegra_dc_ext_control_event_hotplug hotplug;
+	} __packed pack;
+
+	pack.event.type = TEGRA_DC_EXT_EVENT_HOTPLUG;
+	pack.event.data_size = sizeof(pack.hotplug);
+
+	pack.hotplug.handle = output;
+
+	tegra_dc_ext_queue_event(control, &pack.event);
+
+	return 0;
+}
diff --git a/drivers/video/tegra/dc/ext/tegra_dc_ext_priv.h b/drivers/video/tegra/dc/ext/tegra_dc_ext_priv.h
new file mode 100644
index 000000000000..7238e95e59db
--- /dev/null
+++ b/drivers/video/tegra/dc/ext/tegra_dc_ext_priv.h
@@ -0,0 +1,153 @@
+/*
+ * drivers/video/tegra/dc/ext/tegra_dc_ext_priv.h
+ *
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * Author: Robert Morell <rmorell@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * 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.
+ */
+
+#ifndef __TEGRA_DC_EXT_PRIV_H
+#define __TEGRA_DC_EXT_PRIV_H
+
+#include <linux/cdev.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/poll.h>
+
+#include <mach/dc.h>
+#include <linux/nvmap.h>
+
+#include <video/tegra_dc_ext.h>
+
+struct tegra_dc_ext;
+
+struct tegra_dc_ext_user {
+	struct tegra_dc_ext	*ext;
+	struct nvmap_client	*nvmap;
+};
+
+enum {
+	TEGRA_DC_Y,
+	TEGRA_DC_U,
+	TEGRA_DC_V,
+	TEGRA_DC_NUM_PLANES,
+};
+
+struct tegra_dc_ext_win {
+	struct tegra_dc_ext	*ext;
+
+	int			idx;
+
+	struct tegra_dc_ext_user *user;
+
+	struct mutex		lock;
+
+	/* Current nvmap handle (if any) for Y, U, V planes */
+	struct nvmap_handle_ref	*cur_handle[TEGRA_DC_NUM_PLANES];
+
+	struct workqueue_struct	*flip_wq;
+
+	atomic_t		nr_pending_flips;
+
+#ifndef CONFIG_ANDROID
+	struct mutex		queue_lock;
+
+	struct list_head	timestamp_queue;
+#endif /* !CONFIG_ANDROID */
+};
+
+struct tegra_dc_ext {
+	struct tegra_dc			*dc;
+
+	struct cdev			cdev;
+	struct device			*dev;
+
+	struct nvmap_client		*nvmap;
+
+	struct tegra_dc_ext_win		win[DC_N_WINDOWS];
+
+	struct {
+		struct tegra_dc_ext_user	*user;
+		struct nvmap_handle_ref		*cur_handle;
+		struct mutex			lock;
+	} cursor;
+
+	bool				enabled;
+};
+
+#define TEGRA_DC_EXT_EVENT_MASK_ALL \
+	TEGRA_DC_EXT_EVENT_HOTPLUG
+
+#define TEGRA_DC_EXT_EVENT_MAX_SZ	8
+
+struct tegra_dc_ext_event_list {
+	struct tegra_dc_ext_event	event;
+	/* The data field _must_ follow the event field. */
+	char				data[TEGRA_DC_EXT_EVENT_MAX_SZ];
+
+	struct list_head		list;
+};
+
+#define TEGRA_DC_EXT_CAPABILITIES \
+	TEGRA_DC_EXT_CAPABILITIES_CURSOR_MODE
+
+struct tegra_dc_ext_control_user {
+	struct tegra_dc_ext_control	*control;
+
+	struct list_head		event_list;
+	atomic_t			num_events;
+
+	u32				event_mask;
+
+	struct tegra_dc_ext_event_list	event_to_copy;
+	loff_t				partial_copy;
+
+	struct mutex			lock;
+
+	struct list_head		list;
+};
+
+struct tegra_dc_ext_control {
+	struct cdev			cdev;
+	struct device			*dev;
+
+	struct list_head		users;
+
+	struct mutex			lock;
+};
+
+extern int tegra_dc_ext_devno;
+extern struct class *tegra_dc_ext_class;
+
+extern int tegra_dc_ext_pin_window(struct tegra_dc_ext_user *user, u32 id,
+				   struct nvmap_handle_ref **handle,
+				   dma_addr_t *phys_addr);
+
+extern int tegra_dc_ext_get_cursor(struct tegra_dc_ext_user *user);
+extern int tegra_dc_ext_put_cursor(struct tegra_dc_ext_user *user);
+extern int tegra_dc_ext_set_cursor_image(struct tegra_dc_ext_user *user,
+					 struct tegra_dc_ext_cursor_image *);
+extern int tegra_dc_ext_set_cursor(struct tegra_dc_ext_user *user,
+				   struct tegra_dc_ext_cursor *);
+
+extern int tegra_dc_ext_control_init(void);
+
+extern int tegra_dc_ext_queue_hotplug(struct tegra_dc_ext_control *,
+				      int output);
+extern ssize_t tegra_dc_ext_event_read(struct file *filp, char __user *buf,
+				       size_t size, loff_t *ppos);
+extern unsigned int tegra_dc_ext_event_poll(struct file *, poll_table *);
+
+extern int tegra_dc_ext_get_num_outputs(void);
+
+#endif /* __TEGRA_DC_EXT_PRIV_H */
diff --git a/drivers/video/tegra/dc/ext/util.c b/drivers/video/tegra/dc/ext/util.c
new file mode 100644
index 000000000000..bd8b3c012c0e
--- /dev/null
+++ b/drivers/video/tegra/dc/ext/util.c
@@ -0,0 +1,78 @@
+/*
+ * drivers/video/tegra/dc/ext/util.c
+ *
+ * Copyright (c) 2011-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * Author: Robert Morell <rmorell@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * 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.
+ */
+
+#include <linux/err.h>
+#include <linux/types.h>
+
+#include <mach/dc.h>
+#include <linux/nvmap.h>
+
+/* ugh */
+#include "../../nvmap/nvmap.h"
+
+#include "tegra_dc_ext_priv.h"
+
+int tegra_dc_ext_pin_window(struct tegra_dc_ext_user *user, u32 id,
+			    struct nvmap_handle_ref **handle,
+			    dma_addr_t *phys_addr)
+{
+	struct tegra_dc_ext *ext = user->ext;
+	struct nvmap_handle_ref *win_dup;
+	struct nvmap_handle *win_handle;
+	dma_addr_t phys;
+
+	if (!id) {
+		*handle = NULL;
+		*phys_addr = -1;
+
+		return 0;
+	}
+
+	/*
+	 * Take a reference to the buffer using the user's nvmap context, to
+	 * make sure they have permissions to access it.
+	 */
+	win_handle = nvmap_get_handle_id(user->nvmap, id);
+	if (!win_handle)
+		return -EACCES;
+
+	/*
+	 * Duplicate the buffer's handle into the dc_ext driver's nvmap
+	 * context, to ensure that the handle won't be freed as long as it is
+	 * in use by display.
+	 */
+	win_dup = nvmap_duplicate_handle_id(ext->nvmap, id);
+
+	/* Release the reference we took in the user's context above */
+	nvmap_handle_put(win_handle);
+
+	if (IS_ERR(win_dup))
+		return PTR_ERR(win_dup);
+
+	phys = nvmap_pin(ext->nvmap, win_dup);
+	/* XXX this isn't correct for non-pointers... */
+	if (IS_ERR((void *)phys)) {
+		nvmap_free(ext->nvmap, win_dup);
+		return PTR_ERR((void *)phys);
+	}
+
+	*phys_addr = phys;
+	*handle = win_dup;
+
+	return 0;
+}
diff --git a/drivers/video/tegra/dc/hdmi.c b/drivers/video/tegra/dc/hdmi.c
new file mode 100644
index 000000000000..85a5d63e9326
--- /dev/null
+++ b/drivers/video/tegra/dc/hdmi.c
@@ -0,0 +1,2519 @@
+/*
+ * drivers/video/tegra/dc/hdmi.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ *
+ * Copyright (c) 2010-2013, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/fb.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#ifdef CONFIG_SWITCH
+#include <linux/switch.h>
+#endif
+#include <linux/workqueue.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/device.h>
+
+#include <mach/clk.h>
+#include <mach/dc.h>
+#include <mach/fb.h>
+#include <linux/nvhost.h>
+#include <mach/hdmi-audio.h>
+
+#include <video/tegrafb.h>
+
+#include "dc_reg.h"
+#include "dc_priv.h"
+#include "hdmi_reg.h"
+#include "hdmi.h"
+#include "edid.h"
+#include "nvhdcp.h"
+
+/* datasheet claims this will always be 216MHz */
+#define HDMI_AUDIOCLK_FREQ		216000000
+
+#define HDMI_REKEY_DEFAULT		56
+
+#define HDMI_ELD_RESERVED1_INDEX		1
+#define HDMI_ELD_RESERVED2_INDEX		3
+#define HDMI_ELD_VER_INDEX			0
+#define HDMI_ELD_BASELINE_LEN_INDEX		2
+#define HDMI_ELD_CEA_VER_MNL_INDEX		4
+#define HDMI_ELD_SAD_CNT_CON_TYP_SAI_HDCP_INDEX		5
+#define HDMI_ELD_AUD_SYNC_DELAY_INDEX	6
+#define HDMI_ELD_SPK_ALLOC_INDEX		7
+#define HDMI_ELD_PORT_ID_INDEX		8
+#define HDMI_ELD_MANF_NAME_INDEX		16
+#define HDMI_ELD_PRODUCT_CODE_INDEX		18
+#define HDMI_ELD_MONITOR_NAME_INDEX		20
+
+/* These two values need to be cross checked in case of
+     addition/removal from tegra_dc_hdmi_aspect_ratios[] */
+#define TEGRA_DC_HDMI_MIN_ASPECT_RATIO_PERCENT	80
+#define TEGRA_DC_HDMI_MAX_ASPECT_RATIO_PERCENT	320
+
+/* Percentage equivalent of standard aspect ratios
+    accurate upto two decimal digits */
+static int tegra_dc_hdmi_aspect_ratios[] = {
+	/*   3:2	*/	150,
+	/*   4:3	*/	133,
+	/*   4:5	*/	 80,
+	/*   5:4	*/	125,
+	/*   9:5	*/	180,
+	/*  16:5	*/	320,
+	/*  16:9	*/	178,
+	/* 16:10	*/	160,
+	/* 19:10	*/	190,
+	/* 25:16	*/	156,
+	/* 64:35	*/	183,
+	/* 72:35	*/	206
+};
+
+struct tegra_dc_hdmi_data {
+	struct tegra_dc			*dc;
+	struct tegra_edid		*edid;
+	struct tegra_edid_hdmi_eld		eld;
+	struct tegra_nvhdcp		*nvhdcp;
+	struct delayed_work		work;
+
+	struct resource			*base_res;
+	void __iomem			*base;
+	struct clk			*clk;
+
+	struct clk			*disp1_clk;
+	struct clk			*disp2_clk;
+	struct clk			*hda_clk;
+	struct clk			*hda2codec_clk;
+	struct clk			*hda2hdmi_clk;
+
+#ifdef CONFIG_SWITCH
+	struct switch_dev		hpd_switch;
+#endif
+
+	spinlock_t			suspend_lock;
+	bool				suspended;
+	bool				eld_retrieved;
+	bool				clk_enabled;
+	unsigned			audio_freq;
+	unsigned			audio_source;
+	bool				audio_inject_null;
+
+	bool				dvi;
+};
+
+struct tegra_dc_hdmi_data *dc_hdmi;
+
+const struct fb_videomode tegra_dc_hdmi_supported_modes[] = {
+	/* 1280x720p 60hz: EIA/CEA-861-B Format 4 */
+	{
+		.xres =		1280,
+		.yres =		720,
+		.pixclock =	KHZ2PICOS(74250),
+		.hsync_len =	40,	/* h_sync_width */
+		.vsync_len =	5,	/* v_sync_width */
+		.left_margin =	220,	/* h_back_porch */
+		.upper_margin =	20,	/* v_back_porch */
+		.right_margin =	110,	/* h_front_porch */
+		.lower_margin =	5,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+
+	/* 1280x720p 60hz: EIA/CEA-861-B Format 4 (Stereo)*/
+	{
+		.xres =		1280,
+		.yres =		720,
+		.pixclock =	KHZ2PICOS(74250),
+		.hsync_len =	40,	/* h_sync_width */
+		.vsync_len =	5,	/* v_sync_width */
+		.left_margin =	220,	/* h_back_porch */
+		.upper_margin =	20,	/* v_back_porch */
+		.right_margin =	110,	/* h_front_porch */
+		.lower_margin =	5,	/* v_front_porch */
+		.vmode = FB_VMODE_NONINTERLACED |
+#ifndef CONFIG_TEGRA_HDMI_74MHZ_LIMIT
+				 FB_VMODE_STEREO_FRAME_PACK,
+#else
+				 FB_VMODE_STEREO_LEFT_RIGHT,
+#endif
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+
+	/* 720x480p 59.94hz: EIA/CEA-861-B Formats 2 & 3 */
+	{
+		.xres =		720,
+		.yres =		480,
+		.pixclock =	KHZ2PICOS(27000),
+		.hsync_len =	62,	/* h_sync_width */
+		.vsync_len =	6,	/* v_sync_width */
+		.left_margin =	60,	/* h_back_porch */
+		.upper_margin =	30,	/* v_back_porch */
+		.right_margin =	16,	/* h_front_porch */
+		.lower_margin =	9,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = 0,
+	},
+
+	/* 640x480p 60hz: EIA/CEA-861-B Format 1 */
+	{
+		.xres =		640,
+		.yres =		480,
+		.pixclock =	KHZ2PICOS(25200),
+		.hsync_len =	96,	/* h_sync_width */
+		.vsync_len =	2,	/* v_sync_width */
+		.left_margin =	48,	/* h_back_porch */
+		.upper_margin =	33,	/* v_back_porch */
+		.right_margin =	16,	/* h_front_porch */
+		.lower_margin =	10,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = 0,
+	},
+
+	/* 720x576p 50hz EIA/CEA-861-B Formats 17 & 18 */
+	{
+		.xres =		720,
+		.yres =		576,
+		.pixclock =	KHZ2PICOS(27000),
+		.hsync_len =	64,	/* h_sync_width */
+		.vsync_len =	5,	/* v_sync_width */
+		.left_margin =	68,	/* h_back_porch */
+		.upper_margin =	39,	/* v_back_porch */
+		.right_margin =	12,	/* h_front_porch */
+		.lower_margin =	5,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = 0,
+	},
+
+	/* 1920x1080p 23.98/24hz: EIA/CEA-861-B Format 32 (Stereo)*/
+	{
+		.xres =		1920,
+		.yres =		1080,
+		.pixclock =	KHZ2PICOS(74250),
+		.hsync_len =	44,	/* h_sync_width */
+		.vsync_len =	5,	/* v_sync_width */
+		.left_margin =	148,	/* h_back_porch */
+		.upper_margin =	36,	/* v_back_porch */
+		.right_margin =	638,	/* h_front_porch */
+		.lower_margin =	4,	/* v_front_porch */
+		.vmode = FB_VMODE_NONINTERLACED |
+#ifndef CONFIG_TEGRA_HDMI_74MHZ_LIMIT
+				 FB_VMODE_STEREO_FRAME_PACK,
+#else
+				 FB_VMODE_STEREO_LEFT_RIGHT,
+#endif
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+
+	/* 1920x1080p 30Hz EIA/CEA-861-B Format 34 */
+	{
+		.xres =		1920,
+		.yres =		1080,
+		.pixclock =	KHZ2PICOS(74250),
+		.hsync_len =	44,	/* h_sync_width */
+		.vsync_len =	5,	/* v_sync_width */
+		.left_margin =	148,	/* h_back_porch */
+		.upper_margin =	36,	/* v_back_porch */
+		.right_margin =	88,	/* h_front_porch */
+		.lower_margin =	4,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+
+	/* 1920x1080p 59.94/60hz CVT */
+	{
+		.xres =		1920,
+		.yres =		1080,
+		.pixclock =	KHZ2PICOS(138500),
+		.hsync_len =	32,	/* h_sync_width */
+		.vsync_len =	5,	/* v_sync_width */
+		.left_margin =	80,	/* h_back_porch */
+		.upper_margin =	23,	/* v_back_porch */
+		.right_margin =	48,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode = FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_VERT_HIGH_ACT,
+	},
+
+	/* 1920x1080p 59.94/60hz EIA/CEA-861-B Format 16 */
+	{
+		.xres =		1920,
+		.yres =		1080,
+		.pixclock =	KHZ2PICOS(148500),
+		.hsync_len =	44,	/* h_sync_width */
+		.vsync_len =	5,	/* v_sync_width */
+		.left_margin =	148,	/* h_back_porch */
+		.upper_margin =	36,	/* v_back_porch */
+		.right_margin =	88,	/* h_front_porch */
+		.lower_margin =	4,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+
+	/*
+	* Few VGA/SVGA modes to support monitors with lower
+	* resolutions or to support HDMI<->DVI connection
+	*/
+
+	/* 640x480p 75hz */
+	{
+		.xres =		640,
+		.yres =		480,
+		.pixclock =	KHZ2PICOS(31500),
+		.hsync_len =	96,	/* h_sync_width */
+		.vsync_len =	2,	/* v_sync_width */
+		.left_margin =	48,	/* h_back_porch */
+		.upper_margin =	32,	/* v_back_porch */
+		.right_margin =	16,	/* h_front_porch */
+		.lower_margin =	1,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = 0,
+	},
+	/* 720x400p 59hz */
+	{
+		.xres =		720,
+		.yres =		400,
+		.pixclock =	KHZ2PICOS(35500),
+		.hsync_len =	72,	/* h_sync_width */
+		.vsync_len =	3,	/* v_sync_width */
+		.left_margin =	108,	/* h_back_porch */
+		.upper_margin =	42,	/* v_back_porch */
+		.right_margin =	36,	/* h_front_porch */
+		.lower_margin =	1,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync  = FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 800x600p 60hz */
+	{
+		.xres =		800,
+		.yres =		600,
+		.pixclock =	KHZ2PICOS(40000),
+		.hsync_len =	128,	/* h_sync_width */
+		.vsync_len =	4,	/* v_sync_width */
+		.left_margin =	88,	/* h_back_porch */
+		.upper_margin =	23,	/* v_back_porch */
+		.right_margin =	40,	/* h_front_porch */
+		.lower_margin =	1,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 800x600p 75hz */
+	{
+		.xres =		800,
+		.yres =		600,
+		.pixclock =	KHZ2PICOS(49500),
+		.hsync_len =	80,	/* h_sync_width */
+		.vsync_len =	2,	/* v_sync_width */
+		.left_margin =	160,	/* h_back_porch */
+		.upper_margin =	21,	/* v_back_porch */
+		.right_margin =	16,	/* h_front_porch */
+		.lower_margin =	1,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1024x768p 60hz */
+	{
+		.xres =		1024,
+		.yres =		768,
+		.pixclock =	KHZ2PICOS(65000),
+		.hsync_len =	136,	/* h_sync_width */
+		.vsync_len =	6,	/* v_sync_width */
+		.left_margin =	160,	/* h_back_porch */
+		.upper_margin =	29,	/* v_back_porch */
+		.right_margin =	24,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync =	0,
+	},
+	/* 1024x768p 75hz */
+	{
+		.xres =		1024,
+		.yres =		768,
+		.pixclock =	KHZ2PICOS(78800),
+		.hsync_len =	96,	/* h_sync_width */
+		.vsync_len =	3,	/* v_sync_width */
+		.left_margin =	176,	/* h_back_porch */
+		.upper_margin =	28,	/* v_back_porch */
+		.right_margin =	16,	/* h_front_porch */
+		.lower_margin =	1,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = 0,
+	},
+	/* 1152x864p 75hz */
+	{
+		.xres =		1152,
+		.yres =		864,
+		.pixclock =	KHZ2PICOS(108000),
+		.hsync_len =	128,	/* h_sync_width */
+		.vsync_len =	3,	/* v_sync_width */
+		.left_margin =	256,	/* h_back_porch */
+		.upper_margin =	32,	/* v_back_porch */
+		.right_margin =	64,	/* h_front_porch */
+		.lower_margin =	1,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1280x800p 60hz */
+	{
+		.xres =		1280,
+		.yres =		800,
+		.pixclock =	KHZ2PICOS(83460),
+		.hsync_len =	136,	/* h_sync_width */
+		.vsync_len =	3,	/* v_sync_width */
+		.left_margin =	200,	/* h_back_porch */
+		.upper_margin =	24,	/* v_back_porch */
+		.right_margin =	64,	/* h_front_porch */
+		.lower_margin =	1,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync =		FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1280x960p 60hz */
+	{
+		.xres =		1280,
+		.yres =		960,
+		.pixclock =	KHZ2PICOS(108000),
+		.hsync_len =	136,	/* h_sync_width */
+		.vsync_len =	3,	/* v_sync_width */
+		.left_margin =	216,	/* h_back_porch */
+		.upper_margin =	30,	/* v_back_porch */
+		.right_margin =	80,	/* h_front_porch */
+		.lower_margin =	1,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync =		FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1280x1024p 60hz */
+	{
+		.xres =		1280,
+		.yres =		1024,
+		.pixclock =	KHZ2PICOS(108000),
+		.hsync_len =	112,	/* h_sync_width */
+		.vsync_len =	3,	/* v_sync_width */
+		.left_margin =	248,	/* h_back_porch */
+		.upper_margin =	38,	/* v_back_porch */
+		.right_margin =	48,	/* h_front_porch */
+		.lower_margin =	1,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1280x1024p 75hz */
+	{
+		.xres =		1280,
+		.yres =		1024,
+		.pixclock =	KHZ2PICOS(135000),
+		.hsync_len =	144,	/* h_sync_width */
+		.vsync_len =	3,	/* v_sync_width */
+		.left_margin =	248,	/* h_back_porch */
+		.upper_margin =	38,	/* v_back_porch */
+		.right_margin =	16,	/* h_front_porch */
+		.lower_margin =	1,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1368x768p 60hz */
+	{
+		.xres =		1368,
+		.yres =		768,
+		.pixclock =	KHZ2PICOS(85860),
+		.hsync_len =	144,	/* h_sync_width */
+		.vsync_len =	3,	/* v_sync_width */
+		.left_margin =	216,	/* h_back_porch */
+		.upper_margin =	23,	/* v_back_porch */
+		.right_margin =	72,	/* h_front_porch */
+		.lower_margin =	1,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync =		FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1440x900p 60hz */
+	{
+		.xres =		1440,
+		.yres =		900,
+		.pixclock =	KHZ2PICOS(106470),
+		.hsync_len =	152,	/* h_sync_width */
+		.vsync_len =	3,	/* v_sync_width */
+		.left_margin =	232,	/* h_back_porch */
+		.upper_margin =	28,	/* v_back_porch */
+		.right_margin =	80,	/* h_front_porch */
+		.lower_margin =	1,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync =		FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1600x1200p 60hz */
+	{
+		.xres =		1600,
+		.yres =		1200,
+		.pixclock =	KHZ2PICOS(162000),
+		.hsync_len =	192,	/* h_sync_width */
+		.vsync_len =	3,	/* v_sync_width */
+		.left_margin =	304,	/* h_back_porch */
+		.upper_margin =	46,	/* v_back_porch */
+		.right_margin =	64,	/* h_front_porch */
+		.lower_margin =	1,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1600x1200p 75hz */
+	{
+		.xres =		1600,
+		.yres =		1200,
+		.pixclock =	KHZ2PICOS(202500),
+		.hsync_len =	192,	/* h_sync_width */
+		.vsync_len =	3,	/* v_sync_width */
+		.left_margin =	304,	/* h_back_porch */
+		.upper_margin =	46,	/* v_back_porch */
+		.right_margin =	64,	/* h_front_porch */
+		.lower_margin =	1,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1680x1050p 59.94/60hz */
+	{
+		.xres =		1680,
+		.yres =		1050,
+		.pixclock =	KHZ2PICOS(147140),
+		.hsync_len =	184,	/* h_sync_width */
+		.vsync_len =	3,	/* v_sync_width */
+		.left_margin =	288,	/* h_back_porch */
+		.upper_margin =	33,	/* v_back_porch */
+		.right_margin =	104,	/* h_front_porch */
+		.lower_margin =	1,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync =	FB_SYNC_VERT_HIGH_ACT,
+	},
+};
+
+/* CVT timing representation of VESA modes*/
+const struct fb_videomode tegra_dc_hdmi_supported_cvt_modes[] = {
+
+	/* 640x480p 60hz */
+	{
+		.refresh =	60,
+		.xres =		640,
+		.yres =		480,
+		.pixclock =	KHZ2PICOS(23750),
+		.hsync_len =	64,	/* h_sync_width */
+		.vsync_len =	4,	/* v_sync_width */
+		.left_margin =	80,	/* h_back_porch */
+		.upper_margin =	17,	/* v_back_porch */
+		.right_margin =	16,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 640x480p 75hz */
+	{
+		.refresh =	75,
+		.xres =		640,
+		.yres =		480,
+		.pixclock =	KHZ2PICOS(30750),
+		.hsync_len =	64,	/* h_sync_width */
+		.vsync_len =	4,	/* v_sync_width */
+		.left_margin =	88,	/* h_back_porch */
+		.upper_margin =	21,	/* v_back_porch */
+		.right_margin =	24,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 720x400p 59hz */
+	{
+		.refresh =	59,
+		.xres =		720,
+		.yres =		400,
+		.pixclock =	KHZ2PICOS(22000),
+		.hsync_len =	64,	/* h_sync_width */
+		.vsync_len =	10,	/* v_sync_width */
+		.left_margin =	88,	/* h_back_porch */
+		.upper_margin =	14,	/* v_back_porch */
+		.right_margin =	24,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync  = FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 800x600p 60hz */
+	{
+		.refresh =	60,
+		.xres =		800,
+		.yres =		600,
+		.pixclock =	KHZ2PICOS(38250),
+		.hsync_len =	80,	/* h_sync_width */
+		.vsync_len =	4,	/* v_sync_width */
+		.left_margin =	112,	/* h_back_porch */
+		.upper_margin =	21,	/* v_back_porch */
+		.right_margin =	32,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = 	FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 800x600p 75hz */
+	{
+		.refresh =	75,
+		.xres =		800,
+		.yres =		600,
+		.pixclock =	KHZ2PICOS(49000),
+		.hsync_len =	80,	/* h_sync_width */
+		.vsync_len =	4,	/* v_sync_width */
+		.left_margin =	120,	/* h_back_porch */
+		.upper_margin =	26,	/* v_back_porch */
+		.right_margin =	40,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1024x768p 60hz */
+	{
+		.refresh =	60,
+		.xres =		1024,
+		.yres =		768,
+		.pixclock =	KHZ2PICOS(63500),
+		.hsync_len =	104,	/* h_sync_width */
+		.vsync_len =	4,	/* v_sync_width */
+		.left_margin =	152,	/* h_back_porch */
+		.upper_margin =	27,	/* v_back_porch */
+		.right_margin =	48,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync =	FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1024x768p 75hz */
+	{
+		.refresh =	75,
+		.xres =		1024,
+		.yres =		768,
+		.pixclock =	KHZ2PICOS(82000),
+		.hsync_len =	104,	/* h_sync_width */
+		.vsync_len =	4,	/* v_sync_width */
+		.left_margin =	168,	/* h_back_porch */
+		.upper_margin =	34,	/* v_back_porch */
+		.right_margin =	64,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1152x864p 75hz */
+	{
+		.refresh =	75,
+		.xres =		1152,
+		.yres =		864,
+		.pixclock =	KHZ2PICOS(104500),
+		.hsync_len =	120,	/* h_sync_width */
+		.vsync_len =	10,	/* v_sync_width */
+		.left_margin =	192,	/* h_back_porch */
+		.upper_margin =	38,	/* v_back_porch */
+		.right_margin =	72,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1280x800p 60hz */
+	{
+		.refresh =	60,
+		.xres =		1280,
+		.yres =		800,
+		.pixclock =	KHZ2PICOS(83500),
+		.hsync_len =	128,	/* h_sync_width */
+		.vsync_len =	6,	/* v_sync_width */
+		.left_margin =	200,	/* h_back_porch */
+		.upper_margin =	28,	/* v_back_porch */
+		.right_margin =	72,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync =		FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1280x960p 60hz */
+	{
+		.refresh =	60,
+		.xres =		1280,
+		.yres =		960,
+		.pixclock =	KHZ2PICOS(101250),
+		.hsync_len =	128,	/* h_sync_width */
+		.vsync_len =	4,	/* v_sync_width */
+		.left_margin =	208,	/* h_back_porch */
+		.upper_margin =	33,	/* v_back_porch */
+		.right_margin =	80,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1280x1024p 60hz */
+	{
+		.refresh =	60,
+		.xres =		1280,
+		.yres =		1024,
+		.pixclock =	KHZ2PICOS(109000),
+		.hsync_len =	136,	/* h_sync_width */
+		.vsync_len =	7,	/* v_sync_width */
+		.left_margin =	216,	/* h_back_porch */
+		.upper_margin =	36,	/* v_back_porch */
+		.right_margin =	80,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync =	FB_SYNC_VERT_HIGH_ACT,
+	},
+
+	/* 1280x1024p 75hz */
+	{
+		.refresh =	75,
+		.xres =		1280,
+		.yres =		1024,
+		.pixclock =	KHZ2PICOS(138750),
+		.hsync_len =	136,	/* h_sync_width */
+		.vsync_len =	7,	/* v_sync_width */
+		.left_margin =	224,	/* h_back_porch */
+		.upper_margin =	45,	/* v_back_porch */
+		.right_margin =	88,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1368x768p 60hz */
+	{
+		.refresh =	60,
+		.xres =		1368,
+		.yres =		768,
+		.pixclock =	KHZ2PICOS(85250),
+		.hsync_len =	136,	/* h_sync_width */
+		.vsync_len =	10,	/* v_sync_width */
+		.left_margin =	208,	/* h_back_porch */
+		.upper_margin =	27,	/* v_back_porch */
+		.right_margin =	72,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync =		FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1440x900p 60hz */
+	{
+		.refresh =	60,
+		.xres =		1440,
+		.yres =		900,
+		.pixclock =	KHZ2PICOS(106500),
+		.hsync_len =	152,	/* h_sync_width */
+		.vsync_len =	6,	/* v_sync_width */
+		.left_margin =	232,	/* h_back_porch */
+		.upper_margin =	31,	/* v_back_porch */
+		.right_margin =	80,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync =		FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1600x1200p 60hz */
+	{
+		.refresh =	60,
+		.xres =		1600,
+		.yres =		1200,
+		.pixclock =	KHZ2PICOS(161000),
+		.hsync_len =	168,	/* h_sync_width */
+		.vsync_len =	4,	/* v_sync_width */
+		.left_margin =	280,	/* h_back_porch */
+		.upper_margin =	42,	/* v_back_porch */
+		.right_margin =	112,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1600x1200p 75hz */
+	{
+		.refresh =	75,
+		.xres =		1600,
+		.yres =		1200,
+		.pixclock =	KHZ2PICOS(204750),
+		.hsync_len =	168,	/* h_sync_width */
+		.vsync_len =	4,	/* v_sync_width */
+		.left_margin =	288,	/* h_back_porch */
+		.upper_margin =	52,	/* v_back_porch */
+		.right_margin =	120,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_VERT_HIGH_ACT,
+	},
+	/* 1680x1050p 59.94/60hz */
+	{
+		.refresh =	60,
+		.xres =		1680,
+		.yres =		1050,
+		.pixclock =	KHZ2PICOS(140000),
+		.hsync_len =	168,	/* h_sync_width */
+		.vsync_len =	10,	/* v_sync_width */
+		.left_margin =	272,	/* h_back_porch */
+		.upper_margin =	36,	/* v_back_porch */
+		.right_margin =	104,	/* h_front_porch */
+		.lower_margin =	3,	/* v_front_porch */
+		.vmode =	FB_VMODE_NONINTERLACED,
+		.sync =	FB_SYNC_VERT_HIGH_ACT,
+	},
+};
+
+/* table of electrical settings, must be in acending order. */
+struct tdms_config {
+	int pclk;
+	u32 pll0;
+	u32 pll1;
+	u32 pe_current; /* pre-emphasis */
+	u32 drive_current;
+};
+
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+const struct tdms_config tdms_config[] = {
+	{ /* 480p modes */
+	.pclk = 27000000,
+	.pll0 = SOR_PLL_BG_V17_S(3) | SOR_PLL_ICHPMP(1) | SOR_PLL_RESISTORSEL |
+		SOR_PLL_VCOCAP(0) | SOR_PLL_TX_REG_LOAD(0),
+	.pll1 = SOR_PLL_TMDS_TERM_ENABLE,
+	.pe_current = PE_CURRENT0(PE_CURRENT_0_0_mA) |
+		PE_CURRENT1(PE_CURRENT_0_0_mA) |
+		PE_CURRENT2(PE_CURRENT_0_0_mA) |
+		PE_CURRENT3(PE_CURRENT_0_0_mA),
+	.drive_current = DRIVE_CURRENT_LANE0(DRIVE_CURRENT_5_250_mA) |
+		DRIVE_CURRENT_LANE1(DRIVE_CURRENT_5_250_mA) |
+		DRIVE_CURRENT_LANE2(DRIVE_CURRENT_5_250_mA) |
+		DRIVE_CURRENT_LANE3(DRIVE_CURRENT_5_250_mA),
+	},
+	{ /* 720p modes */
+	.pclk = 74250000,
+	.pll0 = SOR_PLL_BG_V17_S(3) | SOR_PLL_ICHPMP(1) | SOR_PLL_RESISTORSEL |
+		SOR_PLL_VCOCAP(1) | SOR_PLL_TX_REG_LOAD(0),
+	.pll1 = SOR_PLL_TMDS_TERM_ENABLE | SOR_PLL_PE_EN,
+	.pe_current = PE_CURRENT0(PE_CURRENT_5_0_mA) |
+		PE_CURRENT1(PE_CURRENT_5_0_mA) |
+		PE_CURRENT2(PE_CURRENT_5_0_mA) |
+		PE_CURRENT3(PE_CURRENT_5_0_mA),
+	.drive_current = DRIVE_CURRENT_LANE0(DRIVE_CURRENT_5_250_mA) |
+		DRIVE_CURRENT_LANE1(DRIVE_CURRENT_5_250_mA) |
+		DRIVE_CURRENT_LANE2(DRIVE_CURRENT_5_250_mA) |
+		DRIVE_CURRENT_LANE3(DRIVE_CURRENT_5_250_mA),
+	},
+	{ /* 1080p modes */
+	.pclk = INT_MAX,
+	.pll0 = SOR_PLL_BG_V17_S(3) | SOR_PLL_ICHPMP(1) | SOR_PLL_RESISTORSEL |
+		SOR_PLL_VCOCAP(3) | SOR_PLL_TX_REG_LOAD(0),
+	.pll1 = SOR_PLL_TMDS_TERM_ENABLE | SOR_PLL_PE_EN,
+	.pe_current = PE_CURRENT0(PE_CURRENT_5_0_mA) |
+		PE_CURRENT1(PE_CURRENT_5_0_mA) |
+		PE_CURRENT2(PE_CURRENT_5_0_mA) |
+		PE_CURRENT3(PE_CURRENT_5_0_mA),
+	.drive_current = DRIVE_CURRENT_LANE0(DRIVE_CURRENT_5_250_mA) |
+		DRIVE_CURRENT_LANE1(DRIVE_CURRENT_5_250_mA) |
+		DRIVE_CURRENT_LANE2(DRIVE_CURRENT_5_250_mA) |
+		DRIVE_CURRENT_LANE3(DRIVE_CURRENT_5_250_mA),
+	},
+};
+#else /*  CONFIG_ARCH_TEGRA_2x_SOC */
+const struct tdms_config tdms_config[] = {
+	{ /* 480p modes */
+	.pclk = 27000000,
+	.pll0 = SOR_PLL_BG_V17_S(3) | SOR_PLL_ICHPMP(1) | SOR_PLL_RESISTORSEL |
+		SOR_PLL_VCOCAP(0) | SOR_PLL_TX_REG_LOAD(3),
+	.pll1 = SOR_PLL_TMDS_TERM_ENABLE,
+	.pe_current = PE_CURRENT0(PE_CURRENT_0_0_mA) |
+		PE_CURRENT1(PE_CURRENT_0_0_mA) |
+		PE_CURRENT2(PE_CURRENT_0_0_mA) |
+		PE_CURRENT3(PE_CURRENT_0_0_mA),
+	.drive_current = DRIVE_CURRENT_LANE0(DRIVE_CURRENT_7_125_mA) |
+		DRIVE_CURRENT_LANE1(DRIVE_CURRENT_7_125_mA) |
+		DRIVE_CURRENT_LANE2(DRIVE_CURRENT_7_125_mA) |
+		DRIVE_CURRENT_LANE3(DRIVE_CURRENT_7_125_mA),
+	},
+	{ /* 720p modes */
+	.pclk = 74250000,
+	.pll0 = SOR_PLL_BG_V17_S(3) | SOR_PLL_ICHPMP(1) | SOR_PLL_RESISTORSEL |
+		SOR_PLL_VCOCAP(1) | SOR_PLL_TX_REG_LOAD(3),
+	.pll1 = SOR_PLL_TMDS_TERM_ENABLE | SOR_PLL_PE_EN,
+	.pe_current = PE_CURRENT0(PE_CURRENT_6_0_mA) |
+		PE_CURRENT1(PE_CURRENT_6_0_mA) |
+		PE_CURRENT2(PE_CURRENT_6_0_mA) |
+		PE_CURRENT3(PE_CURRENT_6_0_mA),
+	.drive_current = DRIVE_CURRENT_LANE0(DRIVE_CURRENT_7_125_mA) |
+		DRIVE_CURRENT_LANE1(DRIVE_CURRENT_7_125_mA) |
+		DRIVE_CURRENT_LANE2(DRIVE_CURRENT_7_125_mA) |
+		DRIVE_CURRENT_LANE3(DRIVE_CURRENT_7_125_mA),
+	},
+	{ /* 1080p modes */
+	.pclk = INT_MAX,
+	.pll0 = SOR_PLL_BG_V17_S(3) | SOR_PLL_ICHPMP(1) | SOR_PLL_RESISTORSEL |
+		SOR_PLL_VCOCAP(1) | SOR_PLL_TX_REG_LOAD(3),
+	.pll1 = SOR_PLL_TMDS_TERM_ENABLE | SOR_PLL_PE_EN,
+	.pe_current = PE_CURRENT0(PE_CURRENT_6_0_mA) |
+		PE_CURRENT1(PE_CURRENT_6_0_mA) |
+		PE_CURRENT2(PE_CURRENT_6_0_mA) |
+		PE_CURRENT3(PE_CURRENT_6_0_mA),
+	.drive_current = DRIVE_CURRENT_LANE0(DRIVE_CURRENT_7_125_mA) |
+		DRIVE_CURRENT_LANE1(DRIVE_CURRENT_7_125_mA) |
+		DRIVE_CURRENT_LANE2(DRIVE_CURRENT_7_125_mA) |
+		DRIVE_CURRENT_LANE3(DRIVE_CURRENT_7_125_mA),
+	},
+};
+#endif
+
+struct tegra_hdmi_audio_config {
+	unsigned pix_clock;
+	unsigned n;
+	unsigned cts;
+	unsigned aval;
+};
+
+
+const struct tegra_hdmi_audio_config tegra_hdmi_audio_32k[] = {
+	{25200000,	4096,	25200,	24000},
+	{27000000,	4096,	27000,	24000},
+	{74250000,	4096,	74250,	24000},
+	{148500000,	4096,	148500,	24000},
+	{0,		0,	0},
+};
+
+const struct tegra_hdmi_audio_config tegra_hdmi_audio_44_1k[] = {
+	{25200000,	5880,	26250,	25000},
+	{27000000,	5880,	28125,	25000},
+	{74250000,	4704,	61875,	20000},
+	{148500000,	4704,	123750,	20000},
+	{0,		0,	0},
+};
+
+const struct tegra_hdmi_audio_config tegra_hdmi_audio_48k[] = {
+	{25200000,	6144,	25200,	24000},
+	{27000000,	6144,	27000,	24000},
+	{74250000,	6144,	74250,	24000},
+	{148500000,	6144,	148500,	24000},
+	{0,		0,	0},
+};
+
+const struct tegra_hdmi_audio_config tegra_hdmi_audio_88_2k[] = {
+	{25200000,	11760,	26250,	25000},
+	{27000000,	11760,	28125,	25000},
+	{74250000,	9408,	61875,	20000},
+	{148500000,	9408,	123750, 20000},
+	{0,		0,	0},
+};
+
+const struct tegra_hdmi_audio_config tegra_hdmi_audio_96k[] = {
+	{25200000,	12288,	25200,	24000},
+	{27000000,	12288,	27000,	24000},
+	{74250000,	12288,	74250,	24000},
+	{148500000,	12288,	148500,	24000},
+	{0,		0,	0},
+};
+
+const struct tegra_hdmi_audio_config tegra_hdmi_audio_176_4k[] = {
+	{25200000,	23520,	26250,	25000},
+	{27000000,	23520,	28125,	25000},
+	{74250000,	18816,	61875,	20000},
+	{148500000,	18816,	123750,	20000},
+	{0,		0,	0},
+};
+
+const struct tegra_hdmi_audio_config tegra_hdmi_audio_192k[] = {
+	{25200000,	24576,	25200,	24000},
+	{27000000,	24576,	27000,	24000},
+	{74250000,	24576,	74250,	24000},
+	{148500000,	24576,	148500,	24000},
+	{0,		0,	0},
+};
+
+static const struct tegra_hdmi_audio_config
+*tegra_hdmi_get_audio_config(unsigned audio_freq, unsigned pix_clock)
+{
+	const struct tegra_hdmi_audio_config *table;
+
+	switch (audio_freq) {
+	case AUDIO_FREQ_32K:
+		table = tegra_hdmi_audio_32k;
+		break;
+	case AUDIO_FREQ_44_1K:
+		table = tegra_hdmi_audio_44_1k;
+		break;
+	case AUDIO_FREQ_48K:
+		table = tegra_hdmi_audio_48k;
+		break;
+	case AUDIO_FREQ_88_2K:
+		table = tegra_hdmi_audio_88_2k;
+		break;
+	case AUDIO_FREQ_96K:
+		table = tegra_hdmi_audio_96k;
+		break;
+	case AUDIO_FREQ_176_4K:
+		table = tegra_hdmi_audio_176_4k;
+		break;
+	case AUDIO_FREQ_192K:
+		table = tegra_hdmi_audio_192k;
+		break;
+	default:
+		return NULL;
+	}
+
+	while (table->pix_clock) {
+		if (table->pix_clock == pix_clock)
+			return table;
+		table++;
+	}
+
+	return NULL;
+}
+
+
+unsigned long tegra_hdmi_readl(struct tegra_dc_hdmi_data *hdmi,
+					     unsigned long reg)
+{
+	unsigned long ret;
+	ret = readl(hdmi->base + reg * 4);
+	trace_printk("readl %p=%#08lx\n", hdmi->base + reg * 4, ret);
+	return ret;
+}
+
+void tegra_hdmi_writel(struct tegra_dc_hdmi_data *hdmi,
+				     unsigned long val, unsigned long reg)
+{
+	trace_printk("writel %p=%#08lx\n", hdmi->base + reg * 4, val);
+	writel(val, hdmi->base + reg * 4);
+}
+
+static inline void tegra_hdmi_clrsetbits(struct tegra_dc_hdmi_data *hdmi,
+					 unsigned long reg, unsigned long clr,
+					 unsigned long set)
+{
+	unsigned long val = tegra_hdmi_readl(hdmi, reg);
+	val &= ~clr;
+	val |= set;
+	tegra_hdmi_writel(hdmi, val, reg);
+}
+
+#ifdef CONFIG_DEBUG_FS
+static int dbg_hdmi_show(struct seq_file *s, void *unused)
+{
+	struct tegra_dc_hdmi_data *hdmi = s->private;
+
+#define DUMP_REG(a) do {						\
+		seq_printf(s, "%-32s\t%03x\t%08lx\n",			\
+		       #a, a, tegra_hdmi_readl(hdmi, a));		\
+	} while (0)
+
+	tegra_dc_io_start(hdmi->dc);
+	clk_enable(hdmi->clk);
+
+	DUMP_REG(HDMI_CTXSW);
+	DUMP_REG(HDMI_NV_PDISP_SOR_STATE0);
+	DUMP_REG(HDMI_NV_PDISP_SOR_STATE1);
+	DUMP_REG(HDMI_NV_PDISP_SOR_STATE2);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_AN_MSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_AN_LSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_CN_MSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_CN_LSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_AKSV_MSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_AKSV_LSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_BKSV_MSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_BKSV_LSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_CKSV_MSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_CKSV_LSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_DKSV_MSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_DKSV_LSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_CTRL);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_CMODE);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_MPRIME_MSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_MPRIME_LSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_SPRIME_MSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_SPRIME_LSB2);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_SPRIME_LSB1);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_RI);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_CS_MSB);
+	DUMP_REG(HDMI_NV_PDISP_RG_HDCP_CS_LSB);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AUDIO_EMU0);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AUDIO_EMU_RDATA0);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AUDIO_EMU1);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AUDIO_EMU2);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AUDIO_INFOFRAME_CTRL);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AUDIO_INFOFRAME_STATUS);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AUDIO_INFOFRAME_HEADER);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AUDIO_INFOFRAME_SUBPACK0_LOW);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AUDIO_INFOFRAME_SUBPACK0_HIGH);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_CTRL);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_STATUS);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_HEADER);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_SUBPACK0_LOW);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_SUBPACK0_HIGH);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_SUBPACK1_LOW);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_SUBPACK1_HIGH);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_GENERIC_CTRL);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_GENERIC_STATUS);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_GENERIC_HEADER);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK0_LOW);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK0_HIGH);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK1_LOW);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK1_HIGH);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK2_LOW);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK2_HIGH);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK3_LOW);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK3_HIGH);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_ACR_CTRL);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_ACR_0320_SUBPACK_LOW);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_ACR_0320_SUBPACK_HIGH);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_ACR_0441_SUBPACK_LOW);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_ACR_0441_SUBPACK_HIGH);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_ACR_0882_SUBPACK_LOW);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_ACR_0882_SUBPACK_HIGH);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_ACR_1764_SUBPACK_LOW);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_ACR_1764_SUBPACK_HIGH);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_ACR_0480_SUBPACK_LOW);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_ACR_0480_SUBPACK_HIGH);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_ACR_0960_SUBPACK_LOW);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_ACR_0960_SUBPACK_HIGH);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_ACR_1920_SUBPACK_LOW);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_ACR_1920_SUBPACK_HIGH);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_CTRL);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_VSYNC_KEEPOUT);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_VSYNC_WINDOW);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_GCP_CTRL);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_GCP_STATUS);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_GCP_SUBPACK);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_CHANNEL_STATUS1);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_CHANNEL_STATUS2);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_EMU0);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_EMU1);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_EMU1_RDATA);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_SPARE);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_SPDIF_CHN_STATUS1);
+	DUMP_REG(HDMI_NV_PDISP_HDMI_SPDIF_CHN_STATUS2);
+	DUMP_REG(HDMI_NV_PDISP_HDCPRIF_ROM_CTRL);
+	DUMP_REG(HDMI_NV_PDISP_SOR_CAP);
+	DUMP_REG(HDMI_NV_PDISP_SOR_PWR);
+	DUMP_REG(HDMI_NV_PDISP_SOR_TEST);
+	DUMP_REG(HDMI_NV_PDISP_SOR_PLL0);
+	DUMP_REG(HDMI_NV_PDISP_SOR_PLL1);
+	DUMP_REG(HDMI_NV_PDISP_SOR_PLL2);
+	DUMP_REG(HDMI_NV_PDISP_SOR_CSTM);
+	DUMP_REG(HDMI_NV_PDISP_SOR_LVDS);
+	DUMP_REG(HDMI_NV_PDISP_SOR_CRCA);
+	DUMP_REG(HDMI_NV_PDISP_SOR_CRCB);
+	DUMP_REG(HDMI_NV_PDISP_SOR_BLANK);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_CTL);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INST0);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INST1);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INST2);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INST3);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INST4);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INST5);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INST6);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INST7);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INST8);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INST9);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INSTA);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INSTB);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INSTC);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INSTD);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INSTE);
+	DUMP_REG(HDMI_NV_PDISP_SOR_SEQ_INSTF);
+	DUMP_REG(HDMI_NV_PDISP_SOR_VCRCA0);
+	DUMP_REG(HDMI_NV_PDISP_SOR_VCRCA1);
+	DUMP_REG(HDMI_NV_PDISP_SOR_CCRCA0);
+	DUMP_REG(HDMI_NV_PDISP_SOR_CCRCA1);
+	DUMP_REG(HDMI_NV_PDISP_SOR_EDATAA0);
+	DUMP_REG(HDMI_NV_PDISP_SOR_EDATAA1);
+	DUMP_REG(HDMI_NV_PDISP_SOR_COUNTA0);
+	DUMP_REG(HDMI_NV_PDISP_SOR_COUNTA1);
+	DUMP_REG(HDMI_NV_PDISP_SOR_DEBUGA0);
+	DUMP_REG(HDMI_NV_PDISP_SOR_DEBUGA1);
+	DUMP_REG(HDMI_NV_PDISP_SOR_TRIG);
+	DUMP_REG(HDMI_NV_PDISP_SOR_MSCHECK);
+	DUMP_REG(HDMI_NV_PDISP_SOR_LANE_DRIVE_CURRENT);
+	DUMP_REG(HDMI_NV_PDISP_AUDIO_DEBUG0);
+	DUMP_REG(HDMI_NV_PDISP_AUDIO_DEBUG1);
+	DUMP_REG(HDMI_NV_PDISP_AUDIO_DEBUG2);
+	DUMP_REG(HDMI_NV_PDISP_AUDIO_FS(0));
+	DUMP_REG(HDMI_NV_PDISP_AUDIO_FS(1));
+	DUMP_REG(HDMI_NV_PDISP_AUDIO_FS(2));
+	DUMP_REG(HDMI_NV_PDISP_AUDIO_FS(3));
+	DUMP_REG(HDMI_NV_PDISP_AUDIO_FS(4));
+	DUMP_REG(HDMI_NV_PDISP_AUDIO_FS(5));
+	DUMP_REG(HDMI_NV_PDISP_AUDIO_FS(6));
+	DUMP_REG(HDMI_NV_PDISP_AUDIO_PULSE_WIDTH);
+	DUMP_REG(HDMI_NV_PDISP_AUDIO_THRESHOLD);
+	DUMP_REG(HDMI_NV_PDISP_AUDIO_CNTRL0);
+	DUMP_REG(HDMI_NV_PDISP_AUDIO_N);
+	DUMP_REG(HDMI_NV_PDISP_HDCPRIF_ROM_TIMING);
+	DUMP_REG(HDMI_NV_PDISP_SOR_REFCLK);
+	DUMP_REG(HDMI_NV_PDISP_CRC_CONTROL);
+	DUMP_REG(HDMI_NV_PDISP_INPUT_CONTROL);
+	DUMP_REG(HDMI_NV_PDISP_SCRATCH);
+	DUMP_REG(HDMI_NV_PDISP_PE_CURRENT);
+	DUMP_REG(HDMI_NV_PDISP_KEY_CTRL);
+	DUMP_REG(HDMI_NV_PDISP_KEY_DEBUG0);
+	DUMP_REG(HDMI_NV_PDISP_KEY_DEBUG1);
+	DUMP_REG(HDMI_NV_PDISP_KEY_DEBUG2);
+	DUMP_REG(HDMI_NV_PDISP_KEY_HDCP_KEY_0);
+	DUMP_REG(HDMI_NV_PDISP_KEY_HDCP_KEY_1);
+	DUMP_REG(HDMI_NV_PDISP_KEY_HDCP_KEY_2);
+	DUMP_REG(HDMI_NV_PDISP_KEY_HDCP_KEY_3);
+	DUMP_REG(HDMI_NV_PDISP_KEY_HDCP_KEY_TRIG);
+	DUMP_REG(HDMI_NV_PDISP_KEY_SKEY_INDEX);
+#undef DUMP_REG
+
+	clk_disable(hdmi->clk);
+	tegra_dc_io_end(hdmi->dc);
+
+	return 0;
+}
+
+static int dbg_hdmi_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dbg_hdmi_show, inode->i_private);
+}
+
+static const struct file_operations dbg_fops = {
+	.open		= dbg_hdmi_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static struct dentry *hdmidir;
+
+static void tegra_dc_hdmi_debug_create(struct tegra_dc_hdmi_data *hdmi)
+{
+	struct dentry *retval;
+
+	hdmidir = debugfs_create_dir("tegra_hdmi", NULL);
+	if (!hdmidir)
+		return;
+	retval = debugfs_create_file("regs", S_IRUGO, hdmidir, hdmi,
+		&dbg_fops);
+	if (!retval)
+		goto free_out;
+	return;
+free_out:
+	debugfs_remove_recursive(hdmidir);
+	hdmidir = NULL;
+	return;
+}
+#else
+static inline void tegra_dc_hdmi_debug_create(struct tegra_dc_hdmi_data *hdmi)
+{ }
+#endif
+
+#define PIXCLOCK_TOLERANCE	200
+
+static int tegra_dc_calc_clock_per_frame(const struct fb_videomode *mode)
+{
+	return (mode->left_margin + mode->xres +
+		mode->right_margin + mode->hsync_len) *
+	       (mode->upper_margin + mode->yres +
+		mode->lower_margin + mode->vsync_len);
+}
+
+static bool tegra_dc_hdmi_valid_pixclock(const struct tegra_dc *dc,
+					const struct fb_videomode *mode)
+{
+	unsigned max_pixclock = tegra_dc_get_out_max_pixclock(dc);
+	if (max_pixclock) {
+		/* this might look counter-intuitive,
+		 * but pixclock's unit is picos(not Khz)
+		 */
+		return mode->pixclock >= max_pixclock;
+	} else {
+		return true;
+	}
+}
+
+static bool tegra_dc_cvt_mode_equal(const struct fb_videomode *mode1,
+				const struct fb_videomode *mode2)
+{
+	return (mode1->xres == mode2->xres &&
+		mode1->yres == mode2->yres &&
+		mode1->refresh == mode2->refresh &&
+		mode1->vmode == mode2->vmode);
+}
+
+static bool tegra_dc_reload_mode(struct fb_videomode *mode)
+{
+	int i = 0;
+	for (i = 0; i < ARRAY_SIZE(tegra_dc_hdmi_supported_cvt_modes); i++) {
+		const struct fb_videomode *cvt_mode
+				= &tegra_dc_hdmi_supported_cvt_modes[i];
+		if (tegra_dc_cvt_mode_equal(cvt_mode, mode)) {
+			memcpy(mode, cvt_mode, sizeof(*mode));
+			return true;
+		}
+	}
+	return false;
+}
+
+static bool tegra_dc_hdmi_valid_asp_ratio(const struct tegra_dc *dc,
+					struct fb_videomode *mode)
+{
+	int count = 0;
+	int m_aspratio = 0;
+	int s_aspratio = 0;
+
+	if (!mode->yres)
+		return false;
+
+	/* To check the aspect upto two decimal digits, calculate in % */
+	if (mode->yres)
+		m_aspratio = (mode->xres*100 / mode->yres);
+
+	if ((m_aspratio < TEGRA_DC_HDMI_MIN_ASPECT_RATIO_PERCENT) ||
+			(m_aspratio > TEGRA_DC_HDMI_MAX_ASPECT_RATIO_PERCENT))
+				return false;
+
+	/* Check from the table of  supported aspect ratios, allow
+	    difference of 1% for second decimal digit calibration */
+	for (count = 0; count < ARRAY_SIZE(tegra_dc_hdmi_aspect_ratios);
+		 count++) {
+			s_aspratio =  tegra_dc_hdmi_aspect_ratios[count];
+			if ((m_aspratio == s_aspratio) ||
+				(abs(m_aspratio - s_aspratio) == 1))
+				return true;
+	}
+
+	return false;
+}
+
+
+static bool tegra_dc_hdmi_mode_filter(const struct tegra_dc *dc,
+					struct fb_videomode *mode)
+{
+	if (mode->vmode & FB_VMODE_INTERLACED)
+		return false;
+
+	/* Ignore modes with a 0 pixel clock */
+	if (!mode->pixclock)
+		return false;
+
+#ifdef CONFIG_TEGRA_HDMI_74MHZ_LIMIT
+		if (PICOS2KHZ(mode->pixclock) > 74250)
+			return false;
+#endif
+
+	/* Check if the mode's pixel clock is more than the max rate*/
+	if (!tegra_dc_hdmi_valid_pixclock(dc, mode))
+		return false;
+
+	/* Check if the mode's aspect ratio is supported */
+	if (!tegra_dc_hdmi_valid_asp_ratio(dc, mode))
+		return false;
+
+	/* Check some of DC's constraints */
+	if (mode->hsync_len > 1 && mode->vsync_len > 1 &&
+		mode->lower_margin + mode->vsync_len + mode->upper_margin > 1 &&
+		mode->xres >= 16 && mode->yres >= 16) {
+
+		if (mode->lower_margin == 1) {
+			/* This might be the case for HDMI<->DVI
+			 * where std VESA representation will not
+			 * pass constraint V_FRONT_PORCH >=
+			 * V_REF_TO_SYNC + 1.So reload mode in
+			 * CVT timing standards.
+			 */
+			if (!tegra_dc_reload_mode(mode))
+				return false;
+		}
+		mode->flag = FB_MODE_IS_DETAILED;
+		mode->refresh = (PICOS2KHZ(mode->pixclock) * 1000) /
+				tegra_dc_calc_clock_per_frame(mode);
+		return true;
+	}
+
+	return false;
+}
+
+static bool tegra_dc_hdmi_hpd(struct tegra_dc *dc)
+{
+	return tegra_dc_hpd(dc);
+}
+
+
+void tegra_dc_hdmi_detect_config(struct tegra_dc *dc,
+						struct fb_monspecs *specs)
+{
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+
+	/* monitors like to lie about these but they are still useful for
+	 * detecting aspect ratios
+	 */
+	dc->out->h_size = specs->max_x * 1000;
+	dc->out->v_size = specs->max_y * 1000;
+
+	hdmi->dvi = !(specs->misc & FB_MISC_HDMI);
+
+	if (dc->fb != NULL)
+		tegra_fb_update_monspecs(dc->fb, specs, tegra_dc_hdmi_mode_filter);
+#ifdef CONFIG_SWITCH
+	hdmi->hpd_switch.state = 0;
+	switch_set_state(&hdmi->hpd_switch, 1);
+#endif
+	dev_info(&dc->ndev->dev, "display detected\n");
+
+	dc->connected = true;
+	tegra_dc_ext_process_hotplug(dc->ndev->id);
+}
+
+/* This function is used to enable DC1 and HDMI for the purpose of testing. */
+bool tegra_dc_hdmi_detect_test(struct tegra_dc *dc, unsigned char *edid_ptr)
+{
+	int err;
+	struct fb_monspecs specs;
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+
+	if (!hdmi || !edid_ptr) {
+		dev_err(&dc->ndev->dev, "HDMI test failed to get arguments.\n");
+		return false;
+	}
+
+	err = tegra_edid_get_monspecs_test(hdmi->edid, &specs, edid_ptr);
+	if (err < 0) {
+		/* Check if there's a hard-wired mode, if so, enable it */
+		if (dc->out->n_modes)
+			tegra_dc_enable(dc);
+		else {
+			dev_err(&dc->ndev->dev, "error reading edid\n");
+			goto fail;
+		}
+#ifdef CONFIG_SWITCH
+		hdmi->hpd_switch.state = 0;
+		switch_set_state(&hdmi->hpd_switch, 1);
+#endif
+		dev_info(&dc->ndev->dev, "display detected\n");
+
+		dc->connected = true;
+		tegra_dc_ext_process_hotplug(dc->ndev->id);
+	} else {
+		err = tegra_edid_get_eld(hdmi->edid, &hdmi->eld);
+		if (err < 0) {
+			dev_err(&dc->ndev->dev, "error populating eld\n");
+			goto fail;
+		}
+		hdmi->eld_retrieved = true;
+
+		tegra_dc_hdmi_detect_config(dc, &specs);
+	}
+
+	return true;
+
+fail:
+	hdmi->eld_retrieved = false;
+#ifdef CONFIG_SWITCH
+	switch_set_state(&hdmi->hpd_switch, 0);
+#endif
+	tegra_nvhdcp_set_plug(hdmi->nvhdcp, 0);
+	return false;
+}
+EXPORT_SYMBOL(tegra_dc_hdmi_detect_test);
+
+static bool tegra_dc_hdmi_detect(struct tegra_dc *dc)
+{
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+	struct fb_monspecs specs;
+	int err;
+	int cnt = 0;
+
+	if (!tegra_dc_hdmi_hpd(dc))
+		goto fail;
+
+	err = tegra_edid_get_monspecs(hdmi->edid, &specs);
+	/* retry, maybe hdmi detect is not debounced or the monitor needs some time */
+	while ( (err < 0) && (cnt++ < 4) ) {
+			dev_err(&dc->ndev->dev, "error reading edid, trying again in 500ms\n");
+			msleep(500);
+			err = tegra_edid_get_monspecs(hdmi->edid, &specs);
+	}
+	if (err < 0) {
+		/*
+		 * this check is basically senseless now due to us always
+		 * providing at least one fallback mode through modedb
+		 */
+		if (dc->out->n_modes)
+			tegra_dc_enable(dc);
+		else {
+			dev_err(&dc->ndev->dev, "error reading edid\n");
+			goto fail;
+		}
+#ifdef CONFIG_SWITCH
+		hdmi->hpd_switch.state = 0;
+		switch_set_state(&hdmi->hpd_switch, 1);
+#endif
+		dev_info(&dc->ndev->dev, "display detected\n");
+
+		dc->connected = true;
+		tegra_dc_ext_process_hotplug(dc->ndev->id);
+	} else {
+		err = tegra_edid_get_eld(hdmi->edid, &hdmi->eld);
+		if (err < 0) {
+			dev_err(&dc->ndev->dev, "error populating eld\n");
+			goto fail;
+		}
+		hdmi->eld_retrieved = true;
+
+		tegra_dc_hdmi_detect_config(dc, &specs);
+	}
+
+	return true;
+
+fail:
+	/* disable upon boot disconnected as well as upon later disconnect */
+	if (dc->enabled)
+		tegra_dc_disable(dc);
+	hdmi->eld_retrieved = false;
+#ifdef CONFIG_SWITCH
+	switch_set_state(&hdmi->hpd_switch, 0);
+#endif
+	tegra_nvhdcp_set_plug(hdmi->nvhdcp, 0);
+	return false;
+}
+
+
+static void tegra_dc_hdmi_detect_worker(struct work_struct *work)
+{
+	struct tegra_dc_hdmi_data *hdmi =
+		container_of(to_delayed_work(work), struct tegra_dc_hdmi_data, work);
+	struct tegra_dc *dc = hdmi->dc;
+
+	tegra_dc_enable(dc);
+	msleep(5);
+	if (!tegra_dc_hdmi_detect(dc)) {
+		tegra_dc_disable(dc);
+		tegra_fb_update_monspecs(dc->fb, NULL, NULL);
+
+		dc->connected = false;
+		tegra_dc_ext_process_hotplug(dc->ndev->id);
+	}
+}
+
+static irqreturn_t tegra_dc_hdmi_irq(int irq, void *ptr)
+{
+	struct tegra_dc *dc = ptr;
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&hdmi->suspend_lock, flags);
+	if (!hdmi->suspended) {
+		__cancel_delayed_work(&hdmi->work);
+		if (tegra_dc_hdmi_hpd(dc))
+			queue_delayed_work(system_nrt_wq, &hdmi->work,
+					   msecs_to_jiffies(100));
+		else
+			queue_delayed_work(system_nrt_wq, &hdmi->work,
+					   msecs_to_jiffies(30));
+	}
+	spin_unlock_irqrestore(&hdmi->suspend_lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+static void tegra_dc_hdmi_suspend(struct tegra_dc *dc)
+{
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+	unsigned long flags;
+
+	tegra_nvhdcp_suspend(hdmi->nvhdcp);
+	spin_lock_irqsave(&hdmi->suspend_lock, flags);
+	hdmi->suspended = true;
+	spin_unlock_irqrestore(&hdmi->suspend_lock, flags);
+}
+
+static void tegra_dc_hdmi_resume(struct tegra_dc *dc)
+{
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&hdmi->suspend_lock, flags);
+	hdmi->suspended = false;
+
+	if (tegra_dc_hdmi_hpd(dc))
+		queue_delayed_work(system_nrt_wq, &hdmi->work,
+				   msecs_to_jiffies(100));
+	else
+		queue_delayed_work(system_nrt_wq, &hdmi->work,
+				   msecs_to_jiffies(30));
+
+	spin_unlock_irqrestore(&hdmi->suspend_lock, flags);
+	tegra_nvhdcp_resume(hdmi->nvhdcp);
+}
+
+#ifdef CONFIG_SWITCH
+static ssize_t underscan_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct tegra_dc_hdmi_data *hdmi =
+			container_of(dev_get_drvdata(dev), struct tegra_dc_hdmi_data, hpd_switch);
+
+	if (hdmi->edid)
+		return sprintf(buf, "%d\n", tegra_edid_underscan_supported(hdmi->edid));
+	else
+		return 0;
+}
+
+static DEVICE_ATTR(underscan, S_IRUGO | S_IWUSR, underscan_show, NULL);
+#endif
+
+static int tegra_dc_hdmi_init(struct tegra_dc *dc)
+{
+	struct tegra_dc_hdmi_data *hdmi;
+	struct resource *res;
+	struct resource *base_res;
+#ifdef CONFIG_SWITCH
+	int ret;
+#endif
+	void __iomem *base;
+	struct clk *clk = NULL;
+	struct clk *disp1_clk = NULL;
+	struct clk *disp2_clk = NULL;
+	int err;
+
+	hdmi = kzalloc(sizeof(*hdmi), GFP_KERNEL);
+	if (!hdmi)
+		return -ENOMEM;
+
+	res = nvhost_get_resource_byname(dc->ndev, IORESOURCE_MEM, "hdmi_regs");
+	if (!res) {
+		dev_err(&dc->ndev->dev, "hdmi: no mem resource\n");
+		err = -ENOENT;
+		goto err_free_hdmi;
+	}
+
+	base_res = request_mem_region(res->start, resource_size(res), dc->ndev->name);
+	if (!base_res) {
+		dev_err(&dc->ndev->dev, "hdmi: request_mem_region failed\n");
+		err = -EBUSY;
+		goto err_free_hdmi;
+	}
+
+	base = ioremap(res->start, resource_size(res));
+	if (!base) {
+		dev_err(&dc->ndev->dev, "hdmi: registers can't be mapped\n");
+		err = -EBUSY;
+		goto err_release_resource_reg;
+	}
+
+	clk = clk_get(&dc->ndev->dev, "hdmi");
+	if (IS_ERR_OR_NULL(clk)) {
+		dev_err(&dc->ndev->dev, "hdmi: can't get clock\n");
+		err = -ENOENT;
+		goto err_iounmap_reg;
+	}
+
+	disp1_clk = clk_get_sys("tegradc.0", NULL);
+	if (IS_ERR_OR_NULL(disp1_clk)) {
+		dev_err(&dc->ndev->dev, "hdmi: can't disp1 clock\n");
+		err = -ENOENT;
+		goto err_put_clock;
+	}
+
+	disp2_clk = clk_get_sys("tegradc.1", NULL);
+	if (IS_ERR_OR_NULL(disp2_clk)) {
+		dev_err(&dc->ndev->dev, "hdmi: can't disp2 clock\n");
+		err = -ENOENT;
+		goto err_put_clock;
+	}
+
+#if !defined(CONFIG_ARCH_TEGRA_2x_SOC)
+	hdmi->hda_clk = clk_get_sys("tegra30-hda", "hda");
+	if (IS_ERR_OR_NULL(hdmi->hda_clk)) {
+		dev_err(&dc->ndev->dev, "hdmi: can't get hda clock\n");
+		err = -ENOENT;
+		goto err_put_clock;
+	}
+
+	hdmi->hda2codec_clk = clk_get_sys("tegra30-hda", "hda2codec");
+	if (IS_ERR_OR_NULL(hdmi->hda2codec_clk)) {
+		dev_err(&dc->ndev->dev, "hdmi: can't get hda2codec clock\n");
+		err = -ENOENT;
+		goto err_put_clock;
+	}
+
+	hdmi->hda2hdmi_clk = clk_get_sys("tegra30-hda", "hda2hdmi");
+	if (IS_ERR_OR_NULL(hdmi->hda2hdmi_clk)) {
+		dev_err(&dc->ndev->dev, "hdmi: can't get hda2hdmi clock\n");
+		err = -ENOENT;
+		goto err_put_clock;
+	}
+#endif
+
+	/* TODO: support non-hotplug */
+	if (request_irq(gpio_to_irq(dc->out->hotplug_gpio), tegra_dc_hdmi_irq,
+			IRQF_DISABLED | IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+			dev_name(&dc->ndev->dev), dc)) {
+		dev_err(&dc->ndev->dev, "hdmi: request_irq %d failed\n",
+			gpio_to_irq(dc->out->hotplug_gpio));
+		err = -EBUSY;
+		goto err_put_clock;
+	}
+
+	hdmi->edid = tegra_edid_create(dc->out->dcc_bus);
+	if (IS_ERR_OR_NULL(hdmi->edid)) {
+		dev_err(&dc->ndev->dev, "hdmi: can't create edid\n");
+		err = PTR_ERR(hdmi->edid);
+		goto err_free_irq;
+	}
+
+#ifdef CONFIG_TEGRA_NVHDCP
+	hdmi->nvhdcp = tegra_nvhdcp_create(hdmi, dc->ndev->id,
+			dc->out->dcc_bus);
+	if (IS_ERR_OR_NULL(hdmi->nvhdcp)) {
+		dev_err(&dc->ndev->dev, "hdmi: can't create nvhdcp\n");
+		err = PTR_ERR(hdmi->nvhdcp);
+		goto err_edid_destroy;
+	}
+#else
+	hdmi->nvhdcp = NULL;
+#endif
+
+	INIT_DELAYED_WORK(&hdmi->work, tegra_dc_hdmi_detect_worker);
+
+	hdmi->dc = dc;
+	hdmi->base = base;
+	hdmi->base_res = base_res;
+	hdmi->clk = clk;
+	hdmi->disp1_clk = disp1_clk;
+	hdmi->disp2_clk = disp2_clk;
+	hdmi->suspended = false;
+	hdmi->eld_retrieved= false;
+	hdmi->clk_enabled = false;
+	hdmi->audio_freq = 44100;
+	hdmi->audio_source = AUTO;
+	spin_lock_init(&hdmi->suspend_lock);
+
+#ifdef CONFIG_SWITCH
+	hdmi->hpd_switch.name = "hdmi";
+	ret = switch_dev_register(&hdmi->hpd_switch);
+
+	if (!ret)
+		ret = device_create_file(hdmi->hpd_switch.dev,
+			&dev_attr_underscan);
+	BUG_ON(ret != 0);
+#endif
+
+	dc->out->depth = 24;
+
+	tegra_dc_set_outdata(dc, hdmi);
+
+	dc_hdmi = hdmi;
+	/* boards can select default content protection policy */
+	if (dc->out->flags & TEGRA_DC_OUT_NVHDCP_POLICY_ON_DEMAND)
+		tegra_nvhdcp_set_policy(hdmi->nvhdcp,
+			TEGRA_NVHDCP_POLICY_ON_DEMAND);
+	else
+		tegra_nvhdcp_set_policy(hdmi->nvhdcp,
+			TEGRA_NVHDCP_POLICY_ALWAYS_ON);
+
+	tegra_dc_hdmi_debug_create(hdmi);
+
+	return 0;
+
+#ifdef CONFIG_TEGRA_NVHDCP
+err_edid_destroy:
+	tegra_edid_destroy(hdmi->edid);
+#endif
+err_free_irq:
+	free_irq(gpio_to_irq(dc->out->hotplug_gpio), dc);
+err_put_clock:
+#if !defined(CONFIG_ARCH_TEGRA_2x_SOC)
+	if (!IS_ERR_OR_NULL(hdmi->hda2hdmi_clk))
+		clk_put(hdmi->hda2hdmi_clk);
+	if (!IS_ERR_OR_NULL(hdmi->hda2codec_clk))
+		clk_put(hdmi->hda2codec_clk);
+	if (!IS_ERR_OR_NULL(hdmi->hda_clk))
+		clk_put(hdmi->hda_clk);
+#endif
+	if (!IS_ERR_OR_NULL(disp2_clk))
+		clk_put(disp2_clk);
+	if (!IS_ERR_OR_NULL(disp1_clk))
+		clk_put(disp1_clk);
+	if (!IS_ERR_OR_NULL(clk))
+		clk_put(clk);
+err_iounmap_reg:
+	iounmap(base);
+err_release_resource_reg:
+	release_resource(base_res);
+err_free_hdmi:
+	kfree(hdmi);
+	return err;
+}
+
+static void tegra_dc_hdmi_destroy(struct tegra_dc *dc)
+{
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+
+	free_irq(gpio_to_irq(dc->out->hotplug_gpio), dc);
+	cancel_delayed_work_sync(&hdmi->work);
+#ifdef CONFIG_SWITCH
+	switch_dev_unregister(&hdmi->hpd_switch);
+#endif
+	iounmap(hdmi->base);
+	release_resource(hdmi->base_res);
+#if !defined(CONFIG_ARCH_TEGRA_2x_SOC)
+	clk_put(hdmi->hda2hdmi_clk);
+	clk_put(hdmi->hda2codec_clk);
+	clk_put(hdmi->hda_clk);
+#endif
+	clk_put(hdmi->clk);
+	clk_put(hdmi->disp1_clk);
+	clk_put(hdmi->disp2_clk);
+	tegra_edid_destroy(hdmi->edid);
+	tegra_nvhdcp_destroy(hdmi->nvhdcp);
+
+	kfree(hdmi);
+
+}
+
+static void tegra_dc_hdmi_setup_audio_fs_tables(struct tegra_dc *dc)
+{
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+	int i;
+	unsigned freqs[] = {
+		32000,
+		44100,
+		48000,
+		88200,
+		96000,
+		176400,
+		192000,
+        };
+
+	for (i = 0; i < ARRAY_SIZE(freqs); i++) {
+		unsigned f = freqs[i];
+		unsigned eight_half;
+		unsigned delta;;
+
+		if (f > 96000)
+			delta = 2;
+		else if (f > 48000)
+			delta = 6;
+		else
+			delta = 9;
+
+		eight_half = (8 * HDMI_AUDIOCLK_FREQ) / (f * 128);
+		tegra_hdmi_writel(hdmi, AUDIO_FS_LOW(eight_half - delta) |
+				  AUDIO_FS_HIGH(eight_half + delta),
+				  HDMI_NV_PDISP_AUDIO_FS(i));
+	}
+}
+
+#if !defined(CONFIG_ARCH_TEGRA_2x_SOC)
+static void tegra_dc_hdmi_setup_eld_buff(struct tegra_dc *dc)
+{
+	int i;
+	int j;
+	u8 tmp;
+
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+
+	/* program ELD stuff */
+	for (i = 0; i < HDMI_ELD_MONITOR_NAME_INDEX; i++) {
+		switch (i) {
+		case HDMI_ELD_VER_INDEX:
+			tmp = (hdmi->eld.eld_ver << 3);
+			tegra_hdmi_writel(hdmi, (i << 8) | tmp,
+				  HDMI_NV_PDISP_SOR_AUDIO_HDA_ELD_BUFWR_0);
+			break;
+		case HDMI_ELD_BASELINE_LEN_INDEX:
+			break;
+		case HDMI_ELD_CEA_VER_MNL_INDEX:
+			tmp = (hdmi->eld.cea_edid_ver << 5);
+			tmp |= (hdmi->eld.mnl & 0x1f);
+			tegra_hdmi_writel(hdmi, (i << 8) | tmp,
+					  HDMI_NV_PDISP_SOR_AUDIO_HDA_ELD_BUFWR_0);
+			break;
+		case HDMI_ELD_SAD_CNT_CON_TYP_SAI_HDCP_INDEX:
+			tmp = (hdmi->eld.sad_count << 4);
+			tmp |= (hdmi->eld.conn_type & 0xC);
+			tmp |= (hdmi->eld.support_ai & 0x2);
+			tmp |= (hdmi->eld.support_hdcp & 0x1);
+			tegra_hdmi_writel(hdmi, (i << 8) | tmp,
+					  HDMI_NV_PDISP_SOR_AUDIO_HDA_ELD_BUFWR_0);
+			break;
+		case HDMI_ELD_AUD_SYNC_DELAY_INDEX:
+			tegra_hdmi_writel(hdmi, (i << 8) | (hdmi->eld.aud_synch_delay),
+					  HDMI_NV_PDISP_SOR_AUDIO_HDA_ELD_BUFWR_0);
+			break;
+		case HDMI_ELD_SPK_ALLOC_INDEX:
+			tegra_hdmi_writel(hdmi, (i << 8) | (hdmi->eld.spk_alloc),
+					  HDMI_NV_PDISP_SOR_AUDIO_HDA_ELD_BUFWR_0);
+			break;
+		case HDMI_ELD_PORT_ID_INDEX:
+			for (j = 0; j < 8;j++) {
+				tegra_hdmi_writel(hdmi, ((i +j) << 8) | (hdmi->eld.port_id[j]),
+					  HDMI_NV_PDISP_SOR_AUDIO_HDA_ELD_BUFWR_0);
+			}
+			break;
+		case HDMI_ELD_MANF_NAME_INDEX:
+			for (j = 0; j < 2;j++) {
+				tegra_hdmi_writel(hdmi, ((i +j) << 8) | (hdmi->eld.manufacture_id[j]),
+					  HDMI_NV_PDISP_SOR_AUDIO_HDA_ELD_BUFWR_0);
+			}
+			break;
+		case HDMI_ELD_PRODUCT_CODE_INDEX:
+			for (j = 0; j < 2;j++) {
+				tegra_hdmi_writel(hdmi, ((i +j) << 8) | (hdmi->eld.product_id[j]),
+					  HDMI_NV_PDISP_SOR_AUDIO_HDA_ELD_BUFWR_0);
+			}
+			break;
+		}
+	}
+	for (j = 0; j < hdmi->eld.mnl;j++) {
+		tegra_hdmi_writel(hdmi, ((j + HDMI_ELD_MONITOR_NAME_INDEX) << 8) |
+				  (hdmi->eld.monitor_name[j]),
+				  HDMI_NV_PDISP_SOR_AUDIO_HDA_ELD_BUFWR_0);
+	}
+	for (j = 0; j < hdmi->eld.sad_count;j++) {
+		tegra_hdmi_writel(hdmi, ((j + HDMI_ELD_MONITOR_NAME_INDEX + hdmi->eld.mnl) << 8) |
+				  (hdmi->eld.sad[j]),
+				  HDMI_NV_PDISP_SOR_AUDIO_HDA_ELD_BUFWR_0);
+	}
+		/* set presence andvalid bit  */
+	tegra_hdmi_writel(hdmi, 3, HDMI_NV_PDISP_SOR_AUDIO_HDA_PRESENSE_0);
+}
+#endif
+
+static int tegra_dc_hdmi_setup_audio(struct tegra_dc *dc, unsigned audio_freq,
+					unsigned audio_source)
+{
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+	const struct tegra_hdmi_audio_config *config;
+	unsigned long audio_n;
+#if !defined(CONFIG_ARCH_TEGRA_2x_SOC)
+	unsigned long reg_addr = 0;
+#endif
+	unsigned a_source = AUDIO_CNTRL0_SOURCE_SELECT_AUTO;
+
+	if (HDA == audio_source)
+		a_source = AUDIO_CNTRL0_SOURCE_SELECT_HDAL;
+	else if (SPDIF == audio_source)
+		a_source = AUDIO_CNTRL0_SOURCE_SELECT_SPDIF;
+
+#if !defined(CONFIG_ARCH_TEGRA_2x_SOC)
+	if (hdmi->audio_inject_null)
+		a_source |= AUDIO_CNTRL0_INJECT_NULLSMPL;
+
+	tegra_hdmi_writel(hdmi,a_source,
+			  HDMI_NV_PDISP_SOR_AUDIO_CNTRL0_0);
+	tegra_hdmi_writel(hdmi,
+			  AUDIO_CNTRL0_ERROR_TOLERANCE(6) |
+			  AUDIO_CNTRL0_FRAMES_PER_BLOCK(0xc0),
+			  HDMI_NV_PDISP_AUDIO_CNTRL0);
+#else
+	tegra_hdmi_writel(hdmi,
+			  AUDIO_CNTRL0_ERROR_TOLERANCE(6) |
+			  AUDIO_CNTRL0_FRAMES_PER_BLOCK(0xc0) |
+			  a_source,
+			  HDMI_NV_PDISP_AUDIO_CNTRL0);
+#endif
+	config = tegra_hdmi_get_audio_config(audio_freq, dc->mode.pclk);
+	if (!config) {
+		dev_err(&dc->ndev->dev,
+			"hdmi: can't set audio to %d at %d pix_clock",
+			audio_freq, dc->mode.pclk);
+		return -EINVAL;
+	}
+
+	tegra_hdmi_writel(hdmi, 0, HDMI_NV_PDISP_HDMI_ACR_CTRL);
+
+	audio_n = AUDIO_N_RESETF | AUDIO_N_GENERATE_ALTERNALTE |
+		AUDIO_N_VALUE(config->n - 1);
+	tegra_hdmi_writel(hdmi, audio_n, HDMI_NV_PDISP_AUDIO_N);
+
+	tegra_hdmi_writel(hdmi, ACR_SUBPACK_N(config->n) | ACR_ENABLE,
+			  HDMI_NV_PDISP_HDMI_ACR_0441_SUBPACK_HIGH);
+
+	tegra_hdmi_writel(hdmi, ACR_SUBPACK_CTS(config->cts),
+			  HDMI_NV_PDISP_HDMI_ACR_0441_SUBPACK_LOW);
+
+	tegra_hdmi_writel(hdmi, SPARE_HW_CTS | SPARE_FORCE_SW_CTS |
+			  SPARE_CTS_RESET_VAL(1),
+			  HDMI_NV_PDISP_HDMI_SPARE);
+
+	audio_n &= ~AUDIO_N_RESETF;
+	tegra_hdmi_writel(hdmi, audio_n, HDMI_NV_PDISP_AUDIO_N);
+
+#if !defined(CONFIG_ARCH_TEGRA_2x_SOC)
+	switch (audio_freq) {
+	case AUDIO_FREQ_32K:
+		reg_addr = HDMI_NV_PDISP_SOR_AUDIO_AVAL_0320_0;
+		break;
+	case AUDIO_FREQ_44_1K:
+		reg_addr = HDMI_NV_PDISP_SOR_AUDIO_AVAL_0441_0;
+		break;
+	case AUDIO_FREQ_48K:
+		reg_addr = HDMI_NV_PDISP_SOR_AUDIO_AVAL_0480_0;
+		break;
+	case AUDIO_FREQ_88_2K:
+		reg_addr = HDMI_NV_PDISP_SOR_AUDIO_AVAL_0882_0;
+		break;
+	case AUDIO_FREQ_96K:
+		reg_addr = HDMI_NV_PDISP_SOR_AUDIO_AVAL_0960_0;
+		break;
+	case AUDIO_FREQ_176_4K:
+		reg_addr = HDMI_NV_PDISP_SOR_AUDIO_AVAL_1764_0;
+		break;
+	case AUDIO_FREQ_192K:
+		reg_addr = HDMI_NV_PDISP_SOR_AUDIO_AVAL_1920_0;
+		break;
+	}
+
+	tegra_hdmi_writel(hdmi, config->aval, reg_addr);
+#endif
+	tegra_dc_hdmi_setup_audio_fs_tables(dc);
+
+	return 0;
+}
+
+int tegra_hdmi_setup_audio_freq_source(unsigned audio_freq, unsigned audio_source)
+{
+	struct tegra_dc_hdmi_data *hdmi = dc_hdmi;
+
+	if (!hdmi)
+		return -EAGAIN;
+
+	/* check for know freq */
+	if (AUDIO_FREQ_32K == audio_freq ||
+		AUDIO_FREQ_44_1K== audio_freq ||
+		AUDIO_FREQ_48K== audio_freq ||
+		AUDIO_FREQ_88_2K== audio_freq ||
+		AUDIO_FREQ_96K== audio_freq ||
+		AUDIO_FREQ_176_4K== audio_freq ||
+		AUDIO_FREQ_192K== audio_freq) {
+		/* If we can program HDMI, then proceed */
+		if (hdmi->clk_enabled)
+			tegra_dc_hdmi_setup_audio(hdmi->dc, audio_freq,audio_source);
+
+		/* Store it for using it in enable */
+		hdmi->audio_freq = audio_freq;
+		hdmi->audio_source = audio_source;
+	}
+	else
+		return -EINVAL;
+
+	return 0;
+}
+EXPORT_SYMBOL(tegra_hdmi_setup_audio_freq_source);
+
+#if !defined(CONFIG_ARCH_TEGRA_2x_SOC)
+int tegra_hdmi_audio_null_sample_inject(bool on)
+{
+	struct tegra_dc_hdmi_data *hdmi = dc_hdmi;
+	unsigned int val = 0;
+
+	if (!hdmi)
+		return -EAGAIN;
+
+	if (hdmi->audio_inject_null != on) {
+		hdmi->audio_inject_null = on;
+		if (hdmi->clk_enabled) {
+			val = tegra_hdmi_readl(hdmi,
+				HDMI_NV_PDISP_SOR_AUDIO_CNTRL0_0);
+			val &= ~AUDIO_CNTRL0_INJECT_NULLSMPL;
+			if (on)
+				val |= AUDIO_CNTRL0_INJECT_NULLSMPL;
+			tegra_hdmi_writel(hdmi,val,
+				HDMI_NV_PDISP_SOR_AUDIO_CNTRL0_0);
+		}
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(tegra_hdmi_audio_null_sample_inject);
+
+int tegra_hdmi_setup_hda_presence()
+{
+	struct tegra_dc_hdmi_data *hdmi = dc_hdmi;
+
+	if (!hdmi)
+		return -EAGAIN;
+
+	if (hdmi->clk_enabled && hdmi->eld_retrieved) {
+		/* If HDA_PRESENCE is already set reset it */
+		if (tegra_hdmi_readl(hdmi,
+				     HDMI_NV_PDISP_SOR_AUDIO_HDA_PRESENSE_0))
+			tegra_hdmi_writel(hdmi, 0,
+				     HDMI_NV_PDISP_SOR_AUDIO_HDA_PRESENSE_0);
+
+		tegra_dc_hdmi_setup_eld_buff(hdmi->dc);
+	}
+	else
+		return -ENODEV;
+
+	return 0;
+}
+EXPORT_SYMBOL(tegra_hdmi_setup_hda_presence);
+#endif
+
+static void tegra_dc_hdmi_write_infopack(struct tegra_dc *dc, int header_reg,
+					 u8 type, u8 version, void *data, int len)
+{
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+	u32 subpack[2];  /* extra byte for zero padding of subpack */
+	int i;
+	u8 csum;
+
+	/* first byte of data is the checksum */
+	csum = type + version + len - 1;
+	for (i = 1; i < len; i++)
+		csum +=((u8 *)data)[i];
+	((u8 *)data)[0] = 0x100 - csum;
+
+	tegra_hdmi_writel(hdmi, INFOFRAME_HEADER_TYPE(type) |
+			  INFOFRAME_HEADER_VERSION(version) |
+			  INFOFRAME_HEADER_LEN(len - 1),
+			  header_reg);
+
+	/* The audio inforame only has one set of subpack registers.  The hdmi
+	 * block pads the rest of the data as per the spec so we have to fixup
+	 * the length before filling in the subpacks.
+	 */
+	if (header_reg == HDMI_NV_PDISP_HDMI_AUDIO_INFOFRAME_HEADER)
+		len = 6;
+
+	/* each subpack 7 bytes devided into:
+	 *   subpack_low - bytes 0 - 3
+	 *   subpack_high - bytes 4 - 6 (with byte 7 padded to 0x00)
+	 */
+	for (i = 0; i < len; i++) {
+		int subpack_idx = i % 7;
+
+		if (subpack_idx == 0)
+			memset(subpack, 0x0, sizeof(subpack));
+
+		((u8 *)subpack)[subpack_idx] = ((u8 *)data)[i];
+
+		if (subpack_idx == 6 || (i + 1 == len)) {
+			int reg = header_reg + 1 + (i / 7) * 2;
+
+			tegra_hdmi_writel(hdmi, subpack[0], reg);
+			tegra_hdmi_writel(hdmi, subpack[1], reg + 1);
+		}
+	}
+}
+
+static void tegra_dc_hdmi_setup_avi_infoframe(struct tegra_dc *dc, bool dvi)
+{
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+	struct hdmi_avi_infoframe avi;
+
+	if (dvi) {
+		tegra_hdmi_writel(hdmi, 0x0,
+				  HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_CTRL);
+		return;
+	}
+
+	memset(&avi, 0x0, sizeof(avi));
+
+	avi.r = HDMI_AVI_R_SAME;
+
+	if ((dc->mode.h_active == 720) && ((dc->mode.v_active == 480) || (dc->mode.v_active == 576)))
+		tegra_dc_writel(dc, 0x00101010, DC_DISP_BORDER_COLOR);
+	else
+		tegra_dc_writel(dc, 0x00000000, DC_DISP_BORDER_COLOR);
+
+	if (dc->mode.v_active == 480) {
+		if (dc->mode.h_active == 640) {
+			avi.m = HDMI_AVI_M_4_3;
+			avi.vic = 1;
+		} else {
+			avi.m = HDMI_AVI_M_16_9;
+			avi.vic = 3;
+		}
+	} else if (dc->mode.v_active == 576) {
+		/* CEC modes 17 and 18 differ only by the pysical size of the
+		 * screen so we have to calculation the physical aspect
+		 * ratio.  4 * 10 / 3  is 13
+		 */
+		if ((dc->out->h_size * 10) / dc->out->v_size > 14) {
+			avi.m = HDMI_AVI_M_16_9;
+			avi.vic = 18;
+		} else {
+			avi.m = HDMI_AVI_M_4_3;
+			avi.vic = 17;
+		}
+	} else if (dc->mode.v_active == 720 ||
+		(dc->mode.v_active == 1470 && dc->mode.stereo_mode)) {
+		/* VIC for both 720p and 720p 3D mode */
+		avi.m = HDMI_AVI_M_16_9;
+		if (dc->mode.h_front_porch == 110)
+			avi.vic = 4; /* 60 Hz */
+		else
+			avi.vic = 19; /* 50 Hz */
+	} else if (dc->mode.v_active == 1080 ||
+		(dc->mode.v_active == 2205 && dc->mode.stereo_mode)) {
+		/* VIC for both 1080p and 1080p 3D mode */
+		avi.m = HDMI_AVI_M_16_9;
+		if (dc->mode.h_front_porch == 88) {
+			if (dc->mode.pclk > 74250000)
+				avi.vic = 16; /* 60 Hz */
+			else
+				avi.vic = 34; /* 30 Hz */
+		} else if (dc->mode.h_front_porch == 528)
+			avi.vic = 31; /* 50 Hz */
+		else
+			avi.vic = 32; /* 24 Hz */
+	} else {
+		avi.m = HDMI_AVI_M_16_9;
+		avi.vic = 0;
+	}
+
+
+	tegra_dc_hdmi_write_infopack(dc, HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_HEADER,
+				     HDMI_INFOFRAME_TYPE_AVI,
+				     HDMI_AVI_VERSION,
+				     &avi, sizeof(avi));
+
+	tegra_hdmi_writel(hdmi, INFOFRAME_CTRL_ENABLE,
+			  HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_CTRL);
+}
+
+static void tegra_dc_hdmi_setup_stereo_infoframe(struct tegra_dc *dc)
+{
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+	struct hdmi_stereo_infoframe stereo;
+	u32 val;
+
+	if (!dc->mode.stereo_mode) {
+		val  = tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_HDMI_GENERIC_CTRL);
+		val &= ~GENERIC_CTRL_ENABLE;
+		tegra_hdmi_writel(hdmi, val, HDMI_NV_PDISP_HDMI_GENERIC_CTRL);
+		return;
+	}
+
+	memset(&stereo, 0x0, sizeof(stereo));
+
+	stereo.regid0 = 0x03;
+	stereo.regid1 = 0x0c;
+	stereo.regid2 = 0x00;
+	stereo.hdmi_video_format = 2; /* 3D_Structure present */
+#ifndef CONFIG_TEGRA_HDMI_74MHZ_LIMIT
+	stereo._3d_structure = 0; /* frame packing */
+#else
+	stereo._3d_structure = 8; /* side-by-side (half) */
+	stereo._3d_ext_data = 0; /* something which fits into 00XX bit req */
+#endif
+
+	tegra_dc_hdmi_write_infopack(dc, HDMI_NV_PDISP_HDMI_GENERIC_HEADER,
+					HDMI_INFOFRAME_TYPE_VENDOR,
+					HDMI_VENDOR_VERSION,
+					&stereo, 6);
+
+	val  = tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_HDMI_GENERIC_CTRL);
+	val |= GENERIC_CTRL_ENABLE;
+
+	tegra_hdmi_writel(hdmi, val, HDMI_NV_PDISP_HDMI_GENERIC_CTRL);
+}
+
+static void tegra_dc_hdmi_setup_audio_infoframe(struct tegra_dc *dc, bool dvi)
+{
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+	struct hdmi_audio_infoframe audio;
+
+	if (dvi) {
+		tegra_hdmi_writel(hdmi, 0x0,
+				  HDMI_NV_PDISP_HDMI_AUDIO_INFOFRAME_CTRL);
+		return;
+	}
+
+	memset(&audio, 0x0, sizeof(audio));
+
+	audio.cc = HDMI_AUDIO_CC_2;
+	tegra_dc_hdmi_write_infopack(dc, HDMI_NV_PDISP_HDMI_AUDIO_INFOFRAME_HEADER,
+				     HDMI_INFOFRAME_TYPE_AUDIO,
+				     HDMI_AUDIO_VERSION,
+				     &audio, sizeof(audio));
+
+	tegra_hdmi_writel(hdmi, INFOFRAME_CTRL_ENABLE,
+			  HDMI_NV_PDISP_HDMI_AUDIO_INFOFRAME_CTRL);
+}
+
+static void tegra_dc_hdmi_setup_tdms(struct tegra_dc_hdmi_data *hdmi,
+		const struct tdms_config *tc)
+{
+	tegra_hdmi_writel(hdmi, tc->pll0, HDMI_NV_PDISP_SOR_PLL0);
+	tegra_hdmi_writel(hdmi, tc->pll1, HDMI_NV_PDISP_SOR_PLL1);
+
+	tegra_hdmi_writel(hdmi, tc->pe_current, HDMI_NV_PDISP_PE_CURRENT);
+
+	tegra_hdmi_writel(hdmi,
+			  tc->drive_current | DRIVE_CURRENT_FUSE_OVERRIDE,
+			  HDMI_NV_PDISP_SOR_LANE_DRIVE_CURRENT);
+}
+
+static void tegra_dc_hdmi_enable(struct tegra_dc *dc)
+{
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+	int pulse_start;
+	int dispclk_div_8_2;
+	int retries;
+	int rekey;
+	int err;
+	unsigned long val;
+	unsigned i;
+	unsigned long oldrate;
+
+	/* enbale power, clocks, resets, etc. */
+
+	/* The upstream DC needs to be clocked for accesses to HDMI to not
+	 * hard lock the system.  Because we don't know if HDMI is conencted
+	 * to disp1 or disp2 we need to enable both until we set the DC mux.
+	 */
+	clk_enable(hdmi->disp1_clk);
+	clk_enable(hdmi->disp2_clk);
+
+#if !defined(CONFIG_ARCH_TEGRA_2x_SOC)
+	/* Enabling HDA clocks before asserting HDA PD and ELDV bits */
+	clk_enable(hdmi->hda_clk);
+	clk_enable(hdmi->hda2codec_clk);
+	clk_enable(hdmi->hda2hdmi_clk);
+#endif
+
+	/* back off multiplier before attaching to parent at new rate. */
+	oldrate = clk_get_rate(hdmi->clk);
+	clk_set_rate(hdmi->clk, oldrate / 2);
+
+	tegra_dc_setup_clk(dc, hdmi->clk);
+	clk_set_rate(hdmi->clk, dc->mode.pclk);
+
+	clk_enable(hdmi->clk);
+	tegra_periph_reset_assert(hdmi->clk);
+	mdelay(1);
+	tegra_periph_reset_deassert(hdmi->clk);
+
+	/* TODO: copy HDCP keys from KFUSE to HDMI */
+
+	/* Program display timing registers: handled by dc */
+
+	/* program HDMI registers and SOR sequencer */
+
+	tegra_dc_writel(dc, VSYNC_H_POSITION(1), DC_DISP_DISP_TIMING_OPTIONS);
+	tegra_dc_writel(dc, DITHER_CONTROL_DISABLE | BASE_COLOR_SIZE888,
+			DC_DISP_DISP_COLOR_CONTROL);
+
+	/* video_preamble uses h_pulse2 */
+	pulse_start = dc->mode.h_ref_to_sync + dc->mode.h_sync_width +
+		dc->mode.h_back_porch - 10;
+	tegra_dc_writel(dc, H_PULSE_2_ENABLE, DC_DISP_DISP_SIGNAL_OPTIONS0);
+	tegra_dc_writel(dc,
+			PULSE_MODE_NORMAL |
+			PULSE_POLARITY_HIGH |
+			PULSE_QUAL_VACTIVE |
+			PULSE_LAST_END_A,
+			DC_DISP_H_PULSE2_CONTROL);
+	tegra_dc_writel(dc, PULSE_START(pulse_start) | PULSE_END(pulse_start + 8),
+		  DC_DISP_H_PULSE2_POSITION_A);
+
+	tegra_hdmi_writel(hdmi,
+			  VSYNC_WINDOW_END(0x210) |
+			  VSYNC_WINDOW_START(0x200) |
+			  VSYNC_WINDOW_ENABLE,
+			  HDMI_NV_PDISP_HDMI_VSYNC_WINDOW);
+
+	if ((dc->mode.h_active == 720) && ((dc->mode.v_active == 480) || (dc->mode.v_active == 576)))
+		tegra_hdmi_writel(hdmi,
+				  (dc->ndev->id ? HDMI_SRC_DISPLAYB : HDMI_SRC_DISPLAYA) |
+				  ARM_VIDEO_RANGE_FULL,
+				  HDMI_NV_PDISP_INPUT_CONTROL);
+	else
+		tegra_hdmi_writel(hdmi,
+				  (dc->ndev->id ? HDMI_SRC_DISPLAYB : HDMI_SRC_DISPLAYA) |
+				  ARM_VIDEO_RANGE_LIMITED,
+				  HDMI_NV_PDISP_INPUT_CONTROL);
+
+	clk_disable(hdmi->disp1_clk);
+	clk_disable(hdmi->disp2_clk);
+
+	dispclk_div_8_2 = clk_get_rate(hdmi->clk) / 1000000 * 4;
+	tegra_hdmi_writel(hdmi,
+			  SOR_REFCLK_DIV_INT(dispclk_div_8_2 >> 2) |
+			  SOR_REFCLK_DIV_FRAC(dispclk_div_8_2),
+			  HDMI_NV_PDISP_SOR_REFCLK);
+
+	hdmi->clk_enabled = true;
+
+	if (!hdmi->dvi) {
+		err = tegra_dc_hdmi_setup_audio(dc, hdmi->audio_freq,
+			hdmi->audio_source);
+
+		if (err < 0)
+			hdmi->dvi = true;
+	}
+
+#if !defined(CONFIG_ARCH_TEGRA_2x_SOC)
+	if (hdmi->eld_retrieved)
+		tegra_dc_hdmi_setup_eld_buff(dc);
+#endif
+
+	rekey = HDMI_REKEY_DEFAULT;
+	val = HDMI_CTRL_REKEY(rekey);
+	val |= HDMI_CTRL_MAX_AC_PACKET((dc->mode.h_sync_width +
+					dc->mode.h_back_porch +
+					dc->mode.h_front_porch -
+					rekey - 18) / 32);
+	if (!hdmi->dvi)
+		val |= HDMI_CTRL_ENABLE;
+	tegra_hdmi_writel(hdmi, val, HDMI_NV_PDISP_HDMI_CTRL);
+
+	if (hdmi->dvi)
+		tegra_hdmi_writel(hdmi, 0x0,
+				  HDMI_NV_PDISP_HDMI_GENERIC_CTRL);
+	else
+		tegra_hdmi_writel(hdmi, GENERIC_CTRL_AUDIO,
+				  HDMI_NV_PDISP_HDMI_GENERIC_CTRL);
+
+	tegra_dc_hdmi_setup_avi_infoframe(dc, hdmi->dvi);
+	tegra_dc_hdmi_setup_audio_infoframe(dc, hdmi->dvi);
+	tegra_dc_hdmi_setup_stereo_infoframe(dc);
+
+	/* TMDS CONFIG */
+	for (i = 0; i < ARRAY_SIZE(tdms_config); i++) {
+		if (dc->mode.pclk <= tdms_config[i].pclk) {
+			tegra_dc_hdmi_setup_tdms(hdmi, &tdms_config[i]);
+			break;
+		}
+	}
+
+	tegra_hdmi_writel(hdmi,
+			  SOR_SEQ_CTL_PU_PC(0) |
+			  SOR_SEQ_PU_PC_ALT(0) |
+			  SOR_SEQ_PD_PC(8) |
+			  SOR_SEQ_PD_PC_ALT(8),
+			  HDMI_NV_PDISP_SOR_SEQ_CTL);
+
+	val = SOR_SEQ_INST_WAIT_TIME(1) |
+		SOR_SEQ_INST_WAIT_UNITS_VSYNC |
+		SOR_SEQ_INST_HALT |
+		SOR_SEQ_INST_PIN_A_LOW |
+		SOR_SEQ_INST_PIN_B_LOW |
+		SOR_SEQ_INST_DRIVE_PWM_OUT_LO;
+
+	tegra_hdmi_writel(hdmi, val, HDMI_NV_PDISP_SOR_SEQ_INST0);
+	tegra_hdmi_writel(hdmi, val, HDMI_NV_PDISP_SOR_SEQ_INST8);
+
+	val = 0x1c800;
+	val &= ~SOR_CSTM_ROTCLK(~0);
+	val |= SOR_CSTM_ROTCLK(2);
+	tegra_hdmi_writel(hdmi, val, HDMI_NV_PDISP_SOR_CSTM);
+
+
+	tegra_dc_writel(dc, DISP_CTRL_MODE_STOP, DC_CMD_DISPLAY_COMMAND);
+	tegra_dc_writel(dc, GENERAL_ACT_REQ << 8, DC_CMD_STATE_CONTROL);
+	tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
+
+
+	/* start SOR */
+	tegra_hdmi_writel(hdmi,
+			  SOR_PWR_NORMAL_STATE_PU |
+			  SOR_PWR_NORMAL_START_NORMAL |
+			  SOR_PWR_SAFE_STATE_PD |
+			  SOR_PWR_SETTING_NEW_TRIGGER,
+			  HDMI_NV_PDISP_SOR_PWR);
+	tegra_hdmi_writel(hdmi,
+			  SOR_PWR_NORMAL_STATE_PU |
+			  SOR_PWR_NORMAL_START_NORMAL |
+			  SOR_PWR_SAFE_STATE_PD |
+			  SOR_PWR_SETTING_NEW_DONE,
+			  HDMI_NV_PDISP_SOR_PWR);
+
+	retries = 1000;
+	do {
+		BUG_ON(--retries < 0);
+		val = tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_SOR_PWR);
+	} while (val & SOR_PWR_SETTING_NEW_PENDING);
+
+	val = SOR_STATE_ASY_CRCMODE_COMPLETE |
+		SOR_STATE_ASY_OWNER_HEAD0 |
+		SOR_STATE_ASY_SUBOWNER_BOTH |
+		SOR_STATE_ASY_PROTOCOL_SINGLE_TMDS_A |
+		SOR_STATE_ASY_DEPOL_POS;
+
+	if (dc->mode.flags & TEGRA_DC_MODE_FLAG_NEG_H_SYNC)
+		val |= SOR_STATE_ASY_HSYNCPOL_NEG;
+	else
+		val |= SOR_STATE_ASY_HSYNCPOL_POS;
+
+	if (dc->mode.flags & TEGRA_DC_MODE_FLAG_NEG_V_SYNC)
+		val |= SOR_STATE_ASY_VSYNCPOL_NEG;
+	else
+		val |= SOR_STATE_ASY_VSYNCPOL_POS;
+
+	tegra_hdmi_writel(hdmi, val, HDMI_NV_PDISP_SOR_STATE2);
+
+	val = SOR_STATE_ASY_HEAD_OPMODE_AWAKE | SOR_STATE_ASY_ORMODE_NORMAL;
+	tegra_hdmi_writel(hdmi, val, HDMI_NV_PDISP_SOR_STATE1);
+
+	tegra_hdmi_writel(hdmi, 0, HDMI_NV_PDISP_SOR_STATE0);
+	tegra_hdmi_writel(hdmi, SOR_STATE_UPDATE, HDMI_NV_PDISP_SOR_STATE0);
+	tegra_hdmi_writel(hdmi, val | SOR_STATE_ATTACHED,
+			  HDMI_NV_PDISP_SOR_STATE1);
+	tegra_hdmi_writel(hdmi, 0, HDMI_NV_PDISP_SOR_STATE0);
+
+	tegra_dc_writel(dc, HDMI_ENABLE, DC_DISP_DISP_WIN_OPTIONS);
+
+	tegra_dc_writel(dc, PW0_ENABLE | PW1_ENABLE | PW2_ENABLE | PW3_ENABLE |
+			PW4_ENABLE | PM0_ENABLE | PM1_ENABLE,
+			DC_CMD_DISPLAY_POWER_CONTROL);
+
+	tegra_dc_writel(dc, DISP_CTRL_MODE_C_DISPLAY, DC_CMD_DISPLAY_COMMAND);
+	tegra_dc_writel(dc, GENERAL_ACT_REQ << 8, DC_CMD_STATE_CONTROL);
+	tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
+
+	tegra_nvhdcp_set_plug(hdmi->nvhdcp, 1);
+}
+
+static void tegra_dc_hdmi_disable(struct tegra_dc *dc)
+{
+	struct tegra_dc_hdmi_data *hdmi = tegra_dc_get_outdata(dc);
+
+	tegra_nvhdcp_set_plug(hdmi->nvhdcp, 0);
+
+#if !defined(CONFIG_ARCH_TEGRA_2x_SOC)
+	tegra_hdmi_writel(hdmi, 0, HDMI_NV_PDISP_SOR_AUDIO_HDA_PRESENSE_0);
+	/* sleep 1ms before disabling clocks to ensure HDA gets the interrupt */
+	msleep(1);
+	clk_disable(hdmi->hda2hdmi_clk);
+	clk_disable(hdmi->hda2codec_clk);
+	clk_disable(hdmi->hda_clk);
+#endif
+	tegra_periph_reset_assert(hdmi->clk);
+	hdmi->clk_enabled = false;
+	clk_disable(hdmi->clk);
+	tegra_dvfs_set_rate(hdmi->clk, 0);
+}
+
+struct tegra_dc_out_ops tegra_dc_hdmi_ops = {
+	.init = tegra_dc_hdmi_init,
+	.destroy = tegra_dc_hdmi_destroy,
+	.enable = tegra_dc_hdmi_enable,
+	.disable = tegra_dc_hdmi_disable,
+	.detect = tegra_dc_hdmi_detect,
+	.suspend = tegra_dc_hdmi_suspend,
+	.resume = tegra_dc_hdmi_resume,
+	.mode_filter = tegra_dc_hdmi_mode_filter,
+};
+
+struct tegra_dc_edid *tegra_dc_get_edid(struct tegra_dc *dc)
+{
+	struct tegra_dc_hdmi_data *hdmi;
+
+	/* TODO: Support EDID on non-HDMI devices */
+	if (dc->out->type != TEGRA_DC_OUT_HDMI)
+		return ERR_PTR(-ENODEV);
+
+	hdmi = tegra_dc_get_outdata(dc);
+
+	return tegra_edid_get_data(hdmi->edid);
+}
+EXPORT_SYMBOL(tegra_dc_get_edid);
+
+void tegra_dc_put_edid(struct tegra_dc_edid *edid)
+{
+	tegra_edid_put_data(edid);
+}
+EXPORT_SYMBOL(tegra_dc_put_edid);
diff --git a/drivers/video/tegra/dc/hdmi.h b/drivers/video/tegra/dc/hdmi.h
new file mode 100644
index 000000000000..5b4c42a31ffa
--- /dev/null
+++ b/drivers/video/tegra/dc/hdmi.h
@@ -0,0 +1,222 @@
+/*
+ * drivers/video/tegra/dc/hdmi.h
+ *
+ * non-tegra specific HDMI declarations
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 __DRIVERS_VIDEO_TEGRA_DC_HDMI_H
+#define __DRIVERS_VIDEO_TEGRA_DC_HDMI_H
+
+#define HDMI_INFOFRAME_TYPE_VENDOR	0x81
+#define HDMI_INFOFRAME_TYPE_AVI		0x82
+#define HDMI_INFOFRAME_TYPE_SPD		0x83
+#define HDMI_INFOFRAME_TYPE_AUDIO	0x84
+#define HDMI_INFOFRAME_TYPE_MPEG_SRC	0x85
+#define HDMI_INFOFRAME_TYPE_NTSC_VBI	0x86
+
+/* all fields little endian */
+struct hdmi_avi_infoframe {
+	/* PB0 */
+	u8		csum;
+
+	/* PB1 */
+	unsigned	s:2;	/* scan information */
+	unsigned	b:2;	/* bar info data valid */
+	unsigned	a:1;	/* active info present */
+	unsigned	y:2;	/* RGB or YCbCr */
+	unsigned	res1:1;
+
+	/* PB2 */
+	unsigned	r:4;	/* active format aspect ratio */
+	unsigned	m:2;	/* picture aspect ratio */
+	unsigned	c:2;	/* colorimetry */
+
+	/* PB3 */
+	unsigned	sc:2;	/* scan information */
+	unsigned	q:2;	/* quantization range */
+	unsigned	ec:3;	/* extended colorimetry */
+	unsigned	itc:1;	/* it content */
+
+	/* PB4 */
+	unsigned	vic:7;	/* video format id code */
+	unsigned	res4:1;
+
+	/* PB5 */
+	unsigned	pr:4;	/* pixel repetition factor */
+	unsigned	cn:2;	/* it content type*/
+	unsigned	yq:2;	/* ycc quantization range */
+
+	/* PB6-7 */
+	u16		top_bar_end_line;
+
+	/* PB8-9 */
+	u16		bot_bar_start_line;
+
+	/* PB10-11 */
+	u16		left_bar_end_pixel;
+
+	/* PB12-13 */
+	u16		right_bar_start_pixel;
+} __attribute__((packed));
+
+#define HDMI_AVI_VERSION		0x02
+
+#define HDMI_AVI_Y_RGB			0x0
+#define HDMI_AVI_Y_YCBCR_422		0x1
+#define HDMI_AVI_Y_YCBCR_444		0x2
+
+#define HDMI_AVI_B_VERT			0x1
+#define HDMI_AVI_B_HORIZ		0x2
+
+#define HDMI_AVI_S_NONE			0x0
+#define HDMI_AVI_S_OVERSCAN		0x1
+#define HDMI_AVI_S_UNDERSCAN		0x2
+
+#define HDMI_AVI_C_NONE			0x0
+#define HDMI_AVI_C_SMPTE		0x1
+#define HDMI_AVI_C_ITU_R		0x2
+#define HDMI_AVI_C_EXTENDED		0x4
+
+#define HDMI_AVI_M_4_3			0x1
+#define HDMI_AVI_M_16_9			0x2
+
+#define HDMI_AVI_R_SAME			0x8
+#define HDMI_AVI_R_4_3_CENTER		0x9
+#define HDMI_AVI_R_16_9_CENTER		0xa
+#define HDMI_AVI_R_14_9_CENTER		0xb
+
+/* all fields little endian */
+struct hdmi_audio_infoframe {
+	/* PB0 */
+	u8		csum;
+
+	/* PB1 */
+	unsigned	cc:3;		/* channel count */
+	unsigned	res1:1;
+	unsigned	ct:4;		/* coding type */
+
+	/* PB2 */
+	unsigned	ss:2;		/* sample size */
+	unsigned	sf:3;		/* sample frequency */
+	unsigned	res2:3;
+
+	/* PB3 */
+	unsigned	cxt:5;		/* coding extention type */
+	unsigned	res3:3;
+
+	/* PB4 */
+	u8		ca;		/* channel/speaker allocation */
+
+	/* PB5 */
+	unsigned	res5:3;
+	unsigned	lsv:4;		/* level shift value */
+	unsigned	dm_inh:1;	/* downmix inhibit */
+
+	/* PB6-10 reserved */
+	u8		res6;
+	u8		res7;
+	u8		res8;
+	u8		res9;
+	u8		res10;
+} __attribute__((packed));
+
+#define HDMI_AUDIO_VERSION		0x01
+
+#define HDMI_AUDIO_CC_STREAM		0x0 /* specified by audio stream */
+#define HDMI_AUDIO_CC_2			0x1
+#define HDMI_AUDIO_CC_3			0x2
+#define HDMI_AUDIO_CC_4			0x3
+#define HDMI_AUDIO_CC_5			0x4
+#define HDMI_AUDIO_CC_6			0x5
+#define HDMI_AUDIO_CC_7			0x6
+#define HDMI_AUDIO_CC_8			0x7
+
+#define HDMI_AUDIO_CT_STREAM		0x0 /* specified by audio stream */
+#define HDMI_AUDIO_CT_PCM		0x1
+#define HDMI_AUDIO_CT_AC3		0x2
+#define HDMI_AUDIO_CT_MPEG1		0x3
+#define HDMI_AUDIO_CT_MP3		0x4
+#define HDMI_AUDIO_CT_MPEG2		0x5
+#define HDMI_AUDIO_CT_AAC_LC		0x6
+#define HDMI_AUDIO_CT_DTS		0x7
+#define HDMI_AUDIO_CT_ATRAC		0x8
+#define HDMI_AUDIO_CT_DSD		0x9
+#define HDMI_AUDIO_CT_E_AC3		0xa
+#define HDMI_AUDIO_CT_DTS_HD		0xb
+#define HDMI_AUDIO_CT_MLP		0xc
+#define HDMI_AUDIO_CT_DST		0xd
+#define HDMI_AUDIO_CT_WMA_PRO		0xe
+#define HDMI_AUDIO_CT_CXT		0xf
+
+#define HDMI_AUDIO_SF_STREAM		0x0 /* specified by audio stream */
+#define HDMI_AUIDO_SF_32K		0x1
+#define HDMI_AUDIO_SF_44_1K		0x2
+#define HDMI_AUDIO_SF_48K		0x3
+#define HDMI_AUDIO_SF_88_2K		0x4
+#define HDMI_AUDIO_SF_96K		0x5
+#define HDMI_AUDIO_SF_176_4K		0x6
+#define HDMI_AUDIO_SF_192K		0x7
+
+#define HDMI_AUDIO_SS_STREAM		0x0 /* specified by audio stream */
+#define HDMI_AUDIO_SS_16BIT		0x1
+#define HDMI_AUDIO_SS_20BIT		0x2
+#define HDMI_AUDIO_SS_24BIT		0x3
+
+#define HDMI_AUDIO_CXT_CT		0x0 /* refer to coding in CT */
+#define HDMI_AUDIO_CXT_HE_AAC		0x1
+#define HDMI_AUDIO_CXT_HE_AAC_V2	0x2
+#define HDMI_AUDIO_CXT_MPEG_SURROUND	0x3
+
+/* all fields little endian */
+struct hdmi_stereo_infoframe {
+	/* PB0 */
+	u8		csum;
+
+	/* PB1 */
+	u8		regid0;
+
+	/* PB2 */
+	u8		regid1;
+
+	/* PB3 */
+	u8		regid2;
+
+	/* PB4 */
+	unsigned	res1:5;
+	unsigned	hdmi_video_format:3;
+
+	/* PB5 */
+	unsigned	res2:4;
+	unsigned	_3d_structure:4;
+
+	/* PB6*/
+	unsigned	res3:4;
+	unsigned	_3d_ext_data:4;
+
+} __attribute__((packed));
+
+#define HDMI_VENDOR_VERSION 0x01
+
+struct tegra_dc_hdmi_data;
+
+unsigned long tegra_hdmi_readl(struct tegra_dc_hdmi_data *hdmi,
+				unsigned long reg);
+void tegra_hdmi_writel(struct tegra_dc_hdmi_data *hdmi,
+				unsigned long val, unsigned long reg);
+
+#endif
diff --git a/drivers/video/tegra/dc/hdmi_reg.h b/drivers/video/tegra/dc/hdmi_reg.h
new file mode 100644
index 000000000000..4a5fdcb2aaa6
--- /dev/null
+++ b/drivers/video/tegra/dc/hdmi_reg.h
@@ -0,0 +1,480 @@
+/*
+ * drivers/video/tegra/dc/hdmi_reg.h
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 __DRIVERS_VIDEO_TEGRA_DC_HDMI_REG_H
+#define __DRIVERS_VIDEO_TEGRA_DC_HDMI_REG_H
+
+#define HDMI_CTXSW						0x00
+#define HDMI_NV_PDISP_SOR_STATE0				0x01
+#define  SOR_STATE_UPDATE			(1 << 0)
+
+#define HDMI_NV_PDISP_SOR_STATE1				0x02
+#define  SOR_STATE_ASY_HEAD_OPMODE_SLEEP	(0 << 0)
+#define  SOR_STATE_ASY_HEAD_OPMODE_SNOOSE	(1 << 0)
+#define  SOR_STATE_ASY_HEAD_OPMODE_AWAKE	(2 << 0)
+#define  SOR_STATE_ASY_ORMODE_SAFE		(0 << 2)
+#define  SOR_STATE_ASY_ORMODE_NORMAL		(1 << 2)
+#define  SOR_STATE_ATTACHED			(1 << 3)
+#define  SOR_STATE_ARM_SHOW_VGA			(1 << 4)
+
+#define HDMI_NV_PDISP_SOR_STATE2				0x03
+#define  SOR_STATE_ASY_OWNER_NONE		(0 << 0)
+#define  SOR_STATE_ASY_OWNER_HEAD0		(1 << 0)
+#define  SOR_STATE_ASY_SUBOWNER_NONE		(0 << 4)
+#define  SOR_STATE_ASY_SUBOWNER_SUBHEAD0	(1 << 4)
+#define  SOR_STATE_ASY_SUBOWNER_SUBHEAD1	(2 << 4)
+#define  SOR_STATE_ASY_SUBOWNER_BOTH		(3 << 4)
+#define  SOR_STATE_ASY_CRCMODE_ACTIVE		(0 << 6)
+#define  SOR_STATE_ASY_CRCMODE_COMPLETE		(1 << 6)
+#define  SOR_STATE_ASY_CRCMODE_NON_ACTIVE	(2 << 6)
+#define  SOR_STATE_ASY_PROTOCOL_SINGLE_TMDS_A	(1 << 8)
+#define  SOR_STATE_ASY_PROTOCOL_CUSTOM		(15 << 8)
+#define  SOR_STATE_ASY_HSYNCPOL_POS		(0 << 12)
+#define  SOR_STATE_ASY_HSYNCPOL_NEG		(1 << 12)
+#define  SOR_STATE_ASY_VSYNCPOL_POS		(0 << 13)
+#define  SOR_STATE_ASY_VSYNCPOL_NEG		(1 << 13)
+#define  SOR_STATE_ASY_DEPOL_POS		(0 << 14)
+#define  SOR_STATE_ASY_DEPOL_NEG		(1 << 14)
+
+#define HDMI_NV_PDISP_RG_HDCP_AN_MSB				0x04
+#define HDMI_NV_PDISP_RG_HDCP_AN_LSB				0x05
+#define HDMI_NV_PDISP_RG_HDCP_CN_MSB				0x06
+#define HDMI_NV_PDISP_RG_HDCP_CN_LSB				0x07
+#define HDMI_NV_PDISP_RG_HDCP_AKSV_MSB				0x08
+#define HDMI_NV_PDISP_RG_HDCP_AKSV_LSB				0x09
+#define HDMI_NV_PDISP_RG_HDCP_BKSV_MSB				0x0a
+#define  REPEATER				(1 << 31)
+#define HDMI_NV_PDISP_RG_HDCP_BKSV_LSB				0x0b
+#define HDMI_NV_PDISP_RG_HDCP_CKSV_MSB				0x0c
+#define HDMI_NV_PDISP_RG_HDCP_CKSV_LSB				0x0d
+#define HDMI_NV_PDISP_RG_HDCP_DKSV_MSB				0x0e
+#define HDMI_NV_PDISP_RG_HDCP_DKSV_LSB				0x0f
+#define HDMI_NV_PDISP_RG_HDCP_CTRL				0x10
+#define  HDCP_RUN_YES				(1 << 0)
+#define  CRYPT_ENABLED				(1 << 1)
+#define  ONEONE_ENABLED				(1 << 3)
+#define  AN_VALID				(1 << 8)
+#define  R0_VALID				(1 << 9)
+#define  SPRIME_VALID				(1 << 10)
+#define  MPRIME_VALID				(1 << 11)
+#define  SROM_ERR				(1 << 13)
+#define HDMI_NV_PDISP_RG_HDCP_CMODE				0x11
+#define  TMDS0_LINK0				(1 << 4)
+#define  READ_S					(1 << 0)
+#define  READ_M					(2 << 0)
+#define HDMI_NV_PDISP_RG_HDCP_MPRIME_MSB			0x12
+#define HDMI_NV_PDISP_RG_HDCP_MPRIME_LSB			0x13
+#define HDMI_NV_PDISP_RG_HDCP_SPRIME_MSB			0x14
+#define  STATUS_CS				(1 << 6)
+#define HDMI_NV_PDISP_RG_HDCP_SPRIME_LSB2			0x15
+#define HDMI_NV_PDISP_RG_HDCP_SPRIME_LSB1			0x16
+#define HDMI_NV_PDISP_RG_HDCP_RI				0x17
+#define HDMI_NV_PDISP_RG_HDCP_CS_MSB				0x18
+#define HDMI_NV_PDISP_RG_HDCP_CS_LSB				0x19
+#define HDMI_NV_PDISP_HDMI_AUDIO_EMU0				0x1a
+#define HDMI_NV_PDISP_HDMI_AUDIO_EMU_RDATA0			0x1b
+#define HDMI_NV_PDISP_HDMI_AUDIO_EMU1				0x1c
+#define HDMI_NV_PDISP_HDMI_AUDIO_EMU2				0x1d
+#define HDMI_NV_PDISP_HDMI_AUDIO_INFOFRAME_CTRL			0x1e
+#define HDMI_NV_PDISP_HDMI_AUDIO_INFOFRAME_STATUS		0x1f
+#define HDMI_NV_PDISP_HDMI_AUDIO_INFOFRAME_HEADER		0x20
+#define HDMI_NV_PDISP_HDMI_AUDIO_INFOFRAME_SUBPACK0_LOW		0x21
+#define HDMI_NV_PDISP_HDMI_AUDIO_INFOFRAME_SUBPACK0_HIGH	0x22
+#define HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_CTRL			0x23
+#define HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_STATUS			0x24
+#define HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_HEADER			0x25
+#define HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_SUBPACK0_LOW		0x26
+#define HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_SUBPACK0_HIGH		0x27
+#define HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_SUBPACK1_LOW		0x28
+#define HDMI_NV_PDISP_HDMI_AVI_INFOFRAME_SUBPACK1_HIGH		0x29
+#define  INFOFRAME_CTRL_ENABLE			(1 << 0)
+#define  INFOFRAME_CTRL_OTHER			(1 << 4)
+#define  INFOFRAME_CTRL_SINGLE			(1 << 8)
+
+#define INFOFRAME_HEADER_TYPE(x)		((x) & 0xff)
+#define INFOFRAME_HEADER_VERSION(x)		(((x) & 0xff) << 8)
+#define INFOFRAME_HEADER_LEN(x)			(((x) & 0xf) << 16)
+
+#define HDMI_NV_PDISP_HDMI_GENERIC_CTRL				0x2a
+#define  GENERIC_CTRL_ENABLE			(1 << 0)
+#define  GENERIC_CTRL_OTHER			(1 << 4)
+#define  GENERIC_CTRL_SINGLE			(1 << 8)
+#define  GENERIC_CTRL_HBLANK			(1 << 12)
+#define  GENERIC_CTRL_AUDIO			(1 << 16)
+
+#define HDMI_NV_PDISP_HDMI_GENERIC_STATUS			0x2b
+#define HDMI_NV_PDISP_HDMI_GENERIC_HEADER			0x2c
+#define HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK0_LOW			0x2d
+#define HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK0_HIGH		0x2e
+#define HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK1_LOW			0x2f
+#define HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK1_HIGH		0x30
+#define HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK2_LOW			0x31
+#define HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK2_HIGH		0x32
+#define HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK3_LOW			0x33
+#define HDMI_NV_PDISP_HDMI_GENERIC_SUBPACK3_HIGH		0x34
+#define HDMI_NV_PDISP_HDMI_ACR_CTRL				0x35
+#define HDMI_NV_PDISP_HDMI_ACR_0320_SUBPACK_LOW			0x36
+#define HDMI_NV_PDISP_HDMI_ACR_0320_SUBPACK_HIGH		0x37
+#define HDMI_NV_PDISP_HDMI_ACR_0441_SUBPACK_LOW			0x38
+#define HDMI_NV_PDISP_HDMI_ACR_0441_SUBPACK_HIGH		0x39
+#define HDMI_NV_PDISP_HDMI_ACR_0882_SUBPACK_LOW			0x3a
+#define HDMI_NV_PDISP_HDMI_ACR_0882_SUBPACK_HIGH		0x3b
+#define HDMI_NV_PDISP_HDMI_ACR_1764_SUBPACK_LOW			0x3c
+#define HDMI_NV_PDISP_HDMI_ACR_1764_SUBPACK_HIGH		0x3d
+#define HDMI_NV_PDISP_HDMI_ACR_0480_SUBPACK_LOW			0x3e
+#define HDMI_NV_PDISP_HDMI_ACR_0480_SUBPACK_HIGH		0x3f
+#define HDMI_NV_PDISP_HDMI_ACR_0960_SUBPACK_LOW			0x40
+#define HDMI_NV_PDISP_HDMI_ACR_0960_SUBPACK_HIGH		0x41
+#define HDMI_NV_PDISP_HDMI_ACR_1920_SUBPACK_LOW			0x42
+#define HDMI_NV_PDISP_HDMI_ACR_1920_SUBPACK_HIGH		0x43
+#define  ACR_SB3(x)				(((x) & 0xff) << 8)
+#define  ACR_SB2(x)				(((x) & 0xff) << 16)
+#define  ACR_SB1(x)				(((x) & 0xff) << 24)
+#define  ACR_SUBPACK_CTS(x)			(((x) & 0xffffff) << 8)
+
+#define  ACR_SB6(x)				(((x) & 0xff) << 0)
+#define  ACR_SB5(x)				(((x) & 0xff) << 8)
+#define  ACR_SB4(x)				(((x) & 0xff) << 16)
+#define  ACR_ENABLE				(1 << 31)
+#define  ACR_SUBPACK_N(x)			((x) & 0xffffff)
+
+#define HDMI_NV_PDISP_HDMI_CTRL					0x44
+#define  HDMI_CTRL_REKEY(x)			(((x) & 0x7f) << 0)
+#define  HDMI_CTRL_AUDIO_LAYOUT			(1 << 8)
+#define  HDMI_CTRL_SAMPLE_FLAT			(1 << 12)
+#define  HDMI_CTRL_MAX_AC_PACKET(x)		(((x) & 0x1f) << 16)
+#define  HDMI_CTRL_ENABLE			(1 << 30)
+
+#define HDMI_NV_PDISP_HDMI_VSYNC_KEEPOUT			0x45
+#define HDMI_NV_PDISP_HDMI_VSYNC_WINDOW				0x46
+#define  VSYNC_WINDOW_END(x)			(((x) & 0x3ff) << 0)
+#define  VSYNC_WINDOW_START(x)			(((x) & 0x3ff) << 16)
+#define  VSYNC_WINDOW_ENABLE			(1 << 31)
+
+#define HDMI_NV_PDISP_HDMI_GCP_CTRL				0x47
+#define HDMI_NV_PDISP_HDMI_GCP_STATUS				0x48
+#define HDMI_NV_PDISP_HDMI_GCP_SUBPACK				0x49
+#define HDMI_NV_PDISP_HDMI_CHANNEL_STATUS1			0x4a
+#define HDMI_NV_PDISP_HDMI_CHANNEL_STATUS2			0x4b
+#define HDMI_NV_PDISP_HDMI_EMU0					0x4c
+#define HDMI_NV_PDISP_HDMI_EMU1					0x4d
+#define HDMI_NV_PDISP_HDMI_EMU1_RDATA				0x4e
+#define HDMI_NV_PDISP_HDMI_SPARE				0x4f
+#define  SPARE_HW_CTS				(1 << 0)
+#define  SPARE_FORCE_SW_CTS			(1 << 1)
+#define  SPARE_CTS_RESET_VAL(x)			(((x) & 0x7) << 16)
+#define  SPARE_ACR_PRIORITY_HIGH		(0 << 31)
+#define  SPARE_ACR_PRIORITY_LOW			(1 << 31)
+
+#define HDMI_NV_PDISP_HDMI_SPDIF_CHN_STATUS1			0x50
+#define HDMI_NV_PDISP_HDMI_SPDIF_CHN_STATUS2			0x51
+#define HDMI_NV_PDISP_HDCPRIF_ROM_CTRL				0x53
+#define HDMI_NV_PDISP_SOR_CAP					0x54
+#define HDMI_NV_PDISP_SOR_PWR					0x55
+#define  SOR_PWR_NORMAL_STATE_PD		(0 << 0)
+#define  SOR_PWR_NORMAL_STATE_PU		(1 << 0)
+#define  SOR_PWR_NORMAL_START_NORMAL		(0 << 1)
+#define  SOR_PWR_NORMAL_START_ALT		(1 << 1)
+#define  SOR_PWR_SAFE_STATE_PD			(0 << 16)
+#define  SOR_PWR_SAFE_STATE_PU			(1 << 16)
+#define  SOR_PWR_SAFE_START_NORMAL		(0 << 17)
+#define  SOR_PWR_SAFE_START_ALT			(1 << 17)
+#define  SOR_PWR_HALT_DELAY			(1 << 24)
+#define  SOR_PWR_MODE				(1 << 28)
+#define  SOR_PWR_SETTING_NEW_DONE		(0 << 31)
+#define  SOR_PWR_SETTING_NEW_PENDING		(1 << 31)
+#define  SOR_PWR_SETTING_NEW_TRIGGER		(1 << 31)
+
+#define HDMI_NV_PDISP_SOR_TEST					0x56
+#define HDMI_NV_PDISP_SOR_PLL0					0x57
+#define  SOR_PLL_PWR				(1 << 0)
+#define  SOR_PLL_PDBG				(1 << 1)
+#define  SOR_PLL_VCOPD				(1 << 2)
+#define  SOR_PLL_PDPORT				(1 << 3)
+#define  SOR_PLL_RESISTORSEL			(1 << 4)
+#define  SOR_PLL_PULLDOWN			(1 << 5)
+#define  SOR_PLL_VCOCAP(x)			(((x) & 0xf) << 8)
+#define  SOR_PLL_BG_V17_S(x)			(((x) & 0xf) << 12)
+#define  SOR_PLL_FILTER(x)			(((x) & 0xf) << 16)
+#define  SOR_PLL_ICHPMP(x)			(((x) & 0xf) << 24)
+#define  SOR_PLL_TX_REG_LOAD(x)			(((x) & 0x3) << 28)
+
+#define HDMI_NV_PDISP_SOR_PLL1					0x58
+#define  SOR_PLL_TMDS_TERM_ENABLE		(1 << 8)
+#define  SOR_PLL_TMDS_TERMADJ(x)		(((x) & 0xf) << 9)
+#define  SOR_PLL_LOADADJ(x)			(((x) & 0xf) << 20)
+#define  SOR_PLL_PE_EN				(1 << 28)
+#define  SOR_PLL_HALF_FULL_PE			(1 << 29)
+#define  SOR_PLL_S_D_PIN_PE			(1 << 30)
+
+#define HDMI_NV_PDISP_SOR_PLL2					0x59
+#define HDMI_NV_PDISP_SOR_CSTM					0x5a
+#define  SOR_CSTM_PD_TXDA_0			(1 << 0)
+#define  SOR_CSTM_PD_TXDA_1			(1 << 1)
+#define  SOR_CSTM_PD_TXDA_2			(1 << 2)
+#define  SOR_CSTM_PD_TXDA_3			(1 << 3)
+#define  SOR_CSTM_PD_TXDB_0			(1 << 4)
+#define  SOR_CSTM_PD_TXDB_1			(1 << 5)
+#define  SOR_CSTM_PD_TXDB_2			(1 << 6)
+#define  SOR_CSTM_PD_TXDB_3			(1 << 7)
+#define  SOR_CSTM_PD_TXCA			(1 << 8)
+#define  SOR_CSTM_PD_TXCB			(1 << 9)
+#define  SOR_CSTM_UPPER				(1 << 11)
+#define  SOR_CSTM_MODE(x)			(((x) & 0x3) << 12)
+#define  SOR_CSTM_LINKACTA			(1 << 14)
+#define  SOR_CSTM_LINKACTB			(1 << 15)
+#define  SOR_CSTM_LVDS_EN			(1 << 16)
+#define  SOR_CSTM_DUP_SYNC			(1 << 17)
+#define  SOR_CSTM_NEW_MODE			(1 << 18)
+#define  SOR_CSTM_BALANCED			(1 << 19)
+#define  SOR_CSTM_PLLDIV			(1 << 21)
+#define  SOR_CSTM_ROTCLK(x)			(((x) & 0xf) << 24)
+#define  SOR_CSTM_ROTDAT(x)			(((x) & 0x7) << 28)
+
+#define HDMI_NV_PDISP_SOR_LVDS					0x5b
+#define HDMI_NV_PDISP_SOR_CRCA					0x5c
+#define HDMI_NV_PDISP_SOR_CRCB					0x5d
+#define HDMI_NV_PDISP_SOR_BLANK					0x5e
+#define HDMI_NV_PDISP_SOR_SEQ_CTL				0x5f
+#define  SOR_SEQ_CTL_PU_PC(x)			(((x) & 0xf) << 0)
+#define  SOR_SEQ_PU_PC_ALT(x)			(((x) & 0xf) << 4)
+#define  SOR_SEQ_PD_PC(x)			(((x) & 0xf) << 8)
+#define  SOR_SEQ_PD_PC_ALT(x)			(((x) & 0xf) << 12)
+#define  SOR_SEQ_PC(x)				(((x) & 0xf) << 16)
+#define  SOR_SEQ_STATUS				(1 << 28)
+#define  SOR_SEQ_SWITCH				(1 << 30)
+
+#define HDMI_NV_PDISP_SOR_SEQ_INST0				0x60
+#define HDMI_NV_PDISP_SOR_SEQ_INST1				0x61
+#define HDMI_NV_PDISP_SOR_SEQ_INST2				0x62
+#define HDMI_NV_PDISP_SOR_SEQ_INST3				0x63
+#define HDMI_NV_PDISP_SOR_SEQ_INST4				0x64
+#define HDMI_NV_PDISP_SOR_SEQ_INST5				0x65
+#define HDMI_NV_PDISP_SOR_SEQ_INST6				0x66
+#define HDMI_NV_PDISP_SOR_SEQ_INST7				0x67
+#define HDMI_NV_PDISP_SOR_SEQ_INST8				0x68
+#define HDMI_NV_PDISP_SOR_SEQ_INST9				0x69
+#define HDMI_NV_PDISP_SOR_SEQ_INSTA				0x6a
+#define HDMI_NV_PDISP_SOR_SEQ_INSTB				0x6b
+#define HDMI_NV_PDISP_SOR_SEQ_INSTC				0x6c
+#define HDMI_NV_PDISP_SOR_SEQ_INSTD				0x6d
+#define HDMI_NV_PDISP_SOR_SEQ_INSTE				0x6e
+#define HDMI_NV_PDISP_SOR_SEQ_INSTF				0x6f
+#define  SOR_SEQ_INST_WAIT_TIME(x)		(((x) & 0x3ff) << 0)
+#define  SOR_SEQ_INST_WAIT_UNITS_US		(0 << 12)
+#define  SOR_SEQ_INST_WAIT_UNITS_MS		(1 << 12)
+#define  SOR_SEQ_INST_WAIT_UNITS_VSYNC		(2 << 12)
+#define  SOR_SEQ_INST_HALT			(1 << 15)
+#define  SOR_SEQ_INST_PIN_A_LOW			(0 << 21)
+#define  SOR_SEQ_INST_PIN_A_HIGH		(1 << 21)
+#define  SOR_SEQ_INST_PIN_B_LOW			(0 << 22)
+#define  SOR_SEQ_INST_PIN_B_HIGH		(1 << 22)
+#define  SOR_SEQ_INST_DRIVE_PWM_OUT_LO		(1 << 23)
+#define  SOR_SEQ_INST_TRISTATE_IOS		(1 << 24)
+#define  SOR_SEQ_INST_SOR_SEQ_INST_BLACK_DATA	(1 << 25)
+#define  SOR_SEQ_INST_BLANK_DE			(1 << 26)
+#define  SOR_SEQ_INST_BLANK_H			(1 << 27)
+#define  SOR_SEQ_INST_BLANK_V			(1 << 28)
+#define  SOR_SEQ_INST_ASSERT_PLL_RESETV		(1 << 29)
+#define  SOR_SEQ_INST_POWERDOWN_MACRO		(1 << 30)
+#define  SOR_SEQ_INST_PLL_PULLDOWN		(1 << 31)
+
+#define HDMI_NV_PDISP_SOR_VCRCA0				0x72
+#define HDMI_NV_PDISP_SOR_VCRCA1				0x73
+#define HDMI_NV_PDISP_SOR_CCRCA0				0x74
+#define HDMI_NV_PDISP_SOR_CCRCA1				0x75
+#define HDMI_NV_PDISP_SOR_EDATAA0				0x76
+#define HDMI_NV_PDISP_SOR_EDATAA1				0x77
+#define HDMI_NV_PDISP_SOR_COUNTA0				0x78
+#define HDMI_NV_PDISP_SOR_COUNTA1				0x79
+#define HDMI_NV_PDISP_SOR_DEBUGA0				0x7a
+#define HDMI_NV_PDISP_SOR_DEBUGA1				0x7b
+#define HDMI_NV_PDISP_SOR_TRIG					0x7c
+#define HDMI_NV_PDISP_SOR_MSCHECK				0x7d
+#define HDMI_NV_PDISP_SOR_LANE_DRIVE_CURRENT			0x7e
+#define  DRIVE_CURRENT_LANE0(x)			(((x) & 0x3f) << 0)
+#define  DRIVE_CURRENT_LANE1(x)			(((x) & 0x3f) << 8)
+#define  DRIVE_CURRENT_LANE2(x)			(((x) & 0x3f) << 16)
+#define  DRIVE_CURRENT_LANE3(x)			(((x) & 0x3f) << 24)
+#define  DRIVE_CURRENT_FUSE_OVERRIDE		(1 << 31)
+#define  DRIVE_CURRENT_1_500_mA			0x00
+#define  DRIVE_CURRENT_1_875_mA			0x01
+#define  DRIVE_CURRENT_2_250_mA			0x02
+#define  DRIVE_CURRENT_2_625_mA			0x03
+#define  DRIVE_CURRENT_3_000_mA			0x04
+#define  DRIVE_CURRENT_3_375_mA			0x05
+#define  DRIVE_CURRENT_3_750_mA			0x06
+#define  DRIVE_CURRENT_4_125_mA			0x07
+#define  DRIVE_CURRENT_4_500_mA			0x08
+#define  DRIVE_CURRENT_4_875_mA			0x09
+#define  DRIVE_CURRENT_5_250_mA			0x0a
+#define  DRIVE_CURRENT_5_625_mA			0x0b
+#define  DRIVE_CURRENT_6_000_mA			0x0c
+#define  DRIVE_CURRENT_6_375_mA			0x0d
+#define  DRIVE_CURRENT_6_750_mA			0x0e
+#define  DRIVE_CURRENT_7_125_mA			0x0f
+#define  DRIVE_CURRENT_7_500_mA			0x10
+#define  DRIVE_CURRENT_7_875_mA			0x11
+#define  DRIVE_CURRENT_8_250_mA			0x12
+#define  DRIVE_CURRENT_8_625_mA			0x13
+#define  DRIVE_CURRENT_9_000_mA			0x14
+#define  DRIVE_CURRENT_9_375_mA			0x15
+#define  DRIVE_CURRENT_9_750_mA			0x16
+#define  DRIVE_CURRENT_10_125_mA		0x17
+#define  DRIVE_CURRENT_10_500_mA		0x18
+#define  DRIVE_CURRENT_10_875_mA		0x19
+#define  DRIVE_CURRENT_11_250_mA		0x1a
+#define  DRIVE_CURRENT_11_625_mA		0x1b
+#define  DRIVE_CURRENT_12_000_mA		0x1c
+#define  DRIVE_CURRENT_12_375_mA		0x1d
+#define  DRIVE_CURRENT_12_750_mA		0x1e
+#define  DRIVE_CURRENT_13_125_mA		0x1f
+#define  DRIVE_CURRENT_13_500_mA		0x20
+#define  DRIVE_CURRENT_13_875_mA		0x21
+#define  DRIVE_CURRENT_14_250_mA		0x22
+#define  DRIVE_CURRENT_14_625_mA		0x23
+#define  DRIVE_CURRENT_15_000_mA		0x24
+#define  DRIVE_CURRENT_15_375_mA		0x25
+#define  DRIVE_CURRENT_15_750_mA		0x26
+#define  DRIVE_CURRENT_16_125_mA		0x27
+#define  DRIVE_CURRENT_16_500_mA		0x28
+#define  DRIVE_CURRENT_16_875_mA		0x29
+#define  DRIVE_CURRENT_17_250_mA		0x2a
+#define  DRIVE_CURRENT_17_625_mA		0x2b
+#define  DRIVE_CURRENT_18_000_mA		0x2c
+#define  DRIVE_CURRENT_18_375_mA		0x2d
+#define  DRIVE_CURRENT_18_750_mA		0x2e
+#define  DRIVE_CURRENT_19_125_mA		0x2f
+#define  DRIVE_CURRENT_19_500_mA		0x30
+#define  DRIVE_CURRENT_19_875_mA		0x31
+#define  DRIVE_CURRENT_20_250_mA		0x32
+#define  DRIVE_CURRENT_20_625_mA		0x33
+#define  DRIVE_CURRENT_21_000_mA		0x34
+#define  DRIVE_CURRENT_21_375_mA		0x35
+#define  DRIVE_CURRENT_21_750_mA		0x36
+#define  DRIVE_CURRENT_22_125_mA		0x37
+#define  DRIVE_CURRENT_22_500_mA		0x38
+#define  DRIVE_CURRENT_22_875_mA		0x39
+#define  DRIVE_CURRENT_23_250_mA		0x3a
+#define  DRIVE_CURRENT_23_625_mA		0x3b
+#define  DRIVE_CURRENT_24_000_mA		0x3c
+#define  DRIVE_CURRENT_24_375_mA		0x3d
+#define  DRIVE_CURRENT_24_750_mA		0x3e
+
+#define HDMI_NV_PDISP_AUDIO_DEBUG0				0x7f
+#define HDMI_NV_PDISP_AUDIO_DEBUG1				0x80
+#define HDMI_NV_PDISP_AUDIO_DEBUG2				0x81
+/* note: datasheet defines FS1..FS7.  we have FS(0)..FS(6) */
+#define HDMI_NV_PDISP_AUDIO_FS(x)				(0x82 + (x))
+#define  AUDIO_FS_LOW(x)			(((x) & 0xfff) << 0)
+#define  AUDIO_FS_HIGH(x)			(((x) & 0xfff) << 16)
+
+
+#define HDMI_NV_PDISP_AUDIO_PULSE_WIDTH				0x89
+#define HDMI_NV_PDISP_AUDIO_THRESHOLD				0x8a
+#define HDMI_NV_PDISP_AUDIO_CNTRL0				0x8b
+#if !defined(CONFIG_ARCH_TEGRA_2x_SOC)
+#define HDMI_NV_PDISP_SOR_AUDIO_CNTRL0_0		0xac
+#define HDMI_NV_PDISP_SOR_AUDIO_HDA_ELD_BUFWR_0		0xbc
+#define HDMI_NV_PDISP_SOR_AUDIO_HDA_PRESENSE_0		0xbd
+#define HDMI_NV_PDISP_SOR_AUDIO_AVAL_0320_0			0xbf
+#define HDMI_NV_PDISP_SOR_AUDIO_AVAL_0441_0			0xc0
+#define HDMI_NV_PDISP_SOR_AUDIO_AVAL_0882_0			0xc1
+#define HDMI_NV_PDISP_SOR_AUDIO_AVAL_1764_0			0xc2
+#define HDMI_NV_PDISP_SOR_AUDIO_AVAL_0480_0			0xc3
+#define HDMI_NV_PDISP_SOR_AUDIO_AVAL_0960_0			0xc4
+#define HDMI_NV_PDISP_SOR_AUDIO_AVAL_1920_0			0xc5
+#define HDMI_NV_PDISP_SOR_AUDIO_AVAL_DEFAULT_0			0xc6
+#endif
+#define  AUDIO_CNTRL0_ERROR_TOLERANCE(x)	(((x) & 0xff) << 0)
+#define  AUDIO_CNTRL0_SOFT_RESET		(1 << 8)
+#define  AUDIO_CNTRL0_SOFT_RESET_ALL		(1 << 12)
+#define  AUDIO_CNTRL0_SAMPLING_FREQ_UNKNOWN	(1 << 16)
+#define  AUDIO_CNTRL0_SAMPLING_FREQ_32K		(2 << 16)
+#define  AUDIO_CNTRL0_SAMPLING_FREQ_44_1K	(0 << 16)
+#define  AUDIO_CNTRL0_SAMPLING_FREQ_48K		(2 << 16)
+#define  AUDIO_CNTRL0_SAMPLING_FREQ_88_2K	(8 << 16)
+#define  AUDIO_CNTRL0_SAMPLING_FREQ_96K		(10 << 16)
+#define  AUDIO_CNTRL0_SAMPLING_FREQ_176_4K	(12 << 16)
+#define  AUDIO_CNTRL0_SAMPLING_FREQ_192K	(14 << 16)
+#define  AUDIO_CNTRL0_SOURCE_SELECT_AUTO	(0 << 20)
+#define  AUDIO_CNTRL0_SOURCE_SELECT_SPDIF	(1 << 20)
+#define  AUDIO_CNTRL0_SOURCE_SELECT_HDAL	(2 << 20)
+#define  AUDIO_CNTRL0_INJECT_NULLSMPL		(1 << 29)
+#define  AUDIO_CNTRL0_FRAMES_PER_BLOCK(x)	(((x) & 0xff) << 24)
+
+#define HDMI_NV_PDISP_AUDIO_N					0x8c
+#define  AUDIO_N_VALUE(x)			(((x) & 0xfffff) << 0)
+#define  AUDIO_N_RESETF				(1 << 20)
+#define  AUDIO_N_GENERATE_NORMAL		(0 << 24)
+#define  AUDIO_N_GENERATE_ALTERNALTE		(1 << 24)
+#define  AUDIO_N_LOOKUP_ENABLE			(1 << 28)
+
+#define HDMI_NV_PDISP_HDCPRIF_ROM_TIMING			0x94
+#define HDMI_NV_PDISP_SOR_REFCLK				0x95
+#define  SOR_REFCLK_DIV_INT(x)			(((x) & 0xff) << 8)
+#define  SOR_REFCLK_DIV_FRAC(x)			(((x) & 0x3) << 6)
+
+#define HDMI_NV_PDISP_CRC_CONTROL				0x96
+#define HDMI_NV_PDISP_INPUT_CONTROL				0x97
+#define  HDMI_SRC_DISPLAYA			(0 << 0)
+#define  HDMI_SRC_DISPLAYB			(1 << 0)
+#define  ARM_VIDEO_RANGE_FULL			(0 << 1)
+#define  ARM_VIDEO_RANGE_LIMITED		(1 << 1)
+
+#define HDMI_NV_PDISP_SCRATCH					0x98
+#define HDMI_NV_PDISP_PE_CURRENT				0x99
+#define  PE_CURRENT0(x)				(((x) & 0xf) << 0)
+#define  PE_CURRENT1(x)				(((x) & 0xf) << 8)
+#define  PE_CURRENT2(x)				(((x) & 0xf) << 16)
+#define  PE_CURRENT3(x)				(((x) & 0xf) << 24)
+#define  PE_CURRENT_0_0_mA			0x0
+#define  PE_CURRENT_0_5_mA			0x1
+#define  PE_CURRENT_1_0_mA			0x2
+#define  PE_CURRENT_1_5_mA			0x3
+#define  PE_CURRENT_2_0_mA			0x4
+#define  PE_CURRENT_2_5_mA			0x5
+#define  PE_CURRENT_3_0_mA			0x6
+#define  PE_CURRENT_3_5_mA			0x7
+#define  PE_CURRENT_4_0_mA			0x8
+#define  PE_CURRENT_4_5_mA			0x9
+#define  PE_CURRENT_5_0_mA			0xa
+#define  PE_CURRENT_5_5_mA			0xb
+#define  PE_CURRENT_6_0_mA			0xc
+#define  PE_CURRENT_6_5_mA			0xd
+#define  PE_CURRENT_7_0_mA			0xe
+#define  PE_CURRENT_7_5_mA			0xf
+
+#define HDMI_NV_PDISP_KEY_CTRL					0x9a
+#define  LOCAL_KEYS				(1 << 0)
+#define  AUTOINC				(1 << 1)
+#define  WRITE16				(1 << 4)
+#define  PKEY_REQUEST_RELOAD_TRIGGER		(1 << 5)
+#define  PKEY_LOADED				(1 << 6)
+#define HDMI_NV_PDISP_KEY_DEBUG0				0x9b
+#define HDMI_NV_PDISP_KEY_DEBUG1				0x9c
+#define HDMI_NV_PDISP_KEY_DEBUG2				0x9d
+#define HDMI_NV_PDISP_KEY_HDCP_KEY_0				0x9e
+#define HDMI_NV_PDISP_KEY_HDCP_KEY_1				0x9f
+#define HDMI_NV_PDISP_KEY_HDCP_KEY_2				0xa0
+#define HDMI_NV_PDISP_KEY_HDCP_KEY_3				0xa1
+#define HDMI_NV_PDISP_KEY_HDCP_KEY_TRIG				0xa2
+#define HDMI_NV_PDISP_KEY_SKEY_INDEX				0xa3
+
+#endif
diff --git a/drivers/video/tegra/dc/lut.c b/drivers/video/tegra/dc/lut.c
new file mode 100644
index 000000000000..7ce8fc6768a0
--- /dev/null
+++ b/drivers/video/tegra/dc/lut.c
@@ -0,0 +1,130 @@
+/*
+ * drivers/video/tegra/dc/lut.c
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/err.h>
+#include <linux/types.h>
+#include <mach/dc.h>
+
+#include "dc_reg.h"
+#include "dc_priv.h"
+
+void tegra_dc_init_lut_defaults(struct tegra_dc_lut *lut)
+{
+	int i;
+	for (i = 0; i < 256; i++)
+		lut->r[i] = lut->g[i] = lut->b[i] = (u8)i;
+}
+
+static int tegra_dc_loop_lut(struct tegra_dc *dc,
+			     struct tegra_dc_win *win,
+			     int(*lambda)(struct tegra_dc *dc, int i, u32 rgb))
+{
+	struct tegra_dc_lut *lut = &win->lut;
+	struct tegra_dc_lut *global_lut = &dc->fb_lut;
+	int i;
+	for (i = 0; i < 256; i++) {
+
+		u32 r = (u32)lut->r[i];
+		u32 g = (u32)lut->g[i];
+		u32 b = (u32)lut->b[i];
+
+		if (!(win->ppflags & TEGRA_WIN_PPFLAG_CP_FBOVERRIDE)) {
+			r = (u32)global_lut->r[r];
+			g = (u32)global_lut->g[g];
+			b = (u32)global_lut->b[b];
+		}
+
+		if (!lambda(dc, i, r | (g<<8) | (b<<16)))
+			return 0;
+	}
+	return 1;
+}
+
+static int tegra_dc_lut_isdefaults_lambda(struct tegra_dc *dc, int i, u32 rgb)
+{
+	if (rgb != (i | (i<<8) | (i<<16)))
+		return 0;
+	return 1;
+}
+
+static int tegra_dc_set_lut_setreg_lambda(struct tegra_dc *dc, int i, u32 rgb)
+{
+	tegra_dc_writel(dc, rgb, DC_WIN_COLOR_PALETTE(i));
+	return 1;
+}
+
+void tegra_dc_set_lut(struct tegra_dc *dc, struct tegra_dc_win *win)
+{
+	unsigned long val = tegra_dc_readl(dc, DC_WIN_WIN_OPTIONS);
+
+	tegra_dc_loop_lut(dc, win, tegra_dc_set_lut_setreg_lambda);
+
+	if (win->ppflags & TEGRA_WIN_PPFLAG_CP_ENABLE)
+		val |= CP_ENABLE;
+	else
+		val &= ~CP_ENABLE;
+
+	tegra_dc_writel(dc, val, DC_WIN_WIN_OPTIONS);
+}
+
+static int tegra_dc_update_winlut(struct tegra_dc *dc, int win_idx, int fbovr)
+{
+	struct tegra_dc_win *win = &dc->windows[win_idx];
+
+	mutex_lock(&dc->lock);
+
+	if (!dc->enabled) {
+		mutex_unlock(&dc->lock);
+		return -EFAULT;
+	}
+
+	if (fbovr > 0)
+		win->ppflags |= TEGRA_WIN_PPFLAG_CP_FBOVERRIDE;
+	else if (fbovr == 0)
+		win->ppflags &= ~TEGRA_WIN_PPFLAG_CP_FBOVERRIDE;
+
+	if (!tegra_dc_loop_lut(dc, win, tegra_dc_lut_isdefaults_lambda))
+		win->ppflags |= TEGRA_WIN_PPFLAG_CP_ENABLE;
+	else
+		win->ppflags &= ~TEGRA_WIN_PPFLAG_CP_ENABLE;
+
+	tegra_dc_writel(dc, WINDOW_A_SELECT << win_idx,
+			DC_CMD_DISPLAY_WINDOW_HEADER);
+
+	tegra_dc_set_lut(dc, win);
+
+	mutex_unlock(&dc->lock);
+
+	tegra_dc_update_windows(&win, 1);
+
+	return 0;
+}
+
+int tegra_dc_update_lut(struct tegra_dc *dc, int win_idx, int fboveride)
+{
+	if (win_idx > -1)
+		return tegra_dc_update_winlut(dc, win_idx, fboveride);
+
+	for (win_idx = 0; win_idx < DC_N_WINDOWS; win_idx++) {
+		int err = tegra_dc_update_winlut(dc, win_idx, fboveride);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(tegra_dc_update_lut);
+
diff --git a/drivers/video/tegra/dc/mode.c b/drivers/video/tegra/dc/mode.c
new file mode 100644
index 000000000000..f5cd4fb0cdea
--- /dev/null
+++ b/drivers/video/tegra/dc/mode.c
@@ -0,0 +1,616 @@
+/*
+ * drivers/video/tegra/dc/mode.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/err.h>
+#include <linux/types.h>
+#include <linux/clk.h>
+
+#include <mach/clk.h>
+#include <mach/dc.h>
+
+#include "dc_reg.h"
+#include "dc_priv.h"
+
+const struct fb_videomode tegra_modes[] = {
+	/* EDT 5.7" ET070080DH or TouchRevolution Fusion 7" */
+	{
+		.name = 	"800x480",
+		.refresh = 	60,
+		.xres =		800,
+		.yres =		480,
+		.pixclock =	30807,
+		.left_margin = 	128,
+		.right_margin = 64,
+		.upper_margin = 22,
+		.lower_margin = 20,
+		.hsync_len = 	64,
+		.vsync_len = 	3,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+		.flag = FB_FLAG_RATIO_16_9,
+		.vmode = FB_VMODE_NONINTERLACED
+	},
+	/* TouchRevolution Fusion 10" aka Chunghwa Picture Tubes
+	 * CLAA100NC05 10.1 inch 1024x600 single channel LVDS panel
+	 */
+	{
+		.name = 	"1024x600",
+		.refresh = 	60,
+		.xres = 	1024,
+		.yres = 	600,
+		.pixclock =     KHZ2PICOS(48000),
+		.left_margin = 	104,
+		.right_margin = 43,
+		.upper_margin = 24,
+		.lower_margin = 20,
+		.hsync_len = 	5,
+		.vsync_len = 	5,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+		.flag = FB_FLAG_RATIO_16_9,
+		.vmode = FB_VMODE_NONINTERLACED
+	},
+	{
+		/* 1366x768 */
+		.refresh =      60,
+		.xres =         1366,
+		.yres =         768,
+		.pixclock =     KHZ2PICOS(72072),
+		.hsync_len =    58,    /* h_sync_width */
+		.vsync_len =    4,     /* v_sync_width */
+		.left_margin =  58,    /* h_back_porch */
+		.upper_margin = 4,     /* v_back_porch */
+		.right_margin = 58,    /* h_front_porch */
+		.lower_margin = 4,      /* v_front_porch */
+		.vmode =        FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+	{
+		/* 1680x1050p 59.94/60hz */
+		.refresh =      60,
+		.xres =         1680,
+		.yres =         1050,
+		.pixclock =     KHZ2PICOS(147140),
+		.hsync_len =    184,    /* h_sync_width */
+		.vsync_len =    3,     /* v_sync_width */
+		.left_margin =  288,    /* h_back_porch */
+		.upper_margin = 33,     /* v_back_porch */
+		.right_margin = 104,    /* h_front_porch */
+		.lower_margin = 1,      /* v_front_porch */
+		.vmode =        FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+	{
+		/* 1920x1080p 59.94/60hz CVT */
+		.refresh =	60,
+		.xres =         1920,
+		.yres =         1080,
+		.pixclock =     KHZ2PICOS(148500),
+		.hsync_len =    44,     /* h_sync_width */
+		.vsync_len =    5,      /* v_sync_width */
+		.left_margin =  148,     /* h_back_porch */
+		.upper_margin = 36,     /* v_back_porch */
+		.right_margin = 88,     /* h_front_porch */
+		.lower_margin = 4,      /* v_front_porch */
+		.vmode =        FB_VMODE_NONINTERLACED,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+	},
+	{
+		/* 1920x1200p 60hz */
+		.refresh =	60,
+		.xres =         1920,
+		.yres =         1200,
+		.pixclock =     KHZ2PICOS(154000),
+		.hsync_len =    32,     /* h_sync_width */
+		.vsync_len =    6,      /* v_sync_width */
+		.left_margin =  80,     /* h_back_porch */
+		.upper_margin = 26,     /* v_back_porch */
+		.right_margin = 48,     /* h_front_porch */
+		.lower_margin = 3,      /* v_front_porch */
+		.vmode =        FB_VMODE_NONINTERLACED,
+		.sync = 0,
+	},
+	/* Portrait modes */
+	{
+		.name = 	"480x640",
+		.refresh = 	60,
+		.xres = 	480,
+		.yres = 	640,
+		.pixclock = 	55555,
+		.left_margin = 	20,
+		.right_margin = 8,
+		.upper_margin = 7,
+		.lower_margin = 8,
+		.hsync_len = 	4,
+		.vsync_len = 	1,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+		.vmode = FB_VMODE_NONINTERLACED
+	},
+	{
+		.name = 	"540x960",
+		.refresh = 	60,
+		.xres = 	540,
+		.yres = 	960,
+		.pixclock = 	100000,
+		.left_margin = 	32,
+		.right_margin = 32,
+		.upper_margin = 1,
+		.lower_margin = 2,
+		.hsync_len = 	16,
+		.vsync_len = 	1,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+		.vmode = FB_VMODE_NONINTERLACED
+	},
+	{
+		.name = 	"720x1280",
+		.refresh = 	60,
+		.xres = 	720,
+		.yres = 	1280,
+		.pixclock = 	16282,
+		.left_margin = 	100,
+		.right_margin = 4,
+		.upper_margin = 14,
+		.lower_margin = 4,
+		.hsync_len = 	4,
+		.vsync_len = 	4,
+		.sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+		.vmode = FB_VMODE_NONINTERLACED
+	},
+};
+
+/* try to find best matching mode using our modes, VESA and CEA modes from
+ * modedb
+ */
+int tegra_fb_find_mode(struct fb_var_screeninfo *var, struct fb_info *info,
+		       const char* option, unsigned int default_bpp)
+{
+	int out;
+
+	out = fb_find_mode(var, info, option, tegra_modes,
+			ARRAY_SIZE(tegra_modes), NULL, default_bpp);
+
+	/* Only accept this mode if we found a reasonable match (resolution) */
+	if (out == 1 || out == 2)
+		return out;
+
+	out = fb_find_mode(&info->var, info, option,
+			cea_modes, CEA_MODEDB_SIZE, NULL, default_bpp);
+
+	/* Check if we found a full match */
+	if (out == 1 || out == 2)
+		return out;
+
+	return fb_find_mode(&info->var, info, option,
+			vesa_modes, VESA_MODEDB_SIZE, NULL, default_bpp);
+}
+EXPORT_SYMBOL(tegra_fb_find_mode);
+
+
+/* return non-zero if constraint is violated */
+static int calc_h_ref_to_sync(const struct tegra_dc_mode *mode, int *href)
+{
+	long a, b;
+
+	/* Constraint 5: H_REF_TO_SYNC >= 0 */
+	a = 0;
+
+	/* Constraint 6: H_FRONT_PORT >= (H_REF_TO_SYNC + 1) */
+	b = mode->h_front_porch - 1;
+
+	/* Constraint 1: H_REF_TO_SYNC + H_SYNC_WIDTH + H_BACK_PORCH > 11 */
+	if (a + mode->h_sync_width + mode->h_back_porch <= 11)
+		a = 1 + 11 - mode->h_sync_width - mode->h_back_porch;
+	/* check Constraint 1 and 6 */
+	if (a > b)
+		return 1;
+
+	/* Constraint 4: H_SYNC_WIDTH >= 1 */
+	if (mode->h_sync_width < 1)
+		return 4;
+
+	/* Constraint 7: H_DISP_ACTIVE >= 16 */
+	if (mode->h_active < 16)
+		return 7;
+
+	if (href) {
+		if (b > a && a % 2)
+			*href = a + 1; /* use smallest even value */
+		else
+			*href = a; /* even or only possible value */
+	}
+
+	return 0;
+}
+
+static int calc_v_ref_to_sync(const struct tegra_dc_mode *mode, int *vref)
+{
+	long a;
+	a = 1; /* Constraint 5: V_REF_TO_SYNC >= 1 */
+
+	/* Constraint 2: V_REF_TO_SYNC + V_SYNC_WIDTH + V_BACK_PORCH > 1 */
+	if (a + mode->v_sync_width + mode->v_back_porch <= 1)
+		a = 1 + 1 - mode->v_sync_width - mode->v_back_porch;
+
+	/* Constraint 6 */
+	if (mode->v_front_porch < a + 1)
+		a = mode->v_front_porch - 1;
+
+	/* Constraint 4: V_SYNC_WIDTH >= 1 */
+	if (mode->v_sync_width < 1)
+		return 4;
+
+	/* Constraint 7: V_DISP_ACTIVE >= 16 */
+	if (mode->v_active < 16)
+		return 7;
+
+	if (vref)
+		*vref = a;
+	return 0;
+}
+
+static int calc_ref_to_sync(struct tegra_dc_mode *mode)
+{
+	int ret;
+	ret = calc_h_ref_to_sync(mode, &mode->h_ref_to_sync);
+	if (ret)
+		return ret;
+	ret = calc_v_ref_to_sync(mode, &mode->v_ref_to_sync);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static bool check_ref_to_sync(struct tegra_dc_mode *mode)
+{
+	/* Constraint 1: H_REF_TO_SYNC + H_SYNC_WIDTH + H_BACK_PORCH > 11. */
+	if (mode->h_ref_to_sync + mode->h_sync_width + mode->h_back_porch <= 11)
+		return false;
+
+	/* Constraint 2: V_REF_TO_SYNC + V_SYNC_WIDTH + V_BACK_PORCH > 1. */
+	if (mode->v_ref_to_sync + mode->v_sync_width + mode->v_back_porch <= 1)
+		return false;
+
+	/* Constraint 3: V_FRONT_PORCH + V_SYNC_WIDTH + V_BACK_PORCH > 1
+	 * (vertical blank). */
+	if (mode->v_front_porch + mode->v_sync_width + mode->v_back_porch <= 1)
+		return false;
+
+	/* Constraint 4: V_SYNC_WIDTH >= 1; H_SYNC_WIDTH >= 1. */
+	if (mode->v_sync_width < 1 || mode->h_sync_width < 1)
+		return false;
+
+	/* Constraint 5: V_REF_TO_SYNC >= 1; H_REF_TO_SYNC >= 0. */
+	if (mode->v_ref_to_sync < 1 || mode->h_ref_to_sync < 0)
+		return false;
+
+	/* Constraint 6: V_FRONT_PORT >= (V_REF_TO_SYNC + 1);
+	 * H_FRONT_PORT >= (H_REF_TO_SYNC + 1). */
+	if (mode->v_front_porch < mode->v_ref_to_sync + 1 ||
+		mode->h_front_porch < mode->h_ref_to_sync + 1)
+		return false;
+
+	/* Constraint 7: H_DISP_ACTIVE >= 16; V_DISP_ACTIVE >= 16. */
+	if (mode->h_active < 16 || mode->v_active < 16)
+		return false;
+
+	return true;
+}
+
+#ifndef CONFIG_ANDROID
+static s64 calc_frametime_ns(const struct tegra_dc_mode *m)
+{
+	long h_total, v_total;
+	h_total = m->h_active + m->h_front_porch + m->h_back_porch +
+		m->h_sync_width;
+	v_total = m->v_active + m->v_front_porch + m->v_back_porch +
+		m->v_sync_width;
+	return (!m->pclk) ? 0 : (s64)(div_s64(((s64)h_total * v_total *
+					1000000000ULL), m->pclk));
+}
+#endif /* !CONFIG_ANDROID */
+
+/* return in 1000ths of a Hertz */
+int tegra_dc_calc_refresh(const struct tegra_dc_mode *m)
+{
+	long h_total, v_total, refresh;
+	h_total = m->h_active + m->h_front_porch + m->h_back_porch +
+		m->h_sync_width;
+	v_total = m->v_active + m->v_front_porch + m->v_back_porch +
+		m->v_sync_width;
+	refresh = m->pclk / h_total;
+	refresh *= 1000;
+	refresh /= v_total;
+	return refresh;
+}
+
+#ifdef DEBUG
+static void print_mode(struct tegra_dc *dc,
+			const struct tegra_dc_mode *mode, const char *note)
+{
+	if (mode) {
+		int refresh = tegra_dc_calc_refresh(mode);
+		dev_info(&dc->ndev->dev, "%s():MODE:%dx%d@%d.%03uHz pclk=%d\n",
+			note ? note : "",
+			mode->h_active, mode->v_active,
+			refresh / 1000, refresh % 1000,
+			mode->pclk);
+	}
+}
+#else /* !DEBUG */
+static inline void print_mode(struct tegra_dc *dc,
+			const struct tegra_dc_mode *mode, const char *note) { }
+#endif /* DEBUG */
+
+int tegra_dc_program_mode(struct tegra_dc *dc, struct tegra_dc_mode *mode)
+{
+	unsigned long val;
+	unsigned long rate;
+	unsigned long div;
+	unsigned long pclk;
+
+	print_mode(dc, mode, __func__);
+
+	/* use default EMC rate when switching modes */
+	dc->new_emc_clk_rate = tegra_dc_get_default_emc_clk_rate(dc);
+	tegra_dc_program_bandwidth(dc, true);
+
+	tegra_dc_writel(dc, 0x0, DC_DISP_DISP_TIMING_OPTIONS);
+	tegra_dc_writel(dc, mode->h_ref_to_sync | (mode->v_ref_to_sync << 16),
+			DC_DISP_REF_TO_SYNC);
+	tegra_dc_writel(dc, mode->h_sync_width | (mode->v_sync_width << 16),
+			DC_DISP_SYNC_WIDTH);
+	tegra_dc_writel(dc, mode->h_back_porch | (mode->v_back_porch << 16),
+			DC_DISP_BACK_PORCH);
+	tegra_dc_writel(dc, mode->h_active | (mode->v_active << 16),
+			DC_DISP_DISP_ACTIVE);
+	tegra_dc_writel(dc, mode->h_front_porch | (mode->v_front_porch << 16),
+			DC_DISP_FRONT_PORCH);
+
+	tegra_dc_writel(dc, DE_SELECT_ACTIVE | DE_CONTROL_NORMAL,
+			DC_DISP_DATA_ENABLE_OPTIONS);
+
+	/* TODO: MIPI/CRT/HDMI clock cals */
+
+	val = DISP_DATA_FORMAT_DF1P1C;
+
+	if (dc->out->align == TEGRA_DC_ALIGN_MSB)
+		val |= DISP_DATA_ALIGNMENT_MSB;
+	else
+		val |= DISP_DATA_ALIGNMENT_LSB;
+
+	if (dc->out->order == TEGRA_DC_ORDER_RED_BLUE)
+		val |= DISP_DATA_ORDER_RED_BLUE;
+	else
+		val |= DISP_DATA_ORDER_BLUE_RED;
+
+	tegra_dc_writel(dc, val, DC_DISP_DISP_INTERFACE_CONTROL);
+
+	rate = tegra_dc_clk_get_rate(dc);
+
+	pclk = tegra_dc_pclk_round_rate(dc, mode->pclk);
+	trace_printk("%s:pclk=%ld\n", dc->ndev->name, pclk);
+	if (pclk < (mode->pclk / 100 * 99) ||
+	    pclk > (mode->pclk / 100 * 109)) {
+		dev_err(&dc->ndev->dev,
+			"can't divide %ld clock to %d -1/+9%% %ld %d %d\n",
+			rate, mode->pclk,
+			pclk, (mode->pclk / 100 * 99),
+			(mode->pclk / 100 * 109));
+		return -EINVAL;
+	}
+
+	div = (rate * 2 / pclk) - 2;
+	trace_printk("%s:div=%ld\n", dc->ndev->name, div);
+
+	tegra_dc_writel(dc, 0x00010001,
+			DC_DISP_SHIFT_CLOCK_OPTIONS);
+	tegra_dc_writel(dc, PIXEL_CLK_DIVIDER_PCD1 | SHIFT_CLK_DIVIDER(div),
+			DC_DISP_DISP_CLOCK_CONTROL);
+
+#ifdef CONFIG_SWITCH
+	switch_set_state(&dc->modeset_switch,
+			 (mode->h_active << 16) | mode->v_active);
+#endif
+
+	tegra_dc_writel(dc, GENERAL_UPDATE, DC_CMD_STATE_CONTROL);
+	tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
+
+	print_mode_info(dc, dc->mode);
+	return 0;
+}
+
+static int panel_sync_rate;
+
+int tegra_dc_get_panel_sync_rate(void)
+{
+	return panel_sync_rate;
+}
+EXPORT_SYMBOL(tegra_dc_get_panel_sync_rate);
+
+int tegra_dc_set_mode(struct tegra_dc *dc, const struct tegra_dc_mode *mode)
+{
+	memcpy(&dc->mode, mode, sizeof(dc->mode));
+
+	dev_info(&dc->ndev->dev, "using mode %dx%d pclk=%d href=%d vref=%d\n",
+		mode->h_active, mode->v_active, mode->pclk,
+		mode->h_ref_to_sync, mode->v_ref_to_sync
+	);
+
+	if (dc->out->type == TEGRA_DC_OUT_RGB)
+		panel_sync_rate = tegra_dc_calc_refresh(mode);
+	else if (dc->out->type == TEGRA_DC_OUT_DSI)
+		panel_sync_rate = dc->out->dsi->rated_refresh_rate * 1000;
+
+	print_mode(dc, mode, __func__);
+#ifndef CONFIG_ANDROID
+	dc->frametime_ns = calc_frametime_ns(mode);
+#endif /* !CONFIG_ANDROID */
+
+	return 0;
+}
+EXPORT_SYMBOL(tegra_dc_set_mode);
+
+int tegra_dc_var_to_dc_mode(struct tegra_dc *dc, struct fb_var_screeninfo *var,
+		struct tegra_dc_mode *mode)
+{
+	bool stereo_mode = false;
+	int err;
+
+	if (!var->pixclock)
+		return -EINVAL;
+
+	mode->pclk = PICOS2KHZ(var->pixclock) * 1000;
+	mode->h_sync_width = var->hsync_len;
+	mode->v_sync_width = var->vsync_len;
+	mode->h_back_porch = var->left_margin;
+	mode->v_back_porch = var->upper_margin;
+	mode->h_active = var->xres;
+	mode->v_active = var->yres;
+	mode->h_front_porch = var->right_margin;
+	mode->v_front_porch = var->lower_margin;
+	mode->stereo_mode = stereo_mode;
+
+	/*
+	 * HACK:
+	 * If v_front_porch is only 1, we would violate Constraint 5/6
+	 * in this case, increase front porch by 1
+	 */
+	if (mode->v_front_porch <= 1)
+		mode->v_front_porch = 2;
+
+
+	if (dc->out->type == TEGRA_DC_OUT_HDMI) {
+		/* HDMI controller requires h_ref=1, v_ref=1 */
+		mode->h_ref_to_sync = 1;
+		mode->v_ref_to_sync = 1;
+	} else {
+		/* Calculate ref_to_sync signals */
+		err = calc_ref_to_sync(mode);
+		if (err) {
+			dev_err(&dc->ndev->dev, "display timing ref_to_sync"
+				"calculation failed with code %d\n", err);
+			return -EINVAL;
+		}
+		dev_info(&dc->ndev->dev, "Calculated sync href=%d vref=%d\n",
+				mode->h_ref_to_sync, mode->v_ref_to_sync);
+	}
+	if (!check_ref_to_sync(mode)) {
+		dev_err(&dc->ndev->dev,
+				"display timing doesn't meet restrictions.\n");
+		return -EINVAL;
+	}
+
+#ifndef CONFIG_TEGRA_HDMI_74MHZ_LIMIT
+	/* Double the pixel clock and update v_active only for
+	 * frame packed mode */
+	if (mode->stereo_mode) {
+		mode->pclk *= 2;
+		/* total v_active = yres*2 + activespace */
+		mode->v_active = var->yres * 2 +
+				var->vsync_len +
+				var->upper_margin +
+				var->lower_margin;
+	}
+#endif
+
+	mode->flags = 0;
+
+	if (!(var->sync & FB_SYNC_HOR_HIGH_ACT))
+		mode->flags |= TEGRA_DC_MODE_FLAG_NEG_H_SYNC;
+
+	if (!(var->sync & FB_SYNC_VERT_HIGH_ACT))
+		mode->flags |= TEGRA_DC_MODE_FLAG_NEG_V_SYNC;
+
+	return 0;
+}
+EXPORT_SYMBOL(tegra_dc_var_to_dc_mode);
+
+/*
+ * This method is only used by sysfs interface
+ * /sys/devices/tegradc.1/nvdps
+ */
+int tegra_dc_set_fb_mode(struct tegra_dc *dc,
+		const struct fb_videomode *fbmode, bool stereo_mode)
+{
+	int err;
+	struct tegra_dc_mode mode;
+
+	if (!fbmode->pixclock)
+		return -EINVAL;
+
+	mode.pclk = PICOS2KHZ(fbmode->pixclock) * 1000;
+	mode.h_sync_width = fbmode->hsync_len;
+	mode.v_sync_width = fbmode->vsync_len;
+	mode.h_back_porch = fbmode->left_margin;
+	mode.v_back_porch = fbmode->upper_margin;
+	mode.h_active = fbmode->xres;
+	mode.v_active = fbmode->yres;
+	mode.h_front_porch = fbmode->right_margin;
+	mode.v_front_porch = fbmode->lower_margin;
+	mode.stereo_mode = stereo_mode;
+	if (dc->out->type == TEGRA_DC_OUT_HDMI) {
+		/* HDMI controller requires h_ref=1, v_ref=1 */
+		mode.h_ref_to_sync = 1;
+		mode.v_ref_to_sync = 1;
+	} else {
+		/*
+		 * HACK:
+		 * If v_front_porch is only 1, we would violate Constraint 5/6
+		 * in this case, increase front porch by 1
+		 */
+		if (mode.v_front_porch <= 1)
+			mode.v_front_porch = 2;
+
+		err = calc_ref_to_sync(&mode);
+		if (err) {
+			dev_err(&dc->ndev->dev, "display timing ref_to_sync"
+				"calculation failed with code %d\n", err);
+			return -EINVAL;
+		}
+		dev_info(&dc->ndev->dev, "Calculated sync href=%d vref=%d\n",
+				mode.h_ref_to_sync, mode.v_ref_to_sync);
+	}
+	if (!check_ref_to_sync(&mode)) {
+		dev_err(&dc->ndev->dev,
+				"display timing doesn't meet restrictions.\n");
+		return -EINVAL;
+	}
+
+#ifndef CONFIG_TEGRA_HDMI_74MHZ_LIMIT
+	/* Double the pixel clock and update v_active only for
+	 * frame packed mode */
+	if (mode.stereo_mode) {
+		mode.pclk *= 2;
+		/* total v_active = yres*2 + activespace */
+		mode.v_active = fbmode->yres * 2 +
+				fbmode->vsync_len +
+				fbmode->upper_margin +
+				fbmode->lower_margin;
+	}
+#endif
+
+	mode.flags = 0;
+
+	if (!(fbmode->sync & FB_SYNC_HOR_HIGH_ACT))
+		mode.flags |= TEGRA_DC_MODE_FLAG_NEG_H_SYNC;
+
+	if (!(fbmode->sync & FB_SYNC_VERT_HIGH_ACT))
+		mode.flags |= TEGRA_DC_MODE_FLAG_NEG_V_SYNC;
+
+	return tegra_dc_set_mode(dc, &mode);
+}
+EXPORT_SYMBOL(tegra_dc_set_fb_mode);
diff --git a/drivers/video/tegra/dc/nvhdcp.c b/drivers/video/tegra/dc/nvhdcp.c
new file mode 100644
index 000000000000..8489ebf69719
--- /dev/null
+++ b/drivers/video/tegra/dc/nvhdcp.c
@@ -0,0 +1,1259 @@
+/*
+ * drivers/video/tegra/dc/nvhdcp.c
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/miscdevice.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/uaccess.h>
+#include <linux/wait.h>
+#include <linux/workqueue.h>
+#include <asm/atomic.h>
+
+#include <mach/dc.h>
+#include <mach/kfuse.h>
+
+#include <video/nvhdcp.h>
+
+#include "dc_reg.h"
+#include "dc_priv.h"
+#include "hdmi_reg.h"
+#include "hdmi.h"
+
+DECLARE_WAIT_QUEUE_HEAD(wq_worker);
+
+/* for 0x40 Bcaps */
+#define BCAPS_REPEATER (1 << 6)
+#define BCAPS_READY (1 << 5)
+#define BCAPS_11 (1 << 1) /* used for both Bcaps and Ainfo */
+
+/* for 0x41 Bstatus */
+#define BSTATUS_MAX_DEVS_EXCEEDED	(1 << 7)
+#define BSTATUS_MAX_CASCADE_EXCEEDED	(1 << 11)
+
+#ifdef VERBOSE_DEBUG
+#define nvhdcp_vdbg(...)	\
+		printk("nvhdcp: " __VA_ARGS__)
+#else
+#define nvhdcp_vdbg(...)		\
+({						\
+	if(0)					\
+		printk("nvhdcp: " __VA_ARGS__); \
+	0;					\
+})
+#endif
+#define nvhdcp_debug(...)	\
+		pr_debug("nvhdcp: " __VA_ARGS__)
+#define nvhdcp_err(...)	\
+		pr_err("nvhdcp: Error: " __VA_ARGS__)
+#define nvhdcp_info(...)	\
+		pr_info("nvhdcp: " __VA_ARGS__)
+
+
+/* for nvhdcp.state */
+enum tegra_nvhdcp_state {
+	STATE_OFF,
+	STATE_UNAUTHENTICATED,
+	STATE_LINK_VERIFY,
+	STATE_RENEGOTIATE,
+};
+
+struct tegra_nvhdcp {
+	struct delayed_work		work;
+	struct tegra_dc_hdmi_data	*hdmi;
+	struct workqueue_struct		*downstream_wq;
+	struct mutex			lock;
+	struct miscdevice		miscdev;
+	char				name[12];
+	unsigned			id;
+	bool				plugged; /* true if hotplug detected */
+	atomic_t			policy; /* set policy */
+	enum tegra_nvhdcp_state		state; /* STATE_xxx */
+	struct i2c_client		*client;
+	struct i2c_board_info		info;
+	int				bus;
+	u32				b_status;
+	u64				a_n;
+	u64				c_n;
+	u64				a_ksv;
+	u64				b_ksv;
+	u64				c_ksv;
+	u64				d_ksv;
+	u8				v_prime[20];
+	u64				m_prime;
+	u32				num_bksv_list;
+	u64				bksv_list[TEGRA_NVHDCP_MAX_DEVS];
+	int				fail_count;
+};
+
+static inline bool nvhdcp_is_plugged(struct tegra_nvhdcp *nvhdcp)
+{
+	rmb();
+	return nvhdcp->plugged;
+}
+
+static inline bool nvhdcp_set_plugged(struct tegra_nvhdcp *nvhdcp, bool plugged)
+{
+	nvhdcp->plugged = plugged;
+	wmb();
+	return plugged;
+}
+
+static int nvhdcp_i2c_read(struct tegra_nvhdcp *nvhdcp, u8 reg,
+					size_t len, void *data)
+{
+	int status;
+	int retries = 15;
+	struct i2c_msg msg[] = {
+		{
+			.addr = 0x74 >> 1, /* primary link */
+			.flags = 0,
+			.len = 1,
+			.buf = &reg,
+		},
+		{
+			.addr = 0x74 >> 1, /* primary link */
+			.flags = I2C_M_RD,
+			.len = len,
+			.buf = data,
+		},
+	};
+
+	do {
+		if (!nvhdcp_is_plugged(nvhdcp)) {
+			nvhdcp_err("disconnect during i2c xfer\n");
+			return -EIO;
+		}
+		status = i2c_transfer(nvhdcp->client->adapter,
+			msg, ARRAY_SIZE(msg));
+		if ((status < 0) && (retries > 1))
+			msleep(250);
+	} while ((status < 0) && retries--);
+
+	if (status < 0) {
+		nvhdcp_err("i2c xfer error %d\n", status);
+		return status;
+	}
+
+	return 0;
+}
+
+static int nvhdcp_i2c_write(struct tegra_nvhdcp *nvhdcp, u8 reg,
+					size_t len, const void *data)
+{
+	int status;
+	u8 buf[len + 1];
+	struct i2c_msg msg[] = {
+		{
+			.addr = 0x74 >> 1, /* primary link */
+			.flags = 0,
+			.len = len + 1,
+			.buf = buf,
+		},
+	};
+	int retries = 15;
+
+	buf[0] = reg;
+	memcpy(buf + 1, data, len);
+
+	do {
+		if (!nvhdcp_is_plugged(nvhdcp)) {
+			nvhdcp_err("disconnect during i2c xfer\n");
+			return -EIO;
+		}
+		status = i2c_transfer(nvhdcp->client->adapter,
+			msg, ARRAY_SIZE(msg));
+		if ((status < 0) && (retries > 1))
+			msleep(250);
+	} while ((status < 0) && retries--);
+
+	if (status < 0) {
+		nvhdcp_err("i2c xfer error %d\n", status);
+		return status;
+	}
+
+	return 0;
+}
+
+static inline int nvhdcp_i2c_read8(struct tegra_nvhdcp *nvhdcp, u8 reg, u8 *val)
+{
+	return nvhdcp_i2c_read(nvhdcp, reg, 1, val);
+}
+
+static inline int nvhdcp_i2c_write8(struct tegra_nvhdcp *nvhdcp, u8 reg, u8 val)
+{
+	return nvhdcp_i2c_write(nvhdcp, reg, 1, &val);
+}
+
+static inline int nvhdcp_i2c_read16(struct tegra_nvhdcp *nvhdcp,
+					u8 reg, u16 *val)
+{
+	u8 buf[2];
+	int e;
+
+	e = nvhdcp_i2c_read(nvhdcp, reg, sizeof buf, buf);
+	if (e)
+		return e;
+
+	if (val)
+		*val = buf[0] | (u16)buf[1] << 8;
+
+	return 0;
+}
+
+static int nvhdcp_i2c_read40(struct tegra_nvhdcp *nvhdcp, u8 reg, u64 *val)
+{
+	u8 buf[5];
+	int e, i;
+	u64 n;
+
+	e = nvhdcp_i2c_read(nvhdcp, reg, sizeof buf, buf);
+	if (e)
+		return e;
+
+	for(i = 0, n = 0; i < 5; i++ ) {
+		n <<= 8;
+		n |= buf[4 - i];
+	}
+
+	if (val)
+		*val = n;
+
+	return 0;
+}
+
+static int nvhdcp_i2c_write40(struct tegra_nvhdcp *nvhdcp, u8 reg, u64 val)
+{
+	char buf[5];
+	int i;
+	for(i = 0; i < 5; i++ ) {
+		buf[i] = val;
+		val >>= 8;
+	}
+	return nvhdcp_i2c_write(nvhdcp, reg, sizeof buf, buf);
+}
+
+static int nvhdcp_i2c_write64(struct tegra_nvhdcp *nvhdcp, u8 reg, u64 val)
+{
+	char buf[8];
+	int i;
+	for(i = 0; i < 8; i++ ) {
+		buf[i] = val;
+		val >>= 8;
+	}
+	return nvhdcp_i2c_write(nvhdcp, reg, sizeof buf, buf);
+}
+
+
+/* 64-bit link encryption session random number */
+static inline u64 get_an(struct tegra_dc_hdmi_data *hdmi)
+{
+	u64 r;
+	r = (u64)tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_AN_MSB) << 32;
+	r |= tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_AN_LSB);
+	return r;
+}
+
+/* 64-bit upstream exchange random number */
+static inline void set_cn(struct tegra_dc_hdmi_data *hdmi, u64 c_n)
+{
+	tegra_hdmi_writel(hdmi, (u32)c_n, HDMI_NV_PDISP_RG_HDCP_CN_LSB);
+	tegra_hdmi_writel(hdmi, c_n >> 32, HDMI_NV_PDISP_RG_HDCP_CN_MSB);
+}
+
+
+/* 40-bit transmitter's key selection vector */
+static inline u64 get_aksv(struct tegra_dc_hdmi_data *hdmi)
+{
+	u64 r;
+	r = (u64)tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_AKSV_MSB) << 32;
+	r |= tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_AKSV_LSB);
+	return r;
+}
+
+/* 40-bit receiver's key selection vector */
+static inline void set_bksv(struct tegra_dc_hdmi_data *hdmi, u64 b_ksv, bool repeater)
+{
+	if (repeater)
+		b_ksv |= (u64)REPEATER << 32;
+	tegra_hdmi_writel(hdmi, (u32)b_ksv, HDMI_NV_PDISP_RG_HDCP_BKSV_LSB);
+	tegra_hdmi_writel(hdmi, b_ksv >> 32, HDMI_NV_PDISP_RG_HDCP_BKSV_MSB);
+}
+
+
+/* 40-bit software's key selection vector */
+static inline void set_cksv(struct tegra_dc_hdmi_data *hdmi, u64 c_ksv)
+{
+	tegra_hdmi_writel(hdmi, (u32)c_ksv, HDMI_NV_PDISP_RG_HDCP_CKSV_LSB);
+	tegra_hdmi_writel(hdmi, c_ksv >> 32, HDMI_NV_PDISP_RG_HDCP_CKSV_MSB);
+}
+
+/* 40-bit connection state */
+static inline u64 get_cs(struct tegra_dc_hdmi_data *hdmi)
+{
+	u64 r;
+	r = (u64)tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_CS_MSB) << 32;
+	r |= tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_CS_LSB);
+	return r;
+}
+
+/* 40-bit upstream key selection vector */
+static inline u64 get_dksv(struct tegra_dc_hdmi_data *hdmi)
+{
+	u64 r;
+	r = (u64)tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_DKSV_MSB) << 32;
+	r |= tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_DKSV_LSB);
+	return r;
+}
+
+/* 64-bit encrypted M0 value */
+static inline u64 get_mprime(struct tegra_dc_hdmi_data *hdmi)
+{
+	u64 r;
+	r = (u64)tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_MPRIME_MSB) << 32;
+	r |= tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_MPRIME_LSB);
+	return r;
+}
+
+static inline u16 get_transmitter_ri(struct tegra_dc_hdmi_data *hdmi)
+{
+	return tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_RI);
+}
+
+static inline int get_receiver_ri(struct tegra_nvhdcp *nvhdcp, u16 *r)
+{
+	return nvhdcp_i2c_read16(nvhdcp, 0x8, r); /* long read */
+}
+
+static int get_bcaps(struct tegra_nvhdcp *nvhdcp, u8 *b_caps)
+{
+	return nvhdcp_i2c_read8(nvhdcp, 0x40, b_caps);
+}
+
+static int get_ksvfifo(struct tegra_nvhdcp *nvhdcp,
+					unsigned num_bksv_list, u64 *ksv_list)
+{
+	u8 *buf, *p;
+	int e;
+	unsigned i;
+	size_t buf_len = num_bksv_list * 5;
+
+	if (!ksv_list || num_bksv_list > TEGRA_NVHDCP_MAX_DEVS)
+		return -EINVAL;
+
+	if (num_bksv_list == 0)
+		return 0;
+
+	buf = kmalloc(buf_len, GFP_KERNEL);
+	if (IS_ERR_OR_NULL(buf))
+		return -ENOMEM;
+
+	e = nvhdcp_i2c_read(nvhdcp, 0x43, buf_len, buf);
+	if (e) {
+		kfree(buf);
+		return e;
+	}
+
+	/* load 40-bit keys from repeater into array of u64 */
+	p = buf;
+	for (i = 0; i < num_bksv_list; i++) {
+		ksv_list[i] = p[0] | ((u64)p[1] << 8) | ((u64)p[2] << 16)
+				| ((u64)p[3] << 24) | ((u64)p[4] << 32);
+		p += 5;
+	}
+
+	kfree(buf);
+	return 0;
+}
+
+/* get V' 160-bit SHA-1 hash from repeater */
+static int get_vprime(struct tegra_nvhdcp *nvhdcp, u8 *v_prime)
+{
+	int e, i;
+
+	for (i = 0; i < 20; i += 4) {
+		e = nvhdcp_i2c_read(nvhdcp, 0x20 + i, 4, v_prime + i);
+		if (e)
+			return e;
+	}
+	return 0;
+}
+
+
+/* set or clear RUN_YES */
+static void hdcp_ctrl_run(struct tegra_dc_hdmi_data *hdmi, bool v)
+{
+	u32 ctrl;
+
+	if (v) {
+		ctrl = tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_CTRL);
+		ctrl |= HDCP_RUN_YES;
+	} else {
+		ctrl = 0;
+	}
+
+	tegra_hdmi_writel(hdmi, ctrl, HDMI_NV_PDISP_RG_HDCP_CTRL);
+}
+
+/* wait for any bits in mask to be set in HDMI_NV_PDISP_RG_HDCP_CTRL
+ * sleeps up to 120mS */
+static int wait_hdcp_ctrl(struct tegra_dc_hdmi_data *hdmi, u32 mask, u32 *v)
+{
+	int retries = 13;
+	u32 ctrl;
+
+	do {
+		ctrl = tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_CTRL);
+		if ((ctrl & mask)) {
+			if (v)
+				*v = ctrl;
+			break;
+		}
+		if (retries > 1)
+			msleep(10);
+	} while (--retries);
+	if (!retries) {
+		nvhdcp_err("ctrl read timeout (mask=0x%x)\n", mask);
+		return -EIO;
+	}
+	return 0;
+}
+
+/* wait for bits in mask to be set to value in HDMI_NV_PDISP_KEY_CTRL
+ * waits up to 100mS */
+static int wait_key_ctrl(struct tegra_dc_hdmi_data *hdmi, u32 mask, u32 value)
+{
+	int retries = 101;
+	u32 ctrl;
+
+	do {
+		msleep(1);
+		ctrl = tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_KEY_CTRL);
+		if (((ctrl ^ value) & mask) == 0)
+			break;
+	} while (--retries);
+	if (!retries) {
+		nvhdcp_err("key ctrl read timeout (mask=0x%x)\n", mask);
+		return -EIO;
+	}
+	return 0;
+}
+
+/* check that key selection vector is well formed.
+ * NOTE: this function assumes KSV has already been checked against
+ * revocation list.
+ */
+static int verify_ksv(u64 k)
+{
+	unsigned i;
+
+	/* count set bits, must be exactly 20 set to be valid */
+	for(i = 0; k; i++)
+		k ^= k & -k;
+
+	return  (i != 20) ? -EINVAL : 0;
+}
+
+/* get Status and Kprime signature - READ_S on TMDS0_LINK0 only */
+static int get_s_prime(struct tegra_nvhdcp *nvhdcp, struct tegra_nvhdcp_packet *pkt)
+{
+	struct tegra_dc_hdmi_data *hdmi = nvhdcp->hdmi;
+	u32 sp_msb, sp_lsb1, sp_lsb2;
+	int e;
+
+	/* if connection isn't authenticated ... */
+	mutex_lock(&nvhdcp->lock);
+	if (nvhdcp->state != STATE_LINK_VERIFY) {
+		memset(pkt, 0, sizeof *pkt);
+		pkt->packet_results = TEGRA_NVHDCP_RESULT_LINK_FAILED;
+		e = 0;
+		goto err;
+	}
+
+	pkt->packet_results = TEGRA_NVHDCP_RESULT_UNSUCCESSFUL;
+
+	/* we will be taking c_n, c_ksv as input */
+	if (!(pkt->value_flags & TEGRA_NVHDCP_FLAG_CN)
+			|| !(pkt->value_flags & TEGRA_NVHDCP_FLAG_CKSV)) {
+		nvhdcp_err("missing value_flags (0x%x)\n", pkt->value_flags);
+		e = -EINVAL;
+		goto err;
+	}
+
+	pkt->value_flags = 0;
+
+	pkt->a_ksv = nvhdcp->a_ksv;
+	pkt->a_n = nvhdcp->a_n;
+	pkt->value_flags = TEGRA_NVHDCP_FLAG_AKSV | TEGRA_NVHDCP_FLAG_AN;
+
+	nvhdcp_vdbg("%s():cn %llx cksv %llx\n", __func__, pkt->c_n, pkt->c_ksv);
+
+	set_cn(hdmi, pkt->c_n);
+
+	tegra_hdmi_writel(hdmi, TMDS0_LINK0 | READ_S,
+					HDMI_NV_PDISP_RG_HDCP_CMODE);
+
+	set_cksv(hdmi, pkt->c_ksv);
+
+	e = wait_hdcp_ctrl(hdmi, SPRIME_VALID, NULL);
+	if (e) {
+		nvhdcp_err("Sprime read timeout\n");
+		pkt->packet_results = TEGRA_NVHDCP_RESULT_UNSUCCESSFUL;
+		e = -EIO;
+		goto err;
+	}
+
+	msleep(50);
+
+	/* read 56-bit Sprime plus 16 status bits */
+	sp_msb = tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_SPRIME_MSB);
+	sp_lsb1 = tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_SPRIME_LSB1);
+	sp_lsb2 = tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_SPRIME_LSB2);
+
+	/* top 8 bits of LSB2 and bottom 8 bits of MSB hold status bits. */
+	pkt->hdcp_status = ( sp_msb << 8 ) | ( sp_lsb2 >> 24);
+	pkt->value_flags |= TEGRA_NVHDCP_FLAG_S;
+
+	/* 56-bit Kprime */
+	pkt->k_prime = ((u64)(sp_lsb2 & 0xffffff) << 32) | sp_lsb1;
+	pkt->value_flags |= TEGRA_NVHDCP_FLAG_KP;
+
+	/* is connection state supported? */
+	if (sp_msb & STATUS_CS) {
+		pkt->cs = get_cs(hdmi);
+		pkt->value_flags |= TEGRA_NVHDCP_FLAG_CS;
+	}
+
+	/* load Dksv */
+	pkt->d_ksv = get_dksv(hdmi);
+	if (verify_ksv(pkt->d_ksv)) {
+		nvhdcp_err("Dksv invalid!\n");
+		pkt->packet_results = TEGRA_NVHDCP_RESULT_UNSUCCESSFUL;
+		e = -EIO; /* treat bad Dksv as I/O error */
+	}
+	pkt->value_flags |= TEGRA_NVHDCP_FLAG_DKSV;
+
+	/* copy current Bksv */
+	pkt->b_ksv = nvhdcp->b_ksv;
+	pkt->value_flags |= TEGRA_NVHDCP_FLAG_BKSV;
+
+	pkt->packet_results = TEGRA_NVHDCP_RESULT_SUCCESS;
+	mutex_unlock(&nvhdcp->lock);
+	return 0;
+
+err:
+	mutex_unlock(&nvhdcp->lock);
+	return e;
+}
+
+/* get M prime - READ_M on TMDS0_LINK0 only */
+static inline int get_m_prime(struct tegra_nvhdcp *nvhdcp, struct tegra_nvhdcp_packet *pkt)
+{
+	struct tegra_dc_hdmi_data *hdmi = nvhdcp->hdmi;
+	int e;
+
+	pkt->packet_results = TEGRA_NVHDCP_RESULT_UNSUCCESSFUL;
+
+	/* if connection isn't authenticated ... */
+	mutex_lock(&nvhdcp->lock);
+	if (nvhdcp->state != STATE_LINK_VERIFY) {
+		memset(pkt, 0, sizeof *pkt);
+		pkt->packet_results = TEGRA_NVHDCP_RESULT_LINK_FAILED;
+		e = 0;
+		goto err;
+	}
+
+	pkt->a_ksv = nvhdcp->a_ksv;
+	pkt->a_n = nvhdcp->a_n;
+	pkt->value_flags = TEGRA_NVHDCP_FLAG_AKSV | TEGRA_NVHDCP_FLAG_AN;
+
+	set_cn(hdmi, pkt->c_n);
+
+	tegra_hdmi_writel(hdmi, TMDS0_LINK0 | READ_M,
+					HDMI_NV_PDISP_RG_HDCP_CMODE);
+
+	/* Cksv write triggers Mprime update */
+	set_cksv(hdmi, pkt->c_ksv);
+
+	e = wait_hdcp_ctrl(hdmi, MPRIME_VALID, NULL);
+	if (e) {
+		nvhdcp_err("Mprime read timeout\n");
+		e = -EIO;
+		goto err;
+	}
+	msleep(50);
+
+	/* load Mprime */
+	pkt->m_prime = get_mprime(hdmi);
+	pkt->value_flags |= TEGRA_NVHDCP_FLAG_MP;
+
+	pkt->b_status = nvhdcp->b_status;
+	pkt->value_flags |= TEGRA_NVHDCP_FLAG_BSTATUS;
+
+	/* copy most recent KSVFIFO, if it is non-zero */
+	pkt->num_bksv_list = nvhdcp->num_bksv_list;
+	if( nvhdcp->num_bksv_list ) {
+		BUILD_BUG_ON(sizeof(pkt->bksv_list) != sizeof(nvhdcp->bksv_list));
+		memcpy(pkt->bksv_list, nvhdcp->bksv_list,
+			nvhdcp->num_bksv_list * sizeof(*pkt->bksv_list));
+		pkt->value_flags |= TEGRA_NVHDCP_FLAG_BKSVLIST;
+	}
+
+	/* copy v_prime */
+	BUILD_BUG_ON(sizeof(pkt->v_prime) != sizeof(nvhdcp->v_prime));
+	memcpy(pkt->v_prime, nvhdcp->v_prime, sizeof(nvhdcp->v_prime));
+	pkt->value_flags |= TEGRA_NVHDCP_FLAG_V;
+
+	/* load Dksv */
+	pkt->d_ksv = get_dksv(hdmi);
+	if (verify_ksv(pkt->d_ksv)) {
+		nvhdcp_err("Dksv invalid!\n");
+		e = -EIO;
+		goto err;
+	}
+	pkt->value_flags |= TEGRA_NVHDCP_FLAG_DKSV;
+
+	/* copy current Bksv */
+	pkt->b_ksv = nvhdcp->b_ksv;
+	pkt->value_flags |= TEGRA_NVHDCP_FLAG_BKSV;
+
+	pkt->packet_results = TEGRA_NVHDCP_RESULT_SUCCESS;
+	mutex_unlock(&nvhdcp->lock);
+	return 0;
+
+err:
+	mutex_unlock(&nvhdcp->lock);
+	return e;
+}
+
+static int load_kfuse(struct tegra_dc_hdmi_data *hdmi)
+{
+	unsigned buf[KFUSE_DATA_SZ / 4];
+	int e, i;
+	u32 ctrl;
+	u32 tmp;
+	int retries;
+
+	/* copy load kfuse into buffer - only needed for early Tegra parts */
+	e = tegra_kfuse_read(buf, sizeof buf);
+	if (e) {
+		nvhdcp_err("Kfuse read failure\n");
+		return e;
+	}
+
+	/* write the kfuse to HDMI SRAM */
+
+	tegra_hdmi_writel(hdmi, 1, HDMI_NV_PDISP_KEY_CTRL); /* LOAD_KEYS */
+
+	/* issue a reload */
+	ctrl = tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_KEY_CTRL);
+	tegra_hdmi_writel(hdmi, ctrl | PKEY_REQUEST_RELOAD_TRIGGER
+					| LOCAL_KEYS , HDMI_NV_PDISP_KEY_CTRL);
+
+	e = wait_key_ctrl(hdmi, PKEY_LOADED, PKEY_LOADED);
+	if (e) {
+		nvhdcp_err("key reload timeout\n");
+		return -EIO;
+	}
+
+	tegra_hdmi_writel(hdmi, 0, HDMI_NV_PDISP_KEY_SKEY_INDEX);
+
+	/* wait for SRAM to be cleared */
+	retries = 6;
+	do {
+		tmp = tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_KEY_DEBUG0);
+		if ((tmp & 1) == 0) break;
+		if (retries > 1)
+			mdelay(1);
+	} while (--retries);
+	if (!retries) {
+		nvhdcp_err("key SRAM clear timeout\n");
+		return -EIO;
+	}
+
+	for (i = 0; i < KFUSE_DATA_SZ / 4; i += 4) {
+
+		/* load 128-bits*/
+		tegra_hdmi_writel(hdmi, buf[i], HDMI_NV_PDISP_KEY_HDCP_KEY_0);
+		tegra_hdmi_writel(hdmi, buf[i+1], HDMI_NV_PDISP_KEY_HDCP_KEY_1);
+		tegra_hdmi_writel(hdmi, buf[i+2], HDMI_NV_PDISP_KEY_HDCP_KEY_2);
+		tegra_hdmi_writel(hdmi, buf[i+3], HDMI_NV_PDISP_KEY_HDCP_KEY_3);
+
+		/* trigger LOAD_HDCP_KEY */
+		tegra_hdmi_writel(hdmi, 0x100, HDMI_NV_PDISP_KEY_HDCP_KEY_TRIG);
+
+		tmp = LOCAL_KEYS | WRITE16;
+		if (i)
+			tmp |= AUTOINC;
+		tegra_hdmi_writel(hdmi, tmp, HDMI_NV_PDISP_KEY_CTRL);
+
+		/* wait for WRITE16 to complete */
+		e = wait_key_ctrl(hdmi, 0x10, 0); /* WRITE16 */
+		if (e) {
+			nvhdcp_err("key write timeout\n");
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+static int verify_link(struct tegra_nvhdcp *nvhdcp, bool wait_ri)
+{
+	struct tegra_dc_hdmi_data *hdmi = nvhdcp->hdmi;
+	int retries = 3;
+	u16 old, rx, tx;
+	int e;
+
+	old = 0;
+	rx = 0;
+	tx = 0;
+	/* retry 3 times to deal with I2C link issues */
+	do {
+		if (wait_ri)
+			old = get_transmitter_ri(hdmi);
+
+		e = get_receiver_ri(nvhdcp, &rx);
+		if (!e) {
+			if (!rx) {
+				nvhdcp_err("Ri is 0!\n");
+				return -EINVAL;
+			}
+
+			tx = get_transmitter_ri(hdmi);
+		} else {
+			rx = ~tx;
+			msleep(50);
+		}
+
+	} while (wait_ri && --retries && old != tx);
+
+	nvhdcp_debug("R0 Ri poll:rx=0x%04x tx=0x%04x\n", rx, tx);
+
+	if (!nvhdcp_is_plugged(nvhdcp)) {
+		nvhdcp_err("aborting verify links - lost hdmi connection\n");
+		return -EIO;
+	}
+
+	if (rx != tx)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int get_repeater_info(struct tegra_nvhdcp *nvhdcp)
+{
+	int e, retries;
+	u8 b_caps;
+	u16 b_status = 0;
+
+	nvhdcp_vdbg("repeater found:fetching repeater info\n");
+
+	/* wait up to 5 seconds for READY on repeater */
+	retries = 51;
+	do {
+		if (!nvhdcp_is_plugged(nvhdcp)) {
+			nvhdcp_err("disconnect while waiting for repeater\n");
+			return -EIO;
+		}
+
+		e = get_bcaps(nvhdcp, &b_caps);
+		if (!e && (b_caps & BCAPS_READY)) {
+			nvhdcp_debug("Bcaps READY from repeater\n");
+			break;
+		}
+		if (retries > 1)
+			msleep(100);
+	} while (--retries);
+	if (!retries) {
+		nvhdcp_err("repeater Bcaps read timeout\n");
+		return -ETIMEDOUT;
+	}
+
+	memset(nvhdcp->v_prime, 0, sizeof nvhdcp->v_prime);
+	e = get_vprime(nvhdcp, nvhdcp->v_prime);
+	if (e) {
+		nvhdcp_err("repeater Vprime read failure!\n");
+		return e;
+	}
+
+	e = nvhdcp_i2c_read16(nvhdcp, 0x41, &b_status);
+	if (e) {
+		nvhdcp_err("Bstatus read failure!\n");
+		return e;
+	}
+
+	if (b_status & BSTATUS_MAX_DEVS_EXCEEDED) {
+		nvhdcp_err("repeater:max devices (0x%04x)\n", b_status);
+		return -EINVAL;
+	}
+
+	if (b_status & BSTATUS_MAX_CASCADE_EXCEEDED) {
+		nvhdcp_err("repeater:max cascade (0x%04x)\n", b_status);
+		return -EINVAL;
+	}
+
+	nvhdcp->b_status = b_status;
+	nvhdcp->num_bksv_list = b_status & 0x7f;
+	nvhdcp_vdbg("Bstatus 0x%x (devices: %d)\n",
+				b_status, nvhdcp->num_bksv_list);
+
+	memset(nvhdcp->bksv_list, 0, sizeof nvhdcp->bksv_list);
+	e = get_ksvfifo(nvhdcp, nvhdcp->num_bksv_list, nvhdcp->bksv_list);
+	if (e) {
+		nvhdcp_err("repeater:could not read KSVFIFO (err %d)\n", e);
+		return e;
+	}
+
+	return 0;
+}
+
+static void nvhdcp_downstream_worker(struct work_struct *work)
+{
+	struct tegra_nvhdcp *nvhdcp =
+		container_of(to_delayed_work(work), struct tegra_nvhdcp, work);
+	struct tegra_dc_hdmi_data *hdmi = nvhdcp->hdmi;
+	int e;
+	u8 b_caps;
+	u32 tmp;
+	u32 res;
+
+	nvhdcp_vdbg("%s():started thread %s\n", __func__, nvhdcp->name);
+
+	mutex_lock(&nvhdcp->lock);
+	if (nvhdcp->state == STATE_OFF) {
+		nvhdcp_err("nvhdcp failure - giving up\n");
+		goto err;
+	}
+	nvhdcp->state = STATE_UNAUTHENTICATED;
+
+	/* check plug state to terminate early in case flush_workqueue() */
+	if (!nvhdcp_is_plugged(nvhdcp)) {
+		nvhdcp_err("worker started while unplugged!\n");
+		goto lost_hdmi;
+	}
+	nvhdcp_vdbg("%s():hpd=%d\n", __func__, nvhdcp->plugged);
+
+	nvhdcp->a_ksv = 0;
+	nvhdcp->b_ksv = 0;
+	nvhdcp->a_n = 0;
+
+	e = get_bcaps(nvhdcp, &b_caps);
+	if (e) {
+		nvhdcp_err("Bcaps read failure\n");
+		goto failure;
+	}
+
+	nvhdcp_vdbg("read Bcaps = 0x%02x\n", b_caps);
+
+	nvhdcp_vdbg("kfuse loading ...\n");
+
+	/* repeater flag in Bskv must be configured before loading fuses */
+	set_bksv(hdmi, 0, (b_caps & BCAPS_REPEATER));
+
+	e = load_kfuse(hdmi);
+	if (e) {
+		nvhdcp_err("kfuse could not be loaded\n");
+		goto failure;
+	}
+
+	hdcp_ctrl_run(hdmi, 1);
+
+	nvhdcp_vdbg("wait AN_VALID ...\n");
+
+	/* wait for hardware to generate HDCP values */
+	e = wait_hdcp_ctrl(hdmi, AN_VALID | SROM_ERR, &res);
+	if (e) {
+		nvhdcp_err("An key generation timeout\n");
+		goto failure;
+	}
+	if (res & SROM_ERR) {
+		nvhdcp_err("SROM error\n");
+		goto failure;
+	}
+
+	msleep(25);
+
+	nvhdcp->a_ksv = get_aksv(hdmi);
+	nvhdcp->a_n = get_an(hdmi);
+	nvhdcp_vdbg("Aksv is 0x%016llx\n", nvhdcp->a_ksv);
+	nvhdcp_vdbg("An is 0x%016llx\n", nvhdcp->a_n);
+	if (verify_ksv(nvhdcp->a_ksv)) {
+		nvhdcp_err("Aksv verify failure! (0x%016llx)\n", nvhdcp->a_ksv);
+		goto disable;
+	}
+
+	/* write Ainfo to receiver - set 1.1 only if b_caps supports it */
+	e = nvhdcp_i2c_write8(nvhdcp, 0x15, b_caps & BCAPS_11);
+	if (e) {
+		nvhdcp_err("Ainfo write failure\n");
+		goto failure;
+	}
+
+	/* write An to receiver */
+	e = nvhdcp_i2c_write64(nvhdcp, 0x18, nvhdcp->a_n);
+	if (e) {
+		nvhdcp_err("An write failure\n");
+		goto failure;
+	}
+
+	nvhdcp_vdbg("wrote An = 0x%016llx\n", nvhdcp->a_n);
+
+	/* write Aksv to receiver - triggers auth sequence */
+	e = nvhdcp_i2c_write40(nvhdcp, 0x10, nvhdcp->a_ksv);
+	if (e) {
+		nvhdcp_err("Aksv write failure\n");
+		goto failure;
+	}
+
+	nvhdcp_vdbg("wrote Aksv = 0x%010llx\n", nvhdcp->a_ksv);
+
+	/* bail out if unplugged in the middle of negotiation */
+	if (!nvhdcp_is_plugged(nvhdcp))
+		goto lost_hdmi;
+
+	/* get Bksv from receiver */
+	e = nvhdcp_i2c_read40(nvhdcp, 0x00, &nvhdcp->b_ksv);
+	if (e) {
+		nvhdcp_err("Bksv read failure\n");
+		goto failure;
+	}
+	nvhdcp_vdbg("Bksv is 0x%016llx\n", nvhdcp->b_ksv);
+	if (verify_ksv(nvhdcp->b_ksv)) {
+		nvhdcp_err("Bksv verify failure!\n");
+		goto failure;
+	}
+
+	nvhdcp_vdbg("read Bksv = 0x%010llx from device\n", nvhdcp->b_ksv);
+
+	set_bksv(hdmi, nvhdcp->b_ksv, (b_caps & BCAPS_REPEATER));
+
+	nvhdcp_vdbg("loaded Bksv into controller\n");
+
+	e = wait_hdcp_ctrl(hdmi, R0_VALID, NULL);
+	if (e) {
+		nvhdcp_err("R0 read failure!\n");
+		goto failure;
+	}
+
+	nvhdcp_vdbg("R0 valid\n");
+
+	msleep(100); /* can't read R0' within 100ms of writing Aksv */
+
+	nvhdcp_vdbg("verifying links ...\n");
+
+	e = verify_link(nvhdcp, false);
+	if (e) {
+		nvhdcp_err("link verification failed err %d\n", e);
+		goto failure;
+	}
+
+	/* if repeater then get repeater info */
+	if (b_caps & BCAPS_REPEATER) {
+		e = get_repeater_info(nvhdcp);
+		if (e) {
+			nvhdcp_err("get repeater info failed\n");
+			goto failure;
+		}
+	}
+
+	tmp = tegra_hdmi_readl(hdmi, HDMI_NV_PDISP_RG_HDCP_CTRL);
+	tmp |= CRYPT_ENABLED;
+	if (b_caps & BCAPS_11) /* HDCP 1.1 ? */
+		tmp |= ONEONE_ENABLED;
+	tegra_hdmi_writel(hdmi, tmp, HDMI_NV_PDISP_RG_HDCP_CTRL);
+
+	nvhdcp_vdbg("CRYPT enabled\n");
+
+	nvhdcp->state = STATE_LINK_VERIFY;
+	nvhdcp_info("link verified!\n");
+
+	while (1) {
+		if (!nvhdcp_is_plugged(nvhdcp))
+			goto lost_hdmi;
+
+		if (nvhdcp->state != STATE_LINK_VERIFY)
+			goto failure;
+
+		e = verify_link(nvhdcp, true);
+		if (e) {
+			nvhdcp_err("link verification failed err %d\n", e);
+			goto failure;
+		}
+		mutex_unlock(&nvhdcp->lock);
+		wait_event_interruptible_timeout(wq_worker,
+			!nvhdcp_is_plugged(nvhdcp), msecs_to_jiffies(1500));
+		mutex_lock(&nvhdcp->lock);
+
+	}
+
+failure:
+	nvhdcp->fail_count++;
+	if(nvhdcp->fail_count > 5) {
+	        nvhdcp_err("nvhdcp failure - too many failures, giving up!\n");
+	} else {
+		nvhdcp_err("nvhdcp failure - renegotiating in 1 second\n");
+		if (!nvhdcp_is_plugged(nvhdcp))
+			goto lost_hdmi;
+		queue_delayed_work(nvhdcp->downstream_wq, &nvhdcp->work,
+						msecs_to_jiffies(1000));
+	}
+
+lost_hdmi:
+	nvhdcp->state = STATE_UNAUTHENTICATED;
+	hdcp_ctrl_run(hdmi, 0);
+
+err:
+	mutex_unlock(&nvhdcp->lock);
+	return;
+disable:
+	nvhdcp->state = STATE_OFF;
+	nvhdcp_set_plugged(nvhdcp, false);
+	mutex_unlock(&nvhdcp->lock);
+	return;
+}
+
+static int tegra_nvhdcp_on(struct tegra_nvhdcp *nvhdcp)
+{
+	nvhdcp->state = STATE_UNAUTHENTICATED;
+	if (nvhdcp_is_plugged(nvhdcp)) {
+		nvhdcp->fail_count = 0;
+		queue_delayed_work(nvhdcp->downstream_wq, &nvhdcp->work,
+						msecs_to_jiffies(100));
+	}
+	return 0;
+}
+
+static int tegra_nvhdcp_off(struct tegra_nvhdcp *nvhdcp)
+{
+	mutex_lock(&nvhdcp->lock);
+	nvhdcp->state = STATE_OFF;
+	nvhdcp_set_plugged(nvhdcp, false);
+	mutex_unlock(&nvhdcp->lock);
+	wake_up_interruptible(&wq_worker);
+	flush_workqueue(nvhdcp->downstream_wq);
+	return 0;
+}
+
+void tegra_nvhdcp_set_plug(struct tegra_nvhdcp *nvhdcp, bool hpd)
+{
+	nvhdcp_debug("hdmi hotplug detected (hpd = %d)\n", hpd);
+
+	if (hpd) {
+		nvhdcp_set_plugged(nvhdcp, true);
+		tegra_nvhdcp_on(nvhdcp);
+	} else {
+		tegra_nvhdcp_off(nvhdcp);
+	}
+}
+
+int tegra_nvhdcp_set_policy(struct tegra_nvhdcp *nvhdcp, int pol)
+{
+	if (pol == TEGRA_NVHDCP_POLICY_ALWAYS_ON) {
+		nvhdcp_info("using \"always on\" policy.\n");
+		if (atomic_xchg(&nvhdcp->policy, pol) != pol) {
+			/* policy changed, start working */
+			tegra_nvhdcp_on(nvhdcp);
+		}
+	} else {
+		/* unsupported policy */
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int tegra_nvhdcp_renegotiate(struct tegra_nvhdcp *nvhdcp)
+{
+	mutex_lock(&nvhdcp->lock);
+	nvhdcp->state = STATE_RENEGOTIATE;
+	mutex_unlock(&nvhdcp->lock);
+	tegra_nvhdcp_on(nvhdcp);
+	return 0;
+}
+
+void tegra_nvhdcp_suspend(struct tegra_nvhdcp *nvhdcp)
+{
+	if (!nvhdcp) return;
+	tegra_nvhdcp_off(nvhdcp);
+}
+
+void tegra_nvhdcp_resume(struct tegra_nvhdcp *nvhdcp)
+{
+	if (!nvhdcp) return;
+	tegra_nvhdcp_renegotiate(nvhdcp);
+}
+
+static long nvhdcp_dev_ioctl(struct file *filp,
+                unsigned int cmd, unsigned long arg)
+{
+	struct tegra_nvhdcp *nvhdcp = filp->private_data;
+	struct tegra_nvhdcp_packet *pkt;
+	int e = -ENOTTY;
+
+	switch (cmd) {
+	case TEGRAIO_NVHDCP_ON:
+		return tegra_nvhdcp_on(nvhdcp);
+
+	case TEGRAIO_NVHDCP_OFF:
+		return tegra_nvhdcp_off(nvhdcp);
+
+	case TEGRAIO_NVHDCP_SET_POLICY:
+		return tegra_nvhdcp_set_policy(nvhdcp, arg);
+
+	case TEGRAIO_NVHDCP_READ_M:
+		pkt = kmalloc(sizeof(*pkt), GFP_KERNEL);
+		if (!pkt)
+			return -ENOMEM;
+		if (copy_from_user(pkt, (void __user *)arg, sizeof(*pkt))) {
+			e = -EFAULT;
+			goto kfree_pkt;
+		}
+		e = get_m_prime(nvhdcp, pkt);
+		if (copy_to_user((void __user *)arg, pkt, sizeof(*pkt))) {
+			e = -EFAULT;
+			goto kfree_pkt;
+		}
+		kfree(pkt);
+		return e;
+
+	case TEGRAIO_NVHDCP_READ_S:
+		pkt = kmalloc(sizeof(*pkt), GFP_KERNEL);
+		if (!pkt)
+			return -ENOMEM;
+		if (copy_from_user(pkt, (void __user *)arg, sizeof(*pkt))) {
+			e = -EFAULT;
+			goto kfree_pkt;
+		}
+		e = get_s_prime(nvhdcp, pkt);
+		if (copy_to_user((void __user *)arg, pkt, sizeof(*pkt))) {
+			e = -EFAULT;
+			goto kfree_pkt;
+		}
+		kfree(pkt);
+		return e;
+
+	case TEGRAIO_NVHDCP_RENEGOTIATE:
+		e = tegra_nvhdcp_renegotiate(nvhdcp);
+		break;
+	}
+
+	return e;
+kfree_pkt:
+	kfree(pkt);
+	return e;
+}
+
+static int nvhdcp_dev_open(struct inode *inode, struct file *filp)
+{
+	struct miscdevice *miscdev = filp->private_data;
+	struct tegra_nvhdcp *nvhdcp =
+		container_of(miscdev, struct tegra_nvhdcp, miscdev);
+	filp->private_data = nvhdcp;
+	return 0;
+}
+
+static int nvhdcp_dev_release(struct inode *inode, struct file *filp)
+{
+	filp->private_data = NULL;
+	return 0;
+}
+
+static const struct file_operations nvhdcp_fops = {
+	.owner          = THIS_MODULE,
+	.llseek         = no_llseek,
+	.unlocked_ioctl = nvhdcp_dev_ioctl,
+	.open           = nvhdcp_dev_open,
+	.release        = nvhdcp_dev_release,
+};
+
+/* we only support one AP right now, so should only call this once. */
+struct tegra_nvhdcp *tegra_nvhdcp_create(struct tegra_dc_hdmi_data *hdmi,
+			int id, int bus)
+{
+	static struct tegra_nvhdcp *nvhdcp; /* prevent multiple calls */
+	struct i2c_adapter *adapter;
+	int e;
+
+	if (nvhdcp)
+		return ERR_PTR(-EMFILE);
+
+	nvhdcp = kzalloc(sizeof(*nvhdcp), GFP_KERNEL);
+	if (!nvhdcp)
+		return ERR_PTR(-ENOMEM);
+
+	nvhdcp->id = id;
+	snprintf(nvhdcp->name, sizeof(nvhdcp->name), "nvhdcp%u", id);
+	nvhdcp->hdmi = hdmi;
+	mutex_init(&nvhdcp->lock);
+
+	strlcpy(nvhdcp->info.type, nvhdcp->name, sizeof(nvhdcp->info.type));
+	nvhdcp->bus = bus;
+	nvhdcp->info.addr = 0x74 >> 1;
+	nvhdcp->info.platform_data = nvhdcp;
+	nvhdcp->fail_count = 0;
+
+	adapter = i2c_get_adapter(bus);
+	if (!adapter) {
+		nvhdcp_err("can't get adapter for bus %d\n", bus);
+		e = -EBUSY;
+		goto free_nvhdcp;
+	}
+
+	nvhdcp->client = i2c_new_device(adapter, &nvhdcp->info);
+	i2c_put_adapter(adapter);
+
+	if (!nvhdcp->client) {
+		nvhdcp_err("can't create new device\n");
+		e = -EBUSY;
+		goto free_nvhdcp;
+	}
+
+	nvhdcp->state = STATE_UNAUTHENTICATED;
+
+	nvhdcp->downstream_wq = create_singlethread_workqueue(nvhdcp->name);
+	INIT_DELAYED_WORK(&nvhdcp->work, nvhdcp_downstream_worker);
+
+	nvhdcp->miscdev.minor = MISC_DYNAMIC_MINOR;
+	nvhdcp->miscdev.name = nvhdcp->name;
+	nvhdcp->miscdev.fops = &nvhdcp_fops;
+
+	e = misc_register(&nvhdcp->miscdev);
+	if (e)
+		goto free_workqueue;
+
+	nvhdcp_vdbg("%s(): created misc device %s\n", __func__, nvhdcp->name);
+
+	return nvhdcp;
+free_workqueue:
+	destroy_workqueue(nvhdcp->downstream_wq);
+	i2c_release_client(nvhdcp->client);
+free_nvhdcp:
+	kfree(nvhdcp);
+	nvhdcp_err("unable to create device.\n");
+	return ERR_PTR(e);
+}
+
+void tegra_nvhdcp_destroy(struct tegra_nvhdcp *nvhdcp)
+{
+	misc_deregister(&nvhdcp->miscdev);
+	tegra_nvhdcp_off(nvhdcp);
+	destroy_workqueue(nvhdcp->downstream_wq);
+	i2c_release_client(nvhdcp->client);
+	kfree(nvhdcp);
+}
diff --git a/drivers/video/tegra/dc/nvhdcp.h b/drivers/video/tegra/dc/nvhdcp.h
new file mode 100644
index 000000000000..ce4c7f806745
--- /dev/null
+++ b/drivers/video/tegra/dc/nvhdcp.h
@@ -0,0 +1,46 @@
+/*
+ * drivers/video/tegra/dc/nvhdcp.h
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 __DRIVERS_VIDEO_TEGRA_DC_NVHDCP_H
+#define __DRIVERS_VIDEO_TEGRA_DC_NVHDCP_H
+#include <video/nvhdcp.h>
+
+struct tegra_nvhdcp;
+#ifdef CONFIG_TEGRA_NVHDCP
+void tegra_nvhdcp_set_plug(struct tegra_nvhdcp *nvhdcp, bool hpd);
+int tegra_nvhdcp_set_policy(struct tegra_nvhdcp *nvhdcp, int pol);
+void tegra_nvhdcp_suspend(struct tegra_nvhdcp *nvhdcp);
+void tegra_nvhdcp_resume(struct tegra_nvhdcp *nvhdcp);
+struct tegra_nvhdcp *tegra_nvhdcp_create(struct tegra_dc_hdmi_data *hdmi,
+					int id, int bus);
+void tegra_nvhdcp_destroy(struct tegra_nvhdcp *nvhdcp);
+#else
+inline void tegra_nvhdcp_set_plug(struct tegra_nvhdcp *nvhdcp, bool hpd) { }
+inline int tegra_nvhdcp_set_policy(struct tegra_nvhdcp *nvhdcp, int pol)
+{
+	return 0;
+}
+inline void tegra_nvhdcp_suspend(struct tegra_nvhdcp *nvhdcp) { }
+inline void tegra_nvhdcp_resume(struct tegra_nvhdcp *nvhdcp) { }
+inline struct tegra_nvhdcp *tegra_nvhdcp_create(struct tegra_dc_hdmi_data *hdmi,
+					int id, int bus)
+{
+	return NULL;
+}
+inline void tegra_nvhdcp_destroy(struct tegra_nvhdcp *nvhdcp) { }
+#endif
+
+#endif
diff --git a/drivers/video/tegra/dc/nvsd.c b/drivers/video/tegra/dc/nvsd.c
new file mode 100644
index 000000000000..6e76ee0f1702
--- /dev/null
+++ b/drivers/video/tegra/dc/nvsd.c
@@ -0,0 +1,913 @@
+/*
+ * drivers/video/tegra/dc/nvsd.c
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/kernel.h>
+#include <mach/dc.h>
+#include <linux/types.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+#include <linux/backlight.h>
+#include <linux/platform_device.h>
+
+#include "dc_reg.h"
+#include "dc_priv.h"
+#include "nvsd.h"
+
+/* Elements for sysfs access */
+#define NVSD_ATTR(__name) static struct kobj_attribute nvsd_attr_##__name = \
+	__ATTR(__name, S_IRUGO|S_IWUSR, nvsd_settings_show, nvsd_settings_store)
+#define NVSD_ATTRS_ENTRY(__name) (&nvsd_attr_##__name.attr)
+#define IS_NVSD_ATTR(__name) (attr == &nvsd_attr_##__name)
+
+static ssize_t nvsd_settings_show(struct kobject *kobj,
+	struct kobj_attribute *attr, char *buf);
+
+static ssize_t nvsd_settings_store(struct kobject *kobj,
+	struct kobj_attribute *attr, const char *buf, size_t count);
+
+static ssize_t nvsd_registers_show(struct kobject *kobj,
+	struct kobj_attribute *attr, char *buf);
+
+NVSD_ATTR(enable);
+NVSD_ATTR(aggressiveness);
+NVSD_ATTR(phase_in_settings);
+NVSD_ATTR(phase_in_adjustments);
+NVSD_ATTR(bin_width);
+NVSD_ATTR(hw_update_delay);
+NVSD_ATTR(use_vid_luma);
+NVSD_ATTR(coeff);
+NVSD_ATTR(blp_time_constant);
+NVSD_ATTR(blp_step);
+NVSD_ATTR(fc_time_limit);
+NVSD_ATTR(fc_threshold);
+NVSD_ATTR(lut);
+NVSD_ATTR(bltf);
+static struct kobj_attribute nvsd_attr_registers =
+	__ATTR(registers, S_IRUGO, nvsd_registers_show, NULL);
+
+static struct attribute *nvsd_attrs[] = {
+	NVSD_ATTRS_ENTRY(enable),
+	NVSD_ATTRS_ENTRY(aggressiveness),
+	NVSD_ATTRS_ENTRY(phase_in_settings),
+	NVSD_ATTRS_ENTRY(phase_in_adjustments),
+	NVSD_ATTRS_ENTRY(bin_width),
+	NVSD_ATTRS_ENTRY(hw_update_delay),
+	NVSD_ATTRS_ENTRY(use_vid_luma),
+	NVSD_ATTRS_ENTRY(coeff),
+	NVSD_ATTRS_ENTRY(blp_time_constant),
+	NVSD_ATTRS_ENTRY(blp_step),
+	NVSD_ATTRS_ENTRY(fc_time_limit),
+	NVSD_ATTRS_ENTRY(fc_threshold),
+	NVSD_ATTRS_ENTRY(lut),
+	NVSD_ATTRS_ENTRY(bltf),
+	NVSD_ATTRS_ENTRY(registers),
+	NULL,
+};
+
+static struct attribute_group nvsd_attr_group = {
+	.attrs = nvsd_attrs,
+};
+
+static struct kobject *nvsd_kobj;
+
+/* shared brightness variable */
+static atomic_t *sd_brightness = NULL;
+/* shared boolean for manual K workaround */
+static atomic_t man_k_until_blank = ATOMIC_INIT(0);
+
+static u8 nvsd_get_bw_idx(struct tegra_dc_sd_settings *settings)
+{
+	u8 bw;
+
+	switch (settings->bin_width) {
+	default:
+	case -1:
+	/* A -1 bin-width indicates 'automatic'
+	   based upon aggressiveness. */
+		settings->bin_width = -1;
+		switch (settings->aggressiveness) {
+		default:
+		case 0:
+		case 1:
+			bw = SD_BIN_WIDTH_ONE;
+			break;
+		case 2:
+		case 3:
+		case 4:
+			bw = SD_BIN_WIDTH_TWO;
+			break;
+		case 5:
+			bw = SD_BIN_WIDTH_FOUR;
+			break;
+		}
+		break;
+	case 1:
+		bw = SD_BIN_WIDTH_ONE;
+		break;
+	case 2:
+		bw = SD_BIN_WIDTH_TWO;
+		break;
+	case 4:
+		bw = SD_BIN_WIDTH_FOUR;
+		break;
+	case 8:
+		bw = SD_BIN_WIDTH_EIGHT;
+		break;
+	}
+	return bw >> 3;
+
+}
+
+static bool nvsd_phase_in_adjustments(struct tegra_dc *dc,
+	struct tegra_dc_sd_settings *settings)
+{
+	u8 step, cur_sd_brightness;
+	u16 target_k, cur_k;
+	u32 man_k, val;
+
+	cur_sd_brightness = atomic_read(sd_brightness);
+
+	target_k = tegra_dc_readl(dc, DC_DISP_SD_HW_K_VALUES);
+	target_k = SD_HW_K_R(target_k);
+	cur_k = tegra_dc_readl(dc, DC_DISP_SD_MAN_K_VALUES);
+	cur_k = SD_HW_K_R(cur_k);
+
+	/* read brightness value */
+	val = tegra_dc_readl(dc, DC_DISP_SD_BL_CONTROL);
+	val = SD_BLC_BRIGHTNESS(val);
+
+	step = settings->phase_adj_step;
+	if (cur_sd_brightness != val || target_k != cur_k) {
+		if (!step)
+			step = ADJ_PHASE_STEP;
+
+		/* Phase in Backlight and Pixel K
+		every ADJ_PHASE_STEP frames*/
+		if ((step-- & ADJ_PHASE_STEP) == ADJ_PHASE_STEP) {
+
+			if (val != cur_sd_brightness) {
+				val > cur_sd_brightness ?
+				(cur_sd_brightness++) :
+				(cur_sd_brightness--);
+			}
+
+			if (target_k != cur_k) {
+				if (target_k > cur_k)
+					cur_k += K_STEP;
+				else
+					cur_k -= K_STEP;
+			}
+
+			/* Set manual k value */
+			man_k = SD_MAN_K_R(cur_k) |
+				SD_MAN_K_G(cur_k) | SD_MAN_K_B(cur_k);
+			tegra_dc_writel(dc, man_k, DC_DISP_SD_MAN_K_VALUES);
+			/* Set manual brightness value */
+			atomic_set(sd_brightness, cur_sd_brightness);
+		}
+		settings->phase_adj_step = step;
+		return true;
+	} else
+		return false;
+}
+
+/* phase in the luts based on the current and max step */
+static void nvsd_phase_in_luts(struct tegra_dc_sd_settings *settings,
+	struct tegra_dc *dc)
+{
+	u32 val;
+	u8 bw_idx;
+	int i;
+	u16 phase_settings_step = settings->phase_settings_step;
+	u16 num_phase_in_steps = settings->num_phase_in_steps;
+
+	bw_idx = nvsd_get_bw_idx(settings);
+
+	/* Phase in Final LUT */
+	for (i = 0; i < DC_DISP_SD_LUT_NUM; i++) {
+		val = SD_LUT_R((settings->lut[bw_idx][i].r *
+				phase_settings_step)/num_phase_in_steps) |
+			SD_LUT_G((settings->lut[bw_idx][i].g *
+				phase_settings_step)/num_phase_in_steps) |
+			SD_LUT_B((settings->lut[bw_idx][i].b *
+				phase_settings_step)/num_phase_in_steps);
+
+		tegra_dc_writel(dc, val, DC_DISP_SD_LUT(i));
+	}
+	/* Phase in Final BLTF */
+	for (i = 0; i < DC_DISP_SD_BL_TF_NUM; i++) {
+		val = SD_BL_TF_POINT_0(255-((255-settings->bltf[bw_idx][i][0])
+				* phase_settings_step)/num_phase_in_steps) |
+			SD_BL_TF_POINT_1(255-((255-settings->bltf[bw_idx][i][1])
+				* phase_settings_step)/num_phase_in_steps) |
+			SD_BL_TF_POINT_2(255-((255-settings->bltf[bw_idx][i][2])
+				* phase_settings_step)/num_phase_in_steps) |
+			SD_BL_TF_POINT_3(255-((255-settings->bltf[bw_idx][i][3])
+				* phase_settings_step)/num_phase_in_steps);
+
+		tegra_dc_writel(dc, val, DC_DISP_SD_BL_TF(i));
+	}
+}
+
+/* handle the commands that may be invoked for phase_in_settings */
+static void nvsd_cmd_handler(struct tegra_dc_sd_settings *settings,
+	struct tegra_dc *dc)
+{
+	u32 val;
+	u8 bw_idx, bw;
+
+	if (settings->cmd & ENABLE) {
+		settings->phase_settings_step++;
+		if (settings->phase_settings_step >=
+				settings->num_phase_in_steps)
+			settings->cmd &= ~ENABLE;
+
+		nvsd_phase_in_luts(settings, dc);
+	}
+	if (settings->cmd & DISABLE) {
+		settings->phase_settings_step--;
+		nvsd_phase_in_luts(settings, dc);
+		if (settings->phase_settings_step == 0) {
+			/* finish up aggressiveness phase in */
+			if (settings->cmd & AGG_CHG)
+				settings->aggressiveness = settings->final_agg;
+			settings->cmd = NO_CMD;
+			settings->enable = 0;
+			nvsd_init(dc, settings);
+		}
+	}
+	if (settings->cmd & AGG_CHG) {
+		if (settings->aggressiveness == settings->final_agg)
+			settings->cmd &= ~AGG_CHG;
+		if ((settings->cur_agg_step++ & (STEPS_PER_AGG_CHG - 1)) == 0) {
+			settings->final_agg > settings->aggressiveness ?
+				settings->aggressiveness++ :
+				settings->aggressiveness--;
+
+			/* Update aggressiveness value in HW */
+			val = tegra_dc_readl(dc, DC_DISP_SD_CONTROL);
+			val &= ~SD_AGGRESSIVENESS(0x7);
+			val |= SD_AGGRESSIVENESS(settings->aggressiveness);
+
+			/* Adjust bin_width for automatic setting */
+			if (settings->bin_width == -1) {
+				bw_idx = nvsd_get_bw_idx(settings);
+
+				bw = bw_idx << 3;
+
+				val &= ~SD_BIN_WIDTH_MASK;
+				val |= bw;
+			}
+			tegra_dc_writel(dc, val, DC_DISP_SD_CONTROL);
+
+			nvsd_phase_in_luts(settings, dc);
+		}
+	}
+}
+
+static bool nvsd_update_enable(struct tegra_dc_sd_settings *settings,
+	int enable_val)
+{
+
+	if (enable_val != 1 && enable_val != 0)
+		return false;
+
+	if (!settings->cmd && settings->enable != enable_val) {
+		settings->num_phase_in_steps =
+			STEPS_PER_AGG_LVL*settings->aggressiveness;
+		settings->phase_settings_step = enable_val ?
+			0 : settings->num_phase_in_steps;
+	}
+
+	if (settings->enable != enable_val || settings->cmd & DISABLE) {
+		settings->cmd &= ~(ENABLE | DISABLE);
+		if (!settings->enable && enable_val)
+			settings->cmd |= PHASE_IN;
+		settings->cmd |= enable_val ? ENABLE : DISABLE;
+		return true;
+	}
+
+	return false;
+}
+
+static bool nvsd_update_agg(struct tegra_dc_sd_settings *settings, int agg_val)
+{
+	int i;
+	int pri_lvl = SD_AGG_PRI_LVL(agg_val);
+	int agg_lvl = SD_GET_AGG(agg_val);
+	struct tegra_dc_sd_agg_priorities *sd_agg_priorities =
+		&settings->agg_priorities;
+
+	if (agg_lvl > 5 || agg_lvl < 0)
+		return false;
+	else if (agg_lvl == 0 && pri_lvl == 0)
+		return false;
+
+	if (pri_lvl >= 0 && pri_lvl < 4)
+		sd_agg_priorities->agg[pri_lvl] = agg_lvl;
+
+	for (i = NUM_AGG_PRI_LVLS - 1; i >= 0; i--) {
+		if (sd_agg_priorities->agg[i])
+			break;
+	}
+
+	sd_agg_priorities->pri_lvl = i;
+	pri_lvl = i;
+	agg_lvl = sd_agg_priorities->agg[i];
+
+	if (settings->phase_in_settings && settings->enable &&
+		settings->aggressiveness != agg_lvl) {
+
+		settings->final_agg = agg_lvl;
+		settings->cmd |= AGG_CHG;
+		settings->cur_agg_step = 0;
+		return true;
+	} else if (settings->aggressiveness != agg_lvl) {
+		settings->aggressiveness = agg_lvl;
+		return true;
+	}
+
+	return false;
+}
+
+/* Functional initialization */
+void nvsd_init(struct tegra_dc *dc, struct tegra_dc_sd_settings *settings)
+{
+	u32 i = 0;
+	u32 val = 0;
+	u32 bw_idx = 0;
+	/* TODO: check if HW says SD's available */
+
+	/* If SD's not present or disabled, clear the register and return. */
+	if (!settings || settings->enable == 0) {
+		/* clear the brightness val, too. */
+		if (sd_brightness)
+			atomic_set(sd_brightness, 255);
+
+		sd_brightness = NULL;
+
+		if (settings)
+			settings->phase_settings_step = 0;
+		tegra_dc_writel(dc, 0, DC_DISP_SD_CONTROL);
+		return;
+	}
+
+	dev_dbg(&dc->ndev->dev, "NVSD Init:\n");
+
+	/* init agg_priorities */
+	if (!settings->agg_priorities.agg[0])
+		settings->agg_priorities.agg[0] = settings->aggressiveness;
+
+	/* WAR: Settings will not be valid until the next flip.
+	 * Thus, set manual K to either HW's current value (if
+	 * we're already enabled) or a non-effective value (if
+	 * we're about to enable). */
+	val = tegra_dc_readl(dc, DC_DISP_SD_CONTROL);
+
+	if (val & SD_ENABLE_NORMAL)
+		if (settings->phase_in_adjustments)
+			i = tegra_dc_readl(dc, DC_DISP_SD_MAN_K_VALUES);
+		else
+			i = tegra_dc_readl(dc, DC_DISP_SD_HW_K_VALUES);
+	else
+		i = 0; /* 0 values for RGB = 1.0, i.e. non-affected */
+
+	tegra_dc_writel(dc, i, DC_DISP_SD_MAN_K_VALUES);
+	/* Enable manual correction mode here so that changing the
+	 * settings won't immediately impact display dehavior. */
+	val |= SD_CORRECTION_MODE_MAN;
+	tegra_dc_writel(dc, val, DC_DISP_SD_CONTROL);
+
+	bw_idx = nvsd_get_bw_idx(settings);
+
+	/* Write LUT */
+	if (!settings->cmd) {
+		dev_dbg(&dc->ndev->dev, "  LUT:\n");
+
+		for (i = 0; i < DC_DISP_SD_LUT_NUM; i++) {
+			val = SD_LUT_R(settings->lut[bw_idx][i].r) |
+				SD_LUT_G(settings->lut[bw_idx][i].g) |
+				SD_LUT_B(settings->lut[bw_idx][i].b);
+			tegra_dc_writel(dc, val, DC_DISP_SD_LUT(i));
+
+			dev_dbg(&dc->ndev->dev, "    %d: 0x%08x\n", i, val);
+		}
+	}
+
+	/* Write BL TF */
+	if (!settings->cmd) {
+		dev_dbg(&dc->ndev->dev, "  BL_TF:\n");
+
+		for (i = 0; i < DC_DISP_SD_BL_TF_NUM; i++) {
+			val = SD_BL_TF_POINT_0(settings->bltf[bw_idx][i][0]) |
+				SD_BL_TF_POINT_1(settings->bltf[bw_idx][i][1]) |
+				SD_BL_TF_POINT_2(settings->bltf[bw_idx][i][2]) |
+				SD_BL_TF_POINT_3(settings->bltf[bw_idx][i][3]);
+
+			tegra_dc_writel(dc, val, DC_DISP_SD_BL_TF(i));
+
+			dev_dbg(&dc->ndev->dev, "    %d: 0x%08x\n", i, val);
+		}
+	} else if ((settings->cmd & PHASE_IN)) {
+		settings->cmd &= ~PHASE_IN;
+		/* Write NO_OP values for BLTF */
+		for (i = 0; i < DC_DISP_SD_BL_TF_NUM; i++) {
+			val = SD_BL_TF_POINT_0(0xFF) |
+				SD_BL_TF_POINT_1(0xFF) |
+				SD_BL_TF_POINT_2(0xFF) |
+				SD_BL_TF_POINT_3(0xFF);
+
+			tegra_dc_writel(dc, val, DC_DISP_SD_BL_TF(i));
+
+			dev_dbg(&dc->ndev->dev, "    %d: 0x%08x\n", i, val);
+		}
+	}
+
+	/* Set step correctly on init */
+	if (!settings->cmd && settings->phase_in_settings) {
+		settings->num_phase_in_steps = STEPS_PER_AGG_LVL *
+			settings->aggressiveness;
+		settings->phase_settings_step = settings->enable ?
+			settings->num_phase_in_steps : 0;
+	}
+
+	/* Write Coeff */
+	val = SD_CSC_COEFF_R(settings->coeff.r) |
+		SD_CSC_COEFF_G(settings->coeff.g) |
+		SD_CSC_COEFF_B(settings->coeff.b);
+	tegra_dc_writel(dc, val, DC_DISP_SD_CSC_COEFF);
+	dev_dbg(&dc->ndev->dev, "  COEFF: 0x%08x\n", val);
+
+	/* Write BL Params */
+	val = SD_BLP_TIME_CONSTANT(settings->blp.time_constant) |
+		SD_BLP_STEP(settings->blp.step);
+	tegra_dc_writel(dc, val, DC_DISP_SD_BL_PARAMETERS);
+	dev_dbg(&dc->ndev->dev, "  BLP: 0x%08x\n", val);
+
+	/* Write Auto/Manual PWM */
+	val = (settings->use_auto_pwm) ? SD_BLC_MODE_AUTO : SD_BLC_MODE_MAN;
+	tegra_dc_writel(dc, val, DC_DISP_SD_BL_CONTROL);
+	dev_dbg(&dc->ndev->dev, "  BL_CONTROL: 0x%08x\n", val);
+
+	/* Write Flicker Control */
+	val = SD_FC_TIME_LIMIT(settings->fc.time_limit) |
+		SD_FC_THRESHOLD(settings->fc.threshold);
+	tegra_dc_writel(dc, val, DC_DISP_SD_FLICKER_CONTROL);
+	dev_dbg(&dc->ndev->dev, "  FLICKER_CONTROL: 0x%08x\n", val);
+
+	/* Manage SD Control */
+	val = 0;
+	/* Stay in manual correction mode until the next flip. */
+	val |= SD_CORRECTION_MODE_MAN;
+	/* Enable / One-Shot */
+	val |= (settings->enable == 2) ?
+		(SD_ENABLE_ONESHOT | SD_ONESHOT_ENABLE) :
+		SD_ENABLE_NORMAL;
+	/* HW Update Delay */
+	val |= SD_HW_UPDATE_DLY(settings->hw_update_delay);
+	/* Video Luma */
+	val |= (settings->use_vid_luma) ? SD_USE_VID_LUMA : 0;
+	/* Aggressiveness */
+	val |= SD_AGGRESSIVENESS(settings->aggressiveness);
+	/* Bin Width (value derived from bw_idx) */
+	val |= bw_idx << 3;
+	/* Finally, Write SD Control */
+	tegra_dc_writel(dc, val, DC_DISP_SD_CONTROL);
+	dev_dbg(&dc->ndev->dev, "  SD_CONTROL: 0x%08x\n", val);
+
+	/* set the brightness pointer */
+	sd_brightness = settings->sd_brightness;
+
+	/* note that we're in manual K until the next flip */
+	atomic_set(&man_k_until_blank, 1);
+}
+
+/* Periodic update */
+bool nvsd_update_brightness(struct tegra_dc *dc)
+{
+	u32 val = 0;
+	int cur_sd_brightness;
+	struct tegra_dc_sd_settings *settings = dc->out->sd_settings;
+
+	if (sd_brightness) {
+		if (atomic_read(&man_k_until_blank) &&
+					!settings->phase_in_adjustments) {
+			val = tegra_dc_readl(dc, DC_DISP_SD_CONTROL);
+			val &= ~SD_CORRECTION_MODE_MAN;
+			tegra_dc_writel(dc, val, DC_DISP_SD_CONTROL);
+			atomic_set(&man_k_until_blank, 0);
+		}
+
+		if (settings->cmd)
+			nvsd_cmd_handler(settings, dc);
+
+		/* nvsd_cmd_handler may turn off didim */
+		if (!settings->enable)
+			return true;
+
+		cur_sd_brightness = atomic_read(sd_brightness);
+
+		/* read brightness value */
+		val = tegra_dc_readl(dc, DC_DISP_SD_BL_CONTROL);
+		val = SD_BLC_BRIGHTNESS(val);
+
+		if (settings->phase_in_adjustments) {
+			return nvsd_phase_in_adjustments(dc, settings);
+		} else if (val != (u32)cur_sd_brightness) {
+			/* set brightness value and note the update */
+			atomic_set(sd_brightness, (int)val);
+			return true;
+		}
+	}
+
+	/* No update needed. */
+	return false;
+}
+
+static ssize_t nvsd_lut_show(struct tegra_dc_sd_settings *sd_settings,
+	char *buf, ssize_t res)
+{
+	u32 i;
+	u32 j;
+
+	for (i = 0; i < NUM_BIN_WIDTHS; i++) {
+		res += snprintf(buf + res, PAGE_SIZE - res,
+			"Bin Width: %d\n", 1 << i);
+
+		for (j = 0; j < DC_DISP_SD_LUT_NUM; j++) {
+			res += snprintf(buf + res,
+				PAGE_SIZE - res,
+				"%d: R: %3d / G: %3d / B: %3d\n",
+				j,
+				sd_settings->lut[i][j].r,
+				sd_settings->lut[i][j].g,
+				sd_settings->lut[i][j].b);
+		}
+	}
+	return res;
+}
+
+static ssize_t nvsd_bltf_show(struct tegra_dc_sd_settings *sd_settings,
+	char *buf, ssize_t res)
+{
+	u32 i;
+	u32 j;
+
+	for (i = 0; i < NUM_BIN_WIDTHS; i++) {
+		res += snprintf(buf + res, PAGE_SIZE - res,
+			"Bin Width: %d\n", 1 << i);
+
+		for (j = 0; j < DC_DISP_SD_BL_TF_NUM; j++) {
+			res += snprintf(buf + res,
+				PAGE_SIZE - res,
+				"%d: 0: %3d / 1: %3d / 2: %3d / 3: %3d\n",
+				j,
+				sd_settings->bltf[i][j][0],
+				sd_settings->bltf[i][j][1],
+				sd_settings->bltf[i][j][2],
+				sd_settings->bltf[i][j][3]);
+		}
+	}
+	return res;
+}
+
+/* Sysfs accessors */
+static ssize_t nvsd_settings_show(struct kobject *kobj,
+	struct kobj_attribute *attr, char *buf)
+{
+	struct device *dev = container_of((kobj->parent), struct device, kobj);
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	struct tegra_dc_sd_settings *sd_settings = dc->out->sd_settings;
+	ssize_t res = 0;
+
+	if (sd_settings) {
+		if (IS_NVSD_ATTR(enable))
+			res = snprintf(buf, PAGE_SIZE, "%d\n",
+				sd_settings->enable);
+		else if (IS_NVSD_ATTR(aggressiveness))
+			res = snprintf(buf, PAGE_SIZE, "%d\n",
+				sd_settings->aggressiveness);
+		else if (IS_NVSD_ATTR(phase_in_settings))
+			res = snprintf(buf, PAGE_SIZE, "%d\n",
+				sd_settings->phase_in_settings);
+		else if (IS_NVSD_ATTR(phase_in_adjustments))
+			res = snprintf(buf, PAGE_SIZE, "%d\n",
+				sd_settings->phase_in_adjustments);
+		else if (IS_NVSD_ATTR(bin_width))
+			res = snprintf(buf, PAGE_SIZE, "%d\n",
+				sd_settings->bin_width);
+		else if (IS_NVSD_ATTR(hw_update_delay))
+			res = snprintf(buf, PAGE_SIZE, "%d\n",
+				sd_settings->hw_update_delay);
+		else if (IS_NVSD_ATTR(use_vid_luma))
+			res = snprintf(buf, PAGE_SIZE, "%d\n",
+				sd_settings->use_vid_luma);
+		else if (IS_NVSD_ATTR(coeff))
+			res = snprintf(buf, PAGE_SIZE,
+				"R: %d / G: %d / B: %d\n",
+				sd_settings->coeff.r,
+				sd_settings->coeff.g,
+				sd_settings->coeff.b);
+		else if (IS_NVSD_ATTR(blp_time_constant))
+			res = snprintf(buf, PAGE_SIZE, "%d\n",
+				sd_settings->blp.time_constant);
+		else if (IS_NVSD_ATTR(blp_step))
+			res = snprintf(buf, PAGE_SIZE, "%d\n",
+				sd_settings->blp.step);
+		else if (IS_NVSD_ATTR(fc_time_limit))
+			res = snprintf(buf, PAGE_SIZE, "%d\n",
+				sd_settings->fc.time_limit);
+		else if (IS_NVSD_ATTR(fc_threshold))
+			res = snprintf(buf, PAGE_SIZE, "%d\n",
+				sd_settings->fc.threshold);
+		else if (IS_NVSD_ATTR(lut))
+			res = nvsd_lut_show(sd_settings, buf, res);
+		else if (IS_NVSD_ATTR(bltf))
+			res = nvsd_bltf_show(sd_settings, buf, res);
+		else
+			res = -EINVAL;
+	} else {
+		/* This shouldn't be reachable. But just in case... */
+		res = -EINVAL;
+	}
+
+	return res;
+}
+
+#define nvsd_check_and_update(_min, _max, _varname) { \
+	int val = simple_strtol(buf, NULL, 10); \
+	if (val >= _min && val <= _max) { \
+		sd_settings->_varname = val; \
+		settings_updated = true; \
+	} }
+
+#define nvsd_get_multi(_ele, _num, _act, _min, _max) { \
+	char *b, *c, *orig_b; \
+	b = orig_b = kstrdup(buf, GFP_KERNEL); \
+	for (_act = 0; _act < _num; _act++) { \
+		if (!b) \
+			break; \
+		b = strim(b); \
+		c = strsep(&b, " "); \
+		if (!strlen(c)) \
+			break; \
+		_ele[_act] = simple_strtol(c, NULL, 10); \
+		if (_ele[_act] < _min || _ele[_act] > _max) \
+			break; \
+	} \
+	if (orig_b) \
+		kfree(orig_b); \
+}
+
+static int nvsd_lut_store(struct tegra_dc_sd_settings *sd_settings,
+	const char *buf)
+{
+	int ele[3 * DC_DISP_SD_LUT_NUM * NUM_BIN_WIDTHS];
+	int i = 0;
+	int j = 0;
+	int num = 3 * DC_DISP_SD_LUT_NUM * NUM_BIN_WIDTHS;
+
+	nvsd_get_multi(ele, num, i, 0, 255);
+
+	if (i != num)
+		return -EINVAL;
+
+	for (i = 0; i < NUM_BIN_WIDTHS; i++) {
+		for (j = 0; j < DC_DISP_SD_LUT_NUM; j++) {
+			sd_settings->lut[i][j].r =
+				ele[i * NUM_BIN_WIDTHS + j * 3 + 0];
+			sd_settings->lut[i][j].g =
+				ele[i * NUM_BIN_WIDTHS + j * 3 + 1];
+			sd_settings->lut[i][j].b =
+				ele[i * NUM_BIN_WIDTHS + j * 3 + 2];
+		}
+	}
+	return 0;
+}
+
+static int nvsd_bltf_store(struct tegra_dc_sd_settings *sd_settings,
+	const char *buf)
+{
+	int ele[4 * DC_DISP_SD_BL_TF_NUM * NUM_BIN_WIDTHS];
+	int i = 0, j = 0, num = ARRAY_SIZE(ele);
+
+	nvsd_get_multi(ele, num, i, 0, 255);
+
+	if (i != num)
+		return -EINVAL;
+
+	for (i = 0; i < NUM_BIN_WIDTHS; i++) {
+		for (j = 0; j < DC_DISP_SD_BL_TF_NUM; j++) {
+			size_t base = (i * NUM_BIN_WIDTHS *
+				       DC_DISP_SD_BL_TF_NUM) + (j * 4);
+			sd_settings->bltf[i][j][0] = ele[base + 0];
+			sd_settings->bltf[i][j][1] = ele[base + 1];
+			sd_settings->bltf[i][j][2] = ele[base + 2];
+			sd_settings->bltf[i][j][3] = ele[base + 3];
+		}
+	}
+
+	return 0;
+}
+
+static ssize_t nvsd_settings_store(struct kobject *kobj,
+	struct kobj_attribute *attr, const char *buf, size_t count)
+{
+	struct device *dev = container_of((kobj->parent), struct device, kobj);
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	struct tegra_dc_sd_settings *sd_settings = dc->out->sd_settings;
+	ssize_t res = count;
+	bool settings_updated = false;
+	long int result;
+	int err;
+
+	if (sd_settings) {
+		if (IS_NVSD_ATTR(enable)) {
+			if (sd_settings->phase_in_settings) {
+				err = strict_strtol(buf, 10, &result);
+				if (err)
+					return err;
+
+				if (nvsd_update_enable(sd_settings, result))
+					nvsd_check_and_update(1, 1, enable);
+
+			} else {
+				nvsd_check_and_update(0, 1, enable);
+			}
+		} else if (IS_NVSD_ATTR(aggressiveness)) {
+			err = strict_strtol(buf, 10, &result);
+			if (err)
+				return err;
+
+			if (nvsd_update_agg(sd_settings, result)
+					&& !sd_settings->phase_in_settings)
+				settings_updated = true;
+
+		} else if (IS_NVSD_ATTR(phase_in_settings)) {
+			nvsd_check_and_update(0, 1, phase_in_settings);
+		} else if (IS_NVSD_ATTR(phase_in_adjustments)) {
+			nvsd_check_and_update(0, 1, phase_in_adjustments);
+		} else if (IS_NVSD_ATTR(bin_width)) {
+			nvsd_check_and_update(0, 8, bin_width);
+		} else if (IS_NVSD_ATTR(hw_update_delay)) {
+			nvsd_check_and_update(0, 2, hw_update_delay);
+		} else if (IS_NVSD_ATTR(use_vid_luma)) {
+			nvsd_check_and_update(0, 1, use_vid_luma);
+		} else if (IS_NVSD_ATTR(coeff)) {
+			int ele[3], i = 0, num = 3;
+			nvsd_get_multi(ele, num, i, 0, 15);
+
+			if (i == num) {
+				sd_settings->coeff.r = ele[0];
+				sd_settings->coeff.g = ele[1];
+				sd_settings->coeff.b = ele[2];
+				settings_updated = true;
+			} else {
+				res = -EINVAL;
+			}
+		} else if (IS_NVSD_ATTR(blp_time_constant)) {
+			nvsd_check_and_update(0, 1024, blp.time_constant);
+		} else if (IS_NVSD_ATTR(blp_step)) {
+			nvsd_check_and_update(0, 255, blp.step);
+		} else if (IS_NVSD_ATTR(fc_time_limit)) {
+			nvsd_check_and_update(0, 255, fc.time_limit);
+		} else if (IS_NVSD_ATTR(fc_threshold)) {
+			nvsd_check_and_update(0, 255, fc.threshold);
+		} else if (IS_NVSD_ATTR(lut)) {
+			if (nvsd_lut_store(sd_settings, buf))
+				res = -EINVAL;
+			else
+				settings_updated = true;
+		} else if (IS_NVSD_ATTR(bltf)) {
+			if (nvsd_bltf_store(sd_settings, buf))
+				res = -EINVAL;
+			else
+				settings_updated = true;
+		} else {
+			res = -EINVAL;
+		}
+
+		/* Re-init if our settings were updated. */
+		if (settings_updated) {
+			mutex_lock(&dc->lock);
+			if (!dc->enabled) {
+				mutex_unlock(&dc->lock);
+				return -ENODEV;
+			}
+
+			tegra_dc_hold_dc_out(dc);
+			nvsd_init(dc, sd_settings);
+			tegra_dc_release_dc_out(dc);
+
+			mutex_unlock(&dc->lock);
+
+			/* Update backlight state IFF we're disabling! */
+			if (!sd_settings->enable && sd_settings->bl_device) {
+				/* Do the actual brightness update outside of
+				 * the mutex */
+				struct platform_device *pdev =
+					sd_settings->bl_device;
+				struct backlight_device *bl =
+					platform_get_drvdata(pdev);
+
+				if (bl)
+					backlight_update_status(bl);
+			}
+		}
+	} else {
+		/* This shouldn't be reachable. But just in case... */
+		res = -EINVAL;
+	}
+
+	return res;
+}
+
+#define NVSD_PRINT_REG(__name) { \
+	u32 val = tegra_dc_readl(dc, __name); \
+	res += snprintf(buf + res, PAGE_SIZE - res, #__name ": 0x%08x\n", \
+		val); \
+}
+
+#define NVSD_PRINT_REG_ARRAY(__name) { \
+	u32 val = 0, i = 0; \
+	res += snprintf(buf + res, PAGE_SIZE - res, #__name ":\n"); \
+	for (i = 0; i < __name##_NUM; i++) { \
+		val = tegra_dc_readl(dc, __name(i)); \
+		res += snprintf(buf + res, PAGE_SIZE - res, "  %d: 0x%08x\n", \
+			i, val); \
+	} \
+}
+
+static ssize_t nvsd_registers_show(struct kobject *kobj,
+	struct kobj_attribute *attr, char *buf)
+{
+	struct device *dev = container_of((kobj->parent), struct device, kobj);
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	struct tegra_dc *dc = nvhost_get_drvdata(ndev);
+	ssize_t res = 0;
+
+	mutex_lock(&dc->lock);
+	if (!dc->enabled) {
+		mutex_unlock(&dc->lock);
+		return -ENODEV;
+	}
+
+	mutex_unlock(&dc->lock);
+	NVSD_PRINT_REG(DC_DISP_SD_CONTROL);
+	NVSD_PRINT_REG(DC_DISP_SD_CSC_COEFF);
+	NVSD_PRINT_REG_ARRAY(DC_DISP_SD_LUT);
+	NVSD_PRINT_REG(DC_DISP_SD_FLICKER_CONTROL);
+	NVSD_PRINT_REG(DC_DISP_SD_PIXEL_COUNT);
+	NVSD_PRINT_REG_ARRAY(DC_DISP_SD_HISTOGRAM);
+	NVSD_PRINT_REG(DC_DISP_SD_BL_PARAMETERS);
+	NVSD_PRINT_REG_ARRAY(DC_DISP_SD_BL_TF);
+	NVSD_PRINT_REG(DC_DISP_SD_BL_CONTROL);
+	NVSD_PRINT_REG(DC_DISP_SD_HW_K_VALUES);
+	NVSD_PRINT_REG(DC_DISP_SD_MAN_K_VALUES);
+
+	return res;
+}
+
+/* Sysfs initializer */
+int nvsd_create_sysfs(struct device *dev)
+{
+	int retval = 0;
+
+	nvsd_kobj = kobject_create_and_add("smartdimmer", &dev->kobj);
+
+	if (!nvsd_kobj)
+		return -ENOMEM;
+
+	retval = sysfs_create_group(nvsd_kobj, &nvsd_attr_group);
+
+	if (retval) {
+		kobject_put(nvsd_kobj);
+		dev_err(dev, "%s: failed to create attributes\n", __func__);
+	}
+
+	return retval;
+}
+
+/* Sysfs destructor */
+void __devexit nvsd_remove_sysfs(struct device *dev)
+{
+	if (nvsd_kobj) {
+		sysfs_remove_group(nvsd_kobj, &nvsd_attr_group);
+		kobject_put(nvsd_kobj);
+	}
+}
diff --git a/drivers/video/tegra/dc/nvsd.h b/drivers/video/tegra/dc/nvsd.h
new file mode 100644
index 000000000000..d6f9dc148320
--- /dev/null
+++ b/drivers/video/tegra/dc/nvsd.h
@@ -0,0 +1,25 @@
+/*
+ * drivers/video/tegra/dc/nvsd.h
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 __DRIVERS_VIDEO_TEGRA_DC_NVSD_H
+#define __DRIVERS_VIDEO_TEGRA_DC_NVSD_H
+
+void nvsd_init(struct tegra_dc *dc, struct tegra_dc_sd_settings *settings);
+bool nvsd_update_brightness(struct tegra_dc *dc);
+int nvsd_create_sysfs(struct device *dev);
+void __devexit nvsd_remove_sysfs(struct device *dev);
+
+#endif
diff --git a/drivers/video/tegra/dc/rgb.c b/drivers/video/tegra/dc/rgb.c
new file mode 100644
index 000000000000..b4097d98a9ba
--- /dev/null
+++ b/drivers/video/tegra/dc/rgb.c
@@ -0,0 +1,166 @@
+/*
+ * drivers/video/tegra/dc/rgb.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/kernel.h>
+
+#include <mach/dc.h>
+
+#include "dc_reg.h"
+#include "dc_priv.h"
+
+
+static const u32 tegra_dc_rgb_enable_partial_pintable[] = {
+	DC_COM_PIN_OUTPUT_ENABLE0,	0x00000000,
+	DC_COM_PIN_OUTPUT_ENABLE1,	0x00000000,
+	DC_COM_PIN_OUTPUT_ENABLE2,	0x00000000,
+	DC_COM_PIN_OUTPUT_ENABLE3,	0x00000000,
+	DC_COM_PIN_OUTPUT_POLARITY0,	0x00000000,
+	DC_COM_PIN_OUTPUT_POLARITY2,	0x00000000,
+	DC_COM_PIN_OUTPUT_DATA0,	0x00000000,
+	DC_COM_PIN_OUTPUT_DATA1,	0x00000000,
+	DC_COM_PIN_OUTPUT_DATA2,	0x00000000,
+	DC_COM_PIN_OUTPUT_DATA3,	0x00000000,
+};
+
+static const u32 tegra_dc_rgb_enable_pintable[] = {
+	DC_COM_PIN_OUTPUT_ENABLE0,	0x00000000,
+	DC_COM_PIN_OUTPUT_ENABLE1,	0x00000000,
+	DC_COM_PIN_OUTPUT_ENABLE2,	0x00000000,
+	DC_COM_PIN_OUTPUT_ENABLE3,	0x00000000,
+	DC_COM_PIN_OUTPUT_POLARITY0,	0x00000000,
+	DC_COM_PIN_OUTPUT_POLARITY1,	0x01000000,
+	DC_COM_PIN_OUTPUT_POLARITY2,	0x00000000,
+	DC_COM_PIN_OUTPUT_POLARITY3,	0x00000000,
+	DC_COM_PIN_OUTPUT_DATA0,	0x00000000,
+	DC_COM_PIN_OUTPUT_DATA1,	0x00000000,
+	DC_COM_PIN_OUTPUT_DATA2,	0x00000000,
+	DC_COM_PIN_OUTPUT_DATA3,	0x00000000,
+};
+
+static const u32 tegra_dc_rgb_enable_out_sel_pintable[] = {
+	DC_COM_PIN_OUTPUT_SELECT0,	0x00000000,
+	DC_COM_PIN_OUTPUT_SELECT1,	0x00000000,
+	DC_COM_PIN_OUTPUT_SELECT2,	0x00000000,
+#ifdef CONFIG_TEGRA_SILICON_PLATFORM
+	DC_COM_PIN_OUTPUT_SELECT3,	0x00000000,
+#else
+	/* The display panel sub-board used on FPGA platforms (panel 86)
+	   is non-standard. It expects the Data Enable signal on the WR
+	   pin instead of the DE pin. */
+	DC_COM_PIN_OUTPUT_SELECT3,	0x00200000,
+#endif
+	DC_COM_PIN_OUTPUT_SELECT4,	0x00210222,
+	DC_COM_PIN_OUTPUT_SELECT5,	0x00002200,
+	DC_COM_PIN_OUTPUT_SELECT6,	0x00020000,
+};
+
+static const u32 tegra_dc_rgb_disable_pintable[] = {
+	DC_COM_PIN_OUTPUT_ENABLE0,	0x55555555,
+	DC_COM_PIN_OUTPUT_ENABLE1,	0x55150005,
+	DC_COM_PIN_OUTPUT_ENABLE2,	0x55555555,
+	DC_COM_PIN_OUTPUT_ENABLE3,	0x55555555,
+	DC_COM_PIN_OUTPUT_POLARITY0,	0x00000000,
+	DC_COM_PIN_OUTPUT_POLARITY1,	0x00000000,
+	DC_COM_PIN_OUTPUT_POLARITY2,	0x00000000,
+	DC_COM_PIN_OUTPUT_POLARITY3,	0x00000000,
+	DC_COM_PIN_OUTPUT_DATA0,	0xaaaaaaaa,
+	DC_COM_PIN_OUTPUT_DATA1,	0xaaaaaaaa,
+	DC_COM_PIN_OUTPUT_DATA2,	0xaaaaaaaa,
+	DC_COM_PIN_OUTPUT_DATA3,	0xaaaaaaaa,
+	DC_COM_PIN_OUTPUT_SELECT0,	0x00000000,
+	DC_COM_PIN_OUTPUT_SELECT1,	0x00000000,
+	DC_COM_PIN_OUTPUT_SELECT2,	0x00000000,
+	DC_COM_PIN_OUTPUT_SELECT3,	0x00000000,
+	DC_COM_PIN_OUTPUT_SELECT4,	0x00000000,
+	DC_COM_PIN_OUTPUT_SELECT5,	0x00000000,
+	DC_COM_PIN_OUTPUT_SELECT6,	0x00000000,
+};
+
+static void tegra_dc_rgb_enable(struct tegra_dc *dc)
+{
+	int i;
+	u32 out_sel_pintable[ARRAY_SIZE(tegra_dc_rgb_enable_out_sel_pintable)];
+
+	tegra_dc_writel(dc, PW0_ENABLE | PW1_ENABLE | PW2_ENABLE | PW3_ENABLE |
+			PW4_ENABLE | PM0_ENABLE | PM1_ENABLE,
+			DC_CMD_DISPLAY_POWER_CONTROL);
+
+	tegra_dc_writel(dc, DISP_CTRL_MODE_C_DISPLAY, DC_CMD_DISPLAY_COMMAND);
+
+	if (dc->out->out_pins) {
+		tegra_dc_set_out_pin_polars(dc, dc->out->out_pins,
+			dc->out->n_out_pins);
+		tegra_dc_write_table(dc, tegra_dc_rgb_enable_partial_pintable);
+	} else {
+		tegra_dc_write_table(dc, tegra_dc_rgb_enable_pintable);
+	}
+
+	memcpy(out_sel_pintable, tegra_dc_rgb_enable_out_sel_pintable,
+		sizeof(tegra_dc_rgb_enable_out_sel_pintable));
+
+	if (dc->out && dc->out->out_sel_configs) {
+		u8 *out_sels = dc->out->out_sel_configs;
+		for (i = 0; i < dc->out->n_out_sel_configs; i++) {
+			switch (out_sels[i]) {
+			case TEGRA_PIN_OUT_CONFIG_SEL_LM1_M1:
+				out_sel_pintable[5*2+1] =
+					(out_sel_pintable[5*2+1] &
+					~PIN5_LM1_LCD_M1_OUTPUT_MASK) |
+					PIN5_LM1_LCD_M1_OUTPUT_M1;
+				break;
+			case TEGRA_PIN_OUT_CONFIG_SEL_LM1_LD21:
+				out_sel_pintable[5*2+1] =
+					(out_sel_pintable[5*2+1] &
+					~PIN5_LM1_LCD_M1_OUTPUT_MASK) |
+					PIN5_LM1_LCD_M1_OUTPUT_LD21;
+				break;
+			case TEGRA_PIN_OUT_CONFIG_SEL_LM1_PM1:
+				out_sel_pintable[5*2+1] =
+					(out_sel_pintable[5*2+1] &
+					~PIN5_LM1_LCD_M1_OUTPUT_MASK) |
+					PIN5_LM1_LCD_M1_OUTPUT_PM1;
+				break;
+			default:
+				dev_err(&dc->ndev->dev,
+					"Invalid pin config[%d]: %d\n",
+					 i, out_sels[i]);
+				break;
+			}
+		}
+	}
+
+	tegra_dc_write_table(dc, out_sel_pintable);
+
+	/* Inform DC register updated */
+	tegra_dc_writel(dc, GENERAL_UPDATE, DC_CMD_STATE_CONTROL);
+	tegra_dc_writel(dc, GENERAL_ACT_REQ, DC_CMD_STATE_CONTROL);
+}
+
+static void tegra_dc_rgb_disable(struct tegra_dc *dc)
+{
+	tegra_dc_writel(dc, 0x00000000, DC_CMD_DISPLAY_POWER_CONTROL);
+
+	tegra_dc_write_table(dc, tegra_dc_rgb_disable_pintable);
+}
+
+struct tegra_dc_out_ops tegra_dc_rgb_ops = {
+	.enable = tegra_dc_rgb_enable,
+	.disable = tegra_dc_rgb_disable,
+};
+
diff --git a/drivers/video/tegra/dc/window.c b/drivers/video/tegra/dc/window.c
new file mode 100644
index 000000000000..af18564f45b9
--- /dev/null
+++ b/drivers/video/tegra/dc/window.c
@@ -0,0 +1,493 @@
+/*
+ * drivers/video/tegra/dc/window.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * Copyright (c) 2010-2012, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/err.h>
+#include <linux/types.h>
+#include <mach/dc.h>
+
+#include "dc_reg.h"
+#include "dc_config.h"
+#include "dc_priv.h"
+
+static int no_vsync;
+#ifndef CONFIG_ANDROID
+static atomic_t frame_end_ref = ATOMIC_INIT(0);
+#endif /* !CONFIG_ANDROID */
+
+module_param_named(no_vsync, no_vsync, int, S_IRUGO | S_IWUSR);
+
+static bool tegra_dc_windows_are_clean(struct tegra_dc_win *windows[],
+					     int n)
+{
+	int i;
+
+	for (i = 0; i < n; i++) {
+		if (windows[i]->dirty)
+			return false;
+	}
+
+	return true;
+}
+
+#ifndef CONFIG_ANDROID
+int tegra_dc_config_frame_end_intr(struct tegra_dc *dc, bool enable)
+{
+	tegra_dc_writel(dc, FRAME_END_INT, DC_CMD_INT_STATUS);
+	if (enable) {
+		atomic_inc(&frame_end_ref);
+		tegra_dc_unmask_interrupt(dc, FRAME_END_INT);
+	} else if (!atomic_dec_return(&frame_end_ref))
+		tegra_dc_mask_interrupt(dc, FRAME_END_INT);
+	return 0;
+}
+#endif /* !CONFIG_ANDROID */
+
+static int get_topmost_window(u32 *depths, unsigned long *wins)
+{
+	int idx, best = -1;
+
+	for_each_set_bit(idx, wins, DC_N_WINDOWS) {
+		if (best == -1 || depths[idx] < depths[best])
+			best = idx;
+	}
+	clear_bit(best, wins);
+	return best;
+}
+
+static u32 blend_topwin(u32 flags)
+{
+	if (flags & TEGRA_WIN_FLAG_BLEND_COVERAGE)
+		return BLEND(NOKEY, ALPHA, 0xff, 0xff);
+	else if (flags & TEGRA_WIN_FLAG_BLEND_PREMULT)
+		return BLEND(NOKEY, PREMULT, 0xff, 0xff);
+	else
+		return BLEND(NOKEY, FIX, 0xff, 0xff);
+}
+
+static u32 blend_2win(int idx, unsigned long behind_mask, u32* flags, int xy)
+{
+	int other;
+
+	for (other = 0; other < DC_N_WINDOWS; other++) {
+		if (other != idx && (xy-- == 0))
+			break;
+	}
+	if (BIT(other) & behind_mask)
+		return blend_topwin(flags[idx]);
+	else if (flags[other])
+		return BLEND(NOKEY, DEPENDANT, 0x00, 0x00);
+	else
+		return BLEND(NOKEY, FIX, 0x00, 0x00);
+}
+
+static u32 blend_3win(int idx, unsigned long behind_mask, u32* flags)
+{
+	unsigned long infront_mask;
+	int first;
+
+	infront_mask = ~(behind_mask | BIT(idx));
+	infront_mask &= (BIT(DC_N_WINDOWS) - 1);
+	first = ffs(infront_mask) - 1;
+
+	if (!infront_mask)
+		return blend_topwin(flags[idx]);
+	else if (behind_mask && first != -1 && flags[first])
+		return BLEND(NOKEY, DEPENDANT, 0x00, 0x00);
+	else
+		return BLEND(NOKEY, FIX, 0x0, 0x0);
+}
+
+static void tegra_dc_set_blending(struct tegra_dc *dc,
+	struct tegra_dc_blend *blend)
+{
+	unsigned long mask = BIT(DC_N_WINDOWS) - 1;
+
+	while (mask) {
+		int idx = get_topmost_window(blend->z, &mask);
+
+		tegra_dc_writel(dc, WINDOW_A_SELECT << idx,
+				DC_CMD_DISPLAY_WINDOW_HEADER);
+		tegra_dc_writel(dc, BLEND(NOKEY, FIX, 0xff, 0xff),
+				DC_WIN_BLEND_NOKEY);
+		tegra_dc_writel(dc, BLEND(NOKEY, FIX, 0xff, 0xff),
+				DC_WIN_BLEND_1WIN);
+		tegra_dc_writel(dc, blend_2win(idx, mask, blend->flags, 0),
+				DC_WIN_BLEND_2WIN_X);
+		tegra_dc_writel(dc, blend_2win(idx, mask, blend->flags, 1),
+				DC_WIN_BLEND_2WIN_Y);
+		tegra_dc_writel(dc, blend_3win(idx, mask, blend->flags),
+				DC_WIN_BLEND_3WIN_XY);
+	}
+}
+
+/* does not support syncing windows on multiple dcs in one call */
+int tegra_dc_sync_windows(struct tegra_dc_win *windows[], int n)
+{
+	int ret;
+	if (n < 1 || n > DC_N_WINDOWS)
+		return -EINVAL;
+
+	if (!windows[0]->dc->enabled)
+		return -EFAULT;
+
+#ifdef CONFIG_TEGRA_SIMULATION_PLATFORM
+	/* Don't want to timeout on simulator */
+	ret = wait_event_interruptible(windows[0]->dc->wq,
+		tegra_dc_windows_are_clean(windows, n));
+#else
+	trace_printk("%s:Before wait_event_interruptible_timeout\n",
+		windows[0]->dc->ndev->name);
+	ret = wait_event_interruptible_timeout(windows[0]->dc->wq,
+		tegra_dc_windows_are_clean(windows, n),
+		HZ);
+	trace_printk("%s:After wait_event_interruptible_timeout\n",
+		windows[0]->dc->ndev->name);
+#endif
+	return ret;
+}
+EXPORT_SYMBOL(tegra_dc_sync_windows);
+
+static inline u32 compute_dda_inc(fixed20_12 in, unsigned out_int,
+				  bool v, unsigned Bpp)
+{
+	/*
+	 * min(round((prescaled_size_in_pixels - 1) * 0x1000 /
+	 *	     (post_scaled_size_in_pixels - 1)), MAX)
+	 * Where the value of MAX is as follows:
+	 * For V_DDA_INCREMENT: 15.0 (0xF000)
+	 * For H_DDA_INCREMENT:  4.0 (0x4000) for 4 Bytes/pix formats.
+	 *			 8.0 (0x8000) for 2 Bytes/pix formats.
+	 */
+
+	fixed20_12 out = dfixed_init(out_int);
+	u32 dda_inc;
+	int max;
+
+	if (v) {
+		max = 15;
+	} else {
+		switch (Bpp) {
+		default:
+			WARN_ON_ONCE(1);
+			/* fallthrough */
+		case 4:
+			max = 4;
+			break;
+		case 2:
+			max = 8;
+			break;
+		}
+	}
+
+	out.full = max_t(u32, out.full - dfixed_const(1), dfixed_const(1));
+	in.full -= dfixed_const(1);
+
+	dda_inc = dfixed_div(in, out);
+
+	dda_inc = min_t(u32, dda_inc, dfixed_const(max));
+
+	return dda_inc;
+}
+
+static inline u32 compute_initial_dda(fixed20_12 in)
+{
+	return dfixed_frac(in);
+}
+
+/* does not support updating windows on multiple dcs in one call */
+int tegra_dc_update_windows(struct tegra_dc_win *windows[], int n)
+{
+	struct tegra_dc *dc;
+	unsigned long update_mask = GENERAL_ACT_REQ;
+	unsigned long val;
+	bool update_blend = false;
+	int i;
+
+	dc = windows[0]->dc;
+
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE) {
+		/* Acquire one_shot_lock to avoid race condition between
+		 * cancellation of old delayed work and schedule of new
+		 * delayed work. */
+		mutex_lock(&dc->one_shot_lock);
+		cancel_delayed_work_sync(&dc->one_shot_work);
+	}
+	mutex_lock(&dc->lock);
+
+	if (!dc->enabled) {
+		mutex_unlock(&dc->lock);
+		if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
+			mutex_unlock(&dc->one_shot_lock);
+		return -EFAULT;
+	}
+
+	tegra_dc_hold_dc_out(dc);
+
+	if (no_vsync)
+		tegra_dc_writel(dc, WRITE_MUX_ACTIVE | READ_MUX_ACTIVE,
+			DC_CMD_STATE_ACCESS);
+	else
+		tegra_dc_writel(dc, WRITE_MUX_ASSEMBLY | READ_MUX_ASSEMBLY,
+			DC_CMD_STATE_ACCESS);
+
+	for (i = 0; i < n; i++) {
+		struct tegra_dc_win *win = windows[i];
+		unsigned h_dda;
+		unsigned v_dda;
+		fixed20_12 h_offset, v_offset;
+		bool invert_h = (win->flags & TEGRA_WIN_FLAG_INVERT_H) != 0;
+		bool invert_v = (win->flags & TEGRA_WIN_FLAG_INVERT_V) != 0;
+		bool yuv = tegra_dc_is_yuv(win->fmt);
+		bool yuvp = tegra_dc_is_yuv_planar(win->fmt);
+		unsigned Bpp = tegra_dc_fmt_bpp(win->fmt) / 8;
+		/* Bytes per pixel of bandwidth, used for dda_inc calculation */
+		unsigned Bpp_bw = Bpp * (yuvp ? 2 : 1);
+		const bool filter_h = win_use_h_filter(dc, win);
+		const bool filter_v = win_use_v_filter(dc, win);
+
+		if (win->z != dc->blend.z[win->idx]) {
+			dc->blend.z[win->idx] = win->z;
+			update_blend = true;
+		}
+		if ((win->flags & TEGRA_WIN_BLEND_FLAGS_MASK) !=
+			dc->blend.flags[win->idx]) {
+			dc->blend.flags[win->idx] =
+				win->flags & TEGRA_WIN_BLEND_FLAGS_MASK;
+			update_blend = true;
+		}
+
+		tegra_dc_writel(dc, WINDOW_A_SELECT << win->idx,
+				DC_CMD_DISPLAY_WINDOW_HEADER);
+
+		if (!no_vsync)
+			update_mask |= WIN_A_ACT_REQ << win->idx;
+
+		if (!WIN_IS_ENABLED(win)) {
+			dc->windows[i].dirty = 1;
+			tegra_dc_writel(dc, 0, DC_WIN_WIN_OPTIONS);
+			continue;
+		}
+
+		tegra_dc_writel(dc, win->fmt & 0x1f, DC_WIN_COLOR_DEPTH);
+		tegra_dc_writel(dc, win->fmt >> 6, DC_WIN_BYTE_SWAP);
+
+		tegra_dc_writel(dc,
+			V_POSITION(win->out_y) | H_POSITION(win->out_x),
+			DC_WIN_POSITION);
+		tegra_dc_writel(dc,
+			V_SIZE(win->out_h) | H_SIZE(win->out_w),
+			DC_WIN_SIZE);
+
+		if (tegra_dc_feature_has_scaling(dc, win->idx)) {
+			tegra_dc_writel(dc,
+				V_PRESCALED_SIZE(dfixed_trunc(win->h)) |
+				H_PRESCALED_SIZE(dfixed_trunc(win->w) * Bpp),
+				DC_WIN_PRESCALED_SIZE);
+
+			h_dda = compute_dda_inc(win->w, win->out_w, false,
+				Bpp_bw);
+			v_dda = compute_dda_inc(win->h, win->out_h, true,
+				Bpp_bw);
+			tegra_dc_writel(dc, V_DDA_INC(v_dda) |
+				H_DDA_INC(h_dda), DC_WIN_DDA_INCREMENT);
+			h_dda = compute_initial_dda(win->x);
+			v_dda = compute_initial_dda(win->y);
+			tegra_dc_writel(dc, h_dda, DC_WIN_H_INITIAL_DDA);
+			tegra_dc_writel(dc, v_dda, DC_WIN_V_INITIAL_DDA);
+		}
+
+		tegra_dc_writel(dc, 0, DC_WIN_BUF_STRIDE);
+		tegra_dc_writel(dc, 0, DC_WIN_UV_BUF_STRIDE);
+		tegra_dc_writel(dc, (unsigned long)win->phys_addr,
+			DC_WINBUF_START_ADDR);
+
+		if (!yuvp) {
+			tegra_dc_writel(dc, win->stride, DC_WIN_LINE_STRIDE);
+		} else {
+			tegra_dc_writel(dc,
+				(unsigned long)win->phys_addr_u,
+				DC_WINBUF_START_ADDR_U);
+			tegra_dc_writel(dc,
+				(unsigned long)win->phys_addr_v,
+				DC_WINBUF_START_ADDR_V);
+			tegra_dc_writel(dc,
+				LINE_STRIDE(win->stride) |
+				UV_LINE_STRIDE(win->stride_uv),
+				DC_WIN_LINE_STRIDE);
+		}
+
+		h_offset = win->x;
+		if (invert_h) {
+			h_offset.full += win->w.full - dfixed_const(1);
+		}
+
+		v_offset = win->y;
+		if (invert_v) {
+			v_offset.full += win->h.full - dfixed_const(1);
+		}
+
+		tegra_dc_writel(dc, dfixed_trunc(h_offset) * Bpp,
+				DC_WINBUF_ADDR_H_OFFSET);
+		tegra_dc_writel(dc, dfixed_trunc(v_offset),
+				DC_WINBUF_ADDR_V_OFFSET);
+
+		if (tegra_dc_feature_has_tiling(dc, win->idx)) {
+			if (WIN_IS_TILED(win))
+				tegra_dc_writel(dc,
+					DC_WIN_BUFFER_ADDR_MODE_TILE |
+					DC_WIN_BUFFER_ADDR_MODE_TILE_UV,
+					DC_WIN_BUFFER_ADDR_MODE);
+			else
+				tegra_dc_writel(dc,
+					DC_WIN_BUFFER_ADDR_MODE_LINEAR |
+					DC_WIN_BUFFER_ADDR_MODE_LINEAR_UV,
+					DC_WIN_BUFFER_ADDR_MODE);
+		}
+
+		val = WIN_ENABLE;
+		if (yuv)
+			val |= CSC_ENABLE;
+		else if (tegra_dc_fmt_bpp(win->fmt) < 24)
+			val |= COLOR_EXPAND;
+
+		if (win->ppflags & TEGRA_WIN_PPFLAG_CP_ENABLE)
+			val |= CP_ENABLE;
+
+		if (filter_h)
+			val |= H_FILTER_ENABLE;
+		if (filter_v)
+			val |= V_FILTER_ENABLE;
+
+		if (invert_h)
+			val |= H_DIRECTION_DECREMENT;
+		if (invert_v)
+			val |= V_DIRECTION_DECREMENT;
+
+		tegra_dc_writel(dc, val, DC_WIN_WIN_OPTIONS);
+
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+		if (win->global_alpha == 255)
+			tegra_dc_writel(dc, 0, DC_WIN_GLOBAL_ALPHA);
+		else
+			tegra_dc_writel(dc, GLOBAL_ALPHA_ENABLE |
+				win->global_alpha, DC_WIN_GLOBAL_ALPHA);
+#endif
+
+		win->dirty = no_vsync ? 0 : 1;
+
+		dev_dbg(&dc->ndev->dev, "%s():idx=%d z=%d x=%d y=%d w=%d h=%d "
+			"out_x=%u out_y=%u out_w=%u out_h=%u "
+			"fmt=%d yuvp=%d Bpp=%u filter_h=%d filter_v=%d",
+			__func__, win->idx, win->z,
+			dfixed_trunc(win->x), dfixed_trunc(win->y),
+			dfixed_trunc(win->w), dfixed_trunc(win->h),
+			win->out_x, win->out_y, win->out_w, win->out_h,
+			win->fmt, yuvp, Bpp, filter_h, filter_v);
+		trace_printk("%s:win%u in:%ux%u out:%ux%u fmt=%d\n",
+			dc->ndev->name, win->idx, dfixed_trunc(win->w),
+			dfixed_trunc(win->h), win->out_w, win->out_h, win->fmt);
+	}
+
+	if (update_blend) {
+		tegra_dc_set_blending(dc, &dc->blend);
+		for (i = 0; i < DC_N_WINDOWS; i++) {
+			if (!no_vsync)
+				dc->windows[i].dirty = 1;
+			update_mask |= WIN_A_ACT_REQ << i;
+		}
+	}
+
+	tegra_dc_set_dynamic_emc(windows, n);
+
+	tegra_dc_writel(dc, update_mask << 8, DC_CMD_STATE_CONTROL);
+
+	tegra_dc_writel(dc, FRAME_END_INT | V_BLANK_INT, DC_CMD_INT_STATUS);
+	if (!no_vsync) {
+		set_bit(V_BLANK_FLIP, &dc->vblank_ref_count);
+		tegra_dc_unmask_interrupt(dc,
+			FRAME_END_INT | V_BLANK_INT | ALL_UF_INT);
+	} else {
+		clear_bit(V_BLANK_FLIP, &dc->vblank_ref_count);
+		tegra_dc_mask_interrupt(dc,
+#ifndef CONFIG_ANDROID
+			V_BLANK_INT | ALL_UF_INT);
+		if (!atomic_read(&frame_end_ref))
+			tegra_dc_mask_interrupt(dc, FRAME_END_INT);
+#else /* !CONFIG_ANDROID */
+			FRAME_END_INT | V_BLANK_INT | ALL_UF_INT);
+#endif /* !CONFIG_ANDROID */
+	}
+
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
+		schedule_delayed_work(&dc->one_shot_work,
+				msecs_to_jiffies(dc->one_shot_delay_ms));
+
+	/* update EMC clock if calculated bandwidth has changed */
+	tegra_dc_program_bandwidth(dc, false);
+
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
+		update_mask |= NC_HOST_TRIG;
+
+	tegra_dc_writel(dc, update_mask, DC_CMD_STATE_CONTROL);
+	trace_printk("%s:update_mask=%#lx\n", dc->ndev->name, update_mask);
+
+	tegra_dc_release_dc_out(dc);
+	mutex_unlock(&dc->lock);
+	if (dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
+		mutex_unlock(&dc->one_shot_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(tegra_dc_update_windows);
+
+void tegra_dc_trigger_windows(struct tegra_dc *dc)
+{
+	u32 val, i;
+	u32 completed = 0;
+	u32 dirty = 0;
+
+	val = tegra_dc_readl(dc, DC_CMD_STATE_CONTROL);
+	for (i = 0; i < DC_N_WINDOWS; i++) {
+#ifdef CONFIG_TEGRA_SIMULATION_PLATFORM
+		/* FIXME: this is not needed when the simulator
+		   clears WIN_x_UPDATE bits as in HW */
+		dc->windows[i].dirty = 0;
+		completed = 1;
+#else
+		if (!(val & (WIN_A_ACT_REQ << i))) {
+			dc->windows[i].dirty = 0;
+			completed = 1;
+		} else {
+			dirty = 1;
+		}
+#endif
+	}
+
+	if (!dirty) {
+		if (!(dc->out->flags & TEGRA_DC_OUT_ONE_SHOT_MODE)
+#ifndef CONFIG_ANDROID
+			&& !atomic_read(&frame_end_ref))
+#else /* !CONFIG_ANDROID */
+			)
+#endif /* !CONFIG_ANDROID */
+			tegra_dc_mask_interrupt(dc, FRAME_END_INT);
+	}
+
+	if (completed)
+		wake_up(&dc->wq);
+}
+
diff --git a/drivers/video/tegra/fb.c b/drivers/video/tegra/fb.c
new file mode 100644
index 000000000000..d0e7809cd83d
--- /dev/null
+++ b/drivers/video/tegra/fb.c
@@ -0,0 +1,844 @@
+/*
+ * drivers/video/tegra/fb.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ *         Colin Cross <ccross@android.com>
+ *         Travis Geiselbrecht <travis@palm.com>
+ *
+ * Copyright (c) 2010-2013, NVIDIA CORPORATION, All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/fb.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/uaccess.h>
+#include <linux/slab.h>
+#include <linux/file.h>
+#include <linux/workqueue.h>
+
+#include <asm/atomic.h>
+
+#include <video/tegrafb.h>
+
+#include <mach/dc.h>
+#include <mach/fb.h>
+#include <linux/nvhost.h>
+#include <linux/nvmap.h>
+#include <linux/console.h>
+
+
+#include "host/dev.h"
+#include "nvmap/nvmap.h"
+#include "dc/dc_priv.h"
+
+/* Pad pitch to 16-byte boundary. */
+#define TEGRA_LINEAR_PITCH_ALIGNMENT 32
+
+struct tegra_fb_info {
+	struct tegra_dc_win	*win;
+	struct nvhost_device	*ndev;
+	struct fb_info		*info;
+	bool			valid;
+
+	struct resource		*fb_mem;
+};
+
+/* palette array used by the fbcon */
+static u32 pseudo_palette[16];
+
+static int tegra_fb_check_var(struct fb_var_screeninfo *var,
+			      struct fb_info *info)
+{
+	struct tegra_fb_info *tegra_fb = info->par;
+	struct tegra_dc *dc = tegra_fb->win->dc;
+	struct tegra_dc_out_ops *ops = dc->out_ops;
+	struct fb_videomode mode;
+
+	if ((var->yres * var->xres * var->bits_per_pixel / 8 * 2) >
+	    info->screen_size)
+		return -EINVAL;
+
+	fb_var_to_videomode(&mode, var);
+
+#if defined(CONFIG_MACH_APALIS_T30) || defined(CONFIG_MACH_COLIBRI_T20) || \
+defined(CONFIG_MACH_COLIBRI_T30)
+	/* Hack: avoid 24 Hz mode in X resulting in no display at all */
+	if (mode.refresh < 50)
+		return -EINVAL;
+#endif /* CONFIG_MACH_APALIS_T30 | CONFIG_MACH_COLIBRI_T20 |
+	  CONFIG_MACH_COLIBRI_T30 */
+
+	/* Apply mode filter for HDMI only -LVDS supports only fix mode */
+	if (ops && ops->mode_filter) {
+		if (!ops->mode_filter(dc, &mode))
+			return -EINVAL;
+
+		/* Mode filter may have modified the mode */
+		fb_videomode_to_var(var, &mode);
+	}
+
+	/* Double yres_virtual to allow double buffering through pan_display */
+	var->xres_virtual = var->xres;
+	var->yres_virtual = var->yres * 2;
+
+	/* we only support RGB ordering for now */
+	switch (var->bits_per_pixel) {
+	case 32:
+	case 24:
+		var->bits_per_pixel = 32;
+		var->red.offset = 0;
+		var->red.length = 8;
+		var->green.offset = 8;
+		var->green.length = 8;
+		var->blue.offset = 16;
+		var->blue.length = 8;
+		var->transp.offset = 24;
+		var->transp.length = 8;
+		break;
+	case 16:
+	default:
+		var->bits_per_pixel = 16;
+		var->red.offset = 11;
+		var->red.length = 5;
+		var->green.offset = 5;
+		var->green.length = 6;
+		var->blue.offset = 0;
+		var->blue.length = 5;
+		break;
+	}
+
+	return 0;
+}
+
+static int tegra_fb_set_par(struct fb_info *info)
+{
+	struct tegra_fb_info *tegra_fb = info->par;
+	struct fb_var_screeninfo *var = &info->var;
+	struct tegra_dc *dc = tegra_fb->win->dc;
+	int err;
+
+	struct tegra_dc_mode mode;
+
+	/* This is usually altered to 16/32 by tegra_fb_check_var
+	 * above which is called before this function
+	 */
+	switch (var->bits_per_pixel) {
+	case 32:
+		tegra_fb->win->fmt = TEGRA_WIN_FMT_R8G8B8A8;
+		break;
+	case 16:
+		tegra_fb->win->fmt = TEGRA_WIN_FMT_B5G6R5;
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+
+	/* if line_length unset, then pad the stride */
+	info->fix.line_length = var->xres * var->bits_per_pixel / 8;
+	info->fix.line_length = round_up(info->fix.line_length,
+				TEGRA_LINEAR_PITCH_ALIGNMENT);
+	tegra_fb->win->stride = info->fix.line_length;
+	tegra_fb->win->stride_uv = 0;
+	tegra_fb->win->phys_addr_u = 0;
+	tegra_fb->win->phys_addr_v = 0;
+
+	tegra_fb->win->w.full = dfixed_const(var->xres);
+	tegra_fb->win->h.full = dfixed_const(var->yres);
+	tegra_fb->win->out_w = var->xres;
+	tegra_fb->win->out_h = var->yres;
+
+	dev_info(&tegra_fb->ndev->dev, "switching framebuffer to %dx%d\n",
+			var->xres, var->yres);
+
+	err = tegra_dc_var_to_dc_mode(dc, var, &mode);
+	if (err) {
+		dev_warn(&tegra_fb->ndev->dev, "could not convert var %d\n", err);
+		return -EINVAL;
+	}
+
+	err = tegra_dc_set_mode(dc, &mode);
+	if (err) {
+		dev_warn(&tegra_fb->ndev->dev, "could not set dc mode %d\n", err);
+		return -EINVAL;
+	}
+
+	/* Reflect changes on HW */
+	if (dc->enabled)
+		tegra_dc_disable(dc);
+	tegra_dc_enable(dc);
+
+	return err;
+
+}
+
+static int tegra_fb_setcolreg(unsigned regno, unsigned red, unsigned green,
+	unsigned blue, unsigned transp, struct fb_info *info)
+{
+	struct fb_var_screeninfo *var = &info->var;
+
+	if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
+	    info->fix.visual == FB_VISUAL_DIRECTCOLOR) {
+		u32 v;
+
+		if (regno >= 16)
+			return -EINVAL;
+
+		red = (red >> (16 - info->var.red.length));
+		green = (green >> (16 - info->var.green.length));
+		blue = (blue >> (16 - info->var.blue.length));
+
+		v = (red << var->red.offset) |
+			(green << var->green.offset) |
+			(blue << var->blue.offset);
+
+		((u32 *)info->pseudo_palette)[regno] = v;
+	}
+
+	return 0;
+}
+
+
+static int tegra_fb_setcmap(struct fb_cmap *cmap, struct fb_info *info)
+{
+	struct tegra_fb_info *tegra_fb = info->par;
+	struct tegra_dc *dc = tegra_fb->win->dc;
+	int i;
+	u16 *red = cmap->red;
+	u16 *green = cmap->green;
+	u16 *blue = cmap->blue;
+	int start = cmap->start;
+
+	if (((unsigned)start > 255) || ((start + cmap->len) > 256))
+		return -EINVAL;
+
+	if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
+		info->fix.visual == FB_VISUAL_DIRECTCOLOR) {
+		/*
+		 * For now we are considering color schemes with
+		 * cmap->len <=16 as special case of basic color
+		 * scheme to support fbconsole.But for DirectColor
+		 * visuals(like the one we actually have, that include
+		 * a HW LUT),the way it's intended to work is that the
+		 * actual LUT HW is programmed to the intended values,
+		 * even for small color maps like those with 16 or fewer
+		 * entries. The pseudo_palette is then programmed to the
+		 * identity transform.
+		 */
+		if (cmap->len <= 16) {
+			/* Low-color schemes like fbconsole*/
+			u16 *transp = cmap->transp;
+			u_int vtransp = 0xffff;
+
+			for (i = 0; i < cmap->len; i++) {
+				if (transp)
+					vtransp = *transp++;
+				if (tegra_fb_setcolreg(start++, *red++,
+					*green++, *blue++,
+					vtransp, info))
+						return -EINVAL;
+			}
+		} else {
+			/* High-color schemes*/
+			for (i = 0; i < cmap->len; i++) {
+				dc->fb_lut.r[start+i] = *red++ >> 8;
+				dc->fb_lut.g[start+i] = *green++ >> 8;
+				dc->fb_lut.b[start+i] = *blue++ >> 8;
+			}
+			tegra_dc_update_lut(dc, -1, -1);
+		}
+	}
+	return 0;
+}
+
+static int tegra_fb_blank(int blank, struct fb_info *info)
+{
+	struct tegra_fb_info *tegra_fb = info->par;
+
+	switch (blank) {
+	case FB_BLANK_UNBLANK:
+		dev_dbg(&tegra_fb->ndev->dev, "unblank\n");
+		tegra_fb->win->flags = TEGRA_WIN_FLAG_ENABLED;
+		tegra_dc_enable(tegra_fb->win->dc);
+		tegra_dc_update_windows(&tegra_fb->win, 1);
+		tegra_dc_sync_windows(&tegra_fb->win, 1);
+		return 0;
+
+	case FB_BLANK_NORMAL:
+		dev_dbg(&tegra_fb->ndev->dev, "blank - normal\n");
+		tegra_dc_blank(tegra_fb->win->dc);
+		return 0;
+
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_HSYNC_SUSPEND:
+	case FB_BLANK_POWERDOWN:
+		dev_dbg(&tegra_fb->ndev->dev, "blank - powerdown\n");
+		tegra_dc_disable(tegra_fb->win->dc);
+		return 0;
+
+	default:
+		return -ENOTTY;
+	}
+}
+
+static int tegra_fb_pan_display(struct fb_var_screeninfo *var,
+				struct fb_info *info)
+{
+	struct tegra_fb_info *tegra_fb = info->par;
+	char __iomem *flush_start;
+	char __iomem *flush_end;
+	u32 addr;
+
+	if (!tegra_fb->win->cur_handle) {
+		flush_start = info->screen_base + (var->yoffset * info->fix.line_length);
+		flush_end = flush_start + (var->yres * info->fix.line_length);
+
+		info->var.xoffset = var->xoffset;
+		info->var.yoffset = var->yoffset;
+
+		addr = info->fix.smem_start + (var->yoffset * info->fix.line_length) +
+			(var->xoffset * (var->bits_per_pixel/8));
+
+		tegra_fb->win->phys_addr = addr;
+		tegra_fb->win->flags = TEGRA_WIN_FLAG_ENABLED;
+		tegra_fb->win->virt_addr = info->screen_base;
+
+		tegra_dc_update_windows(&tegra_fb->win, 1);
+		tegra_dc_sync_windows(&tegra_fb->win, 1);
+	}
+
+	return 0;
+}
+
+static void tegra_fb_fillrect(struct fb_info *info,
+			      const struct fb_fillrect *rect)
+{
+	cfb_fillrect(info, rect);
+}
+
+static void tegra_fb_copyarea(struct fb_info *info,
+			      const struct fb_copyarea *region)
+{
+	cfb_copyarea(info, region);
+}
+
+static void tegra_fb_imageblit(struct fb_info *info,
+			       const struct fb_image *image)
+{
+	cfb_imageblit(info, image);
+}
+
+static int tegra_fb_ioctl(struct fb_info *info, unsigned int cmd, unsigned long arg)
+{
+	struct tegra_fb_info *tegra_fb = (struct tegra_fb_info *)info->par;
+	struct tegra_fb_modedb modedb;
+	struct fb_modelist *modelist;
+	struct fb_vblank vblank = {};
+	int i;
+
+	switch (cmd) {
+	case FBIO_TEGRA_GET_MODEDB:
+		if (copy_from_user(&modedb, (void __user *)arg, sizeof(modedb)))
+			return -EFAULT;
+
+		i = 0;
+		list_for_each_entry(modelist, &info->modelist, list) {
+			struct fb_var_screeninfo var;
+
+			if (i >= modedb.modedb_len)
+				break;
+
+			/* fb_videomode_to_var doesn't fill out all the members
+			   of fb_var_screeninfo */
+			memset(&var, 0x0, sizeof(var));
+
+			fb_videomode_to_var(&var, &modelist->mode);
+
+			if (copy_to_user((void __user *)&modedb.modedb[i],
+					 &var, sizeof(var)))
+				return -EFAULT;
+			i++;
+
+			if (var.vmode & FB_VMODE_STEREO_MASK) {
+				if (i >= modedb.modedb_len)
+					break;
+				var.vmode &= ~FB_VMODE_STEREO_MASK;
+				if (copy_to_user(
+					(void __user *)&modedb.modedb[i],
+					 &var, sizeof(var)))
+					return -EFAULT;
+				i++;
+			}
+		}
+		modedb.modedb_len = i;
+
+		if (copy_to_user((void __user *)arg, &modedb, sizeof(modedb)))
+			return -EFAULT;
+		break;
+
+	case FBIOGET_VBLANK:
+		tegra_dc_get_fbvblank(tegra_fb->win->dc, &vblank);
+
+		if (copy_to_user(
+			(void __user *)arg, &vblank, sizeof(vblank)))
+			return -EFAULT;
+		break;
+
+	case FBIO_WAITFORVSYNC:
+		return tegra_dc_wait_for_vsync(tegra_fb->win->dc);
+
+	default:
+		return -ENOTTY;
+	}
+
+	return 0;
+}
+
+int tegra_fb_get_mode(struct tegra_dc *dc) {
+	/* Avoid error when reading sysfs */
+	if (dc->fb->info->mode == NULL)
+		return 0;
+	return dc->fb->info->mode->refresh;
+}
+
+int tegra_fb_set_mode(struct tegra_dc *dc, int fps) {
+	size_t stereo;
+	struct list_head *pos;
+	struct fb_videomode *best_mode = NULL;
+	int curr_diff = INT_MAX; /* difference of best_mode refresh rate */
+	struct fb_modelist *modelist;
+	struct fb_info *info = dc->fb->info;
+
+	list_for_each(pos, &info->modelist) {
+		struct fb_videomode *mode;
+
+		modelist = list_entry(pos, struct fb_modelist, list);
+		mode = &modelist->mode;
+		if (fps <= mode->refresh && curr_diff > (mode->refresh - fps)) {
+			curr_diff = mode->refresh - fps;
+			best_mode = mode;
+		}
+	}
+	if (best_mode) {
+		info->mode = best_mode;
+		stereo = !!(info->var.vmode & info->mode->vmode &
+#ifndef CONFIG_TEGRA_HDMI_74MHZ_LIMIT
+				FB_VMODE_STEREO_FRAME_PACK);
+#else
+				FB_VMODE_STEREO_LEFT_RIGHT);
+#endif
+		return tegra_dc_set_fb_mode(dc, best_mode, stereo);
+	}
+	return -EIO;
+}
+
+static struct fb_ops tegra_fb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = tegra_fb_check_var,
+	.fb_set_par = tegra_fb_set_par,
+	.fb_setcmap = tegra_fb_setcmap,
+	.fb_blank = tegra_fb_blank,
+	.fb_pan_display = tegra_fb_pan_display,
+	.fb_fillrect = tegra_fb_fillrect,
+	.fb_copyarea = tegra_fb_copyarea,
+	.fb_imageblit = tegra_fb_imageblit,
+	.fb_ioctl = tegra_fb_ioctl,
+};
+
+const struct fb_videomode *tegra_fb_find_best_mode(
+	struct fb_var_screeninfo *var,
+	struct list_head *head)
+{
+	struct list_head *pos;
+	struct fb_modelist *modelist;
+	struct fb_videomode *mode, *best = NULL;
+	int diff = 0;
+
+	list_for_each(pos, head) {
+		int d;
+
+		modelist = list_entry(pos, struct fb_modelist, list);
+		mode = &modelist->mode;
+
+		if (mode->xres >= var->xres && mode->yres >= var->yres) {
+			d = (mode->xres - var->xres) +
+				(mode->yres - var->yres);
+			if (diff < d) {
+				diff = d;
+				best = mode;
+			} else if (diff == d && best &&
+				   mode->refresh > best->refresh)
+				best = mode;
+		}
+	}
+	return best;
+}
+
+void tegra_fb_update_monspecs(struct tegra_fb_info *fb_info,
+			      struct fb_monspecs *specs,
+			      bool (*mode_filter)(const struct tegra_dc *dc,
+						  struct fb_videomode *mode))
+{
+	int i;
+	bool first = false;
+	struct fb_event event;
+	struct fb_info *info = fb_info->info;
+	struct fb_var_screeninfo var = {0,};
+
+	mutex_lock(&fb_info->info->lock);
+	fb_destroy_modedb(fb_info->info->monspecs.modedb);
+
+	fb_destroy_modelist(&fb_info->info->modelist);
+
+	if (specs == NULL) {
+		struct tegra_dc_mode mode;
+		memset(&fb_info->info->monspecs, 0x0,
+		       sizeof(fb_info->info->monspecs));
+		memset(&mode, 0x0, sizeof(mode));
+
+		/*
+		 * reset video mode properties to prevent garbage being displayed on 'mode' device.
+		 */
+		fb_info->info->mode = (struct fb_videomode*) NULL;
+
+		tegra_dc_set_mode(fb_info->win->dc, &mode);
+		mutex_unlock(&fb_info->info->lock);
+		return;
+	}
+
+	memcpy(&fb_info->info->monspecs, specs,
+	       sizeof(fb_info->info->monspecs));
+	fb_info->info->mode = specs->modedb;
+
+	/* Prepare a mode db */
+	for (i = 0; i < specs->modedb_len; i++) {
+		if (info->fbops->fb_check_var) {
+			/* Call mode filter to check mode */
+			fb_videomode_to_var(&var, &specs->modedb[i]);
+			if (!(info->fbops->fb_check_var(&var, info))) {
+				fb_var_to_videomode(&specs->modedb[i], &var);
+				fb_add_videomode(&specs->modedb[i],
+						 &fb_info->info->modelist);
+				/* EDID stds recommend first detailed mode
+				to be applied as default,but if first mode
+				doesn't pass mode filter, we have to select
+				and apply other mode. So flag on if first
+				mode passes mode filter */
+				if (!i)
+					first = true;
+			}
+		} else {
+			fb_add_videomode(&specs->modedb[i],
+					 &fb_info->info->modelist);
+		}
+	}
+
+	/* We can't apply first detailed mode, so get the best mode
+	based on resolution and apply on fb */
+	if (!first) {
+		var.xres = 0;
+		var.yres = 0;
+		info->mode = (struct fb_videomode *)
+			tegra_fb_find_best_mode(&var, &info->modelist);
+	}
+
+	/* Prepare fb info with new mode details */
+	fb_videomode_to_var(&info->var, info->mode);
+	event.info = fb_info->info;
+
+#ifdef CONFIG_FRAMEBUFFER_CONSOLE
+	/* Send a noti to change fb_display[].mode for all vc's */
+	console_lock();
+	fb_notifier_call_chain(FB_EVENT_MODE_CHANGE_ALL, &event);
+	console_unlock();
+
+	/* Notify framebuffer console about mode change */
+	console_lock();
+	fb_notifier_call_chain(FB_EVENT_NEW_MODELIST, &event);
+	console_unlock();
+#else
+	fb_notifier_call_chain(FB_EVENT_NEW_MODELIST, &event);
+#endif
+
+	mutex_unlock(&fb_info->info->lock);
+}
+
+struct tegra_dc_out_pin dc_out_pins[4];
+
+
+static int parse_opt(struct tegra_dc_out *out, char *this_opt)
+{
+	if (!strncmp(this_opt, "hsync:", 6)) {
+		if (simple_strtoul(this_opt+6, NULL, 0) == 0) {
+			out->out_pins[TEGRA_DC_OUT_PIN_H_SYNC].pol =
+				TEGRA_DC_OUT_PIN_POL_LOW;
+		} else {
+			out->out_pins[TEGRA_DC_OUT_PIN_H_SYNC].pol =
+				TEGRA_DC_OUT_PIN_POL_HIGH;
+		}
+		return 0;
+	} else if (!strncmp(this_opt, "vsync:", 6)) {
+		if (simple_strtoul(this_opt+6, NULL, 0) == 0) {
+			out->out_pins[TEGRA_DC_OUT_PIN_V_SYNC].pol =
+				TEGRA_DC_OUT_PIN_POL_LOW;
+		} else {
+			out->out_pins[TEGRA_DC_OUT_PIN_V_SYNC].pol =
+				TEGRA_DC_OUT_PIN_POL_HIGH;
+		}
+		return 0;
+	} else if (!strncmp(this_opt, "outputen:", 9)) {
+		if (simple_strtoul(this_opt+9, NULL, 0) == 0) {
+			out->out_pins[TEGRA_DC_OUT_PIN_DATA_ENABLE].pol =
+				TEGRA_DC_OUT_PIN_POL_LOW;
+		} else {
+			out->out_pins[TEGRA_DC_OUT_PIN_DATA_ENABLE].pol =
+				TEGRA_DC_OUT_PIN_POL_HIGH;
+		}
+		return 0;
+	} else if (!strncmp(this_opt, "pixclockpol:", 12)) {
+		if (simple_strtoul(this_opt+12, NULL, 0) == 0) {
+			out->out_pins[TEGRA_DC_OUT_PIN_PIXEL_CLOCK].pol =
+				TEGRA_DC_OUT_PIN_POL_HIGH;
+		} else {
+			out->out_pins[TEGRA_DC_OUT_PIN_PIXEL_CLOCK].pol =
+				TEGRA_DC_OUT_PIN_POL_LOW;
+		}
+		return 0;
+	}
+
+	return -1;
+}
+
+static void tegra_dc_copy_pin_modes(struct tegra_dc_out *out)
+{
+	int i;
+	struct tegra_dc_out_pin *def = out->out_pins;
+	int n_out_pins_default = out->n_out_pins;
+
+	/* Allocate memory for dynamic output pin configuration... */
+	out->n_out_pins = 4;
+	out->out_pins = kmalloc(sizeof(struct tegra_dc_out_pin) * out->n_out_pins,
+			GFP_KERNEL);
+
+	/* ...set fallback values, we use the pin enum as array index... */
+	out->out_pins[TEGRA_DC_OUT_PIN_DATA_ENABLE].name = TEGRA_DC_OUT_PIN_DATA_ENABLE;
+	out->out_pins[TEGRA_DC_OUT_PIN_DATA_ENABLE].pol = TEGRA_DC_OUT_PIN_POL_HIGH;
+	out->out_pins[TEGRA_DC_OUT_PIN_H_SYNC].name = TEGRA_DC_OUT_PIN_H_SYNC;
+	out->out_pins[TEGRA_DC_OUT_PIN_H_SYNC].pol = TEGRA_DC_OUT_PIN_POL_LOW;
+	out->out_pins[TEGRA_DC_OUT_PIN_V_SYNC].name = TEGRA_DC_OUT_PIN_V_SYNC;
+	out->out_pins[TEGRA_DC_OUT_PIN_V_SYNC].pol = TEGRA_DC_OUT_PIN_POL_LOW;
+	out->out_pins[TEGRA_DC_OUT_PIN_PIXEL_CLOCK].name = TEGRA_DC_OUT_PIN_PIXEL_CLOCK;
+	out->out_pins[TEGRA_DC_OUT_PIN_PIXEL_CLOCK].pol = TEGRA_DC_OUT_PIN_POL_LOW;
+
+	/* ... and copy the static default config from platform data */
+	for (i = 0; i < n_out_pins_default; i++)
+		out->out_pins[def[i].name].pol = def[i].pol;
+}
+
+static int tegra_parse_options(struct tegra_dc_out *out, struct fb_info *info,
+		char *option)
+{
+	char *this_opt;
+
+	/* This off option works perfectly for framebuffer
+	 * device, however the tegra binary driver somehow
+	 * has troubles to handle a missing fb0
+	 * (then, dc1 gets remapped to fb0, which seems
+	 * to be an issue for the binary driver)...
+	 */
+	if (!strcmp(option, "off"))
+		return -ENODEV;
+
+	while ((this_opt = strsep(&option, ",")) != NULL) {
+		/* Parse driver specific arguments for RGB output */
+		if (out->type == TEGRA_DC_OUT_RGB) {
+			if (parse_opt(out, this_opt) == 0)
+				continue;
+		}
+
+		/* No valid driver specific argument, has to be mode */
+		if (!tegra_fb_find_mode(&info->var, info, this_opt, 16))
+			return -EINVAL;
+	}
+	return 0;
+}
+
+struct tegra_fb_info *tegra_fb_register(struct nvhost_device *ndev,
+					struct tegra_dc *dc,
+					struct tegra_fb_data *fb_data,
+					struct resource *fb_mem)
+{
+	struct tegra_dc_win *win;
+	struct fb_info *info;
+	struct tegra_fb_info *tegra_fb;
+	void __iomem *fb_base = NULL;
+	unsigned long fb_size = 0;
+	unsigned long fb_phys = 0;
+	int ret = 0;
+	unsigned stride;
+	char *param_option = NULL;
+	char *option = NULL;
+	char driver[10];
+
+	win = tegra_dc_get_window(dc, fb_data->win);
+	if (!win) {
+		dev_err(&ndev->dev, "dc does not have a window at index %d\n",
+			fb_data->win);
+		return ERR_PTR(-ENOENT);
+	}
+
+	info = framebuffer_alloc(sizeof(struct tegra_fb_info), &ndev->dev);
+	if (!info) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	tegra_fb = info->par;
+	tegra_fb->win = win;
+	tegra_fb->ndev = ndev;
+	tegra_fb->fb_mem = fb_mem;
+
+	if (fb_mem) {
+		fb_size = resource_size(fb_mem);
+		fb_phys = fb_mem->start;
+		fb_base = ioremap_nocache(fb_phys, fb_size);
+		if (!fb_base) {
+			dev_err(&ndev->dev, "fb can't be mapped\n");
+			ret = -EBUSY;
+			goto err_free;
+		}
+		tegra_fb->valid = true;
+	}
+
+	stride = fb_data->xres * fb_data->bits_per_pixel / 8;
+	stride = round_up(stride, TEGRA_LINEAR_PITCH_ALIGNMENT);
+
+	info->fbops = &tegra_fb_ops;
+	info->pseudo_palette = pseudo_palette;
+	info->screen_base = fb_base;
+	info->screen_size = fb_size;
+
+	strlcpy(info->fix.id, "tegra_fb", sizeof(info->fix.id));
+	info->fix.type		= FB_TYPE_PACKED_PIXELS;
+	info->fix.visual	= FB_VISUAL_TRUECOLOR;
+	info->fix.xpanstep	= 1;
+	info->fix.ypanstep	= 1;
+	info->fix.accel		= FB_ACCEL_NONE;
+	info->fix.smem_start	= fb_phys;
+	info->fix.smem_len	= fb_size;
+	info->fix.line_length = stride;
+
+	info->var.xres			= fb_data->xres;
+	info->var.yres			= fb_data->yres;
+	info->var.xres_virtual		= fb_data->xres;
+	info->var.yres_virtual		= fb_data->yres * 2;
+	info->var.bits_per_pixel	= fb_data->bits_per_pixel;
+
+	info->var.activate		= FB_ACTIVATE_VBL;
+	info->var.height		= tegra_dc_get_out_height(dc);
+	info->var.width			= tegra_dc_get_out_width(dc);
+	info->var.pixclock		= 0;
+	info->var.left_margin		= 0;
+	info->var.right_margin		= 0;
+	info->var.upper_margin		= 0;
+	info->var.lower_margin		= 0;
+	info->var.hsync_len		= 0;
+	info->var.vsync_len		= 0;
+	info->var.vmode			= FB_VMODE_NONINTERLACED;
+
+	/* window settings */
+	win->x.full = dfixed_const(0);
+	win->y.full = dfixed_const(0);
+	win->w.full = dfixed_const(fb_data->xres);
+	win->h.full = dfixed_const(fb_data->yres);
+	win->out_x = 0;
+	win->out_y = 0;
+	win->out_w = fb_data->xres;
+	win->out_h = fb_data->yres;
+	win->z = 0;
+	win->phys_addr = fb_phys;
+	win->virt_addr = fb_base;
+	win->phys_addr_u = 0;
+	win->phys_addr_v = 0;
+	win->stride = info->fix.line_length;
+	win->stride_uv = 0;
+	win->flags = TEGRA_WIN_FLAG_ENABLED;
+
+	/* Set/copy default pin modes, if output is RGB... */
+	if (dc->out->type == TEGRA_DC_OUT_RGB)
+		tegra_dc_copy_pin_modes(dc->out);
+
+	/* try to use kernel cmd line specified mode */
+	sprintf(driver, "tegrafb%d", ndev->id);
+	fb_get_options(driver, &param_option);
+	if (param_option != NULL) {
+		option = param_option;
+		dev_info(&ndev->dev, "use cmd options for %s: %s\n",
+				driver, option);
+	} else {
+		option = dc->out->default_mode;
+		dev_info(&ndev->dev, "use default mode for %s: %s\n",
+				driver, option);
+	}
+
+	if (option != NULL) {
+		ret = tegra_parse_options(dc->out, info, option);
+		if (ret < 0)
+			goto err_iounmap_fb;
+	}
+
+	/* Activate current settings (tegra_fb_find_mode has call
+	 * tegra_fb_check_var already)
+	 */
+	if (fb_mem)
+		tegra_fb_set_par(info);
+
+	if (register_framebuffer(info)) {
+		dev_err(&ndev->dev, "failed to register framebuffer\n");
+		ret = -ENODEV;
+		goto err_iounmap_fb;
+	}
+
+	tegra_fb->info = info;
+
+	dev_info(&ndev->dev, "probed\n");
+
+	if (fb_data->flags & TEGRA_FB_FLIP_ON_PROBE) {
+		tegra_dc_update_windows(&tegra_fb->win, 1);
+		tegra_dc_sync_windows(&tegra_fb->win, 1);
+	}
+
+	return tegra_fb;
+
+err_iounmap_fb:
+	if (fb_base)
+		iounmap(fb_base);
+err_free:
+	framebuffer_release(info);
+err:
+	return ERR_PTR(ret);
+}
+
+void tegra_fb_unregister(struct tegra_fb_info *fb_info)
+{
+	struct fb_info *info = fb_info->info;
+
+	unregister_framebuffer(info);
+
+	iounmap(info->screen_base);
+	framebuffer_release(info);
+}
diff --git a/drivers/video/tegra/host/Makefile b/drivers/video/tegra/host/Makefile
new file mode 100644
index 000000000000..c2608218c811
--- /dev/null
+++ b/drivers/video/tegra/host/Makefile
@@ -0,0 +1,30 @@
+GCOV_PROFILE := y
+EXTRA_CFLAGS += -Idrivers/video/tegra/host
+
+nvhost-objs = \
+	nvhost_acm.o \
+	nvhost_syncpt.o \
+	nvhost_cdma.o \
+	nvhost_intr.o \
+	nvhost_channel.o \
+	nvhost_job.o \
+	bus.o \
+	dev.o \
+	debug.o \
+	bus_client.o \
+	chip_support.o \
+	nvhost_memmgr.o
+
+obj-$(CONFIG_TEGRA_GRHOST) += mpe/
+obj-$(CONFIG_TEGRA_GRHOST) += gr3d/
+obj-$(CONFIG_TEGRA_GRHOST) += host1x/
+obj-$(CONFIG_TEGRA_GRHOST) += t20/
+obj-$(CONFIG_TEGRA_GRHOST) += t30/
+obj-$(CONFIG_TEGRA_GRHOST) += gr2d/
+obj-$(CONFIG_TEGRA_GRHOST) += isp/
+ifeq ($(CONFIG_TEGRA_CAMERA),y)
+obj-$(CONFIG_TEGRA_GRHOST) += vi/
+endif
+obj-$(CONFIG_TEGRA_GRHOST) += nvhost.o
+
+obj-$(CONFIG_TEGRA_GRHOST_USE_NVMAP) += nvmap.o
diff --git a/drivers/video/tegra/host/bus.c b/drivers/video/tegra/host/bus.c
new file mode 100644
index 000000000000..f22dac288051
--- /dev/null
+++ b/drivers/video/tegra/host/bus.c
@@ -0,0 +1,629 @@
+/*
+ * drivers/video/tegra/host/bus.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Erik Gilling <konkers@google.com>
+ *
+ * Copyright (C) 2010-2012 NVIDIA Corporation
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/slab.h>
+#include <linux/pm_runtime.h>
+#include <linux/nvhost.h>
+
+#include "bus.h"
+#include "dev.h"
+
+struct nvhost_bus *nvhost_bus_inst;
+struct nvhost_master *nvhost;
+
+struct resource *nvhost_get_resource(struct nvhost_device *dev,
+				       unsigned int type, unsigned int num)
+{
+	int i;
+
+	for (i = 0; i < dev->num_resources; i++) {
+		struct resource *r = &dev->resource[i];
+
+		if (type == resource_type(r) && num-- == 0)
+			return r;
+	}
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(nvhost_get_resource);
+
+int nvhost_get_irq(struct nvhost_device *dev, unsigned int num)
+{
+	struct resource *r = nvhost_get_resource(dev, IORESOURCE_IRQ, num);
+
+	return r ? r->start : -ENXIO;
+}
+EXPORT_SYMBOL_GPL(nvhost_get_irq);
+
+struct resource *nvhost_get_resource_byname(struct nvhost_device *dev,
+					      unsigned int type,
+					      const char *name)
+{
+	int i;
+
+	for (i = 0; i < dev->num_resources; i++) {
+		struct resource *r = &dev->resource[i];
+
+		if (type == resource_type(r) && !strcmp(r->name, name))
+			return r;
+	}
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(nvhost_get_resource_byname);
+
+int nvhost_get_irq_byname(struct nvhost_device *dev, const char *name)
+{
+	struct resource *r = nvhost_get_resource_byname(dev, IORESOURCE_IRQ,
+							  name);
+
+	return r ? r->start : -ENXIO;
+}
+EXPORT_SYMBOL_GPL(nvhost_get_irq_byname);
+
+static struct nvhost_device_id *nvhost_bus_match_id(struct nvhost_device *dev,
+	struct nvhost_device_id *id_table)
+{
+	while (id_table->name[0]) {
+		if (strcmp(dev->name, id_table->name) == 0
+				&& dev->version == id_table->version)
+			return id_table;
+		id_table++;
+	}
+	return NULL;
+}
+
+static int nvhost_bus_match(struct device *_dev, struct device_driver *drv)
+{
+	struct nvhost_device *dev = to_nvhost_device(_dev);
+	struct nvhost_driver *ndrv = to_nvhost_driver(drv);
+
+	/* check if driver support multiple devices through id_table */
+	if (ndrv->id_table)
+		return nvhost_bus_match_id(dev, ndrv->id_table) != NULL;
+	else /* driver does not support id_table */
+		return !strcmp(dev->name, drv->name);
+}
+
+static int nvhost_drv_probe(struct device *_dev)
+{
+	struct nvhost_driver *drv = to_nvhost_driver(_dev->driver);
+	struct nvhost_device *dev = to_nvhost_device(_dev);
+
+	if (drv && drv->probe) {
+		if (drv->id_table)
+			return drv->probe(dev, nvhost_bus_match_id(dev, drv->id_table));
+		else
+			return drv->probe(dev, NULL);
+	}
+	else
+		return -ENODEV;
+}
+
+static int nvhost_drv_remove(struct device *_dev)
+{
+	struct nvhost_driver *drv = to_nvhost_driver(_dev->driver);
+	struct nvhost_device *dev = to_nvhost_device(_dev);
+
+	return drv->remove(dev);
+}
+
+static void nvhost_drv_shutdown(struct device *_dev)
+{
+	struct nvhost_driver *drv = to_nvhost_driver(_dev->driver);
+	struct nvhost_device *dev = to_nvhost_device(_dev);
+
+	drv->shutdown(dev);
+}
+
+int nvhost_driver_register(struct nvhost_driver *drv)
+{
+	drv->driver.bus = &nvhost_bus_inst->nvhost_bus_type;
+	if (drv->probe)
+		drv->driver.probe = nvhost_drv_probe;
+	if (drv->remove)
+		drv->driver.remove = nvhost_drv_remove;
+	if (drv->shutdown)
+		drv->driver.shutdown = nvhost_drv_shutdown;
+
+	return driver_register(&drv->driver);
+}
+EXPORT_SYMBOL(nvhost_driver_register);
+
+void nvhost_driver_unregister(struct nvhost_driver *drv)
+{
+	driver_unregister(&drv->driver);
+}
+EXPORT_SYMBOL_GPL(nvhost_driver_unregister);
+
+int nvhost_add_devices(struct nvhost_device **devs, int num)
+{
+	int i, ret = 0;
+
+	for (i = 0; i < num; i++) {
+		ret = nvhost_device_register(devs[i]);
+		if (ret) {
+			while (--i >= 0)
+				nvhost_device_unregister(devs[i]);
+			break;
+		}
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(nvhost_add_devices);
+
+int nvhost_device_register(struct nvhost_device *dev)
+{
+	int i, ret = 0;
+
+	if (!dev)
+		return -EINVAL;
+
+	device_initialize(&dev->dev);
+
+	/*  If the dev does not have a parent, assign host1x as parent */
+	if (!dev->dev.parent && nvhost && nvhost->dev != dev)
+		dev->dev.parent = &nvhost->dev->dev;
+
+	dev->dev.bus = &nvhost_bus_inst->nvhost_bus_type;
+
+	if (dev->id != -1)
+		dev_set_name(&dev->dev, "%s.%d", dev->name,  dev->id);
+	else
+		dev_set_name(&dev->dev, "%s", dev->name);
+
+	for (i = 0; i < dev->num_resources; i++) {
+		struct resource *p, *r = &dev->resource[i];
+
+		if (r->name == NULL)
+			r->name = dev_name(&dev->dev);
+
+		p = r->parent;
+		if (!p) {
+			if (resource_type(r) == IORESOURCE_MEM)
+				p = &iomem_resource;
+			else if (resource_type(r) == IORESOURCE_IO)
+				p = &ioport_resource;
+		}
+
+		if (p && insert_resource(p, r)) {
+			pr_err("%s: failed to claim resource %d\n",
+			       dev_name(&dev->dev), i);
+			ret = -EBUSY;
+			goto failed;
+		}
+	}
+
+	ret = device_add(&dev->dev);
+	if (ret == 0)
+		return ret;
+
+failed:
+	while (--i >= 0) {
+		struct resource *r = &dev->resource[i];
+		unsigned long type = resource_type(r);
+
+		if (type == IORESOURCE_MEM || type == IORESOURCE_IO)
+			release_resource(r);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(nvhost_device_register);
+
+void nvhost_device_unregister(struct nvhost_device *dev)
+{
+	int i;
+	if (dev) {
+		device_del(&dev->dev);
+
+		for (i = 0; i < dev->num_resources; i++) {
+			struct resource *r = &dev->resource[i];
+			unsigned long type = resource_type(r);
+
+			if (type == IORESOURCE_MEM || type == IORESOURCE_IO)
+				release_resource(r);
+		}
+
+		put_device(&dev->dev);
+	}
+}
+EXPORT_SYMBOL_GPL(nvhost_device_unregister);
+
+#ifdef CONFIG_PM_SLEEP
+
+static int nvhost_legacy_suspend(struct device *dev, pm_message_t mesg)
+{
+	struct nvhost_driver *pdrv = to_nvhost_driver(dev->driver);
+	struct nvhost_device *pdev = to_nvhost_device(dev);
+	int ret = 0;
+
+	if (dev->driver && pdrv->suspend)
+		ret = pdrv->suspend(pdev, mesg);
+
+	return ret;
+}
+
+static int nvhost_legacy_resume(struct device *dev)
+{
+	struct nvhost_driver *pdrv = to_nvhost_driver(dev->driver);
+	struct nvhost_device *pdev = to_nvhost_device(dev);
+	int ret = 0;
+
+	if (dev->driver && pdrv->resume)
+		ret = pdrv->resume(pdev);
+
+	return ret;
+}
+
+static int nvhost_pm_prepare(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (drv && drv->pm && drv->pm->prepare)
+		ret = drv->pm->prepare(dev);
+
+	return ret;
+}
+
+static void nvhost_pm_complete(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+
+	if (drv && drv->pm && drv->pm->complete)
+		drv->pm->complete(dev);
+}
+
+#else /* !CONFIG_PM_SLEEP */
+
+#define nvhost_pm_prepare		NULL
+#define nvhost_pm_complete		NULL
+
+#endif /* !CONFIG_PM_SLEEP */
+
+#ifdef CONFIG_SUSPEND
+
+int __weak nvhost_pm_suspend(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->suspend)
+			ret = drv->pm->suspend(dev);
+	} else {
+		ret = nvhost_legacy_suspend(dev, PMSG_SUSPEND);
+	}
+
+	return ret;
+}
+
+int __weak nvhost_pm_suspend_noirq(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->suspend_noirq)
+			ret = drv->pm->suspend_noirq(dev);
+	}
+
+	return ret;
+}
+
+int __weak nvhost_pm_resume(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->resume)
+			ret = drv->pm->resume(dev);
+	} else {
+		ret = nvhost_legacy_resume(dev);
+	}
+
+	return ret;
+}
+
+int __weak nvhost_pm_resume_noirq(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->resume_noirq)
+			ret = drv->pm->resume_noirq(dev);
+	}
+
+	return ret;
+}
+
+#else /* !CONFIG_SUSPEND */
+
+#define nvhost_pm_suspend		NULL
+#define nvhost_pm_resume		NULL
+#define nvhost_pm_suspend_noirq	NULL
+#define nvhost_pm_resume_noirq	NULL
+
+#endif /* !CONFIG_SUSPEND */
+
+#ifdef CONFIG_HIBERNATION
+
+static int nvhost_pm_freeze(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->freeze)
+			ret = drv->pm->freeze(dev);
+	} else {
+		ret = nvhost_legacy_suspend(dev, PMSG_FREEZE);
+	}
+
+	return ret;
+}
+
+static int nvhost_pm_freeze_noirq(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->freeze_noirq)
+			ret = drv->pm->freeze_noirq(dev);
+	}
+
+	return ret;
+}
+
+static int nvhost_pm_thaw(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->thaw)
+			ret = drv->pm->thaw(dev);
+	} else {
+		ret = nvhost_legacy_resume(dev);
+	}
+
+	return ret;
+}
+
+static int nvhost_pm_thaw_noirq(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->thaw_noirq)
+			ret = drv->pm->thaw_noirq(dev);
+	}
+
+	return ret;
+}
+
+static int nvhost_pm_poweroff(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->poweroff)
+			ret = drv->pm->poweroff(dev);
+	} else {
+		ret = nvhost_legacy_suspend(dev, PMSG_HIBERNATE);
+	}
+
+	return ret;
+}
+
+static int nvhost_pm_poweroff_noirq(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->poweroff_noirq)
+			ret = drv->pm->poweroff_noirq(dev);
+	}
+
+	return ret;
+}
+
+static int nvhost_pm_restore(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->restore)
+			ret = drv->pm->restore(dev);
+	} else {
+		ret = nvhost_legacy_resume(dev);
+	}
+
+	return ret;
+}
+
+static int nvhost_pm_restore_noirq(struct device *dev)
+{
+	struct device_driver *drv = dev->driver;
+	int ret = 0;
+
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
+		if (drv->pm->restore_noirq)
+			ret = drv->pm->restore_noirq(dev);
+	}
+
+	return ret;
+}
+
+#else /* !CONFIG_HIBERNATION */
+
+#define nvhost_pm_freeze		NULL
+#define nvhost_pm_thaw		NULL
+#define nvhost_pm_poweroff		NULL
+#define nvhost_pm_restore		NULL
+#define nvhost_pm_freeze_noirq	NULL
+#define nvhost_pm_thaw_noirq		NULL
+#define nvhost_pm_poweroff_noirq	NULL
+#define nvhost_pm_restore_noirq	NULL
+
+#endif /* !CONFIG_HIBERNATION */
+
+#ifdef CONFIG_PM_RUNTIME
+
+int __weak nvhost_pm_runtime_suspend(struct device *dev)
+{
+	return pm_generic_runtime_suspend(dev);
+};
+
+int __weak nvhost_pm_runtime_resume(struct device *dev)
+{
+	return pm_generic_runtime_resume(dev);
+};
+
+int __weak nvhost_pm_runtime_idle(struct device *dev)
+{
+	return pm_generic_runtime_idle(dev);
+};
+
+#else /* !CONFIG_PM_RUNTIME */
+
+#define nvhost_pm_runtime_suspend NULL
+#define nvhost_pm_runtime_resume NULL
+#define nvhost_pm_runtime_idle NULL
+
+#endif /* !CONFIG_PM_RUNTIME */
+
+static const struct dev_pm_ops nvhost_dev_pm_ops = {
+	.prepare = nvhost_pm_prepare,
+	.complete = nvhost_pm_complete,
+	.suspend = nvhost_pm_suspend,
+	.resume = nvhost_pm_resume,
+	.freeze = nvhost_pm_freeze,
+	.thaw = nvhost_pm_thaw,
+	.poweroff = nvhost_pm_poweroff,
+	.restore = nvhost_pm_restore,
+	.suspend_noirq = nvhost_pm_suspend_noirq,
+	.resume_noirq = nvhost_pm_resume_noirq,
+	.freeze_noirq = nvhost_pm_freeze_noirq,
+	.thaw_noirq = nvhost_pm_thaw_noirq,
+	.poweroff_noirq = nvhost_pm_poweroff_noirq,
+	.restore_noirq = nvhost_pm_restore_noirq,
+	.runtime_suspend = nvhost_pm_runtime_suspend,
+	.runtime_resume = nvhost_pm_runtime_resume,
+	.runtime_idle = nvhost_pm_runtime_idle,
+};
+
+static int set_parent(struct device *dev, void *data)
+{
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	struct nvhost_master *host = data;
+	if (!dev->parent && ndev != host->dev)
+		dev->parent = &host->dev->dev;
+	return 0;
+}
+
+int nvhost_bus_add_host(struct nvhost_master *host)
+{
+	nvhost = host;
+
+	/*  Assign host1x as parent to all devices in nvhost bus */
+	bus_for_each_dev(&nvhost_bus_inst->nvhost_bus_type, NULL, host, set_parent);
+
+	return 0;
+}
+
+struct nvhost_bus *nvhost_bus_get(void)
+{
+	return nvhost_bus_inst;
+}
+
+int nvhost_bus_init(void)
+{
+	int err;
+	struct nvhost_chip_support *chip_ops;
+
+	pr_info("host1x bus init\n");
+
+	nvhost_bus_inst = kzalloc(sizeof(*nvhost_bus_inst), GFP_KERNEL);
+	if (nvhost_bus_inst == NULL) {
+		pr_err("%s: Cannot allocate nvhost_bus\n", __func__);
+		return -ENOMEM;
+	}
+
+	chip_ops = kzalloc(sizeof(*chip_ops), GFP_KERNEL);
+	if (chip_ops == NULL) {
+		pr_err("%s: Cannot allocate nvhost_chip_support\n", __func__);
+		kfree(nvhost_bus_inst);
+		nvhost_bus_inst = NULL;
+		return -ENOMEM;
+	}
+
+	nvhost_bus_inst->nvhost_bus_type.name = "nvhost";
+	nvhost_bus_inst->nvhost_bus_type.match = nvhost_bus_match;
+	nvhost_bus_inst->nvhost_bus_type.pm = &nvhost_dev_pm_ops;
+	nvhost_bus_inst->nvhost_chip_ops = chip_ops;
+
+	err = bus_register(&nvhost_bus_inst->nvhost_bus_type);
+
+	return err;
+}
+postcore_initcall(nvhost_bus_init);
diff --git a/drivers/video/tegra/host/bus.h b/drivers/video/tegra/host/bus.h
new file mode 100644
index 000000000000..99f820335d60
--- /dev/null
+++ b/drivers/video/tegra/host/bus.h
@@ -0,0 +1,38 @@
+/*
+ * drivers/video/tegra/host/bus.h
+ *
+ * Tegra Graphics Host bus API header
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_BUS_H
+#define __NVHOST_BUS_H
+
+#include <linux/types.h>
+#include <linux/device.h>
+
+#include "chip_support.h"
+
+struct nvhost_bus {
+	struct nvhost_chip_support *nvhost_chip_ops;
+	struct bus_type nvhost_bus_type;
+};
+
+struct nvhost_bus *nvhost_bus_get(void);
+
+extern struct nvhost_bus *nvhost_bus_inst;
+
+#endif
diff --git a/drivers/video/tegra/host/bus_client.c b/drivers/video/tegra/host/bus_client.c
new file mode 100644
index 000000000000..2b92a62cc0bc
--- /dev/null
+++ b/drivers/video/tegra/host/bus_client.c
@@ -0,0 +1,675 @@
+/*
+ * drivers/video/tegra/host/bus_client.c
+ *
+ * Tegra Graphics Host Client Module
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/spinlock.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/uaccess.h>
+#include <linux/file.h>
+#include <linux/clk.h>
+#include <linux/hrtimer.h>
+
+#include <trace/events/nvhost.h>
+
+#include <linux/io.h>
+#include <linux/string.h>
+
+#include <linux/nvhost.h>
+#include <linux/nvhost_ioctl.h>
+
+#include <mach/gpufuse.h>
+#include <mach/hardware.h>
+#include <mach/iomap.h>
+
+#include "debug.h"
+#include "bus_client.h"
+#include "dev.h"
+#include "nvhost_memmgr.h"
+#include "chip_support.h"
+#include "nvhost_acm.h"
+
+#include "nvhost_channel.h"
+#include "nvhost_job.h"
+#include "nvhost_hwctx.h"
+
+static int validate_reg(struct nvhost_device *ndev, u32 offset, int count)
+{
+	struct resource *r = nvhost_get_resource(ndev, IORESOURCE_MEM, 0);
+	int err = 0;
+
+	if (offset + 4 * count > resource_size(r)
+			|| (offset + 4 * count < offset))
+		err = -EPERM;
+
+	return err;
+}
+
+int nvhost_read_module_regs(struct nvhost_device *ndev,
+			u32 offset, int count, u32 *values)
+{
+	void __iomem *p = ndev->aperture + offset;
+	int err;
+
+	/* verify offset */
+	err = validate_reg(ndev, offset, count);
+	if (err)
+		return err;
+
+	nvhost_module_busy(ndev);
+	while (count--) {
+		*(values++) = readl(p);
+		p += 4;
+	}
+	rmb();
+	nvhost_module_idle(ndev);
+
+	return 0;
+}
+
+int nvhost_write_module_regs(struct nvhost_device *ndev,
+			u32 offset, int count, const u32 *values)
+{
+	void __iomem *p = ndev->aperture + offset;
+	int err;
+
+	/* verify offset */
+	err = validate_reg(ndev, offset, count);
+	if (err)
+		return err;
+
+	nvhost_module_busy(ndev);
+	while (count--) {
+		writel(*(values++), p);
+		p += 4;
+	}
+	wmb();
+	nvhost_module_idle(ndev);
+
+	return 0;
+}
+
+struct nvhost_channel_userctx {
+	struct nvhost_channel *ch;
+	struct nvhost_hwctx *hwctx;
+	struct nvhost_submit_hdr_ext hdr;
+	int num_relocshifts;
+	struct nvhost_job *job;
+	struct mem_mgr *memmgr;
+	u32 timeout;
+	u32 priority;
+	int clientid;
+};
+
+static int nvhost_channelrelease(struct inode *inode, struct file *filp)
+{
+	struct nvhost_channel_userctx *priv = filp->private_data;
+
+	trace_nvhost_channel_release(priv->ch->dev->name);
+
+	filp->private_data = NULL;
+
+	nvhost_module_remove_client(priv->ch->dev, priv);
+	nvhost_putchannel(priv->ch, priv->hwctx);
+
+	if (priv->hwctx)
+		priv->ch->ctxhandler->put(priv->hwctx);
+
+	if (priv->job)
+		nvhost_job_put(priv->job);
+
+	mem_op().put_mgr(priv->memmgr);
+	kfree(priv);
+	return 0;
+}
+
+static int nvhost_channelopen(struct inode *inode, struct file *filp)
+{
+	struct nvhost_channel_userctx *priv;
+	struct nvhost_channel *ch;
+
+	ch = container_of(inode->i_cdev, struct nvhost_channel, cdev);
+	ch = nvhost_getchannel(ch);
+	if (!ch)
+		return -ENOMEM;
+	trace_nvhost_channel_open(ch->dev->name);
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		nvhost_putchannel(ch, NULL);
+		return -ENOMEM;
+	}
+	filp->private_data = priv;
+	priv->ch = ch;
+	if(nvhost_module_add_client(ch->dev, priv))
+		goto fail;
+
+	if (ch->ctxhandler && ch->ctxhandler->alloc) {
+		priv->hwctx = ch->ctxhandler->alloc(ch->ctxhandler, ch);
+		if (!priv->hwctx)
+			goto fail;
+	}
+	priv->priority = NVHOST_PRIORITY_MEDIUM;
+	priv->clientid = atomic_add_return(1,
+			&nvhost_get_host(ch->dev)->clientid);
+	priv->timeout = CONFIG_TEGRA_GRHOST_DEFAULT_TIMEOUT;
+
+	return 0;
+fail:
+	nvhost_channelrelease(inode, filp);
+	return -ENOMEM;
+}
+
+static int set_submit(struct nvhost_channel_userctx *ctx)
+{
+	struct nvhost_device *ndev = ctx->ch->dev;
+	struct nvhost_master *host = nvhost_get_host(ndev);
+
+	/* submit should have at least 1 cmdbuf */
+	if (!ctx->hdr.num_cmdbufs ||
+			!nvhost_syncpt_is_valid(&host->syncpt,
+				ctx->hdr.syncpt_id))
+		return -EIO;
+
+	if (!ctx->memmgr) {
+		dev_err(&ndev->dev, "no nvmap context set\n");
+		return -EFAULT;
+	}
+
+	ctx->job = nvhost_job_alloc(ctx->ch,
+			ctx->hwctx,
+			&ctx->hdr,
+			ctx->memmgr,
+			ctx->priority,
+			ctx->clientid);
+	if (!ctx->job)
+		return -ENOMEM;
+	ctx->job->timeout = ctx->timeout;
+
+	if (ctx->hdr.submit_version >= NVHOST_SUBMIT_VERSION_V2)
+		ctx->num_relocshifts = ctx->hdr.num_relocs;
+
+	return 0;
+}
+
+static void reset_submit(struct nvhost_channel_userctx *ctx)
+{
+	ctx->hdr.num_cmdbufs = 0;
+	ctx->hdr.num_relocs = 0;
+	ctx->num_relocshifts = 0;
+	ctx->hdr.num_waitchks = 0;
+
+	if (ctx->job) {
+		nvhost_job_put(ctx->job);
+		ctx->job = NULL;
+	}
+}
+
+static ssize_t nvhost_channelwrite(struct file *filp, const char __user *buf,
+				size_t count, loff_t *offp)
+{
+	struct nvhost_channel_userctx *priv = filp->private_data;
+	size_t remaining = count;
+	int err = 0;
+	struct nvhost_job *job = priv->job;
+	struct nvhost_submit_hdr_ext *hdr = &priv->hdr;
+	const char *chname = priv->ch->dev->name;
+
+	if (!job)
+		return -EIO;
+
+	while (remaining) {
+		size_t consumed;
+		if (!hdr->num_relocs &&
+		    !priv->num_relocshifts &&
+		    !hdr->num_cmdbufs &&
+		    !hdr->num_waitchks) {
+			consumed = sizeof(struct nvhost_submit_hdr);
+			if (remaining < consumed)
+				break;
+			if (copy_from_user(hdr, buf, consumed)) {
+				err = -EFAULT;
+				break;
+			}
+			hdr->submit_version = NVHOST_SUBMIT_VERSION_V0;
+			err = set_submit(priv);
+			if (err)
+				break;
+			trace_nvhost_channel_write_submit(chname,
+			  count, hdr->num_cmdbufs, hdr->num_relocs,
+			  hdr->syncpt_id, hdr->syncpt_incrs);
+		} else if (hdr->num_cmdbufs) {
+			struct nvhost_cmdbuf cmdbuf;
+			consumed = sizeof(cmdbuf);
+			if (remaining < consumed)
+				break;
+			if (copy_from_user(&cmdbuf, buf, consumed)) {
+				err = -EFAULT;
+				break;
+			}
+			trace_nvhost_channel_write_cmdbuf(chname,
+				cmdbuf.mem, cmdbuf.words, cmdbuf.offset);
+			nvhost_job_add_gather(job,
+				cmdbuf.mem, cmdbuf.words, cmdbuf.offset);
+			hdr->num_cmdbufs--;
+		} else if (hdr->num_relocs) {
+			int numrelocs = remaining / sizeof(struct nvhost_reloc);
+			if (!numrelocs)
+				break;
+			numrelocs = min_t(int, numrelocs, priv->hdr.num_relocs);
+			consumed = numrelocs * sizeof(struct nvhost_reloc);
+			if (copy_from_user(&job->relocarray[job->num_relocs],
+					buf, consumed)) {
+				err = -EFAULT;
+				break;
+			}
+			while (numrelocs) {
+				struct nvhost_reloc *reloc =
+					&job->relocarray[job->num_relocs];
+				trace_nvhost_channel_write_reloc(chname,
+					reloc->cmdbuf_mem,
+					reloc->cmdbuf_offset,
+					reloc->target,
+					reloc->target_offset);
+				job->num_relocs++;
+				hdr->num_relocs--;
+				numrelocs--;
+			}
+		} else if (hdr->num_waitchks) {
+			int numwaitchks =
+				(remaining / sizeof(struct nvhost_waitchk));
+			if (!numwaitchks)
+				break;
+			numwaitchks = min_t(int,
+				numwaitchks, hdr->num_waitchks);
+			consumed = numwaitchks * sizeof(struct nvhost_waitchk);
+			if (copy_from_user(&job->waitchk[job->num_waitchk],
+					buf, consumed)) {
+				err = -EFAULT;
+				break;
+			}
+			trace_nvhost_channel_write_waitchks(
+			  chname, numwaitchks,
+			  hdr->waitchk_mask);
+			job->num_waitchk += numwaitchks;
+			hdr->num_waitchks -= numwaitchks;
+		} else if (priv->num_relocshifts) {
+			int next_shift =
+				job->num_relocs - priv->num_relocshifts;
+			int num =
+				(remaining / sizeof(struct nvhost_reloc_shift));
+			if (!num)
+				break;
+			num = min_t(int, num, priv->num_relocshifts);
+			consumed = num * sizeof(struct nvhost_reloc_shift);
+			if (copy_from_user(&job->relocshiftarray[next_shift],
+					buf, consumed)) {
+				err = -EFAULT;
+				break;
+			}
+			priv->num_relocshifts -= num;
+		} else {
+			err = -EFAULT;
+			break;
+		}
+		remaining -= consumed;
+		buf += consumed;
+	}
+
+	if (err < 0) {
+		dev_err(&priv->ch->dev->dev, "channel write error\n");
+		reset_submit(priv);
+		return err;
+	}
+
+	return count - remaining;
+}
+
+static int nvhost_ioctl_channel_flush(
+	struct nvhost_channel_userctx *ctx,
+	struct nvhost_get_param_args *args,
+	int null_kickoff)
+{
+	struct nvhost_device *ndev = to_nvhost_device(&ctx->ch->dev->dev);
+	int err;
+
+	trace_nvhost_ioctl_channel_flush(ctx->ch->dev->name);
+
+	if (!ctx->job ||
+	    ctx->hdr.num_relocs ||
+	    ctx->hdr.num_cmdbufs ||
+	    ctx->hdr.num_waitchks) {
+		reset_submit(ctx);
+		dev_err(&ndev->dev, "channel submit out of sync\n");
+		return -EFAULT;
+	}
+
+	err = nvhost_job_pin(ctx->job, &nvhost_get_host(ndev)->syncpt);
+	if (err) {
+		dev_warn(&ndev->dev, "nvhost_job_pin failed: %d\n", err);
+		return err;
+	}
+
+	if (nvhost_debug_null_kickoff_pid == current->tgid)
+		null_kickoff = 1;
+	ctx->job->null_kickoff = null_kickoff;
+
+	if ((nvhost_debug_force_timeout_pid == current->tgid) &&
+	    (nvhost_debug_force_timeout_channel == ctx->ch->chid)) {
+		ctx->timeout = nvhost_debug_force_timeout_val;
+	}
+
+	/* context switch if needed, and submit user's gathers to the channel */
+	err = nvhost_channel_submit(ctx->job);
+	args->value = ctx->job->syncpt_end;
+	if (err)
+		nvhost_job_unpin(ctx->job);
+
+	nvhost_job_put(ctx->job);
+	ctx->job = NULL;
+
+	return err;
+}
+
+static int nvhost_ioctl_channel_read_3d_reg(struct nvhost_channel_userctx *ctx,
+	struct nvhost_read_3d_reg_args *args)
+{
+	BUG_ON(!channel_op().read3dreg);
+	return channel_op().read3dreg(ctx->ch, ctx->hwctx,
+			args->offset, &args->value);
+}
+
+static long nvhost_channelctl(struct file *filp,
+	unsigned int cmd, unsigned long arg)
+{
+	struct nvhost_channel_userctx *priv = filp->private_data;
+	u8 buf[NVHOST_IOCTL_CHANNEL_MAX_ARG_SIZE];
+	int err = 0;
+
+	if ((_IOC_TYPE(cmd) != NVHOST_IOCTL_MAGIC) ||
+		(_IOC_NR(cmd) == 0) ||
+		(_IOC_NR(cmd) > NVHOST_IOCTL_CHANNEL_LAST) ||
+		(_IOC_SIZE(cmd) > NVHOST_IOCTL_CHANNEL_MAX_ARG_SIZE))
+		return -EFAULT;
+
+	if (_IOC_DIR(cmd) & _IOC_WRITE) {
+		if (copy_from_user(buf, (void __user *)arg, _IOC_SIZE(cmd)))
+			return -EFAULT;
+	}
+
+	switch (cmd) {
+	case NVHOST_IOCTL_CHANNEL_FLUSH:
+		err = nvhost_ioctl_channel_flush(priv, (void *)buf, 0);
+		break;
+	case NVHOST_IOCTL_CHANNEL_NULL_KICKOFF:
+		err = nvhost_ioctl_channel_flush(priv, (void *)buf, 1);
+		break;
+	case NVHOST_IOCTL_CHANNEL_SUBMIT_EXT:
+	{
+		struct nvhost_submit_hdr_ext *hdr;
+
+		if (priv->hdr.num_relocs ||
+		    priv->num_relocshifts ||
+		    priv->hdr.num_cmdbufs ||
+		    priv->hdr.num_waitchks) {
+			reset_submit(priv);
+			dev_err(&priv->ch->dev->dev,
+				"channel submit out of sync\n");
+			err = -EIO;
+			break;
+		}
+
+		hdr = (struct nvhost_submit_hdr_ext *)buf;
+		if (hdr->submit_version > NVHOST_SUBMIT_VERSION_MAX_SUPPORTED) {
+			dev_err(&priv->ch->dev->dev,
+				"submit version %d > max supported %d\n",
+				hdr->submit_version,
+				NVHOST_SUBMIT_VERSION_MAX_SUPPORTED);
+			err = -EINVAL;
+			break;
+		}
+		memcpy(&priv->hdr, hdr, sizeof(struct nvhost_submit_hdr_ext));
+		err = set_submit(priv);
+		trace_nvhost_ioctl_channel_submit(priv->ch->dev->name,
+			priv->hdr.submit_version,
+			priv->hdr.num_cmdbufs, priv->hdr.num_relocs,
+			priv->hdr.num_waitchks,
+			priv->hdr.syncpt_id, priv->hdr.syncpt_incrs);
+		break;
+	}
+	case NVHOST_IOCTL_CHANNEL_GET_SYNCPOINTS:
+		/* host syncpt ID is used by the RM (and never be given out) */
+		BUG_ON(priv->ch->dev->syncpts & (1 << NVSYNCPT_GRAPHICS_HOST));
+		((struct nvhost_get_param_args *)buf)->value =
+			priv->ch->dev->syncpts;
+		break;
+	case NVHOST_IOCTL_CHANNEL_GET_WAITBASES:
+		((struct nvhost_get_param_args *)buf)->value =
+			priv->ch->dev->waitbases;
+		break;
+	case NVHOST_IOCTL_CHANNEL_GET_MODMUTEXES:
+		((struct nvhost_get_param_args *)buf)->value =
+			priv->ch->dev->modulemutexes;
+		break;
+	case NVHOST_IOCTL_CHANNEL_SET_NVMAP_FD:
+	{
+		int fd = (int)((struct nvhost_set_nvmap_fd_args *)buf)->fd;
+		struct mem_mgr *new_client = mem_op().get_mgr_file(fd);
+
+		if (IS_ERR(new_client)) {
+			err = PTR_ERR(new_client);
+			break;
+		}
+
+		if (priv->memmgr)
+			mem_op().put_mgr(priv->memmgr);
+
+		priv->memmgr = new_client;
+		break;
+	}
+	case NVHOST_IOCTL_CHANNEL_READ_3D_REG:
+		err = nvhost_ioctl_channel_read_3d_reg(priv, (void *)buf);
+		break;
+	case NVHOST_IOCTL_CHANNEL_GET_CLK_RATE:
+	{
+		unsigned long rate;
+		struct nvhost_clk_rate_args *arg =
+				(struct nvhost_clk_rate_args *)buf;
+
+		err = nvhost_module_get_rate(priv->ch->dev, &rate, 0);
+		if (err == 0)
+			arg->rate = rate;
+		break;
+	}
+	case NVHOST_IOCTL_CHANNEL_SET_CLK_RATE:
+	{
+		struct nvhost_clk_rate_args *arg =
+				(struct nvhost_clk_rate_args *)buf;
+		unsigned long rate = (unsigned long)arg->rate;
+
+		err = nvhost_module_set_rate(priv->ch->dev, priv, rate, 0);
+		break;
+	}
+	case NVHOST_IOCTL_CHANNEL_SET_TIMEOUT:
+		priv->timeout =
+			(u32)((struct nvhost_set_timeout_args *)buf)->timeout;
+		dev_dbg(&priv->ch->dev->dev,
+			"%s: setting buffer timeout (%d ms) for userctx 0x%p\n",
+			__func__, priv->timeout, priv);
+		break;
+	case NVHOST_IOCTL_CHANNEL_GET_TIMEDOUT:
+		((struct nvhost_get_param_args *)buf)->value =
+				priv->hwctx->has_timedout;
+		break;
+	case NVHOST_IOCTL_CHANNEL_SET_PRIORITY:
+		priv->priority =
+			(u32)((struct nvhost_set_priority_args *)buf)->priority;
+		break;
+	default:
+		err = -ENOTTY;
+		break;
+	}
+
+	if ((err == 0) && (_IOC_DIR(cmd) & _IOC_READ))
+		err = copy_to_user((void __user *)arg, buf, _IOC_SIZE(cmd));
+
+	return err;
+}
+
+static const struct file_operations nvhost_channelops = {
+	.owner = THIS_MODULE,
+	.release = nvhost_channelrelease,
+	.open = nvhost_channelopen,
+	.write = nvhost_channelwrite,
+	.unlocked_ioctl = nvhost_channelctl
+};
+
+int nvhost_client_user_init(struct nvhost_device *dev)
+{
+	int err, devno;
+
+	struct nvhost_channel *ch = dev->channel;
+	err = alloc_chrdev_region(&devno, 0, 1, IFACE_NAME);
+	if (err < 0) {
+		dev_err(&dev->dev, "failed to allocate devno\n");
+		goto fail;
+	}
+
+	cdev_init(&ch->cdev, &nvhost_channelops);
+	ch->cdev.owner = THIS_MODULE;
+
+	err = cdev_add(&ch->cdev, devno, 1);
+	if (err < 0) {
+		dev_err(&dev->dev,
+			"failed to add chan %i cdev\n", dev->index);
+		goto fail;
+	}
+	ch->node = device_create(nvhost_get_host(dev)->nvhost_class,
+			NULL, devno, NULL,
+			IFACE_NAME "-%s", dev->name);
+	if (IS_ERR(ch->node)) {
+		err = PTR_ERR(ch->node);
+		dev_err(&dev->dev,
+			"failed to create %s channel device\n", dev->name);
+		goto fail;
+	}
+
+	return 0;
+fail:
+	return err;
+}
+
+int nvhost_client_device_init(struct nvhost_device *dev)
+{
+	int err;
+	struct nvhost_master *nvhost_master = nvhost_get_host(dev);
+	struct nvhost_channel *ch;
+
+	ch = nvhost_alloc_channel(dev);
+	if (ch == NULL)
+		return -ENODEV;
+
+	/* store the pointer to this device for channel */
+	ch->dev = dev;
+
+	err = nvhost_channel_init(ch, nvhost_master, dev->index);
+	if (err)
+		goto fail;
+
+	err = nvhost_client_user_init(dev);
+	if (err)
+		goto fail;
+
+	err = nvhost_module_init(dev);
+	if (err)
+		goto fail;
+
+	dev_info(&dev->dev, "initialized\n");
+
+	return 0;
+
+fail:
+	/* Add clean-up */
+	nvhost_free_channel(ch);
+	return err;
+}
+EXPORT_SYMBOL(nvhost_client_device_init);
+
+int nvhost_client_device_suspend(struct nvhost_device *dev)
+{
+	int ret = 0;
+
+	ret = nvhost_channel_suspend(dev->channel);
+	if (ret)
+		return ret;
+
+	dev_info(&dev->dev, "suspend status: %d\n", ret);
+
+	return ret;
+}
+
+int nvhost_client_device_get_resources(struct nvhost_device *dev)
+{
+	struct resource *r = NULL;
+	void __iomem *regs = NULL;
+	struct resource *reg_mem = NULL;
+
+	r = nvhost_get_resource(dev, IORESOURCE_MEM, 0);
+	if (!r)
+		goto fail;
+
+	reg_mem = request_mem_region(r->start, resource_size(r), dev->name);
+	if (!reg_mem)
+		goto fail;
+
+	regs = ioremap(r->start, resource_size(r));
+	if (!regs)
+		goto fail;
+
+	dev->reg_mem = reg_mem;
+	dev->aperture = regs;
+
+	return 0;
+
+fail:
+	if (reg_mem)
+		release_mem_region(r->start, resource_size(r));
+	if (regs)
+		iounmap(regs);
+
+	dev_err(&dev->dev, "failed to get register memory\n");
+
+	return -ENXIO;
+}
+EXPORT_SYMBOL(nvhost_client_device_get_resources);
+
+void nvhost_client_device_put_resources(struct nvhost_device *dev)
+{
+	struct resource *r;
+
+	r = nvhost_get_resource(dev, IORESOURCE_MEM, 0);
+	BUG_ON(!r);
+
+	iounmap(dev->aperture);
+
+	release_mem_region(r->start, resource_size(r));
+}
+EXPORT_SYMBOL(nvhost_client_device_put_resources);
diff --git a/drivers/video/tegra/host/bus_client.h b/drivers/video/tegra/host/bus_client.h
new file mode 100644
index 000000000000..8c7bdc9faefe
--- /dev/null
+++ b/drivers/video/tegra/host/bus_client.h
@@ -0,0 +1,42 @@
+/*
+ * drivers/video/tegra/host/bus_client.h
+ *
+ * Tegra Graphics Host client
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_BUS_CLIENT_H
+#define __NVHOST_BUS_CLIENT_H
+
+#include <linux/types.h>
+struct nvhost_device;
+
+int nvhost_read_module_regs(struct nvhost_device *ndev,
+			u32 offset, int count, u32 *values);
+
+int nvhost_write_module_regs(struct nvhost_device *ndev,
+			u32 offset, int count, const u32 *values);
+
+int nvhost_client_user_init(struct nvhost_device *dev);
+
+int nvhost_client_device_init(struct nvhost_device *dev);
+
+int nvhost_client_device_suspend(struct nvhost_device *dev);
+
+int nvhost_client_device_get_resources(struct nvhost_device *dev);
+void nvhost_client_device_put_resources(struct nvhost_device *dev);
+
+#endif
diff --git a/drivers/video/tegra/host/chip_support.c b/drivers/video/tegra/host/chip_support.c
new file mode 100644
index 000000000000..9abb1fa026a4
--- /dev/null
+++ b/drivers/video/tegra/host/chip_support.c
@@ -0,0 +1,56 @@
+/*
+ * drivers/video/tegra/host/chip_support.c
+ *
+ * Tegra Graphics Host Chip support module
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/errno.h>
+
+#include <mach/hardware.h>
+
+#include "bus.h"
+#include "chip_support.h"
+#include "t20/t20.h"
+#include "t30/t30.h"
+
+struct nvhost_chip_support *nvhost_get_chip_ops(void)
+{
+	return (nvhost_bus_get())->nvhost_chip_ops;
+}
+
+int nvhost_init_chip_support(struct nvhost_master *host)
+{
+	int err = 0;
+	struct nvhost_chip_support *chip_ops;
+
+	chip_ops = nvhost_get_chip_ops();
+
+	switch (tegra_get_chipid()) {
+	case TEGRA_CHIPID_TEGRA2:
+		err = nvhost_init_t20_support(host, chip_ops);
+		break;
+
+	case TEGRA_CHIPID_TEGRA3:
+		err = nvhost_init_t30_support(host, chip_ops);
+		break;
+
+	default:
+		err = -ENODEV;
+	}
+
+	return err;
+}
diff --git a/drivers/video/tegra/host/chip_support.h b/drivers/video/tegra/host/chip_support.h
new file mode 100644
index 000000000000..412ce8b65466
--- /dev/null
+++ b/drivers/video/tegra/host/chip_support.h
@@ -0,0 +1,181 @@
+/*
+ * drivers/video/tegra/host/chip_support.h
+ *
+ * Tegra Graphics Host Chip Support
+ *
+ * Copyright (c) 2011-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef _NVHOST_CHIP_SUPPORT_H_
+#define _NVHOST_CHIP_SUPPORT_H_
+
+#include <linux/types.h>
+#include "bus.h"
+
+struct output;
+
+struct nvhost_master;
+struct nvhost_intr;
+struct nvhost_syncpt;
+struct nvhost_userctx_timeout;
+struct nvhost_channel;
+struct nvhost_hwctx;
+struct nvhost_cdma;
+struct nvhost_job;
+struct push_buffer;
+struct nvhost_syncpt;
+struct dentry;
+struct nvhost_job;
+struct nvhost_intr_syncpt;
+struct mem_handle;
+struct mem_mgr;
+struct nvhost_device;
+
+struct nvhost_channel_ops {
+	int (*init)(struct nvhost_channel *,
+		    struct nvhost_master *,
+		    int chid);
+	int (*submit)(struct nvhost_job *job);
+	int (*read3dreg)(struct nvhost_channel *channel,
+			struct nvhost_hwctx *hwctx,
+			u32 offset,
+			u32 *value);
+	int (*save_context)(struct nvhost_channel *channel);
+	int (*drain_read_fifo)(struct nvhost_channel *ch,
+		u32 *ptr, unsigned int count, unsigned int *pending);
+};
+
+struct nvhost_cdma_ops {
+	void (*start)(struct nvhost_cdma *);
+	void (*stop)(struct nvhost_cdma *);
+	void (*kick)(struct  nvhost_cdma *);
+	int (*timeout_init)(struct nvhost_cdma *,
+			    u32 syncpt_id);
+	void (*timeout_destroy)(struct nvhost_cdma *);
+	void (*timeout_teardown_begin)(struct nvhost_cdma *);
+	void (*timeout_teardown_end)(struct nvhost_cdma *,
+				     u32 getptr);
+	void (*timeout_cpu_incr)(struct nvhost_cdma *,
+				 u32 getptr,
+				 u32 syncpt_incrs,
+				 u32 syncval,
+				 u32 nr_slots,
+				 u32 waitbases);
+};
+
+struct nvhost_pushbuffer_ops {
+	void (*reset)(struct push_buffer *);
+	int (*init)(struct push_buffer *);
+	void (*destroy)(struct push_buffer *);
+	void (*push_to)(struct push_buffer *,
+			struct mem_mgr *, struct mem_handle *,
+			u32 op1, u32 op2);
+	void (*pop_from)(struct push_buffer *,
+			 unsigned int slots);
+	u32 (*space)(struct push_buffer *);
+	u32 (*putptr)(struct push_buffer *);
+};
+
+struct nvhost_debug_ops {
+	void (*debug_init)(struct dentry *de);
+	void (*show_channel_cdma)(struct nvhost_master *,
+				  struct nvhost_channel *,
+				  struct output *,
+				  int chid);
+	void (*show_channel_fifo)(struct nvhost_master *,
+				  struct nvhost_channel *,
+				  struct output *,
+				  int chid);
+	void (*show_mlocks)(struct nvhost_master *m,
+			    struct output *o);
+
+};
+
+struct nvhost_syncpt_ops {
+	void (*reset)(struct nvhost_syncpt *, u32 id);
+	void (*reset_wait_base)(struct nvhost_syncpt *, u32 id);
+	void (*read_wait_base)(struct nvhost_syncpt *, u32 id);
+	u32 (*update_min)(struct nvhost_syncpt *, u32 id);
+	void (*cpu_incr)(struct nvhost_syncpt *, u32 id);
+	int (*patch_wait)(struct nvhost_syncpt *sp,
+			void *patch_addr);
+	void (*debug)(struct nvhost_syncpt *);
+	const char * (*name)(struct nvhost_syncpt *, u32 id);
+	int (*mutex_try_lock)(struct nvhost_syncpt *,
+			      unsigned int idx);
+	void (*mutex_unlock)(struct nvhost_syncpt *,
+			     unsigned int idx);
+};
+
+struct nvhost_intr_ops {
+	void (*init_host_sync)(struct nvhost_intr *);
+	void (*set_host_clocks_per_usec)(
+		struct nvhost_intr *, u32 clocks);
+	void (*set_syncpt_threshold)(
+		struct nvhost_intr *, u32 id, u32 thresh);
+	void (*enable_syncpt_intr)(struct nvhost_intr *, u32 id);
+	void (*disable_syncpt_intr)(struct nvhost_intr *, u32 id);
+	void (*disable_all_syncpt_intrs)(struct nvhost_intr *);
+	int  (*request_host_general_irq)(struct nvhost_intr *);
+	void (*free_host_general_irq)(struct nvhost_intr *);
+	int (*request_syncpt_irq)(struct nvhost_intr_syncpt *syncpt);
+};
+
+struct nvhost_dev_ops {
+	struct nvhost_channel *(*alloc_nvhost_channel)(
+			struct nvhost_device *dev);
+	void (*free_nvhost_channel)(struct nvhost_channel *ch);
+};
+
+struct nvhost_mem_ops {
+	struct mem_mgr *(*alloc_mgr)(void);
+	void (*put_mgr)(struct mem_mgr *);
+	struct mem_mgr *(*get_mgr)(struct mem_mgr *);
+	struct mem_mgr *(*get_mgr_file)(int fd);
+	struct mem_handle *(*alloc)(struct mem_mgr *,
+			size_t size, size_t align,
+			int flags);
+	struct mem_handle *(*get)(struct mem_mgr *, u32 id);
+	void (*put)(struct mem_mgr *, struct mem_handle *);
+	phys_addr_t (*pin)(struct mem_mgr *, struct mem_handle *);
+	void (*unpin)(struct mem_mgr *, struct mem_handle *);
+	void *(*mmap)(struct mem_handle *);
+	void (*munmap)(struct mem_handle *, void *);
+};
+
+struct nvhost_chip_support {
+	struct nvhost_channel_ops channel;
+	struct nvhost_cdma_ops cdma;
+	struct nvhost_pushbuffer_ops push_buffer;
+	struct nvhost_debug_ops debug;
+	struct nvhost_syncpt_ops syncpt;
+	struct nvhost_intr_ops intr;
+	struct nvhost_dev_ops nvhost_dev;
+	struct nvhost_mem_ops mem;
+};
+
+struct nvhost_chip_support *nvhost_get_chip_ops(void);
+
+#define host_device_op()	nvhost_get_chip_ops()->nvhost_dev
+#define channel_cdma_op()	nvhost_get_chip_ops()->cdma
+#define channel_op()		nvhost_get_chip_ops()->channel
+#define syncpt_op()		nvhost_get_chip_ops()->syncpt
+#define intr_op()		nvhost_get_chip_ops()->intr
+#define cdma_op()		nvhost_get_chip_ops()->cdma
+#define cdma_pb_op()		nvhost_get_chip_ops()->push_buffer
+#define mem_op()		(nvhost_get_chip_ops()->mem)
+
+int nvhost_init_chip_support(struct nvhost_master *);
+
+#endif /* _NVHOST_CHIP_SUPPORT_H_ */
diff --git a/drivers/video/tegra/host/debug.c b/drivers/video/tegra/host/debug.c
new file mode 100644
index 000000000000..58f9348b84bd
--- /dev/null
+++ b/drivers/video/tegra/host/debug.c
@@ -0,0 +1,234 @@
+/*
+ * drivers/video/tegra/host/debug.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/debugfs.h>
+#include <linux/seq_file.h>
+
+#include <linux/io.h>
+
+#include "bus.h"
+#include "dev.h"
+#include "debug.h"
+#include "nvhost_acm.h"
+#include "nvhost_channel.h"
+#include "chip_support.h"
+
+pid_t nvhost_debug_null_kickoff_pid;
+unsigned int nvhost_debug_trace_cmdbuf;
+
+pid_t nvhost_debug_force_timeout_pid;
+u32 nvhost_debug_force_timeout_val;
+u32 nvhost_debug_force_timeout_channel;
+
+void nvhost_debug_output(struct output *o, const char* fmt, ...)
+{
+	va_list args;
+	int len;
+
+	va_start(args, fmt);
+	len = vsnprintf(o->buf, sizeof(o->buf), fmt, args);
+	va_end(args);
+	o->fn(o->ctx, o->buf, len);
+}
+
+static int show_channels(struct device *dev, void *data)
+{
+	struct nvhost_channel *ch;
+	struct nvhost_device *nvdev = to_nvhost_device(dev);
+	struct output *o = data;
+	struct nvhost_master *m;
+
+	if (nvdev == NULL)
+		return 0;
+
+	m = nvhost_get_host(nvdev);
+	ch = nvdev->channel;
+	if (ch) {
+		mutex_lock(&ch->reflock);
+		if (ch->refcount) {
+			mutex_lock(&ch->cdma.lock);
+			nvhost_get_chip_ops()->debug.show_channel_fifo(m, ch, o, nvdev->index);
+			nvhost_get_chip_ops()->debug.show_channel_cdma(m, ch, o, nvdev->index);
+			mutex_unlock(&ch->cdma.lock);
+		}
+		mutex_unlock(&ch->reflock);
+	}
+
+	return 0;
+}
+
+static void show_syncpts(struct nvhost_master *m, struct output *o)
+{
+	int i;
+	BUG_ON(!nvhost_get_chip_ops()->syncpt.name);
+	nvhost_debug_output(o, "---- syncpts ----\n");
+	for (i = 0; i < nvhost_syncpt_nb_pts(&m->syncpt); i++) {
+		u32 max = nvhost_syncpt_read_max(&m->syncpt, i);
+		u32 min = nvhost_syncpt_update_min(&m->syncpt, i);
+		if (!min && !max)
+			continue;
+		nvhost_debug_output(o, "id %d (%s) min %d max %d\n",
+				i, nvhost_get_chip_ops()->syncpt.name(&m->syncpt, i),
+				min, max);
+	}
+
+	for (i = 0; i < nvhost_syncpt_nb_pts(&m->syncpt); i++) {
+		u32 base_val;
+		base_val = nvhost_syncpt_read_wait_base(&m->syncpt, i);
+		if (base_val)
+			nvhost_debug_output(o, "waitbase id %d val %d\n",
+					i, base_val);
+	}
+
+	nvhost_debug_output(o, "\n");
+}
+
+static void show_all(struct nvhost_master *m, struct output *o)
+{
+	nvhost_module_busy(m->dev);
+
+	nvhost_get_chip_ops()->debug.show_mlocks(m, o);
+	show_syncpts(m, o);
+	nvhost_debug_output(o, "---- channels ----\n");
+	bus_for_each_dev(&(nvhost_bus_get())->nvhost_bus_type, NULL, o,
+			show_channels);
+
+	nvhost_module_idle(m->dev);
+}
+
+#ifdef CONFIG_DEBUG_FS
+static int show_channels_no_fifo(struct device *dev, void *data)
+{
+	struct nvhost_channel *ch;
+	struct nvhost_device *nvdev = to_nvhost_device(dev);
+	struct output *o = data;
+	struct nvhost_master *m;
+
+	if (nvdev == NULL)
+		return 0;
+
+	m = nvhost_get_host(nvdev);
+	ch = nvdev->channel;
+	if (ch) {
+		mutex_lock(&ch->reflock);
+		if (ch->refcount) {
+			mutex_lock(&ch->cdma.lock);
+			nvhost_get_chip_ops()->debug.show_channel_cdma(m,
+					ch, o, nvdev->index);
+			mutex_unlock(&ch->cdma.lock);
+		}
+		mutex_unlock(&ch->reflock);
+	}
+
+	return 0;
+}
+
+static void show_all_no_fifo(struct nvhost_master *m, struct output *o)
+{
+	nvhost_module_busy(m->dev);
+
+	nvhost_get_chip_ops()->debug.show_mlocks(m, o);
+	show_syncpts(m, o);
+	nvhost_debug_output(o, "---- channels ----\n");
+	bus_for_each_dev(&(nvhost_bus_get())->nvhost_bus_type, NULL, o,
+			show_channels_no_fifo);
+
+	nvhost_module_idle(m->dev);
+}
+
+static int nvhost_debug_show_all(struct seq_file *s, void *unused)
+{
+	struct output o = {
+		.fn = write_to_seqfile,
+		.ctx = s
+	};
+	show_all(s->private, &o);
+	return 0;
+}
+static int nvhost_debug_show(struct seq_file *s, void *unused)
+{
+	struct output o = {
+		.fn = write_to_seqfile,
+		.ctx = s
+	};
+	show_all_no_fifo(s->private, &o);
+	return 0;
+}
+
+static int nvhost_debug_open_all(struct inode *inode, struct file *file)
+{
+	return single_open(file, nvhost_debug_show_all, inode->i_private);
+}
+
+static const struct file_operations nvhost_debug_all_fops = {
+	.open		= nvhost_debug_open_all,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int nvhost_debug_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, nvhost_debug_show, inode->i_private);
+}
+
+static const struct file_operations nvhost_debug_fops = {
+	.open		= nvhost_debug_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+void nvhost_debug_init(struct nvhost_master *master)
+{
+	struct dentry *de = debugfs_create_dir("tegra_host", NULL);
+
+	debugfs_create_file("status", S_IRUGO, de,
+			master, &nvhost_debug_fops);
+	debugfs_create_file("status_all", S_IRUGO, de,
+			master, &nvhost_debug_all_fops);
+
+	debugfs_create_u32("null_kickoff_pid", S_IRUGO|S_IWUSR, de,
+			&nvhost_debug_null_kickoff_pid);
+	debugfs_create_u32("trace_cmdbuf", S_IRUGO|S_IWUSR, de,
+			&nvhost_debug_trace_cmdbuf);
+
+	if (nvhost_get_chip_ops()->debug.debug_init)
+		nvhost_get_chip_ops()->debug.debug_init(de);
+
+	debugfs_create_u32("force_timeout_pid", S_IRUGO|S_IWUSR, de,
+			&nvhost_debug_force_timeout_pid);
+	debugfs_create_u32("force_timeout_val", S_IRUGO|S_IWUSR, de,
+			&nvhost_debug_force_timeout_val);
+	debugfs_create_u32("force_timeout_channel", S_IRUGO|S_IWUSR, de,
+			&nvhost_debug_force_timeout_channel);
+}
+#else
+void nvhost_debug_init(struct nvhost_master *master)
+{
+}
+#endif
+
+void nvhost_debug_dump(struct nvhost_master *master)
+{
+	struct output o = {
+		.fn = write_to_printk
+	};
+	show_all(master, &o);
+}
diff --git a/drivers/video/tegra/host/debug.h b/drivers/video/tegra/host/debug.h
new file mode 100644
index 000000000000..3dc156ab4741
--- /dev/null
+++ b/drivers/video/tegra/host/debug.h
@@ -0,0 +1,50 @@
+/*
+ * drivers/video/tegra/host/debug.h
+ *
+ * Tegra Graphics Host Debug
+ *
+ * Copyright (c) 2011-2012 NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef __NVHOST_DEBUG_H
+#define __NVHOST_DEBUG_H
+
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+
+struct output {
+	void (*fn)(void *ctx, const char* str, size_t len);
+	void *ctx;
+	char buf[256];
+};
+
+static inline void write_to_seqfile(void *ctx, const char* str, size_t len)
+{
+	seq_write((struct seq_file *)ctx, str, len);
+}
+
+static inline void write_to_printk(void *ctx, const char* str, size_t len)
+{
+	printk(KERN_INFO "%s", str);
+}
+
+void nvhost_debug_output(struct output *o, const char* fmt, ...);
+
+extern pid_t nvhost_debug_null_kickoff_pid;
+extern pid_t nvhost_debug_force_timeout_pid;
+extern u32 nvhost_debug_force_timeout_val;
+extern u32 nvhost_debug_force_timeout_channel;
+extern unsigned int nvhost_debug_trace_cmdbuf;
+
+#endif /*__NVHOST_DEBUG_H */
diff --git a/drivers/video/tegra/host/dev.c b/drivers/video/tegra/host/dev.c
new file mode 100644
index 000000000000..6200507548f7
--- /dev/null
+++ b/drivers/video/tegra/host/dev.c
@@ -0,0 +1,31 @@
+/*
+ * drivers/video/tegra/host/dev.c
+ *
+ * Tegra Graphics Host Driver Entrypoint
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#define CREATE_TRACE_POINTS
+#include <trace/events/nvhost.h>
+
+#include <linux/nvhost.h>
+#include <mach/gpufuse.h>
+
+MODULE_AUTHOR("NVIDIA");
+MODULE_DESCRIPTION("Graphics host driver for Tegra products");
+MODULE_VERSION("1.0");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform-nvhost");
diff --git a/drivers/video/tegra/host/dev.h b/drivers/video/tegra/host/dev.h
new file mode 100644
index 000000000000..53ec2de13aa1
--- /dev/null
+++ b/drivers/video/tegra/host/dev.h
@@ -0,0 +1,24 @@
+/*
+ * drivers/video/tegra/host/dev.h
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef NVHOST_DEV_H
+#define NVHOST_DEV_H
+
+#include "host1x/host1x.h"
+
+#endif
diff --git a/drivers/video/tegra/host/gr2d/Makefile b/drivers/video/tegra/host/gr2d/Makefile
new file mode 100644
index 000000000000..a79a2101677b
--- /dev/null
+++ b/drivers/video/tegra/host/gr2d/Makefile
@@ -0,0 +1,7 @@
+GCOV_PROFILE := y
+EXTRA_CFLAGS += -Idrivers/video/tegra/host
+
+nvhost-gr2d-objs  = \
+		gr2d.o
+
+obj-$(CONFIG_TEGRA_GRHOST) += nvhost-gr2d.o
diff --git a/drivers/video/tegra/host/gr2d/gr2d.c b/drivers/video/tegra/host/gr2d/gr2d.c
new file mode 100644
index 000000000000..56752eba5951
--- /dev/null
+++ b/drivers/video/tegra/host/gr2d/gr2d.c
@@ -0,0 +1,73 @@
+/*
+ * drivers/video/tegra/host/gr2d/gr2d.c
+ *
+ * Tegra Graphics 2D
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "dev.h"
+#include "bus_client.h"
+
+static int __devinit gr2d_probe(struct nvhost_device *dev,
+	struct nvhost_device_id *id_table)
+{
+	return nvhost_client_device_init(dev);
+}
+
+static int __exit gr2d_remove(struct nvhost_device *dev)
+{
+	/* Add clean-up */
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int gr2d_suspend(struct nvhost_device *dev, pm_message_t state)
+{
+	return nvhost_client_device_suspend(dev);
+}
+
+static int gr2d_resume(struct nvhost_device *dev)
+{
+	dev_info(&dev->dev, "resuming\n");
+	return 0;
+}
+#endif
+
+static struct nvhost_driver gr2d_driver = {
+	.probe = gr2d_probe,
+	.remove = __exit_p(gr2d_remove),
+#ifdef CONFIG_PM
+	.suspend = gr2d_suspend,
+	.resume = gr2d_resume,
+#endif
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "gr2d",
+	}
+};
+
+static int __init gr2d_init(void)
+{
+	return nvhost_driver_register(&gr2d_driver);
+}
+
+static void __exit gr2d_exit(void)
+{
+	nvhost_driver_unregister(&gr2d_driver);
+}
+
+module_init(gr2d_init);
+module_exit(gr2d_exit);
diff --git a/drivers/video/tegra/host/gr3d/Makefile b/drivers/video/tegra/host/gr3d/Makefile
new file mode 100644
index 000000000000..dfbd078ab423
--- /dev/null
+++ b/drivers/video/tegra/host/gr3d/Makefile
@@ -0,0 +1,10 @@
+GCOV_PROFILE := y
+EXTRA_CFLAGS += -Idrivers/video/tegra/host
+
+nvhost-gr3d-objs  = \
+		gr3d.o \
+		gr3d_t20.o \
+		gr3d_t30.o \
+		scale3d.o
+
+obj-$(CONFIG_TEGRA_GRHOST) += nvhost-gr3d.o
diff --git a/drivers/video/tegra/host/gr3d/gr3d.c b/drivers/video/tegra/host/gr3d/gr3d.c
new file mode 100644
index 000000000000..775c77b0e88d
--- /dev/null
+++ b/drivers/video/tegra/host/gr3d/gr3d.c
@@ -0,0 +1,265 @@
+/*
+ * drivers/video/tegra/host/gr3d/gr3d.c
+ *
+ * Tegra Graphics Host 3D
+ *
+ * Copyright (c) 2012 NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/slab.h>
+#include <mach/gpufuse.h>
+
+#include "t20/t20.h"
+#include "host1x/host1x01_hardware.h"
+#include "nvhost_hwctx.h"
+#include "dev.h"
+#include "gr3d.h"
+#include "gr3d_t20.h"
+#include "gr3d_t30.h"
+#include "scale3d.h"
+#include "bus_client.h"
+#include "nvhost_channel.h"
+#include "nvhost_memmgr.h"
+#include "chip_support.h"
+
+void nvhost_3dctx_restore_begin(struct host1x_hwctx_handler *p, u32 *ptr)
+{
+	/* set class to host */
+	ptr[0] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					host1x_uclass_incr_syncpt_base_r(), 1);
+	/* increment sync point base */
+	ptr[1] = nvhost_class_host_incr_syncpt_base(p->waitbase,
+			p->restore_incrs);
+	/* set class to 3D */
+	ptr[2] = nvhost_opcode_setclass(NV_GRAPHICS_3D_CLASS_ID, 0, 0);
+	/* program PSEQ_QUAD_ID */
+	ptr[3] = nvhost_opcode_imm(AR3D_PSEQ_QUAD_ID, 0);
+}
+
+void nvhost_3dctx_restore_direct(u32 *ptr, u32 start_reg, u32 count)
+{
+	ptr[0] = nvhost_opcode_incr(start_reg, count);
+}
+
+void nvhost_3dctx_restore_indirect(u32 *ptr, u32 offset_reg, u32 offset,
+			u32 data_reg, u32 count)
+{
+	ptr[0] = nvhost_opcode_imm(offset_reg, offset);
+	ptr[1] = nvhost_opcode_nonincr(data_reg, count);
+}
+
+void nvhost_3dctx_restore_end(struct host1x_hwctx_handler *p, u32 *ptr)
+{
+	/* syncpt increment to track restore gather. */
+	ptr[0] = nvhost_opcode_imm_incr_syncpt(
+			host1x_uclass_incr_syncpt_cond_op_done_v(), p->syncpt);
+}
+
+/*** ctx3d ***/
+struct host1x_hwctx *nvhost_3dctx_alloc_common(struct host1x_hwctx_handler *p,
+		struct nvhost_channel *ch, bool map_restore)
+{
+	struct mem_mgr *memmgr = nvhost_get_host(ch->dev)->memmgr;
+	struct host1x_hwctx *ctx;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return NULL;
+	ctx->restore = mem_op().alloc(memmgr, p->restore_size * 4, 32,
+		map_restore ? mem_mgr_flag_write_combine
+			    : mem_mgr_flag_uncacheable);
+	if (IS_ERR_OR_NULL(ctx->restore)) {
+		ctx->restore = NULL;
+		goto fail;
+	}
+
+	if (map_restore) {
+		ctx->restore_virt = mem_op().mmap(ctx->restore);
+		if (!ctx->restore_virt)
+			goto fail;
+	} else
+		ctx->restore_virt = NULL;
+
+	kref_init(&ctx->hwctx.ref);
+	ctx->hwctx.h = &p->h;
+	ctx->hwctx.channel = ch;
+	ctx->hwctx.valid = false;
+	ctx->save_incrs = p->save_incrs;
+	ctx->save_thresh = p->save_thresh;
+	ctx->save_slots = p->save_slots;
+	ctx->restore_phys = mem_op().pin(memmgr, ctx->restore);
+	if (IS_ERR_VALUE(ctx->restore_phys))
+		goto fail;
+
+	ctx->restore_size = p->restore_size;
+	ctx->restore_incrs = p->restore_incrs;
+	return ctx;
+
+fail:
+	if (map_restore && ctx->restore_virt) {
+		mem_op().munmap(ctx->restore, ctx->restore_virt);
+		ctx->restore_virt = NULL;
+	}
+	mem_op().put(memmgr, ctx->restore);
+	ctx->restore = NULL;
+	kfree(ctx);
+	return NULL;
+}
+
+void nvhost_3dctx_get(struct nvhost_hwctx *ctx)
+{
+	kref_get(&ctx->ref);
+}
+
+void nvhost_3dctx_free(struct kref *ref)
+{
+	struct nvhost_hwctx *nctx = container_of(ref, struct nvhost_hwctx, ref);
+	struct host1x_hwctx *ctx = to_host1x_hwctx(nctx);
+	struct mem_mgr *memmgr = nvhost_get_host(nctx->channel->dev)->memmgr;
+
+	if (ctx->restore_virt) {
+		mem_op().munmap(ctx->restore, ctx->restore_virt);
+		ctx->restore_virt = NULL;
+	}
+	mem_op().unpin(memmgr, ctx->restore);
+	ctx->restore_phys = 0;
+	mem_op().put(memmgr, ctx->restore);
+	ctx->restore = NULL;
+	kfree(ctx);
+}
+
+void nvhost_3dctx_put(struct nvhost_hwctx *ctx)
+{
+	kref_put(&ctx->ref, nvhost_3dctx_free);
+}
+
+int nvhost_gr3d_prepare_power_off(struct nvhost_device *dev)
+{
+	return nvhost_channel_save_context(dev->channel);
+}
+
+enum gr3d_ip_ver {
+	gr3d_01 = 1,
+	gr3d_02,
+};
+
+struct gr3d_desc {
+	void (*finalize_poweron)(struct nvhost_device *dev);
+	void (*busy)(struct nvhost_device *);
+	void (*idle)(struct nvhost_device *);
+	void (*suspend_ndev)(struct nvhost_device *);
+	void (*init)(struct nvhost_device *dev);
+	void (*deinit)(struct nvhost_device *dev);
+	int (*prepare_poweroff)(struct nvhost_device *dev);
+	struct nvhost_hwctx_handler *(*alloc_hwctx_handler)(u32 syncpt,
+			u32 waitbase, struct nvhost_channel *ch);
+};
+
+static const struct gr3d_desc gr3d[] = {
+	[gr3d_01] = {
+		.finalize_poweron = NULL,
+		.busy = NULL,
+		.idle = NULL,
+		.suspend_ndev = NULL,
+		.init = NULL,
+		.deinit = NULL,
+		.prepare_poweroff = nvhost_gr3d_prepare_power_off,
+		.alloc_hwctx_handler = nvhost_gr3d_t20_ctxhandler_init,
+	},
+	[gr3d_02] = {
+		.finalize_poweron = NULL,
+		.busy = nvhost_scale3d_notify_busy,
+		.idle = nvhost_scale3d_notify_idle,
+		.suspend_ndev = nvhost_scale3d_suspend,
+		.init = nvhost_scale3d_init,
+		.deinit = nvhost_scale3d_deinit,
+		.prepare_poweroff = nvhost_gr3d_prepare_power_off,
+		.alloc_hwctx_handler = nvhost_gr3d_t30_ctxhandler_init,
+	},
+};
+
+static struct nvhost_device_id gr3d_id[] = {
+	{ "gr3d", gr3d_01 },
+	{ "gr3d", gr3d_02 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(nvhost, gr3d_id);
+
+static int __devinit gr3d_probe(struct nvhost_device *dev,
+	struct nvhost_device_id *id_table)
+{
+	int index = 0;
+	struct nvhost_driver *drv = to_nvhost_driver(dev->dev.driver);
+
+	index = id_table->version;
+
+	drv->finalize_poweron		= gr3d[index].finalize_poweron;
+	drv->busy			= gr3d[index].busy;
+	drv->idle			= gr3d[index].idle;
+	drv->suspend_ndev		= gr3d[index].suspend_ndev;
+	drv->init			= gr3d[index].init;
+	drv->deinit			= gr3d[index].deinit;
+	drv->prepare_poweroff		= gr3d[index].prepare_poweroff;
+	drv->alloc_hwctx_handler	= gr3d[index].alloc_hwctx_handler;
+
+	return nvhost_client_device_init(dev);
+}
+
+static int __exit gr3d_remove(struct nvhost_device *dev)
+{
+	/* Add clean-up */
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int gr3d_suspend(struct nvhost_device *dev, pm_message_t state)
+{
+	return nvhost_client_device_suspend(dev);
+}
+
+static int gr3d_resume(struct nvhost_device *dev)
+{
+	dev_info(&dev->dev, "resuming\n");
+	return 0;
+}
+#endif
+
+static struct nvhost_driver gr3d_driver = {
+	.probe = gr3d_probe,
+	.remove = __exit_p(gr3d_remove),
+#ifdef CONFIG_PM
+	.suspend = gr3d_suspend,
+	.resume = gr3d_resume,
+#endif
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "gr3d",
+	},
+	.id_table = gr3d_id,
+};
+
+static int __init gr3d_init(void)
+{
+	return nvhost_driver_register(&gr3d_driver);
+}
+
+static void __exit gr3d_exit(void)
+{
+	nvhost_driver_unregister(&gr3d_driver);
+}
+
+module_init(gr3d_init);
+module_exit(gr3d_exit);
diff --git a/drivers/video/tegra/host/gr3d/gr3d.h b/drivers/video/tegra/host/gr3d/gr3d.h
new file mode 100644
index 000000000000..61f708cea95c
--- /dev/null
+++ b/drivers/video/tegra/host/gr3d/gr3d.h
@@ -0,0 +1,57 @@
+/*
+ * drivers/video/tegra/host/gr3d/gr3d.h
+ *
+ * Tegra Graphics Host 3D
+ *
+ * Copyright (c) 2011-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_GR3D_GR3D_H
+#define __NVHOST_GR3D_GR3D_H
+
+#include "host1x/host1x_hwctx.h"
+#include <linux/types.h>
+
+/* Registers of 3D unit */
+
+#define AR3D_PSEQ_QUAD_ID 0x545
+#define AR3D_DW_MEMORY_OUTPUT_ADDRESS 0x904
+#define AR3D_DW_MEMORY_OUTPUT_DATA 0x905
+#define AR3D_FDC_CONTROL_0 0xa00
+#define AR3D_FDC_CONTROL_0_RESET_VAL 0xe00
+#define AR3D_FDC_CONTROL_0_INVALIDATE 1
+#define AR3D_GSHIM_WRITE_MASK 0xb00
+#define AR3D_GSHIM_READ_SELECT 0xb01
+#define AR3D_GLOBAL_MEMORY_OUTPUT_READS 0xe40
+
+struct nvhost_hwctx;
+struct nvhost_channel;
+struct kref;
+
+/* Functions used commonly by all 3D context switch modules */
+void nvhost_3dctx_restore_begin(struct host1x_hwctx_handler *h, u32 *ptr);
+void nvhost_3dctx_restore_direct(u32 *ptr, u32 start_reg, u32 count);
+void nvhost_3dctx_restore_indirect(u32 *ptr, u32 offset_reg,
+		u32 offset,	u32 data_reg, u32 count);
+void nvhost_3dctx_restore_end(struct host1x_hwctx_handler *h, u32 *ptr);
+struct host1x_hwctx *nvhost_3dctx_alloc_common(
+		struct host1x_hwctx_handler *p,
+		struct nvhost_channel *ch, bool map_restore);
+void nvhost_3dctx_get(struct nvhost_hwctx *ctx);
+void nvhost_3dctx_free(struct kref *ref);
+void nvhost_3dctx_put(struct nvhost_hwctx *ctx);
+int nvhost_gr3d_prepare_power_off(struct nvhost_device *dev);
+
+#endif
diff --git a/drivers/video/tegra/host/gr3d/gr3d_t20.c b/drivers/video/tegra/host/gr3d/gr3d_t20.c
new file mode 100644
index 000000000000..694b00527790
--- /dev/null
+++ b/drivers/video/tegra/host/gr3d/gr3d_t20.c
@@ -0,0 +1,399 @@
+/*
+ * drivers/video/tegra/host/gr3d/gr3d_t20.c
+ *
+ * Tegra Graphics Host 3D for Tegra2
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "nvhost_hwctx.h"
+#include "nvhost_channel.h"
+#include "host1x/host1x.h"
+#include "host1x/host1x01_hardware.h"
+#include "gr3d.h"
+#include "chip_support.h"
+#include "nvhost_memmgr.h"
+
+#include <linux/slab.h>
+
+static const struct hwctx_reginfo ctxsave_regs_3d_global[] = {
+	HWCTX_REGINFO(0xe00,    4, DIRECT),
+	HWCTX_REGINFO(0xe05,   30, DIRECT),
+	HWCTX_REGINFO(0xe25,    2, DIRECT),
+	HWCTX_REGINFO(0xe28,    2, DIRECT),
+	HWCTX_REGINFO(0x001,    2, DIRECT),
+	HWCTX_REGINFO(0x00c,   10, DIRECT),
+	HWCTX_REGINFO(0x100,   34, DIRECT),
+	HWCTX_REGINFO(0x124,    2, DIRECT),
+	HWCTX_REGINFO(0x200,    5, DIRECT),
+	HWCTX_REGINFO(0x205, 1024, INDIRECT),
+	HWCTX_REGINFO(0x207, 1024, INDIRECT),
+	HWCTX_REGINFO(0x209,    1, DIRECT),
+	HWCTX_REGINFO(0x300,   64, DIRECT),
+	HWCTX_REGINFO(0x343,   25, DIRECT),
+	HWCTX_REGINFO(0x363,    2, DIRECT),
+	HWCTX_REGINFO(0x400,   16, DIRECT),
+	HWCTX_REGINFO(0x411,    1, DIRECT),
+	HWCTX_REGINFO(0x500,    4, DIRECT),
+	HWCTX_REGINFO(0x520,   32, DIRECT),
+	HWCTX_REGINFO(0x540,   64, INDIRECT),
+	HWCTX_REGINFO(0x600,   16, INDIRECT_4X),
+	HWCTX_REGINFO(0x603,  128, INDIRECT),
+	HWCTX_REGINFO(0x608,    4, DIRECT),
+	HWCTX_REGINFO(0x60e,    1, DIRECT),
+	HWCTX_REGINFO(0x700,   64, INDIRECT),
+	HWCTX_REGINFO(0x710,   50, DIRECT),
+	HWCTX_REGINFO(0x800,   16, INDIRECT_4X),
+	HWCTX_REGINFO(0x803,  512, INDIRECT),
+	HWCTX_REGINFO(0x805,   64, INDIRECT),
+	HWCTX_REGINFO(0x820,   32, DIRECT),
+	HWCTX_REGINFO(0x900,   64, INDIRECT),
+	HWCTX_REGINFO(0x902,    2, DIRECT),
+	HWCTX_REGINFO(0xa02,   10, DIRECT),
+	HWCTX_REGINFO(0xe04,    1, DIRECT),
+	HWCTX_REGINFO(0xe2a,    1, DIRECT),
+};
+
+/* the same context save command sequence is used for all contexts. */
+#define SAVE_BEGIN_V0_SIZE 5
+#define SAVE_DIRECT_V0_SIZE 3
+#define SAVE_INDIRECT_V0_SIZE 5
+#define SAVE_END_V0_SIZE 5
+#define SAVE_INCRS 3
+#define SAVE_THRESH_OFFSET 1
+#define RESTORE_BEGIN_SIZE 4
+#define RESTORE_DIRECT_SIZE 1
+#define RESTORE_INDIRECT_SIZE 2
+#define RESTORE_END_SIZE 1
+
+struct save_info {
+	u32 *ptr;
+	unsigned int save_count;
+	unsigned int restore_count;
+	unsigned int save_incrs;
+	unsigned int restore_incrs;
+};
+
+static u32 *setup_restore_regs_v0(u32 *ptr,
+			const struct hwctx_reginfo *regs,
+			unsigned int nr_regs)
+{
+	const struct hwctx_reginfo *rend = regs + nr_regs;
+
+	for ( ; regs != rend; ++regs) {
+		u32 offset = regs->offset;
+		u32 count = regs->count;
+		u32 indoff = offset + 1;
+		switch (regs->type) {
+		case HWCTX_REGINFO_DIRECT:
+			nvhost_3dctx_restore_direct(ptr, offset, count);
+			ptr += RESTORE_DIRECT_SIZE;
+			break;
+		case HWCTX_REGINFO_INDIRECT_4X:
+			++indoff;
+			/* fall through */
+		case HWCTX_REGINFO_INDIRECT:
+			nvhost_3dctx_restore_indirect(ptr,
+					offset, 0, indoff, count);
+			ptr += RESTORE_INDIRECT_SIZE;
+			break;
+		}
+		ptr += count;
+	}
+	return ptr;
+}
+
+static void setup_restore_v0(struct host1x_hwctx_handler *h, u32 *ptr)
+{
+	nvhost_3dctx_restore_begin(h, ptr);
+	ptr += RESTORE_BEGIN_SIZE;
+
+	ptr = setup_restore_regs_v0(ptr,
+			ctxsave_regs_3d_global,
+			ARRAY_SIZE(ctxsave_regs_3d_global));
+
+	nvhost_3dctx_restore_end(h, ptr);
+
+	wmb();
+}
+
+/*** v0 saver ***/
+
+static void save_push_v0(struct nvhost_hwctx *nctx, struct nvhost_cdma *cdma)
+{
+	struct host1x_hwctx *ctx = to_host1x_hwctx(nctx);
+	struct host1x_hwctx_handler *p = host1x_hwctx_handler(ctx);
+
+	nvhost_cdma_push_gather(cdma,
+			nvhost_get_host(nctx->channel->dev)->memmgr,
+			p->save_buf,
+			0,
+			nvhost_opcode_gather(p->save_size),
+			p->save_phys);
+}
+
+static void save_begin_v0(struct host1x_hwctx_handler *h, u32 *ptr)
+{
+	/* 3d: when done, increment syncpt to base+1 */
+	ptr[0] = nvhost_opcode_setclass(NV_GRAPHICS_3D_CLASS_ID, 0, 0);
+	ptr[1] = nvhost_opcode_imm_incr_syncpt(
+			host1x_uclass_incr_syncpt_cond_op_done_v(),
+			h->syncpt); /*  incr 1 */
+	/* host: wait for syncpt base+1 */
+	ptr[2] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					host1x_uclass_wait_syncpt_base_r(), 1);
+	ptr[3] = nvhost_class_host_wait_syncpt_base(h->syncpt,
+						h->waitbase, 1);
+	/* host: signal context read thread to start reading */
+	ptr[4] = nvhost_opcode_imm_incr_syncpt(
+			host1x_uclass_incr_syncpt_cond_immediate_v(),
+			h->syncpt); /* incr 2 */
+}
+
+static void save_direct_v0(u32 *ptr, u32 start_reg, u32 count)
+{
+	ptr[0] = nvhost_opcode_nonincr(host1x_uclass_indoff_r(), 1);
+	ptr[1] = nvhost_class_host_indoff_reg_read(NV_HOST_MODULE_GR3D,
+						start_reg, true);
+	ptr[2] = nvhost_opcode_nonincr(host1x_uclass_inddata_r(), count);
+}
+
+static void save_indirect_v0(u32 *ptr, u32 offset_reg, u32 offset,
+			u32 data_reg, u32 count)
+{
+	ptr[0] = nvhost_opcode_setclass(NV_GRAPHICS_3D_CLASS_ID,
+					offset_reg, 1);
+	ptr[1] = offset;
+	ptr[2] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					host1x_uclass_indoff_r(), 1);
+	ptr[3] = nvhost_class_host_indoff_reg_read(NV_HOST_MODULE_GR3D,
+						data_reg, false);
+	ptr[4] = nvhost_opcode_nonincr(host1x_uclass_inddata_r(), count);
+}
+
+static void save_end_v0(struct host1x_hwctx_handler *h, u32 *ptr)
+{
+	/* Wait for context read service to finish (cpu incr 3) */
+	ptr[0] = nvhost_opcode_nonincr(host1x_uclass_wait_syncpt_base_r(), 1);
+	ptr[1] = nvhost_class_host_wait_syncpt_base(h->syncpt,
+			h->waitbase, h->save_incrs);
+	/* Advance syncpoint base */
+	ptr[2] = nvhost_opcode_nonincr(host1x_uclass_incr_syncpt_base_r(), 1);
+	ptr[3] = nvhost_class_host_incr_syncpt_base(h->waitbase,
+			h->save_incrs);
+	/* set class back to the unit */
+	ptr[4] = nvhost_opcode_setclass(NV_GRAPHICS_3D_CLASS_ID, 0, 0);
+}
+
+static u32 *save_regs_v0(u32 *ptr, unsigned int *pending,
+			struct nvhost_channel *ch,
+			const struct hwctx_reginfo *regs,
+			unsigned int nr_regs)
+{
+	const struct hwctx_reginfo *rend = regs + nr_regs;
+	int drain_result = 0;
+
+	for ( ; regs != rend; ++regs) {
+		u32 count = regs->count;
+		switch (regs->type) {
+		case HWCTX_REGINFO_DIRECT:
+			ptr += RESTORE_DIRECT_SIZE;
+			break;
+		case HWCTX_REGINFO_INDIRECT:
+		case HWCTX_REGINFO_INDIRECT_4X:
+			ptr += RESTORE_INDIRECT_SIZE;
+			break;
+		}
+		drain_result = nvhost_channel_drain_read_fifo(ch,
+			ptr, count, pending);
+		BUG_ON(drain_result < 0);
+		ptr += count;
+	}
+	return ptr;
+}
+
+/*** save ***/
+
+static void setup_save_regs(struct save_info *info,
+			const struct hwctx_reginfo *regs,
+			unsigned int nr_regs)
+{
+	const struct hwctx_reginfo *rend = regs + nr_regs;
+	u32 *ptr = info->ptr;
+	unsigned int save_count = info->save_count;
+	unsigned int restore_count = info->restore_count;
+
+	for ( ; regs != rend; ++regs) {
+		u32 offset = regs->offset;
+		u32 count = regs->count;
+		u32 indoff = offset + 1;
+		switch (regs->type) {
+		case HWCTX_REGINFO_DIRECT:
+			if (ptr) {
+				save_direct_v0(ptr, offset, count);
+				ptr += SAVE_DIRECT_V0_SIZE;
+			}
+			save_count += SAVE_DIRECT_V0_SIZE;
+			restore_count += RESTORE_DIRECT_SIZE;
+			break;
+		case HWCTX_REGINFO_INDIRECT_4X:
+			++indoff;
+			/* fall through */
+		case HWCTX_REGINFO_INDIRECT:
+			if (ptr) {
+				save_indirect_v0(ptr, offset, 0,
+						indoff, count);
+				ptr += SAVE_INDIRECT_V0_SIZE;
+			}
+			save_count += SAVE_INDIRECT_V0_SIZE;
+			restore_count += RESTORE_INDIRECT_SIZE;
+			break;
+		}
+		if (ptr) {
+			/* SAVE cases only: reserve room for incoming data */
+			u32 k = 0;
+			/*
+			 * Create a signature pattern for indirect data (which
+			 * will be overwritten by true incoming data) for
+			 * better deducing where we are in a long command
+			 * sequence, when given only a FIFO snapshot for debug
+			 * purposes.
+			*/
+			for (k = 0; k < count; k++)
+				*(ptr + k) = 0xd000d000 | (offset << 16) | k;
+			ptr += count;
+		}
+		save_count += count;
+		restore_count += count;
+	}
+
+	info->ptr = ptr;
+	info->save_count = save_count;
+	info->restore_count = restore_count;
+}
+
+static void setup_save(struct host1x_hwctx_handler *h, u32 *ptr)
+{
+	struct save_info info = {
+		ptr,
+		SAVE_BEGIN_V0_SIZE,
+		RESTORE_BEGIN_SIZE,
+		SAVE_INCRS,
+		1
+	};
+
+	if (info.ptr) {
+		save_begin_v0(h, info.ptr);
+		info.ptr += SAVE_BEGIN_V0_SIZE;
+	}
+
+	/* save regs */
+	setup_save_regs(&info,
+			ctxsave_regs_3d_global,
+			ARRAY_SIZE(ctxsave_regs_3d_global));
+
+	if (info.ptr) {
+		save_end_v0(h, info.ptr);
+		info.ptr += SAVE_END_V0_SIZE;
+	}
+
+	wmb();
+
+	h->save_size = info.save_count + SAVE_END_V0_SIZE;
+	h->restore_size = info.restore_count + RESTORE_END_SIZE;
+	h->save_incrs = info.save_incrs;
+	h->save_thresh = h->save_incrs - SAVE_THRESH_OFFSET;
+	h->restore_incrs = info.restore_incrs;
+}
+
+
+
+/*** ctx3d ***/
+
+static struct nvhost_hwctx *ctx3d_alloc_v0(struct nvhost_hwctx_handler *h,
+		struct nvhost_channel *ch)
+{
+	struct host1x_hwctx_handler *p = to_host1x_hwctx_handler(h);
+	struct host1x_hwctx *ctx =
+		nvhost_3dctx_alloc_common(p, ch, true);
+	if (ctx) {
+		setup_restore_v0(p, ctx->restore_virt);
+		return &ctx->hwctx;
+	} else
+		return NULL;
+}
+
+static void ctx3d_save_service(struct nvhost_hwctx *nctx)
+{
+	struct host1x_hwctx *ctx = to_host1x_hwctx(nctx);
+
+	u32 *ptr = (u32 *)ctx->restore_virt + RESTORE_BEGIN_SIZE;
+	unsigned int pending = 0;
+
+	ptr = save_regs_v0(ptr, &pending, nctx->channel,
+			ctxsave_regs_3d_global,
+			ARRAY_SIZE(ctxsave_regs_3d_global));
+
+	wmb();
+	nvhost_syncpt_cpu_incr(&nvhost_get_host(nctx->channel->dev)->syncpt,
+			host1x_hwctx_handler(ctx)->syncpt);
+}
+
+struct nvhost_hwctx_handler *nvhost_gr3d_t20_ctxhandler_init(
+		u32 syncpt, u32 waitbase,
+		struct nvhost_channel *ch)
+{
+	struct mem_mgr *memmgr;
+	u32 *save_ptr;
+	struct host1x_hwctx_handler *p;
+
+	p = kmalloc(sizeof(*p), GFP_KERNEL);
+	if (!p)
+		return NULL;
+	memmgr = nvhost_get_host(ch->dev)->memmgr;
+
+	p->syncpt = syncpt;
+	p->waitbase = waitbase;
+
+	setup_save(p, NULL);
+
+	p->save_buf = mem_op().alloc(memmgr, p->save_size * sizeof(u32), 32,
+				mem_mgr_flag_write_combine);
+	if (IS_ERR_OR_NULL(p->save_buf)) {
+		p->save_buf = NULL;
+		return NULL;
+	}
+
+	p->save_slots = 1;
+
+	save_ptr = mem_op().mmap(p->save_buf);
+	if (!save_ptr) {
+		mem_op().put(memmgr, p->save_buf);
+		p->save_buf = NULL;
+		return NULL;
+	}
+
+	p->save_phys = mem_op().pin(memmgr, p->save_buf);
+
+	setup_save(p, save_ptr);
+
+	p->h.alloc = ctx3d_alloc_v0;
+	p->h.save_push = save_push_v0;
+	p->h.save_service = ctx3d_save_service;
+	p->h.get = nvhost_3dctx_get;
+	p->h.put = nvhost_3dctx_put;
+
+	return &p->h;
+}
diff --git a/drivers/video/tegra/host/gr3d/gr3d_t20.h b/drivers/video/tegra/host/gr3d/gr3d_t20.h
new file mode 100644
index 000000000000..e6fb8fdf8aba
--- /dev/null
+++ b/drivers/video/tegra/host/gr3d/gr3d_t20.h
@@ -0,0 +1,33 @@
+/*
+ * drivers/video/tegra/host/gr3d/gr3d_t20.h
+ *
+ * Tegra Graphics Host 3D for Tegra2
+ *
+ * Copyright (c) 2011-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_GR3D_GR3D_T20_H
+#define __NVHOST_GR3D_GR3D_T20_H
+
+#include <linux/types.h>
+
+struct nvhost_hwctx_handler;
+struct nvhost_channel;
+
+struct nvhost_hwctx_handler *nvhost_gr3d_t20_ctxhandler_init(
+		u32 syncpt, u32 waitbase,
+		struct nvhost_channel *ch);
+
+#endif
diff --git a/drivers/video/tegra/host/gr3d/gr3d_t30.c b/drivers/video/tegra/host/gr3d/gr3d_t30.c
new file mode 100644
index 000000000000..664708c7fc80
--- /dev/null
+++ b/drivers/video/tegra/host/gr3d/gr3d_t30.c
@@ -0,0 +1,439 @@
+/*
+ * drivers/video/tegra/host/gr3d/gr3d_t30.c
+ *
+ * Tegra Graphics Host 3D for Tegra3
+ *
+ * Copyright (c) 2011-2012 NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "nvhost_hwctx.h"
+#include "nvhost_channel.h"
+#include "nvhost_cdma.h"
+#include "dev.h"
+#include "host1x/host1x01_hardware.h"
+#include "gr3d.h"
+#include "chip_support.h"
+#include "nvhost_memmgr.h"
+
+#include <mach/gpufuse.h>
+#include <mach/hardware.h>
+#include <linux/slab.h>
+
+static const struct hwctx_reginfo ctxsave_regs_3d_global[] = {
+	HWCTX_REGINFO(0xe00,    4, DIRECT),
+	HWCTX_REGINFO(0xe05,   30, DIRECT),
+	HWCTX_REGINFO(0xe25,    2, DIRECT),
+	HWCTX_REGINFO(0xe28,    2, DIRECT),
+	HWCTX_REGINFO(0xe30,   16, DIRECT),
+	HWCTX_REGINFO(0x001,    2, DIRECT),
+	HWCTX_REGINFO(0x00c,   10, DIRECT),
+	HWCTX_REGINFO(0x100,   34, DIRECT),
+	HWCTX_REGINFO(0x124,    2, DIRECT),
+	HWCTX_REGINFO(0x200,    5, DIRECT),
+	HWCTX_REGINFO(0x205, 1024, INDIRECT),
+	HWCTX_REGINFO(0x207, 1024, INDIRECT),
+	HWCTX_REGINFO(0x209,    1, DIRECT),
+	HWCTX_REGINFO(0x300,   64, DIRECT),
+	HWCTX_REGINFO(0x343,   25, DIRECT),
+	HWCTX_REGINFO(0x363,    2, DIRECT),
+	HWCTX_REGINFO(0x400,   16, DIRECT),
+	HWCTX_REGINFO(0x411,    1, DIRECT),
+	HWCTX_REGINFO(0x412,    1, DIRECT),
+	HWCTX_REGINFO(0x500,    4, DIRECT),
+	HWCTX_REGINFO(0x520,   32, DIRECT),
+	HWCTX_REGINFO(0x540,   64, INDIRECT),
+	HWCTX_REGINFO(0x600,   16, INDIRECT_4X),
+	HWCTX_REGINFO(0x603,  128, INDIRECT),
+	HWCTX_REGINFO(0x608,    4, DIRECT),
+	HWCTX_REGINFO(0x60e,    1, DIRECT),
+	HWCTX_REGINFO(0x700,   64, INDIRECT),
+	HWCTX_REGINFO(0x710,   50, DIRECT),
+	HWCTX_REGINFO(0x750,   16, DIRECT),
+	HWCTX_REGINFO(0x800,   16, INDIRECT_4X),
+	HWCTX_REGINFO(0x803,  512, INDIRECT),
+	HWCTX_REGINFO(0x805,   64, INDIRECT),
+	HWCTX_REGINFO(0x820,   32, DIRECT),
+	HWCTX_REGINFO(0x900,   64, INDIRECT),
+	HWCTX_REGINFO(0x902,    2, DIRECT),
+	HWCTX_REGINFO(0x90a,    1, DIRECT),
+	HWCTX_REGINFO(0xa02,   10, DIRECT),
+	HWCTX_REGINFO(0xb04,    1, DIRECT),
+	HWCTX_REGINFO(0xb06,   13, DIRECT),
+};
+
+static const struct hwctx_reginfo ctxsave_regs_3d_perset[] = {
+	HWCTX_REGINFO(0xe04,    1, DIRECT),
+	HWCTX_REGINFO(0xe2a,    1, DIRECT),
+	HWCTX_REGINFO(0x413,    1, DIRECT),
+	HWCTX_REGINFO(0x90b,    1, DIRECT),
+	HWCTX_REGINFO(0xe41,    1, DIRECT),
+};
+
+static unsigned int restore_set1_offset;
+
+#define SAVE_BEGIN_V1_SIZE (1 + RESTORE_BEGIN_SIZE)
+#define SAVE_DIRECT_V1_SIZE (4 + RESTORE_DIRECT_SIZE)
+#define SAVE_INDIRECT_V1_SIZE (6 + RESTORE_INDIRECT_SIZE)
+#define SAVE_END_V1_SIZE (9 + RESTORE_END_SIZE)
+#define SAVE_INCRS 3
+#define SAVE_THRESH_OFFSET 0
+#define RESTORE_BEGIN_SIZE 4
+#define RESTORE_DIRECT_SIZE 1
+#define RESTORE_INDIRECT_SIZE 2
+#define RESTORE_END_SIZE 1
+
+struct save_info {
+	u32 *ptr;
+	unsigned int save_count;
+	unsigned int restore_count;
+	unsigned int save_incrs;
+	unsigned int restore_incrs;
+};
+
+/*** v1 saver ***/
+
+static void save_push_v1(struct nvhost_hwctx *nctx, struct nvhost_cdma *cdma)
+{
+	struct host1x_hwctx *ctx = to_host1x_hwctx(nctx);
+	struct host1x_hwctx_handler *p = host1x_hwctx_handler(ctx);
+
+	/* wait for 3d idle */
+	nvhost_cdma_push(cdma,
+			nvhost_opcode_setclass(NV_GRAPHICS_3D_CLASS_ID, 0, 0),
+			nvhost_opcode_imm_incr_syncpt(
+				host1x_uclass_incr_syncpt_cond_op_done_v(),
+				p->syncpt));
+	nvhost_cdma_push(cdma,
+			nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					host1x_uclass_wait_syncpt_base_r(), 1),
+			nvhost_class_host_wait_syncpt_base(p->syncpt,
+							p->waitbase, 1));
+	/* back to 3d */
+	nvhost_cdma_push(cdma,
+			nvhost_opcode_setclass(NV_GRAPHICS_3D_CLASS_ID, 0, 0),
+			NVHOST_OPCODE_NOOP);
+
+	/* invalidate the FDC to prevent cache-coherency issues across GPUs
+	   note that we assume FDC_CONTROL_0 is left in the reset state by all
+	   contexts.  the invalidate bit will clear itself, so the register
+	   should be unchanged after this */
+	nvhost_cdma_push(cdma,
+		nvhost_opcode_imm(AR3D_FDC_CONTROL_0,
+			AR3D_FDC_CONTROL_0_RESET_VAL
+				| AR3D_FDC_CONTROL_0_INVALIDATE),
+		NVHOST_OPCODE_NOOP);
+
+	/* set register set 0 and 1 register read memory output addresses,
+	   and send their reads to memory */
+
+	nvhost_cdma_push(cdma,
+		nvhost_opcode_imm(AR3D_GSHIM_WRITE_MASK, 2),
+		nvhost_opcode_imm(AR3D_GLOBAL_MEMORY_OUTPUT_READS, 1));
+	nvhost_cdma_push(cdma,
+		nvhost_opcode_nonincr(AR3D_DW_MEMORY_OUTPUT_ADDRESS, 1),
+		ctx->restore_phys + restore_set1_offset * 4);
+
+	nvhost_cdma_push(cdma,
+		nvhost_opcode_imm(AR3D_GSHIM_WRITE_MASK, 1),
+		nvhost_opcode_imm(AR3D_GLOBAL_MEMORY_OUTPUT_READS, 1));
+	nvhost_cdma_push(cdma,
+		nvhost_opcode_nonincr(AR3D_DW_MEMORY_OUTPUT_ADDRESS, 1),
+		ctx->restore_phys);
+	/* gather the save buffer */
+	nvhost_cdma_push_gather(cdma,
+			nvhost_get_host(nctx->channel->dev)->memmgr,
+			p->save_buf,
+			0,
+			nvhost_opcode_gather(p->save_size),
+			p->save_phys);
+}
+
+static void save_begin_v1(struct host1x_hwctx_handler *p, u32 *ptr)
+{
+	ptr[0] = nvhost_opcode_nonincr(AR3D_DW_MEMORY_OUTPUT_DATA,
+			RESTORE_BEGIN_SIZE);
+	nvhost_3dctx_restore_begin(p, ptr + 1);
+	ptr += RESTORE_BEGIN_SIZE;
+}
+
+static void save_direct_v1(u32 *ptr, u32 start_reg, u32 count)
+{
+	ptr[0] = nvhost_opcode_setclass(NV_GRAPHICS_3D_CLASS_ID,
+			AR3D_DW_MEMORY_OUTPUT_DATA, 1);
+	nvhost_3dctx_restore_direct(ptr + 1, start_reg, count);
+	ptr += RESTORE_DIRECT_SIZE;
+	ptr[1] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					host1x_uclass_indoff_r(), 1);
+	ptr[2] = nvhost_class_host_indoff_reg_read(NV_HOST_MODULE_GR3D,
+						start_reg, true);
+	/* TODO could do this in the setclass if count < 6 */
+	ptr[3] = nvhost_opcode_nonincr(host1x_uclass_inddata_r(), count);
+}
+
+static void save_indirect_v1(u32 *ptr, u32 offset_reg, u32 offset,
+			u32 data_reg, u32 count)
+{
+	ptr[0] = nvhost_opcode_setclass(NV_GRAPHICS_3D_CLASS_ID, 0, 0);
+	ptr[1] = nvhost_opcode_nonincr(AR3D_DW_MEMORY_OUTPUT_DATA,
+			RESTORE_INDIRECT_SIZE);
+	nvhost_3dctx_restore_indirect(ptr + 2, offset_reg, offset, data_reg,
+			count);
+	ptr += RESTORE_INDIRECT_SIZE;
+	ptr[2] = nvhost_opcode_imm(offset_reg, offset);
+	ptr[3] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					host1x_uclass_indoff_r(), 1);
+	ptr[4] = nvhost_class_host_indoff_reg_read(NV_HOST_MODULE_GR3D,
+						data_reg, false);
+	ptr[5] = nvhost_opcode_nonincr(host1x_uclass_inddata_r(), count);
+}
+
+static void save_end_v1(struct host1x_hwctx_handler *p, u32 *ptr)
+{
+	/* write end of restore buffer */
+	ptr[0] = nvhost_opcode_setclass(NV_GRAPHICS_3D_CLASS_ID,
+			AR3D_DW_MEMORY_OUTPUT_DATA, 1);
+	nvhost_3dctx_restore_end(p, ptr + 1);
+	ptr += RESTORE_END_SIZE;
+	/* reset to dual reg if necessary */
+	ptr[1] = nvhost_opcode_imm(AR3D_GSHIM_WRITE_MASK,
+			(1 << 2) - 1);
+	/* op_done syncpt incr to flush FDC */
+	ptr[2] = nvhost_opcode_imm_incr_syncpt(
+			host1x_uclass_incr_syncpt_cond_op_done_v(), p->syncpt);
+	/* host wait for that syncpt incr, and advance the wait base */
+	ptr[3] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+			host1x_uclass_wait_syncpt_base_r(),
+			nvhost_mask2(
+				host1x_uclass_wait_syncpt_base_r(),
+				host1x_uclass_incr_syncpt_base_r()));
+	ptr[4] = nvhost_class_host_wait_syncpt_base(p->syncpt,
+				p->waitbase, p->save_incrs - 1);
+	ptr[5] = nvhost_class_host_incr_syncpt_base(p->waitbase,
+			p->save_incrs);
+	/* set class back to 3d */
+	ptr[6] = nvhost_opcode_setclass(NV_GRAPHICS_3D_CLASS_ID, 0, 0);
+	/* send reg reads back to host */
+	ptr[7] = nvhost_opcode_imm(AR3D_GLOBAL_MEMORY_OUTPUT_READS, 0);
+	/* final syncpt increment to release waiters */
+	ptr[8] = nvhost_opcode_imm(0, p->syncpt);
+}
+
+/*** save ***/
+
+
+
+static void setup_save_regs(struct save_info *info,
+			const struct hwctx_reginfo *regs,
+			unsigned int nr_regs)
+{
+	const struct hwctx_reginfo *rend = regs + nr_regs;
+	u32 *ptr = info->ptr;
+	unsigned int save_count = info->save_count;
+	unsigned int restore_count = info->restore_count;
+
+	for ( ; regs != rend; ++regs) {
+		u32 offset = regs->offset;
+		u32 count = regs->count;
+		u32 indoff = offset + 1;
+		switch (regs->type) {
+		case HWCTX_REGINFO_DIRECT:
+			if (ptr) {
+				save_direct_v1(ptr, offset, count);
+				ptr += SAVE_DIRECT_V1_SIZE;
+			}
+			save_count += SAVE_DIRECT_V1_SIZE;
+			restore_count += RESTORE_DIRECT_SIZE;
+			break;
+		case HWCTX_REGINFO_INDIRECT_4X:
+			++indoff;
+			/* fall through */
+		case HWCTX_REGINFO_INDIRECT:
+			if (ptr) {
+				save_indirect_v1(ptr, offset, 0,
+						indoff, count);
+				ptr += SAVE_INDIRECT_V1_SIZE;
+			}
+			save_count += SAVE_INDIRECT_V1_SIZE;
+			restore_count += RESTORE_INDIRECT_SIZE;
+			break;
+		}
+		if (ptr) {
+			/* SAVE cases only: reserve room for incoming data */
+			u32 k = 0;
+			/*
+			 * Create a signature pattern for indirect data (which
+			 * will be overwritten by true incoming data) for
+			 * better deducing where we are in a long command
+			 * sequence, when given only a FIFO snapshot for debug
+			 * purposes.
+			*/
+			for (k = 0; k < count; k++)
+				*(ptr + k) = 0xd000d000 | (offset << 16) | k;
+			ptr += count;
+		}
+		save_count += count;
+		restore_count += count;
+	}
+
+	info->ptr = ptr;
+	info->save_count = save_count;
+	info->restore_count = restore_count;
+}
+
+static void switch_gpu(struct save_info *info,
+			unsigned int save_src_set,
+			u32 save_dest_sets,
+			u32 restore_dest_sets)
+{
+	if (info->ptr) {
+		info->ptr[0] = nvhost_opcode_setclass(
+				NV_GRAPHICS_3D_CLASS_ID,
+				AR3D_DW_MEMORY_OUTPUT_DATA, 1);
+		info->ptr[1] = nvhost_opcode_imm(AR3D_GSHIM_WRITE_MASK,
+				restore_dest_sets);
+		info->ptr[2] = nvhost_opcode_imm(AR3D_GSHIM_WRITE_MASK,
+				save_dest_sets);
+		info->ptr[3] = nvhost_opcode_imm(AR3D_GSHIM_READ_SELECT,
+				save_src_set);
+		info->ptr += 4;
+	}
+	info->save_count += 4;
+	info->restore_count += 1;
+}
+
+static void setup_save(struct host1x_hwctx_handler *p, u32 *ptr)
+{
+	struct save_info info = {
+		ptr,
+		SAVE_BEGIN_V1_SIZE,
+		RESTORE_BEGIN_SIZE,
+		SAVE_INCRS,
+		1
+	};
+	int save_end_size = SAVE_END_V1_SIZE;
+
+	if (info.ptr) {
+		save_begin_v1(p, info.ptr);
+		info.ptr += SAVE_BEGIN_V1_SIZE;
+	}
+
+	/* read from set0, write cmds through set0, restore to set0 and 1 */
+	switch_gpu(&info, 0, 1, 3);
+
+	/* save regs that are common to both sets */
+	setup_save_regs(&info,
+			ctxsave_regs_3d_global,
+			ARRAY_SIZE(ctxsave_regs_3d_global));
+
+	/* read from set 0, write cmds through set0, restore to set0 */
+	switch_gpu(&info, 0, 1, 1);
+
+	/* save set 0 specific regs */
+	setup_save_regs(&info,
+			ctxsave_regs_3d_perset,
+			ARRAY_SIZE(ctxsave_regs_3d_perset));
+
+
+	/* read from set1, write cmds through set1, restore to set1 */
+	switch_gpu(&info, 1, 2, 2);
+	/* note offset at which set 1 restore starts */
+	restore_set1_offset = info.restore_count;
+	/* save set 1 specific regs */
+	setup_save_regs(&info,
+			ctxsave_regs_3d_perset,
+			ARRAY_SIZE(ctxsave_regs_3d_perset));
+
+
+	/* read from set0, write cmds through set1, restore to set0 and 1 */
+	switch_gpu(&info, 0, 2, 3);
+
+	if (info.ptr) {
+		save_end_v1(p, info.ptr);
+		info.ptr += SAVE_END_V1_SIZE;
+	}
+
+	wmb();
+
+	p->save_size = info.save_count + save_end_size;
+	p->restore_size = info.restore_count + RESTORE_END_SIZE;
+	p->save_incrs = info.save_incrs;
+	p->save_thresh = p->save_incrs - SAVE_THRESH_OFFSET;
+	p->restore_incrs = info.restore_incrs;
+}
+
+
+/*** ctx3d ***/
+
+static struct nvhost_hwctx *ctx3d_alloc_v1(struct nvhost_hwctx_handler *h,
+		struct nvhost_channel *ch)
+{
+	struct host1x_hwctx_handler *p = to_host1x_hwctx_handler(h);
+	struct host1x_hwctx *ctx = nvhost_3dctx_alloc_common(p, ch, false);
+
+	if (ctx)
+		return &ctx->hwctx;
+	else
+		return NULL;
+}
+
+struct nvhost_hwctx_handler *nvhost_gr3d_t30_ctxhandler_init(
+		u32 syncpt, u32 waitbase,
+		struct nvhost_channel *ch)
+{
+	struct mem_mgr *memmgr;
+	u32 *save_ptr;
+	struct host1x_hwctx_handler *p;
+
+	p = kmalloc(sizeof(*p), GFP_KERNEL);
+	if (!p)
+		return NULL;
+
+	memmgr = nvhost_get_host(ch->dev)->memmgr;
+
+	p->syncpt = syncpt;
+	p->waitbase = waitbase;
+
+	setup_save(p, NULL);
+
+	p->save_buf = mem_op().alloc(memmgr, p->save_size * 4, 32,
+				mem_mgr_flag_write_combine);
+	if (IS_ERR_OR_NULL(p->save_buf)) {
+		p->save_buf = NULL;
+		return NULL;
+	}
+
+	p->save_slots = 8;
+
+	save_ptr = mem_op().mmap(p->save_buf);
+	if (!save_ptr) {
+		mem_op().put(memmgr, p->save_buf);
+		p->save_buf = NULL;
+		return NULL;
+	}
+
+	p->save_phys = mem_op().pin(memmgr, p->save_buf);
+
+	setup_save(p, save_ptr);
+
+	mem_op().munmap(p->save_buf, save_ptr);
+
+	p->h.alloc = ctx3d_alloc_v1;
+	p->h.save_push = save_push_v1;
+	p->h.save_service = NULL;
+	p->h.get = nvhost_3dctx_get;
+	p->h.put = nvhost_3dctx_put;
+
+	return &p->h;
+}
diff --git a/drivers/video/tegra/host/gr3d/gr3d_t30.h b/drivers/video/tegra/host/gr3d/gr3d_t30.h
new file mode 100644
index 000000000000..94d5dc0f353b
--- /dev/null
+++ b/drivers/video/tegra/host/gr3d/gr3d_t30.h
@@ -0,0 +1,33 @@
+/*
+ * drivers/video/tegra/host/gr3d/gr3d_t30.h
+ *
+ * Tegra Graphics Host 3D for Tegra3
+ *
+ * Copyright (c) 2011-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_GR3D_GR3D_T30_H
+#define __NVHOST_GR3D_GR3D_T30_H
+
+#include <linux/types.h>
+
+struct nvhost_hwctx_handler;
+struct nvhost_channel;
+
+struct nvhost_hwctx_handler *nvhost_gr3d_t30_ctxhandler_init(
+		u32 syncpt, u32 waitbase,
+		struct nvhost_channel *ch);
+
+#endif
diff --git a/drivers/video/tegra/host/gr3d/scale3d.c b/drivers/video/tegra/host/gr3d/scale3d.c
new file mode 100644
index 000000000000..49147975a9e4
--- /dev/null
+++ b/drivers/video/tegra/host/gr3d/scale3d.c
@@ -0,0 +1,941 @@
+/*
+ * drivers/video/tegra/host/gr3d/scale3d.c
+ *
+ * Tegra Graphics Host 3D clock scaling
+ *
+ * Copyright (c) 2010-2012, NVIDIA 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ * 3d clock scaling
+ *
+ * module3d_notify_busy() is called upon submit, module3d_notify_idle() is
+ * called when all outstanding submits are completed. Idle times are measured
+ * over a fixed time period (scale3d.p_estimation_window). If the 3d module
+ * idle time percentage goes over the limit (set in scale3d.p_idle_max), 3d
+ * clocks are scaled down. If the percentage goes under the minimum limit (set
+ * in scale3d.p_idle_min), 3d clocks are scaled up. An additional test is made
+ * for clocking up quickly in response to load peaks.
+ *
+ * 3d.emc clock is scaled proportionately to 3d clock, with a quadratic-
+ * bezier-like factor added to pull 3d.emc rate a bit lower.
+ */
+
+#include <linux/debugfs.h>
+#include <linux/types.h>
+#include <linux/clk.h>
+#include <linux/slab.h>
+#include <mach/clk.h>
+#include <mach/hardware.h>
+#include "scale3d.h"
+#include "dev.h"
+#include <media/tegra_camera.h>
+
+#define GR3D_PRINT_STATS   BIT(1)
+#define GR3D_PRINT_BUSY    BIT(2)
+#define GR3D_PRINT_IDLE    BIT(3)
+#define GR3D_PRINT_HINT    BIT(4)
+#define GR3D_PRINT_TARGET  BIT(5)
+
+/* time frame for load and hint tracking - when events come in at a larger
+ * interval, this probably indicates the current estimates are stale
+ */
+#define GR3D_TIMEFRAME 1000000 /* 1 sec */
+
+/* the number of frames to use in the running average of load estimates and
+ * throughput hints. Choosing 6 frames targets a window of about 100 msec.
+ * Large flucutuations in frame times require a window that's large enough to
+ * prevent spiky scaling behavior, which in turn exacerbates frame rate
+ * instability.
+ */
+#define GR3D_FRAME_SPAN 6
+
+static int scale3d_is_enabled(void);
+static void scale3d_enable(int enable);
+
+#define POW2(x) ((x) * (x))
+
+/*
+ * 3D clock scaling should be treated differently when camera is on in AP37.
+ * 3D in AP37 requires 1.3V and combining it with MPE reaches to EDP limit.
+ * 3D clock really needs to be set to lower frequency which requires 1.0V.
+ * The same thing applies to 3D EMC clock.
+ */
+#define CAMERA_3D_CLK 300000000
+#define CAMERA_3D_EMC_CLK 437000000
+
+/*
+ * debugfs parameters to control 3d clock scaling test
+ *
+ * estimation_window  - time period for clock rate evaluation
+ * idle_min           - if less than [idle_min / 10] percent idle over
+ *                      [estimation_window] microseconds, clock up.
+ * idle_max      - if over [idle_max] percent idle over [estimation_window]
+ *                 microseconds, clock down.
+ * max_scale     - limits rate changes to no less than (100 - max_scale)% or
+ *                 (100 + 2 * max_scale)% of current clock rate
+ * verbosity     - bit flag to control debug printouts:
+ *                 1 - stats
+ *                 2 - busy
+ *                 3 - idle
+ *                 4 - hints
+ *                 5 - target frequencies
+ */
+
+struct scale3d_info_rec {
+	struct mutex lock; /* lock for timestamps etc */
+	int enable;
+	int init;
+	ktime_t last_scale;
+	int is_idle;
+	ktime_t last_adjust;
+	int fast_up_count;
+	int slow_down_count;
+	int is_scaled;
+	long emc_slope;
+	long emc_offset;
+	long emc_dip_slope;
+	long emc_dip_offset;
+	long emc_xmid;
+	unsigned long max_rate_3d;
+	unsigned long min_rate_3d;
+	ktime_t last_throughput_hint;
+
+	struct work_struct work;
+	struct delayed_work idle_timer;
+
+	ktime_t last_estimation_window;
+	long last_total_idle;
+	long total_idle;
+	ktime_t estimation_window;
+	ktime_t last_notification;
+	long idle_estimate;
+
+	unsigned int scale;
+	unsigned int p_busy_cutoff;
+	unsigned int p_estimation_window;
+	unsigned int p_use_throughput_hint;
+	unsigned int p_throughput_lo_limit;
+	unsigned int p_throughput_lower_limit;
+	unsigned int p_throughput_hi_limit;
+	unsigned int p_scale_step;
+	unsigned int p_idle_min;
+	unsigned int idle_min;
+	unsigned int p_idle_max;
+	unsigned int idle_max;
+	unsigned int p_adjust;
+	unsigned int p_scale_emc;
+	unsigned int p_emc_dip;
+	unsigned int p_verbosity;
+	struct clk *clk_3d;
+	struct clk *clk_3d2;
+	struct clk *clk_3d_emc;
+	int *freqlist;
+	int freq_count;
+};
+
+static struct scale3d_info_rec scale3d;
+
+static void scale_to_freq(unsigned long hz)
+{
+	unsigned long curr;
+
+	if (!tegra_is_clk_enabled(scale3d.clk_3d))
+		return;
+
+	if (tegra_get_chipid() == TEGRA_CHIPID_TEGRA3)
+		if (!tegra_is_clk_enabled(scale3d.clk_3d2))
+			return;
+
+	curr = clk_get_rate(scale3d.clk_3d);
+	if (hz == curr)
+		return;
+
+	if (!(hz >= scale3d.max_rate_3d && curr == scale3d.max_rate_3d)) {
+		if (tegra_get_chipid() == TEGRA_CHIPID_TEGRA3)
+			clk_set_rate(scale3d.clk_3d2, 0);
+		clk_set_rate(scale3d.clk_3d, hz);
+
+		if (scale3d.p_scale_emc) {
+			long after = (long) clk_get_rate(scale3d.clk_3d);
+			hz = after * scale3d.emc_slope + scale3d.emc_offset;
+			if (scale3d.p_emc_dip)
+				hz -=
+					(scale3d.emc_dip_slope *
+					POW2(after / 1000 - scale3d.emc_xmid) +
+					scale3d.emc_dip_offset);
+			clk_set_rate(scale3d.clk_3d_emc, hz);
+		}
+	}
+}
+
+static void scale3d_clocks(unsigned long percent)
+{
+	unsigned long hz, curr;
+
+	curr = clk_get_rate(scale3d.clk_3d);
+	hz = percent * (curr / 100);
+
+	scale_to_freq(hz);
+}
+
+static void scale3d_clocks_handler(struct work_struct *work)
+{
+	unsigned int scale;
+
+	mutex_lock(&scale3d.lock);
+	scale = scale3d.scale;
+	mutex_unlock(&scale3d.lock);
+
+	if (scale != 0)
+		scale3d_clocks(scale);
+}
+
+void nvhost_scale3d_suspend(struct nvhost_device *dev)
+{
+	if (!scale3d.enable)
+		return;
+
+	cancel_work_sync(&scale3d.work);
+	cancel_delayed_work(&scale3d.idle_timer);
+}
+
+/* set 3d clocks to max */
+static void reset_3d_clocks(void)
+{
+	if (clk_get_rate(scale3d.clk_3d) != scale3d.max_rate_3d) {
+		if (is_tegra_camera_on())
+			clk_set_rate(scale3d.clk_3d, CAMERA_3D_CLK);
+		else
+			clk_set_rate(scale3d.clk_3d, scale3d.max_rate_3d);
+		if (tegra_get_chipid() == TEGRA_CHIPID_TEGRA3) {
+			if (is_tegra_camera_on())
+				clk_set_rate(scale3d.clk_3d2, CAMERA_3D_CLK);
+			else
+				clk_set_rate(scale3d.clk_3d2,
+							scale3d.max_rate_3d);
+		}
+		if (scale3d.p_scale_emc) {
+			if (is_tegra_camera_on())
+				clk_set_rate(scale3d.clk_3d_emc,
+					CAMERA_3D_EMC_CLK);
+			else
+				clk_set_rate(scale3d.clk_3d_emc,
+					clk_round_rate(scale3d.clk_3d_emc,
+								UINT_MAX));
+		}
+	}
+}
+
+static int scale3d_is_enabled(void)
+{
+	int enable;
+
+	if (!scale3d.enable)
+		return 0;
+
+	mutex_lock(&scale3d.lock);
+	enable = scale3d.enable;
+	mutex_unlock(&scale3d.lock);
+
+	return enable;
+}
+
+static void scale3d_enable(int enable)
+{
+	int disable = 0;
+
+	mutex_lock(&scale3d.lock);
+
+	if (enable) {
+		if (scale3d.max_rate_3d != scale3d.min_rate_3d)
+			scale3d.enable = 1;
+	} else {
+		scale3d.enable = 0;
+		disable = 1;
+	}
+
+	mutex_unlock(&scale3d.lock);
+
+	if (disable)
+		reset_3d_clocks();
+}
+
+/* scaling_adjust - use scale up / scale down hint counts to adjust scaling
+ * parameters.
+ *
+ * hint_ratio is 100 x the ratio of scale up to scale down hints. Three cases
+ * are distinguished:
+ *
+ * hint_ratio < HINT_RATIO_MIN - set parameters to maximize scaling effect
+ * hint_ratio > HINT_RATIO_MAX - set parameters to minimize scaling effect
+ * hint_ratio between limits - scale parameters linearly
+ *
+ * the parameters adjusted are
+ *
+ * * idle_min percentage
+ * * idle_max percentage
+ */
+#define SCALING_ADJUST_PERIOD 1000000
+#define HINT_RATIO_MAX 400
+#define HINT_RATIO_MIN 100
+#define HINT_RATIO_MID ((HINT_RATIO_MAX + HINT_RATIO_MIN) / 2)
+#define HINT_RATIO_DIFF (HINT_RATIO_MAX - HINT_RATIO_MIN)
+
+static void scaling_adjust(ktime_t time)
+{
+	long hint_ratio;
+	int idle_min_adjustment;
+	int idle_max_adjustment;
+	unsigned long dt;
+
+	dt = (unsigned long) ktime_us_delta(time, scale3d.last_adjust);
+	if (dt < SCALING_ADJUST_PERIOD)
+		return;
+
+	hint_ratio = (100 * (scale3d.fast_up_count + 1)) /
+				 (scale3d.slow_down_count + 1);
+
+	if (hint_ratio > HINT_RATIO_MAX) {
+		idle_min_adjustment = scale3d.p_idle_min;
+		idle_max_adjustment = scale3d.p_idle_max;
+	} else if (hint_ratio < HINT_RATIO_MIN) {
+		idle_min_adjustment = -((int) scale3d.p_idle_min) / 2;
+		idle_max_adjustment = -((int) scale3d.p_idle_max) / 2;
+	} else {
+		int diff;
+		int factor;
+
+		diff = HINT_RATIO_MID - hint_ratio;
+		if (diff < 0)
+			factor = -diff * 2;
+		else {
+			factor = -diff;
+			diff *= 2;
+		}
+
+		idle_min_adjustment =
+			(factor * (int) scale3d.p_idle_min) / HINT_RATIO_DIFF;
+		idle_max_adjustment =
+			(factor * (int) scale3d.p_idle_max) / HINT_RATIO_DIFF;
+	}
+
+	scale3d.idle_min = scale3d.p_idle_min + idle_min_adjustment;
+	scale3d.idle_max = scale3d.p_idle_max + idle_max_adjustment;
+
+	if (scale3d.p_verbosity & GR3D_PRINT_STATS)
+		pr_info("scale3d stats: + %d - %d min %u max %u\n",
+			scale3d.fast_up_count, scale3d.slow_down_count,
+			scale3d.idle_min, scale3d.idle_max);
+
+	scale3d.fast_up_count = 0;
+	scale3d.slow_down_count = 0;
+	scale3d.last_adjust = time;
+}
+
+#undef SCALING_ADJUST_PERIOD
+#undef HINT_RATIO_MAX
+#undef HINT_RATIO_MIN
+#undef HINT_RATIO_MID
+#undef HINT_RATIO_DIFF
+
+static void scaling_state_check(ktime_t time)
+{
+	unsigned long dt;
+
+	/* adjustment: set scale parameters (idle_min, idle_max) +/- 25%
+	 * based on ratio of scale up to scale down hints
+	 */
+	if (scale3d.p_adjust)
+		scaling_adjust(time);
+	else {
+		scale3d.idle_min = scale3d.p_idle_min;
+		scale3d.idle_max = scale3d.p_idle_max;
+	}
+
+	dt = (unsigned long) ktime_us_delta(time, scale3d.last_scale);
+	if (dt < scale3d.p_estimation_window)
+		return;
+
+	scale3d.last_scale = time;
+
+	/* if too busy, scale up */
+	if (scale3d.idle_estimate < scale3d.idle_min) {
+		scale3d.is_scaled = 0;
+		scale3d.fast_up_count++;
+		if (scale3d.p_verbosity & GR3D_PRINT_BUSY)
+			pr_info("scale3d: %ld/1000 busy\n",
+				1000 - scale3d.idle_estimate);
+
+		reset_3d_clocks();
+		return;
+	}
+
+	if (scale3d.p_verbosity & GR3D_PRINT_IDLE)
+		pr_info("scale3d: idle %lu/1000\n",
+			scale3d.idle_estimate);
+
+	if (scale3d.idle_estimate > scale3d.idle_max) {
+		if (!scale3d.is_scaled)
+			scale3d.is_scaled = 1;
+
+		scale3d.slow_down_count++;
+		/* if idle time is high, clock down */
+		scale3d.scale =
+			100 - (scale3d.idle_estimate - scale3d.idle_min) / 10;
+		schedule_work(&scale3d.work);
+	}
+}
+
+/* the idle estimate is done by keeping 2 time stamps, initially set to the
+ * same time. Once the estimation_window time has been exceeded, one time
+ * stamp is moved up to the current time. The idle estimate is calculated
+ * based on the idle time percentage from the earlier estimate. The next time
+ * an estimation_window time is exceeded, the previous idle time and estimates
+ * are moved up - this is intended to prevent abrupt changes to the idle
+ * estimate.
+ */
+static void update_load_estimate(int idle)
+{
+	unsigned long window;
+	unsigned long t;
+
+	ktime_t now = ktime_get();
+	t = ktime_us_delta(now, scale3d.last_notification);
+
+	/* if the last event was over GR3D_TIMEFRAME usec ago (1 sec), the
+	 * current load tracking data is probably stale
+	 */
+	if (t > GR3D_TIMEFRAME) {
+		scale3d.is_idle = idle;
+		scale3d.last_notification = now;
+		scale3d.estimation_window = now;
+		scale3d.last_estimation_window = now;
+		scale3d.total_idle = 0;
+		scale3d.last_total_idle = 0;
+		scale3d.idle_estimate = idle ? 1000 : 0;
+		return;
+	}
+
+	if (scale3d.is_idle) {
+		scale3d.total_idle += t;
+		scale3d.last_total_idle += t;
+	}
+
+	scale3d.is_idle = idle;
+	scale3d.last_notification = now;
+
+	window = ktime_us_delta(now, scale3d.last_estimation_window);
+	/* prevent division by 0 if events come in less than 1 usec apart */
+	if (window > 0)
+		scale3d.idle_estimate =
+			(1000 * scale3d.last_total_idle) / window;
+
+	/* move up to the last estimation window */
+	if (ktime_us_delta(now, scale3d.estimation_window) >
+		scale3d.p_estimation_window) {
+		scale3d.last_estimation_window = scale3d.estimation_window;
+		scale3d.last_total_idle = scale3d.total_idle;
+		scale3d.total_idle = 0;
+		scale3d.estimation_window = now;
+	}
+}
+
+void nvhost_scale3d_notify_idle(struct nvhost_device *dev)
+{
+	ktime_t t;
+	unsigned long dt;
+	int delay;
+
+	if (!scale3d.enable)
+		return;
+
+	update_load_estimate(1);
+
+	t = ktime_get();
+
+	/* if throughput hint enabled, and last hint is recent enough, return */
+	if (scale3d.p_use_throughput_hint) {
+		dt = ktime_us_delta(t, scale3d.last_throughput_hint);
+		if (dt < GR3D_TIMEFRAME)
+			return;
+	}
+
+	mutex_lock(&scale3d.lock);
+
+	scaling_state_check(t);
+
+	/* delay idle_max % of 2 * estimation_window (given in microseconds) */
+	delay = (scale3d.idle_max * scale3d.p_estimation_window) / 500000;
+	schedule_delayed_work(&scale3d.idle_timer, msecs_to_jiffies(delay));
+
+	mutex_unlock(&scale3d.lock);
+}
+
+void nvhost_scale3d_notify_busy(struct nvhost_device *dev)
+{
+	ktime_t t;
+
+	if (!scale3d.enable)
+		return;
+
+	update_load_estimate(0);
+
+	t = ktime_get();
+
+	/* if throughput hint enabled, and last hint is recent enough, return */
+	if (scale3d.p_use_throughput_hint) {
+		unsigned long dt;
+		dt = ktime_us_delta(t, scale3d.last_throughput_hint);
+		if (dt < GR3D_TIMEFRAME)
+			return;
+	}
+
+	mutex_lock(&scale3d.lock);
+
+	cancel_delayed_work(&scale3d.idle_timer);
+	scaling_state_check(t);
+
+	mutex_unlock(&scale3d.lock);
+}
+
+struct score {
+	int size;		/* number of elements */
+	int pos;		/* position in ring buffer */
+	int count;		/* actual item count */
+	unsigned int sum;	/* running sum */
+	unsigned int prev;	/* previous score after 'reset' operation */
+	unsigned int list[];	/* ring buffer */
+};
+
+static struct score *score_init(int capacity)
+{
+	struct score *s;
+
+	s = kzalloc(sizeof(struct score) + capacity * sizeof(int), GFP_KERNEL);
+	if (s == NULL)
+		return NULL;
+
+	s->size = capacity;
+
+	return s;
+}
+
+static void score_delete(struct score *s)
+{
+	kfree(s);
+}
+
+#define score_get_average(s) ((s)->count ? (s)->sum / (s)->count : 0)
+
+static void score_add(struct score *s, unsigned int reading)
+{
+	if (s->count < s->size) {
+		s->sum += reading;
+		s->count++;
+	} else
+		s->sum = s->sum - s->list[s->pos] + reading;
+
+	s->list[s->pos] = reading;
+	s->pos = (s->pos + 1) % s->size;
+}
+
+
+static unsigned int score_reset(struct score *s)
+{
+	s->prev = s->sum;
+
+	s->count = 0;
+	s->pos = 0;
+	s->sum = 0;
+
+	return s->prev;
+}
+
+int freqlist_up(long target, int steps)
+{
+	int i, pos;
+
+	for (i = 0; i < scale3d.freq_count; i++)
+		if (scale3d.freqlist[i] >= target)
+			break;
+
+	pos = min(scale3d.freq_count - 1, i + steps);
+	return scale3d.freqlist[pos];
+}
+
+int freqlist_down(long target, int steps)
+{
+	int i, pos;
+
+	for (i = scale3d.freq_count - 1; i >= 0; i--)
+		if (scale3d.freqlist[i] <= target)
+			break;
+
+	pos = max(0, i - steps);
+	return scale3d.freqlist[pos];
+}
+
+static struct score *busy_history;
+static struct score *hint_history;
+
+/* When a throughput hint is given, perform scaling based on the hint and on
+ * the current idle estimation. This is done as follows:
+ *
+ * 1. On moderate loads force min frequency if the throughput hint is not too
+ *    low.
+ * 2. Otherwise, calculate target-rate = max-rate * load-percentage
+ * 3. Unless the current or average throughput hint is below the minimum
+ *    limit, in which case, choose a higher rate
+ * 4. Or the average throughput hint is above the maximum limit, in which case,
+ *    choose a lower rate.
+ */
+void nvhost_scale3d_set_throughput_hint(int hint)
+{
+	ktime_t now;
+	long busy;
+	long curr;
+	long target;
+	long dt;
+	int avg_busy, avg_hint;
+
+	if (!scale3d.enable)
+		return;
+
+	if (!scale3d.p_use_throughput_hint)
+		return;
+
+	if (scale3d.p_verbosity & GR3D_PRINT_HINT)
+		pr_info("3fds: idle %ld, hint %d\n",
+			scale3d.idle_estimate, hint);
+
+	now = ktime_get();
+	dt = ktime_us_delta(now, scale3d.last_throughput_hint);
+	if (dt > GR3D_TIMEFRAME) {
+		score_reset(busy_history);
+		score_reset(hint_history);
+	}
+
+	scale3d.last_throughput_hint = now;
+
+	busy = 1000 - scale3d.idle_estimate;
+	curr = clk_get_rate(scale3d.clk_3d);
+	target = scale3d.min_rate_3d;
+
+	score_add(busy_history, busy);
+	score_add(hint_history, hint);
+
+	avg_busy = score_get_average(busy_history);
+	avg_hint = score_get_average(hint_history);
+
+	if (busy > 0)
+		target = (curr / 1000) * busy;
+
+	/* In practice, running the gpu at min frequency is typically
+	 * sufficient to keep up performance at loads up to 70% on cases,
+	 * but the average hint value is tested to keep performance up if
+	 * needed.
+	 */
+	if (avg_busy <= scale3d.p_busy_cutoff &&
+	    avg_hint >= scale3d.p_throughput_lower_limit)
+		target = scale3d.min_rate_3d;
+	else {
+		target = (scale3d.max_rate_3d / 1000) * avg_busy;
+
+		/* Scale up if either the current hint or the running average
+		 * are below the target to prevent performance drop.
+		 */
+		if (hint <= scale3d.p_throughput_lo_limit ||
+		    avg_hint <= scale3d.p_throughput_lo_limit) {
+			if (target < curr)
+				target = curr;
+			target = freqlist_up(target, scale3d.p_scale_step);
+		} else if (avg_hint >= scale3d.p_throughput_hi_limit) {
+			if (target > curr)
+				target = curr;
+			target = freqlist_down(target, scale3d.p_scale_step);
+		}
+	}
+
+	scale_to_freq(target);
+
+	if (scale3d.p_verbosity & GR3D_PRINT_TARGET)
+		pr_info("3dfs: busy %ld <%d>, curr %ld, t %ld, hint %d <%d>\n",
+			busy, avg_busy, curr / 1000000, target, hint, avg_hint);
+}
+EXPORT_SYMBOL(nvhost_scale3d_set_throughput_hint);
+
+static void scale3d_idle_handler(struct work_struct *work)
+{
+	int notify_idle = 0;
+
+	if (!scale3d.enable)
+		return;
+
+	mutex_lock(&scale3d.lock);
+
+	if (scale3d.is_idle && tegra_is_clk_enabled(scale3d.clk_3d)) {
+		unsigned long curr = clk_get_rate(scale3d.clk_3d);
+		if (curr > scale3d.min_rate_3d)
+			notify_idle = 1;
+	}
+
+	mutex_unlock(&scale3d.lock);
+
+	if (notify_idle)
+		nvhost_scale3d_notify_idle(NULL);
+}
+
+/*
+ * debugfs parameters to control 3d clock scaling
+ */
+
+void nvhost_scale3d_debug_init(struct dentry *de)
+{
+	struct dentry *d, *f;
+
+	d = debugfs_create_dir("scaling", de);
+	if (!d) {
+		pr_err("scale3d: can\'t create debugfs directory\n");
+		return;
+	}
+
+#define CREATE_SCALE3D_FILE(fname) \
+	do {\
+		f = debugfs_create_u32(#fname, S_IRUGO | S_IWUSR, d,\
+			&scale3d.p_##fname);\
+		if (NULL == f) {\
+			pr_err("scale3d: can\'t create file " #fname "\n");\
+			return;\
+		} \
+	} while (0)
+
+	CREATE_SCALE3D_FILE(estimation_window);
+	CREATE_SCALE3D_FILE(idle_min);
+	CREATE_SCALE3D_FILE(idle_max);
+	CREATE_SCALE3D_FILE(adjust);
+	CREATE_SCALE3D_FILE(scale_emc);
+	CREATE_SCALE3D_FILE(emc_dip);
+	CREATE_SCALE3D_FILE(use_throughput_hint);
+	CREATE_SCALE3D_FILE(throughput_hi_limit);
+	CREATE_SCALE3D_FILE(throughput_lo_limit);
+	CREATE_SCALE3D_FILE(throughput_lower_limit);
+	CREATE_SCALE3D_FILE(scale_step);
+	CREATE_SCALE3D_FILE(verbosity);
+#undef CREATE_SCALE3D_FILE
+}
+
+static ssize_t enable_3d_scaling_show(struct device *device,
+	struct device_attribute *attr, char *buf)
+{
+	ssize_t res;
+
+	res = snprintf(buf, PAGE_SIZE, "%d\n", scale3d_is_enabled());
+
+	return res;
+}
+
+static ssize_t enable_3d_scaling_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	unsigned long val = 0;
+
+	if (strict_strtoul(buf, 10, &val) < 0)
+		return -EINVAL;
+
+	scale3d_enable(val);
+
+	return count;
+}
+
+static DEVICE_ATTR(enable_3d_scaling, S_IRUGO | S_IWUSR,
+	enable_3d_scaling_show, enable_3d_scaling_store);
+
+#define MAX_FREQ_COUNT 0x40 /* 64 frequencies should be enough for anyone */
+
+void nvhost_scale3d_init(struct nvhost_device *d)
+{
+	if (!scale3d.init) {
+		int error;
+		unsigned long max_emc, min_emc;
+		long correction;
+		long rate;
+		int freqs[MAX_FREQ_COUNT];
+
+		mutex_init(&scale3d.lock);
+
+		INIT_WORK(&scale3d.work, scale3d_clocks_handler);
+		INIT_DELAYED_WORK(&scale3d.idle_timer, scale3d_idle_handler);
+
+		scale3d.clk_3d = d->clk[0];
+		if (tegra_get_chipid() == TEGRA_CHIPID_TEGRA3) {
+			scale3d.clk_3d2 = d->clk[1];
+			scale3d.clk_3d_emc = d->clk[2];
+		} else
+			scale3d.clk_3d_emc = d->clk[1];
+
+		scale3d.max_rate_3d = clk_round_rate(scale3d.clk_3d, UINT_MAX);
+		scale3d.min_rate_3d = clk_round_rate(scale3d.clk_3d, 0);
+
+		if (scale3d.max_rate_3d == scale3d.min_rate_3d) {
+			pr_warn("scale3d: 3d max rate = min rate (%lu), "
+				"disabling\n", scale3d.max_rate_3d);
+			scale3d.enable = 0;
+			return;
+		}
+
+		/* emc scaling:
+		 *
+		 * Remc = S * R3d + O - (Sd * (R3d - Rm)^2 + Od)
+		 *
+		 * Remc - 3d.emc rate
+		 * R3d  - 3d.cbus rate
+		 * Rm   - 3d.cbus 'middle' rate = (max + min)/2
+		 * S    - emc_slope
+		 * O    - emc_offset
+		 * Sd   - emc_dip_slope
+		 * Od   - emc_dip_offset
+		 *
+		 * this superposes a quadratic dip centered around the middle 3d
+		 * frequency over a linear correlation of 3d.emc to 3d clock
+		 * rates.
+		 *
+		 * S, O are chosen so that the maximum 3d rate produces the
+		 * maximum 3d.emc rate exactly, and the minimum 3d rate produces
+		 * at least the minimum 3d.emc rate.
+		 *
+		 * Sd and Od are chosen to produce the largest dip that will
+		 * keep 3d.emc frequencies monotonously decreasing with 3d
+		 * frequencies. To achieve this, the first derivative of Remc
+		 * with respect to R3d should be zero for the minimal 3d rate:
+		 *
+		 *   R'emc = S - 2 * Sd * (R3d - Rm)
+		 *   R'emc(R3d-min) = 0
+		 *   S = 2 * Sd * (R3d-min - Rm)
+		 *     = 2 * Sd * (R3d-min - R3d-max) / 2
+		 *   Sd = S / (R3d-min - R3d-max)
+		 *
+		 *   +---------------------------------------------------+
+		 *   | Sd = -(emc-max - emc-min) / (R3d-min - R3d-max)^2 |
+		 *   +---------------------------------------------------+
+		 *
+		 *   dip = Sd * (R3d - Rm)^2 + Od
+		 *
+		 * requiring dip(R3d-min) = 0 and dip(R3d-max) = 0 gives
+		 *
+		 *   Sd * (R3d-min - Rm)^2 + Od = 0
+		 *   Od = -Sd * ((R3d-min - R3d-max) / 2)^2
+		 *      = -Sd * ((R3d-min - R3d-max)^2) / 4
+		 *
+		 *   +------------------------------+
+		 *   | Od = (emc-max - emc-min) / 4 |
+		 *   +------------------------------+
+		 */
+
+		max_emc = clk_round_rate(scale3d.clk_3d_emc, UINT_MAX);
+		min_emc = clk_round_rate(scale3d.clk_3d_emc, 0);
+
+		scale3d.emc_slope = (max_emc - min_emc) /
+			 (scale3d.max_rate_3d - scale3d.min_rate_3d);
+		scale3d.emc_offset = max_emc -
+			scale3d.emc_slope * scale3d.max_rate_3d;
+		/* guarantee max 3d rate maps to max emc rate */
+		scale3d.emc_offset += max_emc -
+			(scale3d.emc_slope * scale3d.max_rate_3d +
+			scale3d.emc_offset);
+
+		scale3d.emc_dip_offset = (max_emc - min_emc) / 4;
+		scale3d.emc_dip_slope =
+			-4 * (scale3d.emc_dip_offset /
+			(POW2(scale3d.max_rate_3d - scale3d.min_rate_3d)));
+		scale3d.emc_xmid =
+			(scale3d.max_rate_3d + scale3d.min_rate_3d) / 2;
+		correction =
+			scale3d.emc_dip_offset +
+				scale3d.emc_dip_slope *
+				POW2(scale3d.max_rate_3d - scale3d.emc_xmid);
+		scale3d.emc_dip_offset -= correction;
+
+		scale3d.is_idle = 1;
+
+		/* set scaling parameter defaults */
+		scale3d.enable = 1;
+		scale3d.idle_min = scale3d.p_idle_min = 100;
+		scale3d.idle_max = scale3d.p_idle_max = 150;
+		scale3d.p_scale_emc = 1;
+		scale3d.p_emc_dip = 1;
+		scale3d.p_verbosity = 0;
+		scale3d.p_adjust = 1;
+		scale3d.p_use_throughput_hint = 1;
+		scale3d.p_throughput_lower_limit = 940;
+		scale3d.p_throughput_lo_limit = 990;
+		scale3d.p_throughput_hi_limit = 1010;
+		scale3d.p_scale_step = 1;
+		scale3d.p_estimation_window = 8000;
+		scale3d.p_busy_cutoff = 750;
+
+		error = device_create_file(&d->dev,
+				&dev_attr_enable_3d_scaling);
+		if (error)
+			dev_err(&d->dev, "failed to create sysfs attributes");
+
+		rate = 0;
+		scale3d.freq_count = 0;
+		while (rate <= scale3d.max_rate_3d) {
+			long rounded_rate;
+			if (unlikely(scale3d.freq_count == MAX_FREQ_COUNT)) {
+				pr_err("%s: too many frequencies\n", __func__);
+				break;
+			}
+			rounded_rate =
+				clk_round_rate(scale3d.clk_3d, rate);
+			freqs[scale3d.freq_count++] = rounded_rate;
+			rate = rounded_rate + 2000;
+		}
+		scale3d.freqlist =
+			kmalloc(scale3d.freq_count * sizeof(int), GFP_KERNEL);
+		if (scale3d.freqlist == NULL)
+			pr_err("%s: can\'t allocate freq table\n", __func__);
+
+		memcpy(scale3d.freqlist, freqs,
+			scale3d.freq_count * sizeof(int));
+
+		busy_history = score_init(GR3D_FRAME_SPAN);
+		if (busy_history == NULL)
+			pr_err("%s: can\'t init load tracking array\n",
+			       __func__);
+
+		hint_history = score_init(GR3D_FRAME_SPAN);
+		if (hint_history == NULL)
+			pr_err("%s: can\'t init throughput tracking array\n",
+			       __func__);
+
+		scale3d.init = 1;
+	}
+}
+
+void nvhost_scale3d_deinit(struct nvhost_device *dev)
+{
+	device_remove_file(&dev->dev, &dev_attr_enable_3d_scaling);
+	scale3d.init = 0;
+	if (scale3d.freqlist != NULL) {
+		kfree(scale3d.freqlist);
+		scale3d.freq_count = 0;
+		scale3d.freqlist = NULL;
+	}
+
+	score_delete(busy_history);
+	score_delete(hint_history);
+}
diff --git a/drivers/video/tegra/host/gr3d/scale3d.h b/drivers/video/tegra/host/gr3d/scale3d.h
new file mode 100644
index 000000000000..f8aae1d591a6
--- /dev/null
+++ b/drivers/video/tegra/host/gr3d/scale3d.h
@@ -0,0 +1,47 @@
+/*
+ * drivers/video/tegra/host/t30/scale3d.h
+ *
+ * Tegra Graphics Host 3D Clock Scaling
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef NVHOST_T30_SCALE3D_H
+#define NVHOST_T30_SCALE3D_H
+
+struct nvhost_device;
+struct device;
+struct dentry;
+
+/* Initialization and de-initialization for module */
+void nvhost_scale3d_init(struct nvhost_device *);
+void nvhost_scale3d_deinit(struct nvhost_device *);
+
+/* Suspend is called when powering down module */
+void nvhost_scale3d_suspend(struct nvhost_device *);
+
+/* reset 3d module load counters, called on resume */
+void nvhost_scale3d_reset(void);
+
+/*
+ * call when performing submit to notify scaling mechanism that 3d module is
+ * in use
+ */
+void nvhost_scale3d_notify_busy(struct nvhost_device *);
+void nvhost_scale3d_notify_idle(struct nvhost_device *);
+
+void nvhost_scale3d_debug_init(struct dentry *de);
+
+#endif
diff --git a/drivers/video/tegra/host/host1x/Makefile b/drivers/video/tegra/host/host1x/Makefile
new file mode 100644
index 000000000000..76664945e12b
--- /dev/null
+++ b/drivers/video/tegra/host/host1x/Makefile
@@ -0,0 +1,8 @@
+GCOV_PROFILE := y
+
+EXTRA_CFLAGS += -Idrivers/video/tegra/host
+
+nvhost-host1x-objs  = \
+	host1x.o
+
+obj-$(CONFIG_TEGRA_GRHOST) += nvhost-host1x.o
diff --git a/drivers/video/tegra/host/host1x/host1x.c b/drivers/video/tegra/host/host1x/host1x.c
new file mode 100644
index 000000000000..31899c78065b
--- /dev/null
+++ b/drivers/video/tegra/host/host1x/host1x.c
@@ -0,0 +1,552 @@
+/*
+ * drivers/video/tegra/host/dev.c
+ *
+ * Tegra Graphics Host Driver Entrypoint
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/uaccess.h>
+#include <linux/file.h>
+#include <linux/clk.h>
+
+#include "dev.h"
+#include "bus.h"
+#include <trace/events/nvhost.h>
+
+#include <linux/nvhost.h>
+#include <linux/nvhost_ioctl.h>
+
+#include "debug.h"
+#include "bus_client.h"
+#include "nvhost_acm.h"
+#include "nvhost_channel.h"
+#include "nvhost_job.h"
+
+#define DRIVER_NAME		"host1x"
+
+struct nvhost_ctrl_userctx {
+	struct nvhost_master *dev;
+	u32 *mod_locks;
+};
+
+static int nvhost_ctrlrelease(struct inode *inode, struct file *filp)
+{
+	struct nvhost_ctrl_userctx *priv = filp->private_data;
+	int i;
+
+	trace_nvhost_ctrlrelease(priv->dev->dev->name);
+
+	filp->private_data = NULL;
+	if (priv->mod_locks[0])
+		nvhost_module_idle(priv->dev->dev);
+	for (i = 1; i < nvhost_syncpt_nb_mlocks(&priv->dev->syncpt); i++)
+		if (priv->mod_locks[i])
+			nvhost_mutex_unlock(&priv->dev->syncpt, i);
+	kfree(priv->mod_locks);
+	kfree(priv);
+	return 0;
+}
+
+static int nvhost_ctrlopen(struct inode *inode, struct file *filp)
+{
+	struct nvhost_master *host =
+		container_of(inode->i_cdev, struct nvhost_master, cdev);
+	struct nvhost_ctrl_userctx *priv;
+	u32 *mod_locks;
+
+	trace_nvhost_ctrlopen(host->dev->name);
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	mod_locks = kzalloc(sizeof(u32)
+			* nvhost_syncpt_nb_mlocks(&host->syncpt),
+			GFP_KERNEL);
+
+	if (!(priv && mod_locks)) {
+		kfree(priv);
+		kfree(mod_locks);
+		return -ENOMEM;
+	}
+
+	priv->dev = host;
+	priv->mod_locks = mod_locks;
+	filp->private_data = priv;
+	return 0;
+}
+
+static int nvhost_ioctl_ctrl_syncpt_read(struct nvhost_ctrl_userctx *ctx,
+	struct nvhost_ctrl_syncpt_read_args *args)
+{
+	if (args->id >= nvhost_syncpt_nb_pts(&ctx->dev->syncpt))
+		return -EINVAL;
+	args->value = nvhost_syncpt_read(&ctx->dev->syncpt, args->id);
+	trace_nvhost_ioctl_ctrl_syncpt_read(args->id, args->value);
+	return 0;
+}
+
+static int nvhost_ioctl_ctrl_syncpt_incr(struct nvhost_ctrl_userctx *ctx,
+	struct nvhost_ctrl_syncpt_incr_args *args)
+{
+	if (args->id >= nvhost_syncpt_nb_pts(&ctx->dev->syncpt))
+		return -EINVAL;
+	trace_nvhost_ioctl_ctrl_syncpt_incr(args->id);
+	nvhost_syncpt_incr(&ctx->dev->syncpt, args->id);
+	return 0;
+}
+
+static int nvhost_ioctl_ctrl_syncpt_waitex(struct nvhost_ctrl_userctx *ctx,
+	struct nvhost_ctrl_syncpt_waitex_args *args)
+{
+	u32 timeout;
+	int err;
+	if (args->id >= nvhost_syncpt_nb_pts(&ctx->dev->syncpt))
+		return -EINVAL;
+	if (args->timeout == NVHOST_NO_TIMEOUT)
+		timeout = MAX_SCHEDULE_TIMEOUT;
+	else
+		timeout = (u32)msecs_to_jiffies(args->timeout);
+
+	err = nvhost_syncpt_wait_timeout(&ctx->dev->syncpt, args->id,
+					args->thresh, timeout, &args->value);
+	trace_nvhost_ioctl_ctrl_syncpt_wait(args->id, args->thresh,
+	  args->timeout, args->value, err);
+
+	return err;
+}
+
+static int nvhost_ioctl_ctrl_module_mutex(struct nvhost_ctrl_userctx *ctx,
+	struct nvhost_ctrl_module_mutex_args *args)
+{
+	int err = 0;
+	if (args->id >= nvhost_syncpt_nb_mlocks(&ctx->dev->syncpt) ||
+	    args->lock > 1)
+		return -EINVAL;
+
+	trace_nvhost_ioctl_ctrl_module_mutex(args->lock, args->id);
+	if (args->lock && !ctx->mod_locks[args->id]) {
+		if (args->id == 0)
+			nvhost_module_busy(ctx->dev->dev);
+		else
+			err = nvhost_mutex_try_lock(&ctx->dev->syncpt,
+					args->id);
+		if (!err)
+			ctx->mod_locks[args->id] = 1;
+	} else if (!args->lock && ctx->mod_locks[args->id]) {
+		if (args->id == 0)
+			nvhost_module_idle(ctx->dev->dev);
+		else
+			nvhost_mutex_unlock(&ctx->dev->syncpt, args->id);
+		ctx->mod_locks[args->id] = 0;
+	}
+	return err;
+}
+
+static int match_by_moduleid(struct device *dev, void *data)
+{
+	struct nvhost_device *ndev = to_nvhost_device(dev);
+	u32 id = (u32)data;
+
+	return id == ndev->moduleid;
+}
+
+static struct nvhost_device *get_ndev_by_moduleid(struct nvhost_master *host,
+		u32 id)
+{
+	struct device *dev = bus_find_device(&nvhost_bus_inst->nvhost_bus_type,
+			NULL, (void *)id, match_by_moduleid);
+
+	return dev ? to_nvhost_device(dev) : NULL;
+}
+
+static int nvhost_ioctl_ctrl_module_regrdwr(struct nvhost_ctrl_userctx *ctx,
+	struct nvhost_ctrl_module_regrdwr_args *args)
+{
+	u32 num_offsets = args->num_offsets;
+	u32 *offsets = args->offsets;
+	u32 *values = args->values;
+	u32 vals[64];
+	struct nvhost_device *ndev;
+
+	trace_nvhost_ioctl_ctrl_module_regrdwr(args->id,
+			args->num_offsets, args->write);
+	/* Check that there is something to read and that block size is
+	 * u32 aligned */
+	if (num_offsets == 0 || args->block_size & 3)
+		return -EINVAL;
+
+	ndev = get_ndev_by_moduleid(ctx->dev, args->id);
+	if (!ndev)
+		return -EINVAL;
+
+	while (num_offsets--) {
+		int err;
+		int remaining = args->block_size >> 2;
+		u32 offs;
+		if (get_user(offs, offsets))
+			return -EFAULT;
+		offsets++;
+		while (remaining) {
+			int batch = min(remaining, 64);
+			if (args->write) {
+				if (copy_from_user(vals, values,
+							batch*sizeof(u32)))
+					return -EFAULT;
+				err = nvhost_write_module_regs(ndev,
+						offs, batch, vals);
+				if (err)
+					return err;
+			} else {
+				err = nvhost_read_module_regs(ndev,
+						offs, batch, vals);
+				if (err)
+					return err;
+				if (copy_to_user(values, vals,
+							batch*sizeof(u32)))
+					return -EFAULT;
+			}
+			remaining -= batch;
+			offs += batch*sizeof(u32);
+			values += batch;
+		}
+	}
+
+	return 0;
+}
+
+static int nvhost_ioctl_ctrl_get_version(struct nvhost_ctrl_userctx *ctx,
+	struct nvhost_get_param_args *args)
+{
+	args->value = NVHOST_SUBMIT_VERSION_MAX_SUPPORTED;
+	return 0;
+}
+
+static long nvhost_ctrlctl(struct file *filp,
+	unsigned int cmd, unsigned long arg)
+{
+	struct nvhost_ctrl_userctx *priv = filp->private_data;
+	u8 buf[NVHOST_IOCTL_CTRL_MAX_ARG_SIZE];
+	int err = 0;
+
+	if ((_IOC_TYPE(cmd) != NVHOST_IOCTL_MAGIC) ||
+		(_IOC_NR(cmd) == 0) ||
+		(_IOC_NR(cmd) > NVHOST_IOCTL_CTRL_LAST) ||
+		(_IOC_SIZE(cmd) > NVHOST_IOCTL_CTRL_MAX_ARG_SIZE))
+		return -EFAULT;
+
+	if (_IOC_DIR(cmd) & _IOC_WRITE) {
+		if (copy_from_user(buf, (void __user *)arg, _IOC_SIZE(cmd)))
+			return -EFAULT;
+	}
+
+	switch (cmd) {
+	case NVHOST_IOCTL_CTRL_SYNCPT_READ:
+		err = nvhost_ioctl_ctrl_syncpt_read(priv, (void *)buf);
+		break;
+	case NVHOST_IOCTL_CTRL_SYNCPT_INCR:
+		err = nvhost_ioctl_ctrl_syncpt_incr(priv, (void *)buf);
+		break;
+	case NVHOST_IOCTL_CTRL_SYNCPT_WAIT:
+		err = nvhost_ioctl_ctrl_syncpt_waitex(priv, (void *)buf);
+		break;
+	case NVHOST_IOCTL_CTRL_MODULE_MUTEX:
+		err = nvhost_ioctl_ctrl_module_mutex(priv, (void *)buf);
+		break;
+	case NVHOST_IOCTL_CTRL_MODULE_REGRDWR:
+		err = nvhost_ioctl_ctrl_module_regrdwr(priv, (void *)buf);
+		break;
+	case NVHOST_IOCTL_CTRL_SYNCPT_WAITEX:
+		err = nvhost_ioctl_ctrl_syncpt_waitex(priv, (void *)buf);
+		break;
+	case NVHOST_IOCTL_CTRL_GET_VERSION:
+		err = nvhost_ioctl_ctrl_get_version(priv, (void *)buf);
+		break;
+	default:
+		err = -ENOTTY;
+		break;
+	}
+
+	if ((err == 0) && (_IOC_DIR(cmd) & _IOC_READ))
+		err = copy_to_user((void __user *)arg, buf, _IOC_SIZE(cmd));
+
+	return err;
+}
+
+static const struct file_operations nvhost_ctrlops = {
+	.owner = THIS_MODULE,
+	.release = nvhost_ctrlrelease,
+	.open = nvhost_ctrlopen,
+	.unlocked_ioctl = nvhost_ctrlctl
+};
+
+static void power_on_host(struct nvhost_device *dev)
+{
+	struct nvhost_master *host = nvhost_get_drvdata(dev);
+	nvhost_syncpt_reset(&host->syncpt);
+	nvhost_intr_start(&host->intr, clk_get_rate(dev->clk[0]));
+}
+
+static int power_off_host(struct nvhost_device *dev)
+{
+	struct nvhost_master *host = nvhost_get_drvdata(dev);
+	nvhost_syncpt_save(&host->syncpt);
+	nvhost_intr_stop(&host->intr);
+	return 0;
+}
+
+static void clock_on_host(struct nvhost_device *dev)
+{
+	struct nvhost_master *host = nvhost_get_drvdata(dev);
+	nvhost_intr_start(&host->intr, clk_get_rate(dev->clk[0]));
+}
+
+static int clock_off_host(struct nvhost_device *dev)
+{
+	struct nvhost_master *host = nvhost_get_drvdata(dev);
+	nvhost_intr_stop(&host->intr);
+	return 0;
+}
+
+static int __devinit nvhost_user_init(struct nvhost_master *host)
+{
+	int err, devno;
+
+	host->nvhost_class = class_create(THIS_MODULE, IFACE_NAME);
+	if (IS_ERR(host->nvhost_class)) {
+		err = PTR_ERR(host->nvhost_class);
+		dev_err(&host->dev->dev, "failed to create class\n");
+		goto fail;
+	}
+
+	err = alloc_chrdev_region(&devno, 0, 1, IFACE_NAME);
+	if (err < 0) {
+		dev_err(&host->dev->dev, "failed to reserve chrdev region\n");
+		goto fail;
+	}
+
+	cdev_init(&host->cdev, &nvhost_ctrlops);
+	host->cdev.owner = THIS_MODULE;
+	err = cdev_add(&host->cdev, devno, 1);
+	if (err < 0)
+		goto fail;
+	host->ctrl = device_create(host->nvhost_class, NULL, devno, NULL,
+			IFACE_NAME "-ctrl");
+	if (IS_ERR(host->ctrl)) {
+		err = PTR_ERR(host->ctrl);
+		dev_err(&host->dev->dev, "failed to create ctrl device\n");
+		goto fail;
+	}
+
+	return 0;
+fail:
+	return err;
+}
+
+struct nvhost_channel *nvhost_alloc_channel(struct nvhost_device *dev)
+{
+	BUG_ON(!host_device_op().alloc_nvhost_channel);
+	return host_device_op().alloc_nvhost_channel(dev);
+}
+
+void nvhost_free_channel(struct nvhost_channel *ch)
+{
+	BUG_ON(!host_device_op().free_nvhost_channel);
+	host_device_op().free_nvhost_channel(ch);
+}
+
+static void nvhost_free_resources(struct nvhost_master *host)
+{
+	kfree(host->intr.syncpt);
+	host->intr.syncpt = 0;
+}
+
+static int __devinit nvhost_alloc_resources(struct nvhost_master *host)
+{
+	int err;
+
+	err = nvhost_init_chip_support(host);
+	if (err)
+		return err;
+
+	host->intr.syncpt = kzalloc(sizeof(struct nvhost_intr_syncpt) *
+				    nvhost_syncpt_nb_pts(&host->syncpt),
+				    GFP_KERNEL);
+
+	if (!host->intr.syncpt) {
+		/* frees happen in the support removal phase */
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static int __devinit nvhost_probe(struct nvhost_device *dev,
+	struct nvhost_device_id *id_table)
+{
+	struct nvhost_master *host;
+	struct resource *regs, *intr0, *intr1;
+	int i, err;
+
+	regs = nvhost_get_resource(dev, IORESOURCE_MEM, 0);
+	intr0 = nvhost_get_resource(dev, IORESOURCE_IRQ, 0);
+	intr1 = nvhost_get_resource(dev, IORESOURCE_IRQ, 1);
+
+	if (!regs || !intr0 || !intr1) {
+		dev_err(&dev->dev, "missing required platform resources\n");
+		return -ENXIO;
+	}
+
+	host = kzalloc(sizeof(*host), GFP_KERNEL);
+	if (!host)
+		return -ENOMEM;
+
+	/*  Register host1x device as bus master */
+	host->dev = dev;
+
+	/* Copy host1x parameters */
+	memcpy(&host->info, dev->dev.platform_data,
+			sizeof(struct host1x_device_info));
+
+	host->reg_mem = request_mem_region(regs->start,
+					resource_size(regs), dev->name);
+	if (!host->reg_mem) {
+		dev_err(&dev->dev, "failed to get host register memory\n");
+		err = -ENXIO;
+		goto fail;
+	}
+
+	host->aperture = ioremap(regs->start, resource_size(regs));
+	if (!host->aperture) {
+		dev_err(&dev->dev, "failed to remap host registers\n");
+		err = -ENXIO;
+		goto fail;
+	}
+
+	err = nvhost_alloc_resources(host);
+	if (err) {
+		dev_err(&dev->dev, "failed to init chip support\n");
+		goto fail;
+	}
+
+	host->memmgr = mem_op().alloc_mgr();
+	if (!host->memmgr) {
+		dev_err(&dev->dev, "unable to create nvmap client\n");
+		err = -EIO;
+		goto fail;
+	}
+
+	/*  Register host1x device as bus master */
+	host->dev = dev;
+
+	/*  Give pointer to host1x via driver */
+	nvhost_set_drvdata(dev, host);
+
+	nvhost_bus_add_host(host);
+
+	err = nvhost_syncpt_init(dev, &host->syncpt);
+	if (err)
+		goto fail;
+
+	err = nvhost_intr_init(&host->intr, intr1->start, intr0->start);
+	if (err)
+		goto fail;
+
+	err = nvhost_user_init(host);
+	if (err)
+		goto fail;
+
+	err = nvhost_module_init(dev);
+	if (err)
+		goto fail;
+
+	for (i = 0; i < host->dev->num_clks; i++)
+		clk_enable(host->dev->clk[i]);
+	nvhost_syncpt_reset(&host->syncpt);
+	for (i = 0; i < host->dev->num_clks; i++)
+		clk_disable(host->dev->clk[0]);
+
+	nvhost_debug_init(host);
+
+	dev_info(&dev->dev, "initialized\n");
+	return 0;
+
+fail:
+	nvhost_free_resources(host);
+	if (host->memmgr)
+		mem_op().put_mgr(host->memmgr);
+	kfree(host);
+	return err;
+}
+
+static int __exit nvhost_remove(struct nvhost_device *dev)
+{
+	struct nvhost_master *host = nvhost_get_drvdata(dev);
+	nvhost_intr_deinit(&host->intr);
+	nvhost_syncpt_deinit(&host->syncpt);
+	nvhost_free_resources(host);
+	return 0;
+}
+
+static int nvhost_suspend(struct nvhost_device *dev, pm_message_t state)
+{
+	struct nvhost_master *host = nvhost_get_drvdata(dev);
+	int ret = 0;
+
+	ret = nvhost_module_suspend(host->dev);
+	dev_info(&dev->dev, "suspend status: %d\n", ret);
+
+	return ret;
+}
+
+static int nvhost_resume(struct nvhost_device *dev)
+{
+	dev_info(&dev->dev, "resuming\n");
+	return 0;
+}
+
+static struct nvhost_driver nvhost_driver = {
+	.probe = nvhost_probe,
+	.remove = __exit_p(nvhost_remove),
+	.suspend = nvhost_suspend,
+	.resume = nvhost_resume,
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = DRIVER_NAME
+	},
+	.finalize_poweron = power_on_host,
+	.prepare_poweroff = power_off_host,
+	.finalize_clockon = clock_on_host,
+	.prepare_clockoff = clock_off_host,
+};
+
+static int __init nvhost_mod_init(void)
+{
+	return nvhost_driver_register(&nvhost_driver);
+}
+
+static void __exit nvhost_mod_exit(void)
+{
+	nvhost_driver_unregister(&nvhost_driver);
+}
+
+/* host1x master device needs nvmap to be instantiated first.
+ * nvmap is instantiated via fs_initcall.
+ * Hence instantiate host1x master device using rootfs_initcall
+ * which is one level after fs_initcall. */
+rootfs_initcall(nvhost_mod_init);
+module_exit(nvhost_mod_exit);
+
diff --git a/drivers/video/tegra/host/host1x/host1x.h b/drivers/video/tegra/host/host1x/host1x.h
new file mode 100644
index 000000000000..49916b0757d4
--- /dev/null
+++ b/drivers/video/tegra/host/host1x/host1x.h
@@ -0,0 +1,78 @@
+/*
+ * drivers/video/tegra/host/host1x/host1x.h
+ *
+ * Tegra Graphics Host Driver Entrypoint
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_HOST1X_H
+#define __NVHOST_HOST1X_H
+
+#include <linux/cdev.h>
+#include <linux/nvhost.h>
+
+#include "nvhost_syncpt.h"
+#include "nvhost_intr.h"
+
+#define TRACE_MAX_LENGTH	128U
+#define IFACE_NAME		"nvhost"
+
+struct nvhost_channel;
+struct mem_mgr;
+
+struct host1x_device_info {
+	int		nb_channels;	/* host1x: num channels supported */
+	int		nb_pts; 	/* host1x: num syncpoints supported */
+	int		nb_bases;	/* host1x: num syncpoints supported */
+	u32		client_managed; /* host1x: client managed syncpts */
+	int		nb_mlocks;	/* host1x: number of mlocks */
+	const char	**syncpt_names;	/* names of sync points */
+};
+
+struct nvhost_master {
+	void __iomem *aperture;
+	void __iomem *sync_aperture;
+	struct resource *reg_mem;
+	struct class *nvhost_class;
+	struct cdev cdev;
+	struct device *ctrl;
+	struct nvhost_syncpt syncpt;
+	struct mem_mgr *memmgr;
+	struct nvhost_intr intr;
+	struct nvhost_device *dev;
+	atomic_t clientid;
+
+	struct host1x_device_info info;
+};
+
+extern struct nvhost_master *nvhost;
+
+void nvhost_debug_init(struct nvhost_master *master);
+void nvhost_debug_dump(struct nvhost_master *master);
+
+struct nvhost_channel *nvhost_alloc_channel(struct nvhost_device *dev);
+void nvhost_free_channel(struct nvhost_channel *ch);
+
+extern pid_t nvhost_debug_null_kickoff_pid;
+
+static inline struct nvhost_master *nvhost_get_host(struct nvhost_device *_dev)
+{
+	return (_dev->dev.parent) ? \
+		((struct nvhost_master *) dev_get_drvdata(_dev->dev.parent)) : \
+		((struct nvhost_master *) dev_get_drvdata(&(_dev->dev)));
+}
+
+#endif
diff --git a/drivers/video/tegra/host/host1x/host1x01_hardware.h b/drivers/video/tegra/host/host1x/host1x01_hardware.h
new file mode 100644
index 000000000000..1d30cc74266a
--- /dev/null
+++ b/drivers/video/tegra/host/host1x/host1x01_hardware.h
@@ -0,0 +1,170 @@
+/*
+ * drivers/video/tegra/host/host1x/host1x01_hardware.h
+ *
+ * Tegra Graphics Host Register Offsets for T20/T30
+ *
+ * Copyright (c) 2010-2012 NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_HOST1X01_HARDWARE_H
+#define __NVHOST_HOST1X01_HARDWARE_H
+
+#include <linux/types.h>
+#include <linux/bitops.h>
+#include "hw_host1x01_channel.h"
+#include "hw_host1x01_sync.h"
+#include "hw_host1x01_uclass.h"
+
+/* class ids */
+enum {
+	NV_HOST1X_CLASS_ID = 0x1,
+	NV_VIDEO_ENCODE_MPEG_CLASS_ID = 0x20,
+	NV_GRAPHICS_3D_CLASS_ID = 0x60
+};
+
+
+/* channel registers */
+#define NV_HOST1X_CHANNEL_MAP_SIZE_BYTES 16384
+#define NV_HOST1X_SYNC_MLOCK_NUM 16
+
+/* sync registers */
+#define HOST1X_CHANNEL_SYNC_REG_BASE   0x3000
+#define NV_HOST1X_NB_MLOCKS 16
+
+static inline u32 nvhost_class_host_wait_syncpt(
+	unsigned indx, unsigned threshold)
+{
+	return (indx << 24) | (threshold & 0xffffff);
+}
+
+static inline u32 nvhost_class_host_load_syncpt_base(
+	unsigned indx, unsigned threshold)
+{
+	return host1x_uclass_wait_syncpt_indx_f(indx)
+		| host1x_uclass_wait_syncpt_thresh_f(threshold);
+}
+
+static inline u32 nvhost_class_host_wait_syncpt_base(
+	unsigned indx, unsigned base_indx, unsigned offset)
+{
+	return host1x_uclass_wait_syncpt_base_indx_f(indx)
+		| host1x_uclass_wait_syncpt_base_base_indx_f(base_indx)
+		| host1x_uclass_wait_syncpt_base_offset_f(offset);
+}
+
+static inline u32 nvhost_class_host_incr_syncpt_base(
+	unsigned base_indx, unsigned offset)
+{
+	return host1x_uclass_incr_syncpt_base_base_indx_f(base_indx)
+		| host1x_uclass_incr_syncpt_base_offset_f(offset);
+}
+
+static inline u32 nvhost_class_host_incr_syncpt(
+	unsigned cond, unsigned indx)
+{
+	return host1x_uclass_incr_syncpt_cond_f(cond)
+		| host1x_uclass_incr_syncpt_indx_f(indx);
+}
+
+enum {
+	NV_HOST_MODULE_HOST1X = 0,
+	NV_HOST_MODULE_MPE = 1,
+	NV_HOST_MODULE_GR3D = 6
+};
+
+static inline u32 nvhost_class_host_indoff_reg_write(
+	unsigned mod_id, unsigned offset, bool auto_inc)
+{
+	u32 v = host1x_uclass_indoff_indbe_f(0xf)
+		| host1x_uclass_indoff_indmodid_f(mod_id)
+		| host1x_uclass_indoff_indroffset_f(offset);
+	if (auto_inc)
+		v |= host1x_uclass_indoff_autoinc_f(1);
+	return v;
+}
+
+static inline u32 nvhost_class_host_indoff_reg_read(
+	unsigned mod_id, unsigned offset, bool auto_inc)
+{
+	u32 v = host1x_uclass_indoff_indmodid_f(mod_id)
+		| host1x_uclass_indoff_indroffset_f(offset)
+		| host1x_uclass_indoff_rwn_read_v();
+	if (auto_inc)
+		v |= host1x_uclass_indoff_autoinc_f(1);
+	return v;
+}
+
+
+/* cdma opcodes */
+static inline u32 nvhost_opcode_setclass(
+	unsigned class_id, unsigned offset, unsigned mask)
+{
+	return (0 << 28) | (offset << 16) | (class_id << 6) | mask;
+}
+
+static inline u32 nvhost_opcode_incr(unsigned offset, unsigned count)
+{
+	return (1 << 28) | (offset << 16) | count;
+}
+
+static inline u32 nvhost_opcode_nonincr(unsigned offset, unsigned count)
+{
+	return (2 << 28) | (offset << 16) | count;
+}
+
+static inline u32 nvhost_opcode_mask(unsigned offset, unsigned mask)
+{
+	return (3 << 28) | (offset << 16) | mask;
+}
+
+static inline u32 nvhost_opcode_imm(unsigned offset, unsigned value)
+{
+	return (4 << 28) | (offset << 16) | value;
+}
+
+static inline u32 nvhost_opcode_imm_incr_syncpt(unsigned cond, unsigned indx)
+{
+	return nvhost_opcode_imm(host1x_uclass_incr_syncpt_r(),
+		nvhost_class_host_incr_syncpt(cond, indx));
+}
+
+static inline u32 nvhost_opcode_restart(unsigned address)
+{
+	return (5 << 28) | (address >> 4);
+}
+
+static inline u32 nvhost_opcode_gather(unsigned count)
+{
+	return (6 << 28) | count;
+}
+
+static inline u32 nvhost_opcode_gather_nonincr(unsigned offset,	unsigned count)
+{
+	return (6 << 28) | (offset << 16) | BIT(15) | count;
+}
+
+static inline u32 nvhost_opcode_gather_incr(unsigned offset, unsigned count)
+{
+	return (6 << 28) | (offset << 16) | BIT(15) | BIT(14) | count;
+}
+
+#define NVHOST_OPCODE_NOOP nvhost_opcode_nonincr(0, 0)
+
+static inline u32 nvhost_mask2(unsigned x, unsigned y)
+{
+	return 1 | (1 << (y - x));
+}
+
+#endif
diff --git a/drivers/video/tegra/host/host1x/host1x_cdma.c b/drivers/video/tegra/host/host1x/host1x_cdma.c
new file mode 100644
index 000000000000..5a29ff652efe
--- /dev/null
+++ b/drivers/video/tegra/host/host1x/host1x_cdma.c
@@ -0,0 +1,517 @@
+/*
+ * drivers/video/tegra/host/host1x/host1x_cdma.c
+ *
+ * Tegra Graphics Host Command DMA
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/slab.h>
+#include "nvhost_acm.h"
+#include "nvhost_cdma.h"
+#include "nvhost_channel.h"
+#include "dev.h"
+#include "chip_support.h"
+#include "nvhost_memmgr.h"
+
+#include "host1x_cdma.h"
+#include "host1x_hwctx.h"
+
+static inline u32 host1x_channel_dmactrl(int stop, int get_rst, int init_get)
+{
+	return host1x_channel_dmactrl_dmastop_f(stop)
+		| host1x_channel_dmactrl_dmagetrst_f(get_rst)
+		| host1x_channel_dmactrl_dmainitget_f(init_get);
+}
+
+static void cdma_timeout_handler(struct work_struct *work);
+
+/*
+ * push_buffer
+ *
+ * The push buffer is a circular array of words to be fetched by command DMA.
+ * Note that it works slightly differently to the sync queue; fence == cur
+ * means that the push buffer is full, not empty.
+ */
+
+
+/**
+ * Reset to empty push buffer
+ */
+static void push_buffer_reset(struct push_buffer *pb)
+{
+	pb->fence = PUSH_BUFFER_SIZE - 8;
+	pb->cur = 0;
+}
+
+/**
+ * Init push buffer resources
+ */
+static int push_buffer_init(struct push_buffer *pb)
+{
+	struct nvhost_cdma *cdma = pb_to_cdma(pb);
+	struct mem_mgr *mgr = cdma_to_memmgr(cdma);
+	pb->mem = NULL;
+	pb->mapped = NULL;
+	pb->phys = 0;
+	pb->client_handle = NULL;
+
+	BUG_ON(!cdma_pb_op().reset);
+	cdma_pb_op().reset(pb);
+
+	/* allocate and map pushbuffer memory */
+	pb->mem = mem_op().alloc(mgr, PUSH_BUFFER_SIZE + 4, 32,
+			      mem_mgr_flag_write_combine);
+	if (IS_ERR_OR_NULL(pb->mem)) {
+		pb->mem = NULL;
+		goto fail;
+	}
+	pb->mapped = mem_op().mmap(pb->mem);
+	if (pb->mapped == NULL)
+		goto fail;
+
+	/* pin pushbuffer and get physical address */
+	pb->phys = mem_op().pin(mgr, pb->mem);
+	if (pb->phys >= 0xfffff000) {
+		pb->phys = 0;
+		goto fail;
+	}
+
+	/* memory for storing nvmap client and handles for each opcode pair */
+	pb->client_handle = kzalloc(NVHOST_GATHER_QUEUE_SIZE *
+				sizeof(struct mem_mgr_handle),
+			GFP_KERNEL);
+	if (!pb->client_handle)
+		goto fail;
+
+	/* put the restart at the end of pushbuffer memory */
+	*(pb->mapped + (PUSH_BUFFER_SIZE >> 2)) =
+		nvhost_opcode_restart(pb->phys);
+
+	return 0;
+
+fail:
+	cdma_pb_op().destroy(pb);
+	return -ENOMEM;
+}
+
+/**
+ * Clean up push buffer resources
+ */
+static void push_buffer_destroy(struct push_buffer *pb)
+{
+	struct nvhost_cdma *cdma = pb_to_cdma(pb);
+	struct mem_mgr *mgr = cdma_to_memmgr(cdma);
+	if (pb->mapped)
+		mem_op().munmap(pb->mem, pb->mapped);
+
+	if (pb->phys != 0)
+		mem_op().unpin(mgr, pb->mem);
+
+	if (pb->mem)
+		mem_op().put(mgr, pb->mem);
+
+	kfree(pb->client_handle);
+
+	pb->mem = NULL;
+	pb->mapped = NULL;
+	pb->phys = 0;
+	pb->client_handle = 0;
+}
+
+/**
+ * Push two words to the push buffer
+ * Caller must ensure push buffer is not full
+ */
+static void push_buffer_push_to(struct push_buffer *pb,
+		struct mem_mgr *client, struct mem_handle *handle,
+		u32 op1, u32 op2)
+{
+	u32 cur = pb->cur;
+	u32 *p = (u32 *)((u32)pb->mapped + cur);
+	u32 cur_nvmap = (cur/8) & (NVHOST_GATHER_QUEUE_SIZE - 1);
+	BUG_ON(cur == pb->fence);
+	*(p++) = op1;
+	*(p++) = op2;
+	pb->client_handle[cur_nvmap].client = client;
+	pb->client_handle[cur_nvmap].handle = handle;
+	pb->cur = (cur + 8) & (PUSH_BUFFER_SIZE - 1);
+}
+
+/**
+ * Pop a number of two word slots from the push buffer
+ * Caller must ensure push buffer is not empty
+ */
+static void push_buffer_pop_from(struct push_buffer *pb,
+		unsigned int slots)
+{
+	/* Clear the nvmap references for old items from pb */
+	unsigned int i;
+	u32 fence_nvmap = pb->fence/8;
+	for (i = 0; i < slots; i++) {
+		int cur_fence_nvmap = (fence_nvmap+i)
+				& (NVHOST_GATHER_QUEUE_SIZE - 1);
+		struct mem_mgr_handle *h = &pb->client_handle[cur_fence_nvmap];
+		h->client = NULL;
+		h->handle = NULL;
+	}
+	/* Advance the next write position */
+	pb->fence = (pb->fence + slots * 8) & (PUSH_BUFFER_SIZE - 1);
+}
+
+/**
+ * Return the number of two word slots free in the push buffer
+ */
+static u32 push_buffer_space(struct push_buffer *pb)
+{
+	return ((pb->fence - pb->cur) & (PUSH_BUFFER_SIZE - 1)) / 8;
+}
+
+static u32 push_buffer_putptr(struct push_buffer *pb)
+{
+	return pb->phys + pb->cur;
+}
+
+/*
+ * The syncpt incr buffer is filled with methods to increment syncpts, which
+ * is later GATHER-ed into the mainline PB. It's used when a timed out context
+ * is interleaved with other work, so needs to inline the syncpt increments
+ * to maintain the count (but otherwise does no work).
+ */
+
+/**
+ * Init timeout resources
+ */
+static int cdma_timeout_init(struct nvhost_cdma *cdma,
+				 u32 syncpt_id)
+{
+	if (syncpt_id == NVSYNCPT_INVALID)
+		return -EINVAL;
+
+	INIT_DELAYED_WORK(&cdma->timeout.wq, cdma_timeout_handler);
+	cdma->timeout.initialized = true;
+
+	return 0;
+}
+
+/**
+ * Clean up timeout resources
+ */
+static void cdma_timeout_destroy(struct nvhost_cdma *cdma)
+{
+	if (cdma->timeout.initialized)
+		cancel_delayed_work(&cdma->timeout.wq);
+	cdma->timeout.initialized = false;
+}
+
+/**
+ * Increment timedout buffer's syncpt via CPU.
+ */
+static void cdma_timeout_cpu_incr(struct nvhost_cdma *cdma, u32 getptr,
+				u32 syncpt_incrs, u32 syncval, u32 nr_slots,
+				u32 waitbases)
+{
+	struct nvhost_master *dev = cdma_to_dev(cdma);
+	struct push_buffer *pb = &cdma->push_buffer;
+	u32 i, getidx;
+
+	for (i = 0; i < syncpt_incrs; i++)
+		nvhost_syncpt_cpu_incr(&dev->syncpt, cdma->timeout.syncpt_id);
+
+	/* after CPU incr, ensure shadow is up to date */
+	nvhost_syncpt_update_min(&dev->syncpt, cdma->timeout.syncpt_id);
+
+	/* Synchronize wait bases. 2D wait bases are synchronized with
+	 * syncpoint 19. Hence wait bases are not updated when syncptid=18. */
+
+	if (cdma->timeout.syncpt_id != NVSYNCPT_2D_0 && waitbases) {
+		void __iomem *p;
+		p = dev->sync_aperture + host1x_sync_syncpt_base_0_r() +
+				(__ffs(waitbases) * sizeof(u32));
+		writel(syncval, p);
+		dev->syncpt.base_val[__ffs(waitbases)] = syncval;
+	}
+
+	/* NOP all the PB slots */
+	getidx = getptr - pb->phys;
+	while (nr_slots--) {
+		u32 *p = (u32 *)((u32)pb->mapped + getidx);
+		*(p++) = NVHOST_OPCODE_NOOP;
+		*(p++) = NVHOST_OPCODE_NOOP;
+		dev_dbg(&dev->dev->dev, "%s: NOP at 0x%x\n",
+			__func__, pb->phys + getidx);
+		getidx = (getidx + 8) & (PUSH_BUFFER_SIZE - 1);
+	}
+	wmb();
+}
+
+/**
+ * Start channel DMA
+ */
+static void cdma_start(struct nvhost_cdma *cdma)
+{
+	void __iomem *chan_regs = cdma_to_channel(cdma)->aperture;
+
+	if (cdma->running)
+		return;
+
+	BUG_ON(!cdma_pb_op().putptr);
+	cdma->last_put = cdma_pb_op().putptr(&cdma->push_buffer);
+
+	writel(host1x_channel_dmactrl(true, false, false),
+		chan_regs + host1x_channel_dmactrl_r());
+
+	/* set base, put, end pointer (all of memory) */
+	writel(0, chan_regs + host1x_channel_dmastart_r());
+	writel(cdma->last_put, chan_regs + host1x_channel_dmaput_r());
+	writel(0xFFFFFFFF, chan_regs + host1x_channel_dmaend_r());
+
+	/* reset GET */
+	writel(host1x_channel_dmactrl(true, true, true),
+		chan_regs + host1x_channel_dmactrl_r());
+
+	/* start the command DMA */
+	writel(host1x_channel_dmactrl(false, false, false),
+		chan_regs + host1x_channel_dmactrl_r());
+
+	cdma->running = true;
+}
+
+/**
+ * Similar to cdma_start(), but rather than starting from an idle
+ * state (where DMA GET is set to DMA PUT), on a timeout we restore
+ * DMA GET from an explicit value (so DMA may again be pending).
+ */
+static void cdma_timeout_restart(struct nvhost_cdma *cdma, u32 getptr)
+{
+	struct nvhost_master *dev = cdma_to_dev(cdma);
+	void __iomem *chan_regs = cdma_to_channel(cdma)->aperture;
+
+	if (cdma->running)
+		return;
+
+	BUG_ON(!cdma_pb_op().putptr);
+	cdma->last_put = cdma_pb_op().putptr(&cdma->push_buffer);
+
+	writel(host1x_channel_dmactrl(true, false, false),
+		chan_regs + host1x_channel_dmactrl_r());
+
+	/* set base, end pointer (all of memory) */
+	writel(0, chan_regs + host1x_channel_dmastart_r());
+	writel(0xFFFFFFFF, chan_regs + host1x_channel_dmaend_r());
+
+	/* set GET, by loading the value in PUT (then reset GET) */
+	writel(getptr, chan_regs + host1x_channel_dmaput_r());
+	writel(host1x_channel_dmactrl(true, true, true),
+		chan_regs + host1x_channel_dmactrl_r());
+
+	dev_dbg(&dev->dev->dev,
+		"%s: DMA GET 0x%x, PUT HW 0x%x / shadow 0x%x\n",
+		__func__,
+		readl(chan_regs + host1x_channel_dmaget_r()),
+		readl(chan_regs + host1x_channel_dmaput_r()),
+		cdma->last_put);
+
+	/* deassert GET reset and set PUT */
+	writel(host1x_channel_dmactrl(true, false, false),
+		chan_regs + host1x_channel_dmactrl_r());
+	writel(cdma->last_put, chan_regs + host1x_channel_dmaput_r());
+
+	/* start the command DMA */
+	writel(host1x_channel_dmactrl(false, false, false),
+		chan_regs + host1x_channel_dmactrl_r());
+
+	cdma->running = true;
+}
+
+/**
+ * Kick channel DMA into action by writing its PUT offset (if it has changed)
+ */
+static void cdma_kick(struct nvhost_cdma *cdma)
+{
+	u32 put;
+	BUG_ON(!cdma_pb_op().putptr);
+
+	put = cdma_pb_op().putptr(&cdma->push_buffer);
+
+	if (put != cdma->last_put) {
+		void __iomem *chan_regs = cdma_to_channel(cdma)->aperture;
+		wmb();
+		writel(put, chan_regs + host1x_channel_dmaput_r());
+		cdma->last_put = put;
+	}
+}
+
+static void cdma_stop(struct nvhost_cdma *cdma)
+{
+	void __iomem *chan_regs = cdma_to_channel(cdma)->aperture;
+
+	mutex_lock(&cdma->lock);
+	if (cdma->running) {
+		nvhost_cdma_wait_locked(cdma, CDMA_EVENT_SYNC_QUEUE_EMPTY);
+		writel(host1x_channel_dmactrl(true, false, false),
+			chan_regs + host1x_channel_dmactrl_r());
+		cdma->running = false;
+	}
+	mutex_unlock(&cdma->lock);
+}
+
+/**
+ * Stops both channel's command processor and CDMA immediately.
+ * Also, tears down the channel and resets corresponding module.
+ */
+static void cdma_timeout_teardown_begin(struct nvhost_cdma *cdma)
+{
+	struct nvhost_master *dev = cdma_to_dev(cdma);
+	struct nvhost_channel *ch = cdma_to_channel(cdma);
+	u32 cmdproc_stop;
+
+	BUG_ON(cdma->torndown);
+
+	dev_dbg(&dev->dev->dev,
+		"begin channel teardown (channel id %d)\n", ch->chid);
+
+	cmdproc_stop = readl(dev->sync_aperture + host1x_sync_cmdproc_stop_r());
+	cmdproc_stop |= BIT(ch->chid);
+	writel(cmdproc_stop, dev->sync_aperture + host1x_sync_cmdproc_stop_r());
+
+	dev_dbg(&dev->dev->dev,
+		"%s: DMA GET 0x%x, PUT HW 0x%x / shadow 0x%x\n",
+		__func__,
+		readl(ch->aperture + host1x_channel_dmaget_r()),
+		readl(ch->aperture + host1x_channel_dmaput_r()),
+		cdma->last_put);
+
+	writel(host1x_channel_dmactrl(true, false, false),
+		ch->aperture + host1x_channel_dmactrl_r());
+
+	writel(BIT(ch->chid), dev->sync_aperture + host1x_sync_ch_teardown_r());
+	nvhost_module_reset(ch->dev);
+
+	cdma->running = false;
+	cdma->torndown = true;
+}
+
+static void cdma_timeout_teardown_end(struct nvhost_cdma *cdma, u32 getptr)
+{
+	struct nvhost_master *dev = cdma_to_dev(cdma);
+	struct nvhost_channel *ch = cdma_to_channel(cdma);
+	u32 cmdproc_stop;
+
+	BUG_ON(!cdma->torndown || cdma->running);
+
+	dev_dbg(&dev->dev->dev,
+		"end channel teardown (id %d, DMAGET restart = 0x%x)\n",
+		ch->chid, getptr);
+
+	cmdproc_stop = readl(dev->sync_aperture + host1x_sync_cmdproc_stop_r());
+	cmdproc_stop &= ~(BIT(ch->chid));
+	writel(cmdproc_stop, dev->sync_aperture + host1x_sync_cmdproc_stop_r());
+
+	cdma->torndown = false;
+	cdma_timeout_restart(cdma, getptr);
+}
+
+/**
+ * If this timeout fires, it indicates the current sync_queue entry has
+ * exceeded its TTL and the userctx should be timed out and remaining
+ * submits already issued cleaned up (future submits return an error).
+ */
+static void cdma_timeout_handler(struct work_struct *work)
+{
+	struct nvhost_cdma *cdma;
+	struct nvhost_master *dev;
+	struct nvhost_syncpt *sp;
+	struct nvhost_channel *ch;
+
+	u32 syncpt_val;
+
+	u32 prev_cmdproc, cmdproc_stop;
+
+	cdma = container_of(to_delayed_work(work), struct nvhost_cdma,
+			    timeout.wq);
+	dev = cdma_to_dev(cdma);
+	sp = &dev->syncpt;
+	ch = cdma_to_channel(cdma);
+
+	mutex_lock(&cdma->lock);
+
+	if (!cdma->timeout.clientid) {
+		dev_dbg(&dev->dev->dev,
+			 "cdma_timeout: expired, but has no clientid\n");
+		mutex_unlock(&cdma->lock);
+		return;
+	}
+
+	/* stop processing to get a clean snapshot */
+	prev_cmdproc = readl(dev->sync_aperture + host1x_sync_cmdproc_stop_r());
+	cmdproc_stop = prev_cmdproc | BIT(ch->chid);
+	writel(cmdproc_stop, dev->sync_aperture + host1x_sync_cmdproc_stop_r());
+
+	dev_dbg(&dev->dev->dev, "cdma_timeout: cmdproc was 0x%x is 0x%x\n",
+		prev_cmdproc, cmdproc_stop);
+
+	syncpt_val = nvhost_syncpt_update_min(&dev->syncpt,
+			cdma->timeout.syncpt_id);
+
+	/* has buffer actually completed? */
+	if ((s32)(syncpt_val - cdma->timeout.syncpt_val) >= 0) {
+		dev_dbg(&dev->dev->dev,
+			 "cdma_timeout: expired, but buffer had completed\n");
+		/* restore */
+		cmdproc_stop = prev_cmdproc & ~(BIT(ch->chid));
+		writel(cmdproc_stop,
+			dev->sync_aperture + host1x_sync_cmdproc_stop_r());
+		mutex_unlock(&cdma->lock);
+		return;
+	}
+
+	dev_warn(&dev->dev->dev,
+		"%s: timeout: %d (%s) ctx 0x%p, HW thresh %d, done %d\n",
+		__func__,
+		cdma->timeout.syncpt_id,
+		syncpt_op().name(sp, cdma->timeout.syncpt_id),
+		cdma->timeout.ctx,
+		syncpt_val, cdma->timeout.syncpt_val);
+
+	/* stop HW, resetting channel/module */
+	cdma_op().timeout_teardown_begin(cdma);
+
+	nvhost_cdma_update_sync_queue(cdma, sp, ch->dev);
+	mutex_unlock(&cdma->lock);
+}
+
+static const struct nvhost_cdma_ops host1x_cdma_ops = {
+	.start = cdma_start,
+	.stop = cdma_stop,
+	.kick = cdma_kick,
+
+	.timeout_init = cdma_timeout_init,
+	.timeout_destroy = cdma_timeout_destroy,
+	.timeout_teardown_begin = cdma_timeout_teardown_begin,
+	.timeout_teardown_end = cdma_timeout_teardown_end,
+	.timeout_cpu_incr = cdma_timeout_cpu_incr,
+};
+
+static const struct nvhost_pushbuffer_ops host1x_pushbuffer_ops = {
+	.reset = push_buffer_reset,
+	.init = push_buffer_init,
+	.destroy = push_buffer_destroy,
+	.push_to = push_buffer_push_to,
+	.pop_from = push_buffer_pop_from,
+	.space = push_buffer_space,
+	.putptr = push_buffer_putptr,
+};
+
diff --git a/drivers/video/tegra/host/host1x/host1x_cdma.h b/drivers/video/tegra/host/host1x/host1x_cdma.h
new file mode 100644
index 000000000000..94bfc092c8c9
--- /dev/null
+++ b/drivers/video/tegra/host/host1x/host1x_cdma.h
@@ -0,0 +1,39 @@
+/*
+ * drivers/video/tegra/host/host1x/host1x_cdma.h
+ *
+ * Tegra Graphics Host Command DMA
+ *
+ * Copyright (c) 2011-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_HOST1X_HOST1X_CDMA_H
+#define __NVHOST_HOST1X_HOST1X_CDMA_H
+
+/* Size of the sync queue. If it is too small, we won't be able to queue up
+ * many command buffers. If it is too large, we waste memory. */
+#define NVHOST_SYNC_QUEUE_SIZE 512
+
+/* Number of gathers we allow to be queued up per channel. Must be a
+ * power of two. Currently sized such that pushbuffer is 4KB (512*8B). */
+#define NVHOST_GATHER_QUEUE_SIZE 512
+
+/* 8 bytes per slot. (This number does not include the final RESTART.) */
+#define PUSH_BUFFER_SIZE (NVHOST_GATHER_QUEUE_SIZE * 8)
+
+/* 4K page containing GATHERed methods to increment channel syncpts
+ * and replaces the original timed out contexts GATHER slots */
+#define SYNCPT_INCR_BUFFER_SIZE_WORDS   (4096 / sizeof(u32))
+
+#endif
diff --git a/drivers/video/tegra/host/host1x/host1x_channel.c b/drivers/video/tegra/host/host1x/host1x_channel.c
new file mode 100644
index 000000000000..0274413ff698
--- /dev/null
+++ b/drivers/video/tegra/host/host1x/host1x_channel.c
@@ -0,0 +1,681 @@
+/*
+ * drivers/video/tegra/host/host1x/channel_host1x.c
+ *
+ * Tegra Graphics Host Channel
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "nvhost_channel.h"
+#include "dev.h"
+#include "nvhost_acm.h"
+#include "nvhost_job.h"
+#include "nvhost_hwctx.h"
+#include <trace/events/nvhost.h>
+#include <linux/slab.h>
+
+#include "host1x_hwctx.h"
+#include "nvhost_intr.h"
+
+#define NV_FIFO_READ_TIMEOUT 200000
+
+static int host1x_drain_read_fifo(struct nvhost_channel *ch,
+	u32 *ptr, unsigned int count, unsigned int *pending);
+
+static void sync_waitbases(struct nvhost_channel *ch, u32 syncpt_val)
+{
+	unsigned long waitbase;
+	unsigned long int waitbase_mask = ch->dev->waitbases;
+	if (ch->dev->waitbasesync) {
+		waitbase = find_first_bit(&waitbase_mask, BITS_PER_LONG);
+		nvhost_cdma_push(&ch->cdma,
+			nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+				host1x_uclass_load_syncpt_base_r(),
+				1),
+				nvhost_class_host_load_syncpt_base(waitbase,
+						syncpt_val));
+	}
+}
+
+static void *pre_submit_ctxsave(struct nvhost_job *job,
+		struct nvhost_hwctx *cur_ctx)
+{
+	struct nvhost_channel *ch = job->ch;
+	void *ctxsave_waiter = NULL;
+
+	/* Is a save needed? */
+	if (!cur_ctx || ch->cur_ctx == job->hwctx)
+		return NULL;
+
+	if (cur_ctx->has_timedout) {
+		dev_dbg(&ch->dev->dev,
+			"%s: skip save of timed out context (0x%p)\n",
+			__func__, ch->cur_ctx);
+
+		return NULL;
+	}
+
+	/* Allocate save waiter if needed */
+	if (ch->ctxhandler->save_service) {
+		ctxsave_waiter = nvhost_intr_alloc_waiter();
+		if (!ctxsave_waiter)
+			return ERR_PTR(-ENOMEM);
+	}
+
+	return ctxsave_waiter;
+}
+
+static void submit_ctxsave(struct nvhost_job *job, void *ctxsave_waiter,
+		struct nvhost_hwctx *cur_ctx)
+{
+	struct nvhost_master *host = nvhost_get_host(job->ch->dev);
+	struct nvhost_channel *ch = job->ch;
+	u32 syncval;
+	int err;
+	u32 save_thresh = 0;
+
+	/* Is a save needed? */
+	if (!cur_ctx || cur_ctx == job->hwctx || cur_ctx->has_timedout)
+		return;
+
+	/* Retrieve save threshold if we have a waiter */
+	if (ctxsave_waiter)
+		save_thresh =
+			nvhost_syncpt_read_max(&host->syncpt, job->syncpt_id)
+			+ to_host1x_hwctx(cur_ctx)->save_thresh;
+
+	/* Adjust the syncpoint max */
+	job->syncpt_incrs += to_host1x_hwctx(cur_ctx)->save_incrs;
+	syncval = nvhost_syncpt_incr_max(&host->syncpt,
+			job->syncpt_id,
+			to_host1x_hwctx(cur_ctx)->save_incrs);
+
+	/* Send the save to channel */
+	cur_ctx->valid = true;
+	ch->ctxhandler->save_push(cur_ctx, &ch->cdma);
+	nvhost_job_get_hwctx(job, cur_ctx);
+
+	/* Notify save service */
+	if (ctxsave_waiter) {
+		err = nvhost_intr_add_action(&host->intr,
+			job->syncpt_id,
+			save_thresh,
+			NVHOST_INTR_ACTION_CTXSAVE, cur_ctx,
+			ctxsave_waiter,
+			NULL);
+		ctxsave_waiter = NULL;
+		WARN(err, "Failed to set ctx save interrupt");
+	}
+
+	trace_nvhost_channel_context_save(ch->dev->name, cur_ctx);
+}
+
+static void submit_ctxrestore(struct nvhost_job *job)
+{
+	struct nvhost_master *host = nvhost_get_host(job->ch->dev);
+	struct nvhost_channel *ch = job->ch;
+	u32 syncval;
+	struct host1x_hwctx *ctx =
+		job->hwctx ? to_host1x_hwctx(job->hwctx) : NULL;
+
+	/* First check if we have a valid context to restore */
+	if(ch->cur_ctx == job->hwctx || !job->hwctx || !job->hwctx->valid)
+		return;
+
+	/* Increment syncpt max */
+	job->syncpt_incrs += ctx->restore_incrs;
+	syncval = nvhost_syncpt_incr_max(&host->syncpt,
+			job->syncpt_id,
+			ctx->restore_incrs);
+
+	/* Send restore buffer to channel */
+	nvhost_cdma_push_gather(&ch->cdma,
+		host->memmgr,
+		ctx->restore,
+		0,
+		nvhost_opcode_gather(ctx->restore_size),
+		ctx->restore_phys);
+
+	trace_nvhost_channel_context_restore(ch->dev->name, &ctx->hwctx);
+}
+
+static void submit_nullkickoff(struct nvhost_job *job, int user_syncpt_incrs)
+{
+	struct nvhost_channel *ch = job->ch;
+	int incr;
+	u32 op_incr;
+
+	/* push increments that correspond to nulled out commands */
+	op_incr = nvhost_opcode_imm_incr_syncpt(
+			host1x_uclass_incr_syncpt_cond_op_done_v(),
+			job->syncpt_id);
+	for (incr = 0; incr < (user_syncpt_incrs >> 1); incr++)
+		nvhost_cdma_push(&ch->cdma, op_incr, op_incr);
+	if (user_syncpt_incrs & 1)
+		nvhost_cdma_push(&ch->cdma, op_incr, NVHOST_OPCODE_NOOP);
+
+	/* for 3d, waitbase needs to be incremented after each submit */
+	if (ch->dev->class == NV_GRAPHICS_3D_CLASS_ID) {
+		u32 waitbase = to_host1x_hwctx_handler(job->hwctx->h)->waitbase;
+		nvhost_cdma_push(&ch->cdma,
+			nvhost_opcode_setclass(
+				NV_HOST1X_CLASS_ID,
+				host1x_uclass_incr_syncpt_base_r(),
+				1),
+			nvhost_class_host_incr_syncpt_base(
+				waitbase,
+				user_syncpt_incrs));
+	}
+}
+
+static void submit_gathers(struct nvhost_job *job)
+{
+	/* push user gathers */
+	int i;
+	for (i = 0 ; i < job->num_gathers; i++) {
+		u32 op1 = nvhost_opcode_gather(job->gathers[i].words);
+		u32 op2 = job->gathers[i].mem;
+		nvhost_cdma_push_gather(&job->ch->cdma,
+				job->memmgr,
+				job->gathers[i].ref,
+				job->gathers[i].offset,
+				op1, op2);
+	}
+}
+
+static int host1x_channel_submit(struct nvhost_job *job)
+{
+	struct nvhost_channel *ch = job->ch;
+	struct nvhost_syncpt *sp = &nvhost_get_host(job->ch->dev)->syncpt;
+	u32 user_syncpt_incrs = job->syncpt_incrs;
+	u32 prev_max = 0;
+	u32 syncval;
+	int err;
+	void *completed_waiter = NULL, *ctxsave_waiter = NULL;
+	struct nvhost_driver *drv = to_nvhost_driver(ch->dev->dev.driver);
+
+	/* Bail out on timed out contexts */
+	if (job->hwctx && job->hwctx->has_timedout)
+		return -ETIMEDOUT;
+
+	/* Turn on the client module and host1x */
+	nvhost_module_busy(ch->dev);
+	if (drv->busy)
+		drv->busy(ch->dev);
+
+	/* before error checks, return current max */
+	prev_max = job->syncpt_end =
+		nvhost_syncpt_read_max(sp, job->syncpt_id);
+
+	/* get submit lock */
+	err = mutex_lock_interruptible(&ch->submitlock);
+	if (err) {
+		nvhost_module_idle(ch->dev);
+		goto error;
+	}
+
+	/* Do the needed allocations */
+	ctxsave_waiter = pre_submit_ctxsave(job, ch->cur_ctx);
+	if (IS_ERR(ctxsave_waiter)) {
+		err = PTR_ERR(ctxsave_waiter);
+		nvhost_module_idle(ch->dev);
+		mutex_unlock(&ch->submitlock);
+		goto error;
+	}
+
+	completed_waiter = nvhost_intr_alloc_waiter();
+	if (!completed_waiter) {
+		nvhost_module_idle(ch->dev);
+		mutex_unlock(&ch->submitlock);
+		err = -ENOMEM;
+		goto error;
+	}
+
+	/* begin a CDMA submit */
+	err = nvhost_cdma_begin(&ch->cdma, job);
+	if (err) {
+		mutex_unlock(&ch->submitlock);
+		nvhost_module_idle(ch->dev);
+		goto error;
+	}
+
+	if (ch->dev->serialize) {
+		/* Force serialization by inserting a host wait for the
+		 * previous job to finish before this one can commence. */
+		nvhost_cdma_push(&ch->cdma,
+				nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					host1x_uclass_wait_syncpt_r(),
+					1),
+				nvhost_class_host_wait_syncpt(job->syncpt_id,
+					nvhost_syncpt_read_max(sp,
+						job->syncpt_id)));
+	}
+
+	submit_ctxsave(job, ctxsave_waiter, ch->cur_ctx);
+	submit_ctxrestore(job);
+	ch->cur_ctx = job->hwctx;
+
+	syncval = nvhost_syncpt_incr_max(sp,
+			job->syncpt_id, user_syncpt_incrs);
+
+	job->syncpt_end = syncval;
+
+	/* add a setclass for modules that require it */
+	if (ch->dev->class)
+		nvhost_cdma_push(&ch->cdma,
+			nvhost_opcode_setclass(ch->dev->class, 0, 0),
+			NVHOST_OPCODE_NOOP);
+
+	if (job->null_kickoff)
+		submit_nullkickoff(job, user_syncpt_incrs);
+	else
+		submit_gathers(job);
+
+	sync_waitbases(ch, job->syncpt_end);
+
+	/* end CDMA submit & stash pinned hMems into sync queue */
+	nvhost_cdma_end(&ch->cdma, job);
+
+	trace_nvhost_channel_submitted(ch->dev->name,
+			prev_max, syncval);
+
+	/* schedule a submit complete interrupt */
+	err = nvhost_intr_add_action(&nvhost_get_host(ch->dev)->intr,
+			job->syncpt_id, syncval,
+			NVHOST_INTR_ACTION_SUBMIT_COMPLETE, ch,
+			completed_waiter,
+			NULL);
+	completed_waiter = NULL;
+	WARN(err, "Failed to set submit complete interrupt");
+
+	mutex_unlock(&ch->submitlock);
+
+	return 0;
+
+error:
+	kfree(ctxsave_waiter);
+	kfree(completed_waiter);
+	return err;
+}
+
+static int host1x_channel_read_3d_reg(
+	struct nvhost_channel *channel,
+	struct nvhost_hwctx *hwctx,
+	u32 offset,
+	u32 *value)
+{
+	struct host1x_hwctx *hwctx_to_save = NULL;
+	struct nvhost_hwctx_handler *h = hwctx->h;
+	struct host1x_hwctx_handler *p = to_host1x_hwctx_handler(h);
+	bool need_restore = false;
+	u32 syncpt_incrs = 4;
+	unsigned int pending = 0;
+	DECLARE_WAIT_QUEUE_HEAD_ONSTACK(wq);
+	void *ref;
+	void *ctx_waiter, *read_waiter, *completed_waiter;
+	struct nvhost_job *job;
+	u32 syncval;
+	int err;
+
+	if (hwctx && hwctx->has_timedout)
+		return -ETIMEDOUT;
+
+	ctx_waiter = nvhost_intr_alloc_waiter();
+	read_waiter = nvhost_intr_alloc_waiter();
+	completed_waiter = nvhost_intr_alloc_waiter();
+	if (!ctx_waiter || !read_waiter || !completed_waiter) {
+		err = -ENOMEM;
+		goto done;
+	}
+
+	job = nvhost_job_alloc(channel, hwctx,
+			NULL,
+			nvhost_get_host(channel->dev)->memmgr, 0, 0);
+	if (!job) {
+		err = -ENOMEM;
+		goto done;
+	}
+
+	/* keep module powered */
+	nvhost_module_busy(channel->dev);
+
+	/* get submit lock */
+	err = mutex_lock_interruptible(&channel->submitlock);
+	if (err) {
+		nvhost_module_idle(channel->dev);
+		return err;
+	}
+
+	/* context switch */
+	if (channel->cur_ctx != hwctx) {
+		hwctx_to_save = channel->cur_ctx ?
+			to_host1x_hwctx(channel->cur_ctx) : NULL;
+		if (hwctx_to_save) {
+			syncpt_incrs += hwctx_to_save->save_incrs;
+			hwctx_to_save->hwctx.valid = true;
+			nvhost_job_get_hwctx(job, &hwctx_to_save->hwctx);
+		}
+		channel->cur_ctx = hwctx;
+		if (channel->cur_ctx && channel->cur_ctx->valid) {
+			need_restore = true;
+			syncpt_incrs += to_host1x_hwctx(channel->cur_ctx)
+				->restore_incrs;
+		}
+	}
+
+	syncval = nvhost_syncpt_incr_max(&nvhost_get_host(channel->dev)->syncpt,
+		p->syncpt, syncpt_incrs);
+
+	job->syncpt_id = p->syncpt;
+	job->syncpt_incrs = syncpt_incrs;
+	job->syncpt_end = syncval;
+
+	/* begin a CDMA submit */
+	nvhost_cdma_begin(&channel->cdma, job);
+
+	/* push save buffer (pre-gather setup depends on unit) */
+	if (hwctx_to_save)
+		h->save_push(&hwctx_to_save->hwctx, &channel->cdma);
+
+	/* gather restore buffer */
+	if (need_restore)
+		nvhost_cdma_push(&channel->cdma,
+			nvhost_opcode_gather(to_host1x_hwctx(channel->cur_ctx)
+				->restore_size),
+			to_host1x_hwctx(channel->cur_ctx)->restore_phys);
+
+	/* Switch to 3D - wait for it to complete what it was doing */
+	nvhost_cdma_push(&channel->cdma,
+		nvhost_opcode_setclass(NV_GRAPHICS_3D_CLASS_ID, 0, 0),
+		nvhost_opcode_imm_incr_syncpt(
+			host1x_uclass_incr_syncpt_cond_op_done_v(),
+			p->syncpt));
+	nvhost_cdma_push(&channel->cdma,
+		nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+			host1x_uclass_wait_syncpt_base_r(), 1),
+		nvhost_class_host_wait_syncpt_base(p->syncpt,
+			p->waitbase, 1));
+	/*  Tell 3D to send register value to FIFO */
+	nvhost_cdma_push(&channel->cdma,
+		nvhost_opcode_nonincr(host1x_uclass_indoff_r(), 1),
+		nvhost_class_host_indoff_reg_read(NV_HOST_MODULE_GR3D,
+			offset, false));
+	nvhost_cdma_push(&channel->cdma,
+		nvhost_opcode_imm(host1x_uclass_inddata_r(), 0),
+		NVHOST_OPCODE_NOOP);
+	/*  Increment syncpt to indicate that FIFO can be read */
+	nvhost_cdma_push(&channel->cdma,
+		nvhost_opcode_imm_incr_syncpt(
+			host1x_uclass_incr_syncpt_cond_immediate_v(),
+			p->syncpt),
+		NVHOST_OPCODE_NOOP);
+	/*  Wait for value to be read from FIFO */
+	nvhost_cdma_push(&channel->cdma,
+		nvhost_opcode_nonincr(host1x_uclass_wait_syncpt_base_r(), 1),
+		nvhost_class_host_wait_syncpt_base(p->syncpt,
+			p->waitbase, 3));
+	/*  Indicate submit complete */
+	nvhost_cdma_push(&channel->cdma,
+		nvhost_opcode_nonincr(host1x_uclass_incr_syncpt_base_r(), 1),
+		nvhost_class_host_incr_syncpt_base(p->waitbase, 4));
+	nvhost_cdma_push(&channel->cdma,
+		NVHOST_OPCODE_NOOP,
+		nvhost_opcode_imm_incr_syncpt(
+			host1x_uclass_incr_syncpt_cond_immediate_v(),
+			p->syncpt));
+
+	/* end CDMA submit  */
+	nvhost_cdma_end(&channel->cdma, job);
+	nvhost_job_put(job);
+	job = NULL;
+
+	/*
+	 * schedule a context save interrupt (to drain the host FIFO
+	 * if necessary, and to release the restore buffer)
+	 */
+	if (hwctx_to_save) {
+		err = nvhost_intr_add_action(
+			&nvhost_get_host(channel->dev)->intr,
+			p->syncpt,
+			syncval - syncpt_incrs
+				+ hwctx_to_save->save_incrs
+				- 1,
+			NVHOST_INTR_ACTION_CTXSAVE, hwctx_to_save,
+			ctx_waiter,
+			NULL);
+		ctx_waiter = NULL;
+		WARN(err, "Failed to set context save interrupt");
+	}
+
+	/* Wait for FIFO to be ready */
+	err = nvhost_intr_add_action(&nvhost_get_host(channel->dev)->intr,
+			p->syncpt, syncval - 2,
+			NVHOST_INTR_ACTION_WAKEUP, &wq,
+			read_waiter,
+			&ref);
+	read_waiter = NULL;
+	WARN(err, "Failed to set wakeup interrupt");
+	wait_event(wq,
+		nvhost_syncpt_is_expired(&nvhost_get_host(channel->dev)->syncpt,
+				p->syncpt, syncval - 2));
+	nvhost_intr_put_ref(&nvhost_get_host(channel->dev)->intr, p->syncpt,
+			ref);
+
+	/* Read the register value from FIFO */
+	err = host1x_drain_read_fifo(channel, value, 1, &pending);
+
+	/* Indicate we've read the value */
+	nvhost_syncpt_cpu_incr(&nvhost_get_host(channel->dev)->syncpt,
+			p->syncpt);
+
+	/* Schedule a submit complete interrupt */
+	err = nvhost_intr_add_action(&nvhost_get_host(channel->dev)->intr,
+			p->syncpt, syncval,
+			NVHOST_INTR_ACTION_SUBMIT_COMPLETE, channel,
+			completed_waiter, NULL);
+	completed_waiter = NULL;
+	WARN(err, "Failed to set submit complete interrupt");
+
+	mutex_unlock(&channel->submitlock);
+
+done:
+	kfree(ctx_waiter);
+	kfree(read_waiter);
+	kfree(completed_waiter);
+	return err;
+}
+
+
+static int host1x_drain_read_fifo(struct nvhost_channel *ch,
+	u32 *ptr, unsigned int count, unsigned int *pending)
+{
+	unsigned int entries = *pending;
+	unsigned long timeout = jiffies + NV_FIFO_READ_TIMEOUT;
+	void __iomem *chan_regs = ch->aperture;
+	while (count) {
+		unsigned int num;
+
+		while (!entries && time_before(jiffies, timeout)) {
+			/* query host for number of entries in fifo */
+			entries = host1x_channel_fifostat_outfentries_v(
+				readl(chan_regs + host1x_channel_fifostat_r()));
+			if (!entries)
+				cpu_relax();
+		}
+
+		/*  timeout -> return error */
+		if (!entries)
+			return -EIO;
+
+		num = min(entries, count);
+		entries -= num;
+		count -= num;
+
+		while (num & ~0x3) {
+			u32 arr[4];
+			arr[0] = readl(chan_regs + host1x_channel_inddata_r());
+			arr[1] = readl(chan_regs + host1x_channel_inddata_r());
+			arr[2] = readl(chan_regs + host1x_channel_inddata_r());
+			arr[3] = readl(chan_regs + host1x_channel_inddata_r());
+			memcpy(ptr, arr, 4*sizeof(u32));
+			ptr += 4;
+			num -= 4;
+		}
+		while (num--)
+			*ptr++ = readl(chan_regs + host1x_channel_inddata_r());
+	}
+	*pending = entries;
+
+	return 0;
+}
+
+static int host1x_save_context(struct nvhost_channel *ch)
+{
+	struct nvhost_device *dev = ch->dev;
+	struct nvhost_hwctx *hwctx_to_save;
+	DECLARE_WAIT_QUEUE_HEAD_ONSTACK(wq);
+	u32 syncpt_incrs, syncpt_val;
+	int err = 0;
+	void *ref;
+	void *ctx_waiter = NULL, *wakeup_waiter = NULL;
+	struct nvhost_job *job;
+	struct nvhost_driver *drv = to_nvhost_driver(dev->dev.driver);
+	u32 syncpt_id;
+
+	ctx_waiter = nvhost_intr_alloc_waiter();
+	wakeup_waiter = nvhost_intr_alloc_waiter();
+	if (!ctx_waiter || !wakeup_waiter) {
+		err = -ENOMEM;
+		goto done;
+	}
+
+	if (drv->busy)
+		drv->busy(dev);
+
+	mutex_lock(&ch->submitlock);
+	hwctx_to_save = ch->cur_ctx;
+	if (!hwctx_to_save) {
+		mutex_unlock(&ch->submitlock);
+		goto done;
+	}
+
+	job = nvhost_job_alloc(ch, hwctx_to_save,
+			NULL,
+			nvhost_get_host(ch->dev)->memmgr, 0, 0);
+	if (IS_ERR_OR_NULL(job)) {
+		err = PTR_ERR(job);
+		mutex_unlock(&ch->submitlock);
+		goto done;
+	}
+
+	hwctx_to_save->valid = true;
+	ch->cur_ctx = NULL;
+	syncpt_id = to_host1x_hwctx_handler(hwctx_to_save->h)->syncpt;
+
+	syncpt_incrs = to_host1x_hwctx(hwctx_to_save)->save_incrs;
+	syncpt_val = nvhost_syncpt_incr_max(&nvhost_get_host(ch->dev)->syncpt,
+					syncpt_id, syncpt_incrs);
+
+	job->syncpt_id = syncpt_id;
+	job->syncpt_incrs = syncpt_incrs;
+	job->syncpt_end = syncpt_val;
+
+	err = nvhost_cdma_begin(&ch->cdma, job);
+	if (err) {
+		mutex_unlock(&ch->submitlock);
+		goto done;
+	}
+
+	ch->ctxhandler->save_push(hwctx_to_save, &ch->cdma);
+	nvhost_cdma_end(&ch->cdma, job);
+	nvhost_job_put(job);
+	job = NULL;
+
+	err = nvhost_intr_add_action(&nvhost_get_host(ch->dev)->intr, syncpt_id,
+			syncpt_val - syncpt_incrs +
+				to_host1x_hwctx(hwctx_to_save)->save_thresh,
+			NVHOST_INTR_ACTION_CTXSAVE, hwctx_to_save,
+			ctx_waiter,
+			NULL);
+	ctx_waiter = NULL;
+	WARN(err, "Failed to set context save interrupt");
+
+	err = nvhost_intr_add_action(&nvhost_get_host(ch->dev)->intr,
+			syncpt_id, syncpt_val,
+			NVHOST_INTR_ACTION_WAKEUP, &wq,
+			wakeup_waiter,
+			&ref);
+	wakeup_waiter = NULL;
+	WARN(err, "Failed to set wakeup interrupt");
+	wait_event(wq,
+		nvhost_syncpt_is_expired(&nvhost_get_host(ch->dev)->syncpt,
+				syncpt_id, syncpt_val));
+
+	nvhost_intr_put_ref(&nvhost_get_host(ch->dev)->intr, syncpt_id, ref);
+
+	nvhost_cdma_update(&ch->cdma);
+
+	mutex_unlock(&ch->submitlock);
+
+done:
+	kfree(ctx_waiter);
+	kfree(wakeup_waiter);
+	return err;
+}
+
+static inline void __iomem *host1x_channel_aperture(void __iomem *p, int ndx)
+{
+	p += ndx * NV_HOST1X_CHANNEL_MAP_SIZE_BYTES;
+	return p;
+}
+
+static inline int host1x_hwctx_handler_init(struct nvhost_channel *ch)
+{
+	int err = 0;
+	unsigned long syncpts = ch->dev->syncpts;
+	unsigned long waitbases = ch->dev->waitbases;
+	u32 syncpt = find_first_bit(&syncpts, BITS_PER_LONG);
+	u32 waitbase = find_first_bit(&waitbases, BITS_PER_LONG);
+	struct nvhost_driver *drv = to_nvhost_driver(ch->dev->dev.driver);
+
+	if (drv->alloc_hwctx_handler) {
+		ch->ctxhandler = drv->alloc_hwctx_handler(syncpt,
+				waitbase, ch);
+		if (!ch->ctxhandler)
+			err = -ENOMEM;
+	}
+
+	return err;
+}
+
+static int host1x_channel_init(struct nvhost_channel *ch,
+	struct nvhost_master *dev, int index)
+{
+	ch->chid = index;
+	mutex_init(&ch->reflock);
+	mutex_init(&ch->submitlock);
+
+	ch->aperture = host1x_channel_aperture(dev->aperture, index);
+
+	return host1x_hwctx_handler_init(ch);
+}
+
+static const struct nvhost_channel_ops host1x_channel_ops = {
+	.init = host1x_channel_init,
+	.submit = host1x_channel_submit,
+	.read3dreg = host1x_channel_read_3d_reg,
+	.save_context = host1x_save_context,
+	.drain_read_fifo = host1x_drain_read_fifo,
+};
diff --git a/drivers/video/tegra/host/host1x/host1x_debug.c b/drivers/video/tegra/host/host1x/host1x_debug.c
new file mode 100644
index 000000000000..1c4ed684dd84
--- /dev/null
+++ b/drivers/video/tegra/host/host1x/host1x_debug.c
@@ -0,0 +1,405 @@
+/*
+ * drivers/video/tegra/host/host1x/host1x_debug.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ *
+ * Copyright (C) 2011 NVIDIA Corporation
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/mm.h>
+
+#include <linux/io.h>
+
+#include "dev.h"
+#include "debug.h"
+#include "nvhost_cdma.h"
+#include "nvhost_channel.h"
+#include "nvhost_job.h"
+#include "chip_support.h"
+#include "nvhost_memmgr.h"
+
+#define NVHOST_DEBUG_MAX_PAGE_OFFSET 102400
+
+enum {
+	NVHOST_DBG_STATE_CMD = 0,
+	NVHOST_DBG_STATE_DATA = 1,
+	NVHOST_DBG_STATE_GATHER = 2
+};
+
+static int show_channel_command(struct output *o, u32 addr, u32 val, int *count)
+{
+	unsigned mask;
+	unsigned subop;
+
+	switch (val >> 28) {
+	case 0x0:
+		mask = val & 0x3f;
+		if (mask) {
+			nvhost_debug_output(o,
+				"SETCL(class=%03x, offset=%03x, mask=%02x, [",
+				val >> 6 & 0x3ff, val >> 16 & 0xfff, mask);
+			*count = hweight8(mask);
+			return NVHOST_DBG_STATE_DATA;
+		} else {
+			nvhost_debug_output(o, "SETCL(class=%03x)\n",
+				val >> 6 & 0x3ff);
+			return NVHOST_DBG_STATE_CMD;
+		}
+
+	case 0x1:
+		nvhost_debug_output(o, "INCR(offset=%03x, [",
+			val >> 16 & 0xfff);
+		*count = val & 0xffff;
+		return NVHOST_DBG_STATE_DATA;
+
+	case 0x2:
+		nvhost_debug_output(o, "NONINCR(offset=%03x, [",
+			val >> 16 & 0xfff);
+		*count = val & 0xffff;
+		return NVHOST_DBG_STATE_DATA;
+
+	case 0x3:
+		mask = val & 0xffff;
+		nvhost_debug_output(o, "MASK(offset=%03x, mask=%03x, [",
+			   val >> 16 & 0xfff, mask);
+		*count = hweight16(mask);
+		return NVHOST_DBG_STATE_DATA;
+
+	case 0x4:
+		nvhost_debug_output(o, "IMM(offset=%03x, data=%03x)\n",
+			   val >> 16 & 0xfff, val & 0xffff);
+		return NVHOST_DBG_STATE_CMD;
+
+	case 0x5:
+		nvhost_debug_output(o, "RESTART(offset=%08x)\n", val << 4);
+		return NVHOST_DBG_STATE_CMD;
+
+	case 0x6:
+		nvhost_debug_output(o, "GATHER(offset=%03x, insert=%d, type=%d, count=%04x, addr=[",
+			val >> 16 & 0xfff, val >> 15 & 0x1, val >> 14 & 0x1,
+			val & 0x3fff);
+		*count = val & 0x3fff; /* TODO: insert */
+		return NVHOST_DBG_STATE_GATHER;
+
+	case 0xe:
+		subop = val >> 24 & 0xf;
+		if (subop == 0)
+			nvhost_debug_output(o, "ACQUIRE_MLOCK(index=%d)\n",
+				val & 0xff);
+		else if (subop == 1)
+			nvhost_debug_output(o, "RELEASE_MLOCK(index=%d)\n",
+				val & 0xff);
+		else
+			nvhost_debug_output(o, "EXTEND_UNKNOWN(%08x)\n", val);
+		return NVHOST_DBG_STATE_CMD;
+
+	default:
+		return NVHOST_DBG_STATE_CMD;
+	}
+}
+
+static void show_channel_gather(struct output *o, u32 addr,
+		phys_addr_t phys_addr, u32 words, struct nvhost_cdma *cdma);
+
+static void show_channel_word(struct output *o, int *state, int *count,
+		u32 addr, u32 val, struct nvhost_cdma *cdma)
+{
+	static int start_count, dont_print;
+
+	switch (*state) {
+	case NVHOST_DBG_STATE_CMD:
+		if (addr)
+			nvhost_debug_output(o, "%08x: %08x:", addr, val);
+		else
+			nvhost_debug_output(o, "%08x:", val);
+
+		*state = show_channel_command(o, addr, val, count);
+		dont_print = 0;
+		start_count = *count;
+		if (*state == NVHOST_DBG_STATE_DATA && *count == 0) {
+			*state = NVHOST_DBG_STATE_CMD;
+			nvhost_debug_output(o, "])\n");
+		}
+		break;
+
+	case NVHOST_DBG_STATE_DATA:
+		(*count)--;
+		if (start_count - *count < 64)
+			nvhost_debug_output(o, "%08x%s",
+				val, *count > 0 ? ", " : "])\n");
+		else if (!dont_print && (*count > 0)) {
+			nvhost_debug_output(o, "[truncated; %d more words]\n",
+				*count);
+			dont_print = 1;
+		}
+		if (*count == 0)
+			*state = NVHOST_DBG_STATE_CMD;
+		break;
+
+	case NVHOST_DBG_STATE_GATHER:
+		*state = NVHOST_DBG_STATE_CMD;
+		nvhost_debug_output(o, "%08x]):\n", val);
+		if (cdma) {
+			show_channel_gather(o, addr, val,
+					*count, cdma);
+		}
+		break;
+	}
+}
+
+static void do_show_channel_gather(struct output *o,
+		phys_addr_t phys_addr,
+		u32 words, struct nvhost_cdma *cdma,
+		phys_addr_t pin_addr, u32 *map_addr)
+{
+	/* Map dmaget cursor to corresponding nvmap_handle */
+	u32 offset;
+	int state, count, i;
+
+	offset = phys_addr - pin_addr;
+	/*
+	 * Sometimes we're given different hardware address to the same
+	 * page - in these cases the offset will get an invalid number and
+	 * we just have to bail out.
+	 */
+	if (offset > NVHOST_DEBUG_MAX_PAGE_OFFSET) {
+		nvhost_debug_output(o, "[address mismatch]\n");
+	} else {
+		/* GATHER buffer starts always with commands */
+		state = NVHOST_DBG_STATE_CMD;
+		for (i = 0; i < words; i++)
+			show_channel_word(o, &state, &count,
+					phys_addr + i * 4,
+					*(map_addr + offset/4 + i),
+					cdma);
+	}
+}
+
+static void show_channel_gather(struct output *o, u32 addr,
+		phys_addr_t phys_addr,
+		u32 words, struct nvhost_cdma *cdma)
+{
+#if defined(CONFIG_TEGRA_NVMAP)
+	/* Map dmaget cursor to corresponding nvmap_handle */
+	struct push_buffer *pb = &cdma->push_buffer;
+	u32 cur = addr - pb->phys;
+	struct mem_mgr_handle *nvmap = &pb->client_handle[cur/8];
+	u32 *map_addr, offset;
+	phys_addr_t pin_addr;
+
+	if (!nvmap || !nvmap->handle || !nvmap->client) {
+		nvhost_debug_output(o, "[already deallocated]\n");
+		return;
+	}
+
+	map_addr = mem_op().mmap(nvmap->handle);
+	if (!map_addr) {
+		nvhost_debug_output(o, "[could not mmap]\n");
+		return;
+	}
+
+	/* Get base address from nvmap */
+	pin_addr = mem_op().pin(nvmap->client, nvmap->handle);
+	if (IS_ERR_VALUE(pin_addr)) {
+		nvhost_debug_output(o, "[couldn't pin]\n");
+		mem_op().munmap(nvmap->handle, map_addr);
+		return;
+	}
+
+	offset = phys_addr - pin_addr;
+	do_show_channel_gather(o, phys_addr, words, cdma,
+			pin_addr, map_addr);
+	mem_op().unpin(nvmap->client, nvmap->handle);
+	mem_op().munmap(nvmap->handle, map_addr);
+#endif
+}
+
+static void show_channel_gathers(struct output *o, struct nvhost_cdma *cdma)
+{
+	struct nvhost_job *job;
+
+	list_for_each_entry(job, &cdma->sync_queue, list) {
+		int i;
+		nvhost_debug_output(o, "\n%p: JOB, syncpt_id=%d, syncpt_val=%d,"
+				" first_get=%08x, timeout=%d, ctx=%p,"
+				" num_slots=%d, num_handles=%d\n",
+				job,
+				job->syncpt_id,
+				job->syncpt_end,
+				job->first_get,
+				job->timeout,
+				job->hwctx,
+				job->num_slots,
+				job->num_unpins);
+
+		for (i = 0; i < job->num_gathers; i++) {
+			struct nvhost_job_gather *g = &job->gathers[i];
+			u32 *mapped = mem_op().mmap(g->ref);
+			if (!mapped) {
+				nvhost_debug_output(o, "[could not mmap]\n");
+				continue;
+			}
+
+			nvhost_debug_output(o, "    GATHER at %08x, %d words\n",
+				g->mem, g->words);
+
+			do_show_channel_gather(o, g->mem + g->offset,
+					g->words, cdma, g->mem, mapped);
+			mem_op().munmap(g->ref, mapped);
+		}
+	}
+}
+
+static void t20_debug_show_channel_cdma(struct nvhost_master *m,
+	struct nvhost_channel *ch, struct output *o, int chid)
+{
+	struct nvhost_channel *channel = ch;
+	struct nvhost_cdma *cdma = &channel->cdma;
+	u32 dmaput, dmaget, dmactrl;
+	u32 cbstat, cbread;
+	u32 val, base, baseval;
+
+	dmaput = readl(channel->aperture + host1x_channel_dmaput_r());
+	dmaget = readl(channel->aperture + host1x_channel_dmaget_r());
+	dmactrl = readl(channel->aperture + host1x_channel_dmactrl_r());
+	cbread = readl(m->sync_aperture + host1x_sync_cbread0_r() + 4 * chid);
+	cbstat = readl(m->sync_aperture + host1x_sync_cbstat_0_r() + 4 * chid);
+
+	nvhost_debug_output(o, "%d-%s (%d): ", chid,
+			    channel->dev->name,
+			    channel->dev->refcount);
+
+	if (host1x_channel_dmactrl_dmastop_v(dmactrl)
+		|| !channel->cdma.push_buffer.mapped) {
+		nvhost_debug_output(o, "inactive\n\n");
+		return;
+	}
+
+	switch (cbstat) {
+	case 0x00010008:
+		nvhost_debug_output(o, "waiting on syncpt %d val %d\n",
+			cbread >> 24, cbread & 0xffffff);
+		break;
+
+	case 0x00010009:
+		base = (cbread >> 16) & 0xff;
+		baseval = readl(m->sync_aperture +
+				host1x_sync_syncpt_base_0_r() + 4 * base);
+		val = cbread & 0xffff;
+		nvhost_debug_output(o, "waiting on syncpt %d val %d "
+			  "(base %d = %d; offset = %d)\n",
+			cbread >> 24, baseval + val,
+			base, baseval, val);
+		break;
+
+	default:
+		nvhost_debug_output(o,
+				"active class %02x, offset %04x, val %08x\n",
+				host1x_sync_cbstat_0_cbclass0_v(cbstat),
+				host1x_sync_cbstat_0_cboffset0_v(cbstat),
+				cbread);
+		break;
+	}
+
+	nvhost_debug_output(o, "DMAPUT %08x, DMAGET %08x, DMACTL %08x\n",
+		dmaput, dmaget, dmactrl);
+	nvhost_debug_output(o, "CBREAD %08x, CBSTAT %08x\n", cbread, cbstat);
+
+	show_channel_gathers(o, cdma);
+	nvhost_debug_output(o, "\n");
+}
+
+static void t20_debug_show_channel_fifo(struct nvhost_master *m,
+	struct nvhost_channel *ch, struct output *o, int chid)
+{
+	u32 val, rd_ptr, wr_ptr, start, end;
+	struct nvhost_channel *channel = ch;
+	int state, count;
+
+	nvhost_debug_output(o, "%d: fifo:\n", chid);
+
+	val = readl(channel->aperture + host1x_channel_fifostat_r());
+	nvhost_debug_output(o, "FIFOSTAT %08x\n", val);
+	if (host1x_channel_fifostat_cfempty_v(val)) {
+		nvhost_debug_output(o, "[empty]\n");
+		return;
+	}
+
+	writel(0x0, m->sync_aperture + host1x_sync_cfpeek_ctrl_r());
+	writel(host1x_sync_cfpeek_ctrl_cfpeek_ena_f(1)
+			| host1x_sync_cfpeek_ctrl_cfpeek_channr_f(chid),
+		m->sync_aperture + host1x_sync_cfpeek_ctrl_r());
+
+	val = readl(m->sync_aperture + host1x_sync_cfpeek_ptrs_r());
+	rd_ptr = host1x_sync_cfpeek_ptrs_cf_rd_ptr_v(val);
+	wr_ptr = host1x_sync_cfpeek_ptrs_cf_wr_ptr_v(val);
+
+	val = readl(m->sync_aperture + host1x_sync_cf0_setup_r() + 4 * chid);
+	start = host1x_sync_cf0_setup_cf0_base_v(val);
+	end = host1x_sync_cf0_setup_cf0_limit_v(val);
+
+	state = NVHOST_DBG_STATE_CMD;
+
+	do {
+		writel(0x0, m->sync_aperture + host1x_sync_cfpeek_ctrl_r());
+		writel(host1x_sync_cfpeek_ctrl_cfpeek_ena_f(1)
+				| host1x_sync_cfpeek_ctrl_cfpeek_channr_f(chid)
+				| host1x_sync_cfpeek_ctrl_cfpeek_addr_f(rd_ptr),
+			m->sync_aperture + host1x_sync_cfpeek_ctrl_r());
+		val = readl(m->sync_aperture + host1x_sync_cfpeek_read_r());
+
+		show_channel_word(o, &state, &count, 0, val, NULL);
+
+		if (rd_ptr == end)
+			rd_ptr = start;
+		else
+			rd_ptr++;
+	} while (rd_ptr != wr_ptr);
+
+	if (state == NVHOST_DBG_STATE_DATA)
+		nvhost_debug_output(o, ", ...])\n");
+	nvhost_debug_output(o, "\n");
+
+	writel(0x0, m->sync_aperture + host1x_sync_cfpeek_ctrl_r());
+}
+
+static void t20_debug_show_mlocks(struct nvhost_master *m, struct output *o)
+{
+	u32 __iomem *mlo_regs = m->sync_aperture +
+		host1x_sync_mlock_owner_0_r();
+	int i;
+
+	nvhost_debug_output(o, "---- mlocks ----\n");
+	for (i = 0; i < NV_HOST1X_NB_MLOCKS; i++) {
+		u32 owner = readl(mlo_regs + i);
+		if (host1x_sync_mlock_owner_0_mlock_ch_owns_0_v(owner))
+			nvhost_debug_output(o, "%d: locked by channel %d\n",
+				i,
+				host1x_sync_mlock_owner_0_mlock_owner_chid_0_f(
+					owner));
+		else if (host1x_sync_mlock_owner_0_mlock_cpu_owns_0_v(owner))
+			nvhost_debug_output(o, "%d: locked by cpu\n", i);
+		else
+			nvhost_debug_output(o, "%d: unlocked\n", i);
+	}
+	nvhost_debug_output(o, "\n");
+}
+
+static const struct nvhost_debug_ops host1x_debug_ops = {
+	.show_channel_cdma = t20_debug_show_channel_cdma,
+	.show_channel_fifo = t20_debug_show_channel_fifo,
+	.show_mlocks = t20_debug_show_mlocks,
+};
diff --git a/drivers/video/tegra/host/host1x/host1x_hwctx.h b/drivers/video/tegra/host/host1x/host1x_hwctx.h
new file mode 100644
index 000000000000..13f0071d1e33
--- /dev/null
+++ b/drivers/video/tegra/host/host1x/host1x_hwctx.h
@@ -0,0 +1,66 @@
+/*
+ * drivers/video/tegra/host/host1x/host1x_hwctx.h
+ *
+ * Tegra Graphics Host HOST1X Hardware Context Interface
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __NVHOST_HOST1X_HWCTX_H
+#define __NVHOST_HOST1X_HWCTX_H
+
+#include <linux/kref.h>
+#include "nvhost_hwctx.h"
+
+struct nvhost_hwctx_handler;
+struct nvhost_channel;
+
+#define to_host1x_hwctx_handler(handler) \
+	container_of((handler), struct host1x_hwctx_handler, h)
+#define to_host1x_hwctx(h) container_of((h), struct host1x_hwctx, hwctx)
+#define host1x_hwctx_handler(_hwctx) to_host1x_hwctx_handler((_hwctx)->hwctx.h)
+
+struct host1x_hwctx {
+	struct nvhost_hwctx hwctx;
+
+	u32 save_incrs;
+	u32 save_thresh;
+	u32 save_slots;
+
+	struct mem_handle *restore;
+	u32 *restore_virt;
+	phys_addr_t restore_phys;
+	u32 restore_size;
+	u32 restore_incrs;
+};
+
+struct host1x_hwctx_handler {
+	struct nvhost_hwctx_handler h;
+
+	u32 syncpt;
+	u32 waitbase;
+	u32 restore_size;
+	u32 restore_incrs;
+	struct mem_handle *save_buf;
+	u32 save_incrs;
+	u32 save_thresh;
+	u32 save_slots;
+	phys_addr_t save_phys;
+	u32 save_size;
+};
+
+#endif
diff --git a/drivers/video/tegra/host/host1x/host1x_intr.c b/drivers/video/tegra/host/host1x/host1x_intr.c
new file mode 100644
index 000000000000..facb818a0c24
--- /dev/null
+++ b/drivers/video/tegra/host/host1x/host1x_intr.c
@@ -0,0 +1,294 @@
+/*
+ * drivers/video/tegra/host/host1x/host1x_intr.c
+ *
+ * Tegra Graphics Host Interrupt Management
+ *
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/io.h>
+#include <asm/mach/irq.h>
+
+#include "nvhost_intr.h"
+#include "dev.h"
+
+/* Spacing between sync registers */
+#define REGISTER_STRIDE 4
+
+/*** HW host sync management ***/
+
+static void syncpt_thresh_mask(struct irq_data *data)
+{
+	(void)data;
+}
+
+static void syncpt_thresh_unmask(struct irq_data *data)
+{
+	(void)data;
+}
+
+static void syncpt_thresh_cascade(unsigned int irq, struct irq_desc *desc)
+{
+	struct nvhost_master *dev = irq_desc_get_handler_data(desc);
+	void __iomem *sync_regs = dev->sync_aperture;
+	unsigned long reg;
+	int i, id;
+	struct irq_chip *chip = irq_desc_get_chip(desc);
+
+	chained_irq_enter(chip, desc);
+
+	for (i = 0; i < dev->info.nb_pts / BITS_PER_LONG; i++) {
+		reg = readl(sync_regs +
+				host1x_sync_syncpt_thresh_cpu0_int_status_r() +
+				i * REGISTER_STRIDE);
+		for_each_set_bit(id, &reg, BITS_PER_LONG)
+			generic_handle_irq(id +
+					dev->intr.host_syncpt_irq_base +
+					i * BITS_PER_LONG);
+	}
+
+	chained_irq_exit(chip, desc);
+}
+
+static struct irq_chip syncpt_thresh_irq = {
+	.name		= "syncpt",
+	.irq_mask	= syncpt_thresh_mask,
+	.irq_unmask	= syncpt_thresh_unmask
+};
+
+static void t20_intr_init_host_sync(struct nvhost_intr *intr)
+{
+	struct nvhost_master *dev = intr_to_dev(intr);
+	void __iomem *sync_regs = dev->sync_aperture;
+	int i, irq;
+
+	writel(0xffffffffUL,
+		sync_regs + host1x_sync_syncpt_thresh_int_disable_r());
+	writel(0xffffffffUL,
+		sync_regs + host1x_sync_syncpt_thresh_cpu0_int_status_r());
+
+	for (i = 0; i < dev->info.nb_pts; i++) {
+		irq = intr->host_syncpt_irq_base + i;
+		irq_set_chip_and_handler(irq, &syncpt_thresh_irq,
+			handle_simple_irq);
+		irq_set_chip_data(irq, sync_regs);
+		set_irq_flags(irq, IRQF_VALID);
+	}
+	irq_set_chained_handler(INT_HOST1X_MPCORE_SYNCPT,
+		syncpt_thresh_cascade);
+	irq_set_handler_data(INT_HOST1X_MPCORE_SYNCPT, dev);
+	/* disable the ip_busy_timeout. this prevents write drops, etc.
+	 * there's no real way to recover from a hung client anyway.
+	 */
+	writel(0, sync_regs + host1x_sync_ip_busy_timeout_r());
+
+	/* increase the auto-ack timout to the maximum value. 2d will hang
+	 * otherwise on ap20.
+	 */
+	writel(0xff, sync_regs + host1x_sync_ctxsw_timeout_cfg_r());
+}
+
+static void t20_intr_set_host_clocks_per_usec(struct nvhost_intr *intr, u32 cpm)
+{
+	struct nvhost_master *dev = intr_to_dev(intr);
+	void __iomem *sync_regs = dev->sync_aperture;
+	/* write microsecond clock register */
+	writel(cpm, sync_regs + host1x_sync_usec_clk_r());
+}
+
+static void t20_intr_set_syncpt_threshold(struct nvhost_intr *intr,
+	u32 id, u32 thresh)
+{
+	struct nvhost_master *dev = intr_to_dev(intr);
+	void __iomem *sync_regs = dev->sync_aperture;
+	thresh &= 0xffff;
+	writel(thresh, sync_regs +
+		(host1x_sync_syncpt_int_thresh_0_r() + id * REGISTER_STRIDE));
+}
+
+static void t20_intr_enable_syncpt_intr(struct nvhost_intr *intr, u32 id)
+{
+	struct nvhost_master *dev = intr_to_dev(intr);
+	void __iomem *sync_regs = dev->sync_aperture;
+
+	writel(BIT_MASK(id), sync_regs +
+			host1x_sync_syncpt_thresh_int_enable_cpu0_r() +
+			BIT_WORD(id) * REGISTER_STRIDE);
+}
+
+static void t20_intr_disable_syncpt_intr(struct nvhost_intr *intr, u32 id)
+{
+	struct nvhost_master *dev = intr_to_dev(intr);
+	void __iomem *sync_regs = dev->sync_aperture;
+
+	writel(BIT_MASK(id), sync_regs +
+			host1x_sync_syncpt_thresh_int_disable_r() +
+			BIT_WORD(id) * REGISTER_STRIDE);
+}
+
+static void t20_intr_disable_all_syncpt_intrs(struct nvhost_intr *intr)
+{
+	struct nvhost_master *dev = intr_to_dev(intr);
+	void __iomem *sync_regs = dev->sync_aperture;
+	u32 reg;
+
+	for (reg = 0; reg <= BIT_WORD(dev->info.nb_pts) * REGISTER_STRIDE;
+			reg += REGISTER_STRIDE) {
+		/* disable interrupts for both cpu's */
+		writel(0xffffffffu, sync_regs +
+				host1x_sync_syncpt_thresh_int_disable_r() +
+				reg);
+
+		/* clear status for both cpu's */
+		writel(0xffffffffu, sync_regs +
+			host1x_sync_syncpt_thresh_cpu0_int_status_r() + reg);
+		writel(0xffffffffu, sync_regs +
+			host1x_sync_syncpt_thresh_cpu1_int_status_r() + reg);
+	}
+}
+
+/**
+ * Sync point threshold interrupt service function
+ * Handles sync point threshold triggers, in interrupt context
+ */
+static irqreturn_t t20_intr_syncpt_thresh_isr(int irq, void *dev_id)
+{
+	struct nvhost_intr_syncpt *syncpt = dev_id;
+	unsigned int id = syncpt->id;
+	struct nvhost_intr *intr = intr_syncpt_to_intr(syncpt);
+
+	void __iomem *sync_regs = intr_to_dev(intr)->sync_aperture;
+
+	u32 reg = BIT_WORD(id) * REGISTER_STRIDE;
+
+	writel(BIT_MASK(id), sync_regs +
+		host1x_sync_syncpt_thresh_int_disable_r() + reg);
+	writel(BIT_MASK(id), sync_regs +
+		host1x_sync_syncpt_thresh_cpu0_int_status_r() + reg);
+
+	return IRQ_WAKE_THREAD;
+}
+
+/**
+ * Host general interrupt service function
+ * Handles read / write failures
+ */
+static irqreturn_t t20_intr_host1x_isr(int irq, void *dev_id)
+{
+	struct nvhost_intr *intr = dev_id;
+	void __iomem *sync_regs = intr_to_dev(intr)->sync_aperture;
+	u32 stat;
+	u32 ext_stat;
+	u32 addr;
+
+	stat = readl(sync_regs + host1x_sync_hintstatus_r());
+	ext_stat = readl(sync_regs + host1x_sync_hintstatus_ext_r());
+
+	if (host1x_sync_hintstatus_ext_ip_read_int_v(ext_stat)) {
+		addr = readl(sync_regs + host1x_sync_ip_read_timeout_addr_r());
+		pr_err("Host read timeout at address %x\n", addr);
+	}
+
+	if (host1x_sync_hintstatus_ext_ip_write_int_v(ext_stat)) {
+		addr = readl(sync_regs + host1x_sync_ip_write_timeout_addr_r());
+		pr_err("Host write timeout at address %x\n", addr);
+	}
+
+	writel(ext_stat, sync_regs + host1x_sync_hintstatus_ext_r());
+	writel(stat, sync_regs + host1x_sync_hintstatus_r());
+
+	return IRQ_HANDLED;
+}
+static int t20_intr_request_host_general_irq(struct nvhost_intr *intr)
+{
+	void __iomem *sync_regs = intr_to_dev(intr)->sync_aperture;
+	int err;
+
+	if (intr->host_general_irq_requested)
+		return 0;
+
+	/* master disable for general (not syncpt) host interrupts */
+	writel(0, sync_regs + host1x_sync_intmask_r());
+
+	/* clear status & extstatus */
+	writel(0xfffffffful, sync_regs + host1x_sync_hintstatus_ext_r());
+	writel(0xfffffffful, sync_regs + host1x_sync_hintstatus_r());
+
+	err = request_irq(intr->host_general_irq, t20_intr_host1x_isr, 0,
+			"host_status", intr);
+	if (err)
+		return err;
+
+	/* enable extra interrupt sources IP_READ_INT and IP_WRITE_INT */
+	writel(BIT(30) | BIT(31), sync_regs + host1x_sync_hintmask_ext_r());
+
+	/* enable extra interrupt sources */
+	writel(BIT(31), sync_regs + host1x_sync_hintmask_r());
+
+	/* enable host module interrupt to CPU0 */
+	writel(BIT(0), sync_regs + host1x_sync_intc0mask_r());
+
+	/* master enable for general (not syncpt) host interrupts */
+	writel(BIT(0), sync_regs + host1x_sync_intmask_r());
+
+	intr->host_general_irq_requested = true;
+
+	return err;
+}
+
+static void t20_intr_free_host_general_irq(struct nvhost_intr *intr)
+{
+	if (intr->host_general_irq_requested) {
+		void __iomem *sync_regs = intr_to_dev(intr)->sync_aperture;
+
+		/* master disable for general (not syncpt) host interrupts */
+		writel(0, sync_regs + host1x_sync_intmask_r());
+
+		free_irq(intr->host_general_irq, intr);
+		intr->host_general_irq_requested = false;
+	}
+}
+
+static int t20_request_syncpt_irq(struct nvhost_intr_syncpt *syncpt)
+{
+	int err;
+	if (syncpt->irq_requested)
+		return 0;
+
+	err = request_threaded_irq(syncpt->irq,
+				   t20_intr_syncpt_thresh_isr,
+				   nvhost_syncpt_thresh_fn,
+				   0, syncpt->thresh_irq_name, syncpt);
+	if (err)
+		return err;
+
+	syncpt->irq_requested = 1;
+	return 0;
+}
+
+static const struct nvhost_intr_ops host1x_intr_ops = {
+	.init_host_sync = t20_intr_init_host_sync,
+	.set_host_clocks_per_usec = t20_intr_set_host_clocks_per_usec,
+	.set_syncpt_threshold = t20_intr_set_syncpt_threshold,
+	.enable_syncpt_intr = t20_intr_enable_syncpt_intr,
+	.disable_syncpt_intr = t20_intr_disable_syncpt_intr,
+	.disable_all_syncpt_intrs = t20_intr_disable_all_syncpt_intrs,
+	.request_host_general_irq = t20_intr_request_host_general_irq,
+	.free_host_general_irq = t20_intr_free_host_general_irq,
+	.request_syncpt_irq = t20_request_syncpt_irq,
+};
diff --git a/drivers/video/tegra/host/host1x/host1x_syncpt.c b/drivers/video/tegra/host/host1x/host1x_syncpt.c
new file mode 100644
index 000000000000..8cca9dbbbc08
--- /dev/null
+++ b/drivers/video/tegra/host/host1x/host1x_syncpt.c
@@ -0,0 +1,180 @@
+/*
+ * drivers/video/tegra/host/host1x/host1x_syncpt.c
+ *
+ * Tegra Graphics Host Syncpoints for HOST1X
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/nvhost_ioctl.h>
+#include <linux/io.h>
+#include <trace/events/nvhost.h>
+#include "nvhost_syncpt.h"
+#include "nvhost_acm.h"
+#include "host1x.h"
+#include "chip_support.h"
+
+/**
+ * Write the current syncpoint value back to hw.
+ */
+static void t20_syncpt_reset(struct nvhost_syncpt *sp, u32 id)
+{
+	struct nvhost_master *dev = syncpt_to_dev(sp);
+	int min = nvhost_syncpt_read_min(sp, id);
+	writel(min, dev->sync_aperture + (host1x_sync_syncpt_0_r() + id * 4));
+}
+
+/**
+ * Write the current waitbase value back to hw.
+ */
+static void t20_syncpt_reset_wait_base(struct nvhost_syncpt *sp, u32 id)
+{
+	struct nvhost_master *dev = syncpt_to_dev(sp);
+	writel(sp->base_val[id],
+		dev->sync_aperture + (host1x_sync_syncpt_base_0_r() + id * 4));
+}
+
+/**
+ * Read waitbase value from hw.
+ */
+static void t20_syncpt_read_wait_base(struct nvhost_syncpt *sp, u32 id)
+{
+	struct nvhost_master *dev = syncpt_to_dev(sp);
+	sp->base_val[id] = readl(dev->sync_aperture +
+				(host1x_sync_syncpt_base_0_r() + id * 4));
+}
+
+/**
+ * Updates the last value read from hardware.
+ * (was nvhost_syncpt_update_min)
+ */
+static u32 t20_syncpt_update_min(struct nvhost_syncpt *sp, u32 id)
+{
+	struct nvhost_master *dev = syncpt_to_dev(sp);
+	void __iomem *sync_regs = dev->sync_aperture;
+	u32 old, live;
+
+	do {
+		old = nvhost_syncpt_read_min(sp, id);
+		live = readl(sync_regs + (host1x_sync_syncpt_0_r() + id * 4));
+	} while ((u32)atomic_cmpxchg(&sp->min_val[id], old, live) != old);
+
+	if (!nvhost_syncpt_check_max(sp, id, live))
+		dev_err(&syncpt_to_dev(sp)->dev->dev,
+				"%s failed: id=%u, min=%d, max=%d\n",
+				__func__,
+				nvhost_syncpt_read_min(sp, id),
+				nvhost_syncpt_read_max(sp, id),
+				id);
+
+	return live;
+}
+
+/**
+ * Write a cpu syncpoint increment to the hardware, without touching
+ * the cache. Caller is responsible for host being powered.
+ */
+static void t20_syncpt_cpu_incr(struct nvhost_syncpt *sp, u32 id)
+{
+	struct nvhost_master *dev = syncpt_to_dev(sp);
+	u32 reg_offset = id / 32;
+
+	BUG_ON(!nvhost_module_powered(dev->dev));
+	if (!nvhost_syncpt_client_managed(sp, id)
+			&& nvhost_syncpt_min_eq_max(sp, id)) {
+		dev_err(&syncpt_to_dev(sp)->dev->dev,
+			"Trying to increment syncpoint id %d beyond max\n",
+			id);
+		nvhost_debug_dump(syncpt_to_dev(sp));
+		return;
+	}
+	writel(BIT_MASK(id), dev->sync_aperture +
+			host1x_sync_syncpt_cpu_incr_r() + reg_offset * 4);
+	wmb();
+}
+
+/* remove a wait pointed to by patch_addr */
+static int host1x_syncpt_patch_wait(struct nvhost_syncpt *sp,
+		void *patch_addr)
+{
+	u32 override = nvhost_class_host_wait_syncpt(
+			NVSYNCPT_GRAPHICS_HOST, 0);
+	__raw_writel(override, patch_addr);
+	return 0;
+}
+
+
+static const char *t20_syncpt_name(struct nvhost_syncpt *sp, u32 id)
+{
+	struct host1x_device_info *info = &syncpt_to_dev(sp)->info;
+	return (id >= info->nb_pts) ? NULL : info->syncpt_names[id];
+}
+
+static void t20_syncpt_debug(struct nvhost_syncpt *sp)
+{
+	u32 i;
+	for (i = 0; i < nvhost_syncpt_nb_pts(sp); i++) {
+		u32 max = nvhost_syncpt_read_max(sp, i);
+		u32 min = nvhost_syncpt_update_min(sp, i);
+		if (!max && !min)
+			continue;
+		dev_info(&syncpt_to_dev(sp)->dev->dev,
+			"id %d (%s) min %d max %d\n",
+			i, syncpt_op().name(sp, i),
+			min, max);
+
+	}
+
+	for (i = 0; i < nvhost_syncpt_nb_bases(sp); i++) {
+		u32 base_val;
+		t20_syncpt_read_wait_base(sp, i);
+		base_val = sp->base_val[i];
+		if (base_val)
+			dev_info(&syncpt_to_dev(sp)->dev->dev,
+					"waitbase id %d val %d\n",
+					i, base_val);
+
+	}
+}
+
+static int syncpt_mutex_try_lock(struct nvhost_syncpt *sp,
+		unsigned int idx)
+{
+	void __iomem *sync_regs = syncpt_to_dev(sp)->sync_aperture;
+	/* mlock registers returns 0 when the lock is aquired.
+	 * writing 0 clears the lock. */
+	return !!readl(sync_regs + (host1x_sync_mlock_0_r() + idx * 4));
+}
+
+static void syncpt_mutex_unlock(struct nvhost_syncpt *sp,
+	       unsigned int idx)
+{
+	void __iomem *sync_regs = syncpt_to_dev(sp)->sync_aperture;
+
+	writel(0, sync_regs + (host1x_sync_mlock_0_r() + idx * 4));
+}
+
+static const struct nvhost_syncpt_ops host1x_syncpt_ops = {
+	.reset = t20_syncpt_reset,
+	.reset_wait_base = t20_syncpt_reset_wait_base,
+	.read_wait_base = t20_syncpt_read_wait_base,
+	.update_min = t20_syncpt_update_min,
+	.cpu_incr = t20_syncpt_cpu_incr,
+	.patch_wait = host1x_syncpt_patch_wait,
+	.debug = t20_syncpt_debug,
+	.name = t20_syncpt_name,
+	.mutex_try_lock = syncpt_mutex_try_lock,
+	.mutex_unlock = syncpt_mutex_unlock,
+};
diff --git a/drivers/video/tegra/host/host1x/host1x_syncpt.h b/drivers/video/tegra/host/host1x/host1x_syncpt.h
new file mode 100644
index 000000000000..a971db8b1d94
--- /dev/null
+++ b/drivers/video/tegra/host/host1x/host1x_syncpt.h
@@ -0,0 +1,62 @@
+/*
+ * drivers/video/tegra/host/host1x/host1x_syncpt.h
+ *
+ * Tegra Graphics Host Syncpoints for HOST1X
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_HOST1X_HOST1X_SYNCPT_H
+#define __NVHOST_HOST1X_HOST1X_SYNCPT_H
+
+/* FIXME:
+ * Sync point ids are now split into 2 files.
+ * 1 if this one and other is in include/linux/nvhost.h
+ * So if someone decides to add new sync point in future
+ * please check both the header files
+ */
+#define NVSYNCPT_CSI_VI_0		     (11)
+#define NVSYNCPT_CSI_VI_1		     (12)
+#define NVSYNCPT_VI_ISP_0		     (13)
+#define NVSYNCPT_VI_ISP_1		     (14)
+#define NVSYNCPT_VI_ISP_2		     (15)
+#define NVSYNCPT_VI_ISP_3		     (16)
+#define NVSYNCPT_VI_ISP_4		     (17)
+#define NVSYNCPT_2D_0			     (18)
+#define NVSYNCPT_2D_1			     (19)
+#define NVSYNCPT_3D			     (22)
+#define NVSYNCPT_MPE			     (23)
+#define NVSYNCPT_MPE_EBM_EOF		     (28)
+#define NVSYNCPT_MPE_WR_SAFE		     (29)
+
+/* sync points that are wholly managed by the client */
+#define NVSYNCPTS_CLIENT_MANAGED ( \
+	BIT(NVSYNCPT_DISP0_A) | BIT(NVSYNCPT_DISP1_A) | \
+	BIT(NVSYNCPT_DISP0_B) | BIT(NVSYNCPT_DISP1_B) | \
+	BIT(NVSYNCPT_DISP0_C) | BIT(NVSYNCPT_DISP1_C) | \
+	BIT(NVSYNCPT_DSI) | \
+	BIT(NVSYNCPT_VBLANK0) | BIT(NVSYNCPT_VBLANK1) | \
+	BIT(NVSYNCPT_CSI_VI_0) | BIT(NVSYNCPT_CSI_VI_1) | \
+	BIT(NVSYNCPT_VI_ISP_1) | BIT(NVSYNCPT_VI_ISP_2) | \
+	BIT(NVSYNCPT_VI_ISP_3) | BIT(NVSYNCPT_VI_ISP_4) | \
+	BIT(NVSYNCPT_MPE_EBM_EOF) | BIT(NVSYNCPT_MPE_WR_SAFE) | \
+	BIT(NVSYNCPT_2D_1) | BIT(NVSYNCPT_AVP_0))
+
+#define NVWAITBASE_2D_0 (1)
+#define NVWAITBASE_2D_1 (2)
+#define NVWAITBASE_3D   (3)
+#define NVWAITBASE_MPE  (4)
+
+#endif
diff --git a/drivers/video/tegra/host/host1x/hw_host1x01_channel.h b/drivers/video/tegra/host/host1x/hw_host1x01_channel.h
new file mode 100644
index 000000000000..ca2f9a0778cd
--- /dev/null
+++ b/drivers/video/tegra/host/host1x/hw_host1x01_channel.h
@@ -0,0 +1,182 @@
+/*
+ * drivers/video/tegra/host/host1x/hw_host1x_channel_host1x.h
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+ /*
+  * Function naming determines intended use:
+  *
+  *     <x>_r(void) : Returns the offset for register <x>.
+  *
+  *     <x>_w(void) : Returns the word offset for word (4 byte) element <x>.
+  *
+  *     <x>_<y>_s(void) : Returns size of field <y> of register <x> in bits.
+  *
+  *     <x>_<y>_f(u32 v) : Returns a value based on 'v' which has been shifted
+  *         and masked to place it at field <y> of register <x>.  This value
+  *         can be |'d with others to produce a full register value for
+  *         register <x>.
+  *
+  *     <x>_<y>_m(void) : Returns a mask for field <y> of register <x>.  This
+  *         value can be ~'d and then &'d to clear the value of field <y> for
+  *         register <x>.
+  *
+  *     <x>_<y>_<z>_f(void) : Returns the constant value <z> after being shifted
+  *         to place it at field <y> of register <x>.  This value can be |'d
+  *         with others to produce a full register value for <x>.
+  *
+  *     <x>_<y>_v(u32 r) : Returns the value of field <y> from a full register
+  *         <x> value 'r' after being shifted to place its LSB at bit 0.
+  *         This value is suitable for direct comparison with other unshifted
+  *         values appropriate for use in field <y> of register <x>.
+  *
+  *     <x>_<y>_<z>_v(void) : Returns the constant value for <z> defined for
+  *         field <y> of register <x>.  This value is suitable for direct
+  *         comparison with unshifted values appropriate for use in field <y>
+  *         of register <x>.
+  */
+
+#ifndef __hw_host1x_channel_host1x_h__
+#define __hw_host1x_channel_host1x_h__
+/*This file is autogenerated.  Do not edit. */
+
+static inline u32 host1x_channel_fifostat_r(void)
+{
+	return 0x0;
+}
+static inline u32 host1x_channel_fifostat_cfempty_s(void)
+{
+	return 1;
+}
+static inline u32 host1x_channel_fifostat_cfempty_f(u32 v)
+{
+	return (v & 0x1) << 10;
+}
+static inline u32 host1x_channel_fifostat_cfempty_m(void)
+{
+	return 0x1 << 10;
+}
+static inline u32 host1x_channel_fifostat_cfempty_v(u32 r)
+{
+	return (r >> 10) & 0x1;
+}
+static inline u32 host1x_channel_fifostat_cfempty_notempty_v(void)
+{
+	return 0;
+}
+static inline u32 host1x_channel_fifostat_cfempty_empty_v(void)
+{
+	return 1;
+}
+static inline u32 host1x_channel_fifostat_outfentries_s(void)
+{
+	return 5;
+}
+static inline u32 host1x_channel_fifostat_outfentries_f(u32 v)
+{
+	return (v & 0x1f) << 24;
+}
+static inline u32 host1x_channel_fifostat_outfentries_m(void)
+{
+	return 0x1f << 24;
+}
+static inline u32 host1x_channel_fifostat_outfentries_v(u32 r)
+{
+	return (r >> 24) & 0x1f;
+}
+static inline u32 host1x_channel_inddata_r(void)
+{
+	return 0xc;
+}
+static inline u32 host1x_channel_dmastart_r(void)
+{
+	return 0x14;
+}
+static inline u32 host1x_channel_dmaput_r(void)
+{
+	return 0x18;
+}
+static inline u32 host1x_channel_dmaget_r(void)
+{
+	return 0x1c;
+}
+static inline u32 host1x_channel_dmaend_r(void)
+{
+	return 0x20;
+}
+static inline u32 host1x_channel_dmactrl_r(void)
+{
+	return 0x24;
+}
+static inline u32 host1x_channel_dmactrl_dmastop_s(void)
+{
+	return 1;
+}
+static inline u32 host1x_channel_dmactrl_dmastop_f(u32 v)
+{
+	return (v & 0x1) << 0;
+}
+static inline u32 host1x_channel_dmactrl_dmastop_m(void)
+{
+	return 0x1 << 0;
+}
+static inline u32 host1x_channel_dmactrl_dmastop_v(u32 r)
+{
+	return (r >> 0) & 0x1;
+}
+static inline u32 host1x_channel_dmactrl_dmastop_run_v(void)
+{
+	return 0;
+}
+static inline u32 host1x_channel_dmactrl_dmastop_stop_v(void)
+{
+	return 1;
+}
+static inline u32 host1x_channel_dmactrl_dmagetrst_s(void)
+{
+	return 1;
+}
+static inline u32 host1x_channel_dmactrl_dmagetrst_f(u32 v)
+{
+	return (v & 0x1) << 1;
+}
+static inline u32 host1x_channel_dmactrl_dmagetrst_m(void)
+{
+	return 0x1 << 1;
+}
+static inline u32 host1x_channel_dmactrl_dmagetrst_v(u32 r)
+{
+	return (r >> 1) & 0x1;
+}
+static inline u32 host1x_channel_dmactrl_dmainitget_s(void)
+{
+	return 1;
+}
+static inline u32 host1x_channel_dmactrl_dmainitget_f(u32 v)
+{
+	return (v & 0x1) << 2;
+}
+static inline u32 host1x_channel_dmactrl_dmainitget_m(void)
+{
+	return 0x1 << 2;
+}
+static inline u32 host1x_channel_dmactrl_dmainitget_v(u32 r)
+{
+	return (r >> 2) & 0x1;
+}
+
+#endif /* __hw_host1x_channel_host1x_h__ */
diff --git a/drivers/video/tegra/host/host1x/hw_host1x01_sync.h b/drivers/video/tegra/host/host1x/hw_host1x01_sync.h
new file mode 100644
index 000000000000..67f0cbfb85b9
--- /dev/null
+++ b/drivers/video/tegra/host/host1x/hw_host1x01_sync.h
@@ -0,0 +1,398 @@
+/*
+ * drivers/video/tegra/host/host1x/hw_host1x_sync_host1x.h
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+ /*
+  * Function naming determines intended use:
+  *
+  *     <x>_r(void) : Returns the offset for register <x>.
+  *
+  *     <x>_w(void) : Returns the word offset for word (4 byte) element <x>.
+  *
+  *     <x>_<y>_s(void) : Returns size of field <y> of register <x> in bits.
+  *
+  *     <x>_<y>_f(u32 v) : Returns a value based on 'v' which has been shifted
+  *         and masked to place it at field <y> of register <x>.  This value
+  *         can be |'d with others to produce a full register value for
+  *         register <x>.
+  *
+  *     <x>_<y>_m(void) : Returns a mask for field <y> of register <x>.  This
+  *         value can be ~'d and then &'d to clear the value of field <y> for
+  *         register <x>.
+  *
+  *     <x>_<y>_<z>_f(void) : Returns the constant value <z> after being shifted
+  *         to place it at field <y> of register <x>.  This value can be |'d
+  *         with others to produce a full register value for <x>.
+  *
+  *     <x>_<y>_v(u32 r) : Returns the value of field <y> from a full register
+  *         <x> value 'r' after being shifted to place its LSB at bit 0.
+  *         This value is suitable for direct comparison with other unshifted
+  *         values appropriate for use in field <y> of register <x>.
+  *
+  *     <x>_<y>_<z>_v(void) : Returns the constant value for <z> defined for
+  *         field <y> of register <x>.  This value is suitable for direct
+  *         comparison with unshifted values appropriate for use in field <y>
+  *         of register <x>.
+  */
+
+#ifndef __hw_host1x_sync_host1x_h__
+#define __hw_host1x_sync_host1x_h__
+/*This file is autogenerated.  Do not edit. */
+
+static inline u32 host1x_sync_intmask_r(void)
+{
+	return 0x4;
+}
+static inline u32 host1x_sync_intc0mask_r(void)
+{
+	return 0x8;
+}
+static inline u32 host1x_sync_hintstatus_r(void)
+{
+	return 0x20;
+}
+static inline u32 host1x_sync_hintmask_r(void)
+{
+	return 0x24;
+}
+static inline u32 host1x_sync_hintstatus_ext_r(void)
+{
+	return 0x28;
+}
+static inline u32 host1x_sync_hintstatus_ext_ip_read_int_s(void)
+{
+	return 1;
+}
+static inline u32 host1x_sync_hintstatus_ext_ip_read_int_f(u32 v)
+{
+	return (v & 0x1) << 30;
+}
+static inline u32 host1x_sync_hintstatus_ext_ip_read_int_m(void)
+{
+	return 0x1 << 30;
+}
+static inline u32 host1x_sync_hintstatus_ext_ip_read_int_v(u32 r)
+{
+	return (r >> 30) & 0x1;
+}
+static inline u32 host1x_sync_hintstatus_ext_ip_write_int_s(void)
+{
+	return 1;
+}
+static inline u32 host1x_sync_hintstatus_ext_ip_write_int_f(u32 v)
+{
+	return (v & 0x1) << 31;
+}
+static inline u32 host1x_sync_hintstatus_ext_ip_write_int_m(void)
+{
+	return 0x1 << 31;
+}
+static inline u32 host1x_sync_hintstatus_ext_ip_write_int_v(u32 r)
+{
+	return (r >> 31) & 0x1;
+}
+static inline u32 host1x_sync_hintmask_ext_r(void)
+{
+	return 0x2c;
+}
+static inline u32 host1x_sync_syncpt_thresh_cpu0_int_status_r(void)
+{
+	return 0x40;
+}
+static inline u32 host1x_sync_syncpt_thresh_cpu1_int_status_r(void)
+{
+	return 0x48;
+}
+static inline u32 host1x_sync_syncpt_thresh_int_disable_r(void)
+{
+	return 0x60;
+}
+static inline u32 host1x_sync_syncpt_thresh_int_enable_cpu0_r(void)
+{
+	return 0x68;
+}
+static inline u32 host1x_sync_cf0_setup_r(void)
+{
+	return 0x80;
+}
+static inline u32 host1x_sync_cf0_setup_cf0_base_s(void)
+{
+	return 9;
+}
+static inline u32 host1x_sync_cf0_setup_cf0_base_f(u32 v)
+{
+	return (v & 0x1ff) << 0;
+}
+static inline u32 host1x_sync_cf0_setup_cf0_base_m(void)
+{
+	return 0x1ff << 0;
+}
+static inline u32 host1x_sync_cf0_setup_cf0_base_v(u32 r)
+{
+	return (r >> 0) & 0x1ff;
+}
+static inline u32 host1x_sync_cf0_setup_cf0_limit_s(void)
+{
+	return 9;
+}
+static inline u32 host1x_sync_cf0_setup_cf0_limit_f(u32 v)
+{
+	return (v & 0x1ff) << 16;
+}
+static inline u32 host1x_sync_cf0_setup_cf0_limit_m(void)
+{
+	return 0x1ff << 16;
+}
+static inline u32 host1x_sync_cf0_setup_cf0_limit_v(u32 r)
+{
+	return (r >> 16) & 0x1ff;
+}
+static inline u32 host1x_sync_cmdproc_stop_r(void)
+{
+	return 0xac;
+}
+static inline u32 host1x_sync_ch_teardown_r(void)
+{
+	return 0xb0;
+}
+static inline u32 host1x_sync_usec_clk_r(void)
+{
+	return 0x1a4;
+}
+static inline u32 host1x_sync_ctxsw_timeout_cfg_r(void)
+{
+	return 0x1a8;
+}
+static inline u32 host1x_sync_ip_busy_timeout_r(void)
+{
+	return 0x1bc;
+}
+static inline u32 host1x_sync_ip_read_timeout_addr_r(void)
+{
+	return 0x1c0;
+}
+static inline u32 host1x_sync_ip_write_timeout_addr_r(void)
+{
+	return 0x1c4;
+}
+static inline u32 host1x_sync_mlock_0_r(void)
+{
+	return 0x2c0;
+}
+static inline u32 host1x_sync_mlock_owner_0_r(void)
+{
+	return 0x340;
+}
+static inline u32 host1x_sync_mlock_owner_0_mlock_owner_chid_0_s(void)
+{
+	return 4;
+}
+static inline u32 host1x_sync_mlock_owner_0_mlock_owner_chid_0_f(u32 v)
+{
+	return (v & 0xf) << 8;
+}
+static inline u32 host1x_sync_mlock_owner_0_mlock_owner_chid_0_m(void)
+{
+	return 0xf << 8;
+}
+static inline u32 host1x_sync_mlock_owner_0_mlock_owner_chid_0_v(u32 r)
+{
+	return (r >> 8) & 0xf;
+}
+static inline u32 host1x_sync_mlock_owner_0_mlock_cpu_owns_0_s(void)
+{
+	return 1;
+}
+static inline u32 host1x_sync_mlock_owner_0_mlock_cpu_owns_0_f(u32 v)
+{
+	return (v & 0x1) << 1;
+}
+static inline u32 host1x_sync_mlock_owner_0_mlock_cpu_owns_0_m(void)
+{
+	return 0x1 << 1;
+}
+static inline u32 host1x_sync_mlock_owner_0_mlock_cpu_owns_0_v(u32 r)
+{
+	return (r >> 1) & 0x1;
+}
+static inline u32 host1x_sync_mlock_owner_0_mlock_ch_owns_0_s(void)
+{
+	return 1;
+}
+static inline u32 host1x_sync_mlock_owner_0_mlock_ch_owns_0_f(u32 v)
+{
+	return (v & 0x1) << 0;
+}
+static inline u32 host1x_sync_mlock_owner_0_mlock_ch_owns_0_m(void)
+{
+	return 0x1 << 0;
+}
+static inline u32 host1x_sync_mlock_owner_0_mlock_ch_owns_0_v(u32 r)
+{
+	return (r >> 0) & 0x1;
+}
+static inline u32 host1x_sync_syncpt_0_r(void)
+{
+	return 0x400;
+}
+static inline u32 host1x_sync_syncpt_int_thresh_0_r(void)
+{
+	return 0x500;
+}
+static inline u32 host1x_sync_syncpt_base_0_r(void)
+{
+	return 0x600;
+}
+static inline u32 host1x_sync_syncpt_cpu_incr_r(void)
+{
+	return 0x700;
+}
+static inline u32 host1x_sync_cbread0_r(void)
+{
+	return 0x720;
+}
+static inline u32 host1x_sync_cfpeek_ctrl_r(void)
+{
+	return 0x74c;
+}
+static inline u32 host1x_sync_cfpeek_ctrl_cfpeek_addr_s(void)
+{
+	return 9;
+}
+static inline u32 host1x_sync_cfpeek_ctrl_cfpeek_addr_f(u32 v)
+{
+	return (v & 0x1ff) << 0;
+}
+static inline u32 host1x_sync_cfpeek_ctrl_cfpeek_addr_m(void)
+{
+	return 0x1ff << 0;
+}
+static inline u32 host1x_sync_cfpeek_ctrl_cfpeek_addr_v(u32 r)
+{
+	return (r >> 0) & 0x1ff;
+}
+static inline u32 host1x_sync_cfpeek_ctrl_cfpeek_channr_s(void)
+{
+	return 3;
+}
+static inline u32 host1x_sync_cfpeek_ctrl_cfpeek_channr_f(u32 v)
+{
+	return (v & 0x7) << 16;
+}
+static inline u32 host1x_sync_cfpeek_ctrl_cfpeek_channr_m(void)
+{
+	return 0x7 << 16;
+}
+static inline u32 host1x_sync_cfpeek_ctrl_cfpeek_channr_v(u32 r)
+{
+	return (r >> 16) & 0x7;
+}
+static inline u32 host1x_sync_cfpeek_ctrl_cfpeek_ena_s(void)
+{
+	return 1;
+}
+static inline u32 host1x_sync_cfpeek_ctrl_cfpeek_ena_f(u32 v)
+{
+	return (v & 0x1) << 31;
+}
+static inline u32 host1x_sync_cfpeek_ctrl_cfpeek_ena_m(void)
+{
+	return 0x1 << 31;
+}
+static inline u32 host1x_sync_cfpeek_ctrl_cfpeek_ena_v(u32 r)
+{
+	return (r >> 31) & 0x1;
+}
+static inline u32 host1x_sync_cfpeek_read_r(void)
+{
+	return 0x750;
+}
+static inline u32 host1x_sync_cfpeek_ptrs_r(void)
+{
+	return 0x754;
+}
+static inline u32 host1x_sync_cfpeek_ptrs_cf_rd_ptr_s(void)
+{
+	return 9;
+}
+static inline u32 host1x_sync_cfpeek_ptrs_cf_rd_ptr_f(u32 v)
+{
+	return (v & 0x1ff) << 0;
+}
+static inline u32 host1x_sync_cfpeek_ptrs_cf_rd_ptr_m(void)
+{
+	return 0x1ff << 0;
+}
+static inline u32 host1x_sync_cfpeek_ptrs_cf_rd_ptr_v(u32 r)
+{
+	return (r >> 0) & 0x1ff;
+}
+static inline u32 host1x_sync_cfpeek_ptrs_cf_wr_ptr_s(void)
+{
+	return 9;
+}
+static inline u32 host1x_sync_cfpeek_ptrs_cf_wr_ptr_f(u32 v)
+{
+	return (v & 0x1ff) << 16;
+}
+static inline u32 host1x_sync_cfpeek_ptrs_cf_wr_ptr_m(void)
+{
+	return 0x1ff << 16;
+}
+static inline u32 host1x_sync_cfpeek_ptrs_cf_wr_ptr_v(u32 r)
+{
+	return (r >> 16) & 0x1ff;
+}
+static inline u32 host1x_sync_cbstat_0_r(void)
+{
+	return 0x758;
+}
+static inline u32 host1x_sync_cbstat_0_cboffset0_s(void)
+{
+	return 16;
+}
+static inline u32 host1x_sync_cbstat_0_cboffset0_f(u32 v)
+{
+	return (v & 0xffff) << 0;
+}
+static inline u32 host1x_sync_cbstat_0_cboffset0_m(void)
+{
+	return 0xffff << 0;
+}
+static inline u32 host1x_sync_cbstat_0_cboffset0_v(u32 r)
+{
+	return (r >> 0) & 0xffff;
+}
+static inline u32 host1x_sync_cbstat_0_cbclass0_s(void)
+{
+	return 10;
+}
+static inline u32 host1x_sync_cbstat_0_cbclass0_f(u32 v)
+{
+	return (v & 0x3ff) << 16;
+}
+static inline u32 host1x_sync_cbstat_0_cbclass0_m(void)
+{
+	return 0x3ff << 16;
+}
+static inline u32 host1x_sync_cbstat_0_cbclass0_v(u32 r)
+{
+	return (r >> 16) & 0x3ff;
+}
+
+#endif /* __hw_host1x_sync_host1x_h__ */
diff --git a/drivers/video/tegra/host/host1x/hw_host1x01_uclass.h b/drivers/video/tegra/host/host1x/hw_host1x01_uclass.h
new file mode 100644
index 000000000000..ed6e4b706ab9
--- /dev/null
+++ b/drivers/video/tegra/host/host1x/hw_host1x01_uclass.h
@@ -0,0 +1,474 @@
+/*
+ * drivers/video/tegra/host/host1x/hw_host1x_uclass_host1x.h
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+ /*
+  * Function naming determines intended use:
+  *
+  *     <x>_r(void) : Returns the offset for register <x>.
+  *
+  *     <x>_w(void) : Returns the word offset for word (4 byte) element <x>.
+  *
+  *     <x>_<y>_s(void) : Returns size of field <y> of register <x> in bits.
+  *
+  *     <x>_<y>_f(u32 v) : Returns a value based on 'v' which has been shifted
+  *         and masked to place it at field <y> of register <x>.  This value
+  *         can be |'d with others to produce a full register value for
+  *         register <x>.
+  *
+  *     <x>_<y>_m(void) : Returns a mask for field <y> of register <x>.  This
+  *         value can be ~'d and then &'d to clear the value of field <y> for
+  *         register <x>.
+  *
+  *     <x>_<y>_<z>_f(void) : Returns the constant value <z> after being shifted
+  *         to place it at field <y> of register <x>.  This value can be |'d
+  *         with others to produce a full register value for <x>.
+  *
+  *     <x>_<y>_v(u32 r) : Returns the value of field <y> from a full register
+  *         <x> value 'r' after being shifted to place its LSB at bit 0.
+  *         This value is suitable for direct comparison with other unshifted
+  *         values appropriate for use in field <y> of register <x>.
+  *
+  *     <x>_<y>_<z>_v(void) : Returns the constant value for <z> defined for
+  *         field <y> of register <x>.  This value is suitable for direct
+  *         comparison with unshifted values appropriate for use in field <y>
+  *         of register <x>.
+  */
+
+#ifndef __hw_host1x_uclass_host1x_h__
+#define __hw_host1x_uclass_host1x_h__
+/*This file is autogenerated.  Do not edit. */
+
+static inline u32 host1x_uclass_incr_syncpt_r(void)
+{
+	return 0x0;
+}
+static inline u32 host1x_uclass_incr_syncpt_cond_s(void)
+{
+	return 8;
+}
+static inline u32 host1x_uclass_incr_syncpt_cond_f(u32 v)
+{
+	return (v & 0xff) << 8;
+}
+static inline u32 host1x_uclass_incr_syncpt_cond_m(void)
+{
+	return 0xff << 8;
+}
+static inline u32 host1x_uclass_incr_syncpt_cond_v(u32 r)
+{
+	return (r >> 8) & 0xff;
+}
+static inline u32 host1x_uclass_incr_syncpt_cond_immediate_v(void)
+{
+	return 0;
+}
+static inline u32 host1x_uclass_incr_syncpt_cond_op_done_v(void)
+{
+	return 1;
+}
+static inline u32 host1x_uclass_incr_syncpt_cond_rd_done_v(void)
+{
+	return 2;
+}
+static inline u32 host1x_uclass_incr_syncpt_cond_reg_wr_safe_v(void)
+{
+	return 3;
+}
+static inline u32 host1x_uclass_incr_syncpt_indx_s(void)
+{
+	return 8;
+}
+static inline u32 host1x_uclass_incr_syncpt_indx_f(u32 v)
+{
+	return (v & 0xff) << 0;
+}
+static inline u32 host1x_uclass_incr_syncpt_indx_m(void)
+{
+	return 0xff << 0;
+}
+static inline u32 host1x_uclass_incr_syncpt_indx_v(u32 r)
+{
+	return (r >> 0) & 0xff;
+}
+static inline u32 host1x_uclass_wait_syncpt_r(void)
+{
+	return 0x8;
+}
+static inline u32 host1x_uclass_wait_syncpt_indx_s(void)
+{
+	return 8;
+}
+static inline u32 host1x_uclass_wait_syncpt_indx_f(u32 v)
+{
+	return (v & 0xff) << 24;
+}
+static inline u32 host1x_uclass_wait_syncpt_indx_m(void)
+{
+	return 0xff << 24;
+}
+static inline u32 host1x_uclass_wait_syncpt_indx_v(u32 r)
+{
+	return (r >> 24) & 0xff;
+}
+static inline u32 host1x_uclass_wait_syncpt_thresh_s(void)
+{
+	return 24;
+}
+static inline u32 host1x_uclass_wait_syncpt_thresh_f(u32 v)
+{
+	return (v & 0xffffff) << 0;
+}
+static inline u32 host1x_uclass_wait_syncpt_thresh_m(void)
+{
+	return 0xffffff << 0;
+}
+static inline u32 host1x_uclass_wait_syncpt_thresh_v(u32 r)
+{
+	return (r >> 0) & 0xffffff;
+}
+static inline u32 host1x_uclass_wait_syncpt_base_r(void)
+{
+	return 0x9;
+}
+static inline u32 host1x_uclass_wait_syncpt_base_indx_s(void)
+{
+	return 8;
+}
+static inline u32 host1x_uclass_wait_syncpt_base_indx_f(u32 v)
+{
+	return (v & 0xff) << 24;
+}
+static inline u32 host1x_uclass_wait_syncpt_base_indx_m(void)
+{
+	return 0xff << 24;
+}
+static inline u32 host1x_uclass_wait_syncpt_base_indx_v(u32 r)
+{
+	return (r >> 24) & 0xff;
+}
+static inline u32 host1x_uclass_wait_syncpt_base_base_indx_s(void)
+{
+	return 8;
+}
+static inline u32 host1x_uclass_wait_syncpt_base_base_indx_f(u32 v)
+{
+	return (v & 0xff) << 16;
+}
+static inline u32 host1x_uclass_wait_syncpt_base_base_indx_m(void)
+{
+	return 0xff << 16;
+}
+static inline u32 host1x_uclass_wait_syncpt_base_base_indx_v(u32 r)
+{
+	return (r >> 16) & 0xff;
+}
+static inline u32 host1x_uclass_wait_syncpt_base_offset_s(void)
+{
+	return 16;
+}
+static inline u32 host1x_uclass_wait_syncpt_base_offset_f(u32 v)
+{
+	return (v & 0xffff) << 0;
+}
+static inline u32 host1x_uclass_wait_syncpt_base_offset_m(void)
+{
+	return 0xffff << 0;
+}
+static inline u32 host1x_uclass_wait_syncpt_base_offset_v(u32 r)
+{
+	return (r >> 0) & 0xffff;
+}
+static inline u32 host1x_uclass_load_syncpt_base_r(void)
+{
+	return 0xb;
+}
+static inline u32 host1x_uclass_load_syncpt_base_base_indx_s(void)
+{
+	return 8;
+}
+static inline u32 host1x_uclass_load_syncpt_base_base_indx_f(u32 v)
+{
+	return (v & 0xff) << 24;
+}
+static inline u32 host1x_uclass_load_syncpt_base_base_indx_m(void)
+{
+	return 0xff << 24;
+}
+static inline u32 host1x_uclass_load_syncpt_base_base_indx_v(u32 r)
+{
+	return (r >> 24) & 0xff;
+}
+static inline u32 host1x_uclass_load_syncpt_base_value_s(void)
+{
+	return 24;
+}
+static inline u32 host1x_uclass_load_syncpt_base_value_f(u32 v)
+{
+	return (v & 0xffffff) << 0;
+}
+static inline u32 host1x_uclass_load_syncpt_base_value_m(void)
+{
+	return 0xffffff << 0;
+}
+static inline u32 host1x_uclass_load_syncpt_base_value_v(u32 r)
+{
+	return (r >> 0) & 0xffffff;
+}
+static inline u32 host1x_uclass_incr_syncpt_base_r(void)
+{
+	return 0xc;
+}
+static inline u32 host1x_uclass_incr_syncpt_base_base_indx_s(void)
+{
+	return 8;
+}
+static inline u32 host1x_uclass_incr_syncpt_base_base_indx_f(u32 v)
+{
+	return (v & 0xff) << 24;
+}
+static inline u32 host1x_uclass_incr_syncpt_base_base_indx_m(void)
+{
+	return 0xff << 24;
+}
+static inline u32 host1x_uclass_incr_syncpt_base_base_indx_v(u32 r)
+{
+	return (r >> 24) & 0xff;
+}
+static inline u32 host1x_uclass_incr_syncpt_base_offset_s(void)
+{
+	return 24;
+}
+static inline u32 host1x_uclass_incr_syncpt_base_offset_f(u32 v)
+{
+	return (v & 0xffffff) << 0;
+}
+static inline u32 host1x_uclass_incr_syncpt_base_offset_m(void)
+{
+	return 0xffffff << 0;
+}
+static inline u32 host1x_uclass_incr_syncpt_base_offset_v(u32 r)
+{
+	return (r >> 0) & 0xffffff;
+}
+static inline u32 host1x_uclass_indoff_r(void)
+{
+	return 0x2d;
+}
+static inline u32 host1x_uclass_indoff_indbe_s(void)
+{
+	return 4;
+}
+static inline u32 host1x_uclass_indoff_indbe_f(u32 v)
+{
+	return (v & 0xf) << 28;
+}
+static inline u32 host1x_uclass_indoff_indbe_m(void)
+{
+	return 0xf << 28;
+}
+static inline u32 host1x_uclass_indoff_indbe_v(u32 r)
+{
+	return (r >> 28) & 0xf;
+}
+static inline u32 host1x_uclass_indoff_autoinc_s(void)
+{
+	return 1;
+}
+static inline u32 host1x_uclass_indoff_autoinc_f(u32 v)
+{
+	return (v & 0x1) << 27;
+}
+static inline u32 host1x_uclass_indoff_autoinc_m(void)
+{
+	return 0x1 << 27;
+}
+static inline u32 host1x_uclass_indoff_autoinc_v(u32 r)
+{
+	return (r >> 27) & 0x1;
+}
+static inline u32 host1x_uclass_indoff_spool_s(void)
+{
+	return 1;
+}
+static inline u32 host1x_uclass_indoff_spool_f(u32 v)
+{
+	return (v & 0x1) << 26;
+}
+static inline u32 host1x_uclass_indoff_spool_m(void)
+{
+	return 0x1 << 26;
+}
+static inline u32 host1x_uclass_indoff_spool_v(u32 r)
+{
+	return (r >> 26) & 0x1;
+}
+static inline u32 host1x_uclass_indoff_indoffset_s(void)
+{
+	return 24;
+}
+static inline u32 host1x_uclass_indoff_indoffset_f(u32 v)
+{
+	return (v & 0xffffff) << 2;
+}
+static inline u32 host1x_uclass_indoff_indoffset_m(void)
+{
+	return 0xffffff << 2;
+}
+static inline u32 host1x_uclass_indoff_indoffset_v(u32 r)
+{
+	return (r >> 2) & 0xffffff;
+}
+static inline u32 host1x_uclass_indoff_indmodid_s(void)
+{
+	return 8;
+}
+static inline u32 host1x_uclass_indoff_indmodid_f(u32 v)
+{
+	return (v & 0xff) << 18;
+}
+static inline u32 host1x_uclass_indoff_indmodid_m(void)
+{
+	return 0xff << 18;
+}
+static inline u32 host1x_uclass_indoff_indmodid_v(u32 r)
+{
+	return (r >> 18) & 0xff;
+}
+static inline u32 host1x_uclass_indoff_indmodid_host1x_v(void)
+{
+	return 0;
+}
+static inline u32 host1x_uclass_indoff_indmodid_mpe_v(void)
+{
+	return 1;
+}
+static inline u32 host1x_uclass_indoff_indmodid_vi_v(void)
+{
+	return 2;
+}
+static inline u32 host1x_uclass_indoff_indmodid_epp_v(void)
+{
+	return 3;
+}
+static inline u32 host1x_uclass_indoff_indmodid_isp_v(void)
+{
+	return 4;
+}
+static inline u32 host1x_uclass_indoff_indmodid_gr2d_v(void)
+{
+	return 5;
+}
+static inline u32 host1x_uclass_indoff_indmodid_gr3d_v(void)
+{
+	return 6;
+}
+static inline u32 host1x_uclass_indoff_indmodid_display_v(void)
+{
+	return 8;
+}
+static inline u32 host1x_uclass_indoff_indmodid_tvo_v(void)
+{
+	return 11;
+}
+static inline u32 host1x_uclass_indoff_indmodid_displayb_v(void)
+{
+	return 9;
+}
+static inline u32 host1x_uclass_indoff_indmodid_dsi_v(void)
+{
+	return 12;
+}
+static inline u32 host1x_uclass_indoff_indmodid_hdmi_v(void)
+{
+	return 10;
+}
+static inline u32 host1x_uclass_indoff_indmodid_dsib_v(void)
+{
+	return 16;
+}
+static inline u32 host1x_uclass_indoff_indroffset_s(void)
+{
+	return 16;
+}
+static inline u32 host1x_uclass_indoff_indroffset_f(u32 v)
+{
+	return (v & 0xffff) << 2;
+}
+static inline u32 host1x_uclass_indoff_indroffset_m(void)
+{
+	return 0xffff << 2;
+}
+static inline u32 host1x_uclass_indoff_indroffset_v(u32 r)
+{
+	return (r >> 2) & 0xffff;
+}
+static inline u32 host1x_uclass_indoff_acctype_s(void)
+{
+	return 1;
+}
+static inline u32 host1x_uclass_indoff_acctype_f(u32 v)
+{
+	return (v & 0x1) << 1;
+}
+static inline u32 host1x_uclass_indoff_acctype_m(void)
+{
+	return 0x1 << 1;
+}
+static inline u32 host1x_uclass_indoff_acctype_v(u32 r)
+{
+	return (r >> 1) & 0x1;
+}
+static inline u32 host1x_uclass_indoff_acctype_reg_v(void)
+{
+	return 0;
+}
+static inline u32 host1x_uclass_indoff_acctype_fb_v(void)
+{
+	return 1;
+}
+static inline u32 host1x_uclass_indoff_rwn_s(void)
+{
+	return 1;
+}
+static inline u32 host1x_uclass_indoff_rwn_f(u32 v)
+{
+	return (v & 0x1) << 0;
+}
+static inline u32 host1x_uclass_indoff_rwn_m(void)
+{
+	return 0x1 << 0;
+}
+static inline u32 host1x_uclass_indoff_rwn_v(u32 r)
+{
+	return (r >> 0) & 0x1;
+}
+static inline u32 host1x_uclass_indoff_rwn_write_v(void)
+{
+	return 0;
+}
+static inline u32 host1x_uclass_indoff_rwn_read_v(void)
+{
+	return 1;
+}
+static inline u32 host1x_uclass_inddata_r(void)
+{
+	return 0x2e;
+}
+
+#endif /* __hw_host1x_uclass_host1x_h__ */
diff --git a/drivers/video/tegra/host/isp/Makefile b/drivers/video/tegra/host/isp/Makefile
new file mode 100644
index 000000000000..7bcdc33c83dc
--- /dev/null
+++ b/drivers/video/tegra/host/isp/Makefile
@@ -0,0 +1,7 @@
+GCOV_PROFILE := y
+EXTRA_CFLAGS += -Idrivers/video/tegra/host
+
+nvhost-isp-objs  = \
+		isp.o
+
+obj-$(CONFIG_TEGRA_GRHOST) += nvhost-isp.o
diff --git a/drivers/video/tegra/host/isp/isp.c b/drivers/video/tegra/host/isp/isp.c
new file mode 100644
index 000000000000..0a3cc3b03578
--- /dev/null
+++ b/drivers/video/tegra/host/isp/isp.c
@@ -0,0 +1,79 @@
+/*
+ * drivers/video/tegra/host/isp/isp.c
+ *
+ * Tegra Graphics ISP
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "dev.h"
+#include "bus_client.h"
+
+static int __devinit isp_probe(struct nvhost_device *dev,
+	struct nvhost_device_id *id_table)
+{
+	int err = 0;
+
+	err = nvhost_client_device_get_resources(dev);
+	if (err)
+		return err;
+
+	return nvhost_client_device_init(dev);
+}
+
+static int __exit isp_remove(struct nvhost_device *dev)
+{
+	/* Add clean-up */
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int isp_suspend(struct nvhost_device *dev, pm_message_t state)
+{
+	return nvhost_client_device_suspend(dev);
+}
+
+static int isp_resume(struct nvhost_device *dev)
+{
+	dev_info(&dev->dev, "resuming\n");
+	return 0;
+}
+#endif
+
+static struct nvhost_driver isp_driver = {
+	.probe = isp_probe,
+	.remove = __exit_p(isp_remove),
+#ifdef CONFIG_PM
+	.suspend = isp_suspend,
+	.resume = isp_resume,
+#endif
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "isp",
+	}
+};
+
+static int __init isp_init(void)
+{
+	return nvhost_driver_register(&isp_driver);
+}
+
+static void __exit isp_exit(void)
+{
+	nvhost_driver_unregister(&isp_driver);
+}
+
+module_init(isp_init);
+module_exit(isp_exit);
diff --git a/drivers/video/tegra/host/mpe/Makefile b/drivers/video/tegra/host/mpe/Makefile
new file mode 100644
index 000000000000..efd77bb88fe7
--- /dev/null
+++ b/drivers/video/tegra/host/mpe/Makefile
@@ -0,0 +1,7 @@
+GCOV_PROFILE := y
+EXTRA_CFLAGS += -Idrivers/video/tegra/host
+
+nvhost-mpe-objs  = \
+		mpe.o
+
+obj-$(CONFIG_TEGRA_GRHOST) += nvhost-mpe.o
diff --git a/drivers/video/tegra/host/mpe/mpe.c b/drivers/video/tegra/host/mpe/mpe.c
new file mode 100644
index 000000000000..d76ee0108eef
--- /dev/null
+++ b/drivers/video/tegra/host/mpe/mpe.c
@@ -0,0 +1,680 @@
+/*
+ * drivers/video/tegra/host/mpe/mpe.c
+ *
+ * Tegra Graphics Host MPE
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "nvhost_hwctx.h"
+#include "nvhost_channel.h"
+#include "dev.h"
+#include "host1x/host1x01_hardware.h"
+#include "host1x/host1x_hwctx.h"
+#include "t20/t20.h"
+#include "chip_support.h"
+#include "nvhost_memmgr.h"
+
+#include <linux/slab.h>
+
+#include "bus_client.h"
+
+enum {
+	HWCTX_REGINFO_NORMAL = 0,
+	HWCTX_REGINFO_STASH,
+	HWCTX_REGINFO_CALCULATE,
+	HWCTX_REGINFO_WRITEBACK
+};
+
+const struct hwctx_reginfo ctxsave_regs_mpe[] = {
+	HWCTX_REGINFO(0x124,  1, STASH),
+	HWCTX_REGINFO(0x123,  1, STASH),
+	HWCTX_REGINFO(0x103,  1, STASH),
+	HWCTX_REGINFO(0x074,  1, STASH),
+	HWCTX_REGINFO(0x021,  1, NORMAL),
+	HWCTX_REGINFO(0x020,  1, STASH),
+	HWCTX_REGINFO(0x024,  2, NORMAL),
+	HWCTX_REGINFO(0x0e6,  1, NORMAL),
+	HWCTX_REGINFO(0x3fc,  1, NORMAL),
+	HWCTX_REGINFO(0x3d0,  1, NORMAL),
+	HWCTX_REGINFO(0x3d4,  1, NORMAL),
+	HWCTX_REGINFO(0x013,  1, NORMAL),
+	HWCTX_REGINFO(0x022,  1, NORMAL),
+	HWCTX_REGINFO(0x030,  4, NORMAL),
+	HWCTX_REGINFO(0x023,  1, NORMAL),
+	HWCTX_REGINFO(0x070,  1, NORMAL),
+	HWCTX_REGINFO(0x0a0,  9, NORMAL),
+	HWCTX_REGINFO(0x071,  1, NORMAL),
+	HWCTX_REGINFO(0x100,  4, NORMAL),
+	HWCTX_REGINFO(0x104,  2, NORMAL),
+	HWCTX_REGINFO(0x108,  9, NORMAL),
+	HWCTX_REGINFO(0x112,  2, NORMAL),
+	HWCTX_REGINFO(0x114,  1, STASH),
+	HWCTX_REGINFO(0x014,  1, NORMAL),
+	HWCTX_REGINFO(0x072,  1, NORMAL),
+	HWCTX_REGINFO(0x200,  1, NORMAL),
+	HWCTX_REGINFO(0x0d1,  1, NORMAL),
+	HWCTX_REGINFO(0x0d0,  1, NORMAL),
+	HWCTX_REGINFO(0x0c0,  1, NORMAL),
+	HWCTX_REGINFO(0x0c3,  2, NORMAL),
+	HWCTX_REGINFO(0x0d2,  1, NORMAL),
+	HWCTX_REGINFO(0x0d8,  1, NORMAL),
+	HWCTX_REGINFO(0x0e0,  2, NORMAL),
+	HWCTX_REGINFO(0x07f,  2, NORMAL),
+	HWCTX_REGINFO(0x084,  8, NORMAL),
+	HWCTX_REGINFO(0x0d3,  1, NORMAL),
+	HWCTX_REGINFO(0x040, 13, NORMAL),
+	HWCTX_REGINFO(0x050,  6, NORMAL),
+	HWCTX_REGINFO(0x058,  1, NORMAL),
+	HWCTX_REGINFO(0x057,  1, NORMAL),
+	HWCTX_REGINFO(0x111,  1, NORMAL),
+	HWCTX_REGINFO(0x130,  3, NORMAL),
+	HWCTX_REGINFO(0x201,  1, NORMAL),
+	HWCTX_REGINFO(0x068,  2, NORMAL),
+	HWCTX_REGINFO(0x08c,  1, NORMAL),
+	HWCTX_REGINFO(0x0cf,  1, NORMAL),
+	HWCTX_REGINFO(0x082,  2, NORMAL),
+	HWCTX_REGINFO(0x075,  1, NORMAL),
+	HWCTX_REGINFO(0x0e8,  1, NORMAL),
+	HWCTX_REGINFO(0x056,  1, NORMAL),
+	HWCTX_REGINFO(0x057,  1, NORMAL),
+	HWCTX_REGINFO(0x073,  1, CALCULATE),
+	HWCTX_REGINFO(0x074,  1, NORMAL),
+	HWCTX_REGINFO(0x075,  1, NORMAL),
+	HWCTX_REGINFO(0x076,  1, STASH),
+	HWCTX_REGINFO(0x11a,  9, NORMAL),
+	HWCTX_REGINFO(0x123,  1, NORMAL),
+	HWCTX_REGINFO(0x124,  1, NORMAL),
+	HWCTX_REGINFO(0x12a,  5, NORMAL),
+	HWCTX_REGINFO(0x12f,  1, STASH),
+	HWCTX_REGINFO(0x125,  2, NORMAL),
+	HWCTX_REGINFO(0x034,  1, NORMAL),
+	HWCTX_REGINFO(0x133,  2, NORMAL),
+	HWCTX_REGINFO(0x127,  1, NORMAL),
+	HWCTX_REGINFO(0x106,  1, WRITEBACK),
+	HWCTX_REGINFO(0x107,  1, WRITEBACK)
+};
+
+#define NR_STASHES 8
+#define NR_WRITEBACKS 2
+
+#define RC_RAM_LOAD_CMD 0x115
+#define RC_RAM_LOAD_DATA 0x116
+#define RC_RAM_READ_CMD 0x128
+#define RC_RAM_READ_DATA 0x129
+#define RC_RAM_SIZE 692
+
+#define IRFR_RAM_LOAD_CMD 0xc5
+#define IRFR_RAM_LOAD_DATA 0xc6
+#define IRFR_RAM_READ_CMD 0xcd
+#define IRFR_RAM_READ_DATA 0xce
+#define IRFR_RAM_SIZE 408
+
+struct mpe_save_info {
+	u32 in[NR_STASHES];
+	u32 out[NR_WRITEBACKS];
+	unsigned in_pos;
+	unsigned out_pos;
+	u32 h264_mode;
+};
+
+/*** restore ***/
+
+static unsigned int restore_size;
+
+static void restore_begin(struct host1x_hwctx_handler *h, u32 *ptr)
+{
+	/* set class to host */
+	ptr[0] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					host1x_uclass_incr_syncpt_base_r(), 1);
+	/* increment sync point base */
+	ptr[1] = nvhost_class_host_incr_syncpt_base(h->waitbase, 1);
+	/* set class to MPE */
+	ptr[2] = nvhost_opcode_setclass(NV_VIDEO_ENCODE_MPEG_CLASS_ID, 0, 0);
+}
+#define RESTORE_BEGIN_SIZE 3
+
+static void restore_ram(u32 *ptr, unsigned words,
+			unsigned cmd_reg, unsigned data_reg)
+{
+	ptr[0] = nvhost_opcode_imm(cmd_reg, words);
+	ptr[1] = nvhost_opcode_nonincr(data_reg, words);
+}
+#define RESTORE_RAM_SIZE 2
+
+static void restore_end(struct host1x_hwctx_handler *h, u32 *ptr)
+{
+	/* syncpt increment to track restore gather. */
+	ptr[0] = nvhost_opcode_imm_incr_syncpt(
+			host1x_uclass_incr_syncpt_cond_op_done_v(),
+			h->syncpt);
+}
+#define RESTORE_END_SIZE 1
+
+static u32 *setup_restore_regs(u32 *ptr,
+			const struct hwctx_reginfo *regs,
+			unsigned int nr_regs)
+{
+	const struct hwctx_reginfo *rend = regs + nr_regs;
+
+	for ( ; regs != rend; ++regs) {
+		u32 offset = regs->offset;
+		u32 count = regs->count;
+		*ptr++ = nvhost_opcode_incr(offset, count);
+		ptr += count;
+	}
+	return ptr;
+}
+
+static u32 *setup_restore_ram(u32 *ptr, unsigned words,
+			unsigned cmd_reg, unsigned data_reg)
+{
+	restore_ram(ptr, words, cmd_reg, data_reg);
+	return ptr + (RESTORE_RAM_SIZE + words);
+}
+
+static void setup_restore(struct host1x_hwctx_handler *h, u32 *ptr)
+{
+	restore_begin(h, ptr);
+	ptr += RESTORE_BEGIN_SIZE;
+
+	ptr = setup_restore_regs(ptr, ctxsave_regs_mpe,
+				ARRAY_SIZE(ctxsave_regs_mpe));
+
+	ptr = setup_restore_ram(ptr, RC_RAM_SIZE,
+			RC_RAM_LOAD_CMD, RC_RAM_LOAD_DATA);
+
+	ptr = setup_restore_ram(ptr, IRFR_RAM_SIZE,
+			IRFR_RAM_LOAD_CMD, IRFR_RAM_LOAD_DATA);
+
+	restore_end(h, ptr);
+
+	wmb();
+}
+
+/*** save ***/
+
+struct save_info {
+	u32 *ptr;
+	unsigned int save_count;
+	unsigned int restore_count;
+};
+
+static void save_begin(struct host1x_hwctx_handler *h, u32 *ptr)
+{
+	/* MPE: when done, increment syncpt to base+1 */
+	ptr[0] = nvhost_opcode_setclass(NV_VIDEO_ENCODE_MPEG_CLASS_ID, 0, 0);
+	ptr[1] = nvhost_opcode_imm_incr_syncpt(
+			host1x_uclass_incr_syncpt_cond_op_done_v(), h->syncpt);
+	/* host: wait for syncpt base+1 */
+	ptr[2] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					host1x_uclass_wait_syncpt_base_r(), 1);
+	ptr[3] = nvhost_class_host_wait_syncpt_base(h->syncpt, h->waitbase, 1);
+	/* host: signal context read thread to start reading */
+	ptr[4] = nvhost_opcode_imm_incr_syncpt(
+			host1x_uclass_incr_syncpt_cond_immediate_v(),
+			h->syncpt);
+}
+#define SAVE_BEGIN_SIZE 5
+
+static void save_direct(u32 *ptr, u32 start_reg, u32 count)
+{
+	ptr[0] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					host1x_uclass_indoff_r(), 1);
+	ptr[1] = nvhost_class_host_indoff_reg_read(NV_HOST_MODULE_MPE,
+						start_reg, true);
+	ptr[2] = nvhost_opcode_nonincr(host1x_uclass_inddata_r(), count);
+}
+#define SAVE_DIRECT_SIZE 3
+
+static void save_set_ram_cmd(u32 *ptr, u32 cmd_reg, u32 count)
+{
+	ptr[0] = nvhost_opcode_setclass(NV_VIDEO_ENCODE_MPEG_CLASS_ID,
+					cmd_reg, 1);
+	ptr[1] = count;
+}
+#define SAVE_SET_RAM_CMD_SIZE 2
+
+static void save_read_ram_data_nasty(u32 *ptr, u32 data_reg)
+{
+	ptr[0] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					host1x_uclass_indoff_r(), 1);
+	ptr[1] = nvhost_class_host_indoff_reg_read(NV_HOST_MODULE_MPE,
+						data_reg, false);
+	ptr[2] = nvhost_opcode_imm(host1x_uclass_inddata_r(), 0);
+	/* write junk data to avoid 'cached problem with register memory' */
+	ptr[3] = nvhost_opcode_setclass(NV_VIDEO_ENCODE_MPEG_CLASS_ID,
+					data_reg, 1);
+	ptr[4] = 0x99;
+}
+#define SAVE_READ_RAM_DATA_NASTY_SIZE 5
+
+static void save_end(struct host1x_hwctx_handler *h, u32 *ptr)
+{
+	/* Wait for context read service to finish (cpu incr 3) */
+	ptr[0] = nvhost_opcode_setclass(NV_HOST1X_CLASS_ID,
+					host1x_uclass_wait_syncpt_base_r(), 1);
+	ptr[1] = nvhost_class_host_wait_syncpt_base(h->syncpt, h->waitbase, 3);
+	/* Advance syncpoint base */
+	ptr[2] = nvhost_opcode_nonincr(host1x_uclass_incr_syncpt_base_r(), 1);
+	ptr[3] = nvhost_class_host_incr_syncpt_base(h->waitbase, 3);
+	/* set class back to the unit */
+	ptr[4] = nvhost_opcode_setclass(NV_VIDEO_ENCODE_MPEG_CLASS_ID, 0, 0);
+}
+#define SAVE_END_SIZE 5
+
+static void setup_save_regs(struct save_info *info,
+			const struct hwctx_reginfo *regs,
+			unsigned int nr_regs)
+{
+	const struct hwctx_reginfo *rend = regs + nr_regs;
+	u32 *ptr = info->ptr;
+	unsigned int save_count = info->save_count;
+	unsigned int restore_count = info->restore_count;
+
+	for ( ; regs != rend; ++regs) {
+		u32 offset = regs->offset;
+		u32 count = regs->count;
+		if (regs->type != HWCTX_REGINFO_WRITEBACK) {
+			if (ptr) {
+				save_direct(ptr, offset, count);
+				ptr += SAVE_DIRECT_SIZE;
+				memset(ptr, 0, count * 4);
+				ptr += count;
+			}
+			save_count += (SAVE_DIRECT_SIZE + count);
+		}
+		restore_count += (1 + count);
+	}
+
+	info->ptr = ptr;
+	info->save_count = save_count;
+	info->restore_count = restore_count;
+}
+
+static void setup_save_ram_nasty(struct save_info *info,	unsigned words,
+					unsigned cmd_reg, unsigned data_reg)
+{
+	u32 *ptr = info->ptr;
+	unsigned int save_count = info->save_count;
+	unsigned int restore_count = info->restore_count;
+	unsigned i;
+
+	if (ptr) {
+		save_set_ram_cmd(ptr, cmd_reg, words);
+		ptr += SAVE_SET_RAM_CMD_SIZE;
+		for (i = words; i; --i) {
+			save_read_ram_data_nasty(ptr, data_reg);
+			ptr += SAVE_READ_RAM_DATA_NASTY_SIZE;
+		}
+	}
+
+	save_count += SAVE_SET_RAM_CMD_SIZE;
+	save_count += words * SAVE_READ_RAM_DATA_NASTY_SIZE;
+	restore_count += (RESTORE_RAM_SIZE + words);
+
+	info->ptr = ptr;
+	info->save_count = save_count;
+	info->restore_count = restore_count;
+}
+
+static void setup_save(struct host1x_hwctx_handler *h, u32 *ptr)
+{
+	struct save_info info = {
+		ptr,
+		SAVE_BEGIN_SIZE,
+		RESTORE_BEGIN_SIZE
+	};
+
+	if (info.ptr) {
+		save_begin(h, info.ptr);
+		info.ptr += SAVE_BEGIN_SIZE;
+	}
+
+	setup_save_regs(&info, ctxsave_regs_mpe,
+			ARRAY_SIZE(ctxsave_regs_mpe));
+
+	setup_save_ram_nasty(&info, RC_RAM_SIZE,
+			RC_RAM_READ_CMD, RC_RAM_READ_DATA);
+
+	setup_save_ram_nasty(&info, IRFR_RAM_SIZE,
+			IRFR_RAM_READ_CMD, IRFR_RAM_READ_DATA);
+
+	if (info.ptr) {
+		save_end(h, info.ptr);
+		info.ptr += SAVE_END_SIZE;
+	}
+
+	wmb();
+
+	h->save_size = info.save_count + SAVE_END_SIZE;
+	restore_size = info.restore_count + RESTORE_END_SIZE;
+}
+
+static u32 calculate_mpe(u32 word, struct mpe_save_info *msi)
+{
+	u32 buffer_full_read = msi->in[0] & 0x01ffffff;
+	u32 byte_len = msi->in[1];
+	u32 drain = (msi->in[2] >> 2) & 0x007fffff;
+	u32 rep_frame = msi->in[3] & 0x0000ffff;
+	u32 h264_mode = (msi->in[4] >> 11) & 1;
+	int new_buffer_full;
+
+	if (h264_mode)
+		byte_len >>= 3;
+	new_buffer_full = buffer_full_read + byte_len - (drain * 4);
+	msi->out[0] = max(0, new_buffer_full);
+	msi->out[1] = rep_frame;
+	if (rep_frame == 0)
+		word &= 0xffff0000;
+	return word;
+}
+
+static u32 *save_regs(u32 *ptr, unsigned int *pending,
+		struct nvhost_channel *channel,
+		const struct hwctx_reginfo *regs,
+		unsigned int nr_regs,
+		struct mpe_save_info *msi)
+{
+	const struct hwctx_reginfo *rend = regs + nr_regs;
+
+	for ( ; regs != rend; ++regs) {
+		u32 count = regs->count;
+		++ptr; /* restore incr */
+		if (regs->type == HWCTX_REGINFO_NORMAL) {
+			nvhost_channel_drain_read_fifo(channel,
+						ptr, count, pending);
+			ptr += count;
+		} else {
+			u32 word;
+			if (regs->type == HWCTX_REGINFO_WRITEBACK) {
+				BUG_ON(msi->out_pos >= NR_WRITEBACKS);
+				word = msi->out[msi->out_pos++];
+			} else {
+				nvhost_channel_drain_read_fifo(channel,
+						&word, 1, pending);
+				if (regs->type == HWCTX_REGINFO_STASH) {
+					BUG_ON(msi->in_pos >= NR_STASHES);
+					msi->in[msi->in_pos++] = word;
+				} else {
+					word = calculate_mpe(word, msi);
+				}
+			}
+			*ptr++ = word;
+		}
+	}
+	return ptr;
+}
+
+static u32 *save_ram(u32 *ptr, unsigned int *pending,
+		struct nvhost_channel *channel,
+		unsigned words,	unsigned cmd_reg, unsigned data_reg)
+{
+	int err = 0;
+	ptr += RESTORE_RAM_SIZE;
+	err = nvhost_channel_drain_read_fifo(channel, ptr, words, pending);
+	WARN_ON(err);
+	return ptr + words;
+}
+
+/*** ctxmpe ***/
+
+static struct nvhost_hwctx *ctxmpe_alloc(struct nvhost_hwctx_handler *h,
+		struct nvhost_channel *ch)
+{
+	struct mem_mgr *memmgr = nvhost_get_host(ch->dev)->memmgr;
+	struct host1x_hwctx_handler *p = to_host1x_hwctx_handler(h);
+	struct host1x_hwctx *ctx;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return NULL;
+	ctx->restore = mem_op().alloc(memmgr, restore_size * 4, 32,
+				mem_mgr_flag_write_combine);
+	if (IS_ERR_OR_NULL(ctx->restore)) {
+		kfree(ctx);
+		return NULL;
+	}
+
+	ctx->restore_virt = mem_op().mmap(ctx->restore);
+	if (!ctx->restore_virt) {
+		mem_op().put(memmgr, ctx->restore);
+		kfree(ctx);
+		return NULL;
+	}
+
+	kref_init(&ctx->hwctx.ref);
+	ctx->hwctx.h = &p->h;
+	ctx->hwctx.channel = ch;
+	ctx->hwctx.valid = false;
+	ctx->save_incrs = 3;
+	ctx->save_thresh = 2;
+	ctx->save_slots = p->save_slots;
+	ctx->restore_phys = mem_op().pin(memmgr, ctx->restore);
+	ctx->restore_size = restore_size;
+	ctx->restore_incrs = 1;
+
+	setup_restore(p, ctx->restore_virt);
+
+	return &ctx->hwctx;
+}
+
+static void ctxmpe_get(struct nvhost_hwctx *ctx)
+{
+	kref_get(&ctx->ref);
+}
+
+static void ctxmpe_free(struct kref *ref)
+{
+	struct nvhost_hwctx *nctx = container_of(ref, struct nvhost_hwctx, ref);
+	struct host1x_hwctx *ctx = to_host1x_hwctx(nctx);
+	struct mem_mgr *memmgr = nvhost_get_host(nctx->channel->dev)->memmgr;
+
+	if (ctx->restore_virt)
+		mem_op().munmap(ctx->restore, ctx->restore_virt);
+	mem_op().unpin(memmgr, ctx->restore);
+	mem_op().put(memmgr, ctx->restore);
+	kfree(ctx);
+}
+
+static void ctxmpe_put(struct nvhost_hwctx *ctx)
+{
+	kref_put(&ctx->ref, ctxmpe_free);
+}
+
+static void ctxmpe_save_push(struct nvhost_hwctx *nctx,
+		struct nvhost_cdma *cdma)
+{
+	struct host1x_hwctx *ctx = to_host1x_hwctx(nctx);
+	struct host1x_hwctx_handler *h = host1x_hwctx_handler(ctx);
+	nvhost_cdma_push_gather(cdma,
+			nvhost_get_host(nctx->channel->dev)->memmgr,
+			h->save_buf,
+			0,
+			nvhost_opcode_gather(h->save_size),
+			h->save_phys);
+}
+
+static void ctxmpe_save_service(struct nvhost_hwctx *nctx)
+{
+	struct host1x_hwctx *ctx = to_host1x_hwctx(nctx);
+	struct host1x_hwctx_handler *h = host1x_hwctx_handler(ctx);
+
+	u32 *ptr = (u32 *)ctx->restore_virt + RESTORE_BEGIN_SIZE;
+	unsigned int pending = 0;
+	struct mpe_save_info msi;
+
+	msi.in_pos = 0;
+	msi.out_pos = 0;
+
+	ptr = save_regs(ptr, &pending, nctx->channel,
+			ctxsave_regs_mpe, ARRAY_SIZE(ctxsave_regs_mpe), &msi);
+
+	ptr = save_ram(ptr, &pending, nctx->channel,
+		RC_RAM_SIZE, RC_RAM_READ_CMD, RC_RAM_READ_DATA);
+
+	ptr = save_ram(ptr, &pending, nctx->channel,
+		IRFR_RAM_SIZE, IRFR_RAM_READ_CMD, IRFR_RAM_READ_DATA);
+
+	wmb();
+	nvhost_syncpt_cpu_incr(&nvhost_get_host(nctx->channel->dev)->syncpt,
+			h->syncpt);
+}
+
+struct nvhost_hwctx_handler *nvhost_mpe_ctxhandler_init(u32 syncpt,
+	u32 waitbase, struct nvhost_channel *ch)
+{
+	struct mem_mgr *memmgr;
+	u32 *save_ptr;
+	struct host1x_hwctx_handler *p;
+
+	p = kmalloc(sizeof(*p), GFP_KERNEL);
+	if (!p)
+		return NULL;
+
+	memmgr = nvhost_get_host(ch->dev)->memmgr;
+
+	p->syncpt = syncpt;
+	p->waitbase = waitbase;
+
+	setup_save(p, NULL);
+
+	p->save_buf = mem_op().alloc(memmgr, p->save_size * 4, 32,
+				mem_mgr_flag_write_combine);
+	if (IS_ERR_OR_NULL(p->save_buf)) {
+		p->save_buf = NULL;
+		return NULL;
+	}
+
+	save_ptr = mem_op().mmap(p->save_buf);
+	if (!save_ptr) {
+		mem_op().put(memmgr, p->save_buf);
+		p->save_buf = NULL;
+		return NULL;
+	}
+
+	p->save_phys = mem_op().pin(memmgr, p->save_buf);
+	p->save_slots = 1;
+
+	setup_save(p, save_ptr);
+
+	p->h.alloc = ctxmpe_alloc;
+	p->h.save_push = ctxmpe_save_push;
+	p->h.save_service = ctxmpe_save_service;
+	p->h.get = ctxmpe_get;
+	p->h.put = ctxmpe_put;
+
+	return &p->h;
+}
+
+int nvhost_mpe_prepare_power_off(struct nvhost_device *dev)
+{
+	return nvhost_channel_save_context(dev->channel);
+}
+
+enum mpe_ip_ver {
+	mpe_01 = 1,
+	mpe_02,
+};
+
+struct mpe_desc {
+	int (*prepare_poweroff)(struct nvhost_device *dev);
+	struct nvhost_hwctx_handler *(*alloc_hwctx_handler)(u32 syncpt,
+			u32 waitbase, struct nvhost_channel *ch);
+};
+
+static const struct mpe_desc mpe[] = {
+	[mpe_01] = {
+		.prepare_poweroff = nvhost_mpe_prepare_power_off,
+		.alloc_hwctx_handler = nvhost_mpe_ctxhandler_init,
+	},
+	[mpe_02] = {
+		.prepare_poweroff = nvhost_mpe_prepare_power_off,
+		.alloc_hwctx_handler = nvhost_mpe_ctxhandler_init,
+	},
+};
+
+static struct nvhost_device_id mpe_id[] = {
+	{ "mpe", mpe_01 },
+	{ "mpe", mpe_02 },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(nvhost, mpe_id);
+
+static int __devinit mpe_probe(struct nvhost_device *dev,
+	struct nvhost_device_id *id_table)
+{
+	int err = 0;
+	int index = 0;
+	struct nvhost_driver *drv = to_nvhost_driver(dev->dev.driver);
+
+	index = id_table->version;
+
+	drv->prepare_poweroff		= mpe[index].prepare_poweroff;
+	drv->alloc_hwctx_handler	= mpe[index].alloc_hwctx_handler;
+
+	err = nvhost_client_device_get_resources(dev);
+	if (err)
+		return err;
+
+	return nvhost_client_device_init(dev);
+}
+
+static int __exit mpe_remove(struct nvhost_device *dev)
+{
+	/* Add clean-up */
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int mpe_suspend(struct nvhost_device *dev, pm_message_t state)
+{
+	return nvhost_client_device_suspend(dev);
+}
+
+static int mpe_resume(struct nvhost_device *dev)
+{
+	dev_info(&dev->dev, "resuming\n");
+	return 0;
+}
+#endif
+
+static struct nvhost_driver mpe_driver = {
+	.probe = mpe_probe,
+	.remove = __exit_p(mpe_remove),
+#ifdef CONFIG_PM
+	.suspend = mpe_suspend,
+	.resume = mpe_resume,
+#endif
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "mpe",
+	},
+	.id_table = mpe_id,
+};
+
+static int __init mpe_init(void)
+{
+	return nvhost_driver_register(&mpe_driver);
+}
+
+static void __exit mpe_exit(void)
+{
+	nvhost_driver_unregister(&mpe_driver);
+}
+
+module_init(mpe_init);
+module_exit(mpe_exit);
diff --git a/drivers/video/tegra/host/mpe/mpe.h b/drivers/video/tegra/host/mpe/mpe.h
new file mode 100644
index 000000000000..1bc2a8a04c1a
--- /dev/null
+++ b/drivers/video/tegra/host/mpe/mpe.h
@@ -0,0 +1,32 @@
+/*
+ * drivers/video/tegra/host/mpe/mpe.h
+ *
+ * Tegra Graphics Host MPE
+ *
+ * Copyright (c) 2011-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_MPE_MPE_H
+#define __NVHOST_MPE_MPE_H
+
+struct nvhost_hwctx_handler;
+struct nvhost_device;
+
+struct nvhost_hwctx_handler *nvhost_mpe_ctxhandler_init(
+		u32 syncpt, u32 waitbase,
+		struct nvhost_channel *ch);
+int nvhost_mpe_prepare_power_off(struct nvhost_device *dev);
+
+#endif
diff --git a/drivers/video/tegra/host/nvhost_acm.c b/drivers/video/tegra/host/nvhost_acm.c
new file mode 100644
index 000000000000..860ce6b35572
--- /dev/null
+++ b/drivers/video/tegra/host/nvhost_acm.c
@@ -0,0 +1,649 @@
+/*
+ * drivers/video/tegra/host/nvhost_acm.c
+ *
+ * Tegra Graphics Host Automatic Clock Management
+ *
+ * Copyright (c) 2010-2014, NVIDIA 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "nvhost_acm.h"
+#include "dev.h"
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/sched.h>
+#include <linux/err.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <mach/powergate.h>
+#include <mach/clk.h>
+#include <mach/hardware.h>
+
+#define ACM_SUSPEND_WAIT_FOR_IDLE_TIMEOUT	(2 * HZ)
+#define POWERGATE_DELAY 			10
+#define MAX_DEVID_LENGTH			16
+
+DEFINE_MUTEX(client_list_lock);
+
+struct nvhost_module_client {
+	struct list_head node;
+	unsigned long rate[NVHOST_MODULE_MAX_CLOCKS];
+	void *priv;
+};
+
+static void do_powergate_locked(int id)
+{
+	if (id != -1 && tegra_powergate_is_powered(id))
+		tegra_powergate_partition(id);
+}
+
+static void do_unpowergate_locked(int id)
+{
+	if (id != -1)
+		tegra_unpowergate_partition(id);
+}
+
+static void do_module_reset_locked(struct nvhost_device *dev)
+{
+	/* assert module and mc client reset */
+	if (dev->powergate_ids[0] != -1) {
+		tegra_powergate_mc_disable(dev->powergate_ids[0]);
+		tegra_periph_reset_assert(dev->clk[0]);
+		tegra_powergate_mc_flush(dev->powergate_ids[0]);
+	}
+	if (dev->powergate_ids[1] != -1) {
+		tegra_powergate_mc_disable(dev->powergate_ids[1]);
+		tegra_periph_reset_assert(dev->clk[1]);
+		tegra_powergate_mc_flush(dev->powergate_ids[1]);
+	}
+
+	udelay(POWERGATE_DELAY);
+
+	/* deassert reset */
+	if (dev->powergate_ids[0] != -1) {
+		tegra_powergate_mc_flush_done(dev->powergate_ids[0]);
+		tegra_periph_reset_deassert(dev->clk[0]);
+		tegra_powergate_mc_enable(dev->powergate_ids[0]);
+	}
+	if (dev->powergate_ids[1] != -1) {
+		tegra_powergate_mc_flush_done(dev->powergate_ids[1]);
+		tegra_periph_reset_deassert(dev->clk[1]);
+		tegra_powergate_mc_enable(dev->powergate_ids[1]);
+	}
+}
+
+void nvhost_module_reset(struct nvhost_device *dev)
+{
+	dev_dbg(&dev->dev,
+		"%s: asserting %s module reset (id %d, id2 %d)\n",
+		__func__, dev->name,
+		dev->powergate_ids[0], dev->powergate_ids[1]);
+
+	mutex_lock(&dev->lock);
+	do_module_reset_locked(dev);
+	mutex_unlock(&dev->lock);
+
+	dev_dbg(&dev->dev, "%s: module %s out of reset\n",
+		__func__, dev->name);
+}
+
+static void to_state_clockgated_locked(struct nvhost_device *dev)
+{
+	struct nvhost_driver *drv = to_nvhost_driver(dev->dev.driver);
+
+	if (dev->powerstate == NVHOST_POWER_STATE_RUNNING) {
+		int i, err;
+		if (drv->prepare_clockoff) {
+			err = drv->prepare_clockoff(dev);
+			if (err) {
+				dev_err(&dev->dev, "error clock gating");
+				return;
+			}
+		}
+		for (i = 0; i < dev->num_clks; i++)
+			clk_disable(dev->clk[i]);
+		if (dev->dev.parent)
+			nvhost_module_idle(to_nvhost_device(dev->dev.parent));
+	} else if (dev->powerstate == NVHOST_POWER_STATE_POWERGATED
+			&& dev->can_powergate) {
+		do_unpowergate_locked(dev->powergate_ids[0]);
+		do_unpowergate_locked(dev->powergate_ids[1]);
+
+		if (dev->powerup_reset)
+			do_module_reset_locked(dev);
+	}
+	dev->powerstate = NVHOST_POWER_STATE_CLOCKGATED;
+}
+
+static void to_state_running_locked(struct nvhost_device *dev)
+{
+	struct nvhost_driver *drv = to_nvhost_driver(dev->dev.driver);
+	int prev_state = dev->powerstate;
+
+	if (dev->powerstate == NVHOST_POWER_STATE_POWERGATED)
+		to_state_clockgated_locked(dev);
+
+	if (dev->powerstate == NVHOST_POWER_STATE_CLOCKGATED) {
+		int i;
+
+		if (dev->dev.parent)
+			nvhost_module_busy(to_nvhost_device(dev->dev.parent));
+
+		for (i = 0; i < dev->num_clks; i++) {
+			int err = clk_enable(dev->clk[i]);
+			if (err) {
+				dev_err(&dev->dev, "Cannot turn on clock %s",
+					dev->clocks[i].name);
+				return;
+			}
+		}
+
+		/* Invoke callback after enabling clock. This is used for
+		 * re-enabling host1x interrupts. */
+		if (prev_state == NVHOST_POWER_STATE_CLOCKGATED
+				&& drv->finalize_clockon)
+			drv->finalize_clockon(dev);
+
+		/* Invoke callback after power un-gating. This is used for
+		 * restoring context. */
+		if (prev_state == NVHOST_POWER_STATE_POWERGATED
+				&& drv->finalize_poweron)
+			drv->finalize_poweron(dev);
+	}
+	dev->powerstate = NVHOST_POWER_STATE_RUNNING;
+}
+
+/* This gets called from powergate_handler() and from module suspend.
+ * Module suspend is done for all modules, runtime power gating only
+ * for modules with can_powergate set.
+ */
+static int to_state_powergated_locked(struct nvhost_device *dev)
+{
+	int err = 0;
+	struct nvhost_driver *drv = to_nvhost_driver(dev->dev.driver);
+
+	if (drv->prepare_poweroff
+			&& dev->powerstate != NVHOST_POWER_STATE_POWERGATED) {
+		/* Clock needs to be on in prepare_poweroff */
+		to_state_running_locked(dev);
+		err = drv->prepare_poweroff(dev);
+		if (err)
+			return err;
+	}
+
+	if (dev->powerstate == NVHOST_POWER_STATE_RUNNING)
+		to_state_clockgated_locked(dev);
+
+	if (dev->can_powergate) {
+		do_powergate_locked(dev->powergate_ids[0]);
+		do_powergate_locked(dev->powergate_ids[1]);
+	}
+
+	dev->powerstate = NVHOST_POWER_STATE_POWERGATED;
+	return 0;
+}
+
+static void schedule_powergating_locked(struct nvhost_device *dev)
+{
+	if (dev->can_powergate)
+		schedule_delayed_work(&dev->powerstate_down,
+				msecs_to_jiffies(dev->powergate_delay));
+}
+
+static void schedule_clockgating_locked(struct nvhost_device *dev)
+{
+	schedule_delayed_work(&dev->powerstate_down,
+			msecs_to_jiffies(dev->clockgate_delay));
+}
+
+void nvhost_module_busy(struct nvhost_device *dev)
+{
+	struct nvhost_driver *drv = to_nvhost_driver(dev->dev.driver);
+
+	if (drv->busy)
+		drv->busy(dev);
+
+	mutex_lock(&dev->lock);
+	cancel_delayed_work(&dev->powerstate_down);
+
+	dev->refcount++;
+	if (dev->refcount > 0 && !nvhost_module_powered(dev))
+		to_state_running_locked(dev);
+
+	mutex_unlock(&dev->lock);
+}
+
+static void powerstate_down_handler(struct work_struct *work)
+{
+	struct nvhost_device *dev;
+
+	dev = container_of(to_delayed_work(work),
+			struct nvhost_device,
+			powerstate_down);
+
+	mutex_lock(&dev->lock);
+	if (dev->refcount == 0) {
+		switch (dev->powerstate) {
+		case NVHOST_POWER_STATE_RUNNING:
+			to_state_clockgated_locked(dev);
+			schedule_powergating_locked(dev);
+			break;
+		case NVHOST_POWER_STATE_CLOCKGATED:
+			if (to_state_powergated_locked(dev))
+				schedule_powergating_locked(dev);
+			break;
+		default:
+			break;
+		}
+	}
+	mutex_unlock(&dev->lock);
+}
+
+void nvhost_module_idle_mult(struct nvhost_device *dev, int refs)
+{
+	struct nvhost_driver *drv = to_nvhost_driver(dev->dev.driver);
+	bool kick = false;
+
+	mutex_lock(&dev->lock);
+	dev->refcount -= refs;
+	if (dev->refcount == 0) {
+		if (nvhost_module_powered(dev))
+			schedule_clockgating_locked(dev);
+		kick = true;
+	}
+	mutex_unlock(&dev->lock);
+
+	if (kick) {
+		wake_up(&dev->idle_wq);
+
+		if (drv->idle)
+			drv->idle(dev);
+	}
+}
+
+int nvhost_module_get_rate(struct nvhost_device *dev, unsigned long *rate,
+		int index)
+{
+	struct clk *c;
+
+	c = dev->clk[index];
+	if (IS_ERR_OR_NULL(c))
+		return -EINVAL;
+
+	/* Need to enable client to get correct rate */
+	nvhost_module_busy(dev);
+	*rate = clk_get_rate(c);
+	nvhost_module_idle(dev);
+	return 0;
+
+}
+
+static int nvhost_module_update_rate(struct nvhost_device *dev, int index)
+{
+	unsigned long rate = 0;
+	struct nvhost_module_client *m;
+
+	if (!dev->clk[index])
+		return -EINVAL;
+
+	list_for_each_entry(m, &dev->client_list, node) {
+		rate = max(m->rate[index], rate);
+	}
+	if (!rate)
+		rate = clk_round_rate(dev->clk[index],
+				dev->clocks[index].default_rate);
+
+	return clk_set_rate(dev->clk[index], rate);
+}
+
+int nvhost_module_set_rate(struct nvhost_device *dev, void *priv,
+		unsigned long rate, int index)
+{
+	struct nvhost_module_client *m;
+	int i, ret = 0;
+
+	mutex_lock(&client_list_lock);
+	list_for_each_entry(m, &dev->client_list, node) {
+		if (m->priv == priv) {
+			for (i = 0; i < dev->num_clks; i++)
+				m->rate[i] = clk_round_rate(dev->clk[i], rate);
+			break;
+		}
+	}
+
+	for (i = 0; i < dev->num_clks; i++) {
+		ret = nvhost_module_update_rate(dev, i);
+		if (ret < 0)
+			break;
+	}
+	mutex_unlock(&client_list_lock);
+	return ret;
+
+}
+
+int nvhost_module_add_client(struct nvhost_device *dev, void *priv)
+{
+	int i;
+	unsigned long rate;
+	struct nvhost_module_client *client;
+
+	client = kzalloc(sizeof(*client), GFP_KERNEL);
+	if (!client)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&client->node);
+	client->priv = priv;
+
+	for (i = 0; i < dev->num_clks; i++) {
+		rate = clk_round_rate(dev->clk[i],
+				dev->clocks[i].default_rate);
+		client->rate[i] = rate;
+	}
+	mutex_lock(&client_list_lock);
+	list_add_tail(&client->node, &dev->client_list);
+	mutex_unlock(&client_list_lock);
+	return 0;
+}
+
+void nvhost_module_remove_client(struct nvhost_device *dev, void *priv)
+{
+	int i;
+	struct nvhost_module_client *m;
+	int found = 0;
+
+	mutex_lock(&client_list_lock);
+	list_for_each_entry(m, &dev->client_list, node) {
+		if (priv == m->priv) {
+			list_del(&m->node);
+			found = 1;
+			break;
+		}
+	}
+	if (found) {
+		kfree(m);
+		for (i = 0; i < dev->num_clks; i++)
+			nvhost_module_update_rate(dev, i);
+	}
+	mutex_unlock(&client_list_lock);
+}
+
+static ssize_t refcount_show(struct kobject *kobj,
+	struct kobj_attribute *attr, char *buf)
+{
+	int ret;
+	struct nvhost_device_power_attr *power_attribute =
+		container_of(attr, struct nvhost_device_power_attr, \
+			power_attr[NVHOST_POWER_SYSFS_ATTRIB_REFCOUNT]);
+	struct nvhost_device *dev = power_attribute->ndev;
+
+	mutex_lock(&dev->lock);
+	ret = sprintf(buf, "%d\n", dev->refcount);
+	mutex_unlock(&dev->lock);
+
+	return ret;
+}
+
+static ssize_t powergate_delay_store(struct kobject *kobj,
+	struct kobj_attribute *attr, const char *buf, size_t count)
+{
+	int powergate_delay = 0, ret = 0;
+	struct nvhost_device_power_attr *power_attribute =
+		container_of(attr, struct nvhost_device_power_attr, \
+			power_attr[NVHOST_POWER_SYSFS_ATTRIB_POWERGATE_DELAY]);
+	struct nvhost_device *dev = power_attribute->ndev;
+
+	if (!dev->can_powergate) {
+		dev_info(&dev->dev, "does not support power-gating\n");
+		return count;
+	}
+
+	mutex_lock(&dev->lock);
+	ret = sscanf(buf, "%d", &powergate_delay);
+	if (ret == 1 && powergate_delay >= 0)
+		dev->powergate_delay = powergate_delay;
+	else
+		dev_err(&dev->dev, "Invalid powergate delay\n");
+	mutex_unlock(&dev->lock);
+
+	return count;
+}
+
+static ssize_t powergate_delay_show(struct kobject *kobj,
+	struct kobj_attribute *attr, char *buf)
+{
+	int ret;
+	struct nvhost_device_power_attr *power_attribute =
+		container_of(attr, struct nvhost_device_power_attr, \
+			power_attr[NVHOST_POWER_SYSFS_ATTRIB_POWERGATE_DELAY]);
+	struct nvhost_device *dev = power_attribute->ndev;
+
+	mutex_lock(&dev->lock);
+	ret = sprintf(buf, "%d\n", dev->powergate_delay);
+	mutex_unlock(&dev->lock);
+
+	return ret;
+}
+
+static ssize_t clockgate_delay_store(struct kobject *kobj,
+	struct kobj_attribute *attr, const char *buf, size_t count)
+{
+	int clockgate_delay = 0, ret = 0;
+	struct nvhost_device_power_attr *power_attribute =
+		container_of(attr, struct nvhost_device_power_attr, \
+			power_attr[NVHOST_POWER_SYSFS_ATTRIB_CLOCKGATE_DELAY]);
+	struct nvhost_device *dev = power_attribute->ndev;
+
+	mutex_lock(&dev->lock);
+	ret = sscanf(buf, "%d", &clockgate_delay);
+	if (ret == 1 && clockgate_delay >= 0)
+		dev->clockgate_delay = clockgate_delay;
+	else
+		dev_err(&dev->dev, "Invalid clockgate delay\n");
+	mutex_unlock(&dev->lock);
+
+	return count;
+}
+
+static ssize_t clockgate_delay_show(struct kobject *kobj,
+	struct kobj_attribute *attr, char *buf)
+{
+	int ret;
+	struct nvhost_device_power_attr *power_attribute =
+		container_of(attr, struct nvhost_device_power_attr, \
+			power_attr[NVHOST_POWER_SYSFS_ATTRIB_CLOCKGATE_DELAY]);
+	struct nvhost_device *dev = power_attribute->ndev;
+
+	mutex_lock(&dev->lock);
+	ret = sprintf(buf, "%d\n", dev->clockgate_delay);
+	mutex_unlock(&dev->lock);
+
+	return ret;
+}
+
+int nvhost_module_init(struct nvhost_device *dev)
+{
+	int i = 0, err = 0;
+	struct kobj_attribute *attr = NULL;
+
+	/* initialize clocks to known state */
+	INIT_LIST_HEAD(&dev->client_list);
+	while (dev->clocks[i].name && i < NVHOST_MODULE_MAX_CLOCKS) {
+		char devname[MAX_DEVID_LENGTH];
+		long rate = dev->clocks[i].default_rate;
+		struct clk *c;
+
+		snprintf(devname, MAX_DEVID_LENGTH, "tegra_%s", dev->name);
+		c = clk_get_sys(devname, dev->clocks[i].name);
+		if (IS_ERR_OR_NULL(c)) {
+			dev_err(&dev->dev, "Cannot get clock %s\n",
+					dev->clocks[i].name);
+			continue;
+		}
+
+		rate = clk_round_rate(c, rate);
+		clk_enable(c);
+		clk_set_rate(c, rate);
+		clk_disable(c);
+		dev->clk[i] = c;
+		i++;
+	}
+	dev->num_clks = i;
+
+	mutex_init(&dev->lock);
+	init_waitqueue_head(&dev->idle_wq);
+	INIT_DELAYED_WORK(&dev->powerstate_down, powerstate_down_handler);
+
+	/* power gate units that we can power gate */
+	if (dev->can_powergate) {
+		do_powergate_locked(dev->powergate_ids[0]);
+		do_powergate_locked(dev->powergate_ids[1]);
+		dev->powerstate = NVHOST_POWER_STATE_POWERGATED;
+	} else {
+		do_unpowergate_locked(dev->powergate_ids[0]);
+		do_unpowergate_locked(dev->powergate_ids[1]);
+		dev->powerstate = NVHOST_POWER_STATE_CLOCKGATED;
+	}
+
+	/* Init the power sysfs attributes for this device */
+	dev->power_attrib = kzalloc(sizeof(struct nvhost_device_power_attr),
+		GFP_KERNEL);
+	if (!dev->power_attrib) {
+		dev_err(&dev->dev, "Unable to allocate sysfs attributes\n");
+		return -ENOMEM;
+	}
+	dev->power_attrib->ndev = dev;
+
+	dev->power_kobj = kobject_create_and_add("acm", &dev->dev.kobj);
+	if (!dev->power_kobj) {
+		dev_err(&dev->dev, "Could not add dir 'power'\n");
+		err = -EIO;
+		goto fail_attrib_alloc;
+	}
+
+	attr = &dev->power_attrib->power_attr[NVHOST_POWER_SYSFS_ATTRIB_CLOCKGATE_DELAY];
+	attr->attr.name = "clockgate_delay";
+	attr->attr.mode = S_IWUSR | S_IRUGO;
+	attr->show = clockgate_delay_show;
+	attr->store = clockgate_delay_store;
+	if (sysfs_create_file(dev->power_kobj, &attr->attr)) {
+		dev_err(&dev->dev, "Could not create sysfs attribute clockgate_delay\n");
+		err = -EIO;
+		goto fail_clockdelay;
+	}
+
+	attr = &dev->power_attrib->power_attr[NVHOST_POWER_SYSFS_ATTRIB_POWERGATE_DELAY];
+	attr->attr.name = "powergate_delay";
+	attr->attr.mode = S_IWUSR | S_IRUGO;
+	attr->show = powergate_delay_show;
+	attr->store = powergate_delay_store;
+	if (sysfs_create_file(dev->power_kobj, &attr->attr)) {
+		dev_err(&dev->dev, "Could not create sysfs attribute powergate_delay\n");
+		err = -EIO;
+		goto fail_powergatedelay;
+	}
+
+	attr = &dev->power_attrib->power_attr[NVHOST_POWER_SYSFS_ATTRIB_REFCOUNT];
+	attr->attr.name = "refcount";
+	attr->attr.mode = S_IRUGO;
+	attr->show = refcount_show;
+	if (sysfs_create_file(dev->power_kobj, &attr->attr)) {
+		dev_err(&dev->dev, "Could not create sysfs attribute refcount\n");
+		err = -EIO;
+		goto fail_refcount;
+	}
+
+	return 0;
+
+fail_refcount:
+	attr = &dev->power_attrib->power_attr[NVHOST_POWER_SYSFS_ATTRIB_POWERGATE_DELAY];
+	sysfs_remove_file(dev->power_kobj, &attr->attr);
+
+fail_powergatedelay:
+	attr = &dev->power_attrib->power_attr[NVHOST_POWER_SYSFS_ATTRIB_CLOCKGATE_DELAY];
+	sysfs_remove_file(dev->power_kobj, &attr->attr);
+
+fail_clockdelay:
+	kobject_put(dev->power_kobj);
+
+fail_attrib_alloc:
+	kfree(dev->power_attrib);
+
+	return err;
+}
+
+static int is_module_idle(struct nvhost_device *dev)
+{
+	int count;
+	mutex_lock(&dev->lock);
+	count = dev->refcount;
+	mutex_unlock(&dev->lock);
+	return (count == 0);
+}
+
+int nvhost_module_suspend(struct nvhost_device *dev)
+{
+	int ret;
+	struct nvhost_driver *drv = to_nvhost_driver(dev->dev.driver);
+
+	ret = wait_event_timeout(dev->idle_wq, is_module_idle(dev),
+			ACM_SUSPEND_WAIT_FOR_IDLE_TIMEOUT);
+	if (ret == 0) {
+		dev_info(&dev->dev, "%s prevented suspend\n",
+				dev->name);
+		return -EBUSY;
+	}
+
+	mutex_lock(&dev->lock);
+	cancel_delayed_work(&dev->powerstate_down);
+	to_state_powergated_locked(dev);
+	mutex_unlock(&dev->lock);
+
+	if (drv->suspend_ndev)
+		drv->suspend_ndev(dev);
+
+	return 0;
+}
+
+void nvhost_module_deinit(struct nvhost_device *dev)
+{
+	int i;
+	struct nvhost_driver *drv = to_nvhost_driver(dev->dev.driver);
+
+	if (drv->deinit)
+		drv->deinit(dev);
+
+	nvhost_module_suspend(dev);
+	for (i = 0; i < dev->num_clks; i++)
+		clk_put(dev->clk[i]);
+	dev->powerstate = NVHOST_POWER_STATE_DEINIT;
+}
+
+/* public host1x power management APIs */
+bool nvhost_module_powered_ext(struct nvhost_device *dev)
+{
+	return nvhost_module_powered(dev);
+}
+
+void nvhost_module_busy_ext(struct nvhost_device *dev)
+{
+	nvhost_module_busy(dev);
+}
+EXPORT_SYMBOL(nvhost_module_busy_ext);
+
+void nvhost_module_idle_ext(struct nvhost_device *dev)
+{
+	nvhost_module_idle(dev);
+}
+EXPORT_SYMBOL(nvhost_module_idle_ext);
diff --git a/drivers/video/tegra/host/nvhost_acm.h b/drivers/video/tegra/host/nvhost_acm.h
new file mode 100644
index 000000000000..a5894dcfc0b2
--- /dev/null
+++ b/drivers/video/tegra/host/nvhost_acm.h
@@ -0,0 +1,58 @@
+/*
+ * drivers/video/tegra/host/nvhost_acm.h
+ *
+ * Tegra Graphics Host Automatic Clock Management
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_ACM_H
+#define __NVHOST_ACM_H
+
+#include <linux/workqueue.h>
+#include <linux/wait.h>
+#include <linux/mutex.h>
+#include <linux/clk.h>
+#include <linux/nvhost.h>
+
+/* Sets clocks and powergating state for a module */
+int nvhost_module_init(struct nvhost_device *ndev);
+void nvhost_module_deinit(struct nvhost_device *dev);
+int nvhost_module_suspend(struct nvhost_device *dev);
+
+void nvhost_module_reset(struct nvhost_device *dev);
+void nvhost_module_busy(struct nvhost_device *dev);
+void nvhost_module_idle_mult(struct nvhost_device *dev, int refs);
+int nvhost_module_add_client(struct nvhost_device *dev,
+		void *priv);
+void nvhost_module_remove_client(struct nvhost_device *dev,
+		void *priv);
+int nvhost_module_get_rate(struct nvhost_device *dev,
+		unsigned long *rate,
+		int index);
+int nvhost_module_set_rate(struct nvhost_device *dev, void *priv,
+		unsigned long rate, int index);
+
+static inline bool nvhost_module_powered(struct nvhost_device *dev)
+{
+	return dev->powerstate == NVHOST_POWER_STATE_RUNNING;
+}
+
+static inline void nvhost_module_idle(struct nvhost_device *dev)
+{
+	nvhost_module_idle_mult(dev, 1);
+}
+
+#endif
diff --git a/drivers/video/tegra/host/nvhost_cdma.c b/drivers/video/tegra/host/nvhost_cdma.c
new file mode 100644
index 000000000000..dae3b7e6182d
--- /dev/null
+++ b/drivers/video/tegra/host/nvhost_cdma.c
@@ -0,0 +1,559 @@
+/*
+ * drivers/video/tegra/host/nvhost_cdma.c
+ *
+ * Tegra Graphics Host Command DMA
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "nvhost_cdma.h"
+#include "nvhost_channel.h"
+#include "nvhost_job.h"
+#include "nvhost_hwctx.h"
+#include "dev.h"
+#include "debug.h"
+#include "nvhost_memmgr.h"
+#include "chip_support.h"
+#include <asm/cacheflush.h>
+
+#include <linux/slab.h>
+#include <linux/kfifo.h>
+#include <trace/events/nvhost.h>
+#include <linux/interrupt.h>
+
+/*
+ * TODO:
+ *   stats
+ *     - for figuring out what to optimize further
+ *   resizable push buffer
+ *     - some channels hardly need any, some channels (3d) could use more
+ */
+
+/**
+ * Add an entry to the sync queue.
+ */
+static void add_to_sync_queue(struct nvhost_cdma *cdma,
+			      struct nvhost_job *job,
+			      u32 nr_slots,
+			      u32 first_get)
+{
+	BUG_ON(job->syncpt_id == NVSYNCPT_INVALID);
+
+	job->first_get = first_get;
+	job->num_slots = nr_slots;
+	nvhost_job_get(job);
+	list_add_tail(&job->list, &cdma->sync_queue);
+
+	switch (job->priority) {
+	case NVHOST_PRIORITY_HIGH:
+		cdma->high_prio_count++;
+		break;
+	case NVHOST_PRIORITY_MEDIUM:
+		cdma->med_prio_count++;
+		break;
+	case NVHOST_PRIORITY_LOW:
+		cdma->low_prio_count++;
+		break;
+	}
+}
+
+/**
+ * Return the status of the cdma's sync queue or push buffer for the given event
+ *  - sq empty: returns 1 for empty, 0 for not empty (as in "1 empty queue" :-)
+ *  - pb space: returns the number of free slots in the channel's push buffer
+ * Must be called with the cdma lock held.
+ */
+static unsigned int cdma_status_locked(struct nvhost_cdma *cdma,
+		enum cdma_event event)
+{
+	switch (event) {
+	case CDMA_EVENT_SYNC_QUEUE_EMPTY:
+		return list_empty(&cdma->sync_queue) ? 1 : 0;
+	case CDMA_EVENT_PUSH_BUFFER_SPACE: {
+		struct push_buffer *pb = &cdma->push_buffer;
+		BUG_ON(!cdma_pb_op().space);
+		return cdma_pb_op().space(pb);
+	}
+	default:
+		return 0;
+	}
+}
+
+/**
+ * Sleep (if necessary) until the requested event happens
+ *   - CDMA_EVENT_SYNC_QUEUE_EMPTY : sync queue is completely empty.
+ *     - Returns 1
+ *   - CDMA_EVENT_PUSH_BUFFER_SPACE : there is space in the push buffer
+ *     - Return the amount of space (> 0)
+ * Must be called with the cdma lock held.
+ */
+unsigned int nvhost_cdma_wait_locked(struct nvhost_cdma *cdma,
+		enum cdma_event event)
+{
+	for (;;) {
+		unsigned int space = cdma_status_locked(cdma, event);
+		if (space)
+			return space;
+
+		trace_nvhost_wait_cdma(cdma_to_channel(cdma)->dev->name,
+				event);
+
+		/* If somebody has managed to already start waiting, yield */
+		if (cdma->event != CDMA_EVENT_NONE) {
+			mutex_unlock(&cdma->lock);
+			schedule();
+			mutex_lock(&cdma->lock);
+			continue;
+		}
+		cdma->event = event;
+
+		mutex_unlock(&cdma->lock);
+		down(&cdma->sem);
+		mutex_lock(&cdma->lock);
+	}
+	return 0;
+}
+
+/**
+ * Start timer for a buffer submition that has completed yet.
+ * Must be called with the cdma lock held.
+ */
+static void cdma_start_timer_locked(struct nvhost_cdma *cdma,
+		struct nvhost_job *job)
+{
+	BUG_ON(!job);
+	if (cdma->timeout.clientid) {
+		/* timer already started */
+		return;
+	}
+
+	cdma->timeout.ctx = job->hwctx;
+	cdma->timeout.clientid = job->clientid;
+	cdma->timeout.syncpt_id = job->syncpt_id;
+	cdma->timeout.syncpt_val = job->syncpt_end;
+	cdma->timeout.start_ktime = ktime_get();
+
+	schedule_delayed_work(&cdma->timeout.wq,
+			msecs_to_jiffies(job->timeout));
+}
+
+/**
+ * Stop timer when a buffer submition completes.
+ * Must be called with the cdma lock held.
+ */
+static void stop_cdma_timer_locked(struct nvhost_cdma *cdma)
+{
+	cancel_delayed_work(&cdma->timeout.wq);
+	cdma->timeout.ctx = NULL;
+	cdma->timeout.clientid = 0;
+}
+
+/**
+ * For all sync queue entries that have already finished according to the
+ * current sync point registers:
+ *  - unpin & unref their mems
+ *  - pop their push buffer slots
+ *  - remove them from the sync queue
+ * This is normally called from the host code's worker thread, but can be
+ * called manually if necessary.
+ * Must be called with the cdma lock held.
+ */
+static void update_cdma_locked(struct nvhost_cdma *cdma)
+{
+	bool signal = false;
+	struct nvhost_master *dev = cdma_to_dev(cdma);
+	struct nvhost_syncpt *sp = &dev->syncpt;
+	struct nvhost_job *job, *n;
+
+	/* If CDMA is stopped, queue is cleared and we can return */
+	if (!cdma->running)
+		return;
+
+	/*
+	 * Walk the sync queue, reading the sync point registers as necessary,
+	 * to consume as many sync queue entries as possible without blocking
+	 */
+	list_for_each_entry_safe(job, n, &cdma->sync_queue, list) {
+		BUG_ON(job->syncpt_id == NVSYNCPT_INVALID);
+
+		/* Check whether this syncpt has completed, and bail if not */
+		if (!nvhost_syncpt_is_expired(sp,
+				job->syncpt_id, job->syncpt_end)) {
+			/* Start timer on next pending syncpt */
+			if (job->timeout)
+				cdma_start_timer_locked(cdma, job);
+			break;
+		}
+
+		/* Cancel timeout, when a buffer completes */
+		if (cdma->timeout.clientid)
+			stop_cdma_timer_locked(cdma);
+
+		/* Unpin the memory */
+		nvhost_job_unpin(job);
+
+		/* Pop push buffer slots */
+		if (job->num_slots) {
+			struct push_buffer *pb = &cdma->push_buffer;
+			BUG_ON(!cdma_pb_op().pop_from);
+			cdma_pb_op().pop_from(pb, job->num_slots);
+			if (cdma->event == CDMA_EVENT_PUSH_BUFFER_SPACE)
+				signal = true;
+		}
+
+		list_del(&job->list);
+
+		switch (job->priority) {
+		case NVHOST_PRIORITY_HIGH:
+			cdma->high_prio_count--;
+			break;
+		case NVHOST_PRIORITY_MEDIUM:
+			cdma->med_prio_count--;
+			break;
+		case NVHOST_PRIORITY_LOW:
+			cdma->low_prio_count--;
+			break;
+		}
+
+		nvhost_job_put(job);
+	}
+
+	if (list_empty(&cdma->sync_queue) &&
+				cdma->event == CDMA_EVENT_SYNC_QUEUE_EMPTY)
+			signal = true;
+
+	/* Wake up CdmaWait() if the requested event happened */
+	if (signal) {
+		cdma->event = CDMA_EVENT_NONE;
+		up(&cdma->sem);
+	}
+}
+
+void nvhost_cdma_update_sync_queue(struct nvhost_cdma *cdma,
+		struct nvhost_syncpt *syncpt, struct nvhost_device *dev)
+{
+	u32 get_restart;
+	u32 syncpt_incrs;
+	struct nvhost_job *job = NULL;
+	u32 syncpt_val;
+
+	syncpt_val = nvhost_syncpt_update_min(syncpt, cdma->timeout.syncpt_id);
+
+	dev_dbg(&dev->dev,
+		"%s: starting cleanup (thresh %d)\n",
+		__func__, syncpt_val);
+
+	/*
+	 * Move the sync_queue read pointer to the first entry that hasn't
+	 * completed based on the current HW syncpt value. It's likely there
+	 * won't be any (i.e. we're still at the head), but covers the case
+	 * where a syncpt incr happens just prior/during the teardown.
+	 */
+
+	dev_dbg(&dev->dev,
+		"%s: skip completed buffers still in sync_queue\n",
+		__func__);
+
+	list_for_each_entry(job, &cdma->sync_queue, list) {
+		if (syncpt_val < job->syncpt_end)
+			break;
+
+		nvhost_job_dump(&dev->dev, job);
+	}
+
+	/*
+	 * Walk the sync_queue, first incrementing with the CPU syncpts that
+	 * are partially executed (the first buffer) or fully skipped while
+	 * still in the current context (slots are also NOP-ed).
+	 *
+	 * At the point contexts are interleaved, syncpt increments must be
+	 * done inline with the pushbuffer from a GATHER buffer to maintain
+	 * the order (slots are modified to be a GATHER of syncpt incrs).
+	 *
+	 * Note: save in get_restart the location where the timed out buffer
+	 * started in the PB, so we can start the refetch from there (with the
+	 * modified NOP-ed PB slots). This lets things appear to have completed
+	 * properly for this buffer and resources are freed.
+	 */
+
+	dev_dbg(&dev->dev,
+		"%s: perform CPU incr on pending same ctx buffers\n",
+		__func__);
+
+	get_restart = cdma->last_put;
+	if (!list_empty(&cdma->sync_queue))
+		get_restart = job->first_get;
+
+	/* do CPU increments as long as this context continues */
+	list_for_each_entry_from(job, &cdma->sync_queue, list) {
+		/* different context, gets us out of this loop */
+		if (job->clientid != cdma->timeout.clientid)
+			break;
+
+		/* won't need a timeout when replayed */
+		job->timeout = 0;
+
+		syncpt_incrs = job->syncpt_end - syncpt_val;
+		dev_dbg(&dev->dev,
+			"%s: CPU incr (%d)\n", __func__, syncpt_incrs);
+
+		nvhost_job_dump(&dev->dev, job);
+
+		/* safe to use CPU to incr syncpts */
+		cdma_op().timeout_cpu_incr(cdma,
+				job->first_get,
+				syncpt_incrs,
+				job->syncpt_end,
+				job->num_slots,
+				dev->waitbases);
+
+		syncpt_val += syncpt_incrs;
+	}
+
+	dev_dbg(&dev->dev,
+		"%s: finished sync_queue modification\n", __func__);
+
+	/* roll back DMAGET and start up channel again */
+	cdma_op().timeout_teardown_end(cdma, get_restart);
+
+	if (cdma->timeout.ctx)
+		cdma->timeout.ctx->has_timedout = true;
+}
+
+/**
+ * Create a cdma
+ */
+int nvhost_cdma_init(struct nvhost_cdma *cdma)
+{
+	int err;
+	struct push_buffer *pb = &cdma->push_buffer;
+	BUG_ON(!cdma_pb_op().init);
+	mutex_init(&cdma->lock);
+	sema_init(&cdma->sem, 0);
+
+	INIT_LIST_HEAD(&cdma->sync_queue);
+
+	cdma->event = CDMA_EVENT_NONE;
+	cdma->running = false;
+	cdma->torndown = false;
+
+	err = cdma_pb_op().init(pb);
+	if (err)
+		return err;
+	return 0;
+}
+
+/**
+ * Destroy a cdma
+ */
+void nvhost_cdma_deinit(struct nvhost_cdma *cdma)
+{
+	struct push_buffer *pb = &cdma->push_buffer;
+
+	BUG_ON(!cdma_pb_op().destroy);
+	BUG_ON(cdma->running);
+	cdma_pb_op().destroy(pb);
+	cdma_op().timeout_destroy(cdma);
+}
+
+/**
+ * Begin a cdma submit
+ */
+int nvhost_cdma_begin(struct nvhost_cdma *cdma, struct nvhost_job *job)
+{
+	mutex_lock(&cdma->lock);
+
+	if (job->timeout) {
+		/* init state on first submit with timeout value */
+		if (!cdma->timeout.initialized) {
+			int err;
+			BUG_ON(!cdma_op().timeout_init);
+			err = cdma_op().timeout_init(cdma,
+				job->syncpt_id);
+			if (err) {
+				mutex_unlock(&cdma->lock);
+				return err;
+			}
+		}
+	}
+	if (!cdma->running) {
+		BUG_ON(!cdma_op().start);
+		cdma_op().start(cdma);
+	}
+	cdma->slots_free = 0;
+	cdma->slots_used = 0;
+	cdma->first_get = cdma_pb_op().putptr(&cdma->push_buffer);
+	return 0;
+}
+
+static void trace_write_gather(struct nvhost_cdma *cdma,
+		struct mem_handle *ref,
+		u32 offset, u32 words)
+{
+	void *mem = NULL;
+
+	if (nvhost_debug_trace_cmdbuf) {
+		mem = mem_op().mmap(ref);
+		if (IS_ERR_OR_NULL(mem))
+			mem = NULL;
+	};
+
+	if (mem) {
+		u32 i;
+		/*
+		 * Write in batches of 128 as there seems to be a limit
+		 * of how much you can output to ftrace at once.
+		 */
+		for (i = 0; i < words; i += TRACE_MAX_LENGTH) {
+			trace_nvhost_cdma_push_gather(
+				cdma_to_channel(cdma)->dev->name,
+				(u32)ref,
+				min(words - i, TRACE_MAX_LENGTH),
+				offset + i * sizeof(u32),
+				mem);
+		}
+		mem_op().munmap(ref, mem);
+	}
+}
+
+/**
+ * Push two words into a push buffer slot
+ * Blocks as necessary if the push buffer is full.
+ */
+void nvhost_cdma_push(struct nvhost_cdma *cdma, u32 op1, u32 op2)
+{
+	if (nvhost_debug_trace_cmdbuf)
+		trace_nvhost_cdma_push(cdma_to_channel(cdma)->dev->name,
+				op1, op2);
+
+	nvhost_cdma_push_gather(cdma, NULL, NULL, 0, op1, op2);
+}
+
+/**
+ * Push two words into a push buffer slot
+ * Blocks as necessary if the push buffer is full.
+ */
+void nvhost_cdma_push_gather(struct nvhost_cdma *cdma,
+		struct mem_mgr *client, struct mem_handle *handle,
+		u32 offset, u32 op1, u32 op2)
+{
+	u32 slots_free = cdma->slots_free;
+	struct push_buffer *pb = &cdma->push_buffer;
+
+	BUG_ON(!cdma_pb_op().push_to);
+	BUG_ON(!cdma_op().kick);
+
+	if (handle)
+		trace_write_gather(cdma, handle, offset, op1 & 0xffff);
+
+	if (slots_free == 0) {
+		cdma_op().kick(cdma);
+		slots_free = nvhost_cdma_wait_locked(cdma,
+				CDMA_EVENT_PUSH_BUFFER_SPACE);
+	}
+	cdma->slots_free = slots_free - 1;
+	cdma->slots_used++;
+	cdma_pb_op().push_to(pb, client, handle, op1, op2);
+}
+
+/**
+ * End a cdma submit
+ * Kick off DMA, add job to the sync queue, and a number of slots to be freed
+ * from the pushbuffer. The handles for a submit must all be pinned at the same
+ * time, but they can be unpinned in smaller chunks.
+ */
+void nvhost_cdma_end(struct nvhost_cdma *cdma,
+		struct nvhost_job *job)
+{
+	bool was_idle = list_empty(&cdma->sync_queue);
+
+	BUG_ON(!cdma_op().kick);
+	cdma_op().kick(cdma);
+
+	BUG_ON(job->syncpt_id == NVSYNCPT_INVALID);
+
+	add_to_sync_queue(cdma,
+			job,
+			cdma->slots_used,
+			cdma->first_get);
+
+	/* start timer on idle -> active transitions */
+	if (job->timeout && was_idle)
+		cdma_start_timer_locked(cdma, job);
+
+	trace_nvhost_cdma_end(job->ch->dev->name,
+			job->priority,
+			job->ch->cdma.high_prio_count,
+			job->ch->cdma.med_prio_count,
+			job->ch->cdma.low_prio_count);
+
+	mutex_unlock(&cdma->lock);
+}
+
+/**
+ * Update cdma state according to current sync point values
+ */
+void nvhost_cdma_update(struct nvhost_cdma *cdma)
+{
+	mutex_lock(&cdma->lock);
+	update_cdma_locked(cdma);
+	mutex_unlock(&cdma->lock);
+}
+
+/**
+ * Wait for push buffer to be empty.
+ * @cdma pointer to channel cdma
+ * @timeout timeout in ms
+ * Returns -ETIME if timeout was reached, zero if push buffer is empty.
+ */
+int nvhost_cdma_flush(struct nvhost_cdma *cdma, int timeout)
+{
+	unsigned int space, err = 0;
+	unsigned long end_jiffies = jiffies + msecs_to_jiffies(timeout);
+
+	trace_nvhost_cdma_flush(cdma_to_channel(cdma)->dev->name, timeout);
+
+	/*
+	 * Wait for at most timeout ms. Recalculate timeout at each iteration
+	 * to better keep within given timeout.
+	 */
+	while(!err && time_before(jiffies, end_jiffies)) {
+		int timeout_jiffies = end_jiffies - jiffies;
+
+		mutex_lock(&cdma->lock);
+		space = cdma_status_locked(cdma,
+				CDMA_EVENT_SYNC_QUEUE_EMPTY);
+		if (space) {
+			mutex_unlock(&cdma->lock);
+			return 0;
+		}
+
+		/*
+		 * Wait for sync queue to become empty. If there is already
+		 * an event pending, we need to poll.
+		 */
+		if (cdma->event != CDMA_EVENT_NONE) {
+			mutex_unlock(&cdma->lock);
+			schedule();
+		} else {
+			cdma->event = CDMA_EVENT_SYNC_QUEUE_EMPTY;
+
+			mutex_unlock(&cdma->lock);
+			err = down_timeout(&cdma->sem,
+					jiffies_to_msecs(timeout_jiffies));
+		}
+	}
+	return err;
+}
diff --git a/drivers/video/tegra/host/nvhost_cdma.h b/drivers/video/tegra/host/nvhost_cdma.h
new file mode 100644
index 000000000000..a9522c5f6326
--- /dev/null
+++ b/drivers/video/tegra/host/nvhost_cdma.h
@@ -0,0 +1,117 @@
+/*
+ * drivers/video/tegra/host/nvhost_cdma.h
+ *
+ * Tegra Graphics Host Command DMA
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_CDMA_H
+#define __NVHOST_CDMA_H
+
+#include <linux/sched.h>
+#include <linux/semaphore.h>
+
+#include <linux/nvhost.h>
+#include <linux/list.h>
+
+struct nvhost_syncpt;
+struct nvhost_userctx_timeout;
+struct nvhost_job;
+struct mem_mgr;
+struct mem_handle;
+
+/*
+ * cdma
+ *
+ * This is in charge of a host command DMA channel.
+ * Sends ops to a push buffer, and takes responsibility for unpinning
+ * (& possibly freeing) of memory after those ops have completed.
+ * Producer:
+ *	begin
+ *		push - send ops to the push buffer
+ *	end - start command DMA and enqueue handles to be unpinned
+ * Consumer:
+ *	update - call to update sync queue and push buffer, unpin memory
+ */
+
+struct push_buffer {
+	struct mem_handle *mem;		/* handle to pushbuffer memory */
+	u32 *mapped;			/* mapped pushbuffer memory */
+	u32 phys;			/* physical address of pushbuffer */
+	u32 fence;			/* index we've written */
+	u32 cur;			/* index to write to */
+	struct mem_mgr_handle *client_handle; /* handle for each opcode pair */
+};
+
+struct buffer_timeout {
+	struct delayed_work wq;		/* work queue */
+	bool initialized;		/* timer one-time setup flag */
+	u32 syncpt_id;			/* buffer completion syncpt id */
+	u32 syncpt_val;			/* syncpt value when completed */
+	ktime_t start_ktime;		/* starting time */
+	/* context timeout information */
+	struct nvhost_hwctx *ctx;
+	int clientid;
+};
+
+enum cdma_event {
+	CDMA_EVENT_NONE,		/* not waiting for any event */
+	CDMA_EVENT_SYNC_QUEUE_EMPTY,	/* wait for empty sync queue */
+	CDMA_EVENT_PUSH_BUFFER_SPACE	/* wait for space in push buffer */
+};
+
+struct nvhost_cdma {
+	struct mutex lock;		/* controls access to shared state */
+	struct semaphore sem;		/* signalled when event occurs */
+	enum cdma_event event;		/* event that sem is waiting for */
+	unsigned int slots_used;	/* pb slots used in current submit */
+	unsigned int slots_free;	/* pb slots free in current submit */
+	unsigned int first_get;		/* DMAGET value, where submit begins */
+	unsigned int last_put;		/* last value written to DMAPUT */
+	struct push_buffer push_buffer;	/* channel's push buffer */
+	struct list_head sync_queue;	/* job queue */
+	struct buffer_timeout timeout;	/* channel's timeout state/wq */
+	bool running;
+	bool torndown;
+	int high_prio_count;
+	int med_prio_count;
+	int low_prio_count;
+};
+
+#define cdma_to_channel(cdma) container_of(cdma, struct nvhost_channel, cdma)
+#define cdma_to_dev(cdma) nvhost_get_host(cdma_to_channel(cdma)->dev)
+#define cdma_to_memmgr(cdma) ((cdma_to_dev(cdma))->memmgr)
+#define pb_to_cdma(pb) container_of(pb, struct nvhost_cdma, push_buffer)
+
+int	nvhost_cdma_init(struct nvhost_cdma *cdma);
+void	nvhost_cdma_deinit(struct nvhost_cdma *cdma);
+void	nvhost_cdma_stop(struct nvhost_cdma *cdma);
+int	nvhost_cdma_begin(struct nvhost_cdma *cdma, struct nvhost_job *job);
+void	nvhost_cdma_push(struct nvhost_cdma *cdma, u32 op1, u32 op2);
+void	nvhost_cdma_push_gather(struct nvhost_cdma *cdma,
+		struct mem_mgr *client,
+		struct mem_handle *handle, u32 offset, u32 op1, u32 op2);
+void	nvhost_cdma_end(struct nvhost_cdma *cdma,
+		struct nvhost_job *job);
+void	nvhost_cdma_update(struct nvhost_cdma *cdma);
+int	nvhost_cdma_flush(struct nvhost_cdma *cdma, int timeout);
+void	nvhost_cdma_peek(struct nvhost_cdma *cdma,
+		u32 dmaget, int slot, u32 *out);
+unsigned int nvhost_cdma_wait_locked(struct nvhost_cdma *cdma,
+		enum cdma_event event);
+void nvhost_cdma_update_sync_queue(struct nvhost_cdma *cdma,
+		struct nvhost_syncpt *syncpt, struct nvhost_device *dev);
+#endif
diff --git a/drivers/video/tegra/host/nvhost_channel.c b/drivers/video/tegra/host/nvhost_channel.c
new file mode 100644
index 000000000000..fd309ee9917b
--- /dev/null
+++ b/drivers/video/tegra/host/nvhost_channel.c
@@ -0,0 +1,188 @@
+/*
+ * drivers/video/tegra/host/nvhost_channel.c
+ *
+ * Tegra Graphics Host Channel
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "nvhost_channel.h"
+#include "dev.h"
+#include "nvhost_acm.h"
+#include "nvhost_job.h"
+#include "chip_support.h"
+
+#include <trace/events/nvhost.h>
+#include <linux/nvhost_ioctl.h>
+#include <linux/slab.h>
+
+#define NVHOST_CHANNEL_LOW_PRIO_MAX_WAIT 50
+
+int nvhost_channel_init(struct nvhost_channel *ch,
+		struct nvhost_master *dev, int index)
+{
+	int err;
+	struct nvhost_device *ndev;
+
+	/* Link nvhost_device to nvhost_channel */
+	err = channel_op().init(ch, dev, index);
+	if (err < 0) {
+		dev_err(&dev->dev->dev, "failed to init channel %d\n",
+				index);
+		return err;
+	}
+	ndev = ch->dev;
+	ndev->channel = ch;
+
+	return 0;
+}
+
+int nvhost_channel_submit(struct nvhost_job *job)
+{
+	/*
+	 * Check if queue has higher priority jobs running. If so, wait until
+	 * queue is empty. Ignores result from nvhost_cdma_flush, as we submit
+	 * either when push buffer is empty or when we reach the timeout.
+	 */
+	int higher_count = 0;
+
+	switch (job->priority) {
+	case NVHOST_PRIORITY_HIGH:
+		higher_count = 0;
+		break;
+	case NVHOST_PRIORITY_MEDIUM:
+		higher_count = job->ch->cdma.high_prio_count;
+		break;
+	case NVHOST_PRIORITY_LOW:
+		higher_count = job->ch->cdma.high_prio_count
+			+ job->ch->cdma.med_prio_count;
+		break;
+	}
+	if (higher_count > 0)
+		(void)nvhost_cdma_flush(&job->ch->cdma,
+				NVHOST_CHANNEL_LOW_PRIO_MAX_WAIT);
+
+	return channel_op().submit(job);
+}
+
+struct nvhost_channel *nvhost_getchannel(struct nvhost_channel *ch)
+{
+	int err = 0;
+	struct nvhost_driver *drv = to_nvhost_driver(ch->dev->dev.driver);
+
+	mutex_lock(&ch->reflock);
+	if (ch->refcount == 0) {
+		if (drv->init)
+			drv->init(ch->dev);
+		err = nvhost_cdma_init(&ch->cdma);
+	} else if (ch->dev->exclusive) {
+		err = -EBUSY;
+	}
+	if (!err)
+		ch->refcount++;
+
+	mutex_unlock(&ch->reflock);
+
+	/* Keep alive modules that needs to be when a channel is open */
+	if (!err && ch->dev->keepalive)
+		nvhost_module_busy(ch->dev);
+
+	return err ? NULL : ch;
+}
+
+void nvhost_putchannel(struct nvhost_channel *ch, struct nvhost_hwctx *ctx)
+{
+	BUG_ON(!channel_cdma_op().stop);
+
+	if (ctx) {
+		mutex_lock(&ch->submitlock);
+		if (ch->cur_ctx == ctx)
+			ch->cur_ctx = NULL;
+		mutex_unlock(&ch->submitlock);
+	}
+
+	/* Allow keep-alive'd module to be turned off */
+	if (ch->dev->keepalive)
+		nvhost_module_idle(ch->dev);
+
+	mutex_lock(&ch->reflock);
+	if (ch->refcount == 1) {
+		channel_cdma_op().stop(&ch->cdma);
+		nvhost_cdma_deinit(&ch->cdma);
+		nvhost_module_suspend(ch->dev);
+	}
+	ch->refcount--;
+	mutex_unlock(&ch->reflock);
+}
+
+int nvhost_channel_suspend(struct nvhost_channel *ch)
+{
+	int ret = 0;
+
+	mutex_lock(&ch->reflock);
+	BUG_ON(!channel_cdma_op().stop);
+
+	if (ch->refcount) {
+		ret = nvhost_module_suspend(ch->dev);
+		if (!ret)
+			channel_cdma_op().stop(&ch->cdma);
+	}
+	mutex_unlock(&ch->reflock);
+
+	return ret;
+}
+
+struct nvhost_channel *nvhost_alloc_channel_internal(int chindex,
+	int max_channels, int *current_channel_count)
+{
+	struct nvhost_channel *ch = NULL;
+
+	if ( (chindex > max_channels) ||
+	     ( (*current_channel_count + 1) > max_channels) )
+		return NULL;
+	else {
+		ch = kzalloc(sizeof(*ch), GFP_KERNEL);
+		if (ch == NULL)
+			return NULL;
+		else {
+			(*current_channel_count)++;
+			return ch;
+		}
+	}
+}
+
+void nvhost_free_channel_internal(struct nvhost_channel *ch,
+	int *current_channel_count)
+{
+	kfree(ch);
+	(*current_channel_count)--;
+}
+
+int nvhost_channel_save_context(struct nvhost_channel *ch)
+{
+	struct nvhost_hwctx *cur_ctx = ch->cur_ctx;
+	int err = 0;
+	if (cur_ctx)
+		err = channel_op().save_context(ch);
+
+	return err;
+
+}
+
+int nvhost_channel_drain_read_fifo(struct nvhost_channel *ch,
+			u32 *ptr, unsigned int count, unsigned int *pending)
+{
+	return channel_op().drain_read_fifo(ch, ptr, count, pending);
+}
diff --git a/drivers/video/tegra/host/nvhost_channel.h b/drivers/video/tegra/host/nvhost_channel.h
new file mode 100644
index 000000000000..d7f096db1ffa
--- /dev/null
+++ b/drivers/video/tegra/host/nvhost_channel.h
@@ -0,0 +1,77 @@
+/*
+ * drivers/video/tegra/host/nvhost_channel.h
+ *
+ * Tegra Graphics Host Channel
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_CHANNEL_H
+#define __NVHOST_CHANNEL_H
+
+#include <linux/cdev.h>
+#include <linux/io.h>
+#include "nvhost_cdma.h"
+
+#define NVHOST_MAX_WAIT_CHECKS		256
+#define NVHOST_MAX_GATHERS		512
+#define NVHOST_MAX_HANDLES		1280
+#define NVHOST_MAX_POWERGATE_IDS	2
+
+struct nvhost_master;
+struct nvhost_device;
+struct nvhost_channel;
+struct nvhost_hwctx;
+
+struct nvhost_channel {
+	int refcount;
+	int chid;
+	u32 syncpt_id;
+	struct mutex reflock;
+	struct mutex submitlock;
+	void __iomem *aperture;
+	struct nvhost_hwctx *cur_ctx;
+	struct device *node;
+	struct nvhost_device *dev;
+	struct cdev cdev;
+	struct nvhost_hwctx_handler *ctxhandler;
+	struct nvhost_cdma cdma;
+};
+
+int nvhost_channel_init(struct nvhost_channel *ch,
+	struct nvhost_master *dev, int index);
+
+int nvhost_channel_submit(struct nvhost_job *job);
+
+struct nvhost_channel *nvhost_getchannel(struct nvhost_channel *ch);
+void nvhost_putchannel(struct nvhost_channel *ch, struct nvhost_hwctx *ctx);
+int nvhost_channel_suspend(struct nvhost_channel *ch);
+
+int nvhost_channel_drain_read_fifo(struct nvhost_channel *ch,
+			u32 *ptr, unsigned int count, unsigned int *pending);
+
+int nvhost_channel_read_3d_reg(struct nvhost_channel *channel,
+	struct nvhost_hwctx *hwctx,
+	u32 offset, u32 *value);
+
+struct nvhost_channel *nvhost_alloc_channel_internal(int chindex,
+	int max_channels, int *current_channel_count);
+
+void nvhost_free_channel_internal(struct nvhost_channel *ch,
+	int *current_channel_count);
+
+int nvhost_channel_save_context(struct nvhost_channel *ch);
+
+#endif
diff --git a/drivers/video/tegra/host/nvhost_hwctx.h b/drivers/video/tegra/host/nvhost_hwctx.h
new file mode 100644
index 000000000000..47bc3d408fde
--- /dev/null
+++ b/drivers/video/tegra/host/nvhost_hwctx.h
@@ -0,0 +1,66 @@
+/*
+ * drivers/video/tegra/host/nvhost_hwctx.h
+ *
+ * Tegra Graphics Host Hardware Context Interface
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_HWCTX_H
+#define __NVHOST_HWCTX_H
+
+#include <linux/string.h>
+#include <linux/kref.h>
+
+#include <linux/nvhost.h>
+
+struct nvhost_channel;
+struct nvhost_cdma;
+
+struct nvhost_hwctx {
+	struct kref ref;
+	struct nvhost_hwctx_handler *h;
+	struct nvhost_channel *channel;
+	bool valid;
+	bool has_timedout;
+};
+
+struct nvhost_hwctx_handler {
+	struct nvhost_hwctx * (*alloc) (struct nvhost_hwctx_handler *h,
+			struct nvhost_channel *ch);
+	void (*get) (struct nvhost_hwctx *ctx);
+	void (*put) (struct nvhost_hwctx *ctx);
+	void (*save_push) (struct nvhost_hwctx *ctx,
+			struct nvhost_cdma *cdma);
+	void (*save_service) (struct nvhost_hwctx *ctx);
+	void *priv;
+};
+
+
+struct hwctx_reginfo {
+	unsigned int offset:12;
+	unsigned int count:16;
+	unsigned int type:2;
+};
+
+enum {
+	HWCTX_REGINFO_DIRECT = 0,
+	HWCTX_REGINFO_INDIRECT,
+	HWCTX_REGINFO_INDIRECT_4X
+};
+
+#define HWCTX_REGINFO(offset, count, type) {offset, count, HWCTX_REGINFO_##type}
+
+#endif
diff --git a/drivers/video/tegra/host/nvhost_intr.c b/drivers/video/tegra/host/nvhost_intr.c
new file mode 100644
index 000000000000..9788d32bd4a9
--- /dev/null
+++ b/drivers/video/tegra/host/nvhost_intr.c
@@ -0,0 +1,441 @@
+/*
+ * drivers/video/tegra/host/nvhost_intr.c
+ *
+ * Tegra Graphics Host Interrupt Management
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "nvhost_intr.h"
+#include "dev.h"
+#include "nvhost_acm.h"
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <trace/events/nvhost.h>
+#include "nvhost_channel.h"
+#include "nvhost_hwctx.h"
+#include "chip_support.h"
+
+/*** Wait list management ***/
+
+struct nvhost_waitlist {
+	struct list_head list;
+	struct kref refcount;
+	u32 thresh;
+	enum nvhost_intr_action action;
+	atomic_t state;
+	void *data;
+	int count;
+};
+
+enum waitlist_state {
+	WLS_PENDING,
+	WLS_REMOVED,
+	WLS_CANCELLED,
+	WLS_HANDLED
+};
+
+static void waiter_release(struct kref *kref)
+{
+	kfree(container_of(kref, struct nvhost_waitlist, refcount));
+}
+
+/**
+ * add a waiter to a waiter queue, sorted by threshold
+ * returns true if it was added at the head of the queue
+ */
+static bool add_waiter_to_queue(struct nvhost_waitlist *waiter,
+				struct list_head *queue)
+{
+	struct nvhost_waitlist *pos;
+	u32 thresh = waiter->thresh;
+
+	list_for_each_entry_reverse(pos, queue, list)
+		if ((s32)(pos->thresh - thresh) <= 0) {
+			list_add(&waiter->list, &pos->list);
+			return false;
+		}
+
+	list_add(&waiter->list, queue);
+	return true;
+}
+
+/**
+ * run through a waiter queue for a single sync point ID
+ * and gather all completed waiters into lists by actions
+ */
+static void remove_completed_waiters(struct list_head *head, u32 sync,
+			struct list_head completed[NVHOST_INTR_ACTION_COUNT])
+{
+	struct list_head *dest;
+	struct nvhost_waitlist *waiter, *next, *prev;
+
+	list_for_each_entry_safe(waiter, next, head, list) {
+		if ((s32)(waiter->thresh - sync) > 0)
+			break;
+
+		dest = completed + waiter->action;
+
+		/* consolidate submit cleanups */
+		if (waiter->action == NVHOST_INTR_ACTION_SUBMIT_COMPLETE
+			&& !list_empty(dest)) {
+			prev = list_entry(dest->prev,
+					struct nvhost_waitlist, list);
+			if (prev->data == waiter->data) {
+				prev->count++;
+				dest = NULL;
+			}
+		}
+
+		/* PENDING->REMOVED or CANCELLED->HANDLED */
+		if (atomic_inc_return(&waiter->state) == WLS_HANDLED || !dest) {
+			list_del(&waiter->list);
+			kref_put(&waiter->refcount, waiter_release);
+		} else {
+			list_move_tail(&waiter->list, dest);
+		}
+	}
+}
+
+void reset_threshold_interrupt(struct nvhost_intr *intr,
+			       struct list_head *head,
+			       unsigned int id)
+{
+	u32 thresh = list_first_entry(head,
+				struct nvhost_waitlist, list)->thresh;
+	BUG_ON(!(intr_op().set_syncpt_threshold &&
+		 intr_op().enable_syncpt_intr));
+
+	intr_op().set_syncpt_threshold(intr, id, thresh);
+	intr_op().enable_syncpt_intr(intr, id);
+}
+
+
+static void action_submit_complete(struct nvhost_waitlist *waiter)
+{
+	struct nvhost_channel *channel = waiter->data;
+	int nr_completed = waiter->count;
+
+	nvhost_cdma_update(&channel->cdma);
+	nvhost_module_idle_mult(channel->dev, nr_completed);
+
+	/*  Add nr_completed to trace */
+	trace_nvhost_channel_submit_complete(channel->dev->name,
+			nr_completed, waiter->thresh,
+			channel->cdma.high_prio_count,
+			channel->cdma.med_prio_count,
+			channel->cdma.low_prio_count);
+
+}
+
+static void action_ctxsave(struct nvhost_waitlist *waiter)
+{
+	struct nvhost_hwctx *hwctx = waiter->data;
+	struct nvhost_channel *channel = hwctx->channel;
+
+	if (channel->ctxhandler->save_service)
+		channel->ctxhandler->save_service(hwctx);
+}
+
+static void action_wakeup(struct nvhost_waitlist *waiter)
+{
+	wait_queue_head_t *wq = waiter->data;
+
+	wake_up(wq);
+}
+
+static void action_wakeup_interruptible(struct nvhost_waitlist *waiter)
+{
+	wait_queue_head_t *wq = waiter->data;
+
+	wake_up_interruptible(wq);
+}
+
+typedef void (*action_handler)(struct nvhost_waitlist *waiter);
+
+static action_handler action_handlers[NVHOST_INTR_ACTION_COUNT] = {
+	action_submit_complete,
+	action_ctxsave,
+	action_wakeup,
+	action_wakeup_interruptible,
+};
+
+static void run_handlers(struct list_head completed[NVHOST_INTR_ACTION_COUNT])
+{
+	struct list_head *head = completed;
+	int i;
+
+	for (i = 0; i < NVHOST_INTR_ACTION_COUNT; ++i, ++head) {
+		action_handler handler = action_handlers[i];
+		struct nvhost_waitlist *waiter, *next;
+
+		list_for_each_entry_safe(waiter, next, head, list) {
+			list_del(&waiter->list);
+			handler(waiter);
+			WARN_ON(atomic_xchg(&waiter->state, WLS_HANDLED) != WLS_REMOVED);
+			kref_put(&waiter->refcount, waiter_release);
+		}
+	}
+}
+
+/**
+ * Remove & handle all waiters that have completed for the given syncpt
+ */
+static int process_wait_list(struct nvhost_intr *intr,
+			     struct nvhost_intr_syncpt *syncpt,
+			     u32 threshold)
+{
+	struct list_head completed[NVHOST_INTR_ACTION_COUNT];
+	unsigned int i;
+	int empty;
+
+	for (i = 0; i < NVHOST_INTR_ACTION_COUNT; ++i)
+		INIT_LIST_HEAD(completed + i);
+
+	spin_lock(&syncpt->lock);
+
+	remove_completed_waiters(&syncpt->wait_head, threshold, completed);
+
+	empty = list_empty(&syncpt->wait_head);
+	if (empty)
+		intr_op().disable_syncpt_intr(intr, syncpt->id);
+	else
+		reset_threshold_interrupt(intr, &syncpt->wait_head,
+					  syncpt->id);
+
+	spin_unlock(&syncpt->lock);
+
+	run_handlers(completed);
+
+	return empty;
+}
+
+/*** host syncpt interrupt service functions ***/
+/**
+ * Sync point threshold interrupt service thread function
+ * Handles sync point threshold triggers, in thread context
+ */
+irqreturn_t nvhost_syncpt_thresh_fn(int irq, void *dev_id)
+{
+	struct nvhost_intr_syncpt *syncpt = dev_id;
+	unsigned int id = syncpt->id;
+	struct nvhost_intr *intr = intr_syncpt_to_intr(syncpt);
+	struct nvhost_master *dev = intr_to_dev(intr);
+
+	(void)process_wait_list(intr, syncpt,
+				nvhost_syncpt_update_min(&dev->syncpt, id));
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * free a syncpt's irq. syncpt interrupt should be disabled first.
+ */
+static void free_syncpt_irq(struct nvhost_intr_syncpt *syncpt)
+{
+	if (syncpt->irq_requested) {
+		free_irq(syncpt->irq, syncpt);
+		syncpt->irq_requested = 0;
+	}
+}
+
+
+/*** host general interrupt service functions ***/
+
+
+/*** Main API ***/
+
+int nvhost_intr_add_action(struct nvhost_intr *intr, u32 id, u32 thresh,
+			enum nvhost_intr_action action, void *data,
+			void *_waiter,
+			void **ref)
+{
+	struct nvhost_waitlist *waiter = _waiter;
+	struct nvhost_intr_syncpt *syncpt;
+	int queue_was_empty;
+	int err;
+
+	BUG_ON(waiter == NULL);
+
+	BUG_ON(!(intr_op().set_syncpt_threshold &&
+		 intr_op().enable_syncpt_intr));
+
+	/* initialize a new waiter */
+	INIT_LIST_HEAD(&waiter->list);
+	kref_init(&waiter->refcount);
+	if (ref)
+		kref_get(&waiter->refcount);
+	waiter->thresh = thresh;
+	waiter->action = action;
+	atomic_set(&waiter->state, WLS_PENDING);
+	waiter->data = data;
+	waiter->count = 1;
+
+	syncpt = intr->syncpt + id;
+
+	spin_lock(&syncpt->lock);
+
+	/* lazily request irq for this sync point */
+	if (!syncpt->irq_requested) {
+		spin_unlock(&syncpt->lock);
+
+		mutex_lock(&intr->mutex);
+		BUG_ON(!(intr_op().request_syncpt_irq));
+		err = intr_op().request_syncpt_irq(syncpt);
+		mutex_unlock(&intr->mutex);
+
+		if (err) {
+			kfree(waiter);
+			return err;
+		}
+
+		spin_lock(&syncpt->lock);
+	}
+
+	queue_was_empty = list_empty(&syncpt->wait_head);
+
+	if (add_waiter_to_queue(waiter, &syncpt->wait_head)) {
+		/* added at head of list - new threshold value */
+		intr_op().set_syncpt_threshold(intr, id, thresh);
+
+		/* added as first waiter - enable interrupt */
+		if (queue_was_empty)
+			intr_op().enable_syncpt_intr(intr, id);
+	}
+
+	spin_unlock(&syncpt->lock);
+
+	if (ref)
+		*ref = waiter;
+	return 0;
+}
+
+void *nvhost_intr_alloc_waiter()
+{
+	return kzalloc(sizeof(struct nvhost_waitlist),
+			GFP_KERNEL|__GFP_REPEAT);
+}
+
+void nvhost_intr_put_ref(struct nvhost_intr *intr, u32 id, void *ref)
+{
+	struct nvhost_waitlist *waiter = ref;
+	struct nvhost_intr_syncpt *syncpt;
+	struct nvhost_master *host = intr_to_dev(intr);
+
+	while (atomic_cmpxchg(&waiter->state,
+				WLS_PENDING, WLS_CANCELLED) == WLS_REMOVED)
+		schedule();
+
+	syncpt = intr->syncpt + id;
+	(void)process_wait_list(intr, syncpt,
+				nvhost_syncpt_update_min(&host->syncpt, id));
+
+	kref_put(&waiter->refcount, waiter_release);
+}
+
+
+/*** Init & shutdown ***/
+
+int nvhost_intr_init(struct nvhost_intr *intr, u32 irq_gen, u32 irq_sync)
+{
+	unsigned int id;
+	struct nvhost_intr_syncpt *syncpt;
+	struct nvhost_master *host = intr_to_dev(intr);
+	u32 nb_pts = nvhost_syncpt_nb_pts(&host->syncpt);
+
+	mutex_init(&intr->mutex);
+	intr->host_syncpt_irq_base = irq_sync;
+	intr_op().init_host_sync(intr);
+	intr->host_general_irq = irq_gen;
+	intr->host_general_irq_requested = false;
+	intr_op().request_host_general_irq(intr);
+
+	for (id = 0, syncpt = intr->syncpt;
+	     id < nb_pts;
+	     ++id, ++syncpt) {
+		syncpt->intr = &host->intr;
+		syncpt->id = id;
+		syncpt->irq = irq_sync + id;
+		syncpt->irq_requested = 0;
+		spin_lock_init(&syncpt->lock);
+		INIT_LIST_HEAD(&syncpt->wait_head);
+		snprintf(syncpt->thresh_irq_name,
+			sizeof(syncpt->thresh_irq_name),
+			"host_sp_%02d", id);
+	}
+
+	return 0;
+}
+
+void nvhost_intr_deinit(struct nvhost_intr *intr)
+{
+	nvhost_intr_stop(intr);
+}
+
+void nvhost_intr_start(struct nvhost_intr *intr, u32 hz)
+{
+	BUG_ON(!(intr_op().init_host_sync &&
+		 intr_op().set_host_clocks_per_usec &&
+		 intr_op().request_host_general_irq));
+
+	mutex_lock(&intr->mutex);
+
+	intr_op().init_host_sync(intr);
+	intr_op().set_host_clocks_per_usec(intr,
+					       (hz + 1000000 - 1)/1000000);
+
+	intr_op().request_host_general_irq(intr);
+
+	mutex_unlock(&intr->mutex);
+}
+
+void nvhost_intr_stop(struct nvhost_intr *intr)
+{
+	unsigned int id;
+	struct nvhost_intr_syncpt *syncpt;
+	u32 nb_pts = nvhost_syncpt_nb_pts(&intr_to_dev(intr)->syncpt);
+
+	BUG_ON(!(intr_op().disable_all_syncpt_intrs &&
+		 intr_op().free_host_general_irq));
+
+	mutex_lock(&intr->mutex);
+
+	intr_op().disable_all_syncpt_intrs(intr);
+
+	for (id = 0, syncpt = intr->syncpt;
+	     id < nb_pts;
+	     ++id, ++syncpt) {
+		struct nvhost_waitlist *waiter, *next;
+		list_for_each_entry_safe(waiter, next, &syncpt->wait_head, list) {
+			if (atomic_cmpxchg(&waiter->state, WLS_CANCELLED, WLS_HANDLED)
+				== WLS_CANCELLED) {
+				list_del(&waiter->list);
+				kref_put(&waiter->refcount, waiter_release);
+			}
+		}
+
+		if (!list_empty(&syncpt->wait_head)) {  /* output diagnostics */
+			printk(KERN_DEBUG "%s id=%d\n", __func__, id);
+			BUG_ON(1);
+		}
+
+		free_syncpt_irq(syncpt);
+	}
+
+	intr_op().free_host_general_irq(intr);
+
+	mutex_unlock(&intr->mutex);
+}
diff --git a/drivers/video/tegra/host/nvhost_intr.h b/drivers/video/tegra/host/nvhost_intr.h
new file mode 100644
index 000000000000..d4a6157eced1
--- /dev/null
+++ b/drivers/video/tegra/host/nvhost_intr.h
@@ -0,0 +1,115 @@
+/*
+ * drivers/video/tegra/host/nvhost_intr.h
+ *
+ * Tegra Graphics Host Interrupt Management
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_INTR_H
+#define __NVHOST_INTR_H
+
+#include <linux/kthread.h>
+#include <linux/semaphore.h>
+#include <linux/interrupt.h>
+
+struct nvhost_channel;
+
+enum nvhost_intr_action {
+	/**
+	 * Perform cleanup after a submit has completed.
+	 * 'data' points to a channel
+	 */
+	NVHOST_INTR_ACTION_SUBMIT_COMPLETE = 0,
+
+	/**
+	 * Save a HW context.
+	 * 'data' points to a context
+	 */
+	NVHOST_INTR_ACTION_CTXSAVE,
+
+	/**
+	 * Wake up a  task.
+	 * 'data' points to a wait_queue_head_t
+	 */
+	NVHOST_INTR_ACTION_WAKEUP,
+
+	/**
+	 * Wake up a interruptible task.
+	 * 'data' points to a wait_queue_head_t
+	 */
+	NVHOST_INTR_ACTION_WAKEUP_INTERRUPTIBLE,
+
+	NVHOST_INTR_ACTION_COUNT
+};
+
+struct nvhost_intr;
+
+struct nvhost_intr_syncpt {
+	struct  nvhost_intr *intr;
+	u8 id;
+	u8 irq_requested;
+	u16 irq;
+	spinlock_t lock;
+	struct list_head wait_head;
+	char thresh_irq_name[12];
+};
+
+struct nvhost_intr {
+	struct nvhost_intr_syncpt *syncpt;
+	struct mutex mutex;
+	int host_general_irq;
+	int host_syncpt_irq_base;
+	bool host_general_irq_requested;
+};
+#define intr_to_dev(x) container_of(x, struct nvhost_master, intr)
+#define intr_syncpt_to_intr(is) (is->intr)
+
+/**
+ * Schedule an action to be taken when a sync point reaches the given threshold.
+ *
+ * @id the sync point
+ * @thresh the threshold
+ * @action the action to take
+ * @data a pointer to extra data depending on action, see above
+ * @waiter waiter allocated with nvhost_intr_alloc_waiter - assumes ownership
+ * @ref must be passed if cancellation is possible, else NULL
+ *
+ * This is a non-blocking api.
+ */
+int nvhost_intr_add_action(struct nvhost_intr *intr, u32 id, u32 thresh,
+			enum nvhost_intr_action action, void *data,
+			void *waiter,
+			void **ref);
+
+/**
+ * Allocate a waiter.
+ */
+void *nvhost_intr_alloc_waiter(void);
+
+/**
+ * Unreference an action submitted to nvhost_intr_add_action().
+ * You must call this if you passed non-NULL as ref.
+ * @ref the ref returned from nvhost_intr_add_action()
+ */
+void nvhost_intr_put_ref(struct nvhost_intr *intr, u32 id, void *ref);
+
+int nvhost_intr_init(struct nvhost_intr *intr, u32 irq_gen, u32 irq_sync);
+void nvhost_intr_deinit(struct nvhost_intr *intr);
+void nvhost_intr_start(struct nvhost_intr *intr, u32 hz);
+void nvhost_intr_stop(struct nvhost_intr *intr);
+
+irqreturn_t nvhost_syncpt_thresh_fn(int irq, void *dev_id);
+#endif
diff --git a/drivers/video/tegra/host/nvhost_job.c b/drivers/video/tegra/host/nvhost_job.c
new file mode 100644
index 000000000000..5587f51ea996
--- /dev/null
+++ b/drivers/video/tegra/host/nvhost_job.c
@@ -0,0 +1,361 @@
+/*
+ * drivers/video/tegra/host/nvhost_job.c
+ *
+ * Tegra Graphics Host Job
+ *
+ * Copyright (c) 2010-2014, NVIDIA 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/slab.h>
+#include <linux/kref.h>
+#include <linux/err.h>
+#include <linux/vmalloc.h>
+#include <trace/events/nvhost.h>
+#include "nvhost_channel.h"
+#include "nvhost_job.h"
+#include "nvhost_hwctx.h"
+#include "nvhost_syncpt.h"
+#include "dev.h"
+#include "nvhost_memmgr.h"
+#include "chip_support.h"
+#include "nvmap.h"
+
+/* Magic to use to fill freed handle slots */
+#define BAD_MAGIC 0xdeadbeef
+
+static size_t job_size(struct nvhost_submit_hdr_ext *hdr)
+{
+	s64 num_relocs = hdr ? (int)hdr->num_relocs : 0;
+	s64 num_waitchks = hdr ? (int)hdr->num_waitchks : 0;
+	s64 num_cmdbufs = hdr ? (int)hdr->num_cmdbufs : 0;
+	s64 num_unpins = num_cmdbufs + num_relocs;
+	s64 total;
+
+	if(num_relocs < 0 || num_waitchks < 0 || num_cmdbufs < 0)
+		return 0;
+
+	total = sizeof(struct nvhost_job)
+			+ num_relocs * sizeof(struct nvhost_reloc)
+			+ num_relocs * sizeof(struct nvhost_reloc_shift)
+			+ num_unpins * sizeof(struct mem_handle *)
+			+ num_waitchks * sizeof(struct nvhost_waitchk)
+			+ num_cmdbufs * sizeof(struct nvhost_job_gather);
+
+	if(total > ULONG_MAX)
+		return 0;
+	return (size_t)total;
+}
+
+static void init_fields(struct nvhost_job *job,
+		struct nvhost_submit_hdr_ext *hdr,
+		int priority, int clientid)
+{
+	int num_relocs = hdr ? hdr->num_relocs : 0;
+	int num_waitchks = hdr ? hdr->num_waitchks : 0;
+	int num_cmdbufs = hdr ? hdr->num_cmdbufs : 0;
+	int num_unpins = num_cmdbufs + num_relocs;
+	void *mem = job;
+
+	/* First init state to zero */
+	job->priority = priority;
+	job->clientid = clientid;
+
+	/*
+	 * Redistribute memory to the structs.
+	 * Overflows and negative conditions have
+	 * already been checked in job_alloc().
+	 */
+	mem += sizeof(struct nvhost_job);
+	job->relocarray = num_relocs ? mem : NULL;
+	mem += num_relocs * sizeof(struct nvhost_reloc);
+	job->relocshiftarray = num_relocs ? mem : NULL;
+	mem += num_relocs * sizeof(struct nvhost_reloc_shift);
+	job->unpins = num_unpins ? mem : NULL;
+	mem += num_unpins * sizeof(struct mem_handle *);
+	job->waitchk = num_waitchks ? mem : NULL;
+	mem += num_waitchks * sizeof(struct nvhost_waitchk);
+	job->gathers = num_cmdbufs ? mem : NULL;
+
+	/* Copy information from header */
+	if (hdr) {
+		job->waitchk_mask = hdr->waitchk_mask;
+		job->syncpt_id = hdr->syncpt_id;
+		job->syncpt_incrs = hdr->syncpt_incrs;
+	}
+}
+
+struct nvhost_job *nvhost_job_alloc(struct nvhost_channel *ch,
+		struct nvhost_hwctx *hwctx,
+		struct nvhost_submit_hdr_ext *hdr,
+		struct mem_mgr *memmgr,
+		int priority,
+		int clientid)
+{
+	struct nvhost_job *job = NULL;
+	size_t size = job_size(hdr);
+
+	if(!size)
+		goto error;
+	job = vzalloc(size);
+	if (!job)
+		goto error;
+
+	kref_init(&job->ref);
+	job->ch = ch;
+	job->hwctx = hwctx;
+	if (hwctx)
+		hwctx->h->get(hwctx);
+	job->memmgr = memmgr ? mem_op().get_mgr(memmgr) : NULL;
+
+	init_fields(job, hdr, priority, clientid);
+
+	return job;
+
+error:
+	if (job)
+		nvhost_job_put(job);
+	return NULL;
+}
+
+void nvhost_job_get(struct nvhost_job *job)
+{
+	kref_get(&job->ref);
+}
+
+static void job_free(struct kref *ref)
+{
+	struct nvhost_job *job = container_of(ref, struct nvhost_job, ref);
+
+	if (job->hwctxref)
+		job->hwctxref->h->put(job->hwctxref);
+	if (job->hwctx)
+		job->hwctx->h->put(job->hwctx);
+	if (job->memmgr)
+		mem_op().put_mgr(job->memmgr);
+	vfree(job);
+}
+
+/* Acquire reference to a hardware context. Used for keeping saved contexts in
+ * memory. */
+void nvhost_job_get_hwctx(struct nvhost_job *job, struct nvhost_hwctx *hwctx)
+{
+	BUG_ON(job->hwctxref);
+
+	job->hwctxref = hwctx;
+	hwctx->h->get(hwctx);
+}
+
+void nvhost_job_put(struct nvhost_job *job)
+{
+	kref_put(&job->ref, job_free);
+}
+
+void nvhost_job_add_gather(struct nvhost_job *job,
+		u32 mem_id, u32 words, u32 offset)
+{
+	struct nvhost_job_gather *cur_gather =
+			&job->gathers[job->num_gathers];
+
+	cur_gather->words = words;
+	cur_gather->mem_id = mem_id;
+	cur_gather->offset = offset;
+	job->num_gathers += 1;
+}
+
+static int do_relocs(struct nvhost_job *job, u32 cmdbuf_mem, void *cmdbuf_addr)
+{
+	phys_addr_t target_phys = -EINVAL;
+	int i;
+	u32 mem_id = 0;
+	struct mem_handle *target_ref = NULL;
+
+	/* pin & patch the relocs for one gather */
+	for (i = 0; i < job->num_relocs; i++) {
+		struct nvhost_reloc *reloc = &job->relocarray[i];
+		struct nvhost_reloc_shift *shift = &job->relocshiftarray[i];
+
+		/* skip all other gathers */
+		if (cmdbuf_mem != reloc->cmdbuf_mem)
+			continue;
+
+		/* check if pin-mem is same as previous */
+		if (reloc->target != mem_id) {
+			target_ref = mem_op().get(job->memmgr, reloc->target);
+			if (IS_ERR(target_ref))
+				return PTR_ERR(target_ref);
+
+			target_phys = mem_op().pin(job->memmgr, target_ref);
+			if (IS_ERR((void *)target_phys)) {
+				mem_op().put(job->memmgr, target_ref);
+				return target_phys;
+			}
+
+			mem_id = reloc->target;
+			job->unpins[job->num_unpins++] = target_ref;
+		}
+
+		__raw_writel(
+			(target_phys + reloc->target_offset) >> shift->shift,
+			(cmdbuf_addr + reloc->cmdbuf_offset));
+
+		/* Different gathers might have same mem_id. This ensures we
+		 * perform reloc only once per gather memid. */
+		reloc->cmdbuf_mem = 0;
+	}
+
+	return 0;
+}
+
+/*
+ * Check driver supplied waitchk structs for syncpt thresholds
+ * that have already been satisfied and NULL the comparison (to
+ * avoid a wrap condition in the HW).
+ */
+static int do_waitchks(struct nvhost_job *job, struct nvhost_syncpt *sp,
+		u32 patch_mem, void *patch_addr)
+{
+	int i;
+
+	/* compare syncpt vs wait threshold */
+	for (i = 0; i < job->num_waitchk; i++) {
+		struct nvhost_waitchk *wait = &job->waitchk[i];
+
+		/* skip all other gathers */
+		if (patch_mem != wait->mem)
+			continue;
+
+		trace_nvhost_syncpt_wait_check(wait->mem, wait->offset,
+				wait->syncpt_id, wait->thresh,
+				nvhost_syncpt_read(sp, wait->syncpt_id));
+		if (nvhost_syncpt_is_expired(sp,
+					wait->syncpt_id, wait->thresh)) {
+			/*
+			 * NULL an already satisfied WAIT_SYNCPT host method,
+			 * by patching its args in the command stream. The
+			 * method data is changed to reference a reserved
+			 * (never given out or incr) NVSYNCPT_GRAPHICS_HOST
+			 * syncpt with a matching threshold value of 0, so
+			 * is guaranteed to be popped by the host HW.
+			 */
+			dev_dbg(&syncpt_to_dev(sp)->dev->dev,
+			    "drop WAIT id %d (%s) thresh 0x%x, min 0x%x\n",
+			    wait->syncpt_id,
+			    syncpt_op().name(sp, wait->syncpt_id),
+			    wait->thresh,
+			    nvhost_syncpt_read_min(sp, wait->syncpt_id));
+
+			/* patch the wait */
+			nvhost_syncpt_patch_wait(sp,
+					(patch_addr + wait->offset));
+		}
+
+		wait->mem = 0;
+	}
+	return 0;
+}
+
+int nvhost_job_pin(struct nvhost_job *job, struct nvhost_syncpt *sp)
+{
+	int err = 0, i = 0;
+	phys_addr_t gather_phys = 0;
+	void *gather_addr = NULL;
+	unsigned long waitchk_mask = job->waitchk_mask;
+
+	/* get current syncpt values for waitchk */
+	for_each_set_bit(i, &waitchk_mask, sizeof(job->waitchk_mask))
+		nvhost_syncpt_update_min(sp, i);
+
+	/* pin gathers */
+	for (i = 0; i < job->num_gathers; i++) {
+		struct nvhost_job_gather *g = &job->gathers[i];
+
+		/* process each gather mem only once */
+		if (!g->ref) {
+			g->ref = mem_op().get(job->memmgr,
+					job->gathers[i].mem_id);
+			if (IS_ERR(g->ref)) {
+				err = PTR_ERR(g->ref);
+				g->ref = NULL;
+				break;
+			}
+
+			gather_phys = mem_op().pin(job->memmgr, g->ref);
+			if (IS_ERR((void *)gather_phys)) {
+				mem_op().put(job->memmgr, g->ref);
+				err = gather_phys;
+				break;
+			}
+
+			/* store the gather ref into unpin array */
+			job->unpins[job->num_unpins++] = g->ref;
+
+			gather_addr = mem_op().mmap(g->ref);
+			if (!gather_addr) {
+				err = -ENOMEM;
+				break;
+			}
+
+			err = do_relocs(job, g->mem_id, gather_addr);
+			if (!err)
+				err = do_waitchks(job, sp,
+						g->mem_id, gather_addr);
+			mem_op().munmap(g->ref, gather_addr);
+
+			if (err)
+				break;
+		}
+		g->mem = gather_phys + g->offset;
+	}
+	wmb();
+
+	return err;
+}
+
+void nvhost_job_unpin(struct nvhost_job *job)
+{
+	int i;
+
+	for (i = 0; i < job->num_unpins; i++) {
+		struct mem_handle *handle;
+		handle = nvhost_nvmap_validate_ref(job->memmgr, job->unpins[i]);
+		mem_op().unpin(job->memmgr, handle);
+		mem_op().put(job->memmgr, handle);
+	}
+
+	memset(job->unpins, BAD_MAGIC,
+			job->num_unpins * sizeof(struct mem_handle *));
+	job->num_unpins = 0;
+}
+
+/**
+ * Debug routine used to dump job entries
+ */
+void nvhost_job_dump(struct device *dev, struct nvhost_job *job)
+{
+	dev_dbg(dev, "    SYNCPT_ID   %d\n",
+		job->syncpt_id);
+	dev_dbg(dev, "    SYNCPT_VAL  %d\n",
+		job->syncpt_end);
+	dev_dbg(dev, "    FIRST_GET   0x%x\n",
+		job->first_get);
+	dev_dbg(dev, "    TIMEOUT     %d\n",
+		job->timeout);
+	dev_dbg(dev, "    CTX 0x%p\n",
+		job->hwctx);
+	dev_dbg(dev, "    NUM_SLOTS   %d\n",
+		job->num_slots);
+	dev_dbg(dev, "    NUM_HANDLES %d\n",
+		job->num_unpins);
+}
diff --git a/drivers/video/tegra/host/nvhost_job.h b/drivers/video/tegra/host/nvhost_job.h
new file mode 100644
index 000000000000..3b444579c543
--- /dev/null
+++ b/drivers/video/tegra/host/nvhost_job.h
@@ -0,0 +1,148 @@
+/*
+ * drivers/video/tegra/host/nvhost_job.h
+ *
+ * Tegra Graphics Host Interrupt Management
+ *
+ * Copyright (c) 2011-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_JOB_H
+#define __NVHOST_JOB_H
+
+#include <linux/nvhost_ioctl.h>
+
+struct nvhost_channel;
+struct nvhost_hwctx;
+struct nvhost_waitchk;
+struct nvhost_syncpt;
+
+struct nvhost_job_gather {
+	u32 words;
+	phys_addr_t mem;
+	u32 mem_id;
+	int offset;
+	struct mem_handle *ref;
+};
+
+/*
+ * Each submit is tracked as a nvhost_job.
+ */
+struct nvhost_job {
+	/* When refcount goes to zero, job can be freed */
+	struct kref ref;
+
+	/* List entry */
+	struct list_head list;
+
+	/* Channel where job is submitted to */
+	struct nvhost_channel *ch;
+
+	/* Hardware context valid for this client */
+	struct nvhost_hwctx *hwctx;
+	int clientid;
+
+	/* Nvmap to be used for pinning & unpinning memory */
+	struct mem_mgr *memmgr;
+
+	/* Gathers and their memory */
+	struct nvhost_job_gather *gathers;
+	int num_gathers;
+
+	/* Wait checks to be processed at submit time */
+	struct nvhost_waitchk *waitchk;
+	int num_waitchk;
+	u32 waitchk_mask;
+
+	/* Array of handles to be pinned & unpinned */
+	struct nvhost_reloc *relocarray;
+	struct nvhost_reloc_shift *relocshiftarray;
+	int num_relocs;
+	struct mem_handle **unpins;
+	int num_unpins;
+
+	/* Sync point id, number of increments and end related to the submit */
+	u32 syncpt_id;
+	u32 syncpt_incrs;
+	u32 syncpt_end;
+
+	/* Priority of this submit. */
+	int priority;
+
+	/* Maximum time to wait for this job */
+	int timeout;
+
+	/* Null kickoff prevents submit from being sent to hardware */
+	bool null_kickoff;
+
+	/* Index and number of slots used in the push buffer */
+	int first_get;
+	int num_slots;
+
+	/* Context to be freed */
+	struct nvhost_hwctx *hwctxref;
+};
+
+/*
+ * Allocate memory for a job. Just enough memory will be allocated to
+ * accomodate the submit announced in submit header.
+ */
+struct nvhost_job *nvhost_job_alloc(struct nvhost_channel *ch,
+		struct nvhost_hwctx *hwctx,
+		struct nvhost_submit_hdr_ext *hdr,
+		struct mem_mgr *memmgr,
+		int priority, int clientid);
+
+/*
+ * Add a gather to a job.
+ */
+void nvhost_job_add_gather(struct nvhost_job *job,
+		u32 mem_id, u32 words, u32 offset);
+
+/*
+ * Increment reference going to nvhost_job.
+ */
+void nvhost_job_get(struct nvhost_job *job);
+
+/*
+ * Increment reference for a hardware context.
+ */
+void nvhost_job_get_hwctx(struct nvhost_job *job, struct nvhost_hwctx *hwctx);
+
+/*
+ * Decrement reference job, free if goes to zero.
+ */
+void nvhost_job_put(struct nvhost_job *job);
+
+/*
+ * Pin memory related to job. This handles relocation of addresses to the
+ * host1x address space. Handles both the gather memory and any other memory
+ * referred to from the gather buffers.
+ *
+ * Handles also patching out host waits that would wait for an expired sync
+ * point value.
+ */
+int nvhost_job_pin(struct nvhost_job *job, struct nvhost_syncpt *sp);
+
+/*
+ * Unpin memory related to job.
+ */
+void nvhost_job_unpin(struct nvhost_job *job);
+
+/*
+ * Dump contents of job to debug output.
+ */
+void nvhost_job_dump(struct device *dev, struct nvhost_job *job);
+
+#endif
diff --git a/drivers/video/tegra/host/nvhost_memmgr.c b/drivers/video/tegra/host/nvhost_memmgr.c
new file mode 100644
index 000000000000..f530c2e63006
--- /dev/null
+++ b/drivers/video/tegra/host/nvhost_memmgr.c
@@ -0,0 +1,34 @@
+/*
+ * drivers/video/tegra/host/nvhost_memmgr.c
+ *
+ * Tegra Graphics Host Memory Management Abstraction
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+
+#include "nvhost_memmgr.h"
+#include "nvmap.h"
+
+int nvhost_memmgr_init(struct nvhost_chip_support *chip)
+{
+#ifdef CONFIG_TEGRA_GRHOST_USE_NVMAP
+	return nvhost_init_nvmap_support(chip);
+#endif
+	BUG_ON(!"No memory manager selected");
+	return -ENODEV;
+}
diff --git a/drivers/video/tegra/host/nvhost_memmgr.h b/drivers/video/tegra/host/nvhost_memmgr.h
new file mode 100644
index 000000000000..d61379b6ff55
--- /dev/null
+++ b/drivers/video/tegra/host/nvhost_memmgr.h
@@ -0,0 +1,38 @@
+/*
+ * drivers/video/tegra/host/nvhost_memmgr.h
+ *
+ * Tegra Graphics Host Memory Management Abstraction header
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _NVHOST_MEM_MGR_H_
+#define _NVHOST_MEM_MGR_H_
+
+struct nvhost_chip_support;
+
+enum mem_mgr_flag {
+	mem_mgr_flag_uncacheable = 0,
+	mem_mgr_flag_write_combine = 1,
+};
+
+struct mem_mgr_handle {
+	struct mem_mgr *client;
+	struct mem_handle *handle;
+};
+
+int nvhost_memmgr_init(struct nvhost_chip_support *chip);
+
+#endif
diff --git a/drivers/video/tegra/host/nvhost_syncpt.c b/drivers/video/tegra/host/nvhost_syncpt.c
new file mode 100644
index 000000000000..5837a3f76cf0
--- /dev/null
+++ b/drivers/video/tegra/host/nvhost_syncpt.c
@@ -0,0 +1,512 @@
+/*
+ * drivers/video/tegra/host/nvhost_syncpt.c
+ *
+ * Tegra Graphics Host Syncpoints
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/nvhost_ioctl.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <trace/events/nvhost.h>
+#include "nvhost_syncpt.h"
+#include "nvhost_acm.h"
+#include "dev.h"
+#include "chip_support.h"
+
+#define MAX_SYNCPT_LENGTH 5
+/* Name of sysfs node for min and max value */
+static const char *min_name = "min";
+static const char *max_name = "max";
+
+/**
+ * Resets syncpoint and waitbase values to sw shadows
+ */
+void nvhost_syncpt_reset(struct nvhost_syncpt *sp)
+{
+	u32 i;
+	BUG_ON(!(syncpt_op().reset && syncpt_op().reset_wait_base));
+
+	for (i = 0; i < nvhost_syncpt_nb_pts(sp); i++)
+		syncpt_op().reset(sp, i);
+	for (i = 0; i < nvhost_syncpt_nb_bases(sp); i++)
+		syncpt_op().reset_wait_base(sp, i);
+	wmb();
+}
+
+/**
+ * Updates sw shadow state for client managed registers
+ */
+void nvhost_syncpt_save(struct nvhost_syncpt *sp)
+{
+	u32 i;
+	BUG_ON(!(syncpt_op().update_min && syncpt_op().read_wait_base));
+
+	for (i = 0; i < nvhost_syncpt_nb_pts(sp); i++) {
+		if (nvhost_syncpt_client_managed(sp, i))
+			syncpt_op().update_min(sp, i);
+		else
+			BUG_ON(!nvhost_syncpt_min_eq_max(sp, i));
+	}
+
+	for (i = 0; i < nvhost_syncpt_nb_bases(sp); i++)
+		syncpt_op().read_wait_base(sp, i);
+}
+
+/**
+ * Updates the last value read from hardware.
+ */
+u32 nvhost_syncpt_update_min(struct nvhost_syncpt *sp, u32 id)
+{
+	u32 val;
+
+	BUG_ON(!syncpt_op().update_min);
+
+	val = syncpt_op().update_min(sp, id);
+	trace_nvhost_syncpt_update_min(id, val);
+
+	return val;
+}
+
+/**
+ * Get the current syncpoint value
+ */
+u32 nvhost_syncpt_read(struct nvhost_syncpt *sp, u32 id)
+{
+	u32 val;
+	BUG_ON(!syncpt_op().update_min);
+	nvhost_module_busy(syncpt_to_dev(sp)->dev);
+	val = syncpt_op().update_min(sp, id);
+	nvhost_module_idle(syncpt_to_dev(sp)->dev);
+	return val;
+}
+
+/**
+ * Get the current syncpoint base
+ */
+u32 nvhost_syncpt_read_wait_base(struct nvhost_syncpt *sp, u32 id)
+{
+	u32 val;
+	BUG_ON(!syncpt_op().read_wait_base);
+	nvhost_module_busy(syncpt_to_dev(sp)->dev);
+	syncpt_op().read_wait_base(sp, id);
+	val = sp->base_val[id];
+	nvhost_module_idle(syncpt_to_dev(sp)->dev);
+	return val;
+}
+
+/**
+ * Write a cpu syncpoint increment to the hardware, without touching
+ * the cache. Caller is responsible for host being powered.
+ */
+void nvhost_syncpt_cpu_incr(struct nvhost_syncpt *sp, u32 id)
+{
+	BUG_ON(!syncpt_op().cpu_incr);
+	syncpt_op().cpu_incr(sp, id);
+}
+
+/**
+ * Increment syncpoint value from cpu, updating cache
+ */
+void nvhost_syncpt_incr(struct nvhost_syncpt *sp, u32 id)
+{
+	if (nvhost_syncpt_client_managed(sp, id))
+		nvhost_syncpt_incr_max(sp, id, 1);
+	nvhost_module_busy(syncpt_to_dev(sp)->dev);
+	nvhost_syncpt_cpu_incr(sp, id);
+	nvhost_module_idle(syncpt_to_dev(sp)->dev);
+}
+
+/**
+ * Updated sync point form hardware, and returns true if syncpoint is expired,
+ * false if we may need to wait
+ */
+static bool syncpt_update_min_is_expired(
+	struct nvhost_syncpt *sp,
+	u32 id,
+	u32 thresh)
+{
+	syncpt_op().update_min(sp, id);
+	return nvhost_syncpt_is_expired(sp, id, thresh);
+}
+
+/**
+ * Main entrypoint for syncpoint value waits.
+ */
+int nvhost_syncpt_wait_timeout(struct nvhost_syncpt *sp, u32 id,
+			u32 thresh, u32 timeout, u32 *value)
+{
+	DECLARE_WAIT_QUEUE_HEAD_ONSTACK(wq);
+	void *ref;
+	void *waiter;
+	int err = 0, check_count = 0, low_timeout = 0;
+	u32 val;
+
+	if (value)
+		*value = 0;
+
+	/* first check cache */
+	if (nvhost_syncpt_is_expired(sp, id, thresh)) {
+		if (value)
+			*value = nvhost_syncpt_read_min(sp, id);
+		return 0;
+	}
+
+	/* keep host alive */
+	nvhost_module_busy(syncpt_to_dev(sp)->dev);
+
+	/* try to read from register */
+	val = syncpt_op().update_min(sp, id);
+	if (nvhost_syncpt_is_expired(sp, id, thresh)) {
+		if (value)
+			*value = val;
+		goto done;
+	}
+
+	if (!timeout) {
+		err = -EAGAIN;
+		goto done;
+	}
+
+	/* schedule a wakeup when the syncpoint value is reached */
+	waiter = nvhost_intr_alloc_waiter();
+	if (!waiter) {
+		err = -ENOMEM;
+		goto done;
+	}
+
+	err = nvhost_intr_add_action(&(syncpt_to_dev(sp)->intr), id, thresh,
+				NVHOST_INTR_ACTION_WAKEUP_INTERRUPTIBLE, &wq,
+				waiter,
+				&ref);
+	if (err)
+		goto done;
+
+	err = -EAGAIN;
+	/* Caller-specified timeout may be impractically low */
+	if (timeout < SYNCPT_CHECK_PERIOD)
+		low_timeout = timeout;
+
+	/* wait for the syncpoint, or timeout, or signal */
+	while (timeout) {
+		u32 check = min_t(u32, SYNCPT_CHECK_PERIOD, timeout);
+		int remain = wait_event_interruptible_timeout(wq,
+				syncpt_update_min_is_expired(sp, id, thresh),
+				check);
+		if (remain > 0 || nvhost_syncpt_is_expired(sp, id, thresh)) {
+			if (value)
+				*value = nvhost_syncpt_read_min(sp, id);
+			err = 0;
+			break;
+		}
+		if (remain < 0) {
+			err = remain;
+			break;
+		}
+		if (timeout != NVHOST_NO_TIMEOUT)
+			timeout -= check;
+		if (timeout && check_count <= MAX_STUCK_CHECK_COUNT) {
+			dev_warn(&syncpt_to_dev(sp)->dev->dev,
+				"%s: syncpoint id %d (%s) stuck waiting %d, timeout=%d\n",
+				 current->comm, id, syncpt_op().name(sp, id),
+				 thresh, timeout);
+			syncpt_op().debug(sp);
+			if (check_count == MAX_STUCK_CHECK_COUNT) {
+				if (low_timeout) {
+					dev_warn(&syncpt_to_dev(sp)->dev->dev,
+						"is timeout %d too low?\n",
+						low_timeout);
+				}
+				nvhost_debug_dump(syncpt_to_dev(sp));
+			}
+			check_count++;
+		}
+	}
+	nvhost_intr_put_ref(&(syncpt_to_dev(sp)->intr), id, ref);
+
+done:
+	nvhost_module_idle(syncpt_to_dev(sp)->dev);
+	return err;
+}
+
+/**
+ * Returns true if syncpoint is expired, false if we may need to wait
+ */
+bool nvhost_syncpt_is_expired(
+	struct nvhost_syncpt *sp,
+	u32 id,
+	u32 thresh)
+{
+	u32 current_val;
+	u32 future_val;
+	smp_rmb();
+	current_val = (u32)atomic_read(&sp->min_val[id]);
+	future_val = (u32)atomic_read(&sp->max_val[id]);
+
+	/* Note the use of unsigned arithmetic here (mod 1<<32).
+	 *
+	 * c = current_val = min_val	= the current value of the syncpoint.
+	 * t = thresh			= the value we are checking
+	 * f = future_val  = max_val	= the value c will reach when all
+	 *			   	  outstanding increments have completed.
+	 *
+	 * Note that c always chases f until it reaches f.
+	 *
+	 * Dtf = (f - t)
+	 * Dtc = (c - t)
+	 *
+	 *  Consider all cases:
+	 *
+	 *	A) .....c..t..f.....	Dtf < Dtc	need to wait
+	 *	B) .....c.....f..t..	Dtf > Dtc	expired
+	 *	C) ..t..c.....f.....	Dtf > Dtc	expired	   (Dct very large)
+	 *
+	 *  Any case where f==c: always expired (for any t).  	Dtf == Dcf
+	 *  Any case where t==c: always expired (for any f).  	Dtf >= Dtc (because Dtc==0)
+	 *  Any case where t==f!=c: always wait.	 	Dtf <  Dtc (because Dtf==0,
+	 *							Dtc!=0)
+	 *
+	 *  Other cases:
+	 *
+	 *	A) .....t..f..c.....	Dtf < Dtc	need to wait
+	 *	A) .....f..c..t.....	Dtf < Dtc	need to wait
+	 *	A) .....f..t..c.....	Dtf > Dtc	expired
+	 *
+	 *   So:
+	 *	   Dtf >= Dtc implies EXPIRED	(return true)
+	 *	   Dtf <  Dtc implies WAIT	(return false)
+	 *
+	 * Note: If t is expired then we *cannot* wait on it. We would wait
+	 * forever (hang the system).
+	 *
+	 * Note: do NOT get clever and remove the -thresh from both sides. It
+	 * is NOT the same.
+	 *
+	 * If future valueis zero, we have a client managed sync point. In that
+	 * case we do a direct comparison.
+	 */
+	if (!nvhost_syncpt_client_managed(sp, id))
+		return future_val - thresh >= current_val - thresh;
+	else
+		return (s32)(current_val - thresh) >= 0;
+}
+
+void nvhost_syncpt_debug(struct nvhost_syncpt *sp)
+{
+	syncpt_op().debug(sp);
+}
+
+int nvhost_mutex_try_lock(struct nvhost_syncpt *sp, int idx)
+{
+	struct nvhost_master *host = syncpt_to_dev(sp);
+	u32 reg;
+
+	nvhost_module_busy(host->dev);
+	reg = syncpt_op().mutex_try_lock(sp, idx);
+	if (reg) {
+		nvhost_module_idle(host->dev);
+		return -EBUSY;
+	}
+	atomic_inc(&sp->lock_counts[idx]);
+	return 0;
+}
+
+void nvhost_mutex_unlock(struct nvhost_syncpt *sp, int idx)
+{
+	syncpt_op().mutex_unlock(sp, idx);
+	nvhost_module_idle(syncpt_to_dev(sp)->dev);
+	atomic_dec(&sp->lock_counts[idx]);
+}
+
+/* remove a wait pointed to by patch_addr */
+int nvhost_syncpt_patch_wait(struct nvhost_syncpt *sp, void *patch_addr)
+{
+	return syncpt_op().patch_wait(sp, patch_addr);
+}
+
+/* Displays the current value of the sync point via sysfs */
+static ssize_t syncpt_min_show(struct kobject *kobj,
+		struct kobj_attribute *attr, char *buf)
+{
+	struct nvhost_syncpt_attr *syncpt_attr =
+		container_of(attr, struct nvhost_syncpt_attr, attr);
+
+	return snprintf(buf, PAGE_SIZE, "%u",
+			nvhost_syncpt_read(&syncpt_attr->host->syncpt,
+				syncpt_attr->id));
+}
+
+static ssize_t syncpt_max_show(struct kobject *kobj,
+		struct kobj_attribute *attr, char *buf)
+{
+	struct nvhost_syncpt_attr *syncpt_attr =
+		container_of(attr, struct nvhost_syncpt_attr, attr);
+
+	return snprintf(buf, PAGE_SIZE, "%u",
+			nvhost_syncpt_read_max(&syncpt_attr->host->syncpt,
+				syncpt_attr->id));
+}
+
+int nvhost_syncpt_init(struct nvhost_device *dev,
+		struct nvhost_syncpt *sp)
+{
+	int i;
+	struct nvhost_master *host = syncpt_to_dev(sp);
+	int err = 0;
+
+	/* Allocate structs for min, max and base values */
+	sp->min_val = kzalloc(sizeof(atomic_t) * nvhost_syncpt_nb_pts(sp),
+			GFP_KERNEL);
+	sp->max_val = kzalloc(sizeof(atomic_t) * nvhost_syncpt_nb_pts(sp),
+			GFP_KERNEL);
+	sp->base_val = kzalloc(sizeof(u32) * nvhost_syncpt_nb_bases(sp),
+			GFP_KERNEL);
+	sp->lock_counts =
+		kzalloc(sizeof(atomic_t) * nvhost_syncpt_nb_mlocks(sp),
+			GFP_KERNEL);
+
+	if (!(sp->min_val && sp->max_val && sp->base_val && sp->lock_counts)) {
+		/* frees happen in the deinit */
+		err = -ENOMEM;
+		goto fail;
+	}
+
+	sp->kobj = kobject_create_and_add("syncpt", &dev->dev.kobj);
+	if (!sp->kobj) {
+		err = -EIO;
+		goto fail;
+	}
+
+	/* Allocate two attributes for each sync point: min and max */
+	sp->syncpt_attrs = kzalloc(sizeof(*sp->syncpt_attrs)
+			* nvhost_syncpt_nb_pts(sp) * 2, GFP_KERNEL);
+	if (!sp->syncpt_attrs) {
+		err = -ENOMEM;
+		goto fail;
+	}
+
+	/* Fill in the attributes */
+	for (i = 0; i < nvhost_syncpt_nb_pts(sp); i++) {
+		char name[MAX_SYNCPT_LENGTH];
+		struct kobject *kobj;
+		struct nvhost_syncpt_attr *min = &sp->syncpt_attrs[i*2];
+		struct nvhost_syncpt_attr *max = &sp->syncpt_attrs[i*2+1];
+
+		/* Create one directory per sync point */
+		snprintf(name, sizeof(name), "%d", i);
+		kobj = kobject_create_and_add(name, sp->kobj);
+		if (!kobj) {
+			err = -EIO;
+			goto fail;
+		}
+
+		min->id = i;
+		min->host = host;
+		min->attr.attr.name = min_name;
+		min->attr.attr.mode = S_IRUGO;
+		min->attr.show = syncpt_min_show;
+		if (sysfs_create_file(kobj, &min->attr.attr)) {
+			err = -EIO;
+			goto fail;
+		}
+
+		max->id = i;
+		max->host = host;
+		max->attr.attr.name = max_name;
+		max->attr.attr.mode = S_IRUGO;
+		max->attr.show = syncpt_max_show;
+		if (sysfs_create_file(kobj, &max->attr.attr)) {
+			err = -EIO;
+			goto fail;
+		}
+	}
+
+	return err;
+
+fail:
+	nvhost_syncpt_deinit(sp);
+	return err;
+}
+
+void nvhost_syncpt_deinit(struct nvhost_syncpt *sp)
+{
+	kobject_put(sp->kobj);
+
+	kfree(sp->min_val);
+	sp->min_val = NULL;
+
+	kfree(sp->max_val);
+	sp->max_val = NULL;
+
+	kfree(sp->base_val);
+	sp->base_val = NULL;
+
+	kfree(sp->lock_counts);
+	sp->lock_counts = 0;
+
+	kfree(sp->syncpt_attrs);
+	sp->syncpt_attrs = NULL;
+}
+
+int nvhost_syncpt_client_managed(struct nvhost_syncpt *sp, u32 id)
+{
+	return BIT(id) & syncpt_to_dev(sp)->info.client_managed;
+}
+
+int nvhost_syncpt_nb_pts(struct nvhost_syncpt *sp)
+{
+	return syncpt_to_dev(sp)->info.nb_pts;
+}
+
+int nvhost_syncpt_nb_bases(struct nvhost_syncpt *sp)
+{
+	return syncpt_to_dev(sp)->info.nb_bases;
+}
+
+int nvhost_syncpt_nb_mlocks(struct nvhost_syncpt *sp)
+{
+	return syncpt_to_dev(sp)->info.nb_mlocks;
+}
+
+/* public sync point API */
+u32 nvhost_syncpt_incr_max_ext(struct nvhost_device *dev, u32 id, u32 incrs)
+{
+	struct nvhost_syncpt *sp = &(nvhost_get_host(dev)->syncpt);
+	return nvhost_syncpt_incr_max(sp, id, incrs);
+}
+EXPORT_SYMBOL(nvhost_syncpt_incr_max_ext);
+
+void nvhost_syncpt_cpu_incr_ext(struct nvhost_device *dev, u32 id)
+{
+	struct nvhost_syncpt *sp = &(nvhost_get_host(dev)->syncpt);
+	nvhost_syncpt_cpu_incr(sp, id);
+}
+EXPORT_SYMBOL(nvhost_syncpt_cpu_incr_ext);
+
+u32 nvhost_syncpt_read_ext(struct nvhost_device *dev, u32 id)
+{
+	struct nvhost_syncpt *sp = &(nvhost_get_host(dev)->syncpt);
+	return nvhost_syncpt_read(sp, id);
+}
+EXPORT_SYMBOL(nvhost_syncpt_read_ext);
+
+int nvhost_syncpt_wait_timeout_ext(struct nvhost_device *dev, u32 id, u32 thresh,
+	u32 timeout, u32 *value)
+{
+	struct nvhost_syncpt *sp = &(nvhost_get_host(dev)->syncpt);
+	return nvhost_syncpt_wait_timeout(sp, id, thresh, timeout, value);
+}
+EXPORT_SYMBOL(nvhost_syncpt_wait_timeout_ext);
diff --git a/drivers/video/tegra/host/nvhost_syncpt.h b/drivers/video/tegra/host/nvhost_syncpt.h
new file mode 100644
index 000000000000..9ee4f3a8d49d
--- /dev/null
+++ b/drivers/video/tegra/host/nvhost_syncpt.h
@@ -0,0 +1,151 @@
+/*
+ * drivers/video/tegra/host/nvhost_syncpt.h
+ *
+ * Tegra Graphics Host Syncpoints
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_SYNCPT_H
+#define __NVHOST_SYNCPT_H
+
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/nvhost.h>
+#include <linux/atomic.h>
+
+/* host managed and invalid syncpt id */
+#define NVSYNCPT_GRAPHICS_HOST		     (0)
+
+/* Attribute struct for sysfs min and max attributes */
+struct nvhost_syncpt_attr {
+	struct kobj_attribute attr;
+	struct nvhost_master *host;
+	int id;
+};
+
+struct nvhost_syncpt {
+	struct kobject *kobj;
+	atomic_t *min_val;
+	atomic_t *max_val;
+	u32 *base_val;
+	atomic_t *lock_counts;
+	const char **syncpt_names;
+	struct nvhost_syncpt_attr *syncpt_attrs;
+};
+
+int nvhost_syncpt_init(struct nvhost_device *, struct nvhost_syncpt *);
+void nvhost_syncpt_deinit(struct nvhost_syncpt *);
+
+#define syncpt_to_dev(sp) container_of(sp, struct nvhost_master, syncpt)
+#define SYNCPT_CHECK_PERIOD (2 * HZ)
+#define MAX_STUCK_CHECK_COUNT 15
+
+/**
+ * Updates the value sent to hardware.
+ */
+static inline u32 nvhost_syncpt_incr_max(struct nvhost_syncpt *sp,
+					u32 id, u32 incrs)
+{
+	return (u32)atomic_add_return(incrs, &sp->max_val[id]);
+}
+
+/**
+ * Updated the value sent to hardware.
+ */
+static inline u32 nvhost_syncpt_set_max(struct nvhost_syncpt *sp,
+					u32 id, u32 val)
+{
+	atomic_set(&sp->max_val[id], val);
+	smp_wmb();
+	return val;
+}
+
+static inline u32 nvhost_syncpt_read_max(struct nvhost_syncpt *sp, u32 id)
+{
+	smp_rmb();
+	return (u32)atomic_read(&sp->max_val[id]);
+}
+
+static inline u32 nvhost_syncpt_read_min(struct nvhost_syncpt *sp, u32 id)
+{
+	smp_rmb();
+	return (u32)atomic_read(&sp->min_val[id]);
+}
+
+int nvhost_syncpt_client_managed(struct nvhost_syncpt *sp, u32 id);
+int nvhost_syncpt_nb_pts(struct nvhost_syncpt *sp);
+int nvhost_syncpt_nb_bases(struct nvhost_syncpt *sp);
+int nvhost_syncpt_nb_mlocks(struct nvhost_syncpt *sp);
+
+static inline bool nvhost_syncpt_check_max(struct nvhost_syncpt *sp,
+		u32 id, u32 real)
+{
+	u32 max;
+	if (nvhost_syncpt_client_managed(sp, id))
+		return true;
+	max = nvhost_syncpt_read_max(sp, id);
+	return (s32)(max - real) >= 0;
+}
+
+/**
+ * Returns true if syncpoint min == max
+ */
+static inline bool nvhost_syncpt_min_eq_max(struct nvhost_syncpt *sp, u32 id)
+{
+	int min, max;
+	smp_rmb();
+	min = atomic_read(&sp->min_val[id]);
+	max = atomic_read(&sp->max_val[id]);
+	return (min == max);
+}
+
+void nvhost_syncpt_cpu_incr(struct nvhost_syncpt *sp, u32 id);
+
+u32 nvhost_syncpt_update_min(struct nvhost_syncpt *sp, u32 id);
+bool nvhost_syncpt_is_expired(struct nvhost_syncpt *sp, u32 id, u32 thresh);
+
+void nvhost_syncpt_save(struct nvhost_syncpt *sp);
+
+void nvhost_syncpt_reset(struct nvhost_syncpt *sp);
+
+u32 nvhost_syncpt_read(struct nvhost_syncpt *sp, u32 id);
+u32 nvhost_syncpt_read_wait_base(struct nvhost_syncpt *sp, u32 id);
+
+void nvhost_syncpt_incr(struct nvhost_syncpt *sp, u32 id);
+
+int nvhost_syncpt_wait_timeout(struct nvhost_syncpt *sp, u32 id, u32 thresh,
+			u32 timeout, u32 *value);
+
+static inline int nvhost_syncpt_wait(struct nvhost_syncpt *sp, u32 id, u32 thresh)
+{
+	return nvhost_syncpt_wait_timeout(sp, id, thresh,
+					  MAX_SCHEDULE_TIMEOUT, NULL);
+}
+
+int nvhost_syncpt_patch_wait(struct nvhost_syncpt *sp, void *patch_addr);
+
+void nvhost_syncpt_debug(struct nvhost_syncpt *sp);
+
+static inline int nvhost_syncpt_is_valid(struct nvhost_syncpt *sp, u32 id)
+{
+	return id != NVSYNCPT_INVALID && id < nvhost_syncpt_nb_pts(sp);
+}
+
+int nvhost_mutex_try_lock(struct nvhost_syncpt *sp, int idx);
+
+void nvhost_mutex_unlock(struct nvhost_syncpt *sp, int idx);
+
+#endif
diff --git a/drivers/video/tegra/host/nvmap.c b/drivers/video/tegra/host/nvmap.c
new file mode 100644
index 000000000000..b8361c4a1a36
--- /dev/null
+++ b/drivers/video/tegra/host/nvmap.c
@@ -0,0 +1,109 @@
+/*
+ * drivers/video/tegra/host/nvmap.c
+ *
+ * Tegra Graphics Host Nvmap support
+ *
+ * Copyright (c) 2012-2014, NVIDIA 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "chip_support.h"
+#include <linux/nvmap.h>
+
+struct mem_mgr *nvhost_nvmap_alloc_mgr(void)
+{
+	return (struct mem_mgr *)nvmap_create_client(nvmap_dev, "nvhost");
+}
+
+void nvhost_nvmap_put_mgr(struct mem_mgr *mgr)
+{
+	nvmap_client_put((struct nvmap_client *)mgr);
+}
+
+struct mem_mgr *nvhost_nvmap_get_mgr(struct mem_mgr *mgr)
+{
+	return (struct mem_mgr *)nvmap_client_get((struct nvmap_client *)mgr);
+}
+
+struct mem_mgr *nvhost_nvmap_get_mgr_file(int fd)
+{
+	return (struct mem_mgr *)nvmap_client_get_file(fd);
+}
+
+struct mem_handle *nvhost_nvmap_alloc(struct mem_mgr *mgr,
+		size_t size, size_t align, int flags)
+{
+	return (struct mem_handle *)nvmap_alloc((struct nvmap_client *)mgr,
+			size, align, flags, 0);
+}
+
+void nvhost_nvmap_put(struct mem_mgr *mgr, struct mem_handle *handle)
+{
+	return nvmap_free((struct nvmap_client *)mgr,
+			(struct nvmap_handle_ref *)handle);
+}
+
+phys_addr_t nvhost_nvmap_pin(struct mem_mgr *mgr, struct mem_handle *handle)
+{
+	return nvmap_pin((struct nvmap_client *)mgr,
+			(struct nvmap_handle_ref *)handle);
+}
+
+void nvhost_nvmap_unpin(struct mem_mgr *mgr, struct mem_handle *handle)
+{
+	return nvmap_unpin((struct nvmap_client *)mgr,
+			(struct nvmap_handle_ref *)handle);
+}
+
+void *nvhost_nvmap_mmap(struct mem_handle *handle)
+{
+	return nvmap_mmap((struct nvmap_handle_ref *)handle);
+}
+
+void nvhost_nvmap_munmap(struct mem_handle *handle, void *addr)
+{
+	nvmap_munmap((struct nvmap_handle_ref *)handle, addr);
+}
+
+struct mem_handle *nvhost_nvmap_get(struct mem_mgr *mgr, u32 id)
+{
+	return (struct mem_handle *)
+		nvmap_duplicate_handle_id((struct nvmap_client *)mgr, id);
+}
+
+struct mem_handle *nvhost_nvmap_validate_ref(struct mem_mgr *mgr,
+				struct mem_handle *handle)
+{
+	unsigned long ref;
+	ref = nvmap_validate_ref((struct nvmap_client *)mgr,
+			(struct nvmap_handle_ref *)handle);
+	return (struct mem_handle *)ref;
+}
+
+int nvhost_init_nvmap_support(struct nvhost_chip_support *chip)
+{
+	chip->mem.alloc_mgr = nvhost_nvmap_alloc_mgr;
+	chip->mem.put_mgr = nvhost_nvmap_put_mgr;
+	chip->mem.get_mgr = nvhost_nvmap_get_mgr;
+	chip->mem.get_mgr_file = nvhost_nvmap_get_mgr_file;
+	chip->mem.alloc = nvhost_nvmap_alloc;
+	chip->mem.put = nvhost_nvmap_put;
+	chip->mem.get = nvhost_nvmap_get;
+	chip->mem.pin = nvhost_nvmap_pin;
+	chip->mem.unpin = nvhost_nvmap_unpin;
+	chip->mem.mmap = nvhost_nvmap_mmap;
+	chip->mem.munmap = nvhost_nvmap_munmap;
+
+	return 0;
+}
diff --git a/drivers/video/tegra/host/nvmap.h b/drivers/video/tegra/host/nvmap.h
new file mode 100644
index 000000000000..25e3535ce544
--- /dev/null
+++ b/drivers/video/tegra/host/nvmap.h
@@ -0,0 +1,28 @@
+/*
+ * drivers/video/tegra/host/nvmap.h
+ *
+ * Tegra Graphics Host nvmap memory manager
+ *
+ * Copyright (c) 2010-2014, NVIDIA 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __NVHOST_NVMAP_H
+#define __NVHOST_NVMAP_H
+
+struct nvhost_chip_support;
+int nvhost_init_nvmap_support(struct nvhost_chip_support *op);
+struct mem_handle *nvhost_nvmap_validate_ref(struct mem_mgr *mgr,
+				struct mem_handle *handle);
+#endif
diff --git a/drivers/video/tegra/host/t20/Makefile b/drivers/video/tegra/host/t20/Makefile
new file mode 100644
index 000000000000..c2ade9bf925b
--- /dev/null
+++ b/drivers/video/tegra/host/t20/Makefile
@@ -0,0 +1,8 @@
+GCOV_PROFILE := y
+
+EXTRA_CFLAGS += -Idrivers/video/tegra/host
+
+nvhost-t20-objs  = \
+	t20.o
+
+obj-$(CONFIG_TEGRA_GRHOST) += nvhost-t20.o
diff --git a/drivers/video/tegra/host/t20/t20.c b/drivers/video/tegra/host/t20/t20.c
new file mode 100644
index 000000000000..e6840ae7ba1c
--- /dev/null
+++ b/drivers/video/tegra/host/t20/t20.c
@@ -0,0 +1,294 @@
+/*
+ * drivers/video/tegra/host/t20/t20.c
+ *
+ * Tegra Graphics Init for T20 Architecture Chips
+ *
+ * Copyright (c) 2011-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/nvhost_ioctl.h>
+#include <mach/powergate.h>
+#include <mach/iomap.h>
+#include "t20.h"
+#include "gr3d/gr3d_t20.h"
+#include "mpe/mpe.h"
+#include "host1x/host1x.h"
+#include "nvhost_channel.h"
+#include "nvhost_memmgr.h"
+#include "host1x/host1x01_hardware.h"
+#include "host1x/host1x_syncpt.h"
+#include "chip_support.h"
+
+#define NVMODMUTEX_2D_FULL	(1)
+#define NVMODMUTEX_2D_SIMPLE	(2)
+#define NVMODMUTEX_2D_SB_A	(3)
+#define NVMODMUTEX_2D_SB_B	(4)
+#define NVMODMUTEX_3D		(5)
+#define NVMODMUTEX_DISPLAYA	(6)
+#define NVMODMUTEX_DISPLAYB	(7)
+#define NVMODMUTEX_VI		(8)
+#define NVMODMUTEX_DSI		(9)
+
+static int t20_num_alloc_channels = 0;
+
+static struct resource tegra_host1x01_resources[] = {
+	{
+		.start = TEGRA_HOST1X_BASE,
+		.end = TEGRA_HOST1X_BASE + TEGRA_HOST1X_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = INT_SYNCPT_THRESH_BASE,
+		.end = INT_SYNCPT_THRESH_BASE + INT_SYNCPT_THRESH_NR - 1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = INT_HOST1X_MPCORE_GENERAL,
+		.end = INT_HOST1X_MPCORE_GENERAL,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static const char *s_syncpt_names[32] = {
+	"gfx_host",
+	"", "", "", "", "", "", "",
+	"disp0_a", "disp1_a", "avp_0",
+	"csi_vi_0", "csi_vi_1",
+	"vi_isp_0", "vi_isp_1", "vi_isp_2", "vi_isp_3", "vi_isp_4",
+	"2d_0", "2d_1",
+	"disp0_b", "disp1_b",
+	"3d",
+	"mpe",
+	"disp0_c", "disp1_c",
+	"vblank0", "vblank1",
+	"mpe_ebm_eof", "mpe_wr_safe",
+	"2d_tinyblt",
+	"dsi"
+};
+
+static struct host1x_device_info host1x01_info = {
+	.nb_channels	= 8,
+	.nb_pts		= 32,
+	.nb_mlocks	= 16,
+	.nb_bases	= 8,
+	.syncpt_names	= s_syncpt_names,
+	.client_managed	= NVSYNCPTS_CLIENT_MANAGED,
+};
+
+static struct nvhost_device tegra_host1x01_device = {
+	.dev		= {.platform_data = &host1x01_info},
+	.name		= "host1x",
+	.id		= -1,
+	.resource	= tegra_host1x01_resources,
+	.num_resources	= ARRAY_SIZE(tegra_host1x01_resources),
+	.clocks		= {{"host1x", UINT_MAX}, {} },
+	NVHOST_MODULE_NO_POWERGATE_IDS,
+};
+
+static struct nvhost_device tegra_display01_device = {
+	.name		= "display",
+	.id		= -1,
+	.index		= 0,
+	.syncpts	= BIT(NVSYNCPT_DISP0_A) | BIT(NVSYNCPT_DISP1_A) |
+			  BIT(NVSYNCPT_DISP0_B) | BIT(NVSYNCPT_DISP1_B) |
+			  BIT(NVSYNCPT_DISP0_C) | BIT(NVSYNCPT_DISP1_C) |
+			  BIT(NVSYNCPT_VBLANK0) | BIT(NVSYNCPT_VBLANK1),
+	.modulemutexes	= BIT(NVMODMUTEX_DISPLAYA) | BIT(NVMODMUTEX_DISPLAYB),
+	NVHOST_MODULE_NO_POWERGATE_IDS,
+	NVHOST_DEFAULT_CLOCKGATE_DELAY,
+	.moduleid	= NVHOST_MODULE_NONE,
+};
+
+static struct nvhost_device tegra_gr3d01_device = {
+	.name		= "gr3d",
+	.version	= 1,
+	.id		= -1,
+	.index		= 1,
+	.syncpts	= BIT(NVSYNCPT_3D),
+	.waitbases	= BIT(NVWAITBASE_3D),
+	.modulemutexes	= BIT(NVMODMUTEX_3D),
+	.class		= NV_GRAPHICS_3D_CLASS_ID,
+	.clocks		= {{"gr3d", UINT_MAX}, {"emc", UINT_MAX}, {} },
+	.powergate_ids	= {TEGRA_POWERGATE_3D, -1},
+	NVHOST_DEFAULT_CLOCKGATE_DELAY,
+	.moduleid	= NVHOST_MODULE_NONE,
+};
+
+static struct nvhost_device tegra_gr2d01_device = {
+	.name		= "gr2d",
+	.id		= -1,
+	.index		= 2,
+	.syncpts	= BIT(NVSYNCPT_2D_0) | BIT(NVSYNCPT_2D_1),
+	.waitbases	= BIT(NVWAITBASE_2D_0) | BIT(NVWAITBASE_2D_1),
+	.modulemutexes	= BIT(NVMODMUTEX_2D_FULL) | BIT(NVMODMUTEX_2D_SIMPLE) |
+			  BIT(NVMODMUTEX_2D_SB_A) | BIT(NVMODMUTEX_2D_SB_B),
+	.clocks		= { {"gr2d", UINT_MAX},
+			    {"epp", UINT_MAX},
+			    {"emc", UINT_MAX} },
+	NVHOST_MODULE_NO_POWERGATE_IDS,
+	.clockgate_delay = 0,
+	.moduleid	= NVHOST_MODULE_NONE,
+	.serialize	= true,
+};
+
+static struct resource isp_resources_t20[] = {
+	{
+		.name = "regs",
+		.start = TEGRA_ISP_BASE,
+		.end = TEGRA_ISP_BASE + TEGRA_ISP_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	}
+};
+
+static struct nvhost_device tegra_isp01_device = {
+	.name		= "isp",
+	.id		= -1,
+	.resource = isp_resources_t20,
+	.num_resources = ARRAY_SIZE(isp_resources_t20),
+	.index		= 3,
+	.syncpts	= 0,
+	NVHOST_MODULE_NO_POWERGATE_IDS,
+	NVHOST_DEFAULT_CLOCKGATE_DELAY,
+	.moduleid	= NVHOST_MODULE_ISP,
+};
+
+static struct resource vi_resources[] = {
+	{
+		.name = "regs",
+		.start = TEGRA_VI_BASE,
+		.end = TEGRA_VI_BASE + TEGRA_VI_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static struct nvhost_device tegra_vi01_device = {
+	.name		= "vi",
+	.resource = vi_resources,
+	.num_resources = ARRAY_SIZE(vi_resources),
+	.id		= -1,
+	.index		= 4,
+	.syncpts	= BIT(NVSYNCPT_CSI_VI_0) | BIT(NVSYNCPT_CSI_VI_1) |
+			  BIT(NVSYNCPT_VI_ISP_0) | BIT(NVSYNCPT_VI_ISP_1) |
+			  BIT(NVSYNCPT_VI_ISP_2) | BIT(NVSYNCPT_VI_ISP_3) |
+			  BIT(NVSYNCPT_VI_ISP_4),
+	.modulemutexes	= BIT(NVMODMUTEX_VI),
+	.exclusive	= true,
+	NVHOST_MODULE_NO_POWERGATE_IDS,
+	NVHOST_DEFAULT_CLOCKGATE_DELAY,
+	.moduleid	= NVHOST_MODULE_VI,
+};
+
+static struct resource tegra_mpe01_resources[] = {
+	{
+		.name = "regs",
+		.start = TEGRA_MPE_BASE,
+		.end = TEGRA_MPE_BASE + TEGRA_MPE_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static struct nvhost_device tegra_mpe01_device = {
+	.name		= "mpe",
+	.version	= 1,
+	.id		= -1,
+	.resource	= tegra_mpe01_resources,
+	.num_resources	= ARRAY_SIZE(tegra_mpe01_resources),
+	.index		= 5,
+	.syncpts	= BIT(NVSYNCPT_MPE) | BIT(NVSYNCPT_MPE_EBM_EOF) |
+			  BIT(NVSYNCPT_MPE_WR_SAFE),
+	.waitbases	= BIT(NVWAITBASE_MPE),
+	.class		= NV_VIDEO_ENCODE_MPEG_CLASS_ID,
+	.waitbasesync	= true,
+	.keepalive	= true,
+	.clocks		= { {"mpe", UINT_MAX},
+			    {"emc", UINT_MAX} },
+	.powergate_ids	= {TEGRA_POWERGATE_MPE, -1},
+	NVHOST_DEFAULT_CLOCKGATE_DELAY,
+	.moduleid	= NVHOST_MODULE_MPE,
+};
+
+static struct nvhost_device tegra_dsi01_device = {
+	.name		= "dsi",
+	.id		= -1,
+	.index		= 6,
+	.syncpts	= BIT(NVSYNCPT_DSI),
+	.modulemutexes	= BIT(NVMODMUTEX_DSI),
+	NVHOST_MODULE_NO_POWERGATE_IDS,
+	NVHOST_DEFAULT_CLOCKGATE_DELAY,
+	.moduleid	= NVHOST_MODULE_NONE,
+};
+
+static struct nvhost_device *t20_devices[] = {
+	&tegra_host1x01_device,
+	&tegra_display01_device,
+	&tegra_gr3d01_device,
+	&tegra_gr2d01_device,
+	&tegra_isp01_device,
+	&tegra_vi01_device,
+	&tegra_mpe01_device,
+	&tegra_dsi01_device,
+};
+
+int tegra2_register_host1x_devices(void)
+{
+	return nvhost_add_devices(t20_devices, ARRAY_SIZE(t20_devices));
+}
+
+static void t20_free_nvhost_channel(struct nvhost_channel *ch)
+{
+	nvhost_free_channel_internal(ch, &t20_num_alloc_channels);
+}
+
+static struct nvhost_channel *t20_alloc_nvhost_channel(
+		struct nvhost_device *dev)
+{
+	return nvhost_alloc_channel_internal(dev->index,
+		nvhost_get_host(dev)->info.nb_channels,
+		&t20_num_alloc_channels);
+}
+
+#include "host1x/host1x_channel.c"
+#include "host1x/host1x_cdma.c"
+#include "host1x/host1x_debug.c"
+#include "host1x/host1x_syncpt.c"
+#include "host1x/host1x_intr.c"
+
+int nvhost_init_t20_support(struct nvhost_master *host,
+	struct nvhost_chip_support *op)
+{
+	int err;
+
+	op->channel = host1x_channel_ops;
+	op->cdma = host1x_cdma_ops;
+	op->push_buffer = host1x_pushbuffer_ops;
+	op->debug = host1x_debug_ops;
+	host->sync_aperture = host->aperture + HOST1X_CHANNEL_SYNC_REG_BASE;
+	op->syncpt = host1x_syncpt_ops;
+	op->intr = host1x_intr_ops;
+	err = nvhost_memmgr_init(op);
+	if (err)
+		return err;
+
+	op->nvhost_dev.alloc_nvhost_channel = t20_alloc_nvhost_channel;
+	op->nvhost_dev.free_nvhost_channel = t20_free_nvhost_channel;
+
+	return 0;
+}
+
+/* Hacky way to get access to struct nvhost_device tegra_vi01_device. */
+struct nvhost_device *t20_get_tegra_vi01_device(void)
+{
+	return &tegra_vi01_device;
+}
diff --git a/drivers/video/tegra/host/t20/t20.h b/drivers/video/tegra/host/t20/t20.h
new file mode 100644
index 000000000000..729f9d8e85e4
--- /dev/null
+++ b/drivers/video/tegra/host/t20/t20.h
@@ -0,0 +1,29 @@
+/*
+ * drivers/video/tegra/host/t20/t20.h
+ *
+ * Tegra Graphics Chip support for T20
+ *
+ * Copyright (c) 2011-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef _NVHOST_T20_H_
+#define _NVHOST_T20_H_
+
+struct nvhost_master;
+struct nvhost_chip_support;
+
+int nvhost_init_t20_support(struct nvhost_master *,
+	struct nvhost_chip_support *);
+
+#endif /* _NVHOST_T20_H_ */
diff --git a/drivers/video/tegra/host/t30/Makefile b/drivers/video/tegra/host/t30/Makefile
new file mode 100644
index 000000000000..b343eb4fc7cc
--- /dev/null
+++ b/drivers/video/tegra/host/t30/Makefile
@@ -0,0 +1,8 @@
+GCOV_PROFILE := y
+
+EXTRA_CFLAGS += -Idrivers/video/tegra/host
+
+nvhost-t30-objs  = \
+	t30.o
+
+obj-$(CONFIG_TEGRA_GRHOST) += nvhost-t30.o
diff --git a/drivers/video/tegra/host/t30/t30.c b/drivers/video/tegra/host/t30/t30.c
new file mode 100644
index 000000000000..6c3a7f925177
--- /dev/null
+++ b/drivers/video/tegra/host/t30/t30.c
@@ -0,0 +1,309 @@
+/*
+ * drivers/video/tegra/host/t30/t30.c
+ *
+ * Tegra Graphics Init for T30 Architecture Chips
+ *
+ * Copyright (c) 2011-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/nvhost_ioctl.h>
+#include <mach/powergate.h>
+#include <mach/iomap.h>
+#include "t20/t20.h"
+#include "t30.h"
+#include "gr3d/gr3d_t30.h"
+#include "gr3d/scale3d.h"
+#include "mpe/mpe.h"
+#include "host1x/host1x.h"
+#include "host1x/host1x01_hardware.h"
+#include "chip_support.h"
+#include "nvhost_channel.h"
+#include "nvhost_memmgr.h"
+#include "host1x/host1x_syncpt.h"
+
+#define NVMODMUTEX_2D_FULL	(1)
+#define NVMODMUTEX_2D_SIMPLE	(2)
+#define NVMODMUTEX_2D_SB_A	(3)
+#define NVMODMUTEX_2D_SB_B	(4)
+#define NVMODMUTEX_3D		(5)
+#define NVMODMUTEX_DISPLAYA	(6)
+#define NVMODMUTEX_DISPLAYB	(7)
+#define NVMODMUTEX_VI		(8)
+#define NVMODMUTEX_DSI		(9)
+
+static int t30_num_alloc_channels = 0;
+
+static struct resource tegra_host1x01_resources[] = {
+	{
+		.start = TEGRA_HOST1X_BASE,
+		.end = TEGRA_HOST1X_BASE + TEGRA_HOST1X_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = INT_SYNCPT_THRESH_BASE,
+		.end = INT_SYNCPT_THRESH_BASE + INT_SYNCPT_THRESH_NR - 1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = INT_HOST1X_MPCORE_GENERAL,
+		.end = INT_HOST1X_MPCORE_GENERAL,
+		.flags = IORESOURCE_IRQ,
+	},
+};
+
+static const char *s_syncpt_names[32] = {
+	"gfx_host",
+	"", "", "", "", "", "", "",
+	"disp0_a", "disp1_a", "avp_0",
+	"csi_vi_0", "csi_vi_1",
+	"vi_isp_0", "vi_isp_1", "vi_isp_2", "vi_isp_3", "vi_isp_4",
+	"2d_0", "2d_1",
+	"disp0_b", "disp1_b",
+	"3d",
+	"mpe",
+	"disp0_c", "disp1_c",
+	"vblank0", "vblank1",
+	"mpe_ebm_eof", "mpe_wr_safe",
+	"2d_tinyblt",
+	"dsi"
+};
+
+static struct host1x_device_info host1x01_info = {
+	.nb_channels	= 8,
+	.nb_pts		= 32,
+	.nb_mlocks	= 16,
+	.nb_bases	= 8,
+	.syncpt_names	= s_syncpt_names,
+	.client_managed	= NVSYNCPTS_CLIENT_MANAGED,
+};
+
+static struct nvhost_device tegra_host1x01_device = {
+	.dev		= {.platform_data = &host1x01_info},
+	.name		= "host1x",
+	.id		= -1,
+	.resource	= tegra_host1x01_resources,
+	.num_resources	= ARRAY_SIZE(tegra_host1x01_resources),
+	.clocks		= {{"host1x", UINT_MAX}, {} },
+	NVHOST_MODULE_NO_POWERGATE_IDS,
+};
+
+static struct nvhost_device tegra_display01_device = {
+	.name		= "display",
+	.id		= -1,
+	.index		= 0,
+	.syncpts	= BIT(NVSYNCPT_DISP0_A) | BIT(NVSYNCPT_DISP1_A) |
+			  BIT(NVSYNCPT_DISP0_B) | BIT(NVSYNCPT_DISP1_B) |
+			  BIT(NVSYNCPT_DISP0_C) | BIT(NVSYNCPT_DISP1_C) |
+			  BIT(NVSYNCPT_VBLANK0) | BIT(NVSYNCPT_VBLANK1),
+	.modulemutexes	= BIT(NVMODMUTEX_DISPLAYA) | BIT(NVMODMUTEX_DISPLAYB),
+	NVHOST_MODULE_NO_POWERGATE_IDS,
+	NVHOST_DEFAULT_CLOCKGATE_DELAY,
+	.moduleid	= NVHOST_MODULE_NONE,
+};
+
+static struct nvhost_device tegra_gr3d02_device = {
+	.name		= "gr3d",
+	.version	= 2,
+	.id		= -1,
+	.index		= 1,
+	.syncpts	= BIT(NVSYNCPT_3D),
+	.waitbases	= BIT(NVWAITBASE_3D),
+	.modulemutexes	= BIT(NVMODMUTEX_3D),
+	.class		= NV_GRAPHICS_3D_CLASS_ID,
+	.clocks		= { {"gr3d", UINT_MAX},
+			    {"gr3d2", UINT_MAX},
+			    {"emc", UINT_MAX} },
+	.powergate_ids = { TEGRA_POWERGATE_3D,
+			   TEGRA_POWERGATE_3D1 },
+	NVHOST_DEFAULT_CLOCKGATE_DELAY,
+	.can_powergate = true,
+	.powerup_reset = true,
+	.powergate_delay = 250,
+	.moduleid	= NVHOST_MODULE_NONE,
+};
+
+static struct nvhost_device tegra_gr2d02_device = {
+	.name		= "gr2d",
+	.id		= -1,
+	.index		= 2,
+	.syncpts	= BIT(NVSYNCPT_2D_0) | BIT(NVSYNCPT_2D_1),
+	.waitbases	= BIT(NVWAITBASE_2D_0) | BIT(NVWAITBASE_2D_1),
+	.modulemutexes	= BIT(NVMODMUTEX_2D_FULL) | BIT(NVMODMUTEX_2D_SIMPLE) |
+			  BIT(NVMODMUTEX_2D_SB_A) | BIT(NVMODMUTEX_2D_SB_B),
+	.clocks		= { {"gr2d", 0},
+			  {"epp", 0},
+			  {"emc", 300000000} },
+	NVHOST_MODULE_NO_POWERGATE_IDS,
+	.clockgate_delay = 0,
+	.moduleid	= NVHOST_MODULE_NONE,
+	.serialize	= true,
+};
+
+static struct resource isp_resources_t20[] = {
+	{
+		.name = "regs",
+		.start = TEGRA_ISP_BASE,
+		.end = TEGRA_ISP_BASE + TEGRA_ISP_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	}
+};
+
+static struct nvhost_device tegra_isp01_device = {
+	.name		= "isp",
+	.id		= -1,
+	.resource = isp_resources_t20,
+	.num_resources = ARRAY_SIZE(isp_resources_t20),
+	.index		= 3,
+	.syncpts	= BIT(NVSYNCPT_VI_ISP_2) | BIT(NVSYNCPT_VI_ISP_3) |
+			  BIT(NVSYNCPT_VI_ISP_4),
+	.clocks		= { {"epp", 0}
+			  },
+	.keepalive	= true,
+	NVHOST_MODULE_NO_POWERGATE_IDS,
+	NVHOST_DEFAULT_CLOCKGATE_DELAY,
+	.moduleid	= NVHOST_MODULE_ISP,
+};
+
+static struct resource vi_resources[] = {
+	{
+		.name = "regs",
+		.start = TEGRA_VI_BASE,
+		.end = TEGRA_VI_BASE + TEGRA_VI_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static struct nvhost_device tegra_vi01_device = {
+	.name		= "vi",
+	.resource = vi_resources,
+	.num_resources = ARRAY_SIZE(vi_resources),
+	.id		= -1,
+	.index		= 4,
+	.syncpts	= BIT(NVSYNCPT_CSI_VI_0) | BIT(NVSYNCPT_CSI_VI_1) |
+			  BIT(NVSYNCPT_VI_ISP_0) | BIT(NVSYNCPT_VI_ISP_1) |
+			  BIT(NVSYNCPT_VI_ISP_2) | BIT(NVSYNCPT_VI_ISP_3) |
+			  BIT(NVSYNCPT_VI_ISP_4),
+	.modulemutexes	= BIT(NVMODMUTEX_VI),
+	.exclusive	= true,
+	NVHOST_MODULE_NO_POWERGATE_IDS,
+	NVHOST_DEFAULT_CLOCKGATE_DELAY,
+	.moduleid	= NVHOST_MODULE_VI,
+};
+
+static struct resource tegra_mpe01_resources[] = {
+	{
+		.name = "regs",
+		.start = TEGRA_MPE_BASE,
+		.end = TEGRA_MPE_BASE + TEGRA_MPE_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+static struct nvhost_device tegra_mpe02_device = {
+	.name		= "mpe",
+	.version	= 2,
+	.id		= -1,
+	.resource	= tegra_mpe01_resources,
+	.num_resources	= ARRAY_SIZE(tegra_mpe01_resources),
+	.index		= 5,
+	.syncpts	= BIT(NVSYNCPT_MPE) | BIT(NVSYNCPT_MPE_EBM_EOF) |
+			  BIT(NVSYNCPT_MPE_WR_SAFE),
+	.waitbases	= BIT(NVWAITBASE_MPE),
+	.class		= NV_VIDEO_ENCODE_MPEG_CLASS_ID,
+	.waitbasesync	= true,
+	.keepalive	= true,
+	.clocks		= { {"mpe", UINT_MAX},
+			    {"emc", UINT_MAX} },
+	.powergate_ids	= {TEGRA_POWERGATE_MPE, -1},
+	NVHOST_DEFAULT_CLOCKGATE_DELAY,
+	.can_powergate	= true,
+	.powergate_delay = 100,
+	.moduleid	= NVHOST_MODULE_MPE,
+};
+
+static struct nvhost_device tegra_dsi01_device = {
+	.name		= "dsi",
+	.id		= -1,
+	.index		= 6,
+	.syncpts	= BIT(NVSYNCPT_DSI),
+	.modulemutexes	= BIT(NVMODMUTEX_DSI),
+	NVHOST_MODULE_NO_POWERGATE_IDS,
+	NVHOST_DEFAULT_CLOCKGATE_DELAY,
+	.moduleid	= NVHOST_MODULE_NONE,
+};
+
+static struct nvhost_device *t30_devices[] = {
+	&tegra_host1x01_device,
+	&tegra_display01_device,
+	&tegra_gr3d02_device,
+	&tegra_gr2d02_device,
+	&tegra_isp01_device,
+	&tegra_vi01_device,
+	&tegra_mpe02_device,
+	&tegra_dsi01_device,
+};
+
+int tegra3_register_host1x_devices(void)
+{
+	return nvhost_add_devices(t30_devices, ARRAY_SIZE(t30_devices));
+}
+
+static void t30_free_nvhost_channel(struct nvhost_channel *ch)
+{
+	nvhost_free_channel_internal(ch, &t30_num_alloc_channels);
+}
+
+static struct nvhost_channel *t30_alloc_nvhost_channel(
+		struct nvhost_device *dev)
+{
+	return nvhost_alloc_channel_internal(dev->index,
+		nvhost_get_host(dev)->info.nb_channels,
+		&t30_num_alloc_channels);
+}
+
+#include "host1x/host1x_channel.c"
+#include "host1x/host1x_cdma.c"
+#include "host1x/host1x_debug.c"
+#include "host1x/host1x_syncpt.c"
+#include "host1x/host1x_intr.c"
+
+int nvhost_init_t30_support(struct nvhost_master *host,
+	struct nvhost_chip_support *op)
+{
+	int err;
+
+	op->channel = host1x_channel_ops;
+	op->cdma = host1x_cdma_ops;
+	op->push_buffer = host1x_pushbuffer_ops;
+	op->debug = host1x_debug_ops;
+	op->debug.debug_init = nvhost_scale3d_debug_init;
+	host->sync_aperture = host->aperture + HOST1X_CHANNEL_SYNC_REG_BASE;
+	op->syncpt = host1x_syncpt_ops;
+	op->intr = host1x_intr_ops;
+	err = nvhost_memmgr_init(op);
+	if (err)
+		return err;
+
+	op->nvhost_dev.alloc_nvhost_channel = t30_alloc_nvhost_channel;
+	op->nvhost_dev.free_nvhost_channel = t30_free_nvhost_channel;
+
+	return 0;
+}
+
+/* Hacky way to get access to struct nvhost_device tegra_vi01_device. */
+struct nvhost_device *t30_get_tegra_vi01_device(void)
+{
+	return &tegra_vi01_device;
+}
diff --git a/drivers/video/tegra/host/t30/t30.h b/drivers/video/tegra/host/t30/t30.h
new file mode 100644
index 000000000000..80838a5e287c
--- /dev/null
+++ b/drivers/video/tegra/host/t30/t30.h
@@ -0,0 +1,29 @@
+/*
+ * drivers/video/tegra/host/t30/t30.h
+ *
+ * Tegra Graphics Chip support for Tegra3
+ *
+ * Copyright (c) 2011-2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef _NVHOST_T30_H_
+#define _NVHOST_T30_H_
+
+struct nvhost_master;
+struct nvhost_chip_support;
+
+int nvhost_init_t30_support(struct nvhost_master *host,
+	struct nvhost_chip_support *);
+
+#endif /* _NVHOST_T30_H_ */
diff --git a/drivers/video/tegra/host/vi/Makefile b/drivers/video/tegra/host/vi/Makefile
new file mode 100644
index 000000000000..8c130e49814d
--- /dev/null
+++ b/drivers/video/tegra/host/vi/Makefile
@@ -0,0 +1,7 @@
+GCOV_PROFILE := y
+EXTRA_CFLAGS += -Idrivers/video/tegra/host
+
+nvhost-vi-objs  = \
+		vi.o
+
+obj-$(CONFIG_TEGRA_GRHOST) += nvhost-vi.o
diff --git a/drivers/video/tegra/host/vi/vi.c b/drivers/video/tegra/host/vi/vi.c
new file mode 100644
index 000000000000..ee801c91efa5
--- /dev/null
+++ b/drivers/video/tegra/host/vi/vi.c
@@ -0,0 +1,79 @@
+/*
+ * drivers/video/tegra/host/vi/vi.c
+ *
+ * Tegra Graphics Host VI
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * 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 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "dev.h"
+#include "bus_client.h"
+
+static int __devinit vi_probe(struct nvhost_device *dev,
+	struct nvhost_device_id *id_table)
+{
+	int err = 0;
+
+	err = nvhost_client_device_get_resources(dev);
+	if (err)
+		return err;
+
+	return nvhost_client_device_init(dev);
+}
+
+static int __exit vi_remove(struct nvhost_device *dev)
+{
+	/* Add clean-up */
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int vi_suspend(struct nvhost_device *dev, pm_message_t state)
+{
+	return nvhost_client_device_suspend(dev);
+}
+
+static int vi_resume(struct nvhost_device *dev)
+{
+	dev_info(&dev->dev, "resuming\n");
+	return 0;
+}
+#endif
+
+static struct nvhost_driver vi_driver = {
+	.probe = vi_probe,
+	.remove = __exit_p(vi_remove),
+#ifdef CONFIG_PM
+	.suspend = vi_suspend,
+	.resume = vi_resume,
+#endif
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "vi",
+	}
+};
+
+static int __init vi_init(void)
+{
+	return nvhost_driver_register(&vi_driver);
+}
+
+static void __exit vi_exit(void)
+{
+	nvhost_driver_unregister(&vi_driver);
+}
+
+module_init(vi_init);
+module_exit(vi_exit);
diff --git a/drivers/video/tegra/nvmap/Makefile b/drivers/video/tegra/nvmap/Makefile
new file mode 100644
index 000000000000..3da03af2b1eb
--- /dev/null
+++ b/drivers/video/tegra/nvmap/Makefile
@@ -0,0 +1,8 @@
+GCOV_PROFILE := y
+obj-y += nvmap.o
+obj-y += nvmap_dev.o
+obj-y += nvmap_handle.o
+obj-y += nvmap_heap.o
+obj-y += nvmap_ioctl.o
+obj-${CONFIG_IOMMU_API}	+= nvmap_iommu.o
+obj-${CONFIG_NVMAP_RECLAIM_UNPINNED_VM} += nvmap_mru.o
\ No newline at end of file
diff --git a/drivers/video/tegra/nvmap/nvmap.c b/drivers/video/tegra/nvmap/nvmap.c
new file mode 100644
index 000000000000..3329153b2c1b
--- /dev/null
+++ b/drivers/video/tegra/nvmap/nvmap.c
@@ -0,0 +1,636 @@
+/*
+ * drivers/video/tegra/nvmap/nvmap.c
+ *
+ * Memory manager for Tegra GPU
+ *
+ * Copyright (c) 2009-2014, NVIDIA 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/err.h>
+#include <linux/highmem.h>
+#include <linux/io.h>
+#include <linux/rbtree.h>
+#include <linux/vmalloc.h>
+#include <linux/wait.h>
+#include <linux/slab.h>
+
+#include <asm/pgtable.h>
+#include <asm/tlbflush.h>
+
+#include <mach/iovmm.h>
+#include <linux/nvmap.h>
+
+#include "nvmap.h"
+#include "nvmap_mru.h"
+
+/* private nvmap_handle flag for pinning duplicate detection */
+#define NVMAP_HANDLE_VISITED (0x1ul << 31)
+
+/* map the backing pages for a heap_pgalloc handle into its IOVMM area */
+static void map_iovmm_area(struct nvmap_handle *h)
+{
+	tegra_iovmm_addr_t va;
+	unsigned long i;
+
+	BUG_ON(!h->heap_pgalloc || !h->pgalloc.area);
+	BUG_ON(h->size & ~PAGE_MASK);
+	WARN_ON(!h->pgalloc.dirty);
+
+	for (va = h->pgalloc.area->iovm_start, i = 0;
+	     va < (h->pgalloc.area->iovm_start + h->size);
+	     i++, va += PAGE_SIZE) {
+		BUG_ON(!pfn_valid(page_to_pfn(h->pgalloc.pages[i])));
+		tegra_iovmm_vm_insert_pfn(h->pgalloc.area, va,
+					  page_to_pfn(h->pgalloc.pages[i]));
+	}
+	h->pgalloc.dirty = false;
+}
+
+/* must be called inside nvmap_pin_lock, to ensure that an entire stream
+ * of pins will complete without racing with a second stream. handle should
+ * have nvmap_handle_get (or nvmap_validate_get) called before calling
+ * this function. */
+static int pin_locked(struct nvmap_client *client, struct nvmap_handle *h)
+{
+	struct tegra_iovmm_area *area;
+	BUG_ON(!h->alloc);
+
+	nvmap_mru_lock(client->share);
+	if (atomic_inc_return(&h->pin) == 1) {
+		if (h->heap_pgalloc && !h->pgalloc.contig) {
+			area = nvmap_handle_iovmm_locked(client, h);
+			if (!area) {
+				/* no race here, inside the pin mutex */
+				atomic_dec(&h->pin);
+				nvmap_mru_unlock(client->share);
+				return -ENOMEM;
+			}
+			if (area != h->pgalloc.area)
+				h->pgalloc.dirty = true;
+			h->pgalloc.area = area;
+		}
+	}
+	nvmap_mru_unlock(client->share);
+	return 0;
+}
+
+/* doesn't need to be called inside nvmap_pin_lock, since this will only
+ * expand the available VM area */
+static int handle_unpin(struct nvmap_client *client,
+		struct nvmap_handle *h, int free_vm)
+{
+	int ret = 0;
+	nvmap_mru_lock(client->share);
+
+	if (atomic_read(&h->pin) == 0) {
+		nvmap_err(client, "%s unpinning unpinned handle %p\n",
+			  current->group_leader->comm, h);
+		nvmap_mru_unlock(client->share);
+		return 0;
+	}
+
+	BUG_ON(!h->alloc);
+
+	if (!atomic_dec_return(&h->pin)) {
+		if (h->heap_pgalloc && h->pgalloc.area) {
+			/* if a secure handle is clean (i.e., mapped into
+			 * IOVMM, it needs to be zapped on unpin. */
+			if (h->secure && !h->pgalloc.dirty) {
+				tegra_iovmm_zap_vm(h->pgalloc.area);
+				h->pgalloc.dirty = true;
+			}
+			if (free_vm) {
+				tegra_iovmm_free_vm(h->pgalloc.area);
+				h->pgalloc.area = NULL;
+			} else
+				nvmap_mru_insert_locked(client->share, h);
+			ret = 1;
+		}
+	}
+
+	nvmap_mru_unlock(client->share);
+	nvmap_handle_put(h);
+	return ret;
+}
+
+static int pin_array_locked(struct nvmap_client *client,
+		struct nvmap_handle **h, int count)
+{
+	int pinned;
+	int i;
+	int err = 0;
+
+	for (pinned = 0; pinned < count; pinned++) {
+		err = pin_locked(client, h[pinned]);
+		if (err)
+			break;
+	}
+
+	if (err) {
+		/* unpin pinned handles */
+		for (i = 0; i < pinned; i++) {
+			/* inc ref counter, because
+			 * handle_unpin decrements it */
+			nvmap_handle_get(h[i]);
+			/* unpin handles and free vm */
+			handle_unpin(client, h[i], true);
+		}
+	}
+
+	if (err && tegra_iovmm_get_max_free(client->share->iovmm) >=
+							client->iovm_limit) {
+		/* First attempt to pin in empty iovmm
+		 * may still fail because of fragmentation caused by
+		 * placing handles in MRU areas. After such failure
+		 * all MRU gets cleaned and iovm space is freed.
+		 *
+		 * We have to do pinning again here since there might be is
+		 * no more incoming pin_wait wakeup calls from unpin
+		 * operations */
+		for (pinned = 0; pinned < count; pinned++) {
+			err = pin_locked(client, h[pinned]);
+			if (err)
+				break;
+		}
+		if (err) {
+			pr_err("Pinning in empty iovmm failed!!!\n");
+			BUG_ON(1);
+		}
+	}
+	return err;
+}
+
+static int wait_pin_array_locked(struct nvmap_client *client,
+		struct nvmap_handle **h, int count)
+{
+	int ret = 0;
+
+	ret = pin_array_locked(client, h, count);
+
+	if (ret) {
+		ret = wait_event_interruptible(client->share->pin_wait,
+				!pin_array_locked(client, h, count));
+	}
+	return ret ? -EINTR : 0;
+}
+
+static int handle_unpin_noref(struct nvmap_client *client, unsigned long id)
+{
+	struct nvmap_handle *h;
+	int w;
+
+	h = nvmap_validate_get(client, id);
+	if (unlikely(!h)) {
+		nvmap_err(client, "%s attempting to unpin invalid handle %p\n",
+			  current->group_leader->comm, (void *)id);
+		return 0;
+	}
+
+	nvmap_err(client, "%s unpinning unreferenced handle %p\n",
+		  current->group_leader->comm, h);
+	WARN_ON(1);
+
+	w = handle_unpin(client, h, false);
+	nvmap_handle_put(h);
+	return w;
+}
+
+void nvmap_unpin_ids(struct nvmap_client *client,
+		     unsigned int nr, const unsigned long *ids)
+{
+	unsigned int i;
+	int do_wake = 0;
+
+	for (i = 0; i < nr; i++) {
+		struct nvmap_handle_ref *ref;
+
+		if (!ids[i])
+			continue;
+
+		nvmap_ref_lock(client);
+		ref = _nvmap_validate_id_locked(client, ids[i]);
+		if (ref) {
+			struct nvmap_handle *h = ref->handle;
+			int e = atomic_add_unless(&ref->pin, -1, 0);
+
+			nvmap_ref_unlock(client);
+
+			if (!e) {
+				nvmap_err(client, "%s unpinning unpinned "
+					  "handle %08lx\n",
+					  current->group_leader->comm, ids[i]);
+			} else {
+				do_wake |= handle_unpin(client, h, false);
+			}
+		} else {
+			nvmap_ref_unlock(client);
+			if (client->super)
+				do_wake |= handle_unpin_noref(client, ids[i]);
+			else
+				nvmap_err(client, "%s unpinning invalid "
+					  "handle %08lx\n",
+					  current->group_leader->comm, ids[i]);
+		}
+	}
+
+	if (do_wake)
+		wake_up(&client->share->pin_wait);
+}
+
+/* pins a list of handle_ref objects; same conditions apply as to
+ * _nvmap_handle_pin, but also bumps the pin count of each handle_ref. */
+int nvmap_pin_ids(struct nvmap_client *client,
+		  unsigned int nr, const unsigned long *ids)
+{
+	int ret = 0;
+	unsigned int i;
+	struct nvmap_handle **h = (struct nvmap_handle **)ids;
+	struct nvmap_handle_ref *ref;
+
+	/* to optimize for the common case (client provided valid handle
+	 * references and the pin succeeds), increment the handle_ref pin
+	 * count during validation. in error cases, the tree will need to
+	 * be re-walked, since the handle_ref is discarded so that an
+	 * allocation isn't required. if a handle_ref is not found,
+	 * locally validate that the caller has permission to pin the handle;
+	 * handle_refs are not created in this case, so it is possible that
+	 * if the caller crashes after pinning a global handle, the handle
+	 * will be permanently leaked. */
+	nvmap_ref_lock(client);
+	for (i = 0; i < nr; i++) {
+		ref = _nvmap_validate_id_locked(client, ids[i]);
+		if (ref) {
+			atomic_inc(&ref->pin);
+			nvmap_handle_get(h[i]);
+		} else {
+			struct nvmap_handle *verify;
+			nvmap_ref_unlock(client);
+			verify = nvmap_validate_get(client, ids[i]);
+			if (verify) {
+				nvmap_warn(client, "%s pinning unreferenced "
+					   "handle %p\n",
+					   current->group_leader->comm, h[i]);
+			} else {
+				h[i] = NULL;
+				ret = -EPERM;
+			}
+			nvmap_ref_lock(client);
+		}
+	}
+	nvmap_ref_unlock(client);
+
+	if (ret)
+		goto out;
+
+	ret = mutex_lock_interruptible(&client->share->pin_lock);
+	if (WARN_ON(ret))
+		goto out;
+
+	ret = wait_pin_array_locked(client, h, nr);
+
+	mutex_unlock(&client->share->pin_lock);
+
+	if (ret) {
+		ret = -EINTR;
+	} else {
+		for (i = 0; i < nr; i++) {
+			if (h[i]->heap_pgalloc && h[i]->pgalloc.dirty)
+				map_iovmm_area(h[i]);
+		}
+	}
+
+out:
+	if (ret) {
+		nvmap_ref_lock(client);
+		for (i = 0; i < nr; i++) {
+			if(!ids[i])
+				continue;
+
+			ref = _nvmap_validate_id_locked(client, ids[i]);
+			if (!ref) {
+				nvmap_warn(client, "%s freed handle %p "
+					   "during pinning\n",
+					   current->group_leader->comm,
+					   (void *)ids[i]);
+				continue;
+			}
+			atomic_dec(&ref->pin);
+		}
+		nvmap_ref_unlock(client);
+
+		for (i = 0; i < nr; i++)
+			if(h[i])
+				nvmap_handle_put(h[i]);
+	}
+
+	return ret;
+}
+
+static phys_addr_t handle_phys(struct nvmap_handle *h)
+{
+	phys_addr_t addr;
+
+	if (h->heap_pgalloc && h->pgalloc.contig) {
+		addr = page_to_phys(h->pgalloc.pages[0]);
+	} else if (h->heap_pgalloc) {
+		BUG_ON(!h->pgalloc.area);
+		addr = h->pgalloc.area->iovm_start;
+	} else {
+		addr = h->carveout->base;
+	}
+
+	return addr;
+}
+
+phys_addr_t nvmap_pin(struct nvmap_client *client,
+			struct nvmap_handle_ref *ref)
+{
+	struct nvmap_handle *h;
+	phys_addr_t phys;
+	int ret = 0;
+
+	h = nvmap_handle_get(ref->handle);
+	if (WARN_ON(!h))
+		return -EINVAL;
+	atomic_inc(&ref->pin);
+
+	if (WARN_ON(mutex_lock_interruptible(&client->share->pin_lock))) {
+		ret = -EINTR;
+	} else {
+		ret = wait_pin_array_locked(client, &h, 1);
+		mutex_unlock(&client->share->pin_lock);
+	}
+
+	if (ret) {
+		atomic_dec(&ref->pin);
+		nvmap_handle_put(h);
+	} else {
+		if (h->heap_pgalloc && h->pgalloc.dirty)
+			map_iovmm_area(h);
+		phys = handle_phys(h);
+	}
+	return ret ?: phys;
+}
+EXPORT_SYMBOL(nvmap_pin);
+
+phys_addr_t nvmap_handle_address(struct nvmap_client *c, unsigned long id)
+{
+	struct nvmap_handle *h;
+	phys_addr_t phys;
+
+	h = nvmap_get_handle_id(c, id);
+	if (!h)
+		return -EPERM;
+	mutex_lock(&h->lock);
+	phys = handle_phys(h);
+	mutex_unlock(&h->lock);
+	nvmap_handle_put(h);
+
+	return phys;
+}
+
+void nvmap_unpin(struct nvmap_client *client, struct nvmap_handle_ref *ref)
+{
+	if (!ref)
+		return;
+
+	atomic_dec(&ref->pin);
+	if (handle_unpin(client, ref->handle, false))
+		wake_up(&client->share->pin_wait);
+}
+EXPORT_SYMBOL(nvmap_unpin);
+
+void nvmap_unpin_handles(struct nvmap_client *client,
+			 struct nvmap_handle **h, int nr)
+{
+	int i;
+	int do_wake = 0;
+
+	for (i = 0; i < nr; i++) {
+		if (WARN_ON(!h[i]))
+			continue;
+		do_wake |= handle_unpin(client, h[i], false);
+	}
+
+	if (do_wake)
+		wake_up(&client->share->pin_wait);
+}
+
+void *nvmap_mmap(struct nvmap_handle_ref *ref)
+{
+	struct nvmap_handle *h;
+	pgprot_t prot;
+	unsigned long adj_size;
+	unsigned long offs;
+	struct vm_struct *v;
+	void *p;
+
+	h = nvmap_handle_get(ref->handle);
+	if (!h)
+		return NULL;
+
+	prot = nvmap_pgprot(h, pgprot_kernel);
+
+	if (h->heap_pgalloc)
+		return vm_map_ram(h->pgalloc.pages, h->size >> PAGE_SHIFT,
+				  -1, prot);
+
+	/* carveout - explicitly map the pfns into a vmalloc area */
+
+	nvmap_usecount_inc(h);
+
+	adj_size = h->carveout->base & ~PAGE_MASK;
+	adj_size += h->size;
+	adj_size = PAGE_ALIGN(adj_size);
+
+	v = alloc_vm_area(adj_size);
+	if (!v) {
+		nvmap_usecount_dec(h);
+		nvmap_handle_put(h);
+		return NULL;
+	}
+
+	p = v->addr + (h->carveout->base & ~PAGE_MASK);
+
+	for (offs = 0; offs < adj_size; offs += PAGE_SIZE) {
+		unsigned long addr = (unsigned long) v->addr + offs;
+		unsigned int pfn;
+		pgd_t *pgd;
+		pud_t *pud;
+		pmd_t *pmd;
+		pte_t *pte;
+
+		pfn = __phys_to_pfn(h->carveout->base + offs);
+		pgd = pgd_offset_k(addr);
+		pud = pud_alloc(&init_mm, pgd, addr);
+		if (!pud)
+			break;
+		pmd = pmd_alloc(&init_mm, pud, addr);
+		if (!pmd)
+			break;
+		pte = pte_alloc_kernel(pmd, addr);
+		if (!pte)
+			break;
+		set_pte_at(&init_mm, addr, pte, pfn_pte(pfn, prot));
+		flush_tlb_kernel_page(addr);
+	}
+
+	if (offs != adj_size) {
+		free_vm_area(v);
+		nvmap_usecount_dec(h);
+		nvmap_handle_put(h);
+		return NULL;
+	}
+
+	/* leave the handle ref count incremented by 1, so that
+	 * the handle will not be freed while the kernel mapping exists.
+	 * nvmap_handle_put will be called by unmapping this address */
+	return p;
+}
+EXPORT_SYMBOL(nvmap_mmap);
+
+void nvmap_munmap(struct nvmap_handle_ref *ref, void *addr)
+{
+	struct nvmap_handle *h;
+
+	if (!ref)
+		return;
+
+	h = ref->handle;
+
+	if (h->heap_pgalloc) {
+		vm_unmap_ram(addr, h->size >> PAGE_SHIFT);
+	} else {
+		struct vm_struct *vm;
+		addr -= (h->carveout->base & ~PAGE_MASK);
+		vm = remove_vm_area(addr);
+		BUG_ON(!vm);
+		kfree(vm);
+		nvmap_usecount_dec(h);
+	}
+	nvmap_handle_put(h);
+}
+EXPORT_SYMBOL(nvmap_munmap);
+
+struct nvmap_handle_ref *nvmap_alloc(struct nvmap_client *client, size_t size,
+				     size_t align, unsigned int flags,
+				     unsigned int heap_mask)
+{
+	const unsigned int default_heap = (NVMAP_HEAP_SYSMEM |
+					   NVMAP_HEAP_CARVEOUT_GENERIC);
+	struct nvmap_handle_ref *r = NULL;
+	int err;
+
+	if (heap_mask == 0)
+		heap_mask = default_heap;
+
+	r = nvmap_create_handle(client, size);
+	if (IS_ERR(r))
+		return r;
+
+	err = nvmap_alloc_handle_id(client, nvmap_ref_to_id(r),
+				    heap_mask, align, flags);
+
+	if (err) {
+		nvmap_free_handle_id(client, nvmap_ref_to_id(r));
+		return ERR_PTR(err);
+	}
+
+	return r;
+}
+EXPORT_SYMBOL(nvmap_alloc);
+
+/* allocates memory with specifed iovm_start address. */
+struct nvmap_handle_ref *nvmap_alloc_iovm(struct nvmap_client *client,
+	size_t size, size_t align, unsigned int flags, unsigned int iovm_start)
+{
+	int err;
+	struct nvmap_handle *h;
+	struct nvmap_handle_ref *r;
+	const unsigned int default_heap = NVMAP_HEAP_IOVMM;
+
+	/* size need to be more than one page.
+	 * otherwise heap preference would change to system heap.
+	 */
+	if (size <= PAGE_SIZE)
+		size = PAGE_SIZE << 1;
+	r = nvmap_create_handle(client, size);
+	if (IS_ERR_OR_NULL(r))
+		return r;
+
+	h = r->handle;
+	h->pgalloc.iovm_addr = iovm_start;
+	err = nvmap_alloc_handle_id(client, nvmap_ref_to_id(r),
+			default_heap, align, flags);
+	if (err)
+		goto fail;
+
+	err = mutex_lock_interruptible(&client->share->pin_lock);
+	if (WARN_ON(err))
+		goto fail;
+	err = pin_locked(client, h);
+	mutex_unlock(&client->share->pin_lock);
+	if (err)
+		goto fail;
+	return r;
+
+fail:
+	nvmap_free_handle_id(client, nvmap_ref_to_id(r));
+	return ERR_PTR(err);
+}
+
+void nvmap_free_iovm(struct nvmap_client *client, struct nvmap_handle_ref *r)
+{
+	unsigned long ref_id = nvmap_ref_to_id(r);
+
+	nvmap_unpin_ids(client, 1, &ref_id);
+	nvmap_free_handle_id(client, ref_id);
+}
+
+void nvmap_free(struct nvmap_client *client, struct nvmap_handle_ref *r)
+{
+	if (!r)
+		return;
+
+	nvmap_free_handle_id(client, nvmap_ref_to_id(r));
+}
+EXPORT_SYMBOL(nvmap_free);
+
+int nvmap_mark_global(struct nvmap_client *client, struct nvmap_handle_ref *r)
+{
+	struct nvmap_handle *h;
+
+	h = nvmap_get_handle_id(client, (unsigned long)r->handle);
+	if (!h)
+		return -EINVAL;
+	r->handle->global = true;
+	nvmap_handle_put(h);
+
+	return 0;
+}
+
+unsigned long nvmap_validate_ref(struct nvmap_client *client,
+				struct nvmap_handle_ref *r)
+{
+	struct nvmap_handle_ref *ref;
+	unsigned long id = nvmap_ref_to_id(r);
+
+	nvmap_ref_lock(client);
+	ref = _nvmap_validate_id_locked(client, id);
+	nvmap_ref_unlock(client);
+	return (unsigned long)ref;
+}
diff --git a/drivers/video/tegra/nvmap/nvmap.h b/drivers/video/tegra/nvmap/nvmap.h
new file mode 100644
index 000000000000..25403f5e7098
--- /dev/null
+++ b/drivers/video/tegra/nvmap/nvmap.h
@@ -0,0 +1,271 @@
+/*
+ * drivers/video/tegra/nvmap/nvmap.h
+ *
+ * GPU memory management driver for Tegra
+ *
+ * Copyright (c) 2010-2012, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __VIDEO_TEGRA_NVMAP_NVMAP_H
+#define __VIDEO_TEGRA_NVMAP_NVMAP_H
+
+#include <linux/list.h>
+#include <linux/mm.h>
+#include <linux/mutex.h>
+#include <linux/rbtree.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/atomic.h>
+#include <linux/nvmap.h>
+#include "nvmap_heap.h"
+
+struct nvmap_device;
+struct page;
+struct tegra_iovmm_area;
+
+void _nvmap_handle_free(struct nvmap_handle *h);
+
+#if defined(CONFIG_TEGRA_NVMAP)
+#define nvmap_err(_client, _fmt, ...)				\
+	dev_err(nvmap_client_to_device(_client),		\
+		"%s: "_fmt, __func__, ##__VA_ARGS__)
+
+#define nvmap_warn(_client, _fmt, ...)				\
+	dev_warn(nvmap_client_to_device(_client),		\
+		 "%s: "_fmt, __func__, ##__VA_ARGS__)
+
+#define nvmap_debug(_client, _fmt, ...)				\
+	dev_dbg(nvmap_client_to_device(_client),		\
+		"%s: "_fmt, __func__, ##__VA_ARGS__)
+
+#define nvmap_ref_to_id(_ref)		((unsigned long)(_ref)->handle)
+
+/* handles allocated using shared system memory (either IOVMM- or high-order
+ * page allocations */
+struct nvmap_pgalloc {
+	struct page **pages;
+	struct tegra_iovmm_area *area;
+	struct list_head mru_list;	/* MRU entry for IOVMM reclamation */
+	bool contig;			/* contiguous system memory */
+	bool dirty;			/* area is invalid and needs mapping */
+	u32 iovm_addr;	/* is non-zero, if client need specific iova mapping */
+};
+
+struct nvmap_handle {
+	struct rb_node node;	/* entry on global handle tree */
+	atomic_t ref;		/* reference count (i.e., # of duplications) */
+	atomic_t pin;		/* pin count */
+	unsigned int usecount;	/* how often is used */
+	unsigned long flags;
+	size_t size;		/* padded (as-allocated) size */
+	size_t orig_size;	/* original (as-requested) size */
+	size_t align;
+	struct nvmap_client *owner;
+	struct nvmap_device *dev;
+	union {
+		struct nvmap_pgalloc pgalloc;
+		struct nvmap_heap_block *carveout;
+	};
+	bool global;		/* handle may be duplicated by other clients */
+	bool secure;		/* zap IOVMM area on unpin */
+	bool heap_pgalloc;	/* handle is page allocated (sysmem / iovmm) */
+	bool alloc;		/* handle has memory allocated */
+	unsigned int userflags;	/* flags passed from userspace */
+	struct mutex lock;
+};
+
+#ifdef CONFIG_NVMAP_PAGE_POOLS
+#define NVMAP_UC_POOL NVMAP_HANDLE_UNCACHEABLE
+#define NVMAP_WC_POOL NVMAP_HANDLE_WRITE_COMBINE
+#define NVMAP_IWB_POOL NVMAP_HANDLE_INNER_CACHEABLE
+#define NVMAP_WB_POOL NVMAP_HANDLE_CACHEABLE
+#define NVMAP_NUM_POOLS (NVMAP_HANDLE_CACHEABLE + 1)
+
+struct nvmap_page_pool {
+	struct mutex lock;
+	int npages;
+	struct page **page_array;
+	struct page **shrink_array;
+	int max_pages;
+	int flags;
+};
+
+int nvmap_page_pool_init(struct nvmap_page_pool *pool, int flags);
+#endif
+
+struct nvmap_share {
+	struct tegra_iovmm_client *iovmm;
+	wait_queue_head_t pin_wait;
+	struct mutex pin_lock;
+#ifdef CONFIG_NVMAP_PAGE_POOLS
+	union {
+		struct nvmap_page_pool pools[NVMAP_NUM_POOLS];
+		struct {
+			struct nvmap_page_pool uc_pool;
+			struct nvmap_page_pool wc_pool;
+			struct nvmap_page_pool iwb_pool;
+			struct nvmap_page_pool wb_pool;
+		};
+	};
+#endif
+#ifdef CONFIG_NVMAP_RECLAIM_UNPINNED_VM
+	struct mutex mru_lock;
+	struct list_head *mru_lists;
+	int nr_mru;
+#endif
+};
+
+struct nvmap_carveout_commit {
+	size_t commit;
+	struct list_head list;
+};
+
+struct nvmap_client {
+	const char			*name;
+	struct nvmap_device		*dev;
+	struct nvmap_share		*share;
+	struct rb_root			handle_refs;
+	atomic_t			iovm_commit;
+	size_t				iovm_limit;
+	struct mutex			ref_lock;
+	bool				super;
+	atomic_t			count;
+	struct task_struct		*task;
+	struct list_head		list;
+	struct nvmap_carveout_commit	carveout_commit[0];
+};
+
+struct nvmap_vma_priv {
+	struct nvmap_handle *handle;
+	size_t		offs;
+	atomic_t	count;	/* number of processes cloning the VMA */
+};
+
+static inline void nvmap_ref_lock(struct nvmap_client *priv)
+{
+	mutex_lock(&priv->ref_lock);
+}
+
+static inline void nvmap_ref_unlock(struct nvmap_client *priv)
+{
+	mutex_unlock(&priv->ref_lock);
+}
+
+static inline struct nvmap_handle *nvmap_handle_get(struct nvmap_handle *h)
+{
+	if (unlikely(atomic_inc_return(&h->ref) <= 1)) {
+		pr_err("%s: %s getting a freed handle\n",
+			__func__, current->group_leader->comm);
+		if (atomic_read(&h->ref) <= 0)
+			return NULL;
+	}
+	return h;
+}
+
+static inline void nvmap_handle_put(struct nvmap_handle *h)
+{
+	int cnt = atomic_dec_return(&h->ref);
+
+	if (WARN_ON(cnt < 0)) {
+		pr_err("%s: %s put to negative references\n",
+			__func__, current->comm);
+	} else if (cnt == 0)
+		_nvmap_handle_free(h);
+}
+
+static inline pgprot_t nvmap_pgprot(struct nvmap_handle *h, pgprot_t prot)
+{
+	if (h->flags == NVMAP_HANDLE_UNCACHEABLE)
+		return pgprot_noncached(prot);
+	else if (h->flags == NVMAP_HANDLE_WRITE_COMBINE)
+		return pgprot_writecombine(prot);
+	else if (h->flags == NVMAP_HANDLE_INNER_CACHEABLE)
+		return pgprot_inner_writeback(prot);
+	return prot;
+}
+
+#else /* CONFIG_TEGRA_NVMAP */
+struct nvmap_handle *nvmap_handle_get(struct nvmap_handle *h);
+void nvmap_handle_put(struct nvmap_handle *h);
+pgprot_t nvmap_pgprot(struct nvmap_handle *h, pgprot_t prot);
+
+#endif /* !CONFIG_TEGRA_NVMAP */
+
+struct device *nvmap_client_to_device(struct nvmap_client *client);
+
+pte_t **nvmap_alloc_pte(struct nvmap_device *dev, void **vaddr);
+
+pte_t **nvmap_alloc_pte_irq(struct nvmap_device *dev, void **vaddr);
+
+void nvmap_free_pte(struct nvmap_device *dev, pte_t **pte);
+
+void nvmap_usecount_inc(struct nvmap_handle *h);
+void nvmap_usecount_dec(struct nvmap_handle *h);
+
+struct nvmap_heap_block *nvmap_carveout_alloc(struct nvmap_client *dev,
+					      struct nvmap_handle *handle,
+					      unsigned long type);
+
+unsigned long nvmap_carveout_usage(struct nvmap_client *c,
+				   struct nvmap_heap_block *b);
+
+struct nvmap_carveout_node;
+void nvmap_carveout_commit_add(struct nvmap_client *client,
+			       struct nvmap_carveout_node *node, size_t len);
+
+void nvmap_carveout_commit_subtract(struct nvmap_client *client,
+				    struct nvmap_carveout_node *node,
+				    size_t len);
+
+struct nvmap_share *nvmap_get_share_from_dev(struct nvmap_device *dev);
+
+struct nvmap_handle *nvmap_validate_get(struct nvmap_client *client,
+					unsigned long handle);
+
+struct nvmap_handle_ref *_nvmap_validate_id_locked(struct nvmap_client *priv,
+						   unsigned long id);
+
+struct nvmap_handle *nvmap_get_handle_id(struct nvmap_client *client,
+					 unsigned long id);
+
+struct nvmap_handle_ref *nvmap_create_handle(struct nvmap_client *client,
+					     size_t size);
+
+int nvmap_alloc_handle_id(struct nvmap_client *client,
+			  unsigned long id, unsigned int heap_mask,
+			  size_t align, unsigned int flags);
+
+void nvmap_free_handle_id(struct nvmap_client *c, unsigned long id);
+
+int nvmap_pin_ids(struct nvmap_client *client,
+		  unsigned int nr, const unsigned long *ids);
+
+void nvmap_unpin_ids(struct nvmap_client *priv,
+		     unsigned int nr, const unsigned long *ids);
+
+int nvmap_handle_remove(struct nvmap_device *dev, struct nvmap_handle *h);
+
+void nvmap_handle_add(struct nvmap_device *dev, struct nvmap_handle *h);
+
+int is_nvmap_vma(struct vm_area_struct *vma);
+
+struct nvmap_handle_ref *nvmap_alloc_iovm(struct nvmap_client *client,
+	size_t size, size_t align, unsigned int flags, unsigned int iova_start);
+
+void nvmap_free_iovm(struct nvmap_client *client, struct nvmap_handle_ref *r);
+
+#endif /* __VIDEO_TEGRA_NVMAP_NVMAP_H */
diff --git a/drivers/video/tegra/nvmap/nvmap_common.h b/drivers/video/tegra/nvmap/nvmap_common.h
new file mode 100644
index 000000000000..2a1e2b4b3c4b
--- /dev/null
+++ b/drivers/video/tegra/nvmap/nvmap_common.h
@@ -0,0 +1,39 @@
+/*
+ * drivers/video/tegra/nvmap/nvmap_common.h
+ *
+ * GPU memory management driver for Tegra
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+extern void v7_flush_kern_cache_all(void *);
+extern void v7_clean_kern_cache_all(void *);
+
+#define FLUSH_CLEAN_BY_SET_WAY_THRESHOLD_INNER (8 * PAGE_SIZE)
+#define FLUSH_CLEAN_BY_SET_WAY_THRESHOLD_OUTER (1024 * 1024)
+
+static inline void inner_flush_cache_all(void)
+{
+	on_each_cpu(v7_flush_kern_cache_all, NULL, 1);
+}
+
+static inline void inner_clean_cache_all(void)
+{
+	on_each_cpu(v7_clean_kern_cache_all, NULL, 1);
+}
+
+extern void __flush_dcache_page(struct address_space *, struct page *);
diff --git a/drivers/video/tegra/nvmap/nvmap_dev.c b/drivers/video/tegra/nvmap/nvmap_dev.c
new file mode 100644
index 000000000000..40a9e5b9f2ed
--- /dev/null
+++ b/drivers/video/tegra/nvmap/nvmap_dev.c
@@ -0,0 +1,1455 @@
+/*
+ * Copyright (c) 2011-2013, NVIDIA CORPORATION. All rights reserved.
+ *
+ * drivers/video/tegra/nvmap/nvmap_dev.c
+ *
+ * User-space interface to nvmap
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/backing-dev.h>
+#include <linux/bitmap.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/miscdevice.h>
+#include <linux/mm.h>
+#include <linux/oom.h>
+#include <linux/platform_device.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/uaccess.h>
+#include <linux/vmalloc.h>
+#include <linux/nvmap.h>
+
+#include <asm/cacheflush.h>
+#include <asm/tlbflush.h>
+
+#include <mach/iovmm.h>
+
+#define CREATE_TRACE_POINTS
+#include <trace/events/nvmap.h>
+
+#include "nvmap.h"
+#include "nvmap_ioctl.h"
+#include "nvmap_mru.h"
+#include "nvmap_common.h"
+
+#define NVMAP_NUM_PTES		64
+#define NVMAP_CARVEOUT_KILLER_RETRY_TIME 100 /* msecs */
+
+#ifdef CONFIG_NVMAP_CARVEOUT_KILLER
+static bool carveout_killer = true;
+#else
+static bool carveout_killer;
+#endif
+module_param(carveout_killer, bool, 0640);
+
+struct nvmap_carveout_node {
+	unsigned int		heap_bit;
+	struct nvmap_heap	*carveout;
+	int			index;
+	struct list_head	clients;
+	spinlock_t		clients_lock;
+};
+
+struct nvmap_device {
+	struct vm_struct *vm_rgn;
+	pte_t		*ptes[NVMAP_NUM_PTES];
+	unsigned long	ptebits[NVMAP_NUM_PTES / BITS_PER_LONG];
+	unsigned int	lastpte;
+	spinlock_t	ptelock;
+
+	struct rb_root	handles;
+	spinlock_t	handle_lock;
+	wait_queue_head_t pte_wait;
+	struct miscdevice dev_super;
+	struct miscdevice dev_user;
+	struct nvmap_carveout_node *heaps;
+	int nr_carveouts;
+	struct nvmap_share iovmm_master;
+	struct list_head clients;
+	spinlock_t	clients_lock;
+};
+
+struct nvmap_device *nvmap_dev;
+EXPORT_SYMBOL(nvmap_dev);
+
+static struct backing_dev_info nvmap_bdi = {
+	.ra_pages	= 0,
+	.capabilities	= (BDI_CAP_NO_ACCT_AND_WRITEBACK |
+			   BDI_CAP_READ_MAP | BDI_CAP_WRITE_MAP),
+};
+
+static int nvmap_open(struct inode *inode, struct file *filp);
+static int nvmap_release(struct inode *inode, struct file *filp);
+static long nvmap_ioctl(struct file *filp, unsigned int cmd, unsigned long arg);
+static int nvmap_map(struct file *filp, struct vm_area_struct *vma);
+static void nvmap_vma_open(struct vm_area_struct *vma);
+static void nvmap_vma_close(struct vm_area_struct *vma);
+static int nvmap_vma_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
+
+static const struct file_operations nvmap_user_fops = {
+	.owner		= THIS_MODULE,
+	.open		= nvmap_open,
+	.release	= nvmap_release,
+	.unlocked_ioctl	= nvmap_ioctl,
+	.mmap		= nvmap_map,
+};
+
+static const struct file_operations nvmap_super_fops = {
+	.owner		= THIS_MODULE,
+	.open		= nvmap_open,
+	.release	= nvmap_release,
+	.unlocked_ioctl	= nvmap_ioctl,
+	.mmap		= nvmap_map,
+};
+
+static struct vm_operations_struct nvmap_vma_ops = {
+	.open		= nvmap_vma_open,
+	.close		= nvmap_vma_close,
+	.fault		= nvmap_vma_fault,
+};
+
+int is_nvmap_vma(struct vm_area_struct *vma)
+{
+	return vma->vm_ops == &nvmap_vma_ops;
+}
+
+struct device *nvmap_client_to_device(struct nvmap_client *client)
+{
+	if (client->super)
+		return client->dev->dev_super.this_device;
+	else
+		return client->dev->dev_user.this_device;
+}
+
+struct nvmap_share *nvmap_get_share_from_dev(struct nvmap_device *dev)
+{
+	return &dev->iovmm_master;
+}
+
+/* allocates a PTE for the caller's use; returns the PTE pointer or
+ * a negative errno. may be called from IRQs */
+pte_t **nvmap_alloc_pte_irq(struct nvmap_device *dev, void **vaddr)
+{
+	unsigned long flags;
+	unsigned long bit;
+
+	spin_lock_irqsave(&dev->ptelock, flags);
+	bit = find_next_zero_bit(dev->ptebits, NVMAP_NUM_PTES, dev->lastpte);
+	if (bit == NVMAP_NUM_PTES) {
+		bit = find_first_zero_bit(dev->ptebits, dev->lastpte);
+		if (bit == dev->lastpte)
+			bit = NVMAP_NUM_PTES;
+	}
+
+	if (bit == NVMAP_NUM_PTES) {
+		spin_unlock_irqrestore(&dev->ptelock, flags);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	dev->lastpte = bit;
+	set_bit(bit, dev->ptebits);
+	spin_unlock_irqrestore(&dev->ptelock, flags);
+
+	*vaddr = dev->vm_rgn->addr + bit * PAGE_SIZE;
+	return &(dev->ptes[bit]);
+}
+
+/* allocates a PTE for the caller's use; returns the PTE pointer or
+ * a negative errno. must be called from sleepable contexts */
+pte_t **nvmap_alloc_pte(struct nvmap_device *dev, void **vaddr)
+{
+	int ret;
+	pte_t **pte;
+	ret = wait_event_interruptible(dev->pte_wait,
+			!IS_ERR(pte = nvmap_alloc_pte_irq(dev, vaddr)));
+
+	if (ret == -ERESTARTSYS)
+		return ERR_PTR(-EINTR);
+
+	return pte;
+}
+
+/* frees a PTE */
+void nvmap_free_pte(struct nvmap_device *dev, pte_t **pte)
+{
+	unsigned long addr;
+	unsigned int bit = pte - dev->ptes;
+	unsigned long flags;
+
+	if (WARN_ON(bit >= NVMAP_NUM_PTES))
+		return;
+
+	addr = (unsigned long)dev->vm_rgn->addr + bit * PAGE_SIZE;
+	set_pte_at(&init_mm, addr, *pte, 0);
+
+	spin_lock_irqsave(&dev->ptelock, flags);
+	clear_bit(bit, dev->ptebits);
+	spin_unlock_irqrestore(&dev->ptelock, flags);
+	wake_up(&dev->pte_wait);
+}
+
+/* verifies that the handle ref value "ref" is a valid handle ref for the
+ * file. caller must hold the file's ref_lock prior to calling this function */
+struct nvmap_handle_ref *_nvmap_validate_id_locked(struct nvmap_client *c,
+						   unsigned long id)
+{
+	struct rb_node *n = c->handle_refs.rb_node;
+
+	while (n) {
+		struct nvmap_handle_ref *ref;
+		ref = rb_entry(n, struct nvmap_handle_ref, node);
+		if ((unsigned long)ref->handle == id)
+			return ref;
+		else if (id > (unsigned long)ref->handle)
+			n = n->rb_right;
+		else
+			n = n->rb_left;
+	}
+
+	return NULL;
+}
+
+struct nvmap_handle *nvmap_get_handle_id(struct nvmap_client *client,
+					 unsigned long id)
+{
+	struct nvmap_handle_ref *ref;
+	struct nvmap_handle *h = NULL;
+
+	/* Allow the handle to be accessed by other (non-owner)
+	clients only if the owner is "videobuf2-dma-nvmap",
+	which is a V4L2 capture kernel module. This handle can
+	be accessed by the "user" client for rendering */
+	if (((struct nvmap_handle *)id)->owner &&
+			!strcmp(((struct nvmap_handle *)id)->owner->name,
+			"videobuf2-dma-nvmap"))
+		client = ((struct nvmap_handle *)id)->owner;
+
+	nvmap_ref_lock(client);
+	ref = _nvmap_validate_id_locked(client, id);
+	if (ref)
+		h = ref->handle;
+	if (h)
+		h = nvmap_handle_get(h);
+	nvmap_ref_unlock(client);
+	return h;
+}
+
+unsigned long nvmap_carveout_usage(struct nvmap_client *c,
+				   struct nvmap_heap_block *b)
+{
+	struct nvmap_heap *h = nvmap_block_to_heap(b);
+	struct nvmap_carveout_node *n;
+	int i;
+
+	for (i = 0; i < c->dev->nr_carveouts; i++) {
+		n = &c->dev->heaps[i];
+		if (n->carveout == h)
+			return n->heap_bit;
+	}
+	return 0;
+}
+
+/*
+ * This routine is used to flush the carveout memory from cache.
+ * Why cache flush is needed for carveout? Consider the case, where a piece of
+ * carveout is allocated as cached and released. After this, if the same memory is
+ * allocated for uncached request and the memory is not flushed out from cache.
+ * In this case, the client might pass this to H/W engine and it could start modify
+ * the memory. As this was cached earlier, it might have some portion of it in cache.
+ * During cpu request to read/write other memory, the cached portion of this memory
+ * might get flushed back to main memory and would cause corruptions, if it happens
+ * after H/W writes data to memory.
+ *
+ * But flushing out the memory blindly on each carveout allocation is redundant.
+ *
+ * In order to optimize the carveout buffer cache flushes, the following
+ * strategy is used.
+ *
+ * The whole Carveout is flushed out from cache during its initialization.
+ * During allocation, carveout buffers are not flused from cache.
+ * During deallocation, carveout buffers are flushed, if they were allocated as cached.
+ * if they were allocated as uncached/writecombined, no cache flush is needed.
+ * Just draining store buffers is enough.
+ */
+int nvmap_flush_heap_block(struct nvmap_client *client,
+	struct nvmap_heap_block *block, size_t len, unsigned int prot)
+{
+	pte_t **pte;
+	void *addr;
+	phys_addr_t kaddr;
+	phys_addr_t phys = block->base;
+	phys_addr_t end = block->base + len;
+
+	if (prot == NVMAP_HANDLE_UNCACHEABLE || prot == NVMAP_HANDLE_WRITE_COMBINE)
+		goto out;
+
+	if (len >= FLUSH_CLEAN_BY_SET_WAY_THRESHOLD_INNER) {
+		inner_flush_cache_all();
+		if (prot != NVMAP_HANDLE_INNER_CACHEABLE)
+			outer_flush_range(block->base, block->base + len);
+		goto out;
+	}
+
+	pte = nvmap_alloc_pte((client ? client->dev : nvmap_dev), &addr);
+	if (IS_ERR(pte))
+		return PTR_ERR(pte);
+
+	kaddr = (phys_addr_t)addr;
+
+	while (phys < end) {
+		phys_addr_t next = (phys + PAGE_SIZE) & PAGE_MASK;
+		unsigned long pfn = __phys_to_pfn(phys);
+		void *base = (void *)kaddr + (phys & ~PAGE_MASK);
+
+		next = min(next, end);
+		set_pte_at(&init_mm, kaddr, *pte, pfn_pte(pfn, pgprot_kernel));
+		flush_tlb_kernel_page(kaddr);
+		__cpuc_flush_dcache_area(base, next - phys);
+		phys = next;
+	}
+
+	if (prot != NVMAP_HANDLE_INNER_CACHEABLE)
+		outer_flush_range(block->base, block->base + len);
+
+	nvmap_free_pte((client ? client->dev : nvmap_dev), pte);
+out:
+	wmb();
+	return 0;
+}
+
+void nvmap_carveout_commit_add(struct nvmap_client *client,
+			       struct nvmap_carveout_node *node,
+			       size_t len)
+{
+	unsigned long flags;
+
+	nvmap_ref_lock(client);
+	spin_lock_irqsave(&node->clients_lock, flags);
+	BUG_ON(list_empty(&client->carveout_commit[node->index].list) &&
+	       client->carveout_commit[node->index].commit != 0);
+
+	client->carveout_commit[node->index].commit += len;
+	/* if this client isn't already on the list of nodes for this heap,
+	   add it */
+	if (list_empty(&client->carveout_commit[node->index].list)) {
+		list_add(&client->carveout_commit[node->index].list,
+			 &node->clients);
+	}
+	spin_unlock_irqrestore(&node->clients_lock, flags);
+	nvmap_ref_unlock(client);
+}
+
+void nvmap_carveout_commit_subtract(struct nvmap_client *client,
+				    struct nvmap_carveout_node *node,
+				    size_t len)
+{
+	unsigned long flags;
+
+	if (!client)
+		return;
+
+	spin_lock_irqsave(&node->clients_lock, flags);
+	BUG_ON(client->carveout_commit[node->index].commit < len);
+	client->carveout_commit[node->index].commit -= len;
+	/* if no more allocation in this carveout for this node, delete it */
+	if (!client->carveout_commit[node->index].commit)
+		list_del_init(&client->carveout_commit[node->index].list);
+	spin_unlock_irqrestore(&node->clients_lock, flags);
+}
+
+static struct nvmap_client *get_client_from_carveout_commit(
+	struct nvmap_carveout_node *node, struct nvmap_carveout_commit *commit)
+{
+	struct nvmap_carveout_commit *first_commit = commit - node->index;
+	return (void *)first_commit - offsetof(struct nvmap_client,
+					       carveout_commit);
+}
+
+static DECLARE_WAIT_QUEUE_HEAD(wait_reclaim);
+static int wait_count;
+bool nvmap_shrink_carveout(struct nvmap_carveout_node *node)
+{
+	struct nvmap_carveout_commit *commit;
+	size_t selected_size = 0;
+	int selected_oom_adj = OOM_ADJUST_MIN;
+	struct task_struct *selected_task = NULL;
+	unsigned long flags;
+	bool wait = false;
+	int current_oom_adj = OOM_ADJUST_MIN;
+
+	task_lock(current);
+	if (current->signal)
+		current_oom_adj = current->signal->oom_adj;
+	task_unlock(current);
+
+	spin_lock_irqsave(&node->clients_lock, flags);
+	/* find the task with the smallest oom_adj (lowest priority)
+	 * and largest carveout allocation -- ignore kernel allocations,
+	 * there's no way to handle them */
+	list_for_each_entry(commit, &node->clients, list) {
+		struct nvmap_client *client =
+			get_client_from_carveout_commit(node, commit);
+		size_t size = commit->commit;
+		struct task_struct *task = client->task;
+		struct signal_struct *sig;
+
+		if (!task)
+			continue;
+
+		task_lock(task);
+		sig = task->signal;
+		if (!task->mm || !sig)
+			goto end;
+		/* don't try to kill current */
+		if (task == current->group_leader)
+			goto end;
+		/* don't try to kill higher priority tasks */
+		if (sig->oom_adj < current_oom_adj)
+			goto end;
+		if (sig->oom_adj < selected_oom_adj)
+			goto end;
+		if (sig->oom_adj == selected_oom_adj &&
+		    size <= selected_size)
+			goto end;
+		selected_oom_adj = sig->oom_adj;
+		selected_size = size;
+		selected_task = task;
+end:
+		task_unlock(task);
+	}
+	if (selected_task) {
+		wait = true;
+		if (fatal_signal_pending(selected_task)) {
+			pr_warning("carveout_killer: process %d dying "
+				   "slowly\n", selected_task->pid);
+			goto out;
+		}
+		pr_info("carveout_killer: killing process %d with oom_adj %d "
+			"to reclaim %d (for process with oom_adj %d)\n",
+			selected_task->pid, selected_oom_adj,
+			selected_size, current_oom_adj);
+		force_sig(SIGKILL, selected_task);
+	}
+out:
+	spin_unlock_irqrestore(&node->clients_lock, flags);
+	return wait;
+}
+
+static
+struct nvmap_heap_block *do_nvmap_carveout_alloc(struct nvmap_client *client,
+					      struct nvmap_handle *handle,
+					      unsigned long type)
+{
+	struct nvmap_carveout_node *co_heap;
+	struct nvmap_device *dev = client->dev;
+	int i;
+
+	for (i = 0; i < dev->nr_carveouts; i++) {
+		struct nvmap_heap_block *block;
+		co_heap = &dev->heaps[i];
+
+		if (!(co_heap->heap_bit & type))
+			continue;
+
+		block = nvmap_heap_alloc(co_heap->carveout, handle);
+		if (block)
+			return block;
+	}
+	return NULL;
+}
+
+static bool nvmap_carveout_freed(int count)
+{
+	smp_rmb();
+	return count != wait_count;
+}
+
+struct nvmap_heap_block *nvmap_carveout_alloc(struct nvmap_client *client,
+					      struct nvmap_handle *handle,
+					      unsigned long type)
+{
+	struct nvmap_heap_block *block;
+	struct nvmap_carveout_node *co_heap;
+	struct nvmap_device *dev = client->dev;
+	int i;
+	unsigned long end = jiffies +
+		msecs_to_jiffies(NVMAP_CARVEOUT_KILLER_RETRY_TIME);
+	int count = 0;
+
+	do {
+		block = do_nvmap_carveout_alloc(client, handle, type);
+		if (!carveout_killer)
+			return block;
+
+		if (block)
+			return block;
+
+		if (!count++) {
+			char task_comm[TASK_COMM_LEN];
+			if (client->task)
+				get_task_comm(task_comm, client->task);
+			else
+				task_comm[0] = 0;
+			pr_info("%s: failed to allocate %u bytes for "
+				"process %s, firing carveout "
+				"killer!\n", __func__, handle->size, task_comm);
+
+		} else {
+			pr_info("%s: still can't allocate %u bytes, "
+				"attempt %d!\n", __func__, handle->size, count);
+		}
+
+		/* shrink carveouts that matter and try again */
+		for (i = 0; i < dev->nr_carveouts; i++) {
+			int count;
+			co_heap = &dev->heaps[i];
+
+			if (!(co_heap->heap_bit & type))
+				continue;
+
+			count = wait_count;
+			/* indicates we didn't find anything to kill,
+			   might as well stop trying */
+			if (!nvmap_shrink_carveout(co_heap))
+				return NULL;
+
+			if (time_is_after_jiffies(end))
+				wait_event_interruptible_timeout(wait_reclaim,
+					 nvmap_carveout_freed(count),
+					 end - jiffies);
+		}
+	} while (time_is_after_jiffies(end));
+
+	if (time_is_before_jiffies(end))
+		pr_info("carveout_killer: timeout expired without "
+			"allocation succeeding.\n");
+
+	return NULL;
+}
+
+/* remove a handle from the device's tree of all handles; called
+ * when freeing handles. */
+int nvmap_handle_remove(struct nvmap_device *dev, struct nvmap_handle *h)
+{
+	spin_lock(&dev->handle_lock);
+
+	/* re-test inside the spinlock if the handle really has no clients;
+	 * only remove the handle if it is unreferenced */
+	if (atomic_add_return(0, &h->ref) > 0) {
+		spin_unlock(&dev->handle_lock);
+		return -EBUSY;
+	}
+	smp_rmb();
+	BUG_ON(atomic_read(&h->ref) < 0);
+	BUG_ON(atomic_read(&h->pin) != 0);
+
+	rb_erase(&h->node, &dev->handles);
+
+	spin_unlock(&dev->handle_lock);
+	return 0;
+}
+
+/* adds a newly-created handle to the device master tree */
+void nvmap_handle_add(struct nvmap_device *dev, struct nvmap_handle *h)
+{
+	struct rb_node **p;
+	struct rb_node *parent = NULL;
+
+	spin_lock(&dev->handle_lock);
+	p = &dev->handles.rb_node;
+	while (*p) {
+		struct nvmap_handle *b;
+
+		parent = *p;
+		b = rb_entry(parent, struct nvmap_handle, node);
+		if (h > b)
+			p = &parent->rb_right;
+		else
+			p = &parent->rb_left;
+	}
+	rb_link_node(&h->node, parent, p);
+	rb_insert_color(&h->node, &dev->handles);
+	spin_unlock(&dev->handle_lock);
+}
+
+/* validates that a handle is in the device master tree, and that the
+ * client has permission to access it */
+struct nvmap_handle *nvmap_validate_get(struct nvmap_client *client,
+					unsigned long id)
+{
+	struct nvmap_handle *h = NULL;
+	struct rb_node *n;
+
+	spin_lock(&client->dev->handle_lock);
+
+	n = client->dev->handles.rb_node;
+
+	while (n) {
+		h = rb_entry(n, struct nvmap_handle, node);
+		if ((unsigned long)h == id) {
+			if (client->super || h->global || (h->owner == client))
+				h = nvmap_handle_get(h);
+			else
+				h = NULL;
+			spin_unlock(&client->dev->handle_lock);
+			return h;
+		}
+		if (id > (unsigned long)h)
+			n = n->rb_right;
+		else
+			n = n->rb_left;
+	}
+	spin_unlock(&client->dev->handle_lock);
+	return NULL;
+}
+
+struct nvmap_client *nvmap_create_client(struct nvmap_device *dev,
+					 const char *name)
+{
+	struct nvmap_client *client;
+	struct task_struct *task;
+	int i;
+
+	if (WARN_ON(!dev))
+		return NULL;
+
+	client = kzalloc(sizeof(*client) + (sizeof(struct nvmap_carveout_commit)
+			 * dev->nr_carveouts), GFP_KERNEL);
+	if (!client)
+		return NULL;
+
+	client->name = name;
+	client->super = true;
+	client->dev = dev;
+	/* TODO: allocate unique IOVMM client for each nvmap client */
+	client->share = &dev->iovmm_master;
+	client->handle_refs = RB_ROOT;
+
+	atomic_set(&client->iovm_commit, 0);
+
+	client->iovm_limit = nvmap_mru_vm_size(client->share->iovmm);
+
+	for (i = 0; i < dev->nr_carveouts; i++) {
+		INIT_LIST_HEAD(&client->carveout_commit[i].list);
+		client->carveout_commit[i].commit = 0;
+	}
+
+	get_task_struct(current->group_leader);
+	task_lock(current->group_leader);
+	/* don't bother to store task struct for kernel threads,
+	   they can't be killed anyway */
+	if (current->flags & PF_KTHREAD) {
+		put_task_struct(current->group_leader);
+		task = NULL;
+	} else {
+		task = current->group_leader;
+	}
+	task_unlock(current->group_leader);
+	client->task = task;
+
+	mutex_init(&client->ref_lock);
+	atomic_set(&client->count, 1);
+
+	spin_lock(&dev->clients_lock);
+	list_add(&client->list, &dev->clients);
+	spin_unlock(&dev->clients_lock);
+	return client;
+}
+EXPORT_SYMBOL(nvmap_create_client);
+
+static void destroy_client(struct nvmap_client *client)
+{
+	struct rb_node *n;
+	int i;
+
+	if (!client)
+		return;
+
+
+	while ((n = rb_first(&client->handle_refs))) {
+		struct nvmap_handle_ref *ref;
+		int pins, dupes;
+
+		ref = rb_entry(n, struct nvmap_handle_ref, node);
+		rb_erase(&ref->node, &client->handle_refs);
+
+		smp_rmb();
+		pins = atomic_read(&ref->pin);
+
+		if (ref->handle->owner == client)
+			ref->handle->owner = NULL;
+
+		while (pins--)
+			nvmap_unpin_handles(client, &ref->handle, 1);
+
+		dupes = atomic_read(&ref->dupes);
+		while (dupes--)
+			nvmap_handle_put(ref->handle);
+
+		kfree(ref);
+	}
+
+	if (carveout_killer) {
+		wait_count++;
+		smp_wmb();
+		wake_up_all(&wait_reclaim);
+	}
+
+	for (i = 0; i < client->dev->nr_carveouts; i++)
+		list_del(&client->carveout_commit[i].list);
+
+	if (client->task)
+		put_task_struct(client->task);
+
+	spin_lock(&client->dev->clients_lock);
+	list_del(&client->list);
+	spin_unlock(&client->dev->clients_lock);
+	kfree(client);
+}
+
+struct nvmap_client *nvmap_client_get(struct nvmap_client *client)
+{
+	if (WARN_ON(!client))
+		return NULL;
+
+	if (WARN_ON(!atomic_add_unless(&client->count, 1, 0)))
+		return NULL;
+
+	return client;
+}
+
+struct nvmap_client *nvmap_client_get_file(int fd)
+{
+	struct nvmap_client *client = ERR_PTR(-EFAULT);
+	struct file *f = fget(fd);
+	if (!f)
+		return ERR_PTR(-EINVAL);
+
+	if ((f->f_op == &nvmap_user_fops) || (f->f_op == &nvmap_super_fops)) {
+		client = f->private_data;
+		atomic_inc(&client->count);
+	}
+
+	fput(f);
+	return client;
+}
+
+void nvmap_client_put(struct nvmap_client *client)
+{
+	if (!client)
+		return;
+
+	if (!atomic_dec_return(&client->count))
+		destroy_client(client);
+}
+EXPORT_SYMBOL(nvmap_client_put);
+
+static int nvmap_open(struct inode *inode, struct file *filp)
+{
+	struct miscdevice *miscdev = filp->private_data;
+	struct nvmap_device *dev = dev_get_drvdata(miscdev->parent);
+	struct nvmap_client *priv;
+	int ret;
+
+	ret = nonseekable_open(inode, filp);
+	if (unlikely(ret))
+		return ret;
+
+	BUG_ON(dev != nvmap_dev);
+	priv = nvmap_create_client(dev, "user");
+	if (!priv)
+		return -ENOMEM;
+	trace_nvmap_open(priv);
+
+	priv->super = (filp->f_op == &nvmap_super_fops);
+
+	filp->f_mapping->backing_dev_info = &nvmap_bdi;
+
+	filp->private_data = priv;
+	return 0;
+}
+
+static int nvmap_release(struct inode *inode, struct file *filp)
+{
+	trace_nvmap_release(filp->private_data);
+	nvmap_client_put(filp->private_data);
+	return 0;
+}
+
+static int nvmap_map(struct file *filp, struct vm_area_struct *vma)
+{
+	struct nvmap_vma_priv *priv;
+
+	/* after NVMAP_IOC_MMAP, the handle that is mapped by this VMA
+	 * will be stored in vm_private_data and faulted in. until the
+	 * ioctl is made, the VMA is mapped no-access */
+	vma->vm_private_data = NULL;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->offs = 0;
+	priv->handle = NULL;
+	atomic_set(&priv->count, 1);
+
+	vma->vm_flags |= VM_SHARED;
+	vma->vm_flags |= (VM_IO | VM_DONTEXPAND | VM_MIXEDMAP | VM_RESERVED);
+	vma->vm_ops = &nvmap_vma_ops;
+	vma->vm_private_data = priv;
+
+	return 0;
+}
+
+static long nvmap_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	int err = 0;
+	void __user *uarg = (void __user *)arg;
+
+	if (_IOC_TYPE(cmd) != NVMAP_IOC_MAGIC)
+		return -ENOTTY;
+
+	if (_IOC_NR(cmd) > NVMAP_IOC_MAXNR)
+		return -ENOTTY;
+
+	if (_IOC_DIR(cmd) & _IOC_READ)
+		err = !access_ok(VERIFY_WRITE, uarg, _IOC_SIZE(cmd));
+	if (_IOC_DIR(cmd) & _IOC_WRITE)
+		err = !access_ok(VERIFY_READ, uarg, _IOC_SIZE(cmd));
+
+	if (err)
+		return -EFAULT;
+
+	switch (cmd) {
+	case NVMAP_IOC_CLAIM:
+		nvmap_warn(filp->private_data, "preserved handles not"
+			   "supported\n");
+		err = -ENODEV;
+		break;
+	case NVMAP_IOC_CREATE:
+	case NVMAP_IOC_FROM_ID:
+		err = nvmap_ioctl_create(filp, cmd, uarg);
+		break;
+
+	case NVMAP_IOC_GET_ID:
+		err = nvmap_ioctl_getid(filp, uarg);
+		break;
+
+	case NVMAP_IOC_PARAM:
+		err = nvmap_ioctl_get_param(filp, uarg);
+		break;
+
+	case NVMAP_IOC_UNPIN_MULT:
+	case NVMAP_IOC_PIN_MULT:
+		err = nvmap_ioctl_pinop(filp, cmd == NVMAP_IOC_PIN_MULT, uarg);
+		break;
+
+	case NVMAP_IOC_ALLOC:
+		err = nvmap_ioctl_alloc(filp, uarg);
+		break;
+
+	case NVMAP_IOC_FREE:
+		err = nvmap_ioctl_free(filp, arg);
+		break;
+
+	case NVMAP_IOC_MMAP:
+		err = nvmap_map_into_caller_ptr(filp, uarg);
+		break;
+
+	case NVMAP_IOC_WRITE:
+	case NVMAP_IOC_READ:
+		err = nvmap_ioctl_rw_handle(filp, cmd == NVMAP_IOC_READ, uarg);
+		break;
+
+	case NVMAP_IOC_CACHE:
+		err = nvmap_ioctl_cache_maint(filp, uarg);
+		break;
+
+	default:
+		return -ENOTTY;
+	}
+	return err;
+}
+
+/* to ensure that the backing store for the VMA isn't freed while a fork'd
+ * reference still exists, nvmap_vma_open increments the reference count on
+ * the handle, and nvmap_vma_close decrements it. alternatively, we could
+ * disallow copying of the vma, or behave like pmem and zap the pages. FIXME.
+*/
+static void nvmap_vma_open(struct vm_area_struct *vma)
+{
+	struct nvmap_vma_priv *priv;
+
+	priv = vma->vm_private_data;
+	BUG_ON(!priv);
+
+	atomic_inc(&priv->count);
+	if(priv->handle)
+		nvmap_usecount_inc(priv->handle);
+}
+
+static void nvmap_vma_close(struct vm_area_struct *vma)
+{
+	struct nvmap_vma_priv *priv = vma->vm_private_data;
+
+	if (priv) {
+		if (priv->handle) {
+			BUG_ON(priv->handle->usecount == 0);
+			nvmap_usecount_dec(priv->handle);
+		}
+		if (!atomic_dec_return(&priv->count)) {
+			if (priv->handle)
+				nvmap_handle_put(priv->handle);
+			kfree(priv);
+		}
+	}
+	vma->vm_private_data = NULL;
+}
+
+static int nvmap_vma_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
+{
+	struct nvmap_vma_priv *priv;
+	unsigned long offs;
+
+	offs = (unsigned long)(vmf->virtual_address - vma->vm_start);
+	priv = vma->vm_private_data;
+	if (!priv || !priv->handle || !priv->handle->alloc)
+		return VM_FAULT_SIGBUS;
+
+	offs += priv->offs;
+	/* if the VMA was split for some reason, vm_pgoff will be the VMA's
+	 * offset from the original VMA */
+	offs += (vma->vm_pgoff << PAGE_SHIFT);
+
+	if (offs >= priv->handle->size)
+		return VM_FAULT_SIGBUS;
+
+	if (!priv->handle->heap_pgalloc) {
+		unsigned long pfn;
+		BUG_ON(priv->handle->carveout->base & ~PAGE_MASK);
+		pfn = ((priv->handle->carveout->base + offs) >> PAGE_SHIFT);
+		vm_insert_pfn(vma, (unsigned long)vmf->virtual_address, pfn);
+		return VM_FAULT_NOPAGE;
+	} else {
+		struct page *page;
+		offs >>= PAGE_SHIFT;
+		page = priv->handle->pgalloc.pages[offs];
+		if (page)
+			get_page(page);
+		vmf->page = page;
+		return (page) ? 0 : VM_FAULT_SIGBUS;
+	}
+}
+
+static ssize_t attr_show_usage(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	struct nvmap_carveout_node *node = nvmap_heap_device_to_arg(dev);
+
+	return sprintf(buf, "%08x\n", node->heap_bit);
+}
+
+static struct device_attribute heap_attr_show_usage =
+	__ATTR(usage, S_IRUGO, attr_show_usage, NULL);
+
+static struct attribute *heap_extra_attrs[] = {
+	&heap_attr_show_usage.attr,
+	NULL,
+};
+
+static struct attribute_group heap_extra_attr_group = {
+	.attrs = heap_extra_attrs,
+};
+
+static void client_stringify(struct nvmap_client *client, struct seq_file *s)
+{
+	char task_comm[TASK_COMM_LEN];
+	if (!client->task) {
+		seq_printf(s, "%-18s %18s %8u", client->name, "kernel", 0);
+		return;
+	}
+	get_task_comm(task_comm, client->task);
+	seq_printf(s, "%-18s %18s %8u", client->name, task_comm,
+		   client->task->pid);
+}
+
+static void allocations_stringify(struct nvmap_client *client,
+				  struct seq_file *s, bool iovmm)
+{
+	struct rb_node *n = rb_first(&client->handle_refs);
+
+	for (; n != NULL; n = rb_next(n)) {
+		struct nvmap_handle_ref *ref =
+			rb_entry(n, struct nvmap_handle_ref, node);
+		struct nvmap_handle *handle = ref->handle;
+		if (handle->alloc && handle->heap_pgalloc == iovmm) {
+			unsigned long base = iovmm ? 0:
+				(unsigned long)(handle->carveout->base);
+			seq_printf(s, "%-18s %-18s %8lx %10u %8x\n", "", "",
+					base, handle->size, handle->userflags);
+		}
+	}
+}
+
+static int nvmap_debug_allocations_show(struct seq_file *s, void *unused)
+{
+	struct nvmap_carveout_node *node = s->private;
+	struct nvmap_carveout_commit *commit;
+	unsigned long flags;
+	unsigned int total = 0;
+
+	spin_lock_irqsave(&node->clients_lock, flags);
+	seq_printf(s, "%-18s %18s %8s %10s %8s\n", "CLIENT", "PROCESS", "PID",
+		"SIZE", "FLAGS");
+	seq_printf(s, "%-18s %18s %8s %10s\n", "", "",
+					"BASE", "SIZE");
+	list_for_each_entry(commit, &node->clients, list) {
+		struct nvmap_client *client =
+			get_client_from_carveout_commit(node, commit);
+		client_stringify(client, s);
+		seq_printf(s, " %10u\n", commit->commit);
+		allocations_stringify(client, s, false);
+		seq_printf(s, "\n");
+		total += commit->commit;
+	}
+	seq_printf(s, "%-18s %-18s %8u %10u\n", "total", "", 0, total);
+	spin_unlock_irqrestore(&node->clients_lock, flags);
+
+	return 0;
+}
+
+static int nvmap_debug_allocations_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, nvmap_debug_allocations_show,
+			   inode->i_private);
+}
+
+static const struct file_operations debug_allocations_fops = {
+	.open = nvmap_debug_allocations_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static int nvmap_debug_clients_show(struct seq_file *s, void *unused)
+{
+	struct nvmap_carveout_node *node = s->private;
+	struct nvmap_carveout_commit *commit;
+	unsigned long flags;
+	unsigned int total = 0;
+
+	spin_lock_irqsave(&node->clients_lock, flags);
+	seq_printf(s, "%-18s %18s %8s %10s\n", "CLIENT", "PROCESS", "PID",
+		"SIZE");
+	list_for_each_entry(commit, &node->clients, list) {
+		struct nvmap_client *client =
+			get_client_from_carveout_commit(node, commit);
+		client_stringify(client, s);
+		seq_printf(s, " %10u\n", commit->commit);
+		total += commit->commit;
+	}
+	seq_printf(s, "%-18s %18s %8u %10u\n", "total", "", 0, total);
+	spin_unlock_irqrestore(&node->clients_lock, flags);
+
+	return 0;
+}
+
+static int nvmap_debug_clients_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, nvmap_debug_clients_show, inode->i_private);
+}
+
+static const struct file_operations debug_clients_fops = {
+	.open = nvmap_debug_clients_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static int nvmap_debug_iovmm_clients_show(struct seq_file *s, void *unused)
+{
+	unsigned long flags;
+	unsigned int total = 0;
+	struct nvmap_client *client;
+	struct nvmap_device *dev = s->private;
+
+	spin_lock_irqsave(&dev->clients_lock, flags);
+	seq_printf(s, "%-18s %18s %8s %10s\n", "CLIENT", "PROCESS", "PID",
+		"SIZE");
+	list_for_each_entry(client, &dev->clients, list) {
+		client_stringify(client, s);
+		seq_printf(s, " %10u\n", atomic_read(&client->iovm_commit));
+		total += atomic_read(&client->iovm_commit);
+	}
+	seq_printf(s, "%-18s %18s %8u %10u\n", "total", "", 0, total);
+	spin_unlock_irqrestore(&dev->clients_lock, flags);
+
+	return 0;
+}
+
+static int nvmap_debug_iovmm_clients_open(struct inode *inode,
+					    struct file *file)
+{
+	return single_open(file, nvmap_debug_iovmm_clients_show,
+			    inode->i_private);
+}
+
+static const struct file_operations debug_iovmm_clients_fops = {
+	.open = nvmap_debug_iovmm_clients_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static int nvmap_debug_iovmm_allocations_show(struct seq_file *s, void *unused)
+{
+	unsigned long flags;
+	unsigned int total = 0;
+	struct nvmap_client *client;
+	struct nvmap_device *dev = s->private;
+
+	spin_lock_irqsave(&dev->clients_lock, flags);
+	seq_printf(s, "%-18s %18s %8s %10s %8s\n", "CLIENT", "PROCESS", "PID",
+		"SIZE", "FLAGS");
+	seq_printf(s, "%-18s %18s %8s %10s\n", "", "",
+					"BASE", "SIZE");
+	list_for_each_entry(client, &dev->clients, list) {
+		client_stringify(client, s);
+		seq_printf(s, " %10u\n", atomic_read(&client->iovm_commit));
+		allocations_stringify(client, s, true);
+		seq_printf(s, "\n");
+		total += atomic_read(&client->iovm_commit);
+	}
+	seq_printf(s, "%-18s %-18s %8u %10u\n", "total", "", 0, total);
+	spin_unlock_irqrestore(&dev->clients_lock, flags);
+
+	return 0;
+}
+
+static int nvmap_debug_iovmm_allocations_open(struct inode *inode,
+						struct file *file)
+{
+	return single_open(file, nvmap_debug_iovmm_allocations_show,
+			    inode->i_private);
+}
+
+static const struct file_operations debug_iovmm_allocations_fops = {
+	.open = nvmap_debug_iovmm_allocations_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static int nvmap_probe(struct platform_device *pdev)
+{
+	struct nvmap_platform_data *plat = pdev->dev.platform_data;
+	struct nvmap_device *dev;
+	struct dentry *nvmap_debug_root;
+	unsigned int i;
+	int e;
+
+	if (!plat) {
+		dev_err(&pdev->dev, "no platform data?\n");
+		return -ENODEV;
+	}
+
+	if (WARN_ON(nvmap_dev != NULL)) {
+		dev_err(&pdev->dev, "only one nvmap device may be present\n");
+		return -ENODEV;
+	}
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		dev_err(&pdev->dev, "out of memory for device\n");
+		return -ENOMEM;
+	}
+
+	dev->dev_user.minor = MISC_DYNAMIC_MINOR;
+	dev->dev_user.name = "nvmap";
+	dev->dev_user.fops = &nvmap_user_fops;
+	dev->dev_user.parent = &pdev->dev;
+
+	dev->dev_super.minor = MISC_DYNAMIC_MINOR;
+	dev->dev_super.name = "knvmap";
+	dev->dev_super.fops = &nvmap_super_fops;
+	dev->dev_super.parent = &pdev->dev;
+
+	dev->handles = RB_ROOT;
+
+	init_waitqueue_head(&dev->pte_wait);
+
+	init_waitqueue_head(&dev->iovmm_master.pin_wait);
+	mutex_init(&dev->iovmm_master.pin_lock);
+#ifdef CONFIG_NVMAP_PAGE_POOLS
+	for (i = 0; i < NVMAP_NUM_POOLS; i++)
+		nvmap_page_pool_init(&dev->iovmm_master.pools[i], i);
+#endif
+
+	dev->iovmm_master.iovmm =
+		tegra_iovmm_alloc_client(&pdev->dev, NULL,
+			&(dev->dev_user));
+#if defined(CONFIG_TEGRA_IOVMM) || defined(CONFIG_IOMMU_API)
+	if (!dev->iovmm_master.iovmm) {
+		e = PTR_ERR(dev->iovmm_master.iovmm);
+		dev_err(&pdev->dev, "couldn't create iovmm client\n");
+		goto fail;
+	}
+#endif
+	dev->vm_rgn = alloc_vm_area(NVMAP_NUM_PTES * PAGE_SIZE);
+	if (!dev->vm_rgn) {
+		e = -ENOMEM;
+		dev_err(&pdev->dev, "couldn't allocate remapping region\n");
+		goto fail;
+	}
+	e = nvmap_mru_init(&dev->iovmm_master);
+	if (e) {
+		dev_err(&pdev->dev, "couldn't initialize MRU lists\n");
+		goto fail;
+	}
+
+	spin_lock_init(&dev->ptelock);
+	spin_lock_init(&dev->handle_lock);
+	INIT_LIST_HEAD(&dev->clients);
+	spin_lock_init(&dev->clients_lock);
+
+	for (i = 0; i < NVMAP_NUM_PTES; i++) {
+		unsigned long addr;
+		pgd_t *pgd;
+		pud_t *pud;
+		pmd_t *pmd;
+
+		addr = (unsigned long)dev->vm_rgn->addr + (i * PAGE_SIZE);
+		pgd = pgd_offset_k(addr);
+		pud = pud_alloc(&init_mm, pgd, addr);
+		if (!pud) {
+			e = -ENOMEM;
+			dev_err(&pdev->dev, "couldn't allocate page tables\n");
+			goto fail;
+		}
+		pmd = pmd_alloc(&init_mm, pud, addr);
+		if (!pmd) {
+			e = -ENOMEM;
+			dev_err(&pdev->dev, "couldn't allocate page tables\n");
+			goto fail;
+		}
+		dev->ptes[i] = pte_alloc_kernel(pmd, addr);
+		if (!dev->ptes[i]) {
+			e = -ENOMEM;
+			dev_err(&pdev->dev, "couldn't allocate page tables\n");
+			goto fail;
+		}
+	}
+
+	e = misc_register(&dev->dev_user);
+	if (e) {
+		dev_err(&pdev->dev, "unable to register miscdevice %s\n",
+			dev->dev_user.name);
+		goto fail;
+	}
+
+	e = misc_register(&dev->dev_super);
+	if (e) {
+		dev_err(&pdev->dev, "unable to register miscdevice %s\n",
+			dev->dev_super.name);
+		goto fail;
+	}
+
+	dev->nr_carveouts = 0;
+	dev->heaps = kzalloc(sizeof(struct nvmap_carveout_node) *
+			     plat->nr_carveouts, GFP_KERNEL);
+	if (!dev->heaps) {
+		e = -ENOMEM;
+		dev_err(&pdev->dev, "couldn't allocate carveout memory\n");
+		goto fail;
+	}
+
+	nvmap_debug_root = debugfs_create_dir("nvmap", NULL);
+	if (IS_ERR_OR_NULL(nvmap_debug_root))
+		dev_err(&pdev->dev, "couldn't create debug files\n");
+
+	for (i = 0; i < plat->nr_carveouts; i++) {
+		struct nvmap_carveout_node *node = &dev->heaps[dev->nr_carveouts];
+		const struct nvmap_platform_carveout *co = &plat->carveouts[i];
+		if (!co->size)
+			continue;
+		node->carveout = nvmap_heap_create(dev->dev_user.this_device,
+				   co->name, co->base, co->size,
+				   co->buddy_size, node);
+		if (!node->carveout) {
+			e = -ENOMEM;
+			dev_err(&pdev->dev, "couldn't create %s\n", co->name);
+			goto fail_heaps;
+		}
+		node->index = dev->nr_carveouts;
+		dev->nr_carveouts++;
+		spin_lock_init(&node->clients_lock);
+		INIT_LIST_HEAD(&node->clients);
+		node->heap_bit = co->usage_mask;
+		if (nvmap_heap_create_group(node->carveout,
+					    &heap_extra_attr_group))
+			dev_warn(&pdev->dev, "couldn't add extra attributes\n");
+
+		dev_info(&pdev->dev, "created carveout %s (%uKiB)\n",
+			 co->name, co->size / 1024);
+
+		if (!IS_ERR_OR_NULL(nvmap_debug_root)) {
+			struct dentry *heap_root =
+				debugfs_create_dir(co->name, nvmap_debug_root);
+			if (!IS_ERR_OR_NULL(heap_root)) {
+				debugfs_create_file("clients", 0664, heap_root,
+				    node, &debug_clients_fops);
+				debugfs_create_file("allocations", 0664,
+				    heap_root, node, &debug_allocations_fops);
+			}
+		}
+	}
+	if (!IS_ERR_OR_NULL(nvmap_debug_root)) {
+		struct dentry *iovmm_root =
+			debugfs_create_dir("iovmm", nvmap_debug_root);
+		if (!IS_ERR_OR_NULL(iovmm_root)) {
+			debugfs_create_file("clients", 0664, iovmm_root,
+				dev, &debug_iovmm_clients_fops);
+			debugfs_create_file("allocations", 0664, iovmm_root,
+				dev, &debug_iovmm_allocations_fops);
+#ifdef CONFIG_NVMAP_PAGE_POOLS
+			for (i = 0; i < NVMAP_NUM_POOLS; i++) {
+				char name[40];
+				char *memtype_string[] = {"uc", "wc",
+							  "iwb", "wb"};
+				sprintf(name, "%s_page_pool_available_pages",
+					memtype_string[i]);
+				debugfs_create_u32(name, S_IRUGO|S_IWUSR,
+					iovmm_root,
+					&dev->iovmm_master.pools[i].npages);
+			}
+#endif
+		}
+	}
+
+	platform_set_drvdata(pdev, dev);
+	nvmap_dev = dev;
+
+	return 0;
+fail_heaps:
+	for (i = 0; i < dev->nr_carveouts; i++) {
+		struct nvmap_carveout_node *node = &dev->heaps[i];
+		nvmap_heap_remove_group(node->carveout, &heap_extra_attr_group);
+		nvmap_heap_destroy(node->carveout);
+	}
+fail:
+	kfree(dev->heaps);
+	nvmap_mru_destroy(&dev->iovmm_master);
+	if (dev->dev_super.minor != MISC_DYNAMIC_MINOR)
+		misc_deregister(&dev->dev_super);
+	if (dev->dev_user.minor != MISC_DYNAMIC_MINOR)
+		misc_deregister(&dev->dev_user);
+	if (!IS_ERR_OR_NULL(dev->iovmm_master.iovmm))
+		tegra_iovmm_free_client(dev->iovmm_master.iovmm);
+	if (dev->vm_rgn)
+		free_vm_area(dev->vm_rgn);
+	kfree(dev);
+	nvmap_dev = NULL;
+	return e;
+}
+
+static int nvmap_remove(struct platform_device *pdev)
+{
+	struct nvmap_device *dev = platform_get_drvdata(pdev);
+	struct rb_node *n;
+	struct nvmap_handle *h;
+	int i;
+
+	misc_deregister(&dev->dev_super);
+	misc_deregister(&dev->dev_user);
+
+	while ((n = rb_first(&dev->handles))) {
+		h = rb_entry(n, struct nvmap_handle, node);
+		rb_erase(&h->node, &dev->handles);
+		kfree(h);
+	}
+
+	if (!IS_ERR_OR_NULL(dev->iovmm_master.iovmm))
+		tegra_iovmm_free_client(dev->iovmm_master.iovmm);
+
+	nvmap_mru_destroy(&dev->iovmm_master);
+
+	for (i = 0; i < dev->nr_carveouts; i++) {
+		struct nvmap_carveout_node *node = &dev->heaps[i];
+		nvmap_heap_remove_group(node->carveout, &heap_extra_attr_group);
+		nvmap_heap_destroy(node->carveout);
+	}
+	kfree(dev->heaps);
+
+	free_vm_area(dev->vm_rgn);
+	kfree(dev);
+	nvmap_dev = NULL;
+	return 0;
+}
+
+static int nvmap_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	return 0;
+}
+
+static int nvmap_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static struct platform_driver nvmap_driver = {
+	.probe		= nvmap_probe,
+	.remove		= nvmap_remove,
+	.suspend	= nvmap_suspend,
+	.resume		= nvmap_resume,
+
+	.driver = {
+		.name	= "tegra-nvmap",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init nvmap_init_driver(void)
+{
+	int e;
+
+	nvmap_dev = NULL;
+
+	e = nvmap_heap_init();
+	if (e)
+		goto fail;
+
+	e = platform_driver_register(&nvmap_driver);
+	if (e) {
+		nvmap_heap_deinit();
+		goto fail;
+	}
+
+fail:
+	return e;
+}
+fs_initcall(nvmap_init_driver);
+
+static void __exit nvmap_exit_driver(void)
+{
+	platform_driver_unregister(&nvmap_driver);
+	nvmap_heap_deinit();
+	nvmap_dev = NULL;
+}
+module_exit(nvmap_exit_driver);
diff --git a/drivers/video/tegra/nvmap/nvmap_handle.c b/drivers/video/tegra/nvmap/nvmap_handle.c
new file mode 100644
index 000000000000..1e0ce2c59c17
--- /dev/null
+++ b/drivers/video/tegra/nvmap/nvmap_handle.c
@@ -0,0 +1,1065 @@
+/*
+ * drivers/video/tegra/nvmap/nvmap_handle.c
+ *
+ * Handle allocation and freeing routines for nvmap
+ *
+ * Copyright (c) 2009-2014, NVIDIA 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#define pr_fmt(fmt)	"%s: " fmt, __func__
+
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/mm.h>
+#include <linux/rbtree.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/fs.h>
+#include <linux/shrinker.h>
+#include <linux/moduleparam.h>
+#include <linux/nvmap.h>
+
+#include <asm/cacheflush.h>
+#include <asm/outercache.h>
+#include <asm/tlbflush.h>
+#include <asm/pgtable.h>
+
+#include <mach/iovmm.h>
+#include <trace/events/nvmap.h>
+
+#include "nvmap.h"
+#include "nvmap_mru.h"
+#include "nvmap_common.h"
+
+#define NVMAP_SECURE_HEAPS	(NVMAP_HEAP_CARVEOUT_IRAM | NVMAP_HEAP_IOVMM | \
+				 NVMAP_HEAP_CARVEOUT_VPR)
+#ifdef CONFIG_NVMAP_HIGHMEM_ONLY
+#define GFP_NVMAP		(__GFP_HIGHMEM | __GFP_NOWARN)
+#else
+#define GFP_NVMAP		(GFP_KERNEL | __GFP_HIGHMEM | __GFP_NOWARN)
+#endif
+/* handles may be arbitrarily large (16+MiB), and any handle allocated from
+ * the kernel (i.e., not a carveout handle) includes its array of pages. to
+ * preserve kmalloc space, if the array of pages exceeds PAGELIST_VMALLOC_MIN,
+ * the array is allocated using vmalloc. */
+#define PAGELIST_VMALLOC_MIN	(PAGE_SIZE)
+
+#ifdef CONFIG_NVMAP_PAGE_POOLS
+
+#define NVMAP_TEST_PAGE_POOL_SHRINKER 1
+static bool enable_pp = 1;
+static int pool_size[NVMAP_NUM_POOLS];
+
+static char *s_memtype_str[] = {
+	"uc",
+	"wc",
+	"iwb",
+	"wb",
+};
+
+static inline void nvmap_page_pool_lock(struct nvmap_page_pool *pool)
+{
+	mutex_lock(&pool->lock);
+}
+
+static inline void nvmap_page_pool_unlock(struct nvmap_page_pool *pool)
+{
+	mutex_unlock(&pool->lock);
+}
+
+static struct page *nvmap_page_pool_alloc_locked(struct nvmap_page_pool *pool)
+{
+	struct page *page = NULL;
+
+	if (pool->npages > 0) {
+		page = pool->page_array[--pool->npages];
+		atomic_dec(&page->_count);
+		BUG_ON(atomic_read(&page->_count) != 1);
+	}
+	return page;
+}
+
+static struct page *nvmap_page_pool_alloc(struct nvmap_page_pool *pool)
+{
+	struct page *page = NULL;
+
+	if (pool) {
+		nvmap_page_pool_lock(pool);
+		page = nvmap_page_pool_alloc_locked(pool);
+		nvmap_page_pool_unlock(pool);
+	}
+	return page;
+}
+
+static bool nvmap_page_pool_release_locked(struct nvmap_page_pool *pool,
+					    struct page *page)
+{
+	int ret = false;
+
+	BUG_ON(atomic_read(&page->_count) != 1);
+	if (enable_pp && pool->npages < pool->max_pages) {
+		atomic_inc(&page->_count);
+		pool->page_array[pool->npages++] = page;
+		ret = true;
+	}
+	return ret;
+}
+
+static bool nvmap_page_pool_release(struct nvmap_page_pool *pool,
+					  struct page *page)
+{
+	int ret = false;
+
+	if (pool) {
+		nvmap_page_pool_lock(pool);
+		ret = nvmap_page_pool_release_locked(pool, page);
+		nvmap_page_pool_unlock(pool);
+	}
+	return ret;
+}
+
+static int nvmap_page_pool_get_available_count(struct nvmap_page_pool *pool)
+{
+	return pool->npages;
+}
+
+static int nvmap_page_pool_free(struct nvmap_page_pool *pool, int nr_free)
+{
+	int err;
+	int i = nr_free;
+	int idx = 0;
+	struct page *page;
+
+	if (!nr_free)
+		return nr_free;
+	nvmap_page_pool_lock(pool);
+	while (i) {
+		page = nvmap_page_pool_alloc_locked(pool);
+		if (!page)
+			break;
+		pool->shrink_array[idx++] = page;
+		i--;
+	}
+
+	if (idx) {
+		/* This op should never fail. */
+		err = set_pages_array_wb(pool->shrink_array, idx);
+		BUG_ON(err);
+	}
+
+	while (idx--)
+		__free_page(pool->shrink_array[idx]);
+	nvmap_page_pool_unlock(pool);
+	return i;
+}
+
+static int nvmap_page_pool_get_unused_pages(void)
+{
+	unsigned int i;
+	int total = 0;
+	struct nvmap_share *share = nvmap_get_share_from_dev(nvmap_dev);
+
+	for (i = 0; i < NVMAP_NUM_POOLS; i++)
+		total += nvmap_page_pool_get_available_count(&share->pools[i]);
+
+	return total;
+}
+
+static void nvmap_page_pool_resize(struct nvmap_page_pool *pool, int size)
+{
+	int available_pages;
+	int pages_to_release = 0;
+	struct page **page_array = NULL;
+	struct page **shrink_array = NULL;
+
+	if (size == pool->max_pages)
+		return;
+repeat:
+	nvmap_page_pool_free(pool, pages_to_release);
+	nvmap_page_pool_lock(pool);
+	available_pages = nvmap_page_pool_get_available_count(pool);
+	if (available_pages > size) {
+		nvmap_page_pool_unlock(pool);
+		pages_to_release = available_pages - size;
+		goto repeat;
+	}
+
+	if (size == 0) {
+		vfree(pool->page_array);
+		vfree(pool->shrink_array);
+		pool->page_array = pool->shrink_array = NULL;
+		goto out;
+	}
+
+	page_array = vmalloc(sizeof(struct page *) * size);
+	shrink_array = vmalloc(sizeof(struct page *) * size);
+	if (!page_array || !shrink_array)
+		goto fail;
+
+	memcpy(page_array, pool->page_array,
+		pool->npages * sizeof(struct page *));
+	vfree(pool->page_array);
+	vfree(pool->shrink_array);
+	pool->page_array = page_array;
+	pool->shrink_array = shrink_array;
+out:
+	pr_debug("%s pool resized to %d from %d pages",
+		s_memtype_str[pool->flags], size, pool->max_pages);
+	pool->max_pages = size;
+	goto exit;
+fail:
+	vfree(page_array);
+	vfree(shrink_array);
+	pr_err("failed");
+exit:
+	nvmap_page_pool_unlock(pool);
+}
+
+static int nvmap_page_pool_shrink(struct shrinker *shrinker,
+				  struct shrink_control *sc)
+{
+	unsigned int i;
+	unsigned int pool_offset;
+	struct nvmap_page_pool *pool;
+	int shrink_pages = sc->nr_to_scan;
+	static atomic_t start_pool = ATOMIC_INIT(-1);
+	struct nvmap_share *share = nvmap_get_share_from_dev(nvmap_dev);
+
+	if (!shrink_pages)
+		goto out;
+
+	pr_debug("sh_pages=%d", shrink_pages);
+
+	for (i = 0; i < NVMAP_NUM_POOLS && shrink_pages; i++) {
+		pool_offset = atomic_add_return(1, &start_pool) %
+				NVMAP_NUM_POOLS;
+		pool = &share->pools[pool_offset];
+		shrink_pages = nvmap_page_pool_free(pool, shrink_pages);
+	}
+out:
+	return nvmap_page_pool_get_unused_pages();
+}
+
+static struct shrinker nvmap_page_pool_shrinker = {
+	.shrink = nvmap_page_pool_shrink,
+	.seeks = 1,
+};
+
+static void shrink_page_pools(int *total_pages, int *available_pages)
+{
+	struct shrink_control sc;
+
+	sc.gfp_mask = GFP_KERNEL;
+	sc.nr_to_scan = 0;
+	*total_pages = nvmap_page_pool_shrink(NULL, &sc);
+	sc.nr_to_scan = *total_pages * 2;
+	*available_pages = nvmap_page_pool_shrink(NULL, &sc);
+}
+
+#if NVMAP_TEST_PAGE_POOL_SHRINKER
+static bool shrink_pp;
+static int shrink_set(const char *arg, const struct kernel_param *kp)
+{
+	int cpu = smp_processor_id();
+	unsigned long long t1, t2;
+	int total_pages, available_pages;
+
+	param_set_bool(arg, kp);
+
+	if (shrink_pp) {
+		t1 = cpu_clock(cpu);
+		shrink_page_pools(&total_pages, &available_pages);
+		t2 = cpu_clock(cpu);
+		pr_info("shrink page pools: time=%lldns, "
+			"total_pages_released=%d, free_pages_available=%d",
+			t2-t1, total_pages, available_pages);
+	}
+	return 0;
+}
+
+static int shrink_get(char *buff, const struct kernel_param *kp)
+{
+	return param_get_bool(buff, kp);
+}
+
+static struct kernel_param_ops shrink_ops = {
+	.get = shrink_get,
+	.set = shrink_set,
+};
+
+module_param_cb(shrink_page_pools, &shrink_ops, &shrink_pp, 0644);
+#endif
+
+static int enable_pp_set(const char *arg, const struct kernel_param *kp)
+{
+	int total_pages, available_pages;
+
+	param_set_bool(arg, kp);
+
+	if (!enable_pp) {
+		shrink_page_pools(&total_pages, &available_pages);
+		pr_info("disabled page pools and released pages, "
+			"total_pages_released=%d, free_pages_available=%d",
+			total_pages, available_pages);
+	}
+	return 0;
+}
+
+static int enable_pp_get(char *buff, const struct kernel_param *kp)
+{
+	return param_get_int(buff, kp);
+}
+
+static struct kernel_param_ops enable_pp_ops = {
+	.get = enable_pp_get,
+	.set = enable_pp_set,
+};
+
+module_param_cb(enable_page_pools, &enable_pp_ops, &enable_pp, 0644);
+
+#define POOL_SIZE_SET(m, i) \
+static int pool_size_##m##_set(const char *arg, const struct kernel_param *kp) \
+{ \
+	struct nvmap_share *share = nvmap_get_share_from_dev(nvmap_dev); \
+	param_set_int(arg, kp); \
+	nvmap_page_pool_resize(&share->pools[i], pool_size[i]); \
+	return 0; \
+}
+
+#define POOL_SIZE_GET(m) \
+static int pool_size_##m##_get(char *buff, const struct kernel_param *kp) \
+{ \
+	return param_get_int(buff, kp); \
+}
+
+#define POOL_SIZE_OPS(m) \
+static struct kernel_param_ops pool_size_##m##_ops = { \
+	.get = pool_size_##m##_get, \
+	.set = pool_size_##m##_set, \
+};
+
+#define POOL_SIZE_MOUDLE_PARAM_CB(m, i) \
+module_param_cb(m##_pool_size, &pool_size_##m##_ops, &pool_size[i], 0644)
+
+POOL_SIZE_SET(uc, NVMAP_HANDLE_UNCACHEABLE);
+POOL_SIZE_GET(uc);
+POOL_SIZE_OPS(uc);
+POOL_SIZE_MOUDLE_PARAM_CB(uc, NVMAP_HANDLE_UNCACHEABLE);
+
+POOL_SIZE_SET(wc, NVMAP_HANDLE_WRITE_COMBINE);
+POOL_SIZE_GET(wc);
+POOL_SIZE_OPS(wc);
+POOL_SIZE_MOUDLE_PARAM_CB(wc, NVMAP_HANDLE_WRITE_COMBINE);
+
+POOL_SIZE_SET(iwb, NVMAP_HANDLE_INNER_CACHEABLE);
+POOL_SIZE_GET(iwb);
+POOL_SIZE_OPS(iwb);
+POOL_SIZE_MOUDLE_PARAM_CB(iwb, NVMAP_HANDLE_INNER_CACHEABLE);
+
+POOL_SIZE_SET(wb, NVMAP_HANDLE_CACHEABLE);
+POOL_SIZE_GET(wb);
+POOL_SIZE_OPS(wb);
+POOL_SIZE_MOUDLE_PARAM_CB(wb, NVMAP_HANDLE_CACHEABLE);
+
+int nvmap_page_pool_init(struct nvmap_page_pool *pool, int flags)
+{
+	int i;
+	int err;
+	struct page *page;
+	static int reg = 1;
+	struct sysinfo info;
+	int highmem_pages = 0;
+	typedef int (*set_pages_array) (struct page **pages, int addrinarray);
+	set_pages_array s_cpa[] = {
+		set_pages_array_uc,
+		set_pages_array_wc,
+		set_pages_array_iwb,
+		set_pages_array_wb
+	};
+
+	BUG_ON(flags >= NVMAP_NUM_POOLS);
+	memset(pool, 0x0, sizeof(*pool));
+	mutex_init(&pool->lock);
+	pool->flags = flags;
+
+	/* No default pool for cached memory. */
+	if (flags == NVMAP_HANDLE_CACHEABLE)
+		return 0;
+
+	si_meminfo(&info);
+	if (!pool_size[flags] && !CONFIG_NVMAP_PAGE_POOL_SIZE)
+		/* Use 3/8th of total ram for page pools.
+		 * 1/8th for uc, 1/8th for wc and 1/8th for iwb.
+		 */
+		pool->max_pages = info.totalram >> 3;
+	else
+		pool->max_pages = CONFIG_NVMAP_PAGE_POOL_SIZE;
+
+	if (pool->max_pages <= 0 || pool->max_pages >= info.totalram)
+		goto fail;
+	pool_size[flags] = pool->max_pages;
+	pr_info("nvmap %s page pool size=%d pages",
+		s_memtype_str[flags], pool->max_pages);
+	pool->page_array = vmalloc(sizeof(void *) * pool->max_pages);
+	pool->shrink_array = vmalloc(sizeof(struct page *) * pool->max_pages);
+	if (!pool->page_array || !pool->shrink_array)
+		goto fail;
+
+	if (reg) {
+		reg = 0;
+		register_shrinker(&nvmap_page_pool_shrinker);
+	}
+
+	nvmap_page_pool_lock(pool);
+	for (i = 0; i < pool->max_pages; i++) {
+		page = alloc_page(GFP_NVMAP);
+		if (!page)
+			goto do_cpa;
+		if (!nvmap_page_pool_release_locked(pool, page)) {
+			__free_page(page);
+			goto do_cpa;
+		}
+		if (PageHighMem(page))
+			highmem_pages++;
+	}
+	si_meminfo(&info);
+	pr_info("nvmap pool = %s, highmem=%d, pool_size=%d,"
+		"totalram=%lu, freeram=%lu, totalhigh=%lu, freehigh=%lu",
+		s_memtype_str[flags], highmem_pages, pool->max_pages,
+		info.totalram, info.freeram, info.totalhigh, info.freehigh);
+do_cpa:
+	err = (*s_cpa[flags])(pool->page_array, pool->npages);
+	BUG_ON(err);
+	nvmap_page_pool_unlock(pool);
+	return 0;
+fail:
+	pool->max_pages = 0;
+	vfree(pool->shrink_array);
+	vfree(pool->page_array);
+	return -ENOMEM;
+}
+#endif
+
+static inline void *altalloc(size_t len)
+{
+	if (len > PAGELIST_VMALLOC_MIN)
+		return vmalloc(len);
+	else
+		return kmalloc(len, GFP_KERNEL);
+}
+
+static inline void altfree(void *ptr, size_t len)
+{
+	if (!ptr)
+		return;
+
+	if (len > PAGELIST_VMALLOC_MIN)
+		vfree(ptr);
+	else
+		kfree(ptr);
+}
+
+void _nvmap_handle_free(struct nvmap_handle *h)
+{
+	int err;
+	struct nvmap_share *share = nvmap_get_share_from_dev(h->dev);
+	unsigned int i, nr_page, page_index = 0;
+#ifdef CONFIG_NVMAP_PAGE_POOLS
+	struct nvmap_page_pool *pool = NULL;
+#endif
+
+	if (nvmap_handle_remove(h->dev, h) != 0)
+		return;
+
+	if (!h->alloc)
+		goto out;
+
+	if (!h->heap_pgalloc) {
+		nvmap_usecount_inc(h);
+		nvmap_heap_free(h->carveout);
+		goto out;
+	}
+
+	nr_page = DIV_ROUND_UP(h->size, PAGE_SIZE);
+
+	BUG_ON(h->size & ~PAGE_MASK);
+	BUG_ON(!h->pgalloc.pages);
+
+	nvmap_mru_remove(share, h);
+
+#ifdef CONFIG_NVMAP_PAGE_POOLS
+	if (h->flags < NVMAP_NUM_POOLS)
+		pool = &share->pools[h->flags];
+
+	while (page_index < nr_page) {
+		if (!nvmap_page_pool_release(pool,
+		    h->pgalloc.pages[page_index]))
+			break;
+		page_index++;
+	}
+#endif
+
+	if (page_index == nr_page)
+		goto skip_attr_restore;
+
+	/* Restore page attributes. */
+	if (h->flags == NVMAP_HANDLE_WRITE_COMBINE ||
+	    h->flags == NVMAP_HANDLE_UNCACHEABLE ||
+	    h->flags == NVMAP_HANDLE_INNER_CACHEABLE) {
+		/* This op should never fail. */
+		err = set_pages_array_wb(&h->pgalloc.pages[page_index],
+				nr_page - page_index);
+		BUG_ON(err);
+	}
+
+skip_attr_restore:
+	if (h->pgalloc.area)
+		tegra_iovmm_free_vm(h->pgalloc.area);
+
+	for (i = page_index; i < nr_page; i++)
+		__free_page(h->pgalloc.pages[i]);
+
+	altfree(h->pgalloc.pages, nr_page * sizeof(struct page *));
+
+out:
+	kfree(h);
+}
+
+static struct page *nvmap_alloc_pages_exact(gfp_t gfp, size_t size)
+{
+	struct page *page, *p, *e;
+	unsigned int order;
+
+	size = PAGE_ALIGN(size);
+	order = get_order(size);
+	page = alloc_pages(gfp, order);
+
+	if (!page)
+		return NULL;
+
+	split_page(page, order);
+	e = page + (1 << order);
+	for (p = page + (size >> PAGE_SHIFT); p < e; p++)
+		__free_page(p);
+
+	return page;
+}
+
+static int handle_page_alloc(struct nvmap_client *client,
+			     struct nvmap_handle *h, bool contiguous)
+{
+	int err = 0;
+	size_t size = PAGE_ALIGN(h->size);
+	unsigned int nr_page = size >> PAGE_SHIFT;
+	pgprot_t prot;
+	unsigned int i = 0, page_index = 0;
+	struct page **pages;
+#ifdef CONFIG_NVMAP_PAGE_POOLS
+	struct nvmap_page_pool *pool = NULL;
+	struct nvmap_share *share = nvmap_get_share_from_dev(h->dev);
+	unsigned long paddr;
+#endif
+	gfp_t gfp = GFP_NVMAP;
+	unsigned long kaddr;
+	pte_t **pte = NULL;
+
+	if (h->userflags & NVMAP_HANDLE_ZEROED_PAGES) {
+		gfp |= __GFP_ZERO;
+		prot = nvmap_pgprot(h, pgprot_kernel);
+		pte = nvmap_alloc_pte(client->dev, (void **)&kaddr);
+		if (IS_ERR(pte))
+			return -ENOMEM;
+	}
+
+	pages = altalloc(nr_page * sizeof(*pages));
+	if (!pages)
+		return -ENOMEM;
+
+	prot = nvmap_pgprot(h, pgprot_kernel);
+
+	h->pgalloc.area = NULL;
+	if (contiguous) {
+		struct page *page;
+		page = nvmap_alloc_pages_exact(gfp, size);
+		if (!page)
+			goto fail;
+
+		for (i = 0; i < nr_page; i++)
+			pages[i] = nth_page(page, i);
+
+	} else {
+#ifdef CONFIG_NVMAP_PAGE_POOLS
+		if (h->flags < NVMAP_NUM_POOLS)
+			pool = &share->pools[h->flags];
+
+		for (i = 0; i < nr_page; i++) {
+			/* Get pages from pool, if available. */
+			pages[i] = nvmap_page_pool_alloc(pool);
+			if (!pages[i])
+				break;
+			if (h->userflags & NVMAP_HANDLE_ZEROED_PAGES) {
+				/*
+				 * Just memset low mem pages; they will for
+				 * sure have a virtual address. Otherwise, build
+				 * a mapping for the page in the kernel.
+				 */
+				if (!PageHighMem(pages[i])) {
+					memset(page_address(pages[i]), 0,
+					       PAGE_SIZE);
+				} else {
+					paddr = page_to_phys(pages[i]);
+					set_pte_at(&init_mm, kaddr, *pte,
+						   pfn_pte(__phys_to_pfn(paddr),
+							   prot));
+					flush_tlb_kernel_page(kaddr);
+					memset((char *)kaddr, 0, PAGE_SIZE);
+				}
+			}
+			page_index++;
+		}
+#endif
+		for (; i < nr_page; i++) {
+			pages[i] = nvmap_alloc_pages_exact(gfp,	PAGE_SIZE);
+			if (!pages[i])
+				goto fail;
+		}
+
+#ifndef CONFIG_NVMAP_RECLAIM_UNPINNED_VM
+		h->pgalloc.area = tegra_iovmm_create_vm(client->share->iovmm,
+					NULL, size, h->align, prot,
+					h->pgalloc.iovm_addr);
+		if (!h->pgalloc.area)
+			goto fail;
+
+		h->pgalloc.dirty = true;
+#endif
+	}
+
+	if (nr_page == page_index)
+		goto skip_attr_change;
+
+	/* Update the pages mapping in kernel page table. */
+	if (h->flags == NVMAP_HANDLE_WRITE_COMBINE)
+		err = set_pages_array_wc(&pages[page_index],
+					nr_page - page_index);
+	else if (h->flags == NVMAP_HANDLE_UNCACHEABLE)
+		err = set_pages_array_uc(&pages[page_index],
+					nr_page - page_index);
+	else if (h->flags == NVMAP_HANDLE_INNER_CACHEABLE)
+		err = set_pages_array_iwb(&pages[page_index],
+					nr_page - page_index);
+
+	if (err)
+		goto fail;
+
+skip_attr_change:
+	if (h->userflags & NVMAP_HANDLE_ZEROED_PAGES)
+		nvmap_free_pte(client->dev, pte);
+	h->size = size;
+	h->pgalloc.pages = pages;
+	h->pgalloc.contig = contiguous;
+	INIT_LIST_HEAD(&h->pgalloc.mru_list);
+	return 0;
+
+fail:
+	if (h->userflags & NVMAP_HANDLE_ZEROED_PAGES)
+		nvmap_free_pte(client->dev, pte);
+	err = set_pages_array_wb(pages, i);
+	BUG_ON(err);
+	while (i--)
+		__free_page(pages[i]);
+	altfree(pages, nr_page * sizeof(*pages));
+	wmb();
+	return -ENOMEM;
+}
+
+static void alloc_handle(struct nvmap_client *client,
+			 struct nvmap_handle *h, unsigned int type)
+{
+	unsigned int carveout_mask = NVMAP_HEAP_CARVEOUT_MASK;
+	unsigned int iovmm_mask = NVMAP_HEAP_IOVMM;
+
+	BUG_ON(type & (type - 1));
+
+#ifdef CONFIG_NVMAP_CONVERT_CARVEOUT_TO_IOVMM
+	/* Convert generic carveout requests to iovmm requests. */
+	carveout_mask &= ~NVMAP_HEAP_CARVEOUT_GENERIC;
+	iovmm_mask |= NVMAP_HEAP_CARVEOUT_GENERIC;
+#endif
+
+	if (type & carveout_mask) {
+		struct nvmap_heap_block *b;
+		/* Protect handle from relocation */
+		nvmap_usecount_inc(h);
+
+		b = nvmap_carveout_alloc(client, h, type);
+		if (b) {
+			h->heap_pgalloc = false;
+			h->alloc = true;
+			nvmap_carveout_commit_add(client,
+				nvmap_heap_to_arg(nvmap_block_to_heap(b)),
+				h->size);
+		}
+		nvmap_usecount_dec(h);
+
+	} else if (type & iovmm_mask) {
+		size_t reserved = PAGE_ALIGN(h->size);
+		int commit = 0;
+		int ret;
+
+		/* increment the committed IOVM space prior to allocation
+		 * to avoid race conditions with other threads simultaneously
+		 * allocating. */
+		commit = atomic_add_return(reserved,
+					    &client->iovm_commit);
+
+		if (commit < client->iovm_limit)
+			ret = handle_page_alloc(client, h, false);
+		else
+			ret = -ENOMEM;
+
+		if (!ret) {
+			h->heap_pgalloc = true;
+			h->alloc = true;
+		} else {
+			atomic_sub(reserved, &client->iovm_commit);
+		}
+
+	} else if (type & NVMAP_HEAP_SYSMEM) {
+		if (handle_page_alloc(client, h, true) == 0) {
+			BUG_ON(!h->pgalloc.contig);
+			h->heap_pgalloc = true;
+			h->alloc = true;
+		}
+	}
+}
+
+/* small allocations will try to allocate from generic OS memory before
+ * any of the limited heaps, to increase the effective memory for graphics
+ * allocations, and to reduce fragmentation of the graphics heaps with
+ * sub-page splinters */
+static const unsigned int heap_policy_small[] = {
+	NVMAP_HEAP_CARVEOUT_VPR,
+	NVMAP_HEAP_CARVEOUT_IRAM,
+#ifdef CONFIG_NVMAP_ALLOW_SYSMEM
+	NVMAP_HEAP_SYSMEM,
+#endif
+	NVMAP_HEAP_CARVEOUT_MASK,
+	NVMAP_HEAP_IOVMM,
+	0,
+};
+
+static const unsigned int heap_policy_large[] = {
+	NVMAP_HEAP_CARVEOUT_VPR,
+	NVMAP_HEAP_CARVEOUT_IRAM,
+	NVMAP_HEAP_IOVMM,
+	NVMAP_HEAP_CARVEOUT_MASK,
+#ifdef CONFIG_NVMAP_ALLOW_SYSMEM
+	NVMAP_HEAP_SYSMEM,
+#endif
+	0,
+};
+
+int nvmap_alloc_handle_id(struct nvmap_client *client,
+			  unsigned long id, unsigned int heap_mask,
+			  size_t align, unsigned int flags)
+{
+	struct nvmap_handle *h = NULL;
+	const unsigned int *alloc_policy;
+	int nr_page;
+	int err = -ENOMEM;
+
+	h = nvmap_get_handle_id(client, id);
+
+	if (!h)
+		return -EINVAL;
+
+	if (h->alloc)
+		goto out;
+
+	trace_nvmap_alloc_handle_id(client, id, heap_mask, align, flags);
+	h->userflags = flags;
+	nr_page = ((h->size + PAGE_SIZE - 1) >> PAGE_SHIFT);
+	h->secure = !!(flags & NVMAP_HANDLE_SECURE);
+	h->flags = (flags & NVMAP_HANDLE_CACHE_FLAG);
+	h->align = max_t(size_t, align, L1_CACHE_BYTES);
+
+#ifndef CONFIG_TEGRA_IOVMM
+	/* convert iovmm requests to generic carveout. */
+	if (heap_mask & NVMAP_HEAP_IOVMM) {
+		heap_mask = (heap_mask & ~NVMAP_HEAP_IOVMM) |
+			    NVMAP_HEAP_CARVEOUT_GENERIC;
+	}
+#endif
+#ifdef CONFIG_NVMAP_ALLOW_SYSMEM
+	/* Allow single pages allocations in system memory to save
+	 * carveout space and avoid extra iovm mappings */
+	if (nr_page == 1) {
+		if (heap_mask &
+		    (NVMAP_HEAP_IOVMM | NVMAP_HEAP_CARVEOUT_GENERIC))
+			heap_mask |= NVMAP_HEAP_SYSMEM;
+	}
+#endif
+#ifndef CONFIG_NVMAP_CONVERT_CARVEOUT_TO_IOVMM
+	/* This restriction is deprecated as alignments greater than
+	   PAGE_SIZE are now correctly handled, but it is retained for
+	   AP20 compatibility. */
+	if (h->align > PAGE_SIZE)
+		heap_mask &= NVMAP_HEAP_CARVEOUT_MASK;
+#endif
+	/* secure allocations can only be served from secure heaps */
+	if (h->secure)
+		heap_mask &= NVMAP_SECURE_HEAPS;
+
+	if (!heap_mask) {
+		err = -EINVAL;
+		goto out;
+	}
+
+	alloc_policy = (nr_page == 1) ? heap_policy_small : heap_policy_large;
+
+	while (!h->alloc && *alloc_policy) {
+		unsigned int heap_type;
+
+		heap_type = *alloc_policy++;
+		heap_type &= heap_mask;
+
+		if (!heap_type)
+			continue;
+
+		heap_mask &= ~heap_type;
+
+		while (heap_type && !h->alloc) {
+			unsigned int heap;
+
+			/* iterate possible heaps MSB-to-LSB, since higher-
+			 * priority carveouts will have higher usage masks */
+			heap = 1 << __fls(heap_type);
+			alloc_handle(client, h, heap);
+			heap_type &= ~heap;
+		}
+	}
+
+out:
+	err = (h->alloc) ? 0 : err;
+	nvmap_handle_put(h);
+	return err;
+}
+
+void nvmap_free_handle_id(struct nvmap_client *client, unsigned long id)
+{
+	struct nvmap_handle_ref *ref;
+	struct nvmap_handle *h;
+	int pins;
+
+	nvmap_ref_lock(client);
+
+	ref = _nvmap_validate_id_locked(client, id);
+	if (!ref) {
+		nvmap_ref_unlock(client);
+		return;
+	}
+
+	trace_nvmap_free_handle_id(client, id);
+	BUG_ON(!ref->handle);
+	h = ref->handle;
+
+	if (atomic_dec_return(&ref->dupes)) {
+		nvmap_ref_unlock(client);
+		goto out;
+	}
+
+	smp_rmb();
+	pins = atomic_read(&ref->pin);
+	rb_erase(&ref->node, &client->handle_refs);
+
+	if (h->alloc && h->heap_pgalloc && !h->pgalloc.contig)
+		atomic_sub(h->size, &client->iovm_commit);
+
+	if (h->alloc && !h->heap_pgalloc) {
+		mutex_lock(&h->lock);
+		nvmap_carveout_commit_subtract(client,
+			nvmap_heap_to_arg(nvmap_block_to_heap(h->carveout)),
+			h->size);
+		mutex_unlock(&h->lock);
+	}
+
+	nvmap_ref_unlock(client);
+
+	if (pins)
+		nvmap_err(client, "%s freeing pinned handle %p\n",
+			  current->group_leader->comm, h);
+
+	while (pins-- > 0)
+		nvmap_unpin_handles(client, &ref->handle, 1);
+
+	if (h->owner == client)
+		h->owner = NULL;
+
+	kfree(ref);
+
+out:
+	BUG_ON(!atomic_read(&h->ref));
+	nvmap_handle_put(h);
+}
+
+static void add_handle_ref(struct nvmap_client *client,
+			   struct nvmap_handle_ref *ref)
+{
+	struct rb_node **p, *parent = NULL;
+
+	nvmap_ref_lock(client);
+	p = &client->handle_refs.rb_node;
+	while (*p) {
+		struct nvmap_handle_ref *node;
+		parent = *p;
+		node = rb_entry(parent, struct nvmap_handle_ref, node);
+		if (ref->handle > node->handle)
+			p = &parent->rb_right;
+		else
+			p = &parent->rb_left;
+	}
+	rb_link_node(&ref->node, parent, p);
+	rb_insert_color(&ref->node, &client->handle_refs);
+	nvmap_ref_unlock(client);
+}
+
+struct nvmap_handle_ref *nvmap_create_handle(struct nvmap_client *client,
+					     size_t size)
+{
+	struct nvmap_handle *h;
+	struct nvmap_handle_ref *ref = NULL;
+
+	if (!client)
+		return ERR_PTR(-EINVAL);
+
+	if (!size)
+		return ERR_PTR(-EINVAL);
+
+	h = kzalloc(sizeof(*h), GFP_KERNEL);
+	if (!h)
+		return ERR_PTR(-ENOMEM);
+
+	ref = kzalloc(sizeof(*ref), GFP_KERNEL);
+	if (!ref) {
+		kfree(h);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	atomic_set(&h->ref, 1);
+	atomic_set(&h->pin, 0);
+	h->owner = client;
+	h->dev = client->dev;
+	BUG_ON(!h->owner);
+	h->size = h->orig_size = size;
+	h->flags = NVMAP_HANDLE_WRITE_COMBINE;
+	mutex_init(&h->lock);
+
+	nvmap_handle_add(client->dev, h);
+
+	atomic_set(&ref->dupes, 1);
+	ref->handle = h;
+	atomic_set(&ref->pin, 0);
+	add_handle_ref(client, ref);
+	trace_nvmap_create_handle(client, h, size, ref);
+	return ref;
+}
+
+struct nvmap_handle_ref *nvmap_duplicate_handle_id(struct nvmap_client *client,
+						   unsigned long id)
+{
+	struct nvmap_handle_ref *ref = NULL;
+	struct nvmap_handle *h = NULL;
+
+	BUG_ON(!client || client->dev != nvmap_dev);
+	/* on success, the reference count for the handle should be
+	 * incremented, so the success paths will not call nvmap_handle_put */
+
+	/* Allow the handle to be accessed by other (non-owner)
+	 clients only if the owner is "videobuf2-dma-nvmap
+	 which is a V4L2 capture kernel module. This handle can
+	 be accessed by the "user" client for rendering/encoding */
+	if (id && ((struct nvmap_handle *)id)->owner &&
+	    !strcmp(((struct nvmap_handle *)id)->owner->name,
+	            "videobuf2-dma-nvmap"))
+		client = ((struct nvmap_handle *)id)->owner;
+
+	h = nvmap_validate_get(client, id);
+
+	if (!h) {
+		nvmap_debug(client, "%s duplicate handle failed\n",
+			    current->group_leader->comm);
+		return ERR_PTR(-EPERM);
+	}
+
+	if (!h->alloc) {
+		nvmap_err(client, "%s duplicating unallocated handle\n",
+			  current->group_leader->comm);
+		nvmap_handle_put(h);
+		return ERR_PTR(-EINVAL);
+	}
+
+	nvmap_ref_lock(client);
+	ref = _nvmap_validate_id_locked(client, (unsigned long)h);
+
+	if (ref) {
+		/* handle already duplicated in client; just increment
+		 * the reference count rather than re-duplicating it */
+		atomic_inc(&ref->dupes);
+		nvmap_ref_unlock(client);
+		return ref;
+	}
+
+	nvmap_ref_unlock(client);
+
+	/* verify that adding this handle to the process' access list
+	 * won't exceed the IOVM limit */
+	if (h->heap_pgalloc && !h->pgalloc.contig) {
+		int oc;
+		oc = atomic_add_return(h->size, &client->iovm_commit);
+		if (oc > client->iovm_limit && !client->super) {
+			atomic_sub(h->size, &client->iovm_commit);
+			nvmap_handle_put(h);
+			nvmap_err(client, "duplicating %p in %s over-commits"
+				  " IOVMM space\n", (void *)id,
+				  current->group_leader->comm);
+			return ERR_PTR(-ENOMEM);
+		}
+	}
+
+	ref = kzalloc(sizeof(*ref), GFP_KERNEL);
+	if (!ref) {
+		nvmap_handle_put(h);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	if (!h->heap_pgalloc) {
+		mutex_lock(&h->lock);
+		nvmap_carveout_commit_add(client,
+			nvmap_heap_to_arg(nvmap_block_to_heap(h->carveout)),
+			h->size);
+		mutex_unlock(&h->lock);
+	}
+
+	atomic_set(&ref->dupes, 1);
+	ref->handle = h;
+	atomic_set(&ref->pin, 0);
+	add_handle_ref(client, ref);
+	trace_nvmap_duplicate_handle_id(client, id, ref);
+	return ref;
+}
diff --git a/drivers/video/tegra/nvmap/nvmap_heap.c b/drivers/video/tegra/nvmap/nvmap_heap.c
new file mode 100644
index 000000000000..738ba26232d3
--- /dev/null
+++ b/drivers/video/tegra/nvmap/nvmap_heap.c
@@ -0,0 +1,1116 @@
+/*
+ * drivers/video/tegra/nvmap/nvmap_heap.c
+ *
+ * GPU heap allocator.
+ *
+ * Copyright (c) 2012, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/mm.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+
+#include <linux/nvmap.h>
+#include "nvmap.h"
+#include "nvmap_heap.h"
+#include "nvmap_common.h"
+
+#include <asm/tlbflush.h>
+#include <asm/cacheflush.h>
+
+/*
+ * "carveouts" are platform-defined regions of physically contiguous memory
+ * which are not managed by the OS. a platform may specify multiple carveouts,
+ * for either small special-purpose memory regions (like IRAM on Tegra SoCs)
+ * or reserved regions of main system memory.
+ *
+ * the carveout allocator returns allocations which are physically contiguous.
+ * to reduce external fragmentation, the allocation algorithm implemented in
+ * this file employs 3 strategies for keeping allocations of similar size
+ * grouped together inside the larger heap: the "small", "normal" and "huge"
+ * strategies. the size thresholds (in bytes) for determining which strategy
+ * to employ should be provided by the platform for each heap. it is possible
+ * for a platform to define a heap where only the "normal" strategy is used.
+ *
+ * o "normal" allocations use an address-order first-fit allocator (called
+ *   BOTTOM_UP in the code below). each allocation is rounded up to be
+ *   an integer multiple of the "small" allocation size.
+ *
+ * o "huge" allocations use an address-order last-fit allocator (called
+ *   TOP_DOWN in the code below). like "normal" allocations, each allocation
+ *   is rounded up to be an integer multiple of the "small" allocation size.
+ *
+ * o "small" allocations are treated differently: the heap manager maintains
+ *   a pool of "small"-sized blocks internally from which allocations less
+ *   than 1/2 of the "small" size are buddy-allocated. if a "small" allocation
+ *   is requested and none of the buddy sub-heaps is able to service it,
+ *   the heap manager will try to allocate a new buddy-heap.
+ *
+ * this allocator is intended to keep "splinters" colocated in the carveout,
+ * and to ensure that the minimum free block size in the carveout (i.e., the
+ * "small" threshold) is still a meaningful size.
+ *
+ */
+
+#define MAX_BUDDY_NR	128	/* maximum buddies in a buddy allocator */
+
+enum direction {
+	TOP_DOWN,
+	BOTTOM_UP
+};
+
+enum block_type {
+	BLOCK_FIRST_FIT,	/* block was allocated directly from the heap */
+	BLOCK_BUDDY,		/* block was allocated from a buddy sub-heap */
+	BLOCK_EMPTY,
+};
+
+struct heap_stat {
+	size_t free;		/* total free size */
+	size_t free_largest;	/* largest free block */
+	size_t free_count;	/* number of free blocks */
+	size_t total;		/* total size */
+	size_t largest;		/* largest unique block */
+	size_t count;		/* total number of blocks */
+	/* fast compaction attempt counter */
+	unsigned int compaction_count_fast;
+	/* full compaction attempt counter */
+	unsigned int compaction_count_full;
+};
+
+struct buddy_heap;
+
+struct buddy_block {
+	struct nvmap_heap_block block;
+	struct buddy_heap *heap;
+};
+
+struct list_block {
+	struct nvmap_heap_block block;
+	struct list_head all_list;
+	unsigned int mem_prot;
+	unsigned long orig_addr;
+	size_t size;
+	size_t align;
+	struct nvmap_heap *heap;
+	struct list_head free_list;
+};
+
+struct combo_block {
+	union {
+		struct list_block lb;
+		struct buddy_block bb;
+	};
+};
+
+struct buddy_bits {
+	unsigned int alloc:1;
+	unsigned int order:7;	/* log2(MAX_BUDDY_NR); */
+};
+
+struct buddy_heap {
+	struct list_block *heap_base;
+	unsigned int nr_buddies;
+	struct list_head buddy_list;
+	struct buddy_bits bitmap[MAX_BUDDY_NR];
+};
+
+struct nvmap_heap {
+	struct list_head all_list;
+	struct list_head free_list;
+	struct mutex lock;
+	struct list_head buddy_list;
+	unsigned int min_buddy_shift;
+	unsigned int buddy_heap_size;
+	unsigned int small_alloc;
+	const char *name;
+	void *arg;
+	struct device dev;
+};
+
+static struct kmem_cache *buddy_heap_cache;
+static struct kmem_cache *block_cache;
+
+static inline struct nvmap_heap *parent_of(struct buddy_heap *heap)
+{
+	return heap->heap_base->heap;
+}
+
+static inline unsigned int order_of(size_t len, size_t min_shift)
+{
+	len = 2 * DIV_ROUND_UP(len, (1 << min_shift)) - 1;
+	return fls(len)-1;
+}
+
+/* returns the free size in bytes of the buddy heap; must be called while
+ * holding the parent heap's lock. */
+static void buddy_stat(struct buddy_heap *heap, struct heap_stat *stat)
+{
+	unsigned int index;
+	unsigned int shift = parent_of(heap)->min_buddy_shift;
+
+	for (index = 0; index < heap->nr_buddies;
+	     index += (1 << heap->bitmap[index].order)) {
+		size_t curr = 1 << (heap->bitmap[index].order + shift);
+
+		stat->largest = max(stat->largest, curr);
+		stat->total += curr;
+		stat->count++;
+
+		if (!heap->bitmap[index].alloc) {
+			stat->free += curr;
+			stat->free_largest = max(stat->free_largest, curr);
+			stat->free_count++;
+		}
+	}
+}
+
+/* returns the free size of the heap (including any free blocks in any
+ * buddy-heap suballocators; must be called while holding the parent
+ * heap's lock. */
+static unsigned long heap_stat(struct nvmap_heap *heap, struct heap_stat *stat)
+{
+	struct buddy_heap *bh;
+	struct list_block *l = NULL;
+	unsigned long base = -1ul;
+
+	memset(stat, 0, sizeof(*stat));
+	mutex_lock(&heap->lock);
+	list_for_each_entry(l, &heap->all_list, all_list) {
+		stat->total += l->size;
+		stat->largest = max(l->size, stat->largest);
+		stat->count++;
+		base = min(base, l->orig_addr);
+	}
+
+	list_for_each_entry(bh, &heap->buddy_list, buddy_list) {
+		buddy_stat(bh, stat);
+		/* the total counts are double-counted for buddy heaps
+		 * since the blocks allocated for buddy heaps exist in the
+		 * all_list; subtract out the doubly-added stats */
+		stat->total -= bh->heap_base->size;
+		stat->count--;
+	}
+
+	list_for_each_entry(l, &heap->free_list, free_list) {
+		stat->free += l->size;
+		stat->free_count++;
+		stat->free_largest = max(l->size, stat->free_largest);
+	}
+	mutex_unlock(&heap->lock);
+
+	return base;
+}
+
+static ssize_t heap_name_show(struct device *dev,
+			      struct device_attribute *attr, char *buf);
+
+static ssize_t heap_stat_show(struct device *dev,
+			      struct device_attribute *attr, char *buf);
+
+static struct device_attribute heap_stat_total_max =
+	__ATTR(total_max, S_IRUGO, heap_stat_show, NULL);
+
+static struct device_attribute heap_stat_total_count =
+	__ATTR(total_count, S_IRUGO, heap_stat_show, NULL);
+
+static struct device_attribute heap_stat_total_size =
+	__ATTR(total_size, S_IRUGO, heap_stat_show, NULL);
+
+static struct device_attribute heap_stat_free_max =
+	__ATTR(free_max, S_IRUGO, heap_stat_show, NULL);
+
+static struct device_attribute heap_stat_free_count =
+	__ATTR(free_count, S_IRUGO, heap_stat_show, NULL);
+
+static struct device_attribute heap_stat_free_size =
+	__ATTR(free_size, S_IRUGO, heap_stat_show, NULL);
+
+static struct device_attribute heap_stat_base =
+	__ATTR(base, S_IRUGO, heap_stat_show, NULL);
+
+static struct device_attribute heap_attr_name =
+	__ATTR(name, S_IRUGO, heap_name_show, NULL);
+
+static struct attribute *heap_stat_attrs[] = {
+	&heap_stat_total_max.attr,
+	&heap_stat_total_count.attr,
+	&heap_stat_total_size.attr,
+	&heap_stat_free_max.attr,
+	&heap_stat_free_count.attr,
+	&heap_stat_free_size.attr,
+	&heap_stat_base.attr,
+	&heap_attr_name.attr,
+	NULL,
+};
+
+static struct attribute_group heap_stat_attr_group = {
+	.attrs	= heap_stat_attrs,
+};
+
+static ssize_t heap_name_show(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+
+	struct nvmap_heap *heap = container_of(dev, struct nvmap_heap, dev);
+	return sprintf(buf, "%s\n", heap->name);
+}
+
+static ssize_t heap_stat_show(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	struct nvmap_heap *heap = container_of(dev, struct nvmap_heap, dev);
+	struct heap_stat stat;
+	unsigned long base;
+
+	base = heap_stat(heap, &stat);
+
+	if (attr == &heap_stat_total_max)
+		return sprintf(buf, "%u\n", stat.largest);
+	else if (attr == &heap_stat_total_count)
+		return sprintf(buf, "%u\n", stat.count);
+	else if (attr == &heap_stat_total_size)
+		return sprintf(buf, "%u\n", stat.total);
+	else if (attr == &heap_stat_free_max)
+		return sprintf(buf, "%u\n", stat.free_largest);
+	else if (attr == &heap_stat_free_count)
+		return sprintf(buf, "%u\n", stat.free_count);
+	else if (attr == &heap_stat_free_size)
+		return sprintf(buf, "%u\n", stat.free);
+	else if (attr == &heap_stat_base)
+		return sprintf(buf, "%08lx\n", base);
+	else
+		return -EINVAL;
+}
+#ifndef CONFIG_NVMAP_CARVEOUT_COMPACTOR
+static struct nvmap_heap_block *buddy_alloc(struct buddy_heap *heap,
+					    size_t size, size_t align,
+					    unsigned int mem_prot)
+{
+	unsigned int index = 0;
+	unsigned int min_shift = parent_of(heap)->min_buddy_shift;
+	unsigned int order = order_of(size, min_shift);
+	unsigned int align_mask;
+	unsigned int best = heap->nr_buddies;
+	struct buddy_block *b;
+
+	if (heap->heap_base->mem_prot != mem_prot)
+		return NULL;
+
+	align = max(align, (size_t)(1 << min_shift));
+	align_mask = (align >> min_shift) - 1;
+
+	for (index = 0; index < heap->nr_buddies;
+	     index += (1 << heap->bitmap[index].order)) {
+
+		if (heap->bitmap[index].alloc || (index & align_mask) ||
+		    (heap->bitmap[index].order < order))
+			continue;
+
+		if (best == heap->nr_buddies ||
+		    heap->bitmap[index].order < heap->bitmap[best].order)
+			best = index;
+
+		if (heap->bitmap[best].order == order)
+			break;
+	}
+
+	if (best == heap->nr_buddies)
+		return NULL;
+
+	b = kmem_cache_zalloc(block_cache, GFP_KERNEL);
+	if (!b)
+		return NULL;
+
+	while (heap->bitmap[best].order != order) {
+		unsigned int buddy;
+		heap->bitmap[best].order--;
+		buddy = best ^ (1 << heap->bitmap[best].order);
+		heap->bitmap[buddy].order = heap->bitmap[best].order;
+		heap->bitmap[buddy].alloc = 0;
+	}
+	heap->bitmap[best].alloc = 1;
+	b->block.base = heap->heap_base->block.base + (best << min_shift);
+	b->heap = heap;
+	b->block.type = BLOCK_BUDDY;
+	return &b->block;
+}
+#endif
+
+static struct buddy_heap *do_buddy_free(struct nvmap_heap_block *block)
+{
+	struct buddy_block *b = container_of(block, struct buddy_block, block);
+	struct buddy_heap *h = b->heap;
+	unsigned int min_shift = parent_of(h)->min_buddy_shift;
+	unsigned int index;
+
+	index = (block->base - h->heap_base->block.base) >> min_shift;
+	h->bitmap[index].alloc = 0;
+
+	for (;;) {
+		unsigned int buddy = index ^ (1 << h->bitmap[index].order);
+		if (buddy >= h->nr_buddies || h->bitmap[buddy].alloc ||
+		    h->bitmap[buddy].order != h->bitmap[index].order)
+			break;
+
+		h->bitmap[buddy].order++;
+		h->bitmap[index].order++;
+		index = min(buddy, index);
+	}
+
+	kmem_cache_free(block_cache, b);
+	if ((1 << h->bitmap[0].order) == h->nr_buddies)
+		return h;
+
+	return NULL;
+}
+
+
+/*
+ * base_max limits position of allocated chunk in memory.
+ * if base_max is 0 then there is no such limitation.
+ */
+static struct nvmap_heap_block *do_heap_alloc(struct nvmap_heap *heap,
+					      size_t len, size_t align,
+					      unsigned int mem_prot,
+					      unsigned long base_max)
+{
+	struct list_block *b = NULL;
+	struct list_block *i = NULL;
+	struct list_block *rem = NULL;
+	unsigned long fix_base;
+	enum direction dir;
+
+	/* since pages are only mappable with one cache attribute,
+	 * and most allocations from carveout heaps are DMA coherent
+	 * (i.e., non-cacheable), round cacheable allocations up to
+	 * a page boundary to ensure that the physical pages will
+	 * only be mapped one way. */
+	if (mem_prot == NVMAP_HANDLE_CACHEABLE ||
+	    mem_prot == NVMAP_HANDLE_INNER_CACHEABLE) {
+		align = max_t(size_t, align, PAGE_SIZE);
+		len = PAGE_ALIGN(len);
+	}
+
+#ifdef CONFIG_NVMAP_CARVEOUT_COMPACTOR
+	dir = BOTTOM_UP;
+#else
+	dir = (len <= heap->small_alloc) ? BOTTOM_UP : TOP_DOWN;
+#endif
+
+	if (dir == BOTTOM_UP) {
+		list_for_each_entry(i, &heap->free_list, free_list) {
+			size_t fix_size;
+			fix_base = ALIGN(i->block.base, align);
+			if(!fix_base || fix_base >= i->block.base + i->size)
+				continue;
+
+			fix_size = i->size - (fix_base - i->block.base);
+
+			/* needed for compaction. relocated chunk
+			 * should never go up */
+			if (base_max && fix_base > base_max)
+				break;
+
+			if (fix_size >= len) {
+				b = i;
+				break;
+			}
+		}
+	} else {
+		list_for_each_entry_reverse(i, &heap->free_list, free_list) {
+			if (i->size >= len) {
+				fix_base = i->block.base + i->size - len;
+				fix_base &= ~(align-1);
+				if (fix_base >= i->block.base) {
+					b = i;
+					break;
+				}
+			}
+		}
+	}
+
+	if (!b)
+		return NULL;
+
+	if (dir == BOTTOM_UP)
+		b->block.type = BLOCK_FIRST_FIT;
+
+	/* split free block */
+	if (b->block.base != fix_base) {
+		/* insert a new free block before allocated */
+		rem = kmem_cache_zalloc(block_cache, GFP_KERNEL);
+		if (!rem) {
+			b->orig_addr = b->block.base;
+			b->block.base = fix_base;
+			b->size -= (b->block.base - b->orig_addr);
+			goto out;
+		}
+
+		rem->block.type = BLOCK_EMPTY;
+		rem->block.base = b->block.base;
+		rem->orig_addr = rem->block.base;
+		rem->size = fix_base - rem->block.base;
+		b->block.base = fix_base;
+		b->orig_addr = fix_base;
+		b->size -= rem->size;
+		list_add_tail(&rem->all_list,  &b->all_list);
+		list_add_tail(&rem->free_list, &b->free_list);
+	}
+
+	b->orig_addr = b->block.base;
+
+	if (b->size > len) {
+		/* insert a new free block after allocated */
+		rem = kmem_cache_zalloc(block_cache, GFP_KERNEL);
+		if (!rem)
+			goto out;
+
+		rem->block.type = BLOCK_EMPTY;
+		rem->block.base = b->block.base + len;
+		rem->size = b->size - len;
+		BUG_ON(rem->size > b->size);
+		rem->orig_addr = rem->block.base;
+		b->size = len;
+		list_add(&rem->all_list,  &b->all_list);
+		list_add(&rem->free_list, &b->free_list);
+	}
+
+out:
+	list_del(&b->free_list);
+	b->heap = heap;
+	b->mem_prot = mem_prot;
+	b->align = align;
+	return &b->block;
+}
+
+#ifdef DEBUG_FREE_LIST
+static void freelist_debug(struct nvmap_heap *heap, const char *title,
+			   struct list_block *token)
+{
+	int i;
+	struct list_block *n;
+
+	dev_debug(&heap->dev, "%s\n", title);
+	i = 0;
+	list_for_each_entry(n, &heap->free_list, free_list) {
+		dev_debug(&heap->dev, "\t%d [%p..%p]%s\n", i, (void *)n->orig_addr,
+			  (void *)(n->orig_addr + n->size),
+			  (n == token) ? "<--" : "");
+		i++;
+	}
+}
+#else
+#define freelist_debug(_heap, _title, _token)	do { } while (0)
+#endif
+
+static struct list_block *do_heap_free(struct nvmap_heap_block *block)
+{
+	struct list_block *b = container_of(block, struct list_block, block);
+	struct list_block *n = NULL;
+	struct nvmap_heap *heap = b->heap;
+
+	BUG_ON(b->block.base > b->orig_addr);
+	b->size += (b->block.base - b->orig_addr);
+	b->block.base = b->orig_addr;
+
+	freelist_debug(heap, "free list before", b);
+
+	/* Find position of first free block to the right of freed one */
+	list_for_each_entry(n, &heap->free_list, free_list) {
+		if (n->block.base > b->block.base)
+			break;
+	}
+
+	/* Add freed block before found free one */
+	list_add_tail(&b->free_list, &n->free_list);
+	BUG_ON(list_empty(&b->all_list));
+
+	freelist_debug(heap, "free list pre-merge", b);
+
+	/* merge freed block with next if they connect
+	 * freed block becomes bigger, next one is destroyed */
+	if (!list_is_last(&b->free_list, &heap->free_list)) {
+		n = list_first_entry(&b->free_list, struct list_block, free_list);
+		if (n->block.base == b->block.base + b->size) {
+			list_del(&n->all_list);
+			list_del(&n->free_list);
+			BUG_ON(b->orig_addr >= n->orig_addr);
+			b->size += n->size;
+			kmem_cache_free(block_cache, n);
+		}
+	}
+
+	/* merge freed block with prev if they connect
+	 * previous free block becomes bigger, freed one is destroyed */
+	if (b->free_list.prev != &heap->free_list) {
+		n = list_entry(b->free_list.prev, struct list_block, free_list);
+		if (n->block.base + n->size == b->block.base) {
+			list_del(&b->all_list);
+			list_del(&b->free_list);
+			BUG_ON(n->orig_addr >= b->orig_addr);
+			n->size += b->size;
+			kmem_cache_free(block_cache, b);
+			b = n;
+		}
+	}
+
+	freelist_debug(heap, "free list after", b);
+	b->block.type = BLOCK_EMPTY;
+	return b;
+}
+
+#ifndef CONFIG_NVMAP_CARVEOUT_COMPACTOR
+
+static struct nvmap_heap_block *do_buddy_alloc(struct nvmap_heap *h,
+					       size_t len, size_t align,
+					       unsigned int mem_prot)
+{
+	struct buddy_heap *bh;
+	struct nvmap_heap_block *b = NULL;
+
+	list_for_each_entry(bh, &h->buddy_list, buddy_list) {
+		b = buddy_alloc(bh, len, align, mem_prot);
+		if (b)
+			return b;
+	}
+
+	/* no buddy heaps could service this allocation: try to create a new
+	 * buddy heap instead */
+	bh = kmem_cache_zalloc(buddy_heap_cache, GFP_KERNEL);
+	if (!bh)
+		return NULL;
+
+	b = do_heap_alloc(h, h->buddy_heap_size,
+			h->buddy_heap_size, mem_prot, 0);
+	if (!b) {
+		kmem_cache_free(buddy_heap_cache, bh);
+		return NULL;
+	}
+
+	bh->heap_base = container_of(b, struct list_block, block);
+	bh->nr_buddies = h->buddy_heap_size >> h->min_buddy_shift;
+	bh->bitmap[0].alloc = 0;
+	bh->bitmap[0].order = order_of(h->buddy_heap_size, h->min_buddy_shift);
+	list_add_tail(&bh->buddy_list, &h->buddy_list);
+	return buddy_alloc(bh, len, align, mem_prot);
+}
+
+#endif
+
+#ifdef CONFIG_NVMAP_CARVEOUT_COMPACTOR
+
+static int do_heap_copy_listblock(struct nvmap_device *dev,
+		 unsigned long dst_base, unsigned long src_base, size_t len)
+{
+	pte_t **pte_src = NULL;
+	pte_t **pte_dst = NULL;
+	void *addr_src = NULL;
+	void *addr_dst = NULL;
+	unsigned long kaddr_src;
+	unsigned long kaddr_dst;
+	unsigned long phys_src = src_base;
+	unsigned long phys_dst = dst_base;
+	unsigned long pfn_src;
+	unsigned long pfn_dst;
+	int error = 0;
+
+	pgprot_t prot = pgprot_writecombine(pgprot_kernel);
+
+	int page;
+
+	pte_src = nvmap_alloc_pte(dev, &addr_src);
+	if (IS_ERR(pte_src)) {
+		pr_err("Error when allocating pte_src\n");
+		pte_src = NULL;
+		error = -1;
+		goto fail;
+	}
+
+	pte_dst = nvmap_alloc_pte(dev, &addr_dst);
+	if (IS_ERR(pte_dst)) {
+		pr_err("Error while allocating pte_dst\n");
+		pte_dst = NULL;
+		error = -1;
+		goto fail;
+	}
+
+	kaddr_src = (unsigned long)addr_src;
+	kaddr_dst = (unsigned long)addr_dst;
+
+	BUG_ON(phys_dst > phys_src);
+	BUG_ON((phys_src & PAGE_MASK) != phys_src);
+	BUG_ON((phys_dst & PAGE_MASK) != phys_dst);
+	BUG_ON((len & PAGE_MASK) != len);
+
+	for (page = 0; page < (len >> PAGE_SHIFT) ; page++) {
+
+		pfn_src = __phys_to_pfn(phys_src) + page;
+		pfn_dst = __phys_to_pfn(phys_dst) + page;
+
+		set_pte_at(&init_mm, kaddr_src, *pte_src,
+				pfn_pte(pfn_src, prot));
+		flush_tlb_kernel_page(kaddr_src);
+
+		set_pte_at(&init_mm, kaddr_dst, *pte_dst,
+				pfn_pte(pfn_dst, prot));
+		flush_tlb_kernel_page(kaddr_dst);
+
+		memcpy(addr_dst, addr_src, PAGE_SIZE);
+	}
+
+fail:
+	if (pte_src)
+		nvmap_free_pte(dev, pte_src);
+	if (pte_dst)
+		nvmap_free_pte(dev, pte_dst);
+	return error;
+}
+
+
+static struct nvmap_heap_block *do_heap_relocate_listblock(
+		struct list_block *block, bool fast)
+{
+	struct nvmap_heap_block *heap_block = &block->block;
+	struct nvmap_heap_block *heap_block_new = NULL;
+	struct nvmap_heap *heap = block->heap;
+	struct nvmap_handle *handle = heap_block->handle;
+	unsigned long src_base = heap_block->base;
+	unsigned long dst_base;
+	size_t src_size = block->size;
+	size_t src_align = block->align;
+	unsigned int src_prot = block->mem_prot;
+	int error = 0;
+	struct nvmap_share *share;
+
+	if (!handle) {
+		pr_err("INVALID HANDLE!\n");
+		return NULL;
+	}
+
+	mutex_lock(&handle->lock);
+
+	share = nvmap_get_share_from_dev(handle->dev);
+
+	/* TODO: It is possible to use only handle lock and no share
+	 * pin_lock, but then we'll need to lock every handle during
+	 * each pinning operation. Need to estimate performance impact
+	 * if we decide to simplify locking this way. */
+	mutex_lock(&share->pin_lock);
+
+	/* abort if block is pinned */
+	if (atomic_read(&handle->pin))
+		goto fail;
+	/* abort if block is mapped */
+	if (handle->usecount)
+		goto fail;
+
+	if (fast) {
+		/* Fast compaction path - first allocate, then free. */
+		heap_block_new = do_heap_alloc(heap, src_size, src_align,
+				src_prot, src_base);
+		if (heap_block_new)
+			do_heap_free(heap_block);
+		else
+			goto fail;
+	} else {
+		/* Full compaction path, first free, then allocate
+		 * It is slower but provide best compaction results */
+		do_heap_free(heap_block);
+		heap_block_new = do_heap_alloc(heap, src_size, src_align,
+				src_prot, src_base);
+		/* Allocation should always succeed*/
+		BUG_ON(!heap_block_new);
+	}
+
+	/* update handle */
+	handle->carveout = heap_block_new;
+	heap_block_new->handle = handle;
+
+	/* copy source data to new block location */
+	dst_base = heap_block_new->base;
+
+	/* new allocation should always go lower addresses */
+	BUG_ON(dst_base >= src_base);
+
+	error = do_heap_copy_listblock(handle->dev,
+				dst_base, src_base, src_size);
+	BUG_ON(error);
+
+fail:
+	mutex_unlock(&share->pin_lock);
+	mutex_unlock(&handle->lock);
+	return heap_block_new;
+}
+
+static void nvmap_heap_compact(struct nvmap_heap *heap,
+				size_t requested_size, bool fast)
+{
+	struct list_block *block_current = NULL;
+	struct list_block *block_prev = NULL;
+	struct list_block *block_next = NULL;
+
+	struct list_head *ptr, *ptr_prev, *ptr_next;
+	int relocation_count = 0;
+
+	ptr = heap->all_list.next;
+
+	/* walk through all blocks */
+	while (ptr != &heap->all_list) {
+		block_current = list_entry(ptr, struct list_block, all_list);
+
+		ptr_prev = ptr->prev;
+		ptr_next = ptr->next;
+
+		if (block_current->block.type != BLOCK_EMPTY) {
+			ptr = ptr_next;
+			continue;
+		}
+
+		if (fast && block_current->size >= requested_size)
+			break;
+
+		/* relocate prev block */
+		if (ptr_prev != &heap->all_list) {
+
+			block_prev = list_entry(ptr_prev,
+					struct list_block, all_list);
+
+			BUG_ON(block_prev->block.type != BLOCK_FIRST_FIT);
+
+			if (do_heap_relocate_listblock(block_prev, true)) {
+
+				/* After relocation current free block can be
+				 * destroyed when it is merged with previous
+				 * free block. Updated pointer to new free
+				 * block can be obtained from the next block */
+				relocation_count++;
+				ptr = ptr_next->prev;
+				continue;
+			}
+		}
+
+		if (ptr_next != &heap->all_list) {
+
+			block_next = list_entry(ptr_next,
+					struct list_block, all_list);
+
+			BUG_ON(block_next->block.type != BLOCK_FIRST_FIT);
+
+			if (do_heap_relocate_listblock(block_next, fast)) {
+				ptr = ptr_prev->next;
+				relocation_count++;
+				continue;
+			}
+		}
+		ptr = ptr_next;
+	}
+	pr_err("Relocated %d chunks\n", relocation_count);
+}
+#endif
+
+void nvmap_usecount_inc(struct nvmap_handle *h)
+{
+	if (h->alloc && !h->heap_pgalloc) {
+		mutex_lock(&h->lock);
+		h->usecount++;
+		mutex_unlock(&h->lock);
+	} else {
+		h->usecount++;
+	}
+}
+
+
+void nvmap_usecount_dec(struct nvmap_handle *h)
+{
+	h->usecount--;
+}
+
+/* nvmap_heap_alloc: allocates a block of memory of len bytes, aligned to
+ * align bytes. */
+struct nvmap_heap_block *nvmap_heap_alloc(struct nvmap_heap *h,
+					  struct nvmap_handle *handle)
+{
+	struct nvmap_heap_block *b;
+	size_t len        = handle->size;
+	size_t align      = handle->align;
+	unsigned int prot = handle->flags;
+
+	mutex_lock(&h->lock);
+
+#ifdef CONFIG_NVMAP_CARVEOUT_COMPACTOR
+	/* Align to page size */
+	align = ALIGN(align, PAGE_SIZE);
+	len = ALIGN(len, PAGE_SIZE);
+	b = do_heap_alloc(h, len, align, prot, 0);
+	if (!b) {
+		pr_err("Compaction triggered!\n");
+		nvmap_heap_compact(h, len, true);
+		b = do_heap_alloc(h, len, align, prot, 0);
+		if (!b) {
+			pr_err("Full compaction triggered!\n");
+			nvmap_heap_compact(h, len, false);
+			b = do_heap_alloc(h, len, align, prot, 0);
+		}
+	}
+#else
+	if (len <= h->buddy_heap_size / 2) {
+		b = do_buddy_alloc(h, len, align, prot);
+	} else {
+		if (h->buddy_heap_size)
+			len = ALIGN(len, h->buddy_heap_size);
+		align = max(align, (size_t)L1_CACHE_BYTES);
+		b = do_heap_alloc(h, len, align, prot, 0);
+	}
+#endif
+
+	if (b) {
+		b->handle = handle;
+		handle->carveout = b;
+	}
+	mutex_unlock(&h->lock);
+	return b;
+}
+
+struct nvmap_heap *nvmap_block_to_heap(struct nvmap_heap_block *b)
+{
+	if (b->type == BLOCK_BUDDY) {
+		struct buddy_block *bb;
+		bb = container_of(b, struct buddy_block, block);
+		return parent_of(bb->heap);
+	} else {
+		struct list_block *lb;
+		lb = container_of(b, struct list_block, block);
+		return lb->heap;
+	}
+}
+
+/* nvmap_heap_free: frees block b*/
+void nvmap_heap_free(struct nvmap_heap_block *b)
+{
+	struct buddy_heap *bh = NULL;
+	struct nvmap_heap *h = nvmap_block_to_heap(b);
+	struct list_block *lb;
+
+	mutex_lock(&h->lock);
+	if (b->type == BLOCK_BUDDY)
+		bh = do_buddy_free(b);
+	else {
+		lb = container_of(b, struct list_block, block);
+		nvmap_flush_heap_block(NULL, b, lb->size, lb->mem_prot);
+		do_heap_free(b);
+	}
+
+	if (bh) {
+		list_del(&bh->buddy_list);
+		mutex_unlock(&h->lock);
+		nvmap_heap_free(&bh->heap_base->block);
+		kmem_cache_free(buddy_heap_cache, bh);
+	} else
+		mutex_unlock(&h->lock);
+}
+
+
+static void heap_release(struct device *heap)
+{
+}
+
+/* nvmap_heap_create: create a heap object of len bytes, starting from
+ * address base.
+ *
+ * if buddy_size is >= NVMAP_HEAP_MIN_BUDDY_SIZE, then allocations <= 1/2
+ * of the buddy heap size will use a buddy sub-allocator, where each buddy
+ * heap is buddy_size bytes (should be a power of 2). all other allocations
+ * will be rounded up to be a multiple of buddy_size bytes.
+ */
+struct nvmap_heap *nvmap_heap_create(struct device *parent, const char *name,
+				     phys_addr_t base, size_t len,
+				     size_t buddy_size, void *arg)
+{
+	struct nvmap_heap *h = NULL;
+	struct list_block *l = NULL;
+
+	if (WARN_ON(buddy_size && buddy_size < NVMAP_HEAP_MIN_BUDDY_SIZE)) {
+		dev_warn(parent, "%s: buddy_size %u too small\n", __func__,
+			buddy_size);
+		buddy_size = 0;
+	} else if (WARN_ON(buddy_size >= len)) {
+		dev_warn(parent, "%s: buddy_size %u too large\n", __func__,
+			buddy_size);
+		buddy_size = 0;
+	} else if (WARN_ON(buddy_size & (buddy_size - 1))) {
+		dev_warn(parent, "%s: buddy_size %u not a power of 2\n",
+			 __func__, buddy_size);
+		buddy_size = 1 << (ilog2(buddy_size) + 1);
+	}
+
+	if (WARN_ON(buddy_size && (base & (buddy_size - 1)))) {
+		unsigned long orig = base;
+		dev_warn(parent, "%s: base address %p not aligned to "
+			 "buddy_size %u\n", __func__, (void *)base, buddy_size);
+		base = ALIGN(base, buddy_size);
+		len -= (base - orig);
+	}
+
+	if (WARN_ON(buddy_size && (len & (buddy_size - 1)))) {
+		dev_warn(parent, "%s: length %u not aligned to "
+			 "buddy_size %u\n", __func__, len, buddy_size);
+		len &= ~(buddy_size - 1);
+	}
+
+	h = kzalloc(sizeof(*h), GFP_KERNEL);
+	if (!h) {
+		dev_err(parent, "%s: out of memory\n", __func__);
+		goto fail_alloc;
+	}
+
+	l = kmem_cache_zalloc(block_cache, GFP_KERNEL);
+	if (!l) {
+		dev_err(parent, "%s: out of memory\n", __func__);
+		goto fail_alloc;
+	}
+
+	dev_set_name(&h->dev, "heap-%s", name);
+	h->name = name;
+	h->arg = arg;
+	h->dev.parent = parent;
+	h->dev.driver = NULL;
+	h->dev.release = heap_release;
+	if (device_register(&h->dev)) {
+		dev_err(parent, "%s: failed to register %s\n", __func__,
+			dev_name(&h->dev));
+		goto fail_alloc;
+	}
+	if (sysfs_create_group(&h->dev.kobj, &heap_stat_attr_group)) {
+		dev_err(&h->dev, "%s: failed to create attributes\n", __func__);
+		goto fail_register;
+	}
+	h->small_alloc = max(2 * buddy_size, len / 256);
+	h->buddy_heap_size = buddy_size;
+	if (buddy_size)
+		h->min_buddy_shift = ilog2(buddy_size / MAX_BUDDY_NR);
+	INIT_LIST_HEAD(&h->free_list);
+	INIT_LIST_HEAD(&h->buddy_list);
+	INIT_LIST_HEAD(&h->all_list);
+	mutex_init(&h->lock);
+	l->block.base = base;
+	l->block.type = BLOCK_EMPTY;
+	l->size = len;
+	l->orig_addr = base;
+	list_add_tail(&l->free_list, &h->free_list);
+	list_add_tail(&l->all_list, &h->all_list);
+
+	inner_flush_cache_all();
+	outer_flush_range(base, base + len);
+	wmb();
+	return h;
+
+fail_register:
+	device_unregister(&h->dev);
+fail_alloc:
+	if (l)
+		kmem_cache_free(block_cache, l);
+	kfree(h);
+	return NULL;
+}
+
+void *nvmap_heap_device_to_arg(struct device *dev)
+{
+	struct nvmap_heap *heap = container_of(dev, struct nvmap_heap, dev);
+	return heap->arg;
+}
+
+void *nvmap_heap_to_arg(struct nvmap_heap *heap)
+{
+	return heap->arg;
+}
+
+/* nvmap_heap_destroy: frees all resources in heap */
+void nvmap_heap_destroy(struct nvmap_heap *heap)
+{
+	WARN_ON(!list_empty(&heap->buddy_list));
+
+	sysfs_remove_group(&heap->dev.kobj, &heap_stat_attr_group);
+	device_unregister(&heap->dev);
+
+	while (!list_empty(&heap->buddy_list)) {
+		struct buddy_heap *b;
+		b = list_first_entry(&heap->buddy_list, struct buddy_heap,
+				     buddy_list);
+		list_del(&heap->buddy_list);
+		nvmap_heap_free(&b->heap_base->block);
+		kmem_cache_free(buddy_heap_cache, b);
+	}
+
+	WARN_ON(!list_is_singular(&heap->all_list));
+	while (!list_empty(&heap->all_list)) {
+		struct list_block *l;
+		l = list_first_entry(&heap->all_list, struct list_block,
+				     all_list);
+		list_del(&l->all_list);
+		kmem_cache_free(block_cache, l);
+	}
+
+	kfree(heap);
+}
+
+/* nvmap_heap_create_group: adds the attribute_group grp to the heap kobject */
+int nvmap_heap_create_group(struct nvmap_heap *heap,
+			    const struct attribute_group *grp)
+{
+	return sysfs_create_group(&heap->dev.kobj, grp);
+}
+
+/* nvmap_heap_remove_group: removes the attribute_group grp  */
+void nvmap_heap_remove_group(struct nvmap_heap *heap,
+			     const struct attribute_group *grp)
+{
+	sysfs_remove_group(&heap->dev.kobj, grp);
+}
+
+int nvmap_heap_init(void)
+{
+	BUG_ON(buddy_heap_cache != NULL);
+	buddy_heap_cache = KMEM_CACHE(buddy_heap, 0);
+	if (!buddy_heap_cache) {
+		pr_err("%s: unable to create buddy heap cache\n", __func__);
+		return -ENOMEM;
+	}
+
+	block_cache = KMEM_CACHE(combo_block, 0);
+	if (!block_cache) {
+		kmem_cache_destroy(buddy_heap_cache);
+		pr_err("%s: unable to create block cache\n", __func__);
+		return -ENOMEM;
+	}
+	return 0;
+}
+
+void nvmap_heap_deinit(void)
+{
+	if (buddy_heap_cache)
+		kmem_cache_destroy(buddy_heap_cache);
+	if (block_cache)
+		kmem_cache_destroy(block_cache);
+
+	block_cache = NULL;
+	buddy_heap_cache = NULL;
+}
diff --git a/drivers/video/tegra/nvmap/nvmap_heap.h b/drivers/video/tegra/nvmap/nvmap_heap.h
new file mode 100644
index 000000000000..158a1fa3d33c
--- /dev/null
+++ b/drivers/video/tegra/nvmap/nvmap_heap.h
@@ -0,0 +1,68 @@
+/*
+ * drivers/video/tegra/nvmap/nvmap_heap.h
+ *
+ * GPU heap allocator.
+ *
+ * Copyright (c) 2010-2011, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __NVMAP_HEAP_H
+#define __NVMAP_HEAP_H
+
+struct device;
+struct nvmap_heap;
+struct attribute_group;
+
+struct nvmap_heap_block {
+	phys_addr_t	base;
+	unsigned int	type;
+	struct nvmap_handle *handle;
+};
+
+#define NVMAP_HEAP_MIN_BUDDY_SIZE	8192
+
+struct nvmap_heap *nvmap_heap_create(struct device *parent, const char *name,
+				     phys_addr_t base, size_t len,
+				     unsigned int buddy_size, void *arg);
+
+void nvmap_heap_destroy(struct nvmap_heap *heap);
+
+void *nvmap_heap_device_to_arg(struct device *dev);
+
+void *nvmap_heap_to_arg(struct nvmap_heap *heap);
+
+struct nvmap_heap_block *nvmap_heap_alloc(struct nvmap_heap *heap,
+					  struct nvmap_handle *handle);
+
+struct nvmap_heap *nvmap_block_to_heap(struct nvmap_heap_block *b);
+
+void nvmap_heap_free(struct nvmap_heap_block *block);
+
+int nvmap_heap_create_group(struct nvmap_heap *heap,
+			    const struct attribute_group *grp);
+
+void nvmap_heap_remove_group(struct nvmap_heap *heap,
+			     const struct attribute_group *grp);
+
+int __init nvmap_heap_init(void);
+
+void nvmap_heap_deinit(void);
+
+int nvmap_flush_heap_block(struct nvmap_client *client,
+	struct nvmap_heap_block *block, size_t len, unsigned int prot);
+
+#endif
diff --git a/drivers/video/tegra/nvmap/nvmap_ioctl.c b/drivers/video/tegra/nvmap/nvmap_ioctl.c
new file mode 100644
index 000000000000..da974b2c6846
--- /dev/null
+++ b/drivers/video/tegra/nvmap/nvmap_ioctl.c
@@ -0,0 +1,791 @@
+/*
+ * drivers/video/tegra/nvmap/nvmap_ioctl.c
+ *
+ * User-space interface to nvmap
+ *
+ * Copyright (c) 2011-2012, NVIDIA 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/fs.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/nvmap.h>
+
+#include <asm/cacheflush.h>
+#include <asm/outercache.h>
+#include <asm/tlbflush.h>
+
+#include <mach/iovmm.h>
+#include <trace/events/nvmap.h>
+
+#include "nvmap_ioctl.h"
+#include "nvmap.h"
+#include "nvmap_common.h"
+
+static ssize_t rw_handle(struct nvmap_client *client, struct nvmap_handle *h,
+			 int is_read, unsigned long h_offs,
+			 unsigned long sys_addr, unsigned long h_stride,
+			 unsigned long sys_stride, unsigned long elem_size,
+			 unsigned long count);
+
+static int cache_maint(struct nvmap_client *client, struct nvmap_handle *h,
+		       unsigned long start, unsigned long end, unsigned int op);
+
+
+int nvmap_ioctl_pinop(struct file *filp, bool is_pin, void __user *arg)
+{
+	struct nvmap_pin_handle op;
+	struct nvmap_handle *h;
+	unsigned long on_stack[16];
+	unsigned long *refs;
+	unsigned long __user *output;
+	unsigned int i;
+	int err = 0;
+
+	if (copy_from_user(&op, arg, sizeof(op)))
+		return -EFAULT;
+
+	if (!op.count)
+		return -EINVAL;
+
+	if (op.count > 1) {
+		size_t bytes = op.count * sizeof(*refs); /* kcalloc below will catch overflow. */
+
+		if (op.count > ARRAY_SIZE(on_stack))
+			refs = kcalloc(op.count, sizeof(*refs), GFP_KERNEL);
+		else
+			refs = on_stack;
+
+		if (!refs)
+			return -ENOMEM;
+
+		if (copy_from_user(refs, (void *)op.handles, bytes)) {
+			err = -EFAULT;
+			goto out;
+		}
+	} else {
+		refs = on_stack;
+		on_stack[0] = (unsigned long)op.handles;
+	}
+
+	trace_nvmap_ioctl_pinop(filp->private_data, is_pin, op.count, refs);
+	if (is_pin)
+		err = nvmap_pin_ids(filp->private_data, op.count, refs);
+	else
+		nvmap_unpin_ids(filp->private_data, op.count, refs);
+
+	/* skip the output stage on unpin */
+	if (err || !is_pin)
+		goto out;
+
+	/* it is guaranteed that if nvmap_pin_ids returns 0 that
+	 * all of the handle_ref objects are valid, so dereferencing
+	 * directly here is safe */
+	if (op.count > 1)
+		output = (unsigned long __user *)op.addr;
+	else {
+		struct nvmap_pin_handle __user *tmp = arg;
+		output = (unsigned long __user *)&(tmp->addr);
+	}
+
+	if (!output)
+		goto out;
+
+	for (i = 0; i < op.count && !err; i++) {
+		unsigned long addr;
+
+		h = (struct nvmap_handle *)refs[i];
+
+		if (h->heap_pgalloc && h->pgalloc.contig)
+			addr = page_to_phys(h->pgalloc.pages[0]);
+		else if (h->heap_pgalloc)
+			addr = h->pgalloc.area->iovm_start;
+		else
+			addr = h->carveout->base;
+
+		err = put_user(addr, &output[i]);
+	}
+
+	if (err)
+		nvmap_unpin_ids(filp->private_data, op.count, refs);
+
+out:
+	if (refs != on_stack)
+		kfree(refs);
+
+	return err;
+}
+
+int nvmap_ioctl_getid(struct file *filp, void __user *arg)
+{
+	struct nvmap_client *client = filp->private_data;
+	struct nvmap_create_handle op;
+	struct nvmap_handle *h = NULL;
+
+	if (copy_from_user(&op, arg, sizeof(op)))
+		return -EFAULT;
+
+	if (!op.handle)
+		return -EINVAL;
+
+	h = nvmap_get_handle_id(client, op.handle);
+
+	if (!h)
+		return -EPERM;
+
+	op.id = (__u32)h;
+	if (client == h->owner)
+		h->global = true;
+
+	nvmap_handle_put(h);
+
+	return copy_to_user(arg, &op, sizeof(op)) ? -EFAULT : 0;
+}
+
+int nvmap_ioctl_alloc(struct file *filp, void __user *arg)
+{
+	struct nvmap_alloc_handle op;
+	struct nvmap_client *client = filp->private_data;
+
+	if (copy_from_user(&op, arg, sizeof(op)))
+		return -EFAULT;
+
+	if (!op.handle)
+		return -EINVAL;
+
+	if (op.align & (op.align - 1))
+		return -EINVAL;
+
+	/* user-space handles are aligned to page boundaries, to prevent
+	 * data leakage. */
+	op.align = max_t(size_t, op.align, PAGE_SIZE);
+#if defined(CONFIG_NVMAP_FORCE_ZEROED_USER_PAGES)
+	op.flags |= NVMAP_HANDLE_ZEROED_PAGES;
+#endif
+
+	return nvmap_alloc_handle_id(client, op.handle, op.heap_mask,
+				     op.align, op.flags);
+}
+
+int nvmap_ioctl_create(struct file *filp, unsigned int cmd, void __user *arg)
+{
+	struct nvmap_create_handle op;
+	struct nvmap_handle_ref *ref = NULL;
+	struct nvmap_client *client = filp->private_data;
+	int err = 0;
+
+	if (copy_from_user(&op, arg, sizeof(op)))
+		return -EFAULT;
+
+	if (!client)
+		return -ENODEV;
+
+	if (cmd == NVMAP_IOC_CREATE) {
+		ref = nvmap_create_handle(client, PAGE_ALIGN(op.size));
+		if (!IS_ERR(ref))
+			ref->handle->orig_size = op.size;
+	} else if (cmd == NVMAP_IOC_FROM_ID) {
+		ref = nvmap_duplicate_handle_id(client, op.id);
+	} else {
+		return -EINVAL;
+	}
+
+	if (IS_ERR(ref))
+		return PTR_ERR(ref);
+
+	op.handle = nvmap_ref_to_id(ref);
+	if (copy_to_user(arg, &op, sizeof(op))) {
+		err = -EFAULT;
+		nvmap_free_handle_id(client, op.handle);
+	}
+
+	return err;
+}
+
+int nvmap_map_into_caller_ptr(struct file *filp, void __user *arg)
+{
+	struct nvmap_client *client = filp->private_data;
+	struct nvmap_map_caller op;
+	struct nvmap_vma_priv *vpriv;
+	struct vm_area_struct *vma;
+	struct nvmap_handle *h = NULL;
+	unsigned int cache_flags;
+	int err = 0;
+
+	if (copy_from_user(&op, arg, sizeof(op)))
+		return -EFAULT;
+
+	if (!op.handle)
+		return -EINVAL;
+
+	h = nvmap_get_handle_id(client, op.handle);
+
+	if (!h)
+		return -EPERM;
+
+	if(!h->alloc) {
+		nvmap_handle_put(h);
+		return -EFAULT;
+	}
+
+	trace_nvmap_map_into_caller_ptr(client, h, op.offset,
+					op.length, op.flags);
+	down_read(&current->mm->mmap_sem);
+
+	vma = find_vma(current->mm, op.addr);
+	if (!vma || !vma->vm_private_data) {
+		err = -ENOMEM;
+		goto out;
+	}
+
+	if (op.offset & ~PAGE_MASK) {
+		err = -EFAULT;
+		goto out;
+	}
+
+	if (op.offset > h->size || (op.offset + op.length) > h->size) {
+		err = -EADDRNOTAVAIL;
+		goto out;
+	}
+
+	vpriv = vma->vm_private_data;
+	BUG_ON(!vpriv);
+
+	/* the VMA must exactly match the requested mapping operation, and the
+	 * VMA that is targetted must have been created by this driver
+	 */
+	if ((vma->vm_start != op.addr) || !is_nvmap_vma(vma) ||
+	    (vma->vm_end-vma->vm_start != op.length)) {
+		err = -EPERM;
+		goto out;
+	}
+
+	/* verify that each mmap() system call creates a unique VMA */
+
+	if (vpriv->handle && (h == vpriv->handle)) {
+		goto out;
+	} else if (vpriv->handle) {
+		err = -EADDRNOTAVAIL;
+		goto out;
+	}
+
+	nvmap_usecount_inc(h);
+
+	if (!h->heap_pgalloc && (h->carveout->base & ~PAGE_MASK)) {
+		nvmap_usecount_dec(h);
+		err = -EFAULT;
+		goto out;
+	}
+
+	vpriv->handle = h;
+	vpriv->offs = op.offset;
+
+	cache_flags = op.flags & NVMAP_HANDLE_CACHE_FLAG;
+	if ((cache_flags == NVMAP_HANDLE_INNER_CACHEABLE ||
+	     cache_flags == NVMAP_HANDLE_CACHEABLE) &&
+	    (h->flags == NVMAP_HANDLE_UNCACHEABLE ||
+	     h->flags == NVMAP_HANDLE_WRITE_COMBINE)) {
+		if (h->size & ~PAGE_MASK) {
+			pr_err("\n%s:attempt to convert a buffer from uc/wc to"
+				" wb, whose size is not a multiple of page size."
+				" request ignored.\n", __func__);
+		} else {
+			unsigned int nr_page = h->size >> PAGE_SHIFT;
+			wmb();
+			/* override allocation time cache coherency attributes. */
+			h->flags &= ~NVMAP_HANDLE_CACHE_FLAG;
+			h->flags |= cache_flags;
+
+			/* Update page attributes, if the memory is allocated
+			 *  from system heap pages.
+			 */
+			if (cache_flags == NVMAP_HANDLE_INNER_CACHEABLE &&
+				h->heap_pgalloc)
+				set_pages_array_iwb(h->pgalloc.pages, nr_page);
+			else if (h->heap_pgalloc)
+				set_pages_array_wb(h->pgalloc.pages, nr_page);
+		}
+	}
+	vma->vm_page_prot = nvmap_pgprot(h, vma->vm_page_prot);
+
+out:
+	up_read(&current->mm->mmap_sem);
+
+	if (err)
+		nvmap_handle_put(h);
+	return err;
+}
+
+int nvmap_ioctl_get_param(struct file *filp, void __user* arg)
+{
+	struct nvmap_handle_param op;
+	struct nvmap_client *client = filp->private_data;
+	struct nvmap_handle *h;
+	int err = 0;
+
+	if (copy_from_user(&op, arg, sizeof(op)))
+		return -EFAULT;
+
+	h = nvmap_get_handle_id(client, op.handle);
+	if (!h)
+		return -EINVAL;
+
+	switch (op.param) {
+	case NVMAP_HANDLE_PARAM_SIZE:
+		op.result = h->orig_size;
+		break;
+	case NVMAP_HANDLE_PARAM_ALIGNMENT:
+		mutex_lock(&h->lock);
+		if (!h->alloc)
+			op.result = 0;
+		else if (h->heap_pgalloc)
+			op.result = PAGE_SIZE;
+		else if (h->carveout->base)
+			op.result = (h->carveout->base & -h->carveout->base);
+		else
+			op.result = SZ_4M;
+		mutex_unlock(&h->lock);
+		break;
+	case NVMAP_HANDLE_PARAM_BASE:
+		if (WARN_ON(!h->alloc || !atomic_add_return(0, &h->pin)))
+			op.result = -1ul;
+		else if (!h->heap_pgalloc) {
+			mutex_lock(&h->lock);
+			op.result = h->carveout->base;
+			mutex_unlock(&h->lock);
+		} else if (h->pgalloc.contig)
+			op.result = page_to_phys(h->pgalloc.pages[0]);
+		else if (h->pgalloc.area)
+			op.result = h->pgalloc.area->iovm_start;
+		else
+			op.result = -1ul;
+		break;
+	case NVMAP_HANDLE_PARAM_HEAP:
+		if (!h->alloc)
+			op.result = 0;
+		else if (!h->heap_pgalloc) {
+			mutex_lock(&h->lock);
+			op.result = nvmap_carveout_usage(client, h->carveout);
+			mutex_unlock(&h->lock);
+		} else if (h->pgalloc.contig)
+			op.result = NVMAP_HEAP_SYSMEM;
+		else
+			op.result = NVMAP_HEAP_IOVMM;
+		break;
+	default:
+		err = -EINVAL;
+		break;
+	}
+
+	if (!err && copy_to_user(arg, &op, sizeof(op)))
+		err = -EFAULT;
+
+	nvmap_handle_put(h);
+	return err;
+}
+
+int nvmap_ioctl_rw_handle(struct file *filp, int is_read, void __user* arg)
+{
+	struct nvmap_client *client = filp->private_data;
+	struct nvmap_rw_handle __user *uarg = arg;
+	struct nvmap_rw_handle op;
+	struct nvmap_handle *h;
+	ssize_t copied;
+	int err = 0;
+
+	if (copy_from_user(&op, arg, sizeof(op)))
+		return -EFAULT;
+
+	if (!op.handle || !op.addr || !op.count || !op.elem_size)
+		return -EINVAL;
+
+	h = nvmap_get_handle_id(client, op.handle);
+	if (!h)
+		return -EPERM;
+
+	nvmap_usecount_inc(h);
+
+	trace_nvmap_ioctl_rw_handle(client, h, is_read, op.offset,
+				    op.addr, op.hmem_stride,
+				    op.user_stride, op.elem_size, op.count);
+	copied = rw_handle(client, h, is_read, op.offset,
+			   (unsigned long)op.addr, op.hmem_stride,
+			   op.user_stride, op.elem_size, op.count);
+
+	if (copied < 0) {
+		err = copied;
+		copied = 0;
+	} else if (copied < (op.count * op.elem_size))
+		err = -EINTR;
+
+	__put_user(copied, &uarg->count);
+
+	nvmap_usecount_dec(h);
+
+	nvmap_handle_put(h);
+
+	return err;
+}
+
+int nvmap_ioctl_cache_maint(struct file *filp, void __user *arg)
+{
+	struct nvmap_client *client = filp->private_data;
+	struct nvmap_cache_op op;
+	struct vm_area_struct *vma;
+	struct nvmap_vma_priv *vpriv;
+	unsigned long start;
+	unsigned long end;
+	int err = 0;
+
+	if (copy_from_user(&op, arg, sizeof(op)))
+		return -EFAULT;
+
+	if (!op.handle || !op.addr || op.op < NVMAP_CACHE_OP_WB ||
+	    op.op > NVMAP_CACHE_OP_WB_INV)
+		return -EINVAL;
+
+	down_read(&current->mm->mmap_sem);
+
+	vma = find_vma(current->active_mm, (unsigned long)op.addr);
+	if (!vma || !is_nvmap_vma(vma) ||
+	    (unsigned long)op.addr + op.len > vma->vm_end) {
+		err = -EADDRNOTAVAIL;
+		goto out;
+	}
+
+	vpriv = (struct nvmap_vma_priv *)vma->vm_private_data;
+
+	if ((unsigned long)vpriv->handle != op.handle) {
+		err = -EFAULT;
+		goto out;
+	}
+
+	start = (unsigned long)op.addr - vma->vm_start;
+	end = start + op.len;
+
+	err = cache_maint(client, vpriv->handle, start, end, op.op);
+out:
+	up_read(&current->mm->mmap_sem);
+	return err;
+}
+
+int nvmap_ioctl_free(struct file *filp, unsigned long arg)
+{
+	struct nvmap_client *client = filp->private_data;
+
+	if (!arg)
+		return 0;
+
+	nvmap_free_handle_id(client, arg);
+	return 0;
+}
+
+static void inner_cache_maint(unsigned int op, void *vaddr, size_t size)
+{
+	if (op == NVMAP_CACHE_OP_WB_INV)
+		dmac_flush_range(vaddr, vaddr + size);
+	else if (op == NVMAP_CACHE_OP_INV)
+		dmac_map_area(vaddr, size, DMA_FROM_DEVICE);
+	else
+		dmac_map_area(vaddr, size, DMA_TO_DEVICE);
+}
+
+static void outer_cache_maint(unsigned int op, unsigned long paddr, size_t size)
+{
+	if (op == NVMAP_CACHE_OP_WB_INV)
+		outer_flush_range(paddr, paddr + size);
+	else if (op == NVMAP_CACHE_OP_INV)
+		outer_inv_range(paddr, paddr + size);
+	else
+		outer_clean_range(paddr, paddr + size);
+}
+
+static void heap_page_cache_maint(struct nvmap_client *client,
+	struct nvmap_handle *h, unsigned long start, unsigned long end,
+	unsigned int op, bool inner, bool outer, pte_t **pte,
+	unsigned long kaddr, pgprot_t prot)
+{
+	struct page *page;
+	unsigned long paddr;
+	unsigned long next;
+	unsigned long off;
+	size_t size;
+
+	while (start < end) {
+		page = h->pgalloc.pages[start >> PAGE_SHIFT];
+		next = min(((start + PAGE_SIZE) & PAGE_MASK), end);
+		off = start & ~PAGE_MASK;
+		size = next - start;
+		paddr = page_to_phys(page) + off;
+
+		if (inner) {
+			void *vaddr = (void *)kaddr + off;
+			BUG_ON(!pte);
+			BUG_ON(!kaddr);
+			set_pte_at(&init_mm, kaddr, *pte,
+				pfn_pte(__phys_to_pfn(paddr), prot));
+			flush_tlb_kernel_page(kaddr);
+			inner_cache_maint(op, vaddr, size);
+		}
+
+		if (outer)
+			outer_cache_maint(op, paddr, size);
+		start = next;
+	}
+}
+
+static bool fast_cache_maint_outer(unsigned long start,
+		unsigned long end, unsigned int op)
+{
+	bool result = false;
+#if defined(CONFIG_NVMAP_OUTER_CACHE_MAINT_BY_SET_WAYS)
+	if (end - start >= FLUSH_CLEAN_BY_SET_WAY_THRESHOLD_OUTER) {
+		if (op == NVMAP_CACHE_OP_WB_INV) {
+			outer_flush_all();
+			result = true;
+		}
+		if (op == NVMAP_CACHE_OP_WB) {
+			outer_clean_all();
+			result = true;
+		}
+	}
+#endif
+	return result;
+}
+
+static bool fast_cache_maint(struct nvmap_client *client, struct nvmap_handle *h,
+	unsigned long start, unsigned long end, unsigned int op)
+{
+	int ret = false;
+#if defined(CONFIG_NVMAP_CACHE_MAINT_BY_SET_WAYS)
+	if ((op == NVMAP_CACHE_OP_INV) ||
+		((end - start) < FLUSH_CLEAN_BY_SET_WAY_THRESHOLD_INNER))
+		goto out;
+
+	if (op == NVMAP_CACHE_OP_WB_INV)
+		inner_flush_cache_all();
+	else if (op == NVMAP_CACHE_OP_WB)
+		inner_clean_cache_all();
+
+	/* outer maintenance */
+	if (h->flags != NVMAP_HANDLE_INNER_CACHEABLE ) {
+		if(!fast_cache_maint_outer(start, end, op))
+		{
+			if (h->heap_pgalloc) {
+				heap_page_cache_maint(client, h, start,
+					end, op, false, true, NULL, 0, 0);
+			} else  {
+				start += h->carveout->base;
+				end += h->carveout->base;
+				outer_cache_maint(op, start, end - start);
+			}
+		}
+	}
+	ret = true;
+out:
+#endif
+	return ret;
+}
+
+static int cache_maint(struct nvmap_client *client, struct nvmap_handle *h,
+		       unsigned long start, unsigned long end, unsigned int op)
+{
+	pgprot_t prot;
+	pte_t **pte = NULL;
+	unsigned long kaddr;
+	unsigned long loop;
+	int err = 0;
+
+	h = nvmap_handle_get(h);
+	if (!h)
+		return -EFAULT;
+
+	if (!h->alloc) {
+		err = -EFAULT;
+		goto out;
+	}
+
+	trace_cache_maint(client, h, start, end, op);
+	wmb();
+	if (h->flags == NVMAP_HANDLE_UNCACHEABLE ||
+	    h->flags == NVMAP_HANDLE_WRITE_COMBINE || start == end)
+		goto out;
+
+	if (fast_cache_maint(client, h, start, end, op))
+		goto out;
+
+	prot = nvmap_pgprot(h, pgprot_kernel);
+	pte = nvmap_alloc_pte(client->dev, (void **)&kaddr);
+	if (IS_ERR(pte)) {
+		err = PTR_ERR(pte);
+		pte = NULL;
+		goto out;
+	}
+
+	if (h->heap_pgalloc) {
+		heap_page_cache_maint(client, h, start, end, op, true,
+			(h->flags == NVMAP_HANDLE_INNER_CACHEABLE) ? false : true,
+			pte, kaddr, prot);
+		goto out;
+	}
+
+	if (start > h->size || end > h->size) {
+		nvmap_warn(client, "cache maintenance outside handle\n");
+		return -EINVAL;
+	}
+
+	/* lock carveout from relocation by mapcount */
+	nvmap_usecount_inc(h);
+
+	start += h->carveout->base;
+	end += h->carveout->base;
+
+	loop = start;
+
+	while (loop < end) {
+		unsigned long next = (loop + PAGE_SIZE) & PAGE_MASK;
+		void *base = (void *)kaddr + (loop & ~PAGE_MASK);
+		next = min(next, end);
+
+		set_pte_at(&init_mm, kaddr, *pte,
+			   pfn_pte(__phys_to_pfn(loop), prot));
+		flush_tlb_kernel_page(kaddr);
+
+		inner_cache_maint(op, base, next - loop);
+		loop = next;
+	}
+
+	if (h->flags != NVMAP_HANDLE_INNER_CACHEABLE)
+		outer_cache_maint(op, start, end - start);
+
+	/* unlock carveout */
+	nvmap_usecount_dec(h);
+
+out:
+	if (pte)
+		nvmap_free_pte(client->dev, pte);
+	nvmap_handle_put(h);
+	return err;
+}
+
+static int rw_handle_page(struct nvmap_handle *h, int is_read,
+			  phys_addr_t start, unsigned long rw_addr,
+			  unsigned long bytes, unsigned long kaddr, pte_t *pte)
+{
+	pgprot_t prot = nvmap_pgprot(h, pgprot_kernel);
+	unsigned long end = start + bytes;
+	int err = 0;
+
+	while (!err && start < end) {
+		struct page *page = NULL;
+		phys_addr_t phys;
+		size_t count;
+		void *src;
+
+		if (!h->heap_pgalloc) {
+			phys = h->carveout->base + start;
+		} else {
+			page = h->pgalloc.pages[start >> PAGE_SHIFT];
+			BUG_ON(!page);
+			get_page(page);
+			phys = page_to_phys(page) + (start & ~PAGE_MASK);
+		}
+
+		set_pte_at(&init_mm, kaddr, pte,
+			   pfn_pte(__phys_to_pfn(phys), prot));
+		flush_tlb_kernel_page(kaddr);
+
+		src = (void *)kaddr + (phys & ~PAGE_MASK);
+		phys = PAGE_SIZE - (phys & ~PAGE_MASK);
+		count = min_t(size_t, end - start, phys);
+
+		if (is_read)
+			err = copy_to_user((void *)rw_addr, src, count);
+		else
+			err = copy_from_user(src, (void *)rw_addr, count);
+
+		if (err)
+			err = -EFAULT;
+
+		rw_addr += count;
+		start += count;
+
+		if (page)
+			put_page(page);
+	}
+
+	return err;
+}
+
+static ssize_t rw_handle(struct nvmap_client *client, struct nvmap_handle *h,
+			 int is_read, unsigned long h_offs,
+			 unsigned long sys_addr, unsigned long h_stride,
+			 unsigned long sys_stride, unsigned long elem_size,
+			 unsigned long count)
+{
+	ssize_t copied = 0;
+	pte_t **pte;
+	void *addr;
+	int ret = 0;
+
+	if (!elem_size)
+		return -EINVAL;
+
+	if (!h->alloc)
+		return -EFAULT;
+
+	if (elem_size == h_stride && elem_size == sys_stride) {
+		elem_size *= count;
+		h_stride = elem_size;
+		sys_stride = elem_size;
+		count = 1;
+	}
+
+	pte = nvmap_alloc_pte(client->dev, &addr);
+	if (IS_ERR(pte))
+		return PTR_ERR(pte);
+
+	while (count--) {
+		if (h_offs + elem_size > h->size) {
+			nvmap_warn(client, "read/write outside of handle\n");
+			ret = -EFAULT;
+			break;
+		}
+		if (is_read)
+			cache_maint(client, h, h_offs,
+				h_offs + elem_size, NVMAP_CACHE_OP_INV);
+
+		ret = rw_handle_page(h, is_read, h_offs, sys_addr,
+				     elem_size, (unsigned long)addr, *pte);
+
+		if (ret)
+			break;
+
+		if (!is_read)
+			cache_maint(client, h, h_offs,
+				h_offs + elem_size, NVMAP_CACHE_OP_WB);
+
+		copied += elem_size;
+		sys_addr += sys_stride;
+		h_offs += h_stride;
+	}
+
+	nvmap_free_pte(client->dev, pte);
+	return ret ?: copied;
+}
diff --git a/drivers/video/tegra/nvmap/nvmap_ioctl.h b/drivers/video/tegra/nvmap/nvmap_ioctl.h
new file mode 100644
index 000000000000..300ce9b9a6ea
--- /dev/null
+++ b/drivers/video/tegra/nvmap/nvmap_ioctl.h
@@ -0,0 +1,163 @@
+/*
+ * drivers/video/tegra/nvmap/nvmap_ioctl.h
+ *
+ * ioctl declarations for nvmap
+ *
+ * Copyright (c) 2010, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __VIDEO_TEGRA_NVMAP_IOCTL_H
+#define __VIDEO_TEGRA_NVMAP_IOCTL_H
+
+#include <linux/ioctl.h>
+
+#ifdef  __KERNEL__
+#include <linux/file.h>
+#include <linux/nvmap.h>
+#endif
+
+enum {
+	NVMAP_HANDLE_PARAM_SIZE = 1,
+	NVMAP_HANDLE_PARAM_ALIGNMENT,
+	NVMAP_HANDLE_PARAM_BASE,
+	NVMAP_HANDLE_PARAM_HEAP,
+};
+
+enum {
+	NVMAP_CACHE_OP_WB = 0,
+	NVMAP_CACHE_OP_INV,
+	NVMAP_CACHE_OP_WB_INV,
+};
+
+
+struct nvmap_create_handle {
+	union {
+		__u32 key;	/* ClaimPreservedHandle */
+		__u32 id;	/* FromId */
+		__u32 size;	/* CreateHandle */
+	};
+	__u32 handle;
+};
+
+struct nvmap_alloc_handle {
+	__u32 handle;
+	__u32 heap_mask;
+	__u32 flags;
+	__u32 align;
+};
+
+struct nvmap_map_caller {
+	__u32 handle;		/* hmem */
+	__u32 offset;		/* offset into hmem; should be page-aligned */
+	__u32 length;		/* number of bytes to map */
+	__u32 flags;
+	unsigned long addr;	/* user pointer */
+};
+
+struct nvmap_rw_handle {
+	unsigned long addr;	/* user pointer */
+	__u32 handle;		/* hmem */
+	__u32 offset;		/* offset into hmem */
+	__u32 elem_size;	/* individual atom size */
+	__u32 hmem_stride;	/* delta in bytes between atoms in hmem */
+	__u32 user_stride;	/* delta in bytes between atoms in user */
+	__u32 count;		/* number of atoms to copy */
+};
+
+struct nvmap_pin_handle {
+	unsigned long handles;	/* array of handles to pin/unpin */
+	unsigned long addr;	/* array of addresses to return */
+	__u32 count;		/* number of entries in handles */
+};
+
+struct nvmap_handle_param {
+	__u32 handle;
+	__u32 param;
+	unsigned long result;
+};
+
+struct nvmap_cache_op {
+	unsigned long addr;
+	__u32 handle;
+	__u32 len;
+	__s32 op;
+};
+
+#define NVMAP_IOC_MAGIC 'N'
+
+/* Creates a new memory handle. On input, the argument is the size of the new
+ * handle; on return, the argument is the name of the new handle
+ */
+#define NVMAP_IOC_CREATE  _IOWR(NVMAP_IOC_MAGIC, 0, struct nvmap_create_handle)
+#define NVMAP_IOC_CLAIM   _IOWR(NVMAP_IOC_MAGIC, 1, struct nvmap_create_handle)
+#define NVMAP_IOC_FROM_ID _IOWR(NVMAP_IOC_MAGIC, 2, struct nvmap_create_handle)
+
+/* Actually allocates memory for the specified handle */
+#define NVMAP_IOC_ALLOC    _IOW(NVMAP_IOC_MAGIC, 3, struct nvmap_alloc_handle)
+
+/* Frees a memory handle, unpinning any pinned pages and unmapping any mappings
+ */
+#define NVMAP_IOC_FREE       _IO(NVMAP_IOC_MAGIC, 4)
+
+/* Maps the region of the specified handle into a user-provided virtual address
+ * that was previously created via an mmap syscall on this fd */
+#define NVMAP_IOC_MMAP       _IOWR(NVMAP_IOC_MAGIC, 5, struct nvmap_map_caller)
+
+/* Reads/writes data (possibly strided) from a user-provided buffer into the
+ * hmem at the specified offset */
+#define NVMAP_IOC_WRITE      _IOW(NVMAP_IOC_MAGIC, 6, struct nvmap_rw_handle)
+#define NVMAP_IOC_READ       _IOW(NVMAP_IOC_MAGIC, 7, struct nvmap_rw_handle)
+
+#define NVMAP_IOC_PARAM _IOWR(NVMAP_IOC_MAGIC, 8, struct nvmap_handle_param)
+
+/* Pins a list of memory handles into IO-addressable memory (either IOVMM
+ * space or physical memory, depending on the allocation), and returns the
+ * address. Handles may be pinned recursively. */
+#define NVMAP_IOC_PIN_MULT   _IOWR(NVMAP_IOC_MAGIC, 10, struct nvmap_pin_handle)
+#define NVMAP_IOC_UNPIN_MULT _IOW(NVMAP_IOC_MAGIC, 11, struct nvmap_pin_handle)
+
+#define NVMAP_IOC_CACHE      _IOW(NVMAP_IOC_MAGIC, 12, struct nvmap_cache_op)
+
+/* Returns a global ID usable to allow a remote process to create a handle
+ * reference to the same handle */
+#define NVMAP_IOC_GET_ID  _IOWR(NVMAP_IOC_MAGIC, 13, struct nvmap_create_handle)
+
+#define NVMAP_IOC_MAXNR (_IOC_NR(NVMAP_IOC_GET_ID))
+
+#ifdef  __KERNEL__
+int nvmap_ioctl_pinop(struct file *filp, bool is_pin, void __user *arg);
+
+int nvmap_ioctl_get_param(struct file *filp, void __user* arg);
+
+int nvmap_ioctl_getid(struct file *filp, void __user *arg);
+
+int nvmap_ioctl_alloc(struct file *filp, void __user *arg);
+
+int nvmap_ioctl_free(struct file *filp, unsigned long arg);
+
+int nvmap_ioctl_create(struct file *filp, unsigned int cmd, void __user *arg);
+
+int nvmap_map_into_caller_ptr(struct file *filp, void __user *arg);
+
+int nvmap_ioctl_cache_maint(struct file *filp, void __user *arg);
+
+int nvmap_ioctl_rw_handle(struct file *filp, int is_read, void __user* arg);
+#endif
+
+
+
+#endif
diff --git a/drivers/video/tegra/nvmap/nvmap_iommu.c b/drivers/video/tegra/nvmap/nvmap_iommu.c
new file mode 100644
index 000000000000..ca868032db3f
--- /dev/null
+++ b/drivers/video/tegra/nvmap/nvmap_iommu.c
@@ -0,0 +1,97 @@
+/*
+ * IOMMU backend support for NVMAP
+ *
+ * Copyright (c) 2012, NVIDIA 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 it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <linux/slab.h>
+#include <mach/iomap.h>
+#include <mach/iovmm.h>
+
+#include "nvmap.h"
+
+struct tegra_iovmm_area *tegra_iommu_create_vm(struct device *dev,
+			       dma_addr_t req, size_t size, pgprot_t prot)
+{
+	struct tegra_iovmm_area *area;
+	dma_addr_t iova;
+
+	area = kmalloc(sizeof(*area), GFP_KERNEL);
+	if (!area)
+		return NULL;
+
+	if (!req)
+		req = DMA_ANON_ADDR;
+
+	iova = arm_iommu_alloc_iova_at(dev, req, size);
+	if (iova == DMA_ERROR_CODE)
+		goto err_out;
+	area->iovm_start = iova;
+	area->iovm_length = size;
+	area->pgprot = prot;
+	area->dev = dev;
+	return area;
+
+err_out:
+	kfree(area);
+	return NULL;
+}
+
+void tegra_iommu_free_vm(struct tegra_iovmm_area *area)
+{
+	int i;
+	size_t count = area->iovm_length >> PAGE_SHIFT;
+
+	for (i = 0; i < count; i++) {
+		dma_addr_t iova;
+
+		iova = area->iovm_start + i * PAGE_SIZE;
+		dma_unmap_page(area->dev, iova, PAGE_SIZE, DMA_NONE);
+	}
+	kfree(area);
+}
+
+struct tegra_iovmm_client *tegra_iommu_alloc_client(struct device *dev)
+{
+	struct dma_iommu_mapping *map;
+	struct tegra_iovmm_client *client;
+
+	client = kzalloc(sizeof(*client), GFP_KERNEL);
+	if (!client)
+		return NULL;
+
+	map = arm_iommu_create_mapping(&platform_bus_type,
+		       TEGRA_IOMMU_BASE, TEGRA_IOMMU_SIZE, 0);
+	if (IS_ERR(map))
+		goto err_map;
+
+	if (arm_iommu_attach_device(dev, map))
+		goto err_attach;
+	client->dev = dev;
+	return client;
+
+err_attach:
+	arm_iommu_release_mapping(map);
+err_map:
+	kfree(client);
+	return NULL;
+}
+
+void tegra_iommu_free_client(struct tegra_iovmm_client *client)
+{
+	arm_iommu_release_mapping(client->dev->archdata.mapping);
+	kfree(client);
+}
diff --git a/drivers/video/tegra/nvmap/nvmap_mru.c b/drivers/video/tegra/nvmap/nvmap_mru.c
new file mode 100644
index 000000000000..f54d44923ebf
--- /dev/null
+++ b/drivers/video/tegra/nvmap/nvmap_mru.c
@@ -0,0 +1,187 @@
+/*
+ * drivers/video/tegra/nvmap/nvmap_mru.c
+ *
+ * IOVMM virtualization support for nvmap
+ *
+ * Copyright (c) 2009-2011, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/list.h>
+#include <linux/slab.h>
+
+#include <asm/pgtable.h>
+
+#include <mach/iovmm.h>
+
+#include "nvmap.h"
+#include "nvmap_mru.h"
+
+/* if IOVMM reclamation is enabled (CONFIG_NVMAP_RECLAIM_UNPINNED_VM),
+ * unpinned handles are placed onto a most-recently-used eviction list;
+ * multiple lists are maintained, segmented by size (sizes were chosen to
+ * roughly correspond with common sizes for graphics surfaces).
+ *
+ * if a handle is located on the MRU list, then the code below may
+ * steal its IOVMM area at any time to satisfy a pin operation if no
+ * free IOVMM space is available
+ */
+
+static const size_t mru_cutoff[] = {
+	262144, 393216, 786432, 1048576, 1572864
+};
+
+static inline struct list_head *mru_list(struct nvmap_share *share, size_t size)
+{
+	unsigned int i;
+
+	BUG_ON(!share->mru_lists);
+	for (i = 0; i < ARRAY_SIZE(mru_cutoff); i++)
+		if (size <= mru_cutoff[i])
+			break;
+
+	return &share->mru_lists[i];
+}
+
+size_t nvmap_mru_vm_size(struct tegra_iovmm_client *iovmm)
+{
+	size_t vm_size = tegra_iovmm_get_vm_size(iovmm);
+	return (vm_size >> 2) * 3;
+}
+
+/*  nvmap_mru_vma_lock should be acquired by the caller before calling this */
+void nvmap_mru_insert_locked(struct nvmap_share *share, struct nvmap_handle *h)
+{
+	size_t len = h->pgalloc.area->iovm_length;
+	list_add(&h->pgalloc.mru_list, mru_list(share, len));
+}
+
+void nvmap_mru_remove(struct nvmap_share *s, struct nvmap_handle *h)
+{
+	nvmap_mru_lock(s);
+	if (!list_empty(&h->pgalloc.mru_list))
+		list_del(&h->pgalloc.mru_list);
+	nvmap_mru_unlock(s);
+	INIT_LIST_HEAD(&h->pgalloc.mru_list);
+}
+
+/* returns a tegra_iovmm_area for a handle. if the handle already has
+ * an iovmm_area allocated, the handle is simply removed from its MRU list
+ * and the existing iovmm_area is returned.
+ *
+ * if no existing allocation exists, try to allocate a new IOVMM area.
+ *
+ * if a new area can not be allocated, try to re-use the most-recently-unpinned
+ * handle's allocation.
+ *
+ * and if that fails, iteratively evict handles from the MRU lists and free
+ * their allocations, until the new allocation succeeds.
+ */
+struct tegra_iovmm_area *nvmap_handle_iovmm_locked(struct nvmap_client *c,
+					    struct nvmap_handle *h)
+{
+	struct list_head *mru;
+	struct nvmap_handle *evict = NULL;
+	struct tegra_iovmm_area *vm = NULL;
+	unsigned int i, idx;
+	pgprot_t prot;
+
+	BUG_ON(!h || !c || !c->share);
+
+	prot = nvmap_pgprot(h, pgprot_kernel);
+
+	if (h->pgalloc.area) {
+		BUG_ON(list_empty(&h->pgalloc.mru_list));
+		list_del(&h->pgalloc.mru_list);
+		INIT_LIST_HEAD(&h->pgalloc.mru_list);
+		return h->pgalloc.area;
+	}
+
+	vm = tegra_iovmm_create_vm(c->share->iovmm, NULL,
+			h->size, h->align, prot,
+			h->pgalloc.iovm_addr);
+
+	if (vm) {
+		INIT_LIST_HEAD(&h->pgalloc.mru_list);
+		return vm;
+	}
+	/* if client is looking for specific iovm address, return from here. */
+	if ((vm == NULL) && (h->pgalloc.iovm_addr != 0))
+		return NULL;
+	/* attempt to re-use the most recently unpinned IOVMM area in the
+	 * same size bin as the current handle. If that fails, iteratively
+	 * evict handles (starting from the current bin) until an allocation
+	 * succeeds or no more areas can be evicted */
+	mru = mru_list(c->share, h->size);
+	if (!list_empty(mru))
+		evict = list_first_entry(mru, struct nvmap_handle,
+					 pgalloc.mru_list);
+
+	if (evict && evict->pgalloc.area->iovm_length >= h->size) {
+		list_del(&evict->pgalloc.mru_list);
+		vm = evict->pgalloc.area;
+		evict->pgalloc.area = NULL;
+		INIT_LIST_HEAD(&evict->pgalloc.mru_list);
+		return vm;
+	}
+
+	idx = mru - c->share->mru_lists;
+
+	for (i = 0; i < c->share->nr_mru && !vm; i++, idx++) {
+		if (idx >= c->share->nr_mru)
+			idx = 0;
+		mru = &c->share->mru_lists[idx];
+		while (!list_empty(mru) && !vm) {
+			evict = list_first_entry(mru, struct nvmap_handle,
+						 pgalloc.mru_list);
+
+			BUG_ON(atomic_read(&evict->pin) != 0);
+			BUG_ON(!evict->pgalloc.area);
+			list_del(&evict->pgalloc.mru_list);
+			INIT_LIST_HEAD(&evict->pgalloc.mru_list);
+			tegra_iovmm_free_vm(evict->pgalloc.area);
+			evict->pgalloc.area = NULL;
+			vm = tegra_iovmm_create_vm(c->share->iovmm,
+					NULL, h->size, h->align,
+					prot, h->pgalloc.iovm_addr);
+		}
+	}
+	return vm;
+}
+
+int nvmap_mru_init(struct nvmap_share *share)
+{
+	int i;
+	mutex_init(&share->mru_lock);
+	share->nr_mru = ARRAY_SIZE(mru_cutoff) + 1;
+
+	share->mru_lists = kzalloc(sizeof(struct list_head) * share->nr_mru,
+				   GFP_KERNEL);
+
+	if (!share->mru_lists)
+		return -ENOMEM;
+
+	for (i = 0; i < share->nr_mru; i++)
+		INIT_LIST_HEAD(&share->mru_lists[i]);
+
+	return 0;
+}
+
+void nvmap_mru_destroy(struct nvmap_share *share)
+{
+	kfree(share->mru_lists);
+	share->mru_lists = NULL;
+}
diff --git a/drivers/video/tegra/nvmap/nvmap_mru.h b/drivers/video/tegra/nvmap/nvmap_mru.h
new file mode 100644
index 000000000000..6c94630bc3ef
--- /dev/null
+++ b/drivers/video/tegra/nvmap/nvmap_mru.h
@@ -0,0 +1,84 @@
+/*
+ * drivers/video/tegra/nvmap_mru.c
+ *
+ * IOVMM virtualization support for nvmap
+ *
+ * Copyright (c) 2009-2010, NVIDIA Corporation.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+
+#ifndef __VIDEO_TEGRA_NVMAP_MRU_H
+#define __VIDEO_TEGRA_NVMAP_MRU_H
+
+#include <linux/spinlock.h>
+
+#include "nvmap.h"
+
+struct tegra_iovmm_area;
+struct tegra_iovmm_client;
+
+#ifdef CONFIG_NVMAP_RECLAIM_UNPINNED_VM
+
+static inline void nvmap_mru_lock(struct nvmap_share *share)
+{
+	mutex_lock(&share->mru_lock);
+}
+
+static inline void nvmap_mru_unlock(struct nvmap_share *share)
+{
+	mutex_unlock(&share->mru_lock);
+}
+
+int nvmap_mru_init(struct nvmap_share *share);
+
+void nvmap_mru_destroy(struct nvmap_share *share);
+
+size_t nvmap_mru_vm_size(struct tegra_iovmm_client *iovmm);
+
+void nvmap_mru_insert_locked(struct nvmap_share *share, struct nvmap_handle *h);
+
+void nvmap_mru_remove(struct nvmap_share *s, struct nvmap_handle *h);
+
+struct tegra_iovmm_area *nvmap_handle_iovmm_locked(struct nvmap_client *c,
+					    struct nvmap_handle *h);
+
+#else
+
+#define nvmap_mru_lock(_s)	do { } while (0)
+#define nvmap_mru_unlock(_s)	do { } while (0)
+#define nvmap_mru_init(_s)	0
+#define nvmap_mru_destroy(_s)	do { } while (0)
+#define nvmap_mru_vm_size(_a)	tegra_iovmm_get_vm_size(_a)
+
+static inline void nvmap_mru_insert_locked(struct nvmap_share *share,
+					   struct nvmap_handle *h)
+{ }
+
+static inline void nvmap_mru_remove(struct nvmap_share *s,
+				    struct nvmap_handle *h)
+{ }
+
+static inline struct tegra_iovmm_area *nvmap_handle_iovmm_locked(struct nvmap_client *c,
+							  struct nvmap_handle *h)
+{
+	BUG_ON(!h->pgalloc.area);
+	return h->pgalloc.area;
+}
+
+#endif
+
+#endif
diff --git a/drivers/video/via/share.h b/drivers/video/via/share.h
index 61b0bd596b85..1603023e3aa5 100644
--- a/drivers/video/via/share.h
+++ b/drivers/video/via/share.h
@@ -557,8 +557,8 @@
 #define M1200X720_R60_VSP       POSITIVE
 
 /* 1200x900@60 Sync Polarity (DCON) */
-#define M1200X900_R60_HSP       NEGATIVE
-#define M1200X900_R60_VSP       NEGATIVE
+#define M1200X900_R60_HSP       POSITIVE
+#define M1200X900_R60_VSP       POSITIVE
 
 /* 1280x600@60 Sync Polarity (GTF Mode) */
 #define M1280x600_R60_HSP       NEGATIVE
diff --git a/drivers/video/via/via_modesetting.h b/drivers/video/via/via_modesetting.h
index ae35cfdeb37c..013884543e91 100644
--- a/drivers/video/via/via_modesetting.h
+++ b/drivers/video/via/via_modesetting.h
@@ -28,6 +28,11 @@
 
 #include <linux/types.h>
 
+
+#define VIA_PITCH_SIZE	(1<<3)
+#define VIA_PITCH_MAX	0x3FF8
+
+
 void via_set_primary_address(u32 addr);
 void via_set_secondary_address(u32 addr);
 void via_set_primary_pitch(u32 pitch);
diff --git a/drivers/video/via/viafbdev.c b/drivers/video/via/viafbdev.c
index 53aa4430d86e..09fa57cea844 100644
--- a/drivers/video/via/viafbdev.c
+++ b/drivers/video/via/viafbdev.c
@@ -151,7 +151,8 @@ static void viafb_update_fix(struct fb_info *info)
 
 	info->fix.visual =
 		bpp == 8 ? FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_TRUECOLOR;
-	info->fix.line_length = (info->var.xres_virtual * bpp / 8 + 7) & ~7;
+	info->fix.line_length = ALIGN(info->var.xres_virtual * bpp / 8,
+		VIA_PITCH_SIZE);
 }
 
 static void viafb_setup_fixinfo(struct fb_fix_screeninfo *fix,
@@ -238,8 +239,12 @@ static int viafb_check_var(struct fb_var_screeninfo *var,
 		depth = 24;
 
 	viafb_fill_var_color_info(var, depth);
-	line = (var->xres_virtual * var->bits_per_pixel / 8 + 7) & ~7;
-	if (line * var->yres_virtual > ppar->memsize)
+	if (var->xres_virtual < var->xres)
+		var->xres_virtual = var->xres;
+
+	line = ALIGN(var->xres_virtual * var->bits_per_pixel / 8,
+		VIA_PITCH_SIZE);
+	if (line > VIA_PITCH_MAX || line * var->yres_virtual > ppar->memsize)
 		return -EINVAL;
 
 	/* Based on var passed in to calculate the refresh,
@@ -348,8 +353,9 @@ static int viafb_pan_display(struct fb_var_screeninfo *var,
 	struct fb_info *info)
 {
 	struct viafb_par *viapar = info->par;
-	u32 vram_addr = (var->yoffset * var->xres_virtual + var->xoffset)
-		* (var->bits_per_pixel / 8) + viapar->vram_addr;
+	u32 vram_addr = viapar->vram_addr
+		+ var->yoffset * info->fix.line_length
+		+ var->xoffset * info->var.bits_per_pixel / 8;
 
 	DEBUG_MSG(KERN_DEBUG "viafb_pan_display, address = %d\n", vram_addr);
 	if (!viafb_dual_fb) {
diff --git a/drivers/virtio/virtio_pci.c b/drivers/virtio/virtio_pci.c
index 4bcc8b82640b..ecb925411e09 100644
--- a/drivers/virtio/virtio_pci.c
+++ b/drivers/virtio/virtio_pci.c
@@ -590,11 +590,11 @@ static struct virtio_config_ops virtio_pci_config_ops = {
 
 static void virtio_pci_release_dev(struct device *_d)
 {
-	struct virtio_device *dev = container_of(_d, struct virtio_device,
-						 dev);
-	struct virtio_pci_device *vp_dev = to_vp_device(dev);
-
-	kfree(vp_dev);
+	/*
+	 * No need for a release method as we allocate/free
+	 * all devices together with the pci devices.
+	 * Provide an empty one to avoid getting a warning from core.
+	 */
 }
 
 /* the PCI probing function */
@@ -682,6 +682,7 @@ static void __devexit virtio_pci_remove(struct pci_dev *pci_dev)
 	pci_iounmap(pci_dev, vp_dev->ioaddr);
 	pci_release_regions(pci_dev);
 	pci_disable_device(pci_dev);
+	kfree(vp_dev);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/w1/Makefile b/drivers/w1/Makefile
index 6bb0b54965f2..ae25ae5fdaf3 100644
--- a/drivers/w1/Makefile
+++ b/drivers/w1/Makefile
@@ -1,6 +1,7 @@
 #
 # Makefile for the Dallas's 1-wire bus.
 #
+GCOV_PROFILE := y
 
 obj-$(CONFIG_W1)	+= wire.o
 wire-objs		:= w1.o w1_int.o w1_family.o w1_netlink.o w1_io.o
diff --git a/drivers/w1/masters/Kconfig b/drivers/w1/masters/Kconfig
index 979d6eed9a0f..dd278e51ed88 100644
--- a/drivers/w1/masters/Kconfig
+++ b/drivers/w1/masters/Kconfig
@@ -58,6 +58,13 @@ config W1_MASTER_GPIO
 	  This support is also available as a module.  If so, the module
 	  will be called w1-gpio.
 
+config W1_MASTER_TEGRA
+	tristate "NVidia Tegra SoC 1-wire busmaster"
+	depends on ARCH_TEGRA
+	help
+	  Say Y here if you want to communicate with your 1-wire devices using
+	  the NVidia Tegra SoC one-wire interfaces.
+
 config HDQ_MASTER_OMAP
 	tristate "OMAP HDQ driver"
 	depends on SOC_OMAP2430 || ARCH_OMAP3
diff --git a/drivers/w1/masters/Makefile b/drivers/w1/masters/Makefile
index c5a3e96fcbab..1f05328a7013 100644
--- a/drivers/w1/masters/Makefile
+++ b/drivers/w1/masters/Makefile
@@ -1,6 +1,7 @@
 #
 # Makefile for 1-wire bus master drivers.
 #
+GCOV_PROFILE_tegra_w1.o := y
 
 obj-$(CONFIG_W1_MASTER_MATROX)		+= matrox_w1.o
 obj-$(CONFIG_W1_MASTER_DS2490)		+= ds2490.o
@@ -10,3 +11,5 @@ obj-$(CONFIG_W1_MASTER_MXC)		+= mxc_w1.o
 obj-$(CONFIG_W1_MASTER_DS1WM)		+= ds1wm.o
 obj-$(CONFIG_W1_MASTER_GPIO)		+= w1-gpio.o
 obj-$(CONFIG_HDQ_MASTER_OMAP)		+= omap_hdq.o
+CFLAGS_tegra_w1.o			 = -Werror
+obj-$(CONFIG_W1_MASTER_TEGRA)		+= tegra_w1.o
diff --git a/drivers/w1/masters/tegra_w1.c b/drivers/w1/masters/tegra_w1.c
new file mode 100644
index 000000000000..9443c4b1dbc6
--- /dev/null
+++ b/drivers/w1/masters/tegra_w1.c
@@ -0,0 +1,491 @@
+/*
+ * drivers/w1/masters/tegra-w1.c
+ *
+ * W1 master driver for internal OWR controllers in NVIDIA Tegra SoCs.
+ *
+ * Copyright (C) 2010 Motorola, Inc
+ * Author: Andrei Warkentin <andreiw@motorola.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+
+#include <mach/w1.h>
+
+#include "../w1.h"
+#include "../w1_int.h"
+#include "../w1_log.h"
+
+#define DRIVER_NAME "tegra_w1"
+
+/* OWR_CONTROL_0 is the main control register, and should be configured
+   last after configuring all other settings. */
+#define OWR_CONTROL          (0x0)
+#define OC_RD_BIT            (1 << 31)
+#define OC_WR0_BIT           (1 << 30)
+#define OC_RD_SCLK_SHIFT     (23)
+#define OC_RD_SCLK_MASK      (0xF)
+#define OC_P_SCLK_SHIFT      (15)
+#define OC_P_SCLK_MASK       (0xFF)
+#define OC_BIT_XMODE         (1 << 2)
+#define OC_GO                (1 << 0)
+
+/* OWR_WR_RD_TCTL_0 controls read/write timings. */
+#define OWR_WR_RD_TCTL       (0xc)
+#define ORWT_TSU_SHIFT       (28)
+#define ORWT_TSU_MASK        (0x3)
+#define ORWT_TRELEASE_SHIFT  (22)
+#define ORWT_TRELEASE_MASK   (0x3F)
+#define ORWT_TRDV_SHIFT      (18)
+#define ORWT_TRDV_MASK       (0xF)
+#define ORWT_TLOW0_SHIFT     (11)
+#define ORWT_TLOW0_MASK      (0x7F)
+#define ORWT_TLOW1_SHIFT     (7)
+#define ORWT_TLOW1_MASK      (0xF)
+#define ORWT_TSLOT_SHIFT     (0)
+#define ORWT_TSLOT_MASK      (0x7F)
+
+/* OWR_RST_PRES_TCTL_0 controls reset presence timings. */
+#define OWR_RST_PRES_TCTL    (0x10)
+#define ORPT_TPDL_SHIFT      (24)
+#define ORPT_TPDL_MASK       (0xFF)
+#define ORPT_TPDH_SHIFT      (18)
+#define ORPT_TPDH_MASK       (0x3F)
+#define ORPT_TRSTL_SHIFT     (9)
+#define ORPT_TRSTL_MASK      (0x1FF)
+#define ORPT_TRSTH_SHIFT     (0)
+#define ORPT_TRSTH_MASK      (0x1FF)
+
+/* OWR_INTR_MASK_0 stores the masks for the interrupts. */
+#define OWR_INTR_MASK         (0x24)
+#define OI_BIT_XFER_DONE      (1 << 13)
+#define OI_PRESENCE_DONE      (1 << 5)
+#define OI_PRESENCE_ERR       (1 << 0)
+
+/* OWR_INTR_STATUS_0 is the interrupt status register. */
+#define OWR_INTR_STATUS       (0x28)
+
+/* OWR_STATUS_0 is the status register. */
+#define OWR_STATUS            (0x34)
+#define OS_READ_BIT_SHIFT     (23)
+#define OS_RDY                (1 << 0)
+
+/* Transfer_completion wait time. */
+#define BIT_XFER_COMPLETION_TIMEOUT_MSEC (5000)
+
+/* Errors in the interrupt status register for bit
+   transfers. */
+#define BIT_XFER_ERRORS (OI_PRESENCE_ERR)
+
+/* OWR requires 1MHz clock. This value is in Herz. */
+#define OWR_CLOCK (1000000)
+
+#define W1_ERR(format, ...) \
+	printk(KERN_ERR "(%s: line %d) " format, \
+	__func__, __LINE__, ## __VA_ARGS__)
+
+struct tegra_device {
+	bool ready;
+	struct w1_bus_master bus_master;
+	struct clk *clk;
+	void __iomem *ioaddr;
+	struct mutex mutex;
+	spinlock_t spinlock;
+	struct completion *transfer_completion;
+	unsigned long intr_status;
+	struct tegra_w1_timings *timings;
+};
+
+/* If debug_print & DEBUG_PRESENCE, print whether slaves detected
+   or not in reset_bus. */
+#define DEBUG_PRESENCE (0x1)
+
+/* If debug_print & DEBUG_TIMEOUT, print whether timeouts on waiting
+   for device interrupts occurs. */
+#define DEBUG_TIMEOUT (0x2)
+
+static uint32_t debug_print;
+module_param_named(debug, debug_print, uint, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Debugging output commands:\n"
+		 "\tbit 0 - log reset_bus presence detects\n"
+		 "\tbit 1 - log interrupt timeouts\n");
+
+/* Reads the OWR register specified by base offset in 'reg'. */
+static inline unsigned long w1_readl(struct tegra_device *dev,
+				     unsigned long reg)
+{
+	return readl(dev->ioaddr + reg);
+}
+
+/* Writes 'val' into the OWR registers specified by base offset in 'reg'. */
+static inline void w1_writel(struct tegra_device *dev, unsigned long val,
+			     unsigned long reg)
+{
+	writel(val, dev->ioaddr + reg);
+}
+
+/* Sets interrupt mask the device. */
+static inline void w1_imask(struct tegra_device *dev, unsigned long mask)
+{
+	w1_writel(dev, mask, OWR_INTR_MASK);
+}
+
+/* Waits for completion of a bit transfer, checks intr_status against
+   BIT_XFER_ERRORS and an additional provided bit mask. */
+static inline int w1_wait(struct tegra_device *dev, unsigned long mask)
+{
+	int ret;
+	unsigned long irq_flags;
+	unsigned long intr_status;
+
+	ret = wait_for_completion_timeout(dev->transfer_completion,
+		msecs_to_jiffies(BIT_XFER_COMPLETION_TIMEOUT_MSEC));
+
+	if (unlikely(!ret)) {
+		if (debug_print & DEBUG_TIMEOUT)
+			W1_ERR("timeout\n");
+		return -ETIME;
+	}
+
+	spin_lock_irqsave(&dev->spinlock, irq_flags);
+	intr_status = dev->intr_status;
+	dev->intr_status = 0;
+	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
+
+	if (unlikely(intr_status & BIT_XFER_ERRORS ||
+		     !(intr_status & mask)))
+		return -EIO;
+	return 0;
+}
+
+/* Programs timing registers, and puts the device into a known state.
+   Interrupts are safe to enable past this point. */
+static int w1_setup(struct tegra_device *dev)
+{
+	unsigned long value;
+	clk_enable(dev->clk);
+
+	value =
+		((dev->timings->tslot & ORWT_TSLOT_MASK) << ORWT_TSLOT_SHIFT) |
+		((dev->timings->tlow1 & ORWT_TLOW1_MASK) << ORWT_TLOW1_SHIFT) |
+		((dev->timings->tlow0 & ORWT_TLOW0_MASK) << ORWT_TLOW0_SHIFT) |
+		((dev->timings->trdv & ORWT_TRDV_MASK) << ORWT_TRDV_SHIFT) |
+		((dev->timings->trelease & ORWT_TRELEASE_MASK) <<
+		 ORWT_TRELEASE_SHIFT) |
+		((dev->timings->tsu & ORWT_TSU_MASK) << ORWT_TSU_SHIFT);
+	w1_writel(dev, value, OWR_WR_RD_TCTL);
+
+	value =
+		((dev->timings->trsth & ORPT_TRSTH_MASK) << ORPT_TRSTH_SHIFT) |
+		((dev->timings->trstl & ORPT_TRSTL_MASK) << ORPT_TRSTL_SHIFT) |
+		((dev->timings->tpdh & ORPT_TPDH_MASK) << ORPT_TPDH_SHIFT) |
+		((dev->timings->tpdl & ORPT_TPDL_MASK) << ORPT_TPDL_SHIFT);
+	w1_writel(dev, value, OWR_RST_PRES_TCTL);
+
+	/* Clear interrupt status/mask registers in case
+	   anything was set in it. */
+	w1_imask(dev, 0);
+	w1_writel(dev, 0xFFFFFFFF, OWR_INTR_STATUS);
+	clk_disable(dev->clk);
+	return 0;
+}
+
+/* Interrupt handler for OWR communication. */
+static irqreturn_t tegra_w1_irq(int irq, void *cookie)
+{
+	unsigned long irq_flags;
+	unsigned long status;
+	struct tegra_device *dev = cookie;
+
+	status = w1_readl(dev, OWR_INTR_STATUS);
+	if (unlikely(!status)) {
+
+		/* Not for me if no status bits are set. */
+		return IRQ_NONE;
+	}
+
+	spin_lock_irqsave(&dev->spinlock, irq_flags);
+
+	if (likely(dev->transfer_completion)) {
+		dev->intr_status = status;
+		w1_writel(dev, status, OWR_INTR_STATUS);
+		complete(dev->transfer_completion);
+	} else {
+		W1_ERR("spurious interrupt, status = 0x%lx\n", status);
+	}
+
+	spin_unlock_irqrestore(&dev->spinlock, irq_flags);
+	return IRQ_HANDLED;
+}
+
+/* Perform a write-0 cycle if bit == 0, otherwise
+   perform a read cycle. */
+static u8 tegra_w1_touch_bit(void *data, u8 bit)
+{
+	int rc;
+	u8 return_bit;
+	unsigned long control;
+	DECLARE_COMPLETION_ONSTACK(touch_done);
+	struct tegra_device *dev = (struct tegra_device *) data;
+
+	return_bit = 0;
+	mutex_lock(&dev->mutex);
+	if (!dev->ready)
+		goto done;
+
+	clk_enable(dev->clk);
+	w1_imask(dev, OI_BIT_XFER_DONE);
+	dev->transfer_completion = &touch_done;
+	control =
+		((dev->timings->rdsclk & OC_RD_SCLK_MASK) << OC_RD_SCLK_SHIFT) |
+		((dev->timings->psclk & OC_P_SCLK_MASK) << OC_P_SCLK_SHIFT) |
+		OC_BIT_XMODE;
+
+	/* Read bit (well, writes a 1 to the bus as well). */
+	if (bit) {
+		w1_writel(dev, control | OC_RD_BIT, OWR_CONTROL);
+		rc = w1_wait(dev, OI_BIT_XFER_DONE);
+
+		if (rc) {
+			W1_ERR("write-1/read failed\n");
+			goto done;
+		}
+
+		return_bit =
+		    (w1_readl(dev, OWR_STATUS) >> OS_READ_BIT_SHIFT) & 1;
+
+	}
+
+	/* Write 0. */
+	else {
+		w1_writel(dev, control | OC_WR0_BIT, OWR_CONTROL);
+		rc = w1_wait(dev, OI_BIT_XFER_DONE);
+		if (rc) {
+			W1_ERR("write-0 failed\n");
+			goto done;
+		}
+	}
+
+done:
+
+	w1_imask(dev, 0);
+	dev->transfer_completion = NULL;
+	clk_disable(dev->clk);
+	mutex_unlock(&dev->mutex);
+	return return_bit;
+}
+
+/* Performs a bus reset cycle, and returns 0 if slaves present. */
+static u8 tegra_w1_reset_bus(void *data)
+{
+	int rc;
+	int presence;
+	unsigned long value;
+	DECLARE_COMPLETION_ONSTACK(reset_done);
+	struct tegra_device *dev = (struct tegra_device *) data;
+
+	presence = 1;
+	mutex_lock(&dev->mutex);
+	if (!dev->ready)
+		goto done;
+
+	clk_enable(dev->clk);
+	w1_imask(dev, OI_PRESENCE_DONE);
+	dev->transfer_completion = &reset_done;
+	value =
+	    ((dev->timings->rdsclk & OC_RD_SCLK_MASK) << OC_RD_SCLK_SHIFT) |
+	    ((dev->timings->psclk & OC_P_SCLK_MASK) << OC_P_SCLK_SHIFT) |
+	    OC_BIT_XMODE | OC_GO;
+	w1_writel(dev, value, OWR_CONTROL);
+
+	rc = w1_wait(dev, OI_PRESENCE_DONE);
+	if (rc)
+		goto done;
+
+	presence = 0;
+done:
+
+	if (debug_print & DEBUG_PRESENCE) {
+		if (presence)
+			W1_ERR("no slaves present\n");
+		else
+			W1_ERR("slaves present\n");
+	}
+
+	w1_imask(dev, 0);
+	dev->transfer_completion = NULL;
+	clk_disable(dev->clk);
+	mutex_unlock(&dev->mutex);
+	return presence;
+}
+
+static int tegra_w1_probe(struct platform_device *pdev)
+{
+	int rc;
+	int irq;
+	struct resource *res;
+	struct tegra_device *dev;
+	struct tegra_w1_platform_data *plat = pdev->dev.platform_data;
+
+	printk(KERN_INFO "Driver for Tegra SoC 1-wire controller\n");
+
+	if (plat == NULL || plat->timings == NULL)
+		return -ENXIO;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL)
+		return -ENODEV;
+
+	irq = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL)
+		return -ENODEV;
+
+	dev = kzalloc(sizeof(struct tegra_device), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, dev);
+	dev->clk = clk_get(&pdev->dev, plat->clk_id);
+	if (IS_ERR(dev->clk)) {
+		rc = PTR_ERR(dev->clk);
+		goto cleanup_alloc;
+	}
+
+	/* OWR requires 1MHz clock. */
+	rc = clk_set_rate(dev->clk, OWR_CLOCK);
+	if (rc)
+		goto cleanup_clock;
+
+	if (!request_mem_region
+	    (res->start, res->end - res->start + 1, dev_name(&pdev->dev))) {
+		rc = -EBUSY;
+		goto cleanup_clock;
+	}
+
+	dev->ioaddr = ioremap(res->start, res->end - res->start + 1);
+	if (!dev->ioaddr) {
+		rc = -ENOMEM;
+		goto cleanup_reqmem;
+	}
+
+	dev->timings = plat->timings;
+	dev->bus_master.data = dev;
+	dev->bus_master.touch_bit = tegra_w1_touch_bit;
+	dev->bus_master.reset_bus = tegra_w1_reset_bus;
+
+	spin_lock_init(&dev->spinlock);
+	mutex_init(&dev->mutex);
+
+	/* Program device into known state. */
+	w1_setup(dev);
+
+	rc = request_irq(irq, tegra_w1_irq, IRQF_SHARED, DRIVER_NAME, dev);
+	if (rc)
+		goto cleanup_ioremap;
+
+	rc = w1_add_master_device(&dev->bus_master);
+	if (rc)
+		goto cleanup_irq;
+
+	dev->ready = true;
+	return 0;
+
+cleanup_irq:
+	free_irq(irq, dev);
+cleanup_ioremap:
+	iounmap(dev->ioaddr);
+cleanup_reqmem:
+	release_mem_region(res->start,
+			   res->end - res->start + 1);
+cleanup_clock:
+	clk_put(dev->clk);
+cleanup_alloc:
+	platform_set_drvdata(pdev, NULL);
+	kfree(dev);
+	return rc;
+}
+
+static int tegra_w1_remove(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct tegra_device *dev = platform_get_drvdata(pdev);
+
+	mutex_lock(&dev->mutex);
+	dev->ready = false;
+	mutex_unlock(&dev->mutex);
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	free_irq(res->start, dev);
+	iounmap(dev->ioaddr);
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	release_mem_region(res->start, res->end - res->start + 1);
+	clk_put(dev->clk);
+	platform_set_drvdata(pdev, NULL);
+	kfree(dev);
+	return 0;
+}
+
+static int tegra_w1_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	return 0;
+}
+
+static int tegra_w1_resume(struct platform_device *pdev)
+{
+	struct tegra_device *dev = platform_get_drvdata(pdev);
+
+	/* TODO: Is this necessary? I would assume yes. */
+	w1_setup(dev);
+	return 0;
+}
+
+static struct platform_driver tegra_w1_driver = {
+	.probe = tegra_w1_probe,
+	.remove = tegra_w1_remove,
+	.suspend = tegra_w1_suspend,
+	.resume = tegra_w1_resume,
+	.driver = {
+		   .name = DRIVER_NAME,
+		   .owner = THIS_MODULE,
+	},
+};
+
+static int __init tegra_w1_init(void)
+{
+	return platform_driver_register(&tegra_w1_driver);
+}
+
+static void __exit tegra_w1_exit(void)
+{
+	platform_driver_unregister(&tegra_w1_driver);
+}
+
+module_init(tegra_w1_init);
+module_exit(tegra_w1_exit);
+
+MODULE_DESCRIPTION("Tegra W1 master driver");
+MODULE_AUTHOR("Andrei Warkentin <andreiw@motorola.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/w1/slaves/w1_ds2780.c b/drivers/w1/slaves/w1_ds2780.c
index 274c8f38303f..505b17d8c67c 100644
--- a/drivers/w1/slaves/w1_ds2780.c
+++ b/drivers/w1/slaves/w1_ds2780.c
@@ -26,20 +26,14 @@
 #include "../w1_family.h"
 #include "w1_ds2780.h"
 
-int w1_ds2780_io(struct device *dev, char *buf, int addr, size_t count,
-			int io)
+static int w1_ds2780_do_io(struct device *dev, char *buf, int addr,
+			size_t count, int io)
 {
 	struct w1_slave *sl = container_of(dev, struct w1_slave, dev);
 
-	if (!dev)
-		return -ENODEV;
+	if (addr > DS2780_DATA_SIZE || addr < 0)
+		return 0;
 
-	mutex_lock(&sl->master->mutex);
-
-	if (addr > DS2780_DATA_SIZE || addr < 0) {
-		count = 0;
-		goto out;
-	}
 	count = min_t(int, count, DS2780_DATA_SIZE - addr);
 
 	if (w1_reset_select_slave(sl) == 0) {
@@ -47,7 +41,6 @@ int w1_ds2780_io(struct device *dev, char *buf, int addr, size_t count,
 			w1_write_8(sl->master, W1_DS2780_WRITE_DATA);
 			w1_write_8(sl->master, addr);
 			w1_write_block(sl->master, buf, count);
-			/* XXX w1_write_block returns void, not n_written */
 		} else {
 			w1_write_8(sl->master, W1_DS2780_READ_DATA);
 			w1_write_8(sl->master, addr);
@@ -55,13 +48,42 @@ int w1_ds2780_io(struct device *dev, char *buf, int addr, size_t count,
 		}
 	}
 
-out:
+	return count;
+}
+
+int w1_ds2780_io(struct device *dev, char *buf, int addr, size_t count,
+			int io)
+{
+	struct w1_slave *sl = container_of(dev, struct w1_slave, dev);
+	int ret;
+
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&sl->master->mutex);
+
+	ret = w1_ds2780_do_io(dev, buf, addr, count, io);
+
 	mutex_unlock(&sl->master->mutex);
 
-	return count;
+	return ret;
 }
 EXPORT_SYMBOL(w1_ds2780_io);
 
+int w1_ds2780_io_nolock(struct device *dev, char *buf, int addr, size_t count,
+			int io)
+{
+	int ret;
+
+	if (!dev)
+		return -ENODEV;
+
+	ret = w1_ds2780_do_io(dev, buf, addr, count, io);
+
+	return ret;
+}
+EXPORT_SYMBOL(w1_ds2780_io_nolock);
+
 int w1_ds2780_eeprom_cmd(struct device *dev, int addr, int cmd)
 {
 	struct w1_slave *sl = container_of(dev, struct w1_slave, dev);
diff --git a/drivers/w1/slaves/w1_ds2780.h b/drivers/w1/slaves/w1_ds2780.h
index a1fba79eb1b5..737379365021 100644
--- a/drivers/w1/slaves/w1_ds2780.h
+++ b/drivers/w1/slaves/w1_ds2780.h
@@ -124,6 +124,8 @@
 
 extern int w1_ds2780_io(struct device *dev, char *buf, int addr, size_t count,
 			int io);
+extern int w1_ds2780_io_nolock(struct device *dev, char *buf, int addr,
+			size_t count, int io);
 extern int w1_ds2780_eeprom_cmd(struct device *dev, int addr, int cmd);
 
 #endif /* !_W1_DS2780_H */
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index 86b0735e6aa0..8d41441b05ed 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -212,6 +212,26 @@ config MPCORE_WATCHDOG
 	  To compile this driver as a module, choose M here: the
 	  module will be called mpcore_wdt.
 
+config TEGRA_WATCHDOG
+	tristate "Tegra watchdog"
+	depends on ARCH_TEGRA
+	help
+	  Say Y here to include support for the watchdog timer
+	  embedded in NVIDIA Tegra SoCs. The default timeout
+	  is set to 60 seconds.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tegra_wdt.
+
+config TEGRA_WATCHDOG_ENABLE_HEARTBEAT
+	bool "Use the first Tegra watchdog as kernel heartbeats"
+	depends on TEGRA_WATCHDOG
+	default n
+	help
+	  Say Y here to enable the first tegra watchdog at driver
+	  probe time. The kernel will reload the watchdog by
+	  itself in an interrupt handler.
+
 config EP93XX_WATCHDOG
 	tristate "EP93xx Watchdog"
 	depends on ARCH_EP93XX
diff --git a/drivers/watchdog/Makefile b/drivers/watchdog/Makefile
index 55bd5740e910..9c6cc30bb905 100644
--- a/drivers/watchdog/Makefile
+++ b/drivers/watchdog/Makefile
@@ -1,6 +1,7 @@
 #
 # Makefile for the WatchDog device drivers.
 #
+GCOV_PROFILE_tegra_wdt.o := y
 
 # The WatchDog Timer Driver Core.
 watchdog-objs	+= watchdog_core.o watchdog_dev.o
@@ -43,6 +44,8 @@ obj-$(CONFIG_S3C2410_WATCHDOG) += s3c2410_wdt.o
 obj-$(CONFIG_SA1100_WATCHDOG) += sa1100_wdt.o
 obj-$(CONFIG_DW_WATCHDOG) += dw_wdt.o
 obj-$(CONFIG_MPCORE_WATCHDOG) += mpcore_wdt.o
+CFLAGS_tegra_wdt.o	= -Werror
+obj-$(CONFIG_TEGRA_WATCHDOG) += tegra_wdt.o
 obj-$(CONFIG_EP93XX_WATCHDOG) += ep93xx_wdt.o
 obj-$(CONFIG_PNX4008_WATCHDOG) += pnx4008_wdt.o
 obj-$(CONFIG_IOP_WATCHDOG) += iop_wdt.o
diff --git a/drivers/watchdog/hpwdt.c b/drivers/watchdog/hpwdt.c
index 809cbda03d7a..179597713516 100644
--- a/drivers/watchdog/hpwdt.c
+++ b/drivers/watchdog/hpwdt.c
@@ -230,6 +230,7 @@ static int __devinit cru_detect(unsigned long map_entry,
 
 	cmn_regs.u1.reax = CRU_BIOS_SIGNATURE_VALUE;
 
+	set_memory_x((unsigned long)bios32_entrypoint, (2 * PAGE_SIZE));
 	asminline_call(&cmn_regs, bios32_entrypoint);
 
 	if (cmn_regs.u1.ral != 0) {
@@ -247,8 +248,10 @@ static int __devinit cru_detect(unsigned long map_entry,
 		if ((physical_bios_base + physical_bios_offset)) {
 			cru_rom_addr =
 				ioremap(cru_physical_address, cru_length);
-			if (cru_rom_addr)
+			if (cru_rom_addr) {
+				set_memory_x((unsigned long)cru_rom_addr, cru_length);
 				retval = 0;
+			}
 		}
 
 		printk(KERN_DEBUG "hpwdt: CRU Base Address:   0x%lx\n",
diff --git a/drivers/watchdog/tegra_wdt.c b/drivers/watchdog/tegra_wdt.c
new file mode 100644
index 000000000000..3e58a984d9c2
--- /dev/null
+++ b/drivers/watchdog/tegra_wdt.c
@@ -0,0 +1,676 @@
+/*
+ * drivers/watchdog/tegra_wdt.c
+ *
+ * watchdog driver for NVIDIA tegra internal watchdog
+ *
+ * Copyright (c) 2012-2014, NVIDIA Corporation. All rights reserved.
+ * Copyright (c) 2013, Toradex AG
+ *
+ * based on drivers/watchdog/softdog.c and drivers/watchdog/omap_wdt.c
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/miscdevice.h>
+#include <linux/notifier.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/uaccess.h>
+#include <linux/watchdog.h>
+#ifdef CONFIG_TEGRA_FIQ_DEBUGGER
+#include <mach/irqs.h>
+#endif
+
+/* minimum and maximum watchdog trigger periods, in seconds */
+#define MIN_WDT_PERIOD	5
+#define MAX_WDT_PERIOD	1000
+
+/* Assign Timer 7 to Timer 10 for WDT0 to WDT3, respectively */
+#define TMR_SRC_START	7
+/*
+ * For spinlock lockup detection to work, the heartbeat should be 2*lockup
+ * for cases where the spinlock disabled irqs.
+ * Must be between MIN_WDT_PERIOD and MAX_WDT_PERIOD
+ */
+#define WDT_DEFAULT_TIME 60
+
+static int heartbeat = WDT_DEFAULT_TIME;
+static bool nowayout = WATCHDOG_NOWAYOUT;
+
+module_param(heartbeat, int, 0);
+MODULE_PARM_DESC(heartbeat, "Watchdog heartbeat period in seconds (default="
+		__MODULE_STRING(WDT_DEFAULT_TIME));
+
+#ifdef CONFIG_WATCHDOG_NOWAYOUT
+module_param(nowayout, bool, 0);
+MODULE_PARM_DESC(nowayout, "Watchdog cannot be stopped once started (default="
+				__MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
+#endif
+
+enum tegra_wdt_status {
+	WDT_DISABLED = 1 << 0,
+	WDT_ENABLED = 1 << 1,
+	WDT_KERNEL_HEARTBEAT = 1 << 2,
+};
+
+struct tegra_wdt {
+	struct miscdevice	miscdev;
+	struct notifier_block	notifier;
+	struct resource		*res_src;
+	struct resource		*res_wdt;
+	struct resource		*res_int_base;
+	unsigned long		users;
+	void __iomem		*wdt_source;
+	void __iomem		*wdt_timer;
+	void __iomem		*int_base;
+	int			tmrsrc;
+	int			timeout;
+	int			status;
+	int			way_out_ok;
+	struct tasklet_struct	tasklet;
+	int			irq;
+	int			irq_counter;
+};
+
+#if defined(CONFIG_ARCH_TEGRA_2x_SOC)
+
+#define TIMER_PTV		0x0
+ #define TIMER_EN		(1 << 31)
+ #define TIMER_PERIODIC		(1 << 30)
+#define TIMER_PCR		0x4
+ #define TIMER_PCR_INTR		(1 << 30)
+#define WDT_EN			(1 << 5)
+#define WDT_SEL_TMR1		(0 << 4)
+#define WDT_SYS_RST		(1 << 2)
+
+static void tegra_wdt_enable(struct tegra_wdt *wdt)
+{
+	u32 val;
+
+	/* since the watchdog reset occurs when a second interrupt
+	 * is asserted before the first is processed, program the
+	 * timer period to one-half of the watchdog period */
+	val = wdt->timeout * 1000000ul / 2;
+	val |= (TIMER_EN | TIMER_PERIODIC);
+	writel(val, wdt->wdt_timer + TIMER_PTV);
+
+	val = WDT_EN | WDT_SEL_TMR1 | WDT_SYS_RST;
+	writel(val, wdt->wdt_source);
+}
+
+static void tegra_wdt_disable(struct tegra_wdt *wdt)
+{
+	writel(0, wdt->wdt_source);
+	writel(0, wdt->wdt_timer + TIMER_PTV);
+}
+
+static inline void tegra_wdt_ping(struct tegra_wdt *wdt)
+{
+	/* Reset timer */
+	tegra_wdt_enable(wdt);
+
+	/* Reenable IRQ in case an interrupt already happend */
+	if (wdt->irq_counter) {
+		writel(TIMER_PCR_INTR, wdt->wdt_timer + TIMER_PCR);
+		wdt->irq_counter = 0;
+		enable_irq(wdt->irq);
+	}
+
+	return;
+}
+
+static irqreturn_t tegra_wdt_interrupt(int irq, void *dev_id)
+{
+	struct tegra_wdt *wdt = dev_id;
+
+	wdt->irq_counter++;
+
+	if (wdt->status & WDT_KERNEL_HEARTBEAT) {
+		tegra_wdt_ping(wdt);
+	} else {
+		/* If not in heartbeat mode, disable IRQs to avoid IRQ storm
+		 * We don't acknowledge the interrupt here since the user
+		 * did not reload the timer in time. Upon next interrupt
+		 * the system will reset...
+		 */
+		tasklet_schedule(&wdt->tasklet);
+		disable_irq_nosync(irq);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void tegra_wdt_do_tasklet(unsigned long data)
+{
+	struct tegra_wdt *wdt = (struct tegra_wdt *)data;
+	dev_info(wdt->miscdev.parent, "Watchdog interrupt received, system "
+		       "will reset soon if no ping arrives\n");
+}
+
+#elif defined(CONFIG_ARCH_TEGRA_3x_SOC)
+
+#define TIMER_PTV			0
+ #define TIMER_EN			(1 << 31)
+ #define TIMER_PERIODIC			(1 << 30)
+#define TIMER_PCR			0x4
+ #define TIMER_PCR_INTR			(1 << 30)
+#define WDT_CFG				(0)
+ #define WDT_CFG_PERIOD			(1 << 4)
+ #define WDT_CFG_INT_EN			(1 << 12)
+ #define WDT_CFG_FIQ_INT_EN		(1 << 13)
+ #define WDT_CFG_SYS_RST_EN		(1 << 14)
+ #define WDT_CFG_PMC2CAR_RST_EN		(1 << 15)
+#define WDT_STATUS			(4)
+ #define WDT_INTR_STAT			(1 << 1)
+ #define WDT_STATUS_EXPIR_COUNTER	(3 << 12)
+#define WDT_CMD				(8)
+ #define WDT_CMD_START_COUNTER		(1 << 0)
+ #define WDT_CMD_DISABLE_COUNTER	(1 << 1)
+#define WDT_UNLOCK			(0xC)
+ #define WDT_UNLOCK_PATTERN		(0xC45A << 0)
+#define ICTLR_IEP_CLASS			0x2C
+#define MAX_NR_CPU_WDT			0x4
+
+struct tegra_wdt *tegra_wdt[MAX_NR_CPU_WDT];
+
+static inline void tegra_wdt_ping(struct tegra_wdt *wdt)
+{
+	writel(WDT_CMD_START_COUNTER, wdt->wdt_source + WDT_CMD);
+}
+
+#ifdef CONFIG_TEGRA_FIQ_DEBUGGER
+static void tegra_wdt_int_priority(struct tegra_wdt *wdt)
+{
+	unsigned val = 0;
+
+	if (!wdt->int_base)
+		return;
+	val = readl(wdt->int_base + ICTLR_IEP_CLASS);
+	val &= ~(1 << (INT_WDT_CPU & 31));
+	writel(val, wdt->int_base + ICTLR_IEP_CLASS);
+}
+#endif
+
+static void tegra_wdt_enable(struct tegra_wdt *wdt)
+{
+	u32 val;
+
+	writel(TIMER_PCR_INTR, wdt->wdt_timer + TIMER_PCR);
+	val = (wdt->timeout * 1000000ul) / 4;
+	val |= (TIMER_EN | TIMER_PERIODIC);
+	writel(val, wdt->wdt_timer + TIMER_PTV);
+
+	writel(WDT_CMD_START_COUNTER, wdt->wdt_source + WDT_CMD);
+
+	/* Interrupt handler is not required for user space since
+	 * a warning in a fourth of time don't make sense. Then
+	 * the interrupt line is also shared, so it can't be disabled
+	 * if one watchdog is about to expire... (interrupt storm)
+	 * SYS_RST_EN doesnt work as there is no external reset
+	 * from Tegra.
+	 */
+	val = wdt->tmrsrc | WDT_CFG_PERIOD | /* WDT_CFG_INT_EN |  */
+		/*WDT_CFG_SYS_RST_EN |*/ WDT_CFG_PMC2CAR_RST_EN;
+#ifdef CONFIG_TEGRA_FIQ_DEBUGGER
+	val |= WDT_CFG_FIQ_INT_EN;
+#endif
+	writel(val, wdt->wdt_source + WDT_CFG);
+}
+
+static void tegra_wdt_disable(struct tegra_wdt *wdt)
+{
+	writel(WDT_UNLOCK_PATTERN, wdt->wdt_source + WDT_UNLOCK);
+	writel(WDT_CMD_DISABLE_COUNTER, wdt->wdt_source + WDT_CMD);
+
+	writel(0, wdt->wdt_timer + TIMER_PTV);
+}
+
+static void tegra_wdt_interrupt_instance(struct tegra_wdt *wdt)
+{
+	WARN_ON_ONCE(!(wdt->status & WDT_KERNEL_HEARTBEAT));
+
+	if (wdt->status & WDT_KERNEL_HEARTBEAT) {
+		tegra_wdt_ping(wdt);
+	}
+}
+
+static irqreturn_t tegra_wdt_interrupt(int irq, void *dev_id)
+{
+	struct tegra_wdt *wdt = NULL;
+	unsigned i, status;
+
+	for (i = 0; i < MAX_NR_CPU_WDT; i++) {
+		wdt = tegra_wdt[i];
+		if (wdt == NULL)
+			continue;
+		status = readl(wdt->wdt_source + WDT_STATUS);
+		if ((wdt->status & WDT_ENABLED) &&
+			(status & WDT_INTR_STAT)) {
+			tegra_wdt_interrupt_instance(wdt);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+#endif
+
+static int tegra_wdt_notify(struct notifier_block *this,
+			    unsigned long code, void *dev)
+{
+	struct tegra_wdt *wdt = container_of(this, struct tegra_wdt, notifier);
+
+	if (code == SYS_DOWN || code == SYS_HALT)
+		tegra_wdt_disable(wdt);
+	return NOTIFY_DONE;
+}
+
+static int tegra_wdt_open(struct inode *inode, struct file *file)
+{
+	struct miscdevice *mdev = file->private_data;
+	struct tegra_wdt *wdt = container_of(mdev, struct tegra_wdt,
+					     miscdev);
+
+	if (test_and_set_bit(1, &wdt->users))
+		return -EBUSY;
+
+	/* Reset no way out, we need a new magic again */
+	wdt->way_out_ok = 0;
+
+	wdt->status |= WDT_ENABLED;
+	wdt->timeout = heartbeat;
+	tegra_wdt_enable(wdt);
+	file->private_data = wdt;
+	return nonseekable_open(inode, file);
+}
+
+static int tegra_wdt_release(struct inode *inode, struct file *file)
+{
+	struct tegra_wdt *wdt = file->private_data;
+
+	if (wdt->status & WDT_ENABLED && !nowayout) {
+		if (wdt->way_out_ok) {
+			tegra_wdt_disable(wdt);
+			wdt->status = WDT_DISABLED;
+		} else {
+			dev_info(wdt->miscdev.parent, "No Magic Close "
+					"received, watchdog not disabled!\n");
+		}
+	} else if (nowayout) {
+		dev_info(wdt->miscdev.parent, "No way out is "
+				"enabled, watchdog not disabled!\n");
+	}
+	clear_bit(1, &wdt->users);
+	return 0;
+}
+
+static long tegra_wdt_ioctl(struct file *file, unsigned int cmd,
+			    unsigned long arg)
+{
+	struct tegra_wdt *wdt = file->private_data;
+	static DEFINE_SPINLOCK(lock);
+	int new_timeout;
+	int option;
+	static const struct watchdog_info ident = {
+		.identity = "Tegra Watchdog",
+		.options = WDIOF_SETTIMEOUT,
+		.firmware_version = 0,
+	};
+
+	switch (cmd) {
+	case WDIOC_GETSUPPORT:
+		return copy_to_user((struct watchdog_info __user *)arg, &ident,
+				    sizeof(ident));
+	case WDIOC_GETSTATUS:
+	case WDIOC_GETBOOTSTATUS:
+		return put_user(0, (int __user *)arg);
+
+	case WDIOC_KEEPALIVE:
+		spin_lock(&lock);
+		tegra_wdt_ping(wdt);
+		spin_unlock(&lock);
+		return 0;
+
+	case WDIOC_SETTIMEOUT:
+		if (get_user(new_timeout, (int __user *)arg))
+			return -EFAULT;
+		spin_lock(&lock);
+		tegra_wdt_disable(wdt);
+		wdt->timeout = clamp(new_timeout, MIN_WDT_PERIOD, MAX_WDT_PERIOD);
+		tegra_wdt_enable(wdt);
+		spin_unlock(&lock);
+	case WDIOC_GETTIMEOUT:
+		return put_user(wdt->timeout, (int __user *)arg);
+
+	case WDIOC_SETOPTIONS:
+		if (get_user(option, (int __user *)arg))
+			return -EFAULT;
+		spin_lock(&lock);
+		if (option & WDIOS_DISABLECARD && !nowayout) {
+			wdt->status &= ~WDT_ENABLED;
+			wdt->status |= WDT_DISABLED;
+			tegra_wdt_disable(wdt);
+		} else if (option & WDIOS_ENABLECARD) {
+			tegra_wdt_enable(wdt);
+			wdt->status |= WDT_ENABLED;
+			wdt->status &= ~WDT_DISABLED;
+		} else {
+			spin_unlock(&lock);
+			return -EINVAL;
+		}
+		spin_unlock(&lock);
+		return 0;
+	}
+	return -ENOTTY;
+}
+
+static ssize_t tegra_wdt_write(struct file *file, const char __user *data,
+			       size_t len, loff_t *ppos)
+{
+	struct tegra_wdt *wdt = file->private_data;
+
+	/* check if way-out char was written as last data */
+	char c;
+	if (len) {
+		tegra_wdt_ping(wdt);
+
+		if (get_user(c, data + len - 1))
+			return -EFAULT;
+		else
+			wdt->way_out_ok = (('V' == c) ? 1 : 0);
+	}
+
+	return len;
+}
+
+static const struct file_operations tegra_wdt_fops = {
+	.owner		= THIS_MODULE,
+	.llseek		= no_llseek,
+	.write		= tegra_wdt_write,
+	.unlocked_ioctl	= tegra_wdt_ioctl,
+	.open		= tegra_wdt_open,
+	.release	= tegra_wdt_release,
+};
+
+static int tegra_wdt_probe(struct platform_device *pdev)
+{
+	struct resource *res_src, *res_wdt, *res_irq;
+	struct resource	*res_int_base = NULL;
+	struct tegra_wdt *wdt;
+	int ret = 0;
+#ifdef CONFIG_TEGRA_WATCHDOG_ENABLE_HEARTBEAT
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	int is_heartbeat_wdt = 1;
+#else
+	int is_heartbeat_wdt = !(pdev->id);
+#endif
+#endif
+
+	if (pdev->id < -1 && pdev->id > 3) {
+		dev_err(&pdev->dev, "only IDs 3:0 supported\n");
+		return -ENODEV;
+	}
+
+	res_src = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	res_wdt = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+
+	if (!res_src || !res_wdt || (!pdev->id && !res_irq)) {
+		dev_err(&pdev->dev, "incorrect resources\n");
+		return -ENOENT;
+	}
+
+#ifdef CONFIG_TEGRA_FIQ_DEBUGGER
+	res_int_base = platform_get_resource(pdev, IORESOURCE_MEM, 2);
+	if (!pdev->id && !res_int_base) {
+		dev_err(&pdev->dev, "FIQ_DBG: INT base not defined\n");
+		return -ENOENT;
+	}
+#endif
+
+	if (pdev->id == -1 && !res_irq) {
+		dev_err(&pdev->dev, "incorrect irq\n");
+		return -ENOENT;
+	}
+
+	wdt = kzalloc(sizeof(*wdt), GFP_KERNEL);
+	if (!wdt) {
+		dev_err(&pdev->dev, "out of memory\n");
+		return -ENOMEM;
+	}
+
+	wdt->miscdev.parent = &pdev->dev;
+	if (pdev->id == -1) {
+		wdt->miscdev.minor = WATCHDOG_MINOR;
+		wdt->miscdev.name = "watchdog";
+	} else {
+		wdt->miscdev.minor = MISC_DYNAMIC_MINOR;
+		if (pdev->id == 0)
+			wdt->miscdev.name = "watchdog0";
+		else if (pdev->id == 1)
+			wdt->miscdev.name = "watchdog1";
+		else if (pdev->id == 2)
+			wdt->miscdev.name = "watchdog2";
+		else if (pdev->id == 3)
+			wdt->miscdev.name = "watchdog3";
+	}
+	wdt->miscdev.fops = &tegra_wdt_fops;
+
+	wdt->notifier.notifier_call = tegra_wdt_notify;
+
+	res_src = request_mem_region(res_src->start, resource_size(res_src),
+				     pdev->name);
+	res_wdt = request_mem_region(res_wdt->start, resource_size(res_wdt),
+				     pdev->name);
+
+	if (!res_src || !res_wdt) {
+		dev_err(&pdev->dev, "unable to request memory resources\n");
+		ret = -EBUSY;
+		goto fail;
+	}
+
+	wdt->wdt_source = ioremap(res_src->start, resource_size(res_src));
+	wdt->wdt_timer = ioremap(res_wdt->start, resource_size(res_wdt));
+	/* tmrsrc will be used to set WDT_CFG */
+	wdt->tmrsrc = (TMR_SRC_START + pdev->id) % 10;
+	if (!wdt->wdt_source || !wdt->wdt_timer) {
+		dev_err(&pdev->dev, "unable to map registers\n");
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	/* Watchdog of Tegra 3 are not at the reset controller regs */
+	if (readl(wdt->wdt_source) & BIT(12))
+		dev_info(&pdev->dev, "last reset due to watchdog timeout\n");
+#endif
+
+	tegra_wdt_disable(wdt);
+	writel(TIMER_PCR_INTR, wdt->wdt_timer + TIMER_PCR);
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+	writel(WDT_CMD_START_COUNTER, wdt->wdt_source + WDT_CMD);
+#endif
+
+	if (res_irq != NULL) {
+#ifdef CONFIG_TEGRA_FIQ_DEBUGGER
+		/* FIQ debugger enables FIQ priority for INT_WDT_CPU.
+		 * But that will disable IRQ on WDT expiration.
+		 * Reset the priority back to IRQ on INT_WDT_CPU so
+		 * that tegra_wdt_interrupt gets its chance to restart the
+		 * counter before expiration.
+		 */
+		res_int_base = request_mem_region(res_int_base->start,
+						  resource_size(res_int_base),
+						  pdev->name);
+		if (!res_int_base)
+			goto fail;
+		wdt->int_base = ioremap(res_int_base->start,
+					resource_size(res_int_base));
+		if (!wdt->int_base)
+			goto fail;
+		tegra_wdt_int_priority(wdt);
+#endif
+		wdt->irq = -1;
+		ret = request_irq(res_irq->start, tegra_wdt_interrupt,
+				  IRQF_DISABLED, dev_name(&pdev->dev), wdt);
+		if (ret) {
+			dev_err(&pdev->dev, "unable to configure IRQ\n");
+			goto fail;
+		}
+		wdt->irq = res_irq->start;
+	}
+
+	wdt->res_src = res_src;
+	wdt->res_wdt = res_wdt;
+	wdt->res_int_base = res_int_base;
+	wdt->status = WDT_DISABLED;
+
+	ret = register_reboot_notifier(&wdt->notifier);
+	if (ret) {
+		dev_err(&pdev->dev, "cannot register reboot notifier\n");
+		goto fail;
+	}
+
+	ret = misc_register(&wdt->miscdev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register misc device\n");
+		unregister_reboot_notifier(&wdt->notifier);
+		goto fail;
+	}
+
+	platform_set_drvdata(pdev, wdt);
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	tasklet_init(&wdt->tasklet, tegra_wdt_do_tasklet, (unsigned long)wdt);
+#endif
+
+#ifdef CONFIG_TEGRA_WATCHDOG_ENABLE_HEARTBEAT
+	/* Enable first watchdog during probe */
+	if (is_heartbeat_wdt) {
+		wdt->status = WDT_ENABLED | WDT_KERNEL_HEARTBEAT;
+		wdt->timeout = heartbeat;
+		set_bit(1, &wdt->users);
+		tegra_wdt_enable(wdt);
+		pr_info("WDT kernel heartbeat enabled on probe\n");
+	}
+#endif
+
+#ifndef CONFIG_ARCH_TEGRA_2x_SOC
+	tegra_wdt[pdev->id] = wdt;
+#endif
+
+	dev_info(&pdev->dev, "%s done\n", __func__);
+	return 0;
+fail:
+	if (wdt->irq != -1)
+		free_irq(wdt->irq, wdt);
+	if (wdt->wdt_source)
+		iounmap(wdt->wdt_source);
+	if (wdt->wdt_timer)
+		iounmap(wdt->wdt_timer);
+	if (wdt->int_base)
+		iounmap(wdt->int_base);
+	if (res_src)
+		release_mem_region(res_src->start, resource_size(res_src));
+	if (res_wdt)
+		release_mem_region(res_wdt->start, resource_size(res_wdt));
+	if (res_int_base)
+		release_mem_region(res_int_base->start,
+					resource_size(res_int_base));
+	kfree(wdt);
+	return ret;
+}
+
+static int tegra_wdt_remove(struct platform_device *pdev)
+{
+	struct tegra_wdt *wdt = platform_get_drvdata(pdev);
+
+	tegra_wdt_disable(wdt);
+
+	unregister_reboot_notifier(&wdt->notifier);
+	misc_deregister(&wdt->miscdev);
+	if (wdt->irq != -1)
+		free_irq(wdt->irq, wdt);
+	iounmap(wdt->wdt_source);
+	iounmap(wdt->wdt_timer);
+	if (wdt->int_base)
+		iounmap(wdt->int_base);
+	release_mem_region(wdt->res_src->start, resource_size(wdt->res_src));
+	release_mem_region(wdt->res_wdt->start, resource_size(wdt->res_wdt));
+	if (wdt->res_int_base)
+		release_mem_region(wdt->res_int_base->start,
+					resource_size(wdt->res_int_base));
+	kfree(wdt);
+	platform_set_drvdata(pdev, NULL);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int tegra_wdt_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct tegra_wdt *wdt = platform_get_drvdata(pdev);
+
+	tegra_wdt_disable(wdt);
+	return 0;
+}
+
+static int tegra_wdt_resume(struct platform_device *pdev)
+{
+	struct tegra_wdt *wdt = platform_get_drvdata(pdev);
+
+	if (wdt->status & WDT_ENABLED)
+		tegra_wdt_enable(wdt);
+
+	return 0;
+}
+#endif
+
+static struct platform_driver tegra_wdt_driver = {
+	.probe		= tegra_wdt_probe,
+	.remove		= __devexit_p(tegra_wdt_remove),
+#ifdef CONFIG_PM
+	.suspend	= tegra_wdt_suspend,
+	.resume		= tegra_wdt_resume,
+#endif
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= "tegra_wdt",
+	},
+};
+
+static int __init tegra_wdt_init(void)
+{
+	return platform_driver_register(&tegra_wdt_driver);
+}
+
+static void __exit tegra_wdt_exit(void)
+{
+	platform_driver_unregister(&tegra_wdt_driver);
+}
+
+module_init(tegra_wdt_init);
+module_exit(tegra_wdt_exit);
+
+MODULE_AUTHOR("NVIDIA Corporation");
+MODULE_DESCRIPTION("Tegra Watchdog Driver");
+
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_MISCDEV(WATCHDOG_MINOR);
+MODULE_ALIAS("platform:tegra_wdt");
diff --git a/drivers/xen/events.c b/drivers/xen/events.c
index 7523719bf8a4..44490de5aec6 100644
--- a/drivers/xen/events.c
+++ b/drivers/xen/events.c
@@ -1021,7 +1021,7 @@ int bind_ipi_to_irqhandler(enum ipi_vector ipi,
 	if (irq < 0)
 		return irq;
 
-	irqflags |= IRQF_NO_SUSPEND | IRQF_FORCE_RESUME;
+	irqflags |= IRQF_NO_SUSPEND | IRQF_FORCE_RESUME | IRQF_EARLY_RESUME;
 	retval = request_irq(irq, handler, irqflags, devname, dev_id);
 	if (retval != 0) {
 		unbind_from_irq(irq);
diff --git a/drivers/xen/gntalloc.c b/drivers/xen/gntalloc.c
index f6832f46aea4..e1c4c6e5b469 100644
--- a/drivers/xen/gntalloc.c
+++ b/drivers/xen/gntalloc.c
@@ -135,7 +135,7 @@ static int add_grefs(struct ioctl_gntalloc_alloc_gref *op,
 		/* Grant foreign access to the page. */
 		gref->gref_id = gnttab_grant_foreign_access(op->domid,
 			pfn_to_mfn(page_to_pfn(gref->page)), readonly);
-		if (gref->gref_id < 0) {
+		if ((int)gref->gref_id < 0) {
 			rc = gref->gref_id;
 			goto undo;
 		}
@@ -280,7 +280,7 @@ static long gntalloc_ioctl_alloc(struct gntalloc_file_private_data *priv,
 		goto out;
 	}
 
-	gref_ids = kzalloc(sizeof(gref_ids[0]) * op.count, GFP_TEMPORARY);
+	gref_ids = kcalloc(op.count, sizeof(gref_ids[0]), GFP_TEMPORARY);
 	if (!gref_ids) {
 		rc = -ENOMEM;
 		goto out;
diff --git a/drivers/xen/swiotlb-xen.c b/drivers/xen/swiotlb-xen.c
index 6e8c15a23201..84f317e0cc2e 100644
--- a/drivers/xen/swiotlb-xen.c
+++ b/drivers/xen/swiotlb-xen.c
@@ -278,9 +278,10 @@ dma_addr_t xen_swiotlb_map_page(struct device *dev, struct page *page,
 	/*
 	 * Ensure that the address returned is DMA'ble
 	 */
-	if (!dma_capable(dev, dev_addr, size))
-		panic("map_single: bounce buffer is not DMA'ble");
-
+	if (!dma_capable(dev, dev_addr, size)) {
+		swiotlb_tbl_unmap_single(dev, map, size, dir);
+		dev_addr = 0;
+	}
 	return dev_addr;
 }
 EXPORT_SYMBOL_GPL(xen_swiotlb_map_page);
diff --git a/drivers/xen/xenbus/xenbus_xs.c b/drivers/xen/xenbus/xenbus_xs.c
index 5534690075af..daee5db4bef8 100644
--- a/drivers/xen/xenbus/xenbus_xs.c
+++ b/drivers/xen/xenbus/xenbus_xs.c
@@ -801,6 +801,12 @@ static int process_msg(void)
 		goto out;
 	}
 
+	if (msg->hdr.len > XENSTORE_PAYLOAD_MAX) {
+		kfree(msg);
+		err = -EINVAL;
+		goto out;
+	}
+
 	body = kmalloc(msg->hdr.len + 1, GFP_NOIO | __GFP_HIGH);
 	if (body == NULL) {
 		kfree(msg);